4508 files changed, 351175 insertions, 182939 deletions

diff --git a/drivers/Makefile b/drivers/Makefile
index 53abb4a5f736..194d20bee7dc 100644
--- a/drivers/Makefile
+++ b/drivers/Makefile
@@ -21,7 +21,7 @@ obj-y				+= video/
 obj-y				+= idle/
 
 # IPMI must come before ACPI in order to provide IPMI opregion support
-obj-$(CONFIG_IPMI_HANDLER)	+= char/ipmi/
+obj-y				+= char/ipmi/
 
 obj-$(CONFIG_ACPI)		+= acpi/
 obj-$(CONFIG_SFI)		+= sfi/
@@ -29,6 +29,8 @@ obj-$(CONFIG_SFI)		+= sfi/
 # was used and do nothing if so
 obj-$(CONFIG_PNP)		+= pnp/
 obj-y				+= amba/
+
+obj-y				+= clk/
 # Many drivers will want to use DMA so this has to be made available
 # really early.
 obj-$(CONFIG_DMADEVICES)	+= dma/
@@ -142,8 +144,6 @@ obj-$(CONFIG_VHOST)		+= vhost/
 obj-$(CONFIG_VLYNQ)		+= vlynq/
 obj-$(CONFIG_STAGING)		+= staging/
 obj-y				+= platform/
-#common clk code
-obj-y				+= clk/
 
 obj-$(CONFIG_MAILBOX)		+= mailbox/
 obj-$(CONFIG_HWSPINLOCK)	+= hwspinlock/
diff --git a/drivers/acpi/Kconfig b/drivers/acpi/Kconfig
index 445ce28475b3..535e7828445a 100644
--- a/drivers/acpi/Kconfig
+++ b/drivers/acpi/Kconfig
@@ -77,6 +77,9 @@ config ACPI_DEBUGGER_USER
 
 endif
 
+config ACPI_SPCR_TABLE
+	bool
+
 config ACPI_SLEEP
 	bool
 	depends on SUSPEND || HIBERNATION
@@ -227,7 +230,6 @@ config ACPI_MCFG
 config ACPI_CPPC_LIB
 	bool
 	depends on ACPI_PROCESSOR
-	depends on !ACPI_CPU_FREQ_PSS
 	select MAILBOX
 	select PCC
 	help
@@ -462,6 +464,9 @@ source "drivers/acpi/nfit/Kconfig"
 source "drivers/acpi/apei/Kconfig"
 source "drivers/acpi/dptf/Kconfig"
 
+config ACPI_WATCHDOG
+	bool
+
 config ACPI_EXTLOG
 	tristate "Extended Error Log support"
 	depends on X86_MCE && X86_LOCAL_APIC
@@ -521,4 +526,8 @@ config ACPI_CONFIGFS
 	  userspace. The configurable ACPI groups will be visible under
 	  /config/acpi, assuming configfs is mounted under /config.
 
+if ARM64
+source "drivers/acpi/arm64/Kconfig"
+endif
+
 endif	# ACPI
diff --git a/drivers/acpi/Makefile b/drivers/acpi/Makefile
index 5ae9d85c5159..9ed087853dee 100644
--- a/drivers/acpi/Makefile
+++ b/drivers/acpi/Makefile
@@ -56,6 +56,7 @@ acpi-$(CONFIG_ACPI_NUMA)	+= numa.o
 acpi-$(CONFIG_ACPI_PROCFS_POWER) += cm_sbs.o
 acpi-y				+= acpi_lpat.o
 acpi-$(CONFIG_ACPI_GENERIC_GSI) += gsi.o
+acpi-$(CONFIG_ACPI_WATCHDOG)	+= acpi_watchdog.o
 
 # These are (potentially) separate modules
 
@@ -81,6 +82,7 @@ obj-$(CONFIG_ACPI_EC_DEBUGFS)	+= ec_sys.o
 obj-$(CONFIG_ACPI_CUSTOM_METHOD)+= custom_method.o
 obj-$(CONFIG_ACPI_BGRT)		+= bgrt.o
 obj-$(CONFIG_ACPI_CPPC_LIB)	+= cppc_acpi.o
+obj-$(CONFIG_ACPI_SPCR_TABLE)	+= spcr.o
 obj-$(CONFIG_ACPI_DEBUGGER_USER) += acpi_dbg.o
 
 # processor has its own "processor." module_param namespace
@@ -105,3 +107,5 @@ obj-$(CONFIG_ACPI_CONFIGFS)	+= acpi_configfs.o
 
 video-objs			+= acpi_video.o video_detect.o
 obj-y				+= dptf/
+
+obj-$(CONFIG_ARM64)		+= arm64/
diff --git a/drivers/acpi/acpi_apd.c b/drivers/acpi/acpi_apd.c
index 1daf9c46df8e..d58fbf7f04e6 100644
--- a/drivers/acpi/acpi_apd.c
+++ b/drivers/acpi/acpi_apd.c
@@ -42,6 +42,7 @@ struct apd_private_data;
 struct apd_device_desc {
 	unsigned int flags;
 	unsigned int fixed_clk_rate;
+	struct property_entry *properties;
 	int (*setup)(struct apd_private_data *pdata);
 };
 
@@ -71,22 +72,35 @@ static int acpi_apd_setup(struct apd_private_data *pdata)
 }
 
 #ifdef CONFIG_X86_AMD_PLATFORM_DEVICE
-static struct apd_device_desc cz_i2c_desc = {
+static const struct apd_device_desc cz_i2c_desc = {
 	.setup = acpi_apd_setup,
 	.fixed_clk_rate = 133000000,
 };
 
-static struct apd_device_desc cz_uart_desc = {
+static struct property_entry uart_properties[] = {
+	PROPERTY_ENTRY_U32("reg-io-width", 4),
+	PROPERTY_ENTRY_U32("reg-shift", 2),
+	PROPERTY_ENTRY_BOOL("snps,uart-16550-compatible"),
+	{ },
+};
+
+static const struct apd_device_desc cz_uart_desc = {
 	.setup = acpi_apd_setup,
 	.fixed_clk_rate = 48000000,
+	.properties = uart_properties,
 };
 #endif
 
 #ifdef CONFIG_ARM64
-static struct apd_device_desc xgene_i2c_desc = {
+static const struct apd_device_desc xgene_i2c_desc = {
 	.setup = acpi_apd_setup,
 	.fixed_clk_rate = 100000000,
 };
+
+static const struct apd_device_desc vulcan_spi_desc = {
+	.setup = acpi_apd_setup,
+	.fixed_clk_rate = 133000000,
+};
 #endif
 
 #else
@@ -125,6 +139,12 @@ static int acpi_apd_create_device(struct acpi_device *adev,
 			goto err_out;
 	}
 
+	if (dev_desc->properties) {
+		ret = device_add_properties(&adev->dev, dev_desc->properties);
+		if (ret)
+			goto err_out;
+	}
+
 	adev->driver_data = pdata;
 	pdev = acpi_create_platform_device(adev);
 	if (!IS_ERR_OR_NULL(pdev))
@@ -149,6 +169,7 @@ static const struct acpi_device_id acpi_apd_device_ids[] = {
 #endif
 #ifdef CONFIG_ARM64
 	{ "APMC0D0F", APD_ADDR(xgene_i2c_desc) },
+	{ "BRCM900D", APD_ADDR(vulcan_spi_desc) },
 #endif
 	{ }
 };
diff --git a/drivers/acpi/acpi_lpss.c b/drivers/acpi/acpi_lpss.c
index 357a0b8f860b..552010288135 100644
--- a/drivers/acpi/acpi_lpss.c
+++ b/drivers/acpi/acpi_lpss.c
@@ -75,6 +75,7 @@ struct lpss_device_desc {
 	const char *clk_con_id;
 	unsigned int prv_offset;
 	size_t prv_size_override;
+	struct property_entry *properties;
 	void (*setup)(struct lpss_private_data *pdata);
 };
 
@@ -163,11 +164,19 @@ static const struct lpss_device_desc lpt_i2c_dev_desc = {
 	.prv_offset = 0x800,
 };
 
+static struct property_entry uart_properties[] = {
+	PROPERTY_ENTRY_U32("reg-io-width", 4),
+	PROPERTY_ENTRY_U32("reg-shift", 2),
+	PROPERTY_ENTRY_BOOL("snps,uart-16550-compatible"),
+	{ },
+};
+
 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,
+	.properties = uart_properties,
 };
 
 static const struct lpss_device_desc lpt_sdio_dev_desc = {
@@ -189,6 +198,7 @@ static const struct lpss_device_desc byt_uart_dev_desc = {
 	.clk_con_id = "baudclk",
 	.prv_offset = 0x800,
 	.setup = lpss_uart_setup,
+	.properties = uart_properties,
 };
 
 static const struct lpss_device_desc bsw_uart_dev_desc = {
@@ -197,6 +207,7 @@ static const struct lpss_device_desc bsw_uart_dev_desc = {
 	.clk_con_id = "baudclk",
 	.prv_offset = 0x800,
 	.setup = lpss_uart_setup,
+	.properties = uart_properties,
 };
 
 static const struct lpss_device_desc byt_spi_dev_desc = {
@@ -440,6 +451,12 @@ static int acpi_lpss_create_device(struct acpi_device *adev,
 		goto err_out;
 	}
 
+	if (dev_desc->properties) {
+		ret = device_add_properties(&adev->dev, dev_desc->properties);
+		if (ret)
+			goto err_out;
+	}
+
 	adev->driver_data = pdata;
 	pdev = acpi_create_platform_device(adev);
 	if (!IS_ERR_OR_NULL(pdev)) {
diff --git a/drivers/acpi/acpi_pad.c b/drivers/acpi/acpi_pad.c
index 8ea8211b2d58..eb76a4c10dbf 100644
--- a/drivers/acpi/acpi_pad.c
+++ b/drivers/acpi/acpi_pad.c
@@ -26,6 +26,7 @@
 #include <linux/slab.h>
 #include <linux/acpi.h>
 #include <asm/mwait.h>
+#include <xen/xen.h>
 
 #define ACPI_PROCESSOR_AGGREGATOR_CLASS	"acpi_pad"
 #define ACPI_PROCESSOR_AGGREGATOR_DEVICE_NAME "Processor Aggregator"
@@ -477,6 +478,10 @@ static struct acpi_driver acpi_pad_driver = {
 
 static int __init acpi_pad_init(void)
 {
+	/* Xen ACPI PAD is used when running as Xen Dom0. */
+	if (xen_initial_domain())
+		return -ENODEV;
+
 	power_saving_mwait_init();
 	if (power_saving_mwait_eax == 0)
 		return -EINVAL;
diff --git a/drivers/acpi/acpi_platform.c b/drivers/acpi/acpi_platform.c
index 159f7f19abce..b200ae1f3c6f 100644
--- a/drivers/acpi/acpi_platform.c
+++ b/drivers/acpi/acpi_platform.c
@@ -17,6 +17,7 @@
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/dma-mapping.h>
+#include <linux/pci.h>
 #include <linux/platform_device.h>
 
 #include "internal.h"
@@ -30,6 +31,22 @@ static const struct acpi_device_id forbidden_id_list[] = {
 	{"", 0},
 };
 
+static void acpi_platform_fill_resource(struct acpi_device *adev,
+	const struct resource *src, struct resource *dest)
+{
+	struct device *parent;
+
+	*dest = *src;
+
+	/*
+	 * If the device has parent we need to take its resources into
+	 * account as well because this device might consume part of those.
+	 */
+	parent = acpi_get_first_physical_node(adev->parent);
+	if (parent && dev_is_pci(parent))
+		dest->parent = pci_find_resource(to_pci_dev(parent), dest);
+}
+
 /**
  * acpi_create_platform_device - Create platform device for ACPI device node
  * @adev: ACPI device node to create a platform device for.
@@ -70,7 +87,8 @@ struct platform_device *acpi_create_platform_device(struct acpi_device *adev)
 		}
 		count = 0;
 		list_for_each_entry(rentry, &resource_list, node)
-			resources[count++] = *rentry->res;
+			acpi_platform_fill_resource(adev, rentry->res,
+						    &resources[count++]);
 
 		acpi_dev_free_resource_list(&resource_list);
 	}
diff --git a/drivers/acpi/acpi_processor.c b/drivers/acpi/acpi_processor.c
index c7ba948d253c..3de3b6b8f0f1 100644
--- a/drivers/acpi/acpi_processor.c
+++ b/drivers/acpi/acpi_processor.c
@@ -182,6 +182,11 @@ int __weak arch_register_cpu(int cpu)
 
 void __weak arch_unregister_cpu(int cpu) {}
 
+int __weak acpi_map_cpu2node(acpi_handle handle, int cpu, int physid)
+{
+	return -ENODEV;
+}
+
 static int acpi_processor_hotadd_init(struct acpi_processor *pr)
 {
 	unsigned long long sta;
@@ -300,8 +305,11 @@ static int acpi_processor_get_info(struct acpi_device *device)
 	 *  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.
+	 *
+	 *  NOTE: Even if the processor has a cpuid, it may not be present
+	 *  because cpuid <-> apicid mapping is persistent now.
 	 */
-	if (invalid_logical_cpuid(pr->id)) {
+	if (invalid_logical_cpuid(pr->id) || !cpu_present(pr->id)) {
 		int ret = acpi_processor_hotadd_init(pr);
 		if (ret)
 			return ret;
@@ -573,8 +581,102 @@ static struct acpi_scan_handler processor_container_handler = {
 	.attach = acpi_processor_container_attach,
 };
 
+/* The number of the unique processor IDs */
+static int nr_unique_ids __initdata;
+
+/* The number of the duplicate processor IDs */
+static int nr_duplicate_ids __initdata;
+
+/* Used to store the unique processor IDs */
+static int unique_processor_ids[] __initdata = {
+	[0 ... NR_CPUS - 1] = -1,
+};
+
+/* Used to store the duplicate processor IDs */
+static int duplicate_processor_ids[] __initdata = {
+	[0 ... NR_CPUS - 1] = -1,
+};
+
+static void __init processor_validated_ids_update(int proc_id)
+{
+	int i;
+
+	if (nr_unique_ids == NR_CPUS||nr_duplicate_ids == NR_CPUS)
+		return;
+
+	/*
+	 * Firstly, compare the proc_id with duplicate IDs, if the proc_id is
+	 * already in the IDs, do nothing.
+	 */
+	for (i = 0; i < nr_duplicate_ids; i++) {
+		if (duplicate_processor_ids[i] == proc_id)
+			return;
+	}
+
+	/*
+	 * Secondly, compare the proc_id with unique IDs, if the proc_id is in
+	 * the IDs, put it in the duplicate IDs.
+	 */
+	for (i = 0; i < nr_unique_ids; i++) {
+		if (unique_processor_ids[i] == proc_id) {
+			duplicate_processor_ids[nr_duplicate_ids] = proc_id;
+			nr_duplicate_ids++;
+			return;
+		}
+	}
+
+	/*
+	 * Lastly, the proc_id is a unique ID, put it in the unique IDs.
+	 */
+	unique_processor_ids[nr_unique_ids] = proc_id;
+	nr_unique_ids++;
+}
+
+static acpi_status __init acpi_processor_ids_walk(acpi_handle handle,
+						  u32 lvl,
+						  void *context,
+						  void **rv)
+{
+	acpi_status status;
+	union acpi_object object = { 0 };
+	struct acpi_buffer buffer = { sizeof(union acpi_object), &object };
+
+	status = acpi_evaluate_object(handle, NULL, NULL, &buffer);
+	if (ACPI_FAILURE(status))
+		acpi_handle_info(handle, "Not get the processor object\n");
+	else
+		processor_validated_ids_update(object.processor.proc_id);
+
+	return AE_OK;
+}
+
+static void __init acpi_processor_check_duplicates(void)
+{
+	/* Search all processor nodes in ACPI namespace */
+	acpi_walk_namespace(ACPI_TYPE_PROCESSOR, ACPI_ROOT_OBJECT,
+						ACPI_UINT32_MAX,
+						acpi_processor_ids_walk,
+						NULL, NULL, NULL);
+}
+
+bool __init acpi_processor_validate_proc_id(int proc_id)
+{
+	int i;
+
+	/*
+	 * compare the proc_id with duplicate IDs, if the proc_id is already
+	 * in the duplicate IDs, return true, otherwise, return false.
+	 */
+	for (i = 0; i < nr_duplicate_ids; i++) {
+		if (duplicate_processor_ids[i] == proc_id)
+			return true;
+	}
+	return false;
+}
+
 void __init acpi_processor_init(void)
 {
+	acpi_processor_check_duplicates();
 	acpi_scan_add_handler_with_hotplug(&processor_handler, "processor");
 	acpi_scan_add_handler(&processor_container_handler);
 }
diff --git a/drivers/acpi/acpi_watchdog.c b/drivers/acpi/acpi_watchdog.c
new file mode 100644
index 000000000000..13caebd679f5
--- /dev/null
+++ b/drivers/acpi/acpi_watchdog.c
@@ -0,0 +1,123 @@
+/*
+ * ACPI watchdog table parsing support.
+ *
+ * Copyright (C) 2016, Intel Corporation
+ * Author: Mika Westerberg <mika.westerberg@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.
+ */
+
+#define pr_fmt(fmt) "ACPI: watchdog: " fmt
+
+#include <linux/acpi.h>
+#include <linux/ioport.h>
+#include <linux/platform_device.h>
+
+#include "internal.h"
+
+/**
+ * Returns true if this system should prefer ACPI based watchdog instead of
+ * the native one (which are typically the same hardware).
+ */
+bool acpi_has_watchdog(void)
+{
+	struct acpi_table_header hdr;
+
+	if (acpi_disabled)
+		return false;
+
+	return ACPI_SUCCESS(acpi_get_table_header(ACPI_SIG_WDAT, 0, &hdr));
+}
+EXPORT_SYMBOL_GPL(acpi_has_watchdog);
+
+void __init acpi_watchdog_init(void)
+{
+	const struct acpi_wdat_entry *entries;
+	const struct acpi_table_wdat *wdat;
+	struct list_head resource_list;
+	struct resource_entry *rentry;
+	struct platform_device *pdev;
+	struct resource *resources;
+	size_t nresources = 0;
+	acpi_status status;
+	int i;
+
+	status = acpi_get_table(ACPI_SIG_WDAT, 0,
+				(struct acpi_table_header **)&wdat);
+	if (ACPI_FAILURE(status)) {
+		/* It is fine if there is no WDAT */
+		return;
+	}
+
+	/* Watchdog disabled by BIOS */
+	if (!(wdat->flags & ACPI_WDAT_ENABLED))
+		return;
+
+	/* Skip legacy PCI WDT devices */
+	if (wdat->pci_segment != 0xff || wdat->pci_bus != 0xff ||
+	    wdat->pci_device != 0xff || wdat->pci_function != 0xff)
+		return;
+
+	INIT_LIST_HEAD(&resource_list);
+
+	entries = (struct acpi_wdat_entry *)(wdat + 1);
+	for (i = 0; i < wdat->entries; i++) {
+		const struct acpi_generic_address *gas;
+		struct resource_entry *rentry;
+		struct resource res;
+		bool found;
+
+		gas = &entries[i].register_region;
+
+		res.start = gas->address;
+		if (gas->space_id == ACPI_ADR_SPACE_SYSTEM_MEMORY) {
+			res.flags = IORESOURCE_MEM;
+			res.end = res.start + ALIGN(gas->access_width, 4);
+		} else if (gas->space_id == ACPI_ADR_SPACE_SYSTEM_IO) {
+			res.flags = IORESOURCE_IO;
+			res.end = res.start + gas->access_width;
+		} else {
+			pr_warn("Unsupported address space: %u\n",
+				gas->space_id);
+			goto fail_free_resource_list;
+		}
+
+		found = false;
+		resource_list_for_each_entry(rentry, &resource_list) {
+			if (resource_contains(rentry->res, &res)) {
+				found = true;
+				break;
+			}
+		}
+
+		if (!found) {
+			rentry = resource_list_create_entry(NULL, 0);
+			if (!rentry)
+				goto fail_free_resource_list;
+
+			*rentry->res = res;
+			resource_list_add_tail(rentry, &resource_list);
+			nresources++;
+		}
+	}
+
+	resources = kcalloc(nresources, sizeof(*resources), GFP_KERNEL);
+	if (!resources)
+		goto fail_free_resource_list;
+
+	i = 0;
+	resource_list_for_each_entry(rentry, &resource_list)
+		resources[i++] = *rentry->res;
+
+	pdev = platform_device_register_simple("wdat_wdt", PLATFORM_DEVID_NONE,
+					       resources, nresources);
+	if (IS_ERR(pdev))
+		pr_err("Failed to create platform device\n");
+
+	kfree(resources);
+
+fail_free_resource_list:
+	resource_list_free(&resource_list);
+}
diff --git a/drivers/acpi/acpica/Makefile b/drivers/acpi/acpica/Makefile
index 227bb7bb19d7..32d93edbc479 100644
--- a/drivers/acpi/acpica/Makefile
+++ b/drivers/acpi/acpica/Makefile
@@ -175,6 +175,7 @@ acpi-y +=		\
 	utresrc.o	\
 	utstate.o	\
 	utstring.o	\
+	utstrtoul64.o	\
 	utxface.o	\
 	utxfinit.o	\
 	utxferror.o	\
diff --git a/drivers/acpi/acpica/acapps.h b/drivers/acpi/acpica/acapps.h
index ca2c0607104b..0bd6307e1f3c 100644
--- a/drivers/acpi/acpica/acapps.h
+++ b/drivers/acpi/acpica/acapps.h
@@ -44,7 +44,9 @@
 #ifndef _ACAPPS
 #define _ACAPPS
 
-#include <stdio.h>
+#ifdef ACPI_USE_STANDARD_HEADERS
+#include <sys/stat.h>
+#endif				/* ACPI_USE_STANDARD_HEADERS */
 
 /* Common info for tool signons */
 
@@ -81,13 +83,13 @@
 /* Macros for usage messages */
 
 #define ACPI_USAGE_HEADER(usage) \
-	acpi_os_printf ("Usage: %s\nOptions:\n", usage);
+	printf ("Usage: %s\nOptions:\n", usage);
 
 #define ACPI_USAGE_TEXT(description) \
-	acpi_os_printf (description);
+	printf (description);
 
 #define ACPI_OPTION(name, description) \
-	acpi_os_printf (" %-20s%s\n", name, description);
+	printf ("  %-20s%s\n", name, description);
 
 /* Check for unexpected exceptions */
 
diff --git a/drivers/acpi/acpica/acdebug.h b/drivers/acpi/acpica/acdebug.h
index f6404ea928cb..94737f8845ac 100644
--- a/drivers/acpi/acpica/acdebug.h
+++ b/drivers/acpi/acpica/acdebug.h
@@ -155,7 +155,7 @@ acpi_status acpi_db_disassemble_method(char *name);
 
 void acpi_db_disassemble_aml(char *statements, union acpi_parse_object *op);
 
-void acpi_db_batch_execute(char *count_arg);
+void acpi_db_evaluate_predefined_names(void);
 
 /*
  * dbnames - namespace commands
diff --git a/drivers/acpi/acpica/acevents.h b/drivers/acpi/acpica/acevents.h
index 77af91cf46d4..92fa47c6498c 100644
--- a/drivers/acpi/acpica/acevents.h
+++ b/drivers/acpi/acpica/acevents.h
@@ -86,6 +86,9 @@ acpi_ev_update_gpe_enable_mask(struct acpi_gpe_event_info *gpe_event_info);
 acpi_status acpi_ev_enable_gpe(struct acpi_gpe_event_info *gpe_event_info);
 
 acpi_status
+acpi_ev_mask_gpe(struct acpi_gpe_event_info *gpe_event_info, u8 is_masked);
+
+acpi_status
 acpi_ev_add_gpe_reference(struct acpi_gpe_event_info *gpe_event_info);
 
 acpi_status
diff --git a/drivers/acpi/acpica/acglobal.h b/drivers/acpi/acpica/acglobal.h
index fded776236e2..750fa824d42c 100644
--- a/drivers/acpi/acpica/acglobal.h
+++ b/drivers/acpi/acpica/acglobal.h
@@ -317,6 +317,7 @@ ACPI_INIT_GLOBAL(u8, acpi_gbl_ignore_noop_operator, FALSE);
 ACPI_INIT_GLOBAL(u8, acpi_gbl_cstyle_disassembly, TRUE);
 ACPI_INIT_GLOBAL(u8, acpi_gbl_force_aml_disassembly, FALSE);
 ACPI_INIT_GLOBAL(u8, acpi_gbl_dm_opt_verbose, TRUE);
+ACPI_INIT_GLOBAL(u8, acpi_gbl_dm_emit_external_opcodes, FALSE);
 
 ACPI_GLOBAL(u8, acpi_gbl_dm_opt_disasm);
 ACPI_GLOBAL(u8, acpi_gbl_dm_opt_listing);
@@ -382,6 +383,7 @@ ACPI_GLOBAL(const char, *acpi_gbl_pld_shape_list[]);
 
 ACPI_INIT_GLOBAL(ACPI_FILE, acpi_gbl_debug_file, NULL);
 ACPI_INIT_GLOBAL(ACPI_FILE, acpi_gbl_output_file, NULL);
+ACPI_INIT_GLOBAL(u8, acpi_gbl_debug_timeout, FALSE);
 
 /* Print buffer */
 
diff --git a/drivers/acpi/acpica/aclocal.h b/drivers/acpi/acpica/aclocal.h
index 13331d70dea0..dff1207a6078 100644
--- a/drivers/acpi/acpica/aclocal.h
+++ b/drivers/acpi/acpica/aclocal.h
@@ -484,6 +484,7 @@ struct acpi_gpe_event_info {
 	u8 flags;		/* Misc info about this GPE */
 	u8 gpe_number;		/* This GPE */
 	u8 runtime_count;	/* References to a run GPE */
+	u8 disable_for_dispatch;	/* Masked during dispatching */
 };
 
 /* Information about a GPE register pair, one per each status/enable pair in an array */
@@ -494,6 +495,7 @@ struct acpi_gpe_register_info {
 	u16 base_gpe_number;	/* Base GPE number for this register */
 	u8 enable_for_wake;	/* GPEs to keep enabled when sleeping */
 	u8 enable_for_run;	/* GPEs to keep enabled when running */
+	u8 mask_for_run;	/* GPEs to keep masked when running */
 	u8 enable_mask;		/* Current mask of enabled GPEs */
 };
 
diff --git a/drivers/acpi/acpica/acnamesp.h b/drivers/acpi/acpica/acnamesp.h
index f33a4ba8e0cb..bb7fca1c8ba3 100644
--- a/drivers/acpi/acpica/acnamesp.h
+++ b/drivers/acpi/acpica/acnamesp.h
@@ -130,6 +130,9 @@ acpi_status
 acpi_ns_parse_table(u32 table_index, struct acpi_namespace_node *start_node);
 
 acpi_status
+acpi_ns_execute_table(u32 table_index, struct acpi_namespace_node *start_node);
+
+acpi_status
 acpi_ns_one_complete_parse(u32 pass_number,
 			   u32 table_index,
 			   struct acpi_namespace_node *start_node);
@@ -296,6 +299,11 @@ u8
 acpi_ns_pattern_match(struct acpi_namespace_node *obj_node, char *search_for);
 
 acpi_status
+acpi_ns_get_node_unlocked(struct acpi_namespace_node *prefix_node,
+			  const char *external_pathname,
+			  u32 flags, struct acpi_namespace_node **out_node);
+
+acpi_status
 acpi_ns_get_node(struct acpi_namespace_node *prefix_node,
 		 const char *external_pathname,
 		 u32 flags, struct acpi_namespace_node **out_node);
diff --git a/drivers/acpi/acpica/acparser.h b/drivers/acpi/acpica/acparser.h
index fc305775c3d7..939d41113970 100644
--- a/drivers/acpi/acpica/acparser.h
+++ b/drivers/acpi/acpica/acparser.h
@@ -78,6 +78,8 @@ extern const u8 acpi_gbl_long_op_index[];
  */
 acpi_status acpi_ps_execute_method(struct acpi_evaluate_info *info);
 
+acpi_status acpi_ps_execute_table(struct acpi_evaluate_info *info);
+
 /*
  * psargs - Parse AML opcode arguments
  */
diff --git a/drivers/acpi/acpica/actables.h b/drivers/acpi/acpica/actables.h
index cd5a135fcf29..e85953b6fa0e 100644
--- a/drivers/acpi/acpica/actables.h
+++ b/drivers/acpi/acpica/actables.h
@@ -123,6 +123,14 @@ acpi_tb_install_standard_table(acpi_physical_address address,
 
 void acpi_tb_uninstall_table(struct acpi_table_desc *table_desc);
 
+acpi_status
+acpi_tb_load_table(u32 table_index, struct acpi_namespace_node *parent_node);
+
+acpi_status
+acpi_tb_install_and_load_table(struct acpi_table_header *table,
+			       acpi_physical_address address,
+			       u8 flags, u8 override, u32 *table_index);
+
 void acpi_tb_terminate(void);
 
 acpi_status acpi_tb_delete_namespace_by_owner(u32 table_index);
@@ -155,10 +163,6 @@ void
 acpi_tb_install_table_with_override(struct acpi_table_desc *new_table_desc,
 				    u8 override, u32 *table_index);
 
-acpi_status
-acpi_tb_install_fixed_table(acpi_physical_address address,
-			    char *signature, u32 *table_index);
-
 acpi_status acpi_tb_parse_root_table(acpi_physical_address rsdp_address);
 
 /*
diff --git a/drivers/acpi/acpica/acutils.h b/drivers/acpi/acpica/acutils.h
index a7dbb2b882cf..0a1b53c9ee0e 100644
--- a/drivers/acpi/acpica/acutils.h
+++ b/drivers/acpi/acpica/acutils.h
@@ -114,13 +114,25 @@ extern const char *acpi_gbl_pt_decode[];
 /*
  * Common error message prefixes
  */
+#ifndef ACPI_MSG_ERROR
 #define ACPI_MSG_ERROR          "ACPI Error: "
+#endif
+#ifndef ACPI_MSG_EXCEPTION
 #define ACPI_MSG_EXCEPTION      "ACPI Exception: "
+#endif
+#ifndef ACPI_MSG_WARNING
 #define ACPI_MSG_WARNING        "ACPI Warning: "
+#endif
+#ifndef ACPI_MSG_INFO
 #define ACPI_MSG_INFO           "ACPI: "
+#endif
 
+#ifndef ACPI_MSG_BIOS_ERROR
 #define ACPI_MSG_BIOS_ERROR     "ACPI BIOS Error (bug): "
+#endif
+#ifndef ACPI_MSG_BIOS_WARNING
 #define ACPI_MSG_BIOS_WARNING   "ACPI BIOS Warning (bug): "
+#endif
 
 /*
  * Common message suffix
@@ -184,14 +196,15 @@ void acpi_ut_strlwr(char *src_string);
 
 int acpi_ut_stricmp(char *string1, char *string2);
 
-acpi_status
-acpi_ut_strtoul64(char *string,
-		  u32 base, u32 max_integer_byte_width, u64 *ret_integer);
-
-/* Values for max_integer_byte_width above */
+acpi_status acpi_ut_strtoul64(char *string, u32 flags, u64 *ret_integer);
 
-#define ACPI_MAX32_BYTE_WIDTH       4
-#define ACPI_MAX64_BYTE_WIDTH       8
+/*
+ * Values for Flags above
+ * Note: LIMIT values correspond to acpi_gbl_integer_byte_width values (4/8)
+ */
+#define ACPI_STRTOUL_32BIT          0x04	/* 4 bytes */
+#define ACPI_STRTOUL_64BIT          0x08	/* 8 bytes */
+#define ACPI_STRTOUL_BASE16         0x10	/* Default: Base10/16 */
 
 /*
  * utglobal - Global data structures and procedures
@@ -221,6 +234,8 @@ const char *acpi_ut_get_event_name(u32 event_id);
 
 char acpi_ut_hex_to_ascii_char(u64 integer, u32 position);
 
+acpi_status acpi_ut_ascii_to_hex_byte(char *two_ascii_chars, u8 *return_byte);
+
 u8 acpi_ut_ascii_char_to_hex(int hex_char);
 
 u8 acpi_ut_valid_object_type(acpi_object_type type);
@@ -318,6 +333,11 @@ acpi_ut_ptr_exit(u32 line_number,
 		 const char *module_name, u32 component_id, u8 *ptr);
 
 void
+acpi_ut_str_exit(u32 line_number,
+		 const char *function_name,
+		 const char *module_name, u32 component_id, const char *string);
+
+void
 acpi_ut_debug_dump_buffer(u8 *buffer, u32 count, u32 display, u32 component_id);
 
 void acpi_ut_dump_buffer(u8 *buffer, u32 count, u32 display, u32 offset);
@@ -707,25 +727,6 @@ const struct ah_device_id *acpi_ah_match_hardware_id(char *hid);
 const char *acpi_ah_match_uuid(u8 *data);
 
 /*
- * utprint - printf/vprintf output functions
- */
-const char *acpi_ut_scan_number(const char *string, u64 *number_ptr);
-
-const char *acpi_ut_print_number(char *string, u64 number);
-
-int
-acpi_ut_vsnprintf(char *string,
-		  acpi_size size, const char *format, va_list args);
-
-int acpi_ut_snprintf(char *string, acpi_size size, const char *format, ...);
-
-#ifdef ACPI_APPLICATION
-int acpi_ut_file_vprintf(ACPI_FILE file, const char *format, va_list args);
-
-int acpi_ut_file_printf(ACPI_FILE file, const char *format, ...);
-#endif
-
-/*
  * utuuid -- UUID support functions
  */
 #if (defined ACPI_ASL_COMPILER || defined ACPI_EXEC_APP || defined ACPI_HELP_APP)
diff --git a/drivers/acpi/acpica/dbconvert.c b/drivers/acpi/acpica/dbconvert.c
index 7cd07b27f758..147ce8894f76 100644
--- a/drivers/acpi/acpica/dbconvert.c
+++ b/drivers/acpi/acpica/dbconvert.c
@@ -277,9 +277,10 @@ acpi_db_convert_to_object(acpi_object_type type,
 	default:
 
 		object->type = ACPI_TYPE_INTEGER;
-		status =
-		    acpi_ut_strtoul64(string, 16, acpi_gbl_integer_byte_width,
-				      &object->integer.value);
+		status = acpi_ut_strtoul64(string,
+					   (acpi_gbl_integer_byte_width |
+					    ACPI_STRTOUL_BASE16),
+					   &object->integer.value);
 		break;
 	}
 
diff --git a/drivers/acpi/acpica/dbexec.c b/drivers/acpi/acpica/dbexec.c
index 12df2915ad74..fe3da7c31bb7 100644
--- a/drivers/acpi/acpica/dbexec.c
+++ b/drivers/acpi/acpica/dbexec.c
@@ -392,42 +392,48 @@ acpi_db_execute(char *name, char **args, acpi_object_type *types, u32 flags)
 					  acpi_db_execution_walk, NULL, NULL,
 					  NULL);
 		return;
-	} else {
-		name_string = ACPI_ALLOCATE(strlen(name) + 1);
-		if (!name_string) {
-			return;
-		}
+	}
 
-		memset(&acpi_gbl_db_method_info, 0,
-		       sizeof(struct acpi_db_method_info));
+	name_string = ACPI_ALLOCATE(strlen(name) + 1);
+	if (!name_string) {
+		return;
+	}
 
-		strcpy(name_string, name);
-		acpi_ut_strupr(name_string);
-		acpi_gbl_db_method_info.name = name_string;
-		acpi_gbl_db_method_info.args = args;
-		acpi_gbl_db_method_info.types = types;
-		acpi_gbl_db_method_info.flags = flags;
+	memset(&acpi_gbl_db_method_info, 0, sizeof(struct acpi_db_method_info));
+	strcpy(name_string, name);
+	acpi_ut_strupr(name_string);
 
-		return_obj.pointer = NULL;
-		return_obj.length = ACPI_ALLOCATE_BUFFER;
+	/* Subcommand to Execute all predefined names in the namespace */
 
-		status = acpi_db_execute_setup(&acpi_gbl_db_method_info);
-		if (ACPI_FAILURE(status)) {
-			ACPI_FREE(name_string);
-			return;
-		}
+	if (!strncmp(name_string, "PREDEF", 6)) {
+		acpi_db_evaluate_predefined_names();
+		ACPI_FREE(name_string);
+		return;
+	}
 
-		/* Get the NS node, determines existence also */
+	acpi_gbl_db_method_info.name = name_string;
+	acpi_gbl_db_method_info.args = args;
+	acpi_gbl_db_method_info.types = types;
+	acpi_gbl_db_method_info.flags = flags;
 
-		status = acpi_get_handle(NULL, acpi_gbl_db_method_info.pathname,
-					 &acpi_gbl_db_method_info.method);
-		if (ACPI_SUCCESS(status)) {
-			status =
-			    acpi_db_execute_method(&acpi_gbl_db_method_info,
-						   &return_obj);
-		}
+	return_obj.pointer = NULL;
+	return_obj.length = ACPI_ALLOCATE_BUFFER;
+
+	status = acpi_db_execute_setup(&acpi_gbl_db_method_info);
+	if (ACPI_FAILURE(status)) {
 		ACPI_FREE(name_string);
+		return;
+	}
+
+	/* Get the NS node, determines existence also */
+
+	status = acpi_get_handle(NULL, acpi_gbl_db_method_info.pathname,
+				 &acpi_gbl_db_method_info.method);
+	if (ACPI_SUCCESS(status)) {
+		status = acpi_db_execute_method(&acpi_gbl_db_method_info,
+						&return_obj);
 	}
+	ACPI_FREE(name_string);
 
 	/*
 	 * Allow any handlers in separate threads to complete.
diff --git a/drivers/acpi/acpica/dbfileio.c b/drivers/acpi/acpica/dbfileio.c
index 483287942372..6f05b8c271a5 100644
--- a/drivers/acpi/acpica/dbfileio.c
+++ b/drivers/acpi/acpica/dbfileio.c
@@ -46,14 +46,12 @@
 #include "accommon.h"
 #include "acdebug.h"
 #include "actables.h"
-#include <stdio.h>
-#ifdef ACPI_APPLICATION
-#include "acapps.h"
-#endif
 
 #define _COMPONENT          ACPI_CA_DEBUGGER
 ACPI_MODULE_NAME("dbfileio")
 
+#ifdef ACPI_APPLICATION
+#include "acapps.h"
 #ifdef ACPI_DEBUGGER
 /*******************************************************************************
  *
@@ -69,8 +67,6 @@ ACPI_MODULE_NAME("dbfileio")
 void acpi_db_close_debug_file(void)
 {
 
-#ifdef ACPI_APPLICATION
-
 	if (acpi_gbl_debug_file) {
 		fclose(acpi_gbl_debug_file);
 		acpi_gbl_debug_file = NULL;
@@ -78,7 +74,6 @@ void acpi_db_close_debug_file(void)
 		acpi_os_printf("Debug output file %s closed\n",
 			       acpi_gbl_db_debug_filename);
 	}
-#endif
 }
 
 /*******************************************************************************
@@ -96,8 +91,6 @@ void acpi_db_close_debug_file(void)
 void acpi_db_open_debug_file(char *name)
 {
 
-#ifdef ACPI_APPLICATION
-
 	acpi_db_close_debug_file();
 	acpi_gbl_debug_file = fopen(name, "w+");
 	if (!acpi_gbl_debug_file) {
@@ -109,8 +102,6 @@ void acpi_db_open_debug_file(char *name)
 	strncpy(acpi_gbl_db_debug_filename, name,
 		sizeof(acpi_gbl_db_debug_filename));
 	acpi_gbl_db_output_to_file = TRUE;
-
-#endif
 }
 #endif
 
@@ -152,12 +143,13 @@ acpi_status acpi_db_load_tables(struct acpi_new_table_desc *list_head)
 			return (status);
 		}
 
-		fprintf(stderr,
-			"Acpi table [%4.4s] successfully installed and loaded\n",
-			table->signature);
+		acpi_os_printf
+		    ("Acpi table [%4.4s] successfully installed and loaded\n",
+		     table->signature);
 
 		table_list_head = table_list_head->next;
 	}
 
 	return (AE_OK);
 }
+#endif
diff --git a/drivers/acpi/acpica/dbinput.c b/drivers/acpi/acpica/dbinput.c
index 7cd5d2e022da..068214f9cc9d 100644
--- a/drivers/acpi/acpica/dbinput.c
+++ b/drivers/acpi/acpica/dbinput.c
@@ -286,6 +286,8 @@ static const struct acpi_db_command_help acpi_gbl_db_command_help[] = {
 	{1, "     \"Ascii String\"", "String method argument\n"},
 	{1, "     (Hex Byte List)", "Buffer method argument\n"},
 	{1, "     [Package Element List]", "Package method argument\n"},
+	{5, "  Execute predefined",
+	 "Execute all predefined (public) methods\n"},
 	{1, "  Go", "Allow method to run to completion\n"},
 	{1, "  Information", "Display info about the current method\n"},
 	{1, "  Into", "Step into (not over) a method call\n"},
diff --git a/drivers/acpi/acpica/dbmethod.c b/drivers/acpi/acpica/dbmethod.c
index f17a86f6b16b..314b94cf086a 100644
--- a/drivers/acpi/acpica/dbmethod.c
+++ b/drivers/acpi/acpica/dbmethod.c
@@ -52,6 +52,11 @@
 #define _COMPONENT          ACPI_CA_DEBUGGER
 ACPI_MODULE_NAME("dbmethod")
 
+/* Local prototypes */
+static acpi_status
+acpi_db_walk_for_execute(acpi_handle obj_handle,
+			 u32 nesting_level, void *context, void **return_value);
+
 /*******************************************************************************
  *
  * FUNCTION:    acpi_db_set_method_breakpoint
@@ -66,6 +71,7 @@ ACPI_MODULE_NAME("dbmethod")
  *              AML offset
  *
  ******************************************************************************/
+
 void
 acpi_db_set_method_breakpoint(char *location,
 			      struct acpi_walk_state *walk_state,
@@ -367,3 +373,129 @@ acpi_status acpi_db_disassemble_method(char *name)
 	acpi_ut_release_owner_id(&obj_desc->method.owner_id);
 	return (AE_OK);
 }
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_db_walk_for_execute
+ *
+ * PARAMETERS:  Callback from walk_namespace
+ *
+ * RETURN:      Status
+ *
+ * DESCRIPTION: Batch execution module. Currently only executes predefined
+ *              ACPI names.
+ *
+ ******************************************************************************/
+
+static acpi_status
+acpi_db_walk_for_execute(acpi_handle obj_handle,
+			 u32 nesting_level, void *context, void **return_value)
+{
+	struct acpi_namespace_node *node =
+	    (struct acpi_namespace_node *)obj_handle;
+	struct acpi_db_execute_walk *info =
+	    (struct acpi_db_execute_walk *)context;
+	struct acpi_buffer return_obj;
+	acpi_status status;
+	char *pathname;
+	u32 i;
+	struct acpi_device_info *obj_info;
+	struct acpi_object_list param_objects;
+	union acpi_object params[ACPI_METHOD_NUM_ARGS];
+	const union acpi_predefined_info *predefined;
+
+	predefined = acpi_ut_match_predefined_method(node->name.ascii);
+	if (!predefined) {
+		return (AE_OK);
+	}
+
+	if (node->type == ACPI_TYPE_LOCAL_SCOPE) {
+		return (AE_OK);
+	}
+
+	pathname = acpi_ns_get_external_pathname(node);
+	if (!pathname) {
+		return (AE_OK);
+	}
+
+	/* Get the object info for number of method parameters */
+
+	status = acpi_get_object_info(obj_handle, &obj_info);
+	if (ACPI_FAILURE(status)) {
+		return (status);
+	}
+
+	param_objects.pointer = NULL;
+	param_objects.count = 0;
+
+	if (obj_info->type == ACPI_TYPE_METHOD) {
+
+		/* Setup default parameters */
+
+		for (i = 0; i < obj_info->param_count; i++) {
+			params[i].type = ACPI_TYPE_INTEGER;
+			params[i].integer.value = 1;
+		}
+
+		param_objects.pointer = params;
+		param_objects.count = obj_info->param_count;
+	}
+
+	ACPI_FREE(obj_info);
+	return_obj.pointer = NULL;
+	return_obj.length = ACPI_ALLOCATE_BUFFER;
+
+	/* Do the actual method execution */
+
+	acpi_gbl_method_executing = TRUE;
+
+	status = acpi_evaluate_object(node, NULL, &param_objects, &return_obj);
+
+	acpi_os_printf("%-32s returned %s\n", pathname,
+		       acpi_format_exception(status));
+	acpi_gbl_method_executing = FALSE;
+	ACPI_FREE(pathname);
+
+	/* Ignore status from method execution */
+
+	status = AE_OK;
+
+	/* Update count, check if we have executed enough methods */
+
+	info->count++;
+	if (info->count >= info->max_count) {
+		status = AE_CTRL_TERMINATE;
+	}
+
+	return (status);
+}
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_db_evaluate_predefined_names
+ *
+ * PARAMETERS:  None
+ *
+ * RETURN:      None
+ *
+ * DESCRIPTION: Namespace batch execution. Execute predefined names in the
+ *              namespace, up to the max count, if specified.
+ *
+ ******************************************************************************/
+
+void acpi_db_evaluate_predefined_names(void)
+{
+	struct acpi_db_execute_walk info;
+
+	info.count = 0;
+	info.max_count = ACPI_UINT32_MAX;
+
+	/* Search all nodes in namespace */
+
+	(void)acpi_walk_namespace(ACPI_TYPE_ANY, ACPI_ROOT_OBJECT,
+				  ACPI_UINT32_MAX, acpi_db_walk_for_execute,
+				  NULL, (void *)&info, NULL);
+
+	acpi_os_printf("Evaluated %u predefined names in the namespace\n",
+		       info.count);
+}
diff --git a/drivers/acpi/acpica/dbobject.c b/drivers/acpi/acpica/dbobject.c
index 1d59e8b6f859..08eaaf350b24 100644
--- a/drivers/acpi/acpica/dbobject.c
+++ b/drivers/acpi/acpica/dbobject.c
@@ -142,11 +142,11 @@ void acpi_db_decode_internal_object(union acpi_operand_object *obj_desc)
 
 	case ACPI_TYPE_STRING:
 
-		acpi_os_printf("(%u) \"%.24s",
+		acpi_os_printf("(%u) \"%.60s",
 			       obj_desc->string.length,
 			       obj_desc->string.pointer);
 
-		if (obj_desc->string.length > 24) {
+		if (obj_desc->string.length > 60) {
 			acpi_os_printf("...");
 		} else {
 			acpi_os_printf("\"");
diff --git a/drivers/acpi/acpica/dsmethod.c b/drivers/acpi/acpica/dsmethod.c
index 47c7b52a519c..32e9ddc0cf2b 100644
--- a/drivers/acpi/acpica/dsmethod.c
+++ b/drivers/acpi/acpica/dsmethod.c
@@ -99,11 +99,14 @@ acpi_ds_auto_serialize_method(struct acpi_namespace_node *node,
 			  "Method auto-serialization parse [%4.4s] %p\n",
 			  acpi_ut_get_node_name(node), node));
 
+	acpi_ex_enter_interpreter();
+
 	/* Create/Init a root op for the method parse tree */
 
 	op = acpi_ps_alloc_op(AML_METHOD_OP, obj_desc->method.aml_start);
 	if (!op) {
-		return_ACPI_STATUS(AE_NO_MEMORY);
+		status = AE_NO_MEMORY;
+		goto unlock;
 	}
 
 	acpi_ps_set_name(op, node->name.integer);
@@ -115,7 +118,8 @@ acpi_ds_auto_serialize_method(struct acpi_namespace_node *node,
 	    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);
+		status = AE_NO_MEMORY;
+		goto unlock;
 	}
 
 	status = acpi_ds_init_aml_walk(walk_state, op, node,
@@ -134,6 +138,8 @@ acpi_ds_auto_serialize_method(struct acpi_namespace_node *node,
 	status = acpi_ps_parse_aml(walk_state);
 
 	acpi_ps_delete_parse_tree(op);
+unlock:
+	acpi_ex_exit_interpreter();
 	return_ACPI_STATUS(status);
 }
 
@@ -757,8 +763,10 @@ acpi_ds_terminate_control_method(union acpi_operand_object *method_desc,
 
 			/* Delete any direct children of (created by) this method */
 
+			(void)acpi_ex_exit_interpreter();
 			acpi_ns_delete_namespace_subtree(walk_state->
 							 method_node);
+			(void)acpi_ex_enter_interpreter();
 
 			/*
 			 * Delete any objects that were created by this method
@@ -769,9 +777,11 @@ acpi_ds_terminate_control_method(union acpi_operand_object *method_desc,
 			 */
 			if (method_desc->method.
 			    info_flags & ACPI_METHOD_MODIFIED_NAMESPACE) {
+				(void)acpi_ex_exit_interpreter();
 				acpi_ns_delete_namespace_by_owner(method_desc->
 								  method.
 								  owner_id);
+				(void)acpi_ex_enter_interpreter();
 				method_desc->method.info_flags &=
 				    ~ACPI_METHOD_MODIFIED_NAMESPACE;
 			}
diff --git a/drivers/acpi/acpica/dsutils.c b/drivers/acpi/acpica/dsutils.c
index f393de9f5887..7d8ef52fb88d 100644
--- a/drivers/acpi/acpica/dsutils.c
+++ b/drivers/acpi/acpica/dsutils.c
@@ -565,15 +565,14 @@ acpi_ds_create_operand(struct acpi_walk_state *walk_state,
 					status = AE_OK;
 				} else if (parent_op->common.aml_opcode ==
 					   AML_EXTERNAL_OP) {
-
-					/* TBD: May only be temporary */
-
-					obj_desc =
-					    acpi_ut_create_string_object((acpi_size)name_length);
-
-					strncpy(obj_desc->string.pointer,
-						name_string, name_length);
-					status = AE_OK;
+					/*
+					 * This opcode should never appear here. It is used only
+					 * by AML disassemblers and is surrounded by an If(0)
+					 * by the ASL compiler.
+					 *
+					 * Therefore, if we see it here, it is a serious error.
+					 */
+					status = AE_AML_BAD_OPCODE;
 				} else {
 					/*
 					 * We just plain didn't find it -- which is a
diff --git a/drivers/acpi/acpica/dswexec.c b/drivers/acpi/acpica/dswexec.c
index 402ecc590c56..438597cf6cc5 100644
--- a/drivers/acpi/acpica/dswexec.c
+++ b/drivers/acpi/acpica/dswexec.c
@@ -133,7 +133,8 @@ acpi_ds_get_predicate_value(struct acpi_walk_state *walk_state,
 	 * Result of predicate evaluation must be an Integer
 	 * object. Implicitly convert the argument if necessary.
 	 */
-	status = acpi_ex_convert_to_integer(obj_desc, &local_obj_desc, 16);
+	status = acpi_ex_convert_to_integer(obj_desc, &local_obj_desc,
+					    ACPI_STRTOUL_BASE16);
 	if (ACPI_FAILURE(status)) {
 		goto cleanup;
 	}
diff --git a/drivers/acpi/acpica/dswload2.c b/drivers/acpi/acpica/dswload2.c
index 762db3fa70e0..028b22a3154e 100644
--- a/drivers/acpi/acpica/dswload2.c
+++ b/drivers/acpi/acpica/dswload2.c
@@ -605,16 +605,13 @@ acpi_status acpi_ds_load2_end_op(struct acpi_walk_state *walk_state)
 				if (ACPI_FAILURE(status)) {
 					return_ACPI_STATUS(status);
 				}
-
-				acpi_ex_exit_interpreter();
 			}
 
+			acpi_ex_exit_interpreter();
 			status =
 			    acpi_ev_initialize_region
 			    (acpi_ns_get_attached_object(node), FALSE);
-			if (walk_state->method_node) {
-				acpi_ex_enter_interpreter();
-			}
+			acpi_ex_enter_interpreter();
 
 			if (ACPI_FAILURE(status)) {
 				/*
diff --git a/drivers/acpi/acpica/evgpe.c b/drivers/acpi/acpica/evgpe.c
index 4b4949ce05bc..bdb10bee13ce 100644
--- a/drivers/acpi/acpica/evgpe.c
+++ b/drivers/acpi/acpica/evgpe.c
@@ -130,6 +130,60 @@ acpi_status acpi_ev_enable_gpe(struct acpi_gpe_event_info *gpe_event_info)
 
 /*******************************************************************************
  *
+ * FUNCTION:    acpi_ev_mask_gpe
+ *
+ * PARAMETERS:  gpe_event_info          - GPE to be blocked/unblocked
+ *              is_masked               - Whether the GPE is masked or not
+ *
+ * RETURN:      Status
+ *
+ * DESCRIPTION: Unconditionally mask/unmask a GPE during runtime.
+ *
+ ******************************************************************************/
+
+acpi_status
+acpi_ev_mask_gpe(struct acpi_gpe_event_info *gpe_event_info, u8 is_masked)
+{
+	struct acpi_gpe_register_info *gpe_register_info;
+	u32 register_bit;
+
+	ACPI_FUNCTION_TRACE(ev_mask_gpe);
+
+	gpe_register_info = gpe_event_info->register_info;
+	if (!gpe_register_info) {
+		return_ACPI_STATUS(AE_NOT_EXIST);
+	}
+
+	register_bit = acpi_hw_get_gpe_register_bit(gpe_event_info);
+
+	/* Perform the action */
+
+	if (is_masked) {
+		if (register_bit & gpe_register_info->mask_for_run) {
+			return_ACPI_STATUS(AE_BAD_PARAMETER);
+		}
+
+		(void)acpi_hw_low_set_gpe(gpe_event_info, ACPI_GPE_DISABLE);
+		ACPI_SET_BIT(gpe_register_info->mask_for_run, (u8)register_bit);
+	} else {
+		if (!(register_bit & gpe_register_info->mask_for_run)) {
+			return_ACPI_STATUS(AE_BAD_PARAMETER);
+		}
+
+		ACPI_CLEAR_BIT(gpe_register_info->mask_for_run,
+			       (u8)register_bit);
+		if (gpe_event_info->runtime_count
+		    && !gpe_event_info->disable_for_dispatch) {
+			(void)acpi_hw_low_set_gpe(gpe_event_info,
+						  ACPI_GPE_ENABLE);
+		}
+	}
+
+	return_ACPI_STATUS(AE_OK);
+}
+
+/*******************************************************************************
+ *
  * FUNCTION:    acpi_ev_add_gpe_reference
  *
  * PARAMETERS:  gpe_event_info          - Add a reference to this GPE
@@ -674,6 +728,7 @@ acpi_status acpi_ev_finish_gpe(struct acpi_gpe_event_info *gpe_event_info)
 	 * in the event_info.
 	 */
 	(void)acpi_hw_low_set_gpe(gpe_event_info, ACPI_GPE_CONDITIONAL_ENABLE);
+	gpe_event_info->disable_for_dispatch = FALSE;
 	return (AE_OK);
 }
 
@@ -737,6 +792,8 @@ acpi_ev_gpe_dispatch(struct acpi_namespace_node *gpe_device,
 		}
 	}
 
+	gpe_event_info->disable_for_dispatch = TRUE;
+
 	/*
 	 * Dispatch the GPE to either an installed handler or the control
 	 * method associated with this GPE (_Lxx or _Exx). If a handler
diff --git a/drivers/acpi/acpica/evgpeinit.c b/drivers/acpi/acpica/evgpeinit.c
index 7dc75474c897..16ce4835e7d0 100644
--- a/drivers/acpi/acpica/evgpeinit.c
+++ b/drivers/acpi/acpica/evgpeinit.c
@@ -323,7 +323,9 @@ acpi_ev_match_gpe_method(acpi_handle obj_handle,
 	struct acpi_gpe_walk_info *walk_info =
 	    ACPI_CAST_PTR(struct acpi_gpe_walk_info, context);
 	struct acpi_gpe_event_info *gpe_event_info;
+	acpi_status status;
 	u32 gpe_number;
+	u8 temp_gpe_number;
 	char name[ACPI_NAME_SIZE + 1];
 	u8 type;
 
@@ -377,8 +379,8 @@ acpi_ev_match_gpe_method(acpi_handle obj_handle,
 
 	/* 4) The last two characters of the name are the hex GPE Number */
 
-	gpe_number = strtoul(&name[2], NULL, 16);
-	if (gpe_number == ACPI_UINT32_MAX) {
+	status = acpi_ut_ascii_to_hex_byte(&name[2], &temp_gpe_number);
+	if (ACPI_FAILURE(status)) {
 
 		/* Conversion failed; invalid method, just ignore it */
 
@@ -390,6 +392,7 @@ acpi_ev_match_gpe_method(acpi_handle obj_handle,
 
 	/* Ensure that we have a valid GPE number for this GPE block */
 
+	gpe_number = (u32)temp_gpe_number;
 	gpe_event_info =
 	    acpi_ev_low_get_gpe_info(gpe_number, walk_info->gpe_block);
 	if (!gpe_event_info) {
diff --git a/drivers/acpi/acpica/evrgnini.c b/drivers/acpi/acpica/evrgnini.c
index b6ea9c0d0d8c..3843f1fc5dbb 100644
--- a/drivers/acpi/acpica/evrgnini.c
+++ b/drivers/acpi/acpica/evrgnini.c
@@ -553,7 +553,8 @@ acpi_ev_initialize_region(union acpi_operand_object *region_obj,
 				 *
 				 * See acpi_ns_exec_module_code
 				 */
-				if (obj_desc->method.
+				if (!acpi_gbl_parse_table_as_term_list &&
+				    obj_desc->method.
 				    info_flags & ACPI_METHOD_MODULE_LEVEL) {
 					handler_obj =
 					    obj_desc->method.dispatch.handler;
diff --git a/drivers/acpi/acpica/evxfgpe.c b/drivers/acpi/acpica/evxfgpe.c
index 17cfef721d00..d7a3b2775505 100644
--- a/drivers/acpi/acpica/evxfgpe.c
+++ b/drivers/acpi/acpica/evxfgpe.c
@@ -235,11 +235,13 @@ acpi_status acpi_set_gpe(acpi_handle gpe_device, u32 gpe_number, u8 action)
 	case ACPI_GPE_ENABLE:
 
 		status = acpi_hw_low_set_gpe(gpe_event_info, ACPI_GPE_ENABLE);
+		gpe_event_info->disable_for_dispatch = FALSE;
 		break;
 
 	case ACPI_GPE_DISABLE:
 
 		status = acpi_hw_low_set_gpe(gpe_event_info, ACPI_GPE_DISABLE);
+		gpe_event_info->disable_for_dispatch = TRUE;
 		break;
 
 	default:
@@ -257,6 +259,47 @@ ACPI_EXPORT_SYMBOL(acpi_set_gpe)
 
 /*******************************************************************************
  *
+ * FUNCTION:    acpi_mask_gpe
+ *
+ * PARAMETERS:  gpe_device          - Parent GPE Device. NULL for GPE0/GPE1
+ *              gpe_number          - GPE level within the GPE block
+ *              is_masked           - Whether the GPE is masked or not
+ *
+ * RETURN:      Status
+ *
+ * DESCRIPTION: Unconditionally mask/unmask the an individual GPE, ex., to
+ *              prevent a GPE flooding.
+ *
+ ******************************************************************************/
+acpi_status acpi_mask_gpe(acpi_handle gpe_device, u32 gpe_number, u8 is_masked)
+{
+	struct acpi_gpe_event_info *gpe_event_info;
+	acpi_status status;
+	acpi_cpu_flags flags;
+
+	ACPI_FUNCTION_TRACE(acpi_mask_gpe);
+
+	flags = acpi_os_acquire_lock(acpi_gbl_gpe_lock);
+
+	/* Ensure that we have a valid GPE number */
+
+	gpe_event_info = acpi_ev_get_gpe_event_info(gpe_device, gpe_number);
+	if (!gpe_event_info) {
+		status = AE_BAD_PARAMETER;
+		goto unlock_and_exit;
+	}
+
+	status = acpi_ev_mask_gpe(gpe_event_info, is_masked);
+
+unlock_and_exit:
+	acpi_os_release_lock(acpi_gbl_gpe_lock, flags);
+	return_ACPI_STATUS(status);
+}
+
+ACPI_EXPORT_SYMBOL(acpi_mask_gpe)
+
+/*******************************************************************************
+ *
  * FUNCTION:    acpi_mark_gpe_for_wake
  *
  * PARAMETERS:  gpe_device          - Parent GPE Device. NULL for GPE0/GPE1
diff --git a/drivers/acpi/acpica/exconcat.c b/drivers/acpi/acpica/exconcat.c
index 2423fe03e879..5429c2a6bc41 100644
--- a/drivers/acpi/acpica/exconcat.c
+++ b/drivers/acpi/acpica/exconcat.c
@@ -156,7 +156,7 @@ acpi_ex_do_concatenate(union acpi_operand_object *operand0,
 
 		status =
 		    acpi_ex_convert_to_integer(local_operand1, &temp_operand1,
-					       16);
+					       ACPI_STRTOUL_BASE16);
 		break;
 
 	case ACPI_TYPE_BUFFER:
diff --git a/drivers/acpi/acpica/exconfig.c b/drivers/acpi/acpica/exconfig.c
index a1d177d58254..718428ba0b89 100644
--- a/drivers/acpi/acpica/exconfig.c
+++ b/drivers/acpi/acpica/exconfig.c
@@ -55,9 +55,7 @@ ACPI_MODULE_NAME("exconfig")
 
 /* Local prototypes */
 static acpi_status
-acpi_ex_add_table(u32 table_index,
-		  struct acpi_namespace_node *parent_node,
-		  union acpi_operand_object **ddb_handle);
+acpi_ex_add_table(u32 table_index, union acpi_operand_object **ddb_handle);
 
 static acpi_status
 acpi_ex_region_read(union acpi_operand_object *obj_desc,
@@ -79,13 +77,9 @@ acpi_ex_region_read(union acpi_operand_object *obj_desc,
  ******************************************************************************/
 
 static acpi_status
-acpi_ex_add_table(u32 table_index,
-		  struct acpi_namespace_node *parent_node,
-		  union acpi_operand_object **ddb_handle)
+acpi_ex_add_table(u32 table_index, union acpi_operand_object **ddb_handle)
 {
 	union acpi_operand_object *obj_desc;
-	acpi_status status;
-	acpi_owner_id owner_id;
 
 	ACPI_FUNCTION_TRACE(ex_add_table);
 
@@ -100,39 +94,8 @@ acpi_ex_add_table(u32 table_index,
 
 	obj_desc->common.flags |= AOPOBJ_DATA_VALID;
 	obj_desc->reference.class = ACPI_REFCLASS_TABLE;
-	*ddb_handle = obj_desc;
-
-	/* Install the new table into the local data structures */
-
 	obj_desc->reference.value = table_index;
-
-	/* Add the table to the namespace */
-
-	status = acpi_ns_load_table(table_index, parent_node);
-	if (ACPI_FAILURE(status)) {
-		acpi_ut_remove_reference(obj_desc);
-		*ddb_handle = NULL;
-		return_ACPI_STATUS(status);
-	}
-
-	/* Execute any module-level code that was found in the table */
-
-	acpi_ex_exit_interpreter();
-	if (acpi_gbl_group_module_level_code) {
-		acpi_ns_exec_module_code_list();
-	}
-	acpi_ex_enter_interpreter();
-
-	/*
-	 * Update GPEs for any new _Lxx/_Exx methods. Ignore errors. The host is
-	 * responsible for discovering any new wake GPEs by running _PRW methods
-	 * that may have been loaded by this table.
-	 */
-	status = acpi_tb_get_owner_id(table_index, &owner_id);
-	if (ACPI_SUCCESS(status)) {
-		acpi_ev_update_gpes(owner_id);
-	}
-
+	*ddb_handle = obj_desc;
 	return_ACPI_STATUS(AE_OK);
 }
 
@@ -159,16 +122,17 @@ acpi_ex_load_table_op(struct acpi_walk_state *walk_state,
 	struct acpi_namespace_node *start_node;
 	struct acpi_namespace_node *parameter_node = NULL;
 	union acpi_operand_object *ddb_handle;
-	struct acpi_table_header *table;
 	u32 table_index;
 
 	ACPI_FUNCTION_TRACE(ex_load_table_op);
 
 	/* Find the ACPI table in the RSDT/XSDT */
 
+	acpi_ex_exit_interpreter();
 	status = acpi_tb_find_table(operand[0]->string.pointer,
 				    operand[1]->string.pointer,
 				    operand[2]->string.pointer, &table_index);
+	acpi_ex_enter_interpreter();
 	if (ACPI_FAILURE(status)) {
 		if (status != AE_NOT_FOUND) {
 			return_ACPI_STATUS(status);
@@ -197,9 +161,10 @@ acpi_ex_load_table_op(struct acpi_walk_state *walk_state,
 		 * Find the node referenced by the root_path_string. This is the
 		 * location within the namespace where the table will be loaded.
 		 */
-		status =
-		    acpi_ns_get_node(start_node, operand[3]->string.pointer,
-				     ACPI_NS_SEARCH_PARENT, &parent_node);
+		status = acpi_ns_get_node_unlocked(start_node,
+						   operand[3]->string.pointer,
+						   ACPI_NS_SEARCH_PARENT,
+						   &parent_node);
 		if (ACPI_FAILURE(status)) {
 			return_ACPI_STATUS(status);
 		}
@@ -219,9 +184,10 @@ acpi_ex_load_table_op(struct acpi_walk_state *walk_state,
 
 		/* Find the node referenced by the parameter_path_string */
 
-		status =
-		    acpi_ns_get_node(start_node, operand[4]->string.pointer,
-				     ACPI_NS_SEARCH_PARENT, &parameter_node);
+		status = acpi_ns_get_node_unlocked(start_node,
+						   operand[4]->string.pointer,
+						   ACPI_NS_SEARCH_PARENT,
+						   &parameter_node);
 		if (ACPI_FAILURE(status)) {
 			return_ACPI_STATUS(status);
 		}
@@ -229,7 +195,15 @@ acpi_ex_load_table_op(struct acpi_walk_state *walk_state,
 
 	/* Load the table into the namespace */
 
-	status = acpi_ex_add_table(table_index, parent_node, &ddb_handle);
+	ACPI_INFO(("Dynamic OEM Table Load:"));
+	acpi_ex_exit_interpreter();
+	status = acpi_tb_load_table(table_index, parent_node);
+	acpi_ex_enter_interpreter();
+	if (ACPI_FAILURE(status)) {
+		return_ACPI_STATUS(status);
+	}
+
+	status = acpi_ex_add_table(table_index, &ddb_handle);
 	if (ACPI_FAILURE(status)) {
 		return_ACPI_STATUS(status);
 	}
@@ -252,19 +226,6 @@ acpi_ex_load_table_op(struct acpi_walk_state *walk_state,
 		}
 	}
 
-	status = acpi_get_table_by_index(table_index, &table);
-	if (ACPI_SUCCESS(status)) {
-		ACPI_INFO(("Dynamic OEM Table Load:"));
-		acpi_tb_print_table_header(0, table);
-	}
-
-	/* Invoke table handler if present */
-
-	if (acpi_gbl_table_handler) {
-		(void)acpi_gbl_table_handler(ACPI_TABLE_EVENT_LOAD, table,
-					     acpi_gbl_table_handler_context);
-	}
-
 	*return_desc = ddb_handle;
 	return_ACPI_STATUS(status);
 }
@@ -475,13 +436,12 @@ acpi_ex_load_op(union acpi_operand_object *obj_desc,
 	/* Install the new table into the local data structures */
 
 	ACPI_INFO(("Dynamic OEM Table Load:"));
-	(void)acpi_ut_acquire_mutex(ACPI_MTX_TABLES);
-
-	status = acpi_tb_install_standard_table(ACPI_PTR_TO_PHYSADDR(table),
-						ACPI_TABLE_ORIGIN_INTERNAL_VIRTUAL,
-						TRUE, TRUE, &table_index);
-
-	(void)acpi_ut_release_mutex(ACPI_MTX_TABLES);
+	acpi_ex_exit_interpreter();
+	status =
+	    acpi_tb_install_and_load_table(table, ACPI_PTR_TO_PHYSADDR(table),
+					   ACPI_TABLE_ORIGIN_INTERNAL_VIRTUAL,
+					   TRUE, &table_index);
+	acpi_ex_enter_interpreter();
 	if (ACPI_FAILURE(status)) {
 
 		/* Delete allocated table buffer */
@@ -491,25 +451,13 @@ acpi_ex_load_op(union acpi_operand_object *obj_desc,
 	}
 
 	/*
-	 * Note: Now table is "INSTALLED", it must be validated before
-	 * loading.
-	 */
-	status =
-	    acpi_tb_validate_table(&acpi_gbl_root_table_list.
-				   tables[table_index]);
-	if (ACPI_FAILURE(status)) {
-		return_ACPI_STATUS(status);
-	}
-
-	/*
 	 * Add the table to the namespace.
 	 *
 	 * Note: Load the table objects relative to the root of the namespace.
 	 * This appears to go against the ACPI specification, but we do it for
 	 * compatibility with other ACPI implementations.
 	 */
-	status =
-	    acpi_ex_add_table(table_index, acpi_gbl_root_node, &ddb_handle);
+	status = acpi_ex_add_table(table_index, &ddb_handle);
 	if (ACPI_FAILURE(status)) {
 
 		/* On error, table_ptr was deallocated above */
@@ -532,14 +480,6 @@ acpi_ex_load_op(union acpi_operand_object *obj_desc,
 	/* Remove the reference by added by acpi_ex_store above */
 
 	acpi_ut_remove_reference(ddb_handle);
-
-	/* Invoke table handler if present */
-
-	if (acpi_gbl_table_handler) {
-		(void)acpi_gbl_table_handler(ACPI_TABLE_EVENT_LOAD, table,
-					     acpi_gbl_table_handler_context);
-	}
-
 	return_ACPI_STATUS(status);
 }
 
@@ -592,10 +532,17 @@ acpi_status acpi_ex_unload_table(union acpi_operand_object *ddb_handle)
 
 	table_index = table_desc->reference.value;
 
+	/*
+	 * Release the interpreter lock so that the table lock won't have
+	 * strict order requirement against it.
+	 */
+	acpi_ex_exit_interpreter();
+
 	/* Ensure the table is still loaded */
 
 	if (!acpi_tb_is_table_loaded(table_index)) {
-		return_ACPI_STATUS(AE_NOT_EXIST);
+		status = AE_NOT_EXIST;
+		goto lock_and_exit;
 	}
 
 	/* Invoke table handler if present */
@@ -613,16 +560,24 @@ acpi_status acpi_ex_unload_table(union acpi_operand_object *ddb_handle)
 
 	status = acpi_tb_delete_namespace_by_owner(table_index);
 	if (ACPI_FAILURE(status)) {
-		return_ACPI_STATUS(status);
+		goto lock_and_exit;
 	}
 
 	(void)acpi_tb_release_owner_id(table_index);
 	acpi_tb_set_table_loaded_flag(table_index, FALSE);
 
+lock_and_exit:
+
+	/* Re-acquire the interpreter lock */
+
+	acpi_ex_enter_interpreter();
+
 	/*
 	 * Invalidate the handle. We do this because the handle may be stored
 	 * in a named object and may not be actually deleted until much later.
 	 */
-	ddb_handle->common.flags &= ~AOPOBJ_DATA_VALID;
-	return_ACPI_STATUS(AE_OK);
+	if (ACPI_SUCCESS(status)) {
+		ddb_handle->common.flags &= ~AOPOBJ_DATA_VALID;
+	}
+	return_ACPI_STATUS(status);
 }
diff --git a/drivers/acpi/acpica/exconvrt.c b/drivers/acpi/acpica/exconvrt.c
index b7e9b3d803e1..588ad1409dbe 100644
--- a/drivers/acpi/acpica/exconvrt.c
+++ b/drivers/acpi/acpica/exconvrt.c
@@ -124,9 +124,9 @@ acpi_ex_convert_to_integer(union acpi_operand_object *obj_desc,
 		 * of ACPI 3.0) is that the to_integer() operator allows both decimal
 		 * and hexadecimal strings (hex prefixed with "0x").
 		 */
-		status = acpi_ut_strtoul64((char *)pointer, flags,
-					   acpi_gbl_integer_byte_width,
-					   &result);
+		status = acpi_ut_strtoul64(ACPI_CAST_PTR(char, pointer),
+					   (acpi_gbl_integer_byte_width |
+					    flags), &result);
 		if (ACPI_FAILURE(status)) {
 			return_ACPI_STATUS(status);
 		}
@@ -632,7 +632,7 @@ acpi_ex_convert_to_target_type(acpi_object_type destination_type,
 			 */
 			status =
 			    acpi_ex_convert_to_integer(source_desc, result_desc,
-						       16);
+						       ACPI_STRTOUL_BASE16);
 			break;
 
 		case ACPI_TYPE_STRING:
diff --git a/drivers/acpi/acpica/exmisc.c b/drivers/acpi/acpica/exmisc.c
index 4f7e667624b3..37c88b424a02 100644
--- a/drivers/acpi/acpica/exmisc.c
+++ b/drivers/acpi/acpica/exmisc.c
@@ -327,8 +327,8 @@ acpi_ex_do_logical_op(u16 opcode,
 	switch (operand0->common.type) {
 	case ACPI_TYPE_INTEGER:
 
-		status =
-		    acpi_ex_convert_to_integer(operand1, &local_operand1, 16);
+		status = acpi_ex_convert_to_integer(operand1, &local_operand1,
+						    ACPI_STRTOUL_BASE16);
 		break;
 
 	case ACPI_TYPE_STRING:
diff --git a/drivers/acpi/acpica/exoparg1.c b/drivers/acpi/acpica/exoparg1.c
index 4e17506a7384..007300433cde 100644
--- a/drivers/acpi/acpica/exoparg1.c
+++ b/drivers/acpi/acpica/exoparg1.c
@@ -521,9 +521,10 @@ acpi_status acpi_ex_opcode_1A_1T_1R(struct acpi_walk_state *walk_state)
 
 	case AML_TO_INTEGER_OP:	/* to_integer (Data, Result) */
 
+		/* Perform "explicit" conversion */
+
 		status =
-		    acpi_ex_convert_to_integer(operand[0], &return_desc,
-					       ACPI_ANY_BASE);
+		    acpi_ex_convert_to_integer(operand[0], &return_desc, 0);
 		if (return_desc == operand[0]) {
 
 			/* No conversion performed, add ref to handle return value */
@@ -890,14 +891,16 @@ acpi_status acpi_ex_opcode_1A_0T_1R(struct acpi_walk_state *walk_state)
 				 *    Field, so we need to resolve the node to a value.
 				 */
 				status =
-				    acpi_ns_get_node(walk_state->scope_info->
-						     scope.node,
-						     operand[0]->string.pointer,
-						     ACPI_NS_SEARCH_PARENT,
-						     ACPI_CAST_INDIRECT_PTR
-						     (struct
-						      acpi_namespace_node,
-						      &return_desc));
+				    acpi_ns_get_node_unlocked(walk_state->
+							      scope_info->scope.
+							      node,
+							      operand[0]->
+							      string.pointer,
+							      ACPI_NS_SEARCH_PARENT,
+							      ACPI_CAST_INDIRECT_PTR
+							      (struct
+							       acpi_namespace_node,
+							       &return_desc));
 				if (ACPI_FAILURE(status)) {
 					goto cleanup;
 				}
diff --git a/drivers/acpi/acpica/exresop.c b/drivers/acpi/acpica/exresop.c
index 27b41fd7542d..f29eba1dc5e9 100644
--- a/drivers/acpi/acpica/exresop.c
+++ b/drivers/acpi/acpica/exresop.c
@@ -410,12 +410,13 @@ acpi_ex_resolve_operands(u16 opcode,
 		case ARGI_INTEGER:
 
 			/*
-			 * Need an operand of type ACPI_TYPE_INTEGER,
-			 * But we can implicitly convert from a STRING or BUFFER
-			 * aka - "Implicit Source Operand Conversion"
+			 * Need an operand of type ACPI_TYPE_INTEGER, but we can
+			 * implicitly convert from a STRING or BUFFER.
+			 *
+			 * Known as "Implicit Source Operand Conversion"
 			 */
-			status =
-			    acpi_ex_convert_to_integer(obj_desc, stack_ptr, 16);
+			status = acpi_ex_convert_to_integer(obj_desc, stack_ptr,
+							    ACPI_STRTOUL_BASE16);
 			if (ACPI_FAILURE(status)) {
 				if (status == AE_TYPE) {
 					ACPI_ERROR((AE_INFO,
diff --git a/drivers/acpi/acpica/extrace.c b/drivers/acpi/acpica/extrace.c
index b52e84841c1a..c9ca82610d77 100644
--- a/drivers/acpi/acpica/extrace.c
+++ b/drivers/acpi/acpica/extrace.c
@@ -201,7 +201,6 @@ acpi_ex_start_trace_method(struct acpi_namespace_node *method_node,
 			   union acpi_operand_object *obj_desc,
 			   struct acpi_walk_state *walk_state)
 {
-	acpi_status status;
 	char *pathname = NULL;
 	u8 enabled = FALSE;
 
@@ -211,11 +210,6 @@ acpi_ex_start_trace_method(struct acpi_namespace_node *method_node,
 		pathname = acpi_ns_get_normalized_pathname(method_node, TRUE);
 	}
 
-	status = acpi_ut_acquire_mutex(ACPI_MTX_NAMESPACE);
-	if (ACPI_FAILURE(status)) {
-		goto exit;
-	}
-
 	enabled = acpi_ex_interpreter_trace_enabled(pathname);
 	if (enabled && !acpi_gbl_trace_method_object) {
 		acpi_gbl_trace_method_object = obj_desc;
@@ -233,9 +227,6 @@ acpi_ex_start_trace_method(struct acpi_namespace_node *method_node,
 		}
 	}
 
-	(void)acpi_ut_release_mutex(ACPI_MTX_NAMESPACE);
-
-exit:
 	if (enabled) {
 		ACPI_TRACE_POINT(ACPI_TRACE_AML_METHOD, TRUE,
 				 obj_desc ? obj_desc->method.aml_start : NULL,
@@ -267,7 +258,6 @@ acpi_ex_stop_trace_method(struct acpi_namespace_node *method_node,
 			  union acpi_operand_object *obj_desc,
 			  struct acpi_walk_state *walk_state)
 {
-	acpi_status status;
 	char *pathname = NULL;
 	u8 enabled;
 
@@ -277,26 +267,14 @@ acpi_ex_stop_trace_method(struct acpi_namespace_node *method_node,
 		pathname = acpi_ns_get_normalized_pathname(method_node, TRUE);
 	}
 
-	status = acpi_ut_acquire_mutex(ACPI_MTX_NAMESPACE);
-	if (ACPI_FAILURE(status)) {
-		goto exit_path;
-	}
-
 	enabled = acpi_ex_interpreter_trace_enabled(NULL);
 
-	(void)acpi_ut_release_mutex(ACPI_MTX_NAMESPACE);
-
 	if (enabled) {
 		ACPI_TRACE_POINT(ACPI_TRACE_AML_METHOD, FALSE,
 				 obj_desc ? obj_desc->method.aml_start : NULL,
 				 pathname);
 	}
 
-	status = acpi_ut_acquire_mutex(ACPI_MTX_NAMESPACE);
-	if (ACPI_FAILURE(status)) {
-		goto exit_path;
-	}
-
 	/* Check whether the tracer should be stopped */
 
 	if (acpi_gbl_trace_method_object == obj_desc) {
@@ -312,9 +290,6 @@ acpi_ex_stop_trace_method(struct acpi_namespace_node *method_node,
 		acpi_gbl_trace_method_object = NULL;
 	}
 
-	(void)acpi_ut_release_mutex(ACPI_MTX_NAMESPACE);
-
-exit_path:
 	if (pathname) {
 		ACPI_FREE(pathname);
 	}
diff --git a/drivers/acpi/acpica/exutils.c b/drivers/acpi/acpica/exutils.c
index 425f13372e68..a8b857a7e9fb 100644
--- a/drivers/acpi/acpica/exutils.c
+++ b/drivers/acpi/acpica/exutils.c
@@ -94,6 +94,10 @@ void acpi_ex_enter_interpreter(void)
 		ACPI_ERROR((AE_INFO,
 			    "Could not acquire AML Interpreter mutex"));
 	}
+	status = acpi_ut_acquire_mutex(ACPI_MTX_NAMESPACE);
+	if (ACPI_FAILURE(status)) {
+		ACPI_ERROR((AE_INFO, "Could not acquire AML Namespace mutex"));
+	}
 
 	return_VOID;
 }
@@ -127,6 +131,10 @@ void acpi_ex_exit_interpreter(void)
 
 	ACPI_FUNCTION_TRACE(ex_exit_interpreter);
 
+	status = acpi_ut_release_mutex(ACPI_MTX_NAMESPACE);
+	if (ACPI_FAILURE(status)) {
+		ACPI_ERROR((AE_INFO, "Could not release AML Namespace mutex"));
+	}
 	status = acpi_ut_release_mutex(ACPI_MTX_INTERPRETER);
 	if (ACPI_FAILURE(status)) {
 		ACPI_ERROR((AE_INFO,
diff --git a/drivers/acpi/acpica/hwgpe.c b/drivers/acpi/acpica/hwgpe.c
index bdecd5e76e87..76b0e350f5bb 100644
--- a/drivers/acpi/acpica/hwgpe.c
+++ b/drivers/acpi/acpica/hwgpe.c
@@ -98,7 +98,7 @@ acpi_status
 acpi_hw_low_set_gpe(struct acpi_gpe_event_info *gpe_event_info, u32 action)
 {
 	struct acpi_gpe_register_info *gpe_register_info;
-	acpi_status status;
+	acpi_status status = AE_OK;
 	u32 enable_mask;
 	u32 register_bit;
 
@@ -148,9 +148,14 @@ acpi_hw_low_set_gpe(struct acpi_gpe_event_info *gpe_event_info, u32 action)
 		return (AE_BAD_PARAMETER);
 	}
 
-	/* Write the updated enable mask */
+	if (!(register_bit & gpe_register_info->mask_for_run)) {
 
-	status = acpi_hw_write(enable_mask, &gpe_register_info->enable_address);
+		/* Write the updated enable mask */
+
+		status =
+		    acpi_hw_write(enable_mask,
+				  &gpe_register_info->enable_address);
+	}
 	return (status);
 }
 
@@ -242,6 +247,12 @@ acpi_hw_get_gpe_status(struct acpi_gpe_event_info *gpe_event_info,
 		local_event_status |= ACPI_EVENT_FLAG_ENABLED;
 	}
 
+	/* GPE currently masked? (masked for runtime?) */
+
+	if (register_bit & gpe_register_info->mask_for_run) {
+		local_event_status |= ACPI_EVENT_FLAG_MASKED;
+	}
+
 	/* GPE enabled for wake? */
 
 	if (register_bit & gpe_register_info->enable_for_wake) {
@@ -397,6 +408,7 @@ acpi_hw_enable_runtime_gpe_block(struct acpi_gpe_xrupt_info *gpe_xrupt_info,
 	u32 i;
 	acpi_status status;
 	struct acpi_gpe_register_info *gpe_register_info;
+	u8 enable_mask;
 
 	/* NOTE: assumes that all GPEs are currently disabled */
 
@@ -410,9 +422,10 @@ acpi_hw_enable_runtime_gpe_block(struct acpi_gpe_xrupt_info *gpe_xrupt_info,
 
 		/* Enable all "runtime" GPEs in this register */
 
+		enable_mask = gpe_register_info->enable_for_run &
+		    ~gpe_register_info->mask_for_run;
 		status =
-		    acpi_hw_gpe_enable_write(gpe_register_info->enable_for_run,
-					     gpe_register_info);
+		    acpi_hw_gpe_enable_write(enable_mask, gpe_register_info);
 		if (ACPI_FAILURE(status)) {
 			return (status);
 		}
diff --git a/drivers/acpi/acpica/nsaccess.c b/drivers/acpi/acpica/nsaccess.c
index 426a6307eafa..73f98d3fed25 100644
--- a/drivers/acpi/acpica/nsaccess.c
+++ b/drivers/acpi/acpica/nsaccess.c
@@ -108,9 +108,9 @@ acpi_status acpi_ns_root_initialize(void)
 		}
 
 		status =
-		    acpi_ns_lookup(NULL, (char *)init_val->name, init_val->type,
-				   ACPI_IMODE_LOAD_PASS2, ACPI_NS_NO_UPSEARCH,
-				   NULL, &new_node);
+		    acpi_ns_lookup(NULL, ACPI_CAST_PTR(char, init_val->name),
+				   init_val->type, ACPI_IMODE_LOAD_PASS2,
+				   ACPI_NS_NO_UPSEARCH, NULL, &new_node);
 		if (ACPI_FAILURE(status)) {
 			ACPI_EXCEPTION((AE_INFO, status,
 					"Could not create predefined name %s",
diff --git a/drivers/acpi/acpica/nsconvert.c b/drivers/acpi/acpica/nsconvert.c
index c803bda7915c..2b85dee6d4c0 100644
--- a/drivers/acpi/acpica/nsconvert.c
+++ b/drivers/acpi/acpica/nsconvert.c
@@ -79,7 +79,6 @@ acpi_ns_convert_to_integer(union acpi_operand_object *original_object,
 		/* String-to-Integer conversion */
 
 		status = acpi_ut_strtoul64(original_object->string.pointer,
-					   ACPI_ANY_BASE,
 					   acpi_gbl_integer_byte_width, &value);
 		if (ACPI_FAILURE(status)) {
 			return (status);
diff --git a/drivers/acpi/acpica/nsdump.c b/drivers/acpi/acpica/nsdump.c
index ce1f8605d996..84f35dd27033 100644
--- a/drivers/acpi/acpica/nsdump.c
+++ b/drivers/acpi/acpica/nsdump.c
@@ -338,7 +338,7 @@ acpi_ns_dump_one_object(acpi_handle obj_handle,
 		case ACPI_TYPE_STRING:
 
 			acpi_os_printf("Len %.2X ", obj_desc->string.length);
-			acpi_ut_print_string(obj_desc->string.pointer, 32);
+			acpi_ut_print_string(obj_desc->string.pointer, 80);
 			acpi_os_printf("\n");
 			break;
 
diff --git a/drivers/acpi/acpica/nsload.c b/drivers/acpi/acpica/nsload.c
index b5e2b0ada0ab..334d3c5ba617 100644
--- a/drivers/acpi/acpica/nsload.c
+++ b/drivers/acpi/acpica/nsload.c
@@ -46,6 +46,7 @@
 #include "acnamesp.h"
 #include "acdispat.h"
 #include "actables.h"
+#include "acinterp.h"
 
 #define _COMPONENT          ACPI_NAMESPACE
 ACPI_MODULE_NAME("nsload")
@@ -78,20 +79,6 @@ acpi_ns_load_table(u32 table_index, struct acpi_namespace_node *node)
 
 	ACPI_FUNCTION_TRACE(ns_load_table);
 
-	/*
-	 * Parse the table and load the namespace with all named
-	 * objects found within. Control methods are NOT parsed
-	 * at this time. In fact, the control methods cannot be
-	 * parsed until the entire namespace is loaded, because
-	 * if a control method makes a forward reference (call)
-	 * to another control method, we can't continue parsing
-	 * because we don't know how many arguments to parse next!
-	 */
-	status = acpi_ut_acquire_mutex(ACPI_MTX_NAMESPACE);
-	if (ACPI_FAILURE(status)) {
-		return_ACPI_STATUS(status);
-	}
-
 	/* If table already loaded into namespace, just return */
 
 	if (acpi_tb_is_table_loaded(table_index)) {
@@ -107,6 +94,15 @@ acpi_ns_load_table(u32 table_index, struct acpi_namespace_node *node)
 		goto unlock;
 	}
 
+	/*
+	 * Parse the table and load the namespace with all named
+	 * objects found within. Control methods are NOT parsed
+	 * at this time. In fact, the control methods cannot be
+	 * parsed until the entire namespace is loaded, because
+	 * if a control method makes a forward reference (call)
+	 * to another control method, we can't continue parsing
+	 * because we don't know how many arguments to parse next!
+	 */
 	status = acpi_ns_parse_table(table_index, node);
 	if (ACPI_SUCCESS(status)) {
 		acpi_tb_set_table_loaded_flag(table_index, TRUE);
@@ -120,7 +116,6 @@ acpi_ns_load_table(u32 table_index, struct acpi_namespace_node *node)
 		 * exist. This target of Scope must already exist in the
 		 * namespace, as per the ACPI specification.
 		 */
-		(void)acpi_ut_release_mutex(ACPI_MTX_NAMESPACE);
 		acpi_ns_delete_namespace_by_owner(acpi_gbl_root_table_list.
 						  tables[table_index].owner_id);
 
@@ -129,8 +124,6 @@ acpi_ns_load_table(u32 table_index, struct acpi_namespace_node *node)
 	}
 
 unlock:
-	(void)acpi_ut_release_mutex(ACPI_MTX_NAMESPACE);
-
 	if (ACPI_FAILURE(status)) {
 		return_ACPI_STATUS(status);
 	}
@@ -162,7 +155,8 @@ unlock:
 	 * other ACPI implementations. Optionally, the execution can be deferred
 	 * until later, see acpi_initialize_objects.
 	 */
-	if (!acpi_gbl_group_module_level_code) {
+	if (!acpi_gbl_parse_table_as_term_list
+	    && !acpi_gbl_group_module_level_code) {
 		acpi_ns_exec_module_code_list();
 	}
 
diff --git a/drivers/acpi/acpica/nsparse.c b/drivers/acpi/acpica/nsparse.c
index f631a47724f0..4f14e9205bff 100644
--- a/drivers/acpi/acpica/nsparse.c
+++ b/drivers/acpi/acpica/nsparse.c
@@ -47,12 +47,103 @@
 #include "acparser.h"
 #include "acdispat.h"
 #include "actables.h"
+#include "acinterp.h"
 
 #define _COMPONENT          ACPI_NAMESPACE
 ACPI_MODULE_NAME("nsparse")
 
 /*******************************************************************************
  *
+ * FUNCTION:    ns_execute_table
+ *
+ * PARAMETERS:  table_desc      - An ACPI table descriptor for table to parse
+ *              start_node      - Where to enter the table into the namespace
+ *
+ * RETURN:      Status
+ *
+ * DESCRIPTION: Load ACPI/AML table by executing the entire table as a
+ *              term_list.
+ *
+ ******************************************************************************/
+acpi_status
+acpi_ns_execute_table(u32 table_index, struct acpi_namespace_node *start_node)
+{
+	acpi_status status;
+	struct acpi_table_header *table;
+	acpi_owner_id owner_id;
+	struct acpi_evaluate_info *info = NULL;
+	u32 aml_length;
+	u8 *aml_start;
+	union acpi_operand_object *method_obj = NULL;
+
+	ACPI_FUNCTION_TRACE(ns_execute_table);
+
+	status = acpi_get_table_by_index(table_index, &table);
+	if (ACPI_FAILURE(status)) {
+		return_ACPI_STATUS(status);
+	}
+
+	/* Table must consist of at least a complete header */
+
+	if (table->length < sizeof(struct acpi_table_header)) {
+		return_ACPI_STATUS(AE_BAD_HEADER);
+	}
+
+	aml_start = (u8 *)table + sizeof(struct acpi_table_header);
+	aml_length = table->length - sizeof(struct acpi_table_header);
+
+	status = acpi_tb_get_owner_id(table_index, &owner_id);
+	if (ACPI_FAILURE(status)) {
+		return_ACPI_STATUS(status);
+	}
+
+	/* Create, initialize, and link a new temporary method object */
+
+	method_obj = acpi_ut_create_internal_object(ACPI_TYPE_METHOD);
+	if (!method_obj) {
+		return_ACPI_STATUS(AE_NO_MEMORY);
+	}
+
+	/* Allocate the evaluation information block */
+
+	info = ACPI_ALLOCATE_ZEROED(sizeof(struct acpi_evaluate_info));
+	if (!info) {
+		status = AE_NO_MEMORY;
+		goto cleanup;
+	}
+
+	ACPI_DEBUG_PRINT((ACPI_DB_PARSE,
+			  "Create table code block: %p\n", method_obj));
+
+	method_obj->method.aml_start = aml_start;
+	method_obj->method.aml_length = aml_length;
+	method_obj->method.owner_id = owner_id;
+	method_obj->method.info_flags |= ACPI_METHOD_MODULE_LEVEL;
+
+	info->pass_number = ACPI_IMODE_EXECUTE;
+	info->node = start_node;
+	info->obj_desc = method_obj;
+	info->node_flags = info->node->flags;
+	info->full_pathname = acpi_ns_get_normalized_pathname(info->node, TRUE);
+	if (!info->full_pathname) {
+		status = AE_NO_MEMORY;
+		goto cleanup;
+	}
+
+	status = acpi_ps_execute_table(info);
+
+cleanup:
+	if (info) {
+		ACPI_FREE(info->full_pathname);
+		info->full_pathname = NULL;
+	}
+	ACPI_FREE(info);
+	acpi_ut_remove_reference(method_obj);
+	return_ACPI_STATUS(status);
+}
+
+/*******************************************************************************
+ *
  * FUNCTION:    ns_one_complete_parse
  *
  * PARAMETERS:  pass_number             - 1 or 2
@@ -63,6 +154,7 @@ ACPI_MODULE_NAME("nsparse")
  * DESCRIPTION: Perform one complete parse of an ACPI/AML table.
  *
  ******************************************************************************/
+
 acpi_status
 acpi_ns_one_complete_parse(u32 pass_number,
 			   u32 table_index,
@@ -143,7 +235,9 @@ acpi_ns_one_complete_parse(u32 pass_number,
 
 	ACPI_DEBUG_PRINT((ACPI_DB_PARSE,
 			  "*PARSE* pass %u parse\n", pass_number));
+	acpi_ex_enter_interpreter();
 	status = acpi_ps_parse_aml(walk_state);
+	acpi_ex_exit_interpreter();
 
 cleanup:
 	acpi_ps_delete_parse_tree(parse_root);
@@ -170,38 +264,47 @@ acpi_ns_parse_table(u32 table_index, struct acpi_namespace_node *start_node)
 
 	ACPI_FUNCTION_TRACE(ns_parse_table);
 
-	/*
-	 * AML Parse, pass 1
-	 *
-	 * In this pass, we load most of the namespace. Control methods
-	 * are not parsed until later. A parse tree is not created. Instead,
-	 * each Parser Op subtree is deleted when it is finished. This saves
-	 * a great deal of memory, and allows a small cache of parse objects
-	 * to service the entire parse. The second pass of the parse then
-	 * performs another complete parse of the AML.
-	 */
-	ACPI_DEBUG_PRINT((ACPI_DB_PARSE, "**** Start pass 1\n"));
-
-	status = acpi_ns_one_complete_parse(ACPI_IMODE_LOAD_PASS1,
-					    table_index, start_node);
-	if (ACPI_FAILURE(status)) {
-		return_ACPI_STATUS(status);
-	}
+	if (acpi_gbl_parse_table_as_term_list) {
+		ACPI_DEBUG_PRINT((ACPI_DB_PARSE, "**** Start load pass\n"));
 
-	/*
-	 * AML Parse, pass 2
-	 *
-	 * In this pass, we resolve forward references and other things
-	 * that could not be completed during the first pass.
-	 * Another complete parse of the AML is performed, but the
-	 * overhead of this is compensated for by the fact that the
-	 * parse objects are all cached.
-	 */
-	ACPI_DEBUG_PRINT((ACPI_DB_PARSE, "**** Start pass 2\n"));
-	status = acpi_ns_one_complete_parse(ACPI_IMODE_LOAD_PASS2,
-					    table_index, start_node);
-	if (ACPI_FAILURE(status)) {
-		return_ACPI_STATUS(status);
+		status = acpi_ns_execute_table(table_index, start_node);
+		if (ACPI_FAILURE(status)) {
+			return_ACPI_STATUS(status);
+		}
+	} else {
+		/*
+		 * AML Parse, pass 1
+		 *
+		 * In this pass, we load most of the namespace. Control methods
+		 * are not parsed until later. A parse tree is not created.
+		 * Instead, each Parser Op subtree is deleted when it is finished.
+		 * This saves a great deal of memory, and allows a small cache of
+		 * parse objects to service the entire parse. The second pass of
+		 * the parse then performs another complete parse of the AML.
+		 */
+		ACPI_DEBUG_PRINT((ACPI_DB_PARSE, "**** Start pass 1\n"));
+
+		status = acpi_ns_one_complete_parse(ACPI_IMODE_LOAD_PASS1,
+						    table_index, start_node);
+		if (ACPI_FAILURE(status)) {
+			return_ACPI_STATUS(status);
+		}
+
+		/*
+		 * AML Parse, pass 2
+		 *
+		 * In this pass, we resolve forward references and other things
+		 * that could not be completed during the first pass.
+		 * Another complete parse of the AML is performed, but the
+		 * overhead of this is compensated for by the fact that the
+		 * parse objects are all cached.
+		 */
+		ACPI_DEBUG_PRINT((ACPI_DB_PARSE, "**** Start pass 2\n"));
+		status = acpi_ns_one_complete_parse(ACPI_IMODE_LOAD_PASS2,
+						    table_index, start_node);
+		if (ACPI_FAILURE(status)) {
+			return_ACPI_STATUS(status);
+		}
 	}
 
 	return_ACPI_STATUS(status);
diff --git a/drivers/acpi/acpica/nsutils.c b/drivers/acpi/acpica/nsutils.c
index 784a30b76e0f..691814dfed31 100644
--- a/drivers/acpi/acpica/nsutils.c
+++ b/drivers/acpi/acpica/nsutils.c
@@ -662,7 +662,7 @@ u32 acpi_ns_opens_scope(acpi_object_type type)
 
 /*******************************************************************************
  *
- * FUNCTION:    acpi_ns_get_node
+ * FUNCTION:    acpi_ns_get_node_unlocked
  *
  * PARAMETERS:  *pathname   - Name to be found, in external (ASL) format. The
  *                            \ (backslash) and ^ (carat) prefixes, and the
@@ -678,20 +678,21 @@ u32 acpi_ns_opens_scope(acpi_object_type type)
  * DESCRIPTION: Look up a name relative to a given scope and return the
  *              corresponding Node. NOTE: Scope can be null.
  *
- * MUTEX:       Locks namespace
+ * MUTEX:       Doesn't locks namespace
  *
  ******************************************************************************/
 
 acpi_status
-acpi_ns_get_node(struct acpi_namespace_node *prefix_node,
-		 const char *pathname,
-		 u32 flags, struct acpi_namespace_node **return_node)
+acpi_ns_get_node_unlocked(struct acpi_namespace_node *prefix_node,
+			  const char *pathname,
+			  u32 flags, struct acpi_namespace_node **return_node)
 {
 	union acpi_generic_state scope_info;
 	acpi_status status;
 	char *internal_path;
 
-	ACPI_FUNCTION_TRACE_PTR(ns_get_node, ACPI_CAST_PTR(char, pathname));
+	ACPI_FUNCTION_TRACE_PTR(ns_get_node_unlocked,
+				ACPI_CAST_PTR(char, pathname));
 
 	/* Simplest case is a null pathname */
 
@@ -718,13 +719,6 @@ acpi_ns_get_node(struct acpi_namespace_node *prefix_node,
 		return_ACPI_STATUS(status);
 	}
 
-	/* Must lock namespace during lookup */
-
-	status = acpi_ut_acquire_mutex(ACPI_MTX_NAMESPACE);
-	if (ACPI_FAILURE(status)) {
-		goto cleanup;
-	}
-
 	/* Setup lookup scope (search starting point) */
 
 	scope_info.scope.node = prefix_node;
@@ -740,9 +734,49 @@ acpi_ns_get_node(struct acpi_namespace_node *prefix_node,
 				  pathname, acpi_format_exception(status)));
 	}
 
-	(void)acpi_ut_release_mutex(ACPI_MTX_NAMESPACE);
-
-cleanup:
 	ACPI_FREE(internal_path);
 	return_ACPI_STATUS(status);
 }
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_ns_get_node
+ *
+ * PARAMETERS:  *pathname   - Name to be found, in external (ASL) format. The
+ *                            \ (backslash) and ^ (carat) prefixes, and the
+ *                            . (period) to separate segments are supported.
+ *              prefix_node  - Root of subtree to be searched, or NS_ALL for the
+ *                            root of the name space. If Name is fully
+ *                            qualified (first s8 is '\'), the passed value
+ *                            of Scope will not be accessed.
+ *              flags       - Used to indicate whether to perform upsearch or
+ *                            not.
+ *              return_node - Where the Node is returned
+ *
+ * DESCRIPTION: Look up a name relative to a given scope and return the
+ *              corresponding Node. NOTE: Scope can be null.
+ *
+ * MUTEX:       Locks namespace
+ *
+ ******************************************************************************/
+
+acpi_status
+acpi_ns_get_node(struct acpi_namespace_node *prefix_node,
+		 const char *pathname,
+		 u32 flags, struct acpi_namespace_node **return_node)
+{
+	acpi_status status;
+
+	ACPI_FUNCTION_TRACE_PTR(ns_get_node, ACPI_CAST_PTR(char, pathname));
+
+	status = acpi_ut_acquire_mutex(ACPI_MTX_NAMESPACE);
+	if (ACPI_FAILURE(status)) {
+		return_ACPI_STATUS(status);
+	}
+
+	status = acpi_ns_get_node_unlocked(prefix_node, pathname,
+					   flags, return_node);
+
+	(void)acpi_ut_release_mutex(ACPI_MTX_NAMESPACE);
+	return_ACPI_STATUS(status);
+}
diff --git a/drivers/acpi/acpica/psparse.c b/drivers/acpi/acpica/psparse.c
index 0a23897d8adf..1ce26d9f8ff6 100644
--- a/drivers/acpi/acpica/psparse.c
+++ b/drivers/acpi/acpica/psparse.c
@@ -537,9 +537,11 @@ acpi_status acpi_ps_parse_aml(struct acpi_walk_state *walk_state)
 
 			/* Either the method parse or actual execution failed */
 
+			acpi_ex_exit_interpreter();
 			ACPI_ERROR_METHOD("Method parse/execution failed",
 					  walk_state->method_node, NULL,
 					  status);
+			acpi_ex_enter_interpreter();
 
 			/* Check for possible multi-thread reentrancy problem */
 
@@ -571,7 +573,9 @@ acpi_status acpi_ps_parse_aml(struct acpi_walk_state *walk_state)
 		 * cleanup to do
 		 */
 		if (((walk_state->parse_flags & ACPI_PARSE_MODE_MASK) ==
-		     ACPI_PARSE_EXECUTE) || (ACPI_FAILURE(status))) {
+		     ACPI_PARSE_EXECUTE &&
+		     !(walk_state->parse_flags & ACPI_PARSE_MODULE_LEVEL)) ||
+		    (ACPI_FAILURE(status))) {
 			acpi_ds_terminate_control_method(walk_state->
 							 method_desc,
 							 walk_state);
diff --git a/drivers/acpi/acpica/psxface.c b/drivers/acpi/acpica/psxface.c
index cf30cd821f5e..f3c87264bd1b 100644
--- a/drivers/acpi/acpica/psxface.c
+++ b/drivers/acpi/acpica/psxface.c
@@ -252,6 +252,90 @@ cleanup:
 
 /*******************************************************************************
  *
+ * FUNCTION:    acpi_ps_execute_table
+ *
+ * PARAMETERS:  info            - Method info block, contains:
+ *              node            - Node to where the is entered into the
+ *                                namespace
+ *              obj_desc        - Pseudo method object describing the AML
+ *                                code of the entire table
+ *              pass_number     - Parse or execute pass
+ *
+ * RETURN:      Status
+ *
+ * DESCRIPTION: Execute a table
+ *
+ ******************************************************************************/
+
+acpi_status acpi_ps_execute_table(struct acpi_evaluate_info *info)
+{
+	acpi_status status;
+	union acpi_parse_object *op = NULL;
+	struct acpi_walk_state *walk_state = NULL;
+
+	ACPI_FUNCTION_TRACE(ps_execute_table);
+
+	/* Create and init a Root Node */
+
+	op = acpi_ps_create_scope_op(info->obj_desc->method.aml_start);
+	if (!op) {
+		status = AE_NO_MEMORY;
+		goto cleanup;
+	}
+
+	/* Create and initialize a new walk state */
+
+	walk_state =
+	    acpi_ds_create_walk_state(info->obj_desc->method.owner_id, NULL,
+				      NULL, NULL);
+	if (!walk_state) {
+		status = AE_NO_MEMORY;
+		goto cleanup;
+	}
+
+	status = acpi_ds_init_aml_walk(walk_state, op, info->node,
+				       info->obj_desc->method.aml_start,
+				       info->obj_desc->method.aml_length, info,
+				       info->pass_number);
+	if (ACPI_FAILURE(status)) {
+		goto cleanup;
+	}
+
+	if (info->obj_desc->method.info_flags & ACPI_METHOD_MODULE_LEVEL) {
+		walk_state->parse_flags |= ACPI_PARSE_MODULE_LEVEL;
+	}
+
+	/* Info->Node is the default location to load the table  */
+
+	if (info->node && info->node != acpi_gbl_root_node) {
+		status =
+		    acpi_ds_scope_stack_push(info->node, ACPI_TYPE_METHOD,
+					     walk_state);
+		if (ACPI_FAILURE(status)) {
+			goto cleanup;
+		}
+	}
+
+	/*
+	 * Parse the AML, walk_state will be deleted by parse_aml
+	 */
+	acpi_ex_enter_interpreter();
+	status = acpi_ps_parse_aml(walk_state);
+	acpi_ex_exit_interpreter();
+	walk_state = NULL;
+
+cleanup:
+	if (walk_state) {
+		acpi_ds_delete_walk_state(walk_state);
+	}
+	if (op) {
+		acpi_ps_delete_parse_tree(op);
+	}
+	return_ACPI_STATUS(status);
+}
+
+/*******************************************************************************
+ *
  * FUNCTION:    acpi_ps_update_parameter_list
  *
  * PARAMETERS:  info            - See struct acpi_evaluate_info
diff --git a/drivers/acpi/acpica/tbdata.c b/drivers/acpi/acpica/tbdata.c
index 1388a19e5db8..d9ca8c2aa2d3 100644
--- a/drivers/acpi/acpica/tbdata.c
+++ b/drivers/acpi/acpica/tbdata.c
@@ -45,6 +45,7 @@
 #include "accommon.h"
 #include "acnamesp.h"
 #include "actables.h"
+#include "acevents.h"
 
 #define _COMPONENT          ACPI_TABLES
 ACPI_MODULE_NAME("tbdata")
@@ -613,17 +614,12 @@ acpi_status acpi_tb_delete_namespace_by_owner(u32 table_index)
 	 * lock may block, and also since the execution of a namespace walk
 	 * must be allowed to use the interpreter.
 	 */
-	(void)acpi_ut_release_mutex(ACPI_MTX_INTERPRETER);
 	status = acpi_ut_acquire_write_lock(&acpi_gbl_namespace_rw_lock);
-
-	acpi_ns_delete_namespace_by_owner(owner_id);
 	if (ACPI_FAILURE(status)) {
 		return_ACPI_STATUS(status);
 	}
-
+	acpi_ns_delete_namespace_by_owner(owner_id);
 	acpi_ut_release_write_lock(&acpi_gbl_namespace_rw_lock);
-
-	status = acpi_ut_acquire_mutex(ACPI_MTX_INTERPRETER);
 	return_ACPI_STATUS(status);
 }
 
@@ -771,3 +767,142 @@ void acpi_tb_set_table_loaded_flag(u32 table_index, u8 is_loaded)
 
 	(void)acpi_ut_release_mutex(ACPI_MTX_TABLES);
 }
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_tb_load_table
+ *
+ * PARAMETERS:  table_index             - Table index
+ *              parent_node             - Where table index is returned
+ *
+ * RETURN:      Status
+ *
+ * DESCRIPTION: Load an ACPI table
+ *
+ ******************************************************************************/
+
+acpi_status
+acpi_tb_load_table(u32 table_index, struct acpi_namespace_node *parent_node)
+{
+	struct acpi_table_header *table;
+	acpi_status status;
+	acpi_owner_id owner_id;
+
+	ACPI_FUNCTION_TRACE(tb_load_table);
+
+	/*
+	 * Note: Now table is "INSTALLED", it must be validated before
+	 * using.
+	 */
+	status = acpi_get_table_by_index(table_index, &table);
+	if (ACPI_FAILURE(status)) {
+		return_ACPI_STATUS(status);
+	}
+
+	status = acpi_ns_load_table(table_index, parent_node);
+
+	/* Execute any module-level code that was found in the table */
+
+	if (!acpi_gbl_parse_table_as_term_list
+	    && acpi_gbl_group_module_level_code) {
+		acpi_ns_exec_module_code_list();
+	}
+
+	/*
+	 * Update GPEs for any new _Lxx/_Exx methods. Ignore errors. The host is
+	 * responsible for discovering any new wake GPEs by running _PRW methods
+	 * that may have been loaded by this table.
+	 */
+	status = acpi_tb_get_owner_id(table_index, &owner_id);
+	if (ACPI_SUCCESS(status)) {
+		acpi_ev_update_gpes(owner_id);
+	}
+
+	/* Invoke table handler if present */
+
+	if (acpi_gbl_table_handler) {
+		(void)acpi_gbl_table_handler(ACPI_TABLE_EVENT_LOAD, table,
+					     acpi_gbl_table_handler_context);
+	}
+
+	return_ACPI_STATUS(status);
+}
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_tb_install_and_load_table
+ *
+ * PARAMETERS:  table                   - Pointer to the table
+ *              address                 - Physical address of the table
+ *              flags                   - Allocation flags of the table
+ *              table_index             - Where table index is returned
+ *
+ * RETURN:      Status
+ *
+ * DESCRIPTION: Install and load an ACPI table
+ *
+ ******************************************************************************/
+
+acpi_status
+acpi_tb_install_and_load_table(struct acpi_table_header *table,
+			       acpi_physical_address address,
+			       u8 flags, u8 override, u32 *table_index)
+{
+	acpi_status status;
+	u32 i;
+	acpi_owner_id owner_id;
+
+	ACPI_FUNCTION_TRACE(acpi_load_table);
+
+	(void)acpi_ut_acquire_mutex(ACPI_MTX_TABLES);
+
+	/* Install the table and load it into the namespace */
+
+	status = acpi_tb_install_standard_table(address, flags, TRUE,
+						override, &i);
+	if (ACPI_FAILURE(status)) {
+		goto unlock_and_exit;
+	}
+
+	/*
+	 * Note: Now table is "INSTALLED", it must be validated before
+	 * using.
+	 */
+	status = acpi_tb_validate_table(&acpi_gbl_root_table_list.tables[i]);
+	if (ACPI_FAILURE(status)) {
+		goto unlock_and_exit;
+	}
+
+	(void)acpi_ut_release_mutex(ACPI_MTX_TABLES);
+	status = acpi_ns_load_table(i, acpi_gbl_root_node);
+
+	/* Execute any module-level code that was found in the table */
+
+	if (!acpi_gbl_parse_table_as_term_list
+	    && acpi_gbl_group_module_level_code) {
+		acpi_ns_exec_module_code_list();
+	}
+
+	/*
+	 * Update GPEs for any new _Lxx/_Exx methods. Ignore errors. The host is
+	 * responsible for discovering any new wake GPEs by running _PRW methods
+	 * that may have been loaded by this table.
+	 */
+	status = acpi_tb_get_owner_id(i, &owner_id);
+	if (ACPI_SUCCESS(status)) {
+		acpi_ev_update_gpes(owner_id);
+	}
+
+	/* Invoke table handler if present */
+
+	if (acpi_gbl_table_handler) {
+		(void)acpi_gbl_table_handler(ACPI_TABLE_EVENT_LOAD, table,
+					     acpi_gbl_table_handler_context);
+	}
+	(void)acpi_ut_acquire_mutex(ACPI_MTX_TABLES);
+
+unlock_and_exit:
+	*table_index = i;
+	(void)acpi_ut_release_mutex(ACPI_MTX_TABLES);
+	return_ACPI_STATUS(status);
+}
diff --git a/drivers/acpi/acpica/tbfadt.c b/drivers/acpi/acpica/tbfadt.c
index 620806965243..046c4d0394ee 100644
--- a/drivers/acpi/acpica/tbfadt.c
+++ b/drivers/acpi/acpica/tbfadt.c
@@ -344,23 +344,27 @@ void acpi_tb_parse_fadt(void)
 
 	/* Obtain the DSDT and FACS tables via their addresses within the FADT */
 
-	acpi_tb_install_fixed_table((acpi_physical_address)acpi_gbl_FADT.Xdsdt,
-				    ACPI_SIG_DSDT, &acpi_gbl_dsdt_index);
+	acpi_tb_install_standard_table((acpi_physical_address)acpi_gbl_FADT.
+				       Xdsdt,
+				       ACPI_TABLE_ORIGIN_INTERNAL_PHYSICAL,
+				       FALSE, TRUE, &acpi_gbl_dsdt_index);
 
 	/* If Hardware Reduced flag is set, there is no FACS */
 
 	if (!acpi_gbl_reduced_hardware) {
 		if (acpi_gbl_FADT.facs) {
-			acpi_tb_install_fixed_table((acpi_physical_address)
-						    acpi_gbl_FADT.facs,
-						    ACPI_SIG_FACS,
-						    &acpi_gbl_facs_index);
+			acpi_tb_install_standard_table((acpi_physical_address)
+						       acpi_gbl_FADT.facs,
+						       ACPI_TABLE_ORIGIN_INTERNAL_PHYSICAL,
+						       FALSE, TRUE,
+						       &acpi_gbl_facs_index);
 		}
 		if (acpi_gbl_FADT.Xfacs) {
-			acpi_tb_install_fixed_table((acpi_physical_address)
-						    acpi_gbl_FADT.Xfacs,
-						    ACPI_SIG_FACS,
-						    &acpi_gbl_xfacs_index);
+			acpi_tb_install_standard_table((acpi_physical_address)
+						       acpi_gbl_FADT.Xfacs,
+						       ACPI_TABLE_ORIGIN_INTERNAL_PHYSICAL,
+						       FALSE, TRUE,
+						       &acpi_gbl_xfacs_index);
 		}
 	}
 }
@@ -476,17 +480,19 @@ static void acpi_tb_convert_fadt(void)
 	u32 i;
 
 	/*
-	 * For ACPI 1.0 FADTs (revision 1 or 2), ensure that reserved fields which
+	 * For ACPI 1.0 FADTs (revision 1), ensure that reserved fields which
 	 * should be zero are indeed zero. This will workaround BIOSs that
 	 * inadvertently place values in these fields.
 	 *
 	 * The ACPI 1.0 reserved fields that will be zeroed are the bytes located
 	 * at offset 45, 55, 95, and the word located at offset 109, 110.
 	 *
-	 * Note: The FADT revision value is unreliable. Only the length can be
-	 * trusted.
+	 * Note: The FADT revision value is unreliable because of BIOS errors.
+	 * The table length is instead used as the final word on the version.
+	 *
+	 * Note: FADT revision 3 is the ACPI 2.0 version of the FADT.
 	 */
-	if (acpi_gbl_FADT.header.length <= ACPI_FADT_V2_SIZE) {
+	if (acpi_gbl_FADT.header.length <= ACPI_FADT_V3_SIZE) {
 		acpi_gbl_FADT.preferred_profile = 0;
 		acpi_gbl_FADT.pstate_control = 0;
 		acpi_gbl_FADT.cst_control = 0;
@@ -552,78 +558,74 @@ static void acpi_tb_convert_fadt(void)
 		 *
 		 * Address32 zero, Address64 [don't care]   - Use Address64
 		 *
+		 * No override: if acpi_gbl_use32_bit_fadt_addresses is FALSE, and:
 		 * Address32 non-zero, Address64 zero       - Copy/use Address32
 		 * Address32 non-zero == Address64 non-zero - Use Address64
 		 * Address32 non-zero != Address64 non-zero - Warning, use Address64
 		 *
 		 * Override: if acpi_gbl_use32_bit_fadt_addresses is TRUE, and:
+		 * Address32 non-zero, Address64 zero       - Copy/use Address32
+		 * Address32 non-zero == Address64 non-zero - Copy/use Address32
 		 * Address32 non-zero != Address64 non-zero - Warning, copy/use Address32
 		 *
 		 * Note: space_id is always I/O for 32-bit legacy address fields
 		 */
 		if (address32) {
-			if (!address64->address) {
+			if (address64->address) {
+				if (address64->address != (u64)address32) {
+
+					/* Address mismatch */
+
+					ACPI_BIOS_WARNING((AE_INFO,
+							   "32/64X address mismatch in FADT/%s: "
+							   "0x%8.8X/0x%8.8X%8.8X, using %u-bit address",
+							   name, address32,
+							   ACPI_FORMAT_UINT64
+							   (address64->address),
+							   acpi_gbl_use32_bit_fadt_addresses
+							   ? 32 : 64));
+				}
 
-				/* 64-bit address is zero, use 32-bit address */
+				/*
+				 * For each extended field, check for length mismatch
+				 * between the legacy length field and the corresponding
+				 * 64-bit X length field.
+				 * Note: If the legacy length field is > 0xFF bits, ignore
+				 * this check. (GPE registers can be larger than the
+				 * 64-bit GAS structure can accomodate, 0xFF bits).
+				 */
+				if ((ACPI_MUL_8(length) <= ACPI_UINT8_MAX) &&
+				    (address64->bit_width !=
+				     ACPI_MUL_8(length))) {
+					ACPI_BIOS_WARNING((AE_INFO,
+							   "32/64X length mismatch in FADT/%s: %u/%u",
+							   name,
+							   ACPI_MUL_8(length),
+							   address64->
+							   bit_width));
+				}
+			}
 
+			/*
+			 * Hardware register access code always uses the 64-bit fields.
+			 * So if the 64-bit field is zero or is to be overridden,
+			 * initialize it with the 32-bit fields.
+			 * Note that when the 32-bit address favor is specified, the
+			 * 64-bit fields are always re-initialized so that
+			 * access_size/bit_width/bit_offset fields can be correctly
+			 * configured to the values to trigger a 32-bit compatible
+			 * access mode in the hardware register access code.
+			 */
+			if (!address64->address
+			    || acpi_gbl_use32_bit_fadt_addresses) {
 				acpi_tb_init_generic_address(address64,
 							     ACPI_ADR_SPACE_SYSTEM_IO,
-							     *ACPI_ADD_PTR(u8,
-									   &acpi_gbl_FADT,
-									   fadt_info_table
-									   [i].
-									   length),
+							     length,
 							     (u64)address32,
 							     name, flags);
-			} else if (address64->address != (u64)address32) {
-
-				/* Address mismatch */
-
-				ACPI_BIOS_WARNING((AE_INFO,
-						   "32/64X address mismatch in FADT/%s: "
-						   "0x%8.8X/0x%8.8X%8.8X, using %u-bit address",
-						   name, address32,
-						   ACPI_FORMAT_UINT64
-						   (address64->address),
-						   acpi_gbl_use32_bit_fadt_addresses
-						   ? 32 : 64));
-
-				if (acpi_gbl_use32_bit_fadt_addresses) {
-
-					/* 32-bit address override */
-
-					acpi_tb_init_generic_address(address64,
-								     ACPI_ADR_SPACE_SYSTEM_IO,
-								     *ACPI_ADD_PTR
-								     (u8,
-								      &acpi_gbl_FADT,
-								      fadt_info_table
-								      [i].
-								      length),
-								     (u64)
-								     address32,
-								     name,
-								     flags);
-				}
 			}
 		}
 
-		/*
-		 * For each extended field, check for length mismatch between the
-		 * legacy length field and the corresponding 64-bit X length field.
-		 * Note: If the legacy length field is > 0xFF bits, ignore this
-		 * check. (GPE registers can be larger than the 64-bit GAS structure
-		 * can accomodate, 0xFF bits).
-		 */
-		if (address64->address &&
-		    (ACPI_MUL_8(length) <= ACPI_UINT8_MAX) &&
-		    (address64->bit_width != ACPI_MUL_8(length))) {
-			ACPI_BIOS_WARNING((AE_INFO,
-					   "32/64X length mismatch in FADT/%s: %u/%u",
-					   name, ACPI_MUL_8(length),
-					   address64->bit_width));
-		}
-
 		if (fadt_info_table[i].flags & ACPI_FADT_REQUIRED) {
 			/*
 			 * Field is required (Pm1a_event, Pm1a_control).
diff --git a/drivers/acpi/acpica/tbfind.c b/drivers/acpi/acpica/tbfind.c
index e348d616e60f..f6b9b4e4298b 100644
--- a/drivers/acpi/acpica/tbfind.c
+++ b/drivers/acpi/acpica/tbfind.c
@@ -68,7 +68,7 @@ acpi_status
 acpi_tb_find_table(char *signature,
 		   char *oem_id, char *oem_table_id, u32 *table_index)
 {
-	acpi_status status;
+	acpi_status status = AE_OK;
 	struct acpi_table_header header;
 	u32 i;
 
@@ -96,6 +96,7 @@ acpi_tb_find_table(char *signature,
 
 	/* Search for the table */
 
+	(void)acpi_ut_acquire_mutex(ACPI_MTX_TABLES);
 	for (i = 0; i < acpi_gbl_root_table_list.current_table_count; ++i) {
 		if (memcmp(&(acpi_gbl_root_table_list.tables[i].signature),
 			   header.signature, ACPI_NAME_SIZE)) {
@@ -115,7 +116,7 @@ acpi_tb_find_table(char *signature,
 			    acpi_tb_validate_table(&acpi_gbl_root_table_list.
 						   tables[i]);
 			if (ACPI_FAILURE(status)) {
-				return_ACPI_STATUS(status);
+				goto unlock_and_exit;
 			}
 
 			if (!acpi_gbl_root_table_list.tables[i].pointer) {
@@ -144,9 +145,12 @@ acpi_tb_find_table(char *signature,
 			ACPI_DEBUG_PRINT((ACPI_DB_TABLES,
 					  "Found table [%4.4s]\n",
 					  header.signature));
-			return_ACPI_STATUS(AE_OK);
+			goto unlock_and_exit;
 		}
 	}
+	status = AE_NOT_FOUND;
 
-	return_ACPI_STATUS(AE_NOT_FOUND);
+unlock_and_exit:
+	(void)acpi_ut_release_mutex(ACPI_MTX_TABLES);
+	return_ACPI_STATUS(status);
 }
diff --git a/drivers/acpi/acpica/tbinstal.c b/drivers/acpi/acpica/tbinstal.c
index 8b13052128fc..5fdf251a9f97 100644
--- a/drivers/acpi/acpica/tbinstal.c
+++ b/drivers/acpi/acpica/tbinstal.c
@@ -157,68 +157,6 @@ acpi_tb_install_table_with_override(struct acpi_table_desc *new_table_desc,
 
 /*******************************************************************************
  *
- * FUNCTION:    acpi_tb_install_fixed_table
- *
- * PARAMETERS:  address                 - Physical address of DSDT or FACS
- *              signature               - Table signature, NULL if no need to
- *                                        match
- *              table_index             - Where the table index is returned
- *
- * RETURN:      Status
- *
- * DESCRIPTION: Install a fixed ACPI table (DSDT/FACS) into the global data
- *              structure.
- *
- ******************************************************************************/
-
-acpi_status
-acpi_tb_install_fixed_table(acpi_physical_address address,
-			    char *signature, u32 *table_index)
-{
-	struct acpi_table_desc new_table_desc;
-	acpi_status status;
-
-	ACPI_FUNCTION_TRACE(tb_install_fixed_table);
-
-	if (!address) {
-		ACPI_ERROR((AE_INFO,
-			    "Null physical address for ACPI table [%s]",
-			    signature));
-		return (AE_NO_MEMORY);
-	}
-
-	/* Fill a table descriptor for validation */
-
-	status = acpi_tb_acquire_temp_table(&new_table_desc, address,
-					    ACPI_TABLE_ORIGIN_INTERNAL_PHYSICAL);
-	if (ACPI_FAILURE(status)) {
-		ACPI_ERROR((AE_INFO,
-			    "Could not acquire table length at %8.8X%8.8X",
-			    ACPI_FORMAT_UINT64(address)));
-		return_ACPI_STATUS(status);
-	}
-
-	/* Validate and verify a table before installation */
-
-	status = acpi_tb_verify_temp_table(&new_table_desc, signature);
-	if (ACPI_FAILURE(status)) {
-		goto release_and_exit;
-	}
-
-	/* Add the table to the global root table list */
-
-	acpi_tb_install_table_with_override(&new_table_desc, TRUE, table_index);
-
-release_and_exit:
-
-	/* Release the temporary table descriptor */
-
-	acpi_tb_release_temp_table(&new_table_desc);
-	return_ACPI_STATUS(status);
-}
-
-/*******************************************************************************
- *
  * FUNCTION:    acpi_tb_install_standard_table
  *
  * PARAMETERS:  address             - Address of the table (might be a virtual
@@ -230,8 +168,7 @@ release_and_exit:
  *
  * RETURN:      Status
  *
- * DESCRIPTION: This function is called to install an ACPI table that is
- *              neither DSDT nor FACS (a "standard" table.)
+ * DESCRIPTION: This function is called to verify and install an ACPI table.
  *              When this function is called by "Load" or "LoadTable" opcodes,
  *              or by acpi_load_table() API, the "Reload" parameter is set.
  *              After sucessfully returning from this function, table is
@@ -364,6 +301,14 @@ acpi_tb_install_standard_table(acpi_physical_address address,
 	acpi_tb_install_table_with_override(&new_table_desc, override,
 					    table_index);
 
+	/* Invoke table handler if present */
+
+	if (acpi_gbl_table_handler) {
+		(void)acpi_gbl_table_handler(ACPI_TABLE_EVENT_INSTALL,
+					     new_table_desc.pointer,
+					     acpi_gbl_table_handler_context);
+	}
+
 release_and_exit:
 
 	/* Release the temporary table descriptor */
diff --git a/drivers/acpi/acpica/tbutils.c b/drivers/acpi/acpica/tbutils.c
index e28553914bf5..51eb07cf9898 100644
--- a/drivers/acpi/acpica/tbutils.c
+++ b/drivers/acpi/acpica/tbutils.c
@@ -252,7 +252,8 @@ acpi_tb_get_root_table_entry(u8 *table_entry, u32 table_entry_size)
  *
  ******************************************************************************/
 
-acpi_status __init acpi_tb_parse_root_table(acpi_physical_address rsdp_address)
+acpi_status ACPI_INIT_FUNCTION
+acpi_tb_parse_root_table(acpi_physical_address rsdp_address)
 {
 	struct acpi_table_rsdp *rsdp;
 	u32 table_entry_size;
diff --git a/drivers/acpi/acpica/tbxface.c b/drivers/acpi/acpica/tbxface.c
index 3ecec937e8c9..4ab6b9cd0aec 100644
--- a/drivers/acpi/acpica/tbxface.c
+++ b/drivers/acpi/acpica/tbxface.c
@@ -98,7 +98,7 @@ acpi_status acpi_allocate_root_table(u32 initial_table_count)
  *
  ******************************************************************************/
 
-acpi_status __init
+acpi_status ACPI_INIT_FUNCTION
 acpi_initialize_tables(struct acpi_table_desc *initial_table_array,
 		       u32 initial_table_count, u8 allow_resize)
 {
@@ -164,7 +164,7 @@ ACPI_EXPORT_SYMBOL_INIT(acpi_initialize_tables)
  *              kernel.
  *
  ******************************************************************************/
-acpi_status __init acpi_reallocate_root_table(void)
+acpi_status ACPI_INIT_FUNCTION acpi_reallocate_root_table(void)
 {
 	acpi_status status;
 
diff --git a/drivers/acpi/acpica/tbxfload.c b/drivers/acpi/acpica/tbxfload.c
index ac71abcd32bb..5569f637f669 100644
--- a/drivers/acpi/acpica/tbxfload.c
+++ b/drivers/acpi/acpica/tbxfload.c
@@ -63,7 +63,7 @@ ACPI_MODULE_NAME("tbxfload")
  * DESCRIPTION: Load the ACPI tables from the RSDT/XSDT
  *
  ******************************************************************************/
-acpi_status __init acpi_load_tables(void)
+acpi_status ACPI_INIT_FUNCTION acpi_load_tables(void)
 {
 	acpi_status status;
 
@@ -103,7 +103,8 @@ acpi_status __init acpi_load_tables(void)
 				"While loading namespace from ACPI tables"));
 	}
 
-	if (!acpi_gbl_group_module_level_code) {
+	if (acpi_gbl_parse_table_as_term_list
+	    || !acpi_gbl_group_module_level_code) {
 		/*
 		 * Initialize the objects that remain uninitialized. This
 		 * runs the executable AML that may be part of the
@@ -188,11 +189,11 @@ acpi_status acpi_tb_load_namespace(void)
 	memcpy(&acpi_gbl_original_dsdt_header, acpi_gbl_DSDT,
 	       sizeof(struct acpi_table_header));
 
-	(void)acpi_ut_release_mutex(ACPI_MTX_TABLES);
-
 	/* Load and parse tables */
 
+	(void)acpi_ut_release_mutex(ACPI_MTX_TABLES);
 	status = acpi_ns_load_table(acpi_gbl_dsdt_index, acpi_gbl_root_node);
+	(void)acpi_ut_acquire_mutex(ACPI_MTX_TABLES);
 	if (ACPI_FAILURE(status)) {
 		ACPI_EXCEPTION((AE_INFO, status, "[DSDT] table load failed"));
 		tables_failed++;
@@ -202,7 +203,6 @@ acpi_status acpi_tb_load_namespace(void)
 
 	/* Load any SSDT or PSDT tables. Note: Loop leaves tables locked */
 
-	(void)acpi_ut_acquire_mutex(ACPI_MTX_TABLES);
 	for (i = 0; i < acpi_gbl_root_table_list.current_table_count; ++i) {
 		table = &acpi_gbl_root_table_list.tables[i];
 
@@ -220,6 +220,7 @@ acpi_status acpi_tb_load_namespace(void)
 
 		(void)acpi_ut_release_mutex(ACPI_MTX_TABLES);
 		status = acpi_ns_load_table(i, acpi_gbl_root_node);
+		(void)acpi_ut_acquire_mutex(ACPI_MTX_TABLES);
 		if (ACPI_FAILURE(status)) {
 			ACPI_EXCEPTION((AE_INFO, status,
 					"(%4.4s:%8.8s) while loading table",
@@ -235,8 +236,6 @@ acpi_status acpi_tb_load_namespace(void)
 		} else {
 			tables_loaded++;
 		}
-
-		(void)acpi_ut_acquire_mutex(ACPI_MTX_TABLES);
 	}
 
 	if (!tables_failed) {
@@ -272,7 +271,7 @@ unlock_and_exit:
  *
  ******************************************************************************/
 
-acpi_status __init
+acpi_status ACPI_INIT_FUNCTION
 acpi_install_table(acpi_physical_address address, u8 physical)
 {
 	acpi_status status;
@@ -324,49 +323,13 @@ acpi_status acpi_load_table(struct acpi_table_header *table)
 		return_ACPI_STATUS(AE_BAD_PARAMETER);
 	}
 
-	/* Must acquire the interpreter lock during this operation */
-
-	status = acpi_ut_acquire_mutex(ACPI_MTX_INTERPRETER);
-	if (ACPI_FAILURE(status)) {
-		return_ACPI_STATUS(status);
-	}
-
 	/* Install the table and load it into the namespace */
 
 	ACPI_INFO(("Host-directed Dynamic ACPI Table Load:"));
-	(void)acpi_ut_acquire_mutex(ACPI_MTX_TABLES);
-
-	status = acpi_tb_install_standard_table(ACPI_PTR_TO_PHYSADDR(table),
-						ACPI_TABLE_ORIGIN_EXTERNAL_VIRTUAL,
-						TRUE, FALSE, &table_index);
-
-	(void)acpi_ut_release_mutex(ACPI_MTX_TABLES);
-	if (ACPI_FAILURE(status)) {
-		goto unlock_and_exit;
-	}
-
-	/*
-	 * Note: Now table is "INSTALLED", it must be validated before
-	 * using.
-	 */
 	status =
-	    acpi_tb_validate_table(&acpi_gbl_root_table_list.
-				   tables[table_index]);
-	if (ACPI_FAILURE(status)) {
-		goto unlock_and_exit;
-	}
-
-	status = acpi_ns_load_table(table_index, acpi_gbl_root_node);
-
-	/* Invoke table handler if present */
-
-	if (acpi_gbl_table_handler) {
-		(void)acpi_gbl_table_handler(ACPI_TABLE_EVENT_LOAD, table,
-					     acpi_gbl_table_handler_context);
-	}
-
-unlock_and_exit:
-	(void)acpi_ut_release_mutex(ACPI_MTX_INTERPRETER);
+	    acpi_tb_install_and_load_table(table, ACPI_PTR_TO_PHYSADDR(table),
+					   ACPI_TABLE_ORIGIN_EXTERNAL_VIRTUAL,
+					   FALSE, &table_index);
 	return_ACPI_STATUS(status);
 }
 
@@ -415,9 +378,9 @@ acpi_status acpi_unload_parent_table(acpi_handle object)
 		return_ACPI_STATUS(AE_TYPE);
 	}
 
-	/* Must acquire the interpreter lock during this operation */
+	/* Must acquire the table lock during this operation */
 
-	status = acpi_ut_acquire_mutex(ACPI_MTX_INTERPRETER);
+	status = acpi_ut_acquire_mutex(ACPI_MTX_TABLES);
 	if (ACPI_FAILURE(status)) {
 		return_ACPI_STATUS(status);
 	}
@@ -444,8 +407,10 @@ acpi_status acpi_unload_parent_table(acpi_handle object)
 
 		/* Ensure the table is actually loaded */
 
+		(void)acpi_ut_release_mutex(ACPI_MTX_TABLES);
 		if (!acpi_tb_is_table_loaded(i)) {
 			status = AE_NOT_EXIST;
+			(void)acpi_ut_acquire_mutex(ACPI_MTX_TABLES);
 			break;
 		}
 
@@ -471,10 +436,11 @@ acpi_status acpi_unload_parent_table(acpi_handle object)
 
 		status = acpi_tb_release_owner_id(i);
 		acpi_tb_set_table_loaded_flag(i, FALSE);
+		(void)acpi_ut_acquire_mutex(ACPI_MTX_TABLES);
 		break;
 	}
 
-	(void)acpi_ut_release_mutex(ACPI_MTX_INTERPRETER);
+	(void)acpi_ut_release_mutex(ACPI_MTX_TABLES);
 	return_ACPI_STATUS(status);
 }
 
diff --git a/drivers/acpi/acpica/tbxfroot.c b/drivers/acpi/acpica/tbxfroot.c
index adb6cfc54661..0adb1c78d863 100644
--- a/drivers/acpi/acpica/tbxfroot.c
+++ b/drivers/acpi/acpica/tbxfroot.c
@@ -142,7 +142,8 @@ acpi_status acpi_tb_validate_rsdp(struct acpi_table_rsdp *rsdp)
  *
  ******************************************************************************/
 
-acpi_status __init acpi_find_root_pointer(acpi_physical_address *table_address)
+acpi_status ACPI_INIT_FUNCTION
+acpi_find_root_pointer(acpi_physical_address *table_address)
 {
 	u8 *table_ptr;
 	u8 *mem_rover;
@@ -244,6 +245,8 @@ acpi_status __init acpi_find_root_pointer(acpi_physical_address *table_address)
 	return_ACPI_STATUS(AE_NOT_FOUND);
 }
 
+ACPI_EXPORT_SYMBOL_INIT(acpi_find_root_pointer)
+
 /*******************************************************************************
  *
  * FUNCTION:    acpi_tb_scan_memory_for_rsdp
diff --git a/drivers/acpi/acpica/utaddress.c b/drivers/acpi/acpica/utaddress.c
index c986ec66a118..433d822798b6 100644
--- a/drivers/acpi/acpica/utaddress.c
+++ b/drivers/acpi/acpica/utaddress.c
@@ -77,7 +77,6 @@ acpi_ut_add_address_range(acpi_adr_space_type space_id,
 			  u32 length, struct acpi_namespace_node *region_node)
 {
 	struct acpi_address_range *range_info;
-	acpi_status status;
 
 	ACPI_FUNCTION_TRACE(ut_add_address_range);
 
@@ -97,12 +96,6 @@ acpi_ut_add_address_range(acpi_adr_space_type space_id,
 	range_info->end_address = (address + length - 1);
 	range_info->region_node = region_node;
 
-	status = acpi_ut_acquire_mutex(ACPI_MTX_NAMESPACE);
-	if (ACPI_FAILURE(status)) {
-		ACPI_FREE(range_info);
-		return_ACPI_STATUS(status);
-	}
-
 	range_info->next = acpi_gbl_address_range_list[space_id];
 	acpi_gbl_address_range_list[space_id] = range_info;
 
@@ -112,7 +105,6 @@ acpi_ut_add_address_range(acpi_adr_space_type space_id,
 			  ACPI_FORMAT_UINT64(address),
 			  ACPI_FORMAT_UINT64(range_info->end_address)));
 
-	(void)acpi_ut_release_mutex(ACPI_MTX_NAMESPACE);
 	return_ACPI_STATUS(AE_OK);
 }
 
diff --git a/drivers/acpi/acpica/utbuffer.c b/drivers/acpi/acpica/utbuffer.c
index bd31faf5da7c..ff2981275b9a 100644
--- a/drivers/acpi/acpica/utbuffer.c
+++ b/drivers/acpi/acpica/utbuffer.c
@@ -239,8 +239,7 @@ acpi_ut_dump_buffer_to_file(ACPI_FILE file,
 	u8 buf_char;
 
 	if (!buffer) {
-		acpi_ut_file_printf(file,
-				    "Null Buffer Pointer in DumpBuffer!\n");
+		fprintf(file, "Null Buffer Pointer in DumpBuffer!\n");
 		return;
 	}
 
@@ -254,7 +253,7 @@ acpi_ut_dump_buffer_to_file(ACPI_FILE file,
 
 		/* Print current offset */
 
-		acpi_ut_file_printf(file, "%6.4X: ", (base_offset + i));
+		fprintf(file, "%6.4X: ", (base_offset + i));
 
 		/* Print 16 hex chars */
 
@@ -263,8 +262,7 @@ acpi_ut_dump_buffer_to_file(ACPI_FILE file,
 
 				/* Dump fill spaces */
 
-				acpi_ut_file_printf(file, "%*s",
-						    ((display * 2) + 1), " ");
+				fprintf(file, "%*s", ((display * 2) + 1), " ");
 				j += display;
 				continue;
 			}
@@ -273,34 +271,34 @@ acpi_ut_dump_buffer_to_file(ACPI_FILE file,
 			case DB_BYTE_DISPLAY:
 			default:	/* Default is BYTE display */
 
-				acpi_ut_file_printf(file, "%02X ",
-						    buffer[(acpi_size)i + j]);
+				fprintf(file, "%02X ",
+					buffer[(acpi_size)i + j]);
 				break;
 
 			case DB_WORD_DISPLAY:
 
 				ACPI_MOVE_16_TO_32(&temp32,
 						   &buffer[(acpi_size)i + j]);
-				acpi_ut_file_printf(file, "%04X ", temp32);
+				fprintf(file, "%04X ", temp32);
 				break;
 
 			case DB_DWORD_DISPLAY:
 
 				ACPI_MOVE_32_TO_32(&temp32,
 						   &buffer[(acpi_size)i + j]);
-				acpi_ut_file_printf(file, "%08X ", temp32);
+				fprintf(file, "%08X ", temp32);
 				break;
 
 			case DB_QWORD_DISPLAY:
 
 				ACPI_MOVE_32_TO_32(&temp32,
 						   &buffer[(acpi_size)i + j]);
-				acpi_ut_file_printf(file, "%08X", temp32);
+				fprintf(file, "%08X", temp32);
 
 				ACPI_MOVE_32_TO_32(&temp32,
 						   &buffer[(acpi_size)i + j +
 							   4]);
-				acpi_ut_file_printf(file, "%08X ", temp32);
+				fprintf(file, "%08X ", temp32);
 				break;
 			}
 
@@ -311,24 +309,24 @@ acpi_ut_dump_buffer_to_file(ACPI_FILE file,
 		 * Print the ASCII equivalent characters but watch out for the bad
 		 * unprintable ones (printable chars are 0x20 through 0x7E)
 		 */
-		acpi_ut_file_printf(file, " ");
+		fprintf(file, " ");
 		for (j = 0; j < 16; j++) {
 			if (i + j >= count) {
-				acpi_ut_file_printf(file, "\n");
+				fprintf(file, "\n");
 				return;
 			}
 
 			buf_char = buffer[(acpi_size)i + j];
 			if (isprint(buf_char)) {
-				acpi_ut_file_printf(file, "%c", buf_char);
+				fprintf(file, "%c", buf_char);
 			} else {
-				acpi_ut_file_printf(file, ".");
+				fprintf(file, ".");
 			}
 		}
 
 		/* Done with that line. */
 
-		acpi_ut_file_printf(file, "\n");
+		fprintf(file, "\n");
 		i += 16;
 	}
 
diff --git a/drivers/acpi/acpica/utdebug.c b/drivers/acpi/acpica/utdebug.c
index 574422205005..044df9b0356e 100644
--- a/drivers/acpi/acpica/utdebug.c
+++ b/drivers/acpi/acpica/utdebug.c
@@ -562,6 +562,43 @@ acpi_ut_ptr_exit(u32 line_number,
 
 /*******************************************************************************
  *
+ * FUNCTION:    acpi_ut_str_exit
+ *
+ * PARAMETERS:  line_number         - Caller's line number
+ *              function_name       - Caller's procedure name
+ *              module_name         - Caller's module name
+ *              component_id        - Caller's component ID
+ *              string              - String to display
+ *
+ * RETURN:      None
+ *
+ * DESCRIPTION: Function exit trace. Prints only if TRACE_FUNCTIONS bit is
+ *              set in debug_level. Prints exit value also.
+ *
+ ******************************************************************************/
+
+void
+acpi_ut_str_exit(u32 line_number,
+		 const char *function_name,
+		 const char *module_name, u32 component_id, const char *string)
+{
+
+	/* Check if enabled up-front for performance */
+
+	if (ACPI_IS_DEBUG_ENABLED(ACPI_LV_FUNCTIONS, component_id)) {
+		acpi_debug_print(ACPI_LV_FUNCTIONS,
+				 line_number, function_name, module_name,
+				 component_id, "%s %s\n",
+				 acpi_gbl_function_exit_prefix, string);
+	}
+
+	if (acpi_gbl_nesting_level) {
+		acpi_gbl_nesting_level--;
+	}
+}
+
+/*******************************************************************************
+ *
  * FUNCTION:    acpi_trace_point
  *
  * PARAMETERS:  type                - Trace event type
@@ -591,27 +628,3 @@ acpi_trace_point(acpi_trace_event_type type, u8 begin, u8 *aml, char *pathname)
 
 ACPI_EXPORT_SYMBOL(acpi_trace_point)
 #endif
-#ifdef ACPI_APPLICATION
-/*******************************************************************************
- *
- * FUNCTION:    acpi_log_error
- *
- * PARAMETERS:  format              - Printf format field
- *              ...                 - Optional printf arguments
- *
- * RETURN:      None
- *
- * DESCRIPTION: Print error message to the console, used by applications.
- *
- ******************************************************************************/
-void ACPI_INTERNAL_VAR_XFACE acpi_log_error(const char *format, ...)
-{
-	va_list args;
-
-	va_start(args, format);
-	(void)acpi_ut_file_vprintf(ACPI_FILE_ERR, format, args);
-	va_end(args);
-}
-
-ACPI_EXPORT_SYMBOL(acpi_log_error)
-#endif
diff --git a/drivers/acpi/acpica/utdecode.c b/drivers/acpi/acpica/utdecode.c
index efd7988e34cb..15728ad8356b 100644
--- a/drivers/acpi/acpica/utdecode.c
+++ b/drivers/acpi/acpica/utdecode.c
@@ -253,7 +253,7 @@ const char *acpi_ut_get_object_type_name(union acpi_operand_object *obj_desc)
 		return_PTR("Invalid object");
 	}
 
-	return_PTR(acpi_ut_get_type_name(obj_desc->common.type));
+	return_STR(acpi_ut_get_type_name(obj_desc->common.type));
 }
 
 /*******************************************************************************
diff --git a/drivers/acpi/acpica/uthex.c b/drivers/acpi/acpica/uthex.c
index 4354fb800fe4..36d2fc789088 100644
--- a/drivers/acpi/acpica/uthex.c
+++ b/drivers/acpi/acpica/uthex.c
@@ -75,9 +75,41 @@ char acpi_ut_hex_to_ascii_char(u64 integer, u32 position)
 
 /*******************************************************************************
  *
+ * FUNCTION:    acpi_ut_ascii_to_hex_byte
+ *
+ * PARAMETERS:  two_ascii_chars             - Pointer to two ASCII characters
+ *              return_byte                 - Where converted byte is returned
+ *
+ * RETURN:      Status and converted hex byte
+ *
+ * DESCRIPTION: Perform ascii-to-hex translation, exactly two ASCII characters
+ *              to a single converted byte value.
+ *
+ ******************************************************************************/
+
+acpi_status acpi_ut_ascii_to_hex_byte(char *two_ascii_chars, u8 *return_byte)
+{
+
+	/* Both ASCII characters must be valid hex digits */
+
+	if (!isxdigit((int)two_ascii_chars[0]) ||
+	    !isxdigit((int)two_ascii_chars[1])) {
+		return (AE_BAD_HEX_CONSTANT);
+	}
+
+	*return_byte =
+	    acpi_ut_ascii_char_to_hex(two_ascii_chars[1]) |
+	    (acpi_ut_ascii_char_to_hex(two_ascii_chars[0]) << 4);
+
+	return (AE_OK);
+}
+
+/*******************************************************************************
+ *
  * FUNCTION:    acpi_ut_ascii_char_to_hex
  *
- * PARAMETERS:  hex_char                - Hex character in Ascii
+ * PARAMETERS:  hex_char                - Hex character in Ascii. Must be:
+ *                                        0-9 or A-F or a-f
  *
  * RETURN:      The binary value of the ascii/hex character
  *
@@ -88,13 +120,19 @@ char acpi_ut_hex_to_ascii_char(u64 integer, u32 position)
 u8 acpi_ut_ascii_char_to_hex(int hex_char)
 {
 
-	if (hex_char <= 0x39) {
-		return ((u8)(hex_char - 0x30));
+	/* Values 0-9 */
+
+	if (hex_char <= '9') {
+		return ((u8)(hex_char - '0'));
 	}
 
-	if (hex_char <= 0x46) {
+	/* Upper case A-F */
+
+	if (hex_char <= 'F') {
 		return ((u8)(hex_char - 0x37));
 	}
 
+	/* Lower case a-f */
+
 	return ((u8)(hex_char - 0x57));
 }
diff --git a/drivers/acpi/acpica/utinit.c b/drivers/acpi/acpica/utinit.c
index f91f724c487c..1711fdf41709 100644
--- a/drivers/acpi/acpica/utinit.c
+++ b/drivers/acpi/acpica/utinit.c
@@ -206,7 +206,7 @@ acpi_status acpi_ut_init_globals(void)
 	acpi_gbl_next_owner_id_offset = 0;
 	acpi_gbl_debugger_configuration = DEBUGGER_THREADING;
 	acpi_gbl_osi_mutex = NULL;
-	acpi_gbl_max_loop_iterations = 0xFFFF;
+	acpi_gbl_max_loop_iterations = ACPI_MAX_LOOP_COUNT;
 
 	/* Hardware oriented */
 
diff --git a/drivers/acpi/acpica/utnonansi.c b/drivers/acpi/acpica/utnonansi.c
index 3465fe2c5a5c..2514239282c2 100644
--- a/drivers/acpi/acpica/utnonansi.c
+++ b/drivers/acpi/acpica/utnonansi.c
@@ -48,8 +48,8 @@
 ACPI_MODULE_NAME("utnonansi")
 
 /*
- * Non-ANSI C library functions - strlwr, strupr, stricmp, and a 64-bit
- * version of strtoul.
+ * Non-ANSI C library functions - strlwr, strupr, stricmp, and "safe"
+ * string functions.
  */
 /*******************************************************************************
  *
@@ -200,356 +200,3 @@ acpi_ut_safe_strncat(char *dest,
 	return (FALSE);
 }
 #endif
-
-/*******************************************************************************
- *
- * FUNCTION:    acpi_ut_strtoul64
- *
- * PARAMETERS:  string                  - Null terminated string
- *              base                    - Radix of the string: 16 or 10 or
- *                                        ACPI_ANY_BASE
- *              max_integer_byte_width  - Maximum allowable integer,in bytes:
- *                                        4 or 8 (32 or 64 bits)
- *              ret_integer             - Where the converted integer is
- *                                        returned
- *
- * RETURN:      Status and Converted value
- *
- * DESCRIPTION: Convert a string into an unsigned value. Performs either a
- *              32-bit or 64-bit conversion, depending on the input integer
- *              size (often the current mode of the interpreter).
- *
- * NOTES:       Negative numbers are not supported, as they are not supported
- *              by ACPI.
- *
- *              acpi_gbl_integer_byte_width should be set to the proper width.
- *              For the core ACPICA code, this width depends on the DSDT
- *              version. For iASL, the default byte width is always 8 for the
- *              parser, but error checking is performed later to flag cases
- *              where a 64-bit constant is defined in a 32-bit DSDT/SSDT.
- *
- *              Does not support Octal strings, not needed at this time.
- *
- ******************************************************************************/
-
-acpi_status
-acpi_ut_strtoul64(char *string,
-		  u32 base, u32 max_integer_byte_width, u64 *ret_integer)
-{
-	u32 this_digit = 0;
-	u64 return_value = 0;
-	u64 quotient;
-	u64 dividend;
-	u8 valid_digits = 0;
-	u8 sign_of0x = 0;
-	u8 term = 0;
-
-	ACPI_FUNCTION_TRACE_STR(ut_strtoul64, string);
-
-	switch (base) {
-	case ACPI_ANY_BASE:
-	case 10:
-	case 16:
-
-		break;
-
-	default:
-
-		/* Invalid Base */
-
-		return_ACPI_STATUS(AE_BAD_PARAMETER);
-	}
-
-	if (!string) {
-		goto error_exit;
-	}
-
-	/* Skip over any white space in the buffer */
-
-	while ((*string) && (isspace((int)*string) || *string == '\t')) {
-		string++;
-	}
-
-	if (base == ACPI_ANY_BASE) {
-		/*
-		 * Base equal to ACPI_ANY_BASE means 'Either decimal or hex'.
-		 * We need to determine if it is decimal or hexadecimal.
-		 */
-		if ((*string == '0') && (tolower((int)*(string + 1)) == 'x')) {
-			sign_of0x = 1;
-			base = 16;
-
-			/* Skip over the leading '0x' */
-			string += 2;
-		} else {
-			base = 10;
-		}
-	}
-
-	/* Any string left? Check that '0x' is not followed by white space. */
-
-	if (!(*string) || isspace((int)*string) || *string == '\t') {
-		if (base == ACPI_ANY_BASE) {
-			goto error_exit;
-		} else {
-			goto all_done;
-		}
-	}
-
-	/*
-	 * Perform a 32-bit or 64-bit conversion, depending upon the input
-	 * byte width
-	 */
-	dividend = (max_integer_byte_width <= ACPI_MAX32_BYTE_WIDTH) ?
-	    ACPI_UINT32_MAX : ACPI_UINT64_MAX;
-
-	/* Main loop: convert the string to a 32- or 64-bit integer */
-
-	while (*string) {
-		if (isdigit((int)*string)) {
-
-			/* Convert ASCII 0-9 to Decimal value */
-
-			this_digit = ((u8)*string) - '0';
-		} else if (base == 10) {
-
-			/* Digit is out of range; possible in to_integer case only */
-
-			term = 1;
-		} else {
-			this_digit = (u8)toupper((int)*string);
-			if (isxdigit((int)this_digit)) {
-
-				/* Convert ASCII Hex char to value */
-
-				this_digit = this_digit - 'A' + 10;
-			} else {
-				term = 1;
-			}
-		}
-
-		if (term) {
-			if (base == ACPI_ANY_BASE) {
-				goto error_exit;
-			} else {
-				break;
-			}
-		} else if ((valid_digits == 0) && (this_digit == 0)
-			   && !sign_of0x) {
-
-			/* Skip zeros */
-			string++;
-			continue;
-		}
-
-		valid_digits++;
-
-		if (sign_of0x && ((valid_digits > 16) ||
-				  ((valid_digits > 8)
-				   && (max_integer_byte_width <=
-				       ACPI_MAX32_BYTE_WIDTH)))) {
-			/*
-			 * This is to_integer operation case.
-			 * No restrictions for string-to-integer conversion,
-			 * see ACPI spec.
-			 */
-			goto error_exit;
-		}
-
-		/* Divide the digit into the correct position */
-
-		(void)acpi_ut_short_divide((dividend - (u64)this_digit), base,
-					   &quotient, NULL);
-
-		if (return_value > quotient) {
-			if (base == ACPI_ANY_BASE) {
-				goto error_exit;
-			} else {
-				break;
-			}
-		}
-
-		return_value *= base;
-		return_value += this_digit;
-		string++;
-	}
-
-	/* All done, normal exit */
-
-all_done:
-
-	ACPI_DEBUG_PRINT((ACPI_DB_EXEC, "Converted value: %8.8X%8.8X\n",
-			  ACPI_FORMAT_UINT64(return_value)));
-
-	*ret_integer = return_value;
-	return_ACPI_STATUS(AE_OK);
-
-error_exit:
-
-	/* Base was set/validated above (10 or 16) */
-
-	if (base == 10) {
-		return_ACPI_STATUS(AE_BAD_DECIMAL_CONSTANT);
-	} else {
-		return_ACPI_STATUS(AE_BAD_HEX_CONSTANT);
-	}
-}
-
-#ifdef _OBSOLETE_FUNCTIONS
-/* Removed: 01/2016 */
-
-/*******************************************************************************
- *
- * FUNCTION:    strtoul64
- *
- * PARAMETERS:  string              - Null terminated string
- *              terminater          - Where a pointer to the terminating byte
- *                                    is returned
- *              base                - Radix of the string
- *
- * RETURN:      Converted value
- *
- * DESCRIPTION: Convert a string into an unsigned value.
- *
- ******************************************************************************/
-
-acpi_status strtoul64(char *string, u32 base, u64 *ret_integer)
-{
-	u32 index;
-	u32 sign;
-	u64 return_value = 0;
-	acpi_status status = AE_OK;
-
-	*ret_integer = 0;
-
-	switch (base) {
-	case 0:
-	case 8:
-	case 10:
-	case 16:
-
-		break;
-
-	default:
-		/*
-		 * The specified Base parameter is not in the domain of
-		 * this function:
-		 */
-		return (AE_BAD_PARAMETER);
-	}
-
-	/* Skip over any white space in the buffer: */
-
-	while (isspace((int)*string) || *string == '\t') {
-		++string;
-	}
-
-	/*
-	 * The buffer may contain an optional plus or minus sign.
-	 * If it does, then skip over it but remember what is was:
-	 */
-	if (*string == '-') {
-		sign = ACPI_SIGN_NEGATIVE;
-		++string;
-	} else if (*string == '+') {
-		++string;
-		sign = ACPI_SIGN_POSITIVE;
-	} else {
-		sign = ACPI_SIGN_POSITIVE;
-	}
-
-	/*
-	 * If the input parameter Base is zero, then we need to
-	 * determine if it is octal, decimal, or hexadecimal:
-	 */
-	if (base == 0) {
-		if (*string == '0') {
-			if (tolower((int)*(++string)) == 'x') {
-				base = 16;
-				++string;
-			} else {
-				base = 8;
-			}
-		} else {
-			base = 10;
-		}
-	}
-
-	/*
-	 * For octal and hexadecimal bases, skip over the leading
-	 * 0 or 0x, if they are present.
-	 */
-	if (base == 8 && *string == '0') {
-		string++;
-	}
-
-	if (base == 16 && *string == '0' && tolower((int)*(++string)) == 'x') {
-		string++;
-	}
-
-	/* Main loop: convert the string to an unsigned long */
-
-	while (*string) {
-		if (isdigit((int)*string)) {
-			index = ((u8)*string) - '0';
-		} else {
-			index = (u8)toupper((int)*string);
-			if (isupper((int)index)) {
-				index = index - 'A' + 10;
-			} else {
-				goto error_exit;
-			}
-		}
-
-		if (index >= base) {
-			goto error_exit;
-		}
-
-		/* Check to see if value is out of range: */
-
-		if (return_value > ((ACPI_UINT64_MAX - (u64)index) / (u64)base)) {
-			goto error_exit;
-		} else {
-			return_value *= base;
-			return_value += index;
-		}
-
-		++string;
-	}
-
-	/* If a minus sign was present, then "the conversion is negated": */
-
-	if (sign == ACPI_SIGN_NEGATIVE) {
-		return_value = (ACPI_UINT32_MAX - return_value) + 1;
-	}
-
-	*ret_integer = return_value;
-	return (status);
-
-error_exit:
-	switch (base) {
-	case 8:
-
-		status = AE_BAD_OCTAL_CONSTANT;
-		break;
-
-	case 10:
-
-		status = AE_BAD_DECIMAL_CONSTANT;
-		break;
-
-	case 16:
-
-		status = AE_BAD_HEX_CONSTANT;
-		break;
-
-	default:
-
-		/* Base validated above */
-
-		break;
-	}
-
-	return (status);
-}
-#endif
diff --git a/drivers/acpi/acpica/utosi.c b/drivers/acpi/acpica/utosi.c
index 3f5fed670271..f0484b058c44 100644
--- a/drivers/acpi/acpica/utosi.c
+++ b/drivers/acpi/acpica/utosi.c
@@ -390,11 +390,22 @@ struct acpi_interface_info *acpi_ut_get_interface(acpi_string interface_name)
  * PARAMETERS:  walk_state          - Current walk state
  *
  * RETURN:      Status
+ *              Integer: TRUE (0) if input string is matched
+ *                       FALSE (-1) if string is not matched
  *
  * DESCRIPTION: Implementation of the _OSI predefined control method. When
  *              an invocation of _OSI is encountered in the system AML,
  *              control is transferred to this function.
  *
+ * (August 2016)
+ * Note:  _OSI is now defined to return "Ones" to indicate a match, for
+ * compatibility with other ACPI implementations. On a 32-bit DSDT, Ones
+ * is 0xFFFFFFFF. On a 64-bit DSDT, Ones is 0xFFFFFFFFFFFFFFFF
+ * (ACPI_UINT64_MAX).
+ *
+ * This function always returns ACPI_UINT64_MAX for TRUE, and later code
+ * will truncate this to 32 bits if necessary.
+ *
  ******************************************************************************/
 
 acpi_status acpi_ut_osi_implementation(struct acpi_walk_state *walk_state)
@@ -404,7 +415,7 @@ acpi_status acpi_ut_osi_implementation(struct acpi_walk_state *walk_state)
 	struct acpi_interface_info *interface_info;
 	acpi_interface_handler interface_handler;
 	acpi_status status;
-	u32 return_value;
+	u64 return_value;
 
 	ACPI_FUNCTION_TRACE(ut_osi_implementation);
 
@@ -444,7 +455,7 @@ acpi_status acpi_ut_osi_implementation(struct acpi_walk_state *walk_state)
 			acpi_gbl_osi_data = interface_info->value;
 		}
 
-		return_value = ACPI_UINT32_MAX;
+		return_value = ACPI_UINT64_MAX;
 	}
 
 	acpi_os_release_mutex(acpi_gbl_osi_mutex);
@@ -456,9 +467,10 @@ acpi_status acpi_ut_osi_implementation(struct acpi_walk_state *walk_state)
 	 */
 	interface_handler = acpi_gbl_interface_handler;
 	if (interface_handler) {
-		return_value =
-		    interface_handler(string_desc->string.pointer,
-				      return_value);
+		if (interface_handler
+		    (string_desc->string.pointer, (u32)return_value)) {
+			return_value = ACPI_UINT64_MAX;
+		}
 	}
 
 	ACPI_DEBUG_PRINT_RAW((ACPI_DB_INFO,
diff --git a/drivers/acpi/acpica/utpredef.c b/drivers/acpi/acpica/utpredef.c
index 770a1775b264..ce18346b6144 100644
--- a/drivers/acpi/acpica/utpredef.c
+++ b/drivers/acpi/acpica/utpredef.c
@@ -176,8 +176,6 @@ void acpi_ut_get_expected_return_types(char *buffer, u32 expected_btypes)
  ******************************************************************************/
 
 #if (defined ACPI_ASL_COMPILER || defined ACPI_HELP_APP)
-#include <stdio.h>
-#include <string.h>
 
 /* Local prototypes */
 
diff --git a/drivers/acpi/acpica/utprint.c b/drivers/acpi/acpica/utprint.c
index dd084cf52502..40eba804d49c 100644
--- a/drivers/acpi/acpica/utprint.c
+++ b/drivers/acpi/acpica/utprint.c
@@ -336,7 +336,7 @@ static char *acpi_ut_format_number(char *string,
 
 /*******************************************************************************
  *
- * FUNCTION:    acpi_ut_vsnprintf
+ * FUNCTION:    vsnprintf
  *
  * PARAMETERS:  string              - String with boundary
  *              size                - Boundary of the string
@@ -349,9 +349,7 @@ static char *acpi_ut_format_number(char *string,
  *
  ******************************************************************************/
 
-int
-acpi_ut_vsnprintf(char *string,
-		  acpi_size size, const char *format, va_list args)
+int vsnprintf(char *string, acpi_size size, const char *format, va_list args)
 {
 	u8 base;
 	u8 type;
@@ -586,7 +584,7 @@ acpi_ut_vsnprintf(char *string,
 
 /*******************************************************************************
  *
- * FUNCTION:    acpi_ut_snprintf
+ * FUNCTION:    snprintf
  *
  * PARAMETERS:  string              - String with boundary
  *              size                - Boundary of the string
@@ -598,13 +596,38 @@ acpi_ut_vsnprintf(char *string,
  *
  ******************************************************************************/
 
-int acpi_ut_snprintf(char *string, acpi_size size, const char *format, ...)
+int snprintf(char *string, acpi_size size, const char *format, ...)
 {
 	va_list args;
 	int length;
 
 	va_start(args, format);
-	length = acpi_ut_vsnprintf(string, size, format, args);
+	length = vsnprintf(string, size, format, args);
+	va_end(args);
+
+	return (length);
+}
+
+/*******************************************************************************
+ *
+ * FUNCTION:    sprintf
+ *
+ * PARAMETERS:  string              - String with boundary
+ *              Format, ...         - Standard printf format
+ *
+ * RETURN:      Number of bytes actually written.
+ *
+ * DESCRIPTION: Formatted output to a string.
+ *
+ ******************************************************************************/
+
+int sprintf(char *string, const char *format, ...)
+{
+	va_list args;
+	int length;
+
+	va_start(args, format);
+	length = vsnprintf(string, ACPI_UINT32_MAX, format, args);
 	va_end(args);
 
 	return (length);
@@ -613,7 +636,59 @@ int acpi_ut_snprintf(char *string, acpi_size size, const char *format, ...)
 #ifdef ACPI_APPLICATION
 /*******************************************************************************
  *
- * FUNCTION:    acpi_ut_file_vprintf
+ * FUNCTION:    vprintf
+ *
+ * PARAMETERS:  format              - Standard printf format
+ *              args                - Argument list
+ *
+ * RETURN:      Number of bytes actually written.
+ *
+ * DESCRIPTION: Formatted output to stdout using argument list pointer.
+ *
+ ******************************************************************************/
+
+int vprintf(const char *format, va_list args)
+{
+	acpi_cpu_flags flags;
+	int length;
+
+	flags = acpi_os_acquire_lock(acpi_gbl_print_lock);
+	length = vsnprintf(acpi_gbl_print_buffer,
+			   sizeof(acpi_gbl_print_buffer), format, args);
+
+	(void)fwrite(acpi_gbl_print_buffer, length, 1, ACPI_FILE_OUT);
+	acpi_os_release_lock(acpi_gbl_print_lock, flags);
+
+	return (length);
+}
+
+/*******************************************************************************
+ *
+ * FUNCTION:    printf
+ *
+ * PARAMETERS:  Format, ...         - Standard printf format
+ *
+ * RETURN:      Number of bytes actually written.
+ *
+ * DESCRIPTION: Formatted output to stdout.
+ *
+ ******************************************************************************/
+
+int printf(const char *format, ...)
+{
+	va_list args;
+	int length;
+
+	va_start(args, format);
+	length = vprintf(format, args);
+	va_end(args);
+
+	return (length);
+}
+
+/*******************************************************************************
+ *
+ * FUNCTION:    vfprintf
  *
  * PARAMETERS:  file                - File descriptor
  *              format              - Standard printf format
@@ -625,16 +700,16 @@ int acpi_ut_snprintf(char *string, acpi_size size, const char *format, ...)
  *
  ******************************************************************************/
 
-int acpi_ut_file_vprintf(ACPI_FILE file, const char *format, va_list args)
+int vfprintf(FILE * file, const char *format, va_list args)
 {
 	acpi_cpu_flags flags;
 	int length;
 
 	flags = acpi_os_acquire_lock(acpi_gbl_print_lock);
-	length = acpi_ut_vsnprintf(acpi_gbl_print_buffer,
-				   sizeof(acpi_gbl_print_buffer), format, args);
+	length = vsnprintf(acpi_gbl_print_buffer,
+			   sizeof(acpi_gbl_print_buffer), format, args);
 
-	(void)acpi_os_write_file(file, acpi_gbl_print_buffer, length, 1);
+	(void)fwrite(acpi_gbl_print_buffer, length, 1, file);
 	acpi_os_release_lock(acpi_gbl_print_lock, flags);
 
 	return (length);
@@ -642,7 +717,7 @@ int acpi_ut_file_vprintf(ACPI_FILE file, const char *format, va_list args)
 
 /*******************************************************************************
  *
- * FUNCTION:    acpi_ut_file_printf
+ * FUNCTION:    fprintf
  *
  * PARAMETERS:  file                - File descriptor
  *              Format, ...         - Standard printf format
@@ -653,13 +728,13 @@ int acpi_ut_file_vprintf(ACPI_FILE file, const char *format, va_list args)
  *
  ******************************************************************************/
 
-int acpi_ut_file_printf(ACPI_FILE file, const char *format, ...)
+int fprintf(FILE * file, const char *format, ...)
 {
 	va_list args;
 	int length;
 
 	va_start(args, format);
-	length = acpi_ut_file_vprintf(file, format, args);
+	length = vfprintf(file, format, args);
 	va_end(args);
 
 	return (length);
diff --git a/drivers/acpi/acpica/utstrtoul64.c b/drivers/acpi/acpica/utstrtoul64.c
new file mode 100644
index 000000000000..b4f341c98a95
--- /dev/null
+++ b/drivers/acpi/acpica/utstrtoul64.c
@@ -0,0 +1,348 @@
+/*******************************************************************************
+ *
+ * Module Name: utstrtoul64 - string to 64-bit integer support
+ *
+ ******************************************************************************/
+
+/*
+ * Copyright (C) 2000 - 2016, Intel Corp.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions, and the following disclaimer,
+ *    without modification.
+ * 2. Redistributions in binary form must reproduce at minimum a disclaimer
+ *    substantially similar to the "NO WARRANTY" disclaimer below
+ *    ("Disclaimer") and any redistribution must be conditioned upon
+ *    including a substantially similar Disclaimer requirement for further
+ *    binary redistribution.
+ * 3. Neither the names of the above-listed copyright holders nor the names
+ *    of any contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * Alternatively, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") version 2 as published by the Free
+ * Software Foundation.
+ *
+ * NO WARRANTY
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTIBILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * HOLDERS OR CONTRIBUTORS BE LIABLE FOR SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
+ * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
+ * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGES.
+ */
+
+#include <acpi/acpi.h>
+#include "accommon.h"
+
+/*******************************************************************************
+ *
+ * The functions in this module satisfy the need for 64-bit string-to-integer
+ * conversions on both 32-bit and 64-bit platforms.
+ *
+ ******************************************************************************/
+
+#define _COMPONENT          ACPI_UTILITIES
+ACPI_MODULE_NAME("utstrtoul64")
+
+/* Local prototypes */
+static u64 acpi_ut_strtoul_base10(char *string, u32 flags);
+
+static u64 acpi_ut_strtoul_base16(char *string, u32 flags);
+
+/*******************************************************************************
+ *
+ * String conversion rules as written in the ACPI specification. The error
+ * conditions and behavior are different depending on the type of conversion.
+ *
+ *
+ * Implicit data type conversion: string-to-integer
+ * --------------------------------------------------
+ *
+ * Base is always 16. This is the ACPI_STRTOUL_BASE16 case.
+ *
+ * Example:
+ *      Add ("BA98", Arg0, Local0)
+ *
+ * The integer is initialized to the value zero.
+ * The ASCII string is interpreted as a hexadecimal constant.
+ *
+ *  1)  A "0x" prefix is not allowed. However, ACPICA allows this for
+ *      compatibility with previous ACPICA. (NO ERROR)
+ *
+ *  2)  Terminates when the size of an integer is reached (32 or 64 bits).
+ *      (NO ERROR)
+ *
+ *  3)  The first non-hex character terminates the conversion without error.
+ *      (NO ERROR)
+ *
+ *  4)  Conversion of a null (zero-length) string to an integer is not
+ *      allowed. However, ACPICA allows this for compatibility with previous
+ *      ACPICA. This conversion returns the value 0. (NO ERROR)
+ *
+ *
+ * Explicit data type conversion:  to_integer() with string operand
+ * ---------------------------------------------------------------
+ *
+ * Base is either 10 (default) or 16 (with 0x prefix)
+ *
+ * Examples:
+ *      to_integer ("1000")
+ *      to_integer ("0xABCD")
+ *
+ *  1)  Can be (must be) either a decimal or hexadecimal numeric string.
+ *      A hex value must be prefixed by "0x" or it is interpreted as a decimal.
+ *
+ *  2)  The value must not exceed the maximum of an integer value. ACPI spec
+ *      states the behavior is "unpredictable", so ACPICA matches the behavior
+ *      of the implicit conversion case.(NO ERROR)
+ *
+ *  3)  Behavior on the first non-hex character is not specified by the ACPI
+ *      spec, so ACPICA matches the behavior of the implicit conversion case
+ *      and terminates. (NO ERROR)
+ *
+ *  4)  A null (zero-length) string is illegal.
+ *      However, ACPICA allows this for compatibility with previous ACPICA.
+ *      This conversion returns the value 0. (NO ERROR)
+ *
+ ******************************************************************************/
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_ut_strtoul64
+ *
+ * PARAMETERS:  string                  - Null terminated input string
+ *              flags                   - Conversion info, see below
+ *              return_value            - Where the converted integer is
+ *                                        returned
+ *
+ * RETURN:      Status and Converted value
+ *
+ * DESCRIPTION: Convert a string into an unsigned value. Performs either a
+ *              32-bit or 64-bit conversion, depending on the input integer
+ *              size in Flags (often the current mode of the interpreter).
+ *
+ * Values for Flags:
+ *      ACPI_STRTOUL_32BIT      - Max integer value is 32 bits
+ *      ACPI_STRTOUL_64BIT      - Max integer value is 64 bits
+ *      ACPI_STRTOUL_BASE16     - Input string is hexadecimal. Default
+ *                                is 10/16 based on string prefix (0x).
+ *
+ * NOTES:
+ *   Negative numbers are not supported, as they are not supported by ACPI.
+ *
+ *   Supports only base 16 or base 10 strings/values. Does not
+ *   support Octal strings, as these are not supported by ACPI.
+ *
+ * Current users of this support:
+ *
+ *  interpreter - Implicit and explicit conversions, GPE method names
+ *  debugger    - Command line input string conversion
+ *  iASL        - Main parser, conversion of constants to integers
+ *  iASL        - Data Table Compiler parser (constant math expressions)
+ *  iASL        - Preprocessor (constant math expressions)
+ *  acpi_dump   - Input table addresses
+ *  acpi_exec   - Testing of the acpi_ut_strtoul64 function
+ *
+ * Note concerning callers:
+ *   acpi_gbl_integer_byte_width can be used to set the 32/64 limit. If used,
+ *   this global should be set to the proper width. For the core ACPICA code,
+ *   this width depends on the DSDT version. For iASL, the default byte
+ *   width is always 8 for the parser, but error checking is performed later
+ *   to flag cases where a 64-bit constant is defined in a 32-bit DSDT/SSDT.
+ *
+ ******************************************************************************/
+
+acpi_status acpi_ut_strtoul64(char *string, u32 flags, u64 *return_value)
+{
+	acpi_status status = AE_OK;
+	u32 base;
+
+	ACPI_FUNCTION_TRACE_STR(ut_strtoul64, string);
+
+	/* Parameter validation */
+
+	if (!string || !return_value) {
+		return_ACPI_STATUS(AE_BAD_PARAMETER);
+	}
+
+	*return_value = 0;
+
+	/* Check for zero-length string, returns 0 */
+
+	if (*string == 0) {
+		return_ACPI_STATUS(AE_OK);
+	}
+
+	/* Skip over any white space at start of string */
+
+	while (isspace((int)*string)) {
+		string++;
+	}
+
+	/* End of string? return 0 */
+
+	if (*string == 0) {
+		return_ACPI_STATUS(AE_OK);
+	}
+
+	/*
+	 * 1) The "0x" prefix indicates base 16. Per the ACPI specification,
+	 * the "0x" prefix is only allowed for implicit (non-strict) conversions.
+	 * However, we always allow it for compatibility with older ACPICA.
+	 */
+	if ((*string == ACPI_ASCII_ZERO) &&
+	    (tolower((int)*(string + 1)) == 'x')) {
+		string += 2;	/* Go past the 0x */
+		if (*string == 0) {
+			return_ACPI_STATUS(AE_OK);	/* Return value 0 */
+		}
+
+		base = 16;
+	}
+
+	/* 2) Force to base 16 (implicit conversion case) */
+
+	else if (flags & ACPI_STRTOUL_BASE16) {
+		base = 16;
+	}
+
+	/* 3) Default fallback is to Base 10 */
+
+	else {
+		base = 10;
+	}
+
+	/* Skip all leading zeros */
+
+	while (*string == ACPI_ASCII_ZERO) {
+		string++;
+		if (*string == 0) {
+			return_ACPI_STATUS(AE_OK);	/* Return value 0 */
+		}
+	}
+
+	/* Perform the base 16 or 10 conversion */
+
+	if (base == 16) {
+		*return_value = acpi_ut_strtoul_base16(string, flags);
+	} else {
+		*return_value = acpi_ut_strtoul_base10(string, flags);
+	}
+
+	return_ACPI_STATUS(status);
+}
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_ut_strtoul_base10
+ *
+ * PARAMETERS:  string                  - Null terminated input string
+ *              flags                   - Conversion info
+ *
+ * RETURN:      64-bit converted integer
+ *
+ * DESCRIPTION: Performs a base 10 conversion of the input string to an
+ *              integer value, either 32 or 64 bits.
+ *              Note: String must be valid and non-null.
+ *
+ ******************************************************************************/
+
+static u64 acpi_ut_strtoul_base10(char *string, u32 flags)
+{
+	int ascii_digit;
+	u64 next_value;
+	u64 return_value = 0;
+
+	/* Main loop: convert each ASCII byte in the input string */
+
+	while (*string) {
+		ascii_digit = *string;
+		if (!isdigit(ascii_digit)) {
+
+			/* Not ASCII 0-9, terminate */
+
+			goto exit;
+		}
+
+		/* Convert and insert (add) the decimal digit */
+
+		next_value =
+		    (return_value * 10) + (ascii_digit - ACPI_ASCII_ZERO);
+
+		/* Check for overflow (32 or 64 bit) - return current converted value */
+
+		if (((flags & ACPI_STRTOUL_32BIT) && (next_value > ACPI_UINT32_MAX)) || (next_value < return_value)) {	/* 64-bit overflow case */
+			goto exit;
+		}
+
+		return_value = next_value;
+		string++;
+	}
+
+exit:
+	return (return_value);
+}
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_ut_strtoul_base16
+ *
+ * PARAMETERS:  string                  - Null terminated input string
+ *              flags                   - conversion info
+ *
+ * RETURN:      64-bit converted integer
+ *
+ * DESCRIPTION: Performs a base 16 conversion of the input string to an
+ *              integer value, either 32 or 64 bits.
+ *              Note: String must be valid and non-null.
+ *
+ ******************************************************************************/
+
+static u64 acpi_ut_strtoul_base16(char *string, u32 flags)
+{
+	int ascii_digit;
+	u32 valid_digits = 1;
+	u64 return_value = 0;
+
+	/* Main loop: convert each ASCII byte in the input string */
+
+	while (*string) {
+
+		/* Check for overflow (32 or 64 bit) - return current converted value */
+
+		if ((valid_digits > 16) ||
+		    ((valid_digits > 8) && (flags & ACPI_STRTOUL_32BIT))) {
+			goto exit;
+		}
+
+		ascii_digit = *string;
+		if (!isxdigit(ascii_digit)) {
+
+			/* Not Hex ASCII A-F, a-f, or 0-9, terminate */
+
+			goto exit;
+		}
+
+		/* Convert and insert the hex digit */
+
+		return_value =
+		    (return_value << 4) |
+		    acpi_ut_ascii_char_to_hex(ascii_digit);
+
+		string++;
+		valid_digits++;
+	}
+
+exit:
+	return (return_value);
+}
diff --git a/drivers/acpi/acpica/uttrack.c b/drivers/acpi/acpica/uttrack.c
index 0df07dfa53b6..df31d71ce596 100644
--- a/drivers/acpi/acpica/uttrack.c
+++ b/drivers/acpi/acpica/uttrack.c
@@ -95,13 +95,11 @@ acpi_ut_create_list(const char *list_name,
 {
 	struct acpi_memory_list *cache;
 
-	cache = acpi_os_allocate(sizeof(struct acpi_memory_list));
+	cache = acpi_os_allocate_zeroed(sizeof(struct acpi_memory_list));
 	if (!cache) {
 		return (AE_NO_MEMORY);
 	}
 
-	memset(cache, 0, sizeof(struct acpi_memory_list));
-
 	cache->list_name = list_name;
 	cache->object_size = object_size;
 
diff --git a/drivers/acpi/acpica/utxface.c b/drivers/acpi/acpica/utxface.c
index d9e6aac7dc83..ec503c862961 100644
--- a/drivers/acpi/acpica/utxface.c
+++ b/drivers/acpi/acpica/utxface.c
@@ -61,7 +61,7 @@ ACPI_MODULE_NAME("utxface")
  * DESCRIPTION: Shutdown the ACPICA subsystem and release all resources.
  *
  ******************************************************************************/
-acpi_status __init acpi_terminate(void)
+acpi_status ACPI_INIT_FUNCTION acpi_terminate(void)
 {
 	acpi_status status;
 
diff --git a/drivers/acpi/acpica/utxfinit.c b/drivers/acpi/acpica/utxfinit.c
index 75b5f27da267..a5ca0f57cd08 100644
--- a/drivers/acpi/acpica/utxfinit.c
+++ b/drivers/acpi/acpica/utxfinit.c
@@ -69,7 +69,7 @@ void ae_do_object_overrides(void);
  *
  ******************************************************************************/
 
-acpi_status __init acpi_initialize_subsystem(void)
+acpi_status ACPI_INIT_FUNCTION acpi_initialize_subsystem(void)
 {
 	acpi_status status;
 
@@ -141,7 +141,7 @@ ACPI_EXPORT_SYMBOL_INIT(acpi_initialize_subsystem)
  *              Puts system into ACPI mode if it isn't already.
  *
  ******************************************************************************/
-acpi_status __init acpi_enable_subsystem(u32 flags)
+acpi_status ACPI_INIT_FUNCTION acpi_enable_subsystem(u32 flags)
 {
 	acpi_status status = AE_OK;
 
@@ -239,7 +239,7 @@ ACPI_EXPORT_SYMBOL_INIT(acpi_enable_subsystem)
  *              objects and executing AML code for Regions, buffers, etc.
  *
  ******************************************************************************/
-acpi_status __init acpi_initialize_objects(u32 flags)
+acpi_status ACPI_INIT_FUNCTION acpi_initialize_objects(u32 flags)
 {
 	acpi_status status = AE_OK;
 
@@ -265,7 +265,8 @@ acpi_status __init acpi_initialize_objects(u32 flags)
 	 * all of the tables have been loaded. It is a legacy option and is
 	 * not compatible with other ACPI implementations. See acpi_ns_load_table.
 	 */
-	if (acpi_gbl_group_module_level_code) {
+	if (!acpi_gbl_parse_table_as_term_list
+	    && acpi_gbl_group_module_level_code) {
 		acpi_ns_exec_module_code_list();
 
 		/*
diff --git a/drivers/acpi/apei/erst.c b/drivers/acpi/apei/erst.c
index f096ab3cb54d..ec4f507b524f 100644
--- a/drivers/acpi/apei/erst.c
+++ b/drivers/acpi/apei/erst.c
@@ -938,7 +938,7 @@ static int erst_clearer(enum pstore_type_id type, u64 id, int count,
 static struct pstore_info erst_info = {
 	.owner		= THIS_MODULE,
 	.name		= "erst",
-	.flags		= PSTORE_FLAGS_FRAGILE,
+	.flags		= PSTORE_FLAGS_DMESG,
 	.open		= erst_open_pstore,
 	.close		= erst_close_pstore,
 	.read		= erst_reader,
diff --git a/drivers/acpi/apei/ghes.c b/drivers/acpi/apei/ghes.c
index 60746ef904e4..f0a029e68d3e 100644
--- a/drivers/acpi/apei/ghes.c
+++ b/drivers/acpi/apei/ghes.c
@@ -457,7 +457,7 @@ static void ghes_do_proc(struct ghes *ghes,
 
 				devfn = PCI_DEVFN(pcie_err->device_id.device,
 						  pcie_err->device_id.function);
-				aer_severity = cper_severity_to_aer(sev);
+				aer_severity = cper_severity_to_aer(gdata->error_severity);
 
 				/*
 				 * If firmware reset the component to contain
diff --git a/drivers/acpi/arm64/Kconfig b/drivers/acpi/arm64/Kconfig
new file mode 100644
index 000000000000..4616da4c15be
--- /dev/null
+++ b/drivers/acpi/arm64/Kconfig
@@ -0,0 +1,6 @@
+#
+# ACPI Configuration for ARM64
+#
+
+config ACPI_IORT
+	bool
diff --git a/drivers/acpi/arm64/Makefile b/drivers/acpi/arm64/Makefile
new file mode 100644
index 000000000000..72331f2ce0e9
--- /dev/null
+++ b/drivers/acpi/arm64/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_ACPI_IORT) 	+= iort.o
diff --git a/drivers/acpi/arm64/iort.c b/drivers/acpi/arm64/iort.c
new file mode 100644
index 000000000000..6b81746cd13c
--- /dev/null
+++ b/drivers/acpi/arm64/iort.c
@@ -0,0 +1,368 @@
+/*
+ * Copyright (C) 2016, Semihalf
+ *	Author: Tomasz Nowicki <tn@semihalf.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.
+ *
+ * This file implements early detection/parsing of I/O mapping
+ * reported to OS through firmware via I/O Remapping Table (IORT)
+ * IORT document number: ARM DEN 0049A
+ */
+
+#define pr_fmt(fmt)	"ACPI: IORT: " fmt
+
+#include <linux/acpi_iort.h>
+#include <linux/kernel.h>
+#include <linux/pci.h>
+
+struct iort_its_msi_chip {
+	struct list_head	list;
+	struct fwnode_handle	*fw_node;
+	u32			translation_id;
+};
+
+typedef acpi_status (*iort_find_node_callback)
+	(struct acpi_iort_node *node, void *context);
+
+/* Root pointer to the mapped IORT table */
+static struct acpi_table_header *iort_table;
+
+static LIST_HEAD(iort_msi_chip_list);
+static DEFINE_SPINLOCK(iort_msi_chip_lock);
+
+/**
+ * iort_register_domain_token() - register domain token and related ITS ID
+ * to the list from where we can get it back later on.
+ * @trans_id: ITS ID.
+ * @fw_node: Domain token.
+ *
+ * Returns: 0 on success, -ENOMEM if no memory when allocating list element
+ */
+int iort_register_domain_token(int trans_id, struct fwnode_handle *fw_node)
+{
+	struct iort_its_msi_chip *its_msi_chip;
+
+	its_msi_chip = kzalloc(sizeof(*its_msi_chip), GFP_KERNEL);
+	if (!its_msi_chip)
+		return -ENOMEM;
+
+	its_msi_chip->fw_node = fw_node;
+	its_msi_chip->translation_id = trans_id;
+
+	spin_lock(&iort_msi_chip_lock);
+	list_add(&its_msi_chip->list, &iort_msi_chip_list);
+	spin_unlock(&iort_msi_chip_lock);
+
+	return 0;
+}
+
+/**
+ * iort_deregister_domain_token() - Deregister domain token based on ITS ID
+ * @trans_id: ITS ID.
+ *
+ * Returns: none.
+ */
+void iort_deregister_domain_token(int trans_id)
+{
+	struct iort_its_msi_chip *its_msi_chip, *t;
+
+	spin_lock(&iort_msi_chip_lock);
+	list_for_each_entry_safe(its_msi_chip, t, &iort_msi_chip_list, list) {
+		if (its_msi_chip->translation_id == trans_id) {
+			list_del(&its_msi_chip->list);
+			kfree(its_msi_chip);
+			break;
+		}
+	}
+	spin_unlock(&iort_msi_chip_lock);
+}
+
+/**
+ * iort_find_domain_token() - Find domain token based on given ITS ID
+ * @trans_id: ITS ID.
+ *
+ * Returns: domain token when find on the list, NULL otherwise
+ */
+struct fwnode_handle *iort_find_domain_token(int trans_id)
+{
+	struct fwnode_handle *fw_node = NULL;
+	struct iort_its_msi_chip *its_msi_chip;
+
+	spin_lock(&iort_msi_chip_lock);
+	list_for_each_entry(its_msi_chip, &iort_msi_chip_list, list) {
+		if (its_msi_chip->translation_id == trans_id) {
+			fw_node = its_msi_chip->fw_node;
+			break;
+		}
+	}
+	spin_unlock(&iort_msi_chip_lock);
+
+	return fw_node;
+}
+
+static struct acpi_iort_node *iort_scan_node(enum acpi_iort_node_type type,
+					     iort_find_node_callback callback,
+					     void *context)
+{
+	struct acpi_iort_node *iort_node, *iort_end;
+	struct acpi_table_iort *iort;
+	int i;
+
+	if (!iort_table)
+		return NULL;
+
+	/* Get the first IORT node */
+	iort = (struct acpi_table_iort *)iort_table;
+	iort_node = ACPI_ADD_PTR(struct acpi_iort_node, iort,
+				 iort->node_offset);
+	iort_end = ACPI_ADD_PTR(struct acpi_iort_node, iort_table,
+				iort_table->length);
+
+	for (i = 0; i < iort->node_count; i++) {
+		if (WARN_TAINT(iort_node >= iort_end, TAINT_FIRMWARE_WORKAROUND,
+			       "IORT node pointer overflows, bad table!\n"))
+			return NULL;
+
+		if (iort_node->type == type &&
+		    ACPI_SUCCESS(callback(iort_node, context)))
+				return iort_node;
+
+		iort_node = ACPI_ADD_PTR(struct acpi_iort_node, iort_node,
+					 iort_node->length);
+	}
+
+	return NULL;
+}
+
+static acpi_status iort_match_node_callback(struct acpi_iort_node *node,
+					    void *context)
+{
+	struct device *dev = context;
+	acpi_status status;
+
+	if (node->type == ACPI_IORT_NODE_NAMED_COMPONENT) {
+		struct acpi_buffer buf = { ACPI_ALLOCATE_BUFFER, NULL };
+		struct acpi_device *adev = to_acpi_device_node(dev->fwnode);
+		struct acpi_iort_named_component *ncomp;
+
+		if (!adev) {
+			status = AE_NOT_FOUND;
+			goto out;
+		}
+
+		status = acpi_get_name(adev->handle, ACPI_FULL_PATHNAME, &buf);
+		if (ACPI_FAILURE(status)) {
+			dev_warn(dev, "Can't get device full path name\n");
+			goto out;
+		}
+
+		ncomp = (struct acpi_iort_named_component *)node->node_data;
+		status = !strcmp(ncomp->device_name, buf.pointer) ?
+							AE_OK : AE_NOT_FOUND;
+		acpi_os_free(buf.pointer);
+	} else if (node->type == ACPI_IORT_NODE_PCI_ROOT_COMPLEX) {
+		struct acpi_iort_root_complex *pci_rc;
+		struct pci_bus *bus;
+
+		bus = to_pci_bus(dev);
+		pci_rc = (struct acpi_iort_root_complex *)node->node_data;
+
+		/*
+		 * It is assumed that PCI segment numbers maps one-to-one
+		 * with root complexes. Each segment number can represent only
+		 * one root complex.
+		 */
+		status = pci_rc->pci_segment_number == pci_domain_nr(bus) ?
+							AE_OK : AE_NOT_FOUND;
+	} else {
+		status = AE_NOT_FOUND;
+	}
+out:
+	return status;
+}
+
+static int iort_id_map(struct acpi_iort_id_mapping *map, u8 type, u32 rid_in,
+		       u32 *rid_out)
+{
+	/* Single mapping does not care for input id */
+	if (map->flags & ACPI_IORT_ID_SINGLE_MAPPING) {
+		if (type == ACPI_IORT_NODE_NAMED_COMPONENT ||
+		    type == ACPI_IORT_NODE_PCI_ROOT_COMPLEX) {
+			*rid_out = map->output_base;
+			return 0;
+		}
+
+		pr_warn(FW_BUG "[map %p] SINGLE MAPPING flag not allowed for node type %d, skipping ID map\n",
+			map, type);
+		return -ENXIO;
+	}
+
+	if (rid_in < map->input_base ||
+	    (rid_in >= map->input_base + map->id_count))
+		return -ENXIO;
+
+	*rid_out = map->output_base + (rid_in - map->input_base);
+	return 0;
+}
+
+static struct acpi_iort_node *iort_node_map_rid(struct acpi_iort_node *node,
+						u32 rid_in, u32 *rid_out,
+						u8 type)
+{
+	u32 rid = rid_in;
+
+	/* Parse the ID mapping tree to find specified node type */
+	while (node) {
+		struct acpi_iort_id_mapping *map;
+		int i;
+
+		if (node->type == type) {
+			if (rid_out)
+				*rid_out = rid;
+			return node;
+		}
+
+		if (!node->mapping_offset || !node->mapping_count)
+			goto fail_map;
+
+		map = ACPI_ADD_PTR(struct acpi_iort_id_mapping, node,
+				   node->mapping_offset);
+
+		/* Firmware bug! */
+		if (!map->output_reference) {
+			pr_err(FW_BUG "[node %p type %d] ID map has NULL parent reference\n",
+			       node, node->type);
+			goto fail_map;
+		}
+
+		/* Do the RID translation */
+		for (i = 0; i < node->mapping_count; i++, map++) {
+			if (!iort_id_map(map, node->type, rid, &rid))
+				break;
+		}
+
+		if (i == node->mapping_count)
+			goto fail_map;
+
+		node = ACPI_ADD_PTR(struct acpi_iort_node, iort_table,
+				    map->output_reference);
+	}
+
+fail_map:
+	/* Map input RID to output RID unchanged on mapping failure*/
+	if (rid_out)
+		*rid_out = rid_in;
+
+	return NULL;
+}
+
+static struct acpi_iort_node *iort_find_dev_node(struct device *dev)
+{
+	struct pci_bus *pbus;
+
+	if (!dev_is_pci(dev))
+		return iort_scan_node(ACPI_IORT_NODE_NAMED_COMPONENT,
+				      iort_match_node_callback, dev);
+
+	/* Find a PCI root bus */
+	pbus = to_pci_dev(dev)->bus;
+	while (!pci_is_root_bus(pbus))
+		pbus = pbus->parent;
+
+	return iort_scan_node(ACPI_IORT_NODE_PCI_ROOT_COMPLEX,
+			      iort_match_node_callback, &pbus->dev);
+}
+
+/**
+ * iort_msi_map_rid() - Map a MSI requester ID for a device
+ * @dev: The device for which the mapping is to be done.
+ * @req_id: The device requester ID.
+ *
+ * Returns: mapped MSI RID on success, input requester ID otherwise
+ */
+u32 iort_msi_map_rid(struct device *dev, u32 req_id)
+{
+	struct acpi_iort_node *node;
+	u32 dev_id;
+
+	node = iort_find_dev_node(dev);
+	if (!node)
+		return req_id;
+
+	iort_node_map_rid(node, req_id, &dev_id, ACPI_IORT_NODE_ITS_GROUP);
+	return dev_id;
+}
+
+/**
+ * iort_dev_find_its_id() - Find the ITS identifier for a device
+ * @dev: The device.
+ * @idx: Index of the ITS identifier list.
+ * @its_id: ITS identifier.
+ *
+ * Returns: 0 on success, appropriate error value otherwise
+ */
+static int iort_dev_find_its_id(struct device *dev, u32 req_id,
+				unsigned int idx, int *its_id)
+{
+	struct acpi_iort_its_group *its;
+	struct acpi_iort_node *node;
+
+	node = iort_find_dev_node(dev);
+	if (!node)
+		return -ENXIO;
+
+	node = iort_node_map_rid(node, req_id, NULL, ACPI_IORT_NODE_ITS_GROUP);
+	if (!node)
+		return -ENXIO;
+
+	/* Move to ITS specific data */
+	its = (struct acpi_iort_its_group *)node->node_data;
+	if (idx > its->its_count) {
+		dev_err(dev, "requested ITS ID index [%d] is greater than available [%d]\n",
+			idx, its->its_count);
+		return -ENXIO;
+	}
+
+	*its_id = its->identifiers[idx];
+	return 0;
+}
+
+/**
+ * iort_get_device_domain() - Find MSI domain related to a device
+ * @dev: The device.
+ * @req_id: Requester ID for the device.
+ *
+ * Returns: the MSI domain for this device, NULL otherwise
+ */
+struct irq_domain *iort_get_device_domain(struct device *dev, u32 req_id)
+{
+	struct fwnode_handle *handle;
+	int its_id;
+
+	if (iort_dev_find_its_id(dev, req_id, 0, &its_id))
+		return NULL;
+
+	handle = iort_find_domain_token(its_id);
+	if (!handle)
+		return NULL;
+
+	return irq_find_matching_fwnode(handle, DOMAIN_BUS_PCI_MSI);
+}
+
+void __init acpi_iort_init(void)
+{
+	acpi_status status;
+
+	status = acpi_get_table(ACPI_SIG_IORT, 0, &iort_table);
+	if (ACPI_FAILURE(status) && status != AE_NOT_FOUND) {
+		const char *msg = acpi_format_exception(status);
+		pr_err("Failed to get table, %s\n", msg);
+	}
+}
diff --git a/drivers/acpi/battery.c b/drivers/acpi/battery.c
index ab234791a0ba..93ecae55fe6a 100644
--- a/drivers/acpi/battery.c
+++ b/drivers/acpi/battery.c
@@ -733,15 +733,17 @@ static int acpi_battery_update(struct acpi_battery *battery, bool resume)
 			return result;
 		acpi_battery_init_alarm(battery);
 	}
+
+	result = acpi_battery_get_state(battery);
+	if (result)
+		return result;
+	acpi_battery_quirks(battery);
+
 	if (!battery->bat) {
 		result = sysfs_add_battery(battery);
 		if (result)
 			return result;
 	}
-	result = acpi_battery_get_state(battery);
-	if (result)
-		return result;
-	acpi_battery_quirks(battery);
 
 	/*
 	 * Wakeup the system if battery is critical low
diff --git a/drivers/acpi/bus.c b/drivers/acpi/bus.c
index 85b7d07fe5c8..56190d00fd87 100644
--- a/drivers/acpi/bus.c
+++ b/drivers/acpi/bus.c
@@ -36,6 +36,7 @@
 #ifdef CONFIG_X86
 #include <asm/mpspec.h>
 #endif
+#include <linux/acpi_iort.h>
 #include <linux/pci.h>
 #include <acpi/apei.h>
 #include <linux/dmi.h>
@@ -985,7 +986,8 @@ void __init acpi_early_init(void)
 		goto error0;
 	}
 
-	if (acpi_gbl_group_module_level_code) {
+	if (!acpi_gbl_parse_table_as_term_list &&
+	    acpi_gbl_group_module_level_code) {
 		status = acpi_load_tables();
 		if (ACPI_FAILURE(status)) {
 			printk(KERN_ERR PREFIX
@@ -1074,7 +1076,8 @@ static int __init acpi_bus_init(void)
 	status = acpi_ec_ecdt_probe();
 	/* Ignore result. Not having an ECDT is not fatal. */
 
-	if (!acpi_gbl_group_module_level_code) {
+	if (acpi_gbl_parse_table_as_term_list ||
+	    !acpi_gbl_group_module_level_code) {
 		status = acpi_load_tables();
 		if (ACPI_FAILURE(status)) {
 			printk(KERN_ERR PREFIX
@@ -1186,6 +1189,7 @@ static int __init acpi_init(void)
 	}
 
 	pci_mmcfg_late_init();
+	acpi_iort_init();
 	acpi_scan_init();
 	acpi_ec_init();
 	acpi_debugfs_init();
@@ -1193,6 +1197,7 @@ static int __init acpi_init(void)
 	acpi_wakeup_device_init();
 	acpi_debugger_init();
 	acpi_setup_sb_notify_handler();
+	acpi_set_processor_mapping();
 	return 0;
 }
 
diff --git a/drivers/acpi/button.c b/drivers/acpi/button.c
index 31abb0bdd4f2..e19f530f1083 100644
--- a/drivers/acpi/button.c
+++ b/drivers/acpi/button.c
@@ -19,6 +19,8 @@
  * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  */
 
+#define pr_fmt(fmt) "ACPI : button: " fmt
+
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/init.h>
@@ -104,6 +106,8 @@ struct acpi_button {
 	struct input_dev *input;
 	char phys[32];			/* for input device */
 	unsigned long pushed;
+	int last_state;
+	ktime_t last_time;
 	bool suspended;
 };
 
@@ -111,6 +115,10 @@ static BLOCKING_NOTIFIER_HEAD(acpi_lid_notifier);
 static struct acpi_device *lid_device;
 static u8 lid_init_state = ACPI_BUTTON_LID_INIT_METHOD;
 
+static unsigned long lid_report_interval __read_mostly = 500;
+module_param(lid_report_interval, ulong, 0644);
+MODULE_PARM_DESC(lid_report_interval, "Interval (ms) between lid key events");
+
 /* --------------------------------------------------------------------------
                               FS Interface (/proc)
    -------------------------------------------------------------------------- */
@@ -134,10 +142,79 @@ static int acpi_lid_notify_state(struct acpi_device *device, int state)
 {
 	struct acpi_button *button = acpi_driver_data(device);
 	int ret;
+	ktime_t next_report;
+	bool do_update;
+
+	/*
+	 * In lid_init_state=ignore mode, if user opens/closes lid
+	 * frequently with "open" missing, and "last_time" is also updated
+	 * frequently, "close" cannot be delivered to the userspace.
+	 * So "last_time" is only updated after a timeout or an actual
+	 * switch.
+	 */
+	if (lid_init_state != ACPI_BUTTON_LID_INIT_IGNORE ||
+	    button->last_state != !!state)
+		do_update = true;
+	else
+		do_update = false;
+
+	next_report = ktime_add(button->last_time,
+				ms_to_ktime(lid_report_interval));
+	if (button->last_state == !!state &&
+	    ktime_after(ktime_get(), next_report)) {
+		/* Complain the buggy firmware */
+		pr_warn_once("The lid device is not compliant to SW_LID.\n");
 
-	/* input layer checks if event is redundant */
-	input_report_switch(button->input, SW_LID, !state);
-	input_sync(button->input);
+		/*
+		 * Send the unreliable complement switch event:
+		 *
+		 * On most platforms, the lid device is reliable. However
+		 * there are exceptions:
+		 * 1. Platforms returning initial lid state as "close" by
+		 *    default after booting/resuming:
+		 *     https://bugzilla.kernel.org/show_bug.cgi?id=89211
+		 *     https://bugzilla.kernel.org/show_bug.cgi?id=106151
+		 * 2. Platforms never reporting "open" events:
+		 *     https://bugzilla.kernel.org/show_bug.cgi?id=106941
+		 * On these buggy platforms, the usage model of the ACPI
+		 * lid device actually is:
+		 * 1. The initial returning value of _LID may not be
+		 *    reliable.
+		 * 2. The open event may not be reliable.
+		 * 3. The close event is reliable.
+		 *
+		 * But SW_LID is typed as input switch event, the input
+		 * layer checks if the event is redundant. Hence if the
+		 * state is not switched, the userspace cannot see this
+		 * platform triggered reliable event. By inserting a
+		 * complement switch event, it then is guaranteed that the
+		 * platform triggered reliable one can always be seen by
+		 * the userspace.
+		 */
+		if (lid_init_state == ACPI_BUTTON_LID_INIT_IGNORE) {
+			do_update = true;
+			/*
+			 * Do generate complement switch event for "close"
+			 * as "close" is reliable and wrong "open" won't
+			 * trigger unexpected behaviors.
+			 * Do not generate complement switch event for
+			 * "open" as "open" is not reliable and wrong
+			 * "close" will trigger unexpected behaviors.
+			 */
+			if (!state) {
+				input_report_switch(button->input,
+						    SW_LID, state);
+				input_sync(button->input);
+			}
+		}
+	}
+	/* Send the platform triggered reliable event */
+	if (do_update) {
+		input_report_switch(button->input, SW_LID, !state);
+		input_sync(button->input);
+		button->last_state = !!state;
+		button->last_time = ktime_get();
+	}
 
 	if (state)
 		pm_wakeup_event(&device->dev, 0);
@@ -411,6 +488,8 @@ static int acpi_button_add(struct acpi_device *device)
 		strcpy(name, ACPI_BUTTON_DEVICE_NAME_LID);
 		sprintf(class, "%s/%s",
 			ACPI_BUTTON_CLASS, ACPI_BUTTON_SUBCLASS_LID);
+		button->last_state = !!acpi_lid_evaluate_state(device);
+		button->last_time = ktime_get();
 	} else {
 		printk(KERN_ERR PREFIX "Unsupported hid [%s]\n", hid);
 		error = -ENODEV;
diff --git a/drivers/acpi/cppc_acpi.c b/drivers/acpi/cppc_acpi.c
index 2e981732805b..d0d0504b7c89 100644
--- a/drivers/acpi/cppc_acpi.c
+++ b/drivers/acpi/cppc_acpi.c
@@ -40,15 +40,48 @@
 #include <linux/cpufreq.h>
 #include <linux/delay.h>
 #include <linux/ktime.h>
+#include <linux/rwsem.h>
+#include <linux/wait.h>
 
 #include <acpi/cppc_acpi.h>
-/*
- * Lock to provide mutually exclusive access to the PCC
- * channel. e.g. When the remote updates the shared region
- * with new data, the reader needs to be protected from
- * other CPUs activity on the same channel.
- */
-static DEFINE_SPINLOCK(pcc_lock);
+
+struct cppc_pcc_data {
+	struct mbox_chan *pcc_channel;
+	void __iomem *pcc_comm_addr;
+	int pcc_subspace_idx;
+	bool pcc_channel_acquired;
+	ktime_t deadline;
+	unsigned int pcc_mpar, pcc_mrtt, pcc_nominal;
+
+	bool pending_pcc_write_cmd;	/* Any pending/batched PCC write cmds? */
+	bool platform_owns_pcc;		/* Ownership of PCC subspace */
+	unsigned int pcc_write_cnt;	/* Running count of PCC write commands */
+
+	/*
+	 * Lock to provide controlled access to the PCC channel.
+	 *
+	 * For performance critical usecases(currently cppc_set_perf)
+	 *	We need to take read_lock and check if channel belongs to OSPM
+	 * before reading or writing to PCC subspace
+	 *	We need to take write_lock before transferring the channel
+	 * ownership to the platform via a Doorbell
+	 *	This allows us to batch a number of CPPC requests if they happen
+	 * to originate in about the same time
+	 *
+	 * For non-performance critical usecases(init)
+	 *	Take write_lock for all purposes which gives exclusive access
+	 */
+	struct rw_semaphore pcc_lock;
+
+	/* Wait queue for CPUs whose requests were batched */
+	wait_queue_head_t pcc_write_wait_q;
+};
+
+/* Structure to represent the single PCC channel */
+static struct cppc_pcc_data pcc_data = {
+	.pcc_subspace_idx = -1,
+	.platform_owns_pcc = true,
+};
 
 /*
  * The cpc_desc structure contains the ACPI register details
@@ -59,18 +92,25 @@ static DEFINE_SPINLOCK(pcc_lock);
  */
 static DEFINE_PER_CPU(struct cpc_desc *, cpc_desc_ptr);
 
-/* This layer handles all the PCC specifics for CPPC. */
-static struct mbox_chan *pcc_channel;
-static void __iomem *pcc_comm_addr;
-static u64 comm_base_addr;
-static int pcc_subspace_idx = -1;
-static bool pcc_channel_acquired;
-static ktime_t deadline;
-static unsigned int pcc_mpar, pcc_mrtt;
-
 /* pcc mapped address + header size + offset within PCC subspace */
-#define GET_PCC_VADDR(offs) (pcc_comm_addr + 0x8 + (offs))
-
+#define GET_PCC_VADDR(offs) (pcc_data.pcc_comm_addr + 0x8 + (offs))
+
+/* Check if a CPC regsiter is in PCC */
+#define CPC_IN_PCC(cpc) ((cpc)->type == ACPI_TYPE_BUFFER &&		\
+				(cpc)->cpc_entry.reg.space_id ==	\
+				ACPI_ADR_SPACE_PLATFORM_COMM)
+
+/* Evalutes to True if reg is a NULL register descriptor */
+#define IS_NULL_REG(reg) ((reg)->space_id ==  ACPI_ADR_SPACE_SYSTEM_MEMORY && \
+				(reg)->address == 0 &&			\
+				(reg)->bit_width == 0 &&		\
+				(reg)->bit_offset == 0 &&		\
+				(reg)->access_width == 0)
+
+/* Evalutes to True if an optional cpc field is supported */
+#define CPC_SUPPORTED(cpc) ((cpc)->type == ACPI_TYPE_INTEGER ?		\
+				!!(cpc)->cpc_entry.int_value :		\
+				!IS_NULL_REG(&(cpc)->cpc_entry.reg))
 /*
  * Arbitrary Retries in case the remote processor is slow to respond
  * to PCC commands. Keeping it high enough to cover emulators where
@@ -78,11 +118,79 @@ static unsigned int pcc_mpar, pcc_mrtt;
  */
 #define NUM_RETRIES 500
 
-static int check_pcc_chan(void)
+struct cppc_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 *c, ssize_t count);
+};
+
+#define define_one_cppc_ro(_name)		\
+static struct cppc_attr _name =			\
+__ATTR(_name, 0444, show_##_name, NULL)
+
+#define to_cpc_desc(a) container_of(a, struct cpc_desc, kobj)
+
+static ssize_t show_feedback_ctrs(struct kobject *kobj,
+		struct attribute *attr, char *buf)
+{
+	struct cpc_desc *cpc_ptr = to_cpc_desc(kobj);
+	struct cppc_perf_fb_ctrs fb_ctrs = {0};
+
+	cppc_get_perf_ctrs(cpc_ptr->cpu_id, &fb_ctrs);
+
+	return scnprintf(buf, PAGE_SIZE, "ref:%llu del:%llu\n",
+			fb_ctrs.reference, fb_ctrs.delivered);
+}
+define_one_cppc_ro(feedback_ctrs);
+
+static ssize_t show_reference_perf(struct kobject *kobj,
+		struct attribute *attr, char *buf)
+{
+	struct cpc_desc *cpc_ptr = to_cpc_desc(kobj);
+	struct cppc_perf_fb_ctrs fb_ctrs = {0};
+
+	cppc_get_perf_ctrs(cpc_ptr->cpu_id, &fb_ctrs);
+
+	return scnprintf(buf, PAGE_SIZE, "%llu\n",
+			fb_ctrs.reference_perf);
+}
+define_one_cppc_ro(reference_perf);
+
+static ssize_t show_wraparound_time(struct kobject *kobj,
+				struct attribute *attr, char *buf)
+{
+	struct cpc_desc *cpc_ptr = to_cpc_desc(kobj);
+	struct cppc_perf_fb_ctrs fb_ctrs = {0};
+
+	cppc_get_perf_ctrs(cpc_ptr->cpu_id, &fb_ctrs);
+
+	return scnprintf(buf, PAGE_SIZE, "%llu\n", fb_ctrs.ctr_wrap_time);
+
+}
+define_one_cppc_ro(wraparound_time);
+
+static struct attribute *cppc_attrs[] = {
+	&feedback_ctrs.attr,
+	&reference_perf.attr,
+	&wraparound_time.attr,
+	NULL
+};
+
+static struct kobj_type cppc_ktype = {
+	.sysfs_ops = &kobj_sysfs_ops,
+	.default_attrs = cppc_attrs,
+};
+
+static int check_pcc_chan(bool chk_err_bit)
 {
-	int ret = -EIO;
-	struct acpi_pcct_shared_memory __iomem *generic_comm_base = pcc_comm_addr;
-	ktime_t next_deadline = ktime_add(ktime_get(), deadline);
+	int ret = -EIO, status = 0;
+	struct acpi_pcct_shared_memory __iomem *generic_comm_base = pcc_data.pcc_comm_addr;
+	ktime_t next_deadline = ktime_add(ktime_get(), pcc_data.deadline);
+
+	if (!pcc_data.platform_owns_pcc)
+		return 0;
 
 	/* Retry in case the remote processor was too slow to catch up. */
 	while (!ktime_after(ktime_get(), next_deadline)) {
@@ -91,8 +199,11 @@ static int check_pcc_chan(void)
 		 * platform and should have set the command completion bit when
 		 * PCC can be used by OSPM
 		 */
-		if (readw_relaxed(&generic_comm_base->status) & PCC_CMD_COMPLETE) {
+		status = readw_relaxed(&generic_comm_base->status);
+		if (status & PCC_CMD_COMPLETE_MASK) {
 			ret = 0;
+			if (chk_err_bit && (status & PCC_ERROR_MASK))
+				ret = -EIO;
 			break;
 		}
 		/*
@@ -102,14 +213,23 @@ static int check_pcc_chan(void)
 		udelay(3);
 	}
 
+	if (likely(!ret))
+		pcc_data.platform_owns_pcc = false;
+	else
+		pr_err("PCC check channel failed. Status=%x\n", status);
+
 	return ret;
 }
 
+/*
+ * This function transfers the ownership of the PCC to the platform
+ * So it must be called while holding write_lock(pcc_lock)
+ */
 static int send_pcc_cmd(u16 cmd)
 {
-	int ret = -EIO;
+	int ret = -EIO, i;
 	struct acpi_pcct_shared_memory *generic_comm_base =
-		(struct acpi_pcct_shared_memory *) pcc_comm_addr;
+		(struct acpi_pcct_shared_memory *) pcc_data.pcc_comm_addr;
 	static ktime_t last_cmd_cmpl_time, last_mpar_reset;
 	static int mpar_count;
 	unsigned int time_delta;
@@ -119,20 +239,29 @@ static int send_pcc_cmd(u16 cmd)
 	 * the channel before writing to PCC space
 	 */
 	if (cmd == CMD_READ) {
-		ret = check_pcc_chan();
+		/*
+		 * If there are pending cpc_writes, then we stole the channel
+		 * before write completion, so first send a WRITE command to
+		 * platform
+		 */
+		if (pcc_data.pending_pcc_write_cmd)
+			send_pcc_cmd(CMD_WRITE);
+
+		ret = check_pcc_chan(false);
 		if (ret)
-			return ret;
-	}
+			goto end;
+	} else /* CMD_WRITE */
+		pcc_data.pending_pcc_write_cmd = FALSE;
 
 	/*
 	 * Handle the Minimum Request Turnaround Time(MRTT)
 	 * "The minimum amount of time that OSPM must wait after the completion
 	 * of a command before issuing the next command, in microseconds"
 	 */
-	if (pcc_mrtt) {
+	if (pcc_data.pcc_mrtt) {
 		time_delta = ktime_us_delta(ktime_get(), last_cmd_cmpl_time);
-		if (pcc_mrtt > time_delta)
-			udelay(pcc_mrtt - time_delta);
+		if (pcc_data.pcc_mrtt > time_delta)
+			udelay(pcc_data.pcc_mrtt - time_delta);
 	}
 
 	/*
@@ -146,15 +275,16 @@ static int send_pcc_cmd(u16 cmd)
 	 * not send the request to the platform after hitting the MPAR limit in
 	 * any 60s window
 	 */
-	if (pcc_mpar) {
+	if (pcc_data.pcc_mpar) {
 		if (mpar_count == 0) {
 			time_delta = ktime_ms_delta(ktime_get(), last_mpar_reset);
 			if (time_delta < 60 * MSEC_PER_SEC) {
 				pr_debug("PCC cmd not sent due to MPAR limit");
-				return -EIO;
+				ret = -EIO;
+				goto end;
 			}
 			last_mpar_reset = ktime_get();
-			mpar_count = pcc_mpar;
+			mpar_count = pcc_data.pcc_mpar;
 		}
 		mpar_count--;
 	}
@@ -165,33 +295,43 @@ static int send_pcc_cmd(u16 cmd)
 	/* Flip CMD COMPLETE bit */
 	writew_relaxed(0, &generic_comm_base->status);
 
+	pcc_data.platform_owns_pcc = true;
+
 	/* Ring doorbell */
-	ret = mbox_send_message(pcc_channel, &cmd);
+	ret = mbox_send_message(pcc_data.pcc_channel, &cmd);
 	if (ret < 0) {
 		pr_err("Err sending PCC mbox message. cmd:%d, ret:%d\n",
 				cmd, ret);
-		return ret;
+		goto end;
 	}
 
-	/*
-	 * For READs we need to ensure the cmd completed to ensure
-	 * the ensuing read()s can proceed. For WRITEs we dont care
-	 * because the actual write()s are done before coming here
-	 * and the next READ or WRITE will check if the channel
-	 * is busy/free at the entry of this call.
-	 *
-	 * If Minimum Request Turnaround Time is non-zero, we need
-	 * to record the completion time of both READ and WRITE
-	 * command for proper handling of MRTT, so we need to check
-	 * for pcc_mrtt in addition to CMD_READ
-	 */
-	if (cmd == CMD_READ || pcc_mrtt) {
-		ret = check_pcc_chan();
-		if (pcc_mrtt)
-			last_cmd_cmpl_time = ktime_get();
+	/* wait for completion and check for PCC errro bit */
+	ret = check_pcc_chan(true);
+
+	if (pcc_data.pcc_mrtt)
+		last_cmd_cmpl_time = ktime_get();
+
+	if (pcc_data.pcc_channel->mbox->txdone_irq)
+		mbox_chan_txdone(pcc_data.pcc_channel, ret);
+	else
+		mbox_client_txdone(pcc_data.pcc_channel, ret);
+
+end:
+	if (cmd == CMD_WRITE) {
+		if (unlikely(ret)) {
+			for_each_possible_cpu(i) {
+				struct cpc_desc *desc = per_cpu(cpc_desc_ptr, i);
+				if (!desc)
+					continue;
+
+				if (desc->write_cmd_id == pcc_data.pcc_write_cnt)
+					desc->write_cmd_status = ret;
+			}
+		}
+		pcc_data.pcc_write_cnt++;
+		wake_up_all(&pcc_data.pcc_write_wait_q);
 	}
 
-	mbox_client_txdone(pcc_channel, ret);
 	return ret;
 }
 
@@ -272,13 +412,13 @@ end:
  *
  *	Return: 0 for success or negative value for err.
  */
-int acpi_get_psd_map(struct cpudata **all_cpu_data)
+int acpi_get_psd_map(struct cppc_cpudata **all_cpu_data)
 {
 	int count_target;
 	int retval = 0;
 	unsigned int i, j;
 	cpumask_var_t covered_cpus;
-	struct cpudata *pr, *match_pr;
+	struct cppc_cpudata *pr, *match_pr;
 	struct acpi_psd_package *pdomain;
 	struct acpi_psd_package *match_pdomain;
 	struct cpc_desc *cpc_ptr, *match_cpc_ptr;
@@ -394,14 +534,13 @@ EXPORT_SYMBOL_GPL(acpi_get_psd_map);
 static int register_pcc_channel(int pcc_subspace_idx)
 {
 	struct acpi_pcct_hw_reduced *cppc_ss;
-	unsigned int len;
 	u64 usecs_lat;
 
 	if (pcc_subspace_idx >= 0) {
-		pcc_channel = pcc_mbox_request_channel(&cppc_mbox_cl,
+		pcc_data.pcc_channel = pcc_mbox_request_channel(&cppc_mbox_cl,
 				pcc_subspace_idx);
 
-		if (IS_ERR(pcc_channel)) {
+		if (IS_ERR(pcc_data.pcc_channel)) {
 			pr_err("Failed to find PCC communication channel\n");
 			return -ENODEV;
 		}
@@ -412,7 +551,7 @@ static int register_pcc_channel(int pcc_subspace_idx)
 		 * PCC channels) and stored pointers to the
 		 * subspace communication region in con_priv.
 		 */
-		cppc_ss = pcc_channel->con_priv;
+		cppc_ss = (pcc_data.pcc_channel)->con_priv;
 
 		if (!cppc_ss) {
 			pr_err("No PCC subspace found for CPPC\n");
@@ -420,35 +559,42 @@ static int register_pcc_channel(int pcc_subspace_idx)
 		}
 
 		/*
-		 * This is the shared communication region
-		 * for the OS and Platform to communicate over.
-		 */
-		comm_base_addr = cppc_ss->base_address;
-		len = cppc_ss->length;
-
-		/*
 		 * cppc_ss->latency is just a Nominal value. In reality
 		 * the remote processor could be much slower to reply.
 		 * So add an arbitrary amount of wait on top of Nominal.
 		 */
 		usecs_lat = NUM_RETRIES * cppc_ss->latency;
-		deadline = ns_to_ktime(usecs_lat * NSEC_PER_USEC);
-		pcc_mrtt = cppc_ss->min_turnaround_time;
-		pcc_mpar = cppc_ss->max_access_rate;
+		pcc_data.deadline = ns_to_ktime(usecs_lat * NSEC_PER_USEC);
+		pcc_data.pcc_mrtt = cppc_ss->min_turnaround_time;
+		pcc_data.pcc_mpar = cppc_ss->max_access_rate;
+		pcc_data.pcc_nominal = cppc_ss->latency;
 
-		pcc_comm_addr = acpi_os_ioremap(comm_base_addr, len);
-		if (!pcc_comm_addr) {
+		pcc_data.pcc_comm_addr = acpi_os_ioremap(cppc_ss->base_address, cppc_ss->length);
+		if (!pcc_data.pcc_comm_addr) {
 			pr_err("Failed to ioremap PCC comm region mem\n");
 			return -ENOMEM;
 		}
 
 		/* Set flag so that we dont come here for each CPU. */
-		pcc_channel_acquired = true;
+		pcc_data.pcc_channel_acquired = true;
 	}
 
 	return 0;
 }
 
+/**
+ * cpc_ffh_supported() - check if FFH reading supported
+ *
+ * Check if the architecture has support for functional fixed hardware
+ * read/write capability.
+ *
+ * Return: true for supported, false for not supported
+ */
+bool __weak cpc_ffh_supported(void)
+{
+	return false;
+}
+
 /*
  * An example CPC table looks like the following.
  *
@@ -507,6 +653,7 @@ int acpi_cppc_processor_probe(struct acpi_processor *pr)
 	union acpi_object *out_obj, *cpc_obj;
 	struct cpc_desc *cpc_ptr;
 	struct cpc_reg *gas_t;
+	struct device *cpu_dev;
 	acpi_handle handle = pr->handle;
 	unsigned int num_ent, i, cpc_rev;
 	acpi_status status;
@@ -545,6 +692,8 @@ int acpi_cppc_processor_probe(struct acpi_processor *pr)
 		goto out_free;
 	}
 
+	cpc_ptr->num_entries = num_ent;
+
 	/* Second entry should be revision. */
 	cpc_obj = &out_obj->package.elements[1];
 	if (cpc_obj->type == ACPI_TYPE_INTEGER)	{
@@ -579,16 +728,27 @@ int acpi_cppc_processor_probe(struct acpi_processor *pr)
 			 * so extract it only once.
 			 */
 			if (gas_t->space_id == ACPI_ADR_SPACE_PLATFORM_COMM) {
-				if (pcc_subspace_idx < 0)
-					pcc_subspace_idx = gas_t->access_width;
-				else if (pcc_subspace_idx != gas_t->access_width) {
+				if (pcc_data.pcc_subspace_idx < 0)
+					pcc_data.pcc_subspace_idx = gas_t->access_width;
+				else if (pcc_data.pcc_subspace_idx != gas_t->access_width) {
 					pr_debug("Mismatched PCC ids.\n");
 					goto out_free;
 				}
-			} else if (gas_t->space_id != ACPI_ADR_SPACE_SYSTEM_MEMORY) {
-				/* Support only PCC and SYS MEM type regs */
-				pr_debug("Unsupported register type: %d\n", gas_t->space_id);
-				goto out_free;
+			} else if (gas_t->space_id == ACPI_ADR_SPACE_SYSTEM_MEMORY) {
+				if (gas_t->address) {
+					void __iomem *addr;
+
+					addr = ioremap(gas_t->address, gas_t->bit_width/8);
+					if (!addr)
+						goto out_free;
+					cpc_ptr->cpc_regs[i-2].sys_mem_vaddr = addr;
+				}
+			} else {
+				if (gas_t->space_id != ACPI_ADR_SPACE_FIXED_HARDWARE || !cpc_ffh_supported()) {
+					/* Support only PCC ,SYS MEM and FFH type regs */
+					pr_debug("Unsupported register type: %d\n", gas_t->space_id);
+					goto out_free;
+				}
 			}
 
 			cpc_ptr->cpc_regs[i-2].type = ACPI_TYPE_BUFFER;
@@ -607,10 +767,13 @@ int acpi_cppc_processor_probe(struct acpi_processor *pr)
 		goto out_free;
 
 	/* Register PCC channel once for all CPUs. */
-	if (!pcc_channel_acquired) {
-		ret = register_pcc_channel(pcc_subspace_idx);
+	if (!pcc_data.pcc_channel_acquired) {
+		ret = register_pcc_channel(pcc_data.pcc_subspace_idx);
 		if (ret)
 			goto out_free;
+
+		init_rwsem(&pcc_data.pcc_lock);
+		init_waitqueue_head(&pcc_data.pcc_write_wait_q);
 	}
 
 	/* Plug PSD data into this CPUs CPC descriptor. */
@@ -619,10 +782,27 @@ int acpi_cppc_processor_probe(struct acpi_processor *pr)
 	/* Everything looks okay */
 	pr_debug("Parsed CPC struct for CPU: %d\n", pr->id);
 
+	/* Add per logical CPU nodes for reading its feedback counters. */
+	cpu_dev = get_cpu_device(pr->id);
+	if (!cpu_dev)
+		goto out_free;
+
+	ret = kobject_init_and_add(&cpc_ptr->kobj, &cppc_ktype, &cpu_dev->kobj,
+			"acpi_cppc");
+	if (ret)
+		goto out_free;
+
 	kfree(output.pointer);
 	return 0;
 
 out_free:
+	/* Free all the mapped sys mem areas for this CPU */
+	for (i = 2; i < cpc_ptr->num_entries; i++) {
+		void __iomem *addr = cpc_ptr->cpc_regs[i-2].sys_mem_vaddr;
+
+		if (addr)
+			iounmap(addr);
+	}
 	kfree(cpc_ptr);
 
 out_buf_free:
@@ -640,26 +820,82 @@ EXPORT_SYMBOL_GPL(acpi_cppc_processor_probe);
 void acpi_cppc_processor_exit(struct acpi_processor *pr)
 {
 	struct cpc_desc *cpc_ptr;
+	unsigned int i;
+	void __iomem *addr;
+
 	cpc_ptr = per_cpu(cpc_desc_ptr, pr->id);
+
+	/* Free all the mapped sys mem areas for this CPU */
+	for (i = 2; i < cpc_ptr->num_entries; i++) {
+		addr = cpc_ptr->cpc_regs[i-2].sys_mem_vaddr;
+		if (addr)
+			iounmap(addr);
+	}
+
+	kobject_put(&cpc_ptr->kobj);
 	kfree(cpc_ptr);
 }
 EXPORT_SYMBOL_GPL(acpi_cppc_processor_exit);
 
+/**
+ * cpc_read_ffh() - Read FFH register
+ * @cpunum:	cpu number to read
+ * @reg:	cppc register information
+ * @val:	place holder for return value
+ *
+ * Read bit_width bits from a specified address and bit_offset
+ *
+ * Return: 0 for success and error code
+ */
+int __weak cpc_read_ffh(int cpunum, struct cpc_reg *reg, u64 *val)
+{
+	return -ENOTSUPP;
+}
+
+/**
+ * cpc_write_ffh() - Write FFH register
+ * @cpunum:	cpu number to write
+ * @reg:	cppc register information
+ * @val:	value to write
+ *
+ * Write value of bit_width bits to a specified address and bit_offset
+ *
+ * Return: 0 for success and error code
+ */
+int __weak cpc_write_ffh(int cpunum, struct cpc_reg *reg, u64 val)
+{
+	return -ENOTSUPP;
+}
+
 /*
  * Since cpc_read and cpc_write are called while holding pcc_lock, it should be
  * as fast as possible. We have already mapped the PCC subspace during init, so
  * we can directly write to it.
  */
 
-static int cpc_read(struct cpc_reg *reg, u64 *val)
+static int cpc_read(int cpu, struct cpc_register_resource *reg_res, u64 *val)
 {
 	int ret_val = 0;
+	void __iomem *vaddr = 0;
+	struct cpc_reg *reg = &reg_res->cpc_entry.reg;
+
+	if (reg_res->type == ACPI_TYPE_INTEGER) {
+		*val = reg_res->cpc_entry.int_value;
+		return ret_val;
+	}
 
 	*val = 0;
-	if (reg->space_id == ACPI_ADR_SPACE_PLATFORM_COMM) {
-		void __iomem *vaddr = GET_PCC_VADDR(reg->address);
+	if (reg->space_id == ACPI_ADR_SPACE_PLATFORM_COMM)
+		vaddr = GET_PCC_VADDR(reg->address);
+	else if (reg->space_id == ACPI_ADR_SPACE_SYSTEM_MEMORY)
+		vaddr = reg_res->sys_mem_vaddr;
+	else if (reg->space_id == ACPI_ADR_SPACE_FIXED_HARDWARE)
+		return cpc_read_ffh(cpu, reg, val);
+	else
+		return acpi_os_read_memory((acpi_physical_address)reg->address,
+				val, reg->bit_width);
 
-		switch (reg->bit_width) {
+	switch (reg->bit_width) {
 		case 8:
 			*val = readb_relaxed(vaddr);
 			break;
@@ -674,23 +910,30 @@ static int cpc_read(struct cpc_reg *reg, u64 *val)
 			break;
 		default:
 			pr_debug("Error: Cannot read %u bit width from PCC\n",
-				reg->bit_width);
+					reg->bit_width);
 			ret_val = -EFAULT;
-		}
-	} else
-		ret_val = acpi_os_read_memory((acpi_physical_address)reg->address,
-					val, reg->bit_width);
+	}
+
 	return ret_val;
 }
 
-static int cpc_write(struct cpc_reg *reg, u64 val)
+static int cpc_write(int cpu, struct cpc_register_resource *reg_res, u64 val)
 {
 	int ret_val = 0;
+	void __iomem *vaddr = 0;
+	struct cpc_reg *reg = &reg_res->cpc_entry.reg;
+
+	if (reg->space_id == ACPI_ADR_SPACE_PLATFORM_COMM)
+		vaddr = GET_PCC_VADDR(reg->address);
+	else if (reg->space_id == ACPI_ADR_SPACE_SYSTEM_MEMORY)
+		vaddr = reg_res->sys_mem_vaddr;
+	else if (reg->space_id == ACPI_ADR_SPACE_FIXED_HARDWARE)
+		return cpc_write_ffh(cpu, reg, val);
+	else
+		return acpi_os_write_memory((acpi_physical_address)reg->address,
+				val, reg->bit_width);
 
-	if (reg->space_id == ACPI_ADR_SPACE_PLATFORM_COMM) {
-		void __iomem *vaddr = GET_PCC_VADDR(reg->address);
-
-		switch (reg->bit_width) {
+	switch (reg->bit_width) {
 		case 8:
 			writeb_relaxed(val, vaddr);
 			break;
@@ -705,13 +948,11 @@ static int cpc_write(struct cpc_reg *reg, u64 val)
 			break;
 		default:
 			pr_debug("Error: Cannot write %u bit width to PCC\n",
-				reg->bit_width);
+					reg->bit_width);
 			ret_val = -EFAULT;
 			break;
-		}
-	} else
-		ret_val = acpi_os_write_memory((acpi_physical_address)reg->address,
-				val, reg->bit_width);
+	}
+
 	return ret_val;
 }
 
@@ -727,8 +968,8 @@ int cppc_get_perf_caps(int cpunum, struct cppc_perf_caps *perf_caps)
 	struct cpc_desc *cpc_desc = per_cpu(cpc_desc_ptr, cpunum);
 	struct cpc_register_resource *highest_reg, *lowest_reg, *ref_perf,
 								 *nom_perf;
-	u64 high, low, ref, nom;
-	int ret = 0;
+	u64 high, low, nom;
+	int ret = 0, regs_in_pcc = 0;
 
 	if (!cpc_desc) {
 		pr_debug("No CPC descriptor for CPU:%d\n", cpunum);
@@ -740,13 +981,11 @@ int cppc_get_perf_caps(int cpunum, struct cppc_perf_caps *perf_caps)
 	ref_perf = &cpc_desc->cpc_regs[REFERENCE_PERF];
 	nom_perf = &cpc_desc->cpc_regs[NOMINAL_PERF];
 
-	spin_lock(&pcc_lock);
-
 	/* Are any of the regs PCC ?*/
-	if ((highest_reg->cpc_entry.reg.space_id == ACPI_ADR_SPACE_PLATFORM_COMM) ||
-			(lowest_reg->cpc_entry.reg.space_id == ACPI_ADR_SPACE_PLATFORM_COMM) ||
-			(ref_perf->cpc_entry.reg.space_id == ACPI_ADR_SPACE_PLATFORM_COMM) ||
-			(nom_perf->cpc_entry.reg.space_id == ACPI_ADR_SPACE_PLATFORM_COMM)) {
+	if (CPC_IN_PCC(highest_reg) || CPC_IN_PCC(lowest_reg) ||
+		CPC_IN_PCC(ref_perf) || CPC_IN_PCC(nom_perf)) {
+		regs_in_pcc = 1;
+		down_write(&pcc_data.pcc_lock);
 		/* Ring doorbell once to update PCC subspace */
 		if (send_pcc_cmd(CMD_READ) < 0) {
 			ret = -EIO;
@@ -754,26 +993,21 @@ int cppc_get_perf_caps(int cpunum, struct cppc_perf_caps *perf_caps)
 		}
 	}
 
-	cpc_read(&highest_reg->cpc_entry.reg, &high);
+	cpc_read(cpunum, highest_reg, &high);
 	perf_caps->highest_perf = high;
 
-	cpc_read(&lowest_reg->cpc_entry.reg, &low);
+	cpc_read(cpunum, lowest_reg, &low);
 	perf_caps->lowest_perf = low;
 
-	cpc_read(&ref_perf->cpc_entry.reg, &ref);
-	perf_caps->reference_perf = ref;
-
-	cpc_read(&nom_perf->cpc_entry.reg, &nom);
+	cpc_read(cpunum, nom_perf, &nom);
 	perf_caps->nominal_perf = nom;
 
-	if (!ref)
-		perf_caps->reference_perf = perf_caps->nominal_perf;
-
 	if (!high || !low || !nom)
 		ret = -EFAULT;
 
 out_err:
-	spin_unlock(&pcc_lock);
+	if (regs_in_pcc)
+		up_write(&pcc_data.pcc_lock);
 	return ret;
 }
 EXPORT_SYMBOL_GPL(cppc_get_perf_caps);
@@ -788,9 +1022,10 @@ EXPORT_SYMBOL_GPL(cppc_get_perf_caps);
 int cppc_get_perf_ctrs(int cpunum, struct cppc_perf_fb_ctrs *perf_fb_ctrs)
 {
 	struct cpc_desc *cpc_desc = per_cpu(cpc_desc_ptr, cpunum);
-	struct cpc_register_resource *delivered_reg, *reference_reg;
-	u64 delivered, reference;
-	int ret = 0;
+	struct cpc_register_resource *delivered_reg, *reference_reg,
+		*ref_perf_reg, *ctr_wrap_reg;
+	u64 delivered, reference, ref_perf, ctr_wrap_time;
+	int ret = 0, regs_in_pcc = 0;
 
 	if (!cpc_desc) {
 		pr_debug("No CPC descriptor for CPU:%d\n", cpunum);
@@ -799,12 +1034,21 @@ int cppc_get_perf_ctrs(int cpunum, struct cppc_perf_fb_ctrs *perf_fb_ctrs)
 
 	delivered_reg = &cpc_desc->cpc_regs[DELIVERED_CTR];
 	reference_reg = &cpc_desc->cpc_regs[REFERENCE_CTR];
+	ref_perf_reg = &cpc_desc->cpc_regs[REFERENCE_PERF];
+	ctr_wrap_reg = &cpc_desc->cpc_regs[CTR_WRAP_TIME];
 
-	spin_lock(&pcc_lock);
+	/*
+	 * If refernce perf register is not supported then we should
+	 * use the nominal perf value
+	 */
+	if (!CPC_SUPPORTED(ref_perf_reg))
+		ref_perf_reg = &cpc_desc->cpc_regs[NOMINAL_PERF];
 
 	/* Are any of the regs PCC ?*/
-	if ((delivered_reg->cpc_entry.reg.space_id == ACPI_ADR_SPACE_PLATFORM_COMM) ||
-			(reference_reg->cpc_entry.reg.space_id == ACPI_ADR_SPACE_PLATFORM_COMM)) {
+	if (CPC_IN_PCC(delivered_reg) || CPC_IN_PCC(reference_reg) ||
+		CPC_IN_PCC(ctr_wrap_reg) || CPC_IN_PCC(ref_perf_reg)) {
+		down_write(&pcc_data.pcc_lock);
+		regs_in_pcc = 1;
 		/* Ring doorbell once to update PCC subspace */
 		if (send_pcc_cmd(CMD_READ) < 0) {
 			ret = -EIO;
@@ -812,25 +1056,31 @@ int cppc_get_perf_ctrs(int cpunum, struct cppc_perf_fb_ctrs *perf_fb_ctrs)
 		}
 	}
 
-	cpc_read(&delivered_reg->cpc_entry.reg, &delivered);
-	cpc_read(&reference_reg->cpc_entry.reg, &reference);
+	cpc_read(cpunum, delivered_reg, &delivered);
+	cpc_read(cpunum, reference_reg, &reference);
+	cpc_read(cpunum, ref_perf_reg, &ref_perf);
 
-	if (!delivered || !reference) {
+	/*
+	 * Per spec, if ctr_wrap_time optional register is unsupported, then the
+	 * performance counters are assumed to never wrap during the lifetime of
+	 * platform
+	 */
+	ctr_wrap_time = (u64)(~((u64)0));
+	if (CPC_SUPPORTED(ctr_wrap_reg))
+		cpc_read(cpunum, ctr_wrap_reg, &ctr_wrap_time);
+
+	if (!delivered || !reference ||	!ref_perf) {
 		ret = -EFAULT;
 		goto out_err;
 	}
 
 	perf_fb_ctrs->delivered = delivered;
 	perf_fb_ctrs->reference = reference;
-
-	perf_fb_ctrs->delivered -= perf_fb_ctrs->prev_delivered;
-	perf_fb_ctrs->reference -= perf_fb_ctrs->prev_reference;
-
-	perf_fb_ctrs->prev_delivered = delivered;
-	perf_fb_ctrs->prev_reference = reference;
-
+	perf_fb_ctrs->reference_perf = ref_perf;
+	perf_fb_ctrs->ctr_wrap_time = ctr_wrap_time;
 out_err:
-	spin_unlock(&pcc_lock);
+	if (regs_in_pcc)
+		up_write(&pcc_data.pcc_lock);
 	return ret;
 }
 EXPORT_SYMBOL_GPL(cppc_get_perf_ctrs);
@@ -855,30 +1105,142 @@ int cppc_set_perf(int cpu, struct cppc_perf_ctrls *perf_ctrls)
 
 	desired_reg = &cpc_desc->cpc_regs[DESIRED_PERF];
 
-	spin_lock(&pcc_lock);
-
-	/* If this is PCC reg, check if channel is free before writing */
-	if (desired_reg->cpc_entry.reg.space_id == ACPI_ADR_SPACE_PLATFORM_COMM) {
-		ret = check_pcc_chan();
-		if (ret)
-			goto busy_channel;
+	/*
+	 * This is Phase-I where we want to write to CPC registers
+	 * -> We want all CPUs to be able to execute this phase in parallel
+	 *
+	 * Since read_lock can be acquired by multiple CPUs simultaneously we
+	 * achieve that goal here
+	 */
+	if (CPC_IN_PCC(desired_reg)) {
+		down_read(&pcc_data.pcc_lock);	/* BEGIN Phase-I */
+		if (pcc_data.platform_owns_pcc) {
+			ret = check_pcc_chan(false);
+			if (ret) {
+				up_read(&pcc_data.pcc_lock);
+				return ret;
+			}
+		}
+		/*
+		 * Update the pending_write to make sure a PCC CMD_READ will not
+		 * arrive and steal the channel during the switch to write lock
+		 */
+		pcc_data.pending_pcc_write_cmd = true;
+		cpc_desc->write_cmd_id = pcc_data.pcc_write_cnt;
+		cpc_desc->write_cmd_status = 0;
 	}
 
 	/*
 	 * Skip writing MIN/MAX until Linux knows how to come up with
 	 * useful values.
 	 */
-	cpc_write(&desired_reg->cpc_entry.reg, perf_ctrls->desired_perf);
+	cpc_write(cpu, desired_reg, perf_ctrls->desired_perf);
 
-	/* Is this a PCC reg ?*/
-	if (desired_reg->cpc_entry.reg.space_id == ACPI_ADR_SPACE_PLATFORM_COMM) {
-		/* Ring doorbell so Remote can get our perf request. */
-		if (send_pcc_cmd(CMD_WRITE) < 0)
-			ret = -EIO;
+	if (CPC_IN_PCC(desired_reg))
+		up_read(&pcc_data.pcc_lock);	/* END Phase-I */
+	/*
+	 * This is Phase-II where we transfer the ownership of PCC to Platform
+	 *
+	 * Short Summary: Basically if we think of a group of cppc_set_perf
+	 * requests that happened in short overlapping interval. The last CPU to
+	 * come out of Phase-I will enter Phase-II and ring the doorbell.
+	 *
+	 * We have the following requirements for Phase-II:
+	 *     1. We want to execute Phase-II only when there are no CPUs
+	 * currently executing in Phase-I
+	 *     2. Once we start Phase-II we want to avoid all other CPUs from
+	 * entering Phase-I.
+	 *     3. We want only one CPU among all those who went through Phase-I
+	 * to run phase-II
+	 *
+	 * If write_trylock fails to get the lock and doesn't transfer the
+	 * PCC ownership to the platform, then one of the following will be TRUE
+	 *     1. There is at-least one CPU in Phase-I which will later execute
+	 * write_trylock, so the CPUs in Phase-I will be responsible for
+	 * executing the Phase-II.
+	 *     2. Some other CPU has beaten this CPU to successfully execute the
+	 * write_trylock and has already acquired the write_lock. We know for a
+	 * fact it(other CPU acquiring the write_lock) couldn't have happened
+	 * before this CPU's Phase-I as we held the read_lock.
+	 *     3. Some other CPU executing pcc CMD_READ has stolen the
+	 * down_write, in which case, send_pcc_cmd will check for pending
+	 * CMD_WRITE commands by checking the pending_pcc_write_cmd.
+	 * So this CPU can be certain that its request will be delivered
+	 *    So in all cases, this CPU knows that its request will be delivered
+	 * by another CPU and can return
+	 *
+	 * After getting the down_write we still need to check for
+	 * pending_pcc_write_cmd to take care of the following scenario
+	 *    The thread running this code could be scheduled out between
+	 * Phase-I and Phase-II. Before it is scheduled back on, another CPU
+	 * could have delivered the request to Platform by triggering the
+	 * doorbell and transferred the ownership of PCC to platform. So this
+	 * avoids triggering an unnecessary doorbell and more importantly before
+	 * triggering the doorbell it makes sure that the PCC channel ownership
+	 * is still with OSPM.
+	 *   pending_pcc_write_cmd can also be cleared by a different CPU, if
+	 * there was a pcc CMD_READ waiting on down_write and it steals the lock
+	 * before the pcc CMD_WRITE is completed. pcc_send_cmd checks for this
+	 * case during a CMD_READ and if there are pending writes it delivers
+	 * the write command before servicing the read command
+	 */
+	if (CPC_IN_PCC(desired_reg)) {
+		if (down_write_trylock(&pcc_data.pcc_lock)) {	/* BEGIN Phase-II */
+			/* Update only if there are pending write commands */
+			if (pcc_data.pending_pcc_write_cmd)
+				send_pcc_cmd(CMD_WRITE);
+			up_write(&pcc_data.pcc_lock);		/* END Phase-II */
+		} else
+			/* Wait until pcc_write_cnt is updated by send_pcc_cmd */
+			wait_event(pcc_data.pcc_write_wait_q,
+				cpc_desc->write_cmd_id != pcc_data.pcc_write_cnt);
+
+		/* send_pcc_cmd updates the status in case of failure */
+		ret = cpc_desc->write_cmd_status;
 	}
-busy_channel:
-	spin_unlock(&pcc_lock);
-
 	return ret;
 }
 EXPORT_SYMBOL_GPL(cppc_set_perf);
+
+/**
+ * cppc_get_transition_latency - returns frequency transition latency in ns
+ *
+ * ACPI CPPC does not explicitly specifiy how a platform can specify the
+ * transition latency for perfromance change requests. The closest we have
+ * is the timing information from the PCCT tables which provides the info
+ * on the number and frequency of PCC commands the platform can handle.
+ */
+unsigned int cppc_get_transition_latency(int cpu_num)
+{
+	/*
+	 * Expected transition latency is based on the PCCT timing values
+	 * Below are definition from ACPI spec:
+	 * pcc_nominal- Expected latency to process a command, in microseconds
+	 * pcc_mpar   - The maximum number of periodic requests that the subspace
+	 *              channel can support, reported in commands per minute. 0
+	 *              indicates no limitation.
+	 * pcc_mrtt   - The minimum amount of time that OSPM must wait after the
+	 *              completion of a command before issuing the next command,
+	 *              in microseconds.
+	 */
+	unsigned int latency_ns = 0;
+	struct cpc_desc *cpc_desc;
+	struct cpc_register_resource *desired_reg;
+
+	cpc_desc = per_cpu(cpc_desc_ptr, cpu_num);
+	if (!cpc_desc)
+		return CPUFREQ_ETERNAL;
+
+	desired_reg = &cpc_desc->cpc_regs[DESIRED_PERF];
+	if (!CPC_IN_PCC(desired_reg))
+		return CPUFREQ_ETERNAL;
+
+	if (pcc_data.pcc_mpar)
+		latency_ns = 60 * (1000 * 1000 * 1000 / pcc_data.pcc_mpar);
+
+	latency_ns = max(latency_ns, pcc_data.pcc_nominal * 1000);
+	latency_ns = max(latency_ns, pcc_data.pcc_mrtt * 1000);
+
+	return latency_ns;
+}
+EXPORT_SYMBOL_GPL(cppc_get_transition_latency);
diff --git a/drivers/acpi/ec.c b/drivers/acpi/ec.c
index e7bd57cc550a..48e19d013170 100644
--- a/drivers/acpi/ec.c
+++ b/drivers/acpi/ec.c
@@ -104,10 +104,12 @@ enum ec_command {
 #define ACPI_EC_MAX_QUERIES	16	/* Maximum number of parallel queries */
 
 enum {
+	EC_FLAGS_QUERY_ENABLED,		/* Query is enabled */
 	EC_FLAGS_QUERY_PENDING,		/* Query is pending */
 	EC_FLAGS_QUERY_GUARDING,	/* Guard for SCI_EVT check */
 	EC_FLAGS_GPE_HANDLER_INSTALLED,	/* GPE handler installed */
 	EC_FLAGS_EC_HANDLER_INSTALLED,	/* OpReg handler installed */
+	EC_FLAGS_EVT_HANDLER_INSTALLED, /* _Qxx handlers installed */
 	EC_FLAGS_STARTED,		/* Driver is started */
 	EC_FLAGS_STOPPED,		/* Driver is stopped */
 	EC_FLAGS_COMMAND_STORM,		/* GPE storms occurred to the
@@ -145,6 +147,10 @@ static unsigned int ec_storm_threshold  __read_mostly = 8;
 module_param(ec_storm_threshold, uint, 0644);
 MODULE_PARM_DESC(ec_storm_threshold, "Maxim false GPE numbers not considered as GPE storm");
 
+static bool ec_freeze_events __read_mostly = true;
+module_param(ec_freeze_events, bool, 0644);
+MODULE_PARM_DESC(ec_freeze_events, "Disabling event handling during suspend/resume");
+
 struct acpi_ec_query_handler {
 	struct list_head node;
 	acpi_ec_query_func func;
@@ -179,6 +185,7 @@ static void acpi_ec_event_processor(struct work_struct *work);
 
 struct acpi_ec *boot_ec, *first_ec;
 EXPORT_SYMBOL(first_ec);
+static bool boot_ec_is_ecdt = false;
 static struct workqueue_struct *ec_query_wq;
 
 static int EC_FLAGS_CLEAR_ON_RESUME; /* Needs acpi_ec_clear() on boot/resume */
@@ -239,6 +246,30 @@ static bool acpi_ec_started(struct acpi_ec *ec)
 	       !test_bit(EC_FLAGS_STOPPED, &ec->flags);
 }
 
+static bool acpi_ec_event_enabled(struct acpi_ec *ec)
+{
+	/*
+	 * There is an OSPM early stage logic. During the early stages
+	 * (boot/resume), OSPMs shouldn't enable the event handling, only
+	 * the EC transactions are allowed to be performed.
+	 */
+	if (!test_bit(EC_FLAGS_QUERY_ENABLED, &ec->flags))
+		return false;
+	/*
+	 * However, disabling the event handling is experimental for late
+	 * stage (suspend), and is controlled by the boot parameter of
+	 * "ec_freeze_events":
+	 * 1. true:  The EC event handling is disabled before entering
+	 *           the noirq stage.
+	 * 2. false: The EC event handling is automatically disabled as
+	 *           soon as the EC driver is stopped.
+	 */
+	if (ec_freeze_events)
+		return acpi_ec_started(ec);
+	else
+		return test_bit(EC_FLAGS_STARTED, &ec->flags);
+}
+
 static bool acpi_ec_flushed(struct acpi_ec *ec)
 {
 	return ec->reference_count == 1;
@@ -429,7 +460,8 @@ 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)) {
+	if (acpi_ec_event_enabled(ec) &&
+	    !test_and_set_bit(EC_FLAGS_QUERY_PENDING, &ec->flags)) {
 		ec_dbg_evt("Command(%s) submitted/blocked",
 			   acpi_ec_cmd_string(ACPI_EC_COMMAND_QUERY));
 		ec->nr_pending_queries++;
@@ -446,6 +478,88 @@ static void acpi_ec_complete_query(struct acpi_ec *ec)
 	}
 }
 
+static inline void __acpi_ec_enable_event(struct acpi_ec *ec)
+{
+	if (!test_and_set_bit(EC_FLAGS_QUERY_ENABLED, &ec->flags))
+		ec_log_drv("event unblocked");
+	if (!test_bit(EC_FLAGS_QUERY_PENDING, &ec->flags))
+		advance_transaction(ec);
+}
+
+static inline void __acpi_ec_disable_event(struct acpi_ec *ec)
+{
+	if (test_and_clear_bit(EC_FLAGS_QUERY_ENABLED, &ec->flags))
+		ec_log_drv("event blocked");
+}
+
+/*
+ * Process _Q events that might have accumulated in the EC.
+ * Run with locked ec mutex.
+ */
+static void acpi_ec_clear(struct acpi_ec *ec)
+{
+	int i, status;
+	u8 value = 0;
+
+	for (i = 0; i < ACPI_EC_CLEAR_MAX; i++) {
+		status = acpi_ec_query(ec, &value);
+		if (status || !value)
+			break;
+	}
+	if (unlikely(i == ACPI_EC_CLEAR_MAX))
+		pr_warn("Warning: Maximum of %d stale EC events cleared\n", i);
+	else
+		pr_info("%d stale EC events cleared\n", i);
+}
+
+static void acpi_ec_enable_event(struct acpi_ec *ec)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&ec->lock, flags);
+	if (acpi_ec_started(ec))
+		__acpi_ec_enable_event(ec);
+	spin_unlock_irqrestore(&ec->lock, flags);
+
+	/* Drain additional events if hardware requires that */
+	if (EC_FLAGS_CLEAR_ON_RESUME)
+		acpi_ec_clear(ec);
+}
+
+#ifdef CONFIG_PM_SLEEP
+static bool acpi_ec_query_flushed(struct acpi_ec *ec)
+{
+	bool flushed;
+	unsigned long flags;
+
+	spin_lock_irqsave(&ec->lock, flags);
+	flushed = !ec->nr_pending_queries;
+	spin_unlock_irqrestore(&ec->lock, flags);
+	return flushed;
+}
+
+static void __acpi_ec_flush_event(struct acpi_ec *ec)
+{
+	/*
+	 * When ec_freeze_events is true, we need to flush events in
+	 * the proper position before entering the noirq stage.
+	 */
+	wait_event(ec->wait, acpi_ec_query_flushed(ec));
+	if (ec_query_wq)
+		flush_workqueue(ec_query_wq);
+}
+
+static void acpi_ec_disable_event(struct acpi_ec *ec)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&ec->lock, flags);
+	__acpi_ec_disable_event(ec);
+	spin_unlock_irqrestore(&ec->lock, flags);
+	__acpi_ec_flush_event(ec);
+}
+#endif /* CONFIG_PM_SLEEP */
+
 static bool acpi_ec_guard_event(struct acpi_ec *ec)
 {
 	bool guarded = true;
@@ -832,27 +946,6 @@ acpi_handle ec_get_handle(void)
 }
 EXPORT_SYMBOL(ec_get_handle);
 
-/*
- * Process _Q events that might have accumulated in the EC.
- * Run with locked ec mutex.
- */
-static void acpi_ec_clear(struct acpi_ec *ec)
-{
-	int i, status;
-	u8 value = 0;
-
-	for (i = 0; i < ACPI_EC_CLEAR_MAX; i++) {
-		status = acpi_ec_query(ec, &value);
-		if (status || !value)
-			break;
-	}
-
-	if (unlikely(i == ACPI_EC_CLEAR_MAX))
-		pr_warn("Warning: Maximum of %d stale EC events cleared\n", i);
-	else
-		pr_info("%d stale EC events cleared\n", i);
-}
-
 static void acpi_ec_start(struct acpi_ec *ec, bool resuming)
 {
 	unsigned long flags;
@@ -896,7 +989,8 @@ static void acpi_ec_stop(struct acpi_ec *ec, bool suspending)
 		if (!suspending) {
 			acpi_ec_complete_request(ec);
 			ec_dbg_ref(ec, "Decrease driver");
-		}
+		} else if (!ec_freeze_events)
+			__acpi_ec_disable_event(ec);
 		clear_bit(EC_FLAGS_STARTED, &ec->flags);
 		clear_bit(EC_FLAGS_STOPPED, &ec->flags);
 		ec_log_drv("EC stopped");
@@ -919,20 +1013,6 @@ void acpi_ec_block_transactions(void)
 
 void acpi_ec_unblock_transactions(void)
 {
-	struct acpi_ec *ec = first_ec;
-
-	if (!ec)
-		return;
-
-	/* Allow transactions to be carried out again */
-	acpi_ec_start(ec, true);
-
-	if (EC_FLAGS_CLEAR_ON_RESUME)
-		acpi_ec_clear(ec);
-}
-
-void acpi_ec_unblock_transactions_early(void)
-{
 	/*
 	 * Allow transactions to happen again (this function is called from
 	 * atomic context during wakeup, so we don't need to acquire the mutex).
@@ -1228,13 +1308,21 @@ acpi_ec_space_handler(u32 function, acpi_physical_address address,
 static acpi_status
 ec_parse_io_ports(struct acpi_resource *resource, void *context);
 
-static struct acpi_ec *make_acpi_ec(void)
+static void acpi_ec_free(struct acpi_ec *ec)
+{
+	if (first_ec == ec)
+		first_ec = NULL;
+	if (boot_ec == ec)
+		boot_ec = NULL;
+	kfree(ec);
+}
+
+static struct acpi_ec *acpi_ec_alloc(void)
 {
 	struct acpi_ec *ec = kzalloc(sizeof(struct acpi_ec), GFP_KERNEL);
 
 	if (!ec)
 		return NULL;
-	ec->flags = 1 << EC_FLAGS_QUERY_PENDING;
 	mutex_init(&ec->mutex);
 	init_waitqueue_head(&ec->wait);
 	INIT_LIST_HEAD(&ec->list);
@@ -1290,7 +1378,12 @@ ec_parse_device(acpi_handle handle, u32 Level, void *context, void **retval)
 	return AE_CTRL_TERMINATE;
 }
 
-static int ec_install_handlers(struct acpi_ec *ec)
+/*
+ * Note: This function returns an error code only when the address space
+ *       handler is not installed, which means "not able to handle
+ *       transactions".
+ */
+static int ec_install_handlers(struct acpi_ec *ec, bool handle_events)
 {
 	acpi_status status;
 
@@ -1319,6 +1412,16 @@ static int ec_install_handlers(struct acpi_ec *ec)
 		set_bit(EC_FLAGS_EC_HANDLER_INSTALLED, &ec->flags);
 	}
 
+	if (!handle_events)
+		return 0;
+
+	if (!test_bit(EC_FLAGS_EVT_HANDLER_INSTALLED, &ec->flags)) {
+		/* Find and register all query methods */
+		acpi_walk_namespace(ACPI_TYPE_METHOD, ec->handle, 1,
+				    acpi_ec_register_query_methods,
+				    NULL, ec, NULL);
+		set_bit(EC_FLAGS_EVT_HANDLER_INSTALLED, &ec->flags);
+	}
 	if (!test_bit(EC_FLAGS_GPE_HANDLER_INSTALLED, &ec->flags)) {
 		status = acpi_install_gpe_raw_handler(NULL, ec->gpe,
 					  ACPI_GPE_EDGE_TRIGGERED,
@@ -1329,6 +1432,9 @@ static int ec_install_handlers(struct acpi_ec *ec)
 			if (test_bit(EC_FLAGS_STARTED, &ec->flags) &&
 			    ec->reference_count >= 1)
 				acpi_ec_enable_gpe(ec, true);
+
+			/* EC is fully operational, allow queries */
+			acpi_ec_enable_event(ec);
 		}
 	}
 
@@ -1363,23 +1469,104 @@ static void ec_remove_handlers(struct acpi_ec *ec)
 			pr_err("failed to remove gpe handler\n");
 		clear_bit(EC_FLAGS_GPE_HANDLER_INSTALLED, &ec->flags);
 	}
+	if (test_bit(EC_FLAGS_EVT_HANDLER_INSTALLED, &ec->flags)) {
+		acpi_ec_remove_query_handlers(ec, true, 0);
+		clear_bit(EC_FLAGS_EVT_HANDLER_INSTALLED, &ec->flags);
+	}
 }
 
-static struct acpi_ec *acpi_ec_alloc(void)
+static int acpi_ec_setup(struct acpi_ec *ec, bool handle_events)
 {
-	struct acpi_ec *ec;
+	int ret;
 
-	/* Check for boot EC */
-	if (boot_ec) {
-		ec = boot_ec;
-		boot_ec = NULL;
-		ec_remove_handlers(ec);
-		if (first_ec == ec)
-			first_ec = NULL;
-	} else {
-		ec = make_acpi_ec();
+	ret = ec_install_handlers(ec, handle_events);
+	if (ret)
+		return ret;
+
+	/* First EC capable of handling transactions */
+	if (!first_ec) {
+		first_ec = ec;
+		acpi_handle_info(first_ec->handle, "Used as first EC\n");
 	}
-	return ec;
+
+	acpi_handle_info(ec->handle,
+			 "GPE=0x%lx, EC_CMD/EC_SC=0x%lx, EC_DATA=0x%lx\n",
+			 ec->gpe, ec->command_addr, ec->data_addr);
+	return ret;
+}
+
+static int acpi_config_boot_ec(struct acpi_ec *ec, acpi_handle handle,
+			       bool handle_events, bool is_ecdt)
+{
+	int ret;
+
+	/*
+	 * Changing the ACPI handle results in a re-configuration of the
+	 * boot EC. And if it happens after the namespace initialization,
+	 * it causes _REG evaluations.
+	 */
+	if (boot_ec && boot_ec->handle != handle)
+		ec_remove_handlers(boot_ec);
+
+	/* Unset old boot EC */
+	if (boot_ec != ec)
+		acpi_ec_free(boot_ec);
+
+	/*
+	 * ECDT device creation is split into acpi_ec_ecdt_probe() and
+	 * acpi_ec_ecdt_start(). This function takes care of completing the
+	 * ECDT parsing logic as the handle update should be performed
+	 * between the installation/uninstallation of the handlers.
+	 */
+	if (ec->handle != handle)
+		ec->handle = handle;
+
+	ret = acpi_ec_setup(ec, handle_events);
+	if (ret)
+		return ret;
+
+	/* Set new boot EC */
+	if (!boot_ec) {
+		boot_ec = ec;
+		boot_ec_is_ecdt = is_ecdt;
+	}
+
+	acpi_handle_info(boot_ec->handle,
+			 "Used as boot %s EC to handle transactions%s\n",
+			 is_ecdt ? "ECDT" : "DSDT",
+			 handle_events ? " and events" : "");
+	return ret;
+}
+
+static bool acpi_ec_ecdt_get_handle(acpi_handle *phandle)
+{
+	struct acpi_table_ecdt *ecdt_ptr;
+	acpi_status status;
+	acpi_handle handle;
+
+	status = acpi_get_table(ACPI_SIG_ECDT, 1,
+				(struct acpi_table_header **)&ecdt_ptr);
+	if (ACPI_FAILURE(status))
+		return false;
+
+	status = acpi_get_handle(NULL, ecdt_ptr->id, &handle);
+	if (ACPI_FAILURE(status))
+		return false;
+
+	*phandle = handle;
+	return true;
+}
+
+static bool acpi_is_boot_ec(struct acpi_ec *ec)
+{
+	if (!boot_ec)
+		return false;
+	if (ec->handle == boot_ec->handle &&
+	    ec->gpe == boot_ec->gpe &&
+	    ec->command_addr == boot_ec->command_addr &&
+	    ec->data_addr == boot_ec->data_addr)
+		return true;
+	return false;
 }
 
 static int acpi_ec_add(struct acpi_device *device)
@@ -1395,16 +1582,21 @@ static int acpi_ec_add(struct acpi_device *device)
 		return -ENOMEM;
 	if (ec_parse_device(device->handle, 0, ec, NULL) !=
 		AE_CTRL_TERMINATE) {
-			kfree(ec);
-			return -EINVAL;
+			ret = -EINVAL;
+			goto err_alloc;
 	}
 
-	/* Find and register all query methods */
-	acpi_walk_namespace(ACPI_TYPE_METHOD, ec->handle, 1,
-			    acpi_ec_register_query_methods, NULL, ec, NULL);
+	if (acpi_is_boot_ec(ec)) {
+		boot_ec_is_ecdt = false;
+		acpi_handle_debug(ec->handle, "duplicated.\n");
+		acpi_ec_free(ec);
+		ec = boot_ec;
+		ret = acpi_config_boot_ec(ec, ec->handle, true, false);
+	} else
+		ret = acpi_ec_setup(ec, true);
+	if (ret)
+		goto err_query;
 
-	if (!first_ec)
-		first_ec = ec;
 	device->driver_data = ec;
 
 	ret = !!request_region(ec->data_addr, 1, "EC data");
@@ -1412,20 +1604,17 @@ static int acpi_ec_add(struct acpi_device *device)
 	ret = !!request_region(ec->command_addr, 1, "EC cmd");
 	WARN(!ret, "Could not request EC cmd io port 0x%lx", ec->command_addr);
 
-	pr_info("GPE = 0x%lx, I/O: command/status = 0x%lx, data = 0x%lx\n",
-			  ec->gpe, ec->command_addr, ec->data_addr);
-
-	ret = ec_install_handlers(ec);
-
 	/* Reprobe devices depending on the EC */
 	acpi_walk_dep_device_list(ec->handle);
+	acpi_handle_debug(ec->handle, "enumerated.\n");
+	return 0;
 
-	/* EC is fully operational, allow queries */
-	clear_bit(EC_FLAGS_QUERY_PENDING, &ec->flags);
-
-	/* Clear stale _Q events if hardware might require that */
-	if (EC_FLAGS_CLEAR_ON_RESUME)
-		acpi_ec_clear(ec);
+err_query:
+	if (ec != boot_ec)
+		acpi_ec_remove_query_handlers(ec, true, 0);
+err_alloc:
+	if (ec != boot_ec)
+		acpi_ec_free(ec);
 	return ret;
 }
 
@@ -1437,14 +1626,13 @@ static int acpi_ec_remove(struct acpi_device *device)
 		return -EINVAL;
 
 	ec = acpi_driver_data(device);
-	ec_remove_handlers(ec);
-	acpi_ec_remove_query_handlers(ec, true, 0);
 	release_region(ec->data_addr, 1);
 	release_region(ec->command_addr, 1);
 	device->driver_data = NULL;
-	if (ec == first_ec)
-		first_ec = NULL;
-	kfree(ec);
+	if (ec != boot_ec) {
+		ec_remove_handlers(ec);
+		acpi_ec_free(ec);
+	}
 	return 0;
 }
 
@@ -1486,9 +1674,8 @@ int __init acpi_ec_dsdt_probe(void)
 	if (!ec)
 		return -ENOMEM;
 	/*
-	 * Finding EC from DSDT if there is no ECDT EC available. When this
-	 * function is invoked, ACPI tables have been fully loaded, we can
-	 * walk namespace now.
+	 * At this point, the namespace is initialized, so start to find
+	 * the namespace objects.
 	 */
 	status = acpi_get_devices(ec_device_ids[0].id,
 				  ec_parse_device, ec, NULL);
@@ -1496,16 +1683,47 @@ int __init acpi_ec_dsdt_probe(void)
 		ret = -ENODEV;
 		goto error;
 	}
-	ret = ec_install_handlers(ec);
-
+	/*
+	 * When the DSDT EC is available, always re-configure boot EC to
+	 * have _REG evaluated. _REG can only be evaluated after the
+	 * namespace initialization.
+	 * At this point, the GPE is not fully initialized, so do not to
+	 * handle the events.
+	 */
+	ret = acpi_config_boot_ec(ec, ec->handle, false, false);
 error:
 	if (ret)
-		kfree(ec);
-	else
-		first_ec = boot_ec = ec;
+		acpi_ec_free(ec);
 	return ret;
 }
 
+/*
+ * If the DSDT EC is not functioning, we still need to prepare a fully
+ * functioning ECDT EC first in order to handle the events.
+ * https://bugzilla.kernel.org/show_bug.cgi?id=115021
+ */
+int __init acpi_ec_ecdt_start(void)
+{
+	acpi_handle handle;
+
+	if (!boot_ec)
+		return -ENODEV;
+	/*
+	 * The DSDT EC should have already been started in
+	 * acpi_ec_add().
+	 */
+	if (!boot_ec_is_ecdt)
+		return -ENODEV;
+
+	/*
+	 * At this point, the namespace and the GPE is initialized, so
+	 * start to find the namespace objects and handle the events.
+	 */
+	if (!acpi_ec_ecdt_get_handle(&handle))
+		return -ENODEV;
+	return acpi_config_boot_ec(boot_ec, handle, true, true);
+}
+
 #if 0
 /*
  * Some EC firmware variations refuses to respond QR_EC when SCI_EVT is not
@@ -1600,7 +1818,6 @@ int __init acpi_ec_ecdt_probe(void)
 		goto error;
 	}
 
-	pr_info("EC description table is found, configuring boot EC\n");
 	if (EC_FLAGS_CORRECT_ECDT) {
 		ec->command_addr = ecdt_ptr->data.address;
 		ec->data_addr = ecdt_ptr->control.address;
@@ -1609,16 +1826,90 @@ int __init acpi_ec_ecdt_probe(void)
 		ec->data_addr = ecdt_ptr->data.address;
 	}
 	ec->gpe = ecdt_ptr->gpe;
-	ec->handle = ACPI_ROOT_OBJECT;
-	ret = ec_install_handlers(ec);
+
+	/*
+	 * At this point, the namespace is not initialized, so do not find
+	 * the namespace objects, or handle the events.
+	 */
+	ret = acpi_config_boot_ec(ec, ACPI_ROOT_OBJECT, false, true);
 error:
 	if (ret)
-		kfree(ec);
-	else
-		first_ec = boot_ec = ec;
+		acpi_ec_free(ec);
 	return ret;
 }
 
+#ifdef CONFIG_PM_SLEEP
+static void acpi_ec_enter_noirq(struct acpi_ec *ec)
+{
+	unsigned long flags;
+
+	if (ec == first_ec) {
+		spin_lock_irqsave(&ec->lock, flags);
+		ec->saved_busy_polling = ec_busy_polling;
+		ec->saved_polling_guard = ec_polling_guard;
+		ec_busy_polling = true;
+		ec_polling_guard = 0;
+		ec_log_drv("interrupt blocked");
+		spin_unlock_irqrestore(&ec->lock, flags);
+	}
+}
+
+static void acpi_ec_leave_noirq(struct acpi_ec *ec)
+{
+	unsigned long flags;
+
+	if (ec == first_ec) {
+		spin_lock_irqsave(&ec->lock, flags);
+		ec_busy_polling = ec->saved_busy_polling;
+		ec_polling_guard = ec->saved_polling_guard;
+		ec_log_drv("interrupt unblocked");
+		spin_unlock_irqrestore(&ec->lock, flags);
+	}
+}
+
+static int acpi_ec_suspend_noirq(struct device *dev)
+{
+	struct acpi_ec *ec =
+		acpi_driver_data(to_acpi_device(dev));
+
+	acpi_ec_enter_noirq(ec);
+	return 0;
+}
+
+static int acpi_ec_resume_noirq(struct device *dev)
+{
+	struct acpi_ec *ec =
+		acpi_driver_data(to_acpi_device(dev));
+
+	acpi_ec_leave_noirq(ec);
+	return 0;
+}
+
+static int acpi_ec_suspend(struct device *dev)
+{
+	struct acpi_ec *ec =
+		acpi_driver_data(to_acpi_device(dev));
+
+	if (ec_freeze_events)
+		acpi_ec_disable_event(ec);
+	return 0;
+}
+
+static int acpi_ec_resume(struct device *dev)
+{
+	struct acpi_ec *ec =
+		acpi_driver_data(to_acpi_device(dev));
+
+	acpi_ec_enable_event(ec);
+	return 0;
+}
+#endif
+
+static const struct dev_pm_ops acpi_ec_pm = {
+	SET_NOIRQ_SYSTEM_SLEEP_PM_OPS(acpi_ec_suspend_noirq, acpi_ec_resume_noirq)
+	SET_SYSTEM_SLEEP_PM_OPS(acpi_ec_suspend, acpi_ec_resume)
+};
+
 static int param_set_event_clearing(const char *val, struct kernel_param *kp)
 {
 	int result = 0;
@@ -1664,6 +1955,7 @@ static struct acpi_driver acpi_ec_driver = {
 		.add = acpi_ec_add,
 		.remove = acpi_ec_remove,
 		},
+	.drv.pm = &acpi_ec_pm,
 };
 
 static inline int acpi_ec_query_init(void)
diff --git a/drivers/acpi/fan.c b/drivers/acpi/fan.c
index 384cfc3083e1..6cf4988206f2 100644
--- a/drivers/acpi/fan.c
+++ b/drivers/acpi/fan.c
@@ -129,8 +129,18 @@ static int fan_get_state_acpi4(struct acpi_device *device, unsigned long *state)
 
 	control = obj->package.elements[1].integer.value;
 	for (i = 0; i < fan->fps_count; i++) {
-		if (control == fan->fps[i].control)
+		/*
+		 * When Fine Grain Control is set, return the state
+		 * corresponding to maximum fan->fps[i].control
+		 * value compared to the current speed. Here the
+		 * fan->fps[] is sorted array with increasing speed.
+		 */
+		if (fan->fif.fine_grain_ctrl && control < fan->fps[i].control) {
+			i = (i > 0) ? i - 1 : 0;
 			break;
+		} else if (control == fan->fps[i].control) {
+			break;
+		}
 	}
 	if (i == fan->fps_count) {
 		dev_dbg(&device->dev, "Invalid control value returned\n");
diff --git a/drivers/acpi/internal.h b/drivers/acpi/internal.h
index 940218ff0193..1b41a2739dac 100644
--- a/drivers/acpi/internal.h
+++ b/drivers/acpi/internal.h
@@ -40,10 +40,8 @@ int acpi_sysfs_init(void);
 void acpi_container_init(void);
 void acpi_memory_hotplug_init(void);
 #ifdef	CONFIG_ACPI_HOTPLUG_IOAPIC
-int acpi_ioapic_add(struct acpi_pci_root *root);
 int acpi_ioapic_remove(struct acpi_pci_root *root);
 #else
-static inline int acpi_ioapic_add(struct acpi_pci_root *root) { return 0; }
 static inline int acpi_ioapic_remove(struct acpi_pci_root *root) { return 0; }
 #endif
 #ifdef CONFIG_ACPI_DOCK
@@ -116,7 +114,6 @@ bool acpi_device_is_present(struct acpi_device *adev);
 bool acpi_device_is_battery(struct acpi_device *adev);
 bool acpi_device_is_first_physical_node(struct acpi_device *adev,
 					const struct device *dev);
-struct device *acpi_get_first_physical_node(struct acpi_device *adev);
 
 /* --------------------------------------------------------------------------
                      Device Matching and Notification
@@ -174,6 +171,8 @@ struct acpi_ec {
 	struct work_struct work;
 	unsigned long timestamp;
 	unsigned long nr_pending_queries;
+	bool saved_busy_polling;
+	unsigned int saved_polling_guard;
 };
 
 extern struct acpi_ec *first_ec;
@@ -185,9 +184,9 @@ typedef int (*acpi_ec_query_func) (void *data);
 int acpi_ec_init(void);
 int acpi_ec_ecdt_probe(void);
 int acpi_ec_dsdt_probe(void);
+int acpi_ec_ecdt_start(void);
 void acpi_ec_block_transactions(void);
 void acpi_ec_unblock_transactions(void);
-void acpi_ec_unblock_transactions_early(void);
 int acpi_ec_add_query_handler(struct acpi_ec *ec, u8 query_bit,
 			      acpi_handle handle, acpi_ec_query_func func,
 			      void *data);
@@ -225,4 +224,14 @@ static inline void suspend_nvs_restore(void) {}
 void acpi_init_properties(struct acpi_device *adev);
 void acpi_free_properties(struct acpi_device *adev);
 
+/*--------------------------------------------------------------------------
+				Watchdog
+  -------------------------------------------------------------------------- */
+
+#ifdef CONFIG_ACPI_WATCHDOG
+void acpi_watchdog_init(void);
+#else
+static inline void acpi_watchdog_init(void) {}
+#endif
+
 #endif /* _ACPI_INTERNAL_H_ */
diff --git a/drivers/acpi/ioapic.c b/drivers/acpi/ioapic.c
index ccdc8db16bb8..6d7ce6e12aaa 100644
--- a/drivers/acpi/ioapic.c
+++ b/drivers/acpi/ioapic.c
@@ -46,7 +46,7 @@ static acpi_status setup_res(struct acpi_resource *acpi_res, void *data)
 	struct resource_win win;
 
 	res->flags = 0;
-	if (acpi_dev_filter_resource_type(acpi_res, IORESOURCE_MEM) == 0)
+	if (acpi_dev_filter_resource_type(acpi_res, IORESOURCE_MEM))
 		return AE_OK;
 
 	if (!acpi_dev_resource_memory(acpi_res, res)) {
@@ -97,7 +97,7 @@ static acpi_status handle_ioapic_add(acpi_handle handle, u32 lvl,
 	unsigned long long gsi_base;
 	struct acpi_pci_ioapic *ioapic;
 	struct pci_dev *dev = NULL;
-	struct resource *res = NULL;
+	struct resource *res = NULL, *pci_res = NULL, *crs_res;
 	char *type = NULL;
 
 	if (!acpi_is_ioapic(handle, &type))
@@ -137,23 +137,30 @@ static acpi_status handle_ioapic_add(acpi_handle handle, u32 lvl,
 		pci_set_master(dev);
 		if (pci_request_region(dev, 0, type))
 			goto exit_disable;
-		res = &dev->resource[0];
+		pci_res = &dev->resource[0];
 		ioapic->pdev = dev;
 	} else {
 		pci_dev_put(dev);
 		dev = NULL;
+	}
 
-		res = &ioapic->res;
-		acpi_walk_resources(handle, METHOD_NAME__CRS, setup_res, res);
-		if (res->flags == 0) {
-			acpi_handle_warn(handle, "failed to get resource\n");
-			goto exit_free;
-		} else if (request_resource(&iomem_resource, res)) {
-			acpi_handle_warn(handle, "failed to insert resource\n");
-			goto exit_free;
-		}
+	crs_res = &ioapic->res;
+	acpi_walk_resources(handle, METHOD_NAME__CRS, setup_res, crs_res);
+	crs_res->name = type;
+	crs_res->flags |= IORESOURCE_BUSY;
+	if (crs_res->flags == 0) {
+		acpi_handle_warn(handle, "failed to get resource\n");
+		goto exit_release;
+	} else if (insert_resource(&iomem_resource, crs_res)) {
+		acpi_handle_warn(handle, "failed to insert resource\n");
+		goto exit_release;
 	}
 
+	/* try pci resource first, then "_CRS" resource */
+	res = pci_res;
+	if (!res || !res->flags)
+		res = crs_res;
+
 	if (acpi_register_ioapic(handle, res->start, (u32)gsi_base)) {
 		acpi_handle_warn(handle, "failed to register IOAPIC\n");
 		goto exit_release;
@@ -174,14 +181,13 @@ done:
 exit_release:
 	if (dev)
 		pci_release_region(dev, 0);
-	else
-		release_resource(res);
+	if (ioapic->res.flags && ioapic->res.parent)
+		release_resource(&ioapic->res);
 exit_disable:
 	if (dev)
 		pci_disable_device(dev);
 exit_put:
 	pci_dev_put(dev);
-exit_free:
 	kfree(ioapic);
 exit:
 	mutex_unlock(&ioapic_list_lock);
@@ -189,13 +195,13 @@ exit:
 	return AE_OK;
 }
 
-int acpi_ioapic_add(struct acpi_pci_root *root)
+int acpi_ioapic_add(acpi_handle root_handle)
 {
 	acpi_status status, retval = AE_OK;
 
-	status = acpi_walk_namespace(ACPI_TYPE_DEVICE, root->device->handle,
+	status = acpi_walk_namespace(ACPI_TYPE_DEVICE, root_handle,
 				     UINT_MAX, handle_ioapic_add, NULL,
-				     root->device->handle, (void **)&retval);
+				     root_handle, (void **)&retval);
 
 	return ACPI_SUCCESS(status) && ACPI_SUCCESS(retval) ? 0 : -ENODEV;
 }
@@ -217,9 +223,9 @@ int acpi_ioapic_remove(struct acpi_pci_root *root)
 			pci_release_region(ioapic->pdev, 0);
 			pci_disable_device(ioapic->pdev);
 			pci_dev_put(ioapic->pdev);
-		} else if (ioapic->res.flags && ioapic->res.parent) {
-			release_resource(&ioapic->res);
 		}
+		if (ioapic->res.flags && ioapic->res.parent)
+			release_resource(&ioapic->res);
 		list_del(&ioapic->list);
 		kfree(ioapic);
 	}
diff --git a/drivers/acpi/nfit/core.c b/drivers/acpi/nfit/core.c
index 80cc7c089a15..71a7d07c28c9 100644
--- a/drivers/acpi/nfit/core.c
+++ b/drivers/acpi/nfit/core.c
@@ -94,54 +94,50 @@ static struct acpi_device *to_acpi_dev(struct acpi_nfit_desc *acpi_desc)
 	return to_acpi_device(acpi_desc->dev);
 }
 
-static int xlat_status(void *buf, unsigned int cmd)
+static int xlat_status(void *buf, unsigned int cmd, u32 status)
 {
 	struct nd_cmd_clear_error *clear_err;
 	struct nd_cmd_ars_status *ars_status;
-	struct nd_cmd_ars_start *ars_start;
-	struct nd_cmd_ars_cap *ars_cap;
 	u16 flags;
 
 	switch (cmd) {
 	case ND_CMD_ARS_CAP:
-		ars_cap = buf;
-		if ((ars_cap->status & 0xffff) == NFIT_ARS_CAP_NONE)
+		if ((status & 0xffff) == NFIT_ARS_CAP_NONE)
 			return -ENOTTY;
 
 		/* Command failed */
-		if (ars_cap->status & 0xffff)
+		if (status & 0xffff)
 			return -EIO;
 
 		/* No supported scan types for this range */
 		flags = ND_ARS_PERSISTENT | ND_ARS_VOLATILE;
-		if ((ars_cap->status >> 16 & flags) == 0)
+		if ((status >> 16 & flags) == 0)
 			return -ENOTTY;
 		break;
 	case ND_CMD_ARS_START:
-		ars_start = buf;
 		/* ARS is in progress */
-		if ((ars_start->status & 0xffff) == NFIT_ARS_START_BUSY)
+		if ((status & 0xffff) == NFIT_ARS_START_BUSY)
 			return -EBUSY;
 
 		/* Command failed */
-		if (ars_start->status & 0xffff)
+		if (status & 0xffff)
 			return -EIO;
 		break;
 	case ND_CMD_ARS_STATUS:
 		ars_status = buf;
 		/* Command failed */
-		if (ars_status->status & 0xffff)
+		if (status & 0xffff)
 			return -EIO;
 		/* Check extended status (Upper two bytes) */
-		if (ars_status->status == NFIT_ARS_STATUS_DONE)
+		if (status == NFIT_ARS_STATUS_DONE)
 			return 0;
 
 		/* ARS is in progress */
-		if (ars_status->status == NFIT_ARS_STATUS_BUSY)
+		if (status == NFIT_ARS_STATUS_BUSY)
 			return -EBUSY;
 
 		/* No ARS performed for the current boot */
-		if (ars_status->status == NFIT_ARS_STATUS_NONE)
+		if (status == NFIT_ARS_STATUS_NONE)
 			return -EAGAIN;
 
 		/*
@@ -149,19 +145,19 @@ static int xlat_status(void *buf, unsigned int cmd)
 		 * agent wants the scan to stop.  If we didn't overflow
 		 * then just continue with the returned results.
 		 */
-		if (ars_status->status == NFIT_ARS_STATUS_INTR) {
+		if (status == NFIT_ARS_STATUS_INTR) {
 			if (ars_status->flags & NFIT_ARS_F_OVERFLOW)
 				return -ENOSPC;
 			return 0;
 		}
 
 		/* Unknown status */
-		if (ars_status->status >> 16)
+		if (status >> 16)
 			return -EIO;
 		break;
 	case ND_CMD_CLEAR_ERROR:
 		clear_err = buf;
-		if (clear_err->status & 0xffff)
+		if (status & 0xffff)
 			return -EIO;
 		if (!clear_err->cleared)
 			return -EIO;
@@ -172,6 +168,9 @@ static int xlat_status(void *buf, unsigned int cmd)
 		break;
 	}
 
+	/* all other non-zero status results in an error */
+	if (status)
+		return -EIO;
 	return 0;
 }
 
@@ -186,10 +185,10 @@ static int acpi_nfit_ctl(struct nvdimm_bus_descriptor *nd_desc,
 	struct nd_cmd_pkg *call_pkg = NULL;
 	const char *cmd_name, *dimm_name;
 	unsigned long cmd_mask, dsm_mask;
+	u32 offset, fw_status = 0;
 	acpi_handle handle;
 	unsigned int func;
 	const u8 *uuid;
-	u32 offset;
 	int rc, i;
 
 	func = cmd;
@@ -317,6 +316,15 @@ static int acpi_nfit_ctl(struct nvdimm_bus_descriptor *nd_desc,
 				out_obj->buffer.pointer + offset, out_size);
 		offset += out_size;
 	}
+
+	/*
+	 * Set fw_status for all the commands with a known format to be
+	 * later interpreted by xlat_status().
+	 */
+	if (i >= 1 && ((cmd >= ND_CMD_ARS_CAP && cmd <= ND_CMD_CLEAR_ERROR)
+			|| (cmd >= ND_CMD_SMART && cmd <= ND_CMD_VENDOR)))
+		fw_status = *(u32 *) out_obj->buffer.pointer;
+
 	if (offset + in_buf.buffer.length < buf_len) {
 		if (i >= 1) {
 			/*
@@ -325,7 +333,7 @@ static int acpi_nfit_ctl(struct nvdimm_bus_descriptor *nd_desc,
 			 */
 			rc = buf_len - offset - in_buf.buffer.length;
 			if (cmd_rc)
-				*cmd_rc = xlat_status(buf, cmd);
+				*cmd_rc = xlat_status(buf, cmd, fw_status);
 		} else {
 			dev_err(dev, "%s:%s underrun cmd: %s buf_len: %d out_len: %d\n",
 					__func__, dimm_name, cmd_name, buf_len,
@@ -335,7 +343,7 @@ static int acpi_nfit_ctl(struct nvdimm_bus_descriptor *nd_desc,
 	} else {
 		rc = 0;
 		if (cmd_rc)
-			*cmd_rc = xlat_status(buf, cmd);
+			*cmd_rc = xlat_status(buf, cmd, fw_status);
 	}
 
  out:
@@ -878,6 +886,58 @@ static ssize_t revision_show(struct device *dev,
 }
 static DEVICE_ATTR_RO(revision);
 
+static ssize_t hw_error_scrub_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->scrub_mode);
+}
+
+/*
+ * The 'hw_error_scrub' attribute can have the following values written to it:
+ * '0': Switch to the default mode where an exception will only insert
+ *      the address of the memory error into the poison and badblocks lists.
+ * '1': Enable a full scrub to happen if an exception for a memory error is
+ *      received.
+ */
+static ssize_t hw_error_scrub_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t size)
+{
+	struct nvdimm_bus_descriptor *nd_desc;
+	ssize_t rc;
+	long val;
+
+	rc = kstrtol(buf, 0, &val);
+	if (rc)
+		return rc;
+
+	device_lock(dev);
+	nd_desc = dev_get_drvdata(dev);
+	if (nd_desc) {
+		struct acpi_nfit_desc *acpi_desc = to_acpi_desc(nd_desc);
+
+		switch (val) {
+		case HW_ERROR_SCRUB_ON:
+			acpi_desc->scrub_mode = HW_ERROR_SCRUB_ON;
+			break;
+		case HW_ERROR_SCRUB_OFF:
+			acpi_desc->scrub_mode = HW_ERROR_SCRUB_OFF;
+			break;
+		default:
+			rc = -EINVAL;
+			break;
+		}
+	}
+	device_unlock(dev);
+	if (rc)
+		return rc;
+	return size;
+}
+static DEVICE_ATTR_RW(hw_error_scrub);
+
 /*
  * This shows the number of full Address Range Scrubs that have been
  * completed since driver load time. Userspace can wait on this using
@@ -950,6 +1010,7 @@ static umode_t nfit_visible(struct kobject *kobj, struct attribute *a, int n)
 static struct attribute *acpi_nfit_attributes[] = {
 	&dev_attr_revision.attr,
 	&dev_attr_scrub.attr,
+	&dev_attr_hw_error_scrub.attr,
 	NULL,
 };
 
@@ -1248,6 +1309,44 @@ static struct nvdimm *acpi_nfit_dimm_by_handle(struct acpi_nfit_desc *acpi_desc,
 	return NULL;
 }
 
+void __acpi_nvdimm_notify(struct device *dev, u32 event)
+{
+	struct nfit_mem *nfit_mem;
+	struct acpi_nfit_desc *acpi_desc;
+
+	dev_dbg(dev->parent, "%s: %s: event: %d\n", dev_name(dev), __func__,
+			event);
+
+	if (event != NFIT_NOTIFY_DIMM_HEALTH) {
+		dev_dbg(dev->parent, "%s: unknown event: %d\n", dev_name(dev),
+				event);
+		return;
+	}
+
+	acpi_desc = dev_get_drvdata(dev->parent);
+	if (!acpi_desc)
+		return;
+
+	/*
+	 * If we successfully retrieved acpi_desc, then we know nfit_mem data
+	 * is still valid.
+	 */
+	nfit_mem = dev_get_drvdata(dev);
+	if (nfit_mem && nfit_mem->flags_attr)
+		sysfs_notify_dirent(nfit_mem->flags_attr);
+}
+EXPORT_SYMBOL_GPL(__acpi_nvdimm_notify);
+
+static void acpi_nvdimm_notify(acpi_handle handle, u32 event, void *data)
+{
+	struct acpi_device *adev = data;
+	struct device *dev = &adev->dev;
+
+	device_lock(dev->parent);
+	__acpi_nvdimm_notify(dev, event);
+	device_unlock(dev->parent);
+}
+
 static int acpi_nfit_add_dimm(struct acpi_nfit_desc *acpi_desc,
 		struct nfit_mem *nfit_mem, u32 device_handle)
 {
@@ -1272,6 +1371,13 @@ static int acpi_nfit_add_dimm(struct acpi_nfit_desc *acpi_desc,
 		return force_enable_dimms ? 0 : -ENODEV;
 	}
 
+	if (ACPI_FAILURE(acpi_install_notify_handler(adev_dimm->handle,
+		ACPI_DEVICE_NOTIFY, acpi_nvdimm_notify, adev_dimm))) {
+		dev_err(dev, "%s: notification registration failed\n",
+				dev_name(&adev_dimm->dev));
+		return -ENXIO;
+	}
+
 	/*
 	 * Until standardization materializes we need to consider 4
 	 * different command sets.  Note, that checking for function0 (bit0)
@@ -1310,18 +1416,41 @@ static int acpi_nfit_add_dimm(struct acpi_nfit_desc *acpi_desc,
 	return 0;
 }
 
+static void shutdown_dimm_notify(void *data)
+{
+	struct acpi_nfit_desc *acpi_desc = data;
+	struct nfit_mem *nfit_mem;
+
+	mutex_lock(&acpi_desc->init_mutex);
+	/*
+	 * Clear out the nfit_mem->flags_attr and shut down dimm event
+	 * notifications.
+	 */
+	list_for_each_entry(nfit_mem, &acpi_desc->dimms, list) {
+		struct acpi_device *adev_dimm = nfit_mem->adev;
+
+		if (nfit_mem->flags_attr) {
+			sysfs_put(nfit_mem->flags_attr);
+			nfit_mem->flags_attr = NULL;
+		}
+		if (adev_dimm)
+			acpi_remove_notify_handler(adev_dimm->handle,
+					ACPI_DEVICE_NOTIFY, acpi_nvdimm_notify);
+	}
+	mutex_unlock(&acpi_desc->init_mutex);
+}
+
 static int acpi_nfit_register_dimms(struct acpi_nfit_desc *acpi_desc)
 {
 	struct nfit_mem *nfit_mem;
-	int dimm_count = 0;
+	int dimm_count = 0, rc;
+	struct nvdimm *nvdimm;
 
 	list_for_each_entry(nfit_mem, &acpi_desc->dimms, list) {
 		struct acpi_nfit_flush_address *flush;
 		unsigned long flags = 0, cmd_mask;
-		struct nvdimm *nvdimm;
 		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);
@@ -1374,7 +1503,30 @@ static int acpi_nfit_register_dimms(struct acpi_nfit_desc *acpi_desc)
 
 	}
 
-	return nvdimm_bus_check_dimm_count(acpi_desc->nvdimm_bus, dimm_count);
+	rc = nvdimm_bus_check_dimm_count(acpi_desc->nvdimm_bus, dimm_count);
+	if (rc)
+		return rc;
+
+	/*
+	 * Now that dimms are successfully registered, and async registration
+	 * is flushed, attempt to enable event notification.
+	 */
+	list_for_each_entry(nfit_mem, &acpi_desc->dimms, list) {
+		struct kernfs_node *nfit_kernfs;
+
+		nvdimm = nfit_mem->nvdimm;
+		nfit_kernfs = sysfs_get_dirent(nvdimm_kobj(nvdimm)->sd, "nfit");
+		if (nfit_kernfs)
+			nfit_mem->flags_attr = sysfs_get_dirent(nfit_kernfs,
+					"flags");
+		sysfs_put(nfit_kernfs);
+		if (!nfit_mem->flags_attr)
+			dev_warn(acpi_desc->dev, "%s: notifications disabled\n",
+					nvdimm_name(nvdimm));
+	}
+
+	return devm_add_action_or_reset(acpi_desc->dev, shutdown_dimm_notify,
+			acpi_desc);
 }
 
 static void acpi_nfit_init_dsms(struct acpi_nfit_desc *acpi_desc)
@@ -1483,9 +1635,9 @@ static int acpi_nfit_init_interleave_set(struct acpi_nfit_desc *acpi_desc,
 	if (!info)
 		return -ENOMEM;
 	for (i = 0; i < nr; i++) {
-		struct nd_mapping *nd_mapping = &ndr_desc->nd_mapping[i];
+		struct nd_mapping_desc *mapping = &ndr_desc->mapping[i];
 		struct nfit_set_info_map *map = &info->mapping[i];
-		struct nvdimm *nvdimm = nd_mapping->nvdimm;
+		struct nvdimm *nvdimm = 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);
@@ -1909,7 +2061,7 @@ static int acpi_nfit_insert_resource(struct acpi_nfit_desc *acpi_desc,
 }
 
 static int acpi_nfit_init_mapping(struct acpi_nfit_desc *acpi_desc,
-		struct nd_mapping *nd_mapping, struct nd_region_desc *ndr_desc,
+		struct nd_mapping_desc *mapping, struct nd_region_desc *ndr_desc,
 		struct acpi_nfit_memory_map *memdev,
 		struct nfit_spa *nfit_spa)
 {
@@ -1926,12 +2078,12 @@ static int acpi_nfit_init_mapping(struct acpi_nfit_desc *acpi_desc,
 		return -ENODEV;
 	}
 
-	nd_mapping->nvdimm = nvdimm;
+	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;
+		mapping->start = memdev->address;
+		mapping->size = memdev->region_size;
 		break;
 	case NFIT_SPA_DCR:
 		nfit_mem = nvdimm_provider_data(nvdimm);
@@ -1939,13 +2091,13 @@ static int acpi_nfit_init_mapping(struct acpi_nfit_desc *acpi_desc,
 			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;
+			mapping->size = nfit_mem->bdw->capacity;
+			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->mapping = mapping;
 		ndr_desc->num_mappings = blk_valid;
 		ndbr_desc = to_blk_region_desc(ndr_desc);
 		ndbr_desc->enable = acpi_nfit_blk_region_enable;
@@ -1971,7 +2123,7 @@ static bool nfit_spa_is_virtual(struct acpi_nfit_system_address *spa)
 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];
+	static struct nd_mapping_desc 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;
@@ -1990,7 +2142,7 @@ static int acpi_nfit_register_region(struct acpi_nfit_desc *acpi_desc,
 	}
 
 	memset(&res, 0, sizeof(res));
-	memset(&nd_mappings, 0, sizeof(nd_mappings));
+	memset(&mappings, 0, sizeof(mappings));
 	memset(&ndbr_desc, 0, sizeof(ndbr_desc));
 	res.start = spa->address;
 	res.end = res.start + spa->length - 1;
@@ -2006,7 +2158,7 @@ static int acpi_nfit_register_region(struct acpi_nfit_desc *acpi_desc,
 
 	list_for_each_entry(nfit_memdev, &acpi_desc->memdevs, list) {
 		struct acpi_nfit_memory_map *memdev = nfit_memdev->memdev;
-		struct nd_mapping *nd_mapping;
+		struct nd_mapping_desc *mapping;
 
 		if (memdev->range_index != spa->range_index)
 			continue;
@@ -2015,14 +2167,14 @@ static int acpi_nfit_register_region(struct acpi_nfit_desc *acpi_desc,
 					spa->range_index, ND_MAX_MAPPINGS);
 			return -ENXIO;
 		}
-		nd_mapping = &nd_mappings[count++];
-		rc = acpi_nfit_init_mapping(acpi_desc, nd_mapping, ndr_desc,
+		mapping = &mappings[count++];
+		rc = acpi_nfit_init_mapping(acpi_desc, mapping, ndr_desc,
 				memdev, nfit_spa);
 		if (rc)
 			goto out;
 	}
 
-	ndr_desc->nd_mapping = nd_mappings;
+	ndr_desc->mapping = mappings;
 	ndr_desc->num_mappings = count;
 	rc = acpi_nfit_init_interleave_set(acpi_desc, ndr_desc, spa);
 	if (rc)
@@ -2670,29 +2822,30 @@ static int acpi_nfit_remove(struct acpi_device *adev)
 	return 0;
 }
 
-static void acpi_nfit_notify(struct acpi_device *adev, u32 event)
+void __acpi_nfit_notify(struct device *dev, acpi_handle handle, u32 event)
 {
-	struct acpi_nfit_desc *acpi_desc = dev_get_drvdata(&adev->dev);
+	struct acpi_nfit_desc *acpi_desc = dev_get_drvdata(dev);
 	struct acpi_buffer buf = { ACPI_ALLOCATE_BUFFER, NULL };
-	struct device *dev = &adev->dev;
 	union acpi_object *obj;
 	acpi_status status;
 	int ret;
 
 	dev_dbg(dev, "%s: event: %d\n", __func__, event);
 
-	device_lock(dev);
+	if (event != NFIT_NOTIFY_UPDATE)
+		return;
+
 	if (!dev->driver) {
 		/* dev->driver may be null if we're being removed */
 		dev_dbg(dev, "%s: no driver found for dev\n", __func__);
-		goto out_unlock;
+		return;
 	}
 
 	if (!acpi_desc) {
 		acpi_desc = devm_kzalloc(dev, sizeof(*acpi_desc), GFP_KERNEL);
 		if (!acpi_desc)
-			goto out_unlock;
-		acpi_nfit_desc_init(acpi_desc, &adev->dev);
+			return;
+		acpi_nfit_desc_init(acpi_desc, dev);
 	} else {
 		/*
 		 * Finish previous registration before considering new
@@ -2702,10 +2855,10 @@ static void acpi_nfit_notify(struct acpi_device *adev, u32 event)
 	}
 
 	/* Evaluate _FIT */
-	status = acpi_evaluate_object(adev->handle, "_FIT", NULL, &buf);
+	status = acpi_evaluate_object(handle, "_FIT", NULL, &buf);
 	if (ACPI_FAILURE(status)) {
 		dev_err(dev, "failed to evaluate _FIT\n");
-		goto out_unlock;
+		return;
 	}
 
 	obj = buf.pointer;
@@ -2717,9 +2870,14 @@ static void acpi_nfit_notify(struct acpi_device *adev, u32 event)
 	} else
 		dev_err(dev, "Invalid _FIT\n");
 	kfree(buf.pointer);
+}
+EXPORT_SYMBOL_GPL(__acpi_nfit_notify);
 
- out_unlock:
-	device_unlock(dev);
+static void acpi_nfit_notify(struct acpi_device *adev, u32 event)
+{
+	device_lock(&adev->dev);
+	__acpi_nfit_notify(&adev->dev, adev->handle, event);
+	device_unlock(&adev->dev);
 }
 
 static const struct acpi_device_id acpi_nfit_ids[] = {
diff --git a/drivers/acpi/nfit/mce.c b/drivers/acpi/nfit/mce.c
index 161f91539ae6..e5ce81c38eed 100644
--- a/drivers/acpi/nfit/mce.c
+++ b/drivers/acpi/nfit/mce.c
@@ -14,6 +14,7 @@
  */
 #include <linux/notifier.h>
 #include <linux/acpi.h>
+#include <linux/nd.h>
 #include <asm/mce.h>
 #include "nfit.h"
 
@@ -62,12 +63,25 @@ static int nfit_handle_mce(struct notifier_block *nb, unsigned long val,
 		}
 		mutex_unlock(&acpi_desc->init_mutex);
 
-		/*
-		 * We can ignore an -EBUSY here because if an ARS is already
-		 * in progress, just let that be the last authoritative one
-		 */
-		if (found_match)
+		if (!found_match)
+			continue;
+
+		/* If this fails due to an -ENOMEM, there is little we can do */
+		nvdimm_bus_add_poison(acpi_desc->nvdimm_bus,
+				ALIGN(mce->addr, L1_CACHE_BYTES),
+				L1_CACHE_BYTES);
+		nvdimm_region_notify(nfit_spa->nd_region,
+				NVDIMM_REVALIDATE_POISON);
+
+		if (acpi_desc->scrub_mode == HW_ERROR_SCRUB_ON) {
+			/*
+			 * We can ignore an -EBUSY here because if an ARS is
+			 * already in progress, just let that be the last
+			 * authoritative one
+			 */
 			acpi_nfit_ars_rescan(acpi_desc);
+		}
+		break;
 	}
 
 	mutex_unlock(&acpi_desc_lock);
diff --git a/drivers/acpi/nfit/nfit.h b/drivers/acpi/nfit/nfit.h
index e894ded24d99..14296f5267c8 100644
--- a/drivers/acpi/nfit/nfit.h
+++ b/drivers/acpi/nfit/nfit.h
@@ -78,6 +78,14 @@ enum {
 	NFIT_ARS_TIMEOUT = 90,
 };
 
+enum nfit_root_notifiers {
+	NFIT_NOTIFY_UPDATE = 0x80,
+};
+
+enum nfit_dimm_notifiers {
+	NFIT_NOTIFY_DIMM_HEALTH = 0x81,
+};
+
 struct nfit_spa {
 	struct list_head list;
 	struct nd_region *nd_region;
@@ -124,6 +132,7 @@ struct nfit_mem {
 	struct acpi_nfit_system_address *spa_bdw;
 	struct acpi_nfit_interleave *idt_dcr;
 	struct acpi_nfit_interleave *idt_bdw;
+	struct kernfs_node *flags_attr;
 	struct nfit_flush *nfit_flush;
 	struct list_head list;
 	struct acpi_device *adev;
@@ -152,6 +161,7 @@ struct acpi_nfit_desc {
 	struct list_head list;
 	struct kernfs_node *scrub_count_state;
 	unsigned int scrub_count;
+	unsigned int scrub_mode;
 	unsigned int cancel:1;
 	unsigned long dimm_cmd_force_en;
 	unsigned long bus_cmd_force_en;
@@ -159,6 +169,11 @@ struct acpi_nfit_desc {
 			void *iobuf, u64 len, int rw);
 };
 
+enum scrub_mode {
+	HW_ERROR_SCRUB_OFF,
+	HW_ERROR_SCRUB_ON,
+};
+
 enum nd_blk_mmio_selector {
 	BDW,
 	DCR,
@@ -223,5 +238,7 @@ static inline struct acpi_nfit_desc *to_acpi_desc(
 
 const u8 *to_nfit_uuid(enum nfit_uuids id);
 int acpi_nfit_init(struct acpi_nfit_desc *acpi_desc, void *nfit, acpi_size sz);
+void __acpi_nfit_notify(struct device *dev, acpi_handle handle, u32 event);
+void __acpi_nvdimm_notify(struct device *dev, u32 event);
 void acpi_nfit_desc_init(struct acpi_nfit_desc *acpi_desc, struct device *dev);
 #endif /* __NFIT_H__ */
diff --git a/drivers/acpi/osl.c b/drivers/acpi/osl.c
index 4305ee9db4b2..416953a42510 100644
--- a/drivers/acpi/osl.c
+++ b/drivers/acpi/osl.c
@@ -162,11 +162,18 @@ void acpi_os_vprintf(const char *fmt, va_list args)
 	if (acpi_in_debugger) {
 		kdb_printf("%s", buffer);
 	} else {
-		printk(KERN_CONT "%s", buffer);
+		if (printk_get_level(buffer))
+			printk("%s", buffer);
+		else
+			printk(KERN_CONT "%s", buffer);
 	}
 #else
-	if (acpi_debugger_write_log(buffer) < 0)
-		printk(KERN_CONT "%s", buffer);
+	if (acpi_debugger_write_log(buffer) < 0) {
+		if (printk_get_level(buffer))
+			printk("%s", buffer);
+		else
+			printk(KERN_CONT "%s", buffer);
+	}
 #endif
 }
 
diff --git a/drivers/acpi/pci_irq.c b/drivers/acpi/pci_irq.c
index 2c45dd3acc17..c576a6fe4ebb 100644
--- a/drivers/acpi/pci_irq.c
+++ b/drivers/acpi/pci_irq.c
@@ -411,7 +411,15 @@ int acpi_pci_irq_enable(struct pci_dev *dev)
 	int gsi;
 	u8 pin;
 	int triggering = ACPI_LEVEL_SENSITIVE;
-	int polarity = ACPI_ACTIVE_LOW;
+	/*
+	 * On ARM systems with the GIC interrupt model, level interrupts
+	 * are always polarity high by specification; PCI legacy
+	 * IRQs lines are inverted before reaching the interrupt
+	 * controller and must therefore be considered active high
+	 * as default.
+	 */
+	int polarity = acpi_irq_model == ACPI_IRQ_MODEL_GIC ?
+				      ACPI_ACTIVE_HIGH : ACPI_ACTIVE_LOW;
 	char *link = NULL;
 	char link_desc[16];
 	int rc;
diff --git a/drivers/acpi/pci_root.c b/drivers/acpi/pci_root.c
index d144168d4ef9..bf601d4df8cf 100644
--- a/drivers/acpi/pci_root.c
+++ b/drivers/acpi/pci_root.c
@@ -614,7 +614,17 @@ static int acpi_pci_root_add(struct acpi_device *device,
 	if (hotadd) {
 		pcibios_resource_survey_bus(root->bus);
 		pci_assign_unassigned_root_bus_resources(root->bus);
-		acpi_ioapic_add(root);
+		/*
+		 * This is only called for the hotadd case. For the boot-time
+		 * case, we need to wait until after PCI initialization in
+		 * order to deal with IOAPICs mapped in on a PCI BAR.
+		 *
+		 * This is currently x86-specific, because acpi_ioapic_add()
+		 * is an empty function without CONFIG_ACPI_HOTPLUG_IOAPIC.
+		 * And CONFIG_ACPI_HOTPLUG_IOAPIC depends on CONFIG_X86_IO_APIC
+		 * (see drivers/acpi/Kconfig).
+		 */
+		acpi_ioapic_add(root->device->handle);
 	}
 
 	pci_lock_rescan_remove();
diff --git a/drivers/acpi/processor_core.c b/drivers/acpi/processor_core.c
index 9125d7d96372..5c78ee1860b0 100644
--- a/drivers/acpi/processor_core.c
+++ b/drivers/acpi/processor_core.c
@@ -32,12 +32,12 @@ static struct acpi_table_madt *get_madt_table(void)
 }
 
 static int map_lapic_id(struct acpi_subtable_header *entry,
-		 u32 acpi_id, phys_cpuid_t *apic_id)
+		 u32 acpi_id, phys_cpuid_t *apic_id, bool ignore_disabled)
 {
 	struct acpi_madt_local_apic *lapic =
 		container_of(entry, struct acpi_madt_local_apic, header);
 
-	if (!(lapic->lapic_flags & ACPI_MADT_ENABLED))
+	if (ignore_disabled && !(lapic->lapic_flags & ACPI_MADT_ENABLED))
 		return -ENODEV;
 
 	if (lapic->processor_id != acpi_id)
@@ -48,12 +48,13 @@ static int map_lapic_id(struct acpi_subtable_header *entry,
 }
 
 static int map_x2apic_id(struct acpi_subtable_header *entry,
-		int device_declaration, u32 acpi_id, phys_cpuid_t *apic_id)
+		int device_declaration, u32 acpi_id, phys_cpuid_t *apic_id,
+		bool ignore_disabled)
 {
 	struct acpi_madt_local_x2apic *apic =
 		container_of(entry, struct acpi_madt_local_x2apic, header);
 
-	if (!(apic->lapic_flags & ACPI_MADT_ENABLED))
+	if (ignore_disabled && !(apic->lapic_flags & ACPI_MADT_ENABLED))
 		return -ENODEV;
 
 	if (device_declaration && (apic->uid == acpi_id)) {
@@ -65,12 +66,13 @@ static int map_x2apic_id(struct acpi_subtable_header *entry,
 }
 
 static int map_lsapic_id(struct acpi_subtable_header *entry,
-		int device_declaration, u32 acpi_id, phys_cpuid_t *apic_id)
+		int device_declaration, u32 acpi_id, phys_cpuid_t *apic_id,
+		bool ignore_disabled)
 {
 	struct acpi_madt_local_sapic *lsapic =
 		container_of(entry, struct acpi_madt_local_sapic, header);
 
-	if (!(lsapic->lapic_flags & ACPI_MADT_ENABLED))
+	if (ignore_disabled && !(lsapic->lapic_flags & ACPI_MADT_ENABLED))
 		return -ENODEV;
 
 	if (device_declaration) {
@@ -87,12 +89,13 @@ static int map_lsapic_id(struct acpi_subtable_header *entry,
  * Retrieve the ARM CPU physical identifier (MPIDR)
  */
 static int map_gicc_mpidr(struct acpi_subtable_header *entry,
-		int device_declaration, u32 acpi_id, phys_cpuid_t *mpidr)
+		int device_declaration, u32 acpi_id, phys_cpuid_t *mpidr,
+		bool ignore_disabled)
 {
 	struct acpi_madt_generic_interrupt *gicc =
 	    container_of(entry, struct acpi_madt_generic_interrupt, header);
 
-	if (!(gicc->flags & ACPI_MADT_ENABLED))
+	if (ignore_disabled && !(gicc->flags & ACPI_MADT_ENABLED))
 		return -ENODEV;
 
 	/* device_declaration means Device object in DSDT, in the
@@ -109,7 +112,7 @@ static int map_gicc_mpidr(struct acpi_subtable_header *entry,
 }
 
 static phys_cpuid_t map_madt_entry(struct acpi_table_madt *madt,
-				   int type, u32 acpi_id)
+				   int type, u32 acpi_id, bool ignore_disabled)
 {
 	unsigned long madt_end, entry;
 	phys_cpuid_t phys_id = PHYS_CPUID_INVALID;	/* CPU hardware ID */
@@ -127,16 +130,20 @@ static phys_cpuid_t map_madt_entry(struct acpi_table_madt *madt,
 		struct acpi_subtable_header *header =
 			(struct acpi_subtable_header *)entry;
 		if (header->type == ACPI_MADT_TYPE_LOCAL_APIC) {
-			if (!map_lapic_id(header, acpi_id, &phys_id))
+			if (!map_lapic_id(header, acpi_id, &phys_id,
+					  ignore_disabled))
 				break;
 		} else if (header->type == ACPI_MADT_TYPE_LOCAL_X2APIC) {
-			if (!map_x2apic_id(header, type, acpi_id, &phys_id))
+			if (!map_x2apic_id(header, type, acpi_id, &phys_id,
+					   ignore_disabled))
 				break;
 		} else if (header->type == ACPI_MADT_TYPE_LOCAL_SAPIC) {
-			if (!map_lsapic_id(header, type, acpi_id, &phys_id))
+			if (!map_lsapic_id(header, type, acpi_id, &phys_id,
+					   ignore_disabled))
 				break;
 		} else if (header->type == ACPI_MADT_TYPE_GENERIC_INTERRUPT) {
-			if (!map_gicc_mpidr(header, type, acpi_id, &phys_id))
+			if (!map_gicc_mpidr(header, type, acpi_id, &phys_id,
+					    ignore_disabled))
 				break;
 		}
 		entry += header->length;
@@ -156,14 +163,15 @@ phys_cpuid_t __init acpi_map_madt_entry(u32 acpi_id)
 	if (!madt)
 		return PHYS_CPUID_INVALID;
 
-	rv = map_madt_entry(madt, 1, acpi_id);
+	rv = map_madt_entry(madt, 1, acpi_id, true);
 
 	early_acpi_os_unmap_memory(madt, tbl_size);
 
 	return rv;
 }
 
-static phys_cpuid_t map_mat_entry(acpi_handle handle, int type, u32 acpi_id)
+static phys_cpuid_t map_mat_entry(acpi_handle handle, int type, u32 acpi_id,
+				  bool ignore_disabled)
 {
 	struct acpi_buffer buffer = { ACPI_ALLOCATE_BUFFER, NULL };
 	union acpi_object *obj;
@@ -184,30 +192,38 @@ static phys_cpuid_t map_mat_entry(acpi_handle handle, int type, u32 acpi_id)
 
 	header = (struct acpi_subtable_header *)obj->buffer.pointer;
 	if (header->type == ACPI_MADT_TYPE_LOCAL_APIC)
-		map_lapic_id(header, acpi_id, &phys_id);
+		map_lapic_id(header, acpi_id, &phys_id, ignore_disabled);
 	else if (header->type == ACPI_MADT_TYPE_LOCAL_SAPIC)
-		map_lsapic_id(header, type, acpi_id, &phys_id);
+		map_lsapic_id(header, type, acpi_id, &phys_id, ignore_disabled);
 	else if (header->type == ACPI_MADT_TYPE_LOCAL_X2APIC)
-		map_x2apic_id(header, type, acpi_id, &phys_id);
+		map_x2apic_id(header, type, acpi_id, &phys_id, ignore_disabled);
 	else if (header->type == ACPI_MADT_TYPE_GENERIC_INTERRUPT)
-		map_gicc_mpidr(header, type, acpi_id, &phys_id);
+		map_gicc_mpidr(header, type, acpi_id, &phys_id,
+			       ignore_disabled);
 
 exit:
 	kfree(buffer.pointer);
 	return phys_id;
 }
 
-phys_cpuid_t acpi_get_phys_id(acpi_handle handle, int type, u32 acpi_id)
+static phys_cpuid_t __acpi_get_phys_id(acpi_handle handle, int type,
+				       u32 acpi_id, bool ignore_disabled)
 {
 	phys_cpuid_t phys_id;
 
-	phys_id = map_mat_entry(handle, type, acpi_id);
+	phys_id = map_mat_entry(handle, type, acpi_id, ignore_disabled);
 	if (invalid_phys_cpuid(phys_id))
-		phys_id = map_madt_entry(get_madt_table(), type, acpi_id);
+		phys_id = map_madt_entry(get_madt_table(), type, acpi_id,
+					   ignore_disabled);
 
 	return phys_id;
 }
 
+phys_cpuid_t acpi_get_phys_id(acpi_handle handle, int type, u32 acpi_id)
+{
+	return __acpi_get_phys_id(handle, type, acpi_id, true);
+}
+
 int acpi_map_cpuid(phys_cpuid_t phys_id, u32 acpi_id)
 {
 #ifdef CONFIG_SMP
@@ -264,6 +280,79 @@ int acpi_get_cpuid(acpi_handle handle, int type, u32 acpi_id)
 }
 EXPORT_SYMBOL_GPL(acpi_get_cpuid);
 
+#ifdef CONFIG_ACPI_HOTPLUG_CPU
+static bool __init
+map_processor(acpi_handle handle, phys_cpuid_t *phys_id, int *cpuid)
+{
+	int type, id;
+	u32 acpi_id;
+	acpi_status status;
+	acpi_object_type acpi_type;
+	unsigned long long tmp;
+	union acpi_object object = { 0 };
+	struct acpi_buffer buffer = { sizeof(union acpi_object), &object };
+
+	status = acpi_get_type(handle, &acpi_type);
+	if (ACPI_FAILURE(status))
+		return false;
+
+	switch (acpi_type) {
+	case ACPI_TYPE_PROCESSOR:
+		status = acpi_evaluate_object(handle, NULL, NULL, &buffer);
+		if (ACPI_FAILURE(status))
+			return false;
+		acpi_id = object.processor.proc_id;
+
+		/* validate the acpi_id */
+		if(acpi_processor_validate_proc_id(acpi_id))
+			return false;
+		break;
+	case ACPI_TYPE_DEVICE:
+		status = acpi_evaluate_integer(handle, "_UID", NULL, &tmp);
+		if (ACPI_FAILURE(status))
+			return false;
+		acpi_id = tmp;
+		break;
+	default:
+		return false;
+	}
+
+	type = (acpi_type == ACPI_TYPE_DEVICE) ? 1 : 0;
+
+	*phys_id = __acpi_get_phys_id(handle, type, acpi_id, false);
+	id = acpi_map_cpuid(*phys_id, acpi_id);
+
+	if (id < 0)
+		return false;
+	*cpuid = id;
+	return true;
+}
+
+static acpi_status __init
+set_processor_node_mapping(acpi_handle handle, u32 lvl, void *context,
+			   void **rv)
+{
+	phys_cpuid_t phys_id;
+	int cpu_id;
+
+	if (!map_processor(handle, &phys_id, &cpu_id))
+		return AE_ERROR;
+
+	acpi_map_cpu2node(handle, cpu_id, phys_id);
+	return AE_OK;
+}
+
+void __init acpi_set_processor_mapping(void)
+{
+	/* Set persistent cpu <-> node mapping for all processors. */
+	acpi_walk_namespace(ACPI_TYPE_PROCESSOR, ACPI_ROOT_OBJECT,
+			    ACPI_UINT32_MAX, set_processor_node_mapping,
+			    NULL, NULL, NULL);
+}
+#else
+void __init acpi_set_processor_mapping(void) {}
+#endif /* CONFIG_ACPI_HOTPLUG_CPU */
+
 #ifdef CONFIG_ACPI_HOTPLUG_IOAPIC
 static int get_ioapic_id(struct acpi_subtable_header *entry, u32 gsi_base,
 			 u64 *phys_addr, int *ioapic_id)
diff --git a/drivers/acpi/processor_driver.c b/drivers/acpi/processor_driver.c
index 0553aeebb228..9d5f0c7ed3f7 100644
--- a/drivers/acpi/processor_driver.c
+++ b/drivers/acpi/processor_driver.c
@@ -110,55 +110,46 @@ static void acpi_processor_notify(acpi_handle handle, u32 event, void *data)
 
 static int __acpi_processor_start(struct acpi_device *device);
 
-static int acpi_cpu_soft_notify(struct notifier_block *nfb,
-					  unsigned long action, void *hcpu)
+static int acpi_soft_cpu_online(unsigned int cpu)
 {
-	unsigned int cpu = (unsigned long)hcpu;
 	struct acpi_processor *pr = per_cpu(processors, cpu);
 	struct acpi_device *device;
-	action &= ~CPU_TASKS_FROZEN;
-
-	switch (action) {
-	case CPU_ONLINE:
-	case CPU_DEAD:
-		break;
-	default:
-		return NOTIFY_DONE;
-	}
 
 	if (!pr || acpi_bus_get_device(pr->handle, &device))
-		return NOTIFY_DONE;
-
-	if (action == CPU_ONLINE) {
-		/*
-		 * CPU got physically hotplugged and onlined for the first time:
-		 * Initialize missing things.
-		 */
-		if (pr->flags.need_hotplug_init) {
-			int ret;
-
-			pr_info("Will online and init hotplugged CPU: %d\n",
-				pr->id);
-			pr->flags.need_hotplug_init = 0;
-			ret = __acpi_processor_start(device);
-			WARN(ret, "Failed to start CPU: %d\n", pr->id);
-		} else {
-			/* Normal CPU soft online event. */
-			acpi_processor_ppc_has_changed(pr, 0);
-			acpi_processor_hotplug(pr);
-			acpi_processor_reevaluate_tstate(pr, action);
-			acpi_processor_tstate_has_changed(pr);
-		}
-	} else if (action == CPU_DEAD) {
-		/* Invalidate flag.throttling after the CPU is offline. */
-		acpi_processor_reevaluate_tstate(pr, action);
+		return 0;
+	/*
+	 * CPU got physically hotplugged and onlined for the first time:
+	 * Initialize missing things.
+	 */
+	if (pr->flags.need_hotplug_init) {
+		int ret;
+
+		pr_info("Will online and init hotplugged CPU: %d\n",
+			pr->id);
+		pr->flags.need_hotplug_init = 0;
+		ret = __acpi_processor_start(device);
+		WARN(ret, "Failed to start CPU: %d\n", pr->id);
+	} else {
+		/* Normal CPU soft online event. */
+		acpi_processor_ppc_has_changed(pr, 0);
+		acpi_processor_hotplug(pr);
+		acpi_processor_reevaluate_tstate(pr, false);
+		acpi_processor_tstate_has_changed(pr);
 	}
-	return NOTIFY_OK;
+	return 0;
 }
 
-static struct notifier_block acpi_cpu_notifier = {
-	    .notifier_call = acpi_cpu_soft_notify,
-};
+static int acpi_soft_cpu_dead(unsigned int cpu)
+{
+	struct acpi_processor *pr = per_cpu(processors, cpu);
+	struct acpi_device *device;
+
+	if (!pr || acpi_bus_get_device(pr->handle, &device))
+		return 0;
+
+	acpi_processor_reevaluate_tstate(pr, true);
+	return 0;
+}
 
 #ifdef CONFIG_ACPI_CPU_FREQ_PSS
 static int acpi_pss_perf_init(struct acpi_processor *pr,
@@ -245,8 +236,8 @@ static int __acpi_processor_start(struct acpi_device *device)
 		return 0;
 
 	result = acpi_cppc_processor_probe(pr);
-	if (result)
-		return -ENODEV;
+	if (result && !IS_ENABLED(CONFIG_ACPI_CPU_FREQ_PSS))
+		dev_warn(&device->dev, "CPPC data invalid or not present\n");
 
 	if (!cpuidle_get_driver() || cpuidle_get_driver() == &acpi_idle_driver)
 		acpi_processor_power_init(pr);
@@ -303,7 +294,7 @@ static int acpi_processor_stop(struct device *dev)
  * This is needed for the powernow-k8 driver, that works even without
  * ACPI, but needs symbols from this driver
  */
-
+static enum cpuhp_state hp_online;
 static int __init acpi_processor_driver_init(void)
 {
 	int result = 0;
@@ -315,11 +306,22 @@ static int __init acpi_processor_driver_init(void)
 	if (result < 0)
 		return result;
 
-	register_hotcpu_notifier(&acpi_cpu_notifier);
+	result = cpuhp_setup_state_nocalls(CPUHP_AP_ONLINE_DYN,
+					   "acpi/cpu-drv:online",
+					   acpi_soft_cpu_online, NULL);
+	if (result < 0)
+		goto err;
+	hp_online = result;
+	cpuhp_setup_state_nocalls(CPUHP_ACPI_CPUDRV_DEAD, "acpi/cpu-drv:dead",
+				  NULL, acpi_soft_cpu_dead);
+
 	acpi_thermal_cpufreq_init();
 	acpi_processor_ppc_init();
 	acpi_processor_throttling_init();
 	return 0;
+err:
+	driver_unregister(&acpi_processor_driver);
+	return result;
 }
 
 static void __exit acpi_processor_driver_exit(void)
@@ -329,7 +331,8 @@ static void __exit acpi_processor_driver_exit(void)
 
 	acpi_processor_ppc_exit();
 	acpi_thermal_cpufreq_exit();
-	unregister_hotcpu_notifier(&acpi_cpu_notifier);
+	cpuhp_remove_state_nocalls(hp_online);
+	cpuhp_remove_state_nocalls(CPUHP_ACPI_CPUDRV_DEAD);
 	driver_unregister(&acpi_processor_driver);
 }
 
diff --git a/drivers/acpi/processor_idle.c b/drivers/acpi/processor_idle.c
index cea52528aa18..2237d3f24f0e 100644
--- a/drivers/acpi/processor_idle.c
+++ b/drivers/acpi/processor_idle.c
@@ -31,6 +31,7 @@
 #include <linux/sched.h>       /* need_resched() */
 #include <linux/tick.h>
 #include <linux/cpuidle.h>
+#include <linux/cpu.h>
 #include <acpi/processor.h>
 
 /*
@@ -115,7 +116,7 @@ static const struct dmi_system_id processor_power_dmi_table[] = {
  * Callers should disable interrupts before the call and enable
  * interrupts after return.
  */
-static void acpi_safe_halt(void)
+static void __cpuidle acpi_safe_halt(void)
 {
 	if (!tif_need_resched()) {
 		safe_halt();
@@ -645,7 +646,7 @@ static int acpi_idle_bm_check(void)
  *
  * Caller disables interrupt before call and enables interrupt after return.
  */
-static void acpi_idle_do_entry(struct acpi_processor_cx *cx)
+static void __cpuidle acpi_idle_do_entry(struct acpi_processor_cx *cx)
 {
 	if (cx->entry_method == ACPI_CSTATE_FFH) {
 		/* Call into architectural FFH based C-state */
diff --git a/drivers/acpi/processor_throttling.c b/drivers/acpi/processor_throttling.c
index c72e64893d03..d51ca1c05619 100644
--- a/drivers/acpi/processor_throttling.c
+++ b/drivers/acpi/processor_throttling.c
@@ -375,11 +375,11 @@ int acpi_processor_tstate_has_changed(struct acpi_processor *pr)
  *	3. TSD domain
  */
 void acpi_processor_reevaluate_tstate(struct acpi_processor *pr,
-					unsigned long action)
+					bool is_dead)
 {
 	int result = 0;
 
-	if (action == CPU_DEAD) {
+	if (is_dead) {
 		/* When one CPU is offline, the T-state throttling
 		 * will be invalidated.
 		 */
diff --git a/drivers/acpi/property.c b/drivers/acpi/property.c
index f2fd3fee588a..03f5ec11ab31 100644
--- a/drivers/acpi/property.c
+++ b/drivers/acpi/property.c
@@ -468,10 +468,11 @@ static int acpi_data_get_property_array(struct acpi_device_data *data,
 }
 
 /**
- * acpi_data_get_property_reference - returns handle to the referenced object
- * @data: ACPI device data object containing the property
+ * __acpi_node_get_property_reference - returns handle to the referenced object
+ * @fwnode: Firmware node to get the property from
  * @propname: Name of the property
  * @index: Index of the reference to return
+ * @num_args: Maximum number of arguments after each reference
  * @args: Location to store the returned reference with optional arguments
  *
  * Find property with @name, verifify that it is a package containing at least
@@ -482,17 +483,40 @@ static int acpi_data_get_property_array(struct acpi_device_data *data,
  * If there's more than one reference in the property value package, @index is
  * used to select the one to return.
  *
+ * It is possible to leave holes in the property value set like in the
+ * example below:
+ *
+ * Package () {
+ *     "cs-gpios",
+ *     Package () {
+ *        ^GPIO, 19, 0, 0,
+ *        ^GPIO, 20, 0, 0,
+ *        0,
+ *        ^GPIO, 21, 0, 0,
+ *     }
+ * }
+ *
+ * Calling this function with index %2 return %-ENOENT and with index %3
+ * returns the last entry. If the property does not contain any more values
+ * %-ENODATA is returned. The NULL entry must be single integer and
+ * preferably contain value %0.
+ *
  * Return: %0 on success, negative error code on failure.
  */
-static int acpi_data_get_property_reference(struct acpi_device_data *data,
-					    const char *propname, size_t index,
-					    struct acpi_reference_args *args)
+int __acpi_node_get_property_reference(struct fwnode_handle *fwnode,
+	const char *propname, size_t index, size_t num_args,
+	struct acpi_reference_args *args)
 {
 	const union acpi_object *element, *end;
 	const union acpi_object *obj;
+	struct acpi_device_data *data;
 	struct acpi_device *device;
 	int ret, idx = 0;
 
+	data = acpi_device_data_of_node(fwnode);
+	if (!data)
+		return -EINVAL;
+
 	ret = acpi_data_get_property(data, propname, ACPI_TYPE_ANY, &obj);
 	if (ret)
 		return ret;
@@ -532,59 +556,54 @@ static int acpi_data_get_property_reference(struct acpi_device_data *data,
 	while (element < end) {
 		u32 nargs, i;
 
-		if (element->type != ACPI_TYPE_LOCAL_REFERENCE)
-			return -EPROTO;
-
-		ret = acpi_bus_get_device(element->reference.handle, &device);
-		if (ret)
-			return -ENODEV;
-
-		element++;
-		nargs = 0;
-
-		/* assume following integer elements are all args */
-		for (i = 0; element + i < end; i++) {
-			int type = element[i].type;
+		if (element->type == ACPI_TYPE_LOCAL_REFERENCE) {
+			ret = acpi_bus_get_device(element->reference.handle,
+						  &device);
+			if (ret)
+				return -ENODEV;
+
+			nargs = 0;
+			element++;
+
+			/* assume following integer elements are all args */
+			for (i = 0; element + i < end && i < num_args; i++) {
+				int type = element[i].type;
+
+				if (type == ACPI_TYPE_INTEGER)
+					nargs++;
+				else if (type == ACPI_TYPE_LOCAL_REFERENCE)
+					break;
+				else
+					return -EPROTO;
+			}
 
-			if (type == ACPI_TYPE_INTEGER)
-				nargs++;
-			else if (type == ACPI_TYPE_LOCAL_REFERENCE)
-				break;
-			else
+			if (nargs > MAX_ACPI_REFERENCE_ARGS)
 				return -EPROTO;
-		}
 
-		if (idx++ == index) {
-			args->adev = device;
-			args->nargs = nargs;
-			for (i = 0; i < nargs; i++)
-				args->args[i] = element[i].integer.value;
+			if (idx == index) {
+				args->adev = device;
+				args->nargs = nargs;
+				for (i = 0; i < nargs; i++)
+					args->args[i] = element[i].integer.value;
 
-			return 0;
+				return 0;
+			}
+
+			element += nargs;
+		} else if (element->type == ACPI_TYPE_INTEGER) {
+			if (idx == index)
+				return -ENOENT;
+			element++;
+		} else {
+			return -EPROTO;
 		}
 
-		element += nargs;
+		idx++;
 	}
 
-	return -EPROTO;
-}
-
-/**
- * acpi_node_get_property_reference - get a handle to the referenced object.
- * @fwnode: Firmware node to get the property from.
- * @propname: Name of the property.
- * @index: Index of the reference to return.
- * @args: Location to store the returned reference with optional arguments.
- */
-int acpi_node_get_property_reference(struct fwnode_handle *fwnode,
-				     const char *name, size_t index,
-				     struct acpi_reference_args *args)
-{
-	struct acpi_device_data *data = acpi_device_data_of_node(fwnode);
-
-	return data ? acpi_data_get_property_reference(data, name, index, args) : -EINVAL;
+	return -ENODATA;
 }
-EXPORT_SYMBOL_GPL(acpi_node_get_property_reference);
+EXPORT_SYMBOL_GPL(__acpi_node_get_property_reference);
 
 static int acpi_data_prop_read_single(struct acpi_device_data *data,
 				      const char *propname,
diff --git a/drivers/acpi/scan.c b/drivers/acpi/scan.c
index e878fc799af7..035ac646d8db 100644
--- a/drivers/acpi/scan.c
+++ b/drivers/acpi/scan.c
@@ -2002,6 +2002,7 @@ int __init acpi_scan_init(void)
 	acpi_pnp_init();
 	acpi_int340x_thermal_init();
 	acpi_amba_init();
+	acpi_watchdog_init();
 
 	acpi_scan_add_handler(&generic_device_handler);
 
@@ -2044,6 +2045,7 @@ int __init acpi_scan_init(void)
 	}
 
 	acpi_update_all_gpes();
+	acpi_ec_ecdt_start();
 
 	acpi_scan_initialized = true;
 
diff --git a/drivers/acpi/sleep.c b/drivers/acpi/sleep.c
index 2b38c1bb0446..deb0ff78eba8 100644
--- a/drivers/acpi/sleep.c
+++ b/drivers/acpi/sleep.c
@@ -572,7 +572,7 @@ static int acpi_suspend_enter(suspend_state_t pm_state)
 
 		acpi_get_event_status(ACPI_EVENT_POWER_BUTTON, &pwr_btn_status);
 
-		if (pwr_btn_status & ACPI_EVENT_FLAG_SET) {
+		if (pwr_btn_status & ACPI_EVENT_FLAG_STATUS_SET) {
 			acpi_clear_event(ACPI_EVENT_POWER_BUTTON);
 			/* Flag for later */
 			pwr_btn_event_pending = true;
@@ -586,7 +586,7 @@ static int acpi_suspend_enter(suspend_state_t pm_state)
 	 */
 	acpi_disable_all_gpes();
 	/* Allow EC transactions to happen. */
-	acpi_ec_unblock_transactions_early();
+	acpi_ec_unblock_transactions();
 
 	suspend_nvs_restore();
 
@@ -784,7 +784,7 @@ static void acpi_hibernation_leave(void)
 	/* Restore the NVS memory area */
 	suspend_nvs_restore();
 	/* Allow EC transactions to happen. */
-	acpi_ec_unblock_transactions_early();
+	acpi_ec_unblock_transactions();
 }
 
 static void acpi_pm_thaw(void)
diff --git a/drivers/acpi/spcr.c b/drivers/acpi/spcr.c
new file mode 100644
index 000000000000..e8d7bc7d4da8
--- /dev/null
+++ b/drivers/acpi/spcr.c
@@ -0,0 +1,111 @@
+/*
+ * Copyright (c) 2012, Intel Corporation
+ * Copyright (c) 2015, Red Hat, Inc.
+ * Copyright (c) 2015, 2016 Linaro Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ */
+
+#define pr_fmt(fmt) "ACPI: SPCR: " fmt
+
+#include <linux/acpi.h>
+#include <linux/console.h>
+#include <linux/kernel.h>
+#include <linux/serial_core.h>
+
+/**
+ * parse_spcr() - parse ACPI SPCR table and add preferred console
+ *
+ * @earlycon: set up earlycon for the console specified by the table
+ *
+ * For the architectures with support for ACPI, CONFIG_ACPI_SPCR_TABLE may be
+ * defined to parse ACPI SPCR table.  As a result of the parsing preferred
+ * console is registered and if @earlycon is true, earlycon is set up.
+ *
+ * When CONFIG_ACPI_SPCR_TABLE is defined, this function should be called
+ * from arch inintialization code as soon as the DT/ACPI decision is made.
+ *
+ */
+int __init parse_spcr(bool earlycon)
+{
+	static char opts[64];
+	struct acpi_table_spcr *table;
+	acpi_size table_size;
+	acpi_status status;
+	char *uart;
+	char *iotype;
+	int baud_rate;
+	int err;
+
+	if (acpi_disabled)
+		return -ENODEV;
+
+	status = acpi_get_table_with_size(ACPI_SIG_SPCR, 0,
+					  (struct acpi_table_header **)&table,
+					  &table_size);
+
+	if (ACPI_FAILURE(status))
+		return -ENOENT;
+
+	if (table->header.revision < 2) {
+		err = -ENOENT;
+		pr_err("wrong table version\n");
+		goto done;
+	}
+
+	iotype = table->serial_port.space_id == ACPI_ADR_SPACE_SYSTEM_MEMORY ?
+			"mmio" : "io";
+
+	switch (table->interface_type) {
+	case ACPI_DBG2_ARM_SBSA_32BIT:
+		iotype = "mmio32";
+		/* fall through */
+	case ACPI_DBG2_ARM_PL011:
+	case ACPI_DBG2_ARM_SBSA_GENERIC:
+	case ACPI_DBG2_BCM2835:
+		uart = "pl011";
+		break;
+	case ACPI_DBG2_16550_COMPATIBLE:
+	case ACPI_DBG2_16550_SUBSET:
+		uart = "uart";
+		break;
+	default:
+		err = -ENOENT;
+		goto done;
+	}
+
+	switch (table->baud_rate) {
+	case 3:
+		baud_rate = 9600;
+		break;
+	case 4:
+		baud_rate = 19200;
+		break;
+	case 6:
+		baud_rate = 57600;
+		break;
+	case 7:
+		baud_rate = 115200;
+		break;
+	default:
+		err = -ENOENT;
+		goto done;
+	}
+
+	snprintf(opts, sizeof(opts), "%s,%s,0x%llx,%d", uart, iotype,
+		 table->serial_port.address, baud_rate);
+
+	pr_info("console: %s\n", opts);
+
+	if (earlycon)
+		setup_earlycon(opts);
+
+	err = add_preferred_console(uart, 0, opts + strlen(uart) + 1);
+
+done:
+	early_acpi_os_unmap_memory((void __iomem *)table, table_size);
+	return err;
+}
diff --git a/drivers/acpi/sysfs.c b/drivers/acpi/sysfs.c
index 358165e9f5b8..703c26e7022c 100644
--- a/drivers/acpi/sysfs.c
+++ b/drivers/acpi/sysfs.c
@@ -314,10 +314,14 @@ static struct kobject *tables_kobj;
 static struct kobject *dynamic_tables_kobj;
 static struct kobject *hotplug_kobj;
 
+#define ACPI_MAX_TABLE_INSTANCES	999
+#define ACPI_INST_SIZE			4 /* including trailing 0 */
+
 struct acpi_table_attr {
 	struct bin_attribute attr;
-	char name[8];
+	char name[ACPI_NAME_SIZE];
 	int instance;
+	char filename[ACPI_NAME_SIZE+ACPI_INST_SIZE];
 	struct list_head node;
 };
 
@@ -329,14 +333,9 @@ static ssize_t acpi_table_show(struct file *filp, struct kobject *kobj,
 	    container_of(bin_attr, struct acpi_table_attr, attr);
 	struct acpi_table_header *table_header = NULL;
 	acpi_status status;
-	char name[ACPI_NAME_SIZE];
-
-	if (strncmp(table_attr->name, "NULL", 4))
-		memcpy(name, table_attr->name, ACPI_NAME_SIZE);
-	else
-		memcpy(name, "\0\0\0\0", 4);
 
-	status = acpi_get_table(name, table_attr->instance, &table_header);
+	status = acpi_get_table(table_attr->name, table_attr->instance,
+				&table_header);
 	if (ACPI_FAILURE(status))
 		return -ENODEV;
 
@@ -344,38 +343,45 @@ static ssize_t acpi_table_show(struct file *filp, struct kobject *kobj,
 				       table_header, table_header->length);
 }
 
-static void acpi_table_attr_init(struct acpi_table_attr *table_attr,
-				 struct acpi_table_header *table_header)
+static int acpi_table_attr_init(struct kobject *tables_obj,
+				struct acpi_table_attr *table_attr,
+				struct acpi_table_header *table_header)
 {
 	struct acpi_table_header *header = NULL;
 	struct acpi_table_attr *attr = NULL;
+	char instance_str[ACPI_INST_SIZE];
 
 	sysfs_attr_init(&table_attr->attr.attr);
-	if (table_header->signature[0] != '\0')
-		memcpy(table_attr->name, table_header->signature,
-		       ACPI_NAME_SIZE);
-	else
-		memcpy(table_attr->name, "NULL", 4);
+	ACPI_MOVE_NAME(table_attr->name, table_header->signature);
 
 	list_for_each_entry(attr, &acpi_table_attr_list, node) {
-		if (!memcmp(table_attr->name, attr->name, ACPI_NAME_SIZE))
+		if (ACPI_COMPARE_NAME(table_attr->name, attr->name))
 			if (table_attr->instance < attr->instance)
 				table_attr->instance = attr->instance;
 	}
 	table_attr->instance++;
+	if (table_attr->instance > ACPI_MAX_TABLE_INSTANCES) {
+		pr_warn("%4.4s: too many table instances\n",
+			table_attr->name);
+		return -ERANGE;
+	}
 
+	ACPI_MOVE_NAME(table_attr->filename, table_header->signature);
+	table_attr->filename[ACPI_NAME_SIZE] = '\0';
 	if (table_attr->instance > 1 || (table_attr->instance == 1 &&
 					 !acpi_get_table
-					 (table_header->signature, 2, &header)))
-		sprintf(table_attr->name + ACPI_NAME_SIZE, "%d",
-			table_attr->instance);
+					 (table_header->signature, 2, &header))) {
+		snprintf(instance_str, sizeof(instance_str), "%u",
+			 table_attr->instance);
+		strcat(table_attr->filename, instance_str);
+	}
 
 	table_attr->attr.size = table_header->length;
 	table_attr->attr.read = acpi_table_show;
-	table_attr->attr.attr.name = table_attr->name;
+	table_attr->attr.attr.name = table_attr->filename;
 	table_attr->attr.attr.mode = 0400;
 
-	return;
+	return sysfs_create_bin_file(tables_obj, &table_attr->attr);
 }
 
 acpi_status acpi_sysfs_table_handler(u32 event, void *table, void *context)
@@ -383,21 +389,22 @@ acpi_status acpi_sysfs_table_handler(u32 event, void *table, void *context)
 	struct acpi_table_attr *table_attr;
 
 	switch (event) {
-	case ACPI_TABLE_EVENT_LOAD:
+	case ACPI_TABLE_EVENT_INSTALL:
 		table_attr =
 		    kzalloc(sizeof(struct acpi_table_attr), GFP_KERNEL);
 		if (!table_attr)
 			return AE_NO_MEMORY;
 
-		acpi_table_attr_init(table_attr, table);
-		if (sysfs_create_bin_file(dynamic_tables_kobj,
-					  &table_attr->attr)) {
+		if (acpi_table_attr_init(dynamic_tables_kobj,
+					 table_attr, table)) {
 			kfree(table_attr);
 			return AE_ERROR;
-		} else
-			list_add_tail(&table_attr->node, &acpi_table_attr_list);
+		}
+		list_add_tail(&table_attr->node, &acpi_table_attr_list);
 		break;
+	case ACPI_TABLE_EVENT_LOAD:
 	case ACPI_TABLE_EVENT_UNLOAD:
+	case ACPI_TABLE_EVENT_UNINSTALL:
 		/*
 		 * we do not need to do anything right now
 		 * because the table is not deleted from the
@@ -435,13 +442,12 @@ static int acpi_tables_sysfs_init(void)
 		if (ACPI_FAILURE(status))
 			continue;
 
-		table_attr = NULL;
 		table_attr = kzalloc(sizeof(*table_attr), GFP_KERNEL);
 		if (!table_attr)
 			return -ENOMEM;
 
-		acpi_table_attr_init(table_attr, table_header);
-		ret = sysfs_create_bin_file(tables_kobj, &table_attr->attr);
+		ret = acpi_table_attr_init(tables_kobj,
+					   table_attr, table_header);
 		if (ret) {
 			kfree(table_attr);
 			return ret;
@@ -597,14 +603,27 @@ static ssize_t counter_show(struct kobject *kobj,
 	if (result)
 		goto end;
 
+	if (status & ACPI_EVENT_FLAG_ENABLE_SET)
+		size += sprintf(buf + size, "  EN");
+	else
+		size += sprintf(buf + size, "    ");
+	if (status & ACPI_EVENT_FLAG_STATUS_SET)
+		size += sprintf(buf + size, " STS");
+	else
+		size += sprintf(buf + size, "    ");
+
 	if (!(status & ACPI_EVENT_FLAG_HAS_HANDLER))
-		size += sprintf(buf + size, "   invalid");
+		size += sprintf(buf + size, " invalid     ");
 	else if (status & ACPI_EVENT_FLAG_ENABLED)
-		size += sprintf(buf + size, "   enabled");
+		size += sprintf(buf + size, " enabled     ");
 	else if (status & ACPI_EVENT_FLAG_WAKE_ENABLED)
-		size += sprintf(buf + size, "   wake_enabled");
+		size += sprintf(buf + size, " wake_enabled");
+	else
+		size += sprintf(buf + size, " disabled    ");
+	if (status & ACPI_EVENT_FLAG_MASKED)
+		size += sprintf(buf + size, " masked  ");
 	else
-		size += sprintf(buf + size, "   disabled");
+		size += sprintf(buf + size, " unmasked");
 
 end:
 	size += sprintf(buf + size, "\n");
@@ -655,8 +674,12 @@ static ssize_t counter_set(struct kobject *kobj,
 			 !(status & ACPI_EVENT_FLAG_ENABLED))
 			result = acpi_enable_gpe(handle, index);
 		else if (!strcmp(buf, "clear\n") &&
-			 (status & ACPI_EVENT_FLAG_SET))
+			 (status & ACPI_EVENT_FLAG_STATUS_SET))
 			result = acpi_clear_gpe(handle, index);
+		else if (!strcmp(buf, "mask\n"))
+			result = acpi_mask_gpe(handle, index, TRUE);
+		else if (!strcmp(buf, "unmask\n"))
+			result = acpi_mask_gpe(handle, index, FALSE);
 		else if (!kstrtoul(buf, 0, &tmp))
 			all_counters[index].count = tmp;
 		else
@@ -664,13 +687,13 @@ static ssize_t counter_set(struct kobject *kobj,
 	} else if (index < num_gpes + ACPI_NUM_FIXED_EVENTS) {
 		int event = index - num_gpes;
 		if (!strcmp(buf, "disable\n") &&
-		    (status & ACPI_EVENT_FLAG_ENABLED))
+		    (status & ACPI_EVENT_FLAG_ENABLE_SET))
 			result = acpi_disable_event(event, ACPI_NOT_ISR);
 		else if (!strcmp(buf, "enable\n") &&
-			 !(status & ACPI_EVENT_FLAG_ENABLED))
+			 !(status & ACPI_EVENT_FLAG_ENABLE_SET))
 			result = acpi_enable_event(event, ACPI_NOT_ISR);
 		else if (!strcmp(buf, "clear\n") &&
-			 (status & ACPI_EVENT_FLAG_SET))
+			 (status & ACPI_EVENT_FLAG_STATUS_SET))
 			result = acpi_clear_event(event);
 		else if (!kstrtoul(buf, 0, &tmp))
 			all_counters[index].count = tmp;
diff --git a/drivers/acpi/tables.c b/drivers/acpi/tables.c
index 9f0ad6ebb368..cdd56c4657e0 100644
--- a/drivers/acpi/tables.c
+++ b/drivers/acpi/tables.c
@@ -35,7 +35,6 @@
 #include <linux/earlycpio.h>
 #include <linux/memblock.h>
 #include <linux/initrd.h>
-#include <linux/acpi.h>
 #include "internal.h"
 
 #ifdef CONFIG_ACPI_CUSTOM_DSDT
@@ -246,6 +245,7 @@ acpi_parse_entries_array(char *id, unsigned long table_size,
 	struct acpi_subtable_header *entry;
 	unsigned long table_end;
 	int count = 0;
+	int errs = 0;
 	int i;
 
 	if (acpi_disabled)
@@ -278,10 +278,12 @@ acpi_parse_entries_array(char *id, unsigned long table_size,
 			if (entry->type != proc[i].id)
 				continue;
 			if (!proc[i].handler ||
-			     proc[i].handler(entry, table_end))
-				return -EINVAL;
+			     (!errs && proc[i].handler(entry, table_end))) {
+				errs++;
+				continue;
+			}
 
-			proc->count++;
+			proc[i].count++;
 			break;
 		}
 		if (i != proc_num)
@@ -301,11 +303,11 @@ acpi_parse_entries_array(char *id, unsigned long table_size,
 	}
 
 	if (max_entries && count > max_entries) {
-		pr_warn("[%4.4s:0x%02x] ignored %i entries of %i found\n",
-			id, proc->id, count - max_entries, count);
+		pr_warn("[%4.4s:0x%02x] found the maximum %i entries\n",
+			id, proc->id, count);
 	}
 
-	return count;
+	return errs ? -EINVAL : count;
 }
 
 int __init
diff --git a/drivers/acpi/thermal.c b/drivers/acpi/thermal.c
index f4ebe39539af..35e8fbca10ad 100644
--- a/drivers/acpi/thermal.c
+++ b/drivers/acpi/thermal.c
@@ -520,7 +520,8 @@ static void acpi_thermal_check(void *data)
 	if (!tz->tz_enabled)
 		return;
 
-	thermal_zone_device_update(tz->thermal_zone);
+	thermal_zone_device_update(tz->thermal_zone,
+				   THERMAL_EVENT_UNSPECIFIED);
 }
 
 /* sys I/F for generic thermal sysfs support */
diff --git a/drivers/amba/bus.c b/drivers/amba/bus.c
index a5b5c87e2114..a56fa2a1e9aa 100644
--- a/drivers/amba/bus.c
+++ b/drivers/amba/bus.c
@@ -19,6 +19,7 @@
 #include <linux/amba/bus.h>
 #include <linux/sizes.h>
 #include <linux/limits.h>
+#include <linux/clk/clk-conf.h>
 
 #include <asm/irq.h>
 
@@ -237,6 +238,10 @@ static int amba_probe(struct device *dev)
 	int ret;
 
 	do {
+		ret = of_clk_set_defaults(dev->of_node, false);
+		if (ret < 0)
+			break;
+
 		ret = dev_pm_domain_attach(dev, true);
 		if (ret == -EPROBE_DEFER)
 			break;
diff --git a/drivers/android/binder.c b/drivers/android/binder.c
index 16288e777ec3..562af94bec35 100644
--- a/drivers/android/binder.c
+++ b/drivers/android/binder.c
@@ -59,7 +59,6 @@ static struct dentry *binder_debugfs_dir_entry_proc;
 static struct binder_node *binder_context_mgr_node;
 static kuid_t binder_context_mgr_uid = INVALID_UID;
 static int binder_last_id;
-static struct workqueue_struct *binder_deferred_workqueue;
 
 #define BINDER_DEBUG_ENTRY(name) \
 static int binder_##name##_open(struct inode *inode, struct file *file) \
@@ -3227,7 +3226,7 @@ binder_defer_work(struct binder_proc *proc, enum binder_deferred_state defer)
 	if (hlist_unhashed(&proc->deferred_work_node)) {
 		hlist_add_head(&proc->deferred_work_node,
 				&binder_deferred_list);
-		queue_work(binder_deferred_workqueue, &binder_deferred_work);
+		schedule_work(&binder_deferred_work);
 	}
 	mutex_unlock(&binder_deferred_lock);
 }
@@ -3679,10 +3678,6 @@ static int __init binder_init(void)
 {
 	int ret;
 
-	binder_deferred_workqueue = create_singlethread_workqueue("binder");
-	if (!binder_deferred_workqueue)
-		return -ENOMEM;
-
 	binder_debugfs_dir_entry_root = debugfs_create_dir("binder", NULL);
 	if (binder_debugfs_dir_entry_root)
 		binder_debugfs_dir_entry_proc = debugfs_create_dir("proc",
diff --git a/drivers/ata/ahci.c b/drivers/ata/ahci.c
index 90eabaf81215..ba5f11cebee2 100644
--- a/drivers/ata/ahci.c
+++ b/drivers/ata/ahci.c
@@ -1400,142 +1400,56 @@ static irqreturn_t ahci_thunderx_irq_handler(int irq, void *dev_instance)
 }
 #endif
 
-/*
- * ahci_init_msix() - optionally enable per-port MSI-X otherwise defer
- * to single msi.
- */
-static int ahci_init_msix(struct pci_dev *pdev, unsigned int n_ports,
-			  struct ahci_host_priv *hpriv, unsigned long flags)
+static int ahci_get_irq_vector(struct ata_host *host, int port)
 {
-	int nvec, i, rc;
-
-	/* Do not init MSI-X if MSI is disabled for the device */
-	if (hpriv->flags & AHCI_HFLAG_NO_MSI)
-		return -ENODEV;
-
-	nvec = pci_msix_vec_count(pdev);
-	if (nvec < 0)
-		return nvec;
-
-	/*
-	 * Proper MSI-X implementations will have a vector per-port.
-	 * Barring that, we prefer single-MSI over single-MSIX.  If this
-	 * check fails (not enough MSI-X vectors for all ports) we will
-	 * be called again with the flag clear iff ahci_init_msi()
-	 * fails.
-	 */
-	if (flags & AHCI_HFLAG_MULTI_MSIX) {
-		if (nvec < n_ports)
-			return -ENODEV;
-		nvec = n_ports;
-	} else if (nvec) {
-		nvec = 1;
-	} else {
-		/*
-		 * Emit dev_err() since this was the non-legacy irq
-		 * method of last resort.
-		 */
-		rc = -ENODEV;
-		goto fail;
-	}
-
-	for (i = 0; i < nvec; i++)
-		hpriv->msix[i].entry = i;
-	rc = pci_enable_msix_exact(pdev, hpriv->msix, nvec);
-	if (rc < 0)
-		goto fail;
-
-	if (nvec > 1)
-		hpriv->flags |= AHCI_HFLAG_MULTI_MSIX;
-	hpriv->irq = hpriv->msix[0].vector; /* for single msi-x */
-
-	return nvec;
-fail:
-	dev_err(&pdev->dev,
-		"failed to enable MSI-X with error %d, # of vectors: %d\n",
-		rc, nvec);
-
-	return rc;
+	return pci_irq_vector(to_pci_dev(host->dev), port);
 }
 
 static int ahci_init_msi(struct pci_dev *pdev, unsigned int n_ports,
 			struct ahci_host_priv *hpriv)
 {
-	int rc, nvec;
+	int nvec;
 
 	if (hpriv->flags & AHCI_HFLAG_NO_MSI)
 		return -ENODEV;
 
-	nvec = pci_msi_vec_count(pdev);
-	if (nvec < 0)
-		return nvec;
-
 	/*
 	 * If number of MSIs is less than number of ports then Sharing Last
 	 * Message mode could be enforced. In this case assume that advantage
 	 * of multipe MSIs is negated and use single MSI mode instead.
 	 */
-	if (nvec < n_ports)
-		goto single_msi;
-
-	rc = pci_enable_msi_exact(pdev, nvec);
-	if (rc == -ENOSPC)
-		goto single_msi;
-	if (rc < 0)
-		return rc;
+	nvec = pci_alloc_irq_vectors(pdev, n_ports, INT_MAX,
+			PCI_IRQ_MSIX | PCI_IRQ_MSI);
+	if (nvec > 0) {
+		if (!(readl(hpriv->mmio + HOST_CTL) & HOST_MRSM)) {
+			hpriv->get_irq_vector = ahci_get_irq_vector;
+			hpriv->flags |= AHCI_HFLAG_MULTI_MSI;
+			return nvec;
+		}
 
-	/* fallback to single MSI mode if the controller enforced MRSM mode */
-	if (readl(hpriv->mmio + HOST_CTL) & HOST_MRSM) {
-		pci_disable_msi(pdev);
+		/*
+		 * Fallback to single MSI mode if the controller enforced MRSM
+		 * mode.
+		 */
 		printk(KERN_INFO "ahci: MRSM is on, fallback to single MSI\n");
-		goto single_msi;
+		pci_free_irq_vectors(pdev);
 	}
 
-	if (nvec > 1)
-		hpriv->flags |= AHCI_HFLAG_MULTI_MSI;
-
-	goto out;
-
-single_msi:
-	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;
-
 	/*
-	 * Try to enable per-port MSI-X.  If the host is not capable
-	 * fall back to single MSI before finally attempting single
-	 * MSI-X.
+	 * -ENOSPC indicated we don't have enough vectors.  Don't bother trying
+	 * a single vectors for any other error:
 	 */
-	nvec = ahci_init_msix(pdev, n_ports, hpriv, AHCI_HFLAG_MULTI_MSIX);
-	if (nvec >= 0)
+	if (nvec < 0 && nvec != -ENOSPC)
 		return nvec;
 
-	nvec = ahci_init_msi(pdev, n_ports, hpriv);
-	if (nvec >= 0)
-		return nvec;
-
-	/* try single-msix */
-	nvec = ahci_init_msix(pdev, n_ports, hpriv, 0);
-	if (nvec >= 0)
+	/*
+	 * If the host is not capable of supporting per-port vectors, fall
+	 * back to single MSI before finally attempting single MSI-X.
+	 */
+	nvec = pci_alloc_irq_vectors(pdev, 1, 1, PCI_IRQ_MSI);
+	if (nvec == 1)
 		return nvec;
-
-	/* legacy intx interrupts */
-	pci_intx(pdev, 1);
-	hpriv->irq = pdev->irq;
-
-	return 0;
+	return pci_alloc_irq_vectors(pdev, 1, 1, PCI_IRQ_MSIX);
 }
 
 static int ahci_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
@@ -1698,11 +1612,12 @@ static int ahci_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	if (!host)
 		return -ENOMEM;
 	host->private_data = hpriv;
-	hpriv->msix = devm_kzalloc(&pdev->dev,
-			sizeof(struct msix_entry) * n_ports, GFP_KERNEL);
-	if (!hpriv->msix)
-		return -ENOMEM;
-	ahci_init_interrupts(pdev, n_ports, hpriv);
+
+	if (ahci_init_msi(pdev, n_ports, hpriv) < 0) {
+		/* legacy intx interrupts */
+		pci_intx(pdev, 1);
+	}
+	hpriv->irq = pdev->irq;
 
 	if (!(hpriv->cap & HOST_CAP_SSS) || ahci_ignore_sss)
 		host->flags |= ATA_HOST_PARALLEL_SCAN;
diff --git a/drivers/ata/ahci.h b/drivers/ata/ahci.h
index 70b06bcfb7e3..0cc08f892fea 100644
--- a/drivers/ata/ahci.h
+++ b/drivers/ata/ahci.h
@@ -242,12 +242,10 @@ enum {
 	AHCI_HFLAG_NO_FBS		= (1 << 18), /* no FBS */
 
 #ifdef CONFIG_PCI_MSI
-	AHCI_HFLAG_MULTI_MSI		= (1 << 20), /* multiple PCI MSIs */
-	AHCI_HFLAG_MULTI_MSIX		= (1 << 21), /* per-port MSI-X */
+	AHCI_HFLAG_MULTI_MSI		= (1 << 20), /* per-port MSI(-X) */
 #else
 	/* compile out MSI infrastructure */
 	AHCI_HFLAG_MULTI_MSI		= 0,
-	AHCI_HFLAG_MULTI_MSIX		= 0,
 #endif
 	AHCI_HFLAG_WAKE_BEFORE_STOP	= (1 << 22), /* wake before DMA stop */
 
@@ -351,7 +349,6 @@ struct ahci_host_priv {
 	 * the PHY position in this array.
 	 */
 	struct phy		**phys;
-	struct msix_entry	*msix;		/* Optional MSI-X support */
 	unsigned		nports;		/* Number of ports */
 	void			*plat_data;	/* Other platform data */
 	unsigned int		irq;		/* interrupt line */
@@ -362,22 +359,11 @@ struct ahci_host_priv {
 	 */
 	void			(*start_engine)(struct ata_port *ap);
 	irqreturn_t 		(*irq_handler)(int irq, void *dev_instance);
-};
 
-#ifdef CONFIG_PCI_MSI
-static inline int ahci_irq_vector(struct ahci_host_priv *hpriv, int port)
-{
-	if (hpriv->flags & AHCI_HFLAG_MULTI_MSIX)
-		return hpriv->msix[port].vector;
-	else
-		return hpriv->irq + port;
-}
-#else
-static inline int ahci_irq_vector(struct ahci_host_priv *hpriv, int port)
-{
-	return hpriv->irq;
-}
-#endif
+	/* only required for per-port MSI(-X) support */
+	int			(*get_irq_vector)(struct ata_host *host,
+						  int port);
+};
 
 extern int ahci_ignore_sss;
 
diff --git a/drivers/ata/ahci_qoriq.c b/drivers/ata/ahci_qoriq.c
index 7bdee9bd8786..1eba8dff875e 100644
--- a/drivers/ata/ahci_qoriq.c
+++ b/drivers/ata/ahci_qoriq.c
@@ -30,24 +30,23 @@
 #define PORT_PHY3	0xB0
 #define PORT_PHY4	0xB4
 #define PORT_PHY5	0xB8
+#define PORT_AXICC	0xBC
 #define PORT_TRANS	0xC8
 
 /* port register default value */
 #define AHCI_PORT_PHY_1_CFG	0xa003fffe
 #define AHCI_PORT_TRANS_CFG	0x08000029
+#define AHCI_PORT_AXICC_CFG	0x3fffffff
 
 /* for ls1021a */
 #define LS1021A_PORT_PHY2	0x28183414
 #define LS1021A_PORT_PHY3	0x0e080e06
 #define LS1021A_PORT_PHY4	0x064a080b
 #define LS1021A_PORT_PHY5	0x2aa86470
+#define LS1021A_AXICC_ADDR	0xC0
 
 #define SATA_ECC_DISABLE	0x00020000
 
-/* for ls1043a */
-#define LS1043A_PORT_PHY2	0x28184d1f
-#define LS1043A_PORT_PHY3	0x0e081509
-
 enum ahci_qoriq_type {
 	AHCI_LS1021A,
 	AHCI_LS1043A,
@@ -137,7 +136,7 @@ static struct ata_port_operations ahci_qoriq_ops = {
 	.hardreset	= ahci_qoriq_hardreset,
 };
 
-static struct ata_port_info ahci_qoriq_port_info = {
+static const struct ata_port_info ahci_qoriq_port_info = {
 	.flags		= AHCI_FLAG_COMMON | ATA_FLAG_NCQ,
 	.pio_mask	= ATA_PIO4,
 	.udma_mask	= ATA_UDMA6,
@@ -162,18 +161,19 @@ static int ahci_qoriq_phy_init(struct ahci_host_priv *hpriv)
 		writel(LS1021A_PORT_PHY4, reg_base + PORT_PHY4);
 		writel(LS1021A_PORT_PHY5, reg_base + PORT_PHY5);
 		writel(AHCI_PORT_TRANS_CFG, reg_base + PORT_TRANS);
+		writel(AHCI_PORT_AXICC_CFG, reg_base + LS1021A_AXICC_ADDR);
 		break;
 
 	case AHCI_LS1043A:
 		writel(AHCI_PORT_PHY_1_CFG, reg_base + PORT_PHY1);
-		writel(LS1043A_PORT_PHY2, reg_base + PORT_PHY2);
-		writel(LS1043A_PORT_PHY3, reg_base + PORT_PHY3);
 		writel(AHCI_PORT_TRANS_CFG, reg_base + PORT_TRANS);
+		writel(AHCI_PORT_AXICC_CFG, reg_base + PORT_AXICC);
 		break;
 
 	case AHCI_LS2080A:
 		writel(AHCI_PORT_PHY_1_CFG, reg_base + PORT_PHY1);
 		writel(AHCI_PORT_TRANS_CFG, reg_base + PORT_TRANS);
+		writel(AHCI_PORT_AXICC_CFG, reg_base + PORT_AXICC);
 		break;
 	}
 
@@ -221,12 +221,6 @@ static int ahci_qoriq_probe(struct platform_device *pdev)
 	if (rc)
 		goto disable_resources;
 
-	/* Workaround for ls2080a */
-	if (qoriq_priv->type == AHCI_LS2080A) {
-		hpriv->flags |= AHCI_HFLAG_NO_NCQ;
-		ahci_qoriq_port_info.flags &= ~ATA_FLAG_NCQ;
-	}
-
 	rc = ahci_platform_init_host(pdev, hpriv, &ahci_qoriq_port_info,
 				     &ahci_qoriq_sht);
 	if (rc)
diff --git a/drivers/ata/ahci_st.c b/drivers/ata/ahci_st.c
index 8ff428fe8e0f..bc345f249555 100644
--- a/drivers/ata/ahci_st.c
+++ b/drivers/ata/ahci_st.c
@@ -147,6 +147,7 @@ static struct scsi_host_template ahci_platform_sht = {
 
 static int st_ahci_probe(struct platform_device *pdev)
 {
+	struct device *dev = &pdev->dev;
 	struct st_ahci_drv_data *drv_data;
 	struct ahci_host_priv *hpriv;
 	int err;
@@ -170,6 +171,9 @@ static int st_ahci_probe(struct platform_device *pdev)
 
 	st_ahci_configure_oob(hpriv->mmio);
 
+	of_property_read_u32(dev->of_node,
+			     "ports-implemented", &hpriv->force_port_map);
+
 	err = ahci_platform_init_host(pdev, hpriv, &st_ahci_port_info,
 				      &ahci_platform_sht);
 	if (err) {
diff --git a/drivers/ata/libahci.c b/drivers/ata/libahci.c
index dcf2c724fd06..0d028ead99e8 100644
--- a/drivers/ata/libahci.c
+++ b/drivers/ata/libahci.c
@@ -2520,7 +2520,7 @@ static int ahci_host_activate_multi_irqs(struct ata_host *host,
 	 */
 	for (i = 0; i < host->n_ports; i++) {
 		struct ahci_port_priv *pp = host->ports[i]->private_data;
-		int irq = ahci_irq_vector(hpriv, i);
+		int irq = hpriv->get_irq_vector(host, i);
 
 		/* Do not receive interrupts sent by dummy ports */
 		if (!pp) {
@@ -2556,10 +2556,15 @@ int ahci_host_activate(struct ata_host *host, struct scsi_host_template *sht)
 	int irq = hpriv->irq;
 	int rc;
 
-	if (hpriv->flags & (AHCI_HFLAG_MULTI_MSI | AHCI_HFLAG_MULTI_MSIX)) {
+	if (hpriv->flags & AHCI_HFLAG_MULTI_MSI) {
 		if (hpriv->irq_handler)
 			dev_warn(host->dev,
 			         "both AHCI_HFLAG_MULTI_MSI flag set and custom irq handler implemented\n");
+		if (!hpriv->get_irq_vector) {
+			dev_err(host->dev,
+				"AHCI_HFLAG_MULTI_MSI requires ->get_irq_vector!\n");
+			return -EIO;
+		}
 
 		rc = ahci_host_activate_multi_irqs(host, sht);
 	} else {
diff --git a/drivers/ata/libata-scsi.c b/drivers/ata/libata-scsi.c
index e207b33e4ce9..9cceb4a875a5 100644
--- a/drivers/ata/libata-scsi.c
+++ b/drivers/ata/libata-scsi.c
@@ -1159,8 +1159,6 @@ static void ata_scsi_sdev_config(struct scsi_device *sdev)
 {
 	sdev->use_10_for_rw = 1;
 	sdev->use_10_for_ms = 1;
-	sdev->no_report_opcodes = 1;
-	sdev->no_write_same = 1;
 
 	/* Schedule policy is determined by ->qc_defer() callback and
 	 * it needs to see every deferred qc.  Set dev_blocked to 1 to
@@ -3282,18 +3280,125 @@ static unsigned int ata_scsi_pass_thru(struct ata_queued_cmd *qc)
 	return 1;
 }
 
+/**
+ * ata_format_dsm_trim_descr() - SATL Write Same to DSM Trim
+ * @cmd: SCSI command being translated
+ * @trmax: Maximum number of entries that will fit in sector_size bytes.
+ * @sector: Starting sector
+ * @count: Total Range of request in logical sectors
+ *
+ * Rewrite the WRITE SAME descriptor to be a DSM TRIM little-endian formatted
+ * descriptor.
+ *
+ * Upto 64 entries of the format:
+ *   63:48 Range Length
+ *   47:0  LBA
+ *
+ *  Range Length of 0 is ignored.
+ *  LBA's should be sorted order and not overlap.
+ *
+ * NOTE: this is the same format as ADD LBA(S) TO NV CACHE PINNED SET
+ *
+ * Return: Number of bytes copied into sglist.
+ */
+static size_t ata_format_dsm_trim_descr(struct scsi_cmnd *cmd, u32 trmax,
+					u64 sector, u32 count)
+{
+	struct scsi_device *sdp = cmd->device;
+	size_t len = sdp->sector_size;
+	size_t r;
+	__le64 *buf;
+	u32 i = 0;
+	unsigned long flags;
+
+	WARN_ON(len > ATA_SCSI_RBUF_SIZE);
+
+	if (len > ATA_SCSI_RBUF_SIZE)
+		len = ATA_SCSI_RBUF_SIZE;
+
+	spin_lock_irqsave(&ata_scsi_rbuf_lock, flags);
+	buf = ((void *)ata_scsi_rbuf);
+	memset(buf, 0, len);
+	while (i < trmax) {
+		u64 entry = sector |
+			((u64)(count > 0xffff ? 0xffff : count) << 48);
+		buf[i++] = __cpu_to_le64(entry);
+		if (count <= 0xffff)
+			break;
+		count -= 0xffff;
+		sector += 0xffff;
+	}
+	r = sg_copy_from_buffer(scsi_sglist(cmd), scsi_sg_count(cmd), buf, len);
+	spin_unlock_irqrestore(&ata_scsi_rbuf_lock, flags);
+
+	return r;
+}
+
+/**
+ * ata_format_dsm_trim_descr() - SATL Write Same to ATA SCT Write Same
+ * @cmd: SCSI command being translated
+ * @lba: Starting sector
+ * @num: Number of sectors to be zero'd.
+ *
+ * Rewrite the WRITE SAME payload to be an SCT Write Same formatted
+ * descriptor.
+ * NOTE: Writes a pattern (0's) in the foreground.
+ *
+ * Return: Number of bytes copied into sglist.
+ */
+static size_t ata_format_sct_write_same(struct scsi_cmnd *cmd, u64 lba, u64 num)
+{
+	struct scsi_device *sdp = cmd->device;
+	size_t len = sdp->sector_size;
+	size_t r;
+	u16 *buf;
+	unsigned long flags;
+
+	spin_lock_irqsave(&ata_scsi_rbuf_lock, flags);
+	buf = ((void *)ata_scsi_rbuf);
+
+	put_unaligned_le16(0x0002,  &buf[0]); /* SCT_ACT_WRITE_SAME */
+	put_unaligned_le16(0x0101,  &buf[1]); /* WRITE PTRN FG */
+	put_unaligned_le64(lba,     &buf[2]);
+	put_unaligned_le64(num,     &buf[6]);
+	put_unaligned_le32(0u,      &buf[10]); /* pattern */
+
+	WARN_ON(len > ATA_SCSI_RBUF_SIZE);
+
+	if (len > ATA_SCSI_RBUF_SIZE)
+		len = ATA_SCSI_RBUF_SIZE;
+
+	r = sg_copy_from_buffer(scsi_sglist(cmd), scsi_sg_count(cmd), buf, len);
+	spin_unlock_irqrestore(&ata_scsi_rbuf_lock, flags);
+
+	return r;
+}
+
+/**
+ * ata_scsi_write_same_xlat() - SATL Write Same to ATA SCT Write Same
+ * @qc: Command to be translated
+ *
+ * Translate a SCSI WRITE SAME command to be either a DSM TRIM command or
+ * an SCT Write Same command.
+ * Based on WRITE SAME has the UNMAP flag
+ *   When set translate to DSM TRIM
+ *   When clear translate to SCT Write Same
+ */
 static unsigned int ata_scsi_write_same_xlat(struct ata_queued_cmd *qc)
 {
 	struct ata_taskfile *tf = &qc->tf;
 	struct scsi_cmnd *scmd = qc->scsicmd;
+	struct scsi_device *sdp = scmd->device;
+	size_t len = sdp->sector_size;
 	struct ata_device *dev = qc->dev;
 	const u8 *cdb = scmd->cmnd;
 	u64 block;
 	u32 n_block;
+	const u32 trmax = len >> 3;
 	u32 size;
-	void *buf;
 	u16 fp;
 	u8 bp = 0xff;
+	u8 unmap = cdb[1] & 0x8;
 
 	/* we may not issue DMA commands if no DMA mode is set */
 	if (unlikely(!dev->dma_mode))
@@ -3305,11 +3410,26 @@ static unsigned int ata_scsi_write_same_xlat(struct ata_queued_cmd *qc)
 	}
 	scsi_16_lba_len(cdb, &block, &n_block);
 
-	/* for now we only support WRITE SAME with the unmap bit set */
-	if (unlikely(!(cdb[1] & 0x8))) {
-		fp = 1;
-		bp = 3;
-		goto invalid_fld;
+	if (unmap) {
+		/* If trim is not enabled the cmd is invalid. */
+		if ((dev->horkage & ATA_HORKAGE_NOTRIM) ||
+		    !ata_id_has_trim(dev->id)) {
+			fp = 1;
+			bp = 3;
+			goto invalid_fld;
+		}
+		/* If the request is too large the cmd is invalid */
+		if (n_block > 0xffff * trmax) {
+			fp = 2;
+			goto invalid_fld;
+		}
+	} else {
+		/* If write same is not available the cmd is invalid */
+		if (!ata_id_sct_write_same(dev->id)) {
+			fp = 1;
+			bp = 3;
+			goto invalid_fld;
+		}
 	}
 
 	/*
@@ -3319,32 +3439,54 @@ static unsigned int ata_scsi_write_same_xlat(struct ata_queued_cmd *qc)
 	if (!scsi_sg_count(scmd))
 		goto invalid_param_len;
 
-	buf = page_address(sg_page(scsi_sglist(scmd)));
-
-	if (n_block <= 65535 * ATA_MAX_TRIM_RNUM) {
-		size = ata_set_lba_range_entries(buf, ATA_MAX_TRIM_RNUM, block, n_block);
-	} else {
-		fp = 2;
-		goto invalid_fld;
-	}
+	/*
+	 * size must match sector size in bytes
+	 * For DATA SET MANAGEMENT TRIM in ACS-2 nsect (aka count)
+	 * is defined as number of 512 byte blocks to be transferred.
+	 */
+	if (unmap) {
+		size = ata_format_dsm_trim_descr(scmd, trmax, block, n_block);
+		if (size != len)
+			goto invalid_param_len;
 
-	if (ata_ncq_enabled(dev) && ata_fpdma_dsm_supported(dev)) {
-		/* Newer devices support queued TRIM commands */
-		tf->protocol = ATA_PROT_NCQ;
-		tf->command = ATA_CMD_FPDMA_SEND;
-		tf->hob_nsect = ATA_SUBCMD_FPDMA_SEND_DSM & 0x1f;
-		tf->nsect = qc->tag << 3;
-		tf->hob_feature = (size / 512) >> 8;
-		tf->feature = size / 512;
+		if (ata_ncq_enabled(dev) && ata_fpdma_dsm_supported(dev)) {
+			/* Newer devices support queued TRIM commands */
+			tf->protocol = ATA_PROT_NCQ;
+			tf->command = ATA_CMD_FPDMA_SEND;
+			tf->hob_nsect = ATA_SUBCMD_FPDMA_SEND_DSM & 0x1f;
+			tf->nsect = qc->tag << 3;
+			tf->hob_feature = (size / 512) >> 8;
+			tf->feature = size / 512;
 
-		tf->auxiliary = 1;
+			tf->auxiliary = 1;
+		} else {
+			tf->protocol = ATA_PROT_DMA;
+			tf->hob_feature = 0;
+			tf->feature = ATA_DSM_TRIM;
+			tf->hob_nsect = (size / 512) >> 8;
+			tf->nsect = size / 512;
+			tf->command = ATA_CMD_DSM;
+		}
 	} else {
-		tf->protocol = ATA_PROT_DMA;
+		size = ata_format_sct_write_same(scmd, block, n_block);
+		if (size != len)
+			goto invalid_param_len;
+
 		tf->hob_feature = 0;
-		tf->feature = ATA_DSM_TRIM;
-		tf->hob_nsect = (size / 512) >> 8;
-		tf->nsect = size / 512;
-		tf->command = ATA_CMD_DSM;
+		tf->feature = 0;
+		tf->hob_nsect = 0;
+		tf->nsect = 1;
+		tf->lbah = 0;
+		tf->lbam = 0;
+		tf->lbal = ATA_CMD_STANDBYNOW1;
+		tf->hob_lbah = 0;
+		tf->hob_lbam = 0;
+		tf->hob_lbal = 0;
+		tf->device = ATA_CMD_STANDBYNOW1;
+		tf->protocol = ATA_PROT_DMA;
+		tf->command = ATA_CMD_WRITE_LOG_DMA_EXT;
+		if (unlikely(dev->flags & ATA_DFLAG_PIO))
+			tf->command = ATA_CMD_WRITE_LOG_EXT;
 	}
 
 	tf->flags |= ATA_TFLAG_ISADDR | ATA_TFLAG_DEVICE | ATA_TFLAG_LBA48 |
@@ -3368,6 +3510,76 @@ invalid_opcode:
 }
 
 /**
+ *	ata_scsiop_maint_in - Simulate a subset of MAINTENANCE_IN
+ *	@args: device MAINTENANCE_IN data / SCSI command of interest.
+ *	@rbuf: Response buffer, to which simulated SCSI cmd output is sent.
+ *
+ *	Yields a subset to satisfy scsi_report_opcode()
+ *
+ *	LOCKING:
+ *	spin_lock_irqsave(host lock)
+ */
+static unsigned int ata_scsiop_maint_in(struct ata_scsi_args *args, u8 *rbuf)
+{
+	struct ata_device *dev = args->dev;
+	u8 *cdb = args->cmd->cmnd;
+	u8 supported = 0;
+	unsigned int err = 0;
+
+	if (cdb[2] != 1) {
+		ata_dev_warn(dev, "invalid command format %d\n", cdb[2]);
+		err = 2;
+		goto out;
+	}
+	switch (cdb[3]) {
+	case INQUIRY:
+	case MODE_SENSE:
+	case MODE_SENSE_10:
+	case READ_CAPACITY:
+	case SERVICE_ACTION_IN_16:
+	case REPORT_LUNS:
+	case REQUEST_SENSE:
+	case SYNCHRONIZE_CACHE:
+	case REZERO_UNIT:
+	case SEEK_6:
+	case SEEK_10:
+	case TEST_UNIT_READY:
+	case SEND_DIAGNOSTIC:
+	case MAINTENANCE_IN:
+	case READ_6:
+	case READ_10:
+	case READ_16:
+	case WRITE_6:
+	case WRITE_10:
+	case WRITE_16:
+	case ATA_12:
+	case ATA_16:
+	case VERIFY:
+	case VERIFY_16:
+	case MODE_SELECT:
+	case MODE_SELECT_10:
+	case START_STOP:
+		supported = 3;
+		break;
+	case WRITE_SAME_16:
+		if (!ata_id_sct_write_same(dev->id))
+			break;
+		/* fallthrough: if SCT ... only enable for ZBC */
+	case ZBC_IN:
+	case ZBC_OUT:
+		if (ata_id_zoned_cap(dev->id) ||
+		    dev->class == ATA_DEV_ZAC)
+			supported = 3;
+		break;
+	default:
+		break;
+	}
+out:
+	rbuf[1] = supported; /* supported */
+	return err;
+}
+
+/**
  *	ata_scsi_report_zones_complete - convert ATA output
  *	@qc: command structure returning the data
  *
@@ -3610,7 +3822,7 @@ static int ata_mselect_caching(struct ata_queued_cmd *qc,
 {
 	struct ata_taskfile *tf = &qc->tf;
 	struct ata_device *dev = qc->dev;
-	char mpage[CACHE_MPAGE_LEN];
+	u8 mpage[CACHE_MPAGE_LEN];
 	u8 wce;
 	int i;
 
@@ -3666,7 +3878,7 @@ static int ata_mselect_control(struct ata_queued_cmd *qc,
 			       const u8 *buf, int len, u16 *fp)
 {
 	struct ata_device *dev = qc->dev;
-	char mpage[CONTROL_MPAGE_LEN];
+	u8 mpage[CONTROL_MPAGE_LEN];
 	u8 d_sense;
 	int i;
 
@@ -3701,8 +3913,6 @@ static int ata_mselect_control(struct ata_queued_cmd *qc,
 		dev->flags |= ATA_DFLAG_D_SENSE;
 	else
 		dev->flags &= ~ATA_DFLAG_D_SENSE;
-	qc->scsicmd->result = SAM_STAT_GOOD;
-	qc->scsicmd->scsi_done(qc->scsicmd);
 	return 0;
 }
 
@@ -3829,6 +4039,8 @@ static unsigned int ata_scsi_mode_select_xlat(struct ata_queued_cmd *qc)
 		if (ata_mselect_control(qc, p, pg_len, &fp) < 0) {
 			fp += hdr_len + bd_len;
 			goto invalid_param;
+		} else {
+			goto skip; /* No ATA command to send */
 		}
 		break;
 	default:		/* invalid page code */
@@ -4147,6 +4359,13 @@ void ata_scsi_simulate(struct ata_device *dev, struct scsi_cmnd *cmd)
 			ata_scsi_invalid_field(dev, cmd, 1);
 		break;
 
+	case MAINTENANCE_IN:
+		if (scsicmd[1] == MI_REPORT_SUPPORTED_OPERATION_CODES)
+			ata_scsi_rbuf_fill(&args, ata_scsiop_maint_in);
+		else
+			ata_scsi_invalid_field(dev, cmd, 1);
+		break;
+
 	/* all other commands */
 	default:
 		ata_scsi_set_sense(dev, cmd, ILLEGAL_REQUEST, 0x20, 0x0);
@@ -4179,7 +4398,6 @@ int ata_scsi_add_hosts(struct ata_host *host, struct scsi_host_template *sht)
 		shost->max_lun = 1;
 		shost->max_channel = 1;
 		shost->max_cmd_len = 16;
-		shost->no_write_same = 1;
 
 		/* Schedule policy is determined by ->qc_defer()
 		 * callback and it needs to see every deferred qc.
diff --git a/drivers/ata/pata_at91.c b/drivers/ata/pata_at91.c
index 9f27b14009f9..1611e0e8d767 100644
--- a/drivers/ata/pata_at91.c
+++ b/drivers/ata/pata_at91.c
@@ -347,10 +347,8 @@ static int at91sam9_smc_fields_init(struct device *dev)
 
 	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;
+	return PTR_ERR_OR_ZERO(fields.mode);
 }
 
 static int pata_at91_probe(struct platform_device *pdev)
diff --git a/drivers/ata/pata_octeon_cf.c b/drivers/ata/pata_octeon_cf.c
index 27245957eee3..475a00669427 100644
--- a/drivers/ata/pata_octeon_cf.c
+++ b/drivers/ata/pata_octeon_cf.c
@@ -152,8 +152,7 @@ static void octeon_cf_set_piomode(struct ata_port *ap, struct ata_device *dev)
 		div = 8;
 	T = (int)((1000000000000LL * div) / octeon_get_io_clock_rate());
 
-	if (ata_timing_compute(dev, dev->pio_mode, &timing, T, T))
-		BUG();
+	BUG_ON(ata_timing_compute(dev, dev->pio_mode, &timing, T, T));
 
 	t1 = timing.setup;
 	if (t1)
diff --git a/drivers/ata/sata_mv.c b/drivers/ata/sata_mv.c
index 745489a1c86a..efc48bf89d51 100644
--- a/drivers/ata/sata_mv.c
+++ b/drivers/ata/sata_mv.c
@@ -1727,15 +1727,13 @@ static int mv_port_start(struct ata_port *ap)
 		return -ENOMEM;
 	ap->private_data = pp;
 
-	pp->crqb = dma_pool_alloc(hpriv->crqb_pool, GFP_KERNEL, &pp->crqb_dma);
+	pp->crqb = dma_pool_zalloc(hpriv->crqb_pool, GFP_KERNEL, &pp->crqb_dma);
 	if (!pp->crqb)
 		return -ENOMEM;
-	memset(pp->crqb, 0, MV_CRQB_Q_SZ);
 
-	pp->crpb = dma_pool_alloc(hpriv->crpb_pool, GFP_KERNEL, &pp->crpb_dma);
+	pp->crpb = dma_pool_zalloc(hpriv->crpb_pool, GFP_KERNEL, &pp->crpb_dma);
 	if (!pp->crpb)
 		goto out_port_free_dma_mem;
-	memset(pp->crpb, 0, MV_CRPB_Q_SZ);
 
 	/* 6041/6081 Rev. "C0" (and newer) are okay with async notify */
 	if (hpriv->hp_flags & MV_HP_ERRATA_60X1C0)
diff --git a/drivers/atm/eni.c b/drivers/atm/eni.c
index 6339efd32697..f2aaf9e32a36 100644
--- a/drivers/atm/eni.c
+++ b/drivers/atm/eni.c
@@ -1845,8 +1845,9 @@ static int eni_start(struct atm_dev *dev)
 	/* initialize memory management */
 	buffer_mem = eni_dev->mem - (buf - eni_dev->ram);
 	eni_dev->free_list_size = buffer_mem/MID_MIN_BUF_SIZE/2;
-	eni_dev->free_list = kmalloc(
-	    sizeof(struct eni_free)*(eni_dev->free_list_size+1),GFP_KERNEL);
+	eni_dev->free_list = kmalloc_array(eni_dev->free_list_size + 1,
+					   sizeof(*eni_dev->free_list),
+					   GFP_KERNEL);
 	if (!eni_dev->free_list) {
 		printk(KERN_ERR DEV_LABEL "(itf %d): couldn't get free page\n",
 		    dev->number);
diff --git a/drivers/atm/fore200e.c b/drivers/atm/fore200e.c
index 75dde903b238..81aaa505862c 100644
--- a/drivers/atm/fore200e.c
+++ b/drivers/atm/fore200e.c
@@ -2489,7 +2489,7 @@ static int fore200e_load_and_start_fw(struct fore200e *fore200e)
 {
     const struct firmware *firmware;
     struct device *device;
-    struct fw_header *fw_header;
+    const struct fw_header *fw_header;
     const __le32 *fw_data;
     u32 fw_size;
     u32 __iomem *load_addr;
@@ -2511,9 +2511,9 @@ static int fore200e_load_and_start_fw(struct fore200e *fore200e)
 	return err;
     }
 
-    fw_data = (__le32 *) firmware->data;
+    fw_data = (const __le32 *)firmware->data;
     fw_size = firmware->size / sizeof(u32);
-    fw_header = (struct fw_header *) firmware->data;
+    fw_header = (const struct fw_header *)firmware->data;
     load_addr = fore200e->virt_base + le32_to_cpu(fw_header->load_offset);
 
     DPRINTK(2, "device %s firmware being loaded at 0x%p (%d words)\n",
diff --git a/drivers/atm/he.c b/drivers/atm/he.c
index 0f5cb37636bc..31b513a23ae0 100644
--- a/drivers/atm/he.c
+++ b/drivers/atm/he.c
@@ -779,8 +779,9 @@ static int he_init_group(struct he_dev *he_dev, int group)
 		  G0_RBPS_BS + (group * 32));
 
 	/* bitmap table */
-	he_dev->rbpl_table = kmalloc(BITS_TO_LONGS(RBPL_TABLE_SIZE)
-				     * sizeof(unsigned long), GFP_KERNEL);
+	he_dev->rbpl_table = kmalloc_array(BITS_TO_LONGS(RBPL_TABLE_SIZE),
+					   sizeof(*he_dev->rbpl_table),
+					   GFP_KERNEL);
 	if (!he_dev->rbpl_table) {
 		hprintk("unable to allocate rbpl bitmap table\n");
 		return -ENOMEM;
@@ -788,8 +789,9 @@ static int he_init_group(struct he_dev *he_dev, int group)
 	bitmap_zero(he_dev->rbpl_table, RBPL_TABLE_SIZE);
 
 	/* rbpl_virt 64-bit pointers */
-	he_dev->rbpl_virt = kmalloc(RBPL_TABLE_SIZE
-				    * sizeof(struct he_buff *), GFP_KERNEL);
+	he_dev->rbpl_virt = kmalloc_array(RBPL_TABLE_SIZE,
+					  sizeof(*he_dev->rbpl_virt),
+					  GFP_KERNEL);
 	if (!he_dev->rbpl_virt) {
 		hprintk("unable to allocate rbpl virt table\n");
 		goto out_free_rbpl_table;
diff --git a/drivers/atm/iphase.c b/drivers/atm/iphase.c
index 809dd1e02091..b2756765950e 100644
--- a/drivers/atm/iphase.c
+++ b/drivers/atm/iphase.c
@@ -1885,9 +1885,9 @@ static int open_tx(struct atm_vcc *vcc)
                 if ((ret = ia_cbr_setup (iadev, vcc)) < 0) {     
                     return ret;
                 }
-       } 
-	else  
-           printk("iadev:  Non UBR, ABR and CBR traffic not supportedn"); 
+	} else {
+		printk("iadev:  Non UBR, ABR and CBR traffic not supported\n");
+	}
         
         iadev->testTable[vcc->vci]->vc_status |= VC_ACTIVE;
 	IF_EVENT(printk("ia open_tx returning \n");)  
@@ -1975,7 +1975,9 @@ static int tx_init(struct atm_dev *dev)
 		buf_desc_ptr++;		  
 		tx_pkt_start += iadev->tx_buf_sz;  
 	}  
-        iadev->tx_buf = kmalloc(iadev->num_tx_desc*sizeof(struct cpcs_trailer_desc), GFP_KERNEL);
+	iadev->tx_buf = kmalloc_array(iadev->num_tx_desc,
+				      sizeof(*iadev->tx_buf),
+				      GFP_KERNEL);
         if (!iadev->tx_buf) {
             printk(KERN_ERR DEV_LABEL " couldn't get mem\n");
 	    goto err_free_dle;
@@ -1995,8 +1997,9 @@ static int tx_init(struct atm_dev *dev)
 						       sizeof(*cpcs),
 						       DMA_TO_DEVICE);
         }
-        iadev->desc_tbl = kmalloc(iadev->num_tx_desc *
-                                   sizeof(struct desc_tbl_t), GFP_KERNEL);
+	iadev->desc_tbl = kmalloc_array(iadev->num_tx_desc,
+					sizeof(*iadev->desc_tbl),
+					GFP_KERNEL);
 	if (!iadev->desc_tbl) {
 		printk(KERN_ERR DEV_LABEL " couldn't get mem\n");
 		goto err_free_all_tx_bufs;
@@ -2124,7 +2127,9 @@ static int tx_init(struct atm_dev *dev)
 	memset((caddr_t)(iadev->seg_ram+i),  0, iadev->num_vc*4);
 	vc = (struct main_vc *)iadev->MAIN_VC_TABLE_ADDR;  
 	evc = (struct ext_vc *)iadev->EXT_VC_TABLE_ADDR;  
-        iadev->testTable = kmalloc(sizeof(long)*iadev->num_vc, GFP_KERNEL); 
+	iadev->testTable = kmalloc_array(iadev->num_vc,
+					 sizeof(*iadev->testTable),
+					 GFP_KERNEL);
         if (!iadev->testTable) {
            printk("Get freepage  failed\n");
 	   goto err_free_desc_tbl;
diff --git a/drivers/atm/nicstar.c b/drivers/atm/nicstar.c
index 700ed15c2362..c7296b583787 100644
--- a/drivers/atm/nicstar.c
+++ b/drivers/atm/nicstar.c
@@ -370,7 +370,8 @@ static int ns_init_card(int i, struct pci_dev *pcidev)
 		return error;
         }
 
-	if ((card = kmalloc(sizeof(ns_dev), GFP_KERNEL)) == NULL) {
+	card = kmalloc(sizeof(*card), GFP_KERNEL);
+	if (!card) {
 		printk
 		    ("nicstar%d: can't allocate memory for device structure.\n",
 		     i);
@@ -611,7 +612,7 @@ static int ns_init_card(int i, struct pci_dev *pcidev)
 	for (j = 0; j < card->rct_size; j++)
 		ns_write_sram(card, j * 4, u32d, 4);
 
-	memset(card->vcmap, 0, NS_MAX_RCTSIZE * sizeof(vc_map));
+	memset(card->vcmap, 0, sizeof(card->vcmap));
 
 	for (j = 0; j < NS_FRSCD_NUM; j++)
 		card->scd2vc[j] = NULL;
@@ -862,7 +863,7 @@ static scq_info *get_scq(ns_dev *card, int size, u32 scd)
 	if (size != VBR_SCQSIZE && size != CBR_SCQSIZE)
 		return NULL;
 
-	scq = kmalloc(sizeof(scq_info), GFP_KERNEL);
+	scq = kmalloc(sizeof(*scq), GFP_KERNEL);
 	if (!scq)
 		return NULL;
         scq->org = dma_alloc_coherent(&card->pcidev->dev,
@@ -871,8 +872,9 @@ static scq_info *get_scq(ns_dev *card, int size, u32 scd)
 		kfree(scq);
 		return NULL;
 	}
-	scq->skb = kmalloc(sizeof(struct sk_buff *) *
-			   (size / NS_SCQE_SIZE), GFP_KERNEL);
+	scq->skb = kmalloc_array(size / NS_SCQE_SIZE,
+				 sizeof(*scq->skb),
+				 GFP_KERNEL);
 	if (!scq->skb) {
 		dma_free_coherent(&card->pcidev->dev,
 				  2 * size, scq->org, scq->dma);
@@ -2021,7 +2023,8 @@ static void dequeue_rx(ns_dev * card, ns_rsqe * rsqe)
 
 		cell = skb->data;
 		for (i = ns_rsqe_cellcount(rsqe); i; i--) {
-			if ((sb = dev_alloc_skb(NS_SMSKBSIZE)) == NULL) {
+			sb = dev_alloc_skb(NS_SMSKBSIZE);
+			if (!sb) {
 				printk
 				    ("nicstar%d: Can't allocate buffers for aal0.\n",
 				     card->index);
diff --git a/drivers/atm/zatm.c b/drivers/atm/zatm.c
index cecfb943762f..d3dc95484161 100644
--- a/drivers/atm/zatm.c
+++ b/drivers/atm/zatm.c
@@ -598,12 +598,13 @@ static void close_rx(struct atm_vcc *vcc)
 static int start_rx(struct atm_dev *dev)
 {
 	struct zatm_dev *zatm_dev;
-	int size,i;
+	int i;
 
-DPRINTK("start_rx\n");
+	DPRINTK("start_rx\n");
 	zatm_dev = ZATM_DEV(dev);
-	size = sizeof(struct atm_vcc *)*zatm_dev->chans;
-	zatm_dev->rx_map =  kzalloc(size,GFP_KERNEL);
+	zatm_dev->rx_map = kcalloc(zatm_dev->chans,
+				   sizeof(*zatm_dev->rx_map),
+				   GFP_KERNEL);
 	if (!zatm_dev->rx_map) return -ENOMEM;
 	/* set VPI/VCI split (use all VCIs and give what's left to VPIs) */
 	zpokel(zatm_dev,(1 << dev->ci_range.vci_bits)-1,uPD98401_VRR);
@@ -998,8 +999,9 @@ static int start_tx(struct atm_dev *dev)
 
 	DPRINTK("start_tx\n");
 	zatm_dev = ZATM_DEV(dev);
-	zatm_dev->tx_map = kmalloc(sizeof(struct atm_vcc *)*
-	    zatm_dev->chans,GFP_KERNEL);
+	zatm_dev->tx_map = kmalloc_array(zatm_dev->chans,
+					 sizeof(*zatm_dev->tx_map),
+					 GFP_KERNEL);
 	if (!zatm_dev->tx_map) return -ENOMEM;
 	zatm_dev->tx_bw = ATM_OC3_PCR;
 	zatm_dev->free_shapers = (1 << NR_SHAPERS)-1;
@@ -1398,7 +1400,7 @@ static int zatm_open(struct atm_vcc *vcc)
 	DPRINTK(DEV_LABEL "(itf %d): open %d.%d\n",vcc->dev->number,vcc->vpi,
 	    vcc->vci);
 	if (!test_bit(ATM_VF_PARTIAL,&vcc->flags)) {
-		zatm_vcc = kmalloc(sizeof(struct zatm_vcc),GFP_KERNEL);
+		zatm_vcc = kmalloc(sizeof(*zatm_vcc), GFP_KERNEL);
 		if (!zatm_vcc) {
 			clear_bit(ATM_VF_ADDR,&vcc->flags);
 			return -ENOMEM;
diff --git a/drivers/auxdisplay/Kconfig b/drivers/auxdisplay/Kconfig
index c07e725ea93d..10e1b9eee10e 100644
--- a/drivers/auxdisplay/Kconfig
+++ b/drivers/auxdisplay/Kconfig
@@ -119,4 +119,13 @@ config CFAG12864B_RATE
 	  If you compile this as a module, you can still override this
 	  value using the module parameters.
 
+config IMG_ASCII_LCD
+	tristate "Imagination Technologies ASCII LCD Display"
+	default y if MIPS_MALTA || MIPS_SEAD3
+	select SYSCON
+	help
+	  Enable this to support the simple ASCII LCD displays found on
+	  development boards such as the MIPS Boston, MIPS Malta & MIPS SEAD3
+	  from Imagination Technologies.
+
 endif # AUXDISPLAY
diff --git a/drivers/auxdisplay/Makefile b/drivers/auxdisplay/Makefile
index 8a8936a468b9..3127175c89df 100644
--- a/drivers/auxdisplay/Makefile
+++ b/drivers/auxdisplay/Makefile
@@ -4,3 +4,4 @@
 
 obj-$(CONFIG_KS0108)		+= ks0108.o
 obj-$(CONFIG_CFAG12864B)	+= cfag12864b.o cfag12864bfb.o
+obj-$(CONFIG_IMG_ASCII_LCD)	+= img-ascii-lcd.o
diff --git a/drivers/auxdisplay/img-ascii-lcd.c b/drivers/auxdisplay/img-ascii-lcd.c
new file mode 100644
index 000000000000..bf43b5d2aafc
--- /dev/null
+++ b/drivers/auxdisplay/img-ascii-lcd.c
@@ -0,0 +1,443 @@
+/*
+ * Copyright (C) 2016 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.
+ */
+
+#include <generated/utsrelease.h>
+#include <linux/kernel.h>
+#include <linux/io.h>
+#include <linux/mfd/syscon.h>
+#include <linux/module.h>
+#include <linux/of_address.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+#include <linux/slab.h>
+#include <linux/sysfs.h>
+
+struct img_ascii_lcd_ctx;
+
+/**
+ * struct img_ascii_lcd_config - Configuration information about an LCD model
+ * @num_chars: the number of characters the LCD can display
+ * @external_regmap: true if registers are in a system controller, else false
+ * @update: function called to update the LCD
+ */
+struct img_ascii_lcd_config {
+	unsigned int num_chars;
+	bool external_regmap;
+	void (*update)(struct img_ascii_lcd_ctx *ctx);
+};
+
+/**
+ * struct img_ascii_lcd_ctx - Private data structure
+ * @pdev: the ASCII LCD platform device
+ * @base: the base address of the LCD registers
+ * @regmap: the regmap through which LCD registers are accessed
+ * @offset: the offset within regmap to the start of the LCD registers
+ * @cfg: pointer to the LCD model configuration
+ * @message: the full message to display or scroll on the LCD
+ * @message_len: the length of the @message string
+ * @scroll_pos: index of the first character of @message currently displayed
+ * @scroll_rate: scroll interval in jiffies
+ * @timer: timer used to implement scrolling
+ * @curr: the string currently displayed on the LCD
+ */
+struct img_ascii_lcd_ctx {
+	struct platform_device *pdev;
+	union {
+		void __iomem *base;
+		struct regmap *regmap;
+	};
+	u32 offset;
+	const struct img_ascii_lcd_config *cfg;
+	char *message;
+	unsigned int message_len;
+	unsigned int scroll_pos;
+	unsigned int scroll_rate;
+	struct timer_list timer;
+	char curr[] __aligned(8);
+};
+
+/*
+ * MIPS Boston development board
+ */
+
+static void boston_update(struct img_ascii_lcd_ctx *ctx)
+{
+	ulong val;
+
+#if BITS_PER_LONG == 64
+	val = *((u64 *)&ctx->curr[0]);
+	__raw_writeq(val, ctx->base);
+#elif BITS_PER_LONG == 32
+	val = *((u32 *)&ctx->curr[0]);
+	__raw_writel(val, ctx->base);
+	val = *((u32 *)&ctx->curr[4]);
+	__raw_writel(val, ctx->base + 4);
+#else
+# error Not 32 or 64 bit
+#endif
+}
+
+static struct img_ascii_lcd_config boston_config = {
+	.num_chars = 8,
+	.update = boston_update,
+};
+
+/*
+ * MIPS Malta development board
+ */
+
+static void malta_update(struct img_ascii_lcd_ctx *ctx)
+{
+	unsigned int i;
+	int err;
+
+	for (i = 0; i < ctx->cfg->num_chars; i++) {
+		err = regmap_write(ctx->regmap,
+				   ctx->offset + (i * 8), ctx->curr[i]);
+		if (err)
+			break;
+	}
+
+	if (unlikely(err))
+		pr_err_ratelimited("Failed to update LCD display: %d\n", err);
+}
+
+static struct img_ascii_lcd_config malta_config = {
+	.num_chars = 8,
+	.external_regmap = true,
+	.update = malta_update,
+};
+
+/*
+ * MIPS SEAD3 development board
+ */
+
+enum {
+	SEAD3_REG_LCD_CTRL		= 0x00,
+#define SEAD3_REG_LCD_CTRL_SETDRAM	BIT(7)
+	SEAD3_REG_LCD_DATA		= 0x08,
+	SEAD3_REG_CPLD_STATUS		= 0x10,
+#define SEAD3_REG_CPLD_STATUS_BUSY	BIT(0)
+	SEAD3_REG_CPLD_DATA		= 0x18,
+#define SEAD3_REG_CPLD_DATA_BUSY	BIT(7)
+};
+
+static int sead3_wait_sm_idle(struct img_ascii_lcd_ctx *ctx)
+{
+	unsigned int status;
+	int err;
+
+	do {
+		err = regmap_read(ctx->regmap,
+				  ctx->offset + SEAD3_REG_CPLD_STATUS,
+				  &status);
+		if (err)
+			return err;
+	} while (status & SEAD3_REG_CPLD_STATUS_BUSY);
+
+	return 0;
+
+}
+
+static int sead3_wait_lcd_idle(struct img_ascii_lcd_ctx *ctx)
+{
+	unsigned int cpld_data;
+	int err;
+
+	err = sead3_wait_sm_idle(ctx);
+	if (err)
+		return err;
+
+	do {
+		err = regmap_read(ctx->regmap,
+				  ctx->offset + SEAD3_REG_LCD_CTRL,
+				  &cpld_data);
+		if (err)
+			return err;
+
+		err = sead3_wait_sm_idle(ctx);
+		if (err)
+			return err;
+
+		err = regmap_read(ctx->regmap,
+				  ctx->offset + SEAD3_REG_CPLD_DATA,
+				  &cpld_data);
+		if (err)
+			return err;
+	} while (cpld_data & SEAD3_REG_CPLD_DATA_BUSY);
+
+	return 0;
+}
+
+static void sead3_update(struct img_ascii_lcd_ctx *ctx)
+{
+	unsigned int i;
+	int err;
+
+	for (i = 0; i < ctx->cfg->num_chars; i++) {
+		err = sead3_wait_lcd_idle(ctx);
+		if (err)
+			break;
+
+		err = regmap_write(ctx->regmap,
+				   ctx->offset + SEAD3_REG_LCD_CTRL,
+				   SEAD3_REG_LCD_CTRL_SETDRAM | i);
+		if (err)
+			break;
+
+		err = sead3_wait_lcd_idle(ctx);
+		if (err)
+			break;
+
+		err = regmap_write(ctx->regmap,
+				   ctx->offset + SEAD3_REG_LCD_DATA,
+				   ctx->curr[i]);
+		if (err)
+			break;
+	}
+
+	if (unlikely(err))
+		pr_err_ratelimited("Failed to update LCD display: %d\n", err);
+}
+
+static struct img_ascii_lcd_config sead3_config = {
+	.num_chars = 16,
+	.external_regmap = true,
+	.update = sead3_update,
+};
+
+static const struct of_device_id img_ascii_lcd_matches[] = {
+	{ .compatible = "img,boston-lcd", .data = &boston_config },
+	{ .compatible = "mti,malta-lcd", .data = &malta_config },
+	{ .compatible = "mti,sead3-lcd", .data = &sead3_config },
+};
+
+/**
+ * img_ascii_lcd_scroll() - scroll the display by a character
+ * @arg: really a pointer to the private data structure
+ *
+ * Scroll the current message along the LCD by one character, rearming the
+ * timer if required.
+ */
+static void img_ascii_lcd_scroll(unsigned long arg)
+{
+	struct img_ascii_lcd_ctx *ctx = (struct img_ascii_lcd_ctx *)arg;
+	unsigned int i, ch = ctx->scroll_pos;
+	unsigned int num_chars = ctx->cfg->num_chars;
+
+	/* update the current message string */
+	for (i = 0; i < num_chars;) {
+		/* copy as many characters from the string as possible */
+		for (; i < num_chars && ch < ctx->message_len; i++, ch++)
+			ctx->curr[i] = ctx->message[ch];
+
+		/* wrap around to the start of the string */
+		ch = 0;
+	}
+
+	/* update the LCD */
+	ctx->cfg->update(ctx);
+
+	/* move on to the next character */
+	ctx->scroll_pos++;
+	ctx->scroll_pos %= ctx->message_len;
+
+	/* rearm the timer */
+	if (ctx->message_len > ctx->cfg->num_chars)
+		mod_timer(&ctx->timer, jiffies + ctx->scroll_rate);
+}
+
+/**
+ * img_ascii_lcd_display() - set the message to be displayed
+ * @ctx: pointer to the private data structure
+ * @msg: the message to display
+ * @count: length of msg, or -1
+ *
+ * Display a new message @msg on the LCD. @msg can be longer than the number of
+ * characters the LCD can display, in which case it will begin scrolling across
+ * the LCD display.
+ *
+ * Return: 0 on success, -ENOMEM on memory allocation failure
+ */
+static int img_ascii_lcd_display(struct img_ascii_lcd_ctx *ctx,
+			     const char *msg, ssize_t count)
+{
+	char *new_msg;
+
+	/* stop the scroll timer */
+	del_timer_sync(&ctx->timer);
+
+	if (count == -1)
+		count = strlen(msg);
+
+	/* if the string ends with a newline, trim it */
+	if (msg[count - 1] == '\n')
+		count--;
+
+	new_msg = devm_kmalloc(&ctx->pdev->dev, count + 1, GFP_KERNEL);
+	if (!new_msg)
+		return -ENOMEM;
+
+	memcpy(new_msg, msg, count);
+	new_msg[count] = 0;
+
+	if (ctx->message)
+		devm_kfree(&ctx->pdev->dev, ctx->message);
+
+	ctx->message = new_msg;
+	ctx->message_len = count;
+	ctx->scroll_pos = 0;
+
+	/* update the LCD */
+	img_ascii_lcd_scroll((unsigned long)ctx);
+
+	return 0;
+}
+
+/**
+ * message_show() - read message via sysfs
+ * @dev: the LCD device
+ * @attr: the LCD message attribute
+ * @buf: the buffer to read the message into
+ *
+ * Read the current message being displayed or scrolled across the LCD display
+ * into @buf, for reads from sysfs.
+ *
+ * Return: the number of characters written to @buf
+ */
+static ssize_t message_show(struct device *dev, struct device_attribute *attr,
+			    char *buf)
+{
+	struct img_ascii_lcd_ctx *ctx = dev_get_drvdata(dev);
+
+	return sprintf(buf, "%s\n", ctx->message);
+}
+
+/**
+ * message_store() - write a new message via sysfs
+ * @dev: the LCD device
+ * @attr: the LCD message attribute
+ * @buf: the buffer containing the new message
+ * @count: the size of the message in @buf
+ *
+ * Write a new message to display or scroll across the LCD display from sysfs.
+ *
+ * Return: the size of the message on success, else -ERRNO
+ */
+static ssize_t message_store(struct device *dev, struct device_attribute *attr,
+			     const char *buf, size_t count)
+{
+	struct img_ascii_lcd_ctx *ctx = dev_get_drvdata(dev);
+	int err;
+
+	err = img_ascii_lcd_display(ctx, buf, count);
+	return err ?: count;
+}
+
+static DEVICE_ATTR_RW(message);
+
+/**
+ * img_ascii_lcd_probe() - probe an LCD display device
+ * @pdev: the LCD platform device
+ *
+ * Probe an LCD display device, ensuring that we have the required resources in
+ * order to access the LCD & setting up private data as well as sysfs files.
+ *
+ * Return: 0 on success, else -ERRNO
+ */
+static int img_ascii_lcd_probe(struct platform_device *pdev)
+{
+	const struct of_device_id *match;
+	const struct img_ascii_lcd_config *cfg;
+	struct img_ascii_lcd_ctx *ctx;
+	struct resource *res;
+	int err;
+
+	match = of_match_device(img_ascii_lcd_matches, &pdev->dev);
+	if (!match)
+		return -ENODEV;
+
+	cfg = match->data;
+	ctx = devm_kzalloc(&pdev->dev, sizeof(*ctx) + cfg->num_chars,
+			   GFP_KERNEL);
+	if (!ctx)
+		return -ENOMEM;
+
+	if (cfg->external_regmap) {
+		ctx->regmap = syscon_node_to_regmap(pdev->dev.parent->of_node);
+		if (IS_ERR(ctx->regmap))
+			return PTR_ERR(ctx->regmap);
+
+		if (of_property_read_u32(pdev->dev.of_node, "offset",
+					 &ctx->offset))
+			return -EINVAL;
+	} else {
+		res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+		ctx->base = devm_ioremap_resource(&pdev->dev, res);
+		if (IS_ERR(ctx->base))
+			return PTR_ERR(ctx->base);
+	}
+
+	ctx->pdev = pdev;
+	ctx->cfg = cfg;
+	ctx->message = NULL;
+	ctx->scroll_pos = 0;
+	ctx->scroll_rate = HZ / 2;
+
+	/* initialise a timer for scrolling the message */
+	init_timer(&ctx->timer);
+	ctx->timer.function = img_ascii_lcd_scroll;
+	ctx->timer.data = (unsigned long)ctx;
+
+	platform_set_drvdata(pdev, ctx);
+
+	/* display a default message */
+	err = img_ascii_lcd_display(ctx, "Linux " UTS_RELEASE "       ", -1);
+	if (err)
+		goto out_del_timer;
+
+	err = device_create_file(&pdev->dev, &dev_attr_message);
+	if (err)
+		goto out_del_timer;
+
+	return 0;
+out_del_timer:
+	del_timer_sync(&ctx->timer);
+	return err;
+}
+
+/**
+ * img_ascii_lcd_remove() - remove an LCD display device
+ * @pdev: the LCD platform device
+ *
+ * Remove an LCD display device, freeing private resources & ensuring that the
+ * driver stops using the LCD display registers.
+ *
+ * Return: 0
+ */
+static int img_ascii_lcd_remove(struct platform_device *pdev)
+{
+	struct img_ascii_lcd_ctx *ctx = platform_get_drvdata(pdev);
+
+	device_remove_file(&pdev->dev, &dev_attr_message);
+	del_timer_sync(&ctx->timer);
+	return 0;
+}
+
+static struct platform_driver img_ascii_lcd_driver = {
+	.driver = {
+		.name		= "img-ascii-lcd",
+		.of_match_table	= img_ascii_lcd_matches,
+	},
+	.probe	= img_ascii_lcd_probe,
+	.remove	= img_ascii_lcd_remove,
+};
+module_platform_driver(img_ascii_lcd_driver);
diff --git a/drivers/base/Kconfig b/drivers/base/Kconfig
index 98504ec99c7d..fdf44cac08e6 100644
--- a/drivers/base/Kconfig
+++ b/drivers/base/Kconfig
@@ -212,6 +212,16 @@ config DEBUG_DEVRES
 
 	  If you are unsure about this, Say N here.
 
+config DEBUG_TEST_DRIVER_REMOVE
+	bool "Test driver remove calls during probe"
+	depends on DEBUG_KERNEL
+	help
+	  Say Y here if you want the Driver core to test driver remove functions
+	  by calling probe, remove, probe. This tests the remove path without
+	  having to unbind the driver or unload the driver module.
+
+	  If you are unsure about this, say N here.
+
 config SYS_HYPERVISOR
 	bool
 	default n
diff --git a/drivers/base/attribute_container.c b/drivers/base/attribute_container.c
index 2ba4cac080c5..95e3ef82f3b7 100644
--- a/drivers/base/attribute_container.c
+++ b/drivers/base/attribute_container.c
@@ -243,7 +243,7 @@ attribute_container_remove_device(struct device *dev,
  * @dev:  The generic device to run the trigger for
  * @fn	  the function to execute for each classdev.
  *
- * This funcion is for executing a trigger when you need to know both
+ * This function is for executing a trigger when you need to know both
  * the container and the classdev.  If you only care about the
  * container, then use attribute_container_trigger() instead.
  */
diff --git a/drivers/base/core.c b/drivers/base/core.c
index 0a8bdade53f2..ce057a568673 100644
--- a/drivers/base/core.c
+++ b/drivers/base/core.c
@@ -836,11 +836,29 @@ static struct kobject *get_device_parent(struct device *dev,
 	return NULL;
 }
 
+static inline bool live_in_glue_dir(struct kobject *kobj,
+				    struct device *dev)
+{
+	if (!kobj || !dev->class ||
+	    kobj->kset != &dev->class->p->glue_dirs)
+		return false;
+	return true;
+}
+
+static inline struct kobject *get_glue_dir(struct device *dev)
+{
+	return dev->kobj.parent;
+}
+
+/*
+ * make sure cleaning up dir as the last step, we need to make
+ * sure .release handler of kobject is run with holding the
+ * global lock
+ */
 static void cleanup_glue_dir(struct device *dev, struct kobject *glue_dir)
 {
 	/* see if we live in a "glue" directory */
-	if (!glue_dir || !dev->class ||
-	    glue_dir->kset != &dev->class->p->glue_dirs)
+	if (!live_in_glue_dir(glue_dir, dev))
 		return;
 
 	mutex_lock(&gdp_mutex);
@@ -848,11 +866,6 @@ static void cleanup_glue_dir(struct device *dev, struct kobject *glue_dir)
 	mutex_unlock(&gdp_mutex);
 }
 
-static void cleanup_device_parent(struct device *dev)
-{
-	cleanup_glue_dir(dev, dev->kobj.parent);
-}
-
 static int device_add_class_symlinks(struct device *dev)
 {
 	struct device_node *of_node = dev_of_node(dev);
@@ -1028,6 +1041,7 @@ int device_add(struct device *dev)
 	struct kobject *kobj;
 	struct class_interface *class_intf;
 	int error = -EINVAL;
+	struct kobject *glue_dir = NULL;
 
 	dev = get_device(dev);
 	if (!dev)
@@ -1072,8 +1086,10 @@ int device_add(struct device *dev)
 	/* first, register with generic layer. */
 	/* we require the name to be set before, and pass NULL */
 	error = kobject_add(&dev->kobj, dev->kobj.parent, NULL);
-	if (error)
+	if (error) {
+		glue_dir = get_glue_dir(dev);
 		goto Error;
+	}
 
 	/* notify platform of device entry */
 	if (platform_notify)
@@ -1154,9 +1170,10 @@ done:
 	device_remove_file(dev, &dev_attr_uevent);
  attrError:
 	kobject_uevent(&dev->kobj, KOBJ_REMOVE);
+	glue_dir = get_glue_dir(dev);
 	kobject_del(&dev->kobj);
  Error:
-	cleanup_device_parent(dev);
+	cleanup_glue_dir(dev, glue_dir);
 	put_device(parent);
 name_error:
 	kfree(dev->p);
@@ -1232,6 +1249,7 @@ EXPORT_SYMBOL_GPL(put_device);
 void device_del(struct device *dev)
 {
 	struct device *parent = dev->parent;
+	struct kobject *glue_dir = NULL;
 	struct class_interface *class_intf;
 
 	/* Notify clients of device removal.  This call must come
@@ -1266,6 +1284,7 @@ void device_del(struct device *dev)
 	bus_remove_device(dev);
 	device_pm_remove(dev);
 	driver_deferred_probe_del(dev);
+	device_remove_properties(dev);
 
 	/* Notify the platform of the removal, in case they
 	 * need to do anything...
@@ -1276,8 +1295,9 @@ void device_del(struct device *dev)
 		blocking_notifier_call_chain(&dev->bus->p->bus_notifier,
 					     BUS_NOTIFY_REMOVED_DEVICE, dev);
 	kobject_uevent(&dev->kobj, KOBJ_REMOVE);
-	cleanup_device_parent(dev);
+	glue_dir = get_glue_dir(dev);
 	kobject_del(&dev->kobj);
+	cleanup_glue_dir(dev, glue_dir);
 	put_device(parent);
 }
 EXPORT_SYMBOL_GPL(device_del);
diff --git a/drivers/base/cpu.c b/drivers/base/cpu.c
index 691eeea2f19a..4c28e1a09786 100644
--- a/drivers/base/cpu.c
+++ b/drivers/base/cpu.c
@@ -371,12 +371,13 @@ int register_cpu(struct cpu *cpu, int num)
 	if (cpu->hotpluggable)
 		cpu->dev.groups = hotplugable_cpu_attr_groups;
 	error = device_register(&cpu->dev);
-	if (!error)
-		per_cpu(cpu_sys_devices, num) = &cpu->dev;
-	if (!error)
-		register_cpu_under_node(num, cpu_to_node(num));
+	if (error)
+		return error;
 
-	return error;
+	per_cpu(cpu_sys_devices, num) = &cpu->dev;
+	register_cpu_under_node(num, cpu_to_node(num));
+
+	return 0;
 }
 
 struct device *get_cpu_device(unsigned cpu)
diff --git a/drivers/base/dd.c b/drivers/base/dd.c
index 16688f50729c..d22a7260f42b 100644
--- a/drivers/base/dd.c
+++ b/drivers/base/dd.c
@@ -51,7 +51,6 @@
 static DEFINE_MUTEX(deferred_probe_mutex);
 static LIST_HEAD(deferred_probe_pending_list);
 static LIST_HEAD(deferred_probe_active_list);
-static struct workqueue_struct *deferred_wq;
 static atomic_t deferred_trigger_count = ATOMIC_INIT(0);
 
 /*
@@ -175,7 +174,7 @@ static void driver_deferred_probe_trigger(void)
 	 * Kick the re-probe thread.  It may already be scheduled, but it is
 	 * safe to kick it again.
 	 */
-	queue_work(deferred_wq, &deferred_probe_work);
+	schedule_work(&deferred_probe_work);
 }
 
 /**
@@ -211,14 +210,10 @@ void device_unblock_probing(void)
  */
 static int deferred_probe_initcall(void)
 {
-	deferred_wq = create_singlethread_workqueue("deferwq");
-	if (WARN_ON(!deferred_wq))
-		return -ENOMEM;
-
 	driver_deferred_probe_enable = true;
 	driver_deferred_probe_trigger();
 	/* Sort as many dependencies as possible before exiting initcalls */
-	flush_workqueue(deferred_wq);
+	flush_work(&deferred_probe_work);
 	return 0;
 }
 late_initcall(deferred_probe_initcall);
@@ -329,6 +324,7 @@ static int really_probe(struct device *dev, struct device_driver *drv)
 {
 	int ret = -EPROBE_DEFER;
 	int local_trigger_count = atomic_read(&deferred_trigger_count);
+	bool test_remove = IS_ENABLED(CONFIG_DEBUG_TEST_DRIVER_REMOVE);
 
 	if (defer_all_probes) {
 		/*
@@ -346,6 +342,7 @@ static int really_probe(struct device *dev, struct device_driver *drv)
 		 drv->bus->name, __func__, drv->name, dev_name(dev));
 	WARN_ON(!list_empty(&dev->devres_head));
 
+re_probe:
 	dev->driver = drv;
 
 	/* If using pinctrl, bind pins now before probing */
@@ -383,6 +380,25 @@ static int really_probe(struct device *dev, struct device_driver *drv)
 			goto probe_failed;
 	}
 
+	if (test_remove) {
+		test_remove = false;
+
+		if (dev->bus && dev->bus->remove)
+			dev->bus->remove(dev);
+		else if (drv->remove)
+			drv->remove(dev);
+
+		devres_release_all(dev);
+		driver_sysfs_remove(dev);
+		dev->driver = NULL;
+		dev_set_drvdata(dev, NULL);
+		if (dev->pm_domain && dev->pm_domain->dismiss)
+			dev->pm_domain->dismiss(dev);
+		pm_runtime_reinit(dev);
+
+		goto re_probe;
+	}
+
 	pinctrl_init_done(dev);
 
 	if (dev->pm_domain && dev->pm_domain->sync)
@@ -460,8 +476,7 @@ int driver_probe_done(void)
 void wait_for_device_probe(void)
 {
 	/* wait for the deferred probe workqueue to finish */
-	if (driver_deferred_probe_enable)
-		flush_workqueue(deferred_wq);
+	flush_work(&deferred_probe_work);
 
 	/* wait for the known devices to complete their probing */
 	wait_event(probe_waitqueue, atomic_read(&probe_count) == 0);
diff --git a/drivers/base/dma-coherent.c b/drivers/base/dma-coherent.c
index bdf28f7dd5e8..640a7e63c453 100644
--- a/drivers/base/dma-coherent.c
+++ b/drivers/base/dma-coherent.c
@@ -165,6 +165,7 @@ int dma_alloc_from_coherent(struct device *dev, ssize_t size,
 	int order = get_order(size);
 	unsigned long flags;
 	int pageno;
+	int dma_memory_map;
 
 	if (!dev)
 		return 0;
@@ -187,11 +188,12 @@ int dma_alloc_from_coherent(struct device *dev, ssize_t size,
 	 */
 	*dma_handle = mem->device_base + (pageno << PAGE_SHIFT);
 	*ret = mem->virt_base + (pageno << PAGE_SHIFT);
-	if (mem->flags & DMA_MEMORY_MAP)
+	dma_memory_map = (mem->flags & DMA_MEMORY_MAP);
+	spin_unlock_irqrestore(&mem->spinlock, flags);
+	if (dma_memory_map)
 		memset(*ret, 0, size);
 	else
 		memset_io(*ret, 0, size);
-	spin_unlock_irqrestore(&mem->spinlock, flags);
 
 	return 1;
 
@@ -261,8 +263,8 @@ int dma_mmap_from_coherent(struct device *dev, struct vm_area_struct *vma,
 		   (mem->virt_base + (mem->size << PAGE_SHIFT))) {
 		unsigned long off = vma->vm_pgoff;
 		int start = (vaddr - mem->virt_base) >> PAGE_SHIFT;
-		int user_count = (vma->vm_end - vma->vm_start) >> PAGE_SHIFT;
-		int count = size >> PAGE_SHIFT;
+		int user_count = vma_pages(vma);
+		int count = PAGE_ALIGN(size) >> PAGE_SHIFT;
 
 		*ret = -ENXIO;
 		if (off < count && user_count <= count - off) {
diff --git a/drivers/base/dma-mapping.c b/drivers/base/dma-mapping.c
index d799662f19eb..8f8b68c80986 100644
--- a/drivers/base/dma-mapping.c
+++ b/drivers/base/dma-mapping.c
@@ -198,10 +198,13 @@ int dmam_declare_coherent_memory(struct device *dev, phys_addr_t phys_addr,
 
 	rc = dma_declare_coherent_memory(dev, phys_addr, device_addr, size,
 					 flags);
-	if (rc == 0)
+	if (rc) {
 		devres_add(dev, res);
-	else
+		rc = 0;
+	} else {
 		devres_free(res);
+		rc = -ENOMEM;
+	}
 
 	return rc;
 }
@@ -247,7 +250,7 @@ int dma_common_mmap(struct device *dev, struct vm_area_struct *vma,
 {
 	int ret = -ENXIO;
 #if defined(CONFIG_MMU) && !defined(CONFIG_ARCH_NO_COHERENT_DMA_MMAP)
-	unsigned long user_count = (vma->vm_end - vma->vm_start) >> PAGE_SHIFT;
+	unsigned long user_count = vma_pages(vma);
 	unsigned long count = PAGE_ALIGN(size) >> PAGE_SHIFT;
 	unsigned long pfn = page_to_pfn(virt_to_page(cpu_addr));
 	unsigned long off = vma->vm_pgoff;
@@ -334,7 +337,7 @@ void dma_common_free_remap(void *cpu_addr, size_t size, unsigned long vm_flags)
 		return;
 	}
 
-	unmap_kernel_range((unsigned long)cpu_addr, size);
+	unmap_kernel_range((unsigned long)cpu_addr, PAGE_ALIGN(size));
 	vunmap(cpu_addr);
 }
 #endif
diff --git a/drivers/base/memory.c b/drivers/base/memory.c
index dc75de9059cd..62c63c0c5c22 100644
--- a/drivers/base/memory.c
+++ b/drivers/base/memory.c
@@ -361,8 +361,11 @@ store_mem_state(struct device *dev,
 err:
 	unlock_device_hotplug();
 
-	if (ret)
+	if (ret < 0)
 		return ret;
+	if (ret)
+		return -EINVAL;
+
 	return count;
 }
 
diff --git a/drivers/base/pinctrl.c b/drivers/base/pinctrl.c
index 076297592754..5917b4b5fb99 100644
--- a/drivers/base/pinctrl.c
+++ b/drivers/base/pinctrl.c
@@ -91,9 +91,13 @@ cleanup_alloc:
 	devm_kfree(dev, dev->pins);
 	dev->pins = NULL;
 
-	/* Only return deferrals */
-	if (ret != -EPROBE_DEFER)
-		ret = 0;
+	/* Return deferrals */
+	if (ret == -EPROBE_DEFER)
+		return ret;
+	/* Return serious errors */
+	if (ret == -EINVAL)
+		return ret;
+	/* We ignore errors like -ENOENT meaning no pinctrl state */
 
-	return ret;
+	return 0;
 }
diff --git a/drivers/base/platform-msi.c b/drivers/base/platform-msi.c
index 279e53989374..be6a599bc0c1 100644
--- a/drivers/base/platform-msi.c
+++ b/drivers/base/platform-msi.c
@@ -142,13 +142,12 @@ static int platform_msi_alloc_descs_with_irq(struct device *dev, int virq,
 	}
 
 	for (i = 0; i < nvec; i++) {
-		desc = alloc_msi_entry(dev);
+		desc = alloc_msi_entry(dev, 1, NULL);
 		if (!desc)
 			break;
 
 		desc->platform.msi_priv_data = data;
 		desc->platform.msi_index = base + i;
-		desc->nvec_used = 1;
 		desc->irq = virq ? virq + i : 0;
 
 		list_add_tail(&desc->list, dev_to_msi_list(dev));
diff --git a/drivers/base/platform.c b/drivers/base/platform.c
index 6482d47deb50..c4af00385502 100644
--- a/drivers/base/platform.c
+++ b/drivers/base/platform.c
@@ -97,7 +97,7 @@ int platform_get_irq(struct platform_device *dev, unsigned int num)
 		int ret;
 
 		ret = of_irq_get(dev->dev.of_node, num);
-		if (ret >= 0 || ret == -EPROBE_DEFER)
+		if (ret > 0 || ret == -EPROBE_DEFER)
 			return ret;
 	}
 
@@ -108,9 +108,14 @@ int platform_get_irq(struct platform_device *dev, unsigned int num)
 	 * IORESOURCE_BITS correspond 1-to-1 to the IRQF_TRIGGER*
 	 * settings.
 	 */
-	if (r && r->flags & IORESOURCE_BITS)
-		irqd_set_trigger_type(irq_get_irq_data(r->start),
-				      r->flags & IORESOURCE_BITS);
+	if (r && r->flags & IORESOURCE_BITS) {
+		struct irq_data *irqd;
+
+		irqd = irq_get_irq_data(r->start);
+		if (!irqd)
+			return -ENXIO;
+		irqd_set_trigger_type(irqd, r->flags & IORESOURCE_BITS);
+	}
 
 	return r ? r->start : -ENXIO;
 #endif
@@ -175,7 +180,7 @@ int platform_get_irq_byname(struct platform_device *dev, const char *name)
 		int ret;
 
 		ret = of_irq_get_byname(dev->dev.of_node, name);
-		if (ret >= 0 || ret == -EPROBE_DEFER)
+		if (ret > 0 || ret == -EPROBE_DEFER)
 			return ret;
 	}
 
@@ -434,6 +439,7 @@ void platform_device_del(struct platform_device *pdev)
 	int i;
 
 	if (pdev) {
+		device_remove_properties(&pdev->dev);
 		device_del(&pdev->dev);
 
 		if (pdev->id_auto) {
@@ -446,8 +452,6 @@ void platform_device_del(struct platform_device *pdev)
 			if (r->parent)
 				release_resource(r);
 		}
-
-		device_remove_properties(&pdev->dev);
 	}
 }
 EXPORT_SYMBOL_GPL(platform_device_del);
diff --git a/drivers/base/power/domain.c b/drivers/base/power/domain.c
index a1f2aff33997..e023066e4215 100644
--- a/drivers/base/power/domain.c
+++ b/drivers/base/power/domain.c
@@ -45,7 +45,7 @@ static DEFINE_MUTEX(gpd_list_lock);
  * and checks that the PM domain pointer is a real generic PM domain.
  * Any failure results in NULL being returned.
  */
-struct generic_pm_domain *pm_genpd_lookup_dev(struct device *dev)
+static struct generic_pm_domain *genpd_lookup_dev(struct device *dev)
 {
 	struct generic_pm_domain *genpd = NULL, *gpd;
 
@@ -586,7 +586,7 @@ static int __init genpd_poweroff_unused(void)
 }
 late_initcall(genpd_poweroff_unused);
 
-#ifdef CONFIG_PM_SLEEP
+#if defined(CONFIG_PM_SLEEP) || defined(CONFIG_PM_GENERIC_DOMAINS_OF)
 
 /**
  * pm_genpd_present - Check if the given PM domain has been initialized.
@@ -606,6 +606,10 @@ static bool pm_genpd_present(const struct generic_pm_domain *genpd)
 	return false;
 }
 
+#endif
+
+#ifdef CONFIG_PM_SLEEP
+
 static bool genpd_dev_active_wakeup(struct generic_pm_domain *genpd,
 				    struct device *dev)
 {
@@ -613,9 +617,8 @@ 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_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.
@@ -625,8 +628,7 @@ 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,
-				   bool timed)
+static void genpd_sync_poweroff(struct generic_pm_domain *genpd)
 {
 	struct gpd_link *link;
 
@@ -639,28 +641,26 @@ static void pm_genpd_sync_poweroff(struct generic_pm_domain *genpd,
 
 	/* Choose the deepest state when suspending */
 	genpd->state_idx = genpd->state_count - 1;
-	genpd_power_off(genpd, timed);
+	genpd_power_off(genpd, false);
 
 	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, timed);
+		genpd_sync_poweroff(link->master);
 	}
 }
 
 /**
- * pm_genpd_sync_poweron - Synchronously power on a PM domain and its masters.
+ * 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,
-				  bool timed)
+static void genpd_sync_poweron(struct generic_pm_domain *genpd)
 {
 	struct gpd_link *link;
 
@@ -668,11 +668,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, timed);
+		genpd_sync_poweron(link->master);
 		genpd_sd_counter_inc(link->master);
 	}
 
-	genpd_power_on(genpd, timed);
+	genpd_power_on(genpd, false);
 
 	genpd->status = GPD_STATE_ACTIVE;
 }
@@ -784,7 +784,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, true);
+	genpd_sync_poweroff(genpd);
 
 	return 0;
 }
@@ -814,7 +814,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, true);
+	genpd_sync_poweron(genpd);
 	genpd->suspended_count--;
 
 	if (genpd->dev_ops.stop && genpd->dev_ops.start)
@@ -902,12 +902,12 @@ static int pm_genpd_restore_noirq(struct device *dev)
 	if (genpd->suspended_count++ == 0)
 		/*
 		 * The boot kernel might put the domain into arbitrary state,
-		 * so make it appear as powered off to pm_genpd_sync_poweron(),
+		 * so make it appear as powered off to genpd_sync_poweron(),
 		 * so that it tries to power it on in case it was really off.
 		 */
 		genpd->status = GPD_STATE_POWER_OFF;
 
-	pm_genpd_sync_poweron(genpd, true);
+	genpd_sync_poweron(genpd);
 
 	if (genpd->dev_ops.stop && genpd->dev_ops.start)
 		ret = pm_runtime_force_resume(dev);
@@ -962,9 +962,9 @@ static void genpd_syscore_switch(struct device *dev, bool suspend)
 
 	if (suspend) {
 		genpd->suspended_count++;
-		pm_genpd_sync_poweroff(genpd, false);
+		genpd_sync_poweroff(genpd);
 	} else {
-		pm_genpd_sync_poweron(genpd, false);
+		genpd_sync_poweron(genpd);
 		genpd->suspended_count--;
 	}
 }
@@ -1056,14 +1056,8 @@ static void genpd_free_dev_data(struct device *dev,
 	dev_pm_put_subsys_data(dev);
 }
 
-/**
- * __pm_genpd_add_device - Add a device to an I/O PM domain.
- * @genpd: PM domain to add the device to.
- * @dev: Device to be added.
- * @td: Set of PM QoS timing parameters to attach to the device.
- */
-int __pm_genpd_add_device(struct generic_pm_domain *genpd, struct device *dev,
-			  struct gpd_timing_data *td)
+static int genpd_add_device(struct generic_pm_domain *genpd, struct device *dev,
+			    struct gpd_timing_data *td)
 {
 	struct generic_pm_domain_data *gpd_data;
 	int ret = 0;
@@ -1103,15 +1097,28 @@ int __pm_genpd_add_device(struct generic_pm_domain *genpd, struct device *dev,
 
 	return ret;
 }
-EXPORT_SYMBOL_GPL(__pm_genpd_add_device);
 
 /**
- * pm_genpd_remove_device - Remove a device from an I/O PM domain.
- * @genpd: PM domain to remove the device from.
- * @dev: Device to be removed.
+ * __pm_genpd_add_device - Add a device to an I/O PM domain.
+ * @genpd: PM domain to add the device to.
+ * @dev: Device to be added.
+ * @td: Set of PM QoS timing parameters to attach to the device.
  */
-int pm_genpd_remove_device(struct generic_pm_domain *genpd,
-			   struct device *dev)
+int __pm_genpd_add_device(struct generic_pm_domain *genpd, struct device *dev,
+			  struct gpd_timing_data *td)
+{
+	int ret;
+
+	mutex_lock(&gpd_list_lock);
+	ret = genpd_add_device(genpd, dev, td);
+	mutex_unlock(&gpd_list_lock);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(__pm_genpd_add_device);
+
+static int genpd_remove_device(struct generic_pm_domain *genpd,
+			       struct device *dev)
 {
 	struct generic_pm_domain_data *gpd_data;
 	struct pm_domain_data *pdd;
@@ -1119,10 +1126,6 @@ int pm_genpd_remove_device(struct generic_pm_domain *genpd,
 
 	dev_dbg(dev, "%s()\n", __func__);
 
-	if (!genpd || genpd != pm_genpd_lookup_dev(dev))
-		return -EINVAL;
-
-	/* The above validation also means we have existing domain_data. */
 	pdd = dev->power.subsys_data->domain_data;
 	gpd_data = to_gpd_data(pdd);
 	dev_pm_qos_remove_notifier(dev, &gpd_data->nb);
@@ -1154,15 +1157,24 @@ int pm_genpd_remove_device(struct generic_pm_domain *genpd,
 
 	return ret;
 }
-EXPORT_SYMBOL_GPL(pm_genpd_remove_device);
 
 /**
- * pm_genpd_add_subdomain - Add a subdomain to an I/O PM domain.
- * @genpd: Master PM domain to add the subdomain to.
- * @subdomain: Subdomain to be added.
+ * pm_genpd_remove_device - Remove a device from an I/O PM domain.
+ * @genpd: PM domain to remove the device from.
+ * @dev: Device to be removed.
  */
-int pm_genpd_add_subdomain(struct generic_pm_domain *genpd,
-			   struct generic_pm_domain *subdomain)
+int pm_genpd_remove_device(struct generic_pm_domain *genpd,
+			   struct device *dev)
+{
+	if (!genpd || genpd != genpd_lookup_dev(dev))
+		return -EINVAL;
+
+	return genpd_remove_device(genpd, dev);
+}
+EXPORT_SYMBOL_GPL(pm_genpd_remove_device);
+
+static int genpd_add_subdomain(struct generic_pm_domain *genpd,
+			       struct generic_pm_domain *subdomain)
 {
 	struct gpd_link *link, *itr;
 	int ret = 0;
@@ -1205,6 +1217,23 @@ int pm_genpd_add_subdomain(struct generic_pm_domain *genpd,
 		kfree(link);
 	return ret;
 }
+
+/**
+ * pm_genpd_add_subdomain - Add a subdomain to an I/O PM domain.
+ * @genpd: Master PM domain to add the subdomain to.
+ * @subdomain: Subdomain to be added.
+ */
+int pm_genpd_add_subdomain(struct generic_pm_domain *genpd,
+			   struct generic_pm_domain *subdomain)
+{
+	int ret;
+
+	mutex_lock(&gpd_list_lock);
+	ret = genpd_add_subdomain(genpd, subdomain);
+	mutex_unlock(&gpd_list_lock);
+
+	return ret;
+}
 EXPORT_SYMBOL_GPL(pm_genpd_add_subdomain);
 
 /**
@@ -1278,27 +1307,17 @@ int pm_genpd_init(struct generic_pm_domain *genpd,
 	genpd->device_count = 0;
 	genpd->max_off_time_ns = -1;
 	genpd->max_off_time_changed = true;
+	genpd->provider = NULL;
+	genpd->has_provider = false;
 	genpd->domain.ops.runtime_suspend = genpd_runtime_suspend;
 	genpd->domain.ops.runtime_resume = genpd_runtime_resume;
 	genpd->domain.ops.prepare = pm_genpd_prepare;
-	genpd->domain.ops.suspend = pm_generic_suspend;
-	genpd->domain.ops.suspend_late = pm_generic_suspend_late;
 	genpd->domain.ops.suspend_noirq = pm_genpd_suspend_noirq;
 	genpd->domain.ops.resume_noirq = pm_genpd_resume_noirq;
-	genpd->domain.ops.resume_early = pm_generic_resume_early;
-	genpd->domain.ops.resume = pm_generic_resume;
-	genpd->domain.ops.freeze = pm_generic_freeze;
-	genpd->domain.ops.freeze_late = pm_generic_freeze_late;
 	genpd->domain.ops.freeze_noirq = pm_genpd_freeze_noirq;
 	genpd->domain.ops.thaw_noirq = pm_genpd_thaw_noirq;
-	genpd->domain.ops.thaw_early = pm_generic_thaw_early;
-	genpd->domain.ops.thaw = pm_generic_thaw;
-	genpd->domain.ops.poweroff = pm_generic_poweroff;
-	genpd->domain.ops.poweroff_late = pm_generic_poweroff_late;
 	genpd->domain.ops.poweroff_noirq = pm_genpd_suspend_noirq;
 	genpd->domain.ops.restore_noirq = pm_genpd_restore_noirq;
-	genpd->domain.ops.restore_early = pm_generic_restore_early;
-	genpd->domain.ops.restore = pm_generic_restore;
 	genpd->domain.ops.complete = pm_genpd_complete;
 
 	if (genpd->flags & GENPD_FLAG_PM_CLK) {
@@ -1328,7 +1347,71 @@ int pm_genpd_init(struct generic_pm_domain *genpd,
 }
 EXPORT_SYMBOL_GPL(pm_genpd_init);
 
+static int genpd_remove(struct generic_pm_domain *genpd)
+{
+	struct gpd_link *l, *link;
+
+	if (IS_ERR_OR_NULL(genpd))
+		return -EINVAL;
+
+	mutex_lock(&genpd->lock);
+
+	if (genpd->has_provider) {
+		mutex_unlock(&genpd->lock);
+		pr_err("Provider present, unable to remove %s\n", genpd->name);
+		return -EBUSY;
+	}
+
+	if (!list_empty(&genpd->master_links) || genpd->device_count) {
+		mutex_unlock(&genpd->lock);
+		pr_err("%s: unable to remove %s\n", __func__, genpd->name);
+		return -EBUSY;
+	}
+
+	list_for_each_entry_safe(link, l, &genpd->slave_links, slave_node) {
+		list_del(&link->master_node);
+		list_del(&link->slave_node);
+		kfree(link);
+	}
+
+	list_del(&genpd->gpd_list_node);
+	mutex_unlock(&genpd->lock);
+	cancel_work_sync(&genpd->power_off_work);
+	pr_debug("%s: removed %s\n", __func__, genpd->name);
+
+	return 0;
+}
+
+/**
+ * pm_genpd_remove - Remove a generic I/O PM domain
+ * @genpd: Pointer to PM domain that is to be removed.
+ *
+ * To remove the PM domain, this function:
+ *  - Removes the PM domain as a subdomain to any parent domains,
+ *    if it was added.
+ *  - Removes the PM domain from the list of registered PM domains.
+ *
+ * The PM domain will only be removed, if the associated provider has
+ * been removed, it is not a parent to any other PM domain and has no
+ * devices associated with it.
+ */
+int pm_genpd_remove(struct generic_pm_domain *genpd)
+{
+	int ret;
+
+	mutex_lock(&gpd_list_lock);
+	ret = genpd_remove(genpd);
+	mutex_unlock(&gpd_list_lock);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(pm_genpd_remove);
+
 #ifdef CONFIG_PM_GENERIC_DOMAINS_OF
+
+typedef struct generic_pm_domain *(*genpd_xlate_t)(struct of_phandle_args *args,
+						   void *data);
+
 /*
  * Device Tree based PM domain providers.
  *
@@ -1340,8 +1423,8 @@ EXPORT_SYMBOL_GPL(pm_genpd_init);
  * maps a PM domain specifier retrieved from the device tree to a PM domain.
  *
  * Two simple mapping functions have been provided for convenience:
- *  - __of_genpd_xlate_simple() for 1:1 device tree node to PM domain mapping.
- *  - __of_genpd_xlate_onecell() for mapping of multiple PM domains per node by
+ *  - genpd_xlate_simple() for 1:1 device tree node to PM domain mapping.
+ *  - genpd_xlate_onecell() for mapping of multiple PM domains per node by
  *    index.
  */
 
@@ -1366,7 +1449,7 @@ static LIST_HEAD(of_genpd_providers);
 static DEFINE_MUTEX(of_genpd_mutex);
 
 /**
- * __of_genpd_xlate_simple() - Xlate function for direct node-domain mapping
+ * genpd_xlate_simple() - Xlate function for direct node-domain mapping
  * @genpdspec: OF phandle args to map into a PM domain
  * @data: xlate function private data - pointer to struct generic_pm_domain
  *
@@ -1374,7 +1457,7 @@ static DEFINE_MUTEX(of_genpd_mutex);
  * have their own device tree nodes. The private data of xlate function needs
  * to be a valid pointer to struct generic_pm_domain.
  */
-struct generic_pm_domain *__of_genpd_xlate_simple(
+static struct generic_pm_domain *genpd_xlate_simple(
 					struct of_phandle_args *genpdspec,
 					void *data)
 {
@@ -1382,10 +1465,9 @@ struct generic_pm_domain *__of_genpd_xlate_simple(
 		return ERR_PTR(-EINVAL);
 	return data;
 }
-EXPORT_SYMBOL_GPL(__of_genpd_xlate_simple);
 
 /**
- * __of_genpd_xlate_onecell() - Xlate function using a single index.
+ * genpd_xlate_onecell() - Xlate function using a single index.
  * @genpdspec: OF phandle args to map into a PM domain
  * @data: xlate function private data - pointer to struct genpd_onecell_data
  *
@@ -1394,7 +1476,7 @@ EXPORT_SYMBOL_GPL(__of_genpd_xlate_simple);
  * A single cell is used as an index into an array of PM domains specified in
  * the genpd_onecell_data struct when registering the provider.
  */
-struct generic_pm_domain *__of_genpd_xlate_onecell(
+static struct generic_pm_domain *genpd_xlate_onecell(
 					struct of_phandle_args *genpdspec,
 					void *data)
 {
@@ -1414,16 +1496,15 @@ struct generic_pm_domain *__of_genpd_xlate_onecell(
 
 	return genpd_data->domains[idx];
 }
-EXPORT_SYMBOL_GPL(__of_genpd_xlate_onecell);
 
 /**
- * __of_genpd_add_provider() - Register a PM domain provider for a node
+ * genpd_add_provider() - Register a PM domain provider for a node
  * @np: Device node pointer associated with the PM domain provider.
  * @xlate: Callback for decoding PM domain from phandle arguments.
  * @data: Context pointer for @xlate callback.
  */
-int __of_genpd_add_provider(struct device_node *np, genpd_xlate_t xlate,
-			void *data)
+static int genpd_add_provider(struct device_node *np, genpd_xlate_t xlate,
+			      void *data)
 {
 	struct of_genpd_provider *cp;
 
@@ -1442,7 +1523,83 @@ int __of_genpd_add_provider(struct device_node *np, genpd_xlate_t xlate,
 
 	return 0;
 }
-EXPORT_SYMBOL_GPL(__of_genpd_add_provider);
+
+/**
+ * of_genpd_add_provider_simple() - Register a simple PM domain provider
+ * @np: Device node pointer associated with the PM domain provider.
+ * @genpd: Pointer to PM domain associated with the PM domain provider.
+ */
+int of_genpd_add_provider_simple(struct device_node *np,
+				 struct generic_pm_domain *genpd)
+{
+	int ret = -EINVAL;
+
+	if (!np || !genpd)
+		return -EINVAL;
+
+	mutex_lock(&gpd_list_lock);
+
+	if (pm_genpd_present(genpd))
+		ret = genpd_add_provider(np, genpd_xlate_simple, genpd);
+
+	if (!ret) {
+		genpd->provider = &np->fwnode;
+		genpd->has_provider = true;
+	}
+
+	mutex_unlock(&gpd_list_lock);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(of_genpd_add_provider_simple);
+
+/**
+ * of_genpd_add_provider_onecell() - Register a onecell PM domain provider
+ * @np: Device node pointer associated with the PM domain provider.
+ * @data: Pointer to the data associated with the PM domain provider.
+ */
+int of_genpd_add_provider_onecell(struct device_node *np,
+				  struct genpd_onecell_data *data)
+{
+	unsigned int i;
+	int ret = -EINVAL;
+
+	if (!np || !data)
+		return -EINVAL;
+
+	mutex_lock(&gpd_list_lock);
+
+	for (i = 0; i < data->num_domains; i++) {
+		if (!data->domains[i])
+			continue;
+		if (!pm_genpd_present(data->domains[i]))
+			goto error;
+
+		data->domains[i]->provider = &np->fwnode;
+		data->domains[i]->has_provider = true;
+	}
+
+	ret = genpd_add_provider(np, genpd_xlate_onecell, data);
+	if (ret < 0)
+		goto error;
+
+	mutex_unlock(&gpd_list_lock);
+
+	return 0;
+
+error:
+	while (i--) {
+		if (!data->domains[i])
+			continue;
+		data->domains[i]->provider = NULL;
+		data->domains[i]->has_provider = false;
+	}
+
+	mutex_unlock(&gpd_list_lock);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(of_genpd_add_provider_onecell);
 
 /**
  * of_genpd_del_provider() - Remove a previously registered PM domain provider
@@ -1451,10 +1608,21 @@ EXPORT_SYMBOL_GPL(__of_genpd_add_provider);
 void of_genpd_del_provider(struct device_node *np)
 {
 	struct of_genpd_provider *cp;
+	struct generic_pm_domain *gpd;
 
+	mutex_lock(&gpd_list_lock);
 	mutex_lock(&of_genpd_mutex);
 	list_for_each_entry(cp, &of_genpd_providers, link) {
 		if (cp->node == np) {
+			/*
+			 * For each PM domain associated with the
+			 * provider, set the 'has_provider' to false
+			 * so that the PM domain can be safely removed.
+			 */
+			list_for_each_entry(gpd, &gpd_list, gpd_list_node)
+				if (gpd->provider == &np->fwnode)
+					gpd->has_provider = false;
+
 			list_del(&cp->link);
 			of_node_put(cp->node);
 			kfree(cp);
@@ -1462,11 +1630,12 @@ void of_genpd_del_provider(struct device_node *np)
 		}
 	}
 	mutex_unlock(&of_genpd_mutex);
+	mutex_unlock(&gpd_list_lock);
 }
 EXPORT_SYMBOL_GPL(of_genpd_del_provider);
 
 /**
- * of_genpd_get_from_provider() - Look-up PM domain
+ * genpd_get_from_provider() - Look-up PM domain
  * @genpdspec: OF phandle args to use for look-up
  *
  * Looks for a PM domain provider under the node specified by @genpdspec and if
@@ -1476,7 +1645,7 @@ EXPORT_SYMBOL_GPL(of_genpd_del_provider);
  * Returns a valid pointer to struct generic_pm_domain on success or ERR_PTR()
  * on failure.
  */
-struct generic_pm_domain *of_genpd_get_from_provider(
+static struct generic_pm_domain *genpd_get_from_provider(
 					struct of_phandle_args *genpdspec)
 {
 	struct generic_pm_domain *genpd = ERR_PTR(-ENOENT);
@@ -1499,7 +1668,109 @@ struct generic_pm_domain *of_genpd_get_from_provider(
 
 	return genpd;
 }
-EXPORT_SYMBOL_GPL(of_genpd_get_from_provider);
+
+/**
+ * of_genpd_add_device() - Add a device to an I/O PM domain
+ * @genpdspec: OF phandle args to use for look-up PM domain
+ * @dev: Device to be added.
+ *
+ * Looks-up an I/O PM domain based upon phandle args provided and adds
+ * the device to the PM domain. Returns a negative error code on failure.
+ */
+int of_genpd_add_device(struct of_phandle_args *genpdspec, struct device *dev)
+{
+	struct generic_pm_domain *genpd;
+	int ret;
+
+	mutex_lock(&gpd_list_lock);
+
+	genpd = genpd_get_from_provider(genpdspec);
+	if (IS_ERR(genpd)) {
+		ret = PTR_ERR(genpd);
+		goto out;
+	}
+
+	ret = genpd_add_device(genpd, dev, NULL);
+
+out:
+	mutex_unlock(&gpd_list_lock);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(of_genpd_add_device);
+
+/**
+ * of_genpd_add_subdomain - Add a subdomain to an I/O PM domain.
+ * @parent_spec: OF phandle args to use for parent PM domain look-up
+ * @subdomain_spec: OF phandle args to use for subdomain look-up
+ *
+ * Looks-up a parent PM domain and subdomain based upon phandle args
+ * provided and adds the subdomain to the parent PM domain. Returns a
+ * negative error code on failure.
+ */
+int of_genpd_add_subdomain(struct of_phandle_args *parent_spec,
+			   struct of_phandle_args *subdomain_spec)
+{
+	struct generic_pm_domain *parent, *subdomain;
+	int ret;
+
+	mutex_lock(&gpd_list_lock);
+
+	parent = genpd_get_from_provider(parent_spec);
+	if (IS_ERR(parent)) {
+		ret = PTR_ERR(parent);
+		goto out;
+	}
+
+	subdomain = genpd_get_from_provider(subdomain_spec);
+	if (IS_ERR(subdomain)) {
+		ret = PTR_ERR(subdomain);
+		goto out;
+	}
+
+	ret = genpd_add_subdomain(parent, subdomain);
+
+out:
+	mutex_unlock(&gpd_list_lock);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(of_genpd_add_subdomain);
+
+/**
+ * of_genpd_remove_last - Remove the last PM domain registered for a provider
+ * @provider: Pointer to device structure associated with provider
+ *
+ * Find the last PM domain that was added by a particular provider and
+ * remove this PM domain from the list of PM domains. The provider is
+ * identified by the 'provider' device structure that is passed. The PM
+ * domain will only be removed, if the provider associated with domain
+ * has been removed.
+ *
+ * Returns a valid pointer to struct generic_pm_domain on success or
+ * ERR_PTR() on failure.
+ */
+struct generic_pm_domain *of_genpd_remove_last(struct device_node *np)
+{
+	struct generic_pm_domain *gpd, *genpd = ERR_PTR(-ENOENT);
+	int ret;
+
+	if (IS_ERR_OR_NULL(np))
+		return ERR_PTR(-EINVAL);
+
+	mutex_lock(&gpd_list_lock);
+	list_for_each_entry(gpd, &gpd_list, gpd_list_node) {
+		if (gpd->provider == &np->fwnode) {
+			ret = genpd_remove(gpd);
+			genpd = ret ? ERR_PTR(ret) : gpd;
+			break;
+		}
+	}
+	mutex_unlock(&gpd_list_lock);
+
+	return genpd;
+}
+EXPORT_SYMBOL_GPL(of_genpd_remove_last);
 
 /**
  * genpd_dev_pm_detach - Detach a device from its PM domain.
@@ -1515,14 +1786,14 @@ static void genpd_dev_pm_detach(struct device *dev, bool power_off)
 	unsigned int i;
 	int ret = 0;
 
-	pd = pm_genpd_lookup_dev(dev);
-	if (!pd)
+	pd = dev_to_genpd(dev);
+	if (IS_ERR(pd))
 		return;
 
 	dev_dbg(dev, "removing from PM domain %s\n", pd->name);
 
 	for (i = 1; i < GENPD_RETRY_MAX_MS; i <<= 1) {
-		ret = pm_genpd_remove_device(pd, dev);
+		ret = genpd_remove_device(pd, dev);
 		if (ret != -EAGAIN)
 			break;
 
@@ -1596,9 +1867,11 @@ int genpd_dev_pm_attach(struct device *dev)
 			return -ENOENT;
 	}
 
-	pd = of_genpd_get_from_provider(&pd_args);
+	mutex_lock(&gpd_list_lock);
+	pd = genpd_get_from_provider(&pd_args);
 	of_node_put(pd_args.np);
 	if (IS_ERR(pd)) {
+		mutex_unlock(&gpd_list_lock);
 		dev_dbg(dev, "%s() failed to find PM domain: %ld\n",
 			__func__, PTR_ERR(pd));
 		return -EPROBE_DEFER;
@@ -1607,13 +1880,14 @@ int genpd_dev_pm_attach(struct device *dev)
 	dev_dbg(dev, "adding to PM domain %s\n", pd->name);
 
 	for (i = 1; i < GENPD_RETRY_MAX_MS; i <<= 1) {
-		ret = pm_genpd_add_device(pd, dev);
+		ret = genpd_add_device(pd, dev, NULL);
 		if (ret != -EAGAIN)
 			break;
 
 		mdelay(i);
 		cond_resched();
 	}
+	mutex_unlock(&gpd_list_lock);
 
 	if (ret < 0) {
 		dev_err(dev, "failed to add to PM domain %s: %d",
@@ -1636,7 +1910,7 @@ EXPORT_SYMBOL_GPL(genpd_dev_pm_attach);
 
 /***        debugfs support        ***/
 
-#ifdef CONFIG_PM_ADVANCED_DEBUG
+#ifdef CONFIG_DEBUG_FS
 #include <linux/pm.h>
 #include <linux/device.h>
 #include <linux/debugfs.h>
@@ -1784,4 +2058,4 @@ static void __exit pm_genpd_debug_exit(void)
 	debugfs_remove_recursive(pm_genpd_debugfs_dir);
 }
 __exitcall(pm_genpd_debug_exit);
-#endif /* CONFIG_PM_ADVANCED_DEBUG */
+#endif /* CONFIG_DEBUG_FS */
diff --git a/drivers/base/power/opp/core.c b/drivers/base/power/opp/core.c
index df0c70963d9e..4c7c6da7a989 100644
--- a/drivers/base/power/opp/core.c
+++ b/drivers/base/power/opp/core.c
@@ -584,7 +584,6 @@ int dev_pm_opp_set_rate(struct device *dev, unsigned long target_freq)
 	struct clk *clk;
 	unsigned long freq, old_freq;
 	unsigned long u_volt, u_volt_min, u_volt_max;
-	unsigned long ou_volt, ou_volt_min, ou_volt_max;
 	int ret;
 
 	if (unlikely(!target_freq)) {
@@ -620,11 +619,7 @@ int dev_pm_opp_set_rate(struct device *dev, unsigned long target_freq)
 	}
 
 	old_opp = _find_freq_ceil(opp_table, &old_freq);
-	if (!IS_ERR(old_opp)) {
-		ou_volt = old_opp->u_volt;
-		ou_volt_min = old_opp->u_volt_min;
-		ou_volt_max = old_opp->u_volt_max;
-	} else {
+	if (IS_ERR(old_opp)) {
 		dev_err(dev, "%s: failed to find current OPP for freq %lu (%ld)\n",
 			__func__, old_freq, PTR_ERR(old_opp));
 	}
@@ -683,7 +678,8 @@ restore_freq:
 restore_voltage:
 	/* This shouldn't harm even if the voltages weren't updated earlier */
 	if (!IS_ERR(old_opp))
-		_set_opp_voltage(dev, reg, ou_volt, ou_volt_min, ou_volt_max);
+		_set_opp_voltage(dev, reg, old_opp->u_volt,
+				 old_opp->u_volt_min, old_opp->u_volt_max);
 
 	return ret;
 }
diff --git a/drivers/base/power/opp/of.c b/drivers/base/power/opp/of.c
index 1dfd3dd92624..5552211e6fcd 100644
--- a/drivers/base/power/opp/of.c
+++ b/drivers/base/power/opp/of.c
@@ -71,8 +71,18 @@ static bool _opp_is_supported(struct device *dev, struct opp_table *opp_table,
 	u32 version;
 	int ret;
 
-	if (!opp_table->supported_hw)
-		return true;
+	if (!opp_table->supported_hw) {
+		/*
+		 * In the case that no supported_hw has been set by the
+		 * platform but there is an opp-supported-hw value set for
+		 * an OPP then the OPP should not be enabled as there is
+		 * no way to see if the hardware supports it.
+		 */
+		if (of_find_property(np, "opp-supported-hw", NULL))
+			return false;
+		else
+			return true;
+	}
 
 	while (count--) {
 		ret = of_property_read_u32_index(np, "opp-supported-hw", count,
diff --git a/drivers/base/regmap/internal.h b/drivers/base/regmap/internal.h
index a0380338946a..2a4435d76028 100644
--- a/drivers/base/regmap/internal.h
+++ b/drivers/base/regmap/internal.h
@@ -105,8 +105,8 @@ struct regmap {
 
 	bool defer_caching;
 
-	u8 read_flag_mask;
-	u8 write_flag_mask;
+	unsigned long read_flag_mask;
+	unsigned long write_flag_mask;
 
 	/* number of bits to (left) shift the reg value when formatting*/
 	int reg_shift;
@@ -173,6 +173,7 @@ struct regcache_ops {
 	int (*drop)(struct regmap *map, unsigned int min, unsigned int max);
 };
 
+bool regmap_cached(struct regmap *map, unsigned int reg);
 bool regmap_writeable(struct regmap *map, unsigned int reg);
 bool regmap_readable(struct regmap *map, unsigned int reg);
 bool regmap_volatile(struct regmap *map, unsigned int reg);
diff --git a/drivers/base/regmap/regmap-debugfs.c b/drivers/base/regmap/regmap-debugfs.c
index 1ee3d40861c7..36ce3511c733 100644
--- a/drivers/base/regmap/regmap-debugfs.c
+++ b/drivers/base/regmap/regmap-debugfs.c
@@ -77,6 +77,17 @@ static void regmap_debugfs_free_dump_cache(struct regmap *map)
 	}
 }
 
+static bool regmap_printable(struct regmap *map, unsigned int reg)
+{
+	if (regmap_precious(map, reg))
+		return false;
+
+	if (!regmap_readable(map, reg) && !regmap_cached(map, reg))
+		return false;
+
+	return true;
+}
+
 /*
  * Work out where the start offset maps into register numbers, bearing
  * in mind that we suppress hidden registers.
@@ -105,8 +116,7 @@ static unsigned int regmap_debugfs_get_dump_start(struct regmap *map,
 	if (list_empty(&map->debugfs_off_cache)) {
 		for (; i <= map->max_register; i += map->reg_stride) {
 			/* Skip unprinted registers, closing off cache entry */
-			if (!regmap_readable(map, i) ||
-			    regmap_precious(map, i)) {
+			if (!regmap_printable(map, i)) {
 				if (c) {
 					c->max = p - 1;
 					c->max_reg = i - map->reg_stride;
@@ -204,7 +214,7 @@ static ssize_t regmap_read_debugfs(struct regmap *map, unsigned int from,
 	start_reg = regmap_debugfs_get_dump_start(map, from, *ppos, &p);
 
 	for (i = start_reg; i <= to; i += map->reg_stride) {
-		if (!regmap_readable(map, i))
+		if (!regmap_readable(map, i) && !regmap_cached(map, i))
 			continue;
 
 		if (regmap_precious(map, i))
diff --git a/drivers/base/regmap/regmap.c b/drivers/base/regmap/regmap.c
index e964d068874d..ae63bb0875ea 100644
--- a/drivers/base/regmap/regmap.c
+++ b/drivers/base/regmap/regmap.c
@@ -93,6 +93,29 @@ bool regmap_writeable(struct regmap *map, unsigned int reg)
 	return true;
 }
 
+bool regmap_cached(struct regmap *map, unsigned int reg)
+{
+	int ret;
+	unsigned int val;
+
+	if (map->cache == REGCACHE_NONE)
+		return false;
+
+	if (!map->cache_ops)
+		return false;
+
+	if (map->max_register && reg > map->max_register)
+		return false;
+
+	map->lock(map->lock_arg);
+	ret = regcache_read(map, reg, &val);
+	map->unlock(map->lock_arg);
+	if (ret)
+		return false;
+
+	return true;
+}
+
 bool regmap_readable(struct regmap *map, unsigned int reg)
 {
 	if (!map->reg_read)
@@ -749,6 +772,9 @@ struct regmap *__regmap_init(struct device *dev,
 		case REGMAP_ENDIAN_BIG:
 			map->format.format_reg = regmap_format_16_be;
 			break;
+		case REGMAP_ENDIAN_LITTLE:
+			map->format.format_reg = regmap_format_16_le;
+			break;
 		case REGMAP_ENDIAN_NATIVE:
 			map->format.format_reg = regmap_format_16_native;
 			break;
@@ -768,6 +794,9 @@ struct regmap *__regmap_init(struct device *dev,
 		case REGMAP_ENDIAN_BIG:
 			map->format.format_reg = regmap_format_32_be;
 			break;
+		case REGMAP_ENDIAN_LITTLE:
+			map->format.format_reg = regmap_format_32_le;
+			break;
 		case REGMAP_ENDIAN_NATIVE:
 			map->format.format_reg = regmap_format_32_native;
 			break;
@@ -782,6 +811,9 @@ struct regmap *__regmap_init(struct device *dev,
 		case REGMAP_ENDIAN_BIG:
 			map->format.format_reg = regmap_format_64_be;
 			break;
+		case REGMAP_ENDIAN_LITTLE:
+			map->format.format_reg = regmap_format_64_le;
+			break;
 		case REGMAP_ENDIAN_NATIVE:
 			map->format.format_reg = regmap_format_64_native;
 			break;
@@ -1296,12 +1328,26 @@ static int _regmap_select_page(struct regmap *map, unsigned int *reg,
 	return 0;
 }
 
+static void regmap_set_work_buf_flag_mask(struct regmap *map, int max_bytes,
+					  unsigned long mask)
+{
+	u8 *buf;
+	int i;
+
+	if (!mask || !map->work_buf)
+		return;
+
+	buf = map->work_buf;
+
+	for (i = 0; i < max_bytes; i++)
+		buf[i] |= (mask >> (8 * i)) & 0xff;
+}
+
 int _regmap_raw_write(struct regmap *map, unsigned int reg,
 		      const void *val, size_t val_len)
 {
 	struct regmap_range_node *range;
 	unsigned long flags;
-	u8 *u8 = map->work_buf;
 	void *work_val = map->work_buf + map->format.reg_bytes +
 		map->format.pad_bytes;
 	void *buf;
@@ -1370,8 +1416,8 @@ int _regmap_raw_write(struct regmap *map, unsigned int reg,
 	}
 
 	map->format.format_reg(map->work_buf, reg, map->reg_shift);
-
-	u8[0] |= map->write_flag_mask;
+	regmap_set_work_buf_flag_mask(map, map->format.reg_bytes,
+				      map->write_flag_mask);
 
 	/*
 	 * Essentially all I/O mechanisms will be faster with a single
@@ -2251,7 +2297,6 @@ static int _regmap_raw_read(struct regmap *map, unsigned int reg, void *val,
 			    unsigned int val_len)
 {
 	struct regmap_range_node *range;
-	u8 *u8 = map->work_buf;
 	int ret;
 
 	WARN_ON(!map->bus);
@@ -2268,15 +2313,8 @@ static int _regmap_raw_read(struct regmap *map, unsigned int reg, void *val,
 	}
 
 	map->format.format_reg(map->work_buf, reg, map->reg_shift);
-
-	/*
-	 * Some buses or devices flag reads by setting the high bits in the
-	 * register address; since it's always the high bits for all
-	 * current formats we can do this here rather than in
-	 * formatting.  This may break if we get interesting formats.
-	 */
-	u8[0] |= map->read_flag_mask;
-
+	regmap_set_work_buf_flag_mask(map, map->format.reg_bytes,
+				      map->read_flag_mask);
 	trace_regmap_hw_read_start(map, reg, val_len / map->format.val_bytes);
 
 	ret = map->bus->read(map->bus_context, map->work_buf,
diff --git a/drivers/base/soc.c b/drivers/base/soc.c
index 75b98aad6faf..b63f23e6ad61 100644
--- a/drivers/base/soc.c
+++ b/drivers/base/soc.c
@@ -6,7 +6,6 @@
  */
 
 #include <linux/sysfs.h>
-#include <linux/module.h>
 #include <linux/init.h>
 #include <linux/stat.h>
 #include <linux/slab.h>
@@ -160,11 +159,3 @@ static int __init soc_bus_register(void)
 	return bus_register(&soc_bus_type);
 }
 core_initcall(soc_bus_register);
-
-static void __exit soc_bus_unregister(void)
-{
-	ida_destroy(&soc_ida);
-
-	bus_unregister(&soc_bus_type);
-}
-module_exit(soc_bus_unregister);
diff --git a/drivers/bcma/driver_chipcommon.c b/drivers/bcma/driver_chipcommon.c
index 921ce1834673..b4f6520e74f0 100644
--- a/drivers/bcma/driver_chipcommon.c
+++ b/drivers/bcma/driver_chipcommon.c
@@ -36,12 +36,31 @@ u32 bcma_chipco_get_alp_clock(struct bcma_drv_cc *cc)
 }
 EXPORT_SYMBOL_GPL(bcma_chipco_get_alp_clock);
 
+static bool bcma_core_cc_has_pmu_watchdog(struct bcma_drv_cc *cc)
+{
+	struct bcma_bus *bus = cc->core->bus;
+
+	if (cc->capabilities & BCMA_CC_CAP_PMU) {
+		if (bus->chipinfo.id == BCMA_CHIP_ID_BCM53573) {
+			WARN(bus->chipinfo.rev <= 1, "No watchdog available\n");
+			/* 53573B0 and 53573B1 have bugged PMU watchdog. It can
+			 * be enabled but timer can't be bumped. Use CC one
+			 * instead.
+			 */
+			return false;
+		}
+		return true;
+	} else {
+		return false;
+	}
+}
+
 static u32 bcma_chipco_watchdog_get_max_timer(struct bcma_drv_cc *cc)
 {
 	struct bcma_bus *bus = cc->core->bus;
 	u32 nb;
 
-	if (cc->capabilities & BCMA_CC_CAP_PMU) {
+	if (bcma_core_cc_has_pmu_watchdog(cc)) {
 		if (bus->chipinfo.id == BCMA_CHIP_ID_BCM4706)
 			nb = 32;
 		else if (cc->core->id.rev < 26)
@@ -95,9 +114,16 @@ static int bcma_chipco_watchdog_ticks_per_ms(struct bcma_drv_cc *cc)
 
 int bcma_chipco_watchdog_register(struct bcma_drv_cc *cc)
 {
+	struct bcma_bus *bus = cc->core->bus;
 	struct bcm47xx_wdt wdt = {};
 	struct platform_device *pdev;
 
+	if (bus->chipinfo.id == BCMA_CHIP_ID_BCM53573 &&
+	    bus->chipinfo.rev <= 1) {
+		pr_debug("No watchdog on 53573A0 / 53573A1\n");
+		return 0;
+	}
+
 	wdt.driver_data = cc;
 	wdt.timer_set = bcma_chipco_watchdog_timer_set_wdt;
 	wdt.timer_set_ms = bcma_chipco_watchdog_timer_set_ms_wdt;
@@ -105,7 +131,7 @@ int bcma_chipco_watchdog_register(struct bcma_drv_cc *cc)
 		bcma_chipco_watchdog_get_max_timer(cc) / cc->ticks_per_ms;
 
 	pdev = platform_device_register_data(NULL, "bcm47xx-wdt",
-					     cc->core->bus->num, &wdt,
+					     bus->num, &wdt,
 					     sizeof(wdt));
 	if (IS_ERR(pdev))
 		return PTR_ERR(pdev);
@@ -217,7 +243,7 @@ u32 bcma_chipco_watchdog_timer_set(struct bcma_drv_cc *cc, u32 ticks)
 	u32 maxt;
 
 	maxt = bcma_chipco_watchdog_get_max_timer(cc);
-	if (cc->capabilities & BCMA_CC_CAP_PMU) {
+	if (bcma_core_cc_has_pmu_watchdog(cc)) {
 		if (ticks == 1)
 			ticks = 2;
 		else if (ticks > maxt)
diff --git a/drivers/bcma/main.c b/drivers/bcma/main.c
index 1f635471f318..2c1798e38abd 100644
--- a/drivers/bcma/main.c
+++ b/drivers/bcma/main.c
@@ -209,6 +209,8 @@ static void bcma_of_fill_device(struct platform_device *parent,
 		core->dev.of_node = node;
 
 	core->irq = bcma_of_get_irq(parent, core, 0);
+
+	of_dma_configure(&core->dev, node);
 }
 
 unsigned int bcma_core_irq(struct bcma_device *core, int num)
@@ -248,12 +250,12 @@ void bcma_prepare_core(struct bcma_bus *bus, struct bcma_device *core)
 		core->irq = bus->host_pci->irq;
 		break;
 	case BCMA_HOSTTYPE_SOC:
-		core->dev.dma_mask = &core->dev.coherent_dma_mask;
-		if (bus->host_pdev) {
+		if (IS_ENABLED(CONFIG_OF) && bus->host_pdev) {
 			core->dma_dev = &bus->host_pdev->dev;
 			core->dev.parent = &bus->host_pdev->dev;
 			bcma_of_fill_device(bus->host_pdev, core);
 		} else {
+			core->dev.dma_mask = &core->dev.coherent_dma_mask;
 			core->dma_dev = &core->dev;
 		}
 		break;
diff --git a/drivers/block/loop.c b/drivers/block/loop.c
index c9f2107f7095..fa1b7a90ba11 100644
--- a/drivers/block/loop.c
+++ b/drivers/block/loop.c
@@ -840,13 +840,13 @@ static void loop_config_discard(struct loop_device *lo)
 
 static void loop_unprepare_queue(struct loop_device *lo)
 {
-	flush_kthread_worker(&lo->worker);
+	kthread_flush_worker(&lo->worker);
 	kthread_stop(lo->worker_task);
 }
 
 static int loop_prepare_queue(struct loop_device *lo)
 {
-	init_kthread_worker(&lo->worker);
+	kthread_init_worker(&lo->worker);
 	lo->worker_task = kthread_run(kthread_worker_fn,
 			&lo->worker, "loop%d", lo->lo_number);
 	if (IS_ERR(lo->worker_task))
@@ -1658,7 +1658,7 @@ static int loop_queue_rq(struct blk_mq_hw_ctx *hctx,
 		break;
 	}
 
-	queue_kthread_work(&lo->worker, &cmd->work);
+	kthread_queue_work(&lo->worker, &cmd->work);
 
 	return BLK_MQ_RQ_QUEUE_OK;
 }
@@ -1696,14 +1696,13 @@ static int loop_init_request(void *data, struct request *rq,
 	struct loop_cmd *cmd = blk_mq_rq_to_pdu(rq);
 
 	cmd->rq = rq;
-	init_kthread_work(&cmd->work, loop_queue_work);
+	kthread_init_work(&cmd->work, loop_queue_work);
 
 	return 0;
 }
 
 static struct blk_mq_ops loop_mq_ops = {
 	.queue_rq       = loop_queue_rq,
-	.map_queue      = blk_mq_map_queue,
 	.init_request	= loop_init_request,
 };
 
diff --git a/drivers/block/mtip32xx/mtip32xx.c b/drivers/block/mtip32xx/mtip32xx.c
index 2aca98e8e427..3cfd879267b2 100644
--- a/drivers/block/mtip32xx/mtip32xx.c
+++ b/drivers/block/mtip32xx/mtip32xx.c
@@ -3686,7 +3686,7 @@ static int mtip_block_open(struct block_device *dev, fmode_t mode)
 	return -ENODEV;
 }
 
-void mtip_block_release(struct gendisk *disk, fmode_t mode)
+static void mtip_block_release(struct gendisk *disk, fmode_t mode)
 {
 }
 
@@ -3895,7 +3895,6 @@ exit_handler:
 
 static struct blk_mq_ops mtip_mq_ops = {
 	.queue_rq	= mtip_queue_rq,
-	.map_queue	= blk_mq_map_queue,
 	.init_request	= mtip_init_cmd,
 	.exit_request	= mtip_free_cmd,
 	.complete	= mtip_softirq_done_fn,
diff --git a/drivers/block/nbd.c b/drivers/block/nbd.c
index a9e398019f38..ba405b55329f 100644
--- a/drivers/block/nbd.c
+++ b/drivers/block/nbd.c
@@ -34,33 +34,29 @@
 #include <linux/kthread.h>
 #include <linux/types.h>
 #include <linux/debugfs.h>
+#include <linux/blk-mq.h>
 
 #include <asm/uaccess.h>
 #include <asm/types.h>
 
 #include <linux/nbd.h>
 
+#define NBD_TIMEDOUT			0
+#define NBD_DISCONNECT_REQUESTED	1
+
 struct nbd_device {
 	u32 flags;
+	unsigned long runtime_flags;
 	struct socket * sock;	/* If == NULL, device is not ready, yet	*/
 	int magic;
 
-	spinlock_t queue_lock;
-	struct list_head queue_head;	/* Requests waiting result */
-	struct request *active_req;
-	wait_queue_head_t active_wq;
-	struct list_head waiting_queue;	/* Requests to be sent */
-	wait_queue_head_t waiting_wq;
+	struct blk_mq_tag_set tag_set;
 
 	struct mutex tx_lock;
 	struct gendisk *disk;
 	int blksize;
 	loff_t bytesize;
-	int xmit_timeout;
-	bool timedout;
-	bool disconnect; /* a disconnect has been requested by user */
 
-	struct timer_list timeout_timer;
 	/* protects initialization and shutdown of the socket */
 	spinlock_t sock_lock;
 	struct task_struct *task_recv;
@@ -71,6 +67,11 @@ struct nbd_device {
 #endif
 };
 
+struct nbd_cmd {
+	struct nbd_device *nbd;
+	struct list_head list;
+};
+
 #if IS_ENABLED(CONFIG_DEBUG_FS)
 static struct dentry *nbd_dbg_dir;
 #endif
@@ -83,18 +84,6 @@ static unsigned int nbds_max = 16;
 static struct nbd_device *nbd_dev;
 static int max_part;
 
-/*
- * Use just one lock (or at most 1 per NIC). Two arguments for this:
- * 1. Each NIC is essentially a synchronization point for all servers
- *    accessed through that NIC so there's no need to have more locks
- *    than NICs anyway.
- * 2. More locks lead to more "Dirty cache line bouncing" which will slow
- *    down each lock to the point where they're actually slower than just
- *    a single lock.
- * Thanks go to Jens Axboe and Al Viro for their LKML emails explaining this!
- */
-static DEFINE_SPINLOCK(nbd_lock);
-
 static inline struct device *nbd_to_dev(struct nbd_device *nbd)
 {
 	return disk_to_dev(nbd->disk);
@@ -153,18 +142,16 @@ static int nbd_size_set(struct nbd_device *nbd, struct block_device *bdev,
 	return 0;
 }
 
-static void nbd_end_request(struct nbd_device *nbd, struct request *req)
+static void nbd_end_request(struct nbd_cmd *cmd)
 {
+	struct nbd_device *nbd = cmd->nbd;
+	struct request *req = blk_mq_rq_from_pdu(cmd);
 	int error = req->errors ? -EIO : 0;
-	struct request_queue *q = req->q;
-	unsigned long flags;
 
-	dev_dbg(nbd_to_dev(nbd), "request %p: %s\n", req,
+	dev_dbg(nbd_to_dev(nbd), "request %p: %s\n", cmd,
 		error ? "failed" : "done");
 
-	spin_lock_irqsave(q->queue_lock, flags);
-	__blk_end_request_all(req, error);
-	spin_unlock_irqrestore(q->queue_lock, flags);
+	blk_mq_complete_request(req, error);
 }
 
 /*
@@ -172,40 +159,49 @@ static void nbd_end_request(struct nbd_device *nbd, struct request *req)
  */
 static void sock_shutdown(struct nbd_device *nbd)
 {
-	spin_lock_irq(&nbd->sock_lock);
+	struct socket *sock;
+
+	spin_lock(&nbd->sock_lock);
 
 	if (!nbd->sock) {
 		spin_unlock_irq(&nbd->sock_lock);
 		return;
 	}
 
+	sock = nbd->sock;
 	dev_warn(disk_to_dev(nbd->disk), "shutting down socket\n");
-	kernel_sock_shutdown(nbd->sock, SHUT_RDWR);
-	sockfd_put(nbd->sock);
 	nbd->sock = NULL;
-	spin_unlock_irq(&nbd->sock_lock);
+	spin_unlock(&nbd->sock_lock);
 
-	del_timer(&nbd->timeout_timer);
+	kernel_sock_shutdown(sock, SHUT_RDWR);
+	sockfd_put(sock);
 }
 
-static void nbd_xmit_timeout(unsigned long arg)
+static enum blk_eh_timer_return nbd_xmit_timeout(struct request *req,
+						 bool reserved)
 {
-	struct nbd_device *nbd = (struct nbd_device *)arg;
-	unsigned long flags;
-
-	if (list_empty(&nbd->queue_head))
-		return;
+	struct nbd_cmd *cmd = blk_mq_rq_to_pdu(req);
+	struct nbd_device *nbd = cmd->nbd;
+	struct socket *sock = NULL;
 
-	spin_lock_irqsave(&nbd->sock_lock, flags);
+	spin_lock(&nbd->sock_lock);
 
-	nbd->timedout = true;
+	set_bit(NBD_TIMEDOUT, &nbd->runtime_flags);
 
-	if (nbd->sock)
-		kernel_sock_shutdown(nbd->sock, SHUT_RDWR);
+	if (nbd->sock) {
+		sock = nbd->sock;
+		get_file(sock->file);
+	}
 
-	spin_unlock_irqrestore(&nbd->sock_lock, flags);
+	spin_unlock(&nbd->sock_lock);
+	if (sock) {
+		kernel_sock_shutdown(sock, SHUT_RDWR);
+		sockfd_put(sock);
+	}
 
+	req->errors++;
 	dev_err(nbd_to_dev(nbd), "Connection timed out, shutting down connection\n");
+	return BLK_EH_HANDLED;
 }
 
 /*
@@ -255,9 +251,6 @@ static int sock_xmit(struct nbd_device *nbd, int send, void *buf, int size,
 
 	tsk_restore_flags(current, pflags, PF_MEMALLOC);
 
-	if (!send && nbd->xmit_timeout)
-		mod_timer(&nbd->timeout_timer, jiffies + nbd->xmit_timeout);
-
 	return result;
 }
 
@@ -273,8 +266,9 @@ static inline int sock_send_bvec(struct nbd_device *nbd, struct bio_vec *bvec,
 }
 
 /* always call with the tx_lock held */
-static int nbd_send_req(struct nbd_device *nbd, struct request *req)
+static int nbd_send_cmd(struct nbd_device *nbd, struct nbd_cmd *cmd)
 {
+	struct request *req = blk_mq_rq_from_pdu(cmd);
 	int result, flags;
 	struct nbd_request request;
 	unsigned long size = blk_rq_bytes(req);
@@ -298,10 +292,10 @@ static int nbd_send_req(struct nbd_device *nbd, struct request *req)
 		request.from = cpu_to_be64((u64)blk_rq_pos(req) << 9);
 		request.len = htonl(size);
 	}
-	memcpy(request.handle, &req, sizeof(req));
+	memcpy(request.handle, &req->tag, sizeof(req->tag));
 
 	dev_dbg(nbd_to_dev(nbd), "request %p: sending control (%s@%llu,%uB)\n",
-		req, nbdcmd_to_ascii(type),
+		cmd, nbdcmd_to_ascii(type),
 		(unsigned long long)blk_rq_pos(req) << 9, blk_rq_bytes(req));
 	result = sock_xmit(nbd, 1, &request, sizeof(request),
 			(type == NBD_CMD_WRITE) ? MSG_MORE : 0);
@@ -323,7 +317,7 @@ static int nbd_send_req(struct nbd_device *nbd, struct request *req)
 			if (!rq_iter_last(bvec, iter))
 				flags = MSG_MORE;
 			dev_dbg(nbd_to_dev(nbd), "request %p: sending %d bytes data\n",
-				req, bvec.bv_len);
+				cmd, bvec.bv_len);
 			result = sock_send_bvec(nbd, &bvec, flags);
 			if (result <= 0) {
 				dev_err(disk_to_dev(nbd->disk),
@@ -336,29 +330,6 @@ static int nbd_send_req(struct nbd_device *nbd, struct request *req)
 	return 0;
 }
 
-static struct request *nbd_find_request(struct nbd_device *nbd,
-					struct request *xreq)
-{
-	struct request *req, *tmp;
-	int err;
-
-	err = wait_event_interruptible(nbd->active_wq, nbd->active_req != xreq);
-	if (unlikely(err))
-		return ERR_PTR(err);
-
-	spin_lock(&nbd->queue_lock);
-	list_for_each_entry_safe(req, tmp, &nbd->queue_head, queuelist) {
-		if (req != xreq)
-			continue;
-		list_del_init(&req->queuelist);
-		spin_unlock(&nbd->queue_lock);
-		return req;
-	}
-	spin_unlock(&nbd->queue_lock);
-
-	return ERR_PTR(-ENOENT);
-}
-
 static inline int sock_recv_bvec(struct nbd_device *nbd, struct bio_vec *bvec)
 {
 	int result;
@@ -370,11 +341,14 @@ static inline int sock_recv_bvec(struct nbd_device *nbd, struct bio_vec *bvec)
 }
 
 /* NULL returned = something went wrong, inform userspace */
-static struct request *nbd_read_stat(struct nbd_device *nbd)
+static struct nbd_cmd *nbd_read_stat(struct nbd_device *nbd)
 {
 	int result;
 	struct nbd_reply reply;
-	struct request *req;
+	struct nbd_cmd *cmd;
+	struct request *req = NULL;
+	u16 hwq;
+	int tag;
 
 	reply.magic = 0;
 	result = sock_xmit(nbd, 0, &reply, sizeof(reply), MSG_WAITALL);
@@ -390,25 +364,27 @@ static struct request *nbd_read_stat(struct nbd_device *nbd)
 		return ERR_PTR(-EPROTO);
 	}
 
-	req = nbd_find_request(nbd, *(struct request **)reply.handle);
-	if (IS_ERR(req)) {
-		result = PTR_ERR(req);
-		if (result != -ENOENT)
-			return ERR_PTR(result);
+	memcpy(&tag, reply.handle, sizeof(int));
 
-		dev_err(disk_to_dev(nbd->disk), "Unexpected reply (%p)\n",
-			reply.handle);
-		return ERR_PTR(-EBADR);
+	hwq = blk_mq_unique_tag_to_hwq(tag);
+	if (hwq < nbd->tag_set.nr_hw_queues)
+		req = blk_mq_tag_to_rq(nbd->tag_set.tags[hwq],
+				       blk_mq_unique_tag_to_tag(tag));
+	if (!req || !blk_mq_request_started(req)) {
+		dev_err(disk_to_dev(nbd->disk), "Unexpected reply (%d) %p\n",
+			tag, req);
+		return ERR_PTR(-ENOENT);
 	}
+	cmd = blk_mq_rq_to_pdu(req);
 
 	if (ntohl(reply.error)) {
 		dev_err(disk_to_dev(nbd->disk), "Other side returned error (%d)\n",
 			ntohl(reply.error));
 		req->errors++;
-		return req;
+		return cmd;
 	}
 
-	dev_dbg(nbd_to_dev(nbd), "request %p: got reply\n", req);
+	dev_dbg(nbd_to_dev(nbd), "request %p: got reply\n", cmd);
 	if (rq_data_dir(req) != WRITE) {
 		struct req_iterator iter;
 		struct bio_vec bvec;
@@ -419,13 +395,13 @@ static struct request *nbd_read_stat(struct nbd_device *nbd)
 				dev_err(disk_to_dev(nbd->disk), "Receive data failed (result %d)\n",
 					result);
 				req->errors++;
-				return req;
+				return cmd;
 			}
 			dev_dbg(nbd_to_dev(nbd), "request %p: got %d bytes data\n",
-				req, bvec.bv_len);
+				cmd, bvec.bv_len);
 		}
 	}
-	return req;
+	return cmd;
 }
 
 static ssize_t pid_show(struct device *dev,
@@ -444,7 +420,7 @@ static struct device_attribute pid_attr = {
 
 static int nbd_thread_recv(struct nbd_device *nbd, struct block_device *bdev)
 {
-	struct request *req;
+	struct nbd_cmd *cmd;
 	int ret;
 
 	BUG_ON(nbd->magic != NBD_MAGIC);
@@ -460,13 +436,13 @@ static int nbd_thread_recv(struct nbd_device *nbd, struct block_device *bdev)
 	nbd_size_update(nbd, bdev);
 
 	while (1) {
-		req = nbd_read_stat(nbd);
-		if (IS_ERR(req)) {
-			ret = PTR_ERR(req);
+		cmd = nbd_read_stat(nbd);
+		if (IS_ERR(cmd)) {
+			ret = PTR_ERR(cmd);
 			break;
 		}
 
-		nbd_end_request(nbd, req);
+		nbd_end_request(cmd);
 	}
 
 	nbd_size_clear(nbd, bdev);
@@ -475,44 +451,37 @@ static int nbd_thread_recv(struct nbd_device *nbd, struct block_device *bdev)
 	return ret;
 }
 
-static void nbd_clear_que(struct nbd_device *nbd)
+static void nbd_clear_req(struct request *req, void *data, bool reserved)
 {
-	struct request *req;
+	struct nbd_cmd *cmd;
+
+	if (!blk_mq_request_started(req))
+		return;
+	cmd = blk_mq_rq_to_pdu(req);
+	req->errors++;
+	nbd_end_request(cmd);
+}
 
+static void nbd_clear_que(struct nbd_device *nbd)
+{
 	BUG_ON(nbd->magic != NBD_MAGIC);
 
 	/*
 	 * Because we have set nbd->sock to NULL under the tx_lock, all
-	 * modifications to the list must have completed by now.  For
-	 * the same reason, the active_req must be NULL.
-	 *
-	 * As a consequence, we don't need to take the spin lock while
-	 * purging the list here.
+	 * modifications to the list must have completed by now.
 	 */
 	BUG_ON(nbd->sock);
-	BUG_ON(nbd->active_req);
 
-	while (!list_empty(&nbd->queue_head)) {
-		req = list_entry(nbd->queue_head.next, struct request,
-				 queuelist);
-		list_del_init(&req->queuelist);
-		req->errors++;
-		nbd_end_request(nbd, req);
-	}
-
-	while (!list_empty(&nbd->waiting_queue)) {
-		req = list_entry(nbd->waiting_queue.next, struct request,
-				 queuelist);
-		list_del_init(&req->queuelist);
-		req->errors++;
-		nbd_end_request(nbd, req);
-	}
+	blk_mq_tagset_busy_iter(&nbd->tag_set, nbd_clear_req, NULL);
 	dev_dbg(disk_to_dev(nbd->disk), "queue cleared\n");
 }
 
 
-static void nbd_handle_req(struct nbd_device *nbd, struct request *req)
+static void nbd_handle_cmd(struct nbd_cmd *cmd)
 {
+	struct request *req = blk_mq_rq_from_pdu(cmd);
+	struct nbd_device *nbd = cmd->nbd;
+
 	if (req->cmd_type != REQ_TYPE_FS)
 		goto error_out;
 
@@ -526,6 +495,7 @@ static void nbd_handle_req(struct nbd_device *nbd, struct request *req)
 	req->errors = 0;
 
 	mutex_lock(&nbd->tx_lock);
+	nbd->task_send = current;
 	if (unlikely(!nbd->sock)) {
 		mutex_unlock(&nbd->tx_lock);
 		dev_err(disk_to_dev(nbd->disk),
@@ -533,106 +503,30 @@ static void nbd_handle_req(struct nbd_device *nbd, struct request *req)
 		goto error_out;
 	}
 
-	nbd->active_req = req;
-
-	if (nbd->xmit_timeout && list_empty_careful(&nbd->queue_head))
-		mod_timer(&nbd->timeout_timer, jiffies + nbd->xmit_timeout);
-
-	if (nbd_send_req(nbd, req) != 0) {
+	if (nbd_send_cmd(nbd, cmd) != 0) {
 		dev_err(disk_to_dev(nbd->disk), "Request send failed\n");
 		req->errors++;
-		nbd_end_request(nbd, req);
-	} else {
-		spin_lock(&nbd->queue_lock);
-		list_add_tail(&req->queuelist, &nbd->queue_head);
-		spin_unlock(&nbd->queue_lock);
+		nbd_end_request(cmd);
 	}
 
-	nbd->active_req = NULL;
+	nbd->task_send = NULL;
 	mutex_unlock(&nbd->tx_lock);
-	wake_up_all(&nbd->active_wq);
 
 	return;
 
 error_out:
 	req->errors++;
-	nbd_end_request(nbd, req);
+	nbd_end_request(cmd);
 }
 
-static int nbd_thread_send(void *data)
+static int nbd_queue_rq(struct blk_mq_hw_ctx *hctx,
+			const struct blk_mq_queue_data *bd)
 {
-	struct nbd_device *nbd = data;
-	struct request *req;
-
-	nbd->task_send = current;
-
-	set_user_nice(current, MIN_NICE);
-	while (!kthread_should_stop() || !list_empty(&nbd->waiting_queue)) {
-		/* wait for something to do */
-		wait_event_interruptible(nbd->waiting_wq,
-					 kthread_should_stop() ||
-					 !list_empty(&nbd->waiting_queue));
-
-		/* extract request */
-		if (list_empty(&nbd->waiting_queue))
-			continue;
-
-		spin_lock_irq(&nbd->queue_lock);
-		req = list_entry(nbd->waiting_queue.next, struct request,
-				 queuelist);
-		list_del_init(&req->queuelist);
-		spin_unlock_irq(&nbd->queue_lock);
-
-		/* handle request */
-		nbd_handle_req(nbd, req);
-	}
+	struct nbd_cmd *cmd = blk_mq_rq_to_pdu(bd->rq);
 
-	nbd->task_send = NULL;
-
-	return 0;
-}
-
-/*
- * We always wait for result of write, for now. It would be nice to make it optional
- * in future
- * if ((rq_data_dir(req) == WRITE) && (nbd->flags & NBD_WRITE_NOCHK))
- *   { printk( "Warning: Ignoring result!\n"); nbd_end_request( req ); }
- */
-
-static void nbd_request_handler(struct request_queue *q)
-		__releases(q->queue_lock) __acquires(q->queue_lock)
-{
-	struct request *req;
-	
-	while ((req = blk_fetch_request(q)) != NULL) {
-		struct nbd_device *nbd;
-
-		spin_unlock_irq(q->queue_lock);
-
-		nbd = req->rq_disk->private_data;
-
-		BUG_ON(nbd->magic != NBD_MAGIC);
-
-		dev_dbg(nbd_to_dev(nbd), "request %p: dequeued (flags=%x)\n",
-			req, req->cmd_type);
-
-		if (unlikely(!nbd->sock)) {
-			dev_err_ratelimited(disk_to_dev(nbd->disk),
-					    "Attempted send on closed socket\n");
-			req->errors++;
-			nbd_end_request(nbd, req);
-			spin_lock_irq(q->queue_lock);
-			continue;
-		}
-
-		spin_lock_irq(&nbd->queue_lock);
-		list_add_tail(&req->queuelist, &nbd->waiting_queue);
-		spin_unlock_irq(&nbd->queue_lock);
-
-		wake_up(&nbd->waiting_wq);
-
-		spin_lock_irq(q->queue_lock);
-	}
+	blk_mq_start_request(bd->rq);
+	nbd_handle_cmd(cmd);
+	return BLK_MQ_RQ_QUEUE_OK;
 }
 
 static int nbd_set_socket(struct nbd_device *nbd, struct socket *sock)
@@ -657,15 +551,13 @@ out:
 /* Reset all properties of an NBD device */
 static void nbd_reset(struct nbd_device *nbd)
 {
-	nbd->disconnect = false;
-	nbd->timedout = false;
+	nbd->runtime_flags = 0;
 	nbd->blksize = 1024;
 	nbd->bytesize = 0;
 	set_capacity(nbd->disk, 0);
 	nbd->flags = 0;
-	nbd->xmit_timeout = 0;
+	nbd->tag_set.timeout = 0;
 	queue_flag_clear_unlocked(QUEUE_FLAG_DISCARD, nbd->disk->queue);
-	del_timer_sync(&nbd->timeout_timer);
 }
 
 static void nbd_bdev_reset(struct block_device *bdev)
@@ -700,33 +592,37 @@ static int __nbd_ioctl(struct block_device *bdev, struct nbd_device *nbd,
 {
 	switch (cmd) {
 	case NBD_DISCONNECT: {
-		struct request sreq;
+		struct request *sreq;
 
 		dev_info(disk_to_dev(nbd->disk), "NBD_DISCONNECT\n");
 		if (!nbd->sock)
 			return -EINVAL;
 
+		sreq = blk_mq_alloc_request(bdev_get_queue(bdev), WRITE, 0);
+		if (!sreq)
+			return -ENOMEM;
+
 		mutex_unlock(&nbd->tx_lock);
 		fsync_bdev(bdev);
 		mutex_lock(&nbd->tx_lock);
-		blk_rq_init(NULL, &sreq);
-		sreq.cmd_type = REQ_TYPE_DRV_PRIV;
+		sreq->cmd_type = REQ_TYPE_DRV_PRIV;
 
 		/* Check again after getting mutex back.  */
-		if (!nbd->sock)
+		if (!nbd->sock) {
+			blk_mq_free_request(sreq);
 			return -EINVAL;
+		}
 
-		nbd->disconnect = true;
+		set_bit(NBD_DISCONNECT_REQUESTED, &nbd->runtime_flags);
 
-		nbd_send_req(nbd, &sreq);
+		nbd_send_cmd(nbd, blk_mq_rq_to_pdu(sreq));
+		blk_mq_free_request(sreq);
 		return 0;
 	}
  
 	case NBD_CLEAR_SOCK:
 		sock_shutdown(nbd);
 		nbd_clear_que(nbd);
-		BUG_ON(!list_empty(&nbd->queue_head));
-		BUG_ON(!list_empty(&nbd->waiting_queue));
 		kill_bdev(bdev);
 		return 0;
 
@@ -758,13 +654,7 @@ static int __nbd_ioctl(struct block_device *bdev, struct nbd_device *nbd,
 		return nbd_size_set(nbd, bdev, nbd->blksize, arg);
 
 	case NBD_SET_TIMEOUT:
-		nbd->xmit_timeout = arg * HZ;
-		if (arg)
-			mod_timer(&nbd->timeout_timer,
-				  jiffies + nbd->xmit_timeout);
-		else
-			del_timer_sync(&nbd->timeout_timer);
-
+		nbd->tag_set.timeout = arg * HZ;
 		return 0;
 
 	case NBD_SET_FLAGS:
@@ -772,7 +662,6 @@ static int __nbd_ioctl(struct block_device *bdev, struct nbd_device *nbd,
 		return 0;
 
 	case NBD_DO_IT: {
-		struct task_struct *thread;
 		int error;
 
 		if (nbd->task_recv)
@@ -786,18 +675,9 @@ static int __nbd_ioctl(struct block_device *bdev, struct nbd_device *nbd,
 
 		nbd_parse_flags(nbd, bdev);
 
-		thread = kthread_run(nbd_thread_send, nbd, "%s",
-				     nbd_name(nbd));
-		if (IS_ERR(thread)) {
-			mutex_lock(&nbd->tx_lock);
-			nbd->task_recv = NULL;
-			return PTR_ERR(thread);
-		}
-
 		nbd_dev_dbg_init(nbd);
 		error = nbd_thread_recv(nbd, bdev);
 		nbd_dev_dbg_close(nbd);
-		kthread_stop(thread);
 
 		mutex_lock(&nbd->tx_lock);
 		nbd->task_recv = NULL;
@@ -807,9 +687,10 @@ static int __nbd_ioctl(struct block_device *bdev, struct nbd_device *nbd,
 		kill_bdev(bdev);
 		nbd_bdev_reset(bdev);
 
-		if (nbd->disconnect) /* user requested, ignore socket errors */
+		/* user requested, ignore socket errors */
+		if (test_bit(NBD_DISCONNECT_REQUESTED, &nbd->runtime_flags))
 			error = 0;
-		if (nbd->timedout)
+		if (test_bit(NBD_TIMEDOUT, &nbd->runtime_flags))
 			error = -ETIMEDOUT;
 
 		nbd_reset(nbd);
@@ -825,10 +706,10 @@ static int __nbd_ioctl(struct block_device *bdev, struct nbd_device *nbd,
 		return 0;
 
 	case NBD_PRINT_DEBUG:
-		dev_info(disk_to_dev(nbd->disk),
-			"next = %p, prev = %p, head = %p\n",
-			nbd->queue_head.next, nbd->queue_head.prev,
-			&nbd->queue_head);
+		/*
+		 * For compatibility only, we no longer keep a list of
+		 * outstanding requests.
+		 */
 		return 0;
 	}
 	return -ENOTTY;
@@ -935,7 +816,7 @@ static int nbd_dev_dbg_init(struct nbd_device *nbd)
 
 	debugfs_create_file("tasks", 0444, dir, nbd, &nbd_dbg_tasks_ops);
 	debugfs_create_u64("size_bytes", 0444, dir, &nbd->bytesize);
-	debugfs_create_u32("timeout", 0444, dir, &nbd->xmit_timeout);
+	debugfs_create_u32("timeout", 0444, dir, &nbd->tag_set.timeout);
 	debugfs_create_u32("blocksize", 0444, dir, &nbd->blksize);
 	debugfs_create_file("flags", 0444, dir, nbd, &nbd_dbg_flags_ops);
 
@@ -987,6 +868,23 @@ static void nbd_dbg_close(void)
 
 #endif
 
+static int nbd_init_request(void *data, struct request *rq,
+			    unsigned int hctx_idx, unsigned int request_idx,
+			    unsigned int numa_node)
+{
+	struct nbd_cmd *cmd = blk_mq_rq_to_pdu(rq);
+
+	cmd->nbd = data;
+	INIT_LIST_HEAD(&cmd->list);
+	return 0;
+}
+
+static struct blk_mq_ops nbd_mq_ops = {
+	.queue_rq	= nbd_queue_rq,
+	.init_request	= nbd_init_request,
+	.timeout	= nbd_xmit_timeout,
+};
+
 /*
  * And here should be modules and kernel interface 
  *  (Just smiley confuses emacs :-)
@@ -1035,16 +933,34 @@ static int __init nbd_init(void)
 		if (!disk)
 			goto out;
 		nbd_dev[i].disk = disk;
+
+		nbd_dev[i].tag_set.ops = &nbd_mq_ops;
+		nbd_dev[i].tag_set.nr_hw_queues = 1;
+		nbd_dev[i].tag_set.queue_depth = 128;
+		nbd_dev[i].tag_set.numa_node = NUMA_NO_NODE;
+		nbd_dev[i].tag_set.cmd_size = sizeof(struct nbd_cmd);
+		nbd_dev[i].tag_set.flags = BLK_MQ_F_SHOULD_MERGE |
+			BLK_MQ_F_SG_MERGE | BLK_MQ_F_BLOCKING;
+		nbd_dev[i].tag_set.driver_data = &nbd_dev[i];
+
+		err = blk_mq_alloc_tag_set(&nbd_dev[i].tag_set);
+		if (err) {
+			put_disk(disk);
+			goto out;
+		}
+
 		/*
 		 * The new linux 2.5 block layer implementation requires
 		 * every gendisk to have its very own request_queue struct.
 		 * These structs are big so we dynamically allocate them.
 		 */
-		disk->queue = blk_init_queue(nbd_request_handler, &nbd_lock);
+		disk->queue = blk_mq_init_queue(&nbd_dev[i].tag_set);
 		if (!disk->queue) {
+			blk_mq_free_tag_set(&nbd_dev[i].tag_set);
 			put_disk(disk);
 			goto out;
 		}
+
 		/*
 		 * Tell the block layer that we are not a rotational device
 		 */
@@ -1069,16 +985,8 @@ static int __init nbd_init(void)
 	for (i = 0; i < nbds_max; i++) {
 		struct gendisk *disk = nbd_dev[i].disk;
 		nbd_dev[i].magic = NBD_MAGIC;
-		INIT_LIST_HEAD(&nbd_dev[i].waiting_queue);
-		spin_lock_init(&nbd_dev[i].queue_lock);
 		spin_lock_init(&nbd_dev[i].sock_lock);
-		INIT_LIST_HEAD(&nbd_dev[i].queue_head);
 		mutex_init(&nbd_dev[i].tx_lock);
-		init_timer(&nbd_dev[i].timeout_timer);
-		nbd_dev[i].timeout_timer.function = nbd_xmit_timeout;
-		nbd_dev[i].timeout_timer.data = (unsigned long)&nbd_dev[i];
-		init_waitqueue_head(&nbd_dev[i].active_wq);
-		init_waitqueue_head(&nbd_dev[i].waiting_wq);
 		disk->major = NBD_MAJOR;
 		disk->first_minor = i << part_shift;
 		disk->fops = &nbd_fops;
@@ -1091,6 +999,7 @@ static int __init nbd_init(void)
 	return 0;
 out:
 	while (i--) {
+		blk_mq_free_tag_set(&nbd_dev[i].tag_set);
 		blk_cleanup_queue(nbd_dev[i].disk->queue);
 		put_disk(nbd_dev[i].disk);
 	}
@@ -1110,6 +1019,7 @@ static void __exit nbd_cleanup(void)
 		if (disk) {
 			del_gendisk(disk);
 			blk_cleanup_queue(disk->queue);
+			blk_mq_free_tag_set(&nbd_dev[i].tag_set);
 			put_disk(disk);
 		}
 	}
diff --git a/drivers/block/null_blk.c b/drivers/block/null_blk.c
index 75a7f88d6717..ba6f4a2e73db 100644
--- a/drivers/block/null_blk.c
+++ b/drivers/block/null_blk.c
@@ -34,6 +34,7 @@ struct nullb {
 	unsigned int index;
 	struct request_queue *q;
 	struct gendisk *disk;
+	struct nvm_dev *ndev;
 	struct blk_mq_tag_set tag_set;
 	struct hrtimer timer;
 	unsigned int queue_depth;
@@ -393,7 +394,6 @@ static int null_init_hctx(struct blk_mq_hw_ctx *hctx, void *data,
 
 static struct blk_mq_ops null_mq_ops = {
 	.queue_rq       = null_queue_rq,
-	.map_queue      = blk_mq_map_queue,
 	.init_hctx	= null_init_hctx,
 	.complete	= null_softirq_done_fn,
 };
@@ -414,23 +414,6 @@ static void cleanup_queues(struct nullb *nullb)
 	kfree(nullb->queues);
 }
 
-static void null_del_dev(struct nullb *nullb)
-{
-	list_del_init(&nullb->list);
-
-	if (use_lightnvm)
-		nvm_unregister(nullb->disk_name);
-	else
-		del_gendisk(nullb->disk);
-	blk_cleanup_queue(nullb->q);
-	if (queue_mode == NULL_Q_MQ)
-		blk_mq_free_tag_set(&nullb->tag_set);
-	if (!use_lightnvm)
-		put_disk(nullb->disk);
-	cleanup_queues(nullb);
-	kfree(nullb);
-}
-
 #ifdef CONFIG_NVM
 
 static void null_lnvm_end_io(struct request *rq, int error)
@@ -564,10 +547,58 @@ static struct nvm_dev_ops null_lnvm_dev_ops = {
 	/* Simulate nvme protocol restriction */
 	.max_phys_sect		= 64,
 };
+
+static int null_nvm_register(struct nullb *nullb)
+{
+	struct nvm_dev *dev;
+	int rv;
+
+	dev = nvm_alloc_dev(0);
+	if (!dev)
+		return -ENOMEM;
+
+	dev->q = nullb->q;
+	memcpy(dev->name, nullb->disk_name, DISK_NAME_LEN);
+	dev->ops = &null_lnvm_dev_ops;
+
+	rv = nvm_register(dev);
+	if (rv) {
+		kfree(dev);
+		return rv;
+	}
+	nullb->ndev = dev;
+	return 0;
+}
+
+static void null_nvm_unregister(struct nullb *nullb)
+{
+	nvm_unregister(nullb->ndev);
+}
 #else
-static struct nvm_dev_ops null_lnvm_dev_ops;
+static int null_nvm_register(struct nullb *nullb)
+{
+	return -EINVAL;
+}
+static void null_nvm_unregister(struct nullb *nullb) {}
 #endif /* CONFIG_NVM */
 
+static void null_del_dev(struct nullb *nullb)
+{
+	list_del_init(&nullb->list);
+
+	if (use_lightnvm)
+		null_nvm_unregister(nullb);
+	else
+		del_gendisk(nullb->disk);
+	blk_cleanup_queue(nullb->q);
+	if (queue_mode == NULL_Q_MQ)
+		blk_mq_free_tag_set(&nullb->tag_set);
+	if (!use_lightnvm)
+		put_disk(nullb->disk);
+	cleanup_queues(nullb);
+	kfree(nullb);
+}
+
 static int null_open(struct block_device *bdev, fmode_t mode)
 {
 	return 0;
@@ -640,11 +671,32 @@ static int init_driver_queues(struct nullb *nullb)
 	return 0;
 }
 
-static int null_add_dev(void)
+static int null_gendisk_register(struct nullb *nullb)
 {
 	struct gendisk *disk;
-	struct nullb *nullb;
 	sector_t size;
+
+	disk = nullb->disk = alloc_disk_node(1, home_node);
+	if (!disk)
+		return -ENOMEM;
+	size = gb * 1024 * 1024 * 1024ULL;
+	set_capacity(disk, size >> 9);
+
+	disk->flags |= GENHD_FL_EXT_DEVT | GENHD_FL_SUPPRESS_PARTITION_INFO;
+	disk->major		= null_major;
+	disk->first_minor	= nullb->index;
+	disk->fops		= &null_fops;
+	disk->private_data	= nullb;
+	disk->queue		= nullb->q;
+	strncpy(disk->disk_name, nullb->disk_name, DISK_NAME_LEN);
+
+	add_disk(disk);
+	return 0;
+}
+
+static int null_add_dev(void)
+{
+	struct nullb *nullb;
 	int rv;
 
 	nullb = kzalloc_node(sizeof(*nullb), GFP_KERNEL, home_node);
@@ -716,42 +768,19 @@ static int null_add_dev(void)
 
 	sprintf(nullb->disk_name, "nullb%d", nullb->index);
 
-	if (use_lightnvm) {
-		rv = nvm_register(nullb->q, nullb->disk_name,
-							&null_lnvm_dev_ops);
-		if (rv)
-			goto out_cleanup_blk_queue;
-		goto done;
-	}
-
-	disk = nullb->disk = alloc_disk_node(1, home_node);
-	if (!disk) {
-		rv = -ENOMEM;
-		goto out_cleanup_lightnvm;
-	}
-	size = gb * 1024 * 1024 * 1024ULL;
-	set_capacity(disk, size >> 9);
-
-	disk->flags |= GENHD_FL_EXT_DEVT | GENHD_FL_SUPPRESS_PARTITION_INFO;
-	disk->major		= null_major;
-	disk->first_minor	= nullb->index;
-	disk->fops		= &null_fops;
-	disk->private_data	= nullb;
-	disk->queue		= nullb->q;
-	strncpy(disk->disk_name, nullb->disk_name, DISK_NAME_LEN);
+	if (use_lightnvm)
+		rv = null_nvm_register(nullb);
+	else
+		rv = null_gendisk_register(nullb);
 
-	add_disk(disk);
+	if (rv)
+		goto out_cleanup_blk_queue;
 
-done:
 	mutex_lock(&lock);
 	list_add_tail(&nullb->list, &nullb_list);
 	mutex_unlock(&lock);
 
 	return 0;
-
-out_cleanup_lightnvm:
-	if (use_lightnvm)
-		nvm_unregister(nullb->disk_name);
 out_cleanup_blk_queue:
 	blk_cleanup_queue(nullb->q);
 out_cleanup_tags:
diff --git a/drivers/block/rbd.c b/drivers/block/rbd.c
index 6c6519f6492a..7b274ff4632c 100644
--- a/drivers/block/rbd.c
+++ b/drivers/block/rbd.c
@@ -31,6 +31,7 @@
 #include <linux/ceph/libceph.h>
 #include <linux/ceph/osd_client.h>
 #include <linux/ceph/mon_client.h>
+#include <linux/ceph/cls_lock_client.h>
 #include <linux/ceph/decode.h>
 #include <linux/parser.h>
 #include <linux/bsearch.h>
@@ -114,12 +115,17 @@ static int atomic_dec_return_safe(atomic_t *v)
 
 #define RBD_OBJ_PREFIX_LEN_MAX	64
 
+#define RBD_NOTIFY_TIMEOUT	5	/* seconds */
+#define RBD_RETRY_DELAY		msecs_to_jiffies(1000)
+
 /* Feature bits */
 
 #define RBD_FEATURE_LAYERING	(1<<0)
 #define RBD_FEATURE_STRIPINGV2	(1<<1)
-#define RBD_FEATURES_ALL \
-	    (RBD_FEATURE_LAYERING | RBD_FEATURE_STRIPINGV2)
+#define RBD_FEATURE_EXCLUSIVE_LOCK (1<<2)
+#define RBD_FEATURES_ALL	(RBD_FEATURE_LAYERING |		\
+				 RBD_FEATURE_STRIPINGV2 |	\
+				 RBD_FEATURE_EXCLUSIVE_LOCK)
 
 /* Features supported by this (client software) implementation. */
 
@@ -128,11 +134,8 @@ static int atomic_dec_return_safe(atomic_t *v)
 /*
  * An RBD device name will be "rbd#", where the "rbd" comes from
  * RBD_DRV_NAME above, and # is a unique integer identifier.
- * MAX_INT_FORMAT_WIDTH is used in ensuring DEV_NAME_LEN is big
- * enough to hold all possible device names.
  */
 #define DEV_NAME_LEN		32
-#define MAX_INT_FORMAT_WIDTH	((5 * sizeof (int)) / 2 + 1)
 
 /*
  * block device image metadata (in-memory version)
@@ -322,6 +325,24 @@ struct rbd_img_request {
 #define for_each_obj_request_safe(ireq, oreq, n) \
 	list_for_each_entry_safe_reverse(oreq, n, &(ireq)->obj_requests, links)
 
+enum rbd_watch_state {
+	RBD_WATCH_STATE_UNREGISTERED,
+	RBD_WATCH_STATE_REGISTERED,
+	RBD_WATCH_STATE_ERROR,
+};
+
+enum rbd_lock_state {
+	RBD_LOCK_STATE_UNLOCKED,
+	RBD_LOCK_STATE_LOCKED,
+	RBD_LOCK_STATE_RELEASING,
+};
+
+/* WatchNotify::ClientId */
+struct rbd_client_id {
+	u64 gid;
+	u64 handle;
+};
+
 struct rbd_mapping {
 	u64                     size;
 	u64                     features;
@@ -349,13 +370,29 @@ struct rbd_device {
 	unsigned long		flags;		/* possibly lock protected */
 	struct rbd_spec		*spec;
 	struct rbd_options	*opts;
+	char			*config_info;	/* add{,_single_major} string */
 
 	struct ceph_object_id	header_oid;
 	struct ceph_object_locator header_oloc;
 
-	struct ceph_file_layout	layout;
+	struct ceph_file_layout	layout;		/* used for all rbd requests */
 
+	struct mutex		watch_mutex;
+	enum rbd_watch_state	watch_state;
 	struct ceph_osd_linger_request *watch_handle;
+	u64			watch_cookie;
+	struct delayed_work	watch_dwork;
+
+	struct rw_semaphore	lock_rwsem;
+	enum rbd_lock_state	lock_state;
+	struct rbd_client_id	owner_cid;
+	struct work_struct	acquired_lock_work;
+	struct work_struct	released_lock_work;
+	struct delayed_work	lock_dwork;
+	struct work_struct	unlock_work;
+	wait_queue_head_t	lock_waitq;
+
+	struct workqueue_struct	*task_wq;
 
 	struct rbd_spec		*parent_spec;
 	u64			parent_overlap;
@@ -378,15 +415,15 @@ struct rbd_device {
 };
 
 /*
- * Flag bits for rbd_dev->flags.  If atomicity is required,
- * rbd_dev->lock is used to protect access.
- *
- * Currently, only the "removing" flag (which is coupled with the
- * "open_count" field) requires atomic access.
+ * Flag bits for rbd_dev->flags:
+ * - REMOVING (which is coupled with rbd_dev->open_count) is protected
+ *   by rbd_dev->lock
+ * - BLACKLISTED is protected by rbd_dev->lock_rwsem
  */
 enum rbd_dev_flags {
 	RBD_DEV_FLAG_EXISTS,	/* mapped snapshot has not been deleted */
 	RBD_DEV_FLAG_REMOVING,	/* this mapping is being removed */
+	RBD_DEV_FLAG_BLACKLISTED, /* our ceph_client is blacklisted */
 };
 
 static DEFINE_MUTEX(client_mutex);	/* Serialize client creation */
@@ -439,6 +476,29 @@ static int minor_to_rbd_dev_id(int minor)
 	return minor >> RBD_SINGLE_MAJOR_PART_SHIFT;
 }
 
+static bool rbd_is_lock_supported(struct rbd_device *rbd_dev)
+{
+	return (rbd_dev->header.features & RBD_FEATURE_EXCLUSIVE_LOCK) &&
+	       rbd_dev->spec->snap_id == CEPH_NOSNAP &&
+	       !rbd_dev->mapping.read_only;
+}
+
+static bool __rbd_is_lock_owner(struct rbd_device *rbd_dev)
+{
+	return rbd_dev->lock_state == RBD_LOCK_STATE_LOCKED ||
+	       rbd_dev->lock_state == RBD_LOCK_STATE_RELEASING;
+}
+
+static bool rbd_is_lock_owner(struct rbd_device *rbd_dev)
+{
+	bool is_lock_owner;
+
+	down_read(&rbd_dev->lock_rwsem);
+	is_lock_owner = __rbd_is_lock_owner(rbd_dev);
+	up_read(&rbd_dev->lock_rwsem);
+	return is_lock_owner;
+}
+
 static BUS_ATTR(add, S_IWUSR, NULL, rbd_add);
 static BUS_ATTR(remove, S_IWUSR, NULL, rbd_remove);
 static BUS_ATTR(add_single_major, S_IWUSR, NULL, rbd_add_single_major);
@@ -735,6 +795,7 @@ enum {
 	/* string args above */
 	Opt_read_only,
 	Opt_read_write,
+	Opt_lock_on_read,
 	Opt_err
 };
 
@@ -746,16 +807,19 @@ static match_table_t rbd_opts_tokens = {
 	{Opt_read_only, "ro"},		/* Alternate spelling */
 	{Opt_read_write, "read_write"},
 	{Opt_read_write, "rw"},		/* Alternate spelling */
+	{Opt_lock_on_read, "lock_on_read"},
 	{Opt_err, NULL}
 };
 
 struct rbd_options {
 	int	queue_depth;
 	bool	read_only;
+	bool	lock_on_read;
 };
 
 #define RBD_QUEUE_DEPTH_DEFAULT	BLKDEV_MAX_RQ
 #define RBD_READ_ONLY_DEFAULT	false
+#define RBD_LOCK_ON_READ_DEFAULT false
 
 static int parse_rbd_opts_token(char *c, void *private)
 {
@@ -791,6 +855,9 @@ static int parse_rbd_opts_token(char *c, void *private)
 	case Opt_read_write:
 		rbd_opts->read_only = false;
 		break;
+	case Opt_lock_on_read:
+		rbd_opts->lock_on_read = true;
+		break;
 	default:
 		/* libceph prints "bad option" msg */
 		return -EINVAL;
@@ -919,7 +986,6 @@ static int rbd_header_from_disk(struct rbd_device *rbd_dev,
 	char *snap_names = NULL;
 	u64 *snap_sizes = NULL;
 	u32 snap_count;
-	size_t size;
 	int ret = -ENOMEM;
 	u32 i;
 
@@ -957,9 +1023,9 @@ static int rbd_header_from_disk(struct rbd_device *rbd_dev,
 			goto out_err;
 
 		/* ...as well as the array of their sizes. */
-
-		size = snap_count * sizeof (*header->snap_sizes);
-		snap_sizes = kmalloc(size, GFP_KERNEL);
+		snap_sizes = kmalloc_array(snap_count,
+					   sizeof(*header->snap_sizes),
+					   GFP_KERNEL);
 		if (!snap_sizes)
 			goto out_err;
 
@@ -1551,11 +1617,18 @@ static bool obj_request_type_valid(enum obj_request_type type)
 	}
 }
 
-static int rbd_obj_request_submit(struct ceph_osd_client *osdc,
-				struct rbd_obj_request *obj_request)
+static void rbd_img_obj_callback(struct rbd_obj_request *obj_request);
+
+static void rbd_obj_request_submit(struct rbd_obj_request *obj_request)
 {
-	dout("%s %p\n", __func__, obj_request);
-	return ceph_osdc_start_request(osdc, obj_request->osd_req, false);
+	struct ceph_osd_request *osd_req = obj_request->osd_req;
+
+	dout("%s %p osd_req %p\n", __func__, obj_request, osd_req);
+	if (obj_request_img_data_test(obj_request)) {
+		WARN_ON(obj_request->callback != rbd_img_obj_callback);
+		rbd_img_request_get(obj_request->img_request);
+	}
+	ceph_osdc_start_request(osd_req->r_osdc, osd_req, false);
 }
 
 static void rbd_obj_request_end(struct rbd_obj_request *obj_request)
@@ -1745,6 +1818,22 @@ static void rbd_obj_request_complete(struct rbd_obj_request *obj_request)
 		complete_all(&obj_request->completion);
 }
 
+static void rbd_obj_request_error(struct rbd_obj_request *obj_request, int err)
+{
+	obj_request->result = err;
+	obj_request->xferred = 0;
+	/*
+	 * kludge - mirror rbd_obj_request_submit() to match a put in
+	 * rbd_img_obj_callback()
+	 */
+	if (obj_request_img_data_test(obj_request)) {
+		WARN_ON(obj_request->callback != rbd_img_obj_callback);
+		rbd_img_request_get(obj_request->img_request);
+	}
+	obj_request_done_set(obj_request);
+	rbd_obj_request_complete(obj_request);
+}
+
 static void rbd_osd_read_callback(struct rbd_obj_request *obj_request)
 {
 	struct rbd_img_request *img_request = NULL;
@@ -1877,11 +1966,10 @@ static void rbd_osd_req_callback(struct ceph_osd_request *osd_req)
 
 static void rbd_osd_req_format_read(struct rbd_obj_request *obj_request)
 {
-	struct rbd_img_request *img_request = obj_request->img_request;
 	struct ceph_osd_request *osd_req = obj_request->osd_req;
 
-	if (img_request)
-		osd_req->r_snapid = img_request->snap_id;
+	rbd_assert(obj_request_img_data_test(obj_request));
+	osd_req->r_snapid = obj_request->img_request->snap_id;
 }
 
 static void rbd_osd_req_format_write(struct rbd_obj_request *obj_request)
@@ -2074,7 +2162,9 @@ static void rbd_obj_request_destroy(struct kref *kref)
 			bio_chain_put(obj_request->bio_list);
 		break;
 	case OBJ_REQUEST_PAGES:
-		if (obj_request->pages)
+		/* img_data requests don't own their page array */
+		if (obj_request->pages &&
+		    !obj_request_img_data_test(obj_request))
 			ceph_release_page_vector(obj_request->pages,
 						obj_request->page_count);
 		break;
@@ -2295,13 +2385,6 @@ static bool rbd_img_obj_end_request(struct rbd_obj_request *obj_request)
 		xferred = obj_request->length;
 	}
 
-	/* Image object requests don't own their page array */
-
-	if (obj_request->type == OBJ_REQUEST_PAGES) {
-		obj_request->pages = NULL;
-		obj_request->page_count = 0;
-	}
-
 	if (img_request_child_test(img_request)) {
 		rbd_assert(img_request->obj_request != NULL);
 		more = obj_request->which < img_request->obj_request_count - 1;
@@ -2520,8 +2603,6 @@ static int rbd_img_request_fill(struct rbd_img_request *img_request,
 
 		rbd_img_obj_request_fill(obj_request, osd_req, op_type, 0);
 
-		rbd_img_request_get(img_request);
-
 		img_offset += length;
 		resid -= length;
 	}
@@ -2579,7 +2660,6 @@ rbd_img_obj_parent_read_full_callback(struct rbd_img_request *img_request)
 {
 	struct rbd_obj_request *orig_request;
 	struct ceph_osd_request *osd_req;
-	struct ceph_osd_client *osdc;
 	struct rbd_device *rbd_dev;
 	struct page **pages;
 	enum obj_operation_type op_type;
@@ -2603,7 +2683,7 @@ rbd_img_obj_parent_read_full_callback(struct rbd_img_request *img_request)
 	rbd_assert(obj_request_type_valid(orig_request->type));
 	img_result = img_request->result;
 	parent_length = img_request->length;
-	rbd_assert(parent_length == img_request->xferred);
+	rbd_assert(img_result || parent_length == img_request->xferred);
 	rbd_img_request_put(img_request);
 
 	rbd_assert(orig_request->img_request);
@@ -2616,13 +2696,9 @@ rbd_img_obj_parent_read_full_callback(struct rbd_img_request *img_request)
 	 * and re-submit the original write request.
 	 */
 	if (!rbd_dev->parent_overlap) {
-		struct ceph_osd_client *osdc;
-
 		ceph_release_page_vector(pages, page_count);
-		osdc = &rbd_dev->rbd_client->client->osdc;
-		img_result = rbd_obj_request_submit(osdc, orig_request);
-		if (!img_result)
-			return;
+		rbd_obj_request_submit(orig_request);
+		return;
 	}
 
 	if (img_result)
@@ -2656,17 +2732,12 @@ rbd_img_obj_parent_read_full_callback(struct rbd_img_request *img_request)
 
 	/* All set, send it off. */
 
-	osdc = &rbd_dev->rbd_client->client->osdc;
-	img_result = rbd_obj_request_submit(osdc, orig_request);
-	if (!img_result)
-		return;
-out_err:
-	/* Record the error code and complete the request */
+	rbd_obj_request_submit(orig_request);
+	return;
 
-	orig_request->result = img_result;
-	orig_request->xferred = 0;
-	obj_request_done_set(orig_request);
-	rbd_obj_request_complete(orig_request);
+out_err:
+	ceph_release_page_vector(pages, page_count);
+	rbd_obj_request_error(orig_request, img_result);
 }
 
 /*
@@ -2680,26 +2751,19 @@ out_err:
  * When the read completes, this page array will be transferred to
  * the original object request for the copyup operation.
  *
- * If an error occurs, record it as the result of the original
- * object request and mark it done so it gets completed.
+ * If an error occurs, it is recorded as the result of the original
+ * object request in rbd_img_obj_exists_callback().
  */
 static int rbd_img_obj_parent_read_full(struct rbd_obj_request *obj_request)
 {
-	struct rbd_img_request *img_request = NULL;
+	struct rbd_device *rbd_dev = obj_request->img_request->rbd_dev;
 	struct rbd_img_request *parent_request = NULL;
-	struct rbd_device *rbd_dev;
 	u64 img_offset;
 	u64 length;
 	struct page **pages = NULL;
 	u32 page_count;
 	int result;
 
-	rbd_assert(obj_request_img_data_test(obj_request));
-	rbd_assert(obj_request_type_valid(obj_request->type));
-
-	img_request = obj_request->img_request;
-	rbd_assert(img_request != NULL);
-	rbd_dev = img_request->rbd_dev;
 	rbd_assert(rbd_dev->parent != NULL);
 
 	/*
@@ -2740,10 +2804,11 @@ static int rbd_img_obj_parent_read_full(struct rbd_obj_request *obj_request)
 	result = rbd_img_request_fill(parent_request, OBJ_REQUEST_PAGES, pages);
 	if (result)
 		goto out_err;
+
 	parent_request->copyup_pages = pages;
 	parent_request->copyup_page_count = page_count;
-
 	parent_request->callback = rbd_img_obj_parent_read_full_callback;
+
 	result = rbd_img_request_submit(parent_request);
 	if (!result)
 		return 0;
@@ -2757,10 +2822,6 @@ out_err:
 		ceph_release_page_vector(pages, page_count);
 	if (parent_request)
 		rbd_img_request_put(parent_request);
-	obj_request->result = result;
-	obj_request->xferred = 0;
-	obj_request_done_set(obj_request);
-
 	return result;
 }
 
@@ -2793,17 +2854,13 @@ static void rbd_img_obj_exists_callback(struct rbd_obj_request *obj_request)
 
 	/*
 	 * If the overlap has become 0 (most likely because the
-	 * image has been flattened) we need to free the pages
-	 * and re-submit the original write request.
+	 * image has been flattened) we need to re-submit the
+	 * original request.
 	 */
 	rbd_dev = orig_request->img_request->rbd_dev;
 	if (!rbd_dev->parent_overlap) {
-		struct ceph_osd_client *osdc;
-
-		osdc = &rbd_dev->rbd_client->client->osdc;
-		result = rbd_obj_request_submit(osdc, orig_request);
-		if (!result)
-			return;
+		rbd_obj_request_submit(orig_request);
+		return;
 	}
 
 	/*
@@ -2816,31 +2873,45 @@ static void rbd_img_obj_exists_callback(struct rbd_obj_request *obj_request)
 		obj_request_existence_set(orig_request, true);
 	} else if (result == -ENOENT) {
 		obj_request_existence_set(orig_request, false);
-	} else if (result) {
-		orig_request->result = result;
-		goto out;
+	} else {
+		goto fail_orig_request;
 	}
 
 	/*
 	 * Resubmit the original request now that we have recorded
 	 * whether the target object exists.
 	 */
-	orig_request->result = rbd_img_obj_request_submit(orig_request);
-out:
-	if (orig_request->result)
-		rbd_obj_request_complete(orig_request);
+	result = rbd_img_obj_request_submit(orig_request);
+	if (result)
+		goto fail_orig_request;
+
+	return;
+
+fail_orig_request:
+	rbd_obj_request_error(orig_request, result);
 }
 
 static int rbd_img_obj_exists_submit(struct rbd_obj_request *obj_request)
 {
+	struct rbd_device *rbd_dev = obj_request->img_request->rbd_dev;
 	struct rbd_obj_request *stat_request;
-	struct rbd_device *rbd_dev;
-	struct ceph_osd_client *osdc;
-	struct page **pages = NULL;
+	struct page **pages;
 	u32 page_count;
 	size_t size;
 	int ret;
 
+	stat_request = rbd_obj_request_create(obj_request->object_name, 0, 0,
+					      OBJ_REQUEST_PAGES);
+	if (!stat_request)
+		return -ENOMEM;
+
+	stat_request->osd_req = rbd_osd_req_create(rbd_dev, OBJ_OP_READ, 1,
+						   stat_request);
+	if (!stat_request->osd_req) {
+		ret = -ENOMEM;
+		goto fail_stat_request;
+	}
+
 	/*
 	 * The response data for a STAT call consists of:
 	 *     le64 length;
@@ -2852,52 +2923,33 @@ static int rbd_img_obj_exists_submit(struct rbd_obj_request *obj_request)
 	size = sizeof (__le64) + sizeof (__le32) + sizeof (__le32);
 	page_count = (u32)calc_pages_for(0, size);
 	pages = ceph_alloc_page_vector(page_count, GFP_KERNEL);
-	if (IS_ERR(pages))
-		return PTR_ERR(pages);
+	if (IS_ERR(pages)) {
+		ret = PTR_ERR(pages);
+		goto fail_stat_request;
+	}
 
-	ret = -ENOMEM;
-	stat_request = rbd_obj_request_create(obj_request->object_name, 0, 0,
-							OBJ_REQUEST_PAGES);
-	if (!stat_request)
-		goto out;
+	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_obj_request_get(obj_request);
 	stat_request->obj_request = obj_request;
 	stat_request->pages = pages;
 	stat_request->page_count = page_count;
-
-	rbd_assert(obj_request->img_request);
-	rbd_dev = obj_request->img_request->rbd_dev;
-	stat_request->osd_req = rbd_osd_req_create(rbd_dev, OBJ_OP_READ, 1,
-						   stat_request);
-	if (!stat_request->osd_req)
-		goto out;
 	stat_request->callback = rbd_img_obj_exists_callback;
 
-	osd_req_op_init(stat_request->osd_req, 0, CEPH_OSD_OP_STAT, 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);
-
-	osdc = &rbd_dev->rbd_client->client->osdc;
-	ret = rbd_obj_request_submit(osdc, stat_request);
-out:
-	if (ret)
-		rbd_obj_request_put(obj_request);
+	rbd_obj_request_submit(stat_request);
+	return 0;
 
+fail_stat_request:
+	rbd_obj_request_put(stat_request);
 	return ret;
 }
 
 static bool img_obj_request_simple(struct rbd_obj_request *obj_request)
 {
-	struct rbd_img_request *img_request;
-	struct rbd_device *rbd_dev;
-
-	rbd_assert(obj_request_img_data_test(obj_request));
-
-	img_request = obj_request->img_request;
-	rbd_assert(img_request);
-	rbd_dev = img_request->rbd_dev;
+	struct rbd_img_request *img_request = obj_request->img_request;
+	struct rbd_device *rbd_dev = img_request->rbd_dev;
 
 	/* Reads */
 	if (!img_request_write_test(img_request) &&
@@ -2936,14 +2988,13 @@ static bool img_obj_request_simple(struct rbd_obj_request *obj_request)
 
 static int rbd_img_obj_request_submit(struct rbd_obj_request *obj_request)
 {
-	if (img_obj_request_simple(obj_request)) {
-		struct rbd_device *rbd_dev;
-		struct ceph_osd_client *osdc;
-
-		rbd_dev = obj_request->img_request->rbd_dev;
-		osdc = &rbd_dev->rbd_client->client->osdc;
+	rbd_assert(obj_request_img_data_test(obj_request));
+	rbd_assert(obj_request_type_valid(obj_request->type));
+	rbd_assert(obj_request->img_request);
 
-		return rbd_obj_request_submit(osdc, obj_request);
+	if (img_obj_request_simple(obj_request)) {
+		rbd_obj_request_submit(obj_request);
+		return 0;
 	}
 
 	/*
@@ -3006,12 +3057,8 @@ static void rbd_img_parent_read_callback(struct rbd_img_request *img_request)
 	rbd_assert(obj_request->img_request);
 	rbd_dev = obj_request->img_request->rbd_dev;
 	if (!rbd_dev->parent_overlap) {
-		struct ceph_osd_client *osdc;
-
-		osdc = &rbd_dev->rbd_client->client->osdc;
-		img_result = rbd_obj_request_submit(osdc, obj_request);
-		if (!img_result)
-			return;
+		rbd_obj_request_submit(obj_request);
+		return;
 	}
 
 	obj_request->result = img_result;
@@ -3084,65 +3131,724 @@ out_err:
 	obj_request_done_set(obj_request);
 }
 
-static int rbd_dev_header_watch_sync(struct rbd_device *rbd_dev);
-static void __rbd_dev_header_unwatch_sync(struct rbd_device *rbd_dev);
+static const struct rbd_client_id rbd_empty_cid;
 
-static void rbd_watch_cb(void *arg, u64 notify_id, u64 cookie,
-			 u64 notifier_id, void *data, size_t data_len)
+static bool rbd_cid_equal(const struct rbd_client_id *lhs,
+			  const struct rbd_client_id *rhs)
+{
+	return lhs->gid == rhs->gid && lhs->handle == rhs->handle;
+}
+
+static struct rbd_client_id rbd_get_cid(struct rbd_device *rbd_dev)
+{
+	struct rbd_client_id cid;
+
+	mutex_lock(&rbd_dev->watch_mutex);
+	cid.gid = ceph_client_gid(rbd_dev->rbd_client->client);
+	cid.handle = rbd_dev->watch_cookie;
+	mutex_unlock(&rbd_dev->watch_mutex);
+	return cid;
+}
+
+/*
+ * lock_rwsem must be held for write
+ */
+static void rbd_set_owner_cid(struct rbd_device *rbd_dev,
+			      const struct rbd_client_id *cid)
+{
+	dout("%s rbd_dev %p %llu-%llu -> %llu-%llu\n", __func__, rbd_dev,
+	     rbd_dev->owner_cid.gid, rbd_dev->owner_cid.handle,
+	     cid->gid, cid->handle);
+	rbd_dev->owner_cid = *cid; /* struct */
+}
+
+static void format_lock_cookie(struct rbd_device *rbd_dev, char *buf)
+{
+	mutex_lock(&rbd_dev->watch_mutex);
+	sprintf(buf, "%s %llu", RBD_LOCK_COOKIE_PREFIX, rbd_dev->watch_cookie);
+	mutex_unlock(&rbd_dev->watch_mutex);
+}
+
+/*
+ * lock_rwsem must be held for write
+ */
+static int rbd_lock(struct rbd_device *rbd_dev)
+{
+	struct ceph_osd_client *osdc = &rbd_dev->rbd_client->client->osdc;
+	struct rbd_client_id cid = rbd_get_cid(rbd_dev);
+	char cookie[32];
+	int ret;
+
+	WARN_ON(__rbd_is_lock_owner(rbd_dev));
+
+	format_lock_cookie(rbd_dev, cookie);
+	ret = ceph_cls_lock(osdc, &rbd_dev->header_oid, &rbd_dev->header_oloc,
+			    RBD_LOCK_NAME, CEPH_CLS_LOCK_EXCLUSIVE, cookie,
+			    RBD_LOCK_TAG, "", 0);
+	if (ret)
+		return ret;
+
+	rbd_dev->lock_state = RBD_LOCK_STATE_LOCKED;
+	rbd_set_owner_cid(rbd_dev, &cid);
+	queue_work(rbd_dev->task_wq, &rbd_dev->acquired_lock_work);
+	return 0;
+}
+
+/*
+ * lock_rwsem must be held for write
+ */
+static int rbd_unlock(struct rbd_device *rbd_dev)
+{
+	struct ceph_osd_client *osdc = &rbd_dev->rbd_client->client->osdc;
+	char cookie[32];
+	int ret;
+
+	WARN_ON(!__rbd_is_lock_owner(rbd_dev));
+
+	rbd_dev->lock_state = RBD_LOCK_STATE_UNLOCKED;
+
+	format_lock_cookie(rbd_dev, cookie);
+	ret = ceph_cls_unlock(osdc, &rbd_dev->header_oid, &rbd_dev->header_oloc,
+			      RBD_LOCK_NAME, cookie);
+	if (ret && ret != -ENOENT) {
+		rbd_warn(rbd_dev, "cls_unlock failed: %d", ret);
+		return ret;
+	}
+
+	rbd_set_owner_cid(rbd_dev, &rbd_empty_cid);
+	queue_work(rbd_dev->task_wq, &rbd_dev->released_lock_work);
+	return 0;
+}
+
+static int __rbd_notify_op_lock(struct rbd_device *rbd_dev,
+				enum rbd_notify_op notify_op,
+				struct page ***preply_pages,
+				size_t *preply_len)
 {
-	struct rbd_device *rbd_dev = arg;
 	struct ceph_osd_client *osdc = &rbd_dev->rbd_client->client->osdc;
+	struct rbd_client_id cid = rbd_get_cid(rbd_dev);
+	int buf_size = 4 + 8 + 8 + CEPH_ENCODING_START_BLK_LEN;
+	char buf[buf_size];
+	void *p = buf;
+
+	dout("%s rbd_dev %p notify_op %d\n", __func__, rbd_dev, notify_op);
+
+	/* encode *LockPayload NotifyMessage (op + ClientId) */
+	ceph_start_encoding(&p, 2, 1, buf_size - CEPH_ENCODING_START_BLK_LEN);
+	ceph_encode_32(&p, notify_op);
+	ceph_encode_64(&p, cid.gid);
+	ceph_encode_64(&p, cid.handle);
+
+	return ceph_osdc_notify(osdc, &rbd_dev->header_oid,
+				&rbd_dev->header_oloc, buf, buf_size,
+				RBD_NOTIFY_TIMEOUT, preply_pages, preply_len);
+}
+
+static void rbd_notify_op_lock(struct rbd_device *rbd_dev,
+			       enum rbd_notify_op notify_op)
+{
+	struct page **reply_pages;
+	size_t reply_len;
+
+	__rbd_notify_op_lock(rbd_dev, notify_op, &reply_pages, &reply_len);
+	ceph_release_page_vector(reply_pages, calc_pages_for(0, reply_len));
+}
+
+static void rbd_notify_acquired_lock(struct work_struct *work)
+{
+	struct rbd_device *rbd_dev = container_of(work, struct rbd_device,
+						  acquired_lock_work);
+
+	rbd_notify_op_lock(rbd_dev, RBD_NOTIFY_OP_ACQUIRED_LOCK);
+}
+
+static void rbd_notify_released_lock(struct work_struct *work)
+{
+	struct rbd_device *rbd_dev = container_of(work, struct rbd_device,
+						  released_lock_work);
+
+	rbd_notify_op_lock(rbd_dev, RBD_NOTIFY_OP_RELEASED_LOCK);
+}
+
+static int rbd_request_lock(struct rbd_device *rbd_dev)
+{
+	struct page **reply_pages;
+	size_t reply_len;
+	bool lock_owner_responded = false;
+	int ret;
+
+	dout("%s rbd_dev %p\n", __func__, rbd_dev);
+
+	ret = __rbd_notify_op_lock(rbd_dev, RBD_NOTIFY_OP_REQUEST_LOCK,
+				   &reply_pages, &reply_len);
+	if (ret && ret != -ETIMEDOUT) {
+		rbd_warn(rbd_dev, "failed to request lock: %d", ret);
+		goto out;
+	}
+
+	if (reply_len > 0 && reply_len <= PAGE_SIZE) {
+		void *p = page_address(reply_pages[0]);
+		void *const end = p + reply_len;
+		u32 n;
+
+		ceph_decode_32_safe(&p, end, n, e_inval); /* num_acks */
+		while (n--) {
+			u8 struct_v;
+			u32 len;
+
+			ceph_decode_need(&p, end, 8 + 8, e_inval);
+			p += 8 + 8; /* skip gid and cookie */
+
+			ceph_decode_32_safe(&p, end, len, e_inval);
+			if (!len)
+				continue;
+
+			if (lock_owner_responded) {
+				rbd_warn(rbd_dev,
+					 "duplicate lock owners detected");
+				ret = -EIO;
+				goto out;
+			}
+
+			lock_owner_responded = true;
+			ret = ceph_start_decoding(&p, end, 1, "ResponseMessage",
+						  &struct_v, &len);
+			if (ret) {
+				rbd_warn(rbd_dev,
+					 "failed to decode ResponseMessage: %d",
+					 ret);
+				goto e_inval;
+			}
+
+			ret = ceph_decode_32(&p);
+		}
+	}
+
+	if (!lock_owner_responded) {
+		rbd_warn(rbd_dev, "no lock owners detected");
+		ret = -ETIMEDOUT;
+	}
+
+out:
+	ceph_release_page_vector(reply_pages, calc_pages_for(0, reply_len));
+	return ret;
+
+e_inval:
+	ret = -EINVAL;
+	goto out;
+}
+
+static void wake_requests(struct rbd_device *rbd_dev, bool wake_all)
+{
+	dout("%s rbd_dev %p wake_all %d\n", __func__, rbd_dev, wake_all);
+
+	cancel_delayed_work(&rbd_dev->lock_dwork);
+	if (wake_all)
+		wake_up_all(&rbd_dev->lock_waitq);
+	else
+		wake_up(&rbd_dev->lock_waitq);
+}
+
+static int get_lock_owner_info(struct rbd_device *rbd_dev,
+			       struct ceph_locker **lockers, u32 *num_lockers)
+{
+	struct ceph_osd_client *osdc = &rbd_dev->rbd_client->client->osdc;
+	u8 lock_type;
+	char *lock_tag;
+	int ret;
+
+	dout("%s rbd_dev %p\n", __func__, rbd_dev);
+
+	ret = ceph_cls_lock_info(osdc, &rbd_dev->header_oid,
+				 &rbd_dev->header_oloc, RBD_LOCK_NAME,
+				 &lock_type, &lock_tag, lockers, num_lockers);
+	if (ret)
+		return ret;
+
+	if (*num_lockers == 0) {
+		dout("%s rbd_dev %p no lockers detected\n", __func__, rbd_dev);
+		goto out;
+	}
+
+	if (strcmp(lock_tag, RBD_LOCK_TAG)) {
+		rbd_warn(rbd_dev, "locked by external mechanism, tag %s",
+			 lock_tag);
+		ret = -EBUSY;
+		goto out;
+	}
+
+	if (lock_type == CEPH_CLS_LOCK_SHARED) {
+		rbd_warn(rbd_dev, "shared lock type detected");
+		ret = -EBUSY;
+		goto out;
+	}
+
+	if (strncmp((*lockers)[0].id.cookie, RBD_LOCK_COOKIE_PREFIX,
+		    strlen(RBD_LOCK_COOKIE_PREFIX))) {
+		rbd_warn(rbd_dev, "locked by external mechanism, cookie %s",
+			 (*lockers)[0].id.cookie);
+		ret = -EBUSY;
+		goto out;
+	}
+
+out:
+	kfree(lock_tag);
+	return ret;
+}
+
+static int find_watcher(struct rbd_device *rbd_dev,
+			const struct ceph_locker *locker)
+{
+	struct ceph_osd_client *osdc = &rbd_dev->rbd_client->client->osdc;
+	struct ceph_watch_item *watchers;
+	u32 num_watchers;
+	u64 cookie;
+	int i;
+	int ret;
+
+	ret = ceph_osdc_list_watchers(osdc, &rbd_dev->header_oid,
+				      &rbd_dev->header_oloc, &watchers,
+				      &num_watchers);
+	if (ret)
+		return ret;
+
+	sscanf(locker->id.cookie, RBD_LOCK_COOKIE_PREFIX " %llu", &cookie);
+	for (i = 0; i < num_watchers; i++) {
+		if (!memcmp(&watchers[i].addr, &locker->info.addr,
+			    sizeof(locker->info.addr)) &&
+		    watchers[i].cookie == cookie) {
+			struct rbd_client_id cid = {
+				.gid = le64_to_cpu(watchers[i].name.num),
+				.handle = cookie,
+			};
+
+			dout("%s rbd_dev %p found cid %llu-%llu\n", __func__,
+			     rbd_dev, cid.gid, cid.handle);
+			rbd_set_owner_cid(rbd_dev, &cid);
+			ret = 1;
+			goto out;
+		}
+	}
+
+	dout("%s rbd_dev %p no watchers\n", __func__, rbd_dev);
+	ret = 0;
+out:
+	kfree(watchers);
+	return ret;
+}
+
+/*
+ * lock_rwsem must be held for write
+ */
+static int rbd_try_lock(struct rbd_device *rbd_dev)
+{
+	struct ceph_client *client = rbd_dev->rbd_client->client;
+	struct ceph_locker *lockers;
+	u32 num_lockers;
+	int ret;
+
+	for (;;) {
+		ret = rbd_lock(rbd_dev);
+		if (ret != -EBUSY)
+			return ret;
+
+		/* determine if the current lock holder is still alive */
+		ret = get_lock_owner_info(rbd_dev, &lockers, &num_lockers);
+		if (ret)
+			return ret;
+
+		if (num_lockers == 0)
+			goto again;
+
+		ret = find_watcher(rbd_dev, lockers);
+		if (ret) {
+			if (ret > 0)
+				ret = 0; /* have to request lock */
+			goto out;
+		}
+
+		rbd_warn(rbd_dev, "%s%llu seems dead, breaking lock",
+			 ENTITY_NAME(lockers[0].id.name));
+
+		ret = ceph_monc_blacklist_add(&client->monc,
+					      &lockers[0].info.addr);
+		if (ret) {
+			rbd_warn(rbd_dev, "blacklist of %s%llu failed: %d",
+				 ENTITY_NAME(lockers[0].id.name), ret);
+			goto out;
+		}
+
+		ret = ceph_cls_break_lock(&client->osdc, &rbd_dev->header_oid,
+					  &rbd_dev->header_oloc, RBD_LOCK_NAME,
+					  lockers[0].id.cookie,
+					  &lockers[0].id.name);
+		if (ret && ret != -ENOENT)
+			goto out;
+
+again:
+		ceph_free_lockers(lockers, num_lockers);
+	}
+
+out:
+	ceph_free_lockers(lockers, num_lockers);
+	return ret;
+}
+
+/*
+ * ret is set only if lock_state is RBD_LOCK_STATE_UNLOCKED
+ */
+static enum rbd_lock_state rbd_try_acquire_lock(struct rbd_device *rbd_dev,
+						int *pret)
+{
+	enum rbd_lock_state lock_state;
+
+	down_read(&rbd_dev->lock_rwsem);
+	dout("%s rbd_dev %p read lock_state %d\n", __func__, rbd_dev,
+	     rbd_dev->lock_state);
+	if (__rbd_is_lock_owner(rbd_dev)) {
+		lock_state = rbd_dev->lock_state;
+		up_read(&rbd_dev->lock_rwsem);
+		return lock_state;
+	}
+
+	up_read(&rbd_dev->lock_rwsem);
+	down_write(&rbd_dev->lock_rwsem);
+	dout("%s rbd_dev %p write lock_state %d\n", __func__, rbd_dev,
+	     rbd_dev->lock_state);
+	if (!__rbd_is_lock_owner(rbd_dev)) {
+		*pret = rbd_try_lock(rbd_dev);
+		if (*pret)
+			rbd_warn(rbd_dev, "failed to acquire lock: %d", *pret);
+	}
+
+	lock_state = rbd_dev->lock_state;
+	up_write(&rbd_dev->lock_rwsem);
+	return lock_state;
+}
+
+static void rbd_acquire_lock(struct work_struct *work)
+{
+	struct rbd_device *rbd_dev = container_of(to_delayed_work(work),
+					    struct rbd_device, lock_dwork);
+	enum rbd_lock_state lock_state;
 	int ret;
 
-	dout("%s rbd_dev %p cookie %llu notify_id %llu\n", __func__, rbd_dev,
-	     cookie, notify_id);
+	dout("%s rbd_dev %p\n", __func__, rbd_dev);
+again:
+	lock_state = rbd_try_acquire_lock(rbd_dev, &ret);
+	if (lock_state != RBD_LOCK_STATE_UNLOCKED || ret == -EBLACKLISTED) {
+		if (lock_state == RBD_LOCK_STATE_LOCKED)
+			wake_requests(rbd_dev, true);
+		dout("%s rbd_dev %p lock_state %d ret %d - done\n", __func__,
+		     rbd_dev, lock_state, ret);
+		return;
+	}
+
+	ret = rbd_request_lock(rbd_dev);
+	if (ret == -ETIMEDOUT) {
+		goto again; /* treat this as a dead client */
+	} else if (ret < 0) {
+		rbd_warn(rbd_dev, "error requesting lock: %d", ret);
+		mod_delayed_work(rbd_dev->task_wq, &rbd_dev->lock_dwork,
+				 RBD_RETRY_DELAY);
+	} else {
+		/*
+		 * lock owner acked, but resend if we don't see them
+		 * release the lock
+		 */
+		dout("%s rbd_dev %p requeueing lock_dwork\n", __func__,
+		     rbd_dev);
+		mod_delayed_work(rbd_dev->task_wq, &rbd_dev->lock_dwork,
+		    msecs_to_jiffies(2 * RBD_NOTIFY_TIMEOUT * MSEC_PER_SEC));
+	}
+}
+
+/*
+ * lock_rwsem must be held for write
+ */
+static bool rbd_release_lock(struct rbd_device *rbd_dev)
+{
+	dout("%s rbd_dev %p read lock_state %d\n", __func__, rbd_dev,
+	     rbd_dev->lock_state);
+	if (rbd_dev->lock_state != RBD_LOCK_STATE_LOCKED)
+		return false;
 
+	rbd_dev->lock_state = RBD_LOCK_STATE_RELEASING;
+	downgrade_write(&rbd_dev->lock_rwsem);
 	/*
-	 * Until adequate refresh error handling is in place, there is
-	 * not much we can do here, except warn.
+	 * Ensure that all in-flight IO is flushed.
 	 *
-	 * See http://tracker.ceph.com/issues/5040
+	 * FIXME: ceph_osdc_sync() flushes the entire OSD client, which
+	 * may be shared with other devices.
 	 */
-	ret = rbd_dev_refresh(rbd_dev);
-	if (ret)
-		rbd_warn(rbd_dev, "refresh failed: %d", ret);
+	ceph_osdc_sync(&rbd_dev->rbd_client->client->osdc);
+	up_read(&rbd_dev->lock_rwsem);
+
+	down_write(&rbd_dev->lock_rwsem);
+	dout("%s rbd_dev %p write lock_state %d\n", __func__, rbd_dev,
+	     rbd_dev->lock_state);
+	if (rbd_dev->lock_state != RBD_LOCK_STATE_RELEASING)
+		return false;
+
+	if (!rbd_unlock(rbd_dev))
+		/*
+		 * Give others a chance to grab the lock - we would re-acquire
+		 * almost immediately if we got new IO during ceph_osdc_sync()
+		 * otherwise.  We need to ack our own notifications, so this
+		 * lock_dwork will be requeued from rbd_wait_state_locked()
+		 * after wake_requests() in rbd_handle_released_lock().
+		 */
+		cancel_delayed_work(&rbd_dev->lock_dwork);
+
+	return true;
+}
+
+static void rbd_release_lock_work(struct work_struct *work)
+{
+	struct rbd_device *rbd_dev = container_of(work, struct rbd_device,
+						  unlock_work);
+
+	down_write(&rbd_dev->lock_rwsem);
+	rbd_release_lock(rbd_dev);
+	up_write(&rbd_dev->lock_rwsem);
+}
+
+static void rbd_handle_acquired_lock(struct rbd_device *rbd_dev, u8 struct_v,
+				     void **p)
+{
+	struct rbd_client_id cid = { 0 };
+
+	if (struct_v >= 2) {
+		cid.gid = ceph_decode_64(p);
+		cid.handle = ceph_decode_64(p);
+	}
+
+	dout("%s rbd_dev %p cid %llu-%llu\n", __func__, rbd_dev, cid.gid,
+	     cid.handle);
+	if (!rbd_cid_equal(&cid, &rbd_empty_cid)) {
+		down_write(&rbd_dev->lock_rwsem);
+		if (rbd_cid_equal(&cid, &rbd_dev->owner_cid)) {
+			/*
+			 * we already know that the remote client is
+			 * the owner
+			 */
+			up_write(&rbd_dev->lock_rwsem);
+			return;
+		}
+
+		rbd_set_owner_cid(rbd_dev, &cid);
+		downgrade_write(&rbd_dev->lock_rwsem);
+	} else {
+		down_read(&rbd_dev->lock_rwsem);
+	}
+
+	if (!__rbd_is_lock_owner(rbd_dev))
+		wake_requests(rbd_dev, false);
+	up_read(&rbd_dev->lock_rwsem);
+}
+
+static void rbd_handle_released_lock(struct rbd_device *rbd_dev, u8 struct_v,
+				     void **p)
+{
+	struct rbd_client_id cid = { 0 };
+
+	if (struct_v >= 2) {
+		cid.gid = ceph_decode_64(p);
+		cid.handle = ceph_decode_64(p);
+	}
+
+	dout("%s rbd_dev %p cid %llu-%llu\n", __func__, rbd_dev, cid.gid,
+	     cid.handle);
+	if (!rbd_cid_equal(&cid, &rbd_empty_cid)) {
+		down_write(&rbd_dev->lock_rwsem);
+		if (!rbd_cid_equal(&cid, &rbd_dev->owner_cid)) {
+			dout("%s rbd_dev %p unexpected owner, cid %llu-%llu != owner_cid %llu-%llu\n",
+			     __func__, rbd_dev, cid.gid, cid.handle,
+			     rbd_dev->owner_cid.gid, rbd_dev->owner_cid.handle);
+			up_write(&rbd_dev->lock_rwsem);
+			return;
+		}
+
+		rbd_set_owner_cid(rbd_dev, &rbd_empty_cid);
+		downgrade_write(&rbd_dev->lock_rwsem);
+	} else {
+		down_read(&rbd_dev->lock_rwsem);
+	}
+
+	if (!__rbd_is_lock_owner(rbd_dev))
+		wake_requests(rbd_dev, false);
+	up_read(&rbd_dev->lock_rwsem);
+}
+
+static bool rbd_handle_request_lock(struct rbd_device *rbd_dev, u8 struct_v,
+				    void **p)
+{
+	struct rbd_client_id my_cid = rbd_get_cid(rbd_dev);
+	struct rbd_client_id cid = { 0 };
+	bool need_to_send;
+
+	if (struct_v >= 2) {
+		cid.gid = ceph_decode_64(p);
+		cid.handle = ceph_decode_64(p);
+	}
+
+	dout("%s rbd_dev %p cid %llu-%llu\n", __func__, rbd_dev, cid.gid,
+	     cid.handle);
+	if (rbd_cid_equal(&cid, &my_cid))
+		return false;
+
+	down_read(&rbd_dev->lock_rwsem);
+	need_to_send = __rbd_is_lock_owner(rbd_dev);
+	if (rbd_dev->lock_state == RBD_LOCK_STATE_LOCKED) {
+		if (!rbd_cid_equal(&rbd_dev->owner_cid, &rbd_empty_cid)) {
+			dout("%s rbd_dev %p queueing unlock_work\n", __func__,
+			     rbd_dev);
+			queue_work(rbd_dev->task_wq, &rbd_dev->unlock_work);
+		}
+	}
+	up_read(&rbd_dev->lock_rwsem);
+	return need_to_send;
+}
+
+static void __rbd_acknowledge_notify(struct rbd_device *rbd_dev,
+				     u64 notify_id, u64 cookie, s32 *result)
+{
+	struct ceph_osd_client *osdc = &rbd_dev->rbd_client->client->osdc;
+	int buf_size = 4 + CEPH_ENCODING_START_BLK_LEN;
+	char buf[buf_size];
+	int ret;
+
+	if (result) {
+		void *p = buf;
+
+		/* encode ResponseMessage */
+		ceph_start_encoding(&p, 1, 1,
+				    buf_size - CEPH_ENCODING_START_BLK_LEN);
+		ceph_encode_32(&p, *result);
+	} else {
+		buf_size = 0;
+	}
 
 	ret = ceph_osdc_notify_ack(osdc, &rbd_dev->header_oid,
 				   &rbd_dev->header_oloc, notify_id, cookie,
-				   NULL, 0);
+				   buf, buf_size);
 	if (ret)
-		rbd_warn(rbd_dev, "notify_ack ret %d", ret);
+		rbd_warn(rbd_dev, "acknowledge_notify failed: %d", ret);
 }
 
-static void rbd_watch_errcb(void *arg, u64 cookie, int err)
+static void rbd_acknowledge_notify(struct rbd_device *rbd_dev, u64 notify_id,
+				   u64 cookie)
+{
+	dout("%s rbd_dev %p\n", __func__, rbd_dev);
+	__rbd_acknowledge_notify(rbd_dev, notify_id, cookie, NULL);
+}
+
+static void rbd_acknowledge_notify_result(struct rbd_device *rbd_dev,
+					  u64 notify_id, u64 cookie, s32 result)
+{
+	dout("%s rbd_dev %p result %d\n", __func__, rbd_dev, result);
+	__rbd_acknowledge_notify(rbd_dev, notify_id, cookie, &result);
+}
+
+static void rbd_watch_cb(void *arg, u64 notify_id, u64 cookie,
+			 u64 notifier_id, void *data, size_t data_len)
 {
 	struct rbd_device *rbd_dev = arg;
+	void *p = data;
+	void *const end = p + data_len;
+	u8 struct_v;
+	u32 len;
+	u32 notify_op;
 	int ret;
 
-	rbd_warn(rbd_dev, "encountered watch error: %d", err);
+	dout("%s rbd_dev %p cookie %llu notify_id %llu data_len %zu\n",
+	     __func__, rbd_dev, cookie, notify_id, data_len);
+	if (data_len) {
+		ret = ceph_start_decoding(&p, end, 1, "NotifyMessage",
+					  &struct_v, &len);
+		if (ret) {
+			rbd_warn(rbd_dev, "failed to decode NotifyMessage: %d",
+				 ret);
+			return;
+		}
 
-	__rbd_dev_header_unwatch_sync(rbd_dev);
+		notify_op = ceph_decode_32(&p);
+	} else {
+		/* legacy notification for header updates */
+		notify_op = RBD_NOTIFY_OP_HEADER_UPDATE;
+		len = 0;
+	}
 
-	ret = rbd_dev_header_watch_sync(rbd_dev);
-	if (ret) {
-		rbd_warn(rbd_dev, "failed to reregister watch: %d", ret);
-		return;
+	dout("%s rbd_dev %p notify_op %u\n", __func__, rbd_dev, notify_op);
+	switch (notify_op) {
+	case RBD_NOTIFY_OP_ACQUIRED_LOCK:
+		rbd_handle_acquired_lock(rbd_dev, struct_v, &p);
+		rbd_acknowledge_notify(rbd_dev, notify_id, cookie);
+		break;
+	case RBD_NOTIFY_OP_RELEASED_LOCK:
+		rbd_handle_released_lock(rbd_dev, struct_v, &p);
+		rbd_acknowledge_notify(rbd_dev, notify_id, cookie);
+		break;
+	case RBD_NOTIFY_OP_REQUEST_LOCK:
+		if (rbd_handle_request_lock(rbd_dev, struct_v, &p))
+			/*
+			 * send ResponseMessage(0) back so the client
+			 * can detect a missing owner
+			 */
+			rbd_acknowledge_notify_result(rbd_dev, notify_id,
+						      cookie, 0);
+		else
+			rbd_acknowledge_notify(rbd_dev, notify_id, cookie);
+		break;
+	case RBD_NOTIFY_OP_HEADER_UPDATE:
+		ret = rbd_dev_refresh(rbd_dev);
+		if (ret)
+			rbd_warn(rbd_dev, "refresh failed: %d", ret);
+
+		rbd_acknowledge_notify(rbd_dev, notify_id, cookie);
+		break;
+	default:
+		if (rbd_is_lock_owner(rbd_dev))
+			rbd_acknowledge_notify_result(rbd_dev, notify_id,
+						      cookie, -EOPNOTSUPP);
+		else
+			rbd_acknowledge_notify(rbd_dev, notify_id, cookie);
+		break;
 	}
+}
 
-	ret = rbd_dev_refresh(rbd_dev);
-	if (ret)
-		rbd_warn(rbd_dev, "reregisteration refresh failed: %d", ret);
+static void __rbd_unregister_watch(struct rbd_device *rbd_dev);
+
+static void rbd_watch_errcb(void *arg, u64 cookie, int err)
+{
+	struct rbd_device *rbd_dev = arg;
+
+	rbd_warn(rbd_dev, "encountered watch error: %d", err);
+
+	down_write(&rbd_dev->lock_rwsem);
+	rbd_set_owner_cid(rbd_dev, &rbd_empty_cid);
+	up_write(&rbd_dev->lock_rwsem);
+
+	mutex_lock(&rbd_dev->watch_mutex);
+	if (rbd_dev->watch_state == RBD_WATCH_STATE_REGISTERED) {
+		__rbd_unregister_watch(rbd_dev);
+		rbd_dev->watch_state = RBD_WATCH_STATE_ERROR;
+
+		queue_delayed_work(rbd_dev->task_wq, &rbd_dev->watch_dwork, 0);
+	}
+	mutex_unlock(&rbd_dev->watch_mutex);
 }
 
 /*
- * Initiate a watch request, synchronously.
+ * watch_mutex must be locked
  */
-static int rbd_dev_header_watch_sync(struct rbd_device *rbd_dev)
+static int __rbd_register_watch(struct rbd_device *rbd_dev)
 {
 	struct ceph_osd_client *osdc = &rbd_dev->rbd_client->client->osdc;
 	struct ceph_osd_linger_request *handle;
 
 	rbd_assert(!rbd_dev->watch_handle);
+	dout("%s rbd_dev %p\n", __func__, rbd_dev);
 
 	handle = ceph_osdc_watch(osdc, &rbd_dev->header_oid,
 				 &rbd_dev->header_oloc, rbd_watch_cb,
@@ -3154,13 +3860,16 @@ static int rbd_dev_header_watch_sync(struct rbd_device *rbd_dev)
 	return 0;
 }
 
-static void __rbd_dev_header_unwatch_sync(struct rbd_device *rbd_dev)
+/*
+ * watch_mutex must be locked
+ */
+static void __rbd_unregister_watch(struct rbd_device *rbd_dev)
 {
 	struct ceph_osd_client *osdc = &rbd_dev->rbd_client->client->osdc;
 	int ret;
 
-	if (!rbd_dev->watch_handle)
-		return;
+	rbd_assert(rbd_dev->watch_handle);
+	dout("%s rbd_dev %p\n", __func__, rbd_dev);
 
 	ret = ceph_osdc_unwatch(osdc, rbd_dev->watch_handle);
 	if (ret)
@@ -3169,17 +3878,106 @@ static void __rbd_dev_header_unwatch_sync(struct rbd_device *rbd_dev)
 	rbd_dev->watch_handle = NULL;
 }
 
-/*
- * Tear down a watch request, synchronously.
- */
-static void rbd_dev_header_unwatch_sync(struct rbd_device *rbd_dev)
+static int rbd_register_watch(struct rbd_device *rbd_dev)
 {
-	__rbd_dev_header_unwatch_sync(rbd_dev);
+	int ret;
+
+	mutex_lock(&rbd_dev->watch_mutex);
+	rbd_assert(rbd_dev->watch_state == RBD_WATCH_STATE_UNREGISTERED);
+	ret = __rbd_register_watch(rbd_dev);
+	if (ret)
+		goto out;
+
+	rbd_dev->watch_state = RBD_WATCH_STATE_REGISTERED;
+	rbd_dev->watch_cookie = rbd_dev->watch_handle->linger_id;
+
+out:
+	mutex_unlock(&rbd_dev->watch_mutex);
+	return ret;
+}
+
+static void cancel_tasks_sync(struct rbd_device *rbd_dev)
+{
+	dout("%s rbd_dev %p\n", __func__, rbd_dev);
+
+	cancel_delayed_work_sync(&rbd_dev->watch_dwork);
+	cancel_work_sync(&rbd_dev->acquired_lock_work);
+	cancel_work_sync(&rbd_dev->released_lock_work);
+	cancel_delayed_work_sync(&rbd_dev->lock_dwork);
+	cancel_work_sync(&rbd_dev->unlock_work);
+}
+
+static void rbd_unregister_watch(struct rbd_device *rbd_dev)
+{
+	WARN_ON(waitqueue_active(&rbd_dev->lock_waitq));
+	cancel_tasks_sync(rbd_dev);
+
+	mutex_lock(&rbd_dev->watch_mutex);
+	if (rbd_dev->watch_state == RBD_WATCH_STATE_REGISTERED)
+		__rbd_unregister_watch(rbd_dev);
+	rbd_dev->watch_state = RBD_WATCH_STATE_UNREGISTERED;
+	mutex_unlock(&rbd_dev->watch_mutex);
 
-	dout("%s flushing notifies\n", __func__);
 	ceph_osdc_flush_notifies(&rbd_dev->rbd_client->client->osdc);
 }
 
+static void rbd_reregister_watch(struct work_struct *work)
+{
+	struct rbd_device *rbd_dev = container_of(to_delayed_work(work),
+					    struct rbd_device, watch_dwork);
+	bool was_lock_owner = false;
+	bool need_to_wake = false;
+	int ret;
+
+	dout("%s rbd_dev %p\n", __func__, rbd_dev);
+
+	down_write(&rbd_dev->lock_rwsem);
+	if (rbd_dev->lock_state == RBD_LOCK_STATE_LOCKED)
+		was_lock_owner = rbd_release_lock(rbd_dev);
+
+	mutex_lock(&rbd_dev->watch_mutex);
+	if (rbd_dev->watch_state != RBD_WATCH_STATE_ERROR) {
+		mutex_unlock(&rbd_dev->watch_mutex);
+		goto out;
+	}
+
+	ret = __rbd_register_watch(rbd_dev);
+	if (ret) {
+		rbd_warn(rbd_dev, "failed to reregister watch: %d", ret);
+		if (ret == -EBLACKLISTED || ret == -ENOENT) {
+			set_bit(RBD_DEV_FLAG_BLACKLISTED, &rbd_dev->flags);
+			need_to_wake = true;
+		} else {
+			queue_delayed_work(rbd_dev->task_wq,
+					   &rbd_dev->watch_dwork,
+					   RBD_RETRY_DELAY);
+		}
+		mutex_unlock(&rbd_dev->watch_mutex);
+		goto out;
+	}
+
+	need_to_wake = true;
+	rbd_dev->watch_state = RBD_WATCH_STATE_REGISTERED;
+	rbd_dev->watch_cookie = rbd_dev->watch_handle->linger_id;
+	mutex_unlock(&rbd_dev->watch_mutex);
+
+	ret = rbd_dev_refresh(rbd_dev);
+	if (ret)
+		rbd_warn(rbd_dev, "reregisteration refresh failed: %d", ret);
+
+	if (was_lock_owner) {
+		ret = rbd_try_lock(rbd_dev);
+		if (ret)
+			rbd_warn(rbd_dev, "reregisteration lock failed: %d",
+				 ret);
+	}
+
+out:
+	up_write(&rbd_dev->lock_rwsem);
+	if (need_to_wake)
+		wake_requests(rbd_dev, true);
+}
+
 /*
  * Synchronous osd object method call.  Returns the number of bytes
  * returned in the outbound buffer, or a negative error code.
@@ -3193,7 +3991,6 @@ static int rbd_obj_method_sync(struct rbd_device *rbd_dev,
 			     void *inbound,
 			     size_t inbound_size)
 {
-	struct ceph_osd_client *osdc = &rbd_dev->rbd_client->client->osdc;
 	struct rbd_obj_request *obj_request;
 	struct page **pages;
 	u32 page_count;
@@ -3242,11 +4039,8 @@ static int rbd_obj_method_sync(struct rbd_device *rbd_dev,
 	osd_req_op_cls_response_data_pages(obj_request->osd_req, 0,
 					obj_request->pages, inbound_size,
 					0, false, false);
-	rbd_osd_req_format_read(obj_request);
 
-	ret = rbd_obj_request_submit(osdc, obj_request);
-	if (ret)
-		goto out;
+	rbd_obj_request_submit(obj_request);
 	ret = rbd_obj_request_wait(obj_request);
 	if (ret)
 		goto out;
@@ -3267,6 +4061,31 @@ out:
 	return ret;
 }
 
+/*
+ * lock_rwsem must be held for read
+ */
+static void rbd_wait_state_locked(struct rbd_device *rbd_dev)
+{
+	DEFINE_WAIT(wait);
+
+	do {
+		/*
+		 * Note the use of mod_delayed_work() in rbd_acquire_lock()
+		 * and cancel_delayed_work() in wake_requests().
+		 */
+		dout("%s rbd_dev %p queueing lock_dwork\n", __func__, rbd_dev);
+		queue_delayed_work(rbd_dev->task_wq, &rbd_dev->lock_dwork, 0);
+		prepare_to_wait_exclusive(&rbd_dev->lock_waitq, &wait,
+					  TASK_UNINTERRUPTIBLE);
+		up_read(&rbd_dev->lock_rwsem);
+		schedule();
+		down_read(&rbd_dev->lock_rwsem);
+	} while (rbd_dev->lock_state != RBD_LOCK_STATE_LOCKED &&
+		 !test_bit(RBD_DEV_FLAG_BLACKLISTED, &rbd_dev->flags));
+
+	finish_wait(&rbd_dev->lock_waitq, &wait);
+}
+
 static void rbd_queue_workfn(struct work_struct *work)
 {
 	struct request *rq = blk_mq_rq_from_pdu(work);
@@ -3277,6 +4096,7 @@ static void rbd_queue_workfn(struct work_struct *work)
 	u64 length = blk_rq_bytes(rq);
 	enum obj_operation_type op_type;
 	u64 mapping_size;
+	bool must_be_locked;
 	int result;
 
 	if (rq->cmd_type != REQ_TYPE_FS) {
@@ -3338,6 +4158,10 @@ static void rbd_queue_workfn(struct work_struct *work)
 	if (op_type != OBJ_OP_READ) {
 		snapc = rbd_dev->header.snapc;
 		ceph_get_snap_context(snapc);
+		must_be_locked = rbd_is_lock_supported(rbd_dev);
+	} else {
+		must_be_locked = rbd_dev->opts->lock_on_read &&
+					rbd_is_lock_supported(rbd_dev);
 	}
 	up_read(&rbd_dev->header_rwsem);
 
@@ -3348,11 +4172,25 @@ static void rbd_queue_workfn(struct work_struct *work)
 		goto err_rq;
 	}
 
+	if (must_be_locked) {
+		down_read(&rbd_dev->lock_rwsem);
+		if (rbd_dev->lock_state != RBD_LOCK_STATE_LOCKED &&
+		    !test_bit(RBD_DEV_FLAG_BLACKLISTED, &rbd_dev->flags))
+			rbd_wait_state_locked(rbd_dev);
+
+		WARN_ON((rbd_dev->lock_state == RBD_LOCK_STATE_LOCKED) ^
+			!test_bit(RBD_DEV_FLAG_BLACKLISTED, &rbd_dev->flags));
+		if (test_bit(RBD_DEV_FLAG_BLACKLISTED, &rbd_dev->flags)) {
+			result = -EBLACKLISTED;
+			goto err_unlock;
+		}
+	}
+
 	img_request = rbd_img_request_create(rbd_dev, offset, length, op_type,
 					     snapc);
 	if (!img_request) {
 		result = -ENOMEM;
-		goto err_rq;
+		goto err_unlock;
 	}
 	img_request->rq = rq;
 	snapc = NULL; /* img_request consumes a ref */
@@ -3370,10 +4208,15 @@ static void rbd_queue_workfn(struct work_struct *work)
 	if (result)
 		goto err_img_request;
 
+	if (must_be_locked)
+		up_read(&rbd_dev->lock_rwsem);
 	return;
 
 err_img_request:
 	rbd_img_request_put(img_request);
+err_unlock:
+	if (must_be_locked)
+		up_read(&rbd_dev->lock_rwsem);
 err_rq:
 	if (result)
 		rbd_warn(rbd_dev, "%s %llx at %llx result %d",
@@ -3415,7 +4258,6 @@ static int rbd_obj_read_sync(struct rbd_device *rbd_dev,
 				u64 offset, u64 length, void *buf)
 
 {
-	struct ceph_osd_client *osdc = &rbd_dev->rbd_client->client->osdc;
 	struct rbd_obj_request *obj_request;
 	struct page **pages = NULL;
 	u32 page_count;
@@ -3448,11 +4290,8 @@ static int rbd_obj_read_sync(struct rbd_device *rbd_dev,
 					obj_request->length,
 					obj_request->offset & ~PAGE_MASK,
 					false, false);
-	rbd_osd_req_format_read(obj_request);
 
-	ret = rbd_obj_request_submit(osdc, obj_request);
-	if (ret)
-		goto out;
+	rbd_obj_request_submit(obj_request);
 	ret = rbd_obj_request_wait(obj_request);
 	if (ret)
 		goto out;
@@ -3621,7 +4460,6 @@ static int rbd_init_request(void *data, struct request *rq,
 
 static struct blk_mq_ops rbd_mq_ops = {
 	.queue_rq	= rbd_queue_rq,
-	.map_queue	= blk_mq_map_queue,
 	.init_request	= rbd_init_request,
 };
 
@@ -3752,13 +4590,40 @@ static ssize_t rbd_minor_show(struct device *dev,
 	return sprintf(buf, "%d\n", rbd_dev->minor);
 }
 
+static ssize_t rbd_client_addr_show(struct device *dev,
+				    struct device_attribute *attr, char *buf)
+{
+	struct rbd_device *rbd_dev = dev_to_rbd_dev(dev);
+	struct ceph_entity_addr *client_addr =
+	    ceph_client_addr(rbd_dev->rbd_client->client);
+
+	return sprintf(buf, "%pISpc/%u\n", &client_addr->in_addr,
+		       le32_to_cpu(client_addr->nonce));
+}
+
 static ssize_t rbd_client_id_show(struct device *dev,
 				  struct device_attribute *attr, char *buf)
 {
 	struct rbd_device *rbd_dev = dev_to_rbd_dev(dev);
 
 	return sprintf(buf, "client%lld\n",
-			ceph_client_id(rbd_dev->rbd_client->client));
+		       ceph_client_gid(rbd_dev->rbd_client->client));
+}
+
+static ssize_t rbd_cluster_fsid_show(struct device *dev,
+				     struct device_attribute *attr, char *buf)
+{
+	struct rbd_device *rbd_dev = dev_to_rbd_dev(dev);
+
+	return sprintf(buf, "%pU\n", &rbd_dev->rbd_client->client->fsid);
+}
+
+static ssize_t rbd_config_info_show(struct device *dev,
+				    struct device_attribute *attr, char *buf)
+{
+	struct rbd_device *rbd_dev = dev_to_rbd_dev(dev);
+
+	return sprintf(buf, "%s\n", rbd_dev->config_info);
 }
 
 static ssize_t rbd_pool_show(struct device *dev,
@@ -3810,6 +4675,14 @@ static ssize_t rbd_snap_show(struct device *dev,
 	return sprintf(buf, "%s\n", rbd_dev->spec->snap_name);
 }
 
+static ssize_t rbd_snap_id_show(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct rbd_device *rbd_dev = dev_to_rbd_dev(dev);
+
+	return sprintf(buf, "%llu\n", rbd_dev->spec->snap_id);
+}
+
 /*
  * For a v2 image, shows the chain of parent images, separated by empty
  * lines.  For v1 images or if there is no parent, shows "(no parent
@@ -3862,13 +4735,17 @@ static DEVICE_ATTR(size, S_IRUGO, rbd_size_show, NULL);
 static DEVICE_ATTR(features, S_IRUGO, rbd_features_show, NULL);
 static DEVICE_ATTR(major, S_IRUGO, rbd_major_show, NULL);
 static DEVICE_ATTR(minor, S_IRUGO, rbd_minor_show, NULL);
+static DEVICE_ATTR(client_addr, S_IRUGO, rbd_client_addr_show, NULL);
 static DEVICE_ATTR(client_id, S_IRUGO, rbd_client_id_show, NULL);
+static DEVICE_ATTR(cluster_fsid, S_IRUGO, rbd_cluster_fsid_show, NULL);
+static DEVICE_ATTR(config_info, S_IRUSR, rbd_config_info_show, NULL);
 static DEVICE_ATTR(pool, S_IRUGO, rbd_pool_show, NULL);
 static DEVICE_ATTR(pool_id, S_IRUGO, rbd_pool_id_show, NULL);
 static DEVICE_ATTR(name, S_IRUGO, rbd_name_show, NULL);
 static DEVICE_ATTR(image_id, S_IRUGO, rbd_image_id_show, NULL);
 static DEVICE_ATTR(refresh, S_IWUSR, NULL, rbd_image_refresh);
 static DEVICE_ATTR(current_snap, S_IRUGO, rbd_snap_show, NULL);
+static DEVICE_ATTR(snap_id, S_IRUGO, rbd_snap_id_show, NULL);
 static DEVICE_ATTR(parent, S_IRUGO, rbd_parent_show, NULL);
 
 static struct attribute *rbd_attrs[] = {
@@ -3876,12 +4753,16 @@ static struct attribute *rbd_attrs[] = {
 	&dev_attr_features.attr,
 	&dev_attr_major.attr,
 	&dev_attr_minor.attr,
+	&dev_attr_client_addr.attr,
 	&dev_attr_client_id.attr,
+	&dev_attr_cluster_fsid.attr,
+	&dev_attr_config_info.attr,
 	&dev_attr_pool.attr,
 	&dev_attr_pool_id.attr,
 	&dev_attr_name.attr,
 	&dev_attr_image_id.attr,
 	&dev_attr_current_snap.attr,
+	&dev_attr_snap_id.attr,
 	&dev_attr_parent.attr,
 	&dev_attr_refresh.attr,
 	NULL
@@ -3944,18 +4825,32 @@ static void rbd_spec_free(struct kref *kref)
 	kfree(spec);
 }
 
-static void rbd_dev_release(struct device *dev)
+static void rbd_dev_free(struct rbd_device *rbd_dev)
 {
-	struct rbd_device *rbd_dev = dev_to_rbd_dev(dev);
-	bool need_put = !!rbd_dev->opts;
+	WARN_ON(rbd_dev->watch_state != RBD_WATCH_STATE_UNREGISTERED);
+	WARN_ON(rbd_dev->lock_state != RBD_LOCK_STATE_UNLOCKED);
 
 	ceph_oid_destroy(&rbd_dev->header_oid);
 	ceph_oloc_destroy(&rbd_dev->header_oloc);
+	kfree(rbd_dev->config_info);
 
 	rbd_put_client(rbd_dev->rbd_client);
 	rbd_spec_put(rbd_dev->spec);
 	kfree(rbd_dev->opts);
 	kfree(rbd_dev);
+}
+
+static void rbd_dev_release(struct device *dev)
+{
+	struct rbd_device *rbd_dev = dev_to_rbd_dev(dev);
+	bool need_put = !!rbd_dev->opts;
+
+	if (need_put) {
+		destroy_workqueue(rbd_dev->task_wq);
+		ida_simple_remove(&rbd_dev_id_ida, rbd_dev->dev_id);
+	}
+
+	rbd_dev_free(rbd_dev);
 
 	/*
 	 * This is racy, but way better than putting module outside of
@@ -3966,25 +4861,34 @@ static void rbd_dev_release(struct device *dev)
 		module_put(THIS_MODULE);
 }
 
-static struct rbd_device *rbd_dev_create(struct rbd_client *rbdc,
-					 struct rbd_spec *spec,
-					 struct rbd_options *opts)
+static struct rbd_device *__rbd_dev_create(struct rbd_client *rbdc,
+					   struct rbd_spec *spec)
 {
 	struct rbd_device *rbd_dev;
 
-	rbd_dev = kzalloc(sizeof (*rbd_dev), GFP_KERNEL);
+	rbd_dev = kzalloc(sizeof(*rbd_dev), GFP_KERNEL);
 	if (!rbd_dev)
 		return NULL;
 
 	spin_lock_init(&rbd_dev->lock);
-	rbd_dev->flags = 0;
-	atomic_set(&rbd_dev->parent_ref, 0);
 	INIT_LIST_HEAD(&rbd_dev->node);
 	init_rwsem(&rbd_dev->header_rwsem);
 
 	ceph_oid_init(&rbd_dev->header_oid);
 	ceph_oloc_init(&rbd_dev->header_oloc);
 
+	mutex_init(&rbd_dev->watch_mutex);
+	rbd_dev->watch_state = RBD_WATCH_STATE_UNREGISTERED;
+	INIT_DELAYED_WORK(&rbd_dev->watch_dwork, rbd_reregister_watch);
+
+	init_rwsem(&rbd_dev->lock_rwsem);
+	rbd_dev->lock_state = RBD_LOCK_STATE_UNLOCKED;
+	INIT_WORK(&rbd_dev->acquired_lock_work, rbd_notify_acquired_lock);
+	INIT_WORK(&rbd_dev->released_lock_work, rbd_notify_released_lock);
+	INIT_DELAYED_WORK(&rbd_dev->lock_dwork, rbd_acquire_lock);
+	INIT_WORK(&rbd_dev->unlock_work, rbd_release_lock_work);
+	init_waitqueue_head(&rbd_dev->lock_waitq);
+
 	rbd_dev->dev.bus = &rbd_bus_type;
 	rbd_dev->dev.type = &rbd_device_type;
 	rbd_dev->dev.parent = &rbd_root_dev;
@@ -3992,9 +4896,6 @@ static struct rbd_device *rbd_dev_create(struct rbd_client *rbdc,
 
 	rbd_dev->rbd_client = rbdc;
 	rbd_dev->spec = spec;
-	rbd_dev->opts = opts;
-
-	/* Initialize the layout used for all rbd requests */
 
 	rbd_dev->layout.stripe_unit = 1 << RBD_MAX_OBJ_ORDER;
 	rbd_dev->layout.stripe_count = 1;
@@ -4002,15 +4903,48 @@ static struct rbd_device *rbd_dev_create(struct rbd_client *rbdc,
 	rbd_dev->layout.pool_id = spec->pool_id;
 	RCU_INIT_POINTER(rbd_dev->layout.pool_ns, NULL);
 
-	/*
-	 * If this is a mapping rbd_dev (as opposed to a parent one),
-	 * pin our module.  We have a ref from do_rbd_add(), so use
-	 * __module_get().
-	 */
-	if (rbd_dev->opts)
-		__module_get(THIS_MODULE);
+	return rbd_dev;
+}
+
+/*
+ * Create a mapping rbd_dev.
+ */
+static struct rbd_device *rbd_dev_create(struct rbd_client *rbdc,
+					 struct rbd_spec *spec,
+					 struct rbd_options *opts)
+{
+	struct rbd_device *rbd_dev;
+
+	rbd_dev = __rbd_dev_create(rbdc, spec);
+	if (!rbd_dev)
+		return NULL;
+
+	rbd_dev->opts = opts;
+
+	/* get an id and fill in device name */
+	rbd_dev->dev_id = ida_simple_get(&rbd_dev_id_ida, 0,
+					 minor_to_rbd_dev_id(1 << MINORBITS),
+					 GFP_KERNEL);
+	if (rbd_dev->dev_id < 0)
+		goto fail_rbd_dev;
 
+	sprintf(rbd_dev->name, RBD_DRV_NAME "%d", rbd_dev->dev_id);
+	rbd_dev->task_wq = alloc_ordered_workqueue("%s-tasks", WQ_MEM_RECLAIM,
+						   rbd_dev->name);
+	if (!rbd_dev->task_wq)
+		goto fail_dev_id;
+
+	/* we have a ref from do_rbd_add() */
+	__module_get(THIS_MODULE);
+
+	dout("%s rbd_dev %p dev_id %d\n", __func__, rbd_dev, rbd_dev->dev_id);
 	return rbd_dev;
+
+fail_dev_id:
+	ida_simple_remove(&rbd_dev_id_ida, rbd_dev->dev_id);
+fail_rbd_dev:
+	rbd_dev_free(rbd_dev);
+	return NULL;
 }
 
 static void rbd_dev_destroy(struct rbd_device *rbd_dev)
@@ -4646,46 +5580,6 @@ static int rbd_dev_header_info(struct rbd_device *rbd_dev)
 }
 
 /*
- * Get a unique rbd identifier for the given new rbd_dev, and add
- * the rbd_dev to the global list.
- */
-static int rbd_dev_id_get(struct rbd_device *rbd_dev)
-{
-	int new_dev_id;
-
-	new_dev_id = ida_simple_get(&rbd_dev_id_ida,
-				    0, minor_to_rbd_dev_id(1 << MINORBITS),
-				    GFP_KERNEL);
-	if (new_dev_id < 0)
-		return new_dev_id;
-
-	rbd_dev->dev_id = new_dev_id;
-
-	spin_lock(&rbd_dev_list_lock);
-	list_add_tail(&rbd_dev->node, &rbd_dev_list);
-	spin_unlock(&rbd_dev_list_lock);
-
-	dout("rbd_dev %p given dev id %d\n", rbd_dev, rbd_dev->dev_id);
-
-	return 0;
-}
-
-/*
- * Remove an rbd_dev from the global list, and record that its
- * identifier is no longer in use.
- */
-static void rbd_dev_id_put(struct rbd_device *rbd_dev)
-{
-	spin_lock(&rbd_dev_list_lock);
-	list_del_init(&rbd_dev->node);
-	spin_unlock(&rbd_dev_list_lock);
-
-	ida_simple_remove(&rbd_dev_id_ida, rbd_dev->dev_id);
-
-	dout("rbd_dev %p released dev id %d\n", rbd_dev, rbd_dev->dev_id);
-}
-
-/*
  * Skips over white space at *buf, and updates *buf to point to the
  * first found non-space character (if any). Returns the length of
  * the token (string of non-white space characters) found.  Note
@@ -4860,6 +5754,7 @@ static int rbd_add_parse_args(const char *buf,
 
 	rbd_opts->read_only = RBD_READ_ONLY_DEFAULT;
 	rbd_opts->queue_depth = RBD_QUEUE_DEPTH_DEFAULT;
+	rbd_opts->lock_on_read = RBD_LOCK_ON_READ_DEFAULT;
 
 	copts = ceph_parse_options(options, mon_addrs,
 					mon_addrs + mon_addrs_size - 1,
@@ -5077,8 +5972,7 @@ static int rbd_dev_probe_parent(struct rbd_device *rbd_dev, int depth)
 		goto out_err;
 	}
 
-	parent = rbd_dev_create(rbd_dev->rbd_client, rbd_dev->parent_spec,
-				NULL);
+	parent = __rbd_dev_create(rbd_dev->rbd_client, rbd_dev->parent_spec);
 	if (!parent) {
 		ret = -ENOMEM;
 		goto out_err;
@@ -5113,22 +6007,12 @@ static int rbd_dev_device_setup(struct rbd_device *rbd_dev)
 {
 	int ret;
 
-	/* Get an id and fill in device name. */
-
-	ret = rbd_dev_id_get(rbd_dev);
-	if (ret)
-		goto err_out_unlock;
-
-	BUILD_BUG_ON(DEV_NAME_LEN
-			< sizeof (RBD_DRV_NAME) + MAX_INT_FORMAT_WIDTH);
-	sprintf(rbd_dev->name, "%s%d", RBD_DRV_NAME, rbd_dev->dev_id);
-
 	/* Record our major and minor device numbers. */
 
 	if (!single_major) {
 		ret = register_blkdev(0, rbd_dev->name);
 		if (ret < 0)
-			goto err_out_id;
+			goto err_out_unlock;
 
 		rbd_dev->major = ret;
 		rbd_dev->minor = 0;
@@ -5160,9 +6044,14 @@ static int rbd_dev_device_setup(struct rbd_device *rbd_dev)
 	set_bit(RBD_DEV_FLAG_EXISTS, &rbd_dev->flags);
 	up_write(&rbd_dev->header_rwsem);
 
+	spin_lock(&rbd_dev_list_lock);
+	list_add_tail(&rbd_dev->node, &rbd_dev_list);
+	spin_unlock(&rbd_dev_list_lock);
+
 	add_disk(rbd_dev->disk);
-	pr_info("%s: added with size 0x%llx\n", rbd_dev->disk->disk_name,
-		(unsigned long long) rbd_dev->mapping.size);
+	pr_info("%s: capacity %llu features 0x%llx\n", rbd_dev->disk->disk_name,
+		(unsigned long long)get_capacity(rbd_dev->disk) << SECTOR_SHIFT,
+		rbd_dev->header.features);
 
 	return ret;
 
@@ -5173,8 +6062,6 @@ err_out_disk:
 err_out_blkdev:
 	if (!single_major)
 		unregister_blkdev(rbd_dev->major, rbd_dev->name);
-err_out_id:
-	rbd_dev_id_put(rbd_dev);
 err_out_unlock:
 	up_write(&rbd_dev->header_rwsem);
 	return ret;
@@ -5235,7 +6122,7 @@ static int rbd_dev_image_probe(struct rbd_device *rbd_dev, int depth)
 		goto err_out_format;
 
 	if (!depth) {
-		ret = rbd_dev_header_watch_sync(rbd_dev);
+		ret = rbd_register_watch(rbd_dev);
 		if (ret) {
 			if (ret == -ENOENT)
 				pr_info("image %s/%s does not exist\n",
@@ -5294,7 +6181,7 @@ err_out_probe:
 	rbd_dev_unprobe(rbd_dev);
 err_out_watch:
 	if (!depth)
-		rbd_dev_header_unwatch_sync(rbd_dev);
+		rbd_unregister_watch(rbd_dev);
 err_out_format:
 	rbd_dev->image_format = 0;
 	kfree(rbd_dev->spec->image_id);
@@ -5346,10 +6233,18 @@ static ssize_t do_rbd_add(struct bus_type *bus,
 	spec = NULL;		/* rbd_dev now owns this */
 	rbd_opts = NULL;	/* rbd_dev now owns this */
 
+	rbd_dev->config_info = kstrdup(buf, GFP_KERNEL);
+	if (!rbd_dev->config_info) {
+		rc = -ENOMEM;
+		goto err_out_rbd_dev;
+	}
+
 	down_write(&rbd_dev->header_rwsem);
 	rc = rbd_dev_image_probe(rbd_dev, 0);
-	if (rc < 0)
+	if (rc < 0) {
+		up_write(&rbd_dev->header_rwsem);
 		goto err_out_rbd_dev;
+	}
 
 	/* If we are mapping a snapshot it must be marked read-only */
 
@@ -5361,11 +6256,11 @@ static ssize_t do_rbd_add(struct bus_type *bus,
 	rc = rbd_dev_device_setup(rbd_dev);
 	if (rc) {
 		/*
-		 * rbd_dev_header_unwatch_sync() can't be moved into
+		 * rbd_unregister_watch() can't be moved into
 		 * rbd_dev_image_release() without refactoring, see
 		 * commit 1f3ef78861ac.
 		 */
-		rbd_dev_header_unwatch_sync(rbd_dev);
+		rbd_unregister_watch(rbd_dev);
 		rbd_dev_image_release(rbd_dev);
 		goto out;
 	}
@@ -5376,7 +6271,6 @@ out:
 	return rc;
 
 err_out_rbd_dev:
-	up_write(&rbd_dev->header_rwsem);
 	rbd_dev_destroy(rbd_dev);
 err_out_client:
 	rbd_put_client(rbdc);
@@ -5406,12 +6300,16 @@ static ssize_t rbd_add_single_major(struct bus_type *bus,
 static void rbd_dev_device_release(struct rbd_device *rbd_dev)
 {
 	rbd_free_disk(rbd_dev);
+
+	spin_lock(&rbd_dev_list_lock);
+	list_del_init(&rbd_dev->node);
+	spin_unlock(&rbd_dev_list_lock);
+
 	clear_bit(RBD_DEV_FLAG_EXISTS, &rbd_dev->flags);
 	device_del(&rbd_dev->dev);
 	rbd_dev_mapping_clear(rbd_dev);
 	if (!single_major)
 		unregister_blkdev(rbd_dev->major, rbd_dev->name);
-	rbd_dev_id_put(rbd_dev);
 }
 
 static void rbd_dev_remove_parent(struct rbd_device *rbd_dev)
@@ -5447,18 +6345,26 @@ static ssize_t do_rbd_remove(struct bus_type *bus,
 	struct rbd_device *rbd_dev = NULL;
 	struct list_head *tmp;
 	int dev_id;
-	unsigned long ul;
+	char opt_buf[6];
 	bool already = false;
+	bool force = false;
 	int ret;
 
-	ret = kstrtoul(buf, 10, &ul);
-	if (ret)
-		return ret;
-
-	/* convert to int; abort if we lost anything in the conversion */
-	dev_id = (int)ul;
-	if (dev_id != ul)
+	dev_id = -1;
+	opt_buf[0] = '\0';
+	sscanf(buf, "%d %5s", &dev_id, opt_buf);
+	if (dev_id < 0) {
+		pr_err("dev_id out of range\n");
 		return -EINVAL;
+	}
+	if (opt_buf[0] != '\0') {
+		if (!strcmp(opt_buf, "force")) {
+			force = true;
+		} else {
+			pr_err("bad remove option at '%s'\n", opt_buf);
+			return -EINVAL;
+		}
+	}
 
 	ret = -ENOENT;
 	spin_lock(&rbd_dev_list_lock);
@@ -5471,7 +6377,7 @@ static ssize_t do_rbd_remove(struct bus_type *bus,
 	}
 	if (!ret) {
 		spin_lock_irq(&rbd_dev->lock);
-		if (rbd_dev->open_count)
+		if (rbd_dev->open_count && !force)
 			ret = -EBUSY;
 		else
 			already = test_and_set_bit(RBD_DEV_FLAG_REMOVING,
@@ -5482,7 +6388,20 @@ static ssize_t do_rbd_remove(struct bus_type *bus,
 	if (ret < 0 || already)
 		return ret;
 
-	rbd_dev_header_unwatch_sync(rbd_dev);
+	if (force) {
+		/*
+		 * Prevent new IO from being queued and wait for existing
+		 * IO to complete/fail.
+		 */
+		blk_mq_freeze_queue(rbd_dev->disk->queue);
+		blk_set_queue_dying(rbd_dev->disk->queue);
+	}
+
+	down_write(&rbd_dev->lock_rwsem);
+	if (__rbd_is_lock_owner(rbd_dev))
+		rbd_unlock(rbd_dev);
+	up_write(&rbd_dev->lock_rwsem);
+	rbd_unregister_watch(rbd_dev);
 
 	/*
 	 * Don't free anything from rbd_dev->disk until after all
diff --git a/drivers/block/rbd_types.h b/drivers/block/rbd_types.h
index 49d77cbcf8bd..94f367db27b0 100644
--- a/drivers/block/rbd_types.h
+++ b/drivers/block/rbd_types.h
@@ -28,6 +28,17 @@
 #define RBD_DATA_PREFIX        "rbd_data."
 #define RBD_ID_PREFIX          "rbd_id."
 
+#define RBD_LOCK_NAME          "rbd_lock"
+#define RBD_LOCK_TAG           "internal"
+#define RBD_LOCK_COOKIE_PREFIX "auto"
+
+enum rbd_notify_op {
+	RBD_NOTIFY_OP_ACQUIRED_LOCK      = 0,
+	RBD_NOTIFY_OP_RELEASED_LOCK      = 1,
+	RBD_NOTIFY_OP_REQUEST_LOCK       = 2,
+	RBD_NOTIFY_OP_HEADER_UPDATE      = 3,
+};
+
 /*
  * For format version 1, rbd image 'foo' consists of objects
  *   foo.rbd		- image metadata
diff --git a/drivers/block/virtio_blk.c b/drivers/block/virtio_blk.c
index 93b1aaa5ba3b..2dc5c96c186a 100644
--- a/drivers/block/virtio_blk.c
+++ b/drivers/block/virtio_blk.c
@@ -542,7 +542,6 @@ static int virtblk_init_request(void *data, struct request *rq,
 
 static struct blk_mq_ops virtio_mq_ops = {
 	.queue_rq	= virtio_queue_rq,
-	.map_queue	= blk_mq_map_queue,
 	.complete	= virtblk_request_done,
 	.init_request	= virtblk_init_request,
 };
diff --git a/drivers/block/xen-blkfront.c b/drivers/block/xen-blkfront.c
index 88ef6d4729b4..9908597c5209 100644
--- a/drivers/block/xen-blkfront.c
+++ b/drivers/block/xen-blkfront.c
@@ -909,7 +909,6 @@ out_busy:
 
 static struct blk_mq_ops blkfront_mq_ops = {
 	.queue_rq = blkif_queue_rq,
-	.map_queue = blk_mq_map_queue,
 };
 
 static void blkif_set_queue_limits(struct blkfront_info *info)
diff --git a/drivers/bluetooth/Kconfig b/drivers/bluetooth/Kconfig
index cf50fd2e96df..3cc9bff9d99d 100644
--- a/drivers/bluetooth/Kconfig
+++ b/drivers/bluetooth/Kconfig
@@ -180,6 +180,17 @@ config BT_HCIUART_AG6XX
 
 	  Say Y here to compile support for Intel AG6XX protocol.
 
+config BT_HCIUART_MRVL
+	bool "Marvell protocol support"
+	depends on BT_HCIUART
+	select BT_HCIUART_H4
+	help
+	  Marvell is serial protocol for communication between Bluetooth
+	  device and host. This protocol is required for most Marvell Bluetooth
+	  devices with UART interface.
+
+	  Say Y here to compile support for HCI MRVL protocol.
+
 config BT_HCIBCM203X
 	tristate "HCI BCM203x USB driver"
 	depends on USB
@@ -331,4 +342,16 @@ config BT_WILINK
 	  Say Y here to compile support for Texas Instrument's WiLink7 driver
 	  into the kernel or say M to compile it as module (btwilink).
 
+config BT_QCOMSMD
+	tristate "Qualcomm SMD based HCI support"
+	depends on QCOM_SMD && QCOM_WCNSS_CTRL
+	select BT_QCA
+	help
+	  Qualcomm SMD based HCI driver.
+	  This driver is used to bridge HCI data onto the shared memory
+	  channels to the WCNSS core.
+
+	  Say Y here to compile support for HCI over Qualcomm SMD into the
+	  kernel or say M to compile as a module.
+
 endmenu
diff --git a/drivers/bluetooth/Makefile b/drivers/bluetooth/Makefile
index 9c18939fc5c9..b1fc29a697b7 100644
--- a/drivers/bluetooth/Makefile
+++ b/drivers/bluetooth/Makefile
@@ -20,6 +20,7 @@ obj-$(CONFIG_BT_ATH3K)		+= ath3k.o
 obj-$(CONFIG_BT_MRVL)		+= btmrvl.o
 obj-$(CONFIG_BT_MRVL_SDIO)	+= btmrvl_sdio.o
 obj-$(CONFIG_BT_WILINK)		+= btwilink.o
+obj-$(CONFIG_BT_QCOMSMD)	+= btqcomsmd.o
 obj-$(CONFIG_BT_BCM)		+= btbcm.o
 obj-$(CONFIG_BT_RTL)		+= btrtl.o
 obj-$(CONFIG_BT_QCA)		+= btqca.o
@@ -37,6 +38,7 @@ hci_uart-$(CONFIG_BT_HCIUART_INTEL)	+= hci_intel.o
 hci_uart-$(CONFIG_BT_HCIUART_BCM)	+= hci_bcm.o
 hci_uart-$(CONFIG_BT_HCIUART_QCA)	+= hci_qca.o
 hci_uart-$(CONFIG_BT_HCIUART_AG6XX)	+= hci_ag6xx.o
+hci_uart-$(CONFIG_BT_HCIUART_MRVL)	+= hci_mrvl.o
 hci_uart-objs				:= $(hci_uart-y)
 
 ccflags-y += -D__CHECK_ENDIAN__
diff --git a/drivers/bluetooth/bcm203x.c b/drivers/bluetooth/bcm203x.c
index 5b0ef7bbe8ac..5ce6d4176dc3 100644
--- a/drivers/bluetooth/bcm203x.c
+++ b/drivers/bluetooth/bcm203x.c
@@ -185,10 +185,8 @@ static int bcm203x_probe(struct usb_interface *intf, const struct usb_device_id
 	data->state = BCM203X_LOAD_MINIDRV;
 
 	data->urb = usb_alloc_urb(0, GFP_KERNEL);
-	if (!data->urb) {
-		BT_ERR("Can't allocate URB");
+	if (!data->urb)
 		return -ENOMEM;
-	}
 
 	if (request_firmware(&firmware, "BCM2033-MD.hex", &udev->dev) < 0) {
 		BT_ERR("Mini driver request failed");
diff --git a/drivers/bluetooth/btqca.c b/drivers/bluetooth/btqca.c
index 4a6208168850..28afd5d585f9 100644
--- a/drivers/bluetooth/btqca.c
+++ b/drivers/bluetooth/btqca.c
@@ -55,8 +55,8 @@ static int rome_patch_ver_req(struct hci_dev *hdev, u32 *rome_version)
 	}
 
 	edl = (struct edl_event_hdr *)(skb->data);
-	if (!edl || !edl->data) {
-		BT_ERR("%s: TLV with no header or no data", hdev->name);
+	if (!edl) {
+		BT_ERR("%s: TLV with no header", hdev->name);
 		err = -EILSEQ;
 		goto out;
 	}
@@ -224,8 +224,8 @@ static int rome_tlv_send_segment(struct hci_dev *hdev, int idx, int seg_size,
 	}
 
 	edl = (struct edl_event_hdr *)(skb->data);
-	if (!edl || !edl->data) {
-		BT_ERR("%s: TLV with no header or no data", hdev->name);
+	if (!edl) {
+		BT_ERR("%s: TLV with no header", hdev->name);
 		err = -EILSEQ;
 		goto out;
 	}
diff --git a/drivers/bluetooth/btqcomsmd.c b/drivers/bluetooth/btqcomsmd.c
new file mode 100644
index 000000000000..08c2c93887c1
--- /dev/null
+++ b/drivers/bluetooth/btqcomsmd.c
@@ -0,0 +1,182 @@
+/*
+ * Copyright (c) 2016, Linaro Ltd.
+ * Copyright (c) 2015, Sony Mobile 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 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/slab.h>
+#include <linux/soc/qcom/smd.h>
+#include <linux/soc/qcom/wcnss_ctrl.h>
+#include <linux/platform_device.h>
+
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci_core.h>
+
+#include "btqca.h"
+
+struct btqcomsmd {
+	struct hci_dev *hdev;
+
+	struct qcom_smd_channel *acl_channel;
+	struct qcom_smd_channel *cmd_channel;
+};
+
+static int btqcomsmd_recv(struct hci_dev *hdev, unsigned int type,
+			   const void *data, size_t count)
+{
+	struct sk_buff *skb;
+
+	/* Use GFP_ATOMIC as we're in IRQ context */
+	skb = bt_skb_alloc(count, GFP_ATOMIC);
+	if (!skb) {
+		hdev->stat.err_rx++;
+		return -ENOMEM;
+	}
+
+	hci_skb_pkt_type(skb) = type;
+	memcpy(skb_put(skb, count), data, count);
+
+	return hci_recv_frame(hdev, skb);
+}
+
+static int btqcomsmd_acl_callback(struct qcom_smd_channel *channel,
+				  const void *data, size_t count)
+{
+	struct btqcomsmd *btq = qcom_smd_get_drvdata(channel);
+
+	btq->hdev->stat.byte_rx += count;
+	return btqcomsmd_recv(btq->hdev, HCI_ACLDATA_PKT, data, count);
+}
+
+static int btqcomsmd_cmd_callback(struct qcom_smd_channel *channel,
+				  const void *data, size_t count)
+{
+	struct btqcomsmd *btq = qcom_smd_get_drvdata(channel);
+
+	return btqcomsmd_recv(btq->hdev, HCI_EVENT_PKT, data, count);
+}
+
+static int btqcomsmd_send(struct hci_dev *hdev, struct sk_buff *skb)
+{
+	struct btqcomsmd *btq = hci_get_drvdata(hdev);
+	int ret;
+
+	switch (hci_skb_pkt_type(skb)) {
+	case HCI_ACLDATA_PKT:
+		ret = qcom_smd_send(btq->acl_channel, skb->data, skb->len);
+		hdev->stat.acl_tx++;
+		hdev->stat.byte_tx += skb->len;
+		break;
+	case HCI_COMMAND_PKT:
+		ret = qcom_smd_send(btq->cmd_channel, skb->data, skb->len);
+		hdev->stat.cmd_tx++;
+		break;
+	default:
+		ret = -EILSEQ;
+		break;
+	}
+
+	kfree_skb(skb);
+
+	return ret;
+}
+
+static int btqcomsmd_open(struct hci_dev *hdev)
+{
+	return 0;
+}
+
+static int btqcomsmd_close(struct hci_dev *hdev)
+{
+	return 0;
+}
+
+static int btqcomsmd_probe(struct platform_device *pdev)
+{
+	struct btqcomsmd *btq;
+	struct hci_dev *hdev;
+	void *wcnss;
+	int ret;
+
+	btq = devm_kzalloc(&pdev->dev, sizeof(*btq), GFP_KERNEL);
+	if (!btq)
+		return -ENOMEM;
+
+	wcnss = dev_get_drvdata(pdev->dev.parent);
+
+	btq->acl_channel = qcom_wcnss_open_channel(wcnss, "APPS_RIVA_BT_ACL",
+						   btqcomsmd_acl_callback);
+	if (IS_ERR(btq->acl_channel))
+		return PTR_ERR(btq->acl_channel);
+
+	btq->cmd_channel = qcom_wcnss_open_channel(wcnss, "APPS_RIVA_BT_CMD",
+						   btqcomsmd_cmd_callback);
+	if (IS_ERR(btq->cmd_channel))
+		return PTR_ERR(btq->cmd_channel);
+
+	qcom_smd_set_drvdata(btq->acl_channel, btq);
+	qcom_smd_set_drvdata(btq->cmd_channel, btq);
+
+	hdev = hci_alloc_dev();
+	if (!hdev)
+		return -ENOMEM;
+
+	hci_set_drvdata(hdev, btq);
+	btq->hdev = hdev;
+	SET_HCIDEV_DEV(hdev, &pdev->dev);
+
+	hdev->bus = HCI_SMD;
+	hdev->open = btqcomsmd_open;
+	hdev->close = btqcomsmd_close;
+	hdev->send = btqcomsmd_send;
+	hdev->set_bdaddr = qca_set_bdaddr_rome;
+
+	ret = hci_register_dev(hdev);
+	if (ret < 0) {
+		hci_free_dev(hdev);
+		return ret;
+	}
+
+	platform_set_drvdata(pdev, btq);
+
+	return 0;
+}
+
+static int btqcomsmd_remove(struct platform_device *pdev)
+{
+	struct btqcomsmd *btq = platform_get_drvdata(pdev);
+
+	hci_unregister_dev(btq->hdev);
+	hci_free_dev(btq->hdev);
+
+	return 0;
+}
+
+static const struct of_device_id btqcomsmd_of_match[] = {
+	{ .compatible = "qcom,wcnss-bt", },
+	{ },
+};
+
+static struct platform_driver btqcomsmd_driver = {
+	.probe = btqcomsmd_probe,
+	.remove = btqcomsmd_remove,
+	.driver  = {
+		.name  = "btqcomsmd",
+		.of_match_table = btqcomsmd_of_match,
+	},
+};
+
+module_platform_driver(btqcomsmd_driver);
+
+MODULE_AUTHOR("Bjorn Andersson <bjorn.andersson@sonymobile.com>");
+MODULE_DESCRIPTION("Qualcomm SMD HCI driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/bluetooth/btrtl.c b/drivers/bluetooth/btrtl.c
index 84288938f7f2..fc9b25703c67 100644
--- a/drivers/bluetooth/btrtl.c
+++ b/drivers/bluetooth/btrtl.c
@@ -33,6 +33,7 @@
 #define RTL_ROM_LMP_8723B	0x8723
 #define RTL_ROM_LMP_8821A	0x8821
 #define RTL_ROM_LMP_8761A	0x8761
+#define RTL_ROM_LMP_8822B	0x8822
 
 static int rtl_read_rom_version(struct hci_dev *hdev, u8 *version)
 {
@@ -78,11 +79,15 @@ static int rtl8723b_parse_firmware(struct hci_dev *hdev, u16 lmp_subver,
 	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
+	static const struct {
+		__u16 lmp_subver;
+		__u8 id;
+	} project_id_to_lmp_subver[] = {
+		{ RTL_ROM_LMP_8723A, 0 },
+		{ RTL_ROM_LMP_8723B, 1 },
+		{ RTL_ROM_LMP_8821A, 2 },
+		{ RTL_ROM_LMP_8761A, 3 },
+		{ RTL_ROM_LMP_8822B, 8 },
 	};
 
 	ret = rtl_read_rom_version(hdev, &rom_version);
@@ -134,14 +139,20 @@ static int rtl8723b_parse_firmware(struct hci_dev *hdev, u16 lmp_subver,
 		return -EINVAL;
 	}
 
-	if (project_id >= ARRAY_SIZE(project_id_to_lmp_subver)) {
+	/* Find project_id in table */
+	for (i = 0; i < ARRAY_SIZE(project_id_to_lmp_subver); i++) {
+		if (project_id == project_id_to_lmp_subver[i].id)
+			break;
+	}
+
+	if (i >= 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]) {
+	if (lmp_subver != project_id_to_lmp_subver[i].lmp_subver) {
 		BT_ERR("%s: firmware is for %x but this is a %x", hdev->name,
-		       project_id_to_lmp_subver[project_id], lmp_subver);
+		       project_id_to_lmp_subver[i].lmp_subver, lmp_subver);
 		return -EINVAL;
 	}
 
@@ -257,6 +268,26 @@ out:
 	return ret;
 }
 
+static int rtl_load_config(struct hci_dev *hdev, const char *name, u8 **buff)
+{
+	const struct firmware *fw;
+	int ret;
+
+	BT_INFO("%s: rtl: loading %s", hdev->name, name);
+	ret = request_firmware(&fw, name, &hdev->dev);
+	if (ret < 0) {
+		BT_ERR("%s: Failed to load %s", hdev->name, name);
+		return ret;
+	}
+
+	ret = fw->size;
+	*buff = kmemdup(fw->data, ret, GFP_KERNEL);
+
+	release_firmware(fw);
+
+	return ret;
+}
+
 static int btrtl_setup_rtl8723a(struct hci_dev *hdev)
 {
 	const struct firmware *fw;
@@ -296,25 +327,74 @@ static int btrtl_setup_rtl8723b(struct hci_dev *hdev, u16 lmp_subver,
 	unsigned char *fw_data = NULL;
 	const struct firmware *fw;
 	int ret;
+	int cfg_sz;
+	u8 *cfg_buff = NULL;
+	u8 *tbuff;
+	char *cfg_name = NULL;
+
+	switch (lmp_subver) {
+	case RTL_ROM_LMP_8723B:
+		cfg_name = "rtl_bt/rtl8723b_config.bin";
+		break;
+	case RTL_ROM_LMP_8821A:
+		cfg_name = "rtl_bt/rtl8821a_config.bin";
+		break;
+	case RTL_ROM_LMP_8761A:
+		cfg_name = "rtl_bt/rtl8761a_config.bin";
+		break;
+	case RTL_ROM_LMP_8822B:
+		cfg_name = "rtl_bt/rtl8822b_config.bin";
+		break;
+	default:
+		BT_ERR("%s: rtl: no config according to lmp_subver %04x",
+		       hdev->name, lmp_subver);
+		break;
+	}
+
+	if (cfg_name) {
+		cfg_sz = rtl_load_config(hdev, cfg_name, &cfg_buff);
+		if (cfg_sz < 0)
+			cfg_sz = 0;
+	} else
+		cfg_sz = 0;
 
 	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;
+		goto err_req_fw;
 	}
 
 	ret = rtl8723b_parse_firmware(hdev, lmp_subver, fw, &fw_data);
 	if (ret < 0)
 		goto out;
 
+	if (cfg_sz) {
+		tbuff = kzalloc(ret + cfg_sz, GFP_KERNEL);
+		if (!tbuff) {
+			ret = -ENOMEM;
+			goto out;
+		}
+
+		memcpy(tbuff, fw_data, ret);
+		kfree(fw_data);
+
+		memcpy(tbuff + ret, cfg_buff, cfg_sz);
+		ret += cfg_sz;
+
+		fw_data = tbuff;
+	}
+
+	BT_INFO("cfg_sz %d, total size %d", cfg_sz, ret);
+
 	ret = rtl_download_firmware(hdev, fw_data, ret);
-	kfree(fw_data);
-	if (ret < 0)
-		goto out;
 
 out:
 	release_firmware(fw);
+	kfree(fw_data);
+err_req_fw:
+	if (cfg_sz)
+		kfree(cfg_buff);
 	return ret;
 }
 
@@ -377,6 +457,9 @@ int btrtl_setup_realtek(struct hci_dev *hdev)
 	case RTL_ROM_LMP_8761A:
 		return btrtl_setup_rtl8723b(hdev, lmp_subver,
 					    "rtl_bt/rtl8761a_fw.bin");
+	case RTL_ROM_LMP_8822B:
+		return btrtl_setup_rtl8723b(hdev, lmp_subver,
+					    "rtl_bt/rtl8822b_fw.bin");
 	default:
 		BT_INFO("rtl: assuming no firmware upload needed.");
 		return 0;
diff --git a/drivers/bluetooth/btusb.c b/drivers/bluetooth/btusb.c
index 811f9b97e360..2f633df9f4e6 100644
--- a/drivers/bluetooth/btusb.c
+++ b/drivers/bluetooth/btusb.c
@@ -62,6 +62,7 @@ static struct usb_driver btusb_driver;
 #define BTUSB_REALTEK		0x20000
 #define BTUSB_BCM2045		0x40000
 #define BTUSB_IFNUM_2		0x80000
+#define BTUSB_CW6622		0x100000
 
 static const struct usb_device_id btusb_table[] = {
 	/* Generic Bluetooth USB device */
@@ -248,9 +249,11 @@ static const struct usb_device_id blacklist_table[] = {
 
 	/* QCA ROME chipset */
 	{ USB_DEVICE(0x0cf3, 0xe007), .driver_info = BTUSB_QCA_ROME },
+	{ USB_DEVICE(0x0cf3, 0xe009), .driver_info = BTUSB_QCA_ROME },
 	{ USB_DEVICE(0x0cf3, 0xe300), .driver_info = BTUSB_QCA_ROME },
 	{ USB_DEVICE(0x0cf3, 0xe360), .driver_info = BTUSB_QCA_ROME },
 	{ USB_DEVICE(0x0489, 0xe092), .driver_info = BTUSB_QCA_ROME },
+	{ USB_DEVICE(0x04ca, 0x3011), .driver_info = BTUSB_QCA_ROME },
 
 	/* Broadcom BCM2035 */
 	{ USB_DEVICE(0x0a5c, 0x2009), .driver_info = BTUSB_BCM92035 },
@@ -290,7 +293,8 @@ static const struct usb_device_id blacklist_table[] = {
 	{ USB_DEVICE(0x0400, 0x080a), .driver_info = BTUSB_BROKEN_ISOC },
 
 	/* CONWISE Technology based adapters with buggy SCO support */
-	{ USB_DEVICE(0x0e5e, 0x6622), .driver_info = BTUSB_BROKEN_ISOC },
+	{ USB_DEVICE(0x0e5e, 0x6622),
+	  .driver_info = BTUSB_BROKEN_ISOC | BTUSB_CW6622},
 
 	/* Roper Class 1 Bluetooth Dongle (Silicon Wave based) */
 	{ USB_DEVICE(0x1310, 0x0001), .driver_info = BTUSB_SWAVE },
@@ -310,6 +314,7 @@ static const struct usb_device_id blacklist_table[] = {
 	/* Marvell Bluetooth devices */
 	{ USB_DEVICE(0x1286, 0x2044), .driver_info = BTUSB_MARVELL },
 	{ USB_DEVICE(0x1286, 0x2046), .driver_info = BTUSB_MARVELL },
+	{ USB_DEVICE(0x1286, 0x204e), .driver_info = BTUSB_MARVELL },
 
 	/* Intel Bluetooth devices */
 	{ USB_DEVICE(0x8087, 0x07da), .driver_info = BTUSB_CSR },
@@ -1038,6 +1043,10 @@ static int btusb_open(struct hci_dev *hdev)
 
 	BT_DBG("%s", hdev->name);
 
+	err = usb_autopm_get_interface(data->intf);
+	if (err < 0)
+		return err;
+
 	/* Patching USB firmware files prior to starting any URBs of HCI path
 	 * It is more safe to use USB bulk channel for downloading USB patch
 	 */
@@ -1047,10 +1056,6 @@ static int btusb_open(struct hci_dev *hdev)
 			return err;
 	}
 
-	err = usb_autopm_get_interface(data->intf);
-	if (err < 0)
-		return err;
-
 	data->intf->needs_remote_wakeup = 1;
 
 	if (test_and_set_bit(BTUSB_INTR_RUNNING, &data->flags))
@@ -2221,9 +2226,8 @@ static int btusb_setup_intel_new(struct hci_dev *hdev)
 	err = wait_on_bit_timeout(&data->flags, BTUSB_DOWNLOADING,
 				  TASK_INTERRUPTIBLE,
 				  msecs_to_jiffies(5000));
-	if (err == 1) {
+	if (err == -EINTR) {
 		BT_ERR("%s: Firmware loading interrupted", hdev->name);
-		err = -EINTR;
 		goto done;
 	}
 
@@ -2275,7 +2279,7 @@ done:
 				  TASK_INTERRUPTIBLE,
 				  msecs_to_jiffies(1000));
 
-	if (err == 1) {
+	if (err == -EINTR) {
 		BT_ERR("%s: Device boot interrupted", hdev->name);
 		return -EINTR;
 	}
@@ -2845,6 +2849,9 @@ static int btusb_probe(struct usb_interface *intf,
 	hdev->send   = btusb_send_frame;
 	hdev->notify = btusb_notify;
 
+	if (id->driver_info & BTUSB_CW6622)
+		set_bit(HCI_QUIRK_BROKEN_STORED_LINK_KEY, &hdev->quirks);
+
 	if (id->driver_info & BTUSB_BCM2045)
 		set_bit(HCI_QUIRK_BROKEN_STORED_LINK_KEY, &hdev->quirks);
 
diff --git a/drivers/bluetooth/btwilink.c b/drivers/bluetooth/btwilink.c
index 485281b3f167..ef51c9c864c5 100644
--- a/drivers/bluetooth/btwilink.c
+++ b/drivers/bluetooth/btwilink.c
@@ -245,6 +245,7 @@ static int ti_st_send_frame(struct hci_dev *hdev, struct sk_buff *skb)
 {
 	struct ti_st *hst;
 	long len;
+	int pkt_type;
 
 	hst = hci_get_drvdata(hdev);
 
@@ -258,6 +259,7 @@ static int ti_st_send_frame(struct hci_dev *hdev, struct sk_buff *skb)
 	 * Freeing skb memory is taken care in shared transport layer,
 	 * so don't free skb memory here.
 	 */
+	pkt_type = hci_skb_pkt_type(skb);
 	len = hst->st_write(skb);
 	if (len < 0) {
 		kfree_skb(skb);
@@ -268,7 +270,7 @@ static int ti_st_send_frame(struct hci_dev *hdev, struct sk_buff *skb)
 
 	/* ST accepted our skb. So, Go ahead and do rest */
 	hdev->stat.byte_tx += len;
-	ti_st_tx_complete(hst, hci_skb_pkt_type(skb));
+	ti_st_tx_complete(hst, pkt_type);
 
 	return 0;
 }
diff --git a/drivers/bluetooth/hci_bcm.c b/drivers/bluetooth/hci_bcm.c
index 1c97eda8bae3..5ccb90ef0146 100644
--- a/drivers/bluetooth/hci_bcm.c
+++ b/drivers/bluetooth/hci_bcm.c
@@ -798,7 +798,7 @@ static int bcm_remove(struct platform_device *pdev)
 
 static const struct hci_uart_proto bcm_proto = {
 	.id		= HCI_UART_BCM,
-	.name		= "BCM",
+	.name		= "Broadcom",
 	.manufacturer	= 15,
 	.init_speed	= 115200,
 	.oper_speed	= 4000000,
diff --git a/drivers/bluetooth/hci_bcsp.c b/drivers/bluetooth/hci_bcsp.c
index d7d23ceba4d1..a2c921faaa12 100644
--- a/drivers/bluetooth/hci_bcsp.c
+++ b/drivers/bluetooth/hci_bcsp.c
@@ -90,7 +90,8 @@ struct bcsp_struct {
 /* ---- BCSP CRC calculation ---- */
 
 /* Table for calculating CRC for polynomial 0x1021, LSB processed first,
-initial value 0xffff, bits shifted in reverse order. */
+ * initial value 0xffff, bits shifted in reverse order.
+ */
 
 static const u16 crc_table[] = {
 	0x0000, 0x1081, 0x2102, 0x3183,
@@ -174,7 +175,7 @@ static int bcsp_enqueue(struct hci_uart *hu, struct sk_buff *skb)
 }
 
 static struct sk_buff *bcsp_prepare_pkt(struct bcsp_struct *bcsp, u8 *data,
-		int len, int pkt_type)
+					int len, int pkt_type)
 {
 	struct sk_buff *nskb;
 	u8 hdr[4], chan;
@@ -213,6 +214,7 @@ static struct sk_buff *bcsp_prepare_pkt(struct bcsp_struct *bcsp, u8 *data,
 		/* Vendor specific commands */
 		if (hci_opcode_ogf(__le16_to_cpu(opcode)) == 0x3f) {
 			u8 desc = *(data + HCI_COMMAND_HDR_SIZE);
+
 			if ((desc & 0xf0) == 0xc0) {
 				data += HCI_COMMAND_HDR_SIZE + 1;
 				len  -= HCI_COMMAND_HDR_SIZE + 1;
@@ -271,8 +273,8 @@ static struct sk_buff *bcsp_prepare_pkt(struct bcsp_struct *bcsp, u8 *data,
 	/* Put CRC */
 	if (bcsp->use_crc) {
 		bcsp_txmsg_crc = bitrev16(bcsp_txmsg_crc);
-		bcsp_slip_one_byte(nskb, (u8) ((bcsp_txmsg_crc >> 8) & 0x00ff));
-		bcsp_slip_one_byte(nskb, (u8) (bcsp_txmsg_crc & 0x00ff));
+		bcsp_slip_one_byte(nskb, (u8)((bcsp_txmsg_crc >> 8) & 0x00ff));
+		bcsp_slip_one_byte(nskb, (u8)(bcsp_txmsg_crc & 0x00ff));
 	}
 
 	bcsp_slip_msgdelim(nskb);
@@ -287,7 +289,8 @@ static struct sk_buff *bcsp_dequeue(struct hci_uart *hu)
 	struct sk_buff *skb;
 
 	/* First of all, check for unreliable messages in the queue,
-	   since they have priority */
+	 * since they have priority
+	 */
 
 	skb = skb_dequeue(&bcsp->unrel);
 	if (skb != NULL) {
@@ -414,7 +417,7 @@ static void bcsp_handle_le_pkt(struct hci_uart *hu)
 
 	/* spot "conf" pkts and reply with a "conf rsp" pkt */
 	if (bcsp->rx_skb->data[1] >> 4 == 4 && bcsp->rx_skb->data[2] == 0 &&
-			!memcmp(&bcsp->rx_skb->data[4], conf_pkt, 4)) {
+	    !memcmp(&bcsp->rx_skb->data[4], conf_pkt, 4)) {
 		struct sk_buff *nskb = alloc_skb(4, GFP_ATOMIC);
 
 		BT_DBG("Found a LE conf pkt");
@@ -428,7 +431,7 @@ static void bcsp_handle_le_pkt(struct hci_uart *hu)
 	}
 	/* Spot "sync" pkts. If we find one...disaster! */
 	else if (bcsp->rx_skb->data[1] >> 4 == 4 && bcsp->rx_skb->data[2] == 0 &&
-			!memcmp(&bcsp->rx_skb->data[4], sync_pkt, 4)) {
+		 !memcmp(&bcsp->rx_skb->data[4], sync_pkt, 4)) {
 		BT_ERR("Found a LE sync pkt, card has reset");
 	}
 }
@@ -446,7 +449,7 @@ static inline void bcsp_unslip_one_byte(struct bcsp_struct *bcsp, unsigned char
 		default:
 			memcpy(skb_put(bcsp->rx_skb, 1), &byte, 1);
 			if ((bcsp->rx_skb->data[0] & 0x40) != 0 &&
-					bcsp->rx_state != BCSP_W4_CRC)
+			    bcsp->rx_state != BCSP_W4_CRC)
 				bcsp_crc_update(&bcsp->message_crc, byte);
 			bcsp->rx_count--;
 		}
@@ -457,7 +460,7 @@ static inline void bcsp_unslip_one_byte(struct bcsp_struct *bcsp, unsigned char
 		case 0xdc:
 			memcpy(skb_put(bcsp->rx_skb, 1), &c0, 1);
 			if ((bcsp->rx_skb->data[0] & 0x40) != 0 &&
-					bcsp->rx_state != BCSP_W4_CRC)
+			    bcsp->rx_state != BCSP_W4_CRC)
 				bcsp_crc_update(&bcsp->message_crc, 0xc0);
 			bcsp->rx_esc_state = BCSP_ESCSTATE_NOESC;
 			bcsp->rx_count--;
@@ -466,7 +469,7 @@ static inline void bcsp_unslip_one_byte(struct bcsp_struct *bcsp, unsigned char
 		case 0xdd:
 			memcpy(skb_put(bcsp->rx_skb, 1), &db, 1);
 			if ((bcsp->rx_skb->data[0] & 0x40) != 0 &&
-					bcsp->rx_state != BCSP_W4_CRC)
+			    bcsp->rx_state != BCSP_W4_CRC)
 				bcsp_crc_update(&bcsp->message_crc, 0xdb);
 			bcsp->rx_esc_state = BCSP_ESCSTATE_NOESC;
 			bcsp->rx_count--;
@@ -485,13 +488,28 @@ static inline void bcsp_unslip_one_byte(struct bcsp_struct *bcsp, unsigned char
 static void bcsp_complete_rx_pkt(struct hci_uart *hu)
 {
 	struct bcsp_struct *bcsp = hu->priv;
-	int pass_up;
+	int pass_up = 0;
 
 	if (bcsp->rx_skb->data[0] & 0x80) {	/* reliable pkt */
 		BT_DBG("Received seqno %u from card", bcsp->rxseq_txack);
-		bcsp->rxseq_txack++;
-		bcsp->rxseq_txack %= 0x8;
-		bcsp->txack_req    = 1;
+
+		/* check the rx sequence number is as expected */
+		if ((bcsp->rx_skb->data[0] & 0x07) == bcsp->rxseq_txack) {
+			bcsp->rxseq_txack++;
+			bcsp->rxseq_txack %= 0x8;
+		} else {
+			/* handle re-transmitted packet or
+			 * when packet was missed
+			 */
+			BT_ERR("Out-of-order packet arrived, got %u expected %u",
+			       bcsp->rx_skb->data[0] & 0x07, bcsp->rxseq_txack);
+
+			/* do not process out-of-order packet payload */
+			pass_up = 2;
+		}
+
+		/* send current txack value to all received reliable packets */
+		bcsp->txack_req = 1;
 
 		/* If needed, transmit an ack pkt */
 		hci_uart_tx_wakeup(hu);
@@ -500,26 +518,33 @@ static void bcsp_complete_rx_pkt(struct hci_uart *hu)
 	bcsp->rxack = (bcsp->rx_skb->data[0] >> 3) & 0x07;
 	BT_DBG("Request for pkt %u from card", bcsp->rxack);
 
+	/* handle received ACK indications,
+	 * including those from out-of-order packets
+	 */
 	bcsp_pkt_cull(bcsp);
-	if ((bcsp->rx_skb->data[1] & 0x0f) == 6 &&
-			bcsp->rx_skb->data[0] & 0x80) {
-		hci_skb_pkt_type(bcsp->rx_skb) = HCI_ACLDATA_PKT;
-		pass_up = 1;
-	} else if ((bcsp->rx_skb->data[1] & 0x0f) == 5 &&
-			bcsp->rx_skb->data[0] & 0x80) {
-		hci_skb_pkt_type(bcsp->rx_skb) = HCI_EVENT_PKT;
-		pass_up = 1;
-	} else if ((bcsp->rx_skb->data[1] & 0x0f) == 7) {
-		hci_skb_pkt_type(bcsp->rx_skb) = HCI_SCODATA_PKT;
-		pass_up = 1;
-	} else if ((bcsp->rx_skb->data[1] & 0x0f) == 1 &&
-			!(bcsp->rx_skb->data[0] & 0x80)) {
-		bcsp_handle_le_pkt(hu);
-		pass_up = 0;
-	} else
-		pass_up = 0;
-
-	if (!pass_up) {
+
+	if (pass_up != 2) {
+		if ((bcsp->rx_skb->data[1] & 0x0f) == 6 &&
+		    (bcsp->rx_skb->data[0] & 0x80)) {
+			hci_skb_pkt_type(bcsp->rx_skb) = HCI_ACLDATA_PKT;
+			pass_up = 1;
+		} else if ((bcsp->rx_skb->data[1] & 0x0f) == 5 &&
+			   (bcsp->rx_skb->data[0] & 0x80)) {
+			hci_skb_pkt_type(bcsp->rx_skb) = HCI_EVENT_PKT;
+			pass_up = 1;
+		} else if ((bcsp->rx_skb->data[1] & 0x0f) == 7) {
+			hci_skb_pkt_type(bcsp->rx_skb) = HCI_SCODATA_PKT;
+			pass_up = 1;
+		} else if ((bcsp->rx_skb->data[1] & 0x0f) == 1 &&
+			   !(bcsp->rx_skb->data[0] & 0x80)) {
+			bcsp_handle_le_pkt(hu);
+			pass_up = 0;
+		} else {
+			pass_up = 0;
+		}
+	}
+
+	if (pass_up == 0) {
 		struct hci_event_hdr hdr;
 		u8 desc = (bcsp->rx_skb->data[1] & 0x0f);
 
@@ -537,18 +562,23 @@ 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)",
-					bcsp->rx_skb->data[1] & 0x0f,
-					bcsp->rx_skb->data[0] & 0x80 ?
-					"reliable" : "unreliable");
+				       bcsp->rx_skb->data[1] & 0x0f,
+				       bcsp->rx_skb->data[0] & 0x80 ?
+				       "reliable" : "unreliable");
 				kfree_skb(bcsp->rx_skb);
 			}
 		} else
 			kfree_skb(bcsp->rx_skb);
-	} else {
+	} else if (pass_up == 1) {
 		/* Pull out BCSP hdr */
 		skb_pull(bcsp->rx_skb, 4);
 
 		hci_recv_frame(hu->hdev, bcsp->rx_skb);
+	} else {
+		/* ignore packet payload of already ACKed re-transmitted
+		 * packets or when a packet was missed in the BCSP window
+		 */
+		kfree_skb(bcsp->rx_skb);
 	}
 
 	bcsp->rx_state = BCSP_W4_PKT_DELIMITER;
@@ -567,7 +597,7 @@ static int bcsp_recv(struct hci_uart *hu, const void *data, int count)
 	const unsigned char *ptr;
 
 	BT_DBG("hu %p count %d rx_state %d rx_count %ld",
-		hu, count, bcsp->rx_state, bcsp->rx_count);
+	       hu, count, bcsp->rx_state, bcsp->rx_count);
 
 	ptr = data;
 	while (count) {
@@ -586,24 +616,14 @@ static int bcsp_recv(struct hci_uart *hu, const void *data, int count)
 
 		switch (bcsp->rx_state) {
 		case BCSP_W4_BCSP_HDR:
-			if ((0xff & (u8) ~ (bcsp->rx_skb->data[0] + bcsp->rx_skb->data[1] +
-					bcsp->rx_skb->data[2])) != bcsp->rx_skb->data[3]) {
+			if ((0xff & (u8)~(bcsp->rx_skb->data[0] + bcsp->rx_skb->data[1] +
+			    bcsp->rx_skb->data[2])) != bcsp->rx_skb->data[3]) {
 				BT_ERR("Error in BCSP hdr checksum");
 				kfree_skb(bcsp->rx_skb);
 				bcsp->rx_state = BCSP_W4_PKT_DELIMITER;
 				bcsp->rx_count = 0;
 				continue;
 			}
-			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",
-					bcsp->rx_skb->data[0] & 0x07, bcsp->rxseq_txack);
-
-				kfree_skb(bcsp->rx_skb);
-				bcsp->rx_state = BCSP_W4_PKT_DELIMITER;
-				bcsp->rx_count = 0;
-				continue;
-			}
 			bcsp->rx_state = BCSP_W4_DATA;
 			bcsp->rx_count = (bcsp->rx_skb->data[1] >> 4) +
 					(bcsp->rx_skb->data[2] << 4);	/* May be 0 */
@@ -620,8 +640,8 @@ static int bcsp_recv(struct hci_uart *hu, const void *data, int count)
 		case BCSP_W4_CRC:
 			if (bitrev16(bcsp->message_crc) != bscp_get_crc(bcsp)) {
 				BT_ERR("Checksum failed: computed %04x received %04x",
-					bitrev16(bcsp->message_crc),
-					bscp_get_crc(bcsp));
+				       bitrev16(bcsp->message_crc),
+				       bscp_get_crc(bcsp));
 
 				kfree_skb(bcsp->rx_skb);
 				bcsp->rx_state = BCSP_W4_PKT_DELIMITER;
@@ -679,7 +699,7 @@ static int bcsp_recv(struct hci_uart *hu, const void *data, int count)
 	/* Arrange to retransmit all messages in the relq. */
 static void bcsp_timed_event(unsigned long arg)
 {
-	struct hci_uart *hu = (struct hci_uart *) arg;
+	struct hci_uart *hu = (struct hci_uart *)arg;
 	struct bcsp_struct *bcsp = hu->priv;
 	struct sk_buff *skb;
 	unsigned long flags;
@@ -715,7 +735,7 @@ static int bcsp_open(struct hci_uart *hu)
 
 	init_timer(&bcsp->tbcsp);
 	bcsp->tbcsp.function = bcsp_timed_event;
-	bcsp->tbcsp.data     = (u_long) hu;
+	bcsp->tbcsp.data     = (u_long)hu;
 
 	bcsp->rx_state = BCSP_W4_PKT_DELIMITER;
 
diff --git a/drivers/bluetooth/hci_intel.c b/drivers/bluetooth/hci_intel.c
index ed0a4201b551..9e271286c5e5 100644
--- a/drivers/bluetooth/hci_intel.c
+++ b/drivers/bluetooth/hci_intel.c
@@ -128,7 +128,7 @@ static int intel_wait_booting(struct hci_uart *hu)
 				  TASK_INTERRUPTIBLE,
 				  msecs_to_jiffies(1000));
 
-	if (err == 1) {
+	if (err == -EINTR) {
 		bt_dev_err(hu->hdev, "Device boot interrupted");
 		return -EINTR;
 	}
@@ -151,7 +151,7 @@ static int intel_wait_lpm_transaction(struct hci_uart *hu)
 				  TASK_INTERRUPTIBLE,
 				  msecs_to_jiffies(1000));
 
-	if (err == 1) {
+	if (err == -EINTR) {
 		bt_dev_err(hu->hdev, "LPM transaction interrupted");
 		return -EINTR;
 	}
@@ -813,7 +813,7 @@ static int intel_setup(struct hci_uart *hu)
 	err = wait_on_bit_timeout(&intel->flags, STATE_DOWNLOADING,
 				  TASK_INTERRUPTIBLE,
 				  msecs_to_jiffies(5000));
-	if (err == 1) {
+	if (err == -EINTR) {
 		bt_dev_err(hdev, "Firmware loading interrupted");
 		err = -EINTR;
 		goto done;
diff --git a/drivers/bluetooth/hci_ldisc.c b/drivers/bluetooth/hci_ldisc.c
index dda97398c59a..9497c469efd2 100644
--- a/drivers/bluetooth/hci_ldisc.c
+++ b/drivers/bluetooth/hci_ldisc.c
@@ -697,34 +697,36 @@ static int hci_uart_tty_ioctl(struct tty_struct *tty, struct file *file,
 	case HCIUARTSETPROTO:
 		if (!test_and_set_bit(HCI_UART_PROTO_SET, &hu->flags)) {
 			err = hci_uart_set_proto(hu, arg);
-			if (err) {
+			if (err)
 				clear_bit(HCI_UART_PROTO_SET, &hu->flags);
-				return err;
-			}
 		} else
-			return -EBUSY;
+			err = -EBUSY;
 		break;
 
 	case HCIUARTGETPROTO:
 		if (test_bit(HCI_UART_PROTO_SET, &hu->flags))
-			return hu->proto->id;
-		return -EUNATCH;
+			err = hu->proto->id;
+		else
+			err = -EUNATCH;
+		break;
 
 	case HCIUARTGETDEVICE:
 		if (test_bit(HCI_UART_REGISTERED, &hu->flags))
-			return hu->hdev->id;
-		return -EUNATCH;
+			err = hu->hdev->id;
+		else
+			err = -EUNATCH;
+		break;
 
 	case HCIUARTSETFLAGS:
 		if (test_bit(HCI_UART_PROTO_SET, &hu->flags))
-			return -EBUSY;
-		err = hci_uart_set_flags(hu, arg);
-		if (err)
-			return err;
+			err = -EBUSY;
+		else
+			err = hci_uart_set_flags(hu, arg);
 		break;
 
 	case HCIUARTGETFLAGS:
-		return hu->hdev_flags;
+		err = hu->hdev_flags;
+		break;
 
 	default:
 		err = n_tty_ioctl_helper(tty, file, cmd, arg);
@@ -810,6 +812,9 @@ static int __init hci_uart_init(void)
 #ifdef CONFIG_BT_HCIUART_AG6XX
 	ag6xx_init();
 #endif
+#ifdef CONFIG_BT_HCIUART_MRVL
+	mrvl_init();
+#endif
 
 	return 0;
 }
@@ -845,6 +850,9 @@ static void __exit hci_uart_exit(void)
 #ifdef CONFIG_BT_HCIUART_AG6XX
 	ag6xx_deinit();
 #endif
+#ifdef CONFIG_BT_HCIUART_MRVL
+	mrvl_deinit();
+#endif
 
 	/* Release tty registration of line discipline */
 	err = tty_unregister_ldisc(N_HCI);
diff --git a/drivers/bluetooth/hci_mrvl.c b/drivers/bluetooth/hci_mrvl.c
new file mode 100644
index 000000000000..bbc4b39b1dbf
--- /dev/null
+++ b/drivers/bluetooth/hci_mrvl.c
@@ -0,0 +1,387 @@
+/*
+ *
+ *  Bluetooth HCI UART driver for marvell devices
+ *
+ *  Copyright (C) 2016  Marvell International Ltd.
+ *  Copyright (C) 2016  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.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/skbuff.h>
+#include <linux/firmware.h>
+#include <linux/module.h>
+#include <linux/tty.h>
+
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci_core.h>
+
+#include "hci_uart.h"
+
+#define HCI_FW_REQ_PKT 0xA5
+#define HCI_CHIP_VER_PKT 0xAA
+
+#define MRVL_ACK 0x5A
+#define MRVL_NAK 0xBF
+#define MRVL_RAW_DATA 0x1F
+
+enum {
+	STATE_CHIP_VER_PENDING,
+	STATE_FW_REQ_PENDING,
+};
+
+struct mrvl_data {
+	struct sk_buff *rx_skb;
+	struct sk_buff_head txq;
+	struct sk_buff_head rawq;
+	unsigned long flags;
+	unsigned int tx_len;
+	u8 id, rev;
+};
+
+struct hci_mrvl_pkt {
+	__le16 lhs;
+	__le16 rhs;
+} __packed;
+#define HCI_MRVL_PKT_SIZE 4
+
+static int mrvl_open(struct hci_uart *hu)
+{
+	struct mrvl_data *mrvl;
+
+	BT_DBG("hu %p", hu);
+
+	mrvl = kzalloc(sizeof(*mrvl), GFP_KERNEL);
+	if (!mrvl)
+		return -ENOMEM;
+
+	skb_queue_head_init(&mrvl->txq);
+	skb_queue_head_init(&mrvl->rawq);
+
+	set_bit(STATE_CHIP_VER_PENDING, &mrvl->flags);
+
+	hu->priv = mrvl;
+	return 0;
+}
+
+static int mrvl_close(struct hci_uart *hu)
+{
+	struct mrvl_data *mrvl = hu->priv;
+
+	BT_DBG("hu %p", hu);
+
+	skb_queue_purge(&mrvl->txq);
+	skb_queue_purge(&mrvl->rawq);
+	kfree_skb(mrvl->rx_skb);
+	kfree(mrvl);
+
+	hu->priv = NULL;
+	return 0;
+}
+
+static int mrvl_flush(struct hci_uart *hu)
+{
+	struct mrvl_data *mrvl = hu->priv;
+
+	BT_DBG("hu %p", hu);
+
+	skb_queue_purge(&mrvl->txq);
+	skb_queue_purge(&mrvl->rawq);
+
+	return 0;
+}
+
+static struct sk_buff *mrvl_dequeue(struct hci_uart *hu)
+{
+	struct mrvl_data *mrvl = hu->priv;
+	struct sk_buff *skb;
+
+	skb = skb_dequeue(&mrvl->txq);
+	if (!skb) {
+		/* Any raw data ? */
+		skb = skb_dequeue(&mrvl->rawq);
+	} else {
+		/* Prepend skb with frame type */
+		memcpy(skb_push(skb, 1), &bt_cb(skb)->pkt_type, 1);
+	}
+
+	return skb;
+}
+
+static int mrvl_enqueue(struct hci_uart *hu, struct sk_buff *skb)
+{
+	struct mrvl_data *mrvl = hu->priv;
+
+	skb_queue_tail(&mrvl->txq, skb);
+	return 0;
+}
+
+static void mrvl_send_ack(struct hci_uart *hu, unsigned char type)
+{
+	struct mrvl_data *mrvl = hu->priv;
+	struct sk_buff *skb;
+
+	/* No H4 payload, only 1 byte header */
+	skb = bt_skb_alloc(0, GFP_ATOMIC);
+	if (!skb) {
+		bt_dev_err(hu->hdev, "Unable to alloc ack/nak packet");
+		return;
+	}
+	hci_skb_pkt_type(skb) = type;
+
+	skb_queue_tail(&mrvl->txq, skb);
+	hci_uart_tx_wakeup(hu);
+}
+
+static int mrvl_recv_fw_req(struct hci_dev *hdev, struct sk_buff *skb)
+{
+	struct hci_mrvl_pkt *pkt = (void *)skb->data;
+	struct hci_uart *hu = hci_get_drvdata(hdev);
+	struct mrvl_data *mrvl = hu->priv;
+	int ret = 0;
+
+	if ((pkt->lhs ^ pkt->rhs) != 0xffff) {
+		bt_dev_err(hdev, "Corrupted mrvl header");
+		mrvl_send_ack(hu, MRVL_NAK);
+		ret = -EINVAL;
+		goto done;
+	}
+	mrvl_send_ack(hu, MRVL_ACK);
+
+	if (!test_bit(STATE_FW_REQ_PENDING, &mrvl->flags)) {
+		bt_dev_err(hdev, "Received unexpected firmware request");
+		ret = -EINVAL;
+		goto done;
+	}
+
+	mrvl->tx_len = le16_to_cpu(pkt->lhs);
+
+	clear_bit(STATE_FW_REQ_PENDING, &mrvl->flags);
+	smp_mb__after_atomic();
+	wake_up_bit(&mrvl->flags, STATE_FW_REQ_PENDING);
+
+done:
+	kfree_skb(skb);
+	return ret;
+}
+
+static int mrvl_recv_chip_ver(struct hci_dev *hdev, struct sk_buff *skb)
+{
+	struct hci_mrvl_pkt *pkt = (void *)skb->data;
+	struct hci_uart *hu = hci_get_drvdata(hdev);
+	struct mrvl_data *mrvl = hu->priv;
+	u16 version = le16_to_cpu(pkt->lhs);
+	int ret = 0;
+
+	if ((pkt->lhs ^ pkt->rhs) != 0xffff) {
+		bt_dev_err(hdev, "Corrupted mrvl header");
+		mrvl_send_ack(hu, MRVL_NAK);
+		ret = -EINVAL;
+		goto done;
+	}
+	mrvl_send_ack(hu, MRVL_ACK);
+
+	if (!test_bit(STATE_CHIP_VER_PENDING, &mrvl->flags)) {
+		bt_dev_err(hdev, "Received unexpected chip version");
+		goto done;
+	}
+
+	mrvl->id = version;
+	mrvl->rev = version >> 8;
+
+	bt_dev_info(hdev, "Controller id = %x, rev = %x", mrvl->id, mrvl->rev);
+
+	clear_bit(STATE_CHIP_VER_PENDING, &mrvl->flags);
+	smp_mb__after_atomic();
+	wake_up_bit(&mrvl->flags, STATE_CHIP_VER_PENDING);
+
+done:
+	kfree_skb(skb);
+	return ret;
+}
+
+#define HCI_RECV_CHIP_VER \
+	.type = HCI_CHIP_VER_PKT, \
+	.hlen = HCI_MRVL_PKT_SIZE, \
+	.loff = 0, \
+	.lsize = 0, \
+	.maxlen = HCI_MRVL_PKT_SIZE
+
+#define HCI_RECV_FW_REQ \
+	.type = HCI_FW_REQ_PKT, \
+	.hlen = HCI_MRVL_PKT_SIZE, \
+	.loff = 0, \
+	.lsize = 0, \
+	.maxlen = HCI_MRVL_PKT_SIZE
+
+static const struct h4_recv_pkt mrvl_recv_pkts[] = {
+	{ H4_RECV_ACL,       .recv = hci_recv_frame     },
+	{ H4_RECV_SCO,       .recv = hci_recv_frame     },
+	{ H4_RECV_EVENT,     .recv = hci_recv_frame     },
+	{ HCI_RECV_FW_REQ,   .recv = mrvl_recv_fw_req   },
+	{ HCI_RECV_CHIP_VER, .recv = mrvl_recv_chip_ver },
+};
+
+static int mrvl_recv(struct hci_uart *hu, const void *data, int count)
+{
+	struct mrvl_data *mrvl = hu->priv;
+
+	if (!test_bit(HCI_UART_REGISTERED, &hu->flags))
+		return -EUNATCH;
+
+	mrvl->rx_skb = h4_recv_buf(hu->hdev, mrvl->rx_skb, data, count,
+				    mrvl_recv_pkts,
+				    ARRAY_SIZE(mrvl_recv_pkts));
+	if (IS_ERR(mrvl->rx_skb)) {
+		int err = PTR_ERR(mrvl->rx_skb);
+		bt_dev_err(hu->hdev, "Frame reassembly failed (%d)", err);
+		mrvl->rx_skb = NULL;
+		return err;
+	}
+
+	return count;
+}
+
+static int mrvl_load_firmware(struct hci_dev *hdev, const char *name)
+{
+	struct hci_uart *hu = hci_get_drvdata(hdev);
+	struct mrvl_data *mrvl = hu->priv;
+	const struct firmware *fw = NULL;
+	const u8 *fw_ptr, *fw_max;
+	int err;
+
+	err = request_firmware(&fw, name, &hdev->dev);
+	if (err < 0) {
+		bt_dev_err(hdev, "Failed to load firmware file %s", name);
+		return err;
+	}
+
+	fw_ptr = fw->data;
+	fw_max = fw->data + fw->size;
+
+	bt_dev_info(hdev, "Loading %s", name);
+
+	set_bit(STATE_FW_REQ_PENDING, &mrvl->flags);
+
+	while (fw_ptr <= fw_max) {
+		struct sk_buff *skb;
+
+		/* Controller drives the firmware load by sending firmware
+		 * request packets containing the expected fragment size.
+		 */
+		err = wait_on_bit_timeout(&mrvl->flags, STATE_FW_REQ_PENDING,
+					  TASK_INTERRUPTIBLE,
+					  msecs_to_jiffies(2000));
+		if (err == 1) {
+			bt_dev_err(hdev, "Firmware load interrupted");
+			err = -EINTR;
+			break;
+		} else if (err) {
+			bt_dev_err(hdev, "Firmware request timeout");
+			err = -ETIMEDOUT;
+			break;
+		}
+
+		bt_dev_dbg(hdev, "Firmware request, expecting %d bytes",
+			   mrvl->tx_len);
+
+		if (fw_ptr == fw_max) {
+			/* Controller requests a null size once firmware is
+			 * fully loaded. If controller expects more data, there
+			 * is an issue.
+			 */
+			if (!mrvl->tx_len) {
+				bt_dev_info(hdev, "Firmware loading complete");
+			} else {
+				bt_dev_err(hdev, "Firmware loading failure");
+				err = -EINVAL;
+			}
+			break;
+		}
+
+		if (fw_ptr + mrvl->tx_len > fw_max) {
+			mrvl->tx_len = fw_max - fw_ptr;
+			bt_dev_dbg(hdev, "Adjusting tx_len to %d",
+				   mrvl->tx_len);
+		}
+
+		skb = bt_skb_alloc(mrvl->tx_len, GFP_KERNEL);
+		if (!skb) {
+			bt_dev_err(hdev, "Failed to alloc mem for FW packet");
+			err = -ENOMEM;
+			break;
+		}
+		bt_cb(skb)->pkt_type = MRVL_RAW_DATA;
+
+		memcpy(skb_put(skb, mrvl->tx_len), fw_ptr, mrvl->tx_len);
+		fw_ptr += mrvl->tx_len;
+
+		set_bit(STATE_FW_REQ_PENDING, &mrvl->flags);
+
+		skb_queue_tail(&mrvl->rawq, skb);
+		hci_uart_tx_wakeup(hu);
+	}
+
+	release_firmware(fw);
+	return err;
+}
+
+static int mrvl_setup(struct hci_uart *hu)
+{
+	int err;
+
+	hci_uart_set_flow_control(hu, true);
+
+	err = mrvl_load_firmware(hu->hdev, "mrvl/helper_uart_3000000.bin");
+	if (err) {
+		bt_dev_err(hu->hdev, "Unable to download firmware helper");
+		return -EINVAL;
+	}
+
+	hci_uart_set_baudrate(hu, 3000000);
+	hci_uart_set_flow_control(hu, false);
+
+	err = mrvl_load_firmware(hu->hdev, "mrvl/uart8897_bt.bin");
+	if (err)
+		return err;
+
+	return 0;
+}
+
+static const struct hci_uart_proto mrvl_proto = {
+	.id		= HCI_UART_MRVL,
+	.name		= "Marvell",
+	.init_speed	= 115200,
+	.open		= mrvl_open,
+	.close		= mrvl_close,
+	.flush		= mrvl_flush,
+	.setup		= mrvl_setup,
+	.recv		= mrvl_recv,
+	.enqueue	= mrvl_enqueue,
+	.dequeue	= mrvl_dequeue,
+};
+
+int __init mrvl_init(void)
+{
+	return hci_uart_register_proto(&mrvl_proto);
+}
+
+int __exit mrvl_deinit(void)
+{
+	return hci_uart_unregister_proto(&mrvl_proto);
+}
diff --git a/drivers/bluetooth/hci_qca.c b/drivers/bluetooth/hci_qca.c
index 683c2b642057..6c867fbc56a7 100644
--- a/drivers/bluetooth/hci_qca.c
+++ b/drivers/bluetooth/hci_qca.c
@@ -397,7 +397,7 @@ static int qca_open(struct hci_uart *hu)
 	skb_queue_head_init(&qca->txq);
 	skb_queue_head_init(&qca->tx_wait_q);
 	spin_lock_init(&qca->hci_ibs_lock);
-	qca->workqueue = create_singlethread_workqueue("qca_wq");
+	qca->workqueue = alloc_ordered_workqueue("qca_wq", 0);
 	if (!qca->workqueue) {
 		BT_ERR("QCA Workqueue not initialized properly");
 		kfree(qca);
diff --git a/drivers/bluetooth/hci_uart.h b/drivers/bluetooth/hci_uart.h
index 839bad1d8152..070139513e65 100644
--- a/drivers/bluetooth/hci_uart.h
+++ b/drivers/bluetooth/hci_uart.h
@@ -35,7 +35,7 @@
 #define HCIUARTGETFLAGS		_IOR('U', 204, int)
 
 /* UART protocols */
-#define HCI_UART_MAX_PROTO	10
+#define HCI_UART_MAX_PROTO	12
 
 #define HCI_UART_H4	0
 #define HCI_UART_BCSP	1
@@ -47,6 +47,8 @@
 #define HCI_UART_BCM	7
 #define HCI_UART_QCA	8
 #define HCI_UART_AG6XX	9
+#define HCI_UART_NOKIA	10
+#define HCI_UART_MRVL	11
 
 #define HCI_UART_RAW_DEVICE	0
 #define HCI_UART_RESET_ON_INIT	1
@@ -189,3 +191,8 @@ int qca_deinit(void);
 int ag6xx_init(void);
 int ag6xx_deinit(void);
 #endif
+
+#ifdef CONFIG_BT_HCIUART_MRVL
+int mrvl_init(void);
+int mrvl_deinit(void);
+#endif
diff --git a/drivers/bluetooth/hci_vhci.c b/drivers/bluetooth/hci_vhci.c
index 3ff229b2e7f3..c4a75a18dcae 100644
--- a/drivers/bluetooth/hci_vhci.c
+++ b/drivers/bluetooth/hci_vhci.c
@@ -377,21 +377,7 @@ static struct miscdevice vhci_miscdev = {
 	.fops	= &vhci_fops,
 	.minor	= VHCI_MINOR,
 };
-
-static int __init vhci_init(void)
-{
-	BT_INFO("Virtual HCI driver ver %s", VERSION);
-
-	return misc_register(&vhci_miscdev);
-}
-
-static void __exit vhci_exit(void)
-{
-	misc_deregister(&vhci_miscdev);
-}
-
-module_init(vhci_init);
-module_exit(vhci_exit);
+module_misc_device(vhci_miscdev);
 
 module_param(amp, bool, 0644);
 MODULE_PARM_DESC(amp, "Create AMP controller device");
diff --git a/drivers/bus/Kconfig b/drivers/bus/Kconfig
index 3b205e212337..7010dcac9328 100644
--- a/drivers/bus/Kconfig
+++ b/drivers/bus/Kconfig
@@ -108,6 +108,14 @@ config OMAP_OCP2SCP
 	  OCP2SCP and in OMAP5, both USB PHY and SATA PHY is connected via
 	  OCP2SCP.
 
+config QCOM_EBI2
+	bool "Qualcomm External Bus Interface 2 (EBI2)"
+	depends on HAS_IOMEM
+	help
+	  Say y here to enable support for the Qualcomm External Bus
+	  Interface 2, which can be used to connect things like NAND Flash,
+	  SRAM, ethernet adapters, FPGAs and LCD displays.
+
 config SIMPLE_PM_BUS
 	bool "Simple Power-Managed Bus Driver"
 	depends on OF && PM
@@ -132,12 +140,8 @@ config SUNXI_RSB
 	  with various RSB based devices, such as AXP223, AXP8XX PMICs,
 	  and AC100/AC200 ICs.
 
-# TODO: This uses pm_clk_*() symbols that aren't exported in v4.7 and hence
-# the driver will fail to build as a module. However there are patches to
-# address that queued for v4.8, so this can be turned into a tristate symbol
-# after v4.8-rc1.
 config TEGRA_ACONNECT
-	bool "Tegra ACONNECT Bus Driver"
+	tristate "Tegra ACONNECT Bus Driver"
 	depends on ARCH_TEGRA_210_SOC
 	depends on OF && PM
 	select PM_CLK
diff --git a/drivers/bus/Makefile b/drivers/bus/Makefile
index ac84cc4348e3..c6cfa6b2606e 100644
--- a/drivers/bus/Makefile
+++ b/drivers/bus/Makefile
@@ -15,6 +15,7 @@ obj-$(CONFIG_MVEBU_MBUS) 	+= mvebu-mbus.o
 obj-$(CONFIG_OMAP_INTERCONNECT)	+= omap_l3_smx.o omap_l3_noc.o
 
 obj-$(CONFIG_OMAP_OCP2SCP)	+= omap-ocp2scp.o
+obj-$(CONFIG_QCOM_EBI2)		+= qcom-ebi2.o
 obj-$(CONFIG_SUNXI_RSB)		+= sunxi-rsb.o
 obj-$(CONFIG_SIMPLE_PM_BUS)	+= simple-pm-bus.o
 obj-$(CONFIG_TEGRA_ACONNECT)	+= tegra-aconnect.o
diff --git a/drivers/bus/arm-cci.c b/drivers/bus/arm-cci.c
index ffa7c9dcbd7a..890082315054 100644
--- a/drivers/bus/arm-cci.c
+++ b/drivers/bus/arm-cci.c
@@ -144,15 +144,12 @@ struct cci_pmu {
 	int num_cntrs;
 	atomic_t active_events;
 	struct mutex reserve_mutex;
-	struct list_head entry;
+	struct hlist_node node;
 	cpumask_t cpus;
 };
 
 #define to_cci_pmu(c)	(container_of(c, struct cci_pmu, pmu))
 
-static DEFINE_MUTEX(cci_pmu_mutex);
-static LIST_HEAD(cci_pmu_list);
-
 enum cci_models {
 #ifdef CONFIG_ARM_CCI400_PMU
 	CCI400_R0,
@@ -1506,25 +1503,21 @@ static int cci_pmu_init(struct cci_pmu *cci_pmu, struct platform_device *pdev)
 	return perf_pmu_register(&cci_pmu->pmu, name, -1);
 }
 
-static int cci_pmu_offline_cpu(unsigned int cpu)
+static int cci_pmu_offline_cpu(unsigned int cpu, struct hlist_node *node)
 {
-	struct cci_pmu *cci_pmu;
+	struct cci_pmu *cci_pmu = hlist_entry_safe(node, struct cci_pmu, node);
 	unsigned int target;
 
-	mutex_lock(&cci_pmu_mutex);
-	list_for_each_entry(cci_pmu, &cci_pmu_list, entry) {
-		if (!cpumask_test_and_clear_cpu(cpu, &cci_pmu->cpus))
-			continue;
-		target = cpumask_any_but(cpu_online_mask, cpu);
-		if (target >= nr_cpu_ids)
-			continue;
-		/*
-		 * TODO: migrate context once core races on event->ctx have
-		 * been fixed.
-		 */
-		cpumask_set_cpu(target, &cci_pmu->cpus);
-	}
-	mutex_unlock(&cci_pmu_mutex);
+	if (!cpumask_test_and_clear_cpu(cpu, &cci_pmu->cpus))
+		return 0;
+	target = cpumask_any_but(cpu_online_mask, cpu);
+	if (target >= nr_cpu_ids)
+		return 0;
+	/*
+	 * TODO: migrate context once core races on event->ctx have
+	 * been fixed.
+	 */
+	cpumask_set_cpu(target, &cci_pmu->cpus);
 	return 0;
 }
 
@@ -1768,10 +1761,8 @@ static int cci_pmu_probe(struct platform_device *pdev)
 	if (ret)
 		return ret;
 
-	mutex_lock(&cci_pmu_mutex);
-	list_add(&cci_pmu->entry, &cci_pmu_list);
-	mutex_unlock(&cci_pmu_mutex);
-
+	cpuhp_state_add_instance_nocalls(CPUHP_AP_PERF_ARM_CCI_ONLINE,
+					 &cci_pmu->node);
 	pr_info("ARM %s PMU driver probed", cci_pmu->model->name);
 	return 0;
 }
@@ -1804,9 +1795,9 @@ static int __init cci_platform_init(void)
 {
 	int ret;
 
-	ret = cpuhp_setup_state_nocalls(CPUHP_AP_PERF_ARM_CCI_ONLINE,
-					"AP_PERF_ARM_CCI_ONLINE", NULL,
-					cci_pmu_offline_cpu);
+	ret = cpuhp_setup_state_multi(CPUHP_AP_PERF_ARM_CCI_ONLINE,
+				      "AP_PERF_ARM_CCI_ONLINE", NULL,
+				      cci_pmu_offline_cpu);
 	if (ret)
 		return ret;
 
diff --git a/drivers/bus/arm-ccn.c b/drivers/bus/arm-ccn.c
index 884c0305e290..d1074d9b38ba 100644
--- a/drivers/bus/arm-ccn.c
+++ b/drivers/bus/arm-ccn.c
@@ -167,7 +167,7 @@ struct arm_ccn_dt {
 	struct hrtimer hrtimer;
 
 	cpumask_t cpu;
-	struct list_head entry;
+	struct hlist_node node;
 
 	struct pmu pmu;
 };
@@ -190,9 +190,6 @@ struct arm_ccn {
 	int mn_id;
 };
 
-static DEFINE_MUTEX(arm_ccn_mutex);
-static LIST_HEAD(arm_ccn_list);
-
 static int arm_ccn_node_to_xp(int node)
 {
 	return node / CCN_NUM_XP_PORTS;
@@ -1214,30 +1211,24 @@ static enum hrtimer_restart arm_ccn_pmu_timer_handler(struct hrtimer *hrtimer)
 }
 
 
-static int arm_ccn_pmu_offline_cpu(unsigned int cpu)
+static int arm_ccn_pmu_offline_cpu(unsigned int cpu, struct hlist_node *node)
 {
-	struct arm_ccn_dt *dt;
+	struct arm_ccn_dt *dt = hlist_entry_safe(node, struct arm_ccn_dt, node);
+	struct arm_ccn *ccn = container_of(dt, struct arm_ccn, dt);
 	unsigned int target;
 
-	mutex_lock(&arm_ccn_mutex);
-	list_for_each_entry(dt, &arm_ccn_list, entry) {
-		struct arm_ccn *ccn = container_of(dt, struct arm_ccn, dt);
-
-		if (!cpumask_test_and_clear_cpu(cpu, &dt->cpu))
-			continue;
-		target = cpumask_any_but(cpu_online_mask, cpu);
-		if (target >= nr_cpu_ids)
-			continue;
-		perf_pmu_migrate_context(&dt->pmu, cpu, target);
-		cpumask_set_cpu(target, &dt->cpu);
-		if (ccn->irq)
-			WARN_ON(irq_set_affinity_hint(ccn->irq, &dt->cpu) != 0);
-	}
-	mutex_unlock(&arm_ccn_mutex);
+	if (!cpumask_test_and_clear_cpu(cpu, &dt->cpu))
+		return 0;
+	target = cpumask_any_but(cpu_online_mask, cpu);
+	if (target >= nr_cpu_ids)
+		return 0;
+	perf_pmu_migrate_context(&dt->pmu, cpu, target);
+	cpumask_set_cpu(target, &dt->cpu);
+	if (ccn->irq)
+		WARN_ON(irq_set_affinity_hint(ccn->irq, &dt->cpu) != 0);
 	return 0;
 }
 
-
 static DEFINE_IDA(arm_ccn_pmu_ida);
 
 static int arm_ccn_pmu_init(struct arm_ccn *ccn)
@@ -1321,9 +1312,8 @@ static int arm_ccn_pmu_init(struct arm_ccn *ccn)
 	if (err)
 		goto error_pmu_register;
 
-	mutex_lock(&arm_ccn_mutex);
-	list_add(&ccn->dt.entry, &arm_ccn_list);
-	mutex_unlock(&arm_ccn_mutex);
+	cpuhp_state_add_instance_nocalls(CPUHP_AP_PERF_ARM_CCN_ONLINE,
+					 &ccn->dt.node);
 	return 0;
 
 error_pmu_register:
@@ -1339,10 +1329,8 @@ static void arm_ccn_pmu_cleanup(struct arm_ccn *ccn)
 {
 	int i;
 
-	mutex_lock(&arm_ccn_mutex);
-	list_del(&ccn->dt.entry);
-	mutex_unlock(&arm_ccn_mutex);
-
+	cpuhp_state_remove_instance_nocalls(CPUHP_AP_PERF_ARM_CCN_ONLINE,
+					    &ccn->dt.node);
 	if (ccn->irq)
 		irq_set_affinity_hint(ccn->irq, NULL);
 	for (i = 0; i < ccn->num_xps; i++)
@@ -1573,9 +1561,9 @@ static int __init arm_ccn_init(void)
 {
 	int i, ret;
 
-	ret = cpuhp_setup_state_nocalls(CPUHP_AP_PERF_ARM_CCN_ONLINE,
-					"AP_PERF_ARM_CCN_ONLINE", NULL,
-					arm_ccn_pmu_offline_cpu);
+	ret = cpuhp_setup_state_multi(CPUHP_AP_PERF_ARM_CCN_ONLINE,
+				      "AP_PERF_ARM_CCN_ONLINE", NULL,
+				      arm_ccn_pmu_offline_cpu);
 	if (ret)
 		return ret;
 
@@ -1587,7 +1575,7 @@ static int __init arm_ccn_init(void)
 
 static void __exit arm_ccn_exit(void)
 {
-	cpuhp_remove_state_nocalls(CPUHP_AP_PERF_ARM_CCN_ONLINE);
+	cpuhp_remove_multi_state(CPUHP_AP_PERF_ARM_CCN_ONLINE);
 	platform_driver_unregister(&arm_ccn_driver);
 }
 
diff --git a/drivers/bus/mips_cdmm.c b/drivers/bus/mips_cdmm.c
index cad49bc38b3e..1b14256376d2 100644
--- a/drivers/bus/mips_cdmm.c
+++ b/drivers/bus/mips_cdmm.c
@@ -596,19 +596,20 @@ BUILD_PERDEV_HELPER(cpu_down)       /* int mips_cdmm_cpu_down_helper(...) */
 BUILD_PERDEV_HELPER(cpu_up)         /* int mips_cdmm_cpu_up_helper(...) */
 
 /**
- * mips_cdmm_bus_down() - Tear down the CDMM bus.
- * @data:	Pointer to unsigned int CPU number.
+ * mips_cdmm_cpu_down_prep() - Callback for CPUHP DOWN_PREP:
+ *			       Tear down the CDMM bus.
+ * @cpu:	unsigned int CPU number.
  *
  * This function is executed on the hotplugged CPU and calls the CDMM
  * driver cpu_down callback for all devices on that CPU.
  */
-static long mips_cdmm_bus_down(void *data)
+static int mips_cdmm_cpu_down_prep(unsigned int cpu)
 {
 	struct mips_cdmm_bus *bus;
 	long ret;
 
 	/* Inform all the devices on the bus */
-	ret = bus_for_each_dev(&mips_cdmm_bustype, NULL, data,
+	ret = bus_for_each_dev(&mips_cdmm_bustype, NULL, &cpu,
 			       mips_cdmm_cpu_down_helper);
 
 	/*
@@ -623,8 +624,8 @@ static long mips_cdmm_bus_down(void *data)
 }
 
 /**
- * mips_cdmm_bus_up() - Bring up the CDMM bus.
- * @data:	Pointer to unsigned int CPU number.
+ * mips_cdmm_cpu_online() - Callback for CPUHP ONLINE: Bring up the CDMM bus.
+ * @cpu:	unsigned int CPU number.
  *
  * This work_on_cpu callback function is executed on a given CPU to discover
  * CDMM devices on that CPU, or to call the CDMM driver cpu_up callback for all
@@ -634,7 +635,7 @@ static long mips_cdmm_bus_down(void *data)
  * initialisation. When CPUs are brought online the function is
  * invoked directly on the hotplugged CPU.
  */
-static long mips_cdmm_bus_up(void *data)
+static int mips_cdmm_cpu_online(unsigned int cpu)
 {
 	struct mips_cdmm_bus *bus;
 	long ret;
@@ -651,51 +652,13 @@ static long mips_cdmm_bus_up(void *data)
 		mips_cdmm_bus_discover(bus);
 	else
 		/* Inform all the devices on the bus */
-		ret = bus_for_each_dev(&mips_cdmm_bustype, NULL, data,
+		ret = bus_for_each_dev(&mips_cdmm_bustype, NULL, &cpu,
 				       mips_cdmm_cpu_up_helper);
 
 	return ret;
 }
 
 /**
- * mips_cdmm_cpu_notify() - Take action when a CPU is going online or offline.
- * @nb:		CPU notifier block .
- * @action:	Event that has taken place (CPU_*).
- * @data:	CPU number.
- *
- * This notifier is used to keep the CDMM buses updated as CPUs are offlined and
- * onlined. When CPUs go offline or come back online, so does their CDMM bus, so
- * devices must be informed. Also when CPUs come online for the first time the
- * devices on the CDMM bus need discovering.
- *
- * Returns:	NOTIFY_OK if event was used.
- *		NOTIFY_DONE if we didn't care.
- */
-static int mips_cdmm_cpu_notify(struct notifier_block *nb,
-				unsigned long action, void *data)
-{
-	unsigned int cpu = (unsigned int)data;
-
-	switch (action & ~CPU_TASKS_FROZEN) {
-	case CPU_ONLINE:
-	case CPU_DOWN_FAILED:
-		mips_cdmm_bus_up(&cpu);
-		break;
-	case CPU_DOWN_PREPARE:
-		mips_cdmm_bus_down(&cpu);
-		break;
-	default:
-		return NOTIFY_DONE;
-	}
-
-	return NOTIFY_OK;
-}
-
-static struct notifier_block mips_cdmm_cpu_nb = {
-	.notifier_call = mips_cdmm_cpu_notify,
-};
-
-/**
  * mips_cdmm_init() - Initialise CDMM bus.
  *
  * Initialise CDMM bus, discover CDMM devices for online CPUs, and arrange for
@@ -703,7 +666,6 @@ static struct notifier_block mips_cdmm_cpu_nb = {
  */
 static int __init mips_cdmm_init(void)
 {
-	unsigned int cpu;
 	int ret;
 
 	/* Register the bus */
@@ -712,19 +674,11 @@ static int __init mips_cdmm_init(void)
 		return ret;
 
 	/* We want to be notified about new CPUs */
-	ret = register_cpu_notifier(&mips_cdmm_cpu_nb);
-	if (ret) {
+	ret = cpuhp_setup_state(CPUHP_AP_ONLINE_DYN, "bus/cdmm:online",
+				mips_cdmm_cpu_online, mips_cdmm_cpu_down_prep);
+	if (ret < 0)
 		pr_warn("cdmm: Failed to register CPU notifier\n");
-		goto out;
-	}
-
-	/* Discover devices on CDMM of online CPUs */
-	for_each_online_cpu(cpu)
-		work_on_cpu(cpu, mips_cdmm_bus_up, &cpu);
 
-	return 0;
-out:
-	bus_unregister(&mips_cdmm_bustype);
 	return ret;
 }
 subsys_initcall(mips_cdmm_init);
diff --git a/drivers/bus/qcom-ebi2.c b/drivers/bus/qcom-ebi2.c
new file mode 100644
index 000000000000..a6444244c411
--- /dev/null
+++ b/drivers/bus/qcom-ebi2.c
@@ -0,0 +1,408 @@
+/*
+ * Qualcomm External Bus Interface 2 (EBI2) driver
+ * an older version of the Qualcomm Parallel Interface Controller (QPIC)
+ *
+ * Copyright (C) 2016 Linaro Ltd.
+ *
+ * Author: Linus Walleij <linus.walleij@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.
+ *
+ * See the device tree bindings for this block for more details on the
+ * hardware.
+ */
+
+#include <linux/module.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/of.h>
+#include <linux/of_platform.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+#include <linux/bitops.h>
+
+/*
+ * CS0, CS1, CS4 and CS5 are two bits wide, CS2 and CS3 are one bit.
+ */
+#define EBI2_CS0_ENABLE_MASK BIT(0)|BIT(1)
+#define EBI2_CS1_ENABLE_MASK BIT(2)|BIT(3)
+#define EBI2_CS2_ENABLE_MASK BIT(4)
+#define EBI2_CS3_ENABLE_MASK BIT(5)
+#define EBI2_CS4_ENABLE_MASK BIT(6)|BIT(7)
+#define EBI2_CS5_ENABLE_MASK BIT(8)|BIT(9)
+#define EBI2_CSN_MASK GENMASK(9, 0)
+
+#define EBI2_XMEM_CFG 0x0000 /* Power management etc */
+
+/*
+ * SLOW CSn CFG
+ *
+ * Bits 31-28: RECOVERY recovery cycles (0 = 1, 1 = 2 etc) this is the time the
+ *             memory continues to drive the data bus after OE is de-asserted.
+ *             Inserted when reading one CS and switching to another CS or read
+ *             followed by write on the same CS. Valid values 0 thru 15.
+ * Bits 27-24: WR_HOLD write hold cycles, these are extra cycles inserted after
+ *             every write minimum 1. The data out is driven from the time WE is
+ *             asserted until CS is asserted. With a hold of 1, the CS stays
+ *             active for 1 extra cycle etc. Valid values 0 thru 15.
+ * Bits 23-16: WR_DELTA initial latency for write cycles inserted for the first
+ *             write to a page or burst memory
+ * Bits 15-8:  RD_DELTA initial latency for read cycles inserted for the first
+ *             read to a page or burst memory
+ * Bits 7-4:   WR_WAIT number of wait cycles for every write access, 0=1 cycle
+ *             so 1 thru 16 cycles.
+ * Bits 3-0:   RD_WAIT number of wait cycles for every read access, 0=1 cycle
+ *             so 1 thru 16 cycles.
+ */
+#define EBI2_XMEM_CS0_SLOW_CFG 0x0008
+#define EBI2_XMEM_CS1_SLOW_CFG 0x000C
+#define EBI2_XMEM_CS2_SLOW_CFG 0x0010
+#define EBI2_XMEM_CS3_SLOW_CFG 0x0014
+#define EBI2_XMEM_CS4_SLOW_CFG 0x0018
+#define EBI2_XMEM_CS5_SLOW_CFG 0x001C
+
+#define EBI2_XMEM_RECOVERY_SHIFT	28
+#define EBI2_XMEM_WR_HOLD_SHIFT		24
+#define EBI2_XMEM_WR_DELTA_SHIFT	16
+#define EBI2_XMEM_RD_DELTA_SHIFT	8
+#define EBI2_XMEM_WR_WAIT_SHIFT		4
+#define EBI2_XMEM_RD_WAIT_SHIFT		0
+
+/*
+ * FAST CSn CFG
+ * Bits 31-28: ?
+ * Bits 27-24: RD_HOLD: the length in cycles of the first segment of a read
+ *             transfer. For a single read trandfer this will be the time
+ *             from CS assertion to OE assertion.
+ * Bits 18-24: ?
+ * Bits 17-16: ADV_OE_RECOVERY, the number of cycles elapsed before an OE
+ *             assertion, with respect to the cycle where ADV is asserted.
+ *             2 means 2 cycles between ADV and OE. Values 0, 1, 2 or 3.
+ * Bits 5:     ADDR_HOLD_ENA, The address is held for an extra cycle to meet
+ *             hold time requirements with ADV assertion.
+ *
+ * The manual mentions "write precharge cycles" and "precharge cycles".
+ * We have not been able to figure out which bit fields these correspond to
+ * in the hardware, or what valid values exist. The current hypothesis is that
+ * this is something just used on the FAST chip selects. There is also a "byte
+ * device enable" flag somewhere for 8bit memories.
+ */
+#define EBI2_XMEM_CS0_FAST_CFG 0x0028
+#define EBI2_XMEM_CS1_FAST_CFG 0x002C
+#define EBI2_XMEM_CS2_FAST_CFG 0x0030
+#define EBI2_XMEM_CS3_FAST_CFG 0x0034
+#define EBI2_XMEM_CS4_FAST_CFG 0x0038
+#define EBI2_XMEM_CS5_FAST_CFG 0x003C
+
+#define EBI2_XMEM_RD_HOLD_SHIFT		24
+#define EBI2_XMEM_ADV_OE_RECOVERY_SHIFT	16
+#define EBI2_XMEM_ADDR_HOLD_ENA_SHIFT	5
+
+/**
+ * struct cs_data - struct with info on a chipselect setting
+ * @enable_mask: mask to enable the chipselect in the EBI2 config
+ * @slow_cfg0: offset to XMEMC slow CS config
+ * @fast_cfg1: offset to XMEMC fast CS config
+ */
+struct cs_data {
+	u32 enable_mask;
+	u16 slow_cfg;
+	u16 fast_cfg;
+};
+
+static const struct cs_data cs_info[] = {
+	{
+		/* CS0 */
+		.enable_mask = EBI2_CS0_ENABLE_MASK,
+		.slow_cfg = EBI2_XMEM_CS0_SLOW_CFG,
+		.fast_cfg = EBI2_XMEM_CS0_FAST_CFG,
+	},
+	{
+		/* CS1 */
+		.enable_mask = EBI2_CS1_ENABLE_MASK,
+		.slow_cfg = EBI2_XMEM_CS1_SLOW_CFG,
+		.fast_cfg = EBI2_XMEM_CS1_FAST_CFG,
+	},
+	{
+		/* CS2 */
+		.enable_mask = EBI2_CS2_ENABLE_MASK,
+		.slow_cfg = EBI2_XMEM_CS2_SLOW_CFG,
+		.fast_cfg = EBI2_XMEM_CS2_FAST_CFG,
+	},
+	{
+		/* CS3 */
+		.enable_mask = EBI2_CS3_ENABLE_MASK,
+		.slow_cfg = EBI2_XMEM_CS3_SLOW_CFG,
+		.fast_cfg = EBI2_XMEM_CS3_FAST_CFG,
+	},
+	{
+		/* CS4 */
+		.enable_mask = EBI2_CS4_ENABLE_MASK,
+		.slow_cfg = EBI2_XMEM_CS4_SLOW_CFG,
+		.fast_cfg = EBI2_XMEM_CS4_FAST_CFG,
+	},
+	{
+		/* CS5 */
+		.enable_mask = EBI2_CS5_ENABLE_MASK,
+		.slow_cfg = EBI2_XMEM_CS5_SLOW_CFG,
+		.fast_cfg = EBI2_XMEM_CS5_FAST_CFG,
+	},
+};
+
+/**
+ * struct ebi2_xmem_prop - describes an XMEM config property
+ * @prop: the device tree binding name
+ * @max: maximum value for the property
+ * @slowreg: true if this property is in the SLOW CS config register
+ * else it is assumed to be in the FAST config register
+ * @shift: the bit field start in the SLOW or FAST register for this
+ * property
+ */
+struct ebi2_xmem_prop {
+	const char *prop;
+	u32 max;
+	bool slowreg;
+	u16 shift;
+};
+
+static const struct ebi2_xmem_prop xmem_props[] = {
+	{
+		.prop = "qcom,xmem-recovery-cycles",
+		.max = 15,
+		.slowreg = true,
+		.shift = EBI2_XMEM_RECOVERY_SHIFT,
+	},
+	{
+		.prop = "qcom,xmem-write-hold-cycles",
+		.max = 15,
+		.slowreg = true,
+		.shift = EBI2_XMEM_WR_HOLD_SHIFT,
+	},
+	{
+		.prop = "qcom,xmem-write-delta-cycles",
+		.max = 255,
+		.slowreg = true,
+		.shift = EBI2_XMEM_WR_DELTA_SHIFT,
+	},
+	{
+		.prop = "qcom,xmem-read-delta-cycles",
+		.max = 255,
+		.slowreg = true,
+		.shift = EBI2_XMEM_RD_DELTA_SHIFT,
+	},
+	{
+		.prop = "qcom,xmem-write-wait-cycles",
+		.max = 15,
+		.slowreg = true,
+		.shift = EBI2_XMEM_WR_WAIT_SHIFT,
+	},
+	{
+		.prop = "qcom,xmem-read-wait-cycles",
+		.max = 15,
+		.slowreg = true,
+		.shift = EBI2_XMEM_RD_WAIT_SHIFT,
+	},
+	{
+		.prop = "qcom,xmem-address-hold-enable",
+		.max = 1, /* boolean prop */
+		.slowreg = false,
+		.shift = EBI2_XMEM_ADDR_HOLD_ENA_SHIFT,
+	},
+	{
+		.prop = "qcom,xmem-adv-to-oe-recovery-cycles",
+		.max = 3,
+		.slowreg = false,
+		.shift = EBI2_XMEM_ADV_OE_RECOVERY_SHIFT,
+	},
+	{
+		.prop = "qcom,xmem-read-hold-cycles",
+		.max = 15,
+		.slowreg = false,
+		.shift = EBI2_XMEM_RD_HOLD_SHIFT,
+	},
+};
+
+static void qcom_ebi2_setup_chipselect(struct device_node *np,
+				       struct device *dev,
+				       void __iomem *ebi2_base,
+				       void __iomem *ebi2_xmem,
+				       u32 csindex)
+{
+	const struct cs_data *csd;
+	u32 slowcfg, fastcfg;
+	u32 val;
+	int ret;
+	int i;
+
+	csd = &cs_info[csindex];
+	val = readl(ebi2_base);
+	val |= csd->enable_mask;
+	writel(val, ebi2_base);
+	dev_dbg(dev, "enabled CS%u\n", csindex);
+
+	/* Next set up the XMEMC */
+	slowcfg = 0;
+	fastcfg = 0;
+
+	for (i = 0; i < ARRAY_SIZE(xmem_props); i++) {
+		const struct ebi2_xmem_prop *xp = &xmem_props[i];
+
+		/* All are regular u32 values */
+		ret = of_property_read_u32(np, xp->prop, &val);
+		if (ret) {
+			dev_dbg(dev, "could not read %s for CS%d\n",
+				xp->prop, csindex);
+			continue;
+		}
+
+		/* First check boolean props */
+		if (xp->max == 1 && val) {
+			if (xp->slowreg)
+				slowcfg |= BIT(xp->shift);
+			else
+				fastcfg |= BIT(xp->shift);
+			dev_dbg(dev, "set %s flag\n", xp->prop);
+			continue;
+		}
+
+		/* We're dealing with an u32 */
+		if (val > xp->max) {
+			dev_err(dev,
+				"too high value for %s: %u, capped at %u\n",
+				xp->prop, val, xp->max);
+			val = xp->max;
+		}
+		if (xp->slowreg)
+			slowcfg |= (val << xp->shift);
+		else
+			fastcfg |= (val << xp->shift);
+		dev_dbg(dev, "set %s to %u\n", xp->prop, val);
+	}
+
+	dev_info(dev, "CS%u: SLOW CFG 0x%08x, FAST CFG 0x%08x\n",
+		 csindex, slowcfg, fastcfg);
+
+	if (slowcfg)
+		writel(slowcfg, ebi2_xmem + csd->slow_cfg);
+	if (fastcfg)
+		writel(fastcfg, ebi2_xmem + csd->fast_cfg);
+}
+
+static int qcom_ebi2_probe(struct platform_device *pdev)
+{
+	struct device_node *np = pdev->dev.of_node;
+	struct device_node *child;
+	struct device *dev = &pdev->dev;
+	struct resource *res;
+	void __iomem *ebi2_base;
+	void __iomem *ebi2_xmem;
+	struct clk *ebi2xclk;
+	struct clk *ebi2clk;
+	bool have_children = false;
+	u32 val;
+	int ret;
+
+	ebi2xclk = devm_clk_get(dev, "ebi2x");
+	if (IS_ERR(ebi2xclk))
+		return PTR_ERR(ebi2xclk);
+
+	ret = clk_prepare_enable(ebi2xclk);
+	if (ret) {
+		dev_err(dev, "could not enable EBI2X clk (%d)\n", ret);
+		return ret;
+	}
+
+	ebi2clk = devm_clk_get(dev, "ebi2");
+	if (IS_ERR(ebi2clk)) {
+		ret = PTR_ERR(ebi2clk);
+		goto err_disable_2x_clk;
+	}
+
+	ret = clk_prepare_enable(ebi2clk);
+	if (ret) {
+		dev_err(dev, "could not enable EBI2 clk\n");
+		goto err_disable_2x_clk;
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	ebi2_base = devm_ioremap_resource(dev, res);
+	if (IS_ERR(ebi2_base)) {
+		ret = PTR_ERR(ebi2_base);
+		goto err_disable_clk;
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	ebi2_xmem = devm_ioremap_resource(dev, res);
+	if (IS_ERR(ebi2_xmem)) {
+		ret = PTR_ERR(ebi2_xmem);
+		goto err_disable_clk;
+	}
+
+	/* Allegedly this turns the power save mode off */
+	writel(0UL, ebi2_xmem + EBI2_XMEM_CFG);
+
+	/* Disable all chipselects */
+	val = readl(ebi2_base);
+	val &= ~EBI2_CSN_MASK;
+	writel(val, ebi2_base);
+
+	/* Walk over the child nodes and see what chipselects we use */
+	for_each_available_child_of_node(np, child) {
+		u32 csindex;
+
+		/* Figure out the chipselect */
+		ret = of_property_read_u32(child, "reg", &csindex);
+		if (ret)
+			return ret;
+
+		if (csindex > 5) {
+			dev_err(dev,
+				"invalid chipselect %u, we only support 0-5\n",
+				csindex);
+			continue;
+		}
+
+		qcom_ebi2_setup_chipselect(child,
+					   dev,
+					   ebi2_base,
+					   ebi2_xmem,
+					   csindex);
+
+		/* We have at least one child */
+		have_children = true;
+	}
+
+	if (have_children)
+		return of_platform_default_populate(np, NULL, dev);
+	return 0;
+
+err_disable_clk:
+	clk_disable_unprepare(ebi2clk);
+err_disable_2x_clk:
+	clk_disable_unprepare(ebi2xclk);
+
+	return ret;
+}
+
+static const struct of_device_id qcom_ebi2_of_match[] = {
+	{ .compatible = "qcom,msm8660-ebi2", },
+	{ .compatible = "qcom,apq8060-ebi2", },
+	{ }
+};
+
+static struct platform_driver qcom_ebi2_driver = {
+	.probe = qcom_ebi2_probe,
+	.driver = {
+		.name = "qcom-ebi2",
+		.of_match_table = qcom_ebi2_of_match,
+	},
+};
+module_platform_driver(qcom_ebi2_driver);
+MODULE_AUTHOR("Linus Walleij <linus.walleij@linaro.org>");
+MODULE_DESCRIPTION("Qualcomm EBI2 driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/bus/tegra-aconnect.c b/drivers/bus/tegra-aconnect.c
index 7e4104b74fa8..084ae286fa23 100644
--- a/drivers/bus/tegra-aconnect.c
+++ b/drivers/bus/tegra-aconnect.c
@@ -15,24 +15,6 @@
 #include <linux/pm_clock.h>
 #include <linux/pm_runtime.h>
 
-static int tegra_aconnect_add_clock(struct device *dev, char *name)
-{
-	struct clk *clk;
-	int ret;
-
-	clk = clk_get(dev, name);
-	if (IS_ERR(clk)) {
-		dev_err(dev, "%s clock not found\n", name);
-		return PTR_ERR(clk);
-	}
-
-	ret = pm_clk_add_clk(dev, clk);
-	if (ret)
-		clk_put(clk);
-
-	return ret;
-}
-
 static int tegra_aconnect_probe(struct platform_device *pdev)
 {
 	int ret;
@@ -44,11 +26,11 @@ static int tegra_aconnect_probe(struct platform_device *pdev)
 	if (ret)
 		return ret;
 
-	ret = tegra_aconnect_add_clock(&pdev->dev, "ape");
+	ret = of_pm_clk_add_clk(&pdev->dev, "ape");
 	if (ret)
 		goto clk_destroy;
 
-	ret = tegra_aconnect_add_clock(&pdev->dev, "apb2ape");
+	ret = of_pm_clk_add_clk(&pdev->dev, "apb2ape");
 	if (ret)
 		goto clk_destroy;
 
diff --git a/drivers/char/bfin-otp.c b/drivers/char/bfin-otp.c
index 44660f1c4849..35d46da754d7 100644
--- a/drivers/char/bfin-otp.c
+++ b/drivers/char/bfin-otp.c
@@ -230,45 +230,7 @@ static struct miscdevice bfin_otp_misc_device = {
 	.name     = DRIVER_NAME,
 	.fops     = &bfin_otp_fops,
 };
-
-/**
- *	bfin_otp_init - Initialize module
- *
- *	Registers the device and notifier handler. Actual device
- *	initialization is handled by bfin_otp_open().
- */
-static int __init bfin_otp_init(void)
-{
-	int ret;
-
-	stampit();
-
-	ret = misc_register(&bfin_otp_misc_device);
-	if (ret) {
-		pr_init(KERN_ERR PFX "unable to register a misc device\n");
-		return ret;
-	}
-
-	pr_init(KERN_INFO PFX "initialized\n");
-
-	return 0;
-}
-
-/**
- *	bfin_otp_exit - Deinitialize module
- *
- *	Unregisters the device and notifier handler. Actual device
- *	deinitialization is handled by bfin_otp_close().
- */
-static void __exit bfin_otp_exit(void)
-{
-	stampit();
-
-	misc_deregister(&bfin_otp_misc_device);
-}
-
-module_init(bfin_otp_init);
-module_exit(bfin_otp_exit);
+module_misc_device(bfin_otp_misc_device);
 
 MODULE_AUTHOR("Mike Frysinger <vapier@gentoo.org>");
 MODULE_DESCRIPTION("Blackfin OTP Memory Interface");
diff --git a/drivers/char/hw_random/Kconfig b/drivers/char/hw_random/Kconfig
index 8c0770bf8881..200dab5136a7 100644
--- a/drivers/char/hw_random/Kconfig
+++ b/drivers/char/hw_random/Kconfig
@@ -410,6 +410,19 @@ config HW_RANDOM_MESON
 
 	  If unsure, say Y.
 
+config HW_RANDOM_CAVIUM
+       tristate "Cavium ThunderX Random Number Generator support"
+       depends on HW_RANDOM && PCI && (ARM64 || (COMPILE_TEST && 64BIT))
+       default HW_RANDOM
+       ---help---
+         This driver provides kernel-side support for the Random Number
+         Generator hardware found on Cavium SoCs.
+
+         To compile this driver as a module, choose M here: the
+         module will be called cavium_rng.
+
+         If unsure, say Y.
+
 endif # HW_RANDOM
 
 config UML_RANDOM
diff --git a/drivers/char/hw_random/Makefile b/drivers/char/hw_random/Makefile
index 04bb0b03356f..5f52b1e4e7be 100644
--- a/drivers/char/hw_random/Makefile
+++ b/drivers/char/hw_random/Makefile
@@ -35,3 +35,4 @@ obj-$(CONFIG_HW_RANDOM_XGENE) += xgene-rng.o
 obj-$(CONFIG_HW_RANDOM_STM32) += stm32-rng.o
 obj-$(CONFIG_HW_RANDOM_PIC32) += pic32-rng.o
 obj-$(CONFIG_HW_RANDOM_MESON) += meson-rng.o
+obj-$(CONFIG_HW_RANDOM_CAVIUM) += cavium-rng.o cavium-rng-vf.o
diff --git a/drivers/char/hw_random/amd-rng.c b/drivers/char/hw_random/amd-rng.c
index 48f6a83cdd61..4a99ac756f08 100644
--- a/drivers/char/hw_random/amd-rng.c
+++ b/drivers/char/hw_random/amd-rng.c
@@ -24,16 +24,18 @@
  * warranty of any kind, whether express or implied.
  */
 
-#include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/hw_random.h>
 #include <linux/kernel.h>
+#include <linux/module.h>
 #include <linux/pci.h>
-#include <linux/hw_random.h>
-#include <linux/delay.h>
-#include <asm/io.h>
 
+#define DRV_NAME "AMD768-HWRNG"
 
-#define PFX	KBUILD_MODNAME ": "
-
+#define RNGDATA		0x00
+#define RNGDONE		0x04
+#define PMBASE_OFFSET	0xF0
+#define PMBASE_SIZE	8
 
 /*
  * Data for PCI driver interface
@@ -50,72 +52,84 @@ static const struct pci_device_id pci_tbl[] = {
 };
 MODULE_DEVICE_TABLE(pci, pci_tbl);
 
-static struct pci_dev *amd_pdev;
-
+struct amd768_priv {
+	void __iomem *iobase;
+	struct pci_dev *pcidev;
+};
 
-static int amd_rng_data_present(struct hwrng *rng, int wait)
+static int amd_rng_read(struct hwrng *rng, void *buf, size_t max, bool wait)
 {
-	u32 pmbase = (u32)rng->priv;
-	int data, i;
-
-	for (i = 0; i < 20; i++) {
-		data = !!(inl(pmbase + 0xF4) & 1);
-		if (data || !wait)
-			break;
-		udelay(10);
+	u32 *data = buf;
+	struct amd768_priv *priv = (struct amd768_priv *)rng->priv;
+	size_t read = 0;
+	/* We will wait at maximum one time per read */
+	int timeout = max / 4 + 1;
+
+	/*
+	 * RNG data is available when RNGDONE is set to 1
+	 * New random numbers are generated approximately 128 microseconds
+	 * after RNGDATA is read
+	 */
+	while (read < max) {
+		if (ioread32(priv->iobase + RNGDONE) == 0) {
+			if (wait) {
+				/* Delay given by datasheet */
+				usleep_range(128, 196);
+				if (timeout-- == 0)
+					return read;
+			} else {
+				return 0;
+			}
+		} else {
+			*data = ioread32(priv->iobase + RNGDATA);
+			data++;
+			read += 4;
+		}
 	}
-	return data;
-}
 
-static int amd_rng_data_read(struct hwrng *rng, u32 *data)
-{
-	u32 pmbase = (u32)rng->priv;
-
-	*data = inl(pmbase + 0xF0);
-
-	return 4;
+	return read;
 }
 
 static int amd_rng_init(struct hwrng *rng)
 {
+	struct amd768_priv *priv = (struct amd768_priv *)rng->priv;
 	u8 rnen;
 
-	pci_read_config_byte(amd_pdev, 0x40, &rnen);
-	rnen |= (1 << 7);	/* RNG on */
-	pci_write_config_byte(amd_pdev, 0x40, rnen);
+	pci_read_config_byte(priv->pcidev, 0x40, &rnen);
+	rnen |= BIT(7);	/* RNG on */
+	pci_write_config_byte(priv->pcidev, 0x40, rnen);
 
-	pci_read_config_byte(amd_pdev, 0x41, &rnen);
-	rnen |= (1 << 7);	/* PMIO enable */
-	pci_write_config_byte(amd_pdev, 0x41, rnen);
+	pci_read_config_byte(priv->pcidev, 0x41, &rnen);
+	rnen |= BIT(7);	/* PMIO enable */
+	pci_write_config_byte(priv->pcidev, 0x41, rnen);
 
 	return 0;
 }
 
 static void amd_rng_cleanup(struct hwrng *rng)
 {
+	struct amd768_priv *priv = (struct amd768_priv *)rng->priv;
 	u8 rnen;
 
-	pci_read_config_byte(amd_pdev, 0x40, &rnen);
-	rnen &= ~(1 << 7);	/* RNG off */
-	pci_write_config_byte(amd_pdev, 0x40, rnen);
+	pci_read_config_byte(priv->pcidev, 0x40, &rnen);
+	rnen &= ~BIT(7);	/* RNG off */
+	pci_write_config_byte(priv->pcidev, 0x40, rnen);
 }
 
-
 static struct hwrng amd_rng = {
 	.name		= "amd",
 	.init		= amd_rng_init,
 	.cleanup	= amd_rng_cleanup,
-	.data_present	= amd_rng_data_present,
-	.data_read	= amd_rng_data_read,
+	.read		= amd_rng_read,
 };
 
-
 static int __init mod_init(void)
 {
 	int err = -ENODEV;
 	struct pci_dev *pdev = NULL;
 	const struct pci_device_id *ent;
 	u32 pmbase;
+	struct amd768_priv *priv;
 
 	for_each_pci_dev(pdev) {
 		ent = pci_match_id(pci_tbl, pdev);
@@ -123,42 +137,44 @@ static int __init mod_init(void)
 			goto found;
 	}
 	/* Device not found. */
-	goto out;
+	return -ENODEV;
 
 found:
 	err = pci_read_config_dword(pdev, 0x58, &pmbase);
 	if (err)
-		goto out;
-	err = -EIO;
+		return err;
+
 	pmbase &= 0x0000FF00;
 	if (pmbase == 0)
-		goto out;
-	if (!request_region(pmbase + 0xF0, 8, "AMD HWRNG")) {
-		dev_err(&pdev->dev, "AMD HWRNG region 0x%x already in use!\n",
+		return -EIO;
+
+	priv = devm_kzalloc(&pdev->dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	if (!devm_request_region(&pdev->dev, pmbase + PMBASE_OFFSET,
+				PMBASE_SIZE, DRV_NAME)) {
+		dev_err(&pdev->dev, DRV_NAME " region 0x%x already in use!\n",
 			pmbase + 0xF0);
-		err = -EBUSY;
-		goto out;
+		return -EBUSY;
 	}
-	amd_rng.priv = (unsigned long)pmbase;
-	amd_pdev = pdev;
-
-	pr_info("AMD768 RNG detected\n");
-	err = hwrng_register(&amd_rng);
-	if (err) {
-		pr_err(PFX "RNG registering failed (%d)\n",
-		       err);
-		release_region(pmbase + 0xF0, 8);
-		goto out;
+
+	priv->iobase = devm_ioport_map(&pdev->dev, pmbase + PMBASE_OFFSET,
+			PMBASE_SIZE);
+	if (!priv->iobase) {
+		pr_err(DRV_NAME "Cannot map ioport\n");
+		return -ENOMEM;
 	}
-out:
-	return err;
+
+	amd_rng.priv = (unsigned long)priv;
+	priv->pcidev = pdev;
+
+	pr_info(DRV_NAME " detected\n");
+	return devm_hwrng_register(&pdev->dev, &amd_rng);
 }
 
 static void __exit mod_exit(void)
 {
-	u32 pmbase = (unsigned long)amd_rng.priv;
-	release_region(pmbase + 0xF0, 8);
-	hwrng_unregister(&amd_rng);
 }
 
 module_init(mod_init);
diff --git a/drivers/char/hw_random/bcm2835-rng.c b/drivers/char/hw_random/bcm2835-rng.c
index af2149273fe0..574211a49549 100644
--- a/drivers/char/hw_random/bcm2835-rng.c
+++ b/drivers/char/hw_random/bcm2835-rng.c
@@ -92,9 +92,10 @@ static int bcm2835_rng_probe(struct platform_device *pdev)
 	bcm2835_rng_ops.priv = (unsigned long)rng_base;
 
 	rng_id = of_match_node(bcm2835_rng_of_match, np);
-	if (!rng_id)
+	if (!rng_id) {
+		iounmap(rng_base);
 		return -EINVAL;
-
+	}
 	/* Check for rng init function, execute it */
 	rng_setup = rng_id->data;
 	if (rng_setup)
diff --git a/drivers/char/hw_random/cavium-rng-vf.c b/drivers/char/hw_random/cavium-rng-vf.c
new file mode 100644
index 000000000000..066ae0e78d63
--- /dev/null
+++ b/drivers/char/hw_random/cavium-rng-vf.c
@@ -0,0 +1,99 @@
+/*
+ * Hardware Random Number Generator support for Cavium, Inc.
+ * Thunder processor family.
+ *
+ * This file is subject to the terms and conditions of the GNU General Public
+ * License.  See the file "COPYING" in the main directory of this archive
+ * for more details.
+ *
+ * Copyright (C) 2016 Cavium, Inc.
+ */
+
+#include <linux/hw_random.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/pci.h>
+#include <linux/pci_ids.h>
+
+struct cavium_rng {
+	struct hwrng ops;
+	void __iomem *result;
+};
+
+/* Read data from the RNG unit */
+static int cavium_rng_read(struct hwrng *rng, void *dat, size_t max, bool wait)
+{
+	struct cavium_rng *p = container_of(rng, struct cavium_rng, ops);
+	unsigned int size = max;
+
+	while (size >= 8) {
+		*((u64 *)dat) = readq(p->result);
+		size -= 8;
+		dat += 8;
+	}
+	while (size > 0) {
+		*((u8 *)dat) = readb(p->result);
+		size--;
+		dat++;
+	}
+	return max;
+}
+
+/* Map Cavium RNG to an HWRNG object */
+static int cavium_rng_probe_vf(struct	pci_dev		*pdev,
+			 const struct	pci_device_id	*id)
+{
+	struct	cavium_rng *rng;
+	int	ret;
+
+	rng = devm_kzalloc(&pdev->dev, sizeof(*rng), GFP_KERNEL);
+	if (!rng)
+		return -ENOMEM;
+
+	/* Map the RNG result */
+	rng->result = pcim_iomap(pdev, 0, 0);
+	if (!rng->result) {
+		dev_err(&pdev->dev, "Error iomap failed retrieving result.\n");
+		return -ENOMEM;
+	}
+
+	rng->ops.name    = "cavium rng";
+	rng->ops.read    = cavium_rng_read;
+	rng->ops.quality = 1000;
+
+	pci_set_drvdata(pdev, rng);
+
+	ret = hwrng_register(&rng->ops);
+	if (ret) {
+		dev_err(&pdev->dev, "Error registering device as HWRNG.\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+/* Remove the VF */
+void  cavium_rng_remove_vf(struct pci_dev *pdev)
+{
+	struct cavium_rng *rng;
+
+	rng = pci_get_drvdata(pdev);
+	hwrng_unregister(&rng->ops);
+}
+
+static const struct pci_device_id cavium_rng_vf_id_table[] = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_CAVIUM, 0xa033), 0, 0, 0},
+	{0,},
+};
+MODULE_DEVICE_TABLE(pci, cavium_rng_vf_id_table);
+
+static struct pci_driver cavium_rng_vf_driver = {
+	.name		= "cavium_rng_vf",
+	.id_table	= cavium_rng_vf_id_table,
+	.probe		= cavium_rng_probe_vf,
+	.remove		= cavium_rng_remove_vf,
+};
+module_pci_driver(cavium_rng_vf_driver);
+
+MODULE_AUTHOR("Omer Khaliq <okhaliq@caviumnetworks.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/char/hw_random/cavium-rng.c b/drivers/char/hw_random/cavium-rng.c
new file mode 100644
index 000000000000..a944e0a47f42
--- /dev/null
+++ b/drivers/char/hw_random/cavium-rng.c
@@ -0,0 +1,94 @@
+/*
+ * Hardware Random Number Generator support for Cavium Inc.
+ * Thunder processor family.
+ *
+ * This file is subject to the terms and conditions of the GNU General Public
+ * License.  See the file "COPYING" in the main directory of this archive
+ * for more details.
+ *
+ * Copyright (C) 2016 Cavium, Inc.
+ */
+
+#include <linux/hw_random.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/pci.h>
+#include <linux/pci_ids.h>
+
+#define THUNDERX_RNM_ENT_EN     0x1
+#define THUNDERX_RNM_RNG_EN     0x2
+
+struct cavium_rng_pf {
+	void __iomem *control_status;
+};
+
+/* Enable the RNG hardware and activate the VF */
+static int cavium_rng_probe(struct pci_dev *pdev,
+			const struct pci_device_id *id)
+{
+	struct	cavium_rng_pf *rng;
+	int	iov_err;
+
+	rng = devm_kzalloc(&pdev->dev, sizeof(*rng), GFP_KERNEL);
+	if (!rng)
+		return -ENOMEM;
+
+	/*Map the RNG control */
+	rng->control_status = pcim_iomap(pdev, 0, 0);
+	if (!rng->control_status) {
+		dev_err(&pdev->dev,
+			"Error iomap failed retrieving control_status.\n");
+		return -ENOMEM;
+	}
+
+	/* Enable the RNG hardware and entropy source */
+	writeq(THUNDERX_RNM_RNG_EN | THUNDERX_RNM_ENT_EN,
+		rng->control_status);
+
+	pci_set_drvdata(pdev, rng);
+
+	/* Enable the Cavium RNG as a VF */
+	iov_err = pci_enable_sriov(pdev, 1);
+	if (iov_err != 0) {
+		/* Disable the RNG hardware and entropy source */
+		writeq(0, rng->control_status);
+		dev_err(&pdev->dev,
+			"Error initializing RNG virtual function,(%i).\n",
+			iov_err);
+		return iov_err;
+	}
+
+	return 0;
+}
+
+/* Disable VF and RNG Hardware */
+void  cavium_rng_remove(struct pci_dev *pdev)
+{
+	struct cavium_rng_pf *rng;
+
+	rng = pci_get_drvdata(pdev);
+
+	/* Remove the VF */
+	pci_disable_sriov(pdev);
+
+	/* Disable the RNG hardware and entropy source */
+	writeq(0, rng->control_status);
+}
+
+static const struct pci_device_id cavium_rng_pf_id_table[] = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_CAVIUM, 0xa018), 0, 0, 0}, /* Thunder RNM */
+	{0,},
+};
+
+MODULE_DEVICE_TABLE(pci, cavium_rng_pf_id_table);
+
+static struct pci_driver cavium_rng_pf_driver = {
+	.name		= "cavium_rng_pf",
+	.id_table	= cavium_rng_pf_id_table,
+	.probe		= cavium_rng_probe,
+	.remove		= cavium_rng_remove,
+};
+
+module_pci_driver(cavium_rng_pf_driver);
+MODULE_AUTHOR("Omer Khaliq <okhaliq@caviumnetworks.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/char/hw_random/core.c b/drivers/char/hw_random/core.c
index 9203f2d130c0..482794526e8c 100644
--- a/drivers/char/hw_random/core.c
+++ b/drivers/char/hw_random/core.c
@@ -449,22 +449,6 @@ int hwrng_register(struct hwrng *rng)
 		goto out;
 
 	mutex_lock(&rng_mutex);
-
-	/* kmalloc makes this safe for virt_to_page() in virtio_rng.c */
-	err = -ENOMEM;
-	if (!rng_buffer) {
-		rng_buffer = kmalloc(rng_buffer_size(), GFP_KERNEL);
-		if (!rng_buffer)
-			goto out_unlock;
-	}
-	if (!rng_fillbuf) {
-		rng_fillbuf = kmalloc(rng_buffer_size(), GFP_KERNEL);
-		if (!rng_fillbuf) {
-			kfree(rng_buffer);
-			goto out_unlock;
-		}
-	}
-
 	/* Must not register two RNGs with the same name. */
 	err = -EEXIST;
 	list_for_each_entry(tmp, &rng_list, list) {
@@ -573,7 +557,26 @@ EXPORT_SYMBOL_GPL(devm_hwrng_unregister);
 
 static int __init hwrng_modinit(void)
 {
-	return register_miscdev();
+	int ret = -ENOMEM;
+
+	/* kmalloc makes this safe for virt_to_page() in virtio_rng.c */
+	rng_buffer = kmalloc(rng_buffer_size(), GFP_KERNEL);
+	if (!rng_buffer)
+		return -ENOMEM;
+
+	rng_fillbuf = kmalloc(rng_buffer_size(), GFP_KERNEL);
+	if (!rng_fillbuf) {
+		kfree(rng_buffer);
+		return -ENOMEM;
+	}
+
+	ret = register_miscdev();
+	if (ret) {
+		kfree(rng_fillbuf);
+		kfree(rng_buffer);
+	}
+
+	return ret;
 }
 
 static void __exit hwrng_modexit(void)
diff --git a/drivers/char/hw_random/geode-rng.c b/drivers/char/hw_random/geode-rng.c
index 0d0579fe465e..e7a245942029 100644
--- a/drivers/char/hw_random/geode-rng.c
+++ b/drivers/char/hw_random/geode-rng.c
@@ -24,15 +24,12 @@
  * warranty of any kind, whether express or implied.
  */
 
-#include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/hw_random.h>
+#include <linux/io.h>
 #include <linux/kernel.h>
+#include <linux/module.h>
 #include <linux/pci.h>
-#include <linux/hw_random.h>
-#include <linux/delay.h>
-#include <asm/io.h>
-
-
-#define PFX	KBUILD_MODNAME ": "
 
 #define GEODE_RNG_DATA_REG   0x50
 #define GEODE_RNG_STATUS_REG 0x54
@@ -85,7 +82,6 @@ static struct hwrng geode_rng = {
 
 static int __init mod_init(void)
 {
-	int err = -ENODEV;
 	struct pci_dev *pdev = NULL;
 	const struct pci_device_id *ent;
 	void __iomem *mem;
@@ -93,43 +89,27 @@ static int __init mod_init(void)
 
 	for_each_pci_dev(pdev) {
 		ent = pci_match_id(pci_tbl, pdev);
-		if (ent)
-			goto found;
-	}
-	/* Device not found. */
-	goto out;
-
-found:
-	rng_base = pci_resource_start(pdev, 0);
-	if (rng_base == 0)
-		goto out;
-	err = -ENOMEM;
-	mem = ioremap(rng_base, 0x58);
-	if (!mem)
-		goto out;
-	geode_rng.priv = (unsigned long)mem;
-
-	pr_info("AMD Geode RNG detected\n");
-	err = hwrng_register(&geode_rng);
-	if (err) {
-		pr_err(PFX "RNG registering failed (%d)\n",
-		       err);
-		goto err_unmap;
+		if (ent) {
+			rng_base = pci_resource_start(pdev, 0);
+			if (rng_base == 0)
+				return -ENODEV;
+
+			mem = devm_ioremap(&pdev->dev, rng_base, 0x58);
+			if (!mem)
+				return -ENOMEM;
+			geode_rng.priv = (unsigned long)mem;
+
+			pr_info("AMD Geode RNG detected\n");
+			return devm_hwrng_register(&pdev->dev, &geode_rng);
+		}
 	}
-out:
-	return err;
 
-err_unmap:
-	iounmap(mem);
-	goto out;
+	/* Device not found. */
+	return -ENODEV;
 }
 
 static void __exit mod_exit(void)
 {
-	void __iomem *mem = (void __iomem *)geode_rng.priv;
-
-	hwrng_unregister(&geode_rng);
-	iounmap(mem);
 }
 
 module_init(mod_init);
diff --git a/drivers/char/hw_random/meson-rng.c b/drivers/char/hw_random/meson-rng.c
index 0cfd81bcaeac..58bef39f7286 100644
--- a/drivers/char/hw_random/meson-rng.c
+++ b/drivers/char/hw_random/meson-rng.c
@@ -76,9 +76,6 @@ static int meson_rng_read(struct hwrng *rng, void *buf, size_t max, bool wait)
 	struct meson_rng_data *data =
 			container_of(rng, struct meson_rng_data, rng);
 
-	if (max < sizeof(u32))
-		return 0;
-
 	*(u32 *)buf = readl_relaxed(data->base + RNG_DATA);
 
 	return sizeof(u32);
diff --git a/drivers/char/hw_random/omap-rng.c b/drivers/char/hw_random/omap-rng.c
index 01d4be2c354b..f5c26a5f6875 100644
--- a/drivers/char/hw_random/omap-rng.c
+++ b/drivers/char/hw_random/omap-rng.c
@@ -385,7 +385,7 @@ static int omap_rng_probe(struct platform_device *pdev)
 
 	pm_runtime_enable(&pdev->dev);
 	ret = pm_runtime_get_sync(&pdev->dev);
-	if (ret) {
+	if (ret < 0) {
 		dev_err(&pdev->dev, "Failed to runtime_get device: %d\n", ret);
 		pm_runtime_put_noidle(&pdev->dev);
 		goto err_ioremap;
@@ -443,7 +443,7 @@ static int __maybe_unused omap_rng_resume(struct device *dev)
 	int ret;
 
 	ret = pm_runtime_get_sync(dev);
-	if (ret) {
+	if (ret < 0) {
 		dev_err(dev, "Failed to runtime_get device: %d\n", ret);
 		pm_runtime_put_noidle(dev);
 		return ret;
diff --git a/drivers/char/hw_random/omap3-rom-rng.c b/drivers/char/hw_random/omap3-rom-rng.c
index 8da14f1a1f56..37a58d78aab3 100644
--- a/drivers/char/hw_random/omap3-rom-rng.c
+++ b/drivers/char/hw_random/omap3-rom-rng.c
@@ -71,12 +71,7 @@ static int omap3_rom_rng_get_random(void *buf, unsigned int count)
 	return 0;
 }
 
-static int omap3_rom_rng_data_present(struct hwrng *rng, int wait)
-{
-	return 1;
-}
-
-static int omap3_rom_rng_data_read(struct hwrng *rng, u32 *data)
+static int omap3_rom_rng_read(struct hwrng *rng, void *data, size_t max, bool w)
 {
 	int r;
 
@@ -88,8 +83,7 @@ static int omap3_rom_rng_data_read(struct hwrng *rng, u32 *data)
 
 static struct hwrng omap3_rom_rng_ops = {
 	.name		= "omap3-rom",
-	.data_present	= omap3_rom_rng_data_present,
-	.data_read	= omap3_rom_rng_data_read,
+	.read		= omap3_rom_rng_read,
 };
 
 static int omap3_rom_rng_probe(struct platform_device *pdev)
diff --git a/drivers/char/hw_random/pasemi-rng.c b/drivers/char/hw_random/pasemi-rng.c
index 699b7259f5d7..545df485bcc4 100644
--- a/drivers/char/hw_random/pasemi-rng.c
+++ b/drivers/char/hw_random/pasemi-rng.c
@@ -26,7 +26,7 @@
 #include <linux/delay.h>
 #include <linux/of_address.h>
 #include <linux/of_platform.h>
-#include <asm/io.h>
+#include <linux/io.h>
 
 #define SDCRNG_CTL_REG			0x00
 #define   SDCRNG_CTL_FVLD_M		0x0000f000
@@ -95,42 +95,20 @@ static struct hwrng pasemi_rng = {
 	.data_read	= pasemi_rng_data_read,
 };
 
-static int rng_probe(struct platform_device *ofdev)
+static int rng_probe(struct platform_device *pdev)
 {
 	void __iomem *rng_regs;
-	struct device_node *rng_np = ofdev->dev.of_node;
-	struct resource res;
-	int err = 0;
+	struct resource *res;
 
-	err = of_address_to_resource(rng_np, 0, &res);
-	if (err)
-		return -ENODEV;
-
-	rng_regs = ioremap(res.start, 0x100);
-
-	if (!rng_regs)
-		return -ENOMEM;
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	rng_regs = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(rng_regs))
+		return PTR_ERR(rng_regs);
 
 	pasemi_rng.priv = (unsigned long)rng_regs;
 
 	pr_info("Registering PA Semi RNG\n");
-
-	err = hwrng_register(&pasemi_rng);
-
-	if (err)
-		iounmap(rng_regs);
-
-	return err;
-}
-
-static int rng_remove(struct platform_device *dev)
-{
-	void __iomem *rng_regs = (void __iomem *)pasemi_rng.priv;
-
-	hwrng_unregister(&pasemi_rng);
-	iounmap(rng_regs);
-
-	return 0;
+	return devm_hwrng_register(&pdev->dev, &pasemi_rng);
 }
 
 static const struct of_device_id rng_match[] = {
@@ -146,7 +124,6 @@ static struct platform_driver rng_driver = {
 		.of_match_table = rng_match,
 	},
 	.probe		= rng_probe,
-	.remove		= rng_remove,
 };
 
 module_platform_driver(rng_driver);
diff --git a/drivers/char/hw_random/pic32-rng.c b/drivers/char/hw_random/pic32-rng.c
index 108897bea2d0..11dc9b7c09ce 100644
--- a/drivers/char/hw_random/pic32-rng.c
+++ b/drivers/char/hw_random/pic32-rng.c
@@ -143,7 +143,6 @@ static struct platform_driver pic32_rng_driver = {
 	.remove		= pic32_rng_remove,
 	.driver		= {
 		.name	= "pic32-rng",
-		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(pic32_rng_of_match),
 	},
 };
diff --git a/drivers/char/hw_random/st-rng.c b/drivers/char/hw_random/st-rng.c
index 1d35363d23c5..938ec10e733d 100644
--- a/drivers/char/hw_random/st-rng.c
+++ b/drivers/char/hw_random/st-rng.c
@@ -54,9 +54,6 @@ static int st_rng_read(struct hwrng *rng, void *data, size_t max, bool wait)
 	u32 status;
 	int i;
 
-	if (max < sizeof(u16))
-		return -EINVAL;
-
 	/* Wait until FIFO is full - max 4uS*/
 	for (i = 0; i < ST_RNG_FILL_FIFO_TIMEOUT; i++) {
 		status = readl_relaxed(ddata->base + ST_RNG_STATUS_REG);
@@ -111,6 +108,7 @@ static int st_rng_probe(struct platform_device *pdev)
 	ret = hwrng_register(&ddata->ops);
 	if (ret) {
 		dev_err(&pdev->dev, "Failed to register HW RNG\n");
+		clk_disable_unprepare(clk);
 		return ret;
 	}
 
diff --git a/drivers/char/hw_random/tx4939-rng.c b/drivers/char/hw_random/tx4939-rng.c
index a7b694913416..1093583b579c 100644
--- a/drivers/char/hw_random/tx4939-rng.c
+++ b/drivers/char/hw_random/tx4939-rng.c
@@ -144,22 +144,13 @@ static int __init tx4939_rng_probe(struct platform_device *dev)
 	}
 
 	platform_set_drvdata(dev, rngdev);
-	return hwrng_register(&rngdev->rng);
-}
-
-static int __exit tx4939_rng_remove(struct platform_device *dev)
-{
-	struct tx4939_rng *rngdev = platform_get_drvdata(dev);
-
-	hwrng_unregister(&rngdev->rng);
-	return 0;
+	return devm_hwrng_register(&dev->dev, &rngdev->rng);
 }
 
 static struct platform_driver tx4939_rng_driver = {
 	.driver		= {
 		.name	= "tx4939-rng",
 	},
-	.remove = tx4939_rng_remove,
 };
 
 module_platform_driver_probe(tx4939_rng_driver, tx4939_rng_probe);
diff --git a/drivers/char/ipmi/Kconfig b/drivers/char/ipmi/Kconfig
index 5a9350b1069a..7f816655cbbf 100644
--- a/drivers/char/ipmi/Kconfig
+++ b/drivers/char/ipmi/Kconfig
@@ -76,3 +76,11 @@ config IPMI_POWEROFF
 	 the IPMI management controller is capable of this.
 
 endif # IPMI_HANDLER
+
+config ASPEED_BT_IPMI_BMC
+	depends on ARCH_ASPEED
+	tristate "BT IPMI bmc driver"
+	help
+	  Provides a driver for the BT (Block Transfer) IPMI interface
+	  found on Aspeed SOCs (AST2400 and AST2500). The driver
+	  implements the BMC side of the BT interface.
diff --git a/drivers/char/ipmi/Makefile b/drivers/char/ipmi/Makefile
index f3ffde1f5f1f..0d98cd91def1 100644
--- a/drivers/char/ipmi/Makefile
+++ b/drivers/char/ipmi/Makefile
@@ -11,3 +11,4 @@ obj-$(CONFIG_IPMI_SSIF) += ipmi_ssif.o
 obj-$(CONFIG_IPMI_POWERNV) += ipmi_powernv.o
 obj-$(CONFIG_IPMI_WATCHDOG) += ipmi_watchdog.o
 obj-$(CONFIG_IPMI_POWEROFF) += ipmi_poweroff.o
+obj-$(CONFIG_ASPEED_BT_IPMI_BMC) += bt-bmc.o
diff --git a/drivers/char/ipmi/bt-bmc.c b/drivers/char/ipmi/bt-bmc.c
new file mode 100644
index 000000000000..b49e61320952
--- /dev/null
+++ b/drivers/char/ipmi/bt-bmc.c
@@ -0,0 +1,505 @@
+/*
+ * Copyright (c) 2015-2016, IBM 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.
+ */
+
+#include <linux/atomic.h>
+#include <linux/bt-bmc.h>
+#include <linux/errno.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/miscdevice.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/poll.h>
+#include <linux/sched.h>
+#include <linux/timer.h>
+
+/*
+ * This is a BMC device used to communicate to the host
+ */
+#define DEVICE_NAME	"ipmi-bt-host"
+
+#define BT_IO_BASE	0xe4
+#define BT_IRQ		10
+
+#define BT_CR0		0x0
+#define   BT_CR0_IO_BASE		16
+#define   BT_CR0_IRQ			12
+#define   BT_CR0_EN_CLR_SLV_RDP		0x8
+#define   BT_CR0_EN_CLR_SLV_WRP		0x4
+#define   BT_CR0_ENABLE_IBT		0x1
+#define BT_CR1		0x4
+#define   BT_CR1_IRQ_H2B	0x01
+#define   BT_CR1_IRQ_HBUSY	0x40
+#define BT_CR2		0x8
+#define   BT_CR2_IRQ_H2B	0x01
+#define   BT_CR2_IRQ_HBUSY	0x40
+#define BT_CR3		0xc
+#define BT_CTRL		0x10
+#define   BT_CTRL_B_BUSY		0x80
+#define   BT_CTRL_H_BUSY		0x40
+#define   BT_CTRL_OEM0			0x20
+#define   BT_CTRL_SMS_ATN		0x10
+#define   BT_CTRL_B2H_ATN		0x08
+#define   BT_CTRL_H2B_ATN		0x04
+#define   BT_CTRL_CLR_RD_PTR		0x02
+#define   BT_CTRL_CLR_WR_PTR		0x01
+#define BT_BMC2HOST	0x14
+#define BT_INTMASK	0x18
+#define   BT_INTMASK_B2H_IRQEN		0x01
+#define   BT_INTMASK_B2H_IRQ		0x02
+#define   BT_INTMASK_BMC_HWRST		0x80
+
+#define BT_BMC_BUFFER_SIZE 256
+
+struct bt_bmc {
+	struct device		dev;
+	struct miscdevice	miscdev;
+	void __iomem		*base;
+	int			irq;
+	wait_queue_head_t	queue;
+	struct timer_list	poll_timer;
+	struct mutex		mutex;
+};
+
+static atomic_t open_count = ATOMIC_INIT(0);
+
+static u8 bt_inb(struct bt_bmc *bt_bmc, int reg)
+{
+	return ioread8(bt_bmc->base + reg);
+}
+
+static void bt_outb(struct bt_bmc *bt_bmc, u8 data, int reg)
+{
+	iowrite8(data, bt_bmc->base + reg);
+}
+
+static void clr_rd_ptr(struct bt_bmc *bt_bmc)
+{
+	bt_outb(bt_bmc, BT_CTRL_CLR_RD_PTR, BT_CTRL);
+}
+
+static void clr_wr_ptr(struct bt_bmc *bt_bmc)
+{
+	bt_outb(bt_bmc, BT_CTRL_CLR_WR_PTR, BT_CTRL);
+}
+
+static void clr_h2b_atn(struct bt_bmc *bt_bmc)
+{
+	bt_outb(bt_bmc, BT_CTRL_H2B_ATN, BT_CTRL);
+}
+
+static void set_b_busy(struct bt_bmc *bt_bmc)
+{
+	if (!(bt_inb(bt_bmc, BT_CTRL) & BT_CTRL_B_BUSY))
+		bt_outb(bt_bmc, BT_CTRL_B_BUSY, BT_CTRL);
+}
+
+static void clr_b_busy(struct bt_bmc *bt_bmc)
+{
+	if (bt_inb(bt_bmc, BT_CTRL) & BT_CTRL_B_BUSY)
+		bt_outb(bt_bmc, BT_CTRL_B_BUSY, BT_CTRL);
+}
+
+static void set_b2h_atn(struct bt_bmc *bt_bmc)
+{
+	bt_outb(bt_bmc, BT_CTRL_B2H_ATN, BT_CTRL);
+}
+
+static u8 bt_read(struct bt_bmc *bt_bmc)
+{
+	return bt_inb(bt_bmc, BT_BMC2HOST);
+}
+
+static ssize_t bt_readn(struct bt_bmc *bt_bmc, u8 *buf, size_t n)
+{
+	int i;
+
+	for (i = 0; i < n; i++)
+		buf[i] = bt_read(bt_bmc);
+	return n;
+}
+
+static void bt_write(struct bt_bmc *bt_bmc, u8 c)
+{
+	bt_outb(bt_bmc, c, BT_BMC2HOST);
+}
+
+static ssize_t bt_writen(struct bt_bmc *bt_bmc, u8 *buf, size_t n)
+{
+	int i;
+
+	for (i = 0; i < n; i++)
+		bt_write(bt_bmc, buf[i]);
+	return n;
+}
+
+static void set_sms_atn(struct bt_bmc *bt_bmc)
+{
+	bt_outb(bt_bmc, BT_CTRL_SMS_ATN, BT_CTRL);
+}
+
+static struct bt_bmc *file_bt_bmc(struct file *file)
+{
+	return container_of(file->private_data, struct bt_bmc, miscdev);
+}
+
+static int bt_bmc_open(struct inode *inode, struct file *file)
+{
+	struct bt_bmc *bt_bmc = file_bt_bmc(file);
+
+	if (atomic_inc_return(&open_count) == 1) {
+		clr_b_busy(bt_bmc);
+		return 0;
+	}
+
+	atomic_dec(&open_count);
+	return -EBUSY;
+}
+
+/*
+ * The BT (Block Transfer) interface means that entire messages are
+ * buffered by the host before a notification is sent to the BMC that
+ * there is data to be read. The first byte is the length and the
+ * message data follows. The read operation just tries to capture the
+ * whole before returning it to userspace.
+ *
+ * BT Message format :
+ *
+ *    Byte 1  Byte 2     Byte 3  Byte 4  Byte 5:N
+ *    Length  NetFn/LUN  Seq     Cmd     Data
+ *
+ */
+static ssize_t bt_bmc_read(struct file *file, char __user *buf,
+			   size_t count, loff_t *ppos)
+{
+	struct bt_bmc *bt_bmc = file_bt_bmc(file);
+	u8 len;
+	int len_byte = 1;
+	u8 kbuffer[BT_BMC_BUFFER_SIZE];
+	ssize_t ret = 0;
+	ssize_t nread;
+
+	if (!access_ok(VERIFY_WRITE, buf, count))
+		return -EFAULT;
+
+	WARN_ON(*ppos);
+
+	if (wait_event_interruptible(bt_bmc->queue,
+				     bt_inb(bt_bmc, BT_CTRL) & BT_CTRL_H2B_ATN))
+		return -ERESTARTSYS;
+
+	mutex_lock(&bt_bmc->mutex);
+
+	if (unlikely(!(bt_inb(bt_bmc, BT_CTRL) & BT_CTRL_H2B_ATN))) {
+		ret = -EIO;
+		goto out_unlock;
+	}
+
+	set_b_busy(bt_bmc);
+	clr_h2b_atn(bt_bmc);
+	clr_rd_ptr(bt_bmc);
+
+	/*
+	 * The BT frames start with the message length, which does not
+	 * include the length byte.
+	 */
+	kbuffer[0] = bt_read(bt_bmc);
+	len = kbuffer[0];
+
+	/* We pass the length back to userspace as well */
+	if (len + 1 > count)
+		len = count - 1;
+
+	while (len) {
+		nread = min_t(ssize_t, len, sizeof(kbuffer) - len_byte);
+
+		bt_readn(bt_bmc, kbuffer + len_byte, nread);
+
+		if (copy_to_user(buf, kbuffer, nread + len_byte)) {
+			ret = -EFAULT;
+			break;
+		}
+		len -= nread;
+		buf += nread + len_byte;
+		ret += nread + len_byte;
+		len_byte = 0;
+	}
+
+	clr_b_busy(bt_bmc);
+
+out_unlock:
+	mutex_unlock(&bt_bmc->mutex);
+	return ret;
+}
+
+/*
+ * BT Message response format :
+ *
+ *    Byte 1  Byte 2     Byte 3  Byte 4  Byte 5  Byte 6:N
+ *    Length  NetFn/LUN  Seq     Cmd     Code    Data
+ */
+static ssize_t bt_bmc_write(struct file *file, const char __user *buf,
+			    size_t count, loff_t *ppos)
+{
+	struct bt_bmc *bt_bmc = file_bt_bmc(file);
+	u8 kbuffer[BT_BMC_BUFFER_SIZE];
+	ssize_t ret = 0;
+	ssize_t nwritten;
+
+	/*
+	 * send a minimum response size
+	 */
+	if (count < 5)
+		return -EINVAL;
+
+	if (!access_ok(VERIFY_READ, buf, count))
+		return -EFAULT;
+
+	WARN_ON(*ppos);
+
+	/*
+	 * There's no interrupt for clearing bmc busy so we have to
+	 * poll
+	 */
+	if (wait_event_interruptible(bt_bmc->queue,
+				     !(bt_inb(bt_bmc, BT_CTRL) &
+				       (BT_CTRL_H_BUSY | BT_CTRL_B2H_ATN))))
+		return -ERESTARTSYS;
+
+	mutex_lock(&bt_bmc->mutex);
+
+	if (unlikely(bt_inb(bt_bmc, BT_CTRL) &
+		     (BT_CTRL_H_BUSY | BT_CTRL_B2H_ATN))) {
+		ret = -EIO;
+		goto out_unlock;
+	}
+
+	clr_wr_ptr(bt_bmc);
+
+	while (count) {
+		nwritten = min_t(ssize_t, count, sizeof(kbuffer));
+		if (copy_from_user(&kbuffer, buf, nwritten)) {
+			ret = -EFAULT;
+			break;
+		}
+
+		bt_writen(bt_bmc, kbuffer, nwritten);
+
+		count -= nwritten;
+		buf += nwritten;
+		ret += nwritten;
+	}
+
+	set_b2h_atn(bt_bmc);
+
+out_unlock:
+	mutex_unlock(&bt_bmc->mutex);
+	return ret;
+}
+
+static long bt_bmc_ioctl(struct file *file, unsigned int cmd,
+			 unsigned long param)
+{
+	struct bt_bmc *bt_bmc = file_bt_bmc(file);
+
+	switch (cmd) {
+	case BT_BMC_IOCTL_SMS_ATN:
+		set_sms_atn(bt_bmc);
+		return 0;
+	}
+	return -EINVAL;
+}
+
+static int bt_bmc_release(struct inode *inode, struct file *file)
+{
+	struct bt_bmc *bt_bmc = file_bt_bmc(file);
+
+	atomic_dec(&open_count);
+	set_b_busy(bt_bmc);
+	return 0;
+}
+
+static unsigned int bt_bmc_poll(struct file *file, poll_table *wait)
+{
+	struct bt_bmc *bt_bmc = file_bt_bmc(file);
+	unsigned int mask = 0;
+	u8 ctrl;
+
+	poll_wait(file, &bt_bmc->queue, wait);
+
+	ctrl = bt_inb(bt_bmc, BT_CTRL);
+
+	if (ctrl & BT_CTRL_H2B_ATN)
+		mask |= POLLIN;
+
+	if (!(ctrl & (BT_CTRL_H_BUSY | BT_CTRL_B2H_ATN)))
+		mask |= POLLOUT;
+
+	return mask;
+}
+
+static const struct file_operations bt_bmc_fops = {
+	.owner		= THIS_MODULE,
+	.open		= bt_bmc_open,
+	.read		= bt_bmc_read,
+	.write		= bt_bmc_write,
+	.release	= bt_bmc_release,
+	.poll		= bt_bmc_poll,
+	.unlocked_ioctl	= bt_bmc_ioctl,
+};
+
+static void poll_timer(unsigned long data)
+{
+	struct bt_bmc *bt_bmc = (void *)data;
+
+	bt_bmc->poll_timer.expires += msecs_to_jiffies(500);
+	wake_up(&bt_bmc->queue);
+	add_timer(&bt_bmc->poll_timer);
+}
+
+static irqreturn_t bt_bmc_irq(int irq, void *arg)
+{
+	struct bt_bmc *bt_bmc = arg;
+	u32 reg;
+
+	reg = ioread32(bt_bmc->base + BT_CR2);
+	reg &= BT_CR2_IRQ_H2B | BT_CR2_IRQ_HBUSY;
+	if (!reg)
+		return IRQ_NONE;
+
+	/* ack pending IRQs */
+	iowrite32(reg, bt_bmc->base + BT_CR2);
+
+	wake_up(&bt_bmc->queue);
+	return IRQ_HANDLED;
+}
+
+static int bt_bmc_config_irq(struct bt_bmc *bt_bmc,
+			     struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	u32 reg;
+	int rc;
+
+	bt_bmc->irq = platform_get_irq(pdev, 0);
+	if (!bt_bmc->irq)
+		return -ENODEV;
+
+	rc = devm_request_irq(dev, bt_bmc->irq, bt_bmc_irq, IRQF_SHARED,
+			      DEVICE_NAME, bt_bmc);
+	if (rc < 0) {
+		dev_warn(dev, "Unable to request IRQ %d\n", bt_bmc->irq);
+		bt_bmc->irq = 0;
+		return rc;
+	}
+
+	/*
+	 * Configure IRQs on the bmc clearing the H2B and HBUSY bits;
+	 * H2B will be asserted when the bmc has data for us; HBUSY
+	 * will be cleared (along with B2H) when we can write the next
+	 * message to the BT buffer
+	 */
+	reg = ioread32(bt_bmc->base + BT_CR1);
+	reg |= BT_CR1_IRQ_H2B | BT_CR1_IRQ_HBUSY;
+	iowrite32(reg, bt_bmc->base + BT_CR1);
+
+	return 0;
+}
+
+static int bt_bmc_probe(struct platform_device *pdev)
+{
+	struct bt_bmc *bt_bmc;
+	struct device *dev;
+	struct resource *res;
+	int rc;
+
+	if (!pdev || !pdev->dev.of_node)
+		return -ENODEV;
+
+	dev = &pdev->dev;
+	dev_info(dev, "Found bt bmc device\n");
+
+	bt_bmc = devm_kzalloc(dev, sizeof(*bt_bmc), GFP_KERNEL);
+	if (!bt_bmc)
+		return -ENOMEM;
+
+	dev_set_drvdata(&pdev->dev, bt_bmc);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	bt_bmc->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(bt_bmc->base))
+		return PTR_ERR(bt_bmc->base);
+
+	mutex_init(&bt_bmc->mutex);
+	init_waitqueue_head(&bt_bmc->queue);
+
+	bt_bmc->miscdev.minor	= MISC_DYNAMIC_MINOR,
+		bt_bmc->miscdev.name	= DEVICE_NAME,
+		bt_bmc->miscdev.fops	= &bt_bmc_fops,
+		bt_bmc->miscdev.parent = dev;
+	rc = misc_register(&bt_bmc->miscdev);
+	if (rc) {
+		dev_err(dev, "Unable to register misc device\n");
+		return rc;
+	}
+
+	bt_bmc_config_irq(bt_bmc, pdev);
+
+	if (bt_bmc->irq) {
+		dev_info(dev, "Using IRQ %d\n", bt_bmc->irq);
+	} else {
+		dev_info(dev, "No IRQ; using timer\n");
+		setup_timer(&bt_bmc->poll_timer, poll_timer,
+			    (unsigned long)bt_bmc);
+		bt_bmc->poll_timer.expires = jiffies + msecs_to_jiffies(10);
+		add_timer(&bt_bmc->poll_timer);
+	}
+
+	iowrite32((BT_IO_BASE << BT_CR0_IO_BASE) |
+		  (BT_IRQ << BT_CR0_IRQ) |
+		  BT_CR0_EN_CLR_SLV_RDP |
+		  BT_CR0_EN_CLR_SLV_WRP |
+		  BT_CR0_ENABLE_IBT,
+		  bt_bmc->base + BT_CR0);
+
+	clr_b_busy(bt_bmc);
+
+	return 0;
+}
+
+static int bt_bmc_remove(struct platform_device *pdev)
+{
+	struct bt_bmc *bt_bmc = dev_get_drvdata(&pdev->dev);
+
+	misc_deregister(&bt_bmc->miscdev);
+	if (!bt_bmc->irq)
+		del_timer_sync(&bt_bmc->poll_timer);
+	return 0;
+}
+
+static const struct of_device_id bt_bmc_match[] = {
+	{ .compatible = "aspeed,ast2400-bt-bmc" },
+	{ },
+};
+
+static struct platform_driver bt_bmc_driver = {
+	.driver = {
+		.name		= DEVICE_NAME,
+		.of_match_table = bt_bmc_match,
+	},
+	.probe = bt_bmc_probe,
+	.remove = bt_bmc_remove,
+};
+
+module_platform_driver(bt_bmc_driver);
+
+MODULE_DEVICE_TABLE(of, bt_bmc_match);
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Alistair Popple <alistair@popple.id.au>");
+MODULE_DESCRIPTION("Linux device interface to the BT interface");
diff --git a/drivers/char/ipmi/ipmi_msghandler.c b/drivers/char/ipmi/ipmi_msghandler.c
index d8619998cfb5..fcdd886819f5 100644
--- a/drivers/char/ipmi/ipmi_msghandler.c
+++ b/drivers/char/ipmi/ipmi_msghandler.c
@@ -2891,11 +2891,11 @@ int ipmi_register_smi(const struct ipmi_smi_handlers *handlers,
 		intf->curr_channel = IPMI_MAX_CHANNELS;
 	}
 
+	rv = ipmi_bmc_register(intf, i);
+
 	if (rv == 0)
 		rv = add_proc_entries(intf, i);
 
-	rv = ipmi_bmc_register(intf, i);
-
  out:
 	if (rv) {
 		if (intf->proc_dir)
@@ -2982,8 +2982,6 @@ int ipmi_unregister_smi(ipmi_smi_t intf)
 	int intf_num = intf->intf_num;
 	ipmi_user_t user;
 
-	ipmi_bmc_unregister(intf);
-
 	mutex_lock(&smi_watchers_mutex);
 	mutex_lock(&ipmi_interfaces_mutex);
 	intf->intf_num = -1;
@@ -3007,6 +3005,7 @@ int ipmi_unregister_smi(ipmi_smi_t intf)
 	mutex_unlock(&ipmi_interfaces_mutex);
 
 	remove_proc_entries(intf);
+	ipmi_bmc_unregister(intf);
 
 	/*
 	 * Call all the watcher interfaces to tell them that
diff --git a/drivers/char/mem.c b/drivers/char/mem.c
index a33163dbb913..5bb1985ec484 100644
--- a/drivers/char/mem.c
+++ b/drivers/char/mem.c
@@ -381,6 +381,9 @@ static ssize_t read_kmem(struct file *file, char __user *buf,
 	char *kbuf; /* k-addr because vread() takes vmlist_lock rwlock */
 	int err = 0;
 
+	if (!pfn_valid(PFN_DOWN(p)))
+		return -EIO;
+
 	read = 0;
 	if (p < (unsigned long) high_memory) {
 		low_count = count;
@@ -509,6 +512,9 @@ static ssize_t write_kmem(struct file *file, const char __user *buf,
 	char *kbuf; /* k-addr because vwrite() takes vmlist_lock rwlock */
 	int err = 0;
 
+	if (!pfn_valid(PFN_DOWN(p)))
+		return -EIO;
+
 	if (p < (unsigned long) high_memory) {
 		unsigned long to_write = min_t(unsigned long, count,
 					       (unsigned long)high_memory - p);
diff --git a/drivers/char/mwave/3780i.c b/drivers/char/mwave/3780i.c
index 28740046bc83..972c40a19629 100644
--- a/drivers/char/mwave/3780i.c
+++ b/drivers/char/mwave/3780i.c
@@ -124,7 +124,7 @@ static void dsp3780I_WriteGenCfg(unsigned short usDspBaseIO, unsigned uIndex,
 		MKBYTE(rSlaveControl));
 
 	rSlaveControl_Save = rSlaveControl;
-	rSlaveControl.ConfigMode = TRUE;
+	rSlaveControl.ConfigMode = true;
 
 	PRINTK_2(TRACE_3780I,
 		"3780i::dsp3780i_WriteGenCfg entry rSlaveControl+ConfigMode %x\n",
@@ -155,7 +155,7 @@ unsigned char dsp3780I_ReadGenCfg(unsigned short usDspBaseIO,
 
 	MKBYTE(rSlaveControl) = InByteDsp(DSP_IsaSlaveControl);
 	rSlaveControl_Save = rSlaveControl;
-	rSlaveControl.ConfigMode = TRUE;
+	rSlaveControl.ConfigMode = true;
 	OutByteDsp(DSP_IsaSlaveControl, MKBYTE(rSlaveControl));
 	OutByteDsp(DSP_ConfigAddress, (unsigned char) uIndex);
 	ucValue = InByteDsp(DSP_ConfigData);
@@ -230,7 +230,7 @@ int dsp3780I_EnableDSP(DSP_3780I_CONFIG_SETTINGS * pSettings,
 			rUartCfg1.BaseIO = 3;
 			break;
 		}
-		rUartCfg2.Enable = TRUE;
+		rUartCfg2.Enable = true;
 	}
 
 	rHBridgeCfg1.Reserved = rHBridgeCfg2.Reserved = 0;
@@ -238,7 +238,7 @@ int dsp3780I_EnableDSP(DSP_3780I_CONFIG_SETTINGS * pSettings,
 	rHBridgeCfg1.IrqPulse = pSettings->bDspIrqPulse;
 	rHBridgeCfg1.Irq = (unsigned char) pIrqMap[pSettings->usDspIrq];
 	rHBridgeCfg1.AccessMode = 1;
-	rHBridgeCfg2.Enable = TRUE;
+	rHBridgeCfg2.Enable = true;
 
 
 	rBusmasterCfg2.Reserved = 0;
@@ -278,8 +278,8 @@ int dsp3780I_EnableDSP(DSP_3780I_CONFIG_SETTINGS * pSettings,
 	* soft-reset active for 10ms.
 	*/
 	rSlaveControl.ClockControl = 0;
-	rSlaveControl.SoftReset = TRUE;
-	rSlaveControl.ConfigMode = FALSE;
+	rSlaveControl.SoftReset = true;
+	rSlaveControl.ConfigMode = false;
 	rSlaveControl.Reserved = 0;
 
 	PRINTK_4(TRACE_3780I,
@@ -302,7 +302,7 @@ int dsp3780I_EnableDSP(DSP_3780I_CONFIG_SETTINGS * pSettings,
 	for (i = 0; i < 11; i++)
 		udelay(2000);
 
-	rSlaveControl.SoftReset = FALSE;
+	rSlaveControl.SoftReset = false;
 	OutWordDsp(DSP_IsaSlaveControl, MKWORD(rSlaveControl));
 
 	MKWORD(tval) = InWordDsp(DSP_IsaSlaveControl);
@@ -326,10 +326,10 @@ int dsp3780I_EnableDSP(DSP_3780I_CONFIG_SETTINGS * pSettings,
 	}
 
 
-	rHBridgeControl.EnableDspInt = FALSE;
-	rHBridgeControl.MemAutoInc = TRUE;
-	rHBridgeControl.IoAutoInc = FALSE;
-	rHBridgeControl.DiagnosticMode = FALSE;
+	rHBridgeControl.EnableDspInt = false;
+	rHBridgeControl.MemAutoInc = true;
+	rHBridgeControl.IoAutoInc = false;
+	rHBridgeControl.DiagnosticMode = false;
 
 	PRINTK_3(TRACE_3780I,
 		"3780i::dsp3780i_EnableDSP DSP_HBridgeControl %x rHBridgeControl %x\n",
@@ -345,7 +345,7 @@ int dsp3780I_EnableDSP(DSP_3780I_CONFIG_SETTINGS * pSettings,
 	ChipID = ReadMsaCfg(DSP_ChipID);
 
 	PRINTK_2(TRACE_3780I,
-		"3780i::dsp3780I_EnableDSP exiting bRC=TRUE, ChipID %x\n",
+		"3780i::dsp3780I_EnableDSP exiting bRC=true, ChipID %x\n",
 		ChipID);
 
 	return 0;
@@ -361,8 +361,8 @@ int dsp3780I_DisableDSP(DSP_3780I_CONFIG_SETTINGS * pSettings)
 	PRINTK_1(TRACE_3780I, "3780i::dsp3780i_DisableDSP entry\n");
 
 	rSlaveControl.ClockControl = 0;
-	rSlaveControl.SoftReset = TRUE;
-	rSlaveControl.ConfigMode = FALSE;
+	rSlaveControl.SoftReset = true;
+	rSlaveControl.ConfigMode = false;
 	rSlaveControl.Reserved = 0;
 	spin_lock_irqsave(&dsp_lock, flags);
 	OutWordDsp(DSP_IsaSlaveControl, MKWORD(rSlaveControl));
@@ -398,14 +398,14 @@ int dsp3780I_Reset(DSP_3780I_CONFIG_SETTINGS * pSettings)
 	PRINTK_2(TRACE_3780I, "3780i::dsp3780i_Reset rHBridgeControl %x\n",
 		MKWORD(rHBridgeControl));
 
-	rHBridgeControl.EnableDspInt = FALSE;
+	rHBridgeControl.EnableDspInt = false;
 	OutWordDsp(DSP_HBridgeControl, MKWORD(rHBridgeControl));
 	spin_unlock_irqrestore(&dsp_lock, flags);
 
 	/* Reset the core via the boot domain register */
-	rBootDomain.ResetCore = TRUE;
-	rBootDomain.Halt = TRUE;
-	rBootDomain.NMI = TRUE;
+	rBootDomain.ResetCore = true;
+	rBootDomain.Halt = true;
+	rBootDomain.NMI = true;
 	rBootDomain.Reserved = 0;
 
 	PRINTK_2(TRACE_3780I, "3780i::dsp3780i_Reset rBootDomain %x\n",
@@ -438,26 +438,26 @@ int dsp3780I_Run(DSP_3780I_CONFIG_SETTINGS * pSettings)
 
 
 	/* Transition the core to a running state */
-	rBootDomain.ResetCore = TRUE;
-	rBootDomain.Halt = FALSE;
-	rBootDomain.NMI = TRUE;
+	rBootDomain.ResetCore = true;
+	rBootDomain.Halt = false;
+	rBootDomain.NMI = true;
 	rBootDomain.Reserved = 0;
 	WriteMsaCfg(DSP_MspBootDomain, MKWORD(rBootDomain));
 
 	udelay(5);
 
-	rBootDomain.ResetCore = FALSE;
+	rBootDomain.ResetCore = false;
 	WriteMsaCfg(DSP_MspBootDomain, MKWORD(rBootDomain));
 	udelay(5);
 
-	rBootDomain.NMI = FALSE;
+	rBootDomain.NMI = false;
 	WriteMsaCfg(DSP_MspBootDomain, MKWORD(rBootDomain));
 	udelay(5);
 
 	/* Enable DSP to PC interrupt */
 	spin_lock_irqsave(&dsp_lock, flags);
 	MKWORD(rHBridgeControl) = InWordDsp(DSP_HBridgeControl);
-	rHBridgeControl.EnableDspInt = TRUE;
+	rHBridgeControl.EnableDspInt = true;
 
 	PRINTK_2(TRACE_3780I, "3780i::dsp3780i_Run rHBridgeControl %x\n",
 		MKWORD(rHBridgeControl));
@@ -466,7 +466,7 @@ int dsp3780I_Run(DSP_3780I_CONFIG_SETTINGS * pSettings)
 	spin_unlock_irqrestore(&dsp_lock, flags);
 
 
-	PRINTK_1(TRACE_3780I, "3780i::dsp3780i_Run exit bRC=TRUE\n");
+	PRINTK_1(TRACE_3780I, "3780i::dsp3780i_Run exit bRC=true\n");
 
 	return 0;
 }
@@ -508,7 +508,7 @@ int dsp3780I_ReadDStore(unsigned short usDspBaseIO, void __user *pvBuffer,
 
 
 	PRINTK_1(TRACE_3780I,
-		"3780I::dsp3780I_ReadDStore exit bRC=TRUE\n");
+		"3780I::dsp3780I_ReadDStore exit bRC=true\n");
 
 	return 0;
 }
@@ -550,7 +550,7 @@ int dsp3780I_ReadAndClearDStore(unsigned short usDspBaseIO,
 
 
 	PRINTK_1(TRACE_3780I,
-		"3780I::dsp3780I_ReadAndClearDStore exit bRC=TRUE\n");
+		"3780I::dsp3780I_ReadAndClearDStore exit bRC=true\n");
 
 	return 0;
 }
@@ -592,7 +592,7 @@ int dsp3780I_WriteDStore(unsigned short usDspBaseIO, void __user *pvBuffer,
 
 
 	PRINTK_1(TRACE_3780I,
-		"3780I::dsp3780D_WriteDStore exit bRC=TRUE\n");
+		"3780I::dsp3780D_WriteDStore exit bRC=true\n");
 
 	return 0;
 }
@@ -640,7 +640,7 @@ int dsp3780I_ReadIStore(unsigned short usDspBaseIO, void __user *pvBuffer,
 	}
 
 	PRINTK_1(TRACE_3780I,
-		"3780I::dsp3780I_ReadIStore exit bRC=TRUE\n");
+		"3780I::dsp3780I_ReadIStore exit bRC=true\n");
 
 	return 0;
 }
@@ -689,7 +689,7 @@ int dsp3780I_WriteIStore(unsigned short usDspBaseIO, void __user *pvBuffer,
 	}
 
 	PRINTK_1(TRACE_3780I,
-		"3780I::dsp3780I_WriteIStore exit bRC=TRUE\n");
+		"3780I::dsp3780I_WriteIStore exit bRC=true\n");
 
 	return 0;
 }
@@ -713,7 +713,7 @@ int dsp3780I_GetIPCSource(unsigned short usDspBaseIO,
 	*/
 	spin_lock_irqsave(&dsp_lock, flags);
 	MKWORD(rHBridgeControl) = InWordDsp(DSP_HBridgeControl);
-	rHBridgeControl.EnableDspInt = FALSE;
+	rHBridgeControl.EnableDspInt = false;
 	OutWordDsp(DSP_HBridgeControl, MKWORD(rHBridgeControl));
 
 	*pusIPCSource = InWordDsp(DSP_Interrupt);
@@ -725,7 +725,7 @@ int dsp3780I_GetIPCSource(unsigned short usDspBaseIO,
 
 	OutWordDsp(DSP_Interrupt, (unsigned short) ~(*pusIPCSource));
 
-	rHBridgeControl.EnableDspInt = TRUE;
+	rHBridgeControl.EnableDspInt = true;
 	OutWordDsp(DSP_HBridgeControl, MKWORD(rHBridgeControl));
 	spin_unlock_irqrestore(&dsp_lock, flags);
 
diff --git a/drivers/char/mwave/3780i.h b/drivers/char/mwave/3780i.h
index fba6ab1160ce..9ccb6b270b07 100644
--- a/drivers/char/mwave/3780i.h
+++ b/drivers/char/mwave/3780i.h
@@ -101,7 +101,7 @@ typedef struct {
 } DSP_UART_CFG_1;
 
 typedef struct {
-	unsigned char Enable:1;	/* RW: Enable I/O and IRQ: 0=FALSE, 1=TRUE */
+	unsigned char Enable:1;	/* RW: Enable I/O and IRQ: 0=false, 1=true */
 	unsigned char Reserved:7;	/* 0: Reserved */
 } DSP_UART_CFG_2;
 
@@ -114,7 +114,7 @@ typedef struct {
 } DSP_HBRIDGE_CFG_1;
 
 typedef struct {
-	unsigned char Enable:1;	/* RW: enable I/O and IRQ: 0=FALSE, 1=TRUE */
+	unsigned char Enable:1;	/* RW: enable I/O and IRQ: 0=false, 1=true */
 	unsigned char Reserved:7;	/* 0: Reserved */
 } DSP_HBRIDGE_CFG_2;
 
@@ -133,12 +133,12 @@ typedef struct {
 
 
 typedef struct {
-	unsigned char GateIOCHRDY:1;	/* RW: Enable IOCHRDY gating: 0=FALSE, 1=TRUE */
+	unsigned char GateIOCHRDY:1;	/* RW: Enable IOCHRDY gating: 0=false, 1=true */
 	unsigned char Reserved:7;	/* 0: Reserved */
 } DSP_ISA_PROT_CFG;
 
 typedef struct {
-	unsigned char Enable:1;	/* RW: Enable low power suspend/resume 0=FALSE, 1=TRUE */
+	unsigned char Enable:1;	/* RW: Enable low power suspend/resume 0=false, 1=true */
 	unsigned char Reserved:7;	/* 0: Reserved */
 } DSP_POWER_MGMT_CFG;
 
diff --git a/drivers/char/mwave/mwavedd.c b/drivers/char/mwave/mwavedd.c
index 164544afd680..3a3ff2eb6cba 100644
--- a/drivers/char/mwave/mwavedd.c
+++ b/drivers/char/mwave/mwavedd.c
@@ -296,8 +296,8 @@ static long mwave_ioctl(struct file *file, unsigned int iocmd,
 				pDrvData->IPCs[ipcnum].usIntCount);
 
 			mutex_lock(&mwave_mutex);
-			pDrvData->IPCs[ipcnum].bIsHere = FALSE;
-			pDrvData->IPCs[ipcnum].bIsEnabled = TRUE;
+			pDrvData->IPCs[ipcnum].bIsHere = false;
+			pDrvData->IPCs[ipcnum].bIsEnabled = true;
 			mutex_unlock(&mwave_mutex);
 	
 			PRINTK_2(TRACE_MWAVE,
@@ -324,7 +324,7 @@ static long mwave_ioctl(struct file *file, unsigned int iocmd,
 				pDrvData->IPCs[ipcnum].usIntCount);
 	
 			mutex_lock(&mwave_mutex);
-			if (pDrvData->IPCs[ipcnum].bIsEnabled == TRUE) {
+			if (pDrvData->IPCs[ipcnum].bIsEnabled == true) {
 				DECLARE_WAITQUEUE(wait, current);
 
 				PRINTK_2(TRACE_MWAVE,
@@ -332,7 +332,7 @@ static long mwave_ioctl(struct file *file, unsigned int iocmd,
 					" ipc %x going to sleep\n",
 					ipcnum);
 				add_wait_queue(&pDrvData->IPCs[ipcnum].ipc_wait_queue, &wait);
-				pDrvData->IPCs[ipcnum].bIsHere = TRUE;
+				pDrvData->IPCs[ipcnum].bIsHere = true;
 				set_current_state(TASK_INTERRUPTIBLE);
 				/* check whether an event was signalled by */
 				/* the interrupt handler while we were gone */
@@ -355,7 +355,7 @@ static long mwave_ioctl(struct file *file, unsigned int iocmd,
 						" application\n",
 						ipcnum);
 				}
-				pDrvData->IPCs[ipcnum].bIsHere = FALSE;
+				pDrvData->IPCs[ipcnum].bIsHere = false;
 				remove_wait_queue(&pDrvData->IPCs[ipcnum].ipc_wait_queue, &wait);
 				set_current_state(TASK_RUNNING);
 				PRINTK_2(TRACE_MWAVE,
@@ -384,9 +384,9 @@ static long mwave_ioctl(struct file *file, unsigned int iocmd,
 				return -EINVAL;
 			}
 			mutex_lock(&mwave_mutex);
-			if (pDrvData->IPCs[ipcnum].bIsEnabled == TRUE) {
-				pDrvData->IPCs[ipcnum].bIsEnabled = FALSE;
-				if (pDrvData->IPCs[ipcnum].bIsHere == TRUE) {
+			if (pDrvData->IPCs[ipcnum].bIsEnabled == true) {
+				pDrvData->IPCs[ipcnum].bIsEnabled = false;
+				if (pDrvData->IPCs[ipcnum].bIsHere == true) {
 					wake_up_interruptible(&pDrvData->IPCs[ipcnum].ipc_wait_queue);
 				}
 			}
@@ -541,7 +541,7 @@ static void mwave_exit(void)
 
 	if (pDrvData->device_registered) {
 		device_unregister(&mwave_device);
-		pDrvData->device_registered = FALSE;
+		pDrvData->device_registered = false;
 	}
 #endif
 
@@ -576,16 +576,16 @@ static int __init mwave_init(void)
 
 	memset(&mwave_s_mdd, 0, sizeof(MWAVE_DEVICE_DATA));
 
-	pDrvData->bBDInitialized = FALSE;
-	pDrvData->bResourcesClaimed = FALSE;
-	pDrvData->bDSPEnabled = FALSE;
-	pDrvData->bDSPReset = FALSE;
-	pDrvData->bMwaveDevRegistered = FALSE;
+	pDrvData->bBDInitialized = false;
+	pDrvData->bResourcesClaimed = false;
+	pDrvData->bDSPEnabled = false;
+	pDrvData->bDSPReset = false;
+	pDrvData->bMwaveDevRegistered = false;
 	pDrvData->sLine = -1;
 
 	for (i = 0; i < ARRAY_SIZE(pDrvData->IPCs); i++) {
-		pDrvData->IPCs[i].bIsEnabled = FALSE;
-		pDrvData->IPCs[i].bIsHere = FALSE;
+		pDrvData->IPCs[i].bIsEnabled = false;
+		pDrvData->IPCs[i].bIsHere = false;
 		pDrvData->IPCs[i].usIntCount = 0;	/* no ints received yet */
 		init_waitqueue_head(&pDrvData->IPCs[i].ipc_wait_queue);
 	}
@@ -601,7 +601,7 @@ static int __init mwave_init(void)
 				" Failed to initialize board data\n");
 		goto cleanup_error;
 	}
-	pDrvData->bBDInitialized = TRUE;
+	pDrvData->bBDInitialized = true;
 
 	retval = tp3780I_CalcResources(&pDrvData->rBDData);
 	PRINTK_2(TRACE_MWAVE,
@@ -626,7 +626,7 @@ static int __init mwave_init(void)
 				" Failed to claim resources\n");
 		goto cleanup_error;
 	}
-	pDrvData->bResourcesClaimed = TRUE;
+	pDrvData->bResourcesClaimed = true;
 
 	retval = tp3780I_EnableDSP(&pDrvData->rBDData);
 	PRINTK_2(TRACE_MWAVE,
@@ -639,7 +639,7 @@ static int __init mwave_init(void)
 				" Failed to enable DSP\n");
 		goto cleanup_error;
 	}
-	pDrvData->bDSPEnabled = TRUE;
+	pDrvData->bDSPEnabled = true;
 
 	if (misc_register(&mwave_misc_dev) < 0) {
 		PRINTK_ERROR(KERN_ERR_MWAVE
@@ -647,7 +647,7 @@ static int __init mwave_init(void)
 				" Failed to register misc device\n");
 		goto cleanup_error;
 	}
-	pDrvData->bMwaveDevRegistered = TRUE;
+	pDrvData->bMwaveDevRegistered = true;
 
 	pDrvData->sLine = register_serial_portandirq(
 		pDrvData->rBDData.rDspSettings.usUartBaseIO,
@@ -668,7 +668,7 @@ static int __init mwave_init(void)
 
 	if (device_register(&mwave_device))
 		goto cleanup_error;
-	pDrvData->device_registered = TRUE;
+	pDrvData->device_registered = true;
 	for (i = 0; i < ARRAY_SIZE(mwave_dev_attrs); i++) {
 		if(device_create_file(&mwave_device, mwave_dev_attrs[i])) {
 			PRINTK_ERROR(KERN_ERR_MWAVE
diff --git a/drivers/char/mwave/mwavedd.h b/drivers/char/mwave/mwavedd.h
index 7e0d530e2e07..37e0a4926080 100644
--- a/drivers/char/mwave/mwavedd.h
+++ b/drivers/char/mwave/mwavedd.h
@@ -125,8 +125,8 @@ extern int mwave_uart_io;
 
 typedef struct _MWAVE_IPC {
 	unsigned short usIntCount;	/* 0=none, 1=first, 2=greater than 1st */
-	BOOLEAN bIsEnabled;
-	BOOLEAN bIsHere;
+	bool bIsEnabled;
+	bool bIsHere;
 	/* entry spin lock */
 	wait_queue_head_t ipc_wait_queue;
 } MWAVE_IPC;
@@ -135,12 +135,12 @@ typedef struct _MWAVE_DEVICE_DATA {
 	THINKPAD_BD_DATA rBDData;	/* board driver's data area */
 	unsigned long ulIPCSource_ISR;	/* IPC source bits for recently processed intr, set during ISR processing */
 	unsigned long ulIPCSource_DPC;	/* IPC source bits for recently processed intr, set during DPC processing */
-	BOOLEAN bBDInitialized;
-	BOOLEAN bResourcesClaimed;
-	BOOLEAN bDSPEnabled;
-	BOOLEAN bDSPReset;
+	bool bBDInitialized;
+	bool bResourcesClaimed;
+	bool bDSPEnabled;
+	bool bDSPReset;
 	MWAVE_IPC IPCs[16];
-	BOOLEAN bMwaveDevRegistered;
+	bool bMwaveDevRegistered;
 	short sLine;
 	int nr_registered_attrs;
 	int device_registered;
diff --git a/drivers/char/mwave/smapi.c b/drivers/char/mwave/smapi.c
index 6187fd14b3fe..8c5411a8f33f 100644
--- a/drivers/char/mwave/smapi.c
+++ b/drivers/char/mwave/smapi.c
@@ -493,7 +493,7 @@ exit_smapi_request_error:
 }
 
 
-int smapi_set_DSP_power_state(BOOLEAN bOn)
+int smapi_set_DSP_power_state(bool bOn)
 {
 	int bRC = -EIO;
 	unsigned short usAX, usBX, usCX, usDX, usDI, usSI;
@@ -556,7 +556,7 @@ int smapi_init(void)
 			PRINTK_ERROR("smapi::smapi_init, ERROR unable to read from SMAPI port\n");
 		} else {
 			PRINTK_2(TRACE_SMAPI,
-				"smapi::smapi_init, exit TRUE g_usSmapiPort %x\n",
+				"smapi::smapi_init, exit true g_usSmapiPort %x\n",
 				g_usSmapiPort);
 			retval = 0;
 			//SmapiQuerySystemID();
diff --git a/drivers/char/mwave/smapi.h b/drivers/char/mwave/smapi.h
index 64b2ec1420e3..ebc206b000b9 100644
--- a/drivers/char/mwave/smapi.h
+++ b/drivers/char/mwave/smapi.h
@@ -49,10 +49,6 @@
 #ifndef _LINUX_SMAPI_H
 #define _LINUX_SMAPI_H
 
-#define TRUE 1
-#define FALSE 0
-#define BOOLEAN int
-
 typedef struct {
 	int bDSPPresent;
 	int bDSPEnabled;
@@ -74,7 +70,7 @@ typedef struct {
 int smapi_init(void);
 int smapi_query_DSP_cfg(SMAPI_DSP_SETTINGS * pSettings);
 int smapi_set_DSP_cfg(void);
-int smapi_set_DSP_power_state(BOOLEAN bOn);
+int smapi_set_DSP_power_state(bool bOn);
 
 
 #endif
diff --git a/drivers/char/mwave/tp3780i.c b/drivers/char/mwave/tp3780i.c
index 04e6d6a27994..5e1618a76b2a 100644
--- a/drivers/char/mwave/tp3780i.c
+++ b/drivers/char/mwave/tp3780i.c
@@ -80,13 +80,13 @@ static void EnableSRAM(THINKPAD_BD_DATA * pBDData)
 	WriteMsaCfg(DSP_GpioModeControl_15_8, MKWORD(rGpioMode));
 
 	MKWORD(rGpioDriverEnable) = 0;
-	rGpioDriverEnable.Enable10 = TRUE;
-	rGpioDriverEnable.Mask10 = TRUE;
+	rGpioDriverEnable.Enable10 = true;
+	rGpioDriverEnable.Mask10 = true;
 	WriteMsaCfg(DSP_GpioDriverEnable_15_8, MKWORD(rGpioDriverEnable));
 
 	MKWORD(rGpioOutputData) = 0;
 	rGpioOutputData.Latch10 = 0;
-	rGpioOutputData.Mask10 = TRUE;
+	rGpioOutputData.Mask10 = true;
 	WriteMsaCfg(DSP_GpioOutputData_15_8, MKWORD(rGpioOutputData));
 
 	PRINTK_1(TRACE_TP3780I, "tp3780i::EnableSRAM exit\n");
@@ -127,7 +127,7 @@ static irqreturn_t DspInterrupt(int irq, void *dev_id)
 				PRINTK_2(TRACE_TP3780I,
 					"tp3780i::DspInterrupt usIntCount %x\n",
 					pDrvData->IPCs[usPCNum - 1].usIntCount);
-				if (pDrvData->IPCs[usPCNum - 1].bIsEnabled == TRUE) {
+				if (pDrvData->IPCs[usPCNum - 1].bIsEnabled == true) {
 					PRINTK_2(TRACE_TP3780I,
 						"tp3780i::DspInterrupt, waking up usPCNum %x\n",
 						usPCNum - 1);
@@ -160,8 +160,8 @@ int tp3780I_InitializeBoardData(THINKPAD_BD_DATA * pBDData)
 
 	PRINTK_2(TRACE_TP3780I, "tp3780i::tp3780I_InitializeBoardData entry pBDData %p\n", pBDData);
 
-	pBDData->bDSPEnabled = FALSE;
-	pSettings->bInterruptClaimed = FALSE;
+	pBDData->bDSPEnabled = false;
+	pSettings->bInterruptClaimed = false;
 
 	retval = smapi_init();
 	if (retval) {
@@ -269,7 +269,7 @@ int tp3780I_ReleaseResources(THINKPAD_BD_DATA * pBDData)
 
 	if (pSettings->bInterruptClaimed) {
 		free_irq(pSettings->usDspIrq, NULL);
-		pSettings->bInterruptClaimed = FALSE;
+		pSettings->bInterruptClaimed = false;
 	}
 
 	PRINTK_2(TRACE_TP3780I,
@@ -283,7 +283,7 @@ int tp3780I_ReleaseResources(THINKPAD_BD_DATA * pBDData)
 int tp3780I_EnableDSP(THINKPAD_BD_DATA * pBDData)
 {
 	DSP_3780I_CONFIG_SETTINGS *pSettings = &pBDData->rDspSettings;
-	BOOLEAN bDSPPoweredUp = FALSE, bInterruptAllocated = FALSE;
+	bool bDSPPoweredUp = false, bInterruptAllocated = false;
 
 	PRINTK_2(TRACE_TP3780I, "tp3780i::tp3780I_EnableDSP entry pBDData %p\n", pBDData);
 
@@ -336,14 +336,14 @@ int tp3780I_EnableDSP(THINKPAD_BD_DATA * pBDData)
 		}
 	}
 
-	pSettings->bDspIrqActiveLow = pSettings->bDspIrqPulse = TRUE;
-	pSettings->bUartIrqActiveLow = pSettings->bUartIrqPulse = TRUE;
+	pSettings->bDspIrqActiveLow = pSettings->bDspIrqPulse = true;
+	pSettings->bUartIrqActiveLow = pSettings->bUartIrqPulse = true;
 
 	if (pBDData->bShareDspIrq) {
-		pSettings->bDspIrqActiveLow = FALSE;
+		pSettings->bDspIrqActiveLow = false;
 	}
 	if (pBDData->bShareUartIrq) {
-		pSettings->bUartIrqActiveLow = FALSE;
+		pSettings->bUartIrqActiveLow = false;
 	}
 
 	pSettings->usNumTransfers = TP_CFG_NumTransfers;
@@ -373,16 +373,16 @@ int tp3780I_EnableDSP(THINKPAD_BD_DATA * pBDData)
 		PRINTK_3(TRACE_TP3780I,
 			"tp3780i::tp3780I_EnableDSP, got interrupt %x bShareDspIrq %x\n",
 			pSettings->usDspIrq, pBDData->bShareDspIrq);
-		bInterruptAllocated = TRUE;
-		pSettings->bInterruptClaimed = TRUE;
+		bInterruptAllocated = true;
+		pSettings->bInterruptClaimed = true;
 	}
 
-	smapi_set_DSP_power_state(FALSE);
-	if (smapi_set_DSP_power_state(TRUE)) {
-		PRINTK_ERROR(KERN_ERR_MWAVE "tp3780i::tp3780I_EnableDSP: Error: smapi_set_DSP_power_state(TRUE) failed\n");
+	smapi_set_DSP_power_state(false);
+	if (smapi_set_DSP_power_state(true)) {
+		PRINTK_ERROR(KERN_ERR_MWAVE "tp3780i::tp3780I_EnableDSP: Error: smapi_set_DSP_power_state(true) failed\n");
 		goto exit_cleanup;
 	} else {
-		bDSPPoweredUp = TRUE;
+		bDSPPoweredUp = true;
 	}
 
 	if (dsp3780I_EnableDSP(pSettings, s_ausThinkpadIrqToField, s_ausThinkpadDmaToField)) {
@@ -392,7 +392,7 @@ int tp3780I_EnableDSP(THINKPAD_BD_DATA * pBDData)
 
 	EnableSRAM(pBDData);
 
-	pBDData->bDSPEnabled = TRUE;
+	pBDData->bDSPEnabled = true;
 
 	PRINTK_1(TRACE_TP3780I, "tp3780i::tp3780I_EnableDSP exit\n");
 
@@ -401,10 +401,10 @@ int tp3780I_EnableDSP(THINKPAD_BD_DATA * pBDData)
 exit_cleanup:
 	PRINTK_ERROR("tp3780i::tp3780I_EnableDSP: Cleaning up\n");
 	if (bDSPPoweredUp)
-		smapi_set_DSP_power_state(FALSE);
+		smapi_set_DSP_power_state(false);
 	if (bInterruptAllocated) {
 		free_irq(pSettings->usDspIrq, NULL);
-		pSettings->bInterruptClaimed = FALSE;
+		pSettings->bInterruptClaimed = false;
 	}
 	return -EIO;
 }
@@ -421,10 +421,10 @@ int tp3780I_DisableDSP(THINKPAD_BD_DATA * pBDData)
 		dsp3780I_DisableDSP(&pBDData->rDspSettings);
 		if (pSettings->bInterruptClaimed) {
 			free_irq(pSettings->usDspIrq, NULL);
-			pSettings->bInterruptClaimed = FALSE;
+			pSettings->bInterruptClaimed = false;
 		}
-		smapi_set_DSP_power_state(FALSE);
-		pBDData->bDSPEnabled = FALSE;
+		smapi_set_DSP_power_state(false);
+		pBDData->bDSPEnabled = false;
 	}
 
 	PRINTK_2(TRACE_TP3780I, "tp3780i::tp3780I_DisableDSP exit retval %x\n", retval);
@@ -516,7 +516,7 @@ int tp3780I_ReadWriteDspDStore(THINKPAD_BD_DATA * pBDData, unsigned int uOpcode,
 	int retval = 0;
 	DSP_3780I_CONFIG_SETTINGS *pSettings = &pBDData->rDspSettings;
 	unsigned short usDspBaseIO = pSettings->usDspBaseIO;
-	BOOLEAN bRC = 0;
+	bool bRC = 0;
 
 	PRINTK_6(TRACE_TP3780I,
 		"tp3780i::tp3780I_ReadWriteDspDStore entry pBDData %p, uOpcode %x, pvBuffer %p, uCount %x, ulDSPAddr %lx\n",
@@ -552,7 +552,7 @@ int tp3780I_ReadWriteDspIStore(THINKPAD_BD_DATA * pBDData, unsigned int uOpcode,
 	int retval = 0;
 	DSP_3780I_CONFIG_SETTINGS *pSettings = &pBDData->rDspSettings;
 	unsigned short usDspBaseIO = pSettings->usDspBaseIO;
-	BOOLEAN bRC = 0;
+	bool bRC = 0;
 
 	PRINTK_6(TRACE_TP3780I,
 		"tp3780i::tp3780I_ReadWriteDspIStore entry pBDData %p, uOpcode %x, pvBuffer %p, uCount %x, ulDSPAddr %lx\n",
diff --git a/drivers/char/ppdev.c b/drivers/char/ppdev.c
index f8a483c67b07..d23368874710 100644
--- a/drivers/char/ppdev.c
+++ b/drivers/char/ppdev.c
@@ -286,7 +286,7 @@ static int register_device(int minor, struct pp_struct *pp)
 	struct parport *port;
 	struct pardevice *pdev = NULL;
 	char *name;
-	int fl;
+	struct pardev_cb ppdev_cb;
 
 	name = kasprintf(GFP_KERNEL, CHRDEV "%x", minor);
 	if (name == NULL)
@@ -299,9 +299,11 @@ static int register_device(int minor, struct pp_struct *pp)
 		return -ENXIO;
 	}
 
-	fl = (pp->flags & PP_EXCL) ? PARPORT_FLAG_EXCL : 0;
-	pdev = parport_register_device(port, name, NULL,
-				       NULL, pp_irq, fl, pp);
+	memset(&ppdev_cb, 0, sizeof(ppdev_cb));
+	ppdev_cb.irq_func = pp_irq;
+	ppdev_cb.flags = (pp->flags & PP_EXCL) ? PARPORT_FLAG_EXCL : 0;
+	ppdev_cb.private = pp;
+	pdev = parport_register_dev_model(port, name, &ppdev_cb, minor);
 	parport_put_port(port);
 
 	if (!pdev) {
@@ -799,10 +801,23 @@ static void pp_detach(struct parport *port)
 	device_destroy(ppdev_class, MKDEV(PP_MAJOR, port->number));
 }
 
+static int pp_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;
+}
+
 static struct parport_driver pp_driver = {
 	.name		= CHRDEV,
-	.attach		= pp_attach,
+	.probe		= pp_probe,
+	.match_port	= pp_attach,
 	.detach		= pp_detach,
+	.devmodel	= true,
 };
 
 static int __init ppdev_init(void)
diff --git a/drivers/char/random.c b/drivers/char/random.c
index 3efb3bf0ab83..d6876d506220 100644
--- a/drivers/char/random.c
+++ b/drivers/char/random.c
@@ -479,8 +479,8 @@ static ssize_t _extract_entropy(struct entropy_store *r, void *buf,
 
 static void crng_reseed(struct crng_state *crng, struct entropy_store *r);
 static void push_to_pool(struct work_struct *work);
-static __u32 input_pool_data[INPUT_POOL_WORDS];
-static __u32 blocking_pool_data[OUTPUT_POOL_WORDS];
+static __u32 input_pool_data[INPUT_POOL_WORDS] __latent_entropy;
+static __u32 blocking_pool_data[OUTPUT_POOL_WORDS] __latent_entropy;
 
 static struct entropy_store input_pool = {
 	.poolinfo = &poolinfo_table[0],
@@ -2100,23 +2100,37 @@ unsigned long get_random_long(void)
 }
 EXPORT_SYMBOL(get_random_long);
 
-/*
- * randomize_range() returns a start address such that
+/**
+ * randomize_page - Generate a random, page aligned address
+ * @start:	The smallest acceptable address the caller will take.
+ * @range:	The size of the area, starting at @start, within which the
+ *		random address must fall.
+ *
+ * If @start + @range would overflow, @range is capped.
  *
- *    [...... <range> .....]
- *  start                  end
+ * NOTE: Historical use of randomize_range, which this replaces, presumed that
+ * @start was already page aligned.  We now align it regardless.
  *
- * a <range> with size "len" starting at the return value is inside in the
- * area defined by [start, end], but is otherwise randomized.
+ * Return: A page aligned address within [start, start + range).  On error,
+ * @start is returned.
  */
 unsigned long
-randomize_range(unsigned long start, unsigned long end, unsigned long len)
+randomize_page(unsigned long start, unsigned long range)
 {
-	unsigned long range = end - len - start;
+	if (!PAGE_ALIGNED(start)) {
+		range -= PAGE_ALIGN(start) - start;
+		start = PAGE_ALIGN(start);
+	}
 
-	if (end <= start + len)
-		return 0;
-	return PAGE_ALIGN(get_random_int() % range + start);
+	if (start > ULONG_MAX - range)
+		range = ULONG_MAX - start;
+
+	range >>= PAGE_SHIFT;
+
+	if (range == 0)
+		return start;
+
+	return start + (get_random_long() % range << PAGE_SHIFT);
 }
 
 /* Interface for in-kernel drivers of true hardware RNGs.
diff --git a/drivers/char/snsc.c b/drivers/char/snsc.c
index 94006f9c2e43..10e56323f390 100644
--- a/drivers/char/snsc.c
+++ b/drivers/char/snsc.c
@@ -385,13 +385,18 @@ scdrv_init(void)
 
 	event_nasid = ia64_sn_get_console_nasid();
 
+	snsc_class = class_create(THIS_MODULE, SYSCTL_BASENAME);
+	if (IS_ERR(snsc_class)) {
+		printk("%s: failed to allocate class\n", __func__);
+		return PTR_ERR(snsc_class);
+	}
+
 	if (alloc_chrdev_region(&first_dev, 0, num_cnodes,
 				SYSCTL_BASENAME) < 0) {
 		printk("%s: failed to register SN system controller device\n",
 		       __func__);
 		return -ENODEV;
 	}
-	snsc_class = class_create(THIS_MODULE, SYSCTL_BASENAME);
 
 	for (cnode = 0; cnode < num_cnodes; cnode++) {
 			geoid = cnodeid_get_geoid(cnode);
diff --git a/drivers/char/sonypi.c b/drivers/char/sonypi.c
index e496daefe9e0..719c5b4eed39 100644
--- a/drivers/char/sonypi.c
+++ b/drivers/char/sonypi.c
@@ -934,7 +934,7 @@ static ssize_t sonypi_misc_read(struct file *file, char __user *buf,
 
 	if (ret > 0) {
 		struct inode *inode = file_inode(file);
-		inode->i_atime = current_fs_time(inode->i_sb);
+		inode->i_atime = current_time(inode);
 	}
 
 	return ret;
diff --git a/drivers/char/tb0219.c b/drivers/char/tb0219.c
index 480a777db577..7c19d9b22785 100644
--- a/drivers/char/tb0219.c
+++ b/drivers/char/tb0219.c
@@ -21,6 +21,7 @@
 #include <linux/fs.h>
 #include <linux/init.h>
 #include <linux/module.h>
+#include <linux/uaccess.h>
 
 #include <asm/io.h>
 #include <asm/reboot.h>
diff --git a/drivers/char/tile-srom.c b/drivers/char/tile-srom.c
index 69f6b4acc377..398800edb2cc 100644
--- a/drivers/char/tile-srom.c
+++ b/drivers/char/tile-srom.c
@@ -331,13 +331,11 @@ static const struct file_operations srom_fops = {
 /**
  * srom_setup_minor() - Initialize per-minor information.
  * @srom: Per-device SROM state.
- * @index: Device to set up.
+ * @devhdl: Partition device handle.
  */
-static int srom_setup_minor(struct srom_dev *srom, int index)
+static int srom_setup_minor(struct srom_dev *srom, int devhdl)
 {
-	struct device *dev;
-	int devhdl = srom->hv_devhdl;
-
+	srom->hv_devhdl = devhdl;
 	mutex_init(&srom->lock);
 
 	if (_srom_read(devhdl, &srom->total_size,
@@ -350,9 +348,7 @@ static int srom_setup_minor(struct srom_dev *srom, int index)
 		       SROM_PAGE_SIZE_OFF, sizeof(srom->page_size)) < 0)
 		return -EIO;
 
-	dev = device_create(srom_class, &srom_parent->dev,
-			    MKDEV(srom_major, index), srom, "%d", index);
-	return PTR_ERR_OR_ZERO(dev);
+	return 0;
 }
 
 /** srom_init() - Initialize the driver's module. */
@@ -365,7 +361,7 @@ static int srom_init(void)
 	 * Start with a plausible number of partitions; the krealloc() call
 	 * below will yield about log(srom_devs) additional allocations.
 	 */
-	srom_devices = kzalloc(4 * sizeof(struct srom_dev), GFP_KERNEL);
+	srom_devices = kmalloc(4 * sizeof(struct srom_dev), GFP_KERNEL);
 
 	/* Discover the number of srom partitions. */
 	for (i = 0; ; i++) {
@@ -373,7 +369,7 @@ static int srom_init(void)
 		char buf[20];
 		struct srom_dev *new_srom_devices =
 			krealloc(srom_devices, (i+1) * sizeof(struct srom_dev),
-				 GFP_KERNEL | __GFP_ZERO);
+				 GFP_KERNEL);
 		if (!new_srom_devices) {
 			result = -ENOMEM;
 			goto fail_mem;
@@ -387,7 +383,9 @@ static int srom_init(void)
 					  i, devhdl);
 			break;
 		}
-		srom_devices[i].hv_devhdl = devhdl;
+		result = srom_setup_minor(&srom_devices[i], devhdl);
+		if (result != 0)
+			goto fail_mem;
 	}
 	srom_devs = i;
 
@@ -431,9 +429,13 @@ static int srom_init(void)
 	srom_class->dev_groups = srom_dev_groups;
 	srom_class->devnode = srom_devnode;
 
-	/* Do per-partition initialization */
+	/* Create per-partition devices */
 	for (i = 0; i < srom_devs; i++) {
-		result = srom_setup_minor(srom_devices + i, i);
+		struct device *dev =
+			device_create(srom_class, &srom_parent->dev,
+				      MKDEV(srom_major, i), srom_devices + i,
+				      "%d", i);
+		result = PTR_ERR_OR_ZERO(dev);
 		if (result < 0)
 			goto fail_class;
 	}
diff --git a/drivers/char/tpm/st33zp24/st33zp24.c b/drivers/char/tpm/st33zp24/st33zp24.c
index c2ee30451e41..6f060c76217b 100644
--- a/drivers/char/tpm/st33zp24/st33zp24.c
+++ b/drivers/char/tpm/st33zp24/st33zp24.c
@@ -589,8 +589,6 @@ int st33zp24_probe(void *phy_id, const struct st33zp24_phy_ops *ops,
 		chip->flags |= TPM_CHIP_FLAG_IRQ;
 
 		disable_irq_nosync(tpm_dev->irq);
-
-		tpm_gen_interrupt(chip);
 	}
 
 	return tpm_chip_register(chip);
diff --git a/drivers/char/tpm/tpm-dev.c b/drivers/char/tpm/tpm-dev.c
index f5d452151c6b..912ad30be585 100644
--- a/drivers/char/tpm/tpm-dev.c
+++ b/drivers/char/tpm/tpm-dev.c
@@ -145,7 +145,7 @@ static ssize_t tpm_write(struct file *file, const char __user *buf,
 		return -EPIPE;
 	}
 	out_size = tpm_transmit(priv->chip, priv->data_buffer,
-				sizeof(priv->data_buffer));
+				sizeof(priv->data_buffer), 0);
 
 	tpm_put_ops(priv->chip);
 	if (out_size < 0) {
diff --git a/drivers/char/tpm/tpm-interface.c b/drivers/char/tpm/tpm-interface.c
index 1abe2d7a2610..8de61876f633 100644
--- a/drivers/char/tpm/tpm-interface.c
+++ b/drivers/char/tpm/tpm-interface.c
@@ -330,13 +330,16 @@ EXPORT_SYMBOL_GPL(tpm_calc_ordinal_duration);
 /*
  * Internal kernel interface to transmit TPM commands
  */
-ssize_t tpm_transmit(struct tpm_chip *chip, const char *buf,
-		     size_t bufsiz)
+ssize_t tpm_transmit(struct tpm_chip *chip, const u8 *buf, size_t bufsiz,
+		     unsigned int flags)
 {
 	ssize_t rc;
 	u32 count, ordinal;
 	unsigned long stop;
 
+	if (bufsiz < TPM_HEADER_SIZE)
+		return -EINVAL;
+
 	if (bufsiz > TPM_BUFSIZE)
 		bufsiz = TPM_BUFSIZE;
 
@@ -350,7 +353,8 @@ ssize_t tpm_transmit(struct tpm_chip *chip, const char *buf,
 		return -E2BIG;
 	}
 
-	mutex_lock(&chip->tpm_mutex);
+	if (!(flags & TPM_TRANSMIT_UNLOCKED))
+		mutex_lock(&chip->tpm_mutex);
 
 	rc = chip->ops->send(chip, (u8 *) buf, count);
 	if (rc < 0) {
@@ -393,20 +397,21 @@ out_recv:
 		dev_err(&chip->dev,
 			"tpm_transmit: tpm_recv: error %zd\n", rc);
 out:
-	mutex_unlock(&chip->tpm_mutex);
+	if (!(flags & TPM_TRANSMIT_UNLOCKED))
+		mutex_unlock(&chip->tpm_mutex);
 	return rc;
 }
 
 #define TPM_DIGEST_SIZE 20
 #define TPM_RET_CODE_IDX 6
 
-ssize_t tpm_transmit_cmd(struct tpm_chip *chip, void *cmd,
-			 int len, const char *desc)
+ssize_t tpm_transmit_cmd(struct tpm_chip *chip, const void *cmd,
+			 int len, unsigned int flags, const char *desc)
 {
-	struct tpm_output_header *header;
+	const struct tpm_output_header *header;
 	int err;
 
-	len = tpm_transmit(chip, (u8 *) cmd, len);
+	len = tpm_transmit(chip, (const u8 *)cmd, len, flags);
 	if (len <  0)
 		return len;
 	else if (len < TPM_HEADER_SIZE)
@@ -453,26 +458,13 @@ ssize_t tpm_getcap(struct tpm_chip *chip, __be32 subcap_id, cap_t *cap,
 		tpm_cmd.params.getcap_in.subcap_size = cpu_to_be32(4);
 		tpm_cmd.params.getcap_in.subcap = subcap_id;
 	}
-	rc = tpm_transmit_cmd(chip, &tpm_cmd, TPM_INTERNAL_RESULT_SIZE, desc);
+	rc = tpm_transmit_cmd(chip, &tpm_cmd, TPM_INTERNAL_RESULT_SIZE, 0,
+			      desc);
 	if (!rc)
 		*cap = tpm_cmd.params.getcap_out.cap;
 	return rc;
 }
-
-void tpm_gen_interrupt(struct tpm_chip *chip)
-{
-	struct	tpm_cmd_t tpm_cmd;
-	ssize_t rc;
-
-	tpm_cmd.header.in = tpm_getcap_header;
-	tpm_cmd.params.getcap_in.cap = TPM_CAP_PROP;
-	tpm_cmd.params.getcap_in.subcap_size = cpu_to_be32(4);
-	tpm_cmd.params.getcap_in.subcap = TPM_CAP_PROP_TIS_TIMEOUT;
-
-	rc = tpm_transmit_cmd(chip, &tpm_cmd, TPM_INTERNAL_RESULT_SIZE,
-			      "attempting to determine the timeouts");
-}
-EXPORT_SYMBOL_GPL(tpm_gen_interrupt);
+EXPORT_SYMBOL_GPL(tpm_getcap);
 
 #define TPM_ORD_STARTUP cpu_to_be32(153)
 #define TPM_ST_CLEAR cpu_to_be16(1)
@@ -490,7 +482,7 @@ static int tpm_startup(struct tpm_chip *chip, __be16 startup_type)
 	start_cmd.header.in = tpm_startup_header;
 
 	start_cmd.params.startup_in.startup_type = startup_type;
-	return tpm_transmit_cmd(chip, &start_cmd, TPM_INTERNAL_RESULT_SIZE,
+	return tpm_transmit_cmd(chip, &start_cmd, TPM_INTERNAL_RESULT_SIZE, 0,
 				"attempting to start the TPM");
 }
 
@@ -521,7 +513,8 @@ int tpm_get_timeouts(struct tpm_chip *chip)
 	tpm_cmd.params.getcap_in.cap = TPM_CAP_PROP;
 	tpm_cmd.params.getcap_in.subcap_size = cpu_to_be32(4);
 	tpm_cmd.params.getcap_in.subcap = TPM_CAP_PROP_TIS_TIMEOUT;
-	rc = tpm_transmit_cmd(chip, &tpm_cmd, TPM_INTERNAL_RESULT_SIZE, NULL);
+	rc = tpm_transmit_cmd(chip, &tpm_cmd, TPM_INTERNAL_RESULT_SIZE, 0,
+			      NULL);
 
 	if (rc == TPM_ERR_INVALID_POSTINIT) {
 		/* The TPM is not started, we are the first to talk to it.
@@ -535,7 +528,7 @@ int tpm_get_timeouts(struct tpm_chip *chip)
 		tpm_cmd.params.getcap_in.subcap_size = cpu_to_be32(4);
 		tpm_cmd.params.getcap_in.subcap = TPM_CAP_PROP_TIS_TIMEOUT;
 		rc = tpm_transmit_cmd(chip, &tpm_cmd, TPM_INTERNAL_RESULT_SIZE,
-				  NULL);
+				      0, NULL);
 	}
 	if (rc) {
 		dev_err(&chip->dev,
@@ -596,7 +589,7 @@ duration:
 	tpm_cmd.params.getcap_in.subcap_size = cpu_to_be32(4);
 	tpm_cmd.params.getcap_in.subcap = TPM_CAP_PROP_TIS_DURATION;
 
-	rc = tpm_transmit_cmd(chip, &tpm_cmd, TPM_INTERNAL_RESULT_SIZE,
+	rc = tpm_transmit_cmd(chip, &tpm_cmd, TPM_INTERNAL_RESULT_SIZE, 0,
 			      "attempting to determine the durations");
 	if (rc)
 		return rc;
@@ -633,7 +626,7 @@ EXPORT_SYMBOL_GPL(tpm_get_timeouts);
 #define TPM_ORD_CONTINUE_SELFTEST 83
 #define CONTINUE_SELFTEST_RESULT_SIZE 10
 
-static struct tpm_input_header continue_selftest_header = {
+static const struct tpm_input_header continue_selftest_header = {
 	.tag = TPM_TAG_RQU_COMMAND,
 	.length = cpu_to_be32(10),
 	.ordinal = cpu_to_be32(TPM_ORD_CONTINUE_SELFTEST),
@@ -652,14 +645,14 @@ static int tpm_continue_selftest(struct tpm_chip *chip)
 	struct tpm_cmd_t cmd;
 
 	cmd.header.in = continue_selftest_header;
-	rc = tpm_transmit_cmd(chip, &cmd, CONTINUE_SELFTEST_RESULT_SIZE,
+	rc = tpm_transmit_cmd(chip, &cmd, CONTINUE_SELFTEST_RESULT_SIZE, 0,
 			      "continue selftest");
 	return rc;
 }
 
 #define TPM_ORDINAL_PCRREAD cpu_to_be32(21)
 #define READ_PCR_RESULT_SIZE 30
-static struct tpm_input_header pcrread_header = {
+static const struct tpm_input_header pcrread_header = {
 	.tag = TPM_TAG_RQU_COMMAND,
 	.length = cpu_to_be32(14),
 	.ordinal = TPM_ORDINAL_PCRREAD
@@ -672,7 +665,7 @@ int tpm_pcr_read_dev(struct tpm_chip *chip, int pcr_idx, u8 *res_buf)
 
 	cmd.header.in = pcrread_header;
 	cmd.params.pcrread_in.pcr_idx = cpu_to_be32(pcr_idx);
-	rc = tpm_transmit_cmd(chip, &cmd, READ_PCR_RESULT_SIZE,
+	rc = tpm_transmit_cmd(chip, &cmd, READ_PCR_RESULT_SIZE, 0,
 			      "attempting to read a pcr value");
 
 	if (rc == 0)
@@ -745,7 +738,7 @@ EXPORT_SYMBOL_GPL(tpm_pcr_read);
  */
 #define TPM_ORD_PCR_EXTEND cpu_to_be32(20)
 #define EXTEND_PCR_RESULT_SIZE 34
-static struct tpm_input_header pcrextend_header = {
+static const struct tpm_input_header pcrextend_header = {
 	.tag = TPM_TAG_RQU_COMMAND,
 	.length = cpu_to_be32(34),
 	.ordinal = TPM_ORD_PCR_EXTEND
@@ -770,7 +763,7 @@ int tpm_pcr_extend(u32 chip_num, int pcr_idx, const u8 *hash)
 	cmd.header.in = pcrextend_header;
 	cmd.params.pcrextend_in.pcr_idx = cpu_to_be32(pcr_idx);
 	memcpy(cmd.params.pcrextend_in.hash, hash, TPM_DIGEST_SIZE);
-	rc = tpm_transmit_cmd(chip, &cmd, EXTEND_PCR_RESULT_SIZE,
+	rc = tpm_transmit_cmd(chip, &cmd, EXTEND_PCR_RESULT_SIZE, 0,
 			      "attempting extend a PCR value");
 
 	tpm_put_ops(chip);
@@ -792,7 +785,7 @@ int tpm_do_selftest(struct tpm_chip *chip)
 	unsigned int loops;
 	unsigned int delay_msec = 100;
 	unsigned long duration;
-	struct tpm_cmd_t cmd;
+	u8 dummy[TPM_DIGEST_SIZE];
 
 	duration = tpm_calc_ordinal_duration(chip, TPM_ORD_CONTINUE_SELFTEST);
 
@@ -807,9 +800,8 @@ int tpm_do_selftest(struct tpm_chip *chip)
 
 	do {
 		/* Attempt to read a PCR value */
-		cmd.header.in = pcrread_header;
-		cmd.params.pcrread_in.pcr_idx = cpu_to_be32(0);
-		rc = tpm_transmit(chip, (u8 *) &cmd, READ_PCR_RESULT_SIZE);
+		rc = tpm_pcr_read_dev(chip, 0, dummy);
+
 		/* Some buggy TPMs will not respond to tpm_tis_ready() for
 		 * around 300ms while the self test is ongoing, keep trying
 		 * until the self test duration expires. */
@@ -824,7 +816,6 @@ int tpm_do_selftest(struct tpm_chip *chip)
 		if (rc < TPM_HEADER_SIZE)
 			return -EFAULT;
 
-		rc = be32_to_cpu(cmd.header.out.return_code);
 		if (rc == TPM_ERR_DISABLED || rc == TPM_ERR_DEACTIVATED) {
 			dev_info(&chip->dev,
 				 "TPM is disabled/deactivated (0x%X)\n", rc);
@@ -879,7 +870,7 @@ int tpm_send(u32 chip_num, void *cmd, size_t buflen)
 	if (chip == NULL)
 		return -ENODEV;
 
-	rc = tpm_transmit_cmd(chip, cmd, buflen, "attempting tpm_cmd");
+	rc = tpm_transmit_cmd(chip, cmd, buflen, 0, "attempting tpm_cmd");
 
 	tpm_put_ops(chip);
 	return rc;
@@ -949,7 +940,7 @@ EXPORT_SYMBOL_GPL(wait_for_tpm_stat);
 #define TPM_ORD_SAVESTATE cpu_to_be32(152)
 #define SAVESTATE_RESULT_SIZE 10
 
-static struct tpm_input_header savestate_header = {
+static const struct tpm_input_header savestate_header = {
 	.tag = TPM_TAG_RQU_COMMAND,
 	.length = cpu_to_be32(10),
 	.ordinal = TPM_ORD_SAVESTATE
@@ -981,14 +972,15 @@ int tpm_pm_suspend(struct device *dev)
 		cmd.params.pcrextend_in.pcr_idx = cpu_to_be32(tpm_suspend_pcr);
 		memcpy(cmd.params.pcrextend_in.hash, dummy_hash,
 		       TPM_DIGEST_SIZE);
-		rc = tpm_transmit_cmd(chip, &cmd, EXTEND_PCR_RESULT_SIZE,
+		rc = tpm_transmit_cmd(chip, &cmd, EXTEND_PCR_RESULT_SIZE, 0,
 				      "extending dummy pcr before suspend");
 	}
 
 	/* now do the actual savestate */
 	for (try = 0; try < TPM_RETRY; try++) {
 		cmd.header.in = savestate_header;
-		rc = tpm_transmit_cmd(chip, &cmd, SAVESTATE_RESULT_SIZE, NULL);
+		rc = tpm_transmit_cmd(chip, &cmd, SAVESTATE_RESULT_SIZE, 0,
+				      NULL);
 
 		/*
 		 * If the TPM indicates that it is too busy to respond to
@@ -1032,7 +1024,7 @@ int tpm_pm_resume(struct device *dev)
 EXPORT_SYMBOL_GPL(tpm_pm_resume);
 
 #define TPM_GETRANDOM_RESULT_SIZE	18
-static struct tpm_input_header tpm_getrandom_header = {
+static const struct tpm_input_header tpm_getrandom_header = {
 	.tag = TPM_TAG_RQU_COMMAND,
 	.length = cpu_to_be32(14),
 	.ordinal = TPM_ORD_GET_RANDOM
@@ -1072,8 +1064,8 @@ int tpm_get_random(u32 chip_num, u8 *out, size_t max)
 		tpm_cmd.params.getrandom_in.num_bytes = cpu_to_be32(num_bytes);
 
 		err = tpm_transmit_cmd(chip, &tpm_cmd,
-				   TPM_GETRANDOM_RESULT_SIZE + num_bytes,
-				   "attempting get random");
+				       TPM_GETRANDOM_RESULT_SIZE + num_bytes,
+				       0, "attempting get random");
 		if (err)
 			break;
 
diff --git a/drivers/char/tpm/tpm-sysfs.c b/drivers/char/tpm/tpm-sysfs.c
index b46cf70c8b16..a76ab4af9fb2 100644
--- a/drivers/char/tpm/tpm-sysfs.c
+++ b/drivers/char/tpm/tpm-sysfs.c
@@ -22,7 +22,7 @@
 
 #define READ_PUBEK_RESULT_SIZE 314
 #define TPM_ORD_READPUBEK cpu_to_be32(124)
-static struct tpm_input_header tpm_readpubek_header = {
+static const struct tpm_input_header tpm_readpubek_header = {
 	.tag = TPM_TAG_RQU_COMMAND,
 	.length = cpu_to_be32(30),
 	.ordinal = TPM_ORD_READPUBEK
@@ -39,7 +39,7 @@ static ssize_t pubek_show(struct device *dev, struct device_attribute *attr,
 	struct tpm_chip *chip = to_tpm_chip(dev);
 
 	tpm_cmd.header.in = tpm_readpubek_header;
-	err = tpm_transmit_cmd(chip, &tpm_cmd, READ_PUBEK_RESULT_SIZE,
+	err = tpm_transmit_cmd(chip, &tpm_cmd, READ_PUBEK_RESULT_SIZE, 0,
 			       "attempting to read the PUBEK");
 	if (err)
 		goto out;
diff --git a/drivers/char/tpm/tpm.h b/drivers/char/tpm/tpm.h
index 3e32d5bd2dc6..4d183c97f6a6 100644
--- a/drivers/char/tpm/tpm.h
+++ b/drivers/char/tpm/tpm.h
@@ -476,32 +476,35 @@ extern dev_t tpm_devt;
 extern const struct file_operations tpm_fops;
 extern struct idr dev_nums_idr;
 
+enum tpm_transmit_flags {
+	TPM_TRANSMIT_UNLOCKED	= BIT(0),
+};
+
+ssize_t tpm_transmit(struct tpm_chip *chip, const u8 *buf, size_t bufsiz,
+		     unsigned int flags);
+ssize_t tpm_transmit_cmd(struct tpm_chip *chip, const void *cmd, int len,
+			 unsigned int flags, const char *desc);
 ssize_t tpm_getcap(struct tpm_chip *chip, __be32 subcap_id, cap_t *cap,
 		   const char *desc);
-ssize_t tpm_transmit(struct tpm_chip *chip, const char *buf,
-		     size_t bufsiz);
-ssize_t tpm_transmit_cmd(struct tpm_chip *chip, void *cmd, int len,
-			 const char *desc);
-extern int tpm_get_timeouts(struct tpm_chip *);
-extern void tpm_gen_interrupt(struct tpm_chip *);
+int tpm_get_timeouts(struct tpm_chip *);
 int tpm1_auto_startup(struct tpm_chip *chip);
-extern int tpm_do_selftest(struct tpm_chip *);
-extern unsigned long tpm_calc_ordinal_duration(struct tpm_chip *, u32);
-extern int tpm_pm_suspend(struct device *);
-extern int tpm_pm_resume(struct device *);
-extern int wait_for_tpm_stat(struct tpm_chip *, u8, unsigned long,
-			     wait_queue_head_t *, bool);
+int tpm_do_selftest(struct tpm_chip *chip);
+unsigned long tpm_calc_ordinal_duration(struct tpm_chip *chip, u32 ordinal);
+int tpm_pm_suspend(struct device *dev);
+int tpm_pm_resume(struct device *dev);
+int wait_for_tpm_stat(struct tpm_chip *chip, u8 mask, unsigned long timeout,
+		      wait_queue_head_t *queue, bool check_cancel);
 
 struct tpm_chip *tpm_chip_find_get(int chip_num);
 __must_check int tpm_try_get_ops(struct tpm_chip *chip);
 void tpm_put_ops(struct tpm_chip *chip);
 
-extern struct tpm_chip *tpm_chip_alloc(struct device *dev,
-				       const struct tpm_class_ops *ops);
-extern struct tpm_chip *tpmm_chip_alloc(struct device *pdev,
-				       const struct tpm_class_ops *ops);
-extern int tpm_chip_register(struct tpm_chip *chip);
-extern void tpm_chip_unregister(struct tpm_chip *chip);
+struct tpm_chip *tpm_chip_alloc(struct device *dev,
+				const struct tpm_class_ops *ops);
+struct tpm_chip *tpmm_chip_alloc(struct device *pdev,
+				 const struct tpm_class_ops *ops);
+int tpm_chip_register(struct tpm_chip *chip);
+void tpm_chip_unregister(struct tpm_chip *chip);
 
 void tpm_sysfs_add_device(struct tpm_chip *chip);
 
@@ -528,8 +531,7 @@ ssize_t tpm2_get_tpm_pt(struct tpm_chip *chip, u32 property_id,
 			u32 *value, const char *desc);
 
 int tpm2_auto_startup(struct tpm_chip *chip);
-extern void tpm2_shutdown(struct tpm_chip *chip, u16 shutdown_type);
-extern unsigned long tpm2_calc_ordinal_duration(struct tpm_chip *, u32);
-extern int tpm2_gen_interrupt(struct tpm_chip *chip);
-extern int tpm2_probe(struct tpm_chip *chip);
+void tpm2_shutdown(struct tpm_chip *chip, u16 shutdown_type);
+unsigned long tpm2_calc_ordinal_duration(struct tpm_chip *chip, u32 ordinal);
+int tpm2_probe(struct tpm_chip *chip);
 #endif
diff --git a/drivers/char/tpm/tpm2-cmd.c b/drivers/char/tpm/tpm2-cmd.c
index 0c75c3f1689f..7df55d58c939 100644
--- a/drivers/char/tpm/tpm2-cmd.c
+++ b/drivers/char/tpm/tpm2-cmd.c
@@ -282,7 +282,7 @@ int tpm2_pcr_read(struct tpm_chip *chip, int pcr_idx, u8 *res_buf)
 	       sizeof(cmd.params.pcrread_in.pcr_select));
 	cmd.params.pcrread_in.pcr_select[pcr_idx >> 3] = 1 << (pcr_idx & 0x7);
 
-	rc = tpm_transmit_cmd(chip, &cmd, sizeof(cmd),
+	rc = tpm_transmit_cmd(chip, &cmd, sizeof(cmd), 0,
 			      "attempting to read a pcr value");
 	if (rc == 0) {
 		buf = cmd.params.pcrread_out.digest;
@@ -330,7 +330,7 @@ int tpm2_pcr_extend(struct tpm_chip *chip, int pcr_idx, const u8 *hash)
 	cmd.params.pcrextend_in.hash_alg = cpu_to_be16(TPM2_ALG_SHA1);
 	memcpy(cmd.params.pcrextend_in.digest, hash, TPM_DIGEST_SIZE);
 
-	rc = tpm_transmit_cmd(chip, &cmd, sizeof(cmd),
+	rc = tpm_transmit_cmd(chip, &cmd, sizeof(cmd), 0,
 			      "attempting extend a PCR value");
 
 	return rc;
@@ -376,7 +376,7 @@ int tpm2_get_random(struct tpm_chip *chip, u8 *out, size_t max)
 		cmd.header.in = tpm2_getrandom_header;
 		cmd.params.getrandom_in.size = cpu_to_be16(num_bytes);
 
-		err = tpm_transmit_cmd(chip, &cmd, sizeof(cmd),
+		err = tpm_transmit_cmd(chip, &cmd, sizeof(cmd), 0,
 				       "attempting get random");
 		if (err)
 			break;
@@ -434,12 +434,12 @@ static void tpm2_buf_append_auth(struct tpm_buf *buf, u32 session_handle,
 }
 
 /**
- * tpm2_seal_trusted() - seal a trusted key
- * @chip_num: A specific chip number for the request or TPM_ANY_NUM
- * @options: authentication values and other options
+ * tpm2_seal_trusted() - seal the payload of a trusted key
+ * @chip_num: TPM chip to use
  * @payload: the key data in clear and encrypted form
+ * @options: authentication values and other options
  *
- * Returns < 0 on error and 0 on success.
+ * Return: < 0 on error and 0 on success.
  */
 int tpm2_seal_trusted(struct tpm_chip *chip,
 		      struct trusted_key_payload *payload,
@@ -512,7 +512,7 @@ int tpm2_seal_trusted(struct tpm_chip *chip,
 		goto out;
 	}
 
-	rc = tpm_transmit_cmd(chip, buf.data, PAGE_SIZE, "sealing data");
+	rc = tpm_transmit_cmd(chip, buf.data, PAGE_SIZE, 0, "sealing data");
 	if (rc)
 		goto out;
 
@@ -538,10 +538,18 @@ out:
 	return rc;
 }
 
-static int tpm2_load(struct tpm_chip *chip,
-		     struct trusted_key_payload *payload,
-		     struct trusted_key_options *options,
-		     u32 *blob_handle)
+/**
+ * tpm2_load_cmd() - execute a TPM2_Load command
+ * @chip_num: TPM chip to use
+ * @payload: the key data in clear and encrypted form
+ * @options: authentication values and other options
+ *
+ * Return: same as with tpm_transmit_cmd
+ */
+static int tpm2_load_cmd(struct tpm_chip *chip,
+			 struct trusted_key_payload *payload,
+			 struct trusted_key_options *options,
+			 u32 *blob_handle, unsigned int flags)
 {
 	struct tpm_buf buf;
 	unsigned int private_len;
@@ -576,7 +584,7 @@ static int tpm2_load(struct tpm_chip *chip,
 		goto out;
 	}
 
-	rc = tpm_transmit_cmd(chip, buf.data, PAGE_SIZE, "loading blob");
+	rc = tpm_transmit_cmd(chip, buf.data, PAGE_SIZE, flags, "loading blob");
 	if (!rc)
 		*blob_handle = be32_to_cpup(
 			(__be32 *) &buf.data[TPM_HEADER_SIZE]);
@@ -590,7 +598,16 @@ out:
 	return rc;
 }
 
-static void tpm2_flush_context(struct tpm_chip *chip, u32 handle)
+/**
+ * tpm2_flush_context_cmd() - execute a TPM2_FlushContext command
+ * @chip_num: TPM chip to use
+ * @payload: the key data in clear and encrypted form
+ * @options: authentication values and other options
+ *
+ * Return: same as with tpm_transmit_cmd
+ */
+static void tpm2_flush_context_cmd(struct tpm_chip *chip, u32 handle,
+				   unsigned int flags)
 {
 	struct tpm_buf buf;
 	int rc;
@@ -604,7 +621,8 @@ static void tpm2_flush_context(struct tpm_chip *chip, u32 handle)
 
 	tpm_buf_append_u32(&buf, handle);
 
-	rc = tpm_transmit_cmd(chip, buf.data, PAGE_SIZE, "flushing context");
+	rc = tpm_transmit_cmd(chip, buf.data, PAGE_SIZE, flags,
+			      "flushing context");
 	if (rc)
 		dev_warn(&chip->dev, "0x%08x was not flushed, rc=%d\n", handle,
 			 rc);
@@ -612,10 +630,18 @@ static void tpm2_flush_context(struct tpm_chip *chip, u32 handle)
 	tpm_buf_destroy(&buf);
 }
 
-static int tpm2_unseal(struct tpm_chip *chip,
-		       struct trusted_key_payload *payload,
-		       struct trusted_key_options *options,
-		       u32 blob_handle)
+/**
+ * tpm2_unseal_cmd() - execute a TPM2_Unload command
+ * @chip_num: TPM chip to use
+ * @payload: the key data in clear and encrypted form
+ * @options: authentication values and other options
+ *
+ * Return: same as with tpm_transmit_cmd
+ */
+static int tpm2_unseal_cmd(struct tpm_chip *chip,
+			   struct trusted_key_payload *payload,
+			   struct trusted_key_options *options,
+			   u32 blob_handle, unsigned int flags)
 {
 	struct tpm_buf buf;
 	u16 data_len;
@@ -635,7 +661,7 @@ static int tpm2_unseal(struct tpm_chip *chip,
 			     options->blobauth /* hmac */,
 			     TPM_DIGEST_SIZE);
 
-	rc = tpm_transmit_cmd(chip, buf.data, PAGE_SIZE, "unsealing");
+	rc = tpm_transmit_cmd(chip, buf.data, PAGE_SIZE, flags, "unsealing");
 	if (rc > 0)
 		rc = -EPERM;
 
@@ -654,12 +680,12 @@ static int tpm2_unseal(struct tpm_chip *chip,
 }
 
 /**
- * tpm_unseal_trusted() - unseal a trusted key
- * @chip_num: A specific chip number for the request or TPM_ANY_NUM
- * @options: authentication values and other options
+ * tpm_unseal_trusted() - unseal the payload of a trusted key
+ * @chip_num: TPM chip to use
  * @payload: the key data in clear and encrypted form
+ * @options: authentication values and other options
  *
- * Returns < 0 on error and 0 on success.
+ * Return: < 0 on error and 0 on success.
  */
 int tpm2_unseal_trusted(struct tpm_chip *chip,
 			struct trusted_key_payload *payload,
@@ -668,14 +694,17 @@ int tpm2_unseal_trusted(struct tpm_chip *chip,
 	u32 blob_handle;
 	int rc;
 
-	rc = tpm2_load(chip, payload, options, &blob_handle);
+	mutex_lock(&chip->tpm_mutex);
+	rc = tpm2_load_cmd(chip, payload, options, &blob_handle,
+			   TPM_TRANSMIT_UNLOCKED);
 	if (rc)
-		return rc;
-
-	rc = tpm2_unseal(chip, payload, options, blob_handle);
-
-	tpm2_flush_context(chip, blob_handle);
+		goto out;
 
+	rc = tpm2_unseal_cmd(chip, payload, options, blob_handle,
+			     TPM_TRANSMIT_UNLOCKED);
+	tpm2_flush_context_cmd(chip, blob_handle, TPM_TRANSMIT_UNLOCKED);
+out:
+	mutex_unlock(&chip->tpm_mutex);
 	return rc;
 }
 
@@ -701,12 +730,13 @@ ssize_t tpm2_get_tpm_pt(struct tpm_chip *chip, u32 property_id,  u32 *value,
 	cmd.params.get_tpm_pt_in.property_id = cpu_to_be32(property_id);
 	cmd.params.get_tpm_pt_in.property_cnt = cpu_to_be32(1);
 
-	rc = tpm_transmit_cmd(chip, &cmd, sizeof(cmd), desc);
+	rc = tpm_transmit_cmd(chip, &cmd, sizeof(cmd), 0, desc);
 	if (!rc)
 		*value = be32_to_cpu(cmd.params.get_tpm_pt_out.value);
 
 	return rc;
 }
+EXPORT_SYMBOL_GPL(tpm2_get_tpm_pt);
 
 #define TPM2_STARTUP_IN_SIZE \
 	(sizeof(struct tpm_input_header) + \
@@ -735,7 +765,7 @@ static int tpm2_startup(struct tpm_chip *chip, u16 startup_type)
 	cmd.header.in = tpm2_startup_header;
 
 	cmd.params.startup_in.startup_type = cpu_to_be16(startup_type);
-	return tpm_transmit_cmd(chip, &cmd, sizeof(cmd),
+	return tpm_transmit_cmd(chip, &cmd, sizeof(cmd), 0,
 				"attempting to start the TPM");
 }
 
@@ -763,7 +793,7 @@ void tpm2_shutdown(struct tpm_chip *chip, u16 shutdown_type)
 	cmd.header.in = tpm2_shutdown_header;
 	cmd.params.startup_in.startup_type = cpu_to_be16(shutdown_type);
 
-	rc = tpm_transmit_cmd(chip, &cmd, sizeof(cmd), "stopping the TPM");
+	rc = tpm_transmit_cmd(chip, &cmd, sizeof(cmd), 0, "stopping the TPM");
 
 	/* In places where shutdown command is sent there's no much we can do
 	 * except print the error code on a system failure.
@@ -828,7 +858,7 @@ static int tpm2_start_selftest(struct tpm_chip *chip, bool full)
 	cmd.header.in = tpm2_selftest_header;
 	cmd.params.selftest_in.full_test = full;
 
-	rc = tpm_transmit_cmd(chip, &cmd, TPM2_SELF_TEST_IN_SIZE,
+	rc = tpm_transmit_cmd(chip, &cmd, TPM2_SELF_TEST_IN_SIZE, 0,
 			      "continue selftest");
 
 	/* At least some prototype chips seem to give RC_TESTING error
@@ -880,7 +910,7 @@ static int tpm2_do_selftest(struct tpm_chip *chip)
 		cmd.params.pcrread_in.pcr_select[1] = 0x00;
 		cmd.params.pcrread_in.pcr_select[2] = 0x00;
 
-		rc = tpm_transmit_cmd(chip, (u8 *) &cmd, sizeof(cmd), NULL);
+		rc = tpm_transmit_cmd(chip, &cmd, sizeof(cmd), 0, NULL);
 		if (rc < 0)
 			break;
 
@@ -895,23 +925,6 @@ static int tpm2_do_selftest(struct tpm_chip *chip)
 }
 
 /**
- * tpm2_gen_interrupt() - generate an interrupt
- * @chip: TPM chip to use
- *
- * 0 is returned when the operation is successful. If a negative number is
- * returned it remarks a POSIX error code. If a positive number is returned
- * it remarks a TPM error.
- */
-int tpm2_gen_interrupt(struct tpm_chip *chip)
-{
-	u32 dummy;
-
-	return tpm2_get_tpm_pt(chip, 0x100, &dummy,
-			       "attempting to generate an interrupt");
-}
-EXPORT_SYMBOL_GPL(tpm2_gen_interrupt);
-
-/**
  * tpm2_probe() - probe TPM 2.0
  * @chip: TPM chip to use
  *
@@ -928,11 +941,9 @@ int tpm2_probe(struct tpm_chip *chip)
 	cmd.params.get_tpm_pt_in.property_id = cpu_to_be32(0x100);
 	cmd.params.get_tpm_pt_in.property_cnt = cpu_to_be32(1);
 
-	rc = tpm_transmit(chip, (const char *) &cmd, sizeof(cmd));
+	rc = tpm_transmit_cmd(chip, &cmd, sizeof(cmd),  0, NULL);
 	if (rc <  0)
 		return rc;
-	else if (rc < TPM_HEADER_SIZE)
-		return -EFAULT;
 
 	if (be16_to_cpu(cmd.header.out.tag) == TPM2_ST_NO_SESSIONS)
 		chip->flags |= TPM_CHIP_FLAG_TPM2;
diff --git a/drivers/char/tpm/tpm_crb.c b/drivers/char/tpm/tpm_crb.c
index 018c382554ba..a7c870af916c 100644
--- a/drivers/char/tpm/tpm_crb.c
+++ b/drivers/char/tpm/tpm_crb.c
@@ -19,7 +19,6 @@
 #include <linux/highmem.h>
 #include <linux/rculist.h>
 #include <linux/module.h>
-#include <linux/platform_device.h>
 #include "tpm.h"
 
 #define ACPI_SIG_TPM2 "TPM2"
@@ -34,14 +33,14 @@ enum crb_defaults {
 	CRB_ACPI_START_INDEX = 1,
 };
 
-enum crb_ca_request {
-	CRB_CA_REQ_GO_IDLE	= BIT(0),
-	CRB_CA_REQ_CMD_READY	= BIT(1),
+enum crb_ctrl_req {
+	CRB_CTRL_REQ_CMD_READY	= BIT(0),
+	CRB_CTRL_REQ_GO_IDLE	= BIT(1),
 };
 
-enum crb_ca_status {
-	CRB_CA_STS_ERROR	= BIT(0),
-	CRB_CA_STS_TPM_IDLE	= BIT(1),
+enum crb_ctrl_sts {
+	CRB_CTRL_STS_ERROR	= BIT(0),
+	CRB_CTRL_STS_TPM_IDLE	= BIT(1),
 };
 
 enum crb_start {
@@ -67,7 +66,7 @@ struct crb_control_area {
 } __packed;
 
 enum crb_status {
-	CRB_STS_COMPLETE	= BIT(0),
+	CRB_DRV_STS_COMPLETE	= BIT(0),
 };
 
 enum crb_flags {
@@ -81,6 +80,7 @@ struct crb_priv {
 	struct crb_control_area __iomem *cca;
 	u8 __iomem *cmd;
 	u8 __iomem *rsp;
+	u32 cmd_size;
 };
 
 static SIMPLE_DEV_PM_OPS(crb_pm, tpm_pm_suspend, tpm_pm_resume);
@@ -92,7 +92,7 @@ static u8 crb_status(struct tpm_chip *chip)
 
 	if ((ioread32(&priv->cca->start) & CRB_START_INVOKE) !=
 	    CRB_START_INVOKE)
-		sts |= CRB_STS_COMPLETE;
+		sts |= CRB_DRV_STS_COMPLETE;
 
 	return sts;
 }
@@ -106,7 +106,7 @@ static int crb_recv(struct tpm_chip *chip, u8 *buf, size_t count)
 	if (count < 6)
 		return -EIO;
 
-	if (ioread32(&priv->cca->sts) & CRB_CA_STS_ERROR)
+	if (ioread32(&priv->cca->sts) & CRB_CTRL_STS_ERROR)
 		return -EIO;
 
 	memcpy_fromio(buf, priv->rsp, 6);
@@ -142,11 +142,14 @@ static int crb_send(struct tpm_chip *chip, u8 *buf, size_t len)
 	struct crb_priv *priv = dev_get_drvdata(&chip->dev);
 	int rc = 0;
 
-	if (len > ioread32(&priv->cca->cmd_size)) {
-		dev_err(&chip->dev,
-			"invalid command count value %x %zx\n",
-			(unsigned int) len,
-			(size_t) ioread32(&priv->cca->cmd_size));
+	/* Zero the cancel register so that the next command will not get
+	 * canceled.
+	 */
+	iowrite32(0, &priv->cca->cancel);
+
+	if (len > priv->cmd_size) {
+		dev_err(&chip->dev, "invalid command count value %zd %d\n",
+			len, priv->cmd_size);
 		return -E2BIG;
 	}
 
@@ -156,7 +159,7 @@ static int crb_send(struct tpm_chip *chip, u8 *buf, size_t len)
 	wmb();
 
 	if (priv->flags & CRB_FL_CRB_START)
-		iowrite32(cpu_to_le32(CRB_START_INVOKE), &priv->cca->start);
+		iowrite32(CRB_START_INVOKE, &priv->cca->start);
 
 	if (priv->flags & CRB_FL_ACPI_START)
 		rc = crb_do_acpi_start(chip);
@@ -168,15 +171,10 @@ static void crb_cancel(struct tpm_chip *chip)
 {
 	struct crb_priv *priv = dev_get_drvdata(&chip->dev);
 
-	iowrite32(cpu_to_le32(CRB_CANCEL_INVOKE), &priv->cca->cancel);
-
-	/* Make sure that cmd is populated before issuing cancel. */
-	wmb();
+	iowrite32(CRB_CANCEL_INVOKE, &priv->cca->cancel);
 
 	if ((priv->flags & CRB_FL_ACPI_START) && crb_do_acpi_start(chip))
 		dev_err(&chip->dev, "ACPI Start failed\n");
-
-	iowrite32(0, &priv->cca->cancel);
 }
 
 static bool crb_req_canceled(struct tpm_chip *chip, u8 status)
@@ -194,8 +192,8 @@ static const struct tpm_class_ops tpm_crb = {
 	.send = crb_send,
 	.cancel = crb_cancel,
 	.req_canceled = crb_req_canceled,
-	.req_complete_mask = CRB_STS_COMPLETE,
-	.req_complete_val = CRB_STS_COMPLETE,
+	.req_complete_mask = CRB_DRV_STS_COMPLETE,
+	.req_complete_val = CRB_DRV_STS_COMPLETE,
 };
 
 static int crb_init(struct acpi_device *device, struct crb_priv *priv)
@@ -265,8 +263,7 @@ static int crb_map_io(struct acpi_device *device, struct crb_priv *priv,
 	acpi_dev_free_resource_list(&resources);
 
 	if (resource_type(&io_res) != IORESOURCE_MEM) {
-		dev_err(dev,
-			FW_BUG "TPM2 ACPI table does not define a memory resource\n");
+		dev_err(dev, FW_BUG "TPM2 ACPI table does not define a memory resource\n");
 		return -EINVAL;
 	}
 
@@ -302,6 +299,7 @@ static int crb_map_io(struct acpi_device *device, struct crb_priv *priv,
 		dev_err(dev, FW_BUG "overlapping command and response buffer sizes are not identical");
 		return -EINVAL;
 	}
+	priv->cmd_size = cmd_size;
 
 	priv->rsp = priv->cmd;
 	return 0;
diff --git a/drivers/char/tpm/tpm_tis_core.c b/drivers/char/tpm/tpm_tis_core.c
index d66f51b3648e..e3bf31b37138 100644
--- a/drivers/char/tpm/tpm_tis_core.c
+++ b/drivers/char/tpm/tpm_tis_core.c
@@ -440,7 +440,6 @@ static int probe_itpm(struct tpm_chip *chip)
 		0x00, 0x00, 0x00, 0xf1
 	};
 	size_t len = sizeof(cmd_getticks);
-	bool itpm;
 	u16 vendor;
 
 	rc = tpm_tis_read16(priv, TPM_DID_VID(0), &vendor);
@@ -451,8 +450,6 @@ static int probe_itpm(struct tpm_chip *chip)
 	if (vendor != TPM_VID_INTEL)
 		return 0;
 
-	itpm = false;
-
 	rc = tpm_tis_send_data(chip, cmd_getticks, len);
 	if (rc == 0)
 		goto out;
@@ -460,8 +457,6 @@ static int probe_itpm(struct tpm_chip *chip)
 	tpm_tis_ready(chip);
 	release_locality(chip, priv->locality, 0);
 
-	itpm = true;
-
 	rc = tpm_tis_send_data(chip, cmd_getticks, len);
 	if (rc == 0) {
 		dev_info(&chip->dev, "Detected an iTPM.\n");
@@ -526,6 +521,18 @@ static irqreturn_t tis_int_handler(int dummy, void *dev_id)
 	return IRQ_HANDLED;
 }
 
+static int tpm_tis_gen_interrupt(struct tpm_chip *chip)
+{
+	const char *desc = "attempting to generate an interrupt";
+	u32 cap2;
+	cap_t cap;
+
+	if (chip->flags & TPM_CHIP_FLAG_TPM2)
+		return tpm2_get_tpm_pt(chip, 0x100, &cap2, desc);
+	else
+		return tpm_getcap(chip, TPM_CAP_PROP_TIS_TIMEOUT, &cap, desc);
+}
+
 /* Register the IRQ and issue a command that will cause an interrupt. If an
  * irq is seen then leave the chip setup for IRQ operation, otherwise reverse
  * everything and leave in polling mode. Returns 0 on success.
@@ -575,10 +582,9 @@ static int tpm_tis_probe_irq_single(struct tpm_chip *chip, u32 intmask,
 	/* Generate an interrupt by having the core call through to
 	 * tpm_tis_send
 	 */
-	if (chip->flags & TPM_CHIP_FLAG_TPM2)
-		tpm2_gen_interrupt(chip);
-	else
-		tpm_gen_interrupt(chip);
+	rc = tpm_tis_gen_interrupt(chip);
+	if (rc < 0)
+		return rc;
 
 	/* tpm_tis_send will either confirm the interrupt is working or it
 	 * will call disable_irq which undoes all of the above.
diff --git a/drivers/char/ttyprintk.c b/drivers/char/ttyprintk.c
index b098d2d0b7c4..67549ce88cc9 100644
--- a/drivers/char/ttyprintk.c
+++ b/drivers/char/ttyprintk.c
@@ -31,60 +31,53 @@ static struct ttyprintk_port tpk_port;
  * printk messages (also suitable for logging service):
  * - any cr is replaced by nl
  * - adds a ttyprintk source tag in front of each line
- * - too long message is fragmeted, with '\'nl between fragments
- * - TPK_STR_SIZE isn't really the write_room limiting factor, bcause
+ * - too long message is fragmented, with '\'nl between fragments
+ * - TPK_STR_SIZE isn't really the write_room limiting factor, because
  *   it is emptied on the fly during preformatting.
  */
 #define TPK_STR_SIZE 508 /* should be bigger then max expected line length */
 #define TPK_MAX_ROOM 4096 /* we could assume 4K for instance */
-static const char *tpk_tag = "[U] "; /* U for User */
 static int tpk_curr;
 
+static char tpk_buffer[TPK_STR_SIZE + 4];
+
+static void tpk_flush(void)
+{
+	if (tpk_curr > 0) {
+		tpk_buffer[tpk_curr] = '\0';
+		pr_info("[U] %s\n", tpk_buffer);
+		tpk_curr = 0;
+	}
+}
+
 static int tpk_printk(const unsigned char *buf, int count)
 {
-	static char tmp[TPK_STR_SIZE + 4];
 	int i = tpk_curr;
 
 	if (buf == NULL) {
-		/* flush tmp[] */
-		if (tpk_curr > 0) {
-			/* non nl or cr terminated message - add nl */
-			tmp[tpk_curr + 0] = '\n';
-			tmp[tpk_curr + 1] = '\0';
-			printk(KERN_INFO "%s%s", tpk_tag, tmp);
-			tpk_curr = 0;
-		}
+		tpk_flush();
 		return i;
 	}
 
 	for (i = 0; i < count; i++) {
-		tmp[tpk_curr] = buf[i];
-		if (tpk_curr < TPK_STR_SIZE) {
-			switch (buf[i]) {
-			case '\r':
-				/* replace cr with nl */
-				tmp[tpk_curr + 0] = '\n';
-				tmp[tpk_curr + 1] = '\0';
-				printk(KERN_INFO "%s%s", tpk_tag, tmp);
-				tpk_curr = 0;
-				if ((i + 1) < count && buf[i + 1] == '\n')
-					i++;
-				break;
-			case '\n':
-				tmp[tpk_curr + 1] = '\0';
-				printk(KERN_INFO "%s%s", tpk_tag, tmp);
-				tpk_curr = 0;
-				break;
-			default:
-				tpk_curr++;
-			}
-		} else {
+		if (tpk_curr >= TPK_STR_SIZE) {
 			/* end of tmp buffer reached: cut the message in two */
-			tmp[tpk_curr + 1] = '\\';
-			tmp[tpk_curr + 2] = '\n';
-			tmp[tpk_curr + 3] = '\0';
-			printk(KERN_INFO "%s%s", tpk_tag, tmp);
-			tpk_curr = 0;
+			tpk_buffer[tpk_curr++] = '\\';
+			tpk_flush();
+		}
+
+		switch (buf[i]) {
+		case '\r':
+			tpk_flush();
+			if ((i + 1) < count && buf[i + 1] == '\n')
+				i++;
+			break;
+		case '\n':
+			tpk_flush();
+			break;
+		default:
+			tpk_buffer[tpk_curr++] = buf[i];
+			break;
 		}
 	}
 
diff --git a/drivers/char/virtio_console.c b/drivers/char/virtio_console.c
index 5da47e26a012..d433b1db1fdd 100644
--- a/drivers/char/virtio_console.c
+++ b/drivers/char/virtio_console.c
@@ -38,7 +38,6 @@
 #include <linux/workqueue.h>
 #include <linux/module.h>
 #include <linux/dma-mapping.h>
-#include <linux/kconfig.h>
 #include "../tty/hvc/hvc_console.h"
 
 #define is_rproc_enabled IS_ENABLED(CONFIG_REMOTEPROC)
@@ -889,7 +888,7 @@ static int pipe_to_sg(struct pipe_inode_info *pipe, struct pipe_buffer *buf,
 		return 0;
 
 	/* Try lock this page */
-	if (buf->ops->steal(pipe, buf) == 0) {
+	if (pipe_buf_steal(pipe, buf) == 0) {
 		/* Get reference and unlock page for moving */
 		get_page(buf->page);
 		unlock_page(buf->page);
diff --git a/drivers/char/xillybus/xillybus_core.c b/drivers/char/xillybus/xillybus_core.c
index dcd19f3f182e..b6c9cdead7f3 100644
--- a/drivers/char/xillybus/xillybus_core.c
+++ b/drivers/char/xillybus/xillybus_core.c
@@ -655,10 +655,10 @@ static int xilly_obtain_idt(struct xilly_endpoint *endpoint)
 
 	version = channel->wr_buffers[0]->addr;
 
-	/* Check version number. Accept anything below 0x82 for now. */
+	/* Check version number. Reject anything above 0x82. */
 	if (*version > 0x82) {
 		dev_err(endpoint->dev,
-			"No support for IDT version 0x%02x. Maybe the xillybus driver needs an upgarde. Aborting.\n",
+			"No support for IDT version 0x%02x. Maybe the xillybus driver needs an upgrade. Aborting.\n",
 			*version);
 		return -ENODEV;
 	}
diff --git a/drivers/clk/Kconfig b/drivers/clk/Kconfig
index e2d9bd760c84..6a8ac04bedeb 100644
--- a/drivers/clk/Kconfig
+++ b/drivers/clk/Kconfig
@@ -31,22 +31,12 @@ config COMMON_CLK_WM831X
 
 source "drivers/clk/versatile/Kconfig"
 
-config COMMON_CLK_MAX_GEN
-        bool
-
 config COMMON_CLK_MAX77686
-	tristate "Clock driver for Maxim 77686 MFD"
-	depends on MFD_MAX77686
-	select COMMON_CLK_MAX_GEN
-	---help---
-	  This driver supports Maxim 77686 crystal oscillator clock. 
-
-config COMMON_CLK_MAX77802
-	tristate "Clock driver for Maxim 77802 PMIC"
-	depends on MFD_MAX77686
-	select COMMON_CLK_MAX_GEN
+	tristate "Clock driver for Maxim 77620/77686/77802 MFD"
+	depends on MFD_MAX77686 || MFD_MAX77620
 	---help---
-	  This driver supports Maxim 77802 crystal oscillator clock.
+	  This driver supports Maxim 77620/77686/77802 crystal oscillator
+	  clock.
 
 config COMMON_CLK_RK808
 	tristate "Clock driver for RK808/RK818"
@@ -210,6 +200,7 @@ config COMMON_CLK_OXNAS
 
 source "drivers/clk/bcm/Kconfig"
 source "drivers/clk/hisilicon/Kconfig"
+source "drivers/clk/mediatek/Kconfig"
 source "drivers/clk/meson/Kconfig"
 source "drivers/clk/mvebu/Kconfig"
 source "drivers/clk/qcom/Kconfig"
@@ -218,5 +209,6 @@ source "drivers/clk/samsung/Kconfig"
 source "drivers/clk/sunxi-ng/Kconfig"
 source "drivers/clk/tegra/Kconfig"
 source "drivers/clk/ti/Kconfig"
+source "drivers/clk/uniphier/Kconfig"
 
 endmenu
diff --git a/drivers/clk/Makefile b/drivers/clk/Makefile
index 3b6f9cf3464a..925081ec14c0 100644
--- a/drivers/clk/Makefile
+++ b/drivers/clk/Makefile
@@ -26,10 +26,7 @@ obj-$(CONFIG_ARCH_CLPS711X)		+= clk-clps711x.o
 obj-$(CONFIG_COMMON_CLK_CS2000_CP)	+= clk-cs2000-cp.o
 obj-$(CONFIG_ARCH_EFM32)		+= clk-efm32gg.o
 obj-$(CONFIG_ARCH_HIGHBANK)		+= clk-highbank.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
 obj-$(CONFIG_ARCH_MB86S7X)		+= clk-mb86s7x.o
 obj-$(CONFIG_ARCH_MOXART)		+= clk-moxart.o
 obj-$(CONFIG_ARCH_NOMADIK)		+= clk-nomadik.o
@@ -63,6 +60,7 @@ obj-$(CONFIG_ARCH_HISI)			+= hisilicon/
 obj-$(CONFIG_ARCH_MXC)			+= imx/
 obj-$(CONFIG_MACH_INGENIC)		+= ingenic/
 obj-$(CONFIG_COMMON_CLK_KEYSTONE)	+= keystone/
+obj-$(CONFIG_MACH_LOONGSON32)		+= loongson1/
 obj-$(CONFIG_ARCH_MEDIATEK)		+= mediatek/
 obj-$(CONFIG_COMMON_CLK_AMLOGIC)	+= meson/
 obj-$(CONFIG_MACH_PIC32)		+= microchip/
@@ -86,6 +84,7 @@ obj-$(CONFIG_ARCH_SUNXI)		+= sunxi/
 obj-$(CONFIG_ARCH_SUNXI)		+= sunxi-ng/
 obj-$(CONFIG_ARCH_TEGRA)		+= tegra/
 obj-y					+= ti/
+obj-$(CONFIG_CLK_UNIPHIER)		+= uniphier/
 obj-$(CONFIG_ARCH_U8500)		+= ux500/
 obj-$(CONFIG_COMMON_CLK_VERSATILE)	+= versatile/
 obj-$(CONFIG_X86)			+= x86/
diff --git a/drivers/clk/at91/clk-generated.c b/drivers/clk/at91/clk-generated.c
index 7f6bec8837ea..4e1cd5aa69d8 100644
--- a/drivers/clk/at91/clk-generated.c
+++ b/drivers/clk/at91/clk-generated.c
@@ -233,14 +233,16 @@ static void clk_generated_startup(struct clk_generated *gck)
 					>> AT91_PMC_PCR_GCKDIV_OFFSET;
 }
 
-static struct clk * __init
-at91_clk_register_generated(struct regmap *regmap,  spinlock_t *lock, const char
-			    *name, const char **parent_names, u8 num_parents,
-			    u8 id, const struct clk_range *range)
+static struct clk_hw * __init
+at91_clk_register_generated(struct regmap *regmap, spinlock_t *lock,
+			    const char *name, const char **parent_names,
+			    u8 num_parents, u8 id,
+			    const struct clk_range *range)
 {
 	struct clk_generated *gck;
-	struct clk *clk = NULL;
 	struct clk_init_data init;
+	struct clk_hw *hw;
+	int ret;
 
 	gck = kzalloc(sizeof(*gck), GFP_KERNEL);
 	if (!gck)
@@ -258,13 +260,15 @@ at91_clk_register_generated(struct regmap *regmap,  spinlock_t *lock, const char
 	gck->lock = lock;
 	gck->range = *range;
 
-	clk = clk_register(NULL, &gck->hw);
-	if (IS_ERR(clk))
+	hw = &gck->hw;
+	ret = clk_hw_register(NULL, &gck->hw);
+	if (ret) {
 		kfree(gck);
-	else
+		hw = ERR_PTR(ret);
+	} else
 		clk_generated_startup(gck);
 
-	return clk;
+	return hw;
 }
 
 static void __init of_sama5d2_clk_generated_setup(struct device_node *np)
@@ -272,7 +276,7 @@ static void __init of_sama5d2_clk_generated_setup(struct device_node *np)
 	int num;
 	u32 id;
 	const char *name;
-	struct clk *clk;
+	struct clk_hw *hw;
 	unsigned int num_parents;
 	const char *parent_names[GENERATED_SOURCE_MAX];
 	struct device_node *gcknp;
@@ -306,13 +310,13 @@ static void __init of_sama5d2_clk_generated_setup(struct device_node *np)
 		of_at91_get_clk_range(gcknp, "atmel,clk-output-range",
 				      &range);
 
-		clk = at91_clk_register_generated(regmap, &pmc_pcr_lock, name,
+		hw = at91_clk_register_generated(regmap, &pmc_pcr_lock, name,
 						  parent_names, num_parents,
 						  id, &range);
-		if (IS_ERR(clk))
+		if (IS_ERR(hw))
 			continue;
 
-		of_clk_add_provider(gcknp, of_clk_src_simple_get, clk);
+		of_clk_add_hw_provider(gcknp, of_clk_hw_simple_get, hw);
 	}
 }
 CLK_OF_DECLARE(of_sama5d2_clk_generated_setup, "atmel,sama5d2-clk-generated",
diff --git a/drivers/clk/at91/clk-h32mx.c b/drivers/clk/at91/clk-h32mx.c
index 8e20c8a76db7..e0daa4a31f88 100644
--- a/drivers/clk/at91/clk-h32mx.c
+++ b/drivers/clk/at91/clk-h32mx.c
@@ -92,7 +92,7 @@ static void __init of_sama5d4_clk_h32mx_setup(struct device_node *np)
 	struct clk_init_data init;
 	const char *parent_name;
 	struct regmap *regmap;
-	struct clk *clk;
+	int ret;
 
 	regmap = syscon_node_to_regmap(of_get_parent(np));
 	if (IS_ERR(regmap))
@@ -113,13 +113,13 @@ static void __init of_sama5d4_clk_h32mx_setup(struct device_node *np)
 	h32mxclk->hw.init = &init;
 	h32mxclk->regmap = regmap;
 
-	clk = clk_register(NULL, &h32mxclk->hw);
-	if (IS_ERR(clk)) {
+	ret = clk_hw_register(NULL, &h32mxclk->hw);
+	if (ret) {
 		kfree(h32mxclk);
 		return;
 	}
 
-	of_clk_add_provider(np, of_clk_src_simple_get, clk);
+	of_clk_add_hw_provider(np, of_clk_hw_simple_get, &h32mxclk->hw);
 }
 CLK_OF_DECLARE(of_sama5d4_clk_h32mx_setup, "atmel,sama5d4-clk-h32mx",
 	       of_sama5d4_clk_h32mx_setup);
diff --git a/drivers/clk/at91/clk-main.c b/drivers/clk/at91/clk-main.c
index 58b5baca670c..c813c27f2e58 100644
--- a/drivers/clk/at91/clk-main.c
+++ b/drivers/clk/at91/clk-main.c
@@ -128,15 +128,16 @@ static const struct clk_ops main_osc_ops = {
 	.is_prepared = clk_main_osc_is_prepared,
 };
 
-static struct clk * __init
+static struct clk_hw * __init
 at91_clk_register_main_osc(struct regmap *regmap,
 			   const char *name,
 			   const char *parent_name,
 			   bool bypass)
 {
 	struct clk_main_osc *osc;
-	struct clk *clk = NULL;
 	struct clk_init_data init;
+	struct clk_hw *hw;
+	int ret;
 
 	if (!name || !parent_name)
 		return ERR_PTR(-EINVAL);
@@ -160,16 +161,19 @@ at91_clk_register_main_osc(struct regmap *regmap,
 				   AT91_PMC_MOSCEN,
 				   AT91_PMC_OSCBYPASS | AT91_PMC_KEY);
 
-	clk = clk_register(NULL, &osc->hw);
-	if (IS_ERR(clk))
+	hw = &osc->hw;
+	ret = clk_hw_register(NULL, &osc->hw);
+	if (ret) {
 		kfree(osc);
+		hw = ERR_PTR(ret);
+	}
 
-	return clk;
+	return hw;
 }
 
 static void __init of_at91rm9200_clk_main_osc_setup(struct device_node *np)
 {
-	struct clk *clk;
+	struct clk_hw *hw;
 	const char *name = np->name;
 	const char *parent_name;
 	struct regmap *regmap;
@@ -183,11 +187,11 @@ static void __init of_at91rm9200_clk_main_osc_setup(struct device_node *np)
 	if (IS_ERR(regmap))
 		return;
 
-	clk = at91_clk_register_main_osc(regmap, name, parent_name, bypass);
-	if (IS_ERR(clk))
+	hw = at91_clk_register_main_osc(regmap, name, parent_name, bypass);
+	if (IS_ERR(hw))
 		return;
 
-	of_clk_add_provider(np, of_clk_src_simple_get, clk);
+	of_clk_add_hw_provider(np, of_clk_hw_simple_get, hw);
 }
 CLK_OF_DECLARE(at91rm9200_clk_main_osc, "atmel,at91rm9200-clk-main-osc",
 	       of_at91rm9200_clk_main_osc_setup);
@@ -271,14 +275,15 @@ static const struct clk_ops main_rc_osc_ops = {
 	.recalc_accuracy = clk_main_rc_osc_recalc_accuracy,
 };
 
-static struct clk * __init
+static struct clk_hw * __init
 at91_clk_register_main_rc_osc(struct regmap *regmap,
 			      const char *name,
 			      u32 frequency, u32 accuracy)
 {
 	struct clk_main_rc_osc *osc;
-	struct clk *clk = NULL;
 	struct clk_init_data init;
+	struct clk_hw *hw;
+	int ret;
 
 	if (!name || !frequency)
 		return ERR_PTR(-EINVAL);
@@ -298,16 +303,19 @@ at91_clk_register_main_rc_osc(struct regmap *regmap,
 	osc->frequency = frequency;
 	osc->accuracy = accuracy;
 
-	clk = clk_register(NULL, &osc->hw);
-	if (IS_ERR(clk))
+	hw = &osc->hw;
+	ret = clk_hw_register(NULL, hw);
+	if (ret) {
 		kfree(osc);
+		hw = ERR_PTR(ret);
+	}
 
-	return clk;
+	return hw;
 }
 
 static void __init of_at91sam9x5_clk_main_rc_osc_setup(struct device_node *np)
 {
-	struct clk *clk;
+	struct clk_hw *hw;
 	u32 frequency = 0;
 	u32 accuracy = 0;
 	const char *name = np->name;
@@ -321,11 +329,11 @@ static void __init of_at91sam9x5_clk_main_rc_osc_setup(struct device_node *np)
 	if (IS_ERR(regmap))
 		return;
 
-	clk = at91_clk_register_main_rc_osc(regmap, name, frequency, accuracy);
-	if (IS_ERR(clk))
+	hw = at91_clk_register_main_rc_osc(regmap, name, frequency, accuracy);
+	if (IS_ERR(hw))
 		return;
 
-	of_clk_add_provider(np, of_clk_src_simple_get, clk);
+	of_clk_add_hw_provider(np, of_clk_hw_simple_get, hw);
 }
 CLK_OF_DECLARE(at91sam9x5_clk_main_rc_osc, "atmel,at91sam9x5-clk-main-rc-osc",
 	       of_at91sam9x5_clk_main_rc_osc_setup);
@@ -395,14 +403,15 @@ static const struct clk_ops rm9200_main_ops = {
 	.recalc_rate = clk_rm9200_main_recalc_rate,
 };
 
-static struct clk * __init
+static struct clk_hw * __init
 at91_clk_register_rm9200_main(struct regmap *regmap,
 			      const char *name,
 			      const char *parent_name)
 {
 	struct clk_rm9200_main *clkmain;
-	struct clk *clk = NULL;
 	struct clk_init_data init;
+	struct clk_hw *hw;
+	int ret;
 
 	if (!name)
 		return ERR_PTR(-EINVAL);
@@ -423,16 +432,19 @@ at91_clk_register_rm9200_main(struct regmap *regmap,
 	clkmain->hw.init = &init;
 	clkmain->regmap = regmap;
 
-	clk = clk_register(NULL, &clkmain->hw);
-	if (IS_ERR(clk))
+	hw = &clkmain->hw;
+	ret = clk_hw_register(NULL, &clkmain->hw);
+	if (ret) {
 		kfree(clkmain);
+		hw = ERR_PTR(ret);
+	}
 
-	return clk;
+	return hw;
 }
 
 static void __init of_at91rm9200_clk_main_setup(struct device_node *np)
 {
-	struct clk *clk;
+	struct clk_hw *hw;
 	const char *parent_name;
 	const char *name = np->name;
 	struct regmap *regmap;
@@ -444,11 +456,11 @@ static void __init of_at91rm9200_clk_main_setup(struct device_node *np)
 	if (IS_ERR(regmap))
 		return;
 
-	clk = at91_clk_register_rm9200_main(regmap, name, parent_name);
-	if (IS_ERR(clk))
+	hw = at91_clk_register_rm9200_main(regmap, name, parent_name);
+	if (IS_ERR(hw))
 		return;
 
-	of_clk_add_provider(np, of_clk_src_simple_get, clk);
+	of_clk_add_hw_provider(np, of_clk_hw_simple_get, hw);
 }
 CLK_OF_DECLARE(at91rm9200_clk_main, "atmel,at91rm9200-clk-main",
 	       of_at91rm9200_clk_main_setup);
@@ -529,16 +541,17 @@ static const struct clk_ops sam9x5_main_ops = {
 	.get_parent = clk_sam9x5_main_get_parent,
 };
 
-static struct clk * __init
+static struct clk_hw * __init
 at91_clk_register_sam9x5_main(struct regmap *regmap,
 			      const char *name,
 			      const char **parent_names,
 			      int num_parents)
 {
 	struct clk_sam9x5_main *clkmain;
-	struct clk *clk = NULL;
 	struct clk_init_data init;
 	unsigned int status;
+	struct clk_hw *hw;
+	int ret;
 
 	if (!name)
 		return ERR_PTR(-EINVAL);
@@ -561,16 +574,19 @@ at91_clk_register_sam9x5_main(struct regmap *regmap,
 	regmap_read(clkmain->regmap, AT91_CKGR_MOR, &status);
 	clkmain->parent = status & AT91_PMC_MOSCEN ? 1 : 0;
 
-	clk = clk_register(NULL, &clkmain->hw);
-	if (IS_ERR(clk))
+	hw = &clkmain->hw;
+	ret = clk_hw_register(NULL, &clkmain->hw);
+	if (ret) {
 		kfree(clkmain);
+		hw = ERR_PTR(ret);
+	}
 
-	return clk;
+	return hw;
 }
 
 static void __init of_at91sam9x5_clk_main_setup(struct device_node *np)
 {
-	struct clk *clk;
+	struct clk_hw *hw;
 	const char *parent_names[2];
 	unsigned int num_parents;
 	const char *name = np->name;
@@ -587,12 +603,12 @@ static void __init of_at91sam9x5_clk_main_setup(struct device_node *np)
 
 	of_property_read_string(np, "clock-output-names", &name);
 
-	clk = at91_clk_register_sam9x5_main(regmap, name, parent_names,
+	hw = at91_clk_register_sam9x5_main(regmap, name, parent_names,
 					    num_parents);
-	if (IS_ERR(clk))
+	if (IS_ERR(hw))
 		return;
 
-	of_clk_add_provider(np, of_clk_src_simple_get, clk);
+	of_clk_add_hw_provider(np, of_clk_hw_simple_get, hw);
 }
 CLK_OF_DECLARE(at91sam9x5_clk_main, "atmel,at91sam9x5-clk-main",
 	       of_at91sam9x5_clk_main_setup);
diff --git a/drivers/clk/at91/clk-master.c b/drivers/clk/at91/clk-master.c
index d1021e106191..e9cba9fc26d7 100644
--- a/drivers/clk/at91/clk-master.c
+++ b/drivers/clk/at91/clk-master.c
@@ -120,7 +120,7 @@ static const struct clk_ops master_ops = {
 	.get_parent = clk_master_get_parent,
 };
 
-static struct clk * __init
+static struct clk_hw * __init
 at91_clk_register_master(struct regmap *regmap,
 		const char *name, int num_parents,
 		const char **parent_names,
@@ -128,8 +128,9 @@ at91_clk_register_master(struct regmap *regmap,
 		const struct clk_master_characteristics *characteristics)
 {
 	struct clk_master *master;
-	struct clk *clk = NULL;
 	struct clk_init_data init;
+	struct clk_hw *hw;
+	int ret;
 
 	if (!name || !num_parents || !parent_names)
 		return ERR_PTR(-EINVAL);
@@ -149,12 +150,14 @@ at91_clk_register_master(struct regmap *regmap,
 	master->characteristics = characteristics;
 	master->regmap = regmap;
 
-	clk = clk_register(NULL, &master->hw);
-	if (IS_ERR(clk)) {
+	hw = &master->hw;
+	ret = clk_hw_register(NULL, &master->hw);
+	if (ret) {
 		kfree(master);
+		hw = ERR_PTR(ret);
 	}
 
-	return clk;
+	return hw;
 }
 
 
@@ -198,7 +201,7 @@ static void __init
 of_at91_clk_master_setup(struct device_node *np,
 			 const struct clk_master_layout *layout)
 {
-	struct clk *clk;
+	struct clk_hw *hw;
 	unsigned int num_parents;
 	const char *parent_names[MASTER_SOURCE_MAX];
 	const char *name = np->name;
@@ -221,13 +224,13 @@ of_at91_clk_master_setup(struct device_node *np,
 	if (IS_ERR(regmap))
 		return;
 
-	clk = at91_clk_register_master(regmap, name, num_parents,
+	hw = at91_clk_register_master(regmap, name, num_parents,
 				       parent_names, layout,
 				       characteristics);
-	if (IS_ERR(clk))
+	if (IS_ERR(hw))
 		goto out_free_characteristics;
 
-	of_clk_add_provider(np, of_clk_src_simple_get, clk);
+	of_clk_add_hw_provider(np, of_clk_hw_simple_get, hw);
 	return;
 
 out_free_characteristics:
diff --git a/drivers/clk/at91/clk-peripheral.c b/drivers/clk/at91/clk-peripheral.c
index fd160728e990..dc29fd979d3f 100644
--- a/drivers/clk/at91/clk-peripheral.c
+++ b/drivers/clk/at91/clk-peripheral.c
@@ -104,13 +104,14 @@ static const struct clk_ops peripheral_ops = {
 	.is_enabled = clk_peripheral_is_enabled,
 };
 
-static struct clk * __init
+static struct clk_hw * __init
 at91_clk_register_peripheral(struct regmap *regmap, const char *name,
 			     const char *parent_name, u32 id)
 {
 	struct clk_peripheral *periph;
-	struct clk *clk = NULL;
 	struct clk_init_data init;
+	struct clk_hw *hw;
+	int ret;
 
 	if (!name || !parent_name || id > PERIPHERAL_ID_MAX)
 		return ERR_PTR(-EINVAL);
@@ -129,11 +130,14 @@ at91_clk_register_peripheral(struct regmap *regmap, const char *name,
 	periph->hw.init = &init;
 	periph->regmap = regmap;
 
-	clk = clk_register(NULL, &periph->hw);
-	if (IS_ERR(clk))
+	hw = &periph->hw;
+	ret = clk_hw_register(NULL, &periph->hw);
+	if (ret) {
 		kfree(periph);
+		hw = ERR_PTR(ret);
+	}
 
-	return clk;
+	return hw;
 }
 
 static void clk_sam9x5_peripheral_autodiv(struct clk_sam9x5_peripheral *periph)
@@ -327,14 +331,15 @@ static const struct clk_ops sam9x5_peripheral_ops = {
 	.set_rate = clk_sam9x5_peripheral_set_rate,
 };
 
-static struct clk * __init
+static struct clk_hw * __init
 at91_clk_register_sam9x5_peripheral(struct regmap *regmap, spinlock_t *lock,
 				    const char *name, const char *parent_name,
 				    u32 id, const struct clk_range *range)
 {
 	struct clk_sam9x5_peripheral *periph;
-	struct clk *clk = NULL;
 	struct clk_init_data init;
+	struct clk_hw *hw;
+	int ret;
 
 	if (!name || !parent_name)
 		return ERR_PTR(-EINVAL);
@@ -357,13 +362,15 @@ at91_clk_register_sam9x5_peripheral(struct regmap *regmap, spinlock_t *lock,
 	periph->auto_div = true;
 	periph->range = *range;
 
-	clk = clk_register(NULL, &periph->hw);
-	if (IS_ERR(clk))
+	hw = &periph->hw;
+	ret = clk_hw_register(NULL, &periph->hw);
+	if (ret) {
 		kfree(periph);
-	else
+		hw = ERR_PTR(ret);
+	} else
 		clk_sam9x5_peripheral_autodiv(periph);
 
-	return clk;
+	return hw;
 }
 
 static void __init
@@ -371,7 +378,7 @@ of_at91_clk_periph_setup(struct device_node *np, u8 type)
 {
 	int num;
 	u32 id;
-	struct clk *clk;
+	struct clk_hw *hw;
 	const char *parent_name;
 	const char *name;
 	struct device_node *periphclknp;
@@ -400,7 +407,7 @@ of_at91_clk_periph_setup(struct device_node *np, u8 type)
 			name = periphclknp->name;
 
 		if (type == PERIPHERAL_AT91RM9200) {
-			clk = at91_clk_register_peripheral(regmap, name,
+			hw = at91_clk_register_peripheral(regmap, name,
 							   parent_name, id);
 		} else {
 			struct clk_range range = CLK_RANGE(0, 0);
@@ -409,17 +416,17 @@ of_at91_clk_periph_setup(struct device_node *np, u8 type)
 					      "atmel,clk-output-range",
 					      &range);
 
-			clk = at91_clk_register_sam9x5_peripheral(regmap,
+			hw = at91_clk_register_sam9x5_peripheral(regmap,
 								  &pmc_pcr_lock,
 								  name,
 								  parent_name,
 								  id, &range);
 		}
 
-		if (IS_ERR(clk))
+		if (IS_ERR(hw))
 			continue;
 
-		of_clk_add_provider(periphclknp, of_clk_src_simple_get, clk);
+		of_clk_add_hw_provider(periphclknp, of_clk_hw_simple_get, hw);
 	}
 }
 
diff --git a/drivers/clk/at91/clk-pll.c b/drivers/clk/at91/clk-pll.c
index fb2e0b56d4b7..45ad168e1496 100644
--- a/drivers/clk/at91/clk-pll.c
+++ b/drivers/clk/at91/clk-pll.c
@@ -296,17 +296,18 @@ static const struct clk_ops pll_ops = {
 	.set_rate = clk_pll_set_rate,
 };
 
-static struct clk * __init
+static struct clk_hw * __init
 at91_clk_register_pll(struct regmap *regmap, const char *name,
 		      const char *parent_name, u8 id,
 		      const struct clk_pll_layout *layout,
 		      const struct clk_pll_characteristics *characteristics)
 {
 	struct clk_pll *pll;
-	struct clk *clk = NULL;
+	struct clk_hw *hw;
 	struct clk_init_data init;
 	int offset = PLL_REG(id);
 	unsigned int pllr;
+	int ret;
 
 	if (id > PLL_MAX_ID)
 		return ERR_PTR(-EINVAL);
@@ -330,12 +331,14 @@ at91_clk_register_pll(struct regmap *regmap, const char *name,
 	pll->div = PLL_DIV(pllr);
 	pll->mul = PLL_MUL(pllr, layout);
 
-	clk = clk_register(NULL, &pll->hw);
-	if (IS_ERR(clk)) {
+	hw = &pll->hw;
+	ret = clk_hw_register(NULL, &pll->hw);
+	if (ret) {
 		kfree(pll);
+		hw = ERR_PTR(ret);
 	}
 
-	return clk;
+	return hw;
 }
 
 
@@ -465,7 +468,7 @@ of_at91_clk_pll_setup(struct device_node *np,
 		      const struct clk_pll_layout *layout)
 {
 	u32 id;
-	struct clk *clk;
+	struct clk_hw *hw;
 	struct regmap *regmap;
 	const char *parent_name;
 	const char *name = np->name;
@@ -486,12 +489,12 @@ of_at91_clk_pll_setup(struct device_node *np,
 	if (!characteristics)
 		return;
 
-	clk = at91_clk_register_pll(regmap, name, parent_name, id, layout,
+	hw = at91_clk_register_pll(regmap, name, parent_name, id, layout,
 				    characteristics);
-	if (IS_ERR(clk))
+	if (IS_ERR(hw))
 		goto out_free_characteristics;
 
-	of_clk_add_provider(np, of_clk_src_simple_get, clk);
+	of_clk_add_hw_provider(np, of_clk_hw_simple_get, hw);
 	return;
 
 out_free_characteristics:
diff --git a/drivers/clk/at91/clk-plldiv.c b/drivers/clk/at91/clk-plldiv.c
index 2bed26481027..b4afaf22f3fd 100644
--- a/drivers/clk/at91/clk-plldiv.c
+++ b/drivers/clk/at91/clk-plldiv.c
@@ -75,13 +75,14 @@ static const struct clk_ops plldiv_ops = {
 	.set_rate = clk_plldiv_set_rate,
 };
 
-static struct clk * __init
+static struct clk_hw * __init
 at91_clk_register_plldiv(struct regmap *regmap, const char *name,
 			 const char *parent_name)
 {
 	struct clk_plldiv *plldiv;
-	struct clk *clk = NULL;
+	struct clk_hw *hw;
 	struct clk_init_data init;
+	int ret;
 
 	plldiv = kzalloc(sizeof(*plldiv), GFP_KERNEL);
 	if (!plldiv)
@@ -96,18 +97,20 @@ at91_clk_register_plldiv(struct regmap *regmap, const char *name,
 	plldiv->hw.init = &init;
 	plldiv->regmap = regmap;
 
-	clk = clk_register(NULL, &plldiv->hw);
-
-	if (IS_ERR(clk))
+	hw = &plldiv->hw;
+	ret = clk_hw_register(NULL, &plldiv->hw);
+	if (ret) {
 		kfree(plldiv);
+		hw = ERR_PTR(ret);
+	}
 
-	return clk;
+	return hw;
 }
 
 static void __init
 of_at91sam9x5_clk_plldiv_setup(struct device_node *np)
 {
-	struct clk *clk;
+	struct clk_hw *hw;
 	const char *parent_name;
 	const char *name = np->name;
 	struct regmap *regmap;
@@ -120,12 +123,11 @@ of_at91sam9x5_clk_plldiv_setup(struct device_node *np)
 	if (IS_ERR(regmap))
 		return;
 
-	clk = at91_clk_register_plldiv(regmap, name, parent_name);
-	if (IS_ERR(clk))
+	hw = at91_clk_register_plldiv(regmap, name, parent_name);
+	if (IS_ERR(hw))
 		return;
 
-	of_clk_add_provider(np, of_clk_src_simple_get, clk);
-	return;
+	of_clk_add_hw_provider(np, of_clk_hw_simple_get, hw);
 }
 CLK_OF_DECLARE(at91sam9x5_clk_plldiv, "atmel,at91sam9x5-clk-plldiv",
 	       of_at91sam9x5_clk_plldiv_setup);
diff --git a/drivers/clk/at91/clk-programmable.c b/drivers/clk/at91/clk-programmable.c
index 25d5906640c3..190122e64a3a 100644
--- a/drivers/clk/at91/clk-programmable.c
+++ b/drivers/clk/at91/clk-programmable.c
@@ -170,15 +170,16 @@ static const struct clk_ops programmable_ops = {
 	.set_rate = clk_programmable_set_rate,
 };
 
-static struct clk * __init
+static struct clk_hw * __init
 at91_clk_register_programmable(struct regmap *regmap,
 			       const char *name, const char **parent_names,
 			       u8 num_parents, u8 id,
 			       const struct clk_programmable_layout *layout)
 {
 	struct clk_programmable *prog;
-	struct clk *clk = NULL;
+	struct clk_hw *hw;
 	struct clk_init_data init;
+	int ret;
 
 	if (id > PROG_ID_MAX)
 		return ERR_PTR(-EINVAL);
@@ -198,11 +199,14 @@ at91_clk_register_programmable(struct regmap *regmap,
 	prog->hw.init = &init;
 	prog->regmap = regmap;
 
-	clk = clk_register(NULL, &prog->hw);
-	if (IS_ERR(clk))
+	hw = &prog->hw;
+	ret = clk_hw_register(NULL, &prog->hw);
+	if (ret) {
 		kfree(prog);
+		hw = &prog->hw;
+	}
 
-	return clk;
+	return hw;
 }
 
 static const struct clk_programmable_layout at91rm9200_programmable_layout = {
@@ -229,7 +233,7 @@ of_at91_clk_prog_setup(struct device_node *np,
 {
 	int num;
 	u32 id;
-	struct clk *clk;
+	struct clk_hw *hw;
 	unsigned int num_parents;
 	const char *parent_names[PROG_SOURCE_MAX];
 	const char *name;
@@ -257,13 +261,13 @@ of_at91_clk_prog_setup(struct device_node *np,
 		if (of_property_read_string(np, "clock-output-names", &name))
 			name = progclknp->name;
 
-		clk = at91_clk_register_programmable(regmap, name,
+		hw = at91_clk_register_programmable(regmap, name,
 						     parent_names, num_parents,
 						     id, layout);
-		if (IS_ERR(clk))
+		if (IS_ERR(hw))
 			continue;
 
-		of_clk_add_provider(progclknp, of_clk_src_simple_get, clk);
+		of_clk_add_hw_provider(progclknp, of_clk_hw_simple_get, hw);
 	}
 }
 
diff --git a/drivers/clk/at91/clk-slow.c b/drivers/clk/at91/clk-slow.c
index 61090b1146cf..560a8b9abf93 100644
--- a/drivers/clk/at91/clk-slow.c
+++ b/drivers/clk/at91/clk-slow.c
@@ -13,42 +13,11 @@
 #include <linux/clk-provider.h>
 #include <linux/clkdev.h>
 #include <linux/clk/at91_pmc.h>
-#include <linux/delay.h>
 #include <linux/of.h>
 #include <linux/mfd/syscon.h>
 #include <linux/regmap.h>
 
 #include "pmc.h"
-#include "sckc.h"
-
-#define SLOW_CLOCK_FREQ		32768
-#define SLOWCK_SW_CYCLES	5
-#define SLOWCK_SW_TIME_USEC	((SLOWCK_SW_CYCLES * USEC_PER_SEC) / \
-				 SLOW_CLOCK_FREQ)
-
-#define	AT91_SCKC_CR			0x00
-#define		AT91_SCKC_RCEN		(1 << 0)
-#define		AT91_SCKC_OSC32EN	(1 << 1)
-#define		AT91_SCKC_OSC32BYP	(1 << 2)
-#define		AT91_SCKC_OSCSEL	(1 << 3)
-
-struct clk_slow_osc {
-	struct clk_hw hw;
-	void __iomem *sckcr;
-	unsigned long startup_usec;
-};
-
-#define to_clk_slow_osc(hw) container_of(hw, struct clk_slow_osc, hw)
-
-struct clk_slow_rc_osc {
-	struct clk_hw hw;
-	void __iomem *sckcr;
-	unsigned long frequency;
-	unsigned long accuracy;
-	unsigned long startup_usec;
-};
-
-#define to_clk_slow_rc_osc(hw) container_of(hw, struct clk_slow_rc_osc, hw)
 
 struct clk_sam9260_slow {
 	struct clk_hw hw;
@@ -57,328 +26,6 @@ struct clk_sam9260_slow {
 
 #define to_clk_sam9260_slow(hw) container_of(hw, struct clk_sam9260_slow, hw)
 
-struct clk_sam9x5_slow {
-	struct clk_hw hw;
-	void __iomem *sckcr;
-	u8 parent;
-};
-
-#define to_clk_sam9x5_slow(hw) container_of(hw, struct clk_sam9x5_slow, hw)
-
-static int clk_slow_osc_prepare(struct clk_hw *hw)
-{
-	struct clk_slow_osc *osc = to_clk_slow_osc(hw);
-	void __iomem *sckcr = osc->sckcr;
-	u32 tmp = readl(sckcr);
-
-	if (tmp & AT91_SCKC_OSC32BYP)
-		return 0;
-
-	writel(tmp | AT91_SCKC_OSC32EN, sckcr);
-
-	usleep_range(osc->startup_usec, osc->startup_usec + 1);
-
-	return 0;
-}
-
-static void clk_slow_osc_unprepare(struct clk_hw *hw)
-{
-	struct clk_slow_osc *osc = to_clk_slow_osc(hw);
-	void __iomem *sckcr = osc->sckcr;
-	u32 tmp = readl(sckcr);
-
-	if (tmp & AT91_SCKC_OSC32BYP)
-		return;
-
-	writel(tmp & ~AT91_SCKC_OSC32EN, sckcr);
-}
-
-static int clk_slow_osc_is_prepared(struct clk_hw *hw)
-{
-	struct clk_slow_osc *osc = to_clk_slow_osc(hw);
-	void __iomem *sckcr = osc->sckcr;
-	u32 tmp = readl(sckcr);
-
-	if (tmp & AT91_SCKC_OSC32BYP)
-		return 1;
-
-	return !!(tmp & AT91_SCKC_OSC32EN);
-}
-
-static const struct clk_ops slow_osc_ops = {
-	.prepare = clk_slow_osc_prepare,
-	.unprepare = clk_slow_osc_unprepare,
-	.is_prepared = clk_slow_osc_is_prepared,
-};
-
-static struct clk * __init
-at91_clk_register_slow_osc(void __iomem *sckcr,
-			   const char *name,
-			   const char *parent_name,
-			   unsigned long startup,
-			   bool bypass)
-{
-	struct clk_slow_osc *osc;
-	struct clk *clk = NULL;
-	struct clk_init_data init;
-
-	if (!sckcr || !name || !parent_name)
-		return ERR_PTR(-EINVAL);
-
-	osc = kzalloc(sizeof(*osc), GFP_KERNEL);
-	if (!osc)
-		return ERR_PTR(-ENOMEM);
-
-	init.name = name;
-	init.ops = &slow_osc_ops;
-	init.parent_names = &parent_name;
-	init.num_parents = 1;
-	init.flags = CLK_IGNORE_UNUSED;
-
-	osc->hw.init = &init;
-	osc->sckcr = sckcr;
-	osc->startup_usec = startup;
-
-	if (bypass)
-		writel((readl(sckcr) & ~AT91_SCKC_OSC32EN) | AT91_SCKC_OSC32BYP,
-		       sckcr);
-
-	clk = clk_register(NULL, &osc->hw);
-	if (IS_ERR(clk))
-		kfree(osc);
-
-	return clk;
-}
-
-void __init of_at91sam9x5_clk_slow_osc_setup(struct device_node *np,
-					     void __iomem *sckcr)
-{
-	struct clk *clk;
-	const char *parent_name;
-	const char *name = np->name;
-	u32 startup;
-	bool bypass;
-
-	parent_name = of_clk_get_parent_name(np, 0);
-	of_property_read_string(np, "clock-output-names", &name);
-	of_property_read_u32(np, "atmel,startup-time-usec", &startup);
-	bypass = of_property_read_bool(np, "atmel,osc-bypass");
-
-	clk = at91_clk_register_slow_osc(sckcr, name, parent_name, startup,
-					 bypass);
-	if (IS_ERR(clk))
-		return;
-
-	of_clk_add_provider(np, of_clk_src_simple_get, clk);
-}
-
-static unsigned long clk_slow_rc_osc_recalc_rate(struct clk_hw *hw,
-						 unsigned long parent_rate)
-{
-	struct clk_slow_rc_osc *osc = to_clk_slow_rc_osc(hw);
-
-	return osc->frequency;
-}
-
-static unsigned long clk_slow_rc_osc_recalc_accuracy(struct clk_hw *hw,
-						     unsigned long parent_acc)
-{
-	struct clk_slow_rc_osc *osc = to_clk_slow_rc_osc(hw);
-
-	return osc->accuracy;
-}
-
-static int clk_slow_rc_osc_prepare(struct clk_hw *hw)
-{
-	struct clk_slow_rc_osc *osc = to_clk_slow_rc_osc(hw);
-	void __iomem *sckcr = osc->sckcr;
-
-	writel(readl(sckcr) | AT91_SCKC_RCEN, sckcr);
-
-	usleep_range(osc->startup_usec, osc->startup_usec + 1);
-
-	return 0;
-}
-
-static void clk_slow_rc_osc_unprepare(struct clk_hw *hw)
-{
-	struct clk_slow_rc_osc *osc = to_clk_slow_rc_osc(hw);
-	void __iomem *sckcr = osc->sckcr;
-
-	writel(readl(sckcr) & ~AT91_SCKC_RCEN, sckcr);
-}
-
-static int clk_slow_rc_osc_is_prepared(struct clk_hw *hw)
-{
-	struct clk_slow_rc_osc *osc = to_clk_slow_rc_osc(hw);
-
-	return !!(readl(osc->sckcr) & AT91_SCKC_RCEN);
-}
-
-static const struct clk_ops slow_rc_osc_ops = {
-	.prepare = clk_slow_rc_osc_prepare,
-	.unprepare = clk_slow_rc_osc_unprepare,
-	.is_prepared = clk_slow_rc_osc_is_prepared,
-	.recalc_rate = clk_slow_rc_osc_recalc_rate,
-	.recalc_accuracy = clk_slow_rc_osc_recalc_accuracy,
-};
-
-static struct clk * __init
-at91_clk_register_slow_rc_osc(void __iomem *sckcr,
-			      const char *name,
-			      unsigned long frequency,
-			      unsigned long accuracy,
-			      unsigned long startup)
-{
-	struct clk_slow_rc_osc *osc;
-	struct clk *clk = NULL;
-	struct clk_init_data init;
-
-	if (!sckcr || !name)
-		return ERR_PTR(-EINVAL);
-
-	osc = kzalloc(sizeof(*osc), GFP_KERNEL);
-	if (!osc)
-		return ERR_PTR(-ENOMEM);
-
-	init.name = name;
-	init.ops = &slow_rc_osc_ops;
-	init.parent_names = NULL;
-	init.num_parents = 0;
-	init.flags = CLK_IGNORE_UNUSED;
-
-	osc->hw.init = &init;
-	osc->sckcr = sckcr;
-	osc->frequency = frequency;
-	osc->accuracy = accuracy;
-	osc->startup_usec = startup;
-
-	clk = clk_register(NULL, &osc->hw);
-	if (IS_ERR(clk))
-		kfree(osc);
-
-	return clk;
-}
-
-void __init of_at91sam9x5_clk_slow_rc_osc_setup(struct device_node *np,
-						void __iomem *sckcr)
-{
-	struct clk *clk;
-	u32 frequency = 0;
-	u32 accuracy = 0;
-	u32 startup = 0;
-	const char *name = np->name;
-
-	of_property_read_string(np, "clock-output-names", &name);
-	of_property_read_u32(np, "clock-frequency", &frequency);
-	of_property_read_u32(np, "clock-accuracy", &accuracy);
-	of_property_read_u32(np, "atmel,startup-time-usec", &startup);
-
-	clk = at91_clk_register_slow_rc_osc(sckcr, name, frequency, accuracy,
-					    startup);
-	if (IS_ERR(clk))
-		return;
-
-	of_clk_add_provider(np, of_clk_src_simple_get, clk);
-}
-
-static int clk_sam9x5_slow_set_parent(struct clk_hw *hw, u8 index)
-{
-	struct clk_sam9x5_slow *slowck = to_clk_sam9x5_slow(hw);
-	void __iomem *sckcr = slowck->sckcr;
-	u32 tmp;
-
-	if (index > 1)
-		return -EINVAL;
-
-	tmp = readl(sckcr);
-
-	if ((!index && !(tmp & AT91_SCKC_OSCSEL)) ||
-	    (index && (tmp & AT91_SCKC_OSCSEL)))
-		return 0;
-
-	if (index)
-		tmp |= AT91_SCKC_OSCSEL;
-	else
-		tmp &= ~AT91_SCKC_OSCSEL;
-
-	writel(tmp, sckcr);
-
-	usleep_range(SLOWCK_SW_TIME_USEC, SLOWCK_SW_TIME_USEC + 1);
-
-	return 0;
-}
-
-static u8 clk_sam9x5_slow_get_parent(struct clk_hw *hw)
-{
-	struct clk_sam9x5_slow *slowck = to_clk_sam9x5_slow(hw);
-
-	return !!(readl(slowck->sckcr) & AT91_SCKC_OSCSEL);
-}
-
-static const struct clk_ops sam9x5_slow_ops = {
-	.set_parent = clk_sam9x5_slow_set_parent,
-	.get_parent = clk_sam9x5_slow_get_parent,
-};
-
-static struct clk * __init
-at91_clk_register_sam9x5_slow(void __iomem *sckcr,
-			      const char *name,
-			      const char **parent_names,
-			      int num_parents)
-{
-	struct clk_sam9x5_slow *slowck;
-	struct clk *clk = NULL;
-	struct clk_init_data init;
-
-	if (!sckcr || !name || !parent_names || !num_parents)
-		return ERR_PTR(-EINVAL);
-
-	slowck = kzalloc(sizeof(*slowck), GFP_KERNEL);
-	if (!slowck)
-		return ERR_PTR(-ENOMEM);
-
-	init.name = name;
-	init.ops = &sam9x5_slow_ops;
-	init.parent_names = parent_names;
-	init.num_parents = num_parents;
-	init.flags = 0;
-
-	slowck->hw.init = &init;
-	slowck->sckcr = sckcr;
-	slowck->parent = !!(readl(sckcr) & AT91_SCKC_OSCSEL);
-
-	clk = clk_register(NULL, &slowck->hw);
-	if (IS_ERR(clk))
-		kfree(slowck);
-
-	return clk;
-}
-
-void __init of_at91sam9x5_clk_slow_setup(struct device_node *np,
-					 void __iomem *sckcr)
-{
-	struct clk *clk;
-	const char *parent_names[2];
-	unsigned int num_parents;
-	const char *name = np->name;
-
-	num_parents = of_clk_get_parent_count(np);
-	if (num_parents == 0 || num_parents > 2)
-		return;
-
-	of_clk_parent_fill(np, parent_names, num_parents);
-
-	of_property_read_string(np, "clock-output-names", &name);
-
-	clk = at91_clk_register_sam9x5_slow(sckcr, name, parent_names,
-					    num_parents);
-	if (IS_ERR(clk))
-		return;
-
-	of_clk_add_provider(np, of_clk_src_simple_get, clk);
-}
-
 static u8 clk_sam9260_slow_get_parent(struct clk_hw *hw)
 {
 	struct clk_sam9260_slow *slowck = to_clk_sam9260_slow(hw);
@@ -393,15 +40,16 @@ static const struct clk_ops sam9260_slow_ops = {
 	.get_parent = clk_sam9260_slow_get_parent,
 };
 
-static struct clk * __init
+static struct clk_hw * __init
 at91_clk_register_sam9260_slow(struct regmap *regmap,
 			       const char *name,
 			       const char **parent_names,
 			       int num_parents)
 {
 	struct clk_sam9260_slow *slowck;
-	struct clk *clk = NULL;
+	struct clk_hw *hw;
 	struct clk_init_data init;
+	int ret;
 
 	if (!name)
 		return ERR_PTR(-EINVAL);
@@ -422,16 +70,19 @@ at91_clk_register_sam9260_slow(struct regmap *regmap,
 	slowck->hw.init = &init;
 	slowck->regmap = regmap;
 
-	clk = clk_register(NULL, &slowck->hw);
-	if (IS_ERR(clk))
+	hw = &slowck->hw;
+	ret = clk_hw_register(NULL, &slowck->hw);
+	if (ret) {
 		kfree(slowck);
+		hw = ERR_PTR(ret);
+	}
 
-	return clk;
+	return hw;
 }
 
 static void __init of_at91sam9260_clk_slow_setup(struct device_node *np)
 {
-	struct clk *clk;
+	struct clk_hw *hw;
 	const char *parent_names[2];
 	unsigned int num_parents;
 	const char *name = np->name;
@@ -448,12 +99,12 @@ static void __init of_at91sam9260_clk_slow_setup(struct device_node *np)
 
 	of_property_read_string(np, "clock-output-names", &name);
 
-	clk = at91_clk_register_sam9260_slow(regmap, name, parent_names,
+	hw = at91_clk_register_sam9260_slow(regmap, name, parent_names,
 					     num_parents);
-	if (IS_ERR(clk))
+	if (IS_ERR(hw))
 		return;
 
-	of_clk_add_provider(np, of_clk_src_simple_get, clk);
+	of_clk_add_hw_provider(np, of_clk_hw_simple_get, hw);
 }
 
 CLK_OF_DECLARE(at91sam9260_clk_slow, "atmel,at91sam9260-clk-slow",
diff --git a/drivers/clk/at91/clk-smd.c b/drivers/clk/at91/clk-smd.c
index 3c04b069d5b8..965c662b90a5 100644
--- a/drivers/clk/at91/clk-smd.c
+++ b/drivers/clk/at91/clk-smd.c
@@ -111,13 +111,14 @@ static const struct clk_ops at91sam9x5_smd_ops = {
 	.set_rate = at91sam9x5_clk_smd_set_rate,
 };
 
-static struct clk * __init
+static struct clk_hw * __init
 at91sam9x5_clk_register_smd(struct regmap *regmap, const char *name,
 			    const char **parent_names, u8 num_parents)
 {
 	struct at91sam9x5_clk_smd *smd;
-	struct clk *clk = NULL;
+	struct clk_hw *hw;
 	struct clk_init_data init;
+	int ret;
 
 	smd = kzalloc(sizeof(*smd), GFP_KERNEL);
 	if (!smd)
@@ -132,16 +133,19 @@ at91sam9x5_clk_register_smd(struct regmap *regmap, const char *name,
 	smd->hw.init = &init;
 	smd->regmap = regmap;
 
-	clk = clk_register(NULL, &smd->hw);
-	if (IS_ERR(clk))
+	hw = &smd->hw;
+	ret = clk_hw_register(NULL, &smd->hw);
+	if (ret) {
 		kfree(smd);
+		hw = ERR_PTR(ret);
+	}
 
-	return clk;
+	return hw;
 }
 
 static void __init of_at91sam9x5_clk_smd_setup(struct device_node *np)
 {
-	struct clk *clk;
+	struct clk_hw *hw;
 	unsigned int num_parents;
 	const char *parent_names[SMD_SOURCE_MAX];
 	const char *name = np->name;
@@ -159,12 +163,12 @@ static void __init of_at91sam9x5_clk_smd_setup(struct device_node *np)
 	if (IS_ERR(regmap))
 		return;
 
-	clk = at91sam9x5_clk_register_smd(regmap, name, parent_names,
+	hw = at91sam9x5_clk_register_smd(regmap, name, parent_names,
 					  num_parents);
-	if (IS_ERR(clk))
+	if (IS_ERR(hw))
 		return;
 
-	of_clk_add_provider(np, of_clk_src_simple_get, clk);
+	of_clk_add_hw_provider(np, of_clk_hw_simple_get, hw);
 }
 CLK_OF_DECLARE(at91sam9x5_clk_smd, "atmel,at91sam9x5-clk-smd",
 	       of_at91sam9x5_clk_smd_setup);
diff --git a/drivers/clk/at91/clk-system.c b/drivers/clk/at91/clk-system.c
index 8f35d8172909..86a36809765d 100644
--- a/drivers/clk/at91/clk-system.c
+++ b/drivers/clk/at91/clk-system.c
@@ -88,13 +88,14 @@ static const struct clk_ops system_ops = {
 	.is_prepared = clk_system_is_prepared,
 };
 
-static struct clk * __init
+static struct clk_hw * __init
 at91_clk_register_system(struct regmap *regmap, const char *name,
 			 const char *parent_name, u8 id)
 {
 	struct clk_system *sys;
-	struct clk *clk = NULL;
+	struct clk_hw *hw;
 	struct clk_init_data init;
+	int ret;
 
 	if (!parent_name || id > SYSTEM_MAX_ID)
 		return ERR_PTR(-EINVAL);
@@ -113,18 +114,21 @@ at91_clk_register_system(struct regmap *regmap, const char *name,
 	sys->hw.init = &init;
 	sys->regmap = regmap;
 
-	clk = clk_register(NULL, &sys->hw);
-	if (IS_ERR(clk))
+	hw = &sys->hw;
+	ret = clk_hw_register(NULL, &sys->hw);
+	if (ret) {
 		kfree(sys);
+		hw = ERR_PTR(ret);
+	}
 
-	return clk;
+	return hw;
 }
 
 static void __init of_at91rm9200_clk_sys_setup(struct device_node *np)
 {
 	int num;
 	u32 id;
-	struct clk *clk;
+	struct clk_hw *hw;
 	const char *name;
 	struct device_node *sysclknp;
 	const char *parent_name;
@@ -147,11 +151,11 @@ static void __init of_at91rm9200_clk_sys_setup(struct device_node *np)
 
 		parent_name = of_clk_get_parent_name(sysclknp, 0);
 
-		clk = at91_clk_register_system(regmap, name, parent_name, id);
-		if (IS_ERR(clk))
+		hw = at91_clk_register_system(regmap, name, parent_name, id);
+		if (IS_ERR(hw))
 			continue;
 
-		of_clk_add_provider(sysclknp, of_clk_src_simple_get, clk);
+		of_clk_add_hw_provider(sysclknp, of_clk_hw_simple_get, hw);
 	}
 }
 CLK_OF_DECLARE(at91rm9200_clk_sys, "atmel,at91rm9200-clk-system",
diff --git a/drivers/clk/at91/clk-usb.c b/drivers/clk/at91/clk-usb.c
index d80bdb0a8b02..791770a563fc 100644
--- a/drivers/clk/at91/clk-usb.c
+++ b/drivers/clk/at91/clk-usb.c
@@ -192,13 +192,14 @@ static const struct clk_ops at91sam9n12_usb_ops = {
 	.set_rate = at91sam9x5_clk_usb_set_rate,
 };
 
-static struct clk * __init
+static struct clk_hw * __init
 at91sam9x5_clk_register_usb(struct regmap *regmap, const char *name,
 			    const char **parent_names, u8 num_parents)
 {
 	struct at91sam9x5_clk_usb *usb;
-	struct clk *clk = NULL;
+	struct clk_hw *hw;
 	struct clk_init_data init;
+	int ret;
 
 	usb = kzalloc(sizeof(*usb), GFP_KERNEL);
 	if (!usb)
@@ -214,20 +215,24 @@ at91sam9x5_clk_register_usb(struct regmap *regmap, const char *name,
 	usb->hw.init = &init;
 	usb->regmap = regmap;
 
-	clk = clk_register(NULL, &usb->hw);
-	if (IS_ERR(clk))
+	hw = &usb->hw;
+	ret = clk_hw_register(NULL, &usb->hw);
+	if (ret) {
 		kfree(usb);
+		hw = ERR_PTR(ret);
+	}
 
-	return clk;
+	return hw;
 }
 
-static struct clk * __init
+static struct clk_hw * __init
 at91sam9n12_clk_register_usb(struct regmap *regmap, const char *name,
 			     const char *parent_name)
 {
 	struct at91sam9x5_clk_usb *usb;
-	struct clk *clk = NULL;
+	struct clk_hw *hw;
 	struct clk_init_data init;
+	int ret;
 
 	usb = kzalloc(sizeof(*usb), GFP_KERNEL);
 	if (!usb)
@@ -242,11 +247,14 @@ at91sam9n12_clk_register_usb(struct regmap *regmap, const char *name,
 	usb->hw.init = &init;
 	usb->regmap = regmap;
 
-	clk = clk_register(NULL, &usb->hw);
-	if (IS_ERR(clk))
+	hw = &usb->hw;
+	ret = clk_hw_register(NULL, &usb->hw);
+	if (ret) {
 		kfree(usb);
+		hw = ERR_PTR(ret);
+	}
 
-	return clk;
+	return hw;
 }
 
 static unsigned long at91rm9200_clk_usb_recalc_rate(struct clk_hw *hw,
@@ -334,13 +342,14 @@ static const struct clk_ops at91rm9200_usb_ops = {
 	.set_rate = at91rm9200_clk_usb_set_rate,
 };
 
-static struct clk * __init
+static struct clk_hw * __init
 at91rm9200_clk_register_usb(struct regmap *regmap, const char *name,
 			    const char *parent_name, const u32 *divisors)
 {
 	struct at91rm9200_clk_usb *usb;
-	struct clk *clk = NULL;
+	struct clk_hw *hw;
 	struct clk_init_data init;
+	int ret;
 
 	usb = kzalloc(sizeof(*usb), GFP_KERNEL);
 	if (!usb)
@@ -356,16 +365,19 @@ at91rm9200_clk_register_usb(struct regmap *regmap, const char *name,
 	usb->regmap = regmap;
 	memcpy(usb->divisors, divisors, sizeof(usb->divisors));
 
-	clk = clk_register(NULL, &usb->hw);
-	if (IS_ERR(clk))
+	hw = &usb->hw;
+	ret = clk_hw_register(NULL, &usb->hw);
+	if (ret) {
 		kfree(usb);
+		hw = ERR_PTR(ret);
+	}
 
-	return clk;
+	return hw;
 }
 
 static void __init of_at91sam9x5_clk_usb_setup(struct device_node *np)
 {
-	struct clk *clk;
+	struct clk_hw *hw;
 	unsigned int num_parents;
 	const char *parent_names[USB_SOURCE_MAX];
 	const char *name = np->name;
@@ -383,19 +395,19 @@ static void __init of_at91sam9x5_clk_usb_setup(struct device_node *np)
 	if (IS_ERR(regmap))
 		return;
 
-	clk = at91sam9x5_clk_register_usb(regmap, name, parent_names,
-					  num_parents);
-	if (IS_ERR(clk))
+	hw = at91sam9x5_clk_register_usb(regmap, name, parent_names,
+					 num_parents);
+	if (IS_ERR(hw))
 		return;
 
-	of_clk_add_provider(np, of_clk_src_simple_get, clk);
+	of_clk_add_hw_provider(np, of_clk_hw_simple_get, hw);
 }
 CLK_OF_DECLARE(at91sam9x5_clk_usb, "atmel,at91sam9x5-clk-usb",
 	       of_at91sam9x5_clk_usb_setup);
 
 static void __init of_at91sam9n12_clk_usb_setup(struct device_node *np)
 {
-	struct clk *clk;
+	struct clk_hw *hw;
 	const char *parent_name;
 	const char *name = np->name;
 	struct regmap *regmap;
@@ -410,18 +422,18 @@ static void __init of_at91sam9n12_clk_usb_setup(struct device_node *np)
 	if (IS_ERR(regmap))
 		return;
 
-	clk = at91sam9n12_clk_register_usb(regmap, name, parent_name);
-	if (IS_ERR(clk))
+	hw = at91sam9n12_clk_register_usb(regmap, name, parent_name);
+	if (IS_ERR(hw))
 		return;
 
-	of_clk_add_provider(np, of_clk_src_simple_get, clk);
+	of_clk_add_hw_provider(np, of_clk_hw_simple_get, hw);
 }
 CLK_OF_DECLARE(at91sam9n12_clk_usb, "atmel,at91sam9n12-clk-usb",
 	       of_at91sam9n12_clk_usb_setup);
 
 static void __init of_at91rm9200_clk_usb_setup(struct device_node *np)
 {
-	struct clk *clk;
+	struct clk_hw *hw;
 	const char *parent_name;
 	const char *name = np->name;
 	u32 divisors[4] = {0, 0, 0, 0};
@@ -440,12 +452,11 @@ static void __init of_at91rm9200_clk_usb_setup(struct device_node *np)
 	regmap = syscon_node_to_regmap(of_get_parent(np));
 	if (IS_ERR(regmap))
 		return;
-
-	clk = at91rm9200_clk_register_usb(regmap, name, parent_name, divisors);
-	if (IS_ERR(clk))
+	hw = at91rm9200_clk_register_usb(regmap, name, parent_name, divisors);
+	if (IS_ERR(hw))
 		return;
 
-	of_clk_add_provider(np, of_clk_src_simple_get, clk);
+	of_clk_add_hw_provider(np, of_clk_hw_simple_get, hw);
 }
 CLK_OF_DECLARE(at91rm9200_clk_usb, "atmel,at91rm9200-clk-usb",
 	       of_at91rm9200_clk_usb_setup);
diff --git a/drivers/clk/at91/clk-utmi.c b/drivers/clk/at91/clk-utmi.c
index 61fcf399e58c..aadabd9d1e2b 100644
--- a/drivers/clk/at91/clk-utmi.c
+++ b/drivers/clk/at91/clk-utmi.c
@@ -77,13 +77,14 @@ static const struct clk_ops utmi_ops = {
 	.recalc_rate = clk_utmi_recalc_rate,
 };
 
-static struct clk * __init
+static struct clk_hw * __init
 at91_clk_register_utmi(struct regmap *regmap,
 		       const char *name, const char *parent_name)
 {
 	struct clk_utmi *utmi;
-	struct clk *clk = NULL;
+	struct clk_hw *hw;
 	struct clk_init_data init;
+	int ret;
 
 	utmi = kzalloc(sizeof(*utmi), GFP_KERNEL);
 	if (!utmi)
@@ -98,16 +99,19 @@ at91_clk_register_utmi(struct regmap *regmap,
 	utmi->hw.init = &init;
 	utmi->regmap = regmap;
 
-	clk = clk_register(NULL, &utmi->hw);
-	if (IS_ERR(clk))
+	hw = &utmi->hw;
+	ret = clk_hw_register(NULL, &utmi->hw);
+	if (ret) {
 		kfree(utmi);
+		hw = ERR_PTR(ret);
+	}
 
-	return clk;
+	return hw;
 }
 
 static void __init of_at91sam9x5_clk_utmi_setup(struct device_node *np)
 {
-	struct clk *clk;
+	struct clk_hw *hw;
 	const char *parent_name;
 	const char *name = np->name;
 	struct regmap *regmap;
@@ -120,11 +124,11 @@ static void __init of_at91sam9x5_clk_utmi_setup(struct device_node *np)
 	if (IS_ERR(regmap))
 		return;
 
-	clk = at91_clk_register_utmi(regmap, name, parent_name);
-	if (IS_ERR(clk))
+	hw = at91_clk_register_utmi(regmap, name, parent_name);
+	if (IS_ERR(hw))
 		return;
 
-	of_clk_add_provider(np, of_clk_src_simple_get, clk);
+	of_clk_add_hw_provider(np, of_clk_hw_simple_get, hw);
 	return;
 }
 CLK_OF_DECLARE(at91sam9x5_clk_utmi, "atmel,at91sam9x5-clk-utmi",
diff --git a/drivers/clk/at91/sckc.c b/drivers/clk/at91/sckc.c
index 1184d76a7ab7..ab6ecefc49ad 100644
--- a/drivers/clk/at91/sckc.c
+++ b/drivers/clk/at91/sckc.c
@@ -12,11 +12,382 @@
 
 #include <linux/clk-provider.h>
 #include <linux/clkdev.h>
+#include <linux/delay.h>
 #include <linux/of.h>
 #include <linux/of_address.h>
 #include <linux/io.h>
 
-#include "sckc.h"
+#define SLOW_CLOCK_FREQ		32768
+#define SLOWCK_SW_CYCLES	5
+#define SLOWCK_SW_TIME_USEC	((SLOWCK_SW_CYCLES * USEC_PER_SEC) / \
+				 SLOW_CLOCK_FREQ)
+
+#define	AT91_SCKC_CR			0x00
+#define		AT91_SCKC_RCEN		(1 << 0)
+#define		AT91_SCKC_OSC32EN	(1 << 1)
+#define		AT91_SCKC_OSC32BYP	(1 << 2)
+#define		AT91_SCKC_OSCSEL	(1 << 3)
+
+struct clk_slow_osc {
+	struct clk_hw hw;
+	void __iomem *sckcr;
+	unsigned long startup_usec;
+};
+
+#define to_clk_slow_osc(hw) container_of(hw, struct clk_slow_osc, hw)
+
+struct clk_sama5d4_slow_osc {
+	struct clk_hw hw;
+	void __iomem *sckcr;
+	unsigned long startup_usec;
+	bool prepared;
+};
+
+#define to_clk_sama5d4_slow_osc(hw) container_of(hw, struct clk_sama5d4_slow_osc, hw)
+
+struct clk_slow_rc_osc {
+	struct clk_hw hw;
+	void __iomem *sckcr;
+	unsigned long frequency;
+	unsigned long accuracy;
+	unsigned long startup_usec;
+};
+
+#define to_clk_slow_rc_osc(hw) container_of(hw, struct clk_slow_rc_osc, hw)
+
+struct clk_sam9x5_slow {
+	struct clk_hw hw;
+	void __iomem *sckcr;
+	u8 parent;
+};
+
+#define to_clk_sam9x5_slow(hw) container_of(hw, struct clk_sam9x5_slow, hw)
+
+static int clk_slow_osc_prepare(struct clk_hw *hw)
+{
+	struct clk_slow_osc *osc = to_clk_slow_osc(hw);
+	void __iomem *sckcr = osc->sckcr;
+	u32 tmp = readl(sckcr);
+
+	if (tmp & (AT91_SCKC_OSC32BYP | AT91_SCKC_OSC32EN))
+		return 0;
+
+	writel(tmp | AT91_SCKC_OSC32EN, sckcr);
+
+	usleep_range(osc->startup_usec, osc->startup_usec + 1);
+
+	return 0;
+}
+
+static void clk_slow_osc_unprepare(struct clk_hw *hw)
+{
+	struct clk_slow_osc *osc = to_clk_slow_osc(hw);
+	void __iomem *sckcr = osc->sckcr;
+	u32 tmp = readl(sckcr);
+
+	if (tmp & AT91_SCKC_OSC32BYP)
+		return;
+
+	writel(tmp & ~AT91_SCKC_OSC32EN, sckcr);
+}
+
+static int clk_slow_osc_is_prepared(struct clk_hw *hw)
+{
+	struct clk_slow_osc *osc = to_clk_slow_osc(hw);
+	void __iomem *sckcr = osc->sckcr;
+	u32 tmp = readl(sckcr);
+
+	if (tmp & AT91_SCKC_OSC32BYP)
+		return 1;
+
+	return !!(tmp & AT91_SCKC_OSC32EN);
+}
+
+static const struct clk_ops slow_osc_ops = {
+	.prepare = clk_slow_osc_prepare,
+	.unprepare = clk_slow_osc_unprepare,
+	.is_prepared = clk_slow_osc_is_prepared,
+};
+
+static struct clk_hw * __init
+at91_clk_register_slow_osc(void __iomem *sckcr,
+			   const char *name,
+			   const char *parent_name,
+			   unsigned long startup,
+			   bool bypass)
+{
+	struct clk_slow_osc *osc;
+	struct clk_hw *hw;
+	struct clk_init_data init;
+	int ret;
+
+	if (!sckcr || !name || !parent_name)
+		return ERR_PTR(-EINVAL);
+
+	osc = kzalloc(sizeof(*osc), GFP_KERNEL);
+	if (!osc)
+		return ERR_PTR(-ENOMEM);
+
+	init.name = name;
+	init.ops = &slow_osc_ops;
+	init.parent_names = &parent_name;
+	init.num_parents = 1;
+	init.flags = CLK_IGNORE_UNUSED;
+
+	osc->hw.init = &init;
+	osc->sckcr = sckcr;
+	osc->startup_usec = startup;
+
+	if (bypass)
+		writel((readl(sckcr) & ~AT91_SCKC_OSC32EN) | AT91_SCKC_OSC32BYP,
+		       sckcr);
+
+	hw = &osc->hw;
+	ret = clk_hw_register(NULL, &osc->hw);
+	if (ret) {
+		kfree(osc);
+		hw = ERR_PTR(ret);
+	}
+
+	return hw;
+}
+
+static void __init
+of_at91sam9x5_clk_slow_osc_setup(struct device_node *np, void __iomem *sckcr)
+{
+	struct clk_hw *hw;
+	const char *parent_name;
+	const char *name = np->name;
+	u32 startup;
+	bool bypass;
+
+	parent_name = of_clk_get_parent_name(np, 0);
+	of_property_read_string(np, "clock-output-names", &name);
+	of_property_read_u32(np, "atmel,startup-time-usec", &startup);
+	bypass = of_property_read_bool(np, "atmel,osc-bypass");
+
+	hw = at91_clk_register_slow_osc(sckcr, name, parent_name, startup,
+					 bypass);
+	if (IS_ERR(hw))
+		return;
+
+	of_clk_add_hw_provider(np, of_clk_hw_simple_get, hw);
+}
+
+static unsigned long clk_slow_rc_osc_recalc_rate(struct clk_hw *hw,
+						 unsigned long parent_rate)
+{
+	struct clk_slow_rc_osc *osc = to_clk_slow_rc_osc(hw);
+
+	return osc->frequency;
+}
+
+static unsigned long clk_slow_rc_osc_recalc_accuracy(struct clk_hw *hw,
+						     unsigned long parent_acc)
+{
+	struct clk_slow_rc_osc *osc = to_clk_slow_rc_osc(hw);
+
+	return osc->accuracy;
+}
+
+static int clk_slow_rc_osc_prepare(struct clk_hw *hw)
+{
+	struct clk_slow_rc_osc *osc = to_clk_slow_rc_osc(hw);
+	void __iomem *sckcr = osc->sckcr;
+
+	writel(readl(sckcr) | AT91_SCKC_RCEN, sckcr);
+
+	usleep_range(osc->startup_usec, osc->startup_usec + 1);
+
+	return 0;
+}
+
+static void clk_slow_rc_osc_unprepare(struct clk_hw *hw)
+{
+	struct clk_slow_rc_osc *osc = to_clk_slow_rc_osc(hw);
+	void __iomem *sckcr = osc->sckcr;
+
+	writel(readl(sckcr) & ~AT91_SCKC_RCEN, sckcr);
+}
+
+static int clk_slow_rc_osc_is_prepared(struct clk_hw *hw)
+{
+	struct clk_slow_rc_osc *osc = to_clk_slow_rc_osc(hw);
+
+	return !!(readl(osc->sckcr) & AT91_SCKC_RCEN);
+}
+
+static const struct clk_ops slow_rc_osc_ops = {
+	.prepare = clk_slow_rc_osc_prepare,
+	.unprepare = clk_slow_rc_osc_unprepare,
+	.is_prepared = clk_slow_rc_osc_is_prepared,
+	.recalc_rate = clk_slow_rc_osc_recalc_rate,
+	.recalc_accuracy = clk_slow_rc_osc_recalc_accuracy,
+};
+
+static struct clk_hw * __init
+at91_clk_register_slow_rc_osc(void __iomem *sckcr,
+			      const char *name,
+			      unsigned long frequency,
+			      unsigned long accuracy,
+			      unsigned long startup)
+{
+	struct clk_slow_rc_osc *osc;
+	struct clk_hw *hw;
+	struct clk_init_data init;
+	int ret;
+
+	if (!sckcr || !name)
+		return ERR_PTR(-EINVAL);
+
+	osc = kzalloc(sizeof(*osc), GFP_KERNEL);
+	if (!osc)
+		return ERR_PTR(-ENOMEM);
+
+	init.name = name;
+	init.ops = &slow_rc_osc_ops;
+	init.parent_names = NULL;
+	init.num_parents = 0;
+	init.flags = CLK_IGNORE_UNUSED;
+
+	osc->hw.init = &init;
+	osc->sckcr = sckcr;
+	osc->frequency = frequency;
+	osc->accuracy = accuracy;
+	osc->startup_usec = startup;
+
+	hw = &osc->hw;
+	ret = clk_hw_register(NULL, &osc->hw);
+	if (ret) {
+		kfree(osc);
+		hw = ERR_PTR(ret);
+	}
+
+	return hw;
+}
+
+static void __init
+of_at91sam9x5_clk_slow_rc_osc_setup(struct device_node *np, void __iomem *sckcr)
+{
+	struct clk_hw *hw;
+	u32 frequency = 0;
+	u32 accuracy = 0;
+	u32 startup = 0;
+	const char *name = np->name;
+
+	of_property_read_string(np, "clock-output-names", &name);
+	of_property_read_u32(np, "clock-frequency", &frequency);
+	of_property_read_u32(np, "clock-accuracy", &accuracy);
+	of_property_read_u32(np, "atmel,startup-time-usec", &startup);
+
+	hw = at91_clk_register_slow_rc_osc(sckcr, name, frequency, accuracy,
+					    startup);
+	if (IS_ERR(hw))
+		return;
+
+	of_clk_add_hw_provider(np, of_clk_hw_simple_get, hw);
+}
+
+static int clk_sam9x5_slow_set_parent(struct clk_hw *hw, u8 index)
+{
+	struct clk_sam9x5_slow *slowck = to_clk_sam9x5_slow(hw);
+	void __iomem *sckcr = slowck->sckcr;
+	u32 tmp;
+
+	if (index > 1)
+		return -EINVAL;
+
+	tmp = readl(sckcr);
+
+	if ((!index && !(tmp & AT91_SCKC_OSCSEL)) ||
+	    (index && (tmp & AT91_SCKC_OSCSEL)))
+		return 0;
+
+	if (index)
+		tmp |= AT91_SCKC_OSCSEL;
+	else
+		tmp &= ~AT91_SCKC_OSCSEL;
+
+	writel(tmp, sckcr);
+
+	usleep_range(SLOWCK_SW_TIME_USEC, SLOWCK_SW_TIME_USEC + 1);
+
+	return 0;
+}
+
+static u8 clk_sam9x5_slow_get_parent(struct clk_hw *hw)
+{
+	struct clk_sam9x5_slow *slowck = to_clk_sam9x5_slow(hw);
+
+	return !!(readl(slowck->sckcr) & AT91_SCKC_OSCSEL);
+}
+
+static const struct clk_ops sam9x5_slow_ops = {
+	.set_parent = clk_sam9x5_slow_set_parent,
+	.get_parent = clk_sam9x5_slow_get_parent,
+};
+
+static struct clk_hw * __init
+at91_clk_register_sam9x5_slow(void __iomem *sckcr,
+			      const char *name,
+			      const char **parent_names,
+			      int num_parents)
+{
+	struct clk_sam9x5_slow *slowck;
+	struct clk_hw *hw;
+	struct clk_init_data init;
+	int ret;
+
+	if (!sckcr || !name || !parent_names || !num_parents)
+		return ERR_PTR(-EINVAL);
+
+	slowck = kzalloc(sizeof(*slowck), GFP_KERNEL);
+	if (!slowck)
+		return ERR_PTR(-ENOMEM);
+
+	init.name = name;
+	init.ops = &sam9x5_slow_ops;
+	init.parent_names = parent_names;
+	init.num_parents = num_parents;
+	init.flags = 0;
+
+	slowck->hw.init = &init;
+	slowck->sckcr = sckcr;
+	slowck->parent = !!(readl(sckcr) & AT91_SCKC_OSCSEL);
+
+	hw = &slowck->hw;
+	ret = clk_hw_register(NULL, &slowck->hw);
+	if (ret) {
+		kfree(slowck);
+		hw = ERR_PTR(ret);
+	}
+
+	return hw;
+}
+
+static void __init
+of_at91sam9x5_clk_slow_setup(struct device_node *np, void __iomem *sckcr)
+{
+	struct clk_hw *hw;
+	const char *parent_names[2];
+	unsigned int num_parents;
+	const char *name = np->name;
+
+	num_parents = of_clk_get_parent_count(np);
+	if (num_parents == 0 || num_parents > 2)
+		return;
+
+	of_clk_parent_fill(np, parent_names, num_parents);
+
+	of_property_read_string(np, "clock-output-names", &name);
+
+	hw = at91_clk_register_sam9x5_slow(sckcr, name, parent_names,
+					    num_parents);
+	if (IS_ERR(hw))
+		return;
+
+	of_clk_add_hw_provider(np, of_clk_hw_simple_get, hw);
+}
 
 static const struct of_device_id sckc_clk_ids[] __initconst = {
 	/* Slow clock */
@@ -55,3 +426,94 @@ static void __init of_at91sam9x5_sckc_setup(struct device_node *np)
 }
 CLK_OF_DECLARE(at91sam9x5_clk_sckc, "atmel,at91sam9x5-sckc",
 	       of_at91sam9x5_sckc_setup);
+
+static int clk_sama5d4_slow_osc_prepare(struct clk_hw *hw)
+{
+	struct clk_sama5d4_slow_osc *osc = to_clk_sama5d4_slow_osc(hw);
+
+	if (osc->prepared)
+		return 0;
+
+	/*
+	 * Assume that if it has already been selected (for example by the
+	 * bootloader), enough time has aready passed.
+	 */
+	if ((readl(osc->sckcr) & AT91_SCKC_OSCSEL)) {
+		osc->prepared = true;
+		return 0;
+	}
+
+	usleep_range(osc->startup_usec, osc->startup_usec + 1);
+	osc->prepared = true;
+
+	return 0;
+}
+
+static int clk_sama5d4_slow_osc_is_prepared(struct clk_hw *hw)
+{
+	struct clk_sama5d4_slow_osc *osc = to_clk_sama5d4_slow_osc(hw);
+
+	return osc->prepared;
+}
+
+static const struct clk_ops sama5d4_slow_osc_ops = {
+	.prepare = clk_sama5d4_slow_osc_prepare,
+	.is_prepared = clk_sama5d4_slow_osc_is_prepared,
+};
+
+static void __init of_sama5d4_sckc_setup(struct device_node *np)
+{
+	void __iomem *regbase = of_iomap(np, 0);
+	struct clk_hw *hw;
+	struct clk_sama5d4_slow_osc *osc;
+	struct clk_init_data init;
+	const char *xtal_name;
+	const char *parent_names[2] = { "slow_rc_osc", "slow_osc" };
+	bool bypass;
+	int ret;
+
+	if (!regbase)
+		return;
+
+	hw = clk_hw_register_fixed_rate_with_accuracy(NULL, parent_names[0],
+						      NULL, 0, 32768,
+						      250000000);
+	if (IS_ERR(hw))
+		return;
+
+	xtal_name = of_clk_get_parent_name(np, 0);
+
+	bypass = of_property_read_bool(np, "atmel,osc-bypass");
+
+	osc = kzalloc(sizeof(*osc), GFP_KERNEL);
+	if (!osc)
+		return;
+
+	init.name = parent_names[1];
+	init.ops = &sama5d4_slow_osc_ops;
+	init.parent_names = &xtal_name;
+	init.num_parents = 1;
+	init.flags = CLK_IGNORE_UNUSED;
+
+	osc->hw.init = &init;
+	osc->sckcr = regbase;
+	osc->startup_usec = 1200000;
+
+	if (bypass)
+		writel((readl(regbase) | AT91_SCKC_OSC32BYP), regbase);
+
+	hw = &osc->hw;
+	ret = clk_hw_register(NULL, &osc->hw);
+	if (ret) {
+		kfree(osc);
+		return;
+	}
+
+	hw = at91_clk_register_sam9x5_slow(regbase, "slowck", parent_names, 2);
+	if (IS_ERR(hw))
+		return;
+
+	of_clk_add_hw_provider(np, of_clk_hw_simple_get, hw);
+}
+CLK_OF_DECLARE(sama5d4_clk_sckc, "atmel,sama5d4-sckc",
+	       of_sama5d4_sckc_setup);
diff --git a/drivers/clk/at91/sckc.h b/drivers/clk/at91/sckc.h
deleted file mode 100644
index 836fcf59820f..000000000000
--- a/drivers/clk/at91/sckc.h
+++ /dev/null
@@ -1,22 +0,0 @@
-/*
- * drivers/clk/at91/sckc.h
- *
- *  Copyright (C) 2013 Boris BREZILLON <b.brezillon@overkiz.com>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- */
-
-#ifndef __AT91_SCKC_H_
-#define __AT91_SCKC_H_
-
-extern void __init of_at91sam9x5_clk_slow_osc_setup(struct device_node *np,
-						    void __iomem *sckcr);
-extern void __init of_at91sam9x5_clk_slow_rc_osc_setup(struct device_node *np,
-						       void __iomem *sckcr);
-extern void __init of_at91sam9x5_clk_slow_setup(struct device_node *np,
-						void __iomem *sckcr);
-
-#endif /* __AT91_SCKC_H_ */
diff --git a/drivers/clk/axis/clk-artpec6.c b/drivers/clk/axis/clk-artpec6.c
index ffc988b098e4..da1a073c2236 100644
--- a/drivers/clk/axis/clk-artpec6.c
+++ b/drivers/clk/axis/clk-artpec6.c
@@ -113,8 +113,8 @@ static void of_artpec6_clkctrl_setup(struct device_node *np)
 	of_clk_add_provider(np, of_clk_src_onecell_get, &clkdata->clk_data);
 }
 
-CLK_OF_DECLARE(artpec6_clkctrl, "axis,artpec6-clkctrl",
-	       of_artpec6_clkctrl_setup);
+CLK_OF_DECLARE_DRIVER(artpec6_clkctrl, "axis,artpec6-clkctrl",
+		      of_artpec6_clkctrl_setup);
 
 static int artpec6_clkctrl_probe(struct platform_device *pdev)
 {
diff --git a/drivers/clk/bcm/Kconfig b/drivers/clk/bcm/Kconfig
index f2878459199a..f21e9b7afd1a 100644
--- a/drivers/clk/bcm/Kconfig
+++ b/drivers/clk/bcm/Kconfig
@@ -19,8 +19,36 @@ config CLK_BCM_KONA
 	  in the BCM281xx and BCM21664 families.
 
 config COMMON_CLK_IPROC
-	bool
+	bool "Broadcom iProc clock support"
+	depends on ARCH_BCM_IPROC || COMPILE_TEST
 	depends on COMMON_CLK
+	default ARCH_BCM_IPROC
 	help
 	  Enable common clock framework support for Broadcom SoCs
 	  based on the iProc architecture
+
+if COMMON_CLK_IPROC
+
+config CLK_BCM_CYGNUS
+	bool "Broadcom Cygnus clock support"
+	depends on ARCH_BCM_CYGNUS || COMPILE_TEST
+	default ARCH_BCM_CYGNUS
+	help
+	  Enable common clock framework support for the Broadcom Cygnus SoC
+
+config CLK_BCM_NSP
+	bool "Broadcom Northstar/Northstar Plus clock support"
+	depends on ARCH_BCM_5301X || ARCH_BCM_NSP || COMPILE_TEST
+	default ARCH_BCM_5301X || ARCH_BCM_NSP
+	help
+	  Enable common clock framework support for the Broadcom Northstar and
+	  Northstar Plus SoCs
+
+config CLK_BCM_NS2
+	bool "Broadcom Northstar 2 clock support"
+	depends on ARCH_BCM_IPROC || COMPILE_TEST
+	default ARCH_BCM_IPROC
+	help
+	  Enable common clock framework support for the Broadcom Northstar 2 SoC
+
+endif
diff --git a/drivers/clk/bcm/Makefile b/drivers/clk/bcm/Makefile
index 1d79bd2c36f0..d9dc848f18c9 100644
--- a/drivers/clk/bcm/Makefile
+++ b/drivers/clk/bcm/Makefile
@@ -6,7 +6,7 @@ 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_BCM2835)	+= clk-bcm2835.o
 obj-$(CONFIG_ARCH_BCM2835)	+= clk-bcm2835-aux.o
-obj-$(CONFIG_COMMON_CLK_IPROC)	+= clk-ns2.o
-obj-$(CONFIG_ARCH_BCM_CYGNUS)	+= clk-cygnus.o
-obj-$(CONFIG_ARCH_BCM_NSP)	+= clk-nsp.o
-obj-$(CONFIG_ARCH_BCM_5301X)	+= clk-nsp.o
+obj-$(CONFIG_ARCH_BCM_53573)	+= clk-bcm53573-ilp.o
+obj-$(CONFIG_CLK_BCM_CYGNUS)	+= clk-cygnus.o
+obj-$(CONFIG_CLK_BCM_NSP)	+= clk-nsp.o
+obj-$(CONFIG_CLK_BCM_NS2)	+= clk-ns2.o
diff --git a/drivers/clk/bcm/clk-bcm2835-aux.c b/drivers/clk/bcm/clk-bcm2835-aux.c
index 3a177ade6e6c..bd750cf2238d 100644
--- a/drivers/clk/bcm/clk-bcm2835-aux.c
+++ b/drivers/clk/bcm/clk-bcm2835-aux.c
@@ -25,7 +25,7 @@
 static int bcm2835_aux_clk_probe(struct platform_device *pdev)
 {
 	struct device *dev = &pdev->dev;
-	struct clk_onecell_data *onecell;
+	struct clk_hw_onecell_data *onecell;
 	const char *parent;
 	struct clk *parent_clk;
 	struct resource *res;
@@ -41,28 +41,24 @@ static int bcm2835_aux_clk_probe(struct platform_device *pdev)
 	if (IS_ERR(reg))
 		return PTR_ERR(reg);
 
-	onecell = devm_kmalloc(dev, sizeof(*onecell), GFP_KERNEL);
+	onecell = devm_kmalloc(dev, sizeof(*onecell) + sizeof(*onecell->hws) *
+			       BCM2835_AUX_CLOCK_COUNT, GFP_KERNEL);
 	if (!onecell)
 		return -ENOMEM;
-	onecell->clk_num = BCM2835_AUX_CLOCK_COUNT;
-	onecell->clks = devm_kcalloc(dev, BCM2835_AUX_CLOCK_COUNT,
-				     sizeof(*onecell->clks), GFP_KERNEL);
-	if (!onecell->clks)
-		return -ENOMEM;
+	onecell->num = BCM2835_AUX_CLOCK_COUNT;
 
 	gate = reg + BCM2835_AUXENB;
-	onecell->clks[BCM2835_AUX_CLOCK_UART] =
-		clk_register_gate(dev, "aux_uart", parent, 0, gate, 0, 0, NULL);
-
-	onecell->clks[BCM2835_AUX_CLOCK_SPI1] =
-		clk_register_gate(dev, "aux_spi1", parent, 0, gate, 1, 0, NULL);
+	onecell->hws[BCM2835_AUX_CLOCK_UART] =
+		clk_hw_register_gate(dev, "aux_uart", parent, 0, gate, 0, 0, NULL);
 
-	onecell->clks[BCM2835_AUX_CLOCK_SPI2] =
-		clk_register_gate(dev, "aux_spi2", parent, 0, gate, 2, 0, NULL);
+	onecell->hws[BCM2835_AUX_CLOCK_SPI1] =
+		clk_hw_register_gate(dev, "aux_spi1", parent, 0, gate, 1, 0, NULL);
 
-	of_clk_add_provider(pdev->dev.of_node, of_clk_src_onecell_get, onecell);
+	onecell->hws[BCM2835_AUX_CLOCK_SPI2] =
+		clk_hw_register_gate(dev, "aux_spi2", parent, 0, gate, 2, 0, NULL);
 
-	return 0;
+	return of_clk_add_hw_provider(pdev->dev.of_node, of_clk_hw_onecell_get,
+				      onecell);
 }
 
 static const struct of_device_id bcm2835_aux_clk_of_match[] = {
diff --git a/drivers/clk/bcm/clk-bcm2835.c b/drivers/clk/bcm/clk-bcm2835.c
index 7a7970865c2d..b68bf573dcfb 100644
--- a/drivers/clk/bcm/clk-bcm2835.c
+++ b/drivers/clk/bcm/clk-bcm2835.c
@@ -36,6 +36,7 @@
 
 #include <linux/clk-provider.h>
 #include <linux/clkdev.h>
+#include <linux/clk.h>
 #include <linux/clk/bcm2835.h>
 #include <linux/debugfs.h>
 #include <linux/module.h>
@@ -302,8 +303,8 @@ struct bcm2835_cprman {
 	spinlock_t regs_lock; /* spinlock for all clocks */
 	const char *osc_name;
 
-	struct clk_onecell_data onecell;
-	struct clk *clks[];
+	/* Must be last */
+	struct clk_hw_onecell_data onecell;
 };
 
 static inline void cprman_write(struct bcm2835_cprman *cprman, u32 reg, u32 val)
@@ -344,24 +345,24 @@ static int bcm2835_debugfs_regset(struct bcm2835_cprman *cprman, u32 base,
  */
 void __init bcm2835_init_clocks(void)
 {
-	struct clk *clk;
+	struct clk_hw *hw;
 	int ret;
 
-	clk = clk_register_fixed_rate(NULL, "apb_pclk", NULL, 0, 126000000);
-	if (IS_ERR(clk))
+	hw = clk_hw_register_fixed_rate(NULL, "apb_pclk", NULL, 0, 126000000);
+	if (IS_ERR(hw))
 		pr_err("apb_pclk not registered\n");
 
-	clk = clk_register_fixed_rate(NULL, "uart0_pclk", NULL, 0, 3000000);
-	if (IS_ERR(clk))
+	hw = clk_hw_register_fixed_rate(NULL, "uart0_pclk", NULL, 0, 3000000);
+	if (IS_ERR(hw))
 		pr_err("uart0_pclk not registered\n");
-	ret = clk_register_clkdev(clk, NULL, "20201000.uart");
+	ret = clk_hw_register_clkdev(hw, NULL, "20201000.uart");
 	if (ret)
 		pr_err("uart0_pclk alias not registered\n");
 
-	clk = clk_register_fixed_rate(NULL, "uart1_pclk", NULL, 0, 125000000);
-	if (IS_ERR(clk))
+	hw = clk_hw_register_fixed_rate(NULL, "uart1_pclk", NULL, 0, 125000000);
+	if (IS_ERR(hw))
 		pr_err("uart1_pclk not registered\n");
-	ret = clk_register_clkdev(clk, NULL, "20215000.uart");
+	ret = clk_hw_register_clkdev(hw, NULL, "20215000.uart");
 	if (ret)
 		pr_err("uart1_pclk alias not registered\n");
 }
@@ -443,6 +444,8 @@ struct bcm2835_clock_data {
 	/* Number of fractional bits in the divider */
 	u32 frac_bits;
 
+	u32 flags;
+
 	bool is_vpu_clock;
 	bool is_mash_clock;
 };
@@ -1006,16 +1009,28 @@ static int bcm2835_clock_set_rate(struct clk_hw *hw,
 	return 0;
 }
 
+static bool
+bcm2835_clk_is_pllc(struct clk_hw *hw)
+{
+	if (!hw)
+		return false;
+
+	return strncmp(clk_hw_get_name(hw), "pllc", 4) == 0;
+}
+
 static int bcm2835_clock_determine_rate(struct clk_hw *hw,
 					struct clk_rate_request *req)
 {
 	struct bcm2835_clock *clock = bcm2835_clock_from_hw(hw);
 	struct clk_hw *parent, *best_parent = NULL;
+	bool current_parent_is_pllc;
 	unsigned long rate, best_rate = 0;
 	unsigned long prate, best_prate = 0;
 	size_t i;
 	u32 div;
 
+	current_parent_is_pllc = bcm2835_clk_is_pllc(clk_hw_get_parent(hw));
+
 	/*
 	 * Select parent clock that results in the closest but lower rate
 	 */
@@ -1023,6 +1038,17 @@ static int bcm2835_clock_determine_rate(struct clk_hw *hw,
 		parent = clk_hw_get_parent_by_index(hw, i);
 		if (!parent)
 			continue;
+
+		/*
+		 * Don't choose a PLLC-derived clock as our parent
+		 * unless it had been manually set that way.  PLLC's
+		 * frequency gets adjusted by the firmware due to
+		 * over-temp or under-voltage conditions, without
+		 * prior notification to our clock consumer.
+		 */
+		if (bcm2835_clk_is_pllc(parent) && !current_parent_is_pllc)
+			continue;
+
 		prate = clk_hw_get_rate(parent);
 		div = bcm2835_clock_choose_div(hw, req->rate, prate, true);
 		rate = bcm2835_clock_rate_from_divisor(clock, prate, div);
@@ -1121,11 +1147,12 @@ static const struct clk_ops bcm2835_vpu_clock_clk_ops = {
 	.debug_init = bcm2835_clock_debug_init,
 };
 
-static struct clk *bcm2835_register_pll(struct bcm2835_cprman *cprman,
-					const struct bcm2835_pll_data *data)
+static struct clk_hw *bcm2835_register_pll(struct bcm2835_cprman *cprman,
+					   const struct bcm2835_pll_data *data)
 {
 	struct bcm2835_pll *pll;
 	struct clk_init_data init;
+	int ret;
 
 	memset(&init, 0, sizeof(init));
 
@@ -1144,17 +1171,20 @@ static struct clk *bcm2835_register_pll(struct bcm2835_cprman *cprman,
 	pll->data = data;
 	pll->hw.init = &init;
 
-	return devm_clk_register(cprman->dev, &pll->hw);
+	ret = devm_clk_hw_register(cprman->dev, &pll->hw);
+	if (ret)
+		return NULL;
+	return &pll->hw;
 }
 
-static struct clk *
+static struct clk_hw *
 bcm2835_register_pll_divider(struct bcm2835_cprman *cprman,
 			     const struct bcm2835_pll_divider_data *data)
 {
 	struct bcm2835_pll_divider *divider;
 	struct clk_init_data init;
-	struct clk *clk;
 	const char *divider_name;
+	int ret;
 
 	if (data->fixed_divider != 1) {
 		divider_name = devm_kasprintf(cprman->dev, GFP_KERNEL,
@@ -1188,32 +1218,33 @@ bcm2835_register_pll_divider(struct bcm2835_cprman *cprman,
 	divider->cprman = cprman;
 	divider->data = data;
 
-	clk = devm_clk_register(cprman->dev, &divider->div.hw);
-	if (IS_ERR(clk))
-		return clk;
+	ret = devm_clk_hw_register(cprman->dev, &divider->div.hw);
+	if (ret)
+		return ERR_PTR(ret);
 
 	/*
 	 * PLLH's channels have a fixed divide by 10 afterwards, which
 	 * is what our consumers are actually using.
 	 */
 	if (data->fixed_divider != 1) {
-		return clk_register_fixed_factor(cprman->dev, data->name,
-						 divider_name,
-						 CLK_SET_RATE_PARENT,
-						 1,
-						 data->fixed_divider);
+		return clk_hw_register_fixed_factor(cprman->dev, data->name,
+						    divider_name,
+						    CLK_SET_RATE_PARENT,
+						    1,
+						    data->fixed_divider);
 	}
 
-	return clk;
+	return &divider->div.hw;
 }
 
-static struct clk *bcm2835_register_clock(struct bcm2835_cprman *cprman,
+static struct clk_hw *bcm2835_register_clock(struct bcm2835_cprman *cprman,
 					  const struct bcm2835_clock_data *data)
 {
 	struct bcm2835_clock *clock;
 	struct clk_init_data init;
 	const char *parents[1 << CM_SRC_BITS];
 	size_t i;
+	int ret;
 
 	/*
 	 * Replace our "xosc" references with the oscillator's
@@ -1230,13 +1261,19 @@ static struct clk *bcm2835_register_clock(struct bcm2835_cprman *cprman,
 	init.parent_names = parents;
 	init.num_parents = data->num_mux_parents;
 	init.name = data->name;
-	init.flags = CLK_IGNORE_UNUSED;
+	init.flags = data->flags | CLK_IGNORE_UNUSED;
 
 	if (data->is_vpu_clock) {
 		init.ops = &bcm2835_vpu_clock_clk_ops;
 	} else {
 		init.ops = &bcm2835_clock_clk_ops;
 		init.flags |= CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE;
+
+		/* If the clock wasn't actually enabled at boot, it's not
+		 * critical.
+		 */
+		if (!(cprman_read(cprman, data->ctl_reg) & CM_ENABLE))
+			init.flags &= ~CLK_IS_CRITICAL;
 	}
 
 	clock = devm_kzalloc(cprman->dev, sizeof(*clock), GFP_KERNEL);
@@ -1247,7 +1284,10 @@ static struct clk *bcm2835_register_clock(struct bcm2835_cprman *cprman,
 	clock->data = data;
 	clock->hw.init = &init;
 
-	return devm_clk_register(cprman->dev, &clock->hw);
+	ret = devm_clk_hw_register(cprman->dev, &clock->hw);
+	if (ret)
+		return ERR_PTR(ret);
+	return &clock->hw;
 }
 
 static struct clk *bcm2835_register_gate(struct bcm2835_cprman *cprman,
@@ -1259,8 +1299,8 @@ static struct clk *bcm2835_register_gate(struct bcm2835_cprman *cprman,
 				 CM_GATE_BIT, 0, &cprman->regs_lock);
 }
 
-typedef struct clk *(*bcm2835_clk_register)(struct bcm2835_cprman *cprman,
-					    const void *data);
+typedef struct clk_hw *(*bcm2835_clk_register)(struct bcm2835_cprman *cprman,
+					       const void *data);
 struct bcm2835_clk_desc {
 	bcm2835_clk_register clk_register;
 	const void *data;
@@ -1649,6 +1689,7 @@ static const struct bcm2835_clk_desc clk_desc_array[] = {
 		.div_reg = CM_VPUDIV,
 		.int_bits = 12,
 		.frac_bits = 8,
+		.flags = CLK_IS_CRITICAL,
 		.is_vpu_clock = true),
 
 	/* clocks with per parent mux */
@@ -1705,13 +1746,15 @@ static const struct bcm2835_clk_desc clk_desc_array[] = {
 		.div_reg = CM_GP1DIV,
 		.int_bits = 12,
 		.frac_bits = 12,
+		.flags = CLK_IS_CRITICAL,
 		.is_mash_clock = true),
 	[BCM2835_CLOCK_GP2]	= REGISTER_PER_CLK(
 		.name = "gp2",
 		.ctl_reg = CM_GP2CTL,
 		.div_reg = CM_GP2DIV,
 		.int_bits = 12,
-		.frac_bits = 12),
+		.frac_bits = 12,
+		.flags = CLK_IS_CRITICAL),
 
 	/* HDMI state machine */
 	[BCM2835_CLOCK_HSM]	= REGISTER_PER_CLK(
@@ -1790,18 +1833,38 @@ static const struct bcm2835_clk_desc clk_desc_array[] = {
 		.ctl_reg = CM_PERIICTL),
 };
 
+/*
+ * Permanently take a reference on the parent of the SDRAM clock.
+ *
+ * While the SDRAM is being driven by its dedicated PLL most of the
+ * time, there is a little loop running in the firmware that
+ * periodically switches the SDRAM to using our CM clock to do PVT
+ * recalibration, with the assumption that the previously configured
+ * SDRAM parent is still enabled and running.
+ */
+static int bcm2835_mark_sdc_parent_critical(struct clk *sdc)
+{
+	struct clk *parent = clk_get_parent(sdc);
+
+	if (IS_ERR(parent))
+		return PTR_ERR(parent);
+
+	return clk_prepare_enable(parent);
+}
+
 static int bcm2835_clk_probe(struct platform_device *pdev)
 {
 	struct device *dev = &pdev->dev;
-	struct clk **clks;
+	struct clk_hw **hws;
 	struct bcm2835_cprman *cprman;
 	struct resource *res;
 	const struct bcm2835_clk_desc *desc;
 	const size_t asize = ARRAY_SIZE(clk_desc_array);
 	size_t i;
+	int ret;
 
-	cprman = devm_kzalloc(dev,
-			      sizeof(*cprman) + asize * sizeof(*clks),
+	cprman = devm_kzalloc(dev, sizeof(*cprman) +
+			      sizeof(*cprman->onecell.hws) * asize,
 			      GFP_KERNEL);
 	if (!cprman)
 		return -ENOMEM;
@@ -1819,18 +1882,21 @@ static int bcm2835_clk_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, cprman);
 
-	cprman->onecell.clk_num = asize;
-	cprman->onecell.clks = cprman->clks;
-	clks = cprman->clks;
+	cprman->onecell.num = asize;
+	hws = cprman->onecell.hws;
 
 	for (i = 0; i < asize; i++) {
 		desc = &clk_desc_array[i];
 		if (desc->clk_register && desc->data)
-			clks[i] = desc->clk_register(cprman, desc->data);
+			hws[i] = desc->clk_register(cprman, desc->data);
 	}
 
-	return of_clk_add_provider(dev->of_node, of_clk_src_onecell_get,
-				   &cprman->onecell);
+	ret = bcm2835_mark_sdc_parent_critical(hws[BCM2835_CLOCK_SDRAM]->clk);
+	if (ret)
+		return ret;
+
+	return of_clk_add_hw_provider(dev->of_node, of_clk_hw_onecell_get,
+				      &cprman->onecell);
 }
 
 static const struct of_device_id bcm2835_clk_of_match[] = {
diff --git a/drivers/clk/bcm/clk-bcm53573-ilp.c b/drivers/clk/bcm/clk-bcm53573-ilp.c
new file mode 100644
index 000000000000..36eb3716ffb0
--- /dev/null
+++ b/drivers/clk/bcm/clk-bcm53573-ilp.c
@@ -0,0 +1,148 @@
+/*
+ * Copyright (C) 2016 Rafał Miłecki <rafal@milecki.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.
+ */
+
+#include <linux/clk-provider.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/mfd/syscon.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/regmap.h>
+#include <linux/slab.h>
+
+#define PMU_XTAL_FREQ_RATIO			0x66c
+#define  XTAL_ALP_PER_4ILP			0x00001fff
+#define  XTAL_CTL_EN				0x80000000
+#define PMU_SLOW_CLK_PERIOD			0x6dc
+
+struct bcm53573_ilp {
+	struct clk_hw hw;
+	struct regmap *regmap;
+};
+
+static int bcm53573_ilp_enable(struct clk_hw *hw)
+{
+	struct bcm53573_ilp *ilp = container_of(hw, struct bcm53573_ilp, hw);
+
+	regmap_write(ilp->regmap, PMU_SLOW_CLK_PERIOD, 0x10199);
+	regmap_write(ilp->regmap, 0x674, 0x10000);
+
+	return 0;
+}
+
+static void bcm53573_ilp_disable(struct clk_hw *hw)
+{
+	struct bcm53573_ilp *ilp = container_of(hw, struct bcm53573_ilp, hw);
+
+	regmap_write(ilp->regmap, PMU_SLOW_CLK_PERIOD, 0);
+	regmap_write(ilp->regmap, 0x674, 0);
+}
+
+static unsigned long bcm53573_ilp_recalc_rate(struct clk_hw *hw,
+					      unsigned long parent_rate)
+{
+	struct bcm53573_ilp *ilp = container_of(hw, struct bcm53573_ilp, hw);
+	struct regmap *regmap = ilp->regmap;
+	u32 last_val, cur_val;
+	int sum = 0, num = 0, loop_num = 0;
+	int avg;
+
+	/* Enable measurement */
+	regmap_write(regmap, PMU_XTAL_FREQ_RATIO, XTAL_CTL_EN);
+
+	/* Read initial value */
+	regmap_read(regmap, PMU_XTAL_FREQ_RATIO, &last_val);
+	last_val &= XTAL_ALP_PER_4ILP;
+
+	/*
+	 * At minimum we should loop for a bit to let hardware do the
+	 * measurement. This isn't very accurate however, so for a better
+	 * precision lets try getting 20 different values for and use average.
+	 */
+	while (num < 20) {
+		regmap_read(regmap, PMU_XTAL_FREQ_RATIO, &cur_val);
+		cur_val &= XTAL_ALP_PER_4ILP;
+
+		if (cur_val != last_val) {
+			/* Got different value, use it */
+			sum += cur_val;
+			num++;
+			loop_num = 0;
+			last_val = cur_val;
+		} else if (++loop_num > 5000) {
+			/* Same value over and over, give up */
+			sum += cur_val;
+			num++;
+			break;
+		}
+
+		cpu_relax();
+	}
+
+	/* Disable measurement to save power */
+	regmap_write(regmap, PMU_XTAL_FREQ_RATIO, 0x0);
+
+	avg = sum / num;
+
+	return parent_rate * 4 / avg;
+}
+
+static const struct clk_ops bcm53573_ilp_clk_ops = {
+	.enable = bcm53573_ilp_enable,
+	.disable = bcm53573_ilp_disable,
+	.recalc_rate = bcm53573_ilp_recalc_rate,
+};
+
+static void bcm53573_ilp_init(struct device_node *np)
+{
+	struct bcm53573_ilp *ilp;
+	struct clk_init_data init = { };
+	const char *parent_name;
+	int err;
+
+	ilp = kzalloc(sizeof(*ilp), GFP_KERNEL);
+	if (!ilp)
+		return;
+
+	parent_name = of_clk_get_parent_name(np, 0);
+	if (!parent_name) {
+		err = -ENOENT;
+		goto err_free_ilp;
+	}
+
+	ilp->regmap = syscon_node_to_regmap(of_get_parent(np));
+	if (IS_ERR(ilp->regmap)) {
+		err = PTR_ERR(ilp->regmap);
+		goto err_free_ilp;
+	}
+
+	init.name = np->name;
+	init.ops = &bcm53573_ilp_clk_ops;
+	init.parent_names = &parent_name;
+	init.num_parents = 1;
+
+	ilp->hw.init = &init;
+	err = clk_hw_register(NULL, &ilp->hw);
+	if (err)
+		goto err_free_ilp;
+
+	err = of_clk_add_hw_provider(np, of_clk_hw_simple_get, &ilp->hw);
+	if (err)
+		goto err_clk_hw_unregister;
+
+	return;
+
+err_clk_hw_unregister:
+	clk_hw_unregister(&ilp->hw);
+err_free_ilp:
+	kfree(ilp);
+	pr_err("Failed to init ILP clock: %d\n", err);
+}
+
+/* We need it very early for arch code, before device model gets ready */
+CLK_OF_DECLARE(bcm53573_ilp_clk, "brcm,bcm53573-ilp", bcm53573_ilp_init);
diff --git a/drivers/clk/bcm/clk-kona-setup.c b/drivers/clk/bcm/clk-kona-setup.c
index 526b0b0e9a9f..c37a7f0e83aa 100644
--- a/drivers/clk/bcm/clk-kona-setup.c
+++ b/drivers/clk/bcm/clk-kona-setup.c
@@ -586,8 +586,8 @@ static u32 *parent_process(const char *clocks[],
 	}
 
 	/* There is at least one parent, so allocate a selector array */
-
-	parent_sel = kmalloc(parent_count * sizeof(*parent_sel), GFP_KERNEL);
+	parent_sel = kmalloc_array(parent_count, sizeof(*parent_sel),
+				   GFP_KERNEL);
 	if (!parent_sel) {
 		pr_err("%s: error allocating %u parent selectors\n", __func__,
 				parent_count);
@@ -696,77 +696,69 @@ static void bcm_clk_teardown(struct kona_clk *bcm_clk)
 	bcm_clk->type = bcm_clk_none;
 }
 
-static void kona_clk_teardown(struct clk *clk)
+static void kona_clk_teardown(struct clk_hw *hw)
 {
-	struct clk_hw *hw;
 	struct kona_clk *bcm_clk;
 
-	if (!clk)
+	if (!hw)
 		return;
 
-	hw = __clk_get_hw(clk);
-	if (!hw) {
-		pr_err("%s: clk %p has null hw pointer\n", __func__, clk);
-		return;
-	}
-	clk_unregister(clk);
+	clk_hw_unregister(hw);
 
 	bcm_clk = to_kona_clk(hw);
 	bcm_clk_teardown(bcm_clk);
 }
 
-struct clk *kona_clk_setup(struct kona_clk *bcm_clk)
+static int kona_clk_setup(struct kona_clk *bcm_clk)
 {
+	int ret;
 	struct clk_init_data *init_data = &bcm_clk->init_data;
-	struct clk *clk = NULL;
 
 	switch (bcm_clk->type) {
 	case bcm_clk_peri:
-		if (peri_clk_setup(bcm_clk->u.data, init_data))
-			return NULL;
+		ret = peri_clk_setup(bcm_clk->u.data, init_data);
+		if (ret)
+			return ret;
 		break;
 	default:
 		pr_err("%s: clock type %d invalid for %s\n", __func__,
 			(int)bcm_clk->type, init_data->name);
-		return NULL;
+		return -EINVAL;
 	}
 
 	/* Make sure everything makes sense before we set it up */
 	if (!kona_clk_valid(bcm_clk)) {
 		pr_err("%s: clock data invalid for %s\n", __func__,
 			init_data->name);
+		ret = -EINVAL;
 		goto out_teardown;
 	}
 
 	bcm_clk->hw.init = init_data;
-	clk = clk_register(NULL, &bcm_clk->hw);
-	if (IS_ERR(clk)) {
-		pr_err("%s: error registering clock %s (%ld)\n", __func__,
-			init_data->name, PTR_ERR(clk));
+	ret = clk_hw_register(NULL, &bcm_clk->hw);
+	if (ret) {
+		pr_err("%s: error registering clock %s (%d)\n", __func__,
+			init_data->name, ret);
 		goto out_teardown;
 	}
-	BUG_ON(!clk);
 
-	return clk;
+	return 0;
 out_teardown:
 	bcm_clk_teardown(bcm_clk);
 
-	return NULL;
+	return ret;
 }
 
 static void ccu_clks_teardown(struct ccu_data *ccu)
 {
 	u32 i;
 
-	for (i = 0; i < ccu->clk_data.clk_num; i++)
-		kona_clk_teardown(ccu->clk_data.clks[i]);
-	kfree(ccu->clk_data.clks);
+	for (i = 0; i < ccu->clk_num; i++)
+		kona_clk_teardown(&ccu->kona_clks[i].hw);
 }
 
 static void kona_ccu_teardown(struct ccu_data *ccu)
 {
-	kfree(ccu->clk_data.clks);
-	ccu->clk_data.clks = NULL;
 	if (!ccu->base)
 		return;
 
@@ -793,6 +785,20 @@ static bool ccu_data_valid(struct ccu_data *ccu)
 	return true;
 }
 
+static struct clk_hw *
+of_clk_kona_onecell_get(struct of_phandle_args *clkspec, void *data)
+{
+	struct ccu_data *ccu = data;
+	unsigned int idx = clkspec->args[0];
+
+	if (idx >= ccu->clk_num) {
+		pr_err("%s: invalid index %u\n", __func__, idx);
+		return ERR_PTR(-EINVAL);
+	}
+
+	return &ccu->kona_clks[idx].hw;
+}
+
 /*
  * Set up a CCU.  Call the provided ccu_clks_setup callback to
  * initialize the array of clocks provided by the CCU.
@@ -805,18 +811,6 @@ void __init kona_dt_ccu_setup(struct ccu_data *ccu,
 	unsigned int i;
 	int ret;
 
-	if (ccu->clk_data.clk_num) {
-		size_t size;
-
-		size = ccu->clk_data.clk_num * sizeof(*ccu->clk_data.clks);
-		ccu->clk_data.clks = kzalloc(size, GFP_KERNEL);
-		if (!ccu->clk_data.clks) {
-			pr_err("%s: unable to allocate %u clocks for %s\n",
-				__func__, ccu->clk_data.clk_num, node->name);
-			return;
-		}
-	}
-
 	ret = of_address_to_resource(node, 0, &res);
 	if (ret) {
 		pr_err("%s: no valid CCU registers found for %s\n", __func__,
@@ -851,13 +845,13 @@ void __init kona_dt_ccu_setup(struct ccu_data *ccu,
 	 * the clock framework clock array (in ccu->data).  Then
 	 * register as a provider for these clocks.
 	 */
-	for (i = 0; i < ccu->clk_data.clk_num; i++) {
+	for (i = 0; i < ccu->clk_num; i++) {
 		if (!ccu->kona_clks[i].ccu)
 			continue;
-		ccu->clk_data.clks[i] = kona_clk_setup(&ccu->kona_clks[i]);
+		kona_clk_setup(&ccu->kona_clks[i]);
 	}
 
-	ret = of_clk_add_provider(node, of_clk_src_onecell_get, &ccu->clk_data);
+	ret = of_clk_add_hw_provider(node, of_clk_kona_onecell_get, ccu);
 	if (ret) {
 		pr_err("%s: error adding ccu %s as provider (%d)\n", __func__,
 				node->name, ret);
diff --git a/drivers/clk/bcm/clk-kona.c b/drivers/clk/bcm/clk-kona.c
index 3a15347b4233..eee64b9e5d10 100644
--- a/drivers/clk/bcm/clk-kona.c
+++ b/drivers/clk/bcm/clk-kona.c
@@ -1256,19 +1256,18 @@ bool __init kona_ccu_init(struct ccu_data *ccu)
 {
 	unsigned long flags;
 	unsigned int which;
-	struct clk **clks = ccu->clk_data.clks;
 	struct kona_clk *kona_clks = ccu->kona_clks;
 	bool success = true;
 
 	flags = ccu_lock(ccu);
 	__ccu_write_enable(ccu);
 
-	for (which = 0; which < ccu->clk_data.clk_num; which++) {
-		struct kona_clk *bcm_clk;
+	for (which = 0; which < ccu->clk_num; which++) {
+		struct kona_clk *bcm_clk = &kona_clks[which];
 
-		if (!clks[which])
+		if (!bcm_clk->ccu)
 			continue;
-		bcm_clk = &kona_clks[which];
+
 		success &= __kona_clk_init(bcm_clk);
 	}
 
diff --git a/drivers/clk/bcm/clk-kona.h b/drivers/clk/bcm/clk-kona.h
index 906576ec97b6..f4b39bb5558a 100644
--- a/drivers/clk/bcm/clk-kona.h
+++ b/drivers/clk/bcm/clk-kona.h
@@ -481,7 +481,7 @@ struct ccu_data {
 	bool write_enabled;	/* write access is currently enabled */
 	struct ccu_policy policy;
 	struct device_node *node;
-	struct clk_onecell_data clk_data;
+	size_t clk_num;
 	const char *name;
 	u32 range;		/* byte range of address space */
 	struct kona_clk kona_clks[];	/* must be last */
@@ -491,9 +491,7 @@ struct ccu_data {
 #define KONA_CCU_COMMON(_prefix, _name, _ccuname)			    \
 	.name		= #_name "_ccu",				    \
 	.lock		= __SPIN_LOCK_UNLOCKED(_name ## _ccu_data.lock),    \
-	.clk_data	= {						    \
-		.clk_num = _prefix ## _ ## _ccuname ## _CCU_CLOCK_COUNT,    \
-	}
+	.clk_num	= _prefix ## _ ## _ccuname ## _CCU_CLOCK_COUNT
 
 /* Exported globals */
 
@@ -505,7 +503,6 @@ extern u64 scaled_div_max(struct bcm_clk_div *div);
 extern u64 scaled_div_build(struct bcm_clk_div *div, u32 div_value,
 				u32 billionths);
 
-extern struct clk *kona_clk_setup(struct kona_clk *bcm_clk);
 extern void __init kona_dt_ccu_setup(struct ccu_data *ccu,
 				struct device_node *node);
 extern bool __init kona_ccu_init(struct ccu_data *ccu);
diff --git a/drivers/clk/berlin/berlin2-avpll.c b/drivers/clk/berlin/berlin2-avpll.c
index fd0f26c38465..cfcae468e989 100644
--- a/drivers/clk/berlin/berlin2-avpll.c
+++ b/drivers/clk/berlin/berlin2-avpll.c
@@ -188,7 +188,7 @@ static const struct clk_ops berlin2_avpll_vco_ops = {
 	.recalc_rate	= berlin2_avpll_vco_recalc_rate,
 };
 
-struct clk * __init berlin2_avpll_vco_register(void __iomem *base,
+int __init berlin2_avpll_vco_register(void __iomem *base,
 			       const char *name, const char *parent_name,
 			       u8 vco_flags, unsigned long flags)
 {
@@ -197,7 +197,7 @@ struct clk * __init berlin2_avpll_vco_register(void __iomem *base,
 
 	vco = kzalloc(sizeof(*vco), GFP_KERNEL);
 	if (!vco)
-		return ERR_PTR(-ENOMEM);
+		return -ENOMEM;
 
 	vco->base = base;
 	vco->flags = vco_flags;
@@ -208,7 +208,7 @@ struct clk * __init berlin2_avpll_vco_register(void __iomem *base,
 	init.num_parents = 1;
 	init.flags = flags;
 
-	return clk_register(NULL, &vco->hw);
+	return clk_hw_register(NULL, &vco->hw);
 }
 
 struct berlin2_avpll_channel {
@@ -364,7 +364,7 @@ static const struct clk_ops berlin2_avpll_channel_ops = {
  */
 static const u8 quirk_index[] __initconst = { 0, 6, 5, 4, 3, 2, 1, 7 };
 
-struct clk * __init berlin2_avpll_channel_register(void __iomem *base,
+int __init berlin2_avpll_channel_register(void __iomem *base,
 			   const char *name, u8 index, const char *parent_name,
 			   u8 ch_flags, unsigned long flags)
 {
@@ -373,7 +373,7 @@ struct clk * __init berlin2_avpll_channel_register(void __iomem *base,
 
 	ch = kzalloc(sizeof(*ch), GFP_KERNEL);
 	if (!ch)
-		return ERR_PTR(-ENOMEM);
+		return -ENOMEM;
 
 	ch->base = base;
 	if (ch_flags & BERLIN2_AVPLL_SCRAMBLE_QUIRK)
@@ -389,5 +389,5 @@ struct clk * __init berlin2_avpll_channel_register(void __iomem *base,
 	init.num_parents = 1;
 	init.flags = flags;
 
-	return clk_register(NULL, &ch->hw);
+	return clk_hw_register(NULL, &ch->hw);
 }
diff --git a/drivers/clk/berlin/berlin2-avpll.h b/drivers/clk/berlin/berlin2-avpll.h
index a37f5068d299..17e311153b42 100644
--- a/drivers/clk/berlin/berlin2-avpll.h
+++ b/drivers/clk/berlin/berlin2-avpll.h
@@ -19,17 +19,13 @@
 #ifndef __BERLIN2_AVPLL_H
 #define __BERLIN2_AVPLL_H
 
-struct clk;
-
 #define BERLIN2_AVPLL_BIT_QUIRK		BIT(0)
 #define BERLIN2_AVPLL_SCRAMBLE_QUIRK	BIT(1)
 
-struct clk * __init
-berlin2_avpll_vco_register(void __iomem *base, const char *name,
+int berlin2_avpll_vco_register(void __iomem *base, const char *name,
 	   const char *parent_name, u8 vco_flags, unsigned long flags);
 
-struct clk * __init
-berlin2_avpll_channel_register(void __iomem *base, const char *name,
+int berlin2_avpll_channel_register(void __iomem *base, const char *name,
 		       u8 index, const char *parent_name, u8 ch_flags,
 		       unsigned long flags);
 
diff --git a/drivers/clk/berlin/berlin2-div.c b/drivers/clk/berlin/berlin2-div.c
index 81ff97f8aa0b..41ab2d392c57 100644
--- a/drivers/clk/berlin/berlin2-div.c
+++ b/drivers/clk/berlin/berlin2-div.c
@@ -234,7 +234,7 @@ static const struct clk_ops berlin2_div_mux_ops = {
 	.get_parent	= berlin2_div_get_parent,
 };
 
-struct clk * __init
+struct clk_hw * __init
 berlin2_div_register(const struct berlin2_div_map *map,
 		     void __iomem *base, const char *name, u8 div_flags,
 		     const char **parent_names, int num_parents,
@@ -259,7 +259,7 @@ berlin2_div_register(const struct berlin2_div_map *map,
 	if ((div_flags & BERLIN2_DIV_HAS_MUX) == 0)
 		mux_ops = NULL;
 
-	return clk_register_composite(NULL, name, parent_names, num_parents,
+	return clk_hw_register_composite(NULL, name, parent_names, num_parents,
 				      &div->hw, mux_ops, &div->hw, rate_ops,
 				      &div->hw, gate_ops, flags);
 }
diff --git a/drivers/clk/berlin/berlin2-div.h b/drivers/clk/berlin/berlin2-div.h
index 15e3384f3116..e835ddf8374a 100644
--- a/drivers/clk/berlin/berlin2-div.h
+++ b/drivers/clk/berlin/berlin2-div.h
@@ -19,7 +19,7 @@
 #ifndef __BERLIN2_DIV_H
 #define __BERLIN2_DIV_H
 
-struct clk;
+struct clk_hw;
 
 #define BERLIN2_DIV_HAS_GATE		BIT(0)
 #define BERLIN2_DIV_HAS_MUX		BIT(1)
@@ -80,7 +80,7 @@ struct berlin2_div_data {
 	u8 div_flags;
 };
 
-struct clk * __init
+struct clk_hw *
 berlin2_div_register(const struct berlin2_div_map *map,
 	     void __iomem *base,  const char *name, u8 div_flags,
 	     const char **parent_names, int num_parents,
diff --git a/drivers/clk/berlin/berlin2-pll.c b/drivers/clk/berlin/berlin2-pll.c
index 1c2294d3ba85..4ffbe80f6323 100644
--- a/drivers/clk/berlin/berlin2-pll.c
+++ b/drivers/clk/berlin/berlin2-pll.c
@@ -84,7 +84,7 @@ static const struct clk_ops berlin2_pll_ops = {
 	.recalc_rate	= berlin2_pll_recalc_rate,
 };
 
-struct clk * __init
+int __init
 berlin2_pll_register(const struct berlin2_pll_map *map,
 		     void __iomem *base, const char *name,
 		     const char *parent_name, unsigned long flags)
@@ -94,7 +94,7 @@ berlin2_pll_register(const struct berlin2_pll_map *map,
 
 	pll = kzalloc(sizeof(*pll), GFP_KERNEL);
 	if (!pll)
-		return ERR_PTR(-ENOMEM);
+		return -ENOMEM;
 
 	/* copy pll_map to allow __initconst */
 	memcpy(&pll->map, map, sizeof(*map));
@@ -106,5 +106,5 @@ berlin2_pll_register(const struct berlin2_pll_map *map,
 	init.num_parents = 1;
 	init.flags = flags;
 
-	return clk_register(NULL, &pll->hw);
+	return clk_hw_register(NULL, &pll->hw);
 }
diff --git a/drivers/clk/berlin/berlin2-pll.h b/drivers/clk/berlin/berlin2-pll.h
index 8831ce27ac1e..583e024b9bed 100644
--- a/drivers/clk/berlin/berlin2-pll.h
+++ b/drivers/clk/berlin/berlin2-pll.h
@@ -19,8 +19,6 @@
 #ifndef __BERLIN2_PLL_H
 #define __BERLIN2_PLL_H
 
-struct clk;
-
 struct berlin2_pll_map {
 	const u8 vcodiv[16];
 	u8 mult;
@@ -29,9 +27,8 @@ struct berlin2_pll_map {
 	u8 divsel_shift;
 };
 
-struct clk * __init
-berlin2_pll_register(const struct berlin2_pll_map *map,
-		     void __iomem *base, const char *name,
-		     const char *parent_name, unsigned long flags);
+int berlin2_pll_register(const struct berlin2_pll_map *map,
+			 void __iomem *base, const char *name,
+			 const char *parent_name, unsigned long flags);
 
 #endif /* __BERLIN2_PLL_H */
diff --git a/drivers/clk/berlin/bg2.c b/drivers/clk/berlin/bg2.c
index 23e0e3be6c37..edf3b96b3b73 100644
--- a/drivers/clk/berlin/bg2.c
+++ b/drivers/clk/berlin/bg2.c
@@ -92,8 +92,7 @@
  */
 
 #define	MAX_CLKS 41
-static struct clk *clks[MAX_CLKS];
-static struct clk_onecell_data clk_data;
+static struct clk_hw_onecell_data *clk_data;
 static DEFINE_SPINLOCK(lock);
 static void __iomem *gbase;
 
@@ -505,8 +504,17 @@ 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;
+	struct clk_hw *hw;
+	struct clk_hw **hws;
 	u8 avpll_flags = 0;
-	int n;
+	int n, ret;
+
+	clk_data = kzalloc(sizeof(*clk_data) +
+			   sizeof(*clk_data->hws) * MAX_CLKS, GFP_KERNEL);
+	if (!clk_data)
+		return;
+	clk_data->num = MAX_CLKS;
+	hws = clk_data->hws;
 
 	gbase = of_iomap(parent_np, 0);
 	if (!gbase)
@@ -526,118 +534,118 @@ static void __init berlin2_clock_setup(struct device_node *np)
 	}
 
 	/* simple register PLLs */
-	clk = berlin2_pll_register(&bg2_pll_map, gbase + REG_SYSPLLCTL0,
+	ret = berlin2_pll_register(&bg2_pll_map, gbase + REG_SYSPLLCTL0,
 				   clk_names[SYSPLL], clk_names[REFCLK], 0);
-	if (IS_ERR(clk))
+	if (ret)
 		goto bg2_fail;
 
-	clk = berlin2_pll_register(&bg2_pll_map, gbase + REG_MEMPLLCTL0,
+	ret = berlin2_pll_register(&bg2_pll_map, gbase + REG_MEMPLLCTL0,
 				   clk_names[MEMPLL], clk_names[REFCLK], 0);
-	if (IS_ERR(clk))
+	if (ret)
 		goto bg2_fail;
 
-	clk = berlin2_pll_register(&bg2_pll_map, gbase + REG_CPUPLLCTL0,
+	ret = berlin2_pll_register(&bg2_pll_map, gbase + REG_CPUPLLCTL0,
 				   clk_names[CPUPLL], clk_names[REFCLK], 0);
-	if (IS_ERR(clk))
+	if (ret)
 		goto bg2_fail;
 
 	if (of_device_is_compatible(np, "marvell,berlin2-global-register"))
 		avpll_flags |= BERLIN2_AVPLL_SCRAMBLE_QUIRK;
 
 	/* audio/video VCOs */
-	clk = berlin2_avpll_vco_register(gbase + REG_AVPLLCTL0, "avpll_vcoA",
+	ret = berlin2_avpll_vco_register(gbase + REG_AVPLLCTL0, "avpll_vcoA",
 			 clk_names[REFCLK], avpll_flags, 0);
-	if (IS_ERR(clk))
+	if (ret)
 		goto bg2_fail;
 
 	for (n = 0; n < 8; n++) {
-		clk = berlin2_avpll_channel_register(gbase + REG_AVPLLCTL0,
+		ret = berlin2_avpll_channel_register(gbase + REG_AVPLLCTL0,
 			     clk_names[AVPLL_A1 + n], n, "avpll_vcoA",
 			     avpll_flags, 0);
-		if (IS_ERR(clk))
+		if (ret)
 			goto bg2_fail;
 	}
 
-	clk = berlin2_avpll_vco_register(gbase + REG_AVPLLCTL31, "avpll_vcoB",
+	ret = berlin2_avpll_vco_register(gbase + REG_AVPLLCTL31, "avpll_vcoB",
 				 clk_names[REFCLK], BERLIN2_AVPLL_BIT_QUIRK |
 				 avpll_flags, 0);
-	if (IS_ERR(clk))
+	if (ret)
 		goto bg2_fail;
 
 	for (n = 0; n < 8; n++) {
-		clk = berlin2_avpll_channel_register(gbase + REG_AVPLLCTL31,
+		ret = berlin2_avpll_channel_register(gbase + REG_AVPLLCTL31,
 			     clk_names[AVPLL_B1 + n], n, "avpll_vcoB",
 			     BERLIN2_AVPLL_BIT_QUIRK | avpll_flags, 0);
-		if (IS_ERR(clk))
+		if (ret)
 			goto bg2_fail;
 	}
 
 	/* reference clock bypass switches */
 	parent_names[0] = clk_names[SYSPLL];
 	parent_names[1] = clk_names[REFCLK];
-	clk = clk_register_mux(NULL, "syspll_byp", parent_names, 2,
+	hw = clk_hw_register_mux(NULL, "syspll_byp", parent_names, 2,
 			       0, gbase + REG_CLKSWITCH0, 0, 1, 0, &lock);
-	if (IS_ERR(clk))
+	if (IS_ERR(hw))
 		goto bg2_fail;
-	clk_names[SYSPLL] = __clk_get_name(clk);
+	clk_names[SYSPLL] = clk_hw_get_name(hw);
 
 	parent_names[0] = clk_names[MEMPLL];
 	parent_names[1] = clk_names[REFCLK];
-	clk = clk_register_mux(NULL, "mempll_byp", parent_names, 2,
+	hw = clk_hw_register_mux(NULL, "mempll_byp", parent_names, 2,
 			       0, gbase + REG_CLKSWITCH0, 1, 1, 0, &lock);
-	if (IS_ERR(clk))
+	if (IS_ERR(hw))
 		goto bg2_fail;
-	clk_names[MEMPLL] = __clk_get_name(clk);
+	clk_names[MEMPLL] = clk_hw_get_name(hw);
 
 	parent_names[0] = clk_names[CPUPLL];
 	parent_names[1] = clk_names[REFCLK];
-	clk = clk_register_mux(NULL, "cpupll_byp", parent_names, 2,
+	hw = clk_hw_register_mux(NULL, "cpupll_byp", parent_names, 2,
 			       0, gbase + REG_CLKSWITCH0, 2, 1, 0, &lock);
-	if (IS_ERR(clk))
+	if (IS_ERR(hw))
 		goto bg2_fail;
-	clk_names[CPUPLL] = __clk_get_name(clk);
+	clk_names[CPUPLL] = clk_hw_get_name(hw);
 
 	/* clock muxes */
 	parent_names[0] = clk_names[AVPLL_B3];
 	parent_names[1] = clk_names[AVPLL_A3];
-	clk = clk_register_mux(NULL, clk_names[AUDIO1_PLL], parent_names, 2,
+	hw = clk_hw_register_mux(NULL, clk_names[AUDIO1_PLL], parent_names, 2,
 			       0, gbase + REG_CLKSELECT2, 29, 1, 0, &lock);
-	if (IS_ERR(clk))
+	if (IS_ERR(hw))
 		goto bg2_fail;
 
 	parent_names[0] = clk_names[VIDEO0_PLL];
 	parent_names[1] = clk_names[VIDEO_EXT0];
-	clk = clk_register_mux(NULL, clk_names[VIDEO0_IN], parent_names, 2,
+	hw = clk_hw_register_mux(NULL, clk_names[VIDEO0_IN], parent_names, 2,
 			       0, gbase + REG_CLKSELECT3, 4, 1, 0, &lock);
-	if (IS_ERR(clk))
+	if (IS_ERR(hw))
 		goto bg2_fail;
 
 	parent_names[0] = clk_names[VIDEO1_PLL];
 	parent_names[1] = clk_names[VIDEO_EXT0];
-	clk = clk_register_mux(NULL, clk_names[VIDEO1_IN], parent_names, 2,
+	hw = clk_hw_register_mux(NULL, clk_names[VIDEO1_IN], parent_names, 2,
 			       0, gbase + REG_CLKSELECT3, 6, 1, 0, &lock);
-	if (IS_ERR(clk))
+	if (IS_ERR(hw))
 		goto bg2_fail;
 
 	parent_names[0] = clk_names[AVPLL_A2];
 	parent_names[1] = clk_names[AVPLL_B2];
-	clk = clk_register_mux(NULL, clk_names[VIDEO1_PLL], parent_names, 2,
+	hw = clk_hw_register_mux(NULL, clk_names[VIDEO1_PLL], parent_names, 2,
 			       0, gbase + REG_CLKSELECT3, 7, 1, 0, &lock);
-	if (IS_ERR(clk))
+	if (IS_ERR(hw))
 		goto bg2_fail;
 
 	parent_names[0] = clk_names[VIDEO2_PLL];
 	parent_names[1] = clk_names[VIDEO_EXT0];
-	clk = clk_register_mux(NULL, clk_names[VIDEO2_IN], parent_names, 2,
+	hw = clk_hw_register_mux(NULL, clk_names[VIDEO2_IN], parent_names, 2,
 			       0, gbase + REG_CLKSELECT3, 9, 1, 0, &lock);
-	if (IS_ERR(clk))
+	if (IS_ERR(hw))
 		goto bg2_fail;
 
 	parent_names[0] = clk_names[AVPLL_B1];
 	parent_names[1] = clk_names[AVPLL_A5];
-	clk = clk_register_mux(NULL, clk_names[VIDEO2_PLL], parent_names, 2,
+	hw = clk_hw_register_mux(NULL, clk_names[VIDEO2_PLL], parent_names, 2,
 			       0, gbase + REG_CLKSELECT3, 10, 1, 0, &lock);
-	if (IS_ERR(clk))
+	if (IS_ERR(hw))
 		goto bg2_fail;
 
 	/* clock divider cells */
@@ -648,7 +656,7 @@ static void __init berlin2_clock_setup(struct device_node *np)
 		for (k = 0; k < dd->num_parents; k++)
 			parent_names[k] = clk_names[dd->parent_ids[k]];
 
-		clks[CLKID_SYS + n] = berlin2_div_register(&dd->map, gbase,
+		hws[CLKID_SYS + n] = berlin2_div_register(&dd->map, gbase,
 				dd->name, dd->div_flags, parent_names,
 				dd->num_parents, dd->flags, &lock);
 	}
@@ -657,18 +665,18 @@ static void __init berlin2_clock_setup(struct device_node *np)
 	for (n = 0; n < ARRAY_SIZE(bg2_gates); n++) {
 		const struct berlin2_gate_data *gd = &bg2_gates[n];
 
-		clks[CLKID_GETH0 + n] = clk_register_gate(NULL, gd->name,
+		hws[CLKID_GETH0 + n] = clk_hw_register_gate(NULL, gd->name,
 			    gd->parent_name, gd->flags, gbase + REG_CLKENABLE,
 			    gd->bit_idx, 0, &lock);
 	}
 
 	/* twdclk is derived from cpu/3 */
-	clks[CLKID_TWD] =
-		clk_register_fixed_factor(NULL, "twd", "cpu", 0, 1, 3);
+	hws[CLKID_TWD] =
+		clk_hw_register_fixed_factor(NULL, "twd", "cpu", 0, 1, 3);
 
 	/* check for errors on leaf clocks */
 	for (n = 0; n < MAX_CLKS; n++) {
-		if (!IS_ERR(clks[n]))
+		if (!IS_ERR(hws[n]))
 			continue;
 
 		pr_err("%s: Unable to register leaf clock %d\n",
@@ -677,9 +685,7 @@ static void __init berlin2_clock_setup(struct device_node *np)
 	}
 
 	/* register clk-provider */
-	clk_data.clks = clks;
-	clk_data.clk_num = MAX_CLKS;
-	of_clk_add_provider(np, of_clk_src_onecell_get, &clk_data);
+	of_clk_add_hw_provider(np, of_clk_hw_onecell_get, &clk_data);
 
 	return;
 
diff --git a/drivers/clk/berlin/bg2q.c b/drivers/clk/berlin/bg2q.c
index f144547cf76c..0718e831475f 100644
--- a/drivers/clk/berlin/bg2q.c
+++ b/drivers/clk/berlin/bg2q.c
@@ -46,8 +46,7 @@
 #define REG_SDIO1XIN_CLKCTL	0x015c
 
 #define	MAX_CLKS 28
-static struct clk *clks[MAX_CLKS];
-static struct clk_onecell_data clk_data;
+static struct clk_hw_onecell_data *clk_data;
 static DEFINE_SPINLOCK(lock);
 static void __iomem *gbase;
 static void __iomem *cpupll_base;
@@ -293,7 +292,15 @@ 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;
+	struct clk_hw **hws;
+	int n, ret;
+
+	clk_data = kzalloc(sizeof(*clk_data) +
+			   sizeof(*clk_data->hws) * MAX_CLKS, GFP_KERNEL);
+	if (!clk_data)
+		return;
+	clk_data->num = MAX_CLKS;
+	hws = clk_data->hws;
 
 	gbase = of_iomap(parent_np, 0);
 	if (!gbase) {
@@ -317,14 +324,14 @@ static void __init berlin2q_clock_setup(struct device_node *np)
 	}
 
 	/* simple register PLLs */
-	clk = berlin2_pll_register(&bg2q_pll_map, gbase + REG_SYSPLLCTL0,
+	ret = berlin2_pll_register(&bg2q_pll_map, gbase + REG_SYSPLLCTL0,
 				   clk_names[SYSPLL], clk_names[REFCLK], 0);
-	if (IS_ERR(clk))
+	if (ret)
 		goto bg2q_fail;
 
-	clk = berlin2_pll_register(&bg2q_pll_map, cpupll_base,
+	ret = berlin2_pll_register(&bg2q_pll_map, cpupll_base,
 				   clk_names[CPUPLL], clk_names[REFCLK], 0);
-	if (IS_ERR(clk))
+	if (ret)
 		goto bg2q_fail;
 
 	/* TODO: add BG2Q AVPLL */
@@ -342,7 +349,7 @@ static void __init berlin2q_clock_setup(struct device_node *np)
 		for (k = 0; k < dd->num_parents; k++)
 			parent_names[k] = clk_names[dd->parent_ids[k]];
 
-		clks[CLKID_SYS + n] = berlin2_div_register(&dd->map, gbase,
+		hws[CLKID_SYS + n] = berlin2_div_register(&dd->map, gbase,
 				dd->name, dd->div_flags, parent_names,
 				dd->num_parents, dd->flags, &lock);
 	}
@@ -351,22 +358,22 @@ static void __init berlin2q_clock_setup(struct device_node *np)
 	for (n = 0; n < ARRAY_SIZE(bg2q_gates); n++) {
 		const struct berlin2_gate_data *gd = &bg2q_gates[n];
 
-		clks[CLKID_GFX2DAXI + n] = clk_register_gate(NULL, gd->name,
+		hws[CLKID_GFX2DAXI + n] = clk_hw_register_gate(NULL, gd->name,
 			    gd->parent_name, gd->flags, gbase + REG_CLKENABLE,
 			    gd->bit_idx, 0, &lock);
 	}
 
 	/* cpuclk divider is fixed to 1 */
-	clks[CLKID_CPU] =
-		clk_register_fixed_factor(NULL, "cpu", clk_names[CPUPLL],
+	hws[CLKID_CPU] =
+		clk_hw_register_fixed_factor(NULL, "cpu", clk_names[CPUPLL],
 					  0, 1, 1);
 	/* twdclk is derived from cpu/3 */
-	clks[CLKID_TWD] =
-		clk_register_fixed_factor(NULL, "twd", "cpu", 0, 1, 3);
+	hws[CLKID_TWD] =
+		clk_hw_register_fixed_factor(NULL, "twd", "cpu", 0, 1, 3);
 
 	/* check for errors on leaf clocks */
 	for (n = 0; n < MAX_CLKS; n++) {
-		if (!IS_ERR(clks[n]))
+		if (!IS_ERR(hws[n]))
 			continue;
 
 		pr_err("%s: Unable to register leaf clock %d\n",
@@ -375,9 +382,7 @@ static void __init berlin2q_clock_setup(struct device_node *np)
 	}
 
 	/* register clk-provider */
-	clk_data.clks = clks;
-	clk_data.clk_num = MAX_CLKS;
-	of_clk_add_provider(np, of_clk_src_onecell_get, &clk_data);
+	of_clk_add_hw_provider(np, of_clk_hw_onecell_get, &clk_data);
 
 	return;
 
diff --git a/drivers/clk/clk-asm9260.c b/drivers/clk/clk-asm9260.c
index 90897af8d9f7..ea8568536193 100644
--- a/drivers/clk/clk-asm9260.c
+++ b/drivers/clk/clk-asm9260.c
@@ -68,8 +68,7 @@
 #define HW_LCDCLKDIV		0x01fc
 #define HW_ADCANACLKDIV		0x0200
 
-static struct clk *clks[MAX_CLKS];
-static struct clk_onecell_data clk_data;
+static struct clk_hw_onecell_data *clk_data;
 static DEFINE_SPINLOCK(asm9260_clk_lock);
 
 struct asm9260_div_clk {
@@ -267,12 +266,20 @@ static struct asm9260_mux_clock asm9260_mux_clks[] __initdata = {
 
 static void __init asm9260_acc_init(struct device_node *np)
 {
-	struct clk *clk;
+	struct clk_hw *hw;
+	struct clk_hw **hws;
 	const char *ref_clk, *pll_clk = "pll";
 	u32 rate;
 	int n;
 	u32 accuracy = 0;
 
+	clk_data = kzalloc(sizeof(*clk_data) +
+			   sizeof(*clk_data->hws) * MAX_CLKS, GFP_KERNEL);
+	if (!clk_data)
+		return;
+	clk_data->num = MAX_CLKS;
+	hws = clk_data->hws;
+
 	base = of_io_request_and_map(np, 0, np->name);
 	if (IS_ERR(base))
 		panic("%s: unable to map resource", np->name);
@@ -282,10 +289,10 @@ static void __init asm9260_acc_init(struct device_node *np)
 
 	ref_clk = of_clk_get_parent_name(np, 0);
 	accuracy = clk_get_accuracy(__clk_lookup(ref_clk));
-	clk = clk_register_fixed_rate_with_accuracy(NULL, pll_clk,
+	hw = clk_hw_register_fixed_rate_with_accuracy(NULL, pll_clk,
 			ref_clk, 0, rate, accuracy);
 
-	if (IS_ERR(clk))
+	if (IS_ERR(hw))
 		panic("%s: can't register REFCLK. Check DT!", np->name);
 
 	for (n = 0; n < ARRAY_SIZE(asm9260_mux_clks); n++) {
@@ -293,7 +300,7 @@ static void __init asm9260_acc_init(struct device_node *np)
 
 		mc->parent_names[0] = ref_clk;
 		mc->parent_names[1] = pll_clk;
-		clk = clk_register_mux_table(NULL, mc->name, mc->parent_names,
+		hw = clk_hw_register_mux_table(NULL, mc->name, mc->parent_names,
 				mc->num_parents, mc->flags, base + mc->offset,
 				0, mc->mask, 0, mc->table, &asm9260_clk_lock);
 	}
@@ -302,7 +309,7 @@ static void __init asm9260_acc_init(struct device_node *np)
 	for (n = 0; n < ARRAY_SIZE(asm9260_mux_gates); n++) {
 		const struct asm9260_gate_data *gd = &asm9260_mux_gates[n];
 
-		clk = clk_register_gate(NULL, gd->name,
+		hw = clk_hw_register_gate(NULL, gd->name,
 			gd->parent_name, gd->flags | CLK_SET_RATE_PARENT,
 			base + gd->reg, gd->bit_idx, 0, &asm9260_clk_lock);
 	}
@@ -311,7 +318,7 @@ static void __init asm9260_acc_init(struct device_node *np)
 	for (n = 0; n < ARRAY_SIZE(asm9260_div_clks); n++) {
 		const struct asm9260_div_clk *dc = &asm9260_div_clks[n];
 
-		clks[dc->idx] = clk_register_divider(NULL, dc->name,
+		hws[dc->idx] = clk_hw_register_divider(NULL, dc->name,
 				dc->parent_name, CLK_SET_RATE_PARENT,
 				base + dc->reg, 0, 8, CLK_DIVIDER_ONE_BASED,
 				&asm9260_clk_lock);
@@ -321,14 +328,14 @@ static void __init asm9260_acc_init(struct device_node *np)
 	for (n = 0; n < ARRAY_SIZE(asm9260_ahb_gates); n++) {
 		const struct asm9260_gate_data *gd = &asm9260_ahb_gates[n];
 
-		clks[gd->idx] = clk_register_gate(NULL, gd->name,
+		hws[gd->idx] = clk_hw_register_gate(NULL, gd->name,
 				gd->parent_name, gd->flags, base + gd->reg,
 				gd->bit_idx, 0, &asm9260_clk_lock);
 	}
 
 	/* check for errors on leaf clocks */
 	for (n = 0; n < MAX_CLKS; n++) {
-		if (!IS_ERR(clks[n]))
+		if (!IS_ERR(hws[n]))
 			continue;
 
 		pr_err("%s: Unable to register leaf clock %d\n",
@@ -337,9 +344,7 @@ static void __init asm9260_acc_init(struct device_node *np)
 	}
 
 	/* register clk-provider */
-	clk_data.clks = clks;
-	clk_data.clk_num = MAX_CLKS;
-	of_clk_add_provider(np, of_clk_src_onecell_get, &clk_data);
+	of_clk_add_hw_provider(np, of_clk_hw_onecell_get, clk_data);
 	return;
 fail:
 	iounmap(base);
diff --git a/drivers/clk/clk-axi-clkgen.c b/drivers/clk/clk-axi-clkgen.c
index 3294db3b4e4e..5e918e7afaba 100644
--- a/drivers/clk/clk-axi-clkgen.c
+++ b/drivers/clk/clk-axi-clkgen.c
@@ -392,8 +392,8 @@ static int axi_clkgen_probe(struct platform_device *pdev)
 	const char *parent_names[2];
 	const char *clk_name;
 	struct resource *mem;
-	struct clk *clk;
 	unsigned int i;
+	int ret;
 
 	if (!pdev->dev.of_node)
 		return -ENODEV;
@@ -433,12 +433,12 @@ static int axi_clkgen_probe(struct platform_device *pdev)
 	axi_clkgen_mmcm_enable(axi_clkgen, false);
 
 	axi_clkgen->clk_hw.init = &init;
-	clk = devm_clk_register(&pdev->dev, &axi_clkgen->clk_hw);
-	if (IS_ERR(clk))
-		return PTR_ERR(clk);
+	ret = devm_clk_hw_register(&pdev->dev, &axi_clkgen->clk_hw);
+	if (ret)
+		return ret;
 
-	return of_clk_add_provider(pdev->dev.of_node, of_clk_src_simple_get,
-				    clk);
+	return of_clk_add_hw_provider(pdev->dev.of_node, of_clk_hw_simple_get,
+				      &axi_clkgen->clk_hw);
 }
 
 static int axi_clkgen_remove(struct platform_device *pdev)
diff --git a/drivers/clk/clk-axm5516.c b/drivers/clk/clk-axm5516.c
index c7c91a5ecf8b..5d7ae333257e 100644
--- a/drivers/clk/clk-axm5516.c
+++ b/drivers/clk/clk-axm5516.c
@@ -516,6 +516,19 @@ static struct axxia_clk *axmclk_clocks[] = {
 	[AXXIA_CLK_MMC]      = &clk_mmc_mux.aclk,
 };
 
+static struct clk_hw *
+of_clk_axmclk_get(struct of_phandle_args *clkspec, void *unused)
+{
+	unsigned int idx = clkspec->args[0];
+
+	if (idx >= ARRAY_SIZE(axmclk_clocks)) {
+		pr_err("%s: invalid index %u\n", __func__, idx);
+		return ERR_PTR(-EINVAL);
+	}
+
+	return &axmclk_clocks[idx]->hw;
+}
+
 static const struct regmap_config axmclk_regmap_config = {
 	.reg_bits	= 32,
 	.reg_stride	= 4,
@@ -530,21 +543,14 @@ static const struct of_device_id axmclk_match_table[] = {
 };
 MODULE_DEVICE_TABLE(of, axmclk_match_table);
 
-struct axmclk_priv {
-	struct clk_onecell_data onecell;
-	struct clk *clks[];
-};
-
 static int axmclk_probe(struct platform_device *pdev)
 {
 	void __iomem *base;
 	struct resource *res;
 	int i, ret;
 	struct device *dev = &pdev->dev;
-	struct clk *clk;
 	struct regmap *regmap;
 	size_t num_clks;
-	struct axmclk_priv *priv;
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	base = devm_ioremap_resource(dev, res);
@@ -557,29 +563,18 @@ static int axmclk_probe(struct platform_device *pdev)
 
 	num_clks = ARRAY_SIZE(axmclk_clocks);
 	pr_info("axmclk: supporting %zu clocks\n", num_clks);
-	priv = devm_kzalloc(dev, sizeof(*priv) + sizeof(*priv->clks) * num_clks,
-			    GFP_KERNEL);
-	if (!priv)
-		return -ENOMEM;
-
-	priv->onecell.clks = priv->clks;
-	priv->onecell.clk_num = num_clks;
 
 	/* Update each entry with the allocated regmap and register the clock
 	 * with the common clock framework
 	 */
 	for (i = 0; i < num_clks; i++) {
 		axmclk_clocks[i]->regmap = regmap;
-		clk = devm_clk_register(dev, &axmclk_clocks[i]->hw);
-		if (IS_ERR(clk))
-			return PTR_ERR(clk);
-		priv->clks[i] = clk;
+		ret = devm_clk_hw_register(dev, &axmclk_clocks[i]->hw);
+		if (ret)
+			return ret;
 	}
 
-	ret = of_clk_add_provider(dev->of_node,
-				  of_clk_src_onecell_get, &priv->onecell);
-
-	return ret;
+	return of_clk_add_hw_provider(dev->of_node, of_clk_axmclk_get, NULL);
 }
 
 static int axmclk_remove(struct platform_device *pdev)
diff --git a/drivers/clk/clk-cdce706.c b/drivers/clk/clk-cdce706.c
index 01877f64eff6..f21d9092564f 100644
--- a/drivers/clk/clk-cdce706.c
+++ b/drivers/clk/clk-cdce706.c
@@ -71,7 +71,6 @@ struct cdce706_hw_data {
 	struct cdce706_dev_data *dev_data;
 	unsigned idx;
 	unsigned parent;
-	struct clk *clk;
 	struct clk_hw hw;
 	unsigned div;
 	unsigned mul;
@@ -81,8 +80,6 @@ struct cdce706_hw_data {
 struct cdce706_dev_data {
 	struct i2c_client *client;
 	struct regmap *regmap;
-	struct clk_onecell_data onecell;
-	struct clk *clks[6];
 	struct clk *clkin_clk[2];
 	const char *clkin_name[2];
 	struct cdce706_hw_data clkin[1];
@@ -455,18 +452,19 @@ static int cdce706_register_hw(struct cdce706_dev_data *cdce,
 			       struct clk_init_data *init)
 {
 	unsigned i;
+	int ret;
 
 	for (i = 0; i < num_hw; ++i, ++hw) {
 		init->name = clk_names[i];
 		hw->dev_data = cdce;
 		hw->idx = i;
 		hw->hw.init = init;
-		hw->clk = devm_clk_register(&cdce->client->dev,
+		ret = devm_clk_hw_register(&cdce->client->dev,
 					    &hw->hw);
-		if (IS_ERR(hw->clk)) {
+		if (ret) {
 			dev_err(&cdce->client->dev, "Failed to register %s\n",
 				clk_names[i]);
-			return PTR_ERR(hw->clk);
+			return ret;
 		}
 	}
 	return 0;
@@ -613,13 +611,23 @@ static int cdce706_register_clkouts(struct cdce706_dev_data *cdce)
 			cdce->clkout[i].parent);
 	}
 
-	ret = cdce706_register_hw(cdce, cdce->clkout,
-				  ARRAY_SIZE(cdce->clkout),
-				  cdce706_clkout_name, &init);
-	for (i = 0; i < ARRAY_SIZE(cdce->clkout); ++i)
-		cdce->clks[i] = cdce->clkout[i].clk;
+	return cdce706_register_hw(cdce, cdce->clkout,
+				   ARRAY_SIZE(cdce->clkout),
+				   cdce706_clkout_name, &init);
+}
 
-	return ret;
+static struct clk_hw *
+of_clk_cdce_get(struct of_phandle_args *clkspec, void *data)
+{
+	struct cdce706_dev_data *cdce = data;
+	unsigned int idx = clkspec->args[0];
+
+	if (idx >= ARRAY_SIZE(cdce->clkout)) {
+		pr_err("%s: invalid index %u\n", __func__, idx);
+		return ERR_PTR(-EINVAL);
+	}
+
+	return &cdce->clkout[idx].hw;
 }
 
 static int cdce706_probe(struct i2c_client *client,
@@ -657,12 +665,8 @@ static int cdce706_probe(struct i2c_client *client,
 	ret = cdce706_register_clkouts(cdce);
 	if (ret < 0)
 		return ret;
-	cdce->onecell.clks = cdce->clks;
-	cdce->onecell.clk_num = ARRAY_SIZE(cdce->clks);
-	ret = of_clk_add_provider(client->dev.of_node, of_clk_src_onecell_get,
-				  &cdce->onecell);
-
-	return ret;
+	return of_clk_add_hw_provider(client->dev.of_node, of_clk_cdce_get,
+				      cdce);
 }
 
 static int cdce706_remove(struct i2c_client *client)
diff --git a/drivers/clk/clk-cdce925.c b/drivers/clk/clk-cdce925.c
index 089bf88ffa8d..b8459c14a1b7 100644
--- a/drivers/clk/clk-cdce925.c
+++ b/drivers/clk/clk-cdce925.c
@@ -62,8 +62,6 @@ struct clk_cdce925_chip {
 	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;
 };
 
 /* ** ** ** ** ** ** ** ** ** ** ** ** ** ** ** ** ** ** ** ** ** ** */
@@ -557,6 +555,20 @@ static int cdce925_regmap_i2c_read(void *context,
 		return -EIO;
 }
 
+static struct clk_hw *
+of_clk_cdce925_get(struct of_phandle_args *clkspec, void *_data)
+{
+	struct clk_cdce925_chip *data = _data;
+	unsigned int idx = clkspec->args[0];
+
+	if (idx >= ARRAY_SIZE(data->clk)) {
+		pr_err("%s: invalid index %u\n", __func__, idx);
+		return ERR_PTR(-EINVAL);
+	}
+
+	return &data->clk[idx].hw;
+}
+
 /* 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 = {
@@ -572,7 +584,6 @@ static int cdce925_probe(struct i2c_client *client,
 	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;
@@ -622,10 +633,9 @@ static int cdce925_probe(struct i2c_client *client,
 		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)) {
+		err = devm_clk_hw_register(&client->dev, &data->pll[i].hw);
+		if (err) {
 			dev_err(&client->dev, "Failed register PLL %d\n", i);
-			err = PTR_ERR(clk);
 			goto error;
 		}
 		sprintf(child_name, "PLL%d", i+1);
@@ -634,7 +644,7 @@ static int cdce925_probe(struct i2c_client *client,
 			continue;
 		if (!of_property_read_u32(np_output,
 			"clock-frequency", &value)) {
-			err = clk_set_rate(clk, value);
+			err = clk_set_rate(data->pll[i].hw.clk, value);
 			if (err)
 				dev_err(&client->dev,
 					"unable to set PLL frequency %ud\n",
@@ -663,14 +673,12 @@ static int cdce925_probe(struct i2c_client *client,
 	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);
+	err = devm_clk_hw_register(&client->dev, &data->clk[0].hw);
 	kfree(init.name); /* clock framework made a copy of the name */
-	if (IS_ERR(clk)) {
+	if (err) {
 		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;
@@ -695,21 +703,17 @@ static int cdce925_probe(struct i2c_client *client,
 			init.parent_names = &pll_clk_name[1];
 			break;
 		}
-		clk = devm_clk_register(&client->dev, &data->clk[i].hw);
+		err = devm_clk_hw_register(&client->dev, &data->clk[i].hw);
 		kfree(init.name); /* clock framework made a copy of the name */
-		if (IS_ERR(clk)) {
+		if (err) {
 			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);
+	err = of_clk_add_hw_provider(client->dev.of_node, of_clk_cdce925_get,
+				  data);
 	if (err)
 		dev_err(&client->dev, "unable to add OF clock provider\n");
 
diff --git a/drivers/clk/clk-clps711x.c b/drivers/clk/clk-clps711x.c
index adaf109f2fe2..9193f64561f6 100644
--- a/drivers/clk/clk-clps711x.c
+++ b/drivers/clk/clk-clps711x.c
@@ -40,9 +40,8 @@ static const struct clk_div_table timer_div_table[] = {
 };
 
 struct clps711x_clk {
-	struct clk_onecell_data	clk_data;
-	spinlock_t		lock;
-	struct clk		*clks[CLPS711X_CLK_MAX];
+	spinlock_t			lock;
+	struct clk_hw_onecell_data	clk_data;
 };
 
 static struct clps711x_clk * __init _clps711x_clk_init(void __iomem *base,
@@ -55,7 +54,9 @@ static struct clps711x_clk * __init _clps711x_clk_init(void __iomem *base,
 	if (!base)
 		return ERR_PTR(-ENOMEM);
 
-	clps711x_clk = kzalloc(sizeof(*clps711x_clk), GFP_KERNEL);
+	clps711x_clk = kzalloc(sizeof(*clps711x_clk) +
+			sizeof(*clps711x_clk->clk_data.hws) * CLPS711X_CLK_MAX,
+			GFP_KERNEL);
 	if (!clps711x_clk)
 		return ERR_PTR(-ENOMEM);
 
@@ -106,40 +107,40 @@ static struct clps711x_clk * __init _clps711x_clk_init(void __iomem *base,
 	tmp |= SYSCON1_TC2M | SYSCON1_TC2S;
 	writel(tmp, base + CLPS711X_SYSCON1);
 
-	clps711x_clk->clks[CLPS711X_CLK_DUMMY] =
-		clk_register_fixed_rate(NULL, "dummy", NULL, 0, 0);
-	clps711x_clk->clks[CLPS711X_CLK_CPU] =
-		clk_register_fixed_rate(NULL, "cpu", NULL, 0, f_cpu);
-	clps711x_clk->clks[CLPS711X_CLK_BUS] =
-		clk_register_fixed_rate(NULL, "bus", NULL, 0, f_bus);
-	clps711x_clk->clks[CLPS711X_CLK_PLL] =
-		clk_register_fixed_rate(NULL, "pll", NULL, 0, f_pll);
-	clps711x_clk->clks[CLPS711X_CLK_TIMERREF] =
-		clk_register_fixed_rate(NULL, "timer_ref", NULL, 0, f_tim);
-	clps711x_clk->clks[CLPS711X_CLK_TIMER1] =
-		clk_register_divider_table(NULL, "timer1", "timer_ref", 0,
+	clps711x_clk->clk_data.hws[CLPS711X_CLK_DUMMY] =
+		clk_hw_register_fixed_rate(NULL, "dummy", NULL, 0, 0);
+	clps711x_clk->clk_data.hws[CLPS711X_CLK_CPU] =
+		clk_hw_register_fixed_rate(NULL, "cpu", NULL, 0, f_cpu);
+	clps711x_clk->clk_data.hws[CLPS711X_CLK_BUS] =
+		clk_hw_register_fixed_rate(NULL, "bus", NULL, 0, f_bus);
+	clps711x_clk->clk_data.hws[CLPS711X_CLK_PLL] =
+		clk_hw_register_fixed_rate(NULL, "pll", NULL, 0, f_pll);
+	clps711x_clk->clk_data.hws[CLPS711X_CLK_TIMERREF] =
+		clk_hw_register_fixed_rate(NULL, "timer_ref", NULL, 0, f_tim);
+	clps711x_clk->clk_data.hws[CLPS711X_CLK_TIMER1] =
+		clk_hw_register_divider_table(NULL, "timer1", "timer_ref", 0,
 					   base + CLPS711X_SYSCON1, 5, 1, 0,
 					   timer_div_table, &clps711x_clk->lock);
-	clps711x_clk->clks[CLPS711X_CLK_TIMER2] =
-		clk_register_divider_table(NULL, "timer2", "timer_ref", 0,
+	clps711x_clk->clk_data.hws[CLPS711X_CLK_TIMER2] =
+		clk_hw_register_divider_table(NULL, "timer2", "timer_ref", 0,
 					   base + CLPS711X_SYSCON1, 7, 1, 0,
 					   timer_div_table, &clps711x_clk->lock);
-	clps711x_clk->clks[CLPS711X_CLK_PWM] =
-		clk_register_fixed_rate(NULL, "pwm", NULL, 0, f_pwm);
-	clps711x_clk->clks[CLPS711X_CLK_SPIREF] =
-		clk_register_fixed_rate(NULL, "spi_ref", NULL, 0, f_spi);
-	clps711x_clk->clks[CLPS711X_CLK_SPI] =
-		clk_register_divider_table(NULL, "spi", "spi_ref", 0,
+	clps711x_clk->clk_data.hws[CLPS711X_CLK_PWM] =
+		clk_hw_register_fixed_rate(NULL, "pwm", NULL, 0, f_pwm);
+	clps711x_clk->clk_data.hws[CLPS711X_CLK_SPIREF] =
+		clk_hw_register_fixed_rate(NULL, "spi_ref", NULL, 0, f_spi);
+	clps711x_clk->clk_data.hws[CLPS711X_CLK_SPI] =
+		clk_hw_register_divider_table(NULL, "spi", "spi_ref", 0,
 					   base + CLPS711X_SYSCON1, 16, 2, 0,
 					   spi_div_table, &clps711x_clk->lock);
-	clps711x_clk->clks[CLPS711X_CLK_UART] =
-		clk_register_fixed_factor(NULL, "uart", "bus", 0, 1, 10);
-	clps711x_clk->clks[CLPS711X_CLK_TICK] =
-		clk_register_fixed_rate(NULL, "tick", NULL, 0, 64);
+	clps711x_clk->clk_data.hws[CLPS711X_CLK_UART] =
+		clk_hw_register_fixed_factor(NULL, "uart", "bus", 0, 1, 10);
+	clps711x_clk->clk_data.hws[CLPS711X_CLK_TICK] =
+		clk_hw_register_fixed_rate(NULL, "tick", NULL, 0, 64);
 	for (i = 0; i < CLPS711X_CLK_MAX; i++)
-		if (IS_ERR(clps711x_clk->clks[i]))
+		if (IS_ERR(clps711x_clk->clk_data.hws[i]))
 			pr_err("clk %i: register failed with %ld\n",
-			       i, PTR_ERR(clps711x_clk->clks[i]));
+			       i, PTR_ERR(clps711x_clk->clk_data.hws[i]));
 
 	return clps711x_clk;
 }
@@ -153,17 +154,17 @@ void __init clps711x_clk_init(void __iomem *base)
 	BUG_ON(IS_ERR(clps711x_clk));
 
 	/* Clocksource */
-	clk_register_clkdev(clps711x_clk->clks[CLPS711X_CLK_TIMER1],
+	clk_hw_register_clkdev(clps711x_clk->clk_data.hws[CLPS711X_CLK_TIMER1],
 			    NULL, "clps711x-timer.0");
-	clk_register_clkdev(clps711x_clk->clks[CLPS711X_CLK_TIMER2],
+	clk_hw_register_clkdev(clps711x_clk->clk_data.hws[CLPS711X_CLK_TIMER2],
 			    NULL, "clps711x-timer.1");
 
 	/* Drivers */
-	clk_register_clkdev(clps711x_clk->clks[CLPS711X_CLK_PWM],
+	clk_hw_register_clkdev(clps711x_clk->clk_data.hws[CLPS711X_CLK_PWM],
 			    NULL, "clps711x-pwm");
-	clk_register_clkdev(clps711x_clk->clks[CLPS711X_CLK_UART],
+	clk_hw_register_clkdev(clps711x_clk->clk_data.hws[CLPS711X_CLK_UART],
 			    NULL, "clps711x-uart.0");
-	clk_register_clkdev(clps711x_clk->clks[CLPS711X_CLK_UART],
+	clk_hw_register_clkdev(clps711x_clk->clk_data.hws[CLPS711X_CLK_UART],
 			    NULL, "clps711x-uart.1");
 }
 
@@ -179,10 +180,9 @@ static void __init clps711x_clk_init_dt(struct device_node *np)
 	clps711x_clk = _clps711x_clk_init(base, fref);
 	BUG_ON(IS_ERR(clps711x_clk));
 
-	clps711x_clk->clk_data.clks = clps711x_clk->clks;
-	clps711x_clk->clk_data.clk_num = CLPS711X_CLK_MAX;
-	of_clk_add_provider(np, of_clk_src_onecell_get,
-			    &clps711x_clk->clk_data);
+	clps711x_clk->clk_data.num = CLPS711X_CLK_MAX;
+	of_clk_add_hw_provider(np, of_clk_hw_onecell_get,
+			       &clps711x_clk->clk_data);
 }
 CLK_OF_DECLARE(clps711x, "cirrus,ep7209-clk", clps711x_clk_init_dt);
 #endif
diff --git a/drivers/clk/clk-cs2000-cp.c b/drivers/clk/clk-cs2000-cp.c
index 7379de8dc894..021f3daf34e1 100644
--- a/drivers/clk/clk-cs2000-cp.c
+++ b/drivers/clk/clk-cs2000-cp.c
@@ -59,7 +59,6 @@ struct cs2000_priv {
 	struct i2c_client *client;
 	struct clk *clk_in;
 	struct clk *ref_clk;
-	struct clk *clk_out;
 };
 
 static const struct of_device_id cs2000_of_match[] = {
@@ -371,7 +370,6 @@ static int cs2000_clk_register(struct cs2000_priv *priv)
 	struct device_node *np = dev->of_node;
 	struct clk_init_data init;
 	const char *name = np->name;
-	struct clk *clk;
 	static const char *parent_names[CLK_MAX];
 	int ch = 0; /* it uses ch0 only at this point */
 	int rate;
@@ -400,18 +398,16 @@ static int cs2000_clk_register(struct cs2000_priv *priv)
 
 	priv->hw.init = &init;
 
-	clk = clk_register(dev, &priv->hw);
-	if (IS_ERR(clk))
-		return PTR_ERR(clk);
+	ret = clk_hw_register(dev, &priv->hw);
+	if (ret)
+		return ret;
 
-	ret = of_clk_add_provider(np, of_clk_src_simple_get, clk);
+	ret = of_clk_add_hw_provider(np, of_clk_hw_simple_get, &priv->hw);
 	if (ret < 0) {
-		clk_unregister(clk);
+		clk_hw_unregister(&priv->hw);
 		return ret;
 	}
 
-	priv->clk_out = clk;
-
 	return 0;
 }
 
@@ -454,7 +450,7 @@ static int cs2000_remove(struct i2c_client *client)
 
 	of_clk_del_provider(np);
 
-	clk_unregister(priv->clk_out);
+	clk_hw_unregister(&priv->hw);
 
 	return 0;
 }
diff --git a/drivers/clk/clk-divider.c b/drivers/clk/clk-divider.c
index a0f55bc1ad3d..96386ffc8483 100644
--- a/drivers/clk/clk-divider.c
+++ b/drivers/clk/clk-divider.c
@@ -352,7 +352,7 @@ static long clk_divider_round_rate(struct clk_hw *hw, unsigned long rate,
 
 	/* if read only, just return current value */
 	if (divider->flags & CLK_DIVIDER_READ_ONLY) {
-		bestdiv = readl(divider->reg) >> divider->shift;
+		bestdiv = clk_readl(divider->reg) >> divider->shift;
 		bestdiv &= div_mask(divider->width);
 		bestdiv = _get_div(divider->table, bestdiv, divider->flags,
 			divider->width);
diff --git a/drivers/clk/clk-efm32gg.c b/drivers/clk/clk-efm32gg.c
index 22e4c659704e..8802a2dd56ac 100644
--- a/drivers/clk/clk-efm32gg.c
+++ b/drivers/clk/clk-efm32gg.c
@@ -10,24 +10,31 @@
 #include <linux/clk-provider.h>
 #include <linux/of.h>
 #include <linux/of_address.h>
+#include <linux/slab.h>
 
 #include <dt-bindings/clock/efm32-cmu.h>
 
 #define CMU_HFPERCLKEN0		0x44
+#define CMU_MAX_CLKS		37
 
-static struct clk *clk[37];
-static struct clk_onecell_data clk_data = {
-	.clks = clk,
-	.clk_num = ARRAY_SIZE(clk),
-};
+static struct clk_hw_onecell_data *clk_data;
 
 static void __init efm32gg_cmu_init(struct device_node *np)
 {
 	int i;
 	void __iomem *base;
+	struct clk_hw **hws;
 
-	for (i = 0; i < ARRAY_SIZE(clk); ++i)
-		clk[i] = ERR_PTR(-ENOENT);
+	clk_data = kzalloc(sizeof(*clk_data) +
+			   sizeof(*clk_data->hws) * CMU_MAX_CLKS, GFP_KERNEL);
+
+	if (!clk_data)
+		return;
+
+	hws = clk_data->hws;
+
+	for (i = 0; i < CMU_MAX_CLKS; ++i)
+		hws[i] = ERR_PTR(-ENOENT);
 
 	base = of_iomap(np, 0);
 	if (!base) {
@@ -35,46 +42,46 @@ static void __init efm32gg_cmu_init(struct device_node *np)
 		return;
 	}
 
-	clk[clk_HFXO] = clk_register_fixed_rate(NULL, "HFXO", NULL,
-			0, 48000000);
+	hws[clk_HFXO] = clk_hw_register_fixed_rate(NULL, "HFXO", NULL, 0,
+						   48000000);
 
-	clk[clk_HFPERCLKUSART0] = clk_register_gate(NULL, "HFPERCLK.USART0",
+	hws[clk_HFPERCLKUSART0] = clk_hw_register_gate(NULL, "HFPERCLK.USART0",
 			"HFXO", 0, base + CMU_HFPERCLKEN0, 0, 0, NULL);
-	clk[clk_HFPERCLKUSART1] = clk_register_gate(NULL, "HFPERCLK.USART1",
+	hws[clk_HFPERCLKUSART1] = clk_hw_register_gate(NULL, "HFPERCLK.USART1",
 			"HFXO", 0, base + CMU_HFPERCLKEN0, 1, 0, NULL);
-	clk[clk_HFPERCLKUSART2] = clk_register_gate(NULL, "HFPERCLK.USART2",
+	hws[clk_HFPERCLKUSART2] = clk_hw_register_gate(NULL, "HFPERCLK.USART2",
 			"HFXO", 0, base + CMU_HFPERCLKEN0, 2, 0, NULL);
-	clk[clk_HFPERCLKUART0] = clk_register_gate(NULL, "HFPERCLK.UART0",
+	hws[clk_HFPERCLKUART0] = clk_hw_register_gate(NULL, "HFPERCLK.UART0",
 			"HFXO", 0, base + CMU_HFPERCLKEN0, 3, 0, NULL);
-	clk[clk_HFPERCLKUART1] = clk_register_gate(NULL, "HFPERCLK.UART1",
+	hws[clk_HFPERCLKUART1] = clk_hw_register_gate(NULL, "HFPERCLK.UART1",
 			"HFXO", 0, base + CMU_HFPERCLKEN0, 4, 0, NULL);
-	clk[clk_HFPERCLKTIMER0] = clk_register_gate(NULL, "HFPERCLK.TIMER0",
+	hws[clk_HFPERCLKTIMER0] = clk_hw_register_gate(NULL, "HFPERCLK.TIMER0",
 			"HFXO", 0, base + CMU_HFPERCLKEN0, 5, 0, NULL);
-	clk[clk_HFPERCLKTIMER1] = clk_register_gate(NULL, "HFPERCLK.TIMER1",
+	hws[clk_HFPERCLKTIMER1] = clk_hw_register_gate(NULL, "HFPERCLK.TIMER1",
 			"HFXO", 0, base + CMU_HFPERCLKEN0, 6, 0, NULL);
-	clk[clk_HFPERCLKTIMER2] = clk_register_gate(NULL, "HFPERCLK.TIMER2",
+	hws[clk_HFPERCLKTIMER2] = clk_hw_register_gate(NULL, "HFPERCLK.TIMER2",
 			"HFXO", 0, base + CMU_HFPERCLKEN0, 7, 0, NULL);
-	clk[clk_HFPERCLKTIMER3] = clk_register_gate(NULL, "HFPERCLK.TIMER3",
+	hws[clk_HFPERCLKTIMER3] = clk_hw_register_gate(NULL, "HFPERCLK.TIMER3",
 			"HFXO", 0, base + CMU_HFPERCLKEN0, 8, 0, NULL);
-	clk[clk_HFPERCLKACMP0] = clk_register_gate(NULL, "HFPERCLK.ACMP0",
+	hws[clk_HFPERCLKACMP0] = clk_hw_register_gate(NULL, "HFPERCLK.ACMP0",
 			"HFXO", 0, base + CMU_HFPERCLKEN0, 9, 0, NULL);
-	clk[clk_HFPERCLKACMP1] = clk_register_gate(NULL, "HFPERCLK.ACMP1",
+	hws[clk_HFPERCLKACMP1] = clk_hw_register_gate(NULL, "HFPERCLK.ACMP1",
 			"HFXO", 0, base + CMU_HFPERCLKEN0, 10, 0, NULL);
-	clk[clk_HFPERCLKI2C0] = clk_register_gate(NULL, "HFPERCLK.I2C0",
+	hws[clk_HFPERCLKI2C0] = clk_hw_register_gate(NULL, "HFPERCLK.I2C0",
 			"HFXO", 0, base + CMU_HFPERCLKEN0, 11, 0, NULL);
-	clk[clk_HFPERCLKI2C1] = clk_register_gate(NULL, "HFPERCLK.I2C1",
+	hws[clk_HFPERCLKI2C1] = clk_hw_register_gate(NULL, "HFPERCLK.I2C1",
 			"HFXO", 0, base + CMU_HFPERCLKEN0, 12, 0, NULL);
-	clk[clk_HFPERCLKGPIO] = clk_register_gate(NULL, "HFPERCLK.GPIO",
+	hws[clk_HFPERCLKGPIO] = clk_hw_register_gate(NULL, "HFPERCLK.GPIO",
 			"HFXO", 0, base + CMU_HFPERCLKEN0, 13, 0, NULL);
-	clk[clk_HFPERCLKVCMP] = clk_register_gate(NULL, "HFPERCLK.VCMP",
+	hws[clk_HFPERCLKVCMP] = clk_hw_register_gate(NULL, "HFPERCLK.VCMP",
 			"HFXO", 0, base + CMU_HFPERCLKEN0, 14, 0, NULL);
-	clk[clk_HFPERCLKPRS] = clk_register_gate(NULL, "HFPERCLK.PRS",
+	hws[clk_HFPERCLKPRS] = clk_hw_register_gate(NULL, "HFPERCLK.PRS",
 			"HFXO", 0, base + CMU_HFPERCLKEN0, 15, 0, NULL);
-	clk[clk_HFPERCLKADC0] = clk_register_gate(NULL, "HFPERCLK.ADC0",
+	hws[clk_HFPERCLKADC0] = clk_hw_register_gate(NULL, "HFPERCLK.ADC0",
 			"HFXO", 0, base + CMU_HFPERCLKEN0, 16, 0, NULL);
-	clk[clk_HFPERCLKDAC0] = clk_register_gate(NULL, "HFPERCLK.DAC0",
+	hws[clk_HFPERCLKDAC0] = clk_hw_register_gate(NULL, "HFPERCLK.DAC0",
 			"HFXO", 0, base + CMU_HFPERCLKEN0, 17, 0, NULL);
 
-	of_clk_add_provider(np, of_clk_src_onecell_get, &clk_data);
+	of_clk_add_hw_provider(np, of_clk_hw_onecell_get, &clk_data);
 }
 CLK_OF_DECLARE(efm32ggcmu, "efm32gg,cmu", efm32gg_cmu_init);
diff --git a/drivers/clk/clk-fixed-factor.c b/drivers/clk/clk-fixed-factor.c
index 4db3be214077..a5d402de5584 100644
--- a/drivers/clk/clk-fixed-factor.c
+++ b/drivers/clk/clk-fixed-factor.c
@@ -12,6 +12,7 @@
 #include <linux/slab.h>
 #include <linux/err.h>
 #include <linux/of.h>
+#include <linux/platform_device.h>
 
 /*
  * DOC: basic fixed multiplier and divider clock that cannot gate
@@ -147,27 +148,25 @@ static const struct of_device_id set_rate_parent_matches[] = {
 	{ /* Sentinel */ },
 };
 
-/**
- * of_fixed_factor_clk_setup() - Setup function for simple fixed factor clock
- */
-void __init of_fixed_factor_clk_setup(struct device_node *node)
+static struct clk *_of_fixed_factor_clk_setup(struct device_node *node)
 {
 	struct clk *clk;
 	const char *clk_name = node->name;
 	const char *parent_name;
 	unsigned long flags = 0;
 	u32 div, mult;
+	int ret;
 
 	if (of_property_read_u32(node, "clock-div", &div)) {
 		pr_err("%s Fixed factor clock <%s> must have a clock-div property\n",
 			__func__, node->name);
-		return;
+		return ERR_PTR(-EIO);
 	}
 
 	if (of_property_read_u32(node, "clock-mult", &mult)) {
 		pr_err("%s Fixed factor clock <%s> must have a clock-mult property\n",
 			__func__, node->name);
-		return;
+		return ERR_PTR(-EIO);
 	}
 
 	of_property_read_string(node, "clock-output-names", &clk_name);
@@ -178,10 +177,67 @@ void __init of_fixed_factor_clk_setup(struct device_node *node)
 
 	clk = clk_register_fixed_factor(NULL, clk_name, parent_name, flags,
 					mult, div);
-	if (!IS_ERR(clk))
-		of_clk_add_provider(node, of_clk_src_simple_get, clk);
+	if (IS_ERR(clk))
+		return clk;
+
+	ret = of_clk_add_provider(node, of_clk_src_simple_get, clk);
+	if (ret) {
+		clk_unregister(clk);
+		return ERR_PTR(ret);
+	}
+
+	return clk;
+}
+
+/**
+ * of_fixed_factor_clk_setup() - Setup function for simple fixed factor clock
+ */
+void __init of_fixed_factor_clk_setup(struct device_node *node)
+{
+	_of_fixed_factor_clk_setup(node);
 }
-EXPORT_SYMBOL_GPL(of_fixed_factor_clk_setup);
 CLK_OF_DECLARE(fixed_factor_clk, "fixed-factor-clock",
 		of_fixed_factor_clk_setup);
+
+static int of_fixed_factor_clk_remove(struct platform_device *pdev)
+{
+	struct clk *clk = platform_get_drvdata(pdev);
+
+	clk_unregister_fixed_factor(clk);
+
+	return 0;
+}
+
+static int of_fixed_factor_clk_probe(struct platform_device *pdev)
+{
+	struct clk *clk;
+
+	/*
+	 * This function is not executed when of_fixed_factor_clk_setup
+	 * succeeded.
+	 */
+	clk = _of_fixed_factor_clk_setup(pdev->dev.of_node);
+	if (IS_ERR(clk))
+		return PTR_ERR(clk);
+
+	platform_set_drvdata(pdev, clk);
+
+	return 0;
+}
+
+static const struct of_device_id of_fixed_factor_clk_ids[] = {
+	{ .compatible = "fixed-factor-clock" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, of_fixed_factor_clk_ids);
+
+static struct platform_driver of_fixed_factor_clk_driver = {
+	.driver = {
+		.name = "of_fixed_factor_clk",
+		.of_match_table = of_fixed_factor_clk_ids,
+	},
+	.probe = of_fixed_factor_clk_probe,
+	.remove = of_fixed_factor_clk_remove,
+};
+builtin_platform_driver(of_fixed_factor_clk_driver);
 #endif
diff --git a/drivers/clk/clk-fixed-rate.c b/drivers/clk/clk-fixed-rate.c
index 2edb39342a02..b5c46b3f8764 100644
--- a/drivers/clk/clk-fixed-rate.c
+++ b/drivers/clk/clk-fixed-rate.c
@@ -15,6 +15,7 @@
 #include <linux/io.h>
 #include <linux/err.h>
 #include <linux/of.h>
+#include <linux/platform_device.h>
 
 /*
  * DOC: basic fixed-rate clock that cannot gate
@@ -157,18 +158,16 @@ void clk_hw_unregister_fixed_rate(struct clk_hw *hw)
 EXPORT_SYMBOL_GPL(clk_hw_unregister_fixed_rate);
 
 #ifdef CONFIG_OF
-/**
- * of_fixed_clk_setup() - Setup function for simple fixed rate clock
- */
-void of_fixed_clk_setup(struct device_node *node)
+static struct clk *_of_fixed_clk_setup(struct device_node *node)
 {
 	struct clk *clk;
 	const char *clk_name = node->name;
 	u32 rate;
 	u32 accuracy = 0;
+	int ret;
 
 	if (of_property_read_u32(node, "clock-frequency", &rate))
-		return;
+		return ERR_PTR(-EIO);
 
 	of_property_read_u32(node, "clock-accuracy", &accuracy);
 
@@ -176,9 +175,66 @@ void of_fixed_clk_setup(struct device_node *node)
 
 	clk = clk_register_fixed_rate_with_accuracy(NULL, clk_name, NULL,
 						    0, rate, accuracy);
-	if (!IS_ERR(clk))
-		of_clk_add_provider(node, of_clk_src_simple_get, clk);
+	if (IS_ERR(clk))
+		return clk;
+
+	ret = of_clk_add_provider(node, of_clk_src_simple_get, clk);
+	if (ret) {
+		clk_unregister(clk);
+		return ERR_PTR(ret);
+	}
+
+	return clk;
+}
+
+/**
+ * of_fixed_clk_setup() - Setup function for simple fixed rate clock
+ */
+void __init of_fixed_clk_setup(struct device_node *node)
+{
+	_of_fixed_clk_setup(node);
 }
-EXPORT_SYMBOL_GPL(of_fixed_clk_setup);
 CLK_OF_DECLARE(fixed_clk, "fixed-clock", of_fixed_clk_setup);
+
+static int of_fixed_clk_remove(struct platform_device *pdev)
+{
+	struct clk *clk = platform_get_drvdata(pdev);
+
+	clk_unregister_fixed_rate(clk);
+
+	return 0;
+}
+
+static int of_fixed_clk_probe(struct platform_device *pdev)
+{
+	struct clk *clk;
+
+	/*
+	 * This function is not executed when of_fixed_clk_setup
+	 * succeeded.
+	 */
+	clk = _of_fixed_clk_setup(pdev->dev.of_node);
+	if (IS_ERR(clk))
+		return PTR_ERR(clk);
+
+	platform_set_drvdata(pdev, clk);
+
+	return 0;
+}
+
+static const struct of_device_id of_fixed_clk_ids[] = {
+	{ .compatible = "fixed-clock" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, of_fixed_clk_ids);
+
+static struct platform_driver of_fixed_clk_driver = {
+	.driver = {
+		.name = "of_fixed_clk",
+		.of_match_table = of_fixed_clk_ids,
+	},
+	.probe = of_fixed_clk_probe,
+	.remove = of_fixed_clk_remove,
+};
+builtin_platform_driver(of_fixed_clk_driver);
 #endif
diff --git a/drivers/clk/clk-ls1x.c b/drivers/clk/clk-ls1x.c
deleted file mode 100644
index 5097831387ff..000000000000
--- a/drivers/clk/clk-ls1x.c
+++ /dev/null
@@ -1,161 +0,0 @@
-/*
- * Copyright (c) 2012 Zhang, Keguang <keguang.zhang@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/clkdev.h>
-#include <linux/clk-provider.h>
-#include <linux/io.h>
-#include <linux/slab.h>
-#include <linux/err.h>
-
-#include <loongson1.h>
-
-#define OSC		(33 * 1000000)
-#define DIV_APB		2
-
-static DEFINE_SPINLOCK(_lock);
-
-static int ls1x_pll_clk_enable(struct clk_hw *hw)
-{
-	return 0;
-}
-
-static void ls1x_pll_clk_disable(struct clk_hw *hw)
-{
-}
-
-static unsigned long ls1x_pll_recalc_rate(struct clk_hw *hw,
-					  unsigned long parent_rate)
-{
-	u32 pll, rate;
-
-	pll = __raw_readl(LS1X_CLK_PLL_FREQ);
-	rate = 12 + (pll & 0x3f) + (((pll >> 8) & 0x3ff) >> 10);
-	rate *= OSC;
-	rate >>= 1;
-
-	return rate;
-}
-
-static const struct clk_ops ls1x_pll_clk_ops = {
-	.enable = ls1x_pll_clk_enable,
-	.disable = ls1x_pll_clk_disable,
-	.recalc_rate = ls1x_pll_recalc_rate,
-};
-
-static struct clk *__init clk_register_pll(struct device *dev,
-					   const char *name,
-					   const char *parent_name,
-					   unsigned long flags)
-{
-	struct clk_hw *hw;
-	struct clk *clk;
-	struct clk_init_data init;
-
-	/* allocate the divider */
-	hw = kzalloc(sizeof(struct clk_hw), GFP_KERNEL);
-	if (!hw) {
-		pr_err("%s: could not allocate clk_hw\n", __func__);
-		return ERR_PTR(-ENOMEM);
-	}
-
-	init.name = name;
-	init.ops = &ls1x_pll_clk_ops;
-	init.flags = flags | CLK_IS_BASIC;
-	init.parent_names = (parent_name ? &parent_name : NULL);
-	init.num_parents = (parent_name ? 1 : 0);
-	hw->init = &init;
-
-	/* register the clock */
-	clk = clk_register(dev, hw);
-
-	if (IS_ERR(clk))
-		kfree(hw);
-
-	return clk;
-}
-
-static const char * const cpu_parents[] = { "cpu_clk_div", "osc_33m_clk", };
-static const char * const ahb_parents[] = { "ahb_clk_div", "osc_33m_clk", };
-static const char * const dc_parents[] = { "dc_clk_div", "osc_33m_clk", };
-
-void __init ls1x_clk_init(void)
-{
-	struct clk *clk;
-
-	clk = clk_register_fixed_rate(NULL, "osc_33m_clk", NULL, 0, OSC);
-	clk_register_clkdev(clk, "osc_33m_clk", NULL);
-
-	/* clock derived from 33 MHz OSC clk */
-	clk = clk_register_pll(NULL, "pll_clk", "osc_33m_clk", 0);
-	clk_register_clkdev(clk, "pll_clk", NULL);
-
-	/* clock derived from PLL clk */
-	/*                                 _____
-	 *         _______________________|     |
-	 * OSC ___/                       | MUX |___ CPU CLK
-	 *        \___ PLL ___ CPU DIV ___|     |
-	 *                                |_____|
-	 */
-	clk = clk_register_divider(NULL, "cpu_clk_div", "pll_clk",
-				   CLK_GET_RATE_NOCACHE, LS1X_CLK_PLL_DIV,
-				   DIV_CPU_SHIFT, DIV_CPU_WIDTH,
-				   CLK_DIVIDER_ONE_BASED |
-				   CLK_DIVIDER_ROUND_CLOSEST, &_lock);
-	clk_register_clkdev(clk, "cpu_clk_div", NULL);
-	clk = clk_register_mux(NULL, "cpu_clk", cpu_parents,
-			       ARRAY_SIZE(cpu_parents),
-			       CLK_SET_RATE_NO_REPARENT, LS1X_CLK_PLL_DIV,
-			       BYPASS_CPU_SHIFT, BYPASS_CPU_WIDTH, 0, &_lock);
-	clk_register_clkdev(clk, "cpu_clk", NULL);
-
-	/*                                 _____
-	 *         _______________________|     |
-	 * OSC ___/                       | MUX |___ DC  CLK
-	 *        \___ PLL ___ DC  DIV ___|     |
-	 *                                |_____|
-	 */
-	clk = clk_register_divider(NULL, "dc_clk_div", "pll_clk",
-				   0, LS1X_CLK_PLL_DIV, DIV_DC_SHIFT,
-				   DIV_DC_WIDTH, CLK_DIVIDER_ONE_BASED, &_lock);
-	clk_register_clkdev(clk, "dc_clk_div", NULL);
-	clk = clk_register_mux(NULL, "dc_clk", dc_parents,
-			       ARRAY_SIZE(dc_parents),
-			       CLK_SET_RATE_NO_REPARENT, LS1X_CLK_PLL_DIV,
-			       BYPASS_DC_SHIFT, BYPASS_DC_WIDTH, 0, &_lock);
-	clk_register_clkdev(clk, "dc_clk", NULL);
-
-	/*                                 _____
-	 *         _______________________|     |
-	 * OSC ___/                       | MUX |___ DDR CLK
-	 *        \___ PLL ___ DDR DIV ___|     |
-	 *                                |_____|
-	 */
-	clk = clk_register_divider(NULL, "ahb_clk_div", "pll_clk",
-				   0, LS1X_CLK_PLL_DIV, DIV_DDR_SHIFT,
-				   DIV_DDR_WIDTH, CLK_DIVIDER_ONE_BASED,
-				   &_lock);
-	clk_register_clkdev(clk, "ahb_clk_div", NULL);
-	clk = clk_register_mux(NULL, "ahb_clk", ahb_parents,
-			       ARRAY_SIZE(ahb_parents),
-			       CLK_SET_RATE_NO_REPARENT, LS1X_CLK_PLL_DIV,
-			       BYPASS_DDR_SHIFT, BYPASS_DDR_WIDTH, 0, &_lock);
-	clk_register_clkdev(clk, "ahb_clk", NULL);
-	clk_register_clkdev(clk, "stmmaceth", NULL);
-
-	/* clock derived from AHB clk */
-	/* APB clk is always half of the AHB clk */
-	clk = clk_register_fixed_factor(NULL, "apb_clk", "ahb_clk", 0, 1,
-					DIV_APB);
-	clk_register_clkdev(clk, "apb_clk", NULL);
-	clk_register_clkdev(clk, "ls1x_i2c", NULL);
-	clk_register_clkdev(clk, "ls1x_pwmtimer", NULL);
-	clk_register_clkdev(clk, "ls1x_spi", NULL);
-	clk_register_clkdev(clk, "ls1x_wdt", NULL);
-	clk_register_clkdev(clk, "serial8250", NULL);
-}
diff --git a/drivers/clk/clk-max-gen.c b/drivers/clk/clk-max-gen.c
deleted file mode 100644
index 35af9cb6da4f..000000000000
--- a/drivers/clk/clk-max-gen.c
+++ /dev/null
@@ -1,194 +0,0 @@
-/*
- * clk-max-gen.c - Generic clock driver for Maxim PMICs clocks
- *
- * Copyright (C) 2014 Google, Inc
- *
- * Copyright (C) 2012 Samsung Electornics
- * Jonghwa Lee <jonghwa3.lee@samsung.com>
- *
- * This program is free software; you can redistribute  it and/or modify it
- * under  the terms of  the GNU General  Public License as published by the
- * Free Software Foundation;  either version 2 of the  License, or (at your
- * option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * This driver is based on clk-max77686.c
- *
- */
-
-#include <linux/kernel.h>
-#include <linux/slab.h>
-#include <linux/err.h>
-#include <linux/regmap.h>
-#include <linux/platform_device.h>
-#include <linux/clk-provider.h>
-#include <linux/mutex.h>
-#include <linux/clkdev.h>
-#include <linux/of.h>
-#include <linux/export.h>
-
-#include "clk-max-gen.h"
-
-struct max_gen_clk {
-	struct regmap *regmap;
-	u32 mask;
-	u32 reg;
-	struct clk_hw hw;
-};
-
-static struct max_gen_clk *to_max_gen_clk(struct clk_hw *hw)
-{
-	return container_of(hw, struct max_gen_clk, hw);
-}
-
-static int max_gen_clk_prepare(struct clk_hw *hw)
-{
-	struct max_gen_clk *max_gen = to_max_gen_clk(hw);
-
-	return regmap_update_bits(max_gen->regmap, max_gen->reg,
-				  max_gen->mask, max_gen->mask);
-}
-
-static void max_gen_clk_unprepare(struct clk_hw *hw)
-{
-	struct max_gen_clk *max_gen = to_max_gen_clk(hw);
-
-	regmap_update_bits(max_gen->regmap, max_gen->reg,
-			   max_gen->mask, ~max_gen->mask);
-}
-
-static int max_gen_clk_is_prepared(struct clk_hw *hw)
-{
-	struct max_gen_clk *max_gen = to_max_gen_clk(hw);
-	int ret;
-	u32 val;
-
-	ret = regmap_read(max_gen->regmap, max_gen->reg, &val);
-
-	if (ret < 0)
-		return -EINVAL;
-
-	return val & max_gen->mask;
-}
-
-static unsigned long max_gen_recalc_rate(struct clk_hw *hw,
-					 unsigned long parent_rate)
-{
-	return 32768;
-}
-
-struct clk_ops max_gen_clk_ops = {
-	.prepare	= max_gen_clk_prepare,
-	.unprepare	= max_gen_clk_unprepare,
-	.is_prepared	= max_gen_clk_is_prepared,
-	.recalc_rate	= max_gen_recalc_rate,
-};
-EXPORT_SYMBOL_GPL(max_gen_clk_ops);
-
-static struct clk *max_gen_clk_register(struct device *dev,
-					struct max_gen_clk *max_gen)
-{
-	struct clk *clk;
-	struct clk_hw *hw = &max_gen->hw;
-	int ret;
-
-	clk = devm_clk_register(dev, hw);
-	if (IS_ERR(clk))
-		return clk;
-
-	ret = clk_register_clkdev(clk, hw->init->name, NULL);
-
-	if (ret)
-		return ERR_PTR(ret);
-
-	return clk;
-}
-
-int max_gen_clk_probe(struct platform_device *pdev, struct regmap *regmap,
-		      u32 reg, struct clk_init_data *clks_init, int num_init)
-{
-	int i, ret;
-	struct max_gen_clk *max_gen_clks;
-	struct clk **clocks;
-	struct device *dev = pdev->dev.parent;
-	const char *clk_name;
-	struct clk_init_data *init;
-
-	clocks = devm_kzalloc(dev, sizeof(struct clk *) * num_init, GFP_KERNEL);
-	if (!clocks)
-		return -ENOMEM;
-
-	max_gen_clks = devm_kzalloc(dev, sizeof(struct max_gen_clk)
-				    * num_init, GFP_KERNEL);
-	if (!max_gen_clks)
-		return -ENOMEM;
-
-	for (i = 0; i < num_init; i++) {
-		max_gen_clks[i].regmap = regmap;
-		max_gen_clks[i].mask = 1 << i;
-		max_gen_clks[i].reg = reg;
-
-		init = devm_kzalloc(dev, sizeof(*init), GFP_KERNEL);
-		if (!init)
-			return -ENOMEM;
-
-		if (dev->of_node &&
-		    !of_property_read_string_index(dev->of_node,
-						   "clock-output-names",
-						   i, &clk_name))
-			init->name = clk_name;
-		else
-			init->name = clks_init[i].name;
-
-		init->ops = clks_init[i].ops;
-		init->flags = clks_init[i].flags;
-
-		max_gen_clks[i].hw.init = init;
-
-		clocks[i] = max_gen_clk_register(dev, &max_gen_clks[i]);
-		if (IS_ERR(clocks[i])) {
-			ret = PTR_ERR(clocks[i]);
-			dev_err(dev, "failed to register %s\n",
-				max_gen_clks[i].hw.init->name);
-			return ret;
-		}
-	}
-
-	platform_set_drvdata(pdev, clocks);
-
-	if (dev->of_node) {
-		struct clk_onecell_data *of_data;
-
-		of_data = devm_kzalloc(dev, sizeof(*of_data), GFP_KERNEL);
-		if (!of_data)
-			return -ENOMEM;
-
-		of_data->clks = clocks;
-		of_data->clk_num = num_init;
-		ret = of_clk_add_provider(dev->of_node, of_clk_src_onecell_get,
-					  of_data);
-
-		if (ret) {
-			dev_err(dev, "failed to register OF clock provider\n");
-			return ret;
-		}
-	}
-
-	return 0;
-}
-EXPORT_SYMBOL_GPL(max_gen_clk_probe);
-
-int max_gen_clk_remove(struct platform_device *pdev, int num_init)
-{
-	struct device *dev = pdev->dev.parent;
-
-	if (dev->of_node)
-		of_clk_del_provider(dev->of_node);
-
-	return 0;
-}
-EXPORT_SYMBOL_GPL(max_gen_clk_remove);
diff --git a/drivers/clk/clk-max-gen.h b/drivers/clk/clk-max-gen.h
deleted file mode 100644
index 997e86fc3f4d..000000000000
--- a/drivers/clk/clk-max-gen.h
+++ /dev/null
@@ -1,32 +0,0 @@
-/*
- * clk-max-gen.h - Generic clock driver for Maxim PMICs clocks
- *
- * Copyright (C) 2014 Google, 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.
- *
- */
-
-#ifndef __CLK_MAX_GEN_H__
-#define __CLK_MAX_GEN_H__
-
-#include <linux/types.h>
-#include <linux/device.h>
-#include <linux/clkdev.h>
-#include <linux/regmap.h>
-#include <linux/platform_device.h>
-
-int max_gen_clk_probe(struct platform_device *pdev, struct regmap *regmap,
-		      u32 reg, struct clk_init_data *clks_init, int num_init);
-int max_gen_clk_remove(struct platform_device *pdev, int num_init);
-extern struct clk_ops max_gen_clk_ops;
-
-#endif /* __CLK_MAX_GEN_H__ */
diff --git a/drivers/clk/clk-max77686.c b/drivers/clk/clk-max77686.c
index 9b6f2772e948..b637f5979023 100644
--- a/drivers/clk/clk-max77686.c
+++ b/drivers/clk/clk-max77686.c
@@ -1,5 +1,5 @@
 /*
- * clk-max77686.c - Clock driver for Maxim 77686
+ * clk-max77686.c - Clock driver for Maxim 77686/MAX77802
  *
  * Copyright (C) 2012 Samsung Electornics
  * Jonghwa Lee <jonghwa3.lee@samsung.com>
@@ -25,46 +25,284 @@
 #include <linux/err.h>
 #include <linux/module.h>
 #include <linux/platform_device.h>
+#include <linux/mfd/max77620.h>
 #include <linux/mfd/max77686.h>
 #include <linux/mfd/max77686-private.h>
 #include <linux/clk-provider.h>
 #include <linux/mutex.h>
 #include <linux/clkdev.h>
+#include <linux/of.h>
+#include <linux/regmap.h>
 
 #include <dt-bindings/clock/maxim,max77686.h>
-#include "clk-max-gen.h"
+#include <dt-bindings/clock/maxim,max77802.h>
+#include <dt-bindings/clock/maxim,max77620.h>
 
-static struct clk_init_data max77686_clks_init[MAX77686_CLKS_NUM] = {
+#define MAX77802_CLOCK_LOW_JITTER_SHIFT 0x3
+
+enum max77686_chip_name {
+	CHIP_MAX77686,
+	CHIP_MAX77802,
+	CHIP_MAX77620,
+};
+
+struct max77686_hw_clk_info {
+	const char *name;
+	u32 clk_reg;
+	u32 clk_enable_mask;
+	u32 flags;
+};
+
+struct max77686_clk_init_data {
+	struct regmap *regmap;
+	struct clk_hw hw;
+	struct clk_init_data clk_idata;
+	const struct max77686_hw_clk_info *clk_info;
+};
+
+struct max77686_clk_driver_data {
+	enum max77686_chip_name chip;
+	struct max77686_clk_init_data *max_clk_data;
+	size_t num_clks;
+};
+
+static const struct
+max77686_hw_clk_info max77686_hw_clks_info[MAX77686_CLKS_NUM] = {
 	[MAX77686_CLK_AP] = {
 		.name = "32khz_ap",
-		.ops = &max_gen_clk_ops,
+		.clk_reg = MAX77686_REG_32KHZ,
+		.clk_enable_mask = BIT(MAX77686_CLK_AP),
 	},
 	[MAX77686_CLK_CP] = {
 		.name = "32khz_cp",
-		.ops = &max_gen_clk_ops,
+		.clk_reg = MAX77686_REG_32KHZ,
+		.clk_enable_mask = BIT(MAX77686_CLK_CP),
 	},
 	[MAX77686_CLK_PMIC] = {
 		.name = "32khz_pmic",
-		.ops = &max_gen_clk_ops,
+		.clk_reg = MAX77686_REG_32KHZ,
+		.clk_enable_mask = BIT(MAX77686_CLK_PMIC),
+	},
+};
+
+static const struct
+max77686_hw_clk_info max77802_hw_clks_info[MAX77802_CLKS_NUM] = {
+	[MAX77802_CLK_32K_AP] = {
+		.name = "32khz_ap",
+		.clk_reg = MAX77802_REG_32KHZ,
+		.clk_enable_mask = BIT(MAX77802_CLK_32K_AP),
+	},
+	[MAX77802_CLK_32K_CP] = {
+		.name = "32khz_cp",
+		.clk_reg = MAX77802_REG_32KHZ,
+		.clk_enable_mask = BIT(MAX77802_CLK_32K_CP),
+	},
+};
+
+static const struct
+max77686_hw_clk_info max77620_hw_clks_info[MAX77620_CLKS_NUM] = {
+	[MAX77620_CLK_32K_OUT0] = {
+		.name = "32khz_out0",
+		.clk_reg = MAX77620_REG_CNFG1_32K,
+		.clk_enable_mask = MAX77620_CNFG1_32K_OUT0_EN,
 	},
 };
 
+static struct max77686_clk_init_data *to_max77686_clk_init_data(
+				struct clk_hw *hw)
+{
+	return container_of(hw, struct max77686_clk_init_data, hw);
+}
+
+static int max77686_clk_prepare(struct clk_hw *hw)
+{
+	struct max77686_clk_init_data *max77686 = to_max77686_clk_init_data(hw);
+
+	return regmap_update_bits(max77686->regmap, max77686->clk_info->clk_reg,
+				  max77686->clk_info->clk_enable_mask,
+				  max77686->clk_info->clk_enable_mask);
+}
+
+static void max77686_clk_unprepare(struct clk_hw *hw)
+{
+	struct max77686_clk_init_data *max77686 = to_max77686_clk_init_data(hw);
+
+	regmap_update_bits(max77686->regmap, max77686->clk_info->clk_reg,
+			   max77686->clk_info->clk_enable_mask,
+			   ~max77686->clk_info->clk_enable_mask);
+}
+
+static int max77686_clk_is_prepared(struct clk_hw *hw)
+{
+	struct max77686_clk_init_data *max77686 = to_max77686_clk_init_data(hw);
+	int ret;
+	u32 val;
+
+	ret = regmap_read(max77686->regmap, max77686->clk_info->clk_reg, &val);
+
+	if (ret < 0)
+		return -EINVAL;
+
+	return val & max77686->clk_info->clk_enable_mask;
+}
+
+static unsigned long max77686_recalc_rate(struct clk_hw *hw,
+					  unsigned long parent_rate)
+{
+	return 32768;
+}
+
+static struct clk_ops max77686_clk_ops = {
+	.prepare	= max77686_clk_prepare,
+	.unprepare	= max77686_clk_unprepare,
+	.is_prepared	= max77686_clk_is_prepared,
+	.recalc_rate	= max77686_recalc_rate,
+};
+
+static struct clk_hw *
+of_clk_max77686_get(struct of_phandle_args *clkspec, void *data)
+{
+	struct max77686_clk_driver_data *drv_data = data;
+	unsigned int idx = clkspec->args[0];
+
+	if (idx >= drv_data->num_clks) {
+		pr_err("%s: invalid index %u\n", __func__, idx);
+		return ERR_PTR(-EINVAL);
+	}
+
+	return &drv_data->max_clk_data[idx].hw;
+}
+
 static int max77686_clk_probe(struct platform_device *pdev)
 {
-	struct max77686_dev *iodev = dev_get_drvdata(pdev->dev.parent);
+	struct device *dev = &pdev->dev;
+	struct device *parent = dev->parent;
+	const struct platform_device_id *id = platform_get_device_id(pdev);
+	struct max77686_clk_driver_data *drv_data;
+	const struct max77686_hw_clk_info *hw_clks;
+	struct regmap *regmap;
+	int i, ret, num_clks;
+
+	drv_data = devm_kzalloc(dev, sizeof(*drv_data), GFP_KERNEL);
+	if (!drv_data)
+		return -ENOMEM;
+
+	regmap = dev_get_regmap(parent, NULL);
+	if (!regmap) {
+		dev_err(dev, "Failed to get rtc regmap\n");
+		return -ENODEV;
+	}
+
+	drv_data->chip = id->driver_data;
+
+	switch (drv_data->chip) {
+	case CHIP_MAX77686:
+		num_clks = MAX77686_CLKS_NUM;
+		hw_clks = max77686_hw_clks_info;
+		break;
+
+	case CHIP_MAX77802:
+		num_clks = MAX77802_CLKS_NUM;
+		hw_clks = max77802_hw_clks_info;
+		break;
+
+	case CHIP_MAX77620:
+		num_clks = MAX77620_CLKS_NUM;
+		hw_clks = max77620_hw_clks_info;
+		break;
 
-	return max_gen_clk_probe(pdev, iodev->regmap, MAX77686_REG_32KHZ,
-				 max77686_clks_init, MAX77686_CLKS_NUM);
+	default:
+		dev_err(dev, "Unknown Chip ID\n");
+		return -EINVAL;
+	}
+
+	drv_data->max_clk_data = devm_kcalloc(dev, num_clks,
+					      sizeof(*drv_data->max_clk_data),
+					      GFP_KERNEL);
+	if (!drv_data->max_clk_data)
+		return -ENOMEM;
+
+	for (i = 0; i < num_clks; i++) {
+		struct max77686_clk_init_data *max_clk_data;
+		const char *clk_name;
+
+		max_clk_data = &drv_data->max_clk_data[i];
+
+		max_clk_data->regmap = regmap;
+		max_clk_data->clk_info = &hw_clks[i];
+		max_clk_data->clk_idata.flags = hw_clks[i].flags;
+		max_clk_data->clk_idata.ops = &max77686_clk_ops;
+
+		if (parent->of_node &&
+		    !of_property_read_string_index(parent->of_node,
+						   "clock-output-names",
+						   i, &clk_name))
+			max_clk_data->clk_idata.name = clk_name;
+		else
+			max_clk_data->clk_idata.name = hw_clks[i].name;
+
+		max_clk_data->hw.init = &max_clk_data->clk_idata;
+
+		ret = devm_clk_hw_register(dev, &max_clk_data->hw);
+		if (ret) {
+			dev_err(dev, "Failed to clock register: %d\n", ret);
+			return ret;
+		}
+
+		ret = clk_hw_register_clkdev(&max_clk_data->hw,
+					     max_clk_data->clk_idata.name, NULL);
+		if (ret < 0) {
+			dev_err(dev, "Failed to clkdev register: %d\n", ret);
+			return ret;
+		}
+	}
+
+	if (parent->of_node) {
+		ret = of_clk_add_hw_provider(parent->of_node, of_clk_max77686_get,
+					     drv_data);
+
+		if (ret < 0) {
+			dev_err(dev, "Failed to register OF clock provider: %d\n",
+				ret);
+			return ret;
+		}
+	}
+
+	/* MAX77802: Enable low-jitter mode on the 32khz clocks. */
+	if (drv_data->chip == CHIP_MAX77802) {
+		ret = regmap_update_bits(regmap, MAX77802_REG_32KHZ,
+					 1 << MAX77802_CLOCK_LOW_JITTER_SHIFT,
+					 1 << MAX77802_CLOCK_LOW_JITTER_SHIFT);
+		if (ret < 0) {
+			dev_err(dev, "Failed to config low-jitter: %d\n", ret);
+			goto remove_of_clk_provider;
+		}
+	}
+
+	return 0;
+
+remove_of_clk_provider:
+	if (parent->of_node)
+		of_clk_del_provider(parent->of_node);
+
+	return ret;
 }
 
 static int max77686_clk_remove(struct platform_device *pdev)
 {
-	return max_gen_clk_remove(pdev, MAX77686_CLKS_NUM);
+	struct device *parent = pdev->dev.parent;
+
+	if (parent->of_node)
+		of_clk_del_provider(parent->of_node);
+
+	return 0;
 }
 
 static const struct platform_device_id max77686_clk_id[] = {
-	{ "max77686-clk", 0},
-	{ },
+	{ "max77686-clk", .driver_data = CHIP_MAX77686, },
+	{ "max77802-clk", .driver_data = CHIP_MAX77802, },
+	{ "max77620-clock", .driver_data = CHIP_MAX77620, },
+	{},
 };
 MODULE_DEVICE_TABLE(platform, max77686_clk_id);
 
diff --git a/drivers/clk/clk-max77802.c b/drivers/clk/clk-max77802.c
deleted file mode 100644
index 355dd2e522c3..000000000000
--- a/drivers/clk/clk-max77802.c
+++ /dev/null
@@ -1,96 +0,0 @@
-/*
- * clk-max77802.c - Clock driver for Maxim 77802
- *
- * Copyright (C) 2014 Google, Inc
- *
- * Copyright (C) 2012 Samsung Electornics
- * Jonghwa Lee <jonghwa3.lee@samsung.com>
- *
- * This program is free software; you can redistribute  it and/or modify it
- * under  the terms of  the GNU General  Public License as published by the
- * Free Software Foundation;  either version 2 of the  License, or (at your
- * option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * This driver is based on clk-max77686.c
- */
-
-#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>
-#include <linux/mutex.h>
-#include <linux/clkdev.h>
-
-#include <dt-bindings/clock/maxim,max77802.h>
-#include "clk-max-gen.h"
-
-#define MAX77802_CLOCK_OPMODE_MASK	0x1
-#define MAX77802_CLOCK_LOW_JITTER_SHIFT 0x3
-
-static struct clk_init_data max77802_clks_init[MAX77802_CLKS_NUM] = {
-	[MAX77802_CLK_32K_AP] = {
-		.name = "32khz_ap",
-		.ops = &max_gen_clk_ops,
-	},
-	[MAX77802_CLK_32K_CP] = {
-		.name = "32khz_cp",
-		.ops = &max_gen_clk_ops,
-	},
-};
-
-static int max77802_clk_probe(struct platform_device *pdev)
-{
-	struct max77686_dev *iodev = dev_get_drvdata(pdev->dev.parent);
-	int ret;
-
-	ret = max_gen_clk_probe(pdev, iodev->regmap, MAX77802_REG_32KHZ,
-				max77802_clks_init, MAX77802_CLKS_NUM);
-
-	if (ret) {
-		dev_err(&pdev->dev, "generic probe failed %d\n", ret);
-		return ret;
-	}
-
-	/* Enable low-jitter mode on the 32khz clocks. */
-	ret = regmap_update_bits(iodev->regmap, MAX77802_REG_32KHZ,
-				 1 << MAX77802_CLOCK_LOW_JITTER_SHIFT,
-				 1 << MAX77802_CLOCK_LOW_JITTER_SHIFT);
-	if (ret < 0)
-		dev_err(&pdev->dev, "failed to enable low-jitter mode\n");
-
-	return ret;
-}
-
-static int max77802_clk_remove(struct platform_device *pdev)
-{
-	return max_gen_clk_remove(pdev, MAX77802_CLKS_NUM);
-}
-
-static const struct platform_device_id max77802_clk_id[] = {
-	{ "max77802-clk", 0},
-	{ },
-};
-MODULE_DEVICE_TABLE(platform, max77802_clk_id);
-
-static struct platform_driver max77802_clk_driver = {
-	.driver = {
-		.name  = "max77802-clk",
-	},
-	.probe = max77802_clk_probe,
-	.remove = max77802_clk_remove,
-	.id_table = max77802_clk_id,
-};
-
-module_platform_driver(max77802_clk_driver);
-
-MODULE_DESCRIPTION("MAXIM 77802 Clock Driver");
-MODULE_AUTHOR("Javier Martinez Canillas <javier@osg.samsung.com");
-MODULE_LICENSE("GPL");
diff --git a/drivers/clk/clk-mb86s7x.c b/drivers/clk/clk-mb86s7x.c
index e0817754ca3e..2a83a3ff1d09 100644
--- a/drivers/clk/clk-mb86s7x.c
+++ b/drivers/clk/clk-mb86s7x.c
@@ -327,10 +327,11 @@ static struct clk_ops clk_clc_ops = {
 	.set_rate = clc_set_rate,
 };
 
-struct clk *mb86s7x_clclk_register(struct device *cpu_dev)
+static struct clk_hw *mb86s7x_clclk_register(struct device *cpu_dev)
 {
 	struct clk_init_data init;
 	struct cl_clk *clc;
+	int ret;
 
 	clc = kzalloc(sizeof(*clc), GFP_KERNEL);
 	if (!clc)
@@ -344,14 +345,17 @@ struct clk *mb86s7x_clclk_register(struct device *cpu_dev)
 	init.flags = CLK_GET_RATE_NOCACHE;
 	init.num_parents = 0;
 
-	return devm_clk_register(cpu_dev, &clc->hw);
+	ret = devm_clk_hw_register(cpu_dev, &clc->hw);
+	if (ret)
+		return ERR_PTR(ret);
+	return &clc->hw;
 }
 
 static int mb86s7x_clclk_of_init(void)
 {
 	int cpu, ret = -ENODEV;
 	struct device_node *np;
-	struct clk *clk;
+	struct clk_hw *hw;
 
 	np = of_find_compatible_node(NULL, NULL, "fujitsu,mb86s70-scb-1.0");
 	if (!np || !of_device_is_available(np))
@@ -365,12 +369,12 @@ static int mb86s7x_clclk_of_init(void)
 			continue;
 		}
 
-		clk = mb86s7x_clclk_register(cpu_dev);
-		if (IS_ERR(clk)) {
+		hw = mb86s7x_clclk_register(cpu_dev);
+		if (IS_ERR(hw)) {
 			pr_err("failed to register cpu%d clock\n", cpu);
 			continue;
 		}
-		if (clk_register_clkdev(clk, NULL, dev_name(cpu_dev))) {
+		if (clk_hw_register_clkdev(hw, NULL, dev_name(cpu_dev))) {
 			pr_err("failed to register cpu%d clock lookup\n", cpu);
 			continue;
 		}
diff --git a/drivers/clk/clk-moxart.c b/drivers/clk/clk-moxart.c
index f37f719643ec..b86dac851116 100644
--- a/drivers/clk/clk-moxart.c
+++ b/drivers/clk/clk-moxart.c
@@ -19,7 +19,8 @@
 static void __init moxart_of_pll_clk_init(struct device_node *node)
 {
 	static void __iomem *base;
-	struct clk *clk, *ref_clk;
+	struct clk_hw *hw;
+	struct clk *ref_clk;
 	unsigned int mul;
 	const char *name = node->name;
 	const char *parent_name;
@@ -42,14 +43,14 @@ static void __init moxart_of_pll_clk_init(struct device_node *node)
 		return;
 	}
 
-	clk = clk_register_fixed_factor(NULL, name, parent_name, 0, mul, 1);
-	if (IS_ERR(clk)) {
+	hw = clk_hw_register_fixed_factor(NULL, name, parent_name, 0, mul, 1);
+	if (IS_ERR(hw)) {
 		pr_err("%s: failed to register clock\n", node->full_name);
 		return;
 	}
 
-	clk_register_clkdev(clk, NULL, name);
-	of_clk_add_provider(node, of_clk_src_simple_get, clk);
+	clk_hw_register_clkdev(hw, NULL, name);
+	of_clk_add_hw_provider(node, of_clk_hw_simple_get, hw);
 }
 CLK_OF_DECLARE(moxart_pll_clock, "moxa,moxart-pll-clock",
 	       moxart_of_pll_clk_init);
@@ -57,7 +58,8 @@ CLK_OF_DECLARE(moxart_pll_clock, "moxa,moxart-pll-clock",
 static void __init moxart_of_apb_clk_init(struct device_node *node)
 {
 	static void __iomem *base;
-	struct clk *clk, *pll_clk;
+	struct clk_hw *hw;
+	struct clk *pll_clk;
 	unsigned int div, val;
 	unsigned int div_idx[] = { 2, 3, 4, 6, 8};
 	const char *name = node->name;
@@ -85,14 +87,14 @@ static void __init moxart_of_apb_clk_init(struct device_node *node)
 		return;
 	}
 
-	clk = clk_register_fixed_factor(NULL, name, parent_name, 0, 1, div);
-	if (IS_ERR(clk)) {
+	hw = clk_hw_register_fixed_factor(NULL, name, parent_name, 0, 1, div);
+	if (IS_ERR(hw)) {
 		pr_err("%s: failed to register clock\n", node->full_name);
 		return;
 	}
 
-	clk_register_clkdev(clk, NULL, name);
-	of_clk_add_provider(node, of_clk_src_simple_get, clk);
+	clk_hw_register_clkdev(hw, NULL, name);
+	of_clk_add_hw_provider(node, of_clk_hw_simple_get, hw);
 }
 CLK_OF_DECLARE(moxart_apb_clock, "moxa,moxart-apb-clock",
 	       moxart_of_apb_clk_init);
diff --git a/drivers/clk/clk-nspire.c b/drivers/clk/clk-nspire.c
index 64f196a90816..f861011d5d21 100644
--- a/drivers/clk/clk-nspire.c
+++ b/drivers/clk/clk-nspire.c
@@ -69,7 +69,7 @@ static void __init nspire_ahbdiv_setup(struct device_node *node,
 {
 	u32 val;
 	void __iomem *io;
-	struct clk *clk;
+	struct clk_hw *hw;
 	const char *clk_name = node->name;
 	const char *parent_name;
 	struct nspire_clk_info info;
@@ -85,10 +85,10 @@ static void __init nspire_ahbdiv_setup(struct device_node *node,
 	of_property_read_string(node, "clock-output-names", &clk_name);
 	parent_name = of_clk_get_parent_name(node, 0);
 
-	clk = clk_register_fixed_factor(NULL, clk_name, parent_name, 0,
-					1, info.base_ahb_ratio);
-	if (!IS_ERR(clk))
-		of_clk_add_provider(node, of_clk_src_simple_get, clk);
+	hw = clk_hw_register_fixed_factor(NULL, clk_name, parent_name, 0,
+					  1, info.base_ahb_ratio);
+	if (!IS_ERR(hw))
+		of_clk_add_hw_provider(node, of_clk_hw_simple_get, hw);
 }
 
 static void __init nspire_ahbdiv_setup_cx(struct device_node *node)
@@ -111,7 +111,7 @@ static void __init nspire_clk_setup(struct device_node *node,
 {
 	u32 val;
 	void __iomem *io;
-	struct clk *clk;
+	struct clk_hw *hw;
 	const char *clk_name = node->name;
 	struct nspire_clk_info info;
 
@@ -125,9 +125,10 @@ static void __init nspire_clk_setup(struct device_node *node,
 
 	of_property_read_string(node, "clock-output-names", &clk_name);
 
-	clk = clk_register_fixed_rate(NULL, clk_name, NULL, 0, info.base_clock);
-	if (!IS_ERR(clk))
-		of_clk_add_provider(node, of_clk_src_simple_get, clk);
+	hw = clk_hw_register_fixed_rate(NULL, clk_name, NULL, 0,
+					info.base_clock);
+	if (!IS_ERR(hw))
+		of_clk_add_hw_provider(node, of_clk_hw_simple_get, hw);
 	else
 		return;
 
diff --git a/drivers/clk/clk-palmas.c b/drivers/clk/clk-palmas.c
index 8328863cb0e0..31f590cea493 100644
--- a/drivers/clk/clk-palmas.c
+++ b/drivers/clk/clk-palmas.c
@@ -41,7 +41,6 @@ struct palmas_clk32k_desc {
 
 struct palmas_clock_info {
 	struct device *dev;
-	struct clk *clk;
 	struct clk_hw hw;
 	struct palmas *palmas;
 	const struct palmas_clk32k_desc *clk_desc;
@@ -218,7 +217,7 @@ static int palmas_clks_init_configure(struct palmas_clock_info *cinfo)
 	}
 
 	if (cinfo->ext_control_pin) {
-		ret = clk_prepare(cinfo->clk);
+		ret = clk_prepare(cinfo->hw.clk);
 		if (ret < 0) {
 			dev_err(cinfo->dev, "Clock prep failed, %d\n", ret);
 			return ret;
@@ -242,7 +241,6 @@ static int palmas_clks_probe(struct platform_device *pdev)
 	struct device_node *node = pdev->dev.of_node;
 	const struct palmas_clks_of_match_data *match_data;
 	struct palmas_clock_info *cinfo;
-	struct clk *clk;
 	int ret;
 
 	match_data = of_device_get_match_data(&pdev->dev);
@@ -261,22 +259,20 @@ static int palmas_clks_probe(struct platform_device *pdev)
 
 	cinfo->clk_desc = &match_data->desc;
 	cinfo->hw.init = &match_data->init;
-	clk = devm_clk_register(&pdev->dev, &cinfo->hw);
-	if (IS_ERR(clk)) {
-		ret = PTR_ERR(clk);
+	ret = devm_clk_hw_register(&pdev->dev, &cinfo->hw);
+	if (ret) {
 		dev_err(&pdev->dev, "Fail to register clock %s, %d\n",
 			match_data->desc.clk_name, ret);
 		return ret;
 	}
 
-	cinfo->clk = clk;
 	ret = palmas_clks_init_configure(cinfo);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "Clock config failed, %d\n", ret);
 		return ret;
 	}
 
-	ret = of_clk_add_provider(node, of_clk_src_simple_get, cinfo->clk);
+	ret = of_clk_add_hw_provider(node, of_clk_hw_simple_get, &cinfo->hw);
 	if (ret < 0)
 		dev_err(&pdev->dev, "Fail to add clock driver, %d\n", ret);
 	return ret;
diff --git a/drivers/clk/clk-pwm.c b/drivers/clk/clk-pwm.c
index 1630a1f085f7..8cb9d117fdbf 100644
--- a/drivers/clk/clk-pwm.c
+++ b/drivers/clk/clk-pwm.c
@@ -61,7 +61,6 @@ static int clk_pwm_probe(struct platform_device *pdev)
 	struct pwm_device *pwm;
 	struct pwm_args pargs;
 	const char *clk_name;
-	struct clk *clk;
 	int ret;
 
 	clk_pwm = devm_kzalloc(&pdev->dev, sizeof(*clk_pwm), GFP_KERNEL);
@@ -107,11 +106,11 @@ static int clk_pwm_probe(struct platform_device *pdev)
 
 	clk_pwm->pwm = pwm;
 	clk_pwm->hw.init = &init;
-	clk = devm_clk_register(&pdev->dev, &clk_pwm->hw);
-	if (IS_ERR(clk))
-		return PTR_ERR(clk);
+	ret = devm_clk_hw_register(&pdev->dev, &clk_pwm->hw);
+	if (ret)
+		return ret;
 
-	return of_clk_add_provider(node, of_clk_src_simple_get, clk);
+	return of_clk_add_hw_provider(node, of_clk_hw_simple_get, &clk_pwm->hw);
 }
 
 static int clk_pwm_remove(struct platform_device *pdev)
diff --git a/drivers/clk/clk-qoriq.c b/drivers/clk/clk-qoriq.c
index 58566a17944a..20b105584f82 100644
--- a/drivers/clk/clk-qoriq.c
+++ b/drivers/clk/clk-qoriq.c
@@ -766,7 +766,11 @@ static struct clk * __init create_one_cmux(struct clockgen *cg, int idx)
 	if (!hwc)
 		return NULL;
 
-	hwc->reg = cg->regs + 0x20 * idx;
+	if (cg->info.flags & CG_VER3)
+		hwc->reg = cg->regs + 0x70000 + 0x20 * idx;
+	else
+		hwc->reg = cg->regs + 0x20 * idx;
+
 	hwc->info = cg->info.cmux_groups[cg->info.cmux_to_group[idx]];
 
 	/*
diff --git a/drivers/clk/clk-rk808.c b/drivers/clk/clk-rk808.c
index 74383039761e..6461f2820a5b 100644
--- a/drivers/clk/clk-rk808.c
+++ b/drivers/clk/clk-rk808.c
@@ -22,11 +22,8 @@
 #include <linux/mfd/rk808.h>
 #include <linux/i2c.h>
 
-#define RK808_NR_OUTPUT 2
-
 struct rk808_clkout {
 	struct rk808 *rk808;
-	struct clk_onecell_data clk_data;
 	struct clk_hw		clkout1_hw;
 	struct clk_hw		clkout2_hw;
 };
@@ -85,14 +82,28 @@ static const struct clk_ops rk808_clkout2_ops = {
 	.recalc_rate = rk808_clkout_recalc_rate,
 };
 
+static struct clk_hw *
+of_clk_rk808_get(struct of_phandle_args *clkspec, void *data)
+{
+	struct rk808_clkout *rk808_clkout = data;
+	unsigned int idx = clkspec->args[0];
+
+	if (idx >= 2) {
+		pr_err("%s: invalid index %u\n", __func__, idx);
+		return ERR_PTR(-EINVAL);
+	}
+
+	return idx ? &rk808_clkout->clkout2_hw : &rk808_clkout->clkout1_hw;
+}
+
 static int rk808_clkout_probe(struct platform_device *pdev)
 {
 	struct rk808 *rk808 = dev_get_drvdata(pdev->dev.parent);
 	struct i2c_client *client = rk808->i2c;
 	struct device_node *node = client->dev.of_node;
 	struct clk_init_data init = {};
-	struct clk **clk_table;
 	struct rk808_clkout *rk808_clkout;
+	int ret;
 
 	rk808_clkout = devm_kzalloc(&client->dev,
 				    sizeof(*rk808_clkout), GFP_KERNEL);
@@ -101,11 +112,6 @@ static int rk808_clkout_probe(struct platform_device *pdev)
 
 	rk808_clkout->rk808 = rk808;
 
-	clk_table = devm_kcalloc(&client->dev, RK808_NR_OUTPUT,
-				 sizeof(struct clk *), GFP_KERNEL);
-	if (!clk_table)
-		return -ENOMEM;
-
 	init.parent_names = NULL;
 	init.num_parents = 0;
 	init.name = "rk808-clkout1";
@@ -116,10 +122,9 @@ static int rk808_clkout_probe(struct platform_device *pdev)
 	of_property_read_string_index(node, "clock-output-names",
 				      0, &init.name);
 
-	clk_table[0] = devm_clk_register(&client->dev,
-					 &rk808_clkout->clkout1_hw);
-	if (IS_ERR(clk_table[0]))
-		return PTR_ERR(clk_table[0]);
+	ret = devm_clk_hw_register(&client->dev, &rk808_clkout->clkout1_hw);
+	if (ret)
+		return ret;
 
 	init.name = "rk808-clkout2";
 	init.ops = &rk808_clkout2_ops;
@@ -129,16 +134,11 @@ static int rk808_clkout_probe(struct platform_device *pdev)
 	of_property_read_string_index(node, "clock-output-names",
 				      1, &init.name);
 
-	clk_table[1] = devm_clk_register(&client->dev,
-					 &rk808_clkout->clkout2_hw);
-	if (IS_ERR(clk_table[1]))
-		return PTR_ERR(clk_table[1]);
-
-	rk808_clkout->clk_data.clks = clk_table;
-	rk808_clkout->clk_data.clk_num = RK808_NR_OUTPUT;
+	ret = devm_clk_hw_register(&client->dev, &rk808_clkout->clkout2_hw);
+	if (ret)
+		return ret;
 
-	return of_clk_add_provider(node, of_clk_src_onecell_get,
-				   &rk808_clkout->clk_data);
+	return of_clk_add_hw_provider(node, of_clk_rk808_get, rk808_clkout);
 }
 
 static int rk808_clkout_remove(struct platform_device *pdev)
diff --git a/drivers/clk/clk-scpi.c b/drivers/clk/clk-scpi.c
index 6962ee5d1e9a..2a3e9d8e88b0 100644
--- a/drivers/clk/clk-scpi.c
+++ b/drivers/clk/clk-scpi.c
@@ -146,13 +146,13 @@ static const struct of_device_id scpi_clk_match[] = {
 	{}
 };
 
-static struct clk *
+static int
 scpi_clk_ops_init(struct device *dev, const struct of_device_id *match,
 		  struct scpi_clk *sclk, const char *name)
 {
 	struct clk_init_data init;
-	struct clk *clk;
 	unsigned long min = 0, max = 0;
+	int ret;
 
 	init.name = name;
 	init.flags = 0;
@@ -164,18 +164,18 @@ scpi_clk_ops_init(struct device *dev, const struct of_device_id *match,
 	if (init.ops == &scpi_dvfs_ops) {
 		sclk->info = sclk->scpi_ops->dvfs_get_info(sclk->id);
 		if (IS_ERR(sclk->info))
-			return NULL;
+			return PTR_ERR(sclk->info);
 	} else if (init.ops == &scpi_clk_ops) {
 		if (sclk->scpi_ops->clk_get_range(sclk->id, &min, &max) || !max)
-			return NULL;
+			return -EINVAL;
 	} else {
-		return NULL;
+		return -EINVAL;
 	}
 
-	clk = devm_clk_register(dev, &sclk->hw);
-	if (!IS_ERR(clk) && max)
+	ret = devm_clk_hw_register(dev, &sclk->hw);
+	if (!ret && max)
 		clk_hw_set_rate_range(&sclk->hw, min, max);
-	return clk;
+	return ret;
 }
 
 struct scpi_clk_data {
@@ -183,7 +183,7 @@ struct scpi_clk_data {
 	unsigned int clk_num;
 };
 
-static struct clk *
+static struct clk_hw *
 scpi_of_clk_src_get(struct of_phandle_args *clkspec, void *data)
 {
 	struct scpi_clk *sclk;
@@ -193,7 +193,7 @@ scpi_of_clk_src_get(struct of_phandle_args *clkspec, void *data)
 	for (count = 0; count < clk_data->clk_num; count++) {
 		sclk = clk_data->clk[count];
 		if (idx == sclk->id)
-			return sclk->hw.clk;
+			return &sclk->hw;
 	}
 
 	return ERR_PTR(-EINVAL);
@@ -202,8 +202,7 @@ scpi_of_clk_src_get(struct of_phandle_args *clkspec, void *data)
 static int scpi_clk_add(struct device *dev, struct device_node *np,
 			const struct of_device_id *match)
 {
-	struct clk **clks;
-	int idx, count;
+	int idx, count, err;
 	struct scpi_clk_data *clk_data;
 
 	count = of_property_count_strings(np, "clock-output-names");
@@ -222,10 +221,6 @@ static int scpi_clk_add(struct device *dev, struct device_node *np,
 	if (!clk_data->clk)
 		return -ENOMEM;
 
-	clks = devm_kcalloc(dev, count, sizeof(*clks), GFP_KERNEL);
-	if (!clks)
-		return -ENOMEM;
-
 	for (idx = 0; idx < count; idx++) {
 		struct scpi_clk *sclk;
 		const char *name;
@@ -249,15 +244,15 @@ static int scpi_clk_add(struct device *dev, struct device_node *np,
 
 		sclk->id = val;
 
-		clks[idx] = scpi_clk_ops_init(dev, match, sclk, name);
-		if (IS_ERR_OR_NULL(clks[idx]))
+		err = scpi_clk_ops_init(dev, match, sclk, name);
+		if (err)
 			dev_err(dev, "failed to register clock '%s'\n", name);
 		else
 			dev_dbg(dev, "Registered clock '%s'\n", name);
 		clk_data->clk[idx] = sclk;
 	}
 
-	return of_clk_add_provider(np, scpi_of_clk_src_get, clk_data);
+	return of_clk_add_hw_provider(np, scpi_of_clk_src_get, clk_data);
 }
 
 static int scpi_clocks_remove(struct platform_device *pdev)
diff --git a/drivers/clk/clk-si514.c b/drivers/clk/clk-si514.c
index ceef25b0990b..09b6718956bd 100644
--- a/drivers/clk/clk-si514.c
+++ b/drivers/clk/clk-si514.c
@@ -305,7 +305,6 @@ static int si514_probe(struct i2c_client *client,
 {
 	struct clk_si514 *data;
 	struct clk_init_data init;
-	struct clk *clk;
 	int err;
 
 	data = devm_kzalloc(&client->dev, sizeof(*data), GFP_KERNEL);
@@ -330,13 +329,13 @@ static int si514_probe(struct i2c_client *client,
 
 	i2c_set_clientdata(client, data);
 
-	clk = devm_clk_register(&client->dev, &data->hw);
-	if (IS_ERR(clk)) {
+	err = devm_clk_hw_register(&client->dev, &data->hw);
+	if (err) {
 		dev_err(&client->dev, "clock registration failed\n");
-		return PTR_ERR(clk);
+		return err;
 	}
-	err = of_clk_add_provider(client->dev.of_node, of_clk_src_simple_get,
-			clk);
+	err = of_clk_add_hw_provider(client->dev.of_node, of_clk_hw_simple_get,
+				     &data->hw);
 	if (err) {
 		dev_err(&client->dev, "unable to add clk provider\n");
 		return err;
diff --git a/drivers/clk/clk-si5351.c b/drivers/clk/clk-si5351.c
index b1bc12c045d3..b051db43fae1 100644
--- a/drivers/clk/clk-si5351.c
+++ b/drivers/clk/clk-si5351.c
@@ -54,7 +54,6 @@ struct si5351_driver_data {
 	enum si5351_variant	variant;
 	struct i2c_client	*client;
 	struct regmap		*regmap;
-	struct clk_onecell_data onecell;
 
 	struct clk		*pxtal;
 	const char		*pxtal_name;
@@ -66,6 +65,7 @@ struct si5351_driver_data {
 	struct si5351_hw_data	pll[2];
 	struct si5351_hw_data	*msynth;
 	struct si5351_hw_data	*clkout;
+	size_t			num_clkout;
 };
 
 static const char * const si5351_input_names[] = {
@@ -1307,11 +1307,31 @@ put_child:
 	of_node_put(child);
 	return -EINVAL;
 }
+
+static struct clk_hw *
+si53351_of_clk_get(struct of_phandle_args *clkspec, void *data)
+{
+	struct si5351_driver_data *drvdata = data;
+	unsigned int idx = clkspec->args[0];
+
+	if (idx >= drvdata->num_clkout) {
+		pr_err("%s: invalid index %u\n", __func__, idx);
+		return ERR_PTR(-EINVAL);
+	}
+
+	return &drvdata->clkout[idx].hw;
+}
 #else
 static int si5351_dt_parse(struct i2c_client *client, enum si5351_variant variant)
 {
 	return 0;
 }
+
+static struct clk_hw *
+si53351_of_clk_get(struct of_phandle_args *clkspec, void *data)
+{
+	return NULL;
+}
 #endif /* CONFIG_OF */
 
 static int si5351_i2c_probe(struct i2c_client *client,
@@ -1321,7 +1341,6 @@ static int si5351_i2c_probe(struct i2c_client *client,
 	struct si5351_platform_data *pdata;
 	struct si5351_driver_data *drvdata;
 	struct clk_init_data init;
-	struct clk *clk;
 	const char *parent_names[4];
 	u8 num_parents, num_clocks;
 	int ret, n;
@@ -1438,10 +1457,9 @@ static int si5351_i2c_probe(struct i2c_client *client,
 		init.num_parents = 1;
 	}
 	drvdata->xtal.init = &init;
-	clk = devm_clk_register(&client->dev, &drvdata->xtal);
-	if (IS_ERR(clk)) {
+	ret = devm_clk_hw_register(&client->dev, &drvdata->xtal);
+	if (ret) {
 		dev_err(&client->dev, "unable to register %s\n", init.name);
-		ret = PTR_ERR(clk);
 		goto err_clk;
 	}
 
@@ -1456,11 +1474,10 @@ static int si5351_i2c_probe(struct i2c_client *client,
 			init.num_parents = 1;
 		}
 		drvdata->clkin.init = &init;
-		clk = devm_clk_register(&client->dev, &drvdata->clkin);
-		if (IS_ERR(clk)) {
+		ret = devm_clk_hw_register(&client->dev, &drvdata->clkin);
+		if (ret) {
 			dev_err(&client->dev, "unable to register %s\n",
 				init.name);
-			ret = PTR_ERR(clk);
 			goto err_clk;
 		}
 	}
@@ -1480,10 +1497,9 @@ static int si5351_i2c_probe(struct i2c_client *client,
 	init.flags = 0;
 	init.parent_names = parent_names;
 	init.num_parents = num_parents;
-	clk = devm_clk_register(&client->dev, &drvdata->pll[0].hw);
-	if (IS_ERR(clk)) {
+	ret = devm_clk_hw_register(&client->dev, &drvdata->pll[0].hw);
+	if (ret) {
 		dev_err(&client->dev, "unable to register %s\n", init.name);
-		ret = PTR_ERR(clk);
 		goto err_clk;
 	}
 
@@ -1505,10 +1521,9 @@ static int si5351_i2c_probe(struct i2c_client *client,
 		init.parent_names = parent_names;
 		init.num_parents = num_parents;
 	}
-	clk = devm_clk_register(&client->dev, &drvdata->pll[1].hw);
-	if (IS_ERR(clk)) {
+	ret = devm_clk_hw_register(&client->dev, &drvdata->pll[1].hw);
+	if (ret) {
 		dev_err(&client->dev, "unable to register %s\n", init.name);
-		ret = PTR_ERR(clk);
 		goto err_clk;
 	}
 
@@ -1524,13 +1539,9 @@ static int si5351_i2c_probe(struct i2c_client *client,
 				       sizeof(*drvdata->msynth), GFP_KERNEL);
 	drvdata->clkout = devm_kzalloc(&client->dev, num_clocks *
 				       sizeof(*drvdata->clkout), GFP_KERNEL);
+	drvdata->num_clkout = num_clocks;
 
-	drvdata->onecell.clk_num = num_clocks;
-	drvdata->onecell.clks = devm_kzalloc(&client->dev,
-		num_clocks * sizeof(*drvdata->onecell.clks), GFP_KERNEL);
-
-	if (WARN_ON(!drvdata->msynth || !drvdata->clkout ||
-		    !drvdata->onecell.clks)) {
+	if (WARN_ON(!drvdata->msynth || !drvdata->clkout)) {
 		ret = -ENOMEM;
 		goto err_clk;
 	}
@@ -1547,11 +1558,11 @@ static int si5351_i2c_probe(struct i2c_client *client,
 			init.flags |= CLK_SET_RATE_PARENT;
 		init.parent_names = parent_names;
 		init.num_parents = 2;
-		clk = devm_clk_register(&client->dev, &drvdata->msynth[n].hw);
-		if (IS_ERR(clk)) {
+		ret = devm_clk_hw_register(&client->dev,
+					   &drvdata->msynth[n].hw);
+		if (ret) {
 			dev_err(&client->dev, "unable to register %s\n",
 				init.name);
-			ret = PTR_ERR(clk);
 			goto err_clk;
 		}
 	}
@@ -1575,19 +1586,19 @@ static int si5351_i2c_probe(struct i2c_client *client,
 			init.flags |= CLK_SET_RATE_PARENT;
 		init.parent_names = parent_names;
 		init.num_parents = num_parents;
-		clk = devm_clk_register(&client->dev, &drvdata->clkout[n].hw);
-		if (IS_ERR(clk)) {
+		ret = devm_clk_hw_register(&client->dev,
+					   &drvdata->clkout[n].hw);
+		if (ret) {
 			dev_err(&client->dev, "unable to register %s\n",
 				init.name);
-			ret = PTR_ERR(clk);
 			goto err_clk;
 		}
-		drvdata->onecell.clks[n] = clk;
 
 		/* set initial clkout rate */
 		if (pdata->clkout[n].rate != 0) {
 			int ret;
-			ret = clk_set_rate(clk, pdata->clkout[n].rate);
+			ret = clk_set_rate(drvdata->clkout[n].hw.clk,
+					   pdata->clkout[n].rate);
 			if (ret != 0) {
 				dev_err(&client->dev, "Cannot set rate : %d\n",
 					ret);
@@ -1595,8 +1606,8 @@ static int si5351_i2c_probe(struct i2c_client *client,
 		}
 	}
 
-	ret = of_clk_add_provider(client->dev.of_node, of_clk_src_onecell_get,
-				  &drvdata->onecell);
+	ret = of_clk_add_hw_provider(client->dev.of_node, si53351_of_clk_get,
+				     drvdata);
 	if (ret) {
 		dev_err(&client->dev, "unable to add clk provider\n");
 		goto err_clk;
diff --git a/drivers/clk/clk-si570.c b/drivers/clk/clk-si570.c
index d56648521a95..646af1d1898d 100644
--- a/drivers/clk/clk-si570.c
+++ b/drivers/clk/clk-si570.c
@@ -408,7 +408,6 @@ static int si570_probe(struct i2c_client *client,
 {
 	struct clk_si570 *data;
 	struct clk_init_data init;
-	struct clk *clk;
 	u32 initial_fout, factory_fout, stability;
 	int err;
 	enum clk_si570_variant variant = id->driver_data;
@@ -462,13 +461,13 @@ static int si570_probe(struct i2c_client *client,
 	if (err)
 		return err;
 
-	clk = devm_clk_register(&client->dev, &data->hw);
-	if (IS_ERR(clk)) {
+	err = devm_clk_hw_register(&client->dev, &data->hw);
+	if (err) {
 		dev_err(&client->dev, "clock registration failed\n");
-		return PTR_ERR(clk);
+		return err;
 	}
-	err = of_clk_add_provider(client->dev.of_node, of_clk_src_simple_get,
-			clk);
+	err = of_clk_add_hw_provider(client->dev.of_node, of_clk_hw_simple_get,
+				     &data->hw);
 	if (err) {
 		dev_err(&client->dev, "unable to add clk provider\n");
 		return err;
@@ -477,7 +476,7 @@ static int si570_probe(struct i2c_client *client,
 	/* Read the requested initial output frequency from device tree */
 	if (!of_property_read_u32(client->dev.of_node, "clock-frequency",
 				&initial_fout)) {
-		err = clk_set_rate(clk, initial_fout);
+		err = clk_set_rate(data->hw.clk, initial_fout);
 		if (err) {
 			of_clk_del_provider(client->dev.of_node);
 			return err;
diff --git a/drivers/clk/clk-twl6040.c b/drivers/clk/clk-twl6040.c
index 697c66757400..7b222a5db931 100644
--- a/drivers/clk/clk-twl6040.c
+++ b/drivers/clk/clk-twl6040.c
@@ -26,60 +26,73 @@
 #include <linux/mfd/twl6040.h>
 #include <linux/clk-provider.h>
 
-struct twl6040_clk {
+struct twl6040_pdmclk {
 	struct twl6040 *twl6040;
 	struct device *dev;
-	struct clk_hw mcpdm_fclk;
-	struct clk *clk;
+	struct clk_hw pdmclk_hw;
 	int enabled;
 };
 
-static int twl6040_bitclk_is_enabled(struct clk_hw *hw)
+static int twl6040_pdmclk_is_prepared(struct clk_hw *hw)
 {
-	struct twl6040_clk *twl6040_clk = container_of(hw, struct twl6040_clk,
-						       mcpdm_fclk);
-	return twl6040_clk->enabled;
+	struct twl6040_pdmclk *pdmclk = container_of(hw, struct twl6040_pdmclk,
+						     pdmclk_hw);
+
+	return pdmclk->enabled;
 }
 
-static int twl6040_bitclk_prepare(struct clk_hw *hw)
+static int twl6040_pdmclk_prepare(struct clk_hw *hw)
 {
-	struct twl6040_clk *twl6040_clk = container_of(hw, struct twl6040_clk,
-						       mcpdm_fclk);
+	struct twl6040_pdmclk *pdmclk = container_of(hw, struct twl6040_pdmclk,
+						     pdmclk_hw);
 	int ret;
 
-	ret = twl6040_power(twl6040_clk->twl6040, 1);
+	ret = twl6040_power(pdmclk->twl6040, 1);
 	if (!ret)
-		twl6040_clk->enabled = 1;
+		pdmclk->enabled = 1;
 
 	return ret;
 }
 
-static void twl6040_bitclk_unprepare(struct clk_hw *hw)
+static void twl6040_pdmclk_unprepare(struct clk_hw *hw)
 {
-	struct twl6040_clk *twl6040_clk = container_of(hw, struct twl6040_clk,
-						       mcpdm_fclk);
+	struct twl6040_pdmclk *pdmclk = container_of(hw, struct twl6040_pdmclk,
+						     pdmclk_hw);
 	int ret;
 
-	ret = twl6040_power(twl6040_clk->twl6040, 0);
+	ret = twl6040_power(pdmclk->twl6040, 0);
 	if (!ret)
-		twl6040_clk->enabled = 0;
+		pdmclk->enabled = 0;
+
 }
 
-static const struct clk_ops twl6040_mcpdm_ops = {
-	.is_enabled = twl6040_bitclk_is_enabled,
-	.prepare = twl6040_bitclk_prepare,
-	.unprepare = twl6040_bitclk_unprepare,
+static unsigned long twl6040_pdmclk_recalc_rate(struct clk_hw *hw,
+						unsigned long parent_rate)
+{
+	struct twl6040_pdmclk *pdmclk = container_of(hw, struct twl6040_pdmclk,
+						     pdmclk_hw);
+
+	return twl6040_get_sysclk(pdmclk->twl6040);
+}
+
+static const struct clk_ops twl6040_pdmclk_ops = {
+	.is_prepared = twl6040_pdmclk_is_prepared,
+	.prepare = twl6040_pdmclk_prepare,
+	.unprepare = twl6040_pdmclk_unprepare,
+	.recalc_rate = twl6040_pdmclk_recalc_rate,
 };
 
-static struct clk_init_data wm831x_clkout_init = {
-	.name = "mcpdm_fclk",
-	.ops = &twl6040_mcpdm_ops,
+static struct clk_init_data twl6040_pdmclk_init = {
+	.name = "pdmclk",
+	.ops = &twl6040_pdmclk_ops,
+	.flags = CLK_GET_RATE_NOCACHE,
 };
 
-static int twl6040_clk_probe(struct platform_device *pdev)
+static int twl6040_pdmclk_probe(struct platform_device *pdev)
 {
 	struct twl6040 *twl6040 = dev_get_drvdata(pdev->dev.parent);
-	struct twl6040_clk *clkdata;
+	struct twl6040_pdmclk *clkdata;
+	int ret;
 
 	clkdata = devm_kzalloc(&pdev->dev, sizeof(*clkdata), GFP_KERNEL);
 	if (!clkdata)
@@ -88,26 +101,28 @@ static int twl6040_clk_probe(struct platform_device *pdev)
 	clkdata->dev = &pdev->dev;
 	clkdata->twl6040 = twl6040;
 
-	clkdata->mcpdm_fclk.init = &wm831x_clkout_init;
-	clkdata->clk = devm_clk_register(&pdev->dev, &clkdata->mcpdm_fclk);
-	if (IS_ERR(clkdata->clk))
-		return PTR_ERR(clkdata->clk);
+	clkdata->pdmclk_hw.init = &twl6040_pdmclk_init;
+	ret = devm_clk_hw_register(&pdev->dev, &clkdata->pdmclk_hw);
+	if (ret)
+		return ret;
 
 	platform_set_drvdata(pdev, clkdata);
 
-	return 0;
+	return of_clk_add_hw_provider(pdev->dev.parent->of_node,
+				      of_clk_hw_simple_get,
+				      &clkdata->pdmclk_hw);
 }
 
-static struct platform_driver twl6040_clk_driver = {
+static struct platform_driver twl6040_pdmclk_driver = {
 	.driver = {
-		.name = "twl6040-clk",
+		.name = "twl6040-pdmclk",
 	},
-	.probe = twl6040_clk_probe,
+	.probe = twl6040_pdmclk_probe,
 };
 
-module_platform_driver(twl6040_clk_driver);
+module_platform_driver(twl6040_pdmclk_driver);
 
 MODULE_DESCRIPTION("TWL6040 clock driver for McPDM functional clock");
 MODULE_AUTHOR("Peter Ujfalusi <peter.ujfalusi@ti.com>");
-MODULE_ALIAS("platform:twl6040-clk");
+MODULE_ALIAS("platform:twl6040-pdmclk");
 MODULE_LICENSE("GPL");
diff --git a/drivers/clk/clk-vt8500.c b/drivers/clk/clk-vt8500.c
index 37368a399ff9..4161a6f25741 100644
--- a/drivers/clk/clk-vt8500.c
+++ b/drivers/clk/clk-vt8500.c
@@ -232,7 +232,7 @@ static const struct clk_ops vt8500_gated_divisor_clk_ops = {
 static __init void vtwm_device_clk_init(struct device_node *node)
 {
 	u32 en_reg, div_reg;
-	struct clk *clk;
+	struct clk_hw *hw;
 	struct clk_device *dev_clk;
 	const char *clk_name = node->name;
 	const char *parent_name;
@@ -301,13 +301,14 @@ static __init void vtwm_device_clk_init(struct device_node *node)
 
 	dev_clk->hw.init = &init;
 
-	clk = clk_register(NULL, &dev_clk->hw);
-	if (WARN_ON(IS_ERR(clk))) {
+	hw = &dev_clk->hw;
+	rc = clk_hw_register(NULL, hw);
+	if (WARN_ON(rc)) {
 		kfree(dev_clk);
 		return;
 	}
-	rc = of_clk_add_provider(node, of_clk_src_simple_get, clk);
-	clk_register_clkdev(clk, clk_name, NULL);
+	rc = of_clk_add_hw_provider(node, of_clk_hw_simple_get, hw);
+	clk_hw_register_clkdev(hw, clk_name, NULL);
 }
 CLK_OF_DECLARE(vt8500_device, "via,vt8500-device-clock", vtwm_device_clk_init);
 
@@ -681,7 +682,7 @@ static const struct clk_ops vtwm_pll_ops = {
 static __init void vtwm_pll_clk_init(struct device_node *node, int pll_type)
 {
 	u32 reg;
-	struct clk *clk;
+	struct clk_hw *hw;
 	struct clk_pll *pll_clk;
 	const char *clk_name = node->name;
 	const char *parent_name;
@@ -714,13 +715,14 @@ static __init void vtwm_pll_clk_init(struct device_node *node, int pll_type)
 
 	pll_clk->hw.init = &init;
 
-	clk = clk_register(NULL, &pll_clk->hw);
-	if (WARN_ON(IS_ERR(clk))) {
+	hw = &pll_clk->hw;
+	rc = clk_hw_register(NULL, &pll_clk->hw);
+	if (WARN_ON(rc)) {
 		kfree(pll_clk);
 		return;
 	}
-	rc = of_clk_add_provider(node, of_clk_src_simple_get, clk);
-	clk_register_clkdev(clk, clk_name, NULL);
+	rc = of_clk_add_hw_provider(node, of_clk_hw_simple_get, hw);
+	clk_hw_register_clkdev(hw, clk_name, NULL);
 }
 
 
diff --git a/drivers/clk/clk-wm831x.c b/drivers/clk/clk-wm831x.c
index 88def4b2761c..f4fdac55727c 100644
--- a/drivers/clk/clk-wm831x.c
+++ b/drivers/clk/clk-wm831x.c
@@ -24,9 +24,6 @@ struct wm831x_clk {
 	struct clk_hw xtal_hw;
 	struct clk_hw fll_hw;
 	struct clk_hw clkout_hw;
-	struct clk *xtal;
-	struct clk *fll;
-	struct clk *clkout;
 	bool xtal_ena;
 };
 
@@ -370,19 +367,19 @@ static int wm831x_clk_probe(struct platform_device *pdev)
 	clkdata->xtal_ena = ret & WM831X_XTAL_ENA;
 
 	clkdata->xtal_hw.init = &wm831x_xtal_init;
-	clkdata->xtal = devm_clk_register(&pdev->dev, &clkdata->xtal_hw);
-	if (IS_ERR(clkdata->xtal))
-		return PTR_ERR(clkdata->xtal);
+	ret = devm_clk_hw_register(&pdev->dev, &clkdata->xtal_hw);
+	if (ret)
+		return ret;
 
 	clkdata->fll_hw.init = &wm831x_fll_init;
-	clkdata->fll = devm_clk_register(&pdev->dev, &clkdata->fll_hw);
-	if (IS_ERR(clkdata->fll))
-		return PTR_ERR(clkdata->fll);
+	ret = devm_clk_hw_register(&pdev->dev, &clkdata->fll_hw);
+	if (ret)
+		return ret;
 
 	clkdata->clkout_hw.init = &wm831x_clkout_init;
-	clkdata->clkout = devm_clk_register(&pdev->dev, &clkdata->clkout_hw);
-	if (IS_ERR(clkdata->clkout))
-		return PTR_ERR(clkdata->clkout);
+	ret = devm_clk_hw_register(&pdev->dev, &clkdata->clkout_hw);
+	if (ret)
+		return ret;
 
 	platform_set_drvdata(pdev, clkdata);
 
diff --git a/drivers/clk/clk-xgene.c b/drivers/clk/clk-xgene.c
index 343313250c58..5daddf5ecc4b 100644
--- a/drivers/clk/clk-xgene.c
+++ b/drivers/clk/clk-xgene.c
@@ -217,6 +217,226 @@ static void xgene_pcppllclk_init(struct device_node *np)
 	xgene_pllclk_init(np, PLL_TYPE_PCP);
 }
 
+/**
+ * struct xgene_clk_pmd - PMD clock
+ *
+ * @hw:		handle between common and hardware-specific interfaces
+ * @reg:	register containing the fractional scale multiplier (scaler)
+ * @shift:	shift to the unit bit field
+ * @denom:	1/denominator unit
+ * @lock:	register lock
+ * Flags:
+ * XGENE_CLK_PMD_SCALE_INVERTED - By default the scaler is the value read
+ *	from the register plus one. For example,
+ *		0 for (0 + 1) / denom,
+ *		1 for (1 + 1) / denom and etc.
+ *	If this flag is set, it is
+ *		0 for (denom - 0) / denom,
+ *		1 for (denom - 1) / denom and etc.
+ *
+ */
+struct xgene_clk_pmd {
+	struct clk_hw	hw;
+	void __iomem	*reg;
+	u8		shift;
+	u32		mask;
+	u64		denom;
+	u32		flags;
+	spinlock_t	*lock;
+};
+
+#define to_xgene_clk_pmd(_hw) container_of(_hw, struct xgene_clk_pmd, hw)
+
+#define XGENE_CLK_PMD_SCALE_INVERTED	BIT(0)
+#define XGENE_CLK_PMD_SHIFT		8
+#define XGENE_CLK_PMD_WIDTH		3
+
+static unsigned long xgene_clk_pmd_recalc_rate(struct clk_hw *hw,
+					       unsigned long parent_rate)
+{
+	struct xgene_clk_pmd *fd = to_xgene_clk_pmd(hw);
+	unsigned long flags = 0;
+	u64 ret, scale;
+	u32 val;
+
+	if (fd->lock)
+		spin_lock_irqsave(fd->lock, flags);
+	else
+		__acquire(fd->lock);
+
+	val = clk_readl(fd->reg);
+
+	if (fd->lock)
+		spin_unlock_irqrestore(fd->lock, flags);
+	else
+		__release(fd->lock);
+
+	ret = (u64)parent_rate;
+
+	scale = (val & fd->mask) >> fd->shift;
+	if (fd->flags & XGENE_CLK_PMD_SCALE_INVERTED)
+		scale = fd->denom - scale;
+	else
+		scale++;
+
+	/* freq = parent_rate * scaler / denom */
+	do_div(ret, fd->denom);
+	ret *= scale;
+	if (ret == 0)
+		ret = (u64)parent_rate;
+
+	return ret;
+}
+
+static long xgene_clk_pmd_round_rate(struct clk_hw *hw, unsigned long rate,
+				     unsigned long *parent_rate)
+{
+	struct xgene_clk_pmd *fd = to_xgene_clk_pmd(hw);
+	u64 ret, scale;
+
+	if (!rate || rate >= *parent_rate)
+		return *parent_rate;
+
+	/* freq = parent_rate * scaler / denom */
+	ret = rate * fd->denom;
+	scale = DIV_ROUND_UP_ULL(ret, *parent_rate);
+
+	ret = (u64)*parent_rate * scale;
+	do_div(ret, fd->denom);
+
+	return ret;
+}
+
+static int xgene_clk_pmd_set_rate(struct clk_hw *hw, unsigned long rate,
+				  unsigned long parent_rate)
+{
+	struct xgene_clk_pmd *fd = to_xgene_clk_pmd(hw);
+	unsigned long flags = 0;
+	u64 scale, ret;
+	u32 val;
+
+	/*
+	 * Compute the scaler:
+	 *
+	 * freq = parent_rate * scaler / denom, or
+	 * scaler = freq * denom / parent_rate
+	 */
+	ret = rate * fd->denom;
+	scale = DIV_ROUND_UP_ULL(ret, (u64)parent_rate);
+
+	/* Check if inverted */
+	if (fd->flags & XGENE_CLK_PMD_SCALE_INVERTED)
+		scale = fd->denom - scale;
+	else
+		scale--;
+
+	if (fd->lock)
+		spin_lock_irqsave(fd->lock, flags);
+	else
+		__acquire(fd->lock);
+
+	val = clk_readl(fd->reg);
+	val &= ~fd->mask;
+	val |= (scale << fd->shift);
+	clk_writel(val, fd->reg);
+
+	if (fd->lock)
+		spin_unlock_irqrestore(fd->lock, flags);
+	else
+		__release(fd->lock);
+
+	return 0;
+}
+
+static const struct clk_ops xgene_clk_pmd_ops = {
+	.recalc_rate = xgene_clk_pmd_recalc_rate,
+	.round_rate = xgene_clk_pmd_round_rate,
+	.set_rate = xgene_clk_pmd_set_rate,
+};
+
+static struct clk *
+xgene_register_clk_pmd(struct device *dev,
+		       const char *name, const char *parent_name,
+		       unsigned long flags, void __iomem *reg, u8 shift,
+		       u8 width, u64 denom, u32 clk_flags, spinlock_t *lock)
+{
+	struct xgene_clk_pmd *fd;
+	struct clk_init_data init;
+	struct clk *clk;
+
+	fd = kzalloc(sizeof(*fd), GFP_KERNEL);
+	if (!fd)
+		return ERR_PTR(-ENOMEM);
+
+	init.name = name;
+	init.ops = &xgene_clk_pmd_ops;
+	init.flags = flags;
+	init.parent_names = parent_name ? &parent_name : NULL;
+	init.num_parents = parent_name ? 1 : 0;
+
+	fd->reg = reg;
+	fd->shift = shift;
+	fd->mask = (BIT(width) - 1) << shift;
+	fd->denom = denom;
+	fd->flags = clk_flags;
+	fd->lock = lock;
+	fd->hw.init = &init;
+
+	clk = clk_register(dev, &fd->hw);
+	if (IS_ERR(clk)) {
+		pr_err("%s: could not register clk %s\n", __func__, name);
+		kfree(fd);
+		return NULL;
+	}
+
+	return clk;
+}
+
+static void xgene_pmdclk_init(struct device_node *np)
+{
+	const char *clk_name = np->full_name;
+	void __iomem *csr_reg;
+	struct resource res;
+	struct clk *clk;
+	u64 denom;
+	u32 flags = 0;
+	int rc;
+
+	/* Check if the entry is disabled */
+	if (!of_device_is_available(np))
+		return;
+
+	/* Parse the DTS register for resource */
+	rc = of_address_to_resource(np, 0, &res);
+	if (rc != 0) {
+		pr_err("no DTS register for %s\n", np->full_name);
+		return;
+	}
+	csr_reg = of_iomap(np, 0);
+	if (!csr_reg) {
+		pr_err("Unable to map resource for %s\n", np->full_name);
+		return;
+	}
+	of_property_read_string(np, "clock-output-names", &clk_name);
+
+	denom = BIT(XGENE_CLK_PMD_WIDTH);
+	flags |= XGENE_CLK_PMD_SCALE_INVERTED;
+
+	clk = xgene_register_clk_pmd(NULL, clk_name,
+				     of_clk_get_parent_name(np, 0), 0,
+				     csr_reg, XGENE_CLK_PMD_SHIFT,
+				     XGENE_CLK_PMD_WIDTH, denom,
+				     flags, &clk_lock);
+	if (!IS_ERR(clk)) {
+		of_clk_add_provider(np, of_clk_src_simple_get, clk);
+		clk_register_clkdev(clk, clk_name, NULL);
+		pr_debug("Add %s clock\n", clk_name);
+	} else {
+		if (csr_reg)
+			iounmap(csr_reg);
+	}
+}
+
 /* IP Clock */
 struct xgene_dev_parameters {
 	void __iomem *csr_reg;		/* CSR for IP clock */
@@ -543,6 +763,7 @@ err:
 
 CLK_OF_DECLARE(xgene_socpll_clock, "apm,xgene-socpll-clock", xgene_socpllclk_init);
 CLK_OF_DECLARE(xgene_pcppll_clock, "apm,xgene-pcppll-clock", xgene_pcppllclk_init);
+CLK_OF_DECLARE(xgene_pmd_clock, "apm,xgene-pmd-clock", xgene_pmdclk_init);
 CLK_OF_DECLARE(xgene_socpll_v2_clock, "apm,xgene-socpll-v2-clock",
 	       xgene_socpllclk_init);
 CLK_OF_DECLARE(xgene_pcppll_v2_clock, "apm,xgene-pcppll-v2-clock",
diff --git a/drivers/clk/clk.c b/drivers/clk/clk.c
index 820a939fb6bb..0fb39fe217d1 100644
--- a/drivers/clk/clk.c
+++ b/drivers/clk/clk.c
@@ -1908,10 +1908,6 @@ int clk_set_phase(struct clk *clk, int degrees)
 
 	clk_prepare_lock();
 
-	/* bail early if nothing to do */
-	if (degrees == clk->core->phase)
-		goto out;
-
 	trace_clk_set_phase(clk->core, degrees);
 
 	if (clk->core->ops->set_phase)
@@ -1922,7 +1918,6 @@ int clk_set_phase(struct clk *clk, int degrees)
 	if (!ret)
 		clk->core->phase = degrees;
 
-out:
 	clk_prepare_unlock();
 
 	return ret;
@@ -2449,8 +2444,16 @@ static int __clk_core_init(struct clk_core *core)
 	hlist_for_each_entry_safe(orphan, tmp2, &clk_orphan_list, child_node) {
 		struct clk_core *parent = __clk_init_parent(orphan);
 
-		if (parent)
-			clk_core_reparent(orphan, parent);
+		/*
+		 * we could call __clk_set_parent, but that would result in a
+		 * redundant call to the .set_rate op, if it exists
+		 */
+		if (parent) {
+			__clk_set_parent_before(orphan, parent);
+			__clk_set_parent_after(orphan, parent, NULL);
+			__clk_recalc_accuracies(orphan);
+			__clk_recalc_rates(orphan, 0);
+		}
 	}
 
 	/*
@@ -2491,7 +2494,7 @@ struct clk *__clk_create_clk(struct clk_hw *hw, const char *dev_id,
 
 	/* This is to allow this function to be chained to others */
 	if (IS_ERR_OR_NULL(hw))
-		return (struct clk *) hw;
+		return ERR_CAST(hw);
 
 	clk = kzalloc(sizeof(*clk), GFP_KERNEL);
 	if (!clk)
@@ -3166,19 +3169,14 @@ __of_clk_get_hw_from_provider(struct of_clk_provider *provider,
 			      struct of_phandle_args *clkspec)
 {
 	struct clk *clk;
-	struct clk_hw *hw = ERR_PTR(-EPROBE_DEFER);
 
-	if (provider->get_hw) {
-		hw = provider->get_hw(clkspec, provider->data);
-	} else if (provider->get) {
-		clk = provider->get(clkspec, provider->data);
-		if (!IS_ERR(clk))
-			hw = __clk_get_hw(clk);
-		else
-			hw = ERR_CAST(clk);
-	}
+	if (provider->get_hw)
+		return provider->get_hw(clkspec, provider->data);
 
-	return hw;
+	clk = provider->get(clkspec, provider->data);
+	if (IS_ERR(clk))
+		return ERR_CAST(clk);
+	return __clk_get_hw(clk);
 }
 
 struct clk *__of_clk_get_from_provider(struct of_phandle_args *clkspec,
@@ -3186,7 +3184,7 @@ struct clk *__of_clk_get_from_provider(struct of_phandle_args *clkspec,
 {
 	struct of_clk_provider *provider;
 	struct clk *clk = ERR_PTR(-EPROBE_DEFER);
-	struct clk_hw *hw = ERR_PTR(-EPROBE_DEFER);
+	struct clk_hw *hw;
 
 	if (!clkspec)
 		return ERR_PTR(-EINVAL);
@@ -3194,12 +3192,13 @@ struct clk *__of_clk_get_from_provider(struct of_phandle_args *clkspec,
 	/* Check if we have such a provider in our array */
 	mutex_lock(&of_clk_mutex);
 	list_for_each_entry(provider, &of_clk_providers, link) {
-		if (provider->node == clkspec->np)
+		if (provider->node == clkspec->np) {
 			hw = __of_clk_get_hw_from_provider(provider, clkspec);
-		if (!IS_ERR(hw)) {
 			clk = __clk_create_clk(hw, dev_id, con_id);
+		}
 
-			if (!IS_ERR(clk) && !__clk_get(clk)) {
+		if (!IS_ERR(clk)) {
+			if (!__clk_get(clk)) {
 				__clk_free_clk(clk);
 				clk = ERR_PTR(-ENOENT);
 			}
@@ -3451,6 +3450,10 @@ void __init of_clk_init(const struct of_device_id *matches)
 					&clk_provider_list, node) {
 			if (force || parent_ready(clk_provider->np)) {
 
+				/* Don't populate platform devices */
+				of_node_set_flag(clk_provider->np,
+						 OF_POPULATED);
+
 				clk_provider->clk_init_cb(clk_provider->np);
 				of_clk_set_defaults(clk_provider->np, true);
 
diff --git a/drivers/clk/h8300/clk-div.c b/drivers/clk/h8300/clk-div.c
index 4bf44a25d950..715b882205a8 100644
--- a/drivers/clk/h8300/clk-div.c
+++ b/drivers/clk/h8300/clk-div.c
@@ -14,7 +14,7 @@ static DEFINE_SPINLOCK(clklock);
 static void __init h8300_div_clk_setup(struct device_node *node)
 {
 	unsigned int num_parents;
-	struct clk *clk;
+	struct clk_hw *hw;
 	const char *clk_name = node->name;
 	const char *parent_name;
 	void __iomem *divcr = NULL;
@@ -38,15 +38,15 @@ static void __init h8300_div_clk_setup(struct device_node *node)
 
 	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,
+	hw = clk_hw_register_divider(NULL, clk_name, parent_name,
 				   CLK_SET_RATE_GATE, divcr, offset, width,
 				   CLK_DIVIDER_POWER_OF_TWO, &clklock);
-	if (!IS_ERR(clk)) {
-		of_clk_add_provider(node, of_clk_src_simple_get, clk);
+	if (!IS_ERR(hw)) {
+		of_clk_add_hw_provider(node, of_clk_hw_simple_get, hw);
 		return;
 	}
 	pr_err("%s: failed to register %s div clock (%ld)\n",
-	       __func__, clk_name, PTR_ERR(clk));
+	       __func__, clk_name, PTR_ERR(hw));
 error:
 	if (divcr)
 		iounmap(divcr);
diff --git a/drivers/clk/h8300/clk-h8s2678.c b/drivers/clk/h8300/clk-h8s2678.c
index c9c2fd575ef7..a26312460621 100644
--- a/drivers/clk/h8300/clk-h8s2678.c
+++ b/drivers/clk/h8300/clk-h8s2678.c
@@ -84,11 +84,11 @@ static const struct clk_ops pll_ops = {
 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;
+	int ret;
 
 	num_parents = of_clk_get_parent_count(node);
 	if (!num_parents) {
@@ -121,14 +121,14 @@ static void __init h8s2678_pll_clk_setup(struct device_node *node)
 	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));
+	ret = clk_hw_register(NULL, &pll_clock->hw);
+	if (ret) {
+		pr_err("%s: failed to register %s div clock (%d)\n",
+		       __func__, clk_name, ret);
 		goto unmap_pllcr;
 	}
 
-	of_clk_add_provider(node, of_clk_src_simple_get, clk);
+	of_clk_add_hw_provider(node, of_clk_hw_simple_get, &pll_clock->hw);
 	return;
 
 unmap_pllcr:
diff --git a/drivers/clk/imx/clk-imx1.c b/drivers/clk/imx/clk-imx1.c
index 99cf802fa51f..eaa462ad09e8 100644
--- a/drivers/clk/imx/clk-imx1.c
+++ b/drivers/clk/imx/clk-imx1.c
@@ -45,10 +45,13 @@ static void __iomem *ccm __initdata;
 #define CCM_PCDR	(ccm + 0x0020)
 #define SCM_GCCR	(ccm + 0x0810)
 
-static void __init _mx1_clocks_init(unsigned long fref)
+static void __init mx1_clocks_init_dt(struct device_node *np)
 {
+	ccm = of_iomap(np, 0);
+	BUG_ON(!ccm);
+
 	clk[IMX1_CLK_DUMMY] = imx_clk_fixed("dummy", 0);
-	clk[IMX1_CLK_CLK32] = imx_obtain_fixed_clock("clk32", fref);
+	clk[IMX1_CLK_CLK32] = imx_obtain_fixed_clock("clk32", 32768);
 	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);
@@ -74,45 +77,6 @@ static void __init _mx1_clocks_init(unsigned long fref)
 	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);
diff --git a/drivers/clk/imx/clk-imx35.c b/drivers/clk/imx/clk-imx35.c
index b0978d3b83e2..203cad6c9aab 100644
--- a/drivers/clk/imx/clk-imx35.c
+++ b/drivers/clk/imx/clk-imx35.c
@@ -66,20 +66,22 @@ 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, ckil, clk_max
+	/*  0 */ ckih, mpll, ppll, mpll_075, arm, hsp, hsp_div, hsp_sel, ahb,
+	/*  9 */ ipg, arm_per_div, ahb_per_div, ipg_per, uart_sel, uart_div,
+	/* 15 */ esdhc_sel, esdhc1_div, esdhc2_div, esdhc3_div, spdif_sel,
+	/* 20 */ spdif_div_pre, spdif_div_post, ssi_sel, ssi1_div_pre,
+	/* 24 */ ssi1_div_post, ssi2_div_pre, ssi2_div_post, usb_sel, usb_div,
+	/* 29 */ nfc_div, asrc_gate, pata_gate, audmux_gate, can1_gate,
+	/* 34 */ can2_gate, cspi1_gate, cspi2_gate, ect_gate, edio_gate,
+	/* 39 */ emi_gate, epit1_gate, epit2_gate, esai_gate, esdhc1_gate,
+	/* 44 */ esdhc2_gate, esdhc3_gate, fec_gate, gpio1_gate, gpio2_gate,
+	/* 49 */ gpio3_gate, gpt_gate, i2c1_gate, i2c2_gate, i2c3_gate,
+	/* 54 */ iomuxc_gate, ipu_gate, kpp_gate, mlb_gate, mshc_gate,
+	/* 59 */ owire_gate, pwm_gate, rngc_gate, rtc_gate, rtic_gate, scc_gate,
+	/* 65 */ sdma_gate, spba_gate, spdif_gate, ssi1_gate, ssi2_gate,
+	/* 70 */ uart1_gate, uart2_gate, uart3_gate, usbotg_gate, wdog_gate,
+	/* 75 */ max_gate, admux_gate, csi_gate, csi_div, csi_sel, iim_gate,
+	/* 81 */ gpu2d_gate, ckil, clk_max
 };
 
 static struct clk *clk[clk_max];
@@ -115,7 +117,7 @@ static void __init _mx35_clocks_init(void)
 	}
 
 	clk[ckih] = imx_clk_fixed("ckih", 24000000);
-	clk[ckil] = imx_clk_fixed("ckih", 32768);
+	clk[ckil] = imx_clk_fixed("ckil", 32768);
 	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);
 
diff --git a/drivers/clk/imx/clk-imx51-imx53.c b/drivers/clk/imx/clk-imx51-imx53.c
index 29d4c44ef356..1e3c9ea5f9dc 100644
--- a/drivers/clk/imx/clk-imx51-imx53.c
+++ b/drivers/clk/imx/clk-imx51-imx53.c
@@ -126,6 +126,7 @@ 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 const char *ieee1588_sels[] = { "pll3_sw", "pll4_sw", "dummy" /* usbphy2_clk */, "dummy" /* fec_phy_clk */ };
 
 static struct clk *clk[IMX5_CLK_END];
 static struct clk_onecell_data clk_data;
@@ -543,6 +544,25 @@ static void __init mx53_clocks_init(struct device_node *np)
 	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_FIRI_SEL]		= imx_clk_mux("firi_sel", MXC_CCM_CSCMR2, 12, 2,
+						standard_pll_sel, ARRAY_SIZE(standard_pll_sel));
+	clk[IMX5_CLK_FIRI_PRED]		= imx_clk_divider("firi_pred", "firi_sel", MXC_CCM_CSCDR3, 6, 3);
+	clk[IMX5_CLK_FIRI_PODF]		= imx_clk_divider("firi_podf", "firi_pred", MXC_CCM_CSCDR3, 0, 6);
+	clk[IMX5_CLK_FIRI_SERIAL_GATE]	= imx_clk_gate2("firi_serial_gate", "firi_podf", MXC_CCM_CCGR1, 28);
+	clk[IMX5_CLK_FIRI_IPG_GATE]	= imx_clk_gate2("firi_ipg_gate", "ipg", MXC_CCM_CCGR1, 26);
+
+	clk[IMX5_CLK_CSI0_MCLK1_SEL]	= imx_clk_mux("csi0_mclk1_sel", MXC_CCM_CSCMR2, 22, 2,
+						standard_pll_sel, ARRAY_SIZE(standard_pll_sel));
+	clk[IMX5_CLK_CSI0_MCLK1_PRED]	= imx_clk_divider("csi0_mclk1_pred", "csi0_mclk1_sel", MXC_CCM_CSCDR4, 6, 3);
+	clk[IMX5_CLK_CSI0_MCLK1_PODF]	= imx_clk_divider("csi0_mclk1_podf", "csi0_mclk1_pred", MXC_CCM_CSCDR4, 0, 6);
+	clk[IMX5_CLK_CSI0_MCLK1_GATE]	= imx_clk_gate2("csi0_mclk1_serial_gate", "csi0_mclk1_podf", MXC_CCM_CCGR6, 4);
+
+	clk[IMX5_CLK_IEEE1588_SEL]	= imx_clk_mux("ieee1588_sel", MXC_CCM_CSCMR2, 14, 2,
+						ieee1588_sels, ARRAY_SIZE(ieee1588_sels));
+	clk[IMX5_CLK_IEEE1588_PRED]	= imx_clk_divider("ieee1588_pred", "ieee1588_sel", MXC_CCM_CSCDR2, 6, 3);
+	clk[IMX5_CLK_IEEE1588_PODF]	= imx_clk_divider("ieee1588_podf", "ieee1588_pred", MXC_CCM_CSCDR2, 0, 6);
+	clk[IMX5_CLK_IEEE1588_GATE]	= imx_clk_gate2("ieee1588_serial_gate", "ieee1588_podf", MXC_CCM_CCGR7, 6);
+
 	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);
diff --git a/drivers/clk/imx/clk-imx6q.c b/drivers/clk/imx/clk-imx6q.c
index ba1c1ae72ac2..ce8ea10407e4 100644
--- a/drivers/clk/imx/clk-imx6q.c
+++ b/drivers/clk/imx/clk-imx6q.c
@@ -318,11 +318,16 @@ static void __init imx6q_clocks_init(struct device_node *ccm_node)
 		clk[IMX6QDL_CLK_IPG_PER_SEL] = imx_clk_mux("ipg_per_sel", base + 0x1c, 6, 1, ipg_per_sels, ARRAY_SIZE(ipg_per_sels));
 		clk[IMX6QDL_CLK_UART_SEL] = imx_clk_mux("uart_sel", base + 0x24, 6, 1, uart_sels, ARRAY_SIZE(uart_sels));
 		clk[IMX6QDL_CLK_GPU2D_CORE_SEL] = imx_clk_mux("gpu2d_core_sel", base + 0x18, 16, 2, gpu2d_core_sels_2, ARRAY_SIZE(gpu2d_core_sels_2));
+	} else if (clk_on_imx6dl()) {
+		clk[IMX6QDL_CLK_MLB_SEL] = imx_clk_mux("mlb_sel",   base + 0x18, 16, 2, gpu2d_core_sels,   ARRAY_SIZE(gpu2d_core_sels));
 	} else {
 		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));
+	if (clk_on_imx6dl())
+		clk[IMX6QDL_CLK_GPU2D_CORE_SEL] = imx_clk_mux("gpu2d_core_sel", base + 0x18, 8,  2, gpu3d_shader_sels, ARRAY_SIZE(gpu3d_shader_sels));
+	else
+		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);
@@ -400,9 +405,15 @@ static void __init imx6q_clocks_init(struct device_node *ccm_node)
 		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_DI1_DIV_3_5] = imx_clk_fixed_factor("ldb_di1_div_3_5", "ldb_di1_sel", 2, 7);
 	}
-	clk[IMX6QDL_CLK_GPU2D_CORE_PODF]  = imx_clk_divider("gpu2d_core_podf",  "gpu2d_core_sel",    base + 0x18, 23, 3);
+	if (clk_on_imx6dl())
+		clk[IMX6QDL_CLK_MLB_PODF]  = imx_clk_divider("mlb_podf",  "mlb_sel",    base + 0x18, 23, 3);
+	else
+		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);
+	if (clk_on_imx6dl())
+		clk[IMX6QDL_CLK_GPU2D_CORE_PODF]  = imx_clk_divider("gpu2d_core_podf",     "gpu2d_core_sel",  base + 0x18, 29, 3);
+	else
+		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_PODF]     = imx_clk_divider_flags("ldb_di0_podf", "ldb_di0_div_3_5", base + 0x20, 10, 1, 0);
@@ -473,14 +484,7 @@ static void __init imx6q_clocks_init(struct device_node *ccm_node)
 	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_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);
@@ -511,7 +515,7 @@ static void __init imx6q_clocks_init(struct device_node *ccm_node)
 		 * 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);
+		clk[IMX6QDL_CLK_MLB] = imx_clk_gate2("mlb",            "mlb_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);
@@ -629,6 +633,24 @@ static void __init imx6q_clocks_init(struct device_node *ccm_node)
 	if (IS_ENABLED(CONFIG_PCI_IMX6))
 		clk_set_parent(clk[IMX6QDL_CLK_LVDS1_SEL], clk[IMX6QDL_CLK_SATA_REF_100M]);
 
+	/*
+	 * Initialize the GPU clock muxes, so that the maximum specified clock
+	 * rates for the respective SoC are not exceeded.
+	 */
+	if (clk_on_imx6dl()) {
+		clk_set_parent(clk[IMX6QDL_CLK_GPU3D_CORE_SEL],
+			       clk[IMX6QDL_CLK_PLL2_PFD1_594M]);
+		clk_set_parent(clk[IMX6QDL_CLK_GPU2D_CORE_SEL],
+			       clk[IMX6QDL_CLK_PLL2_PFD1_594M]);
+	} else if (clk_on_imx6q()) {
+		clk_set_parent(clk[IMX6QDL_CLK_GPU3D_CORE_SEL],
+			       clk[IMX6QDL_CLK_MMDC_CH0_AXI]);
+		clk_set_parent(clk[IMX6QDL_CLK_GPU3D_SHADER_SEL],
+			       clk[IMX6QDL_CLK_PLL2_PFD1_594M]);
+		clk_set_parent(clk[IMX6QDL_CLK_GPU2D_CORE_SEL],
+			       clk[IMX6QDL_CLK_PLL3_USB_OTG]);
+	}
+
 	imx_register_uart_clocks(uart_clks);
 }
 CLK_OF_DECLARE(imx6q, "fsl,imx6q-ccm", imx6q_clocks_init);
diff --git a/drivers/clk/imx/clk-imx7d.c b/drivers/clk/imx/clk-imx7d.c
index 6ed4f8fa0667..e7c7353a86fc 100644
--- a/drivers/clk/imx/clk-imx7d.c
+++ b/drivers/clk/imx/clk-imx7d.c
@@ -22,43 +22,63 @@
 
 #include "clk.h"
 
+static u32 share_count_sai1;
+static u32 share_count_sai2;
+static u32 share_count_sai3;
+
+static struct clk_div_table test_div_table[] = {
+	{ .val = 3, .div = 1, },
+	{ .val = 2, .div = 1, },
+	{ .val = 1, .div = 2, },
+	{ .val = 0, .div = 4, },
+	{ }
+};
+
+static struct clk_div_table post_div_table[] = {
+	{ .val = 3, .div = 4, },
+	{ .val = 2, .div = 1, },
+	{ .val = 1, .div = 2, },
+	{ .val = 0, .div = 1, },
+	{ }
+};
+
 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_sys_main_clk", "pll_sys_pfd0_392m_clk", "pll_audio_post_div",
 	"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_dram_533m_clk", "pll_audio_post_div", "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_dram_533m_clk", "pll_audio_post_div", "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", };
+	"pll_audio_post_div", "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", };
+	"pll_sys_pfd7_clk", "pll_audio_post_div", "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_main_240m_clk", "pll_audio_post_div", "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", };
+	"pll_audio_post_div", };
 
 static const char *ahb_channel_sel[] = { "osc", "pll_sys_pfd2_270m_clk",
 	"pll_dram_533m_clk", "pll_sys_pfd0_392m_clk",
-	"pll_enet_125m_clk", "pll_usb_main_clk", "pll_audio_main_clk",
+	"pll_enet_125m_clk", "pll_usb_main_clk", "pll_audio_post_div",
 	"pll_video_main_clk", };
 
 static const char *dram_phym_sel[] = { "pll_dram_main_clk",
@@ -69,13 +89,13 @@ static const char *dram_sel[] = { "pll_dram_main_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_usb_main_clk", "pll_sys_pfd7_clk", "pll_audio_post_div",
 	"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", };
+	"pll_audio_post_div", "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",
@@ -101,53 +121,53 @@ static const char *lcdif_pixel_sel[] = { "osc", "pll_sys_pfd5_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", };
+	"pll_dram_533m_clk", "pll_video_main_clk", "pll_audio_post_div", };
 
 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", };
+	"pll_dram_533m_clk", "pll_video_main_clk", "pll_audio_post_div", };
 
 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_audio_post_div", "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_audio_post_div", "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_audio_post_div", "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_audio_post_div", "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",
+	"pll_sys_main_120m_clk", "pll_audio_post_div", "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",
+	"pll_audio_post_div", "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",
+	"pll_sys_main_120m_clk", "pll_audio_post_div", "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",
+	"pll_audio_post_div", "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_dram_533m_clk", "pll_audio_post_div", "pll_video_main_clk",
 	"pll_sys_pfd3_clk", };
 
 static const char *eim_sel[] = { "osc", "pll_sys_pfd2_135m_clk",
@@ -188,22 +208,22 @@ static const char *can2_sel[] = { "osc", "pll_sys_main_120m_clk",
 
 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_audio_post_div", "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_audio_post_div", "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_audio_post_div", "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_audio_post_div", "pll_video_main_clk", "pll_usb_main_clk",
 	"pll_sys_pfd2_135m_clk", };
 
 static const char *uart1_sel[] = { "osc", "pll_sys_main_240m_clk",
@@ -262,32 +282,32 @@ static const char *ecspi4_sel[] = { "osc", "pll_sys_main_240m_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",
+	"pll_sys_main_120m_clk", "pll_enet_40m_clk", "pll_audio_post_div",
 	"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",
+	"pll_sys_main_120m_clk", "pll_enet_40m_clk", "pll_audio_post_div",
 	"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",
+	"pll_sys_main_120m_clk", "pll_enet_40m_clk", "pll_audio_post_div",
 	"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",
+	"pll_sys_main_120m_clk", "pll_enet_40m_clk", "pll_audio_post_div",
 	"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",
+	"pll_sys_main_120m_clk", "pll_enet_40m_clk", "pll_audio_post_div",
 	"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",
+	"pll_sys_main_120m_clk", "pll_enet_40m_clk", "pll_audio_post_div",
 	"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_usb_main_clk", "pll_audio_post_div", "pll_enet_125m_clk",
 	"pll_sys_pfd7_clk", };
 
 static const char *sim2_sel[] = { "osc", "pll_sys_pfd2_135m_clk",
@@ -297,19 +317,19 @@ static const char *sim2_sel[] = { "osc", "pll_sys_pfd2_135m_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", };
+	"ref_1m_clk", "pll_audio_post_div", "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", };
+	"ref_1m_clk", "pll_audio_post_div", "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", };
+	"ref_1m_clk", "pll_audio_post_div", "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", };
+	"ref_1m_clk", "pll_audio_post_div", "ext_clk_4", };
 
 static const char *trace_sel[] = { "osc", "pll_sys_pfd2_135m_clk",
 	"pll_sys_main_120m_clk", "pll_dram_533m_clk",
@@ -323,12 +343,12 @@ static const char *wdog_sel[] = { "osc", "pll_sys_pfd2_135m_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_enet_125m_clk", "pll_audio_post_div", "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_enet_125m_clk", "pll_audio_post_div", "pll_video_main_clk",
 	"pll_usb_main_clk", };
 
 static const char *wrclk_sel[] = { "osc", "pll_enet_40m_clk",
@@ -342,13 +362,13 @@ static const char *clko1_sel[] = { "osc", "pll_sys_main_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", "ckil", };
+	"pll_audio_post_div", "pll_video_main_clk", "ckil", };
 
 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_audio_post_div", "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", };
@@ -430,6 +450,11 @@ static void __init imx7d_clocks_init(struct device_node *ccm_node)
 	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_AUDIO_TEST_DIV]  = clk_register_divider_table(NULL, "pll_audio_test_div", "pll_audio_main_clk",
+				CLK_SET_RATE_PARENT | CLK_SET_RATE_GATE, base + 0xf0, 19, 2, 0, test_div_table, &imx_ccm_lock);
+	clks[IMX7D_PLL_AUDIO_POST_DIV] = clk_register_divider_table(NULL, "pll_audio_post_div", "pll_audio_test_div",
+				CLK_SET_RATE_PARENT | CLK_SET_RATE_GATE, base + 0xf0, 22, 2, 0, post_div_table, &imx_ccm_lock);
+
 	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);
@@ -779,6 +804,7 @@ static void __init imx7d_clocks_init(struct device_node *ccm_node)
 	clks[IMX7D_DRAM_PHYM_ALT_ROOT_CLK] = imx_clk_gate4("dram_phym_alt_root_clk", "dram_phym_alt_post_div", base + 0x4130, 0);
 	clks[IMX7D_DRAM_ALT_ROOT_CLK] = imx_clk_gate4("dram_alt_root_clk", "dram_alt_post_div", base + 0x4130, 0);
 	clks[IMX7D_USB_HSIC_ROOT_CLK] = imx_clk_gate4("usb_hsic_root_clk", "usb_hsic_post_div", base + 0x4420, 0);
+	clks[IMX7D_SDMA_CORE_CLK] = imx_clk_gate4("sdma_root_clk", "ahb_root_clk", base + 0x4480, 0);
 	clks[IMX7D_PCIE_CTRL_ROOT_CLK] = imx_clk_gate4("pcie_ctrl_root_clk", "pcie_ctrl_post_div", base + 0x4600, 0);
 	clks[IMX7D_PCIE_PHY_ROOT_CLK] = imx_clk_gate4("pcie_phy_root_clk", "pcie_phy_post_div", base + 0x4600, 0);
 	clks[IMX7D_EPDC_PIXEL_ROOT_CLK] = imx_clk_gate4("epdc_pixel_root_clk", "epdc_pixel_post_div", base + 0x44a0, 0);
@@ -786,9 +812,12 @@ static void __init imx7d_clocks_init(struct device_node *ccm_node)
 	clks[IMX7D_MIPI_DSI_ROOT_CLK] = imx_clk_gate4("mipi_dsi_root_clk", "mipi_dsi_post_div", base + 0x4650, 0);
 	clks[IMX7D_MIPI_CSI_ROOT_CLK] = imx_clk_gate4("mipi_csi_root_clk", "mipi_csi_post_div", base + 0x4640, 0);
 	clks[IMX7D_MIPI_DPHY_ROOT_CLK] = imx_clk_gate4("mipi_dphy_root_clk", "mipi_dphy_post_div", base + 0x4660, 0);
-	clks[IMX7D_SAI1_ROOT_CLK] = imx_clk_gate4("sai1_root_clk", "sai1_post_div", base + 0x48c0, 0);
-	clks[IMX7D_SAI2_ROOT_CLK] = imx_clk_gate4("sai2_root_clk", "sai2_post_div", base + 0x48d0, 0);
-	clks[IMX7D_SAI3_ROOT_CLK] = imx_clk_gate4("sai3_root_clk", "sai3_post_div", base + 0x48e0, 0);
+	clks[IMX7D_SAI1_ROOT_CLK] = imx_clk_gate2_shared2("sai1_root_clk", "sai1_post_div", base + 0x48c0, 0, &share_count_sai1);
+	clks[IMX7D_SAI1_IPG_CLK]  = imx_clk_gate2_shared2("sai1_ipg_clk",  "ipg_root_clk",  base + 0x48c0, 0, &share_count_sai1);
+	clks[IMX7D_SAI2_ROOT_CLK] = imx_clk_gate2_shared2("sai2_root_clk", "sai2_post_div", base + 0x48d0, 0, &share_count_sai2);
+	clks[IMX7D_SAI2_IPG_CLK]  = imx_clk_gate2_shared2("sai2_ipg_clk",  "ipg_root_clk",  base + 0x48d0, 0, &share_count_sai2);
+	clks[IMX7D_SAI3_ROOT_CLK] = imx_clk_gate2_shared2("sai3_root_clk", "sai3_post_div", base + 0x48e0, 0, &share_count_sai3);
+	clks[IMX7D_SAI3_IPG_CLK]  = imx_clk_gate2_shared2("sai3_ipg_clk",  "ipg_root_clk",  base + 0x48e0, 0, &share_count_sai3);
 	clks[IMX7D_SPDIF_ROOT_CLK] = imx_clk_gate4("spdif_root_clk", "spdif_post_div", base + 0x44d0, 0);
 	clks[IMX7D_ENET1_REF_ROOT_CLK] = imx_clk_gate4("enet1_ref_root_clk", "enet1_ref_post_div", base + 0x44e0, 0);
 	clks[IMX7D_ENET1_TIME_ROOT_CLK] = imx_clk_gate4("enet1_time_root_clk", "enet1_time_post_div", base + 0x44f0, 0);
@@ -860,8 +889,6 @@ static void __init imx7d_clocks_init(struct device_node *ccm_node)
 	/* 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]);
 
-	clk_set_parent(clks[IMX7D_ENET_AXI_ROOT_SRC], clks[IMX7D_PLL_ENET_MAIN_250M_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]);
 
diff --git a/drivers/clk/imx/clk.h b/drivers/clk/imx/clk.h
index a81c0385ed64..3799ff82a9b4 100644
--- a/drivers/clk/imx/clk.h
+++ b/drivers/clk/imx/clk.h
@@ -134,6 +134,15 @@ static inline struct clk *imx_clk_gate2_shared(const char *name,
 			shift, 0x3, 0, &imx_ccm_lock, share_count);
 }
 
+static inline struct clk *imx_clk_gate2_shared2(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 |
+				  CLK_OPS_PARENT_ENABLE, reg, shift, 0x3, 0,
+				  &imx_ccm_lock, share_count);
+}
+
 static inline struct clk *imx_clk_gate2_cgr(const char *name,
 		const char *parent, void __iomem *reg, u8 shift, u8 cgr_val)
 {
diff --git a/drivers/clk/loongson1/Makefile b/drivers/clk/loongson1/Makefile
new file mode 100644
index 000000000000..b7f6a16390e0
--- /dev/null
+++ b/drivers/clk/loongson1/Makefile
@@ -0,0 +1,3 @@
+obj-y				+= clk.o
+obj-$(CONFIG_LOONGSON1_LS1B)	+= clk-loongson1b.o
+obj-$(CONFIG_LOONGSON1_LS1C)	+= clk-loongson1c.o
diff --git a/drivers/clk/loongson1/clk-loongson1b.c b/drivers/clk/loongson1/clk-loongson1b.c
new file mode 100644
index 000000000000..f36a97e993c0
--- /dev/null
+++ b/drivers/clk/loongson1/clk-loongson1b.c
@@ -0,0 +1,122 @@
+/*
+ * Copyright (c) 2012-2016 Zhang, Keguang <keguang.zhang@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/clkdev.h>
+#include <linux/clk-provider.h>
+#include <linux/io.h>
+#include <linux/err.h>
+
+#include <loongson1.h>
+#include "clk.h"
+
+#define OSC		(33 * 1000000)
+#define DIV_APB		2
+
+static DEFINE_SPINLOCK(_lock);
+
+static unsigned long ls1x_pll_recalc_rate(struct clk_hw *hw,
+					  unsigned long parent_rate)
+{
+	u32 pll, rate;
+
+	pll = __raw_readl(LS1X_CLK_PLL_FREQ);
+	rate = 12 + (pll & GENMASK(5, 0));
+	rate *= OSC;
+	rate >>= 1;
+
+	return rate;
+}
+
+static const struct clk_ops ls1x_pll_clk_ops = {
+	.recalc_rate = ls1x_pll_recalc_rate,
+};
+
+static const char *const cpu_parents[] = { "cpu_clk_div", "osc_clk", };
+static const char *const ahb_parents[] = { "ahb_clk_div", "osc_clk", };
+static const char *const dc_parents[] = { "dc_clk_div", "osc_clk", };
+
+void __init ls1x_clk_init(void)
+{
+	struct clk_hw *hw;
+
+	hw = clk_hw_register_fixed_rate(NULL, "osc_clk", NULL, 0, OSC);
+	clk_hw_register_clkdev(hw, "osc_clk", NULL);
+
+	/* clock derived from 33 MHz OSC clk */
+	hw = clk_hw_register_pll(NULL, "pll_clk", "osc_clk",
+				 &ls1x_pll_clk_ops, 0);
+	clk_hw_register_clkdev(hw, "pll_clk", NULL);
+
+	/* clock derived from PLL clk */
+	/*                                 _____
+	 *         _______________________|     |
+	 * OSC ___/                       | MUX |___ CPU CLK
+	 *        \___ PLL ___ CPU DIV ___|     |
+	 *                                |_____|
+	 */
+	hw = clk_hw_register_divider(NULL, "cpu_clk_div", "pll_clk",
+				   CLK_GET_RATE_NOCACHE, LS1X_CLK_PLL_DIV,
+				   DIV_CPU_SHIFT, DIV_CPU_WIDTH,
+				   CLK_DIVIDER_ONE_BASED |
+				   CLK_DIVIDER_ROUND_CLOSEST, &_lock);
+	clk_hw_register_clkdev(hw, "cpu_clk_div", NULL);
+	hw = clk_hw_register_mux(NULL, "cpu_clk", cpu_parents,
+			       ARRAY_SIZE(cpu_parents),
+			       CLK_SET_RATE_NO_REPARENT, LS1X_CLK_PLL_DIV,
+			       BYPASS_CPU_SHIFT, BYPASS_CPU_WIDTH, 0, &_lock);
+	clk_hw_register_clkdev(hw, "cpu_clk", NULL);
+
+	/*                                 _____
+	 *         _______________________|     |
+	 * OSC ___/                       | MUX |___ DC  CLK
+	 *        \___ PLL ___ DC  DIV ___|     |
+	 *                                |_____|
+	 */
+	hw = clk_hw_register_divider(NULL, "dc_clk_div", "pll_clk",
+				   0, LS1X_CLK_PLL_DIV, DIV_DC_SHIFT,
+				   DIV_DC_WIDTH, CLK_DIVIDER_ONE_BASED, &_lock);
+	clk_hw_register_clkdev(hw, "dc_clk_div", NULL);
+	hw = clk_hw_register_mux(NULL, "dc_clk", dc_parents,
+			       ARRAY_SIZE(dc_parents),
+			       CLK_SET_RATE_NO_REPARENT, LS1X_CLK_PLL_DIV,
+			       BYPASS_DC_SHIFT, BYPASS_DC_WIDTH, 0, &_lock);
+	clk_hw_register_clkdev(hw, "dc_clk", NULL);
+
+	/*                                 _____
+	 *         _______________________|     |
+	 * OSC ___/                       | MUX |___ DDR CLK
+	 *        \___ PLL ___ DDR DIV ___|     |
+	 *                                |_____|
+	 */
+	hw = clk_hw_register_divider(NULL, "ahb_clk_div", "pll_clk",
+				   0, LS1X_CLK_PLL_DIV, DIV_DDR_SHIFT,
+				   DIV_DDR_WIDTH, CLK_DIVIDER_ONE_BASED,
+				   &_lock);
+	clk_hw_register_clkdev(hw, "ahb_clk_div", NULL);
+	hw = clk_hw_register_mux(NULL, "ahb_clk", ahb_parents,
+			       ARRAY_SIZE(ahb_parents),
+			       CLK_SET_RATE_NO_REPARENT, LS1X_CLK_PLL_DIV,
+			       BYPASS_DDR_SHIFT, BYPASS_DDR_WIDTH, 0, &_lock);
+	clk_hw_register_clkdev(hw, "ahb_clk", NULL);
+	clk_hw_register_clkdev(hw, "ls1x-dma", NULL);
+	clk_hw_register_clkdev(hw, "stmmaceth", NULL);
+
+	/* clock derived from AHB clk */
+	/* APB clk is always half of the AHB clk */
+	hw = clk_hw_register_fixed_factor(NULL, "apb_clk", "ahb_clk", 0, 1,
+					DIV_APB);
+	clk_hw_register_clkdev(hw, "apb_clk", NULL);
+	clk_hw_register_clkdev(hw, "ls1x-ac97", NULL);
+	clk_hw_register_clkdev(hw, "ls1x-i2c", NULL);
+	clk_hw_register_clkdev(hw, "ls1x-nand", NULL);
+	clk_hw_register_clkdev(hw, "ls1x-pwmtimer", NULL);
+	clk_hw_register_clkdev(hw, "ls1x-spi", NULL);
+	clk_hw_register_clkdev(hw, "ls1x-wdt", NULL);
+	clk_hw_register_clkdev(hw, "serial8250", NULL);
+}
diff --git a/drivers/clk/loongson1/clk-loongson1c.c b/drivers/clk/loongson1/clk-loongson1c.c
new file mode 100644
index 000000000000..3466f7320b40
--- /dev/null
+++ b/drivers/clk/loongson1/clk-loongson1c.c
@@ -0,0 +1,97 @@
+/*
+ * Copyright (c) 2016 Yang Ling <gnaygnil@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/clkdev.h>
+#include <linux/clk-provider.h>
+
+#include <loongson1.h>
+#include "clk.h"
+
+#define OSC		(24 * 1000000)
+#define DIV_APB		1
+
+static DEFINE_SPINLOCK(_lock);
+
+static unsigned long ls1x_pll_recalc_rate(struct clk_hw *hw,
+					  unsigned long parent_rate)
+{
+	u32 pll, rate;
+
+	pll = __raw_readl(LS1X_CLK_PLL_FREQ);
+	rate = ((pll >> 8) & 0xff) + ((pll >> 16) & 0xff);
+	rate *= OSC;
+	rate >>= 2;
+
+	return rate;
+}
+
+static const struct clk_ops ls1x_pll_clk_ops = {
+	.recalc_rate = ls1x_pll_recalc_rate,
+};
+
+static const struct clk_div_table ahb_div_table[] = {
+	[0] = { .val = 0, .div = 2 },
+	[1] = { .val = 1, .div = 4 },
+	[2] = { .val = 2, .div = 3 },
+	[3] = { .val = 3, .div = 3 },
+};
+
+void __init ls1x_clk_init(void)
+{
+	struct clk_hw *hw;
+
+	hw = clk_hw_register_fixed_rate(NULL, "osc_clk", NULL, 0, OSC);
+	clk_hw_register_clkdev(hw, "osc_clk", NULL);
+
+	/* clock derived from 24 MHz OSC clk */
+	hw = clk_hw_register_pll(NULL, "pll_clk", "osc_clk",
+				&ls1x_pll_clk_ops, 0);
+	clk_hw_register_clkdev(hw, "pll_clk", NULL);
+
+	hw = clk_hw_register_divider(NULL, "cpu_clk_div", "pll_clk",
+				   CLK_GET_RATE_NOCACHE, LS1X_CLK_PLL_DIV,
+				   DIV_CPU_SHIFT, DIV_CPU_WIDTH,
+				   CLK_DIVIDER_ONE_BASED |
+				   CLK_DIVIDER_ROUND_CLOSEST, &_lock);
+	clk_hw_register_clkdev(hw, "cpu_clk_div", NULL);
+	hw = clk_hw_register_fixed_factor(NULL, "cpu_clk", "cpu_clk_div",
+					0, 1, 1);
+	clk_hw_register_clkdev(hw, "cpu_clk", NULL);
+
+	hw = clk_hw_register_divider(NULL, "dc_clk_div", "pll_clk",
+				   0, LS1X_CLK_PLL_DIV, DIV_DC_SHIFT,
+				   DIV_DC_WIDTH, CLK_DIVIDER_ONE_BASED, &_lock);
+	clk_hw_register_clkdev(hw, "dc_clk_div", NULL);
+	hw = clk_hw_register_fixed_factor(NULL, "dc_clk", "dc_clk_div",
+					0, 1, 1);
+	clk_hw_register_clkdev(hw, "dc_clk", NULL);
+
+	hw = clk_hw_register_divider_table(NULL, "ahb_clk_div", "cpu_clk_div",
+				0, LS1X_CLK_PLL_FREQ, DIV_DDR_SHIFT,
+				DIV_DDR_WIDTH, CLK_DIVIDER_ALLOW_ZERO,
+				ahb_div_table, &_lock);
+	clk_hw_register_clkdev(hw, "ahb_clk_div", NULL);
+	hw = clk_hw_register_fixed_factor(NULL, "ahb_clk", "ahb_clk_div",
+					0, 1, 1);
+	clk_hw_register_clkdev(hw, "ahb_clk", NULL);
+	clk_hw_register_clkdev(hw, "ls1x-dma", NULL);
+	clk_hw_register_clkdev(hw, "stmmaceth", NULL);
+
+	/* clock derived from AHB clk */
+	hw = clk_hw_register_fixed_factor(NULL, "apb_clk", "ahb_clk", 0, 1,
+					DIV_APB);
+	clk_hw_register_clkdev(hw, "apb_clk", NULL);
+	clk_hw_register_clkdev(hw, "ls1x-ac97", NULL);
+	clk_hw_register_clkdev(hw, "ls1x-i2c", NULL);
+	clk_hw_register_clkdev(hw, "ls1x-nand", NULL);
+	clk_hw_register_clkdev(hw, "ls1x-pwmtimer", NULL);
+	clk_hw_register_clkdev(hw, "ls1x-spi", NULL);
+	clk_hw_register_clkdev(hw, "ls1x-wdt", NULL);
+	clk_hw_register_clkdev(hw, "serial8250", NULL);
+}
diff --git a/drivers/clk/loongson1/clk.c b/drivers/clk/loongson1/clk.c
new file mode 100644
index 000000000000..cfcfd143fccb
--- /dev/null
+++ b/drivers/clk/loongson1/clk.c
@@ -0,0 +1,43 @@
+/*
+ * Copyright (c) 2012-2016 Zhang, Keguang <keguang.zhang@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-provider.h>
+#include <linux/slab.h>
+
+struct clk_hw *__init clk_hw_register_pll(struct device *dev,
+					  const char *name,
+					  const char *parent_name,
+					  const struct clk_ops *ops,
+					  unsigned long flags)
+{
+	int ret;
+	struct clk_hw *hw;
+	struct clk_init_data init;
+
+	/* allocate the divider */
+	hw = kzalloc(sizeof(*hw), GFP_KERNEL);
+	if (!hw)
+		return ERR_PTR(-ENOMEM);
+
+	init.name = name;
+	init.ops = ops;
+	init.flags = flags | CLK_IS_BASIC;
+	init.parent_names = (parent_name ? &parent_name : NULL);
+	init.num_parents = (parent_name ? 1 : 0);
+	hw->init = &init;
+
+	/* register the clock */
+	ret = clk_hw_register(dev, hw);
+	if (ret) {
+		kfree(hw);
+		hw = ERR_PTR(ret);
+	}
+
+	return hw;
+}
diff --git a/drivers/clk/loongson1/clk.h b/drivers/clk/loongson1/clk.h
new file mode 100644
index 000000000000..085d74b5d496
--- /dev/null
+++ b/drivers/clk/loongson1/clk.h
@@ -0,0 +1,19 @@
+/*
+ * Copyright (c) 2012-2016 Zhang, Keguang <keguang.zhang@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.
+ */
+
+#ifndef __LOONGSON1_CLK_H
+#define __LOONGSON1_CLK_H
+
+struct clk_hw *clk_hw_register_pll(struct device *dev,
+				   const char *name,
+				   const char *parent_name,
+				   const struct clk_ops *ops,
+				   unsigned long flags);
+
+#endif /* __LOONGSON1_CLK_H */
diff --git a/drivers/clk/mediatek/Kconfig b/drivers/clk/mediatek/Kconfig
new file mode 100644
index 000000000000..380c372d528e
--- /dev/null
+++ b/drivers/clk/mediatek/Kconfig
@@ -0,0 +1,21 @@
+#
+# MediaTek SoC drivers
+#
+config COMMON_CLK_MEDIATEK
+	bool
+	---help---
+	  Mediatek SoCs' clock support.
+
+config COMMON_CLK_MT8135
+	bool "Clock driver for Mediatek MT8135"
+	select COMMON_CLK_MEDIATEK
+	default ARCH_MEDIATEK
+	---help---
+	  This driver supports Mediatek MT8135 clocks.
+
+config COMMON_CLK_MT8173
+	bool "Clock driver for Mediatek MT8173"
+	select COMMON_CLK_MEDIATEK
+	default ARCH_MEDIATEK
+	---help---
+	  This driver supports Mediatek MT8173 clocks.
diff --git a/drivers/clk/mediatek/Makefile b/drivers/clk/mediatek/Makefile
index 95fdfacb2ebf..32e7222e7305 100644
--- a/drivers/clk/mediatek/Makefile
+++ b/drivers/clk/mediatek/Makefile
@@ -1,4 +1,4 @@
-obj-y += clk-mtk.o clk-pll.o clk-gate.o clk-apmixed.o
+obj-$(CONFIG_COMMON_CLK_MEDIATEK) += clk-mtk.o clk-pll.o clk-gate.o clk-apmixed.o
 obj-$(CONFIG_RESET_CONTROLLER) += reset.o
-obj-y += clk-mt8135.o
-obj-y += clk-mt8173.o
+obj-$(CONFIG_COMMON_CLK_MT8135) += clk-mt8135.o
+obj-$(CONFIG_COMMON_CLK_MT8173) += clk-mt8173.o
diff --git a/drivers/clk/mediatek/clk-gate.c b/drivers/clk/mediatek/clk-gate.c
index 2a76901bf04b..d8787bf444eb 100644
--- a/drivers/clk/mediatek/clk-gate.c
+++ b/drivers/clk/mediatek/clk-gate.c
@@ -97,7 +97,7 @@ const struct clk_ops mtk_clk_gate_ops_setclr_inv = {
 	.disable	= mtk_cg_disable_inv,
 };
 
-struct clk * __init mtk_clk_register_gate(
+struct clk *mtk_clk_register_gate(
 		const char *name,
 		const char *parent_name,
 		struct regmap *regmap,
diff --git a/drivers/clk/mediatek/clk-mt8173.c b/drivers/clk/mediatek/clk-mt8173.c
index 10c986018a08..0ac3aee87726 100644
--- a/drivers/clk/mediatek/clk-mt8173.c
+++ b/drivers/clk/mediatek/clk-mt8173.c
@@ -1074,8 +1074,10 @@ static void __init mtk_apmixedsys_init(struct device_node *node)
 	}
 
 	mt8173_pll_clk_data = clk_data = mtk_alloc_clk_data(CLK_APMIXED_NR_CLK);
-	if (!clk_data)
+	if (!clk_data) {
+		iounmap(base);
 		return;
+	}
 
 	mtk_clk_register_plls(node, plls, ARRAY_SIZE(plls), clk_data);
 
diff --git a/drivers/clk/mediatek/clk-mtk.c b/drivers/clk/mediatek/clk-mtk.c
index 5ada644e6200..bb30f7063569 100644
--- a/drivers/clk/mediatek/clk-mtk.c
+++ b/drivers/clk/mediatek/clk-mtk.c
@@ -24,7 +24,7 @@
 #include "clk-mtk.h"
 #include "clk-gate.h"
 
-struct clk_onecell_data * __init mtk_alloc_clk_data(unsigned int clk_num)
+struct clk_onecell_data *mtk_alloc_clk_data(unsigned int clk_num)
 {
 	int i;
 	struct clk_onecell_data *clk_data;
@@ -49,7 +49,7 @@ err_out:
 	return NULL;
 }
 
-void __init mtk_clk_register_fixed_clks(const struct mtk_fixed_clk *clks,
+void mtk_clk_register_fixed_clks(const struct mtk_fixed_clk *clks,
 		int num, struct clk_onecell_data *clk_data)
 {
 	int i;
@@ -72,7 +72,7 @@ void __init mtk_clk_register_fixed_clks(const struct mtk_fixed_clk *clks,
 	}
 }
 
-void __init mtk_clk_register_factors(const struct mtk_fixed_factor *clks,
+void mtk_clk_register_factors(const struct mtk_fixed_factor *clks,
 		int num, struct clk_onecell_data *clk_data)
 {
 	int i;
@@ -95,7 +95,7 @@ void __init mtk_clk_register_factors(const struct mtk_fixed_factor *clks,
 	}
 }
 
-int __init mtk_clk_register_gates(struct device_node *node,
+int mtk_clk_register_gates(struct device_node *node,
 		const struct mtk_gate *clks,
 		int num, struct clk_onecell_data *clk_data)
 {
@@ -135,7 +135,7 @@ int __init mtk_clk_register_gates(struct device_node *node,
 	return 0;
 }
 
-struct clk * __init mtk_clk_register_composite(const struct mtk_composite *mc,
+struct clk *mtk_clk_register_composite(const struct mtk_composite *mc,
 		void __iomem *base, spinlock_t *lock)
 {
 	struct clk *clk;
@@ -222,7 +222,7 @@ err_out:
 	return ERR_PTR(ret);
 }
 
-void __init mtk_clk_register_composites(const struct mtk_composite *mcs,
+void mtk_clk_register_composites(const struct mtk_composite *mcs,
 		int num, void __iomem *base, spinlock_t *lock,
 		struct clk_onecell_data *clk_data)
 {
diff --git a/drivers/clk/mediatek/clk-pll.c b/drivers/clk/mediatek/clk-pll.c
index 966cab1348da..0c2deac17ce9 100644
--- a/drivers/clk/mediatek/clk-pll.c
+++ b/drivers/clk/mediatek/clk-pll.c
@@ -313,7 +313,7 @@ static struct clk *mtk_clk_register_pll(const struct mtk_pll_data *data,
 	return clk;
 }
 
-void __init mtk_clk_register_plls(struct device_node *node,
+void 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;
diff --git a/drivers/clk/meson/Makefile b/drivers/clk/meson/Makefile
index 197e40175166..349583405b7c 100644
--- a/drivers/clk/meson/Makefile
+++ b/drivers/clk/meson/Makefile
@@ -3,5 +3,5 @@
 #
 
 obj-$(CONFIG_COMMON_CLK_AMLOGIC) += clk-pll.o clk-cpu.o clk-mpll.o
-obj-$(CONFIG_COMMON_CLK_MESON8B) += meson8b-clkc.o
-obj-$(CONFIG_COMMON_CLK_GXBB)	 += gxbb.o
+obj-$(CONFIG_COMMON_CLK_MESON8B) += meson8b.o
+obj-$(CONFIG_COMMON_CLK_GXBB)	 += gxbb.o gxbb-aoclk.o
diff --git a/drivers/clk/meson/clkc.h b/drivers/clk/meson/clkc.h
index 53326c32e853..9bb70e7a7d6a 100644
--- a/drivers/clk/meson/clkc.h
+++ b/drivers/clk/meson/clkc.h
@@ -98,7 +98,7 @@ struct meson_clk_mpll {
 };
 
 #define MESON_GATE(_name, _reg, _bit)					\
-struct clk_gate gxbb_##_name = { 						\
+struct clk_gate _name = { 						\
 	.reg = (void __iomem *) _reg, 					\
 	.bit_idx = (_bit), 						\
 	.lock = &clk_lock,						\
diff --git a/drivers/clk/meson/gxbb-aoclk.c b/drivers/clk/meson/gxbb-aoclk.c
new file mode 100644
index 000000000000..b45c5fba7e35
--- /dev/null
+++ b/drivers/clk/meson/gxbb-aoclk.c
@@ -0,0 +1,191 @@
+/*
+ * 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) 2016 BayLibre, SAS.
+ * Author: Neil Armstrong <narmstrong@baylibre.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ * The full GNU General Public License is included in this distribution
+ * in the file called COPYING.
+ *
+ * BSD LICENSE
+ *
+ * Copyright (c) 2016 BayLibre, SAS.
+ * Author: Neil Armstrong <narmstrong@baylibre.com>
+ *
+ * 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.
+ */
+#include <linux/clk-provider.h>
+#include <linux/of_address.h>
+#include <linux/platform_device.h>
+#include <linux/reset-controller.h>
+#include <linux/init.h>
+#include <dt-bindings/clock/gxbb-aoclkc.h>
+#include <dt-bindings/reset/gxbb-aoclkc.h>
+
+static DEFINE_SPINLOCK(gxbb_aoclk_lock);
+
+struct gxbb_aoclk_reset_controller {
+	struct reset_controller_dev reset;
+	unsigned int *data;
+	void __iomem *base;
+};
+
+static int gxbb_aoclk_do_reset(struct reset_controller_dev *rcdev,
+			       unsigned long id)
+{
+	struct gxbb_aoclk_reset_controller *reset =
+		container_of(rcdev, struct gxbb_aoclk_reset_controller, reset);
+
+	writel(BIT(reset->data[id]), reset->base);
+
+	return 0;
+}
+
+static const struct reset_control_ops gxbb_aoclk_reset_ops = {
+	.reset = gxbb_aoclk_do_reset,
+};
+
+#define GXBB_AO_GATE(_name, _bit)					\
+static struct clk_gate _name##_ao = {					\
+	.reg = (void __iomem *)0,					\
+	.bit_idx = (_bit),						\
+	.lock = &gxbb_aoclk_lock,					\
+	.hw.init = &(struct clk_init_data) {				\
+		.name = #_name "_ao",					\
+		.ops = &clk_gate_ops,					\
+		.parent_names = (const char *[]){ "clk81" },		\
+		.num_parents = 1,					\
+		.flags = (CLK_SET_RATE_PARENT | CLK_IGNORE_UNUSED),	\
+	},								\
+}
+
+GXBB_AO_GATE(remote, 0);
+GXBB_AO_GATE(i2c_master, 1);
+GXBB_AO_GATE(i2c_slave, 2);
+GXBB_AO_GATE(uart1, 3);
+GXBB_AO_GATE(uart2, 5);
+GXBB_AO_GATE(ir_blaster, 6);
+
+static unsigned int gxbb_aoclk_reset[] = {
+	[RESET_AO_REMOTE] = 16,
+	[RESET_AO_I2C_MASTER] = 18,
+	[RESET_AO_I2C_SLAVE] = 19,
+	[RESET_AO_UART1] = 17,
+	[RESET_AO_UART2] = 22,
+	[RESET_AO_IR_BLASTER] = 23,
+};
+
+static struct clk_gate *gxbb_aoclk_gate[] = {
+	[CLKID_AO_REMOTE] = &remote_ao,
+	[CLKID_AO_I2C_MASTER] = &i2c_master_ao,
+	[CLKID_AO_I2C_SLAVE] = &i2c_slave_ao,
+	[CLKID_AO_UART1] = &uart1_ao,
+	[CLKID_AO_UART2] = &uart2_ao,
+	[CLKID_AO_IR_BLASTER] = &ir_blaster_ao,
+};
+
+static struct clk_hw_onecell_data gxbb_aoclk_onecell_data = {
+	.hws = {
+		[CLKID_AO_REMOTE] = &remote_ao.hw,
+		[CLKID_AO_I2C_MASTER] = &i2c_master_ao.hw,
+		[CLKID_AO_I2C_SLAVE] = &i2c_slave_ao.hw,
+		[CLKID_AO_UART1] = &uart1_ao.hw,
+		[CLKID_AO_UART2] = &uart2_ao.hw,
+		[CLKID_AO_IR_BLASTER] = &ir_blaster_ao.hw,
+	},
+	.num = ARRAY_SIZE(gxbb_aoclk_gate),
+};
+
+static int gxbb_aoclkc_probe(struct platform_device *pdev)
+{
+	struct resource *res;
+	void __iomem *base;
+	int ret, clkid;
+	struct device *dev = &pdev->dev;
+	struct gxbb_aoclk_reset_controller *rstc;
+
+	rstc = devm_kzalloc(dev, sizeof(*rstc), GFP_KERNEL);
+	if (!rstc)
+		return -ENOMEM;
+
+	/* Generic clocks */
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	base = devm_ioremap_resource(dev, res);
+	if (IS_ERR(base))
+		return PTR_ERR(base);
+
+	/* Reset Controller */
+	rstc->base = base;
+	rstc->data = gxbb_aoclk_reset;
+	rstc->reset.ops = &gxbb_aoclk_reset_ops;
+	rstc->reset.nr_resets = ARRAY_SIZE(gxbb_aoclk_reset);
+	rstc->reset.of_node = dev->of_node;
+	ret = devm_reset_controller_register(dev, &rstc->reset);
+
+	/*
+	 * Populate base address and register all clks
+	 */
+	for (clkid = 0; clkid < gxbb_aoclk_onecell_data.num; clkid++) {
+		gxbb_aoclk_gate[clkid]->reg = base;
+
+		ret = devm_clk_hw_register(dev,
+					gxbb_aoclk_onecell_data.hws[clkid]);
+		if (ret)
+			return ret;
+	}
+
+	return of_clk_add_hw_provider(dev->of_node, of_clk_hw_onecell_get,
+			&gxbb_aoclk_onecell_data);
+}
+
+static const struct of_device_id gxbb_aoclkc_match_table[] = {
+	{ .compatible = "amlogic,gxbb-aoclkc" },
+	{ }
+};
+
+static struct platform_driver gxbb_aoclkc_driver = {
+	.probe		= gxbb_aoclkc_probe,
+	.driver		= {
+		.name	= "gxbb-aoclkc",
+		.of_match_table = gxbb_aoclkc_match_table,
+	},
+};
+builtin_platform_driver(gxbb_aoclkc_driver);
diff --git a/drivers/clk/meson/gxbb.c b/drivers/clk/meson/gxbb.c
index a4c6684b3019..9d9af446bafc 100644
--- a/drivers/clk/meson/gxbb.c
+++ b/drivers/clk/meson/gxbb.c
@@ -565,90 +565,93 @@ static struct clk_gate gxbb_clk81 = {
 };
 
 /* Everything Else (EE) domain gates */
-static MESON_GATE(ddr, HHI_GCLK_MPEG0, 0);
-static MESON_GATE(dos, HHI_GCLK_MPEG0, 1);
-static MESON_GATE(isa, HHI_GCLK_MPEG0, 5);
-static MESON_GATE(pl301, HHI_GCLK_MPEG0, 6);
-static MESON_GATE(periphs, HHI_GCLK_MPEG0, 7);
-static MESON_GATE(spicc, HHI_GCLK_MPEG0, 8);
-static MESON_GATE(i2c, HHI_GCLK_MPEG0, 9);
-static MESON_GATE(sar_adc, HHI_GCLK_MPEG0, 10);
-static MESON_GATE(smart_card, HHI_GCLK_MPEG0, 11);
-static MESON_GATE(rng0, HHI_GCLK_MPEG0, 12);
-static MESON_GATE(uart0, HHI_GCLK_MPEG0, 13);
-static MESON_GATE(sdhc, HHI_GCLK_MPEG0, 14);
-static MESON_GATE(stream, HHI_GCLK_MPEG0, 15);
-static MESON_GATE(async_fifo, HHI_GCLK_MPEG0, 16);
-static MESON_GATE(sdio, HHI_GCLK_MPEG0, 17);
-static MESON_GATE(abuf, HHI_GCLK_MPEG0, 18);
-static MESON_GATE(hiu_iface, HHI_GCLK_MPEG0, 19);
-static MESON_GATE(assist_misc, HHI_GCLK_MPEG0, 23);
-static MESON_GATE(spi, HHI_GCLK_MPEG0, 30);
-
-static MESON_GATE(i2s_spdif, HHI_GCLK_MPEG1, 2);
-static MESON_GATE(eth, HHI_GCLK_MPEG1, 3);
-static MESON_GATE(demux, HHI_GCLK_MPEG1, 4);
-static MESON_GATE(aiu_glue, HHI_GCLK_MPEG1, 6);
-static MESON_GATE(iec958, HHI_GCLK_MPEG1, 7);
-static MESON_GATE(i2s_out, HHI_GCLK_MPEG1, 8);
-static MESON_GATE(amclk, HHI_GCLK_MPEG1, 9);
-static MESON_GATE(aififo2, HHI_GCLK_MPEG1, 10);
-static MESON_GATE(mixer, HHI_GCLK_MPEG1, 11);
-static MESON_GATE(mixer_iface, HHI_GCLK_MPEG1, 12);
-static MESON_GATE(adc, HHI_GCLK_MPEG1, 13);
-static MESON_GATE(blkmv, HHI_GCLK_MPEG1, 14);
-static MESON_GATE(aiu, HHI_GCLK_MPEG1, 15);
-static MESON_GATE(uart1, HHI_GCLK_MPEG1, 16);
-static MESON_GATE(g2d, HHI_GCLK_MPEG1, 20);
-static MESON_GATE(usb0, HHI_GCLK_MPEG1, 21);
-static MESON_GATE(usb1, HHI_GCLK_MPEG1, 22);
-static MESON_GATE(reset, HHI_GCLK_MPEG1, 23);
-static MESON_GATE(nand, HHI_GCLK_MPEG1, 24);
-static MESON_GATE(dos_parser, HHI_GCLK_MPEG1, 25);
-static MESON_GATE(usb, HHI_GCLK_MPEG1, 26);
-static MESON_GATE(vdin1, HHI_GCLK_MPEG1, 28);
-static MESON_GATE(ahb_arb0, HHI_GCLK_MPEG1, 29);
-static MESON_GATE(efuse, HHI_GCLK_MPEG1, 30);
-static MESON_GATE(boot_rom, HHI_GCLK_MPEG1, 31);
-
-static MESON_GATE(ahb_data_bus, HHI_GCLK_MPEG2, 1);
-static MESON_GATE(ahb_ctrl_bus, HHI_GCLK_MPEG2, 2);
-static MESON_GATE(hdmi_intr_sync, HHI_GCLK_MPEG2, 3);
-static MESON_GATE(hdmi_pclk, HHI_GCLK_MPEG2, 4);
-static MESON_GATE(usb1_ddr_bridge, HHI_GCLK_MPEG2, 8);
-static MESON_GATE(usb0_ddr_bridge, HHI_GCLK_MPEG2, 9);
-static MESON_GATE(mmc_pclk, HHI_GCLK_MPEG2, 11);
-static MESON_GATE(dvin, HHI_GCLK_MPEG2, 12);
-static MESON_GATE(uart2, HHI_GCLK_MPEG2, 15);
-static MESON_GATE(sana, HHI_GCLK_MPEG2, 22);
-static MESON_GATE(vpu_intr, HHI_GCLK_MPEG2, 25);
-static MESON_GATE(sec_ahb_ahb3_bridge, HHI_GCLK_MPEG2, 26);
-static MESON_GATE(clk81_a53, HHI_GCLK_MPEG2, 29);
-
-static MESON_GATE(vclk2_venci0, HHI_GCLK_OTHER, 1);
-static MESON_GATE(vclk2_venci1, HHI_GCLK_OTHER, 2);
-static MESON_GATE(vclk2_vencp0, HHI_GCLK_OTHER, 3);
-static MESON_GATE(vclk2_vencp1, HHI_GCLK_OTHER, 4);
-static MESON_GATE(gclk_venci_int0, HHI_GCLK_OTHER, 8);
-static MESON_GATE(gclk_vencp_int, HHI_GCLK_OTHER, 9);
-static MESON_GATE(dac_clk, HHI_GCLK_OTHER, 10);
-static MESON_GATE(aoclk_gate, HHI_GCLK_OTHER, 14);
-static MESON_GATE(iec958_gate, HHI_GCLK_OTHER, 16);
-static MESON_GATE(enc480p, HHI_GCLK_OTHER, 20);
-static MESON_GATE(rng1, HHI_GCLK_OTHER, 21);
-static MESON_GATE(gclk_venci_int1, HHI_GCLK_OTHER, 22);
-static MESON_GATE(vclk2_venclmcc, HHI_GCLK_OTHER, 24);
-static MESON_GATE(vclk2_vencl, HHI_GCLK_OTHER, 25);
-static MESON_GATE(vclk_other, HHI_GCLK_OTHER, 26);
-static MESON_GATE(edp, HHI_GCLK_OTHER, 31);
+static MESON_GATE(gxbb_ddr, HHI_GCLK_MPEG0, 0);
+static MESON_GATE(gxbb_dos, HHI_GCLK_MPEG0, 1);
+static MESON_GATE(gxbb_isa, HHI_GCLK_MPEG0, 5);
+static MESON_GATE(gxbb_pl301, HHI_GCLK_MPEG0, 6);
+static MESON_GATE(gxbb_periphs, HHI_GCLK_MPEG0, 7);
+static MESON_GATE(gxbb_spicc, HHI_GCLK_MPEG0, 8);
+static MESON_GATE(gxbb_i2c, HHI_GCLK_MPEG0, 9);
+static MESON_GATE(gxbb_sar_adc, HHI_GCLK_MPEG0, 10);
+static MESON_GATE(gxbb_smart_card, HHI_GCLK_MPEG0, 11);
+static MESON_GATE(gxbb_rng0, HHI_GCLK_MPEG0, 12);
+static MESON_GATE(gxbb_uart0, HHI_GCLK_MPEG0, 13);
+static MESON_GATE(gxbb_sdhc, HHI_GCLK_MPEG0, 14);
+static MESON_GATE(gxbb_stream, HHI_GCLK_MPEG0, 15);
+static MESON_GATE(gxbb_async_fifo, HHI_GCLK_MPEG0, 16);
+static MESON_GATE(gxbb_sdio, HHI_GCLK_MPEG0, 17);
+static MESON_GATE(gxbb_abuf, HHI_GCLK_MPEG0, 18);
+static MESON_GATE(gxbb_hiu_iface, HHI_GCLK_MPEG0, 19);
+static MESON_GATE(gxbb_assist_misc, HHI_GCLK_MPEG0, 23);
+static MESON_GATE(gxbb_emmc_a, HHI_GCLK_MPEG0, 24);
+static MESON_GATE(gxbb_emmc_b, HHI_GCLK_MPEG0, 25);
+static MESON_GATE(gxbb_emmc_c, HHI_GCLK_MPEG0, 26);
+static MESON_GATE(gxbb_spi, HHI_GCLK_MPEG0, 30);
+
+static MESON_GATE(gxbb_i2s_spdif, HHI_GCLK_MPEG1, 2);
+static MESON_GATE(gxbb_eth, HHI_GCLK_MPEG1, 3);
+static MESON_GATE(gxbb_demux, HHI_GCLK_MPEG1, 4);
+static MESON_GATE(gxbb_aiu_glue, HHI_GCLK_MPEG1, 6);
+static MESON_GATE(gxbb_iec958, HHI_GCLK_MPEG1, 7);
+static MESON_GATE(gxbb_i2s_out, HHI_GCLK_MPEG1, 8);
+static MESON_GATE(gxbb_amclk, HHI_GCLK_MPEG1, 9);
+static MESON_GATE(gxbb_aififo2, HHI_GCLK_MPEG1, 10);
+static MESON_GATE(gxbb_mixer, HHI_GCLK_MPEG1, 11);
+static MESON_GATE(gxbb_mixer_iface, HHI_GCLK_MPEG1, 12);
+static MESON_GATE(gxbb_adc, HHI_GCLK_MPEG1, 13);
+static MESON_GATE(gxbb_blkmv, HHI_GCLK_MPEG1, 14);
+static MESON_GATE(gxbb_aiu, HHI_GCLK_MPEG1, 15);
+static MESON_GATE(gxbb_uart1, HHI_GCLK_MPEG1, 16);
+static MESON_GATE(gxbb_g2d, HHI_GCLK_MPEG1, 20);
+static MESON_GATE(gxbb_usb0, HHI_GCLK_MPEG1, 21);
+static MESON_GATE(gxbb_usb1, HHI_GCLK_MPEG1, 22);
+static MESON_GATE(gxbb_reset, HHI_GCLK_MPEG1, 23);
+static MESON_GATE(gxbb_nand, HHI_GCLK_MPEG1, 24);
+static MESON_GATE(gxbb_dos_parser, HHI_GCLK_MPEG1, 25);
+static MESON_GATE(gxbb_usb, HHI_GCLK_MPEG1, 26);
+static MESON_GATE(gxbb_vdin1, HHI_GCLK_MPEG1, 28);
+static MESON_GATE(gxbb_ahb_arb0, HHI_GCLK_MPEG1, 29);
+static MESON_GATE(gxbb_efuse, HHI_GCLK_MPEG1, 30);
+static MESON_GATE(gxbb_boot_rom, HHI_GCLK_MPEG1, 31);
+
+static MESON_GATE(gxbb_ahb_data_bus, HHI_GCLK_MPEG2, 1);
+static MESON_GATE(gxbb_ahb_ctrl_bus, HHI_GCLK_MPEG2, 2);
+static MESON_GATE(gxbb_hdmi_intr_sync, HHI_GCLK_MPEG2, 3);
+static MESON_GATE(gxbb_hdmi_pclk, HHI_GCLK_MPEG2, 4);
+static MESON_GATE(gxbb_usb1_ddr_bridge, HHI_GCLK_MPEG2, 8);
+static MESON_GATE(gxbb_usb0_ddr_bridge, HHI_GCLK_MPEG2, 9);
+static MESON_GATE(gxbb_mmc_pclk, HHI_GCLK_MPEG2, 11);
+static MESON_GATE(gxbb_dvin, HHI_GCLK_MPEG2, 12);
+static MESON_GATE(gxbb_uart2, HHI_GCLK_MPEG2, 15);
+static MESON_GATE(gxbb_sana, HHI_GCLK_MPEG2, 22);
+static MESON_GATE(gxbb_vpu_intr, HHI_GCLK_MPEG2, 25);
+static MESON_GATE(gxbb_sec_ahb_ahb3_bridge, HHI_GCLK_MPEG2, 26);
+static MESON_GATE(gxbb_clk81_a53, HHI_GCLK_MPEG2, 29);
+
+static MESON_GATE(gxbb_vclk2_venci0, HHI_GCLK_OTHER, 1);
+static MESON_GATE(gxbb_vclk2_venci1, HHI_GCLK_OTHER, 2);
+static MESON_GATE(gxbb_vclk2_vencp0, HHI_GCLK_OTHER, 3);
+static MESON_GATE(gxbb_vclk2_vencp1, HHI_GCLK_OTHER, 4);
+static MESON_GATE(gxbb_gclk_venci_int0, HHI_GCLK_OTHER, 8);
+static MESON_GATE(gxbb_gclk_vencp_int, HHI_GCLK_OTHER, 9);
+static MESON_GATE(gxbb_dac_clk, HHI_GCLK_OTHER, 10);
+static MESON_GATE(gxbb_aoclk_gate, HHI_GCLK_OTHER, 14);
+static MESON_GATE(gxbb_iec958_gate, HHI_GCLK_OTHER, 16);
+static MESON_GATE(gxbb_enc480p, HHI_GCLK_OTHER, 20);
+static MESON_GATE(gxbb_rng1, HHI_GCLK_OTHER, 21);
+static MESON_GATE(gxbb_gclk_venci_int1, HHI_GCLK_OTHER, 22);
+static MESON_GATE(gxbb_vclk2_venclmcc, HHI_GCLK_OTHER, 24);
+static MESON_GATE(gxbb_vclk2_vencl, HHI_GCLK_OTHER, 25);
+static MESON_GATE(gxbb_vclk_other, HHI_GCLK_OTHER, 26);
+static MESON_GATE(gxbb_edp, HHI_GCLK_OTHER, 31);
 
 /* Always On (AO) domain gates */
 
-static MESON_GATE(ao_media_cpu, HHI_GCLK_AO, 0);
-static MESON_GATE(ao_ahb_sram, HHI_GCLK_AO, 1);
-static MESON_GATE(ao_ahb_bus, HHI_GCLK_AO, 2);
-static MESON_GATE(ao_iface, HHI_GCLK_AO, 3);
-static MESON_GATE(ao_i2c, HHI_GCLK_AO, 4);
+static MESON_GATE(gxbb_ao_media_cpu, HHI_GCLK_AO, 0);
+static MESON_GATE(gxbb_ao_ahb_sram, HHI_GCLK_AO, 1);
+static MESON_GATE(gxbb_ao_ahb_bus, HHI_GCLK_AO, 2);
+static MESON_GATE(gxbb_ao_iface, HHI_GCLK_AO, 3);
+static MESON_GATE(gxbb_ao_i2c, HHI_GCLK_AO, 4);
 
 /* Array of all clocks provided by this provider */
 
@@ -748,6 +751,9 @@ static struct clk_hw_onecell_data gxbb_hw_onecell_data = {
 		[CLKID_AO_AHB_BUS]	    = &gxbb_ao_ahb_bus.hw,
 		[CLKID_AO_IFACE]	    = &gxbb_ao_iface.hw,
 		[CLKID_AO_I2C]		    = &gxbb_ao_i2c.hw,
+		[CLKID_SD_EMMC_A]	    = &gxbb_emmc_a.hw,
+		[CLKID_SD_EMMC_B]	    = &gxbb_emmc_b.hw,
+		[CLKID_SD_EMMC_C]	    = &gxbb_emmc_c.hw,
 	},
 	.num = NR_CLKS,
 };
@@ -847,6 +853,9 @@ static struct clk_gate *gxbb_clk_gates[] = {
 	&gxbb_ao_ahb_bus,
 	&gxbb_ao_iface,
 	&gxbb_ao_i2c,
+	&gxbb_emmc_a,
+	&gxbb_emmc_b,
+	&gxbb_emmc_c,
 };
 
 static int gxbb_clkc_probe(struct platform_device *pdev)
@@ -937,8 +946,4 @@ static struct platform_driver gxbb_driver = {
 	},
 };
 
-static int __init gxbb_clkc_init(void)
-{
-	return platform_driver_register(&gxbb_driver);
-}
-device_initcall(gxbb_clkc_init);
+builtin_platform_driver(gxbb_driver);
diff --git a/drivers/clk/meson/gxbb.h b/drivers/clk/meson/gxbb.h
index a2adf3448b59..0252939ba58f 100644
--- a/drivers/clk/meson/gxbb.h
+++ b/drivers/clk/meson/gxbb.h
@@ -170,11 +170,11 @@
  */
 #define CLKID_SYS_PLL		  0
 /* CLKID_CPUCLK */
-#define CLKID_HDMI_PLL		  2
+/* CLKID_HDMI_PLL */
 #define CLKID_FIXED_PLL		  3
-#define CLKID_FCLK_DIV2		  4
-#define CLKID_FCLK_DIV3		  5
-#define CLKID_FCLK_DIV4		  6
+/* CLKID_FCLK_DIV2 */
+/* CLKID_FCLK_DIV3 */
+/* CLKID_FCLK_DIV4 */
 #define CLKID_FCLK_DIV5		  7
 #define CLKID_FCLK_DIV7		  8
 #define CLKID_GP0_PLL		  9
@@ -183,14 +183,14 @@
 /* CLKID_CLK81 */
 #define CLKID_MPLL0		  13
 #define CLKID_MPLL1		  14
-#define CLKID_MPLL2		  15
+/* CLKID_MPLL2 */
 #define CLKID_DDR		  16
 #define CLKID_DOS		  17
 #define CLKID_ISA		  18
 #define CLKID_PL301		  19
 #define CLKID_PERIPHS		  20
 #define CLKID_SPICC		  21
-#define CLKID_I2C		  22
+/* CLKID_I2C */
 #define CLKID_SAR_ADC		  23
 #define CLKID_SMART_CARD	  24
 #define CLKID_RNG0		  25
@@ -202,7 +202,7 @@
 #define CLKID_ABUF		  31
 #define CLKID_HIU_IFACE		  32
 #define CLKID_ASSIST_MISC	  33
-#define CLKID_SPI		  34
+/* CLKID_SPI */
 #define CLKID_I2S_SPDIF		  35
 #define CLKID_ETH		  36
 #define CLKID_DEMUX		  37
@@ -218,12 +218,12 @@
 #define CLKID_AIU		  47
 #define CLKID_UART1		  48
 #define CLKID_G2D		  49
-#define CLKID_USB0		  50
-#define CLKID_USB1		  51
+/* CLKID_USB0 */
+/* CLKID_USB1 */
 #define CLKID_RESET		  52
 #define CLKID_NAND		  53
 #define CLKID_DOS_PARSER	  54
-#define CLKID_USB		  55
+/* CLKID_USB */
 #define CLKID_VDIN1		  56
 #define CLKID_AHB_ARB0		  57
 #define CLKID_EFUSE		  58
@@ -232,8 +232,8 @@
 #define CLKID_AHB_CTRL_BUS	  61
 #define CLKID_HDMI_INTR_SYNC	  62
 #define CLKID_HDMI_PCLK		  63
-#define CLKID_USB1_DDR_BRIDGE	  64
-#define CLKID_USB0_DDR_BRIDGE	  65
+/* CLKID_USB1_DDR_BRIDGE */
+/* CLKID_USB0_DDR_BRIDGE */
 #define CLKID_MMC_PCLK		  66
 #define CLKID_DVIN		  67
 #define CLKID_UART2		  68
@@ -261,9 +261,12 @@
 #define CLKID_AO_AHB_SRAM	  90
 #define CLKID_AO_AHB_BUS	  91
 #define CLKID_AO_IFACE		  92
-#define CLKID_AO_I2C		  93
+/* CLKID_AO_I2C */
+/* CLKID_SD_EMMC_A */
+/* CLKID_SD_EMMC_B */
+/* CLKID_SD_EMMC_C */
 
-#define NR_CLKS			  94
+#define NR_CLKS			  97
 
 /* include the CLKIDs that have been made part of the stable DT binding */
 #include <dt-bindings/clock/gxbb-clkc.h>
diff --git a/drivers/clk/meson/meson8b-clkc.c b/drivers/clk/meson/meson8b.c
index 4c9413cdf373..3f1be46cbb33 100644
--- a/drivers/clk/meson/meson8b-clkc.c
+++ b/drivers/clk/meson/meson8b.c
@@ -23,27 +23,11 @@
 #include <linux/clk.h>
 #include <linux/clk-provider.h>
 #include <linux/of_address.h>
-#include <dt-bindings/clock/meson8b-clkc.h>
 #include <linux/platform_device.h>
 #include <linux/init.h>
 
 #include "clkc.h"
-
-/*
- * Clock controller register offsets
- *
- * Register offsets from the HardKernel[0] data sheet are listed in comment
- * blocks below. Those offsets must be multiplied by 4 before adding them to
- * the base address to get the right value
- *
- * [0] http://dn.odroid.com/S805/Datasheet/S805_Datasheet%20V0.8%2020150126.pdf
- */
-#define MESON8B_REG_SYS_CPU_CNTL1	0x015c /* 0x57 offset in data sheet */
-#define MESON8B_REG_HHI_MPEG		0x0174 /* 0x5d offset in data sheet */
-#define MESON8B_REG_MALI		0x01b0 /* 0x6c offset in data sheet */
-#define MESON8B_REG_PLL_FIXED		0x0280
-#define MESON8B_REG_PLL_SYS		0x0300
-#define MESON8B_REG_PLL_VID		0x0320
+#include "meson8b.h"
 
 static DEFINE_SPINLOCK(clk_lock);
 
@@ -128,17 +112,17 @@ static struct clk_fixed_rate meson8b_xtal = {
 
 static struct meson_clk_pll meson8b_fixed_pll = {
 	.m = {
-		.reg_off = MESON8B_REG_PLL_FIXED,
+		.reg_off = HHI_MPLL_CNTL,
 		.shift   = 0,
 		.width   = 9,
 	},
 	.n = {
-		.reg_off = MESON8B_REG_PLL_FIXED,
+		.reg_off = HHI_MPLL_CNTL,
 		.shift   = 9,
 		.width   = 5,
 	},
 	.od = {
-		.reg_off = MESON8B_REG_PLL_FIXED,
+		.reg_off = HHI_MPLL_CNTL,
 		.shift   = 16,
 		.width   = 2,
 	},
@@ -154,17 +138,17 @@ static struct meson_clk_pll meson8b_fixed_pll = {
 
 static struct meson_clk_pll meson8b_vid_pll = {
 	.m = {
-		.reg_off = MESON8B_REG_PLL_VID,
+		.reg_off = HHI_VID_PLL_CNTL,
 		.shift   = 0,
 		.width   = 9,
 	},
 	.n = {
-		.reg_off = MESON8B_REG_PLL_VID,
+		.reg_off = HHI_VID_PLL_CNTL,
 		.shift   = 9,
 		.width   = 5,
 	},
 	.od = {
-		.reg_off = MESON8B_REG_PLL_VID,
+		.reg_off = HHI_VID_PLL_CNTL,
 		.shift   = 16,
 		.width   = 2,
 	},
@@ -180,17 +164,17 @@ static struct meson_clk_pll meson8b_vid_pll = {
 
 static struct meson_clk_pll meson8b_sys_pll = {
 	.m = {
-		.reg_off = MESON8B_REG_PLL_SYS,
+		.reg_off = HHI_SYS_PLL_CNTL,
 		.shift   = 0,
 		.width   = 9,
 	},
 	.n = {
-		.reg_off = MESON8B_REG_PLL_SYS,
+		.reg_off = HHI_SYS_PLL_CNTL,
 		.shift   = 9,
 		.width   = 5,
 	},
 	.od = {
-		.reg_off = MESON8B_REG_PLL_SYS,
+		.reg_off = HHI_SYS_PLL_CNTL,
 		.shift   = 16,
 		.width   = 2,
 	},
@@ -267,7 +251,7 @@ static struct clk_fixed_factor meson8b_fclk_div7 = {
  * forthcoming coordinated clock rates feature
  */
 static struct meson_clk_cpu meson8b_cpu_clk = {
-	.reg_off = MESON8B_REG_SYS_CPU_CNTL1,
+	.reg_off = HHI_SYS_CPU_CLK_CNTL1,
 	.div_table = cpu_div_table,
 	.clk_nb.notifier_call = meson_clk_cpu_notifier_cb,
 	.hw.init = &(struct clk_init_data){
@@ -281,7 +265,7 @@ static struct meson_clk_cpu meson8b_cpu_clk = {
 static u32 mux_table_clk81[]	= { 6, 5, 7 };
 
 struct clk_mux meson8b_mpeg_clk_sel = {
-	.reg = (void *)MESON8B_REG_HHI_MPEG,
+	.reg = (void *)HHI_MPEG_CLK_CNTL,
 	.mask = 0x7,
 	.shift = 12,
 	.flags = CLK_MUX_READ_ONLY,
@@ -303,7 +287,7 @@ struct clk_mux meson8b_mpeg_clk_sel = {
 };
 
 struct clk_divider meson8b_mpeg_clk_div = {
-	.reg = (void *)MESON8B_REG_HHI_MPEG,
+	.reg = (void *)HHI_MPEG_CLK_CNTL,
 	.shift = 0,
 	.width = 7,
 	.lock = &clk_lock,
@@ -317,7 +301,7 @@ struct clk_divider meson8b_mpeg_clk_div = {
 };
 
 struct clk_gate meson8b_clk81 = {
-	.reg = (void *)MESON8B_REG_HHI_MPEG,
+	.reg = (void *)HHI_MPEG_CLK_CNTL,
 	.bit_idx = 7,
 	.lock = &clk_lock,
 	.hw.init = &(struct clk_init_data){
@@ -329,6 +313,92 @@ struct clk_gate meson8b_clk81 = {
 	},
 };
 
+/* Everything Else (EE) domain gates */
+
+static MESON_GATE(meson8b_ddr, HHI_GCLK_MPEG0, 0);
+static MESON_GATE(meson8b_dos, HHI_GCLK_MPEG0, 1);
+static MESON_GATE(meson8b_isa, HHI_GCLK_MPEG0, 5);
+static MESON_GATE(meson8b_pl301, HHI_GCLK_MPEG0, 6);
+static MESON_GATE(meson8b_periphs, HHI_GCLK_MPEG0, 7);
+static MESON_GATE(meson8b_spicc, HHI_GCLK_MPEG0, 8);
+static MESON_GATE(meson8b_i2c, HHI_GCLK_MPEG0, 9);
+static MESON_GATE(meson8b_sar_adc, HHI_GCLK_MPEG0, 10);
+static MESON_GATE(meson8b_smart_card, HHI_GCLK_MPEG0, 11);
+static MESON_GATE(meson8b_rng0, HHI_GCLK_MPEG0, 12);
+static MESON_GATE(meson8b_uart0, HHI_GCLK_MPEG0, 13);
+static MESON_GATE(meson8b_sdhc, HHI_GCLK_MPEG0, 14);
+static MESON_GATE(meson8b_stream, HHI_GCLK_MPEG0, 15);
+static MESON_GATE(meson8b_async_fifo, HHI_GCLK_MPEG0, 16);
+static MESON_GATE(meson8b_sdio, HHI_GCLK_MPEG0, 17);
+static MESON_GATE(meson8b_abuf, HHI_GCLK_MPEG0, 18);
+static MESON_GATE(meson8b_hiu_iface, HHI_GCLK_MPEG0, 19);
+static MESON_GATE(meson8b_assist_misc, HHI_GCLK_MPEG0, 23);
+static MESON_GATE(meson8b_spi, HHI_GCLK_MPEG0, 30);
+
+static MESON_GATE(meson8b_i2s_spdif, HHI_GCLK_MPEG1, 2);
+static MESON_GATE(meson8b_eth, HHI_GCLK_MPEG1, 3);
+static MESON_GATE(meson8b_demux, HHI_GCLK_MPEG1, 4);
+static MESON_GATE(meson8b_aiu_glue, HHI_GCLK_MPEG1, 6);
+static MESON_GATE(meson8b_iec958, HHI_GCLK_MPEG1, 7);
+static MESON_GATE(meson8b_i2s_out, HHI_GCLK_MPEG1, 8);
+static MESON_GATE(meson8b_amclk, HHI_GCLK_MPEG1, 9);
+static MESON_GATE(meson8b_aififo2, HHI_GCLK_MPEG1, 10);
+static MESON_GATE(meson8b_mixer, HHI_GCLK_MPEG1, 11);
+static MESON_GATE(meson8b_mixer_iface, HHI_GCLK_MPEG1, 12);
+static MESON_GATE(meson8b_adc, HHI_GCLK_MPEG1, 13);
+static MESON_GATE(meson8b_blkmv, HHI_GCLK_MPEG1, 14);
+static MESON_GATE(meson8b_aiu, HHI_GCLK_MPEG1, 15);
+static MESON_GATE(meson8b_uart1, HHI_GCLK_MPEG1, 16);
+static MESON_GATE(meson8b_g2d, HHI_GCLK_MPEG1, 20);
+static MESON_GATE(meson8b_usb0, HHI_GCLK_MPEG1, 21);
+static MESON_GATE(meson8b_usb1, HHI_GCLK_MPEG1, 22);
+static MESON_GATE(meson8b_reset, HHI_GCLK_MPEG1, 23);
+static MESON_GATE(meson8b_nand, HHI_GCLK_MPEG1, 24);
+static MESON_GATE(meson8b_dos_parser, HHI_GCLK_MPEG1, 25);
+static MESON_GATE(meson8b_usb, HHI_GCLK_MPEG1, 26);
+static MESON_GATE(meson8b_vdin1, HHI_GCLK_MPEG1, 28);
+static MESON_GATE(meson8b_ahb_arb0, HHI_GCLK_MPEG1, 29);
+static MESON_GATE(meson8b_efuse, HHI_GCLK_MPEG1, 30);
+static MESON_GATE(meson8b_boot_rom, HHI_GCLK_MPEG1, 31);
+
+static MESON_GATE(meson8b_ahb_data_bus, HHI_GCLK_MPEG2, 1);
+static MESON_GATE(meson8b_ahb_ctrl_bus, HHI_GCLK_MPEG2, 2);
+static MESON_GATE(meson8b_hdmi_intr_sync, HHI_GCLK_MPEG2, 3);
+static MESON_GATE(meson8b_hdmi_pclk, HHI_GCLK_MPEG2, 4);
+static MESON_GATE(meson8b_usb1_ddr_bridge, HHI_GCLK_MPEG2, 8);
+static MESON_GATE(meson8b_usb0_ddr_bridge, HHI_GCLK_MPEG2, 9);
+static MESON_GATE(meson8b_mmc_pclk, HHI_GCLK_MPEG2, 11);
+static MESON_GATE(meson8b_dvin, HHI_GCLK_MPEG2, 12);
+static MESON_GATE(meson8b_uart2, HHI_GCLK_MPEG2, 15);
+static MESON_GATE(meson8b_sana, HHI_GCLK_MPEG2, 22);
+static MESON_GATE(meson8b_vpu_intr, HHI_GCLK_MPEG2, 25);
+static MESON_GATE(meson8b_sec_ahb_ahb3_bridge, HHI_GCLK_MPEG2, 26);
+static MESON_GATE(meson8b_clk81_a9, HHI_GCLK_MPEG2, 29);
+
+static MESON_GATE(meson8b_vclk2_venci0, HHI_GCLK_OTHER, 1);
+static MESON_GATE(meson8b_vclk2_venci1, HHI_GCLK_OTHER, 2);
+static MESON_GATE(meson8b_vclk2_vencp0, HHI_GCLK_OTHER, 3);
+static MESON_GATE(meson8b_vclk2_vencp1, HHI_GCLK_OTHER, 4);
+static MESON_GATE(meson8b_gclk_venci_int, HHI_GCLK_OTHER, 8);
+static MESON_GATE(meson8b_gclk_vencp_int, HHI_GCLK_OTHER, 9);
+static MESON_GATE(meson8b_dac_clk, HHI_GCLK_OTHER, 10);
+static MESON_GATE(meson8b_aoclk_gate, HHI_GCLK_OTHER, 14);
+static MESON_GATE(meson8b_iec958_gate, HHI_GCLK_OTHER, 16);
+static MESON_GATE(meson8b_enc480p, HHI_GCLK_OTHER, 20);
+static MESON_GATE(meson8b_rng1, HHI_GCLK_OTHER, 21);
+static MESON_GATE(meson8b_gclk_vencl_int, HHI_GCLK_OTHER, 22);
+static MESON_GATE(meson8b_vclk2_venclmcc, HHI_GCLK_OTHER, 24);
+static MESON_GATE(meson8b_vclk2_vencl, HHI_GCLK_OTHER, 25);
+static MESON_GATE(meson8b_vclk2_other, HHI_GCLK_OTHER, 26);
+static MESON_GATE(meson8b_edp, HHI_GCLK_OTHER, 31);
+
+/* Always On (AO) domain gates */
+
+static MESON_GATE(meson8b_ao_media_cpu, HHI_GCLK_AO, 0);
+static MESON_GATE(meson8b_ao_ahb_sram, HHI_GCLK_AO, 1);
+static MESON_GATE(meson8b_ao_ahb_bus, HHI_GCLK_AO, 2);
+static MESON_GATE(meson8b_ao_iface, HHI_GCLK_AO, 3);
+
 static struct clk_hw_onecell_data meson8b_hw_onecell_data = {
 	.hws = {
 		[CLKID_XTAL] = &meson8b_xtal.hw,
@@ -344,6 +414,83 @@ static struct clk_hw_onecell_data meson8b_hw_onecell_data = {
 		[CLKID_MPEG_SEL] = &meson8b_mpeg_clk_sel.hw,
 		[CLKID_MPEG_DIV] = &meson8b_mpeg_clk_div.hw,
 		[CLKID_CLK81] = &meson8b_clk81.hw,
+		[CLKID_DDR]		    = &meson8b_ddr.hw,
+		[CLKID_DOS]		    = &meson8b_dos.hw,
+		[CLKID_ISA]		    = &meson8b_isa.hw,
+		[CLKID_PL301]		    = &meson8b_pl301.hw,
+		[CLKID_PERIPHS]		    = &meson8b_periphs.hw,
+		[CLKID_SPICC]		    = &meson8b_spicc.hw,
+		[CLKID_I2C]		    = &meson8b_i2c.hw,
+		[CLKID_SAR_ADC]		    = &meson8b_sar_adc.hw,
+		[CLKID_SMART_CARD]	    = &meson8b_smart_card.hw,
+		[CLKID_RNG0]		    = &meson8b_rng0.hw,
+		[CLKID_UART0]		    = &meson8b_uart0.hw,
+		[CLKID_SDHC]		    = &meson8b_sdhc.hw,
+		[CLKID_STREAM]		    = &meson8b_stream.hw,
+		[CLKID_ASYNC_FIFO]	    = &meson8b_async_fifo.hw,
+		[CLKID_SDIO]		    = &meson8b_sdio.hw,
+		[CLKID_ABUF]		    = &meson8b_abuf.hw,
+		[CLKID_HIU_IFACE]	    = &meson8b_hiu_iface.hw,
+		[CLKID_ASSIST_MISC]	    = &meson8b_assist_misc.hw,
+		[CLKID_SPI]		    = &meson8b_spi.hw,
+		[CLKID_I2S_SPDIF]	    = &meson8b_i2s_spdif.hw,
+		[CLKID_ETH]		    = &meson8b_eth.hw,
+		[CLKID_DEMUX]		    = &meson8b_demux.hw,
+		[CLKID_AIU_GLUE]	    = &meson8b_aiu_glue.hw,
+		[CLKID_IEC958]		    = &meson8b_iec958.hw,
+		[CLKID_I2S_OUT]		    = &meson8b_i2s_out.hw,
+		[CLKID_AMCLK]		    = &meson8b_amclk.hw,
+		[CLKID_AIFIFO2]		    = &meson8b_aififo2.hw,
+		[CLKID_MIXER]		    = &meson8b_mixer.hw,
+		[CLKID_MIXER_IFACE]	    = &meson8b_mixer_iface.hw,
+		[CLKID_ADC]		    = &meson8b_adc.hw,
+		[CLKID_BLKMV]		    = &meson8b_blkmv.hw,
+		[CLKID_AIU]		    = &meson8b_aiu.hw,
+		[CLKID_UART1]		    = &meson8b_uart1.hw,
+		[CLKID_G2D]		    = &meson8b_g2d.hw,
+		[CLKID_USB0]		    = &meson8b_usb0.hw,
+		[CLKID_USB1]		    = &meson8b_usb1.hw,
+		[CLKID_RESET]		    = &meson8b_reset.hw,
+		[CLKID_NAND]		    = &meson8b_nand.hw,
+		[CLKID_DOS_PARSER]	    = &meson8b_dos_parser.hw,
+		[CLKID_USB]		    = &meson8b_usb.hw,
+		[CLKID_VDIN1]		    = &meson8b_vdin1.hw,
+		[CLKID_AHB_ARB0]	    = &meson8b_ahb_arb0.hw,
+		[CLKID_EFUSE]		    = &meson8b_efuse.hw,
+		[CLKID_BOOT_ROM]	    = &meson8b_boot_rom.hw,
+		[CLKID_AHB_DATA_BUS]	    = &meson8b_ahb_data_bus.hw,
+		[CLKID_AHB_CTRL_BUS]	    = &meson8b_ahb_ctrl_bus.hw,
+		[CLKID_HDMI_INTR_SYNC]	    = &meson8b_hdmi_intr_sync.hw,
+		[CLKID_HDMI_PCLK]	    = &meson8b_hdmi_pclk.hw,
+		[CLKID_USB1_DDR_BRIDGE]	    = &meson8b_usb1_ddr_bridge.hw,
+		[CLKID_USB0_DDR_BRIDGE]	    = &meson8b_usb0_ddr_bridge.hw,
+		[CLKID_MMC_PCLK]	    = &meson8b_mmc_pclk.hw,
+		[CLKID_DVIN]		    = &meson8b_dvin.hw,
+		[CLKID_UART2]		    = &meson8b_uart2.hw,
+		[CLKID_SANA]		    = &meson8b_sana.hw,
+		[CLKID_VPU_INTR]	    = &meson8b_vpu_intr.hw,
+		[CLKID_SEC_AHB_AHB3_BRIDGE] = &meson8b_sec_ahb_ahb3_bridge.hw,
+		[CLKID_CLK81_A9]	    = &meson8b_clk81_a9.hw,
+		[CLKID_VCLK2_VENCI0]	    = &meson8b_vclk2_venci0.hw,
+		[CLKID_VCLK2_VENCI1]	    = &meson8b_vclk2_venci1.hw,
+		[CLKID_VCLK2_VENCP0]	    = &meson8b_vclk2_vencp0.hw,
+		[CLKID_VCLK2_VENCP1]	    = &meson8b_vclk2_vencp1.hw,
+		[CLKID_GCLK_VENCI_INT]	    = &meson8b_gclk_venci_int.hw,
+		[CLKID_GCLK_VENCP_INT]	    = &meson8b_gclk_vencp_int.hw,
+		[CLKID_DAC_CLK]		    = &meson8b_dac_clk.hw,
+		[CLKID_AOCLK_GATE]	    = &meson8b_aoclk_gate.hw,
+		[CLKID_IEC958_GATE]	    = &meson8b_iec958_gate.hw,
+		[CLKID_ENC480P]		    = &meson8b_enc480p.hw,
+		[CLKID_RNG1]		    = &meson8b_rng1.hw,
+		[CLKID_GCLK_VENCL_INT]	    = &meson8b_gclk_vencl_int.hw,
+		[CLKID_VCLK2_VENCLMCC]	    = &meson8b_vclk2_venclmcc.hw,
+		[CLKID_VCLK2_VENCL]	    = &meson8b_vclk2_vencl.hw,
+		[CLKID_VCLK2_OTHER]	    = &meson8b_vclk2_other.hw,
+		[CLKID_EDP]		    = &meson8b_edp.hw,
+		[CLKID_AO_MEDIA_CPU]	    = &meson8b_ao_media_cpu.hw,
+		[CLKID_AO_AHB_SRAM]	    = &meson8b_ao_ahb_sram.hw,
+		[CLKID_AO_AHB_BUS]	    = &meson8b_ao_ahb_bus.hw,
+		[CLKID_AO_IFACE]	    = &meson8b_ao_iface.hw,
 	},
 	.num = CLK_NR_CLKS,
 };
@@ -354,6 +501,87 @@ static struct meson_clk_pll *const meson8b_clk_plls[] = {
 	&meson8b_sys_pll,
 };
 
+static struct clk_gate *meson8b_clk_gates[] = {
+	&meson8b_clk81,
+	&meson8b_ddr,
+	&meson8b_dos,
+	&meson8b_isa,
+	&meson8b_pl301,
+	&meson8b_periphs,
+	&meson8b_spicc,
+	&meson8b_i2c,
+	&meson8b_sar_adc,
+	&meson8b_smart_card,
+	&meson8b_rng0,
+	&meson8b_uart0,
+	&meson8b_sdhc,
+	&meson8b_stream,
+	&meson8b_async_fifo,
+	&meson8b_sdio,
+	&meson8b_abuf,
+	&meson8b_hiu_iface,
+	&meson8b_assist_misc,
+	&meson8b_spi,
+	&meson8b_i2s_spdif,
+	&meson8b_eth,
+	&meson8b_demux,
+	&meson8b_aiu_glue,
+	&meson8b_iec958,
+	&meson8b_i2s_out,
+	&meson8b_amclk,
+	&meson8b_aififo2,
+	&meson8b_mixer,
+	&meson8b_mixer_iface,
+	&meson8b_adc,
+	&meson8b_blkmv,
+	&meson8b_aiu,
+	&meson8b_uart1,
+	&meson8b_g2d,
+	&meson8b_usb0,
+	&meson8b_usb1,
+	&meson8b_reset,
+	&meson8b_nand,
+	&meson8b_dos_parser,
+	&meson8b_usb,
+	&meson8b_vdin1,
+	&meson8b_ahb_arb0,
+	&meson8b_efuse,
+	&meson8b_boot_rom,
+	&meson8b_ahb_data_bus,
+	&meson8b_ahb_ctrl_bus,
+	&meson8b_hdmi_intr_sync,
+	&meson8b_hdmi_pclk,
+	&meson8b_usb1_ddr_bridge,
+	&meson8b_usb0_ddr_bridge,
+	&meson8b_mmc_pclk,
+	&meson8b_dvin,
+	&meson8b_uart2,
+	&meson8b_sana,
+	&meson8b_vpu_intr,
+	&meson8b_sec_ahb_ahb3_bridge,
+	&meson8b_clk81_a9,
+	&meson8b_vclk2_venci0,
+	&meson8b_vclk2_venci1,
+	&meson8b_vclk2_vencp0,
+	&meson8b_vclk2_vencp1,
+	&meson8b_gclk_venci_int,
+	&meson8b_gclk_vencp_int,
+	&meson8b_dac_clk,
+	&meson8b_aoclk_gate,
+	&meson8b_iec958_gate,
+	&meson8b_enc480p,
+	&meson8b_rng1,
+	&meson8b_gclk_vencl_int,
+	&meson8b_vclk2_venclmcc,
+	&meson8b_vclk2_vencl,
+	&meson8b_vclk2_other,
+	&meson8b_edp,
+	&meson8b_ao_media_cpu,
+	&meson8b_ao_ahb_sram,
+	&meson8b_ao_ahb_bus,
+	&meson8b_ao_iface,
+};
+
 static int meson8b_clkc_probe(struct platform_device *pdev)
 {
 	void __iomem *clk_base;
@@ -381,6 +609,11 @@ static int meson8b_clkc_probe(struct platform_device *pdev)
 	meson8b_mpeg_clk_div.reg = clk_base + (u32)meson8b_mpeg_clk_div.reg;
 	meson8b_clk81.reg = clk_base + (u32)meson8b_clk81.reg;
 
+	/* Populate base address for gates */
+	for (i = 0; i < ARRAY_SIZE(meson8b_clk_gates); i++)
+		meson8b_clk_gates[i]->reg = clk_base +
+			(u32)meson8b_clk_gates[i]->reg;
+
 	/*
 	 * register all clks
 	 * CLKID_UNUSED = 0, so skip it and start with CLKID_XTAL = 1
@@ -440,8 +673,4 @@ static struct platform_driver meson8b_driver = {
 	},
 };
 
-static int __init meson8b_clkc_init(void)
-{
-	return platform_driver_register(&meson8b_driver);
-}
-device_initcall(meson8b_clkc_init);
+builtin_platform_driver(meson8b_driver);
diff --git a/drivers/clk/meson/meson8b.h b/drivers/clk/meson/meson8b.h
new file mode 100644
index 000000000000..010e9582888d
--- /dev/null
+++ b/drivers/clk/meson/meson8b.h
@@ -0,0 +1,151 @@
+/*
+ * Copyright (c) 2015 Endless Mobile, Inc.
+ * Author: Carlo Caione <carlo@endlessm.com>
+ *
+ * Copyright (c) 2016 BayLibre, Inc.
+ * Michael Turquette <mturquette@baylibre.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 __MESON8B_H
+#define __MESON8B_H
+
+/*
+ * Clock controller register offsets
+ *
+ * Register offsets from the HardKernel[0] data sheet are listed in comment
+ * blocks below. Those offsets must be multiplied by 4 before adding them to
+ * the base address to get the right value
+ *
+ * [0] http://dn.odroid.com/S805/Datasheet/S805_Datasheet%20V0.8%2020150126.pdf
+ */
+#define HHI_GCLK_MPEG0			0x140 /* 0x50 offset in data sheet */
+#define HHI_GCLK_MPEG1			0x144 /* 0x51 offset in data sheet */
+#define HHI_GCLK_MPEG2			0x148 /* 0x52 offset in data sheet */
+#define HHI_GCLK_OTHER			0x150 /* 0x54 offset in data sheet */
+#define HHI_GCLK_AO			0x154 /* 0x55 offset in data sheet */
+#define HHI_SYS_CPU_CLK_CNTL1		0x15c /* 0x57 offset in data sheet */
+#define HHI_MPEG_CLK_CNTL		0x174 /* 0x5d offset in data sheet */
+#define HHI_MPLL_CNTL			0x280 /* 0xa0 offset in data sheet */
+#define HHI_SYS_PLL_CNTL		0x300 /* 0xc0 offset in data sheet */
+#define HHI_VID_PLL_CNTL		0x320 /* 0xc8 offset in data sheet */
+
+/*
+ * CLKID index values
+ *
+ * These indices are entirely contrived and do not map onto the hardware.
+ * Migrate them out of this header and into the DT header file when they need
+ * to be exposed to client nodes in DT: include/dt-bindings/clock/meson8b-clkc.h
+ */
+
+/* CLKID_UNUSED */
+/* CLKID_XTAL */
+/* CLKID_PLL_FIXED */
+/* CLKID_PLL_VID */
+/* CLKID_PLL_SYS */
+/* CLKID_FCLK_DIV2 */
+/* CLKID_FCLK_DIV3 */
+/* CLKID_FCLK_DIV4 */
+/* CLKID_FCLK_DIV5 */
+/* CLKID_FCLK_DIV7 */
+/* CLKID_CLK81 */
+/* CLKID_MALI */
+/* CLKID_CPUCLK */
+/* CLKID_ZERO */
+/* CLKID_MPEG_SEL */
+/* CLKID_MPEG_DIV */
+#define CLKID_DDR		16
+#define CLKID_DOS		17
+#define CLKID_ISA		18
+#define CLKID_PL301		19
+#define CLKID_PERIPHS		20
+#define CLKID_SPICC		21
+#define CLKID_I2C		22
+#define CLKID_SAR_ADC		23
+#define CLKID_SMART_CARD	24
+#define CLKID_RNG0		25
+#define CLKID_UART0		26
+#define CLKID_SDHC		27
+#define CLKID_STREAM		28
+#define CLKID_ASYNC_FIFO	29
+#define CLKID_SDIO		30
+#define CLKID_ABUF		31
+#define CLKID_HIU_IFACE		32
+#define CLKID_ASSIST_MISC	33
+#define CLKID_SPI		34
+#define CLKID_I2S_SPDIF		35
+#define CLKID_ETH		36
+#define CLKID_DEMUX		37
+#define CLKID_AIU_GLUE		38
+#define CLKID_IEC958		39
+#define CLKID_I2S_OUT		40
+#define CLKID_AMCLK		41
+#define CLKID_AIFIFO2		42
+#define CLKID_MIXER		43
+#define CLKID_MIXER_IFACE	44
+#define CLKID_ADC		45
+#define CLKID_BLKMV		46
+#define CLKID_AIU		47
+#define CLKID_UART1		48
+#define CLKID_G2D		49
+#define CLKID_USB0		50
+#define CLKID_USB1		51
+#define CLKID_RESET		52
+#define CLKID_NAND		53
+#define CLKID_DOS_PARSER	54
+#define CLKID_USB		55
+#define CLKID_VDIN1		56
+#define CLKID_AHB_ARB0		57
+#define CLKID_EFUSE		58
+#define CLKID_BOOT_ROM		59
+#define CLKID_AHB_DATA_BUS	60
+#define CLKID_AHB_CTRL_BUS	61
+#define CLKID_HDMI_INTR_SYNC	62
+#define CLKID_HDMI_PCLK		63
+#define CLKID_USB1_DDR_BRIDGE	64
+#define CLKID_USB0_DDR_BRIDGE	65
+#define CLKID_MMC_PCLK		66
+#define CLKID_DVIN		67
+#define CLKID_UART2		68
+#define CLKID_SANA		69
+#define CLKID_VPU_INTR		70
+#define CLKID_SEC_AHB_AHB3_BRIDGE	71
+#define CLKID_CLK81_A9		72
+#define CLKID_VCLK2_VENCI0	73
+#define CLKID_VCLK2_VENCI1	74
+#define CLKID_VCLK2_VENCP0	75
+#define CLKID_VCLK2_VENCP1	76
+#define CLKID_GCLK_VENCI_INT	77
+#define CLKID_GCLK_VENCP_INT	78
+#define CLKID_DAC_CLK		79
+#define CLKID_AOCLK_GATE	80
+#define CLKID_IEC958_GATE	81
+#define CLKID_ENC480P		82
+#define CLKID_RNG1		83
+#define CLKID_GCLK_VENCL_INT	84
+#define CLKID_VCLK2_VENCLMCC	85
+#define CLKID_VCLK2_VENCL	86
+#define CLKID_VCLK2_OTHER	87
+#define CLKID_EDP		88
+#define CLKID_AO_MEDIA_CPU	89
+#define CLKID_AO_AHB_SRAM	90
+#define CLKID_AO_AHB_BUS	91
+#define CLKID_AO_IFACE		92
+
+#define CLK_NR_CLKS		93
+
+/* include the CLKIDs that have been made part of the stable DT binding */
+#include <dt-bindings/clock/meson8b-clkc.h>
+
+#endif /* __MESON8B_H */
diff --git a/drivers/clk/microchip/clk-core.c b/drivers/clk/microchip/clk-core.c
index ca85cea17839..c3b301463425 100644
--- a/drivers/clk/microchip/clk-core.c
+++ b/drivers/clk/microchip/clk-core.c
@@ -199,9 +199,9 @@ static int pbclk_set_rate(struct clk_hw *hw, unsigned long rate,
 
 	spin_unlock_irqrestore(&pb->core->reg_lock, flags);
 
-	/* wait again, for pbdivready */
-	err = readl_poll_timeout_atomic(pb->ctrl_reg, v, v & PB_DIV_READY,
-					1, LOCK_TIMEOUT_US);
+	/* wait again for DIV_READY */
+	err = readl_poll_timeout(pb->ctrl_reg, v, v & PB_DIV_READY,
+				 1, LOCK_TIMEOUT_US);
 	if (err)
 		return err;
 
diff --git a/drivers/clk/microchip/clk-pic32mzda.c b/drivers/clk/microchip/clk-pic32mzda.c
index 51f54380474b..9f734779be92 100644
--- a/drivers/clk/microchip/clk-pic32mzda.c
+++ b/drivers/clk/microchip/clk-pic32mzda.c
@@ -118,6 +118,7 @@ static const struct pic32_sec_osc_data sosc_clk = {
 	.status_reg = 0x1d0,
 	.enable_mask = BIT(1),
 	.status_mask = BIT(4),
+	.fixed_rate = 32768,
 	.init_data = {
 		.name = "sosc_clk",
 		.parent_names = NULL,
diff --git a/drivers/clk/mmp/clk-mmp2.c b/drivers/clk/mmp/clk-mmp2.c
index 383f6a4f64f0..038023483b98 100644
--- a/drivers/clk/mmp/clk-mmp2.c
+++ b/drivers/clk/mmp/clk-mmp2.c
@@ -16,6 +16,7 @@
 #include <linux/io.h>
 #include <linux/delay.h>
 #include <linux/err.h>
+#include <linux/clk/mmp.h>
 
 #include "clk.h"
 
diff --git a/drivers/clk/mvebu/Kconfig b/drivers/clk/mvebu/Kconfig
index 3165da77d525..fddc8ac5faff 100644
--- a/drivers/clk/mvebu/Kconfig
+++ b/drivers/clk/mvebu/Kconfig
@@ -24,6 +24,9 @@ config ARMADA_39X_CLK
 	bool
 	select MVEBU_CLK_COMMON
 
+config ARMADA_37XX_CLK
+       bool
+
 config ARMADA_XP_CLK
 	bool
 	select MVEBU_CLK_COMMON
diff --git a/drivers/clk/mvebu/Makefile b/drivers/clk/mvebu/Makefile
index 7172ef65693d..d9ae97fb43c4 100644
--- a/drivers/clk/mvebu/Makefile
+++ b/drivers/clk/mvebu/Makefile
@@ -6,6 +6,9 @@ obj-$(CONFIG_ARMADA_370_CLK)	+= armada-370.o
 obj-$(CONFIG_ARMADA_375_CLK)	+= armada-375.o
 obj-$(CONFIG_ARMADA_38X_CLK)	+= armada-38x.o
 obj-$(CONFIG_ARMADA_39X_CLK)	+= armada-39x.o
+obj-$(CONFIG_ARMADA_37XX_CLK)	+= armada-37xx-xtal.o
+obj-$(CONFIG_ARMADA_37XX_CLK)	+= armada-37xx-tbg.o
+obj-$(CONFIG_ARMADA_37XX_CLK)	+= armada-37xx-periph.o
 obj-$(CONFIG_ARMADA_XP_CLK)	+= armada-xp.o
 obj-$(CONFIG_ARMADA_AP806_SYSCON) += ap806-system-controller.o
 obj-$(CONFIG_ARMADA_CP110_SYSCON) += cp110-system-controller.o
diff --git a/drivers/clk/mvebu/armada-37xx-periph.c b/drivers/clk/mvebu/armada-37xx-periph.c
new file mode 100644
index 000000000000..45905fc0d75b
--- /dev/null
+++ b/drivers/clk/mvebu/armada-37xx-periph.c
@@ -0,0 +1,447 @@
+/*
+ * Marvell Armada 37xx SoC Peripheral clocks
+ *
+ * Copyright (C) 2016 Marvell
+ *
+ * Gregory CLEMENT <gregory.clement@free-electrons.com>
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2 or later. This program is licensed "as is"
+ * without any warranty of any kind, whether express or implied.
+ *
+ * Most of the peripheral clocks can be modelled like this:
+ *             _____    _______    _______
+ * TBG-A-P  --|     |  |       |  |       |   ______
+ * TBG-B-P  --| Mux |--| /div1 |--| /div2 |--| Gate |--> perip_clk
+ * TBG-A-S  --|     |  |       |  |       |  |______|
+ * TBG-B-S  --|_____|  |_______|  |_______|
+ *
+ * However some clocks may use only one or two block or and use the
+ * xtal clock as parent.
+ */
+
+#include <linux/clk-provider.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+
+#define TBG_SEL		0x0
+#define DIV_SEL0	0x4
+#define DIV_SEL1	0x8
+#define DIV_SEL2	0xC
+#define CLK_SEL		0x10
+#define CLK_DIS		0x14
+
+struct clk_periph_driver_data {
+	struct clk_hw_onecell_data *hw_data;
+	spinlock_t lock;
+};
+
+struct clk_double_div {
+	struct clk_hw hw;
+	void __iomem *reg1;
+	u8 shift1;
+	void __iomem *reg2;
+	u8 shift2;
+};
+
+#define to_clk_double_div(_hw) container_of(_hw, struct clk_double_div, hw)
+
+struct clk_periph_data {
+	const char *name;
+	const char * const *parent_names;
+	int num_parents;
+	struct clk_hw *mux_hw;
+	struct clk_hw *rate_hw;
+	struct clk_hw *gate_hw;
+	bool is_double_div;
+};
+
+static const struct clk_div_table clk_table6[] = {
+	{ .val = 1, .div = 1, },
+	{ .val = 2, .div = 2, },
+	{ .val = 3, .div = 3, },
+	{ .val = 4, .div = 4, },
+	{ .val = 5, .div = 5, },
+	{ .val = 6, .div = 6, },
+	{ .val = 0, .div = 0, }, /* last entry */
+};
+
+static const struct clk_div_table clk_table1[] = {
+	{ .val = 0, .div = 1, },
+	{ .val = 1, .div = 2, },
+	{ .val = 0, .div = 0, }, /* last entry */
+};
+
+static const struct clk_div_table clk_table2[] = {
+	{ .val = 0, .div = 2, },
+	{ .val = 1, .div = 4, },
+	{ .val = 0, .div = 0, }, /* last entry */
+};
+static const struct clk_ops clk_double_div_ops;
+
+#define PERIPH_GATE(_name, _bit)		\
+struct clk_gate gate_##_name = {		\
+	.reg = (void *)CLK_DIS,			\
+	.bit_idx = _bit,			\
+	.hw.init = &(struct clk_init_data){	\
+		.ops =  &clk_gate_ops,		\
+	}					\
+};
+
+#define PERIPH_MUX(_name, _shift)		\
+struct clk_mux mux_##_name = {			\
+	.reg = (void *)TBG_SEL,			\
+	.shift = _shift,			\
+	.mask = 3,				\
+	.hw.init = &(struct clk_init_data){	\
+		.ops =  &clk_mux_ro_ops,	\
+	}					\
+};
+
+#define PERIPH_DOUBLEDIV(_name, _reg1, _reg2, _shift1, _shift2)	\
+struct clk_double_div rate_##_name = {		\
+	.reg1 = (void *)_reg1,			\
+	.reg2 = (void *)_reg2,			\
+	.shift1 = _shift1,			\
+	.shift2 = _shift2,			\
+	.hw.init = &(struct clk_init_data){	\
+		.ops =  &clk_double_div_ops,	\
+	}					\
+};
+
+#define PERIPH_DIV(_name, _reg, _shift, _table)	\
+struct clk_divider rate_##_name = {		\
+	.reg = (void *)_reg,			\
+	.table = _table,			\
+	.shift = _shift,			\
+	.hw.init = &(struct clk_init_data){	\
+		.ops =  &clk_divider_ro_ops,	\
+	}					\
+};
+
+#define PERIPH_CLK_FULL_DD(_name, _bit, _shift, _reg1, _reg2, _shift1, _shift2)\
+static PERIPH_GATE(_name, _bit);			    \
+static PERIPH_MUX(_name, _shift);			    \
+static PERIPH_DOUBLEDIV(_name, _reg1, _reg2, _shift1, _shift2);
+
+#define PERIPH_CLK_FULL(_name, _bit, _shift, _reg, _shift1, _table)	\
+static PERIPH_GATE(_name, _bit);			    \
+static PERIPH_MUX(_name, _shift);			    \
+static PERIPH_DIV(_name, _reg, _shift1, _table);
+
+#define PERIPH_CLK_GATE_DIV(_name, _bit,  _reg, _shift, _table)	\
+static PERIPH_GATE(_name, _bit);			\
+static PERIPH_DIV(_name, _reg, _shift, _table);
+
+#define PERIPH_CLK_MUX_DIV(_name, _shift,  _reg, _shift_div, _table)	\
+static PERIPH_MUX(_name, _shift);			    \
+static PERIPH_DIV(_name, _reg, _shift_div, _table);
+
+#define PERIPH_CLK_MUX_DD(_name, _shift, _reg1, _reg2, _shift1, _shift2)\
+static PERIPH_MUX(_name, _shift);			    \
+static PERIPH_DOUBLEDIV(_name, _reg1, _reg2, _shift1, _shift2);
+
+#define REF_CLK_FULL(_name)				\
+	{ .name = #_name,				\
+	  .parent_names = (const char *[]){ "TBG-A-P",	\
+	      "TBG-B-P", "TBG-A-S", "TBG-B-S"},		\
+	  .num_parents = 4,				\
+	  .mux_hw = &mux_##_name.hw,			\
+	  .gate_hw = &gate_##_name.hw,			\
+	  .rate_hw = &rate_##_name.hw,			\
+	}
+
+#define REF_CLK_FULL_DD(_name)				\
+	{ .name = #_name,				\
+	  .parent_names = (const char *[]){ "TBG-A-P",	\
+	      "TBG-B-P", "TBG-A-S", "TBG-B-S"},		\
+	  .num_parents = 4,				\
+	  .mux_hw = &mux_##_name.hw,			\
+	  .gate_hw = &gate_##_name.hw,			\
+	  .rate_hw = &rate_##_name.hw,			\
+	  .is_double_div = true,			\
+	}
+
+#define REF_CLK_GATE(_name, _parent_name)			\
+	{ .name = #_name,					\
+	  .parent_names = (const char *[]){ _parent_name},	\
+	  .num_parents = 1,					\
+	  .gate_hw = &gate_##_name.hw,				\
+	}
+
+#define REF_CLK_GATE_DIV(_name, _parent_name)			\
+	{ .name = #_name,					\
+	  .parent_names = (const char *[]){ _parent_name},	\
+	  .num_parents = 1,					\
+	  .gate_hw = &gate_##_name.hw,				\
+	  .rate_hw = &rate_##_name.hw,				\
+	}
+
+#define REF_CLK_MUX_DIV(_name)				\
+	{ .name = #_name,				\
+	  .parent_names = (const char *[]){ "TBG-A-P",	\
+	      "TBG-B-P", "TBG-A-S", "TBG-B-S"},		\
+	  .num_parents = 4,				\
+	  .mux_hw = &mux_##_name.hw,			\
+	  .rate_hw = &rate_##_name.hw,			\
+	}
+
+#define REF_CLK_MUX_DD(_name)				\
+	{ .name = #_name,				\
+	  .parent_names = (const char *[]){ "TBG-A-P",	\
+	      "TBG-B-P", "TBG-A-S", "TBG-B-S"},		\
+	  .num_parents = 4,				\
+	  .mux_hw = &mux_##_name.hw,			\
+	  .rate_hw = &rate_##_name.hw,			\
+	  .is_double_div = true,			\
+	}
+
+/* NB periph clocks */
+PERIPH_CLK_FULL_DD(mmc, 2, 0, DIV_SEL2, DIV_SEL2, 16, 13);
+PERIPH_CLK_FULL_DD(sata_host, 3, 2, DIV_SEL2, DIV_SEL2, 10, 7);
+PERIPH_CLK_FULL_DD(sec_at, 6, 4, DIV_SEL1, DIV_SEL1, 3, 0);
+PERIPH_CLK_FULL_DD(sec_dap, 7, 6, DIV_SEL1, DIV_SEL1, 9, 6);
+PERIPH_CLK_FULL_DD(tscem, 8, 8, DIV_SEL1, DIV_SEL1, 15, 12);
+PERIPH_CLK_FULL(tscem_tmx, 10, 10, DIV_SEL1, 18, clk_table6);
+static PERIPH_GATE(avs, 11);
+PERIPH_CLK_FULL_DD(pwm, 13, 14, DIV_SEL0, DIV_SEL0, 3, 0);
+PERIPH_CLK_FULL_DD(sqf, 12, 12, DIV_SEL1, DIV_SEL1, 27, 24);
+static PERIPH_GATE(i2c_2, 16);
+static PERIPH_GATE(i2c_1, 17);
+PERIPH_CLK_GATE_DIV(ddr_phy, 19, DIV_SEL0, 18, clk_table2);
+PERIPH_CLK_FULL_DD(ddr_fclk, 21, 16, DIV_SEL0, DIV_SEL0, 15, 12);
+PERIPH_CLK_FULL(trace, 22, 18, DIV_SEL0, 20, clk_table6);
+PERIPH_CLK_FULL(counter, 23, 20, DIV_SEL0, 23, clk_table6);
+PERIPH_CLK_FULL_DD(eip97, 24, 24, DIV_SEL2, DIV_SEL2, 22, 19);
+PERIPH_CLK_MUX_DIV(cpu, 22, DIV_SEL0, 28, clk_table6);
+
+static struct clk_periph_data data_nb[] ={
+	REF_CLK_FULL_DD(mmc),
+	REF_CLK_FULL_DD(sata_host),
+	REF_CLK_FULL_DD(sec_at),
+	REF_CLK_FULL_DD(sec_dap),
+	REF_CLK_FULL_DD(tscem),
+	REF_CLK_FULL(tscem_tmx),
+	REF_CLK_GATE(avs, "xtal"),
+	REF_CLK_FULL_DD(sqf),
+	REF_CLK_FULL_DD(pwm),
+	REF_CLK_GATE(i2c_2, "xtal"),
+	REF_CLK_GATE(i2c_1, "xtal"),
+	REF_CLK_GATE_DIV(ddr_phy, "TBG-A-S"),
+	REF_CLK_FULL_DD(ddr_fclk),
+	REF_CLK_FULL(trace),
+	REF_CLK_FULL(counter),
+	REF_CLK_FULL_DD(eip97),
+	REF_CLK_MUX_DIV(cpu),
+	{ },
+};
+
+/* SB periph clocks */
+PERIPH_CLK_MUX_DD(gbe_50, 6, DIV_SEL2, DIV_SEL2, 6, 9);
+PERIPH_CLK_MUX_DD(gbe_core, 8, DIV_SEL1, DIV_SEL1, 18, 21);
+PERIPH_CLK_MUX_DD(gbe_125, 10, DIV_SEL1, DIV_SEL1, 6, 9);
+static PERIPH_GATE(gbe1_50, 0);
+static PERIPH_GATE(gbe0_50, 1);
+static PERIPH_GATE(gbe1_125, 2);
+static PERIPH_GATE(gbe0_125, 3);
+PERIPH_CLK_GATE_DIV(gbe1_core, 4, DIV_SEL1, 13, clk_table1);
+PERIPH_CLK_GATE_DIV(gbe0_core, 5, DIV_SEL1, 14, clk_table1);
+PERIPH_CLK_GATE_DIV(gbe_bm, 12, DIV_SEL1, 0, clk_table1);
+PERIPH_CLK_FULL_DD(sdio, 11, 14, DIV_SEL0, DIV_SEL0, 3, 6);
+PERIPH_CLK_FULL_DD(usb32_usb2_sys, 16, 16, DIV_SEL0, DIV_SEL0, 9, 12);
+PERIPH_CLK_FULL_DD(usb32_ss_sys, 17, 18, DIV_SEL0, DIV_SEL0, 15, 18);
+
+static struct clk_periph_data data_sb[] = {
+	REF_CLK_MUX_DD(gbe_50),
+	REF_CLK_MUX_DD(gbe_core),
+	REF_CLK_MUX_DD(gbe_125),
+	REF_CLK_GATE(gbe1_50, "gbe_50"),
+	REF_CLK_GATE(gbe0_50, "gbe_50"),
+	REF_CLK_GATE(gbe1_125, "gbe_125"),
+	REF_CLK_GATE(gbe0_125, "gbe_125"),
+	REF_CLK_GATE_DIV(gbe1_core, "gbe_core"),
+	REF_CLK_GATE_DIV(gbe0_core, "gbe_core"),
+	REF_CLK_GATE_DIV(gbe_bm, "gbe_core"),
+	REF_CLK_FULL_DD(sdio),
+	REF_CLK_FULL_DD(usb32_usb2_sys),
+	REF_CLK_FULL_DD(usb32_ss_sys),
+	{ },
+};
+
+static unsigned int get_div(void __iomem *reg, int shift)
+{
+	u32 val;
+
+	val = (readl(reg) >> shift) & 0x7;
+	if (val > 6)
+		return 0;
+	return val;
+}
+
+static unsigned long clk_double_div_recalc_rate(struct clk_hw *hw,
+		unsigned long parent_rate)
+{
+	struct clk_double_div *double_div = to_clk_double_div(hw);
+	unsigned int div;
+
+	div = get_div(double_div->reg1, double_div->shift1);
+	div *= get_div(double_div->reg2, double_div->shift2);
+
+	return DIV_ROUND_UP_ULL((u64)parent_rate, div);
+}
+
+static const struct clk_ops clk_double_div_ops = {
+	.recalc_rate = clk_double_div_recalc_rate,
+};
+
+static const struct of_device_id armada_3700_periph_clock_of_match[] = {
+	{ .compatible = "marvell,armada-3700-periph-clock-nb",
+	  .data = data_nb, },
+	{ .compatible = "marvell,armada-3700-periph-clock-sb",
+	.data = data_sb, },
+	{ }
+};
+static int armada_3700_add_composite_clk(const struct clk_periph_data *data,
+					 void __iomem *reg, spinlock_t *lock,
+					 struct device *dev, struct clk_hw *hw)
+{
+	const struct clk_ops *mux_ops = NULL, *gate_ops = NULL,
+		*rate_ops = NULL;
+	struct clk_hw *mux_hw = NULL, *gate_hw = NULL, *rate_hw = NULL;
+
+	if (data->mux_hw) {
+		struct clk_mux *mux;
+
+		mux_hw = data->mux_hw;
+		mux = to_clk_mux(mux_hw);
+		mux->lock = lock;
+		mux_ops = mux_hw->init->ops;
+		mux->reg = reg + (u64)mux->reg;
+	}
+
+	if (data->gate_hw) {
+		struct clk_gate *gate;
+
+		gate_hw = data->gate_hw;
+		gate = to_clk_gate(gate_hw);
+		gate->lock = lock;
+		gate_ops = gate_hw->init->ops;
+		gate->reg = reg + (u64)gate->reg;
+	}
+
+	if (data->rate_hw) {
+		rate_hw = data->rate_hw;
+		rate_ops = rate_hw->init->ops;
+		if (data->is_double_div) {
+			struct clk_double_div *rate;
+
+			rate =  to_clk_double_div(rate_hw);
+			rate->reg1 = reg + (u64)rate->reg1;
+			rate->reg2 = reg + (u64)rate->reg2;
+		} else {
+			struct clk_divider *rate = to_clk_divider(rate_hw);
+			const struct clk_div_table *clkt;
+			int table_size = 0;
+
+			rate->reg = reg + (u64)rate->reg;
+			for (clkt = rate->table; clkt->div; clkt++)
+				table_size++;
+			rate->width = order_base_2(table_size);
+			rate->lock = lock;
+		}
+	}
+
+	hw = clk_hw_register_composite(dev, data->name, data->parent_names,
+				       data->num_parents, mux_hw,
+				       mux_ops, rate_hw, rate_ops,
+				       gate_hw, gate_ops, CLK_IGNORE_UNUSED);
+
+	if (IS_ERR(hw))
+		return PTR_ERR(hw);
+
+	return 0;
+}
+
+static int armada_3700_periph_clock_probe(struct platform_device *pdev)
+{
+	struct clk_periph_driver_data *driver_data;
+	struct device_node *np = pdev->dev.of_node;
+	const struct clk_periph_data *data;
+	struct device *dev = &pdev->dev;
+	int num_periph = 0, i, ret;
+	struct resource *res;
+	void __iomem *reg;
+
+	data = of_device_get_match_data(dev);
+	if (!data)
+		return -ENODEV;
+
+	while (data[num_periph].name)
+		num_periph++;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	reg = devm_ioremap_resource(dev, res);
+	if (IS_ERR(reg))
+		return PTR_ERR(reg);
+
+	driver_data = devm_kzalloc(dev, sizeof(*driver_data), GFP_KERNEL);
+	if (!driver_data)
+		return -ENOMEM;
+
+	driver_data->hw_data = devm_kzalloc(dev, sizeof(*driver_data->hw_data) +
+			    sizeof(*driver_data->hw_data->hws) * num_periph,
+			    GFP_KERNEL);
+	if (!driver_data->hw_data)
+		return -ENOMEM;
+	driver_data->hw_data->num = num_periph;
+
+	spin_lock_init(&driver_data->lock);
+
+	for (i = 0; i < num_periph; i++) {
+		struct clk_hw *hw = driver_data->hw_data->hws[i];
+
+		if (armada_3700_add_composite_clk(&data[i], reg,
+						  &driver_data->lock, dev, hw))
+			dev_err(dev, "Can't register periph clock %s\n",
+			       data[i].name);
+
+	}
+
+	ret = of_clk_add_hw_provider(np, of_clk_hw_onecell_get,
+				  driver_data->hw_data);
+	if (ret) {
+		for (i = 0; i < num_periph; i++)
+			clk_hw_unregister(driver_data->hw_data->hws[i]);
+		return ret;
+	}
+
+	platform_set_drvdata(pdev, driver_data);
+	return 0;
+}
+
+static int armada_3700_periph_clock_remove(struct platform_device *pdev)
+{
+	struct clk_periph_driver_data *data = platform_get_drvdata(pdev);
+	struct clk_hw_onecell_data *hw_data = data->hw_data;
+	int i;
+
+	of_clk_del_provider(pdev->dev.of_node);
+
+	for (i = 0; i < hw_data->num; i++)
+		clk_hw_unregister(hw_data->hws[i]);
+
+	return 0;
+}
+
+static struct platform_driver armada_3700_periph_clock_driver = {
+	.probe = armada_3700_periph_clock_probe,
+	.remove = armada_3700_periph_clock_remove,
+	.driver		= {
+		.name	= "marvell-armada-3700-periph-clock",
+		.of_match_table = armada_3700_periph_clock_of_match,
+	},
+};
+
+builtin_platform_driver(armada_3700_periph_clock_driver);
diff --git a/drivers/clk/mvebu/armada-37xx-tbg.c b/drivers/clk/mvebu/armada-37xx-tbg.c
new file mode 100644
index 000000000000..aa80db11f543
--- /dev/null
+++ b/drivers/clk/mvebu/armada-37xx-tbg.c
@@ -0,0 +1,158 @@
+/*
+ * Marvell Armada 37xx SoC Time Base Generator clocks
+ *
+ * Copyright (C) 2016 Marvell
+ *
+ * Gregory CLEMENT <gregory.clement@free-electrons.com>
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2 or later. This program is licensed "as is"
+ * without any warranty of any kind, whether express or implied.
+ */
+
+#include <linux/clk-provider.h>
+#include <linux/clk.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+
+#define NUM_TBG	    4
+
+#define TBG_CTRL0		0x4
+#define TBG_CTRL1		0x8
+#define TBG_CTRL7		0x20
+#define TBG_CTRL8		0x30
+
+#define TBG_DIV_MASK		0x1FF
+
+#define TBG_A_REFDIV		0
+#define TBG_B_REFDIV		16
+
+#define TBG_A_FBDIV		2
+#define TBG_B_FBDIV		18
+
+#define TBG_A_VCODIV_SE		0
+#define TBG_B_VCODIV_SE		16
+
+#define TBG_A_VCODIV_DIFF	1
+#define TBG_B_VCODIV_DIFF	17
+
+struct tbg_def {
+	char *name;
+	u32 refdiv_offset;
+	u32 fbdiv_offset;
+	u32 vcodiv_reg;
+	u32 vcodiv_offset;
+};
+
+static const struct tbg_def tbg[NUM_TBG] = {
+	{"TBG-A-P", TBG_A_REFDIV, TBG_A_FBDIV, TBG_CTRL8, TBG_A_VCODIV_DIFF},
+	{"TBG-B-P", TBG_B_REFDIV, TBG_B_FBDIV, TBG_CTRL8, TBG_B_VCODIV_DIFF},
+	{"TBG-A-S", TBG_A_REFDIV, TBG_A_FBDIV, TBG_CTRL1, TBG_A_VCODIV_SE},
+	{"TBG-B-S", TBG_B_REFDIV, TBG_B_FBDIV, TBG_CTRL1, TBG_B_VCODIV_SE},
+};
+
+static unsigned int tbg_get_mult(void __iomem *reg, const struct tbg_def *ptbg)
+{
+	u32 val;
+
+	val = readl(reg + TBG_CTRL0);
+
+	return ((val >> ptbg->fbdiv_offset) & TBG_DIV_MASK) << 2;
+}
+
+static unsigned int tbg_get_div(void __iomem *reg, const struct tbg_def *ptbg)
+{
+	u32 val;
+	unsigned int div;
+
+	val = readl(reg + TBG_CTRL7);
+
+	div = (val >> ptbg->refdiv_offset) & TBG_DIV_MASK;
+	if (div == 0)
+		div = 1;
+	val = readl(reg + ptbg->vcodiv_reg);
+
+	div *= 1 << ((val >>  ptbg->vcodiv_offset) & TBG_DIV_MASK);
+
+	return div;
+}
+
+
+static int armada_3700_tbg_clock_probe(struct platform_device *pdev)
+{
+	struct device_node *np = pdev->dev.of_node;
+	struct clk_hw_onecell_data *hw_tbg_data;
+	struct device *dev = &pdev->dev;
+	const char *parent_name;
+	struct resource *res;
+	struct clk *parent;
+	void __iomem *reg;
+	int i, ret;
+
+	hw_tbg_data = devm_kzalloc(&pdev->dev, sizeof(*hw_tbg_data)
+				   + sizeof(*hw_tbg_data->hws) * NUM_TBG,
+				   GFP_KERNEL);
+	if (!hw_tbg_data)
+		return -ENOMEM;
+	hw_tbg_data->num = NUM_TBG;
+	platform_set_drvdata(pdev, hw_tbg_data);
+
+	parent = devm_clk_get(dev, NULL);
+	if (IS_ERR(parent)) {
+		dev_err(dev, "Could get the clock parent\n");
+		return -EINVAL;
+	}
+	parent_name = __clk_get_name(parent);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	reg = devm_ioremap_resource(dev, res);
+	if (IS_ERR(reg))
+		return PTR_ERR(reg);
+
+	for (i = 0; i < NUM_TBG; i++) {
+		const char *name;
+		unsigned int mult, div;
+
+		name = tbg[i].name;
+		mult = tbg_get_mult(reg, &tbg[i]);
+		div = tbg_get_div(reg, &tbg[i]);
+		hw_tbg_data->hws[i] = clk_hw_register_fixed_factor(NULL, name,
+						parent_name, 0, mult, div);
+		if (IS_ERR(hw_tbg_data->hws[i]))
+			dev_err(dev, "Can't register TBG clock %s\n", name);
+	}
+
+	ret = of_clk_add_hw_provider(np, of_clk_hw_onecell_get, hw_tbg_data);
+
+	return ret;
+}
+
+static int armada_3700_tbg_clock_remove(struct platform_device *pdev)
+{
+	int i;
+	struct clk_hw_onecell_data *hw_tbg_data = platform_get_drvdata(pdev);
+
+	of_clk_del_provider(pdev->dev.of_node);
+	for (i = 0; i < hw_tbg_data->num; i++)
+		clk_hw_unregister_fixed_factor(hw_tbg_data->hws[i]);
+
+	return 0;
+}
+
+static const struct of_device_id armada_3700_tbg_clock_of_match[] = {
+	{ .compatible = "marvell,armada-3700-tbg-clock", },
+	{ }
+};
+
+static struct platform_driver armada_3700_tbg_clock_driver = {
+	.probe = armada_3700_tbg_clock_probe,
+	.remove = armada_3700_tbg_clock_remove,
+	.driver		= {
+		.name	= "marvell-armada-3700-tbg-clock",
+		.of_match_table = armada_3700_tbg_clock_of_match,
+	},
+};
+
+builtin_platform_driver(armada_3700_tbg_clock_driver);
diff --git a/drivers/clk/mvebu/armada-37xx-xtal.c b/drivers/clk/mvebu/armada-37xx-xtal.c
new file mode 100644
index 000000000000..612d65ede10a
--- /dev/null
+++ b/drivers/clk/mvebu/armada-37xx-xtal.c
@@ -0,0 +1,91 @@
+/*
+ * Marvell Armada 37xx SoC xtal clocks
+ *
+ * Copyright (C) 2016 Marvell
+ *
+ * Gregory CLEMENT <gregory.clement@free-electrons.com>
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2.  This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+
+#include <linux/clk-provider.h>
+#include <linux/mfd/syscon.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+
+#define NB_GPIO1_LATCH	0xC
+#define XTAL_MODE	    BIT(31)
+
+static int armada_3700_xtal_clock_probe(struct platform_device *pdev)
+{
+	struct device_node *np = pdev->dev.of_node;
+	const char *xtal_name = "xtal";
+	struct device_node *parent;
+	struct regmap *regmap;
+	struct clk_hw *xtal_hw;
+	unsigned int rate;
+	u32 reg;
+	int ret;
+
+	xtal_hw = devm_kzalloc(&pdev->dev, sizeof(*xtal_hw), GFP_KERNEL);
+	if (!xtal_hw)
+		return -ENOMEM;
+
+	platform_set_drvdata(pdev, xtal_hw);
+
+	parent = np->parent;
+	if (!parent) {
+		dev_err(&pdev->dev, "no parent\n");
+		return -ENODEV;
+	}
+
+	regmap = syscon_node_to_regmap(parent);
+	if (IS_ERR(regmap)) {
+		dev_err(&pdev->dev, "cannot get regmap\n");
+		return PTR_ERR(regmap);
+	}
+
+	ret = regmap_read(regmap, NB_GPIO1_LATCH, &reg);
+	if (ret) {
+		dev_err(&pdev->dev, "cannot read from regmap\n");
+		return ret;
+	}
+
+	if (reg & XTAL_MODE)
+		rate = 40000000;
+	else
+		rate = 25000000;
+
+	of_property_read_string_index(np, "clock-output-names", 0, &xtal_name);
+	xtal_hw = clk_hw_register_fixed_rate(NULL, xtal_name, NULL, 0, rate);
+	if (IS_ERR(xtal_hw))
+		return PTR_ERR(xtal_hw);
+	ret = of_clk_add_hw_provider(np, of_clk_hw_simple_get, xtal_hw);
+
+	return ret;
+}
+
+static int armada_3700_xtal_clock_remove(struct platform_device *pdev)
+{
+	of_clk_del_provider(pdev->dev.of_node);
+
+	return 0;
+}
+
+static const struct of_device_id armada_3700_xtal_clock_of_match[] = {
+	{ .compatible = "marvell,armada-3700-xtal-clock", },
+	{ }
+};
+
+static struct platform_driver armada_3700_xtal_clock_driver = {
+	.probe = armada_3700_xtal_clock_probe,
+	.remove = armada_3700_xtal_clock_remove,
+	.driver		= {
+		.name	= "marvell-armada-3700-xtal-clock",
+		.of_match_table = armada_3700_xtal_clock_of_match,
+	},
+};
+
+builtin_platform_driver(armada_3700_xtal_clock_driver);
diff --git a/drivers/clk/mvebu/armada-39x.c b/drivers/clk/mvebu/armada-39x.c
index efb974df9822..4fdfd32247a9 100644
--- a/drivers/clk/mvebu/armada-39x.c
+++ b/drivers/clk/mvebu/armada-39x.c
@@ -142,6 +142,8 @@ static const struct clk_gating_soc_desc armada_39x_gating_desc[] __initconst = {
 	{ "pex3", NULL, 7 },
 	{ "pex0", NULL, 8 },
 	{ "usb3h0", NULL, 9 },
+	{ "usb3h1", NULL, 10 },
+	{ "sata0", NULL, 15 },
 	{ "sdio", NULL, 17 },
 	{ "xor0", NULL, 22 },
 	{ "xor1", NULL, 28 },
diff --git a/drivers/clk/mvebu/cp110-system-controller.c b/drivers/clk/mvebu/cp110-system-controller.c
index 7fa42d6b2b92..f2303da7fda7 100644
--- a/drivers/clk/mvebu/cp110-system-controller.c
+++ b/drivers/clk/mvebu/cp110-system-controller.c
@@ -81,13 +81,6 @@ enum {
 #define CP110_GATE_EIP150		25
 #define CP110_GATE_EIP197		26
 
-static struct clk *cp110_clks[CP110_CLK_NUM];
-
-static struct clk_onecell_data cp110_clk_data = {
-	.clks = cp110_clks,
-	.clk_num = CP110_CLK_NUM,
-};
-
 struct cp110_gate_clk {
 	struct clk_hw hw;
 	struct regmap *regmap;
@@ -142,6 +135,8 @@ static struct clk *cp110_register_gate(const char *name,
 	if (!gate)
 		return ERR_PTR(-ENOMEM);
 
+	memset(&init, 0, sizeof(init));
+
 	init.name = name;
 	init.ops = &cp110_gate_ops;
 	init.parent_names = &parent_name;
@@ -194,7 +189,8 @@ static int cp110_syscon_clk_probe(struct platform_device *pdev)
 	struct regmap *regmap;
 	struct device_node *np = pdev->dev.of_node;
 	const char *ppv2_name, *apll_name, *core_name, *eip_name, *nand_name;
-	struct clk *clk;
+	struct clk_onecell_data *cp110_clk_data;
+	struct clk *clk, **cp110_clks;
 	u32 nand_clk_ctrl;
 	int i, ret;
 
@@ -207,6 +203,20 @@ static int cp110_syscon_clk_probe(struct platform_device *pdev)
 	if (ret)
 		return ret;
 
+	cp110_clks = devm_kcalloc(&pdev->dev, sizeof(struct clk *),
+				  CP110_CLK_NUM, GFP_KERNEL);
+	if (!cp110_clks)
+		return -ENOMEM;
+
+	cp110_clk_data = devm_kzalloc(&pdev->dev,
+				      sizeof(*cp110_clk_data),
+				      GFP_KERNEL);
+	if (!cp110_clk_data)
+		return -ENOMEM;
+
+	cp110_clk_data->clks = cp110_clks;
+	cp110_clk_data->clk_num = CP110_CLK_NUM;
+
 	/* Register the APLL which is the root of the clk tree */
 	of_property_read_string_index(np, "core-clock-output-names",
 				      CP110_CORE_APLL, &apll_name);
@@ -334,10 +344,12 @@ static int cp110_syscon_clk_probe(struct platform_device *pdev)
 		cp110_clks[CP110_MAX_CORE_CLOCKS + i] = clk;
 	}
 
-	ret = of_clk_add_provider(np, cp110_of_clk_get, &cp110_clk_data);
+	ret = of_clk_add_provider(np, cp110_of_clk_get, cp110_clk_data);
 	if (ret)
 		goto fail_clk_add;
 
+	platform_set_drvdata(pdev, cp110_clks);
+
 	return 0;
 
 fail_clk_add:
@@ -364,6 +376,7 @@ fail0:
 
 static int cp110_syscon_clk_remove(struct platform_device *pdev)
 {
+	struct clk **cp110_clks = platform_get_drvdata(pdev);
 	int i;
 
 	of_clk_del_provider(pdev->dev.of_node);
diff --git a/drivers/clk/mvebu/orion.c b/drivers/clk/mvebu/orion.c
index fd129566c1ce..a6e5bee23385 100644
--- a/drivers/clk/mvebu/orion.c
+++ b/drivers/clk/mvebu/orion.c
@@ -21,6 +21,76 @@ static const struct coreclk_ratio orion_coreclk_ratios[] __initconst = {
 };
 
 /*
+ * Orion 5181
+ */
+
+#define SAR_MV88F5181_TCLK_FREQ      8
+#define SAR_MV88F5181_TCLK_FREQ_MASK 0x3
+
+static u32 __init mv88f5181_get_tclk_freq(void __iomem *sar)
+{
+	u32 opt = (readl(sar) >> SAR_MV88F5181_TCLK_FREQ) &
+		SAR_MV88F5181_TCLK_FREQ_MASK;
+	if (opt == 0)
+		return 133333333;
+	else if (opt == 1)
+		return 150000000;
+	else if (opt == 2)
+		return 166666667;
+	else
+		return 0;
+}
+
+#define SAR_MV88F5181_CPU_FREQ       4
+#define SAR_MV88F5181_CPU_FREQ_MASK  0xf
+
+static u32 __init mv88f5181_get_cpu_freq(void __iomem *sar)
+{
+	u32 opt = (readl(sar) >> SAR_MV88F5181_CPU_FREQ) &
+		SAR_MV88F5181_CPU_FREQ_MASK;
+	if (opt == 0)
+		return 333333333;
+	else if (opt == 1 || opt == 2)
+		return 400000000;
+	else if (opt == 3)
+		return 500000000;
+	else
+		return 0;
+}
+
+static void __init mv88f5181_get_clk_ratio(void __iomem *sar, int id,
+					   int *mult, int *div)
+{
+	u32 opt = (readl(sar) >> SAR_MV88F5181_CPU_FREQ) &
+		SAR_MV88F5181_CPU_FREQ_MASK;
+	if (opt == 0 || opt == 1) {
+		*mult = 1;
+		*div  = 2;
+	} else if (opt == 2 || opt == 3) {
+		*mult = 1;
+		*div  = 3;
+	} else {
+		*mult = 0;
+		*div  = 1;
+	}
+}
+
+static const struct coreclk_soc_desc mv88f5181_coreclks = {
+	.get_tclk_freq = mv88f5181_get_tclk_freq,
+	.get_cpu_freq = mv88f5181_get_cpu_freq,
+	.get_clk_ratio = mv88f5181_get_clk_ratio,
+	.ratios = orion_coreclk_ratios,
+	.num_ratios = ARRAY_SIZE(orion_coreclk_ratios),
+};
+
+static void __init mv88f5181_clk_init(struct device_node *np)
+{
+	return mvebu_coreclk_setup(np, &mv88f5181_coreclks);
+}
+
+CLK_OF_DECLARE(mv88f5181_clk, "marvell,mv88f5181-core-clock", mv88f5181_clk_init);
+
+/*
  * Orion 5182
  */
 
diff --git a/drivers/clk/nxp/clk-lpc18xx-creg.c b/drivers/clk/nxp/clk-lpc18xx-creg.c
index 9e35749dafdf..c6e802e7e6ec 100644
--- a/drivers/clk/nxp/clk-lpc18xx-creg.c
+++ b/drivers/clk/nxp/clk-lpc18xx-creg.c
@@ -184,7 +184,8 @@ static void __init lpc18xx_creg_clk_init(struct device_node *np)
 
 	of_clk_add_provider(np, of_clk_src_onecell_get, &clk_creg_early_data);
 }
-CLK_OF_DECLARE(lpc18xx_creg_clk, "nxp,lpc1850-creg-clk", lpc18xx_creg_clk_init);
+CLK_OF_DECLARE_DRIVER(lpc18xx_creg_clk, "nxp,lpc1850-creg-clk",
+		      lpc18xx_creg_clk_init);
 
 static struct clk *clk_creg[CREG_CLK_MAX];
 static struct clk_onecell_data clk_creg_data = {
diff --git a/drivers/clk/nxp/clk-lpc32xx.c b/drivers/clk/nxp/clk-lpc32xx.c
index 90d740a2fc0d..34c97353cdeb 100644
--- a/drivers/clk/nxp/clk-lpc32xx.c
+++ b/drivers/clk/nxp/clk-lpc32xx.c
@@ -1513,6 +1513,7 @@ static void __init lpc32xx_clk_init(struct device_node *np)
 	if (IS_ERR(clk_regmap)) {
 		pr_err("failed to regmap system control block: %ld\n",
 			PTR_ERR(clk_regmap));
+		iounmap(base);
 		return;
 	}
 
diff --git a/drivers/clk/qcom/Kconfig b/drivers/clk/qcom/Kconfig
index 95e3b3e0fa1c..0146d3c2547f 100644
--- a/drivers/clk/qcom/Kconfig
+++ b/drivers/clk/qcom/Kconfig
@@ -87,6 +87,23 @@ config MSM_LCC_8960
 	  Say Y if you want to use audio devices such as i2s, pcm,
 	  SLIMBus, etc.
 
+config MDM_GCC_9615
+	tristate "MDM9615 Global Clock Controller"
+	depends on COMMON_CLK_QCOM
+	help
+	  Support for the global clock controller on mdm9615 devices.
+	  Say Y if you want to use peripheral devices such as UART, SPI,
+	  i2c, USB, SD/eMMC, etc.
+
+config MDM_LCC_9615
+	tristate "MDM9615 LPASS Clock Controller"
+	select MDM_GCC_9615
+	depends on COMMON_CLK_QCOM
+	help
+	  Support for the LPASS clock controller on mdm9615 devices.
+	  Say Y if you want to use audio devices such as i2s, pcm,
+	  SLIMBus, etc.
+
 config MSM_MMCC_8960
 	tristate "MSM8960 Multimedia Clock Controller"
 	select MSM_GCC_8960
@@ -117,6 +134,7 @@ config MSM_MMCC_8974
 
 config MSM_GCC_8996
 	tristate "MSM8996 Global Clock Controller"
+	select QCOM_GDSC
 	depends on COMMON_CLK_QCOM
 	help
 	  Support for the global clock controller on msm8996 devices.
@@ -126,6 +144,7 @@ config MSM_GCC_8996
 config MSM_MMCC_8996
 	tristate "MSM8996 Multimedia Clock Controller"
 	select MSM_GCC_8996
+	select QCOM_GDSC
 	depends on COMMON_CLK_QCOM
 	help
 	  Support for the multimedia clock controller on msm8996 devices.
diff --git a/drivers/clk/qcom/Makefile b/drivers/clk/qcom/Makefile
index 2a25f4e75f49..1fb1f5476cb0 100644
--- a/drivers/clk/qcom/Makefile
+++ b/drivers/clk/qcom/Makefile
@@ -12,17 +12,20 @@ clk-qcom-y += clk-regmap-mux.o
 clk-qcom-y += reset.o
 clk-qcom-$(CONFIG_QCOM_GDSC) += gdsc.o
 
+# Keep alphabetically sorted by config
 obj-$(CONFIG_APQ_GCC_8084) += gcc-apq8084.o
 obj-$(CONFIG_APQ_MMCC_8084) += mmcc-apq8084.o
 obj-$(CONFIG_IPQ_GCC_4019) += gcc-ipq4019.o
 obj-$(CONFIG_IPQ_GCC_806X) += gcc-ipq806x.o
 obj-$(CONFIG_IPQ_LCC_806X) += lcc-ipq806x.o
+obj-$(CONFIG_MDM_GCC_9615) += gcc-mdm9615.o
+obj-$(CONFIG_MDM_LCC_9615) += lcc-mdm9615.o
 obj-$(CONFIG_MSM_GCC_8660) += gcc-msm8660.o
 obj-$(CONFIG_MSM_GCC_8916) += gcc-msm8916.o
 obj-$(CONFIG_MSM_GCC_8960) += gcc-msm8960.o
-obj-$(CONFIG_MSM_LCC_8960) += lcc-msm8960.o
 obj-$(CONFIG_MSM_GCC_8974) += gcc-msm8974.o
 obj-$(CONFIG_MSM_GCC_8996) += gcc-msm8996.o
+obj-$(CONFIG_MSM_LCC_8960) += lcc-msm8960.o
 obj-$(CONFIG_MSM_MMCC_8960) += mmcc-msm8960.o
 obj-$(CONFIG_MSM_MMCC_8974) += mmcc-msm8974.o
 obj-$(CONFIG_MSM_MMCC_8996) += mmcc-msm8996.o
diff --git a/drivers/clk/qcom/clk-regmap.c b/drivers/clk/qcom/clk-regmap.c
index a58ba39a900c..1c856d330733 100644
--- a/drivers/clk/qcom/clk-regmap.c
+++ b/drivers/clk/qcom/clk-regmap.c
@@ -101,14 +101,13 @@ EXPORT_SYMBOL_GPL(clk_disable_regmap);
  * clk_regmap struct via this function so that the regmap is initialized
  * and so that the clock is registered with the common clock framework.
  */
-struct clk *devm_clk_register_regmap(struct device *dev,
-				     struct clk_regmap *rclk)
+int devm_clk_register_regmap(struct device *dev, struct clk_regmap *rclk)
 {
 	if (dev && dev_get_regmap(dev, NULL))
 		rclk->regmap = dev_get_regmap(dev, NULL);
 	else if (dev && dev->parent)
 		rclk->regmap = dev_get_regmap(dev->parent, NULL);
 
-	return devm_clk_register(dev, &rclk->hw);
+	return devm_clk_hw_register(dev, &rclk->hw);
 }
 EXPORT_SYMBOL_GPL(devm_clk_register_regmap);
diff --git a/drivers/clk/qcom/clk-regmap.h b/drivers/clk/qcom/clk-regmap.h
index 491a63d537df..90d95cd11ec6 100644
--- a/drivers/clk/qcom/clk-regmap.h
+++ b/drivers/clk/qcom/clk-regmap.h
@@ -39,7 +39,6 @@ struct clk_regmap {
 int clk_is_enabled_regmap(struct clk_hw *hw);
 int clk_enable_regmap(struct clk_hw *hw);
 void clk_disable_regmap(struct clk_hw *hw);
-struct clk *
-devm_clk_register_regmap(struct device *dev, struct clk_regmap *rclk);
+int devm_clk_register_regmap(struct device *dev, struct clk_regmap *rclk);
 
 #endif
diff --git a/drivers/clk/qcom/common.c b/drivers/clk/qcom/common.c
index f7c226ab4307..fffcbaf0fba7 100644
--- a/drivers/clk/qcom/common.c
+++ b/drivers/clk/qcom/common.c
@@ -27,8 +27,8 @@
 
 struct qcom_cc {
 	struct qcom_reset_controller reset;
-	struct clk_onecell_data data;
-	struct clk *clks[];
+	struct clk_regmap **rclks;
+	size_t num_rclks;
 };
 
 const
@@ -102,8 +102,8 @@ static int _qcom_cc_register_board_clk(struct device *dev, const char *path,
 	struct device_node *clocks_node;
 	struct clk_fixed_factor *factor;
 	struct clk_fixed_rate *fixed;
-	struct clk *clk;
 	struct clk_init_data init_data = { };
+	int ret;
 
 	clocks_node = of_find_node_by_path("/clocks");
 	if (clocks_node)
@@ -121,9 +121,9 @@ static int _qcom_cc_register_board_clk(struct device *dev, const char *path,
 		init_data.name = path;
 		init_data.ops = &clk_fixed_rate_ops;
 
-		clk = devm_clk_register(dev, &fixed->hw);
-		if (IS_ERR(clk))
-			return PTR_ERR(clk);
+		ret = devm_clk_hw_register(dev, &fixed->hw);
+		if (ret)
+			return ret;
 	}
 	of_node_put(node);
 
@@ -141,9 +141,9 @@ static int _qcom_cc_register_board_clk(struct device *dev, const char *path,
 		init_data.flags = 0;
 		init_data.ops = &clk_fixed_factor_ops;
 
-		clk = devm_clk_register(dev, &factor->hw);
-		if (IS_ERR(clk))
-			return PTR_ERR(clk);
+		ret = devm_clk_hw_register(dev, &factor->hw);
+		if (ret)
+			return ret;
 	}
 
 	return 0;
@@ -174,42 +174,48 @@ int qcom_cc_register_sleep_clk(struct device *dev)
 }
 EXPORT_SYMBOL_GPL(qcom_cc_register_sleep_clk);
 
+static struct clk_hw *qcom_cc_clk_hw_get(struct of_phandle_args *clkspec,
+					 void *data)
+{
+	struct qcom_cc *cc = data;
+	unsigned int idx = clkspec->args[0];
+
+	if (idx >= cc->num_rclks) {
+		pr_err("%s: invalid index %u\n", __func__, idx);
+		return ERR_PTR(-EINVAL);
+	}
+
+	return cc->rclks[idx] ? &cc->rclks[idx]->hw : ERR_PTR(-ENOENT);
+}
+
 int qcom_cc_really_probe(struct platform_device *pdev,
 			 const struct qcom_cc_desc *desc, struct regmap *regmap)
 {
 	int i, ret;
 	struct device *dev = &pdev->dev;
-	struct clk *clk;
-	struct clk_onecell_data *data;
-	struct clk **clks;
 	struct qcom_reset_controller *reset;
 	struct qcom_cc *cc;
 	struct gdsc_desc *scd;
 	size_t num_clks = desc->num_clks;
 	struct clk_regmap **rclks = desc->clks;
 
-	cc = devm_kzalloc(dev, sizeof(*cc) + sizeof(*clks) * num_clks,
-			  GFP_KERNEL);
+	cc = devm_kzalloc(dev, sizeof(*cc), GFP_KERNEL);
 	if (!cc)
 		return -ENOMEM;
 
-	clks = cc->clks;
-	data = &cc->data;
-	data->clks = clks;
-	data->clk_num = num_clks;
+	cc->rclks = rclks;
+	cc->num_rclks = num_clks;
 
 	for (i = 0; i < num_clks; i++) {
-		if (!rclks[i]) {
-			clks[i] = ERR_PTR(-ENOENT);
+		if (!rclks[i])
 			continue;
-		}
-		clk = devm_clk_register_regmap(dev, rclks[i]);
-		if (IS_ERR(clk))
-			return PTR_ERR(clk);
-		clks[i] = clk;
+
+		ret = devm_clk_register_regmap(dev, rclks[i]);
+		if (ret)
+			return ret;
 	}
 
-	ret = of_clk_add_provider(dev->of_node, of_clk_src_onecell_get, data);
+	ret = of_clk_add_hw_provider(dev->of_node, qcom_cc_clk_hw_get, cc);
 	if (ret)
 		return ret;
 
diff --git a/drivers/clk/qcom/gcc-ipq4019.c b/drivers/clk/qcom/gcc-ipq4019.c
index 3cd1af0af0d9..b593065de8db 100644
--- a/drivers/clk/qcom/gcc-ipq4019.c
+++ b/drivers/clk/qcom/gcc-ipq4019.c
@@ -1332,7 +1332,6 @@ static struct platform_driver gcc_ipq4019_driver = {
 	.probe		= gcc_ipq4019_probe,
 	.driver		= {
 		.name	= "qcom,gcc-ipq4019",
-		.owner	= THIS_MODULE,
 		.of_match_table = gcc_ipq4019_match_table,
 	},
 };
diff --git a/drivers/clk/qcom/gcc-mdm9615.c b/drivers/clk/qcom/gcc-mdm9615.c
new file mode 100644
index 000000000000..581a17f67379
--- /dev/null
+++ b/drivers/clk/qcom/gcc-mdm9615.c
@@ -0,0 +1,1727 @@
+/*
+ * Copyright (c) 2013-2014, The Linux Foundation. All rights reserved.
+ * Copyright (c) BayLibre, SAS.
+ * Author : Neil Armstrong <narmstrong@baylibre.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/kernel.h>
+#include <linux/bitops.h>
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/clk-provider.h>
+#include <linux/regmap.h>
+#include <linux/reset-controller.h>
+
+#include <dt-bindings/clock/qcom,gcc-mdm9615.h>
+#include <dt-bindings/reset/qcom,gcc-mdm9615.h>
+
+#include "common.h"
+#include "clk-regmap.h"
+#include "clk-pll.h"
+#include "clk-rcg.h"
+#include "clk-branch.h"
+#include "reset.h"
+
+static struct clk_fixed_factor cxo = {
+	.mult = 1,
+	.div = 1,
+	.hw.init = &(struct clk_init_data){
+		.name = "cxo",
+		.parent_names = (const char *[]){ "cxo_board" },
+		.num_parents = 1,
+		.ops = &clk_fixed_factor_ops,
+	},
+};
+
+static struct clk_pll pll0 = {
+	.l_reg = 0x30c4,
+	.m_reg = 0x30c8,
+	.n_reg = 0x30cc,
+	.config_reg = 0x30d4,
+	.mode_reg = 0x30c0,
+	.status_reg = 0x30d8,
+	.status_bit = 16,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "pll0",
+		.parent_names = (const char *[]){ "cxo" },
+		.num_parents = 1,
+		.ops = &clk_pll_ops,
+	},
+};
+
+static struct clk_regmap pll0_vote = {
+	.enable_reg = 0x34c0,
+	.enable_mask = BIT(0),
+	.hw.init = &(struct clk_init_data){
+		.name = "pll0_vote",
+		.parent_names = (const char *[]){ "pll8" },
+		.num_parents = 1,
+		.ops = &clk_pll_vote_ops,
+	},
+};
+
+static struct clk_regmap pll4_vote = {
+	.enable_reg = 0x34c0,
+	.enable_mask = BIT(4),
+	.hw.init = &(struct clk_init_data){
+		.name = "pll4_vote",
+		.parent_names = (const char *[]){ "pll4" },
+		.num_parents = 1,
+		.ops = &clk_pll_vote_ops,
+	},
+};
+
+static struct clk_pll pll8 = {
+	.l_reg = 0x3144,
+	.m_reg = 0x3148,
+	.n_reg = 0x314c,
+	.config_reg = 0x3154,
+	.mode_reg = 0x3140,
+	.status_reg = 0x3158,
+	.status_bit = 16,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "pll8",
+		.parent_names = (const char *[]){ "cxo" },
+		.num_parents = 1,
+		.ops = &clk_pll_ops,
+	},
+};
+
+static struct clk_regmap pll8_vote = {
+	.enable_reg = 0x34c0,
+	.enable_mask = BIT(8),
+	.hw.init = &(struct clk_init_data){
+		.name = "pll8_vote",
+		.parent_names = (const char *[]){ "pll8" },
+		.num_parents = 1,
+		.ops = &clk_pll_vote_ops,
+	},
+};
+
+static struct clk_pll pll14 = {
+	.l_reg = 0x31c4,
+	.m_reg = 0x31c8,
+	.n_reg = 0x31cc,
+	.config_reg = 0x31d4,
+	.mode_reg = 0x31c0,
+	.status_reg = 0x31d8,
+	.status_bit = 16,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "pll14",
+		.parent_names = (const char *[]){ "cxo" },
+		.num_parents = 1,
+		.ops = &clk_pll_ops,
+	},
+};
+
+static struct clk_regmap pll14_vote = {
+	.enable_reg = 0x34c0,
+	.enable_mask = BIT(11),
+	.hw.init = &(struct clk_init_data){
+		.name = "pll14_vote",
+		.parent_names = (const char *[]){ "pll14" },
+		.num_parents = 1,
+		.ops = &clk_pll_vote_ops,
+	},
+};
+
+enum {
+	P_CXO,
+	P_PLL8,
+	P_PLL14,
+};
+
+static const struct parent_map gcc_cxo_pll8_map[] = {
+	{ P_CXO, 0 },
+	{ P_PLL8, 3 }
+};
+
+static const char * const gcc_cxo_pll8[] = {
+	"cxo",
+	"pll8_vote",
+};
+
+static const struct parent_map gcc_cxo_pll14_map[] = {
+	{ P_CXO, 0 },
+	{ P_PLL14, 4 }
+};
+
+static const char * const gcc_cxo_pll14[] = {
+	"cxo",
+	"pll14_vote",
+};
+
+static const struct parent_map gcc_cxo_map[] = {
+	{ P_CXO, 0 },
+};
+
+static const char * const gcc_cxo[] = {
+	"cxo",
+};
+
+static struct freq_tbl clk_tbl_gsbi_uart[] = {
+	{  1843200, P_PLL8, 2,  6, 625 },
+	{  3686400, P_PLL8, 2, 12, 625 },
+	{  7372800, P_PLL8, 2, 24, 625 },
+	{ 14745600, P_PLL8, 2, 48, 625 },
+	{ 16000000, P_PLL8, 4,  1,   6 },
+	{ 24000000, P_PLL8, 4,  1,   4 },
+	{ 32000000, P_PLL8, 4,  1,   3 },
+	{ 40000000, P_PLL8, 1,  5,  48 },
+	{ 46400000, P_PLL8, 1, 29, 240 },
+	{ 48000000, P_PLL8, 4,  1,   2 },
+	{ 51200000, P_PLL8, 1,  2,  15 },
+	{ 56000000, P_PLL8, 1,  7,  48 },
+	{ 58982400, P_PLL8, 1, 96, 625 },
+	{ 64000000, P_PLL8, 2,  1,   3 },
+	{ }
+};
+
+static struct clk_rcg gsbi1_uart_src = {
+	.ns_reg = 0x29d4,
+	.md_reg = 0x29d0,
+	.mn = {
+		.mnctr_en_bit = 8,
+		.mnctr_reset_bit = 7,
+		.mnctr_mode_shift = 5,
+		.n_val_shift = 16,
+		.m_val_shift = 16,
+		.width = 16,
+	},
+	.p = {
+		.pre_div_shift = 3,
+		.pre_div_width = 2,
+	},
+	.s = {
+		.src_sel_shift = 0,
+		.parent_map = gcc_cxo_pll8_map,
+	},
+	.freq_tbl = clk_tbl_gsbi_uart,
+	.clkr = {
+		.enable_reg = 0x29d4,
+		.enable_mask = BIT(11),
+		.hw.init = &(struct clk_init_data){
+			.name = "gsbi1_uart_src",
+			.parent_names = gcc_cxo_pll8,
+			.num_parents = 2,
+			.ops = &clk_rcg_ops,
+			.flags = CLK_SET_PARENT_GATE,
+		},
+	},
+};
+
+static struct clk_branch gsbi1_uart_clk = {
+	.halt_reg = 0x2fcc,
+	.halt_bit = 10,
+	.clkr = {
+		.enable_reg = 0x29d4,
+		.enable_mask = BIT(9),
+		.hw.init = &(struct clk_init_data){
+			.name = "gsbi1_uart_clk",
+			.parent_names = (const char *[]){
+				"gsbi1_uart_src",
+			},
+			.num_parents = 1,
+			.ops = &clk_branch_ops,
+			.flags = CLK_SET_RATE_PARENT,
+		},
+	},
+};
+
+static struct clk_rcg gsbi2_uart_src = {
+	.ns_reg = 0x29f4,
+	.md_reg = 0x29f0,
+	.mn = {
+		.mnctr_en_bit = 8,
+		.mnctr_reset_bit = 7,
+		.mnctr_mode_shift = 5,
+		.n_val_shift = 16,
+		.m_val_shift = 16,
+		.width = 16,
+	},
+	.p = {
+		.pre_div_shift = 3,
+		.pre_div_width = 2,
+	},
+	.s = {
+		.src_sel_shift = 0,
+		.parent_map = gcc_cxo_pll8_map,
+	},
+	.freq_tbl = clk_tbl_gsbi_uart,
+	.clkr = {
+		.enable_reg = 0x29f4,
+		.enable_mask = BIT(11),
+		.hw.init = &(struct clk_init_data){
+			.name = "gsbi2_uart_src",
+			.parent_names = gcc_cxo_pll8,
+			.num_parents = 2,
+			.ops = &clk_rcg_ops,
+			.flags = CLK_SET_PARENT_GATE,
+		},
+	},
+};
+
+static struct clk_branch gsbi2_uart_clk = {
+	.halt_reg = 0x2fcc,
+	.halt_bit = 6,
+	.clkr = {
+		.enable_reg = 0x29f4,
+		.enable_mask = BIT(9),
+		.hw.init = &(struct clk_init_data){
+			.name = "gsbi2_uart_clk",
+			.parent_names = (const char *[]){
+				"gsbi2_uart_src",
+			},
+			.num_parents = 1,
+			.ops = &clk_branch_ops,
+			.flags = CLK_SET_RATE_PARENT,
+		},
+	},
+};
+
+static struct clk_rcg gsbi3_uart_src = {
+	.ns_reg = 0x2a14,
+	.md_reg = 0x2a10,
+	.mn = {
+		.mnctr_en_bit = 8,
+		.mnctr_reset_bit = 7,
+		.mnctr_mode_shift = 5,
+		.n_val_shift = 16,
+		.m_val_shift = 16,
+		.width = 16,
+	},
+	.p = {
+		.pre_div_shift = 3,
+		.pre_div_width = 2,
+	},
+	.s = {
+		.src_sel_shift = 0,
+		.parent_map = gcc_cxo_pll8_map,
+	},
+	.freq_tbl = clk_tbl_gsbi_uart,
+	.clkr = {
+		.enable_reg = 0x2a14,
+		.enable_mask = BIT(11),
+		.hw.init = &(struct clk_init_data){
+			.name = "gsbi3_uart_src",
+			.parent_names = gcc_cxo_pll8,
+			.num_parents = 2,
+			.ops = &clk_rcg_ops,
+			.flags = CLK_SET_PARENT_GATE,
+		},
+	},
+};
+
+static struct clk_branch gsbi3_uart_clk = {
+	.halt_reg = 0x2fcc,
+	.halt_bit = 2,
+	.clkr = {
+		.enable_reg = 0x2a14,
+		.enable_mask = BIT(9),
+		.hw.init = &(struct clk_init_data){
+			.name = "gsbi3_uart_clk",
+			.parent_names = (const char *[]){
+				"gsbi3_uart_src",
+			},
+			.num_parents = 1,
+			.ops = &clk_branch_ops,
+			.flags = CLK_SET_RATE_PARENT,
+		},
+	},
+};
+
+static struct clk_rcg gsbi4_uart_src = {
+	.ns_reg = 0x2a34,
+	.md_reg = 0x2a30,
+	.mn = {
+		.mnctr_en_bit = 8,
+		.mnctr_reset_bit = 7,
+		.mnctr_mode_shift = 5,
+		.n_val_shift = 16,
+		.m_val_shift = 16,
+		.width = 16,
+	},
+	.p = {
+		.pre_div_shift = 3,
+		.pre_div_width = 2,
+	},
+	.s = {
+		.src_sel_shift = 0,
+		.parent_map = gcc_cxo_pll8_map,
+	},
+	.freq_tbl = clk_tbl_gsbi_uart,
+	.clkr = {
+		.enable_reg = 0x2a34,
+		.enable_mask = BIT(11),
+		.hw.init = &(struct clk_init_data){
+			.name = "gsbi4_uart_src",
+			.parent_names = gcc_cxo_pll8,
+			.num_parents = 2,
+			.ops = &clk_rcg_ops,
+			.flags = CLK_SET_PARENT_GATE,
+		},
+	},
+};
+
+static struct clk_branch gsbi4_uart_clk = {
+	.halt_reg = 0x2fd0,
+	.halt_bit = 26,
+	.clkr = {
+		.enable_reg = 0x2a34,
+		.enable_mask = BIT(9),
+		.hw.init = &(struct clk_init_data){
+			.name = "gsbi4_uart_clk",
+			.parent_names = (const char *[]){
+				"gsbi4_uart_src",
+			},
+			.num_parents = 1,
+			.ops = &clk_branch_ops,
+			.flags = CLK_SET_RATE_PARENT,
+		},
+	},
+};
+
+static struct clk_rcg gsbi5_uart_src = {
+	.ns_reg = 0x2a54,
+	.md_reg = 0x2a50,
+	.mn = {
+		.mnctr_en_bit = 8,
+		.mnctr_reset_bit = 7,
+		.mnctr_mode_shift = 5,
+		.n_val_shift = 16,
+		.m_val_shift = 16,
+		.width = 16,
+	},
+	.p = {
+		.pre_div_shift = 3,
+		.pre_div_width = 2,
+	},
+	.s = {
+		.src_sel_shift = 0,
+		.parent_map = gcc_cxo_pll8_map,
+	},
+	.freq_tbl = clk_tbl_gsbi_uart,
+	.clkr = {
+		.enable_reg = 0x2a54,
+		.enable_mask = BIT(11),
+		.hw.init = &(struct clk_init_data){
+			.name = "gsbi5_uart_src",
+			.parent_names = gcc_cxo_pll8,
+			.num_parents = 2,
+			.ops = &clk_rcg_ops,
+			.flags = CLK_SET_PARENT_GATE,
+		},
+	},
+};
+
+static struct clk_branch gsbi5_uart_clk = {
+	.halt_reg = 0x2fd0,
+	.halt_bit = 22,
+	.clkr = {
+		.enable_reg = 0x2a54,
+		.enable_mask = BIT(9),
+		.hw.init = &(struct clk_init_data){
+			.name = "gsbi5_uart_clk",
+			.parent_names = (const char *[]){
+				"gsbi5_uart_src",
+			},
+			.num_parents = 1,
+			.ops = &clk_branch_ops,
+			.flags = CLK_SET_RATE_PARENT,
+		},
+	},
+};
+
+static struct freq_tbl clk_tbl_gsbi_qup[] = {
+	{   960000, P_CXO,  4, 1,  5 },
+	{  4800000, P_CXO,  4, 0,  1 },
+	{  9600000, P_CXO,  2, 0,  1 },
+	{ 15060000, P_PLL8, 1, 2, 51 },
+	{ 24000000, P_PLL8, 4, 1,  4 },
+	{ 25600000, P_PLL8, 1, 1, 15 },
+	{ 48000000, P_PLL8, 4, 1,  2 },
+	{ 51200000, P_PLL8, 1, 2, 15 },
+	{ }
+};
+
+static struct clk_rcg gsbi1_qup_src = {
+	.ns_reg = 0x29cc,
+	.md_reg = 0x29c8,
+	.mn = {
+		.mnctr_en_bit = 8,
+		.mnctr_reset_bit = 7,
+		.mnctr_mode_shift = 5,
+		.n_val_shift = 16,
+		.m_val_shift = 16,
+		.width = 8,
+	},
+	.p = {
+		.pre_div_shift = 3,
+		.pre_div_width = 2,
+	},
+	.s = {
+		.src_sel_shift = 0,
+		.parent_map = gcc_cxo_pll8_map,
+	},
+	.freq_tbl = clk_tbl_gsbi_qup,
+	.clkr = {
+		.enable_reg = 0x29cc,
+		.enable_mask = BIT(11),
+		.hw.init = &(struct clk_init_data){
+			.name = "gsbi1_qup_src",
+			.parent_names = gcc_cxo_pll8,
+			.num_parents = 2,
+			.ops = &clk_rcg_ops,
+			.flags = CLK_SET_PARENT_GATE,
+		},
+	},
+};
+
+static struct clk_branch gsbi1_qup_clk = {
+	.halt_reg = 0x2fcc,
+	.halt_bit = 9,
+	.clkr = {
+		.enable_reg = 0x29cc,
+		.enable_mask = BIT(9),
+		.hw.init = &(struct clk_init_data){
+			.name = "gsbi1_qup_clk",
+			.parent_names = (const char *[]){ "gsbi1_qup_src" },
+			.num_parents = 1,
+			.ops = &clk_branch_ops,
+			.flags = CLK_SET_RATE_PARENT,
+		},
+	},
+};
+
+static struct clk_rcg gsbi2_qup_src = {
+	.ns_reg = 0x29ec,
+	.md_reg = 0x29e8,
+	.mn = {
+		.mnctr_en_bit = 8,
+		.mnctr_reset_bit = 7,
+		.mnctr_mode_shift = 5,
+		.n_val_shift = 16,
+		.m_val_shift = 16,
+		.width = 8,
+	},
+	.p = {
+		.pre_div_shift = 3,
+		.pre_div_width = 2,
+	},
+	.s = {
+		.src_sel_shift = 0,
+		.parent_map = gcc_cxo_pll8_map,
+	},
+	.freq_tbl = clk_tbl_gsbi_qup,
+	.clkr = {
+		.enable_reg = 0x29ec,
+		.enable_mask = BIT(11),
+		.hw.init = &(struct clk_init_data){
+			.name = "gsbi2_qup_src",
+			.parent_names = gcc_cxo_pll8,
+			.num_parents = 2,
+			.ops = &clk_rcg_ops,
+			.flags = CLK_SET_PARENT_GATE,
+		},
+	},
+};
+
+static struct clk_branch gsbi2_qup_clk = {
+	.halt_reg = 0x2fcc,
+	.halt_bit = 4,
+	.clkr = {
+		.enable_reg = 0x29ec,
+		.enable_mask = BIT(9),
+		.hw.init = &(struct clk_init_data){
+			.name = "gsbi2_qup_clk",
+			.parent_names = (const char *[]){ "gsbi2_qup_src" },
+			.num_parents = 1,
+			.ops = &clk_branch_ops,
+			.flags = CLK_SET_RATE_PARENT,
+		},
+	},
+};
+
+static struct clk_rcg gsbi3_qup_src = {
+	.ns_reg = 0x2a0c,
+	.md_reg = 0x2a08,
+	.mn = {
+		.mnctr_en_bit = 8,
+		.mnctr_reset_bit = 7,
+		.mnctr_mode_shift = 5,
+		.n_val_shift = 16,
+		.m_val_shift = 16,
+		.width = 8,
+	},
+	.p = {
+		.pre_div_shift = 3,
+		.pre_div_width = 2,
+	},
+	.s = {
+		.src_sel_shift = 0,
+		.parent_map = gcc_cxo_pll8_map,
+	},
+	.freq_tbl = clk_tbl_gsbi_qup,
+	.clkr = {
+		.enable_reg = 0x2a0c,
+		.enable_mask = BIT(11),
+		.hw.init = &(struct clk_init_data){
+			.name = "gsbi3_qup_src",
+			.parent_names = gcc_cxo_pll8,
+			.num_parents = 2,
+			.ops = &clk_rcg_ops,
+			.flags = CLK_SET_PARENT_GATE,
+		},
+	},
+};
+
+static struct clk_branch gsbi3_qup_clk = {
+	.halt_reg = 0x2fcc,
+	.halt_bit = 0,
+	.clkr = {
+		.enable_reg = 0x2a0c,
+		.enable_mask = BIT(9),
+		.hw.init = &(struct clk_init_data){
+			.name = "gsbi3_qup_clk",
+			.parent_names = (const char *[]){ "gsbi3_qup_src" },
+			.num_parents = 1,
+			.ops = &clk_branch_ops,
+			.flags = CLK_SET_RATE_PARENT,
+		},
+	},
+};
+
+static struct clk_rcg gsbi4_qup_src = {
+	.ns_reg = 0x2a2c,
+	.md_reg = 0x2a28,
+	.mn = {
+		.mnctr_en_bit = 8,
+		.mnctr_reset_bit = 7,
+		.mnctr_mode_shift = 5,
+		.n_val_shift = 16,
+		.m_val_shift = 16,
+		.width = 8,
+	},
+	.p = {
+		.pre_div_shift = 3,
+		.pre_div_width = 2,
+	},
+	.s = {
+		.src_sel_shift = 0,
+		.parent_map = gcc_cxo_pll8_map,
+	},
+	.freq_tbl = clk_tbl_gsbi_qup,
+	.clkr = {
+		.enable_reg = 0x2a2c,
+		.enable_mask = BIT(11),
+		.hw.init = &(struct clk_init_data){
+			.name = "gsbi4_qup_src",
+			.parent_names = gcc_cxo_pll8,
+			.num_parents = 2,
+			.ops = &clk_rcg_ops,
+			.flags = CLK_SET_PARENT_GATE,
+		},
+	},
+};
+
+static struct clk_branch gsbi4_qup_clk = {
+	.halt_reg = 0x2fd0,
+	.halt_bit = 24,
+	.clkr = {
+		.enable_reg = 0x2a2c,
+		.enable_mask = BIT(9),
+		.hw.init = &(struct clk_init_data){
+			.name = "gsbi4_qup_clk",
+			.parent_names = (const char *[]){ "gsbi4_qup_src" },
+			.num_parents = 1,
+			.ops = &clk_branch_ops,
+			.flags = CLK_SET_RATE_PARENT,
+		},
+	},
+};
+
+static struct clk_rcg gsbi5_qup_src = {
+	.ns_reg = 0x2a4c,
+	.md_reg = 0x2a48,
+	.mn = {
+		.mnctr_en_bit = 8,
+		.mnctr_reset_bit = 7,
+		.mnctr_mode_shift = 5,
+		.n_val_shift = 16,
+		.m_val_shift = 16,
+		.width = 8,
+	},
+	.p = {
+		.pre_div_shift = 3,
+		.pre_div_width = 2,
+	},
+	.s = {
+		.src_sel_shift = 0,
+		.parent_map = gcc_cxo_pll8_map,
+	},
+	.freq_tbl = clk_tbl_gsbi_qup,
+	.clkr = {
+		.enable_reg = 0x2a4c,
+		.enable_mask = BIT(11),
+		.hw.init = &(struct clk_init_data){
+			.name = "gsbi5_qup_src",
+			.parent_names = gcc_cxo_pll8,
+			.num_parents = 2,
+			.ops = &clk_rcg_ops,
+			.flags = CLK_SET_PARENT_GATE,
+		},
+	},
+};
+
+static struct clk_branch gsbi5_qup_clk = {
+	.halt_reg = 0x2fd0,
+	.halt_bit = 20,
+	.clkr = {
+		.enable_reg = 0x2a4c,
+		.enable_mask = BIT(9),
+		.hw.init = &(struct clk_init_data){
+			.name = "gsbi5_qup_clk",
+			.parent_names = (const char *[]){ "gsbi5_qup_src" },
+			.num_parents = 1,
+			.ops = &clk_branch_ops,
+			.flags = CLK_SET_RATE_PARENT,
+		},
+	},
+};
+
+static const struct freq_tbl clk_tbl_gp[] = {
+	{ 9600000, P_CXO,  2, 0, 0 },
+	{ 19200000, P_CXO,  1, 0, 0 },
+	{ }
+};
+
+static struct clk_rcg gp0_src = {
+	.ns_reg = 0x2d24,
+	.md_reg = 0x2d00,
+	.mn = {
+		.mnctr_en_bit = 8,
+		.mnctr_reset_bit = 7,
+		.mnctr_mode_shift = 5,
+		.n_val_shift = 16,
+		.m_val_shift = 16,
+		.width = 8,
+	},
+	.p = {
+		.pre_div_shift = 3,
+		.pre_div_width = 2,
+	},
+	.s = {
+		.src_sel_shift = 0,
+		.parent_map = gcc_cxo_map,
+	},
+	.freq_tbl = clk_tbl_gp,
+	.clkr = {
+		.enable_reg = 0x2d24,
+		.enable_mask = BIT(11),
+		.hw.init = &(struct clk_init_data){
+			.name = "gp0_src",
+			.parent_names = gcc_cxo,
+			.num_parents = 1,
+			.ops = &clk_rcg_ops,
+			.flags = CLK_SET_PARENT_GATE,
+		},
+	}
+};
+
+static struct clk_branch gp0_clk = {
+	.halt_reg = 0x2fd8,
+	.halt_bit = 7,
+	.clkr = {
+		.enable_reg = 0x2d24,
+		.enable_mask = BIT(9),
+		.hw.init = &(struct clk_init_data){
+			.name = "gp0_clk",
+			.parent_names = (const char *[]){ "gp0_src" },
+			.num_parents = 1,
+			.ops = &clk_branch_ops,
+			.flags = CLK_SET_RATE_PARENT,
+		},
+	},
+};
+
+static struct clk_rcg gp1_src = {
+	.ns_reg = 0x2d44,
+	.md_reg = 0x2d40,
+	.mn = {
+		.mnctr_en_bit = 8,
+		.mnctr_reset_bit = 7,
+		.mnctr_mode_shift = 5,
+		.n_val_shift = 16,
+		.m_val_shift = 16,
+		.width = 8,
+	},
+	.p = {
+		.pre_div_shift = 3,
+		.pre_div_width = 2,
+	},
+	.s = {
+		.src_sel_shift = 0,
+		.parent_map = gcc_cxo_map,
+	},
+	.freq_tbl = clk_tbl_gp,
+	.clkr = {
+		.enable_reg = 0x2d44,
+		.enable_mask = BIT(11),
+		.hw.init = &(struct clk_init_data){
+			.name = "gp1_src",
+			.parent_names = gcc_cxo,
+			.num_parents = 1,
+			.ops = &clk_rcg_ops,
+			.flags = CLK_SET_RATE_GATE,
+		},
+	}
+};
+
+static struct clk_branch gp1_clk = {
+	.halt_reg = 0x2fd8,
+	.halt_bit = 6,
+	.clkr = {
+		.enable_reg = 0x2d44,
+		.enable_mask = BIT(9),
+		.hw.init = &(struct clk_init_data){
+			.name = "gp1_clk",
+			.parent_names = (const char *[]){ "gp1_src" },
+			.num_parents = 1,
+			.ops = &clk_branch_ops,
+			.flags = CLK_SET_RATE_PARENT,
+		},
+	},
+};
+
+static struct clk_rcg gp2_src = {
+	.ns_reg = 0x2d64,
+	.md_reg = 0x2d60,
+	.mn = {
+		.mnctr_en_bit = 8,
+		.mnctr_reset_bit = 7,
+		.mnctr_mode_shift = 5,
+		.n_val_shift = 16,
+		.m_val_shift = 16,
+		.width = 8,
+	},
+	.p = {
+		.pre_div_shift = 3,
+		.pre_div_width = 2,
+	},
+	.s = {
+		.src_sel_shift = 0,
+		.parent_map = gcc_cxo_map,
+	},
+	.freq_tbl = clk_tbl_gp,
+	.clkr = {
+		.enable_reg = 0x2d64,
+		.enable_mask = BIT(11),
+		.hw.init = &(struct clk_init_data){
+			.name = "gp2_src",
+			.parent_names = gcc_cxo,
+			.num_parents = 1,
+			.ops = &clk_rcg_ops,
+			.flags = CLK_SET_RATE_GATE,
+		},
+	}
+};
+
+static struct clk_branch gp2_clk = {
+	.halt_reg = 0x2fd8,
+	.halt_bit = 5,
+	.clkr = {
+		.enable_reg = 0x2d64,
+		.enable_mask = BIT(9),
+		.hw.init = &(struct clk_init_data){
+			.name = "gp2_clk",
+			.parent_names = (const char *[]){ "gp2_src" },
+			.num_parents = 1,
+			.ops = &clk_branch_ops,
+			.flags = CLK_SET_RATE_PARENT,
+		},
+	},
+};
+
+static struct clk_branch pmem_clk = {
+	.hwcg_reg = 0x25a0,
+	.hwcg_bit = 6,
+	.halt_reg = 0x2fc8,
+	.halt_bit = 20,
+	.clkr = {
+		.enable_reg = 0x25a0,
+		.enable_mask = BIT(4),
+		.hw.init = &(struct clk_init_data){
+			.name = "pmem_clk",
+			.ops = &clk_branch_ops,
+		},
+	},
+};
+
+static struct clk_rcg prng_src = {
+	.ns_reg = 0x2e80,
+	.p = {
+		.pre_div_shift = 3,
+		.pre_div_width = 4,
+	},
+	.s = {
+		.src_sel_shift = 0,
+		.parent_map = gcc_cxo_pll8_map,
+	},
+	.clkr = {
+		.hw.init = &(struct clk_init_data){
+			.name = "prng_src",
+			.parent_names = gcc_cxo_pll8,
+			.num_parents = 2,
+			.ops = &clk_rcg_ops,
+		},
+	},
+};
+
+static struct clk_branch prng_clk = {
+	.halt_reg = 0x2fd8,
+	.halt_check = BRANCH_HALT_VOTED,
+	.halt_bit = 10,
+	.clkr = {
+		.enable_reg = 0x3080,
+		.enable_mask = BIT(10),
+		.hw.init = &(struct clk_init_data){
+			.name = "prng_clk",
+			.parent_names = (const char *[]){ "prng_src" },
+			.num_parents = 1,
+			.ops = &clk_branch_ops,
+		},
+	},
+};
+
+static const struct freq_tbl clk_tbl_sdc[] = {
+	{    144000, P_CXO,   1, 1, 133 },
+	{    400000, P_PLL8,  4, 1, 240 },
+	{  16000000, P_PLL8,  4, 1,   6 },
+	{  17070000, P_PLL8,  1, 2,  45 },
+	{  20210000, P_PLL8,  1, 1,  19 },
+	{  24000000, P_PLL8,  4, 1,   4 },
+	{  38400000, P_PLL8,  2, 1,   5 },
+	{  48000000, P_PLL8,  4, 1,   2 },
+	{  64000000, P_PLL8,  3, 1,   2 },
+	{  76800000, P_PLL8,  1, 1,   5 },
+	{ }
+};
+
+static struct clk_rcg sdc1_src = {
+	.ns_reg = 0x282c,
+	.md_reg = 0x2828,
+	.mn = {
+		.mnctr_en_bit = 8,
+		.mnctr_reset_bit = 7,
+		.mnctr_mode_shift = 5,
+		.n_val_shift = 16,
+		.m_val_shift = 16,
+		.width = 8,
+	},
+	.p = {
+		.pre_div_shift = 3,
+		.pre_div_width = 2,
+	},
+	.s = {
+		.src_sel_shift = 0,
+		.parent_map = gcc_cxo_pll8_map,
+	},
+	.freq_tbl = clk_tbl_sdc,
+	.clkr = {
+		.enable_reg = 0x282c,
+		.enable_mask = BIT(11),
+		.hw.init = &(struct clk_init_data){
+			.name = "sdc1_src",
+			.parent_names = gcc_cxo_pll8,
+			.num_parents = 2,
+			.ops = &clk_rcg_ops,
+			.flags = CLK_SET_RATE_GATE,
+		},
+	}
+};
+
+static struct clk_branch sdc1_clk = {
+	.halt_reg = 0x2fc8,
+	.halt_bit = 6,
+	.clkr = {
+		.enable_reg = 0x282c,
+		.enable_mask = BIT(9),
+		.hw.init = &(struct clk_init_data){
+			.name = "sdc1_clk",
+			.parent_names = (const char *[]){ "sdc1_src" },
+			.num_parents = 1,
+			.ops = &clk_branch_ops,
+			.flags = CLK_SET_RATE_PARENT,
+		},
+	},
+};
+
+static struct clk_rcg sdc2_src = {
+	.ns_reg = 0x284c,
+	.md_reg = 0x2848,
+	.mn = {
+		.mnctr_en_bit = 8,
+		.mnctr_reset_bit = 7,
+		.mnctr_mode_shift = 5,
+		.n_val_shift = 16,
+		.m_val_shift = 16,
+		.width = 8,
+	},
+	.p = {
+		.pre_div_shift = 3,
+		.pre_div_width = 2,
+	},
+	.s = {
+		.src_sel_shift = 0,
+		.parent_map = gcc_cxo_pll8_map,
+	},
+	.freq_tbl = clk_tbl_sdc,
+	.clkr = {
+		.enable_reg = 0x284c,
+		.enable_mask = BIT(11),
+		.hw.init = &(struct clk_init_data){
+			.name = "sdc2_src",
+			.parent_names = gcc_cxo_pll8,
+			.num_parents = 2,
+			.ops = &clk_rcg_ops,
+			.flags = CLK_SET_RATE_GATE,
+		},
+	}
+};
+
+static struct clk_branch sdc2_clk = {
+	.halt_reg = 0x2fc8,
+	.halt_bit = 5,
+	.clkr = {
+		.enable_reg = 0x284c,
+		.enable_mask = BIT(9),
+		.hw.init = &(struct clk_init_data){
+			.name = "sdc2_clk",
+			.parent_names = (const char *[]){ "sdc2_src" },
+			.num_parents = 1,
+			.ops = &clk_branch_ops,
+			.flags = CLK_SET_RATE_PARENT,
+		},
+	},
+};
+
+static const struct freq_tbl clk_tbl_usb[] = {
+	{ 60000000, P_PLL8, 1, 5, 32 },
+	{ }
+};
+
+static struct clk_rcg usb_hs1_xcvr_src = {
+	.ns_reg = 0x290c,
+	.md_reg = 0x2908,
+	.mn = {
+		.mnctr_en_bit = 8,
+		.mnctr_reset_bit = 7,
+		.mnctr_mode_shift = 5,
+		.n_val_shift = 16,
+		.m_val_shift = 16,
+		.width = 8,
+	},
+	.p = {
+		.pre_div_shift = 3,
+		.pre_div_width = 2,
+	},
+	.s = {
+		.src_sel_shift = 0,
+		.parent_map = gcc_cxo_pll8_map,
+	},
+	.freq_tbl = clk_tbl_usb,
+	.clkr = {
+		.enable_reg = 0x290c,
+		.enable_mask = BIT(11),
+		.hw.init = &(struct clk_init_data){
+			.name = "usb_hs1_xcvr_src",
+			.parent_names = gcc_cxo_pll8,
+			.num_parents = 2,
+			.ops = &clk_rcg_ops,
+			.flags = CLK_SET_RATE_GATE,
+		},
+	}
+};
+
+static struct clk_branch usb_hs1_xcvr_clk = {
+	.halt_reg = 0x2fc8,
+	.halt_bit = 0,
+	.clkr = {
+		.enable_reg = 0x290c,
+		.enable_mask = BIT(9),
+		.hw.init = &(struct clk_init_data){
+			.name = "usb_hs1_xcvr_clk",
+			.parent_names = (const char *[]){ "usb_hs1_xcvr_src" },
+			.num_parents = 1,
+			.ops = &clk_branch_ops,
+			.flags = CLK_SET_RATE_PARENT,
+		},
+	},
+};
+
+static struct clk_rcg usb_hsic_xcvr_fs_src = {
+	.ns_reg = 0x2928,
+	.md_reg = 0x2924,
+	.mn = {
+		.mnctr_en_bit = 8,
+		.mnctr_reset_bit = 7,
+		.mnctr_mode_shift = 5,
+		.n_val_shift = 16,
+		.m_val_shift = 16,
+		.width = 8,
+	},
+	.p = {
+		.pre_div_shift = 3,
+		.pre_div_width = 2,
+	},
+	.s = {
+		.src_sel_shift = 0,
+		.parent_map = gcc_cxo_pll8_map,
+	},
+	.freq_tbl = clk_tbl_usb,
+	.clkr = {
+		.enable_reg = 0x2928,
+		.enable_mask = BIT(11),
+		.hw.init = &(struct clk_init_data){
+			.name = "usb_hsic_xcvr_fs_src",
+			.parent_names = gcc_cxo_pll8,
+			.num_parents = 2,
+			.ops = &clk_rcg_ops,
+			.flags = CLK_SET_RATE_GATE,
+		},
+	}
+};
+
+static struct clk_branch usb_hsic_xcvr_fs_clk = {
+	.halt_reg = 0x2fc8,
+	.halt_bit = 9,
+	.clkr = {
+		.enable_reg = 0x2928,
+		.enable_mask = BIT(9),
+		.hw.init = &(struct clk_init_data){
+			.name = "usb_hsic_xcvr_fs_clk",
+			.parent_names =
+				(const char *[]){ "usb_hsic_xcvr_fs_src" },
+			.num_parents = 1,
+			.ops = &clk_branch_ops,
+			.flags = CLK_SET_RATE_PARENT,
+		},
+	},
+};
+
+static const struct freq_tbl clk_tbl_usb_hs1_system[] = {
+	{ 60000000, P_PLL8, 1, 5, 32 },
+	{ }
+};
+
+static struct clk_rcg usb_hs1_system_src = {
+	.ns_reg = 0x36a4,
+	.md_reg = 0x36a0,
+	.mn = {
+		.mnctr_en_bit = 8,
+		.mnctr_reset_bit = 7,
+		.mnctr_mode_shift = 5,
+		.n_val_shift = 16,
+		.m_val_shift = 16,
+		.width = 8,
+	},
+	.p = {
+		.pre_div_shift = 3,
+		.pre_div_width = 2,
+	},
+	.s = {
+		.src_sel_shift = 0,
+		.parent_map = gcc_cxo_pll8_map,
+	},
+	.freq_tbl = clk_tbl_usb_hs1_system,
+	.clkr = {
+		.enable_reg = 0x36a4,
+		.enable_mask = BIT(11),
+		.hw.init = &(struct clk_init_data){
+			.name = "usb_hs1_system_src",
+			.parent_names = gcc_cxo_pll8,
+			.num_parents = 2,
+			.ops = &clk_rcg_ops,
+			.flags = CLK_SET_RATE_GATE,
+		},
+	}
+};
+
+static struct clk_branch usb_hs1_system_clk = {
+	.halt_reg = 0x2fc8,
+	.halt_bit = 4,
+	.clkr = {
+		.enable_reg = 0x36a4,
+		.enable_mask = BIT(9),
+		.hw.init = &(struct clk_init_data){
+			.parent_names =
+				(const char *[]){ "usb_hs1_system_src" },
+			.num_parents = 1,
+			.name = "usb_hs1_system_clk",
+			.ops = &clk_branch_ops,
+			.flags = CLK_SET_RATE_PARENT | CLK_IGNORE_UNUSED,
+		},
+	},
+};
+
+static const struct freq_tbl clk_tbl_usb_hsic_system[] = {
+	{ 64000000, P_PLL8, 1, 1, 6 },
+	{ }
+};
+
+static struct clk_rcg usb_hsic_system_src = {
+	.ns_reg = 0x2b58,
+	.md_reg = 0x2b54,
+	.mn = {
+		.mnctr_en_bit = 8,
+		.mnctr_reset_bit = 7,
+		.mnctr_mode_shift = 5,
+		.n_val_shift = 16,
+		.m_val_shift = 16,
+		.width = 8,
+	},
+	.p = {
+		.pre_div_shift = 3,
+		.pre_div_width = 2,
+	},
+	.s = {
+		.src_sel_shift = 0,
+		.parent_map = gcc_cxo_pll8_map,
+	},
+	.freq_tbl = clk_tbl_usb_hsic_system,
+	.clkr = {
+		.enable_reg = 0x2b58,
+		.enable_mask = BIT(11),
+		.hw.init = &(struct clk_init_data){
+			.name = "usb_hsic_system_src",
+			.parent_names = gcc_cxo_pll8,
+			.num_parents = 2,
+			.ops = &clk_rcg_ops,
+			.flags = CLK_SET_RATE_GATE,
+		},
+	}
+};
+
+static struct clk_branch usb_hsic_system_clk = {
+	.halt_reg = 0x2fc8,
+	.halt_bit = 7,
+	.clkr = {
+		.enable_reg = 0x2b58,
+		.enable_mask = BIT(9),
+		.hw.init = &(struct clk_init_data){
+			.parent_names =
+				(const char *[]){ "usb_hsic_system_src" },
+			.num_parents = 1,
+			.name = "usb_hsic_system_clk",
+			.ops = &clk_branch_ops,
+			.flags = CLK_SET_RATE_PARENT,
+		},
+	},
+};
+
+static const struct freq_tbl clk_tbl_usb_hsic_hsic[] = {
+	{ 48000000, P_PLL14, 1, 0, 0 },
+	{ }
+};
+
+static struct clk_rcg usb_hsic_hsic_src = {
+	.ns_reg = 0x2b50,
+	.md_reg = 0x2b4c,
+	.mn = {
+		.mnctr_en_bit = 8,
+		.mnctr_reset_bit = 7,
+		.mnctr_mode_shift = 5,
+		.n_val_shift = 16,
+		.m_val_shift = 16,
+		.width = 8,
+	},
+	.p = {
+		.pre_div_shift = 3,
+		.pre_div_width = 2,
+	},
+	.s = {
+		.src_sel_shift = 0,
+		.parent_map = gcc_cxo_pll14_map,
+	},
+	.freq_tbl = clk_tbl_usb_hsic_hsic,
+	.clkr = {
+		.enable_reg = 0x2b50,
+		.enable_mask = BIT(11),
+		.hw.init = &(struct clk_init_data){
+			.name = "usb_hsic_hsic_src",
+			.parent_names = gcc_cxo_pll14,
+			.num_parents = 2,
+			.ops = &clk_rcg_ops,
+			.flags = CLK_SET_RATE_GATE,
+		},
+	}
+};
+
+static struct clk_branch usb_hsic_hsic_clk = {
+	.halt_check = BRANCH_HALT_DELAY,
+	.clkr = {
+		.enable_reg = 0x2b50,
+		.enable_mask = BIT(9),
+		.hw.init = &(struct clk_init_data){
+			.parent_names = (const char *[]){ "usb_hsic_hsic_src" },
+			.num_parents = 1,
+			.name = "usb_hsic_hsic_clk",
+			.ops = &clk_branch_ops,
+			.flags = CLK_SET_RATE_PARENT,
+		},
+	},
+};
+
+static struct clk_branch usb_hsic_hsio_cal_clk = {
+	.halt_reg = 0x2fc8,
+	.halt_bit = 8,
+	.clkr = {
+		.enable_reg = 0x2b48,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.parent_names = (const char *[]){ "cxo" },
+			.num_parents = 1,
+			.name = "usb_hsic_hsio_cal_clk",
+			.ops = &clk_branch_ops,
+		},
+	},
+};
+
+static struct clk_branch ce1_core_clk = {
+	.hwcg_reg = 0x2724,
+	.hwcg_bit = 6,
+	.halt_reg = 0x2fd4,
+	.halt_bit = 27,
+	.clkr = {
+		.enable_reg = 0x2724,
+		.enable_mask = BIT(4),
+		.hw.init = &(struct clk_init_data){
+			.name = "ce1_core_clk",
+			.ops = &clk_branch_ops,
+		},
+	},
+};
+
+static struct clk_branch ce1_h_clk = {
+	.halt_reg = 0x2fd4,
+	.halt_bit = 1,
+	.clkr = {
+		.enable_reg = 0x2720,
+		.enable_mask = BIT(4),
+		.hw.init = &(struct clk_init_data){
+			.name = "ce1_h_clk",
+			.ops = &clk_branch_ops,
+		},
+	},
+};
+
+static struct clk_branch dma_bam_h_clk = {
+	.hwcg_reg = 0x25c0,
+	.hwcg_bit = 6,
+	.halt_reg = 0x2fc8,
+	.halt_bit = 12,
+	.clkr = {
+		.enable_reg = 0x25c0,
+		.enable_mask = BIT(4),
+		.hw.init = &(struct clk_init_data){
+			.name = "dma_bam_h_clk",
+			.ops = &clk_branch_ops,
+		},
+	},
+};
+
+static struct clk_branch gsbi1_h_clk = {
+	.hwcg_reg = 0x29c0,
+	.hwcg_bit = 6,
+	.halt_reg = 0x2fcc,
+	.halt_bit = 11,
+	.clkr = {
+		.enable_reg = 0x29c0,
+		.enable_mask = BIT(4),
+		.hw.init = &(struct clk_init_data){
+			.name = "gsbi1_h_clk",
+			.ops = &clk_branch_ops,
+		},
+	},
+};
+
+static struct clk_branch gsbi2_h_clk = {
+	.hwcg_reg = 0x29e0,
+	.hwcg_bit = 6,
+	.halt_reg = 0x2fcc,
+	.halt_bit = 7,
+	.clkr = {
+		.enable_reg = 0x29e0,
+		.enable_mask = BIT(4),
+		.hw.init = &(struct clk_init_data){
+			.name = "gsbi2_h_clk",
+			.ops = &clk_branch_ops,
+		},
+	},
+};
+
+static struct clk_branch gsbi3_h_clk = {
+	.hwcg_reg = 0x2a00,
+	.hwcg_bit = 6,
+	.halt_reg = 0x2fcc,
+	.halt_bit = 3,
+	.clkr = {
+		.enable_reg = 0x2a00,
+		.enable_mask = BIT(4),
+		.hw.init = &(struct clk_init_data){
+			.name = "gsbi3_h_clk",
+			.ops = &clk_branch_ops,
+		},
+	},
+};
+
+static struct clk_branch gsbi4_h_clk = {
+	.hwcg_reg = 0x2a20,
+	.hwcg_bit = 6,
+	.halt_reg = 0x2fd0,
+	.halt_bit = 27,
+	.clkr = {
+		.enable_reg = 0x2a20,
+		.enable_mask = BIT(4),
+		.hw.init = &(struct clk_init_data){
+			.name = "gsbi4_h_clk",
+			.ops = &clk_branch_ops,
+		},
+	},
+};
+
+static struct clk_branch gsbi5_h_clk = {
+	.hwcg_reg = 0x2a40,
+	.hwcg_bit = 6,
+	.halt_reg = 0x2fd0,
+	.halt_bit = 23,
+	.clkr = {
+		.enable_reg = 0x2a40,
+		.enable_mask = BIT(4),
+		.hw.init = &(struct clk_init_data){
+			.name = "gsbi5_h_clk",
+			.ops = &clk_branch_ops,
+		},
+	},
+};
+
+static struct clk_branch usb_hs1_h_clk = {
+	.hwcg_reg = 0x2900,
+	.hwcg_bit = 6,
+	.halt_reg = 0x2fc8,
+	.halt_bit = 1,
+	.clkr = {
+		.enable_reg = 0x2900,
+		.enable_mask = BIT(4),
+		.hw.init = &(struct clk_init_data){
+			.name = "usb_hs1_h_clk",
+			.ops = &clk_branch_ops,
+		},
+	},
+};
+
+static struct clk_branch usb_hsic_h_clk = {
+	.halt_reg = 0x2fcc,
+	.halt_bit = 28,
+	.clkr = {
+		.enable_reg = 0x2920,
+		.enable_mask = BIT(4),
+		.hw.init = &(struct clk_init_data){
+			.name = "usb_hsic_h_clk",
+			.ops = &clk_branch_ops,
+		},
+	},
+};
+
+static struct clk_branch sdc1_h_clk = {
+	.hwcg_reg = 0x2820,
+	.hwcg_bit = 6,
+	.halt_reg = 0x2fc8,
+	.halt_bit = 11,
+	.clkr = {
+		.enable_reg = 0x2820,
+		.enable_mask = BIT(4),
+		.hw.init = &(struct clk_init_data){
+			.name = "sdc1_h_clk",
+			.ops = &clk_branch_ops,
+		},
+	},
+};
+
+static struct clk_branch sdc2_h_clk = {
+	.hwcg_reg = 0x2840,
+	.hwcg_bit = 6,
+	.halt_reg = 0x2fc8,
+	.halt_bit = 10,
+	.clkr = {
+		.enable_reg = 0x2840,
+		.enable_mask = BIT(4),
+		.hw.init = &(struct clk_init_data){
+			.name = "sdc2_h_clk",
+			.ops = &clk_branch_ops,
+		},
+	},
+};
+
+static struct clk_branch adm0_clk = {
+	.halt_reg = 0x2fdc,
+	.halt_check = BRANCH_HALT_VOTED,
+	.halt_bit = 14,
+	.clkr = {
+		.enable_reg = 0x3080,
+		.enable_mask = BIT(2),
+		.hw.init = &(struct clk_init_data){
+			.name = "adm0_clk",
+			.ops = &clk_branch_ops,
+		},
+	},
+};
+
+static struct clk_branch adm0_pbus_clk = {
+	.hwcg_reg = 0x2208,
+	.hwcg_bit = 6,
+	.halt_reg = 0x2fdc,
+	.halt_check = BRANCH_HALT_VOTED,
+	.halt_bit = 13,
+	.clkr = {
+		.enable_reg = 0x3080,
+		.enable_mask = BIT(3),
+		.hw.init = &(struct clk_init_data){
+			.name = "adm0_pbus_clk",
+			.ops = &clk_branch_ops,
+		},
+	},
+};
+
+static struct clk_branch pmic_arb0_h_clk = {
+	.halt_reg = 0x2fd8,
+	.halt_check = BRANCH_HALT_VOTED,
+	.halt_bit = 22,
+	.clkr = {
+		.enable_reg = 0x3080,
+		.enable_mask = BIT(8),
+		.hw.init = &(struct clk_init_data){
+			.name = "pmic_arb0_h_clk",
+			.ops = &clk_branch_ops,
+		},
+	},
+};
+
+static struct clk_branch pmic_arb1_h_clk = {
+	.halt_reg = 0x2fd8,
+	.halt_check = BRANCH_HALT_VOTED,
+	.halt_bit = 21,
+	.clkr = {
+		.enable_reg = 0x3080,
+		.enable_mask = BIT(9),
+		.hw.init = &(struct clk_init_data){
+			.name = "pmic_arb1_h_clk",
+			.ops = &clk_branch_ops,
+		},
+	},
+};
+
+static struct clk_branch pmic_ssbi2_clk = {
+	.halt_reg = 0x2fd8,
+	.halt_check = BRANCH_HALT_VOTED,
+	.halt_bit = 23,
+	.clkr = {
+		.enable_reg = 0x3080,
+		.enable_mask = BIT(7),
+		.hw.init = &(struct clk_init_data){
+			.name = "pmic_ssbi2_clk",
+			.ops = &clk_branch_ops,
+		},
+	},
+};
+
+static struct clk_branch rpm_msg_ram_h_clk = {
+	.hwcg_reg = 0x27e0,
+	.hwcg_bit = 6,
+	.halt_reg = 0x2fd8,
+	.halt_check = BRANCH_HALT_VOTED,
+	.halt_bit = 12,
+	.clkr = {
+		.enable_reg = 0x3080,
+		.enable_mask = BIT(6),
+		.hw.init = &(struct clk_init_data){
+			.name = "rpm_msg_ram_h_clk",
+			.ops = &clk_branch_ops,
+		},
+	},
+};
+
+static struct clk_hw *gcc_mdm9615_hws[] = {
+	&cxo.hw,
+};
+
+static struct clk_regmap *gcc_mdm9615_clks[] = {
+	[PLL0] = &pll0.clkr,
+	[PLL0_VOTE] = &pll0_vote,
+	[PLL4_VOTE] = &pll4_vote,
+	[PLL8] = &pll8.clkr,
+	[PLL8_VOTE] = &pll8_vote,
+	[PLL14] = &pll14.clkr,
+	[PLL14_VOTE] = &pll14_vote,
+	[GSBI1_UART_SRC] = &gsbi1_uart_src.clkr,
+	[GSBI1_UART_CLK] = &gsbi1_uart_clk.clkr,
+	[GSBI2_UART_SRC] = &gsbi2_uart_src.clkr,
+	[GSBI2_UART_CLK] = &gsbi2_uart_clk.clkr,
+	[GSBI3_UART_SRC] = &gsbi3_uart_src.clkr,
+	[GSBI3_UART_CLK] = &gsbi3_uart_clk.clkr,
+	[GSBI4_UART_SRC] = &gsbi4_uart_src.clkr,
+	[GSBI4_UART_CLK] = &gsbi4_uart_clk.clkr,
+	[GSBI5_UART_SRC] = &gsbi5_uart_src.clkr,
+	[GSBI5_UART_CLK] = &gsbi5_uart_clk.clkr,
+	[GSBI1_QUP_SRC] = &gsbi1_qup_src.clkr,
+	[GSBI1_QUP_CLK] = &gsbi1_qup_clk.clkr,
+	[GSBI2_QUP_SRC] = &gsbi2_qup_src.clkr,
+	[GSBI2_QUP_CLK] = &gsbi2_qup_clk.clkr,
+	[GSBI3_QUP_SRC] = &gsbi3_qup_src.clkr,
+	[GSBI3_QUP_CLK] = &gsbi3_qup_clk.clkr,
+	[GSBI4_QUP_SRC] = &gsbi4_qup_src.clkr,
+	[GSBI4_QUP_CLK] = &gsbi4_qup_clk.clkr,
+	[GSBI5_QUP_SRC] = &gsbi5_qup_src.clkr,
+	[GSBI5_QUP_CLK] = &gsbi5_qup_clk.clkr,
+	[GP0_SRC] = &gp0_src.clkr,
+	[GP0_CLK] = &gp0_clk.clkr,
+	[GP1_SRC] = &gp1_src.clkr,
+	[GP1_CLK] = &gp1_clk.clkr,
+	[GP2_SRC] = &gp2_src.clkr,
+	[GP2_CLK] = &gp2_clk.clkr,
+	[PMEM_A_CLK] = &pmem_clk.clkr,
+	[PRNG_SRC] = &prng_src.clkr,
+	[PRNG_CLK] = &prng_clk.clkr,
+	[SDC1_SRC] = &sdc1_src.clkr,
+	[SDC1_CLK] = &sdc1_clk.clkr,
+	[SDC2_SRC] = &sdc2_src.clkr,
+	[SDC2_CLK] = &sdc2_clk.clkr,
+	[USB_HS1_XCVR_SRC] = &usb_hs1_xcvr_src.clkr,
+	[USB_HS1_XCVR_CLK] = &usb_hs1_xcvr_clk.clkr,
+	[USB_HS1_SYSTEM_CLK_SRC] = &usb_hs1_system_src.clkr,
+	[USB_HS1_SYSTEM_CLK] = &usb_hs1_system_clk.clkr,
+	[USB_HSIC_XCVR_FS_SRC] = &usb_hsic_xcvr_fs_src.clkr,
+	[USB_HSIC_XCVR_FS_CLK] = &usb_hsic_xcvr_fs_clk.clkr,
+	[USB_HSIC_SYSTEM_CLK_SRC] = &usb_hsic_system_src.clkr,
+	[USB_HSIC_SYSTEM_CLK] = &usb_hsic_system_clk.clkr,
+	[USB_HSIC_HSIC_CLK_SRC] = &usb_hsic_hsic_src.clkr,
+	[USB_HSIC_HSIC_CLK] = &usb_hsic_hsic_clk.clkr,
+	[USB_HSIC_HSIO_CAL_CLK] = &usb_hsic_hsio_cal_clk.clkr,
+	[CE1_CORE_CLK] = &ce1_core_clk.clkr,
+	[CE1_H_CLK] = &ce1_h_clk.clkr,
+	[DMA_BAM_H_CLK] = &dma_bam_h_clk.clkr,
+	[GSBI1_H_CLK] = &gsbi1_h_clk.clkr,
+	[GSBI2_H_CLK] = &gsbi2_h_clk.clkr,
+	[GSBI3_H_CLK] = &gsbi3_h_clk.clkr,
+	[GSBI4_H_CLK] = &gsbi4_h_clk.clkr,
+	[GSBI5_H_CLK] = &gsbi5_h_clk.clkr,
+	[USB_HS1_H_CLK] = &usb_hs1_h_clk.clkr,
+	[USB_HSIC_H_CLK] = &usb_hsic_h_clk.clkr,
+	[SDC1_H_CLK] = &sdc1_h_clk.clkr,
+	[SDC2_H_CLK] = &sdc2_h_clk.clkr,
+	[ADM0_CLK] = &adm0_clk.clkr,
+	[ADM0_PBUS_CLK] = &adm0_pbus_clk.clkr,
+	[PMIC_ARB0_H_CLK] = &pmic_arb0_h_clk.clkr,
+	[PMIC_ARB1_H_CLK] = &pmic_arb1_h_clk.clkr,
+	[PMIC_SSBI2_CLK] = &pmic_ssbi2_clk.clkr,
+	[RPM_MSG_RAM_H_CLK] = &rpm_msg_ram_h_clk.clkr,
+};
+
+static const struct qcom_reset_map gcc_mdm9615_resets[] = {
+	[DMA_BAM_RESET] = { 0x25c0, 7 },
+	[CE1_H_RESET] = { 0x2720, 7 },
+	[CE1_CORE_RESET] = { 0x2724, 7 },
+	[SDC1_RESET] = { 0x2830 },
+	[SDC2_RESET] = { 0x2850 },
+	[ADM0_C2_RESET] = { 0x220c, 4 },
+	[ADM0_C1_RESET] = { 0x220c, 3 },
+	[ADM0_C0_RESET] = { 0x220c, 2 },
+	[ADM0_PBUS_RESET] = { 0x220c, 1 },
+	[ADM0_RESET] = { 0x220c },
+	[USB_HS1_RESET] = { 0x2910 },
+	[USB_HSIC_RESET] = { 0x2934 },
+	[GSBI1_RESET] = { 0x29dc },
+	[GSBI2_RESET] = { 0x29fc },
+	[GSBI3_RESET] = { 0x2a1c },
+	[GSBI4_RESET] = { 0x2a3c },
+	[GSBI5_RESET] = { 0x2a5c },
+	[PDM_RESET] = { 0x2CC0, 12 },
+};
+
+static const struct regmap_config gcc_mdm9615_regmap_config = {
+	.reg_bits	= 32,
+	.reg_stride	= 4,
+	.val_bits	= 32,
+	.max_register	= 0x3660,
+	.fast_io	= true,
+};
+
+static const struct qcom_cc_desc gcc_mdm9615_desc = {
+	.config = &gcc_mdm9615_regmap_config,
+	.clks = gcc_mdm9615_clks,
+	.num_clks = ARRAY_SIZE(gcc_mdm9615_clks),
+	.resets = gcc_mdm9615_resets,
+	.num_resets = ARRAY_SIZE(gcc_mdm9615_resets),
+};
+
+static const struct of_device_id gcc_mdm9615_match_table[] = {
+	{ .compatible = "qcom,gcc-mdm9615" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, gcc_mdm9615_match_table);
+
+static int gcc_mdm9615_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct regmap *regmap;
+	int ret;
+	int i;
+
+	regmap = qcom_cc_map(pdev, &gcc_mdm9615_desc);
+	if (IS_ERR(regmap))
+		return PTR_ERR(regmap);
+
+	for (i = 0; i < ARRAY_SIZE(gcc_mdm9615_hws); i++) {
+		ret = devm_clk_hw_register(dev, gcc_mdm9615_hws[i]);
+		if (ret)
+			return ret;
+	}
+
+	return qcom_cc_really_probe(pdev, &gcc_mdm9615_desc, regmap);
+}
+
+static struct platform_driver gcc_mdm9615_driver = {
+	.probe		= gcc_mdm9615_probe,
+	.driver		= {
+		.name	= "gcc-mdm9615",
+		.of_match_table = gcc_mdm9615_match_table,
+	},
+};
+
+static int __init gcc_mdm9615_init(void)
+{
+	return platform_driver_register(&gcc_mdm9615_driver);
+}
+core_initcall(gcc_mdm9615_init);
+
+static void __exit gcc_mdm9615_exit(void)
+{
+	platform_driver_unregister(&gcc_mdm9615_driver);
+}
+module_exit(gcc_mdm9615_exit);
+
+MODULE_DESCRIPTION("QCOM GCC MDM9615 Driver");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:gcc-mdm9615");
diff --git a/drivers/clk/qcom/gcc-msm8996.c b/drivers/clk/qcom/gcc-msm8996.c
index bbf732bbc3fd..fe03e6fbc7df 100644
--- a/drivers/clk/qcom/gcc-msm8996.c
+++ b/drivers/clk/qcom/gcc-msm8996.c
@@ -2592,9 +2592,9 @@ static struct clk_branch gcc_pcie_2_aux_clk = {
 };
 
 static struct clk_branch gcc_pcie_2_pipe_clk = {
-	.halt_reg = 0x6e108,
+	.halt_reg = 0x6e018,
 	.clkr = {
-		.enable_reg = 0x6e108,
+		.enable_reg = 0x6e018,
 		.enable_mask = BIT(0),
 		.hw.init = &(struct clk_init_data){
 			.name = "gcc_pcie_2_pipe_clk",
@@ -3329,6 +3329,8 @@ static const struct qcom_reset_map gcc_msm8996_resets[] = {
 	[GCC_USB_20_BCR] = { 0x12000 },
 	[GCC_QUSB2PHY_PRIM_BCR] = { 0x12038 },
 	[GCC_QUSB2PHY_SEC_BCR] = { 0x1203c },
+	[GCC_USB3_PHY_BCR] = { 0x50020 },
+	[GCC_USB3PHY_PHY_BCR] = { 0x50024 },
 	[GCC_USB_PHY_CFG_AHB2PHY_BCR] = { 0x6a000 },
 	[GCC_SDCC1_BCR] = { 0x13000 },
 	[GCC_SDCC2_BCR] = { 0x14000 },
@@ -3404,6 +3406,8 @@ static const struct qcom_reset_map gcc_msm8996_resets[] = {
 	[GCC_PCIE_2_BCR] = { 0x6e000 },
 	[GCC_PCIE_2_PHY_BCR] = { 0x6e038 },
 	[GCC_PCIE_PHY_BCR] = { 0x6f000 },
+	[GCC_PCIE_PHY_COM_BCR] = { 0x6f014 },
+	[GCC_PCIE_PHY_COM_NOCSR_BCR] = { 0x6f00c },
 	[GCC_DCD_BCR] = { 0x70000 },
 	[GCC_OBT_ODT_BCR] = { 0x73000 },
 	[GCC_UFS_BCR] = { 0x75000 },
@@ -3447,9 +3451,8 @@ MODULE_DEVICE_TABLE(of, gcc_msm8996_match_table);
 
 static int gcc_msm8996_probe(struct platform_device *pdev)
 {
-	struct clk *clk;
 	struct device *dev = &pdev->dev;
-	int i;
+	int i, ret;
 	struct regmap *regmap;
 
 	regmap = qcom_cc_map(pdev, &gcc_msm8996_desc);
@@ -3463,9 +3466,9 @@ static int gcc_msm8996_probe(struct platform_device *pdev)
 	regmap_update_bits(regmap, 0x52008, BIT(21), BIT(21));
 
 	for (i = 0; i < ARRAY_SIZE(gcc_msm8996_hws); i++) {
-		clk = devm_clk_register(dev, gcc_msm8996_hws[i]);
-		if (IS_ERR(clk))
-			return PTR_ERR(clk);
+		ret = devm_clk_hw_register(dev, gcc_msm8996_hws[i]);
+		if (ret)
+			return ret;
 	}
 
 	return qcom_cc_really_probe(pdev, &gcc_msm8996_desc, regmap);
diff --git a/drivers/clk/qcom/lcc-mdm9615.c b/drivers/clk/qcom/lcc-mdm9615.c
new file mode 100644
index 000000000000..3237ef4c1197
--- /dev/null
+++ b/drivers/clk/qcom/lcc-mdm9615.c
@@ -0,0 +1,580 @@
+/*
+ * Copyright (c) 2014, The Linux Foundation. All rights reserved.
+ * Copyright (c) BayLibre, SAS.
+ * Author : Neil Armstrong <narmstrong@baylibre.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/kernel.h>
+#include <linux/bitops.h>
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/clk-provider.h>
+#include <linux/regmap.h>
+
+#include <dt-bindings/clock/qcom,lcc-mdm9615.h>
+
+#include "common.h"
+#include "clk-regmap.h"
+#include "clk-pll.h"
+#include "clk-rcg.h"
+#include "clk-branch.h"
+#include "clk-regmap-divider.h"
+#include "clk-regmap-mux.h"
+
+static struct clk_pll pll4 = {
+	.l_reg = 0x4,
+	.m_reg = 0x8,
+	.n_reg = 0xc,
+	.config_reg = 0x14,
+	.mode_reg = 0x0,
+	.status_reg = 0x18,
+	.status_bit = 16,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "pll4",
+		.parent_names = (const char *[]){ "cxo" },
+		.num_parents = 1,
+		.ops = &clk_pll_ops,
+	},
+};
+
+enum {
+	P_CXO,
+	P_PLL4,
+};
+
+static const struct parent_map lcc_cxo_pll4_map[] = {
+	{ P_CXO, 0 },
+	{ P_PLL4, 2 }
+};
+
+static const char * const lcc_cxo_pll4[] = {
+	"cxo",
+	"pll4_vote",
+};
+
+static struct freq_tbl clk_tbl_aif_osr_492[] = {
+	{   512000, P_PLL4, 4, 1, 240 },
+	{   768000, P_PLL4, 4, 1, 160 },
+	{  1024000, P_PLL4, 4, 1, 120 },
+	{  1536000, P_PLL4, 4, 1,  80 },
+	{  2048000, P_PLL4, 4, 1,  60 },
+	{  3072000, P_PLL4, 4, 1,  40 },
+	{  4096000, P_PLL4, 4, 1,  30 },
+	{  6144000, P_PLL4, 4, 1,  20 },
+	{  8192000, P_PLL4, 4, 1,  15 },
+	{ 12288000, P_PLL4, 4, 1,  10 },
+	{ 24576000, P_PLL4, 4, 1,   5 },
+	{ 27000000, P_CXO,  1, 0,   0 },
+	{ }
+};
+
+static struct freq_tbl clk_tbl_aif_osr_393[] = {
+	{   512000, P_PLL4, 4, 1, 192 },
+	{   768000, P_PLL4, 4, 1, 128 },
+	{  1024000, P_PLL4, 4, 1,  96 },
+	{  1536000, P_PLL4, 4, 1,  64 },
+	{  2048000, P_PLL4, 4, 1,  48 },
+	{  3072000, P_PLL4, 4, 1,  32 },
+	{  4096000, P_PLL4, 4, 1,  24 },
+	{  6144000, P_PLL4, 4, 1,  16 },
+	{  8192000, P_PLL4, 4, 1,  12 },
+	{ 12288000, P_PLL4, 4, 1,   8 },
+	{ 24576000, P_PLL4, 4, 1,   4 },
+	{ 27000000, P_CXO,  1, 0,   0 },
+	{ }
+};
+
+static struct clk_rcg mi2s_osr_src = {
+	.ns_reg = 0x48,
+	.md_reg = 0x4c,
+	.mn = {
+		.mnctr_en_bit = 8,
+		.mnctr_reset_bit = 7,
+		.mnctr_mode_shift = 5,
+		.n_val_shift = 24,
+		.m_val_shift = 8,
+		.width = 8,
+	},
+	.p = {
+		.pre_div_shift = 3,
+		.pre_div_width = 2,
+	},
+	.s = {
+		.src_sel_shift = 0,
+		.parent_map = lcc_cxo_pll4_map,
+	},
+	.freq_tbl = clk_tbl_aif_osr_393,
+	.clkr = {
+		.enable_reg = 0x48,
+		.enable_mask = BIT(9),
+		.hw.init = &(struct clk_init_data){
+			.name = "mi2s_osr_src",
+			.parent_names = lcc_cxo_pll4,
+			.num_parents = 2,
+			.ops = &clk_rcg_ops,
+			.flags = CLK_SET_RATE_GATE,
+		},
+	},
+};
+
+static const char * const lcc_mi2s_parents[] = {
+	"mi2s_osr_src",
+};
+
+static struct clk_branch mi2s_osr_clk = {
+	.halt_reg = 0x50,
+	.halt_bit = 1,
+	.halt_check = BRANCH_HALT_ENABLE,
+	.clkr = {
+		.enable_reg = 0x48,
+		.enable_mask = BIT(17),
+		.hw.init = &(struct clk_init_data){
+			.name = "mi2s_osr_clk",
+			.parent_names = lcc_mi2s_parents,
+			.num_parents = 1,
+			.ops = &clk_branch_ops,
+			.flags = CLK_SET_RATE_PARENT,
+		},
+	},
+};
+
+static struct clk_regmap_div mi2s_div_clk = {
+	.reg = 0x48,
+	.shift = 10,
+	.width = 4,
+	.clkr = {
+		.enable_reg = 0x48,
+		.enable_mask = BIT(15),
+		.hw.init = &(struct clk_init_data){
+			.name = "mi2s_div_clk",
+			.parent_names = lcc_mi2s_parents,
+			.num_parents = 1,
+			.ops = &clk_regmap_div_ops,
+		},
+	},
+};
+
+static struct clk_branch mi2s_bit_div_clk = {
+	.halt_reg = 0x50,
+	.halt_bit = 0,
+	.halt_check = BRANCH_HALT_ENABLE,
+	.clkr = {
+		.enable_reg = 0x48,
+		.enable_mask = BIT(15),
+		.hw.init = &(struct clk_init_data){
+			.name = "mi2s_bit_div_clk",
+			.parent_names = (const char *[]){ "mi2s_div_clk" },
+			.num_parents = 1,
+			.ops = &clk_branch_ops,
+			.flags = CLK_SET_RATE_PARENT,
+		},
+	},
+};
+
+static struct clk_regmap_mux mi2s_bit_clk = {
+	.reg = 0x48,
+	.shift = 14,
+	.width = 1,
+	.clkr = {
+		.hw.init = &(struct clk_init_data){
+			.name = "mi2s_bit_clk",
+			.parent_names = (const char *[]){
+				"mi2s_bit_div_clk",
+				"mi2s_codec_clk",
+			},
+			.num_parents = 2,
+			.ops = &clk_regmap_mux_closest_ops,
+			.flags = CLK_SET_RATE_PARENT,
+		},
+	},
+};
+
+#define CLK_AIF_OSR_DIV(prefix, _ns, _md, hr)			\
+static struct clk_rcg prefix##_osr_src = {			\
+	.ns_reg = _ns,						\
+	.md_reg = _md,						\
+	.mn = {							\
+		.mnctr_en_bit = 8,				\
+		.mnctr_reset_bit = 7,				\
+		.mnctr_mode_shift = 5,				\
+		.n_val_shift = 24,				\
+		.m_val_shift = 8,				\
+		.width = 8,					\
+	},							\
+	.p = {							\
+		.pre_div_shift = 3,				\
+		.pre_div_width = 2,				\
+	},							\
+	.s = {							\
+		.src_sel_shift = 0,				\
+		.parent_map = lcc_cxo_pll4_map,			\
+	},							\
+	.freq_tbl = clk_tbl_aif_osr_393,			\
+	.clkr = {						\
+		.enable_reg = _ns,				\
+		.enable_mask = BIT(9),				\
+		.hw.init = &(struct clk_init_data){		\
+			.name = #prefix "_osr_src",		\
+			.parent_names = lcc_cxo_pll4,		\
+			.num_parents = 2,			\
+			.ops = &clk_rcg_ops,			\
+			.flags = CLK_SET_RATE_GATE,		\
+		},						\
+	},							\
+};								\
+								\
+static const char * const lcc_##prefix##_parents[] = {		\
+	#prefix "_osr_src",					\
+};								\
+								\
+static struct clk_branch prefix##_osr_clk = {			\
+	.halt_reg = hr,						\
+	.halt_bit = 1,						\
+	.halt_check = BRANCH_HALT_ENABLE,			\
+	.clkr = {						\
+		.enable_reg = _ns,				\
+		.enable_mask = BIT(21),				\
+		.hw.init = &(struct clk_init_data){		\
+			.name = #prefix "_osr_clk",		\
+			.parent_names = lcc_##prefix##_parents,	\
+			.num_parents = 1,			\
+			.ops = &clk_branch_ops,			\
+			.flags = CLK_SET_RATE_PARENT,		\
+		},						\
+	},							\
+};								\
+								\
+static struct clk_regmap_div prefix##_div_clk = {		\
+	.reg = _ns,						\
+	.shift = 10,						\
+	.width = 8,						\
+	.clkr = {						\
+		.hw.init = &(struct clk_init_data){		\
+			.name = #prefix "_div_clk",		\
+			.parent_names = lcc_##prefix##_parents,	\
+			.num_parents = 1,			\
+			.ops = &clk_regmap_div_ops,		\
+		},						\
+	},							\
+};								\
+								\
+static struct clk_branch prefix##_bit_div_clk = {		\
+	.halt_reg = hr,						\
+	.halt_bit = 0,						\
+	.halt_check = BRANCH_HALT_ENABLE,			\
+	.clkr = {						\
+		.enable_reg = _ns,				\
+		.enable_mask = BIT(19),				\
+		.hw.init = &(struct clk_init_data){		\
+			.name = #prefix "_bit_div_clk",		\
+			.parent_names = (const char *[]){	\
+				#prefix "_div_clk"		\
+			},					\
+			.num_parents = 1,			\
+			.ops = &clk_branch_ops,			\
+			.flags = CLK_SET_RATE_PARENT,		\
+		},						\
+	},							\
+};								\
+								\
+static struct clk_regmap_mux prefix##_bit_clk = {		\
+	.reg = _ns,						\
+	.shift = 18,						\
+	.width = 1,						\
+	.clkr = {						\
+		.hw.init = &(struct clk_init_data){		\
+			.name = #prefix "_bit_clk",		\
+			.parent_names = (const char *[]){	\
+				#prefix "_bit_div_clk",		\
+				#prefix "_codec_clk",		\
+			},					\
+			.num_parents = 2,			\
+			.ops = &clk_regmap_mux_closest_ops,	\
+			.flags = CLK_SET_RATE_PARENT,		\
+		},						\
+	},							\
+}
+
+CLK_AIF_OSR_DIV(codec_i2s_mic, 0x60, 0x64, 0x68);
+CLK_AIF_OSR_DIV(spare_i2s_mic, 0x78, 0x7c, 0x80);
+CLK_AIF_OSR_DIV(codec_i2s_spkr, 0x6c, 0x70, 0x74);
+CLK_AIF_OSR_DIV(spare_i2s_spkr, 0x84, 0x88, 0x8c);
+
+static struct freq_tbl clk_tbl_pcm_492[] = {
+	{   256000, P_PLL4, 4, 1, 480 },
+	{   512000, P_PLL4, 4, 1, 240 },
+	{   768000, P_PLL4, 4, 1, 160 },
+	{  1024000, P_PLL4, 4, 1, 120 },
+	{  1536000, P_PLL4, 4, 1,  80 },
+	{  2048000, P_PLL4, 4, 1,  60 },
+	{  3072000, P_PLL4, 4, 1,  40 },
+	{  4096000, P_PLL4, 4, 1,  30 },
+	{  6144000, P_PLL4, 4, 1,  20 },
+	{  8192000, P_PLL4, 4, 1,  15 },
+	{ 12288000, P_PLL4, 4, 1,  10 },
+	{ 24576000, P_PLL4, 4, 1,   5 },
+	{ 27000000, P_CXO,  1, 0,   0 },
+	{ }
+};
+
+static struct freq_tbl clk_tbl_pcm_393[] = {
+	{   256000, P_PLL4, 4, 1, 384 },
+	{   512000, P_PLL4, 4, 1, 192 },
+	{   768000, P_PLL4, 4, 1, 128 },
+	{  1024000, P_PLL4, 4, 1,  96 },
+	{  1536000, P_PLL4, 4, 1,  64 },
+	{  2048000, P_PLL4, 4, 1,  48 },
+	{  3072000, P_PLL4, 4, 1,  32 },
+	{  4096000, P_PLL4, 4, 1,  24 },
+	{  6144000, P_PLL4, 4, 1,  16 },
+	{  8192000, P_PLL4, 4, 1,  12 },
+	{ 12288000, P_PLL4, 4, 1,   8 },
+	{ 24576000, P_PLL4, 4, 1,   4 },
+	{ 27000000, P_CXO,  1, 0,   0 },
+	{ }
+};
+
+static struct clk_rcg pcm_src = {
+	.ns_reg = 0x54,
+	.md_reg = 0x58,
+	.mn = {
+		.mnctr_en_bit = 8,
+		.mnctr_reset_bit = 7,
+		.mnctr_mode_shift = 5,
+		.n_val_shift = 16,
+		.m_val_shift = 16,
+		.width = 16,
+	},
+	.p = {
+		.pre_div_shift = 3,
+		.pre_div_width = 2,
+	},
+	.s = {
+		.src_sel_shift = 0,
+		.parent_map = lcc_cxo_pll4_map,
+	},
+	.freq_tbl = clk_tbl_pcm_393,
+	.clkr = {
+		.enable_reg = 0x54,
+		.enable_mask = BIT(9),
+		.hw.init = &(struct clk_init_data){
+			.name = "pcm_src",
+			.parent_names = lcc_cxo_pll4,
+			.num_parents = 2,
+			.ops = &clk_rcg_ops,
+			.flags = CLK_SET_RATE_GATE,
+		},
+	},
+};
+
+static struct clk_branch pcm_clk_out = {
+	.halt_reg = 0x5c,
+	.halt_bit = 0,
+	.halt_check = BRANCH_HALT_ENABLE,
+	.clkr = {
+		.enable_reg = 0x54,
+		.enable_mask = BIT(11),
+		.hw.init = &(struct clk_init_data){
+			.name = "pcm_clk_out",
+			.parent_names = (const char *[]){ "pcm_src" },
+			.num_parents = 1,
+			.ops = &clk_branch_ops,
+			.flags = CLK_SET_RATE_PARENT,
+		},
+	},
+};
+
+static struct clk_regmap_mux pcm_clk = {
+	.reg = 0x54,
+	.shift = 10,
+	.width = 1,
+	.clkr = {
+		.hw.init = &(struct clk_init_data){
+			.name = "pcm_clk",
+			.parent_names = (const char *[]){
+				"pcm_clk_out",
+				"pcm_codec_clk",
+			},
+			.num_parents = 2,
+			.ops = &clk_regmap_mux_closest_ops,
+			.flags = CLK_SET_RATE_PARENT,
+		},
+	},
+};
+
+static struct clk_rcg slimbus_src = {
+	.ns_reg = 0xcc,
+	.md_reg = 0xd0,
+	.mn = {
+		.mnctr_en_bit = 8,
+		.mnctr_reset_bit = 7,
+		.mnctr_mode_shift = 5,
+		.n_val_shift = 24,
+		.m_val_shift = 8,
+		.width = 8,
+	},
+	.p = {
+		.pre_div_shift = 3,
+		.pre_div_width = 2,
+	},
+	.s = {
+		.src_sel_shift = 0,
+		.parent_map = lcc_cxo_pll4_map,
+	},
+	.freq_tbl = clk_tbl_aif_osr_393,
+	.clkr = {
+		.enable_reg = 0xcc,
+		.enable_mask = BIT(9),
+		.hw.init = &(struct clk_init_data){
+			.name = "slimbus_src",
+			.parent_names = lcc_cxo_pll4,
+			.num_parents = 2,
+			.ops = &clk_rcg_ops,
+			.flags = CLK_SET_RATE_GATE,
+		},
+	},
+};
+
+static const char * const lcc_slimbus_parents[] = {
+	"slimbus_src",
+};
+
+static struct clk_branch audio_slimbus_clk = {
+	.halt_reg = 0xd4,
+	.halt_bit = 0,
+	.halt_check = BRANCH_HALT_ENABLE,
+	.clkr = {
+		.enable_reg = 0xcc,
+		.enable_mask = BIT(10),
+		.hw.init = &(struct clk_init_data){
+			.name = "audio_slimbus_clk",
+			.parent_names = lcc_slimbus_parents,
+			.num_parents = 1,
+			.ops = &clk_branch_ops,
+			.flags = CLK_SET_RATE_PARENT,
+		},
+	},
+};
+
+static struct clk_branch sps_slimbus_clk = {
+	.halt_reg = 0xd4,
+	.halt_bit = 1,
+	.halt_check = BRANCH_HALT_ENABLE,
+	.clkr = {
+		.enable_reg = 0xcc,
+		.enable_mask = BIT(12),
+		.hw.init = &(struct clk_init_data){
+			.name = "sps_slimbus_clk",
+			.parent_names = lcc_slimbus_parents,
+			.num_parents = 1,
+			.ops = &clk_branch_ops,
+			.flags = CLK_SET_RATE_PARENT,
+		},
+	},
+};
+
+static struct clk_regmap *lcc_mdm9615_clks[] = {
+	[PLL4] = &pll4.clkr,
+	[MI2S_OSR_SRC] = &mi2s_osr_src.clkr,
+	[MI2S_OSR_CLK] = &mi2s_osr_clk.clkr,
+	[MI2S_DIV_CLK] = &mi2s_div_clk.clkr,
+	[MI2S_BIT_DIV_CLK] = &mi2s_bit_div_clk.clkr,
+	[MI2S_BIT_CLK] = &mi2s_bit_clk.clkr,
+	[PCM_SRC] = &pcm_src.clkr,
+	[PCM_CLK_OUT] = &pcm_clk_out.clkr,
+	[PCM_CLK] = &pcm_clk.clkr,
+	[SLIMBUS_SRC] = &slimbus_src.clkr,
+	[AUDIO_SLIMBUS_CLK] = &audio_slimbus_clk.clkr,
+	[SPS_SLIMBUS_CLK] = &sps_slimbus_clk.clkr,
+	[CODEC_I2S_MIC_OSR_SRC] = &codec_i2s_mic_osr_src.clkr,
+	[CODEC_I2S_MIC_OSR_CLK] = &codec_i2s_mic_osr_clk.clkr,
+	[CODEC_I2S_MIC_DIV_CLK] = &codec_i2s_mic_div_clk.clkr,
+	[CODEC_I2S_MIC_BIT_DIV_CLK] = &codec_i2s_mic_bit_div_clk.clkr,
+	[CODEC_I2S_MIC_BIT_CLK] = &codec_i2s_mic_bit_clk.clkr,
+	[SPARE_I2S_MIC_OSR_SRC] = &spare_i2s_mic_osr_src.clkr,
+	[SPARE_I2S_MIC_OSR_CLK] = &spare_i2s_mic_osr_clk.clkr,
+	[SPARE_I2S_MIC_DIV_CLK] = &spare_i2s_mic_div_clk.clkr,
+	[SPARE_I2S_MIC_BIT_DIV_CLK] = &spare_i2s_mic_bit_div_clk.clkr,
+	[SPARE_I2S_MIC_BIT_CLK] = &spare_i2s_mic_bit_clk.clkr,
+	[CODEC_I2S_SPKR_OSR_SRC] = &codec_i2s_spkr_osr_src.clkr,
+	[CODEC_I2S_SPKR_OSR_CLK] = &codec_i2s_spkr_osr_clk.clkr,
+	[CODEC_I2S_SPKR_DIV_CLK] = &codec_i2s_spkr_div_clk.clkr,
+	[CODEC_I2S_SPKR_BIT_DIV_CLK] = &codec_i2s_spkr_bit_div_clk.clkr,
+	[CODEC_I2S_SPKR_BIT_CLK] = &codec_i2s_spkr_bit_clk.clkr,
+	[SPARE_I2S_SPKR_OSR_SRC] = &spare_i2s_spkr_osr_src.clkr,
+	[SPARE_I2S_SPKR_OSR_CLK] = &spare_i2s_spkr_osr_clk.clkr,
+	[SPARE_I2S_SPKR_DIV_CLK] = &spare_i2s_spkr_div_clk.clkr,
+	[SPARE_I2S_SPKR_BIT_DIV_CLK] = &spare_i2s_spkr_bit_div_clk.clkr,
+	[SPARE_I2S_SPKR_BIT_CLK] = &spare_i2s_spkr_bit_clk.clkr,
+};
+
+static const struct regmap_config lcc_mdm9615_regmap_config = {
+	.reg_bits	= 32,
+	.reg_stride	= 4,
+	.val_bits	= 32,
+	.max_register	= 0xfc,
+	.fast_io	= true,
+};
+
+static const struct qcom_cc_desc lcc_mdm9615_desc = {
+	.config = &lcc_mdm9615_regmap_config,
+	.clks = lcc_mdm9615_clks,
+	.num_clks = ARRAY_SIZE(lcc_mdm9615_clks),
+};
+
+static const struct of_device_id lcc_mdm9615_match_table[] = {
+	{ .compatible = "qcom,lcc-mdm9615" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, lcc_mdm9615_match_table);
+
+static int lcc_mdm9615_probe(struct platform_device *pdev)
+{
+	u32 val;
+	struct regmap *regmap;
+
+	regmap = qcom_cc_map(pdev, &lcc_mdm9615_desc);
+	if (IS_ERR(regmap))
+		return PTR_ERR(regmap);
+
+	/* Use the correct frequency plan depending on speed of PLL4 */
+	regmap_read(regmap, 0x4, &val);
+	if (val == 0x12) {
+		slimbus_src.freq_tbl = clk_tbl_aif_osr_492;
+		mi2s_osr_src.freq_tbl = clk_tbl_aif_osr_492;
+		codec_i2s_mic_osr_src.freq_tbl = clk_tbl_aif_osr_492;
+		spare_i2s_mic_osr_src.freq_tbl = clk_tbl_aif_osr_492;
+		codec_i2s_spkr_osr_src.freq_tbl = clk_tbl_aif_osr_492;
+		spare_i2s_spkr_osr_src.freq_tbl = clk_tbl_aif_osr_492;
+		pcm_src.freq_tbl = clk_tbl_pcm_492;
+	}
+	/* Enable PLL4 source on the LPASS Primary PLL Mux */
+	regmap_write(regmap, 0xc4, 0x1);
+
+	return qcom_cc_really_probe(pdev, &lcc_mdm9615_desc, regmap);
+}
+
+static struct platform_driver lcc_mdm9615_driver = {
+	.probe		= lcc_mdm9615_probe,
+	.driver		= {
+		.name	= "lcc-mdm9615",
+		.of_match_table = lcc_mdm9615_match_table,
+	},
+};
+module_platform_driver(lcc_mdm9615_driver);
+
+MODULE_DESCRIPTION("QCOM LCC MDM9615 Driver");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:lcc-mdm9615");
diff --git a/drivers/clk/qcom/mmcc-msm8996.c b/drivers/clk/qcom/mmcc-msm8996.c
index 847dd9dadeca..ca97e1151797 100644
--- a/drivers/clk/qcom/mmcc-msm8996.c
+++ b/drivers/clk/qcom/mmcc-msm8996.c
@@ -2888,6 +2888,14 @@ static struct clk_hw *mmcc_msm8996_hws[] = {
 	&gpll0_div.hw,
 };
 
+static struct gdsc mmagic_bimc_gdsc = {
+	.gdscr = 0x529c,
+	.pd = {
+		.name = "mmagic_bimc",
+	},
+	.pwrsts = PWRSTS_OFF_ON,
+};
+
 static struct gdsc mmagic_video_gdsc = {
 	.gdscr = 0x119c,
 	.gds_hw_ctrl = 0x120c,
@@ -3201,6 +3209,7 @@ static struct clk_regmap *mmcc_msm8996_clocks[] = {
 };
 
 static struct gdsc *mmcc_msm8996_gdscs[] = {
+	[MMAGIC_BIMC_GDSC] = &mmagic_bimc_gdsc,
 	[MMAGIC_VIDEO_GDSC] = &mmagic_video_gdsc,
 	[MMAGIC_MDSS_GDSC] = &mmagic_mdss_gdsc,
 	[MMAGIC_CAMSS_GDSC] = &mmagic_camss_gdsc,
@@ -3305,9 +3314,8 @@ MODULE_DEVICE_TABLE(of, mmcc_msm8996_match_table);
 
 static int mmcc_msm8996_probe(struct platform_device *pdev)
 {
-	struct clk *clk;
 	struct device *dev = &pdev->dev;
-	int i;
+	int i, ret;
 	struct regmap *regmap;
 
 	regmap = qcom_cc_map(pdev, &mmcc_msm8996_desc);
@@ -3320,9 +3328,9 @@ static int mmcc_msm8996_probe(struct platform_device *pdev)
 	regmap_update_bits(regmap, 0x5054, BIT(15), 0);
 
 	for (i = 0; i < ARRAY_SIZE(mmcc_msm8996_hws); i++) {
-		clk = devm_clk_register(dev, mmcc_msm8996_hws[i]);
-		if (IS_ERR(clk))
-			return PTR_ERR(clk);
+		ret = devm_clk_hw_register(dev, mmcc_msm8996_hws[i]);
+		if (ret)
+			return ret;
 	}
 
 	return qcom_cc_really_probe(pdev, &mmcc_msm8996_desc, regmap);
diff --git a/drivers/clk/renesas/clk-mstp.c b/drivers/clk/renesas/clk-mstp.c
index 5093a250650d..9375777776d9 100644
--- a/drivers/clk/renesas/clk-mstp.c
+++ b/drivers/clk/renesas/clk-mstp.c
@@ -167,7 +167,7 @@ static void __init cpg_mstp_clocks_init(struct device_node *np)
 	unsigned int i;
 
 	group = kzalloc(sizeof(*group), GFP_KERNEL);
-	clks = kmalloc(MSTP_MAX_CLOCKS * sizeof(*clks), GFP_KERNEL);
+	clks = kmalloc_array(MSTP_MAX_CLOCKS, sizeof(*clks), GFP_KERNEL);
 	if (group == NULL || clks == NULL) {
 		kfree(group);
 		kfree(clks);
diff --git a/drivers/clk/renesas/clk-rz.c b/drivers/clk/renesas/clk-rz.c
index f6312c62f16b..5adb934326d1 100644
--- a/drivers/clk/renesas/clk-rz.c
+++ b/drivers/clk/renesas/clk-rz.c
@@ -25,10 +25,31 @@ struct rz_cpg {
 #define CPG_FRQCR	0x10
 #define CPG_FRQCR2	0x14
 
+#define PPR0		0xFCFE3200
+#define PIBC0		0xFCFE7000
+
+#define MD_CLK(x)	((x >> 2) & 1)	/* P0_2 */
+
 /* -----------------------------------------------------------------------------
  * Initialization
  */
 
+static u16 __init rz_cpg_read_mode_pins(void)
+{
+	void __iomem *ppr0, *pibc0;
+	u16 modes;
+
+	ppr0 = ioremap_nocache(PPR0, 2);
+	pibc0 = ioremap_nocache(PIBC0, 2);
+	BUG_ON(!ppr0 || !pibc0);
+	iowrite16(4, pibc0);	/* enable input buffer */
+	modes = ioread16(ppr0);
+	iounmap(ppr0);
+	iounmap(pibc0);
+
+	return modes;
+}
+
 static struct clk * __init
 rz_cpg_register_clock(struct device_node *np, struct rz_cpg *cpg, const char *name)
 {
@@ -37,8 +58,7 @@ rz_cpg_register_clock(struct device_node *np, struct rz_cpg *cpg, const char *na
 	static const unsigned frqcr_tab[4] = { 3, 2, 0, 1 };
 
 	if (strcmp(name, "pll") == 0) {
-		/* FIXME: cpg_mode should be read from GPIO. But no GPIO support yet */
-		unsigned cpg_mode = 0; /* hardcoded to EXTAL for now */
+		unsigned int cpg_mode = MD_CLK(rz_cpg_read_mode_pins());
 		const char *parent_name = of_clk_get_parent_name(np, cpg_mode);
 
 		mult = cpg_mode ? (32 / 4) : 30;
diff --git a/drivers/clk/renesas/r8a7795-cpg-mssr.c b/drivers/clk/renesas/r8a7795-cpg-mssr.c
index e38bf60c0ff4..f255e451e8ca 100644
--- a/drivers/clk/renesas/r8a7795-cpg-mssr.c
+++ b/drivers/clk/renesas/r8a7795-cpg-mssr.c
@@ -123,6 +123,10 @@ static const struct mssr_mod_clk r8a7795_mod_clks[] __initconst = {
 	DEF_MOD("sys-dmac2",		 217,	R8A7795_CLK_S3D1),
 	DEF_MOD("sys-dmac1",		 218,	R8A7795_CLK_S3D1),
 	DEF_MOD("sys-dmac0",		 219,	R8A7795_CLK_S3D1),
+	DEF_MOD("cmt3",			 300,	R8A7795_CLK_R),
+	DEF_MOD("cmt2",			 301,	R8A7795_CLK_R),
+	DEF_MOD("cmt1",			 302,	R8A7795_CLK_R),
+	DEF_MOD("cmt0",			 303,	R8A7795_CLK_R),
 	DEF_MOD("scif2",		 310,	R8A7795_CLK_S3D4),
 	DEF_MOD("sdif3",		 311,	R8A7795_CLK_SD3),
 	DEF_MOD("sdif2",		 312,	R8A7795_CLK_SD2),
diff --git a/drivers/clk/renesas/r8a7796-cpg-mssr.c b/drivers/clk/renesas/r8a7796-cpg-mssr.c
index c84b549c14d2..eb347ed265f2 100644
--- a/drivers/clk/renesas/r8a7796-cpg-mssr.c
+++ b/drivers/clk/renesas/r8a7796-cpg-mssr.c
@@ -45,6 +45,7 @@ enum clk_ids {
 	CLK_S3,
 	CLK_SDSRC,
 	CLK_SSPSRC,
+	CLK_RINT,
 
 	/* Module Clocks */
 	MOD_CLK_BASE
@@ -69,6 +70,7 @@ static const struct cpg_core_clk r8a7796_core_clks[] __initconst = {
 	DEF_FIXED(".s1",        CLK_S1,            CLK_PLL1_DIV2,  3, 1),
 	DEF_FIXED(".s2",        CLK_S2,            CLK_PLL1_DIV2,  4, 1),
 	DEF_FIXED(".s3",        CLK_S3,            CLK_PLL1_DIV2,  6, 1),
+	DEF_FIXED(".sdsrc",     CLK_SDSRC,         CLK_PLL1_DIV2,  2, 1),
 
 	/* Core Clock Outputs */
 	DEF_FIXED("ztr",        R8A7796_CLK_ZTR,   CLK_PLL1_DIV2,  6, 1),
@@ -92,13 +94,42 @@ static const struct cpg_core_clk r8a7796_core_clks[] __initconst = {
 	DEF_FIXED("s3d2",       R8A7796_CLK_S3D2,  CLK_S3,         2, 1),
 	DEF_FIXED("s3d4",       R8A7796_CLK_S3D4,  CLK_S3,         4, 1),
 
+	DEF_GEN3_SD("sd0",      R8A7796_CLK_SD0,   CLK_SDSRC,    0x0074),
+	DEF_GEN3_SD("sd1",      R8A7796_CLK_SD1,   CLK_SDSRC,    0x0078),
+	DEF_GEN3_SD("sd2",      R8A7796_CLK_SD2,   CLK_SDSRC,    0x0268),
+	DEF_GEN3_SD("sd3",      R8A7796_CLK_SD3,   CLK_SDSRC,    0x026c),
+
 	DEF_FIXED("cl",         R8A7796_CLK_CL,    CLK_PLL1_DIV2, 48, 1),
 	DEF_FIXED("cp",         R8A7796_CLK_CP,    CLK_EXTAL,      2, 1),
+
+	DEF_DIV6_RO("osc",      R8A7796_CLK_OSC,   CLK_EXTAL, CPG_RCKCR,  8),
+	DEF_DIV6_RO("r_int",    CLK_RINT,          CLK_EXTAL, CPG_RCKCR, 32),
+
+	DEF_BASE("r",           R8A7796_CLK_R,     CLK_TYPE_GEN3_R, CLK_RINT),
 };
 
 static const struct mssr_mod_clk r8a7796_mod_clks[] __initconst = {
+	DEF_MOD("cmt3",			 300,	R8A7796_CLK_R),
+	DEF_MOD("cmt2",			 301,	R8A7796_CLK_R),
+	DEF_MOD("cmt1",			 302,	R8A7796_CLK_R),
+	DEF_MOD("cmt0",			 303,	R8A7796_CLK_R),
 	DEF_MOD("scif2",		 310,	R8A7796_CLK_S3D4),
+	DEF_MOD("sdif3",		 311,	R8A7796_CLK_SD3),
+	DEF_MOD("sdif2",		 312,	R8A7796_CLK_SD2),
+	DEF_MOD("sdif1",		 313,	R8A7796_CLK_SD1),
+	DEF_MOD("sdif0",		 314,	R8A7796_CLK_SD0),
+	DEF_MOD("rwdt0",		 402,	R8A7796_CLK_R),
 	DEF_MOD("intc-ap",		 408,	R8A7796_CLK_S3D1),
+	DEF_MOD("thermal",		 522,	R8A7796_CLK_CP),
+	DEF_MOD("etheravb",		 812,	R8A7796_CLK_S0D6),
+	DEF_MOD("gpio7",		 905,	R8A7796_CLK_S3D4),
+	DEF_MOD("gpio6",		 906,	R8A7796_CLK_S3D4),
+	DEF_MOD("gpio5",		 907,	R8A7796_CLK_S3D4),
+	DEF_MOD("gpio4",		 908,	R8A7796_CLK_S3D4),
+	DEF_MOD("gpio3",		 909,	R8A7796_CLK_S3D4),
+	DEF_MOD("gpio2",		 910,	R8A7796_CLK_S3D4),
+	DEF_MOD("gpio1",		 911,	R8A7796_CLK_S3D4),
+	DEF_MOD("gpio0",		 912,	R8A7796_CLK_S3D4),
 };
 
 static const unsigned int r8a7796_crit_mod_clks[] __initconst = {
diff --git a/drivers/clk/rockchip/Makefile b/drivers/clk/rockchip/Makefile
index f47a2fa962d2..b5f2c8ed12e1 100644
--- a/drivers/clk/rockchip/Makefile
+++ b/drivers/clk/rockchip/Makefile
@@ -8,6 +8,7 @@ obj-y	+= clk-pll.o
 obj-y	+= clk-cpu.o
 obj-y	+= clk-inverter.o
 obj-y	+= clk-mmc-phase.o
+obj-y	+= clk-ddr.o
 obj-$(CONFIG_RESET_CONTROLLER)	+= softrst.o
 
 obj-y	+= clk-rk3036.o
diff --git a/drivers/clk/rockchip/clk-ddr.c b/drivers/clk/rockchip/clk-ddr.c
new file mode 100644
index 000000000000..8feba93672c5
--- /dev/null
+++ b/drivers/clk/rockchip/clk-ddr.c
@@ -0,0 +1,154 @@
+/*
+ * Copyright (c) 2016 Rockchip Electronics Co. Ltd.
+ * Author: Lin Huang <hl@rock-chips.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/arm-smccc.h>
+#include <linux/clk.h>
+#include <linux/clk-provider.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include <soc/rockchip/rockchip_sip.h>
+#include "clk.h"
+
+struct rockchip_ddrclk {
+	struct clk_hw	hw;
+	void __iomem	*reg_base;
+	int		mux_offset;
+	int		mux_shift;
+	int		mux_width;
+	int		div_shift;
+	int		div_width;
+	int		ddr_flag;
+	spinlock_t	*lock;
+};
+
+#define to_rockchip_ddrclk_hw(hw) container_of(hw, struct rockchip_ddrclk, hw)
+
+static int rockchip_ddrclk_sip_set_rate(struct clk_hw *hw, unsigned long drate,
+					unsigned long prate)
+{
+	struct rockchip_ddrclk *ddrclk = to_rockchip_ddrclk_hw(hw);
+	unsigned long flags;
+	struct arm_smccc_res res;
+
+	spin_lock_irqsave(ddrclk->lock, flags);
+	arm_smccc_smc(ROCKCHIP_SIP_DRAM_FREQ, drate, 0,
+		      ROCKCHIP_SIP_CONFIG_DRAM_SET_RATE,
+		      0, 0, 0, 0, &res);
+	spin_unlock_irqrestore(ddrclk->lock, flags);
+
+	return res.a0;
+}
+
+static unsigned long
+rockchip_ddrclk_sip_recalc_rate(struct clk_hw *hw,
+				unsigned long parent_rate)
+{
+	struct arm_smccc_res res;
+
+	arm_smccc_smc(ROCKCHIP_SIP_DRAM_FREQ, 0, 0,
+		      ROCKCHIP_SIP_CONFIG_DRAM_GET_RATE,
+		      0, 0, 0, 0, &res);
+
+	return res.a0;
+}
+
+static long rockchip_ddrclk_sip_round_rate(struct clk_hw *hw,
+					   unsigned long rate,
+					   unsigned long *prate)
+{
+	struct arm_smccc_res res;
+
+	arm_smccc_smc(ROCKCHIP_SIP_DRAM_FREQ, rate, 0,
+		      ROCKCHIP_SIP_CONFIG_DRAM_ROUND_RATE,
+		      0, 0, 0, 0, &res);
+
+	return res.a0;
+}
+
+static u8 rockchip_ddrclk_get_parent(struct clk_hw *hw)
+{
+	struct rockchip_ddrclk *ddrclk = to_rockchip_ddrclk_hw(hw);
+	int num_parents = clk_hw_get_num_parents(hw);
+	u32 val;
+
+	val = clk_readl(ddrclk->reg_base +
+			ddrclk->mux_offset) >> ddrclk->mux_shift;
+	val &= GENMASK(ddrclk->mux_width - 1, 0);
+
+	if (val >= num_parents)
+		return -EINVAL;
+
+	return val;
+}
+
+static const struct clk_ops rockchip_ddrclk_sip_ops = {
+	.recalc_rate = rockchip_ddrclk_sip_recalc_rate,
+	.set_rate = rockchip_ddrclk_sip_set_rate,
+	.round_rate = rockchip_ddrclk_sip_round_rate,
+	.get_parent = rockchip_ddrclk_get_parent,
+};
+
+struct clk *rockchip_clk_register_ddrclk(const char *name, int flags,
+					 const char *const *parent_names,
+					 u8 num_parents, int mux_offset,
+					 int mux_shift, int mux_width,
+					 int div_shift, int div_width,
+					 int ddr_flag, void __iomem *reg_base,
+					 spinlock_t *lock)
+{
+	struct rockchip_ddrclk *ddrclk;
+	struct clk_init_data init;
+	struct clk *clk;
+
+	ddrclk = kzalloc(sizeof(*ddrclk), GFP_KERNEL);
+	if (!ddrclk)
+		return ERR_PTR(-ENOMEM);
+
+	init.name = name;
+	init.parent_names = parent_names;
+	init.num_parents = num_parents;
+
+	init.flags = flags;
+	init.flags |= CLK_SET_RATE_NO_REPARENT;
+
+	switch (ddr_flag) {
+	case ROCKCHIP_DDRCLK_SIP:
+		init.ops = &rockchip_ddrclk_sip_ops;
+		break;
+	default:
+		pr_err("%s: unsupported ddrclk type %d\n", __func__, ddr_flag);
+		kfree(ddrclk);
+		return ERR_PTR(-EINVAL);
+	}
+
+	ddrclk->reg_base = reg_base;
+	ddrclk->lock = lock;
+	ddrclk->hw.init = &init;
+	ddrclk->mux_offset = mux_offset;
+	ddrclk->mux_shift = mux_shift;
+	ddrclk->mux_width = mux_width;
+	ddrclk->div_shift = div_shift;
+	ddrclk->div_width = div_width;
+	ddrclk->ddr_flag = ddr_flag;
+
+	clk = clk_register(NULL, &ddrclk->hw);
+	if (IS_ERR(clk)) {
+		pr_err("%s: could not register ddrclk %s\n", __func__,	name);
+		kfree(ddrclk);
+		return NULL;
+	}
+
+	return clk;
+}
diff --git a/drivers/clk/rockchip/clk-pll.c b/drivers/clk/rockchip/clk-pll.c
index db81e454166b..9c1373e81683 100644
--- a/drivers/clk/rockchip/clk-pll.c
+++ b/drivers/clk/rockchip/clk-pll.c
@@ -837,7 +837,7 @@ struct clk *rockchip_clk_register_pll(struct rockchip_clk_provider *ctx,
 		u8 num_parents, 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)
+		unsigned long flags, u8 clk_pll_flags)
 {
 	const char *pll_parents[3];
 	struct clk_init_data init;
@@ -892,7 +892,7 @@ struct clk *rockchip_clk_register_pll(struct rockchip_clk_provider *ctx,
 	init.name = pll_name;
 
 	/* keep all plls untouched for now */
-	init.flags = CLK_IGNORE_UNUSED;
+	init.flags = flags | CLK_IGNORE_UNUSED;
 
 	init.parent_names = &parent_names[0];
 	init.num_parents = 1;
diff --git a/drivers/clk/rockchip/clk-rk3399.c b/drivers/clk/rockchip/clk-rk3399.c
index cdfabeb9a034..8387c7a40bda 100644
--- a/drivers/clk/rockchip/clk-rk3399.c
+++ b/drivers/clk/rockchip/clk-rk3399.c
@@ -100,8 +100,10 @@ static struct rockchip_pll_rate_table rk3399_pll_rates[] = {
 	RK3036_PLL_RATE( 297000000, 1, 99, 4, 2, 1, 0),
 	RK3036_PLL_RATE( 216000000, 1, 72, 4, 2, 1, 0),
 	RK3036_PLL_RATE( 148500000, 1, 99, 4, 4, 1, 0),
+	RK3036_PLL_RATE( 106500000, 1, 71, 4, 4, 1, 0),
 	RK3036_PLL_RATE(  96000000, 1, 64, 4, 4, 1, 0),
 	RK3036_PLL_RATE(  74250000, 2, 99, 4, 4, 1, 0),
+	RK3036_PLL_RATE(  65000000, 1, 65, 6, 4, 1, 0),
 	RK3036_PLL_RATE(  54000000, 1, 54, 6, 4, 1, 0),
 	RK3036_PLL_RATE(  27000000, 1, 27, 6, 4, 1, 0),
 	{ /* sentinel */ },
@@ -118,6 +120,10 @@ PNAME(mux_armclkb_p)				= { "clk_core_b_lpll_src",
 						    "clk_core_b_bpll_src",
 						    "clk_core_b_dpll_src",
 						    "clk_core_b_gpll_src" };
+PNAME(mux_ddrclk_p)				= { "clk_ddrc_lpll_src",
+						    "clk_ddrc_bpll_src",
+						    "clk_ddrc_dpll_src",
+						    "clk_ddrc_gpll_src" };
 PNAME(mux_aclk_cci_p)				= { "cpll_aclk_cci_src",
 						    "gpll_aclk_cci_src",
 						    "npll_aclk_cci_src",
@@ -373,6 +379,7 @@ static struct rockchip_cpuclk_rate_table rk3399_cpuclkb_rates[] __initdata = {
 	RK3399_CPUCLKB_RATE(2184000000, 1, 11, 11),
 	RK3399_CPUCLKB_RATE(2088000000, 1, 10, 10),
 	RK3399_CPUCLKB_RATE(2040000000, 1, 10, 10),
+	RK3399_CPUCLKB_RATE(2016000000, 1, 9, 9),
 	RK3399_CPUCLKB_RATE(1992000000, 1, 9, 9),
 	RK3399_CPUCLKB_RATE(1896000000, 1, 9, 9),
 	RK3399_CPUCLKB_RATE(1800000000, 1, 8, 8),
@@ -578,7 +585,7 @@ static struct rockchip_clk_branch rk3399_clk_branches[] __initdata = {
 	COMPOSITE(0, "clk_spdif_div", mux_pll_src_cpll_gpll_p, 0,
 			RK3399_CLKSEL_CON(32), 7, 1, MFLAGS, 0, 7, DFLAGS,
 			RK3399_CLKGATE_CON(8), 13, GFLAGS),
-	COMPOSITE_FRACMUX(0, "clk_spdif_frac", "clk_spdif_div", CLK_SET_RATE_PARENT,
+	COMPOSITE_FRACMUX(0, "clk_spdif_frac", "clk_spdif_div", 0,
 			RK3399_CLKSEL_CON(99), 0,
 			RK3399_CLKGATE_CON(8), 14, GFLAGS,
 			&rk3399_spdif_fracmux),
@@ -592,7 +599,7 @@ static struct rockchip_clk_branch rk3399_clk_branches[] __initdata = {
 	COMPOSITE(0, "clk_i2s0_div", mux_pll_src_cpll_gpll_p, 0,
 			RK3399_CLKSEL_CON(28), 7, 1, MFLAGS, 0, 7, DFLAGS,
 			RK3399_CLKGATE_CON(8), 3, GFLAGS),
-	COMPOSITE_FRACMUX(0, "clk_i2s0_frac", "clk_i2s0_div", CLK_SET_RATE_PARENT,
+	COMPOSITE_FRACMUX(0, "clk_i2s0_frac", "clk_i2s0_div", 0,
 			RK3399_CLKSEL_CON(96), 0,
 			RK3399_CLKGATE_CON(8), 4, GFLAGS,
 			&rk3399_i2s0_fracmux),
@@ -602,7 +609,7 @@ static struct rockchip_clk_branch rk3399_clk_branches[] __initdata = {
 	COMPOSITE(0, "clk_i2s1_div", mux_pll_src_cpll_gpll_p, 0,
 			RK3399_CLKSEL_CON(29), 7, 1, MFLAGS, 0, 7, DFLAGS,
 			RK3399_CLKGATE_CON(8), 6, GFLAGS),
-	COMPOSITE_FRACMUX(0, "clk_i2s1_frac", "clk_i2s1_div", CLK_SET_RATE_PARENT,
+	COMPOSITE_FRACMUX(0, "clk_i2s1_frac", "clk_i2s1_div", 0,
 			RK3399_CLKSEL_CON(97), 0,
 			RK3399_CLKGATE_CON(8), 7, GFLAGS,
 			&rk3399_i2s1_fracmux),
@@ -612,7 +619,7 @@ static struct rockchip_clk_branch rk3399_clk_branches[] __initdata = {
 	COMPOSITE(0, "clk_i2s2_div", mux_pll_src_cpll_gpll_p, 0,
 			RK3399_CLKSEL_CON(30), 7, 1, MFLAGS, 0, 7, DFLAGS,
 			RK3399_CLKGATE_CON(8), 9, GFLAGS),
-	COMPOSITE_FRACMUX(0, "clk_i2s2_frac", "clk_i2s2_div", CLK_SET_RATE_PARENT,
+	COMPOSITE_FRACMUX(0, "clk_i2s2_frac", "clk_i2s2_div", 0,
 			RK3399_CLKSEL_CON(98), 0,
 			RK3399_CLKGATE_CON(8), 10, GFLAGS,
 			&rk3399_i2s2_fracmux),
@@ -631,7 +638,7 @@ static struct rockchip_clk_branch rk3399_clk_branches[] __initdata = {
 	COMPOSITE_NOMUX(0, "clk_uart0_div", "clk_uart0_src", 0,
 			RK3399_CLKSEL_CON(33), 0, 7, DFLAGS,
 			RK3399_CLKGATE_CON(9), 0, GFLAGS),
-	COMPOSITE_FRACMUX(0, "clk_uart0_frac", "clk_uart0_div", CLK_SET_RATE_PARENT,
+	COMPOSITE_FRACMUX(0, "clk_uart0_frac", "clk_uart0_div", 0,
 			RK3399_CLKSEL_CON(100), 0,
 			RK3399_CLKGATE_CON(9), 1, GFLAGS,
 			&rk3399_uart0_fracmux),
@@ -641,7 +648,7 @@ static struct rockchip_clk_branch rk3399_clk_branches[] __initdata = {
 	COMPOSITE_NOMUX(0, "clk_uart1_div", "clk_uart_src", 0,
 			RK3399_CLKSEL_CON(34), 0, 7, DFLAGS,
 			RK3399_CLKGATE_CON(9), 2, GFLAGS),
-	COMPOSITE_FRACMUX(0, "clk_uart1_frac", "clk_uart1_div", CLK_SET_RATE_PARENT,
+	COMPOSITE_FRACMUX(0, "clk_uart1_frac", "clk_uart1_div", 0,
 			RK3399_CLKSEL_CON(101), 0,
 			RK3399_CLKGATE_CON(9), 3, GFLAGS,
 			&rk3399_uart1_fracmux),
@@ -649,7 +656,7 @@ static struct rockchip_clk_branch rk3399_clk_branches[] __initdata = {
 	COMPOSITE_NOMUX(0, "clk_uart2_div", "clk_uart_src", 0,
 			RK3399_CLKSEL_CON(35), 0, 7, DFLAGS,
 			RK3399_CLKGATE_CON(9), 4, GFLAGS),
-	COMPOSITE_FRACMUX(0, "clk_uart2_frac", "clk_uart2_div", CLK_SET_RATE_PARENT,
+	COMPOSITE_FRACMUX(0, "clk_uart2_frac", "clk_uart2_div", 0,
 			RK3399_CLKSEL_CON(102), 0,
 			RK3399_CLKGATE_CON(9), 5, GFLAGS,
 			&rk3399_uart2_fracmux),
@@ -657,7 +664,7 @@ static struct rockchip_clk_branch rk3399_clk_branches[] __initdata = {
 	COMPOSITE_NOMUX(0, "clk_uart3_div", "clk_uart_src", 0,
 			RK3399_CLKSEL_CON(36), 0, 7, DFLAGS,
 			RK3399_CLKGATE_CON(9), 6, GFLAGS),
-	COMPOSITE_FRACMUX(0, "clk_uart3_frac", "clk_uart3_div", CLK_SET_RATE_PARENT,
+	COMPOSITE_FRACMUX(0, "clk_uart3_frac", "clk_uart3_div", 0,
 			RK3399_CLKSEL_CON(103), 0,
 			RK3399_CLKGATE_CON(9), 7, GFLAGS,
 			&rk3399_uart3_fracmux),
@@ -846,9 +853,9 @@ static struct rockchip_clk_branch rk3399_clk_branches[] __initdata = {
 			RK3399_CLKSEL_CON(14), 12, 2, DFLAGS,
 			RK3399_CLKGATE_CON(5), 4, GFLAGS),
 
-	GATE(ACLK_PERF_PCIE, "aclk_perf_pcie", "aclk_perihp", CLK_IGNORE_UNUSED,
+	GATE(ACLK_PERF_PCIE, "aclk_perf_pcie", "aclk_perihp", 0,
 			RK3399_CLKGATE_CON(20), 2, GFLAGS),
-	GATE(ACLK_PCIE, "aclk_pcie", "aclk_perihp", CLK_IGNORE_UNUSED,
+	GATE(ACLK_PCIE, "aclk_pcie", "aclk_perihp", 0,
 			RK3399_CLKGATE_CON(20), 10, GFLAGS),
 	GATE(0, "aclk_perihp_noc", "aclk_perihp", CLK_IGNORE_UNUSED,
 			RK3399_CLKGATE_CON(20), 12, GFLAGS),
@@ -1161,7 +1168,7 @@ static struct rockchip_clk_branch rk3399_clk_branches[] __initdata = {
 			RK3399_CLKSEL_CON(49), 8, 2, MFLAGS, 0, 8, DFLAGS,
 			RK3399_CLKGATE_CON(10), 12, GFLAGS),
 
-	COMPOSITE_FRACMUX_NOGATE(0, "dclk_vop0_frac", "dclk_vop0_div", CLK_SET_RATE_PARENT,
+	COMPOSITE_FRACMUX_NOGATE(DCLK_VOP0_FRAC, "dclk_vop0_frac", "dclk_vop0_div", 0,
 			RK3399_CLKSEL_CON(106), 0,
 			&rk3399_dclk_vop0_fracmux),
 
@@ -1191,7 +1198,7 @@ static struct rockchip_clk_branch rk3399_clk_branches[] __initdata = {
 			RK3399_CLKSEL_CON(50), 8, 2, MFLAGS, 0, 8, DFLAGS,
 			RK3399_CLKGATE_CON(10), 13, GFLAGS),
 
-	COMPOSITE_FRACMUX_NOGATE(0, "dclk_vop1_frac", "dclk_vop1_div", CLK_SET_RATE_PARENT,
+	COMPOSITE_FRACMUX_NOGATE(DCLK_VOP1_FRAC, "dclk_vop1_frac", "dclk_vop1_div", 0,
 			RK3399_CLKSEL_CON(107), 0,
 			&rk3399_dclk_vop1_fracmux),
 
@@ -1305,7 +1312,7 @@ static struct rockchip_clk_branch rk3399_clk_branches[] __initdata = {
 	/* testout */
 	MUX(0, "clk_test_pre", mux_pll_src_cpll_gpll_p, CLK_SET_RATE_PARENT,
 			RK3399_CLKSEL_CON(58), 7, 1, MFLAGS),
-	COMPOSITE_FRAC(0, "clk_test_frac", "clk_test_pre", CLK_SET_RATE_PARENT,
+	COMPOSITE_FRAC(0, "clk_test_frac", "clk_test_pre", 0,
 			RK3399_CLKSEL_CON(105), 0,
 			RK3399_CLKGATE_CON(13), 9, GFLAGS),
 
@@ -1377,6 +1384,18 @@ static struct rockchip_clk_branch rk3399_clk_branches[] __initdata = {
 	COMPOSITE_NOMUX(0, "clk_test", "clk_test_pre", CLK_IGNORE_UNUSED,
 			RK3368_CLKSEL_CON(58), 0, 5, DFLAGS,
 			RK3368_CLKGATE_CON(13), 11, GFLAGS),
+
+	/* ddrc */
+	GATE(0, "clk_ddrc_lpll_src", "lpll", 0, RK3399_CLKGATE_CON(3),
+	     0, GFLAGS),
+	GATE(0, "clk_ddrc_bpll_src", "bpll", 0, RK3399_CLKGATE_CON(3),
+	     1, GFLAGS),
+	GATE(0, "clk_ddrc_dpll_src", "dpll", 0, RK3399_CLKGATE_CON(3),
+	     2, GFLAGS),
+	GATE(0, "clk_ddrc_gpll_src", "gpll", 0, RK3399_CLKGATE_CON(3),
+	     3, GFLAGS),
+	COMPOSITE_DDRCLK(SCLK_DDRC, "sclk_ddrc", mux_ddrclk_p, 0,
+		       RK3399_CLKSEL_CON(6), 4, 2, 0, 0, ROCKCHIP_DDRCLK_SIP),
 };
 
 static struct rockchip_clk_branch rk3399_clk_pmu_branches[] __initdata = {
@@ -1398,7 +1417,7 @@ static struct rockchip_clk_branch rk3399_clk_pmu_branches[] __initdata = {
 			RK3399_PMU_CLKSEL_CON(1), 13, 1, MFLAGS, 8, 5, DFLAGS,
 			RK3399_PMU_CLKGATE_CON(0), 8, GFLAGS),
 
-	COMPOSITE_FRACMUX_NOGATE(0, "clk_wifi_frac", "clk_wifi_div", CLK_SET_RATE_PARENT,
+	COMPOSITE_FRACMUX_NOGATE(0, "clk_wifi_frac", "clk_wifi_div", 0,
 			RK3399_PMU_CLKSEL_CON(7), 0,
 			&rk3399_pmuclk_wifi_fracmux),
 
@@ -1426,7 +1445,7 @@ static struct rockchip_clk_branch rk3399_clk_pmu_branches[] __initdata = {
 			RK3399_PMU_CLKSEL_CON(5), 10, 1, MFLAGS, 0, 7, DFLAGS,
 			RK3399_PMU_CLKGATE_CON(0), 5, GFLAGS),
 
-	COMPOSITE_FRACMUX(0, "clk_uart4_frac", "clk_uart4_div", CLK_SET_RATE_PARENT,
+	COMPOSITE_FRACMUX(0, "clk_uart4_frac", "clk_uart4_div", 0,
 			RK3399_PMU_CLKSEL_CON(6), 0,
 			RK3399_PMU_CLKGATE_CON(0), 6, GFLAGS,
 			&rk3399_uart4_pmu_fracmux),
@@ -1468,6 +1487,9 @@ static const char *const rk3399_cru_critical_clocks[] __initconst = {
 	"aclk_cci_pre",
 	"aclk_gic",
 	"aclk_gic_noc",
+	"aclk_hdcp_noc",
+	"hclk_hdcp_noc",
+	"pclk_hdcp_noc",
 	"pclk_perilp0",
 	"pclk_perilp0",
 	"hclk_perilp0",
@@ -1488,6 +1510,10 @@ static const char *const rk3399_cru_critical_clocks[] __initconst = {
 	"gpll_hclk_perilp1_src",
 	"gpll_aclk_perilp0_src",
 	"gpll_aclk_perihp_src",
+	"aclk_vio_noc",
+
+	/* ddrc */
+	"sclk_ddrc"
 };
 
 static const char *const rk3399_pmucru_critical_clocks[] __initconst = {
diff --git a/drivers/clk/rockchip/clk-rockchip.c b/drivers/clk/rockchip/clk-rockchip.c
index 4cf838d52ef6..2c9bb81144c9 100644
--- a/drivers/clk/rockchip/clk-rockchip.c
+++ b/drivers/clk/rockchip/clk-rockchip.c
@@ -49,14 +49,19 @@ static void __init rk2928_gate_clk_init(struct device_node *node)
 	}
 
 	reg = of_iomap(node, 0);
+	if (!reg)
+		return;
 
 	clk_data = kzalloc(sizeof(struct clk_onecell_data), GFP_KERNEL);
-	if (!clk_data)
+	if (!clk_data) {
+		iounmap(reg);
 		return;
+	}
 
 	clk_data->clks = kzalloc(qty * sizeof(struct clk *), GFP_KERNEL);
 	if (!clk_data->clks) {
 		kfree(clk_data);
+		iounmap(reg);
 		return;
 	}
 
diff --git a/drivers/clk/rockchip/clk.c b/drivers/clk/rockchip/clk.c
index 7ffd134995f2..b886be30f34f 100644
--- a/drivers/clk/rockchip/clk.c
+++ b/drivers/clk/rockchip/clk.c
@@ -385,7 +385,7 @@ void __init rockchip_clk_register_plls(struct rockchip_clk_provider *ctx,
 				list->con_offset, grf_lock_offset,
 				list->lock_shift, list->mode_offset,
 				list->mode_shift, list->rate_table,
-				list->pll_flags);
+				list->flags, list->pll_flags);
 		if (IS_ERR(clk)) {
 			pr_err("%s: failed to register clock %s\n", __func__,
 				list->name);
@@ -484,6 +484,15 @@ void __init rockchip_clk_register_branches(
 				list->gate_offset, list->gate_shift,
 				list->gate_flags, flags, &ctx->lock);
 			break;
+		case branch_ddrclk:
+			clk = rockchip_clk_register_ddrclk(
+				list->name, list->flags,
+				list->parent_names, list->num_parents,
+				list->muxdiv_offset, list->mux_shift,
+				list->mux_width, list->div_shift,
+				list->div_width, list->div_flags,
+				ctx->reg_base, &ctx->lock);
+			break;
 		}
 
 		/* none of the cases above matched */
diff --git a/drivers/clk/rockchip/clk.h b/drivers/clk/rockchip/clk.h
index 2194ffa8c9fd..1653edd792a5 100644
--- a/drivers/clk/rockchip/clk.h
+++ b/drivers/clk/rockchip/clk.h
@@ -238,7 +238,7 @@ struct clk *rockchip_clk_register_pll(struct rockchip_clk_provider *ctx,
 		u8 num_parents, 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);
+		unsigned long flags, u8 clk_pll_flags);
 
 struct rockchip_cpuclk_clksel {
 	int reg;
@@ -281,6 +281,20 @@ struct clk *rockchip_clk_register_mmc(const char *name,
 				const char *const *parent_names, u8 num_parents,
 				void __iomem *reg, int shift);
 
+/*
+ * DDRCLK flags, including method of setting the rate
+ * ROCKCHIP_DDRCLK_SIP: use SIP call to bl31 to change ddrclk rate.
+ */
+#define ROCKCHIP_DDRCLK_SIP		BIT(0)
+
+struct clk *rockchip_clk_register_ddrclk(const char *name, int flags,
+					 const char *const *parent_names,
+					 u8 num_parents, int mux_offset,
+					 int mux_shift, int mux_width,
+					 int div_shift, int div_width,
+					 int ddr_flags, void __iomem *reg_base,
+					 spinlock_t *lock);
+
 #define ROCKCHIP_INVERTER_HIWORD_MASK	BIT(0)
 
 struct clk *rockchip_clk_register_inverter(const char *name,
@@ -299,6 +313,7 @@ enum rockchip_clk_branch_type {
 	branch_mmc,
 	branch_inverter,
 	branch_factor,
+	branch_ddrclk,
 };
 
 struct rockchip_clk_branch {
@@ -488,6 +503,24 @@ struct rockchip_clk_branch {
 		.child		= ch,				\
 	}
 
+#define COMPOSITE_DDRCLK(_id, cname, pnames, f, mo, ms, mw,	\
+			 ds, dw, df)				\
+	{							\
+		.id		= _id,				\
+		.branch_type	= branch_ddrclk,		\
+		.name		= cname,			\
+		.parent_names	= pnames,			\
+		.num_parents	= ARRAY_SIZE(pnames),		\
+		.flags		= f,				\
+		.muxdiv_offset  = mo,                           \
+		.mux_shift      = ms,                           \
+		.mux_width      = mw,                           \
+		.div_shift      = ds,                           \
+		.div_width      = dw,                           \
+		.div_flags	= df,				\
+		.gate_offset    = -1,                           \
+	}
+
 #define MUX(_id, cname, pnames, f, o, s, w, mf)			\
 	{							\
 		.id		= _id,				\
diff --git a/drivers/clk/samsung/clk-exynos-audss.c b/drivers/clk/samsung/clk-exynos-audss.c
index bdf8b971f332..51d152f735cc 100644
--- a/drivers/clk/samsung/clk-exynos-audss.c
+++ b/drivers/clk/samsung/clk-exynos-audss.c
@@ -14,18 +14,13 @@
 #include <linux/clk.h>
 #include <linux/clk-provider.h>
 #include <linux/of_address.h>
+#include <linux/of_device.h>
 #include <linux/syscore_ops.h>
 #include <linux/module.h>
 #include <linux/platform_device.h>
 
 #include <dt-bindings/clock/exynos-audss-clk.h>
 
-enum exynos_audss_clk_type {
-	TYPE_EXYNOS4210,
-	TYPE_EXYNOS5250,
-	TYPE_EXYNOS5420,
-};
-
 static DEFINE_SPINLOCK(lock);
 static struct clk **clk_table;
 static void __iomem *reg_base;
@@ -44,9 +39,9 @@ static struct clk *epll;
 
 #ifdef CONFIG_PM_SLEEP
 static unsigned long reg_save[][2] = {
-	{ASS_CLK_SRC,  0},
-	{ASS_CLK_DIV,  0},
-	{ASS_CLK_GATE, 0},
+	{ ASS_CLK_SRC,  0 },
+	{ ASS_CLK_DIV,  0 },
+	{ ASS_CLK_GATE, 0 },
 };
 
 static int exynos_audss_clk_suspend(void)
@@ -73,14 +68,43 @@ static struct syscore_ops exynos_audss_clk_syscore_ops = {
 };
 #endif /* CONFIG_PM_SLEEP */
 
+struct exynos_audss_clk_drvdata {
+	unsigned int has_adma_clk:1;
+	unsigned int has_mst_clk:1;
+	unsigned int enable_epll:1;
+	unsigned int num_clks;
+};
+
+static const struct exynos_audss_clk_drvdata exynos4210_drvdata = {
+	.num_clks	= EXYNOS_AUDSS_MAX_CLKS - 1,
+};
+
+static const struct exynos_audss_clk_drvdata exynos5410_drvdata = {
+	.num_clks	= EXYNOS_AUDSS_MAX_CLKS - 1,
+	.has_mst_clk	= 1,
+};
+
+static const struct exynos_audss_clk_drvdata exynos5420_drvdata = {
+	.num_clks	= EXYNOS_AUDSS_MAX_CLKS,
+	.has_adma_clk	= 1,
+	.enable_epll	= 1,
+};
+
 static const struct of_device_id exynos_audss_clk_of_match[] = {
-	{ .compatible = "samsung,exynos4210-audss-clock",
-	  .data = (void *)TYPE_EXYNOS4210, },
-	{ .compatible = "samsung,exynos5250-audss-clock",
-	  .data = (void *)TYPE_EXYNOS5250, },
-	{ .compatible = "samsung,exynos5420-audss-clock",
-	  .data = (void *)TYPE_EXYNOS5420, },
-	{},
+	{
+		.compatible	= "samsung,exynos4210-audss-clock",
+		.data		= &exynos4210_drvdata,
+	}, {
+		.compatible	= "samsung,exynos5250-audss-clock",
+		.data		= &exynos4210_drvdata,
+	}, {
+		.compatible	= "samsung,exynos5410-audss-clock",
+		.data		= &exynos5410_drvdata,
+	}, {
+		.compatible	= "samsung,exynos5420-audss-clock",
+		.data		= &exynos5420_drvdata,
+	},
+	{ },
 };
 
 static void exynos_audss_clk_teardown(void)
@@ -106,19 +130,17 @@ static void exynos_audss_clk_teardown(void)
 /* register exynos_audss clocks */
 static int exynos_audss_clk_probe(struct platform_device *pdev)
 {
-	int i, ret = 0;
-	struct resource *res;
 	const char *mout_audss_p[] = {"fin_pll", "fout_epll"};
 	const char *mout_i2s_p[] = {"mout_audss", "cdclk0", "sclk_audio0"};
 	const char *sclk_pcm_p = "sclk_pcm0";
 	struct clk *pll_ref, *pll_in, *cdclk, *sclk_audio, *sclk_pcm_in;
-	const struct of_device_id *match;
-	enum exynos_audss_clk_type variant;
+	const struct exynos_audss_clk_drvdata *variant;
+	struct resource *res;
+	int i, ret = 0;
 
-	match = of_match_node(exynos_audss_clk_of_match, pdev->dev.of_node);
-	if (!match)
+	variant = of_device_get_match_data(&pdev->dev);
+	if (!variant)
 		return -EINVAL;
-	variant = (enum exynos_audss_clk_type)match->data;
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	reg_base = devm_ioremap_resource(&pdev->dev, res);
@@ -126,7 +148,7 @@ static int exynos_audss_clk_probe(struct platform_device *pdev)
 		dev_err(&pdev->dev, "failed to map audss registers\n");
 		return PTR_ERR(reg_base);
 	}
-	/* EPLL don't have to be enabled for boards other than Exynos5420 */
+
 	epll = ERR_PTR(-ENODEV);
 
 	clk_table = devm_kzalloc(&pdev->dev,
@@ -136,10 +158,7 @@ static int exynos_audss_clk_probe(struct platform_device *pdev)
 		return -ENOMEM;
 
 	clk_data.clks = clk_table;
-	if (variant == TYPE_EXYNOS5420)
-		clk_data.clk_num = EXYNOS_AUDSS_MAX_CLKS;
-	else
-		clk_data.clk_num = EXYNOS_AUDSS_MAX_CLKS - 1;
+	clk_data.clk_num = variant->num_clks;
 
 	pll_ref = devm_clk_get(&pdev->dev, "pll_ref");
 	pll_in = devm_clk_get(&pdev->dev, "pll_in");
@@ -148,13 +167,13 @@ static int exynos_audss_clk_probe(struct platform_device *pdev)
 	if (!IS_ERR(pll_in)) {
 		mout_audss_p[1] = __clk_get_name(pll_in);
 
-		if (variant == TYPE_EXYNOS5420) {
+		if (variant->enable_epll) {
 			epll = pll_in;
 
 			ret = clk_prepare_enable(epll);
 			if (ret) {
 				dev_err(&pdev->dev,
-						"failed to prepare the epll clock\n");
+					"failed to prepare the epll clock\n");
 				return ret;
 			}
 		}
@@ -210,7 +229,7 @@ static int exynos_audss_clk_probe(struct platform_device *pdev)
 				sclk_pcm_p, CLK_SET_RATE_PARENT,
 				reg_base + ASS_CLK_GATE, 5, 0, &lock);
 
-	if (variant == TYPE_EXYNOS5420) {
+	if (variant->has_adma_clk) {
 		clk_table[EXYNOS_ADMA] = clk_register_gate(NULL, "adma",
 				"dout_srp", CLK_SET_RATE_PARENT,
 				reg_base + ASS_CLK_GATE, 9, 0, &lock);
@@ -234,9 +253,6 @@ static int exynos_audss_clk_probe(struct platform_device *pdev)
 #ifdef CONFIG_PM_SLEEP
 	register_syscore_ops(&exynos_audss_clk_syscore_ops);
 #endif
-
-	dev_info(&pdev->dev, "setup completed\n");
-
 	return 0;
 
 unregister:
diff --git a/drivers/clk/samsung/clk-exynos5260.c b/drivers/clk/samsung/clk-exynos5260.c
index a43642c36039..fd1d9bfc151b 100644
--- a/drivers/clk/samsung/clk-exynos5260.c
+++ b/drivers/clk/samsung/clk-exynos5260.c
@@ -131,21 +131,21 @@ static const struct samsung_gate_clock aud_gate_clks[] __initconst = {
 			EN_IP_AUD, 4, 0, 0),
 };
 
+static const struct samsung_cmu_info aud_cmu __initconst = {
+	.mux_clks	= aud_mux_clks,
+	.nr_mux_clks	= ARRAY_SIZE(aud_mux_clks),
+	.div_clks	= aud_div_clks,
+	.nr_div_clks	= ARRAY_SIZE(aud_div_clks),
+	.gate_clks	= aud_gate_clks,
+	.nr_gate_clks	= ARRAY_SIZE(aud_gate_clks),
+	.nr_clk_ids	= AUD_NR_CLK,
+	.clk_regs	= aud_clk_regs,
+	.nr_clk_regs	= ARRAY_SIZE(aud_clk_regs),
+};
+
 static void __init exynos5260_clk_aud_init(struct device_node *np)
 {
-	struct samsung_cmu_info cmu = { NULL };
-
-	cmu.mux_clks = aud_mux_clks;
-	cmu.nr_mux_clks = ARRAY_SIZE(aud_mux_clks);
-	cmu.div_clks = aud_div_clks;
-	cmu.nr_div_clks = ARRAY_SIZE(aud_div_clks);
-	cmu.gate_clks = aud_gate_clks;
-	cmu.nr_gate_clks = ARRAY_SIZE(aud_gate_clks);
-	cmu.nr_clk_ids = AUD_NR_CLK;
-	cmu.clk_regs = aud_clk_regs;
-	cmu.nr_clk_regs = ARRAY_SIZE(aud_clk_regs);
-
-	samsung_cmu_register_one(np, &cmu);
+	samsung_cmu_register_one(np, &aud_cmu);
 }
 
 CLK_OF_DECLARE(exynos5260_clk_aud, "samsung,exynos5260-clock-aud",
@@ -321,21 +321,21 @@ static const struct samsung_gate_clock disp_gate_clks[] __initconst = {
 			EN_IP_DISP, 25, 0, 0),
 };
 
+static const struct samsung_cmu_info disp_cmu __initconst = {
+	.mux_clks	= disp_mux_clks,
+	.nr_mux_clks	= ARRAY_SIZE(disp_mux_clks),
+	.div_clks	= disp_div_clks,
+	.nr_div_clks	= ARRAY_SIZE(disp_div_clks),
+	.gate_clks	= disp_gate_clks,
+	.nr_gate_clks	= ARRAY_SIZE(disp_gate_clks),
+	.nr_clk_ids	= DISP_NR_CLK,
+	.clk_regs	= disp_clk_regs,
+	.nr_clk_regs	= ARRAY_SIZE(disp_clk_regs),
+};
+
 static void __init exynos5260_clk_disp_init(struct device_node *np)
 {
-	struct samsung_cmu_info cmu = { NULL };
-
-	cmu.mux_clks = disp_mux_clks;
-	cmu.nr_mux_clks = ARRAY_SIZE(disp_mux_clks);
-	cmu.div_clks = disp_div_clks;
-	cmu.nr_div_clks = ARRAY_SIZE(disp_div_clks);
-	cmu.gate_clks = disp_gate_clks;
-	cmu.nr_gate_clks = ARRAY_SIZE(disp_gate_clks);
-	cmu.nr_clk_ids = DISP_NR_CLK;
-	cmu.clk_regs = disp_clk_regs;
-	cmu.nr_clk_regs = ARRAY_SIZE(disp_clk_regs);
-
-	samsung_cmu_register_one(np, &cmu);
+	samsung_cmu_register_one(np, &disp_cmu);
 }
 
 CLK_OF_DECLARE(exynos5260_clk_disp, "samsung,exynos5260-clock-disp",
@@ -385,21 +385,21 @@ static const struct samsung_pll_clock egl_pll_clks[] __initconst = {
 		pll2550_24mhz_tbl),
 };
 
+static const struct samsung_cmu_info egl_cmu __initconst = {
+	.pll_clks	= egl_pll_clks,
+	.nr_pll_clks	= ARRAY_SIZE(egl_pll_clks),
+	.mux_clks	= egl_mux_clks,
+	.nr_mux_clks	= ARRAY_SIZE(egl_mux_clks),
+	.div_clks	= egl_div_clks,
+	.nr_div_clks	= ARRAY_SIZE(egl_div_clks),
+	.nr_clk_ids	= EGL_NR_CLK,
+	.clk_regs	= egl_clk_regs,
+	.nr_clk_regs	= ARRAY_SIZE(egl_clk_regs),
+};
+
 static void __init exynos5260_clk_egl_init(struct device_node *np)
 {
-	struct samsung_cmu_info cmu = { NULL };
-
-	cmu.pll_clks = egl_pll_clks;
-	cmu.nr_pll_clks =  ARRAY_SIZE(egl_pll_clks);
-	cmu.mux_clks = egl_mux_clks;
-	cmu.nr_mux_clks = ARRAY_SIZE(egl_mux_clks);
-	cmu.div_clks = egl_div_clks;
-	cmu.nr_div_clks = ARRAY_SIZE(egl_div_clks);
-	cmu.nr_clk_ids = EGL_NR_CLK;
-	cmu.clk_regs = egl_clk_regs;
-	cmu.nr_clk_regs = ARRAY_SIZE(egl_clk_regs);
-
-	samsung_cmu_register_one(np, &cmu);
+	samsung_cmu_register_one(np, &egl_cmu);
 }
 
 CLK_OF_DECLARE(exynos5260_clk_egl, "samsung,exynos5260-clock-egl",
@@ -487,19 +487,19 @@ static const struct samsung_gate_clock fsys_gate_clks[] __initconst = {
 			EN_IP_FSYS_SECURE_SMMU_RTIC, 12, 0, 0),
 };
 
+static const struct samsung_cmu_info fsys_cmu __initconst = {
+	.mux_clks	= fsys_mux_clks,
+	.nr_mux_clks	= ARRAY_SIZE(fsys_mux_clks),
+	.gate_clks	= fsys_gate_clks,
+	.nr_gate_clks	= ARRAY_SIZE(fsys_gate_clks),
+	.nr_clk_ids	= FSYS_NR_CLK,
+	.clk_regs	= fsys_clk_regs,
+	.nr_clk_regs	= ARRAY_SIZE(fsys_clk_regs),
+};
+
 static void __init exynos5260_clk_fsys_init(struct device_node *np)
 {
-	struct samsung_cmu_info cmu = { NULL };
-
-	cmu.mux_clks = fsys_mux_clks;
-	cmu.nr_mux_clks = ARRAY_SIZE(fsys_mux_clks);
-	cmu.gate_clks = fsys_gate_clks;
-	cmu.nr_gate_clks = ARRAY_SIZE(fsys_gate_clks);
-	cmu.nr_clk_ids = FSYS_NR_CLK;
-	cmu.clk_regs = fsys_clk_regs;
-	cmu.nr_clk_regs = ARRAY_SIZE(fsys_clk_regs);
-
-	samsung_cmu_register_one(np, &cmu);
+	samsung_cmu_register_one(np, &fsys_cmu);
 }
 
 CLK_OF_DECLARE(exynos5260_clk_fsys, "samsung,exynos5260-clock-fsys",
@@ -576,21 +576,21 @@ static const struct samsung_gate_clock g2d_gate_clks[] __initconst = {
 			EN_IP_G2D_SECURE_SMMU_G2D, 15, 0, 0),
 };
 
+static const struct samsung_cmu_info g2d_cmu __initconst = {
+	.mux_clks	= g2d_mux_clks,
+	.nr_mux_clks	= ARRAY_SIZE(g2d_mux_clks),
+	.div_clks	= g2d_div_clks,
+	.nr_div_clks	= ARRAY_SIZE(g2d_div_clks),
+	.gate_clks	= g2d_gate_clks,
+	.nr_gate_clks	= ARRAY_SIZE(g2d_gate_clks),
+	.nr_clk_ids	= G2D_NR_CLK,
+	.clk_regs	= g2d_clk_regs,
+	.nr_clk_regs	= ARRAY_SIZE(g2d_clk_regs),
+};
+
 static void __init exynos5260_clk_g2d_init(struct device_node *np)
 {
-	struct samsung_cmu_info cmu = { NULL };
-
-	cmu.mux_clks = g2d_mux_clks;
-	cmu.nr_mux_clks = ARRAY_SIZE(g2d_mux_clks);
-	cmu.div_clks = g2d_div_clks;
-	cmu.nr_div_clks = ARRAY_SIZE(g2d_div_clks);
-	cmu.gate_clks = g2d_gate_clks;
-	cmu.nr_gate_clks = ARRAY_SIZE(g2d_gate_clks);
-	cmu.nr_clk_ids = G2D_NR_CLK;
-	cmu.clk_regs = g2d_clk_regs;
-	cmu.nr_clk_regs = ARRAY_SIZE(g2d_clk_regs);
-
-	samsung_cmu_register_one(np, &cmu);
+	samsung_cmu_register_one(np, &g2d_cmu);
 }
 
 CLK_OF_DECLARE(exynos5260_clk_g2d, "samsung,exynos5260-clock-g2d",
@@ -637,23 +637,23 @@ static const struct samsung_pll_clock g3d_pll_clks[] __initconst = {
 		pll2550_24mhz_tbl),
 };
 
+static const struct samsung_cmu_info g3d_cmu __initconst = {
+	.pll_clks	= g3d_pll_clks,
+	.nr_pll_clks	= ARRAY_SIZE(g3d_pll_clks),
+	.mux_clks	= g3d_mux_clks,
+	.nr_mux_clks	= ARRAY_SIZE(g3d_mux_clks),
+	.div_clks	= g3d_div_clks,
+	.nr_div_clks	= ARRAY_SIZE(g3d_div_clks),
+	.gate_clks	= g3d_gate_clks,
+	.nr_gate_clks	= ARRAY_SIZE(g3d_gate_clks),
+	.nr_clk_ids	= G3D_NR_CLK,
+	.clk_regs	= g3d_clk_regs,
+	.nr_clk_regs	= ARRAY_SIZE(g3d_clk_regs),
+};
+
 static void __init exynos5260_clk_g3d_init(struct device_node *np)
 {
-	struct samsung_cmu_info cmu = { NULL };
-
-	cmu.pll_clks = g3d_pll_clks;
-	cmu.nr_pll_clks =  ARRAY_SIZE(g3d_pll_clks);
-	cmu.mux_clks = g3d_mux_clks;
-	cmu.nr_mux_clks = ARRAY_SIZE(g3d_mux_clks);
-	cmu.div_clks = g3d_div_clks;
-	cmu.nr_div_clks = ARRAY_SIZE(g3d_div_clks);
-	cmu.gate_clks = g3d_gate_clks;
-	cmu.nr_gate_clks = ARRAY_SIZE(g3d_gate_clks);
-	cmu.nr_clk_ids = G3D_NR_CLK;
-	cmu.clk_regs = g3d_clk_regs;
-	cmu.nr_clk_regs = ARRAY_SIZE(g3d_clk_regs);
-
-	samsung_cmu_register_one(np, &cmu);
+	samsung_cmu_register_one(np, &g3d_cmu);
 }
 
 CLK_OF_DECLARE(exynos5260_clk_g3d, "samsung,exynos5260-clock-g3d",
@@ -772,21 +772,21 @@ static const struct samsung_gate_clock gscl_gate_clks[] __initconst = {
 			EN_IP_GSCL_SECURE_SMMU_MSCL1, 20, 0, 0),
 };
 
+static const struct samsung_cmu_info gscl_cmu __initconst = {
+	.mux_clks	= gscl_mux_clks,
+	.nr_mux_clks	= ARRAY_SIZE(gscl_mux_clks),
+	.div_clks	= gscl_div_clks,
+	.nr_div_clks	= ARRAY_SIZE(gscl_div_clks),
+	.gate_clks	= gscl_gate_clks,
+	.nr_gate_clks	= ARRAY_SIZE(gscl_gate_clks),
+	.nr_clk_ids	= GSCL_NR_CLK,
+	.clk_regs	= gscl_clk_regs,
+	.nr_clk_regs	= ARRAY_SIZE(gscl_clk_regs),
+};
+
 static void __init exynos5260_clk_gscl_init(struct device_node *np)
 {
-	struct samsung_cmu_info cmu = { NULL };
-
-	cmu.mux_clks = gscl_mux_clks;
-	cmu.nr_mux_clks = ARRAY_SIZE(gscl_mux_clks);
-	cmu.div_clks = gscl_div_clks;
-	cmu.nr_div_clks = ARRAY_SIZE(gscl_div_clks);
-	cmu.gate_clks = gscl_gate_clks;
-	cmu.nr_gate_clks = ARRAY_SIZE(gscl_gate_clks);
-	cmu.nr_clk_ids = GSCL_NR_CLK;
-	cmu.clk_regs = gscl_clk_regs;
-	cmu.nr_clk_regs = ARRAY_SIZE(gscl_clk_regs);
-
-	samsung_cmu_register_one(np, &cmu);
+	samsung_cmu_register_one(np, &gscl_cmu);
 }
 
 CLK_OF_DECLARE(exynos5260_clk_gscl, "samsung,exynos5260-clock-gscl",
@@ -891,21 +891,21 @@ static const struct samsung_gate_clock isp_gate_clks[] __initconst = {
 			EN_SCLK_ISP, 9, CLK_SET_RATE_PARENT, 0),
 };
 
+static const struct samsung_cmu_info isp_cmu __initconst = {
+	.mux_clks	= isp_mux_clks,
+	.nr_mux_clks	= ARRAY_SIZE(isp_mux_clks),
+	.div_clks	= isp_div_clks,
+	.nr_div_clks	= ARRAY_SIZE(isp_div_clks),
+	.gate_clks	= isp_gate_clks,
+	.nr_gate_clks	= ARRAY_SIZE(isp_gate_clks),
+	.nr_clk_ids	= ISP_NR_CLK,
+	.clk_regs	= isp_clk_regs,
+	.nr_clk_regs	= ARRAY_SIZE(isp_clk_regs),
+};
+
 static void __init exynos5260_clk_isp_init(struct device_node *np)
 {
-	struct samsung_cmu_info cmu = { NULL };
-
-	cmu.mux_clks = isp_mux_clks;
-	cmu.nr_mux_clks = ARRAY_SIZE(isp_mux_clks);
-	cmu.div_clks = isp_div_clks;
-	cmu.nr_div_clks = ARRAY_SIZE(isp_div_clks);
-	cmu.gate_clks = isp_gate_clks;
-	cmu.nr_gate_clks = ARRAY_SIZE(isp_gate_clks);
-	cmu.nr_clk_ids = ISP_NR_CLK;
-	cmu.clk_regs = isp_clk_regs;
-	cmu.nr_clk_regs = ARRAY_SIZE(isp_clk_regs);
-
-	samsung_cmu_register_one(np, &cmu);
+	samsung_cmu_register_one(np, &isp_cmu);
 }
 
 CLK_OF_DECLARE(exynos5260_clk_isp, "samsung,exynos5260-clock-isp",
@@ -955,21 +955,21 @@ static const struct samsung_pll_clock kfc_pll_clks[] __initconst = {
 		pll2550_24mhz_tbl),
 };
 
+static const struct samsung_cmu_info kfc_cmu __initconst = {
+	.pll_clks	= kfc_pll_clks,
+	.nr_pll_clks	= ARRAY_SIZE(kfc_pll_clks),
+	.mux_clks	= kfc_mux_clks,
+	.nr_mux_clks	= ARRAY_SIZE(kfc_mux_clks),
+	.div_clks	= kfc_div_clks,
+	.nr_div_clks	= ARRAY_SIZE(kfc_div_clks),
+	.nr_clk_ids	= KFC_NR_CLK,
+	.clk_regs	= kfc_clk_regs,
+	.nr_clk_regs	= ARRAY_SIZE(kfc_clk_regs),
+};
+
 static void __init exynos5260_clk_kfc_init(struct device_node *np)
 {
-	struct samsung_cmu_info cmu = { NULL };
-
-	cmu.pll_clks = kfc_pll_clks;
-	cmu.nr_pll_clks =  ARRAY_SIZE(kfc_pll_clks);
-	cmu.mux_clks = kfc_mux_clks;
-	cmu.nr_mux_clks = ARRAY_SIZE(kfc_mux_clks);
-	cmu.div_clks = kfc_div_clks;
-	cmu.nr_div_clks = ARRAY_SIZE(kfc_div_clks);
-	cmu.nr_clk_ids = KFC_NR_CLK;
-	cmu.clk_regs = kfc_clk_regs;
-	cmu.nr_clk_regs = ARRAY_SIZE(kfc_clk_regs);
-
-	samsung_cmu_register_one(np, &cmu);
+	samsung_cmu_register_one(np, &kfc_cmu);
 }
 
 CLK_OF_DECLARE(exynos5260_clk_kfc, "samsung,exynos5260-clock-kfc",
@@ -1011,21 +1011,21 @@ static const struct samsung_gate_clock mfc_gate_clks[] __initconst = {
 			EN_IP_MFC_SECURE_SMMU2_MFC, 7, 0, 0),
 };
 
+static const struct samsung_cmu_info mfc_cmu __initconst = {
+	.mux_clks	= mfc_mux_clks,
+	.nr_mux_clks	= ARRAY_SIZE(mfc_mux_clks),
+	.div_clks	= mfc_div_clks,
+	.nr_div_clks	= ARRAY_SIZE(mfc_div_clks),
+	.gate_clks	= mfc_gate_clks,
+	.nr_gate_clks	= ARRAY_SIZE(mfc_gate_clks),
+	.nr_clk_ids	= MFC_NR_CLK,
+	.clk_regs	= mfc_clk_regs,
+	.nr_clk_regs	= ARRAY_SIZE(mfc_clk_regs),
+};
+
 static void __init exynos5260_clk_mfc_init(struct device_node *np)
 {
-	struct samsung_cmu_info cmu = { NULL };
-
-	cmu.mux_clks = mfc_mux_clks;
-	cmu.nr_mux_clks = ARRAY_SIZE(mfc_mux_clks);
-	cmu.div_clks = mfc_div_clks;
-	cmu.nr_div_clks = ARRAY_SIZE(mfc_div_clks);
-	cmu.gate_clks = mfc_gate_clks;
-	cmu.nr_gate_clks = ARRAY_SIZE(mfc_gate_clks);
-	cmu.nr_clk_ids = MFC_NR_CLK;
-	cmu.clk_regs = mfc_clk_regs;
-	cmu.nr_clk_regs = ARRAY_SIZE(mfc_clk_regs);
-
-	samsung_cmu_register_one(np, &cmu);
+	samsung_cmu_register_one(np, &mfc_cmu);
 }
 
 CLK_OF_DECLARE(exynos5260_clk_mfc, "samsung,exynos5260-clock-mfc",
@@ -1158,23 +1158,23 @@ static const struct samsung_pll_clock mif_pll_clks[] __initconst = {
 		pll2550_24mhz_tbl),
 };
 
+static const struct samsung_cmu_info mif_cmu __initconst = {
+	.pll_clks	= mif_pll_clks,
+	.nr_pll_clks	= ARRAY_SIZE(mif_pll_clks),
+	.mux_clks	= mif_mux_clks,
+	.nr_mux_clks	= ARRAY_SIZE(mif_mux_clks),
+	.div_clks	= mif_div_clks,
+	.nr_div_clks	= ARRAY_SIZE(mif_div_clks),
+	.gate_clks	= mif_gate_clks,
+	.nr_gate_clks	= ARRAY_SIZE(mif_gate_clks),
+	.nr_clk_ids	= MIF_NR_CLK,
+	.clk_regs	= mif_clk_regs,
+	.nr_clk_regs	= ARRAY_SIZE(mif_clk_regs),
+};
+
 static void __init exynos5260_clk_mif_init(struct device_node *np)
 {
-	struct samsung_cmu_info cmu = { NULL };
-
-	cmu.pll_clks = mif_pll_clks;
-	cmu.nr_pll_clks =  ARRAY_SIZE(mif_pll_clks);
-	cmu.mux_clks = mif_mux_clks;
-	cmu.nr_mux_clks = ARRAY_SIZE(mif_mux_clks);
-	cmu.div_clks = mif_div_clks;
-	cmu.nr_div_clks = ARRAY_SIZE(mif_div_clks);
-	cmu.gate_clks = mif_gate_clks;
-	cmu.nr_gate_clks = ARRAY_SIZE(mif_gate_clks);
-	cmu.nr_clk_ids = MIF_NR_CLK;
-	cmu.clk_regs = mif_clk_regs;
-	cmu.nr_clk_regs = ARRAY_SIZE(mif_clk_regs);
-
-	samsung_cmu_register_one(np, &cmu);
+	samsung_cmu_register_one(np, &mif_cmu);
 }
 
 CLK_OF_DECLARE(exynos5260_clk_mif, "samsung,exynos5260-clock-mif",
@@ -1366,21 +1366,21 @@ static const struct samsung_gate_clock peri_gate_clks[] __initconst = {
 		EN_IP_PERI_SECURE_TZPC, 20, 0, 0),
 };
 
+static const struct samsung_cmu_info peri_cmu __initconst = {
+	.mux_clks	= peri_mux_clks,
+	.nr_mux_clks	= ARRAY_SIZE(peri_mux_clks),
+	.div_clks	= peri_div_clks,
+	.nr_div_clks	= ARRAY_SIZE(peri_div_clks),
+	.gate_clks	= peri_gate_clks,
+	.nr_gate_clks	= ARRAY_SIZE(peri_gate_clks),
+	.nr_clk_ids	= PERI_NR_CLK,
+	.clk_regs	= peri_clk_regs,
+	.nr_clk_regs	= ARRAY_SIZE(peri_clk_regs),
+};
+
 static void __init exynos5260_clk_peri_init(struct device_node *np)
 {
-	struct samsung_cmu_info cmu = { NULL };
-
-	cmu.mux_clks = peri_mux_clks;
-	cmu.nr_mux_clks = ARRAY_SIZE(peri_mux_clks);
-	cmu.div_clks = peri_div_clks;
-	cmu.nr_div_clks = ARRAY_SIZE(peri_div_clks);
-	cmu.gate_clks = peri_gate_clks;
-	cmu.nr_gate_clks = ARRAY_SIZE(peri_gate_clks);
-	cmu.nr_clk_ids = PERI_NR_CLK;
-	cmu.clk_regs = peri_clk_regs;
-	cmu.nr_clk_regs = ARRAY_SIZE(peri_clk_regs);
-
-	samsung_cmu_register_one(np, &cmu);
+	samsung_cmu_register_one(np, &peri_cmu);
 }
 
 CLK_OF_DECLARE(exynos5260_clk_peri, "samsung,exynos5260-clock-peri",
@@ -1818,25 +1818,25 @@ static const struct samsung_pll_clock top_pll_clks[] __initconst = {
 		pll2650_24mhz_tbl),
 };
 
+static const struct samsung_cmu_info top_cmu __initconst = {
+	.pll_clks	= top_pll_clks,
+	.nr_pll_clks	= ARRAY_SIZE(top_pll_clks),
+	.mux_clks	= top_mux_clks,
+	.nr_mux_clks	= ARRAY_SIZE(top_mux_clks),
+	.div_clks	= top_div_clks,
+	.nr_div_clks	= ARRAY_SIZE(top_div_clks),
+	.gate_clks	= top_gate_clks,
+	.nr_gate_clks	= ARRAY_SIZE(top_gate_clks),
+	.fixed_clks	= fixed_rate_clks,
+	.nr_fixed_clks	= ARRAY_SIZE(fixed_rate_clks),
+	.nr_clk_ids	= TOP_NR_CLK,
+	.clk_regs	= top_clk_regs,
+	.nr_clk_regs	= ARRAY_SIZE(top_clk_regs),
+};
+
 static void __init exynos5260_clk_top_init(struct device_node *np)
 {
-	struct samsung_cmu_info cmu = { NULL };
-
-	cmu.pll_clks = top_pll_clks;
-	cmu.nr_pll_clks =  ARRAY_SIZE(top_pll_clks);
-	cmu.mux_clks = top_mux_clks;
-	cmu.nr_mux_clks = ARRAY_SIZE(top_mux_clks);
-	cmu.div_clks = top_div_clks;
-	cmu.nr_div_clks = ARRAY_SIZE(top_div_clks);
-	cmu.gate_clks = top_gate_clks;
-	cmu.nr_gate_clks = ARRAY_SIZE(top_gate_clks);
-	cmu.fixed_clks = fixed_rate_clks;
-	cmu.nr_fixed_clks = ARRAY_SIZE(fixed_rate_clks);
-	cmu.nr_clk_ids = TOP_NR_CLK;
-	cmu.clk_regs = top_clk_regs;
-	cmu.nr_clk_regs = ARRAY_SIZE(top_clk_regs);
-
-	samsung_cmu_register_one(np, &cmu);
+	samsung_cmu_register_one(np, &top_cmu);
 }
 
 CLK_OF_DECLARE(exynos5260_clk_top, "samsung,exynos5260-clock-top",
diff --git a/drivers/clk/samsung/clk-exynos5410.c b/drivers/clk/samsung/clk-exynos5410.c
index 54ec486a5e45..fc471a49e8f4 100644
--- a/drivers/clk/samsung/clk-exynos5410.c
+++ b/drivers/clk/samsung/clk-exynos5410.c
@@ -14,6 +14,7 @@
 #include <linux/clk-provider.h>
 #include <linux/of.h>
 #include <linux/of_address.h>
+#include <linux/clk.h>
 
 #include "clk.h"
 
@@ -21,6 +22,8 @@
 #define APLL_CON0               0x100
 #define CPLL_LOCK               0x10020
 #define CPLL_CON0               0x10120
+#define EPLL_LOCK               0x10040
+#define EPLL_CON0               0x10130
 #define MPLL_LOCK               0x4000
 #define MPLL_CON0               0x4100
 #define BPLL_LOCK               0x20010
@@ -58,7 +61,7 @@
 
 /* list of PLLs */
 enum exynos5410_plls {
-	apll, cpll, mpll,
+	apll, cpll, epll, mpll,
 	bpll, kpll,
 	nr_plls                 /* number of PLLs */
 };
@@ -67,6 +70,7 @@ enum exynos5410_plls {
 PNAME(apll_p)		= { "fin_pll", "fout_apll", };
 PNAME(bpll_p)		= { "fin_pll", "fout_bpll", };
 PNAME(cpll_p)		= { "fin_pll", "fout_cpll" };
+PNAME(epll_p)		= { "fin_pll", "fout_epll" };
 PNAME(mpll_p)		= { "fin_pll", "fout_mpll", };
 PNAME(kpll_p)		= { "fin_pll", "fout_kpll", };
 
@@ -95,6 +99,8 @@ static const struct samsung_mux_clock exynos5410_mux_clks[] __initconst = {
 	MUX(0, "sclk_bpll", bpll_p, SRC_CDREX, 0, 1),
 	MUX(0, "sclk_bpll_muxed", bpll_user_p, SRC_TOP2, 24, 1),
 
+	MUX(0, "sclk_epll", epll_p, SRC_TOP2, 12, 1),
+
 	MUX(0, "sclk_cpll", cpll_p, SRC_TOP2, 8, 1),
 
 	MUX(0, "sclk_mpll_bpll", mpll_bpll_p, SRC_TOP1, 20, 1),
@@ -176,6 +182,8 @@ static const struct samsung_gate_clock exynos5410_gate_clks[] __initconst = {
 	GATE(CLK_MMC0, "sdmmc0", "aclk200", GATE_BUS_FSYS0, 12, 0, 0),
 	GATE(CLK_MMC1, "sdmmc1", "aclk200", GATE_BUS_FSYS0, 13, 0, 0),
 	GATE(CLK_MMC2, "sdmmc2", "aclk200", GATE_BUS_FSYS0, 14, 0, 0),
+	GATE(CLK_PDMA1, "pdma1", "aclk200", GATE_BUS_FSYS0, 2, 0, 0),
+	GATE(CLK_PDMA0, "pdma0", "aclk200", GATE_BUS_FSYS0, 1, 0, 0),
 
 	GATE(CLK_SCLK_USBPHY301, "sclk_usbphy301", "dout_usbphy301",
 	     GATE_TOP_SCLK_FSYS, 7, CLK_SET_RATE_PARENT, 0),
@@ -217,11 +225,26 @@ static const struct samsung_gate_clock exynos5410_gate_clks[] __initconst = {
 	GATE(CLK_USBD301, "usbd301", "aclk200_fsys", GATE_IP_FSYS, 20, 0, 0),
 };
 
-static const struct samsung_pll_clock exynos5410_plls[nr_plls] __initconst = {
+static const struct samsung_pll_rate_table exynos5410_pll2550x_24mhz_tbl[] __initconst = {
+	PLL_36XX_RATE(400000000U, 200, 3, 2, 0),
+	PLL_36XX_RATE(333000000U, 111, 2, 2, 0),
+	PLL_36XX_RATE(300000000U, 100, 2, 2, 0),
+	PLL_36XX_RATE(266000000U, 266, 3, 3, 0),
+	PLL_36XX_RATE(200000000U, 200, 3, 3, 0),
+	PLL_36XX_RATE(192000000U, 192, 3, 3, 0),
+	PLL_36XX_RATE(166000000U, 166, 3, 3, 0),
+	PLL_36XX_RATE(133000000U, 266, 3, 4, 0),
+	PLL_36XX_RATE(100000000U, 200, 3, 4, 0),
+	PLL_36XX_RATE(66000000U,  176, 2, 5, 0),
+};
+
+static struct samsung_pll_clock exynos5410_plls[nr_plls] __initdata = {
 	[apll] = PLL(pll_35xx, CLK_FOUT_APLL, "fout_apll", "fin_pll", APLL_LOCK,
 		APLL_CON0, NULL),
 	[cpll] = PLL(pll_35xx, CLK_FOUT_CPLL, "fout_cpll", "fin_pll", CPLL_LOCK,
 		CPLL_CON0, NULL),
+	[epll] = PLL(pll_2650x, CLK_FOUT_EPLL, "fout_epll", "fin_pll", EPLL_LOCK,
+		EPLL_CON0, NULL),
 	[mpll] = PLL(pll_35xx, CLK_FOUT_MPLL, "fout_mpll", "fin_pll", MPLL_LOCK,
 		MPLL_CON0, NULL),
 	[bpll] = PLL(pll_35xx, CLK_FOUT_BPLL, "fout_bpll", "fin_pll", BPLL_LOCK,
@@ -230,29 +253,27 @@ static const struct samsung_pll_clock exynos5410_plls[nr_plls] __initconst = {
 		KPLL_CON0, NULL),
 };
 
+static const struct samsung_cmu_info cmu __initconst = {
+	.pll_clks	= exynos5410_plls,
+	.nr_pll_clks	= ARRAY_SIZE(exynos5410_plls),
+	.mux_clks	= exynos5410_mux_clks,
+	.nr_mux_clks	= ARRAY_SIZE(exynos5410_mux_clks),
+	.div_clks	= exynos5410_div_clks,
+	.nr_div_clks	= ARRAY_SIZE(exynos5410_div_clks),
+	.gate_clks	= exynos5410_gate_clks,
+	.nr_gate_clks	= ARRAY_SIZE(exynos5410_gate_clks),
+	.nr_clk_ids	= CLK_NR_CLKS,
+};
+
 /* register exynos5410 clocks */
 static void __init exynos5410_clk_init(struct device_node *np)
 {
-	struct samsung_clk_provider *ctx;
-	void __iomem *reg_base;
-
-	reg_base = of_iomap(np, 0);
-	if (!reg_base)
-		panic("%s: failed to map registers\n", __func__);
-
-	ctx = samsung_clk_init(np, reg_base, CLK_NR_CLKS);
-
-	samsung_clk_register_pll(ctx, exynos5410_plls,
-			ARRAY_SIZE(exynos5410_plls), reg_base);
+	struct clk *xxti = of_clk_get(np, 0);
 
-	samsung_clk_register_mux(ctx, exynos5410_mux_clks,
-			ARRAY_SIZE(exynos5410_mux_clks));
-	samsung_clk_register_div(ctx, exynos5410_div_clks,
-			ARRAY_SIZE(exynos5410_div_clks));
-	samsung_clk_register_gate(ctx, exynos5410_gate_clks,
-			ARRAY_SIZE(exynos5410_gate_clks));
+	if (!IS_ERR(xxti) && clk_get_rate(xxti) == 24 * MHZ)
+		exynos5410_plls[epll].rate_table = exynos5410_pll2550x_24mhz_tbl;
 
-	samsung_clk_of_add_provider(np, ctx);
+	samsung_cmu_register_one(np, &cmu);
 
 	pr_debug("Exynos5410: clock setup completed.\n");
 }
diff --git a/drivers/clk/samsung/clk-exynos5420.c b/drivers/clk/samsung/clk-exynos5420.c
index bb196ca21a77..8c8b495cbf0d 100644
--- a/drivers/clk/samsung/clk-exynos5420.c
+++ b/drivers/clk/samsung/clk-exynos5420.c
@@ -131,6 +131,9 @@
 #define TOP_SPARE2		0x10b08
 #define BPLL_LOCK		0x20010
 #define BPLL_CON0		0x20110
+#define SRC_CDREX		0x20200
+#define DIV_CDREX0		0x20500
+#define DIV_CDREX1		0x20504
 #define KPLL_LOCK		0x28000
 #define KPLL_CON0		0x28100
 #define SRC_KFC			0x28200
@@ -244,6 +247,9 @@ static const unsigned long exynos5x_clk_regs[] __initconst = {
 	GATE_TOP_SCLK_FSYS,
 	GATE_TOP_SCLK_PERIC,
 	TOP_SPARE2,
+	SRC_CDREX,
+	DIV_CDREX0,
+	DIV_CDREX1,
 	SRC_KFC,
 	DIV_KFC0,
 };
@@ -448,6 +454,8 @@ PNAME(mout_maudio0_p) = {"fin_pll", "maudio_clk", "mout_sclk_dpll",
 			 "mout_sclk_epll", "mout_sclk_rpll"};
 PNAME(mout_mau_epll_clk_p) = {"mout_sclk_epll", "mout_sclk_dpll",
 				"mout_sclk_mpll", "mout_sclk_spll"};
+PNAME(mout_mclk_cdrex_p) = {"mout_bpll", "mout_mx_mspll_ccore"};
+
 /* List of parents specific to exynos5800 */
 PNAME(mout_epll2_5800_p)	= { "mout_sclk_epll", "ff_dout_epll2" };
 PNAME(mout_group1_5800_p)	= { "mout_sclk_cpll", "mout_sclk_dpll",
@@ -465,6 +473,9 @@ PNAME(mout_group6_5800_p)	= { "mout_sclk_ipll", "mout_sclk_dpll",
 PNAME(mout_group7_5800_p)	= { "mout_sclk_cpll", "mout_sclk_dpll",
 					"mout_sclk_mpll", "mout_sclk_spll",
 					"mout_epll2", "mout_sclk_ipll" };
+PNAME(mout_mx_mspll_ccore_p)	= {"sclk_bpll", "mout_sclk_dpll",
+					"mout_sclk_mpll", "ff_dout_spll2",
+					"mout_sclk_spll", "mout_sclk_epll"};
 PNAME(mout_mau_epll_clk_5800_p)	= { "mout_sclk_epll", "mout_sclk_dpll",
 					"mout_sclk_mpll",
 					"ff_dout_spll2" };
@@ -523,6 +534,8 @@ static const struct samsung_mux_clock exynos5800_mux_clks[] __initconst = {
 	MUX(0, "mout_aclk300_disp1", mout_group5_5800_p, SRC_TOP2, 24, 2),
 	MUX(0, "mout_aclk300_gscl", mout_group5_5800_p, SRC_TOP2, 28, 2),
 
+	MUX(CLK_MOUT_MX_MSPLL_CCORE, "mout_mx_mspll_ccore",
+			mout_mx_mspll_ccore_p, SRC_TOP7, 16, 2),
 	MUX(0, "mout_mau_epll_clk", mout_mau_epll_clk_5800_p, SRC_TOP7,
 			20, 2),
 	MUX(0, "sclk_bpll", mout_bpll_p, SRC_TOP7, 24, 1),
@@ -601,6 +614,8 @@ static const struct samsung_mux_clock exynos5420_mux_clks[] __initconst = {
 	MUX(0, "mout_aclk300_disp1", mout_group1_p, SRC_TOP2, 24, 2),
 	MUX(0, "mout_aclk300_gscl", mout_group1_p, SRC_TOP2, 28, 2),
 
+	MUX(CLK_MOUT_MX_MSPLL_CCORE, "mout_mx_mspll_ccore",
+			mout_group5_5800_p, SRC_TOP7, 16, 2),
 	MUX(0, "mout_mau_epll_clk", mout_mau_epll_clk_p, SRC_TOP7, 20, 2),
 
 	MUX(0, "mout_fimd1", mout_group3_p, SRC_DISP10, 4, 1),
@@ -744,6 +759,12 @@ static const struct samsung_mux_clock exynos5x_mux_clks[] __initconst = {
 
 	MUX(0, "mout_fimd1_final", mout_fimd1_final_p, TOP_SPARE2, 8, 1),
 
+	/* CDREX block */
+	MUX_F(CLK_MOUT_MCLK_CDREX, "mout_mclk_cdrex", mout_mclk_cdrex_p,
+			SRC_CDREX, 4, 1, CLK_SET_RATE_PARENT, 0),
+	MUX_F(CLK_MOUT_BPLL, "mout_bpll", mout_bpll_p, SRC_CDREX, 0, 1,
+			CLK_SET_RATE_PARENT, 0),
+
 	/* MAU Block */
 	MUX(CLK_MOUT_MAUDIO0, "mout_maudio0", mout_maudio0_p, SRC_MAU, 28, 3),
 
@@ -836,6 +857,21 @@ static const struct samsung_div_clock exynos5x_div_clks[] __initconst = {
 	DIV(CLK_DOUT_ACLK400_DISP1, "dout_aclk400_disp1",
 			"mout_aclk400_disp1", DIV_TOP2, 4, 3),
 
+	/* CDREX Block */
+	DIV(CLK_DOUT_PCLK_CDREX, "dout_pclk_cdrex", "dout_aclk_cdrex1",
+			DIV_CDREX0, 28, 3),
+	DIV_F(CLK_DOUT_SCLK_CDREX, "dout_sclk_cdrex", "mout_mclk_cdrex",
+			DIV_CDREX0, 24, 3, CLK_SET_RATE_PARENT, 0),
+	DIV(CLK_DOUT_ACLK_CDREX1, "dout_aclk_cdrex1", "dout_clk2x_phy0",
+			DIV_CDREX0, 16, 3),
+	DIV(CLK_DOUT_CCLK_DREX0, "dout_cclk_drex0", "dout_clk2x_phy0",
+			DIV_CDREX0, 8, 3),
+	DIV(CLK_DOUT_CLK2X_PHY0, "dout_clk2x_phy0", "dout_sclk_cdrex",
+			DIV_CDREX0, 3, 5),
+
+	DIV(CLK_DOUT_PCLK_CORE_MEM, "dout_pclk_core_mem", "mout_mclk_cdrex",
+			DIV_CDREX1, 8, 3),
+
 	/* Audio Block */
 	DIV(0, "dout_maudio0", "mout_maudio0", DIV_MAU, 20, 4),
 	DIV(0, "dout_maupcm0", "dout_maudio0", DIV_MAU, 24, 8),
@@ -1364,6 +1400,7 @@ static void __init exynos5x_clk_init(struct device_node *np,
 	if (_get_rate("fin_pll") == 24 * MHZ) {
 		exynos5x_plls[apll].rate_table = exynos5420_pll2550x_24mhz_tbl;
 		exynos5x_plls[kpll].rate_table = exynos5420_pll2550x_24mhz_tbl;
+		exynos5x_plls[bpll].rate_table = exynos5420_pll2550x_24mhz_tbl;
 	}
 
 	samsung_clk_register_pll(ctx, exynos5x_plls, ARRAY_SIZE(exynos5x_plls),
diff --git a/drivers/clk/samsung/clk-exynos5440.c b/drivers/clk/samsung/clk-exynos5440.c
index a57d01b99b76..a80f3ef20801 100644
--- a/drivers/clk/samsung/clk-exynos5440.c
+++ b/drivers/clk/samsung/clk-exynos5440.c
@@ -112,6 +112,11 @@ static struct notifier_block exynos5440_clk_restart_handler = {
 	.priority = 128,
 };
 
+static const struct samsung_pll_clock exynos5440_plls[] __initconst = {
+	PLL(pll_2550x, CLK_CPLLA, "cplla", "xtal", 0, 0x4c, NULL),
+	PLL(pll_2550x, CLK_CPLLB, "cpllb", "xtal", 0, 0x50, NULL),
+};
+
 /* register exynos5440 clocks */
 static void __init exynos5440_clk_init(struct device_node *np)
 {
@@ -129,8 +134,8 @@ static void __init exynos5440_clk_init(struct device_node *np)
 	samsung_clk_of_register_fixed_ext(ctx, exynos5440_fixed_rate_ext_clks,
 		ARRAY_SIZE(exynos5440_fixed_rate_ext_clks), ext_clk_match);
 
-	samsung_clk_register_pll2550x("cplla", "xtal", reg_base + 0x1c, 0x10);
-	samsung_clk_register_pll2550x("cpllb", "xtal", reg_base + 0x20, 0x10);
+	samsung_clk_register_pll(ctx, exynos5440_plls,
+			ARRAY_SIZE(exynos5440_plls), ctx->reg_base);
 
 	samsung_clk_register_fixed_rate(ctx, exynos5440_fixed_rate_clks,
 			ARRAY_SIZE(exynos5440_fixed_rate_clks));
diff --git a/drivers/clk/samsung/clk-pll.c b/drivers/clk/samsung/clk-pll.c
index 48139bd510f1..9617825daabb 100644
--- a/drivers/clk/samsung/clk-pll.c
+++ b/drivers/clk/samsung/clk-pll.c
@@ -890,22 +890,14 @@ static const struct clk_ops samsung_s3c2440_mpll_clk_ops = {
 #define PLL2550X_M_SHIFT      (4)
 #define PLL2550X_S_SHIFT      (0)
 
-struct samsung_clk_pll2550x {
-	struct clk_hw		hw;
-	const void __iomem	*reg_base;
-	unsigned long		offset;
-};
-
-#define to_clk_pll2550x(_hw) container_of(_hw, struct samsung_clk_pll2550x, hw)
-
 static unsigned long samsung_pll2550x_recalc_rate(struct clk_hw *hw,
 				unsigned long parent_rate)
 {
-	struct samsung_clk_pll2550x *pll = to_clk_pll2550x(hw);
+	struct samsung_clk_pll *pll = to_clk_pll(hw);
 	u32 r, p, m, s, pll_stat;
 	u64 fvco = parent_rate;
 
-	pll_stat = readl_relaxed(pll->reg_base + pll->offset * 3);
+	pll_stat = readl_relaxed(pll->con_reg);
 	r = (pll_stat >> PLL2550X_R_SHIFT) & PLL2550X_R_MASK;
 	if (!r)
 		return 0;
@@ -923,43 +915,6 @@ static const struct clk_ops samsung_pll2550x_clk_ops = {
 	.recalc_rate = samsung_pll2550x_recalc_rate,
 };
 
-struct clk * __init samsung_clk_register_pll2550x(const char *name,
-			const char *pname, const void __iomem *reg_base,
-			const unsigned long offset)
-{
-	struct samsung_clk_pll2550x *pll;
-	struct clk *clk;
-	struct clk_init_data init;
-
-	pll = kzalloc(sizeof(*pll), GFP_KERNEL);
-	if (!pll) {
-		pr_err("%s: could not allocate pll clk %s\n", __func__, name);
-		return NULL;
-	}
-
-	init.name = name;
-	init.ops = &samsung_pll2550x_clk_ops;
-	init.flags = CLK_GET_RATE_NOCACHE;
-	init.parent_names = &pname;
-	init.num_parents = 1;
-
-	pll->hw.init = &init;
-	pll->reg_base = reg_base;
-	pll->offset = offset;
-
-	clk = clk_register(NULL, &pll->hw);
-	if (IS_ERR(clk)) {
-		pr_err("%s: failed to register pll clock %s\n", __func__,
-				name);
-		kfree(pll);
-	}
-
-	if (clk_register_clkdev(clk, name, NULL))
-		pr_err("%s: failed to register lookup for %s", __func__, name);
-
-	return clk;
-}
-
 /*
  * PLL2550xx Clock Type
  */
@@ -1063,6 +1018,102 @@ static const struct clk_ops samsung_pll2550xx_clk_min_ops = {
 };
 
 /*
+ * PLL2650x Clock Type
+ */
+
+/* Maximum lock time can be 3000 * PDIV cycles */
+#define PLL2650X_LOCK_FACTOR		3000
+
+#define PLL2650X_M_MASK			0x1ff
+#define PLL2650X_P_MASK			0x3f
+#define PLL2650X_S_MASK			0x7
+#define PLL2650X_K_MASK			0xffff
+#define PLL2650X_LOCK_STAT_MASK		0x1
+#define PLL2650X_M_SHIFT		16
+#define PLL2650X_P_SHIFT		8
+#define PLL2650X_S_SHIFT		0
+#define PLL2650X_K_SHIFT		0
+#define PLL2650X_LOCK_STAT_SHIFT	29
+#define PLL2650X_PLL_ENABLE_SHIFT	31
+
+static unsigned long samsung_pll2650x_recalc_rate(struct clk_hw *hw,
+				unsigned long parent_rate)
+{
+	struct samsung_clk_pll *pll = to_clk_pll(hw);
+	u64 fout = parent_rate;
+	u32 mdiv, pdiv, sdiv, pll_con0, pll_con1;
+	s16 kdiv;
+
+	pll_con0 = readl_relaxed(pll->con_reg);
+	mdiv = (pll_con0 >> PLL2650X_M_SHIFT) & PLL2650X_M_MASK;
+	pdiv = (pll_con0 >> PLL2650X_P_SHIFT) & PLL2650X_P_MASK;
+	sdiv = (pll_con0 >> PLL2650X_S_SHIFT) & PLL2650X_S_MASK;
+
+	pll_con1 = readl_relaxed(pll->con_reg + 4);
+	kdiv = (s16)((pll_con1 >> PLL2650X_K_SHIFT) & PLL2650X_K_MASK);
+
+	fout *= (mdiv << 16) + kdiv;
+	do_div(fout, (pdiv << sdiv));
+	fout >>= 16;
+
+	return (unsigned long)fout;
+}
+
+static int samsung_pll2650x_set_rate(struct clk_hw *hw, unsigned long drate,
+					unsigned long prate)
+{
+	struct samsung_clk_pll *pll = to_clk_pll(hw);
+	const struct samsung_pll_rate_table *rate;
+	u32 con0, con1;
+
+	/* Get required rate settings from table */
+	rate = samsung_get_pll_settings(pll, drate);
+	if (!rate) {
+		pr_err("%s: Invalid rate : %lu for pll clk %s\n", __func__,
+			drate, clk_hw_get_name(hw));
+		return -EINVAL;
+	}
+
+	con0 = readl_relaxed(pll->con_reg);
+	con1 = readl_relaxed(pll->con_reg + 4);
+
+	/* Set PLL lock time. */
+	writel_relaxed(rate->pdiv * PLL2650X_LOCK_FACTOR, pll->lock_reg);
+
+	/* Change PLL PMS values */
+	con0 &= ~((PLL2650X_M_MASK << PLL2650X_M_SHIFT) |
+			(PLL2650X_P_MASK << PLL2650X_P_SHIFT) |
+			(PLL2650X_S_MASK << PLL2650X_S_SHIFT));
+	con0 |= (rate->mdiv << PLL2650X_M_SHIFT) |
+			(rate->pdiv << PLL2650X_P_SHIFT) |
+			(rate->sdiv << PLL2650X_S_SHIFT);
+	con0 |= (1 << PLL2650X_PLL_ENABLE_SHIFT);
+	writel_relaxed(con0, pll->con_reg);
+
+	con1 &= ~(PLL2650X_K_MASK << PLL2650X_K_SHIFT);
+	con1 |= ((rate->kdiv & PLL2650X_K_MASK) << PLL2650X_K_SHIFT);
+	writel_relaxed(con1, pll->con_reg + 4);
+
+	do {
+		cpu_relax();
+		con0 = readl_relaxed(pll->con_reg);
+	} while (!(con0 & (PLL2650X_LOCK_STAT_MASK
+			<< PLL2650X_LOCK_STAT_SHIFT)));
+
+	return 0;
+}
+
+static const struct clk_ops samsung_pll2650x_clk_ops = {
+	.recalc_rate = samsung_pll2650x_recalc_rate,
+	.round_rate = samsung_pll_round_rate,
+	.set_rate = samsung_pll2650x_set_rate,
+};
+
+static const struct clk_ops samsung_pll2650x_clk_min_ops = {
+	.recalc_rate = samsung_pll2650x_recalc_rate,
+};
+
+/*
  * PLL2650XX Clock Type
  */
 
@@ -1263,12 +1314,21 @@ static void __init _samsung_clk_register_pll(struct samsung_clk_provider *ctx,
 		else
 			init.ops = &samsung_s3c2440_mpll_clk_ops;
 		break;
+	case pll_2550x:
+		init.ops = &samsung_pll2550x_clk_ops;
+		break;
 	case pll_2550xx:
 		if (!pll->rate_table)
 			init.ops = &samsung_pll2550xx_clk_min_ops;
 		else
 			init.ops = &samsung_pll2550xx_clk_ops;
 		break;
+	case pll_2650x:
+		if (!pll->rate_table)
+			init.ops = &samsung_pll2650x_clk_min_ops;
+		else
+			init.ops = &samsung_pll2650x_clk_ops;
+		break;
 	case pll_2650xx:
 		if (!pll->rate_table)
 			init.ops = &samsung_pll2650xx_clk_min_ops;
diff --git a/drivers/clk/samsung/clk-pll.h b/drivers/clk/samsung/clk-pll.h
index 213de9af8b4f..a1ca0233cb4b 100644
--- a/drivers/clk/samsung/clk-pll.h
+++ b/drivers/clk/samsung/clk-pll.h
@@ -31,7 +31,9 @@ enum samsung_pll_type {
 	pll_s3c2410_mpll,
 	pll_s3c2410_upll,
 	pll_s3c2440_mpll,
+	pll_2550x,
 	pll_2550xx,
+	pll_2650x,
 	pll_2650xx,
 	pll_1450x,
 	pll_1451x,
diff --git a/drivers/clk/st/clk-flexgen.c b/drivers/clk/st/clk-flexgen.c
index 546bd79c8e3a..a485f3b284b9 100644
--- a/drivers/clk/st/clk-flexgen.c
+++ b/drivers/clk/st/clk-flexgen.c
@@ -15,6 +15,11 @@
 #include <linux/of.h>
 #include <linux/of_address.h>
 
+struct clkgen_data {
+	unsigned long flags;
+	bool mode;
+};
+
 struct flexgen {
 	struct clk_hw hw;
 
@@ -28,9 +33,14 @@ struct flexgen {
 	struct clk_gate fgate;
 	/* Final divisor */
 	struct clk_divider fdiv;
+	/* Asynchronous mode control */
+	struct clk_gate sync;
+	/* hw control flags */
+	bool control_mode;
 };
 
 #define to_flexgen(_hw) container_of(_hw, struct flexgen, hw)
+#define to_clk_gate(_hw) container_of(_hw, struct clk_gate, hw)
 
 static int flexgen_enable(struct clk_hw *hw)
 {
@@ -139,12 +149,21 @@ static int flexgen_set_rate(struct clk_hw *hw, unsigned long rate,
 	struct flexgen *flexgen = to_flexgen(hw);
 	struct clk_hw *pdiv_hw = &flexgen->pdiv.hw;
 	struct clk_hw *fdiv_hw = &flexgen->fdiv.hw;
+	struct clk_hw *sync_hw = &flexgen->sync.hw;
+	struct clk_gate *config = to_clk_gate(sync_hw);
 	unsigned long div = 0;
 	int ret = 0;
+	u32 reg;
 
 	__clk_hw_set_clk(pdiv_hw, hw);
 	__clk_hw_set_clk(fdiv_hw, hw);
 
+	if (flexgen->control_mode) {
+		reg = readl(config->reg);
+		reg &= ~BIT(config->bit_idx);
+		writel(reg, config->reg);
+	}
+
 	div = clk_best_div(parent_rate, rate);
 
 	/*
@@ -178,7 +197,7 @@ static const struct clk_ops flexgen_ops = {
 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) {
+				unsigned long flexgen_flags, bool mode) {
 	struct flexgen *fgxbar;
 	struct clk *clk;
 	struct clk_init_data init;
@@ -227,6 +246,13 @@ static struct clk *clk_register_flexgen(const char *name,
 	fgxbar->fdiv.reg = fdiv_reg;
 	fgxbar->fdiv.width = 6;
 
+	/* Final divider sync config */
+	fgxbar->sync.lock = lock;
+	fgxbar->sync.reg = fdiv_reg;
+	fgxbar->sync.bit_idx = 7;
+
+	fgxbar->control_mode = mode;
+
 	fgxbar->hw.init = &init;
 
 	clk = clk_register(NULL, &fgxbar->hw);
@@ -259,6 +285,27 @@ static const char ** __init flexgen_get_parents(struct device_node *np,
 	return parents;
 }
 
+static const struct clkgen_data clkgen_audio = {
+	.flags = CLK_SET_RATE_PARENT,
+};
+
+static const struct clkgen_data clkgen_video = {
+	.flags = CLK_SET_RATE_PARENT,
+	.mode = 1,
+};
+
+static const struct of_device_id flexgen_of_match[] = {
+	{
+		.compatible = "st,flexgen-audio",
+		.data = &clkgen_audio,
+	},
+	{
+		.compatible = "st,flexgen-video",
+		.data = &clkgen_video,
+	},
+	{}
+};
+
 static void __init st_of_flexgen_setup(struct device_node *np)
 {
 	struct device_node *pnode;
@@ -267,7 +314,11 @@ static void __init st_of_flexgen_setup(struct device_node *np)
 	const char **parents;
 	int num_parents, i;
 	spinlock_t *rlock = NULL;
+	const struct of_device_id *match;
+	struct clkgen_data *data = NULL;
+	unsigned long flex_flags = 0;
 	int ret;
+	bool clk_mode = 0;
 
 	pnode = of_get_parent(np);
 	if (!pnode)
@@ -281,6 +332,13 @@ static void __init st_of_flexgen_setup(struct device_node *np)
 	if (!parents)
 		return;
 
+	match = of_match_node(flexgen_of_match, np);
+	if (match) {
+		data = (struct clkgen_data *)match->data;
+		flex_flags = data->flags;
+		clk_mode = data->mode;
+	}
+
 	clk_data = kzalloc(sizeof(*clk_data), GFP_KERNEL);
 	if (!clk_data)
 		goto err;
@@ -307,7 +365,6 @@ static void __init st_of_flexgen_setup(struct device_node *np)
 	for (i = 0; i < clk_data->clk_num; i++) {
 		struct clk *clk;
 		const char *clk_name;
-		unsigned long flex_flags = 0;
 
 		if (of_property_read_string_index(np, "clock-output-names",
 						  i, &clk_name)) {
@@ -323,7 +380,7 @@ static void __init st_of_flexgen_setup(struct device_node *np)
 			continue;
 
 		clk = clk_register_flexgen(clk_name, parents, num_parents,
-					   reg, rlock, i, flex_flags);
+					   reg, rlock, i, flex_flags, clk_mode);
 
 		if (IS_ERR(clk))
 			goto err;
diff --git a/drivers/clk/st/clkgen-fsyn.c b/drivers/clk/st/clkgen-fsyn.c
index 09afeb85109c..14819d919df1 100644
--- a/drivers/clk/st/clkgen-fsyn.c
+++ b/drivers/clk/st/clkgen-fsyn.c
@@ -42,79 +42,6 @@ struct stm_fs {
 	unsigned long nsdiv;
 };
 
-static const struct stm_fs fs216c65_rtbl[] = {
-	{ .mdiv = 0x1f, .pe = 0x0,	.sdiv = 0x7,	.nsdiv = 0 },	/* 312.5 Khz */
-	{ .mdiv = 0x17, .pe = 0x25ed,	.sdiv = 0x1,	.nsdiv = 0 },	/* 27    MHz */
-	{ .mdiv = 0x1a, .pe = 0x7b36,	.sdiv = 0x2,	.nsdiv = 1 },	/* 36.87 MHz */
-	{ .mdiv = 0x13, .pe = 0x0,	.sdiv = 0x2,	.nsdiv = 1 },	/* 48    MHz */
-	{ .mdiv = 0x11, .pe = 0x1c72,	.sdiv = 0x1,	.nsdiv = 1 },	/* 108   MHz */
-};
-
-static const struct stm_fs fs432c65_rtbl[] = {
-	{ .mdiv = 0x1f, .pe = 0x0,	.sdiv = 0x7,	.nsdiv = 0 },	/* 625     Khz */
-	{ .mdiv = 0x13, .pe = 0x777c,	.sdiv = 0x4,	.nsdiv = 1 },	/* 25.175  MHz */
-	{ .mdiv = 0x19, .pe = 0x4d35,	.sdiv = 0x2,	.nsdiv = 0 },	/* 25.200  MHz */
-	{ .mdiv = 0x11, .pe = 0x1c72,	.sdiv = 0x4,	.nsdiv = 1 },	/* 27.000  MHz */
-	{ .mdiv = 0x17, .pe = 0x28f5,	.sdiv = 0x2,	.nsdiv = 0 },	/* 27.027  MHz */
-	{ .mdiv = 0x16, .pe = 0x3359,	.sdiv = 0x2,	.nsdiv = 0 },	/* 28.320  MHz */
-	{ .mdiv = 0x1f, .pe = 0x2083,	.sdiv = 0x3,	.nsdiv = 1 },	/* 30.240  MHz */
-	{ .mdiv = 0x1e, .pe = 0x430d,	.sdiv = 0x3,	.nsdiv = 1 },	/* 31.500  MHz */
-	{ .mdiv = 0x17, .pe = 0x0,	.sdiv = 0x3,	.nsdiv = 1 },	/* 40.000  MHz */
-	{ .mdiv = 0x19, .pe = 0x121a,	.sdiv = 0x1,	.nsdiv = 0 },	/* 49.500  MHz */
-	{ .mdiv = 0x13, .pe = 0x6667,	.sdiv = 0x3,	.nsdiv = 1 },	/* 50.000  MHz */
-	{ .mdiv = 0x10, .pe = 0x1ee6,	.sdiv = 0x3,	.nsdiv = 1 },	/* 57.284  MHz */
-	{ .mdiv = 0x1d, .pe = 0x3b14,	.sdiv = 0x2,	.nsdiv = 1 },	/* 65.000  MHz */
-	{ .mdiv = 0x12, .pe = 0x7c65,	.sdiv = 0x1,	.nsdiv = 0 },	/* 71.000  MHz */
-	{ .mdiv = 0x19, .pe = 0xecd,	.sdiv = 0x2,	.nsdiv = 1 },	/* 74.176  MHz */
-	{ .mdiv = 0x19, .pe = 0x121a,	.sdiv = 0x2,	.nsdiv = 1 },	/* 74.250  MHz */
-	{ .mdiv = 0x19, .pe = 0x3334,	.sdiv = 0x2,	.nsdiv = 1 },	/* 75.000  MHz */
-	{ .mdiv = 0x18, .pe = 0x5138,	.sdiv = 0x2,	.nsdiv = 1 },	/* 78.800  MHz */
-	{ .mdiv = 0x1d, .pe = 0x77d,	.sdiv = 0x0,	.nsdiv = 0 },	/* 85.500  MHz */
-	{ .mdiv = 0x1c, .pe = 0x13d5,	.sdiv = 0x0,	.nsdiv = 0 },	/* 88.750  MHz */
-	{ .mdiv = 0x11, .pe = 0x1c72,	.sdiv = 0x2,	.nsdiv = 1 },	/* 108.000 MHz */
-	{ .mdiv = 0x17, .pe = 0x28f5,	.sdiv = 0x0,	.nsdiv = 0 },	/* 108.108 MHz */
-	{ .mdiv = 0x10, .pe = 0x6e26,	.sdiv = 0x2,	.nsdiv = 1 },	/* 118.963 MHz */
-	{ .mdiv = 0x15, .pe = 0x3e63,	.sdiv = 0x0,	.nsdiv = 0 },	/* 119.000 MHz */
-	{ .mdiv = 0x1c, .pe = 0x471d,	.sdiv = 0x1,	.nsdiv = 1 },	/* 135.000 MHz */
-	{ .mdiv = 0x19, .pe = 0xecd,	.sdiv = 0x1,	.nsdiv = 1 },	/* 148.352 MHz */
-	{ .mdiv = 0x19, .pe = 0x121a,	.sdiv = 0x1,	.nsdiv = 1 },	/* 148.500 MHz */
-	{ .mdiv = 0x19, .pe = 0x121a,	.sdiv = 0x0,	.nsdiv = 1 },	/* 297     MHz */
-};
-
-static const struct stm_fs fs660c32_rtbl[] = {
-	{ .mdiv = 0x14, .pe = 0x376b,	.sdiv = 0x4,	.nsdiv = 1 },	/* 25.175  MHz */
-	{ .mdiv = 0x14, .pe = 0x30c3,	.sdiv = 0x4,	.nsdiv = 1 },	/* 25.200  MHz */
-	{ .mdiv = 0x10, .pe = 0x71c7,	.sdiv = 0x4,	.nsdiv = 1 },	/* 27.000  MHz */
-	{ .mdiv = 0x00, .pe = 0x47af,	.sdiv = 0x3,	.nsdiv = 0 },	/* 27.027  MHz */
-	{ .mdiv = 0x0e, .pe = 0x4e1a,	.sdiv = 0x4,	.nsdiv = 1 },	/* 28.320  MHz */
-	{ .mdiv = 0x0b, .pe = 0x534d,	.sdiv = 0x4,	.nsdiv = 1 },	/* 30.240  MHz */
-	{ .mdiv = 0x17, .pe = 0x6fbf,	.sdiv = 0x2,	.nsdiv = 0 },	/* 31.500  MHz */
-	{ .mdiv = 0x01, .pe = 0x0,	.sdiv = 0x4,	.nsdiv = 1 },	/* 40.000  MHz */
-	{ .mdiv = 0x15, .pe = 0x2aab,	.sdiv = 0x3,	.nsdiv = 1 },	/* 49.500  MHz */
-	{ .mdiv = 0x14, .pe = 0x6666,	.sdiv = 0x3,	.nsdiv = 1 },	/* 50.000  MHz */
-	{ .mdiv = 0x1d, .pe = 0x395f,	.sdiv = 0x1,	.nsdiv = 0 },	/* 57.284  MHz */
-	{ .mdiv = 0x08, .pe = 0x4ec5,	.sdiv = 0x3,	.nsdiv = 1 },	/* 65.000  MHz */
-	{ .mdiv = 0x05, .pe = 0x1770,	.sdiv = 0x3,	.nsdiv = 1 },	/* 71.000  MHz */
-	{ .mdiv = 0x03, .pe = 0x4ba7,	.sdiv = 0x3,	.nsdiv = 1 },	/* 74.176  MHz */
-	{ .mdiv = 0x0f, .pe = 0x3426,	.sdiv = 0x1,	.nsdiv = 0 },	/* 74.250  MHz */
-	{ .mdiv = 0x0e, .pe = 0x7777,	.sdiv = 0x1,	.nsdiv = 0 },	/* 75.000  MHz */
-	{ .mdiv = 0x01, .pe = 0x4053,	.sdiv = 0x3,	.nsdiv = 1 },	/* 78.800  MHz */
-	{ .mdiv = 0x09, .pe = 0x15b5,	.sdiv = 0x1,	.nsdiv = 0 },	/* 85.500  MHz */
-	{ .mdiv = 0x1b, .pe = 0x3f19,	.sdiv = 0x2,	.nsdiv = 1 },	/* 88.750  MHz */
-	{ .mdiv = 0x10, .pe = 0x71c7,	.sdiv = 0x2,	.nsdiv = 1 },	/* 108.000 MHz */
-	{ .mdiv = 0x00, .pe = 0x47af,	.sdiv = 0x1,	.nsdiv = 0 },	/* 108.108 MHz */
-	{ .mdiv = 0x0c, .pe = 0x3118,	.sdiv = 0x2,	.nsdiv = 1 },	/* 118.963 MHz */
-	{ .mdiv = 0x0c, .pe = 0x2f54,	.sdiv = 0x2,	.nsdiv = 1 },	/* 119.000 MHz */
-	{ .mdiv = 0x07, .pe = 0xe39,	.sdiv = 0x2,	.nsdiv = 1 },	/* 135.000 MHz */
-	{ .mdiv = 0x03, .pe = 0x4ba7,	.sdiv = 0x2,	.nsdiv = 1 },	/* 148.352 MHz */
-	{ .mdiv = 0x0f, .pe = 0x3426,	.sdiv = 0x0,	.nsdiv = 0 },	/* 148.500 MHz */
-	{ .mdiv = 0x03, .pe = 0x4ba7,	.sdiv = 0x1,	.nsdiv = 1 },	/* 296.704 MHz */
-	{ .mdiv = 0x03, .pe = 0x471c,	.sdiv = 0x1,	.nsdiv = 1 },	/* 297.000 MHz */
-	{ .mdiv = 0x00, .pe = 0x295f,	.sdiv = 0x1,	.nsdiv = 1 },	/* 326.700 MHz */
-	{ .mdiv = 0x1f, .pe = 0x3633,	.sdiv = 0x0,	.nsdiv = 1 },	/* 333.000 MHz */
-	{ .mdiv = 0x1c, .pe = 0x0,	.sdiv = 0x0,	.nsdiv = 1 },	/* 352.000 Mhz */
-};
-
 struct clkgen_quadfs_data {
 	bool reset_present;
 	bool bwfilter_present;
@@ -138,174 +65,18 @@ struct clkgen_quadfs_data {
 	struct clkgen_field nsdiv[QUADFS_MAX_CHAN];
 
 	const struct clk_ops *pll_ops;
-	const struct stm_fs *rtbl;
-	u8 rtbl_cnt;
+	int  (*get_params)(unsigned long, unsigned long, struct stm_fs *);
 	int  (*get_rate)(unsigned long , const struct stm_fs *,
 			unsigned long *);
 };
 
-static const struct clk_ops st_quadfs_pll_c65_ops;
 static const struct clk_ops st_quadfs_pll_c32_ops;
-static const struct clk_ops st_quadfs_fs216c65_ops;
-static const struct clk_ops st_quadfs_fs432c65_ops;
 static const struct clk_ops st_quadfs_fs660c32_ops;
 
-static int clk_fs216c65_get_rate(unsigned long, const struct stm_fs *,
-		unsigned long *);
-static int clk_fs432c65_get_rate(unsigned long, const struct stm_fs *,
-		unsigned long *);
+static int clk_fs660c32_dig_get_params(unsigned long input,
+		unsigned long output, struct stm_fs *fs);
 static int clk_fs660c32_dig_get_rate(unsigned long, const struct stm_fs *,
 		unsigned long *);
-/*
- * Values for all of the standalone instances of this clock
- * generator found in STiH415 and STiH416 SYSCFG register banks. Note
- * that the individual channel standby control bits (nsb) are in the
- * first register along with the PLL control bits.
- */
-static const struct clkgen_quadfs_data st_fs216c65_416 = {
-	/* 416 specific */
-	.npda	= CLKGEN_FIELD(0x0, 0x1, 14),
-	.nsb	= { CLKGEN_FIELD(0x0, 0x1, 10),
-		    CLKGEN_FIELD(0x0, 0x1, 11),
-		    CLKGEN_FIELD(0x0, 0x1, 12),
-		    CLKGEN_FIELD(0x0, 0x1, 13) },
-	.nsdiv_present = true,
-	.nsdiv	= { CLKGEN_FIELD(0x0, 0x1, 18),
-		    CLKGEN_FIELD(0x0, 0x1, 19),
-		    CLKGEN_FIELD(0x0, 0x1, 20),
-		    CLKGEN_FIELD(0x0, 0x1, 21) },
-	.mdiv	= { CLKGEN_FIELD(0x4, 0x1f, 0),
-		    CLKGEN_FIELD(0x14, 0x1f, 0),
-		    CLKGEN_FIELD(0x24, 0x1f, 0),
-		    CLKGEN_FIELD(0x34, 0x1f, 0) },
-	.en	= { CLKGEN_FIELD(0x10, 0x1, 0),
-		    CLKGEN_FIELD(0x20, 0x1, 0),
-		    CLKGEN_FIELD(0x30, 0x1, 0),
-		    CLKGEN_FIELD(0x40, 0x1, 0) },
-	.ndiv	= CLKGEN_FIELD(0x0, 0x1, 15),
-	.bwfilter_present = true,
-	.ref_bw = CLKGEN_FIELD(0x0, 0x3, 16),
-	.pe	= { CLKGEN_FIELD(0x8, 0xffff, 0),
-		    CLKGEN_FIELD(0x18, 0xffff, 0),
-		    CLKGEN_FIELD(0x28, 0xffff, 0),
-		    CLKGEN_FIELD(0x38, 0xffff, 0) },
-	.sdiv	= { CLKGEN_FIELD(0xC, 0x7, 0),
-		    CLKGEN_FIELD(0x1C, 0x7, 0),
-		    CLKGEN_FIELD(0x2C, 0x7, 0),
-		    CLKGEN_FIELD(0x3C, 0x7, 0) },
-	.pll_ops	= &st_quadfs_pll_c65_ops,
-	.rtbl		= fs216c65_rtbl,
-	.rtbl_cnt	= ARRAY_SIZE(fs216c65_rtbl),
-	.get_rate	= clk_fs216c65_get_rate,
-};
-
-static const struct clkgen_quadfs_data st_fs432c65_416 = {
-	.npda	= CLKGEN_FIELD(0x0, 0x1, 14),
-	.nsb	= { CLKGEN_FIELD(0x0, 0x1, 10),
-		    CLKGEN_FIELD(0x0, 0x1, 11),
-		    CLKGEN_FIELD(0x0, 0x1, 12),
-		    CLKGEN_FIELD(0x0, 0x1, 13) },
-	.nsdiv_present = true,
-	.nsdiv	= { CLKGEN_FIELD(0x0, 0x1, 18),
-		   CLKGEN_FIELD(0x0, 0x1, 19),
-		   CLKGEN_FIELD(0x0, 0x1, 20),
-		   CLKGEN_FIELD(0x0, 0x1, 21) },
-	.mdiv	= { CLKGEN_FIELD(0x4, 0x1f, 0),
-		    CLKGEN_FIELD(0x14, 0x1f, 0),
-		    CLKGEN_FIELD(0x24, 0x1f, 0),
-		    CLKGEN_FIELD(0x34, 0x1f, 0) },
-	.en	= { CLKGEN_FIELD(0x10, 0x1, 0),
-		    CLKGEN_FIELD(0x20, 0x1, 0),
-		    CLKGEN_FIELD(0x30, 0x1, 0),
-		    CLKGEN_FIELD(0x40, 0x1, 0) },
-	.ndiv	= CLKGEN_FIELD(0x0, 0x1, 15),
-	.bwfilter_present = true,
-	.ref_bw = CLKGEN_FIELD(0x0, 0x3, 16),
-	.pe	= { CLKGEN_FIELD(0x8, 0xffff, 0),
-		    CLKGEN_FIELD(0x18, 0xffff, 0),
-		    CLKGEN_FIELD(0x28, 0xffff, 0),
-		    CLKGEN_FIELD(0x38, 0xffff, 0) },
-	.sdiv	= { CLKGEN_FIELD(0xC, 0x7, 0),
-		    CLKGEN_FIELD(0x1C, 0x7, 0),
-		    CLKGEN_FIELD(0x2C, 0x7, 0),
-		    CLKGEN_FIELD(0x3C, 0x7, 0) },
-	.pll_ops	= &st_quadfs_pll_c65_ops,
-	.rtbl		= fs432c65_rtbl,
-	.rtbl_cnt	= ARRAY_SIZE(fs432c65_rtbl),
-	.get_rate	= clk_fs432c65_get_rate,
-};
-
-static const struct clkgen_quadfs_data st_fs660c32_E_416 = {
-	.npda	= CLKGEN_FIELD(0x0, 0x1, 14),
-	.nsb	= { CLKGEN_FIELD(0x0, 0x1, 10),
-		    CLKGEN_FIELD(0x0, 0x1, 11),
-		    CLKGEN_FIELD(0x0, 0x1, 12),
-		    CLKGEN_FIELD(0x0, 0x1, 13) },
-	.nsdiv_present = true,
-	.nsdiv	= { CLKGEN_FIELD(0x0, 0x1, 18),
-		    CLKGEN_FIELD(0x0, 0x1, 19),
-		    CLKGEN_FIELD(0x0, 0x1, 20),
-		    CLKGEN_FIELD(0x0, 0x1, 21) },
-	.mdiv	= { CLKGEN_FIELD(0x4, 0x1f, 0),
-		    CLKGEN_FIELD(0x14, 0x1f, 0),
-		    CLKGEN_FIELD(0x24, 0x1f, 0),
-		    CLKGEN_FIELD(0x34, 0x1f, 0) },
-	.en	= { CLKGEN_FIELD(0x10, 0x1, 0),
-		    CLKGEN_FIELD(0x20, 0x1, 0),
-		    CLKGEN_FIELD(0x30, 0x1, 0),
-		    CLKGEN_FIELD(0x40, 0x1, 0) },
-	.ndiv	= CLKGEN_FIELD(0x0, 0x7, 15),
-	.pe	= { CLKGEN_FIELD(0x8, 0x7fff, 0),
-		    CLKGEN_FIELD(0x18, 0x7fff, 0),
-		    CLKGEN_FIELD(0x28, 0x7fff, 0),
-		    CLKGEN_FIELD(0x38, 0x7fff, 0) },
-	.sdiv	= { CLKGEN_FIELD(0xC, 0xf, 0),
-		    CLKGEN_FIELD(0x1C, 0xf, 0),
-		    CLKGEN_FIELD(0x2C, 0xf, 0),
-		    CLKGEN_FIELD(0x3C, 0xf, 0) },
-	.lockstatus_present = true,
-	.lock_status = CLKGEN_FIELD(0xAC, 0x1, 0),
-	.pll_ops	= &st_quadfs_pll_c32_ops,
-	.rtbl		= fs660c32_rtbl,
-	.rtbl_cnt	= ARRAY_SIZE(fs660c32_rtbl),
-	.get_rate	= clk_fs660c32_dig_get_rate,
-};
-
-static const struct clkgen_quadfs_data st_fs660c32_F_416 = {
-	.npda	= CLKGEN_FIELD(0x0, 0x1, 14),
-	.nsb	= { CLKGEN_FIELD(0x0, 0x1, 10),
-		    CLKGEN_FIELD(0x0, 0x1, 11),
-		    CLKGEN_FIELD(0x0, 0x1, 12),
-		    CLKGEN_FIELD(0x0, 0x1, 13) },
-	.nsdiv_present = true,
-	.nsdiv	= { CLKGEN_FIELD(0x0, 0x1, 18),
-		    CLKGEN_FIELD(0x0, 0x1, 19),
-		    CLKGEN_FIELD(0x0, 0x1, 20),
-		    CLKGEN_FIELD(0x0, 0x1, 21) },
-	.mdiv	= { CLKGEN_FIELD(0x4, 0x1f, 0),
-		    CLKGEN_FIELD(0x14, 0x1f, 0),
-		    CLKGEN_FIELD(0x24, 0x1f, 0),
-		    CLKGEN_FIELD(0x34, 0x1f, 0) },
-	.en	= { CLKGEN_FIELD(0x10, 0x1, 0),
-		    CLKGEN_FIELD(0x20, 0x1, 0),
-		    CLKGEN_FIELD(0x30, 0x1, 0),
-		    CLKGEN_FIELD(0x40, 0x1, 0) },
-	.ndiv	= CLKGEN_FIELD(0x0, 0x7, 15),
-	.pe	= { CLKGEN_FIELD(0x8, 0x7fff, 0),
-		    CLKGEN_FIELD(0x18, 0x7fff, 0),
-		    CLKGEN_FIELD(0x28, 0x7fff, 0),
-		    CLKGEN_FIELD(0x38, 0x7fff, 0) },
-	.sdiv	= { CLKGEN_FIELD(0xC, 0xf, 0),
-		    CLKGEN_FIELD(0x1C, 0xf, 0),
-		    CLKGEN_FIELD(0x2C, 0xf, 0),
-		    CLKGEN_FIELD(0x3C, 0xf, 0) },
-	.lockstatus_present = true,
-	.lock_status = CLKGEN_FIELD(0xEC, 0x1, 0),
-	.pll_ops	= &st_quadfs_pll_c32_ops,
-	.rtbl		= fs660c32_rtbl,
-	.rtbl_cnt	= ARRAY_SIZE(fs660c32_rtbl),
-	.get_rate	= clk_fs660c32_dig_get_rate,
-};
 
 static const struct clkgen_quadfs_data st_fs660c32_C = {
 	.nrst_present = true,
@@ -345,8 +116,7 @@ static const struct clkgen_quadfs_data st_fs660c32_C = {
 	.powerup_polarity = 1,
 	.standby_polarity = 1,
 	.pll_ops	= &st_quadfs_pll_c32_ops,
-	.rtbl		= fs660c32_rtbl,
-	.rtbl_cnt	= ARRAY_SIZE(fs660c32_rtbl),
+	.get_params	= clk_fs660c32_dig_get_params,
 	.get_rate	= clk_fs660c32_dig_get_rate,
 };
 
@@ -388,8 +158,7 @@ static const struct clkgen_quadfs_data st_fs660c32_D = {
 	.powerup_polarity = 1,
 	.standby_polarity = 1,
 	.pll_ops	= &st_quadfs_pll_c32_ops,
-	.rtbl		= fs660c32_rtbl,
-	.rtbl_cnt	= ARRAY_SIZE(fs660c32_rtbl),
+	.get_params	= clk_fs660c32_dig_get_params,
 	.get_rate	= clk_fs660c32_dig_get_rate,};
 
 /**
@@ -605,12 +374,6 @@ static int quadfs_pll_fs660c32_set_rate(struct clk_hw *hw, unsigned long rate,
 	return 0;
 }
 
-static const struct clk_ops st_quadfs_pll_c65_ops = {
-	.enable		= quadfs_pll_enable,
-	.disable	= quadfs_pll_disable,
-	.is_enabled	= quadfs_pll_is_enabled,
-};
-
 static const struct clk_ops st_quadfs_pll_c32_ops = {
 	.enable		= quadfs_pll_enable,
 	.disable	= quadfs_pll_disable,
@@ -797,48 +560,6 @@ static int quadfs_fsynth_is_enabled(struct clk_hw *hw)
 	return fs->data->standby_polarity ? !nsb : !!nsb;
 }
 
-#define P15			(uint64_t)(1 << 15)
-
-static int clk_fs216c65_get_rate(unsigned long input, const struct stm_fs *fs,
-		unsigned long *rate)
-{
-	uint64_t res;
-	unsigned long ns;
-	unsigned long nd = 8; /* ndiv stuck at 0 => val = 8 */
-	unsigned long s;
-	long m;
-
-	m = fs->mdiv - 32;
-	s = 1 << (fs->sdiv + 1);
-	ns = (fs->nsdiv ? 1 : 3);
-
-	res = (uint64_t)(s * ns * P15 * (uint64_t)(m + 33));
-	res = res - (s * ns * fs->pe);
-	*rate = div64_u64(P15 * nd * input * 32, res);
-
-	return 0;
-}
-
-static int clk_fs432c65_get_rate(unsigned long input, const struct stm_fs *fs,
-		unsigned long *rate)
-{
-	uint64_t res;
-	unsigned long nd = 16; /* ndiv value; stuck at 0 (30Mhz input) */
-	long m;
-	unsigned long sd;
-	unsigned long ns;
-
-	m = fs->mdiv - 32;
-	sd = 1 << (fs->sdiv + 1);
-	ns = (fs->nsdiv ? 1 : 3);
-
-	res = (uint64_t)(sd * ns * P15 * (uint64_t)(m + 33));
-	res = res - (sd * ns * fs->pe);
-	*rate = div64_u64(P15 * nd * input * 32, res);
-
-	return 0;
-}
-
 #define P20		(uint64_t)(1 << 20)
 
 static int clk_fs660c32_dig_get_rate(unsigned long input,
@@ -864,6 +585,107 @@ static int clk_fs660c32_dig_get_rate(unsigned long input,
 	return 0;
 }
 
+
+static int clk_fs660c32_get_pe(int m, int si, unsigned long *deviation,
+		signed long input, unsigned long output, uint64_t *p,
+		struct stm_fs *fs)
+{
+	unsigned long new_freq, new_deviation;
+	struct stm_fs fs_tmp;
+	uint64_t val;
+
+	val = (uint64_t)output << si;
+
+	*p = (uint64_t)input * P20 - (32LL  + (uint64_t)m) * val * (P20 / 32LL);
+
+	*p = div64_u64(*p, val);
+
+	if (*p > 32767LL)
+		return 1;
+
+	fs_tmp.mdiv = (unsigned long) m;
+	fs_tmp.pe = (unsigned long)*p;
+	fs_tmp.sdiv = si;
+	fs_tmp.nsdiv = 1;
+
+	clk_fs660c32_dig_get_rate(input, &fs_tmp, &new_freq);
+
+	new_deviation = abs(output - new_freq);
+
+	if (new_deviation < *deviation) {
+		fs->mdiv = m;
+		fs->pe = (unsigned long)*p;
+		fs->sdiv = si;
+		fs->nsdiv = 1;
+		*deviation = new_deviation;
+	}
+	return 0;
+}
+
+static int clk_fs660c32_dig_get_params(unsigned long input,
+		unsigned long output, struct stm_fs *fs)
+{
+	int si;	/* sdiv_reg (8 downto 0) */
+	int m; /* md value */
+	unsigned long new_freq, new_deviation;
+	/* initial condition to say: "infinite deviation" */
+	unsigned long deviation = ~0;
+	uint64_t p, p1, p2;	/* pe value */
+	int r1, r2;
+
+	struct stm_fs fs_tmp;
+
+	for (si = 0; (si <= 8) && deviation; si++) {
+
+		/* Boundary test to avoid useless iteration */
+		r1 = clk_fs660c32_get_pe(0, si, &deviation,
+				input, output, &p1, fs);
+		r2 = clk_fs660c32_get_pe(31, si, &deviation,
+				input, output, &p2, fs);
+
+		/* No solution */
+		if (r1 && r2 && (p1 > p2))
+			continue;
+
+		/* Try to find best deviation */
+		for (m = 1; (m < 31) && deviation; m++)
+			clk_fs660c32_get_pe(m, si, &deviation,
+					input, output, &p, fs);
+
+	}
+
+	if (deviation == ~0) /* No solution found */
+		return -1;
+
+	/* pe fine tuning if deviation not 0: +/- 2 around computed pe value */
+	if (deviation) {
+		fs_tmp.mdiv = fs->mdiv;
+		fs_tmp.sdiv = fs->sdiv;
+		fs_tmp.nsdiv = fs->nsdiv;
+
+		if (fs->pe > 2)
+			p2 = fs->pe - 2;
+		else
+			p2 = 0;
+
+		for (; p2 < 32768ll && (p2 <= (fs->pe + 2)); p2++) {
+			fs_tmp.pe = (unsigned long)p2;
+
+			clk_fs660c32_dig_get_rate(input, &fs_tmp, &new_freq);
+
+			new_deviation = abs(output - new_freq);
+
+			/* Check if this is a better solution */
+			if (new_deviation < deviation) {
+				fs->pe = (unsigned long)p2;
+				deviation = new_deviation;
+
+			}
+		}
+	}
+	return 0;
+}
+
 static int quadfs_fsynt_get_hw_value_for_recalc(struct st_clk_quadfs_fsynth *fs,
 		struct stm_fs *params)
 {
@@ -899,38 +721,14 @@ static long quadfs_find_best_rate(struct clk_hw *hw, unsigned long drate,
 	struct st_clk_quadfs_fsynth *fs = to_quadfs_fsynth(hw);
 	int (*clk_fs_get_rate)(unsigned long ,
 				const struct stm_fs *, unsigned long *);
-	struct stm_fs prev_params;
-	unsigned long prev_rate, rate = 0;
-	unsigned long diff_rate, prev_diff_rate = ~0;
-	int index;
+	int (*clk_fs_get_params)(unsigned long, unsigned long, struct stm_fs *);
+	unsigned long rate = 0;
 
 	clk_fs_get_rate = fs->data->get_rate;
+	clk_fs_get_params = fs->data->get_params;
 
-	for (index = 0; index < fs->data->rtbl_cnt; index++) {
-		prev_rate = rate;
-
-		*params = fs->data->rtbl[index];
-		prev_params = *params;
-
-		clk_fs_get_rate(prate, &fs->data->rtbl[index], &rate);
-
-		diff_rate = abs(drate - rate);
-
-		if (diff_rate > prev_diff_rate) {
-			rate = prev_rate;
-			*params = prev_params;
-			break;
-		}
-
-		prev_diff_rate = diff_rate;
-
-		if (drate == rate)
-			return rate;
-	}
-
-
-	if (index == fs->data->rtbl_cnt)
-		*params = prev_params;
+	if (!clk_fs_get_params(prate, drate, params))
+		clk_fs_get_rate(prate, params, &rate);
 
 	return rate;
 }
@@ -1063,34 +861,6 @@ static struct clk * __init st_clk_register_quadfs_fsynth(
 	return clk;
 }
 
-static const struct of_device_id quadfs_of_match[] = {
-	{
-		.compatible = "st,stih416-quadfs216",
-		.data = &st_fs216c65_416
-	},
-	{
-		.compatible = "st,stih416-quadfs432",
-		.data = &st_fs432c65_416
-	},
-	{
-		.compatible = "st,stih416-quadfs660-E",
-		.data = &st_fs660c32_E_416
-	},
-	{
-		.compatible = "st,stih416-quadfs660-F",
-		.data = &st_fs660c32_F_416
-	},
-	{
-		.compatible = "st,stih407-quadfs660-C",
-		.data = &st_fs660c32_C
-	},
-	{
-		.compatible = "st,stih407-quadfs660-D",
-		.data = &st_fs660c32_D
-	},
-	{}
-};
-
 static void __init st_of_create_quadfs_fsynths(
 		struct device_node *np, const char *pll_name,
 		struct clkgen_quadfs_data *quadfs, void __iomem *reg,
@@ -1150,18 +920,14 @@ static void __init st_of_create_quadfs_fsynths(
 	of_clk_add_provider(np, of_clk_src_onecell_get, clk_data);
 }
 
-static void __init st_of_quadfs_setup(struct device_node *np)
+static void __init st_of_quadfs_setup(struct device_node *np,
+		struct clkgen_quadfs_data *data)
 {
-	const struct of_device_id *match;
 	struct clk *clk;
 	const char *pll_name, *clk_parent_name;
 	void __iomem *reg;
 	spinlock_t *lock;
 
-	match = of_match_node(quadfs_of_match, np);
-	if (WARN_ON(!match))
-		return;
-
 	reg = of_iomap(np, 0);
 	if (!reg)
 		return;
@@ -1180,8 +946,8 @@ static void __init st_of_quadfs_setup(struct device_node *np)
 
 	spin_lock_init(lock);
 
-	clk = st_clk_register_quadfs_pll(pll_name, clk_parent_name,
-			(struct clkgen_quadfs_data *) match->data, reg, lock);
+	clk = st_clk_register_quadfs_pll(pll_name, clk_parent_name, data,
+			reg, lock);
 	if (IS_ERR(clk))
 		goto err_exit;
 	else
@@ -1190,11 +956,20 @@ static void __init st_of_quadfs_setup(struct device_node *np)
 			__clk_get_name(clk_get_parent(clk)),
 			(unsigned int)clk_get_rate(clk));
 
-	st_of_create_quadfs_fsynths(np, pll_name,
-				    (struct clkgen_quadfs_data *)match->data,
-				    reg, lock);
+	st_of_create_quadfs_fsynths(np, pll_name, data, reg, lock);
 
 err_exit:
 	kfree(pll_name); /* No longer need local copy of the PLL name */
 }
-CLK_OF_DECLARE(quadfs, "st,quadfs", st_of_quadfs_setup);
+
+static void __init st_of_quadfs660C_setup(struct device_node *np)
+{
+	st_of_quadfs_setup(np, (struct clkgen_quadfs_data *) &st_fs660c32_C);
+}
+CLK_OF_DECLARE(quadfs660C, "st,quadfs-pll", st_of_quadfs660C_setup);
+
+static void __init st_of_quadfs660D_setup(struct device_node *np)
+{
+	st_of_quadfs_setup(np, (struct clkgen_quadfs_data *) &st_fs660c32_D);
+}
+CLK_OF_DECLARE(quadfs660D, "st,quadfs", st_of_quadfs660D_setup);
diff --git a/drivers/clk/st/clkgen-mux.c b/drivers/clk/st/clkgen-mux.c
index b1e10ffe7a44..c514d39760cb 100644
--- a/drivers/clk/st/clkgen-mux.c
+++ b/drivers/clk/st/clkgen-mux.c
@@ -19,9 +19,6 @@
 #include <linux/clk-provider.h>
 #include "clkgen.h"
 
-static DEFINE_SPINLOCK(clkgena_divmux_lock);
-static DEFINE_SPINLOCK(clkgenf_lock);
-
 static const char ** __init clkgen_mux_get_parents(struct device_node *np,
 						       int *num_parents)
 {
@@ -40,498 +37,6 @@ static const char ** __init clkgen_mux_get_parents(struct device_node *np,
 	return parents;
 }
 
-/**
- * DOC: Clock mux with a programmable divider on each of its three inputs.
- *      The mux has an input setting which effectively gates its output.
- *
- * 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 - set rate is supported
- * parent - set/get parent
- */
-
-#define NUM_INPUTS 3
-
-struct clkgena_divmux {
-	struct clk_hw hw;
-	/* Subclassed mux and divider structures */
-	struct clk_mux mux;
-	struct clk_divider div[NUM_INPUTS];
-	/* Enable/running feedback register bits for each input */
-	void __iomem *feedback_reg[NUM_INPUTS];
-	int feedback_bit_idx;
-
-	u8              muxsel;
-};
-
-#define to_clkgena_divmux(_hw) container_of(_hw, struct clkgena_divmux, hw)
-
-struct clkgena_divmux_data {
-	int num_outputs;
-	int mux_offset;
-	int mux_offset2;
-	int mux_start_bit;
-	int div_offsets[NUM_INPUTS];
-	int fb_offsets[NUM_INPUTS];
-	int fb_start_bit_idx;
-};
-
-#define CKGAX_CLKOPSRC_SWITCH_OFF 0x3
-
-static int clkgena_divmux_is_running(struct clkgena_divmux *mux)
-{
-	u32 regval = readl(mux->feedback_reg[mux->muxsel]);
-	u32 running = regval & BIT(mux->feedback_bit_idx);
-	return !!running;
-}
-
-static int clkgena_divmux_enable(struct clk_hw *hw)
-{
-	struct clkgena_divmux *genamux = to_clkgena_divmux(hw);
-	struct clk_hw *mux_hw = &genamux->mux.hw;
-	unsigned long timeout;
-	int ret = 0;
-
-	__clk_hw_set_clk(mux_hw, hw);
-
-	ret = clk_mux_ops.set_parent(mux_hw, genamux->muxsel);
-	if (ret)
-		return ret;
-
-	timeout = jiffies + msecs_to_jiffies(10);
-
-	while (!clkgena_divmux_is_running(genamux)) {
-		if (time_after(jiffies, timeout))
-			return -ETIMEDOUT;
-		cpu_relax();
-	}
-
-	return 0;
-}
-
-static void clkgena_divmux_disable(struct clk_hw *hw)
-{
-	struct clkgena_divmux *genamux = to_clkgena_divmux(hw);
-	struct clk_hw *mux_hw = &genamux->mux.hw;
-
-	__clk_hw_set_clk(mux_hw, hw);
-
-	clk_mux_ops.set_parent(mux_hw, CKGAX_CLKOPSRC_SWITCH_OFF);
-}
-
-static int clkgena_divmux_is_enabled(struct clk_hw *hw)
-{
-	struct clkgena_divmux *genamux = to_clkgena_divmux(hw);
-	struct clk_hw *mux_hw = &genamux->mux.hw;
-
-	__clk_hw_set_clk(mux_hw, hw);
-
-	return (s8)clk_mux_ops.get_parent(mux_hw) > 0;
-}
-
-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;
-
-	__clk_hw_set_clk(mux_hw, hw);
-
-	genamux->muxsel = clk_mux_ops.get_parent(mux_hw);
-	if ((s8)genamux->muxsel < 0) {
-		pr_debug("%s: %s: Invalid parent, setting to default.\n",
-		      __func__, clk_hw_get_name(hw));
-		genamux->muxsel = 0;
-	}
-
-	return genamux->muxsel;
-}
-
-static int clkgena_divmux_set_parent(struct clk_hw *hw, u8 index)
-{
-	struct clkgena_divmux *genamux = to_clkgena_divmux(hw);
-
-	if (index >= CKGAX_CLKOPSRC_SWITCH_OFF)
-		return -EINVAL;
-
-	genamux->muxsel = index;
-
-	/*
-	 * If the mux is already enabled, call enable directly to set the
-	 * new mux position and wait for it to start running again. Otherwise
-	 * do nothing.
-	 */
-	if (clkgena_divmux_is_enabled(hw))
-		clkgena_divmux_enable(hw);
-
-	return 0;
-}
-
-static unsigned long clkgena_divmux_recalc_rate(struct clk_hw *hw,
-		unsigned long parent_rate)
-{
-	struct clkgena_divmux *genamux = to_clkgena_divmux(hw);
-	struct clk_hw *div_hw = &genamux->div[genamux->muxsel].hw;
-
-	__clk_hw_set_clk(div_hw, hw);
-
-	return clk_divider_ops.recalc_rate(div_hw, parent_rate);
-}
-
-static int clkgena_divmux_set_rate(struct clk_hw *hw, unsigned long rate,
-				unsigned long parent_rate)
-{
-	struct clkgena_divmux *genamux = to_clkgena_divmux(hw);
-	struct clk_hw *div_hw = &genamux->div[genamux->muxsel].hw;
-
-	__clk_hw_set_clk(div_hw, hw);
-
-	return clk_divider_ops.set_rate(div_hw, rate, parent_rate);
-}
-
-static long clkgena_divmux_round_rate(struct clk_hw *hw, unsigned long rate,
-				   unsigned long *prate)
-{
-	struct clkgena_divmux *genamux = to_clkgena_divmux(hw);
-	struct clk_hw *div_hw = &genamux->div[genamux->muxsel].hw;
-
-	__clk_hw_set_clk(div_hw, hw);
-
-	return clk_divider_ops.round_rate(div_hw, rate, prate);
-}
-
-static const struct clk_ops clkgena_divmux_ops = {
-	.enable = clkgena_divmux_enable,
-	.disable = clkgena_divmux_disable,
-	.is_enabled = clkgena_divmux_is_enabled,
-	.get_parent = clkgena_divmux_get_parent,
-	.set_parent = clkgena_divmux_set_parent,
-	.round_rate = clkgena_divmux_round_rate,
-	.recalc_rate = clkgena_divmux_recalc_rate,
-	.set_rate = clkgena_divmux_set_rate,
-};
-
-/**
- * clk_register_genamux - register a genamux clock with the clock framework
- */
-static struct clk * __init clk_register_genamux(const char *name,
-				const char **parent_names, u8 num_parents,
-				void __iomem *reg,
-				const struct clkgena_divmux_data *muxdata,
-				u32 idx)
-{
-	/*
-	 * Fixed constants across all ClockgenA variants
-	 */
-	const int mux_width = 2;
-	const int divider_width = 5;
-	struct clkgena_divmux *genamux;
-	struct clk *clk;
-	struct clk_init_data init;
-	int i;
-
-	genamux = kzalloc(sizeof(*genamux), GFP_KERNEL);
-	if (!genamux)
-		return ERR_PTR(-ENOMEM);
-
-	init.name = name;
-	init.ops = &clkgena_divmux_ops;
-	init.flags = CLK_IS_BASIC | CLK_GET_RATE_NOCACHE;
-	init.parent_names = parent_names;
-	init.num_parents = num_parents;
-
-	genamux->mux.lock  = &clkgena_divmux_lock;
-	genamux->mux.mask = BIT(mux_width) - 1;
-	genamux->mux.shift = muxdata->mux_start_bit + (idx * mux_width);
-	if (genamux->mux.shift > 31) {
-		/*
-		 * We have spilled into the second mux register so
-		 * adjust the register address and the bit shift accordingly
-		 */
-		genamux->mux.reg = reg + muxdata->mux_offset2;
-		genamux->mux.shift -= 32;
-	} else {
-		genamux->mux.reg   = reg + muxdata->mux_offset;
-	}
-
-	for (i = 0; i < NUM_INPUTS; i++) {
-		/*
-		 * Divider config for each input
-		 */
-		void __iomem *divbase = reg + muxdata->div_offsets[i];
-		genamux->div[i].width = divider_width;
-		genamux->div[i].reg = divbase + (idx * sizeof(u32));
-
-		/*
-		 * Mux enabled/running feedback register for each input.
-		 */
-		genamux->feedback_reg[i] = reg + muxdata->fb_offsets[i];
-	}
-
-	genamux->feedback_bit_idx = muxdata->fb_start_bit_idx + idx;
-	genamux->hw.init = &init;
-
-	clk = clk_register(NULL, &genamux->hw);
-	if (IS_ERR(clk)) {
-		kfree(genamux);
-		goto err;
-	}
-
-	pr_debug("%s: parent %s rate %lu\n",
-			__clk_get_name(clk),
-			__clk_get_name(clk_get_parent(clk)),
-			clk_get_rate(clk));
-err:
-	return clk;
-}
-
-static struct clkgena_divmux_data st_divmux_c65hs = {
-	.num_outputs = 4,
-	.mux_offset = 0x14,
-	.mux_start_bit = 0,
-	.div_offsets = { 0x800, 0x900, 0xb00 },
-	.fb_offsets = { 0x18, 0x1c, 0x20 },
-	.fb_start_bit_idx = 0,
-};
-
-static struct clkgena_divmux_data st_divmux_c65ls = {
-	.num_outputs = 14,
-	.mux_offset = 0x14,
-	.mux_offset2 = 0x24,
-	.mux_start_bit = 8,
-	.div_offsets = { 0x810, 0xa10, 0xb10 },
-	.fb_offsets = { 0x18, 0x1c, 0x20 },
-	.fb_start_bit_idx = 4,
-};
-
-static struct clkgena_divmux_data st_divmux_c32odf0 = {
-	.num_outputs = 8,
-	.mux_offset = 0x1c,
-	.mux_start_bit = 0,
-	.div_offsets = { 0x800, 0x900, 0xa60 },
-	.fb_offsets = { 0x2c, 0x24, 0x28 },
-	.fb_start_bit_idx = 0,
-};
-
-static struct clkgena_divmux_data st_divmux_c32odf1 = {
-	.num_outputs = 8,
-	.mux_offset = 0x1c,
-	.mux_start_bit = 16,
-	.div_offsets = { 0x820, 0x980, 0xa80 },
-	.fb_offsets = { 0x2c, 0x24, 0x28 },
-	.fb_start_bit_idx = 8,
-};
-
-static struct clkgena_divmux_data st_divmux_c32odf2 = {
-	.num_outputs = 8,
-	.mux_offset = 0x20,
-	.mux_start_bit = 0,
-	.div_offsets = { 0x840, 0xa20, 0xb10 },
-	.fb_offsets = { 0x2c, 0x24, 0x28 },
-	.fb_start_bit_idx = 16,
-};
-
-static struct clkgena_divmux_data st_divmux_c32odf3 = {
-	.num_outputs = 8,
-	.mux_offset = 0x20,
-	.mux_start_bit = 16,
-	.div_offsets = { 0x860, 0xa40, 0xb30 },
-	.fb_offsets = { 0x2c, 0x24, 0x28 },
-	.fb_start_bit_idx = 24,
-};
-
-static const struct of_device_id clkgena_divmux_of_match[] = {
-	{
-		.compatible = "st,clkgena-divmux-c65-hs",
-		.data = &st_divmux_c65hs,
-	},
-	{
-		.compatible = "st,clkgena-divmux-c65-ls",
-		.data = &st_divmux_c65ls,
-	},
-	{
-		.compatible = "st,clkgena-divmux-c32-odf0",
-		.data = &st_divmux_c32odf0,
-	},
-	{
-		.compatible = "st,clkgena-divmux-c32-odf1",
-		.data = &st_divmux_c32odf1,
-	},
-	{
-		.compatible = "st,clkgena-divmux-c32-odf2",
-		.data = &st_divmux_c32odf2,
-	},
-	{
-		.compatible = "st,clkgena-divmux-c32-odf3",
-		.data = &st_divmux_c32odf3,
-	},
-	{}
-};
-
-static void __iomem * __init clkgen_get_register_base(struct device_node *np)
-{
-	struct device_node *pnode;
-	void __iomem *reg;
-
-	pnode = of_get_parent(np);
-	if (!pnode)
-		return NULL;
-
-	reg = of_iomap(pnode, 0);
-
-	of_node_put(pnode);
-	return reg;
-}
-
-static void __init st_of_clkgena_divmux_setup(struct device_node *np)
-{
-	const struct of_device_id *match;
-	const struct clkgena_divmux_data *data;
-	struct clk_onecell_data *clk_data;
-	void __iomem *reg;
-	const char **parents;
-	int num_parents = 0, i;
-
-	match = of_match_node(clkgena_divmux_of_match, np);
-	if (WARN_ON(!match))
-		return;
-
-	data = match->data;
-
-	reg = clkgen_get_register_base(np);
-	if (!reg)
-		return;
-
-	parents = clkgen_mux_get_parents(np, &num_parents);
-	if (IS_ERR(parents))
-		goto err_parents;
-
-	clk_data = kzalloc(sizeof(*clk_data), GFP_KERNEL);
-	if (!clk_data)
-		goto err_alloc;
-
-	clk_data->clk_num = data->num_outputs;
-	clk_data->clks = kcalloc(clk_data->clk_num, sizeof(struct clk *),
-				 GFP_KERNEL);
-
-	if (!clk_data->clks)
-		goto err_alloc_clks;
-
-	for (i = 0; i < clk_data->clk_num; i++) {
-		struct clk *clk;
-		const char *clk_name;
-
-		if (of_property_read_string_index(np, "clock-output-names",
-						  i, &clk_name))
-			break;
-
-		/*
-		 * If we read an empty clock name then the output is unused
-		 */
-		if (*clk_name == '\0')
-			continue;
-
-		clk = clk_register_genamux(clk_name, parents, num_parents,
-					   reg, data, i);
-
-		if (IS_ERR(clk))
-			goto err;
-
-		clk_data->clks[i] = clk;
-	}
-
-	kfree(parents);
-
-	of_clk_add_provider(np, of_clk_src_onecell_get, clk_data);
-	return;
-err:
-	kfree(clk_data->clks);
-err_alloc_clks:
-	kfree(clk_data);
-err_alloc:
-	kfree(parents);
-err_parents:
-	iounmap(reg);
-}
-CLK_OF_DECLARE(clkgenadivmux, "st,clkgena-divmux", st_of_clkgena_divmux_setup);
-
-struct clkgena_prediv_data {
-	u32 offset;
-	u8 shift;
-	struct clk_div_table *table;
-};
-
-static struct clk_div_table prediv_table16[] = {
-	{ .val = 0, .div = 1 },
-	{ .val = 1, .div = 16 },
-	{ .div = 0 },
-};
-
-static struct clkgena_prediv_data prediv_c65_data = {
-	.offset = 0x4c,
-	.shift = 31,
-	.table = prediv_table16,
-};
-
-static struct clkgena_prediv_data prediv_c32_data = {
-	.offset = 0x50,
-	.shift = 1,
-	.table = prediv_table16,
-};
-
-static const struct of_device_id clkgena_prediv_of_match[] = {
-	{ .compatible = "st,clkgena-prediv-c65", .data = &prediv_c65_data },
-	{ .compatible = "st,clkgena-prediv-c32", .data = &prediv_c32_data },
-	{}
-};
-
-static void __init st_of_clkgena_prediv_setup(struct device_node *np)
-{
-	const struct of_device_id *match;
-	void __iomem *reg;
-	const char *parent_name, *clk_name;
-	struct clk *clk;
-	const struct clkgena_prediv_data *data;
-
-	match = of_match_node(clkgena_prediv_of_match, np);
-	if (!match) {
-		pr_err("%s: No matching data\n", __func__);
-		return;
-	}
-
-	data = match->data;
-
-	reg = clkgen_get_register_base(np);
-	if (!reg)
-		return;
-
-	parent_name = of_clk_get_parent_name(np, 0);
-	if (!parent_name)
-		goto err;
-
-	if (of_property_read_string_index(np, "clock-output-names",
-					  0, &clk_name))
-		goto err;
-
-	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))
-		goto err;
-
-	of_clk_add_provider(np, of_clk_src_simple_get, clk);
-	pr_debug("%s: parent %s rate %u\n",
-		__clk_get_name(clk),
-		__clk_get_name(clk_get_parent(clk)),
-		(unsigned int)clk_get_rate(clk));
-
-	return;
-err:
-	iounmap(reg);
-}
-CLK_OF_DECLARE(clkgenaprediv, "st,clkgena-prediv", st_of_clkgena_prediv_setup);
-
 struct clkgen_mux_data {
 	u32 offset;
 	u8 shift;
@@ -541,49 +46,6 @@ struct clkgen_mux_data {
 	u8 mux_flags;
 };
 
-static struct clkgen_mux_data clkgen_mux_c_vcc_hd_416 = {
-	.offset = 0,
-	.shift = 0,
-	.width = 1,
-};
-
-static struct clkgen_mux_data clkgen_mux_f_vcc_fvdp_416 = {
-	.offset = 0,
-	.shift = 0,
-	.width = 1,
-};
-
-static struct clkgen_mux_data clkgen_mux_f_vcc_hva_416 = {
-	.offset = 0,
-	.shift = 0,
-	.width = 1,
-};
-
-static struct clkgen_mux_data clkgen_mux_f_vcc_hd_416 = {
-	.offset = 0,
-	.shift = 16,
-	.width = 1,
-	.lock = &clkgenf_lock,
-};
-
-static struct clkgen_mux_data clkgen_mux_c_vcc_sd_416 = {
-	.offset = 0,
-	.shift = 17,
-	.width = 1,
-	.lock = &clkgenf_lock,
-};
-
-static struct clkgen_mux_data stih415_a9_mux_data = {
-	.offset = 0,
-	.shift = 1,
-	.width = 2,
-	.lock = &clkgen_a9_lock,
-};
-static struct clkgen_mux_data stih416_a9_mux_data = {
-	.offset = 0,
-	.shift = 0,
-	.width = 2,
-};
 static struct clkgen_mux_data stih407_a9_mux_data = {
 	.offset = 0x1a4,
 	.shift = 0,
@@ -591,58 +53,13 @@ static struct clkgen_mux_data stih407_a9_mux_data = {
 	.lock = &clkgen_a9_lock,
 };
 
-static const struct of_device_id mux_of_match[] = {
-	{
-		.compatible = "st,stih416-clkgenc-vcc-hd",
-		.data = &clkgen_mux_c_vcc_hd_416,
-	},
-	{
-		.compatible = "st,stih416-clkgenf-vcc-fvdp",
-		.data = &clkgen_mux_f_vcc_fvdp_416,
-	},
-	{
-		.compatible = "st,stih416-clkgenf-vcc-hva",
-		.data = &clkgen_mux_f_vcc_hva_416,
-	},
-	{
-		.compatible = "st,stih416-clkgenf-vcc-hd",
-		.data = &clkgen_mux_f_vcc_hd_416,
-	},
-	{
-		.compatible = "st,stih416-clkgenf-vcc-sd",
-		.data = &clkgen_mux_c_vcc_sd_416,
-	},
-	{
-		.compatible = "st,stih415-clkgen-a9-mux",
-		.data = &stih415_a9_mux_data,
-	},
-	{
-		.compatible = "st,stih416-clkgen-a9-mux",
-		.data = &stih416_a9_mux_data,
-	},
-	{
-		.compatible = "st,stih407-clkgen-a9-mux",
-		.data = &stih407_a9_mux_data,
-	},
-	{}
-};
-
-static void __init st_of_clkgen_mux_setup(struct device_node *np)
+static void __init st_of_clkgen_mux_setup(struct device_node *np,
+		struct clkgen_mux_data *data)
 {
-	const struct of_device_id *match;
 	struct clk *clk;
 	void __iomem *reg;
 	const char **parents;
-	int num_parents;
-	const struct clkgen_mux_data *data;
-
-	match = of_match_node(mux_of_match, np);
-	if (!match) {
-		pr_err("%s: No matching data\n", __func__);
-		return;
-	}
-
-	data = match->data;
+	int num_parents = 0;
 
 	reg = of_iomap(np, 0);
 	if (!reg) {
@@ -679,161 +96,10 @@ err:
 err_parents:
 	iounmap(reg);
 }
-CLK_OF_DECLARE(clkgen_mux, "st,clkgen-mux", st_of_clkgen_mux_setup);
-
-#define VCC_MAX_CHANNELS 16
-
-#define VCC_GATE_OFFSET 0x0
-#define VCC_MUX_OFFSET 0x4
-#define VCC_DIV_OFFSET 0x8
-
-struct clkgen_vcc_data {
-	spinlock_t *lock;
-	unsigned long clk_flags;
-};
-
-static struct clkgen_vcc_data st_clkgenc_vcc_416 = {
-	.clk_flags = CLK_SET_RATE_PARENT,
-};
-
-static struct clkgen_vcc_data st_clkgenf_vcc_416 = {
-	.lock = &clkgenf_lock,
-};
-
-static const struct of_device_id vcc_of_match[] = {
-	{ .compatible = "st,stih416-clkgenc", .data = &st_clkgenc_vcc_416 },
-	{ .compatible = "st,stih416-clkgenf", .data = &st_clkgenf_vcc_416 },
-	{}
-};
 
-static void __init st_of_clkgen_vcc_setup(struct device_node *np)
+static void __init st_of_clkgen_a9_mux_setup(struct device_node *np)
 {
-	const struct of_device_id *match;
-	void __iomem *reg;
-	const char **parents;
-	int num_parents, i;
-	struct clk_onecell_data *clk_data;
-	const struct clkgen_vcc_data *data;
-
-	match = of_match_node(vcc_of_match, np);
-	if (WARN_ON(!match))
-		return;
-	data = match->data;
-
-	reg = of_iomap(np, 0);
-	if (!reg)
-		return;
-
-	parents = clkgen_mux_get_parents(np, &num_parents);
-	if (IS_ERR(parents))
-		goto err_parents;
-
-	clk_data = kzalloc(sizeof(*clk_data), GFP_KERNEL);
-	if (!clk_data)
-		goto err_alloc;
-
-	clk_data->clk_num = VCC_MAX_CHANNELS;
-	clk_data->clks = kcalloc(clk_data->clk_num, sizeof(struct clk *),
-				 GFP_KERNEL);
-
-	if (!clk_data->clks)
-		goto err_alloc_clks;
-
-	for (i = 0; i < clk_data->clk_num; i++) {
-		struct clk *clk;
-		const char *clk_name;
-		struct clk_gate *gate;
-		struct clk_divider *div;
-		struct clk_mux *mux;
-
-		if (of_property_read_string_index(np, "clock-output-names",
-						  i, &clk_name))
-			break;
-
-		/*
-		 * If we read an empty clock name then the output is unused
-		 */
-		if (*clk_name == '\0')
-			continue;
-
-		gate = kzalloc(sizeof(*gate), GFP_KERNEL);
-		if (!gate)
-			goto err;
-
-		div = kzalloc(sizeof(*div), GFP_KERNEL);
-		if (!div) {
-			kfree(gate);
-			goto err;
-		}
-
-		mux = kzalloc(sizeof(*mux), GFP_KERNEL);
-		if (!mux) {
-			kfree(gate);
-			kfree(div);
-			goto err;
-		}
-
-		gate->reg = reg + VCC_GATE_OFFSET;
-		gate->bit_idx = i;
-		gate->flags = CLK_GATE_SET_TO_DISABLE;
-		gate->lock = data->lock;
-
-		div->reg = reg + VCC_DIV_OFFSET;
-		div->shift = 2 * i;
-		div->width = 2;
-		div->flags = CLK_DIVIDER_POWER_OF_TWO |
-			CLK_DIVIDER_ROUND_CLOSEST;
-
-		mux->reg = reg + VCC_MUX_OFFSET;
-		mux->shift = 2 * i;
-		mux->mask = 0x3;
-
-		clk = clk_register_composite(NULL, clk_name, parents,
-					     num_parents,
-					     &mux->hw, &clk_mux_ops,
-					     &div->hw, &clk_divider_ops,
-					     &gate->hw, &clk_gate_ops,
-					     data->clk_flags |
-					     CLK_GET_RATE_NOCACHE);
-		if (IS_ERR(clk)) {
-			kfree(gate);
-			kfree(div);
-			kfree(mux);
-			goto err;
-		}
-
-		pr_debug("%s: parent %s rate %u\n",
-			__clk_get_name(clk),
-			__clk_get_name(clk_get_parent(clk)),
-			(unsigned int)clk_get_rate(clk));
-
-		clk_data->clks[i] = clk;
-	}
-
-	kfree(parents);
-
-	of_clk_add_provider(np, of_clk_src_onecell_get, clk_data);
-	return;
-
-err:
-	for (i = 0; i < clk_data->clk_num; i++) {
-		struct clk_composite *composite;
-
-		if (!clk_data->clks[i])
-			continue;
-
-		composite = to_clk_composite(__clk_get_hw(clk_data->clks[i]));
-		kfree(to_clk_gate(composite->gate_hw));
-		kfree(to_clk_divider(composite->rate_hw));
-		kfree(to_clk_mux(composite->mux_hw));
-	}
-
-	kfree(clk_data->clks);
-err_alloc_clks:
-	kfree(clk_data);
-err_alloc:
-	kfree(parents);
-err_parents:
-	iounmap(reg);
+	st_of_clkgen_mux_setup(np, &stih407_a9_mux_data);
 }
-CLK_OF_DECLARE(clkgen_vcc, "st,clkgen-vcc", st_of_clkgen_vcc_setup);
+CLK_OF_DECLARE(clkgen_a9mux, "st,stih407-clkgen-a9-mux",
+		st_of_clkgen_a9_mux_setup);
diff --git a/drivers/clk/st/clkgen-pll.c b/drivers/clk/st/clkgen-pll.c
index 0b5990e82e0d..25bda48a5d35 100644
--- a/drivers/clk/st/clkgen-pll.c
+++ b/drivers/clk/st/clkgen-pll.c
@@ -26,14 +26,6 @@ static DEFINE_SPINLOCK(clkgena_c32_odf_lock);
 DEFINE_SPINLOCK(clkgen_a9_lock);
 
 /*
- * Common PLL configuration register bits for PLL800 and PLL1600 C65
- */
-#define C65_MDIV_PLL800_MASK	(0xff)
-#define C65_MDIV_PLL1600_MASK	(0x7)
-#define C65_NDIV_MASK		(0xff)
-#define C65_PDIV_MASK		(0x7)
-
-/*
  * PLL configuration register bits for PLL3200 C32
  */
 #define C32_NDIV_MASK (0xff)
@@ -70,144 +62,10 @@ struct clkgen_pll_data {
 	const struct clk_ops *ops;
 };
 
-static const struct clk_ops st_pll1600c65_ops;
-static const struct clk_ops st_pll800c65_ops;
 static const struct clk_ops stm_pll3200c32_ops;
 static const struct clk_ops stm_pll3200c32_a9_ops;
-static const struct clk_ops st_pll1200c32_ops;
 static const struct clk_ops stm_pll4600c28_ops;
 
-static const struct clkgen_pll_data st_pll1600c65_ax = {
-	.pdn_status	= CLKGEN_FIELD(0x0, 0x1,			19),
-	.pdn_ctrl	= CLKGEN_FIELD(0x10,	0x1,			0),
-	.locked_status	= CLKGEN_FIELD(0x0, 0x1,			31),
-	.mdiv		= CLKGEN_FIELD(0x0, C65_MDIV_PLL1600_MASK,	0),
-	.ndiv		= CLKGEN_FIELD(0x0, C65_NDIV_MASK,		8),
-	.ops		= &st_pll1600c65_ops
-};
-
-static const struct clkgen_pll_data st_pll800c65_ax = {
-	.pdn_status	= CLKGEN_FIELD(0x0,	0x1,			19),
-	.pdn_ctrl	= CLKGEN_FIELD(0xC,	0x1,			1),
-	.locked_status	= CLKGEN_FIELD(0x0,	0x1,			31),
-	.mdiv		= CLKGEN_FIELD(0x0,	C65_MDIV_PLL800_MASK,	0),
-	.ndiv		= CLKGEN_FIELD(0x0,	C65_NDIV_MASK,		8),
-	.pdiv		= CLKGEN_FIELD(0x0,	C65_PDIV_MASK,		16),
-	.ops		= &st_pll800c65_ops
-};
-
-static const struct clkgen_pll_data st_pll3200c32_a1x_0 = {
-	.pdn_status	= CLKGEN_FIELD(0x0,	0x1,			31),
-	.pdn_ctrl	= CLKGEN_FIELD(0x18,	0x1,			0),
-	.locked_status	= CLKGEN_FIELD(0x4,	0x1,			31),
-	.ndiv		= CLKGEN_FIELD(0x0,	C32_NDIV_MASK,		0x0),
-	.idf		= CLKGEN_FIELD(0x4,	C32_IDF_MASK,		0x0),
-	.num_odfs = 4,
-	.odf =	{	CLKGEN_FIELD(0x54,	C32_ODF_MASK,		4),
-			CLKGEN_FIELD(0x54,	C32_ODF_MASK,		10),
-			CLKGEN_FIELD(0x54,	C32_ODF_MASK,		16),
-			CLKGEN_FIELD(0x54,	C32_ODF_MASK,		22) },
-	.odf_gate = {	CLKGEN_FIELD(0x54,	0x1,			0),
-			CLKGEN_FIELD(0x54,	0x1,			1),
-			CLKGEN_FIELD(0x54,	0x1,			2),
-			CLKGEN_FIELD(0x54,	0x1,			3) },
-	.ops		= &stm_pll3200c32_ops,
-};
-
-static const struct clkgen_pll_data st_pll3200c32_a1x_1 = {
-	.pdn_status	= CLKGEN_FIELD(0xC,	0x1,			31),
-	.pdn_ctrl	= CLKGEN_FIELD(0x18,	0x1,			1),
-	.locked_status	= CLKGEN_FIELD(0x10,	0x1,			31),
-	.ndiv		= CLKGEN_FIELD(0xC,	C32_NDIV_MASK,		0x0),
-	.idf		= CLKGEN_FIELD(0x10,	C32_IDF_MASK,		0x0),
-	.num_odfs = 4,
-	.odf = {	CLKGEN_FIELD(0x58,	C32_ODF_MASK,		4),
-			CLKGEN_FIELD(0x58,	C32_ODF_MASK,		10),
-			CLKGEN_FIELD(0x58,	C32_ODF_MASK,		16),
-			CLKGEN_FIELD(0x58,	C32_ODF_MASK,		22) },
-	.odf_gate = {	CLKGEN_FIELD(0x58,	0x1,			0),
-			CLKGEN_FIELD(0x58,	0x1,			1),
-			CLKGEN_FIELD(0x58,	0x1,			2),
-			CLKGEN_FIELD(0x58,	0x1,			3) },
-	.ops		= &stm_pll3200c32_ops,
-};
-
-/* 415 specific */
-static const struct clkgen_pll_data st_pll3200c32_a9_415 = {
-	.pdn_status	= CLKGEN_FIELD(0x0,	0x1,			0),
-	.pdn_ctrl	= CLKGEN_FIELD(0x0,	0x1,			0),
-	.locked_status	= CLKGEN_FIELD(0x6C,	0x1,			0),
-	.ndiv		= CLKGEN_FIELD(0x0,	C32_NDIV_MASK,		9),
-	.idf		= CLKGEN_FIELD(0x0,	C32_IDF_MASK,		22),
-	.num_odfs = 1,
-	.odf =		{ CLKGEN_FIELD(0x0,	C32_ODF_MASK,		3) },
-	.odf_gate =	{ CLKGEN_FIELD(0x0,	0x1,			28) },
-	.ops		= &stm_pll3200c32_ops,
-};
-
-static const struct clkgen_pll_data st_pll3200c32_ddr_415 = {
-	.pdn_status	= CLKGEN_FIELD(0x0,	0x1,			0),
-	.pdn_ctrl	= CLKGEN_FIELD(0x0,	0x1,			0),
-	.locked_status	= CLKGEN_FIELD(0x100,	0x1,			0),
-	.ndiv		= CLKGEN_FIELD(0x8,	C32_NDIV_MASK,		0),
-	.idf		= CLKGEN_FIELD(0x0,	C32_IDF_MASK,		25),
-	.num_odfs = 2,
-	.odf		= { CLKGEN_FIELD(0x8,	C32_ODF_MASK,		8),
-			    CLKGEN_FIELD(0x8,	C32_ODF_MASK,		14) },
-	.odf_gate	= { CLKGEN_FIELD(0x4,	0x1,			28),
-			    CLKGEN_FIELD(0x4,	0x1,			29) },
-	.ops		= &stm_pll3200c32_ops,
-};
-
-static const struct clkgen_pll_data st_pll1200c32_gpu_415 = {
-	.pdn_status	= CLKGEN_FIELD(0x4,	0x1,			0),
-	.pdn_ctrl	= CLKGEN_FIELD(0x4,	0x1,			0),
-	.locked_status	= CLKGEN_FIELD(0x168,	0x1,			0),
-	.ldf		= CLKGEN_FIELD(0x0,	C32_LDF_MASK,		3),
-	.idf		= CLKGEN_FIELD(0x0,	C32_IDF_MASK,		0),
-	.num_odfs = 0,
-	.odf		= { CLKGEN_FIELD(0x0,	C32_ODF_MASK,		10) },
-	.ops		= &st_pll1200c32_ops,
-};
-
-/* 416 specific */
-static const struct clkgen_pll_data st_pll3200c32_a9_416 = {
-	.pdn_status	= CLKGEN_FIELD(0x0,	0x1,			0),
-	.pdn_ctrl	= CLKGEN_FIELD(0x0,	0x1,			0),
-	.locked_status	= CLKGEN_FIELD(0x6C,	0x1,			0),
-	.ndiv		= CLKGEN_FIELD(0x8,	C32_NDIV_MASK,		0),
-	.idf		= CLKGEN_FIELD(0x0,	C32_IDF_MASK,		25),
-	.num_odfs = 1,
-	.odf		= { CLKGEN_FIELD(0x8,	C32_ODF_MASK,		8) },
-	.odf_gate	= { CLKGEN_FIELD(0x4,	0x1,			28) },
-	.ops		= &stm_pll3200c32_ops,
-};
-
-static const struct clkgen_pll_data st_pll3200c32_ddr_416 = {
-	.pdn_status	= CLKGEN_FIELD(0x0,	0x1,			0),
-	.pdn_ctrl	= CLKGEN_FIELD(0x0,	0x1,			0),
-	.locked_status	= CLKGEN_FIELD(0x10C,	0x1,			0),
-	.ndiv		= CLKGEN_FIELD(0x8,	C32_NDIV_MASK,		0),
-	.idf		= CLKGEN_FIELD(0x0,	C32_IDF_MASK,		25),
-	.num_odfs = 2,
-	.odf		= { CLKGEN_FIELD(0x8,	C32_ODF_MASK,		8),
-			    CLKGEN_FIELD(0x8,	C32_ODF_MASK,		14) },
-	.odf_gate	= { CLKGEN_FIELD(0x4,	0x1,			28),
-			    CLKGEN_FIELD(0x4,	0x1,			29) },
-	.ops		= &stm_pll3200c32_ops,
-};
-
-static const struct clkgen_pll_data st_pll1200c32_gpu_416 = {
-	.pdn_status	= CLKGEN_FIELD(0x8E4,	0x1,			3),
-	.pdn_ctrl	= CLKGEN_FIELD(0x8E4,	0x1,			3),
-	.locked_status	= CLKGEN_FIELD(0x90C,	0x1,			0),
-	.ldf		= CLKGEN_FIELD(0x0,	C32_LDF_MASK,		3),
-	.idf		= CLKGEN_FIELD(0x0,	C32_IDF_MASK,		0),
-	.num_odfs = 0,
-	.odf		= { CLKGEN_FIELD(0x0,	C32_ODF_MASK,		10) },
-	.ops		= &st_pll1200c32_ops,
-};
-
 static const struct clkgen_pll_data st_pll3200c32_407_a0 = {
 	/* 407 A0 */
 	.pdn_status	= CLKGEN_FIELD(0x2a0,	0x1,			8),
@@ -410,57 +268,6 @@ static void clkgen_pll_disable(struct clk_hw *hw)
 		spin_unlock_irqrestore(pll->lock, flags);
 }
 
-static unsigned long recalc_stm_pll800c65(struct clk_hw *hw,
-		unsigned long parent_rate)
-{
-	struct clkgen_pll *pll = to_clkgen_pll(hw);
-	unsigned long mdiv, ndiv, pdiv;
-	unsigned long rate;
-	uint64_t res;
-
-	if (!clkgen_pll_is_enabled(hw) || !clkgen_pll_is_locked(hw))
-		return 0;
-
-	pdiv = CLKGEN_READ(pll, pdiv);
-	mdiv = CLKGEN_READ(pll, mdiv);
-	ndiv = CLKGEN_READ(pll, ndiv);
-
-	if (!mdiv)
-		mdiv++; /* mdiv=0 or 1 => MDIV=1 */
-
-	res = (uint64_t)2 * (uint64_t)parent_rate * (uint64_t)ndiv;
-	rate = (unsigned long)div64_u64(res, mdiv * (1 << pdiv));
-
-	pr_debug("%s:%s rate %lu\n", clk_hw_get_name(hw), __func__, rate);
-
-	return rate;
-
-}
-
-static unsigned long recalc_stm_pll1600c65(struct clk_hw *hw,
-		unsigned long parent_rate)
-{
-	struct clkgen_pll *pll = to_clkgen_pll(hw);
-	unsigned long mdiv, ndiv;
-	unsigned long rate;
-
-	if (!clkgen_pll_is_enabled(hw) || !clkgen_pll_is_locked(hw))
-		return 0;
-
-	mdiv = CLKGEN_READ(pll, mdiv);
-	ndiv = CLKGEN_READ(pll, ndiv);
-
-	if (!mdiv)
-		mdiv = 1;
-
-	/* Note: input is divided by 1000 to avoid overflow */
-	rate = ((2 * (parent_rate / 1000) * ndiv) / mdiv) * 1000;
-
-	pr_debug("%s:%s rate %lu\n", clk_hw_get_name(hw), __func__, rate);
-
-	return rate;
-}
-
 static int clk_pll3200c32_get_params(unsigned long input, unsigned long output,
 			  struct stm_pll *pll)
 {
@@ -608,33 +415,6 @@ static int set_rate_stm_pll3200c32(struct clk_hw *hw, unsigned long rate,
 	return 0;
 }
 
-static unsigned long recalc_stm_pll1200c32(struct clk_hw *hw,
-		unsigned long parent_rate)
-{
-	struct clkgen_pll *pll = to_clkgen_pll(hw);
-	unsigned long odf, ldf, idf;
-	unsigned long rate;
-
-	if (!clkgen_pll_is_enabled(hw) || !clkgen_pll_is_locked(hw))
-		return 0;
-
-	odf = CLKGEN_READ(pll, odf[0]);
-	ldf = CLKGEN_READ(pll, ldf);
-	idf = CLKGEN_READ(pll, idf);
-
-	if (!idf) /* idf==0 means 1 */
-		idf = 1;
-	if (!odf) /* odf==0 means 1 */
-		odf = 1;
-
-	/* Note: input is divided by 1000 to avoid overflow */
-	rate = (((parent_rate / 1000) * ldf) / (odf * idf)) * 1000;
-
-	pr_debug("%s:%s rate %lu\n", clk_hw_get_name(hw), __func__, rate);
-
-	return rate;
-}
-
 /* PLL output structure
  * FVCO >> /2 >> FVCOBY2 (no output)
  *                 |> Divider (ODF) >> PHI
@@ -792,20 +572,6 @@ static int set_rate_stm_pll4600c28(struct clk_hw *hw, unsigned long rate,
 	return 0;
 }
 
-static const struct clk_ops st_pll1600c65_ops = {
-	.enable		= clkgen_pll_enable,
-	.disable	= clkgen_pll_disable,
-	.is_enabled	= clkgen_pll_is_enabled,
-	.recalc_rate	= recalc_stm_pll1600c65,
-};
-
-static const struct clk_ops st_pll800c65_ops = {
-	.enable		= clkgen_pll_enable,
-	.disable	= clkgen_pll_disable,
-	.is_enabled	= clkgen_pll_is_enabled,
-	.recalc_rate	= recalc_stm_pll800c65,
-};
-
 static const struct clk_ops stm_pll3200c32_ops = {
 	.enable		= clkgen_pll_enable,
 	.disable	= clkgen_pll_disable,
@@ -822,13 +588,6 @@ static const struct clk_ops stm_pll3200c32_a9_ops = {
 	.set_rate	= set_rate_stm_pll3200c32,
 };
 
-static const struct clk_ops st_pll1200c32_ops = {
-	.enable		= clkgen_pll_enable,
-	.disable	= clkgen_pll_disable,
-	.is_enabled	= clkgen_pll_is_enabled,
-	.recalc_rate	= recalc_stm_pll1200c32,
-};
-
 static const struct clk_ops stm_pll4600c28_ops = {
 	.enable		= clkgen_pll_enable,
 	.disable	= clkgen_pll_disable,
@@ -877,22 +636,6 @@ static struct clk * __init clkgen_pll_register(const char *parent_name,
 	return clk;
 }
 
-static struct clk * __init clkgen_c65_lsdiv_register(const char *parent_name,
-						     const char *clk_name)
-{
-	struct clk *clk;
-
-	clk = clk_register_fixed_factor(NULL, clk_name, parent_name, 0, 1, 2);
-	if (IS_ERR(clk))
-		return clk;
-
-	pr_debug("%s: parent %s rate %lu\n",
-			__clk_get_name(clk),
-			__clk_get_name(clk_get_parent(clk)),
-			clk_get_rate(clk));
-	return clk;
-}
-
 static void __iomem * __init clkgen_get_register_base(
 				struct device_node *np)
 {
@@ -909,89 +652,6 @@ static void __iomem * __init clkgen_get_register_base(
 	return reg;
 }
 
-#define CLKGENAx_PLL0_OFFSET 0x0
-#define CLKGENAx_PLL1_OFFSET 0x4
-
-static void __init clkgena_c65_pll_setup(struct device_node *np)
-{
-	const int num_pll_outputs = 3;
-	struct clk_onecell_data *clk_data;
-	const char *parent_name;
-	void __iomem *reg;
-	const char *clk_name;
-
-	parent_name = of_clk_get_parent_name(np, 0);
-	if (!parent_name)
-		return;
-
-	reg = clkgen_get_register_base(np);
-	if (!reg)
-		return;
-
-	clk_data = kzalloc(sizeof(*clk_data), GFP_KERNEL);
-	if (!clk_data)
-		return;
-
-	clk_data->clk_num = num_pll_outputs;
-	clk_data->clks = kzalloc(clk_data->clk_num * sizeof(struct clk *),
-				 GFP_KERNEL);
-
-	if (!clk_data->clks)
-		goto err;
-
-	if (of_property_read_string_index(np, "clock-output-names",
-					  0, &clk_name))
-		goto err;
-
-	/*
-	 * PLL0 HS (high speed) output
-	 */
-	clk_data->clks[0] = clkgen_pll_register(parent_name,
-			(struct clkgen_pll_data *) &st_pll1600c65_ax,
-			reg + CLKGENAx_PLL0_OFFSET, 0, clk_name, NULL);
-
-	if (IS_ERR(clk_data->clks[0]))
-		goto err;
-
-	if (of_property_read_string_index(np, "clock-output-names",
-					  1, &clk_name))
-		goto err;
-
-	/*
-	 * PLL0 LS (low speed) output, which is a fixed divide by 2 of the
-	 * high speed output.
-	 */
-	clk_data->clks[1] = clkgen_c65_lsdiv_register(__clk_get_name
-						      (clk_data->clks[0]),
-						      clk_name);
-
-	if (IS_ERR(clk_data->clks[1]))
-		goto err;
-
-	if (of_property_read_string_index(np, "clock-output-names",
-					  2, &clk_name))
-		goto err;
-
-	/*
-	 * PLL1 output
-	 */
-	clk_data->clks[2] = clkgen_pll_register(parent_name,
-			(struct clkgen_pll_data *) &st_pll800c65_ax,
-			reg + CLKGENAx_PLL1_OFFSET, 0, clk_name, NULL);
-
-	if (IS_ERR(clk_data->clks[2]))
-		goto err;
-
-	of_clk_add_provider(np, of_clk_src_onecell_get, clk_data);
-	return;
-
-err:
-	kfree(clk_data->clks);
-	kfree(clk_data);
-}
-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,
 					       struct clkgen_pll_data *pll_data,
@@ -1042,72 +702,17 @@ static struct clk * __init clkgen_odf_register(const char *parent_name,
 	return clk;
 }
 
-static const struct of_device_id c32_pll_of_match[] = {
-	{
-		.compatible = "st,plls-c32-a1x-0",
-		.data = &st_pll3200c32_a1x_0,
-	},
-	{
-		.compatible = "st,plls-c32-a1x-1",
-		.data = &st_pll3200c32_a1x_1,
-	},
-	{
-		.compatible = "st,stih415-plls-c32-a9",
-		.data = &st_pll3200c32_a9_415,
-	},
-	{
-		.compatible = "st,stih415-plls-c32-ddr",
-		.data = &st_pll3200c32_ddr_415,
-	},
-	{
-		.compatible = "st,stih416-plls-c32-a9",
-		.data = &st_pll3200c32_a9_416,
-	},
-	{
-		.compatible = "st,stih416-plls-c32-ddr",
-		.data = &st_pll3200c32_ddr_416,
-	},
-	{
-		.compatible = "st,stih407-plls-c32-a0",
-		.data = &st_pll3200c32_407_a0,
-	},
-	{
-		.compatible = "st,plls-c32-cx_0",
-		.data = &st_pll3200c32_cx_0,
-	},
-	{
-		.compatible = "st,plls-c32-cx_1",
-		.data = &st_pll3200c32_cx_1,
-	},
-	{
-		.compatible = "st,stih407-plls-c32-a9",
-		.data = &st_pll3200c32_407_a9,
-	},
-	{
-		.compatible = "st,stih418-plls-c28-a9",
-		.data = &st_pll4600c28_418_a9,
-	},
-	{}
-};
 
-static void __init clkgen_c32_pll_setup(struct device_node *np)
+static void __init clkgen_c32_pll_setup(struct device_node *np,
+		struct clkgen_pll_data *data)
 {
-	const struct of_device_id *match;
 	struct clk *clk;
 	const char *parent_name, *pll_name;
 	void __iomem *pll_base;
 	int num_odfs, odf;
 	struct clk_onecell_data *clk_data;
-	struct clkgen_pll_data	*data;
 	unsigned long pll_flags = 0;
 
-	match = of_match_node(c32_pll_of_match, np);
-	if (!match) {
-		pr_err("%s: No matching data\n", __func__);
-		return;
-	}
-
-	data = (struct clkgen_pll_data *) match->data;
 
 	parent_name = of_clk_get_parent_name(np, 0);
 	if (!parent_name)
@@ -1166,59 +771,30 @@ err:
 	kfree(clk_data->clks);
 	kfree(clk_data);
 }
-CLK_OF_DECLARE(clkgen_c32_pll, "st,clkgen-plls-c32", clkgen_c32_pll_setup);
-
-static const struct of_device_id c32_gpu_pll_of_match[] = {
-	{
-		.compatible = "st,stih415-gpu-pll-c32",
-		.data = &st_pll1200c32_gpu_415,
-	},
-	{
-		.compatible = "st,stih416-gpu-pll-c32",
-		.data = &st_pll1200c32_gpu_416,
-	},
-	{}
-};
-
-static void __init clkgengpu_c32_pll_setup(struct device_node *np)
+static void __init clkgen_c32_pll0_setup(struct device_node *np)
 {
-	const struct of_device_id *match;
-	struct clk *clk;
-	const char *parent_name;
-	void __iomem *reg;
-	const char *clk_name;
-	struct clkgen_pll_data	*data;
-
-	match = of_match_node(c32_gpu_pll_of_match, np);
-	if (!match) {
-		pr_err("%s: No matching data\n", __func__);
-		return;
-	}
-
-	data = (struct clkgen_pll_data *)match->data;
-
-	parent_name = of_clk_get_parent_name(np, 0);
-	if (!parent_name)
-		return;
-
-	reg = clkgen_get_register_base(np);
-	if (!reg)
-		return;
-
-	if (of_property_read_string_index(np, "clock-output-names",
-					  0, &clk_name))
-		return;
+	clkgen_c32_pll_setup(np,
+			(struct clkgen_pll_data *) &st_pll3200c32_cx_0);
+}
+CLK_OF_DECLARE(c32_pll0, "st,clkgen-pll0", clkgen_c32_pll0_setup);
 
-	/*
-	 * PLL 1200MHz output
-	 */
-	clk = clkgen_pll_register(parent_name, data, reg,
-				  0, clk_name, data->lock);
+static void __init clkgen_c32_pll1_setup(struct device_node *np)
+{
+	clkgen_c32_pll_setup(np,
+			(struct clkgen_pll_data *) &st_pll3200c32_cx_1);
+}
+CLK_OF_DECLARE(c32_pll1, "st,clkgen-pll1", clkgen_c32_pll1_setup);
 
-	if (!IS_ERR(clk))
-		of_clk_add_provider(np, of_clk_src_simple_get, clk);
+static void __init clkgen_c32_plla9_setup(struct device_node *np)
+{
+	clkgen_c32_pll_setup(np,
+			(struct clkgen_pll_data *) &st_pll3200c32_407_a9);
+}
+CLK_OF_DECLARE(c32_plla9, "st,stih407-clkgen-plla9", clkgen_c32_plla9_setup);
 
-	return;
+static void __init clkgen_c28_plla9_setup(struct device_node *np)
+{
+	clkgen_c32_pll_setup(np,
+			(struct clkgen_pll_data *) &st_pll4600c28_418_a9);
 }
-CLK_OF_DECLARE(clkgengpu_c32_pll,
-	       "st,clkgengpu-pll-c32", clkgengpu_c32_pll_setup);
+CLK_OF_DECLARE(c28_plla9, "st,stih418-clkgen-plla9", clkgen_c28_plla9_setup);
diff --git a/drivers/clk/sunxi-ng/Kconfig b/drivers/clk/sunxi-ng/Kconfig
index 2afcbd39e41e..254d9526c018 100644
--- a/drivers/clk/sunxi-ng/Kconfig
+++ b/drivers/clk/sunxi-ng/Kconfig
@@ -1,5 +1,6 @@
 config SUNXI_CCU
 	bool "Clock support for Allwinner SoCs"
+	depends on ARCH_SUNXI || COMPILE_TEST
 	default ARCH_SUNXI
 
 if SUNXI_CCU
@@ -19,6 +20,10 @@ config SUNXI_CCU_GATE
 config SUNXI_CCU_MUX
 	bool
 
+config SUNXI_CCU_MULT
+	bool
+	select SUNXI_CCU_MUX
+
 config SUNXI_CCU_PHASE
 	bool
 
@@ -51,6 +56,40 @@ config SUNXI_CCU_MP
 
 # SoC Drivers
 
+config SUN6I_A31_CCU
+	bool "Support for the Allwinner A31/A31s CCU"
+	select SUNXI_CCU_DIV
+	select SUNXI_CCU_NK
+	select SUNXI_CCU_NKM
+	select SUNXI_CCU_NM
+	select SUNXI_CCU_MP
+	select SUNXI_CCU_PHASE
+	default MACH_SUN6I
+
+config SUN8I_A23_CCU
+	bool "Support for the Allwinner A23 CCU"
+	select SUNXI_CCU_DIV
+	select SUNXI_CCU_MULT
+	select SUNXI_CCU_NK
+	select SUNXI_CCU_NKM
+	select SUNXI_CCU_NKMP
+	select SUNXI_CCU_NM
+	select SUNXI_CCU_MP
+	select SUNXI_CCU_PHASE
+	default MACH_SUN8I
+
+config SUN8I_A33_CCU
+	bool "Support for the Allwinner A33 CCU"
+	select SUNXI_CCU_DIV
+	select SUNXI_CCU_MULT
+	select SUNXI_CCU_NK
+	select SUNXI_CCU_NKM
+	select SUNXI_CCU_NKMP
+	select SUNXI_CCU_NM
+	select SUNXI_CCU_MP
+	select SUNXI_CCU_PHASE
+	default MACH_SUN8I
+
 config SUN8I_H3_CCU
 	bool "Support for the Allwinner H3 CCU"
 	select SUNXI_CCU_DIV
diff --git a/drivers/clk/sunxi-ng/Makefile b/drivers/clk/sunxi-ng/Makefile
index 633ce642ffae..106cba27c331 100644
--- a/drivers/clk/sunxi-ng/Makefile
+++ b/drivers/clk/sunxi-ng/Makefile
@@ -7,6 +7,7 @@ obj-$(CONFIG_SUNXI_CCU_DIV)	+= ccu_div.o
 obj-$(CONFIG_SUNXI_CCU_FRAC)	+= ccu_frac.o
 obj-$(CONFIG_SUNXI_CCU_GATE)	+= ccu_gate.o
 obj-$(CONFIG_SUNXI_CCU_MUX)	+= ccu_mux.o
+obj-$(CONFIG_SUNXI_CCU_MULT)	+= ccu_mult.o
 obj-$(CONFIG_SUNXI_CCU_PHASE)	+= ccu_phase.o
 
 # Multi-factor clocks
@@ -17,4 +18,7 @@ obj-$(CONFIG_SUNXI_CCU_NM)	+= ccu_nm.o
 obj-$(CONFIG_SUNXI_CCU_MP)	+= ccu_mp.o
 
 # SoC support
+obj-$(CONFIG_SUN6I_A31_CCU)	+= ccu-sun6i-a31.o
+obj-$(CONFIG_SUN8I_A23_CCU)	+= ccu-sun8i-a23.o
+obj-$(CONFIG_SUN8I_A33_CCU)	+= ccu-sun8i-a33.o
 obj-$(CONFIG_SUN8I_H3_CCU)	+= ccu-sun8i-h3.o
diff --git a/drivers/clk/sunxi-ng/ccu-sun6i-a31.c b/drivers/clk/sunxi-ng/ccu-sun6i-a31.c
new file mode 100644
index 000000000000..79596463e0d9
--- /dev/null
+++ b/drivers/clk/sunxi-ng/ccu-sun6i-a31.c
@@ -0,0 +1,1239 @@
+/*
+ * Copyright (c) 2016 Chen-Yu Tsai
+ *
+ * Chen-Yu Tsai <wens@csie.org>
+ *
+ * Based on ccu-sun8i-h3.c by Maxime Ripard.
+ *
+ * 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/of_address.h>
+
+#include "ccu_common.h"
+#include "ccu_reset.h"
+
+#include "ccu_div.h"
+#include "ccu_gate.h"
+#include "ccu_mp.h"
+#include "ccu_mult.h"
+#include "ccu_mux.h"
+#include "ccu_nk.h"
+#include "ccu_nkm.h"
+#include "ccu_nkmp.h"
+#include "ccu_nm.h"
+#include "ccu_phase.h"
+
+#include "ccu-sun6i-a31.h"
+
+static SUNXI_CCU_NKM_WITH_GATE_LOCK(pll_cpu_clk, "pll-cpu",
+				     "osc24M", 0x000,
+				     8, 5,	/* N */
+				     4, 2,	/* K */
+				     0, 2,	/* M */
+				     BIT(31),	/* gate */
+				     BIT(28),	/* lock */
+				     0);
+
+/*
+ * The Audio PLL is supposed to have 4 outputs: 3 fixed factors from
+ * the base (2x, 4x and 8x), and one variable divider (the one true
+ * pll audio).
+ *
+ * We don't have any need for the variable divider for now, so we just
+ * hardcode it to match with the clock names
+ */
+#define SUN6I_A31_PLL_AUDIO_REG	0x008
+
+static SUNXI_CCU_NM_WITH_GATE_LOCK(pll_audio_base_clk, "pll-audio-base",
+				   "osc24M", 0x008,
+				   8, 7,	/* N */
+				   0, 5,	/* M */
+				   BIT(31),	/* gate */
+				   BIT(28),	/* lock */
+				   CLK_SET_RATE_UNGATE);
+
+static SUNXI_CCU_NM_WITH_FRAC_GATE_LOCK(pll_video0_clk, "pll-video0",
+					"osc24M", 0x010,
+					8, 7,		/* N */
+					0, 4,		/* M */
+					BIT(24),	/* frac enable */
+					BIT(25),	/* frac select */
+					270000000,	/* frac rate 0 */
+					297000000,	/* frac rate 1 */
+					BIT(31),	/* gate */
+					BIT(28),	/* lock */
+					CLK_SET_RATE_UNGATE);
+
+static SUNXI_CCU_NM_WITH_FRAC_GATE_LOCK(pll_ve_clk, "pll-ve",
+					"osc24M", 0x018,
+					8, 7,		/* N */
+					0, 4,		/* M */
+					BIT(24),	/* frac enable */
+					BIT(25),	/* frac select */
+					270000000,	/* frac rate 0 */
+					297000000,	/* frac rate 1 */
+					BIT(31),	/* gate */
+					BIT(28),	/* lock */
+					CLK_SET_RATE_UNGATE);
+
+static SUNXI_CCU_NKM_WITH_GATE_LOCK(pll_ddr_clk, "pll-ddr",
+				    "osc24M", 0x020,
+				    8, 5,	/* N */
+				    4, 2,	/* K */
+				    0, 2,	/* M */
+				    BIT(31),	/* gate */
+				    BIT(28),	/* lock */
+				    CLK_SET_RATE_UNGATE);
+
+static SUNXI_CCU_NK_WITH_GATE_LOCK_POSTDIV(pll_periph_clk, "pll-periph",
+					   "osc24M", 0x028,
+					   8, 5,	/* N */
+					   4, 2,	/* K */
+					   BIT(31),	/* gate */
+					   BIT(28),	/* lock */
+					   2,		/* post-div */
+					   CLK_SET_RATE_UNGATE);
+
+static SUNXI_CCU_NM_WITH_FRAC_GATE_LOCK(pll_video1_clk, "pll-video1",
+					"osc24M", 0x030,
+					8, 7,		/* N */
+					0, 4,		/* M */
+					BIT(24),	/* frac enable */
+					BIT(25),	/* frac select */
+					270000000,	/* frac rate 0 */
+					297000000,	/* frac rate 1 */
+					BIT(31),	/* gate */
+					BIT(28),	/* lock */
+					CLK_SET_RATE_UNGATE);
+
+static SUNXI_CCU_NM_WITH_FRAC_GATE_LOCK(pll_gpu_clk, "pll-gpu",
+					"osc24M", 0x038,
+					8, 7,		/* N */
+					0, 4,		/* M */
+					BIT(24),	/* frac enable */
+					BIT(25),	/* frac select */
+					270000000,	/* frac rate 0 */
+					297000000,	/* frac rate 1 */
+					BIT(31),	/* gate */
+					BIT(28),	/* lock */
+					CLK_SET_RATE_UNGATE);
+
+/*
+ * The MIPI PLL has 2 modes: "MIPI" and "HDMI".
+ *
+ * The MIPI mode is a standard NKM-style clock. The HDMI mode is an
+ * integer / fractional clock with switchable multipliers and dividers.
+ * This is not supported here. We hardcode the PLL to MIPI mode.
+ */
+#define SUN6I_A31_PLL_MIPI_REG	0x040
+
+static const char * const pll_mipi_parents[] = { "pll-video0", "pll-video1" };
+static SUNXI_CCU_NKM_WITH_MUX_GATE_LOCK(pll_mipi_clk, "pll-mipi",
+					pll_mipi_parents, 0x040,
+					8, 4,	/* N */
+					4, 2,	/* K */
+					0, 4,	/* M */
+					21, 0,	/* mux */
+					BIT(31),	/* gate */
+					BIT(28),	/* lock */
+					CLK_SET_RATE_UNGATE);
+
+static SUNXI_CCU_NM_WITH_FRAC_GATE_LOCK(pll9_clk, "pll9",
+					"osc24M", 0x044,
+					8, 7,		/* N */
+					0, 4,		/* M */
+					BIT(24),	/* frac enable */
+					BIT(25),	/* frac select */
+					270000000,	/* frac rate 0 */
+					297000000,	/* frac rate 1 */
+					BIT(31),	/* gate */
+					BIT(28),	/* lock */
+					CLK_SET_RATE_UNGATE);
+
+static SUNXI_CCU_NM_WITH_FRAC_GATE_LOCK(pll10_clk, "pll10",
+					"osc24M", 0x048,
+					8, 7,		/* N */
+					0, 4,		/* M */
+					BIT(24),	/* frac enable */
+					BIT(25),	/* frac select */
+					270000000,	/* frac rate 0 */
+					297000000,	/* frac rate 1 */
+					BIT(31),	/* gate */
+					BIT(28),	/* lock */
+					CLK_SET_RATE_UNGATE);
+
+static const char * const cpux_parents[] = { "osc32k", "osc24M",
+					     "pll-cpu", "pll-cpu" };
+static SUNXI_CCU_MUX(cpu_clk, "cpu", cpux_parents,
+		     0x050, 16, 2, CLK_SET_RATE_PARENT | CLK_IS_CRITICAL);
+
+static struct clk_div_table axi_div_table[] = {
+	{ .val = 0, .div = 1 },
+	{ .val = 1, .div = 2 },
+	{ .val = 2, .div = 3 },
+	{ .val = 3, .div = 4 },
+	{ .val = 4, .div = 4 },
+	{ .val = 5, .div = 4 },
+	{ .val = 6, .div = 4 },
+	{ .val = 7, .div = 4 },
+	{ /* Sentinel */ },
+};
+
+static SUNXI_CCU_DIV_TABLE(axi_clk, "axi", "cpu",
+			   0x050, 0, 3, axi_div_table, 0);
+
+static const char * const ahb1_parents[] = { "osc32k", "osc24M",
+					     "axi", "pll-periph" };
+
+static struct ccu_div ahb1_clk = {
+	.div		= _SUNXI_CCU_DIV_FLAGS(4, 2, CLK_DIVIDER_POWER_OF_TWO),
+
+	.mux		= {
+		.shift	= 12,
+		.width	= 2,
+
+		.variable_prediv	= {
+			.index	= 3,
+			.shift	= 6,
+			.width	= 2,
+		},
+	},
+
+	.common		= {
+		.reg		= 0x054,
+		.features	= CCU_FEATURE_VARIABLE_PREDIV,
+		.hw.init	= CLK_HW_INIT_PARENTS("ahb1",
+						      ahb1_parents,
+						      &ccu_div_ops,
+						      0),
+	},
+};
+
+static struct clk_div_table apb1_div_table[] = {
+	{ .val = 0, .div = 2 },
+	{ .val = 1, .div = 2 },
+	{ .val = 2, .div = 4 },
+	{ .val = 3, .div = 8 },
+	{ /* Sentinel */ },
+};
+
+static SUNXI_CCU_DIV_TABLE(apb1_clk, "apb1", "ahb1",
+			   0x054, 8, 2, apb1_div_table, 0);
+
+static const char * const apb2_parents[] = { "osc32k", "osc24M",
+					     "pll-periph", "pll-periph" };
+static SUNXI_CCU_MP_WITH_MUX(apb2_clk, "apb2", apb2_parents, 0x058,
+			     0, 5,	/* M */
+			     16, 2,	/* P */
+			     24, 2,	/* mux */
+			     0);
+
+static SUNXI_CCU_GATE(ahb1_mipidsi_clk,	"ahb1-mipidsi",	"ahb1",
+		      0x060, BIT(1), 0);
+static SUNXI_CCU_GATE(ahb1_ss_clk,	"ahb1-ss",	"ahb1",
+		      0x060, BIT(5), 0);
+static SUNXI_CCU_GATE(ahb1_dma_clk,	"ahb1-dma",	"ahb1",
+		      0x060, BIT(6), 0);
+static SUNXI_CCU_GATE(ahb1_mmc0_clk,	"ahb1-mmc0",	"ahb1",
+		      0x060, BIT(8), 0);
+static SUNXI_CCU_GATE(ahb1_mmc1_clk,	"ahb1-mmc1",	"ahb1",
+		      0x060, BIT(9), 0);
+static SUNXI_CCU_GATE(ahb1_mmc2_clk,	"ahb1-mmc2",	"ahb1",
+		      0x060, BIT(10), 0);
+static SUNXI_CCU_GATE(ahb1_mmc3_clk,	"ahb1-mmc3",	"ahb1",
+		      0x060, BIT(12), 0);
+static SUNXI_CCU_GATE(ahb1_nand1_clk,	"ahb1-nand1",	"ahb1",
+		      0x060, BIT(13), 0);
+static SUNXI_CCU_GATE(ahb1_nand0_clk,	"ahb1-nand0",	"ahb1",
+		      0x060, BIT(13), 0);
+static SUNXI_CCU_GATE(ahb1_sdram_clk,	"ahb1-sdram",	"ahb1",
+		      0x060, BIT(14), 0);
+static SUNXI_CCU_GATE(ahb1_emac_clk,	"ahb1-emac",	"ahb1",
+		      0x060, BIT(17), 0);
+static SUNXI_CCU_GATE(ahb1_ts_clk,	"ahb1-ts",	"ahb1",
+		      0x060, BIT(18), 0);
+static SUNXI_CCU_GATE(ahb1_hstimer_clk,	"ahb1-hstimer",	"ahb1",
+		      0x060, BIT(19), 0);
+static SUNXI_CCU_GATE(ahb1_spi0_clk,	"ahb1-spi0",	"ahb1",
+		      0x060, BIT(20), 0);
+static SUNXI_CCU_GATE(ahb1_spi1_clk,	"ahb1-spi1",	"ahb1",
+		      0x060, BIT(21), 0);
+static SUNXI_CCU_GATE(ahb1_spi2_clk,	"ahb1-spi2",	"ahb1",
+		      0x060, BIT(22), 0);
+static SUNXI_CCU_GATE(ahb1_spi3_clk,	"ahb1-spi3",	"ahb1",
+		      0x060, BIT(23), 0);
+static SUNXI_CCU_GATE(ahb1_otg_clk,	"ahb1-otg",	"ahb1",
+		      0x060, BIT(24), 0);
+static SUNXI_CCU_GATE(ahb1_ehci0_clk,	"ahb1-ehci0",	"ahb1",
+		      0x060, BIT(26), 0);
+static SUNXI_CCU_GATE(ahb1_ehci1_clk,	"ahb1-ehci1",	"ahb1",
+		      0x060, BIT(27), 0);
+static SUNXI_CCU_GATE(ahb1_ohci0_clk,	"ahb1-ohci0",	"ahb1",
+		      0x060, BIT(29), 0);
+static SUNXI_CCU_GATE(ahb1_ohci1_clk,	"ahb1-ohci1",	"ahb1",
+		      0x060, BIT(30), 0);
+static SUNXI_CCU_GATE(ahb1_ohci2_clk,	"ahb1-ohci2",	"ahb1",
+		      0x060, BIT(31), 0);
+
+static SUNXI_CCU_GATE(ahb1_ve_clk,	"ahb1-ve",	"ahb1",
+		      0x064, BIT(0), 0);
+static SUNXI_CCU_GATE(ahb1_lcd0_clk,	"ahb1-lcd0",	"ahb1",
+		      0x064, BIT(4), 0);
+static SUNXI_CCU_GATE(ahb1_lcd1_clk,	"ahb1-lcd1",	"ahb1",
+		      0x064, BIT(5), 0);
+static SUNXI_CCU_GATE(ahb1_csi_clk,	"ahb1-csi",	"ahb1",
+		      0x064, BIT(8), 0);
+static SUNXI_CCU_GATE(ahb1_hdmi_clk,	"ahb1-hdmi",	"ahb1",
+		      0x064, BIT(11), 0);
+static SUNXI_CCU_GATE(ahb1_be0_clk,	"ahb1-be0",	"ahb1",
+		      0x064, BIT(12), 0);
+static SUNXI_CCU_GATE(ahb1_be1_clk,	"ahb1-be1",	"ahb1",
+		      0x064, BIT(13), 0);
+static SUNXI_CCU_GATE(ahb1_fe0_clk,	"ahb1-fe0",	"ahb1",
+		      0x064, BIT(14), 0);
+static SUNXI_CCU_GATE(ahb1_fe1_clk,	"ahb1-fe1",	"ahb1",
+		      0x064, BIT(15), 0);
+static SUNXI_CCU_GATE(ahb1_mp_clk,	"ahb1-mp",	"ahb1",
+		      0x064, BIT(18), 0);
+static SUNXI_CCU_GATE(ahb1_gpu_clk,	"ahb1-gpu",	"ahb1",
+		      0x064, BIT(20), 0);
+static SUNXI_CCU_GATE(ahb1_deu0_clk,	"ahb1-deu0",	"ahb1",
+		      0x064, BIT(23), 0);
+static SUNXI_CCU_GATE(ahb1_deu1_clk,	"ahb1-deu1",	"ahb1",
+		      0x064, BIT(24), 0);
+static SUNXI_CCU_GATE(ahb1_drc0_clk,	"ahb1-drc0",	"ahb1",
+		      0x064, BIT(25), 0);
+static SUNXI_CCU_GATE(ahb1_drc1_clk,	"ahb1-drc1",	"ahb1",
+		      0x064, BIT(26), 0);
+
+static SUNXI_CCU_GATE(apb1_codec_clk,	"apb1-codec",	"apb1",
+		      0x068, BIT(0), 0);
+static SUNXI_CCU_GATE(apb1_spdif_clk,	"apb1-spdif",	"apb1",
+		      0x068, BIT(1), 0);
+static SUNXI_CCU_GATE(apb1_digital_mic_clk,	"apb1-digital-mic",	"apb1",
+		      0x068, BIT(4), 0);
+static SUNXI_CCU_GATE(apb1_pio_clk,	"apb1-pio",	"apb1",
+		      0x068, BIT(5), 0);
+static SUNXI_CCU_GATE(apb1_daudio0_clk,	"apb1-daudio0",	"apb1",
+		      0x068, BIT(12), 0);
+static SUNXI_CCU_GATE(apb1_daudio1_clk,	"apb1-daudio1",	"apb1",
+		      0x068, BIT(13), 0);
+
+static SUNXI_CCU_GATE(apb2_i2c0_clk,	"apb2-i2c0",	"apb2",
+		      0x06c, BIT(0), 0);
+static SUNXI_CCU_GATE(apb2_i2c1_clk,	"apb2-i2c1",	"apb2",
+		      0x06c, BIT(1), 0);
+static SUNXI_CCU_GATE(apb2_i2c2_clk,	"apb2-i2c2",	"apb2",
+		      0x06c, BIT(2), 0);
+static SUNXI_CCU_GATE(apb2_i2c3_clk,	"apb2-i2c3",	"apb2",
+		      0x06c, BIT(3), 0);
+static SUNXI_CCU_GATE(apb2_uart0_clk,	"apb2-uart0",	"apb2",
+		      0x06c, BIT(16), 0);
+static SUNXI_CCU_GATE(apb2_uart1_clk,	"apb2-uart1",	"apb2",
+		      0x06c, BIT(17), 0);
+static SUNXI_CCU_GATE(apb2_uart2_clk,	"apb2-uart2",	"apb2",
+		      0x06c, BIT(18), 0);
+static SUNXI_CCU_GATE(apb2_uart3_clk,	"apb2-uart3",	"apb2",
+		      0x06c, BIT(19), 0);
+static SUNXI_CCU_GATE(apb2_uart4_clk,	"apb2-uart4",	"apb2",
+		      0x06c, BIT(20), 0);
+static SUNXI_CCU_GATE(apb2_uart5_clk,	"apb2-uart5",	"apb2",
+		      0x06c, BIT(21), 0);
+
+static const char * const mod0_default_parents[] = { "osc24M", "pll-periph" };
+static SUNXI_CCU_MP_WITH_MUX_GATE(nand0_clk, "nand0", mod0_default_parents,
+				  0x080,
+				  0, 4,		/* M */
+				  16, 2,	/* P */
+				  24, 2,	/* mux */
+				  BIT(31),	/* gate */
+				  0);
+
+static SUNXI_CCU_MP_WITH_MUX_GATE(nand1_clk, "nand1", mod0_default_parents,
+				  0x084,
+				  0, 4,		/* M */
+				  16, 2,	/* P */
+				  24, 2,	/* mux */
+				  BIT(31),	/* gate */
+				  0);
+
+static SUNXI_CCU_MP_WITH_MUX_GATE(mmc0_clk, "mmc0", mod0_default_parents,
+				  0x088,
+				  0, 4,		/* M */
+				  16, 2,	/* P */
+				  24, 2,	/* mux */
+				  BIT(31),	/* gate */
+				  0);
+
+static SUNXI_CCU_PHASE(mmc0_sample_clk, "mmc0_sample", "mmc0",
+		       0x088, 20, 3, 0);
+static SUNXI_CCU_PHASE(mmc0_output_clk, "mmc0_output", "mmc0",
+		       0x088, 8, 3, 0);
+
+static SUNXI_CCU_MP_WITH_MUX_GATE(mmc1_clk, "mmc1", mod0_default_parents,
+				  0x08c,
+				  0, 4,		/* M */
+				  16, 2,	/* P */
+				  24, 2,	/* mux */
+				  BIT(31),	/* gate */
+				  0);
+
+static SUNXI_CCU_PHASE(mmc1_sample_clk, "mmc1_sample", "mmc1",
+		       0x08c, 20, 3, 0);
+static SUNXI_CCU_PHASE(mmc1_output_clk, "mmc1_output", "mmc1",
+		       0x08c, 8, 3, 0);
+
+static SUNXI_CCU_MP_WITH_MUX_GATE(mmc2_clk, "mmc2", mod0_default_parents,
+				  0x090,
+				  0, 4,		/* M */
+				  16, 2,	/* P */
+				  24, 2,	/* mux */
+				  BIT(31),	/* gate */
+				  0);
+
+static SUNXI_CCU_PHASE(mmc2_sample_clk, "mmc2_sample", "mmc2",
+		       0x090, 20, 3, 0);
+static SUNXI_CCU_PHASE(mmc2_output_clk, "mmc2_output", "mmc2",
+		       0x090, 8, 3, 0);
+
+static SUNXI_CCU_MP_WITH_MUX_GATE(mmc3_clk, "mmc3", mod0_default_parents,
+				  0x094,
+				  0, 4,		/* M */
+				  16, 2,	/* P */
+				  24, 2,	/* mux */
+				  BIT(31),	/* gate */
+				  0);
+
+static SUNXI_CCU_PHASE(mmc3_sample_clk, "mmc3_sample", "mmc3",
+		       0x094, 20, 3, 0);
+static SUNXI_CCU_PHASE(mmc3_output_clk, "mmc3_output", "mmc3",
+		       0x094, 8, 3, 0);
+
+static SUNXI_CCU_MP_WITH_MUX_GATE(ts_clk, "ts", mod0_default_parents, 0x098,
+				  0, 4,		/* M */
+				  16, 2,	/* P */
+				  24, 2,	/* mux */
+				  BIT(31),	/* gate */
+				  0);
+
+static SUNXI_CCU_MP_WITH_MUX_GATE(ss_clk, "ss", mod0_default_parents, 0x09c,
+				  0, 4,		/* M */
+				  16, 2,	/* P */
+				  24, 2,	/* mux */
+				  BIT(31),	/* gate */
+				  0);
+
+static SUNXI_CCU_MP_WITH_MUX_GATE(spi0_clk, "spi0", mod0_default_parents, 0x0a0,
+				  0, 4,		/* M */
+				  16, 2,	/* P */
+				  24, 2,	/* mux */
+				  BIT(31),	/* gate */
+				  0);
+
+static SUNXI_CCU_MP_WITH_MUX_GATE(spi1_clk, "spi1", mod0_default_parents, 0x0a4,
+				  0, 4,		/* M */
+				  16, 2,	/* P */
+				  24, 2,	/* mux */
+				  BIT(31),	/* gate */
+				  0);
+static SUNXI_CCU_MP_WITH_MUX_GATE(spi2_clk, "spi2", mod0_default_parents, 0x0a8,
+				  0, 4,		/* M */
+				  16, 2,	/* P */
+				  24, 2,	/* mux */
+				  BIT(31),	/* gate */
+				  0);
+
+static SUNXI_CCU_MP_WITH_MUX_GATE(spi3_clk, "spi3", mod0_default_parents, 0x0ac,
+				  0, 4,		/* M */
+				  16, 2,	/* P */
+				  24, 2,	/* mux */
+				  BIT(31),	/* gate */
+				  0);
+
+static const char * const daudio_parents[] = { "pll-audio-8x", "pll-audio-4x",
+					       "pll-audio-2x", "pll-audio" };
+static SUNXI_CCU_MUX_WITH_GATE(daudio0_clk, "daudio0", daudio_parents,
+			       0x0b0, 16, 2, BIT(31), CLK_SET_RATE_PARENT);
+static SUNXI_CCU_MUX_WITH_GATE(daudio1_clk, "daudio1", daudio_parents,
+			       0x0b4, 16, 2, BIT(31), CLK_SET_RATE_PARENT);
+
+static SUNXI_CCU_M_WITH_GATE(spdif_clk, "spdif", "pll-audio",
+			     0x0c0, 0, 4, BIT(31), CLK_SET_RATE_PARENT);
+
+static SUNXI_CCU_GATE(usb_phy0_clk,	"usb-phy0",	"osc24M",
+		      0x0cc, BIT(8), 0);
+static SUNXI_CCU_GATE(usb_phy1_clk,	"usb-phy1",	"osc24M",
+		      0x0cc, BIT(9), 0);
+static SUNXI_CCU_GATE(usb_phy2_clk,	"usb-phy2",	"osc24M",
+		      0x0cc, BIT(10), 0);
+static SUNXI_CCU_GATE(usb_ohci0_clk,	"usb-ohci0",	"osc24M",
+		      0x0cc, BIT(16), 0);
+static SUNXI_CCU_GATE(usb_ohci1_clk,	"usb-ohci1",	"osc24M",
+		      0x0cc, BIT(17), 0);
+static SUNXI_CCU_GATE(usb_ohci2_clk,	"usb-ohci2",	"osc24M",
+		      0x0cc, BIT(18), 0);
+
+/* TODO emac clk not supported yet */
+
+static const char * const dram_parents[] = { "pll-ddr", "pll-periph" };
+static SUNXI_CCU_MP_WITH_MUX_GATE(mdfs_clk, "mdfs", dram_parents, 0x0f0,
+				  0, 4,		/* M */
+				  16, 2,	/* P */
+				  24, 2,	/* mux */
+				  BIT(31),	/* gate */
+				  CLK_IS_CRITICAL);
+
+static SUNXI_CCU_M_WITH_MUX(sdram0_clk, "sdram0", dram_parents,
+			    0x0f4, 0, 4, 4, 1, CLK_IS_CRITICAL);
+static SUNXI_CCU_M_WITH_MUX(sdram1_clk, "sdram1", dram_parents,
+			    0x0f4, 8, 4, 12, 1, CLK_IS_CRITICAL);
+
+static SUNXI_CCU_GATE(dram_ve_clk,	"dram-ve",	"mdfs",
+		      0x100, BIT(0), 0);
+static SUNXI_CCU_GATE(dram_csi_isp_clk,	"dram-csi-isp",	"mdfs",
+		      0x100, BIT(1), 0);
+static SUNXI_CCU_GATE(dram_ts_clk,	"dram-ts",	"mdfs",
+		      0x100, BIT(3), 0);
+static SUNXI_CCU_GATE(dram_drc0_clk,	"dram-drc0",	"mdfs",
+		      0x100, BIT(16), 0);
+static SUNXI_CCU_GATE(dram_drc1_clk,	"dram-drc1",	"mdfs",
+		      0x100, BIT(17), 0);
+static SUNXI_CCU_GATE(dram_deu0_clk,	"dram-deu0",	"mdfs",
+		      0x100, BIT(18), 0);
+static SUNXI_CCU_GATE(dram_deu1_clk,	"dram-deu1",	"mdfs",
+		      0x100, BIT(19), 0);
+static SUNXI_CCU_GATE(dram_fe0_clk,	"dram-fe0",	"mdfs",
+		      0x100, BIT(24), 0);
+static SUNXI_CCU_GATE(dram_fe1_clk,	"dram-fe1",	"mdfs",
+		      0x100, BIT(25), 0);
+static SUNXI_CCU_GATE(dram_be0_clk,	"dram-be0",	"mdfs",
+		      0x100, BIT(26), 0);
+static SUNXI_CCU_GATE(dram_be1_clk,	"dram-be1",	"mdfs",
+		      0x100, BIT(27), 0);
+static SUNXI_CCU_GATE(dram_mp_clk,	"dram-mp",	"mdfs",
+		      0x100, BIT(28), 0);
+
+static const char * const de_parents[] = { "pll-video0", "pll-video1",
+					   "pll-periph-2x", "pll-gpu",
+					   "pll9", "pll10" };
+static SUNXI_CCU_M_WITH_MUX_GATE(be0_clk, "be0", de_parents,
+				 0x104, 0, 4, 24, 3, BIT(31), 0);
+static SUNXI_CCU_M_WITH_MUX_GATE(be1_clk, "be1", de_parents,
+				 0x108, 0, 4, 24, 3, BIT(31), 0);
+static SUNXI_CCU_M_WITH_MUX_GATE(fe0_clk, "fe0", de_parents,
+				 0x10c, 0, 4, 24, 3, BIT(31), 0);
+static SUNXI_CCU_M_WITH_MUX_GATE(fe1_clk, "fe1", de_parents,
+				 0x110, 0, 4, 24, 3, BIT(31), 0);
+
+static const char * const mp_parents[] = { "pll-video0", "pll-video1",
+					   "pll9", "pll10" };
+static SUNXI_CCU_M_WITH_MUX_GATE(mp_clk, "mp", mp_parents,
+				 0x114, 0, 4, 24, 3, BIT(31), 0);
+
+static const char * const lcd_ch0_parents[] = { "pll-video0", "pll-video1",
+						"pll-video0-2x",
+						"pll-video1-2x", "pll-mipi" };
+static SUNXI_CCU_MUX_WITH_GATE(lcd0_ch0_clk, "lcd0-ch0", lcd_ch0_parents,
+			       0x118, 24, 2, BIT(31), CLK_SET_RATE_PARENT);
+static SUNXI_CCU_MUX_WITH_GATE(lcd1_ch0_clk, "lcd1-ch0", lcd_ch0_parents,
+			       0x11c, 24, 2, BIT(31), CLK_SET_RATE_PARENT);
+
+static const char * const lcd_ch1_parents[] = { "pll-video0", "pll-video1",
+						"pll-video0-2x",
+						"pll-video1-2x" };
+static SUNXI_CCU_M_WITH_MUX_GATE(lcd0_ch1_clk, "lcd0-ch1", lcd_ch1_parents,
+				 0x12c, 0, 4, 24, 3, BIT(31),
+				 CLK_SET_RATE_PARENT);
+static SUNXI_CCU_M_WITH_MUX_GATE(lcd1_ch1_clk, "lcd1-ch1", lcd_ch1_parents,
+				 0x12c, 0, 4, 24, 3, BIT(31),
+				 CLK_SET_RATE_PARENT);
+
+static const char * const csi_sclk_parents[] = { "pll-video0", "pll-video1",
+						 "pll9", "pll10", "pll-mipi",
+						 "pll-ve" };
+static SUNXI_CCU_M_WITH_MUX_GATE(csi0_sclk_clk, "csi0-sclk", csi_sclk_parents,
+				 0x134, 16, 4, 24, 3, BIT(31), 0);
+
+static const char * const csi_mclk_parents[] = { "pll-video0", "pll-video1",
+						 "osc24M" };
+static const u8 csi_mclk_table[] = { 0, 1, 5 };
+static struct ccu_div csi0_mclk_clk = {
+	.enable		= BIT(15),
+	.div		= _SUNXI_CCU_DIV(0, 4),
+	.mux		= _SUNXI_CCU_MUX_TABLE(8, 3, csi_mclk_table),
+	.common		= {
+		.reg		= 0x134,
+		.hw.init	= CLK_HW_INIT_PARENTS("csi0-mclk",
+						      csi_mclk_parents,
+						      &ccu_div_ops,
+						      0),
+	},
+};
+
+static struct ccu_div csi1_mclk_clk = {
+	.enable		= BIT(15),
+	.div		= _SUNXI_CCU_DIV(0, 4),
+	.mux		= _SUNXI_CCU_MUX_TABLE(8, 3, csi_mclk_table),
+	.common		= {
+		.reg		= 0x138,
+		.hw.init	= CLK_HW_INIT_PARENTS("csi1-mclk",
+						      csi_mclk_parents,
+						      &ccu_div_ops,
+						      0),
+	},
+};
+
+static SUNXI_CCU_M_WITH_GATE(ve_clk, "ve", "pll-ve",
+			     0x13c, 16, 3, BIT(31), 0);
+
+static SUNXI_CCU_GATE(codec_clk,	"codec",	"pll-audio",
+		      0x140, BIT(31), CLK_SET_RATE_PARENT);
+static SUNXI_CCU_GATE(avs_clk,		"avs",		"osc24M",
+		      0x144, BIT(31), 0);
+static SUNXI_CCU_GATE(digital_mic_clk,	"digital-mic",	"pll-audio",
+		      0x148, BIT(31), CLK_SET_RATE_PARENT);
+
+static SUNXI_CCU_M_WITH_MUX_GATE(hdmi_clk, "hdmi", lcd_ch1_parents,
+				 0x150, 0, 4, 24, 2, BIT(31),
+				 CLK_SET_RATE_PARENT);
+
+static SUNXI_CCU_GATE(hdmi_ddc_clk, "hdmi-ddc", "osc24M", 0x150, BIT(31), 0);
+
+static SUNXI_CCU_GATE(ps_clk, "ps", "lcd1-ch1", 0x140, BIT(31), 0);
+
+static const char * const mbus_parents[] = { "osc24M", "pll-periph",
+					     "pll-ddr" };
+static SUNXI_CCU_MP_WITH_MUX_GATE(mbus0_clk, "mbus0", mbus_parents, 0x15c,
+				  0, 3,		/* M */
+				  16, 2,	/* P */
+				  24, 2,	/* mux */
+				  BIT(31),	/* gate */
+				  CLK_IS_CRITICAL);
+
+static SUNXI_CCU_MP_WITH_MUX_GATE(mbus1_clk, "mbus1", mbus_parents, 0x160,
+				  0, 3,		/* M */
+				  16, 2,	/* P */
+				  24, 2,	/* mux */
+				  BIT(31),	/* gate */
+				  CLK_IS_CRITICAL);
+
+static SUNXI_CCU_M_WITH_MUX_GATE(mipi_dsi_clk, "mipi-dsi", lcd_ch1_parents,
+				 0x168, 16, 3, 24, 2, BIT(31),
+				 CLK_SET_RATE_PARENT);
+static SUNXI_CCU_M_WITH_MUX_GATE(mipi_dsi_dphy_clk, "mipi-dsi-dphy",
+				 lcd_ch1_parents, 0x168, 0, 3, 8, 2,
+				 BIT(15), CLK_SET_RATE_PARENT);
+static SUNXI_CCU_M_WITH_MUX_GATE(mipi_csi_dphy_clk, "mipi-csi-dphy",
+				 lcd_ch1_parents, 0x16c, 0, 3, 8, 2,
+				 BIT(15), 0);
+
+static SUNXI_CCU_M_WITH_MUX_GATE(iep_drc0_clk, "iep-drc0", de_parents,
+				 0x180, 0, 3, 24, 2, BIT(31), 0);
+static SUNXI_CCU_M_WITH_MUX_GATE(iep_drc1_clk, "iep-drc1", de_parents,
+				 0x184, 0, 3, 24, 2, BIT(31), 0);
+static SUNXI_CCU_M_WITH_MUX_GATE(iep_deu0_clk, "iep-deu0", de_parents,
+				 0x188, 0, 3, 24, 2, BIT(31), 0);
+static SUNXI_CCU_M_WITH_MUX_GATE(iep_deu1_clk, "iep-deu1", de_parents,
+				 0x18c, 0, 3, 24, 2, BIT(31), 0);
+
+static const char * const gpu_parents[] = { "pll-gpu", "pll-periph-2x",
+					    "pll-video0", "pll-video1",
+					    "pll9", "pll10" };
+static const struct ccu_mux_fixed_prediv gpu_predivs[] = {
+	{ .index = 1, .div = 3, },
+};
+
+static struct ccu_div gpu_core_clk = {
+	.enable		= BIT(31),
+	.div		= _SUNXI_CCU_DIV(0, 3),
+	.mux		= {
+		.shift		= 24,
+		.width		= 3,
+		.fixed_predivs	= gpu_predivs,
+		.n_predivs	= ARRAY_SIZE(gpu_predivs),
+	},
+	.common		= {
+		.reg		= 0x1a0,
+		.features	= CCU_FEATURE_FIXED_PREDIV,
+		.hw.init	= CLK_HW_INIT_PARENTS("gpu-core",
+						      gpu_parents,
+						      &ccu_div_ops,
+						      0),
+	},
+};
+
+static struct ccu_div gpu_memory_clk = {
+	.enable		= BIT(31),
+	.div		= _SUNXI_CCU_DIV(0, 3),
+	.mux		= {
+		.shift		= 24,
+		.width		= 3,
+		.fixed_predivs	= gpu_predivs,
+		.n_predivs	= ARRAY_SIZE(gpu_predivs),
+	},
+	.common		= {
+		.reg		= 0x1a4,
+		.features	= CCU_FEATURE_FIXED_PREDIV,
+		.hw.init	= CLK_HW_INIT_PARENTS("gpu-memory",
+						      gpu_parents,
+						      &ccu_div_ops,
+						      0),
+	},
+};
+
+static struct ccu_div gpu_hyd_clk = {
+	.enable		= BIT(31),
+	.div		= _SUNXI_CCU_DIV(0, 3),
+	.mux		= {
+		.shift		= 24,
+		.width		= 3,
+		.fixed_predivs	= gpu_predivs,
+		.n_predivs	= ARRAY_SIZE(gpu_predivs),
+	},
+	.common		= {
+		.reg		= 0x1a8,
+		.features	= CCU_FEATURE_FIXED_PREDIV,
+		.hw.init	= CLK_HW_INIT_PARENTS("gpu-hyd",
+						      gpu_parents,
+						      &ccu_div_ops,
+						      0),
+	},
+};
+
+static SUNXI_CCU_M_WITH_MUX_GATE(ats_clk, "ats", mod0_default_parents, 0x1b0,
+				 0, 3,		/* M */
+				 24, 2,		/* mux */
+				 BIT(31),	/* gate */
+				 0);
+
+static SUNXI_CCU_M_WITH_MUX_GATE(trace_clk, "trace", mod0_default_parents,
+				 0x1b0,
+				 0, 3,		/* M */
+				 24, 2,		/* mux */
+				 BIT(31),	/* gate */
+				 0);
+
+static const char * const clk_out_parents[] = { "osc24M", "osc32k", "osc24M",
+						"axi", "ahb1" };
+static const u8 clk_out_table[] = { 0, 1, 2, 11, 13 };
+
+static const struct ccu_mux_fixed_prediv clk_out_predivs[] = {
+	{ .index = 0, .div = 750, },
+	{ .index = 3, .div = 4, },
+	{ .index = 4, .div = 4, },
+};
+
+static struct ccu_mp out_a_clk = {
+	.enable		= BIT(31),
+	.m		= _SUNXI_CCU_DIV(8, 5),
+	.p		= _SUNXI_CCU_DIV(20, 2),
+	.mux		= {
+		.shift		= 24,
+		.width		= 4,
+		.table		= clk_out_table,
+		.fixed_predivs	= clk_out_predivs,
+		.n_predivs	= ARRAY_SIZE(clk_out_predivs),
+	},
+	.common		= {
+		.reg		= 0x300,
+		.features	= CCU_FEATURE_FIXED_PREDIV,
+		.hw.init	= CLK_HW_INIT_PARENTS("out-a",
+						      clk_out_parents,
+						      &ccu_div_ops,
+						      0),
+	},
+};
+
+static struct ccu_mp out_b_clk = {
+	.enable		= BIT(31),
+	.m		= _SUNXI_CCU_DIV(8, 5),
+	.p		= _SUNXI_CCU_DIV(20, 2),
+	.mux		= {
+		.shift		= 24,
+		.width		= 4,
+		.table		= clk_out_table,
+		.fixed_predivs	= clk_out_predivs,
+		.n_predivs	= ARRAY_SIZE(clk_out_predivs),
+	},
+	.common		= {
+		.reg		= 0x304,
+		.features	= CCU_FEATURE_FIXED_PREDIV,
+		.hw.init	= CLK_HW_INIT_PARENTS("out-b",
+						      clk_out_parents,
+						      &ccu_div_ops,
+						      0),
+	},
+};
+
+static struct ccu_mp out_c_clk = {
+	.enable		= BIT(31),
+	.m		= _SUNXI_CCU_DIV(8, 5),
+	.p		= _SUNXI_CCU_DIV(20, 2),
+	.mux		= {
+		.shift		= 24,
+		.width		= 4,
+		.table		= clk_out_table,
+		.fixed_predivs	= clk_out_predivs,
+		.n_predivs	= ARRAY_SIZE(clk_out_predivs),
+	},
+	.common		= {
+		.reg		= 0x308,
+		.features	= CCU_FEATURE_FIXED_PREDIV,
+		.hw.init	= CLK_HW_INIT_PARENTS("out-c",
+						      clk_out_parents,
+						      &ccu_div_ops,
+						      0),
+	},
+};
+
+static struct ccu_common *sun6i_a31_ccu_clks[] = {
+	&pll_cpu_clk.common,
+	&pll_audio_base_clk.common,
+	&pll_video0_clk.common,
+	&pll_ve_clk.common,
+	&pll_ddr_clk.common,
+	&pll_periph_clk.common,
+	&pll_video1_clk.common,
+	&pll_gpu_clk.common,
+	&pll_mipi_clk.common,
+	&pll9_clk.common,
+	&pll10_clk.common,
+	&cpu_clk.common,
+	&axi_clk.common,
+	&ahb1_clk.common,
+	&apb1_clk.common,
+	&apb2_clk.common,
+	&ahb1_mipidsi_clk.common,
+	&ahb1_ss_clk.common,
+	&ahb1_dma_clk.common,
+	&ahb1_mmc0_clk.common,
+	&ahb1_mmc1_clk.common,
+	&ahb1_mmc2_clk.common,
+	&ahb1_mmc3_clk.common,
+	&ahb1_nand1_clk.common,
+	&ahb1_nand0_clk.common,
+	&ahb1_sdram_clk.common,
+	&ahb1_emac_clk.common,
+	&ahb1_ts_clk.common,
+	&ahb1_hstimer_clk.common,
+	&ahb1_spi0_clk.common,
+	&ahb1_spi1_clk.common,
+	&ahb1_spi2_clk.common,
+	&ahb1_spi3_clk.common,
+	&ahb1_otg_clk.common,
+	&ahb1_ehci0_clk.common,
+	&ahb1_ehci1_clk.common,
+	&ahb1_ohci0_clk.common,
+	&ahb1_ohci1_clk.common,
+	&ahb1_ohci2_clk.common,
+	&ahb1_ve_clk.common,
+	&ahb1_lcd0_clk.common,
+	&ahb1_lcd1_clk.common,
+	&ahb1_csi_clk.common,
+	&ahb1_hdmi_clk.common,
+	&ahb1_be0_clk.common,
+	&ahb1_be1_clk.common,
+	&ahb1_fe0_clk.common,
+	&ahb1_fe1_clk.common,
+	&ahb1_mp_clk.common,
+	&ahb1_gpu_clk.common,
+	&ahb1_deu0_clk.common,
+	&ahb1_deu1_clk.common,
+	&ahb1_drc0_clk.common,
+	&ahb1_drc1_clk.common,
+	&apb1_codec_clk.common,
+	&apb1_spdif_clk.common,
+	&apb1_digital_mic_clk.common,
+	&apb1_pio_clk.common,
+	&apb1_daudio0_clk.common,
+	&apb1_daudio1_clk.common,
+	&apb2_i2c0_clk.common,
+	&apb2_i2c1_clk.common,
+	&apb2_i2c2_clk.common,
+	&apb2_i2c3_clk.common,
+	&apb2_uart0_clk.common,
+	&apb2_uart1_clk.common,
+	&apb2_uart2_clk.common,
+	&apb2_uart3_clk.common,
+	&apb2_uart4_clk.common,
+	&apb2_uart5_clk.common,
+	&nand0_clk.common,
+	&nand1_clk.common,
+	&mmc0_clk.common,
+	&mmc0_sample_clk.common,
+	&mmc0_output_clk.common,
+	&mmc1_clk.common,
+	&mmc1_sample_clk.common,
+	&mmc1_output_clk.common,
+	&mmc2_clk.common,
+	&mmc2_sample_clk.common,
+	&mmc2_output_clk.common,
+	&mmc3_clk.common,
+	&mmc3_sample_clk.common,
+	&mmc3_output_clk.common,
+	&ts_clk.common,
+	&ss_clk.common,
+	&spi0_clk.common,
+	&spi1_clk.common,
+	&spi2_clk.common,
+	&spi3_clk.common,
+	&daudio0_clk.common,
+	&daudio1_clk.common,
+	&spdif_clk.common,
+	&usb_phy0_clk.common,
+	&usb_phy1_clk.common,
+	&usb_phy2_clk.common,
+	&usb_ohci0_clk.common,
+	&usb_ohci1_clk.common,
+	&usb_ohci2_clk.common,
+	&mdfs_clk.common,
+	&sdram0_clk.common,
+	&sdram1_clk.common,
+	&dram_ve_clk.common,
+	&dram_csi_isp_clk.common,
+	&dram_ts_clk.common,
+	&dram_drc0_clk.common,
+	&dram_drc1_clk.common,
+	&dram_deu0_clk.common,
+	&dram_deu1_clk.common,
+	&dram_fe0_clk.common,
+	&dram_fe1_clk.common,
+	&dram_be0_clk.common,
+	&dram_be1_clk.common,
+	&dram_mp_clk.common,
+	&be0_clk.common,
+	&be1_clk.common,
+	&fe0_clk.common,
+	&fe1_clk.common,
+	&mp_clk.common,
+	&lcd0_ch0_clk.common,
+	&lcd1_ch0_clk.common,
+	&lcd0_ch1_clk.common,
+	&lcd1_ch1_clk.common,
+	&csi0_sclk_clk.common,
+	&csi0_mclk_clk.common,
+	&csi1_mclk_clk.common,
+	&ve_clk.common,
+	&codec_clk.common,
+	&avs_clk.common,
+	&digital_mic_clk.common,
+	&hdmi_clk.common,
+	&hdmi_ddc_clk.common,
+	&ps_clk.common,
+	&mbus0_clk.common,
+	&mbus1_clk.common,
+	&mipi_dsi_clk.common,
+	&mipi_dsi_dphy_clk.common,
+	&mipi_csi_dphy_clk.common,
+	&iep_drc0_clk.common,
+	&iep_drc1_clk.common,
+	&iep_deu0_clk.common,
+	&iep_deu1_clk.common,
+	&gpu_core_clk.common,
+	&gpu_memory_clk.common,
+	&gpu_hyd_clk.common,
+	&ats_clk.common,
+	&trace_clk.common,
+	&out_a_clk.common,
+	&out_b_clk.common,
+	&out_c_clk.common,
+};
+
+/* We hardcode the divider to 4 for now */
+static CLK_FIXED_FACTOR(pll_audio_clk, "pll-audio",
+			"pll-audio-base", 4, 1, CLK_SET_RATE_PARENT);
+static CLK_FIXED_FACTOR(pll_audio_2x_clk, "pll-audio-2x",
+			"pll-audio-base", 2, 1, CLK_SET_RATE_PARENT);
+static CLK_FIXED_FACTOR(pll_audio_4x_clk, "pll-audio-4x",
+			"pll-audio-base", 1, 1, CLK_SET_RATE_PARENT);
+static CLK_FIXED_FACTOR(pll_audio_8x_clk, "pll-audio-8x",
+			"pll-audio-base", 1, 2, CLK_SET_RATE_PARENT);
+static CLK_FIXED_FACTOR(pll_periph_2x_clk, "pll-periph-2x",
+			"pll-periph", 1, 2, 0);
+static CLK_FIXED_FACTOR(pll_video0_2x_clk, "pll-video0-2x",
+			"pll-video0", 1, 2, CLK_SET_RATE_PARENT);
+static CLK_FIXED_FACTOR(pll_video1_2x_clk, "pll-video1-2x",
+			"pll-video1", 1, 2, CLK_SET_RATE_PARENT);
+
+static struct clk_hw_onecell_data sun6i_a31_hw_clks = {
+	.hws	= {
+		[CLK_PLL_CPU]		= &pll_cpu_clk.common.hw,
+		[CLK_PLL_AUDIO_BASE]	= &pll_audio_base_clk.common.hw,
+		[CLK_PLL_AUDIO]		= &pll_audio_clk.hw,
+		[CLK_PLL_AUDIO_2X]	= &pll_audio_2x_clk.hw,
+		[CLK_PLL_AUDIO_4X]	= &pll_audio_4x_clk.hw,
+		[CLK_PLL_AUDIO_8X]	= &pll_audio_8x_clk.hw,
+		[CLK_PLL_VIDEO0]	= &pll_video0_clk.common.hw,
+		[CLK_PLL_VIDEO0_2X]	= &pll_video0_2x_clk.hw,
+		[CLK_PLL_VE]		= &pll_ve_clk.common.hw,
+		[CLK_PLL_DDR]		= &pll_ddr_clk.common.hw,
+		[CLK_PLL_PERIPH]	= &pll_periph_clk.common.hw,
+		[CLK_PLL_PERIPH_2X]	= &pll_periph_2x_clk.hw,
+		[CLK_PLL_VIDEO1]	= &pll_video1_clk.common.hw,
+		[CLK_PLL_VIDEO1_2X]	= &pll_video1_2x_clk.hw,
+		[CLK_PLL_GPU]		= &pll_gpu_clk.common.hw,
+		[CLK_PLL_MIPI]		= &pll_mipi_clk.common.hw,
+		[CLK_PLL9]		= &pll9_clk.common.hw,
+		[CLK_PLL10]		= &pll10_clk.common.hw,
+		[CLK_CPU]		= &cpu_clk.common.hw,
+		[CLK_AXI]		= &axi_clk.common.hw,
+		[CLK_AHB1]		= &ahb1_clk.common.hw,
+		[CLK_APB1]		= &apb1_clk.common.hw,
+		[CLK_APB2]		= &apb2_clk.common.hw,
+		[CLK_AHB1_MIPIDSI]	= &ahb1_mipidsi_clk.common.hw,
+		[CLK_AHB1_SS]		= &ahb1_ss_clk.common.hw,
+		[CLK_AHB1_DMA]		= &ahb1_dma_clk.common.hw,
+		[CLK_AHB1_MMC0]		= &ahb1_mmc0_clk.common.hw,
+		[CLK_AHB1_MMC1]		= &ahb1_mmc1_clk.common.hw,
+		[CLK_AHB1_MMC2]		= &ahb1_mmc2_clk.common.hw,
+		[CLK_AHB1_MMC3]		= &ahb1_mmc3_clk.common.hw,
+		[CLK_AHB1_NAND1]	= &ahb1_nand1_clk.common.hw,
+		[CLK_AHB1_NAND0]	= &ahb1_nand0_clk.common.hw,
+		[CLK_AHB1_SDRAM]	= &ahb1_sdram_clk.common.hw,
+		[CLK_AHB1_EMAC]		= &ahb1_emac_clk.common.hw,
+		[CLK_AHB1_TS]		= &ahb1_ts_clk.common.hw,
+		[CLK_AHB1_HSTIMER]	= &ahb1_hstimer_clk.common.hw,
+		[CLK_AHB1_SPI0]		= &ahb1_spi0_clk.common.hw,
+		[CLK_AHB1_SPI1]		= &ahb1_spi1_clk.common.hw,
+		[CLK_AHB1_SPI2]		= &ahb1_spi2_clk.common.hw,
+		[CLK_AHB1_SPI3]		= &ahb1_spi3_clk.common.hw,
+		[CLK_AHB1_OTG]		= &ahb1_otg_clk.common.hw,
+		[CLK_AHB1_EHCI0]	= &ahb1_ehci0_clk.common.hw,
+		[CLK_AHB1_EHCI1]	= &ahb1_ehci1_clk.common.hw,
+		[CLK_AHB1_OHCI0]	= &ahb1_ohci0_clk.common.hw,
+		[CLK_AHB1_OHCI1]	= &ahb1_ohci1_clk.common.hw,
+		[CLK_AHB1_OHCI2]	= &ahb1_ohci2_clk.common.hw,
+		[CLK_AHB1_VE]		= &ahb1_ve_clk.common.hw,
+		[CLK_AHB1_LCD0]		= &ahb1_lcd0_clk.common.hw,
+		[CLK_AHB1_LCD1]		= &ahb1_lcd1_clk.common.hw,
+		[CLK_AHB1_CSI]		= &ahb1_csi_clk.common.hw,
+		[CLK_AHB1_HDMI]		= &ahb1_hdmi_clk.common.hw,
+		[CLK_AHB1_BE0]		= &ahb1_be0_clk.common.hw,
+		[CLK_AHB1_BE1]		= &ahb1_be1_clk.common.hw,
+		[CLK_AHB1_FE0]		= &ahb1_fe0_clk.common.hw,
+		[CLK_AHB1_FE1]		= &ahb1_fe1_clk.common.hw,
+		[CLK_AHB1_MP]		= &ahb1_mp_clk.common.hw,
+		[CLK_AHB1_GPU]		= &ahb1_gpu_clk.common.hw,
+		[CLK_AHB1_DEU0]		= &ahb1_deu0_clk.common.hw,
+		[CLK_AHB1_DEU1]		= &ahb1_deu1_clk.common.hw,
+		[CLK_AHB1_DRC0]		= &ahb1_drc0_clk.common.hw,
+		[CLK_AHB1_DRC1]		= &ahb1_drc1_clk.common.hw,
+		[CLK_APB1_CODEC]	= &apb1_codec_clk.common.hw,
+		[CLK_APB1_SPDIF]	= &apb1_spdif_clk.common.hw,
+		[CLK_APB1_DIGITAL_MIC]	= &apb1_digital_mic_clk.common.hw,
+		[CLK_APB1_PIO]		= &apb1_pio_clk.common.hw,
+		[CLK_APB1_DAUDIO0]	= &apb1_daudio0_clk.common.hw,
+		[CLK_APB1_DAUDIO1]	= &apb1_daudio1_clk.common.hw,
+		[CLK_APB2_I2C0]		= &apb2_i2c0_clk.common.hw,
+		[CLK_APB2_I2C1]		= &apb2_i2c1_clk.common.hw,
+		[CLK_APB2_I2C2]		= &apb2_i2c2_clk.common.hw,
+		[CLK_APB2_I2C3]		= &apb2_i2c3_clk.common.hw,
+		[CLK_APB2_UART0]	= &apb2_uart0_clk.common.hw,
+		[CLK_APB2_UART1]	= &apb2_uart1_clk.common.hw,
+		[CLK_APB2_UART2]	= &apb2_uart2_clk.common.hw,
+		[CLK_APB2_UART3]	= &apb2_uart3_clk.common.hw,
+		[CLK_APB2_UART4]	= &apb2_uart4_clk.common.hw,
+		[CLK_APB2_UART5]	= &apb2_uart5_clk.common.hw,
+		[CLK_NAND0]		= &nand0_clk.common.hw,
+		[CLK_NAND1]		= &nand1_clk.common.hw,
+		[CLK_MMC0]		= &mmc0_clk.common.hw,
+		[CLK_MMC0_SAMPLE]	= &mmc0_sample_clk.common.hw,
+		[CLK_MMC0_OUTPUT]	= &mmc0_output_clk.common.hw,
+		[CLK_MMC1]		= &mmc1_clk.common.hw,
+		[CLK_MMC1_SAMPLE]	= &mmc1_sample_clk.common.hw,
+		[CLK_MMC1_OUTPUT]	= &mmc1_output_clk.common.hw,
+		[CLK_MMC2]		= &mmc2_clk.common.hw,
+		[CLK_MMC2_SAMPLE]	= &mmc2_sample_clk.common.hw,
+		[CLK_MMC2_OUTPUT]	= &mmc2_output_clk.common.hw,
+		[CLK_MMC3]		= &mmc3_clk.common.hw,
+		[CLK_MMC3_SAMPLE]	= &mmc3_sample_clk.common.hw,
+		[CLK_MMC3_OUTPUT]	= &mmc3_output_clk.common.hw,
+		[CLK_TS]		= &ts_clk.common.hw,
+		[CLK_SS]		= &ss_clk.common.hw,
+		[CLK_SPI0]		= &spi0_clk.common.hw,
+		[CLK_SPI1]		= &spi1_clk.common.hw,
+		[CLK_SPI2]		= &spi2_clk.common.hw,
+		[CLK_SPI3]		= &spi3_clk.common.hw,
+		[CLK_DAUDIO0]		= &daudio0_clk.common.hw,
+		[CLK_DAUDIO1]		= &daudio1_clk.common.hw,
+		[CLK_SPDIF]		= &spdif_clk.common.hw,
+		[CLK_USB_PHY0]		= &usb_phy0_clk.common.hw,
+		[CLK_USB_PHY1]		= &usb_phy1_clk.common.hw,
+		[CLK_USB_PHY2]		= &usb_phy2_clk.common.hw,
+		[CLK_USB_OHCI0]		= &usb_ohci0_clk.common.hw,
+		[CLK_USB_OHCI1]		= &usb_ohci1_clk.common.hw,
+		[CLK_USB_OHCI2]		= &usb_ohci2_clk.common.hw,
+		[CLK_MDFS]		= &mdfs_clk.common.hw,
+		[CLK_SDRAM0]		= &sdram0_clk.common.hw,
+		[CLK_SDRAM1]		= &sdram1_clk.common.hw,
+		[CLK_DRAM_VE]		= &dram_ve_clk.common.hw,
+		[CLK_DRAM_CSI_ISP]	= &dram_csi_isp_clk.common.hw,
+		[CLK_DRAM_TS]		= &dram_ts_clk.common.hw,
+		[CLK_DRAM_DRC0]		= &dram_drc0_clk.common.hw,
+		[CLK_DRAM_DRC1]		= &dram_drc1_clk.common.hw,
+		[CLK_DRAM_DEU0]		= &dram_deu0_clk.common.hw,
+		[CLK_DRAM_DEU1]		= &dram_deu1_clk.common.hw,
+		[CLK_DRAM_FE0]		= &dram_fe0_clk.common.hw,
+		[CLK_DRAM_FE1]		= &dram_fe1_clk.common.hw,
+		[CLK_DRAM_BE0]		= &dram_be0_clk.common.hw,
+		[CLK_DRAM_BE1]		= &dram_be1_clk.common.hw,
+		[CLK_DRAM_MP]		= &dram_mp_clk.common.hw,
+		[CLK_BE0]		= &be0_clk.common.hw,
+		[CLK_BE1]		= &be1_clk.common.hw,
+		[CLK_FE0]		= &fe0_clk.common.hw,
+		[CLK_FE1]		= &fe1_clk.common.hw,
+		[CLK_MP]		= &mp_clk.common.hw,
+		[CLK_LCD0_CH0]		= &lcd0_ch0_clk.common.hw,
+		[CLK_LCD1_CH0]		= &lcd1_ch0_clk.common.hw,
+		[CLK_LCD0_CH1]		= &lcd0_ch1_clk.common.hw,
+		[CLK_LCD1_CH1]		= &lcd1_ch1_clk.common.hw,
+		[CLK_CSI0_SCLK]		= &csi0_sclk_clk.common.hw,
+		[CLK_CSI0_MCLK]		= &csi0_mclk_clk.common.hw,
+		[CLK_CSI1_MCLK]		= &csi1_mclk_clk.common.hw,
+		[CLK_VE]		= &ve_clk.common.hw,
+		[CLK_CODEC]		= &codec_clk.common.hw,
+		[CLK_AVS]		= &avs_clk.common.hw,
+		[CLK_DIGITAL_MIC]	= &digital_mic_clk.common.hw,
+		[CLK_HDMI]		= &hdmi_clk.common.hw,
+		[CLK_HDMI_DDC]		= &hdmi_ddc_clk.common.hw,
+		[CLK_PS]		= &ps_clk.common.hw,
+		[CLK_MBUS0]		= &mbus0_clk.common.hw,
+		[CLK_MBUS1]		= &mbus1_clk.common.hw,
+		[CLK_MIPI_DSI]		= &mipi_dsi_clk.common.hw,
+		[CLK_MIPI_DSI_DPHY]	= &mipi_dsi_dphy_clk.common.hw,
+		[CLK_MIPI_CSI_DPHY]	= &mipi_csi_dphy_clk.common.hw,
+		[CLK_IEP_DRC0]		= &iep_drc0_clk.common.hw,
+		[CLK_IEP_DRC1]		= &iep_drc1_clk.common.hw,
+		[CLK_IEP_DEU0]		= &iep_deu0_clk.common.hw,
+		[CLK_IEP_DEU1]		= &iep_deu1_clk.common.hw,
+		[CLK_GPU_CORE]		= &gpu_core_clk.common.hw,
+		[CLK_GPU_MEMORY]	= &gpu_memory_clk.common.hw,
+		[CLK_GPU_HYD]		= &gpu_hyd_clk.common.hw,
+		[CLK_ATS]		= &ats_clk.common.hw,
+		[CLK_TRACE]		= &trace_clk.common.hw,
+		[CLK_OUT_A]		= &out_a_clk.common.hw,
+		[CLK_OUT_B]		= &out_b_clk.common.hw,
+		[CLK_OUT_C]		= &out_c_clk.common.hw,
+	},
+	.num	= CLK_NUMBER,
+};
+
+static struct ccu_reset_map sun6i_a31_ccu_resets[] = {
+	[RST_USB_PHY0]		= { 0x0cc, BIT(0) },
+	[RST_USB_PHY1]		= { 0x0cc, BIT(1) },
+	[RST_USB_PHY2]		= { 0x0cc, BIT(2) },
+
+	[RST_AHB1_MIPI_DSI]	= { 0x2c0, BIT(1) },
+	[RST_AHB1_SS]		= { 0x2c0, BIT(5) },
+	[RST_AHB1_DMA]		= { 0x2c0, BIT(6) },
+	[RST_AHB1_MMC0]		= { 0x2c0, BIT(8) },
+	[RST_AHB1_MMC1]		= { 0x2c0, BIT(9) },
+	[RST_AHB1_MMC2]		= { 0x2c0, BIT(10) },
+	[RST_AHB1_MMC3]		= { 0x2c0, BIT(11) },
+	[RST_AHB1_NAND1]	= { 0x2c0, BIT(12) },
+	[RST_AHB1_NAND0]	= { 0x2c0, BIT(13) },
+	[RST_AHB1_SDRAM]	= { 0x2c0, BIT(14) },
+	[RST_AHB1_EMAC]		= { 0x2c0, BIT(17) },
+	[RST_AHB1_TS]		= { 0x2c0, BIT(18) },
+	[RST_AHB1_HSTIMER]	= { 0x2c0, BIT(19) },
+	[RST_AHB1_SPI0]		= { 0x2c0, BIT(20) },
+	[RST_AHB1_SPI1]		= { 0x2c0, BIT(21) },
+	[RST_AHB1_SPI2]		= { 0x2c0, BIT(22) },
+	[RST_AHB1_SPI3]		= { 0x2c0, BIT(23) },
+	[RST_AHB1_OTG]		= { 0x2c0, BIT(24) },
+	[RST_AHB1_EHCI0]	= { 0x2c0, BIT(26) },
+	[RST_AHB1_EHCI1]	= { 0x2c0, BIT(27) },
+	[RST_AHB1_OHCI0]	= { 0x2c0, BIT(29) },
+	[RST_AHB1_OHCI1]	= { 0x2c0, BIT(30) },
+	[RST_AHB1_OHCI2]	= { 0x2c0, BIT(31) },
+
+	[RST_AHB1_VE]		= { 0x2c4, BIT(0) },
+	[RST_AHB1_LCD0]		= { 0x2c4, BIT(4) },
+	[RST_AHB1_LCD1]		= { 0x2c4, BIT(5) },
+	[RST_AHB1_CSI]		= { 0x2c4, BIT(8) },
+	[RST_AHB1_HDMI]		= { 0x2c4, BIT(11) },
+	[RST_AHB1_BE0]		= { 0x2c4, BIT(12) },
+	[RST_AHB1_BE1]		= { 0x2c4, BIT(13) },
+	[RST_AHB1_FE0]		= { 0x2c4, BIT(14) },
+	[RST_AHB1_FE1]		= { 0x2c4, BIT(15) },
+	[RST_AHB1_MP]		= { 0x2c4, BIT(18) },
+	[RST_AHB1_GPU]		= { 0x2c4, BIT(20) },
+	[RST_AHB1_DEU0]		= { 0x2c4, BIT(23) },
+	[RST_AHB1_DEU1]		= { 0x2c4, BIT(24) },
+	[RST_AHB1_DRC0]		= { 0x2c4, BIT(25) },
+	[RST_AHB1_DRC1]		= { 0x2c4, BIT(26) },
+	[RST_AHB1_LVDS]		= { 0x2c8, BIT(0) },
+
+	[RST_APB1_CODEC]	= { 0x2d0, BIT(0) },
+	[RST_APB1_SPDIF]	= { 0x2d0, BIT(1) },
+	[RST_APB1_DIGITAL_MIC]	= { 0x2d0, BIT(4) },
+	[RST_APB1_DAUDIO0]	= { 0x2d0, BIT(12) },
+	[RST_APB1_DAUDIO1]	= { 0x2d0, BIT(13) },
+
+	[RST_APB2_I2C0]		= { 0x2d8, BIT(0) },
+	[RST_APB2_I2C1]		= { 0x2d8, BIT(1) },
+	[RST_APB2_I2C2]		= { 0x2d8, BIT(2) },
+	[RST_APB2_I2C3]		= { 0x2d8, BIT(3) },
+	[RST_APB2_UART0]	= { 0x2d8, BIT(16) },
+	[RST_APB2_UART1]	= { 0x2d8, BIT(17) },
+	[RST_APB2_UART2]	= { 0x2d8, BIT(18) },
+	[RST_APB2_UART3]	= { 0x2d8, BIT(19) },
+	[RST_APB2_UART4]	= { 0x2d8, BIT(20) },
+	[RST_APB2_UART5]	= { 0x2d8, BIT(21) },
+};
+
+static const struct sunxi_ccu_desc sun6i_a31_ccu_desc = {
+	.ccu_clks	= sun6i_a31_ccu_clks,
+	.num_ccu_clks	= ARRAY_SIZE(sun6i_a31_ccu_clks),
+
+	.hw_clks	= &sun6i_a31_hw_clks,
+
+	.resets		= sun6i_a31_ccu_resets,
+	.num_resets	= ARRAY_SIZE(sun6i_a31_ccu_resets),
+};
+
+static struct ccu_mux_nb sun6i_a31_cpu_nb = {
+	.common		= &cpu_clk.common,
+	.cm		= &cpu_clk.mux,
+	.delay_us	= 1, /* > 8 clock cycles at 24 MHz */
+	.bypass_index	= 1, /* index of 24 MHz oscillator */
+};
+
+static void __init sun6i_a31_ccu_setup(struct device_node *node)
+{
+	void __iomem *reg;
+	u32 val;
+
+	reg = of_io_request_and_map(node, 0, of_node_full_name(node));
+	if (IS_ERR(reg)) {
+		pr_err("%s: Could not map the clock registers\n",
+		       of_node_full_name(node));
+		return;
+	}
+
+	/* Force the PLL-Audio-1x divider to 4 */
+	val = readl(reg + SUN6I_A31_PLL_AUDIO_REG);
+	val &= ~GENMASK(19, 16);
+	writel(val | (3 << 16), reg + SUN6I_A31_PLL_AUDIO_REG);
+
+	/* Force PLL-MIPI to MIPI mode */
+	val = readl(reg + SUN6I_A31_PLL_MIPI_REG);
+	val &= BIT(16);
+	writel(val, reg + SUN6I_A31_PLL_MIPI_REG);
+
+	sunxi_ccu_probe(node, reg, &sun6i_a31_ccu_desc);
+
+	ccu_mux_notifier_register(pll_cpu_clk.common.hw.clk,
+				  &sun6i_a31_cpu_nb);
+}
+CLK_OF_DECLARE(sun6i_a31_ccu, "allwinner,sun6i-a31-ccu",
+	       sun6i_a31_ccu_setup);
diff --git a/drivers/clk/sunxi-ng/ccu-sun6i-a31.h b/drivers/clk/sunxi-ng/ccu-sun6i-a31.h
new file mode 100644
index 000000000000..4e434011e9e7
--- /dev/null
+++ b/drivers/clk/sunxi-ng/ccu-sun6i-a31.h
@@ -0,0 +1,72 @@
+/*
+ * Copyright 2016 Chen-Yu Tsai
+ *
+ * Chen-Yu Tsai <wens@csie.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef _CCU_SUN6I_A31_H_
+#define _CCU_SUN6I_A31_H_
+
+#include <dt-bindings/clock/sun6i-a31-ccu.h>
+#include <dt-bindings/reset/sun6i-a31-ccu.h>
+
+#define CLK_PLL_CPU		0
+#define CLK_PLL_AUDIO_BASE	1
+#define CLK_PLL_AUDIO		2
+#define CLK_PLL_AUDIO_2X	3
+#define CLK_PLL_AUDIO_4X	4
+#define CLK_PLL_AUDIO_8X	5
+#define CLK_PLL_VIDEO0		6
+#define CLK_PLL_VIDEO0_2X	7
+#define CLK_PLL_VE		8
+#define CLK_PLL_DDR		9
+
+/* The PLL_PERIPH clock is exported */
+
+#define CLK_PLL_PERIPH_2X	11
+#define CLK_PLL_VIDEO1		12
+#define CLK_PLL_VIDEO1_2X	13
+#define CLK_PLL_GPU		14
+#define CLK_PLL_MIPI		15
+#define CLK_PLL9		16
+#define CLK_PLL10		17
+
+/* The CPUX clock is exported */
+
+#define CLK_AXI			19
+#define CLK_AHB1		20
+#define CLK_APB1		21
+#define CLK_APB2		22
+
+/* All the bus gates are exported */
+
+/* The first bunch of module clocks are exported */
+
+/* EMAC clock is not implemented */
+
+#define CLK_MDFS		107
+#define CLK_SDRAM0		108
+#define CLK_SDRAM1		109
+
+/* All the DRAM gates are exported */
+
+/* Some more module clocks are exported */
+
+#define CLK_MBUS0		141
+#define CLK_MBUS1		142
+
+/* Some more module clocks and external clock outputs are exported */
+
+#define CLK_NUMBER		(CLK_OUT_C + 1)
+
+#endif /* _CCU_SUN6I_A31_H_ */
diff --git a/drivers/clk/sunxi-ng/ccu-sun8i-a23-a33.h b/drivers/clk/sunxi-ng/ccu-sun8i-a23-a33.h
new file mode 100644
index 000000000000..62c0f8d49ef8
--- /dev/null
+++ b/drivers/clk/sunxi-ng/ccu-sun8i-a23-a33.h
@@ -0,0 +1,63 @@
+/*
+ * Copyright 2016 Maxime Ripard
+ *
+ * Maxime Ripard <maxime.ripard@free-electrons.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT 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 _CCU_SUN8I_A23_A33_H_
+#define _CCU_SUN8I_A23_A33_H_
+
+#include <dt-bindings/clock/sun8i-a23-a33-ccu.h>
+#include <dt-bindings/reset/sun8i-a23-a33-ccu.h>
+
+#define CLK_PLL_CPUX		0
+#define CLK_PLL_AUDIO_BASE	1
+#define CLK_PLL_AUDIO		2
+#define CLK_PLL_AUDIO_2X	3
+#define CLK_PLL_AUDIO_4X	4
+#define CLK_PLL_AUDIO_8X	5
+#define CLK_PLL_VIDEO		6
+#define CLK_PLL_VIDEO_2X	7
+#define CLK_PLL_VE		8
+#define CLK_PLL_DDR0		9
+#define CLK_PLL_PERIPH		10
+#define CLK_PLL_PERIPH_2X	11
+#define CLK_PLL_GPU		12
+#define CLK_PLL_MIPI		13
+#define CLK_PLL_HSIC		14
+#define CLK_PLL_DE		15
+#define CLK_PLL_DDR1		16
+#define CLK_PLL_DDR		17
+
+/* The CPUX clock is exported */
+
+#define CLK_AXI			19
+#define CLK_AHB1		20
+#define CLK_APB1		21
+#define CLK_APB2		22
+
+/* All the bus gates are exported */
+
+/* The first part of the mod clocks is exported */
+
+#define CLK_DRAM		79
+
+/* Some more module clocks are exported */
+
+#define CLK_MBUS		95
+
+/* And the last module clocks are exported */
+
+#define CLK_NUMBER		(CLK_ATS + 1)
+
+#endif /* _CCU_SUN8I_A23_A33_H_ */
diff --git a/drivers/clk/sunxi-ng/ccu-sun8i-a23.c b/drivers/clk/sunxi-ng/ccu-sun8i-a23.c
new file mode 100644
index 000000000000..2646d980087b
--- /dev/null
+++ b/drivers/clk/sunxi-ng/ccu-sun8i-a23.c
@@ -0,0 +1,737 @@
+/*
+ * Copyright (c) 2016 Maxime Ripard. All rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/clk-provider.h>
+#include <linux/of_address.h>
+
+#include "ccu_common.h"
+#include "ccu_reset.h"
+
+#include "ccu_div.h"
+#include "ccu_gate.h"
+#include "ccu_mp.h"
+#include "ccu_mult.h"
+#include "ccu_nk.h"
+#include "ccu_nkm.h"
+#include "ccu_nkmp.h"
+#include "ccu_nm.h"
+#include "ccu_phase.h"
+
+#include "ccu-sun8i-a23-a33.h"
+
+
+static struct ccu_nkmp pll_cpux_clk = {
+	.enable = BIT(31),
+	.lock	= BIT(28),
+
+	.n	= _SUNXI_CCU_MULT(8, 5),
+	.k	= _SUNXI_CCU_MULT(4, 2),
+	.m	= _SUNXI_CCU_DIV(0, 2),
+	.p	= _SUNXI_CCU_DIV_MAX(16, 2, 4),
+
+	.common	= {
+		.reg		= 0x000,
+		.hw.init	= CLK_HW_INIT("pll-cpux", "osc24M",
+					      &ccu_nkmp_ops,
+					      0),
+	},
+};
+
+/*
+ * The Audio PLL is supposed to have 4 outputs: 3 fixed factors from
+ * the base (2x, 4x and 8x), and one variable divider (the one true
+ * pll audio).
+ *
+ * We don't have any need for the variable divider for now, so we just
+ * hardcode it to match with the clock names
+ */
+#define SUN8I_A23_PLL_AUDIO_REG	0x008
+
+static SUNXI_CCU_NM_WITH_GATE_LOCK(pll_audio_base_clk, "pll-audio-base",
+				   "osc24M", 0x008,
+				   8, 7,		/* N */
+				   0, 5,		/* M */
+				   BIT(31),		/* gate */
+				   BIT(28),		/* lock */
+				   CLK_SET_RATE_UNGATE);
+
+static SUNXI_CCU_NM_WITH_FRAC_GATE_LOCK(pll_video_clk, "pll-video",
+					"osc24M", 0x010,
+					8, 7,		/* N */
+					0, 4,		/* M */
+					BIT(24),	/* frac enable */
+					BIT(25),	/* frac select */
+					270000000,	/* frac rate 0 */
+					297000000,	/* frac rate 1 */
+					BIT(31),	/* gate */
+					BIT(28),	/* lock */
+					CLK_SET_RATE_UNGATE);
+
+static SUNXI_CCU_NM_WITH_FRAC_GATE_LOCK(pll_ve_clk, "pll-ve",
+					"osc24M", 0x018,
+					8, 7,		/* N */
+					0, 4,		/* M */
+					BIT(24),	/* frac enable */
+					BIT(25),	/* frac select */
+					270000000,	/* frac rate 0 */
+					297000000,	/* frac rate 1 */
+					BIT(31),	/* gate */
+					BIT(28),	/* lock */
+					CLK_SET_RATE_UNGATE);
+
+static SUNXI_CCU_NKM_WITH_GATE_LOCK(pll_ddr_clk, "pll-ddr",
+				    "osc24M", 0x020,
+				    8, 5,		/* N */
+				    4, 2,		/* K */
+				    0, 2,		/* M */
+				    BIT(31),		/* gate */
+				    BIT(28),		/* lock */
+				    0);
+
+static SUNXI_CCU_NK_WITH_GATE_LOCK_POSTDIV(pll_periph_clk, "pll-periph",
+					   "osc24M", 0x028,
+					   8, 5,	/* N */
+					   4, 2,	/* K */
+					   BIT(31),	/* gate */
+					   BIT(28),	/* lock */
+					   2,		/* post-div */
+					   CLK_SET_RATE_UNGATE);
+
+static SUNXI_CCU_NM_WITH_FRAC_GATE_LOCK(pll_gpu_clk, "pll-gpu",
+					"osc24M", 0x038,
+					8, 7,		/* N */
+					0, 4,		/* M */
+					BIT(24),	/* frac enable */
+					BIT(25),	/* frac select */
+					270000000,	/* frac rate 0 */
+					297000000,	/* frac rate 1 */
+					BIT(31),	/* gate */
+					BIT(28),	/* lock */
+					CLK_SET_RATE_UNGATE);
+
+/*
+ * The MIPI PLL has 2 modes: "MIPI" and "HDMI".
+ *
+ * The MIPI mode is a standard NKM-style clock. The HDMI mode is an
+ * integer / fractional clock with switchable multipliers and dividers.
+ * This is not supported here. We hardcode the PLL to MIPI mode.
+ */
+#define SUN8I_A23_PLL_MIPI_REG	0x040
+static SUNXI_CCU_NKM_WITH_GATE_LOCK(pll_mipi_clk, "pll-mipi",
+				    "pll-video", 0x040,
+				    8, 4,		/* N */
+				    4, 2,		/* K */
+				    0, 4,		/* M */
+				    BIT(31),		/* gate */
+				    BIT(28),		/* lock */
+				    CLK_SET_RATE_UNGATE);
+
+static SUNXI_CCU_NM_WITH_FRAC_GATE_LOCK(pll_hsic_clk, "pll-hsic",
+					"osc24M", 0x044,
+					8, 7,		/* N */
+					0, 4,		/* M */
+					BIT(24),	/* frac enable */
+					BIT(25),	/* frac select */
+					270000000,	/* frac rate 0 */
+					297000000,	/* frac rate 1 */
+					BIT(31),	/* gate */
+					BIT(28),	/* lock */
+					CLK_SET_RATE_UNGATE);
+
+static SUNXI_CCU_NM_WITH_FRAC_GATE_LOCK(pll_de_clk, "pll-de",
+					"osc24M", 0x048,
+					8, 7,		/* N */
+					0, 4,		/* M */
+					BIT(24),	/* frac enable */
+					BIT(25),	/* frac select */
+					270000000,	/* frac rate 0 */
+					297000000,	/* frac rate 1 */
+					BIT(31),	/* gate */
+					BIT(28),	/* lock */
+					CLK_SET_RATE_UNGATE);
+
+static const char * const cpux_parents[] = { "osc32k", "osc24M",
+					     "pll-cpux" , "pll-cpux" };
+static SUNXI_CCU_MUX(cpux_clk, "cpux", cpux_parents,
+		     0x050, 16, 2, CLK_IS_CRITICAL);
+
+static SUNXI_CCU_M(axi_clk, "axi", "cpux", 0x050, 0, 2, 0);
+
+static const char * const ahb1_parents[] = { "osc32k", "osc24M",
+					     "axi" , "pll-periph" };
+static struct ccu_div ahb1_clk = {
+	.div		= _SUNXI_CCU_DIV_FLAGS(4, 2, CLK_DIVIDER_POWER_OF_TWO),
+
+	.mux		= {
+		.shift	= 12,
+		.width	= 2,
+
+		.variable_prediv	= {
+			.index	= 3,
+			.shift	= 6,
+			.width	= 2,
+		},
+	},
+
+	.common		= {
+		.reg		= 0x054,
+		.features	= CCU_FEATURE_VARIABLE_PREDIV,
+		.hw.init	= CLK_HW_INIT_PARENTS("ahb1",
+						      ahb1_parents,
+						      &ccu_div_ops,
+						      0),
+	},
+};
+
+static struct clk_div_table apb1_div_table[] = {
+	{ .val = 0, .div = 2 },
+	{ .val = 1, .div = 2 },
+	{ .val = 2, .div = 4 },
+	{ .val = 3, .div = 8 },
+	{ /* Sentinel */ },
+};
+static SUNXI_CCU_DIV_TABLE(apb1_clk, "apb1", "ahb1",
+			   0x054, 8, 2, apb1_div_table, 0);
+
+static const char * const apb2_parents[] = { "osc32k", "osc24M",
+					     "pll-periph" , "pll-periph" };
+static SUNXI_CCU_MP_WITH_MUX(apb2_clk, "apb2", apb2_parents, 0x058,
+			     0, 5,	/* M */
+			     16, 2,	/* P */
+			     24, 2,	/* mux */
+			     0);
+
+static SUNXI_CCU_GATE(bus_mipi_dsi_clk,	"bus-mipi-dsi",	"ahb1",
+		      0x060, BIT(1), 0);
+static SUNXI_CCU_GATE(bus_dma_clk,	"bus-dma",	"ahb1",
+		      0x060, BIT(6), 0);
+static SUNXI_CCU_GATE(bus_mmc0_clk,	"bus-mmc0",	"ahb1",
+		      0x060, BIT(8), 0);
+static SUNXI_CCU_GATE(bus_mmc1_clk,	"bus-mmc1",	"ahb1",
+		      0x060, BIT(9), 0);
+static SUNXI_CCU_GATE(bus_mmc2_clk,	"bus-mmc2",	"ahb1",
+		      0x060, BIT(10), 0);
+static SUNXI_CCU_GATE(bus_nand_clk,	"bus-nand",	"ahb1",
+		      0x060, BIT(13), 0);
+static SUNXI_CCU_GATE(bus_dram_clk,	"bus-dram",	"ahb1",
+		      0x060, BIT(14), 0);
+static SUNXI_CCU_GATE(bus_hstimer_clk,	"bus-hstimer",	"ahb1",
+		      0x060, BIT(19), 0);
+static SUNXI_CCU_GATE(bus_spi0_clk,	"bus-spi0",	"ahb1",
+		      0x060, BIT(20), 0);
+static SUNXI_CCU_GATE(bus_spi1_clk,	"bus-spi1",	"ahb1",
+		      0x060, BIT(21), 0);
+static SUNXI_CCU_GATE(bus_otg_clk,	"bus-otg",	"ahb1",
+		      0x060, BIT(24), 0);
+static SUNXI_CCU_GATE(bus_ehci_clk,	"bus-ehci",	"ahb1",
+		      0x060, BIT(26), 0);
+static SUNXI_CCU_GATE(bus_ohci_clk,	"bus-ohci",	"ahb1",
+		      0x060, BIT(29), 0);
+
+static SUNXI_CCU_GATE(bus_ve_clk,	"bus-ve",	"ahb1",
+		      0x064, BIT(0), 0);
+static SUNXI_CCU_GATE(bus_lcd_clk,	"bus-lcd",	"ahb1",
+		      0x064, BIT(4), 0);
+static SUNXI_CCU_GATE(bus_csi_clk,	"bus-csi",	"ahb1",
+		      0x064, BIT(8), 0);
+static SUNXI_CCU_GATE(bus_de_be_clk,	"bus-de-be",	"ahb1",
+		      0x064, BIT(12), 0);
+static SUNXI_CCU_GATE(bus_de_fe_clk,	"bus-de-fe",	"ahb1",
+		      0x064, BIT(14), 0);
+static SUNXI_CCU_GATE(bus_gpu_clk,	"bus-gpu",	"ahb1",
+		      0x064, BIT(20), 0);
+static SUNXI_CCU_GATE(bus_msgbox_clk,	"bus-msgbox",	"ahb1",
+		      0x064, BIT(21), 0);
+static SUNXI_CCU_GATE(bus_spinlock_clk,	"bus-spinlock",	"ahb1",
+		      0x064, BIT(22), 0);
+static SUNXI_CCU_GATE(bus_drc_clk,	"bus-drc",	"ahb1",
+		      0x064, BIT(25), 0);
+
+static SUNXI_CCU_GATE(bus_codec_clk,	"bus-codec",	"apb1",
+		      0x068, BIT(0), 0);
+static SUNXI_CCU_GATE(bus_pio_clk,	"bus-pio",	"apb1",
+		      0x068, BIT(5), 0);
+static SUNXI_CCU_GATE(bus_i2s0_clk,	"bus-i2s0",	"apb1",
+		      0x068, BIT(12), 0);
+static SUNXI_CCU_GATE(bus_i2s1_clk,	"bus-i2s1",	"apb1",
+		      0x068, BIT(13), 0);
+
+static SUNXI_CCU_GATE(bus_i2c0_clk,	"bus-i2c0",	"apb2",
+		      0x06c, BIT(0), 0);
+static SUNXI_CCU_GATE(bus_i2c1_clk,	"bus-i2c1",	"apb2",
+		      0x06c, BIT(1), 0);
+static SUNXI_CCU_GATE(bus_i2c2_clk,	"bus-i2c2",	"apb2",
+		      0x06c, BIT(2), 0);
+static SUNXI_CCU_GATE(bus_uart0_clk,	"bus-uart0",	"apb2",
+		      0x06c, BIT(16), 0);
+static SUNXI_CCU_GATE(bus_uart1_clk,	"bus-uart1",	"apb2",
+		      0x06c, BIT(17), 0);
+static SUNXI_CCU_GATE(bus_uart2_clk,	"bus-uart2",	"apb2",
+		      0x06c, BIT(18), 0);
+static SUNXI_CCU_GATE(bus_uart3_clk,	"bus-uart3",	"apb2",
+		      0x06c, BIT(19), 0);
+static SUNXI_CCU_GATE(bus_uart4_clk,	"bus-uart4",	"apb2",
+		      0x06c, BIT(20), 0);
+
+static const char * const mod0_default_parents[] = { "osc24M", "pll-periph" };
+static SUNXI_CCU_MP_WITH_MUX_GATE(nand_clk, "nand", mod0_default_parents, 0x080,
+				  0, 4,		/* M */
+				  16, 2,	/* P */
+				  24, 2,	/* mux */
+				  BIT(31),	/* gate */
+				  0);
+
+static SUNXI_CCU_MP_WITH_MUX_GATE(mmc0_clk, "mmc0", mod0_default_parents, 0x088,
+				  0, 4,		/* M */
+				  16, 2,	/* P */
+				  24, 2,	/* mux */
+				  BIT(31),	/* gate */
+				  0);
+
+static SUNXI_CCU_PHASE(mmc0_sample_clk, "mmc0_sample", "mmc0",
+		       0x088, 20, 3, 0);
+static SUNXI_CCU_PHASE(mmc0_output_clk, "mmc0_output", "mmc0",
+		       0x088, 8, 3, 0);
+
+static SUNXI_CCU_MP_WITH_MUX_GATE(mmc1_clk, "mmc1", mod0_default_parents, 0x08c,
+				  0, 4,		/* M */
+				  16, 2,	/* P */
+				  24, 2,	/* mux */
+				  BIT(31),	/* gate */
+				  0);
+
+static SUNXI_CCU_PHASE(mmc1_sample_clk, "mmc1_sample", "mmc1",
+		       0x08c, 20, 3, 0);
+static SUNXI_CCU_PHASE(mmc1_output_clk, "mmc1_output", "mmc1",
+		       0x08c, 8, 3, 0);
+
+static SUNXI_CCU_MP_WITH_MUX_GATE(mmc2_clk, "mmc2", mod0_default_parents, 0x090,
+				  0, 4,		/* M */
+				  16, 2,	/* P */
+				  24, 2,	/* mux */
+				  BIT(31),	/* gate */
+				  0);
+
+static SUNXI_CCU_PHASE(mmc2_sample_clk, "mmc2_sample", "mmc2",
+		       0x090, 20, 3, 0);
+static SUNXI_CCU_PHASE(mmc2_output_clk, "mmc2_output", "mmc2",
+		       0x090, 8, 3, 0);
+
+static SUNXI_CCU_MP_WITH_MUX_GATE(spi0_clk, "spi0", mod0_default_parents, 0x0a0,
+				  0, 4,		/* M */
+				  16, 2,	/* P */
+				  24, 2,	/* mux */
+				  BIT(31),	/* gate */
+				  0);
+
+static SUNXI_CCU_MP_WITH_MUX_GATE(spi1_clk, "spi1", mod0_default_parents, 0x0a4,
+				  0, 4,		/* M */
+				  16, 2,	/* P */
+				  24, 2,	/* mux */
+				  BIT(31),	/* gate */
+				  0);
+
+static const char * const i2s_parents[] = { "pll-audio-8x", "pll-audio-4x",
+					    "pll-audio-2x", "pll-audio" };
+static SUNXI_CCU_MUX_WITH_GATE(i2s0_clk, "i2s0", i2s_parents,
+			       0x0b0, 16, 2, BIT(31), 0);
+
+static SUNXI_CCU_MUX_WITH_GATE(i2s1_clk, "i2s1", i2s_parents,
+			       0x0b4, 16, 2, BIT(31), 0);
+
+/* TODO: the parent for most of the USB clocks is not known */
+static SUNXI_CCU_GATE(usb_phy0_clk,	"usb-phy0",	"osc24M",
+		      0x0cc, BIT(8), 0);
+static SUNXI_CCU_GATE(usb_phy1_clk,	"usb-phy1",	"osc24M",
+		      0x0cc, BIT(9), 0);
+static SUNXI_CCU_GATE(usb_hsic_clk,	"usb-hsic",	"pll-hsic",
+		      0x0cc, BIT(10), 0);
+static SUNXI_CCU_GATE(usb_hsic_12M_clk,	"usb-hsic-12M",	"osc24M",
+		      0x0cc, BIT(11), 0);
+static SUNXI_CCU_GATE(usb_ohci_clk,	"usb-ohci",	"osc24M",
+		      0x0cc, BIT(16), 0);
+
+static SUNXI_CCU_GATE(dram_ve_clk,	"dram-ve",	"pll-ddr",
+		      0x100, BIT(0), 0);
+static SUNXI_CCU_GATE(dram_csi_clk,	"dram-csi",	"pll-ddr",
+		      0x100, BIT(1), 0);
+static SUNXI_CCU_GATE(dram_drc_clk,	"dram-drc",	"pll-ddr",
+		      0x100, BIT(16), 0);
+static SUNXI_CCU_GATE(dram_de_fe_clk,	"dram-de-fe",	"pll-ddr",
+		      0x100, BIT(24), 0);
+static SUNXI_CCU_GATE(dram_de_be_clk,	"dram-de-be",	"pll-ddr",
+		      0x100, BIT(26), 0);
+
+static const char * const de_parents[] = { "pll-video", "pll-periph-2x",
+					   "pll-gpu", "pll-de" };
+static const u8 de_table[] = { 0, 2, 3, 5 };
+static SUNXI_CCU_M_WITH_MUX_TABLE_GATE(de_be_clk, "de-be",
+				       de_parents, de_table,
+				       0x104, 0, 4, 24, 3, BIT(31), 0);
+
+static SUNXI_CCU_M_WITH_MUX_TABLE_GATE(de_fe_clk, "de-fe",
+				       de_parents, de_table,
+				       0x10c, 0, 4, 24, 3, BIT(31), 0);
+
+static const char * const lcd_ch0_parents[] = { "pll-video", "pll-video-2x",
+						"pll-mipi" };
+static const u8 lcd_ch0_table[] = { 0, 2, 4 };
+static SUNXI_CCU_MUX_TABLE_WITH_GATE(lcd_ch0_clk, "lcd-ch0",
+				     lcd_ch0_parents, lcd_ch0_table,
+				     0x118, 24, 3, BIT(31),
+				     CLK_SET_RATE_PARENT);
+
+static const char * const lcd_ch1_parents[] = { "pll-video", "pll-video-2x" };
+static const u8 lcd_ch1_table[] = { 0, 2 };
+static SUNXI_CCU_M_WITH_MUX_TABLE_GATE(lcd_ch1_clk, "lcd-ch1",
+				       lcd_ch1_parents, lcd_ch1_table,
+				       0x12c, 0, 4, 24, 2, BIT(31), 0);
+
+static const char * const csi_sclk_parents[] = { "pll-video", "pll-de",
+						 "pll-mipi", "pll-ve" };
+static const u8 csi_sclk_table[] = { 0, 3, 4, 5 };
+static SUNXI_CCU_M_WITH_MUX_TABLE_GATE(csi_sclk_clk, "csi-sclk",
+				       csi_sclk_parents, csi_sclk_table,
+				       0x134, 16, 4, 24, 3, BIT(31), 0);
+
+static const char * const csi_mclk_parents[] = { "pll-video", "pll-de",
+						 "osc24M" };
+static const u8 csi_mclk_table[] = { 0, 3, 5 };
+static SUNXI_CCU_M_WITH_MUX_TABLE_GATE(csi_mclk_clk, "csi-mclk",
+				       csi_mclk_parents, csi_mclk_table,
+				       0x134, 0, 5, 8, 3, BIT(15), 0);
+
+static SUNXI_CCU_M_WITH_GATE(ve_clk, "ve", "pll-ve",
+			     0x13c, 16, 3, BIT(31), CLK_SET_RATE_PARENT);
+
+static SUNXI_CCU_GATE(ac_dig_clk,	"ac-dig",	"pll-audio",
+		      0x140, BIT(31), 0);
+static SUNXI_CCU_GATE(avs_clk,		"avs",		"osc24M",
+		      0x144, BIT(31), 0);
+
+static const char * const mbus_parents[] = { "osc24M", "pll-periph-2x",
+					     "pll-ddr" };
+static SUNXI_CCU_M_WITH_MUX_GATE(mbus_clk, "mbus", mbus_parents,
+				 0x15c, 0, 3, 24, 2, BIT(31), CLK_IS_CRITICAL);
+
+static const char * const dsi_sclk_parents[] = { "pll-video", "pll-video-2x" };
+static const u8 dsi_sclk_table[] = { 0, 2 };
+static SUNXI_CCU_M_WITH_MUX_TABLE_GATE(dsi_sclk_clk, "dsi-sclk",
+				       dsi_sclk_parents, dsi_sclk_table,
+				       0x168, 16, 4, 24, 2, BIT(31), 0);
+
+static const char * const dsi_dphy_parents[] = { "pll-video", "pll-periph" };
+static const u8 dsi_dphy_table[] = { 0, 2 };
+static SUNXI_CCU_M_WITH_MUX_TABLE_GATE(dsi_dphy_clk, "dsi-dphy",
+				       dsi_dphy_parents, dsi_dphy_table,
+				       0x168, 0, 4, 8, 2, BIT(15), 0);
+
+static SUNXI_CCU_M_WITH_MUX_TABLE_GATE(drc_clk, "drc",
+				       de_parents, de_table,
+				       0x180, 0, 4, 24, 3, BIT(31), 0);
+
+static SUNXI_CCU_M_WITH_GATE(gpu_clk, "gpu", "pll-gpu",
+			     0x1a0, 0, 3, BIT(31), 0);
+
+static const char * const ats_parents[] = { "osc24M", "pll-periph" };
+static SUNXI_CCU_M_WITH_MUX_GATE(ats_clk, "ats", ats_parents,
+				 0x1b0, 0, 3, 24, 2, BIT(31), 0);
+
+static struct ccu_common *sun8i_a23_ccu_clks[] = {
+	&pll_cpux_clk.common,
+	&pll_audio_base_clk.common,
+	&pll_video_clk.common,
+	&pll_ve_clk.common,
+	&pll_ddr_clk.common,
+	&pll_periph_clk.common,
+	&pll_gpu_clk.common,
+	&pll_mipi_clk.common,
+	&pll_hsic_clk.common,
+	&pll_de_clk.common,
+	&cpux_clk.common,
+	&axi_clk.common,
+	&ahb1_clk.common,
+	&apb1_clk.common,
+	&apb2_clk.common,
+	&bus_mipi_dsi_clk.common,
+	&bus_dma_clk.common,
+	&bus_mmc0_clk.common,
+	&bus_mmc1_clk.common,
+	&bus_mmc2_clk.common,
+	&bus_nand_clk.common,
+	&bus_dram_clk.common,
+	&bus_hstimer_clk.common,
+	&bus_spi0_clk.common,
+	&bus_spi1_clk.common,
+	&bus_otg_clk.common,
+	&bus_ehci_clk.common,
+	&bus_ohci_clk.common,
+	&bus_ve_clk.common,
+	&bus_lcd_clk.common,
+	&bus_csi_clk.common,
+	&bus_de_fe_clk.common,
+	&bus_de_be_clk.common,
+	&bus_gpu_clk.common,
+	&bus_msgbox_clk.common,
+	&bus_spinlock_clk.common,
+	&bus_drc_clk.common,
+	&bus_codec_clk.common,
+	&bus_pio_clk.common,
+	&bus_i2s0_clk.common,
+	&bus_i2s1_clk.common,
+	&bus_i2c0_clk.common,
+	&bus_i2c1_clk.common,
+	&bus_i2c2_clk.common,
+	&bus_uart0_clk.common,
+	&bus_uart1_clk.common,
+	&bus_uart2_clk.common,
+	&bus_uart3_clk.common,
+	&bus_uart4_clk.common,
+	&nand_clk.common,
+	&mmc0_clk.common,
+	&mmc0_sample_clk.common,
+	&mmc0_output_clk.common,
+	&mmc1_clk.common,
+	&mmc1_sample_clk.common,
+	&mmc1_output_clk.common,
+	&mmc2_clk.common,
+	&mmc2_sample_clk.common,
+	&mmc2_output_clk.common,
+	&spi0_clk.common,
+	&spi1_clk.common,
+	&i2s0_clk.common,
+	&i2s1_clk.common,
+	&usb_phy0_clk.common,
+	&usb_phy1_clk.common,
+	&usb_hsic_clk.common,
+	&usb_hsic_12M_clk.common,
+	&usb_ohci_clk.common,
+	&dram_ve_clk.common,
+	&dram_csi_clk.common,
+	&dram_drc_clk.common,
+	&dram_de_fe_clk.common,
+	&dram_de_be_clk.common,
+	&de_be_clk.common,
+	&de_fe_clk.common,
+	&lcd_ch0_clk.common,
+	&lcd_ch1_clk.common,
+	&csi_sclk_clk.common,
+	&csi_mclk_clk.common,
+	&ve_clk.common,
+	&ac_dig_clk.common,
+	&avs_clk.common,
+	&mbus_clk.common,
+	&dsi_sclk_clk.common,
+	&dsi_dphy_clk.common,
+	&drc_clk.common,
+	&gpu_clk.common,
+	&ats_clk.common,
+};
+
+/* We hardcode the divider to 4 for now */
+static CLK_FIXED_FACTOR(pll_audio_clk, "pll-audio",
+			"pll-audio-base", 4, 1, CLK_SET_RATE_PARENT);
+static CLK_FIXED_FACTOR(pll_audio_2x_clk, "pll-audio-2x",
+			"pll-audio-base", 2, 1, CLK_SET_RATE_PARENT);
+static CLK_FIXED_FACTOR(pll_audio_4x_clk, "pll-audio-4x",
+			"pll-audio-base", 1, 1, CLK_SET_RATE_PARENT);
+static CLK_FIXED_FACTOR(pll_audio_8x_clk, "pll-audio-8x",
+			"pll-audio-base", 1, 2, CLK_SET_RATE_PARENT);
+static CLK_FIXED_FACTOR(pll_periph_2x_clk, "pll-periph-2x",
+			"pll-periph", 1, 2, 0);
+static CLK_FIXED_FACTOR(pll_video_2x_clk, "pll-video-2x",
+			"pll-video", 1, 2, 0);
+
+static struct clk_hw_onecell_data sun8i_a23_hw_clks = {
+	.hws	= {
+		[CLK_PLL_CPUX]		= &pll_cpux_clk.common.hw,
+		[CLK_PLL_AUDIO_BASE]	= &pll_audio_base_clk.common.hw,
+		[CLK_PLL_AUDIO]		= &pll_audio_clk.hw,
+		[CLK_PLL_AUDIO_2X]	= &pll_audio_2x_clk.hw,
+		[CLK_PLL_AUDIO_4X]	= &pll_audio_4x_clk.hw,
+		[CLK_PLL_AUDIO_8X]	= &pll_audio_8x_clk.hw,
+		[CLK_PLL_VIDEO]		= &pll_video_clk.common.hw,
+		[CLK_PLL_VIDEO_2X]	= &pll_video_2x_clk.hw,
+		[CLK_PLL_VE]		= &pll_ve_clk.common.hw,
+		[CLK_PLL_DDR0]		= &pll_ddr_clk.common.hw,
+		[CLK_PLL_PERIPH]	= &pll_periph_clk.common.hw,
+		[CLK_PLL_PERIPH_2X]	= &pll_periph_2x_clk.hw,
+		[CLK_PLL_GPU]		= &pll_gpu_clk.common.hw,
+		[CLK_PLL_MIPI]		= &pll_mipi_clk.common.hw,
+		[CLK_PLL_HSIC]		= &pll_hsic_clk.common.hw,
+		[CLK_PLL_DE]		= &pll_de_clk.common.hw,
+		[CLK_CPUX]		= &cpux_clk.common.hw,
+		[CLK_AXI]		= &axi_clk.common.hw,
+		[CLK_AHB1]		= &ahb1_clk.common.hw,
+		[CLK_APB1]		= &apb1_clk.common.hw,
+		[CLK_APB2]		= &apb2_clk.common.hw,
+		[CLK_BUS_MIPI_DSI]	= &bus_mipi_dsi_clk.common.hw,
+		[CLK_BUS_DMA]		= &bus_dma_clk.common.hw,
+		[CLK_BUS_MMC0]		= &bus_mmc0_clk.common.hw,
+		[CLK_BUS_MMC1]		= &bus_mmc1_clk.common.hw,
+		[CLK_BUS_MMC2]		= &bus_mmc2_clk.common.hw,
+		[CLK_BUS_NAND]		= &bus_nand_clk.common.hw,
+		[CLK_BUS_DRAM]		= &bus_dram_clk.common.hw,
+		[CLK_BUS_HSTIMER]	= &bus_hstimer_clk.common.hw,
+		[CLK_BUS_SPI0]		= &bus_spi0_clk.common.hw,
+		[CLK_BUS_SPI1]		= &bus_spi1_clk.common.hw,
+		[CLK_BUS_OTG]		= &bus_otg_clk.common.hw,
+		[CLK_BUS_EHCI]		= &bus_ehci_clk.common.hw,
+		[CLK_BUS_OHCI]		= &bus_ohci_clk.common.hw,
+		[CLK_BUS_VE]		= &bus_ve_clk.common.hw,
+		[CLK_BUS_LCD]		= &bus_lcd_clk.common.hw,
+		[CLK_BUS_CSI]		= &bus_csi_clk.common.hw,
+		[CLK_BUS_DE_BE]		= &bus_de_be_clk.common.hw,
+		[CLK_BUS_DE_FE]		= &bus_de_fe_clk.common.hw,
+		[CLK_BUS_GPU]		= &bus_gpu_clk.common.hw,
+		[CLK_BUS_MSGBOX]	= &bus_msgbox_clk.common.hw,
+		[CLK_BUS_SPINLOCK]	= &bus_spinlock_clk.common.hw,
+		[CLK_BUS_DRC]		= &bus_drc_clk.common.hw,
+		[CLK_BUS_CODEC]		= &bus_codec_clk.common.hw,
+		[CLK_BUS_PIO]		= &bus_pio_clk.common.hw,
+		[CLK_BUS_I2S0]		= &bus_i2s0_clk.common.hw,
+		[CLK_BUS_I2S1]		= &bus_i2s1_clk.common.hw,
+		[CLK_BUS_I2C0]		= &bus_i2c0_clk.common.hw,
+		[CLK_BUS_I2C1]		= &bus_i2c1_clk.common.hw,
+		[CLK_BUS_I2C2]		= &bus_i2c2_clk.common.hw,
+		[CLK_BUS_UART0]		= &bus_uart0_clk.common.hw,
+		[CLK_BUS_UART1]		= &bus_uart1_clk.common.hw,
+		[CLK_BUS_UART2]		= &bus_uart2_clk.common.hw,
+		[CLK_BUS_UART3]		= &bus_uart3_clk.common.hw,
+		[CLK_BUS_UART4]		= &bus_uart4_clk.common.hw,
+		[CLK_NAND]		= &nand_clk.common.hw,
+		[CLK_MMC0]		= &mmc0_clk.common.hw,
+		[CLK_MMC0_SAMPLE]	= &mmc0_sample_clk.common.hw,
+		[CLK_MMC0_OUTPUT]	= &mmc0_output_clk.common.hw,
+		[CLK_MMC1]		= &mmc1_clk.common.hw,
+		[CLK_MMC1_SAMPLE]	= &mmc1_sample_clk.common.hw,
+		[CLK_MMC1_OUTPUT]	= &mmc1_output_clk.common.hw,
+		[CLK_MMC2]		= &mmc2_clk.common.hw,
+		[CLK_MMC2_SAMPLE]	= &mmc2_sample_clk.common.hw,
+		[CLK_MMC2_OUTPUT]	= &mmc2_output_clk.common.hw,
+		[CLK_SPI0]		= &spi0_clk.common.hw,
+		[CLK_SPI1]		= &spi1_clk.common.hw,
+		[CLK_I2S0]		= &i2s0_clk.common.hw,
+		[CLK_I2S1]		= &i2s1_clk.common.hw,
+		[CLK_USB_PHY0]		= &usb_phy0_clk.common.hw,
+		[CLK_USB_PHY1]		= &usb_phy1_clk.common.hw,
+		[CLK_USB_HSIC]		= &usb_hsic_clk.common.hw,
+		[CLK_USB_HSIC_12M]	= &usb_hsic_12M_clk.common.hw,
+		[CLK_USB_OHCI]		= &usb_ohci_clk.common.hw,
+		[CLK_DRAM_VE]		= &dram_ve_clk.common.hw,
+		[CLK_DRAM_CSI]		= &dram_csi_clk.common.hw,
+		[CLK_DRAM_DRC]		= &dram_drc_clk.common.hw,
+		[CLK_DRAM_DE_FE]	= &dram_de_fe_clk.common.hw,
+		[CLK_DRAM_DE_BE]	= &dram_de_be_clk.common.hw,
+		[CLK_DE_BE]		= &de_be_clk.common.hw,
+		[CLK_DE_FE]		= &de_fe_clk.common.hw,
+		[CLK_LCD_CH0]		= &lcd_ch0_clk.common.hw,
+		[CLK_LCD_CH1]		= &lcd_ch1_clk.common.hw,
+		[CLK_CSI_SCLK]		= &csi_sclk_clk.common.hw,
+		[CLK_CSI_MCLK]		= &csi_mclk_clk.common.hw,
+		[CLK_VE]		= &ve_clk.common.hw,
+		[CLK_AC_DIG]		= &ac_dig_clk.common.hw,
+		[CLK_AVS]		= &avs_clk.common.hw,
+		[CLK_MBUS]		= &mbus_clk.common.hw,
+		[CLK_DSI_SCLK]		= &dsi_sclk_clk.common.hw,
+		[CLK_DSI_DPHY]		= &dsi_dphy_clk.common.hw,
+		[CLK_DRC]		= &drc_clk.common.hw,
+		[CLK_GPU]		= &gpu_clk.common.hw,
+		[CLK_ATS]		= &ats_clk.common.hw,
+	},
+	.num	= CLK_NUMBER,
+};
+
+static struct ccu_reset_map sun8i_a23_ccu_resets[] = {
+	[RST_USB_PHY0]		=  { 0x0cc, BIT(0) },
+	[RST_USB_PHY1]		=  { 0x0cc, BIT(1) },
+	[RST_USB_HSIC]		=  { 0x0cc, BIT(2) },
+
+	[RST_MBUS]		=  { 0x0fc, BIT(31) },
+
+	[RST_BUS_MIPI_DSI]	=  { 0x2c0, BIT(1) },
+	[RST_BUS_DMA]		=  { 0x2c0, BIT(6) },
+	[RST_BUS_MMC0]		=  { 0x2c0, BIT(8) },
+	[RST_BUS_MMC1]		=  { 0x2c0, BIT(9) },
+	[RST_BUS_MMC2]		=  { 0x2c0, BIT(10) },
+	[RST_BUS_NAND]		=  { 0x2c0, BIT(13) },
+	[RST_BUS_DRAM]		=  { 0x2c0, BIT(14) },
+	[RST_BUS_HSTIMER]	=  { 0x2c0, BIT(19) },
+	[RST_BUS_SPI0]		=  { 0x2c0, BIT(20) },
+	[RST_BUS_SPI1]		=  { 0x2c0, BIT(21) },
+	[RST_BUS_OTG]		=  { 0x2c0, BIT(24) },
+	[RST_BUS_EHCI]		=  { 0x2c0, BIT(26) },
+	[RST_BUS_OHCI]		=  { 0x2c0, BIT(29) },
+
+	[RST_BUS_VE]		=  { 0x2c4, BIT(0) },
+	[RST_BUS_LCD]		=  { 0x2c4, BIT(4) },
+	[RST_BUS_CSI]		=  { 0x2c4, BIT(8) },
+	[RST_BUS_DE_BE]		=  { 0x2c4, BIT(12) },
+	[RST_BUS_DE_FE]		=  { 0x2c4, BIT(14) },
+	[RST_BUS_GPU]		=  { 0x2c4, BIT(20) },
+	[RST_BUS_MSGBOX]	=  { 0x2c4, BIT(21) },
+	[RST_BUS_SPINLOCK]	=  { 0x2c4, BIT(22) },
+	[RST_BUS_DRC]		=  { 0x2c4, BIT(25) },
+
+	[RST_BUS_LVDS]		=  { 0x2c8, BIT(0) },
+
+	[RST_BUS_CODEC]		=  { 0x2d0, BIT(0) },
+	[RST_BUS_I2S0]		=  { 0x2d0, BIT(12) },
+	[RST_BUS_I2S1]		=  { 0x2d0, BIT(13) },
+
+	[RST_BUS_I2C0]		=  { 0x2d8, BIT(0) },
+	[RST_BUS_I2C1]		=  { 0x2d8, BIT(1) },
+	[RST_BUS_I2C2]		=  { 0x2d8, BIT(2) },
+	[RST_BUS_UART0]		=  { 0x2d8, BIT(16) },
+	[RST_BUS_UART1]		=  { 0x2d8, BIT(17) },
+	[RST_BUS_UART2]		=  { 0x2d8, BIT(18) },
+	[RST_BUS_UART3]		=  { 0x2d8, BIT(19) },
+	[RST_BUS_UART4]		=  { 0x2d8, BIT(20) },
+};
+
+static const struct sunxi_ccu_desc sun8i_a23_ccu_desc = {
+	.ccu_clks	= sun8i_a23_ccu_clks,
+	.num_ccu_clks	= ARRAY_SIZE(sun8i_a23_ccu_clks),
+
+	.hw_clks	= &sun8i_a23_hw_clks,
+
+	.resets		= sun8i_a23_ccu_resets,
+	.num_resets	= ARRAY_SIZE(sun8i_a23_ccu_resets),
+};
+
+static void __init sun8i_a23_ccu_setup(struct device_node *node)
+{
+	void __iomem *reg;
+	u32 val;
+
+	reg = of_io_request_and_map(node, 0, of_node_full_name(node));
+	if (IS_ERR(reg)) {
+		pr_err("%s: Could not map the clock registers\n",
+		       of_node_full_name(node));
+		return;
+	}
+
+	/* Force the PLL-Audio-1x divider to 4 */
+	val = readl(reg + SUN8I_A23_PLL_AUDIO_REG);
+	val &= ~GENMASK(19, 16);
+	writel(val | (3 << 16), reg + SUN8I_A23_PLL_AUDIO_REG);
+
+	/* Force PLL-MIPI to MIPI mode */
+	val = readl(reg + SUN8I_A23_PLL_MIPI_REG);
+	val &= ~BIT(16);
+	writel(val, reg + SUN8I_A23_PLL_MIPI_REG);
+
+	sunxi_ccu_probe(node, reg, &sun8i_a23_ccu_desc);
+}
+CLK_OF_DECLARE(sun8i_a23_ccu, "allwinner,sun8i-a23-ccu",
+	       sun8i_a23_ccu_setup);
diff --git a/drivers/clk/sunxi-ng/ccu-sun8i-a33.c b/drivers/clk/sunxi-ng/ccu-sun8i-a33.c
new file mode 100644
index 000000000000..96b40ca57697
--- /dev/null
+++ b/drivers/clk/sunxi-ng/ccu-sun8i-a33.c
@@ -0,0 +1,780 @@
+/*
+ * Copyright (c) 2016 Maxime Ripard. All rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/clk-provider.h>
+#include <linux/of_address.h>
+
+#include "ccu_common.h"
+#include "ccu_reset.h"
+
+#include "ccu_div.h"
+#include "ccu_gate.h"
+#include "ccu_mp.h"
+#include "ccu_mult.h"
+#include "ccu_nk.h"
+#include "ccu_nkm.h"
+#include "ccu_nkmp.h"
+#include "ccu_nm.h"
+#include "ccu_phase.h"
+
+#include "ccu-sun8i-a23-a33.h"
+
+static struct ccu_nkmp pll_cpux_clk = {
+	.enable = BIT(31),
+	.lock	= BIT(28),
+
+	.n	= _SUNXI_CCU_MULT(8, 5),
+	.k	= _SUNXI_CCU_MULT(4, 2),
+	.m	= _SUNXI_CCU_DIV(0, 2),
+	.p	= _SUNXI_CCU_DIV_MAX(16, 2, 4),
+
+	.common	= {
+		.reg		= 0x000,
+		.hw.init	= CLK_HW_INIT("pll-cpux", "osc24M",
+					      &ccu_nkmp_ops,
+					      0),
+	},
+};
+
+/*
+ * The Audio PLL is supposed to have 4 outputs: 3 fixed factors from
+ * the base (2x, 4x and 8x), and one variable divider (the one true
+ * pll audio).
+ *
+ * We don't have any need for the variable divider for now, so we just
+ * hardcode it to match with the clock names
+ */
+#define SUN8I_A33_PLL_AUDIO_REG	0x008
+
+static SUNXI_CCU_NM_WITH_GATE_LOCK(pll_audio_base_clk, "pll-audio-base",
+				   "osc24M", 0x008,
+				   8, 7,		/* N */
+				   0, 5,		/* M */
+				   BIT(31),		/* gate */
+				   BIT(28),		/* lock */
+				   CLK_SET_RATE_UNGATE);
+
+static SUNXI_CCU_NM_WITH_FRAC_GATE_LOCK(pll_video_clk, "pll-video",
+					"osc24M", 0x010,
+					8, 7,		/* N */
+					0, 4,		/* M */
+					BIT(24),	/* frac enable */
+					BIT(25),	/* frac select */
+					270000000,	/* frac rate 0 */
+					297000000,	/* frac rate 1 */
+					BIT(31),	/* gate */
+					BIT(28),	/* lock */
+					CLK_SET_RATE_UNGATE);
+
+static SUNXI_CCU_NM_WITH_FRAC_GATE_LOCK(pll_ve_clk, "pll-ve",
+					"osc24M", 0x018,
+					8, 7,		/* N */
+					0, 4,		/* M */
+					BIT(24),	/* frac enable */
+					BIT(25),	/* frac select */
+					270000000,	/* frac rate 0 */
+					297000000,	/* frac rate 1 */
+					BIT(31),	/* gate */
+					BIT(28),	/* lock */
+					CLK_SET_RATE_UNGATE);
+
+static SUNXI_CCU_NKM_WITH_GATE_LOCK(pll_ddr0_clk, "pll-ddr0",
+				    "osc24M", 0x020,
+				    8, 5,		/* N */
+				    4, 2,		/* K */
+				    0, 2,		/* M */
+				    BIT(31),		/* gate */
+				    BIT(28),		/* lock */
+				    0);
+
+static SUNXI_CCU_NK_WITH_GATE_LOCK_POSTDIV(pll_periph_clk, "pll-periph",
+					   "osc24M", 0x028,
+					   8, 5,	/* N */
+					   4, 2,	/* K */
+					   BIT(31),	/* gate */
+					   BIT(28),	/* lock */
+					   2,		/* post-div */
+					   CLK_SET_RATE_UNGATE);
+
+static SUNXI_CCU_NM_WITH_FRAC_GATE_LOCK(pll_gpu_clk, "pll-gpu",
+					"osc24M", 0x038,
+					8, 7,		/* N */
+					0, 4,		/* M */
+					BIT(24),	/* frac enable */
+					BIT(25),	/* frac select */
+					270000000,	/* frac rate 0 */
+					297000000,	/* frac rate 1 */
+					BIT(31),	/* gate */
+					BIT(28),	/* lock */
+					CLK_SET_RATE_UNGATE);
+
+/*
+ * The MIPI PLL has 2 modes: "MIPI" and "HDMI".
+ *
+ * The MIPI mode is a standard NKM-style clock. The HDMI mode is an
+ * integer / fractional clock with switchable multipliers and dividers.
+ * This is not supported here. We hardcode the PLL to MIPI mode.
+ */
+#define SUN8I_A33_PLL_MIPI_REG	0x040
+static SUNXI_CCU_NKM_WITH_GATE_LOCK(pll_mipi_clk, "pll-mipi",
+				    "pll-video", 0x040,
+				    8, 4,		/* N */
+				    4, 2,		/* K */
+				    0, 4,		/* M */
+				    BIT(31),		/* gate */
+				    BIT(28),		/* lock */
+				    CLK_SET_RATE_UNGATE);
+
+static SUNXI_CCU_NM_WITH_FRAC_GATE_LOCK(pll_hsic_clk, "pll-hsic",
+					"osc24M", 0x044,
+					8, 7,		/* N */
+					0, 4,		/* M */
+					BIT(24),	/* frac enable */
+					BIT(25),	/* frac select */
+					270000000,	/* frac rate 0 */
+					297000000,	/* frac rate 1 */
+					BIT(31),	/* gate */
+					BIT(28),	/* lock */
+					CLK_SET_RATE_UNGATE);
+
+static SUNXI_CCU_NM_WITH_FRAC_GATE_LOCK(pll_de_clk, "pll-de",
+					"osc24M", 0x048,
+					8, 7,		/* N */
+					0, 4,		/* M */
+					BIT(24),	/* frac enable */
+					BIT(25),	/* frac select */
+					270000000,	/* frac rate 0 */
+					297000000,	/* frac rate 1 */
+					BIT(31),	/* gate */
+					BIT(28),	/* lock */
+					CLK_SET_RATE_UNGATE);
+
+/* TODO: Fix N */
+static SUNXI_CCU_N_WITH_GATE_LOCK(pll_ddr1_clk, "pll-ddr1",
+				  "osc24M", 0x04c,
+				  8, 6,			/* N */
+				  BIT(31),		/* gate */
+				  BIT(28),		/* lock */
+				  CLK_SET_RATE_UNGATE);
+
+static const char * const cpux_parents[] = { "osc32k", "osc24M",
+					     "pll-cpux" , "pll-cpux" };
+static SUNXI_CCU_MUX(cpux_clk, "cpux", cpux_parents,
+		     0x050, 16, 2, CLK_IS_CRITICAL);
+
+static SUNXI_CCU_M(axi_clk, "axi", "cpux", 0x050, 0, 2, 0);
+
+static const char * const ahb1_parents[] = { "osc32k", "osc24M",
+					     "axi" , "pll-periph" };
+static struct ccu_div ahb1_clk = {
+	.div		= _SUNXI_CCU_DIV_FLAGS(4, 2, CLK_DIVIDER_POWER_OF_TWO),
+
+	.mux		= {
+		.shift	= 12,
+		.width	= 2,
+
+		.variable_prediv	= {
+			.index	= 3,
+			.shift	= 6,
+			.width	= 2,
+		},
+	},
+
+	.common		= {
+		.reg		= 0x054,
+		.features	= CCU_FEATURE_VARIABLE_PREDIV,
+		.hw.init	= CLK_HW_INIT_PARENTS("ahb1",
+						      ahb1_parents,
+						      &ccu_div_ops,
+						      0),
+	},
+};
+
+static struct clk_div_table apb1_div_table[] = {
+	{ .val = 0, .div = 2 },
+	{ .val = 1, .div = 2 },
+	{ .val = 2, .div = 4 },
+	{ .val = 3, .div = 8 },
+	{ /* Sentinel */ },
+};
+static SUNXI_CCU_DIV_TABLE(apb1_clk, "apb1", "ahb1",
+			   0x054, 8, 2, apb1_div_table, 0);
+
+static const char * const apb2_parents[] = { "osc32k", "osc24M",
+					     "pll-periph" , "pll-periph" };
+static SUNXI_CCU_MP_WITH_MUX(apb2_clk, "apb2", apb2_parents, 0x058,
+			     0, 5,	/* M */
+			     16, 2,	/* P */
+			     24, 2,	/* mux */
+			     0);
+
+static SUNXI_CCU_GATE(bus_mipi_dsi_clk,	"bus-mipi-dsi",	"ahb1",
+		      0x060, BIT(1), 0);
+static SUNXI_CCU_GATE(bus_ss_clk,	"bus-ss",	"ahb1",
+		      0x060, BIT(5), 0);
+static SUNXI_CCU_GATE(bus_dma_clk,	"bus-dma",	"ahb1",
+		      0x060, BIT(6), 0);
+static SUNXI_CCU_GATE(bus_mmc0_clk,	"bus-mmc0",	"ahb1",
+		      0x060, BIT(8), 0);
+static SUNXI_CCU_GATE(bus_mmc1_clk,	"bus-mmc1",	"ahb1",
+		      0x060, BIT(9), 0);
+static SUNXI_CCU_GATE(bus_mmc2_clk,	"bus-mmc2",	"ahb1",
+		      0x060, BIT(10), 0);
+static SUNXI_CCU_GATE(bus_nand_clk,	"bus-nand",	"ahb1",
+		      0x060, BIT(13), 0);
+static SUNXI_CCU_GATE(bus_dram_clk,	"bus-dram",	"ahb1",
+		      0x060, BIT(14), 0);
+static SUNXI_CCU_GATE(bus_hstimer_clk,	"bus-hstimer",	"ahb1",
+		      0x060, BIT(19), 0);
+static SUNXI_CCU_GATE(bus_spi0_clk,	"bus-spi0",	"ahb1",
+		      0x060, BIT(20), 0);
+static SUNXI_CCU_GATE(bus_spi1_clk,	"bus-spi1",	"ahb1",
+		      0x060, BIT(21), 0);
+static SUNXI_CCU_GATE(bus_otg_clk,	"bus-otg",	"ahb1",
+		      0x060, BIT(24), 0);
+static SUNXI_CCU_GATE(bus_ehci_clk,	"bus-ehci",	"ahb1",
+		      0x060, BIT(26), 0);
+static SUNXI_CCU_GATE(bus_ohci_clk,	"bus-ohci",	"ahb1",
+		      0x060, BIT(29), 0);
+
+static SUNXI_CCU_GATE(bus_ve_clk,	"bus-ve",	"ahb1",
+		      0x064, BIT(0), 0);
+static SUNXI_CCU_GATE(bus_lcd_clk,	"bus-lcd",	"ahb1",
+		      0x064, BIT(4), 0);
+static SUNXI_CCU_GATE(bus_csi_clk,	"bus-csi",	"ahb1",
+		      0x064, BIT(8), 0);
+static SUNXI_CCU_GATE(bus_de_be_clk,	"bus-de-be",	"ahb1",
+		      0x064, BIT(12), 0);
+static SUNXI_CCU_GATE(bus_de_fe_clk,	"bus-de-fe",	"ahb1",
+		      0x064, BIT(14), 0);
+static SUNXI_CCU_GATE(bus_gpu_clk,	"bus-gpu",	"ahb1",
+		      0x064, BIT(20), 0);
+static SUNXI_CCU_GATE(bus_msgbox_clk,	"bus-msgbox",	"ahb1",
+		      0x064, BIT(21), 0);
+static SUNXI_CCU_GATE(bus_spinlock_clk,	"bus-spinlock",	"ahb1",
+		      0x064, BIT(22), 0);
+static SUNXI_CCU_GATE(bus_drc_clk,	"bus-drc",	"ahb1",
+		      0x064, BIT(25), 0);
+static SUNXI_CCU_GATE(bus_sat_clk,	"bus-sat",	"ahb1",
+		      0x064, BIT(26), 0);
+
+static SUNXI_CCU_GATE(bus_codec_clk,	"bus-codec",	"apb1",
+		      0x068, BIT(0), 0);
+static SUNXI_CCU_GATE(bus_pio_clk,	"bus-pio",	"apb1",
+		      0x068, BIT(5), 0);
+static SUNXI_CCU_GATE(bus_i2s0_clk,	"bus-i2s0",	"apb1",
+		      0x068, BIT(12), 0);
+static SUNXI_CCU_GATE(bus_i2s1_clk,	"bus-i2s1",	"apb1",
+		      0x068, BIT(13), 0);
+
+static SUNXI_CCU_GATE(bus_i2c0_clk,	"bus-i2c0",	"apb2",
+		      0x06c, BIT(0), 0);
+static SUNXI_CCU_GATE(bus_i2c1_clk,	"bus-i2c1",	"apb2",
+		      0x06c, BIT(1), 0);
+static SUNXI_CCU_GATE(bus_i2c2_clk,	"bus-i2c2",	"apb2",
+		      0x06c, BIT(2), 0);
+static SUNXI_CCU_GATE(bus_uart0_clk,	"bus-uart0",	"apb2",
+		      0x06c, BIT(16), 0);
+static SUNXI_CCU_GATE(bus_uart1_clk,	"bus-uart1",	"apb2",
+		      0x06c, BIT(17), 0);
+static SUNXI_CCU_GATE(bus_uart2_clk,	"bus-uart2",	"apb2",
+		      0x06c, BIT(18), 0);
+static SUNXI_CCU_GATE(bus_uart3_clk,	"bus-uart3",	"apb2",
+		      0x06c, BIT(19), 0);
+static SUNXI_CCU_GATE(bus_uart4_clk,	"bus-uart4",	"apb2",
+		      0x06c, BIT(20), 0);
+
+static const char * const mod0_default_parents[] = { "osc24M", "pll-periph" };
+static SUNXI_CCU_MP_WITH_MUX_GATE(nand_clk, "nand", mod0_default_parents, 0x080,
+				  0, 4,		/* M */
+				  16, 2,	/* P */
+				  24, 2,	/* mux */
+				  BIT(31),	/* gate */
+				  0);
+
+static SUNXI_CCU_MP_WITH_MUX_GATE(mmc0_clk, "mmc0", mod0_default_parents, 0x088,
+				  0, 4,		/* M */
+				  16, 2,	/* P */
+				  24, 2,	/* mux */
+				  BIT(31),	/* gate */
+				  0);
+
+static SUNXI_CCU_PHASE(mmc0_sample_clk, "mmc0_sample", "mmc0",
+		       0x088, 20, 3, 0);
+static SUNXI_CCU_PHASE(mmc0_output_clk, "mmc0_output", "mmc0",
+		       0x088, 8, 3, 0);
+
+static SUNXI_CCU_MP_WITH_MUX_GATE(mmc1_clk, "mmc1", mod0_default_parents, 0x08c,
+				  0, 4,		/* M */
+				  16, 2,	/* P */
+				  24, 2,	/* mux */
+				  BIT(31),	/* gate */
+				  0);
+
+static SUNXI_CCU_PHASE(mmc1_sample_clk, "mmc1_sample", "mmc1",
+		       0x08c, 20, 3, 0);
+static SUNXI_CCU_PHASE(mmc1_output_clk, "mmc1_output", "mmc1",
+		       0x08c, 8, 3, 0);
+
+static SUNXI_CCU_MP_WITH_MUX_GATE(mmc2_clk, "mmc2", mod0_default_parents, 0x090,
+				  0, 4,		/* M */
+				  16, 2,	/* P */
+				  24, 2,	/* mux */
+				  BIT(31),	/* gate */
+				  0);
+
+static SUNXI_CCU_PHASE(mmc2_sample_clk, "mmc2_sample", "mmc2",
+		       0x090, 20, 3, 0);
+static SUNXI_CCU_PHASE(mmc2_output_clk, "mmc2_output", "mmc2",
+		       0x090, 8, 3, 0);
+
+static SUNXI_CCU_MP_WITH_MUX_GATE(ss_clk, "ss", mod0_default_parents, 0x09c,
+				  0, 4,		/* M */
+				  16, 2,	/* P */
+				  24, 2,	/* mux */
+				  BIT(31),	/* gate */
+				  0);
+
+static SUNXI_CCU_MP_WITH_MUX_GATE(spi0_clk, "spi0", mod0_default_parents, 0x0a0,
+				  0, 4,		/* M */
+				  16, 2,	/* P */
+				  24, 2,	/* mux */
+				  BIT(31),	/* gate */
+				  0);
+
+static SUNXI_CCU_MP_WITH_MUX_GATE(spi1_clk, "spi1", mod0_default_parents, 0x0a4,
+				  0, 4,		/* M */
+				  16, 2,	/* P */
+				  24, 2,	/* mux */
+				  BIT(31),	/* gate */
+				  0);
+
+static const char * const i2s_parents[] = { "pll-audio-8x", "pll-audio-4x",
+					    "pll-audio-2x", "pll-audio" };
+static SUNXI_CCU_MUX_WITH_GATE(i2s0_clk, "i2s0", i2s_parents,
+			       0x0b0, 16, 2, BIT(31), 0);
+
+static SUNXI_CCU_MUX_WITH_GATE(i2s1_clk, "i2s1", i2s_parents,
+			       0x0b4, 16, 2, BIT(31), 0);
+
+/* TODO: the parent for most of the USB clocks is not known */
+static SUNXI_CCU_GATE(usb_phy0_clk,	"usb-phy0",	"osc24M",
+		      0x0cc, BIT(8), 0);
+static SUNXI_CCU_GATE(usb_phy1_clk,	"usb-phy1",	"osc24M",
+		      0x0cc, BIT(9), 0);
+static SUNXI_CCU_GATE(usb_hsic_clk,	"usb-hsic",	"pll-hsic",
+		      0x0cc, BIT(10), 0);
+static SUNXI_CCU_GATE(usb_hsic_12M_clk,	"usb-hsic-12M",	"osc24M",
+		      0x0cc, BIT(11), 0);
+static SUNXI_CCU_GATE(usb_ohci_clk,	"usb-ohci",	"osc24M",
+		      0x0cc, BIT(16), 0);
+
+static SUNXI_CCU_M(dram_clk, "dram", "pll-ddr",
+		   0x0f4, 0, 4, CLK_IS_CRITICAL);
+
+static const char * const pll_ddr_parents[] = { "pll-ddr0", "pll-ddr1" };
+static SUNXI_CCU_MUX(pll_ddr_clk, "pll-ddr", pll_ddr_parents,
+		     0x0f8, 16, 1, 0);
+
+static SUNXI_CCU_GATE(dram_ve_clk,	"dram-ve",	"dram",
+		      0x100, BIT(0), 0);
+static SUNXI_CCU_GATE(dram_csi_clk,	"dram-csi",	"dram",
+		      0x100, BIT(1), 0);
+static SUNXI_CCU_GATE(dram_drc_clk,	"dram-drc",	"dram",
+		      0x100, BIT(16), 0);
+static SUNXI_CCU_GATE(dram_de_fe_clk,	"dram-de-fe",	"dram",
+		      0x100, BIT(24), 0);
+static SUNXI_CCU_GATE(dram_de_be_clk,	"dram-de-be",	"dram",
+		      0x100, BIT(26), 0);
+
+static const char * const de_parents[] = { "pll-video", "pll-periph-2x",
+					   "pll-gpu", "pll-de" };
+static const u8 de_table[] = { 0, 2, 3, 5 };
+static SUNXI_CCU_M_WITH_MUX_TABLE_GATE(de_be_clk, "de-be",
+				       de_parents, de_table,
+				       0x104, 0, 4, 24, 3, BIT(31), 0);
+
+static SUNXI_CCU_M_WITH_MUX_TABLE_GATE(de_fe_clk, "de-fe",
+				       de_parents, de_table,
+				       0x10c, 0, 4, 24, 3, BIT(31), 0);
+
+static const char * const lcd_ch0_parents[] = { "pll-video", "pll-video-2x",
+						"pll-mipi" };
+static const u8 lcd_ch0_table[] = { 0, 2, 4 };
+static SUNXI_CCU_MUX_TABLE_WITH_GATE(lcd_ch0_clk, "lcd-ch0",
+				     lcd_ch0_parents, lcd_ch0_table,
+				     0x118, 24, 3, BIT(31),
+				     CLK_SET_RATE_PARENT);
+
+static const char * const lcd_ch1_parents[] = { "pll-video", "pll-video-2x" };
+static const u8 lcd_ch1_table[] = { 0, 2 };
+static SUNXI_CCU_M_WITH_MUX_TABLE_GATE(lcd_ch1_clk, "lcd-ch1",
+				       lcd_ch1_parents, lcd_ch1_table,
+				       0x12c, 0, 4, 24, 2, BIT(31), 0);
+
+static const char * const csi_sclk_parents[] = { "pll-video", "pll-de",
+						 "pll-mipi", "pll-ve" };
+static const u8 csi_sclk_table[] = { 0, 3, 4, 5 };
+static SUNXI_CCU_M_WITH_MUX_TABLE_GATE(csi_sclk_clk, "csi-sclk",
+				       csi_sclk_parents, csi_sclk_table,
+				       0x134, 16, 4, 24, 3, BIT(31), 0);
+
+static const char * const csi_mclk_parents[] = { "pll-video", "pll-de",
+						 "osc24M" };
+static const u8 csi_mclk_table[] = { 0, 3, 5 };
+static SUNXI_CCU_M_WITH_MUX_TABLE_GATE(csi_mclk_clk, "csi-mclk",
+				       csi_mclk_parents, csi_mclk_table,
+				       0x134, 0, 5, 8, 3, BIT(15), 0);
+
+static SUNXI_CCU_M_WITH_GATE(ve_clk, "ve", "pll-ve",
+			     0x13c, 16, 3, BIT(31), CLK_SET_RATE_PARENT);
+
+static SUNXI_CCU_GATE(ac_dig_clk,	"ac-dig",	"pll-audio",
+		      0x140, BIT(31), 0);
+static SUNXI_CCU_GATE(ac_dig_4x_clk,	"ac-dig-4x",	"pll-audio-4x",
+		      0x140, BIT(30), 0);
+static SUNXI_CCU_GATE(avs_clk,		"avs",		"osc24M",
+		      0x144, BIT(31), 0);
+
+static const char * const mbus_parents[] = { "osc24M", "pll-periph-2x",
+					     "pll-ddr0", "pll-ddr1" };
+static SUNXI_CCU_M_WITH_MUX_GATE(mbus_clk, "mbus", mbus_parents,
+				 0x15c, 0, 3, 24, 2, BIT(31), CLK_IS_CRITICAL);
+
+static const char * const dsi_sclk_parents[] = { "pll-video", "pll-video-2x" };
+static const u8 dsi_sclk_table[] = { 0, 2 };
+static SUNXI_CCU_M_WITH_MUX_TABLE_GATE(dsi_sclk_clk, "dsi-sclk",
+				       dsi_sclk_parents, dsi_sclk_table,
+				       0x168, 16, 4, 24, 2, BIT(31), 0);
+
+static const char * const dsi_dphy_parents[] = { "pll-video", "pll-periph" };
+static const u8 dsi_dphy_table[] = { 0, 2 };
+static SUNXI_CCU_M_WITH_MUX_TABLE_GATE(dsi_dphy_clk, "dsi-dphy",
+				       dsi_dphy_parents, dsi_dphy_table,
+				       0x168, 0, 4, 8, 2, BIT(15), 0);
+
+static SUNXI_CCU_M_WITH_MUX_TABLE_GATE(drc_clk, "drc",
+				       de_parents, de_table,
+				       0x180, 0, 4, 24, 3, BIT(31), 0);
+
+static SUNXI_CCU_M_WITH_GATE(gpu_clk, "gpu", "pll-gpu",
+			     0x1a0, 0, 3, BIT(31), 0);
+
+static const char * const ats_parents[] = { "osc24M", "pll-periph" };
+static SUNXI_CCU_M_WITH_MUX_GATE(ats_clk, "ats", ats_parents,
+				 0x1b0, 0, 3, 24, 2, BIT(31), 0);
+
+static struct ccu_common *sun8i_a33_ccu_clks[] = {
+	&pll_cpux_clk.common,
+	&pll_audio_base_clk.common,
+	&pll_video_clk.common,
+	&pll_ve_clk.common,
+	&pll_ddr0_clk.common,
+	&pll_periph_clk.common,
+	&pll_gpu_clk.common,
+	&pll_mipi_clk.common,
+	&pll_hsic_clk.common,
+	&pll_de_clk.common,
+	&pll_ddr1_clk.common,
+	&pll_ddr_clk.common,
+	&cpux_clk.common,
+	&axi_clk.common,
+	&ahb1_clk.common,
+	&apb1_clk.common,
+	&apb2_clk.common,
+	&bus_mipi_dsi_clk.common,
+	&bus_ss_clk.common,
+	&bus_dma_clk.common,
+	&bus_mmc0_clk.common,
+	&bus_mmc1_clk.common,
+	&bus_mmc2_clk.common,
+	&bus_nand_clk.common,
+	&bus_dram_clk.common,
+	&bus_hstimer_clk.common,
+	&bus_spi0_clk.common,
+	&bus_spi1_clk.common,
+	&bus_otg_clk.common,
+	&bus_ehci_clk.common,
+	&bus_ohci_clk.common,
+	&bus_ve_clk.common,
+	&bus_lcd_clk.common,
+	&bus_csi_clk.common,
+	&bus_de_fe_clk.common,
+	&bus_de_be_clk.common,
+	&bus_gpu_clk.common,
+	&bus_msgbox_clk.common,
+	&bus_spinlock_clk.common,
+	&bus_drc_clk.common,
+	&bus_sat_clk.common,
+	&bus_codec_clk.common,
+	&bus_pio_clk.common,
+	&bus_i2s0_clk.common,
+	&bus_i2s1_clk.common,
+	&bus_i2c0_clk.common,
+	&bus_i2c1_clk.common,
+	&bus_i2c2_clk.common,
+	&bus_uart0_clk.common,
+	&bus_uart1_clk.common,
+	&bus_uart2_clk.common,
+	&bus_uart3_clk.common,
+	&bus_uart4_clk.common,
+	&nand_clk.common,
+	&mmc0_clk.common,
+	&mmc0_sample_clk.common,
+	&mmc0_output_clk.common,
+	&mmc1_clk.common,
+	&mmc1_sample_clk.common,
+	&mmc1_output_clk.common,
+	&mmc2_clk.common,
+	&mmc2_sample_clk.common,
+	&mmc2_output_clk.common,
+	&ss_clk.common,
+	&spi0_clk.common,
+	&spi1_clk.common,
+	&i2s0_clk.common,
+	&i2s1_clk.common,
+	&usb_phy0_clk.common,
+	&usb_phy1_clk.common,
+	&usb_hsic_clk.common,
+	&usb_hsic_12M_clk.common,
+	&usb_ohci_clk.common,
+	&dram_clk.common,
+	&dram_ve_clk.common,
+	&dram_csi_clk.common,
+	&dram_drc_clk.common,
+	&dram_de_fe_clk.common,
+	&dram_de_be_clk.common,
+	&de_be_clk.common,
+	&de_fe_clk.common,
+	&lcd_ch0_clk.common,
+	&lcd_ch1_clk.common,
+	&csi_sclk_clk.common,
+	&csi_mclk_clk.common,
+	&ve_clk.common,
+	&ac_dig_clk.common,
+	&ac_dig_4x_clk.common,
+	&avs_clk.common,
+	&mbus_clk.common,
+	&dsi_sclk_clk.common,
+	&dsi_dphy_clk.common,
+	&drc_clk.common,
+	&gpu_clk.common,
+	&ats_clk.common,
+};
+
+/* We hardcode the divider to 4 for now */
+static CLK_FIXED_FACTOR(pll_audio_clk, "pll-audio",
+			"pll-audio-base", 4, 1, CLK_SET_RATE_PARENT);
+static CLK_FIXED_FACTOR(pll_audio_2x_clk, "pll-audio-2x",
+			"pll-audio-base", 2, 1, CLK_SET_RATE_PARENT);
+static CLK_FIXED_FACTOR(pll_audio_4x_clk, "pll-audio-4x",
+			"pll-audio-base", 1, 1, CLK_SET_RATE_PARENT);
+static CLK_FIXED_FACTOR(pll_audio_8x_clk, "pll-audio-8x",
+			"pll-audio-base", 1, 2, CLK_SET_RATE_PARENT);
+static CLK_FIXED_FACTOR(pll_periph_2x_clk, "pll-periph-2x",
+			"pll-periph", 1, 2, 0);
+static CLK_FIXED_FACTOR(pll_video_2x_clk, "pll-video-2x",
+			"pll-video", 1, 2, 0);
+
+static struct clk_hw_onecell_data sun8i_a33_hw_clks = {
+	.hws	= {
+		[CLK_PLL_CPUX]		= &pll_cpux_clk.common.hw,
+		[CLK_PLL_AUDIO_BASE]	= &pll_audio_base_clk.common.hw,
+		[CLK_PLL_AUDIO]		= &pll_audio_clk.hw,
+		[CLK_PLL_AUDIO_2X]	= &pll_audio_2x_clk.hw,
+		[CLK_PLL_AUDIO_4X]	= &pll_audio_4x_clk.hw,
+		[CLK_PLL_AUDIO_8X]	= &pll_audio_8x_clk.hw,
+		[CLK_PLL_VIDEO]		= &pll_video_clk.common.hw,
+		[CLK_PLL_VIDEO_2X]	= &pll_video_2x_clk.hw,
+		[CLK_PLL_VE]		= &pll_ve_clk.common.hw,
+		[CLK_PLL_DDR0]		= &pll_ddr0_clk.common.hw,
+		[CLK_PLL_PERIPH]	= &pll_periph_clk.common.hw,
+		[CLK_PLL_PERIPH_2X]	= &pll_periph_2x_clk.hw,
+		[CLK_PLL_GPU]		= &pll_gpu_clk.common.hw,
+		[CLK_PLL_MIPI]		= &pll_mipi_clk.common.hw,
+		[CLK_PLL_HSIC]		= &pll_hsic_clk.common.hw,
+		[CLK_PLL_DE]		= &pll_de_clk.common.hw,
+		[CLK_PLL_DDR1]		= &pll_ddr1_clk.common.hw,
+		[CLK_PLL_DDR]		= &pll_ddr_clk.common.hw,
+		[CLK_CPUX]		= &cpux_clk.common.hw,
+		[CLK_AXI]		= &axi_clk.common.hw,
+		[CLK_AHB1]		= &ahb1_clk.common.hw,
+		[CLK_APB1]		= &apb1_clk.common.hw,
+		[CLK_APB2]		= &apb2_clk.common.hw,
+		[CLK_BUS_MIPI_DSI]	= &bus_mipi_dsi_clk.common.hw,
+		[CLK_BUS_SS]		= &bus_ss_clk.common.hw,
+		[CLK_BUS_DMA]		= &bus_dma_clk.common.hw,
+		[CLK_BUS_MMC0]		= &bus_mmc0_clk.common.hw,
+		[CLK_BUS_MMC1]		= &bus_mmc1_clk.common.hw,
+		[CLK_BUS_MMC2]		= &bus_mmc2_clk.common.hw,
+		[CLK_BUS_NAND]		= &bus_nand_clk.common.hw,
+		[CLK_BUS_DRAM]		= &bus_dram_clk.common.hw,
+		[CLK_BUS_HSTIMER]	= &bus_hstimer_clk.common.hw,
+		[CLK_BUS_SPI0]		= &bus_spi0_clk.common.hw,
+		[CLK_BUS_SPI1]		= &bus_spi1_clk.common.hw,
+		[CLK_BUS_OTG]		= &bus_otg_clk.common.hw,
+		[CLK_BUS_EHCI]		= &bus_ehci_clk.common.hw,
+		[CLK_BUS_OHCI]		= &bus_ohci_clk.common.hw,
+		[CLK_BUS_VE]		= &bus_ve_clk.common.hw,
+		[CLK_BUS_LCD]		= &bus_lcd_clk.common.hw,
+		[CLK_BUS_CSI]		= &bus_csi_clk.common.hw,
+		[CLK_BUS_DE_BE]		= &bus_de_be_clk.common.hw,
+		[CLK_BUS_DE_FE]		= &bus_de_fe_clk.common.hw,
+		[CLK_BUS_GPU]		= &bus_gpu_clk.common.hw,
+		[CLK_BUS_MSGBOX]	= &bus_msgbox_clk.common.hw,
+		[CLK_BUS_SPINLOCK]	= &bus_spinlock_clk.common.hw,
+		[CLK_BUS_DRC]		= &bus_drc_clk.common.hw,
+		[CLK_BUS_SAT]		= &bus_sat_clk.common.hw,
+		[CLK_BUS_CODEC]		= &bus_codec_clk.common.hw,
+		[CLK_BUS_PIO]		= &bus_pio_clk.common.hw,
+		[CLK_BUS_I2S0]		= &bus_i2s0_clk.common.hw,
+		[CLK_BUS_I2S1]		= &bus_i2s1_clk.common.hw,
+		[CLK_BUS_I2C0]		= &bus_i2c0_clk.common.hw,
+		[CLK_BUS_I2C1]		= &bus_i2c1_clk.common.hw,
+		[CLK_BUS_I2C2]		= &bus_i2c2_clk.common.hw,
+		[CLK_BUS_UART0]		= &bus_uart0_clk.common.hw,
+		[CLK_BUS_UART1]		= &bus_uart1_clk.common.hw,
+		[CLK_BUS_UART2]		= &bus_uart2_clk.common.hw,
+		[CLK_BUS_UART3]		= &bus_uart3_clk.common.hw,
+		[CLK_BUS_UART4]		= &bus_uart4_clk.common.hw,
+		[CLK_NAND]		= &nand_clk.common.hw,
+		[CLK_MMC0]		= &mmc0_clk.common.hw,
+		[CLK_MMC0_SAMPLE]	= &mmc0_sample_clk.common.hw,
+		[CLK_MMC0_OUTPUT]	= &mmc0_output_clk.common.hw,
+		[CLK_MMC1]		= &mmc1_clk.common.hw,
+		[CLK_MMC1_SAMPLE]	= &mmc1_sample_clk.common.hw,
+		[CLK_MMC1_OUTPUT]	= &mmc1_output_clk.common.hw,
+		[CLK_MMC2]		= &mmc2_clk.common.hw,
+		[CLK_MMC2_SAMPLE]	= &mmc2_sample_clk.common.hw,
+		[CLK_MMC2_OUTPUT]	= &mmc2_output_clk.common.hw,
+		[CLK_SS]		= &ss_clk.common.hw,
+		[CLK_SPI0]		= &spi0_clk.common.hw,
+		[CLK_SPI1]		= &spi1_clk.common.hw,
+		[CLK_I2S0]		= &i2s0_clk.common.hw,
+		[CLK_I2S1]		= &i2s1_clk.common.hw,
+		[CLK_USB_PHY0]		= &usb_phy0_clk.common.hw,
+		[CLK_USB_PHY1]		= &usb_phy1_clk.common.hw,
+		[CLK_USB_HSIC]		= &usb_hsic_clk.common.hw,
+		[CLK_USB_HSIC_12M]	= &usb_hsic_12M_clk.common.hw,
+		[CLK_USB_OHCI]		= &usb_ohci_clk.common.hw,
+		[CLK_DRAM]		= &dram_clk.common.hw,
+		[CLK_DRAM_VE]		= &dram_ve_clk.common.hw,
+		[CLK_DRAM_CSI]		= &dram_csi_clk.common.hw,
+		[CLK_DRAM_DRC]		= &dram_drc_clk.common.hw,
+		[CLK_DRAM_DE_FE]	= &dram_de_fe_clk.common.hw,
+		[CLK_DRAM_DE_BE]	= &dram_de_be_clk.common.hw,
+		[CLK_DE_BE]		= &de_be_clk.common.hw,
+		[CLK_DE_FE]		= &de_fe_clk.common.hw,
+		[CLK_LCD_CH0]		= &lcd_ch0_clk.common.hw,
+		[CLK_LCD_CH1]		= &lcd_ch1_clk.common.hw,
+		[CLK_CSI_SCLK]		= &csi_sclk_clk.common.hw,
+		[CLK_CSI_MCLK]		= &csi_mclk_clk.common.hw,
+		[CLK_VE]		= &ve_clk.common.hw,
+		[CLK_AC_DIG]		= &ac_dig_clk.common.hw,
+		[CLK_AC_DIG_4X]		= &ac_dig_4x_clk.common.hw,
+		[CLK_AVS]		= &avs_clk.common.hw,
+		[CLK_MBUS]		= &mbus_clk.common.hw,
+		[CLK_DSI_SCLK]		= &dsi_sclk_clk.common.hw,
+		[CLK_DSI_DPHY]		= &dsi_dphy_clk.common.hw,
+		[CLK_DRC]		= &drc_clk.common.hw,
+		[CLK_GPU]		= &gpu_clk.common.hw,
+		[CLK_ATS]		= &ats_clk.common.hw,
+	},
+	.num	= CLK_NUMBER,
+};
+
+static struct ccu_reset_map sun8i_a33_ccu_resets[] = {
+	[RST_USB_PHY0]		=  { 0x0cc, BIT(0) },
+	[RST_USB_PHY1]		=  { 0x0cc, BIT(1) },
+	[RST_USB_HSIC]		=  { 0x0cc, BIT(2) },
+
+	[RST_MBUS]		=  { 0x0fc, BIT(31) },
+
+	[RST_BUS_MIPI_DSI]	=  { 0x2c0, BIT(1) },
+	[RST_BUS_SS]		=  { 0x2c0, BIT(5) },
+	[RST_BUS_DMA]		=  { 0x2c0, BIT(6) },
+	[RST_BUS_MMC0]		=  { 0x2c0, BIT(8) },
+	[RST_BUS_MMC1]		=  { 0x2c0, BIT(9) },
+	[RST_BUS_MMC2]		=  { 0x2c0, BIT(10) },
+	[RST_BUS_NAND]		=  { 0x2c0, BIT(13) },
+	[RST_BUS_DRAM]		=  { 0x2c0, BIT(14) },
+	[RST_BUS_HSTIMER]	=  { 0x2c0, BIT(19) },
+	[RST_BUS_SPI0]		=  { 0x2c0, BIT(20) },
+	[RST_BUS_SPI1]		=  { 0x2c0, BIT(21) },
+	[RST_BUS_OTG]		=  { 0x2c0, BIT(24) },
+	[RST_BUS_EHCI]		=  { 0x2c0, BIT(26) },
+	[RST_BUS_OHCI]		=  { 0x2c0, BIT(29) },
+
+	[RST_BUS_VE]		=  { 0x2c4, BIT(0) },
+	[RST_BUS_LCD]		=  { 0x2c4, BIT(4) },
+	[RST_BUS_CSI]		=  { 0x2c4, BIT(8) },
+	[RST_BUS_DE_BE]		=  { 0x2c4, BIT(12) },
+	[RST_BUS_DE_FE]		=  { 0x2c4, BIT(14) },
+	[RST_BUS_GPU]		=  { 0x2c4, BIT(20) },
+	[RST_BUS_MSGBOX]	=  { 0x2c4, BIT(21) },
+	[RST_BUS_SPINLOCK]	=  { 0x2c4, BIT(22) },
+	[RST_BUS_DRC]		=  { 0x2c4, BIT(25) },
+	[RST_BUS_SAT]		=  { 0x2c4, BIT(26) },
+
+	[RST_BUS_LVDS]		=  { 0x2c8, BIT(0) },
+
+	[RST_BUS_CODEC]		=  { 0x2d0, BIT(0) },
+	[RST_BUS_I2S0]		=  { 0x2d0, BIT(12) },
+	[RST_BUS_I2S1]		=  { 0x2d0, BIT(13) },
+
+	[RST_BUS_I2C0]		=  { 0x2d8, BIT(0) },
+	[RST_BUS_I2C1]		=  { 0x2d8, BIT(1) },
+	[RST_BUS_I2C2]		=  { 0x2d8, BIT(2) },
+	[RST_BUS_UART0]		=  { 0x2d8, BIT(16) },
+	[RST_BUS_UART1]		=  { 0x2d8, BIT(17) },
+	[RST_BUS_UART2]		=  { 0x2d8, BIT(18) },
+	[RST_BUS_UART3]		=  { 0x2d8, BIT(19) },
+	[RST_BUS_UART4]		=  { 0x2d8, BIT(20) },
+};
+
+static const struct sunxi_ccu_desc sun8i_a33_ccu_desc = {
+	.ccu_clks	= sun8i_a33_ccu_clks,
+	.num_ccu_clks	= ARRAY_SIZE(sun8i_a33_ccu_clks),
+
+	.hw_clks	= &sun8i_a33_hw_clks,
+
+	.resets		= sun8i_a33_ccu_resets,
+	.num_resets	= ARRAY_SIZE(sun8i_a33_ccu_resets),
+};
+
+static void __init sun8i_a33_ccu_setup(struct device_node *node)
+{
+	void __iomem *reg;
+	u32 val;
+
+	reg = of_io_request_and_map(node, 0, of_node_full_name(node));
+	if (IS_ERR(reg)) {
+		pr_err("%s: Could not map the clock registers\n",
+		       of_node_full_name(node));
+		return;
+	}
+
+	/* Force the PLL-Audio-1x divider to 4 */
+	val = readl(reg + SUN8I_A33_PLL_AUDIO_REG);
+	val &= ~GENMASK(19, 16);
+	writel(val | (3 << 16), reg + SUN8I_A33_PLL_AUDIO_REG);
+
+	/* Force PLL-MIPI to MIPI mode */
+	val = readl(reg + SUN8I_A33_PLL_MIPI_REG);
+	val &= ~BIT(16);
+	writel(val, reg + SUN8I_A33_PLL_MIPI_REG);
+
+	sunxi_ccu_probe(node, reg, &sun8i_a33_ccu_desc);
+}
+CLK_OF_DECLARE(sun8i_a33_ccu, "allwinner,sun8i-a33-ccu",
+	       sun8i_a33_ccu_setup);
diff --git a/drivers/clk/sunxi-ng/ccu-sun8i-h3.c b/drivers/clk/sunxi-ng/ccu-sun8i-h3.c
index 267f99523fbe..4d70590f05e3 100644
--- a/drivers/clk/sunxi-ng/ccu-sun8i-h3.c
+++ b/drivers/clk/sunxi-ng/ccu-sun8i-h3.c
@@ -184,15 +184,15 @@ static SUNXI_CCU_MP_WITH_MUX(apb2_clk, "apb2", apb2_parents, 0x058,
 			     0);
 
 static const char * const ahb2_parents[] = { "ahb1" , "pll-periph0" };
+static const struct ccu_mux_fixed_prediv ahb2_fixed_predivs[] = {
+	{ .index = 1, .div = 2 },
+};
 static struct ccu_mux ahb2_clk = {
 	.mux		= {
 		.shift	= 0,
 		.width	= 1,
-
-		.fixed_prediv	= {
-			.index	= 1,
-			.div	= 2,
-		},
+		.fixed_predivs	= ahb2_fixed_predivs,
+		.n_predivs	= ARRAY_SIZE(ahb2_fixed_predivs),
 	},
 
 	.common		= {
diff --git a/drivers/clk/sunxi-ng/ccu_div.h b/drivers/clk/sunxi-ng/ccu_div.h
index 653ade5769b3..34c338832c0d 100644
--- a/drivers/clk/sunxi-ng/ccu_div.h
+++ b/drivers/clk/sunxi-ng/ccu_div.h
@@ -19,10 +19,29 @@
 #include "ccu_common.h"
 #include "ccu_mux.h"
 
+/**
+ * struct _ccu_div - Internal divider description
+ * @shift: Bit offset of the divider in its register
+ * @width: Width of the divider field in its register
+ * @max: Maximum value allowed for that divider. This is the
+ *       arithmetic value, not the maximum value to be set in the
+ *       register.
+ * @flags: clk_divider flags to apply on this divider
+ * @table: Divider table pointer (if applicable)
+ *
+ * That structure represents a single divider, and is meant to be
+ * embedded in other structures representing the various clock
+ * classes.
+ *
+ * It is basically a wrapper around the clk_divider functions
+ * arguments.
+ */
 struct _ccu_div {
 	u8			shift;
 	u8			width;
 
+	u32			max;
+
 	u32			flags;
 
 	struct clk_div_table	*table;
@@ -36,14 +55,25 @@ struct _ccu_div {
 		.table	= _table,					\
 	}
 
-#define _SUNXI_CCU_DIV_FLAGS(_shift, _width, _flags)			\
-	_SUNXI_CCU_DIV_TABLE_FLAGS(_shift, _width, NULL, _flags)
-
 #define _SUNXI_CCU_DIV_TABLE(_shift, _width, _table)			\
 	_SUNXI_CCU_DIV_TABLE_FLAGS(_shift, _width, _table, 0)
 
+#define _SUNXI_CCU_DIV_MAX_FLAGS(_shift, _width, _max, _flags) \
+	{								\
+		.shift	= _shift,					\
+		.width	= _width,					\
+		.flags	= _flags,					\
+		.max	= _max,						\
+	}
+
+#define _SUNXI_CCU_DIV_FLAGS(_shift, _width, _flags)			\
+	_SUNXI_CCU_DIV_MAX_FLAGS(_shift, _width, 0, _flags)
+
+#define _SUNXI_CCU_DIV_MAX(_shift, _width, _max)			\
+	_SUNXI_CCU_DIV_MAX_FLAGS(_shift, _width, _max, 0)
+
 #define _SUNXI_CCU_DIV(_shift, _width)					\
-	_SUNXI_CCU_DIV_TABLE_FLAGS(_shift, _width, NULL, 0)
+	_SUNXI_CCU_DIV_FLAGS(_shift, _width, 0)
 
 struct ccu_div {
 	u32			enable;
@@ -77,13 +107,16 @@ struct ccu_div {
 				      _shift, _width, _table, 0,	\
 				      _flags)
 
-#define SUNXI_CCU_M_WITH_MUX_GATE(_struct, _name, _parents, _reg,	\
-				  _mshift, _mwidth, _muxshift, _muxwidth, \
-				  _gate, _flags)			\
+#define SUNXI_CCU_M_WITH_MUX_TABLE_GATE(_struct, _name,			\
+					_parents, _table,		\
+					_reg,				\
+					_mshift, _mwidth,		\
+					_muxshift, _muxwidth,		\
+					_gate, _flags)			\
 	struct ccu_div _struct = {					\
 		.enable	= _gate,					\
 		.div	= _SUNXI_CCU_DIV(_mshift, _mwidth),		\
-		.mux	= SUNXI_CLK_MUX(_muxshift, _muxwidth),		\
+		.mux	= _SUNXI_CCU_MUX_TABLE(_muxshift, _muxwidth, _table), \
 		.common	= {						\
 			.reg		= _reg,				\
 			.hw.init	= CLK_HW_INIT_PARENTS(_name,	\
@@ -93,12 +126,23 @@ struct ccu_div {
 		},							\
 	}
 
+#define SUNXI_CCU_M_WITH_MUX_GATE(_struct, _name, _parents, _reg,	\
+				  _mshift, _mwidth, _muxshift, _muxwidth, \
+				  _gate, _flags)			\
+	SUNXI_CCU_M_WITH_MUX_TABLE_GATE(_struct, _name,			\
+					_parents, NULL,			\
+					_reg, _mshift, _mwidth,		\
+					_muxshift, _muxwidth,		\
+					_gate, _flags)
+
 #define SUNXI_CCU_M_WITH_MUX(_struct, _name, _parents, _reg,		\
 			     _mshift, _mwidth, _muxshift, _muxwidth,	\
 			     _flags)					\
-	SUNXI_CCU_M_WITH_MUX_GATE(_struct, _name, _parents, _reg,	\
-				  _mshift, _mwidth, _muxshift, _muxwidth, \
-				  0, _flags)
+	SUNXI_CCU_M_WITH_MUX_TABLE_GATE(_struct, _name,			\
+					_parents, NULL,			\
+					_reg, _mshift, _mwidth,		\
+					_muxshift, _muxwidth,		\
+					0, _flags)
 
 
 #define SUNXI_CCU_M_WITH_GATE(_struct, _name, _parent, _reg,		\
diff --git a/drivers/clk/sunxi-ng/ccu_mp.c b/drivers/clk/sunxi-ng/ccu_mp.c
index cbf33ef5faa9..ebb1b31568a5 100644
--- a/drivers/clk/sunxi-ng/ccu_mp.c
+++ b/drivers/clk/sunxi-ng/ccu_mp.c
@@ -21,9 +21,9 @@ static void ccu_mp_find_best(unsigned long parent, unsigned long rate,
 	unsigned int best_m = 0, best_p = 0;
 	unsigned int _m, _p;
 
-	for (_p = 0; _p <= max_p; _p++) {
+	for (_p = 1; _p <= max_p; _p <<= 1) {
 		for (_m = 1; _m <= max_m; _m++) {
-			unsigned long tmp_rate = (parent >> _p) / _m;
+			unsigned long tmp_rate = parent / _p / _m;
 
 			if (tmp_rate > rate)
 				continue;
@@ -46,13 +46,15 @@ static unsigned long ccu_mp_round_rate(struct ccu_mux_internal *mux,
 				       void *data)
 {
 	struct ccu_mp *cmp = data;
+	unsigned int max_m, max_p;
 	unsigned int m, p;
 
-	ccu_mp_find_best(parent_rate, rate,
-			 1 << cmp->m.width, (1 << cmp->p.width) - 1,
-			 &m, &p);
+	max_m = cmp->m.max ?: 1 << cmp->m.width;
+	max_p = cmp->p.max ?: 1 << ((1 << cmp->p.width) - 1);
 
-	return (parent_rate >> p) / m;
+	ccu_mp_find_best(parent_rate, rate, max_m, max_p, &m, &p);
+
+	return parent_rate / p / m;
 }
 
 static void ccu_mp_disable(struct clk_hw *hw)
@@ -108,13 +110,14 @@ static int ccu_mp_set_rate(struct clk_hw *hw, unsigned long rate,
 {
 	struct ccu_mp *cmp = hw_to_ccu_mp(hw);
 	unsigned long flags;
+	unsigned int max_m, max_p;
 	unsigned int m, p;
 	u32 reg;
 
-	ccu_mp_find_best(parent_rate, rate,
-			 1 << cmp->m.width, (1 << cmp->p.width) - 1,
-			 &m, &p);
+	max_m = cmp->m.max ?: 1 << cmp->m.width;
+	max_p = cmp->p.max ?: 1 << ((1 << cmp->p.width) - 1);
 
+	ccu_mp_find_best(parent_rate, rate, max_m, max_p, &m, &p);
 
 	spin_lock_irqsave(cmp->common.lock, flags);
 
@@ -122,7 +125,7 @@ static int ccu_mp_set_rate(struct clk_hw *hw, unsigned long rate,
 	reg &= ~GENMASK(cmp->m.width + cmp->m.shift - 1, cmp->m.shift);
 	reg &= ~GENMASK(cmp->p.width + cmp->p.shift - 1, cmp->p.shift);
 
-	writel(reg | (p << cmp->p.shift) | ((m - 1) << cmp->m.shift),
+	writel(reg | (ilog2(p) << cmp->p.shift) | ((m - 1) << cmp->m.shift),
 	       cmp->common.base + cmp->common.reg);
 
 	spin_unlock_irqrestore(cmp->common.lock, flags);
diff --git a/drivers/clk/sunxi-ng/ccu_mp.h b/drivers/clk/sunxi-ng/ccu_mp.h
index 3cf12bf95962..edf9215ea8cc 100644
--- a/drivers/clk/sunxi-ng/ccu_mp.h
+++ b/drivers/clk/sunxi-ng/ccu_mp.h
@@ -44,7 +44,7 @@ struct ccu_mp {
 		.enable	= _gate,					\
 		.m	= _SUNXI_CCU_DIV(_mshift, _mwidth),		\
 		.p	= _SUNXI_CCU_DIV(_pshift, _pwidth),		\
-		.mux	= SUNXI_CLK_MUX(_muxshift, _muxwidth),		\
+		.mux	= _SUNXI_CCU_MUX(_muxshift, _muxwidth),		\
 		.common	= {						\
 			.reg		= _reg,				\
 			.hw.init	= CLK_HW_INIT_PARENTS(_name,	\
diff --git a/drivers/clk/sunxi-ng/ccu_mult.c b/drivers/clk/sunxi-ng/ccu_mult.c
new file mode 100644
index 000000000000..010e9424691d
--- /dev/null
+++ b/drivers/clk/sunxi-ng/ccu_mult.c
@@ -0,0 +1,133 @@
+/*
+ * Copyright (C) 2016 Maxime Ripard
+ * Maxime Ripard <maxime.ripard@free-electrons.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ */
+
+#include <linux/clk-provider.h>
+
+#include "ccu_gate.h"
+#include "ccu_mult.h"
+
+static void ccu_mult_find_best(unsigned long parent, unsigned long rate,
+			       unsigned int max_n, unsigned int *n)
+{
+	*n = rate / parent;
+}
+
+static unsigned long ccu_mult_round_rate(struct ccu_mux_internal *mux,
+					unsigned long parent_rate,
+					unsigned long rate,
+					void *data)
+{
+	struct ccu_mult *cm = data;
+	unsigned int n;
+
+	ccu_mult_find_best(parent_rate, rate, 1 << cm->mult.width, &n);
+
+	return parent_rate * n;
+}
+
+static void ccu_mult_disable(struct clk_hw *hw)
+{
+	struct ccu_mult *cm = hw_to_ccu_mult(hw);
+
+	return ccu_gate_helper_disable(&cm->common, cm->enable);
+}
+
+static int ccu_mult_enable(struct clk_hw *hw)
+{
+	struct ccu_mult *cm = hw_to_ccu_mult(hw);
+
+	return ccu_gate_helper_enable(&cm->common, cm->enable);
+}
+
+static int ccu_mult_is_enabled(struct clk_hw *hw)
+{
+	struct ccu_mult *cm = hw_to_ccu_mult(hw);
+
+	return ccu_gate_helper_is_enabled(&cm->common, cm->enable);
+}
+
+static unsigned long ccu_mult_recalc_rate(struct clk_hw *hw,
+					unsigned long parent_rate)
+{
+	struct ccu_mult *cm = hw_to_ccu_mult(hw);
+	unsigned long val;
+	u32 reg;
+
+	reg = readl(cm->common.base + cm->common.reg);
+	val = reg >> cm->mult.shift;
+	val &= (1 << cm->mult.width) - 1;
+
+	ccu_mux_helper_adjust_parent_for_prediv(&cm->common, &cm->mux, -1,
+						&parent_rate);
+
+	return parent_rate * (val + 1);
+}
+
+static int ccu_mult_determine_rate(struct clk_hw *hw,
+				struct clk_rate_request *req)
+{
+	struct ccu_mult *cm = hw_to_ccu_mult(hw);
+
+	return ccu_mux_helper_determine_rate(&cm->common, &cm->mux,
+					     req, ccu_mult_round_rate, cm);
+}
+
+static int ccu_mult_set_rate(struct clk_hw *hw, unsigned long rate,
+			   unsigned long parent_rate)
+{
+	struct ccu_mult *cm = hw_to_ccu_mult(hw);
+	unsigned long flags;
+	unsigned int n;
+	u32 reg;
+
+	ccu_mux_helper_adjust_parent_for_prediv(&cm->common, &cm->mux, -1,
+						&parent_rate);
+
+	ccu_mult_find_best(parent_rate, rate, 1 << cm->mult.width, &n);
+
+	spin_lock_irqsave(cm->common.lock, flags);
+
+	reg = readl(cm->common.base + cm->common.reg);
+	reg &= ~GENMASK(cm->mult.width + cm->mult.shift - 1, cm->mult.shift);
+
+	writel(reg | ((n - 1) << cm->mult.shift),
+	       cm->common.base + cm->common.reg);
+
+	spin_unlock_irqrestore(cm->common.lock, flags);
+
+	return 0;
+}
+
+static u8 ccu_mult_get_parent(struct clk_hw *hw)
+{
+	struct ccu_mult *cm = hw_to_ccu_mult(hw);
+
+	return ccu_mux_helper_get_parent(&cm->common, &cm->mux);
+}
+
+static int ccu_mult_set_parent(struct clk_hw *hw, u8 index)
+{
+	struct ccu_mult *cm = hw_to_ccu_mult(hw);
+
+	return ccu_mux_helper_set_parent(&cm->common, &cm->mux, index);
+}
+
+const struct clk_ops ccu_mult_ops = {
+	.disable	= ccu_mult_disable,
+	.enable		= ccu_mult_enable,
+	.is_enabled	= ccu_mult_is_enabled,
+
+	.get_parent	= ccu_mult_get_parent,
+	.set_parent	= ccu_mult_set_parent,
+
+	.determine_rate	= ccu_mult_determine_rate,
+	.recalc_rate	= ccu_mult_recalc_rate,
+	.set_rate	= ccu_mult_set_rate,
+};
diff --git a/drivers/clk/sunxi-ng/ccu_mult.h b/drivers/clk/sunxi-ng/ccu_mult.h
index 609db6610880..5d2c8dc14073 100644
--- a/drivers/clk/sunxi-ng/ccu_mult.h
+++ b/drivers/clk/sunxi-ng/ccu_mult.h
@@ -1,6 +1,9 @@
 #ifndef _CCU_MULT_H_
 #define _CCU_MULT_H_
 
+#include "ccu_common.h"
+#include "ccu_mux.h"
+
 struct _ccu_mult {
 	u8	shift;
 	u8	width;
@@ -12,4 +15,36 @@ struct _ccu_mult {
 		.width	= _width,		\
 	}
 
+struct ccu_mult {
+	u32			enable;
+
+	struct _ccu_mult	mult;
+	struct ccu_mux_internal	mux;
+	struct ccu_common	common;
+};
+
+#define SUNXI_CCU_N_WITH_GATE_LOCK(_struct, _name, _parent, _reg,	\
+				   _mshift, _mwidth, _gate, _lock,	\
+				   _flags)				\
+	struct ccu_mult _struct = {					\
+		.enable	= _gate,					\
+		.mult	= _SUNXI_CCU_MULT(_mshift, _mwidth),		\
+		.common	= {						\
+			.reg		= _reg,				\
+			.hw.init	= CLK_HW_INIT(_name,		\
+						      _parent,		\
+						      &ccu_mult_ops,	\
+						      _flags),		\
+		},							\
+	}
+
+static inline struct ccu_mult *hw_to_ccu_mult(struct clk_hw *hw)
+{
+	struct ccu_common *common = hw_to_ccu_common(hw);
+
+	return container_of(common, struct ccu_mult, common);
+}
+
+extern const struct clk_ops ccu_mult_ops;
+
 #endif /* _CCU_MULT_H_ */
diff --git a/drivers/clk/sunxi-ng/ccu_mux.c b/drivers/clk/sunxi-ng/ccu_mux.c
index 58fc36e7dcce..a43ad52a957d 100644
--- a/drivers/clk/sunxi-ng/ccu_mux.c
+++ b/drivers/clk/sunxi-ng/ccu_mux.c
@@ -8,7 +8,9 @@
  * the License, or (at your option) any later version.
  */
 
+#include <linux/clk.h>
 #include <linux/clk-provider.h>
+#include <linux/delay.h>
 
 #include "ccu_gate.h"
 #include "ccu_mux.h"
@@ -18,8 +20,9 @@ void ccu_mux_helper_adjust_parent_for_prediv(struct ccu_common *common,
 					     int parent_index,
 					     unsigned long *parent_rate)
 {
-	u8 prediv = 1;
+	u16 prediv = 1;
 	u32 reg;
+	int i;
 
 	if (!((common->features & CCU_FEATURE_FIXED_PREDIV) ||
 	      (common->features & CCU_FEATURE_VARIABLE_PREDIV)))
@@ -32,8 +35,9 @@ void ccu_mux_helper_adjust_parent_for_prediv(struct ccu_common *common,
 	}
 
 	if (common->features & CCU_FEATURE_FIXED_PREDIV)
-		if (parent_index == cm->fixed_prediv.index)
-			prediv = cm->fixed_prediv.div;
+		for (i = 0; i < cm->n_predivs; i++)
+			if (parent_index == cm->fixed_predivs[i].index)
+				prediv = cm->fixed_predivs[i].div;
 
 	if (common->features & CCU_FEATURE_VARIABLE_PREDIV)
 		if (parent_index == cm->variable_prediv.index) {
@@ -107,6 +111,15 @@ u8 ccu_mux_helper_get_parent(struct ccu_common *common,
 	parent = reg >> cm->shift;
 	parent &= (1 << cm->width) - 1;
 
+	if (cm->table) {
+		int num_parents = clk_hw_get_num_parents(&common->hw);
+		int i;
+
+		for (i = 0; i < num_parents; i++)
+			if (cm->table[i] == parent)
+				return i;
+	}
+
 	return parent;
 }
 
@@ -117,6 +130,9 @@ int ccu_mux_helper_set_parent(struct ccu_common *common,
 	unsigned long flags;
 	u32 reg;
 
+	if (cm->table)
+		index = cm->table[index];
+
 	spin_lock_irqsave(common->lock, flags);
 
 	reg = readl(common->base + common->reg);
@@ -185,3 +201,37 @@ const struct clk_ops ccu_mux_ops = {
 	.determine_rate	= __clk_mux_determine_rate,
 	.recalc_rate	= ccu_mux_recalc_rate,
 };
+
+/*
+ * This clock notifier is called when the frequency of the of the parent
+ * PLL clock is to be changed. The idea is to switch the parent to a
+ * stable clock, such as the main oscillator, while the PLL frequency
+ * stabilizes.
+ */
+static int ccu_mux_notifier_cb(struct notifier_block *nb,
+			       unsigned long event, void *data)
+{
+	struct ccu_mux_nb *mux = to_ccu_mux_nb(nb);
+	int ret = 0;
+
+	if (event == PRE_RATE_CHANGE) {
+		mux->original_index = ccu_mux_helper_get_parent(mux->common,
+								mux->cm);
+		ret = ccu_mux_helper_set_parent(mux->common, mux->cm,
+						mux->bypass_index);
+	} else if (event == POST_RATE_CHANGE) {
+		ret = ccu_mux_helper_set_parent(mux->common, mux->cm,
+						mux->original_index);
+	}
+
+	udelay(mux->delay_us);
+
+	return notifier_from_errno(ret);
+}
+
+int ccu_mux_notifier_register(struct clk *clk, struct ccu_mux_nb *mux_nb)
+{
+	mux_nb->clk_nb.notifier_call = ccu_mux_notifier_cb;
+
+	return clk_notifier_register(clk, &mux_nb->clk_nb);
+}
diff --git a/drivers/clk/sunxi-ng/ccu_mux.h b/drivers/clk/sunxi-ng/ccu_mux.h
index 945082631e7d..47aba3a48245 100644
--- a/drivers/clk/sunxi-ng/ccu_mux.h
+++ b/drivers/clk/sunxi-ng/ccu_mux.h
@@ -5,14 +5,18 @@
 
 #include "ccu_common.h"
 
+struct ccu_mux_fixed_prediv {
+	u8	index;
+	u16	div;
+};
+
 struct ccu_mux_internal {
-	u8	shift;
-	u8	width;
+	u8		shift;
+	u8		width;
+	const u8	*table;
 
-	struct {
-		u8	index;
-		u8	div;
-	} fixed_prediv;
+	const struct ccu_mux_fixed_prediv	*fixed_predivs;
+	u8		n_predivs;
 
 	struct {
 		u8	index;
@@ -21,12 +25,16 @@ struct ccu_mux_internal {
 	} variable_prediv;
 };
 
-#define SUNXI_CLK_MUX(_shift, _width)	\
-	{					\
-		.shift	= _shift,		\
-		.width	= _width,		\
+#define _SUNXI_CCU_MUX_TABLE(_shift, _width, _table)	\
+	{						\
+		.shift	= _shift,			\
+		.width	= _width,			\
+		.table	= _table,			\
 	}
 
+#define _SUNXI_CCU_MUX(_shift, _width) \
+	_SUNXI_CCU_MUX_TABLE(_shift, _width, NULL)
+
 struct ccu_mux {
 	u16			reg;
 	u32			enable;
@@ -35,9 +43,12 @@ struct ccu_mux {
 	struct ccu_common	common;
 };
 
-#define SUNXI_CCU_MUX(_struct, _name, _parents, _reg, _shift, _width, _flags) \
+#define SUNXI_CCU_MUX_TABLE_WITH_GATE(_struct, _name, _parents, _table,	\
+				     _reg, _shift, _width, _gate,	\
+				     _flags)				\
 	struct ccu_mux _struct = {					\
-		.mux	= SUNXI_CLK_MUX(_shift, _width),		\
+		.enable	= _gate,					\
+		.mux	= _SUNXI_CCU_MUX_TABLE(_shift, _width, _table),	\
 		.common	= {						\
 			.reg		= _reg,				\
 			.hw.init	= CLK_HW_INIT_PARENTS(_name,	\
@@ -49,17 +60,14 @@ struct ccu_mux {
 
 #define SUNXI_CCU_MUX_WITH_GATE(_struct, _name, _parents, _reg,		\
 				_shift, _width, _gate, _flags)		\
-	struct ccu_mux _struct = {					\
-		.enable	= _gate,					\
-		.mux	= SUNXI_CLK_MUX(_shift, _width),		\
-		.common	= {						\
-			.reg		= _reg,				\
-			.hw.init	= CLK_HW_INIT_PARENTS(_name,	\
-							      _parents, \
-							      &ccu_mux_ops, \
-							      _flags),	\
-		}							\
-	}
+	SUNXI_CCU_MUX_TABLE_WITH_GATE(_struct, _name, _parents, NULL,	\
+				      _reg, _shift, _width, _gate,	\
+				      _flags)
+
+#define SUNXI_CCU_MUX(_struct, _name, _parents, _reg, _shift, _width,	\
+		      _flags)						\
+	SUNXI_CCU_MUX_TABLE_WITH_GATE(_struct, _name, _parents, NULL,	\
+				      _reg, _shift, _width, 0, _flags)
 
 static inline struct ccu_mux *hw_to_ccu_mux(struct clk_hw *hw)
 {
@@ -88,4 +96,18 @@ int ccu_mux_helper_set_parent(struct ccu_common *common,
 			      struct ccu_mux_internal *cm,
 			      u8 index);
 
+struct ccu_mux_nb {
+	struct notifier_block	clk_nb;
+	struct ccu_common	*common;
+	struct ccu_mux_internal	*cm;
+
+	u32	delay_us;	/* How many us to wait after reparenting */
+	u8	bypass_index;	/* Which parent to temporarily use */
+	u8	original_index;	/* This is set by the notifier callback */
+};
+
+#define to_ccu_mux_nb(_nb) container_of(_nb, struct ccu_mux_nb, clk_nb)
+
+int ccu_mux_notifier_register(struct clk *clk, struct ccu_mux_nb *mux_nb);
+
 #endif /* _CCU_MUX_H_ */
diff --git a/drivers/clk/sunxi-ng/ccu_nkm.c b/drivers/clk/sunxi-ng/ccu_nkm.c
index 2071822b1e9c..059fdc3b4f96 100644
--- a/drivers/clk/sunxi-ng/ccu_nkm.c
+++ b/drivers/clk/sunxi-ng/ccu_nkm.c
@@ -93,19 +93,30 @@ static unsigned long ccu_nkm_recalc_rate(struct clk_hw *hw,
 	return parent_rate * (n + 1) * (k + 1) / (m + 1);
 }
 
-static long ccu_nkm_round_rate(struct clk_hw *hw, unsigned long rate,
-			      unsigned long *parent_rate)
+static unsigned long ccu_nkm_round_rate(struct ccu_mux_internal *mux,
+					unsigned long parent_rate,
+					unsigned long rate,
+					void *data)
 {
-	struct ccu_nkm *nkm = hw_to_ccu_nkm(hw);
+	struct ccu_nkm *nkm = data;
 	struct _ccu_nkm _nkm;
 
 	_nkm.max_n = 1 << nkm->n.width;
 	_nkm.max_k = 1 << nkm->k.width;
-	_nkm.max_m = 1 << nkm->m.width;
+	_nkm.max_m = nkm->m.max ?: 1 << nkm->m.width;
 
-	ccu_nkm_find_best(*parent_rate, rate, &_nkm);
+	ccu_nkm_find_best(parent_rate, rate, &_nkm);
 
-	return *parent_rate * _nkm.n * _nkm.k / _nkm.m;
+	return parent_rate * _nkm.n * _nkm.k / _nkm.m;
+}
+
+static int ccu_nkm_determine_rate(struct clk_hw *hw,
+				  struct clk_rate_request *req)
+{
+	struct ccu_nkm *nkm = hw_to_ccu_nkm(hw);
+
+	return ccu_mux_helper_determine_rate(&nkm->common, &nkm->mux,
+					     req, ccu_nkm_round_rate, nkm);
 }
 
 static int ccu_nkm_set_rate(struct clk_hw *hw, unsigned long rate,
@@ -118,7 +129,7 @@ static int ccu_nkm_set_rate(struct clk_hw *hw, unsigned long rate,
 
 	_nkm.max_n = 1 << nkm->n.width;
 	_nkm.max_k = 1 << nkm->k.width;
-	_nkm.max_m = 1 << nkm->m.width;
+	_nkm.max_m = nkm->m.max ?: 1 << nkm->m.width;
 
 	ccu_nkm_find_best(parent_rate, rate, &_nkm);
 
@@ -142,12 +153,29 @@ static int ccu_nkm_set_rate(struct clk_hw *hw, unsigned long rate,
 	return 0;
 }
 
+static u8 ccu_nkm_get_parent(struct clk_hw *hw)
+{
+	struct ccu_nkm *nkm = hw_to_ccu_nkm(hw);
+
+	return ccu_mux_helper_get_parent(&nkm->common, &nkm->mux);
+}
+
+static int ccu_nkm_set_parent(struct clk_hw *hw, u8 index)
+{
+	struct ccu_nkm *nkm = hw_to_ccu_nkm(hw);
+
+	return ccu_mux_helper_set_parent(&nkm->common, &nkm->mux, index);
+}
+
 const struct clk_ops ccu_nkm_ops = {
 	.disable	= ccu_nkm_disable,
 	.enable		= ccu_nkm_enable,
 	.is_enabled	= ccu_nkm_is_enabled,
 
+	.get_parent	= ccu_nkm_get_parent,
+	.set_parent	= ccu_nkm_set_parent,
+
+	.determine_rate	= ccu_nkm_determine_rate,
 	.recalc_rate	= ccu_nkm_recalc_rate,
-	.round_rate	= ccu_nkm_round_rate,
 	.set_rate	= ccu_nkm_set_rate,
 };
diff --git a/drivers/clk/sunxi-ng/ccu_nkm.h b/drivers/clk/sunxi-ng/ccu_nkm.h
index 1936ac1c6b37..35493fddd8ab 100644
--- a/drivers/clk/sunxi-ng/ccu_nkm.h
+++ b/drivers/clk/sunxi-ng/ccu_nkm.h
@@ -32,10 +32,33 @@ struct ccu_nkm {
 	struct _ccu_mult	n;
 	struct _ccu_mult	k;
 	struct _ccu_div		m;
+	struct ccu_mux_internal	mux;
 
 	struct ccu_common	common;
 };
 
+#define SUNXI_CCU_NKM_WITH_MUX_GATE_LOCK(_struct, _name, _parents, _reg, \
+					 _nshift, _nwidth,		\
+					 _kshift, _kwidth,		\
+					 _mshift, _mwidth,		\
+					 _muxshift, _muxwidth,		\
+					 _gate, _lock, _flags)		\
+	struct ccu_nkm _struct = {					\
+		.enable		= _gate,				\
+		.lock		= _lock,				\
+		.k		= _SUNXI_CCU_MULT(_kshift, _kwidth),	\
+		.n		= _SUNXI_CCU_MULT(_nshift, _nwidth),	\
+		.m		= _SUNXI_CCU_DIV(_mshift, _mwidth),	\
+		.mux		= _SUNXI_CCU_MUX(_muxshift, _muxwidth),	\
+		.common		= {					\
+			.reg		= _reg,				\
+			.hw.init	= CLK_HW_INIT_PARENTS(_name,	\
+						      _parents,		\
+						      &ccu_nkm_ops,	\
+						      _flags),		\
+		},							\
+	}
+
 #define SUNXI_CCU_NKM_WITH_GATE_LOCK(_struct, _name, _parent, _reg,	\
 				     _nshift, _nwidth,			\
 				     _kshift, _kwidth,			\
diff --git a/drivers/clk/sunxi-ng/ccu_nkmp.c b/drivers/clk/sunxi-ng/ccu_nkmp.c
index 9f2b98e19dc9..9769dee99511 100644
--- a/drivers/clk/sunxi-ng/ccu_nkmp.c
+++ b/drivers/clk/sunxi-ng/ccu_nkmp.c
@@ -29,14 +29,14 @@ static void ccu_nkmp_find_best(unsigned long parent, unsigned long rate,
 	unsigned long _n, _k, _m, _p;
 
 	for (_k = 1; _k <= nkmp->max_k; _k++) {
-		for (_p = 0; _p <= nkmp->max_p; _p++) {
+		for (_p = 1; _p <= nkmp->max_p; _p <<= 1) {
 			unsigned long tmp_rate;
 
-			rational_best_approximation(rate / _k, parent >> _p,
+			rational_best_approximation(rate / _k, parent / _p,
 						    nkmp->max_n, nkmp->max_m,
 						    &_n, &_m);
 
-			tmp_rate = (parent * _n * _k >> _p) / _m;
+			tmp_rate = parent * _n * _k / (_m * _p);
 
 			if (tmp_rate > rate)
 				continue;
@@ -110,13 +110,12 @@ static long ccu_nkmp_round_rate(struct clk_hw *hw, unsigned long rate,
 
 	_nkmp.max_n = 1 << nkmp->n.width;
 	_nkmp.max_k = 1 << nkmp->k.width;
-	_nkmp.max_m = 1 << nkmp->m.width;
-	_nkmp.max_p = (1 << nkmp->p.width) - 1;
+	_nkmp.max_m = nkmp->m.max ?: 1 << nkmp->m.width;
+	_nkmp.max_p = nkmp->p.max ?: 1 << ((1 << nkmp->p.width) - 1);
 
-	ccu_nkmp_find_best(*parent_rate, rate,
-			   &_nkmp);
+	ccu_nkmp_find_best(*parent_rate, rate, &_nkmp);
 
-	return (*parent_rate * _nkmp.n * _nkmp.k >> _nkmp.p) / _nkmp.m;
+	return *parent_rate * _nkmp.n * _nkmp.k / (_nkmp.m * _nkmp.p);
 }
 
 static int ccu_nkmp_set_rate(struct clk_hw *hw, unsigned long rate,
@@ -129,8 +128,8 @@ static int ccu_nkmp_set_rate(struct clk_hw *hw, unsigned long rate,
 
 	_nkmp.max_n = 1 << nkmp->n.width;
 	_nkmp.max_k = 1 << nkmp->k.width;
-	_nkmp.max_m = 1 << nkmp->m.width;
-	_nkmp.max_p = (1 << nkmp->p.width) - 1;
+	_nkmp.max_m = nkmp->m.max ?: 1 << nkmp->m.width;
+	_nkmp.max_p = nkmp->p.max ?: 1 << ((1 << nkmp->p.width) - 1);
 
 	ccu_nkmp_find_best(parent_rate, rate, &_nkmp);
 
@@ -145,7 +144,7 @@ static int ccu_nkmp_set_rate(struct clk_hw *hw, unsigned long rate,
 	reg |= (_nkmp.n - 1) << nkmp->n.shift;
 	reg |= (_nkmp.k - 1) << nkmp->k.shift;
 	reg |= (_nkmp.m - 1) << nkmp->m.shift;
-	reg |= _nkmp.p << nkmp->p.shift;
+	reg |= ilog2(_nkmp.p) << nkmp->p.shift;
 
 	writel(reg, nkmp->common.base + nkmp->common.reg);
 
diff --git a/drivers/clk/sunxi-ng/ccu_nm.c b/drivers/clk/sunxi-ng/ccu_nm.c
index e35ddd8eec8b..b61bdd8c7a7f 100644
--- a/drivers/clk/sunxi-ng/ccu_nm.c
+++ b/drivers/clk/sunxi-ng/ccu_nm.c
@@ -61,11 +61,13 @@ static long ccu_nm_round_rate(struct clk_hw *hw, unsigned long rate,
 			      unsigned long *parent_rate)
 {
 	struct ccu_nm *nm = hw_to_ccu_nm(hw);
+	unsigned long max_n, max_m;
 	unsigned long n, m;
 
-	rational_best_approximation(rate, *parent_rate,
-				    1 << nm->n.width, 1 << nm->m.width,
-				    &n, &m);
+	max_n = 1 << nm->n.width;
+	max_m = nm->m.max ?: 1 << nm->m.width;
+
+	rational_best_approximation(rate, *parent_rate, max_n, max_m, &n, &m);
 
 	return *parent_rate * n / m;
 }
@@ -75,6 +77,7 @@ static int ccu_nm_set_rate(struct clk_hw *hw, unsigned long rate,
 {
 	struct ccu_nm *nm = hw_to_ccu_nm(hw);
 	unsigned long flags;
+	unsigned long max_n, max_m;
 	unsigned long n, m;
 	u32 reg;
 
@@ -83,9 +86,10 @@ static int ccu_nm_set_rate(struct clk_hw *hw, unsigned long rate,
 	else
 		ccu_frac_helper_disable(&nm->common, &nm->frac);
 
-	rational_best_approximation(rate, parent_rate,
-				    1 << nm->n.width, 1 << nm->m.width,
-				    &n, &m);
+	max_n = 1 << nm->n.width;
+	max_m = nm->m.max ?: 1 << nm->m.width;
+
+	rational_best_approximation(rate, parent_rate, max_n, max_m, &n, &m);
 
 	spin_lock_irqsave(nm->common.lock, flags);
 
diff --git a/drivers/clk/sunxi/clk-mod0.c b/drivers/clk/sunxi/clk-mod0.c
index b38d71cec74c..e54266cc1c51 100644
--- a/drivers/clk/sunxi/clk-mod0.c
+++ b/drivers/clk/sunxi/clk-mod0.c
@@ -91,7 +91,8 @@ static void __init sun4i_a10_mod0_setup(struct device_node *node)
 	sunxi_factors_register(node, &sun4i_a10_mod0_data,
 			       &sun4i_a10_mod0_lock, reg);
 }
-CLK_OF_DECLARE(sun4i_a10_mod0, "allwinner,sun4i-a10-mod0-clk", sun4i_a10_mod0_setup);
+CLK_OF_DECLARE_DRIVER(sun4i_a10_mod0, "allwinner,sun4i-a10-mod0-clk",
+		      sun4i_a10_mod0_setup);
 
 static int sun4i_a10_mod0_clk_probe(struct platform_device *pdev)
 {
diff --git a/drivers/clk/sunxi/clk-sun8i-apb0.c b/drivers/clk/sunxi/clk-sun8i-apb0.c
index a5666e1d0ce7..ea1eed24778c 100644
--- a/drivers/clk/sunxi/clk-sun8i-apb0.c
+++ b/drivers/clk/sunxi/clk-sun8i-apb0.c
@@ -82,8 +82,8 @@ err_unmap:
 	of_address_to_resource(node, 0, &res);
 	release_mem_region(res.start, resource_size(&res));
 }
-CLK_OF_DECLARE(sun8i_a23_apb0, "allwinner,sun8i-a23-apb0-clk",
-	       sun8i_a23_apb0_setup);
+CLK_OF_DECLARE_DRIVER(sun8i_a23_apb0, "allwinner,sun8i-a23-apb0-clk",
+		      sun8i_a23_apb0_setup);
 
 static int sun8i_a23_apb0_clk_probe(struct platform_device *pdev)
 {
diff --git a/drivers/clk/uniphier/Kconfig b/drivers/clk/uniphier/Kconfig
new file mode 100644
index 000000000000..5512377bd62b
--- /dev/null
+++ b/drivers/clk/uniphier/Kconfig
@@ -0,0 +1,9 @@
+config CLK_UNIPHIER
+	bool "Clock driver for UniPhier SoCs"
+	depends on ARCH_UNIPHIER || COMPILE_TEST
+	depends on OF && MFD_SYSCON
+	default ARCH_UNIPHIER
+	help
+	  Support for clock controllers on UniPhier SoCs.
+	  Say Y if you want to control clocks provided by System Control
+	  block, Media I/O block, Peripheral Block.
diff --git a/drivers/clk/uniphier/Makefile b/drivers/clk/uniphier/Makefile
new file mode 100644
index 000000000000..f27b360329ca
--- /dev/null
+++ b/drivers/clk/uniphier/Makefile
@@ -0,0 +1,8 @@
+obj-y	+= clk-uniphier-core.o
+obj-y	+= clk-uniphier-fixed-factor.o
+obj-y	+= clk-uniphier-fixed-rate.o
+obj-y	+= clk-uniphier-gate.o
+obj-y	+= clk-uniphier-mux.o
+obj-y	+= clk-uniphier-sys.o
+obj-y	+= clk-uniphier-mio.o
+obj-y	+= clk-uniphier-peri.o
diff --git a/drivers/clk/uniphier/clk-uniphier-core.c b/drivers/clk/uniphier/clk-uniphier-core.c
new file mode 100644
index 000000000000..5ffb898d0839
--- /dev/null
+++ b/drivers/clk/uniphier/clk-uniphier-core.c
@@ -0,0 +1,214 @@
+/*
+ * Copyright (C) 2016 Socionext Inc.
+ *   Author: 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-provider.h>
+#include <linux/init.h>
+#include <linux/mfd/syscon.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+
+#include "clk-uniphier.h"
+
+static struct clk_hw *uniphier_clk_register(struct device *dev,
+					    struct regmap *regmap,
+					const struct uniphier_clk_data *data)
+{
+	switch (data->type) {
+	case UNIPHIER_CLK_TYPE_FIXED_FACTOR:
+		return uniphier_clk_register_fixed_factor(dev, data->name,
+							  &data->data.factor);
+	case UNIPHIER_CLK_TYPE_FIXED_RATE:
+		return uniphier_clk_register_fixed_rate(dev, data->name,
+							&data->data.rate);
+	case UNIPHIER_CLK_TYPE_GATE:
+		return uniphier_clk_register_gate(dev, regmap, data->name,
+						  &data->data.gate);
+	case UNIPHIER_CLK_TYPE_MUX:
+		return uniphier_clk_register_mux(dev, regmap, data->name,
+						 &data->data.mux);
+	default:
+		dev_err(dev, "unsupported clock type\n");
+		return ERR_PTR(-EINVAL);
+	}
+}
+
+static int uniphier_clk_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct clk_hw_onecell_data *hw_data;
+	const struct uniphier_clk_data *p, *data;
+	struct regmap *regmap;
+	struct device_node *parent;
+	int clk_num = 0;
+
+	data = of_device_get_match_data(dev);
+	if (WARN_ON(!data))
+		return -EINVAL;
+
+	parent = of_get_parent(dev->of_node); /* parent should be syscon node */
+	regmap = syscon_node_to_regmap(parent);
+	of_node_put(parent);
+	if (IS_ERR(regmap)) {
+		dev_err(dev, "failed to get regmap (error %ld)\n",
+			PTR_ERR(regmap));
+		return PTR_ERR(regmap);
+	}
+
+	for (p = data; p->name; p++)
+		clk_num = max(clk_num, p->idx + 1);
+
+	hw_data = devm_kzalloc(dev,
+			sizeof(*hw_data) + clk_num * sizeof(struct clk_hw *),
+			GFP_KERNEL);
+	if (!hw_data)
+		return -ENOMEM;
+
+	hw_data->num = clk_num;
+
+	/* avoid returning NULL for unused idx */
+	for (; clk_num >= 0; clk_num--)
+		hw_data->hws[clk_num] = ERR_PTR(-EINVAL);
+
+	for (p = data; p->name; p++) {
+		struct clk_hw *hw;
+
+		dev_dbg(dev, "register %s (index=%d)\n", p->name, p->idx);
+		hw = uniphier_clk_register(dev, regmap, p);
+		if (IS_ERR(hw)) {
+			dev_err(dev, "failed to register %s (error %ld)\n",
+				p->name, PTR_ERR(hw));
+			return PTR_ERR(hw);
+		}
+
+		if (p->idx >= 0)
+			hw_data->hws[p->idx] = hw;
+	}
+
+	return of_clk_add_hw_provider(dev->of_node, of_clk_hw_onecell_get,
+				      hw_data);
+}
+
+static int uniphier_clk_remove(struct platform_device *pdev)
+{
+	of_clk_del_provider(pdev->dev.of_node);
+
+	return 0;
+}
+
+static const struct of_device_id uniphier_clk_match[] = {
+	/* System clock */
+	{
+		.compatible = "socionext,uniphier-ld4-clock",
+		.data = uniphier_ld4_sys_clk_data,
+	},
+	{
+		.compatible = "socionext,uniphier-pro4-clock",
+		.data = uniphier_pro4_sys_clk_data,
+	},
+	{
+		.compatible = "socionext,uniphier-sld8-clock",
+		.data = uniphier_sld8_sys_clk_data,
+	},
+	{
+		.compatible = "socionext,uniphier-pro5-clock",
+		.data = uniphier_pro5_sys_clk_data,
+	},
+	{
+		.compatible = "socionext,uniphier-pxs2-clock",
+		.data = uniphier_pxs2_sys_clk_data,
+	},
+	{
+		.compatible = "socionext,uniphier-ld11-clock",
+		.data = uniphier_ld11_sys_clk_data,
+	},
+	{
+		.compatible = "socionext,uniphier-ld20-clock",
+		.data = uniphier_ld20_sys_clk_data,
+	},
+	/* Media I/O clock */
+	{
+		.compatible = "socionext,uniphier-sld3-mio-clock",
+		.data = uniphier_sld3_mio_clk_data,
+	},
+	{
+		.compatible = "socionext,uniphier-ld4-mio-clock",
+		.data = uniphier_sld3_mio_clk_data,
+	},
+	{
+		.compatible = "socionext,uniphier-pro4-mio-clock",
+		.data = uniphier_sld3_mio_clk_data,
+	},
+	{
+		.compatible = "socionext,uniphier-sld8-mio-clock",
+		.data = uniphier_sld3_mio_clk_data,
+	},
+	{
+		.compatible = "socionext,uniphier-pro5-mio-clock",
+		.data = uniphier_pro5_mio_clk_data,
+	},
+	{
+		.compatible = "socionext,uniphier-pxs2-mio-clock",
+		.data = uniphier_pro5_mio_clk_data,
+	},
+	{
+		.compatible = "socionext,uniphier-ld11-mio-clock",
+		.data = uniphier_sld3_mio_clk_data,
+	},
+	{
+		.compatible = "socionext,uniphier-ld20-mio-clock",
+		.data = uniphier_pro5_mio_clk_data,
+	},
+	/* Peripheral clock */
+	{
+		.compatible = "socionext,uniphier-ld4-peri-clock",
+		.data = uniphier_ld4_peri_clk_data,
+	},
+	{
+		.compatible = "socionext,uniphier-pro4-peri-clock",
+		.data = uniphier_pro4_peri_clk_data,
+	},
+	{
+		.compatible = "socionext,uniphier-sld8-peri-clock",
+		.data = uniphier_ld4_peri_clk_data,
+	},
+	{
+		.compatible = "socionext,uniphier-pro5-peri-clock",
+		.data = uniphier_pro4_peri_clk_data,
+	},
+	{
+		.compatible = "socionext,uniphier-pxs2-peri-clock",
+		.data = uniphier_pro4_peri_clk_data,
+	},
+	{
+		.compatible = "socionext,uniphier-ld11-peri-clock",
+		.data = uniphier_pro4_peri_clk_data,
+	},
+	{
+		.compatible = "socionext,uniphier-ld20-peri-clock",
+		.data = uniphier_pro4_peri_clk_data,
+	},
+	{ /* sentinel */ }
+};
+
+static struct platform_driver uniphier_clk_driver = {
+	.probe = uniphier_clk_probe,
+	.remove = uniphier_clk_remove,
+	.driver = {
+		.name = "uniphier-clk",
+		.of_match_table = uniphier_clk_match,
+	},
+};
+builtin_platform_driver(uniphier_clk_driver);
diff --git a/drivers/clk/uniphier/clk-uniphier-fixed-factor.c b/drivers/clk/uniphier/clk-uniphier-fixed-factor.c
new file mode 100644
index 000000000000..da2d9f47ef9f
--- /dev/null
+++ b/drivers/clk/uniphier/clk-uniphier-fixed-factor.c
@@ -0,0 +1,48 @@
+/*
+ * Copyright (C) 2016 Socionext Inc.
+ *   Author: 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-provider.h>
+#include <linux/device.h>
+
+#include "clk-uniphier.h"
+
+struct clk_hw *uniphier_clk_register_fixed_factor(struct device *dev,
+						  const char *name,
+			const struct uniphier_clk_fixed_factor_data *data)
+{
+	struct clk_fixed_factor *fix;
+	struct clk_init_data init;
+	int ret;
+
+	fix = devm_kzalloc(dev, sizeof(*fix), GFP_KERNEL);
+	if (!fix)
+		return ERR_PTR(-ENOMEM);
+
+	init.name = name;
+	init.ops = &clk_fixed_factor_ops;
+	init.flags = data->parent_name ? CLK_SET_RATE_PARENT : 0;
+	init.parent_names = data->parent_name ? &data->parent_name : NULL;
+	init.num_parents = data->parent_name ? 1 : 0;
+
+	fix->mult = data->mult;
+	fix->div = data->div;
+	fix->hw.init = &init;
+
+	ret = devm_clk_hw_register(dev, &fix->hw);
+	if (ret)
+		return ERR_PTR(ret);
+
+	return &fix->hw;
+}
diff --git a/drivers/clk/uniphier/clk-uniphier-fixed-rate.c b/drivers/clk/uniphier/clk-uniphier-fixed-rate.c
new file mode 100644
index 000000000000..0ad0d46173c0
--- /dev/null
+++ b/drivers/clk/uniphier/clk-uniphier-fixed-rate.c
@@ -0,0 +1,47 @@
+/*
+ * Copyright (C) 2016 Socionext Inc.
+ *   Author: 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-provider.h>
+#include <linux/device.h>
+
+#include "clk-uniphier.h"
+
+struct clk_hw *uniphier_clk_register_fixed_rate(struct device *dev,
+						const char *name,
+				const struct uniphier_clk_fixed_rate_data *data)
+{
+	struct clk_fixed_rate *fixed;
+	struct clk_init_data init;
+	int ret;
+
+	/* allocate fixed-rate clock */
+	fixed = devm_kzalloc(dev, sizeof(*fixed), GFP_KERNEL);
+	if (!fixed)
+		return ERR_PTR(-ENOMEM);
+
+	init.name = name;
+	init.ops = &clk_fixed_rate_ops;
+	init.parent_names = NULL;
+	init.num_parents = 0;
+
+	fixed->fixed_rate = data->fixed_rate;
+	fixed->hw.init = &init;
+
+	ret = devm_clk_hw_register(dev, &fixed->hw);
+	if (ret)
+		return ERR_PTR(ret);
+
+	return &fixed->hw;
+}
diff --git a/drivers/clk/uniphier/clk-uniphier-gate.c b/drivers/clk/uniphier/clk-uniphier-gate.c
new file mode 100644
index 000000000000..49142d44446d
--- /dev/null
+++ b/drivers/clk/uniphier/clk-uniphier-gate.c
@@ -0,0 +1,97 @@
+/*
+ * Copyright (C) 2016 Socionext Inc.
+ *   Author: 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-provider.h>
+#include <linux/device.h>
+#include <linux/regmap.h>
+
+#include "clk-uniphier.h"
+
+struct uniphier_clk_gate {
+	struct clk_hw hw;
+	struct regmap *regmap;
+	unsigned int reg;
+	unsigned int bit;
+};
+
+#define to_uniphier_clk_gate(_hw) \
+				container_of(_hw, struct uniphier_clk_gate, hw)
+
+static int uniphier_clk_gate_endisable(struct clk_hw *hw, int enable)
+{
+	struct uniphier_clk_gate *gate = to_uniphier_clk_gate(hw);
+
+	return regmap_write_bits(gate->regmap, gate->reg, BIT(gate->bit),
+				 enable ? BIT(gate->bit) : 0);
+}
+
+static int uniphier_clk_gate_enable(struct clk_hw *hw)
+{
+	return uniphier_clk_gate_endisable(hw, 1);
+}
+
+static void uniphier_clk_gate_disable(struct clk_hw *hw)
+{
+	if (uniphier_clk_gate_endisable(hw, 0) < 0)
+		pr_warn("failed to disable clk\n");
+}
+
+static int uniphier_clk_gate_is_enabled(struct clk_hw *hw)
+{
+	struct uniphier_clk_gate *gate = to_uniphier_clk_gate(hw);
+	unsigned int val;
+
+	if (regmap_read(gate->regmap, gate->reg, &val) < 0)
+		pr_warn("is_enabled() may return wrong result\n");
+
+	return !!(val & BIT(gate->bit));
+}
+
+static const struct clk_ops uniphier_clk_gate_ops = {
+	.enable = uniphier_clk_gate_enable,
+	.disable = uniphier_clk_gate_disable,
+	.is_enabled = uniphier_clk_gate_is_enabled,
+};
+
+struct clk_hw *uniphier_clk_register_gate(struct device *dev,
+					  struct regmap *regmap,
+					  const char *name,
+				const struct uniphier_clk_gate_data *data)
+{
+	struct uniphier_clk_gate *gate;
+	struct clk_init_data init;
+	int ret;
+
+	gate = devm_kzalloc(dev, sizeof(*gate), GFP_KERNEL);
+	if (!gate)
+		return ERR_PTR(-ENOMEM);
+
+	init.name = name;
+	init.ops = &uniphier_clk_gate_ops;
+	init.flags = data->parent_name ? CLK_SET_RATE_PARENT : 0;
+	init.parent_names = data->parent_name ? &data->parent_name : NULL;
+	init.num_parents = data->parent_name ? 1 : 0;
+
+	gate->regmap = regmap;
+	gate->reg = data->reg;
+	gate->bit = data->bit;
+	gate->hw.init = &init;
+
+	ret = devm_clk_hw_register(dev, &gate->hw);
+	if (ret)
+		return ERR_PTR(ret);
+
+	return &gate->hw;
+}
diff --git a/drivers/clk/uniphier/clk-uniphier-mio.c b/drivers/clk/uniphier/clk-uniphier-mio.c
new file mode 100644
index 000000000000..6aa7ec768d0b
--- /dev/null
+++ b/drivers/clk/uniphier/clk-uniphier-mio.c
@@ -0,0 +1,101 @@
+/*
+ * Copyright (C) 2016 Socionext Inc.
+ *   Author: 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 "clk-uniphier.h"
+
+#define UNIPHIER_MIO_CLK_SD_FIXED					\
+	UNIPHIER_CLK_FACTOR("sd-44m", -1, "sd-133m", 1, 3),		\
+	UNIPHIER_CLK_FACTOR("sd-33m", -1, "sd-200m", 1, 6),		\
+	UNIPHIER_CLK_FACTOR("sd-50m", -1, "sd-200m", 1, 4),		\
+	UNIPHIER_CLK_FACTOR("sd-67m", -1, "sd-200m", 1, 3),		\
+	UNIPHIER_CLK_FACTOR("sd-100m", -1, "sd-200m", 1, 2),		\
+	UNIPHIER_CLK_FACTOR("sd-40m", -1, "sd-200m", 1, 5),		\
+	UNIPHIER_CLK_FACTOR("sd-25m", -1, "sd-200m", 1, 8),		\
+	UNIPHIER_CLK_FACTOR("sd-22m", -1, "sd-133m", 1, 6)
+
+#define UNIPHIER_MIO_CLK_SD(_idx, ch)					\
+	{								\
+		.name = "sd" #ch "-sel",				\
+		.type = UNIPHIER_CLK_TYPE_MUX,				\
+		.idx = -1,						\
+		.data.mux = {						\
+			.parent_names = {				\
+				"sd-44m",				\
+				"sd-33m",				\
+				"sd-50m",				\
+				"sd-67m",				\
+				"sd-100m",				\
+				"sd-40m",				\
+				"sd-25m",				\
+				"sd-22m",				\
+			},						\
+			.num_parents = 8,				\
+			.reg = 0x30 + 0x200 * (ch),			\
+			.masks = {					\
+				0x00031000,				\
+				0x00031000,				\
+				0x00031000,				\
+				0x00031000,				\
+				0x00001300,				\
+				0x00001300,				\
+				0x00001300,				\
+				0x00001300,				\
+			},						\
+			.vals = {					\
+				0x00000000,				\
+				0x00010000,				\
+				0x00020000,				\
+				0x00030000,				\
+				0x00001000,				\
+				0x00001100,				\
+				0x00001200,				\
+				0x00001300,				\
+			},						\
+		},							\
+	},								\
+	UNIPHIER_CLK_GATE("sd" #ch, (_idx), "sd" #ch "-sel", 0x20 + 0x200 * (ch), 8)
+
+#define UNIPHIER_MIO_CLK_USB2(idx, ch)					\
+	UNIPHIER_CLK_GATE("usb2" #ch, (idx), "usb2", 0x20 + 0x200 * (ch), 28)
+
+#define UNIPHIER_MIO_CLK_USB2_PHY(idx, ch)				\
+	UNIPHIER_CLK_GATE("usb2" #ch "-phy", (idx), "usb2", 0x20 + 0x200 * (ch), 29)
+
+#define UNIPHIER_MIO_CLK_DMAC(idx)					\
+	UNIPHIER_CLK_GATE("miodmac", (idx), "stdmac", 0x20, 25)
+
+const struct uniphier_clk_data uniphier_sld3_mio_clk_data[] = {
+	UNIPHIER_MIO_CLK_SD_FIXED,
+	UNIPHIER_MIO_CLK_SD(0, 0),
+	UNIPHIER_MIO_CLK_SD(1, 1),
+	UNIPHIER_MIO_CLK_SD(2, 2),
+	UNIPHIER_MIO_CLK_DMAC(7),
+	UNIPHIER_MIO_CLK_USB2(8, 0),
+	UNIPHIER_MIO_CLK_USB2(9, 1),
+	UNIPHIER_MIO_CLK_USB2(10, 2),
+	UNIPHIER_MIO_CLK_USB2(11, 3),
+	UNIPHIER_MIO_CLK_USB2_PHY(12, 0),
+	UNIPHIER_MIO_CLK_USB2_PHY(13, 1),
+	UNIPHIER_MIO_CLK_USB2_PHY(14, 2),
+	UNIPHIER_MIO_CLK_USB2_PHY(15, 3),
+	{ /* sentinel */ }
+};
+
+const struct uniphier_clk_data uniphier_pro5_mio_clk_data[] = {
+	UNIPHIER_MIO_CLK_SD_FIXED,
+	UNIPHIER_MIO_CLK_SD(0, 0),
+	UNIPHIER_MIO_CLK_SD(1, 1),
+	{ /* sentinel */ }
+};
diff --git a/drivers/clk/uniphier/clk-uniphier-mux.c b/drivers/clk/uniphier/clk-uniphier-mux.c
new file mode 100644
index 000000000000..15a2f2cbe0d9
--- /dev/null
+++ b/drivers/clk/uniphier/clk-uniphier-mux.c
@@ -0,0 +1,95 @@
+/*
+ * Copyright (C) 2016 Socionext Inc.
+ *   Author: 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-provider.h>
+#include <linux/device.h>
+#include <linux/regmap.h>
+
+#include "clk-uniphier.h"
+
+struct uniphier_clk_mux {
+	struct clk_hw hw;
+	struct regmap *regmap;
+	unsigned int reg;
+	const unsigned int *masks;
+	const unsigned int *vals;
+};
+
+#define to_uniphier_clk_mux(_hw) container_of(_hw, struct uniphier_clk_mux, hw)
+
+static int uniphier_clk_mux_set_parent(struct clk_hw *hw, u8 index)
+{
+	struct uniphier_clk_mux *mux = to_uniphier_clk_mux(hw);
+
+	return regmap_write_bits(mux->regmap, mux->reg, mux->masks[index],
+				 mux->vals[index]);
+}
+
+static u8 uniphier_clk_mux_get_parent(struct clk_hw *hw)
+{
+	struct uniphier_clk_mux *mux = to_uniphier_clk_mux(hw);
+	int num_parents = clk_hw_get_num_parents(hw);
+	int ret;
+	u32 val;
+	u8 i;
+
+	ret = regmap_read(mux->regmap, mux->reg, &val);
+	if (ret)
+		return ret;
+
+	for (i = 0; i < num_parents; i++)
+		if ((mux->masks[i] & val) == mux->vals[i])
+			return i;
+
+	return -EINVAL;
+}
+
+static const struct clk_ops uniphier_clk_mux_ops = {
+	.determine_rate = __clk_mux_determine_rate,
+	.set_parent = uniphier_clk_mux_set_parent,
+	.get_parent = uniphier_clk_mux_get_parent,
+};
+
+struct clk_hw *uniphier_clk_register_mux(struct device *dev,
+					 struct regmap *regmap,
+					 const char *name,
+				const struct uniphier_clk_mux_data *data)
+{
+	struct uniphier_clk_mux *mux;
+	struct clk_init_data init;
+	int ret;
+
+	mux = devm_kzalloc(dev, sizeof(*mux), GFP_KERNEL);
+	if (!mux)
+		return ERR_PTR(-ENOMEM);
+
+	init.name = name;
+	init.ops = &uniphier_clk_mux_ops;
+	init.flags = CLK_SET_RATE_PARENT;
+	init.parent_names = data->parent_names;
+	init.num_parents = data->num_parents,
+
+	mux->regmap = regmap;
+	mux->reg = data->reg;
+	mux->masks = data->masks;
+	mux->vals = data->vals;
+	mux->hw.init = &init;
+
+	ret = devm_clk_hw_register(dev, &mux->hw);
+	if (ret)
+		return ERR_PTR(ret);
+
+	return &mux->hw;
+}
diff --git a/drivers/clk/uniphier/clk-uniphier-peri.c b/drivers/clk/uniphier/clk-uniphier-peri.c
new file mode 100644
index 000000000000..521c80e9a06f
--- /dev/null
+++ b/drivers/clk/uniphier/clk-uniphier-peri.c
@@ -0,0 +1,57 @@
+/*
+ * Copyright (C) 2016 Socionext Inc.
+ *   Author: 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 "clk-uniphier.h"
+
+#define UNIPHIER_PERI_CLK_UART(idx, ch)					\
+	UNIPHIER_CLK_GATE("uart" #ch, (idx), "uart", 0x24, 19 + (ch))
+
+#define UNIPHIER_PERI_CLK_I2C_COMMON					\
+	UNIPHIER_CLK_GATE("i2c-common", -1, "i2c", 0x20, 1)
+
+#define UNIPHIER_PERI_CLK_I2C(idx, ch)					\
+	UNIPHIER_CLK_GATE("i2c" #ch, (idx), "i2c-common", 0x24, 5 + (ch))
+
+#define UNIPHIER_PERI_CLK_FI2C(idx, ch)					\
+	UNIPHIER_CLK_GATE("i2c" #ch, (idx), "i2c", 0x24, 24 + (ch))
+
+const struct uniphier_clk_data uniphier_ld4_peri_clk_data[] = {
+	UNIPHIER_PERI_CLK_UART(0, 0),
+	UNIPHIER_PERI_CLK_UART(1, 1),
+	UNIPHIER_PERI_CLK_UART(2, 2),
+	UNIPHIER_PERI_CLK_UART(3, 3),
+	UNIPHIER_PERI_CLK_I2C_COMMON,
+	UNIPHIER_PERI_CLK_I2C(4, 0),
+	UNIPHIER_PERI_CLK_I2C(5, 1),
+	UNIPHIER_PERI_CLK_I2C(6, 2),
+	UNIPHIER_PERI_CLK_I2C(7, 3),
+	UNIPHIER_PERI_CLK_I2C(8, 4),
+	{ /* sentinel */ }
+};
+
+const struct uniphier_clk_data uniphier_pro4_peri_clk_data[] = {
+	UNIPHIER_PERI_CLK_UART(0, 0),
+	UNIPHIER_PERI_CLK_UART(1, 1),
+	UNIPHIER_PERI_CLK_UART(2, 2),
+	UNIPHIER_PERI_CLK_UART(3, 3),
+	UNIPHIER_PERI_CLK_FI2C(4, 0),
+	UNIPHIER_PERI_CLK_FI2C(5, 1),
+	UNIPHIER_PERI_CLK_FI2C(6, 2),
+	UNIPHIER_PERI_CLK_FI2C(7, 3),
+	UNIPHIER_PERI_CLK_FI2C(8, 4),
+	UNIPHIER_PERI_CLK_FI2C(9, 5),
+	UNIPHIER_PERI_CLK_FI2C(10, 6),
+	{ /* sentinel */ }
+};
diff --git a/drivers/clk/uniphier/clk-uniphier-sys.c b/drivers/clk/uniphier/clk-uniphier-sys.c
new file mode 100644
index 000000000000..5d029991047d
--- /dev/null
+++ b/drivers/clk/uniphier/clk-uniphier-sys.c
@@ -0,0 +1,151 @@
+/*
+ * Copyright (C) 2016 Socionext Inc.
+ *   Author: 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/stddef.h>
+
+#include "clk-uniphier.h"
+
+#define UNIPHIER_SLD3_SYS_CLK_SD					\
+	UNIPHIER_CLK_FACTOR("sd-200m", -1, "spll", 1, 8),		\
+	UNIPHIER_CLK_FACTOR("sd-133m", -1, "vpll27a", 1, 2)
+
+#define UNIPHIER_PRO5_SYS_CLK_SD					\
+	UNIPHIER_CLK_FACTOR("sd-200m", -1, "spll", 1, 12),		\
+	UNIPHIER_CLK_FACTOR("sd-133m", -1, "spll", 1, 18)
+
+#define UNIPHIER_LD20_SYS_CLK_SD					\
+	UNIPHIER_CLK_FACTOR("sd-200m", -1, "spll", 1, 10),		\
+	UNIPHIER_CLK_FACTOR("sd-133m", -1, "spll", 1, 15)
+
+#define UNIPHIER_SLD3_SYS_CLK_STDMAC(idx)				\
+	UNIPHIER_CLK_GATE("stdmac", (idx), NULL, 0x2104, 10)
+
+#define UNIPHIER_LD11_SYS_CLK_STDMAC(idx)				\
+	UNIPHIER_CLK_GATE("stdmac", (idx), NULL, 0x210c, 8)
+
+#define UNIPHIER_PRO4_SYS_CLK_GIO(idx)					\
+	UNIPHIER_CLK_GATE("gio", (idx), NULL, 0x2104, 6)
+
+#define UNIPHIER_PRO4_SYS_CLK_USB3(idx, ch)				\
+	UNIPHIER_CLK_GATE("usb3" #ch, (idx), NULL, 0x2104, 16 + (ch))
+
+const struct uniphier_clk_data uniphier_sld3_sys_clk_data[] = {
+	UNIPHIER_CLK_FACTOR("spll", -1, "ref", 65, 1),		/* 1597.44 MHz */
+	UNIPHIER_CLK_FACTOR("upll", -1, "ref", 6000, 512),	/* 288 MHz */
+	UNIPHIER_CLK_FACTOR("a2pll", -1, "ref", 24, 1),		/* 589.824 MHz */
+	UNIPHIER_CLK_FACTOR("vpll27a", -1, "ref", 5625, 512),	/* 270 MHz */
+	UNIPHIER_CLK_FACTOR("uart", 0, "a2pll", 1, 16),
+	UNIPHIER_CLK_FACTOR("i2c", 1, "spll", 1, 16),
+	UNIPHIER_SLD3_SYS_CLK_SD,
+	UNIPHIER_CLK_FACTOR("usb2", -1, "upll", 1, 12),
+	UNIPHIER_SLD3_SYS_CLK_STDMAC(8),
+	{ /* sentinel */ }
+};
+
+const struct uniphier_clk_data uniphier_ld4_sys_clk_data[] = {
+	UNIPHIER_CLK_FACTOR("spll", -1, "ref", 65, 1),		/* 1597.44 MHz */
+	UNIPHIER_CLK_FACTOR("upll", -1, "ref", 6000, 512),	/* 288 MHz */
+	UNIPHIER_CLK_FACTOR("a2pll", -1, "ref", 24, 1),		/* 589.824 MHz */
+	UNIPHIER_CLK_FACTOR("vpll27a", -1, "ref", 5625, 512),	/* 270 MHz */
+	UNIPHIER_CLK_FACTOR("uart", 0, "a2pll", 1, 16),
+	UNIPHIER_CLK_FACTOR("i2c", 1, "spll", 1, 16),
+	UNIPHIER_SLD3_SYS_CLK_SD,
+	UNIPHIER_CLK_FACTOR("usb2", -1, "upll", 1, 12),
+	UNIPHIER_SLD3_SYS_CLK_STDMAC(8),		/* Ether, HSC, MIO */
+	{ /* sentinel */ }
+};
+
+const struct uniphier_clk_data uniphier_pro4_sys_clk_data[] = {
+	UNIPHIER_CLK_FACTOR("spll", -1, "ref", 64, 1),		/* 1600 MHz */
+	UNIPHIER_CLK_FACTOR("upll", -1, "ref", 288, 25),	/* 288 MHz */
+	UNIPHIER_CLK_FACTOR("a2pll", -1, "upll", 256, 125),	/* 589.824 MHz */
+	UNIPHIER_CLK_FACTOR("vpll27a", -1, "ref", 270, 25),	/* 270 MHz */
+	UNIPHIER_CLK_FACTOR("uart", 0, "a2pll", 1, 8),
+	UNIPHIER_CLK_FACTOR("i2c", 1, "spll", 1, 32),
+	UNIPHIER_SLD3_SYS_CLK_SD,
+	UNIPHIER_CLK_FACTOR("usb2", -1, "upll", 1, 12),
+	UNIPHIER_SLD3_SYS_CLK_STDMAC(8),		/* HSC, MIO, RLE */
+	UNIPHIER_PRO4_SYS_CLK_GIO(12),			/* Ether, SATA, USB3 */
+	UNIPHIER_PRO4_SYS_CLK_USB3(14, 0),
+	UNIPHIER_PRO4_SYS_CLK_USB3(15, 1),
+	{ /* sentinel */ }
+};
+
+const struct uniphier_clk_data uniphier_sld8_sys_clk_data[] = {
+	UNIPHIER_CLK_FACTOR("spll", -1, "ref", 64, 1),		/* 1600 MHz */
+	UNIPHIER_CLK_FACTOR("upll", -1, "ref", 288, 25),	/* 288 MHz */
+	UNIPHIER_CLK_FACTOR("vpll27a", -1, "ref", 270, 25),	/* 270 MHz */
+	UNIPHIER_CLK_FACTOR("uart", 0, "spll", 1, 20),
+	UNIPHIER_CLK_FACTOR("i2c", 1, "spll", 1, 16),
+	UNIPHIER_SLD3_SYS_CLK_SD,
+	UNIPHIER_CLK_FACTOR("usb2", -1, "upll", 1, 12),
+	UNIPHIER_SLD3_SYS_CLK_STDMAC(8),		/* Ether, HSC, MIO */
+	{ /* sentinel */ }
+};
+
+const struct uniphier_clk_data uniphier_pro5_sys_clk_data[] = {
+	UNIPHIER_CLK_FACTOR("spll", -1, "ref", 120, 1),		/* 2400 MHz */
+	UNIPHIER_CLK_FACTOR("dapll1", -1, "ref", 128, 1),	/* 2560 MHz */
+	UNIPHIER_CLK_FACTOR("dapll2", -1, "ref", 144, 125),	/* 2949.12 MHz */
+	UNIPHIER_CLK_FACTOR("uart", 0, "dapll2", 1, 40),
+	UNIPHIER_CLK_FACTOR("i2c", 1, "spll", 1, 48),
+	UNIPHIER_PRO5_SYS_CLK_SD,
+	UNIPHIER_SLD3_SYS_CLK_STDMAC(8),			/* HSC */
+	UNIPHIER_PRO4_SYS_CLK_GIO(12),				/* PCIe, USB3 */
+	UNIPHIER_PRO4_SYS_CLK_USB3(14, 0),
+	UNIPHIER_PRO4_SYS_CLK_USB3(15, 1),
+	{ /* sentinel */ }
+};
+
+const struct uniphier_clk_data uniphier_pxs2_sys_clk_data[] = {
+	UNIPHIER_CLK_FACTOR("spll", -1, "ref", 96, 1),		/* 2400 MHz */
+	UNIPHIER_CLK_FACTOR("uart", 0, "spll", 1, 27),
+	UNIPHIER_CLK_FACTOR("i2c", 1, "spll", 1, 48),
+	UNIPHIER_PRO5_SYS_CLK_SD,
+	UNIPHIER_SLD3_SYS_CLK_STDMAC(8),			/* HSC, RLE */
+	/* GIO is always clock-enabled: no function for 0x2104 bit6 */
+	UNIPHIER_PRO4_SYS_CLK_USB3(14, 0),
+	UNIPHIER_PRO4_SYS_CLK_USB3(15, 1),
+	/* The document mentions 0x2104 bit 18, but not functional */
+	UNIPHIER_CLK_GATE("usb30-phy", 16, NULL, 0x2104, 19),
+	UNIPHIER_CLK_GATE("usb31-phy", 20, NULL, 0x2104, 20),
+	{ /* sentinel */ }
+};
+
+const struct uniphier_clk_data uniphier_ld11_sys_clk_data[] = {
+	UNIPHIER_CLK_FACTOR("spll", -1, "ref", 80, 1),		/* 2000 MHz */
+	UNIPHIER_CLK_FACTOR("uart", 0, "spll", 1, 34),
+	UNIPHIER_CLK_FACTOR("i2c", 1, "spll", 1, 40),
+	UNIPHIER_LD11_SYS_CLK_STDMAC(8),			/* HSC, MIO */
+	UNIPHIER_CLK_FACTOR("usb2", -1, "ref", 24, 25),
+	{ /* sentinel */ }
+};
+
+const struct uniphier_clk_data uniphier_ld20_sys_clk_data[] = {
+	UNIPHIER_CLK_FACTOR("spll", -1, "ref", 80, 1),		/* 2000 MHz */
+	UNIPHIER_CLK_FACTOR("uart", 0, "spll", 1, 34),
+	UNIPHIER_CLK_FACTOR("i2c", 1, "spll", 1, 40),
+	UNIPHIER_LD20_SYS_CLK_SD,
+	UNIPHIER_LD11_SYS_CLK_STDMAC(8),			/* HSC */
+	/* GIO is always clock-enabled: no function for 0x210c bit5 */
+	/*
+	 * clock for USB Link is enabled by the logic "OR" of bit 14 and bit 15.
+	 * We do not use bit 15 here.
+	 */
+	UNIPHIER_CLK_GATE("usb30", 14, NULL, 0x210c, 14),
+	UNIPHIER_CLK_GATE("usb30-phy0", 16, NULL, 0x210c, 12),
+	UNIPHIER_CLK_GATE("usb30-phy1", 17, NULL, 0x210c, 13),
+	{ /* sentinel */ }
+};
diff --git a/drivers/clk/uniphier/clk-uniphier.h b/drivers/clk/uniphier/clk-uniphier.h
new file mode 100644
index 000000000000..3ae184062388
--- /dev/null
+++ b/drivers/clk/uniphier/clk-uniphier.h
@@ -0,0 +1,122 @@
+/*
+ * Copyright (C) 2016 Socionext Inc.
+ *   Author: 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.
+ */
+
+#ifndef __CLK_UNIPHIER_H__
+#define __CLK_UNIPHIER_H__
+
+struct clk_hw;
+struct device;
+struct regmap;
+
+#define UNIPHIER_CLK_MUX_MAX_PARENTS	8
+
+enum uniphier_clk_type {
+	UNIPHIER_CLK_TYPE_FIXED_FACTOR,
+	UNIPHIER_CLK_TYPE_FIXED_RATE,
+	UNIPHIER_CLK_TYPE_GATE,
+	UNIPHIER_CLK_TYPE_MUX,
+};
+
+struct uniphier_clk_fixed_factor_data {
+	const char *parent_name;
+	unsigned int mult;
+	unsigned int div;
+};
+
+struct uniphier_clk_fixed_rate_data {
+	unsigned long fixed_rate;
+};
+
+struct uniphier_clk_gate_data {
+	const char *parent_name;
+	unsigned int reg;
+	unsigned int bit;
+};
+
+struct uniphier_clk_mux_data {
+	const char *parent_names[UNIPHIER_CLK_MUX_MAX_PARENTS];
+	unsigned int num_parents;
+	unsigned int reg;
+	unsigned int masks[UNIPHIER_CLK_MUX_MAX_PARENTS];
+	unsigned int vals[UNIPHIER_CLK_MUX_MAX_PARENTS];
+};
+
+struct uniphier_clk_data {
+	const char *name;
+	enum uniphier_clk_type type;
+	int idx;
+	union {
+		struct uniphier_clk_fixed_factor_data factor;
+		struct uniphier_clk_fixed_rate_data rate;
+		struct uniphier_clk_gate_data gate;
+		struct uniphier_clk_mux_data mux;
+	} data;
+};
+
+#define UNIPHIER_CLK_FACTOR(_name, _idx, _parent, _mult, _div)	\
+	{							\
+		.name = (_name),				\
+		.type = UNIPHIER_CLK_TYPE_FIXED_FACTOR,		\
+		.idx = (_idx),					\
+		.data.factor = {				\
+			.parent_name = (_parent),		\
+			.mult = (_mult),			\
+			.div = (_div),				\
+		},						\
+	}
+
+
+#define UNIPHIER_CLK_GATE(_name, _idx, _parent, _reg, _bit)	\
+	{							\
+		.name = (_name),				\
+		.type = UNIPHIER_CLK_TYPE_GATE,			\
+		.idx = (_idx),					\
+		.data.gate = {					\
+			.parent_name = (_parent),		\
+			.reg = (_reg),				\
+			.bit = (_bit),				\
+		},						\
+	}
+
+
+struct clk_hw *uniphier_clk_register_fixed_factor(struct device *dev,
+						  const char *name,
+			const struct uniphier_clk_fixed_factor_data *data);
+struct clk_hw *uniphier_clk_register_fixed_rate(struct device *dev,
+						const char *name,
+			const struct uniphier_clk_fixed_rate_data *data);
+struct clk_hw *uniphier_clk_register_gate(struct device *dev,
+					  struct regmap *regmap,
+					  const char *name,
+				const struct uniphier_clk_gate_data *data);
+struct clk_hw *uniphier_clk_register_mux(struct device *dev,
+					 struct regmap *regmap,
+					 const char *name,
+				const struct uniphier_clk_mux_data *data);
+
+extern const struct uniphier_clk_data uniphier_sld3_sys_clk_data[];
+extern const struct uniphier_clk_data uniphier_ld4_sys_clk_data[];
+extern const struct uniphier_clk_data uniphier_pro4_sys_clk_data[];
+extern const struct uniphier_clk_data uniphier_sld8_sys_clk_data[];
+extern const struct uniphier_clk_data uniphier_pro5_sys_clk_data[];
+extern const struct uniphier_clk_data uniphier_pxs2_sys_clk_data[];
+extern const struct uniphier_clk_data uniphier_ld11_sys_clk_data[];
+extern const struct uniphier_clk_data uniphier_ld20_sys_clk_data[];
+extern const struct uniphier_clk_data uniphier_sld3_mio_clk_data[];
+extern const struct uniphier_clk_data uniphier_pro5_mio_clk_data[];
+extern const struct uniphier_clk_data uniphier_ld4_peri_clk_data[];
+extern const struct uniphier_clk_data uniphier_pro4_peri_clk_data[];
+
+#endif /* __CLK_UNIPHIER_H__ */
diff --git a/drivers/clk/versatile/clk-icst.c b/drivers/clk/versatile/clk-icst.c
index 5e9b65278e4c..4faa94440779 100644
--- a/drivers/clk/versatile/clk-icst.c
+++ b/drivers/clk/versatile/clk-icst.c
@@ -27,6 +27,26 @@
 /* Magic unlocking token used on all Versatile boards */
 #define VERSATILE_LOCK_VAL	0xA05F
 
+#define VERSATILE_AUX_OSC_BITS 0x7FFFF
+#define INTEGRATOR_AP_CM_BITS 0xFF
+#define INTEGRATOR_AP_SYS_BITS 0xFF
+#define INTEGRATOR_CP_CM_CORE_BITS 0x7FF
+#define INTEGRATOR_CP_CM_MEM_BITS 0x7FF000
+
+#define INTEGRATOR_AP_PCI_25_33_MHZ BIT(8)
+
+/**
+ * enum icst_control_type - the type of ICST control register
+ */
+enum icst_control_type {
+	ICST_VERSATILE, /* The standard type, all control bits available */
+	ICST_INTEGRATOR_AP_CM, /* Only 8 bits of VDW available */
+	ICST_INTEGRATOR_AP_SYS, /* Only 8 bits of VDW available */
+	ICST_INTEGRATOR_AP_PCI, /* Odd bit pattern storage */
+	ICST_INTEGRATOR_CP_CM_CORE, /* Only 8 bits of VDW and 3 bits of OD */
+	ICST_INTEGRATOR_CP_CM_MEM, /* Only 8 bits of VDW and 3 bits of OD */
+};
+
 /**
  * struct clk_icst - ICST VCO clock wrapper
  * @hw: corresponding clock hardware entry
@@ -34,6 +54,7 @@
  * @lockreg: VCO lock register address
  * @params: parameters for this ICST instance
  * @rate: current rate
+ * @ctype: the type of control register for the ICST
  */
 struct clk_icst {
 	struct clk_hw hw;
@@ -42,6 +63,7 @@ struct clk_icst {
 	u32 lockreg_off;
 	struct icst_params *params;
 	unsigned long rate;
+	enum icst_control_type ctype;
 };
 
 #define to_icst(_hw) container_of(_hw, struct clk_icst, hw)
@@ -59,6 +81,76 @@ static int vco_get(struct clk_icst *icst, struct icst_vco *vco)
 	ret = regmap_read(icst->map, icst->vcoreg_off, &val);
 	if (ret)
 		return ret;
+
+	/*
+	 * The Integrator/AP core clock can only access the low eight
+	 * bits of the v PLL divider. Bit 8 is tied low and always zero,
+	 * r is hardwired to 22 and output divider s is hardwired to 1
+	 * (divide by 2) according to the document
+	 * "Integrator CM926EJ-S, CM946E-S, CM966E-S, CM1026EJ-S and
+	 * CM1136JF-S User Guide" ARM DUI 0138E, page 3-13 thru 3-14.
+	 */
+	if (icst->ctype == ICST_INTEGRATOR_AP_CM) {
+		vco->v = val & INTEGRATOR_AP_CM_BITS;
+		vco->r = 22;
+		vco->s = 1;
+		return 0;
+	}
+
+	/*
+	 * The Integrator/AP system clock on the base board can only
+	 * access the low eight bits of the v PLL divider. Bit 8 is tied low
+	 * and always zero, r is hardwired to 46, and the output divider is
+	 * hardwired to 3 (divide by 4) according to the document
+	 * "Integrator AP ASIC Development Motherboard" ARM DUI 0098B,
+	 * page 3-16.
+	 */
+	if (icst->ctype == ICST_INTEGRATOR_AP_SYS) {
+		vco->v = val & INTEGRATOR_AP_SYS_BITS;
+		vco->r = 46;
+		vco->s = 3;
+		return 0;
+	}
+
+	/*
+	 * The Integrator/AP PCI clock is using an odd pattern to create
+	 * the child clock, basically a single bit called DIVX/Y is used
+	 * to select between two different hardwired values: setting the
+	 * bit to 0 yields v = 17, r = 22 and OD = 1, whereas setting the
+	 * bit to 1 yields v = 14, r = 14 and OD = 1 giving the frequencies
+	 * 33 or 25 MHz respectively.
+	 */
+	if (icst->ctype == ICST_INTEGRATOR_AP_PCI) {
+		bool divxy = !!(val & INTEGRATOR_AP_PCI_25_33_MHZ);
+
+		vco->v = divxy ? 17 : 14;
+		vco->r = divxy ? 22 : 14;
+		vco->s = 1;
+		return 0;
+	}
+
+	/*
+	 * The Integrator/CP core clock can access the low eight bits
+	 * of the v PLL divider. Bit 8 is tied low and always zero,
+	 * r is hardwired to 22 and the output divider s is accessible
+	 * in bits 8 thru 10 according to the document
+	 * "Integrator/CM940T, CM920T, CM740T, and CM720T User Guide"
+	 * ARM DUI 0157A, page 3-20 thru 3-23 and 4-10.
+	 */
+	if (icst->ctype == ICST_INTEGRATOR_CP_CM_CORE) {
+		vco->v = val & 0xFF;
+		vco->r = 22;
+		vco->s = (val >> 8) & 7;
+		return 0;
+	}
+
+	if (icst->ctype == ICST_INTEGRATOR_CP_CM_MEM) {
+		vco->v = (val >> 12) & 0xFF;
+		vco->r = 22;
+		vco->s = (val >> 20) & 7;
+		return 0;
+	}
+
 	vco->v = val & 0x1ff;
 	vco->r = (val >> 9) & 0x7f;
 	vco->s = (val >> 16) & 03;
@@ -72,22 +164,62 @@ static int vco_get(struct clk_icst *icst, struct icst_vco *vco)
  */
 static int vco_set(struct clk_icst *icst, struct icst_vco vco)
 {
+	u32 mask;
 	u32 val;
 	int ret;
 
-	ret = regmap_read(icst->map, icst->vcoreg_off, &val);
-	if (ret)
-		return ret;
+	/* Mask the bits used by the VCO */
+	switch (icst->ctype) {
+	case ICST_INTEGRATOR_AP_CM:
+		mask = INTEGRATOR_AP_CM_BITS;
+		val = vco.v & 0xFF;
+		if (vco.v & 0x100)
+			pr_err("ICST error: tried to set bit 8 of VDW\n");
+		if (vco.s != 1)
+			pr_err("ICST error: tried to use VOD != 1\n");
+		if (vco.r != 22)
+			pr_err("ICST error: tried to use RDW != 22\n");
+		break;
+	case ICST_INTEGRATOR_AP_SYS:
+		mask = INTEGRATOR_AP_SYS_BITS;
+		val = vco.v & 0xFF;
+		if (vco.v & 0x100)
+			pr_err("ICST error: tried to set bit 8 of VDW\n");
+		if (vco.s != 3)
+			pr_err("ICST error: tried to use VOD != 1\n");
+		if (vco.r != 46)
+			pr_err("ICST error: tried to use RDW != 22\n");
+		break;
+	case ICST_INTEGRATOR_CP_CM_CORE:
+		mask = INTEGRATOR_CP_CM_CORE_BITS; /* Uses 12 bits */
+		val = (vco.v & 0xFF) | vco.s << 8;
+		if (vco.v & 0x100)
+			pr_err("ICST error: tried to set bit 8 of VDW\n");
+		if (vco.r != 22)
+			pr_err("ICST error: tried to use RDW != 22\n");
+		break;
+	case ICST_INTEGRATOR_CP_CM_MEM:
+		mask = INTEGRATOR_CP_CM_MEM_BITS; /* Uses 12 bits */
+		val = ((vco.v & 0xFF) << 12) | (vco.s << 20);
+		if (vco.v & 0x100)
+			pr_err("ICST error: tried to set bit 8 of VDW\n");
+		if (vco.r != 22)
+			pr_err("ICST error: tried to use RDW != 22\n");
+		break;
+	default:
+		/* Regular auxilary oscillator */
+		mask = VERSATILE_AUX_OSC_BITS;
+		val = vco.v | (vco.r << 9) | (vco.s << 16);
+		break;
+	}
 
-	/* Mask the 18 bits used by the VCO */
-	val &= ~0x7ffff;
-	val |= vco.v | (vco.r << 9) | (vco.s << 16);
+	pr_debug("ICST: new val = 0x%08x\n", val);
 
 	/* This magic unlocks the VCO so it can be controlled */
 	ret = regmap_write(icst->map, icst->lockreg_off, VERSATILE_LOCK_VAL);
 	if (ret)
 		return ret;
-	ret = regmap_write(icst->map, icst->vcoreg_off, val);
+	ret = regmap_update_bits(icst->map, icst->vcoreg_off, mask, val);
 	if (ret)
 		return ret;
 	/* This locks the VCO again */
@@ -121,6 +253,46 @@ static long icst_round_rate(struct clk_hw *hw, unsigned long rate,
 	struct clk_icst *icst = to_icst(hw);
 	struct icst_vco vco;
 
+	if (icst->ctype == ICST_INTEGRATOR_AP_CM ||
+	    icst->ctype == ICST_INTEGRATOR_CP_CM_CORE) {
+		if (rate <= 12000000)
+			return 12000000;
+		if (rate >= 160000000)
+			return 160000000;
+		/* Slam to closest megahertz */
+		return DIV_ROUND_CLOSEST(rate, 1000000) * 1000000;
+	}
+
+	if (icst->ctype == ICST_INTEGRATOR_CP_CM_MEM) {
+		if (rate <= 6000000)
+			return 6000000;
+		if (rate >= 66000000)
+			return 66000000;
+		/* Slam to closest 0.5 megahertz */
+		return DIV_ROUND_CLOSEST(rate, 500000) * 500000;
+	}
+
+	if (icst->ctype == ICST_INTEGRATOR_AP_SYS) {
+		/* Divides between 3 and 50 MHz in steps of 0.25 MHz */
+		if (rate <= 3000000)
+			return 3000000;
+		if (rate >= 50000000)
+			return 5000000;
+		/* Slam to closest 0.25 MHz */
+		return DIV_ROUND_CLOSEST(rate, 250000) * 250000;
+	}
+
+	if (icst->ctype == ICST_INTEGRATOR_AP_PCI) {
+		/*
+		 * If we're below or less than halfway from 25 to 33 MHz
+		 * select 25 MHz
+		 */
+		if (rate <= 25000000 || rate < 29000000)
+			return 25000000;
+		/* Else just return the default frequency */
+		return 33000000;
+	}
+
 	vco = icst_hz_to_vco(icst->params, rate);
 	return icst_hz(icst->params, vco);
 }
@@ -131,6 +303,36 @@ static int icst_set_rate(struct clk_hw *hw, unsigned long rate,
 	struct clk_icst *icst = to_icst(hw);
 	struct icst_vco vco;
 
+	if (icst->ctype == ICST_INTEGRATOR_AP_PCI) {
+		/* This clock is especially primitive */
+		unsigned int val;
+		int ret;
+
+		if (rate == 25000000) {
+			val = 0;
+		} else if (rate == 33000000) {
+			val = INTEGRATOR_AP_PCI_25_33_MHZ;
+		} else {
+			pr_err("ICST: cannot set PCI frequency %lu\n",
+			       rate);
+			return -EINVAL;
+		}
+		ret = regmap_write(icst->map, icst->lockreg_off,
+				   VERSATILE_LOCK_VAL);
+		if (ret)
+			return ret;
+		ret = regmap_update_bits(icst->map, icst->vcoreg_off,
+					 INTEGRATOR_AP_PCI_25_33_MHZ,
+					 val);
+		if (ret)
+			return ret;
+		/* This locks the VCO again */
+		ret = regmap_write(icst->map, icst->lockreg_off, 0);
+		if (ret)
+			return ret;
+		return 0;
+	}
+
 	if (parent_rate)
 		icst->params->ref = parent_rate;
 	vco = icst_hz_to_vco(icst->params, rate);
@@ -148,7 +350,8 @@ static struct clk *icst_clk_setup(struct device *dev,
 				  const struct clk_icst_desc *desc,
 				  const char *name,
 				  const char *parent_name,
-				  struct regmap *map)
+				  struct regmap *map,
+				  enum icst_control_type ctype)
 {
 	struct clk *clk;
 	struct clk_icst *icst;
@@ -178,6 +381,7 @@ static struct clk *icst_clk_setup(struct device *dev,
 	icst->params = pclone;
 	icst->vcoreg_off = desc->vco_offset;
 	icst->lockreg_off = desc->lock_offset;
+	icst->ctype = ctype;
 
 	clk = clk_register(dev, &icst->hw);
 	if (IS_ERR(clk)) {
@@ -206,7 +410,8 @@ struct clk *icst_clk_register(struct device *dev,
 		pr_err("could not initialize ICST regmap\n");
 		return ERR_CAST(map);
 	}
-	return icst_clk_setup(dev, desc, name, parent_name, map);
+	return icst_clk_setup(dev, desc, name, parent_name, map,
+			      ICST_VERSATILE);
 }
 EXPORT_SYMBOL_GPL(icst_clk_register);
 
@@ -239,6 +444,56 @@ static const struct icst_params icst307_params = {
 	.idx2s		= icst307_idx2s,
 };
 
+/**
+ * The core modules on the Integrator/AP and Integrator/CP have
+ * especially crippled ICST525 control.
+ */
+static const struct icst_params icst525_apcp_cm_params = {
+	.vco_max	= ICST525_VCO_MAX_5V,
+	.vco_min	= ICST525_VCO_MIN,
+	/* Minimum 12 MHz, VDW = 4 */
+	.vd_min		= 12,
+	/*
+	 * Maximum 160 MHz, VDW = 152 for all core modules, but
+	 * CM926EJ-S, CM1026EJ-S and CM1136JF-S can actually
+	 * go to 200 MHz (max VDW = 192).
+	 */
+	.vd_max		= 192,
+	/* r is hardcoded to 22 and this is the actual divisor, +2 */
+	.rd_min		= 24,
+	.rd_max		= 24,
+	.s2div		= icst525_s2div,
+	.idx2s		= icst525_idx2s,
+};
+
+static const struct icst_params icst525_ap_sys_params = {
+	.vco_max	= ICST525_VCO_MAX_5V,
+	.vco_min	= ICST525_VCO_MIN,
+	/* Minimum 3 MHz, VDW = 4 */
+	.vd_min		= 3,
+	/* Maximum 50 MHz, VDW = 192 */
+	.vd_max		= 50,
+	/* r is hardcoded to 46 and this is the actual divisor, +2 */
+	.rd_min		= 48,
+	.rd_max		= 48,
+	.s2div		= icst525_s2div,
+	.idx2s		= icst525_idx2s,
+};
+
+static const struct icst_params icst525_ap_pci_params = {
+	.vco_max	= ICST525_VCO_MAX_5V,
+	.vco_min	= ICST525_VCO_MIN,
+	/* Minimum 25 MHz */
+	.vd_min		= 25,
+	/* Maximum 33 MHz */
+	.vd_max		= 33,
+	/* r is hardcoded to 14 or 22 and this is the actual divisors +2 */
+	.rd_min		= 16,
+	.rd_max		= 24,
+	.s2div		= icst525_s2div,
+	.idx2s		= icst525_idx2s,
+};
+
 static void __init of_syscon_icst_setup(struct device_node *np)
 {
 	struct device_node *parent;
@@ -247,6 +502,7 @@ static void __init of_syscon_icst_setup(struct device_node *np)
 	const char *name = np->name;
 	const char *parent_name;
 	struct clk *regclk;
+	enum icst_control_type ctype;
 
 	/* We do not release this reference, we are using it perpetually */
 	parent = of_get_parent(np);
@@ -269,11 +525,28 @@ static void __init of_syscon_icst_setup(struct device_node *np)
 		return;
 	}
 
-	if (of_device_is_compatible(np, "arm,syscon-icst525"))
+	if (of_device_is_compatible(np, "arm,syscon-icst525")) {
 		icst_desc.params = &icst525_params;
-	else if (of_device_is_compatible(np, "arm,syscon-icst307"))
+		ctype = ICST_VERSATILE;
+	} else if (of_device_is_compatible(np, "arm,syscon-icst307")) {
 		icst_desc.params = &icst307_params;
-	else {
+		ctype = ICST_VERSATILE;
+	} else if (of_device_is_compatible(np, "arm,syscon-icst525-integratorap-cm")) {
+		icst_desc.params = &icst525_apcp_cm_params;
+		ctype = ICST_INTEGRATOR_AP_CM;
+	} else if (of_device_is_compatible(np, "arm,syscon-icst525-integratorap-sys")) {
+		icst_desc.params = &icst525_ap_sys_params;
+		ctype = ICST_INTEGRATOR_AP_SYS;
+	} else if (of_device_is_compatible(np, "arm,syscon-icst525-integratorap-pci")) {
+		icst_desc.params = &icst525_ap_pci_params;
+		ctype = ICST_INTEGRATOR_AP_PCI;
+	} else if (of_device_is_compatible(np, "arm,syscon-icst525-integratorcp-cm-core")) {
+		icst_desc.params = &icst525_apcp_cm_params;
+		ctype = ICST_INTEGRATOR_CP_CM_CORE;
+	} else if (of_device_is_compatible(np, "arm,syscon-icst525-integratorcp-cm-mem")) {
+		icst_desc.params = &icst525_apcp_cm_params;
+		ctype = ICST_INTEGRATOR_CP_CM_MEM;
+	} else {
 		pr_err("unknown ICST clock %s\n", name);
 		return;
 	}
@@ -281,7 +554,7 @@ static void __init of_syscon_icst_setup(struct device_node *np)
 	/* Parent clock name is not the same as node parent */
 	parent_name = of_clk_get_parent_name(np, 0);
 
-	regclk = icst_clk_setup(NULL, &icst_desc, name, parent_name, map);
+	regclk = icst_clk_setup(NULL, &icst_desc, name, parent_name, map, ctype);
 	if (IS_ERR(regclk)) {
 		pr_err("error setting up syscon ICST clock %s\n", name);
 		return;
@@ -294,5 +567,14 @@ CLK_OF_DECLARE(arm_syscon_icst525_clk,
 	       "arm,syscon-icst525", of_syscon_icst_setup);
 CLK_OF_DECLARE(arm_syscon_icst307_clk,
 	       "arm,syscon-icst307", of_syscon_icst_setup);
-
+CLK_OF_DECLARE(arm_syscon_integratorap_cm_clk,
+	       "arm,syscon-icst525-integratorap-cm", of_syscon_icst_setup);
+CLK_OF_DECLARE(arm_syscon_integratorap_sys_clk,
+	       "arm,syscon-icst525-integratorap-sys", of_syscon_icst_setup);
+CLK_OF_DECLARE(arm_syscon_integratorap_pci_clk,
+	       "arm,syscon-icst525-integratorap-pci", of_syscon_icst_setup);
+CLK_OF_DECLARE(arm_syscon_integratorcp_cm_core_clk,
+	       "arm,syscon-icst525-integratorcp-cm-core", of_syscon_icst_setup);
+CLK_OF_DECLARE(arm_syscon_integratorcp_cm_mem_clk,
+	       "arm,syscon-icst525-integratorcp-cm-mem", of_syscon_icst_setup);
 #endif
diff --git a/drivers/clk/zte/Makefile b/drivers/clk/zte/Makefile
index 74005aa322a2..83374bfc4c07 100644
--- a/drivers/clk/zte/Makefile
+++ b/drivers/clk/zte/Makefile
@@ -1,2 +1,3 @@
 obj-y := clk.o
 obj-$(CONFIG_SOC_ZX296702) += clk-zx296702.o
+obj-$(CONFIG_ARCH_ZX) += clk-zx296718.o
diff --git a/drivers/clk/zte/clk-zx296718.c b/drivers/clk/zte/clk-zx296718.c
new file mode 100644
index 000000000000..707d62956e9b
--- /dev/null
+++ b/drivers/clk/zte/clk-zx296718.c
@@ -0,0 +1,924 @@
+/*
+ * Copyright (C) 2015 - 2016 ZTE Corporation.
+ * Copyright (C) 2016 Linaro Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#include <linux/clk-provider.h>
+#include <linux/device.h>
+#include <linux/kernel.h>
+#include <linux/of_address.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+
+#include <dt-bindings/clock/zx296718-clock.h>
+#include "clk.h"
+
+/* TOP CRM */
+#define TOP_CLK_MUX0	0x04
+#define TOP_CLK_MUX1	0x08
+#define TOP_CLK_MUX2	0x0c
+#define TOP_CLK_MUX3	0x10
+#define TOP_CLK_MUX4	0x14
+#define TOP_CLK_MUX5	0x18
+#define TOP_CLK_MUX6	0x1c
+#define TOP_CLK_MUX7	0x20
+#define TOP_CLK_MUX9	0x28
+
+
+#define TOP_CLK_GATE0	0x34
+#define TOP_CLK_GATE1	0x38
+#define TOP_CLK_GATE2	0x3c
+#define TOP_CLK_GATE3	0x40
+#define TOP_CLK_GATE4	0x44
+#define TOP_CLK_GATE5	0x48
+#define TOP_CLK_GATE6	0x4c
+
+#define TOP_CLK_DIV0	0x58
+
+#define PLL_CPU_REG	0x80
+#define PLL_VGA_REG	0xb0
+#define PLL_DDR_REG	0xa0
+
+/* LSP0 CRM */
+#define LSP0_TIMER3_CLK	0x4
+#define LSP0_TIMER4_CLK	0x8
+#define LSP0_TIMER5_CLK	0xc
+#define LSP0_UART3_CLK	0x10
+#define LSP0_UART1_CLK	0x14
+#define LSP0_UART2_CLK	0x18
+#define LSP0_SPIFC0_CLK	0x1c
+#define LSP0_I2C4_CLK	0x20
+#define LSP0_I2C5_CLK	0x24
+#define LSP0_SSP0_CLK	0x28
+#define LSP0_SSP1_CLK	0x2c
+#define LSP0_USIM0_CLK	0x30
+#define LSP0_GPIO_CLK	0x34
+#define LSP0_I2C3_CLK	0x38
+
+/* LSP1 CRM */
+#define LSP1_UART4_CLK	0x08
+#define LSP1_UART5_CLK	0x0c
+#define LSP1_PWM_CLK	0x10
+#define LSP1_I2C2_CLK	0x14
+#define LSP1_SSP2_CLK	0x1c
+#define LSP1_SSP3_CLK	0x20
+#define LSP1_SSP4_CLK	0x24
+#define LSP1_USIM1_CLK	0x28
+
+/* audio lsp */
+#define AUDIO_I2S0_DIV_CFG1	0x10
+#define AUDIO_I2S0_DIV_CFG2	0x14
+#define AUDIO_I2S0_CLK		0x18
+#define AUDIO_I2S1_DIV_CFG1	0x20
+#define AUDIO_I2S1_DIV_CFG2	0x24
+#define AUDIO_I2S1_CLK		0x28
+#define AUDIO_I2S2_DIV_CFG1	0x30
+#define AUDIO_I2S2_DIV_CFG2	0x34
+#define AUDIO_I2S2_CLK		0x38
+#define AUDIO_I2S3_DIV_CFG1	0x40
+#define AUDIO_I2S3_DIV_CFG2	0x44
+#define AUDIO_I2S3_CLK		0x48
+#define AUDIO_I2C0_CLK		0x50
+#define AUDIO_SPDIF0_DIV_CFG1	0x60
+#define AUDIO_SPDIF0_DIV_CFG2	0x64
+#define AUDIO_SPDIF0_CLK	0x68
+#define AUDIO_SPDIF1_DIV_CFG1	0x70
+#define AUDIO_SPDIF1_DIV_CFG2	0x74
+#define AUDIO_SPDIF1_CLK	0x78
+#define AUDIO_TIMER_CLK		0x80
+#define AUDIO_TDM_CLK		0x90
+#define AUDIO_TS_CLK		0xa0
+
+static DEFINE_SPINLOCK(clk_lock);
+
+static struct zx_pll_config pll_cpu_table[] = {
+	PLL_RATE(1312000000, 0x00103621, 0x04aaaaaa),
+	PLL_RATE(1407000000, 0x00103a21, 0x04aaaaaa),
+	PLL_RATE(1503000000, 0x00103e21, 0x04aaaaaa),
+	PLL_RATE(1600000000, 0x00104221, 0x04aaaaaa),
+};
+
+PNAME(osc) = {
+	"osc24m",
+	"osc32k",
+};
+
+PNAME(dbg_wclk_p) = {
+	"clk334m",
+	"clk466m",
+	"clk396m",
+	"clk250m",
+};
+
+PNAME(a72_coreclk_p) = {
+	"osc24m",
+	"pll_mm0_1188m",
+	"pll_mm1_1296m",
+	"clk1000m",
+	"clk648m",
+	"clk1600m",
+	"pll_audio_1800m",
+	"pll_vga_1800m",
+};
+
+PNAME(cpu_periclk_p) = {
+	"osc24m",
+	"clk500m",
+	"clk594m",
+	"clk466m",
+	"clk294m",
+	"clk334m",
+	"clk250m",
+	"clk125m",
+};
+
+PNAME(a53_coreclk_p) = {
+	"osc24m",
+	"clk1000m",
+	"pll_mm0_1188m",
+	"clk648m",
+	"clk500m",
+	"clk800m",
+	"clk1600m",
+	"pll_audio_1800m",
+};
+
+PNAME(sec_wclk_p) = {
+	"osc24m",
+	"clk396m",
+	"clk334m",
+	"clk297m",
+	"clk250m",
+	"clk198m",
+	"clk148m5",
+	"clk99m",
+};
+
+PNAME(sd_nand_wclk_p) = {
+	"osc24m",
+	"clk49m5",
+	"clk99m",
+	"clk198m",
+	"clk167m",
+	"clk148m5",
+	"clk125m",
+	"clk216m",
+};
+
+PNAME(emmc_wclk_p) = {
+	"osc24m",
+	"clk198m",
+	"clk99m",
+	"clk396m",
+	"clk334m",
+	"clk297m",
+	"clk250m",
+	"clk148m5",
+};
+
+PNAME(clk32_p) = {
+	"osc32k",
+	"clk32k768",
+};
+
+PNAME(usb_ref24m_p) = {
+	"osc32k",
+	"clk32k768",
+};
+
+PNAME(sys_noc_alck_p) = {
+	"osc24m",
+	"clk250m",
+	"clk198m",
+	"clk148m5",
+	"clk108m",
+	"clk54m",
+	"clk216m",
+	"clk240m",
+};
+
+PNAME(vde_aclk_p) = {
+	"clk334m",
+	"clk594m",
+	"clk500m",
+	"clk432m",
+	"clk480m",
+	"clk297m",
+	"clk_vga",  /*600MHz*/
+	"clk294m",
+};
+
+PNAME(vce_aclk_p) = {
+	"clk334m",
+	"clk594m",
+	"clk500m",
+	"clk432m",
+	"clk396m",
+	"clk297m",
+	"clk_vga",  /*600MHz*/
+	"clk294m",
+};
+
+PNAME(hde_aclk_p) = {
+	"clk334m",
+	"clk594m",
+	"clk500m",
+	"clk432m",
+	"clk396m",
+	"clk297m",
+	"clk_vga",  /*600MHz*/
+	"clk294m",
+};
+
+PNAME(gpu_aclk_p) = {
+	"clk334m",
+	"clk648m",
+	"clk594m",
+	"clk500m",
+	"clk396m",
+	"clk297m",
+	"clk_vga",  /*600MHz*/
+	"clk294m",
+};
+
+PNAME(sappu_aclk_p) = {
+	"clk396m",
+	"clk500m",
+	"clk250m",
+	"clk148m5",
+};
+
+PNAME(sappu_wclk_p) = {
+	"clk198m",
+	"clk396m",
+	"clk334m",
+	"clk297m",
+	"clk250m",
+	"clk148m5",
+	"clk125m",
+	"clk99m",
+};
+
+PNAME(vou_aclk_p) = {
+	"clk334m",
+	"clk594m",
+	"clk500m",
+	"clk432m",
+	"clk396m",
+	"clk297m",
+	"clk_vga",  /*600MHz*/
+	"clk294m",
+};
+
+PNAME(vou_main_wclk_p) = {
+	"clk108m",
+	"clk594m",
+	"clk297m",
+	"clk148m5",
+	"clk74m25",
+	"clk54m",
+	"clk27m",
+	"clk_vga",
+};
+
+PNAME(vou_aux_wclk_p) = {
+	"clk108m",
+	"clk148m5",
+	"clk74m25",
+	"clk54m",
+	"clk27m",
+	"clk_vga",
+	"clk54m_mm0",
+	"clk"
+};
+
+PNAME(vou_ppu_wclk_p) = {
+	"clk334m",
+	"clk432m",
+	"clk396m",
+	"clk297m",
+	"clk250m",
+	"clk125m",
+	"clk198m",
+	"clk99m",
+};
+
+PNAME(vga_i2c_wclk_p) = {
+	"osc24m",
+	"clk99m",
+};
+
+PNAME(viu_m0_aclk_p) = {
+	"clk334m",
+	"clk432m",
+	"clk396m",
+	"clk297m",
+	"clk250m",
+	"clk125m",
+	"clk198m",
+	"osc24m",
+};
+
+PNAME(viu_m1_aclk_p) = {
+	"clk198m",
+	"clk250m",
+	"clk297m",
+	"clk125m",
+	"clk396m",
+	"clk334m",
+	"clk148m5",
+	"osc24m",
+};
+
+PNAME(viu_clk_p) = {
+	"clk198m",
+	"clk334m",
+	"clk297m",
+	"clk250m",
+	"clk396m",
+	"clk125m",
+	"clk99m",
+	"clk148m5",
+};
+
+PNAME(viu_jpeg_clk_p) = {
+	"clk334m",
+	"clk480m",
+	"clk432m",
+	"clk396m",
+	"clk297m",
+	"clk250m",
+	"clk125m",
+	"clk198m",
+};
+
+PNAME(ts_sys_clk_p) = {
+	"clk192m",
+	"clk167m",
+	"clk125m",
+	"clk99m",
+};
+
+PNAME(wdt_ares_p) = {
+	"osc24m",
+	"clk32k"
+};
+
+static struct clk_zx_pll zx296718_pll_clk[] = {
+	ZX296718_PLL("pll_cpu",	"osc24m",	PLL_CPU_REG,	pll_cpu_table),
+};
+
+static struct zx_clk_fixed_factor top_ffactor_clk[] = {
+	FFACTOR(0, "clk4m",		"osc24m", 1, 6,  0),
+	FFACTOR(0, "clk2m",		"osc24m", 1, 12, 0),
+	/* pll cpu */
+	FFACTOR(0, "clk1600m",		"pll_cpu", 1, 1, CLK_SET_RATE_PARENT),
+	FFACTOR(0, "clk800m",		"pll_cpu", 1, 2, CLK_SET_RATE_PARENT),
+	/* pll mac */
+	FFACTOR(0, "clk25m",		"pll_mac", 1, 40, 0),
+	FFACTOR(0, "clk125m",		"pll_mac", 1, 8, 0),
+	FFACTOR(0, "clk250m",		"pll_mac", 1, 4, 0),
+	FFACTOR(0, "clk50m",		"pll_mac", 1, 20, 0),
+	FFACTOR(0, "clk500m",		"pll_mac", 1, 2, 0),
+	FFACTOR(0, "clk1000m",		"pll_mac", 1, 1, 0),
+	FFACTOR(0, "clk334m",		"pll_mac", 1, 3, 0),
+	FFACTOR(0, "clk167m",		"pll_mac", 1, 6, 0),
+	/* pll mm */
+	FFACTOR(0, "clk54m_mm0",	"pll_mm0", 1, 22, 0),
+	FFACTOR(0, "clk74m25",		"pll_mm0", 1, 16, 0),
+	FFACTOR(0, "clk148m5",		"pll_mm0", 1, 8, 0),
+	FFACTOR(0, "clk297m",		"pll_mm0", 1, 4, 0),
+	FFACTOR(0, "clk594m",		"pll_mm0", 1, 2, 0),
+	FFACTOR(0, "pll_mm0_1188m",	"pll_mm0", 1, 1, 0),
+	FFACTOR(0, "clk396m",		"pll_mm0", 1, 3, 0),
+	FFACTOR(0, "clk198m",		"pll_mm0", 1, 6, 0),
+	FFACTOR(0, "clk99m",		"pll_mm0", 1, 12, 0),
+	FFACTOR(0, "clk49m5",		"pll_mm0", 1, 24, 0),
+	/* pll mm */
+	FFACTOR(0, "clk324m",		"pll_mm1", 1, 4, 0),
+	FFACTOR(0, "clk648m",		"pll_mm1", 1, 2, 0),
+	FFACTOR(0, "pll_mm1_1296m",	"pll_mm1", 1, 1, 0),
+	FFACTOR(0, "clk216m",		"pll_mm1", 1, 6, 0),
+	FFACTOR(0, "clk432m",		"pll_mm1", 1, 3, 0),
+	FFACTOR(0, "clk108m",		"pll_mm1", 1, 12, 0),
+	FFACTOR(0, "clk72m",		"pll_mm1", 1, 18, 0),
+	FFACTOR(0, "clk27m",		"pll_mm1", 1, 48, 0),
+	FFACTOR(0, "clk54m",		"pll_mm1", 1, 24, 0),
+	/* vga */
+	FFACTOR(0, "pll_vga_1800m",	"pll_vga", 1, 1, 0),
+	FFACTOR(0, "clk_vga",		"pll_vga", 1, 2, 0),
+	/* pll ddr */
+	FFACTOR(0, "clk466m",		"pll_ddr", 1, 2, 0),
+
+	/* pll audio */
+	FFACTOR(0, "pll_audio_1800m",	"pll_audio", 1, 1, 0),
+	FFACTOR(0, "clk32k768",		"pll_audio", 1, 27000, 0),
+	FFACTOR(0, "clk16m384",		"pll_audio", 1, 54, 0),
+	FFACTOR(0, "clk294m",		"pll_audio", 1, 3, 0),
+
+	/* pll hsic*/
+	FFACTOR(0, "clk240m",		"pll_hsic", 1, 4, 0),
+	FFACTOR(0, "clk480m",		"pll_hsic", 1, 2, 0),
+	FFACTOR(0, "clk192m",		"pll_hsic", 1, 5, 0),
+	FFACTOR(0, "clk_pll_24m",	"pll_hsic", 1, 40, 0),
+	FFACTOR(0, "emmc_mux_div2",	"emmc_mux", 1, 2, CLK_SET_RATE_PARENT),
+};
+
+static struct clk_div_table noc_div_table[] = {
+	{ .val = 1, .div = 2, },
+	{ .val = 3, .div = 4, },
+};
+static struct zx_clk_div top_div_clk[] = {
+	DIV_T(0, "sys_noc_hclk", "sys_noc_aclk", TOP_CLK_DIV0, 0, 2, 0, noc_div_table),
+	DIV_T(0, "sys_noc_pclk", "sys_noc_aclk", TOP_CLK_DIV0, 4, 2, 0, noc_div_table),
+};
+
+static struct zx_clk_mux top_mux_clk[] = {
+	MUX(0, "dbg_mux",	 dbg_wclk_p,	  TOP_CLK_MUX0, 12, 2),
+	MUX(0, "a72_mux",	 a72_coreclk_p,	  TOP_CLK_MUX0, 8, 3),
+	MUX(0, "cpu_peri_mux",	 cpu_periclk_p,	  TOP_CLK_MUX0, 4, 3),
+	MUX_F(0, "a53_mux",	 a53_coreclk_p,	  TOP_CLK_MUX0, 0, 3, CLK_SET_RATE_PARENT, 0),
+	MUX(0, "sys_noc_aclk",	 sys_noc_alck_p,  TOP_CLK_MUX1, 0, 3),
+	MUX(0, "sec_mux",	 sec_wclk_p,	  TOP_CLK_MUX2, 16, 3),
+	MUX(0, "sd1_mux",	 sd_nand_wclk_p,  TOP_CLK_MUX2, 12, 3),
+	MUX(0, "sd0_mux",	 sd_nand_wclk_p,  TOP_CLK_MUX2, 8, 3),
+	MUX(0, "emmc_mux",	 emmc_wclk_p,	  TOP_CLK_MUX2, 4, 3),
+	MUX(0, "nand_mux",	 sd_nand_wclk_p,  TOP_CLK_MUX2, 0, 3),
+	MUX(0, "usb_ref24m_mux", usb_ref24m_p,	  TOP_CLK_MUX9, 16, 1),
+	MUX(0, "clk32k",	 clk32_p,	  TOP_CLK_MUX9, 12, 1),
+	MUX_F(0, "wdt_mux",	 wdt_ares_p,	  TOP_CLK_MUX9, 8, 1, CLK_SET_RATE_PARENT, 0),
+	MUX(0, "timer_mux",	 osc,		  TOP_CLK_MUX9, 4, 1),
+	MUX(0, "vde_mux",	 vde_aclk_p,	  TOP_CLK_MUX4,  0, 3),
+	MUX(0, "vce_mux",	 vce_aclk_p,	  TOP_CLK_MUX4,  4, 3),
+	MUX(0, "hde_mux",	 hde_aclk_p,	  TOP_CLK_MUX4,  8, 3),
+	MUX(0, "gpu_mux",	 gpu_aclk_p,	  TOP_CLK_MUX5,  0, 3),
+	MUX(0, "sappu_a_mux",	 sappu_aclk_p,	  TOP_CLK_MUX5,  4, 2),
+	MUX(0, "sappu_w_mux",	 sappu_wclk_p,	  TOP_CLK_MUX5,  8, 3),
+	MUX(0, "vou_a_mux",	 vou_aclk_p,	  TOP_CLK_MUX7,  0, 3),
+	MUX(0, "vou_main_w_mux", vou_main_wclk_p, TOP_CLK_MUX7,  4, 3),
+	MUX(0, "vou_aux_w_mux",	 vou_aux_wclk_p,  TOP_CLK_MUX7,  8, 3),
+	MUX(0, "vou_ppu_w_mux",	 vou_ppu_wclk_p,  TOP_CLK_MUX7, 12, 3),
+	MUX(0, "vga_i2c_mux",	 vga_i2c_wclk_p,  TOP_CLK_MUX7, 16, 1),
+	MUX(0, "viu_m0_a_mux",	 viu_m0_aclk_p,	  TOP_CLK_MUX6,  0, 3),
+	MUX(0, "viu_m1_a_mux",	 viu_m1_aclk_p,	  TOP_CLK_MUX6,  4, 3),
+	MUX(0, "viu_w_mux",	 viu_clk_p,	  TOP_CLK_MUX6,  8, 3),
+	MUX(0, "viu_jpeg_w_mux", viu_jpeg_clk_p,  TOP_CLK_MUX6, 12, 3),
+	MUX(0, "ts_sys_mux",	 ts_sys_clk_p,    TOP_CLK_MUX6, 16, 2),
+};
+
+static struct zx_clk_gate top_gate_clk[] = {
+	GATE(CPU_DBG_GATE,    "dbg_wclk",        "dbg_mux",        TOP_CLK_GATE0, 4, CLK_SET_RATE_PARENT, 0),
+	GATE(A72_GATE,        "a72_coreclk",     "a72_mux",        TOP_CLK_GATE0, 3, CLK_SET_RATE_PARENT, 0),
+	GATE(CPU_PERI_GATE,   "cpu_peri",        "cpu_peri_mux",   TOP_CLK_GATE0, 1, CLK_SET_RATE_PARENT, 0),
+	GATE(A53_GATE,        "a53_coreclk",     "a53_mux",        TOP_CLK_GATE0, 0, CLK_SET_RATE_PARENT, 0),
+	GATE(SD1_WCLK,        "sd1_wclk",        "sd1_mux",        TOP_CLK_GATE1, 13, CLK_SET_RATE_PARENT, 0),
+	GATE(SD0_WCLK,        "sd0_wclk",        "sd0_mux",        TOP_CLK_GATE1, 9, CLK_SET_RATE_PARENT, 0),
+	GATE(EMMC_WCLK,       "emmc_wclk",       "emmc_mux_div2",  TOP_CLK_GATE0, 5, CLK_SET_RATE_PARENT, 0),
+	GATE(EMMC_NAND_AXI,   "emmc_nand_aclk",  "sys_noc_aclk",   TOP_CLK_GATE1, 4, CLK_SET_RATE_PARENT, 0),
+	GATE(NAND_WCLK,       "nand_wclk",       "nand_mux",       TOP_CLK_GATE0, 1, CLK_SET_RATE_PARENT, 0),
+	GATE(EMMC_NAND_AHB,   "emmc_nand_hclk",  "sys_noc_hclk",   TOP_CLK_GATE1, 0, CLK_SET_RATE_PARENT, 0),
+	GATE(0,               "lsp1_pclk",       "sys_noc_pclk",   TOP_CLK_GATE2, 31, 0,                  0),
+	GATE(LSP1_148M5,      "lsp1_148m5",      "clk148m5",       TOP_CLK_GATE2, 30, 0,                  0),
+	GATE(LSP1_99M,        "lsp1_99m",        "clk99m",         TOP_CLK_GATE2, 29, 0,                  0),
+	GATE(LSP1_24M,        "lsp1_24m",        "osc24m",         TOP_CLK_GATE2, 28, 0,                  0),
+	GATE(LSP0_74M25,      "lsp0_74m25",      "clk74m25",       TOP_CLK_GATE2, 25, 0,                  0),
+	GATE(0,               "lsp0_pclk",       "sys_noc_pclk",   TOP_CLK_GATE2, 24, 0,                  0),
+	GATE(LSP0_32K,        "lsp0_32k",        "osc32k",         TOP_CLK_GATE2, 23, 0,                  0),
+	GATE(LSP0_148M5,      "lsp0_148m5",      "clk148m5",       TOP_CLK_GATE2, 22, 0,                  0),
+	GATE(LSP0_99M,        "lsp0_99m",        "clk99m",         TOP_CLK_GATE2, 21, 0,                  0),
+	GATE(LSP0_24M,        "lsp0_24m",        "osc24m",         TOP_CLK_GATE2, 20, 0,                  0),
+	GATE(AUDIO_99M,       "audio_99m",       "clk99m",         TOP_CLK_GATE5, 27, 0,                  0),
+	GATE(AUDIO_24M,       "audio_24m",       "osc24m",         TOP_CLK_GATE5, 28, 0,                  0),
+	GATE(AUDIO_16M384,    "audio_16m384",    "clk16m384",      TOP_CLK_GATE5, 29, 0,                  0),
+	GATE(AUDIO_32K,       "audio_32k",       "clk32k",         TOP_CLK_GATE5, 30, 0,                  0),
+	GATE(WDT_WCLK,        "wdt_wclk",        "wdt_mux",        TOP_CLK_GATE6, 9, CLK_SET_RATE_PARENT, 0),
+	GATE(TIMER_WCLK,      "timer_wclk",      "timer_mux",      TOP_CLK_GATE6, 5, CLK_SET_RATE_PARENT, 0),
+	GATE(VDE_ACLK,        "vde_aclk",        "vde_mux",        TOP_CLK_GATE3, 0,  CLK_SET_RATE_PARENT, 0),
+	GATE(VCE_ACLK,        "vce_aclk",        "vce_mux",        TOP_CLK_GATE3, 4,  CLK_SET_RATE_PARENT, 0),
+	GATE(HDE_ACLK,        "hde_aclk",        "hde_mux",        TOP_CLK_GATE3, 8,  CLK_SET_RATE_PARENT, 0),
+	GATE(GPU_ACLK,        "gpu_aclk",        "gpu_mux",        TOP_CLK_GATE3, 16, CLK_SET_RATE_PARENT, 0),
+	GATE(SAPPU_ACLK,      "sappu_aclk",      "sappu_a_mux",    TOP_CLK_GATE3, 20, CLK_SET_RATE_PARENT, 0),
+	GATE(SAPPU_WCLK,      "sappu_wclk",      "sappu_w_mux",    TOP_CLK_GATE3, 22, CLK_SET_RATE_PARENT, 0),
+	GATE(VOU_ACLK,        "vou_aclk",        "vou_a_mux",      TOP_CLK_GATE4, 16, CLK_SET_RATE_PARENT, 0),
+	GATE(VOU_MAIN_WCLK,   "vou_main_wclk",   "vou_main_w_mux", TOP_CLK_GATE4, 18, CLK_SET_RATE_PARENT, 0),
+	GATE(VOU_AUX_WCLK,    "vou_aux_wclk",    "vou_aux_w_mux",  TOP_CLK_GATE4, 19, CLK_SET_RATE_PARENT, 0),
+	GATE(VOU_PPU_WCLK,    "vou_ppu_wclk",    "vou_ppu_w_mux",  TOP_CLK_GATE4, 20, CLK_SET_RATE_PARENT, 0),
+	GATE(MIPI_CFG_CLK,    "mipi_cfg_clk",    "osc24m",         TOP_CLK_GATE4, 21, 0,                   0),
+	GATE(VGA_I2C_WCLK,    "vga_i2c_wclk",    "vga_i2c_mux",    TOP_CLK_GATE4, 23, CLK_SET_RATE_PARENT, 0),
+	GATE(MIPI_REF_CLK,    "mipi_ref_clk",    "clk27m",         TOP_CLK_GATE4, 24, 0,                   0),
+	GATE(HDMI_OSC_CEC,    "hdmi_osc_cec",    "clk2m",          TOP_CLK_GATE4, 22, 0,                   0),
+	GATE(HDMI_OSC_CLK,    "hdmi_osc_clk",    "clk240m",        TOP_CLK_GATE4, 25, 0,                   0),
+	GATE(HDMI_XCLK,       "hdmi_xclk",       "osc24m",         TOP_CLK_GATE4, 26, 0,                   0),
+	GATE(VIU_M0_ACLK,     "viu_m0_aclk",     "viu_m0_a_mux",   TOP_CLK_GATE4, 0,  CLK_SET_RATE_PARENT, 0),
+	GATE(VIU_M1_ACLK,     "viu_m1_aclk",     "viu_m1_a_mux",   TOP_CLK_GATE4, 1,  CLK_SET_RATE_PARENT, 0),
+	GATE(VIU_WCLK,        "viu_wclk",        "viu_w_mux",      TOP_CLK_GATE4, 2,  CLK_SET_RATE_PARENT, 0),
+	GATE(VIU_JPEG_WCLK,   "viu_jpeg_wclk",   "viu_jpeg_w_mux", TOP_CLK_GATE4, 3,  CLK_SET_RATE_PARENT, 0),
+	GATE(VIU_CFG_CLK,     "viu_cfg_clk",     "osc24m",         TOP_CLK_GATE4, 6,  0,                   0),
+	GATE(TS_SYS_WCLK,     "ts_sys_wclk",     "ts_sys_mux",     TOP_CLK_GATE5, 2,  CLK_SET_RATE_PARENT, 0),
+	GATE(TS_SYS_108M,     "ts_sys_108m",     "clk108m",        TOP_CLK_GATE5, 3,  0,                   0),
+	GATE(USB20_HCLK,      "usb20_hclk",      "sys_noc_hclk",   TOP_CLK_GATE2, 12, 0,                   0),
+	GATE(USB20_PHY_CLK,   "usb20_phy_clk",   "usb_ref24m_mux", TOP_CLK_GATE2, 13, 0,                   0),
+	GATE(USB21_HCLK,      "usb21_hclk",      "sys_noc_hclk",   TOP_CLK_GATE2, 14, 0,                   0),
+	GATE(USB21_PHY_CLK,   "usb21_phy_clk",   "usb_ref24m_mux", TOP_CLK_GATE2, 15, 0,                   0),
+	GATE(GMAC_RMIICLK,    "gmac_rmii_clk",   "clk50m",         TOP_CLK_GATE2, 3, 0,                    0),
+	GATE(GMAC_PCLK,       "gmac_pclk",       "clk198m",        TOP_CLK_GATE2, 1, 0,                    0),
+	GATE(GMAC_ACLK,       "gmac_aclk",       "clk49m5",        TOP_CLK_GATE2, 0, 0,                    0),
+	GATE(GMAC_RFCLK,      "gmac_refclk",     "clk25m",         TOP_CLK_GATE2, 4, 0,                    0),
+	GATE(SD1_AHB,         "sd1_hclk",        "sys_noc_hclk",   TOP_CLK_GATE1, 12,  0,                  0),
+	GATE(SD0_AHB,         "sd0_hclk",        "sys_noc_hclk",   TOP_CLK_GATE1, 8,  0,                   0),
+	GATE(TEMPSENSOR_GATE, "tempsensor_gate", "clk4m",          TOP_CLK_GATE5, 31,  0,                  0),
+};
+
+static struct clk_hw_onecell_data top_hw_onecell_data = {
+	.num = TOP_NR_CLKS,
+	.hws = {
+		[TOP_NR_CLKS - 1] = NULL,
+	},
+};
+
+static int __init top_clocks_init(struct device_node *np)
+{
+	void __iomem *reg_base;
+	int i, ret;
+
+	reg_base = of_iomap(np, 0);
+	if (!reg_base) {
+		pr_err("%s: Unable to map clk base\n", __func__);
+		return -ENXIO;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(zx296718_pll_clk); i++) {
+		zx296718_pll_clk[i].reg_base += (uintptr_t)reg_base;
+		ret = clk_hw_register(NULL, &zx296718_pll_clk[i].hw);
+		if (ret) {
+			pr_warn("top clk %s init error!\n",
+				zx296718_pll_clk[i].hw.init->name);
+		}
+	}
+
+	for (i = 0; i < ARRAY_SIZE(top_ffactor_clk); i++) {
+		if (top_ffactor_clk[i].id)
+			top_hw_onecell_data.hws[top_ffactor_clk[i].id] =
+					&top_ffactor_clk[i].factor.hw;
+
+		ret = clk_hw_register(NULL, &top_ffactor_clk[i].factor.hw);
+		if (ret) {
+			pr_warn("top clk %s init error!\n",
+				top_ffactor_clk[i].factor.hw.init->name);
+		}
+	}
+
+	for (i = 0; i < ARRAY_SIZE(top_mux_clk); i++) {
+		if (top_mux_clk[i].id)
+			top_hw_onecell_data.hws[top_mux_clk[i].id] =
+					&top_mux_clk[i].mux.hw;
+
+		top_mux_clk[i].mux.reg += (uintptr_t)reg_base;
+		ret = clk_hw_register(NULL, &top_mux_clk[i].mux.hw);
+		if (ret) {
+			pr_warn("top clk %s init error!\n",
+				top_mux_clk[i].mux.hw.init->name);
+		}
+	}
+
+	for (i = 0; i < ARRAY_SIZE(top_gate_clk); i++) {
+		if (top_gate_clk[i].id)
+			top_hw_onecell_data.hws[top_gate_clk[i].id] =
+					&top_gate_clk[i].gate.hw;
+
+		top_gate_clk[i].gate.reg += (uintptr_t)reg_base;
+		ret = clk_hw_register(NULL, &top_gate_clk[i].gate.hw);
+		if (ret) {
+			pr_warn("top clk %s init error!\n",
+				top_gate_clk[i].gate.hw.init->name);
+		}
+	}
+
+	for (i = 0; i < ARRAY_SIZE(top_div_clk); i++) {
+		if (top_div_clk[i].id)
+			top_hw_onecell_data.hws[top_div_clk[i].id] =
+					&top_div_clk[i].div.hw;
+
+		top_div_clk[i].div.reg += (uintptr_t)reg_base;
+		ret = clk_hw_register(NULL, &top_div_clk[i].div.hw);
+		if (ret) {
+			pr_warn("top clk %s init error!\n",
+				top_div_clk[i].div.hw.init->name);
+		}
+	}
+
+	if (of_clk_add_hw_provider(np, of_clk_hw_onecell_get, &top_hw_onecell_data))
+		panic("could not register clk provider\n");
+	pr_info("top clk init over, nr:%d\n", TOP_NR_CLKS);
+
+	return 0;
+}
+
+static struct clk_div_table common_even_div_table[] = {
+	{ .val = 0, .div = 1, },
+	{ .val = 1, .div = 2, },
+	{ .val = 3, .div = 4, },
+	{ .val = 5, .div = 6, },
+	{ .val = 7, .div = 8, },
+	{ .val = 9, .div = 10, },
+	{ .val = 11, .div = 12, },
+	{ .val = 13, .div = 14, },
+	{ .val = 15, .div = 16, },
+};
+
+static struct clk_div_table common_div_table[] = {
+	{ .val = 0, .div = 1, },
+	{ .val = 1, .div = 2, },
+	{ .val = 2, .div = 3, },
+	{ .val = 3, .div = 4, },
+	{ .val = 4, .div = 5, },
+	{ .val = 5, .div = 6, },
+	{ .val = 6, .div = 7, },
+	{ .val = 7, .div = 8, },
+	{ .val = 8, .div = 9, },
+	{ .val = 9, .div = 10, },
+	{ .val = 10, .div = 11, },
+	{ .val = 11, .div = 12, },
+	{ .val = 12, .div = 13, },
+	{ .val = 13, .div = 14, },
+	{ .val = 14, .div = 15, },
+	{ .val = 15, .div = 16, },
+};
+
+PNAME(lsp0_wclk_common_p) = {
+	"lsp0_24m",
+	"lsp0_99m",
+};
+
+PNAME(lsp0_wclk_timer3_p) = {
+	"timer3_div",
+	"lsp0_32k"
+};
+
+PNAME(lsp0_wclk_timer4_p) = {
+	"timer4_div",
+	"lsp0_32k"
+};
+
+PNAME(lsp0_wclk_timer5_p) = {
+	"timer5_div",
+	"lsp0_32k"
+};
+
+PNAME(lsp0_wclk_spifc0_p) = {
+	"lsp0_148m5",
+	"lsp0_24m",
+	"lsp0_99m",
+	"lsp0_74m25"
+};
+
+PNAME(lsp0_wclk_ssp_p) = {
+	"lsp0_148m5",
+	"lsp0_99m",
+	"lsp0_24m",
+};
+
+static struct zx_clk_mux lsp0_mux_clk[] = {
+	MUX(0, "timer3_wclk_mux", lsp0_wclk_timer3_p, LSP0_TIMER3_CLK, 4, 1),
+	MUX(0, "timer4_wclk_mux", lsp0_wclk_timer4_p, LSP0_TIMER4_CLK, 4, 1),
+	MUX(0, "timer5_wclk_mux", lsp0_wclk_timer5_p, LSP0_TIMER5_CLK, 4, 1),
+	MUX(0, "uart3_wclk_mux",  lsp0_wclk_common_p, LSP0_UART3_CLK,  4, 1),
+	MUX(0, "uart1_wclk_mux",  lsp0_wclk_common_p, LSP0_UART1_CLK,  4, 1),
+	MUX(0, "uart2_wclk_mux",  lsp0_wclk_common_p, LSP0_UART2_CLK,  4, 1),
+	MUX(0, "spifc0_wclk_mux", lsp0_wclk_spifc0_p, LSP0_SPIFC0_CLK, 4, 2),
+	MUX(0, "i2c4_wclk_mux",   lsp0_wclk_common_p, LSP0_I2C4_CLK,   4, 1),
+	MUX(0, "i2c5_wclk_mux",   lsp0_wclk_common_p, LSP0_I2C5_CLK,   4, 1),
+	MUX(0, "ssp0_wclk_mux",   lsp0_wclk_ssp_p,    LSP0_SSP0_CLK,   4, 1),
+	MUX(0, "ssp1_wclk_mux",   lsp0_wclk_ssp_p,    LSP0_SSP1_CLK,   4, 1),
+	MUX(0, "i2c3_wclk_mux",   lsp0_wclk_common_p, LSP0_I2C3_CLK,   4, 1),
+};
+
+static struct zx_clk_gate lsp0_gate_clk[] = {
+	GATE(LSP0_TIMER3_WCLK, "timer3_wclk", "timer3_wclk_mux", LSP0_TIMER3_CLK, 1, CLK_SET_RATE_PARENT, 0),
+	GATE(LSP0_TIMER4_WCLK, "timer4_wclk", "timer4_wclk_mux", LSP0_TIMER4_CLK, 1, CLK_SET_RATE_PARENT, 0),
+	GATE(LSP0_TIMER5_WCLK, "timer5_wclk", "timer5_wclk_mux", LSP0_TIMER5_CLK, 1, CLK_SET_RATE_PARENT, 0),
+	GATE(LSP0_UART3_WCLK,  "uart3_wclk",  "uart3_wclk_mux",  LSP0_UART3_CLK,  1, CLK_SET_RATE_PARENT, 0),
+	GATE(LSP0_UART1_WCLK,  "uart1_wclk",  "uart1_wclk_mux",  LSP0_UART1_CLK,  1, CLK_SET_RATE_PARENT, 0),
+	GATE(LSP0_UART2_WCLK,  "uart2_wclk",  "uart2_wclk_mux",  LSP0_UART2_CLK,  1, CLK_SET_RATE_PARENT, 0),
+	GATE(LSP0_SPIFC0_WCLK, "spifc0_wclk", "spifc0_wclk_mux", LSP0_SPIFC0_CLK, 1, CLK_SET_RATE_PARENT, 0),
+	GATE(LSP0_I2C4_WCLK,   "i2c4_wclk",   "i2c4_wclk_mux",   LSP0_I2C4_CLK,   1, CLK_SET_RATE_PARENT, 0),
+	GATE(LSP0_I2C5_WCLK,   "i2c5_wclk",   "i2c5_wclk_mux",   LSP0_I2C5_CLK,   1, CLK_SET_RATE_PARENT, 0),
+	GATE(LSP0_SSP0_WCLK,   "ssp0_wclk",   "ssp0_div",        LSP0_SSP0_CLK,   1, CLK_SET_RATE_PARENT, 0),
+	GATE(LSP0_SSP1_WCLK,   "ssp1_wclk",   "ssp1_div",        LSP0_SSP1_CLK,   1, CLK_SET_RATE_PARENT, 0),
+	GATE(LSP0_I2C3_WCLK,   "i2c3_wclk",   "i2c3_wclk_mux",   LSP0_I2C3_CLK,   1, CLK_SET_RATE_PARENT, 0),
+};
+
+static struct zx_clk_div lsp0_div_clk[] = {
+	DIV_T(0, "timer3_div", "lsp0_24m", LSP0_TIMER3_CLK,  12, 4, 0, common_even_div_table),
+	DIV_T(0, "timer4_div", "lsp0_24m", LSP0_TIMER4_CLK,  12, 4, 0, common_even_div_table),
+	DIV_T(0, "timer5_div", "lsp0_24m", LSP0_TIMER5_CLK,  12, 4, 0, common_even_div_table),
+	DIV_T(0, "ssp0_div", "ssp0_wclk_mux", LSP0_SSP0_CLK, 12, 4, 0, common_even_div_table),
+	DIV_T(0, "ssp1_div", "ssp1_wclk_mux", LSP0_SSP1_CLK, 12, 4, 0, common_even_div_table),
+};
+
+static struct clk_hw_onecell_data lsp0_hw_onecell_data = {
+	.num = LSP0_NR_CLKS,
+	.hws = {
+		[LSP0_NR_CLKS - 1] = NULL,
+	},
+};
+
+static int __init lsp0_clocks_init(struct device_node *np)
+{
+	void __iomem *reg_base;
+	int i, ret;
+
+	reg_base = of_iomap(np, 0);
+	if (!reg_base) {
+		pr_err("%s: Unable to map clk base\n", __func__);
+		return -ENXIO;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(lsp0_mux_clk); i++) {
+		if (lsp0_mux_clk[i].id)
+			lsp0_hw_onecell_data.hws[lsp0_mux_clk[i].id] =
+					&lsp0_mux_clk[i].mux.hw;
+
+		lsp0_mux_clk[i].mux.reg += (uintptr_t)reg_base;
+		ret = clk_hw_register(NULL, &lsp0_mux_clk[i].mux.hw);
+		if (ret) {
+			pr_warn("lsp0 clk %s init error!\n",
+				lsp0_mux_clk[i].mux.hw.init->name);
+		}
+	}
+
+	for (i = 0; i < ARRAY_SIZE(lsp0_gate_clk); i++) {
+		if (lsp0_gate_clk[i].id)
+			lsp0_hw_onecell_data.hws[lsp0_gate_clk[i].id] =
+					&lsp0_gate_clk[i].gate.hw;
+
+		lsp0_gate_clk[i].gate.reg += (uintptr_t)reg_base;
+		ret = clk_hw_register(NULL, &lsp0_gate_clk[i].gate.hw);
+		if (ret) {
+			pr_warn("lsp0 clk %s init error!\n",
+				lsp0_gate_clk[i].gate.hw.init->name);
+		}
+	}
+
+	for (i = 0; i < ARRAY_SIZE(lsp0_div_clk); i++) {
+		if (lsp0_div_clk[i].id)
+			lsp0_hw_onecell_data.hws[lsp0_div_clk[i].id] =
+					&lsp0_div_clk[i].div.hw;
+
+		lsp0_div_clk[i].div.reg += (uintptr_t)reg_base;
+		ret = clk_hw_register(NULL, &lsp0_div_clk[i].div.hw);
+		if (ret) {
+			pr_warn("lsp0 clk %s init error!\n",
+				lsp0_div_clk[i].div.hw.init->name);
+		}
+	}
+
+	if (of_clk_add_hw_provider(np, of_clk_hw_onecell_get, &lsp0_hw_onecell_data))
+		panic("could not register clk provider\n");
+	pr_info("lsp0-clk init over:%d\n", LSP0_NR_CLKS);
+
+	return 0;
+}
+
+PNAME(lsp1_wclk_common_p) = {
+	"lsp1_24m",
+	"lsp1_99m",
+};
+
+PNAME(lsp1_wclk_ssp_p) = {
+	"lsp1_148m5",
+	"lsp1_99m",
+	"lsp1_24m",
+};
+
+static struct zx_clk_mux lsp1_mux_clk[] = {
+	MUX(0, "uart4_wclk_mux", lsp1_wclk_common_p, LSP1_UART4_CLK, 4, 1),
+	MUX(0, "uart5_wclk_mux", lsp1_wclk_common_p, LSP1_UART5_CLK, 4, 1),
+	MUX(0, "pwm_wclk_mux",   lsp1_wclk_common_p, LSP1_PWM_CLK,   4, 1),
+	MUX(0, "i2c2_wclk_mux",  lsp1_wclk_common_p, LSP1_I2C2_CLK,  4, 1),
+	MUX(0, "ssp2_wclk_mux",  lsp1_wclk_ssp_p,    LSP1_SSP2_CLK,  4, 2),
+	MUX(0, "ssp3_wclk_mux",  lsp1_wclk_ssp_p,    LSP1_SSP3_CLK,  4, 2),
+	MUX(0, "ssp4_wclk_mux",  lsp1_wclk_ssp_p,    LSP1_SSP4_CLK,  4, 2),
+	MUX(0, "usim1_wclk_mux", lsp1_wclk_common_p, LSP1_USIM1_CLK, 4, 1),
+};
+
+static struct zx_clk_div lsp1_div_clk[] = {
+	DIV_T(0, "pwm_div",  "pwm_wclk_mux",  LSP1_PWM_CLK,  12, 4, CLK_SET_RATE_PARENT, common_div_table),
+	DIV_T(0, "ssp2_div", "ssp2_wclk_mux", LSP1_SSP2_CLK, 12, 4, CLK_SET_RATE_PARENT, common_even_div_table),
+	DIV_T(0, "ssp3_div", "ssp3_wclk_mux", LSP1_SSP3_CLK, 12, 4, CLK_SET_RATE_PARENT, common_even_div_table),
+	DIV_T(0, "ssp4_div", "ssp4_wclk_mux", LSP1_SSP4_CLK, 12, 4, CLK_SET_RATE_PARENT, common_even_div_table),
+};
+
+static struct zx_clk_gate lsp1_gate_clk[] = {
+	GATE(LSP1_UART4_WCLK, "lsp1_uart4_wclk", "uart4_wclk_mux", LSP1_UART4_CLK, 1, CLK_SET_RATE_PARENT, 0),
+	GATE(LSP1_UART5_WCLK, "lsp1_uart5_wclk", "uart5_wclk_mux", LSP1_UART5_CLK, 1, CLK_SET_RATE_PARENT, 0),
+	GATE(LSP1_PWM_WCLK,   "lsp1_pwm_wclk",   "pwm_div",        LSP1_PWM_CLK,   1, CLK_SET_RATE_PARENT, 0),
+	GATE(LSP1_PWM_PCLK,   "lsp1_pwm_pclk",   "lsp1_pclk",      LSP1_PWM_CLK,   0, 0,		   0),
+	GATE(LSP1_I2C2_WCLK,  "lsp1_i2c2_wclk",  "i2c2_wclk_mux",  LSP1_I2C2_CLK,  1, CLK_SET_RATE_PARENT, 0),
+	GATE(LSP1_SSP2_WCLK,  "lsp1_ssp2_wclk",  "ssp2_div",       LSP1_SSP2_CLK,  1, CLK_SET_RATE_PARENT, 0),
+	GATE(LSP1_SSP3_WCLK,  "lsp1_ssp3_wclk",  "ssp3_div",       LSP1_SSP3_CLK,  1, CLK_SET_RATE_PARENT, 0),
+	GATE(LSP1_SSP4_WCLK,  "lsp1_ssp4_wclk",  "ssp4_div",       LSP1_SSP4_CLK,  1, CLK_SET_RATE_PARENT, 0),
+	GATE(LSP1_USIM1_WCLK, "lsp1_usim1_wclk", "usim1_wclk_mux", LSP1_USIM1_CLK, 1, CLK_SET_RATE_PARENT, 0),
+};
+
+static struct clk_hw_onecell_data lsp1_hw_onecell_data = {
+	.num = LSP1_NR_CLKS,
+	.hws = {
+		[LSP1_NR_CLKS - 1] = NULL,
+	},
+};
+
+static int __init lsp1_clocks_init(struct device_node *np)
+{
+	void __iomem *reg_base;
+	int i, ret;
+
+	reg_base = of_iomap(np, 0);
+	if (!reg_base) {
+		pr_err("%s: Unable to map clk base\n", __func__);
+		return -ENXIO;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(lsp1_mux_clk); i++) {
+		if (lsp1_mux_clk[i].id)
+			lsp1_hw_onecell_data.hws[lsp1_mux_clk[i].id] =
+					&lsp0_mux_clk[i].mux.hw;
+
+		lsp1_mux_clk[i].mux.reg += (uintptr_t)reg_base;
+		ret = clk_hw_register(NULL, &lsp1_mux_clk[i].mux.hw);
+		if (ret) {
+			pr_warn("lsp1 clk %s init error!\n",
+				lsp1_mux_clk[i].mux.hw.init->name);
+		}
+	}
+
+	for (i = 0; i < ARRAY_SIZE(lsp1_gate_clk); i++) {
+		if (lsp1_gate_clk[i].id)
+			lsp1_hw_onecell_data.hws[lsp1_gate_clk[i].id] =
+					&lsp1_gate_clk[i].gate.hw;
+
+		lsp1_gate_clk[i].gate.reg += (uintptr_t)reg_base;
+		ret = clk_hw_register(NULL, &lsp1_gate_clk[i].gate.hw);
+		if (ret) {
+			pr_warn("lsp1 clk %s init error!\n",
+				lsp1_gate_clk[i].gate.hw.init->name);
+		}
+	}
+
+	for (i = 0; i < ARRAY_SIZE(lsp1_div_clk); i++) {
+		if (lsp1_div_clk[i].id)
+			lsp1_hw_onecell_data.hws[lsp1_div_clk[i].id] =
+					&lsp1_div_clk[i].div.hw;
+
+		lsp1_div_clk[i].div.reg += (uintptr_t)reg_base;
+		ret = clk_hw_register(NULL, &lsp1_div_clk[i].div.hw);
+		if (ret) {
+			pr_warn("lsp1 clk %s init error!\n",
+				lsp1_div_clk[i].div.hw.init->name);
+		}
+	}
+
+	if (of_clk_add_hw_provider(np, of_clk_hw_onecell_get, &lsp1_hw_onecell_data))
+		panic("could not register clk provider\n");
+	pr_info("lsp1-clk init over, nr:%d\n", LSP1_NR_CLKS);
+
+	return 0;
+}
+
+static const struct of_device_id zx_clkc_match_table[] = {
+	{ .compatible = "zte,zx296718-topcrm", .data = &top_clocks_init },
+	{ .compatible = "zte,zx296718-lsp0crm", .data = &lsp0_clocks_init },
+	{ .compatible = "zte,zx296718-lsp1crm", .data = &lsp1_clocks_init },
+	{ }
+};
+
+static int zx_clkc_probe(struct platform_device *pdev)
+{
+	int (*init_fn)(struct device_node *np);
+	struct device_node *np = pdev->dev.of_node;
+
+	init_fn = of_device_get_match_data(&pdev->dev);
+	if (!init_fn) {
+		dev_err(&pdev->dev, "Error: No device match found\n");
+		return -ENODEV;
+	}
+
+	return init_fn(np);
+}
+
+static struct platform_driver zx_clk_driver = {
+	.probe		= zx_clkc_probe,
+	.driver		= {
+		.name	= "zx296718-clkc",
+		.of_match_table = zx_clkc_match_table,
+	},
+};
+
+static int __init zx_clk_init(void)
+{
+	return platform_driver_register(&zx_clk_driver);
+}
+core_initcall(zx_clk_init);
diff --git a/drivers/clk/zte/clk.c b/drivers/clk/zte/clk.c
index 7c73c538c43d..c4c1251bc1e7 100644
--- a/drivers/clk/zte/clk.c
+++ b/drivers/clk/zte/clk.c
@@ -21,8 +21,8 @@
 #define to_clk_zx_audio(_hw) container_of(_hw, struct clk_zx_audio, hw)
 
 #define CFG0_CFG1_OFFSET 4
-#define LOCK_FLAG BIT(30)
-#define POWER_DOWN BIT(31)
+#define LOCK_FLAG 30
+#define POWER_DOWN 31
 
 static int rate_to_idx(struct clk_zx_pll *zx_pll, unsigned long rate)
 {
@@ -50,8 +50,8 @@ static int hw_to_idx(struct clk_zx_pll *zx_pll)
 	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;
+	hw_cfg0 &= ~BIT(zx_pll->lock_bit);
+	hw_cfg0 |= BIT(zx_pll->pd_bit);
 
 	for (i = 0; i < zx_pll->count; i++) {
 		if (hw_cfg0 == config[i].cfg0 && hw_cfg1 == config[i].cfg1)
@@ -108,10 +108,10 @@ static int zx_pll_enable(struct clk_hw *hw)
 	u32 reg;
 
 	reg = readl_relaxed(zx_pll->reg_base);
-	writel_relaxed(reg & ~POWER_DOWN, zx_pll->reg_base);
+	writel_relaxed(reg & ~BIT(zx_pll->pd_bit), zx_pll->reg_base);
 
 	return readl_relaxed_poll_timeout(zx_pll->reg_base, reg,
-					  reg & LOCK_FLAG, 0, 100);
+					  reg & BIT(zx_pll->lock_bit), 0, 100);
 }
 
 static void zx_pll_disable(struct clk_hw *hw)
@@ -120,7 +120,7 @@ static void zx_pll_disable(struct clk_hw *hw)
 	u32 reg;
 
 	reg = readl_relaxed(zx_pll->reg_base);
-	writel_relaxed(reg | POWER_DOWN, zx_pll->reg_base);
+	writel_relaxed(reg | BIT(zx_pll->pd_bit), zx_pll->reg_base);
 }
 
 static int zx_pll_is_enabled(struct clk_hw *hw)
@@ -130,10 +130,10 @@ static int zx_pll_is_enabled(struct clk_hw *hw)
 
 	reg = readl_relaxed(zx_pll->reg_base);
 
-	return !(reg & POWER_DOWN);
+	return !(reg & BIT(zx_pll->pd_bit));
 }
 
-static const struct clk_ops zx_pll_ops = {
+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,
@@ -141,6 +141,7 @@ static const struct clk_ops zx_pll_ops = {
 	.disable = zx_pll_disable,
 	.is_enabled = zx_pll_is_enabled,
 };
+EXPORT_SYMBOL(zx_pll_ops);
 
 struct clk *clk_register_zx_pll(const char *name, const char *parent_name,
 				unsigned long flags, void __iomem *reg_base,
@@ -164,6 +165,8 @@ struct clk *clk_register_zx_pll(const char *name, const char *parent_name,
 	zx_pll->reg_base = reg_base;
 	zx_pll->lookup_table = lookup_table;
 	zx_pll->count = count;
+	zx_pll->lock_bit = LOCK_FLAG;
+	zx_pll->pd_bit = POWER_DOWN;
 	zx_pll->lock = lock;
 	zx_pll->hw.init = &init;
 
diff --git a/drivers/clk/zte/clk.h b/drivers/clk/zte/clk.h
index 65ae08b818d3..0df3474b2cf3 100644
--- a/drivers/clk/zte/clk.h
+++ b/drivers/clk/zte/clk.h
@@ -12,6 +12,26 @@
 #include <linux/clk-provider.h>
 #include <linux/spinlock.h>
 
+#define PNAME(x) static const char *x[]
+
+#define CLK_HW_INIT(_name, _parent, _ops, _flags)			\
+	&(struct clk_init_data) {					\
+		.flags		= _flags,				\
+		.name		= _name,				\
+		.parent_names	= (const char *[]) { _parent },		\
+		.num_parents	= 1,					\
+		.ops		= _ops,					\
+	}
+
+#define CLK_HW_INIT_PARENTS(_name, _parents, _ops, _flags)		\
+	&(struct clk_init_data) {					\
+		.flags		= _flags,				\
+		.name		= _name,				\
+		.parent_names	= _parents,				\
+		.num_parents	= ARRAY_SIZE(_parents),			\
+		.ops		= _ops,					\
+	}
+
 struct zx_pll_config {
 	unsigned long rate;
 	u32 cfg0;
@@ -24,8 +44,115 @@ struct clk_zx_pll {
 	const struct zx_pll_config *lookup_table; /* order by rate asc */
 	int count;
 	spinlock_t *lock;
+	u8 pd_bit;		/* power down bit */
+	u8 lock_bit;		/* pll lock flag bit */
+};
+
+#define PLL_RATE(_rate, _cfg0, _cfg1)	\
+{					\
+	.rate = _rate,			\
+	.cfg0 = _cfg0,			\
+	.cfg1 = _cfg1,			\
+}
+
+#define ZX_PLL(_name, _parent, _reg, _table, _pd, _lock)		\
+{									\
+	.reg_base	= (void __iomem *) _reg,			\
+	.lookup_table	= _table,					\
+	.count		= ARRAY_SIZE(_table),				\
+	.pd_bit		= _pd,						\
+	.lock_bit	= _lock,					\
+	.hw.init	 = CLK_HW_INIT(_name, _parent, &zx_pll_ops,	\
+				CLK_GET_RATE_NOCACHE),			\
+}
+
+#define ZX296718_PLL(_name, _parent, _reg, _table)			\
+ZX_PLL(_name, _parent, _reg, _table, 0, 30)
+
+struct zx_clk_gate {
+	struct clk_gate gate;
+	u16		id;
+};
+
+#define GATE(_id, _name, _parent, _reg, _bit, _flag, _gflags)		\
+{									\
+	.gate = {							\
+		.reg = (void __iomem *) _reg,				\
+		.bit_idx = (_bit),					\
+		.flags = _gflags,					\
+		.lock = &clk_lock,					\
+		.hw.init = CLK_HW_INIT(_name,				\
+					_parent,			\
+					&clk_gate_ops,			\
+					_flag | CLK_IGNORE_UNUSED),	\
+	},								\
+	.id	= _id,							\
+}
+
+struct zx_clk_fixed_factor {
+	struct clk_fixed_factor factor;
+	u16	id;
+};
+
+#define FFACTOR(_id, _name, _parent, _mult, _div, _flag)		\
+{									\
+	.factor = {							\
+		.div		= _div,					\
+		.mult		= _mult,				\
+		.hw.init	= CLK_HW_INIT(_name,			\
+					      _parent,			\
+					      &clk_fixed_factor_ops,	\
+					      _flag),			\
+	},								\
+	.id = _id,							\
+}
+
+struct zx_clk_mux {
+	struct clk_mux mux;
+	u16	id;
+};
+
+#define MUX_F(_id, _name, _parent, _reg, _shift, _width, _flag, _mflag)	\
+{									\
+	.mux = {							\
+		.reg		= (void __iomem *) _reg,		\
+		.mask		= BIT(_width) - 1,			\
+		.shift		= _shift,				\
+		.flags		= _mflag,				\
+		.lock		= &clk_lock,				\
+		.hw.init	= CLK_HW_INIT_PARENTS(_name,		\
+						      _parent,		\
+						      &clk_mux_ops,	\
+						      _flag),		\
+	},								\
+	.id = _id,							\
+}
+
+#define MUX(_id, _name, _parent, _reg, _shift, _width)			\
+MUX_F(_id, _name, _parent, _reg, _shift, _width, 0, 0)
+
+struct zx_clk_div {
+	struct clk_divider div;
+	u16	id;
 };
 
+#define DIV_T(_id, _name, _parent, _reg, _shift, _width, _flag, _table)	\
+{									\
+	.div = {							\
+		.reg		= (void __iomem *) _reg,		\
+		.shift		= _shift,				\
+		.width		= _width,				\
+		.flags		= 0,					\
+		.table		= _table,				\
+		.lock		= &clk_lock,				\
+		.hw.init	= CLK_HW_INIT(_name,			\
+					      _parent,			\
+					      &clk_divider_ops,		\
+					      _flag),			\
+	},								\
+	.id = _id,							\
+}
+
 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);
@@ -38,4 +165,6 @@ struct clk_zx_audio {
 struct clk *clk_register_zx_audio(const char *name,
 				  const char * const parent_name,
 				  unsigned long flags, void __iomem *reg_base);
+
+extern const struct clk_ops zx_pll_ops;
 #endif
diff --git a/drivers/clocksource/Kconfig b/drivers/clocksource/Kconfig
index 567788664723..e2c6e43cf8ca 100644
--- a/drivers/clocksource/Kconfig
+++ b/drivers/clocksource/Kconfig
@@ -305,6 +305,16 @@ config ARM_ARCH_TIMER_EVTSTREAM
 	  This must be disabled for hardware validation purposes to detect any
 	  hardware anomalies of missing events.
 
+config FSL_ERRATUM_A008585
+	bool "Workaround for Freescale/NXP Erratum A-008585"
+	default y
+	depends on ARM_ARCH_TIMER && ARM64
+	help
+	  This option enables a workaround for Freescale/NXP Erratum
+	  A-008585 ("ARM generic timer may contain an erroneous
+	  value").  The workaround will only be active if the
+	  fsl,erratum-a008585 property is found in the timer node.
+
 config ARM_GLOBAL_TIMER
 	bool "Support for the ARM global timer" if COMPILE_TEST
 	select CLKSRC_OF if OF
@@ -351,7 +361,7 @@ config CLKSRC_METAG_GENERIC
 
 config CLKSRC_EXYNOS_MCT
 	bool "Exynos multi core timer driver" if COMPILE_TEST
-	depends on ARM
+	depends on ARM || ARM64
 	help
 	  Support for Multi Core Timer controller on Exynos SoCs.
 
@@ -407,6 +417,16 @@ config SYS_SUPPORTS_SH_TMU
 config SYS_SUPPORTS_EM_STI
         bool
 
+config CLKSRC_JCORE_PIT
+	bool "J-Core PIT timer driver" if COMPILE_TEST
+	depends on OF
+	depends on GENERIC_CLOCKEVENTS
+	depends on HAS_IOMEM
+	select CLKSRC_MMIO
+	help
+	  This enables build of clocksource and clockevent driver for
+	  the integrated PIT in the J-Core synthesizable, open source SoC.
+
 config SH_TIMER_CMT
 	bool "Renesas CMT timer driver" if COMPILE_TEST
 	depends on GENERIC_CLOCKEVENTS
diff --git a/drivers/clocksource/Makefile b/drivers/clocksource/Makefile
index fd9d6df0bbc0..cf87f407f1ad 100644
--- a/drivers/clocksource/Makefile
+++ b/drivers/clocksource/Makefile
@@ -5,6 +5,7 @@ obj-$(CONFIG_ATMEL_TCB_CLKSRC)	+= tcb_clksrc.o
 obj-$(CONFIG_X86_PM_TIMER)	+= acpi_pm.o
 obj-$(CONFIG_SCx200HR_TIMER)	+= scx200_hrt.o
 obj-$(CONFIG_CS5535_CLOCK_EVENT_SRC)	+= cs5535-clockevt.o
+obj-$(CONFIG_CLKSRC_JCORE_PIT)		+= jcore-pit.o
 obj-$(CONFIG_SH_TIMER_CMT)	+= sh_cmt.o
 obj-$(CONFIG_SH_TIMER_MTU2)	+= sh_mtu2.o
 obj-$(CONFIG_SH_TIMER_TMU)	+= sh_tmu.o
diff --git a/drivers/clocksource/arm_arch_timer.c b/drivers/clocksource/arm_arch_timer.c
index 57700541f951..73c487da6d2a 100644
--- a/drivers/clocksource/arm_arch_timer.c
+++ b/drivers/clocksource/arm_arch_timer.c
@@ -94,6 +94,43 @@ early_param("clocksource.arm_arch_timer.evtstrm", early_evtstrm_cfg);
  * Architected system timer support.
  */
 
+#ifdef CONFIG_FSL_ERRATUM_A008585
+DEFINE_STATIC_KEY_FALSE(arch_timer_read_ool_enabled);
+EXPORT_SYMBOL_GPL(arch_timer_read_ool_enabled);
+
+static int fsl_a008585_enable = -1;
+
+static int __init early_fsl_a008585_cfg(char *buf)
+{
+	int ret;
+	bool val;
+
+	ret = strtobool(buf, &val);
+	if (ret)
+		return ret;
+
+	fsl_a008585_enable = val;
+	return 0;
+}
+early_param("clocksource.arm_arch_timer.fsl-a008585", early_fsl_a008585_cfg);
+
+u32 __fsl_a008585_read_cntp_tval_el0(void)
+{
+	return __fsl_a008585_read_reg(cntp_tval_el0);
+}
+
+u32 __fsl_a008585_read_cntv_tval_el0(void)
+{
+	return __fsl_a008585_read_reg(cntv_tval_el0);
+}
+
+u64 __fsl_a008585_read_cntvct_el0(void)
+{
+	return __fsl_a008585_read_reg(cntvct_el0);
+}
+EXPORT_SYMBOL(__fsl_a008585_read_cntvct_el0);
+#endif /* CONFIG_FSL_ERRATUM_A008585 */
+
 static __always_inline
 void arch_timer_reg_write(int access, enum arch_timer_reg reg, u32 val,
 			  struct clock_event_device *clk)
@@ -243,6 +280,40 @@ static __always_inline void set_next_event(const int access, unsigned long evt,
 	arch_timer_reg_write(access, ARCH_TIMER_REG_CTRL, ctrl, clk);
 }
 
+#ifdef CONFIG_FSL_ERRATUM_A008585
+static __always_inline void fsl_a008585_set_next_event(const int access,
+		unsigned long evt, struct clock_event_device *clk)
+{
+	unsigned long ctrl;
+	u64 cval = evt + arch_counter_get_cntvct();
+
+	ctrl = arch_timer_reg_read(access, ARCH_TIMER_REG_CTRL, clk);
+	ctrl |= ARCH_TIMER_CTRL_ENABLE;
+	ctrl &= ~ARCH_TIMER_CTRL_IT_MASK;
+
+	if (access == ARCH_TIMER_PHYS_ACCESS)
+		write_sysreg(cval, cntp_cval_el0);
+	else if (access == ARCH_TIMER_VIRT_ACCESS)
+		write_sysreg(cval, cntv_cval_el0);
+
+	arch_timer_reg_write(access, ARCH_TIMER_REG_CTRL, ctrl, clk);
+}
+
+static int fsl_a008585_set_next_event_virt(unsigned long evt,
+					   struct clock_event_device *clk)
+{
+	fsl_a008585_set_next_event(ARCH_TIMER_VIRT_ACCESS, evt, clk);
+	return 0;
+}
+
+static int fsl_a008585_set_next_event_phys(unsigned long evt,
+					   struct clock_event_device *clk)
+{
+	fsl_a008585_set_next_event(ARCH_TIMER_PHYS_ACCESS, evt, clk);
+	return 0;
+}
+#endif /* CONFIG_FSL_ERRATUM_A008585 */
+
 static int arch_timer_set_next_event_virt(unsigned long evt,
 					  struct clock_event_device *clk)
 {
@@ -271,6 +342,19 @@ static int arch_timer_set_next_event_phys_mem(unsigned long evt,
 	return 0;
 }
 
+static void fsl_a008585_set_sne(struct clock_event_device *clk)
+{
+#ifdef CONFIG_FSL_ERRATUM_A008585
+	if (!static_branch_unlikely(&arch_timer_read_ool_enabled))
+		return;
+
+	if (arch_timer_uses_ppi == VIRT_PPI)
+		clk->set_next_event = fsl_a008585_set_next_event_virt;
+	else
+		clk->set_next_event = fsl_a008585_set_next_event_phys;
+#endif
+}
+
 static void __arch_timer_setup(unsigned type,
 			       struct clock_event_device *clk)
 {
@@ -299,6 +383,8 @@ static void __arch_timer_setup(unsigned type,
 		default:
 			BUG();
 		}
+
+		fsl_a008585_set_sne(clk);
 	} else {
 		clk->features |= CLOCK_EVT_FEAT_DYNIRQ;
 		clk->name = "arch_mem_timer";
@@ -515,15 +601,19 @@ static void __init arch_counter_register(unsigned type)
 			arch_timer_read_counter = arch_counter_get_cntvct;
 		else
 			arch_timer_read_counter = arch_counter_get_cntpct;
-	} else {
-		arch_timer_read_counter = arch_counter_get_cntvct_mem;
 
-		/* If the clocksource name is "arch_sys_counter" the
-		 * VDSO will attempt to read the CP15-based counter.
-		 * Ensure this does not happen when CP15-based
-		 * counter is not available.
+		clocksource_counter.archdata.vdso_direct = true;
+
+#ifdef CONFIG_FSL_ERRATUM_A008585
+		/*
+		 * Don't use the vdso fastpath if errata require using
+		 * the out-of-line counter accessor.
 		 */
-		clocksource_counter.name = "arch_mem_counter";
+		if (static_branch_unlikely(&arch_timer_read_ool_enabled))
+			clocksource_counter.archdata.vdso_direct = false;
+#endif
+	} else {
+		arch_timer_read_counter = arch_counter_get_cntvct_mem;
 	}
 
 	start_count = arch_timer_read_counter();
@@ -800,6 +890,15 @@ static int __init arch_timer_of_init(struct device_node *np)
 
 	arch_timer_c3stop = !of_property_read_bool(np, "always-on");
 
+#ifdef CONFIG_FSL_ERRATUM_A008585
+	if (fsl_a008585_enable < 0)
+		fsl_a008585_enable = of_property_read_bool(np, "fsl,erratum-a008585");
+	if (fsl_a008585_enable) {
+		static_branch_enable(&arch_timer_read_ool_enabled);
+		pr_info("Enabling workaround for FSL erratum A-008585\n");
+	}
+#endif
+
 	/*
 	 * If we cannot rely on firmware initializing the timer registers then
 	 * we should use the physical timers instead.
diff --git a/drivers/clocksource/exynos_mct.c b/drivers/clocksource/exynos_mct.c
index 41840d02c331..8f3488b80896 100644
--- a/drivers/clocksource/exynos_mct.c
+++ b/drivers/clocksource/exynos_mct.c
@@ -223,6 +223,7 @@ static u64 notrace exynos4_read_sched_clock(void)
 	return exynos4_read_count_32();
 }
 
+#if defined(CONFIG_ARM)
 static struct delay_timer exynos4_delay_timer;
 
 static cycles_t exynos4_read_current_timer(void)
@@ -231,14 +232,17 @@ static cycles_t exynos4_read_current_timer(void)
 			 "cycles_t needs to move to 32-bit for ARM64 usage");
 	return exynos4_read_count_32();
 }
+#endif
 
 static int __init exynos4_clocksource_init(void)
 {
 	exynos4_mct_frc_start();
 
+#if defined(CONFIG_ARM)
 	exynos4_delay_timer.read_current_timer = &exynos4_read_current_timer;
 	exynos4_delay_timer.freq = clk_rate;
 	register_current_timer_delay(&exynos4_delay_timer);
+#endif
 
 	if (clocksource_register_hz(&mct_frc, clk_rate))
 		panic("%s: can't register clocksource\n", mct_frc.name);
diff --git a/drivers/clocksource/jcore-pit.c b/drivers/clocksource/jcore-pit.c
new file mode 100644
index 000000000000..54e1665aa03c
--- /dev/null
+++ b/drivers/clocksource/jcore-pit.c
@@ -0,0 +1,249 @@
+/*
+ * J-Core SoC PIT/clocksource driver
+ *
+ * Copyright (C) 2015-2016 Smart Energy Instruments, Inc.
+ *
+ * This file is subject to the terms and conditions of the GNU General Public
+ * License.  See the file "COPYING" in the main directory of this archive
+ * for more details.
+ */
+
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/interrupt.h>
+#include <linux/clockchips.h>
+#include <linux/clocksource.h>
+#include <linux/sched_clock.h>
+#include <linux/cpu.h>
+#include <linux/cpuhotplug.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+
+#define PIT_IRQ_SHIFT		12
+#define PIT_PRIO_SHIFT		20
+#define PIT_ENABLE_SHIFT	26
+#define PIT_PRIO_MASK		0xf
+
+#define REG_PITEN		0x00
+#define REG_THROT		0x10
+#define REG_COUNT		0x14
+#define REG_BUSPD		0x18
+#define REG_SECHI		0x20
+#define REG_SECLO		0x24
+#define REG_NSEC		0x28
+
+struct jcore_pit {
+	struct clock_event_device	ced;
+	void __iomem			*base;
+	unsigned long			periodic_delta;
+	u32				enable_val;
+};
+
+static void __iomem *jcore_pit_base;
+static struct jcore_pit __percpu *jcore_pit_percpu;
+
+static notrace u64 jcore_sched_clock_read(void)
+{
+	u32 seclo, nsec, seclo0;
+	__iomem void *base = jcore_pit_base;
+
+	seclo = readl(base + REG_SECLO);
+	do {
+		seclo0 = seclo;
+		nsec  = readl(base + REG_NSEC);
+		seclo = readl(base + REG_SECLO);
+	} while (seclo0 != seclo);
+
+	return seclo * NSEC_PER_SEC + nsec;
+}
+
+static cycle_t jcore_clocksource_read(struct clocksource *cs)
+{
+	return jcore_sched_clock_read();
+}
+
+static int jcore_pit_disable(struct jcore_pit *pit)
+{
+	writel(0, pit->base + REG_PITEN);
+	return 0;
+}
+
+static int jcore_pit_set(unsigned long delta, struct jcore_pit *pit)
+{
+	jcore_pit_disable(pit);
+	writel(delta, pit->base + REG_THROT);
+	writel(pit->enable_val, pit->base + REG_PITEN);
+	return 0;
+}
+
+static int jcore_pit_set_state_shutdown(struct clock_event_device *ced)
+{
+	struct jcore_pit *pit = container_of(ced, struct jcore_pit, ced);
+
+	return jcore_pit_disable(pit);
+}
+
+static int jcore_pit_set_state_oneshot(struct clock_event_device *ced)
+{
+	struct jcore_pit *pit = container_of(ced, struct jcore_pit, ced);
+
+	return jcore_pit_disable(pit);
+}
+
+static int jcore_pit_set_state_periodic(struct clock_event_device *ced)
+{
+	struct jcore_pit *pit = container_of(ced, struct jcore_pit, ced);
+
+	return jcore_pit_set(pit->periodic_delta, pit);
+}
+
+static int jcore_pit_set_next_event(unsigned long delta,
+				    struct clock_event_device *ced)
+{
+	struct jcore_pit *pit = container_of(ced, struct jcore_pit, ced);
+
+	return jcore_pit_set(delta, pit);
+}
+
+static int jcore_pit_local_init(unsigned cpu)
+{
+	struct jcore_pit *pit = this_cpu_ptr(jcore_pit_percpu);
+	unsigned buspd, freq;
+
+	pr_info("Local J-Core PIT init on cpu %u\n", cpu);
+
+	buspd = readl(pit->base + REG_BUSPD);
+	freq = DIV_ROUND_CLOSEST(NSEC_PER_SEC, buspd);
+	pit->periodic_delta = DIV_ROUND_CLOSEST(NSEC_PER_SEC, HZ * buspd);
+
+	clockevents_config_and_register(&pit->ced, freq, 1, ULONG_MAX);
+
+	return 0;
+}
+
+static irqreturn_t jcore_timer_interrupt(int irq, void *dev_id)
+{
+	struct jcore_pit *pit = this_cpu_ptr(dev_id);
+
+	if (clockevent_state_oneshot(&pit->ced))
+		jcore_pit_disable(pit);
+
+	pit->ced.event_handler(&pit->ced);
+
+	return IRQ_HANDLED;
+}
+
+static int __init jcore_pit_init(struct device_node *node)
+{
+	int err;
+	unsigned pit_irq, cpu;
+	unsigned long hwirq;
+	u32 irqprio, enable_val;
+
+	jcore_pit_base = of_iomap(node, 0);
+	if (!jcore_pit_base) {
+		pr_err("Error: Cannot map base address for J-Core PIT\n");
+		return -ENXIO;
+	}
+
+	pit_irq = irq_of_parse_and_map(node, 0);
+	if (!pit_irq) {
+		pr_err("Error: J-Core PIT has no IRQ\n");
+		return -ENXIO;
+	}
+
+	pr_info("Initializing J-Core PIT at %p IRQ %d\n",
+		jcore_pit_base, pit_irq);
+
+	err = clocksource_mmio_init(jcore_pit_base, "jcore_pit_cs",
+				    NSEC_PER_SEC, 400, 32,
+				    jcore_clocksource_read);
+	if (err) {
+		pr_err("Error registering clocksource device: %d\n", err);
+		return err;
+	}
+
+	sched_clock_register(jcore_sched_clock_read, 32, NSEC_PER_SEC);
+
+	jcore_pit_percpu = alloc_percpu(struct jcore_pit);
+	if (!jcore_pit_percpu) {
+		pr_err("Failed to allocate memory for clock event device\n");
+		return -ENOMEM;
+	}
+
+	err = request_irq(pit_irq, jcore_timer_interrupt,
+			  IRQF_TIMER | IRQF_PERCPU,
+			  "jcore_pit", jcore_pit_percpu);
+	if (err) {
+		pr_err("pit irq request failed: %d\n", err);
+		free_percpu(jcore_pit_percpu);
+		return err;
+	}
+
+	/*
+	 * The J-Core PIT is not hard-wired to a particular IRQ, but
+	 * integrated with the interrupt controller such that the IRQ it
+	 * generates is programmable, as follows:
+	 *
+	 * The bit layout of the PIT enable register is:
+	 *
+	 *	.....e..ppppiiiiiiii............
+	 *
+	 * where the .'s indicate unrelated/unused bits, e is enable,
+	 * p is priority, and i is hard irq number.
+	 *
+	 * For the PIT included in AIC1 (obsolete but still in use),
+	 * any hard irq (trap number) can be programmed via the 8
+	 * iiiiiiii bits, and a priority (0-15) is programmable
+	 * separately in the pppp bits.
+	 *
+	 * For the PIT included in AIC2 (current), the programming
+	 * interface is equivalent modulo interrupt mapping. This is
+	 * why a different compatible tag was not used. However only
+	 * traps 64-127 (the ones actually intended to be used for
+	 * interrupts, rather than syscalls/exceptions/etc.) can be
+	 * programmed (the high 2 bits of i are ignored) and the
+	 * priority pppp is <<2'd and or'd onto the irq number. This
+	 * choice seems to have been made on the hardware engineering
+	 * side under an assumption that preserving old AIC1 priority
+	 * mappings was important. Future models will likely ignore
+	 * the pppp field.
+	 */
+	hwirq = irq_get_irq_data(pit_irq)->hwirq;
+	irqprio = (hwirq >> 2) & PIT_PRIO_MASK;
+	enable_val = (1U << PIT_ENABLE_SHIFT)
+		   | (hwirq << PIT_IRQ_SHIFT)
+		   | (irqprio << PIT_PRIO_SHIFT);
+
+	for_each_present_cpu(cpu) {
+		struct jcore_pit *pit = per_cpu_ptr(jcore_pit_percpu, cpu);
+
+		pit->base = of_iomap(node, cpu);
+		if (!pit->base) {
+			pr_err("Unable to map PIT for cpu %u\n", cpu);
+			continue;
+		}
+
+		pit->ced.name = "jcore_pit";
+		pit->ced.features = CLOCK_EVT_FEAT_PERIODIC
+				  | CLOCK_EVT_FEAT_ONESHOT
+				  | CLOCK_EVT_FEAT_PERCPU;
+		pit->ced.cpumask = cpumask_of(cpu);
+		pit->ced.rating = 400;
+		pit->ced.irq = pit_irq;
+		pit->ced.set_state_shutdown = jcore_pit_set_state_shutdown;
+		pit->ced.set_state_periodic = jcore_pit_set_state_periodic;
+		pit->ced.set_state_oneshot = jcore_pit_set_state_oneshot;
+		pit->ced.set_next_event = jcore_pit_set_next_event;
+
+		pit->enable_val = enable_val;
+	}
+
+	cpuhp_setup_state(CPUHP_AP_JCORE_TIMER_STARTING,
+			  "AP_JCORE_TIMER_STARTING",
+			  jcore_pit_local_init, NULL);
+
+	return 0;
+}
+
+CLOCKSOURCE_OF_DECLARE(jcore_pit, "jcore,pit", jcore_pit_init);
diff --git a/drivers/clocksource/mips-gic-timer.c b/drivers/clocksource/mips-gic-timer.c
index b4b3ab5a11ad..7a960cd01104 100644
--- a/drivers/clocksource/mips-gic-timer.c
+++ b/drivers/clocksource/mips-gic-timer.c
@@ -109,12 +109,15 @@ static int gic_clockevent_init(void)
 {
 	int ret;
 
-	if (!cpu_has_counter || !gic_frequency)
+	if (!gic_frequency)
 		return -ENXIO;
 
 	ret = setup_percpu_irq(gic_timer_irq, &gic_compare_irqaction);
-	if (ret < 0)
+	if (ret < 0) {
+		pr_err("GIC timer IRQ %d setup failed: %d\n",
+		       gic_timer_irq, ret);
 		return ret;
+	}
 
 	cpuhp_setup_state(CPUHP_AP_MIPS_GIC_TIMER_STARTING,
 			  "AP_MIPS_GIC_TIMER_STARTING", gic_starting_cpu,
diff --git a/drivers/clocksource/moxart_timer.c b/drivers/clocksource/moxart_timer.c
index 841454417acd..2a8f4705c734 100644
--- a/drivers/clocksource/moxart_timer.c
+++ b/drivers/clocksource/moxart_timer.c
@@ -21,6 +21,7 @@
 #include <linux/io.h>
 #include <linux/clocksource.h>
 #include <linux/bitops.h>
+#include <linux/slab.h>
 
 #define TIMER1_BASE		0x00
 #define TIMER2_BASE		0x10
@@ -36,75 +37,109 @@
 #define TIMER_INTR_MASK		0x38
 
 /*
- * TIMER_CR flags:
+ * Moxart TIMER_CR flags:
  *
- * TIMEREG_CR_*_CLOCK	0: PCLK, 1: EXT1CLK
- * TIMEREG_CR_*_INT	overflow interrupt enable bit
+ * MOXART_CR_*_CLOCK	0: PCLK, 1: EXT1CLK
+ * MOXART_CR_*_INT	overflow interrupt enable bit
  */
-#define TIMEREG_CR_1_ENABLE	BIT(0)
-#define TIMEREG_CR_1_CLOCK	BIT(1)
-#define TIMEREG_CR_1_INT	BIT(2)
-#define TIMEREG_CR_2_ENABLE	BIT(3)
-#define TIMEREG_CR_2_CLOCK	BIT(4)
-#define TIMEREG_CR_2_INT	BIT(5)
-#define TIMEREG_CR_3_ENABLE	BIT(6)
-#define TIMEREG_CR_3_CLOCK	BIT(7)
-#define TIMEREG_CR_3_INT	BIT(8)
-#define TIMEREG_CR_COUNT_UP	BIT(9)
-
-#define TIMER1_ENABLE		(TIMEREG_CR_2_ENABLE | TIMEREG_CR_1_ENABLE)
-#define TIMER1_DISABLE		(TIMEREG_CR_2_ENABLE)
-
-static void __iomem *base;
-static unsigned int clock_count_per_tick;
+#define MOXART_CR_1_ENABLE	BIT(0)
+#define MOXART_CR_1_CLOCK	BIT(1)
+#define MOXART_CR_1_INT	BIT(2)
+#define MOXART_CR_2_ENABLE	BIT(3)
+#define MOXART_CR_2_CLOCK	BIT(4)
+#define MOXART_CR_2_INT	BIT(5)
+#define MOXART_CR_3_ENABLE	BIT(6)
+#define MOXART_CR_3_CLOCK	BIT(7)
+#define MOXART_CR_3_INT	BIT(8)
+#define MOXART_CR_COUNT_UP	BIT(9)
+
+#define MOXART_TIMER1_ENABLE	(MOXART_CR_2_ENABLE | MOXART_CR_1_ENABLE)
+#define MOXART_TIMER1_DISABLE	(MOXART_CR_2_ENABLE)
+
+/*
+ * The ASpeed variant of the IP block has a different layout
+ * for the control register
+ */
+#define ASPEED_CR_1_ENABLE	BIT(0)
+#define ASPEED_CR_1_CLOCK	BIT(1)
+#define ASPEED_CR_1_INT		BIT(2)
+#define ASPEED_CR_2_ENABLE	BIT(4)
+#define ASPEED_CR_2_CLOCK	BIT(5)
+#define ASPEED_CR_2_INT		BIT(6)
+#define ASPEED_CR_3_ENABLE	BIT(8)
+#define ASPEED_CR_3_CLOCK	BIT(9)
+#define ASPEED_CR_3_INT		BIT(10)
+
+#define ASPEED_TIMER1_ENABLE   (ASPEED_CR_2_ENABLE | ASPEED_CR_1_ENABLE)
+#define ASPEED_TIMER1_DISABLE  (ASPEED_CR_2_ENABLE)
+
+struct moxart_timer {
+	void __iomem *base;
+	unsigned int t1_disable_val;
+	unsigned int t1_enable_val;
+	unsigned int count_per_tick;
+	struct clock_event_device clkevt;
+};
+
+static inline struct moxart_timer *to_moxart(struct clock_event_device *evt)
+{
+	return container_of(evt, struct moxart_timer, clkevt);
+}
+
+static inline void moxart_disable(struct clock_event_device *evt)
+{
+	struct moxart_timer *timer = to_moxart(evt);
+
+	writel(timer->t1_disable_val, timer->base + TIMER_CR);
+}
+
+static inline void moxart_enable(struct clock_event_device *evt)
+{
+	struct moxart_timer *timer = to_moxart(evt);
+
+	writel(timer->t1_enable_val, timer->base + TIMER_CR);
+}
 
 static int moxart_shutdown(struct clock_event_device *evt)
 {
-	writel(TIMER1_DISABLE, base + TIMER_CR);
+	moxart_disable(evt);
 	return 0;
 }
 
 static int moxart_set_oneshot(struct clock_event_device *evt)
 {
-	writel(TIMER1_DISABLE, base + TIMER_CR);
-	writel(~0, base + TIMER1_BASE + REG_LOAD);
+	moxart_disable(evt);
+	writel(~0, to_moxart(evt)->base + TIMER1_BASE + REG_LOAD);
 	return 0;
 }
 
 static int moxart_set_periodic(struct clock_event_device *evt)
 {
-	writel(clock_count_per_tick, base + TIMER1_BASE + REG_LOAD);
-	writel(TIMER1_ENABLE, base + TIMER_CR);
+	struct moxart_timer *timer = to_moxart(evt);
+
+	moxart_disable(evt);
+	writel(timer->count_per_tick, timer->base + TIMER1_BASE + REG_LOAD);
+	writel(0, timer->base + TIMER1_BASE + REG_MATCH1);
+	moxart_enable(evt);
 	return 0;
 }
 
 static int moxart_clkevt_next_event(unsigned long cycles,
-				    struct clock_event_device *unused)
+				    struct clock_event_device *evt)
 {
+	struct moxart_timer *timer = to_moxart(evt);
 	u32 u;
 
-	writel(TIMER1_DISABLE, base + TIMER_CR);
+	moxart_disable(evt);
 
-	u = readl(base + TIMER1_BASE + REG_COUNT) - cycles;
-	writel(u, base + TIMER1_BASE + REG_MATCH1);
+	u = readl(timer->base + TIMER1_BASE + REG_COUNT) - cycles;
+	writel(u, timer->base + TIMER1_BASE + REG_MATCH1);
 
-	writel(TIMER1_ENABLE, base + TIMER_CR);
+	moxart_enable(evt);
 
 	return 0;
 }
 
-static struct clock_event_device moxart_clockevent = {
-	.name			= "moxart_timer",
-	.rating			= 200,
-	.features		= CLOCK_EVT_FEAT_PERIODIC |
-				  CLOCK_EVT_FEAT_ONESHOT,
-	.set_state_shutdown	= moxart_shutdown,
-	.set_state_periodic	= moxart_set_periodic,
-	.set_state_oneshot	= moxart_set_oneshot,
-	.tick_resume		= moxart_set_oneshot,
-	.set_next_event		= moxart_clkevt_next_event,
-};
-
 static irqreturn_t moxart_timer_interrupt(int irq, void *dev_id)
 {
 	struct clock_event_device *evt = dev_id;
@@ -112,21 +147,19 @@ static irqreturn_t moxart_timer_interrupt(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
-static struct irqaction moxart_timer_irq = {
-	.name		= "moxart-timer",
-	.flags		= IRQF_TIMER,
-	.handler	= moxart_timer_interrupt,
-	.dev_id		= &moxart_clockevent,
-};
-
 static int __init moxart_timer_init(struct device_node *node)
 {
 	int ret, irq;
 	unsigned long pclk;
 	struct clk *clk;
+	struct moxart_timer *timer;
+
+	timer = kzalloc(sizeof(*timer), GFP_KERNEL);
+	if (!timer)
+		return -ENOMEM;
 
-	base = of_iomap(node, 0);
-	if (!base) {
+	timer->base = of_iomap(node, 0);
+	if (!timer->base) {
 		pr_err("%s: of_iomap failed\n", node->full_name);
 		return -ENXIO;
 	}
@@ -137,12 +170,6 @@ static int __init moxart_timer_init(struct device_node *node)
 		return -EINVAL;
 	}
 
-	ret = setup_irq(irq, &moxart_timer_irq);
-	if (ret) {
-		pr_err("%s: setup_irq failed\n", node->full_name);
-		return ret;
-	}
-
 	clk = of_clk_get(node, 0);
 	if (IS_ERR(clk))  {
 		pr_err("%s: of_clk_get failed\n", node->full_name);
@@ -151,7 +178,32 @@ static int __init moxart_timer_init(struct device_node *node)
 
 	pclk = clk_get_rate(clk);
 
-	ret = clocksource_mmio_init(base + TIMER2_BASE + REG_COUNT,
+	if (of_device_is_compatible(node, "moxa,moxart-timer")) {
+		timer->t1_enable_val = MOXART_TIMER1_ENABLE;
+		timer->t1_disable_val = MOXART_TIMER1_DISABLE;
+	} else if (of_device_is_compatible(node, "aspeed,ast2400-timer")) {
+		timer->t1_enable_val = ASPEED_TIMER1_ENABLE;
+		timer->t1_disable_val = ASPEED_TIMER1_DISABLE;
+	} else {
+		pr_err("%s: unknown platform\n", node->full_name);
+		return -EINVAL;
+	}
+
+	timer->count_per_tick = DIV_ROUND_CLOSEST(pclk, HZ);
+
+	timer->clkevt.name = node->name;
+	timer->clkevt.rating = 200;
+	timer->clkevt.features = CLOCK_EVT_FEAT_PERIODIC |
+					CLOCK_EVT_FEAT_ONESHOT;
+	timer->clkevt.set_state_shutdown = moxart_shutdown;
+	timer->clkevt.set_state_periodic = moxart_set_periodic;
+	timer->clkevt.set_state_oneshot = moxart_set_oneshot;
+	timer->clkevt.tick_resume = moxart_set_oneshot;
+	timer->clkevt.set_next_event = moxart_clkevt_next_event;
+	timer->clkevt.cpumask = cpumask_of(0);
+	timer->clkevt.irq = irq;
+
+	ret = clocksource_mmio_init(timer->base + TIMER2_BASE + REG_COUNT,
 				    "moxart_timer", pclk, 200, 32,
 				    clocksource_mmio_readl_down);
 	if (ret) {
@@ -159,13 +211,26 @@ static int __init moxart_timer_init(struct device_node *node)
 		return ret;
 	}
 
-	clock_count_per_tick = DIV_ROUND_CLOSEST(pclk, HZ);
+	ret = request_irq(irq, moxart_timer_interrupt, IRQF_TIMER,
+			  node->name, &timer->clkevt);
+	if (ret) {
+		pr_err("%s: setup_irq failed\n", node->full_name);
+		return ret;
+	}
 
-	writel(~0, base + TIMER2_BASE + REG_LOAD);
-	writel(TIMEREG_CR_2_ENABLE, base + TIMER_CR);
+	/* Clear match registers */
+	writel(0, timer->base + TIMER1_BASE + REG_MATCH1);
+	writel(0, timer->base + TIMER1_BASE + REG_MATCH2);
+	writel(0, timer->base + TIMER2_BASE + REG_MATCH1);
+	writel(0, timer->base + TIMER2_BASE + REG_MATCH2);
 
-	moxart_clockevent.cpumask = cpumask_of(0);
-	moxart_clockevent.irq = irq;
+	/*
+	 * Start timer 2 rolling as our main wall clock source, keep timer 1
+	 * disabled
+	 */
+	writel(0, timer->base + TIMER_CR);
+	writel(~0, timer->base + TIMER2_BASE + REG_LOAD);
+	writel(timer->t1_disable_val, timer->base + TIMER_CR);
 
 	/*
 	 * documentation is not publicly available:
@@ -173,9 +238,9 @@ static int __init moxart_timer_init(struct device_node *node)
 	 * max_delta 0xfffffffe should be ok because count
 	 * register size is u32
 	 */
-	clockevents_config_and_register(&moxart_clockevent, pclk,
-					0x4, 0xfffffffe);
+	clockevents_config_and_register(&timer->clkevt, pclk, 0x4, 0xfffffffe);
 
 	return 0;
 }
 CLOCKSOURCE_OF_DECLARE(moxart, "moxa,moxart-timer", moxart_timer_init);
+CLOCKSOURCE_OF_DECLARE(aspeed, "aspeed,ast2400-timer", moxart_timer_init);
diff --git a/drivers/clocksource/timer-atmel-pit.c b/drivers/clocksource/timer-atmel-pit.c
index 7f0f5b26d8c5..6555821bbdae 100644
--- a/drivers/clocksource/timer-atmel-pit.c
+++ b/drivers/clocksource/timer-atmel-pit.c
@@ -149,24 +149,13 @@ static irqreturn_t at91sam926x_pit_interrupt(int irq, void *dev_id)
 {
 	struct pit_data *data = dev_id;
 
-	/*
-	 * irqs should be disabled here, but as the irq is shared they are only
-	 * guaranteed to be off if the timer irq is registered first.
-	 */
-	WARN_ON_ONCE(!irqs_disabled());
-
 	/* The PIT interrupt may be disabled, and is shared */
 	if (clockevent_state_periodic(&data->clkevt) &&
 	    (pit_read(data->base, AT91_PIT_SR) & AT91_PIT_PITS)) {
-		unsigned nr_ticks;
-
 		/* Get number of ticks performed before irq, and ack it */
-		nr_ticks = PIT_PICNT(pit_read(data->base, AT91_PIT_PIVR));
-		do {
-			data->cnt += data->cycle;
-			data->clkevt.event_handler(&data->clkevt);
-			nr_ticks--;
-		} while (nr_ticks);
+		data->cnt += data->cycle * PIT_PICNT(pit_read(data->base,
+							      AT91_PIT_PIVR));
+		data->clkevt.event_handler(&data->clkevt);
 
 		return IRQ_HANDLED;
 	}
@@ -177,11 +166,41 @@ static irqreturn_t at91sam926x_pit_interrupt(int irq, void *dev_id)
 /*
  * Set up both clocksource and clockevent support.
  */
-static int __init at91sam926x_pit_common_init(struct pit_data *data)
+static int __init at91sam926x_pit_dt_init(struct device_node *node)
 {
-	unsigned long	pit_rate;
-	unsigned	bits;
-	int		ret;
+	unsigned long   pit_rate;
+	unsigned        bits;
+	int             ret;
+	struct pit_data *data;
+
+	data = kzalloc(sizeof(*data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	data->base = of_iomap(node, 0);
+	if (!data->base) {
+		pr_err("Could not map PIT address\n");
+		return -ENXIO;
+	}
+
+	data->mck = of_clk_get(node, 0);
+	if (IS_ERR(data->mck)) {
+		pr_err("Unable to get mck clk\n");
+		return PTR_ERR(data->mck);
+	}
+
+	ret = clk_prepare_enable(data->mck);
+	if (ret) {
+		pr_err("Unable to enable mck\n");
+		return ret;
+	}
+
+	/* Get the interrupts property */
+	data->irq = irq_of_parse_and_map(node, 0);
+	if (!data->irq) {
+		pr_err("Unable to get IRQ from DT\n");
+		return -EINVAL;
+	}
 
 	/*
 	 * Use our actual MCK to figure out how many MCK/16 ticks per
@@ -236,46 +255,5 @@ static int __init at91sam926x_pit_common_init(struct pit_data *data)
 
 	return 0;
 }
-
-static int __init at91sam926x_pit_dt_init(struct device_node *node)
-{
-	struct pit_data *data;
-	int ret;
-
-	data = kzalloc(sizeof(*data), GFP_KERNEL);
-	if (!data)
-		return -ENOMEM;
-
-	data->base = of_iomap(node, 0);
-	if (!data->base) {
-		pr_err("Could not map PIT address\n");
-		return -ENXIO;
-	}
-
-	data->mck = of_clk_get(node, 0);
-	if (IS_ERR(data->mck))
-		/* Fallback on clkdev for !CCF-based boards */
-		data->mck = clk_get(NULL, "mck");
-
-	if (IS_ERR(data->mck)) {
-		pr_err("Unable to get mck clk\n");
-		return PTR_ERR(data->mck);
-	}
-
-	ret = clk_prepare_enable(data->mck);
-	if (ret) {
-		pr_err("Unable to enable mck\n");
-		return ret;
-	}
-
-	/* Get the interrupts property */
-	data->irq = irq_of_parse_and_map(node, 0);
-	if (!data->irq) {
-		pr_err("Unable to get IRQ from DT\n");
-		return -EINVAL;
-	}
-
-	return at91sam926x_pit_common_init(data);
-}
 CLOCKSOURCE_OF_DECLARE(at91sam926x_pit, "atmel,at91sam9260-pit",
 		       at91sam926x_pit_dt_init);
diff --git a/drivers/clocksource/timer-oxnas-rps.c b/drivers/clocksource/timer-oxnas-rps.c
index bd887e2a8cf8..d630bf417773 100644
--- a/drivers/clocksource/timer-oxnas-rps.c
+++ b/drivers/clocksource/timer-oxnas-rps.c
@@ -295,3 +295,5 @@ err_alloc:
 
 CLOCKSOURCE_OF_DECLARE(ox810se_rps,
 		       "oxsemi,ox810se-rps-timer", oxnas_rps_timer_init);
+CLOCKSOURCE_OF_DECLARE(ox820_rps,
+		       "oxsemi,ox820se-rps-timer", oxnas_rps_timer_init);
diff --git a/drivers/clocksource/timer-sun5i.c b/drivers/clocksource/timer-sun5i.c
index c184eb84101e..4f87f3e76d83 100644
--- a/drivers/clocksource/timer-sun5i.c
+++ b/drivers/clocksource/timer-sun5i.c
@@ -152,6 +152,13 @@ static irqreturn_t sun5i_timer_interrupt(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
+static cycle_t sun5i_clksrc_read(struct clocksource *clksrc)
+{
+	struct sun5i_timer_clksrc *cs = to_sun5i_timer_clksrc(clksrc);
+
+	return ~readl(cs->timer.base + TIMER_CNTVAL_LO_REG(1));
+}
+
 static int sun5i_rate_cb_clksrc(struct notifier_block *nb,
 				unsigned long event, void *data)
 {
@@ -210,8 +217,13 @@ static int __init sun5i_setup_clocksource(struct device_node *node,
 	writel(TIMER_CTL_ENABLE | TIMER_CTL_RELOAD,
 	       base + TIMER_CTL_REG(1));
 
-	ret = clocksource_mmio_init(base + TIMER_CNTVAL_LO_REG(1), node->name,
-				    rate, 340, 32, clocksource_mmio_readl_down);
+	cs->clksrc.name = node->name;
+	cs->clksrc.rating = 340;
+	cs->clksrc.read = sun5i_clksrc_read;
+	cs->clksrc.mask = CLOCKSOURCE_MASK(32);
+	cs->clksrc.flags = CLOCK_SOURCE_IS_CONTINUOUS;
+
+	ret = clocksource_register_hz(&cs->clksrc, rate);
 	if (ret) {
 		pr_err("Couldn't register clock source.\n");
 		goto err_remove_notifier;
diff --git a/drivers/clocksource/timer-ti-32k.c b/drivers/clocksource/timer-ti-32k.c
index 92b7e390f6c8..cf5b14e442e4 100644
--- a/drivers/clocksource/timer-ti-32k.c
+++ b/drivers/clocksource/timer-ti-32k.c
@@ -65,7 +65,7 @@ static inline struct ti_32k *to_ti_32k(struct clocksource *cs)
 	return container_of(cs, struct ti_32k, cs);
 }
 
-static cycle_t ti_32k_read_cycles(struct clocksource *cs)
+static cycle_t notrace ti_32k_read_cycles(struct clocksource *cs)
 {
 	struct ti_32k *ti = to_ti_32k(cs);
 
diff --git a/drivers/cpufreq/Kconfig b/drivers/cpufreq/Kconfig
index 74919aa81dcb..d8b164a7c4e5 100644
--- a/drivers/cpufreq/Kconfig
+++ b/drivers/cpufreq/Kconfig
@@ -194,7 +194,7 @@ config CPU_FREQ_GOV_CONSERVATIVE
 	  If in doubt, say N.
 
 config CPU_FREQ_GOV_SCHEDUTIL
-	tristate "'schedutil' cpufreq policy governor"
+	bool "'schedutil' cpufreq policy governor"
 	depends on CPU_FREQ && SMP
 	select CPU_FREQ_GOV_ATTR_SET
 	select IRQ_WORK
@@ -208,9 +208,6 @@ config CPU_FREQ_GOV_SCHEDUTIL
 	  frequency tipping point is at utilization/capacity equal to 80% in
 	  both cases.
 
-	  To compile this driver as a module, choose M here: the module will
-	  be called cpufreq_schedutil.
-
 	  If in doubt, say N.
 
 comment "CPU frequency scaling drivers"
@@ -225,7 +222,7 @@ config CPUFREQ_DT
 	help
 	  This adds a generic DT based cpufreq driver for frequency management.
 	  It supports both uniprocessor (UP) and symmetric multiprocessor (SMP)
-	  systems which share clock and voltage across all CPUs.
+	  systems.
 
 	  If in doubt, say N.
 
diff --git a/drivers/cpufreq/cppc_cpufreq.c b/drivers/cpufreq/cppc_cpufreq.c
index 8882b8e2ecd0..4852d9efe74e 100644
--- a/drivers/cpufreq/cppc_cpufreq.c
+++ b/drivers/cpufreq/cppc_cpufreq.c
@@ -19,10 +19,19 @@
 #include <linux/delay.h>
 #include <linux/cpu.h>
 #include <linux/cpufreq.h>
+#include <linux/dmi.h>
 #include <linux/vmalloc.h>
 
+#include <asm/unaligned.h>
+
 #include <acpi/cppc_acpi.h>
 
+/* Minimum struct length needed for the DMI processor entry we want */
+#define DMI_ENTRY_PROCESSOR_MIN_LENGTH	48
+
+/* Offest in the DMI processor structure for the max frequency */
+#define DMI_PROCESSOR_MAX_SPEED  0x14
+
 /*
  * These structs contain information parsed from per CPU
  * ACPI _CPC structures.
@@ -30,19 +39,58 @@
  * performance capabilities, desired performance level
  * requested etc.
  */
-static struct cpudata **all_cpu_data;
+static struct cppc_cpudata **all_cpu_data;
+
+/* Capture the max KHz from DMI */
+static u64 cppc_dmi_max_khz;
+
+/* Callback function used to retrieve the max frequency from DMI */
+static void cppc_find_dmi_mhz(const struct dmi_header *dm, void *private)
+{
+	const u8 *dmi_data = (const u8 *)dm;
+	u16 *mhz = (u16 *)private;
+
+	if (dm->type == DMI_ENTRY_PROCESSOR &&
+	    dm->length >= DMI_ENTRY_PROCESSOR_MIN_LENGTH) {
+		u16 val = (u16)get_unaligned((const u16 *)
+				(dmi_data + DMI_PROCESSOR_MAX_SPEED));
+		*mhz = val > *mhz ? val : *mhz;
+	}
+}
+
+/* Look up the max frequency in DMI */
+static u64 cppc_get_dmi_max_khz(void)
+{
+	u16 mhz = 0;
+
+	dmi_walk(cppc_find_dmi_mhz, &mhz);
+
+	/*
+	 * Real stupid fallback value, just in case there is no
+	 * actual value set.
+	 */
+	mhz = mhz ? mhz : 1;
+
+	return (1000 * mhz);
+}
 
 static int cppc_cpufreq_set_target(struct cpufreq_policy *policy,
 		unsigned int target_freq,
 		unsigned int relation)
 {
-	struct cpudata *cpu;
+	struct cppc_cpudata *cpu;
 	struct cpufreq_freqs freqs;
+	u32 desired_perf;
 	int ret = 0;
 
 	cpu = all_cpu_data[policy->cpu];
 
-	cpu->perf_ctrls.desired_perf = target_freq;
+	desired_perf = (u64)target_freq * cpu->perf_caps.highest_perf / cppc_dmi_max_khz;
+	/* Return if it is exactly the same perf */
+	if (desired_perf == cpu->perf_ctrls.desired_perf)
+		return ret;
+
+	cpu->perf_ctrls.desired_perf = desired_perf;
 	freqs.old = policy->cur;
 	freqs.new = target_freq;
 
@@ -66,7 +114,7 @@ static int cppc_verify_policy(struct cpufreq_policy *policy)
 static void cppc_cpufreq_stop_cpu(struct cpufreq_policy *policy)
 {
 	int cpu_num = policy->cpu;
-	struct cpudata *cpu = all_cpu_data[cpu_num];
+	struct cppc_cpudata *cpu = all_cpu_data[cpu_num];
 	int ret;
 
 	cpu->perf_ctrls.desired_perf = cpu->perf_caps.lowest_perf;
@@ -79,7 +127,7 @@ static void cppc_cpufreq_stop_cpu(struct cpufreq_policy *policy)
 
 static int cppc_cpufreq_cpu_init(struct cpufreq_policy *policy)
 {
-	struct cpudata *cpu;
+	struct cppc_cpudata *cpu;
 	unsigned int cpu_num = policy->cpu;
 	int ret = 0;
 
@@ -94,10 +142,13 @@ static int cppc_cpufreq_cpu_init(struct cpufreq_policy *policy)
 		return ret;
 	}
 
-	policy->min = cpu->perf_caps.lowest_perf;
-	policy->max = cpu->perf_caps.highest_perf;
+	cppc_dmi_max_khz = cppc_get_dmi_max_khz();
+
+	policy->min = cpu->perf_caps.lowest_perf * cppc_dmi_max_khz / cpu->perf_caps.highest_perf;
+	policy->max = cppc_dmi_max_khz;
 	policy->cpuinfo.min_freq = policy->min;
 	policy->cpuinfo.max_freq = policy->max;
+	policy->cpuinfo.transition_latency = cppc_get_transition_latency(cpu_num);
 	policy->shared_type = cpu->shared_type;
 
 	if (policy->shared_type == CPUFREQ_SHARED_TYPE_ANY)
@@ -112,7 +163,8 @@ static int cppc_cpufreq_cpu_init(struct cpufreq_policy *policy)
 	cpu->cur_policy = policy;
 
 	/* Set policy->cur to max now. The governors will adjust later. */
-	policy->cur = cpu->perf_ctrls.desired_perf = cpu->perf_caps.highest_perf;
+	policy->cur = cppc_dmi_max_khz;
+	cpu->perf_ctrls.desired_perf = cpu->perf_caps.highest_perf;
 
 	ret = cppc_set_perf(cpu_num, &cpu->perf_ctrls);
 	if (ret)
@@ -134,7 +186,7 @@ static struct cpufreq_driver cppc_cpufreq_driver = {
 static int __init cppc_cpufreq_init(void)
 {
 	int i, ret = 0;
-	struct cpudata *cpu;
+	struct cppc_cpudata *cpu;
 
 	if (acpi_disabled)
 		return -ENODEV;
@@ -144,7 +196,7 @@ static int __init cppc_cpufreq_init(void)
 		return -ENOMEM;
 
 	for_each_possible_cpu(i) {
-		all_cpu_data[i] = kzalloc(sizeof(struct cpudata), GFP_KERNEL);
+		all_cpu_data[i] = kzalloc(sizeof(struct cppc_cpudata), GFP_KERNEL);
 		if (!all_cpu_data[i])
 			goto out;
 
@@ -175,7 +227,7 @@ out:
 
 static void __exit cppc_cpufreq_exit(void)
 {
-	struct cpudata *cpu;
+	struct cppc_cpudata *cpu;
 	int i;
 
 	cpufreq_unregister_driver(&cppc_cpufreq_driver);
diff --git a/drivers/cpufreq/cpufreq-dt-platdev.c b/drivers/cpufreq/cpufreq-dt-platdev.c
index 2ee40fd360ca..71267626456b 100644
--- a/drivers/cpufreq/cpufreq-dt-platdev.c
+++ b/drivers/cpufreq/cpufreq-dt-platdev.c
@@ -11,6 +11,8 @@
 #include <linux/of.h>
 #include <linux/platform_device.h>
 
+#include "cpufreq-dt.h"
+
 static const struct of_device_id machines[] __initconst = {
 	{ .compatible = "allwinner,sun4i-a10", },
 	{ .compatible = "allwinner,sun5i-a10s", },
@@ -40,6 +42,7 @@ static const struct of_device_id machines[] __initconst = {
 	{ .compatible = "samsung,exynos5250", },
 #ifndef CONFIG_BL_SWITCHER
 	{ .compatible = "samsung,exynos5420", },
+	{ .compatible = "samsung,exynos5433", },
 	{ .compatible = "samsung,exynos5800", },
 #endif
 
@@ -51,6 +54,7 @@ static const struct of_device_id machines[] __initconst = {
 	{ .compatible = "renesas,r8a7779", },
 	{ .compatible = "renesas,r8a7790", },
 	{ .compatible = "renesas,r8a7791", },
+	{ .compatible = "renesas,r8a7792", },
 	{ .compatible = "renesas,r8a7793", },
 	{ .compatible = "renesas,r8a7794", },
 	{ .compatible = "renesas,sh73a0", },
@@ -68,6 +72,8 @@ static const struct of_device_id machines[] __initconst = {
 
 	{ .compatible = "sigma,tango4" },
 
+	{ .compatible = "ti,am33xx", },
+	{ .compatible = "ti,dra7", },
 	{ .compatible = "ti,omap2", },
 	{ .compatible = "ti,omap3", },
 	{ .compatible = "ti,omap4", },
@@ -91,7 +97,8 @@ static int __init cpufreq_dt_platdev_init(void)
 	if (!match)
 		return -ENODEV;
 
-	return PTR_ERR_OR_ZERO(platform_device_register_simple("cpufreq-dt", -1,
-							       NULL, 0));
+	return PTR_ERR_OR_ZERO(platform_device_register_data(NULL, "cpufreq-dt",
+			       -1, match->data,
+			       sizeof(struct cpufreq_dt_platform_data)));
 }
 device_initcall(cpufreq_dt_platdev_init);
diff --git a/drivers/cpufreq/cpufreq-dt.c b/drivers/cpufreq/cpufreq-dt.c
index 3957de801ae8..5c07ae05d69a 100644
--- a/drivers/cpufreq/cpufreq-dt.c
+++ b/drivers/cpufreq/cpufreq-dt.c
@@ -25,6 +25,8 @@
 #include <linux/slab.h>
 #include <linux/thermal.h>
 
+#include "cpufreq-dt.h"
+
 struct private_data {
 	struct device *cpu_dev;
 	struct thermal_cooling_device *cdev;
@@ -353,6 +355,7 @@ static struct cpufreq_driver dt_cpufreq_driver = {
 
 static int dt_cpufreq_probe(struct platform_device *pdev)
 {
+	struct cpufreq_dt_platform_data *data = dev_get_platdata(&pdev->dev);
 	int ret;
 
 	/*
@@ -366,7 +369,8 @@ static int dt_cpufreq_probe(struct platform_device *pdev)
 	if (ret)
 		return ret;
 
-	dt_cpufreq_driver.driver_data = dev_get_platdata(&pdev->dev);
+	if (data && data->have_governor_per_policy)
+		dt_cpufreq_driver.flags |= CPUFREQ_HAVE_GOVERNOR_PER_POLICY;
 
 	ret = cpufreq_register_driver(&dt_cpufreq_driver);
 	if (ret)
diff --git a/drivers/cpufreq/cpufreq-dt.h b/drivers/cpufreq/cpufreq-dt.h
new file mode 100644
index 000000000000..54d774e46c43
--- /dev/null
+++ b/drivers/cpufreq/cpufreq-dt.h
@@ -0,0 +1,19 @@
+/*
+ * Copyright (C) 2016 Linaro
+ * Viresh Kumar <viresh.kumar@linaro.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef __CPUFREQ_DT_H__
+#define __CPUFREQ_DT_H__
+
+#include <linux/types.h>
+
+struct cpufreq_dt_platform_data {
+	bool have_governor_per_policy;
+};
+
+#endif /* __CPUFREQ_DT_H__ */
diff --git a/drivers/cpufreq/cpufreq.c b/drivers/cpufreq/cpufreq.c
index 3dd4884c6f9e..6e6c1fb60fbc 100644
--- a/drivers/cpufreq/cpufreq.c
+++ b/drivers/cpufreq/cpufreq.c
@@ -916,58 +916,18 @@ static struct kobj_type ktype_cpufreq = {
 	.release	= cpufreq_sysfs_release,
 };
 
-static int add_cpu_dev_symlink(struct cpufreq_policy *policy, int cpu)
+static int add_cpu_dev_symlink(struct cpufreq_policy *policy,
+			       struct device *dev)
 {
-	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;
-
-	/* Some related CPUs might not be present (physically hotplugged) */
-	for_each_cpu(j, policy->real_cpus) {
-		ret = add_cpu_dev_symlink(policy, j);
-		if (ret)
-			break;
-	}
-
-	return ret;
+	dev_dbg(dev, "%s: Adding symlink\n", __func__);
+	return sysfs_create_link(&dev->kobj, &policy->kobj, "cpufreq");
 }
 
-static void cpufreq_remove_dev_symlink(struct cpufreq_policy *policy)
+static void remove_cpu_dev_symlink(struct cpufreq_policy *policy,
+				   struct device *dev)
 {
-	unsigned int j;
-
-	/* Some related CPUs might not be present (physically hotplugged) */
-	for_each_cpu(j, policy->real_cpus)
-		remove_cpu_dev_symlink(policy, j);
+	dev_dbg(dev, "%s: Removing symlink\n", __func__);
+	sysfs_remove_link(&dev->kobj, "cpufreq");
 }
 
 static int cpufreq_add_dev_interface(struct cpufreq_policy *policy)
@@ -999,7 +959,7 @@ static int cpufreq_add_dev_interface(struct cpufreq_policy *policy)
 			return ret;
 	}
 
-	return cpufreq_add_dev_symlink(policy);
+	return 0;
 }
 
 __weak struct cpufreq_governor *cpufreq_default_governor(void)
@@ -1073,13 +1033,9 @@ static void handle_update(struct work_struct *work)
 
 static struct cpufreq_policy *cpufreq_policy_alloc(unsigned int cpu)
 {
-	struct device *dev = get_cpu_device(cpu);
 	struct cpufreq_policy *policy;
 	int ret;
 
-	if (WARN_ON(!dev))
-		return NULL;
-
 	policy = kzalloc(sizeof(*policy), GFP_KERNEL);
 	if (!policy)
 		return NULL;
@@ -1133,7 +1089,6 @@ static void cpufreq_policy_put_kobj(struct cpufreq_policy *policy, bool notify)
 
 	down_write(&policy->rwsem);
 	cpufreq_stats_free_table(policy);
-	cpufreq_remove_dev_symlink(policy);
 	kobj = &policy->kobj;
 	cmp = &policy->kobj_unregister;
 	up_write(&policy->rwsem);
@@ -1215,8 +1170,8 @@ static int cpufreq_online(unsigned int cpu)
 	if (new_policy) {
 		/* related_cpus should at least include policy->cpus. */
 		cpumask_copy(policy->related_cpus, policy->cpus);
-		/* Remember CPUs present at the policy creation time. */
-		cpumask_and(policy->real_cpus, policy->cpus, cpu_present_mask);
+		/* Clear mask of registered CPUs */
+		cpumask_clear(policy->real_cpus);
 	}
 
 	/*
@@ -1331,6 +1286,8 @@ out_free_policy:
 	return ret;
 }
 
+static int cpufreq_offline(unsigned int cpu);
+
 /**
  * cpufreq_add_dev - the cpufreq interface for a CPU device.
  * @dev: CPU device.
@@ -1340,25 +1297,31 @@ static int cpufreq_add_dev(struct device *dev, struct subsys_interface *sif)
 {
 	struct cpufreq_policy *policy;
 	unsigned cpu = dev->id;
+	int ret;
 
 	dev_dbg(dev, "%s: adding CPU%u\n", __func__, cpu);
 
-	if (cpu_online(cpu))
-		return cpufreq_online(cpu);
+	if (cpu_online(cpu)) {
+		ret = cpufreq_online(cpu);
+		if (ret)
+			return ret;
+	}
 
-	/*
-	 * A hotplug notifier will follow and we will handle it as CPU online
-	 * then.  For now, just create the sysfs link, unless there is no policy
-	 * or the link is already present.
-	 */
+	/* Create sysfs link on CPU registration */
 	policy = per_cpu(cpufreq_cpu_data, cpu);
 	if (!policy || cpumask_test_and_set_cpu(cpu, policy->real_cpus))
 		return 0;
 
-	return add_cpu_dev_symlink(policy, cpu);
+	ret = add_cpu_dev_symlink(policy, dev);
+	if (ret) {
+		cpumask_clear_cpu(cpu, policy->real_cpus);
+		cpufreq_offline(cpu);
+	}
+
+	return ret;
 }
 
-static void cpufreq_offline(unsigned int cpu)
+static int cpufreq_offline(unsigned int cpu)
 {
 	struct cpufreq_policy *policy;
 	int ret;
@@ -1368,7 +1331,7 @@ static void cpufreq_offline(unsigned int cpu)
 	policy = cpufreq_cpu_get_raw(cpu);
 	if (!policy) {
 		pr_debug("%s: No cpu_data found\n", __func__);
-		return;
+		return 0;
 	}
 
 	down_write(&policy->rwsem);
@@ -1417,6 +1380,7 @@ static void cpufreq_offline(unsigned int cpu)
 
 unlock:
 	up_write(&policy->rwsem);
+	return 0;
 }
 
 /**
@@ -1436,7 +1400,7 @@ static void cpufreq_remove_dev(struct device *dev, struct subsys_interface *sif)
 		cpufreq_offline(cpu);
 
 	cpumask_clear_cpu(cpu, policy->real_cpus);
-	remove_cpu_dev_symlink(policy, cpu);
+	remove_cpu_dev_symlink(policy, dev);
 
 	if (cpumask_empty(policy->real_cpus))
 		cpufreq_policy_free(policy, true);
@@ -2332,28 +2296,6 @@ unlock:
 }
 EXPORT_SYMBOL(cpufreq_update_policy);
 
-static int cpufreq_cpu_callback(struct notifier_block *nfb,
-					unsigned long action, void *hcpu)
-{
-	unsigned int cpu = (unsigned long)hcpu;
-
-	switch (action & ~CPU_TASKS_FROZEN) {
-	case CPU_ONLINE:
-	case CPU_DOWN_FAILED:
-		cpufreq_online(cpu);
-		break;
-
-	case CPU_DOWN_PREPARE:
-		cpufreq_offline(cpu);
-		break;
-	}
-	return NOTIFY_OK;
-}
-
-static struct notifier_block __refdata cpufreq_cpu_notifier = {
-	.notifier_call = cpufreq_cpu_callback,
-};
-
 /*********************************************************************
  *               BOOST						     *
  *********************************************************************/
@@ -2455,6 +2397,7 @@ EXPORT_SYMBOL_GPL(cpufreq_boost_enabled);
 /*********************************************************************
  *               REGISTER / UNREGISTER CPUFREQ DRIVER                *
  *********************************************************************/
+static enum cpuhp_state hp_online;
 
 /**
  * cpufreq_register_driver - register a CPU Frequency driver
@@ -2517,7 +2460,14 @@ int cpufreq_register_driver(struct cpufreq_driver *driver_data)
 		goto err_if_unreg;
 	}
 
-	register_hotcpu_notifier(&cpufreq_cpu_notifier);
+	ret = cpuhp_setup_state_nocalls(CPUHP_AP_ONLINE_DYN, "cpufreq:online",
+					cpufreq_online,
+					cpufreq_offline);
+	if (ret < 0)
+		goto err_if_unreg;
+	hp_online = ret;
+	ret = 0;
+
 	pr_debug("driver %s up and running\n", driver_data->name);
 	goto out;
 
@@ -2556,7 +2506,7 @@ int cpufreq_unregister_driver(struct cpufreq_driver *driver)
 	get_online_cpus();
 	subsys_interface_unregister(&cpufreq_interface);
 	remove_boost_sysfs_file();
-	unregister_hotcpu_notifier(&cpufreq_cpu_notifier);
+	cpuhp_remove_state_nocalls(hp_online);
 
 	write_lock_irqsave(&cpufreq_driver_lock, flags);
 
diff --git a/drivers/cpufreq/cpufreq_conservative.c b/drivers/cpufreq/cpufreq_conservative.c
index 18da4f8051d3..13475890d792 100644
--- a/drivers/cpufreq/cpufreq_conservative.c
+++ b/drivers/cpufreq/cpufreq_conservative.c
@@ -17,6 +17,7 @@
 struct cs_policy_dbs_info {
 	struct policy_dbs_info policy_dbs;
 	unsigned int down_skip;
+	unsigned int requested_freq;
 };
 
 static inline struct cs_policy_dbs_info *to_dbs_info(struct policy_dbs_info *policy_dbs)
@@ -61,6 +62,7 @@ static unsigned int cs_dbs_timer(struct cpufreq_policy *policy)
 {
 	struct policy_dbs_info *policy_dbs = policy->governor_data;
 	struct cs_policy_dbs_info *dbs_info = to_dbs_info(policy_dbs);
+	unsigned int requested_freq = dbs_info->requested_freq;
 	struct dbs_data *dbs_data = policy_dbs->dbs_data;
 	struct cs_dbs_tuners *cs_tuners = dbs_data->tuners;
 	unsigned int load = dbs_update(policy);
@@ -72,10 +74,16 @@ static unsigned int cs_dbs_timer(struct cpufreq_policy *policy)
 	if (cs_tuners->freq_step == 0)
 		goto out;
 
+	/*
+	 * If requested_freq is out of range, it is likely that the limits
+	 * changed in the meantime, so fall back to current frequency in that
+	 * case.
+	 */
+	if (requested_freq > policy->max || requested_freq < policy->min)
+		requested_freq = policy->cur;
+
 	/* Check for frequency increase */
 	if (load > dbs_data->up_threshold) {
-		unsigned int requested_freq = policy->cur;
-
 		dbs_info->down_skip = 0;
 
 		/* if we are already at full speed then break out early */
@@ -83,8 +91,11 @@ static unsigned int cs_dbs_timer(struct cpufreq_policy *policy)
 			goto out;
 
 		requested_freq += get_freq_target(cs_tuners, policy);
+		if (requested_freq > policy->max)
+			requested_freq = policy->max;
 
 		__cpufreq_driver_target(policy, requested_freq, CPUFREQ_RELATION_H);
+		dbs_info->requested_freq = requested_freq;
 		goto out;
 	}
 
@@ -95,7 +106,7 @@ static unsigned int cs_dbs_timer(struct cpufreq_policy *policy)
 
 	/* Check for frequency decrease */
 	if (load < cs_tuners->down_threshold) {
-		unsigned int freq_target, requested_freq = policy->cur;
+		unsigned int freq_target;
 		/*
 		 * if we cannot reduce the frequency anymore, break out early
 		 */
@@ -109,6 +120,7 @@ static unsigned int cs_dbs_timer(struct cpufreq_policy *policy)
 			requested_freq = policy->min;
 
 		__cpufreq_driver_target(policy, requested_freq, CPUFREQ_RELATION_L);
+		dbs_info->requested_freq = requested_freq;
 	}
 
  out:
@@ -287,6 +299,7 @@ static void cs_start(struct cpufreq_policy *policy)
 	struct cs_policy_dbs_info *dbs_info = to_dbs_info(policy->governor_data);
 
 	dbs_info->down_skip = 0;
+	dbs_info->requested_freq = policy->cur;
 }
 
 static struct dbs_governor cs_governor = {
diff --git a/drivers/cpufreq/cpufreq_governor.c b/drivers/cpufreq/cpufreq_governor.c
index e415349ab31b..642dd0f183a8 100644
--- a/drivers/cpufreq/cpufreq_governor.c
+++ b/drivers/cpufreq/cpufreq_governor.c
@@ -260,7 +260,7 @@ static void dbs_irq_work(struct irq_work *irq_work)
 }
 
 static void dbs_update_util_handler(struct update_util_data *data, u64 time,
-				    unsigned long util, unsigned long max)
+				    unsigned int flags)
 {
 	struct cpu_dbs_info *cdbs = container_of(data, struct cpu_dbs_info, update_util);
 	struct policy_dbs_info *policy_dbs = cdbs->policy_dbs;
diff --git a/drivers/cpufreq/intel_pstate.c b/drivers/cpufreq/intel_pstate.c
index be9eade147f2..f535f8123258 100644
--- a/drivers/cpufreq/intel_pstate.c
+++ b/drivers/cpufreq/intel_pstate.c
@@ -181,6 +181,8 @@ struct _pid {
  * @cpu:		CPU number for this instance data
  * @update_util:	CPUFreq utility callback information
  * @update_util_set:	CPUFreq utility callback is set
+ * @iowait_boost:	iowait-related boost fraction
+ * @last_update:	Time of the last update.
  * @pstate:		Stores P state limits for this CPU
  * @vid:		Stores VID limits for this CPU
  * @pid:		Stores PID parameters for this CPU
@@ -206,6 +208,7 @@ struct cpudata {
 	struct vid_data vid;
 	struct _pid pid;
 
+	u64	last_update;
 	u64	last_sample_time;
 	u64	prev_aperf;
 	u64	prev_mperf;
@@ -216,12 +219,13 @@ struct cpudata {
 	struct acpi_processor_performance acpi_perf_data;
 	bool valid_pss_table;
 #endif
+	unsigned int iowait_boost;
 };
 
 static struct cpudata **all_cpu_data;
 
 /**
- * struct pid_adjust_policy - Stores static PID configuration data
+ * struct pstate_adjust_policy - Stores static PID configuration data
  * @sample_rate_ms:	PID calculation sample rate in ms
  * @sample_rate_ns:	Sample rate calculation in ns
  * @deadband:		PID deadband
@@ -229,6 +233,7 @@ static struct cpudata **all_cpu_data;
  * @p_gain_pct:		PID proportional gain
  * @i_gain_pct:		PID integral gain
  * @d_gain_pct:		PID derivative gain
+ * @boost_iowait:	Whether or not to use iowait boosting.
  *
  * Stores per CPU model static PID configuration data.
  */
@@ -240,6 +245,7 @@ struct pstate_adjust_policy {
 	int p_gain_pct;
 	int d_gain_pct;
 	int i_gain_pct;
+	bool boost_iowait;
 };
 
 /**
@@ -556,12 +562,12 @@ static void intel_pstate_hwp_set(const struct cpumask *cpumask)
 	int min, hw_min, max, hw_max, cpu, range, adj_range;
 	u64 value, cap;
 
-	rdmsrl(MSR_HWP_CAPABILITIES, cap);
-	hw_min = HWP_LOWEST_PERF(cap);
-	hw_max = HWP_HIGHEST_PERF(cap);
-	range = hw_max - hw_min;
-
 	for_each_cpu(cpu, cpumask) {
+		rdmsrl_on_cpu(cpu, MSR_HWP_CAPABILITIES, &cap);
+		hw_min = HWP_LOWEST_PERF(cap);
+		hw_max = HWP_HIGHEST_PERF(cap);
+		range = hw_max - hw_min;
+
 		rdmsrl_on_cpu(cpu, MSR_HWP_REQUEST, &value);
 		adj_range = limits->min_perf_pct * range / 100;
 		min = hw_min + adj_range;
@@ -1029,7 +1035,7 @@ static struct cpu_defaults core_params = {
 	},
 };
 
-static struct cpu_defaults silvermont_params = {
+static const struct cpu_defaults silvermont_params = {
 	.pid_policy = {
 		.sample_rate_ms = 10,
 		.deadband = 0,
@@ -1037,6 +1043,7 @@ static struct cpu_defaults silvermont_params = {
 		.p_gain_pct = 14,
 		.d_gain_pct = 0,
 		.i_gain_pct = 4,
+		.boost_iowait = true,
 	},
 	.funcs = {
 		.get_max = atom_get_max_pstate,
@@ -1050,7 +1057,7 @@ static struct cpu_defaults silvermont_params = {
 	},
 };
 
-static struct cpu_defaults airmont_params = {
+static const struct cpu_defaults airmont_params = {
 	.pid_policy = {
 		.sample_rate_ms = 10,
 		.deadband = 0,
@@ -1058,6 +1065,7 @@ static struct cpu_defaults airmont_params = {
 		.p_gain_pct = 14,
 		.d_gain_pct = 0,
 		.i_gain_pct = 4,
+		.boost_iowait = true,
 	},
 	.funcs = {
 		.get_max = atom_get_max_pstate,
@@ -1071,7 +1079,7 @@ static struct cpu_defaults airmont_params = {
 	},
 };
 
-static struct cpu_defaults knl_params = {
+static const struct cpu_defaults knl_params = {
 	.pid_policy = {
 		.sample_rate_ms = 10,
 		.deadband = 0,
@@ -1091,7 +1099,7 @@ static struct cpu_defaults knl_params = {
 	},
 };
 
-static struct cpu_defaults bxt_params = {
+static const struct cpu_defaults bxt_params = {
 	.pid_policy = {
 		.sample_rate_ms = 10,
 		.deadband = 0,
@@ -1099,6 +1107,7 @@ static struct cpu_defaults bxt_params = {
 		.p_gain_pct = 14,
 		.d_gain_pct = 0,
 		.i_gain_pct = 4,
+		.boost_iowait = true,
 	},
 	.funcs = {
 		.get_max = core_get_max_pstate,
@@ -1222,36 +1231,38 @@ static inline int32_t get_avg_pstate(struct cpudata *cpu)
 static inline int32_t get_target_pstate_use_cpu_load(struct cpudata *cpu)
 {
 	struct sample *sample = &cpu->sample;
-	u64 cummulative_iowait, delta_iowait_us;
-	u64 delta_iowait_mperf;
-	u64 mperf, now;
-	int32_t cpu_load;
+	int32_t busy_frac, boost;
+	int target, avg_pstate;
 
-	cummulative_iowait = get_cpu_iowait_time_us(cpu->cpu, &now);
+	busy_frac = div_fp(sample->mperf, sample->tsc);
 
-	/*
-	 * Convert iowait time into number of IO cycles spent at max_freq.
-	 * IO is considered as busy only for the cpu_load algorithm. For
-	 * performance this is not needed since we always try to reach the
-	 * maximum P-State, so we are already boosting the IOs.
-	 */
-	delta_iowait_us = cummulative_iowait - cpu->prev_cummulative_iowait;
-	delta_iowait_mperf = div64_u64(delta_iowait_us * cpu->pstate.scaling *
-		cpu->pstate.max_pstate, MSEC_PER_SEC);
+	boost = cpu->iowait_boost;
+	cpu->iowait_boost >>= 1;
+
+	if (busy_frac < boost)
+		busy_frac = boost;
 
-	mperf = cpu->sample.mperf + delta_iowait_mperf;
-	cpu->prev_cummulative_iowait = cummulative_iowait;
+	sample->busy_scaled = busy_frac * 100;
+
+	target = limits->no_turbo || limits->turbo_disabled ?
+			cpu->pstate.max_pstate : cpu->pstate.turbo_pstate;
+	target += target >> 2;
+	target = mul_fp(target, busy_frac);
+	if (target < cpu->pstate.min_pstate)
+		target = cpu->pstate.min_pstate;
 
 	/*
-	 * The load can be estimated as the ratio of the mperf counter
-	 * running at a constant frequency during active periods
-	 * (C0) and the time stamp counter running at the same frequency
-	 * also during C-states.
+	 * If the average P-state during the previous cycle was higher than the
+	 * current target, add 50% of the difference to the target to reduce
+	 * possible performance oscillations and offset possible performance
+	 * loss related to moving the workload from one CPU to another within
+	 * a package/module.
 	 */
-	cpu_load = div64_u64(int_tofp(100) * mperf, sample->tsc);
-	cpu->sample.busy_scaled = cpu_load;
+	avg_pstate = get_avg_pstate(cpu);
+	if (avg_pstate > target)
+		target += (avg_pstate - target) >> 1;
 
-	return get_avg_pstate(cpu) - pid_calc(&cpu->pid, cpu_load);
+	return target;
 }
 
 static inline int32_t get_target_pstate_use_performance(struct cpudata *cpu)
@@ -1260,10 +1271,11 @@ static inline int32_t get_target_pstate_use_performance(struct cpudata *cpu)
 	u64 duration_ns;
 
 	/*
-	 * perf_scaled is the average performance during the last sampling
-	 * period scaled by the ratio of the maximum P-state to the P-state
-	 * requested last time (in percent).  That measures the system's
-	 * response to the previous P-state selection.
+	 * perf_scaled is the ratio of the average P-state during the last
+	 * sampling period to the P-state requested last time (in percent).
+	 *
+	 * That measures the system's response to the previous P-state
+	 * selection.
 	 */
 	max_pstate = cpu->pstate.max_pstate_physical;
 	current_pstate = cpu->pstate.current_pstate;
@@ -1325,15 +1337,29 @@ static inline void intel_pstate_adjust_busy_pstate(struct cpudata *cpu)
 		sample->mperf,
 		sample->aperf,
 		sample->tsc,
-		get_avg_frequency(cpu));
+		get_avg_frequency(cpu),
+		fp_toint(cpu->iowait_boost * 100));
 }
 
 static void intel_pstate_update_util(struct update_util_data *data, u64 time,
-				     unsigned long util, unsigned long max)
+				     unsigned int flags)
 {
 	struct cpudata *cpu = container_of(data, struct cpudata, update_util);
-	u64 delta_ns = time - cpu->sample.time;
+	u64 delta_ns;
+
+	if (pid_params.boost_iowait) {
+		if (flags & SCHED_CPUFREQ_IOWAIT) {
+			cpu->iowait_boost = int_tofp(1);
+		} else if (cpu->iowait_boost) {
+			/* Clear iowait_boost if the CPU may have been idle. */
+			delta_ns = time - cpu->last_update;
+			if (delta_ns > TICK_NSEC)
+				cpu->iowait_boost = 0;
+		}
+		cpu->last_update = time;
+	}
 
+	delta_ns = time - cpu->sample.time;
 	if ((s64)delta_ns >= pid_params.sample_rate_ns) {
 		bool sample_taken = intel_pstate_sample(cpu, time);
 
diff --git a/drivers/cpufreq/kirkwood-cpufreq.c b/drivers/cpufreq/kirkwood-cpufreq.c
index be42f103db60..1b9bcd76c60e 100644
--- a/drivers/cpufreq/kirkwood-cpufreq.c
+++ b/drivers/cpufreq/kirkwood-cpufreq.c
@@ -123,7 +123,7 @@ static int kirkwood_cpufreq_probe(struct platform_device *pdev)
 
 	priv.cpu_clk = of_clk_get_by_name(np, "cpu_clk");
 	if (IS_ERR(priv.cpu_clk)) {
-		dev_err(priv.dev, "Unable to get cpuclk");
+		dev_err(priv.dev, "Unable to get cpuclk\n");
 		return PTR_ERR(priv.cpu_clk);
 	}
 
@@ -132,7 +132,7 @@ static int kirkwood_cpufreq_probe(struct platform_device *pdev)
 
 	priv.ddr_clk = of_clk_get_by_name(np, "ddrclk");
 	if (IS_ERR(priv.ddr_clk)) {
-		dev_err(priv.dev, "Unable to get ddrclk");
+		dev_err(priv.dev, "Unable to get ddrclk\n");
 		err = PTR_ERR(priv.ddr_clk);
 		goto out_cpu;
 	}
@@ -142,7 +142,7 @@ static int kirkwood_cpufreq_probe(struct platform_device *pdev)
 
 	priv.powersave_clk = of_clk_get_by_name(np, "powersave");
 	if (IS_ERR(priv.powersave_clk)) {
-		dev_err(priv.dev, "Unable to get powersave");
+		dev_err(priv.dev, "Unable to get powersave\n");
 		err = PTR_ERR(priv.powersave_clk);
 		goto out_ddr;
 	}
@@ -155,7 +155,7 @@ static int kirkwood_cpufreq_probe(struct platform_device *pdev)
 	if (!err)
 		return 0;
 
-	dev_err(priv.dev, "Failed to register cpufreq driver");
+	dev_err(priv.dev, "Failed to register cpufreq driver\n");
 
 	clk_disable_unprepare(priv.powersave_clk);
 out_ddr:
diff --git a/drivers/cpufreq/sa1110-cpufreq.c b/drivers/cpufreq/sa1110-cpufreq.c
index b5befc211172..2bac9b6cfeea 100644
--- a/drivers/cpufreq/sa1110-cpufreq.c
+++ b/drivers/cpufreq/sa1110-cpufreq.c
@@ -159,7 +159,7 @@ sdram_calculate_timing(struct sdram_info *sd, u_int cpu_khz,
 	 * half speed or use delayed read latching (errata 13).
 	 */
 	if ((ns_to_cycles(sdram->tck, sd_khz) > 1) ||
-	    (CPU_REVISION < CPU_SA1110_B2 && sd_khz < 62000))
+	    (read_cpuid_revision() < ARM_CPU_REV_SA1110_B2 && sd_khz < 62000))
 		sd_khz /= 2;
 
 	sd->mdcnfg = MDCNFG & 0x007f007f;
diff --git a/drivers/cpufreq/scpi-cpufreq.c b/drivers/cpufreq/scpi-cpufreq.c
index e8a7bf57b31b..ea7a4e1b68c2 100644
--- a/drivers/cpufreq/scpi-cpufreq.c
+++ b/drivers/cpufreq/scpi-cpufreq.c
@@ -105,7 +105,6 @@ static int scpi_cpufreq_remove(struct platform_device *pdev)
 static struct platform_driver scpi_cpufreq_platdrv = {
 	.driver = {
 		.name	= "scpi-cpufreq",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= scpi_cpufreq_probe,
 	.remove		= scpi_cpufreq_remove,
diff --git a/drivers/cpufreq/sti-cpufreq.c b/drivers/cpufreq/sti-cpufreq.c
index 04042038ec4b..b366e6d830ea 100644
--- a/drivers/cpufreq/sti-cpufreq.c
+++ b/drivers/cpufreq/sti-cpufreq.c
@@ -163,7 +163,7 @@ static int sti_cpufreq_set_opp_info(void)
 
 	reg_fields = sti_cpufreq_match();
 	if (!reg_fields) {
-		dev_err(dev, "This SoC doesn't support voltage scaling");
+		dev_err(dev, "This SoC doesn't support voltage scaling\n");
 		return -ENODEV;
 	}
 
diff --git a/drivers/cpuidle/Kconfig.mips b/drivers/cpuidle/Kconfig.mips
index 4102be01d06a..512ee37b374b 100644
--- a/drivers/cpuidle/Kconfig.mips
+++ b/drivers/cpuidle/Kconfig.mips
@@ -5,7 +5,7 @@ config MIPS_CPS_CPUIDLE
 	bool "CPU Idle driver for MIPS CPS platforms"
 	depends on CPU_IDLE && MIPS_CPS
 	depends on SYS_SUPPORTS_MIPS_CPS
-	select ARCH_NEEDS_CPU_IDLE_COUPLED if MIPS_MT
+	select ARCH_NEEDS_CPU_IDLE_COUPLED if MIPS_MT || CPU_MIPSR6
 	select GENERIC_CLOCKEVENTS_BROADCAST if SMP
 	select MIPS_CPS_PM
 	default y
diff --git a/drivers/cpuidle/coupled.c b/drivers/cpuidle/coupled.c
index d5657d50ac40..71e586d7df71 100644
--- a/drivers/cpuidle/coupled.c
+++ b/drivers/cpuidle/coupled.c
@@ -749,65 +749,52 @@ static void cpuidle_coupled_allow_idle(struct cpuidle_coupled *coupled)
 	put_cpu();
 }
 
-/**
- * cpuidle_coupled_cpu_notify - notifier called during hotplug transitions
- * @nb: notifier block
- * @action: hotplug transition
- * @hcpu: target cpu number
- *
- * Called when a cpu is brought on or offline using hotplug.  Updates the
- * coupled cpu set appropriately
- */
-static int cpuidle_coupled_cpu_notify(struct notifier_block *nb,
-		unsigned long action, void *hcpu)
+static int coupled_cpu_online(unsigned int cpu)
 {
-	int cpu = (unsigned long)hcpu;
 	struct cpuidle_device *dev;
 
-	switch (action & ~CPU_TASKS_FROZEN) {
-	case CPU_UP_PREPARE:
-	case CPU_DOWN_PREPARE:
-	case CPU_ONLINE:
-	case CPU_DEAD:
-	case CPU_UP_CANCELED:
-	case CPU_DOWN_FAILED:
-		break;
-	default:
-		return NOTIFY_OK;
-	}
-
 	mutex_lock(&cpuidle_lock);
 
 	dev = per_cpu(cpuidle_devices, cpu);
-	if (!dev || !dev->coupled)
-		goto out;
-
-	switch (action & ~CPU_TASKS_FROZEN) {
-	case CPU_UP_PREPARE:
-	case CPU_DOWN_PREPARE:
-		cpuidle_coupled_prevent_idle(dev->coupled);
-		break;
-	case CPU_ONLINE:
-	case CPU_DEAD:
+	if (dev && dev->coupled) {
 		cpuidle_coupled_update_online_cpus(dev->coupled);
-		/* Fall through */
-	case CPU_UP_CANCELED:
-	case CPU_DOWN_FAILED:
 		cpuidle_coupled_allow_idle(dev->coupled);
-		break;
 	}
 
-out:
 	mutex_unlock(&cpuidle_lock);
-	return NOTIFY_OK;
+	return 0;
 }
 
-static struct notifier_block cpuidle_coupled_cpu_notifier = {
-	.notifier_call = cpuidle_coupled_cpu_notify,
-};
+static int coupled_cpu_up_prepare(unsigned int cpu)
+{
+	struct cpuidle_device *dev;
+
+	mutex_lock(&cpuidle_lock);
+
+	dev = per_cpu(cpuidle_devices, cpu);
+	if (dev && dev->coupled)
+		cpuidle_coupled_prevent_idle(dev->coupled);
+
+	mutex_unlock(&cpuidle_lock);
+	return 0;
+}
 
 static int __init cpuidle_coupled_init(void)
 {
-	return register_cpu_notifier(&cpuidle_coupled_cpu_notifier);
+	int ret;
+
+	ret = cpuhp_setup_state_nocalls(CPUHP_CPUIDLE_COUPLED_PREPARE,
+					"cpuidle/coupled:prepare",
+					coupled_cpu_up_prepare,
+					coupled_cpu_online);
+	if (ret)
+		return ret;
+	ret = cpuhp_setup_state_nocalls(CPUHP_AP_ONLINE_DYN,
+					"cpuidle/coupled:online",
+					coupled_cpu_online,
+					coupled_cpu_up_prepare);
+	if (ret < 0)
+		cpuhp_remove_state_nocalls(CPUHP_CPUIDLE_COUPLED_PREPARE);
+	return ret;
 }
 core_initcall(cpuidle_coupled_init);
diff --git a/drivers/cpuidle/cpuidle-arm.c b/drivers/cpuidle/cpuidle-arm.c
index 4ba3d3fe142f..f440d385ed34 100644
--- a/drivers/cpuidle/cpuidle-arm.c
+++ b/drivers/cpuidle/cpuidle-arm.c
@@ -121,6 +121,7 @@ static int __init arm_idle_init(void)
 		dev = kzalloc(sizeof(*dev), GFP_KERNEL);
 		if (!dev) {
 			pr_err("Failed to allocate cpuidle device\n");
+			ret = -ENOMEM;
 			goto out_fail;
 		}
 		dev->cpu = cpu;
diff --git a/drivers/cpuidle/cpuidle-cps.c b/drivers/cpuidle/cpuidle-cps.c
index 1adb6980b707..926ba9871c62 100644
--- a/drivers/cpuidle/cpuidle-cps.c
+++ b/drivers/cpuidle/cpuidle-cps.c
@@ -163,7 +163,7 @@ static int __init cps_cpuidle_init(void)
 		core = cpu_data[cpu].core;
 		device = &per_cpu(cpuidle_dev, cpu);
 		device->cpu = cpu;
-#ifdef CONFIG_MIPS_MT
+#ifdef CONFIG_ARCH_NEEDS_CPU_IDLE_COUPLED
 		cpumask_copy(&device->coupled_cpus, &cpu_sibling_map[cpu]);
 #endif
 
diff --git a/drivers/cpuidle/cpuidle-powernv.c b/drivers/cpuidle/cpuidle-powernv.c
index f7ca891b5b59..7fe442ca38f4 100644
--- a/drivers/cpuidle/cpuidle-powernv.c
+++ b/drivers/cpuidle/cpuidle-powernv.c
@@ -119,40 +119,30 @@ static struct cpuidle_state powernv_states[CPUIDLE_STATE_MAX] = {
 		.enter = snooze_loop },
 };
 
-static int powernv_cpuidle_add_cpu_notifier(struct notifier_block *n,
-			unsigned long action, void *hcpu)
+static int powernv_cpuidle_cpu_online(unsigned int cpu)
 {
-	int hotcpu = (unsigned long)hcpu;
-	struct cpuidle_device *dev =
-				per_cpu(cpuidle_devices, hotcpu);
+	struct cpuidle_device *dev = per_cpu(cpuidle_devices, cpu);
 
 	if (dev && cpuidle_get_driver()) {
-		switch (action) {
-		case CPU_ONLINE:
-		case CPU_ONLINE_FROZEN:
-			cpuidle_pause_and_lock();
-			cpuidle_enable_device(dev);
-			cpuidle_resume_and_unlock();
-			break;
+		cpuidle_pause_and_lock();
+		cpuidle_enable_device(dev);
+		cpuidle_resume_and_unlock();
+	}
+	return 0;
+}
 
-		case CPU_DEAD:
-		case CPU_DEAD_FROZEN:
-			cpuidle_pause_and_lock();
-			cpuidle_disable_device(dev);
-			cpuidle_resume_and_unlock();
-			break;
+static int powernv_cpuidle_cpu_dead(unsigned int cpu)
+{
+	struct cpuidle_device *dev = per_cpu(cpuidle_devices, cpu);
 
-		default:
-			return NOTIFY_DONE;
-		}
+	if (dev && cpuidle_get_driver()) {
+		cpuidle_pause_and_lock();
+		cpuidle_disable_device(dev);
+		cpuidle_resume_and_unlock();
 	}
-	return NOTIFY_OK;
+	return 0;
 }
 
-static struct notifier_block setup_hotplug_notifier = {
-	.notifier_call = powernv_cpuidle_add_cpu_notifier,
-};
-
 /*
  * powernv_cpuidle_driver_init()
  */
@@ -355,7 +345,14 @@ static int __init powernv_processor_idle_init(void)
 		return retval;
 	}
 
-	register_cpu_notifier(&setup_hotplug_notifier);
+	retval = cpuhp_setup_state_nocalls(CPUHP_AP_ONLINE_DYN,
+					   "cpuidle/powernv:online",
+					   powernv_cpuidle_cpu_online, NULL);
+	WARN_ON(retval < 0);
+	retval = cpuhp_setup_state_nocalls(CPUHP_CPUIDLE_DEAD,
+					   "cpuidle/powernv:dead", NULL,
+					   powernv_cpuidle_cpu_dead);
+	WARN_ON(retval < 0);
 	printk(KERN_DEBUG "powernv_idle_driver registered\n");
 	return 0;
 }
diff --git a/drivers/cpuidle/cpuidle-pseries.c b/drivers/cpuidle/cpuidle-pseries.c
index 07135e009d8b..166ccd711ec9 100644
--- a/drivers/cpuidle/cpuidle-pseries.c
+++ b/drivers/cpuidle/cpuidle-pseries.c
@@ -171,40 +171,30 @@ static struct cpuidle_state shared_states[] = {
 		.enter = &shared_cede_loop },
 };
 
-static int pseries_cpuidle_add_cpu_notifier(struct notifier_block *n,
-			unsigned long action, void *hcpu)
+static int pseries_cpuidle_cpu_online(unsigned int cpu)
 {
-	int hotcpu = (unsigned long)hcpu;
-	struct cpuidle_device *dev =
-				per_cpu(cpuidle_devices, hotcpu);
+	struct cpuidle_device *dev = per_cpu(cpuidle_devices, cpu);
 
 	if (dev && cpuidle_get_driver()) {
-		switch (action) {
-		case CPU_ONLINE:
-		case CPU_ONLINE_FROZEN:
-			cpuidle_pause_and_lock();
-			cpuidle_enable_device(dev);
-			cpuidle_resume_and_unlock();
-			break;
+		cpuidle_pause_and_lock();
+		cpuidle_enable_device(dev);
+		cpuidle_resume_and_unlock();
+	}
+	return 0;
+}
 
-		case CPU_DEAD:
-		case CPU_DEAD_FROZEN:
-			cpuidle_pause_and_lock();
-			cpuidle_disable_device(dev);
-			cpuidle_resume_and_unlock();
-			break;
+static int pseries_cpuidle_cpu_dead(unsigned int cpu)
+{
+	struct cpuidle_device *dev = per_cpu(cpuidle_devices, cpu);
 
-		default:
-			return NOTIFY_DONE;
-		}
+	if (dev && cpuidle_get_driver()) {
+		cpuidle_pause_and_lock();
+		cpuidle_disable_device(dev);
+		cpuidle_resume_and_unlock();
 	}
-	return NOTIFY_OK;
+	return 0;
 }
 
-static struct notifier_block setup_hotplug_notifier = {
-	.notifier_call = pseries_cpuidle_add_cpu_notifier,
-};
-
 /*
  * pseries_cpuidle_driver_init()
  */
@@ -273,7 +263,14 @@ static int __init pseries_processor_idle_init(void)
 		return retval;
 	}
 
-	register_cpu_notifier(&setup_hotplug_notifier);
+	retval = cpuhp_setup_state_nocalls(CPUHP_AP_ONLINE_DYN,
+					   "cpuidle/pseries:online",
+					   pseries_cpuidle_cpu_online, NULL);
+	WARN_ON(retval < 0);
+	retval = cpuhp_setup_state_nocalls(CPUHP_CPUIDLE_DEAD,
+					   "cpuidle/pseries:DEAD", NULL,
+					   pseries_cpuidle_cpu_dead);
+	WARN_ON(retval < 0);
 	printk(KERN_DEBUG "pseries_idle_driver registered\n");
 	return 0;
 }
diff --git a/drivers/cpuidle/driver.c b/drivers/cpuidle/driver.c
index 389ade4572be..ab264d393233 100644
--- a/drivers/cpuidle/driver.c
+++ b/drivers/cpuidle/driver.c
@@ -14,6 +14,7 @@
 #include <linux/cpuidle.h>
 #include <linux/cpumask.h>
 #include <linux/tick.h>
+#include <linux/cpu.h>
 
 #include "cpuidle.h"
 
@@ -178,8 +179,8 @@ static void __cpuidle_driver_init(struct cpuidle_driver *drv)
 }
 
 #ifdef CONFIG_ARCH_HAS_CPU_RELAX
-static int poll_idle(struct cpuidle_device *dev,
-		struct cpuidle_driver *drv, int index)
+static int __cpuidle poll_idle(struct cpuidle_device *dev,
+			       struct cpuidle_driver *drv, int index)
 {
 	local_irq_enable();
 	if (!current_set_polling_and_test()) {
diff --git a/drivers/crypto/Kconfig b/drivers/crypto/Kconfig
index 1af94e2d1a25..4d2b81f2b223 100644
--- a/drivers/crypto/Kconfig
+++ b/drivers/crypto/Kconfig
@@ -318,6 +318,9 @@ config CRYPTO_DEV_OMAP_AES
 	select CRYPTO_AES
 	select CRYPTO_BLKCIPHER
 	select CRYPTO_ENGINE
+	select CRYPTO_CBC
+	select CRYPTO_ECB
+	select CRYPTO_CTR
 	help
 	  OMAP processors have AES module accelerator. Select this if you
 	  want to use the OMAP module for AES algorithms.
@@ -550,4 +553,6 @@ config CRYPTO_DEV_ROCKCHIP
 	  This driver interfaces with the hardware crypto accelerator.
 	  Supporting cbc/ecb chainmode, and aes/des/des3_ede cipher mode.
 
+source "drivers/crypto/chelsio/Kconfig"
+
 endif # CRYPTO_HW
diff --git a/drivers/crypto/Makefile b/drivers/crypto/Makefile
index 3c6432dd09d9..ad7250fa1348 100644
--- a/drivers/crypto/Makefile
+++ b/drivers/crypto/Makefile
@@ -31,3 +31,4 @@ obj-$(CONFIG_CRYPTO_DEV_QCE) += qce/
 obj-$(CONFIG_CRYPTO_DEV_VMX) += vmx/
 obj-$(CONFIG_CRYPTO_DEV_SUN4I_SS) += sunxi-ss/
 obj-$(CONFIG_CRYPTO_DEV_ROCKCHIP) += rockchip/
+obj-$(CONFIG_CRYPTO_DEV_CHELSIO) += chelsio/
diff --git a/drivers/crypto/caam/caamalg.c b/drivers/crypto/caam/caamalg.c
index b3044219772c..156aad167cd6 100644
--- a/drivers/crypto/caam/caamalg.c
+++ b/drivers/crypto/caam/caamalg.c
@@ -111,6 +111,42 @@
 #else
 #define debug(format, arg...)
 #endif
+
+#ifdef DEBUG
+#include <linux/highmem.h>
+
+static void dbg_dump_sg(const char *level, const char *prefix_str,
+			int prefix_type, int rowsize, int groupsize,
+			struct scatterlist *sg, size_t tlen, bool ascii,
+			bool may_sleep)
+{
+	struct scatterlist *it;
+	void *it_page;
+	size_t len;
+	void *buf;
+
+	for (it = sg; it != NULL && tlen > 0 ; it = sg_next(sg)) {
+		/*
+		 * make sure the scatterlist's page
+		 * has a valid virtual memory mapping
+		 */
+		it_page = kmap_atomic(sg_page(it));
+		if (unlikely(!it_page)) {
+			printk(KERN_ERR "dbg_dump_sg: kmap failed\n");
+			return;
+		}
+
+		buf = it_page + it->offset;
+		len = min(tlen, it->length);
+		print_hex_dump(level, prefix_str, prefix_type, rowsize,
+			       groupsize, buf, len, ascii);
+		tlen -= len;
+
+		kunmap_atomic(it_page);
+	}
+}
+#endif
+
 static struct list_head alg_list;
 
 struct caam_alg_entry {
@@ -227,8 +263,9 @@ static void append_key_aead(u32 *desc, struct caam_ctx *ctx,
 	if (is_rfc3686) {
 		nonce = (u32 *)((void *)ctx->key + ctx->split_key_pad_len +
 			       enckeylen);
-		append_load_imm_u32(desc, *nonce, LDST_CLASS_IND_CCB |
-				    LDST_SRCDST_BYTE_OUTFIFO | LDST_IMM);
+		append_load_as_imm(desc, nonce, CTR_RFC3686_NONCE_SIZE,
+				   LDST_CLASS_IND_CCB |
+				   LDST_SRCDST_BYTE_OUTFIFO | LDST_IMM);
 		append_move(desc,
 			    MOVE_SRC_OUTFIFO |
 			    MOVE_DEST_CLASS1CTX |
@@ -500,11 +537,10 @@ static int aead_set_sh_desc(struct crypto_aead *aead)
 
 	/* Load Counter into CONTEXT1 reg */
 	if (is_rfc3686)
-		append_load_imm_u32(desc, be32_to_cpu(1), LDST_IMM |
-				    LDST_CLASS_1_CCB |
-				    LDST_SRCDST_BYTE_CONTEXT |
-				    ((ctx1_iv_off + CTR_RFC3686_IV_SIZE) <<
-				     LDST_OFFSET_SHIFT));
+		append_load_imm_be32(desc, 1, LDST_IMM | LDST_CLASS_1_CCB |
+				     LDST_SRCDST_BYTE_CONTEXT |
+				     ((ctx1_iv_off + CTR_RFC3686_IV_SIZE) <<
+				      LDST_OFFSET_SHIFT));
 
 	/* Class 1 operation */
 	append_operation(desc, ctx->class1_alg_type |
@@ -578,11 +614,10 @@ skip_enc:
 
 	/* Load Counter into CONTEXT1 reg */
 	if (is_rfc3686)
-		append_load_imm_u32(desc, be32_to_cpu(1), LDST_IMM |
-				    LDST_CLASS_1_CCB |
-				    LDST_SRCDST_BYTE_CONTEXT |
-				    ((ctx1_iv_off + CTR_RFC3686_IV_SIZE) <<
-				     LDST_OFFSET_SHIFT));
+		append_load_imm_be32(desc, 1, LDST_IMM | LDST_CLASS_1_CCB |
+				     LDST_SRCDST_BYTE_CONTEXT |
+				     ((ctx1_iv_off + CTR_RFC3686_IV_SIZE) <<
+				      LDST_OFFSET_SHIFT));
 
 	/* Choose operation */
 	if (ctr_mode)
@@ -683,11 +718,10 @@ copy_iv:
 
 	/* Load Counter into CONTEXT1 reg */
 	if (is_rfc3686)
-		append_load_imm_u32(desc, be32_to_cpu(1), LDST_IMM |
-				    LDST_CLASS_1_CCB |
-				    LDST_SRCDST_BYTE_CONTEXT |
-				    ((ctx1_iv_off + CTR_RFC3686_IV_SIZE) <<
-				     LDST_OFFSET_SHIFT));
+		append_load_imm_be32(desc, 1, LDST_IMM | LDST_CLASS_1_CCB |
+				     LDST_SRCDST_BYTE_CONTEXT |
+				     ((ctx1_iv_off + CTR_RFC3686_IV_SIZE) <<
+				      LDST_OFFSET_SHIFT));
 
 	/* Class 1 operation */
 	append_operation(desc, ctx->class1_alg_type |
@@ -1478,7 +1512,7 @@ static int ablkcipher_setkey(struct crypto_ablkcipher *ablkcipher,
 	int ret = 0;
 	u32 *key_jump_cmd;
 	u32 *desc;
-	u32 *nonce;
+	u8 *nonce;
 	u32 geniv;
 	u32 ctx1_iv_off = 0;
 	const bool ctr_mode = ((ctx->class1_alg_type & OP_ALG_AAI_MASK) ==
@@ -1531,9 +1565,10 @@ static int ablkcipher_setkey(struct crypto_ablkcipher *ablkcipher,
 
 	/* Load nonce into CONTEXT1 reg */
 	if (is_rfc3686) {
-		nonce = (u32 *)(key + keylen);
-		append_load_imm_u32(desc, *nonce, LDST_CLASS_IND_CCB |
-				    LDST_SRCDST_BYTE_OUTFIFO | LDST_IMM);
+		nonce = (u8 *)key + keylen;
+		append_load_as_imm(desc, nonce, CTR_RFC3686_NONCE_SIZE,
+				   LDST_CLASS_IND_CCB |
+				   LDST_SRCDST_BYTE_OUTFIFO | LDST_IMM);
 		append_move(desc, MOVE_WAITCOMP |
 			    MOVE_SRC_OUTFIFO |
 			    MOVE_DEST_CLASS1CTX |
@@ -1549,11 +1584,10 @@ static int ablkcipher_setkey(struct crypto_ablkcipher *ablkcipher,
 
 	/* Load counter into CONTEXT1 reg */
 	if (is_rfc3686)
-		append_load_imm_u32(desc, be32_to_cpu(1), LDST_IMM |
-				    LDST_CLASS_1_CCB |
-				    LDST_SRCDST_BYTE_CONTEXT |
-				    ((ctx1_iv_off + CTR_RFC3686_IV_SIZE) <<
-				     LDST_OFFSET_SHIFT));
+		append_load_imm_be32(desc, 1, LDST_IMM | LDST_CLASS_1_CCB |
+				     LDST_SRCDST_BYTE_CONTEXT |
+				     ((ctx1_iv_off + CTR_RFC3686_IV_SIZE) <<
+				      LDST_OFFSET_SHIFT));
 
 	/* Load operation */
 	append_operation(desc, ctx->class1_alg_type |
@@ -1590,9 +1624,10 @@ static int ablkcipher_setkey(struct crypto_ablkcipher *ablkcipher,
 
 	/* Load nonce into CONTEXT1 reg */
 	if (is_rfc3686) {
-		nonce = (u32 *)(key + keylen);
-		append_load_imm_u32(desc, *nonce, LDST_CLASS_IND_CCB |
-				    LDST_SRCDST_BYTE_OUTFIFO | LDST_IMM);
+		nonce = (u8 *)key + keylen;
+		append_load_as_imm(desc, nonce, CTR_RFC3686_NONCE_SIZE,
+				   LDST_CLASS_IND_CCB |
+				   LDST_SRCDST_BYTE_OUTFIFO | LDST_IMM);
 		append_move(desc, MOVE_WAITCOMP |
 			    MOVE_SRC_OUTFIFO |
 			    MOVE_DEST_CLASS1CTX |
@@ -1608,11 +1643,10 @@ static int ablkcipher_setkey(struct crypto_ablkcipher *ablkcipher,
 
 	/* Load counter into CONTEXT1 reg */
 	if (is_rfc3686)
-		append_load_imm_u32(desc, be32_to_cpu(1), LDST_IMM |
-				    LDST_CLASS_1_CCB |
-				    LDST_SRCDST_BYTE_CONTEXT |
-				    ((ctx1_iv_off + CTR_RFC3686_IV_SIZE) <<
-				     LDST_OFFSET_SHIFT));
+		append_load_imm_be32(desc, 1, LDST_IMM | LDST_CLASS_1_CCB |
+				     LDST_SRCDST_BYTE_CONTEXT |
+				     ((ctx1_iv_off + CTR_RFC3686_IV_SIZE) <<
+				      LDST_OFFSET_SHIFT));
 
 	/* Choose operation */
 	if (ctr_mode)
@@ -1653,9 +1687,10 @@ static int ablkcipher_setkey(struct crypto_ablkcipher *ablkcipher,
 
 	/* Load Nonce into CONTEXT1 reg */
 	if (is_rfc3686) {
-		nonce = (u32 *)(key + keylen);
-		append_load_imm_u32(desc, *nonce, LDST_CLASS_IND_CCB |
-				    LDST_SRCDST_BYTE_OUTFIFO | LDST_IMM);
+		nonce = (u8 *)key + keylen;
+		append_load_as_imm(desc, nonce, CTR_RFC3686_NONCE_SIZE,
+				   LDST_CLASS_IND_CCB |
+				   LDST_SRCDST_BYTE_OUTFIFO | LDST_IMM);
 		append_move(desc, MOVE_WAITCOMP |
 			    MOVE_SRC_OUTFIFO |
 			    MOVE_DEST_CLASS1CTX |
@@ -1685,11 +1720,10 @@ static int ablkcipher_setkey(struct crypto_ablkcipher *ablkcipher,
 
 	/* Load Counter into CONTEXT1 reg */
 	if (is_rfc3686)
-		append_load_imm_u32(desc, (u32)1, LDST_IMM |
-				    LDST_CLASS_1_CCB |
-				    LDST_SRCDST_BYTE_CONTEXT |
-				    ((ctx1_iv_off + CTR_RFC3686_IV_SIZE) <<
-				     LDST_OFFSET_SHIFT));
+		append_load_imm_be32(desc, 1, LDST_IMM | LDST_CLASS_1_CCB |
+				     LDST_SRCDST_BYTE_CONTEXT |
+				     ((ctx1_iv_off + CTR_RFC3686_IV_SIZE) <<
+				      LDST_OFFSET_SHIFT));
 
 	if (ctx1_iv_off)
 		append_jump(desc, JUMP_JSL | JUMP_TEST_ALL | JUMP_COND_NCP |
@@ -1995,9 +2029,9 @@ static void ablkcipher_encrypt_done(struct device *jrdev, u32 *desc, u32 err,
 	print_hex_dump(KERN_ERR, "dstiv  @"__stringify(__LINE__)": ",
 		       DUMP_PREFIX_ADDRESS, 16, 4, req->info,
 		       edesc->src_nents > 1 ? 100 : ivsize, 1);
-	print_hex_dump(KERN_ERR, "dst    @"__stringify(__LINE__)": ",
-		       DUMP_PREFIX_ADDRESS, 16, 4, sg_virt(req->src),
-		       edesc->dst_nents > 1 ? 100 : req->nbytes, 1);
+	dbg_dump_sg(KERN_ERR, "dst    @"__stringify(__LINE__)": ",
+		    DUMP_PREFIX_ADDRESS, 16, 4, req->dst,
+		    edesc->dst_nents > 1 ? 100 : req->nbytes, 1, true);
 #endif
 
 	ablkcipher_unmap(jrdev, edesc, req);
@@ -2027,9 +2061,9 @@ static void ablkcipher_decrypt_done(struct device *jrdev, u32 *desc, u32 err,
 	print_hex_dump(KERN_ERR, "dstiv  @"__stringify(__LINE__)": ",
 		       DUMP_PREFIX_ADDRESS, 16, 4, req->info,
 		       ivsize, 1);
-	print_hex_dump(KERN_ERR, "dst    @"__stringify(__LINE__)": ",
-		       DUMP_PREFIX_ADDRESS, 16, 4, sg_virt(req->src),
-		       edesc->dst_nents > 1 ? 100 : req->nbytes, 1);
+	dbg_dump_sg(KERN_ERR, "dst    @"__stringify(__LINE__)": ",
+		    DUMP_PREFIX_ADDRESS, 16, 4, req->dst,
+		    edesc->dst_nents > 1 ? 100 : req->nbytes, 1, true);
 #endif
 
 	ablkcipher_unmap(jrdev, edesc, req);
@@ -2184,12 +2218,15 @@ static void init_ablkcipher_job(u32 *sh_desc, dma_addr_t ptr,
 	int len, sec4_sg_index = 0;
 
 #ifdef DEBUG
+	bool may_sleep = ((req->base.flags & (CRYPTO_TFM_REQ_MAY_BACKLOG |
+					      CRYPTO_TFM_REQ_MAY_SLEEP)) != 0);
 	print_hex_dump(KERN_ERR, "presciv@"__stringify(__LINE__)": ",
 		       DUMP_PREFIX_ADDRESS, 16, 4, req->info,
 		       ivsize, 1);
-	print_hex_dump(KERN_ERR, "src    @"__stringify(__LINE__)": ",
-		       DUMP_PREFIX_ADDRESS, 16, 4, sg_virt(req->src),
-		       edesc->src_nents ? 100 : req->nbytes, 1);
+	printk(KERN_ERR "asked=%d, nbytes%d\n", (int)edesc->src_nents ? 100 : req->nbytes, req->nbytes);
+	dbg_dump_sg(KERN_ERR, "src    @"__stringify(__LINE__)": ",
+		    DUMP_PREFIX_ADDRESS, 16, 4, req->src,
+		    edesc->src_nents ? 100 : req->nbytes, 1, may_sleep);
 #endif
 
 	len = desc_len(sh_desc);
@@ -2241,12 +2278,14 @@ static void init_ablkcipher_giv_job(u32 *sh_desc, dma_addr_t ptr,
 	int len, sec4_sg_index = 0;
 
 #ifdef DEBUG
+	bool may_sleep = ((req->base.flags & (CRYPTO_TFM_REQ_MAY_BACKLOG |
+					      CRYPTO_TFM_REQ_MAY_SLEEP)) != 0);
 	print_hex_dump(KERN_ERR, "presciv@" __stringify(__LINE__) ": ",
 		       DUMP_PREFIX_ADDRESS, 16, 4, req->info,
 		       ivsize, 1);
-	print_hex_dump(KERN_ERR, "src    @" __stringify(__LINE__) ": ",
-		       DUMP_PREFIX_ADDRESS, 16, 4, sg_virt(req->src),
-		       edesc->src_nents ? 100 : req->nbytes, 1);
+	dbg_dump_sg(KERN_ERR, "src    @" __stringify(__LINE__) ": ",
+		    DUMP_PREFIX_ADDRESS, 16, 4, req->src,
+		    edesc->src_nents ? 100 : req->nbytes, 1, may_sleep);
 #endif
 
 	len = desc_len(sh_desc);
@@ -2516,18 +2555,20 @@ static int aead_decrypt(struct aead_request *req)
 	u32 *desc;
 	int ret = 0;
 
+#ifdef DEBUG
+	bool may_sleep = ((req->base.flags & (CRYPTO_TFM_REQ_MAY_BACKLOG |
+					      CRYPTO_TFM_REQ_MAY_SLEEP)) != 0);
+	dbg_dump_sg(KERN_ERR, "dec src@"__stringify(__LINE__)": ",
+		    DUMP_PREFIX_ADDRESS, 16, 4, req->src,
+		    req->assoclen + req->cryptlen, 1, may_sleep);
+#endif
+
 	/* allocate extended descriptor */
 	edesc = aead_edesc_alloc(req, AUTHENC_DESC_JOB_IO_LEN,
 				 &all_contig, false);
 	if (IS_ERR(edesc))
 		return PTR_ERR(edesc);
 
-#ifdef DEBUG
-	print_hex_dump(KERN_ERR, "dec src@"__stringify(__LINE__)": ",
-		       DUMP_PREFIX_ADDRESS, 16, 4, sg_virt(req->src),
-		       req->assoclen + req->cryptlen, 1);
-#endif
-
 	/* Create and submit job descriptor*/
 	init_authenc_job(req, edesc, all_contig, false);
 #ifdef DEBUG
diff --git a/drivers/crypto/caam/caamhash.c b/drivers/crypto/caam/caamhash.c
index 36365b3efdfd..660dc206969f 100644
--- a/drivers/crypto/caam/caamhash.c
+++ b/drivers/crypto/caam/caamhash.c
@@ -99,17 +99,17 @@ static struct list_head hash_list;
 
 /* ahash per-session context */
 struct caam_hash_ctx {
-	struct device *jrdev;
-	u32 sh_desc_update[DESC_HASH_MAX_USED_LEN];
-	u32 sh_desc_update_first[DESC_HASH_MAX_USED_LEN];
-	u32 sh_desc_fin[DESC_HASH_MAX_USED_LEN];
-	u32 sh_desc_digest[DESC_HASH_MAX_USED_LEN];
-	u32 sh_desc_finup[DESC_HASH_MAX_USED_LEN];
-	dma_addr_t sh_desc_update_dma;
+	u32 sh_desc_update[DESC_HASH_MAX_USED_LEN] ____cacheline_aligned;
+	u32 sh_desc_update_first[DESC_HASH_MAX_USED_LEN] ____cacheline_aligned;
+	u32 sh_desc_fin[DESC_HASH_MAX_USED_LEN] ____cacheline_aligned;
+	u32 sh_desc_digest[DESC_HASH_MAX_USED_LEN] ____cacheline_aligned;
+	u32 sh_desc_finup[DESC_HASH_MAX_USED_LEN] ____cacheline_aligned;
+	dma_addr_t sh_desc_update_dma ____cacheline_aligned;
 	dma_addr_t sh_desc_update_first_dma;
 	dma_addr_t sh_desc_fin_dma;
 	dma_addr_t sh_desc_digest_dma;
 	dma_addr_t sh_desc_finup_dma;
+	struct device *jrdev;
 	u32 alg_type;
 	u32 alg_op;
 	u8 key[CAAM_MAX_HASH_KEY_SIZE];
@@ -187,15 +187,6 @@ static inline dma_addr_t buf_map_to_sec4_sg(struct device *jrdev,
 	return buf_dma;
 }
 
-/* Map req->src and put it in link table */
-static inline void src_map_to_sec4_sg(struct device *jrdev,
-				      struct scatterlist *src, int src_nents,
-				      struct sec4_sg_entry *sec4_sg)
-{
-	dma_map_sg(jrdev, src, src_nents, DMA_TO_DEVICE);
-	sg_to_sec4_sg_last(src, src_nents, sec4_sg, 0);
-}
-
 /*
  * Only put buffer in link table if it contains data, which is possible,
  * since a buffer has previously been used, and needs to be unmapped,
@@ -449,7 +440,7 @@ static int hash_digest_key(struct caam_hash_ctx *ctx, const u8 *key_in,
 	u32 *desc;
 	struct split_key_result result;
 	dma_addr_t src_dma, dst_dma;
-	int ret = 0;
+	int ret;
 
 	desc = kmalloc(CAAM_CMD_SZ * 8 + CAAM_PTR_SZ * 2, GFP_KERNEL | GFP_DMA);
 	if (!desc) {
@@ -526,7 +517,7 @@ static int ahash_setkey(struct crypto_ahash *ahash,
 	struct device *jrdev = ctx->jrdev;
 	int blocksize = crypto_tfm_alg_blocksize(&ahash->base);
 	int digestsize = crypto_ahash_digestsize(ahash);
-	int ret = 0;
+	int ret;
 	u8 *hashed_key = NULL;
 
 #ifdef DEBUG
@@ -534,14 +525,15 @@ static int ahash_setkey(struct crypto_ahash *ahash,
 #endif
 
 	if (keylen > blocksize) {
-		hashed_key = kmalloc(sizeof(u8) * digestsize, GFP_KERNEL |
-				     GFP_DMA);
+		hashed_key = kmalloc_array(digestsize,
+					   sizeof(*hashed_key),
+					   GFP_KERNEL | GFP_DMA);
 		if (!hashed_key)
 			return -ENOMEM;
 		ret = hash_digest_key(ctx, key, &keylen, hashed_key,
 				      digestsize);
 		if (ret)
-			goto badkey;
+			goto bad_free_key;
 		key = hashed_key;
 	}
 
@@ -559,14 +551,14 @@ static int ahash_setkey(struct crypto_ahash *ahash,
 
 	ret = gen_split_hash_key(ctx, key, keylen);
 	if (ret)
-		goto badkey;
+		goto bad_free_key;
 
 	ctx->key_dma = dma_map_single(jrdev, ctx->key, ctx->split_key_pad_len,
 				      DMA_TO_DEVICE);
 	if (dma_mapping_error(jrdev, ctx->key_dma)) {
 		dev_err(jrdev, "unable to map key i/o memory\n");
 		ret = -ENOMEM;
-		goto map_err;
+		goto error_free_key;
 	}
 #ifdef DEBUG
 	print_hex_dump(KERN_ERR, "ctx.key@"__stringify(__LINE__)": ",
@@ -579,11 +571,10 @@ static int ahash_setkey(struct crypto_ahash *ahash,
 		dma_unmap_single(jrdev, ctx->key_dma, ctx->split_key_pad_len,
 				 DMA_TO_DEVICE);
 	}
-
-map_err:
+ error_free_key:
 	kfree(hashed_key);
 	return ret;
-badkey:
+ bad_free_key:
 	kfree(hashed_key);
 	crypto_ahash_set_flags(ahash, CRYPTO_TFM_RES_BAD_KEY_LEN);
 	return -EINVAL;
@@ -595,16 +586,16 @@ badkey:
  * @sec4_sg_dma: physical mapped address of h/w link table
  * @src_nents: number of segments in input scatterlist
  * @sec4_sg_bytes: length of dma mapped sec4_sg space
- * @sec4_sg: pointer to h/w link table
  * @hw_desc: the h/w job descriptor followed by any referenced link tables
+ * @sec4_sg: h/w link table
  */
 struct ahash_edesc {
 	dma_addr_t dst_dma;
 	dma_addr_t sec4_sg_dma;
 	int src_nents;
 	int sec4_sg_bytes;
-	struct sec4_sg_entry *sec4_sg;
-	u32 hw_desc[0];
+	u32 hw_desc[DESC_JOB_IO_LEN / sizeof(u32)] ____cacheline_aligned;
+	struct sec4_sg_entry sec4_sg[0];
 };
 
 static inline void ahash_unmap(struct device *dev,
@@ -774,6 +765,65 @@ static void ahash_done_ctx_dst(struct device *jrdev, u32 *desc, u32 err,
 	req->base.complete(&req->base, err);
 }
 
+/*
+ * Allocate an enhanced descriptor, which contains the hardware descriptor
+ * and space for hardware scatter table containing sg_num entries.
+ */
+static struct ahash_edesc *ahash_edesc_alloc(struct caam_hash_ctx *ctx,
+					     int sg_num, u32 *sh_desc,
+					     dma_addr_t sh_desc_dma,
+					     gfp_t flags)
+{
+	struct ahash_edesc *edesc;
+	unsigned int sg_size = sg_num * sizeof(struct sec4_sg_entry);
+
+	edesc = kzalloc(sizeof(*edesc) + sg_size, GFP_DMA | flags);
+	if (!edesc) {
+		dev_err(ctx->jrdev, "could not allocate extended descriptor\n");
+		return NULL;
+	}
+
+	init_job_desc_shared(edesc->hw_desc, sh_desc_dma, desc_len(sh_desc),
+			     HDR_SHARE_DEFER | HDR_REVERSE);
+
+	return edesc;
+}
+
+static int ahash_edesc_add_src(struct caam_hash_ctx *ctx,
+			       struct ahash_edesc *edesc,
+			       struct ahash_request *req, int nents,
+			       unsigned int first_sg,
+			       unsigned int first_bytes, size_t to_hash)
+{
+	dma_addr_t src_dma;
+	u32 options;
+
+	if (nents > 1 || first_sg) {
+		struct sec4_sg_entry *sg = edesc->sec4_sg;
+		unsigned int sgsize = sizeof(*sg) * (first_sg + nents);
+
+		sg_to_sec4_sg_last(req->src, nents, sg + first_sg, 0);
+
+		src_dma = dma_map_single(ctx->jrdev, sg, sgsize, DMA_TO_DEVICE);
+		if (dma_mapping_error(ctx->jrdev, src_dma)) {
+			dev_err(ctx->jrdev, "unable to map S/G table\n");
+			return -ENOMEM;
+		}
+
+		edesc->sec4_sg_bytes = sgsize;
+		edesc->sec4_sg_dma = src_dma;
+		options = LDST_SGF;
+	} else {
+		src_dma = sg_dma_address(req->src);
+		options = 0;
+	}
+
+	append_seq_in_ptr(edesc->hw_desc, src_dma, first_bytes + to_hash,
+			  options);
+
+	return 0;
+}
+
 /* submit update job descriptor */
 static int ahash_update_ctx(struct ahash_request *req)
 {
@@ -789,12 +839,10 @@ static int ahash_update_ctx(struct ahash_request *req)
 	int *next_buflen = state->current_buf ? &state->buflen_0 :
 			   &state->buflen_1, last_buflen;
 	int in_len = *buflen + req->nbytes, to_hash;
-	u32 *sh_desc = ctx->sh_desc_update, *desc;
-	dma_addr_t ptr = ctx->sh_desc_update_dma;
-	int src_nents, sec4_sg_bytes, sec4_sg_src_index;
+	u32 *desc;
+	int src_nents, mapped_nents, sec4_sg_bytes, sec4_sg_src_index;
 	struct ahash_edesc *edesc;
 	int ret = 0;
-	int sh_len;
 
 	last_buflen = *next_buflen;
 	*next_buflen = in_len & (crypto_tfm_alg_blocksize(&ahash->base) - 1);
@@ -807,40 +855,51 @@ static int ahash_update_ctx(struct ahash_request *req)
 			dev_err(jrdev, "Invalid number of src SG.\n");
 			return src_nents;
 		}
+
+		if (src_nents) {
+			mapped_nents = dma_map_sg(jrdev, req->src, src_nents,
+						  DMA_TO_DEVICE);
+			if (!mapped_nents) {
+				dev_err(jrdev, "unable to DMA map source\n");
+				return -ENOMEM;
+			}
+		} else {
+			mapped_nents = 0;
+		}
+
 		sec4_sg_src_index = 1 + (*buflen ? 1 : 0);
-		sec4_sg_bytes = (sec4_sg_src_index + src_nents) *
+		sec4_sg_bytes = (sec4_sg_src_index + mapped_nents) *
 				 sizeof(struct sec4_sg_entry);
 
 		/*
 		 * allocate space for base edesc and hw desc commands,
 		 * link tables
 		 */
-		edesc = kzalloc(sizeof(*edesc) + DESC_JOB_IO_LEN +
-				sec4_sg_bytes, GFP_DMA | flags);
+		edesc = ahash_edesc_alloc(ctx, sec4_sg_src_index + mapped_nents,
+					  ctx->sh_desc_update,
+					  ctx->sh_desc_update_dma, flags);
 		if (!edesc) {
-			dev_err(jrdev,
-				"could not allocate extended descriptor\n");
+			dma_unmap_sg(jrdev, req->src, src_nents, DMA_TO_DEVICE);
 			return -ENOMEM;
 		}
 
 		edesc->src_nents = src_nents;
 		edesc->sec4_sg_bytes = sec4_sg_bytes;
-		edesc->sec4_sg = (void *)edesc + sizeof(struct ahash_edesc) +
-				 DESC_JOB_IO_LEN;
 
 		ret = ctx_map_to_sec4_sg(desc, jrdev, state, ctx->ctx_len,
 					 edesc->sec4_sg, DMA_BIDIRECTIONAL);
 		if (ret)
-			return ret;
+			goto unmap_ctx;
 
 		state->buf_dma = try_buf_map_to_sec4_sg(jrdev,
 							edesc->sec4_sg + 1,
 							buf, state->buf_dma,
 							*buflen, last_buflen);
 
-		if (src_nents) {
-			src_map_to_sec4_sg(jrdev, req->src, src_nents,
-					   edesc->sec4_sg + sec4_sg_src_index);
+		if (mapped_nents) {
+			sg_to_sec4_sg_last(req->src, mapped_nents,
+					   edesc->sec4_sg + sec4_sg_src_index,
+					   0);
 			if (*next_buflen)
 				scatterwalk_map_and_copy(next_buf, req->src,
 							 to_hash - *buflen,
@@ -852,17 +911,15 @@ static int ahash_update_ctx(struct ahash_request *req)
 
 		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 |
-				     HDR_REVERSE);
 
 		edesc->sec4_sg_dma = dma_map_single(jrdev, edesc->sec4_sg,
 						     sec4_sg_bytes,
 						     DMA_TO_DEVICE);
 		if (dma_mapping_error(jrdev, edesc->sec4_sg_dma)) {
 			dev_err(jrdev, "unable to map S/G table\n");
-			return -ENOMEM;
+			ret = -ENOMEM;
+			goto unmap_ctx;
 		}
 
 		append_seq_in_ptr(desc, edesc->sec4_sg_dma, ctx->ctx_len +
@@ -877,13 +934,10 @@ static int ahash_update_ctx(struct ahash_request *req)
 #endif
 
 		ret = caam_jr_enqueue(jrdev, desc, ahash_done_bi, req);
-		if (!ret) {
-			ret = -EINPROGRESS;
-		} else {
-			ahash_unmap_ctx(jrdev, edesc, req, ctx->ctx_len,
-					   DMA_BIDIRECTIONAL);
-			kfree(edesc);
-		}
+		if (ret)
+			goto unmap_ctx;
+
+		ret = -EINPROGRESS;
 	} else if (*next_buflen) {
 		scatterwalk_map_and_copy(buf + *buflen, req->src, 0,
 					 req->nbytes, 0);
@@ -899,6 +953,10 @@ static int ahash_update_ctx(struct ahash_request *req)
 #endif
 
 	return ret;
+ unmap_ctx:
+	ahash_unmap_ctx(jrdev, edesc, req, ctx->ctx_len, DMA_BIDIRECTIONAL);
+	kfree(edesc);
+	return ret;
 }
 
 static int ahash_final_ctx(struct ahash_request *req)
@@ -913,38 +971,31 @@ static int ahash_final_ctx(struct ahash_request *req)
 	int buflen = state->current_buf ? state->buflen_1 : state->buflen_0;
 	int last_buflen = state->current_buf ? state->buflen_0 :
 			  state->buflen_1;
-	u32 *sh_desc = ctx->sh_desc_fin, *desc;
-	dma_addr_t ptr = ctx->sh_desc_fin_dma;
+	u32 *desc;
 	int sec4_sg_bytes, sec4_sg_src_index;
 	int digestsize = crypto_ahash_digestsize(ahash);
 	struct ahash_edesc *edesc;
-	int ret = 0;
-	int sh_len;
+	int ret;
 
 	sec4_sg_src_index = 1 + (buflen ? 1 : 0);
 	sec4_sg_bytes = sec4_sg_src_index * sizeof(struct sec4_sg_entry);
 
 	/* allocate space for base edesc and hw desc commands, link tables */
-	edesc = kzalloc(sizeof(*edesc) + DESC_JOB_IO_LEN + sec4_sg_bytes,
-			GFP_DMA | flags);
-	if (!edesc) {
-		dev_err(jrdev, "could not allocate extended descriptor\n");
+	edesc = ahash_edesc_alloc(ctx, sec4_sg_src_index,
+				  ctx->sh_desc_fin, ctx->sh_desc_fin_dma,
+				  flags);
+	if (!edesc)
 		return -ENOMEM;
-	}
 
-	sh_len = desc_len(sh_desc);
 	desc = edesc->hw_desc;
-	init_job_desc_shared(desc, ptr, sh_len, HDR_SHARE_DEFER | HDR_REVERSE);
 
 	edesc->sec4_sg_bytes = sec4_sg_bytes;
-	edesc->sec4_sg = (void *)edesc + sizeof(struct ahash_edesc) +
-			 DESC_JOB_IO_LEN;
 	edesc->src_nents = 0;
 
 	ret = ctx_map_to_sec4_sg(desc, jrdev, state, ctx->ctx_len,
 				 edesc->sec4_sg, DMA_TO_DEVICE);
 	if (ret)
-		return ret;
+		goto unmap_ctx;
 
 	state->buf_dma = try_buf_map_to_sec4_sg(jrdev, edesc->sec4_sg + 1,
 						buf, state->buf_dma, buflen,
@@ -956,7 +1007,8 @@ static int ahash_final_ctx(struct ahash_request *req)
 					    sec4_sg_bytes, DMA_TO_DEVICE);
 	if (dma_mapping_error(jrdev, edesc->sec4_sg_dma)) {
 		dev_err(jrdev, "unable to map S/G table\n");
-		return -ENOMEM;
+		ret = -ENOMEM;
+		goto unmap_ctx;
 	}
 
 	append_seq_in_ptr(desc, edesc->sec4_sg_dma, ctx->ctx_len + buflen,
@@ -966,7 +1018,8 @@ static int ahash_final_ctx(struct ahash_request *req)
 						digestsize);
 	if (dma_mapping_error(jrdev, edesc->dst_dma)) {
 		dev_err(jrdev, "unable to map dst\n");
-		return -ENOMEM;
+		ret = -ENOMEM;
+		goto unmap_ctx;
 	}
 
 #ifdef DEBUG
@@ -975,13 +1028,13 @@ static int ahash_final_ctx(struct ahash_request *req)
 #endif
 
 	ret = caam_jr_enqueue(jrdev, desc, ahash_done_ctx_src, req);
-	if (!ret) {
-		ret = -EINPROGRESS;
-	} else {
-		ahash_unmap_ctx(jrdev, edesc, req, digestsize, DMA_FROM_DEVICE);
-		kfree(edesc);
-	}
+	if (ret)
+		goto unmap_ctx;
 
+	return -EINPROGRESS;
+ unmap_ctx:
+	ahash_unmap_ctx(jrdev, edesc, req, digestsize, DMA_FROM_DEVICE);
+	kfree(edesc);
 	return ret;
 }
 
@@ -997,68 +1050,66 @@ static int ahash_finup_ctx(struct ahash_request *req)
 	int buflen = state->current_buf ? state->buflen_1 : state->buflen_0;
 	int last_buflen = state->current_buf ? state->buflen_0 :
 			  state->buflen_1;
-	u32 *sh_desc = ctx->sh_desc_finup, *desc;
-	dma_addr_t ptr = ctx->sh_desc_finup_dma;
-	int sec4_sg_bytes, sec4_sg_src_index;
-	int src_nents;
+	u32 *desc;
+	int sec4_sg_src_index;
+	int src_nents, mapped_nents;
 	int digestsize = crypto_ahash_digestsize(ahash);
 	struct ahash_edesc *edesc;
-	int ret = 0;
-	int sh_len;
+	int ret;
 
 	src_nents = sg_nents_for_len(req->src, req->nbytes);
 	if (src_nents < 0) {
 		dev_err(jrdev, "Invalid number of src SG.\n");
 		return src_nents;
 	}
+
+	if (src_nents) {
+		mapped_nents = dma_map_sg(jrdev, req->src, src_nents,
+					  DMA_TO_DEVICE);
+		if (!mapped_nents) {
+			dev_err(jrdev, "unable to DMA map source\n");
+			return -ENOMEM;
+		}
+	} else {
+		mapped_nents = 0;
+	}
+
 	sec4_sg_src_index = 1 + (buflen ? 1 : 0);
-	sec4_sg_bytes = (sec4_sg_src_index + src_nents) *
-			 sizeof(struct sec4_sg_entry);
 
 	/* allocate space for base edesc and hw desc commands, link tables */
-	edesc = kzalloc(sizeof(*edesc) + DESC_JOB_IO_LEN + sec4_sg_bytes,
-			GFP_DMA | flags);
+	edesc = ahash_edesc_alloc(ctx, sec4_sg_src_index + mapped_nents,
+				  ctx->sh_desc_finup, ctx->sh_desc_finup_dma,
+				  flags);
 	if (!edesc) {
-		dev_err(jrdev, "could not allocate extended descriptor\n");
+		dma_unmap_sg(jrdev, req->src, src_nents, DMA_TO_DEVICE);
 		return -ENOMEM;
 	}
 
-	sh_len = desc_len(sh_desc);
 	desc = edesc->hw_desc;
-	init_job_desc_shared(desc, ptr, sh_len, HDR_SHARE_DEFER | HDR_REVERSE);
 
 	edesc->src_nents = src_nents;
-	edesc->sec4_sg_bytes = sec4_sg_bytes;
-	edesc->sec4_sg = (void *)edesc + sizeof(struct ahash_edesc) +
-			 DESC_JOB_IO_LEN;
 
 	ret = ctx_map_to_sec4_sg(desc, jrdev, state, ctx->ctx_len,
 				 edesc->sec4_sg, DMA_TO_DEVICE);
 	if (ret)
-		return ret;
+		goto unmap_ctx;
 
 	state->buf_dma = try_buf_map_to_sec4_sg(jrdev, edesc->sec4_sg + 1,
 						buf, state->buf_dma, buflen,
 						last_buflen);
 
-	src_map_to_sec4_sg(jrdev, req->src, src_nents, edesc->sec4_sg +
-			   sec4_sg_src_index);
-
-	edesc->sec4_sg_dma = dma_map_single(jrdev, edesc->sec4_sg,
-					    sec4_sg_bytes, DMA_TO_DEVICE);
-	if (dma_mapping_error(jrdev, edesc->sec4_sg_dma)) {
-		dev_err(jrdev, "unable to map S/G table\n");
-		return -ENOMEM;
-	}
-
-	append_seq_in_ptr(desc, edesc->sec4_sg_dma, ctx->ctx_len +
-			       buflen + req->nbytes, LDST_SGF);
+	ret = ahash_edesc_add_src(ctx, edesc, req, mapped_nents,
+				  sec4_sg_src_index, ctx->ctx_len + buflen,
+				  req->nbytes);
+	if (ret)
+		goto unmap_ctx;
 
 	edesc->dst_dma = map_seq_out_ptr_result(desc, jrdev, req->result,
 						digestsize);
 	if (dma_mapping_error(jrdev, edesc->dst_dma)) {
 		dev_err(jrdev, "unable to map dst\n");
-		return -ENOMEM;
+		ret = -ENOMEM;
+		goto unmap_ctx;
 	}
 
 #ifdef DEBUG
@@ -1067,13 +1118,13 @@ static int ahash_finup_ctx(struct ahash_request *req)
 #endif
 
 	ret = caam_jr_enqueue(jrdev, desc, ahash_done_ctx_src, req);
-	if (!ret) {
-		ret = -EINPROGRESS;
-	} else {
-		ahash_unmap_ctx(jrdev, edesc, req, digestsize, DMA_FROM_DEVICE);
-		kfree(edesc);
-	}
+	if (ret)
+		goto unmap_ctx;
 
+	return -EINPROGRESS;
+ unmap_ctx:
+	ahash_unmap_ctx(jrdev, edesc, req, digestsize, DMA_FROM_DEVICE);
+	kfree(edesc);
 	return ret;
 }
 
@@ -1084,60 +1135,56 @@ static int ahash_digest(struct ahash_request *req)
 	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;
-	u32 *sh_desc = ctx->sh_desc_digest, *desc;
-	dma_addr_t ptr = ctx->sh_desc_digest_dma;
+	u32 *desc;
 	int digestsize = crypto_ahash_digestsize(ahash);
-	int src_nents, sec4_sg_bytes;
-	dma_addr_t src_dma;
+	int src_nents, mapped_nents;
 	struct ahash_edesc *edesc;
-	int ret = 0;
-	u32 options;
-	int sh_len;
+	int ret;
 
-	src_nents = sg_count(req->src, req->nbytes);
+	src_nents = sg_nents_for_len(req->src, req->nbytes);
 	if (src_nents < 0) {
 		dev_err(jrdev, "Invalid number of src SG.\n");
 		return src_nents;
 	}
-	dma_map_sg(jrdev, req->src, src_nents ? : 1, DMA_TO_DEVICE);
-	sec4_sg_bytes = src_nents * sizeof(struct sec4_sg_entry);
+
+	if (src_nents) {
+		mapped_nents = dma_map_sg(jrdev, req->src, src_nents,
+					  DMA_TO_DEVICE);
+		if (!mapped_nents) {
+			dev_err(jrdev, "unable to map source for DMA\n");
+			return -ENOMEM;
+		}
+	} else {
+		mapped_nents = 0;
+	}
 
 	/* allocate space for base edesc and hw desc commands, link tables */
-	edesc = kzalloc(sizeof(*edesc) + sec4_sg_bytes + DESC_JOB_IO_LEN,
-			GFP_DMA | flags);
+	edesc = ahash_edesc_alloc(ctx, mapped_nents > 1 ? mapped_nents : 0,
+				  ctx->sh_desc_digest, ctx->sh_desc_digest_dma,
+				  flags);
 	if (!edesc) {
-		dev_err(jrdev, "could not allocate extended descriptor\n");
+		dma_unmap_sg(jrdev, req->src, src_nents, DMA_TO_DEVICE);
 		return -ENOMEM;
 	}
-	edesc->sec4_sg = (void *)edesc + sizeof(struct ahash_edesc) +
-			  DESC_JOB_IO_LEN;
-	edesc->sec4_sg_bytes = sec4_sg_bytes;
-	edesc->src_nents = src_nents;
 
-	sh_len = desc_len(sh_desc);
-	desc = edesc->hw_desc;
-	init_job_desc_shared(desc, ptr, sh_len, HDR_SHARE_DEFER | HDR_REVERSE);
+	edesc->src_nents = src_nents;
 
-	if (src_nents) {
-		sg_to_sec4_sg_last(req->src, src_nents, edesc->sec4_sg, 0);
-		edesc->sec4_sg_dma = dma_map_single(jrdev, edesc->sec4_sg,
-					    sec4_sg_bytes, DMA_TO_DEVICE);
-		if (dma_mapping_error(jrdev, edesc->sec4_sg_dma)) {
-			dev_err(jrdev, "unable to map S/G table\n");
-			return -ENOMEM;
-		}
-		src_dma = edesc->sec4_sg_dma;
-		options = LDST_SGF;
-	} else {
-		src_dma = sg_dma_address(req->src);
-		options = 0;
+	ret = ahash_edesc_add_src(ctx, edesc, req, mapped_nents, 0, 0,
+				  req->nbytes);
+	if (ret) {
+		ahash_unmap(jrdev, edesc, req, digestsize);
+		kfree(edesc);
+		return ret;
 	}
-	append_seq_in_ptr(desc, src_dma, req->nbytes, options);
+
+	desc = edesc->hw_desc;
 
 	edesc->dst_dma = map_seq_out_ptr_result(desc, jrdev, req->result,
 						digestsize);
 	if (dma_mapping_error(jrdev, edesc->dst_dma)) {
 		dev_err(jrdev, "unable to map dst\n");
+		ahash_unmap(jrdev, edesc, req, digestsize);
+		kfree(edesc);
 		return -ENOMEM;
 	}
 
@@ -1168,29 +1215,23 @@ static int ahash_final_no_ctx(struct ahash_request *req)
 		       CRYPTO_TFM_REQ_MAY_SLEEP)) ? GFP_KERNEL : GFP_ATOMIC;
 	u8 *buf = state->current_buf ? state->buf_1 : state->buf_0;
 	int buflen = state->current_buf ? state->buflen_1 : state->buflen_0;
-	u32 *sh_desc = ctx->sh_desc_digest, *desc;
-	dma_addr_t ptr = ctx->sh_desc_digest_dma;
+	u32 *desc;
 	int digestsize = crypto_ahash_digestsize(ahash);
 	struct ahash_edesc *edesc;
-	int ret = 0;
-	int sh_len;
+	int ret;
 
 	/* allocate space for base edesc and hw desc commands, link tables */
-	edesc = kzalloc(sizeof(*edesc) + DESC_JOB_IO_LEN, GFP_DMA | flags);
-	if (!edesc) {
-		dev_err(jrdev, "could not allocate extended descriptor\n");
+	edesc = ahash_edesc_alloc(ctx, 0, ctx->sh_desc_digest,
+				  ctx->sh_desc_digest_dma, flags);
+	if (!edesc)
 		return -ENOMEM;
-	}
 
-	edesc->sec4_sg_bytes = 0;
-	sh_len = desc_len(sh_desc);
 	desc = edesc->hw_desc;
-	init_job_desc_shared(desc, ptr, sh_len, HDR_SHARE_DEFER | HDR_REVERSE);
 
 	state->buf_dma = dma_map_single(jrdev, buf, buflen, DMA_TO_DEVICE);
 	if (dma_mapping_error(jrdev, state->buf_dma)) {
 		dev_err(jrdev, "unable to map src\n");
-		return -ENOMEM;
+		goto unmap;
 	}
 
 	append_seq_in_ptr(desc, state->buf_dma, buflen, 0);
@@ -1199,7 +1240,7 @@ static int ahash_final_no_ctx(struct ahash_request *req)
 						digestsize);
 	if (dma_mapping_error(jrdev, edesc->dst_dma)) {
 		dev_err(jrdev, "unable to map dst\n");
-		return -ENOMEM;
+		goto unmap;
 	}
 	edesc->src_nents = 0;
 
@@ -1217,6 +1258,11 @@ static int ahash_final_no_ctx(struct ahash_request *req)
 	}
 
 	return ret;
+ unmap:
+	ahash_unmap(jrdev, edesc, req, digestsize);
+	kfree(edesc);
+	return -ENOMEM;
+
 }
 
 /* submit ahash update if it the first job descriptor after update */
@@ -1234,48 +1280,58 @@ static int ahash_update_no_ctx(struct ahash_request *req)
 	int *next_buflen = state->current_buf ? &state->buflen_0 :
 			   &state->buflen_1;
 	int in_len = *buflen + req->nbytes, to_hash;
-	int sec4_sg_bytes, src_nents;
+	int sec4_sg_bytes, src_nents, mapped_nents;
 	struct ahash_edesc *edesc;
-	u32 *desc, *sh_desc = ctx->sh_desc_update_first;
-	dma_addr_t ptr = ctx->sh_desc_update_first_dma;
+	u32 *desc;
 	int ret = 0;
-	int sh_len;
 
 	*next_buflen = in_len & (crypto_tfm_alg_blocksize(&ahash->base) - 1);
 	to_hash = in_len - *next_buflen;
 
 	if (to_hash) {
 		src_nents = sg_nents_for_len(req->src,
-					     req->nbytes - (*next_buflen));
+					     req->nbytes - *next_buflen);
 		if (src_nents < 0) {
 			dev_err(jrdev, "Invalid number of src SG.\n");
 			return src_nents;
 		}
-		sec4_sg_bytes = (1 + src_nents) *
+
+		if (src_nents) {
+			mapped_nents = dma_map_sg(jrdev, req->src, src_nents,
+						  DMA_TO_DEVICE);
+			if (!mapped_nents) {
+				dev_err(jrdev, "unable to DMA map source\n");
+				return -ENOMEM;
+			}
+		} else {
+			mapped_nents = 0;
+		}
+
+		sec4_sg_bytes = (1 + mapped_nents) *
 				sizeof(struct sec4_sg_entry);
 
 		/*
 		 * allocate space for base edesc and hw desc commands,
 		 * link tables
 		 */
-		edesc = kzalloc(sizeof(*edesc) + DESC_JOB_IO_LEN +
-				sec4_sg_bytes, GFP_DMA | flags);
+		edesc = ahash_edesc_alloc(ctx, 1 + mapped_nents,
+					  ctx->sh_desc_update_first,
+					  ctx->sh_desc_update_first_dma,
+					  flags);
 		if (!edesc) {
-			dev_err(jrdev,
-				"could not allocate extended descriptor\n");
+			dma_unmap_sg(jrdev, req->src, src_nents, DMA_TO_DEVICE);
 			return -ENOMEM;
 		}
 
 		edesc->src_nents = src_nents;
 		edesc->sec4_sg_bytes = sec4_sg_bytes;
-		edesc->sec4_sg = (void *)edesc + sizeof(struct ahash_edesc) +
-				 DESC_JOB_IO_LEN;
 		edesc->dst_dma = 0;
 
 		state->buf_dma = buf_map_to_sec4_sg(jrdev, edesc->sec4_sg,
 						    buf, *buflen);
-		src_map_to_sec4_sg(jrdev, req->src, src_nents,
-				   edesc->sec4_sg + 1);
+		sg_to_sec4_sg_last(req->src, mapped_nents,
+				   edesc->sec4_sg + 1, 0);
+
 		if (*next_buflen) {
 			scatterwalk_map_and_copy(next_buf, req->src,
 						 to_hash - *buflen,
@@ -1284,24 +1340,22 @@ static int ahash_update_no_ctx(struct ahash_request *req)
 
 		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 |
-				     HDR_REVERSE);
 
 		edesc->sec4_sg_dma = dma_map_single(jrdev, edesc->sec4_sg,
 						    sec4_sg_bytes,
 						    DMA_TO_DEVICE);
 		if (dma_mapping_error(jrdev, edesc->sec4_sg_dma)) {
 			dev_err(jrdev, "unable to map S/G table\n");
-			return -ENOMEM;
+			ret = -ENOMEM;
+			goto unmap_ctx;
 		}
 
 		append_seq_in_ptr(desc, edesc->sec4_sg_dma, to_hash, LDST_SGF);
 
 		ret = map_seq_out_ptr_ctx(desc, jrdev, state, ctx->ctx_len);
 		if (ret)
-			return ret;
+			goto unmap_ctx;
 
 #ifdef DEBUG
 		print_hex_dump(KERN_ERR, "jobdesc@"__stringify(__LINE__)": ",
@@ -1310,16 +1364,13 @@ static int ahash_update_no_ctx(struct ahash_request *req)
 #endif
 
 		ret = caam_jr_enqueue(jrdev, desc, ahash_done_ctx_dst, req);
-		if (!ret) {
-			ret = -EINPROGRESS;
-			state->update = ahash_update_ctx;
-			state->finup = ahash_finup_ctx;
-			state->final = ahash_final_ctx;
-		} else {
-			ahash_unmap_ctx(jrdev, edesc, req, ctx->ctx_len,
-					DMA_TO_DEVICE);
-			kfree(edesc);
-		}
+		if (ret)
+			goto unmap_ctx;
+
+		ret = -EINPROGRESS;
+		state->update = ahash_update_ctx;
+		state->finup = ahash_finup_ctx;
+		state->final = ahash_final_ctx;
 	} else if (*next_buflen) {
 		scatterwalk_map_and_copy(buf + *buflen, req->src, 0,
 					 req->nbytes, 0);
@@ -1335,6 +1386,10 @@ static int ahash_update_no_ctx(struct ahash_request *req)
 #endif
 
 	return ret;
+ unmap_ctx:
+	ahash_unmap_ctx(jrdev, edesc, req, ctx->ctx_len, DMA_TO_DEVICE);
+	kfree(edesc);
+	return ret;
 }
 
 /* submit ahash finup if it the first job descriptor after update */
@@ -1350,61 +1405,63 @@ static int ahash_finup_no_ctx(struct ahash_request *req)
 	int buflen = state->current_buf ? state->buflen_1 : state->buflen_0;
 	int last_buflen = state->current_buf ? state->buflen_0 :
 			  state->buflen_1;
-	u32 *sh_desc = ctx->sh_desc_digest, *desc;
-	dma_addr_t ptr = ctx->sh_desc_digest_dma;
-	int sec4_sg_bytes, sec4_sg_src_index, src_nents;
+	u32 *desc;
+	int sec4_sg_bytes, sec4_sg_src_index, src_nents, mapped_nents;
 	int digestsize = crypto_ahash_digestsize(ahash);
 	struct ahash_edesc *edesc;
-	int sh_len;
-	int ret = 0;
+	int ret;
 
 	src_nents = sg_nents_for_len(req->src, req->nbytes);
 	if (src_nents < 0) {
 		dev_err(jrdev, "Invalid number of src SG.\n");
 		return src_nents;
 	}
+
+	if (src_nents) {
+		mapped_nents = dma_map_sg(jrdev, req->src, src_nents,
+					  DMA_TO_DEVICE);
+		if (!mapped_nents) {
+			dev_err(jrdev, "unable to DMA map source\n");
+			return -ENOMEM;
+		}
+	} else {
+		mapped_nents = 0;
+	}
+
 	sec4_sg_src_index = 2;
-	sec4_sg_bytes = (sec4_sg_src_index + src_nents) *
+	sec4_sg_bytes = (sec4_sg_src_index + mapped_nents) *
 			 sizeof(struct sec4_sg_entry);
 
 	/* allocate space for base edesc and hw desc commands, link tables */
-	edesc = kzalloc(sizeof(*edesc) + DESC_JOB_IO_LEN + sec4_sg_bytes,
-			GFP_DMA | flags);
+	edesc = ahash_edesc_alloc(ctx, sec4_sg_src_index + mapped_nents,
+				  ctx->sh_desc_digest, ctx->sh_desc_digest_dma,
+				  flags);
 	if (!edesc) {
-		dev_err(jrdev, "could not allocate extended descriptor\n");
+		dma_unmap_sg(jrdev, req->src, src_nents, DMA_TO_DEVICE);
 		return -ENOMEM;
 	}
 
-	sh_len = desc_len(sh_desc);
 	desc = edesc->hw_desc;
-	init_job_desc_shared(desc, ptr, sh_len, HDR_SHARE_DEFER | HDR_REVERSE);
 
 	edesc->src_nents = src_nents;
 	edesc->sec4_sg_bytes = sec4_sg_bytes;
-	edesc->sec4_sg = (void *)edesc + sizeof(struct ahash_edesc) +
-			 DESC_JOB_IO_LEN;
 
 	state->buf_dma = try_buf_map_to_sec4_sg(jrdev, edesc->sec4_sg, buf,
 						state->buf_dma, buflen,
 						last_buflen);
 
-	src_map_to_sec4_sg(jrdev, req->src, src_nents, edesc->sec4_sg + 1);
-
-	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)) {
+	ret = ahash_edesc_add_src(ctx, edesc, req, mapped_nents, 1, buflen,
+				  req->nbytes);
+	if (ret) {
 		dev_err(jrdev, "unable to map S/G table\n");
-		return -ENOMEM;
+		goto unmap;
 	}
 
-	append_seq_in_ptr(desc, edesc->sec4_sg_dma, buflen +
-			       req->nbytes, LDST_SGF);
-
 	edesc->dst_dma = map_seq_out_ptr_result(desc, jrdev, req->result,
 						digestsize);
 	if (dma_mapping_error(jrdev, edesc->dst_dma)) {
 		dev_err(jrdev, "unable to map dst\n");
-		return -ENOMEM;
+		goto unmap;
 	}
 
 #ifdef DEBUG
@@ -1421,6 +1478,11 @@ static int ahash_finup_no_ctx(struct ahash_request *req)
 	}
 
 	return ret;
+ unmap:
+	ahash_unmap(jrdev, edesc, req, digestsize);
+	kfree(edesc);
+	return -ENOMEM;
+
 }
 
 /* submit first update job descriptor after init */
@@ -1436,78 +1498,65 @@ static int ahash_update_first(struct ahash_request *req)
 	int *next_buflen = state->current_buf ?
 		&state->buflen_1 : &state->buflen_0;
 	int to_hash;
-	u32 *sh_desc = ctx->sh_desc_update_first, *desc;
-	dma_addr_t ptr = ctx->sh_desc_update_first_dma;
-	int sec4_sg_bytes, src_nents;
-	dma_addr_t src_dma;
-	u32 options;
+	u32 *desc;
+	int src_nents, mapped_nents;
 	struct ahash_edesc *edesc;
 	int ret = 0;
-	int sh_len;
 
 	*next_buflen = req->nbytes & (crypto_tfm_alg_blocksize(&ahash->base) -
 				      1);
 	to_hash = req->nbytes - *next_buflen;
 
 	if (to_hash) {
-		src_nents = sg_count(req->src, req->nbytes - (*next_buflen));
+		src_nents = sg_nents_for_len(req->src,
+					     req->nbytes - *next_buflen);
 		if (src_nents < 0) {
 			dev_err(jrdev, "Invalid number of src SG.\n");
 			return src_nents;
 		}
-		dma_map_sg(jrdev, req->src, src_nents ? : 1, DMA_TO_DEVICE);
-		sec4_sg_bytes = src_nents * sizeof(struct sec4_sg_entry);
+
+		if (src_nents) {
+			mapped_nents = dma_map_sg(jrdev, req->src, src_nents,
+						  DMA_TO_DEVICE);
+			if (!mapped_nents) {
+				dev_err(jrdev, "unable to map source for DMA\n");
+				return -ENOMEM;
+			}
+		} else {
+			mapped_nents = 0;
+		}
 
 		/*
 		 * allocate space for base edesc and hw desc commands,
 		 * link tables
 		 */
-		edesc = kzalloc(sizeof(*edesc) + DESC_JOB_IO_LEN +
-				sec4_sg_bytes, GFP_DMA | flags);
+		edesc = ahash_edesc_alloc(ctx, mapped_nents > 1 ?
+					  mapped_nents : 0,
+					  ctx->sh_desc_update_first,
+					  ctx->sh_desc_update_first_dma,
+					  flags);
 		if (!edesc) {
-			dev_err(jrdev,
-				"could not allocate extended descriptor\n");
+			dma_unmap_sg(jrdev, req->src, src_nents, DMA_TO_DEVICE);
 			return -ENOMEM;
 		}
 
 		edesc->src_nents = src_nents;
-		edesc->sec4_sg_bytes = sec4_sg_bytes;
-		edesc->sec4_sg = (void *)edesc + sizeof(struct ahash_edesc) +
-				 DESC_JOB_IO_LEN;
 		edesc->dst_dma = 0;
 
-		if (src_nents) {
-			sg_to_sec4_sg_last(req->src, src_nents,
-					   edesc->sec4_sg, 0);
-			edesc->sec4_sg_dma = dma_map_single(jrdev,
-							    edesc->sec4_sg,
-							    sec4_sg_bytes,
-							    DMA_TO_DEVICE);
-			if (dma_mapping_error(jrdev, edesc->sec4_sg_dma)) {
-				dev_err(jrdev, "unable to map S/G table\n");
-				return -ENOMEM;
-			}
-			src_dma = edesc->sec4_sg_dma;
-			options = LDST_SGF;
-		} else {
-			src_dma = sg_dma_address(req->src);
-			options = 0;
-		}
+		ret = ahash_edesc_add_src(ctx, edesc, req, mapped_nents, 0, 0,
+					  to_hash);
+		if (ret)
+			goto unmap_ctx;
 
 		if (*next_buflen)
 			scatterwalk_map_and_copy(next_buf, req->src, to_hash,
 						 *next_buflen, 0);
 
-		sh_len = desc_len(sh_desc);
 		desc = edesc->hw_desc;
-		init_job_desc_shared(desc, ptr, sh_len, HDR_SHARE_DEFER |
-				     HDR_REVERSE);
-
-		append_seq_in_ptr(desc, src_dma, to_hash, options);
 
 		ret = map_seq_out_ptr_ctx(desc, jrdev, state, ctx->ctx_len);
 		if (ret)
-			return ret;
+			goto unmap_ctx;
 
 #ifdef DEBUG
 		print_hex_dump(KERN_ERR, "jobdesc@"__stringify(__LINE__)": ",
@@ -1515,18 +1564,14 @@ static int ahash_update_first(struct ahash_request *req)
 			       desc_bytes(desc), 1);
 #endif
 
-		ret = caam_jr_enqueue(jrdev, desc, ahash_done_ctx_dst,
-				      req);
-		if (!ret) {
-			ret = -EINPROGRESS;
-			state->update = ahash_update_ctx;
-			state->finup = ahash_finup_ctx;
-			state->final = ahash_final_ctx;
-		} else {
-			ahash_unmap_ctx(jrdev, edesc, req, ctx->ctx_len,
-					DMA_TO_DEVICE);
-			kfree(edesc);
-		}
+		ret = caam_jr_enqueue(jrdev, desc, ahash_done_ctx_dst, req);
+		if (ret)
+			goto unmap_ctx;
+
+		ret = -EINPROGRESS;
+		state->update = ahash_update_ctx;
+		state->finup = ahash_finup_ctx;
+		state->final = ahash_final_ctx;
 	} else if (*next_buflen) {
 		state->update = ahash_update_no_ctx;
 		state->finup = ahash_finup_no_ctx;
@@ -1541,6 +1586,10 @@ static int ahash_update_first(struct ahash_request *req)
 #endif
 
 	return ret;
+ unmap_ctx:
+	ahash_unmap_ctx(jrdev, edesc, req, ctx->ctx_len, DMA_TO_DEVICE);
+	kfree(edesc);
+	return ret;
 }
 
 static int ahash_finup_first(struct ahash_request *req)
@@ -1799,7 +1848,6 @@ static int caam_hash_cra_init(struct crypto_tfm *tfm)
 					 HASH_MSG_LEN + SHA256_DIGEST_SIZE,
 					 HASH_MSG_LEN + 64,
 					 HASH_MSG_LEN + SHA512_DIGEST_SIZE };
-	int ret = 0;
 
 	/*
 	 * Get a Job ring from Job Ring driver to ensure in-order
@@ -1819,10 +1867,7 @@ static int caam_hash_cra_init(struct crypto_tfm *tfm)
 
 	crypto_ahash_set_reqsize(__crypto_ahash_cast(tfm),
 				 sizeof(struct caam_hash_state));
-
-	ret = ahash_set_sh_desc(ahash);
-
-	return ret;
+	return ahash_set_sh_desc(ahash);
 }
 
 static void caam_hash_cra_exit(struct crypto_tfm *tfm)
diff --git a/drivers/crypto/caam/ctrl.c b/drivers/crypto/caam/ctrl.c
index 0ec112ee5204..72ff19658985 100644
--- a/drivers/crypto/caam/ctrl.c
+++ b/drivers/crypto/caam/ctrl.c
@@ -14,6 +14,7 @@
 #include "jr.h"
 #include "desc_constr.h"
 #include "error.h"
+#include "ctrl.h"
 
 bool caam_little_end;
 EXPORT_SYMBOL(caam_little_end);
@@ -826,6 +827,8 @@ static int caam_probe(struct platform_device *pdev)
 
 caam_remove:
 	caam_remove(pdev);
+	return ret;
+
 iounmap_ctrl:
 	iounmap(ctrl);
 disable_caam_emi_slow:
diff --git a/drivers/crypto/caam/desc.h b/drivers/crypto/caam/desc.h
index 26427c11ad87..513b6646bb36 100644
--- a/drivers/crypto/caam/desc.h
+++ b/drivers/crypto/caam/desc.h
@@ -23,13 +23,7 @@
 #define SEC4_SG_OFFSET_MASK	0x00001fff
 
 struct sec4_sg_entry {
-#if !defined(CONFIG_ARCH_DMA_ADDR_T_64BIT) && \
-	defined(CONFIG_CRYPTO_DEV_FSL_CAAM_IMX)
-	u32 rsvd1;
-	dma_addr_t ptr;
-#else
 	u64 ptr;
-#endif /* CONFIG_CRYPTO_DEV_FSL_CAAM_IMX */
 	u32 len;
 	u32 bpid_offset;
 };
diff --git a/drivers/crypto/caam/desc_constr.h b/drivers/crypto/caam/desc_constr.h
index d3869b95e7b1..a8cd8a78ec1f 100644
--- a/drivers/crypto/caam/desc_constr.h
+++ b/drivers/crypto/caam/desc_constr.h
@@ -325,6 +325,23 @@ static inline void append_##cmd##_imm_##type(u32 *desc, type immediate, \
 APPEND_CMD_RAW_IMM(load, LOAD, u32);
 
 /*
+ * ee - endianness
+ * size - size of immediate type in bytes
+ */
+#define APPEND_CMD_RAW_IMM2(cmd, op, ee, size) \
+static inline void append_##cmd##_imm_##ee##size(u32 *desc, \
+						   u##size immediate, \
+						   u32 options) \
+{ \
+	__##ee##size data = cpu_to_##ee##size(immediate); \
+	PRINT_POS; \
+	append_cmd(desc, CMD_##op | IMMEDIATE | options | sizeof(data)); \
+	append_data(desc, &data, sizeof(data)); \
+}
+
+APPEND_CMD_RAW_IMM2(load, LOAD, be, 32);
+
+/*
  * Append math command. Only the last part of destination and source need to
  * be specified
  */
diff --git a/drivers/crypto/caam/intern.h b/drivers/crypto/caam/intern.h
index e2bcacc1a921..5d4c05074a5c 100644
--- a/drivers/crypto/caam/intern.h
+++ b/drivers/crypto/caam/intern.h
@@ -41,7 +41,6 @@ struct caam_drv_private_jr {
 	struct device		*dev;
 	int ridx;
 	struct caam_job_ring __iomem *rregs;	/* JobR's register space */
-	struct tasklet_struct irqtask;
 	int irq;			/* One per queue */
 
 	/* Number of scatterlist crypt transforms active on the JobR */
diff --git a/drivers/crypto/caam/jr.c b/drivers/crypto/caam/jr.c
index a81f551ac222..757c27f9953d 100644
--- a/drivers/crypto/caam/jr.c
+++ b/drivers/crypto/caam/jr.c
@@ -73,8 +73,6 @@ static int caam_jr_shutdown(struct device *dev)
 
 	ret = caam_reset_hw_jr(dev);
 
-	tasklet_kill(&jrp->irqtask);
-
 	/* Release interrupt */
 	free_irq(jrp->irq, dev);
 
@@ -130,7 +128,7 @@ static irqreturn_t caam_jr_interrupt(int irq, void *st_dev)
 
 	/*
 	 * Check the output ring for ready responses, kick
-	 * tasklet if jobs done.
+	 * the threaded irq if jobs done.
 	 */
 	irqstate = rd_reg32(&jrp->rregs->jrintstatus);
 	if (!irqstate)
@@ -152,18 +150,13 @@ static irqreturn_t caam_jr_interrupt(int irq, void *st_dev)
 	/* Have valid interrupt at this point, just ACK and trigger */
 	wr_reg32(&jrp->rregs->jrintstatus, irqstate);
 
-	preempt_disable();
-	tasklet_schedule(&jrp->irqtask);
-	preempt_enable();
-
-	return IRQ_HANDLED;
+	return IRQ_WAKE_THREAD;
 }
 
-/* Deferred service handler, run as interrupt-fired tasklet */
-static void caam_jr_dequeue(unsigned long devarg)
+static irqreturn_t caam_jr_threadirq(int irq, void *st_dev)
 {
 	int hw_idx, sw_idx, i, head, tail;
-	struct device *dev = (struct device *)devarg;
+	struct device *dev = st_dev;
 	struct caam_drv_private_jr *jrp = dev_get_drvdata(dev);
 	void (*usercall)(struct device *dev, u32 *desc, u32 status, void *arg);
 	u32 *userdesc, userstatus;
@@ -237,6 +230,8 @@ static void caam_jr_dequeue(unsigned long devarg)
 
 	/* reenable / unmask IRQs */
 	clrsetbits_32(&jrp->rregs->rconfig_lo, JRCFG_IMSK, 0);
+
+	return IRQ_HANDLED;
 }
 
 /**
@@ -394,11 +389,10 @@ static int caam_jr_init(struct device *dev)
 
 	jrp = dev_get_drvdata(dev);
 
-	tasklet_init(&jrp->irqtask, caam_jr_dequeue, (unsigned long)dev);
-
 	/* Connect job ring interrupt handler. */
-	error = request_irq(jrp->irq, caam_jr_interrupt, IRQF_SHARED,
-			    dev_name(dev), dev);
+	error = request_threaded_irq(jrp->irq, caam_jr_interrupt,
+				     caam_jr_threadirq, IRQF_SHARED,
+				     dev_name(dev), dev);
 	if (error) {
 		dev_err(dev, "can't connect JobR %d interrupt (%d)\n",
 			jrp->ridx, jrp->irq);
@@ -460,7 +454,6 @@ out_free_inpring:
 out_free_irq:
 	free_irq(jrp->irq, dev);
 out_kill_deq:
-	tasklet_kill(&jrp->irqtask);
 	return error;
 }
 
@@ -513,6 +506,7 @@ static int caam_jr_probe(struct platform_device *pdev)
 	error = caam_jr_init(jrdev); /* now turn on hardware */
 	if (error) {
 		irq_dispose_mapping(jrpriv->irq);
+		iounmap(ctrl);
 		return error;
 	}
 
diff --git a/drivers/crypto/caam/regs.h b/drivers/crypto/caam/regs.h
index b3c5016f6458..84d2f838a063 100644
--- a/drivers/crypto/caam/regs.h
+++ b/drivers/crypto/caam/regs.h
@@ -196,6 +196,14 @@ static inline u64 rd_reg64(void __iomem *reg)
 #define caam_dma_to_cpu(value) caam32_to_cpu(value)
 #endif /* CONFIG_ARCH_DMA_ADDR_T_64BIT  */
 
+#ifdef CONFIG_CRYPTO_DEV_FSL_CAAM_IMX
+#define cpu_to_caam_dma64(value) \
+		(((u64)cpu_to_caam32(lower_32_bits(value)) << 32) | \
+		 (u64)cpu_to_caam32(upper_32_bits(value)))
+#else
+#define cpu_to_caam_dma64(value) cpu_to_caam64(value)
+#endif
+
 /*
  * jr_outentry
  * Represents each entry in a JobR output ring
diff --git a/drivers/crypto/caam/sg_sw_sec4.h b/drivers/crypto/caam/sg_sw_sec4.h
index 19dc64fede0d..41cd5a356d05 100644
--- a/drivers/crypto/caam/sg_sw_sec4.h
+++ b/drivers/crypto/caam/sg_sw_sec4.h
@@ -15,7 +15,7 @@ struct sec4_sg_entry;
 static inline void dma_to_sec4_sg_one(struct sec4_sg_entry *sec4_sg_ptr,
 				      dma_addr_t dma, u32 len, u16 offset)
 {
-	sec4_sg_ptr->ptr = cpu_to_caam_dma(dma);
+	sec4_sg_ptr->ptr = cpu_to_caam_dma64(dma);
 	sec4_sg_ptr->len = cpu_to_caam32(len);
 	sec4_sg_ptr->bpid_offset = cpu_to_caam32(offset & SEC4_SG_OFFSET_MASK);
 #ifdef DEBUG
diff --git a/drivers/crypto/ccp/Makefile b/drivers/crypto/ccp/Makefile
index ee4d2741b3ab..346ceb8f17bd 100644
--- a/drivers/crypto/ccp/Makefile
+++ b/drivers/crypto/ccp/Makefile
@@ -2,6 +2,7 @@ obj-$(CONFIG_CRYPTO_DEV_CCP_DD) += ccp.o
 ccp-objs := ccp-dev.o \
 	    ccp-ops.o \
 	    ccp-dev-v3.o \
+	    ccp-dev-v5.o \
 	    ccp-platform.o \
 	    ccp-dmaengine.o
 ccp-$(CONFIG_PCI) += ccp-pci.o
diff --git a/drivers/crypto/ccp/ccp-crypto-sha.c b/drivers/crypto/ccp/ccp-crypto-sha.c
index 8f36af62fe95..84a652be4274 100644
--- a/drivers/crypto/ccp/ccp-crypto-sha.c
+++ b/drivers/crypto/ccp/ccp-crypto-sha.c
@@ -4,6 +4,7 @@
  * Copyright (C) 2013,2016 Advanced Micro Devices, Inc.
  *
  * Author: Tom Lendacky <thomas.lendacky@amd.com>
+ * Author: Gary R Hook <gary.hook@amd.com>
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
@@ -134,7 +135,22 @@ static int ccp_do_sha_update(struct ahash_request *req, unsigned int nbytes,
 	rctx->cmd.engine = CCP_ENGINE_SHA;
 	rctx->cmd.u.sha.type = rctx->type;
 	rctx->cmd.u.sha.ctx = &rctx->ctx_sg;
-	rctx->cmd.u.sha.ctx_len = sizeof(rctx->ctx);
+
+	switch (rctx->type) {
+	case CCP_SHA_TYPE_1:
+		rctx->cmd.u.sha.ctx_len = SHA1_DIGEST_SIZE;
+		break;
+	case CCP_SHA_TYPE_224:
+		rctx->cmd.u.sha.ctx_len = SHA224_DIGEST_SIZE;
+		break;
+	case CCP_SHA_TYPE_256:
+		rctx->cmd.u.sha.ctx_len = SHA256_DIGEST_SIZE;
+		break;
+	default:
+		/* Should never get here */
+		break;
+	}
+
 	rctx->cmd.u.sha.src = sg;
 	rctx->cmd.u.sha.src_len = rctx->hash_cnt;
 	rctx->cmd.u.sha.opad = ctx->u.sha.key_len ?
diff --git a/drivers/crypto/ccp/ccp-dev-v3.c b/drivers/crypto/ccp/ccp-dev-v3.c
index d7a710347967..8d2dbacc6161 100644
--- a/drivers/crypto/ccp/ccp-dev-v3.c
+++ b/drivers/crypto/ccp/ccp-dev-v3.c
@@ -4,6 +4,7 @@
  * Copyright (C) 2013,2016 Advanced Micro Devices, Inc.
  *
  * Author: Tom Lendacky <thomas.lendacky@amd.com>
+ * Author: Gary R Hook <gary.hook@amd.com>
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
@@ -19,6 +20,61 @@
 
 #include "ccp-dev.h"
 
+static u32 ccp_alloc_ksb(struct ccp_cmd_queue *cmd_q, unsigned int count)
+{
+	int start;
+	struct ccp_device *ccp = cmd_q->ccp;
+
+	for (;;) {
+		mutex_lock(&ccp->sb_mutex);
+
+		start = (u32)bitmap_find_next_zero_area(ccp->sb,
+							ccp->sb_count,
+							ccp->sb_start,
+							count, 0);
+		if (start <= ccp->sb_count) {
+			bitmap_set(ccp->sb, start, count);
+
+			mutex_unlock(&ccp->sb_mutex);
+			break;
+		}
+
+		ccp->sb_avail = 0;
+
+		mutex_unlock(&ccp->sb_mutex);
+
+		/* Wait for KSB entries to become available */
+		if (wait_event_interruptible(ccp->sb_queue, ccp->sb_avail))
+			return 0;
+	}
+
+	return KSB_START + start;
+}
+
+static void ccp_free_ksb(struct ccp_cmd_queue *cmd_q, unsigned int start,
+			 unsigned int count)
+{
+	struct ccp_device *ccp = cmd_q->ccp;
+
+	if (!start)
+		return;
+
+	mutex_lock(&ccp->sb_mutex);
+
+	bitmap_clear(ccp->sb, start - KSB_START, count);
+
+	ccp->sb_avail = 1;
+
+	mutex_unlock(&ccp->sb_mutex);
+
+	wake_up_interruptible_all(&ccp->sb_queue);
+}
+
+static unsigned int ccp_get_free_slots(struct ccp_cmd_queue *cmd_q)
+{
+	return CMD_Q_DEPTH(ioread32(cmd_q->reg_status));
+}
+
 static int ccp_do_cmd(struct ccp_op *op, u32 *cr, unsigned int cr_count)
 {
 	struct ccp_cmd_queue *cmd_q = op->cmd_q;
@@ -68,6 +124,9 @@ static int ccp_do_cmd(struct ccp_op *op, u32 *cr, unsigned int cr_count)
 			/* On error delete all related jobs from the queue */
 			cmd = (cmd_q->id << DEL_Q_ID_SHIFT)
 			      | op->jobid;
+			if (cmd_q->cmd_error)
+				ccp_log_error(cmd_q->ccp,
+					      cmd_q->cmd_error);
 
 			iowrite32(cmd, ccp->io_regs + DEL_CMD_Q_JOB);
 
@@ -99,10 +158,10 @@ static int ccp_perform_aes(struct ccp_op *op)
 		| (op->u.aes.type << REQ1_AES_TYPE_SHIFT)
 		| (op->u.aes.mode << REQ1_AES_MODE_SHIFT)
 		| (op->u.aes.action << REQ1_AES_ACTION_SHIFT)
-		| (op->ksb_key << REQ1_KEY_KSB_SHIFT);
+		| (op->sb_key << REQ1_KEY_KSB_SHIFT);
 	cr[1] = op->src.u.dma.length - 1;
 	cr[2] = ccp_addr_lo(&op->src.u.dma);
-	cr[3] = (op->ksb_ctx << REQ4_KSB_SHIFT)
+	cr[3] = (op->sb_ctx << REQ4_KSB_SHIFT)
 		| (CCP_MEMTYPE_SYSTEM << REQ4_MEMTYPE_SHIFT)
 		| ccp_addr_hi(&op->src.u.dma);
 	cr[4] = ccp_addr_lo(&op->dst.u.dma);
@@ -129,10 +188,10 @@ static int ccp_perform_xts_aes(struct ccp_op *op)
 	cr[0] = (CCP_ENGINE_XTS_AES_128 << REQ1_ENGINE_SHIFT)
 		| (op->u.xts.action << REQ1_AES_ACTION_SHIFT)
 		| (op->u.xts.unit_size << REQ1_XTS_AES_SIZE_SHIFT)
-		| (op->ksb_key << REQ1_KEY_KSB_SHIFT);
+		| (op->sb_key << REQ1_KEY_KSB_SHIFT);
 	cr[1] = op->src.u.dma.length - 1;
 	cr[2] = ccp_addr_lo(&op->src.u.dma);
-	cr[3] = (op->ksb_ctx << REQ4_KSB_SHIFT)
+	cr[3] = (op->sb_ctx << REQ4_KSB_SHIFT)
 		| (CCP_MEMTYPE_SYSTEM << REQ4_MEMTYPE_SHIFT)
 		| ccp_addr_hi(&op->src.u.dma);
 	cr[4] = ccp_addr_lo(&op->dst.u.dma);
@@ -158,7 +217,7 @@ static int ccp_perform_sha(struct ccp_op *op)
 		| REQ1_INIT;
 	cr[1] = op->src.u.dma.length - 1;
 	cr[2] = ccp_addr_lo(&op->src.u.dma);
-	cr[3] = (op->ksb_ctx << REQ4_KSB_SHIFT)
+	cr[3] = (op->sb_ctx << REQ4_KSB_SHIFT)
 		| (CCP_MEMTYPE_SYSTEM << REQ4_MEMTYPE_SHIFT)
 		| ccp_addr_hi(&op->src.u.dma);
 
@@ -181,11 +240,11 @@ static int ccp_perform_rsa(struct ccp_op *op)
 	/* Fill out the register contents for REQ1 through REQ6 */
 	cr[0] = (CCP_ENGINE_RSA << REQ1_ENGINE_SHIFT)
 		| (op->u.rsa.mod_size << REQ1_RSA_MOD_SIZE_SHIFT)
-		| (op->ksb_key << REQ1_KEY_KSB_SHIFT)
+		| (op->sb_key << REQ1_KEY_KSB_SHIFT)
 		| REQ1_EOM;
 	cr[1] = op->u.rsa.input_len - 1;
 	cr[2] = ccp_addr_lo(&op->src.u.dma);
-	cr[3] = (op->ksb_ctx << REQ4_KSB_SHIFT)
+	cr[3] = (op->sb_ctx << REQ4_KSB_SHIFT)
 		| (CCP_MEMTYPE_SYSTEM << REQ4_MEMTYPE_SHIFT)
 		| ccp_addr_hi(&op->src.u.dma);
 	cr[4] = ccp_addr_lo(&op->dst.u.dma);
@@ -215,10 +274,10 @@ static int ccp_perform_passthru(struct ccp_op *op)
 			| ccp_addr_hi(&op->src.u.dma);
 
 		if (op->u.passthru.bit_mod != CCP_PASSTHRU_BITWISE_NOOP)
-			cr[3] |= (op->ksb_key << REQ4_KSB_SHIFT);
+			cr[3] |= (op->sb_key << REQ4_KSB_SHIFT);
 	} else {
-		cr[2] = op->src.u.ksb * CCP_KSB_BYTES;
-		cr[3] = (CCP_MEMTYPE_KSB << REQ4_MEMTYPE_SHIFT);
+		cr[2] = op->src.u.sb * CCP_SB_BYTES;
+		cr[3] = (CCP_MEMTYPE_SB << REQ4_MEMTYPE_SHIFT);
 	}
 
 	if (op->dst.type == CCP_MEMTYPE_SYSTEM) {
@@ -226,8 +285,8 @@ static int ccp_perform_passthru(struct ccp_op *op)
 		cr[5] = (CCP_MEMTYPE_SYSTEM << REQ6_MEMTYPE_SHIFT)
 			| ccp_addr_hi(&op->dst.u.dma);
 	} else {
-		cr[4] = op->dst.u.ksb * CCP_KSB_BYTES;
-		cr[5] = (CCP_MEMTYPE_KSB << REQ6_MEMTYPE_SHIFT);
+		cr[4] = op->dst.u.sb * CCP_SB_BYTES;
+		cr[5] = (CCP_MEMTYPE_SB << REQ6_MEMTYPE_SHIFT);
 	}
 
 	if (op->eom)
@@ -256,35 +315,6 @@ static int ccp_perform_ecc(struct ccp_op *op)
 	return ccp_do_cmd(op, cr, ARRAY_SIZE(cr));
 }
 
-static int ccp_trng_read(struct hwrng *rng, void *data, size_t max, bool wait)
-{
-	struct ccp_device *ccp = container_of(rng, struct ccp_device, hwrng);
-	u32 trng_value;
-	int len = min_t(int, sizeof(trng_value), max);
-
-	/*
-	 * Locking is provided by the caller so we can update device
-	 * hwrng-related fields safely
-	 */
-	trng_value = ioread32(ccp->io_regs + TRNG_OUT_REG);
-	if (!trng_value) {
-		/* Zero is returned if not data is available or if a
-		 * bad-entropy error is present. Assume an error if
-		 * we exceed TRNG_RETRIES reads of zero.
-		 */
-		if (ccp->hwrng_retries++ > TRNG_RETRIES)
-			return -EIO;
-
-		return 0;
-	}
-
-	/* Reset the counter and save the rng value */
-	ccp->hwrng_retries = 0;
-	memcpy(data, &trng_value, len);
-
-	return len;
-}
-
 static int ccp_init(struct ccp_device *ccp)
 {
 	struct device *dev = ccp->dev;
@@ -321,9 +351,9 @@ static int ccp_init(struct ccp_device *ccp)
 		cmd_q->dma_pool = dma_pool;
 
 		/* Reserve 2 KSB regions for the queue */
-		cmd_q->ksb_key = KSB_START + ccp->ksb_start++;
-		cmd_q->ksb_ctx = KSB_START + ccp->ksb_start++;
-		ccp->ksb_count -= 2;
+		cmd_q->sb_key = KSB_START + ccp->sb_start++;
+		cmd_q->sb_ctx = KSB_START + ccp->sb_start++;
+		ccp->sb_count -= 2;
 
 		/* Preset some register values and masks that are queue
 		 * number dependent
@@ -335,7 +365,7 @@ static int ccp_init(struct ccp_device *ccp)
 		cmd_q->int_ok = 1 << (i * 2);
 		cmd_q->int_err = 1 << ((i * 2) + 1);
 
-		cmd_q->free_slots = CMD_Q_DEPTH(ioread32(cmd_q->reg_status));
+		cmd_q->free_slots = ccp_get_free_slots(cmd_q);
 
 		init_waitqueue_head(&cmd_q->int_queue);
 
@@ -375,9 +405,10 @@ static int ccp_init(struct ccp_device *ccp)
 	}
 
 	/* Initialize the queues used to wait for KSB space and suspend */
-	init_waitqueue_head(&ccp->ksb_queue);
+	init_waitqueue_head(&ccp->sb_queue);
 	init_waitqueue_head(&ccp->suspend_queue);
 
+	dev_dbg(dev, "Starting threads...\n");
 	/* Create a kthread for each queue */
 	for (i = 0; i < ccp->cmd_q_count; i++) {
 		struct task_struct *kthread;
@@ -397,29 +428,26 @@ static int ccp_init(struct ccp_device *ccp)
 		wake_up_process(kthread);
 	}
 
-	/* Register the RNG */
-	ccp->hwrng.name = ccp->rngname;
-	ccp->hwrng.read = ccp_trng_read;
-	ret = hwrng_register(&ccp->hwrng);
-	if (ret) {
-		dev_err(dev, "error registering hwrng (%d)\n", ret);
+	dev_dbg(dev, "Enabling interrupts...\n");
+	/* Enable interrupts */
+	iowrite32(qim, ccp->io_regs + IRQ_MASK_REG);
+
+	dev_dbg(dev, "Registering device...\n");
+	ccp_add_device(ccp);
+
+	ret = ccp_register_rng(ccp);
+	if (ret)
 		goto e_kthread;
-	}
 
 	/* Register the DMA engine support */
 	ret = ccp_dmaengine_register(ccp);
 	if (ret)
 		goto e_hwrng;
 
-	ccp_add_device(ccp);
-
-	/* Enable interrupts */
-	iowrite32(qim, ccp->io_regs + IRQ_MASK_REG);
-
 	return 0;
 
 e_hwrng:
-	hwrng_unregister(&ccp->hwrng);
+	ccp_unregister_rng(ccp);
 
 e_kthread:
 	for (i = 0; i < ccp->cmd_q_count; i++)
@@ -441,19 +469,14 @@ static void ccp_destroy(struct ccp_device *ccp)
 	struct ccp_cmd *cmd;
 	unsigned int qim, i;
 
-	/* Remove this device from the list of available units first */
-	ccp_del_device(ccp);
-
 	/* Unregister the DMA engine */
 	ccp_dmaengine_unregister(ccp);
 
 	/* Unregister the RNG */
-	hwrng_unregister(&ccp->hwrng);
+	ccp_unregister_rng(ccp);
 
-	/* Stop the queue kthreads */
-	for (i = 0; i < ccp->cmd_q_count; i++)
-		if (ccp->cmd_q[i].kthread)
-			kthread_stop(ccp->cmd_q[i].kthread);
+	/* Remove this device from the list of available units */
+	ccp_del_device(ccp);
 
 	/* Build queue interrupt mask (two interrupt masks per queue) */
 	qim = 0;
@@ -472,6 +495,11 @@ static void ccp_destroy(struct ccp_device *ccp)
 	}
 	iowrite32(qim, ccp->io_regs + IRQ_STATUS_REG);
 
+	/* Stop the queue kthreads */
+	for (i = 0; i < ccp->cmd_q_count; i++)
+		if (ccp->cmd_q[i].kthread)
+			kthread_stop(ccp->cmd_q[i].kthread);
+
 	ccp->free_irq(ccp);
 
 	for (i = 0; i < ccp->cmd_q_count; i++)
@@ -527,18 +555,24 @@ static irqreturn_t ccp_irq_handler(int irq, void *data)
 }
 
 static const struct ccp_actions ccp3_actions = {
-	.perform_aes = ccp_perform_aes,
-	.perform_xts_aes = ccp_perform_xts_aes,
-	.perform_sha = ccp_perform_sha,
-	.perform_rsa = ccp_perform_rsa,
-	.perform_passthru = ccp_perform_passthru,
-	.perform_ecc = ccp_perform_ecc,
+	.aes = ccp_perform_aes,
+	.xts_aes = ccp_perform_xts_aes,
+	.sha = ccp_perform_sha,
+	.rsa = ccp_perform_rsa,
+	.passthru = ccp_perform_passthru,
+	.ecc = ccp_perform_ecc,
+	.sballoc = ccp_alloc_ksb,
+	.sbfree = ccp_free_ksb,
 	.init = ccp_init,
 	.destroy = ccp_destroy,
+	.get_free_slots = ccp_get_free_slots,
 	.irqhandler = ccp_irq_handler,
 };
 
-struct ccp_vdata ccpv3 = {
+const struct ccp_vdata ccpv3 = {
 	.version = CCP_VERSION(3, 0),
+	.setup = NULL,
 	.perform = &ccp3_actions,
+	.bar = 2,
+	.offset = 0x20000,
 };
diff --git a/drivers/crypto/ccp/ccp-dev-v5.c b/drivers/crypto/ccp/ccp-dev-v5.c
new file mode 100644
index 000000000000..faf3cb3ddce2
--- /dev/null
+++ b/drivers/crypto/ccp/ccp-dev-v5.c
@@ -0,0 +1,1017 @@
+/*
+ * AMD Cryptographic Coprocessor (CCP) driver
+ *
+ * Copyright (C) 2016 Advanced Micro Devices, Inc.
+ *
+ * Author: Gary R Hook <gary.hook@amd.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/pci.h>
+#include <linux/kthread.h>
+#include <linux/dma-mapping.h>
+#include <linux/interrupt.h>
+#include <linux/compiler.h>
+#include <linux/ccp.h>
+
+#include "ccp-dev.h"
+
+static u32 ccp_lsb_alloc(struct ccp_cmd_queue *cmd_q, unsigned int count)
+{
+	struct ccp_device *ccp;
+	int start;
+
+	/* First look at the map for the queue */
+	if (cmd_q->lsb >= 0) {
+		start = (u32)bitmap_find_next_zero_area(cmd_q->lsbmap,
+							LSB_SIZE,
+							0, count, 0);
+		if (start < LSB_SIZE) {
+			bitmap_set(cmd_q->lsbmap, start, count);
+			return start + cmd_q->lsb * LSB_SIZE;
+		}
+	}
+
+	/* No joy; try to get an entry from the shared blocks */
+	ccp = cmd_q->ccp;
+	for (;;) {
+		mutex_lock(&ccp->sb_mutex);
+
+		start = (u32)bitmap_find_next_zero_area(ccp->lsbmap,
+							MAX_LSB_CNT * LSB_SIZE,
+							0,
+							count, 0);
+		if (start <= MAX_LSB_CNT * LSB_SIZE) {
+			bitmap_set(ccp->lsbmap, start, count);
+
+			mutex_unlock(&ccp->sb_mutex);
+			return start * LSB_ITEM_SIZE;
+		}
+
+		ccp->sb_avail = 0;
+
+		mutex_unlock(&ccp->sb_mutex);
+
+		/* Wait for KSB entries to become available */
+		if (wait_event_interruptible(ccp->sb_queue, ccp->sb_avail))
+			return 0;
+	}
+}
+
+static void ccp_lsb_free(struct ccp_cmd_queue *cmd_q, unsigned int start,
+			 unsigned int count)
+{
+	int lsbno = start / LSB_SIZE;
+
+	if (!start)
+		return;
+
+	if (cmd_q->lsb == lsbno) {
+		/* An entry from the private LSB */
+		bitmap_clear(cmd_q->lsbmap, start % LSB_SIZE, count);
+	} else {
+		/* From the shared LSBs */
+		struct ccp_device *ccp = cmd_q->ccp;
+
+		mutex_lock(&ccp->sb_mutex);
+		bitmap_clear(ccp->lsbmap, start, count);
+		ccp->sb_avail = 1;
+		mutex_unlock(&ccp->sb_mutex);
+		wake_up_interruptible_all(&ccp->sb_queue);
+	}
+}
+
+/* CCP version 5: Union to define the function field (cmd_reg1/dword0) */
+union ccp_function {
+	struct {
+		u16 size:7;
+		u16 encrypt:1;
+		u16 mode:5;
+		u16 type:2;
+	} aes;
+	struct {
+		u16 size:7;
+		u16 encrypt:1;
+		u16 rsvd:5;
+		u16 type:2;
+	} aes_xts;
+	struct {
+		u16 rsvd1:10;
+		u16 type:4;
+		u16 rsvd2:1;
+	} sha;
+	struct {
+		u16 mode:3;
+		u16 size:12;
+	} rsa;
+	struct {
+		u16 byteswap:2;
+		u16 bitwise:3;
+		u16 reflect:2;
+		u16 rsvd:8;
+	} pt;
+	struct  {
+		u16 rsvd:13;
+	} zlib;
+	struct {
+		u16 size:10;
+		u16 type:2;
+		u16 mode:3;
+	} ecc;
+	u16 raw;
+};
+
+#define	CCP_AES_SIZE(p)		((p)->aes.size)
+#define	CCP_AES_ENCRYPT(p)	((p)->aes.encrypt)
+#define	CCP_AES_MODE(p)		((p)->aes.mode)
+#define	CCP_AES_TYPE(p)		((p)->aes.type)
+#define	CCP_XTS_SIZE(p)		((p)->aes_xts.size)
+#define	CCP_XTS_ENCRYPT(p)	((p)->aes_xts.encrypt)
+#define	CCP_SHA_TYPE(p)		((p)->sha.type)
+#define	CCP_RSA_SIZE(p)		((p)->rsa.size)
+#define	CCP_PT_BYTESWAP(p)	((p)->pt.byteswap)
+#define	CCP_PT_BITWISE(p)	((p)->pt.bitwise)
+#define	CCP_ECC_MODE(p)		((p)->ecc.mode)
+#define	CCP_ECC_AFFINE(p)	((p)->ecc.one)
+
+/* Word 0 */
+#define CCP5_CMD_DW0(p)		((p)->dw0)
+#define CCP5_CMD_SOC(p)		(CCP5_CMD_DW0(p).soc)
+#define CCP5_CMD_IOC(p)		(CCP5_CMD_DW0(p).ioc)
+#define CCP5_CMD_INIT(p)	(CCP5_CMD_DW0(p).init)
+#define CCP5_CMD_EOM(p)		(CCP5_CMD_DW0(p).eom)
+#define CCP5_CMD_FUNCTION(p)	(CCP5_CMD_DW0(p).function)
+#define CCP5_CMD_ENGINE(p)	(CCP5_CMD_DW0(p).engine)
+#define CCP5_CMD_PROT(p)	(CCP5_CMD_DW0(p).prot)
+
+/* Word 1 */
+#define CCP5_CMD_DW1(p)		((p)->length)
+#define CCP5_CMD_LEN(p)		(CCP5_CMD_DW1(p))
+
+/* Word 2 */
+#define CCP5_CMD_DW2(p)		((p)->src_lo)
+#define CCP5_CMD_SRC_LO(p)	(CCP5_CMD_DW2(p))
+
+/* Word 3 */
+#define CCP5_CMD_DW3(p)		((p)->dw3)
+#define CCP5_CMD_SRC_MEM(p)	((p)->dw3.src_mem)
+#define CCP5_CMD_SRC_HI(p)	((p)->dw3.src_hi)
+#define CCP5_CMD_LSB_ID(p)	((p)->dw3.lsb_cxt_id)
+#define CCP5_CMD_FIX_SRC(p)	((p)->dw3.fixed)
+
+/* Words 4/5 */
+#define CCP5_CMD_DW4(p)		((p)->dw4)
+#define CCP5_CMD_DST_LO(p)	(CCP5_CMD_DW4(p).dst_lo)
+#define CCP5_CMD_DW5(p)		((p)->dw5.fields.dst_hi)
+#define CCP5_CMD_DST_HI(p)	(CCP5_CMD_DW5(p))
+#define CCP5_CMD_DST_MEM(p)	((p)->dw5.fields.dst_mem)
+#define CCP5_CMD_FIX_DST(p)	((p)->dw5.fields.fixed)
+#define CCP5_CMD_SHA_LO(p)	((p)->dw4.sha_len_lo)
+#define CCP5_CMD_SHA_HI(p)	((p)->dw5.sha_len_hi)
+
+/* Word 6/7 */
+#define CCP5_CMD_DW6(p)		((p)->key_lo)
+#define CCP5_CMD_KEY_LO(p)	(CCP5_CMD_DW6(p))
+#define CCP5_CMD_DW7(p)		((p)->dw7)
+#define CCP5_CMD_KEY_HI(p)	((p)->dw7.key_hi)
+#define CCP5_CMD_KEY_MEM(p)	((p)->dw7.key_mem)
+
+static inline u32 low_address(unsigned long addr)
+{
+	return (u64)addr & 0x0ffffffff;
+}
+
+static inline u32 high_address(unsigned long addr)
+{
+	return ((u64)addr >> 32) & 0x00000ffff;
+}
+
+static unsigned int ccp5_get_free_slots(struct ccp_cmd_queue *cmd_q)
+{
+	unsigned int head_idx, n;
+	u32 head_lo, queue_start;
+
+	queue_start = low_address(cmd_q->qdma_tail);
+	head_lo = ioread32(cmd_q->reg_head_lo);
+	head_idx = (head_lo - queue_start) / sizeof(struct ccp5_desc);
+
+	n = head_idx + COMMANDS_PER_QUEUE - cmd_q->qidx - 1;
+
+	return n % COMMANDS_PER_QUEUE; /* Always one unused spot */
+}
+
+static int ccp5_do_cmd(struct ccp5_desc *desc,
+		       struct ccp_cmd_queue *cmd_q)
+{
+	u32 *mP;
+	__le32 *dP;
+	u32 tail;
+	int	i;
+	int ret = 0;
+
+	if (CCP5_CMD_SOC(desc)) {
+		CCP5_CMD_IOC(desc) = 1;
+		CCP5_CMD_SOC(desc) = 0;
+	}
+	mutex_lock(&cmd_q->q_mutex);
+
+	mP = (u32 *) &cmd_q->qbase[cmd_q->qidx];
+	dP = (__le32 *) desc;
+	for (i = 0; i < 8; i++)
+		mP[i] = cpu_to_le32(dP[i]); /* handle endianness */
+
+	cmd_q->qidx = (cmd_q->qidx + 1) % COMMANDS_PER_QUEUE;
+
+	/* The data used by this command must be flushed to memory */
+	wmb();
+
+	/* Write the new tail address back to the queue register */
+	tail = low_address(cmd_q->qdma_tail + cmd_q->qidx * Q_DESC_SIZE);
+	iowrite32(tail, cmd_q->reg_tail_lo);
+
+	/* Turn the queue back on using our cached control register */
+	iowrite32(cmd_q->qcontrol | CMD5_Q_RUN, cmd_q->reg_control);
+	mutex_unlock(&cmd_q->q_mutex);
+
+	if (CCP5_CMD_IOC(desc)) {
+		/* Wait for the job to complete */
+		ret = wait_event_interruptible(cmd_q->int_queue,
+					       cmd_q->int_rcvd);
+		if (ret || cmd_q->cmd_error) {
+			if (cmd_q->cmd_error)
+				ccp_log_error(cmd_q->ccp,
+					      cmd_q->cmd_error);
+			/* A version 5 device doesn't use Job IDs... */
+			if (!ret)
+				ret = -EIO;
+		}
+		cmd_q->int_rcvd = 0;
+	}
+
+	return 0;
+}
+
+static int ccp5_perform_aes(struct ccp_op *op)
+{
+	struct ccp5_desc desc;
+	union ccp_function function;
+	u32 key_addr = op->sb_key * LSB_ITEM_SIZE;
+
+	/* Zero out all the fields of the command desc */
+	memset(&desc, 0, Q_DESC_SIZE);
+
+	CCP5_CMD_ENGINE(&desc) = CCP_ENGINE_AES;
+
+	CCP5_CMD_SOC(&desc) = op->soc;
+	CCP5_CMD_IOC(&desc) = 1;
+	CCP5_CMD_INIT(&desc) = op->init;
+	CCP5_CMD_EOM(&desc) = op->eom;
+	CCP5_CMD_PROT(&desc) = 0;
+
+	function.raw = 0;
+	CCP_AES_ENCRYPT(&function) = op->u.aes.action;
+	CCP_AES_MODE(&function) = op->u.aes.mode;
+	CCP_AES_TYPE(&function) = op->u.aes.type;
+	if (op->u.aes.mode == CCP_AES_MODE_CFB)
+		CCP_AES_SIZE(&function) = 0x7f;
+
+	CCP5_CMD_FUNCTION(&desc) = function.raw;
+
+	CCP5_CMD_LEN(&desc) = op->src.u.dma.length;
+
+	CCP5_CMD_SRC_LO(&desc) = ccp_addr_lo(&op->src.u.dma);
+	CCP5_CMD_SRC_HI(&desc) = ccp_addr_hi(&op->src.u.dma);
+	CCP5_CMD_SRC_MEM(&desc) = CCP_MEMTYPE_SYSTEM;
+
+	CCP5_CMD_DST_LO(&desc) = ccp_addr_lo(&op->dst.u.dma);
+	CCP5_CMD_DST_HI(&desc) = ccp_addr_hi(&op->dst.u.dma);
+	CCP5_CMD_DST_MEM(&desc) = CCP_MEMTYPE_SYSTEM;
+
+	CCP5_CMD_KEY_LO(&desc) = lower_32_bits(key_addr);
+	CCP5_CMD_KEY_HI(&desc) = 0;
+	CCP5_CMD_KEY_MEM(&desc) = CCP_MEMTYPE_SB;
+	CCP5_CMD_LSB_ID(&desc) = op->sb_ctx;
+
+	return ccp5_do_cmd(&desc, op->cmd_q);
+}
+
+static int ccp5_perform_xts_aes(struct ccp_op *op)
+{
+	struct ccp5_desc desc;
+	union ccp_function function;
+	u32 key_addr = op->sb_key * LSB_ITEM_SIZE;
+
+	/* Zero out all the fields of the command desc */
+	memset(&desc, 0, Q_DESC_SIZE);
+
+	CCP5_CMD_ENGINE(&desc) = CCP_ENGINE_XTS_AES_128;
+
+	CCP5_CMD_SOC(&desc) = op->soc;
+	CCP5_CMD_IOC(&desc) = 1;
+	CCP5_CMD_INIT(&desc) = op->init;
+	CCP5_CMD_EOM(&desc) = op->eom;
+	CCP5_CMD_PROT(&desc) = 0;
+
+	function.raw = 0;
+	CCP_XTS_ENCRYPT(&function) = op->u.xts.action;
+	CCP_XTS_SIZE(&function) = op->u.xts.unit_size;
+	CCP5_CMD_FUNCTION(&desc) = function.raw;
+
+	CCP5_CMD_LEN(&desc) = op->src.u.dma.length;
+
+	CCP5_CMD_SRC_LO(&desc) = ccp_addr_lo(&op->src.u.dma);
+	CCP5_CMD_SRC_HI(&desc) = ccp_addr_hi(&op->src.u.dma);
+	CCP5_CMD_SRC_MEM(&desc) = CCP_MEMTYPE_SYSTEM;
+
+	CCP5_CMD_DST_LO(&desc) = ccp_addr_lo(&op->dst.u.dma);
+	CCP5_CMD_DST_HI(&desc) = ccp_addr_hi(&op->dst.u.dma);
+	CCP5_CMD_DST_MEM(&desc) = CCP_MEMTYPE_SYSTEM;
+
+	CCP5_CMD_KEY_LO(&desc) = lower_32_bits(key_addr);
+	CCP5_CMD_KEY_HI(&desc) =  0;
+	CCP5_CMD_KEY_MEM(&desc) = CCP_MEMTYPE_SB;
+	CCP5_CMD_LSB_ID(&desc) = op->sb_ctx;
+
+	return ccp5_do_cmd(&desc, op->cmd_q);
+}
+
+static int ccp5_perform_sha(struct ccp_op *op)
+{
+	struct ccp5_desc desc;
+	union ccp_function function;
+
+	/* Zero out all the fields of the command desc */
+	memset(&desc, 0, Q_DESC_SIZE);
+
+	CCP5_CMD_ENGINE(&desc) = CCP_ENGINE_SHA;
+
+	CCP5_CMD_SOC(&desc) = op->soc;
+	CCP5_CMD_IOC(&desc) = 1;
+	CCP5_CMD_INIT(&desc) = 1;
+	CCP5_CMD_EOM(&desc) = op->eom;
+	CCP5_CMD_PROT(&desc) = 0;
+
+	function.raw = 0;
+	CCP_SHA_TYPE(&function) = op->u.sha.type;
+	CCP5_CMD_FUNCTION(&desc) = function.raw;
+
+	CCP5_CMD_LEN(&desc) = op->src.u.dma.length;
+
+	CCP5_CMD_SRC_LO(&desc) = ccp_addr_lo(&op->src.u.dma);
+	CCP5_CMD_SRC_HI(&desc) = ccp_addr_hi(&op->src.u.dma);
+	CCP5_CMD_SRC_MEM(&desc) = CCP_MEMTYPE_SYSTEM;
+
+	CCP5_CMD_LSB_ID(&desc) = op->sb_ctx;
+
+	if (op->eom) {
+		CCP5_CMD_SHA_LO(&desc) = lower_32_bits(op->u.sha.msg_bits);
+		CCP5_CMD_SHA_HI(&desc) = upper_32_bits(op->u.sha.msg_bits);
+	} else {
+		CCP5_CMD_SHA_LO(&desc) = 0;
+		CCP5_CMD_SHA_HI(&desc) = 0;
+	}
+
+	return ccp5_do_cmd(&desc, op->cmd_q);
+}
+
+static int ccp5_perform_rsa(struct ccp_op *op)
+{
+	struct ccp5_desc desc;
+	union ccp_function function;
+
+	/* Zero out all the fields of the command desc */
+	memset(&desc, 0, Q_DESC_SIZE);
+
+	CCP5_CMD_ENGINE(&desc) = CCP_ENGINE_RSA;
+
+	CCP5_CMD_SOC(&desc) = op->soc;
+	CCP5_CMD_IOC(&desc) = 1;
+	CCP5_CMD_INIT(&desc) = 0;
+	CCP5_CMD_EOM(&desc) = 1;
+	CCP5_CMD_PROT(&desc) = 0;
+
+	function.raw = 0;
+	CCP_RSA_SIZE(&function) = op->u.rsa.mod_size;
+	CCP5_CMD_FUNCTION(&desc) = function.raw;
+
+	CCP5_CMD_LEN(&desc) = op->u.rsa.input_len;
+
+	/* Source is from external memory */
+	CCP5_CMD_SRC_LO(&desc) = ccp_addr_lo(&op->src.u.dma);
+	CCP5_CMD_SRC_HI(&desc) = ccp_addr_hi(&op->src.u.dma);
+	CCP5_CMD_SRC_MEM(&desc) = CCP_MEMTYPE_SYSTEM;
+
+	/* Destination is in external memory */
+	CCP5_CMD_DST_LO(&desc) = ccp_addr_lo(&op->dst.u.dma);
+	CCP5_CMD_DST_HI(&desc) = ccp_addr_hi(&op->dst.u.dma);
+	CCP5_CMD_DST_MEM(&desc) = CCP_MEMTYPE_SYSTEM;
+
+	/* Key (Exponent) is in external memory */
+	CCP5_CMD_KEY_LO(&desc) = ccp_addr_lo(&op->exp.u.dma);
+	CCP5_CMD_KEY_HI(&desc) = ccp_addr_hi(&op->exp.u.dma);
+	CCP5_CMD_KEY_MEM(&desc) = CCP_MEMTYPE_SYSTEM;
+
+	return ccp5_do_cmd(&desc, op->cmd_q);
+}
+
+static int ccp5_perform_passthru(struct ccp_op *op)
+{
+	struct ccp5_desc desc;
+	union ccp_function function;
+	struct ccp_dma_info *saddr = &op->src.u.dma;
+	struct ccp_dma_info *daddr = &op->dst.u.dma;
+
+	memset(&desc, 0, Q_DESC_SIZE);
+
+	CCP5_CMD_ENGINE(&desc) = CCP_ENGINE_PASSTHRU;
+
+	CCP5_CMD_SOC(&desc) = 0;
+	CCP5_CMD_IOC(&desc) = 1;
+	CCP5_CMD_INIT(&desc) = 0;
+	CCP5_CMD_EOM(&desc) = op->eom;
+	CCP5_CMD_PROT(&desc) = 0;
+
+	function.raw = 0;
+	CCP_PT_BYTESWAP(&function) = op->u.passthru.byte_swap;
+	CCP_PT_BITWISE(&function) = op->u.passthru.bit_mod;
+	CCP5_CMD_FUNCTION(&desc) = function.raw;
+
+	/* Length of source data is always 256 bytes */
+	if (op->src.type == CCP_MEMTYPE_SYSTEM)
+		CCP5_CMD_LEN(&desc) = saddr->length;
+	else
+		CCP5_CMD_LEN(&desc) = daddr->length;
+
+	if (op->src.type == CCP_MEMTYPE_SYSTEM) {
+		CCP5_CMD_SRC_LO(&desc) = ccp_addr_lo(&op->src.u.dma);
+		CCP5_CMD_SRC_HI(&desc) = ccp_addr_hi(&op->src.u.dma);
+		CCP5_CMD_SRC_MEM(&desc) = CCP_MEMTYPE_SYSTEM;
+
+		if (op->u.passthru.bit_mod != CCP_PASSTHRU_BITWISE_NOOP)
+			CCP5_CMD_LSB_ID(&desc) = op->sb_key;
+	} else {
+		u32 key_addr = op->src.u.sb * CCP_SB_BYTES;
+
+		CCP5_CMD_SRC_LO(&desc) = lower_32_bits(key_addr);
+		CCP5_CMD_SRC_HI(&desc) = 0;
+		CCP5_CMD_SRC_MEM(&desc) = CCP_MEMTYPE_SB;
+	}
+
+	if (op->dst.type == CCP_MEMTYPE_SYSTEM) {
+		CCP5_CMD_DST_LO(&desc) = ccp_addr_lo(&op->dst.u.dma);
+		CCP5_CMD_DST_HI(&desc) = ccp_addr_hi(&op->dst.u.dma);
+		CCP5_CMD_DST_MEM(&desc) = CCP_MEMTYPE_SYSTEM;
+	} else {
+		u32 key_addr = op->dst.u.sb * CCP_SB_BYTES;
+
+		CCP5_CMD_DST_LO(&desc) = lower_32_bits(key_addr);
+		CCP5_CMD_DST_HI(&desc) = 0;
+		CCP5_CMD_DST_MEM(&desc) = CCP_MEMTYPE_SB;
+	}
+
+	return ccp5_do_cmd(&desc, op->cmd_q);
+}
+
+static int ccp5_perform_ecc(struct ccp_op *op)
+{
+	struct ccp5_desc desc;
+	union ccp_function function;
+
+	/* Zero out all the fields of the command desc */
+	memset(&desc, 0, Q_DESC_SIZE);
+
+	CCP5_CMD_ENGINE(&desc) = CCP_ENGINE_ECC;
+
+	CCP5_CMD_SOC(&desc) = 0;
+	CCP5_CMD_IOC(&desc) = 1;
+	CCP5_CMD_INIT(&desc) = 0;
+	CCP5_CMD_EOM(&desc) = 1;
+	CCP5_CMD_PROT(&desc) = 0;
+
+	function.raw = 0;
+	function.ecc.mode = op->u.ecc.function;
+	CCP5_CMD_FUNCTION(&desc) = function.raw;
+
+	CCP5_CMD_LEN(&desc) = op->src.u.dma.length;
+
+	CCP5_CMD_SRC_LO(&desc) = ccp_addr_lo(&op->src.u.dma);
+	CCP5_CMD_SRC_HI(&desc) = ccp_addr_hi(&op->src.u.dma);
+	CCP5_CMD_SRC_MEM(&desc) = CCP_MEMTYPE_SYSTEM;
+
+	CCP5_CMD_DST_LO(&desc) = ccp_addr_lo(&op->dst.u.dma);
+	CCP5_CMD_DST_HI(&desc) = ccp_addr_hi(&op->dst.u.dma);
+	CCP5_CMD_DST_MEM(&desc) = CCP_MEMTYPE_SYSTEM;
+
+	return ccp5_do_cmd(&desc, op->cmd_q);
+}
+
+static int ccp_find_lsb_regions(struct ccp_cmd_queue *cmd_q, u64 status)
+{
+	int q_mask = 1 << cmd_q->id;
+	int queues = 0;
+	int j;
+
+	/* Build a bit mask to know which LSBs this queue has access to.
+	 * Don't bother with segment 0 as it has special privileges.
+	 */
+	for (j = 1; j < MAX_LSB_CNT; j++) {
+		if (status & q_mask)
+			bitmap_set(cmd_q->lsbmask, j, 1);
+		status >>= LSB_REGION_WIDTH;
+	}
+	queues = bitmap_weight(cmd_q->lsbmask, MAX_LSB_CNT);
+	dev_info(cmd_q->ccp->dev, "Queue %d can access %d LSB regions\n",
+		 cmd_q->id, queues);
+
+	return queues ? 0 : -EINVAL;
+}
+
+
+static int ccp_find_and_assign_lsb_to_q(struct ccp_device *ccp,
+					int lsb_cnt, int n_lsbs,
+					unsigned long *lsb_pub)
+{
+	DECLARE_BITMAP(qlsb, MAX_LSB_CNT);
+	int bitno;
+	int qlsb_wgt;
+	int i;
+
+	/* For each queue:
+	 * If the count of potential LSBs available to a queue matches the
+	 * ordinal given to us in lsb_cnt:
+	 * Copy the mask of possible LSBs for this queue into "qlsb";
+	 * For each bit in qlsb, see if the corresponding bit in the
+	 * aggregation mask is set; if so, we have a match.
+	 *     If we have a match, clear the bit in the aggregation to
+	 *     mark it as no longer available.
+	 *     If there is no match, clear the bit in qlsb and keep looking.
+	 */
+	for (i = 0; i < ccp->cmd_q_count; i++) {
+		struct ccp_cmd_queue *cmd_q = &ccp->cmd_q[i];
+
+		qlsb_wgt = bitmap_weight(cmd_q->lsbmask, MAX_LSB_CNT);
+
+		if (qlsb_wgt == lsb_cnt) {
+			bitmap_copy(qlsb, cmd_q->lsbmask, MAX_LSB_CNT);
+
+			bitno = find_first_bit(qlsb, MAX_LSB_CNT);
+			while (bitno < MAX_LSB_CNT) {
+				if (test_bit(bitno, lsb_pub)) {
+					/* We found an available LSB
+					 * that this queue can access
+					 */
+					cmd_q->lsb = bitno;
+					bitmap_clear(lsb_pub, bitno, 1);
+					dev_info(ccp->dev,
+						 "Queue %d gets LSB %d\n",
+						 i, bitno);
+					break;
+				}
+				bitmap_clear(qlsb, bitno, 1);
+				bitno = find_first_bit(qlsb, MAX_LSB_CNT);
+			}
+			if (bitno >= MAX_LSB_CNT)
+				return -EINVAL;
+			n_lsbs--;
+		}
+	}
+	return n_lsbs;
+}
+
+/* For each queue, from the most- to least-constrained:
+ * find an LSB that can be assigned to the queue. If there are N queues that
+ * can only use M LSBs, where N > M, fail; otherwise, every queue will get a
+ * dedicated LSB. Remaining LSB regions become a shared resource.
+ * If we have fewer LSBs than queues, all LSB regions become shared resources.
+ */
+static int ccp_assign_lsbs(struct ccp_device *ccp)
+{
+	DECLARE_BITMAP(lsb_pub, MAX_LSB_CNT);
+	DECLARE_BITMAP(qlsb, MAX_LSB_CNT);
+	int n_lsbs = 0;
+	int bitno;
+	int i, lsb_cnt;
+	int rc = 0;
+
+	bitmap_zero(lsb_pub, MAX_LSB_CNT);
+
+	/* Create an aggregate bitmap to get a total count of available LSBs */
+	for (i = 0; i < ccp->cmd_q_count; i++)
+		bitmap_or(lsb_pub,
+			  lsb_pub, ccp->cmd_q[i].lsbmask,
+			  MAX_LSB_CNT);
+
+	n_lsbs = bitmap_weight(lsb_pub, MAX_LSB_CNT);
+
+	if (n_lsbs >= ccp->cmd_q_count) {
+		/* We have enough LSBS to give every queue a private LSB.
+		 * Brute force search to start with the queues that are more
+		 * constrained in LSB choice. When an LSB is privately
+		 * assigned, it is removed from the public mask.
+		 * This is an ugly N squared algorithm with some optimization.
+		 */
+		for (lsb_cnt = 1;
+		     n_lsbs && (lsb_cnt <= MAX_LSB_CNT);
+		     lsb_cnt++) {
+			rc = ccp_find_and_assign_lsb_to_q(ccp, lsb_cnt, n_lsbs,
+							  lsb_pub);
+			if (rc < 0)
+				return -EINVAL;
+			n_lsbs = rc;
+		}
+	}
+
+	rc = 0;
+	/* What's left of the LSBs, according to the public mask, now become
+	 * shared. Any zero bits in the lsb_pub mask represent an LSB region
+	 * that can't be used as a shared resource, so mark the LSB slots for
+	 * them as "in use".
+	 */
+	bitmap_copy(qlsb, lsb_pub, MAX_LSB_CNT);
+
+	bitno = find_first_zero_bit(qlsb, MAX_LSB_CNT);
+	while (bitno < MAX_LSB_CNT) {
+		bitmap_set(ccp->lsbmap, bitno * LSB_SIZE, LSB_SIZE);
+		bitmap_set(qlsb, bitno, 1);
+		bitno = find_first_zero_bit(qlsb, MAX_LSB_CNT);
+	}
+
+	return rc;
+}
+
+static int ccp5_init(struct ccp_device *ccp)
+{
+	struct device *dev = ccp->dev;
+	struct ccp_cmd_queue *cmd_q;
+	struct dma_pool *dma_pool;
+	char dma_pool_name[MAX_DMAPOOL_NAME_LEN];
+	unsigned int qmr, qim, i;
+	u64 status;
+	u32 status_lo, status_hi;
+	int ret;
+
+	/* Find available queues */
+	qim = 0;
+	qmr = ioread32(ccp->io_regs + Q_MASK_REG);
+	for (i = 0; i < MAX_HW_QUEUES; i++) {
+
+		if (!(qmr & (1 << i)))
+			continue;
+
+		/* Allocate a dma pool for this queue */
+		snprintf(dma_pool_name, sizeof(dma_pool_name), "%s_q%d",
+			 ccp->name, i);
+		dma_pool = dma_pool_create(dma_pool_name, dev,
+					   CCP_DMAPOOL_MAX_SIZE,
+					   CCP_DMAPOOL_ALIGN, 0);
+		if (!dma_pool) {
+			dev_err(dev, "unable to allocate dma pool\n");
+			ret = -ENOMEM;
+		}
+
+		cmd_q = &ccp->cmd_q[ccp->cmd_q_count];
+		ccp->cmd_q_count++;
+
+		cmd_q->ccp = ccp;
+		cmd_q->id = i;
+		cmd_q->dma_pool = dma_pool;
+		mutex_init(&cmd_q->q_mutex);
+
+		/* Page alignment satisfies our needs for N <= 128 */
+		BUILD_BUG_ON(COMMANDS_PER_QUEUE > 128);
+		cmd_q->qsize = Q_SIZE(Q_DESC_SIZE);
+		cmd_q->qbase = dma_zalloc_coherent(dev, cmd_q->qsize,
+						   &cmd_q->qbase_dma,
+						   GFP_KERNEL);
+		if (!cmd_q->qbase) {
+			dev_err(dev, "unable to allocate command queue\n");
+			ret = -ENOMEM;
+			goto e_pool;
+		}
+
+		cmd_q->qidx = 0;
+		/* Preset some register values and masks that are queue
+		 * number dependent
+		 */
+		cmd_q->reg_control = ccp->io_regs +
+				     CMD5_Q_STATUS_INCR * (i + 1);
+		cmd_q->reg_tail_lo = cmd_q->reg_control + CMD5_Q_TAIL_LO_BASE;
+		cmd_q->reg_head_lo = cmd_q->reg_control + CMD5_Q_HEAD_LO_BASE;
+		cmd_q->reg_int_enable = cmd_q->reg_control +
+					CMD5_Q_INT_ENABLE_BASE;
+		cmd_q->reg_interrupt_status = cmd_q->reg_control +
+					      CMD5_Q_INTERRUPT_STATUS_BASE;
+		cmd_q->reg_status = cmd_q->reg_control + CMD5_Q_STATUS_BASE;
+		cmd_q->reg_int_status = cmd_q->reg_control +
+					CMD5_Q_INT_STATUS_BASE;
+		cmd_q->reg_dma_status = cmd_q->reg_control +
+					CMD5_Q_DMA_STATUS_BASE;
+		cmd_q->reg_dma_read_status = cmd_q->reg_control +
+					     CMD5_Q_DMA_READ_STATUS_BASE;
+		cmd_q->reg_dma_write_status = cmd_q->reg_control +
+					      CMD5_Q_DMA_WRITE_STATUS_BASE;
+
+		init_waitqueue_head(&cmd_q->int_queue);
+
+		dev_dbg(dev, "queue #%u available\n", i);
+	}
+	if (ccp->cmd_q_count == 0) {
+		dev_notice(dev, "no command queues available\n");
+		ret = -EIO;
+		goto e_pool;
+	}
+	dev_notice(dev, "%u command queues available\n", ccp->cmd_q_count);
+
+	/* Turn off the queues and disable interrupts until ready */
+	for (i = 0; i < ccp->cmd_q_count; i++) {
+		cmd_q = &ccp->cmd_q[i];
+
+		cmd_q->qcontrol = 0; /* Start with nothing */
+		iowrite32(cmd_q->qcontrol, cmd_q->reg_control);
+
+		/* Disable the interrupts */
+		iowrite32(0x00, cmd_q->reg_int_enable);
+		ioread32(cmd_q->reg_int_status);
+		ioread32(cmd_q->reg_status);
+
+		/* Clear the interrupts */
+		iowrite32(ALL_INTERRUPTS, cmd_q->reg_interrupt_status);
+	}
+
+	dev_dbg(dev, "Requesting an IRQ...\n");
+	/* Request an irq */
+	ret = ccp->get_irq(ccp);
+	if (ret) {
+		dev_err(dev, "unable to allocate an IRQ\n");
+		goto e_pool;
+	}
+
+	/* Initialize the queue used to suspend */
+	init_waitqueue_head(&ccp->suspend_queue);
+
+	dev_dbg(dev, "Loading LSB map...\n");
+	/* Copy the private LSB mask to the public registers */
+	status_lo = ioread32(ccp->io_regs + LSB_PRIVATE_MASK_LO_OFFSET);
+	status_hi = ioread32(ccp->io_regs + LSB_PRIVATE_MASK_HI_OFFSET);
+	iowrite32(status_lo, ccp->io_regs + LSB_PUBLIC_MASK_LO_OFFSET);
+	iowrite32(status_hi, ccp->io_regs + LSB_PUBLIC_MASK_HI_OFFSET);
+	status = ((u64)status_hi<<30) | (u64)status_lo;
+
+	dev_dbg(dev, "Configuring virtual queues...\n");
+	/* Configure size of each virtual queue accessible to host */
+	for (i = 0; i < ccp->cmd_q_count; i++) {
+		u32 dma_addr_lo;
+		u32 dma_addr_hi;
+
+		cmd_q = &ccp->cmd_q[i];
+
+		cmd_q->qcontrol &= ~(CMD5_Q_SIZE << CMD5_Q_SHIFT);
+		cmd_q->qcontrol |= QUEUE_SIZE_VAL << CMD5_Q_SHIFT;
+
+		cmd_q->qdma_tail = cmd_q->qbase_dma;
+		dma_addr_lo = low_address(cmd_q->qdma_tail);
+		iowrite32((u32)dma_addr_lo, cmd_q->reg_tail_lo);
+		iowrite32((u32)dma_addr_lo, cmd_q->reg_head_lo);
+
+		dma_addr_hi = high_address(cmd_q->qdma_tail);
+		cmd_q->qcontrol |= (dma_addr_hi << 16);
+		iowrite32(cmd_q->qcontrol, cmd_q->reg_control);
+
+		/* Find the LSB regions accessible to the queue */
+		ccp_find_lsb_regions(cmd_q, status);
+		cmd_q->lsb = -1; /* Unassigned value */
+	}
+
+	dev_dbg(dev, "Assigning LSBs...\n");
+	ret = ccp_assign_lsbs(ccp);
+	if (ret) {
+		dev_err(dev, "Unable to assign LSBs (%d)\n", ret);
+		goto e_irq;
+	}
+
+	/* Optimization: pre-allocate LSB slots for each queue */
+	for (i = 0; i < ccp->cmd_q_count; i++) {
+		ccp->cmd_q[i].sb_key = ccp_lsb_alloc(&ccp->cmd_q[i], 2);
+		ccp->cmd_q[i].sb_ctx = ccp_lsb_alloc(&ccp->cmd_q[i], 2);
+	}
+
+	dev_dbg(dev, "Starting threads...\n");
+	/* Create a kthread for each queue */
+	for (i = 0; i < ccp->cmd_q_count; i++) {
+		struct task_struct *kthread;
+
+		cmd_q = &ccp->cmd_q[i];
+
+		kthread = kthread_create(ccp_cmd_queue_thread, cmd_q,
+					 "%s-q%u", ccp->name, cmd_q->id);
+		if (IS_ERR(kthread)) {
+			dev_err(dev, "error creating queue thread (%ld)\n",
+				PTR_ERR(kthread));
+			ret = PTR_ERR(kthread);
+			goto e_kthread;
+		}
+
+		cmd_q->kthread = kthread;
+		wake_up_process(kthread);
+	}
+
+	dev_dbg(dev, "Enabling interrupts...\n");
+	/* Enable interrupts */
+	for (i = 0; i < ccp->cmd_q_count; i++) {
+		cmd_q = &ccp->cmd_q[i];
+		iowrite32(ALL_INTERRUPTS, cmd_q->reg_int_enable);
+	}
+
+	dev_dbg(dev, "Registering device...\n");
+	/* Put this on the unit list to make it available */
+	ccp_add_device(ccp);
+
+	ret = ccp_register_rng(ccp);
+	if (ret)
+		goto e_kthread;
+
+	/* Register the DMA engine support */
+	ret = ccp_dmaengine_register(ccp);
+	if (ret)
+		goto e_hwrng;
+
+	return 0;
+
+e_hwrng:
+	ccp_unregister_rng(ccp);
+
+e_kthread:
+	for (i = 0; i < ccp->cmd_q_count; i++)
+		if (ccp->cmd_q[i].kthread)
+			kthread_stop(ccp->cmd_q[i].kthread);
+
+e_irq:
+	ccp->free_irq(ccp);
+
+e_pool:
+	for (i = 0; i < ccp->cmd_q_count; i++)
+		dma_pool_destroy(ccp->cmd_q[i].dma_pool);
+
+	return ret;
+}
+
+static void ccp5_destroy(struct ccp_device *ccp)
+{
+	struct device *dev = ccp->dev;
+	struct ccp_cmd_queue *cmd_q;
+	struct ccp_cmd *cmd;
+	unsigned int i;
+
+	/* Unregister the DMA engine */
+	ccp_dmaengine_unregister(ccp);
+
+	/* Unregister the RNG */
+	ccp_unregister_rng(ccp);
+
+	/* Remove this device from the list of available units first */
+	ccp_del_device(ccp);
+
+	/* Disable and clear interrupts */
+	for (i = 0; i < ccp->cmd_q_count; i++) {
+		cmd_q = &ccp->cmd_q[i];
+
+		/* Turn off the run bit */
+		iowrite32(cmd_q->qcontrol & ~CMD5_Q_RUN, cmd_q->reg_control);
+
+		/* Disable the interrupts */
+		iowrite32(ALL_INTERRUPTS, cmd_q->reg_interrupt_status);
+
+		/* Clear the interrupt status */
+		iowrite32(0x00, cmd_q->reg_int_enable);
+		ioread32(cmd_q->reg_int_status);
+		ioread32(cmd_q->reg_status);
+	}
+
+	/* Stop the queue kthreads */
+	for (i = 0; i < ccp->cmd_q_count; i++)
+		if (ccp->cmd_q[i].kthread)
+			kthread_stop(ccp->cmd_q[i].kthread);
+
+	ccp->free_irq(ccp);
+
+	for (i = 0; i < ccp->cmd_q_count; i++) {
+		cmd_q = &ccp->cmd_q[i];
+		dma_free_coherent(dev, cmd_q->qsize, cmd_q->qbase,
+				  cmd_q->qbase_dma);
+	}
+
+	/* Flush the cmd and backlog queue */
+	while (!list_empty(&ccp->cmd)) {
+		/* Invoke the callback directly with an error code */
+		cmd = list_first_entry(&ccp->cmd, struct ccp_cmd, entry);
+		list_del(&cmd->entry);
+		cmd->callback(cmd->data, -ENODEV);
+	}
+	while (!list_empty(&ccp->backlog)) {
+		/* Invoke the callback directly with an error code */
+		cmd = list_first_entry(&ccp->backlog, struct ccp_cmd, entry);
+		list_del(&cmd->entry);
+		cmd->callback(cmd->data, -ENODEV);
+	}
+}
+
+static irqreturn_t ccp5_irq_handler(int irq, void *data)
+{
+	struct device *dev = data;
+	struct ccp_device *ccp = dev_get_drvdata(dev);
+	u32 status;
+	unsigned int i;
+
+	for (i = 0; i < ccp->cmd_q_count; i++) {
+		struct ccp_cmd_queue *cmd_q = &ccp->cmd_q[i];
+
+		status = ioread32(cmd_q->reg_interrupt_status);
+
+		if (status) {
+			cmd_q->int_status = status;
+			cmd_q->q_status = ioread32(cmd_q->reg_status);
+			cmd_q->q_int_status = ioread32(cmd_q->reg_int_status);
+
+			/* On error, only save the first error value */
+			if ((status & INT_ERROR) && !cmd_q->cmd_error)
+				cmd_q->cmd_error = CMD_Q_ERROR(cmd_q->q_status);
+
+			cmd_q->int_rcvd = 1;
+
+			/* Acknowledge the interrupt and wake the kthread */
+			iowrite32(ALL_INTERRUPTS, cmd_q->reg_interrupt_status);
+			wake_up_interruptible(&cmd_q->int_queue);
+		}
+	}
+
+	return IRQ_HANDLED;
+}
+
+static void ccp5_config(struct ccp_device *ccp)
+{
+	/* Public side */
+	iowrite32(0x00001249, ccp->io_regs + CMD5_REQID_CONFIG_OFFSET);
+}
+
+static void ccp5other_config(struct ccp_device *ccp)
+{
+	int i;
+	u32 rnd;
+
+	/* We own all of the queues on the NTB CCP */
+
+	iowrite32(0x00012D57, ccp->io_regs + CMD5_TRNG_CTL_OFFSET);
+	iowrite32(0x00000003, ccp->io_regs + CMD5_CONFIG_0_OFFSET);
+	for (i = 0; i < 12; i++) {
+		rnd = ioread32(ccp->io_regs + TRNG_OUT_REG);
+		iowrite32(rnd, ccp->io_regs + CMD5_AES_MASK_OFFSET);
+	}
+
+	iowrite32(0x0000001F, ccp->io_regs + CMD5_QUEUE_MASK_OFFSET);
+	iowrite32(0x00005B6D, ccp->io_regs + CMD5_QUEUE_PRIO_OFFSET);
+	iowrite32(0x00000000, ccp->io_regs + CMD5_CMD_TIMEOUT_OFFSET);
+
+	iowrite32(0x3FFFFFFF, ccp->io_regs + LSB_PRIVATE_MASK_LO_OFFSET);
+	iowrite32(0x000003FF, ccp->io_regs + LSB_PRIVATE_MASK_HI_OFFSET);
+
+	iowrite32(0x00108823, ccp->io_regs + CMD5_CLK_GATE_CTL_OFFSET);
+
+	ccp5_config(ccp);
+}
+
+/* Version 5 adds some function, but is essentially the same as v5 */
+static const struct ccp_actions ccp5_actions = {
+	.aes = ccp5_perform_aes,
+	.xts_aes = ccp5_perform_xts_aes,
+	.sha = ccp5_perform_sha,
+	.rsa = ccp5_perform_rsa,
+	.passthru = ccp5_perform_passthru,
+	.ecc = ccp5_perform_ecc,
+	.sballoc = ccp_lsb_alloc,
+	.sbfree = ccp_lsb_free,
+	.init = ccp5_init,
+	.destroy = ccp5_destroy,
+	.get_free_slots = ccp5_get_free_slots,
+	.irqhandler = ccp5_irq_handler,
+};
+
+const struct ccp_vdata ccpv5a = {
+	.version = CCP_VERSION(5, 0),
+	.setup = ccp5_config,
+	.perform = &ccp5_actions,
+	.bar = 2,
+	.offset = 0x0,
+};
+
+const struct ccp_vdata ccpv5b = {
+	.version = CCP_VERSION(5, 0),
+	.setup = ccp5other_config,
+	.perform = &ccp5_actions,
+	.bar = 2,
+	.offset = 0x0,
+};
diff --git a/drivers/crypto/ccp/ccp-dev.c b/drivers/crypto/ccp/ccp-dev.c
index 87b9f2bfa623..cafa633aae10 100644
--- a/drivers/crypto/ccp/ccp-dev.c
+++ b/drivers/crypto/ccp/ccp-dev.c
@@ -4,6 +4,7 @@
  * Copyright (C) 2013,2016 Advanced Micro Devices, Inc.
  *
  * Author: Tom Lendacky <thomas.lendacky@amd.com>
+ * Author: Gary R Hook <gary.hook@amd.com>
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
@@ -39,6 +40,59 @@ struct ccp_tasklet_data {
 	struct ccp_cmd *cmd;
 };
 
+/* Human-readable error strings */
+char *ccp_error_codes[] = {
+	"",
+	"ERR 01: ILLEGAL_ENGINE",
+	"ERR 02: ILLEGAL_KEY_ID",
+	"ERR 03: ILLEGAL_FUNCTION_TYPE",
+	"ERR 04: ILLEGAL_FUNCTION_MODE",
+	"ERR 05: ILLEGAL_FUNCTION_ENCRYPT",
+	"ERR 06: ILLEGAL_FUNCTION_SIZE",
+	"ERR 07: Zlib_MISSING_INIT_EOM",
+	"ERR 08: ILLEGAL_FUNCTION_RSVD",
+	"ERR 09: ILLEGAL_BUFFER_LENGTH",
+	"ERR 10: VLSB_FAULT",
+	"ERR 11: ILLEGAL_MEM_ADDR",
+	"ERR 12: ILLEGAL_MEM_SEL",
+	"ERR 13: ILLEGAL_CONTEXT_ID",
+	"ERR 14: ILLEGAL_KEY_ADDR",
+	"ERR 15: 0xF Reserved",
+	"ERR 16: Zlib_ILLEGAL_MULTI_QUEUE",
+	"ERR 17: Zlib_ILLEGAL_JOBID_CHANGE",
+	"ERR 18: CMD_TIMEOUT",
+	"ERR 19: IDMA0_AXI_SLVERR",
+	"ERR 20: IDMA0_AXI_DECERR",
+	"ERR 21: 0x15 Reserved",
+	"ERR 22: IDMA1_AXI_SLAVE_FAULT",
+	"ERR 23: IDMA1_AIXI_DECERR",
+	"ERR 24: 0x18 Reserved",
+	"ERR 25: ZLIBVHB_AXI_SLVERR",
+	"ERR 26: ZLIBVHB_AXI_DECERR",
+	"ERR 27: 0x1B Reserved",
+	"ERR 27: ZLIB_UNEXPECTED_EOM",
+	"ERR 27: ZLIB_EXTRA_DATA",
+	"ERR 30: ZLIB_BTYPE",
+	"ERR 31: ZLIB_UNDEFINED_SYMBOL",
+	"ERR 32: ZLIB_UNDEFINED_DISTANCE_S",
+	"ERR 33: ZLIB_CODE_LENGTH_SYMBOL",
+	"ERR 34: ZLIB _VHB_ILLEGAL_FETCH",
+	"ERR 35: ZLIB_UNCOMPRESSED_LEN",
+	"ERR 36: ZLIB_LIMIT_REACHED",
+	"ERR 37: ZLIB_CHECKSUM_MISMATCH0",
+	"ERR 38: ODMA0_AXI_SLVERR",
+	"ERR 39: ODMA0_AXI_DECERR",
+	"ERR 40: 0x28 Reserved",
+	"ERR 41: ODMA1_AXI_SLVERR",
+	"ERR 42: ODMA1_AXI_DECERR",
+	"ERR 43: LSB_PARITY_ERR",
+};
+
+void ccp_log_error(struct ccp_device *d, int e)
+{
+	dev_err(d->dev, "CCP error: %s (0x%x)\n", ccp_error_codes[e], e);
+}
+
 /* List of CCPs, CCP count, read-write access lock, and access functions
  *
  * Lock structure: get ccp_unit_lock for reading whenever we need to
@@ -58,7 +112,7 @@ static struct ccp_device *ccp_rr;
 
 /* Ever-increasing value to produce unique unit numbers */
 static atomic_t ccp_unit_ordinal;
-unsigned int ccp_increment_unit_ordinal(void)
+static unsigned int ccp_increment_unit_ordinal(void)
 {
 	return atomic_inc_return(&ccp_unit_ordinal);
 }
@@ -118,6 +172,29 @@ void ccp_del_device(struct ccp_device *ccp)
 	write_unlock_irqrestore(&ccp_unit_lock, flags);
 }
 
+
+
+int ccp_register_rng(struct ccp_device *ccp)
+{
+	int ret = 0;
+
+	dev_dbg(ccp->dev, "Registering RNG...\n");
+	/* Register an RNG */
+	ccp->hwrng.name = ccp->rngname;
+	ccp->hwrng.read = ccp_trng_read;
+	ret = hwrng_register(&ccp->hwrng);
+	if (ret)
+		dev_err(ccp->dev, "error registering hwrng (%d)\n", ret);
+
+	return ret;
+}
+
+void ccp_unregister_rng(struct ccp_device *ccp)
+{
+	if (ccp->hwrng.name)
+		hwrng_unregister(&ccp->hwrng);
+}
+
 static struct ccp_device *ccp_get_device(void)
 {
 	unsigned long flags;
@@ -397,9 +474,9 @@ struct ccp_device *ccp_alloc_struct(struct device *dev)
 
 	spin_lock_init(&ccp->cmd_lock);
 	mutex_init(&ccp->req_mutex);
-	mutex_init(&ccp->ksb_mutex);
-	ccp->ksb_count = KSB_COUNT;
-	ccp->ksb_start = 0;
+	mutex_init(&ccp->sb_mutex);
+	ccp->sb_count = KSB_COUNT;
+	ccp->sb_start = 0;
 
 	ccp->ord = ccp_increment_unit_ordinal();
 	snprintf(ccp->name, MAX_CCP_NAME_LEN, "ccp-%u", ccp->ord);
@@ -408,6 +485,34 @@ struct ccp_device *ccp_alloc_struct(struct device *dev)
 	return ccp;
 }
 
+int ccp_trng_read(struct hwrng *rng, void *data, size_t max, bool wait)
+{
+	struct ccp_device *ccp = container_of(rng, struct ccp_device, hwrng);
+	u32 trng_value;
+	int len = min_t(int, sizeof(trng_value), max);
+
+	/* Locking is provided by the caller so we can update device
+	 * hwrng-related fields safely
+	 */
+	trng_value = ioread32(ccp->io_regs + TRNG_OUT_REG);
+	if (!trng_value) {
+		/* Zero is returned if not data is available or if a
+		 * bad-entropy error is present. Assume an error if
+		 * we exceed TRNG_RETRIES reads of zero.
+		 */
+		if (ccp->hwrng_retries++ > TRNG_RETRIES)
+			return -EIO;
+
+		return 0;
+	}
+
+	/* Reset the counter and save the rng value */
+	ccp->hwrng_retries = 0;
+	memcpy(data, &trng_value, len);
+
+	return len;
+}
+
 #ifdef CONFIG_PM
 bool ccp_queues_suspended(struct ccp_device *ccp)
 {
diff --git a/drivers/crypto/ccp/ccp-dev.h b/drivers/crypto/ccp/ccp-dev.h
index bd41ffceff82..da5f4a678083 100644
--- a/drivers/crypto/ccp/ccp-dev.h
+++ b/drivers/crypto/ccp/ccp-dev.h
@@ -4,6 +4,7 @@
  * Copyright (C) 2013,2016 Advanced Micro Devices, Inc.
  *
  * Author: Tom Lendacky <thomas.lendacky@amd.com>
+ * Author: Gary R Hook <gary.hook@amd.com>
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
@@ -60,7 +61,69 @@
 #define CMD_Q_ERROR(__qs)		((__qs) & 0x0000003f)
 #define CMD_Q_DEPTH(__qs)		(((__qs) >> 12) & 0x0000000f)
 
-/****** REQ0 Related Values ******/
+/* ------------------------ CCP Version 5 Specifics ------------------------ */
+#define CMD5_QUEUE_MASK_OFFSET		0x00
+#define	CMD5_QUEUE_PRIO_OFFSET		0x04
+#define CMD5_REQID_CONFIG_OFFSET	0x08
+#define	CMD5_CMD_TIMEOUT_OFFSET		0x10
+#define LSB_PUBLIC_MASK_LO_OFFSET	0x18
+#define LSB_PUBLIC_MASK_HI_OFFSET	0x1C
+#define LSB_PRIVATE_MASK_LO_OFFSET	0x20
+#define LSB_PRIVATE_MASK_HI_OFFSET	0x24
+
+#define CMD5_Q_CONTROL_BASE		0x0000
+#define CMD5_Q_TAIL_LO_BASE		0x0004
+#define CMD5_Q_HEAD_LO_BASE		0x0008
+#define CMD5_Q_INT_ENABLE_BASE		0x000C
+#define CMD5_Q_INTERRUPT_STATUS_BASE	0x0010
+
+#define CMD5_Q_STATUS_BASE		0x0100
+#define CMD5_Q_INT_STATUS_BASE		0x0104
+#define CMD5_Q_DMA_STATUS_BASE		0x0108
+#define CMD5_Q_DMA_READ_STATUS_BASE	0x010C
+#define CMD5_Q_DMA_WRITE_STATUS_BASE	0x0110
+#define CMD5_Q_ABORT_BASE		0x0114
+#define CMD5_Q_AX_CACHE_BASE		0x0118
+
+#define	CMD5_CONFIG_0_OFFSET		0x6000
+#define	CMD5_TRNG_CTL_OFFSET		0x6008
+#define	CMD5_AES_MASK_OFFSET		0x6010
+#define	CMD5_CLK_GATE_CTL_OFFSET	0x603C
+
+/* Address offset between two virtual queue registers */
+#define CMD5_Q_STATUS_INCR		0x1000
+
+/* Bit masks */
+#define CMD5_Q_RUN			0x1
+#define CMD5_Q_HALT			0x2
+#define CMD5_Q_MEM_LOCATION		0x4
+#define CMD5_Q_SIZE			0x1F
+#define CMD5_Q_SHIFT			3
+#define COMMANDS_PER_QUEUE		16
+#define QUEUE_SIZE_VAL			((ffs(COMMANDS_PER_QUEUE) - 2) & \
+					  CMD5_Q_SIZE)
+#define Q_PTR_MASK			(2 << (QUEUE_SIZE_VAL + 5) - 1)
+#define Q_DESC_SIZE			sizeof(struct ccp5_desc)
+#define Q_SIZE(n)			(COMMANDS_PER_QUEUE*(n))
+
+#define INT_COMPLETION			0x1
+#define INT_ERROR			0x2
+#define INT_QUEUE_STOPPED		0x4
+#define ALL_INTERRUPTS			(INT_COMPLETION| \
+					 INT_ERROR| \
+					 INT_QUEUE_STOPPED)
+
+#define LSB_REGION_WIDTH		5
+#define MAX_LSB_CNT			8
+
+#define LSB_SIZE			16
+#define LSB_ITEM_SIZE			32
+#define PLSB_MAP_SIZE			(LSB_SIZE)
+#define SLSB_MAP_SIZE			(MAX_LSB_CNT * LSB_SIZE)
+
+#define LSB_ENTRY_NUMBER(LSB_ADDR)	(LSB_ADDR / LSB_ITEM_SIZE)
+
+/* ------------------------ CCP Version 3 Specifics ------------------------ */
 #define REQ0_WAIT_FOR_WRITE		0x00000004
 #define REQ0_INT_ON_COMPLETE		0x00000002
 #define REQ0_STOP_ON_COMPLETE		0x00000001
@@ -110,29 +173,30 @@
 #define KSB_START			77
 #define KSB_END				127
 #define KSB_COUNT			(KSB_END - KSB_START + 1)
-#define CCP_KSB_BITS			256
-#define CCP_KSB_BYTES			32
+#define CCP_SB_BITS			256
 
 #define CCP_JOBID_MASK			0x0000003f
 
+/* ------------------------ General CCP Defines ------------------------ */
+
 #define CCP_DMAPOOL_MAX_SIZE		64
 #define CCP_DMAPOOL_ALIGN		BIT(5)
 
 #define CCP_REVERSE_BUF_SIZE		64
 
-#define CCP_AES_KEY_KSB_COUNT		1
-#define CCP_AES_CTX_KSB_COUNT		1
+#define CCP_AES_KEY_SB_COUNT		1
+#define CCP_AES_CTX_SB_COUNT		1
 
-#define CCP_XTS_AES_KEY_KSB_COUNT	1
-#define CCP_XTS_AES_CTX_KSB_COUNT	1
+#define CCP_XTS_AES_KEY_SB_COUNT	1
+#define CCP_XTS_AES_CTX_SB_COUNT	1
 
-#define CCP_SHA_KSB_COUNT		1
+#define CCP_SHA_SB_COUNT		1
 
 #define CCP_RSA_MAX_WIDTH		4096
 
 #define CCP_PASSTHRU_BLOCKSIZE		256
 #define CCP_PASSTHRU_MASKSIZE		32
-#define CCP_PASSTHRU_KSB_COUNT		1
+#define CCP_PASSTHRU_SB_COUNT		1
 
 #define CCP_ECC_MODULUS_BYTES		48      /* 384-bits */
 #define CCP_ECC_MAX_OPERANDS		6
@@ -144,31 +208,12 @@
 #define CCP_ECC_RESULT_OFFSET		60
 #define CCP_ECC_RESULT_SUCCESS		0x0001
 
-struct ccp_op;
-
-/* Structure for computation functions that are device-specific */
-struct ccp_actions {
-	int (*perform_aes)(struct ccp_op *);
-	int (*perform_xts_aes)(struct ccp_op *);
-	int (*perform_sha)(struct ccp_op *);
-	int (*perform_rsa)(struct ccp_op *);
-	int (*perform_passthru)(struct ccp_op *);
-	int (*perform_ecc)(struct ccp_op *);
-	int (*init)(struct ccp_device *);
-	void (*destroy)(struct ccp_device *);
-	irqreturn_t (*irqhandler)(int, void *);
-};
-
-/* Structure to hold CCP version-specific values */
-struct ccp_vdata {
-	unsigned int version;
-	const struct ccp_actions *perform;
-};
-
-extern struct ccp_vdata ccpv3;
+#define CCP_SB_BYTES			32
 
+struct ccp_op;
 struct ccp_device;
 struct ccp_cmd;
+struct ccp_fns;
 
 struct ccp_dma_cmd {
 	struct list_head entry;
@@ -212,9 +257,29 @@ struct ccp_cmd_queue {
 	/* Queue dma pool */
 	struct dma_pool *dma_pool;
 
-	/* Queue reserved KSB regions */
-	u32 ksb_key;
-	u32 ksb_ctx;
+	/* Queue base address (not neccessarily aligned)*/
+	struct ccp5_desc *qbase;
+
+	/* Aligned queue start address (per requirement) */
+	struct mutex q_mutex ____cacheline_aligned;
+	unsigned int qidx;
+
+	/* Version 5 has different requirements for queue memory */
+	unsigned int qsize;
+	dma_addr_t qbase_dma;
+	dma_addr_t qdma_tail;
+
+	/* Per-queue reserved storage block(s) */
+	u32 sb_key;
+	u32 sb_ctx;
+
+	/* Bitmap of LSBs that can be accessed by this queue */
+	DECLARE_BITMAP(lsbmask, MAX_LSB_CNT);
+	/* Private LSB that is assigned to this queue, or -1 if none.
+	 * Bitmap for my private LSB, unused otherwise
+	 */
+	unsigned int lsb;
+	DECLARE_BITMAP(lsbmap, PLSB_MAP_SIZE);
 
 	/* Queue processing thread */
 	struct task_struct *kthread;
@@ -229,8 +294,17 @@ struct ccp_cmd_queue {
 	u32 int_err;
 
 	/* Register addresses for queue */
+	void __iomem *reg_control;
+	void __iomem *reg_tail_lo;
+	void __iomem *reg_head_lo;
+	void __iomem *reg_int_enable;
+	void __iomem *reg_interrupt_status;
 	void __iomem *reg_status;
 	void __iomem *reg_int_status;
+	void __iomem *reg_dma_status;
+	void __iomem *reg_dma_read_status;
+	void __iomem *reg_dma_write_status;
+	u32 qcontrol; /* Cached control register */
 
 	/* Status values from job */
 	u32 int_status;
@@ -253,16 +327,14 @@ struct ccp_device {
 
 	struct device *dev;
 
-	/*
-	 * Bus specific device information
+	/* Bus specific device information
 	 */
 	void *dev_specific;
 	int (*get_irq)(struct ccp_device *ccp);
 	void (*free_irq)(struct ccp_device *ccp);
 	unsigned int irq;
 
-	/*
-	 * I/O area used for device communication. The register mapping
+	/* I/O area used for device communication. The register mapping
 	 * starts at an offset into the mapped bar.
 	 *   The CMD_REQx registers and the Delete_Cmd_Queue_Job register
 	 *   need to be protected while a command queue thread is accessing
@@ -272,8 +344,7 @@ struct ccp_device {
 	void __iomem *io_map;
 	void __iomem *io_regs;
 
-	/*
-	 * Master lists that all cmds are queued on. Because there can be
+	/* Master lists that all cmds are queued on. Because there can be
 	 * more than one CCP command queue that can process a cmd a separate
 	 * backlog list is neeeded so that the backlog completion call
 	 * completes before the cmd is available for execution.
@@ -283,47 +354,54 @@ struct ccp_device {
 	struct list_head cmd;
 	struct list_head backlog;
 
-	/*
-	 * The command queues. These represent the queues available on the
+	/* The command queues. These represent the queues available on the
 	 * CCP that are available for processing cmds
 	 */
 	struct ccp_cmd_queue cmd_q[MAX_HW_QUEUES];
 	unsigned int cmd_q_count;
 
-	/*
-	 * Support for the CCP True RNG
+	/* Support for the CCP True RNG
 	 */
 	struct hwrng hwrng;
 	unsigned int hwrng_retries;
 
-	/*
-	 * Support for the CCP DMA capabilities
+	/* Support for the CCP DMA capabilities
 	 */
 	struct dma_device dma_dev;
 	struct ccp_dma_chan *ccp_dma_chan;
 	struct kmem_cache *dma_cmd_cache;
 	struct kmem_cache *dma_desc_cache;
 
-	/*
-	 * A counter used to generate job-ids for cmds submitted to the CCP
+	/* A counter used to generate job-ids for cmds submitted to the CCP
 	 */
 	atomic_t current_id ____cacheline_aligned;
 
-	/*
-	 * The CCP uses key storage blocks (KSB) to maintain context for certain
-	 * operations. To prevent multiple cmds from using the same KSB range
-	 * a command queue reserves a KSB range for the duration of the cmd.
-	 * Each queue, will however, reserve 2 KSB blocks for operations that
-	 * only require single KSB entries (eg. AES context/iv and key) in order
-	 * to avoid allocation contention.  This will reserve at most 10 KSB
-	 * entries, leaving 40 KSB entries available for dynamic allocation.
+	/* The v3 CCP uses key storage blocks (SB) to maintain context for
+	 * certain operations. To prevent multiple cmds from using the same
+	 * SB range a command queue reserves an SB range for the duration of
+	 * the cmd. Each queue, will however, reserve 2 SB blocks for
+	 * operations that only require single SB entries (eg. AES context/iv
+	 * and key) in order to avoid allocation contention.  This will reserve
+	 * at most 10 SB entries, leaving 40 SB entries available for dynamic
+	 * allocation.
+	 *
+	 * The v5 CCP Local Storage Block (LSB) is broken up into 8
+	 * memrory ranges, each of which can be enabled for access by one
+	 * or more queues. Device initialization takes this into account,
+	 * and attempts to assign one region for exclusive use by each
+	 * available queue; the rest are then aggregated as "public" use.
+	 * If there are fewer regions than queues, all regions are shared
+	 * amongst all queues.
 	 */
-	struct mutex ksb_mutex ____cacheline_aligned;
-	DECLARE_BITMAP(ksb, KSB_COUNT);
-	wait_queue_head_t ksb_queue;
-	unsigned int ksb_avail;
-	unsigned int ksb_count;
-	u32 ksb_start;
+	struct mutex sb_mutex ____cacheline_aligned;
+	DECLARE_BITMAP(sb, KSB_COUNT);
+	wait_queue_head_t sb_queue;
+	unsigned int sb_avail;
+	unsigned int sb_count;
+	u32 sb_start;
+
+	/* Bitmap of shared LSBs, if any */
+	DECLARE_BITMAP(lsbmap, SLSB_MAP_SIZE);
 
 	/* Suspend support */
 	unsigned int suspending;
@@ -335,10 +413,11 @@ struct ccp_device {
 
 enum ccp_memtype {
 	CCP_MEMTYPE_SYSTEM = 0,
-	CCP_MEMTYPE_KSB,
+	CCP_MEMTYPE_SB,
 	CCP_MEMTYPE_LOCAL,
 	CCP_MEMTYPE__LAST,
 };
+#define	CCP_MEMTYPE_LSB	CCP_MEMTYPE_KSB
 
 struct ccp_dma_info {
 	dma_addr_t address;
@@ -379,7 +458,7 @@ struct ccp_mem {
 	enum ccp_memtype type;
 	union {
 		struct ccp_dma_info dma;
-		u32 ksb;
+		u32 sb;
 	} u;
 };
 
@@ -419,13 +498,14 @@ struct ccp_op {
 	u32 jobid;
 	u32 ioc;
 	u32 soc;
-	u32 ksb_key;
-	u32 ksb_ctx;
+	u32 sb_key;
+	u32 sb_ctx;
 	u32 init;
 	u32 eom;
 
 	struct ccp_mem src;
 	struct ccp_mem dst;
+	struct ccp_mem exp;
 
 	union {
 		struct ccp_aes_op aes;
@@ -435,6 +515,7 @@ struct ccp_op {
 		struct ccp_passthru_op passthru;
 		struct ccp_ecc_op ecc;
 	} u;
+	struct ccp_mem key;
 };
 
 static inline u32 ccp_addr_lo(struct ccp_dma_info *info)
@@ -447,6 +528,70 @@ static inline u32 ccp_addr_hi(struct ccp_dma_info *info)
 	return upper_32_bits(info->address + info->offset) & 0x0000ffff;
 }
 
+/**
+ * descriptor for version 5 CPP commands
+ * 8 32-bit words:
+ * word 0: function; engine; control bits
+ * word 1: length of source data
+ * word 2: low 32 bits of source pointer
+ * word 3: upper 16 bits of source pointer; source memory type
+ * word 4: low 32 bits of destination pointer
+ * word 5: upper 16 bits of destination pointer; destination memory type
+ * word 6: low 32 bits of key pointer
+ * word 7: upper 16 bits of key pointer; key memory type
+ */
+struct dword0 {
+	__le32 soc:1;
+	__le32 ioc:1;
+	__le32 rsvd1:1;
+	__le32 init:1;
+	__le32 eom:1;		/* AES/SHA only */
+	__le32 function:15;
+	__le32 engine:4;
+	__le32 prot:1;
+	__le32 rsvd2:7;
+};
+
+struct dword3 {
+	__le32 src_hi:16;
+	__le32 src_mem:2;
+	__le32 lsb_cxt_id:8;
+	__le32 rsvd1:5;
+	__le32 fixed:1;
+};
+
+union dword4 {
+	__le32 dst_lo;		/* NON-SHA	*/
+	__le32 sha_len_lo;	/* SHA		*/
+};
+
+union dword5 {
+	struct {
+		__le32 dst_hi:16;
+		__le32 dst_mem:2;
+		__le32 rsvd1:13;
+		__le32 fixed:1;
+	} fields;
+	__le32 sha_len_hi;
+};
+
+struct dword7 {
+	__le32 key_hi:16;
+	__le32 key_mem:2;
+	__le32 rsvd1:14;
+};
+
+struct ccp5_desc {
+	struct dword0 dw0;
+	__le32 length;
+	__le32 src_lo;
+	struct dword3 dw3;
+	union dword4 dw4;
+	union dword5 dw5;
+	__le32 key_lo;
+	struct dword7 dw7;
+};
+
 int ccp_pci_init(void);
 void ccp_pci_exit(void);
 
@@ -456,13 +601,48 @@ void ccp_platform_exit(void);
 void ccp_add_device(struct ccp_device *ccp);
 void ccp_del_device(struct ccp_device *ccp);
 
+extern void ccp_log_error(struct ccp_device *, int);
+
 struct ccp_device *ccp_alloc_struct(struct device *dev);
 bool ccp_queues_suspended(struct ccp_device *ccp);
 int ccp_cmd_queue_thread(void *data);
+int ccp_trng_read(struct hwrng *rng, void *data, size_t max, bool wait);
 
 int ccp_run_cmd(struct ccp_cmd_queue *cmd_q, struct ccp_cmd *cmd);
 
+int ccp_register_rng(struct ccp_device *ccp);
+void ccp_unregister_rng(struct ccp_device *ccp);
 int ccp_dmaengine_register(struct ccp_device *ccp);
 void ccp_dmaengine_unregister(struct ccp_device *ccp);
 
+/* Structure for computation functions that are device-specific */
+struct ccp_actions {
+	int (*aes)(struct ccp_op *);
+	int (*xts_aes)(struct ccp_op *);
+	int (*sha)(struct ccp_op *);
+	int (*rsa)(struct ccp_op *);
+	int (*passthru)(struct ccp_op *);
+	int (*ecc)(struct ccp_op *);
+	u32 (*sballoc)(struct ccp_cmd_queue *, unsigned int);
+	void (*sbfree)(struct ccp_cmd_queue *, unsigned int,
+			       unsigned int);
+	unsigned int (*get_free_slots)(struct ccp_cmd_queue *);
+	int (*init)(struct ccp_device *);
+	void (*destroy)(struct ccp_device *);
+	irqreturn_t (*irqhandler)(int, void *);
+};
+
+/* Structure to hold CCP version-specific values */
+struct ccp_vdata {
+	const unsigned int version;
+	void (*setup)(struct ccp_device *);
+	const struct ccp_actions *perform;
+	const unsigned int bar;
+	const unsigned int offset;
+};
+
+extern const struct ccp_vdata ccpv3;
+extern const struct ccp_vdata ccpv5a;
+extern const struct ccp_vdata ccpv5b;
+
 #endif
diff --git a/drivers/crypto/ccp/ccp-dmaengine.c b/drivers/crypto/ccp/ccp-dmaengine.c
index 94f77b0f9ae7..6553912804f7 100644
--- a/drivers/crypto/ccp/ccp-dmaengine.c
+++ b/drivers/crypto/ccp/ccp-dmaengine.c
@@ -299,12 +299,10 @@ static struct ccp_dma_desc *ccp_alloc_dma_desc(struct ccp_dma_chan *chan,
 {
 	struct ccp_dma_desc *desc;
 
-	desc = kmem_cache_alloc(chan->ccp->dma_desc_cache, GFP_NOWAIT);
+	desc = kmem_cache_zalloc(chan->ccp->dma_desc_cache, GFP_NOWAIT);
 	if (!desc)
 		return NULL;
 
-	memset(desc, 0, sizeof(*desc));
-
 	dma_async_tx_descriptor_init(&desc->tx_desc, &chan->dma_chan);
 	desc->tx_desc.flags = flags;
 	desc->tx_desc.tx_submit = ccp_tx_submit;
@@ -650,8 +648,11 @@ int ccp_dmaengine_register(struct ccp_device *ccp)
 	dma_desc_cache_name = devm_kasprintf(ccp->dev, GFP_KERNEL,
 					     "%s-dmaengine-desc-cache",
 					     ccp->name);
-	if (!dma_cmd_cache_name)
-		return -ENOMEM;
+	if (!dma_desc_cache_name) {
+		ret = -ENOMEM;
+		goto err_cache;
+	}
+
 	ccp->dma_desc_cache = kmem_cache_create(dma_desc_cache_name,
 						sizeof(struct ccp_dma_desc),
 						sizeof(void *),
diff --git a/drivers/crypto/ccp/ccp-ops.c b/drivers/crypto/ccp/ccp-ops.c
index ffa2891035ac..50fae4442801 100644
--- a/drivers/crypto/ccp/ccp-ops.c
+++ b/drivers/crypto/ccp/ccp-ops.c
@@ -4,6 +4,7 @@
  * Copyright (C) 2013,2016 Advanced Micro Devices, Inc.
  *
  * Author: Tom Lendacky <thomas.lendacky@amd.com>
+ * Author: Gary R Hook <gary.hook@amd.com>
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
@@ -20,72 +21,28 @@
 #include "ccp-dev.h"
 
 /* SHA initial context values */
-static const __be32 ccp_sha1_init[CCP_SHA_CTXSIZE / sizeof(__be32)] = {
+static const __be32 ccp_sha1_init[SHA1_DIGEST_SIZE / sizeof(__be32)] = {
 	cpu_to_be32(SHA1_H0), cpu_to_be32(SHA1_H1),
 	cpu_to_be32(SHA1_H2), cpu_to_be32(SHA1_H3),
-	cpu_to_be32(SHA1_H4), 0, 0, 0,
+	cpu_to_be32(SHA1_H4),
 };
 
-static const __be32 ccp_sha224_init[CCP_SHA_CTXSIZE / sizeof(__be32)] = {
+static const __be32 ccp_sha224_init[SHA256_DIGEST_SIZE / sizeof(__be32)] = {
 	cpu_to_be32(SHA224_H0), cpu_to_be32(SHA224_H1),
 	cpu_to_be32(SHA224_H2), cpu_to_be32(SHA224_H3),
 	cpu_to_be32(SHA224_H4), cpu_to_be32(SHA224_H5),
 	cpu_to_be32(SHA224_H6), cpu_to_be32(SHA224_H7),
 };
 
-static const __be32 ccp_sha256_init[CCP_SHA_CTXSIZE / sizeof(__be32)] = {
+static const __be32 ccp_sha256_init[SHA256_DIGEST_SIZE / sizeof(__be32)] = {
 	cpu_to_be32(SHA256_H0), cpu_to_be32(SHA256_H1),
 	cpu_to_be32(SHA256_H2), cpu_to_be32(SHA256_H3),
 	cpu_to_be32(SHA256_H4), cpu_to_be32(SHA256_H5),
 	cpu_to_be32(SHA256_H6), cpu_to_be32(SHA256_H7),
 };
 
-static u32 ccp_alloc_ksb(struct ccp_device *ccp, unsigned int count)
-{
-	int start;
-
-	for (;;) {
-		mutex_lock(&ccp->ksb_mutex);
-
-		start = (u32)bitmap_find_next_zero_area(ccp->ksb,
-							ccp->ksb_count,
-							ccp->ksb_start,
-							count, 0);
-		if (start <= ccp->ksb_count) {
-			bitmap_set(ccp->ksb, start, count);
-
-			mutex_unlock(&ccp->ksb_mutex);
-			break;
-		}
-
-		ccp->ksb_avail = 0;
-
-		mutex_unlock(&ccp->ksb_mutex);
-
-		/* Wait for KSB entries to become available */
-		if (wait_event_interruptible(ccp->ksb_queue, ccp->ksb_avail))
-			return 0;
-	}
-
-	return KSB_START + start;
-}
-
-static void ccp_free_ksb(struct ccp_device *ccp, unsigned int start,
-			 unsigned int count)
-{
-	if (!start)
-		return;
-
-	mutex_lock(&ccp->ksb_mutex);
-
-	bitmap_clear(ccp->ksb, start - KSB_START, count);
-
-	ccp->ksb_avail = 1;
-
-	mutex_unlock(&ccp->ksb_mutex);
-
-	wake_up_interruptible_all(&ccp->ksb_queue);
-}
+#define	CCP_NEW_JOBID(ccp)	((ccp->vdata->version == CCP_VERSION(3, 0)) ? \
+					ccp_gen_jobid(ccp) : 0)
 
 static u32 ccp_gen_jobid(struct ccp_device *ccp)
 {
@@ -231,7 +188,7 @@ static int ccp_reverse_set_dm_area(struct ccp_dm_workarea *wa,
 				   unsigned int len, unsigned int se_len,
 				   bool sign_extend)
 {
-	unsigned int nbytes, sg_offset, dm_offset, ksb_len, i;
+	unsigned int nbytes, sg_offset, dm_offset, sb_len, i;
 	u8 buffer[CCP_REVERSE_BUF_SIZE];
 
 	if (WARN_ON(se_len > sizeof(buffer)))
@@ -241,21 +198,21 @@ static int ccp_reverse_set_dm_area(struct ccp_dm_workarea *wa,
 	dm_offset = 0;
 	nbytes = len;
 	while (nbytes) {
-		ksb_len = min_t(unsigned int, nbytes, se_len);
-		sg_offset -= ksb_len;
+		sb_len = min_t(unsigned int, nbytes, se_len);
+		sg_offset -= sb_len;
 
-		scatterwalk_map_and_copy(buffer, sg, sg_offset, ksb_len, 0);
-		for (i = 0; i < ksb_len; i++)
-			wa->address[dm_offset + i] = buffer[ksb_len - i - 1];
+		scatterwalk_map_and_copy(buffer, sg, sg_offset, sb_len, 0);
+		for (i = 0; i < sb_len; i++)
+			wa->address[dm_offset + i] = buffer[sb_len - i - 1];
 
-		dm_offset += ksb_len;
-		nbytes -= ksb_len;
+		dm_offset += sb_len;
+		nbytes -= sb_len;
 
-		if ((ksb_len != se_len) && sign_extend) {
+		if ((sb_len != se_len) && sign_extend) {
 			/* Must sign-extend to nearest sign-extend length */
 			if (wa->address[dm_offset - 1] & 0x80)
 				memset(wa->address + dm_offset, 0xff,
-				       se_len - ksb_len);
+				       se_len - sb_len);
 		}
 	}
 
@@ -266,22 +223,22 @@ static void ccp_reverse_get_dm_area(struct ccp_dm_workarea *wa,
 				    struct scatterlist *sg,
 				    unsigned int len)
 {
-	unsigned int nbytes, sg_offset, dm_offset, ksb_len, i;
+	unsigned int nbytes, sg_offset, dm_offset, sb_len, i;
 	u8 buffer[CCP_REVERSE_BUF_SIZE];
 
 	sg_offset = 0;
 	dm_offset = len;
 	nbytes = len;
 	while (nbytes) {
-		ksb_len = min_t(unsigned int, nbytes, sizeof(buffer));
-		dm_offset -= ksb_len;
+		sb_len = min_t(unsigned int, nbytes, sizeof(buffer));
+		dm_offset -= sb_len;
 
-		for (i = 0; i < ksb_len; i++)
-			buffer[ksb_len - i - 1] = wa->address[dm_offset + i];
-		scatterwalk_map_and_copy(buffer, sg, sg_offset, ksb_len, 1);
+		for (i = 0; i < sb_len; i++)
+			buffer[sb_len - i - 1] = wa->address[dm_offset + i];
+		scatterwalk_map_and_copy(buffer, sg, sg_offset, sb_len, 1);
 
-		sg_offset += ksb_len;
-		nbytes -= ksb_len;
+		sg_offset += sb_len;
+		nbytes -= sb_len;
 	}
 }
 
@@ -449,9 +406,9 @@ static void ccp_process_data(struct ccp_data *src, struct ccp_data *dst,
 	}
 }
 
-static int ccp_copy_to_from_ksb(struct ccp_cmd_queue *cmd_q,
-				struct ccp_dm_workarea *wa, u32 jobid, u32 ksb,
-				u32 byte_swap, bool from)
+static int ccp_copy_to_from_sb(struct ccp_cmd_queue *cmd_q,
+			       struct ccp_dm_workarea *wa, u32 jobid, u32 sb,
+			       u32 byte_swap, bool from)
 {
 	struct ccp_op op;
 
@@ -463,8 +420,8 @@ static int ccp_copy_to_from_ksb(struct ccp_cmd_queue *cmd_q,
 
 	if (from) {
 		op.soc = 1;
-		op.src.type = CCP_MEMTYPE_KSB;
-		op.src.u.ksb = ksb;
+		op.src.type = CCP_MEMTYPE_SB;
+		op.src.u.sb = sb;
 		op.dst.type = CCP_MEMTYPE_SYSTEM;
 		op.dst.u.dma.address = wa->dma.address;
 		op.dst.u.dma.length = wa->length;
@@ -472,27 +429,27 @@ static int ccp_copy_to_from_ksb(struct ccp_cmd_queue *cmd_q,
 		op.src.type = CCP_MEMTYPE_SYSTEM;
 		op.src.u.dma.address = wa->dma.address;
 		op.src.u.dma.length = wa->length;
-		op.dst.type = CCP_MEMTYPE_KSB;
-		op.dst.u.ksb = ksb;
+		op.dst.type = CCP_MEMTYPE_SB;
+		op.dst.u.sb = sb;
 	}
 
 	op.u.passthru.byte_swap = byte_swap;
 
-	return cmd_q->ccp->vdata->perform->perform_passthru(&op);
+	return cmd_q->ccp->vdata->perform->passthru(&op);
 }
 
-static int ccp_copy_to_ksb(struct ccp_cmd_queue *cmd_q,
-			   struct ccp_dm_workarea *wa, u32 jobid, u32 ksb,
-			   u32 byte_swap)
+static int ccp_copy_to_sb(struct ccp_cmd_queue *cmd_q,
+			  struct ccp_dm_workarea *wa, u32 jobid, u32 sb,
+			  u32 byte_swap)
 {
-	return ccp_copy_to_from_ksb(cmd_q, wa, jobid, ksb, byte_swap, false);
+	return ccp_copy_to_from_sb(cmd_q, wa, jobid, sb, byte_swap, false);
 }
 
-static int ccp_copy_from_ksb(struct ccp_cmd_queue *cmd_q,
-			     struct ccp_dm_workarea *wa, u32 jobid, u32 ksb,
-			     u32 byte_swap)
+static int ccp_copy_from_sb(struct ccp_cmd_queue *cmd_q,
+			    struct ccp_dm_workarea *wa, u32 jobid, u32 sb,
+			    u32 byte_swap)
 {
-	return ccp_copy_to_from_ksb(cmd_q, wa, jobid, ksb, byte_swap, true);
+	return ccp_copy_to_from_sb(cmd_q, wa, jobid, sb, byte_swap, true);
 }
 
 static int ccp_run_aes_cmac_cmd(struct ccp_cmd_queue *cmd_q,
@@ -527,54 +484,54 @@ static int ccp_run_aes_cmac_cmd(struct ccp_cmd_queue *cmd_q,
 			return -EINVAL;
 	}
 
-	BUILD_BUG_ON(CCP_AES_KEY_KSB_COUNT != 1);
-	BUILD_BUG_ON(CCP_AES_CTX_KSB_COUNT != 1);
+	BUILD_BUG_ON(CCP_AES_KEY_SB_COUNT != 1);
+	BUILD_BUG_ON(CCP_AES_CTX_SB_COUNT != 1);
 
 	ret = -EIO;
 	memset(&op, 0, sizeof(op));
 	op.cmd_q = cmd_q;
-	op.jobid = ccp_gen_jobid(cmd_q->ccp);
-	op.ksb_key = cmd_q->ksb_key;
-	op.ksb_ctx = cmd_q->ksb_ctx;
+	op.jobid = CCP_NEW_JOBID(cmd_q->ccp);
+	op.sb_key = cmd_q->sb_key;
+	op.sb_ctx = cmd_q->sb_ctx;
 	op.init = 1;
 	op.u.aes.type = aes->type;
 	op.u.aes.mode = aes->mode;
 	op.u.aes.action = aes->action;
 
-	/* All supported key sizes fit in a single (32-byte) KSB entry
+	/* All supported key sizes fit in a single (32-byte) SB entry
 	 * and must be in little endian format. Use the 256-bit byte
 	 * swap passthru option to convert from big endian to little
 	 * endian.
 	 */
 	ret = ccp_init_dm_workarea(&key, cmd_q,
-				   CCP_AES_KEY_KSB_COUNT * CCP_KSB_BYTES,
+				   CCP_AES_KEY_SB_COUNT * CCP_SB_BYTES,
 				   DMA_TO_DEVICE);
 	if (ret)
 		return ret;
 
-	dm_offset = CCP_KSB_BYTES - aes->key_len;
+	dm_offset = CCP_SB_BYTES - aes->key_len;
 	ccp_set_dm_area(&key, dm_offset, aes->key, 0, aes->key_len);
-	ret = ccp_copy_to_ksb(cmd_q, &key, op.jobid, op.ksb_key,
-			      CCP_PASSTHRU_BYTESWAP_256BIT);
+	ret = ccp_copy_to_sb(cmd_q, &key, op.jobid, op.sb_key,
+			     CCP_PASSTHRU_BYTESWAP_256BIT);
 	if (ret) {
 		cmd->engine_error = cmd_q->cmd_error;
 		goto e_key;
 	}
 
-	/* The AES context fits in a single (32-byte) KSB entry and
+	/* The AES context fits in a single (32-byte) SB entry and
 	 * must be in little endian format. Use the 256-bit byte swap
 	 * passthru option to convert from big endian to little endian.
 	 */
 	ret = ccp_init_dm_workarea(&ctx, cmd_q,
-				   CCP_AES_CTX_KSB_COUNT * CCP_KSB_BYTES,
+				   CCP_AES_CTX_SB_COUNT * CCP_SB_BYTES,
 				   DMA_BIDIRECTIONAL);
 	if (ret)
 		goto e_key;
 
-	dm_offset = CCP_KSB_BYTES - AES_BLOCK_SIZE;
+	dm_offset = CCP_SB_BYTES - AES_BLOCK_SIZE;
 	ccp_set_dm_area(&ctx, dm_offset, aes->iv, 0, aes->iv_len);
-	ret = ccp_copy_to_ksb(cmd_q, &ctx, op.jobid, op.ksb_ctx,
-			      CCP_PASSTHRU_BYTESWAP_256BIT);
+	ret = ccp_copy_to_sb(cmd_q, &ctx, op.jobid, op.sb_ctx,
+			     CCP_PASSTHRU_BYTESWAP_256BIT);
 	if (ret) {
 		cmd->engine_error = cmd_q->cmd_error;
 		goto e_ctx;
@@ -592,9 +549,9 @@ static int ccp_run_aes_cmac_cmd(struct ccp_cmd_queue *cmd_q,
 			op.eom = 1;
 
 			/* Push the K1/K2 key to the CCP now */
-			ret = ccp_copy_from_ksb(cmd_q, &ctx, op.jobid,
-						op.ksb_ctx,
-						CCP_PASSTHRU_BYTESWAP_256BIT);
+			ret = ccp_copy_from_sb(cmd_q, &ctx, op.jobid,
+					       op.sb_ctx,
+					       CCP_PASSTHRU_BYTESWAP_256BIT);
 			if (ret) {
 				cmd->engine_error = cmd_q->cmd_error;
 				goto e_src;
@@ -602,15 +559,15 @@ static int ccp_run_aes_cmac_cmd(struct ccp_cmd_queue *cmd_q,
 
 			ccp_set_dm_area(&ctx, 0, aes->cmac_key, 0,
 					aes->cmac_key_len);
-			ret = ccp_copy_to_ksb(cmd_q, &ctx, op.jobid, op.ksb_ctx,
-					      CCP_PASSTHRU_BYTESWAP_256BIT);
+			ret = ccp_copy_to_sb(cmd_q, &ctx, op.jobid, op.sb_ctx,
+					     CCP_PASSTHRU_BYTESWAP_256BIT);
 			if (ret) {
 				cmd->engine_error = cmd_q->cmd_error;
 				goto e_src;
 			}
 		}
 
-		ret = cmd_q->ccp->vdata->perform->perform_aes(&op);
+		ret = cmd_q->ccp->vdata->perform->aes(&op);
 		if (ret) {
 			cmd->engine_error = cmd_q->cmd_error;
 			goto e_src;
@@ -622,15 +579,15 @@ static int ccp_run_aes_cmac_cmd(struct ccp_cmd_queue *cmd_q,
 	/* Retrieve the AES context - convert from LE to BE using
 	 * 32-byte (256-bit) byteswapping
 	 */
-	ret = ccp_copy_from_ksb(cmd_q, &ctx, op.jobid, op.ksb_ctx,
-				CCP_PASSTHRU_BYTESWAP_256BIT);
+	ret = ccp_copy_from_sb(cmd_q, &ctx, op.jobid, op.sb_ctx,
+			       CCP_PASSTHRU_BYTESWAP_256BIT);
 	if (ret) {
 		cmd->engine_error = cmd_q->cmd_error;
 		goto e_src;
 	}
 
 	/* ...but we only need AES_BLOCK_SIZE bytes */
-	dm_offset = CCP_KSB_BYTES - AES_BLOCK_SIZE;
+	dm_offset = CCP_SB_BYTES - AES_BLOCK_SIZE;
 	ccp_get_dm_area(&ctx, dm_offset, aes->iv, 0, aes->iv_len);
 
 e_src:
@@ -680,56 +637,56 @@ static int ccp_run_aes_cmd(struct ccp_cmd_queue *cmd_q, struct ccp_cmd *cmd)
 			return -EINVAL;
 	}
 
-	BUILD_BUG_ON(CCP_AES_KEY_KSB_COUNT != 1);
-	BUILD_BUG_ON(CCP_AES_CTX_KSB_COUNT != 1);
+	BUILD_BUG_ON(CCP_AES_KEY_SB_COUNT != 1);
+	BUILD_BUG_ON(CCP_AES_CTX_SB_COUNT != 1);
 
 	ret = -EIO;
 	memset(&op, 0, sizeof(op));
 	op.cmd_q = cmd_q;
-	op.jobid = ccp_gen_jobid(cmd_q->ccp);
-	op.ksb_key = cmd_q->ksb_key;
-	op.ksb_ctx = cmd_q->ksb_ctx;
+	op.jobid = CCP_NEW_JOBID(cmd_q->ccp);
+	op.sb_key = cmd_q->sb_key;
+	op.sb_ctx = cmd_q->sb_ctx;
 	op.init = (aes->mode == CCP_AES_MODE_ECB) ? 0 : 1;
 	op.u.aes.type = aes->type;
 	op.u.aes.mode = aes->mode;
 	op.u.aes.action = aes->action;
 
-	/* All supported key sizes fit in a single (32-byte) KSB entry
+	/* All supported key sizes fit in a single (32-byte) SB entry
 	 * and must be in little endian format. Use the 256-bit byte
 	 * swap passthru option to convert from big endian to little
 	 * endian.
 	 */
 	ret = ccp_init_dm_workarea(&key, cmd_q,
-				   CCP_AES_KEY_KSB_COUNT * CCP_KSB_BYTES,
+				   CCP_AES_KEY_SB_COUNT * CCP_SB_BYTES,
 				   DMA_TO_DEVICE);
 	if (ret)
 		return ret;
 
-	dm_offset = CCP_KSB_BYTES - aes->key_len;
+	dm_offset = CCP_SB_BYTES - aes->key_len;
 	ccp_set_dm_area(&key, dm_offset, aes->key, 0, aes->key_len);
-	ret = ccp_copy_to_ksb(cmd_q, &key, op.jobid, op.ksb_key,
-			      CCP_PASSTHRU_BYTESWAP_256BIT);
+	ret = ccp_copy_to_sb(cmd_q, &key, op.jobid, op.sb_key,
+			     CCP_PASSTHRU_BYTESWAP_256BIT);
 	if (ret) {
 		cmd->engine_error = cmd_q->cmd_error;
 		goto e_key;
 	}
 
-	/* The AES context fits in a single (32-byte) KSB entry and
+	/* The AES context fits in a single (32-byte) SB entry and
 	 * must be in little endian format. Use the 256-bit byte swap
 	 * passthru option to convert from big endian to little endian.
 	 */
 	ret = ccp_init_dm_workarea(&ctx, cmd_q,
-				   CCP_AES_CTX_KSB_COUNT * CCP_KSB_BYTES,
+				   CCP_AES_CTX_SB_COUNT * CCP_SB_BYTES,
 				   DMA_BIDIRECTIONAL);
 	if (ret)
 		goto e_key;
 
 	if (aes->mode != CCP_AES_MODE_ECB) {
-		/* Load the AES context - conver to LE */
-		dm_offset = CCP_KSB_BYTES - AES_BLOCK_SIZE;
+		/* Load the AES context - convert to LE */
+		dm_offset = CCP_SB_BYTES - AES_BLOCK_SIZE;
 		ccp_set_dm_area(&ctx, dm_offset, aes->iv, 0, aes->iv_len);
-		ret = ccp_copy_to_ksb(cmd_q, &ctx, op.jobid, op.ksb_ctx,
-				      CCP_PASSTHRU_BYTESWAP_256BIT);
+		ret = ccp_copy_to_sb(cmd_q, &ctx, op.jobid, op.sb_ctx,
+				     CCP_PASSTHRU_BYTESWAP_256BIT);
 		if (ret) {
 			cmd->engine_error = cmd_q->cmd_error;
 			goto e_ctx;
@@ -772,7 +729,7 @@ static int ccp_run_aes_cmd(struct ccp_cmd_queue *cmd_q, struct ccp_cmd *cmd)
 				op.soc = 1;
 		}
 
-		ret = cmd_q->ccp->vdata->perform->perform_aes(&op);
+		ret = cmd_q->ccp->vdata->perform->aes(&op);
 		if (ret) {
 			cmd->engine_error = cmd_q->cmd_error;
 			goto e_dst;
@@ -785,15 +742,15 @@ static int ccp_run_aes_cmd(struct ccp_cmd_queue *cmd_q, struct ccp_cmd *cmd)
 		/* Retrieve the AES context - convert from LE to BE using
 		 * 32-byte (256-bit) byteswapping
 		 */
-		ret = ccp_copy_from_ksb(cmd_q, &ctx, op.jobid, op.ksb_ctx,
-					CCP_PASSTHRU_BYTESWAP_256BIT);
+		ret = ccp_copy_from_sb(cmd_q, &ctx, op.jobid, op.sb_ctx,
+				       CCP_PASSTHRU_BYTESWAP_256BIT);
 		if (ret) {
 			cmd->engine_error = cmd_q->cmd_error;
 			goto e_dst;
 		}
 
 		/* ...but we only need AES_BLOCK_SIZE bytes */
-		dm_offset = CCP_KSB_BYTES - AES_BLOCK_SIZE;
+		dm_offset = CCP_SB_BYTES - AES_BLOCK_SIZE;
 		ccp_get_dm_area(&ctx, dm_offset, aes->iv, 0, aes->iv_len);
 	}
 
@@ -857,53 +814,53 @@ static int ccp_run_xts_aes_cmd(struct ccp_cmd_queue *cmd_q,
 	if (!xts->key || !xts->iv || !xts->src || !xts->dst)
 		return -EINVAL;
 
-	BUILD_BUG_ON(CCP_XTS_AES_KEY_KSB_COUNT != 1);
-	BUILD_BUG_ON(CCP_XTS_AES_CTX_KSB_COUNT != 1);
+	BUILD_BUG_ON(CCP_XTS_AES_KEY_SB_COUNT != 1);
+	BUILD_BUG_ON(CCP_XTS_AES_CTX_SB_COUNT != 1);
 
 	ret = -EIO;
 	memset(&op, 0, sizeof(op));
 	op.cmd_q = cmd_q;
-	op.jobid = ccp_gen_jobid(cmd_q->ccp);
-	op.ksb_key = cmd_q->ksb_key;
-	op.ksb_ctx = cmd_q->ksb_ctx;
+	op.jobid = CCP_NEW_JOBID(cmd_q->ccp);
+	op.sb_key = cmd_q->sb_key;
+	op.sb_ctx = cmd_q->sb_ctx;
 	op.init = 1;
 	op.u.xts.action = xts->action;
 	op.u.xts.unit_size = xts->unit_size;
 
-	/* All supported key sizes fit in a single (32-byte) KSB entry
+	/* All supported key sizes fit in a single (32-byte) SB entry
 	 * and must be in little endian format. Use the 256-bit byte
 	 * swap passthru option to convert from big endian to little
 	 * endian.
 	 */
 	ret = ccp_init_dm_workarea(&key, cmd_q,
-				   CCP_XTS_AES_KEY_KSB_COUNT * CCP_KSB_BYTES,
+				   CCP_XTS_AES_KEY_SB_COUNT * CCP_SB_BYTES,
 				   DMA_TO_DEVICE);
 	if (ret)
 		return ret;
 
-	dm_offset = CCP_KSB_BYTES - AES_KEYSIZE_128;
+	dm_offset = CCP_SB_BYTES - AES_KEYSIZE_128;
 	ccp_set_dm_area(&key, dm_offset, xts->key, 0, xts->key_len);
 	ccp_set_dm_area(&key, 0, xts->key, dm_offset, xts->key_len);
-	ret = ccp_copy_to_ksb(cmd_q, &key, op.jobid, op.ksb_key,
-			      CCP_PASSTHRU_BYTESWAP_256BIT);
+	ret = ccp_copy_to_sb(cmd_q, &key, op.jobid, op.sb_key,
+			     CCP_PASSTHRU_BYTESWAP_256BIT);
 	if (ret) {
 		cmd->engine_error = cmd_q->cmd_error;
 		goto e_key;
 	}
 
-	/* The AES context fits in a single (32-byte) KSB entry and
+	/* The AES context fits in a single (32-byte) SB entry and
 	 * for XTS is already in little endian format so no byte swapping
 	 * is needed.
 	 */
 	ret = ccp_init_dm_workarea(&ctx, cmd_q,
-				   CCP_XTS_AES_CTX_KSB_COUNT * CCP_KSB_BYTES,
+				   CCP_XTS_AES_CTX_SB_COUNT * CCP_SB_BYTES,
 				   DMA_BIDIRECTIONAL);
 	if (ret)
 		goto e_key;
 
 	ccp_set_dm_area(&ctx, 0, xts->iv, 0, xts->iv_len);
-	ret = ccp_copy_to_ksb(cmd_q, &ctx, op.jobid, op.ksb_ctx,
-			      CCP_PASSTHRU_BYTESWAP_NOOP);
+	ret = ccp_copy_to_sb(cmd_q, &ctx, op.jobid, op.sb_ctx,
+			     CCP_PASSTHRU_BYTESWAP_NOOP);
 	if (ret) {
 		cmd->engine_error = cmd_q->cmd_error;
 		goto e_ctx;
@@ -937,7 +894,7 @@ static int ccp_run_xts_aes_cmd(struct ccp_cmd_queue *cmd_q,
 		if (!src.sg_wa.bytes_left)
 			op.eom = 1;
 
-		ret = cmd_q->ccp->vdata->perform->perform_xts_aes(&op);
+		ret = cmd_q->ccp->vdata->perform->xts_aes(&op);
 		if (ret) {
 			cmd->engine_error = cmd_q->cmd_error;
 			goto e_dst;
@@ -949,15 +906,15 @@ static int ccp_run_xts_aes_cmd(struct ccp_cmd_queue *cmd_q,
 	/* Retrieve the AES context - convert from LE to BE using
 	 * 32-byte (256-bit) byteswapping
 	 */
-	ret = ccp_copy_from_ksb(cmd_q, &ctx, op.jobid, op.ksb_ctx,
-				CCP_PASSTHRU_BYTESWAP_256BIT);
+	ret = ccp_copy_from_sb(cmd_q, &ctx, op.jobid, op.sb_ctx,
+			       CCP_PASSTHRU_BYTESWAP_256BIT);
 	if (ret) {
 		cmd->engine_error = cmd_q->cmd_error;
 		goto e_dst;
 	}
 
 	/* ...but we only need AES_BLOCK_SIZE bytes */
-	dm_offset = CCP_KSB_BYTES - AES_BLOCK_SIZE;
+	dm_offset = CCP_SB_BYTES - AES_BLOCK_SIZE;
 	ccp_get_dm_area(&ctx, dm_offset, xts->iv, 0, xts->iv_len);
 
 e_dst:
@@ -982,163 +939,227 @@ static int ccp_run_sha_cmd(struct ccp_cmd_queue *cmd_q, struct ccp_cmd *cmd)
 	struct ccp_dm_workarea ctx;
 	struct ccp_data src;
 	struct ccp_op op;
+	unsigned int ioffset, ooffset;
+	unsigned int digest_size;
+	int sb_count;
+	const void *init;
+	u64 block_size;
+	int ctx_size;
 	int ret;
 
-	if (sha->ctx_len != CCP_SHA_CTXSIZE)
+	switch (sha->type) {
+	case CCP_SHA_TYPE_1:
+		if (sha->ctx_len < SHA1_DIGEST_SIZE)
+			return -EINVAL;
+		block_size = SHA1_BLOCK_SIZE;
+		break;
+	case CCP_SHA_TYPE_224:
+		if (sha->ctx_len < SHA224_DIGEST_SIZE)
+			return -EINVAL;
+		block_size = SHA224_BLOCK_SIZE;
+		break;
+	case CCP_SHA_TYPE_256:
+		if (sha->ctx_len < SHA256_DIGEST_SIZE)
+			return -EINVAL;
+		block_size = SHA256_BLOCK_SIZE;
+		break;
+	default:
 		return -EINVAL;
+	}
 
 	if (!sha->ctx)
 		return -EINVAL;
 
-	if (!sha->final && (sha->src_len & (CCP_SHA_BLOCKSIZE - 1)))
+	if (!sha->final && (sha->src_len & (block_size - 1)))
 		return -EINVAL;
 
-	if (!sha->src_len) {
-		const u8 *sha_zero;
+	/* The version 3 device can't handle zero-length input */
+	if (cmd_q->ccp->vdata->version == CCP_VERSION(3, 0)) {
 
-		/* Not final, just return */
-		if (!sha->final)
-			return 0;
+		if (!sha->src_len) {
+			unsigned int digest_len;
+			const u8 *sha_zero;
 
-		/* CCP can't do a zero length sha operation so the caller
-		 * must buffer the data.
-		 */
-		if (sha->msg_bits)
-			return -EINVAL;
+			/* Not final, just return */
+			if (!sha->final)
+				return 0;
 
-		/* The CCP cannot perform zero-length sha operations so the
-		 * caller is required to buffer data for the final operation.
-		 * However, a sha operation for a message with a total length
-		 * of zero is valid so known values are required to supply
-		 * the result.
-		 */
-		switch (sha->type) {
-		case CCP_SHA_TYPE_1:
-			sha_zero = sha1_zero_message_hash;
-			break;
-		case CCP_SHA_TYPE_224:
-			sha_zero = sha224_zero_message_hash;
-			break;
-		case CCP_SHA_TYPE_256:
-			sha_zero = sha256_zero_message_hash;
-			break;
-		default:
-			return -EINVAL;
-		}
+			/* CCP can't do a zero length sha operation so the
+			 * caller must buffer the data.
+			 */
+			if (sha->msg_bits)
+				return -EINVAL;
+
+			/* The CCP cannot perform zero-length sha operations
+			 * so the caller is required to buffer data for the
+			 * final operation. However, a sha operation for a
+			 * message with a total length of zero is valid so
+			 * known values are required to supply the result.
+			 */
+			switch (sha->type) {
+			case CCP_SHA_TYPE_1:
+				sha_zero = sha1_zero_message_hash;
+				digest_len = SHA1_DIGEST_SIZE;
+				break;
+			case CCP_SHA_TYPE_224:
+				sha_zero = sha224_zero_message_hash;
+				digest_len = SHA224_DIGEST_SIZE;
+				break;
+			case CCP_SHA_TYPE_256:
+				sha_zero = sha256_zero_message_hash;
+				digest_len = SHA256_DIGEST_SIZE;
+				break;
+			default:
+				return -EINVAL;
+			}
 
-		scatterwalk_map_and_copy((void *)sha_zero, sha->ctx, 0,
-					 sha->ctx_len, 1);
+			scatterwalk_map_and_copy((void *)sha_zero, sha->ctx, 0,
+						 digest_len, 1);
 
-		return 0;
+			return 0;
+		}
 	}
 
-	if (!sha->src)
-		return -EINVAL;
+	/* Set variables used throughout */
+	switch (sha->type) {
+	case CCP_SHA_TYPE_1:
+		digest_size = SHA1_DIGEST_SIZE;
+		init = (void *) ccp_sha1_init;
+		ctx_size = SHA1_DIGEST_SIZE;
+		sb_count = 1;
+		if (cmd_q->ccp->vdata->version != CCP_VERSION(3, 0))
+			ooffset = ioffset = CCP_SB_BYTES - SHA1_DIGEST_SIZE;
+		else
+			ooffset = ioffset = 0;
+		break;
+	case CCP_SHA_TYPE_224:
+		digest_size = SHA224_DIGEST_SIZE;
+		init = (void *) ccp_sha224_init;
+		ctx_size = SHA256_DIGEST_SIZE;
+		sb_count = 1;
+		ioffset = 0;
+		if (cmd_q->ccp->vdata->version != CCP_VERSION(3, 0))
+			ooffset = CCP_SB_BYTES - SHA224_DIGEST_SIZE;
+		else
+			ooffset = 0;
+		break;
+	case CCP_SHA_TYPE_256:
+		digest_size = SHA256_DIGEST_SIZE;
+		init = (void *) ccp_sha256_init;
+		ctx_size = SHA256_DIGEST_SIZE;
+		sb_count = 1;
+		ooffset = ioffset = 0;
+		break;
+	default:
+		ret = -EINVAL;
+		goto e_data;
+	}
 
-	BUILD_BUG_ON(CCP_SHA_KSB_COUNT != 1);
+	/* For zero-length plaintext the src pointer is ignored;
+	 * otherwise both parts must be valid
+	 */
+	if (sha->src_len && !sha->src)
+		return -EINVAL;
 
 	memset(&op, 0, sizeof(op));
 	op.cmd_q = cmd_q;
-	op.jobid = ccp_gen_jobid(cmd_q->ccp);
-	op.ksb_ctx = cmd_q->ksb_ctx;
+	op.jobid = CCP_NEW_JOBID(cmd_q->ccp);
+	op.sb_ctx = cmd_q->sb_ctx; /* Pre-allocated */
 	op.u.sha.type = sha->type;
 	op.u.sha.msg_bits = sha->msg_bits;
 
-	/* The SHA context fits in a single (32-byte) KSB entry and
-	 * must be in little endian format. Use the 256-bit byte swap
-	 * passthru option to convert from big endian to little endian.
-	 */
-	ret = ccp_init_dm_workarea(&ctx, cmd_q,
-				   CCP_SHA_KSB_COUNT * CCP_KSB_BYTES,
+	ret = ccp_init_dm_workarea(&ctx, cmd_q, sb_count * CCP_SB_BYTES,
 				   DMA_BIDIRECTIONAL);
 	if (ret)
 		return ret;
-
 	if (sha->first) {
-		const __be32 *init;
-
 		switch (sha->type) {
 		case CCP_SHA_TYPE_1:
-			init = ccp_sha1_init;
-			break;
 		case CCP_SHA_TYPE_224:
-			init = ccp_sha224_init;
-			break;
 		case CCP_SHA_TYPE_256:
-			init = ccp_sha256_init;
+			memcpy(ctx.address + ioffset, init, ctx_size);
 			break;
 		default:
 			ret = -EINVAL;
 			goto e_ctx;
 		}
-		memcpy(ctx.address, init, CCP_SHA_CTXSIZE);
 	} else {
-		ccp_set_dm_area(&ctx, 0, sha->ctx, 0, sha->ctx_len);
+		/* Restore the context */
+		ccp_set_dm_area(&ctx, 0, sha->ctx, 0,
+				sb_count * CCP_SB_BYTES);
 	}
 
-	ret = ccp_copy_to_ksb(cmd_q, &ctx, op.jobid, op.ksb_ctx,
-			      CCP_PASSTHRU_BYTESWAP_256BIT);
+	ret = ccp_copy_to_sb(cmd_q, &ctx, op.jobid, op.sb_ctx,
+			     CCP_PASSTHRU_BYTESWAP_256BIT);
 	if (ret) {
 		cmd->engine_error = cmd_q->cmd_error;
 		goto e_ctx;
 	}
 
-	/* Send data to the CCP SHA engine */
-	ret = ccp_init_data(&src, cmd_q, sha->src, sha->src_len,
-			    CCP_SHA_BLOCKSIZE, DMA_TO_DEVICE);
-	if (ret)
-		goto e_ctx;
+	if (sha->src) {
+		/* Send data to the CCP SHA engine; block_size is set above */
+		ret = ccp_init_data(&src, cmd_q, sha->src, sha->src_len,
+				    block_size, DMA_TO_DEVICE);
+		if (ret)
+			goto e_ctx;
 
-	while (src.sg_wa.bytes_left) {
-		ccp_prepare_data(&src, NULL, &op, CCP_SHA_BLOCKSIZE, false);
-		if (sha->final && !src.sg_wa.bytes_left)
-			op.eom = 1;
+		while (src.sg_wa.bytes_left) {
+			ccp_prepare_data(&src, NULL, &op, block_size, false);
+			if (sha->final && !src.sg_wa.bytes_left)
+				op.eom = 1;
+
+			ret = cmd_q->ccp->vdata->perform->sha(&op);
+			if (ret) {
+				cmd->engine_error = cmd_q->cmd_error;
+				goto e_data;
+			}
 
-		ret = cmd_q->ccp->vdata->perform->perform_sha(&op);
+			ccp_process_data(&src, NULL, &op);
+		}
+	} else {
+		op.eom = 1;
+		ret = cmd_q->ccp->vdata->perform->sha(&op);
 		if (ret) {
 			cmd->engine_error = cmd_q->cmd_error;
 			goto e_data;
 		}
-
-		ccp_process_data(&src, NULL, &op);
 	}
 
 	/* Retrieve the SHA context - convert from LE to BE using
 	 * 32-byte (256-bit) byteswapping to BE
 	 */
-	ret = ccp_copy_from_ksb(cmd_q, &ctx, op.jobid, op.ksb_ctx,
-				CCP_PASSTHRU_BYTESWAP_256BIT);
+	ret = ccp_copy_from_sb(cmd_q, &ctx, op.jobid, op.sb_ctx,
+			       CCP_PASSTHRU_BYTESWAP_256BIT);
 	if (ret) {
 		cmd->engine_error = cmd_q->cmd_error;
 		goto e_data;
 	}
 
-	ccp_get_dm_area(&ctx, 0, sha->ctx, 0, sha->ctx_len);
-
-	if (sha->final && sha->opad) {
-		/* HMAC operation, recursively perform final SHA */
-		struct ccp_cmd hmac_cmd;
-		struct scatterlist sg;
-		u64 block_size, digest_size;
-		u8 *hmac_buf;
-
+	if (sha->final) {
+		/* Finishing up, so get the digest */
 		switch (sha->type) {
 		case CCP_SHA_TYPE_1:
-			block_size = SHA1_BLOCK_SIZE;
-			digest_size = SHA1_DIGEST_SIZE;
-			break;
 		case CCP_SHA_TYPE_224:
-			block_size = SHA224_BLOCK_SIZE;
-			digest_size = SHA224_DIGEST_SIZE;
-			break;
 		case CCP_SHA_TYPE_256:
-			block_size = SHA256_BLOCK_SIZE;
-			digest_size = SHA256_DIGEST_SIZE;
+			ccp_get_dm_area(&ctx, ooffset,
+					sha->ctx, 0,
+					digest_size);
 			break;
 		default:
 			ret = -EINVAL;
-			goto e_data;
+			goto e_ctx;
 		}
+	} else {
+		/* Stash the context */
+		ccp_get_dm_area(&ctx, 0, sha->ctx, 0,
+				sb_count * CCP_SB_BYTES);
+	}
+
+	if (sha->final && sha->opad) {
+		/* HMAC operation, recursively perform final SHA */
+		struct ccp_cmd hmac_cmd;
+		struct scatterlist sg;
+		u8 *hmac_buf;
 
 		if (sha->opad_len != block_size) {
 			ret = -EINVAL;
@@ -1153,7 +1174,18 @@ static int ccp_run_sha_cmd(struct ccp_cmd_queue *cmd_q, struct ccp_cmd *cmd)
 		sg_init_one(&sg, hmac_buf, block_size + digest_size);
 
 		scatterwalk_map_and_copy(hmac_buf, sha->opad, 0, block_size, 0);
-		memcpy(hmac_buf + block_size, ctx.address, digest_size);
+		switch (sha->type) {
+		case CCP_SHA_TYPE_1:
+		case CCP_SHA_TYPE_224:
+		case CCP_SHA_TYPE_256:
+			memcpy(hmac_buf + block_size,
+			       ctx.address + ooffset,
+			       digest_size);
+			break;
+		default:
+			ret = -EINVAL;
+			goto e_ctx;
+		}
 
 		memset(&hmac_cmd, 0, sizeof(hmac_cmd));
 		hmac_cmd.engine = CCP_ENGINE_SHA;
@@ -1176,7 +1208,8 @@ static int ccp_run_sha_cmd(struct ccp_cmd_queue *cmd_q, struct ccp_cmd *cmd)
 	}
 
 e_data:
-	ccp_free_data(&src, cmd_q);
+	if (sha->src)
+		ccp_free_data(&src, cmd_q);
 
 e_ctx:
 	ccp_dm_free(&ctx);
@@ -1190,7 +1223,7 @@ static int ccp_run_rsa_cmd(struct ccp_cmd_queue *cmd_q, struct ccp_cmd *cmd)
 	struct ccp_dm_workarea exp, src;
 	struct ccp_data dst;
 	struct ccp_op op;
-	unsigned int ksb_count, i_len, o_len;
+	unsigned int sb_count, i_len, o_len;
 	int ret;
 
 	if (rsa->key_size > CCP_RSA_MAX_WIDTH)
@@ -1208,16 +1241,17 @@ static int ccp_run_rsa_cmd(struct ccp_cmd_queue *cmd_q, struct ccp_cmd *cmd)
 	o_len = ((rsa->key_size + 255) / 256) * 32;
 	i_len = o_len * 2;
 
-	ksb_count = o_len / CCP_KSB_BYTES;
+	sb_count = o_len / CCP_SB_BYTES;
 
 	memset(&op, 0, sizeof(op));
 	op.cmd_q = cmd_q;
 	op.jobid = ccp_gen_jobid(cmd_q->ccp);
-	op.ksb_key = ccp_alloc_ksb(cmd_q->ccp, ksb_count);
-	if (!op.ksb_key)
+	op.sb_key = cmd_q->ccp->vdata->perform->sballoc(cmd_q, sb_count);
+
+	if (!op.sb_key)
 		return -EIO;
 
-	/* The RSA exponent may span multiple (32-byte) KSB entries and must
+	/* The RSA exponent may span multiple (32-byte) SB entries and must
 	 * be in little endian format. Reverse copy each 32-byte chunk
 	 * of the exponent (En chunk to E0 chunk, E(n-1) chunk to E1 chunk)
 	 * and each byte within that chunk and do not perform any byte swap
@@ -1225,14 +1259,14 @@ static int ccp_run_rsa_cmd(struct ccp_cmd_queue *cmd_q, struct ccp_cmd *cmd)
 	 */
 	ret = ccp_init_dm_workarea(&exp, cmd_q, o_len, DMA_TO_DEVICE);
 	if (ret)
-		goto e_ksb;
+		goto e_sb;
 
 	ret = ccp_reverse_set_dm_area(&exp, rsa->exp, rsa->exp_len,
-				      CCP_KSB_BYTES, false);
+				      CCP_SB_BYTES, false);
 	if (ret)
 		goto e_exp;
-	ret = ccp_copy_to_ksb(cmd_q, &exp, op.jobid, op.ksb_key,
-			      CCP_PASSTHRU_BYTESWAP_NOOP);
+	ret = ccp_copy_to_sb(cmd_q, &exp, op.jobid, op.sb_key,
+			     CCP_PASSTHRU_BYTESWAP_NOOP);
 	if (ret) {
 		cmd->engine_error = cmd_q->cmd_error;
 		goto e_exp;
@@ -1247,12 +1281,12 @@ static int ccp_run_rsa_cmd(struct ccp_cmd_queue *cmd_q, struct ccp_cmd *cmd)
 		goto e_exp;
 
 	ret = ccp_reverse_set_dm_area(&src, rsa->mod, rsa->mod_len,
-				      CCP_KSB_BYTES, false);
+				      CCP_SB_BYTES, false);
 	if (ret)
 		goto e_src;
 	src.address += o_len;	/* Adjust the address for the copy operation */
 	ret = ccp_reverse_set_dm_area(&src, rsa->src, rsa->src_len,
-				      CCP_KSB_BYTES, false);
+				      CCP_SB_BYTES, false);
 	if (ret)
 		goto e_src;
 	src.address -= o_len;	/* Reset the address to original value */
@@ -1274,7 +1308,7 @@ static int ccp_run_rsa_cmd(struct ccp_cmd_queue *cmd_q, struct ccp_cmd *cmd)
 	op.u.rsa.mod_size = rsa->key_size;
 	op.u.rsa.input_len = i_len;
 
-	ret = cmd_q->ccp->vdata->perform->perform_rsa(&op);
+	ret = cmd_q->ccp->vdata->perform->rsa(&op);
 	if (ret) {
 		cmd->engine_error = cmd_q->cmd_error;
 		goto e_dst;
@@ -1291,8 +1325,8 @@ e_src:
 e_exp:
 	ccp_dm_free(&exp);
 
-e_ksb:
-	ccp_free_ksb(cmd_q->ccp, op.ksb_key, ksb_count);
+e_sb:
+	cmd_q->ccp->vdata->perform->sbfree(cmd_q, op.sb_key, sb_count);
 
 	return ret;
 }
@@ -1306,7 +1340,7 @@ static int ccp_run_passthru_cmd(struct ccp_cmd_queue *cmd_q,
 	struct ccp_op op;
 	bool in_place = false;
 	unsigned int i;
-	int ret;
+	int ret = 0;
 
 	if (!pt->final && (pt->src_len & (CCP_PASSTHRU_BLOCKSIZE - 1)))
 		return -EINVAL;
@@ -1321,26 +1355,26 @@ static int ccp_run_passthru_cmd(struct ccp_cmd_queue *cmd_q,
 			return -EINVAL;
 	}
 
-	BUILD_BUG_ON(CCP_PASSTHRU_KSB_COUNT != 1);
+	BUILD_BUG_ON(CCP_PASSTHRU_SB_COUNT != 1);
 
 	memset(&op, 0, sizeof(op));
 	op.cmd_q = cmd_q;
-	op.jobid = ccp_gen_jobid(cmd_q->ccp);
+	op.jobid = CCP_NEW_JOBID(cmd_q->ccp);
 
 	if (pt->bit_mod != CCP_PASSTHRU_BITWISE_NOOP) {
 		/* Load the mask */
-		op.ksb_key = cmd_q->ksb_key;
+		op.sb_key = cmd_q->sb_key;
 
 		ret = ccp_init_dm_workarea(&mask, cmd_q,
-					   CCP_PASSTHRU_KSB_COUNT *
-					   CCP_KSB_BYTES,
+					   CCP_PASSTHRU_SB_COUNT *
+					   CCP_SB_BYTES,
 					   DMA_TO_DEVICE);
 		if (ret)
 			return ret;
 
 		ccp_set_dm_area(&mask, 0, pt->mask, 0, pt->mask_len);
-		ret = ccp_copy_to_ksb(cmd_q, &mask, op.jobid, op.ksb_key,
-				      CCP_PASSTHRU_BYTESWAP_NOOP);
+		ret = ccp_copy_to_sb(cmd_q, &mask, op.jobid, op.sb_key,
+				     CCP_PASSTHRU_BYTESWAP_NOOP);
 		if (ret) {
 			cmd->engine_error = cmd_q->cmd_error;
 			goto e_mask;
@@ -1399,7 +1433,7 @@ static int ccp_run_passthru_cmd(struct ccp_cmd_queue *cmd_q,
 		op.dst.u.dma.offset = dst.sg_wa.sg_used;
 		op.dst.u.dma.length = op.src.u.dma.length;
 
-		ret = cmd_q->ccp->vdata->perform->perform_passthru(&op);
+		ret = cmd_q->ccp->vdata->perform->passthru(&op);
 		if (ret) {
 			cmd->engine_error = cmd_q->cmd_error;
 			goto e_dst;
@@ -1448,7 +1482,7 @@ static int ccp_run_passthru_nomap_cmd(struct ccp_cmd_queue *cmd_q,
 			return -EINVAL;
 	}
 
-	BUILD_BUG_ON(CCP_PASSTHRU_KSB_COUNT != 1);
+	BUILD_BUG_ON(CCP_PASSTHRU_SB_COUNT != 1);
 
 	memset(&op, 0, sizeof(op));
 	op.cmd_q = cmd_q;
@@ -1456,13 +1490,13 @@ static int ccp_run_passthru_nomap_cmd(struct ccp_cmd_queue *cmd_q,
 
 	if (pt->bit_mod != CCP_PASSTHRU_BITWISE_NOOP) {
 		/* Load the mask */
-		op.ksb_key = cmd_q->ksb_key;
+		op.sb_key = cmd_q->sb_key;
 
 		mask.length = pt->mask_len;
 		mask.dma.address = pt->mask;
 		mask.dma.length = pt->mask_len;
 
-		ret = ccp_copy_to_ksb(cmd_q, &mask, op.jobid, op.ksb_key,
+		ret = ccp_copy_to_sb(cmd_q, &mask, op.jobid, op.sb_key,
 				     CCP_PASSTHRU_BYTESWAP_NOOP);
 		if (ret) {
 			cmd->engine_error = cmd_q->cmd_error;
@@ -1484,7 +1518,7 @@ static int ccp_run_passthru_nomap_cmd(struct ccp_cmd_queue *cmd_q,
 	op.dst.u.dma.offset = 0;
 	op.dst.u.dma.length = pt->src_len;
 
-	ret = cmd_q->ccp->vdata->perform->perform_passthru(&op);
+	ret = cmd_q->ccp->vdata->perform->passthru(&op);
 	if (ret)
 		cmd->engine_error = cmd_q->cmd_error;
 
@@ -1514,7 +1548,7 @@ static int ccp_run_ecc_mm_cmd(struct ccp_cmd_queue *cmd_q, struct ccp_cmd *cmd)
 
 	memset(&op, 0, sizeof(op));
 	op.cmd_q = cmd_q;
-	op.jobid = ccp_gen_jobid(cmd_q->ccp);
+	op.jobid = CCP_NEW_JOBID(cmd_q->ccp);
 
 	/* Concatenate the modulus and the operands. Both the modulus and
 	 * the operands must be in little endian format.  Since the input
@@ -1575,7 +1609,7 @@ static int ccp_run_ecc_mm_cmd(struct ccp_cmd_queue *cmd_q, struct ccp_cmd *cmd)
 
 	op.u.ecc.function = cmd->u.ecc.function;
 
-	ret = cmd_q->ccp->vdata->perform->perform_ecc(&op);
+	ret = cmd_q->ccp->vdata->perform->ecc(&op);
 	if (ret) {
 		cmd->engine_error = cmd_q->cmd_error;
 		goto e_dst;
@@ -1639,7 +1673,7 @@ static int ccp_run_ecc_pm_cmd(struct ccp_cmd_queue *cmd_q, struct ccp_cmd *cmd)
 
 	memset(&op, 0, sizeof(op));
 	op.cmd_q = cmd_q;
-	op.jobid = ccp_gen_jobid(cmd_q->ccp);
+	op.jobid = CCP_NEW_JOBID(cmd_q->ccp);
 
 	/* Concatenate the modulus and the operands. Both the modulus and
 	 * the operands must be in little endian format.  Since the input
@@ -1677,7 +1711,7 @@ static int ccp_run_ecc_pm_cmd(struct ccp_cmd_queue *cmd_q, struct ccp_cmd *cmd)
 		goto e_src;
 	src.address += CCP_ECC_OPERAND_SIZE;
 
-	/* Set the first point Z coordianate to 1 */
+	/* Set the first point Z coordinate to 1 */
 	*src.address = 0x01;
 	src.address += CCP_ECC_OPERAND_SIZE;
 
@@ -1696,7 +1730,7 @@ static int ccp_run_ecc_pm_cmd(struct ccp_cmd_queue *cmd_q, struct ccp_cmd *cmd)
 			goto e_src;
 		src.address += CCP_ECC_OPERAND_SIZE;
 
-		/* Set the second point Z coordianate to 1 */
+		/* Set the second point Z coordinate to 1 */
 		*src.address = 0x01;
 		src.address += CCP_ECC_OPERAND_SIZE;
 	} else {
@@ -1739,7 +1773,7 @@ static int ccp_run_ecc_pm_cmd(struct ccp_cmd_queue *cmd_q, struct ccp_cmd *cmd)
 
 	op.u.ecc.function = cmd->u.ecc.function;
 
-	ret = cmd_q->ccp->vdata->perform->perform_ecc(&op);
+	ret = cmd_q->ccp->vdata->perform->ecc(&op);
 	if (ret) {
 		cmd->engine_error = cmd_q->cmd_error;
 		goto e_dst;
@@ -1810,7 +1844,7 @@ int ccp_run_cmd(struct ccp_cmd_queue *cmd_q, struct ccp_cmd *cmd)
 	cmd->engine_error = 0;
 	cmd_q->cmd_error = 0;
 	cmd_q->int_rcvd = 0;
-	cmd_q->free_slots = CMD_Q_DEPTH(ioread32(cmd_q->reg_status));
+	cmd_q->free_slots = cmd_q->ccp->vdata->perform->get_free_slots(cmd_q);
 
 	switch (cmd->engine) {
 	case CCP_ENGINE_AES:
diff --git a/drivers/crypto/ccp/ccp-pci.c b/drivers/crypto/ccp/ccp-pci.c
index 0bf262e36b6b..28a9996c1085 100644
--- a/drivers/crypto/ccp/ccp-pci.c
+++ b/drivers/crypto/ccp/ccp-pci.c
@@ -4,6 +4,7 @@
  * Copyright (C) 2013,2016 Advanced Micro Devices, Inc.
  *
  * Author: Tom Lendacky <thomas.lendacky@amd.com>
+ * Author: Gary R Hook <gary.hook@amd.com>
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
@@ -25,9 +26,6 @@
 
 #include "ccp-dev.h"
 
-#define IO_BAR				2
-#define IO_OFFSET			0x20000
-
 #define MSIX_VECTORS			2
 
 struct ccp_msix {
@@ -143,10 +141,11 @@ static void ccp_free_irqs(struct ccp_device *ccp)
 			free_irq(ccp_pci->msix[ccp_pci->msix_count].vector,
 				 dev);
 		pci_disable_msix(pdev);
-	} else {
+	} else if (ccp->irq) {
 		free_irq(ccp->irq, dev);
 		pci_disable_msi(pdev);
 	}
+	ccp->irq = 0;
 }
 
 static int ccp_find_mmio_area(struct ccp_device *ccp)
@@ -156,10 +155,11 @@ static int ccp_find_mmio_area(struct ccp_device *ccp)
 	resource_size_t io_len;
 	unsigned long io_flags;
 
-	io_flags = pci_resource_flags(pdev, IO_BAR);
-	io_len = pci_resource_len(pdev, IO_BAR);
-	if ((io_flags & IORESOURCE_MEM) && (io_len >= (IO_OFFSET + 0x800)))
-		return IO_BAR;
+	io_flags = pci_resource_flags(pdev, ccp->vdata->bar);
+	io_len = pci_resource_len(pdev, ccp->vdata->bar);
+	if ((io_flags & IORESOURCE_MEM) &&
+	    (io_len >= (ccp->vdata->offset + 0x800)))
+		return ccp->vdata->bar;
 
 	return -EIO;
 }
@@ -216,7 +216,7 @@ static int ccp_pci_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 		dev_err(dev, "pci_iomap failed\n");
 		goto e_device;
 	}
-	ccp->io_regs = ccp->io_map + IO_OFFSET;
+	ccp->io_regs = ccp->io_map + ccp->vdata->offset;
 
 	ret = dma_set_mask_and_coherent(dev, DMA_BIT_MASK(48));
 	if (ret) {
@@ -230,6 +230,9 @@ static int ccp_pci_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 
 	dev_set_drvdata(dev, ccp);
 
+	if (ccp->vdata->setup)
+		ccp->vdata->setup(ccp);
+
 	ret = ccp->vdata->perform->init(ccp);
 	if (ret)
 		goto e_iomap;
@@ -322,6 +325,8 @@ static int ccp_pci_resume(struct pci_dev *pdev)
 
 static const struct pci_device_id ccp_pci_table[] = {
 	{ PCI_VDEVICE(AMD, 0x1537), (kernel_ulong_t)&ccpv3 },
+	{ PCI_VDEVICE(AMD, 0x1456), (kernel_ulong_t)&ccpv5a },
+	{ PCI_VDEVICE(AMD, 0x1468), (kernel_ulong_t)&ccpv5b },
 	/* Last entry must be zero */
 	{ 0, }
 };
diff --git a/drivers/crypto/chelsio/Kconfig b/drivers/crypto/chelsio/Kconfig
new file mode 100644
index 000000000000..4ce67fb9a880
--- /dev/null
+++ b/drivers/crypto/chelsio/Kconfig
@@ -0,0 +1,19 @@
+config CRYPTO_DEV_CHELSIO
+	tristate "Chelsio Crypto Co-processor Driver"
+	depends on CHELSIO_T4
+	select CRYPTO_SHA1
+	select CRYPTO_SHA256
+	select CRYPTO_SHA512
+	---help---
+	  The Chelsio Crypto Co-processor driver for T6 adapters.
+
+	  For general information about Chelsio and our products, visit
+	  our website at <http://www.chelsio.com>.
+
+	  For customer support, please visit our customer support page at
+	  <http://www.chelsio.com/support.html>.
+
+	  Please send feedback to <linux-bugs@chelsio.com>.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called chcr.
diff --git a/drivers/crypto/chelsio/Makefile b/drivers/crypto/chelsio/Makefile
new file mode 100644
index 000000000000..bebdf06687ad
--- /dev/null
+++ b/drivers/crypto/chelsio/Makefile
@@ -0,0 +1,4 @@
+ccflags-y := -Idrivers/net/ethernet/chelsio/cxgb4
+
+obj-$(CONFIG_CRYPTO_DEV_CHELSIO) += chcr.o
+chcr-objs :=  chcr_core.o chcr_algo.o
diff --git a/drivers/crypto/chelsio/chcr_algo.c b/drivers/crypto/chelsio/chcr_algo.c
new file mode 100644
index 000000000000..e4ddb921d7b3
--- /dev/null
+++ b/drivers/crypto/chelsio/chcr_algo.c
@@ -0,0 +1,1525 @@
+/*
+ * This file is part of the Chelsio T6 Crypto driver for Linux.
+ *
+ * Copyright (c) 2003-2016 Chelsio Communications, Inc. All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * 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.
+ *
+ * Written and Maintained by:
+ *	Manoj Malviya (manojmalviya@chelsio.com)
+ *	Atul Gupta (atul.gupta@chelsio.com)
+ *	Jitendra Lulla (jlulla@chelsio.com)
+ *	Yeshaswi M R Gowda (yeshaswi@chelsio.com)
+ *	Harsh Jain (harsh@chelsio.com)
+ */
+
+#define pr_fmt(fmt) "chcr:" fmt
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/crypto.h>
+#include <linux/cryptohash.h>
+#include <linux/skbuff.h>
+#include <linux/rtnetlink.h>
+#include <linux/highmem.h>
+#include <linux/scatterlist.h>
+
+#include <crypto/aes.h>
+#include <crypto/algapi.h>
+#include <crypto/hash.h>
+#include <crypto/sha.h>
+#include <crypto/internal/hash.h>
+
+#include "t4fw_api.h"
+#include "t4_msg.h"
+#include "chcr_core.h"
+#include "chcr_algo.h"
+#include "chcr_crypto.h"
+
+static inline struct ablk_ctx *ABLK_CTX(struct chcr_context *ctx)
+{
+	return ctx->crypto_ctx->ablkctx;
+}
+
+static inline struct hmac_ctx *HMAC_CTX(struct chcr_context *ctx)
+{
+	return ctx->crypto_ctx->hmacctx;
+}
+
+static inline struct uld_ctx *ULD_CTX(struct chcr_context *ctx)
+{
+	return ctx->dev->u_ctx;
+}
+
+static inline int is_ofld_imm(const struct sk_buff *skb)
+{
+	return (skb->len <= CRYPTO_MAX_IMM_TX_PKT_LEN);
+}
+
+/*
+ *	sgl_len - calculates the size of an SGL of the given capacity
+ *	@n: the number of SGL entries
+ *	Calculates the number of flits needed for a scatter/gather list that
+ *	can hold the given number of entries.
+ */
+static inline unsigned int sgl_len(unsigned int n)
+{
+	n--;
+	return (3 * n) / 2 + (n & 1) + 2;
+}
+
+/*
+ *	chcr_handle_resp - Unmap the DMA buffers associated with the request
+ *	@req: crypto request
+ */
+int chcr_handle_resp(struct crypto_async_request *req, unsigned char *input,
+		     int error_status)
+{
+	struct crypto_tfm *tfm = req->tfm;
+	struct chcr_context *ctx = crypto_tfm_ctx(tfm);
+	struct uld_ctx *u_ctx = ULD_CTX(ctx);
+	struct chcr_req_ctx ctx_req;
+	struct cpl_fw6_pld *fw6_pld;
+	unsigned int digestsize, updated_digestsize;
+
+	switch (tfm->__crt_alg->cra_flags & CRYPTO_ALG_TYPE_MASK) {
+	case CRYPTO_ALG_TYPE_BLKCIPHER:
+		ctx_req.req.ablk_req = (struct ablkcipher_request *)req;
+		ctx_req.ctx.ablk_ctx =
+			ablkcipher_request_ctx(ctx_req.req.ablk_req);
+		if (!error_status) {
+			fw6_pld = (struct cpl_fw6_pld *)input;
+			memcpy(ctx_req.req.ablk_req->info, &fw6_pld->data[2],
+			       AES_BLOCK_SIZE);
+		}
+		dma_unmap_sg(&u_ctx->lldi.pdev->dev, ctx_req.req.ablk_req->dst,
+			     ABLK_CTX(ctx)->dst_nents, DMA_FROM_DEVICE);
+		if (ctx_req.ctx.ablk_ctx->skb) {
+			kfree_skb(ctx_req.ctx.ablk_ctx->skb);
+			ctx_req.ctx.ablk_ctx->skb = NULL;
+		}
+		break;
+
+	case CRYPTO_ALG_TYPE_AHASH:
+		ctx_req.req.ahash_req = (struct ahash_request *)req;
+		ctx_req.ctx.ahash_ctx =
+			ahash_request_ctx(ctx_req.req.ahash_req);
+		digestsize =
+			crypto_ahash_digestsize(crypto_ahash_reqtfm(
+							ctx_req.req.ahash_req));
+		updated_digestsize = digestsize;
+		if (digestsize == SHA224_DIGEST_SIZE)
+			updated_digestsize = SHA256_DIGEST_SIZE;
+		else if (digestsize == SHA384_DIGEST_SIZE)
+			updated_digestsize = SHA512_DIGEST_SIZE;
+		if (ctx_req.ctx.ahash_ctx->skb)
+			ctx_req.ctx.ahash_ctx->skb = NULL;
+		if (ctx_req.ctx.ahash_ctx->result == 1) {
+			ctx_req.ctx.ahash_ctx->result = 0;
+			memcpy(ctx_req.req.ahash_req->result, input +
+			       sizeof(struct cpl_fw6_pld),
+			       digestsize);
+		} else {
+			memcpy(ctx_req.ctx.ahash_ctx->partial_hash, input +
+			       sizeof(struct cpl_fw6_pld),
+			       updated_digestsize);
+		}
+		kfree(ctx_req.ctx.ahash_ctx->dummy_payload_ptr);
+		ctx_req.ctx.ahash_ctx->dummy_payload_ptr = NULL;
+		break;
+	}
+	return 0;
+}
+
+/*
+ *	calc_tx_flits_ofld - calculate # of flits for an offload packet
+ *	@skb: the packet
+ *	Returns the number of flits needed for the given offload packet.
+ *	These packets are already fully constructed and no additional headers
+ *	will be added.
+ */
+static inline unsigned int calc_tx_flits_ofld(const struct sk_buff *skb)
+{
+	unsigned int flits, cnt;
+
+	if (is_ofld_imm(skb))
+		return DIV_ROUND_UP(skb->len, 8);
+
+	flits = skb_transport_offset(skb) / 8;   /* headers */
+	cnt = skb_shinfo(skb)->nr_frags;
+	if (skb_tail_pointer(skb) != skb_transport_header(skb))
+		cnt++;
+	return flits + sgl_len(cnt);
+}
+
+static struct shash_desc *chcr_alloc_shash(unsigned int ds)
+{
+	struct crypto_shash *base_hash = NULL;
+	struct shash_desc *desc;
+
+	switch (ds) {
+	case SHA1_DIGEST_SIZE:
+		base_hash = crypto_alloc_shash("sha1-generic", 0, 0);
+		break;
+	case SHA224_DIGEST_SIZE:
+		base_hash = crypto_alloc_shash("sha224-generic", 0, 0);
+		break;
+	case SHA256_DIGEST_SIZE:
+		base_hash = crypto_alloc_shash("sha256-generic", 0, 0);
+		break;
+	case SHA384_DIGEST_SIZE:
+		base_hash = crypto_alloc_shash("sha384-generic", 0, 0);
+		break;
+	case SHA512_DIGEST_SIZE:
+		base_hash = crypto_alloc_shash("sha512-generic", 0, 0);
+		break;
+	}
+	if (IS_ERR(base_hash)) {
+		pr_err("Can not allocate sha-generic algo.\n");
+		return (void *)base_hash;
+	}
+
+	desc = kmalloc(sizeof(*desc) + crypto_shash_descsize(base_hash),
+		       GFP_KERNEL);
+	if (!desc)
+		return ERR_PTR(-ENOMEM);
+	desc->tfm = base_hash;
+	desc->flags = crypto_shash_get_flags(base_hash);
+	return desc;
+}
+
+static int chcr_compute_partial_hash(struct shash_desc *desc,
+				     char *iopad, char *result_hash,
+				     int digest_size)
+{
+	struct sha1_state sha1_st;
+	struct sha256_state sha256_st;
+	struct sha512_state sha512_st;
+	int error;
+
+	if (digest_size == SHA1_DIGEST_SIZE) {
+		error = crypto_shash_init(desc) ?:
+			crypto_shash_update(desc, iopad, SHA1_BLOCK_SIZE) ?:
+			crypto_shash_export(desc, (void *)&sha1_st);
+		memcpy(result_hash, sha1_st.state, SHA1_DIGEST_SIZE);
+	} else if (digest_size == SHA224_DIGEST_SIZE) {
+		error = crypto_shash_init(desc) ?:
+			crypto_shash_update(desc, iopad, SHA256_BLOCK_SIZE) ?:
+			crypto_shash_export(desc, (void *)&sha256_st);
+		memcpy(result_hash, sha256_st.state, SHA256_DIGEST_SIZE);
+
+	} else if (digest_size == SHA256_DIGEST_SIZE) {
+		error = crypto_shash_init(desc) ?:
+			crypto_shash_update(desc, iopad, SHA256_BLOCK_SIZE) ?:
+			crypto_shash_export(desc, (void *)&sha256_st);
+		memcpy(result_hash, sha256_st.state, SHA256_DIGEST_SIZE);
+
+	} else if (digest_size == SHA384_DIGEST_SIZE) {
+		error = crypto_shash_init(desc) ?:
+			crypto_shash_update(desc, iopad, SHA512_BLOCK_SIZE) ?:
+			crypto_shash_export(desc, (void *)&sha512_st);
+		memcpy(result_hash, sha512_st.state, SHA512_DIGEST_SIZE);
+
+	} else if (digest_size == SHA512_DIGEST_SIZE) {
+		error = crypto_shash_init(desc) ?:
+			crypto_shash_update(desc, iopad, SHA512_BLOCK_SIZE) ?:
+			crypto_shash_export(desc, (void *)&sha512_st);
+		memcpy(result_hash, sha512_st.state, SHA512_DIGEST_SIZE);
+	} else {
+		error = -EINVAL;
+		pr_err("Unknown digest size %d\n", digest_size);
+	}
+	return error;
+}
+
+static void chcr_change_order(char *buf, int ds)
+{
+	int i;
+
+	if (ds == SHA512_DIGEST_SIZE) {
+		for (i = 0; i < (ds / sizeof(u64)); i++)
+			*((__be64 *)buf + i) =
+				cpu_to_be64(*((u64 *)buf + i));
+	} else {
+		for (i = 0; i < (ds / sizeof(u32)); i++)
+			*((__be32 *)buf + i) =
+				cpu_to_be32(*((u32 *)buf + i));
+	}
+}
+
+static inline int is_hmac(struct crypto_tfm *tfm)
+{
+	struct crypto_alg *alg = tfm->__crt_alg;
+	struct chcr_alg_template *chcr_crypto_alg =
+		container_of(__crypto_ahash_alg(alg), struct chcr_alg_template,
+			     alg.hash);
+	if ((chcr_crypto_alg->type & CRYPTO_ALG_SUB_TYPE_MASK) ==
+	    CRYPTO_ALG_SUB_TYPE_HASH_HMAC)
+		return 1;
+	return 0;
+}
+
+static inline unsigned int ch_nents(struct scatterlist *sg,
+				    unsigned int *total_size)
+{
+	unsigned int nents;
+
+	for (nents = 0, *total_size = 0; sg; sg = sg_next(sg)) {
+		nents++;
+		*total_size += sg->length;
+	}
+	return nents;
+}
+
+static void write_phys_cpl(struct cpl_rx_phys_dsgl *phys_cpl,
+			   struct scatterlist *sg,
+			   struct phys_sge_parm *sg_param)
+{
+	struct phys_sge_pairs *to;
+	unsigned int out_buf_size = sg_param->obsize;
+	unsigned int nents = sg_param->nents, i, j, tot_len = 0;
+
+	phys_cpl->op_to_tid = htonl(CPL_RX_PHYS_DSGL_OPCODE_V(CPL_RX_PHYS_DSGL)
+				    | CPL_RX_PHYS_DSGL_ISRDMA_V(0));
+	phys_cpl->pcirlxorder_to_noofsgentr =
+		htonl(CPL_RX_PHYS_DSGL_PCIRLXORDER_V(0) |
+		      CPL_RX_PHYS_DSGL_PCINOSNOOP_V(0) |
+		      CPL_RX_PHYS_DSGL_PCITPHNTENB_V(0) |
+		      CPL_RX_PHYS_DSGL_PCITPHNT_V(0) |
+		      CPL_RX_PHYS_DSGL_DCAID_V(0) |
+		      CPL_RX_PHYS_DSGL_NOOFSGENTR_V(nents));
+	phys_cpl->rss_hdr_int.opcode = CPL_RX_PHYS_ADDR;
+	phys_cpl->rss_hdr_int.qid = htons(sg_param->qid);
+	phys_cpl->rss_hdr_int.hash_val = 0;
+	to = (struct phys_sge_pairs *)((unsigned char *)phys_cpl +
+				       sizeof(struct cpl_rx_phys_dsgl));
+
+	for (i = 0; nents; to++) {
+		for (j = i; (nents && (j < (8 + i))); j++, nents--) {
+			to->len[j] = htons(sg->length);
+			to->addr[j] = cpu_to_be64(sg_dma_address(sg));
+			if (out_buf_size) {
+				if (tot_len + sg_dma_len(sg) >= out_buf_size) {
+					to->len[j] = htons(out_buf_size -
+							   tot_len);
+					return;
+				}
+				tot_len += sg_dma_len(sg);
+			}
+			sg = sg_next(sg);
+		}
+	}
+}
+
+static inline unsigned
+int map_writesg_phys_cpl(struct device *dev, struct cpl_rx_phys_dsgl *phys_cpl,
+			 struct scatterlist *sg, struct phys_sge_parm *sg_param)
+{
+	if (!sg || !sg_param->nents)
+		return 0;
+
+	sg_param->nents = dma_map_sg(dev, sg, sg_param->nents, DMA_FROM_DEVICE);
+	if (sg_param->nents == 0) {
+		pr_err("CHCR : DMA mapping failed\n");
+		return -EINVAL;
+	}
+	write_phys_cpl(phys_cpl, sg, sg_param);
+	return 0;
+}
+
+static inline int get_cryptoalg_subtype(struct crypto_tfm *tfm)
+{
+	struct crypto_alg *alg = tfm->__crt_alg;
+	struct chcr_alg_template *chcr_crypto_alg =
+		container_of(alg, struct chcr_alg_template, alg.crypto);
+
+	return chcr_crypto_alg->type & CRYPTO_ALG_SUB_TYPE_MASK;
+}
+
+static inline void
+write_sg_data_page_desc(struct sk_buff *skb, unsigned int *frags,
+			struct scatterlist *sg, unsigned int count)
+{
+	struct page *spage;
+	unsigned int page_len;
+
+	skb->len += count;
+	skb->data_len += count;
+	skb->truesize += count;
+	while (count > 0) {
+		if (sg && (!(sg->length)))
+			break;
+		spage = sg_page(sg);
+		get_page(spage);
+		page_len = min(sg->length, count);
+		skb_fill_page_desc(skb, *frags, spage, sg->offset, page_len);
+		(*frags)++;
+		count -= page_len;
+		sg = sg_next(sg);
+	}
+}
+
+static int generate_copy_rrkey(struct ablk_ctx *ablkctx,
+			       struct _key_ctx *key_ctx)
+{
+	if (ablkctx->ciph_mode == CHCR_SCMD_CIPHER_MODE_AES_CBC) {
+		get_aes_decrypt_key(key_ctx->key, ablkctx->key,
+				    ablkctx->enckey_len << 3);
+		memset(key_ctx->key + ablkctx->enckey_len, 0,
+		       CHCR_AES_MAX_KEY_LEN - ablkctx->enckey_len);
+	} else {
+		memcpy(key_ctx->key,
+		       ablkctx->key + (ablkctx->enckey_len >> 1),
+		       ablkctx->enckey_len >> 1);
+		get_aes_decrypt_key(key_ctx->key + (ablkctx->enckey_len >> 1),
+				    ablkctx->key, ablkctx->enckey_len << 2);
+	}
+	return 0;
+}
+
+static inline void create_wreq(struct chcr_context *ctx,
+			       struct fw_crypto_lookaside_wr *wreq,
+			       void *req, struct sk_buff *skb,
+			       int kctx_len, int hash_sz,
+			       unsigned int phys_dsgl)
+{
+	struct uld_ctx *u_ctx = ULD_CTX(ctx);
+	struct ulp_txpkt *ulptx = (struct ulp_txpkt *)(wreq + 1);
+	struct ulptx_idata *sc_imm = (struct ulptx_idata *)(ulptx + 1);
+	int iv_loc = IV_DSGL;
+	int qid = u_ctx->lldi.rxq_ids[ctx->tx_channel_id];
+	unsigned int immdatalen = 0, nr_frags = 0;
+
+	if (is_ofld_imm(skb)) {
+		immdatalen = skb->data_len;
+		iv_loc = IV_IMMEDIATE;
+	} else {
+		nr_frags = skb_shinfo(skb)->nr_frags;
+	}
+
+	wreq->op_to_cctx_size = FILL_WR_OP_CCTX_SIZE(immdatalen,
+						     (kctx_len >> 4));
+	wreq->pld_size_hash_size =
+		htonl(FW_CRYPTO_LOOKASIDE_WR_PLD_SIZE_V(sgl_lengths[nr_frags]) |
+		      FW_CRYPTO_LOOKASIDE_WR_HASH_SIZE_V(hash_sz));
+	wreq->len16_pkd = htonl(FW_CRYPTO_LOOKASIDE_WR_LEN16_V(DIV_ROUND_UP(
+				    (calc_tx_flits_ofld(skb) * 8), 16)));
+	wreq->cookie = cpu_to_be64((uintptr_t)req);
+	wreq->rx_chid_to_rx_q_id =
+		FILL_WR_RX_Q_ID(ctx->dev->tx_channel_id, qid,
+				(hash_sz) ? IV_NOP : iv_loc);
+
+	ulptx->cmd_dest = FILL_ULPTX_CMD_DEST(ctx->dev->tx_channel_id);
+	ulptx->len = htonl((DIV_ROUND_UP((calc_tx_flits_ofld(skb) * 8),
+					 16) - ((sizeof(*wreq)) >> 4)));
+
+	sc_imm->cmd_more = FILL_CMD_MORE(immdatalen);
+	sc_imm->len = cpu_to_be32(sizeof(struct cpl_tx_sec_pdu) + kctx_len +
+				  ((hash_sz) ? DUMMY_BYTES :
+				  (sizeof(struct cpl_rx_phys_dsgl) +
+				   phys_dsgl)) + immdatalen);
+}
+
+/**
+ *	create_cipher_wr - form the WR for cipher operations
+ *	@req: cipher req.
+ *	@ctx: crypto driver context of the request.
+ *	@qid: ingress qid where response of this WR should be received.
+ *	@op_type:	encryption or decryption
+ */
+static struct sk_buff
+*create_cipher_wr(struct crypto_async_request *req_base,
+		  struct chcr_context *ctx, unsigned short qid,
+		  unsigned short op_type)
+{
+	struct ablkcipher_request *req = (struct ablkcipher_request *)req_base;
+	struct crypto_ablkcipher *tfm = crypto_ablkcipher_reqtfm(req);
+	struct uld_ctx *u_ctx = ULD_CTX(ctx);
+	struct ablk_ctx *ablkctx = ABLK_CTX(ctx);
+	struct sk_buff *skb = NULL;
+	struct _key_ctx *key_ctx;
+	struct fw_crypto_lookaside_wr *wreq;
+	struct cpl_tx_sec_pdu *sec_cpl;
+	struct cpl_rx_phys_dsgl *phys_cpl;
+	struct chcr_blkcipher_req_ctx *req_ctx = ablkcipher_request_ctx(req);
+	struct phys_sge_parm sg_param;
+	unsigned int frags = 0, transhdr_len, phys_dsgl, dst_bufsize = 0;
+	unsigned int ivsize = crypto_ablkcipher_ivsize(tfm), kctx_len;
+
+	if (!req->info)
+		return ERR_PTR(-EINVAL);
+	ablkctx->dst_nents = ch_nents(req->dst, &dst_bufsize);
+	ablkctx->enc = op_type;
+
+	if ((ablkctx->enckey_len == 0) || (ivsize > AES_BLOCK_SIZE) ||
+	    (req->nbytes <= 0) || (req->nbytes % AES_BLOCK_SIZE))
+		return ERR_PTR(-EINVAL);
+
+	phys_dsgl = get_space_for_phys_dsgl(ablkctx->dst_nents);
+
+	kctx_len = sizeof(*key_ctx) +
+		(DIV_ROUND_UP(ablkctx->enckey_len, 16) * 16);
+	transhdr_len = CIPHER_TRANSHDR_SIZE(kctx_len, phys_dsgl);
+	skb = alloc_skb((transhdr_len + sizeof(struct sge_opaque_hdr)),
+			GFP_ATOMIC);
+	if (!skb)
+		return ERR_PTR(-ENOMEM);
+	skb_reserve(skb, sizeof(struct sge_opaque_hdr));
+	wreq = (struct fw_crypto_lookaside_wr *)__skb_put(skb, transhdr_len);
+
+	sec_cpl = (struct cpl_tx_sec_pdu *)((u8 *)wreq + SEC_CPL_OFFSET);
+	sec_cpl->op_ivinsrtofst =
+		FILL_SEC_CPL_OP_IVINSR(ctx->dev->tx_channel_id, 2, 1, 1);
+
+	sec_cpl->pldlen = htonl(ivsize + req->nbytes);
+	sec_cpl->aadstart_cipherstop_hi = FILL_SEC_CPL_CIPHERSTOP_HI(0, 0,
+								ivsize + 1, 0);
+
+	sec_cpl->cipherstop_lo_authinsert =  FILL_SEC_CPL_AUTHINSERT(0, 0,
+								     0, 0);
+	sec_cpl->seqno_numivs = FILL_SEC_CPL_SCMD0_SEQNO(op_type, 0,
+							 ablkctx->ciph_mode,
+							 0, 0, ivsize >> 1, 1);
+	sec_cpl->ivgen_hdrlen = FILL_SEC_CPL_IVGEN_HDRLEN(0, 0, 0,
+							  0, 1, phys_dsgl);
+
+	key_ctx = (struct _key_ctx *)((u8 *)sec_cpl + sizeof(*sec_cpl));
+	key_ctx->ctx_hdr = ablkctx->key_ctx_hdr;
+	if (op_type == CHCR_DECRYPT_OP) {
+		if (generate_copy_rrkey(ablkctx, key_ctx))
+			goto map_fail1;
+	} else {
+		if (ablkctx->ciph_mode == CHCR_SCMD_CIPHER_MODE_AES_CBC) {
+			memcpy(key_ctx->key, ablkctx->key, ablkctx->enckey_len);
+		} else {
+			memcpy(key_ctx->key, ablkctx->key +
+			       (ablkctx->enckey_len >> 1),
+			       ablkctx->enckey_len >> 1);
+			memcpy(key_ctx->key +
+			       (ablkctx->enckey_len >> 1),
+			       ablkctx->key,
+			       ablkctx->enckey_len >> 1);
+		}
+	}
+	phys_cpl = (struct cpl_rx_phys_dsgl *)((u8 *)key_ctx + kctx_len);
+
+	memcpy(ablkctx->iv, req->info, ivsize);
+	sg_init_table(&ablkctx->iv_sg, 1);
+	sg_set_buf(&ablkctx->iv_sg, ablkctx->iv, ivsize);
+	sg_param.nents = ablkctx->dst_nents;
+	sg_param.obsize = dst_bufsize;
+	sg_param.qid = qid;
+	sg_param.align = 1;
+	if (map_writesg_phys_cpl(&u_ctx->lldi.pdev->dev, phys_cpl, req->dst,
+				 &sg_param))
+		goto map_fail1;
+
+	skb_set_transport_header(skb, transhdr_len);
+	write_sg_data_page_desc(skb, &frags, &ablkctx->iv_sg, ivsize);
+	write_sg_data_page_desc(skb, &frags, req->src, req->nbytes);
+	create_wreq(ctx, wreq, req, skb, kctx_len, 0, phys_dsgl);
+	req_ctx->skb = skb;
+	skb_get(skb);
+	return skb;
+map_fail1:
+	kfree_skb(skb);
+	return ERR_PTR(-ENOMEM);
+}
+
+static int chcr_aes_cbc_setkey(struct crypto_ablkcipher *tfm, const u8 *key,
+			       unsigned int keylen)
+{
+	struct chcr_context *ctx = crypto_ablkcipher_ctx(tfm);
+	struct ablk_ctx *ablkctx = ABLK_CTX(ctx);
+	struct ablkcipher_alg *alg = crypto_ablkcipher_alg(tfm);
+	unsigned int ck_size, context_size;
+	u16 alignment = 0;
+
+	if ((keylen < alg->min_keysize) || (keylen > alg->max_keysize))
+		goto badkey_err;
+
+	memcpy(ablkctx->key, key, keylen);
+	ablkctx->enckey_len = keylen;
+	if (keylen == AES_KEYSIZE_128) {
+		ck_size = CHCR_KEYCTX_CIPHER_KEY_SIZE_128;
+	} else if (keylen == AES_KEYSIZE_192) {
+		alignment = 8;
+		ck_size = CHCR_KEYCTX_CIPHER_KEY_SIZE_192;
+	} else if (keylen == AES_KEYSIZE_256) {
+		ck_size = CHCR_KEYCTX_CIPHER_KEY_SIZE_256;
+	} else {
+		goto badkey_err;
+	}
+
+	context_size = (KEY_CONTEXT_HDR_SALT_AND_PAD +
+			keylen + alignment) >> 4;
+
+	ablkctx->key_ctx_hdr = FILL_KEY_CTX_HDR(ck_size, CHCR_KEYCTX_NO_KEY,
+						0, 0, context_size);
+	ablkctx->ciph_mode = CHCR_SCMD_CIPHER_MODE_AES_CBC;
+	return 0;
+badkey_err:
+	crypto_ablkcipher_set_flags(tfm, CRYPTO_TFM_RES_BAD_KEY_LEN);
+	ablkctx->enckey_len = 0;
+	return -EINVAL;
+}
+
+static int cxgb4_is_crypto_q_full(struct net_device *dev, unsigned int idx)
+{
+	int ret = 0;
+	struct sge_ofld_txq *q;
+	struct adapter *adap = netdev2adap(dev);
+
+	local_bh_disable();
+	q = &adap->sge.ofldtxq[idx];
+	spin_lock(&q->sendq.lock);
+	if (q->full)
+		ret = -1;
+	spin_unlock(&q->sendq.lock);
+	local_bh_enable();
+	return ret;
+}
+
+static int chcr_aes_encrypt(struct ablkcipher_request *req)
+{
+	struct crypto_ablkcipher *tfm = crypto_ablkcipher_reqtfm(req);
+	struct chcr_context *ctx = crypto_ablkcipher_ctx(tfm);
+	struct crypto_async_request *req_base = &req->base;
+	struct uld_ctx *u_ctx = ULD_CTX(ctx);
+	struct sk_buff *skb;
+
+	if (unlikely(cxgb4_is_crypto_q_full(u_ctx->lldi.ports[0],
+					    ctx->tx_channel_id))) {
+		if (!(req->base.flags & CRYPTO_TFM_REQ_MAY_BACKLOG))
+			return -EBUSY;
+	}
+
+	skb = create_cipher_wr(req_base, ctx,
+			       u_ctx->lldi.rxq_ids[ctx->tx_channel_id],
+			       CHCR_ENCRYPT_OP);
+	if (IS_ERR(skb)) {
+		pr_err("chcr : %s : Failed to form WR. No memory\n", __func__);
+		return  PTR_ERR(skb);
+	}
+	skb->dev = u_ctx->lldi.ports[0];
+	set_wr_txq(skb, CPL_PRIORITY_DATA, ctx->tx_channel_id);
+	chcr_send_wr(skb);
+	return -EINPROGRESS;
+}
+
+static int chcr_aes_decrypt(struct ablkcipher_request *req)
+{
+	struct crypto_ablkcipher *tfm = crypto_ablkcipher_reqtfm(req);
+	struct chcr_context *ctx = crypto_ablkcipher_ctx(tfm);
+	struct crypto_async_request *req_base = &req->base;
+	struct uld_ctx *u_ctx = ULD_CTX(ctx);
+	struct sk_buff *skb;
+
+	if (unlikely(cxgb4_is_crypto_q_full(u_ctx->lldi.ports[0],
+					    ctx->tx_channel_id))) {
+		if (!(req->base.flags & CRYPTO_TFM_REQ_MAY_BACKLOG))
+			return -EBUSY;
+	}
+
+	skb = create_cipher_wr(req_base, ctx, u_ctx->lldi.rxq_ids[0],
+			       CHCR_DECRYPT_OP);
+	if (IS_ERR(skb)) {
+		pr_err("chcr : %s : Failed to form WR. No memory\n", __func__);
+		return PTR_ERR(skb);
+	}
+	skb->dev = u_ctx->lldi.ports[0];
+	set_wr_txq(skb, CPL_PRIORITY_DATA, ctx->tx_channel_id);
+	chcr_send_wr(skb);
+	return -EINPROGRESS;
+}
+
+static int chcr_device_init(struct chcr_context *ctx)
+{
+	struct uld_ctx *u_ctx;
+	unsigned int id;
+	int err = 0, rxq_perchan, rxq_idx;
+
+	id = smp_processor_id();
+	if (!ctx->dev) {
+		err = assign_chcr_device(&ctx->dev);
+		if (err) {
+			pr_err("chcr device assignment fails\n");
+			goto out;
+		}
+		u_ctx = ULD_CTX(ctx);
+		rxq_perchan = u_ctx->lldi.nrxq / u_ctx->lldi.nchan;
+		ctx->dev->tx_channel_id = 0;
+		rxq_idx = ctx->dev->tx_channel_id * rxq_perchan;
+		rxq_idx += id % rxq_perchan;
+		spin_lock(&ctx->dev->lock_chcr_dev);
+		ctx->tx_channel_id = rxq_idx;
+		spin_unlock(&ctx->dev->lock_chcr_dev);
+	}
+out:
+	return err;
+}
+
+static int chcr_cra_init(struct crypto_tfm *tfm)
+{
+	tfm->crt_ablkcipher.reqsize =  sizeof(struct chcr_blkcipher_req_ctx);
+	return chcr_device_init(crypto_tfm_ctx(tfm));
+}
+
+static int get_alg_config(struct algo_param *params,
+			  unsigned int auth_size)
+{
+	switch (auth_size) {
+	case SHA1_DIGEST_SIZE:
+		params->mk_size = CHCR_KEYCTX_MAC_KEY_SIZE_160;
+		params->auth_mode = CHCR_SCMD_AUTH_MODE_SHA1;
+		params->result_size = SHA1_DIGEST_SIZE;
+		break;
+	case SHA224_DIGEST_SIZE:
+		params->mk_size = CHCR_KEYCTX_MAC_KEY_SIZE_256;
+		params->auth_mode = CHCR_SCMD_AUTH_MODE_SHA224;
+		params->result_size = SHA256_DIGEST_SIZE;
+		break;
+	case SHA256_DIGEST_SIZE:
+		params->mk_size = CHCR_KEYCTX_MAC_KEY_SIZE_256;
+		params->auth_mode = CHCR_SCMD_AUTH_MODE_SHA256;
+		params->result_size = SHA256_DIGEST_SIZE;
+		break;
+	case SHA384_DIGEST_SIZE:
+		params->mk_size = CHCR_KEYCTX_MAC_KEY_SIZE_512;
+		params->auth_mode = CHCR_SCMD_AUTH_MODE_SHA512_384;
+		params->result_size = SHA512_DIGEST_SIZE;
+		break;
+	case SHA512_DIGEST_SIZE:
+		params->mk_size = CHCR_KEYCTX_MAC_KEY_SIZE_512;
+		params->auth_mode = CHCR_SCMD_AUTH_MODE_SHA512_512;
+		params->result_size = SHA512_DIGEST_SIZE;
+		break;
+	default:
+		pr_err("chcr : ERROR, unsupported digest size\n");
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static inline int
+write_buffer_data_page_desc(struct chcr_ahash_req_ctx *req_ctx,
+			    struct sk_buff *skb, unsigned int *frags, char *bfr,
+			    u8 bfr_len)
+{
+	void *page_ptr = NULL;
+
+	skb->len += bfr_len;
+	skb->data_len += bfr_len;
+	skb->truesize += bfr_len;
+	page_ptr = kmalloc(CHCR_HASH_MAX_BLOCK_SIZE_128, GFP_ATOMIC | GFP_DMA);
+	if (!page_ptr)
+		return -ENOMEM;
+	get_page(virt_to_page(page_ptr));
+	req_ctx->dummy_payload_ptr = page_ptr;
+	memcpy(page_ptr, bfr, bfr_len);
+	skb_fill_page_desc(skb, *frags, virt_to_page(page_ptr),
+			   offset_in_page(page_ptr), bfr_len);
+	(*frags)++;
+	return 0;
+}
+
+/**
+ *	create_final_hash_wr - Create hash work request
+ *	@req - Cipher req base
+ */
+static struct sk_buff *create_final_hash_wr(struct ahash_request *req,
+					    struct hash_wr_param *param)
+{
+	struct chcr_ahash_req_ctx *req_ctx = ahash_request_ctx(req);
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct chcr_context *ctx = crypto_tfm_ctx(crypto_ahash_tfm(tfm));
+	struct hmac_ctx *hmacctx = HMAC_CTX(ctx);
+	struct sk_buff *skb = NULL;
+	struct _key_ctx *key_ctx;
+	struct fw_crypto_lookaside_wr *wreq;
+	struct cpl_tx_sec_pdu *sec_cpl;
+	unsigned int frags = 0, transhdr_len, iopad_alignment = 0;
+	unsigned int digestsize = crypto_ahash_digestsize(tfm);
+	unsigned int kctx_len = sizeof(*key_ctx);
+	u8 hash_size_in_response = 0;
+
+	iopad_alignment = KEYCTX_ALIGN_PAD(digestsize);
+	kctx_len += param->alg_prm.result_size + iopad_alignment;
+	if (param->opad_needed)
+		kctx_len += param->alg_prm.result_size + iopad_alignment;
+
+	if (req_ctx->result)
+		hash_size_in_response = digestsize;
+	else
+		hash_size_in_response = param->alg_prm.result_size;
+	transhdr_len = HASH_TRANSHDR_SIZE(kctx_len);
+	skb = alloc_skb((transhdr_len + sizeof(struct sge_opaque_hdr)),
+			GFP_ATOMIC);
+	if (!skb)
+		return skb;
+
+	skb_reserve(skb, sizeof(struct sge_opaque_hdr));
+	wreq = (struct fw_crypto_lookaside_wr *)__skb_put(skb, transhdr_len);
+	memset(wreq, 0, transhdr_len);
+
+	sec_cpl = (struct cpl_tx_sec_pdu *)((u8 *)wreq + SEC_CPL_OFFSET);
+	sec_cpl->op_ivinsrtofst =
+		FILL_SEC_CPL_OP_IVINSR(ctx->dev->tx_channel_id, 2, 0, 0);
+	sec_cpl->pldlen = htonl(param->bfr_len + param->sg_len);
+
+	sec_cpl->aadstart_cipherstop_hi =
+		FILL_SEC_CPL_CIPHERSTOP_HI(0, 0, 0, 0);
+	sec_cpl->cipherstop_lo_authinsert =
+		FILL_SEC_CPL_AUTHINSERT(0, 1, 0, 0);
+	sec_cpl->seqno_numivs =
+		FILL_SEC_CPL_SCMD0_SEQNO(0, 0, 0, param->alg_prm.auth_mode,
+					 param->opad_needed, 0, 0);
+
+	sec_cpl->ivgen_hdrlen =
+		FILL_SEC_CPL_IVGEN_HDRLEN(param->last, param->more, 0, 1, 0, 0);
+
+	key_ctx = (struct _key_ctx *)((u8 *)sec_cpl + sizeof(*sec_cpl));
+	memcpy(key_ctx->key, req_ctx->partial_hash, param->alg_prm.result_size);
+
+	if (param->opad_needed)
+		memcpy(key_ctx->key + ((param->alg_prm.result_size <= 32) ? 32 :
+				       CHCR_HASH_MAX_DIGEST_SIZE),
+		       hmacctx->opad, param->alg_prm.result_size);
+
+	key_ctx->ctx_hdr = FILL_KEY_CTX_HDR(CHCR_KEYCTX_NO_KEY,
+					    param->alg_prm.mk_size, 0,
+					    param->opad_needed,
+					    (kctx_len >> 4));
+	sec_cpl->scmd1 = cpu_to_be64((u64)param->scmd1);
+
+	skb_set_transport_header(skb, transhdr_len);
+	if (param->bfr_len != 0)
+		write_buffer_data_page_desc(req_ctx, skb, &frags, req_ctx->bfr,
+					    param->bfr_len);
+	if (param->sg_len != 0)
+		write_sg_data_page_desc(skb, &frags, req->src, param->sg_len);
+
+	create_wreq(ctx, wreq, req, skb, kctx_len, hash_size_in_response,
+		    0);
+	req_ctx->skb = skb;
+	skb_get(skb);
+	return skb;
+}
+
+static int chcr_ahash_update(struct ahash_request *req)
+{
+	struct chcr_ahash_req_ctx *req_ctx = ahash_request_ctx(req);
+	struct crypto_ahash *rtfm = crypto_ahash_reqtfm(req);
+	struct chcr_context *ctx = crypto_tfm_ctx(crypto_ahash_tfm(rtfm));
+	struct uld_ctx *u_ctx = NULL;
+	struct sk_buff *skb;
+	u8 remainder = 0, bs;
+	unsigned int nbytes = req->nbytes;
+	struct hash_wr_param params;
+
+	bs = crypto_tfm_alg_blocksize(crypto_ahash_tfm(rtfm));
+
+	u_ctx = ULD_CTX(ctx);
+	if (unlikely(cxgb4_is_crypto_q_full(u_ctx->lldi.ports[0],
+					    ctx->tx_channel_id))) {
+		if (!(req->base.flags & CRYPTO_TFM_REQ_MAY_BACKLOG))
+			return -EBUSY;
+	}
+
+	if (nbytes + req_ctx->bfr_len >= bs) {
+		remainder = (nbytes + req_ctx->bfr_len) % bs;
+		nbytes = nbytes + req_ctx->bfr_len - remainder;
+	} else {
+		sg_pcopy_to_buffer(req->src, sg_nents(req->src), req_ctx->bfr +
+				   req_ctx->bfr_len, nbytes, 0);
+		req_ctx->bfr_len += nbytes;
+		return 0;
+	}
+
+	params.opad_needed = 0;
+	params.more = 1;
+	params.last = 0;
+	params.sg_len = nbytes - req_ctx->bfr_len;
+	params.bfr_len = req_ctx->bfr_len;
+	params.scmd1 = 0;
+	get_alg_config(&params.alg_prm, crypto_ahash_digestsize(rtfm));
+	req_ctx->result = 0;
+	req_ctx->data_len += params.sg_len + params.bfr_len;
+	skb = create_final_hash_wr(req, &params);
+	if (!skb)
+		return -ENOMEM;
+
+	req_ctx->bfr_len = remainder;
+	if (remainder)
+		sg_pcopy_to_buffer(req->src, sg_nents(req->src),
+				   req_ctx->bfr, remainder, req->nbytes -
+				   remainder);
+	skb->dev = u_ctx->lldi.ports[0];
+	set_wr_txq(skb, CPL_PRIORITY_DATA, ctx->tx_channel_id);
+	chcr_send_wr(skb);
+
+	return -EINPROGRESS;
+}
+
+static void create_last_hash_block(char *bfr_ptr, unsigned int bs, u64 scmd1)
+{
+	memset(bfr_ptr, 0, bs);
+	*bfr_ptr = 0x80;
+	if (bs == 64)
+		*(__be64 *)(bfr_ptr + 56) = cpu_to_be64(scmd1  << 3);
+	else
+		*(__be64 *)(bfr_ptr + 120) =  cpu_to_be64(scmd1  << 3);
+}
+
+static int chcr_ahash_final(struct ahash_request *req)
+{
+	struct chcr_ahash_req_ctx *req_ctx = ahash_request_ctx(req);
+	struct crypto_ahash *rtfm = crypto_ahash_reqtfm(req);
+	struct chcr_context *ctx = crypto_tfm_ctx(crypto_ahash_tfm(rtfm));
+	struct hash_wr_param params;
+	struct sk_buff *skb;
+	struct uld_ctx *u_ctx = NULL;
+	u8 bs = crypto_tfm_alg_blocksize(crypto_ahash_tfm(rtfm));
+
+	u_ctx = ULD_CTX(ctx);
+	if (is_hmac(crypto_ahash_tfm(rtfm)))
+		params.opad_needed = 1;
+	else
+		params.opad_needed = 0;
+	params.sg_len = 0;
+	get_alg_config(&params.alg_prm, crypto_ahash_digestsize(rtfm));
+	req_ctx->result = 1;
+	params.bfr_len = req_ctx->bfr_len;
+	req_ctx->data_len += params.bfr_len + params.sg_len;
+	if (req_ctx->bfr && (req_ctx->bfr_len == 0)) {
+		create_last_hash_block(req_ctx->bfr, bs, req_ctx->data_len);
+		params.last = 0;
+		params.more = 1;
+		params.scmd1 = 0;
+		params.bfr_len = bs;
+
+	} else {
+		params.scmd1 = req_ctx->data_len;
+		params.last = 1;
+		params.more = 0;
+	}
+	skb = create_final_hash_wr(req, &params);
+	skb->dev = u_ctx->lldi.ports[0];
+	set_wr_txq(skb, CPL_PRIORITY_DATA, ctx->tx_channel_id);
+	chcr_send_wr(skb);
+	return -EINPROGRESS;
+}
+
+static int chcr_ahash_finup(struct ahash_request *req)
+{
+	struct chcr_ahash_req_ctx *req_ctx = ahash_request_ctx(req);
+	struct crypto_ahash *rtfm = crypto_ahash_reqtfm(req);
+	struct chcr_context *ctx = crypto_tfm_ctx(crypto_ahash_tfm(rtfm));
+	struct uld_ctx *u_ctx = NULL;
+	struct sk_buff *skb;
+	struct hash_wr_param params;
+	u8  bs;
+
+	bs = crypto_tfm_alg_blocksize(crypto_ahash_tfm(rtfm));
+	u_ctx = ULD_CTX(ctx);
+
+	if (unlikely(cxgb4_is_crypto_q_full(u_ctx->lldi.ports[0],
+					    ctx->tx_channel_id))) {
+		if (!(req->base.flags & CRYPTO_TFM_REQ_MAY_BACKLOG))
+			return -EBUSY;
+	}
+
+	if (is_hmac(crypto_ahash_tfm(rtfm)))
+		params.opad_needed = 1;
+	else
+		params.opad_needed = 0;
+
+	params.sg_len = req->nbytes;
+	params.bfr_len = req_ctx->bfr_len;
+	get_alg_config(&params.alg_prm, crypto_ahash_digestsize(rtfm));
+	req_ctx->data_len += params.bfr_len + params.sg_len;
+	req_ctx->result = 1;
+	if (req_ctx->bfr && (req_ctx->bfr_len + req->nbytes) == 0) {
+		create_last_hash_block(req_ctx->bfr, bs, req_ctx->data_len);
+		params.last = 0;
+		params.more = 1;
+		params.scmd1 = 0;
+		params.bfr_len = bs;
+	} else {
+		params.scmd1 = req_ctx->data_len;
+		params.last = 1;
+		params.more = 0;
+	}
+
+	skb = create_final_hash_wr(req, &params);
+	if (!skb)
+		return -ENOMEM;
+	skb->dev = u_ctx->lldi.ports[0];
+	set_wr_txq(skb, CPL_PRIORITY_DATA, ctx->tx_channel_id);
+	chcr_send_wr(skb);
+
+	return -EINPROGRESS;
+}
+
+static int chcr_ahash_digest(struct ahash_request *req)
+{
+	struct chcr_ahash_req_ctx *req_ctx = ahash_request_ctx(req);
+	struct crypto_ahash *rtfm = crypto_ahash_reqtfm(req);
+	struct chcr_context *ctx = crypto_tfm_ctx(crypto_ahash_tfm(rtfm));
+	struct uld_ctx *u_ctx = NULL;
+	struct sk_buff *skb;
+	struct hash_wr_param params;
+	u8  bs;
+
+	rtfm->init(req);
+	bs = crypto_tfm_alg_blocksize(crypto_ahash_tfm(rtfm));
+
+	u_ctx = ULD_CTX(ctx);
+	if (unlikely(cxgb4_is_crypto_q_full(u_ctx->lldi.ports[0],
+					    ctx->tx_channel_id))) {
+		if (!(req->base.flags & CRYPTO_TFM_REQ_MAY_BACKLOG))
+			return -EBUSY;
+	}
+
+	if (is_hmac(crypto_ahash_tfm(rtfm)))
+		params.opad_needed = 1;
+	else
+		params.opad_needed = 0;
+
+	params.last = 0;
+	params.more = 0;
+	params.sg_len = req->nbytes;
+	params.bfr_len = 0;
+	params.scmd1 = 0;
+	get_alg_config(&params.alg_prm, crypto_ahash_digestsize(rtfm));
+	req_ctx->result = 1;
+	req_ctx->data_len += params.bfr_len + params.sg_len;
+
+	if (req_ctx->bfr && req->nbytes == 0) {
+		create_last_hash_block(req_ctx->bfr, bs, 0);
+		params.more = 1;
+		params.bfr_len = bs;
+	}
+
+	skb = create_final_hash_wr(req, &params);
+	if (!skb)
+		return -ENOMEM;
+
+	skb->dev = u_ctx->lldi.ports[0];
+	set_wr_txq(skb, CPL_PRIORITY_DATA, ctx->tx_channel_id);
+	chcr_send_wr(skb);
+	return -EINPROGRESS;
+}
+
+static int chcr_ahash_export(struct ahash_request *areq, void *out)
+{
+	struct chcr_ahash_req_ctx *req_ctx = ahash_request_ctx(areq);
+	struct chcr_ahash_req_ctx *state = out;
+
+	state->bfr_len = req_ctx->bfr_len;
+	state->data_len = req_ctx->data_len;
+	memcpy(state->bfr, req_ctx->bfr, CHCR_HASH_MAX_BLOCK_SIZE_128);
+	memcpy(state->partial_hash, req_ctx->partial_hash,
+	       CHCR_HASH_MAX_DIGEST_SIZE);
+	return 0;
+}
+
+static int chcr_ahash_import(struct ahash_request *areq, const void *in)
+{
+	struct chcr_ahash_req_ctx *req_ctx = ahash_request_ctx(areq);
+	struct chcr_ahash_req_ctx *state = (struct chcr_ahash_req_ctx *)in;
+
+	req_ctx->bfr_len = state->bfr_len;
+	req_ctx->data_len = state->data_len;
+	req_ctx->dummy_payload_ptr = NULL;
+	memcpy(req_ctx->bfr, state->bfr, CHCR_HASH_MAX_BLOCK_SIZE_128);
+	memcpy(req_ctx->partial_hash, state->partial_hash,
+	       CHCR_HASH_MAX_DIGEST_SIZE);
+	return 0;
+}
+
+static int chcr_ahash_setkey(struct crypto_ahash *tfm, const u8 *key,
+			     unsigned int keylen)
+{
+	struct chcr_context *ctx = crypto_tfm_ctx(crypto_ahash_tfm(tfm));
+	struct hmac_ctx *hmacctx = HMAC_CTX(ctx);
+	unsigned int digestsize = crypto_ahash_digestsize(tfm);
+	unsigned int bs = crypto_tfm_alg_blocksize(crypto_ahash_tfm(tfm));
+	unsigned int i, err = 0, updated_digestsize;
+
+	/*
+	 * use the key to calculate the ipad and opad. ipad will sent with the
+	 * first request's data. opad will be sent with the final hash result
+	 * ipad in hmacctx->ipad and opad in hmacctx->opad location
+	 */
+	if (!hmacctx->desc)
+		return -EINVAL;
+	if (keylen > bs) {
+		err = crypto_shash_digest(hmacctx->desc, key, keylen,
+					  hmacctx->ipad);
+		if (err)
+			goto out;
+		keylen = digestsize;
+	} else {
+		memcpy(hmacctx->ipad, key, keylen);
+	}
+	memset(hmacctx->ipad + keylen, 0, bs - keylen);
+	memcpy(hmacctx->opad, hmacctx->ipad, bs);
+
+	for (i = 0; i < bs / sizeof(int); i++) {
+		*((unsigned int *)(&hmacctx->ipad) + i) ^= IPAD_DATA;
+		*((unsigned int *)(&hmacctx->opad) + i) ^= OPAD_DATA;
+	}
+
+	updated_digestsize = digestsize;
+	if (digestsize == SHA224_DIGEST_SIZE)
+		updated_digestsize = SHA256_DIGEST_SIZE;
+	else if (digestsize == SHA384_DIGEST_SIZE)
+		updated_digestsize = SHA512_DIGEST_SIZE;
+	err = chcr_compute_partial_hash(hmacctx->desc, hmacctx->ipad,
+					hmacctx->ipad, digestsize);
+	if (err)
+		goto out;
+	chcr_change_order(hmacctx->ipad, updated_digestsize);
+
+	err = chcr_compute_partial_hash(hmacctx->desc, hmacctx->opad,
+					hmacctx->opad, digestsize);
+	if (err)
+		goto out;
+	chcr_change_order(hmacctx->opad, updated_digestsize);
+out:
+	return err;
+}
+
+static int chcr_aes_xts_setkey(struct crypto_ablkcipher *tfm, const u8 *key,
+			       unsigned int key_len)
+{
+	struct chcr_context *ctx = crypto_ablkcipher_ctx(tfm);
+	struct ablk_ctx *ablkctx = ABLK_CTX(ctx);
+	int status = 0;
+	unsigned short context_size = 0;
+
+	if ((key_len == (AES_KEYSIZE_128 << 1)) ||
+	    (key_len == (AES_KEYSIZE_256 << 1))) {
+		memcpy(ablkctx->key, key, key_len);
+		ablkctx->enckey_len = key_len;
+		context_size = (KEY_CONTEXT_HDR_SALT_AND_PAD + key_len) >> 4;
+		ablkctx->key_ctx_hdr =
+			FILL_KEY_CTX_HDR((key_len == AES_KEYSIZE_256) ?
+					 CHCR_KEYCTX_CIPHER_KEY_SIZE_128 :
+					 CHCR_KEYCTX_CIPHER_KEY_SIZE_256,
+					 CHCR_KEYCTX_NO_KEY, 1,
+					 0, context_size);
+		ablkctx->ciph_mode = CHCR_SCMD_CIPHER_MODE_AES_XTS;
+	} else {
+		crypto_tfm_set_flags((struct crypto_tfm *)tfm,
+				     CRYPTO_TFM_RES_BAD_KEY_LEN);
+		ablkctx->enckey_len = 0;
+		status = -EINVAL;
+	}
+	return status;
+}
+
+static int chcr_sha_init(struct ahash_request *areq)
+{
+	struct chcr_ahash_req_ctx *req_ctx = ahash_request_ctx(areq);
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(areq);
+	int digestsize =  crypto_ahash_digestsize(tfm);
+
+	req_ctx->data_len = 0;
+	req_ctx->dummy_payload_ptr = NULL;
+	req_ctx->bfr_len = 0;
+	req_ctx->skb = NULL;
+	req_ctx->result = 0;
+	copy_hash_init_values(req_ctx->partial_hash, digestsize);
+	return 0;
+}
+
+static int chcr_sha_cra_init(struct crypto_tfm *tfm)
+{
+	crypto_ahash_set_reqsize(__crypto_ahash_cast(tfm),
+				 sizeof(struct chcr_ahash_req_ctx));
+	return chcr_device_init(crypto_tfm_ctx(tfm));
+}
+
+static int chcr_hmac_init(struct ahash_request *areq)
+{
+	struct chcr_ahash_req_ctx *req_ctx = ahash_request_ctx(areq);
+	struct crypto_ahash *rtfm = crypto_ahash_reqtfm(areq);
+	struct chcr_context *ctx = crypto_tfm_ctx(crypto_ahash_tfm(rtfm));
+	struct hmac_ctx *hmacctx = HMAC_CTX(ctx);
+	unsigned int digestsize = crypto_ahash_digestsize(rtfm);
+	unsigned int bs = crypto_tfm_alg_blocksize(crypto_ahash_tfm(rtfm));
+
+	chcr_sha_init(areq);
+	req_ctx->data_len = bs;
+	if (is_hmac(crypto_ahash_tfm(rtfm))) {
+		if (digestsize == SHA224_DIGEST_SIZE)
+			memcpy(req_ctx->partial_hash, hmacctx->ipad,
+			       SHA256_DIGEST_SIZE);
+		else if (digestsize == SHA384_DIGEST_SIZE)
+			memcpy(req_ctx->partial_hash, hmacctx->ipad,
+			       SHA512_DIGEST_SIZE);
+		else
+			memcpy(req_ctx->partial_hash, hmacctx->ipad,
+			       digestsize);
+	}
+	return 0;
+}
+
+static int chcr_hmac_cra_init(struct crypto_tfm *tfm)
+{
+	struct chcr_context *ctx = crypto_tfm_ctx(tfm);
+	struct hmac_ctx *hmacctx = HMAC_CTX(ctx);
+	unsigned int digestsize =
+		crypto_ahash_digestsize(__crypto_ahash_cast(tfm));
+
+	crypto_ahash_set_reqsize(__crypto_ahash_cast(tfm),
+				 sizeof(struct chcr_ahash_req_ctx));
+	hmacctx->desc = chcr_alloc_shash(digestsize);
+	if (IS_ERR(hmacctx->desc))
+		return PTR_ERR(hmacctx->desc);
+	return chcr_device_init(crypto_tfm_ctx(tfm));
+}
+
+static void chcr_free_shash(struct shash_desc *desc)
+{
+	crypto_free_shash(desc->tfm);
+	kfree(desc);
+}
+
+static void chcr_hmac_cra_exit(struct crypto_tfm *tfm)
+{
+	struct chcr_context *ctx = crypto_tfm_ctx(tfm);
+	struct hmac_ctx *hmacctx = HMAC_CTX(ctx);
+
+	if (hmacctx->desc) {
+		chcr_free_shash(hmacctx->desc);
+		hmacctx->desc = NULL;
+	}
+}
+
+static struct chcr_alg_template driver_algs[] = {
+	/* AES-CBC */
+	{
+		.type = CRYPTO_ALG_TYPE_ABLKCIPHER,
+		.is_registered = 0,
+		.alg.crypto = {
+			.cra_name		= "cbc(aes)",
+			.cra_driver_name	= "cbc(aes-chcr)",
+			.cra_priority		= CHCR_CRA_PRIORITY,
+			.cra_flags		= CRYPTO_ALG_TYPE_BLKCIPHER |
+				CRYPTO_ALG_ASYNC,
+			.cra_blocksize		= AES_BLOCK_SIZE,
+			.cra_ctxsize		= sizeof(struct chcr_context)
+				+ sizeof(struct ablk_ctx),
+			.cra_alignmask		= 0,
+			.cra_type		= &crypto_ablkcipher_type,
+			.cra_module		= THIS_MODULE,
+			.cra_init		= chcr_cra_init,
+			.cra_exit		= NULL,
+			.cra_u.ablkcipher	= {
+				.min_keysize	= AES_MIN_KEY_SIZE,
+				.max_keysize	= AES_MAX_KEY_SIZE,
+				.ivsize		= AES_BLOCK_SIZE,
+				.setkey			= chcr_aes_cbc_setkey,
+				.encrypt		= chcr_aes_encrypt,
+				.decrypt		= chcr_aes_decrypt,
+			}
+		}
+	},
+	{
+		.type = CRYPTO_ALG_TYPE_ABLKCIPHER,
+		.is_registered = 0,
+		.alg.crypto =   {
+			.cra_name		= "xts(aes)",
+			.cra_driver_name	= "xts(aes-chcr)",
+			.cra_priority		= CHCR_CRA_PRIORITY,
+			.cra_flags		= CRYPTO_ALG_TYPE_BLKCIPHER |
+				CRYPTO_ALG_ASYNC,
+			.cra_blocksize		= AES_BLOCK_SIZE,
+			.cra_ctxsize		= sizeof(struct chcr_context) +
+				sizeof(struct ablk_ctx),
+			.cra_alignmask		= 0,
+			.cra_type		= &crypto_ablkcipher_type,
+			.cra_module		= THIS_MODULE,
+			.cra_init		= chcr_cra_init,
+			.cra_exit		= NULL,
+			.cra_u = {
+				.ablkcipher = {
+					.min_keysize	= 2 * AES_MIN_KEY_SIZE,
+					.max_keysize	= 2 * AES_MAX_KEY_SIZE,
+					.ivsize		= AES_BLOCK_SIZE,
+					.setkey		= chcr_aes_xts_setkey,
+					.encrypt	= chcr_aes_encrypt,
+					.decrypt	= chcr_aes_decrypt,
+				}
+			}
+		}
+	},
+	/* SHA */
+	{
+		.type = CRYPTO_ALG_TYPE_AHASH,
+		.is_registered = 0,
+		.alg.hash = {
+			.halg.digestsize = SHA1_DIGEST_SIZE,
+			.halg.base = {
+				.cra_name = "sha1",
+				.cra_driver_name = "sha1-chcr",
+				.cra_blocksize = SHA1_BLOCK_SIZE,
+			}
+		}
+	},
+	{
+		.type = CRYPTO_ALG_TYPE_AHASH,
+		.is_registered = 0,
+		.alg.hash = {
+			.halg.digestsize = SHA256_DIGEST_SIZE,
+			.halg.base = {
+				.cra_name = "sha256",
+				.cra_driver_name = "sha256-chcr",
+				.cra_blocksize = SHA256_BLOCK_SIZE,
+			}
+		}
+	},
+	{
+		.type = CRYPTO_ALG_TYPE_AHASH,
+		.is_registered = 0,
+		.alg.hash = {
+			.halg.digestsize = SHA224_DIGEST_SIZE,
+			.halg.base = {
+				.cra_name = "sha224",
+				.cra_driver_name = "sha224-chcr",
+				.cra_blocksize = SHA224_BLOCK_SIZE,
+			}
+		}
+	},
+	{
+		.type = CRYPTO_ALG_TYPE_AHASH,
+		.is_registered = 0,
+		.alg.hash = {
+			.halg.digestsize = SHA384_DIGEST_SIZE,
+			.halg.base = {
+				.cra_name = "sha384",
+				.cra_driver_name = "sha384-chcr",
+				.cra_blocksize = SHA384_BLOCK_SIZE,
+			}
+		}
+	},
+	{
+		.type = CRYPTO_ALG_TYPE_AHASH,
+		.is_registered = 0,
+		.alg.hash = {
+			.halg.digestsize = SHA512_DIGEST_SIZE,
+			.halg.base = {
+				.cra_name = "sha512",
+				.cra_driver_name = "sha512-chcr",
+				.cra_blocksize = SHA512_BLOCK_SIZE,
+			}
+		}
+	},
+	/* HMAC */
+	{
+		.type = CRYPTO_ALG_TYPE_HMAC,
+		.is_registered = 0,
+		.alg.hash = {
+			.halg.digestsize = SHA1_DIGEST_SIZE,
+			.halg.base = {
+				.cra_name = "hmac(sha1)",
+				.cra_driver_name = "hmac(sha1-chcr)",
+				.cra_blocksize = SHA1_BLOCK_SIZE,
+			}
+		}
+	},
+	{
+		.type = CRYPTO_ALG_TYPE_HMAC,
+		.is_registered = 0,
+		.alg.hash = {
+			.halg.digestsize = SHA224_DIGEST_SIZE,
+			.halg.base = {
+				.cra_name = "hmac(sha224)",
+				.cra_driver_name = "hmac(sha224-chcr)",
+				.cra_blocksize = SHA224_BLOCK_SIZE,
+			}
+		}
+	},
+	{
+		.type = CRYPTO_ALG_TYPE_HMAC,
+		.is_registered = 0,
+		.alg.hash = {
+			.halg.digestsize = SHA256_DIGEST_SIZE,
+			.halg.base = {
+				.cra_name = "hmac(sha256)",
+				.cra_driver_name = "hmac(sha256-chcr)",
+				.cra_blocksize = SHA256_BLOCK_SIZE,
+			}
+		}
+	},
+	{
+		.type = CRYPTO_ALG_TYPE_HMAC,
+		.is_registered = 0,
+		.alg.hash = {
+			.halg.digestsize = SHA384_DIGEST_SIZE,
+			.halg.base = {
+				.cra_name = "hmac(sha384)",
+				.cra_driver_name = "hmac(sha384-chcr)",
+				.cra_blocksize = SHA384_BLOCK_SIZE,
+			}
+		}
+	},
+	{
+		.type = CRYPTO_ALG_TYPE_HMAC,
+		.is_registered = 0,
+		.alg.hash = {
+			.halg.digestsize = SHA512_DIGEST_SIZE,
+			.halg.base = {
+				.cra_name = "hmac(sha512)",
+				.cra_driver_name = "hmac(sha512-chcr)",
+				.cra_blocksize = SHA512_BLOCK_SIZE,
+			}
+		}
+	},
+};
+
+/*
+ *	chcr_unregister_alg - Deregister crypto algorithms with
+ *	kernel framework.
+ */
+static int chcr_unregister_alg(void)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(driver_algs); i++) {
+		switch (driver_algs[i].type & CRYPTO_ALG_TYPE_MASK) {
+		case CRYPTO_ALG_TYPE_ABLKCIPHER:
+			if (driver_algs[i].is_registered)
+				crypto_unregister_alg(
+						&driver_algs[i].alg.crypto);
+			break;
+		case CRYPTO_ALG_TYPE_AHASH:
+			if (driver_algs[i].is_registered)
+				crypto_unregister_ahash(
+						&driver_algs[i].alg.hash);
+			break;
+		}
+		driver_algs[i].is_registered = 0;
+	}
+	return 0;
+}
+
+#define SZ_AHASH_CTX sizeof(struct chcr_context)
+#define SZ_AHASH_H_CTX (sizeof(struct chcr_context) + sizeof(struct hmac_ctx))
+#define SZ_AHASH_REQ_CTX sizeof(struct chcr_ahash_req_ctx)
+#define AHASH_CRA_FLAGS (CRYPTO_ALG_TYPE_AHASH | CRYPTO_ALG_ASYNC)
+
+/*
+ *	chcr_register_alg - Register crypto algorithms with kernel framework.
+ */
+static int chcr_register_alg(void)
+{
+	struct crypto_alg ai;
+	struct ahash_alg *a_hash;
+	int err = 0, i;
+	char *name = NULL;
+
+	for (i = 0; i < ARRAY_SIZE(driver_algs); i++) {
+		if (driver_algs[i].is_registered)
+			continue;
+		switch (driver_algs[i].type & CRYPTO_ALG_TYPE_MASK) {
+		case CRYPTO_ALG_TYPE_ABLKCIPHER:
+			err = crypto_register_alg(&driver_algs[i].alg.crypto);
+			name = driver_algs[i].alg.crypto.cra_driver_name;
+			break;
+		case CRYPTO_ALG_TYPE_AHASH:
+			a_hash = &driver_algs[i].alg.hash;
+			a_hash->update = chcr_ahash_update;
+			a_hash->final = chcr_ahash_final;
+			a_hash->finup = chcr_ahash_finup;
+			a_hash->digest = chcr_ahash_digest;
+			a_hash->export = chcr_ahash_export;
+			a_hash->import = chcr_ahash_import;
+			a_hash->halg.statesize = SZ_AHASH_REQ_CTX;
+			a_hash->halg.base.cra_priority = CHCR_CRA_PRIORITY;
+			a_hash->halg.base.cra_module = THIS_MODULE;
+			a_hash->halg.base.cra_flags = AHASH_CRA_FLAGS;
+			a_hash->halg.base.cra_alignmask = 0;
+			a_hash->halg.base.cra_exit = NULL;
+			a_hash->halg.base.cra_type = &crypto_ahash_type;
+
+			if (driver_algs[i].type == CRYPTO_ALG_TYPE_HMAC) {
+				a_hash->halg.base.cra_init = chcr_hmac_cra_init;
+				a_hash->halg.base.cra_exit = chcr_hmac_cra_exit;
+				a_hash->init = chcr_hmac_init;
+				a_hash->setkey = chcr_ahash_setkey;
+				a_hash->halg.base.cra_ctxsize = SZ_AHASH_H_CTX;
+			} else {
+				a_hash->init = chcr_sha_init;
+				a_hash->halg.base.cra_ctxsize = SZ_AHASH_CTX;
+				a_hash->halg.base.cra_init = chcr_sha_cra_init;
+			}
+			err = crypto_register_ahash(&driver_algs[i].alg.hash);
+			ai = driver_algs[i].alg.hash.halg.base;
+			name = ai.cra_driver_name;
+			break;
+		}
+		if (err) {
+			pr_err("chcr : %s : Algorithm registration failed\n",
+			       name);
+			goto register_err;
+		} else {
+			driver_algs[i].is_registered = 1;
+		}
+	}
+	return 0;
+
+register_err:
+	chcr_unregister_alg();
+	return err;
+}
+
+/*
+ *	start_crypto - Register the crypto algorithms.
+ *	This should called once when the first device comesup. After this
+ *	kernel will start calling driver APIs for crypto operations.
+ */
+int start_crypto(void)
+{
+	return chcr_register_alg();
+}
+
+/*
+ *	stop_crypto - Deregister all the crypto algorithms with kernel.
+ *	This should be called once when the last device goes down. After this
+ *	kernel will not call the driver API for crypto operations.
+ */
+int stop_crypto(void)
+{
+	chcr_unregister_alg();
+	return 0;
+}
diff --git a/drivers/crypto/chelsio/chcr_algo.h b/drivers/crypto/chelsio/chcr_algo.h
new file mode 100644
index 000000000000..ec64fbcdeb49
--- /dev/null
+++ b/drivers/crypto/chelsio/chcr_algo.h
@@ -0,0 +1,471 @@
+/*
+ * This file is part of the Chelsio T6 Crypto driver for Linux.
+ *
+ * Copyright (c) 2003-2016 Chelsio Communications, Inc. All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * 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 __CHCR_ALGO_H__
+#define __CHCR_ALGO_H__
+
+/* Crypto key context */
+#define KEY_CONTEXT_CTX_LEN_S           24
+#define KEY_CONTEXT_CTX_LEN_M           0xff
+#define KEY_CONTEXT_CTX_LEN_V(x)        ((x) << KEY_CONTEXT_CTX_LEN_S)
+#define KEY_CONTEXT_CTX_LEN_G(x) \
+	(((x) >> KEY_CONTEXT_CTX_LEN_S) & KEY_CONTEXT_CTX_LEN_M)
+
+#define KEY_CONTEXT_DUAL_CK_S      12
+#define KEY_CONTEXT_DUAL_CK_M      0x1
+#define KEY_CONTEXT_DUAL_CK_V(x)   ((x) << KEY_CONTEXT_DUAL_CK_S)
+#define KEY_CONTEXT_DUAL_CK_G(x)   \
+(((x) >> KEY_CONTEXT_DUAL_CK_S) & KEY_CONTEXT_DUAL_CK_M)
+#define KEY_CONTEXT_DUAL_CK_F      KEY_CONTEXT_DUAL_CK_V(1U)
+
+#define KEY_CONTEXT_SALT_PRESENT_S      10
+#define KEY_CONTEXT_SALT_PRESENT_M      0x1
+#define KEY_CONTEXT_SALT_PRESENT_V(x)   ((x) << KEY_CONTEXT_SALT_PRESENT_S)
+#define KEY_CONTEXT_SALT_PRESENT_G(x)   \
+	(((x) >> KEY_CONTEXT_SALT_PRESENT_S) & \
+	 KEY_CONTEXT_SALT_PRESENT_M)
+#define KEY_CONTEXT_SALT_PRESENT_F      KEY_CONTEXT_SALT_PRESENT_V(1U)
+
+#define KEY_CONTEXT_VALID_S     0
+#define KEY_CONTEXT_VALID_M     0x1
+#define KEY_CONTEXT_VALID_V(x)  ((x) << KEY_CONTEXT_VALID_S)
+#define KEY_CONTEXT_VALID_G(x)  \
+	(((x) >> KEY_CONTEXT_VALID_S) & \
+	 KEY_CONTEXT_VALID_M)
+#define KEY_CONTEXT_VALID_F     KEY_CONTEXT_VALID_V(1U)
+
+#define KEY_CONTEXT_CK_SIZE_S           6
+#define KEY_CONTEXT_CK_SIZE_M           0xf
+#define KEY_CONTEXT_CK_SIZE_V(x)        ((x) << KEY_CONTEXT_CK_SIZE_S)
+#define KEY_CONTEXT_CK_SIZE_G(x)        \
+	(((x) >> KEY_CONTEXT_CK_SIZE_S) & KEY_CONTEXT_CK_SIZE_M)
+
+#define KEY_CONTEXT_MK_SIZE_S           2
+#define KEY_CONTEXT_MK_SIZE_M           0xf
+#define KEY_CONTEXT_MK_SIZE_V(x)        ((x) << KEY_CONTEXT_MK_SIZE_S)
+#define KEY_CONTEXT_MK_SIZE_G(x)        \
+	(((x) >> KEY_CONTEXT_MK_SIZE_S) & KEY_CONTEXT_MK_SIZE_M)
+
+#define KEY_CONTEXT_OPAD_PRESENT_S      11
+#define KEY_CONTEXT_OPAD_PRESENT_M      0x1
+#define KEY_CONTEXT_OPAD_PRESENT_V(x)   ((x) << KEY_CONTEXT_OPAD_PRESENT_S)
+#define KEY_CONTEXT_OPAD_PRESENT_G(x)   \
+	(((x) >> KEY_CONTEXT_OPAD_PRESENT_S) & \
+	 KEY_CONTEXT_OPAD_PRESENT_M)
+#define KEY_CONTEXT_OPAD_PRESENT_F      KEY_CONTEXT_OPAD_PRESENT_V(1U)
+
+#define CHCR_HASH_MAX_DIGEST_SIZE 64
+#define CHCR_MAX_SHA_DIGEST_SIZE 64
+
+#define IPSEC_TRUNCATED_ICV_SIZE 12
+#define TLS_TRUNCATED_HMAC_SIZE 10
+#define CBCMAC_DIGEST_SIZE 16
+#define MAX_HASH_NAME 20
+
+#define SHA1_INIT_STATE_5X4B    5
+#define SHA256_INIT_STATE_8X4B  8
+#define SHA512_INIT_STATE_8X8B  8
+#define SHA1_INIT_STATE         SHA1_INIT_STATE_5X4B
+#define SHA224_INIT_STATE       SHA256_INIT_STATE_8X4B
+#define SHA256_INIT_STATE       SHA256_INIT_STATE_8X4B
+#define SHA384_INIT_STATE       SHA512_INIT_STATE_8X8B
+#define SHA512_INIT_STATE       SHA512_INIT_STATE_8X8B
+
+#define DUMMY_BYTES 16
+
+#define IPAD_DATA 0x36363636
+#define OPAD_DATA 0x5c5c5c5c
+
+#define TRANSHDR_SIZE(alignedkctx_len)\
+	(sizeof(struct ulptx_idata) +\
+	 sizeof(struct ulp_txpkt) +\
+	 sizeof(struct fw_crypto_lookaside_wr) +\
+	 sizeof(struct cpl_tx_sec_pdu) +\
+	 (alignedkctx_len))
+#define CIPHER_TRANSHDR_SIZE(alignedkctx_len, sge_pairs) \
+	(TRANSHDR_SIZE(alignedkctx_len) + sge_pairs +\
+	 sizeof(struct cpl_rx_phys_dsgl))
+#define HASH_TRANSHDR_SIZE(alignedkctx_len)\
+	(TRANSHDR_SIZE(alignedkctx_len) + DUMMY_BYTES)
+
+#define SEC_CPL_OFFSET (sizeof(struct fw_crypto_lookaside_wr) + \
+			sizeof(struct ulp_txpkt) + \
+			sizeof(struct ulptx_idata))
+
+#define FILL_SEC_CPL_OP_IVINSR(id, len, hldr, ofst)      \
+	htonl( \
+	       CPL_TX_SEC_PDU_OPCODE_V(CPL_TX_SEC_PDU) | \
+	       CPL_TX_SEC_PDU_RXCHID_V((id)) | \
+	       CPL_TX_SEC_PDU_ACKFOLLOWS_V(0) | \
+	       CPL_TX_SEC_PDU_ULPTXLPBK_V(1) | \
+	       CPL_TX_SEC_PDU_CPLLEN_V((len)) | \
+	       CPL_TX_SEC_PDU_PLACEHOLDER_V((hldr)) | \
+	       CPL_TX_SEC_PDU_IVINSRTOFST_V((ofst)))
+
+#define  FILL_SEC_CPL_CIPHERSTOP_HI(a_start, a_stop, c_start, c_stop_hi) \
+	htonl( \
+	       CPL_TX_SEC_PDU_AADSTART_V((a_start)) | \
+	       CPL_TX_SEC_PDU_AADSTOP_V((a_stop)) | \
+	       CPL_TX_SEC_PDU_CIPHERSTART_V((c_start)) | \
+	       CPL_TX_SEC_PDU_CIPHERSTOP_HI_V((c_stop_hi)))
+
+#define  FILL_SEC_CPL_AUTHINSERT(c_stop_lo, a_start, a_stop, a_inst) \
+	htonl( \
+	       CPL_TX_SEC_PDU_CIPHERSTOP_LO_V((c_stop_lo)) | \
+		CPL_TX_SEC_PDU_AUTHSTART_V((a_start)) | \
+		CPL_TX_SEC_PDU_AUTHSTOP_V((a_stop)) | \
+		CPL_TX_SEC_PDU_AUTHINSERT_V((a_inst)))
+
+#define  FILL_SEC_CPL_SCMD0_SEQNO(ctrl, seq, cmode, amode, opad, size, nivs)  \
+		htonl( \
+		SCMD_SEQ_NO_CTRL_V(0) | \
+		SCMD_STATUS_PRESENT_V(0) | \
+		SCMD_PROTO_VERSION_V(CHCR_SCMD_PROTO_VERSION_GENERIC) | \
+		SCMD_ENC_DEC_CTRL_V((ctrl)) | \
+		SCMD_CIPH_AUTH_SEQ_CTRL_V((seq)) | \
+		SCMD_CIPH_MODE_V((cmode)) | \
+		SCMD_AUTH_MODE_V((amode)) | \
+		SCMD_HMAC_CTRL_V((opad)) | \
+		SCMD_IV_SIZE_V((size)) | \
+		SCMD_NUM_IVS_V((nivs)))
+
+#define FILL_SEC_CPL_IVGEN_HDRLEN(last, more, ctx_in, mac, ivdrop, len) htonl( \
+		SCMD_ENB_DBGID_V(0) | \
+		SCMD_IV_GEN_CTRL_V(0) | \
+		SCMD_LAST_FRAG_V((last)) | \
+		SCMD_MORE_FRAGS_V((more)) | \
+		SCMD_TLS_COMPPDU_V(0) | \
+		SCMD_KEY_CTX_INLINE_V((ctx_in)) | \
+		SCMD_TLS_FRAG_ENABLE_V(0) | \
+		SCMD_MAC_ONLY_V((mac)) |  \
+		SCMD_AADIVDROP_V((ivdrop)) | \
+		SCMD_HDR_LEN_V((len)))
+
+#define  FILL_KEY_CTX_HDR(ck_size, mk_size, d_ck, opad, ctx_len) \
+		htonl(KEY_CONTEXT_VALID_V(1) | \
+		      KEY_CONTEXT_CK_SIZE_V((ck_size)) | \
+		      KEY_CONTEXT_MK_SIZE_V(mk_size) | \
+		      KEY_CONTEXT_DUAL_CK_V((d_ck)) | \
+		      KEY_CONTEXT_OPAD_PRESENT_V((opad)) | \
+		      KEY_CONTEXT_SALT_PRESENT_V(1) | \
+		      KEY_CONTEXT_CTX_LEN_V((ctx_len)))
+
+#define FILL_WR_OP_CCTX_SIZE(len, ctx_len) \
+		htonl( \
+			FW_CRYPTO_LOOKASIDE_WR_OPCODE_V( \
+			FW_CRYPTO_LOOKASIDE_WR) | \
+			FW_CRYPTO_LOOKASIDE_WR_COMPL_V(0) | \
+			FW_CRYPTO_LOOKASIDE_WR_IMM_LEN_V((len)) | \
+			FW_CRYPTO_LOOKASIDE_WR_CCTX_LOC_V(1) | \
+			FW_CRYPTO_LOOKASIDE_WR_CCTX_SIZE_V((ctx_len)))
+
+#define FILL_WR_RX_Q_ID(cid, qid, wr_iv) \
+		htonl( \
+			FW_CRYPTO_LOOKASIDE_WR_RX_CHID_V((cid)) | \
+			FW_CRYPTO_LOOKASIDE_WR_RX_Q_ID_V((qid)) | \
+			FW_CRYPTO_LOOKASIDE_WR_LCB_V(0) | \
+			FW_CRYPTO_LOOKASIDE_WR_IV_V((wr_iv)))
+
+#define FILL_ULPTX_CMD_DEST(cid) \
+	htonl(ULPTX_CMD_V(ULP_TX_PKT) | \
+	      ULP_TXPKT_DEST_V(0) | \
+	      ULP_TXPKT_DATAMODIFY_V(0) | \
+	      ULP_TXPKT_CHANNELID_V((cid)) | \
+	      ULP_TXPKT_RO_V(1) | \
+	      ULP_TXPKT_FID_V(0))
+
+#define KEYCTX_ALIGN_PAD(bs) ({unsigned int _bs = (bs);\
+			      _bs == SHA1_DIGEST_SIZE ? 12 : 0; })
+
+#define FILL_PLD_SIZE_HASH_SIZE(payload_sgl_len, sgl_lengths, total_frags) \
+	htonl(FW_CRYPTO_LOOKASIDE_WR_PLD_SIZE_V(payload_sgl_len ? \
+						sgl_lengths[total_frags] : 0) |\
+	      FW_CRYPTO_LOOKASIDE_WR_HASH_SIZE_V(0))
+
+#define FILL_LEN_PKD(calc_tx_flits_ofld, skb) \
+	htonl(FW_CRYPTO_LOOKASIDE_WR_LEN16_V(DIV_ROUND_UP((\
+					   calc_tx_flits_ofld(skb) * 8), 16)))
+
+#define FILL_CMD_MORE(immdatalen) htonl(ULPTX_CMD_V(ULP_TX_SC_IMM) |\
+					ULP_TX_SC_MORE_V((immdatalen) ? 0 : 1))
+
+#define MAX_NK 8
+#define CRYPTO_MAX_IMM_TX_PKT_LEN 256
+
+struct algo_param {
+	unsigned int auth_mode;
+	unsigned int mk_size;
+	unsigned int result_size;
+};
+
+struct hash_wr_param {
+	unsigned int opad_needed;
+	unsigned int more;
+	unsigned int last;
+	struct algo_param alg_prm;
+	unsigned int sg_len;
+	unsigned int bfr_len;
+	u64 scmd1;
+};
+
+enum {
+	AES_KEYLENGTH_128BIT = 128,
+	AES_KEYLENGTH_192BIT = 192,
+	AES_KEYLENGTH_256BIT = 256
+};
+
+enum {
+	KEYLENGTH_3BYTES = 3,
+	KEYLENGTH_4BYTES = 4,
+	KEYLENGTH_6BYTES = 6,
+	KEYLENGTH_8BYTES = 8
+};
+
+enum {
+	NUMBER_OF_ROUNDS_10 = 10,
+	NUMBER_OF_ROUNDS_12 = 12,
+	NUMBER_OF_ROUNDS_14 = 14,
+};
+
+/*
+ * CCM defines values of 4, 6, 8, 10, 12, 14, and 16 octets,
+ * where they indicate the size of the integrity check value (ICV)
+ */
+enum {
+	AES_CCM_ICV_4   = 4,
+	AES_CCM_ICV_6   = 6,
+	AES_CCM_ICV_8   = 8,
+	AES_CCM_ICV_10  = 10,
+	AES_CCM_ICV_12  = 12,
+	AES_CCM_ICV_14  = 14,
+	AES_CCM_ICV_16 = 16
+};
+
+struct hash_op_params {
+	unsigned char mk_size;
+	unsigned char pad_align;
+	unsigned char auth_mode;
+	char hash_name[MAX_HASH_NAME];
+	unsigned short block_size;
+	unsigned short word_size;
+	unsigned short ipad_size;
+};
+
+struct phys_sge_pairs {
+	__be16 len[8];
+	__be64 addr[8];
+};
+
+struct phys_sge_parm {
+	unsigned int nents;
+	unsigned int obsize;
+	unsigned short qid;
+	unsigned char align;
+};
+
+struct crypto_result {
+	struct completion completion;
+	int err;
+};
+
+static const u32 sha1_init[SHA1_DIGEST_SIZE / 4] = {
+		SHA1_H0, SHA1_H1, SHA1_H2, SHA1_H3, SHA1_H4,
+};
+
+static const u32 sha224_init[SHA256_DIGEST_SIZE / 4] = {
+		SHA224_H0, SHA224_H1, SHA224_H2, SHA224_H3,
+		SHA224_H4, SHA224_H5, SHA224_H6, SHA224_H7,
+};
+
+static const u32 sha256_init[SHA256_DIGEST_SIZE / 4] = {
+		SHA256_H0, SHA256_H1, SHA256_H2, SHA256_H3,
+		SHA256_H4, SHA256_H5, SHA256_H6, SHA256_H7,
+};
+
+static const u64 sha384_init[SHA512_DIGEST_SIZE / 8] = {
+		SHA384_H0, SHA384_H1, SHA384_H2, SHA384_H3,
+		SHA384_H4, SHA384_H5, SHA384_H6, SHA384_H7,
+};
+
+static const u64 sha512_init[SHA512_DIGEST_SIZE / 8] = {
+		SHA512_H0, SHA512_H1, SHA512_H2, SHA512_H3,
+		SHA512_H4, SHA512_H5, SHA512_H6, SHA512_H7,
+};
+
+static inline void copy_hash_init_values(char *key, int digestsize)
+{
+	u8 i;
+	__be32 *dkey = (__be32 *)key;
+	u64 *ldkey = (u64 *)key;
+	__be64 *sha384 = (__be64 *)sha384_init;
+	__be64 *sha512 = (__be64 *)sha512_init;
+
+	switch (digestsize) {
+	case SHA1_DIGEST_SIZE:
+		for (i = 0; i < SHA1_INIT_STATE; i++)
+			dkey[i] = cpu_to_be32(sha1_init[i]);
+		break;
+	case SHA224_DIGEST_SIZE:
+		for (i = 0; i < SHA224_INIT_STATE; i++)
+			dkey[i] = cpu_to_be32(sha224_init[i]);
+		break;
+	case SHA256_DIGEST_SIZE:
+		for (i = 0; i < SHA256_INIT_STATE; i++)
+			dkey[i] = cpu_to_be32(sha256_init[i]);
+		break;
+	case SHA384_DIGEST_SIZE:
+		for (i = 0; i < SHA384_INIT_STATE; i++)
+			ldkey[i] = be64_to_cpu(sha384[i]);
+		break;
+	case SHA512_DIGEST_SIZE:
+		for (i = 0; i < SHA512_INIT_STATE; i++)
+			ldkey[i] = be64_to_cpu(sha512[i]);
+		break;
+	}
+}
+
+static const u8 sgl_lengths[20] = {
+	0, 1, 2, 3, 4, 4, 5, 6, 7, 7, 8, 9, 10, 10, 11, 12, 13, 13, 14, 15
+};
+
+/* Number of len fields(8) * size of one addr field */
+#define PHYSDSGL_MAX_LEN_SIZE 16
+
+static inline u16 get_space_for_phys_dsgl(unsigned int sgl_entr)
+{
+	/* len field size + addr field size */
+	return ((sgl_entr >> 3) + ((sgl_entr % 8) ?
+				   1 : 0)) * PHYSDSGL_MAX_LEN_SIZE +
+		(sgl_entr << 3) + ((sgl_entr % 2 ? 1 : 0) << 3);
+}
+
+/* The AES s-transform matrix (s-box). */
+static const u8 aes_sbox[256] = {
+	99,  124, 119, 123, 242, 107, 111, 197, 48,  1,   103, 43,  254, 215,
+	171, 118, 202, 130, 201, 125, 250, 89,  71,  240, 173, 212, 162, 175,
+	156, 164, 114, 192, 183, 253, 147, 38,  54,  63,  247, 204, 52,  165,
+	229, 241, 113, 216, 49,  21, 4,   199, 35,  195, 24,  150, 5, 154, 7,
+	18,  128, 226, 235, 39,  178, 117, 9,   131, 44,  26,  27,  110, 90,
+	160, 82,  59,  214, 179, 41,  227, 47,  132, 83,  209, 0,   237, 32,
+	252, 177, 91,  106, 203, 190, 57,  74,  76,  88,  207, 208, 239, 170,
+	251, 67,  77,  51,  133, 69,  249, 2,   127, 80,  60,  159, 168, 81,
+	163, 64,  143, 146, 157, 56,  245, 188, 182, 218, 33,  16,  255, 243,
+	210, 205, 12,  19,  236, 95,  151, 68,  23,  196, 167, 126, 61,  100,
+	93,  25,  115, 96,  129, 79,  220, 34,  42,  144, 136, 70,  238, 184,
+	20,  222, 94,  11,  219, 224, 50,  58,  10,  73,  6,   36,  92,  194,
+	211, 172, 98,  145, 149, 228, 121, 231, 200, 55,  109, 141, 213, 78,
+	169, 108, 86,  244, 234, 101, 122, 174, 8, 186, 120, 37,  46,  28, 166,
+	180, 198, 232, 221, 116, 31,  75,  189, 139, 138, 112, 62,  181, 102,
+	72,  3,   246, 14,  97,  53,  87,  185, 134, 193, 29,  158, 225, 248,
+	152, 17,  105, 217, 142, 148, 155, 30,  135, 233, 206, 85,  40,  223,
+	140, 161, 137, 13,  191, 230, 66,  104, 65,  153, 45,  15,  176, 84,
+	187, 22
+};
+
+static u32 aes_ks_subword(const u32 w)
+{
+	u8 bytes[4];
+
+	*(u32 *)(&bytes[0]) = w;
+	bytes[0] = aes_sbox[bytes[0]];
+	bytes[1] = aes_sbox[bytes[1]];
+	bytes[2] = aes_sbox[bytes[2]];
+	bytes[3] = aes_sbox[bytes[3]];
+	return *(u32 *)(&bytes[0]);
+}
+
+static u32 round_constant[11] = {
+	0x01000000, 0x02000000, 0x04000000, 0x08000000,
+	0x10000000, 0x20000000, 0x40000000, 0x80000000,
+	0x1B000000, 0x36000000, 0x6C000000
+};
+
+/* dec_key - OUTPUT - Reverse round key
+ * key - INPUT - key
+ * keylength - INPUT - length of the key in number of bits
+ */
+static inline void get_aes_decrypt_key(unsigned char *dec_key,
+				       const unsigned char *key,
+				       unsigned int keylength)
+{
+	u32 temp;
+	u32 w_ring[MAX_NK];
+	int i, j, k = 0;
+	u8  nr, nk;
+
+	switch (keylength) {
+	case AES_KEYLENGTH_128BIT:
+		nk = KEYLENGTH_4BYTES;
+		nr = NUMBER_OF_ROUNDS_10;
+		break;
+
+	case AES_KEYLENGTH_192BIT:
+		nk = KEYLENGTH_6BYTES;
+		nr = NUMBER_OF_ROUNDS_12;
+		break;
+	case AES_KEYLENGTH_256BIT:
+		nk = KEYLENGTH_8BYTES;
+		nr = NUMBER_OF_ROUNDS_14;
+		break;
+	default:
+		return;
+	}
+	for (i = 0; i < nk; i++ )
+		w_ring[i] = be32_to_cpu(*(u32 *)&key[4 * i]);
+
+	i = 0;
+	temp = w_ring[nk - 1];
+	while(i + nk < (nr + 1) * 4) {
+		if(!(i % nk)) {
+			/* RotWord(temp) */
+			temp = (temp << 8) | (temp >> 24);
+			temp = aes_ks_subword(temp);
+			temp ^= round_constant[i / nk];
+		}
+		else if (nk == 8 && (i % 4 == 0))
+			temp = aes_ks_subword(temp);
+		w_ring[i % nk] ^= temp;
+		temp = w_ring[i % nk];
+		i++;
+	}
+	for (k = 0, j = i % nk; k < nk; k++) {
+		*((u32 *)dec_key + k) = htonl(w_ring[j]);
+		j--;
+		if(j < 0)
+			j += nk;
+	}
+}
+
+#endif /* __CHCR_ALGO_H__ */
diff --git a/drivers/crypto/chelsio/chcr_core.c b/drivers/crypto/chelsio/chcr_core.c
new file mode 100644
index 000000000000..fb5f9bbfa09c
--- /dev/null
+++ b/drivers/crypto/chelsio/chcr_core.c
@@ -0,0 +1,238 @@
+/**
+ * This file is part of the Chelsio T4/T5/T6 Ethernet driver for Linux.
+ *
+ * Copyright (C) 2011-2016 Chelsio Communications.  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.
+ *
+ * Written and Maintained by:
+ * Manoj Malviya (manojmalviya@chelsio.com)
+ * Atul Gupta (atul.gupta@chelsio.com)
+ * Jitendra Lulla (jlulla@chelsio.com)
+ * Yeshaswi M R Gowda (yeshaswi@chelsio.com)
+ * Harsh Jain (harsh@chelsio.com)
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/skbuff.h>
+
+#include <crypto/aes.h>
+#include <crypto/hash.h>
+
+#include "t4_msg.h"
+#include "chcr_core.h"
+#include "cxgb4_uld.h"
+
+static LIST_HEAD(uld_ctx_list);
+static DEFINE_MUTEX(dev_mutex);
+static atomic_t dev_count;
+
+typedef int (*chcr_handler_func)(struct chcr_dev *dev, unsigned char *input);
+static int cpl_fw6_pld_handler(struct chcr_dev *dev, unsigned char *input);
+static void *chcr_uld_add(const struct cxgb4_lld_info *lld);
+static int chcr_uld_state_change(void *handle, enum cxgb4_state state);
+
+static chcr_handler_func work_handlers[NUM_CPL_CMDS] = {
+	[CPL_FW6_PLD] = cpl_fw6_pld_handler,
+};
+
+static struct cxgb4_uld_info chcr_uld_info = {
+	.name = DRV_MODULE_NAME,
+	.nrxq = MAX_ULD_QSETS,
+	.rxq_size = 1024,
+	.add = chcr_uld_add,
+	.state_change = chcr_uld_state_change,
+	.rx_handler = chcr_uld_rx_handler,
+};
+
+int assign_chcr_device(struct chcr_dev **dev)
+{
+	struct uld_ctx *u_ctx;
+
+	/*
+	 * Which device to use if multiple devices are available TODO
+	 * May be select the device based on round robin. One session
+	 * must go to the same device to maintain the ordering.
+	 */
+	mutex_lock(&dev_mutex); /* TODO ? */
+	u_ctx = list_first_entry(&uld_ctx_list, struct uld_ctx, entry);
+	if (!u_ctx) {
+		mutex_unlock(&dev_mutex);
+		return -ENXIO;
+	}
+
+	*dev = u_ctx->dev;
+	mutex_unlock(&dev_mutex);
+	return 0;
+}
+
+static int chcr_dev_add(struct uld_ctx *u_ctx)
+{
+	struct chcr_dev *dev;
+
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (!dev)
+		return -ENXIO;
+
+	spin_lock_init(&dev->lock_chcr_dev);
+	u_ctx->dev = dev;
+	dev->u_ctx = u_ctx;
+	atomic_inc(&dev_count);
+	return 0;
+}
+
+static int chcr_dev_remove(struct uld_ctx *u_ctx)
+{
+	kfree(u_ctx->dev);
+	u_ctx->dev = NULL;
+	atomic_dec(&dev_count);
+	return 0;
+}
+
+static int cpl_fw6_pld_handler(struct chcr_dev *dev,
+			       unsigned char *input)
+{
+	struct crypto_async_request *req;
+	struct cpl_fw6_pld *fw6_pld;
+	u32 ack_err_status = 0;
+	int error_status = 0;
+
+	fw6_pld = (struct cpl_fw6_pld *)input;
+	req = (struct crypto_async_request *)(uintptr_t)be64_to_cpu(
+						    fw6_pld->data[1]);
+
+	ack_err_status =
+		ntohl(*(__be32 *)((unsigned char *)&fw6_pld->data[0] + 4));
+	if (ack_err_status) {
+		if (CHK_MAC_ERR_BIT(ack_err_status) ||
+		    CHK_PAD_ERR_BIT(ack_err_status))
+			error_status = -EINVAL;
+	}
+	/* call completion callback with failure status */
+	if (req) {
+		if (!chcr_handle_resp(req, input, error_status))
+			req->complete(req, error_status);
+		else
+			return -EINVAL;
+	} else {
+		pr_err("Incorrect request address from the firmware\n");
+		return -EFAULT;
+	}
+	return 0;
+}
+
+int chcr_send_wr(struct sk_buff *skb)
+{
+	return cxgb4_ofld_send(skb->dev, skb);
+}
+
+static void *chcr_uld_add(const struct cxgb4_lld_info *lld)
+{
+	struct uld_ctx *u_ctx;
+
+	/* Create the device and add it in the device list */
+	u_ctx = kzalloc(sizeof(*u_ctx), GFP_KERNEL);
+	if (!u_ctx) {
+		u_ctx = ERR_PTR(-ENOMEM);
+		goto out;
+	}
+	u_ctx->lldi = *lld;
+	mutex_lock(&dev_mutex);
+	list_add_tail(&u_ctx->entry, &uld_ctx_list);
+	mutex_unlock(&dev_mutex);
+out:
+	return u_ctx;
+}
+
+int chcr_uld_rx_handler(void *handle, const __be64 *rsp,
+			const struct pkt_gl *pgl)
+{
+	struct uld_ctx *u_ctx = (struct uld_ctx *)handle;
+	struct chcr_dev *dev = u_ctx->dev;
+	const struct cpl_act_establish *rpl = (struct cpl_act_establish
+					       *)rsp;
+
+	if (rpl->ot.opcode != CPL_FW6_PLD) {
+		pr_err("Unsupported opcode\n");
+		return 0;
+	}
+
+	if (!pgl)
+		work_handlers[rpl->ot.opcode](dev, (unsigned char *)&rsp[1]);
+	else
+		work_handlers[rpl->ot.opcode](dev, pgl->va);
+	return 0;
+}
+
+static int chcr_uld_state_change(void *handle, enum cxgb4_state state)
+{
+	struct uld_ctx *u_ctx = handle;
+	int ret = 0;
+
+	switch (state) {
+	case CXGB4_STATE_UP:
+		if (!u_ctx->dev) {
+			ret = chcr_dev_add(u_ctx);
+			if (ret != 0)
+				return ret;
+		}
+		if (atomic_read(&dev_count) == 1)
+			ret = start_crypto();
+		break;
+
+	case CXGB4_STATE_DETACH:
+		if (u_ctx->dev) {
+			mutex_lock(&dev_mutex);
+			chcr_dev_remove(u_ctx);
+			mutex_unlock(&dev_mutex);
+		}
+		if (!atomic_read(&dev_count))
+			stop_crypto();
+		break;
+
+	case CXGB4_STATE_START_RECOVERY:
+	case CXGB4_STATE_DOWN:
+	default:
+		break;
+	}
+	return ret;
+}
+
+static int __init chcr_crypto_init(void)
+{
+	if (cxgb4_register_uld(CXGB4_ULD_CRYPTO, &chcr_uld_info)) {
+		pr_err("ULD register fail: No chcr crypto support in cxgb4");
+		return -1;
+	}
+
+	return 0;
+}
+
+static void __exit chcr_crypto_exit(void)
+{
+	struct uld_ctx *u_ctx, *tmp;
+
+	if (atomic_read(&dev_count))
+		stop_crypto();
+
+	/* Remove all devices from list */
+	mutex_lock(&dev_mutex);
+	list_for_each_entry_safe(u_ctx, tmp, &uld_ctx_list, entry) {
+		if (u_ctx->dev)
+			chcr_dev_remove(u_ctx);
+		kfree(u_ctx);
+	}
+	mutex_unlock(&dev_mutex);
+	cxgb4_unregister_uld(CXGB4_ULD_CRYPTO);
+}
+
+module_init(chcr_crypto_init);
+module_exit(chcr_crypto_exit);
+
+MODULE_DESCRIPTION("Crypto Co-processor for Chelsio Terminator cards.");
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Chelsio Communications");
+MODULE_VERSION(DRV_VERSION);
diff --git a/drivers/crypto/chelsio/chcr_core.h b/drivers/crypto/chelsio/chcr_core.h
new file mode 100644
index 000000000000..2a5c671a4232
--- /dev/null
+++ b/drivers/crypto/chelsio/chcr_core.h
@@ -0,0 +1,80 @@
+/*
+ * This file is part of the Chelsio T6 Crypto driver for Linux.
+ *
+ * Copyright (c) 2003-2016 Chelsio Communications, Inc. All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * 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 __CHCR_CORE_H__
+#define __CHCR_CORE_H__
+
+#include <crypto/algapi.h>
+#include "t4_hw.h"
+#include "cxgb4.h"
+#include "cxgb4_uld.h"
+
+#define DRV_MODULE_NAME "chcr"
+#define DRV_VERSION "1.0.0.0"
+
+#define MAX_PENDING_REQ_TO_HW 20
+#define CHCR_TEST_RESPONSE_TIMEOUT 1000
+
+#define PAD_ERROR_BIT		1
+#define CHK_PAD_ERR_BIT(x)	(((x) >> PAD_ERROR_BIT) & 1)
+
+#define MAC_ERROR_BIT		0
+#define CHK_MAC_ERR_BIT(x)	(((x) >> MAC_ERROR_BIT) & 1)
+
+struct uld_ctx;
+
+struct chcr_dev {
+	/* Request submited to h/w and waiting for response. */
+	spinlock_t lock_chcr_dev;
+	struct crypto_queue pending_queue;
+	struct uld_ctx *u_ctx;
+	unsigned char tx_channel_id;
+};
+
+struct uld_ctx {
+	struct list_head entry;
+	struct cxgb4_lld_info lldi;
+	struct chcr_dev *dev;
+};
+
+int assign_chcr_device(struct chcr_dev **dev);
+int chcr_send_wr(struct sk_buff *skb);
+int start_crypto(void);
+int stop_crypto(void);
+int chcr_uld_rx_handler(void *handle, const __be64 *rsp,
+			const struct pkt_gl *pgl);
+int chcr_handle_resp(struct crypto_async_request *req, unsigned char *input,
+		     int err);
+#endif /* __CHCR_CORE_H__ */
diff --git a/drivers/crypto/chelsio/chcr_crypto.h b/drivers/crypto/chelsio/chcr_crypto.h
new file mode 100644
index 000000000000..d7d75605da8b
--- /dev/null
+++ b/drivers/crypto/chelsio/chcr_crypto.h
@@ -0,0 +1,203 @@
+/*
+ * This file is part of the Chelsio T6 Crypto driver for Linux.
+ *
+ * Copyright (c) 2003-2016 Chelsio Communications, Inc. All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * 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 __CHCR_CRYPTO_H__
+#define __CHCR_CRYPTO_H__
+
+/* Define following if h/w is not dropping the AAD and IV data before
+ * giving the processed data
+ */
+
+#define CHCR_CRA_PRIORITY 300
+
+#define CHCR_AES_MAX_KEY_LEN  (2 * (AES_MAX_KEY_SIZE)) /* consider xts */
+#define CHCR_MAX_CRYPTO_IV_LEN 16 /* AES IV len */
+
+#define CHCR_MAX_AUTHENC_AES_KEY_LEN 32 /* max aes key length*/
+#define CHCR_MAX_AUTHENC_SHA_KEY_LEN 128 /* max sha key length*/
+
+#define CHCR_GIVENCRYPT_OP 2
+/* CPL/SCMD parameters */
+
+#define CHCR_ENCRYPT_OP 0
+#define CHCR_DECRYPT_OP 1
+
+#define CHCR_SCMD_SEQ_NO_CTRL_32BIT     1
+#define CHCR_SCMD_SEQ_NO_CTRL_48BIT     2
+#define CHCR_SCMD_SEQ_NO_CTRL_64BIT     3
+
+#define CHCR_SCMD_PROTO_VERSION_GENERIC 4
+
+#define CHCR_SCMD_AUTH_CTRL_AUTH_CIPHER 0
+#define CHCR_SCMD_AUTH_CTRL_CIPHER_AUTH 1
+
+#define CHCR_SCMD_CIPHER_MODE_NOP           0
+#define CHCR_SCMD_CIPHER_MODE_AES_CBC       1
+#define CHCR_SCMD_CIPHER_MODE_GENERIC_AES   4
+#define CHCR_SCMD_CIPHER_MODE_AES_XTS       6
+
+#define CHCR_SCMD_AUTH_MODE_NOP             0
+#define CHCR_SCMD_AUTH_MODE_SHA1            1
+#define CHCR_SCMD_AUTH_MODE_SHA224          2
+#define CHCR_SCMD_AUTH_MODE_SHA256          3
+#define CHCR_SCMD_AUTH_MODE_SHA512_224      5
+#define CHCR_SCMD_AUTH_MODE_SHA512_256      6
+#define CHCR_SCMD_AUTH_MODE_SHA512_384      7
+#define CHCR_SCMD_AUTH_MODE_SHA512_512      8
+
+#define CHCR_SCMD_HMAC_CTRL_NOP             0
+#define CHCR_SCMD_HMAC_CTRL_NO_TRUNC        1
+
+#define CHCR_SCMD_IVGEN_CTRL_HW             0
+#define CHCR_SCMD_IVGEN_CTRL_SW             1
+/* This are not really mac key size. They are intermediate values
+ * of sha engine and its size
+ */
+#define CHCR_KEYCTX_MAC_KEY_SIZE_128        0
+#define CHCR_KEYCTX_MAC_KEY_SIZE_160        1
+#define CHCR_KEYCTX_MAC_KEY_SIZE_192        2
+#define CHCR_KEYCTX_MAC_KEY_SIZE_256        3
+#define CHCR_KEYCTX_MAC_KEY_SIZE_512        4
+#define CHCR_KEYCTX_CIPHER_KEY_SIZE_128     0
+#define CHCR_KEYCTX_CIPHER_KEY_SIZE_192     1
+#define CHCR_KEYCTX_CIPHER_KEY_SIZE_256     2
+#define CHCR_KEYCTX_NO_KEY                  15
+
+#define CHCR_CPL_FW4_PLD_IV_OFFSET          (5 * 64) /* bytes. flt #5 and #6 */
+#define CHCR_CPL_FW4_PLD_HASH_RESULT_OFFSET (7 * 64) /* bytes. flt #7 */
+#define CHCR_CPL_FW4_PLD_DATA_SIZE          (4 * 64) /* bytes. flt #4 to #7 */
+
+#define KEY_CONTEXT_HDR_SALT_AND_PAD	    16
+#define flits_to_bytes(x)  (x * 8)
+
+#define IV_NOP                  0
+#define IV_IMMEDIATE            1
+#define IV_DSGL			2
+
+#define CRYPTO_ALG_SUB_TYPE_MASK            0x0f000000
+#define CRYPTO_ALG_SUB_TYPE_HASH_HMAC       0x01000000
+#define CRYPTO_ALG_TYPE_HMAC (CRYPTO_ALG_TYPE_AHASH |\
+			      CRYPTO_ALG_SUB_TYPE_HASH_HMAC)
+
+#define MAX_SALT                4
+#define MAX_SCRATCH_PAD_SIZE    32
+
+#define CHCR_HASH_MAX_BLOCK_SIZE_64  64
+#define CHCR_HASH_MAX_BLOCK_SIZE_128 128
+
+/* Aligned to 128 bit boundary */
+struct _key_ctx {
+	__be32 ctx_hdr;
+	u8 salt[MAX_SALT];
+	__be64 reserverd;
+	unsigned char key[0];
+};
+
+struct ablk_ctx {
+	u8 enc;
+	unsigned int processed_len;
+	__be32 key_ctx_hdr;
+	unsigned int enckey_len;
+	unsigned int dst_nents;
+	struct scatterlist iv_sg;
+	u8 key[CHCR_AES_MAX_KEY_LEN];
+	u8 iv[CHCR_MAX_CRYPTO_IV_LEN];
+	unsigned char ciph_mode;
+};
+
+struct hmac_ctx {
+	struct shash_desc *desc;
+	u8 ipad[CHCR_HASH_MAX_BLOCK_SIZE_128];
+	u8 opad[CHCR_HASH_MAX_BLOCK_SIZE_128];
+};
+
+struct __crypto_ctx {
+	struct hmac_ctx hmacctx[0];
+	struct ablk_ctx ablkctx[0];
+};
+
+struct chcr_context {
+	struct chcr_dev *dev;
+	unsigned char tx_channel_id;
+	struct __crypto_ctx crypto_ctx[0];
+};
+
+struct chcr_ahash_req_ctx {
+	u32 result;
+	char bfr[CHCR_HASH_MAX_BLOCK_SIZE_128];
+	u8 bfr_len;
+	/* DMA the partial hash in it */
+	u8 partial_hash[CHCR_HASH_MAX_DIGEST_SIZE];
+	u64 data_len;  /* Data len till time */
+	void *dummy_payload_ptr;
+	/* SKB which is being sent to the hardware for processing */
+	struct sk_buff *skb;
+};
+
+struct chcr_blkcipher_req_ctx {
+	struct sk_buff *skb;
+};
+
+struct chcr_alg_template {
+	u32 type;
+	u32 is_registered;
+	union {
+		struct crypto_alg crypto;
+		struct ahash_alg hash;
+	} alg;
+};
+
+struct chcr_req_ctx {
+	union {
+		struct ahash_request *ahash_req;
+		struct ablkcipher_request *ablk_req;
+	} req;
+	union {
+		struct chcr_ahash_req_ctx *ahash_ctx;
+		struct chcr_blkcipher_req_ctx *ablk_ctx;
+	} ctx;
+};
+
+struct sge_opaque_hdr {
+	void *dev;
+	dma_addr_t addr[MAX_SKB_FRAGS + 1];
+};
+
+typedef struct sk_buff *(*create_wr_t)(struct crypto_async_request *req,
+				       struct chcr_context *ctx,
+				       unsigned short qid,
+				       unsigned short op_type);
+
+#endif /* __CHCR_CRYPTO_H__ */
diff --git a/drivers/crypto/hifn_795x.c b/drivers/crypto/hifn_795x.c
index eee2c7e6c299..e09d4055b19e 100644
--- a/drivers/crypto/hifn_795x.c
+++ b/drivers/crypto/hifn_795x.c
@@ -636,20 +636,12 @@ struct hifn_request_context {
 
 static inline u32 hifn_read_0(struct hifn_device *dev, u32 reg)
 {
-	u32 ret;
-
-	ret = readl(dev->bar[0] + reg);
-
-	return ret;
+	return readl(dev->bar[0] + reg);
 }
 
 static inline u32 hifn_read_1(struct hifn_device *dev, u32 reg)
 {
-	u32 ret;
-
-	ret = readl(dev->bar[1] + reg);
-
-	return ret;
+	return readl(dev->bar[1] + reg);
 }
 
 static inline void hifn_write_0(struct hifn_device *dev, u32 reg, u32 val)
diff --git a/drivers/crypto/img-hash.c b/drivers/crypto/img-hash.c
index 68e8aa90fe01..a2e77b87485b 100644
--- a/drivers/crypto/img-hash.c
+++ b/drivers/crypto/img-hash.c
@@ -71,6 +71,7 @@
 #define DRIVER_FLAGS_MD5		BIT(21)
 
 #define IMG_HASH_QUEUE_LENGTH		20
+#define IMG_HASH_DMA_BURST		4
 #define IMG_HASH_DMA_THRESHOLD		64
 
 #ifdef __LITTLE_ENDIAN
@@ -102,8 +103,10 @@ struct img_hash_request_ctx {
 	unsigned long		op;
 
 	size_t			bufcnt;
-	u8 buffer[0] __aligned(sizeof(u32));
 	struct ahash_request	fallback_req;
+
+	/* Zero length buffer must remain last member of struct */
+	u8 buffer[0] __aligned(sizeof(u32));
 };
 
 struct img_hash_ctx {
@@ -340,7 +343,7 @@ static int img_hash_dma_init(struct img_hash_dev *hdev)
 	dma_conf.direction = DMA_MEM_TO_DEV;
 	dma_conf.dst_addr = hdev->bus_addr;
 	dma_conf.dst_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES;
-	dma_conf.dst_maxburst = 16;
+	dma_conf.dst_maxburst = IMG_HASH_DMA_BURST;
 	dma_conf.device_fc = false;
 
 	err = dmaengine_slave_config(hdev->dma_lch,  &dma_conf);
@@ -361,7 +364,7 @@ static void img_hash_dma_task(unsigned long d)
 	size_t nbytes, bleft, wsend, len, tbc;
 	struct scatterlist tsg;
 
-	if (!ctx->sg)
+	if (!hdev->req || !ctx->sg)
 		return;
 
 	addr = sg_virt(ctx->sg);
@@ -587,6 +590,32 @@ static int img_hash_finup(struct ahash_request *req)
 	return crypto_ahash_finup(&rctx->fallback_req);
 }
 
+static int img_hash_import(struct ahash_request *req, const void *in)
+{
+	struct img_hash_request_ctx *rctx = ahash_request_ctx(req);
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct img_hash_ctx *ctx = crypto_ahash_ctx(tfm);
+
+	ahash_request_set_tfm(&rctx->fallback_req, ctx->fallback);
+	rctx->fallback_req.base.flags = req->base.flags
+		& CRYPTO_TFM_REQ_MAY_SLEEP;
+
+	return crypto_ahash_import(&rctx->fallback_req, in);
+}
+
+static int img_hash_export(struct ahash_request *req, void *out)
+{
+	struct img_hash_request_ctx *rctx = ahash_request_ctx(req);
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct img_hash_ctx *ctx = crypto_ahash_ctx(tfm);
+
+	ahash_request_set_tfm(&rctx->fallback_req, ctx->fallback);
+	rctx->fallback_req.base.flags = req->base.flags
+		& CRYPTO_TFM_REQ_MAY_SLEEP;
+
+	return crypto_ahash_export(&rctx->fallback_req, out);
+}
+
 static int img_hash_digest(struct ahash_request *req)
 {
 	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
@@ -643,10 +672,9 @@ static int img_hash_digest(struct ahash_request *req)
 	return err;
 }
 
-static int img_hash_cra_init(struct crypto_tfm *tfm)
+static int img_hash_cra_init(struct crypto_tfm *tfm, const char *alg_name)
 {
 	struct img_hash_ctx *ctx = crypto_tfm_ctx(tfm);
-	const char *alg_name = crypto_tfm_alg_name(tfm);
 	int err = -ENOMEM;
 
 	ctx->fallback = crypto_alloc_ahash(alg_name, 0,
@@ -658,6 +686,7 @@ static int img_hash_cra_init(struct crypto_tfm *tfm)
 	}
 	crypto_ahash_set_reqsize(__crypto_ahash_cast(tfm),
 				 sizeof(struct img_hash_request_ctx) +
+				 crypto_ahash_reqsize(ctx->fallback) +
 				 IMG_HASH_DMA_THRESHOLD);
 
 	return 0;
@@ -666,6 +695,26 @@ err:
 	return err;
 }
 
+static int img_hash_cra_md5_init(struct crypto_tfm *tfm)
+{
+	return img_hash_cra_init(tfm, "md5-generic");
+}
+
+static int img_hash_cra_sha1_init(struct crypto_tfm *tfm)
+{
+	return img_hash_cra_init(tfm, "sha1-generic");
+}
+
+static int img_hash_cra_sha224_init(struct crypto_tfm *tfm)
+{
+	return img_hash_cra_init(tfm, "sha224-generic");
+}
+
+static int img_hash_cra_sha256_init(struct crypto_tfm *tfm)
+{
+	return img_hash_cra_init(tfm, "sha256-generic");
+}
+
 static void img_hash_cra_exit(struct crypto_tfm *tfm)
 {
 	struct img_hash_ctx *tctx = crypto_tfm_ctx(tfm);
@@ -711,9 +760,12 @@ static struct ahash_alg img_algs[] = {
 		.update = img_hash_update,
 		.final = img_hash_final,
 		.finup = img_hash_finup,
+		.export = img_hash_export,
+		.import = img_hash_import,
 		.digest = img_hash_digest,
 		.halg = {
 			.digestsize = MD5_DIGEST_SIZE,
+			.statesize = sizeof(struct md5_state),
 			.base = {
 				.cra_name = "md5",
 				.cra_driver_name = "img-md5",
@@ -723,7 +775,7 @@ static struct ahash_alg img_algs[] = {
 				CRYPTO_ALG_NEED_FALLBACK,
 				.cra_blocksize = MD5_HMAC_BLOCK_SIZE,
 				.cra_ctxsize = sizeof(struct img_hash_ctx),
-				.cra_init = img_hash_cra_init,
+				.cra_init = img_hash_cra_md5_init,
 				.cra_exit = img_hash_cra_exit,
 				.cra_module = THIS_MODULE,
 			}
@@ -734,9 +786,12 @@ static struct ahash_alg img_algs[] = {
 		.update = img_hash_update,
 		.final = img_hash_final,
 		.finup = img_hash_finup,
+		.export = img_hash_export,
+		.import = img_hash_import,
 		.digest = img_hash_digest,
 		.halg = {
 			.digestsize = SHA1_DIGEST_SIZE,
+			.statesize = sizeof(struct sha1_state),
 			.base = {
 				.cra_name = "sha1",
 				.cra_driver_name = "img-sha1",
@@ -746,7 +801,7 @@ static struct ahash_alg img_algs[] = {
 				CRYPTO_ALG_NEED_FALLBACK,
 				.cra_blocksize = SHA1_BLOCK_SIZE,
 				.cra_ctxsize = sizeof(struct img_hash_ctx),
-				.cra_init = img_hash_cra_init,
+				.cra_init = img_hash_cra_sha1_init,
 				.cra_exit = img_hash_cra_exit,
 				.cra_module = THIS_MODULE,
 			}
@@ -757,9 +812,12 @@ static struct ahash_alg img_algs[] = {
 		.update = img_hash_update,
 		.final = img_hash_final,
 		.finup = img_hash_finup,
+		.export = img_hash_export,
+		.import = img_hash_import,
 		.digest = img_hash_digest,
 		.halg = {
 			.digestsize = SHA224_DIGEST_SIZE,
+			.statesize = sizeof(struct sha256_state),
 			.base = {
 				.cra_name = "sha224",
 				.cra_driver_name = "img-sha224",
@@ -769,7 +827,7 @@ static struct ahash_alg img_algs[] = {
 				CRYPTO_ALG_NEED_FALLBACK,
 				.cra_blocksize = SHA224_BLOCK_SIZE,
 				.cra_ctxsize = sizeof(struct img_hash_ctx),
-				.cra_init = img_hash_cra_init,
+				.cra_init = img_hash_cra_sha224_init,
 				.cra_exit = img_hash_cra_exit,
 				.cra_module = THIS_MODULE,
 			}
@@ -780,9 +838,12 @@ static struct ahash_alg img_algs[] = {
 		.update = img_hash_update,
 		.final = img_hash_final,
 		.finup = img_hash_finup,
+		.export = img_hash_export,
+		.import = img_hash_import,
 		.digest = img_hash_digest,
 		.halg = {
 			.digestsize = SHA256_DIGEST_SIZE,
+			.statesize = sizeof(struct sha256_state),
 			.base = {
 				.cra_name = "sha256",
 				.cra_driver_name = "img-sha256",
@@ -792,7 +853,7 @@ static struct ahash_alg img_algs[] = {
 				CRYPTO_ALG_NEED_FALLBACK,
 				.cra_blocksize = SHA256_BLOCK_SIZE,
 				.cra_ctxsize = sizeof(struct img_hash_ctx),
-				.cra_init = img_hash_cra_init,
+				.cra_init = img_hash_cra_sha256_init,
 				.cra_exit = img_hash_cra_exit,
 				.cra_module = THIS_MODULE,
 			}
@@ -971,7 +1032,7 @@ static int img_hash_probe(struct platform_device *pdev)
 	err = img_register_algs(hdev);
 	if (err)
 		goto err_algs;
-	dev_dbg(dev, "Img MD5/SHA1/SHA224/SHA256 Hardware accelerator initialized\n");
+	dev_info(dev, "Img MD5/SHA1/SHA224/SHA256 Hardware accelerator initialized\n");
 
 	return 0;
 
@@ -1013,11 +1074,38 @@ static int img_hash_remove(struct platform_device *pdev)
 	return 0;
 }
 
+#ifdef CONFIG_PM_SLEEP
+static int img_hash_suspend(struct device *dev)
+{
+	struct img_hash_dev *hdev = dev_get_drvdata(dev);
+
+	clk_disable_unprepare(hdev->hash_clk);
+	clk_disable_unprepare(hdev->sys_clk);
+
+	return 0;
+}
+
+static int img_hash_resume(struct device *dev)
+{
+	struct img_hash_dev *hdev = dev_get_drvdata(dev);
+
+	clk_prepare_enable(hdev->hash_clk);
+	clk_prepare_enable(hdev->sys_clk);
+
+	return 0;
+}
+#endif /* CONFIG_PM_SLEEP */
+
+static const struct dev_pm_ops img_hash_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(img_hash_suspend, img_hash_resume)
+};
+
 static struct platform_driver img_hash_driver = {
 	.probe		= img_hash_probe,
 	.remove		= img_hash_remove,
 	.driver		= {
 		.name	= "img-hash-accelerator",
+		.pm	= &img_hash_pm_ops,
 		.of_match_table	= of_match_ptr(img_hash_match),
 	}
 };
diff --git a/drivers/crypto/ixp4xx_crypto.c b/drivers/crypto/ixp4xx_crypto.c
index 2296934455fc..7868765a70c5 100644
--- a/drivers/crypto/ixp4xx_crypto.c
+++ b/drivers/crypto/ixp4xx_crypto.c
@@ -447,9 +447,8 @@ static int init_ixp_crypto(struct device *dev)
 
 	if (!npe_running(npe_c)) {
 		ret = npe_load_firmware(npe_c, npe_name(npe_c), dev);
-		if (ret) {
-			return ret;
-		}
+		if (ret)
+			goto npe_release;
 		if (npe_recv_message(npe_c, msg, "STATUS_MSG"))
 			goto npe_error;
 	} else {
@@ -473,7 +472,8 @@ static int init_ixp_crypto(struct device *dev)
 	default:
 		printk(KERN_ERR "Firmware of %s lacks crypto support\n",
 			npe_name(npe_c));
-		return -ENODEV;
+		ret = -ENODEV;
+		goto npe_release;
 	}
 	/* buffer_pool will also be used to sometimes store the hmac,
 	 * so assure it is large enough
@@ -512,6 +512,7 @@ npe_error:
 err:
 	dma_pool_destroy(ctx_pool);
 	dma_pool_destroy(buffer_pool);
+npe_release:
 	npe_release(npe_c);
 	return ret;
 }
diff --git a/drivers/crypto/marvell/cesa.c b/drivers/crypto/marvell/cesa.c
index d64af8625d7e..37dadb2a4feb 100644
--- a/drivers/crypto/marvell/cesa.c
+++ b/drivers/crypto/marvell/cesa.c
@@ -166,6 +166,7 @@ static irqreturn_t mv_cesa_int(int irq, void *priv)
 			if (!req)
 				break;
 
+			ctx = crypto_tfm_ctx(req->tfm);
 			mv_cesa_complete_req(ctx, req, 0);
 		}
 	}
diff --git a/drivers/crypto/marvell/hash.c b/drivers/crypto/marvell/hash.c
index 82e0f4e6eb1c..9f284682c091 100644
--- a/drivers/crypto/marvell/hash.c
+++ b/drivers/crypto/marvell/hash.c
@@ -374,7 +374,7 @@ static const struct mv_cesa_req_ops mv_cesa_ahash_req_ops = {
 	.complete = mv_cesa_ahash_complete,
 };
 
-static int mv_cesa_ahash_init(struct ahash_request *req,
+static void mv_cesa_ahash_init(struct ahash_request *req,
 			      struct mv_cesa_op_ctx *tmpl, bool algo_le)
 {
 	struct mv_cesa_ahash_req *creq = ahash_request_ctx(req);
@@ -390,8 +390,6 @@ static int mv_cesa_ahash_init(struct ahash_request *req,
 	creq->op_tmpl = *tmpl;
 	creq->len = 0;
 	creq->algo_le = algo_le;
-
-	return 0;
 }
 
 static inline int mv_cesa_ahash_cra_init(struct crypto_tfm *tfm)
@@ -405,15 +403,16 @@ static inline int mv_cesa_ahash_cra_init(struct crypto_tfm *tfm)
 	return 0;
 }
 
-static int mv_cesa_ahash_cache_req(struct ahash_request *req, bool *cached)
+static bool mv_cesa_ahash_cache_req(struct ahash_request *req)
 {
 	struct mv_cesa_ahash_req *creq = ahash_request_ctx(req);
+	bool cached = false;
 
-	if (creq->cache_ptr + req->nbytes < 64 && !creq->last_req) {
-		*cached = true;
+	if (creq->cache_ptr + req->nbytes < CESA_MAX_HASH_BLOCK_SIZE && !creq->last_req) {
+		cached = true;
 
 		if (!req->nbytes)
-			return 0;
+			return cached;
 
 		sg_pcopy_to_buffer(req->src, creq->src_nents,
 				   creq->cache + creq->cache_ptr,
@@ -422,7 +421,7 @@ static int mv_cesa_ahash_cache_req(struct ahash_request *req, bool *cached)
 		creq->cache_ptr += req->nbytes;
 	}
 
-	return 0;
+	return cached;
 }
 
 static struct mv_cesa_op_ctx *
@@ -455,7 +454,6 @@ mv_cesa_dma_add_frag(struct mv_cesa_tdma_chain *chain,
 
 static int
 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)
 {
@@ -586,7 +584,7 @@ static int mv_cesa_ahash_dma_req_init(struct ahash_request *req)
 	 * Add the cache (left-over data from a previous block) first.
 	 * This will never overflow the SRAM size.
 	 */
-	ret = mv_cesa_ahash_dma_add_cache(&basereq->chain, &iter, creq, flags);
+	ret = mv_cesa_ahash_dma_add_cache(&basereq->chain, creq, flags);
 	if (ret)
 		goto err_free_tdma;
 
@@ -668,7 +666,6 @@ err:
 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;
 
 	creq->src_nents = sg_nents_for_len(req->src, req->nbytes);
 	if (creq->src_nents < 0) {
@@ -676,17 +673,15 @@ static int mv_cesa_ahash_req_init(struct ahash_request *req, bool *cached)
 		return creq->src_nents;
 	}
 
-	ret = mv_cesa_ahash_cache_req(req, cached);
-	if (ret)
-		return ret;
+	*cached = mv_cesa_ahash_cache_req(req);
 
 	if (*cached)
 		return 0;
 
 	if (cesa_dev->caps->has_tdma)
-		ret = mv_cesa_ahash_dma_req_init(req);
-
-	return ret;
+		return mv_cesa_ahash_dma_req_init(req);
+	else
+		return 0;
 }
 
 static int mv_cesa_ahash_queue_req(struct ahash_request *req)
@@ -805,13 +800,14 @@ 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, true);
+
 	creq->state[0] = MD5_H0;
 	creq->state[1] = MD5_H1;
 	creq->state[2] = MD5_H2;
 	creq->state[3] = MD5_H3;
 
-	mv_cesa_ahash_init(req, &tmpl, true);
-
 	return 0;
 }
 
@@ -873,14 +869,15 @@ 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, false);
+
 	creq->state[0] = SHA1_H0;
 	creq->state[1] = SHA1_H1;
 	creq->state[2] = SHA1_H2;
 	creq->state[3] = SHA1_H3;
 	creq->state[4] = SHA1_H4;
 
-	mv_cesa_ahash_init(req, &tmpl, false);
-
 	return 0;
 }
 
@@ -942,6 +939,9 @@ 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, false);
+
 	creq->state[0] = SHA256_H0;
 	creq->state[1] = SHA256_H1;
 	creq->state[2] = SHA256_H2;
@@ -951,8 +951,6 @@ static int mv_cesa_sha256_init(struct ahash_request *req)
 	creq->state[6] = SHA256_H6;
 	creq->state[7] = SHA256_H7;
 
-	mv_cesa_ahash_init(req, &tmpl, false);
-
 	return 0;
 }
 
diff --git a/drivers/crypto/marvell/tdma.c b/drivers/crypto/marvell/tdma.c
index 86a065bcc187..9fd7a5fbaa1b 100644
--- a/drivers/crypto/marvell/tdma.c
+++ b/drivers/crypto/marvell/tdma.c
@@ -261,6 +261,7 @@ struct mv_cesa_op_ctx *mv_cesa_dma_add_op(struct mv_cesa_tdma_chain *chain,
 	tdma->op = op;
 	tdma->byte_cnt = cpu_to_le32(size | BIT(31));
 	tdma->src = cpu_to_le32(dma_handle);
+	tdma->dst = CESA_SA_CFG_SRAM_OFFSET;
 	tdma->flags = CESA_TDMA_DST_IN_SRAM | CESA_TDMA_OP;
 
 	return op;
diff --git a/drivers/crypto/mv_cesa.c b/drivers/crypto/mv_cesa.c
index e6b658faef63..104e9ce9400a 100644
--- a/drivers/crypto/mv_cesa.c
+++ b/drivers/crypto/mv_cesa.c
@@ -1091,11 +1091,8 @@ static int mv_probe(struct platform_device *pdev)
 
 	cp->max_req_size = cp->sram_size - SRAM_CFG_SPACE;
 
-	if (pdev->dev.of_node)
-		irq = irq_of_parse_and_map(pdev->dev.of_node, 0);
-	else
-		irq = platform_get_irq(pdev, 0);
-	if (irq < 0 || irq == NO_IRQ) {
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0) {
 		ret = irq;
 		goto err;
 	}
diff --git a/drivers/crypto/mxc-scc.c b/drivers/crypto/mxc-scc.c
index ff383ef83871..ee4be1b0d30b 100644
--- a/drivers/crypto/mxc-scc.c
+++ b/drivers/crypto/mxc-scc.c
@@ -668,7 +668,9 @@ static int mxc_scc_probe(struct platform_device *pdev)
 		return PTR_ERR(scc->clk);
 	}
 
-	clk_prepare_enable(scc->clk);
+	ret = clk_prepare_enable(scc->clk);
+	if (ret)
+		return ret;
 
 	/* clear error status register */
 	writel(0x0, scc->base + SCC_SCM_ERROR_STATUS);
diff --git a/drivers/crypto/omap-aes.c b/drivers/crypto/omap-aes.c
index 4ab53a604312..fe32dd95ae4f 100644
--- a/drivers/crypto/omap-aes.c
+++ b/drivers/crypto/omap-aes.c
@@ -35,7 +35,8 @@
 #include <linux/interrupt.h>
 #include <crypto/scatterwalk.h>
 #include <crypto/aes.h>
-#include <crypto/algapi.h>
+#include <crypto/engine.h>
+#include <crypto/internal/skcipher.h>
 
 #define DST_MAXBURST			4
 #define DMA_MIN				(DST_MAXBURST * sizeof(u32))
@@ -85,6 +86,8 @@
 #define AES_REG_IRQ_DATA_OUT           BIT(2)
 #define DEFAULT_TIMEOUT		(5*HZ)
 
+#define DEFAULT_AUTOSUSPEND_DELAY	1000
+
 #define FLAGS_MODE_MASK		0x000f
 #define FLAGS_ENCRYPT		BIT(0)
 #define FLAGS_CBC		BIT(1)
@@ -103,6 +106,7 @@ struct omap_aes_ctx {
 	int		keylen;
 	u32		key[AES_KEYSIZE_256 / sizeof(u32)];
 	unsigned long	flags;
+	struct crypto_skcipher	*fallback;
 };
 
 struct omap_aes_reqctx {
@@ -238,11 +242,19 @@ static void omap_aes_write_n(struct omap_aes_dev *dd, u32 offset,
 
 static int omap_aes_hw_init(struct omap_aes_dev *dd)
 {
+	int err;
+
 	if (!(dd->flags & FLAGS_INIT)) {
 		dd->flags |= FLAGS_INIT;
 		dd->err = 0;
 	}
 
+	err = pm_runtime_get_sync(dd->dev);
+	if (err < 0) {
+		dev_err(dd->dev, "failed to get sync: %d\n", err);
+		return err;
+	}
+
 	return 0;
 }
 
@@ -319,20 +331,12 @@ static void omap_aes_dma_stop(struct omap_aes_dev *dd)
 
 static struct omap_aes_dev *omap_aes_find_dev(struct omap_aes_ctx *ctx)
 {
-	struct omap_aes_dev *dd = NULL, *tmp;
+	struct omap_aes_dev *dd;
 
 	spin_lock_bh(&list_lock);
-	if (!ctx->dd) {
-		list_for_each_entry(tmp, &dev_list, list) {
-			/* FIXME: take fist available aes core */
-			dd = tmp;
-			break;
-		}
-		ctx->dd = dd;
-	} else {
-		/* already found before */
-		dd = ctx->dd;
-	}
+	dd = list_first_entry(&dev_list, struct omap_aes_dev, list);
+	list_move_tail(&dd->list, &dev_list);
+	ctx->dd = dd;
 	spin_unlock_bh(&list_lock);
 
 	return dd;
@@ -519,7 +523,10 @@ static void omap_aes_finish_req(struct omap_aes_dev *dd, int err)
 
 	pr_debug("err: %d\n", err);
 
-	crypto_finalize_request(dd->engine, req, err);
+	crypto_finalize_cipher_request(dd->engine, req, err);
+
+	pm_runtime_mark_last_busy(dd->dev);
+	pm_runtime_put_autosuspend(dd->dev);
 }
 
 static int omap_aes_crypt_dma_stop(struct omap_aes_dev *dd)
@@ -592,7 +599,7 @@ static int omap_aes_handle_queue(struct omap_aes_dev *dd,
 				 struct ablkcipher_request *req)
 {
 	if (req)
-		return crypto_transfer_request_to_engine(dd->engine, req);
+		return crypto_transfer_cipher_request_to_engine(dd->engine, req);
 
 	return 0;
 }
@@ -602,7 +609,7 @@ static int omap_aes_prepare_req(struct crypto_engine *engine,
 {
 	struct omap_aes_ctx *ctx = crypto_ablkcipher_ctx(
 			crypto_ablkcipher_reqtfm(req));
-	struct omap_aes_dev *dd = omap_aes_find_dev(ctx);
+	struct omap_aes_dev *dd = ctx->dd;
 	struct omap_aes_reqctx *rctx;
 
 	if (!dd)
@@ -648,7 +655,7 @@ static int omap_aes_crypt_req(struct crypto_engine *engine,
 {
 	struct omap_aes_ctx *ctx = crypto_ablkcipher_ctx(
 			crypto_ablkcipher_reqtfm(req));
-	struct omap_aes_dev *dd = omap_aes_find_dev(ctx);
+	struct omap_aes_dev *dd = ctx->dd;
 
 	if (!dd)
 		return -ENODEV;
@@ -696,11 +703,29 @@ static int omap_aes_crypt(struct ablkcipher_request *req, unsigned long mode)
 			crypto_ablkcipher_reqtfm(req));
 	struct omap_aes_reqctx *rctx = ablkcipher_request_ctx(req);
 	struct omap_aes_dev *dd;
+	int ret;
 
 	pr_debug("nbytes: %d, enc: %d, cbc: %d\n", req->nbytes,
 		  !!(mode & FLAGS_ENCRYPT),
 		  !!(mode & FLAGS_CBC));
 
+	if (req->nbytes < 200) {
+		SKCIPHER_REQUEST_ON_STACK(subreq, ctx->fallback);
+
+		skcipher_request_set_tfm(subreq, ctx->fallback);
+		skcipher_request_set_callback(subreq, req->base.flags, NULL,
+					      NULL);
+		skcipher_request_set_crypt(subreq, req->src, req->dst,
+					   req->nbytes, req->info);
+
+		if (mode & FLAGS_ENCRYPT)
+			ret = crypto_skcipher_encrypt(subreq);
+		else
+			ret = crypto_skcipher_decrypt(subreq);
+
+		skcipher_request_zero(subreq);
+		return ret;
+	}
 	dd = omap_aes_find_dev(ctx);
 	if (!dd)
 		return -ENODEV;
@@ -716,6 +741,7 @@ static int omap_aes_setkey(struct crypto_ablkcipher *tfm, const u8 *key,
 			   unsigned int keylen)
 {
 	struct omap_aes_ctx *ctx = crypto_ablkcipher_ctx(tfm);
+	int ret;
 
 	if (keylen != AES_KEYSIZE_128 && keylen != AES_KEYSIZE_192 &&
 		   keylen != AES_KEYSIZE_256)
@@ -726,6 +752,14 @@ static int omap_aes_setkey(struct crypto_ablkcipher *tfm, const u8 *key,
 	memcpy(ctx->key, key, keylen);
 	ctx->keylen = keylen;
 
+	crypto_skcipher_clear_flags(ctx->fallback, CRYPTO_TFM_REQ_MASK);
+	crypto_skcipher_set_flags(ctx->fallback, tfm->base.crt_flags &
+						 CRYPTO_TFM_REQ_MASK);
+
+	ret = crypto_skcipher_setkey(ctx->fallback, key, keylen);
+	if (!ret)
+		return 0;
+
 	return 0;
 }
 
@@ -761,22 +795,16 @@ static int omap_aes_ctr_decrypt(struct ablkcipher_request *req)
 
 static int omap_aes_cra_init(struct crypto_tfm *tfm)
 {
-	struct omap_aes_dev *dd = NULL;
-	int err;
+	const char *name = crypto_tfm_alg_name(tfm);
+	const u32 flags = CRYPTO_ALG_ASYNC | CRYPTO_ALG_NEED_FALLBACK;
+	struct omap_aes_ctx *ctx = crypto_tfm_ctx(tfm);
+	struct crypto_skcipher *blk;
 
-	/* Find AES device, currently picks the first device */
-	spin_lock_bh(&list_lock);
-	list_for_each_entry(dd, &dev_list, list) {
-		break;
-	}
-	spin_unlock_bh(&list_lock);
+	blk = crypto_alloc_skcipher(name, 0, flags);
+	if (IS_ERR(blk))
+		return PTR_ERR(blk);
 
-	err = pm_runtime_get_sync(dd->dev);
-	if (err < 0) {
-		dev_err(dd->dev, "%s: failed to get_sync(%d)\n",
-			__func__, err);
-		return err;
-	}
+	ctx->fallback = blk;
 
 	tfm->crt_ablkcipher.reqsize = sizeof(struct omap_aes_reqctx);
 
@@ -785,16 +813,12 @@ static int omap_aes_cra_init(struct crypto_tfm *tfm)
 
 static void omap_aes_cra_exit(struct crypto_tfm *tfm)
 {
-	struct omap_aes_dev *dd = NULL;
+	struct omap_aes_ctx *ctx = crypto_tfm_ctx(tfm);
 
-	/* Find AES device, currently picks the first device */
-	spin_lock_bh(&list_lock);
-	list_for_each_entry(dd, &dev_list, list) {
-		break;
-	}
-	spin_unlock_bh(&list_lock);
+	if (ctx->fallback)
+		crypto_free_skcipher(ctx->fallback);
 
-	pm_runtime_put_sync(dd->dev);
+	ctx->fallback = NULL;
 }
 
 /* ********************** ALGS ************************************ */
@@ -806,7 +830,7 @@ static struct crypto_alg algs_ecb_cbc[] = {
 	.cra_priority		= 300,
 	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER |
 				  CRYPTO_ALG_KERN_DRIVER_ONLY |
-				  CRYPTO_ALG_ASYNC,
+				  CRYPTO_ALG_ASYNC | CRYPTO_ALG_NEED_FALLBACK,
 	.cra_blocksize		= AES_BLOCK_SIZE,
 	.cra_ctxsize		= sizeof(struct omap_aes_ctx),
 	.cra_alignmask		= 0,
@@ -828,7 +852,7 @@ static struct crypto_alg algs_ecb_cbc[] = {
 	.cra_priority		= 300,
 	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER |
 				  CRYPTO_ALG_KERN_DRIVER_ONLY |
-				  CRYPTO_ALG_ASYNC,
+				  CRYPTO_ALG_ASYNC | CRYPTO_ALG_NEED_FALLBACK,
 	.cra_blocksize		= AES_BLOCK_SIZE,
 	.cra_ctxsize		= sizeof(struct omap_aes_ctx),
 	.cra_alignmask		= 0,
@@ -854,7 +878,7 @@ static struct crypto_alg algs_ctr[] = {
 	.cra_priority		= 300,
 	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER |
 				  CRYPTO_ALG_KERN_DRIVER_ONLY |
-				  CRYPTO_ALG_ASYNC,
+				  CRYPTO_ALG_ASYNC | CRYPTO_ALG_NEED_FALLBACK,
 	.cra_blocksize		= AES_BLOCK_SIZE,
 	.cra_ctxsize		= sizeof(struct omap_aes_ctx),
 	.cra_alignmask		= 0,
@@ -1140,6 +1164,9 @@ static int omap_aes_probe(struct platform_device *pdev)
 	}
 	dd->phys_base = res.start;
 
+	pm_runtime_use_autosuspend(dev);
+	pm_runtime_set_autosuspend_delay(dev, DEFAULT_AUTOSUSPEND_DELAY);
+
 	pm_runtime_enable(dev);
 	err = pm_runtime_get_sync(dev);
 	if (err < 0) {
@@ -1186,6 +1213,19 @@ static int omap_aes_probe(struct platform_device *pdev)
 	list_add_tail(&dd->list, &dev_list);
 	spin_unlock(&list_lock);
 
+	/* Initialize crypto engine */
+	dd->engine = crypto_engine_alloc_init(dev, 1);
+	if (!dd->engine) {
+		err = -ENOMEM;
+		goto err_engine;
+	}
+
+	dd->engine->prepare_cipher_request = omap_aes_prepare_req;
+	dd->engine->cipher_one_request = omap_aes_crypt_req;
+	err = crypto_engine_start(dd->engine);
+	if (err)
+		goto err_engine;
+
 	for (i = 0; i < dd->pdata->algs_info_size; i++) {
 		if (!dd->pdata->algs_info[i].registered) {
 			for (j = 0; j < dd->pdata->algs_info[i].size; j++) {
@@ -1203,26 +1243,17 @@ static int omap_aes_probe(struct platform_device *pdev)
 		}
 	}
 
-	/* Initialize crypto engine */
-	dd->engine = crypto_engine_alloc_init(dev, 1);
-	if (!dd->engine)
-		goto err_algs;
-
-	dd->engine->prepare_request = omap_aes_prepare_req;
-	dd->engine->crypt_one_request = omap_aes_crypt_req;
-	err = crypto_engine_start(dd->engine);
-	if (err)
-		goto err_engine;
-
 	return 0;
-err_engine:
-	crypto_engine_exit(dd->engine);
 err_algs:
 	for (i = dd->pdata->algs_info_size - 1; i >= 0; i--)
 		for (j = dd->pdata->algs_info[i].registered - 1; j >= 0; j--)
 			crypto_unregister_alg(
 					&dd->pdata->algs_info[i].algs_list[j]);
 
+err_engine:
+	if (dd->engine)
+		crypto_engine_exit(dd->engine);
+
 	omap_aes_dma_cleanup(dd);
 err_irq:
 	tasklet_kill(&dd->done_task);
diff --git a/drivers/crypto/omap-des.c b/drivers/crypto/omap-des.c
index 5691434ffb2d..a6f65532fd16 100644
--- a/drivers/crypto/omap-des.c
+++ b/drivers/crypto/omap-des.c
@@ -39,6 +39,7 @@
 #include <crypto/scatterwalk.h>
 #include <crypto/des.h>
 #include <crypto/algapi.h>
+#include <crypto/engine.h>
 
 #define DST_MAXBURST			2
 
@@ -506,7 +507,7 @@ static void omap_des_finish_req(struct omap_des_dev *dd, int err)
 	pr_debug("err: %d\n", err);
 
 	pm_runtime_put(dd->dev);
-	crypto_finalize_request(dd->engine, req, err);
+	crypto_finalize_cipher_request(dd->engine, req, err);
 }
 
 static int omap_des_crypt_dma_stop(struct omap_des_dev *dd)
@@ -574,7 +575,7 @@ static int omap_des_handle_queue(struct omap_des_dev *dd,
 				 struct ablkcipher_request *req)
 {
 	if (req)
-		return crypto_transfer_request_to_engine(dd->engine, req);
+		return crypto_transfer_cipher_request_to_engine(dd->engine, req);
 
 	return 0;
 }
@@ -1078,6 +1079,19 @@ static int omap_des_probe(struct platform_device *pdev)
 	list_add_tail(&dd->list, &dev_list);
 	spin_unlock(&list_lock);
 
+	/* Initialize des crypto engine */
+	dd->engine = crypto_engine_alloc_init(dev, 1);
+	if (!dd->engine) {
+		err = -ENOMEM;
+		goto err_engine;
+	}
+
+	dd->engine->prepare_cipher_request = omap_des_prepare_req;
+	dd->engine->cipher_one_request = omap_des_crypt_req;
+	err = crypto_engine_start(dd->engine);
+	if (err)
+		goto err_engine;
+
 	for (i = 0; i < dd->pdata->algs_info_size; i++) {
 		for (j = 0; j < dd->pdata->algs_info[i].size; j++) {
 			algp = &dd->pdata->algs_info[i].algs_list[j];
@@ -1093,27 +1107,18 @@ static int omap_des_probe(struct platform_device *pdev)
 		}
 	}
 
-	/* Initialize des crypto engine */
-	dd->engine = crypto_engine_alloc_init(dev, 1);
-	if (!dd->engine)
-		goto err_algs;
-
-	dd->engine->prepare_request = omap_des_prepare_req;
-	dd->engine->crypt_one_request = omap_des_crypt_req;
-	err = crypto_engine_start(dd->engine);
-	if (err)
-		goto err_engine;
-
 	return 0;
 
-err_engine:
-	crypto_engine_exit(dd->engine);
 err_algs:
 	for (i = dd->pdata->algs_info_size - 1; i >= 0; i--)
 		for (j = dd->pdata->algs_info[i].registered - 1; j >= 0; j--)
 			crypto_unregister_alg(
 					&dd->pdata->algs_info[i].algs_list[j]);
 
+err_engine:
+	if (dd->engine)
+		crypto_engine_exit(dd->engine);
+
 	omap_des_dma_cleanup(dd);
 err_irq:
 	tasklet_kill(&dd->done_task);
diff --git a/drivers/crypto/omap-sham.c b/drivers/crypto/omap-sham.c
index 7fe4eef12fe2..d0b16e5e4ee5 100644
--- a/drivers/crypto/omap-sham.c
+++ b/drivers/crypto/omap-sham.c
@@ -112,9 +112,10 @@
 #define FLAGS_DMA_READY		6
 #define FLAGS_AUTO_XOR		7
 #define FLAGS_BE32_SHA1		8
+#define FLAGS_SGS_COPIED	9
+#define FLAGS_SGS_ALLOCED	10
 /* context flags */
 #define FLAGS_FINUP		16
-#define FLAGS_SG		17
 
 #define FLAGS_MODE_SHIFT	18
 #define FLAGS_MODE_MASK		(SHA_REG_MODE_ALGO_MASK	<< FLAGS_MODE_SHIFT)
@@ -134,7 +135,8 @@
 #define OMAP_ALIGN_MASK		(sizeof(u32)-1)
 #define OMAP_ALIGNED		__attribute__((aligned(sizeof(u32))))
 
-#define BUFLEN			PAGE_SIZE
+#define BUFLEN			SHA512_BLOCK_SIZE
+#define OMAP_SHA_DMA_THRESHOLD	256
 
 struct omap_sham_dev;
 
@@ -147,12 +149,12 @@ struct omap_sham_reqctx {
 	size_t			digcnt;
 	size_t			bufcnt;
 	size_t			buflen;
-	dma_addr_t		dma_addr;
 
 	/* walk state */
 	struct scatterlist	*sg;
-	struct scatterlist	sgl;
-	unsigned int		offset;	/* offset in current sg */
+	struct scatterlist	sgl[2];
+	int			offset;	/* offset in current sg */
+	int			sg_len;
 	unsigned int		total;	/* total request */
 
 	u8			buffer[0] OMAP_ALIGNED;
@@ -223,6 +225,7 @@ struct omap_sham_dev {
 	struct dma_chan		*dma_lch;
 	struct tasklet_struct	done_task;
 	u8			polling_mode;
+	u8			xmit_buf[BUFLEN];
 
 	unsigned long		flags;
 	struct crypto_queue	queue;
@@ -510,12 +513,14 @@ static int omap_sham_poll_irq_omap4(struct omap_sham_dev *dd)
 			      SHA_REG_IRQSTATUS_INPUT_RDY);
 }
 
-static int omap_sham_xmit_cpu(struct omap_sham_dev *dd, const u8 *buf,
-			      size_t length, int final)
+static int omap_sham_xmit_cpu(struct omap_sham_dev *dd, size_t length,
+			      int final)
 {
 	struct omap_sham_reqctx *ctx = ahash_request_ctx(dd->req);
 	int count, len32, bs32, offset = 0;
-	const u32 *buffer = (const u32 *)buf;
+	const u32 *buffer;
+	int mlen;
+	struct sg_mapping_iter mi;
 
 	dev_dbg(dd->dev, "xmit_cpu: digcnt: %d, length: %d, final: %d\n",
 						ctx->digcnt, length, final);
@@ -525,6 +530,7 @@ static int omap_sham_xmit_cpu(struct omap_sham_dev *dd, const u8 *buf,
 
 	/* should be non-zero before next lines to disable clocks later */
 	ctx->digcnt += length;
+	ctx->total -= length;
 
 	if (final)
 		set_bit(FLAGS_FINAL, &dd->flags); /* catch last interrupt */
@@ -534,16 +540,35 @@ static int omap_sham_xmit_cpu(struct omap_sham_dev *dd, const u8 *buf,
 	len32 = DIV_ROUND_UP(length, sizeof(u32));
 	bs32 = get_block_size(ctx) / sizeof(u32);
 
+	sg_miter_start(&mi, ctx->sg, ctx->sg_len,
+		       SG_MITER_FROM_SG | SG_MITER_ATOMIC);
+
+	mlen = 0;
+
 	while (len32) {
 		if (dd->pdata->poll_irq(dd))
 			return -ETIMEDOUT;
 
-		for (count = 0; count < min(len32, bs32); count++, offset++)
+		for (count = 0; count < min(len32, bs32); count++, offset++) {
+			if (!mlen) {
+				sg_miter_next(&mi);
+				mlen = mi.length;
+				if (!mlen) {
+					pr_err("sg miter failure.\n");
+					return -EINVAL;
+				}
+				offset = 0;
+				buffer = mi.addr;
+			}
 			omap_sham_write(dd, SHA_REG_DIN(dd, count),
 					buffer[offset]);
+			mlen -= 4;
+		}
 		len32 -= min(len32, bs32);
 	}
 
+	sg_miter_stop(&mi);
+
 	return -EINPROGRESS;
 }
 
@@ -555,22 +580,27 @@ static void omap_sham_dma_callback(void *param)
 	tasklet_schedule(&dd->done_task);
 }
 
-static int omap_sham_xmit_dma(struct omap_sham_dev *dd, dma_addr_t dma_addr,
-			      size_t length, int final, int is_sg)
+static int omap_sham_xmit_dma(struct omap_sham_dev *dd, size_t length,
+			      int final)
 {
 	struct omap_sham_reqctx *ctx = ahash_request_ctx(dd->req);
 	struct dma_async_tx_descriptor *tx;
 	struct dma_slave_config cfg;
-	int len32, ret, dma_min = get_block_size(ctx);
+	int ret;
 
 	dev_dbg(dd->dev, "xmit_dma: digcnt: %d, length: %d, final: %d\n",
 						ctx->digcnt, length, final);
 
+	if (!dma_map_sg(dd->dev, ctx->sg, ctx->sg_len, DMA_TO_DEVICE)) {
+		dev_err(dd->dev, "dma_map_sg error\n");
+		return -EINVAL;
+	}
+
 	memset(&cfg, 0, sizeof(cfg));
 
 	cfg.dst_addr = dd->phys_base + SHA_REG_DIN(dd, 0);
 	cfg.dst_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES;
-	cfg.dst_maxburst = dma_min / DMA_SLAVE_BUSWIDTH_4_BYTES;
+	cfg.dst_maxburst = get_block_size(ctx) / DMA_SLAVE_BUSWIDTH_4_BYTES;
 
 	ret = dmaengine_slave_config(dd->dma_lch, &cfg);
 	if (ret) {
@@ -578,30 +608,12 @@ static int omap_sham_xmit_dma(struct omap_sham_dev *dd, dma_addr_t dma_addr,
 		return ret;
 	}
 
-	len32 = DIV_ROUND_UP(length, dma_min) * dma_min;
-
-	if (is_sg) {
-		/*
-		 * The SG entry passed in may not have the 'length' member
-		 * set correctly so use a local SG entry (sgl) with the
-		 * proper value for 'length' instead.  If this is not done,
-		 * the dmaengine may try to DMA the incorrect amount of data.
-		 */
-		sg_init_table(&ctx->sgl, 1);
-		sg_assign_page(&ctx->sgl, sg_page(ctx->sg));
-		ctx->sgl.offset = ctx->sg->offset;
-		sg_dma_len(&ctx->sgl) = len32;
-		sg_dma_address(&ctx->sgl) = sg_dma_address(ctx->sg);
-
-		tx = dmaengine_prep_slave_sg(dd->dma_lch, &ctx->sgl, 1,
-			DMA_MEM_TO_DEV, DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
-	} else {
-		tx = dmaengine_prep_slave_single(dd->dma_lch, dma_addr, len32,
-			DMA_MEM_TO_DEV, DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
-	}
+	tx = dmaengine_prep_slave_sg(dd->dma_lch, ctx->sg, ctx->sg_len,
+				     DMA_MEM_TO_DEV,
+				     DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
 
 	if (!tx) {
-		dev_err(dd->dev, "prep_slave_sg/single() failed\n");
+		dev_err(dd->dev, "prep_slave_sg failed\n");
 		return -EINVAL;
 	}
 
@@ -611,6 +623,7 @@ static int omap_sham_xmit_dma(struct omap_sham_dev *dd, dma_addr_t dma_addr,
 	dd->pdata->write_ctrl(dd, length, final, 1);
 
 	ctx->digcnt += length;
+	ctx->total -= length;
 
 	if (final)
 		set_bit(FLAGS_FINAL, &dd->flags); /* catch last interrupt */
@@ -625,189 +638,257 @@ static int omap_sham_xmit_dma(struct omap_sham_dev *dd, dma_addr_t dma_addr,
 	return -EINPROGRESS;
 }
 
-static size_t omap_sham_append_buffer(struct omap_sham_reqctx *ctx,
-				const u8 *data, size_t length)
+static int omap_sham_copy_sg_lists(struct omap_sham_reqctx *ctx,
+				   struct scatterlist *sg, int bs, int new_len)
 {
-	size_t count = min(length, ctx->buflen - ctx->bufcnt);
+	int n = sg_nents(sg);
+	struct scatterlist *tmp;
+	int offset = ctx->offset;
 
-	count = min(count, ctx->total);
-	if (count <= 0)
-		return 0;
-	memcpy(ctx->buffer + ctx->bufcnt, data, count);
-	ctx->bufcnt += count;
+	if (ctx->bufcnt)
+		n++;
 
-	return count;
-}
+	ctx->sg = kmalloc_array(n, sizeof(*sg), GFP_KERNEL);
+	if (!ctx->sg)
+		return -ENOMEM;
 
-static size_t omap_sham_append_sg(struct omap_sham_reqctx *ctx)
-{
-	size_t count;
-	const u8 *vaddr;
+	sg_init_table(ctx->sg, n);
 
-	while (ctx->sg) {
-		vaddr = kmap_atomic(sg_page(ctx->sg));
-		vaddr += ctx->sg->offset;
+	tmp = ctx->sg;
 
-		count = omap_sham_append_buffer(ctx,
-				vaddr + ctx->offset,
-				ctx->sg->length - ctx->offset);
+	ctx->sg_len = 0;
 
-		kunmap_atomic((void *)vaddr);
+	if (ctx->bufcnt) {
+		sg_set_buf(tmp, ctx->dd->xmit_buf, ctx->bufcnt);
+		tmp = sg_next(tmp);
+		ctx->sg_len++;
+	}
 
-		if (!count)
-			break;
-		ctx->offset += count;
-		ctx->total -= count;
-		if (ctx->offset == ctx->sg->length) {
-			ctx->sg = sg_next(ctx->sg);
-			if (ctx->sg)
-				ctx->offset = 0;
-			else
-				ctx->total = 0;
+	while (sg && new_len) {
+		int len = sg->length - offset;
+
+		if (offset) {
+			offset -= sg->length;
+			if (offset < 0)
+				offset = 0;
+		}
+
+		if (new_len < len)
+			len = new_len;
+
+		if (len > 0) {
+			new_len -= len;
+			sg_set_page(tmp, sg_page(sg), len, sg->offset);
+			if (new_len <= 0)
+				sg_mark_end(tmp);
+			tmp = sg_next(tmp);
+			ctx->sg_len++;
 		}
+
+		sg = sg_next(sg);
 	}
 
+	set_bit(FLAGS_SGS_ALLOCED, &ctx->dd->flags);
+
+	ctx->bufcnt = 0;
+
 	return 0;
 }
 
-static int omap_sham_xmit_dma_map(struct omap_sham_dev *dd,
-					struct omap_sham_reqctx *ctx,
-					size_t length, int final)
+static int omap_sham_copy_sgs(struct omap_sham_reqctx *ctx,
+			      struct scatterlist *sg, int bs, int new_len)
 {
-	int ret;
+	int pages;
+	void *buf;
+	int len;
 
-	ctx->dma_addr = dma_map_single(dd->dev, ctx->buffer, ctx->buflen,
-				       DMA_TO_DEVICE);
-	if (dma_mapping_error(dd->dev, ctx->dma_addr)) {
-		dev_err(dd->dev, "dma %u bytes error\n", ctx->buflen);
-		return -EINVAL;
+	len = new_len + ctx->bufcnt;
+
+	pages = get_order(ctx->total);
+
+	buf = (void *)__get_free_pages(GFP_ATOMIC, pages);
+	if (!buf) {
+		pr_err("Couldn't allocate pages for unaligned cases.\n");
+		return -ENOMEM;
 	}
 
-	ctx->flags &= ~BIT(FLAGS_SG);
+	if (ctx->bufcnt)
+		memcpy(buf, ctx->dd->xmit_buf, ctx->bufcnt);
 
-	ret = omap_sham_xmit_dma(dd, ctx->dma_addr, length, final, 0);
-	if (ret != -EINPROGRESS)
-		dma_unmap_single(dd->dev, ctx->dma_addr, ctx->buflen,
-				 DMA_TO_DEVICE);
+	scatterwalk_map_and_copy(buf + ctx->bufcnt, sg, ctx->offset,
+				 ctx->total - ctx->bufcnt, 0);
+	sg_init_table(ctx->sgl, 1);
+	sg_set_buf(ctx->sgl, buf, len);
+	ctx->sg = ctx->sgl;
+	set_bit(FLAGS_SGS_COPIED, &ctx->dd->flags);
+	ctx->sg_len = 1;
+	ctx->bufcnt = 0;
+	ctx->offset = 0;
 
-	return ret;
+	return 0;
 }
 
-static int omap_sham_update_dma_slow(struct omap_sham_dev *dd)
+static int omap_sham_align_sgs(struct scatterlist *sg,
+			       int nbytes, int bs, bool final,
+			       struct omap_sham_reqctx *rctx)
 {
-	struct omap_sham_reqctx *ctx = ahash_request_ctx(dd->req);
-	unsigned int final;
-	size_t count;
+	int n = 0;
+	bool aligned = true;
+	bool list_ok = true;
+	struct scatterlist *sg_tmp = sg;
+	int new_len;
+	int offset = rctx->offset;
 
-	omap_sham_append_sg(ctx);
+	if (!sg || !sg->length || !nbytes)
+		return 0;
+
+	new_len = nbytes;
 
-	final = (ctx->flags & BIT(FLAGS_FINUP)) && !ctx->total;
+	if (offset)
+		list_ok = false;
 
-	dev_dbg(dd->dev, "slow: bufcnt: %u, digcnt: %d, final: %d\n",
-					 ctx->bufcnt, ctx->digcnt, final);
+	if (final)
+		new_len = DIV_ROUND_UP(new_len, bs) * bs;
+	else
+		new_len = new_len / bs * bs;
 
-	if (final || (ctx->bufcnt == ctx->buflen && ctx->total)) {
-		count = ctx->bufcnt;
-		ctx->bufcnt = 0;
-		return omap_sham_xmit_dma_map(dd, ctx, count, final);
+	while (nbytes > 0 && sg_tmp) {
+		n++;
+
+		if (offset < sg_tmp->length) {
+			if (!IS_ALIGNED(offset + sg_tmp->offset, 4)) {
+				aligned = false;
+				break;
+			}
+
+			if (!IS_ALIGNED(sg_tmp->length - offset, bs)) {
+				aligned = false;
+				break;
+			}
+		}
+
+		if (offset) {
+			offset -= sg_tmp->length;
+			if (offset < 0) {
+				nbytes += offset;
+				offset = 0;
+			}
+		} else {
+			nbytes -= sg_tmp->length;
+		}
+
+		sg_tmp = sg_next(sg_tmp);
+
+		if (nbytes < 0) {
+			list_ok = false;
+			break;
+		}
 	}
 
+	if (!aligned)
+		return omap_sham_copy_sgs(rctx, sg, bs, new_len);
+	else if (!list_ok)
+		return omap_sham_copy_sg_lists(rctx, sg, bs, new_len);
+
+	rctx->sg_len = n;
+	rctx->sg = sg;
+
 	return 0;
 }
 
-/* Start address alignment */
-#define SG_AA(sg)	(IS_ALIGNED(sg->offset, sizeof(u32)))
-/* SHA1 block size alignment */
-#define SG_SA(sg, bs)	(IS_ALIGNED(sg->length, bs))
-
-static int omap_sham_update_dma_start(struct omap_sham_dev *dd)
+static int omap_sham_prepare_request(struct ahash_request *req, bool update)
 {
-	struct omap_sham_reqctx *ctx = ahash_request_ctx(dd->req);
-	unsigned int length, final, tail;
-	struct scatterlist *sg;
-	int ret, bs;
+	struct omap_sham_reqctx *rctx = ahash_request_ctx(req);
+	int bs;
+	int ret;
+	int nbytes;
+	bool final = rctx->flags & BIT(FLAGS_FINUP);
+	int xmit_len, hash_later;
 
-	if (!ctx->total)
+	if (!req)
 		return 0;
 
-	if (ctx->bufcnt || ctx->offset)
-		return omap_sham_update_dma_slow(dd);
-
-	/*
-	 * Don't use the sg interface when the transfer size is less
-	 * than the number of elements in a DMA frame.  Otherwise,
-	 * the dmaengine infrastructure will calculate that it needs
-	 * to transfer 0 frames which ultimately fails.
-	 */
-	if (ctx->total < get_block_size(ctx))
-		return omap_sham_update_dma_slow(dd);
-
-	dev_dbg(dd->dev, "fast: digcnt: %d, bufcnt: %u, total: %u\n",
-			ctx->digcnt, ctx->bufcnt, ctx->total);
+	bs = get_block_size(rctx);
 
-	sg = ctx->sg;
-	bs = get_block_size(ctx);
+	if (update)
+		nbytes = req->nbytes;
+	else
+		nbytes = 0;
 
-	if (!SG_AA(sg))
-		return omap_sham_update_dma_slow(dd);
+	rctx->total = nbytes + rctx->bufcnt;
 
-	if (!sg_is_last(sg) && !SG_SA(sg, bs))
-		/* size is not BLOCK_SIZE aligned */
-		return omap_sham_update_dma_slow(dd);
+	if (!rctx->total)
+		return 0;
 
-	length = min(ctx->total, sg->length);
+	if (nbytes && (!IS_ALIGNED(rctx->bufcnt, bs))) {
+		int len = bs - rctx->bufcnt % bs;
 
-	if (sg_is_last(sg)) {
-		if (!(ctx->flags & BIT(FLAGS_FINUP))) {
-			/* not last sg must be BLOCK_SIZE aligned */
-			tail = length & (bs - 1);
-			/* without finup() we need one block to close hash */
-			if (!tail)
-				tail = bs;
-			length -= tail;
-		}
+		if (len > nbytes)
+			len = nbytes;
+		scatterwalk_map_and_copy(rctx->buffer + rctx->bufcnt, req->src,
+					 0, len, 0);
+		rctx->bufcnt += len;
+		nbytes -= len;
+		rctx->offset = len;
 	}
 
-	if (!dma_map_sg(dd->dev, ctx->sg, 1, DMA_TO_DEVICE)) {
-		dev_err(dd->dev, "dma_map_sg  error\n");
-		return -EINVAL;
-	}
+	if (rctx->bufcnt)
+		memcpy(rctx->dd->xmit_buf, rctx->buffer, rctx->bufcnt);
 
-	ctx->flags |= BIT(FLAGS_SG);
+	ret = omap_sham_align_sgs(req->src, nbytes, bs, final, rctx);
+	if (ret)
+		return ret;
 
-	ctx->total -= length;
-	ctx->offset = length; /* offset where to start slow */
+	xmit_len = rctx->total;
 
-	final = (ctx->flags & BIT(FLAGS_FINUP)) && !ctx->total;
+	if (!IS_ALIGNED(xmit_len, bs)) {
+		if (final)
+			xmit_len = DIV_ROUND_UP(xmit_len, bs) * bs;
+		else
+			xmit_len = xmit_len / bs * bs;
+	}
 
-	ret = omap_sham_xmit_dma(dd, sg_dma_address(ctx->sg), length, final, 1);
-	if (ret != -EINPROGRESS)
-		dma_unmap_sg(dd->dev, ctx->sg, 1, DMA_TO_DEVICE);
+	hash_later = rctx->total - xmit_len;
+	if (hash_later < 0)
+		hash_later = 0;
 
-	return ret;
-}
+	if (rctx->bufcnt && nbytes) {
+		/* have data from previous operation and current */
+		sg_init_table(rctx->sgl, 2);
+		sg_set_buf(rctx->sgl, rctx->dd->xmit_buf, rctx->bufcnt);
 
-static int omap_sham_update_cpu(struct omap_sham_dev *dd)
-{
-	struct omap_sham_reqctx *ctx = ahash_request_ctx(dd->req);
-	int bufcnt, final;
+		sg_chain(rctx->sgl, 2, req->src);
 
-	if (!ctx->total)
-		return 0;
+		rctx->sg = rctx->sgl;
 
-	omap_sham_append_sg(ctx);
+		rctx->sg_len++;
+	} else if (rctx->bufcnt) {
+		/* have buffered data only */
+		sg_init_table(rctx->sgl, 1);
+		sg_set_buf(rctx->sgl, rctx->dd->xmit_buf, xmit_len);
 
-	final = (ctx->flags & BIT(FLAGS_FINUP)) && !ctx->total;
+		rctx->sg = rctx->sgl;
 
-	dev_dbg(dd->dev, "cpu: bufcnt: %u, digcnt: %d, final: %d\n",
-		ctx->bufcnt, ctx->digcnt, final);
+		rctx->sg_len = 1;
+	}
 
-	if (final || (ctx->bufcnt == ctx->buflen && ctx->total)) {
-		bufcnt = ctx->bufcnt;
-		ctx->bufcnt = 0;
-		return omap_sham_xmit_cpu(dd, ctx->buffer, bufcnt, final);
+	if (hash_later) {
+		if (req->nbytes) {
+			scatterwalk_map_and_copy(rctx->buffer, req->src,
+						 req->nbytes - hash_later,
+						 hash_later, 0);
+		} else {
+			memcpy(rctx->buffer, rctx->buffer + xmit_len,
+			       hash_later);
+		}
+		rctx->bufcnt = hash_later;
+	} else {
+		rctx->bufcnt = 0;
 	}
 
+	if (!final)
+		rctx->total = xmit_len;
+
 	return 0;
 }
 
@@ -815,18 +896,9 @@ static int omap_sham_update_dma_stop(struct omap_sham_dev *dd)
 {
 	struct omap_sham_reqctx *ctx = ahash_request_ctx(dd->req);
 
+	dma_unmap_sg(dd->dev, ctx->sg, ctx->sg_len, DMA_TO_DEVICE);
 
-	if (ctx->flags & BIT(FLAGS_SG)) {
-		dma_unmap_sg(dd->dev, ctx->sg, 1, DMA_TO_DEVICE);
-		if (ctx->sg->length == ctx->offset) {
-			ctx->sg = sg_next(ctx->sg);
-			if (ctx->sg)
-				ctx->offset = 0;
-		}
-	} else {
-		dma_unmap_single(dd->dev, ctx->dma_addr, ctx->buflen,
-				 DMA_TO_DEVICE);
-	}
+	clear_bit(FLAGS_DMA_ACTIVE, &dd->flags);
 
 	return 0;
 }
@@ -887,6 +959,8 @@ static int omap_sham_init(struct ahash_request *req)
 
 	ctx->bufcnt = 0;
 	ctx->digcnt = 0;
+	ctx->total = 0;
+	ctx->offset = 0;
 	ctx->buflen = BUFLEN;
 
 	if (tctx->flags & BIT(FLAGS_HMAC)) {
@@ -909,14 +983,19 @@ static int omap_sham_update_req(struct omap_sham_dev *dd)
 	struct ahash_request *req = dd->req;
 	struct omap_sham_reqctx *ctx = ahash_request_ctx(req);
 	int err;
+	bool final = ctx->flags & BIT(FLAGS_FINUP);
 
 	dev_dbg(dd->dev, "update_req: total: %u, digcnt: %d, finup: %d\n",
 		 ctx->total, ctx->digcnt, (ctx->flags & BIT(FLAGS_FINUP)) != 0);
 
+	if (ctx->total < get_block_size(ctx) ||
+	    ctx->total < OMAP_SHA_DMA_THRESHOLD)
+		ctx->flags |= BIT(FLAGS_CPU);
+
 	if (ctx->flags & BIT(FLAGS_CPU))
-		err = omap_sham_update_cpu(dd);
+		err = omap_sham_xmit_cpu(dd, ctx->total, final);
 	else
-		err = omap_sham_update_dma_start(dd);
+		err = omap_sham_xmit_dma(dd, ctx->total, final);
 
 	/* wait for dma completion before can take more data */
 	dev_dbg(dd->dev, "update: err: %d, digcnt: %d\n", err, ctx->digcnt);
@@ -930,7 +1009,7 @@ static int omap_sham_final_req(struct omap_sham_dev *dd)
 	struct omap_sham_reqctx *ctx = ahash_request_ctx(req);
 	int err = 0, use_dma = 1;
 
-	if ((ctx->bufcnt <= get_block_size(ctx)) || dd->polling_mode)
+	if ((ctx->total <= get_block_size(ctx)) || dd->polling_mode)
 		/*
 		 * faster to handle last block with cpu or
 		 * use cpu when dma is not present.
@@ -938,9 +1017,9 @@ static int omap_sham_final_req(struct omap_sham_dev *dd)
 		use_dma = 0;
 
 	if (use_dma)
-		err = omap_sham_xmit_dma_map(dd, ctx, ctx->bufcnt, 1);
+		err = omap_sham_xmit_dma(dd, ctx->total, 1);
 	else
-		err = omap_sham_xmit_cpu(dd, ctx->buffer, ctx->bufcnt, 1);
+		err = omap_sham_xmit_cpu(dd, ctx->total, 1);
 
 	ctx->bufcnt = 0;
 
@@ -988,6 +1067,17 @@ static void omap_sham_finish_req(struct ahash_request *req, int err)
 	struct omap_sham_reqctx *ctx = ahash_request_ctx(req);
 	struct omap_sham_dev *dd = ctx->dd;
 
+	if (test_bit(FLAGS_SGS_COPIED, &dd->flags))
+		free_pages((unsigned long)sg_virt(ctx->sg),
+			   get_order(ctx->sg->length));
+
+	if (test_bit(FLAGS_SGS_ALLOCED, &dd->flags))
+		kfree(ctx->sg);
+
+	ctx->sg = NULL;
+
+	dd->flags &= ~(BIT(FLAGS_SGS_ALLOCED) | BIT(FLAGS_SGS_COPIED));
+
 	if (!err) {
 		dd->pdata->copy_hash(req, 1);
 		if (test_bit(FLAGS_FINAL, &dd->flags))
@@ -1005,9 +1095,6 @@ static void omap_sham_finish_req(struct ahash_request *req, int err)
 
 	if (req->base.complete)
 		req->base.complete(&req->base, err);
-
-	/* handle new request */
-	tasklet_schedule(&dd->done_task);
 }
 
 static int omap_sham_handle_queue(struct omap_sham_dev *dd,
@@ -1018,6 +1105,7 @@ static int omap_sham_handle_queue(struct omap_sham_dev *dd,
 	unsigned long flags;
 	int err = 0, ret = 0;
 
+retry:
 	spin_lock_irqsave(&dd->lock, flags);
 	if (req)
 		ret = ahash_enqueue_request(&dd->queue, req);
@@ -1041,6 +1129,10 @@ static int omap_sham_handle_queue(struct omap_sham_dev *dd,
 	dd->req = req;
 	ctx = ahash_request_ctx(req);
 
+	err = omap_sham_prepare_request(req, ctx->op == OP_UPDATE);
+	if (err)
+		goto err1;
+
 	dev_dbg(dd->dev, "handling new req, op: %lu, nbytes: %d\n",
 						ctx->op, req->nbytes);
 
@@ -1061,11 +1153,19 @@ static int omap_sham_handle_queue(struct omap_sham_dev *dd,
 		err = omap_sham_final_req(dd);
 	}
 err1:
-	if (err != -EINPROGRESS)
+	dev_dbg(dd->dev, "exit, err: %d\n", err);
+
+	if (err != -EINPROGRESS) {
 		/* done_task will not finish it, so do it here */
 		omap_sham_finish_req(req, err);
+		req = NULL;
 
-	dev_dbg(dd->dev, "exit, err: %d\n", err);
+		/*
+		 * Execute next request immediately if there is anything
+		 * in queue.
+		 */
+		goto retry;
+	}
 
 	return ret;
 }
@@ -1085,34 +1185,15 @@ static int omap_sham_update(struct ahash_request *req)
 {
 	struct omap_sham_reqctx *ctx = ahash_request_ctx(req);
 	struct omap_sham_dev *dd = ctx->dd;
-	int bs = get_block_size(ctx);
 
 	if (!req->nbytes)
 		return 0;
 
-	ctx->total = req->nbytes;
-	ctx->sg = req->src;
-	ctx->offset = 0;
-
-	if (ctx->flags & BIT(FLAGS_FINUP)) {
-		if ((ctx->digcnt + ctx->bufcnt + ctx->total) < 240) {
-			/*
-			* OMAP HW accel works only with buffers >= 9
-			* will switch to bypass in final()
-			* final has the same request and data
-			*/
-			omap_sham_append_sg(ctx);
-			return 0;
-		} else if ((ctx->bufcnt + ctx->total <= bs) ||
-			   dd->polling_mode) {
-			/*
-			 * faster to use CPU for short transfers or
-			 * use cpu when dma is not present.
-			 */
-			ctx->flags |= BIT(FLAGS_CPU);
-		}
-	} else if (ctx->bufcnt + ctx->total < ctx->buflen) {
-		omap_sham_append_sg(ctx);
+	if (ctx->total + req->nbytes < ctx->buflen) {
+		scatterwalk_map_and_copy(ctx->buffer + ctx->bufcnt, req->src,
+					 0, req->nbytes, 0);
+		ctx->bufcnt += req->nbytes;
+		ctx->total += req->nbytes;
 		return 0;
 	}
 
@@ -1137,9 +1218,20 @@ static int omap_sham_final_shash(struct ahash_request *req)
 {
 	struct omap_sham_ctx *tctx = crypto_tfm_ctx(req->base.tfm);
 	struct omap_sham_reqctx *ctx = ahash_request_ctx(req);
+	int offset = 0;
+
+	/*
+	 * If we are running HMAC on limited hardware support, skip
+	 * the ipad in the beginning of the buffer if we are going for
+	 * software fallback algorithm.
+	 */
+	if (test_bit(FLAGS_HMAC, &ctx->flags) &&
+	    !test_bit(FLAGS_AUTO_XOR, &ctx->dd->flags))
+		offset = get_block_size(ctx);
 
 	return omap_sham_shash_digest(tctx->fallback, req->base.flags,
-				      ctx->buffer, ctx->bufcnt, req->result);
+				      ctx->buffer + offset,
+				      ctx->bufcnt - offset, req->result);
 }
 
 static int omap_sham_final(struct ahash_request *req)
@@ -1154,10 +1246,11 @@ static int omap_sham_final(struct ahash_request *req)
 	/*
 	 * OMAP HW accel works only with buffers >= 9.
 	 * HMAC is always >= 9 because ipad == block size.
-	 * If buffersize is less than 240, we use fallback SW encoding,
-	 * as using DMA + HW in this case doesn't provide any benefit.
+	 * If buffersize is less than DMA_THRESHOLD, we use fallback
+	 * SW encoding, as using DMA + HW in this case doesn't provide
+	 * any benefit.
 	 */
-	if ((ctx->digcnt + ctx->bufcnt) < 240)
+	if (!ctx->digcnt && ctx->bufcnt < OMAP_SHA_DMA_THRESHOLD)
 		return omap_sham_final_shash(req);
 	else if (ctx->bufcnt)
 		return omap_sham_enqueue(req, OP_FINAL);
@@ -1323,6 +1416,25 @@ static void omap_sham_cra_exit(struct crypto_tfm *tfm)
 	}
 }
 
+static int omap_sham_export(struct ahash_request *req, void *out)
+{
+	struct omap_sham_reqctx *rctx = ahash_request_ctx(req);
+
+	memcpy(out, rctx, sizeof(*rctx) + rctx->bufcnt);
+
+	return 0;
+}
+
+static int omap_sham_import(struct ahash_request *req, const void *in)
+{
+	struct omap_sham_reqctx *rctx = ahash_request_ctx(req);
+	const struct omap_sham_reqctx *ctx_in = in;
+
+	memcpy(rctx, in, sizeof(*rctx) + ctx_in->bufcnt);
+
+	return 0;
+}
+
 static struct ahash_alg algs_sha1_md5[] = {
 {
 	.init		= omap_sham_init,
@@ -1341,7 +1453,7 @@ static struct ahash_alg algs_sha1_md5[] = {
 						CRYPTO_ALG_NEED_FALLBACK,
 		.cra_blocksize		= SHA1_BLOCK_SIZE,
 		.cra_ctxsize		= sizeof(struct omap_sham_ctx),
-		.cra_alignmask		= 0,
+		.cra_alignmask		= OMAP_ALIGN_MASK,
 		.cra_module		= THIS_MODULE,
 		.cra_init		= omap_sham_cra_init,
 		.cra_exit		= omap_sham_cra_exit,
@@ -1440,7 +1552,7 @@ static struct ahash_alg algs_sha224_sha256[] = {
 						CRYPTO_ALG_NEED_FALLBACK,
 		.cra_blocksize		= SHA224_BLOCK_SIZE,
 		.cra_ctxsize		= sizeof(struct omap_sham_ctx),
-		.cra_alignmask		= 0,
+		.cra_alignmask		= OMAP_ALIGN_MASK,
 		.cra_module		= THIS_MODULE,
 		.cra_init		= omap_sham_cra_init,
 		.cra_exit		= omap_sham_cra_exit,
@@ -1462,7 +1574,7 @@ static struct ahash_alg algs_sha224_sha256[] = {
 						CRYPTO_ALG_NEED_FALLBACK,
 		.cra_blocksize		= SHA256_BLOCK_SIZE,
 		.cra_ctxsize		= sizeof(struct omap_sham_ctx),
-		.cra_alignmask		= 0,
+		.cra_alignmask		= OMAP_ALIGN_MASK,
 		.cra_module		= THIS_MODULE,
 		.cra_init		= omap_sham_cra_init,
 		.cra_exit		= omap_sham_cra_exit,
@@ -1535,7 +1647,7 @@ static struct ahash_alg algs_sha384_sha512[] = {
 						CRYPTO_ALG_NEED_FALLBACK,
 		.cra_blocksize		= SHA384_BLOCK_SIZE,
 		.cra_ctxsize		= sizeof(struct omap_sham_ctx),
-		.cra_alignmask		= 0,
+		.cra_alignmask		= OMAP_ALIGN_MASK,
 		.cra_module		= THIS_MODULE,
 		.cra_init		= omap_sham_cra_init,
 		.cra_exit		= omap_sham_cra_exit,
@@ -1557,7 +1669,7 @@ static struct ahash_alg algs_sha384_sha512[] = {
 						CRYPTO_ALG_NEED_FALLBACK,
 		.cra_blocksize		= SHA512_BLOCK_SIZE,
 		.cra_ctxsize		= sizeof(struct omap_sham_ctx),
-		.cra_alignmask		= 0,
+		.cra_alignmask		= OMAP_ALIGN_MASK,
 		.cra_module		= THIS_MODULE,
 		.cra_init		= omap_sham_cra_init,
 		.cra_exit		= omap_sham_cra_exit,
@@ -1624,12 +1736,8 @@ static void omap_sham_done_task(unsigned long data)
 	}
 
 	if (test_bit(FLAGS_CPU, &dd->flags)) {
-		if (test_and_clear_bit(FLAGS_OUTPUT_READY, &dd->flags)) {
-			/* hash or semi-hash ready */
-			err = omap_sham_update_cpu(dd);
-			if (err != -EINPROGRESS)
-				goto finish;
-		}
+		if (test_and_clear_bit(FLAGS_OUTPUT_READY, &dd->flags))
+			goto finish;
 	} else if (test_bit(FLAGS_DMA_READY, &dd->flags)) {
 		if (test_and_clear_bit(FLAGS_DMA_ACTIVE, &dd->flags)) {
 			omap_sham_update_dma_stop(dd);
@@ -1641,8 +1749,6 @@ static void omap_sham_done_task(unsigned long data)
 		if (test_and_clear_bit(FLAGS_OUTPUT_READY, &dd->flags)) {
 			/* hash or semi-hash ready */
 			clear_bit(FLAGS_DMA_READY, &dd->flags);
-			err = omap_sham_update_dma_start(dd);
-			if (err != -EINPROGRESS)
 				goto finish;
 		}
 	}
@@ -1653,6 +1759,10 @@ finish:
 	dev_dbg(dd->dev, "update done: err: %d\n", err);
 	/* finish curent request */
 	omap_sham_finish_req(dd->req, err);
+
+	/* If we are not busy, process next req */
+	if (!test_bit(FLAGS_BUSY, &dd->flags))
+		omap_sham_handle_queue(dd, NULL);
 }
 
 static irqreturn_t omap_sham_irq_common(struct omap_sham_dev *dd)
@@ -1977,8 +2087,14 @@ static int omap_sham_probe(struct platform_device *pdev)
 
 	for (i = 0; i < dd->pdata->algs_info_size; i++) {
 		for (j = 0; j < dd->pdata->algs_info[i].size; j++) {
-			err = crypto_register_ahash(
-					&dd->pdata->algs_info[i].algs_list[j]);
+			struct ahash_alg *alg;
+
+			alg = &dd->pdata->algs_info[i].algs_list[j];
+			alg->export = omap_sham_export;
+			alg->import = omap_sham_import;
+			alg->halg.statesize = sizeof(struct omap_sham_reqctx) +
+					      BUFLEN;
+			err = crypto_register_ahash(alg);
 			if (err)
 				goto err_algs;
 
diff --git a/drivers/crypto/qat/qat_c3xxx/adf_c3xxx_hw_data.h b/drivers/crypto/qat/qat_c3xxx/adf_c3xxx_hw_data.h
index 2f2681d3458a..afc9a0a86747 100644
--- a/drivers/crypto/qat/qat_c3xxx/adf_c3xxx_hw_data.h
+++ b/drivers/crypto/qat/qat_c3xxx/adf_c3xxx_hw_data.h
@@ -55,7 +55,7 @@
 #define ADF_C3XXX_MAX_ACCELERATORS 3
 #define ADF_C3XXX_MAX_ACCELENGINES 6
 #define ADF_C3XXX_ACCELERATORS_REG_OFFSET 16
-#define ADF_C3XXX_ACCELERATORS_MASK 0x3
+#define ADF_C3XXX_ACCELERATORS_MASK 0x7
 #define ADF_C3XXX_ACCELENGINES_MASK 0x3F
 #define ADF_C3XXX_ETR_MAX_BANKS 16
 #define ADF_C3XXX_SMIAPF0_MASK_OFFSET (0x3A000 + 0x28)
diff --git a/drivers/crypto/qat/qat_common/adf_admin.c b/drivers/crypto/qat/qat_common/adf_admin.c
index ce7c4626c983..3744b22f0c46 100644
--- a/drivers/crypto/qat/qat_common/adf_admin.c
+++ b/drivers/crypto/qat/qat_common/adf_admin.c
@@ -146,6 +146,7 @@ struct adf_admin_comms {
 	dma_addr_t phy_addr;
 	dma_addr_t const_tbl_addr;
 	void *virt_addr;
+	void *virt_tbl_addr;
 	void __iomem *mailbox_addr;
 	struct mutex lock;	/* protects adf_admin_comms struct */
 };
@@ -251,17 +252,19 @@ int adf_init_admin_comms(struct adf_accel_dev *accel_dev)
 		return -ENOMEM;
 	}
 
-	admin->const_tbl_addr = dma_map_single(&GET_DEV(accel_dev),
-					       (void *) const_tab, 1024,
-					       DMA_TO_DEVICE);
-
-	if (unlikely(dma_mapping_error(&GET_DEV(accel_dev),
-				       admin->const_tbl_addr))) {
+	admin->virt_tbl_addr = dma_zalloc_coherent(&GET_DEV(accel_dev),
+						   PAGE_SIZE,
+						   &admin->const_tbl_addr,
+						   GFP_KERNEL);
+	if (!admin->virt_tbl_addr) {
+		dev_err(&GET_DEV(accel_dev), "Failed to allocate const_tbl\n");
 		dma_free_coherent(&GET_DEV(accel_dev), PAGE_SIZE,
 				  admin->virt_addr, admin->phy_addr);
 		kfree(admin);
 		return -ENOMEM;
 	}
+
+	memcpy(admin->virt_tbl_addr, const_tab, sizeof(const_tab));
 	reg_val = (u64)admin->phy_addr;
 	ADF_CSR_WR(csr, ADF_DH895XCC_ADMINMSGUR_OFFSET, reg_val >> 32);
 	ADF_CSR_WR(csr, ADF_DH895XCC_ADMINMSGLR_OFFSET, reg_val);
@@ -282,9 +285,10 @@ void adf_exit_admin_comms(struct adf_accel_dev *accel_dev)
 	if (admin->virt_addr)
 		dma_free_coherent(&GET_DEV(accel_dev), PAGE_SIZE,
 				  admin->virt_addr, admin->phy_addr);
+	if (admin->virt_tbl_addr)
+		dma_free_coherent(&GET_DEV(accel_dev), PAGE_SIZE,
+				  admin->virt_tbl_addr, admin->const_tbl_addr);
 
-	dma_unmap_single(&GET_DEV(accel_dev), admin->const_tbl_addr, 1024,
-			 DMA_TO_DEVICE);
 	mutex_destroy(&admin->lock);
 	kfree(admin);
 	accel_dev->admin = NULL;
diff --git a/drivers/crypto/qat/qat_common/qat_uclo.c b/drivers/crypto/qat/qat_common/qat_uclo.c
index 9b961b37a282..e2454d90d949 100644
--- a/drivers/crypto/qat/qat_common/qat_uclo.c
+++ b/drivers/crypto/qat/qat_common/qat_uclo.c
@@ -967,10 +967,6 @@ static int qat_uclo_parse_uof_obj(struct icp_qat_fw_loader_handle *handle)
 	struct icp_qat_uclo_objhandle *obj_handle = handle->obj_handle;
 	unsigned int ae;
 
-	obj_handle->uword_buf = kcalloc(UWORD_CPYBUF_SIZE, sizeof(uint64_t),
-					GFP_KERNEL);
-	if (!obj_handle->uword_buf)
-		return -ENOMEM;
 	obj_handle->encap_uof_obj.beg_uof = obj_handle->obj_hdr->file_buff;
 	obj_handle->encap_uof_obj.obj_hdr = (struct icp_qat_uof_objhdr *)
 					     obj_handle->obj_hdr->file_buff;
@@ -982,6 +978,10 @@ static int qat_uclo_parse_uof_obj(struct icp_qat_fw_loader_handle *handle)
 		pr_err("QAT: UOF incompatible\n");
 		return -EINVAL;
 	}
+	obj_handle->uword_buf = kcalloc(UWORD_CPYBUF_SIZE, sizeof(uint64_t),
+					GFP_KERNEL);
+	if (!obj_handle->uword_buf)
+		return -ENOMEM;
 	obj_handle->ustore_phy_size = ICP_QAT_UCLO_MAX_USTORE;
 	if (!obj_handle->obj_hdr->file_buff ||
 	    !qat_uclo_map_str_table(obj_handle->obj_hdr, ICP_QAT_UOF_STRT,
diff --git a/drivers/crypto/rockchip/rk3288_crypto.c b/drivers/crypto/rockchip/rk3288_crypto.c
index af508258d2ea..d0f80c6241f9 100644
--- a/drivers/crypto/rockchip/rk3288_crypto.c
+++ b/drivers/crypto/rockchip/rk3288_crypto.c
@@ -304,11 +304,9 @@ static int rk_crypto_probe(struct platform_device *pdev)
 	usleep_range(10, 20);
 	reset_control_deassert(crypto_info->rst);
 
-	err = devm_add_action(dev, rk_crypto_action, crypto_info);
-	if (err) {
-		reset_control_assert(crypto_info->rst);
+	err = devm_add_action_or_reset(dev, rk_crypto_action, crypto_info);
+	if (err)
 		goto err_crypto;
-	}
 
 	spin_lock_init(&crypto_info->lock);
 
diff --git a/drivers/crypto/sunxi-ss/sun4i-ss-cipher.c b/drivers/crypto/sunxi-ss/sun4i-ss-cipher.c
index 3830d7c4e138..90efd10d57a1 100644
--- a/drivers/crypto/sunxi-ss/sun4i-ss-cipher.c
+++ b/drivers/crypto/sunxi-ss/sun4i-ss-cipher.c
@@ -29,7 +29,8 @@ static int sun4i_ss_opti_poll(struct ablkcipher_request *areq)
 	u32 tx_cnt = 0;
 	u32 spaces;
 	u32 v;
-	int i, err = 0;
+	int err = 0;
+	unsigned int i;
 	unsigned int ileft = areq->nbytes;
 	unsigned int oleft = areq->nbytes;
 	unsigned int todo;
@@ -139,7 +140,8 @@ static int sun4i_ss_cipher_poll(struct ablkcipher_request *areq)
 	u32 tx_cnt = 0;
 	u32 v;
 	u32 spaces;
-	int i, err = 0;
+	int err = 0;
+	unsigned int i;
 	unsigned int ileft = areq->nbytes;
 	unsigned int oleft = areq->nbytes;
 	unsigned int todo;
diff --git a/drivers/crypto/sunxi-ss/sun4i-ss-core.c b/drivers/crypto/sunxi-ss/sun4i-ss-core.c
index 107cd2a41cae..3ac6c6c4ad18 100644
--- a/drivers/crypto/sunxi-ss/sun4i-ss-core.c
+++ b/drivers/crypto/sunxi-ss/sun4i-ss-core.c
@@ -172,45 +172,45 @@ static struct sun4i_ss_alg_template ss_algs[] = {
 },
 {       .type = CRYPTO_ALG_TYPE_ABLKCIPHER,
 	.alg.crypto = {
-			.cra_name = "cbc(des3_ede)",
-			.cra_driver_name = "cbc-des3-sun4i-ss",
-			.cra_priority = 300,
-			.cra_blocksize = DES3_EDE_BLOCK_SIZE,
-			.cra_flags = CRYPTO_ALG_TYPE_ABLKCIPHER,
-			.cra_ctxsize = sizeof(struct sun4i_req_ctx),
-			.cra_module = THIS_MODULE,
-			.cra_alignmask = 3,
-			.cra_type = &crypto_ablkcipher_type,
-			.cra_init = sun4i_ss_cipher_init,
-			.cra_u.ablkcipher = {
-				.min_keysize    = DES3_EDE_KEY_SIZE,
-				.max_keysize    = DES3_EDE_KEY_SIZE,
-				.ivsize         = DES3_EDE_BLOCK_SIZE,
-				.setkey         = sun4i_ss_des3_setkey,
-				.encrypt        = sun4i_ss_cbc_des3_encrypt,
-				.decrypt        = sun4i_ss_cbc_des3_decrypt,
+		.cra_name = "cbc(des3_ede)",
+		.cra_driver_name = "cbc-des3-sun4i-ss",
+		.cra_priority = 300,
+		.cra_blocksize = DES3_EDE_BLOCK_SIZE,
+		.cra_flags = CRYPTO_ALG_TYPE_ABLKCIPHER,
+		.cra_ctxsize = sizeof(struct sun4i_req_ctx),
+		.cra_module = THIS_MODULE,
+		.cra_alignmask = 3,
+		.cra_type = &crypto_ablkcipher_type,
+		.cra_init = sun4i_ss_cipher_init,
+		.cra_u.ablkcipher = {
+			.min_keysize    = DES3_EDE_KEY_SIZE,
+			.max_keysize    = DES3_EDE_KEY_SIZE,
+			.ivsize         = DES3_EDE_BLOCK_SIZE,
+			.setkey         = sun4i_ss_des3_setkey,
+			.encrypt        = sun4i_ss_cbc_des3_encrypt,
+			.decrypt        = sun4i_ss_cbc_des3_decrypt,
 		}
 	}
 },
 {       .type = CRYPTO_ALG_TYPE_ABLKCIPHER,
 	.alg.crypto = {
-			.cra_name = "ecb(des3_ede)",
-			.cra_driver_name = "ecb-des3-sun4i-ss",
-			.cra_priority = 300,
-			.cra_blocksize = DES3_EDE_BLOCK_SIZE,
-			.cra_flags = CRYPTO_ALG_TYPE_ABLKCIPHER,
-			.cra_ctxsize = sizeof(struct sun4i_req_ctx),
-			.cra_module = THIS_MODULE,
-			.cra_alignmask = 3,
-			.cra_type = &crypto_ablkcipher_type,
-			.cra_init = sun4i_ss_cipher_init,
-			.cra_u.ablkcipher = {
-				.min_keysize    = DES3_EDE_KEY_SIZE,
-				.max_keysize    = DES3_EDE_KEY_SIZE,
-				.ivsize         = DES3_EDE_BLOCK_SIZE,
-				.setkey         = sun4i_ss_des3_setkey,
-				.encrypt        = sun4i_ss_ecb_des3_encrypt,
-				.decrypt        = sun4i_ss_ecb_des3_decrypt,
+		.cra_name = "ecb(des3_ede)",
+		.cra_driver_name = "ecb-des3-sun4i-ss",
+		.cra_priority = 300,
+		.cra_blocksize = DES3_EDE_BLOCK_SIZE,
+		.cra_flags = CRYPTO_ALG_TYPE_ABLKCIPHER,
+		.cra_ctxsize = sizeof(struct sun4i_req_ctx),
+		.cra_module = THIS_MODULE,
+		.cra_alignmask = 3,
+		.cra_type = &crypto_ablkcipher_type,
+		.cra_init = sun4i_ss_cipher_init,
+		.cra_u.ablkcipher = {
+			.min_keysize    = DES3_EDE_KEY_SIZE,
+			.max_keysize    = DES3_EDE_KEY_SIZE,
+			.ivsize         = DES3_EDE_BLOCK_SIZE,
+			.setkey         = sun4i_ss_des3_setkey,
+			.encrypt        = sun4i_ss_ecb_des3_encrypt,
+			.decrypt        = sun4i_ss_ecb_des3_decrypt,
 		}
 	}
 },
diff --git a/drivers/crypto/sunxi-ss/sun4i-ss-hash.c b/drivers/crypto/sunxi-ss/sun4i-ss-hash.c
index ff8031498809..0de2f62d51ff 100644
--- a/drivers/crypto/sunxi-ss/sun4i-ss-hash.c
+++ b/drivers/crypto/sunxi-ss/sun4i-ss-hash.c
@@ -20,6 +20,15 @@
 
 int sun4i_hash_crainit(struct crypto_tfm *tfm)
 {
+	struct sun4i_tfm_ctx *op = crypto_tfm_ctx(tfm);
+	struct ahash_alg *alg = __crypto_ahash_alg(tfm->__crt_alg);
+	struct sun4i_ss_alg_template *algt;
+
+	memset(op, 0, sizeof(struct sun4i_tfm_ctx));
+
+	algt = container_of(alg, struct sun4i_ss_alg_template, alg.hash);
+	op->ss = algt->ss;
+
 	crypto_ahash_set_reqsize(__crypto_ahash_cast(tfm),
 				 sizeof(struct sun4i_req_ctx));
 	return 0;
@@ -32,13 +41,10 @@ int sun4i_hash_init(struct ahash_request *areq)
 	struct crypto_ahash *tfm = crypto_ahash_reqtfm(areq);
 	struct ahash_alg *alg = __crypto_ahash_alg(tfm->base.__crt_alg);
 	struct sun4i_ss_alg_template *algt;
-	struct sun4i_ss_ctx *ss;
 
 	memset(op, 0, sizeof(struct sun4i_req_ctx));
 
 	algt = container_of(alg, struct sun4i_ss_alg_template, alg.hash);
-	ss = algt->ss;
-	op->ss = algt->ss;
 	op->mode = algt->mode;
 
 	return 0;
@@ -129,6 +135,9 @@ int sun4i_hash_import_sha1(struct ahash_request *areq, const void *in)
 	return 0;
 }
 
+#define SS_HASH_UPDATE 1
+#define SS_HASH_FINAL 2
+
 /*
  * sun4i_hash_update: update hash engine
  *
@@ -156,7 +165,7 @@ int sun4i_hash_import_sha1(struct ahash_request *areq, const void *in)
  * write remaining data in op->buf
  * final state op->len=56
  */
-int sun4i_hash_update(struct ahash_request *areq)
+static int sun4i_hash(struct ahash_request *areq)
 {
 	u32 v, ivmode = 0;
 	unsigned int i = 0;
@@ -167,8 +176,9 @@ int sun4i_hash_update(struct ahash_request *areq)
 	 */
 
 	struct sun4i_req_ctx *op = ahash_request_ctx(areq);
-	struct sun4i_ss_ctx *ss = op->ss;
 	struct crypto_ahash *tfm = crypto_ahash_reqtfm(areq);
+	struct sun4i_tfm_ctx *tfmctx = crypto_ahash_ctx(tfm);
+	struct sun4i_ss_ctx *ss = tfmctx->ss;
 	unsigned int in_i = 0; /* advancement in the current SG */
 	unsigned int end;
 	/*
@@ -180,22 +190,30 @@ int sun4i_hash_update(struct ahash_request *areq)
 	u32 spaces, rx_cnt = SS_RX_DEFAULT;
 	size_t copied = 0;
 	struct sg_mapping_iter mi;
+	unsigned int j = 0;
+	int zeros;
+	unsigned int index, padlen;
+	__be64 bits;
+	u32 bf[32];
+	u32 wb = 0;
+	unsigned int nwait, nbw = 0;
+	struct scatterlist *in_sg = areq->src;
 
 	dev_dbg(ss->dev, "%s %s bc=%llu len=%u mode=%x wl=%u h0=%0x",
 		__func__, crypto_tfm_alg_name(areq->base.tfm),
 		op->byte_count, areq->nbytes, op->mode,
 		op->len, op->hash[0]);
 
-	if (areq->nbytes == 0)
+	if (unlikely(areq->nbytes == 0) && (op->flags & SS_HASH_FINAL) == 0)
 		return 0;
 
 	/* protect against overflow */
-	if (areq->nbytes > UINT_MAX - op->len) {
+	if (unlikely(areq->nbytes > UINT_MAX - op->len)) {
 		dev_err(ss->dev, "Cannot process too large request\n");
 		return -EINVAL;
 	}
 
-	if (op->len + areq->nbytes < 64) {
+	if (op->len + areq->nbytes < 64 && (op->flags & SS_HASH_FINAL) == 0) {
 		/* linearize data to op->buf */
 		copied = sg_pcopy_to_buffer(areq->src, sg_nents(areq->src),
 					    op->buf + op->len, areq->nbytes, 0);
@@ -203,14 +221,6 @@ int sun4i_hash_update(struct ahash_request *areq)
 		return 0;
 	}
 
-	end = ((areq->nbytes + op->len) / 64) * 64 - op->len;
-
-	if (end > areq->nbytes || areq->nbytes - end > 63) {
-		dev_err(ss->dev, "ERROR: Bound error %u %u\n",
-			end, areq->nbytes);
-		return -EINVAL;
-	}
-
 	spin_lock_bh(&ss->slock);
 
 	/*
@@ -225,6 +235,34 @@ int sun4i_hash_update(struct ahash_request *areq)
 	/* Enable the device */
 	writel(op->mode | SS_ENABLED | ivmode, ss->base + SS_CTL);
 
+	if ((op->flags & SS_HASH_UPDATE) == 0)
+		goto hash_final;
+
+	/* start of handling data */
+	if ((op->flags & SS_HASH_FINAL) == 0) {
+		end = ((areq->nbytes + op->len) / 64) * 64 - op->len;
+
+		if (end > areq->nbytes || areq->nbytes - end > 63) {
+			dev_err(ss->dev, "ERROR: Bound error %u %u\n",
+				end, areq->nbytes);
+			err = -EINVAL;
+			goto release_ss;
+		}
+	} else {
+		/* Since we have the flag final, we can go up to modulo 4 */
+		end = ((areq->nbytes + op->len) / 4) * 4 - op->len;
+	}
+
+	/* TODO if SGlen % 4 and op->len == 0 then DMA */
+	i = 1;
+	while (in_sg && i == 1) {
+		if ((in_sg->length % 4) != 0)
+			i = 0;
+		in_sg = sg_next(in_sg);
+	}
+	if (i == 1 && op->len == 0)
+		dev_dbg(ss->dev, "We can DMA\n");
+
 	i = 0;
 	sg_miter_start(&mi, areq->src, sg_nents(areq->src),
 		       SG_MITER_FROM_SG | SG_MITER_ATOMIC);
@@ -285,7 +323,11 @@ int sun4i_hash_update(struct ahash_request *areq)
 			}
 		}
 	} while (i < end);
-	/* final linear */
+
+	/*
+	 * Now we have written to the device all that we can,
+	 * store the remaining bytes in op->buf
+	 */
 	if ((areq->nbytes - i) < 64) {
 		while (i < areq->nbytes && in_i < mi.length && op->len < 64) {
 			/* how many bytes we can read from current SG */
@@ -304,13 +346,21 @@ int sun4i_hash_update(struct ahash_request *areq)
 
 	sg_miter_stop(&mi);
 
+	/*
+	 * End of data process
+	 * Now if we have the flag final go to finalize part
+	 * If not, store the partial hash
+	 */
+	if ((op->flags & SS_HASH_FINAL) > 0)
+		goto hash_final;
+
 	writel(op->mode | SS_ENABLED | SS_DATA_END, ss->base + SS_CTL);
 	i = 0;
 	do {
 		v = readl(ss->base + SS_CTL);
 		i++;
 	} while (i < SS_TIMEOUT && (v & SS_DATA_END) > 0);
-	if (i >= SS_TIMEOUT) {
+	if (unlikely(i >= SS_TIMEOUT)) {
 		dev_err_ratelimited(ss->dev,
 				    "ERROR: hash end timeout %d>%d ctl=%x len=%u\n",
 				    i, SS_TIMEOUT, v, areq->nbytes);
@@ -318,56 +368,24 @@ int sun4i_hash_update(struct ahash_request *areq)
 		goto release_ss;
 	}
 
-	/* get the partial hash only if something was written */
 	for (i = 0; i < crypto_ahash_digestsize(tfm) / 4; i++)
 		op->hash[i] = readl(ss->base + SS_MD0 + i * 4);
 
-release_ss:
-	writel(0, ss->base + SS_CTL);
-	spin_unlock_bh(&ss->slock);
-	return err;
-}
+	goto release_ss;
 
 /*
- * sun4i_hash_final: finalize hashing operation
+ * hash_final: finalize hashing operation
  *
  * If we have some remaining bytes, we write them.
  * Then ask the SS for finalizing the hashing operation
  *
  * I do not check RX FIFO size in this function since the size is 32
  * after each enabling and this function neither write more than 32 words.
+ * If we come from the update part, we cannot have more than
+ * 3 remaining bytes to write and SS is fast enough to not care about it.
  */
-int sun4i_hash_final(struct ahash_request *areq)
-{
-	u32 v, ivmode = 0;
-	unsigned int i;
-	unsigned int j = 0;
-	int zeros, err = 0;
-	unsigned int index, padlen;
-	__be64 bits;
-	struct sun4i_req_ctx *op = ahash_request_ctx(areq);
-	struct sun4i_ss_ctx *ss = op->ss;
-	struct crypto_ahash *tfm = crypto_ahash_reqtfm(areq);
-	u32 bf[32];
-	u32 wb = 0;
-	unsigned int nwait, nbw = 0;
-
-	dev_dbg(ss->dev, "%s: byte=%llu len=%u mode=%x wl=%u h=%x",
-		__func__, op->byte_count, areq->nbytes, op->mode,
-		op->len, op->hash[0]);
 
-	spin_lock_bh(&ss->slock);
-
-	/*
-	 * if we have already written something,
-	 * restore the partial hash state
-	 */
-	if (op->byte_count > 0) {
-		ivmode = SS_IV_ARBITRARY;
-		for (i = 0; i < crypto_ahash_digestsize(tfm) / 4; i++)
-			writel(op->hash[i], ss->base + SS_IV0 + i * 4);
-	}
-	writel(op->mode | SS_ENABLED | ivmode, ss->base + SS_CTL);
+hash_final:
 
 	/* write the remaining words of the wait buffer */
 	if (op->len > 0) {
@@ -428,7 +446,7 @@ int sun4i_hash_final(struct ahash_request *areq)
 
 	/*
 	 * Wait for SS to finish the hash.
-	 * The timeout could happen only in case of bad overcloking
+	 * The timeout could happen only in case of bad overclocking
 	 * or driver bug.
 	 */
 	i = 0;
@@ -436,7 +454,7 @@ int sun4i_hash_final(struct ahash_request *areq)
 		v = readl(ss->base + SS_CTL);
 		i++;
 	} while (i < SS_TIMEOUT && (v & SS_DATA_END) > 0);
-	if (i >= SS_TIMEOUT) {
+	if (unlikely(i >= SS_TIMEOUT)) {
 		dev_err_ratelimited(ss->dev,
 				    "ERROR: hash end timeout %d>%d ctl=%x len=%u\n",
 				    i, SS_TIMEOUT, v, areq->nbytes);
@@ -463,30 +481,41 @@ release_ss:
 	return err;
 }
 
+int sun4i_hash_final(struct ahash_request *areq)
+{
+	struct sun4i_req_ctx *op = ahash_request_ctx(areq);
+
+	op->flags = SS_HASH_FINAL;
+	return sun4i_hash(areq);
+}
+
+int sun4i_hash_update(struct ahash_request *areq)
+{
+	struct sun4i_req_ctx *op = ahash_request_ctx(areq);
+
+	op->flags = SS_HASH_UPDATE;
+	return sun4i_hash(areq);
+}
+
 /* sun4i_hash_finup: finalize hashing operation after an update */
 int sun4i_hash_finup(struct ahash_request *areq)
 {
-	int err;
-
-	err = sun4i_hash_update(areq);
-	if (err != 0)
-		return err;
+	struct sun4i_req_ctx *op = ahash_request_ctx(areq);
 
-	return sun4i_hash_final(areq);
+	op->flags = SS_HASH_UPDATE | SS_HASH_FINAL;
+	return sun4i_hash(areq);
 }
 
 /* combo of init/update/final functions */
 int sun4i_hash_digest(struct ahash_request *areq)
 {
 	int err;
+	struct sun4i_req_ctx *op = ahash_request_ctx(areq);
 
 	err = sun4i_hash_init(areq);
 	if (err != 0)
 		return err;
 
-	err = sun4i_hash_update(areq);
-	if (err != 0)
-		return err;
-
-	return sun4i_hash_final(areq);
+	op->flags = SS_HASH_UPDATE | SS_HASH_FINAL;
+	return sun4i_hash(areq);
 }
diff --git a/drivers/crypto/sunxi-ss/sun4i-ss.h b/drivers/crypto/sunxi-ss/sun4i-ss.h
index 8e9c05f6e4d4..f04c0f8cf026 100644
--- a/drivers/crypto/sunxi-ss/sun4i-ss.h
+++ b/drivers/crypto/sunxi-ss/sun4i-ss.h
@@ -163,7 +163,7 @@ struct sun4i_req_ctx {
 	u32 hash[5]; /* for storing SS_IVx register */
 	char buf[64];
 	unsigned int len;
-	struct sun4i_ss_ctx *ss;
+	int flags;
 };
 
 int sun4i_hash_crainit(struct crypto_tfm *tfm);
diff --git a/drivers/crypto/vmx/Kconfig b/drivers/crypto/vmx/Kconfig
index a83ead109d5f..c3d524ea6998 100644
--- a/drivers/crypto/vmx/Kconfig
+++ b/drivers/crypto/vmx/Kconfig
@@ -1,6 +1,7 @@
 config CRYPTO_DEV_VMX_ENCRYPT
 	tristate "Encryption acceleration support on P8 CPU"
 	depends on CRYPTO_DEV_VMX
+	select CRYPTO_GHASH
 	default m
 	help
 	  Support for VMX cryptographic acceleration instructions on Power8 CPU.
diff --git a/drivers/crypto/vmx/ghash.c b/drivers/crypto/vmx/ghash.c
index 6c999cb01b80..27a94a119009 100644
--- a/drivers/crypto/vmx/ghash.c
+++ b/drivers/crypto/vmx/ghash.c
@@ -26,16 +26,13 @@
 #include <linux/hardirq.h>
 #include <asm/switch_to.h>
 #include <crypto/aes.h>
+#include <crypto/ghash.h>
 #include <crypto/scatterwalk.h>
 #include <crypto/internal/hash.h>
 #include <crypto/b128ops.h>
 
 #define IN_INTERRUPT in_interrupt()
 
-#define GHASH_BLOCK_SIZE (16)
-#define GHASH_DIGEST_SIZE (16)
-#define GHASH_KEY_LEN (16)
-
 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],
@@ -55,16 +52,11 @@ struct p8_ghash_desc_ctx {
 
 static int p8_ghash_init_tfm(struct crypto_tfm *tfm)
 {
-	const char *alg;
+	const char *alg = "ghash-generic";
 	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
@@ -78,10 +70,18 @@ static int p8_ghash_init_tfm(struct crypto_tfm *tfm)
 	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);
+	/* Check if the descsize defined in the algorithm is still enough. */
+	if (shash_tfm->descsize < sizeof(struct p8_ghash_desc_ctx)
+	    + crypto_shash_descsize(fallback)) {
+		printk(KERN_ERR
+		       "Desc size of the fallback implementation (%s) does not match the expected value: %lu vs %u\n",
+		       alg,
+		       shash_tfm->descsize - sizeof(struct p8_ghash_desc_ctx),
+		       crypto_shash_descsize(fallback));
+		return -EINVAL;
+	}
+	ctx->fallback = fallback;
 
 	return 0;
 }
@@ -113,7 +113,7 @@ static int p8_ghash_setkey(struct crypto_shash *tfm, const u8 *key,
 {
 	struct p8_ghash_ctx *ctx = crypto_tfm_ctx(crypto_shash_tfm(tfm));
 
-	if (keylen != GHASH_KEY_LEN)
+	if (keylen != GHASH_BLOCK_SIZE)
 		return -EINVAL;
 
 	preempt_disable();
@@ -211,7 +211,8 @@ struct shash_alg p8_ghash_alg = {
 	.update = p8_ghash_update,
 	.final = p8_ghash_final,
 	.setkey = p8_ghash_setkey,
-	.descsize = sizeof(struct p8_ghash_desc_ctx),
+	.descsize = sizeof(struct p8_ghash_desc_ctx)
+		+ sizeof(struct ghash_desc_ctx),
 	.base = {
 		 .cra_name = "ghash",
 		 .cra_driver_name = "p8_ghash",
diff --git a/drivers/dax/Kconfig b/drivers/dax/Kconfig
index cedab7572de3..daadd20aa936 100644
--- a/drivers/dax/Kconfig
+++ b/drivers/dax/Kconfig
@@ -23,4 +23,9 @@ config DEV_DAX_PMEM
 
 	  Say Y if unsure
 
+config NR_DEV_DAX
+	int "Maximum number of Device-DAX instances"
+	default 32768
+	range 256 2147483647
+
 endif
diff --git a/drivers/dax/dax.c b/drivers/dax/dax.c
index 29f600f2c447..0e499bfca41c 100644
--- a/drivers/dax/dax.c
+++ b/drivers/dax/dax.c
@@ -13,15 +13,25 @@
 #include <linux/pagemap.h>
 #include <linux/module.h>
 #include <linux/device.h>
+#include <linux/mount.h>
 #include <linux/pfn_t.h>
+#include <linux/hash.h>
+#include <linux/cdev.h>
 #include <linux/slab.h>
 #include <linux/dax.h>
 #include <linux/fs.h>
 #include <linux/mm.h>
+#include "dax.h"
 
-static int dax_major;
+static dev_t dax_devt;
 static struct class *dax_class;
 static DEFINE_IDA(dax_minor_ida);
+static int nr_dax = CONFIG_NR_DEV_DAX;
+module_param(nr_dax, int, S_IRUGO);
+static struct vfsmount *dax_mnt;
+static struct kmem_cache *dax_cache __read_mostly;
+static struct super_block *dax_superblock __read_mostly;
+MODULE_PARM_DESC(nr_dax, "max number of device-dax instances");
 
 /**
  * struct dax_region - mapping infrastructure for dax devices
@@ -48,7 +58,7 @@ struct dax_region {
  * struct dax_dev - subdivision of a dax region
  * @region - parent region
  * @dev - device backing the character device
- * @kref - enable this data to be tracked in filp->private_data
+ * @cdev - core chardev data
  * @alive - !alive + rcu grace period == no new mappings can be established
  * @id - child id in the region
  * @num_resources - number of physical address extents in this device
@@ -56,41 +66,139 @@ struct dax_region {
  */
 struct dax_dev {
 	struct dax_region *region;
-	struct device *dev;
-	struct kref kref;
+	struct inode *inode;
+	struct device dev;
+	struct cdev cdev;
 	bool alive;
 	int id;
 	int num_resources;
 	struct resource res[0];
 };
 
-static void dax_region_free(struct kref *kref)
+static struct inode *dax_alloc_inode(struct super_block *sb)
 {
-	struct dax_region *dax_region;
+	return kmem_cache_alloc(dax_cache, GFP_KERNEL);
+}
 
-	dax_region = container_of(kref, struct dax_region, kref);
-	kfree(dax_region);
+static void dax_i_callback(struct rcu_head *head)
+{
+	struct inode *inode = container_of(head, struct inode, i_rcu);
+
+	kmem_cache_free(dax_cache, inode);
 }
 
-void dax_region_put(struct dax_region *dax_region)
+static void dax_destroy_inode(struct inode *inode)
 {
-	kref_put(&dax_region->kref, dax_region_free);
+	call_rcu(&inode->i_rcu, dax_i_callback);
 }
-EXPORT_SYMBOL_GPL(dax_region_put);
 
-static void dax_dev_free(struct kref *kref)
+static const struct super_operations dax_sops = {
+	.statfs = simple_statfs,
+	.alloc_inode = dax_alloc_inode,
+	.destroy_inode = dax_destroy_inode,
+	.drop_inode = generic_delete_inode,
+};
+
+static struct dentry *dax_mount(struct file_system_type *fs_type,
+		int flags, const char *dev_name, void *data)
 {
-	struct dax_dev *dax_dev;
+	return mount_pseudo(fs_type, "dax:", &dax_sops, NULL, DAXFS_MAGIC);
+}
 
-	dax_dev = container_of(kref, struct dax_dev, kref);
-	dax_region_put(dax_dev->region);
-	kfree(dax_dev);
+static struct file_system_type dax_type = {
+	.name = "dax",
+	.mount = dax_mount,
+	.kill_sb = kill_anon_super,
+};
+
+static int dax_test(struct inode *inode, void *data)
+{
+	return inode->i_cdev == data;
+}
+
+static int dax_set(struct inode *inode, void *data)
+{
+	inode->i_cdev = data;
+	return 0;
+}
+
+static struct inode *dax_inode_get(struct cdev *cdev, dev_t devt)
+{
+	struct inode *inode;
+
+	inode = iget5_locked(dax_superblock, hash_32(devt + DAXFS_MAGIC, 31),
+			dax_test, dax_set, cdev);
+
+	if (!inode)
+		return NULL;
+
+	if (inode->i_state & I_NEW) {
+		inode->i_mode = S_IFCHR;
+		inode->i_flags = S_DAX;
+		inode->i_rdev = devt;
+		mapping_set_gfp_mask(&inode->i_data, GFP_USER);
+		unlock_new_inode(inode);
+	}
+	return inode;
+}
+
+static void init_once(void *inode)
+{
+	inode_init_once(inode);
+}
+
+static int dax_inode_init(void)
+{
+	int rc;
+
+	dax_cache = kmem_cache_create("dax_cache", sizeof(struct inode), 0,
+			(SLAB_HWCACHE_ALIGN|SLAB_RECLAIM_ACCOUNT|
+			 SLAB_MEM_SPREAD|SLAB_ACCOUNT),
+			init_once);
+	if (!dax_cache)
+		return -ENOMEM;
+
+	rc = register_filesystem(&dax_type);
+	if (rc)
+		goto err_register_fs;
+
+	dax_mnt = kern_mount(&dax_type);
+	if (IS_ERR(dax_mnt)) {
+		rc = PTR_ERR(dax_mnt);
+		goto err_mount;
+	}
+	dax_superblock = dax_mnt->mnt_sb;
+
+	return 0;
+
+ err_mount:
+	unregister_filesystem(&dax_type);
+ err_register_fs:
+	kmem_cache_destroy(dax_cache);
+
+	return rc;
 }
 
-static void dax_dev_put(struct dax_dev *dax_dev)
+static void dax_inode_exit(void)
+{
+	kern_unmount(dax_mnt);
+	unregister_filesystem(&dax_type);
+	kmem_cache_destroy(dax_cache);
+}
+
+static void dax_region_free(struct kref *kref)
+{
+	struct dax_region *dax_region;
+
+	dax_region = container_of(kref, struct dax_region, kref);
+	kfree(dax_region);
+}
+
+void dax_region_put(struct dax_region *dax_region)
 {
-	kref_put(&dax_dev->kref, dax_dev_free);
+	kref_put(&dax_region->kref, dax_region_free);
 }
+EXPORT_SYMBOL_GPL(dax_region_put);
 
 struct dax_region *alloc_dax_region(struct device *parent, int region_id,
 		struct resource *res, unsigned int align, void *addr,
@@ -98,8 +206,11 @@ struct dax_region *alloc_dax_region(struct device *parent, int region_id,
 {
 	struct dax_region *dax_region;
 
-	dax_region = kzalloc(sizeof(*dax_region), GFP_KERNEL);
+	if (!IS_ALIGNED(res->start, align)
+			|| !IS_ALIGNED(resource_size(res), align))
+		return NULL;
 
+	dax_region = kzalloc(sizeof(*dax_region), GFP_KERNEL);
 	if (!dax_region)
 		return NULL;
 
@@ -116,10 +227,15 @@ struct dax_region *alloc_dax_region(struct device *parent, int region_id,
 }
 EXPORT_SYMBOL_GPL(alloc_dax_region);
 
+static struct dax_dev *to_dax_dev(struct device *dev)
+{
+	return container_of(dev, struct dax_dev, dev);
+}
+
 static ssize_t size_show(struct device *dev,
 		struct device_attribute *attr, char *buf)
 {
-	struct dax_dev *dax_dev = dev_get_drvdata(dev);
+	struct dax_dev *dax_dev = to_dax_dev(dev);
 	unsigned long long size = 0;
 	int i;
 
@@ -144,180 +260,11 @@ static const struct attribute_group *dax_attribute_groups[] = {
 	NULL,
 };
 
-static void unregister_dax_dev(void *_dev)
-{
-	struct device *dev = _dev;
-	struct dax_dev *dax_dev = dev_get_drvdata(dev);
-	struct dax_region *dax_region = dax_dev->region;
-
-	dev_dbg(dev, "%s\n", __func__);
-
-	/*
-	 * Note, rcu is not protecting the liveness of dax_dev, rcu is
-	 * ensuring that any fault handlers that might have seen
-	 * dax_dev->alive == true, have completed.  Any fault handlers
-	 * that start after synchronize_rcu() has started will abort
-	 * upon seeing dax_dev->alive == false.
-	 */
-	dax_dev->alive = false;
-	synchronize_rcu();
-
-	get_device(dev);
-	device_unregister(dev);
-	ida_simple_remove(&dax_region->ida, dax_dev->id);
-	ida_simple_remove(&dax_minor_ida, MINOR(dev->devt));
-	put_device(dev);
-	dax_dev_put(dax_dev);
-}
-
-int devm_create_dax_dev(struct dax_region *dax_region, struct resource *res,
-		int count)
-{
-	struct device *parent = dax_region->dev;
-	struct dax_dev *dax_dev;
-	struct device *dev;
-	int rc, minor;
-	dev_t dev_t;
-
-	dax_dev = kzalloc(sizeof(*dax_dev) + sizeof(*res) * count, GFP_KERNEL);
-	if (!dax_dev)
-		return -ENOMEM;
-	memcpy(dax_dev->res, res, sizeof(*res) * count);
-	dax_dev->num_resources = count;
-	kref_init(&dax_dev->kref);
-	dax_dev->alive = true;
-	dax_dev->region = dax_region;
-	kref_get(&dax_region->kref);
-
-	dax_dev->id = ida_simple_get(&dax_region->ida, 0, 0, GFP_KERNEL);
-	if (dax_dev->id < 0) {
-		rc = dax_dev->id;
-		goto err_id;
-	}
-
-	minor = ida_simple_get(&dax_minor_ida, 0, 0, GFP_KERNEL);
-	if (minor < 0) {
-		rc = minor;
-		goto err_minor;
-	}
-
-	dev_t = MKDEV(dax_major, minor);
-	dev = device_create_with_groups(dax_class, parent, dev_t, dax_dev,
-			dax_attribute_groups, "dax%d.%d", dax_region->id,
-			dax_dev->id);
-	if (IS_ERR(dev)) {
-		rc = PTR_ERR(dev);
-		goto err_create;
-	}
-	dax_dev->dev = dev;
-
-	rc = devm_add_action_or_reset(dax_region->dev, unregister_dax_dev, dev);
-	if (rc)
-		return rc;
-
-	return 0;
-
- err_create:
-	ida_simple_remove(&dax_minor_ida, minor);
- err_minor:
-	ida_simple_remove(&dax_region->ida, dax_dev->id);
- err_id:
-	dax_dev_put(dax_dev);
-
-	return rc;
-}
-EXPORT_SYMBOL_GPL(devm_create_dax_dev);
-
-/* return an unmapped area aligned to the dax region specified alignment */
-static unsigned long dax_dev_get_unmapped_area(struct file *filp,
-		unsigned long addr, unsigned long len, unsigned long pgoff,
-		unsigned long flags)
-{
-	unsigned long off, off_end, off_align, len_align, addr_align, align;
-	struct dax_dev *dax_dev = filp ? filp->private_data : NULL;
-	struct dax_region *dax_region;
-
-	if (!dax_dev || addr)
-		goto out;
-
-	dax_region = dax_dev->region;
-	align = dax_region->align;
-	off = pgoff << PAGE_SHIFT;
-	off_end = off + len;
-	off_align = round_up(off, align);
-
-	if ((off_end <= off_align) || ((off_end - off_align) < align))
-		goto out;
-
-	len_align = len + align;
-	if ((off + len_align) < off)
-		goto out;
-
-	addr_align = current->mm->get_unmapped_area(filp, addr, len_align,
-			pgoff, flags);
-	if (!IS_ERR_VALUE(addr_align)) {
-		addr_align += (off - addr_align) & (align - 1);
-		return addr_align;
-	}
- out:
-	return current->mm->get_unmapped_area(filp, addr, len, pgoff, flags);
-}
-
-static int __match_devt(struct device *dev, const void *data)
-{
-	const dev_t *devt = data;
-
-	return dev->devt == *devt;
-}
-
-static struct device *dax_dev_find(dev_t dev_t)
-{
-	return class_find_device(dax_class, NULL, &dev_t, __match_devt);
-}
-
-static int dax_dev_open(struct inode *inode, struct file *filp)
-{
-	struct dax_dev *dax_dev = NULL;
-	struct device *dev;
-
-	dev = dax_dev_find(inode->i_rdev);
-	if (!dev)
-		return -ENXIO;
-
-	device_lock(dev);
-	dax_dev = dev_get_drvdata(dev);
-	if (dax_dev) {
-		dev_dbg(dev, "%s\n", __func__);
-		filp->private_data = dax_dev;
-		kref_get(&dax_dev->kref);
-		inode->i_flags = S_DAX;
-	}
-	device_unlock(dev);
-
-	if (!dax_dev) {
-		put_device(dev);
-		return -ENXIO;
-	}
-	return 0;
-}
-
-static int dax_dev_release(struct inode *inode, struct file *filp)
-{
-	struct dax_dev *dax_dev = filp->private_data;
-	struct device *dev = dax_dev->dev;
-
-	dev_dbg(dax_dev->dev, "%s\n", __func__);
-	dax_dev_put(dax_dev);
-	put_device(dev);
-
-	return 0;
-}
-
 static int check_vma(struct dax_dev *dax_dev, struct vm_area_struct *vma,
 		const char *func)
 {
 	struct dax_region *dax_region = dax_dev->region;
-	struct device *dev = dax_dev->dev;
+	struct device *dev = &dax_dev->dev;
 	unsigned long mask;
 
 	if (!dax_dev->alive)
@@ -382,7 +329,7 @@ static int __dax_dev_fault(struct dax_dev *dax_dev, struct vm_area_struct *vma,
 		struct vm_fault *vmf)
 {
 	unsigned long vaddr = (unsigned long) vmf->virtual_address;
-	struct device *dev = dax_dev->dev;
+	struct device *dev = &dax_dev->dev;
 	struct dax_region *dax_region;
 	int rc = VM_FAULT_SIGBUS;
 	phys_addr_t phys;
@@ -422,7 +369,7 @@ static int dax_dev_fault(struct vm_area_struct *vma, struct vm_fault *vmf)
 	struct file *filp = vma->vm_file;
 	struct dax_dev *dax_dev = filp->private_data;
 
-	dev_dbg(dax_dev->dev, "%s: %s: %s (%#lx - %#lx)\n", __func__,
+	dev_dbg(&dax_dev->dev, "%s: %s: %s (%#lx - %#lx)\n", __func__,
 			current->comm, (vmf->flags & FAULT_FLAG_WRITE)
 			? "write" : "read", vma->vm_start, vma->vm_end);
 	rcu_read_lock();
@@ -437,7 +384,7 @@ static int __dax_dev_pmd_fault(struct dax_dev *dax_dev,
 		unsigned int flags)
 {
 	unsigned long pmd_addr = addr & PMD_MASK;
-	struct device *dev = dax_dev->dev;
+	struct device *dev = &dax_dev->dev;
 	struct dax_region *dax_region;
 	phys_addr_t phys;
 	pgoff_t pgoff;
@@ -479,7 +426,7 @@ static int dax_dev_pmd_fault(struct vm_area_struct *vma, unsigned long addr,
 	struct file *filp = vma->vm_file;
 	struct dax_dev *dax_dev = filp->private_data;
 
-	dev_dbg(dax_dev->dev, "%s: %s: %s (%#lx - %#lx)\n", __func__,
+	dev_dbg(&dax_dev->dev, "%s: %s: %s (%#lx - %#lx)\n", __func__,
 			current->comm, (flags & FAULT_FLAG_WRITE)
 			? "write" : "read", vma->vm_start, vma->vm_end);
 
@@ -490,81 +437,257 @@ static int dax_dev_pmd_fault(struct vm_area_struct *vma, unsigned long addr,
 	return rc;
 }
 
-static void dax_dev_vm_open(struct vm_area_struct *vma)
-{
-	struct file *filp = vma->vm_file;
-	struct dax_dev *dax_dev = filp->private_data;
-
-	dev_dbg(dax_dev->dev, "%s\n", __func__);
-	kref_get(&dax_dev->kref);
-}
-
-static void dax_dev_vm_close(struct vm_area_struct *vma)
-{
-	struct file *filp = vma->vm_file;
-	struct dax_dev *dax_dev = filp->private_data;
-
-	dev_dbg(dax_dev->dev, "%s\n", __func__);
-	dax_dev_put(dax_dev);
-}
-
 static const struct vm_operations_struct dax_dev_vm_ops = {
 	.fault = dax_dev_fault,
 	.pmd_fault = dax_dev_pmd_fault,
-	.open = dax_dev_vm_open,
-	.close = dax_dev_vm_close,
 };
 
-static int dax_dev_mmap(struct file *filp, struct vm_area_struct *vma)
+static int dax_mmap(struct file *filp, struct vm_area_struct *vma)
 {
 	struct dax_dev *dax_dev = filp->private_data;
 	int rc;
 
-	dev_dbg(dax_dev->dev, "%s\n", __func__);
+	dev_dbg(&dax_dev->dev, "%s\n", __func__);
 
 	rc = check_vma(dax_dev, vma, __func__);
 	if (rc)
 		return rc;
 
-	kref_get(&dax_dev->kref);
 	vma->vm_ops = &dax_dev_vm_ops;
 	vma->vm_flags |= VM_MIXEDMAP | VM_HUGEPAGE;
 	return 0;
+}
+
+/* return an unmapped area aligned to the dax region specified alignment */
+static unsigned long dax_get_unmapped_area(struct file *filp,
+		unsigned long addr, unsigned long len, unsigned long pgoff,
+		unsigned long flags)
+{
+	unsigned long off, off_end, off_align, len_align, addr_align, align;
+	struct dax_dev *dax_dev = filp ? filp->private_data : NULL;
+	struct dax_region *dax_region;
+
+	if (!dax_dev || addr)
+		goto out;
+
+	dax_region = dax_dev->region;
+	align = dax_region->align;
+	off = pgoff << PAGE_SHIFT;
+	off_end = off + len;
+	off_align = round_up(off, align);
+
+	if ((off_end <= off_align) || ((off_end - off_align) < align))
+		goto out;
+
+	len_align = len + align;
+	if ((off + len_align) < off)
+		goto out;
 
+	addr_align = current->mm->get_unmapped_area(filp, addr, len_align,
+			pgoff, flags);
+	if (!IS_ERR_VALUE(addr_align)) {
+		addr_align += (off - addr_align) & (align - 1);
+		return addr_align;
+	}
+ out:
+	return current->mm->get_unmapped_area(filp, addr, len, pgoff, flags);
+}
+
+static int dax_open(struct inode *inode, struct file *filp)
+{
+	struct dax_dev *dax_dev;
+
+	dax_dev = container_of(inode->i_cdev, struct dax_dev, cdev);
+	dev_dbg(&dax_dev->dev, "%s\n", __func__);
+	inode->i_mapping = dax_dev->inode->i_mapping;
+	inode->i_mapping->host = dax_dev->inode;
+	filp->f_mapping = inode->i_mapping;
+	filp->private_data = dax_dev;
+	inode->i_flags = S_DAX;
+
+	return 0;
+}
+
+static int dax_release(struct inode *inode, struct file *filp)
+{
+	struct dax_dev *dax_dev = filp->private_data;
+
+	dev_dbg(&dax_dev->dev, "%s\n", __func__);
+	return 0;
 }
 
 static const struct file_operations dax_fops = {
 	.llseek = noop_llseek,
 	.owner = THIS_MODULE,
-	.open = dax_dev_open,
-	.release = dax_dev_release,
-	.get_unmapped_area = dax_dev_get_unmapped_area,
-	.mmap = dax_dev_mmap,
+	.open = dax_open,
+	.release = dax_release,
+	.get_unmapped_area = dax_get_unmapped_area,
+	.mmap = dax_mmap,
 };
 
+static void dax_dev_release(struct device *dev)
+{
+	struct dax_dev *dax_dev = to_dax_dev(dev);
+	struct dax_region *dax_region = dax_dev->region;
+
+	ida_simple_remove(&dax_region->ida, dax_dev->id);
+	ida_simple_remove(&dax_minor_ida, MINOR(dev->devt));
+	dax_region_put(dax_region);
+	iput(dax_dev->inode);
+	kfree(dax_dev);
+}
+
+static void unregister_dax_dev(void *dev)
+{
+	struct dax_dev *dax_dev = to_dax_dev(dev);
+	struct cdev *cdev = &dax_dev->cdev;
+
+	dev_dbg(dev, "%s\n", __func__);
+
+	/*
+	 * Note, rcu is not protecting the liveness of dax_dev, rcu is
+	 * ensuring that any fault handlers that might have seen
+	 * dax_dev->alive == true, have completed.  Any fault handlers
+	 * that start after synchronize_rcu() has started will abort
+	 * upon seeing dax_dev->alive == false.
+	 */
+	dax_dev->alive = false;
+	synchronize_rcu();
+	unmap_mapping_range(dax_dev->inode->i_mapping, 0, 0, 1);
+	cdev_del(cdev);
+	device_unregister(dev);
+}
+
+struct dax_dev *devm_create_dax_dev(struct dax_region *dax_region,
+		struct resource *res, int count)
+{
+	struct device *parent = dax_region->dev;
+	struct dax_dev *dax_dev;
+	int rc = 0, minor, i;
+	struct device *dev;
+	struct cdev *cdev;
+	dev_t dev_t;
+
+	dax_dev = kzalloc(sizeof(*dax_dev) + sizeof(*res) * count, GFP_KERNEL);
+	if (!dax_dev)
+		return ERR_PTR(-ENOMEM);
+
+	for (i = 0; i < count; i++) {
+		if (!IS_ALIGNED(res[i].start, dax_region->align)
+				|| !IS_ALIGNED(resource_size(&res[i]),
+					dax_region->align)) {
+			rc = -EINVAL;
+			break;
+		}
+		dax_dev->res[i].start = res[i].start;
+		dax_dev->res[i].end = res[i].end;
+	}
+
+	if (i < count)
+		goto err_id;
+
+	dax_dev->id = ida_simple_get(&dax_region->ida, 0, 0, GFP_KERNEL);
+	if (dax_dev->id < 0) {
+		rc = dax_dev->id;
+		goto err_id;
+	}
+
+	minor = ida_simple_get(&dax_minor_ida, 0, 0, GFP_KERNEL);
+	if (minor < 0) {
+		rc = minor;
+		goto err_minor;
+	}
+
+	dev_t = MKDEV(MAJOR(dax_devt), minor);
+	dev = &dax_dev->dev;
+	dax_dev->inode = dax_inode_get(&dax_dev->cdev, dev_t);
+	if (!dax_dev->inode) {
+		rc = -ENOMEM;
+		goto err_inode;
+	}
+
+	/* device_initialize() so cdev can reference kobj parent */
+	device_initialize(dev);
+
+	cdev = &dax_dev->cdev;
+	cdev_init(cdev, &dax_fops);
+	cdev->owner = parent->driver->owner;
+	cdev->kobj.parent = &dev->kobj;
+	rc = cdev_add(&dax_dev->cdev, dev_t, 1);
+	if (rc)
+		goto err_cdev;
+
+	/* from here on we're committed to teardown via dax_dev_release() */
+	dax_dev->num_resources = count;
+	dax_dev->alive = true;
+	dax_dev->region = dax_region;
+	kref_get(&dax_region->kref);
+
+	dev->devt = dev_t;
+	dev->class = dax_class;
+	dev->parent = parent;
+	dev->groups = dax_attribute_groups;
+	dev->release = dax_dev_release;
+	dev_set_name(dev, "dax%d.%d", dax_region->id, dax_dev->id);
+	rc = device_add(dev);
+	if (rc) {
+		put_device(dev);
+		return ERR_PTR(rc);
+	}
+
+	rc = devm_add_action_or_reset(dax_region->dev, unregister_dax_dev, dev);
+	if (rc)
+		return ERR_PTR(rc);
+
+	return dax_dev;
+
+ err_cdev:
+	iput(dax_dev->inode);
+ err_inode:
+	ida_simple_remove(&dax_minor_ida, minor);
+ err_minor:
+	ida_simple_remove(&dax_region->ida, dax_dev->id);
+ err_id:
+	kfree(dax_dev);
+
+	return ERR_PTR(rc);
+}
+EXPORT_SYMBOL_GPL(devm_create_dax_dev);
+
 static int __init dax_init(void)
 {
 	int rc;
 
-	rc = register_chrdev(0, "dax", &dax_fops);
-	if (rc < 0)
+	rc = dax_inode_init();
+	if (rc)
 		return rc;
-	dax_major = rc;
+
+	nr_dax = max(nr_dax, 256);
+	rc = alloc_chrdev_region(&dax_devt, 0, nr_dax, "dax");
+	if (rc)
+		goto err_chrdev;
 
 	dax_class = class_create(THIS_MODULE, "dax");
 	if (IS_ERR(dax_class)) {
-		unregister_chrdev(dax_major, "dax");
-		return PTR_ERR(dax_class);
+		rc = PTR_ERR(dax_class);
+		goto err_class;
 	}
 
 	return 0;
+
+ err_class:
+	unregister_chrdev_region(dax_devt, nr_dax);
+ err_chrdev:
+	dax_inode_exit();
+	return rc;
 }
 
 static void __exit dax_exit(void)
 {
 	class_destroy(dax_class);
-	unregister_chrdev(dax_major, "dax");
+	unregister_chrdev_region(dax_devt, nr_dax);
 	ida_destroy(&dax_minor_ida);
+	dax_inode_exit();
 }
 
 MODULE_AUTHOR("Intel Corporation");
diff --git a/drivers/dax/dax.h b/drivers/dax/dax.h
index d8b8f1f25054..ddd829ab58c0 100644
--- a/drivers/dax/dax.h
+++ b/drivers/dax/dax.h
@@ -13,12 +13,13 @@
 #ifndef __DAX_H__
 #define __DAX_H__
 struct device;
+struct dax_dev;
 struct resource;
 struct dax_region;
 void dax_region_put(struct dax_region *dax_region);
 struct dax_region *alloc_dax_region(struct device *parent,
 		int region_id, struct resource *res, unsigned int align,
 		void *addr, unsigned long flags);
-int devm_create_dax_dev(struct dax_region *dax_region, struct resource *res,
-		int count);
+struct dax_dev *devm_create_dax_dev(struct dax_region *dax_region,
+		struct resource *res, int count);
 #endif /* __DAX_H__ */
diff --git a/drivers/dax/pmem.c b/drivers/dax/pmem.c
index 1f01e98c83c7..9630d8837ba9 100644
--- a/drivers/dax/pmem.c
+++ b/drivers/dax/pmem.c
@@ -24,7 +24,7 @@ struct dax_pmem {
 	struct completion cmp;
 };
 
-struct dax_pmem *to_dax_pmem(struct percpu_ref *ref)
+static struct dax_pmem *to_dax_pmem(struct percpu_ref *ref)
 {
 	return container_of(ref, struct dax_pmem, ref);
 }
@@ -61,6 +61,7 @@ static int dax_pmem_probe(struct device *dev)
 	int rc;
 	void *addr;
 	struct resource res;
+	struct dax_dev *dax_dev;
 	struct nd_pfn_sb *pfn_sb;
 	struct dax_pmem *dax_pmem;
 	struct nd_region *nd_region;
@@ -126,12 +127,12 @@ static int dax_pmem_probe(struct device *dev)
 		return -ENOMEM;
 
 	/* TODO: support for subdividing a dax region... */
-	rc = devm_create_dax_dev(dax_region, &res, 1);
+	dax_dev = devm_create_dax_dev(dax_region, &res, 1);
 
 	/* child dax_dev instances now own the lifetime of the dax_region */
 	dax_region_put(dax_region);
 
-	return rc;
+	return PTR_ERR_OR_ZERO(dax_dev);
 }
 
 static struct nd_device_driver dax_pmem_driver = {
diff --git a/drivers/devfreq/Kconfig b/drivers/devfreq/Kconfig
index a5be56ec57f2..41254e702f1e 100644
--- a/drivers/devfreq/Kconfig
+++ b/drivers/devfreq/Kconfig
@@ -76,7 +76,7 @@ comment "DEVFREQ Drivers"
 
 config ARM_EXYNOS_BUS_DEVFREQ
 	tristate "ARM EXYNOS Generic Memory Bus DEVFREQ Driver"
-	depends on ARCH_EXYNOS
+	depends on ARCH_EXYNOS || COMPILE_TEST
 	select DEVFREQ_GOV_SIMPLE_ONDEMAND
 	select DEVFREQ_GOV_PASSIVE
 	select DEVFREQ_EVENT_EXYNOS_PPMU
@@ -91,14 +91,26 @@ config ARM_EXYNOS_BUS_DEVFREQ
 	  This does not yet operate with optimal voltages.
 
 config ARM_TEGRA_DEVFREQ
-       tristate "Tegra DEVFREQ Driver"
-       depends on ARCH_TEGRA_124_SOC
-       select DEVFREQ_GOV_SIMPLE_ONDEMAND
-       select PM_OPP
-       help
-         This adds the DEVFREQ driver for the Tegra family of SoCs.
-         It reads ACTMON counters of memory controllers and adjusts the
-         operating frequencies and voltages with OPP support.
+	tristate "Tegra DEVFREQ Driver"
+	depends on ARCH_TEGRA_124_SOC
+	select DEVFREQ_GOV_SIMPLE_ONDEMAND
+	select PM_OPP
+	help
+	  This adds the DEVFREQ driver for the Tegra family of SoCs.
+	  It reads ACTMON counters of memory controllers and adjusts the
+	  operating frequencies and voltages with OPP support.
+
+config ARM_RK3399_DMC_DEVFREQ
+	tristate "ARM RK3399 DMC DEVFREQ Driver"
+	depends on ARCH_ROCKCHIP
+	select DEVFREQ_EVENT_ROCKCHIP_DFI
+	select DEVFREQ_GOV_SIMPLE_ONDEMAND
+	select PM_DEVFREQ_EVENT
+	select PM_OPP
+	help
+          This adds the DEVFREQ driver for the RK3399 DMC(Dynamic Memory Controller).
+          It sets the frequency for the memory controller and reads the usage counts
+          from hardware.
 
 source "drivers/devfreq/event/Kconfig"
 
diff --git a/drivers/devfreq/Makefile b/drivers/devfreq/Makefile
index 09f11d9d40d5..fbff40a508a4 100644
--- a/drivers/devfreq/Makefile
+++ b/drivers/devfreq/Makefile
@@ -8,6 +8,7 @@ obj-$(CONFIG_DEVFREQ_GOV_PASSIVE)	+= governor_passive.o
 
 # DEVFREQ Drivers
 obj-$(CONFIG_ARM_EXYNOS_BUS_DEVFREQ)	+= exynos-bus.o
+obj-$(CONFIG_ARM_RK3399_DMC_DEVFREQ)	+= rk3399_dmc.o
 obj-$(CONFIG_ARM_TEGRA_DEVFREQ)		+= tegra-devfreq.o
 
 # DEVFREQ Event Drivers
diff --git a/drivers/devfreq/devfreq.c b/drivers/devfreq/devfreq.c
index 478006b7764a..bf3ea7603a58 100644
--- a/drivers/devfreq/devfreq.c
+++ b/drivers/devfreq/devfreq.c
@@ -137,6 +137,10 @@ static int devfreq_update_status(struct devfreq *devfreq, unsigned long freq)
 
 	cur_time = jiffies;
 
+	/* Immediately exit if previous_freq is not initialized yet. */
+	if (!devfreq->previous_freq)
+		goto out;
+
 	prev_lev = devfreq_get_freq_level(devfreq, devfreq->previous_freq);
 	if (prev_lev < 0) {
 		ret = prev_lev;
@@ -594,17 +598,19 @@ struct devfreq *devfreq_add_device(struct device *dev,
 	if (devfreq->governor)
 		err = devfreq->governor->event_handler(devfreq,
 					DEVFREQ_GOV_START, NULL);
-	mutex_unlock(&devfreq_list_lock);
 	if (err) {
 		dev_err(dev, "%s: Unable to start governor for the device\n",
 			__func__);
 		goto err_init;
 	}
+	mutex_unlock(&devfreq_list_lock);
 
 	return devfreq;
 
 err_init:
 	list_del(&devfreq->node);
+	mutex_unlock(&devfreq_list_lock);
+
 	device_unregister(&devfreq->dev);
 err_out:
 	return ERR_PTR(err);
diff --git a/drivers/devfreq/event/Kconfig b/drivers/devfreq/event/Kconfig
index eb6f74a2b6b9..cd949800eed9 100644
--- a/drivers/devfreq/event/Kconfig
+++ b/drivers/devfreq/event/Kconfig
@@ -15,19 +15,27 @@ if PM_DEVFREQ_EVENT
 
 config DEVFREQ_EVENT_EXYNOS_NOCP
 	tristate "EXYNOS NoC (Network On Chip) Probe DEVFREQ event Driver"
-	depends on ARCH_EXYNOS
+	depends on ARCH_EXYNOS || COMPILE_TEST
 	select PM_OPP
+	select REGMAP_MMIO
 	help
 	  This add the devfreq-event driver for Exynos SoC. It provides NoC
 	  (Network on Chip) Probe counters to measure the bandwidth of AXI bus.
 
 config DEVFREQ_EVENT_EXYNOS_PPMU
 	tristate "EXYNOS PPMU (Platform Performance Monitoring Unit) DEVFREQ event Driver"
-	depends on ARCH_EXYNOS
+	depends on ARCH_EXYNOS || COMPILE_TEST
 	select PM_OPP
 	help
 	  This add the devfreq-event driver for Exynos SoC. It provides PPMU
 	  (Platform Performance Monitoring Unit) counters to estimate the
 	  utilization of each module.
 
+config DEVFREQ_EVENT_ROCKCHIP_DFI
+	tristate "ROCKCHIP DFI DEVFREQ event Driver"
+	depends on ARCH_ROCKCHIP
+	help
+	  This add the devfreq-event driver for Rockchip SoC. It provides DFI
+	  (DDR Monitor Module) driver to count ddr load.
+
 endif # PM_DEVFREQ_EVENT
diff --git a/drivers/devfreq/event/Makefile b/drivers/devfreq/event/Makefile
index 3d6afd352253..dda7090a47c6 100644
--- a/drivers/devfreq/event/Makefile
+++ b/drivers/devfreq/event/Makefile
@@ -2,3 +2,4 @@
 
 obj-$(CONFIG_DEVFREQ_EVENT_EXYNOS_NOCP) += exynos-nocp.o
 obj-$(CONFIG_DEVFREQ_EVENT_EXYNOS_PPMU) += exynos-ppmu.o
+obj-$(CONFIG_DEVFREQ_EVENT_ROCKCHIP_DFI) += rockchip-dfi.o
diff --git a/drivers/devfreq/event/exynos-nocp.c b/drivers/devfreq/event/exynos-nocp.c
index a5841403bde8..49e712aca0c1 100644
--- a/drivers/devfreq/event/exynos-nocp.c
+++ b/drivers/devfreq/event/exynos-nocp.c
@@ -176,9 +176,6 @@ static int exynos_nocp_get_event(struct devfreq_event_dev *edev,
 	return 0;
 
 out:
-	edata->load_count = 0;
-	edata->total_count = 0;
-
 	dev_err(nocp->dev, "Failed to read the counter of NoC probe device\n");
 
 	return ret;
diff --git a/drivers/devfreq/event/exynos-ppmu.c b/drivers/devfreq/event/exynos-ppmu.c
index 845bf25fb9fb..f55cf0eb2a66 100644
--- a/drivers/devfreq/event/exynos-ppmu.c
+++ b/drivers/devfreq/event/exynos-ppmu.c
@@ -406,8 +406,6 @@ static int of_get_devfreq_events(struct device_node *np,
 		of_property_read_string(node, "event-name", &desc[j].name);
 
 		j++;
-
-		of_node_put(node);
 	}
 	info->desc = desc;
 
diff --git a/drivers/devfreq/event/rockchip-dfi.c b/drivers/devfreq/event/rockchip-dfi.c
new file mode 100644
index 000000000000..43fcc5a7f515
--- /dev/null
+++ b/drivers/devfreq/event/rockchip-dfi.c
@@ -0,0 +1,256 @@
+/*
+ * Copyright (c) 2016, Fuzhou Rockchip Electronics Co., Ltd
+ * Author: Lin Huang <hl@rock-chips.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.
+ */
+
+#include <linux/clk.h>
+#include <linux/devfreq-event.h>
+#include <linux/kernel.h>
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/mfd/syscon.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+#include <linux/slab.h>
+#include <linux/list.h>
+#include <linux/of.h>
+
+#define RK3399_DMC_NUM_CH	2
+
+/* DDRMON_CTRL */
+#define DDRMON_CTRL	0x04
+#define CLR_DDRMON_CTRL	(0x1f0000 << 0)
+#define LPDDR4_EN	(0x10001 << 4)
+#define HARDWARE_EN	(0x10001 << 3)
+#define LPDDR3_EN	(0x10001 << 2)
+#define SOFTWARE_EN	(0x10001 << 1)
+#define SOFTWARE_DIS	(0x10000 << 1)
+#define TIME_CNT_EN	(0x10001 << 0)
+
+#define DDRMON_CH0_COUNT_NUM		0x28
+#define DDRMON_CH0_DFI_ACCESS_NUM	0x2c
+#define DDRMON_CH1_COUNT_NUM		0x3c
+#define DDRMON_CH1_DFI_ACCESS_NUM	0x40
+
+/* pmu grf */
+#define PMUGRF_OS_REG2	0x308
+#define DDRTYPE_SHIFT	13
+#define DDRTYPE_MASK	7
+
+enum {
+	DDR3 = 3,
+	LPDDR3 = 6,
+	LPDDR4 = 7,
+	UNUSED = 0xFF
+};
+
+struct dmc_usage {
+	u32 access;
+	u32 total;
+};
+
+/*
+ * The dfi controller can monitor DDR load. It has an upper and lower threshold
+ * for the operating points. Whenever the usage leaves these bounds an event is
+ * generated to indicate the DDR frequency should be changed.
+ */
+struct rockchip_dfi {
+	struct devfreq_event_dev *edev;
+	struct devfreq_event_desc *desc;
+	struct dmc_usage ch_usage[RK3399_DMC_NUM_CH];
+	struct device *dev;
+	void __iomem *regs;
+	struct regmap *regmap_pmu;
+	struct clk *clk;
+};
+
+static void rockchip_dfi_start_hardware_counter(struct devfreq_event_dev *edev)
+{
+	struct rockchip_dfi *info = devfreq_event_get_drvdata(edev);
+	void __iomem *dfi_regs = info->regs;
+	u32 val;
+	u32 ddr_type;
+
+	/* get ddr type */
+	regmap_read(info->regmap_pmu, PMUGRF_OS_REG2, &val);
+	ddr_type = (val >> DDRTYPE_SHIFT) & DDRTYPE_MASK;
+
+	/* clear DDRMON_CTRL setting */
+	writel_relaxed(CLR_DDRMON_CTRL, dfi_regs + DDRMON_CTRL);
+
+	/* set ddr type to dfi */
+	if (ddr_type == LPDDR3)
+		writel_relaxed(LPDDR3_EN, dfi_regs + DDRMON_CTRL);
+	else if (ddr_type == LPDDR4)
+		writel_relaxed(LPDDR4_EN, dfi_regs + DDRMON_CTRL);
+
+	/* enable count, use software mode */
+	writel_relaxed(SOFTWARE_EN, dfi_regs + DDRMON_CTRL);
+}
+
+static void rockchip_dfi_stop_hardware_counter(struct devfreq_event_dev *edev)
+{
+	struct rockchip_dfi *info = devfreq_event_get_drvdata(edev);
+	void __iomem *dfi_regs = info->regs;
+
+	writel_relaxed(SOFTWARE_DIS, dfi_regs + DDRMON_CTRL);
+}
+
+static int rockchip_dfi_get_busier_ch(struct devfreq_event_dev *edev)
+{
+	struct rockchip_dfi *info = devfreq_event_get_drvdata(edev);
+	u32 tmp, max = 0;
+	u32 i, busier_ch = 0;
+	void __iomem *dfi_regs = info->regs;
+
+	rockchip_dfi_stop_hardware_counter(edev);
+
+	/* Find out which channel is busier */
+	for (i = 0; i < RK3399_DMC_NUM_CH; i++) {
+		info->ch_usage[i].access = readl_relaxed(dfi_regs +
+				DDRMON_CH0_DFI_ACCESS_NUM + i * 20) * 4;
+		info->ch_usage[i].total = readl_relaxed(dfi_regs +
+				DDRMON_CH0_COUNT_NUM + i * 20);
+		tmp = info->ch_usage[i].access;
+		if (tmp > max) {
+			busier_ch = i;
+			max = tmp;
+		}
+	}
+	rockchip_dfi_start_hardware_counter(edev);
+
+	return busier_ch;
+}
+
+static int rockchip_dfi_disable(struct devfreq_event_dev *edev)
+{
+	struct rockchip_dfi *info = devfreq_event_get_drvdata(edev);
+
+	rockchip_dfi_stop_hardware_counter(edev);
+	clk_disable_unprepare(info->clk);
+
+	return 0;
+}
+
+static int rockchip_dfi_enable(struct devfreq_event_dev *edev)
+{
+	struct rockchip_dfi *info = devfreq_event_get_drvdata(edev);
+	int ret;
+
+	ret = clk_prepare_enable(info->clk);
+	if (ret) {
+		dev_err(&edev->dev, "failed to enable dfi clk: %d\n", ret);
+		return ret;
+	}
+
+	rockchip_dfi_start_hardware_counter(edev);
+	return 0;
+}
+
+static int rockchip_dfi_set_event(struct devfreq_event_dev *edev)
+{
+	return 0;
+}
+
+static int rockchip_dfi_get_event(struct devfreq_event_dev *edev,
+				  struct devfreq_event_data *edata)
+{
+	struct rockchip_dfi *info = devfreq_event_get_drvdata(edev);
+	int busier_ch;
+
+	busier_ch = rockchip_dfi_get_busier_ch(edev);
+
+	edata->load_count = info->ch_usage[busier_ch].access;
+	edata->total_count = info->ch_usage[busier_ch].total;
+
+	return 0;
+}
+
+static const struct devfreq_event_ops rockchip_dfi_ops = {
+	.disable = rockchip_dfi_disable,
+	.enable = rockchip_dfi_enable,
+	.get_event = rockchip_dfi_get_event,
+	.set_event = rockchip_dfi_set_event,
+};
+
+static const struct of_device_id rockchip_dfi_id_match[] = {
+	{ .compatible = "rockchip,rk3399-dfi" },
+	{ },
+};
+
+static int rockchip_dfi_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct rockchip_dfi *data;
+	struct resource *res;
+	struct devfreq_event_desc *desc;
+	struct device_node *np = pdev->dev.of_node, *node;
+
+	data = devm_kzalloc(dev, sizeof(struct rockchip_dfi), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	data->regs = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(data->regs))
+		return PTR_ERR(data->regs);
+
+	data->clk = devm_clk_get(dev, "pclk_ddr_mon");
+	if (IS_ERR(data->clk)) {
+		dev_err(dev, "Cannot get the clk dmc_clk\n");
+		return PTR_ERR(data->clk);
+	};
+
+	/* try to find the optional reference to the pmu syscon */
+	node = of_parse_phandle(np, "rockchip,pmu", 0);
+	if (node) {
+		data->regmap_pmu = syscon_node_to_regmap(node);
+		if (IS_ERR(data->regmap_pmu))
+			return PTR_ERR(data->regmap_pmu);
+	}
+	data->dev = dev;
+
+	desc = devm_kzalloc(dev, sizeof(*desc), GFP_KERNEL);
+	if (!desc)
+		return -ENOMEM;
+
+	desc->ops = &rockchip_dfi_ops;
+	desc->driver_data = data;
+	desc->name = np->name;
+	data->desc = desc;
+
+	data->edev = devm_devfreq_event_add_edev(&pdev->dev, desc);
+	if (IS_ERR(data->edev)) {
+		dev_err(&pdev->dev,
+			"failed to add devfreq-event device\n");
+		return PTR_ERR(data->edev);
+	}
+
+	platform_set_drvdata(pdev, data);
+
+	return 0;
+}
+
+static struct platform_driver rockchip_dfi_driver = {
+	.probe	= rockchip_dfi_probe,
+	.driver = {
+		.name	= "rockchip-dfi",
+		.of_match_table = rockchip_dfi_id_match,
+	},
+};
+module_platform_driver(rockchip_dfi_driver);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Lin Huang <hl@rock-chips.com>");
+MODULE_DESCRIPTION("Rockchip DFI driver");
diff --git a/drivers/devfreq/rk3399_dmc.c b/drivers/devfreq/rk3399_dmc.c
new file mode 100644
index 000000000000..e24b73d66659
--- /dev/null
+++ b/drivers/devfreq/rk3399_dmc.c
@@ -0,0 +1,470 @@
+/*
+ * Copyright (c) 2016, Fuzhou Rockchip Electronics Co., Ltd.
+ * Author: Lin Huang <hl@rock-chips.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.
+ */
+
+#include <linux/arm-smccc.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/devfreq.h>
+#include <linux/devfreq-event.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/pm_opp.h>
+#include <linux/regulator/consumer.h>
+#include <linux/rwsem.h>
+#include <linux/suspend.h>
+
+#include <soc/rockchip/rockchip_sip.h>
+
+struct dram_timing {
+	unsigned int ddr3_speed_bin;
+	unsigned int pd_idle;
+	unsigned int sr_idle;
+	unsigned int sr_mc_gate_idle;
+	unsigned int srpd_lite_idle;
+	unsigned int standby_idle;
+	unsigned int auto_pd_dis_freq;
+	unsigned int dram_dll_dis_freq;
+	unsigned int phy_dll_dis_freq;
+	unsigned int ddr3_odt_dis_freq;
+	unsigned int ddr3_drv;
+	unsigned int ddr3_odt;
+	unsigned int phy_ddr3_ca_drv;
+	unsigned int phy_ddr3_dq_drv;
+	unsigned int phy_ddr3_odt;
+	unsigned int lpddr3_odt_dis_freq;
+	unsigned int lpddr3_drv;
+	unsigned int lpddr3_odt;
+	unsigned int phy_lpddr3_ca_drv;
+	unsigned int phy_lpddr3_dq_drv;
+	unsigned int phy_lpddr3_odt;
+	unsigned int lpddr4_odt_dis_freq;
+	unsigned int lpddr4_drv;
+	unsigned int lpddr4_dq_odt;
+	unsigned int lpddr4_ca_odt;
+	unsigned int phy_lpddr4_ca_drv;
+	unsigned int phy_lpddr4_ck_cs_drv;
+	unsigned int phy_lpddr4_dq_drv;
+	unsigned int phy_lpddr4_odt;
+};
+
+struct rk3399_dmcfreq {
+	struct device *dev;
+	struct devfreq *devfreq;
+	struct devfreq_simple_ondemand_data ondemand_data;
+	struct clk *dmc_clk;
+	struct devfreq_event_dev *edev;
+	struct mutex lock;
+	struct dram_timing timing;
+
+	/*
+	 * DDR Converser of Frequency (DCF) is used to implement DDR frequency
+	 * conversion without the participation of CPU, we will implement and
+	 * control it in arm trust firmware.
+	 */
+	wait_queue_head_t	wait_dcf_queue;
+	int irq;
+	int wait_dcf_flag;
+	struct regulator *vdd_center;
+	unsigned long rate, target_rate;
+	unsigned long volt, target_volt;
+	struct dev_pm_opp *curr_opp;
+};
+
+static int rk3399_dmcfreq_target(struct device *dev, unsigned long *freq,
+				 u32 flags)
+{
+	struct rk3399_dmcfreq *dmcfreq = dev_get_drvdata(dev);
+	struct dev_pm_opp *opp;
+	unsigned long old_clk_rate = dmcfreq->rate;
+	unsigned long target_volt, target_rate;
+	int err;
+
+	rcu_read_lock();
+	opp = devfreq_recommended_opp(dev, freq, flags);
+	if (IS_ERR(opp)) {
+		rcu_read_unlock();
+		return PTR_ERR(opp);
+	}
+
+	target_rate = dev_pm_opp_get_freq(opp);
+	target_volt = dev_pm_opp_get_voltage(opp);
+
+	dmcfreq->rate = dev_pm_opp_get_freq(dmcfreq->curr_opp);
+	dmcfreq->volt = dev_pm_opp_get_voltage(dmcfreq->curr_opp);
+
+	rcu_read_unlock();
+
+	if (dmcfreq->rate == target_rate)
+		return 0;
+
+	mutex_lock(&dmcfreq->lock);
+
+	/*
+	 * If frequency scaling from low to high, adjust voltage first.
+	 * If frequency scaling from high to low, adjust frequency first.
+	 */
+	if (old_clk_rate < target_rate) {
+		err = regulator_set_voltage(dmcfreq->vdd_center, target_volt,
+					    target_volt);
+		if (err) {
+			dev_err(dev, "Cannot to set voltage %lu uV\n",
+				target_volt);
+			goto out;
+		}
+	}
+	dmcfreq->wait_dcf_flag = 1;
+
+	err = clk_set_rate(dmcfreq->dmc_clk, target_rate);
+	if (err) {
+		dev_err(dev, "Cannot to set frequency %lu (%d)\n",
+			target_rate, err);
+		regulator_set_voltage(dmcfreq->vdd_center, dmcfreq->volt,
+				      dmcfreq->volt);
+		goto out;
+	}
+
+	/*
+	 * Wait until bcf irq happen, it means freq scaling finish in
+	 * arm trust firmware, use 100ms as timeout time.
+	 */
+	if (!wait_event_timeout(dmcfreq->wait_dcf_queue,
+				!dmcfreq->wait_dcf_flag, HZ / 10))
+		dev_warn(dev, "Timeout waiting for dcf interrupt\n");
+
+	/*
+	 * Check the dpll rate,
+	 * There only two result we will get,
+	 * 1. Ddr frequency scaling fail, we still get the old rate.
+	 * 2. Ddr frequency scaling sucessful, we get the rate we set.
+	 */
+	dmcfreq->rate = clk_get_rate(dmcfreq->dmc_clk);
+
+	/* If get the incorrect rate, set voltage to old value. */
+	if (dmcfreq->rate != target_rate) {
+		dev_err(dev, "Get wrong ddr frequency, Request frequency %lu,\
+			Current frequency %lu\n", target_rate, dmcfreq->rate);
+		regulator_set_voltage(dmcfreq->vdd_center, dmcfreq->volt,
+				      dmcfreq->volt);
+		goto out;
+	} else if (old_clk_rate > target_rate)
+		err = regulator_set_voltage(dmcfreq->vdd_center, target_volt,
+					    target_volt);
+	if (err)
+		dev_err(dev, "Cannot to set vol %lu uV\n", target_volt);
+
+	dmcfreq->curr_opp = opp;
+out:
+	mutex_unlock(&dmcfreq->lock);
+	return err;
+}
+
+static int rk3399_dmcfreq_get_dev_status(struct device *dev,
+					 struct devfreq_dev_status *stat)
+{
+	struct rk3399_dmcfreq *dmcfreq = dev_get_drvdata(dev);
+	struct devfreq_event_data edata;
+	int ret = 0;
+
+	ret = devfreq_event_get_event(dmcfreq->edev, &edata);
+	if (ret < 0)
+		return ret;
+
+	stat->current_frequency = dmcfreq->rate;
+	stat->busy_time = edata.load_count;
+	stat->total_time = edata.total_count;
+
+	return ret;
+}
+
+static int rk3399_dmcfreq_get_cur_freq(struct device *dev, unsigned long *freq)
+{
+	struct rk3399_dmcfreq *dmcfreq = dev_get_drvdata(dev);
+
+	*freq = dmcfreq->rate;
+
+	return 0;
+}
+
+static struct devfreq_dev_profile rk3399_devfreq_dmc_profile = {
+	.polling_ms	= 200,
+	.target		= rk3399_dmcfreq_target,
+	.get_dev_status	= rk3399_dmcfreq_get_dev_status,
+	.get_cur_freq	= rk3399_dmcfreq_get_cur_freq,
+};
+
+static __maybe_unused int rk3399_dmcfreq_suspend(struct device *dev)
+{
+	struct rk3399_dmcfreq *dmcfreq = dev_get_drvdata(dev);
+	int ret = 0;
+
+	ret = devfreq_event_disable_edev(dmcfreq->edev);
+	if (ret < 0) {
+		dev_err(dev, "failed to disable the devfreq-event devices\n");
+		return ret;
+	}
+
+	ret = devfreq_suspend_device(dmcfreq->devfreq);
+	if (ret < 0) {
+		dev_err(dev, "failed to suspend the devfreq devices\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static __maybe_unused int rk3399_dmcfreq_resume(struct device *dev)
+{
+	struct rk3399_dmcfreq *dmcfreq = dev_get_drvdata(dev);
+	int ret = 0;
+
+	ret = devfreq_event_enable_edev(dmcfreq->edev);
+	if (ret < 0) {
+		dev_err(dev, "failed to enable the devfreq-event devices\n");
+		return ret;
+	}
+
+	ret = devfreq_resume_device(dmcfreq->devfreq);
+	if (ret < 0) {
+		dev_err(dev, "failed to resume the devfreq devices\n");
+		return ret;
+	}
+	return ret;
+}
+
+static SIMPLE_DEV_PM_OPS(rk3399_dmcfreq_pm, rk3399_dmcfreq_suspend,
+			 rk3399_dmcfreq_resume);
+
+static irqreturn_t rk3399_dmc_irq(int irq, void *dev_id)
+{
+	struct rk3399_dmcfreq *dmcfreq = dev_id;
+	struct arm_smccc_res res;
+
+	dmcfreq->wait_dcf_flag = 0;
+	wake_up(&dmcfreq->wait_dcf_queue);
+
+	/* Clear the DCF interrupt */
+	arm_smccc_smc(ROCKCHIP_SIP_DRAM_FREQ, 0, 0,
+		      ROCKCHIP_SIP_CONFIG_DRAM_CLR_IRQ,
+		      0, 0, 0, 0, &res);
+
+	return IRQ_HANDLED;
+}
+
+static int of_get_ddr_timings(struct dram_timing *timing,
+			      struct device_node *np)
+{
+	int ret = 0;
+
+	ret = of_property_read_u32(np, "rockchip,ddr3_speed_bin",
+				   &timing->ddr3_speed_bin);
+	ret |= of_property_read_u32(np, "rockchip,pd_idle",
+				    &timing->pd_idle);
+	ret |= of_property_read_u32(np, "rockchip,sr_idle",
+				    &timing->sr_idle);
+	ret |= of_property_read_u32(np, "rockchip,sr_mc_gate_idle",
+				    &timing->sr_mc_gate_idle);
+	ret |= of_property_read_u32(np, "rockchip,srpd_lite_idle",
+				    &timing->srpd_lite_idle);
+	ret |= of_property_read_u32(np, "rockchip,standby_idle",
+				    &timing->standby_idle);
+	ret |= of_property_read_u32(np, "rockchip,auto_pd_dis_freq",
+				    &timing->auto_pd_dis_freq);
+	ret |= of_property_read_u32(np, "rockchip,dram_dll_dis_freq",
+				    &timing->dram_dll_dis_freq);
+	ret |= of_property_read_u32(np, "rockchip,phy_dll_dis_freq",
+				    &timing->phy_dll_dis_freq);
+	ret |= of_property_read_u32(np, "rockchip,ddr3_odt_dis_freq",
+				    &timing->ddr3_odt_dis_freq);
+	ret |= of_property_read_u32(np, "rockchip,ddr3_drv",
+				    &timing->ddr3_drv);
+	ret |= of_property_read_u32(np, "rockchip,ddr3_odt",
+				    &timing->ddr3_odt);
+	ret |= of_property_read_u32(np, "rockchip,phy_ddr3_ca_drv",
+				    &timing->phy_ddr3_ca_drv);
+	ret |= of_property_read_u32(np, "rockchip,phy_ddr3_dq_drv",
+				    &timing->phy_ddr3_dq_drv);
+	ret |= of_property_read_u32(np, "rockchip,phy_ddr3_odt",
+				    &timing->phy_ddr3_odt);
+	ret |= of_property_read_u32(np, "rockchip,lpddr3_odt_dis_freq",
+				    &timing->lpddr3_odt_dis_freq);
+	ret |= of_property_read_u32(np, "rockchip,lpddr3_drv",
+				    &timing->lpddr3_drv);
+	ret |= of_property_read_u32(np, "rockchip,lpddr3_odt",
+				    &timing->lpddr3_odt);
+	ret |= of_property_read_u32(np, "rockchip,phy_lpddr3_ca_drv",
+				    &timing->phy_lpddr3_ca_drv);
+	ret |= of_property_read_u32(np, "rockchip,phy_lpddr3_dq_drv",
+				    &timing->phy_lpddr3_dq_drv);
+	ret |= of_property_read_u32(np, "rockchip,phy_lpddr3_odt",
+				    &timing->phy_lpddr3_odt);
+	ret |= of_property_read_u32(np, "rockchip,lpddr4_odt_dis_freq",
+				    &timing->lpddr4_odt_dis_freq);
+	ret |= of_property_read_u32(np, "rockchip,lpddr4_drv",
+				    &timing->lpddr4_drv);
+	ret |= of_property_read_u32(np, "rockchip,lpddr4_dq_odt",
+				    &timing->lpddr4_dq_odt);
+	ret |= of_property_read_u32(np, "rockchip,lpddr4_ca_odt",
+				    &timing->lpddr4_ca_odt);
+	ret |= of_property_read_u32(np, "rockchip,phy_lpddr4_ca_drv",
+				    &timing->phy_lpddr4_ca_drv);
+	ret |= of_property_read_u32(np, "rockchip,phy_lpddr4_ck_cs_drv",
+				    &timing->phy_lpddr4_ck_cs_drv);
+	ret |= of_property_read_u32(np, "rockchip,phy_lpddr4_dq_drv",
+				    &timing->phy_lpddr4_dq_drv);
+	ret |= of_property_read_u32(np, "rockchip,phy_lpddr4_odt",
+				    &timing->phy_lpddr4_odt);
+
+	return ret;
+}
+
+static int rk3399_dmcfreq_probe(struct platform_device *pdev)
+{
+	struct arm_smccc_res res;
+	struct device *dev = &pdev->dev;
+	struct device_node *np = pdev->dev.of_node;
+	struct rk3399_dmcfreq *data;
+	int ret, irq, index, size;
+	uint32_t *timing;
+	struct dev_pm_opp *opp;
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0) {
+		dev_err(&pdev->dev, "Cannot get the dmc interrupt resource\n");
+		return -EINVAL;
+	}
+	data = devm_kzalloc(dev, sizeof(struct rk3399_dmcfreq), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	mutex_init(&data->lock);
+
+	data->vdd_center = devm_regulator_get(dev, "center");
+	if (IS_ERR(data->vdd_center)) {
+		dev_err(dev, "Cannot get the regulator \"center\"\n");
+		return PTR_ERR(data->vdd_center);
+	}
+
+	data->dmc_clk = devm_clk_get(dev, "dmc_clk");
+	if (IS_ERR(data->dmc_clk)) {
+		dev_err(dev, "Cannot get the clk dmc_clk\n");
+		return PTR_ERR(data->dmc_clk);
+	};
+
+	data->irq = irq;
+	ret = devm_request_irq(dev, irq, rk3399_dmc_irq, 0,
+			       dev_name(dev), data);
+	if (ret) {
+		dev_err(dev, "Failed to request dmc irq: %d\n", ret);
+		return ret;
+	}
+
+	init_waitqueue_head(&data->wait_dcf_queue);
+	data->wait_dcf_flag = 0;
+
+	data->edev = devfreq_event_get_edev_by_phandle(dev, 0);
+	if (IS_ERR(data->edev))
+		return -EPROBE_DEFER;
+
+	ret = devfreq_event_enable_edev(data->edev);
+	if (ret < 0) {
+		dev_err(dev, "failed to enable devfreq-event devices\n");
+		return ret;
+	}
+
+	/*
+	 * Get dram timing and pass it to arm trust firmware,
+	 * the dram drvier in arm trust firmware will get these
+	 * timing and to do dram initial.
+	 */
+	if (!of_get_ddr_timings(&data->timing, np)) {
+		timing = &data->timing.ddr3_speed_bin;
+		size = sizeof(struct dram_timing) / 4;
+		for (index = 0; index < size; index++) {
+			arm_smccc_smc(ROCKCHIP_SIP_DRAM_FREQ, *timing++, index,
+				      ROCKCHIP_SIP_CONFIG_DRAM_SET_PARAM,
+				      0, 0, 0, 0, &res);
+			if (res.a0) {
+				dev_err(dev, "Failed to set dram param: %ld\n",
+					res.a0);
+				return -EINVAL;
+			}
+		}
+	}
+
+	arm_smccc_smc(ROCKCHIP_SIP_DRAM_FREQ, 0, 0,
+		      ROCKCHIP_SIP_CONFIG_DRAM_INIT,
+		      0, 0, 0, 0, &res);
+
+	/*
+	 * We add a devfreq driver to our parent since it has a device tree node
+	 * with operating points.
+	 */
+	if (dev_pm_opp_of_add_table(dev)) {
+		dev_err(dev, "Invalid operating-points in device tree.\n");
+		rcu_read_unlock();
+		return -EINVAL;
+	}
+
+	of_property_read_u32(np, "upthreshold",
+			     &data->ondemand_data.upthreshold);
+	of_property_read_u32(np, "downdifferential",
+			     &data->ondemand_data.downdifferential);
+
+	data->rate = clk_get_rate(data->dmc_clk);
+
+	rcu_read_lock();
+	opp = devfreq_recommended_opp(dev, &data->rate, 0);
+	if (IS_ERR(opp)) {
+		rcu_read_unlock();
+		return PTR_ERR(opp);
+	}
+	rcu_read_unlock();
+	data->curr_opp = opp;
+
+	rk3399_devfreq_dmc_profile.initial_freq = data->rate;
+
+	data->devfreq = devfreq_add_device(dev,
+					   &rk3399_devfreq_dmc_profile,
+					   "simple_ondemand",
+					   &data->ondemand_data);
+	if (IS_ERR(data->devfreq))
+		return PTR_ERR(data->devfreq);
+	devm_devfreq_register_opp_notifier(dev, data->devfreq);
+
+	data->dev = dev;
+	platform_set_drvdata(pdev, data);
+
+	return 0;
+}
+
+static const struct of_device_id rk3399dmc_devfreq_of_match[] = {
+	{ .compatible = "rockchip,rk3399-dmc" },
+	{ },
+};
+
+static struct platform_driver rk3399_dmcfreq_driver = {
+	.probe	= rk3399_dmcfreq_probe,
+	.driver = {
+		.name	= "rk3399-dmc-freq",
+		.pm	= &rk3399_dmcfreq_pm,
+		.of_match_table = rk3399dmc_devfreq_of_match,
+	},
+};
+module_platform_driver(rk3399_dmcfreq_driver);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Lin Huang <hl@rock-chips.com>");
+MODULE_DESCRIPTION("RK3399 dmcfreq driver with devfreq framework");
diff --git a/drivers/dma-buf/Kconfig b/drivers/dma-buf/Kconfig
index 25bcfa0b474f..2585821b24ab 100644
--- a/drivers/dma-buf/Kconfig
+++ b/drivers/dma-buf/Kconfig
@@ -17,4 +17,17 @@ config SYNC_FILE
 	  Files fds, to the DRM driver for example. More details at
 	  Documentation/sync_file.txt.
 
+config SW_SYNC
+	bool "Sync File Validation Framework"
+	default n
+	depends on SYNC_FILE
+	depends on DEBUG_FS
+	---help---
+	  A sync object driver that uses a 32bit counter to coordinate
+	  synchronization.  Useful when there is no hardware primitive backing
+	  the synchronization.
+
+	  WARNING: improper use of this can result in deadlocking kernel
+	  drivers from userspace. Intended for test and debug only.
+
 endmenu
diff --git a/drivers/dma-buf/Makefile b/drivers/dma-buf/Makefile
index f353db213a81..210a10bfad2b 100644
--- a/drivers/dma-buf/Makefile
+++ b/drivers/dma-buf/Makefile
@@ -1,2 +1,3 @@
 obj-y := dma-buf.o fence.o reservation.o seqno-fence.o fence-array.o
 obj-$(CONFIG_SYNC_FILE)		+= sync_file.o
+obj-$(CONFIG_SW_SYNC)		+= sw_sync.o sync_debug.o
diff --git a/drivers/staging/android/sw_sync.c b/drivers/dma-buf/sw_sync.c
index 115c9174705f..62e8e6dc7953 100644
--- a/drivers/staging/android/sw_sync.c
+++ b/drivers/dma-buf/sw_sync.c
@@ -1,5 +1,5 @@
 /*
- * drivers/dma-buf/sw_sync.c
+ * Sync File validation framework
  *
  * Copyright (C) 2012 Google, Inc.
  *
@@ -23,8 +23,38 @@
 #include "sync_debug.h"
 
 #define CREATE_TRACE_POINTS
-#include "trace/sync.h"
+#include "sync_trace.h"
 
+/*
+ * SW SYNC validation framework
+ *
+ * A sync object driver that uses a 32bit counter to coordinate
+ * synchronization.  Useful when there is no hardware primitive backing
+ * the synchronization.
+ *
+ * To start the framework just open:
+ *
+ * <debugfs>/sync/sw_sync
+ *
+ * That will create a sync timeline, all fences created under this timeline
+ * file descriptor will belong to the this timeline.
+ *
+ * The 'sw_sync' file can be opened many times as to create different
+ * timelines.
+ *
+ * Fences can be created with SW_SYNC_IOC_CREATE_FENCE ioctl with struct
+ * sw_sync_ioctl_create_fence as parameter.
+ *
+ * To increment the timeline counter, SW_SYNC_IOC_INC ioctl should be used
+ * with the increment as u32. This will update the last signaled value
+ * from the timeline and signal any fence that has a seqno smaller or equal
+ * to it.
+ *
+ * struct sw_sync_ioctl_create_fence
+ * @value:	the seqno to initialise the fence with
+ * @name:	the name of the new sync point
+ * @fence:	return the fd of the new sync_file with the created fence
+ */
 struct sw_sync_create_fence_data {
 	__u32	value;
 	char	name[32];
@@ -35,6 +65,7 @@ struct sw_sync_create_fence_data {
 
 #define SW_SYNC_IOC_CREATE_FENCE	_IOWR(SW_SYNC_IOC_MAGIC, 0,\
 		struct sw_sync_create_fence_data)
+
 #define SW_SYNC_IOC_INC			_IOW(SW_SYNC_IOC_MAGIC, 1, __u32)
 
 static const struct fence_ops timeline_fence_ops;
@@ -176,7 +207,7 @@ static void timeline_fence_release(struct fence *fence)
 
 	spin_lock_irqsave(fence->lock, flags);
 	list_del(&pt->child_list);
-	if (WARN_ON_ONCE(!list_empty(&pt->active_list)))
+	if (!list_empty(&pt->active_list))
 		list_del(&pt->active_list);
 	spin_unlock_irqrestore(fence->lock, flags);
 
diff --git a/drivers/staging/android/sync_debug.c b/drivers/dma-buf/sync_debug.c
index 961a94bf156c..2dd4c3db6caa 100644
--- a/drivers/staging/android/sync_debug.c
+++ b/drivers/dma-buf/sync_debug.c
@@ -1,5 +1,5 @@
 /*
- * drivers/base/sync.c
+ * Sync File validation framework and debug information
  *
  * Copyright (C) 2012 Google, Inc.
  *
diff --git a/drivers/staging/android/sync_debug.h b/drivers/dma-buf/sync_debug.h
index fab66396d421..d269aa6783aa 100644
--- a/drivers/staging/android/sync_debug.h
+++ b/drivers/dma-buf/sync_debug.h
@@ -1,5 +1,5 @@
 /*
- * include/linux/sync.h
+ * Sync File validation framework and debug infomation
  *
  * Copyright (C) 2012 Google, Inc.
  *
diff --git a/drivers/staging/android/trace/sync.h b/drivers/dma-buf/sync_trace.h
index 6b5ce9640ddd..d13d59ff1b85 100644
--- a/drivers/staging/android/trace/sync.h
+++ b/drivers/dma-buf/sync_trace.h
@@ -1,11 +1,11 @@
 #undef TRACE_SYSTEM
-#define TRACE_INCLUDE_PATH ../../drivers/staging/android/trace
-#define TRACE_SYSTEM sync
+#define TRACE_INCLUDE_PATH ../../drivers/dma-buf
+#define TRACE_SYSTEM sync_trace
 
 #if !defined(_TRACE_SYNC_H) || defined(TRACE_HEADER_MULTI_READ)
 #define _TRACE_SYNC_H
 
-#include "../sync_debug.h"
+#include "sync_debug.h"
 #include <linux/tracepoint.h>
 
 TRACE_EVENT(sync_timeline,
diff --git a/drivers/dma/Kconfig b/drivers/dma/Kconfig
index 739f797b40d9..af63a6bcf564 100644
--- a/drivers/dma/Kconfig
+++ b/drivers/dma/Kconfig
@@ -102,7 +102,7 @@ config AXI_DMAC
 config COH901318
 	bool "ST-Ericsson COH901318 DMA support"
 	select DMA_ENGINE
-	depends on ARCH_U300
+	depends on ARCH_U300 || COMPILE_TEST
 	help
 	  Enable support for ST-Ericsson COH 901 318 DMA.
 
@@ -114,13 +114,13 @@ config DMA_BCM2835
 
 config DMA_JZ4740
 	tristate "JZ4740 DMA support"
-	depends on MACH_JZ4740
+	depends on MACH_JZ4740 || COMPILE_TEST
 	select DMA_ENGINE
 	select DMA_VIRTUAL_CHANNELS
 
 config DMA_JZ4780
 	tristate "JZ4780 DMA support"
-	depends on MACH_JZ4780
+	depends on MACH_JZ4780 || COMPILE_TEST
 	select DMA_ENGINE
 	select DMA_VIRTUAL_CHANNELS
 	help
@@ -130,14 +130,14 @@ config DMA_JZ4780
 
 config DMA_OMAP
 	tristate "OMAP DMA support"
-	depends on ARCH_OMAP
+	depends on ARCH_OMAP || COMPILE_TEST
 	select DMA_ENGINE
 	select DMA_VIRTUAL_CHANNELS
-	select TI_DMA_CROSSBAR if SOC_DRA7XX
+	select TI_DMA_CROSSBAR if (SOC_DRA7XX || COMPILE_TEST)
 
 config DMA_SA11X0
 	tristate "SA-11x0 DMA support"
-	depends on ARCH_SA1100
+	depends on ARCH_SA1100 || COMPILE_TEST
 	select DMA_ENGINE
 	select DMA_VIRTUAL_CHANNELS
 	help
@@ -150,7 +150,6 @@ config DMA_SUN4I
 	depends on MACH_SUN4I || MACH_SUN5I || MACH_SUN7I
 	default (MACH_SUN4I || MACH_SUN5I || MACH_SUN7I)
 	select DMA_ENGINE
-	select DMA_OF
 	select DMA_VIRTUAL_CHANNELS
 	help
 	  Enable support for the DMA controller present in the sun4i,
@@ -167,7 +166,7 @@ config DMA_SUN6I
 
 config EP93XX_DMA
 	bool "Cirrus Logic EP93xx DMA support"
-	depends on ARCH_EP93XX
+	depends on ARCH_EP93XX || COMPILE_TEST
 	select DMA_ENGINE
 	help
 	  Enable support for the Cirrus Logic EP93xx M2P/M2M DMA controller.
@@ -279,7 +278,7 @@ config INTEL_MIC_X100_DMA
 
 config K3_DMA
 	tristate "Hisilicon K3 DMA support"
-	depends on ARCH_HI3xxx
+	depends on ARCH_HI3xxx || ARCH_HISI || COMPILE_TEST
 	select DMA_ENGINE
 	select DMA_VIRTUAL_CHANNELS
 	help
@@ -297,16 +296,16 @@ config LPC18XX_DMAMUX
 
 config MMP_PDMA
 	bool "MMP PDMA support"
-	depends on (ARCH_MMP || ARCH_PXA)
+	depends on ARCH_MMP || ARCH_PXA || COMPILE_TEST
 	select DMA_ENGINE
 	help
 	  Support the MMP PDMA engine for PXA and MMP platform.
 
 config MMP_TDMA
 	bool "MMP Two-Channel DMA support"
-	depends on ARCH_MMP
+	depends on ARCH_MMP || COMPILE_TEST
 	select DMA_ENGINE
-	select MMP_SRAM
+	select MMP_SRAM if ARCH_MMP
 	help
 	  Support the MMP Two-Channel DMA engine.
 	  This engine used for MMP Audio DMA and pxa910 SQU.
@@ -316,7 +315,6 @@ config MOXART_DMA
 	tristate "MOXART DMA support"
 	depends on ARCH_MOXART
 	select DMA_ENGINE
-	select DMA_OF
 	select DMA_VIRTUAL_CHANNELS
 	help
 	  Enable support for the MOXA ART SoC DMA controller.
@@ -439,9 +437,8 @@ config STE_DMA40
 
 config STM32_DMA
 	bool "STMicroelectronics STM32 DMA support"
-	depends on ARCH_STM32
+	depends on ARCH_STM32 || COMPILE_TEST
 	select DMA_ENGINE
-	select DMA_OF
 	select DMA_VIRTUAL_CHANNELS
 	help
 	  Enable support for the on-chip DMA controller on STMicroelectronics
@@ -451,7 +448,7 @@ config STM32_DMA
 
 config S3C24XX_DMAC
 	bool "Samsung S3C24XX DMA support"
-	depends on ARCH_S3C24XX
+	depends on ARCH_S3C24XX || COMPILE_TEST
 	select DMA_ENGINE
 	select DMA_VIRTUAL_CHANNELS
 	help
@@ -483,10 +480,9 @@ config TEGRA20_APB_DMA
 
 config TEGRA210_ADMA
 	bool "NVIDIA Tegra210 ADMA support"
-	depends on ARCH_TEGRA_210_SOC
+	depends on (ARCH_TEGRA_210_SOC || COMPILE_TEST) && PM_CLK
 	select DMA_ENGINE
 	select DMA_VIRTUAL_CHANNELS
-	select PM_CLK
 	help
 	  Support for the NVIDIA Tegra210 ADMA controller driver. The
 	  DMA controller has multiple DMA channels and is used to service
@@ -497,7 +493,7 @@ config TEGRA210_ADMA
 
 config TIMB_DMA
 	tristate "Timberdale FPGA DMA support"
-	depends on MFD_TIMBERDALE
+	depends on MFD_TIMBERDALE || COMPILE_TEST
 	select DMA_ENGINE
 	help
 	  Enable support for the Timberdale FPGA DMA engine.
@@ -515,10 +511,10 @@ config TI_DMA_CROSSBAR
 
 config TI_EDMA
 	bool "TI EDMA support"
-	depends on ARCH_DAVINCI || ARCH_OMAP || ARCH_KEYSTONE
+	depends on ARCH_DAVINCI || ARCH_OMAP || ARCH_KEYSTONE || COMPILE_TEST
 	select DMA_ENGINE
 	select DMA_VIRTUAL_CHANNELS
-	select TI_DMA_CROSSBAR if ARCH_OMAP
+	select TI_DMA_CROSSBAR if (ARCH_OMAP || COMPILE_TEST)
 	default n
 	help
 	  Enable support for the TI EDMA controller. This DMA
@@ -561,7 +557,7 @@ config XILINX_ZYNQMP_DMA
 
 config ZX_DMA
 	tristate "ZTE ZX296702 DMA support"
-	depends on ARCH_ZX
+	depends on ARCH_ZX || COMPILE_TEST
 	select DMA_ENGINE
 	select DMA_VIRTUAL_CHANNELS
 	help
diff --git a/drivers/dma/at_hdmac.c b/drivers/dma/at_hdmac.c
index 53d22eb73b56..a4c8f80db29d 100644
--- a/drivers/dma/at_hdmac.c
+++ b/drivers/dma/at_hdmac.c
@@ -473,15 +473,11 @@ atc_chain_complete(struct at_dma_chan *atchan, struct at_desc *desc)
 	/* for cyclic transfers,
 	 * no need to replay callback function while stopping */
 	if (!atc_chan_is_cyclic(atchan)) {
-		dma_async_tx_callback	callback = txd->callback;
-		void			*param = txd->callback_param;
-
 		/*
 		 * The API requires that no submissions are done from a
 		 * callback, so we don't need to drop the lock here
 		 */
-		if (callback)
-			callback(param);
+		dmaengine_desc_get_callback_invoke(txd, NULL);
 	}
 
 	dma_run_dependencies(txd);
@@ -598,15 +594,12 @@ static void atc_handle_cyclic(struct at_dma_chan *atchan)
 {
 	struct at_desc			*first = atc_first_active(atchan);
 	struct dma_async_tx_descriptor	*txd = &first->txd;
-	dma_async_tx_callback		callback = txd->callback;
-	void				*param = txd->callback_param;
 
 	dev_vdbg(chan2dev(&atchan->chan_common),
 			"new cyclic period llp 0x%08x\n",
 			channel_readl(atchan, DSCR));
 
-	if (callback)
-		callback(param);
+	dmaengine_desc_get_callback_invoke(txd, NULL);
 }
 
 /*--  IRQ & Tasklet  ---------------------------------------------------*/
diff --git a/drivers/dma/at_xdmac.c b/drivers/dma/at_xdmac.c
index 832cbd647145..b7d7f2d443a1 100644
--- a/drivers/dma/at_xdmac.c
+++ b/drivers/dma/at_xdmac.c
@@ -1572,8 +1572,8 @@ static void at_xdmac_handle_cyclic(struct at_xdmac_chan *atchan)
 	desc = list_first_entry(&atchan->xfers_list, struct at_xdmac_desc, xfer_node);
 	txd = &desc->tx_dma_desc;
 
-	if (txd->callback && (txd->flags & DMA_PREP_INTERRUPT))
-		txd->callback(txd->callback_param);
+	if (txd->flags & DMA_PREP_INTERRUPT)
+		dmaengine_desc_get_callback_invoke(txd, NULL);
 }
 
 static void at_xdmac_tasklet(unsigned long data)
@@ -1616,8 +1616,8 @@ static void at_xdmac_tasklet(unsigned long data)
 
 		if (!at_xdmac_chan_is_cyclic(atchan)) {
 			dma_cookie_complete(txd);
-			if (txd->callback && (txd->flags & DMA_PREP_INTERRUPT))
-				txd->callback(txd->callback_param);
+			if (txd->flags & DMA_PREP_INTERRUPT)
+				dmaengine_desc_get_callback_invoke(txd, NULL);
 		}
 
 		dma_run_dependencies(txd);
diff --git a/drivers/dma/bestcomm/bestcomm.c b/drivers/dma/bestcomm/bestcomm.c
index 7ce843723003..7a67b8345092 100644
--- a/drivers/dma/bestcomm/bestcomm.c
+++ b/drivers/dma/bestcomm/bestcomm.c
@@ -82,7 +82,7 @@ bcom_task_alloc(int bd_count, int bd_size, int priv_size)
 
 	/* Get IRQ of that task */
 	tsk->irq = irq_of_parse_and_map(bcom_eng->ofnode, tsk->tasknum);
-	if (tsk->irq == NO_IRQ)
+	if (!tsk->irq)
 		goto error;
 
 	/* Init the BDs, if needed */
@@ -104,7 +104,7 @@ bcom_task_alloc(int bd_count, int bd_size, int priv_size)
 
 error:
 	if (tsk) {
-		if (tsk->irq != NO_IRQ)
+		if (tsk->irq)
 			irq_dispose_mapping(tsk->irq);
 		bcom_sram_free(tsk->bd);
 		kfree(tsk->cookie);
diff --git a/drivers/dma/coh901318.c b/drivers/dma/coh901318.c
index e4acd63e42aa..74794c9859f6 100644
--- a/drivers/dma/coh901318.c
+++ b/drivers/dma/coh901318.c
@@ -1319,10 +1319,10 @@ static void coh901318_list_print(struct coh901318_chan *cohc,
 	int i = 0;
 
 	while (l) {
-		dev_vdbg(COHC_2_DEV(cohc), "i %d, lli %p, ctrl 0x%x, src 0x%x"
-			 ", dst 0x%x, link 0x%x virt_link_addr 0x%p\n",
-			 i, l, l->control, l->src_addr, l->dst_addr,
-			 l->link_addr, l->virt_link_addr);
+		dev_vdbg(COHC_2_DEV(cohc), "i %d, lli %p, ctrl 0x%x, src 0x%pad"
+			 ", dst 0x%pad, link 0x%pad virt_link_addr 0x%p\n",
+			 i, l, l->control, &l->src_addr, &l->dst_addr,
+			 &l->link_addr, l->virt_link_addr);
 		i++;
 		l = l->virt_link_addr;
 	}
@@ -1335,7 +1335,7 @@ static void coh901318_list_print(struct coh901318_chan *cohc,
 static struct coh901318_base *debugfs_dma_base;
 static struct dentry *dma_dentry;
 
-static int coh901318_debugfs_read(struct file *file, char __user *buf,
+static ssize_t coh901318_debugfs_read(struct file *file, char __user *buf,
 				  size_t count, loff_t *f_pos)
 {
 	u64 started_channels = debugfs_dma_base->pm.started_channels;
@@ -1352,9 +1352,10 @@ static int coh901318_debugfs_read(struct file *file, char __user *buf,
 
 	tmp += sprintf(tmp, "DMA -- enabled dma channels\n");
 
-	for (i = 0; i < U300_DMA_CHANNELS; i++)
-		if (started_channels & (1 << i))
+	for (i = 0; i < U300_DMA_CHANNELS; i++) {
+		if (started_channels & (1ULL << i))
 			tmp += sprintf(tmp, "channel %d\n", i);
+	}
 
 	tmp += sprintf(tmp, "Pool alloc nbr %d\n", pool_count);
 
@@ -1553,15 +1554,8 @@ coh901318_desc_submit(struct coh901318_chan *cohc, struct coh901318_desc *desc)
 static struct coh901318_desc *
 coh901318_first_active_get(struct coh901318_chan *cohc)
 {
-	struct coh901318_desc *d;
-
-	if (list_empty(&cohc->active))
-		return NULL;
-
-	d = list_first_entry(&cohc->active,
-			     struct coh901318_desc,
-			     node);
-	return d;
+	return list_first_entry_or_null(&cohc->active, struct coh901318_desc,
+					node);
 }
 
 static void
@@ -1579,15 +1573,8 @@ coh901318_desc_queue(struct coh901318_chan *cohc, struct coh901318_desc *desc)
 static struct coh901318_desc *
 coh901318_first_queued(struct coh901318_chan *cohc)
 {
-	struct coh901318_desc *d;
-
-	if (list_empty(&cohc->queue))
-		return NULL;
-
-	d = list_first_entry(&cohc->queue,
-			     struct coh901318_desc,
-			     node);
-	return d;
+	return list_first_entry_or_null(&cohc->queue, struct coh901318_desc,
+					node);
 }
 
 static inline u32 coh901318_get_bytes_in_lli(struct coh901318_lli *in_lli)
@@ -1766,7 +1753,7 @@ static int coh901318_resume(struct dma_chan *chan)
 
 bool coh901318_filter_id(struct dma_chan *chan, void *chan_id)
 {
-	unsigned int ch_nr = (unsigned int) chan_id;
+	unsigned long ch_nr = (unsigned long) chan_id;
 
 	if (ch_nr == to_coh901318_chan(chan)->id)
 		return true;
@@ -1888,8 +1875,7 @@ static void dma_tasklet(unsigned long data)
 	struct coh901318_chan *cohc = (struct coh901318_chan *) data;
 	struct coh901318_desc *cohd_fin;
 	unsigned long flags;
-	dma_async_tx_callback callback;
-	void *callback_param;
+	struct dmaengine_desc_callback cb;
 
 	dev_vdbg(COHC_2_DEV(cohc), "[%s] chan_id %d"
 		 " nbr_active_done %ld\n", __func__,
@@ -1904,8 +1890,7 @@ static void dma_tasklet(unsigned long data)
 		goto err;
 
 	/* locate callback to client */
-	callback = cohd_fin->desc.callback;
-	callback_param = cohd_fin->desc.callback_param;
+	dmaengine_desc_get_callback(&cohd_fin->desc, &cb);
 
 	/* sign this job as completed on the channel */
 	dma_cookie_complete(&cohd_fin->desc);
@@ -1920,8 +1905,7 @@ static void dma_tasklet(unsigned long data)
 	spin_unlock_irqrestore(&cohc->lock, flags);
 
 	/* Call the callback when we're done */
-	if (callback)
-		callback(callback_param);
+	dmaengine_desc_callback_invoke(&cb, NULL);
 
 	spin_lock_irqsave(&cohc->lock, flags);
 
@@ -2247,8 +2231,8 @@ coh901318_prep_memcpy(struct dma_chan *chan, dma_addr_t dest, dma_addr_t src,
 	spin_lock_irqsave(&cohc->lock, flg);
 
 	dev_vdbg(COHC_2_DEV(cohc),
-		 "[%s] channel %d src 0x%x dest 0x%x size %d\n",
-		 __func__, cohc->id, src, dest, size);
+		 "[%s] channel %d src 0x%pad dest 0x%pad size %zu\n",
+		 __func__, cohc->id, &src, &dest, size);
 
 	if (flags & DMA_PREP_INTERRUPT)
 		/* Trigger interrupt after last lli */
@@ -2744,8 +2728,8 @@ static int __init coh901318_probe(struct platform_device *pdev)
 		goto err_register_of_dma;
 
 	platform_set_drvdata(pdev, base);
-	dev_info(&pdev->dev, "Initialized COH901318 DMA on virtual base 0x%08x\n",
-		(u32) base->virtbase);
+	dev_info(&pdev->dev, "Initialized COH901318 DMA on virtual base 0x%p\n",
+		base->virtbase);
 
 	return err;
 
diff --git a/drivers/dma/coh901318_lli.c b/drivers/dma/coh901318_lli.c
index 702112d547c8..d612b2e5abc4 100644
--- a/drivers/dma/coh901318_lli.c
+++ b/drivers/dma/coh901318_lli.c
@@ -75,7 +75,7 @@ coh901318_lli_alloc(struct coh901318_pool *pool, unsigned int len)
 	lli = head;
 	lli->phy_this = phy;
 	lli->link_addr = 0x00000000;
-	lli->virt_link_addr = 0x00000000U;
+	lli->virt_link_addr = NULL;
 
 	for (i = 1; i < len; i++) {
 		lli_prev = lli;
@@ -88,7 +88,7 @@ coh901318_lli_alloc(struct coh901318_pool *pool, unsigned int len)
 		DEBUGFS_POOL_COUNTER_ADD(pool, 1);
 		lli->phy_this = phy;
 		lli->link_addr = 0x00000000;
-		lli->virt_link_addr = 0x00000000U;
+		lli->virt_link_addr = NULL;
 
 		lli_prev->link_addr = phy;
 		lli_prev->virt_link_addr = lli;
diff --git a/drivers/dma/cppi41.c b/drivers/dma/cppi41.c
index 4b2317426c8e..bac5f023013b 100644
--- a/drivers/dma/cppi41.c
+++ b/drivers/dma/cppi41.c
@@ -108,6 +108,8 @@ struct cppi41_channel {
 	unsigned td_queued:1;
 	unsigned td_seen:1;
 	unsigned td_desc_seen:1;
+
+	struct list_head node;		/* Node for pending list */
 };
 
 struct cppi41_desc {
@@ -146,6 +148,9 @@ struct cppi41_dd {
 	const struct chan_queues *queues_tx;
 	struct chan_queues td_queue;
 
+	struct list_head pending;	/* Pending queued transfers */
+	spinlock_t lock;		/* Lock for pending list */
+
 	/* context for suspend/resume */
 	unsigned int dma_tdfdq;
 };
@@ -331,7 +336,11 @@ static irqreturn_t cppi41_irq(int irq, void *data)
 
 			c->residue = pd_trans_len(c->desc->pd6) - len;
 			dma_cookie_complete(&c->txd);
-			c->txd.callback(c->txd.callback_param);
+			dmaengine_desc_get_callback_invoke(&c->txd, NULL);
+
+			/* Paired with cppi41_dma_issue_pending */
+			pm_runtime_mark_last_busy(cdd->ddev.dev);
+			pm_runtime_put_autosuspend(cdd->ddev.dev);
 		}
 	}
 	return IRQ_HANDLED;
@@ -349,6 +358,12 @@ static dma_cookie_t cppi41_tx_submit(struct dma_async_tx_descriptor *tx)
 static int cppi41_dma_alloc_chan_resources(struct dma_chan *chan)
 {
 	struct cppi41_channel *c = to_cpp41_chan(chan);
+	struct cppi41_dd *cdd = c->cdd;
+	int error;
+
+	error = pm_runtime_get_sync(cdd->ddev.dev);
+	if (error < 0)
+		return error;
 
 	dma_cookie_init(chan);
 	dma_async_tx_descriptor_init(&c->txd, chan);
@@ -357,11 +372,26 @@ static int cppi41_dma_alloc_chan_resources(struct dma_chan *chan)
 	if (!c->is_tx)
 		cppi_writel(c->q_num, c->gcr_reg + RXHPCRA0);
 
+	pm_runtime_mark_last_busy(cdd->ddev.dev);
+	pm_runtime_put_autosuspend(cdd->ddev.dev);
+
 	return 0;
 }
 
 static void cppi41_dma_free_chan_resources(struct dma_chan *chan)
 {
+	struct cppi41_channel *c = to_cpp41_chan(chan);
+	struct cppi41_dd *cdd = c->cdd;
+	int error;
+
+	error = pm_runtime_get_sync(cdd->ddev.dev);
+	if (error < 0)
+		return;
+
+	WARN_ON(!list_empty(&cdd->pending));
+
+	pm_runtime_mark_last_busy(cdd->ddev.dev);
+	pm_runtime_put_autosuspend(cdd->ddev.dev);
 }
 
 static enum dma_status cppi41_dma_tx_status(struct dma_chan *chan,
@@ -386,21 +416,6 @@ static void push_desc_queue(struct cppi41_channel *c)
 	u32 desc_phys;
 	u32 reg;
 
-	desc_phys = lower_32_bits(c->desc_phys);
-	desc_num = (desc_phys - cdd->descs_phys) / sizeof(struct cppi41_desc);
-	WARN_ON(cdd->chan_busy[desc_num]);
-	cdd->chan_busy[desc_num] = c;
-
-	reg = (sizeof(struct cppi41_desc) - 24) / 4;
-	reg |= desc_phys;
-	cppi_writel(reg, cdd->qmgr_mem + QMGR_QUEUE_D(c->q_num));
-}
-
-static void cppi41_dma_issue_pending(struct dma_chan *chan)
-{
-	struct cppi41_channel *c = to_cpp41_chan(chan);
-	u32 reg;
-
 	c->residue = 0;
 
 	reg = GCR_CHAN_ENABLE;
@@ -418,7 +433,46 @@ static void cppi41_dma_issue_pending(struct dma_chan *chan)
 	 * before starting the dma engine.
 	 */
 	__iowmb();
-	push_desc_queue(c);
+
+	desc_phys = lower_32_bits(c->desc_phys);
+	desc_num = (desc_phys - cdd->descs_phys) / sizeof(struct cppi41_desc);
+	WARN_ON(cdd->chan_busy[desc_num]);
+	cdd->chan_busy[desc_num] = c;
+
+	reg = (sizeof(struct cppi41_desc) - 24) / 4;
+	reg |= desc_phys;
+	cppi_writel(reg, cdd->qmgr_mem + QMGR_QUEUE_D(c->q_num));
+}
+
+static void pending_desc(struct cppi41_channel *c)
+{
+	struct cppi41_dd *cdd = c->cdd;
+	unsigned long flags;
+
+	spin_lock_irqsave(&cdd->lock, flags);
+	list_add_tail(&c->node, &cdd->pending);
+	spin_unlock_irqrestore(&cdd->lock, flags);
+}
+
+static void cppi41_dma_issue_pending(struct dma_chan *chan)
+{
+	struct cppi41_channel *c = to_cpp41_chan(chan);
+	struct cppi41_dd *cdd = c->cdd;
+	int error;
+
+	/* PM runtime paired with dmaengine_desc_get_callback_invoke */
+	error = pm_runtime_get(cdd->ddev.dev);
+	if ((error != -EINPROGRESS) && error < 0) {
+		dev_err(cdd->ddev.dev, "Failed to pm_runtime_get: %i\n",
+			error);
+
+		return;
+	}
+
+	if (likely(pm_runtime_active(cdd->ddev.dev)))
+		push_desc_queue(c);
+	else
+		pending_desc(c);
 }
 
 static u32 get_host_pd0(u32 length)
@@ -940,12 +994,18 @@ static int cppi41_dma_probe(struct platform_device *pdev)
 	cdd->ctrl_mem = of_iomap(dev->of_node, 1);
 	cdd->sched_mem = of_iomap(dev->of_node, 2);
 	cdd->qmgr_mem = of_iomap(dev->of_node, 3);
+	spin_lock_init(&cdd->lock);
+	INIT_LIST_HEAD(&cdd->pending);
+
+	platform_set_drvdata(pdev, cdd);
 
 	if (!cdd->usbss_mem || !cdd->ctrl_mem || !cdd->sched_mem ||
 			!cdd->qmgr_mem)
 		return -ENXIO;
 
 	pm_runtime_enable(dev);
+	pm_runtime_set_autosuspend_delay(dev, 100);
+	pm_runtime_use_autosuspend(dev);
 	ret = pm_runtime_get_sync(dev);
 	if (ret < 0)
 		goto err_get_sync;
@@ -985,7 +1045,9 @@ static int cppi41_dma_probe(struct platform_device *pdev)
 	if (ret)
 		goto err_of;
 
-	platform_set_drvdata(pdev, cdd);
+	pm_runtime_mark_last_busy(dev);
+	pm_runtime_put_autosuspend(dev);
+
 	return 0;
 err_of:
 	dma_async_device_unregister(&cdd->ddev);
@@ -996,7 +1058,8 @@ err_irq:
 err_chans:
 	deinit_cppi41(dev, cdd);
 err_init_cppi:
-	pm_runtime_put(dev);
+	pm_runtime_dont_use_autosuspend(dev);
+	pm_runtime_put_sync(dev);
 err_get_sync:
 	pm_runtime_disable(dev);
 	iounmap(cdd->usbss_mem);
@@ -1021,13 +1084,13 @@ static int cppi41_dma_remove(struct platform_device *pdev)
 	iounmap(cdd->ctrl_mem);
 	iounmap(cdd->sched_mem);
 	iounmap(cdd->qmgr_mem);
-	pm_runtime_put(&pdev->dev);
+	pm_runtime_dont_use_autosuspend(&pdev->dev);
+	pm_runtime_put_sync(&pdev->dev);
 	pm_runtime_disable(&pdev->dev);
 	return 0;
 }
 
-#ifdef CONFIG_PM_SLEEP
-static int cppi41_suspend(struct device *dev)
+static int __maybe_unused cppi41_suspend(struct device *dev)
 {
 	struct cppi41_dd *cdd = dev_get_drvdata(dev);
 
@@ -1038,7 +1101,7 @@ static int cppi41_suspend(struct device *dev)
 	return 0;
 }
 
-static int cppi41_resume(struct device *dev)
+static int __maybe_unused cppi41_resume(struct device *dev)
 {
 	struct cppi41_dd *cdd = dev_get_drvdata(dev);
 	struct cppi41_channel *c;
@@ -1062,9 +1125,38 @@ static int cppi41_resume(struct device *dev)
 
 	return 0;
 }
-#endif
 
-static SIMPLE_DEV_PM_OPS(cppi41_pm_ops, cppi41_suspend, cppi41_resume);
+static int __maybe_unused cppi41_runtime_suspend(struct device *dev)
+{
+	struct cppi41_dd *cdd = dev_get_drvdata(dev);
+
+	WARN_ON(!list_empty(&cdd->pending));
+
+	return 0;
+}
+
+static int __maybe_unused cppi41_runtime_resume(struct device *dev)
+{
+	struct cppi41_dd *cdd = dev_get_drvdata(dev);
+	struct cppi41_channel *c, *_c;
+	unsigned long flags;
+
+	spin_lock_irqsave(&cdd->lock, flags);
+	list_for_each_entry_safe(c, _c, &cdd->pending, node) {
+		push_desc_queue(c);
+		list_del(&c->node);
+	}
+	spin_unlock_irqrestore(&cdd->lock, flags);
+
+	return 0;
+}
+
+static const struct dev_pm_ops cppi41_pm_ops = {
+	SET_LATE_SYSTEM_SLEEP_PM_OPS(cppi41_suspend, cppi41_resume)
+	SET_RUNTIME_PM_OPS(cppi41_runtime_suspend,
+			   cppi41_runtime_resume,
+			   NULL)
+};
 
 static struct platform_driver cpp41_dma_driver = {
 	.probe  = cppi41_dma_probe,
diff --git a/drivers/dma/dma-jz4740.c b/drivers/dma/dma-jz4740.c
index 9689b36c005a..d50273fed715 100644
--- a/drivers/dma/dma-jz4740.c
+++ b/drivers/dma/dma-jz4740.c
@@ -21,8 +21,6 @@
 #include <linux/irq.h>
 #include <linux/clk.h>
 
-#include <asm/mach-jz4740/dma.h>
-
 #include "virt-dma.h"
 
 #define JZ_DMA_NR_CHANS 6
diff --git a/drivers/dma/dma-jz4780.c b/drivers/dma/dma-jz4780.c
index dade7c47ff18..7373b7a555ec 100644
--- a/drivers/dma/dma-jz4780.c
+++ b/drivers/dma/dma-jz4780.c
@@ -324,8 +324,10 @@ static struct dma_async_tx_descriptor *jz4780_dma_prep_slave_sg(
 					      sg_dma_address(&sgl[i]),
 					      sg_dma_len(&sgl[i]),
 					      direction);
-		if (err < 0)
+		if (err < 0) {
+			jz4780_dma_desc_free(&jzchan->desc->vdesc);
 			return NULL;
+		}
 
 		desc->desc[i].dcm |= JZ_DMA_DCM_TIE;
 
@@ -368,8 +370,10 @@ static struct dma_async_tx_descriptor *jz4780_dma_prep_dma_cyclic(
 	for (i = 0; i < periods; i++) {
 		err = jz4780_dma_setup_hwdesc(jzchan, &desc->desc[i], buf_addr,
 					      period_len, direction);
-		if (err < 0)
+		if (err < 0) {
+			jz4780_dma_desc_free(&jzchan->desc->vdesc);
 			return NULL;
+		}
 
 		buf_addr += period_len;
 
@@ -396,7 +400,7 @@ static struct dma_async_tx_descriptor *jz4780_dma_prep_dma_cyclic(
 	return vchan_tx_prep(&jzchan->vchan, &desc->vdesc, flags);
 }
 
-struct dma_async_tx_descriptor *jz4780_dma_prep_dma_memcpy(
+static struct dma_async_tx_descriptor *jz4780_dma_prep_dma_memcpy(
 	struct dma_chan *chan, dma_addr_t dest, dma_addr_t src,
 	size_t len, unsigned long flags)
 {
diff --git a/drivers/dma/dmaengine.c b/drivers/dma/dmaengine.c
index 8c9f45fd55fc..6b535262ac5d 100644
--- a/drivers/dma/dmaengine.c
+++ b/drivers/dma/dmaengine.c
@@ -997,6 +997,13 @@ int dma_async_device_register(struct dma_device *device)
 		}
 		chan->client_count = 0;
 	}
+
+	if (!chancnt) {
+		dev_err(device->dev, "%s: device has no channels!\n", __func__);
+		rc = -ENODEV;
+		goto err_out;
+	}
+
 	device->chancnt = chancnt;
 
 	mutex_lock(&dma_list_mutex);
diff --git a/drivers/dma/dmaengine.h b/drivers/dma/dmaengine.h
index 17f983a4e9ba..882ff9448c3b 100644
--- a/drivers/dma/dmaengine.h
+++ b/drivers/dma/dmaengine.h
@@ -86,4 +86,88 @@ static inline void dma_set_residue(struct dma_tx_state *state, u32 residue)
 		state->residue = residue;
 }
 
+struct dmaengine_desc_callback {
+	dma_async_tx_callback callback;
+	dma_async_tx_callback_result callback_result;
+	void *callback_param;
+};
+
+/**
+ * dmaengine_desc_get_callback - get the passed in callback function
+ * @tx: tx descriptor
+ * @cb: temp struct to hold the callback info
+ *
+ * Fill the passed in cb struct with what's available in the passed in
+ * tx descriptor struct
+ * No locking is required.
+ */
+static inline void
+dmaengine_desc_get_callback(struct dma_async_tx_descriptor *tx,
+			    struct dmaengine_desc_callback *cb)
+{
+	cb->callback = tx->callback;
+	cb->callback_result = tx->callback_result;
+	cb->callback_param = tx->callback_param;
+}
+
+/**
+ * dmaengine_desc_callback_invoke - call the callback function in cb struct
+ * @cb: temp struct that is holding the callback info
+ * @result: transaction result
+ *
+ * Call the callback function provided in the cb struct with the parameter
+ * in the cb struct.
+ * Locking is dependent on the driver.
+ */
+static inline void
+dmaengine_desc_callback_invoke(struct dmaengine_desc_callback *cb,
+			       const struct dmaengine_result *result)
+{
+	struct dmaengine_result dummy_result = {
+		.result = DMA_TRANS_NOERROR,
+		.residue = 0
+	};
+
+	if (cb->callback_result) {
+		if (!result)
+			result = &dummy_result;
+		cb->callback_result(cb->callback_param, result);
+	} else if (cb->callback) {
+		cb->callback(cb->callback_param);
+	}
+}
+
+/**
+ * dmaengine_desc_get_callback_invoke - get the callback in tx descriptor and
+ * 					then immediately call the callback.
+ * @tx: dma async tx descriptor
+ * @result: transaction result
+ *
+ * Call dmaengine_desc_get_callback() and dmaengine_desc_callback_invoke()
+ * in a single function since no work is necessary in between for the driver.
+ * Locking is dependent on the driver.
+ */
+static inline void
+dmaengine_desc_get_callback_invoke(struct dma_async_tx_descriptor *tx,
+				   const struct dmaengine_result *result)
+{
+	struct dmaengine_desc_callback cb;
+
+	dmaengine_desc_get_callback(tx, &cb);
+	dmaengine_desc_callback_invoke(&cb, result);
+}
+
+/**
+ * dmaengine_desc_callback_valid - verify the callback is valid in cb
+ * @cb: callback info struct
+ *
+ * Return a bool that verifies whether callback in cb is valid or not.
+ * No locking is required.
+ */
+static inline bool
+dmaengine_desc_callback_valid(struct dmaengine_desc_callback *cb)
+{
+	return (cb->callback) ? true : false;
+}
+
 #endif
diff --git a/drivers/dma/dmatest.c b/drivers/dma/dmatest.c
index 1245db5438e1..cf76fc6149e5 100644
--- a/drivers/dma/dmatest.c
+++ b/drivers/dma/dmatest.c
@@ -56,10 +56,10 @@ module_param(sg_buffers, uint, S_IRUGO | S_IWUSR);
 MODULE_PARM_DESC(sg_buffers,
 		"Number of scatter gather buffers (default: 1)");
 
-static unsigned int dmatest = 1;
+static unsigned int dmatest;
 module_param(dmatest, uint, S_IRUGO | S_IWUSR);
 MODULE_PARM_DESC(dmatest,
-		"dmatest 0-memcpy 1-slave_sg (default: 1)");
+		"dmatest 0-memcpy 1-slave_sg (default: 0)");
 
 static unsigned int xor_sources = 3;
 module_param(xor_sources, uint, S_IRUGO | S_IWUSR);
@@ -426,7 +426,9 @@ static int dmatest_func(void *data)
 	int			src_cnt;
 	int			dst_cnt;
 	int			i;
-	ktime_t			ktime;
+	ktime_t			ktime, start, diff;
+	ktime_t			filltime = ktime_set(0, 0);
+	ktime_t			comparetime = ktime_set(0, 0);
 	s64			runtime = 0;
 	unsigned long long	total_len = 0;
 
@@ -503,7 +505,7 @@ static int dmatest_func(void *data)
 		total_tests++;
 
 		/* honor alignment restrictions */
-		if (thread->type == DMA_MEMCPY)
+		if (thread->type == DMA_MEMCPY || thread->type == DMA_SG)
 			align = dev->copy_align;
 		else if (thread->type == DMA_XOR)
 			align = dev->xor_align;
@@ -531,6 +533,7 @@ static int dmatest_func(void *data)
 			src_off = 0;
 			dst_off = 0;
 		} else {
+			start = ktime_get();
 			src_off = dmatest_random() % (params->buf_size - len + 1);
 			dst_off = dmatest_random() % (params->buf_size - len + 1);
 
@@ -541,6 +544,9 @@ static int dmatest_func(void *data)
 					  params->buf_size);
 			dmatest_init_dsts(thread->dsts, dst_off, len,
 					  params->buf_size);
+
+			diff = ktime_sub(ktime_get(), start);
+			filltime = ktime_add(filltime, diff);
 		}
 
 		um = dmaengine_get_unmap_data(dev->dev, src_cnt+dst_cnt,
@@ -683,6 +689,7 @@ static int dmatest_func(void *data)
 			continue;
 		}
 
+		start = ktime_get();
 		pr_debug("%s: verifying source buffer...\n", current->comm);
 		error_count = dmatest_verify(thread->srcs, 0, src_off,
 				0, PATTERN_SRC, true);
@@ -703,6 +710,9 @@ static int dmatest_func(void *data)
 				params->buf_size, dst_off + len,
 				PATTERN_DST, false);
 
+		diff = ktime_sub(ktime_get(), start);
+		comparetime = ktime_add(comparetime, diff);
+
 		if (error_count) {
 			result("data error", total_tests, src_off, dst_off,
 			       len, error_count);
@@ -712,7 +722,10 @@ static int dmatest_func(void *data)
 				       dst_off, len, 0);
 		}
 	}
-	runtime = ktime_us_delta(ktime_get(), ktime);
+	ktime = ktime_sub(ktime_get(), ktime);
+	ktime = ktime_sub(ktime, comparetime);
+	ktime = ktime_sub(ktime, filltime);
+	runtime = ktime_to_us(ktime);
 
 	ret = 0;
 err_dstbuf:
diff --git a/drivers/dma/dw/core.c b/drivers/dma/dw/core.c
index edf053f73a49..c2c0a613cb7a 100644
--- a/drivers/dma/dw/core.c
+++ b/drivers/dma/dw/core.c
@@ -46,9 +46,9 @@
 		u8 _dmsize = _is_slave ? _sconfig->dst_maxburst :	\
 			DW_DMA_MSIZE_16;			\
 		u8 _dms = (_dwc->direction == DMA_MEM_TO_DEV) ?		\
-			_dwc->p_master : _dwc->m_master;		\
+			_dwc->dws.p_master : _dwc->dws.m_master;	\
 		u8 _sms = (_dwc->direction == DMA_DEV_TO_MEM) ?		\
-			_dwc->p_master : _dwc->m_master;		\
+			_dwc->dws.p_master : _dwc->dws.m_master;	\
 								\
 		(DWC_CTLL_DST_MSIZE(_dmsize)			\
 		 | DWC_CTLL_SRC_MSIZE(_smsize)			\
@@ -143,12 +143,16 @@ static void dwc_initialize(struct dw_dma_chan *dwc)
 	struct dw_dma *dw = to_dw_dma(dwc->chan.device);
 	u32 cfghi = DWC_CFGH_FIFO_MODE;
 	u32 cfglo = DWC_CFGL_CH_PRIOR(dwc->priority);
+	bool hs_polarity = dwc->dws.hs_polarity;
 
 	if (test_bit(DW_DMA_IS_INITIALIZED, &dwc->flags))
 		return;
 
-	cfghi |= DWC_CFGH_DST_PER(dwc->dst_id);
-	cfghi |= DWC_CFGH_SRC_PER(dwc->src_id);
+	cfghi |= DWC_CFGH_DST_PER(dwc->dws.dst_id);
+	cfghi |= DWC_CFGH_SRC_PER(dwc->dws.src_id);
+
+	/* Set polarity of handshake interface */
+	cfglo |= hs_polarity ? DWC_CFGL_HS_DST_POL | DWC_CFGL_HS_SRC_POL : 0;
 
 	channel_writel(dwc, CFG_LO, cfglo);
 	channel_writel(dwc, CFG_HI, cfghi);
@@ -209,7 +213,7 @@ static inline void dwc_do_single_block(struct dw_dma_chan *dwc,
 static void dwc_dostart(struct dw_dma_chan *dwc, struct dw_desc *first)
 {
 	struct dw_dma	*dw = to_dw_dma(dwc->chan.device);
-	u8		lms = DWC_LLP_LMS(dwc->m_master);
+	u8		lms = DWC_LLP_LMS(dwc->dws.m_master);
 	unsigned long	was_soft_llp;
 
 	/* ASSERT:  channel is idle */
@@ -270,20 +274,19 @@ static void
 dwc_descriptor_complete(struct dw_dma_chan *dwc, struct dw_desc *desc,
 		bool callback_required)
 {
-	dma_async_tx_callback		callback = NULL;
-	void				*param = NULL;
 	struct dma_async_tx_descriptor	*txd = &desc->txd;
 	struct dw_desc			*child;
 	unsigned long			flags;
+	struct dmaengine_desc_callback	cb;
 
 	dev_vdbg(chan2dev(&dwc->chan), "descriptor %u complete\n", txd->cookie);
 
 	spin_lock_irqsave(&dwc->lock, flags);
 	dma_cookie_complete(txd);
-	if (callback_required) {
-		callback = txd->callback;
-		param = txd->callback_param;
-	}
+	if (callback_required)
+		dmaengine_desc_get_callback(txd, &cb);
+	else
+		memset(&cb, 0, sizeof(cb));
 
 	/* async_tx_ack */
 	list_for_each_entry(child, &desc->tx_list, desc_node)
@@ -292,8 +295,7 @@ dwc_descriptor_complete(struct dw_dma_chan *dwc, struct dw_desc *desc,
 	dwc_desc_put(dwc, desc);
 	spin_unlock_irqrestore(&dwc->lock, flags);
 
-	if (callback)
-		callback(param);
+	dmaengine_desc_callback_invoke(&cb, NULL);
 }
 
 static void dwc_complete_all(struct dw_dma *dw, struct dw_dma_chan *dwc)
@@ -662,7 +664,7 @@ dwc_prep_dma_memcpy(struct dma_chan *chan, dma_addr_t dest, dma_addr_t src,
 	struct dw_desc		*prev;
 	size_t			xfer_count;
 	size_t			offset;
-	u8			m_master = dwc->m_master;
+	u8			m_master = dwc->dws.m_master;
 	unsigned int		src_width;
 	unsigned int		dst_width;
 	unsigned int		data_width = dw->pdata->data_width[m_master];
@@ -740,7 +742,7 @@ dwc_prep_slave_sg(struct dma_chan *chan, struct scatterlist *sgl,
 	struct dw_desc		*prev;
 	struct dw_desc		*first;
 	u32			ctllo;
-	u8			m_master = dwc->m_master;
+	u8			m_master = dwc->dws.m_master;
 	u8			lms = DWC_LLP_LMS(m_master);
 	dma_addr_t		reg;
 	unsigned int		reg_width;
@@ -895,12 +897,7 @@ bool dw_dma_filter(struct dma_chan *chan, void *param)
 		return false;
 
 	/* We have to copy data since dws can be temporary storage */
-
-	dwc->src_id = dws->src_id;
-	dwc->dst_id = dws->dst_id;
-
-	dwc->m_master = dws->m_master;
-	dwc->p_master = dws->p_master;
+	memcpy(&dwc->dws, dws, sizeof(struct dw_dma_slave));
 
 	return true;
 }
@@ -1167,11 +1164,7 @@ static void dwc_free_chan_resources(struct dma_chan *chan)
 	spin_lock_irqsave(&dwc->lock, flags);
 
 	/* Clear custom channel configuration */
-	dwc->src_id = 0;
-	dwc->dst_id = 0;
-
-	dwc->m_master = 0;
-	dwc->p_master = 0;
+	memset(&dwc->dws, 0, sizeof(struct dw_dma_slave));
 
 	clear_bit(DW_DMA_IS_INITIALIZED, &dwc->flags);
 
@@ -1264,7 +1257,7 @@ struct dw_cyclic_desc *dw_dma_cyclic_prep(struct dma_chan *chan,
 	struct dw_cyclic_desc		*retval = NULL;
 	struct dw_desc			*desc;
 	struct dw_desc			*last = NULL;
-	u8				lms = DWC_LLP_LMS(dwc->m_master);
+	u8				lms = DWC_LLP_LMS(dwc->dws.m_master);
 	unsigned long			was_cyclic;
 	unsigned int			reg_width;
 	unsigned int			periods;
@@ -1576,11 +1569,7 @@ int dw_dma_probe(struct dw_dma_chip *chip)
 				(dwc_params >> DWC_PARAMS_MBLK_EN & 0x1) == 0;
 		} else {
 			dwc->block_size = pdata->block_size;
-
-			/* Check if channel supports multi block transfer */
-			channel_writel(dwc, LLP, DWC_LLP_LOC(0xffffffff));
-			dwc->nollp = DWC_LLP_LOC(channel_readl(dwc, LLP)) == 0;
-			channel_writel(dwc, LLP, 0);
+			dwc->nollp = pdata->is_nollp;
 		}
 	}
 
diff --git a/drivers/dma/dw/regs.h b/drivers/dma/dw/regs.h
index 4b7bd7834046..f65dd104479f 100644
--- a/drivers/dma/dw/regs.h
+++ b/drivers/dma/dw/regs.h
@@ -245,10 +245,7 @@ struct dw_dma_chan {
 	bool			nollp;
 
 	/* custom slave configuration */
-	u8			src_id;
-	u8			dst_id;
-	u8			m_master;
-	u8			p_master;
+	struct dw_dma_slave	dws;
 
 	/* configuration passed via .device_config */
 	struct dma_slave_config dma_sconfig;
diff --git a/drivers/dma/edma.c b/drivers/dma/edma.c
index 3d277fa76c1a..e18a58068bca 100644
--- a/drivers/dma/edma.c
+++ b/drivers/dma/edma.c
@@ -263,22 +263,29 @@ static const struct edmacc_param dummy_paramset = {
 
 #define EDMA_BINDING_LEGACY	0
 #define EDMA_BINDING_TPCC	1
+static const u32 edma_binding_type[] = {
+	[EDMA_BINDING_LEGACY] = EDMA_BINDING_LEGACY,
+	[EDMA_BINDING_TPCC] = EDMA_BINDING_TPCC,
+};
+
 static const struct of_device_id edma_of_ids[] = {
 	{
 		.compatible = "ti,edma3",
-		.data = (void *)EDMA_BINDING_LEGACY,
+		.data = &edma_binding_type[EDMA_BINDING_LEGACY],
 	},
 	{
 		.compatible = "ti,edma3-tpcc",
-		.data = (void *)EDMA_BINDING_TPCC,
+		.data = &edma_binding_type[EDMA_BINDING_TPCC],
 	},
 	{}
 };
+MODULE_DEVICE_TABLE(of, edma_of_ids);
 
 static const struct of_device_id edma_tptc_of_ids[] = {
 	{ .compatible = "ti,edma3-tptc", },
 	{}
 };
+MODULE_DEVICE_TABLE(of, edma_tptc_of_ids);
 
 static inline unsigned int edma_read(struct edma_cc *ecc, int offset)
 {
@@ -405,18 +412,12 @@ static inline void edma_param_or(struct edma_cc *ecc, int offset, int param_no,
 	edma_or(ecc, EDMA_PARM + offset + (param_no << 5), or);
 }
 
-static inline void set_bits(int offset, int len, unsigned long *p)
+static inline void edma_set_bits(int offset, int len, unsigned long *p)
 {
 	for (; len > 0; len--)
 		set_bit(offset + (len - 1), p);
 }
 
-static inline void clear_bits(int offset, int len, unsigned long *p)
-{
-	for (; len > 0; len--)
-		clear_bit(offset + (len - 1), p);
-}
-
 static void edma_assign_priority_to_queue(struct edma_cc *ecc, int queue_no,
 					  int priority)
 {
@@ -464,13 +465,15 @@ static void edma_write_slot(struct edma_cc *ecc, unsigned slot,
 	memcpy_toio(ecc->base + PARM_OFFSET(slot), param, PARM_SIZE);
 }
 
-static void edma_read_slot(struct edma_cc *ecc, unsigned slot,
+static int edma_read_slot(struct edma_cc *ecc, unsigned slot,
 			   struct edmacc_param *param)
 {
 	slot = EDMA_CHAN_SLOT(slot);
 	if (slot >= ecc->num_slots)
-		return;
+		return -EINVAL;
 	memcpy_fromio(param, ecc->base + PARM_OFFSET(slot), PARM_SIZE);
+
+	return 0;
 }
 
 /**
@@ -1476,13 +1479,15 @@ static void edma_error_handler(struct edma_chan *echan)
 	struct edma_cc *ecc = echan->ecc;
 	struct device *dev = echan->vchan.chan.device->dev;
 	struct edmacc_param p;
+	int err;
 
 	if (!echan->edesc)
 		return;
 
 	spin_lock(&echan->vchan.lock);
 
-	edma_read_slot(ecc, echan->slot[0], &p);
+	err = edma_read_slot(ecc, echan->slot[0], &p);
+
 	/*
 	 * Issue later based on missed flag which will be sure
 	 * to happen as:
@@ -1495,7 +1500,7 @@ static void edma_error_handler(struct edma_chan *echan)
 	 * lead to some nasty recursion when we are in a NULL
 	 * slot. So we avoid doing so and set the missed flag.
 	 */
-	if (p.a_b_cnt == 0 && p.ccnt == 0) {
+	if (err || (p.a_b_cnt == 0 && p.ccnt == 0)) {
 		dev_dbg(dev, "Error on null slot, setting miss\n");
 		echan->missed = 1;
 	} else {
@@ -2019,8 +2024,7 @@ static struct edma_soc_info *edma_setup_info_from_dt(struct device *dev,
 {
 	struct edma_soc_info *info;
 	struct property *prop;
-	size_t sz;
-	int ret;
+	int sz, ret;
 
 	info = devm_kzalloc(dev, sizeof(struct edma_soc_info), GFP_KERNEL);
 	if (!info)
@@ -2182,7 +2186,7 @@ static int edma_probe(struct platform_device *pdev)
 		const struct of_device_id *match;
 
 		match = of_match_node(edma_of_ids, node);
-		if (match && (u32)match->data == EDMA_BINDING_TPCC)
+		if (match && (*(u32 *)match->data) == EDMA_BINDING_TPCC)
 			legacy_mode = false;
 
 		info = edma_setup_info_from_dt(dev, legacy_mode);
@@ -2260,7 +2264,7 @@ static int edma_probe(struct platform_device *pdev)
 			for (i = 0; rsv_slots[i][0] != -1; i++) {
 				off = rsv_slots[i][0];
 				ln = rsv_slots[i][1];
-				set_bits(off, ln, ecc->slot_inuse);
+				edma_set_bits(off, ln, ecc->slot_inuse);
 			}
 		}
 	}
diff --git a/drivers/dma/ep93xx_dma.c b/drivers/dma/ep93xx_dma.c
index 21f08cc3352b..d37e8dda8079 100644
--- a/drivers/dma/ep93xx_dma.c
+++ b/drivers/dma/ep93xx_dma.c
@@ -262,10 +262,8 @@ static void ep93xx_dma_set_active(struct ep93xx_dma_chan *edmac,
 static struct ep93xx_dma_desc *
 ep93xx_dma_get_active(struct ep93xx_dma_chan *edmac)
 {
-	if (list_empty(&edmac->active))
-		return NULL;
-
-	return list_first_entry(&edmac->active, struct ep93xx_dma_desc, node);
+	return list_first_entry_or_null(&edmac->active,
+					struct ep93xx_dma_desc, node);
 }
 
 /**
@@ -739,10 +737,10 @@ static void ep93xx_dma_tasklet(unsigned long data)
 {
 	struct ep93xx_dma_chan *edmac = (struct ep93xx_dma_chan *)data;
 	struct ep93xx_dma_desc *desc, *d;
-	dma_async_tx_callback callback = NULL;
-	void *callback_param = NULL;
+	struct dmaengine_desc_callback cb;
 	LIST_HEAD(list);
 
+	memset(&cb, 0, sizeof(cb));
 	spin_lock_irq(&edmac->lock);
 	/*
 	 * If dma_terminate_all() was called before we get to run, the active
@@ -757,8 +755,7 @@ static void ep93xx_dma_tasklet(unsigned long data)
 				dma_cookie_complete(&desc->txd);
 			list_splice_init(&edmac->active, &list);
 		}
-		callback = desc->txd.callback;
-		callback_param = desc->txd.callback_param;
+		dmaengine_desc_get_callback(&desc->txd, &cb);
 	}
 	spin_unlock_irq(&edmac->lock);
 
@@ -771,8 +768,7 @@ static void ep93xx_dma_tasklet(unsigned long data)
 		ep93xx_dma_desc_put(edmac, desc);
 	}
 
-	if (callback)
-		callback(callback_param);
+	dmaengine_desc_callback_invoke(&cb, NULL);
 }
 
 static irqreturn_t ep93xx_dma_interrupt(int irq, void *dev_id)
@@ -1047,11 +1043,11 @@ ep93xx_dma_prep_slave_sg(struct dma_chan *chan, struct scatterlist *sgl,
 
 	first = NULL;
 	for_each_sg(sgl, sg, sg_len, i) {
-		size_t sg_len = sg_dma_len(sg);
+		size_t len = sg_dma_len(sg);
 
-		if (sg_len > DMA_MAX_CHAN_BYTES) {
-			dev_warn(chan2dev(edmac), "too big transfer size %d\n",
-				 sg_len);
+		if (len > DMA_MAX_CHAN_BYTES) {
+			dev_warn(chan2dev(edmac), "too big transfer size %zu\n",
+				 len);
 			goto fail;
 		}
 
@@ -1068,7 +1064,7 @@ ep93xx_dma_prep_slave_sg(struct dma_chan *chan, struct scatterlist *sgl,
 			desc->src_addr = edmac->runtime_addr;
 			desc->dst_addr = sg_dma_address(sg);
 		}
-		desc->size = sg_len;
+		desc->size = len;
 
 		if (!first)
 			first = desc;
@@ -1125,7 +1121,7 @@ ep93xx_dma_prep_dma_cyclic(struct dma_chan *chan, dma_addr_t dma_addr,
 	}
 
 	if (period_len > DMA_MAX_CHAN_BYTES) {
-		dev_warn(chan2dev(edmac), "too big period length %d\n",
+		dev_warn(chan2dev(edmac), "too big period length %zu\n",
 			 period_len);
 		return NULL;
 	}
diff --git a/drivers/dma/fsl_raid.c b/drivers/dma/fsl_raid.c
index de2a2a2b1d75..db2f9e1653a2 100644
--- a/drivers/dma/fsl_raid.c
+++ b/drivers/dma/fsl_raid.c
@@ -134,17 +134,9 @@ static void fsl_re_issue_pending(struct dma_chan *chan)
 
 static void fsl_re_desc_done(struct fsl_re_desc *desc)
 {
-	dma_async_tx_callback callback;
-	void *callback_param;
-
 	dma_cookie_complete(&desc->async_tx);
-
-	callback = desc->async_tx.callback;
-	callback_param = desc->async_tx.callback_param;
-	if (callback)
-		callback(callback_param);
-
 	dma_descriptor_unmap(&desc->async_tx);
+	dmaengine_desc_get_callback_invoke(&desc->async_tx, NULL);
 }
 
 static void fsl_re_cleanup_descs(struct fsl_re_chan *re_chan)
@@ -670,7 +662,7 @@ static int fsl_re_chan_probe(struct platform_device *ofdev,
 
 	/* read irq property from dts */
 	chan->irq = irq_of_parse_and_map(np, 0);
-	if (chan->irq == NO_IRQ) {
+	if (!chan->irq) {
 		dev_err(dev, "No IRQ defined for JR %d\n", q);
 		ret = -ENODEV;
 		goto err_free;
diff --git a/drivers/dma/fsldma.c b/drivers/dma/fsldma.c
index 911b7177eb50..51c75bf2b9b6 100644
--- a/drivers/dma/fsldma.c
+++ b/drivers/dma/fsldma.c
@@ -516,13 +516,9 @@ static dma_cookie_t fsldma_run_tx_complete_actions(struct fsldma_chan *chan,
 	if (txd->cookie > 0) {
 		ret = txd->cookie;
 
-		/* Run the link descriptor callback function */
-		if (txd->callback) {
-			chan_dbg(chan, "LD %p callback\n", desc);
-			txd->callback(txd->callback_param);
-		}
-
 		dma_descriptor_unmap(txd);
+		/* Run the link descriptor callback function */
+		dmaengine_desc_get_callback_invoke(txd, NULL);
 	}
 
 	/* Run any dependencies */
@@ -1153,7 +1149,7 @@ static void fsldma_free_irqs(struct fsldma_device *fdev)
 	struct fsldma_chan *chan;
 	int i;
 
-	if (fdev->irq != NO_IRQ) {
+	if (fdev->irq) {
 		dev_dbg(fdev->dev, "free per-controller IRQ\n");
 		free_irq(fdev->irq, fdev);
 		return;
@@ -1161,7 +1157,7 @@ static void fsldma_free_irqs(struct fsldma_device *fdev)
 
 	for (i = 0; i < FSL_DMA_MAX_CHANS_PER_DEVICE; i++) {
 		chan = fdev->chan[i];
-		if (chan && chan->irq != NO_IRQ) {
+		if (chan && chan->irq) {
 			chan_dbg(chan, "free per-channel IRQ\n");
 			free_irq(chan->irq, chan);
 		}
@@ -1175,7 +1171,7 @@ static int fsldma_request_irqs(struct fsldma_device *fdev)
 	int i;
 
 	/* if we have a per-controller IRQ, use that */
-	if (fdev->irq != NO_IRQ) {
+	if (fdev->irq) {
 		dev_dbg(fdev->dev, "request per-controller IRQ\n");
 		ret = request_irq(fdev->irq, fsldma_ctrl_irq, IRQF_SHARED,
 				  "fsldma-controller", fdev);
@@ -1188,7 +1184,7 @@ static int fsldma_request_irqs(struct fsldma_device *fdev)
 		if (!chan)
 			continue;
 
-		if (chan->irq == NO_IRQ) {
+		if (!chan->irq) {
 			chan_err(chan, "interrupts property missing in device tree\n");
 			ret = -ENODEV;
 			goto out_unwind;
@@ -1211,7 +1207,7 @@ out_unwind:
 		if (!chan)
 			continue;
 
-		if (chan->irq == NO_IRQ)
+		if (!chan->irq)
 			continue;
 
 		free_irq(chan->irq, chan);
@@ -1311,7 +1307,7 @@ static int fsl_dma_chan_probe(struct fsldma_device *fdev,
 	list_add_tail(&chan->common.device_node, &fdev->common.channels);
 
 	dev_info(fdev->dev, "#%d (%s), irq %d\n", chan->id, compatible,
-		 chan->irq != NO_IRQ ? chan->irq : fdev->irq);
+		 chan->irq ? chan->irq : fdev->irq);
 
 	return 0;
 
@@ -1351,7 +1347,7 @@ static int fsldma_of_probe(struct platform_device *op)
 	if (!fdev->regs) {
 		dev_err(&op->dev, "unable to ioremap registers\n");
 		err = -ENOMEM;
-		goto out_free_fdev;
+		goto out_free;
 	}
 
 	/* map the channel IRQ if it exists, but don't hookup the handler yet */
@@ -1416,6 +1412,8 @@ static int fsldma_of_probe(struct platform_device *op)
 
 out_free_fdev:
 	irq_dispose_mapping(fdev->irq);
+	iounmap(fdev->regs);
+out_free:
 	kfree(fdev);
 out_return:
 	return err;
diff --git a/drivers/dma/hsu/hsu.c b/drivers/dma/hsu/hsu.c
index c5f21efd6090..29d04ca71d52 100644
--- a/drivers/dma/hsu/hsu.c
+++ b/drivers/dma/hsu/hsu.c
@@ -200,10 +200,9 @@ EXPORT_SYMBOL_GPL(hsu_dma_get_status);
  *      is not a normal timeout interrupt, ie. hsu_dma_get_status() returned 0.
  *
  *      Return:
- *      IRQ_NONE for invalid channel number, IRQ_HANDLED otherwise.
+ *      0 for invalid channel number, 1 otherwise.
  */
-irqreturn_t hsu_dma_do_irq(struct hsu_dma_chip *chip, unsigned short nr,
-			   u32 status)
+int hsu_dma_do_irq(struct hsu_dma_chip *chip, unsigned short nr, u32 status)
 {
 	struct hsu_dma_chan *hsuc;
 	struct hsu_dma_desc *desc;
@@ -211,7 +210,7 @@ irqreturn_t hsu_dma_do_irq(struct hsu_dma_chip *chip, unsigned short nr,
 
 	/* Sanity check */
 	if (nr >= chip->hsu->nr_channels)
-		return IRQ_NONE;
+		return 0;
 
 	hsuc = &chip->hsu->chan[nr];
 
@@ -230,7 +229,7 @@ irqreturn_t hsu_dma_do_irq(struct hsu_dma_chip *chip, unsigned short nr,
 	}
 	spin_unlock_irqrestore(&hsuc->vchan.lock, flags);
 
-	return IRQ_HANDLED;
+	return 1;
 }
 EXPORT_SYMBOL_GPL(hsu_dma_do_irq);
 
diff --git a/drivers/dma/hsu/pci.c b/drivers/dma/hsu/pci.c
index 9916058531d9..b51639f045ed 100644
--- a/drivers/dma/hsu/pci.c
+++ b/drivers/dma/hsu/pci.c
@@ -29,7 +29,7 @@ static irqreturn_t hsu_pci_irq(int irq, void *dev)
 	u32 dmaisr;
 	u32 status;
 	unsigned short i;
-	irqreturn_t ret = IRQ_NONE;
+	int ret = 0;
 	int err;
 
 	dmaisr = readl(chip->regs + HSU_PCI_DMAISR);
@@ -37,14 +37,14 @@ static irqreturn_t hsu_pci_irq(int irq, void *dev)
 		if (dmaisr & 0x1) {
 			err = hsu_dma_get_status(chip, i, &status);
 			if (err > 0)
-				ret |= IRQ_HANDLED;
+				ret |= 1;
 			else if (err == 0)
 				ret |= hsu_dma_do_irq(chip, i, status);
 		}
 		dmaisr >>= 1;
 	}
 
-	return ret;
+	return IRQ_RETVAL(ret);
 }
 
 static int hsu_pci_probe(struct pci_dev *pdev, const struct pci_device_id *id)
diff --git a/drivers/dma/imx-dma.c b/drivers/dma/imx-dma.c
index a960608c0a4d..ab0fb804fb1e 100644
--- a/drivers/dma/imx-dma.c
+++ b/drivers/dma/imx-dma.c
@@ -663,9 +663,7 @@ static void imxdma_tasklet(unsigned long data)
 out:
 	spin_unlock_irqrestore(&imxdma->lock, flags);
 
-	if (desc->desc.callback)
-		desc->desc.callback(desc->desc.callback_param);
-
+	dmaengine_desc_get_callback_invoke(&desc->desc, NULL);
 }
 
 static int imxdma_terminate_all(struct dma_chan *chan)
diff --git a/drivers/dma/imx-sdma.c b/drivers/dma/imx-sdma.c
index 03ec76fc22ff..b9629b2bfc05 100644
--- a/drivers/dma/imx-sdma.c
+++ b/drivers/dma/imx-sdma.c
@@ -184,7 +184,7 @@
 struct sdma_mode_count {
 	u32 count   : 16; /* size of the buffer pointed by this BD */
 	u32 status  :  8; /* E,R,I,C,W,D status bits stored here */
-	u32 command :  8; /* command mostlky used for channel 0 */
+	u32 command :  8; /* command mostly used for channel 0 */
 };
 
 /*
@@ -479,6 +479,24 @@ static struct sdma_driver_data sdma_imx6q = {
 	.script_addrs = &sdma_script_imx6q,
 };
 
+static struct sdma_script_start_addrs sdma_script_imx7d = {
+	.ap_2_ap_addr = 644,
+	.uart_2_mcu_addr = 819,
+	.mcu_2_app_addr = 749,
+	.uartsh_2_mcu_addr = 1034,
+	.mcu_2_shp_addr = 962,
+	.app_2_mcu_addr = 685,
+	.shp_2_mcu_addr = 893,
+	.spdif_2_mcu_addr = 1102,
+	.mcu_2_spdif_addr = 1136,
+};
+
+static struct sdma_driver_data sdma_imx7d = {
+	.chnenbl0 = SDMA_CHNENBL0_IMX35,
+	.num_events = 48,
+	.script_addrs = &sdma_script_imx7d,
+};
+
 static const struct platform_device_id sdma_devtypes[] = {
 	{
 		.name = "imx25-sdma",
@@ -499,6 +517,9 @@ static const struct platform_device_id sdma_devtypes[] = {
 		.name = "imx6q-sdma",
 		.driver_data = (unsigned long)&sdma_imx6q,
 	}, {
+		.name = "imx7d-sdma",
+		.driver_data = (unsigned long)&sdma_imx7d,
+	}, {
 		/* sentinel */
 	}
 };
@@ -511,6 +532,7 @@ static const struct of_device_id sdma_dt_ids[] = {
 	{ .compatible = "fsl,imx35-sdma", .data = &sdma_imx35, },
 	{ .compatible = "fsl,imx31-sdma", .data = &sdma_imx31, },
 	{ .compatible = "fsl,imx25-sdma", .data = &sdma_imx25, },
+	{ .compatible = "fsl,imx7d-sdma", .data = &sdma_imx7d, },
 	{ /* sentinel */ }
 };
 MODULE_DEVICE_TABLE(of, sdma_dt_ids);
@@ -648,15 +670,11 @@ static void sdma_event_disable(struct sdma_channel *sdmac, unsigned int event)
 	writel_relaxed(val, sdma->regs + chnenbl);
 }
 
-static void sdma_handle_channel_loop(struct sdma_channel *sdmac)
-{
-	if (sdmac->desc.callback)
-		sdmac->desc.callback(sdmac->desc.callback_param);
-}
-
 static void sdma_update_channel_loop(struct sdma_channel *sdmac)
 {
 	struct sdma_buffer_descriptor *bd;
+	int error = 0;
+	enum dma_status	old_status = sdmac->status;
 
 	/*
 	 * loop mode. Iterate over descriptors, re-setup them and
@@ -668,17 +686,41 @@ static void sdma_update_channel_loop(struct sdma_channel *sdmac)
 		if (bd->mode.status & BD_DONE)
 			break;
 
-		if (bd->mode.status & BD_RROR)
+		if (bd->mode.status & BD_RROR) {
+			bd->mode.status &= ~BD_RROR;
 			sdmac->status = DMA_ERROR;
+			error = -EIO;
+		}
 
+	       /*
+		* We use bd->mode.count to calculate the residue, since contains
+		* the number of bytes present in the current buffer descriptor.
+		*/
+
+		sdmac->chn_real_count = bd->mode.count;
 		bd->mode.status |= BD_DONE;
+		bd->mode.count = sdmac->period_len;
+
+		/*
+		 * The callback is called from the interrupt context in order
+		 * to reduce latency and to avoid the risk of altering the
+		 * SDMA transaction status by the time the client tasklet is
+		 * executed.
+		 */
+
+		dmaengine_desc_get_callback_invoke(&sdmac->desc, NULL);
+
 		sdmac->buf_tail++;
 		sdmac->buf_tail %= sdmac->num_bd;
+
+		if (error)
+			sdmac->status = old_status;
 	}
 }
 
-static void mxc_sdma_handle_channel_normal(struct sdma_channel *sdmac)
+static void mxc_sdma_handle_channel_normal(unsigned long data)
 {
+	struct sdma_channel *sdmac = (struct sdma_channel *) data;
 	struct sdma_buffer_descriptor *bd;
 	int i, error = 0;
 
@@ -701,18 +743,8 @@ static void mxc_sdma_handle_channel_normal(struct sdma_channel *sdmac)
 		sdmac->status = DMA_COMPLETE;
 
 	dma_cookie_complete(&sdmac->desc);
-	if (sdmac->desc.callback)
-		sdmac->desc.callback(sdmac->desc.callback_param);
-}
-
-static void sdma_tasklet(unsigned long data)
-{
-	struct sdma_channel *sdmac = (struct sdma_channel *) data;
 
-	if (sdmac->flags & IMX_DMA_SG_LOOP)
-		sdma_handle_channel_loop(sdmac);
-	else
-		mxc_sdma_handle_channel_normal(sdmac);
+	dmaengine_desc_get_callback_invoke(&sdmac->desc, NULL);
 }
 
 static irqreturn_t sdma_int_handler(int irq, void *dev_id)
@@ -731,8 +763,8 @@ static irqreturn_t sdma_int_handler(int irq, void *dev_id)
 
 		if (sdmac->flags & IMX_DMA_SG_LOOP)
 			sdma_update_channel_loop(sdmac);
-
-		tasklet_schedule(&sdmac->tasklet);
+		else
+			tasklet_schedule(&sdmac->tasklet);
 
 		__clear_bit(channel, &stat);
 	}
@@ -1353,7 +1385,8 @@ static enum dma_status sdma_tx_status(struct dma_chan *chan,
 	u32 residue;
 
 	if (sdmac->flags & IMX_DMA_SG_LOOP)
-		residue = (sdmac->num_bd - sdmac->buf_tail) * sdmac->period_len;
+		residue = (sdmac->num_bd - sdmac->buf_tail) *
+			   sdmac->period_len - sdmac->chn_real_count;
 	else
 		residue = sdmac->chn_count - sdmac->chn_real_count;
 
@@ -1375,6 +1408,7 @@ static void sdma_issue_pending(struct dma_chan *chan)
 #define SDMA_SCRIPT_ADDRS_ARRAY_SIZE_V1	34
 #define SDMA_SCRIPT_ADDRS_ARRAY_SIZE_V2	38
 #define SDMA_SCRIPT_ADDRS_ARRAY_SIZE_V3	41
+#define SDMA_SCRIPT_ADDRS_ARRAY_SIZE_V4	42
 
 static void sdma_add_scripts(struct sdma_engine *sdma,
 		const struct sdma_script_start_addrs *addr)
@@ -1424,6 +1458,9 @@ static void sdma_load_firmware(const struct firmware *fw, void *context)
 	case 3:
 		sdma->script_number = SDMA_SCRIPT_ADDRS_ARRAY_SIZE_V3;
 		break;
+	case 4:
+		sdma->script_number = SDMA_SCRIPT_ADDRS_ARRAY_SIZE_V4;
+		break;
 	default:
 		dev_err(sdma->dev, "unknown firmware version\n");
 		goto err_firmware;
@@ -1732,7 +1769,7 @@ static int sdma_probe(struct platform_device *pdev)
 		dma_cookie_init(&sdmac->chan);
 		sdmac->channel = i;
 
-		tasklet_init(&sdmac->tasklet, sdma_tasklet,
+		tasklet_init(&sdmac->tasklet, mxc_sdma_handle_channel_normal,
 			     (unsigned long) sdmac);
 		/*
 		 * Add the channel to the DMAC list. Do not add channel 0 though
diff --git a/drivers/dma/ioat/dma.c b/drivers/dma/ioat/dma.c
index bd09961443b1..49386ce04bf5 100644
--- a/drivers/dma/ioat/dma.c
+++ b/drivers/dma/ioat/dma.c
@@ -38,8 +38,54 @@
 
 #include "../dmaengine.h"
 
+static char *chanerr_str[] = {
+	"DMA Transfer Destination Address Error",
+	"Next Descriptor Address Error",
+	"Descriptor Error",
+	"Chan Address Value Error",
+	"CHANCMD Error",
+	"Chipset Uncorrectable Data Integrity Error",
+	"DMA Uncorrectable Data Integrity Error",
+	"Read Data Error",
+	"Write Data Error",
+	"Descriptor Control Error",
+	"Descriptor Transfer Size Error",
+	"Completion Address Error",
+	"Interrupt Configuration Error",
+	"Super extended descriptor Address Error",
+	"Unaffiliated Error",
+	"CRC or XOR P Error",
+	"XOR Q Error",
+	"Descriptor Count Error",
+	"DIF All F detect Error",
+	"Guard Tag verification Error",
+	"Application Tag verification Error",
+	"Reference Tag verification Error",
+	"Bundle Bit Error",
+	"Result DIF All F detect Error",
+	"Result Guard Tag verification Error",
+	"Result Application Tag verification Error",
+	"Result Reference Tag verification Error",
+	NULL
+};
+
 static void ioat_eh(struct ioatdma_chan *ioat_chan);
 
+static void ioat_print_chanerrs(struct ioatdma_chan *ioat_chan, u32 chanerr)
+{
+	int i;
+
+	for (i = 0; i < 32; i++) {
+		if ((chanerr >> i) & 1) {
+			if (chanerr_str[i]) {
+				dev_err(to_dev(ioat_chan), "Err(%d): %s\n",
+					i, chanerr_str[i]);
+			} else
+				break;
+		}
+	}
+}
+
 /**
  * ioat_dma_do_interrupt - handler used for single vector interrupt mode
  * @irq: interrupt id
@@ -568,12 +614,14 @@ static void __cleanup(struct ioatdma_chan *ioat_chan, dma_addr_t phys_complete)
 
 		tx = &desc->txd;
 		if (tx->cookie) {
+			struct dmaengine_result res;
+
 			dma_cookie_complete(tx);
 			dma_descriptor_unmap(tx);
-			if (tx->callback) {
-				tx->callback(tx->callback_param);
-				tx->callback = NULL;
-			}
+			res.result = DMA_TRANS_NOERROR;
+			dmaengine_desc_get_callback_invoke(tx, NULL);
+			tx->callback = NULL;
+			tx->callback_result = NULL;
 		}
 
 		if (tx->phys == phys_complete)
@@ -622,7 +670,8 @@ static void ioat_cleanup(struct ioatdma_chan *ioat_chan)
 	if (is_ioat_halted(*ioat_chan->completion)) {
 		u32 chanerr = readl(ioat_chan->reg_base + IOAT_CHANERR_OFFSET);
 
-		if (chanerr & IOAT_CHANERR_HANDLE_MASK) {
+		if (chanerr &
+		    (IOAT_CHANERR_HANDLE_MASK | IOAT_CHANERR_RECOVER_MASK)) {
 			mod_timer(&ioat_chan->timer, jiffies + IDLE_TIMEOUT);
 			ioat_eh(ioat_chan);
 		}
@@ -652,6 +701,61 @@ static void ioat_restart_channel(struct ioatdma_chan *ioat_chan)
 	__ioat_restart_chan(ioat_chan);
 }
 
+
+static void ioat_abort_descs(struct ioatdma_chan *ioat_chan)
+{
+	struct ioatdma_device *ioat_dma = ioat_chan->ioat_dma;
+	struct ioat_ring_ent *desc;
+	u16 active;
+	int idx = ioat_chan->tail, i;
+
+	/*
+	 * We assume that the failed descriptor has been processed.
+	 * Now we are just returning all the remaining submitted
+	 * descriptors to abort.
+	 */
+	active = ioat_ring_active(ioat_chan);
+
+	/* we skip the failed descriptor that tail points to */
+	for (i = 1; i < active; i++) {
+		struct dma_async_tx_descriptor *tx;
+
+		smp_read_barrier_depends();
+		prefetch(ioat_get_ring_ent(ioat_chan, idx + i + 1));
+		desc = ioat_get_ring_ent(ioat_chan, idx + i);
+
+		tx = &desc->txd;
+		if (tx->cookie) {
+			struct dmaengine_result res;
+
+			dma_cookie_complete(tx);
+			dma_descriptor_unmap(tx);
+			res.result = DMA_TRANS_ABORTED;
+			dmaengine_desc_get_callback_invoke(tx, &res);
+			tx->callback = NULL;
+			tx->callback_result = NULL;
+		}
+
+		/* skip extended descriptors */
+		if (desc_has_ext(desc)) {
+			WARN_ON(i + 1 >= active);
+			i++;
+		}
+
+		/* cleanup super extended descriptors */
+		if (desc->sed) {
+			ioat_free_sed(ioat_dma, desc->sed);
+			desc->sed = NULL;
+		}
+	}
+
+	smp_mb(); /* finish all descriptor reads before incrementing tail */
+	ioat_chan->tail = idx + active;
+
+	desc = ioat_get_ring_ent(ioat_chan, ioat_chan->tail);
+	ioat_chan->last_completion = *ioat_chan->completion = desc->txd.phys;
+}
+
 static void ioat_eh(struct ioatdma_chan *ioat_chan)
 {
 	struct pci_dev *pdev = to_pdev(ioat_chan);
@@ -662,6 +766,8 @@ static void ioat_eh(struct ioatdma_chan *ioat_chan)
 	u32 err_handled = 0;
 	u32 chanerr_int;
 	u32 chanerr;
+	bool abort = false;
+	struct dmaengine_result res;
 
 	/* cleanup so tail points to descriptor that caused the error */
 	if (ioat_cleanup_preamble(ioat_chan, &phys_complete))
@@ -697,30 +803,55 @@ static void ioat_eh(struct ioatdma_chan *ioat_chan)
 		break;
 	}
 
+	if (chanerr & IOAT_CHANERR_RECOVER_MASK) {
+		if (chanerr & IOAT_CHANERR_READ_DATA_ERR) {
+			res.result = DMA_TRANS_READ_FAILED;
+			err_handled |= IOAT_CHANERR_READ_DATA_ERR;
+		} else if (chanerr & IOAT_CHANERR_WRITE_DATA_ERR) {
+			res.result = DMA_TRANS_WRITE_FAILED;
+			err_handled |= IOAT_CHANERR_WRITE_DATA_ERR;
+		}
+
+		abort = true;
+	} else
+		res.result = DMA_TRANS_NOERROR;
+
 	/* fault on unhandled error or spurious halt */
 	if (chanerr ^ err_handled || chanerr == 0) {
 		dev_err(to_dev(ioat_chan), "%s: fatal error (%x:%x)\n",
 			__func__, chanerr, err_handled);
+		dev_err(to_dev(ioat_chan), "Errors handled:\n");
+		ioat_print_chanerrs(ioat_chan, err_handled);
+		dev_err(to_dev(ioat_chan), "Errors not handled:\n");
+		ioat_print_chanerrs(ioat_chan, (chanerr & ~err_handled));
+
 		BUG();
-	} else { /* cleanup the faulty descriptor */
-		tx = &desc->txd;
-		if (tx->cookie) {
-			dma_cookie_complete(tx);
-			dma_descriptor_unmap(tx);
-			if (tx->callback) {
-				tx->callback(tx->callback_param);
-				tx->callback = NULL;
-			}
-		}
 	}
 
-	writel(chanerr, ioat_chan->reg_base + IOAT_CHANERR_OFFSET);
-	pci_write_config_dword(pdev, IOAT_PCI_CHANERR_INT_OFFSET, chanerr_int);
+	/* cleanup the faulty descriptor since we are continuing */
+	tx = &desc->txd;
+	if (tx->cookie) {
+		dma_cookie_complete(tx);
+		dma_descriptor_unmap(tx);
+		dmaengine_desc_get_callback_invoke(tx, &res);
+		tx->callback = NULL;
+		tx->callback_result = NULL;
+	}
 
 	/* mark faulting descriptor as complete */
 	*ioat_chan->completion = desc->txd.phys;
 
 	spin_lock_bh(&ioat_chan->prep_lock);
+	/* we need abort all descriptors */
+	if (abort) {
+		ioat_abort_descs(ioat_chan);
+		/* clean up the channel, we could be in weird state */
+		ioat_reset_hw(ioat_chan);
+	}
+
+	writel(chanerr, ioat_chan->reg_base + IOAT_CHANERR_OFFSET);
+	pci_write_config_dword(pdev, IOAT_PCI_CHANERR_INT_OFFSET, chanerr_int);
+
 	ioat_restart_channel(ioat_chan);
 	spin_unlock_bh(&ioat_chan->prep_lock);
 }
@@ -753,10 +884,28 @@ void ioat_timer_event(unsigned long data)
 		chanerr = readl(ioat_chan->reg_base + IOAT_CHANERR_OFFSET);
 		dev_err(to_dev(ioat_chan), "%s: Channel halted (%x)\n",
 			__func__, chanerr);
-		if (test_bit(IOAT_RUN, &ioat_chan->state))
-			BUG_ON(is_ioat_bug(chanerr));
-		else /* we never got off the ground */
-			return;
+		dev_err(to_dev(ioat_chan), "Errors:\n");
+		ioat_print_chanerrs(ioat_chan, chanerr);
+
+		if (test_bit(IOAT_RUN, &ioat_chan->state)) {
+			spin_lock_bh(&ioat_chan->cleanup_lock);
+			spin_lock_bh(&ioat_chan->prep_lock);
+			set_bit(IOAT_CHAN_DOWN, &ioat_chan->state);
+			spin_unlock_bh(&ioat_chan->prep_lock);
+
+			ioat_abort_descs(ioat_chan);
+			dev_warn(to_dev(ioat_chan), "Reset channel...\n");
+			ioat_reset_hw(ioat_chan);
+			dev_warn(to_dev(ioat_chan), "Restart channel...\n");
+			ioat_restart_channel(ioat_chan);
+
+			spin_lock_bh(&ioat_chan->prep_lock);
+			clear_bit(IOAT_CHAN_DOWN, &ioat_chan->state);
+			spin_unlock_bh(&ioat_chan->prep_lock);
+			spin_unlock_bh(&ioat_chan->cleanup_lock);
+		}
+
+		return;
 	}
 
 	spin_lock_bh(&ioat_chan->cleanup_lock);
@@ -780,14 +929,26 @@ void ioat_timer_event(unsigned long data)
 		u32 chanerr;
 
 		chanerr = readl(ioat_chan->reg_base + IOAT_CHANERR_OFFSET);
-		dev_warn(to_dev(ioat_chan), "Restarting channel...\n");
-		dev_warn(to_dev(ioat_chan), "CHANSTS: %#Lx CHANERR: %#x\n",
-			 status, chanerr);
-		dev_warn(to_dev(ioat_chan), "Active descriptors: %d\n",
-			 ioat_ring_active(ioat_chan));
+		dev_err(to_dev(ioat_chan), "CHANSTS: %#Lx CHANERR: %#x\n",
+			status, chanerr);
+		dev_err(to_dev(ioat_chan), "Errors:\n");
+		ioat_print_chanerrs(ioat_chan, chanerr);
+
+		dev_dbg(to_dev(ioat_chan), "Active descriptors: %d\n",
+			ioat_ring_active(ioat_chan));
 
 		spin_lock_bh(&ioat_chan->prep_lock);
+		set_bit(IOAT_CHAN_DOWN, &ioat_chan->state);
+		spin_unlock_bh(&ioat_chan->prep_lock);
+
+		ioat_abort_descs(ioat_chan);
+		dev_warn(to_dev(ioat_chan), "Resetting channel...\n");
+		ioat_reset_hw(ioat_chan);
+		dev_warn(to_dev(ioat_chan), "Restarting channel...\n");
 		ioat_restart_channel(ioat_chan);
+
+		spin_lock_bh(&ioat_chan->prep_lock);
+		clear_bit(IOAT_CHAN_DOWN, &ioat_chan->state);
 		spin_unlock_bh(&ioat_chan->prep_lock);
 		spin_unlock_bh(&ioat_chan->cleanup_lock);
 		return;
diff --git a/drivers/dma/ioat/init.c b/drivers/dma/ioat/init.c
index 7145f7716a92..015f7110b96d 100644
--- a/drivers/dma/ioat/init.c
+++ b/drivers/dma/ioat/init.c
@@ -828,7 +828,7 @@ static int ioat_xor_val_self_test(struct ioatdma_device *ioat_dma)
 
 	dest_dma = dma_map_page(dev, dest, 0, PAGE_SIZE, DMA_FROM_DEVICE);
 	if (dma_mapping_error(dev, dest_dma))
-		goto dma_unmap;
+		goto free_resources;
 
 	for (i = 0; i < IOAT_NUM_SRC_TEST; i++)
 		dma_srcs[i] = DMA_ERROR_CODE;
diff --git a/drivers/dma/ioat/registers.h b/drivers/dma/ioat/registers.h
index 70534981a49b..48fa4cf9f64a 100644
--- a/drivers/dma/ioat/registers.h
+++ b/drivers/dma/ioat/registers.h
@@ -240,6 +240,8 @@
 #define IOAT_CHANERR_DESCRIPTOR_COUNT_ERR	0x40000
 
 #define IOAT_CHANERR_HANDLE_MASK (IOAT_CHANERR_XOR_P_OR_CRC_ERR | IOAT_CHANERR_XOR_Q_ERR)
+#define IOAT_CHANERR_RECOVER_MASK (IOAT_CHANERR_READ_DATA_ERR | \
+				   IOAT_CHANERR_WRITE_DATA_ERR)
 
 #define IOAT_CHANERR_MASK_OFFSET		0x2C	/* 32-bit Channel Error Register */
 
diff --git a/drivers/dma/iop-adma.c b/drivers/dma/iop-adma.c
index f039cfadf17b..a410657f7bcd 100644
--- a/drivers/dma/iop-adma.c
+++ b/drivers/dma/iop-adma.c
@@ -71,8 +71,7 @@ iop_adma_run_tx_complete_actions(struct iop_adma_desc_slot *desc,
 		/* call the callback (must not sleep or submit new
 		 * operations to this channel)
 		 */
-		if (tx->callback)
-			tx->callback(tx->callback_param);
+		dmaengine_desc_get_callback_invoke(tx, NULL);
 
 		dma_descriptor_unmap(tx);
 		if (desc->group_head)
diff --git a/drivers/dma/ipu/ipu_idmac.c b/drivers/dma/ipu/ipu_idmac.c
index b54f62de9232..ed76044ce4b9 100644
--- a/drivers/dma/ipu/ipu_idmac.c
+++ b/drivers/dma/ipu/ipu_idmac.c
@@ -1160,11 +1160,10 @@ static irqreturn_t idmac_interrupt(int irq, void *dev_id)
 	struct scatterlist **sg, *sgnext, *sgnew = NULL;
 	/* Next transfer descriptor */
 	struct idmac_tx_desc *desc, *descnew;
-	dma_async_tx_callback callback;
-	void *callback_param;
 	bool done = false;
 	u32 ready0, ready1, curbuf, err;
 	unsigned long flags;
+	struct dmaengine_desc_callback cb;
 
 	/* IDMAC has cleared the respective BUFx_RDY bit, we manage the buffer */
 
@@ -1278,12 +1277,12 @@ static irqreturn_t idmac_interrupt(int irq, void *dev_id)
 
 	if (likely(sgnew) &&
 	    ipu_submit_buffer(ichan, descnew, sgnew, ichan->active_buffer) < 0) {
-		callback = descnew->txd.callback;
-		callback_param = descnew->txd.callback_param;
+		dmaengine_desc_get_callback(&descnew->txd, &cb);
+
 		list_del_init(&descnew->list);
 		spin_unlock(&ichan->lock);
-		if (callback)
-			callback(callback_param);
+
+		dmaengine_desc_callback_invoke(&cb, NULL);
 		spin_lock(&ichan->lock);
 	}
 
@@ -1292,13 +1291,12 @@ static irqreturn_t idmac_interrupt(int irq, void *dev_id)
 	if (done)
 		dma_cookie_complete(&desc->txd);
 
-	callback = desc->txd.callback;
-	callback_param = desc->txd.callback_param;
+	dmaengine_desc_get_callback(&desc->txd, &cb);
 
 	spin_unlock(&ichan->lock);
 
-	if (done && (desc->txd.flags & DMA_PREP_INTERRUPT) && callback)
-		callback(callback_param);
+	if (done && (desc->txd.flags & DMA_PREP_INTERRUPT))
+		dmaengine_desc_callback_invoke(&cb, NULL);
 
 	return IRQ_HANDLED;
 }
diff --git a/drivers/dma/ipu/ipu_irq.c b/drivers/dma/ipu/ipu_irq.c
index 2bf37e68ad0f..dd184b50e5b4 100644
--- a/drivers/dma/ipu/ipu_irq.c
+++ b/drivers/dma/ipu/ipu_irq.c
@@ -286,22 +286,21 @@ static void ipu_irq_handler(struct irq_desc *desc)
 		raw_spin_unlock(&bank_lock);
 		while ((line = ffs(status))) {
 			struct ipu_irq_map *map;
-			unsigned int irq = NO_IRQ;
+			unsigned int irq;
 
 			line--;
 			status &= ~(1UL << line);
 
 			raw_spin_lock(&bank_lock);
 			map = src2map(32 * i + line);
-			if (map)
-				irq = map->irq;
-			raw_spin_unlock(&bank_lock);
-
 			if (!map) {
+				raw_spin_unlock(&bank_lock);
 				pr_err("IPU: Interrupt on unmapped source %u bank %d\n",
 				       line, i);
 				continue;
 			}
+			irq = map->irq;
+			raw_spin_unlock(&bank_lock);
 			generic_handle_irq(irq);
 		}
 	}
diff --git a/drivers/dma/k3dma.c b/drivers/dma/k3dma.c
index 39de8980128c..aabcb7934b05 100644
--- a/drivers/dma/k3dma.c
+++ b/drivers/dma/k3dma.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013 Linaro Ltd.
+ * Copyright (c) 2013 - 2015 Linaro Ltd.
  * Copyright (c) 2013 Hisilicon Limited.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -8,6 +8,8 @@
  */
 #include <linux/sched.h>
 #include <linux/device.h>
+#include <linux/dma-mapping.h>
+#include <linux/dmapool.h>
 #include <linux/dmaengine.h>
 #include <linux/init.h>
 #include <linux/interrupt.h>
@@ -25,22 +27,28 @@
 
 #define DRIVER_NAME		"k3-dma"
 #define DMA_MAX_SIZE		0x1ffc
+#define DMA_CYCLIC_MAX_PERIOD	0x1000
+#define LLI_BLOCK_SIZE		(4 * PAGE_SIZE)
 
 #define INT_STAT		0x00
 #define INT_TC1			0x04
+#define INT_TC2			0x08
 #define INT_ERR1		0x0c
 #define INT_ERR2		0x10
 #define INT_TC1_MASK		0x18
+#define INT_TC2_MASK		0x1c
 #define INT_ERR1_MASK		0x20
 #define INT_ERR2_MASK		0x24
 #define INT_TC1_RAW		0x600
-#define INT_ERR1_RAW		0x608
-#define INT_ERR2_RAW		0x610
+#define INT_TC2_RAW		0x608
+#define INT_ERR1_RAW		0x610
+#define INT_ERR2_RAW		0x618
 #define CH_PRI			0x688
 #define CH_STAT			0x690
 #define CX_CUR_CNT		0x704
 #define CX_LLI			0x800
-#define CX_CNT			0x810
+#define CX_CNT1			0x80c
+#define CX_CNT0			0x810
 #define CX_SRC			0x814
 #define CX_DST			0x818
 #define CX_CFG			0x81c
@@ -49,6 +57,7 @@
 
 #define CX_LLI_CHAIN_EN		0x2
 #define CX_CFG_EN		0x1
+#define CX_CFG_NODEIRQ		BIT(1)
 #define CX_CFG_MEM2PER		(0x1 << 2)
 #define CX_CFG_PER2MEM		(0x2 << 2)
 #define CX_CFG_SRCINCR		(0x1 << 31)
@@ -68,7 +77,7 @@ struct k3_dma_desc_sw {
 	dma_addr_t		desc_hw_lli;
 	size_t			desc_num;
 	size_t			size;
-	struct k3_desc_hw	desc_hw[0];
+	struct k3_desc_hw	*desc_hw;
 };
 
 struct k3_dma_phy;
@@ -81,6 +90,7 @@ struct k3_dma_chan {
 	enum dma_transfer_direction dir;
 	dma_addr_t		dev_addr;
 	enum dma_status		status;
+	bool			cyclic;
 };
 
 struct k3_dma_phy {
@@ -100,6 +110,7 @@ struct k3_dma_dev {
 	struct k3_dma_phy	*phy;
 	struct k3_dma_chan	*chans;
 	struct clk		*clk;
+	struct dma_pool		*pool;
 	u32			dma_channels;
 	u32			dma_requests;
 	unsigned int		irq;
@@ -135,6 +146,7 @@ static void k3_dma_terminate_chan(struct k3_dma_phy *phy, struct k3_dma_dev *d)
 
 	val = 0x1 << phy->idx;
 	writel_relaxed(val, d->base + INT_TC1_RAW);
+	writel_relaxed(val, d->base + INT_TC2_RAW);
 	writel_relaxed(val, d->base + INT_ERR1_RAW);
 	writel_relaxed(val, d->base + INT_ERR2_RAW);
 }
@@ -142,7 +154,7 @@ static void k3_dma_terminate_chan(struct k3_dma_phy *phy, struct k3_dma_dev *d)
 static void k3_dma_set_desc(struct k3_dma_phy *phy, struct k3_desc_hw *hw)
 {
 	writel_relaxed(hw->lli, phy->base + CX_LLI);
-	writel_relaxed(hw->count, phy->base + CX_CNT);
+	writel_relaxed(hw->count, phy->base + CX_CNT0);
 	writel_relaxed(hw->saddr, phy->base + CX_SRC);
 	writel_relaxed(hw->daddr, phy->base + CX_DST);
 	writel_relaxed(AXI_CFG_DEFAULT, phy->base + AXI_CFG);
@@ -176,11 +188,13 @@ static void k3_dma_enable_dma(struct k3_dma_dev *d, bool on)
 
 		/* unmask irq */
 		writel_relaxed(0xffff, d->base + INT_TC1_MASK);
+		writel_relaxed(0xffff, d->base + INT_TC2_MASK);
 		writel_relaxed(0xffff, d->base + INT_ERR1_MASK);
 		writel_relaxed(0xffff, d->base + INT_ERR2_MASK);
 	} else {
 		/* mask irq */
 		writel_relaxed(0x0, d->base + INT_TC1_MASK);
+		writel_relaxed(0x0, d->base + INT_TC2_MASK);
 		writel_relaxed(0x0, d->base + INT_ERR1_MASK);
 		writel_relaxed(0x0, d->base + INT_ERR2_MASK);
 	}
@@ -193,22 +207,31 @@ static irqreturn_t k3_dma_int_handler(int irq, void *dev_id)
 	struct k3_dma_chan *c;
 	u32 stat = readl_relaxed(d->base + INT_STAT);
 	u32 tc1  = readl_relaxed(d->base + INT_TC1);
+	u32 tc2  = readl_relaxed(d->base + INT_TC2);
 	u32 err1 = readl_relaxed(d->base + INT_ERR1);
 	u32 err2 = readl_relaxed(d->base + INT_ERR2);
 	u32 i, irq_chan = 0;
 
 	while (stat) {
 		i = __ffs(stat);
-		stat &= (stat - 1);
-		if (likely(tc1 & BIT(i))) {
+		stat &= ~BIT(i);
+		if (likely(tc1 & BIT(i)) || (tc2 & BIT(i))) {
+			unsigned long flags;
+
 			p = &d->phy[i];
 			c = p->vchan;
-			if (c) {
-				unsigned long flags;
-
+			if (c && (tc1 & BIT(i))) {
 				spin_lock_irqsave(&c->vc.lock, flags);
 				vchan_cookie_complete(&p->ds_run->vd);
+				WARN_ON_ONCE(p->ds_done);
 				p->ds_done = p->ds_run;
+				p->ds_run = NULL;
+				spin_unlock_irqrestore(&c->vc.lock, flags);
+			}
+			if (c && (tc2 & BIT(i))) {
+				spin_lock_irqsave(&c->vc.lock, flags);
+				if (p->ds_run != NULL)
+					vchan_cyclic_callback(&p->ds_run->vd);
 				spin_unlock_irqrestore(&c->vc.lock, flags);
 			}
 			irq_chan |= BIT(i);
@@ -218,14 +241,17 @@ static irqreturn_t k3_dma_int_handler(int irq, void *dev_id)
 	}
 
 	writel_relaxed(irq_chan, d->base + INT_TC1_RAW);
+	writel_relaxed(irq_chan, d->base + INT_TC2_RAW);
 	writel_relaxed(err1, d->base + INT_ERR1_RAW);
 	writel_relaxed(err2, d->base + INT_ERR2_RAW);
 
-	if (irq_chan) {
+	if (irq_chan)
 		tasklet_schedule(&d->task);
+
+	if (irq_chan || err1 || err2)
 		return IRQ_HANDLED;
-	} else
-		return IRQ_NONE;
+
+	return IRQ_NONE;
 }
 
 static int k3_dma_start_txd(struct k3_dma_chan *c)
@@ -247,14 +273,14 @@ static int k3_dma_start_txd(struct k3_dma_chan *c)
 		 * so vc->desc_issued only contains desc pending
 		 */
 		list_del(&ds->vd.node);
+
+		WARN_ON_ONCE(c->phy->ds_run);
+		WARN_ON_ONCE(c->phy->ds_done);
 		c->phy->ds_run = ds;
-		c->phy->ds_done = NULL;
 		/* start dma */
 		k3_dma_set_desc(c->phy, &ds->desc_hw[0]);
 		return 0;
 	}
-	c->phy->ds_done = NULL;
-	c->phy->ds_run = NULL;
 	return -EAGAIN;
 }
 
@@ -351,7 +377,7 @@ static enum dma_status k3_dma_tx_status(struct dma_chan *chan,
 	 * its total size.
 	 */
 	vd = vchan_find_desc(&c->vc, cookie);
-	if (vd) {
+	if (vd && !c->cyclic) {
 		bytes = container_of(vd, struct k3_dma_desc_sw, vd)->size;
 	} else if ((!p) || (!p->ds_run)) {
 		bytes = 0;
@@ -361,7 +387,8 @@ static enum dma_status k3_dma_tx_status(struct dma_chan *chan,
 
 		bytes = k3_dma_get_curr_cnt(d, p);
 		clli = k3_dma_get_curr_lli(p);
-		index = (clli - ds->desc_hw_lli) / sizeof(struct k3_desc_hw);
+		index = ((clli - ds->desc_hw_lli) /
+				sizeof(struct k3_desc_hw)) + 1;
 		for (; index < ds->desc_num; index++) {
 			bytes += ds->desc_hw[index].count;
 			/* end of lli */
@@ -402,9 +429,10 @@ static void k3_dma_issue_pending(struct dma_chan *chan)
 static void k3_dma_fill_desc(struct k3_dma_desc_sw *ds, dma_addr_t dst,
 			dma_addr_t src, size_t len, u32 num, u32 ccfg)
 {
-	if ((num + 1) < ds->desc_num)
+	if (num != ds->desc_num - 1)
 		ds->desc_hw[num].lli = ds->desc_hw_lli + (num + 1) *
 			sizeof(struct k3_desc_hw);
+
 	ds->desc_hw[num].lli |= CX_LLI_CHAIN_EN;
 	ds->desc_hw[num].count = len;
 	ds->desc_hw[num].saddr = src;
@@ -412,6 +440,35 @@ static void k3_dma_fill_desc(struct k3_dma_desc_sw *ds, dma_addr_t dst,
 	ds->desc_hw[num].config = ccfg;
 }
 
+static struct k3_dma_desc_sw *k3_dma_alloc_desc_resource(int num,
+							struct dma_chan *chan)
+{
+	struct k3_dma_chan *c = to_k3_chan(chan);
+	struct k3_dma_desc_sw *ds;
+	struct k3_dma_dev *d = to_k3_dma(chan->device);
+	int lli_limit = LLI_BLOCK_SIZE / sizeof(struct k3_desc_hw);
+
+	if (num > lli_limit) {
+		dev_dbg(chan->device->dev, "vch %p: sg num %d exceed max %d\n",
+			&c->vc, num, lli_limit);
+		return NULL;
+	}
+
+	ds = kzalloc(sizeof(*ds), GFP_NOWAIT);
+	if (!ds)
+		return NULL;
+
+	ds->desc_hw = dma_pool_alloc(d->pool, GFP_NOWAIT, &ds->desc_hw_lli);
+	if (!ds->desc_hw) {
+		dev_dbg(chan->device->dev, "vch %p: dma alloc fail\n", &c->vc);
+		kfree(ds);
+		return NULL;
+	}
+	memset(ds->desc_hw, 0, sizeof(struct k3_desc_hw) * num);
+	ds->desc_num = num;
+	return ds;
+}
+
 static struct dma_async_tx_descriptor *k3_dma_prep_memcpy(
 	struct dma_chan *chan,	dma_addr_t dst, dma_addr_t src,
 	size_t len, unsigned long flags)
@@ -425,13 +482,13 @@ static struct dma_async_tx_descriptor *k3_dma_prep_memcpy(
 		return NULL;
 
 	num = DIV_ROUND_UP(len, DMA_MAX_SIZE);
-	ds = kzalloc(sizeof(*ds) + num * sizeof(ds->desc_hw[0]), GFP_ATOMIC);
+
+	ds = k3_dma_alloc_desc_resource(num, chan);
 	if (!ds)
 		return NULL;
 
-	ds->desc_hw_lli = __virt_to_phys((unsigned long)&ds->desc_hw[0]);
+	c->cyclic = 0;
 	ds->size = len;
-	ds->desc_num = num;
 	num = 0;
 
 	if (!c->ccfg) {
@@ -474,18 +531,17 @@ static struct dma_async_tx_descriptor *k3_dma_prep_slave_sg(
 	if (sgl == NULL)
 		return NULL;
 
+	c->cyclic = 0;
+
 	for_each_sg(sgl, sg, sglen, i) {
 		avail = sg_dma_len(sg);
 		if (avail > DMA_MAX_SIZE)
 			num += DIV_ROUND_UP(avail, DMA_MAX_SIZE) - 1;
 	}
 
-	ds = kzalloc(sizeof(*ds) + num * sizeof(ds->desc_hw[0]), GFP_ATOMIC);
+	ds = k3_dma_alloc_desc_resource(num, chan);
 	if (!ds)
 		return NULL;
-
-	ds->desc_hw_lli = __virt_to_phys((unsigned long)&ds->desc_hw[0]);
-	ds->desc_num = num;
 	num = 0;
 
 	for_each_sg(sgl, sg, sglen, i) {
@@ -516,6 +572,73 @@ static struct dma_async_tx_descriptor *k3_dma_prep_slave_sg(
 	return vchan_tx_prep(&c->vc, &ds->vd, flags);
 }
 
+static struct dma_async_tx_descriptor *
+k3_dma_prep_dma_cyclic(struct dma_chan *chan, dma_addr_t buf_addr,
+		       size_t buf_len, size_t period_len,
+		       enum dma_transfer_direction dir,
+		       unsigned long flags)
+{
+	struct k3_dma_chan *c = to_k3_chan(chan);
+	struct k3_dma_desc_sw *ds;
+	size_t len, avail, total = 0;
+	dma_addr_t addr, src = 0, dst = 0;
+	int num = 1, since = 0;
+	size_t modulo = DMA_CYCLIC_MAX_PERIOD;
+	u32 en_tc2 = 0;
+
+	dev_dbg(chan->device->dev, "%s: buf %pad, dst %pad, buf len %zu, period_len = %zu, dir %d\n",
+	       __func__, &buf_addr, &to_k3_chan(chan)->dev_addr,
+	       buf_len, period_len, (int)dir);
+
+	avail = buf_len;
+	if (avail > modulo)
+		num += DIV_ROUND_UP(avail, modulo) - 1;
+
+	ds = k3_dma_alloc_desc_resource(num, chan);
+	if (!ds)
+		return NULL;
+
+	c->cyclic = 1;
+	addr = buf_addr;
+	avail = buf_len;
+	total = avail;
+	num = 0;
+
+	if (period_len < modulo)
+		modulo = period_len;
+
+	do {
+		len = min_t(size_t, avail, modulo);
+
+		if (dir == DMA_MEM_TO_DEV) {
+			src = addr;
+			dst = c->dev_addr;
+		} else if (dir == DMA_DEV_TO_MEM) {
+			src = c->dev_addr;
+			dst = addr;
+		}
+		since += len;
+		if (since >= period_len) {
+			/* descriptor asks for TC2 interrupt on completion */
+			en_tc2 = CX_CFG_NODEIRQ;
+			since -= period_len;
+		} else
+			en_tc2 = 0;
+
+		k3_dma_fill_desc(ds, dst, src, len, num++, c->ccfg | en_tc2);
+
+		addr += len;
+		avail -= len;
+	} while (avail);
+
+	/* "Cyclic" == end of link points back to start of link */
+	ds->desc_hw[num - 1].lli |= ds->desc_hw_lli;
+
+	ds->size = total;
+
+	return vchan_tx_prep(&c->vc, &ds->vd, flags);
+}
+
 static int k3_dma_config(struct dma_chan *chan,
 			 struct dma_slave_config *cfg)
 {
@@ -551,7 +674,7 @@ static int k3_dma_config(struct dma_chan *chan,
 	c->ccfg |= (val << 12) | (val << 16);
 
 	if ((maxburst == 0) || (maxburst > 16))
-		val = 16;
+		val = 15;
 	else
 		val = maxburst - 1;
 	c->ccfg |= (val << 20) | (val << 24);
@@ -563,6 +686,16 @@ static int k3_dma_config(struct dma_chan *chan,
 	return 0;
 }
 
+static void k3_dma_free_desc(struct virt_dma_desc *vd)
+{
+	struct k3_dma_desc_sw *ds =
+		container_of(vd, struct k3_dma_desc_sw, vd);
+	struct k3_dma_dev *d = to_k3_dma(vd->tx.chan->device);
+
+	dma_pool_free(d->pool, ds->desc_hw, ds->desc_hw_lli);
+	kfree(ds);
+}
+
 static int k3_dma_terminate_all(struct dma_chan *chan)
 {
 	struct k3_dma_chan *c = to_k3_chan(chan);
@@ -586,7 +719,15 @@ static int k3_dma_terminate_all(struct dma_chan *chan)
 		k3_dma_terminate_chan(p, d);
 		c->phy = NULL;
 		p->vchan = NULL;
-		p->ds_run = p->ds_done = NULL;
+		if (p->ds_run) {
+			k3_dma_free_desc(&p->ds_run->vd);
+			p->ds_run = NULL;
+		}
+		if (p->ds_done) {
+			k3_dma_free_desc(&p->ds_done->vd);
+			p->ds_done = NULL;
+		}
+
 	}
 	spin_unlock_irqrestore(&c->vc.lock, flags);
 	vchan_dma_desc_free_list(&c->vc, &head);
@@ -639,14 +780,6 @@ static int k3_dma_transfer_resume(struct dma_chan *chan)
 	return 0;
 }
 
-static void k3_dma_free_desc(struct virt_dma_desc *vd)
-{
-	struct k3_dma_desc_sw *ds =
-		container_of(vd, struct k3_dma_desc_sw, vd);
-
-	kfree(ds);
-}
-
 static const struct of_device_id k3_pdma_dt_ids[] = {
 	{ .compatible = "hisilicon,k3-dma-1.0", },
 	{}
@@ -706,6 +839,12 @@ static int k3_dma_probe(struct platform_device *op)
 
 	d->irq = irq;
 
+	/* A DMA memory pool for LLIs, align on 32-byte boundary */
+	d->pool = dmam_pool_create(DRIVER_NAME, &op->dev,
+					LLI_BLOCK_SIZE, 32, 0);
+	if (!d->pool)
+		return -ENOMEM;
+
 	/* init phy channel */
 	d->phy = devm_kzalloc(&op->dev,
 		d->dma_channels * sizeof(struct k3_dma_phy), GFP_KERNEL);
@@ -722,11 +861,13 @@ static int k3_dma_probe(struct platform_device *op)
 	INIT_LIST_HEAD(&d->slave.channels);
 	dma_cap_set(DMA_SLAVE, d->slave.cap_mask);
 	dma_cap_set(DMA_MEMCPY, d->slave.cap_mask);
+	dma_cap_set(DMA_CYCLIC, d->slave.cap_mask);
 	d->slave.dev = &op->dev;
 	d->slave.device_free_chan_resources = k3_dma_free_chan_resources;
 	d->slave.device_tx_status = k3_dma_tx_status;
 	d->slave.device_prep_dma_memcpy = k3_dma_prep_memcpy;
 	d->slave.device_prep_slave_sg = k3_dma_prep_slave_sg;
+	d->slave.device_prep_dma_cyclic = k3_dma_prep_dma_cyclic;
 	d->slave.device_issue_pending = k3_dma_issue_pending;
 	d->slave.device_config = k3_dma_config;
 	d->slave.device_pause = k3_dma_transfer_pause;
diff --git a/drivers/dma/mic_x100_dma.c b/drivers/dma/mic_x100_dma.c
index 1502b24b7c7d..818255844a3c 100644
--- a/drivers/dma/mic_x100_dma.c
+++ b/drivers/dma/mic_x100_dma.c
@@ -104,10 +104,8 @@ static void mic_dma_cleanup(struct mic_dma_chan *ch)
 		tx = &ch->tx_array[last_tail];
 		if (tx->cookie) {
 			dma_cookie_complete(tx);
-			if (tx->callback) {
-				tx->callback(tx->callback_param);
-				tx->callback = NULL;
-			}
+			dmaengine_desc_get_callback_invoke(tx, NULL);
+			tx->callback = NULL;
 		}
 		last_tail = mic_dma_hw_ring_inc(last_tail);
 	}
diff --git a/drivers/dma/mmp_pdma.c b/drivers/dma/mmp_pdma.c
index f4b25fb0d040..eb3a1f42ab06 100644
--- a/drivers/dma/mmp_pdma.c
+++ b/drivers/dma/mmp_pdma.c
@@ -864,19 +864,15 @@ static void dma_do_tasklet(unsigned long data)
 	struct mmp_pdma_desc_sw *desc, *_desc;
 	LIST_HEAD(chain_cleanup);
 	unsigned long flags;
+	struct dmaengine_desc_callback cb;
 
 	if (chan->cyclic_first) {
-		dma_async_tx_callback cb = NULL;
-		void *cb_data = NULL;
-
 		spin_lock_irqsave(&chan->desc_lock, flags);
 		desc = chan->cyclic_first;
-		cb = desc->async_tx.callback;
-		cb_data = desc->async_tx.callback_param;
+		dmaengine_desc_get_callback(&desc->async_tx, &cb);
 		spin_unlock_irqrestore(&chan->desc_lock, flags);
 
-		if (cb)
-			cb(cb_data);
+		dmaengine_desc_callback_invoke(&cb, NULL);
 
 		return;
 	}
@@ -921,8 +917,8 @@ static void dma_do_tasklet(unsigned long data)
 		/* Remove from the list of transactions */
 		list_del(&desc->node);
 		/* Run the link descriptor callback function */
-		if (txd->callback)
-			txd->callback(txd->callback_param);
+		dmaengine_desc_get_callback(txd, &cb);
+		dmaengine_desc_callback_invoke(&cb, NULL);
 
 		dma_pool_free(chan->desc_pool, desc, txd->phys);
 	}
diff --git a/drivers/dma/mmp_tdma.c b/drivers/dma/mmp_tdma.c
index b3441f57a364..13c68b6434ce 100644
--- a/drivers/dma/mmp_tdma.c
+++ b/drivers/dma/mmp_tdma.c
@@ -349,9 +349,7 @@ static void dma_do_tasklet(unsigned long data)
 {
 	struct mmp_tdma_chan *tdmac = (struct mmp_tdma_chan *)data;
 
-	if (tdmac->desc.callback)
-		tdmac->desc.callback(tdmac->desc.callback_param);
-
+	dmaengine_desc_get_callback_invoke(&tdmac->desc, NULL);
 }
 
 static void mmp_tdma_free_descriptor(struct mmp_tdma_chan *tdmac)
@@ -433,7 +431,7 @@ static struct dma_async_tx_descriptor *mmp_tdma_prep_dma_cyclic(
 
 	if (period_len > TDMA_MAX_XFER_BYTES) {
 		dev_err(tdmac->dev,
-				"maximum period size exceeded: %d > %d\n",
+				"maximum period size exceeded: %zu > %d\n",
 				period_len, TDMA_MAX_XFER_BYTES);
 		goto err_out;
 	}
diff --git a/drivers/dma/moxart-dma.c b/drivers/dma/moxart-dma.c
index a6e642792e5a..e1a5c2242f6f 100644
--- a/drivers/dma/moxart-dma.c
+++ b/drivers/dma/moxart-dma.c
@@ -579,7 +579,7 @@ static int moxart_probe(struct platform_device *pdev)
 		return -ENOMEM;
 
 	irq = irq_of_parse_and_map(node, 0);
-	if (irq == NO_IRQ) {
+	if (!irq) {
 		dev_err(dev, "no IRQ resource\n");
 		return -EINVAL;
 	}
diff --git a/drivers/dma/mpc512x_dma.c b/drivers/dma/mpc512x_dma.c
index fa86592c7ae1..dde713461a95 100644
--- a/drivers/dma/mpc512x_dma.c
+++ b/drivers/dma/mpc512x_dma.c
@@ -411,8 +411,7 @@ static void mpc_dma_process_completed(struct mpc_dma *mdma)
 		list_for_each_entry(mdesc, &list, node) {
 			desc = &mdesc->desc;
 
-			if (desc->callback)
-				desc->callback(desc->callback_param);
+			dmaengine_desc_get_callback_invoke(desc, NULL);
 
 			last_cookie = desc->cookie;
 			dma_run_dependencies(desc);
@@ -926,7 +925,7 @@ static int mpc_dma_probe(struct platform_device *op)
 	}
 
 	mdma->irq = irq_of_parse_and_map(dn, 0);
-	if (mdma->irq == NO_IRQ) {
+	if (!mdma->irq) {
 		dev_err(dev, "Error mapping IRQ!\n");
 		retval = -EINVAL;
 		goto err;
@@ -935,7 +934,7 @@ static int mpc_dma_probe(struct platform_device *op)
 	if (of_device_is_compatible(dn, "fsl,mpc8308-dma")) {
 		mdma->is_mpc8308 = 1;
 		mdma->irq2 = irq_of_parse_and_map(dn, 1);
-		if (mdma->irq2 == NO_IRQ) {
+		if (!mdma->irq2) {
 			dev_err(dev, "Error mapping IRQ!\n");
 			retval = -EINVAL;
 			goto err_dispose1;
diff --git a/drivers/dma/mv_xor.c b/drivers/dma/mv_xor.c
index f4c9f98ec35e..23f75285a4d9 100644
--- a/drivers/dma/mv_xor.c
+++ b/drivers/dma/mv_xor.c
@@ -206,14 +206,11 @@ mv_desc_run_tx_complete_actions(struct mv_xor_desc_slot *desc,
 	if (desc->async_tx.cookie > 0) {
 		cookie = desc->async_tx.cookie;
 
+		dma_descriptor_unmap(&desc->async_tx);
 		/* call the callback (must not sleep or submit new
 		 * operations to this channel)
 		 */
-		if (desc->async_tx.callback)
-			desc->async_tx.callback(
-				desc->async_tx.callback_param);
-
-		dma_descriptor_unmap(&desc->async_tx);
+		dmaengine_desc_get_callback_invoke(&desc->async_tx, NULL);
 	}
 
 	/* run dependent operations */
@@ -470,12 +467,90 @@ static int mv_xor_alloc_chan_resources(struct dma_chan *chan)
 	return mv_chan->slots_allocated ? : -ENOMEM;
 }
 
+/*
+ * Check if source or destination is an PCIe/IO address (non-SDRAM) and add
+ * a new MBus window if necessary. Use a cache for these check so that
+ * the MMIO mapped registers don't have to be accessed for this check
+ * to speed up this process.
+ */
+static int mv_xor_add_io_win(struct mv_xor_chan *mv_chan, u32 addr)
+{
+	struct mv_xor_device *xordev = mv_chan->xordev;
+	void __iomem *base = mv_chan->mmr_high_base;
+	u32 win_enable;
+	u32 size;
+	u8 target, attr;
+	int ret;
+	int i;
+
+	/* Nothing needs to get done for the Armada 3700 */
+	if (xordev->xor_type == XOR_ARMADA_37XX)
+		return 0;
+
+	/*
+	 * Loop over the cached windows to check, if the requested area
+	 * is already mapped. If this the case, nothing needs to be done
+	 * and we can return.
+	 */
+	for (i = 0; i < WINDOW_COUNT; i++) {
+		if (addr >= xordev->win_start[i] &&
+		    addr <= xordev->win_end[i]) {
+			/* Window is already mapped */
+			return 0;
+		}
+	}
+
+	/*
+	 * The window is not mapped, so we need to create the new mapping
+	 */
+
+	/* If no IO window is found that addr has to be located in SDRAM */
+	ret = mvebu_mbus_get_io_win_info(addr, &size, &target, &attr);
+	if (ret < 0)
+		return 0;
+
+	/*
+	 * Mask the base addr 'addr' according to 'size' read back from the
+	 * MBus window. Otherwise we might end up with an address located
+	 * somewhere in the middle of this area here.
+	 */
+	size -= 1;
+	addr &= ~size;
+
+	/*
+	 * Reading one of both enabled register is enough, as they are always
+	 * programmed to the identical values
+	 */
+	win_enable = readl(base + WINDOW_BAR_ENABLE(0));
+
+	/* Set 'i' to the first free window to write the new values to */
+	i = ffs(~win_enable) - 1;
+	if (i >= WINDOW_COUNT)
+		return -ENOMEM;
+
+	writel((addr & 0xffff0000) | (attr << 8) | target,
+	       base + WINDOW_BASE(i));
+	writel(size & 0xffff0000, base + WINDOW_SIZE(i));
+
+	/* Fill the caching variables for later use */
+	xordev->win_start[i] = addr;
+	xordev->win_end[i] = addr + size;
+
+	win_enable |= (1 << i);
+	win_enable |= 3 << (16 + (2 * i));
+	writel(win_enable, base + WINDOW_BAR_ENABLE(0));
+	writel(win_enable, base + WINDOW_BAR_ENABLE(1));
+
+	return 0;
+}
+
 static struct dma_async_tx_descriptor *
 mv_xor_prep_dma_xor(struct dma_chan *chan, dma_addr_t dest, dma_addr_t *src,
 		    unsigned int src_cnt, size_t len, unsigned long flags)
 {
 	struct mv_xor_chan *mv_chan = to_mv_xor_chan(chan);
 	struct mv_xor_desc_slot *sw_desc;
+	int ret;
 
 	if (unlikely(len < MV_XOR_MIN_BYTE_COUNT))
 		return NULL;
@@ -486,6 +561,11 @@ mv_xor_prep_dma_xor(struct dma_chan *chan, dma_addr_t dest, dma_addr_t *src,
 		"%s src_cnt: %d len: %zu dest %pad flags: %ld\n",
 		__func__, src_cnt, len, &dest, flags);
 
+	/* Check if a new window needs to get added for 'dest' */
+	ret = mv_xor_add_io_win(mv_chan, dest);
+	if (ret)
+		return NULL;
+
 	sw_desc = mv_chan_alloc_slot(mv_chan);
 	if (sw_desc) {
 		sw_desc->type = DMA_XOR;
@@ -493,8 +573,13 @@ mv_xor_prep_dma_xor(struct dma_chan *chan, dma_addr_t dest, dma_addr_t *src,
 		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--)
+		while (src_cnt--) {
+			/* Check if a new window needs to get added for 'src' */
+			ret = mv_xor_add_io_win(mv_chan, src[src_cnt]);
+			if (ret)
+				return NULL;
 			mv_desc_set_src_addr(sw_desc, src_cnt, src[src_cnt]);
+		}
 	}
 
 	dev_dbg(mv_chan_to_devp(mv_chan),
@@ -959,6 +1044,7 @@ mv_xor_channel_add(struct mv_xor_device *xordev,
 		mv_chan->op_in_desc = XOR_MODE_IN_DESC;
 
 	dma_dev = &mv_chan->dmadev;
+	mv_chan->xordev = xordev;
 
 	/*
 	 * These source and destination dummy buffers are used to implement
@@ -1086,6 +1172,10 @@ mv_xor_conf_mbus_windows(struct mv_xor_device *xordev,
 		       dram->mbus_dram_target_id, base + WINDOW_BASE(i));
 		writel((cs->size - 1) & 0xffff0000, base + WINDOW_SIZE(i));
 
+		/* Fill the caching variables for later use */
+		xordev->win_start[i] = cs->base;
+		xordev->win_end[i] = cs->base + cs->size - 1;
+
 		win_enable |= (1 << i);
 		win_enable |= 3 << (16 + (2 * i));
 	}
diff --git a/drivers/dma/mv_xor.h b/drivers/dma/mv_xor.h
index bf56e082e7cd..88eeab222a23 100644
--- a/drivers/dma/mv_xor.h
+++ b/drivers/dma/mv_xor.h
@@ -80,12 +80,17 @@
 #define WINDOW_BAR_ENABLE(chan)	(0x40 + ((chan) << 2))
 #define WINDOW_OVERRIDE_CTRL(chan)	(0xA0 + ((chan) << 2))
 
+#define WINDOW_COUNT		8
+
 struct mv_xor_device {
 	void __iomem	     *xor_base;
 	void __iomem	     *xor_high_base;
 	struct clk	     *clk;
 	struct mv_xor_chan   *channels[MV_XOR_MAX_CHANNELS];
 	int		     xor_type;
+
+	u32                  win_start[WINDOW_COUNT];
+	u32                  win_end[WINDOW_COUNT];
 };
 
 /**
@@ -127,6 +132,8 @@ struct mv_xor_chan {
 	char			dummy_dst[MV_XOR_MIN_BYTE_COUNT];
 	dma_addr_t		dummy_src_addr, dummy_dst_addr;
 	u32                     saved_config_reg, saved_int_mask_reg;
+
+	struct mv_xor_device	*xordev;
 };
 
 /**
diff --git a/drivers/dma/mxs-dma.c b/drivers/dma/mxs-dma.c
index 60de35251da5..e217268c7098 100644
--- a/drivers/dma/mxs-dma.c
+++ b/drivers/dma/mxs-dma.c
@@ -326,8 +326,7 @@ static void mxs_dma_tasklet(unsigned long data)
 {
 	struct mxs_dma_chan *mxs_chan = (struct mxs_dma_chan *) data;
 
-	if (mxs_chan->desc.callback)
-		mxs_chan->desc.callback(mxs_chan->desc.callback_param);
+	dmaengine_desc_get_callback_invoke(&mxs_chan->desc, NULL);
 }
 
 static int mxs_dma_irq_to_chan(struct mxs_dma_engine *mxs_dma, int irq)
@@ -429,12 +428,10 @@ static int mxs_dma_alloc_chan_resources(struct dma_chan *chan)
 		goto err_alloc;
 	}
 
-	if (mxs_chan->chan_irq != NO_IRQ) {
-		ret = request_irq(mxs_chan->chan_irq, mxs_dma_int_handler,
-					0, "mxs-dma", mxs_dma);
-		if (ret)
-			goto err_irq;
-	}
+	ret = request_irq(mxs_chan->chan_irq, mxs_dma_int_handler,
+			  0, "mxs-dma", mxs_dma);
+	if (ret)
+		goto err_irq;
 
 	ret = clk_prepare_enable(mxs_dma->clk);
 	if (ret)
diff --git a/drivers/dma/nbpfaxi.c b/drivers/dma/nbpfaxi.c
index 08c45c185549..09de71519d37 100644
--- a/drivers/dma/nbpfaxi.c
+++ b/drivers/dma/nbpfaxi.c
@@ -1102,8 +1102,7 @@ static void nbpf_chan_tasklet(unsigned long data)
 {
 	struct nbpf_channel *chan = (struct nbpf_channel *)data;
 	struct nbpf_desc *desc, *tmp;
-	dma_async_tx_callback callback;
-	void *param;
+	struct dmaengine_desc_callback cb;
 
 	while (!list_empty(&chan->done)) {
 		bool found = false, must_put, recycling = false;
@@ -1151,14 +1150,12 @@ static void nbpf_chan_tasklet(unsigned long data)
 			must_put = false;
 		}
 
-		callback = desc->async_tx.callback;
-		param = desc->async_tx.callback_param;
+		dmaengine_desc_get_callback(&desc->async_tx, &cb);
 
 		/* ack and callback completed descriptor */
 		spin_unlock_irq(&chan->lock);
 
-		if (callback)
-			callback(param);
+		dmaengine_desc_callback_invoke(&cb, NULL);
 
 		if (must_put)
 			nbpf_desc_put(desc);
diff --git a/drivers/dma/omap-dma.c b/drivers/dma/omap-dma.c
index d99ca2b511c4..7ca27d4b1c54 100644
--- a/drivers/dma/omap-dma.c
+++ b/drivers/dma/omap-dma.c
@@ -8,6 +8,7 @@
 #include <linux/delay.h>
 #include <linux/dmaengine.h>
 #include <linux/dma-mapping.h>
+#include <linux/dmapool.h>
 #include <linux/err.h>
 #include <linux/init.h>
 #include <linux/interrupt.h>
@@ -32,10 +33,12 @@ struct omap_dmadev {
 	const struct omap_dma_reg *reg_map;
 	struct omap_system_dma_plat_info *plat;
 	bool legacy;
+	bool ll123_supported;
+	struct dma_pool *desc_pool;
 	unsigned dma_requests;
 	spinlock_t irq_lock;
 	uint32_t irq_enable_mask;
-	struct omap_chan *lch_map[OMAP_SDMA_CHANNELS];
+	struct omap_chan **lch_map;
 };
 
 struct omap_chan {
@@ -55,16 +58,40 @@ struct omap_chan {
 	unsigned sgidx;
 };
 
+#define DESC_NXT_SV_REFRESH	(0x1 << 24)
+#define DESC_NXT_SV_REUSE	(0x2 << 24)
+#define DESC_NXT_DV_REFRESH	(0x1 << 26)
+#define DESC_NXT_DV_REUSE	(0x2 << 26)
+#define DESC_NTYPE_TYPE2	(0x2 << 29)
+
+/* Type 2 descriptor with Source or Destination address update */
+struct omap_type2_desc {
+	uint32_t next_desc;
+	uint32_t en;
+	uint32_t addr; /* src or dst */
+	uint16_t fn;
+	uint16_t cicr;
+	int16_t cdei;
+	int16_t csei;
+	int32_t cdfi;
+	int32_t csfi;
+} __packed;
+
 struct omap_sg {
 	dma_addr_t addr;
 	uint32_t en;		/* number of elements (24-bit) */
 	uint32_t fn;		/* number of frames (16-bit) */
 	int32_t fi;		/* for double indexing */
 	int16_t ei;		/* for double indexing */
+
+	/* Linked list */
+	struct omap_type2_desc *t2_desc;
+	dma_addr_t t2_desc_paddr;
 };
 
 struct omap_desc {
 	struct virt_dma_desc vd;
+	bool using_ll;
 	enum dma_transfer_direction dir;
 	dma_addr_t dev_addr;
 
@@ -81,6 +108,9 @@ struct omap_desc {
 };
 
 enum {
+	CAPS_0_SUPPORT_LL123	= BIT(20),	/* Linked List type1/2/3 */
+	CAPS_0_SUPPORT_LL4	= BIT(21),	/* Linked List type4 */
+
 	CCR_FS			= BIT(5),
 	CCR_READ_PRIORITY	= BIT(6),
 	CCR_ENABLE		= BIT(7),
@@ -151,6 +181,19 @@ enum {
 	CICR_SUPER_BLOCK_IE	= BIT(14),	/* OMAP2+ only */
 
 	CLNK_CTRL_ENABLE_LNK	= BIT(15),
+
+	CDP_DST_VALID_INC	= 0 << 0,
+	CDP_DST_VALID_RELOAD	= 1 << 0,
+	CDP_DST_VALID_REUSE	= 2 << 0,
+	CDP_SRC_VALID_INC	= 0 << 2,
+	CDP_SRC_VALID_RELOAD	= 1 << 2,
+	CDP_SRC_VALID_REUSE	= 2 << 2,
+	CDP_NTYPE_TYPE1		= 1 << 4,
+	CDP_NTYPE_TYPE2		= 2 << 4,
+	CDP_NTYPE_TYPE3		= 3 << 4,
+	CDP_TMODE_NORMAL	= 0 << 8,
+	CDP_TMODE_LLIST		= 1 << 8,
+	CDP_FAST		= BIT(10),
 };
 
 static const unsigned es_bytes[] = {
@@ -180,7 +223,64 @@ static inline struct omap_desc *to_omap_dma_desc(struct dma_async_tx_descriptor
 
 static void omap_dma_desc_free(struct virt_dma_desc *vd)
 {
-	kfree(container_of(vd, struct omap_desc, vd));
+	struct omap_desc *d = to_omap_dma_desc(&vd->tx);
+
+	if (d->using_ll) {
+		struct omap_dmadev *od = to_omap_dma_dev(vd->tx.chan->device);
+		int i;
+
+		for (i = 0; i < d->sglen; i++) {
+			if (d->sg[i].t2_desc)
+				dma_pool_free(od->desc_pool, d->sg[i].t2_desc,
+					      d->sg[i].t2_desc_paddr);
+		}
+	}
+
+	kfree(d);
+}
+
+static void omap_dma_fill_type2_desc(struct omap_desc *d, int idx,
+				     enum dma_transfer_direction dir, bool last)
+{
+	struct omap_sg *sg = &d->sg[idx];
+	struct omap_type2_desc *t2_desc = sg->t2_desc;
+
+	if (idx)
+		d->sg[idx - 1].t2_desc->next_desc = sg->t2_desc_paddr;
+	if (last)
+		t2_desc->next_desc = 0xfffffffc;
+
+	t2_desc->en = sg->en;
+	t2_desc->addr = sg->addr;
+	t2_desc->fn = sg->fn & 0xffff;
+	t2_desc->cicr = d->cicr;
+	if (!last)
+		t2_desc->cicr &= ~CICR_BLOCK_IE;
+
+	switch (dir) {
+	case DMA_DEV_TO_MEM:
+		t2_desc->cdei = sg->ei;
+		t2_desc->csei = d->ei;
+		t2_desc->cdfi = sg->fi;
+		t2_desc->csfi = d->fi;
+
+		t2_desc->en |= DESC_NXT_DV_REFRESH;
+		t2_desc->en |= DESC_NXT_SV_REUSE;
+		break;
+	case DMA_MEM_TO_DEV:
+		t2_desc->cdei = d->ei;
+		t2_desc->csei = sg->ei;
+		t2_desc->cdfi = d->fi;
+		t2_desc->csfi = sg->fi;
+
+		t2_desc->en |= DESC_NXT_SV_REFRESH;
+		t2_desc->en |= DESC_NXT_DV_REUSE;
+		break;
+	default:
+		return;
+	}
+
+	t2_desc->en |= DESC_NTYPE_TYPE2;
 }
 
 static void omap_dma_write(uint32_t val, unsigned type, void __iomem *addr)
@@ -285,6 +385,7 @@ static void omap_dma_assign(struct omap_dmadev *od, struct omap_chan *c,
 static void omap_dma_start(struct omap_chan *c, struct omap_desc *d)
 {
 	struct omap_dmadev *od = to_omap_dma_dev(c->vc.chan.device);
+	uint16_t cicr = d->cicr;
 
 	if (__dma_omap15xx(od->plat->dma_attr))
 		omap_dma_chan_write(c, CPC, 0);
@@ -293,8 +394,27 @@ static void omap_dma_start(struct omap_chan *c, struct omap_desc *d)
 
 	omap_dma_clear_csr(c);
 
+	if (d->using_ll) {
+		uint32_t cdp = CDP_TMODE_LLIST | CDP_NTYPE_TYPE2 | CDP_FAST;
+
+		if (d->dir == DMA_DEV_TO_MEM)
+			cdp |= (CDP_DST_VALID_RELOAD | CDP_SRC_VALID_REUSE);
+		else
+			cdp |= (CDP_DST_VALID_REUSE | CDP_SRC_VALID_RELOAD);
+		omap_dma_chan_write(c, CDP, cdp);
+
+		omap_dma_chan_write(c, CNDP, d->sg[0].t2_desc_paddr);
+		omap_dma_chan_write(c, CCDN, 0);
+		omap_dma_chan_write(c, CCFN, 0xffff);
+		omap_dma_chan_write(c, CCEN, 0xffffff);
+
+		cicr &= ~CICR_BLOCK_IE;
+	} else if (od->ll123_supported) {
+		omap_dma_chan_write(c, CDP, 0);
+	}
+
 	/* Enable interrupts */
-	omap_dma_chan_write(c, CICR, d->cicr);
+	omap_dma_chan_write(c, CICR, cicr);
 
 	/* Enable channel */
 	omap_dma_chan_write(c, CCR, d->ccr | CCR_ENABLE);
@@ -365,10 +485,9 @@ static void omap_dma_stop(struct omap_chan *c)
 	c->running = false;
 }
 
-static void omap_dma_start_sg(struct omap_chan *c, struct omap_desc *d,
-	unsigned idx)
+static void omap_dma_start_sg(struct omap_chan *c, struct omap_desc *d)
 {
-	struct omap_sg *sg = d->sg + idx;
+	struct omap_sg *sg = d->sg + c->sgidx;
 	unsigned cxsa, cxei, cxfi;
 
 	if (d->dir == DMA_DEV_TO_MEM || d->dir == DMA_MEM_TO_MEM) {
@@ -388,6 +507,7 @@ static void omap_dma_start_sg(struct omap_chan *c, struct omap_desc *d,
 	omap_dma_chan_write(c, CFN, sg->fn);
 
 	omap_dma_start(c, d);
+	c->sgidx++;
 }
 
 static void omap_dma_start_desc(struct omap_chan *c)
@@ -433,7 +553,7 @@ static void omap_dma_start_desc(struct omap_chan *c)
 	omap_dma_chan_write(c, CSDP, d->csdp);
 	omap_dma_chan_write(c, CLNK_CTRL, d->clnk_ctrl);
 
-	omap_dma_start_sg(c, d, 0);
+	omap_dma_start_sg(c, d);
 }
 
 static void omap_dma_callback(int ch, u16 status, void *data)
@@ -445,15 +565,13 @@ static void omap_dma_callback(int ch, u16 status, void *data)
 	spin_lock_irqsave(&c->vc.lock, flags);
 	d = c->desc;
 	if (d) {
-		if (!c->cyclic) {
-			if (++c->sgidx < d->sglen) {
-				omap_dma_start_sg(c, d, c->sgidx);
-			} else {
-				omap_dma_start_desc(c);
-				vchan_cookie_complete(&d->vd);
-			}
-		} else {
+		if (c->cyclic) {
 			vchan_cyclic_callback(&d->vd);
+		} else if (d->using_ll || c->sgidx == d->sglen) {
+			omap_dma_start_desc(c);
+			vchan_cookie_complete(&d->vd);
+		} else {
+			omap_dma_start_sg(c, d);
 		}
 	}
 	spin_unlock_irqrestore(&c->vc.lock, flags);
@@ -503,6 +621,7 @@ static int omap_dma_alloc_chan_resources(struct dma_chan *chan)
 {
 	struct omap_dmadev *od = to_omap_dma_dev(chan->device);
 	struct omap_chan *c = to_omap_dma_chan(chan);
+	struct device *dev = od->ddev.dev;
 	int ret;
 
 	if (od->legacy) {
@@ -513,8 +632,7 @@ static int omap_dma_alloc_chan_resources(struct dma_chan *chan)
 				       &c->dma_ch);
 	}
 
-	dev_dbg(od->ddev.dev, "allocating channel %u for %u\n",
-		c->dma_ch, c->dma_sig);
+	dev_dbg(dev, "allocating channel %u for %u\n", c->dma_ch, c->dma_sig);
 
 	if (ret >= 0) {
 		omap_dma_assign(od, c, c->dma_ch);
@@ -570,7 +688,8 @@ static void omap_dma_free_chan_resources(struct dma_chan *chan)
 	vchan_free_chan_resources(&c->vc);
 	omap_free_dma(c->dma_ch);
 
-	dev_dbg(od->ddev.dev, "freeing channel for %u\n", c->dma_sig);
+	dev_dbg(od->ddev.dev, "freeing channel %u used for %u\n", c->dma_ch,
+		c->dma_sig);
 	c->dma_sig = 0;
 }
 
@@ -744,6 +863,7 @@ static struct dma_async_tx_descriptor *omap_dma_prep_slave_sg(
 	struct omap_desc *d;
 	dma_addr_t dev_addr;
 	unsigned i, es, en, frame_bytes;
+	bool ll_failed = false;
 	u32 burst;
 
 	if (dir == DMA_DEV_TO_MEM) {
@@ -784,13 +904,16 @@ static struct dma_async_tx_descriptor *omap_dma_prep_slave_sg(
 	d->es = es;
 
 	d->ccr = c->ccr | CCR_SYNC_FRAME;
-	if (dir == DMA_DEV_TO_MEM)
+	if (dir == DMA_DEV_TO_MEM) {
 		d->ccr |= CCR_DST_AMODE_POSTINC | CCR_SRC_AMODE_CONSTANT;
-	else
+		d->csdp = CSDP_DST_BURST_64 | CSDP_DST_PACKED;
+	} else {
 		d->ccr |= CCR_DST_AMODE_CONSTANT | CCR_SRC_AMODE_POSTINC;
+		d->csdp = CSDP_SRC_BURST_64 | CSDP_SRC_PACKED;
+	}
 
 	d->cicr = CICR_DROP_IE | CICR_BLOCK_IE;
-	d->csdp = es;
+	d->csdp |= es;
 
 	if (dma_omap1()) {
 		d->cicr |= CICR_TOUT_IE;
@@ -819,14 +942,47 @@ static struct dma_async_tx_descriptor *omap_dma_prep_slave_sg(
 	 */
 	en = burst;
 	frame_bytes = es_bytes[es] * en;
+
+	if (sglen >= 2)
+		d->using_ll = od->ll123_supported;
+
 	for_each_sg(sgl, sgent, sglen, i) {
-		d->sg[i].addr = sg_dma_address(sgent);
-		d->sg[i].en = en;
-		d->sg[i].fn = sg_dma_len(sgent) / frame_bytes;
+		struct omap_sg *osg = &d->sg[i];
+
+		osg->addr = sg_dma_address(sgent);
+		osg->en = en;
+		osg->fn = sg_dma_len(sgent) / frame_bytes;
+
+		if (d->using_ll) {
+			osg->t2_desc = dma_pool_alloc(od->desc_pool, GFP_ATOMIC,
+						      &osg->t2_desc_paddr);
+			if (!osg->t2_desc) {
+				dev_err(chan->device->dev,
+					"t2_desc[%d] allocation failed\n", i);
+				ll_failed = true;
+				d->using_ll = false;
+				continue;
+			}
+
+			omap_dma_fill_type2_desc(d, i, dir, (i == sglen - 1));
+		}
 	}
 
 	d->sglen = sglen;
 
+	/* Release the dma_pool entries if one allocation failed */
+	if (ll_failed) {
+		for (i = 0; i < d->sglen; i++) {
+			struct omap_sg *osg = &d->sg[i];
+
+			if (osg->t2_desc) {
+				dma_pool_free(od->desc_pool, osg->t2_desc,
+					      osg->t2_desc_paddr);
+				osg->t2_desc = NULL;
+			}
+		}
+	}
+
 	return vchan_tx_prep(&c->vc, &d->vd, tx_flags);
 }
 
@@ -1225,16 +1381,24 @@ static int omap_dma_probe(struct platform_device *pdev)
 	spin_lock_init(&od->lock);
 	spin_lock_init(&od->irq_lock);
 
-	od->dma_requests = OMAP_SDMA_REQUESTS;
-	if (pdev->dev.of_node && of_property_read_u32(pdev->dev.of_node,
-						      "dma-requests",
-						      &od->dma_requests)) {
+	if (!pdev->dev.of_node) {
+		od->dma_requests = od->plat->dma_attr->lch_count;
+		if (unlikely(!od->dma_requests))
+			od->dma_requests = OMAP_SDMA_REQUESTS;
+	} else if (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);
+		od->dma_requests = OMAP_SDMA_REQUESTS;
 	}
 
-	for (i = 0; i < OMAP_SDMA_CHANNELS; i++) {
+	od->lch_map = devm_kcalloc(&pdev->dev, od->dma_requests,
+				   sizeof(*od->lch_map), GFP_KERNEL);
+	if (!od->lch_map)
+		return -ENOMEM;
+
+	for (i = 0; i < od->dma_requests; i++) {
 		rc = omap_dma_chan_init(od);
 		if (rc) {
 			omap_dma_free(od);
@@ -1257,10 +1421,25 @@ static int omap_dma_probe(struct platform_device *pdev)
 			return rc;
 	}
 
+	if (omap_dma_glbl_read(od, CAPS_0) & CAPS_0_SUPPORT_LL123)
+		od->ll123_supported = true;
+
 	od->ddev.filter.map = od->plat->slave_map;
 	od->ddev.filter.mapcnt = od->plat->slavecnt;
 	od->ddev.filter.fn = omap_dma_filter_fn;
 
+	if (od->ll123_supported) {
+		od->desc_pool = dma_pool_create(dev_name(&pdev->dev),
+						&pdev->dev,
+						sizeof(struct omap_type2_desc),
+						4, 0);
+		if (!od->desc_pool) {
+			dev_err(&pdev->dev,
+				"unable to allocate descriptor pool\n");
+			od->ll123_supported = false;
+		}
+	}
+
 	rc = dma_async_device_register(&od->ddev);
 	if (rc) {
 		pr_warn("OMAP-DMA: failed to register slave DMA engine device: %d\n",
@@ -1284,7 +1463,8 @@ static int omap_dma_probe(struct platform_device *pdev)
 		}
 	}
 
-	dev_info(&pdev->dev, "OMAP DMA engine driver\n");
+	dev_info(&pdev->dev, "OMAP DMA engine driver%s\n",
+		 od->ll123_supported ? " (LinkedList1/2/3 supported)" : "");
 
 	return rc;
 }
@@ -1307,6 +1487,9 @@ static int omap_dma_remove(struct platform_device *pdev)
 		omap_dma_glbl_write(od, IRQENABLE_L0, 0);
 	}
 
+	if (od->ll123_supported)
+		dma_pool_destroy(od->desc_pool);
+
 	omap_dma_free(od);
 
 	return 0;
diff --git a/drivers/dma/pch_dma.c b/drivers/dma/pch_dma.c
index 113605f6fe20..df95727dc2fb 100644
--- a/drivers/dma/pch_dma.c
+++ b/drivers/dma/pch_dma.c
@@ -357,14 +357,13 @@ static void pdc_chain_complete(struct pch_dma_chan *pd_chan,
 			       struct pch_dma_desc *desc)
 {
 	struct dma_async_tx_descriptor *txd = &desc->txd;
-	dma_async_tx_callback callback = txd->callback;
-	void *param = txd->callback_param;
+	struct dmaengine_desc_callback cb;
 
+	dmaengine_desc_get_callback(txd, &cb);
 	list_splice_init(&desc->tx_list, &pd_chan->free_list);
 	list_move(&desc->desc_node, &pd_chan->free_list);
 
-	if (callback)
-		callback(param);
+	dmaengine_desc_callback_invoke(&cb, NULL);
 }
 
 static void pdc_complete_all(struct pch_dma_chan *pd_chan)
diff --git a/drivers/dma/pl330.c b/drivers/dma/pl330.c
index 4fc3ffbd5ca0..030fe05ed43b 100644
--- a/drivers/dma/pl330.c
+++ b/drivers/dma/pl330.c
@@ -2039,14 +2039,12 @@ static void pl330_tasklet(unsigned long data)
 	}
 
 	while (!list_empty(&pch->completed_list)) {
-		dma_async_tx_callback callback;
-		void *callback_param;
+		struct dmaengine_desc_callback cb;
 
 		desc = list_first_entry(&pch->completed_list,
 					struct dma_pl330_desc, node);
 
-		callback = desc->txd.callback;
-		callback_param = desc->txd.callback_param;
+		dmaengine_desc_get_callback(&desc->txd, &cb);
 
 		if (pch->cyclic) {
 			desc->status = PREP;
@@ -2064,9 +2062,9 @@ static void pl330_tasklet(unsigned long data)
 
 		dma_descriptor_unmap(&desc->txd);
 
-		if (callback) {
+		if (dmaengine_desc_callback_valid(&cb)) {
 			spin_unlock_irqrestore(&pch->lock, flags);
-			callback(callback_param);
+			dmaengine_desc_callback_invoke(&cb, NULL);
 			spin_lock_irqsave(&pch->lock, flags);
 		}
 	}
@@ -2274,7 +2272,7 @@ pl330_tx_status(struct dma_chan *chan, dma_cookie_t cookie,
 {
 	enum dma_status ret;
 	unsigned long flags;
-	struct dma_pl330_desc *desc, *running = NULL;
+	struct dma_pl330_desc *desc, *running = NULL, *last_enq = NULL;
 	struct dma_pl330_chan *pch = to_pchan(chan);
 	unsigned int transferred, residual = 0;
 
@@ -2287,10 +2285,13 @@ pl330_tx_status(struct dma_chan *chan, dma_cookie_t cookie,
 		goto out;
 
 	spin_lock_irqsave(&pch->lock, flags);
+	spin_lock(&pch->thread->dmac->lock);
 
 	if (pch->thread->req_running != -1)
 		running = pch->thread->req[pch->thread->req_running].desc;
 
+	last_enq = pch->thread->req[pch->thread->lstenq].desc;
+
 	/* Check in pending list */
 	list_for_each_entry(desc, &pch->work_list, node) {
 		if (desc->status == DONE)
@@ -2298,6 +2299,15 @@ pl330_tx_status(struct dma_chan *chan, dma_cookie_t cookie,
 		else if (running && desc == running)
 			transferred =
 				pl330_get_current_xferred_count(pch, desc);
+		else if (desc->status == BUSY)
+			/*
+			 * Busy but not running means either just enqueued,
+			 * or finished and not yet marked done
+			 */
+			if (desc == last_enq)
+				transferred = 0;
+			else
+				transferred = desc->bytes_requested;
 		else
 			transferred = 0;
 		residual += desc->bytes_requested - transferred;
@@ -2318,6 +2328,7 @@ pl330_tx_status(struct dma_chan *chan, dma_cookie_t cookie,
 		if (desc->last)
 			residual = 0;
 	}
+	spin_unlock(&pch->thread->dmac->lock);
 	spin_unlock_irqrestore(&pch->lock, flags);
 
 out:
diff --git a/drivers/dma/ppc4xx/adma.c b/drivers/dma/ppc4xx/adma.c
index da3688b94bdc..b1535b1fe95c 100644
--- a/drivers/dma/ppc4xx/adma.c
+++ b/drivers/dma/ppc4xx/adma.c
@@ -1482,14 +1482,11 @@ static dma_cookie_t ppc440spe_adma_run_tx_complete_actions(
 		cookie = desc->async_tx.cookie;
 		desc->async_tx.cookie = 0;
 
+		dma_descriptor_unmap(&desc->async_tx);
 		/* call the callback (must not sleep or submit new
 		 * operations to this channel)
 		 */
-		if (desc->async_tx.callback)
-			desc->async_tx.callback(
-				desc->async_tx.callback_param);
-
-		dma_descriptor_unmap(&desc->async_tx);
+		dmaengine_desc_get_callback_invoke(&desc->async_tx, NULL);
 	}
 
 	/* run dependent operations */
@@ -3891,7 +3888,7 @@ static int ppc440spe_adma_setup_irqs(struct ppc440spe_adma_device *adev,
 	np = ofdev->dev.of_node;
 	if (adev->id != PPC440SPE_XOR_ID) {
 		adev->err_irq = irq_of_parse_and_map(np, 1);
-		if (adev->err_irq == NO_IRQ) {
+		if (!adev->err_irq) {
 			dev_warn(adev->dev, "no err irq resource?\n");
 			*initcode = PPC_ADMA_INIT_IRQ2;
 			adev->err_irq = -ENXIO;
@@ -3902,7 +3899,7 @@ static int ppc440spe_adma_setup_irqs(struct ppc440spe_adma_device *adev,
 	}
 
 	adev->irq = irq_of_parse_and_map(np, 0);
-	if (adev->irq == NO_IRQ) {
+	if (!adev->irq) {
 		dev_err(adev->dev, "no irq resource\n");
 		*initcode = PPC_ADMA_INIT_IRQ1;
 		ret = -ENXIO;
diff --git a/drivers/dma/qcom/hidma.c b/drivers/dma/qcom/hidma.c
index b2374cd91e45..e244e10a94b5 100644
--- a/drivers/dma/qcom/hidma.c
+++ b/drivers/dma/qcom/hidma.c
@@ -111,6 +111,7 @@ static void hidma_process_completed(struct hidma_chan *mchan)
 	struct dma_async_tx_descriptor *desc;
 	dma_cookie_t last_cookie;
 	struct hidma_desc *mdesc;
+	struct hidma_desc *next;
 	unsigned long irqflags;
 	struct list_head list;
 
@@ -122,28 +123,36 @@ static void hidma_process_completed(struct hidma_chan *mchan)
 	spin_unlock_irqrestore(&mchan->lock, irqflags);
 
 	/* Execute callbacks and run dependencies */
-	list_for_each_entry(mdesc, &list, node) {
+	list_for_each_entry_safe(mdesc, next, &list, node) {
 		enum dma_status llstat;
+		struct dmaengine_desc_callback cb;
+		struct dmaengine_result result;
 
 		desc = &mdesc->desc;
+		last_cookie = desc->cookie;
 
 		spin_lock_irqsave(&mchan->lock, irqflags);
 		dma_cookie_complete(desc);
 		spin_unlock_irqrestore(&mchan->lock, irqflags);
 
 		llstat = hidma_ll_status(mdma->lldev, mdesc->tre_ch);
-		if (desc->callback && (llstat == DMA_COMPLETE))
-			desc->callback(desc->callback_param);
+		dmaengine_desc_get_callback(desc, &cb);
 
-		last_cookie = desc->cookie;
 		dma_run_dependencies(desc);
-	}
 
-	/* Free descriptors */
-	spin_lock_irqsave(&mchan->lock, irqflags);
-	list_splice_tail_init(&list, &mchan->free);
-	spin_unlock_irqrestore(&mchan->lock, irqflags);
+		spin_lock_irqsave(&mchan->lock, irqflags);
+		list_move(&mdesc->node, &mchan->free);
+
+		if (llstat == DMA_COMPLETE) {
+			mchan->last_success = last_cookie;
+			result.result = DMA_TRANS_NOERROR;
+		} else
+			result.result = DMA_TRANS_ABORTED;
+
+		spin_unlock_irqrestore(&mchan->lock, irqflags);
 
+		dmaengine_desc_callback_invoke(&cb, &result);
+	}
 }
 
 /*
@@ -238,6 +247,19 @@ static void hidma_issue_pending(struct dma_chan *dmach)
 		hidma_ll_start(dmadev->lldev);
 }
 
+static inline bool hidma_txn_is_success(dma_cookie_t cookie,
+		dma_cookie_t last_success, dma_cookie_t last_used)
+{
+	if (last_success <= last_used) {
+		if ((cookie <= last_success) || (cookie > last_used))
+			return true;
+	} else {
+		if ((cookie <= last_success) && (cookie > last_used))
+			return true;
+	}
+	return false;
+}
+
 static enum dma_status hidma_tx_status(struct dma_chan *dmach,
 				       dma_cookie_t cookie,
 				       struct dma_tx_state *txstate)
@@ -246,8 +268,13 @@ static enum dma_status hidma_tx_status(struct dma_chan *dmach,
 	enum dma_status ret;
 
 	ret = dma_cookie_status(dmach, cookie, txstate);
-	if (ret == DMA_COMPLETE)
-		return ret;
+	if (ret == DMA_COMPLETE) {
+		bool is_success;
+
+		is_success = hidma_txn_is_success(cookie, mchan->last_success,
+						  dmach->cookie);
+		return is_success ? ret : DMA_ERROR;
+	}
 
 	if (mchan->paused && (ret == DMA_IN_PROGRESS)) {
 		unsigned long flags;
@@ -398,6 +425,7 @@ static int hidma_terminate_channel(struct dma_chan *chan)
 	hidma_process_completed(mchan);
 
 	spin_lock_irqsave(&mchan->lock, irqflags);
+	mchan->last_success = 0;
 	list_splice_init(&mchan->active, &list);
 	list_splice_init(&mchan->prepared, &list);
 	list_splice_init(&mchan->completed, &list);
@@ -413,14 +441,9 @@ static int hidma_terminate_channel(struct dma_chan *chan)
 	/* return all user requests */
 	list_for_each_entry_safe(mdesc, tmp, &list, node) {
 		struct dma_async_tx_descriptor *txd = &mdesc->desc;
-		dma_async_tx_callback callback = mdesc->desc.callback;
-		void *param = mdesc->desc.callback_param;
 
 		dma_descriptor_unmap(txd);
-
-		if (callback)
-			callback(param);
-
+		dmaengine_desc_get_callback_invoke(txd, NULL);
 		dma_run_dependencies(txd);
 
 		/* move myself to free_list */
diff --git a/drivers/dma/qcom/hidma.h b/drivers/dma/qcom/hidma.h
index db413a5efc4e..e52e20716303 100644
--- a/drivers/dma/qcom/hidma.h
+++ b/drivers/dma/qcom/hidma.h
@@ -72,7 +72,6 @@ struct hidma_lldev {
 
 	u32 tre_write_offset;           /* TRE write location              */
 	struct tasklet_struct task;	/* task delivering notifications   */
-	struct tasklet_struct rst_task;	/* task to reset HW                */
 	DECLARE_KFIFO_PTR(handoff_fifo,
 		struct hidma_tre *);    /* pending TREs FIFO               */
 };
@@ -89,6 +88,7 @@ struct hidma_chan {
 	bool				allocated;
 	char				dbg_name[16];
 	u32				dma_sig;
+	dma_cookie_t			last_success;
 
 	/*
 	 * active descriptor on this channel
diff --git a/drivers/dma/qcom/hidma_ll.c b/drivers/dma/qcom/hidma_ll.c
index ad20dfb64c71..3224f24c577b 100644
--- a/drivers/dma/qcom/hidma_ll.c
+++ b/drivers/dma/qcom/hidma_ll.c
@@ -381,27 +381,6 @@ static int hidma_ll_reset(struct hidma_lldev *lldev)
 }
 
 /*
- * Abort all transactions and perform a reset.
- */
-static void hidma_ll_abort(unsigned long arg)
-{
-	struct hidma_lldev *lldev = (struct hidma_lldev *)arg;
-	u8 err_code = HIDMA_EVRE_STATUS_ERROR;
-	u8 err_info = 0xFF;
-	int rc;
-
-	hidma_cleanup_pending_tre(lldev, err_info, err_code);
-
-	/* reset the channel for recovery */
-	rc = hidma_ll_setup(lldev);
-	if (rc) {
-		dev_err(lldev->dev, "channel reinitialize failed after error\n");
-		return;
-	}
-	writel(ENABLE_IRQS, lldev->evca + HIDMA_EVCA_IRQ_EN_REG);
-}
-
-/*
  * The interrupt handler for HIDMA will try to consume as many pending
  * EVRE from the event queue as possible. Each EVRE has an associated
  * TRE that holds the user interface parameters. EVRE reports the
@@ -454,13 +433,18 @@ irqreturn_t hidma_ll_inthandler(int chirq, void *arg)
 
 	while (cause) {
 		if (cause & HIDMA_ERR_INT_MASK) {
-			dev_err(lldev->dev, "error 0x%x, resetting...\n",
+			dev_err(lldev->dev, "error 0x%x, disabling...\n",
 					cause);
 
 			/* Clear out pending interrupts */
 			writel(cause, lldev->evca + HIDMA_EVCA_IRQ_CLR_REG);
 
-			tasklet_schedule(&lldev->rst_task);
+			/* No further submissions. */
+			hidma_ll_disable(lldev);
+
+			/* Driver completes the txn and intimates the client.*/
+			hidma_cleanup_pending_tre(lldev, 0xFF,
+						  HIDMA_EVRE_STATUS_ERROR);
 			goto out;
 		}
 
@@ -808,7 +792,6 @@ struct hidma_lldev *hidma_ll_init(struct device *dev, u32 nr_tres,
 		return NULL;
 
 	spin_lock_init(&lldev->lock);
-	tasklet_init(&lldev->rst_task, hidma_ll_abort, (unsigned long)lldev);
 	tasklet_init(&lldev->task, hidma_ll_tre_complete, (unsigned long)lldev);
 	lldev->initialized = 1;
 	writel(ENABLE_IRQS, lldev->evca + HIDMA_EVCA_IRQ_EN_REG);
@@ -831,7 +814,6 @@ int hidma_ll_uninit(struct hidma_lldev *lldev)
 
 	required_bytes = sizeof(struct hidma_tre) * lldev->nr_tres;
 	tasklet_kill(&lldev->task);
-	tasklet_kill(&lldev->rst_task);
 	memset(lldev->trepool, 0, required_bytes);
 	lldev->trepool = NULL;
 	lldev->pending_tre_count = 0;
diff --git a/drivers/dma/s3c24xx-dma.c b/drivers/dma/s3c24xx-dma.c
index ce67075589f5..3c579abbabb7 100644
--- a/drivers/dma/s3c24xx-dma.c
+++ b/drivers/dma/s3c24xx-dma.c
@@ -823,11 +823,11 @@ static struct dma_async_tx_descriptor *s3c24xx_dma_prep_memcpy(
 	struct s3c24xx_sg *dsg;
 	int src_mod, dest_mod;
 
-	dev_dbg(&s3cdma->pdev->dev, "prepare memcpy of %d bytes from %s\n",
+	dev_dbg(&s3cdma->pdev->dev, "prepare memcpy of %zu bytes from %s\n",
 			len, s3cchan->name);
 
 	if ((len & S3C24XX_DCON_TC_MASK) != len) {
-		dev_err(&s3cdma->pdev->dev, "memcpy size %d to large\n", len);
+		dev_err(&s3cdma->pdev->dev, "memcpy size %zu to large\n", len);
 		return NULL;
 	}
 
@@ -1301,6 +1301,9 @@ static int s3c24xx_dma_probe(struct platform_device *pdev)
 	s3cdma->slave.device_prep_dma_cyclic = s3c24xx_dma_prep_dma_cyclic;
 	s3cdma->slave.device_config = s3c24xx_dma_set_runtime_config;
 	s3cdma->slave.device_terminate_all = s3c24xx_dma_terminate_all;
+	s3cdma->slave.filter.map = pdata->slave_map;
+	s3cdma->slave.filter.mapcnt = pdata->slavecnt;
+	s3cdma->slave.filter.fn = s3c24xx_dma_filter;
 
 	/* Register as many memcpy channels as there are physical channels */
 	ret = s3c24xx_dma_init_virtual_channels(s3cdma, &s3cdma->memcpy,
@@ -1418,7 +1421,7 @@ bool s3c24xx_dma_filter(struct dma_chan *chan, void *param)
 
 	s3cchan = to_s3c24xx_dma_chan(chan);
 
-	return s3cchan->id == (int)param;
+	return s3cchan->id == (uintptr_t)param;
 }
 EXPORT_SYMBOL(s3c24xx_dma_filter);
 
diff --git a/drivers/dma/sa11x0-dma.c b/drivers/dma/sa11x0-dma.c
index 43db255050d2..1adeb3265085 100644
--- a/drivers/dma/sa11x0-dma.c
+++ b/drivers/dma/sa11x0-dma.c
@@ -463,7 +463,7 @@ static enum dma_status sa11x0_dma_tx_status(struct dma_chan *chan,
 			dma_addr_t addr = sa11x0_dma_pos(p);
 			unsigned i;
 
-			dev_vdbg(d->slave.dev, "tx_status: addr:%x\n", addr);
+			dev_vdbg(d->slave.dev, "tx_status: addr:%pad\n", &addr);
 
 			for (i = 0; i < txd->sglen; i++) {
 				dev_vdbg(d->slave.dev, "tx_status: [%u] %x+%x\n",
@@ -491,7 +491,7 @@ static enum dma_status sa11x0_dma_tx_status(struct dma_chan *chan,
 	}
 	spin_unlock_irqrestore(&c->vc.lock, flags);
 
-	dev_vdbg(d->slave.dev, "tx_status: bytes 0x%zx\n", state->residue);
+	dev_vdbg(d->slave.dev, "tx_status: bytes 0x%x\n", state->residue);
 
 	return ret;
 }
@@ -551,8 +551,8 @@ static struct dma_async_tx_descriptor *sa11x0_dma_prep_slave_sg(
 		if (len > DMA_MAX_SIZE)
 			j += DIV_ROUND_UP(len, DMA_MAX_SIZE & ~DMA_ALIGN) - 1;
 		if (addr & DMA_ALIGN) {
-			dev_dbg(chan->device->dev, "vchan %p: bad buffer alignment: %08x\n",
-				&c->vc, addr);
+			dev_dbg(chan->device->dev, "vchan %p: bad buffer alignment: %pad\n",
+				&c->vc, &addr);
 			return NULL;
 		}
 	}
@@ -599,7 +599,7 @@ static struct dma_async_tx_descriptor *sa11x0_dma_prep_slave_sg(
 	txd->size = size;
 	txd->sglen = j;
 
-	dev_dbg(chan->device->dev, "vchan %p: txd %p: size %u nr %u\n",
+	dev_dbg(chan->device->dev, "vchan %p: txd %p: size %zu nr %u\n",
 		&c->vc, &txd->vd, txd->size, txd->sglen);
 
 	return vchan_tx_prep(&c->vc, &txd->vd, flags);
@@ -693,8 +693,8 @@ static int sa11x0_dma_device_config(struct dma_chan *chan,
 	if (maxburst == 8)
 		ddar |= DDAR_BS;
 
-	dev_dbg(c->vc.chan.device->dev, "vchan %p: dma_slave_config addr %x width %u burst %u\n",
-		&c->vc, addr, width, maxburst);
+	dev_dbg(c->vc.chan.device->dev, "vchan %p: dma_slave_config addr %pad width %u burst %u\n",
+		&c->vc, &addr, width, maxburst);
 
 	c->ddar = ddar | (addr & 0xf0000000) | (addr & 0x003ffffc) << 6;
 
diff --git a/drivers/dma/sh/rcar-dmac.c b/drivers/dma/sh/rcar-dmac.c
index 0dd953884d1d..2e441d0ccd79 100644
--- a/drivers/dma/sh/rcar-dmac.c
+++ b/drivers/dma/sh/rcar-dmac.c
@@ -118,14 +118,34 @@ struct rcar_dmac_desc_page {
 	sizeof(struct rcar_dmac_xfer_chunk))
 
 /*
+ * struct rcar_dmac_chan_slave - Slave configuration
+ * @slave_addr: slave memory address
+ * @xfer_size: size (in bytes) of hardware transfers
+ */
+struct rcar_dmac_chan_slave {
+	phys_addr_t slave_addr;
+	unsigned int xfer_size;
+};
+
+/*
+ * struct rcar_dmac_chan_map - Map of slave device phys to dma address
+ * @addr: slave dma address
+ * @dir: direction of mapping
+ * @slave: slave configuration that is mapped
+ */
+struct rcar_dmac_chan_map {
+	dma_addr_t addr;
+	enum dma_data_direction dir;
+	struct rcar_dmac_chan_slave slave;
+};
+
+/*
  * struct rcar_dmac_chan - R-Car Gen2 DMA Controller Channel
  * @chan: base DMA channel object
  * @iomem: channel I/O memory base
  * @index: index of this channel in the controller
- * @src_xfer_size: size (in bytes) of hardware transfers on the source side
- * @dst_xfer_size: size (in bytes) of hardware transfers on the destination side
- * @src_slave_addr: slave source memory address
- * @dst_slave_addr: slave destination memory address
+ * @src: slave memory address and size on the source side
+ * @dst: slave memory address and size on the destination side
  * @mid_rid: hardware MID/RID for the DMA client using this channel
  * @lock: protects the channel CHCR register and the desc members
  * @desc.free: list of free descriptors
@@ -142,10 +162,9 @@ struct rcar_dmac_chan {
 	void __iomem *iomem;
 	unsigned int index;
 
-	unsigned int src_xfer_size;
-	unsigned int dst_xfer_size;
-	dma_addr_t src_slave_addr;
-	dma_addr_t dst_slave_addr;
+	struct rcar_dmac_chan_slave src;
+	struct rcar_dmac_chan_slave dst;
+	struct rcar_dmac_chan_map map;
 	int mid_rid;
 
 	spinlock_t lock;
@@ -793,13 +812,13 @@ static void rcar_dmac_chan_configure_desc(struct rcar_dmac_chan *chan,
 	case DMA_DEV_TO_MEM:
 		chcr = RCAR_DMACHCR_DM_INC | RCAR_DMACHCR_SM_FIXED
 		     | RCAR_DMACHCR_RS_DMARS;
-		xfer_size = chan->src_xfer_size;
+		xfer_size = chan->src.xfer_size;
 		break;
 
 	case DMA_MEM_TO_DEV:
 		chcr = RCAR_DMACHCR_DM_FIXED | RCAR_DMACHCR_SM_INC
 		     | RCAR_DMACHCR_RS_DMARS;
-		xfer_size = chan->dst_xfer_size;
+		xfer_size = chan->dst.xfer_size;
 		break;
 
 	case DMA_MEM_TO_MEM:
@@ -1023,13 +1042,65 @@ rcar_dmac_prep_dma_memcpy(struct dma_chan *chan, dma_addr_t dma_dest,
 				      DMA_MEM_TO_MEM, flags, false);
 }
 
+static int rcar_dmac_map_slave_addr(struct dma_chan *chan,
+				    enum dma_transfer_direction dir)
+{
+	struct rcar_dmac_chan *rchan = to_rcar_dmac_chan(chan);
+	struct rcar_dmac_chan_map *map = &rchan->map;
+	phys_addr_t dev_addr;
+	size_t dev_size;
+	enum dma_data_direction dev_dir;
+
+	if (dir == DMA_DEV_TO_MEM) {
+		dev_addr = rchan->src.slave_addr;
+		dev_size = rchan->src.xfer_size;
+		dev_dir = DMA_TO_DEVICE;
+	} else {
+		dev_addr = rchan->dst.slave_addr;
+		dev_size = rchan->dst.xfer_size;
+		dev_dir = DMA_FROM_DEVICE;
+	}
+
+	/* Reuse current map if possible. */
+	if (dev_addr == map->slave.slave_addr &&
+	    dev_size == map->slave.xfer_size &&
+	    dev_dir == map->dir)
+		return 0;
+
+	/* Remove old mapping if present. */
+	if (map->slave.xfer_size)
+		dma_unmap_resource(chan->device->dev, map->addr,
+				   map->slave.xfer_size, map->dir, 0);
+	map->slave.xfer_size = 0;
+
+	/* Create new slave address map. */
+	map->addr = dma_map_resource(chan->device->dev, dev_addr, dev_size,
+				     dev_dir, 0);
+
+	if (dma_mapping_error(chan->device->dev, map->addr)) {
+		dev_err(chan->device->dev,
+			"chan%u: failed to map %zx@%pap", rchan->index,
+			dev_size, &dev_addr);
+		return -EIO;
+	}
+
+	dev_dbg(chan->device->dev, "chan%u: map %zx@%pap to %pad dir: %s\n",
+		rchan->index, dev_size, &dev_addr, &map->addr,
+		dev_dir == DMA_TO_DEVICE ? "DMA_TO_DEVICE" : "DMA_FROM_DEVICE");
+
+	map->slave.slave_addr = dev_addr;
+	map->slave.xfer_size = dev_size;
+	map->dir = dev_dir;
+
+	return 0;
+}
+
 static struct dma_async_tx_descriptor *
 rcar_dmac_prep_slave_sg(struct dma_chan *chan, struct scatterlist *sgl,
 			unsigned int sg_len, enum dma_transfer_direction dir,
 			unsigned long flags, void *context)
 {
 	struct rcar_dmac_chan *rchan = to_rcar_dmac_chan(chan);
-	dma_addr_t dev_addr;
 
 	/* Someone calling slave DMA on a generic channel? */
 	if (rchan->mid_rid < 0 || !sg_len) {
@@ -1039,9 +1110,10 @@ rcar_dmac_prep_slave_sg(struct dma_chan *chan, struct scatterlist *sgl,
 		return NULL;
 	}
 
-	dev_addr = dir == DMA_DEV_TO_MEM
-		 ? rchan->src_slave_addr : rchan->dst_slave_addr;
-	return rcar_dmac_chan_prep_sg(rchan, sgl, sg_len, dev_addr,
+	if (rcar_dmac_map_slave_addr(chan, dir))
+		return NULL;
+
+	return rcar_dmac_chan_prep_sg(rchan, sgl, sg_len, rchan->map.addr,
 				      dir, flags, false);
 }
 
@@ -1055,7 +1127,6 @@ rcar_dmac_prep_dma_cyclic(struct dma_chan *chan, dma_addr_t buf_addr,
 	struct rcar_dmac_chan *rchan = to_rcar_dmac_chan(chan);
 	struct dma_async_tx_descriptor *desc;
 	struct scatterlist *sgl;
-	dma_addr_t dev_addr;
 	unsigned int sg_len;
 	unsigned int i;
 
@@ -1067,6 +1138,9 @@ rcar_dmac_prep_dma_cyclic(struct dma_chan *chan, dma_addr_t buf_addr,
 		return NULL;
 	}
 
+	if (rcar_dmac_map_slave_addr(chan, dir))
+		return NULL;
+
 	sg_len = buf_len / period_len;
 	if (sg_len > RCAR_DMAC_MAX_SG_LEN) {
 		dev_err(chan->device->dev,
@@ -1094,9 +1168,7 @@ rcar_dmac_prep_dma_cyclic(struct dma_chan *chan, dma_addr_t buf_addr,
 		sg_dma_len(&sgl[i]) = period_len;
 	}
 
-	dev_addr = dir == DMA_DEV_TO_MEM
-		 ? rchan->src_slave_addr : rchan->dst_slave_addr;
-	desc = rcar_dmac_chan_prep_sg(rchan, sgl, sg_len, dev_addr,
+	desc = rcar_dmac_chan_prep_sg(rchan, sgl, sg_len, rchan->map.addr,
 				      dir, flags, true);
 
 	kfree(sgl);
@@ -1112,10 +1184,10 @@ static int rcar_dmac_device_config(struct dma_chan *chan,
 	 * We could lock this, but you shouldn't be configuring the
 	 * channel, while using it...
 	 */
-	rchan->src_slave_addr = cfg->src_addr;
-	rchan->dst_slave_addr = cfg->dst_addr;
-	rchan->src_xfer_size = cfg->src_addr_width;
-	rchan->dst_xfer_size = cfg->dst_addr_width;
+	rchan->src.slave_addr = cfg->src_addr;
+	rchan->dst.slave_addr = cfg->dst_addr;
+	rchan->src.xfer_size = cfg->src_addr_width;
+	rchan->dst.xfer_size = cfg->dst_addr_width;
 
 	return 0;
 }
@@ -1389,21 +1461,18 @@ static irqreturn_t rcar_dmac_isr_channel_thread(int irq, void *dev)
 {
 	struct rcar_dmac_chan *chan = dev;
 	struct rcar_dmac_desc *desc;
+	struct dmaengine_desc_callback cb;
 
 	spin_lock_irq(&chan->lock);
 
 	/* For cyclic transfers notify the user after every chunk. */
 	if (chan->desc.running && chan->desc.running->cyclic) {
-		dma_async_tx_callback callback;
-		void *callback_param;
-
 		desc = chan->desc.running;
-		callback = desc->async_tx.callback;
-		callback_param = desc->async_tx.callback_param;
+		dmaengine_desc_get_callback(&desc->async_tx, &cb);
 
-		if (callback) {
+		if (dmaengine_desc_callback_valid(&cb)) {
 			spin_unlock_irq(&chan->lock);
-			callback(callback_param);
+			dmaengine_desc_callback_invoke(&cb, NULL);
 			spin_lock_irq(&chan->lock);
 		}
 	}
@@ -1418,14 +1487,15 @@ static irqreturn_t rcar_dmac_isr_channel_thread(int irq, void *dev)
 		dma_cookie_complete(&desc->async_tx);
 		list_del(&desc->node);
 
-		if (desc->async_tx.callback) {
+		dmaengine_desc_get_callback(&desc->async_tx, &cb);
+		if (dmaengine_desc_callback_valid(&cb)) {
 			spin_unlock_irq(&chan->lock);
 			/*
 			 * We own the only reference to this descriptor, we can
 			 * safely dereference it without holding the channel
 			 * lock.
 			 */
-			desc->async_tx.callback(desc->async_tx.callback_param);
+			dmaengine_desc_callback_invoke(&cb, NULL);
 			spin_lock_irq(&chan->lock);
 		}
 
diff --git a/drivers/dma/sh/shdma-base.c b/drivers/dma/sh/shdma-base.c
index 10fcabad80f3..12fa48e380cf 100644
--- a/drivers/dma/sh/shdma-base.c
+++ b/drivers/dma/sh/shdma-base.c
@@ -330,10 +330,11 @@ static dma_async_tx_callback __ld_cleanup(struct shdma_chan *schan, bool all)
 	bool head_acked = false;
 	dma_cookie_t cookie = 0;
 	dma_async_tx_callback callback = NULL;
-	void *param = NULL;
+	struct dmaengine_desc_callback cb;
 	unsigned long flags;
 	LIST_HEAD(cyclic_list);
 
+	memset(&cb, 0, sizeof(cb));
 	spin_lock_irqsave(&schan->chan_lock, flags);
 	list_for_each_entry_safe(desc, _desc, &schan->ld_queue, node) {
 		struct dma_async_tx_descriptor *tx = &desc->async_tx;
@@ -367,8 +368,8 @@ static dma_async_tx_callback __ld_cleanup(struct shdma_chan *schan, bool all)
 		/* Call callback on the last chunk */
 		if (desc->mark == DESC_COMPLETED && tx->callback) {
 			desc->mark = DESC_WAITING;
+			dmaengine_desc_get_callback(tx, &cb);
 			callback = tx->callback;
-			param = tx->callback_param;
 			dev_dbg(schan->dev, "descriptor #%d@%p on %d callback\n",
 				tx->cookie, tx, schan->id);
 			BUG_ON(desc->chunks != 1);
@@ -430,8 +431,7 @@ static dma_async_tx_callback __ld_cleanup(struct shdma_chan *schan, bool all)
 
 	spin_unlock_irqrestore(&schan->chan_lock, flags);
 
-	if (callback)
-		callback(param);
+	dmaengine_desc_callback_invoke(&cb, NULL);
 
 	return callback;
 }
@@ -885,9 +885,9 @@ bool shdma_reset(struct shdma_dev *sdev)
 		/* Complete all  */
 		list_for_each_entry(sdesc, &dl, node) {
 			struct dma_async_tx_descriptor *tx = &sdesc->async_tx;
+
 			sdesc->mark = DESC_IDLE;
-			if (tx->callback)
-				tx->callback(tx->callback_param);
+			dmaengine_desc_get_callback_invoke(tx, NULL);
 		}
 
 		spin_lock(&schan->chan_lock);
diff --git a/drivers/dma/sirf-dma.c b/drivers/dma/sirf-dma.c
index d8bc3f2a71db..8f62edad51be 100644
--- a/drivers/dma/sirf-dma.c
+++ b/drivers/dma/sirf-dma.c
@@ -360,9 +360,7 @@ static void sirfsoc_dma_process_completed(struct sirfsoc_dma *sdma)
 			list_for_each_entry(sdesc, &list, node) {
 				desc = &sdesc->desc;
 
-				if (desc->callback)
-					desc->callback(desc->callback_param);
-
+				dmaengine_desc_get_callback_invoke(desc, NULL);
 				last_cookie = desc->cookie;
 				dma_run_dependencies(desc);
 			}
@@ -388,8 +386,7 @@ static void sirfsoc_dma_process_completed(struct sirfsoc_dma *sdma)
 
 			desc = &sdesc->desc;
 			while (happened_cyclic != schan->completed_cyclic) {
-				if (desc->callback)
-					desc->callback(desc->callback_param);
+				dmaengine_desc_get_callback_invoke(desc, NULL);
 				schan->completed_cyclic++;
 			}
 		}
@@ -869,7 +866,7 @@ static int sirfsoc_dma_probe(struct platform_device *op)
 	}
 
 	sdma->irq = irq_of_parse_and_map(dn, 0);
-	if (sdma->irq == NO_IRQ) {
+	if (!sdma->irq) {
 		dev_err(dev, "Error mapping IRQ!\n");
 		return -EINVAL;
 	}
diff --git a/drivers/dma/ste_dma40.c b/drivers/dma/ste_dma40.c
index 8b18e44a02d5..8684d11b29bb 100644
--- a/drivers/dma/ste_dma40.c
+++ b/drivers/dma/ste_dma40.c
@@ -874,7 +874,7 @@ static void d40_log_lli_to_lcxa(struct d40_chan *chan, struct d40_desc *desc)
 	}
 
 	if (curr_lcla < 0)
-		goto out;
+		goto set_current;
 
 	for (; lli_current < lli_len; lli_current++) {
 		unsigned int lcla_offset = chan->phy_chan->num * 1024 +
@@ -925,8 +925,7 @@ static void d40_log_lli_to_lcxa(struct d40_chan *chan, struct d40_desc *desc)
 			break;
 		}
 	}
-
-out:
+ set_current:
 	desc->lli_current = lli_current;
 }
 
@@ -941,15 +940,7 @@ static void d40_desc_load(struct d40_chan *d40c, struct d40_desc *d40d)
 
 static struct d40_desc *d40_first_active_get(struct d40_chan *d40c)
 {
-	struct d40_desc *d;
-
-	if (list_empty(&d40c->active))
-		return NULL;
-
-	d = list_first_entry(&d40c->active,
-			     struct d40_desc,
-			     node);
-	return d;
+	return list_first_entry_or_null(&d40c->active, struct d40_desc, node);
 }
 
 /* remove desc from current queue and add it to the pending_queue */
@@ -962,36 +953,18 @@ static void d40_desc_queue(struct d40_chan *d40c, struct d40_desc *desc)
 
 static struct d40_desc *d40_first_pending(struct d40_chan *d40c)
 {
-	struct d40_desc *d;
-
-	if (list_empty(&d40c->pending_queue))
-		return NULL;
-
-	d = list_first_entry(&d40c->pending_queue,
-			     struct d40_desc,
-			     node);
-	return d;
+	return list_first_entry_or_null(&d40c->pending_queue, struct d40_desc,
+					node);
 }
 
 static struct d40_desc *d40_first_queued(struct d40_chan *d40c)
 {
-	struct d40_desc *d;
-
-	if (list_empty(&d40c->queue))
-		return NULL;
-
-	d = list_first_entry(&d40c->queue,
-			     struct d40_desc,
-			     node);
-	return d;
+	return list_first_entry_or_null(&d40c->queue, struct d40_desc, node);
 }
 
 static struct d40_desc *d40_first_done(struct d40_chan *d40c)
 {
-	if (list_empty(&d40c->done))
-		return NULL;
-
-	return list_first_entry(&d40c->done, struct d40_desc, node);
+	return list_first_entry_or_null(&d40c->done, struct d40_desc, node);
 }
 
 static int d40_psize_2_burst_size(bool is_log, int psize)
@@ -1083,7 +1056,7 @@ static int __d40_execute_command_phy(struct d40_chan *d40c,
 			D40_CHAN_POS(d40c->phy_chan->num);
 
 		if (status == D40_DMA_SUSPENDED || status == D40_DMA_STOP)
-			goto done;
+			goto unlock;
 	}
 
 	wmask = 0xffffffff & ~(D40_CHAN_POS_MASK(d40c->phy_chan->num));
@@ -1119,7 +1092,7 @@ static int __d40_execute_command_phy(struct d40_chan *d40c,
 		}
 
 	}
-done:
+ unlock:
 	spin_unlock_irqrestore(&d40c->base->execmd_lock, flags);
 	return ret;
 }
@@ -1596,8 +1569,7 @@ static void dma_tasklet(unsigned long data)
 	struct d40_desc *d40d;
 	unsigned long flags;
 	bool callback_active;
-	dma_async_tx_callback callback;
-	void *callback_param;
+	struct dmaengine_desc_callback cb;
 
 	spin_lock_irqsave(&d40c->lock, flags);
 
@@ -1607,7 +1579,7 @@ static void dma_tasklet(unsigned long data)
 		/* Check if we have reached here for cyclic job */
 		d40d = d40_first_active_get(d40c);
 		if (d40d == NULL || !d40d->cyclic)
-			goto err;
+			goto check_pending_tx;
 	}
 
 	if (!d40d->cyclic)
@@ -1624,8 +1596,7 @@ static void dma_tasklet(unsigned long data)
 
 	/* Callback to client */
 	callback_active = !!(d40d->txd.flags & DMA_PREP_INTERRUPT);
-	callback = d40d->txd.callback;
-	callback_param = d40d->txd.callback_param;
+	dmaengine_desc_get_callback(&d40d->txd, &cb);
 
 	if (!d40d->cyclic) {
 		if (async_tx_test_ack(&d40d->txd)) {
@@ -1646,12 +1617,11 @@ static void dma_tasklet(unsigned long data)
 
 	spin_unlock_irqrestore(&d40c->lock, flags);
 
-	if (callback_active && callback)
-		callback(callback_param);
+	if (callback_active)
+		dmaengine_desc_callback_invoke(&cb, NULL);
 
 	return;
-
-err:
+ check_pending_tx:
 	/* Rescue manouver if receiving double interrupts */
 	if (d40c->pending_tx > 0)
 		d40c->pending_tx--;
@@ -1780,42 +1750,40 @@ static bool d40_alloc_mask_set(struct d40_phy_res *phy,
 		    phy->allocated_dst == D40_ALLOC_FREE) {
 			phy->allocated_dst = D40_ALLOC_PHY;
 			phy->allocated_src = D40_ALLOC_PHY;
-			goto found;
+			goto found_unlock;
 		} else
-			goto not_found;
+			goto not_found_unlock;
 	}
 
 	/* Logical channel */
 	if (is_src) {
 		if (phy->allocated_src == D40_ALLOC_PHY)
-			goto not_found;
+			goto not_found_unlock;
 
 		if (phy->allocated_src == D40_ALLOC_FREE)
 			phy->allocated_src = D40_ALLOC_LOG_FREE;
 
 		if (!(phy->allocated_src & BIT(log_event_line))) {
 			phy->allocated_src |= BIT(log_event_line);
-			goto found;
+			goto found_unlock;
 		} else
-			goto not_found;
+			goto not_found_unlock;
 	} else {
 		if (phy->allocated_dst == D40_ALLOC_PHY)
-			goto not_found;
+			goto not_found_unlock;
 
 		if (phy->allocated_dst == D40_ALLOC_FREE)
 			phy->allocated_dst = D40_ALLOC_LOG_FREE;
 
 		if (!(phy->allocated_dst & BIT(log_event_line))) {
 			phy->allocated_dst |= BIT(log_event_line);
-			goto found;
-		} else
-			goto not_found;
+			goto found_unlock;
+		}
 	}
-
-not_found:
+ not_found_unlock:
 	spin_unlock_irqrestore(&phy->lock, flags);
 	return false;
-found:
+ found_unlock:
 	spin_unlock_irqrestore(&phy->lock, flags);
 	return true;
 }
@@ -1831,7 +1799,7 @@ static bool d40_alloc_mask_free(struct d40_phy_res *phy, bool is_src,
 		phy->allocated_dst = D40_ALLOC_FREE;
 		phy->allocated_src = D40_ALLOC_FREE;
 		is_free = true;
-		goto out;
+		goto unlock;
 	}
 
 	/* Logical channel */
@@ -1847,8 +1815,7 @@ static bool d40_alloc_mask_free(struct d40_phy_res *phy, bool is_src,
 
 	is_free = ((phy->allocated_src | phy->allocated_dst) ==
 		   D40_ALLOC_FREE);
-
-out:
+ unlock:
 	spin_unlock_irqrestore(&phy->lock, flags);
 
 	return is_free;
@@ -2047,7 +2014,7 @@ static int d40_free_dma(struct d40_chan *d40c)
 	res = d40_channel_execute_command(d40c, D40_DMA_STOP);
 	if (res) {
 		chan_err(d40c, "stop failed\n");
-		goto out;
+		goto mark_last_busy;
 	}
 
 	d40_alloc_mask_free(phy, is_src, chan_is_logical(d40c) ? event : 0);
@@ -2065,8 +2032,7 @@ static int d40_free_dma(struct d40_chan *d40c)
 	d40c->busy = false;
 	d40c->phy_chan = NULL;
 	d40c->configured = false;
-out:
-
+ mark_last_busy:
 	pm_runtime_mark_last_busy(d40c->base->dev);
 	pm_runtime_put_autosuspend(d40c->base->dev);
 	return res;
@@ -2094,8 +2060,7 @@ static bool d40_is_paused(struct d40_chan *d40c)
 			D40_CHAN_POS(d40c->phy_chan->num);
 		if (status == D40_DMA_SUSPENDED || status == D40_DMA_STOP)
 			is_paused = true;
-
-		goto _exit;
+		goto unlock;
 	}
 
 	if (d40c->dma_cfg.dir == DMA_MEM_TO_DEV ||
@@ -2105,7 +2070,7 @@ static bool d40_is_paused(struct d40_chan *d40c)
 		status = readl(chanbase + D40_CHAN_REG_SSLNK);
 	} else {
 		chan_err(d40c, "Unknown direction\n");
-		goto _exit;
+		goto unlock;
 	}
 
 	status = (status & D40_EVENTLINE_MASK(event)) >>
@@ -2113,7 +2078,7 @@ static bool d40_is_paused(struct d40_chan *d40c)
 
 	if (status != D40_DMA_RUN)
 		is_paused = true;
-_exit:
+ unlock:
 	spin_unlock_irqrestore(&d40c->lock, flags);
 	return is_paused;
 
@@ -2198,7 +2163,7 @@ static struct d40_desc *
 d40_prep_desc(struct d40_chan *chan, struct scatterlist *sg,
 	      unsigned int sg_len, unsigned long dma_flags)
 {
-	struct stedma40_chan_cfg *cfg = &chan->dma_cfg;
+	struct stedma40_chan_cfg *cfg;
 	struct d40_desc *desc;
 	int ret;
 
@@ -2206,17 +2171,18 @@ d40_prep_desc(struct d40_chan *chan, struct scatterlist *sg,
 	if (!desc)
 		return NULL;
 
+	cfg = &chan->dma_cfg;
 	desc->lli_len = d40_sg_2_dmalen(sg, sg_len, cfg->src_info.data_width,
 					cfg->dst_info.data_width);
 	if (desc->lli_len < 0) {
 		chan_err(chan, "Unaligned size\n");
-		goto err;
+		goto free_desc;
 	}
 
 	ret = d40_pool_lli_alloc(chan, desc, desc->lli_len);
 	if (ret < 0) {
 		chan_err(chan, "Could not allocate lli\n");
-		goto err;
+		goto free_desc;
 	}
 
 	desc->lli_current = 0;
@@ -2226,8 +2192,7 @@ d40_prep_desc(struct d40_chan *chan, struct scatterlist *sg,
 	dma_async_tx_descriptor_init(&desc->txd, &chan->chan);
 
 	return desc;
-
-err:
+ free_desc:
 	d40_desc_free(chan, desc);
 	return NULL;
 }
@@ -2238,8 +2203,8 @@ d40_prep_sg(struct dma_chan *dchan, struct scatterlist *sg_src,
 	    enum dma_transfer_direction direction, unsigned long dma_flags)
 {
 	struct d40_chan *chan = container_of(dchan, struct d40_chan, chan);
-	dma_addr_t src_dev_addr = 0;
-	dma_addr_t dst_dev_addr = 0;
+	dma_addr_t src_dev_addr;
+	dma_addr_t dst_dev_addr;
 	struct d40_desc *desc;
 	unsigned long flags;
 	int ret;
@@ -2253,11 +2218,13 @@ d40_prep_sg(struct dma_chan *dchan, struct scatterlist *sg_src,
 
 	desc = d40_prep_desc(chan, sg_src, sg_len, dma_flags);
 	if (desc == NULL)
-		goto err;
+		goto unlock;
 
 	if (sg_next(&sg_src[sg_len - 1]) == sg_src)
 		desc->cyclic = true;
 
+	src_dev_addr = 0;
+	dst_dev_addr = 0;
 	if (direction == DMA_DEV_TO_MEM)
 		src_dev_addr = chan->runtime_addr;
 	else if (direction == DMA_MEM_TO_DEV)
@@ -2273,7 +2240,7 @@ d40_prep_sg(struct dma_chan *dchan, struct scatterlist *sg_src,
 	if (ret) {
 		chan_err(chan, "Failed to prepare %s sg job: %d\n",
 			 chan_is_logical(chan) ? "log" : "phy", ret);
-		goto err;
+		goto free_desc;
 	}
 
 	/*
@@ -2285,10 +2252,9 @@ d40_prep_sg(struct dma_chan *dchan, struct scatterlist *sg_src,
 	spin_unlock_irqrestore(&chan->lock, flags);
 
 	return &desc->txd;
-
-err:
-	if (desc)
-		d40_desc_free(chan, desc);
+ free_desc:
+	d40_desc_free(chan, desc);
+ unlock:
 	spin_unlock_irqrestore(&chan->lock, flags);
 	return NULL;
 }
@@ -2426,7 +2392,7 @@ static int d40_alloc_chan_resources(struct dma_chan *chan)
 		err = d40_config_memcpy(d40c);
 		if (err) {
 			chan_err(d40c, "Failed to configure memcpy channel\n");
-			goto fail;
+			goto mark_last_busy;
 		}
 	}
 
@@ -2434,7 +2400,7 @@ static int d40_alloc_chan_resources(struct dma_chan *chan)
 	if (err) {
 		chan_err(d40c, "Failed to allocate channel\n");
 		d40c->configured = false;
-		goto fail;
+		goto mark_last_busy;
 	}
 
 	pm_runtime_get_sync(d40c->base->dev);
@@ -2468,7 +2434,7 @@ static int d40_alloc_chan_resources(struct dma_chan *chan)
 	 */
 	if (is_free_phy)
 		d40_config_write(d40c);
-fail:
+ mark_last_busy:
 	pm_runtime_mark_last_busy(d40c->base->dev);
 	pm_runtime_put_autosuspend(d40c->base->dev);
 	spin_unlock_irqrestore(&d40c->lock, flags);
@@ -2891,7 +2857,7 @@ static int __init d40_dmaengine_init(struct d40_base *base,
 
 	if (err) {
 		d40_err(base->dev, "Failed to register slave channels\n");
-		goto failure1;
+		goto exit;
 	}
 
 	d40_chan_init(base, &base->dma_memcpy, base->log_chans,
@@ -2908,7 +2874,7 @@ static int __init d40_dmaengine_init(struct d40_base *base,
 	if (err) {
 		d40_err(base->dev,
 			"Failed to register memcpy only channels\n");
-		goto failure2;
+		goto unregister_slave;
 	}
 
 	d40_chan_init(base, &base->dma_both, base->phy_chans,
@@ -2926,14 +2892,14 @@ static int __init d40_dmaengine_init(struct d40_base *base,
 	if (err) {
 		d40_err(base->dev,
 			"Failed to register logical and physical capable channels\n");
-		goto failure3;
+		goto unregister_memcpy;
 	}
 	return 0;
-failure3:
+ unregister_memcpy:
 	dma_async_device_unregister(&base->dma_memcpy);
-failure2:
+ unregister_slave:
 	dma_async_device_unregister(&base->dma_slave);
-failure1:
+ exit:
 	return err;
 }
 
@@ -3144,11 +3110,11 @@ static int __init d40_phy_res_init(struct d40_base *base)
 static struct d40_base * __init d40_hw_detect_init(struct platform_device *pdev)
 {
 	struct stedma40_platform_data *plat_data = dev_get_platdata(&pdev->dev);
-	struct clk *clk = NULL;
-	void __iomem *virtbase = NULL;
-	struct resource *res = NULL;
-	struct d40_base *base = NULL;
-	int num_log_chans = 0;
+	struct clk *clk;
+	void __iomem *virtbase;
+	struct resource *res;
+	struct d40_base *base;
+	int num_log_chans;
 	int num_phy_chans;
 	int num_memcpy_chans;
 	int clk_ret = -EINVAL;
@@ -3160,27 +3126,27 @@ static struct d40_base * __init d40_hw_detect_init(struct platform_device *pdev)
 	clk = clk_get(&pdev->dev, NULL);
 	if (IS_ERR(clk)) {
 		d40_err(&pdev->dev, "No matching clock found\n");
-		goto failure;
+		goto check_prepare_enabled;
 	}
 
 	clk_ret = clk_prepare_enable(clk);
 	if (clk_ret) {
 		d40_err(&pdev->dev, "Failed to prepare/enable clock\n");
-		goto failure;
+		goto disable_unprepare;
 	}
 
 	/* Get IO for DMAC base address */
 	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "base");
 	if (!res)
-		goto failure;
+		goto disable_unprepare;
 
 	if (request_mem_region(res->start, resource_size(res),
 			       D40_NAME " I/O base") == NULL)
-		goto failure;
+		goto release_region;
 
 	virtbase = ioremap(res->start, resource_size(res));
 	if (!virtbase)
-		goto failure;
+		goto release_region;
 
 	/* This is just a regular AMBA PrimeCell ID actually */
 	for (pid = 0, i = 0; i < 4; i++)
@@ -3192,13 +3158,13 @@ static struct d40_base * __init d40_hw_detect_init(struct platform_device *pdev)
 
 	if (cid != AMBA_CID) {
 		d40_err(&pdev->dev, "Unknown hardware! No PrimeCell ID\n");
-		goto failure;
+		goto unmap_io;
 	}
 	if (AMBA_MANF_BITS(pid) != AMBA_VENDOR_ST) {
 		d40_err(&pdev->dev, "Unknown designer! Got %x wanted %x\n",
 			AMBA_MANF_BITS(pid),
 			AMBA_VENDOR_ST);
-		goto failure;
+		goto unmap_io;
 	}
 	/*
 	 * HW revision:
@@ -3212,7 +3178,7 @@ static struct d40_base * __init d40_hw_detect_init(struct platform_device *pdev)
 	rev = AMBA_REV_BITS(pid);
 	if (rev < 2) {
 		d40_err(&pdev->dev, "hardware revision: %d is not supported", rev);
-		goto failure;
+		goto unmap_io;
 	}
 
 	/* The number of physical channels on this HW */
@@ -3238,7 +3204,7 @@ static struct d40_base * __init d40_hw_detect_init(struct platform_device *pdev)
 		       sizeof(struct d40_chan), GFP_KERNEL);
 
 	if (base == NULL)
-		goto failure;
+		goto unmap_io;
 
 	base->rev = rev;
 	base->clk = clk;
@@ -3283,65 +3249,66 @@ static struct d40_base * __init d40_hw_detect_init(struct platform_device *pdev)
 		base->gen_dmac.init_reg_size = ARRAY_SIZE(dma_init_reg_v4a);
 	}
 
-	base->phy_res = kzalloc(num_phy_chans * sizeof(struct d40_phy_res),
+	base->phy_res = kcalloc(num_phy_chans,
+				sizeof(*base->phy_res),
 				GFP_KERNEL);
 	if (!base->phy_res)
-		goto failure;
+		goto free_base;
 
-	base->lookup_phy_chans = kzalloc(num_phy_chans *
-					 sizeof(struct d40_chan *),
+	base->lookup_phy_chans = kcalloc(num_phy_chans,
+					 sizeof(*base->lookup_phy_chans),
 					 GFP_KERNEL);
 	if (!base->lookup_phy_chans)
-		goto failure;
+		goto free_phy_res;
 
-	base->lookup_log_chans = kzalloc(num_log_chans *
-					 sizeof(struct d40_chan *),
+	base->lookup_log_chans = kcalloc(num_log_chans,
+					 sizeof(*base->lookup_log_chans),
 					 GFP_KERNEL);
 	if (!base->lookup_log_chans)
-		goto failure;
+		goto free_phy_chans;
 
-	base->reg_val_backup_chan = kmalloc(base->num_phy_chans *
-					    sizeof(d40_backup_regs_chan),
-					    GFP_KERNEL);
+	base->reg_val_backup_chan = kmalloc_array(base->num_phy_chans,
+						  sizeof(d40_backup_regs_chan),
+						  GFP_KERNEL);
 	if (!base->reg_val_backup_chan)
-		goto failure;
+		goto free_log_chans;
 
-	base->lcla_pool.alloc_map =
-		kzalloc(num_phy_chans * sizeof(struct d40_desc *)
-			* D40_LCLA_LINK_PER_EVENT_GRP, GFP_KERNEL);
+	base->lcla_pool.alloc_map = kcalloc(num_phy_chans
+					    * D40_LCLA_LINK_PER_EVENT_GRP,
+					    sizeof(*base->lcla_pool.alloc_map),
+					    GFP_KERNEL);
 	if (!base->lcla_pool.alloc_map)
-		goto failure;
+		goto free_backup_chan;
 
 	base->desc_slab = kmem_cache_create(D40_NAME, sizeof(struct d40_desc),
 					    0, SLAB_HWCACHE_ALIGN,
 					    NULL);
 	if (base->desc_slab == NULL)
-		goto failure;
+		goto free_map;
 
 	return base;
-
-failure:
+ free_map:
+	kfree(base->lcla_pool.alloc_map);
+ free_backup_chan:
+	kfree(base->reg_val_backup_chan);
+ free_log_chans:
+	kfree(base->lookup_log_chans);
+ free_phy_chans:
+	kfree(base->lookup_phy_chans);
+ free_phy_res:
+	kfree(base->phy_res);
+ free_base:
+	kfree(base);
+ unmap_io:
+	iounmap(virtbase);
+ release_region:
+	release_mem_region(res->start, resource_size(res));
+ check_prepare_enabled:
 	if (!clk_ret)
+ disable_unprepare:
 		clk_disable_unprepare(clk);
 	if (!IS_ERR(clk))
 		clk_put(clk);
-	if (virtbase)
-		iounmap(virtbase);
-	if (res)
-		release_mem_region(res->start,
-				   resource_size(res));
-	if (virtbase)
-		iounmap(virtbase);
-
-	if (base) {
-		kfree(base->lcla_pool.alloc_map);
-		kfree(base->reg_val_backup_chan);
-		kfree(base->lookup_log_chans);
-		kfree(base->lookup_phy_chans);
-		kfree(base->phy_res);
-		kfree(base);
-	}
-
 	return NULL;
 }
 
@@ -3404,20 +3371,18 @@ static int __init d40_lcla_allocate(struct d40_base *base)
 	struct d40_lcla_pool *pool = &base->lcla_pool;
 	unsigned long *page_list;
 	int i, j;
-	int ret = 0;
+	int ret;
 
 	/*
 	 * This is somewhat ugly. We need 8192 bytes that are 18 bit aligned,
 	 * To full fill this hardware requirement without wasting 256 kb
 	 * we allocate pages until we get an aligned one.
 	 */
-	page_list = kmalloc(sizeof(unsigned long) * MAX_LCLA_ALLOC_ATTEMPTS,
-			    GFP_KERNEL);
-
-	if (!page_list) {
-		ret = -ENOMEM;
-		goto failure;
-	}
+	page_list = kmalloc_array(MAX_LCLA_ALLOC_ATTEMPTS,
+				  sizeof(*page_list),
+				  GFP_KERNEL);
+	if (!page_list)
+		return -ENOMEM;
 
 	/* Calculating how many pages that are required */
 	base->lcla_pool.pages = SZ_1K * base->num_phy_chans / PAGE_SIZE;
@@ -3433,7 +3398,7 @@ static int __init d40_lcla_allocate(struct d40_base *base)
 
 			for (j = 0; j < i; j++)
 				free_pages(page_list[j], base->lcla_pool.pages);
-			goto failure;
+			goto free_page_list;
 		}
 
 		if ((virt_to_phys((void *)page_list[i]) &
@@ -3460,7 +3425,7 @@ static int __init d40_lcla_allocate(struct d40_base *base)
 							 GFP_KERNEL);
 		if (!base->lcla_pool.base_unaligned) {
 			ret = -ENOMEM;
-			goto failure;
+			goto free_page_list;
 		}
 
 		base->lcla_pool.base = PTR_ALIGN(base->lcla_pool.base_unaligned,
@@ -3473,12 +3438,13 @@ static int __init d40_lcla_allocate(struct d40_base *base)
 	if (dma_mapping_error(base->dev, pool->dma_addr)) {
 		pool->dma_addr = 0;
 		ret = -ENOMEM;
-		goto failure;
+		goto free_page_list;
 	}
 
 	writel(virt_to_phys(base->lcla_pool.base),
 	       base->virtbase + D40_DREG_LCLA);
-failure:
+	ret = 0;
+ free_page_list:
 	kfree(page_list);
 	return ret;
 }
@@ -3490,9 +3456,7 @@ static int __init d40_of_probe(struct platform_device *pdev,
 	int num_phy = 0, num_memcpy = 0, num_disabled = 0;
 	const __be32 *list;
 
-	pdata = devm_kzalloc(&pdev->dev,
-			     sizeof(struct stedma40_platform_data),
-			     GFP_KERNEL);
+	pdata = devm_kzalloc(&pdev->dev, sizeof(*pdata), GFP_KERNEL);
 	if (!pdata)
 		return -ENOMEM;
 
@@ -3574,7 +3538,7 @@ static int __init d40_probe(struct platform_device *pdev)
 	if (!res) {
 		ret = -ENOENT;
 		d40_err(&pdev->dev, "No \"lcpa\" memory resource\n");
-		goto failure;
+		goto destroy_cache;
 	}
 	base->lcpa_size = resource_size(res);
 	base->phy_lcpa = res->start;
@@ -3583,7 +3547,7 @@ static int __init d40_probe(struct platform_device *pdev)
 			       D40_NAME " I/O lcpa") == NULL) {
 		ret = -EBUSY;
 		d40_err(&pdev->dev, "Failed to request LCPA region %pR\n", res);
-		goto failure;
+		goto destroy_cache;
 	}
 
 	/* We make use of ESRAM memory for this. */
@@ -3599,7 +3563,7 @@ static int __init d40_probe(struct platform_device *pdev)
 	if (!base->lcpa_base) {
 		ret = -ENOMEM;
 		d40_err(&pdev->dev, "Failed to ioremap LCPA region\n");
-		goto failure;
+		goto destroy_cache;
 	}
 	/* If lcla has to be located in ESRAM we don't need to allocate */
 	if (base->plat_data->use_esram_lcla) {
@@ -3609,14 +3573,14 @@ static int __init d40_probe(struct platform_device *pdev)
 			ret = -ENOENT;
 			d40_err(&pdev->dev,
 				"No \"lcla_esram\" memory resource\n");
-			goto failure;
+			goto destroy_cache;
 		}
 		base->lcla_pool.base = ioremap(res->start,
 						resource_size(res));
 		if (!base->lcla_pool.base) {
 			ret = -ENOMEM;
 			d40_err(&pdev->dev, "Failed to ioremap LCLA region\n");
-			goto failure;
+			goto destroy_cache;
 		}
 		writel(res->start, base->virtbase + D40_DREG_LCLA);
 
@@ -3624,7 +3588,7 @@ static int __init d40_probe(struct platform_device *pdev)
 		ret = d40_lcla_allocate(base);
 		if (ret) {
 			d40_err(&pdev->dev, "Failed to allocate LCLA area\n");
-			goto failure;
+			goto destroy_cache;
 		}
 	}
 
@@ -3635,7 +3599,7 @@ static int __init d40_probe(struct platform_device *pdev)
 	ret = request_irq(base->irq, d40_handle_interrupt, 0, D40_NAME, base);
 	if (ret) {
 		d40_err(&pdev->dev, "No IRQ defined\n");
-		goto failure;
+		goto destroy_cache;
 	}
 
 	if (base->plat_data->use_esram_lcla) {
@@ -3645,7 +3609,7 @@ static int __init d40_probe(struct platform_device *pdev)
 			d40_err(&pdev->dev, "Failed to get lcpa_regulator\n");
 			ret = PTR_ERR(base->lcpa_regulator);
 			base->lcpa_regulator = NULL;
-			goto failure;
+			goto destroy_cache;
 		}
 
 		ret = regulator_enable(base->lcpa_regulator);
@@ -3654,7 +3618,7 @@ static int __init d40_probe(struct platform_device *pdev)
 				"Failed to enable lcpa_regulator\n");
 			regulator_put(base->lcpa_regulator);
 			base->lcpa_regulator = NULL;
-			goto failure;
+			goto destroy_cache;
 		}
 	}
 
@@ -3669,13 +3633,13 @@ static int __init d40_probe(struct platform_device *pdev)
 
 	ret = d40_dmaengine_init(base, num_reserved_chans);
 	if (ret)
-		goto failure;
+		goto destroy_cache;
 
 	base->dev->dma_parms = &base->dma_parms;
 	ret = dma_set_max_seg_size(base->dev, STEDMA40_MAX_SEG_SIZE);
 	if (ret) {
 		d40_err(&pdev->dev, "Failed to set dma max seg size\n");
-		goto failure;
+		goto destroy_cache;
 	}
 
 	d40_hw_init(base);
@@ -3689,8 +3653,7 @@ static int __init d40_probe(struct platform_device *pdev)
 
 	dev_info(base->dev, "initialized\n");
 	return 0;
-
-failure:
+ destroy_cache:
 	kmem_cache_destroy(base->desc_slab);
 	if (base->virtbase)
 		iounmap(base->virtbase);
@@ -3732,7 +3695,7 @@ failure:
 	kfree(base->lookup_phy_chans);
 	kfree(base->phy_res);
 	kfree(base);
-report_failure:
+ report_failure:
 	d40_err(&pdev->dev, "probe failed\n");
 	return ret;
 }
diff --git a/drivers/dma/stm32-dma.c b/drivers/dma/stm32-dma.c
index 047476a1383d..307547f4848d 100644
--- a/drivers/dma/stm32-dma.c
+++ b/drivers/dma/stm32-dma.c
@@ -954,7 +954,7 @@ static void stm32_dma_desc_free(struct virt_dma_desc *vdesc)
 	kfree(container_of(vdesc, struct stm32_dma_desc, vdesc));
 }
 
-void stm32_dma_set_config(struct stm32_dma_chan *chan,
+static void stm32_dma_set_config(struct stm32_dma_chan *chan,
 			  struct stm32_dma_cfg *cfg)
 {
 	stm32_dma_clear_reg(&chan->chan_reg);
diff --git a/drivers/dma/sun6i-dma.c b/drivers/dma/sun6i-dma.c
index 3835fcde3545..83461994e418 100644
--- a/drivers/dma/sun6i-dma.c
+++ b/drivers/dma/sun6i-dma.c
@@ -1011,6 +1011,12 @@ static struct sun6i_dma_config sun8i_a23_dma_cfg = {
 	.nr_max_vchans   = 37,
 };
 
+static struct sun6i_dma_config sun8i_a83t_dma_cfg = {
+	.nr_max_channels = 8,
+	.nr_max_requests = 28,
+	.nr_max_vchans   = 39,
+};
+
 /*
  * The H3 has 12 physical channels, a maximum DRQ port id of 27,
  * and a total of 34 usable source and destination endpoints.
@@ -1025,6 +1031,7 @@ static struct sun6i_dma_config sun8i_h3_dma_cfg = {
 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-a83t-dma", .data = &sun8i_a83t_dma_cfg },
 	{ .compatible = "allwinner,sun8i-h3-dma", .data = &sun8i_h3_dma_cfg },
 	{ /* sentinel */ }
 };
diff --git a/drivers/dma/tegra20-apb-dma.c b/drivers/dma/tegra20-apb-dma.c
index 6ab9eb98588a..3722b9d8d9fe 100644
--- a/drivers/dma/tegra20-apb-dma.c
+++ b/drivers/dma/tegra20-apb-dma.c
@@ -655,8 +655,7 @@ static void handle_cont_sngl_cycle_dma_done(struct tegra_dma_channel *tdc,
 static void tegra_dma_tasklet(unsigned long data)
 {
 	struct tegra_dma_channel *tdc = (struct tegra_dma_channel *)data;
-	dma_async_tx_callback callback = NULL;
-	void *callback_param = NULL;
+	struct dmaengine_desc_callback cb;
 	struct tegra_dma_desc *dma_desc;
 	unsigned long flags;
 	int cb_count;
@@ -666,13 +665,12 @@ static void tegra_dma_tasklet(unsigned long data)
 		dma_desc  = list_first_entry(&tdc->cb_desc,
 					typeof(*dma_desc), cb_node);
 		list_del(&dma_desc->cb_node);
-		callback = dma_desc->txd.callback;
-		callback_param = dma_desc->txd.callback_param;
+		dmaengine_desc_get_callback(&dma_desc->txd, &cb);
 		cb_count = dma_desc->cb_count;
 		dma_desc->cb_count = 0;
 		spin_unlock_irqrestore(&tdc->lock, flags);
-		while (cb_count-- && callback)
-			callback(callback_param);
+		while (cb_count--)
+			dmaengine_desc_callback_invoke(&cb, NULL);
 		spin_lock_irqsave(&tdc->lock, flags);
 	}
 	spin_unlock_irqrestore(&tdc->lock, flags);
diff --git a/drivers/dma/tegra210-adma.c b/drivers/dma/tegra210-adma.c
index c4b121c4559d..b10cbaa82ff5 100644
--- a/drivers/dma/tegra210-adma.c
+++ b/drivers/dma/tegra210-adma.c
@@ -670,7 +670,6 @@ static int tegra_adma_probe(struct platform_device *pdev)
 	const struct tegra_adma_chip_data *cdata;
 	struct tegra_adma *tdma;
 	struct resource	*res;
-	struct clk *clk;
 	int ret, i;
 
 	cdata = of_device_get_match_data(&pdev->dev);
@@ -697,18 +696,9 @@ static int tegra_adma_probe(struct platform_device *pdev)
 	if (ret)
 		return ret;
 
-	clk = clk_get(&pdev->dev, "d_audio");
-	if (IS_ERR(clk)) {
-		dev_err(&pdev->dev, "ADMA clock not found\n");
-		ret = PTR_ERR(clk);
-		goto clk_destroy;
-	}
-
-	ret = pm_clk_add_clk(&pdev->dev, clk);
-	if (ret) {
-		clk_put(clk);
+	ret = of_pm_clk_add_clk(&pdev->dev, "d_audio");
+	if (ret)
 		goto clk_destroy;
-	}
 
 	pm_runtime_enable(&pdev->dev);
 
diff --git a/drivers/dma/ti-dma-crossbar.c b/drivers/dma/ti-dma-crossbar.c
index 5ae294b256a7..3f24aeb48c0e 100644
--- a/drivers/dma/ti-dma-crossbar.c
+++ b/drivers/dma/ti-dma-crossbar.c
@@ -18,15 +18,19 @@
 
 #define TI_XBAR_DRA7		0
 #define TI_XBAR_AM335X		1
+static const u32 ti_xbar_type[] = {
+	[TI_XBAR_DRA7] = TI_XBAR_DRA7,
+	[TI_XBAR_AM335X] = TI_XBAR_AM335X,
+};
 
 static const struct of_device_id ti_dma_xbar_match[] = {
 	{
 		.compatible = "ti,dra7-dma-crossbar",
-		.data = (void *)TI_XBAR_DRA7,
+		.data = &ti_xbar_type[TI_XBAR_DRA7],
 	},
 	{
 		.compatible = "ti,am335x-edma-crossbar",
-		.data = (void *)TI_XBAR_AM335X,
+		.data = &ti_xbar_type[TI_XBAR_AM335X],
 	},
 	{},
 };
@@ -190,9 +194,6 @@ static int ti_am335x_xbar_probe(struct platform_device *pdev)
 #define TI_DRA7_XBAR_OUTPUTS	127
 #define TI_DRA7_XBAR_INPUTS	256
 
-#define TI_XBAR_EDMA_OFFSET	0
-#define TI_XBAR_SDMA_OFFSET	1
-
 struct ti_dra7_xbar_data {
 	void __iomem *iomem;
 
@@ -280,18 +281,25 @@ static void *ti_dra7_xbar_route_allocate(struct of_phandle_args *dma_spec,
 	return map;
 }
 
+#define TI_XBAR_EDMA_OFFSET	0
+#define TI_XBAR_SDMA_OFFSET	1
+static const u32 ti_dma_offset[] = {
+	[TI_XBAR_EDMA_OFFSET] = 0,
+	[TI_XBAR_SDMA_OFFSET] = 1,
+};
+
 static const struct of_device_id ti_dra7_master_match[] = {
 	{
 		.compatible = "ti,omap4430-sdma",
-		.data = (void *)TI_XBAR_SDMA_OFFSET,
+		.data = &ti_dma_offset[TI_XBAR_SDMA_OFFSET],
 	},
 	{
 		.compatible = "ti,edma3",
-		.data = (void *)TI_XBAR_EDMA_OFFSET,
+		.data = &ti_dma_offset[TI_XBAR_EDMA_OFFSET],
 	},
 	{
 		.compatible = "ti,edma3-tpcc",
-		.data = (void *)TI_XBAR_EDMA_OFFSET,
+		.data = &ti_dma_offset[TI_XBAR_EDMA_OFFSET],
 	},
 	{},
 };
@@ -311,7 +319,7 @@ static int ti_dra7_xbar_probe(struct platform_device *pdev)
 	struct property *prop;
 	struct resource *res;
 	u32 safe_val;
-	size_t sz;
+	int sz;
 	void __iomem *iomem;
 	int i, ret;
 
@@ -395,7 +403,7 @@ static int ti_dra7_xbar_probe(struct platform_device *pdev)
 
 	xbar->dmarouter.dev = &pdev->dev;
 	xbar->dmarouter.route_free = ti_dra7_xbar_free;
-	xbar->dma_offset = (u32)match->data;
+	xbar->dma_offset = *(u32 *)match->data;
 
 	mutex_init(&xbar->mutex);
 	platform_set_drvdata(pdev, xbar);
@@ -428,7 +436,7 @@ static int ti_dma_xbar_probe(struct platform_device *pdev)
 	if (unlikely(!match))
 		return -EINVAL;
 
-	switch ((u32)match->data) {
+	switch (*(u32 *)match->data) {
 	case TI_XBAR_DRA7:
 		ret = ti_dra7_xbar_probe(pdev);
 		break;
diff --git a/drivers/dma/timb_dma.c b/drivers/dma/timb_dma.c
index e82745aa42a8..896bafb7a532 100644
--- a/drivers/dma/timb_dma.c
+++ b/drivers/dma/timb_dma.c
@@ -226,8 +226,7 @@ static void __td_start_dma(struct timb_dma_chan *td_chan)
 
 static void __td_finish(struct timb_dma_chan *td_chan)
 {
-	dma_async_tx_callback		callback;
-	void				*param;
+	struct dmaengine_desc_callback	cb;
 	struct dma_async_tx_descriptor	*txd;
 	struct timb_dma_desc		*td_desc;
 
@@ -252,8 +251,7 @@ static void __td_finish(struct timb_dma_chan *td_chan)
 	dma_cookie_complete(txd);
 	td_chan->ongoing = false;
 
-	callback = txd->callback;
-	param = txd->callback_param;
+	dmaengine_desc_get_callback(txd, &cb);
 
 	list_move(&td_desc->desc_node, &td_chan->free_list);
 
@@ -262,8 +260,7 @@ static void __td_finish(struct timb_dma_chan *td_chan)
 	 * The API requires that no submissions are done from a
 	 * callback, so we don't need to drop the lock here
 	 */
-	if (callback)
-		callback(param);
+	dmaengine_desc_callback_invoke(&cb, NULL);
 }
 
 static u32 __td_ier_mask(struct timb_dma *td)
diff --git a/drivers/dma/txx9dmac.c b/drivers/dma/txx9dmac.c
index 7632290e7c14..4d8c7b9078fd 100644
--- a/drivers/dma/txx9dmac.c
+++ b/drivers/dma/txx9dmac.c
@@ -403,16 +403,14 @@ static void
 txx9dmac_descriptor_complete(struct txx9dmac_chan *dc,
 			     struct txx9dmac_desc *desc)
 {
-	dma_async_tx_callback callback;
-	void *param;
+	struct dmaengine_desc_callback cb;
 	struct dma_async_tx_descriptor *txd = &desc->txd;
 
 	dev_vdbg(chan2dev(&dc->chan), "descriptor %u %p complete\n",
 		 txd->cookie, desc);
 
 	dma_cookie_complete(txd);
-	callback = txd->callback;
-	param = txd->callback_param;
+	dmaengine_desc_get_callback(txd, &cb);
 
 	txx9dmac_sync_desc_for_cpu(dc, desc);
 	list_splice_init(&desc->tx_list, &dc->free_list);
@@ -423,8 +421,7 @@ txx9dmac_descriptor_complete(struct txx9dmac_chan *dc,
 	 * The API requires that no submissions are done from a
 	 * callback, so we don't need to drop the lock here
 	 */
-	if (callback)
-		callback(param);
+	dmaengine_desc_callback_invoke(&cb, NULL);
 	dma_run_dependencies(txd);
 }
 
diff --git a/drivers/dma/virt-dma.c b/drivers/dma/virt-dma.c
index a35c211857dd..e47fc9b0944f 100644
--- a/drivers/dma/virt-dma.c
+++ b/drivers/dma/virt-dma.c
@@ -87,8 +87,7 @@ static void vchan_complete(unsigned long arg)
 {
 	struct virt_dma_chan *vc = (struct virt_dma_chan *)arg;
 	struct virt_dma_desc *vd;
-	dma_async_tx_callback cb = NULL;
-	void *cb_data = NULL;
+	struct dmaengine_desc_callback cb;
 	LIST_HEAD(head);
 
 	spin_lock_irq(&vc->lock);
@@ -96,18 +95,17 @@ static void vchan_complete(unsigned long arg)
 	vd = vc->cyclic;
 	if (vd) {
 		vc->cyclic = NULL;
-		cb = vd->tx.callback;
-		cb_data = vd->tx.callback_param;
+		dmaengine_desc_get_callback(&vd->tx, &cb);
+	} else {
+		memset(&cb, 0, sizeof(cb));
 	}
 	spin_unlock_irq(&vc->lock);
 
-	if (cb)
-		cb(cb_data);
+	dmaengine_desc_callback_invoke(&cb, NULL);
 
 	while (!list_empty(&head)) {
 		vd = list_first_entry(&head, struct virt_dma_desc, node);
-		cb = vd->tx.callback;
-		cb_data = vd->tx.callback_param;
+		dmaengine_desc_get_callback(&vd->tx, &cb);
 
 		list_del(&vd->node);
 		if (dmaengine_desc_test_reuse(&vd->tx))
@@ -115,8 +113,7 @@ static void vchan_complete(unsigned long arg)
 		else
 			vc->desc_free(vd);
 
-		if (cb)
-			cb(cb_data);
+		dmaengine_desc_callback_invoke(&cb, NULL);
 	}
 }
 
diff --git a/drivers/dma/virt-dma.h b/drivers/dma/virt-dma.h
index d9731ca5e262..3f776a46a29c 100644
--- a/drivers/dma/virt-dma.h
+++ b/drivers/dma/virt-dma.h
@@ -45,6 +45,8 @@ static inline struct virt_dma_chan *to_virt_chan(struct dma_chan *chan)
 void vchan_dma_desc_free_list(struct virt_dma_chan *vc, struct list_head *head);
 void vchan_init(struct virt_dma_chan *vc, struct dma_device *dmadev);
 struct virt_dma_desc *vchan_find_desc(struct virt_dma_chan *, dma_cookie_t);
+extern dma_cookie_t vchan_tx_submit(struct dma_async_tx_descriptor *);
+extern int vchan_tx_desc_free(struct dma_async_tx_descriptor *);
 
 /**
  * vchan_tx_prep - prepare a descriptor
@@ -55,8 +57,6 @@ struct virt_dma_desc *vchan_find_desc(struct virt_dma_chan *, dma_cookie_t);
 static inline struct dma_async_tx_descriptor *vchan_tx_prep(struct virt_dma_chan *vc,
 	struct virt_dma_desc *vd, unsigned long tx_flags)
 {
-	extern dma_cookie_t vchan_tx_submit(struct dma_async_tx_descriptor *);
-	extern int vchan_tx_desc_free(struct dma_async_tx_descriptor *);
 	unsigned long flags;
 
 	dma_async_tx_descriptor_init(&vd->tx, &vc->chan);
@@ -123,10 +123,8 @@ static inline void vchan_cyclic_callback(struct virt_dma_desc *vd)
  */
 static inline struct virt_dma_desc *vchan_next_desc(struct virt_dma_chan *vc)
 {
-	if (list_empty(&vc->desc_issued))
-		return NULL;
-
-	return list_first_entry(&vc->desc_issued, struct virt_dma_desc, node);
+	return list_first_entry_or_null(&vc->desc_issued,
+					struct virt_dma_desc, node);
 }
 
 /**
diff --git a/drivers/dma/xgene-dma.c b/drivers/dma/xgene-dma.c
index 9cb93c5b655d..8b693b712d0f 100644
--- a/drivers/dma/xgene-dma.c
+++ b/drivers/dma/xgene-dma.c
@@ -606,12 +606,10 @@ static void xgene_dma_run_tx_complete_actions(struct xgene_dma_chan *chan,
 		return;
 
 	dma_cookie_complete(tx);
+	dma_descriptor_unmap(tx);
 
 	/* Run the link descriptor callback function */
-	if (tx->callback)
-		tx->callback(tx->callback_param);
-
-	dma_descriptor_unmap(tx);
+	dmaengine_desc_get_callback_invoke(tx, NULL);
 
 	/* Run any dependencies */
 	dma_run_dependencies(tx);
diff --git a/drivers/dma/xilinx/xilinx_dma.c b/drivers/dma/xilinx/xilinx_dma.c
index 4e223d094433..8288fe4d17c3 100644
--- a/drivers/dma/xilinx/xilinx_dma.c
+++ b/drivers/dma/xilinx/xilinx_dma.c
@@ -755,8 +755,7 @@ static void xilinx_dma_chan_desc_cleanup(struct xilinx_dma_chan *chan)
 	spin_lock_irqsave(&chan->lock, flags);
 
 	list_for_each_entry_safe(desc, next, &chan->done_list, node) {
-		dma_async_tx_callback callback;
-		void *callback_param;
+		struct dmaengine_desc_callback cb;
 
 		if (desc->cyclic) {
 			xilinx_dma_chan_handle_cyclic(chan, desc, &flags);
@@ -767,11 +766,10 @@ static void xilinx_dma_chan_desc_cleanup(struct xilinx_dma_chan *chan)
 		list_del(&desc->node);
 
 		/* Run the link descriptor callback function */
-		callback = desc->async_tx.callback;
-		callback_param = desc->async_tx.callback_param;
-		if (callback) {
+		dmaengine_desc_get_callback(&desc->async_tx, &cb);
+		if (dmaengine_desc_callback_valid(&cb)) {
 			spin_unlock_irqrestore(&chan->lock, flags);
-			callback(callback_param);
+			dmaengine_desc_callback_invoke(&cb, NULL);
 			spin_lock_irqsave(&chan->lock, flags);
 		}
 
diff --git a/drivers/edac/Kconfig b/drivers/edac/Kconfig
index dff1a4a6dc1b..82d85cce81f8 100644
--- a/drivers/edac/Kconfig
+++ b/drivers/edac/Kconfig
@@ -266,6 +266,13 @@ config EDAC_MPC85XX
 	  Support for error detection and correction on the Freescale
 	  MPC8349, MPC8560, MPC8540, MPC8548, T4240
 
+config EDAC_LAYERSCAPE
+	tristate "Freescale Layerscape DDR"
+	depends on EDAC_MM_EDAC && ARCH_LAYERSCAPE
+	help
+	  Support for error detection and correction on Freescale memory
+	  controllers on Layerscape SoCs.
+
 config EDAC_MV64X60
 	tristate "Marvell MV64x60"
 	depends on EDAC_MM_EDAC && MV64X60
@@ -406,6 +413,41 @@ config EDAC_ALTERA_ETHERNET
 	  Support for error detection and correction on the
 	  Altera Ethernet FIFO Memory for Altera SoCs.
 
+config EDAC_ALTERA_NAND
+	bool "Altera NAND FIFO ECC"
+	depends on EDAC_ALTERA=y && MTD_NAND_DENALI
+	help
+	  Support for error detection and correction on the
+	  Altera NAND FIFO Memory for Altera SoCs.
+
+config EDAC_ALTERA_DMA
+	bool "Altera DMA FIFO ECC"
+	depends on EDAC_ALTERA=y && PL330_DMA=y
+	help
+	  Support for error detection and correction on the
+	  Altera DMA FIFO Memory for Altera SoCs.
+
+config EDAC_ALTERA_USB
+	bool "Altera USB FIFO ECC"
+	depends on EDAC_ALTERA=y && USB_DWC2
+	help
+	  Support for error detection and correction on the
+	  Altera USB FIFO Memory for Altera SoCs.
+
+config EDAC_ALTERA_QSPI
+	bool "Altera QSPI FIFO ECC"
+	depends on EDAC_ALTERA=y && SPI_CADENCE_QUADSPI
+	help
+	  Support for error detection and correction on the
+	  Altera QSPI FIFO Memory for Altera SoCs.
+
+config EDAC_ALTERA_SDMMC
+	bool "Altera SDMMC FIFO ECC"
+	depends on EDAC_ALTERA=y && MMC_DW
+	help
+	  Support for error detection and correction on the
+	  Altera SDMMC FIFO Memory for Altera SoCs.
+
 config EDAC_SYNOPSYS
 	tristate "Synopsys DDR Memory Controller"
 	depends on EDAC_MM_EDAC && ARCH_ZYNQ
diff --git a/drivers/edac/Makefile b/drivers/edac/Makefile
index 986049925b08..88e472e8b9a9 100644
--- a/drivers/edac/Makefile
+++ b/drivers/edac/Makefile
@@ -51,7 +51,13 @@ amd64_edac_mod-$(CONFIG_EDAC_AMD64_ERROR_INJECTION) += amd64_edac_inj.o
 obj-$(CONFIG_EDAC_AMD64)		+= amd64_edac_mod.o
 
 obj-$(CONFIG_EDAC_PASEMI)		+= pasemi_edac.o
-obj-$(CONFIG_EDAC_MPC85XX)		+= mpc85xx_edac.o
+
+mpc85xx_edac_mod-y			:= fsl_ddr_edac.o mpc85xx_edac.o
+obj-$(CONFIG_EDAC_MPC85XX)		+= mpc85xx_edac_mod.o
+
+layerscape_edac_mod-y			:= fsl_ddr_edac.o layerscape_edac.o
+obj-$(CONFIG_EDAC_LAYERSCAPE)		+= layerscape_edac_mod.o
+
 obj-$(CONFIG_EDAC_MV64X60)		+= mv64x60_edac.o
 obj-$(CONFIG_EDAC_CELL)			+= cell_edac.o
 obj-$(CONFIG_EDAC_PPC4XX)		+= ppc4xx_edac.o
diff --git a/drivers/edac/altera_edac.c b/drivers/edac/altera_edac.c
index 2398d0701f5b..58d3e2b39b5b 100644
--- a/drivers/edac/altera_edac.c
+++ b/drivers/edac/altera_edac.c
@@ -203,7 +203,7 @@ static void altr_sdr_mc_create_debugfs_nodes(struct mem_ctl_info *mci)
 	if (!mci->debugfs)
 		return;
 
-	edac_debugfs_create_file("inject_ctrl", S_IWUSR, mci->debugfs, mci,
+	edac_debugfs_create_file("altr_trigger", S_IWUSR, mci->debugfs, mci,
 				 &altr_sdr_mc_debug_inject_fops);
 }
 
@@ -680,7 +680,7 @@ static void altr_create_edacdev_dbgfs(struct edac_device_ctl_info *edac_dci,
 	if (!drvdata->debugfs_dir)
 		return;
 
-	if (!edac_debugfs_create_file(priv->dbgfs_name, S_IWUSR,
+	if (!edac_debugfs_create_file("altr_trigger", S_IWUSR,
 				      drvdata->debugfs_dir, edac_dci,
 				      priv->inject_fops))
 		debugfs_remove_recursive(drvdata->debugfs_dir);
@@ -1108,7 +1108,6 @@ static const struct edac_device_prv_data ocramecc_data = {
 	.setup = altr_check_ecc_deps,
 	.ce_clear_mask = (ALTR_OCR_ECC_EN | ALTR_OCR_ECC_SERR),
 	.ue_clear_mask = (ALTR_OCR_ECC_EN | ALTR_OCR_ECC_DERR),
-	.dbgfs_name = "altr_ocram_trigger",
 	.alloc_mem = ocram_alloc_mem,
 	.free_mem = ocram_free_mem,
 	.ecc_enable_mask = ALTR_OCR_ECC_EN,
@@ -1125,7 +1124,6 @@ static const struct edac_device_prv_data a10_ocramecc_data = {
 	.ce_clear_mask = ALTR_A10_ECC_SERRPENA,
 	.ue_clear_mask = ALTR_A10_ECC_DERRPENA,
 	.irq_status_mask = A10_SYSMGR_ECC_INTSTAT_OCRAM,
-	.dbgfs_name = "altr_ocram_trigger",
 	.ecc_enable_mask = ALTR_A10_OCRAM_ECC_EN_CTL,
 	.ecc_en_ofst = ALTR_A10_ECC_CTRL_OFST,
 	.ce_set_mask = ALTR_A10_ECC_TSERRA,
@@ -1228,7 +1226,6 @@ static const struct edac_device_prv_data l2ecc_data = {
 	.setup = altr_l2_check_deps,
 	.ce_clear_mask = 0,
 	.ue_clear_mask = 0,
-	.dbgfs_name = "altr_l2_trigger",
 	.alloc_mem = l2_alloc_mem,
 	.free_mem = l2_free_mem,
 	.ecc_enable_mask = ALTR_L2_ECC_EN,
@@ -1244,7 +1241,6 @@ static const struct edac_device_prv_data a10_l2ecc_data = {
 	.ce_clear_mask = ALTR_A10_L2_ECC_SERR_CLR,
 	.ue_clear_mask = ALTR_A10_L2_ECC_MERR_CLR,
 	.irq_status_mask = A10_SYSMGR_ECC_INTSTAT_L2,
-	.dbgfs_name = "altr_l2_trigger",
 	.alloc_mem = l2_alloc_mem,
 	.free_mem = l2_free_mem,
 	.ecc_enable_mask = ALTR_A10_L2_ECC_EN_CTL,
@@ -1266,7 +1262,6 @@ static const struct edac_device_prv_data a10_enetecc_data = {
 	.setup = altr_check_ecc_deps,
 	.ce_clear_mask = ALTR_A10_ECC_SERRPENA,
 	.ue_clear_mask = ALTR_A10_ECC_DERRPENA,
-	.dbgfs_name = "altr_trigger",
 	.ecc_enable_mask = ALTR_A10_COMMON_ECC_EN_CTL,
 	.ecc_en_ofst = ALTR_A10_ECC_CTRL_OFST,
 	.ce_set_mask = ALTR_A10_ECC_TSERRA,
@@ -1285,6 +1280,292 @@ early_initcall(socfpga_init_ethernet_ecc);
 
 #endif	/* CONFIG_EDAC_ALTERA_ETHERNET */
 
+/********************** NAND Device Functions **********************/
+
+#ifdef CONFIG_EDAC_ALTERA_NAND
+
+static const struct edac_device_prv_data a10_nandecc_data = {
+	.setup = altr_check_ecc_deps,
+	.ce_clear_mask = ALTR_A10_ECC_SERRPENA,
+	.ue_clear_mask = ALTR_A10_ECC_DERRPENA,
+	.ecc_enable_mask = ALTR_A10_COMMON_ECC_EN_CTL,
+	.ecc_en_ofst = ALTR_A10_ECC_CTRL_OFST,
+	.ce_set_mask = ALTR_A10_ECC_TSERRA,
+	.ue_set_mask = ALTR_A10_ECC_TDERRA,
+	.set_err_ofst = ALTR_A10_ECC_INTTEST_OFST,
+	.ecc_irq_handler = altr_edac_a10_ecc_irq,
+	.inject_fops = &altr_edac_a10_device_inject_fops,
+};
+
+static int __init socfpga_init_nand_ecc(void)
+{
+	return altr_init_a10_ecc_device_type("altr,socfpga-nand-ecc");
+}
+
+early_initcall(socfpga_init_nand_ecc);
+
+#endif	/* CONFIG_EDAC_ALTERA_NAND */
+
+/********************** DMA Device Functions **********************/
+
+#ifdef CONFIG_EDAC_ALTERA_DMA
+
+static const struct edac_device_prv_data a10_dmaecc_data = {
+	.setup = altr_check_ecc_deps,
+	.ce_clear_mask = ALTR_A10_ECC_SERRPENA,
+	.ue_clear_mask = ALTR_A10_ECC_DERRPENA,
+	.ecc_enable_mask = ALTR_A10_COMMON_ECC_EN_CTL,
+	.ecc_en_ofst = ALTR_A10_ECC_CTRL_OFST,
+	.ce_set_mask = ALTR_A10_ECC_TSERRA,
+	.ue_set_mask = ALTR_A10_ECC_TDERRA,
+	.set_err_ofst = ALTR_A10_ECC_INTTEST_OFST,
+	.ecc_irq_handler = altr_edac_a10_ecc_irq,
+	.inject_fops = &altr_edac_a10_device_inject_fops,
+};
+
+static int __init socfpga_init_dma_ecc(void)
+{
+	return altr_init_a10_ecc_device_type("altr,socfpga-dma-ecc");
+}
+
+early_initcall(socfpga_init_dma_ecc);
+
+#endif	/* CONFIG_EDAC_ALTERA_DMA */
+
+/********************** USB Device Functions **********************/
+
+#ifdef CONFIG_EDAC_ALTERA_USB
+
+static const struct edac_device_prv_data a10_usbecc_data = {
+	.setup = altr_check_ecc_deps,
+	.ce_clear_mask = ALTR_A10_ECC_SERRPENA,
+	.ue_clear_mask = ALTR_A10_ECC_DERRPENA,
+	.ecc_enable_mask = ALTR_A10_COMMON_ECC_EN_CTL,
+	.ecc_en_ofst = ALTR_A10_ECC_CTRL_OFST,
+	.ce_set_mask = ALTR_A10_ECC_TSERRA,
+	.ue_set_mask = ALTR_A10_ECC_TDERRA,
+	.set_err_ofst = ALTR_A10_ECC_INTTEST_OFST,
+	.ecc_irq_handler = altr_edac_a10_ecc_irq,
+	.inject_fops = &altr_edac_a10_device_inject_fops,
+};
+
+static int __init socfpga_init_usb_ecc(void)
+{
+	return altr_init_a10_ecc_device_type("altr,socfpga-usb-ecc");
+}
+
+early_initcall(socfpga_init_usb_ecc);
+
+#endif	/* CONFIG_EDAC_ALTERA_USB */
+
+/********************** QSPI Device Functions **********************/
+
+#ifdef CONFIG_EDAC_ALTERA_QSPI
+
+static const struct edac_device_prv_data a10_qspiecc_data = {
+	.setup = altr_check_ecc_deps,
+	.ce_clear_mask = ALTR_A10_ECC_SERRPENA,
+	.ue_clear_mask = ALTR_A10_ECC_DERRPENA,
+	.ecc_enable_mask = ALTR_A10_COMMON_ECC_EN_CTL,
+	.ecc_en_ofst = ALTR_A10_ECC_CTRL_OFST,
+	.ce_set_mask = ALTR_A10_ECC_TSERRA,
+	.ue_set_mask = ALTR_A10_ECC_TDERRA,
+	.set_err_ofst = ALTR_A10_ECC_INTTEST_OFST,
+	.ecc_irq_handler = altr_edac_a10_ecc_irq,
+	.inject_fops = &altr_edac_a10_device_inject_fops,
+};
+
+static int __init socfpga_init_qspi_ecc(void)
+{
+	return altr_init_a10_ecc_device_type("altr,socfpga-qspi-ecc");
+}
+
+early_initcall(socfpga_init_qspi_ecc);
+
+#endif	/* CONFIG_EDAC_ALTERA_QSPI */
+
+/********************* SDMMC Device Functions **********************/
+
+#ifdef CONFIG_EDAC_ALTERA_SDMMC
+
+static const struct edac_device_prv_data a10_sdmmceccb_data;
+static int altr_portb_setup(struct altr_edac_device_dev *device)
+{
+	struct edac_device_ctl_info *dci;
+	struct altr_edac_device_dev *altdev;
+	char *ecc_name = "sdmmcb-ecc";
+	int edac_idx, rc;
+	struct device_node *np;
+	const struct edac_device_prv_data *prv = &a10_sdmmceccb_data;
+
+	rc = altr_check_ecc_deps(device);
+	if (rc)
+		return rc;
+
+	np = of_find_compatible_node(NULL, NULL, "altr,socfpga-sdmmc-ecc");
+	if (!np) {
+		edac_printk(KERN_WARNING, EDAC_DEVICE, "SDMMC node not found\n");
+		return -ENODEV;
+	}
+
+	/* Create the PortB EDAC device */
+	edac_idx = edac_device_alloc_index();
+	dci = edac_device_alloc_ctl_info(sizeof(*altdev), ecc_name, 1,
+					 ecc_name, 1, 0, NULL, 0, edac_idx);
+	if (!dci) {
+		edac_printk(KERN_ERR, EDAC_DEVICE,
+			    "%s: Unable to allocate PortB EDAC device\n",
+			    ecc_name);
+		return -ENOMEM;
+	}
+
+	/* Initialize the PortB EDAC device structure from PortA structure */
+	altdev = dci->pvt_info;
+	*altdev = *device;
+
+	if (!devres_open_group(&altdev->ddev, altr_portb_setup, GFP_KERNEL))
+		return -ENOMEM;
+
+	/* Update PortB specific values */
+	altdev->edac_dev_name = ecc_name;
+	altdev->edac_idx = edac_idx;
+	altdev->edac_dev = dci;
+	altdev->data = prv;
+	dci->dev = &altdev->ddev;
+	dci->ctl_name = "Altera ECC Manager";
+	dci->mod_name = ecc_name;
+	dci->dev_name = ecc_name;
+
+	/* Update the IRQs for PortB */
+	altdev->sb_irq = irq_of_parse_and_map(np, 2);
+	if (!altdev->sb_irq) {
+		edac_printk(KERN_ERR, EDAC_DEVICE, "Error PortB SBIRQ alloc\n");
+		rc = -ENODEV;
+		goto err_release_group_1;
+	}
+	rc = devm_request_irq(&altdev->ddev, altdev->sb_irq,
+			      prv->ecc_irq_handler,
+			      IRQF_ONESHOT | IRQF_TRIGGER_HIGH,
+			      ecc_name, altdev);
+	if (rc) {
+		edac_printk(KERN_ERR, EDAC_DEVICE, "PortB SBERR IRQ error\n");
+		goto err_release_group_1;
+	}
+
+	altdev->db_irq = irq_of_parse_and_map(np, 3);
+	if (!altdev->db_irq) {
+		edac_printk(KERN_ERR, EDAC_DEVICE, "Error PortB DBIRQ alloc\n");
+		rc = -ENODEV;
+		goto err_release_group_1;
+	}
+	rc = devm_request_irq(&altdev->ddev, altdev->db_irq,
+			      prv->ecc_irq_handler,
+			      IRQF_ONESHOT | IRQF_TRIGGER_HIGH,
+			      ecc_name, altdev);
+	if (rc) {
+		edac_printk(KERN_ERR, EDAC_DEVICE, "PortB DBERR IRQ error\n");
+		goto err_release_group_1;
+	}
+
+	rc = edac_device_add_device(dci);
+	if (rc) {
+		edac_printk(KERN_ERR, EDAC_DEVICE,
+			    "edac_device_add_device portB failed\n");
+		rc = -ENOMEM;
+		goto err_release_group_1;
+	}
+	altr_create_edacdev_dbgfs(dci, prv);
+
+	list_add(&altdev->next, &altdev->edac->a10_ecc_devices);
+
+	devres_remove_group(&altdev->ddev, altr_portb_setup);
+
+	return 0;
+
+err_release_group_1:
+	edac_device_free_ctl_info(dci);
+	devres_release_group(&altdev->ddev, altr_portb_setup);
+	edac_printk(KERN_ERR, EDAC_DEVICE,
+		    "%s:Error setting up EDAC device: %d\n", ecc_name, rc);
+	return rc;
+}
+
+static irqreturn_t altr_edac_a10_ecc_irq_portb(int irq, void *dev_id)
+{
+	struct altr_edac_device_dev *ad = dev_id;
+	void __iomem  *base = ad->base;
+	const struct edac_device_prv_data *priv = ad->data;
+
+	if (irq == ad->sb_irq) {
+		writel(priv->ce_clear_mask,
+		       base + ALTR_A10_ECC_INTSTAT_OFST);
+		edac_device_handle_ce(ad->edac_dev, 0, 0, ad->edac_dev_name);
+		return IRQ_HANDLED;
+	} else if (irq == ad->db_irq) {
+		writel(priv->ue_clear_mask,
+		       base + ALTR_A10_ECC_INTSTAT_OFST);
+		edac_device_handle_ue(ad->edac_dev, 0, 0, ad->edac_dev_name);
+		return IRQ_HANDLED;
+	}
+
+	WARN_ONCE(1, "Unhandled IRQ%d on Port B.", irq);
+
+	return IRQ_NONE;
+}
+
+static const struct edac_device_prv_data a10_sdmmcecca_data = {
+	.setup = altr_portb_setup,
+	.ce_clear_mask = ALTR_A10_ECC_SERRPENA,
+	.ue_clear_mask = ALTR_A10_ECC_DERRPENA,
+	.ecc_enable_mask = ALTR_A10_COMMON_ECC_EN_CTL,
+	.ecc_en_ofst = ALTR_A10_ECC_CTRL_OFST,
+	.ce_set_mask = ALTR_A10_ECC_SERRPENA,
+	.ue_set_mask = ALTR_A10_ECC_DERRPENA,
+	.set_err_ofst = ALTR_A10_ECC_INTTEST_OFST,
+	.ecc_irq_handler = altr_edac_a10_ecc_irq,
+	.inject_fops = &altr_edac_a10_device_inject_fops,
+};
+
+static const struct edac_device_prv_data a10_sdmmceccb_data = {
+	.setup = altr_portb_setup,
+	.ce_clear_mask = ALTR_A10_ECC_SERRPENB,
+	.ue_clear_mask = ALTR_A10_ECC_DERRPENB,
+	.ecc_enable_mask = ALTR_A10_COMMON_ECC_EN_CTL,
+	.ecc_en_ofst = ALTR_A10_ECC_CTRL_OFST,
+	.ce_set_mask = ALTR_A10_ECC_TSERRB,
+	.ue_set_mask = ALTR_A10_ECC_TDERRB,
+	.set_err_ofst = ALTR_A10_ECC_INTTEST_OFST,
+	.ecc_irq_handler = altr_edac_a10_ecc_irq_portb,
+	.inject_fops = &altr_edac_a10_device_inject_fops,
+};
+
+static int __init socfpga_init_sdmmc_ecc(void)
+{
+	int rc = -ENODEV;
+	struct device_node *child = of_find_compatible_node(NULL, NULL,
+						"altr,socfpga-sdmmc-ecc");
+	if (!child) {
+		edac_printk(KERN_WARNING, EDAC_DEVICE, "SDMMC node not found\n");
+		return -ENODEV;
+	}
+
+	if (!of_device_is_available(child))
+		goto exit;
+
+	if (validate_parent_available(child))
+		goto exit;
+
+	rc = altr_init_a10_ecc_block(child, ALTR_A10_SDMMC_IRQ_MASK,
+				     a10_sdmmcecca_data.ecc_enable_mask, 1);
+exit:
+	of_node_put(child);
+	return rc;
+}
+
+early_initcall(socfpga_init_sdmmc_ecc);
+
+#endif	/* CONFIG_EDAC_ALTERA_SDMMC */
+
 /********************* Arria10 EDAC Device Functions *************************/
 static const struct of_device_id altr_edac_a10_device_of_match[] = {
 #ifdef CONFIG_EDAC_ALTERA_L2C
@@ -1298,6 +1579,21 @@ static const struct of_device_id altr_edac_a10_device_of_match[] = {
 	{ .compatible = "altr,socfpga-eth-mac-ecc",
 	  .data = &a10_enetecc_data },
 #endif
+#ifdef CONFIG_EDAC_ALTERA_NAND
+	{ .compatible = "altr,socfpga-nand-ecc", .data = &a10_nandecc_data },
+#endif
+#ifdef CONFIG_EDAC_ALTERA_DMA
+	{ .compatible = "altr,socfpga-dma-ecc", .data = &a10_dmaecc_data },
+#endif
+#ifdef CONFIG_EDAC_ALTERA_USB
+	{ .compatible = "altr,socfpga-usb-ecc", .data = &a10_usbecc_data },
+#endif
+#ifdef CONFIG_EDAC_ALTERA_QSPI
+	{ .compatible = "altr,socfpga-qspi-ecc", .data = &a10_qspiecc_data },
+#endif
+#ifdef CONFIG_EDAC_ALTERA_SDMMC
+	{ .compatible = "altr,socfpga-sdmmc-ecc", .data = &a10_sdmmcecca_data },
+#endif
 	{},
 };
 MODULE_DEVICE_TABLE(of, altr_edac_a10_device_of_match);
@@ -1451,11 +1747,11 @@ static int altr_edac_a10_device_add(struct altr_arria10_edac *edac,
 		rc = -ENODEV;
 		goto err_release_group1;
 	}
-	rc = devm_request_irq(edac->dev, altdev->sb_irq,
-			      prv->ecc_irq_handler,
-			      IRQF_SHARED, ecc_name, altdev);
+	rc = devm_request_irq(edac->dev, altdev->sb_irq, prv->ecc_irq_handler,
+			      IRQF_ONESHOT | IRQF_TRIGGER_HIGH,
+			      ecc_name, altdev);
 	if (rc) {
-		edac_printk(KERN_ERR, EDAC_DEVICE, "No DBERR IRQ resource\n");
+		edac_printk(KERN_ERR, EDAC_DEVICE, "No SBERR IRQ resource\n");
 		goto err_release_group1;
 	}
 
@@ -1465,9 +1761,9 @@ static int altr_edac_a10_device_add(struct altr_arria10_edac *edac,
 		rc = -ENODEV;
 		goto err_release_group1;
 	}
-	rc = devm_request_irq(edac->dev, altdev->db_irq,
-			      prv->ecc_irq_handler,
-			      IRQF_SHARED, ecc_name, altdev);
+	rc = devm_request_irq(edac->dev, altdev->db_irq, prv->ecc_irq_handler,
+			      IRQF_ONESHOT | IRQF_TRIGGER_HIGH,
+			      ecc_name, altdev);
 	if (rc) {
 		edac_printk(KERN_ERR, EDAC_DEVICE, "No DBERR IRQ resource\n");
 		goto err_release_group1;
@@ -1526,7 +1822,7 @@ static int a10_eccmgr_irqdomain_map(struct irq_domain *d, unsigned int irq,
 	return 0;
 }
 
-struct irq_domain_ops a10_eccmgr_ic_ops = {
+static struct irq_domain_ops a10_eccmgr_ic_ops = {
 	.map = a10_eccmgr_irqdomain_map,
 	.xlate = irq_domain_xlate_twocell,
 };
@@ -1584,15 +1880,19 @@ static int altr_edac_a10_probe(struct platform_device *pdev)
 	for_each_child_of_node(pdev->dev.of_node, child) {
 		if (!of_device_is_available(child))
 			continue;
-		if (of_device_is_compatible(child, "altr,socfpga-a10-l2-ecc"))
-			altr_edac_a10_device_add(edac, child);
-		else if ((of_device_is_compatible(child,
-					"altr,socfpga-a10-ocram-ecc")) ||
-			 (of_device_is_compatible(child,
-					"altr,socfpga-eth-mac-ecc")))
+
+		if (of_device_is_compatible(child, "altr,socfpga-a10-l2-ecc") || 
+		    of_device_is_compatible(child, "altr,socfpga-a10-ocram-ecc") ||
+		    of_device_is_compatible(child, "altr,socfpga-eth-mac-ecc") ||
+		    of_device_is_compatible(child, "altr,socfpga-nand-ecc") ||
+		    of_device_is_compatible(child, "altr,socfpga-dma-ecc") ||
+		    of_device_is_compatible(child, "altr,socfpga-usb-ecc") ||
+		    of_device_is_compatible(child, "altr,socfpga-qspi-ecc") ||
+		    of_device_is_compatible(child, "altr,socfpga-sdmmc-ecc"))
+
 			altr_edac_a10_device_add(edac, child);
-		else if (of_device_is_compatible(child,
-						 "altr,sdram-edac-a10"))
+
+		else if (of_device_is_compatible(child, "altr,sdram-edac-a10"))
 			of_platform_populate(pdev->dev.of_node,
 					     altr_sdram_ctrl_of_match,
 					     NULL, &pdev->dev);
diff --git a/drivers/edac/altera_edac.h b/drivers/edac/altera_edac.h
index 687d8e754d36..cbc96290f743 100644
--- a/drivers/edac/altera_edac.h
+++ b/drivers/edac/altera_edac.h
@@ -250,6 +250,8 @@ struct altr_sdram_mc_data {
 #define ALTR_A10_ECC_INTTEST_OFST       0x24
 #define ALTR_A10_ECC_TSERRA             BIT(0)
 #define ALTR_A10_ECC_TDERRA             BIT(8)
+#define ALTR_A10_ECC_TSERRB             BIT(16)
+#define ALTR_A10_ECC_TDERRB             BIT(24)
 
 /* ECC Manager Defines */
 #define A10_SYSMGR_ECC_INTMASK_SET_OFST   0x94
@@ -288,6 +290,9 @@ struct altr_sdram_mc_data {
 /* Arria 10 Ethernet ECC Management Group Defines */
 #define ALTR_A10_COMMON_ECC_EN_CTL      BIT(0)
 
+/* Arria 10 SDMMC ECC Management Group Defines */
+#define ALTR_A10_SDMMC_IRQ_MASK         (BIT(16) | BIT(15))
+
 /* A10 ECC Controller memory initialization timeout */
 #define ALTR_A10_ECC_INIT_WATCHDOG_10US      10000
 
@@ -298,7 +303,6 @@ struct edac_device_prv_data {
 	int ce_clear_mask;
 	int ue_clear_mask;
 	int irq_status_mask;
-	char dbgfs_name[20];
 	void * (*alloc_mem)(size_t size, void **other);
 	void (*free_mem)(void *p, size_t size, void *other);
 	int ecc_enable_mask;
diff --git a/drivers/edac/amd64_edac.c b/drivers/edac/amd64_edac.c
index 8c0ec2128907..ee181c53626f 100644
--- a/drivers/edac/amd64_edac.c
+++ b/drivers/edac/amd64_edac.c
@@ -1425,11 +1425,17 @@ static u8 f1x_determine_channel(struct amd64_pvt *pvt, u64 sys_addr,
 
 		if (intlv_addr & 0x2) {
 			u8 shift = intlv_addr & 0x1 ? 9 : 6;
-			u32 temp = hweight_long((u32) ((sys_addr >> 16) & 0x1F)) % 2;
+			u32 temp = hweight_long((u32) ((sys_addr >> 16) & 0x1F)) & 1;
 
 			return ((sys_addr >> shift) & 1) ^ temp;
 		}
 
+		if (intlv_addr & 0x4) {
+			u8 shift = intlv_addr & 0x1 ? 9 : 8;
+
+			return (sys_addr >> shift) & 1;
+		}
+
 		return (sys_addr >> (12 + hweight8(intlv_en))) & 1;
 	}
 
@@ -1726,8 +1732,11 @@ static int f15_m30h_match_to_this_node(struct amd64_pvt *pvt, unsigned range,
 	if (!(num_dcts_intlv % 2 == 0) || (num_dcts_intlv > 4))
 		return -EINVAL;
 
-	channel = f15_m30h_determine_channel(pvt, sys_addr, intlv_en,
-					     num_dcts_intlv, dct_sel);
+	if (pvt->model >= 0x60)
+		channel = f1x_determine_channel(pvt, sys_addr, false, intlv_en);
+	else
+		channel = f15_m30h_determine_channel(pvt, sys_addr, intlv_en,
+						     num_dcts_intlv, dct_sel);
 
 	/* Verify we stay within the MAX number of channels allowed */
 	if (channel > 3)
@@ -2961,6 +2970,15 @@ static void setup_pci_device(void)
 	}
 }
 
+static const struct x86_cpu_id amd64_cpuids[] = {
+	{ X86_VENDOR_AMD, 0xF,	X86_MODEL_ANY,	X86_FEATURE_ANY, 0 },
+	{ X86_VENDOR_AMD, 0x10, X86_MODEL_ANY,	X86_FEATURE_ANY, 0 },
+	{ X86_VENDOR_AMD, 0x15, X86_MODEL_ANY,	X86_FEATURE_ANY, 0 },
+	{ X86_VENDOR_AMD, 0x16, X86_MODEL_ANY,	X86_FEATURE_ANY, 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(x86cpu, amd64_cpuids);
+
 static int __init amd64_edac_init(void)
 {
 	int err = -ENODEV;
diff --git a/drivers/edac/fsl_ddr_edac.c b/drivers/edac/fsl_ddr_edac.c
new file mode 100644
index 000000000000..9774f52f0c3e
--- /dev/null
+++ b/drivers/edac/fsl_ddr_edac.c
@@ -0,0 +1,633 @@
+/*
+ * Freescale Memory Controller kernel module
+ *
+ * Support Power-based SoCs including MPC85xx, MPC86xx, MPC83xx and
+ * ARM-based Layerscape SoCs including LS2xxx. Originally split
+ * out from mpc85xx_edac EDAC driver.
+ *
+ * Parts Copyrighted (c) 2013 by Freescale Semiconductor, Inc.
+ *
+ * Author: Dave Jiang <djiang@mvista.com>
+ *
+ * 2006-2007 (c) MontaVista Software, Inc. This file is licensed under
+ * the terms of the GNU General Public License version 2. This program
+ * is licensed "as is" without any warranty of any kind, whether express
+ * or implied.
+ */
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/ctype.h>
+#include <linux/io.h>
+#include <linux/mod_devicetable.h>
+#include <linux/edac.h>
+#include <linux/smp.h>
+#include <linux/gfp.h>
+
+#include <linux/of_platform.h>
+#include <linux/of_device.h>
+#include <linux/of_address.h>
+#include "edac_module.h"
+#include "edac_core.h"
+#include "fsl_ddr_edac.h"
+
+#define EDAC_MOD_STR	"fsl_ddr_edac"
+
+static int edac_mc_idx;
+
+static u32 orig_ddr_err_disable;
+static u32 orig_ddr_err_sbe;
+static bool little_endian;
+
+static inline u32 ddr_in32(void __iomem *addr)
+{
+	return little_endian ? ioread32(addr) : ioread32be(addr);
+}
+
+static inline void ddr_out32(void __iomem *addr, u32 value)
+{
+	if (little_endian)
+		iowrite32(value, addr);
+	else
+		iowrite32be(value, addr);
+}
+
+/************************ MC SYSFS parts ***********************************/
+
+#define to_mci(k) container_of(k, struct mem_ctl_info, dev)
+
+static ssize_t fsl_mc_inject_data_hi_show(struct device *dev,
+					  struct device_attribute *mattr,
+					  char *data)
+{
+	struct mem_ctl_info *mci = to_mci(dev);
+	struct fsl_mc_pdata *pdata = mci->pvt_info;
+	return sprintf(data, "0x%08x",
+		       ddr_in32(pdata->mc_vbase + FSL_MC_DATA_ERR_INJECT_HI));
+}
+
+static ssize_t fsl_mc_inject_data_lo_show(struct device *dev,
+					  struct device_attribute *mattr,
+					      char *data)
+{
+	struct mem_ctl_info *mci = to_mci(dev);
+	struct fsl_mc_pdata *pdata = mci->pvt_info;
+	return sprintf(data, "0x%08x",
+		       ddr_in32(pdata->mc_vbase + FSL_MC_DATA_ERR_INJECT_LO));
+}
+
+static ssize_t fsl_mc_inject_ctrl_show(struct device *dev,
+				       struct device_attribute *mattr,
+					   char *data)
+{
+	struct mem_ctl_info *mci = to_mci(dev);
+	struct fsl_mc_pdata *pdata = mci->pvt_info;
+	return sprintf(data, "0x%08x",
+		       ddr_in32(pdata->mc_vbase + FSL_MC_ECC_ERR_INJECT));
+}
+
+static ssize_t fsl_mc_inject_data_hi_store(struct device *dev,
+					   struct device_attribute *mattr,
+					       const char *data, size_t count)
+{
+	struct mem_ctl_info *mci = to_mci(dev);
+	struct fsl_mc_pdata *pdata = mci->pvt_info;
+	unsigned long val;
+	int rc;
+
+	if (isdigit(*data)) {
+		rc = kstrtoul(data, 0, &val);
+		if (rc)
+			return rc;
+
+		ddr_out32(pdata->mc_vbase + FSL_MC_DATA_ERR_INJECT_HI, val);
+		return count;
+	}
+	return 0;
+}
+
+static ssize_t fsl_mc_inject_data_lo_store(struct device *dev,
+					   struct device_attribute *mattr,
+					       const char *data, size_t count)
+{
+	struct mem_ctl_info *mci = to_mci(dev);
+	struct fsl_mc_pdata *pdata = mci->pvt_info;
+	unsigned long val;
+	int rc;
+
+	if (isdigit(*data)) {
+		rc = kstrtoul(data, 0, &val);
+		if (rc)
+			return rc;
+
+		ddr_out32(pdata->mc_vbase + FSL_MC_DATA_ERR_INJECT_LO, val);
+		return count;
+	}
+	return 0;
+}
+
+static ssize_t fsl_mc_inject_ctrl_store(struct device *dev,
+					struct device_attribute *mattr,
+					       const char *data, size_t count)
+{
+	struct mem_ctl_info *mci = to_mci(dev);
+	struct fsl_mc_pdata *pdata = mci->pvt_info;
+	unsigned long val;
+	int rc;
+
+	if (isdigit(*data)) {
+		rc = kstrtoul(data, 0, &val);
+		if (rc)
+			return rc;
+
+		ddr_out32(pdata->mc_vbase + FSL_MC_ECC_ERR_INJECT, val);
+		return count;
+	}
+	return 0;
+}
+
+DEVICE_ATTR(inject_data_hi, S_IRUGO | S_IWUSR,
+	    fsl_mc_inject_data_hi_show, fsl_mc_inject_data_hi_store);
+DEVICE_ATTR(inject_data_lo, S_IRUGO | S_IWUSR,
+	    fsl_mc_inject_data_lo_show, fsl_mc_inject_data_lo_store);
+DEVICE_ATTR(inject_ctrl, S_IRUGO | S_IWUSR,
+	    fsl_mc_inject_ctrl_show, fsl_mc_inject_ctrl_store);
+
+static struct attribute *fsl_ddr_dev_attrs[] = {
+	&dev_attr_inject_data_hi.attr,
+	&dev_attr_inject_data_lo.attr,
+	&dev_attr_inject_ctrl.attr,
+	NULL
+};
+
+ATTRIBUTE_GROUPS(fsl_ddr_dev);
+
+/**************************** MC Err device ***************************/
+
+/*
+ * Taken from table 8-55 in the MPC8641 User's Manual and/or 9-61 in the
+ * MPC8572 User's Manual.  Each line represents a syndrome bit column as a
+ * 64-bit value, but split into an upper and lower 32-bit chunk.  The labels
+ * below correspond to Freescale's manuals.
+ */
+static unsigned int ecc_table[16] = {
+	/* MSB           LSB */
+	/* [0:31]    [32:63] */
+	0xf00fe11e, 0xc33c0ff7,	/* Syndrome bit 7 */
+	0x00ff00ff, 0x00fff0ff,
+	0x0f0f0f0f, 0x0f0fff00,
+	0x11113333, 0x7777000f,
+	0x22224444, 0x8888222f,
+	0x44448888, 0xffff4441,
+	0x8888ffff, 0x11118882,
+	0xffff1111, 0x22221114,	/* Syndrome bit 0 */
+};
+
+/*
+ * Calculate the correct ECC value for a 64-bit value specified by high:low
+ */
+static u8 calculate_ecc(u32 high, u32 low)
+{
+	u32 mask_low;
+	u32 mask_high;
+	int bit_cnt;
+	u8 ecc = 0;
+	int i;
+	int j;
+
+	for (i = 0; i < 8; i++) {
+		mask_high = ecc_table[i * 2];
+		mask_low = ecc_table[i * 2 + 1];
+		bit_cnt = 0;
+
+		for (j = 0; j < 32; j++) {
+			if ((mask_high >> j) & 1)
+				bit_cnt ^= (high >> j) & 1;
+			if ((mask_low >> j) & 1)
+				bit_cnt ^= (low >> j) & 1;
+		}
+
+		ecc |= bit_cnt << i;
+	}
+
+	return ecc;
+}
+
+/*
+ * Create the syndrome code which is generated if the data line specified by
+ * 'bit' failed.  Eg generate an 8-bit codes seen in Table 8-55 in the MPC8641
+ * User's Manual and 9-61 in the MPC8572 User's Manual.
+ */
+static u8 syndrome_from_bit(unsigned int bit) {
+	int i;
+	u8 syndrome = 0;
+
+	/*
+	 * Cycle through the upper or lower 32-bit portion of each value in
+	 * ecc_table depending on if 'bit' is in the upper or lower half of
+	 * 64-bit data.
+	 */
+	for (i = bit < 32; i < 16; i += 2)
+		syndrome |= ((ecc_table[i] >> (bit % 32)) & 1) << (i / 2);
+
+	return syndrome;
+}
+
+/*
+ * Decode data and ecc syndrome to determine what went wrong
+ * Note: This can only decode single-bit errors
+ */
+static void sbe_ecc_decode(u32 cap_high, u32 cap_low, u32 cap_ecc,
+		       int *bad_data_bit, int *bad_ecc_bit)
+{
+	int i;
+	u8 syndrome;
+
+	*bad_data_bit = -1;
+	*bad_ecc_bit = -1;
+
+	/*
+	 * Calculate the ECC of the captured data and XOR it with the captured
+	 * ECC to find an ECC syndrome value we can search for
+	 */
+	syndrome = calculate_ecc(cap_high, cap_low) ^ cap_ecc;
+
+	/* Check if a data line is stuck... */
+	for (i = 0; i < 64; i++) {
+		if (syndrome == syndrome_from_bit(i)) {
+			*bad_data_bit = i;
+			return;
+		}
+	}
+
+	/* If data is correct, check ECC bits for errors... */
+	for (i = 0; i < 8; i++) {
+		if ((syndrome >> i) & 0x1) {
+			*bad_ecc_bit = i;
+			return;
+		}
+	}
+}
+
+#define make64(high, low) (((u64)(high) << 32) | (low))
+
+static void fsl_mc_check(struct mem_ctl_info *mci)
+{
+	struct fsl_mc_pdata *pdata = mci->pvt_info;
+	struct csrow_info *csrow;
+	u32 bus_width;
+	u32 err_detect;
+	u32 syndrome;
+	u64 err_addr;
+	u32 pfn;
+	int row_index;
+	u32 cap_high;
+	u32 cap_low;
+	int bad_data_bit;
+	int bad_ecc_bit;
+
+	err_detect = ddr_in32(pdata->mc_vbase + FSL_MC_ERR_DETECT);
+	if (!err_detect)
+		return;
+
+	fsl_mc_printk(mci, KERN_ERR, "Err Detect Register: %#8.8x\n",
+		      err_detect);
+
+	/* no more processing if not ECC bit errors */
+	if (!(err_detect & (DDR_EDE_SBE | DDR_EDE_MBE))) {
+		ddr_out32(pdata->mc_vbase + FSL_MC_ERR_DETECT, err_detect);
+		return;
+	}
+
+	syndrome = ddr_in32(pdata->mc_vbase + FSL_MC_CAPTURE_ECC);
+
+	/* Mask off appropriate bits of syndrome based on bus width */
+	bus_width = (ddr_in32(pdata->mc_vbase + FSL_MC_DDR_SDRAM_CFG) &
+		     DSC_DBW_MASK) ? 32 : 64;
+	if (bus_width == 64)
+		syndrome &= 0xff;
+	else
+		syndrome &= 0xffff;
+
+	err_addr = make64(
+		ddr_in32(pdata->mc_vbase + FSL_MC_CAPTURE_EXT_ADDRESS),
+		ddr_in32(pdata->mc_vbase + FSL_MC_CAPTURE_ADDRESS));
+	pfn = err_addr >> PAGE_SHIFT;
+
+	for (row_index = 0; row_index < mci->nr_csrows; row_index++) {
+		csrow = mci->csrows[row_index];
+		if ((pfn >= csrow->first_page) && (pfn <= csrow->last_page))
+			break;
+	}
+
+	cap_high = ddr_in32(pdata->mc_vbase + FSL_MC_CAPTURE_DATA_HI);
+	cap_low = ddr_in32(pdata->mc_vbase + FSL_MC_CAPTURE_DATA_LO);
+
+	/*
+	 * Analyze single-bit errors on 64-bit wide buses
+	 * TODO: Add support for 32-bit wide buses
+	 */
+	if ((err_detect & DDR_EDE_SBE) && (bus_width == 64)) {
+		sbe_ecc_decode(cap_high, cap_low, syndrome,
+				&bad_data_bit, &bad_ecc_bit);
+
+		if (bad_data_bit != -1)
+			fsl_mc_printk(mci, KERN_ERR,
+				"Faulty Data bit: %d\n", bad_data_bit);
+		if (bad_ecc_bit != -1)
+			fsl_mc_printk(mci, KERN_ERR,
+				"Faulty ECC bit: %d\n", bad_ecc_bit);
+
+		fsl_mc_printk(mci, KERN_ERR,
+			"Expected Data / ECC:\t%#8.8x_%08x / %#2.2x\n",
+			cap_high ^ (1 << (bad_data_bit - 32)),
+			cap_low ^ (1 << bad_data_bit),
+			syndrome ^ (1 << bad_ecc_bit));
+	}
+
+	fsl_mc_printk(mci, KERN_ERR,
+			"Captured Data / ECC:\t%#8.8x_%08x / %#2.2x\n",
+			cap_high, cap_low, syndrome);
+	fsl_mc_printk(mci, KERN_ERR, "Err addr: %#8.8llx\n", err_addr);
+	fsl_mc_printk(mci, KERN_ERR, "PFN: %#8.8x\n", pfn);
+
+	/* we are out of range */
+	if (row_index == mci->nr_csrows)
+		fsl_mc_printk(mci, KERN_ERR, "PFN out of range!\n");
+
+	if (err_detect & DDR_EDE_SBE)
+		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci, 1,
+				     pfn, err_addr & ~PAGE_MASK, syndrome,
+				     row_index, 0, -1,
+				     mci->ctl_name, "");
+
+	if (err_detect & DDR_EDE_MBE)
+		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 1,
+				     pfn, err_addr & ~PAGE_MASK, syndrome,
+				     row_index, 0, -1,
+				     mci->ctl_name, "");
+
+	ddr_out32(pdata->mc_vbase + FSL_MC_ERR_DETECT, err_detect);
+}
+
+static irqreturn_t fsl_mc_isr(int irq, void *dev_id)
+{
+	struct mem_ctl_info *mci = dev_id;
+	struct fsl_mc_pdata *pdata = mci->pvt_info;
+	u32 err_detect;
+
+	err_detect = ddr_in32(pdata->mc_vbase + FSL_MC_ERR_DETECT);
+	if (!err_detect)
+		return IRQ_NONE;
+
+	fsl_mc_check(mci);
+
+	return IRQ_HANDLED;
+}
+
+static void fsl_ddr_init_csrows(struct mem_ctl_info *mci)
+{
+	struct fsl_mc_pdata *pdata = mci->pvt_info;
+	struct csrow_info *csrow;
+	struct dimm_info *dimm;
+	u32 sdram_ctl;
+	u32 sdtype;
+	enum mem_type mtype;
+	u32 cs_bnds;
+	int index;
+
+	sdram_ctl = ddr_in32(pdata->mc_vbase + FSL_MC_DDR_SDRAM_CFG);
+
+	sdtype = sdram_ctl & DSC_SDTYPE_MASK;
+	if (sdram_ctl & DSC_RD_EN) {
+		switch (sdtype) {
+		case 0x02000000:
+			mtype = MEM_RDDR;
+			break;
+		case 0x03000000:
+			mtype = MEM_RDDR2;
+			break;
+		case 0x07000000:
+			mtype = MEM_RDDR3;
+			break;
+		case 0x05000000:
+			mtype = MEM_RDDR4;
+			break;
+		default:
+			mtype = MEM_UNKNOWN;
+			break;
+		}
+	} else {
+		switch (sdtype) {
+		case 0x02000000:
+			mtype = MEM_DDR;
+			break;
+		case 0x03000000:
+			mtype = MEM_DDR2;
+			break;
+		case 0x07000000:
+			mtype = MEM_DDR3;
+			break;
+		case 0x05000000:
+			mtype = MEM_DDR4;
+			break;
+		default:
+			mtype = MEM_UNKNOWN;
+			break;
+		}
+	}
+
+	for (index = 0; index < mci->nr_csrows; index++) {
+		u32 start;
+		u32 end;
+
+		csrow = mci->csrows[index];
+		dimm = csrow->channels[0]->dimm;
+
+		cs_bnds = ddr_in32(pdata->mc_vbase + FSL_MC_CS_BNDS_0 +
+				   (index * FSL_MC_CS_BNDS_OFS));
+
+		start = (cs_bnds & 0xffff0000) >> 16;
+		end   = (cs_bnds & 0x0000ffff);
+
+		if (start == end)
+			continue;	/* not populated */
+
+		start <<= (24 - PAGE_SHIFT);
+		end   <<= (24 - PAGE_SHIFT);
+		end    |= (1 << (24 - PAGE_SHIFT)) - 1;
+
+		csrow->first_page = start;
+		csrow->last_page = end;
+
+		dimm->nr_pages = end + 1 - start;
+		dimm->grain = 8;
+		dimm->mtype = mtype;
+		dimm->dtype = DEV_UNKNOWN;
+		if (sdram_ctl & DSC_X32_EN)
+			dimm->dtype = DEV_X32;
+		dimm->edac_mode = EDAC_SECDED;
+	}
+}
+
+int fsl_mc_err_probe(struct platform_device *op)
+{
+	struct mem_ctl_info *mci;
+	struct edac_mc_layer layers[2];
+	struct fsl_mc_pdata *pdata;
+	struct resource r;
+	u32 sdram_ctl;
+	int res;
+
+	if (!devres_open_group(&op->dev, fsl_mc_err_probe, GFP_KERNEL))
+		return -ENOMEM;
+
+	layers[0].type = EDAC_MC_LAYER_CHIP_SELECT;
+	layers[0].size = 4;
+	layers[0].is_virt_csrow = true;
+	layers[1].type = EDAC_MC_LAYER_CHANNEL;
+	layers[1].size = 1;
+	layers[1].is_virt_csrow = false;
+	mci = edac_mc_alloc(edac_mc_idx, ARRAY_SIZE(layers), layers,
+			    sizeof(*pdata));
+	if (!mci) {
+		devres_release_group(&op->dev, fsl_mc_err_probe);
+		return -ENOMEM;
+	}
+
+	pdata = mci->pvt_info;
+	pdata->name = "fsl_mc_err";
+	mci->pdev = &op->dev;
+	pdata->edac_idx = edac_mc_idx++;
+	dev_set_drvdata(mci->pdev, mci);
+	mci->ctl_name = pdata->name;
+	mci->dev_name = pdata->name;
+
+	/*
+	 * Get the endianness of DDR controller registers.
+	 * Default is big endian.
+	 */
+	little_endian = of_property_read_bool(op->dev.of_node, "little-endian");
+
+	res = of_address_to_resource(op->dev.of_node, 0, &r);
+	if (res) {
+		pr_err("%s: Unable to get resource for MC err regs\n",
+		       __func__);
+		goto err;
+	}
+
+	if (!devm_request_mem_region(&op->dev, r.start, resource_size(&r),
+				     pdata->name)) {
+		pr_err("%s: Error while requesting mem region\n",
+		       __func__);
+		res = -EBUSY;
+		goto err;
+	}
+
+	pdata->mc_vbase = devm_ioremap(&op->dev, r.start, resource_size(&r));
+	if (!pdata->mc_vbase) {
+		pr_err("%s: Unable to setup MC err regs\n", __func__);
+		res = -ENOMEM;
+		goto err;
+	}
+
+	sdram_ctl = ddr_in32(pdata->mc_vbase + FSL_MC_DDR_SDRAM_CFG);
+	if (!(sdram_ctl & DSC_ECC_EN)) {
+		/* no ECC */
+		pr_warn("%s: No ECC DIMMs discovered\n", __func__);
+		res = -ENODEV;
+		goto err;
+	}
+
+	edac_dbg(3, "init mci\n");
+	mci->mtype_cap = MEM_FLAG_DDR | MEM_FLAG_RDDR |
+			 MEM_FLAG_DDR2 | MEM_FLAG_RDDR2 |
+			 MEM_FLAG_DDR3 | MEM_FLAG_RDDR3 |
+			 MEM_FLAG_DDR4 | MEM_FLAG_RDDR4;
+	mci->edac_ctl_cap = EDAC_FLAG_NONE | EDAC_FLAG_SECDED;
+	mci->edac_cap = EDAC_FLAG_SECDED;
+	mci->mod_name = EDAC_MOD_STR;
+
+	if (edac_op_state == EDAC_OPSTATE_POLL)
+		mci->edac_check = fsl_mc_check;
+
+	mci->ctl_page_to_phys = NULL;
+
+	mci->scrub_mode = SCRUB_SW_SRC;
+
+	fsl_ddr_init_csrows(mci);
+
+	/* store the original error disable bits */
+	orig_ddr_err_disable = ddr_in32(pdata->mc_vbase + FSL_MC_ERR_DISABLE);
+	ddr_out32(pdata->mc_vbase + FSL_MC_ERR_DISABLE, 0);
+
+	/* clear all error bits */
+	ddr_out32(pdata->mc_vbase + FSL_MC_ERR_DETECT, ~0);
+
+	res = edac_mc_add_mc_with_groups(mci, fsl_ddr_dev_groups);
+	if (res) {
+		edac_dbg(3, "failed edac_mc_add_mc()\n");
+		goto err;
+	}
+
+	if (edac_op_state == EDAC_OPSTATE_INT) {
+		ddr_out32(pdata->mc_vbase + FSL_MC_ERR_INT_EN,
+			  DDR_EIE_MBEE | DDR_EIE_SBEE);
+
+		/* store the original error management threshold */
+		orig_ddr_err_sbe = ddr_in32(pdata->mc_vbase +
+					    FSL_MC_ERR_SBE) & 0xff0000;
+
+		/* set threshold to 1 error per interrupt */
+		ddr_out32(pdata->mc_vbase + FSL_MC_ERR_SBE, 0x10000);
+
+		/* register interrupts */
+		pdata->irq = platform_get_irq(op, 0);
+		res = devm_request_irq(&op->dev, pdata->irq,
+				       fsl_mc_isr,
+				       IRQF_SHARED,
+				       "[EDAC] MC err", mci);
+		if (res < 0) {
+			pr_err("%s: Unable to request irq %d for FSL DDR DRAM ERR\n",
+			       __func__, pdata->irq);
+			res = -ENODEV;
+			goto err2;
+		}
+
+		pr_info(EDAC_MOD_STR " acquired irq %d for MC\n",
+		       pdata->irq);
+	}
+
+	devres_remove_group(&op->dev, fsl_mc_err_probe);
+	edac_dbg(3, "success\n");
+	pr_info(EDAC_MOD_STR " MC err registered\n");
+
+	return 0;
+
+err2:
+	edac_mc_del_mc(&op->dev);
+err:
+	devres_release_group(&op->dev, fsl_mc_err_probe);
+	edac_mc_free(mci);
+	return res;
+}
+
+int fsl_mc_err_remove(struct platform_device *op)
+{
+	struct mem_ctl_info *mci = dev_get_drvdata(&op->dev);
+	struct fsl_mc_pdata *pdata = mci->pvt_info;
+
+	edac_dbg(0, "\n");
+
+	if (edac_op_state == EDAC_OPSTATE_INT) {
+		ddr_out32(pdata->mc_vbase + FSL_MC_ERR_INT_EN, 0);
+	}
+
+	ddr_out32(pdata->mc_vbase + FSL_MC_ERR_DISABLE,
+		  orig_ddr_err_disable);
+	ddr_out32(pdata->mc_vbase + FSL_MC_ERR_SBE, orig_ddr_err_sbe);
+
+	edac_mc_del_mc(&op->dev);
+	edac_mc_free(mci);
+	return 0;
+}
diff --git a/drivers/edac/fsl_ddr_edac.h b/drivers/edac/fsl_ddr_edac.h
new file mode 100644
index 000000000000..4ccee292eff1
--- /dev/null
+++ b/drivers/edac/fsl_ddr_edac.h
@@ -0,0 +1,79 @@
+/*
+ * Freescale Memory Controller kernel module
+ *
+ * Support  Power-based SoCs including MPC85xx, MPC86xx, MPC83xx and
+ * ARM-based Layerscape SoCs including LS2xxx. Originally split
+ * out from mpc85xx_edac EDAC driver.
+ *
+ * Author: Dave Jiang <djiang@mvista.com>
+ *
+ * 2006-2007 (c) MontaVista Software, Inc. This file is licensed under
+ * the terms of the GNU General Public License version 2. This program
+ * is licensed "as is" without any warranty of any kind, whether express
+ * or implied.
+ *
+ */
+#ifndef _FSL_DDR_EDAC_H_
+#define _FSL_DDR_EDAC_H_
+
+#define fsl_mc_printk(mci, level, fmt, arg...) \
+	edac_mc_chipset_printk(mci, level, "FSL_DDR", fmt, ##arg)
+
+/*
+ * DRAM error defines
+ */
+
+/* DDR_SDRAM_CFG */
+#define FSL_MC_DDR_SDRAM_CFG	0x0110
+#define FSL_MC_CS_BNDS_0		0x0000
+#define FSL_MC_CS_BNDS_OFS		0x0008
+
+#define FSL_MC_DATA_ERR_INJECT_HI	0x0e00
+#define FSL_MC_DATA_ERR_INJECT_LO	0x0e04
+#define FSL_MC_ECC_ERR_INJECT	0x0e08
+#define FSL_MC_CAPTURE_DATA_HI	0x0e20
+#define FSL_MC_CAPTURE_DATA_LO	0x0e24
+#define FSL_MC_CAPTURE_ECC		0x0e28
+#define FSL_MC_ERR_DETECT		0x0e40
+#define FSL_MC_ERR_DISABLE		0x0e44
+#define FSL_MC_ERR_INT_EN		0x0e48
+#define FSL_MC_CAPTURE_ATRIBUTES	0x0e4c
+#define FSL_MC_CAPTURE_ADDRESS	0x0e50
+#define FSL_MC_CAPTURE_EXT_ADDRESS	0x0e54
+#define FSL_MC_ERR_SBE		0x0e58
+
+#define DSC_MEM_EN	0x80000000
+#define DSC_ECC_EN	0x20000000
+#define DSC_RD_EN	0x10000000
+#define DSC_DBW_MASK	0x00180000
+#define DSC_DBW_32	0x00080000
+#define DSC_DBW_64	0x00000000
+
+#define DSC_SDTYPE_MASK		0x07000000
+#define DSC_X32_EN	0x00000020
+
+/* Err_Int_En */
+#define DDR_EIE_MSEE	0x1	/* memory select */
+#define DDR_EIE_SBEE	0x4	/* single-bit ECC error */
+#define DDR_EIE_MBEE	0x8	/* multi-bit ECC error */
+
+/* Err_Detect */
+#define DDR_EDE_MSE		0x1	/* memory select */
+#define DDR_EDE_SBE		0x4	/* single-bit ECC error */
+#define DDR_EDE_MBE		0x8	/* multi-bit ECC error */
+#define DDR_EDE_MME		0x80000000	/* multiple memory errors */
+
+/* Err_Disable */
+#define DDR_EDI_MSED	0x1	/* memory select disable */
+#define	DDR_EDI_SBED	0x4	/* single-bit ECC error disable */
+#define	DDR_EDI_MBED	0x8	/* multi-bit ECC error disable */
+
+struct fsl_mc_pdata {
+	char *name;
+	int edac_idx;
+	void __iomem *mc_vbase;
+	int irq;
+};
+int fsl_mc_err_probe(struct platform_device *op);
+int fsl_mc_err_remove(struct platform_device *op);
+#endif
diff --git a/drivers/edac/layerscape_edac.c b/drivers/edac/layerscape_edac.c
new file mode 100644
index 000000000000..6c59d897ad12
--- /dev/null
+++ b/drivers/edac/layerscape_edac.c
@@ -0,0 +1,73 @@
+/*
+ * Freescale Memory Controller kernel module
+ *
+ * Author: York Sun <york.sun@nxp.com>
+ *
+ * Copyright 2016 NXP Semiconductor
+ *
+ * Derived from mpc85xx_edac.c
+ * Author: Dave Jiang <djiang@mvista.com>
+ *
+ * 2006-2007 (c) MontaVista Software, Inc. This file is licensed under
+ * the terms of the GNU General Public License version 2. This program
+ * is licensed "as is" without any warranty of any kind, whether express
+ * or implied.
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include "edac_core.h"
+#include "fsl_ddr_edac.h"
+
+static const struct of_device_id fsl_ddr_mc_err_of_match[] = {
+	{ .compatible = "fsl,qoriq-memory-controller", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, fsl_ddr_mc_err_of_match);
+
+static struct platform_driver fsl_ddr_mc_err_driver = {
+	.probe = fsl_mc_err_probe,
+	.remove = fsl_mc_err_remove,
+	.driver = {
+		.name = "fsl_ddr_mc_err",
+		.of_match_table = fsl_ddr_mc_err_of_match,
+	},
+};
+
+static int __init fsl_ddr_mc_init(void)
+{
+	int res;
+
+	/* 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;
+	}
+
+	res = platform_driver_register(&fsl_ddr_mc_err_driver);
+	if (res) {
+		pr_err("MC fails to register\n");
+		return res;
+	}
+
+	return 0;
+}
+
+module_init(fsl_ddr_mc_init);
+
+static void __exit fsl_ddr_mc_exit(void)
+{
+	platform_driver_unregister(&fsl_ddr_mc_err_driver);
+}
+
+module_exit(fsl_ddr_mc_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("NXP Semiconductor");
+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/edac/mce_amd.c b/drivers/edac/mce_amd.c
index 9b6800a79c7f..daaac2c79ca7 100644
--- a/drivers/edac/mce_amd.c
+++ b/drivers/edac/mce_amd.c
@@ -148,12 +148,12 @@ static const char * const mc6_mce_desc[] = {
 };
 
 /* Scalable MCA error strings */
-static const char * const f17h_ls_mce_desc[] = {
+static const char * const smca_ls_mce_desc[] = {
 	"Load queue parity",
 	"Store queue parity",
 	"Miss address buffer payload parity",
 	"L1 TLB parity",
-	"",						/* reserved */
+	"Reserved",
 	"DC tag error type 6",
 	"DC tag error type 1",
 	"Internal error type 1",
@@ -172,7 +172,7 @@ static const char * const f17h_ls_mce_desc[] = {
 	"L2 fill data error",
 };
 
-static const char * const f17h_if_mce_desc[] = {
+static const char * const smca_if_mce_desc[] = {
 	"microtag probe port parity error",
 	"IC microtag or full tag multi-hit error",
 	"IC full tag parity",
@@ -185,19 +185,22 @@ static const char * const f17h_if_mce_desc[] = {
 	"BPQ snoop parity on Thread 1",
 	"L1 BTB multi-match error",
 	"L2 BTB multi-match error",
+	"L2 Cache Response Poison error",
+	"System Read Data error",
 };
 
-static const char * const f17h_l2_mce_desc[] = {
+static const char * const smca_l2_mce_desc[] = {
 	"L2M tag multi-way-hit error",
 	"L2M tag ECC error",
 	"L2M data ECC error",
 	"HW assert",
 };
 
-static const char * const f17h_de_mce_desc[] = {
+static const char * const smca_de_mce_desc[] = {
 	"uop cache tag parity error",
 	"uop cache data parity error",
 	"Insn buffer parity error",
+	"uop queue parity error",
 	"Insn dispatch queue parity error",
 	"Fetch address FIFO parity",
 	"Patch RAM data parity",
@@ -205,7 +208,7 @@ static const char * const f17h_de_mce_desc[] = {
 	"uop buffer parity"
 };
 
-static const char * const f17h_ex_mce_desc[] = {
+static const char * const smca_ex_mce_desc[] = {
 	"Watchdog timeout error",
 	"Phy register file parity",
 	"Flag register file parity",
@@ -214,18 +217,22 @@ static const char * const f17h_ex_mce_desc[] = {
 	"EX payload parity",
 	"Checkpoint queue parity",
 	"Retire dispatch queue parity",
+	"Retire status queue parity error",
+	"Scheduling queue parity error",
+	"Branch buffer queue parity error",
 };
 
-static const char * const f17h_fp_mce_desc[] = {
+static const char * const smca_fp_mce_desc[] = {
 	"Physical register file parity",
 	"Freelist parity error",
 	"Schedule queue parity",
 	"NSQ parity error",
 	"Retire queue parity",
 	"Status register file parity",
+	"Hardware assertion",
 };
 
-static const char * const f17h_l3_mce_desc[] = {
+static const char * const smca_l3_mce_desc[] = {
 	"Shadow tag macro ECC error",
 	"Shadow tag macro multi-way-hit error",
 	"L3M tag ECC error",
@@ -236,7 +243,7 @@ static const char * const f17h_l3_mce_desc[] = {
 	"L3 HW assert",
 };
 
-static const char * const f17h_cs_mce_desc[] = {
+static const char * const smca_cs_mce_desc[] = {
 	"Illegal request from transport layer",
 	"Address violation",
 	"Security violation",
@@ -248,14 +255,14 @@ static const char * const f17h_cs_mce_desc[] = {
 	"ECC error on probe filter access",
 };
 
-static const char * const f17h_pie_mce_desc[] = {
+static const char * const smca_pie_mce_desc[] = {
 	"HW assert",
 	"Internal PIE register security violation",
 	"Error on GMI link",
 	"Poison data written to internal PIE register",
 };
 
-static const char * const f17h_umc_mce_desc[] = {
+static const char * const smca_umc_mce_desc[] = {
 	"DRAM ECC error",
 	"Data poison error on DRAM",
 	"SDP parity error",
@@ -264,18 +271,39 @@ static const char * const f17h_umc_mce_desc[] = {
 	"Write data CRC error",
 };
 
-static const char * const f17h_pb_mce_desc[] = {
+static const char * const smca_pb_mce_desc[] = {
 	"Parameter Block RAM ECC error",
 };
 
-static const char * const f17h_psp_mce_desc[] = {
+static const char * const smca_psp_mce_desc[] = {
 	"PSP RAM ECC or parity error",
 };
 
-static const char * const f17h_smu_mce_desc[] = {
+static const char * const smca_smu_mce_desc[] = {
 	"SMU RAM ECC or parity error",
 };
 
+struct smca_mce_desc {
+	const char * const *descs;
+	unsigned int num_descs;
+};
+
+static struct smca_mce_desc smca_mce_descs[] = {
+	[SMCA_LS]	= { smca_ls_mce_desc,	ARRAY_SIZE(smca_ls_mce_desc)	},
+	[SMCA_IF]	= { smca_if_mce_desc,	ARRAY_SIZE(smca_if_mce_desc)	},
+	[SMCA_L2_CACHE]	= { smca_l2_mce_desc,	ARRAY_SIZE(smca_l2_mce_desc)	},
+	[SMCA_DE]	= { smca_de_mce_desc,	ARRAY_SIZE(smca_de_mce_desc)	},
+	[SMCA_EX]	= { smca_ex_mce_desc,	ARRAY_SIZE(smca_ex_mce_desc)	},
+	[SMCA_FP]	= { smca_fp_mce_desc,	ARRAY_SIZE(smca_fp_mce_desc)	},
+	[SMCA_L3_CACHE]	= { smca_l3_mce_desc,	ARRAY_SIZE(smca_l3_mce_desc)	},
+	[SMCA_CS]	= { smca_cs_mce_desc,	ARRAY_SIZE(smca_cs_mce_desc)	},
+	[SMCA_PIE]	= { smca_pie_mce_desc,	ARRAY_SIZE(smca_pie_mce_desc)	},
+	[SMCA_UMC]	= { smca_umc_mce_desc,	ARRAY_SIZE(smca_umc_mce_desc)	},
+	[SMCA_PB]	= { smca_pb_mce_desc,	ARRAY_SIZE(smca_pb_mce_desc)	},
+	[SMCA_PSP]	= { smca_psp_mce_desc,	ARRAY_SIZE(smca_psp_mce_desc)	},
+	[SMCA_SMU]	= { smca_smu_mce_desc,	ARRAY_SIZE(smca_smu_mce_desc)	},
+};
+
 static bool f12h_mc0_mce(u16 ec, u8 xec)
 {
 	bool ret = false;
@@ -820,175 +848,35 @@ static void decode_mc6_mce(struct mce *m)
 	pr_emerg(HW_ERR "Corrupted MC6 MCE info?\n");
 }
 
-static void decode_f17h_core_errors(const char *ip_name, u8 xec,
-				   unsigned int mca_type)
-{
-	const char * const *error_desc_array;
-	size_t len;
-
-	pr_emerg(HW_ERR "%s Error: ", ip_name);
-
-	switch (mca_type) {
-	case SMCA_LS:
-		error_desc_array = f17h_ls_mce_desc;
-		len = ARRAY_SIZE(f17h_ls_mce_desc) - 1;
-
-		if (xec == 0x4) {
-			pr_cont("Unrecognized LS MCA error code.\n");
-			return;
-		}
-		break;
-
-	case SMCA_IF:
-		error_desc_array = f17h_if_mce_desc;
-		len = ARRAY_SIZE(f17h_if_mce_desc) - 1;
-		break;
-
-	case SMCA_L2_CACHE:
-		error_desc_array = f17h_l2_mce_desc;
-		len = ARRAY_SIZE(f17h_l2_mce_desc) - 1;
-		break;
-
-	case SMCA_DE:
-		error_desc_array = f17h_de_mce_desc;
-		len = ARRAY_SIZE(f17h_de_mce_desc) - 1;
-		break;
-
-	case SMCA_EX:
-		error_desc_array = f17h_ex_mce_desc;
-		len = ARRAY_SIZE(f17h_ex_mce_desc) - 1;
-		break;
-
-	case SMCA_FP:
-		error_desc_array = f17h_fp_mce_desc;
-		len = ARRAY_SIZE(f17h_fp_mce_desc) - 1;
-		break;
-
-	case SMCA_L3_CACHE:
-		error_desc_array = f17h_l3_mce_desc;
-		len = ARRAY_SIZE(f17h_l3_mce_desc) - 1;
-		break;
-
-	default:
-		pr_cont("Corrupted MCA core error info.\n");
-		return;
-	}
-
-	if (xec > len) {
-		pr_cont("Unrecognized %s MCA bank error code.\n",
-			 amd_core_mcablock_names[mca_type]);
-		return;
-	}
-
-	pr_cont("%s.\n", error_desc_array[xec]);
-}
-
-static void decode_df_errors(u8 xec, unsigned int mca_type)
-{
-	const char * const *error_desc_array;
-	size_t len;
-
-	pr_emerg(HW_ERR "Data Fabric Error: ");
-
-	switch (mca_type) {
-	case  SMCA_CS:
-		error_desc_array = f17h_cs_mce_desc;
-		len = ARRAY_SIZE(f17h_cs_mce_desc) - 1;
-		break;
-
-	case SMCA_PIE:
-		error_desc_array = f17h_pie_mce_desc;
-		len = ARRAY_SIZE(f17h_pie_mce_desc) - 1;
-		break;
-
-	default:
-		pr_cont("Corrupted MCA Data Fabric info.\n");
-		return;
-	}
-
-	if (xec > len) {
-		pr_cont("Unrecognized %s MCA bank error code.\n",
-			 amd_df_mcablock_names[mca_type]);
-		return;
-	}
-
-	pr_cont("%s.\n", error_desc_array[xec]);
-}
-
 /* Decode errors according to Scalable MCA specification */
 static void decode_smca_errors(struct mce *m)
 {
-	u32 addr = MSR_AMD64_SMCA_MCx_IPID(m->bank);
-	unsigned int hwid, mca_type, i;
-	u8 xec = XEC(m->status, xec_mask);
-	const char * const *error_desc_array;
+	struct smca_hwid_mcatype *type;
+	unsigned int bank_type;
 	const char *ip_name;
-	u32 low, high;
-	size_t len;
+	u8 xec = XEC(m->status, xec_mask);
 
-	if (rdmsr_safe(addr, &low, &high)) {
-		pr_emerg("Invalid IP block specified, error information is unreliable.\n");
+	if (m->bank >= ARRAY_SIZE(smca_banks))
 		return;
-	}
-
-	hwid = high & MCI_IPID_HWID;
-	mca_type = (high & MCI_IPID_MCATYPE) >> 16;
-
-	pr_emerg(HW_ERR "MC%d IPID value: 0x%08x%08x\n", m->bank, high, low);
-
-	/*
-	 * Based on hwid and mca_type values, decode errors from respective IPs.
-	 * Note: mca_type values make sense only in the context of an hwid.
-	 */
-	for (i = 0; i < ARRAY_SIZE(amd_hwids); i++)
-		if (amd_hwids[i].hwid == hwid)
-			break;
-
-	switch (i) {
-	case SMCA_F17H_CORE:
-		ip_name = (mca_type == SMCA_L3_CACHE) ?
-			  "L3 Cache" : "F17h Core";
-		return decode_f17h_core_errors(ip_name, xec, mca_type);
-		break;
 
-	case SMCA_DF:
-		return decode_df_errors(xec, mca_type);
-		break;
-
-	case SMCA_UMC:
-		error_desc_array = f17h_umc_mce_desc;
-		len = ARRAY_SIZE(f17h_umc_mce_desc) - 1;
-		break;
-
-	case SMCA_PB:
-		error_desc_array = f17h_pb_mce_desc;
-		len = ARRAY_SIZE(f17h_pb_mce_desc) - 1;
-		break;
+	if (boot_cpu_data.x86 >= 0x17 && m->bank == 4)
+		pr_emerg(HW_ERR "Bank 4 is reserved on Fam17h.\n");
 
-	case SMCA_PSP:
-		error_desc_array = f17h_psp_mce_desc;
-		len = ARRAY_SIZE(f17h_psp_mce_desc) - 1;
-		break;
-
-	case SMCA_SMU:
-		error_desc_array = f17h_smu_mce_desc;
-		len = ARRAY_SIZE(f17h_smu_mce_desc) - 1;
-		break;
-
-	default:
-		pr_emerg(HW_ERR "HWID:%d does not match any existing IPs.\n", hwid);
+	type = smca_banks[m->bank].type;
+	if (!type)
 		return;
-	}
 
-	ip_name = amd_hwids[i].name;
-	pr_emerg(HW_ERR "%s Error: ", ip_name);
+	bank_type = type->bank_type;
+	ip_name = smca_bank_names[bank_type].long_name;
 
-	if (xec > len) {
-		pr_cont("Unrecognized %s MCA bank error code.\n", ip_name);
-		return;
-	}
+	pr_emerg(HW_ERR "%s Extended Error Code: %d\n", ip_name, xec);
 
-	pr_cont("%s.\n", error_desc_array[xec]);
+	/* Only print the decode of valid error codes */
+	if (xec < smca_mce_descs[bank_type].num_descs &&
+			(type->xec_bitmap & BIT_ULL(xec))) {
+		pr_emerg(HW_ERR "%s Error: ", ip_name);
+		pr_cont("%s.\n", smca_mce_descs[bank_type].descs[xec]);
+	}
 }
 
 static inline void amd_decode_err_code(u16 ec)
@@ -1078,6 +966,8 @@ int amd_decode_mce(struct notifier_block *nb, unsigned long val, void *data)
 		u32 low, high;
 		u32 addr = MSR_AMD64_SMCA_MCx_CONFIG(m->bank);
 
+		pr_cont("|%s", ((m->status & MCI_STATUS_SYNDV) ? "SyndV" : "-"));
+
 		if (!rdmsr_safe(addr, &low, &high) &&
 		    (low & MCI_CONFIG_MCAX))
 			pr_cont("|%s", ((m->status & MCI_STATUS_TCC) ? "TCC" : "-"));
@@ -1091,12 +981,20 @@ int amd_decode_mce(struct notifier_block *nb, unsigned long val, void *data)
 	pr_cont("]: 0x%016llx\n", m->status);
 
 	if (m->status & MCI_STATUS_ADDRV)
-		pr_emerg(HW_ERR "MC%d Error Address: 0x%016llx\n", m->bank, m->addr);
+		pr_emerg(HW_ERR "Error Addr: 0x%016llx", m->addr);
 
 	if (boot_cpu_has(X86_FEATURE_SMCA)) {
+		if (m->status & MCI_STATUS_SYNDV)
+			pr_cont(", Syndrome: 0x%016llx", m->synd);
+
+		pr_cont(", IPID: 0x%016llx", m->ipid);
+
+		pr_cont("\n");
+
 		decode_smca_errors(m);
 		goto err_code;
-	}
+	} else
+		pr_cont("\n");
 
 	if (!fam_ops)
 		goto err_code;
diff --git a/drivers/edac/mpc85xx_edac.c b/drivers/edac/mpc85xx_edac.c
index ca63d0da8889..ff0567526ee3 100644
--- a/drivers/edac/mpc85xx_edac.c
+++ b/drivers/edac/mpc85xx_edac.c
@@ -27,15 +27,12 @@
 #include "edac_module.h"
 #include "edac_core.h"
 #include "mpc85xx_edac.h"
+#include "fsl_ddr_edac.h"
 
 static int edac_dev_idx;
 #ifdef CONFIG_PCI
 static int edac_pci_idx;
 #endif
-static int edac_mc_idx;
-
-static u32 orig_ddr_err_disable;
-static u32 orig_ddr_err_sbe;
 
 /*
  * PCI Err defines
@@ -46,103 +43,6 @@ static u32 orig_pci_err_en;
 #endif
 
 static u32 orig_l2_err_disable;
-#ifdef CONFIG_FSL_SOC_BOOKE
-static u32 orig_hid1[2];
-#endif
-
-/************************ MC SYSFS parts ***********************************/
-
-#define to_mci(k) container_of(k, struct mem_ctl_info, dev)
-
-static ssize_t mpc85xx_mc_inject_data_hi_show(struct device *dev,
-					      struct device_attribute *mattr,
-					      char *data)
-{
-	struct mem_ctl_info *mci = to_mci(dev);
-	struct mpc85xx_mc_pdata *pdata = mci->pvt_info;
-	return sprintf(data, "0x%08x",
-		       in_be32(pdata->mc_vbase +
-			       MPC85XX_MC_DATA_ERR_INJECT_HI));
-}
-
-static ssize_t mpc85xx_mc_inject_data_lo_show(struct device *dev,
-					      struct device_attribute *mattr,
-					      char *data)
-{
-	struct mem_ctl_info *mci = to_mci(dev);
-	struct mpc85xx_mc_pdata *pdata = mci->pvt_info;
-	return sprintf(data, "0x%08x",
-		       in_be32(pdata->mc_vbase +
-			       MPC85XX_MC_DATA_ERR_INJECT_LO));
-}
-
-static ssize_t mpc85xx_mc_inject_ctrl_show(struct device *dev,
-					   struct device_attribute *mattr,
-					   char *data)
-{
-	struct mem_ctl_info *mci = to_mci(dev);
-	struct mpc85xx_mc_pdata *pdata = mci->pvt_info;
-	return sprintf(data, "0x%08x",
-		       in_be32(pdata->mc_vbase + MPC85XX_MC_ECC_ERR_INJECT));
-}
-
-static ssize_t mpc85xx_mc_inject_data_hi_store(struct device *dev,
-					       struct device_attribute *mattr,
-					       const char *data, size_t count)
-{
-	struct mem_ctl_info *mci = to_mci(dev);
-	struct mpc85xx_mc_pdata *pdata = mci->pvt_info;
-	if (isdigit(*data)) {
-		out_be32(pdata->mc_vbase + MPC85XX_MC_DATA_ERR_INJECT_HI,
-			 simple_strtoul(data, NULL, 0));
-		return count;
-	}
-	return 0;
-}
-
-static ssize_t mpc85xx_mc_inject_data_lo_store(struct device *dev,
-					       struct device_attribute *mattr,
-					       const char *data, size_t count)
-{
-	struct mem_ctl_info *mci = to_mci(dev);
-	struct mpc85xx_mc_pdata *pdata = mci->pvt_info;
-	if (isdigit(*data)) {
-		out_be32(pdata->mc_vbase + MPC85XX_MC_DATA_ERR_INJECT_LO,
-			 simple_strtoul(data, NULL, 0));
-		return count;
-	}
-	return 0;
-}
-
-static ssize_t mpc85xx_mc_inject_ctrl_store(struct device *dev,
-					       struct device_attribute *mattr,
-					       const char *data, size_t count)
-{
-	struct mem_ctl_info *mci = to_mci(dev);
-	struct mpc85xx_mc_pdata *pdata = mci->pvt_info;
-	if (isdigit(*data)) {
-		out_be32(pdata->mc_vbase + MPC85XX_MC_ECC_ERR_INJECT,
-			 simple_strtoul(data, NULL, 0));
-		return count;
-	}
-	return 0;
-}
-
-DEVICE_ATTR(inject_data_hi, S_IRUGO | S_IWUSR,
-	    mpc85xx_mc_inject_data_hi_show, mpc85xx_mc_inject_data_hi_store);
-DEVICE_ATTR(inject_data_lo, S_IRUGO | S_IWUSR,
-	    mpc85xx_mc_inject_data_lo_show, mpc85xx_mc_inject_data_lo_store);
-DEVICE_ATTR(inject_ctrl, S_IRUGO | S_IWUSR,
-	    mpc85xx_mc_inject_ctrl_show, mpc85xx_mc_inject_ctrl_store);
-
-static struct attribute *mpc85xx_dev_attrs[] = {
-	&dev_attr_inject_data_hi.attr,
-	&dev_attr_inject_data_lo.attr,
-	&dev_attr_inject_ctrl.attr,
-	NULL
-};
-
-ATTRIBUTE_GROUPS(mpc85xx_dev);
 
 /**************************** PCI Err device ***************************/
 #ifdef CONFIG_PCI
@@ -160,18 +60,18 @@ static void mpc85xx_pci_check(struct edac_pci_ctl_info *pci)
 		return;
 	}
 
-	printk(KERN_ERR "PCI error(s) detected\n");
-	printk(KERN_ERR "PCI/X ERR_DR register: %#08x\n", err_detect);
+	pr_err("PCI error(s) detected\n");
+	pr_err("PCI/X ERR_DR register: %#08x\n", err_detect);
 
-	printk(KERN_ERR "PCI/X ERR_ATTRIB register: %#08x\n",
+	pr_err("PCI/X ERR_ATTRIB register: %#08x\n",
 	       in_be32(pdata->pci_vbase + MPC85XX_PCI_ERR_ATTRIB));
-	printk(KERN_ERR "PCI/X ERR_ADDR register: %#08x\n",
+	pr_err("PCI/X ERR_ADDR register: %#08x\n",
 	       in_be32(pdata->pci_vbase + MPC85XX_PCI_ERR_ADDR));
-	printk(KERN_ERR "PCI/X ERR_EXT_ADDR register: %#08x\n",
+	pr_err("PCI/X ERR_EXT_ADDR register: %#08x\n",
 	       in_be32(pdata->pci_vbase + MPC85XX_PCI_ERR_EXT_ADDR));
-	printk(KERN_ERR "PCI/X ERR_DL register: %#08x\n",
+	pr_err("PCI/X ERR_DL register: %#08x\n",
 	       in_be32(pdata->pci_vbase + MPC85XX_PCI_ERR_DL));
-	printk(KERN_ERR "PCI/X ERR_DH register: %#08x\n",
+	pr_err("PCI/X ERR_DH register: %#08x\n",
 	       in_be32(pdata->pci_vbase + MPC85XX_PCI_ERR_DH));
 
 	/* clear error bits */
@@ -187,14 +87,14 @@ static void mpc85xx_pci_check(struct edac_pci_ctl_info *pci)
 static void mpc85xx_pcie_check(struct edac_pci_ctl_info *pci)
 {
 	struct mpc85xx_pci_pdata *pdata = pci->pvt_info;
-	u32 err_detect;
+	u32 err_detect, err_cap_stat;
 
 	err_detect = in_be32(pdata->pci_vbase + MPC85XX_PCI_ERR_DR);
+	err_cap_stat = in_be32(pdata->pci_vbase + MPC85XX_PCI_GAS_TIMR);
 
 	pr_err("PCIe error(s) detected\n");
 	pr_err("PCIe ERR_DR register: 0x%08x\n", err_detect);
-	pr_err("PCIe ERR_CAP_STAT register: 0x%08x\n",
-			in_be32(pdata->pci_vbase + MPC85XX_PCI_GAS_TIMR));
+	pr_err("PCIe ERR_CAP_STAT register: 0x%08x\n", err_cap_stat);
 	pr_err("PCIe ERR_CAP_R0 register: 0x%08x\n",
 			in_be32(pdata->pci_vbase + MPC85XX_PCIE_ERR_CAP_R0));
 	pr_err("PCIe ERR_CAP_R1 register: 0x%08x\n",
@@ -206,6 +106,9 @@ static void mpc85xx_pcie_check(struct edac_pci_ctl_info *pci)
 
 	/* clear error bits */
 	out_be32(pdata->pci_vbase + MPC85XX_PCI_ERR_DR, err_detect);
+
+	/* reset error capture */
+	out_be32(pdata->pci_vbase + MPC85XX_PCI_GAS_TIMR, err_cap_stat | 0x1);
 }
 
 static int mpc85xx_pcie_find_capability(struct device_node *np)
@@ -267,7 +170,6 @@ static int mpc85xx_pci_err_probe(struct platform_device *op)
 
 	pdata = pci->pvt_info;
 	pdata->name = "mpc85xx_pci_err";
-	pdata->irq = NO_IRQ;
 
 	plat_data = op->dev.platform_data;
 	if (!plat_data) {
@@ -297,8 +199,7 @@ static int mpc85xx_pci_err_probe(struct platform_device *op)
 
 	res = of_address_to_resource(of_node, 0, &r);
 	if (res) {
-		printk(KERN_ERR "%s: Unable to get resource for "
-		       "PCI err regs\n", __func__);
+		pr_err("%s: Unable to get resource for PCI err regs\n", __func__);
 		goto err;
 	}
 
@@ -307,15 +208,14 @@ static int mpc85xx_pci_err_probe(struct platform_device *op)
 
 	if (!devm_request_mem_region(&op->dev, r.start, resource_size(&r),
 					pdata->name)) {
-		printk(KERN_ERR "%s: Error while requesting mem region\n",
-		       __func__);
+		pr_err("%s: Error while requesting mem region\n", __func__);
 		res = -EBUSY;
 		goto err;
 	}
 
 	pdata->pci_vbase = devm_ioremap(&op->dev, r.start, resource_size(&r));
 	if (!pdata->pci_vbase) {
-		printk(KERN_ERR "%s: Unable to setup PCI err regs\n", __func__);
+		pr_err("%s: Unable to setup PCI err regs\n", __func__);
 		res = -ENOMEM;
 		goto err;
 	}
@@ -344,6 +244,9 @@ static int mpc85xx_pci_err_probe(struct platform_device *op)
 	/* clear error bits */
 	out_be32(pdata->pci_vbase + MPC85XX_PCI_ERR_DR, ~0);
 
+	/* reset error capture */
+	out_be32(pdata->pci_vbase + MPC85XX_PCI_GAS_TIMR, 0x1);
+
 	if (edac_pci_add_device(pci, pdata->edac_idx) > 0) {
 		edac_dbg(3, "failed edac_pci_add_device()\n");
 		goto err;
@@ -356,15 +259,14 @@ static int mpc85xx_pci_err_probe(struct platform_device *op)
 				       IRQF_SHARED,
 				       "[EDAC] PCI err", pci);
 		if (res < 0) {
-			printk(KERN_ERR
-			       "%s: Unable to request irq %d for "
-			       "MPC85xx PCI err\n", __func__, pdata->irq);
+			pr_err("%s: Unable to request irq %d for MPC85xx PCI err\n",
+				__func__, pdata->irq);
 			irq_dispose_mapping(pdata->irq);
 			res = -ENODEV;
 			goto err2;
 		}
 
-		printk(KERN_INFO EDAC_MOD_STR " acquired irq %d for PCI Err\n",
+		pr_info(EDAC_MOD_STR " acquired irq %d for PCI Err\n",
 		       pdata->irq);
 	}
 
@@ -386,7 +288,7 @@ static int mpc85xx_pci_err_probe(struct platform_device *op)
 
 	devres_remove_group(&op->dev, mpc85xx_pci_err_probe);
 	edac_dbg(3, "success\n");
-	printk(KERN_INFO EDAC_MOD_STR " PCI err registered\n");
+	pr_info(EDAC_MOD_STR " PCI err registered\n");
 
 	return 0;
 
@@ -529,17 +431,17 @@ static void mpc85xx_l2_check(struct edac_device_ctl_info *edac_dev)
 	if (!(err_detect & L2_EDE_MASK))
 		return;
 
-	printk(KERN_ERR "ECC Error in CPU L2 cache\n");
-	printk(KERN_ERR "L2 Error Detect Register: 0x%08x\n", err_detect);
-	printk(KERN_ERR "L2 Error Capture Data High Register: 0x%08x\n",
+	pr_err("ECC Error in CPU L2 cache\n");
+	pr_err("L2 Error Detect Register: 0x%08x\n", err_detect);
+	pr_err("L2 Error Capture Data High Register: 0x%08x\n",
 	       in_be32(pdata->l2_vbase + MPC85XX_L2_CAPTDATAHI));
-	printk(KERN_ERR "L2 Error Capture Data Lo Register: 0x%08x\n",
+	pr_err("L2 Error Capture Data Lo Register: 0x%08x\n",
 	       in_be32(pdata->l2_vbase + MPC85XX_L2_CAPTDATALO));
-	printk(KERN_ERR "L2 Error Syndrome Register: 0x%08x\n",
+	pr_err("L2 Error Syndrome Register: 0x%08x\n",
 	       in_be32(pdata->l2_vbase + MPC85XX_L2_CAPTECC));
-	printk(KERN_ERR "L2 Error Attributes Capture Register: 0x%08x\n",
+	pr_err("L2 Error Attributes Capture Register: 0x%08x\n",
 	       in_be32(pdata->l2_vbase + MPC85XX_L2_ERRATTR));
-	printk(KERN_ERR "L2 Error Address Capture Register: 0x%08x\n",
+	pr_err("L2 Error Address Capture Register: 0x%08x\n",
 	       in_be32(pdata->l2_vbase + MPC85XX_L2_ERRADDR));
 
 	/* clear error detect register */
@@ -588,7 +490,6 @@ static int mpc85xx_l2_err_probe(struct platform_device *op)
 
 	pdata = edac_dev->pvt_info;
 	pdata->name = "mpc85xx_l2_err";
-	pdata->irq = NO_IRQ;
 	edac_dev->dev = &op->dev;
 	dev_set_drvdata(edac_dev->dev, edac_dev);
 	edac_dev->ctl_name = pdata->name;
@@ -596,8 +497,7 @@ static int mpc85xx_l2_err_probe(struct platform_device *op)
 
 	res = of_address_to_resource(op->dev.of_node, 0, &r);
 	if (res) {
-		printk(KERN_ERR "%s: Unable to get resource for "
-		       "L2 err regs\n", __func__);
+		pr_err("%s: Unable to get resource for L2 err regs\n", __func__);
 		goto err;
 	}
 
@@ -606,15 +506,14 @@ static int mpc85xx_l2_err_probe(struct platform_device *op)
 
 	if (!devm_request_mem_region(&op->dev, r.start, resource_size(&r),
 				     pdata->name)) {
-		printk(KERN_ERR "%s: Error while requesting mem region\n",
-		       __func__);
+		pr_err("%s: Error while requesting mem region\n", __func__);
 		res = -EBUSY;
 		goto err;
 	}
 
 	pdata->l2_vbase = devm_ioremap(&op->dev, r.start, resource_size(&r));
 	if (!pdata->l2_vbase) {
-		printk(KERN_ERR "%s: Unable to setup L2 err regs\n", __func__);
+		pr_err("%s: Unable to setup L2 err regs\n", __func__);
 		res = -ENOMEM;
 		goto err;
 	}
@@ -646,16 +545,14 @@ static int mpc85xx_l2_err_probe(struct platform_device *op)
 				       mpc85xx_l2_isr, IRQF_SHARED,
 				       "[EDAC] L2 err", edac_dev);
 		if (res < 0) {
-			printk(KERN_ERR
-			       "%s: Unable to request irq %d for "
-			       "MPC85xx L2 err\n", __func__, pdata->irq);
+			pr_err("%s: Unable to request irq %d for MPC85xx L2 err\n",
+				__func__, pdata->irq);
 			irq_dispose_mapping(pdata->irq);
 			res = -ENODEV;
 			goto err2;
 		}
 
-		printk(KERN_INFO EDAC_MOD_STR " acquired irq %d for L2 Err\n",
-		       pdata->irq);
+		pr_info(EDAC_MOD_STR " acquired irq %d for L2 Err\n", pdata->irq);
 
 		edac_dev->op_state = OP_RUNNING_INTERRUPT;
 
@@ -665,7 +562,7 @@ static int mpc85xx_l2_err_probe(struct platform_device *op)
 	devres_remove_group(&op->dev, mpc85xx_l2_err_probe);
 
 	edac_dbg(3, "success\n");
-	printk(KERN_INFO EDAC_MOD_STR " L2 err registered\n");
+	pr_info(EDAC_MOD_STR " L2 err registered\n");
 
 	return 0;
 
@@ -729,466 +626,6 @@ static struct platform_driver mpc85xx_l2_err_driver = {
 	},
 };
 
-/**************************** MC Err device ***************************/
-
-/*
- * Taken from table 8-55 in the MPC8641 User's Manual and/or 9-61 in the
- * MPC8572 User's Manual.  Each line represents a syndrome bit column as a
- * 64-bit value, but split into an upper and lower 32-bit chunk.  The labels
- * below correspond to Freescale's manuals.
- */
-static unsigned int ecc_table[16] = {
-	/* MSB           LSB */
-	/* [0:31]    [32:63] */
-	0xf00fe11e, 0xc33c0ff7,	/* Syndrome bit 7 */
-	0x00ff00ff, 0x00fff0ff,
-	0x0f0f0f0f, 0x0f0fff00,
-	0x11113333, 0x7777000f,
-	0x22224444, 0x8888222f,
-	0x44448888, 0xffff4441,
-	0x8888ffff, 0x11118882,
-	0xffff1111, 0x22221114,	/* Syndrome bit 0 */
-};
-
-/*
- * Calculate the correct ECC value for a 64-bit value specified by high:low
- */
-static u8 calculate_ecc(u32 high, u32 low)
-{
-	u32 mask_low;
-	u32 mask_high;
-	int bit_cnt;
-	u8 ecc = 0;
-	int i;
-	int j;
-
-	for (i = 0; i < 8; i++) {
-		mask_high = ecc_table[i * 2];
-		mask_low = ecc_table[i * 2 + 1];
-		bit_cnt = 0;
-
-		for (j = 0; j < 32; j++) {
-			if ((mask_high >> j) & 1)
-				bit_cnt ^= (high >> j) & 1;
-			if ((mask_low >> j) & 1)
-				bit_cnt ^= (low >> j) & 1;
-		}
-
-		ecc |= bit_cnt << i;
-	}
-
-	return ecc;
-}
-
-/*
- * Create the syndrome code which is generated if the data line specified by
- * 'bit' failed.  Eg generate an 8-bit codes seen in Table 8-55 in the MPC8641
- * User's Manual and 9-61 in the MPC8572 User's Manual.
- */
-static u8 syndrome_from_bit(unsigned int bit) {
-	int i;
-	u8 syndrome = 0;
-
-	/*
-	 * Cycle through the upper or lower 32-bit portion of each value in
-	 * ecc_table depending on if 'bit' is in the upper or lower half of
-	 * 64-bit data.
-	 */
-	for (i = bit < 32; i < 16; i += 2)
-		syndrome |= ((ecc_table[i] >> (bit % 32)) & 1) << (i / 2);
-
-	return syndrome;
-}
-
-/*
- * Decode data and ecc syndrome to determine what went wrong
- * Note: This can only decode single-bit errors
- */
-static void sbe_ecc_decode(u32 cap_high, u32 cap_low, u32 cap_ecc,
-		       int *bad_data_bit, int *bad_ecc_bit)
-{
-	int i;
-	u8 syndrome;
-
-	*bad_data_bit = -1;
-	*bad_ecc_bit = -1;
-
-	/*
-	 * Calculate the ECC of the captured data and XOR it with the captured
-	 * ECC to find an ECC syndrome value we can search for
-	 */
-	syndrome = calculate_ecc(cap_high, cap_low) ^ cap_ecc;
-
-	/* Check if a data line is stuck... */
-	for (i = 0; i < 64; i++) {
-		if (syndrome == syndrome_from_bit(i)) {
-			*bad_data_bit = i;
-			return;
-		}
-	}
-
-	/* If data is correct, check ECC bits for errors... */
-	for (i = 0; i < 8; i++) {
-		if ((syndrome >> i) & 0x1) {
-			*bad_ecc_bit = i;
-			return;
-		}
-	}
-}
-
-#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;
-	struct csrow_info *csrow;
-	u32 bus_width;
-	u32 err_detect;
-	u32 syndrome;
-	u64 err_addr;
-	u32 pfn;
-	int row_index;
-	u32 cap_high;
-	u32 cap_low;
-	int bad_data_bit;
-	int bad_ecc_bit;
-
-	err_detect = in_be32(pdata->mc_vbase + MPC85XX_MC_ERR_DETECT);
-	if (!err_detect)
-		return;
-
-	mpc85xx_mc_printk(mci, KERN_ERR, "Err Detect Register: %#8.8x\n",
-			  err_detect);
-
-	/* no more processing if not ECC bit errors */
-	if (!(err_detect & (DDR_EDE_SBE | DDR_EDE_MBE))) {
-		out_be32(pdata->mc_vbase + MPC85XX_MC_ERR_DETECT, err_detect);
-		return;
-	}
-
-	syndrome = in_be32(pdata->mc_vbase + MPC85XX_MC_CAPTURE_ECC);
-
-	/* Mask off appropriate bits of syndrome based on bus width */
-	bus_width = (in_be32(pdata->mc_vbase + MPC85XX_MC_DDR_SDRAM_CFG) &
-			DSC_DBW_MASK) ? 32 : 64;
-	if (bus_width == 64)
-		syndrome &= 0xff;
-	else
-		syndrome &= 0xffff;
-
-	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++) {
-		csrow = mci->csrows[row_index];
-		if ((pfn >= csrow->first_page) && (pfn <= csrow->last_page))
-			break;
-	}
-
-	cap_high = in_be32(pdata->mc_vbase + MPC85XX_MC_CAPTURE_DATA_HI);
-	cap_low = in_be32(pdata->mc_vbase + MPC85XX_MC_CAPTURE_DATA_LO);
-
-	/*
-	 * Analyze single-bit errors on 64-bit wide buses
-	 * TODO: Add support for 32-bit wide buses
-	 */
-	if ((err_detect & DDR_EDE_SBE) && (bus_width == 64)) {
-		sbe_ecc_decode(cap_high, cap_low, syndrome,
-				&bad_data_bit, &bad_ecc_bit);
-
-		if (bad_data_bit != -1)
-			mpc85xx_mc_printk(mci, KERN_ERR,
-				"Faulty Data bit: %d\n", bad_data_bit);
-		if (bad_ecc_bit != -1)
-			mpc85xx_mc_printk(mci, KERN_ERR,
-				"Faulty ECC bit: %d\n", bad_ecc_bit);
-
-		mpc85xx_mc_printk(mci, KERN_ERR,
-			"Expected Data / ECC:\t%#8.8x_%08x / %#2.2x\n",
-			cap_high ^ (1 << (bad_data_bit - 32)),
-			cap_low ^ (1 << bad_data_bit),
-			syndrome ^ (1 << bad_ecc_bit));
-	}
-
-	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.8llx\n", err_addr);
-	mpc85xx_mc_printk(mci, KERN_ERR, "PFN: %#8.8x\n", pfn);
-
-	/* we are out of range */
-	if (row_index == mci->nr_csrows)
-		mpc85xx_mc_printk(mci, KERN_ERR, "PFN out of range!\n");
-
-	if (err_detect & DDR_EDE_SBE)
-		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci, 1,
-				     pfn, err_addr & ~PAGE_MASK, syndrome,
-				     row_index, 0, -1,
-				     mci->ctl_name, "");
-
-	if (err_detect & DDR_EDE_MBE)
-		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 1,
-				     pfn, err_addr & ~PAGE_MASK, syndrome,
-				     row_index, 0, -1,
-				     mci->ctl_name, "");
-
-	out_be32(pdata->mc_vbase + MPC85XX_MC_ERR_DETECT, err_detect);
-}
-
-static irqreturn_t mpc85xx_mc_isr(int irq, void *dev_id)
-{
-	struct mem_ctl_info *mci = dev_id;
-	struct mpc85xx_mc_pdata *pdata = mci->pvt_info;
-	u32 err_detect;
-
-	err_detect = in_be32(pdata->mc_vbase + MPC85XX_MC_ERR_DETECT);
-	if (!err_detect)
-		return IRQ_NONE;
-
-	mpc85xx_mc_check(mci);
-
-	return IRQ_HANDLED;
-}
-
-static void mpc85xx_init_csrows(struct mem_ctl_info *mci)
-{
-	struct mpc85xx_mc_pdata *pdata = mci->pvt_info;
-	struct csrow_info *csrow;
-	struct dimm_info *dimm;
-	u32 sdram_ctl;
-	u32 sdtype;
-	enum mem_type mtype;
-	u32 cs_bnds;
-	int index;
-
-	sdram_ctl = in_be32(pdata->mc_vbase + MPC85XX_MC_DDR_SDRAM_CFG);
-
-	sdtype = sdram_ctl & DSC_SDTYPE_MASK;
-	if (sdram_ctl & DSC_RD_EN) {
-		switch (sdtype) {
-		case DSC_SDTYPE_DDR:
-			mtype = MEM_RDDR;
-			break;
-		case DSC_SDTYPE_DDR2:
-			mtype = MEM_RDDR2;
-			break;
-		case DSC_SDTYPE_DDR3:
-			mtype = MEM_RDDR3;
-			break;
-		default:
-			mtype = MEM_UNKNOWN;
-			break;
-		}
-	} else {
-		switch (sdtype) {
-		case DSC_SDTYPE_DDR:
-			mtype = MEM_DDR;
-			break;
-		case DSC_SDTYPE_DDR2:
-			mtype = MEM_DDR2;
-			break;
-		case DSC_SDTYPE_DDR3:
-			mtype = MEM_DDR3;
-			break;
-		default:
-			mtype = MEM_UNKNOWN;
-			break;
-		}
-	}
-
-	for (index = 0; index < mci->nr_csrows; index++) {
-		u32 start;
-		u32 end;
-
-		csrow = mci->csrows[index];
-		dimm = csrow->channels[0]->dimm;
-
-		cs_bnds = in_be32(pdata->mc_vbase + MPC85XX_MC_CS_BNDS_0 +
-				  (index * MPC85XX_MC_CS_BNDS_OFS));
-
-		start = (cs_bnds & 0xffff0000) >> 16;
-		end   = (cs_bnds & 0x0000ffff);
-
-		if (start == end)
-			continue;	/* not populated */
-
-		start <<= (24 - PAGE_SHIFT);
-		end   <<= (24 - PAGE_SHIFT);
-		end    |= (1 << (24 - PAGE_SHIFT)) - 1;
-
-		csrow->first_page = start;
-		csrow->last_page = end;
-
-		dimm->nr_pages = end + 1 - start;
-		dimm->grain = 8;
-		dimm->mtype = mtype;
-		dimm->dtype = DEV_UNKNOWN;
-		if (sdram_ctl & DSC_X32_EN)
-			dimm->dtype = DEV_X32;
-		dimm->edac_mode = EDAC_SECDED;
-	}
-}
-
-static int mpc85xx_mc_err_probe(struct platform_device *op)
-{
-	struct mem_ctl_info *mci;
-	struct edac_mc_layer layers[2];
-	struct mpc85xx_mc_pdata *pdata;
-	struct resource r;
-	u32 sdram_ctl;
-	int res;
-
-	if (!devres_open_group(&op->dev, mpc85xx_mc_err_probe, GFP_KERNEL))
-		return -ENOMEM;
-
-	layers[0].type = EDAC_MC_LAYER_CHIP_SELECT;
-	layers[0].size = 4;
-	layers[0].is_virt_csrow = true;
-	layers[1].type = EDAC_MC_LAYER_CHANNEL;
-	layers[1].size = 1;
-	layers[1].is_virt_csrow = false;
-	mci = edac_mc_alloc(edac_mc_idx, ARRAY_SIZE(layers), layers,
-			    sizeof(*pdata));
-	if (!mci) {
-		devres_release_group(&op->dev, mpc85xx_mc_err_probe);
-		return -ENOMEM;
-	}
-
-	pdata = mci->pvt_info;
-	pdata->name = "mpc85xx_mc_err";
-	pdata->irq = NO_IRQ;
-	mci->pdev = &op->dev;
-	pdata->edac_idx = edac_mc_idx++;
-	dev_set_drvdata(mci->pdev, mci);
-	mci->ctl_name = pdata->name;
-	mci->dev_name = pdata->name;
-
-	res = of_address_to_resource(op->dev.of_node, 0, &r);
-	if (res) {
-		printk(KERN_ERR "%s: Unable to get resource for MC err regs\n",
-		       __func__);
-		goto err;
-	}
-
-	if (!devm_request_mem_region(&op->dev, r.start, resource_size(&r),
-				     pdata->name)) {
-		printk(KERN_ERR "%s: Error while requesting mem region\n",
-		       __func__);
-		res = -EBUSY;
-		goto err;
-	}
-
-	pdata->mc_vbase = devm_ioremap(&op->dev, r.start, resource_size(&r));
-	if (!pdata->mc_vbase) {
-		printk(KERN_ERR "%s: Unable to setup MC err regs\n", __func__);
-		res = -ENOMEM;
-		goto err;
-	}
-
-	sdram_ctl = in_be32(pdata->mc_vbase + MPC85XX_MC_DDR_SDRAM_CFG);
-	if (!(sdram_ctl & DSC_ECC_EN)) {
-		/* no ECC */
-		printk(KERN_WARNING "%s: No ECC DIMMs discovered\n", __func__);
-		res = -ENODEV;
-		goto err;
-	}
-
-	edac_dbg(3, "init mci\n");
-	mci->mtype_cap = MEM_FLAG_RDDR | MEM_FLAG_RDDR2 |
-	    MEM_FLAG_DDR | MEM_FLAG_DDR2;
-	mci->edac_ctl_cap = EDAC_FLAG_NONE | EDAC_FLAG_SECDED;
-	mci->edac_cap = EDAC_FLAG_SECDED;
-	mci->mod_name = EDAC_MOD_STR;
-	mci->mod_ver = MPC85XX_REVISION;
-
-	if (edac_op_state == EDAC_OPSTATE_POLL)
-		mci->edac_check = mpc85xx_mc_check;
-
-	mci->ctl_page_to_phys = NULL;
-
-	mci->scrub_mode = SCRUB_SW_SRC;
-
-	mpc85xx_init_csrows(mci);
-
-	/* store the original error disable bits */
-	orig_ddr_err_disable =
-	    in_be32(pdata->mc_vbase + MPC85XX_MC_ERR_DISABLE);
-	out_be32(pdata->mc_vbase + MPC85XX_MC_ERR_DISABLE, 0);
-
-	/* clear all error bits */
-	out_be32(pdata->mc_vbase + MPC85XX_MC_ERR_DETECT, ~0);
-
-	if (edac_mc_add_mc_with_groups(mci, mpc85xx_dev_groups)) {
-		edac_dbg(3, "failed edac_mc_add_mc()\n");
-		goto err;
-	}
-
-	if (edac_op_state == EDAC_OPSTATE_INT) {
-		out_be32(pdata->mc_vbase + MPC85XX_MC_ERR_INT_EN,
-			 DDR_EIE_MBEE | DDR_EIE_SBEE);
-
-		/* store the original error management threshold */
-		orig_ddr_err_sbe = in_be32(pdata->mc_vbase +
-					   MPC85XX_MC_ERR_SBE) & 0xff0000;
-
-		/* set threshold to 1 error per interrupt */
-		out_be32(pdata->mc_vbase + MPC85XX_MC_ERR_SBE, 0x10000);
-
-		/* register interrupts */
-		pdata->irq = irq_of_parse_and_map(op->dev.of_node, 0);
-		res = devm_request_irq(&op->dev, pdata->irq,
-				       mpc85xx_mc_isr,
-				       IRQF_SHARED,
-				       "[EDAC] MC err", mci);
-		if (res < 0) {
-			printk(KERN_ERR "%s: Unable to request irq %d for "
-			       "MPC85xx DRAM ERR\n", __func__, pdata->irq);
-			irq_dispose_mapping(pdata->irq);
-			res = -ENODEV;
-			goto err2;
-		}
-
-		printk(KERN_INFO EDAC_MOD_STR " acquired irq %d for MC\n",
-		       pdata->irq);
-	}
-
-	devres_remove_group(&op->dev, mpc85xx_mc_err_probe);
-	edac_dbg(3, "success\n");
-	printk(KERN_INFO EDAC_MOD_STR " MC err registered\n");
-
-	return 0;
-
-err2:
-	edac_mc_del_mc(&op->dev);
-err:
-	devres_release_group(&op->dev, mpc85xx_mc_err_probe);
-	edac_mc_free(mci);
-	return res;
-}
-
-static int mpc85xx_mc_err_remove(struct platform_device *op)
-{
-	struct mem_ctl_info *mci = dev_get_drvdata(&op->dev);
-	struct mpc85xx_mc_pdata *pdata = mci->pvt_info;
-
-	edac_dbg(0, "\n");
-
-	if (edac_op_state == EDAC_OPSTATE_INT) {
-		out_be32(pdata->mc_vbase + MPC85XX_MC_ERR_INT_EN, 0);
-		irq_dispose_mapping(pdata->irq);
-	}
-
-	out_be32(pdata->mc_vbase + MPC85XX_MC_ERR_DISABLE,
-		 orig_ddr_err_disable);
-	out_be32(pdata->mc_vbase + MPC85XX_MC_ERR_SBE, orig_ddr_err_sbe);
-
-	edac_mc_del_mc(&op->dev);
-	edac_mc_free(mci);
-	return 0;
-}
-
 static const struct of_device_id mpc85xx_mc_err_of_match[] = {
 /* deprecate the fsl,85.. forms in the future, 2.6.30? */
 	{ .compatible = "fsl,8540-memory-controller", },
@@ -1217,22 +654,14 @@ static const struct of_device_id mpc85xx_mc_err_of_match[] = {
 MODULE_DEVICE_TABLE(of, mpc85xx_mc_err_of_match);
 
 static struct platform_driver mpc85xx_mc_err_driver = {
-	.probe = mpc85xx_mc_err_probe,
-	.remove = mpc85xx_mc_err_remove,
+	.probe = fsl_mc_err_probe,
+	.remove = fsl_mc_err_remove,
 	.driver = {
 		.name = "mpc85xx_mc_err",
 		.of_match_table = mpc85xx_mc_err_of_match,
 	},
 };
 
-#ifdef CONFIG_FSL_SOC_BOOKE
-static void __init mpc85xx_mc_clear_rfxe(void *data)
-{
-	orig_hid1[smp_processor_id()] = mfspr(SPRN_HID1);
-	mtspr(SPRN_HID1, (orig_hid1[smp_processor_id()] & ~HID1_RFXE));
-}
-#endif
-
 static struct platform_driver * const drivers[] = {
 	&mpc85xx_mc_err_driver,
 	&mpc85xx_l2_err_driver,
@@ -1246,8 +675,7 @@ static int __init mpc85xx_mc_init(void)
 	int res = 0;
 	u32 __maybe_unused pvr = 0;
 
-	printk(KERN_INFO "Freescale(R) MPC85xx EDAC driver, "
-	       "(C) 2006 Montavista Software\n");
+	pr_info("Freescale(R) MPC85xx EDAC driver, (C) 2006 Montavista Software\n");
 
 	/* make sure error reporting method is sane */
 	switch (edac_op_state) {
@@ -1261,44 +689,15 @@ static int __init mpc85xx_mc_init(void)
 
 	res = platform_register_drivers(drivers, ARRAY_SIZE(drivers));
 	if (res)
-		printk(KERN_WARNING EDAC_MOD_STR "drivers fail to register\n");
-
-#ifdef CONFIG_FSL_SOC_BOOKE
-	pvr = mfspr(SPRN_PVR);
-
-	if ((PVR_VER(pvr) == PVR_VER_E500V1) ||
-	    (PVR_VER(pvr) == PVR_VER_E500V2)) {
-		/*
-		 * need to clear HID1[RFXE] to disable machine check int
-		 * so we can catch it
-		 */
-		if (edac_op_state == EDAC_OPSTATE_INT)
-			on_each_cpu(mpc85xx_mc_clear_rfxe, NULL, 0);
-	}
-#endif
+		pr_warn(EDAC_MOD_STR "drivers fail to register\n");
 
 	return 0;
 }
 
 module_init(mpc85xx_mc_init);
 
-#ifdef CONFIG_FSL_SOC_BOOKE
-static void __exit mpc85xx_mc_restore_hid1(void *data)
-{
-	mtspr(SPRN_HID1, orig_hid1[smp_processor_id()]);
-}
-#endif
-
 static void __exit mpc85xx_mc_exit(void)
 {
-#ifdef CONFIG_FSL_SOC_BOOKE
-	u32 pvr = mfspr(SPRN_PVR);
-
-	if ((PVR_VER(pvr) == PVR_VER_E500V1) ||
-	    (PVR_VER(pvr) == PVR_VER_E500V2)) {
-		on_each_cpu(mpc85xx_mc_restore_hid1, NULL, 0);
-	}
-#endif
 	platform_unregister_drivers(drivers, ARRAY_SIZE(drivers));
 }
 
diff --git a/drivers/edac/mpc85xx_edac.h b/drivers/edac/mpc85xx_edac.h
index 9352e88d53e5..3f6fb16ad34f 100644
--- a/drivers/edac/mpc85xx_edac.h
+++ b/drivers/edac/mpc85xx_edac.h
@@ -17,65 +17,6 @@
 #define mpc85xx_printk(level, fmt, arg...) \
 	edac_printk(level, "MPC85xx", fmt, ##arg)
 
-#define mpc85xx_mc_printk(mci, level, fmt, arg...) \
-	edac_mc_chipset_printk(mci, level, "MPC85xx", fmt, ##arg)
-
-/*
- * DRAM error defines
- */
-
-/* DDR_SDRAM_CFG */
-#define MPC85XX_MC_DDR_SDRAM_CFG	0x0110
-#define MPC85XX_MC_CS_BNDS_0		0x0000
-#define MPC85XX_MC_CS_BNDS_1		0x0008
-#define MPC85XX_MC_CS_BNDS_2		0x0010
-#define MPC85XX_MC_CS_BNDS_3		0x0018
-#define MPC85XX_MC_CS_BNDS_OFS		0x0008
-
-#define MPC85XX_MC_DATA_ERR_INJECT_HI	0x0e00
-#define MPC85XX_MC_DATA_ERR_INJECT_LO	0x0e04
-#define MPC85XX_MC_ECC_ERR_INJECT	0x0e08
-#define MPC85XX_MC_CAPTURE_DATA_HI	0x0e20
-#define MPC85XX_MC_CAPTURE_DATA_LO	0x0e24
-#define MPC85XX_MC_CAPTURE_ECC		0x0e28
-#define MPC85XX_MC_ERR_DETECT		0x0e40
-#define MPC85XX_MC_ERR_DISABLE		0x0e44
-#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
-#define DSC_ECC_EN	0x20000000
-#define DSC_RD_EN	0x10000000
-#define DSC_DBW_MASK	0x00180000
-#define DSC_DBW_32	0x00080000
-#define DSC_DBW_64	0x00000000
-
-#define DSC_SDTYPE_MASK		0x07000000
-
-#define DSC_SDTYPE_DDR		0x02000000
-#define DSC_SDTYPE_DDR2		0x03000000
-#define DSC_SDTYPE_DDR3		0x07000000
-#define DSC_X32_EN	0x00000020
-
-/* Err_Int_En */
-#define DDR_EIE_MSEE	0x1	/* memory select */
-#define DDR_EIE_SBEE	0x4	/* single-bit ECC error */
-#define DDR_EIE_MBEE	0x8	/* multi-bit ECC error */
-
-/* Err_Detect */
-#define DDR_EDE_MSE		0x1	/* memory select */
-#define DDR_EDE_SBE		0x4	/* single-bit ECC error */
-#define DDR_EDE_MBE		0x8	/* multi-bit ECC error */
-#define DDR_EDE_MME		0x80000000	/* multiple memory errors */
-
-/* Err_Disable */
-#define DDR_EDI_MSED	0x1	/* memory select disable */
-#define	DDR_EDI_SBED	0x4	/* single-bit ECC error disable */
-#define	DDR_EDI_MBED	0x8	/* multi-bit ECC error disable */
-
 /*
  * L2 Err defines
  */
@@ -149,13 +90,6 @@
 #define MPC85XX_PCIE_ERR_CAP_R2		0x0030
 #define MPC85XX_PCIE_ERR_CAP_R3		0x0034
 
-struct mpc85xx_mc_pdata {
-	char *name;
-	int edac_idx;
-	void __iomem *mc_vbase;
-	int irq;
-};
-
 struct mpc85xx_l2_pdata {
 	char *name;
 	int edac_idx;
diff --git a/drivers/edac/mv64x60_edac.c b/drivers/edac/mv64x60_edac.c
index 6c54127e6eae..cb9b8577acbc 100644
--- a/drivers/edac/mv64x60_edac.c
+++ b/drivers/edac/mv64x60_edac.c
@@ -118,7 +118,6 @@ static int mv64x60_pci_err_probe(struct platform_device *pdev)
 
 	pdata->pci_hose = pdev->id;
 	pdata->name = "mpc85xx_pci_err";
-	pdata->irq = NO_IRQ;
 	platform_set_drvdata(pdev, pci);
 	pci->dev = &pdev->dev;
 	pci->dev_name = dev_name(&pdev->dev);
@@ -291,7 +290,6 @@ static int mv64x60_sram_err_probe(struct platform_device *pdev)
 
 	pdata = edac_dev->pvt_info;
 	pdata->name = "mv64x60_sram_err";
-	pdata->irq = NO_IRQ;
 	edac_dev->dev = &pdev->dev;
 	platform_set_drvdata(pdev, edac_dev);
 	edac_dev->dev_name = dev_name(&pdev->dev);
@@ -459,7 +457,6 @@ static int mv64x60_cpu_err_probe(struct platform_device *pdev)
 
 	pdata = edac_dev->pvt_info;
 	pdata->name = "mv64x60_cpu_err";
-	pdata->irq = NO_IRQ;
 	edac_dev->dev = &pdev->dev;
 	platform_set_drvdata(pdev, edac_dev);
 	edac_dev->dev_name = dev_name(&pdev->dev);
@@ -727,7 +724,6 @@ static int mv64x60_mc_err_probe(struct platform_device *pdev)
 	mci->pdev = &pdev->dev;
 	platform_set_drvdata(pdev, mci);
 	pdata->name = "mv64x60_mc_err";
-	pdata->irq = NO_IRQ;
 	mci->dev_name = dev_name(&pdev->dev);
 	pdata->edac_idx = edac_mc_idx++;
 
diff --git a/drivers/edac/ppc4xx_edac.c b/drivers/edac/ppc4xx_edac.c
index d3a64ba61fa3..691ce25e9010 100644
--- a/drivers/edac/ppc4xx_edac.c
+++ b/drivers/edac/ppc4xx_edac.c
@@ -1029,8 +1029,6 @@ static int ppc4xx_edac_mc_init(struct mem_ctl_info *mci,
 	pdata			= mci->pvt_info;
 
 	pdata->dcr_host		= *dcr_host;
-	pdata->irqs.sec		= NO_IRQ;
-	pdata->irqs.ded		= NO_IRQ;
 
 	/* Initialize controller capabilities and configuration */
 
@@ -1111,7 +1109,7 @@ static int ppc4xx_edac_register_irq(struct platform_device *op,
 	ded_irq = irq_of_parse_and_map(np, INTMAP_ECCDED_INDEX);
 	sec_irq = irq_of_parse_and_map(np, INTMAP_ECCSEC_INDEX);
 
-	if (ded_irq == NO_IRQ || sec_irq == NO_IRQ) {
+	if (!ded_irq || !sec_irq) {
 		ppc4xx_edac_mc_printk(KERN_ERR, mci,
 				      "Unable to map interrupts.\n");
 		status = -ENODEV;
diff --git a/drivers/edac/sb_edac.c b/drivers/edac/sb_edac.c
index ce0067b7a2f6..54775221a01f 100644
--- a/drivers/edac/sb_edac.c
+++ b/drivers/edac/sb_edac.c
@@ -2474,7 +2474,7 @@ static int sbridge_mci_bind_devs(struct mem_ctl_info *mci,
 
 	/* Check if everything were registered */
 	if (!pvt->pci_sad0 || !pvt->pci_sad1 || !pvt->pci_ha0 ||
-	    !pvt-> pci_tad || !pvt->pci_ras  || !pvt->pci_ta)
+	    !pvt->pci_ras || !pvt->pci_ta)
 		goto enodev;
 
 	if (saw_chan_mask != 0x0f)
@@ -2563,8 +2563,7 @@ static int ibridge_mci_bind_devs(struct mem_ctl_info *mci,
 
 	/* Check if everything were registered */
 	if (!pvt->pci_sad0 || !pvt->pci_ha0 || !pvt->pci_br0 ||
-	    !pvt->pci_br1 || !pvt->pci_tad || !pvt->pci_ras  ||
-	    !pvt->pci_ta)
+	    !pvt->pci_br1 || !pvt->pci_ras || !pvt->pci_ta)
 		goto enodev;
 
 	if (saw_chan_mask != 0x0f && /* -EN */
diff --git a/drivers/edac/wq.c b/drivers/edac/wq.c
index 1b8c07e44fd8..2a9a11ae2461 100644
--- a/drivers/edac/wq.c
+++ b/drivers/edac/wq.c
@@ -27,7 +27,7 @@ EXPORT_SYMBOL_GPL(edac_stop_work);
 
 int edac_workqueue_setup(void)
 {
-	wq = create_singlethread_workqueue("edac-poller");
+	wq = alloc_ordered_workqueue("edac-poller", WQ_MEM_RECLAIM);
 	if (!wq)
 		return -ENODEV;
 	else
diff --git a/drivers/extcon/Kconfig b/drivers/extcon/Kconfig
index 3d89e60a3e71..04788d92ea52 100644
--- a/drivers/extcon/Kconfig
+++ b/drivers/extcon/Kconfig
@@ -96,6 +96,12 @@ config EXTCON_PALMAS
 	  Say Y here to enable support for USB peripheral and USB host
 	  detection by palmas usb.
 
+config EXTCON_QCOM_SPMI_MISC
+	tristate "Qualcomm USB extcon support"
+	help
+	  Say Y here to enable SPMI PMIC based USB cable detection
+	  support on Qualcomm PMICs such as PM8941.
+
 config EXTCON_RT8973A
 	tristate "Richtek RT8973A EXTCON support"
 	depends on I2C
diff --git a/drivers/extcon/Makefile b/drivers/extcon/Makefile
index 972c813c375b..31a0a999c4fb 100644
--- a/drivers/extcon/Makefile
+++ b/drivers/extcon/Makefile
@@ -14,6 +14,7 @@ obj-$(CONFIG_EXTCON_MAX77693)	+= extcon-max77693.o
 obj-$(CONFIG_EXTCON_MAX77843)	+= extcon-max77843.o
 obj-$(CONFIG_EXTCON_MAX8997)	+= extcon-max8997.o
 obj-$(CONFIG_EXTCON_PALMAS)	+= extcon-palmas.o
+obj-$(CONFIG_EXTCON_QCOM_SPMI_MISC) += extcon-qcom-spmi-misc.o
 obj-$(CONFIG_EXTCON_RT8973A)	+= extcon-rt8973a.o
 obj-$(CONFIG_EXTCON_SM5502)	+= extcon-sm5502.o
 obj-$(CONFIG_EXTCON_USB_GPIO)	+= extcon-usb-gpio.o
diff --git a/drivers/extcon/extcon-adc-jack.c b/drivers/extcon/extcon-adc-jack.c
index 44e48aa78a84..bc538708c753 100644
--- a/drivers/extcon/extcon-adc-jack.c
+++ b/drivers/extcon/extcon-adc-jack.c
@@ -3,6 +3,9 @@
  *
  * Analog Jack extcon driver with ADC-based detection capability.
  *
+ * Copyright (C) 2016 Samsung Electronics
+ * Chanwoo Choi <cw00.choi@samsung.com>
+ *
  * Copyright (C) 2012 Samsung Electronics
  * MyungJoo Ham <myungjoo.ham@samsung.com>
  *
@@ -58,7 +61,7 @@ static void adc_jack_handler(struct work_struct *work)
 	struct adc_jack_data *data = container_of(to_delayed_work(work),
 			struct adc_jack_data,
 			handler);
-	u32 state = 0;
+	struct adc_jack_cond *def;
 	int ret, adc_val;
 	int i;
 
@@ -70,17 +73,18 @@ static void adc_jack_handler(struct work_struct *work)
 
 	/* Get state from adc value with adc_conditions */
 	for (i = 0; i < data->num_conditions; i++) {
-		struct adc_jack_cond *def = &data->adc_conditions[i];
-		if (!def->state)
-			break;
+		def = &data->adc_conditions[i];
 		if (def->min_adc <= adc_val && def->max_adc >= adc_val) {
-			state = def->state;
-			break;
+			extcon_set_state_sync(data->edev, def->id, true);
+			return;
 		}
 	}
-	/* if no def has met, it means state = 0 (no cables attached) */
 
-	extcon_set_state(data->edev, state);
+	/* Set the detached state if adc value is not included in the range */
+	for (i = 0; i < data->num_conditions; i++) {
+		def = &data->adc_conditions[i];
+		extcon_set_state_sync(data->edev, def->id, false);
+	}
 }
 
 static irqreturn_t adc_jack_irq_thread(int irq, void *_data)
@@ -114,16 +118,14 @@ static int adc_jack_probe(struct platform_device *pdev)
 		return -ENOMEM;
 	}
 
-	if (!pdata->adc_conditions ||
-			!pdata->adc_conditions[0].state) {
+	if (!pdata->adc_conditions) {
 		dev_err(&pdev->dev, "error: adc_conditions not defined.\n");
 		return -EINVAL;
 	}
 	data->adc_conditions = pdata->adc_conditions;
 
 	/* Check the length of array and set num_conditions */
-	for (i = 0; data->adc_conditions[i].state; i++)
-		;
+	for (i = 0; data->adc_conditions[i].id != EXTCON_NONE; i++);
 	data->num_conditions = i;
 
 	data->chan = iio_channel_get(&pdev->dev, pdata->consumer_channel);
@@ -158,6 +160,7 @@ static int adc_jack_probe(struct platform_device *pdev)
 	if (data->wakeup_source)
 		device_init_wakeup(&pdev->dev, 1);
 
+	adc_jack_handler(&data->handler.work);
 	return 0;
 }
 
diff --git a/drivers/extcon/extcon-arizona.c b/drivers/extcon/extcon-arizona.c
index 1d8e0a57bd51..56e6c4c7c60d 100644
--- a/drivers/extcon/extcon-arizona.c
+++ b/drivers/extcon/extcon-arizona.c
@@ -183,7 +183,7 @@ static void arizona_extcon_hp_clamp(struct arizona_extcon_info *info,
 		if (clamp)
 			val = ARIZONA_RMV_SHRT_HP1L;
 		break;
-	};
+	}
 
 	snd_soc_dapm_mutex_lock(arizona->dapm);
 
@@ -614,7 +614,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, EXTCON_MECHANICAL);
+	ret = extcon_get_state(info->edev, EXTCON_MECHANICAL);
 	if (ret < 0) {
 		dev_err(arizona->dev, "Failed to check cable state: %d\n",
 			ret);
@@ -649,7 +649,7 @@ static irqreturn_t arizona_hpdet_irq(int irq, void *data)
 	else
 		report = EXTCON_JACK_HEADPHONE;
 
-	ret = extcon_set_cable_state_(info->edev, report, true);
+	ret = extcon_set_state_sync(info->edev, report, true);
 	if (ret != 0)
 		dev_err(arizona->dev, "Failed to report HP/line: %d\n",
 			ret);
@@ -732,7 +732,7 @@ err:
 			   ARIZONA_ACCDET_MODE_MASK, ARIZONA_ACCDET_MODE_MIC);
 
 	/* Just report headphone */
-	ret = extcon_set_cable_state_(info->edev, EXTCON_JACK_HEADPHONE, true);
+	ret = extcon_set_state_sync(info->edev, EXTCON_JACK_HEADPHONE, true);
 	if (ret != 0)
 		dev_err(arizona->dev, "Failed to report headphone: %d\n", ret);
 
@@ -789,7 +789,7 @@ err:
 			   ARIZONA_ACCDET_MODE_MASK, ARIZONA_ACCDET_MODE_MIC);
 
 	/* Just report headphone */
-	ret = extcon_set_cable_state_(info->edev, EXTCON_JACK_HEADPHONE, true);
+	ret = extcon_set_state_sync(info->edev, EXTCON_JACK_HEADPHONE, true);
 	if (ret != 0)
 		dev_err(arizona->dev, "Failed to report headphone: %d\n", ret);
 
@@ -829,7 +829,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, EXTCON_MECHANICAL);
+	ret = extcon_get_state(info->edev, EXTCON_MECHANICAL);
 	if (ret < 0) {
 		dev_err(arizona->dev, "Failed to check cable state: %d\n",
 				ret);
@@ -914,7 +914,7 @@ static void arizona_micd_detect(struct work_struct *work)
 
 		arizona_identify_headphone(info);
 
-		ret = extcon_set_cable_state_(info->edev,
+		ret = extcon_set_state_sync(info->edev,
 					      EXTCON_JACK_MICROPHONE, true);
 		if (ret != 0)
 			dev_err(arizona->dev, "Headset report failed: %d\n",
@@ -1108,7 +1108,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,
+		ret = extcon_set_state_sync(info->edev,
 					      EXTCON_MECHANICAL, true);
 
 		if (ret != 0)
@@ -1149,10 +1149,13 @@ static irqreturn_t arizona_jackdet(int irq, void *data)
 					 info->micd_ranges[i].key, 0);
 		input_sync(info->input);
 
-		ret = extcon_update_state(info->edev, 0xffffffff, 0);
-		if (ret != 0)
-			dev_err(arizona->dev, "Removal report failed: %d\n",
-				ret);
+		for (i = 0; i < ARRAY_SIZE(arizona_cable) - 1; i++) {
+			ret = extcon_set_state_sync(info->edev,
+					arizona_cable[i], false);
+			if (ret != 0)
+				dev_err(arizona->dev,
+					"Removal report failed: %d\n", ret);
+		}
 
 		regmap_update_bits(arizona->regmap,
 				   ARIZONA_JACK_DETECT_DEBOUNCE,
diff --git a/drivers/extcon/extcon-axp288.c b/drivers/extcon/extcon-axp288.c
index fd55c2f2080a..42f41e808292 100644
--- a/drivers/extcon/extcon-axp288.c
+++ b/drivers/extcon/extcon-axp288.c
@@ -189,19 +189,19 @@ static int axp288_handle_chrg_det_event(struct axp288_extcon_info *info)
 
 	switch (chrg_type) {
 	case DET_STAT_SDP:
-		dev_dbg(info->dev, "sdp cable is connecetd\n");
+		dev_dbg(info->dev, "sdp cable is connected\n");
 		notify_otg = true;
 		notify_charger = true;
 		cable = EXTCON_CHG_USB_SDP;
 		break;
 	case DET_STAT_CDP:
-		dev_dbg(info->dev, "cdp cable is connecetd\n");
+		dev_dbg(info->dev, "cdp cable is connected\n");
 		notify_otg = true;
 		notify_charger = true;
 		cable = EXTCON_CHG_USB_CDP;
 		break;
 	case DET_STAT_DCP:
-		dev_dbg(info->dev, "dcp cable is connecetd\n");
+		dev_dbg(info->dev, "dcp cable is connected\n");
 		notify_charger = true;
 		cable = EXTCON_CHG_USB_DCP;
 		break;
@@ -226,7 +226,7 @@ notify_otg:
 	}
 
 	if (notify_charger)
-		extcon_set_cable_state_(info->edev, cable, vbus_attach);
+		extcon_set_state_sync(info->edev, cable, vbus_attach);
 
 	/* Clear the flags on disconnect event */
 	if (!vbus_attach)
diff --git a/drivers/extcon/extcon-gpio.c b/drivers/extcon/extcon-gpio.c
index d023789f0fda..ebed22f22d75 100644
--- a/drivers/extcon/extcon-gpio.c
+++ b/drivers/extcon/extcon-gpio.c
@@ -49,7 +49,8 @@ static void gpio_extcon_work(struct work_struct *work)
 	state = gpiod_get_value_cansleep(data->id_gpiod);
 	if (data->pdata->gpio_active_low)
 		state = !state;
-	extcon_set_state(data->edev, state);
+
+	extcon_set_state_sync(data->edev, data->pdata->extcon_id, state);
 }
 
 static irqreturn_t gpio_irq_handler(int irq, void *dev_id)
diff --git a/drivers/extcon/extcon-max14577.c b/drivers/extcon/extcon-max14577.c
index 852a7112f451..12e26c4e7763 100644
--- a/drivers/extcon/extcon-max14577.c
+++ b/drivers/extcon/extcon-max14577.c
@@ -3,7 +3,7 @@
  *
  * Copyright (C) 2013,2014 Samsung Electronics
  * Chanwoo Choi <cw00.choi@samsung.com>
- * Krzysztof Kozlowski <k.kozlowski@samsung.com>
+ * Krzysztof Kozlowski <krzk@kernel.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
@@ -357,7 +357,7 @@ static int max14577_muic_jig_handler(struct max14577_muic_info *info,
 	if (ret < 0)
 		return ret;
 
-	extcon_set_cable_state_(info->edev, EXTCON_JIG, attached);
+	extcon_set_state_sync(info->edev, EXTCON_JIG, attached);
 
 	return 0;
 }
@@ -454,24 +454,24 @@ static int max14577_muic_chg_handler(struct max14577_muic_info *info)
 		if (ret < 0)
 			return ret;
 
-		extcon_set_cable_state_(info->edev, EXTCON_USB, attached);
-		extcon_set_cable_state_(info->edev, EXTCON_CHG_USB_SDP,
+		extcon_set_state_sync(info->edev, EXTCON_USB, attached);
+		extcon_set_state_sync(info->edev, EXTCON_CHG_USB_SDP,
 					attached);
 		break;
 	case MAX14577_CHARGER_TYPE_DEDICATED_CHG:
-		extcon_set_cable_state_(info->edev, EXTCON_CHG_USB_DCP,
+		extcon_set_state_sync(info->edev, EXTCON_CHG_USB_DCP,
 					attached);
 		break;
 	case MAX14577_CHARGER_TYPE_DOWNSTREAM_PORT:
-		extcon_set_cable_state_(info->edev, EXTCON_CHG_USB_CDP,
+		extcon_set_state_sync(info->edev, EXTCON_CHG_USB_CDP,
 					attached);
 		break;
 	case MAX14577_CHARGER_TYPE_SPECIAL_500MA:
-		extcon_set_cable_state_(info->edev, EXTCON_CHG_USB_SLOW,
+		extcon_set_state_sync(info->edev, EXTCON_CHG_USB_SLOW,
 					attached);
 		break;
 	case MAX14577_CHARGER_TYPE_SPECIAL_1A:
-		extcon_set_cable_state_(info->edev, EXTCON_CHG_USB_FAST,
+		extcon_set_state_sync(info->edev, EXTCON_CHG_USB_FAST,
 					attached);
 		break;
 	case MAX14577_CHARGER_TYPE_NONE:
@@ -791,6 +791,6 @@ static struct platform_driver max14577_muic_driver = {
 module_platform_driver(max14577_muic_driver);
 
 MODULE_DESCRIPTION("Maxim 14577/77836 Extcon driver");
-MODULE_AUTHOR("Chanwoo Choi <cw00.choi@samsung.com>, Krzysztof Kozlowski <k.kozlowski@samsung.com>");
+MODULE_AUTHOR("Chanwoo Choi <cw00.choi@samsung.com>, Krzysztof Kozlowski <krzk@kernel.org>");
 MODULE_LICENSE("GPL");
 MODULE_ALIAS("platform:extcon-max14577");
diff --git a/drivers/extcon/extcon-max3355.c b/drivers/extcon/extcon-max3355.c
index c24abec5d06c..533e16a952b8 100644
--- a/drivers/extcon/extcon-max3355.c
+++ b/drivers/extcon/extcon-max3355.c
@@ -39,16 +39,16 @@ static irqreturn_t max3355_id_irq(int irq, void *dev_id)
 		 * As we don't have event for USB peripheral cable attached,
 		 * we simulate USB peripheral attach here.
 		 */
-		extcon_set_cable_state_(data->edev, EXTCON_USB_HOST, false);
-		extcon_set_cable_state_(data->edev, EXTCON_USB, true);
+		extcon_set_state_sync(data->edev, EXTCON_USB_HOST, false);
+		extcon_set_state_sync(data->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_(data->edev, EXTCON_USB, false);
-		extcon_set_cable_state_(data->edev, EXTCON_USB_HOST, true);
+		extcon_set_state_sync(data->edev, EXTCON_USB, false);
+		extcon_set_state_sync(data->edev, EXTCON_USB_HOST, true);
 	}
 
 	return IRQ_HANDLED;
diff --git a/drivers/extcon/extcon-max77693.c b/drivers/extcon/extcon-max77693.c
index f17cb76b567c..68dbcb814b2f 100644
--- a/drivers/extcon/extcon-max77693.c
+++ b/drivers/extcon/extcon-max77693.c
@@ -505,8 +505,8 @@ static int max77693_muic_dock_handler(struct max77693_muic_info *info,
 		if (ret < 0)
 			return ret;
 
-		extcon_set_cable_state_(info->edev, EXTCON_DOCK, attached);
-		extcon_set_cable_state_(info->edev, EXTCON_DISP_MHL, attached);
+		extcon_set_state_sync(info->edev, EXTCON_DOCK, attached);
+		extcon_set_state_sync(info->edev, EXTCON_DISP_MHL, attached);
 		goto out;
 	case MAX77693_MUIC_ADC_AUDIO_MODE_REMOTE:	/* Dock-Desk */
 		dock_id = EXTCON_DOCK;
@@ -514,8 +514,8 @@ static int max77693_muic_dock_handler(struct max77693_muic_info *info,
 	case MAX77693_MUIC_ADC_AV_CABLE_NOLOAD:		/* Dock-Audio */
 		dock_id = EXTCON_DOCK;
 		if (!attached) {
-			extcon_set_cable_state_(info->edev, EXTCON_USB, false);
-			extcon_set_cable_state_(info->edev, EXTCON_CHG_USB_SDP,
+			extcon_set_state_sync(info->edev, EXTCON_USB, false);
+			extcon_set_state_sync(info->edev, EXTCON_CHG_USB_SDP,
 						false);
 		}
 		break;
@@ -530,7 +530,7 @@ static int max77693_muic_dock_handler(struct max77693_muic_info *info,
 					attached);
 	if (ret < 0)
 		return ret;
-	extcon_set_cable_state_(info->edev, dock_id, attached);
+	extcon_set_state_sync(info->edev, dock_id, attached);
 
 out:
 	return 0;
@@ -596,7 +596,7 @@ static int max77693_muic_adc_ground_handler(struct max77693_muic_info *info)
 						attached);
 		if (ret < 0)
 			return ret;
-		extcon_set_cable_state_(info->edev, EXTCON_USB_HOST, attached);
+		extcon_set_state_sync(info->edev, EXTCON_USB_HOST, attached);
 		break;
 	case MAX77693_MUIC_GND_AV_CABLE_LOAD:
 		/* Audio Video Cable with load, PATH:AUDIO */
@@ -604,14 +604,14 @@ static int max77693_muic_adc_ground_handler(struct max77693_muic_info *info)
 						attached);
 		if (ret < 0)
 			return ret;
-		extcon_set_cable_state_(info->edev, EXTCON_USB, attached);
-		extcon_set_cable_state_(info->edev, EXTCON_CHG_USB_SDP,
+		extcon_set_state_sync(info->edev, EXTCON_USB, attached);
+		extcon_set_state_sync(info->edev, EXTCON_CHG_USB_SDP,
 					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, EXTCON_DISP_MHL, attached);
+		extcon_set_state_sync(info->edev, EXTCON_DISP_MHL, attached);
 		break;
 	default:
 		dev_err(info->dev, "failed to detect %s cable of gnd type\n",
@@ -653,7 +653,7 @@ static int max77693_muic_jig_handler(struct max77693_muic_info *info,
 	if (ret < 0)
 		return ret;
 
-	extcon_set_cable_state_(info->edev, EXTCON_JIG, attached);
+	extcon_set_state_sync(info->edev, EXTCON_JIG, attached);
 
 	return 0;
 }
@@ -807,10 +807,10 @@ 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, EXTCON_CHG_USB_DCP,
+			extcon_set_state_sync(info->edev, EXTCON_CHG_USB_DCP,
 						attached);
 			if (!cable_attached)
-				extcon_set_cable_state_(info->edev,
+				extcon_set_state_sync(info->edev,
 					EXTCON_DISP_MHL, cable_attached);
 			break;
 		}
@@ -834,13 +834,13 @@ 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, EXTCON_USB,
+			extcon_set_state_sync(info->edev, EXTCON_USB,
 						attached);
-			extcon_set_cable_state_(info->edev, EXTCON_CHG_USB_SDP,
+			extcon_set_state_sync(info->edev, EXTCON_CHG_USB_SDP,
 						attached);
 
 			if (!cable_attached)
-				extcon_set_cable_state_(info->edev, EXTCON_DOCK,
+				extcon_set_state_sync(info->edev, EXTCON_DOCK,
 							cable_attached);
 			break;
 		case MAX77693_MUIC_ADC_RESERVED_ACC_3:		/* Dock-Smart */
@@ -869,9 +869,9 @@ static int max77693_muic_chg_handler(struct max77693_muic_info *info)
 			if (ret < 0)
 				return ret;
 
-			extcon_set_cable_state_(info->edev, EXTCON_DOCK,
+			extcon_set_state_sync(info->edev, EXTCON_DOCK,
 						attached);
-			extcon_set_cable_state_(info->edev, EXTCON_DISP_MHL,
+			extcon_set_state_sync(info->edev, EXTCON_DISP_MHL,
 						attached);
 			break;
 		}
@@ -905,28 +905,28 @@ static int max77693_muic_chg_handler(struct max77693_muic_info *info)
 			if (ret < 0)
 				return ret;
 
-			extcon_set_cable_state_(info->edev, EXTCON_USB,
+			extcon_set_state_sync(info->edev, EXTCON_USB,
 						attached);
-			extcon_set_cable_state_(info->edev, EXTCON_CHG_USB_SDP,
+			extcon_set_state_sync(info->edev, EXTCON_CHG_USB_SDP,
 						attached);
 			break;
 		case MAX77693_CHARGER_TYPE_DEDICATED_CHG:
 			/* Only TA cable */
-			extcon_set_cable_state_(info->edev, EXTCON_CHG_USB_DCP,
+			extcon_set_state_sync(info->edev, EXTCON_CHG_USB_DCP,
 						attached);
 			break;
 		}
 		break;
 	case MAX77693_CHARGER_TYPE_DOWNSTREAM_PORT:
-		extcon_set_cable_state_(info->edev, EXTCON_CHG_USB_CDP,
+		extcon_set_state_sync(info->edev, EXTCON_CHG_USB_CDP,
 					attached);
 		break;
 	case MAX77693_CHARGER_TYPE_APPLE_500MA:
-		extcon_set_cable_state_(info->edev, EXTCON_CHG_USB_SLOW,
+		extcon_set_state_sync(info->edev, EXTCON_CHG_USB_SLOW,
 					attached);
 		break;
 	case MAX77693_CHARGER_TYPE_APPLE_1A_2A:
-		extcon_set_cable_state_(info->edev, EXTCON_CHG_USB_FAST,
+		extcon_set_state_sync(info->edev, EXTCON_CHG_USB_FAST,
 					attached);
 		break;
 	case MAX77693_CHARGER_TYPE_DEAD_BATTERY:
diff --git a/drivers/extcon/extcon-max77843.c b/drivers/extcon/extcon-max77843.c
index b188bd650efa..5d11fdf36e94 100644
--- a/drivers/extcon/extcon-max77843.c
+++ b/drivers/extcon/extcon-max77843.c
@@ -346,7 +346,7 @@ static int max77843_muic_adc_gnd_handler(struct max77843_muic_info *info)
 		if (ret < 0)
 			return ret;
 
-		extcon_set_cable_state_(info->edev, EXTCON_USB_HOST, attached);
+		extcon_set_state_sync(info->edev, EXTCON_USB_HOST, attached);
 		break;
 	case MAX77843_MUIC_GND_MHL_VB:
 	case MAX77843_MUIC_GND_MHL:
@@ -356,7 +356,7 @@ static int max77843_muic_adc_gnd_handler(struct max77843_muic_info *info)
 		if (ret < 0)
 			return ret;
 
-		extcon_set_cable_state_(info->edev, EXTCON_DISP_MHL, attached);
+		extcon_set_state_sync(info->edev, EXTCON_DISP_MHL, attached);
 		break;
 	default:
 		dev_err(info->dev, "failed to detect %s accessory(gnd:0x%x)\n",
@@ -392,7 +392,7 @@ static int max77843_muic_jig_handler(struct max77843_muic_info *info,
 	if (ret < 0)
 		return ret;
 
-	extcon_set_cable_state_(info->edev, EXTCON_JIG, attached);
+	extcon_set_state_sync(info->edev, EXTCON_JIG, attached);
 
 	return 0;
 }
@@ -486,8 +486,8 @@ static int max77843_muic_chg_handler(struct max77843_muic_info *info)
 		if (ret < 0)
 			return ret;
 
-		extcon_set_cable_state_(info->edev, EXTCON_USB, attached);
-		extcon_set_cable_state_(info->edev, EXTCON_CHG_USB_SDP,
+		extcon_set_state_sync(info->edev, EXTCON_USB, attached);
+		extcon_set_state_sync(info->edev, EXTCON_CHG_USB_SDP,
 					attached);
 		break;
 	case MAX77843_MUIC_CHG_DOWNSTREAM:
@@ -497,7 +497,7 @@ static int max77843_muic_chg_handler(struct max77843_muic_info *info)
 		if (ret < 0)
 			return ret;
 
-		extcon_set_cable_state_(info->edev, EXTCON_CHG_USB_CDP,
+		extcon_set_state_sync(info->edev, EXTCON_CHG_USB_CDP,
 					attached);
 		break;
 	case MAX77843_MUIC_CHG_DEDICATED:
@@ -507,7 +507,7 @@ static int max77843_muic_chg_handler(struct max77843_muic_info *info)
 		if (ret < 0)
 			return ret;
 
-		extcon_set_cable_state_(info->edev, EXTCON_CHG_USB_DCP,
+		extcon_set_state_sync(info->edev, EXTCON_CHG_USB_DCP,
 					attached);
 		break;
 	case MAX77843_MUIC_CHG_SPECIAL_500MA:
@@ -517,7 +517,7 @@ static int max77843_muic_chg_handler(struct max77843_muic_info *info)
 		if (ret < 0)
 			return ret;
 
-		extcon_set_cable_state_(info->edev, EXTCON_CHG_USB_SLOW,
+		extcon_set_state_sync(info->edev, EXTCON_CHG_USB_SLOW,
 					attached);
 		break;
 	case MAX77843_MUIC_CHG_SPECIAL_1A:
@@ -527,7 +527,7 @@ static int max77843_muic_chg_handler(struct max77843_muic_info *info)
 		if (ret < 0)
 			return ret;
 
-		extcon_set_cable_state_(info->edev, EXTCON_CHG_USB_FAST,
+		extcon_set_state_sync(info->edev, EXTCON_CHG_USB_FAST,
 					attached);
 		break;
 	case MAX77843_MUIC_CHG_GND:
@@ -536,10 +536,10 @@ 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, EXTCON_CHG_USB_DCP,
+			extcon_set_state_sync(info->edev, EXTCON_CHG_USB_DCP,
 						true);
 		else if (gnd_type == MAX77843_MUIC_GND_MHL)
-			extcon_set_cable_state_(info->edev, EXTCON_CHG_USB_DCP,
+			extcon_set_state_sync(info->edev, EXTCON_CHG_USB_DCP,
 						false);
 		break;
 	case MAX77843_MUIC_CHG_NONE:
diff --git a/drivers/extcon/extcon-max8997.c b/drivers/extcon/extcon-max8997.c
index 9a89320d09a8..4a0612fb9c07 100644
--- a/drivers/extcon/extcon-max8997.c
+++ b/drivers/extcon/extcon-max8997.c
@@ -331,11 +331,11 @@ static int max8997_muic_handle_usb(struct max8997_muic_info *info,
 
 	switch (usb_type) {
 	case MAX8997_USB_HOST:
-		extcon_set_cable_state_(info->edev, EXTCON_USB_HOST, attached);
+		extcon_set_state_sync(info->edev, EXTCON_USB_HOST, attached);
 		break;
 	case MAX8997_USB_DEVICE:
-		extcon_set_cable_state_(info->edev, EXTCON_USB, attached);
-		extcon_set_cable_state_(info->edev, EXTCON_CHG_USB_SDP,
+		extcon_set_state_sync(info->edev, EXTCON_USB, attached);
+		extcon_set_state_sync(info->edev, EXTCON_CHG_USB_SDP,
 					attached);
 		break;
 	default:
@@ -361,7 +361,7 @@ static int max8997_muic_handle_dock(struct max8997_muic_info *info,
 	switch (cable_type) {
 	case MAX8997_MUIC_ADC_AV_CABLE_NOLOAD:
 	case MAX8997_MUIC_ADC_FACTORY_MODE_UART_ON:
-		extcon_set_cable_state_(info->edev, EXTCON_DOCK, attached);
+		extcon_set_state_sync(info->edev, EXTCON_DOCK, attached);
 		break;
 	default:
 		dev_err(info->dev, "failed to detect %s dock device\n",
@@ -384,7 +384,7 @@ static int max8997_muic_handle_jig_uart(struct max8997_muic_info *info,
 		return ret;
 	}
 
-	extcon_set_cable_state_(info->edev, EXTCON_JIG, attached);
+	extcon_set_state_sync(info->edev, EXTCON_JIG, attached);
 
 	return 0;
 }
@@ -406,7 +406,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, EXTCON_DISP_MHL, attached);
+		extcon_set_state_sync(info->edev, EXTCON_DISP_MHL, attached);
 		break;
 	case MAX8997_MUIC_ADC_FACTORY_MODE_USB_OFF:
 	case MAX8997_MUIC_ADC_FACTORY_MODE_USB_ON:
@@ -489,19 +489,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, EXTCON_CHG_USB_CDP,
+		extcon_set_state_sync(info->edev, EXTCON_CHG_USB_CDP,
 					attached);
 		break;
 	case MAX8997_CHARGER_TYPE_DEDICATED_CHG:
-		extcon_set_cable_state_(info->edev, EXTCON_CHG_USB_DCP,
+		extcon_set_state_sync(info->edev, EXTCON_CHG_USB_DCP,
 					attached);
 		break;
 	case MAX8997_CHARGER_TYPE_500MA:
-		extcon_set_cable_state_(info->edev, EXTCON_CHG_USB_SLOW,
+		extcon_set_state_sync(info->edev, EXTCON_CHG_USB_SLOW,
 					attached);
 		break;
 	case MAX8997_CHARGER_TYPE_1A:
-		extcon_set_cable_state_(info->edev, EXTCON_CHG_USB_FAST,
+		extcon_set_state_sync(info->edev, EXTCON_CHG_USB_FAST,
 					attached);
 		break;
 	default:
diff --git a/drivers/extcon/extcon-palmas.c b/drivers/extcon/extcon-palmas.c
index caff46c0e214..634ba70782de 100644
--- a/drivers/extcon/extcon-palmas.c
+++ b/drivers/extcon/extcon-palmas.c
@@ -61,7 +61,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_(edev, EXTCON_USB, true);
+			extcon_set_state_sync(edev, EXTCON_USB, true);
 			dev_info(palmas_usb->dev, "USB cable is attached\n");
 		} else {
 			dev_dbg(palmas_usb->dev,
@@ -70,7 +70,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_(edev, EXTCON_USB, false);
+			extcon_set_state_sync(edev, EXTCON_USB, false);
 			dev_info(palmas_usb->dev, "USB cable is detached\n");
 		} else {
 			dev_dbg(palmas_usb->dev,
@@ -98,7 +98,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_(edev, EXTCON_USB_HOST, true);
+		extcon_set_state_sync(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)) {
@@ -106,17 +106,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_(edev, EXTCON_USB_HOST, false);
+		extcon_set_state_sync(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_(edev, EXTCON_USB_HOST, false);
+		extcon_set_state_sync(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_(edev, EXTCON_USB_HOST, true);
+		extcon_set_state_sync(edev, EXTCON_USB_HOST, true);
 		dev_info(palmas_usb->dev, " USB-HOST cable is attached\n");
 	}
 
@@ -137,10 +137,10 @@ static void palmas_gpio_id_detect(struct work_struct *work)
 	id = gpiod_get_value_cansleep(palmas_usb->id_gpiod);
 
 	if (id) {
-		extcon_set_cable_state_(edev, EXTCON_USB_HOST, false);
+		extcon_set_state_sync(edev, EXTCON_USB_HOST, false);
 		dev_info(palmas_usb->dev, "USB-HOST cable is detached\n");
 	} else {
-		extcon_set_cable_state_(edev, EXTCON_USB_HOST, true);
+		extcon_set_state_sync(edev, EXTCON_USB_HOST, true);
 		dev_info(palmas_usb->dev, "USB-HOST cable is attached\n");
 	}
 }
diff --git a/drivers/extcon/extcon-qcom-spmi-misc.c b/drivers/extcon/extcon-qcom-spmi-misc.c
new file mode 100644
index 000000000000..ca957a5f4291
--- /dev/null
+++ b/drivers/extcon/extcon-qcom-spmi-misc.c
@@ -0,0 +1,170 @@
+/**
+ * extcon-qcom-spmi-misc.c - Qualcomm USB extcon driver to support USB ID
+ *				detection based on extcon-usb-gpio.c.
+ *
+ * Copyright (C) 2016 Linaro, Ltd.
+ * Stephen Boyd <stephen.boyd@linaro.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/extcon.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/workqueue.h>
+
+#define USB_ID_DEBOUNCE_MS	5	/* ms */
+
+struct qcom_usb_extcon_info {
+	struct extcon_dev *edev;
+	int irq;
+	struct delayed_work wq_detcable;
+	unsigned long debounce_jiffies;
+};
+
+static const unsigned int qcom_usb_extcon_cable[] = {
+	EXTCON_USB_HOST,
+	EXTCON_NONE,
+};
+
+static void qcom_usb_extcon_detect_cable(struct work_struct *work)
+{
+	bool id;
+	int ret;
+	struct qcom_usb_extcon_info *info = container_of(to_delayed_work(work),
+						    struct qcom_usb_extcon_info,
+						    wq_detcable);
+
+	/* check ID and update cable state */
+	ret = irq_get_irqchip_state(info->irq, IRQCHIP_STATE_LINE_LEVEL, &id);
+	if (ret)
+		return;
+
+	extcon_set_state(info->edev, EXTCON_USB_HOST, !id);
+}
+
+static irqreturn_t qcom_usb_irq_handler(int irq, void *dev_id)
+{
+	struct qcom_usb_extcon_info *info = dev_id;
+
+	queue_delayed_work(system_power_efficient_wq, &info->wq_detcable,
+			   info->debounce_jiffies);
+
+	return IRQ_HANDLED;
+}
+
+static int qcom_usb_extcon_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct qcom_usb_extcon_info *info;
+	int ret;
+
+	info = devm_kzalloc(dev, sizeof(*info), GFP_KERNEL);
+	if (!info)
+		return -ENOMEM;
+
+	info->edev = devm_extcon_dev_allocate(dev, qcom_usb_extcon_cable);
+	if (IS_ERR(info->edev)) {
+		dev_err(dev, "failed to allocate extcon device\n");
+		return -ENOMEM;
+	}
+
+	ret = devm_extcon_dev_register(dev, info->edev);
+	if (ret < 0) {
+		dev_err(dev, "failed to register extcon device\n");
+		return ret;
+	}
+
+	info->debounce_jiffies = msecs_to_jiffies(USB_ID_DEBOUNCE_MS);
+	INIT_DELAYED_WORK(&info->wq_detcable, qcom_usb_extcon_detect_cable);
+
+	info->irq = platform_get_irq_byname(pdev, "usb_id");
+	if (info->irq < 0)
+		return info->irq;
+
+	ret = devm_request_threaded_irq(dev, info->irq, NULL,
+					qcom_usb_irq_handler,
+					IRQF_TRIGGER_RISING |
+					IRQF_TRIGGER_FALLING | IRQF_ONESHOT,
+					pdev->name, info);
+	if (ret < 0) {
+		dev_err(dev, "failed to request handler for ID IRQ\n");
+		return ret;
+	}
+
+	platform_set_drvdata(pdev, info);
+	device_init_wakeup(dev, 1);
+
+	/* Perform initial detection */
+	qcom_usb_extcon_detect_cable(&info->wq_detcable.work);
+
+	return 0;
+}
+
+static int qcom_usb_extcon_remove(struct platform_device *pdev)
+{
+	struct qcom_usb_extcon_info *info = platform_get_drvdata(pdev);
+
+	cancel_delayed_work_sync(&info->wq_detcable);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int qcom_usb_extcon_suspend(struct device *dev)
+{
+	struct qcom_usb_extcon_info *info = dev_get_drvdata(dev);
+	int ret = 0;
+
+	if (device_may_wakeup(dev))
+		ret = enable_irq_wake(info->irq);
+
+	return ret;
+}
+
+static int qcom_usb_extcon_resume(struct device *dev)
+{
+	struct qcom_usb_extcon_info *info = dev_get_drvdata(dev);
+	int ret = 0;
+
+	if (device_may_wakeup(dev))
+		ret = disable_irq_wake(info->irq);
+
+	return ret;
+}
+#endif
+
+static SIMPLE_DEV_PM_OPS(qcom_usb_extcon_pm_ops,
+			 qcom_usb_extcon_suspend, qcom_usb_extcon_resume);
+
+static const struct of_device_id qcom_usb_extcon_dt_match[] = {
+	{ .compatible = "qcom,pm8941-misc", },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, qcom_usb_extcon_dt_match);
+
+static struct platform_driver qcom_usb_extcon_driver = {
+	.probe		= qcom_usb_extcon_probe,
+	.remove		= qcom_usb_extcon_remove,
+	.driver		= {
+		.name	= "extcon-pm8941-misc",
+		.pm	= &qcom_usb_extcon_pm_ops,
+		.of_match_table = qcom_usb_extcon_dt_match,
+	},
+};
+module_platform_driver(qcom_usb_extcon_driver);
+
+MODULE_DESCRIPTION("QCOM USB ID extcon driver");
+MODULE_AUTHOR("Stephen Boyd <stephen.boyd@linaro.org>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/extcon/extcon-rt8973a.c b/drivers/extcon/extcon-rt8973a.c
index 97e074d70eca..174c388739ea 100644
--- a/drivers/extcon/extcon-rt8973a.c
+++ b/drivers/extcon/extcon-rt8973a.c
@@ -398,9 +398,9 @@ 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, id, attached);
+	extcon_set_state_sync(info->edev, id, attached);
 	if (id == EXTCON_USB)
-		extcon_set_cable_state_(info->edev, EXTCON_CHG_USB_SDP,
+		extcon_set_state_sync(info->edev, EXTCON_CHG_USB_SDP,
 					attached);
 
 	return 0;
diff --git a/drivers/extcon/extcon-sm5502.c b/drivers/extcon/extcon-sm5502.c
index df769a17e736..b22325688503 100644
--- a/drivers/extcon/extcon-sm5502.c
+++ b/drivers/extcon/extcon-sm5502.c
@@ -411,9 +411,9 @@ 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, id, attached);
+	extcon_set_state_sync(info->edev, id, attached);
 	if (id == EXTCON_USB)
-		extcon_set_cable_state_(info->edev, EXTCON_CHG_USB_SDP,
+		extcon_set_state_sync(info->edev, EXTCON_CHG_USB_SDP,
 					attached);
 
 	return 0;
diff --git a/drivers/extcon/extcon-usb-gpio.c b/drivers/extcon/extcon-usb-gpio.c
index 2512660dc4b9..a27d350f69e3 100644
--- a/drivers/extcon/extcon-usb-gpio.c
+++ b/drivers/extcon/extcon-usb-gpio.c
@@ -63,16 +63,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, EXTCON_USB_HOST, false);
-		extcon_set_cable_state_(info->edev, EXTCON_USB, true);
+		extcon_set_state_sync(info->edev, EXTCON_USB_HOST, false);
+		extcon_set_state_sync(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, EXTCON_USB, false);
-		extcon_set_cable_state_(info->edev, EXTCON_USB_HOST, true);
+		extcon_set_state_sync(info->edev, EXTCON_USB, false);
+		extcon_set_state_sync(info->edev, EXTCON_USB_HOST, true);
 	}
 }
 
diff --git a/drivers/extcon/extcon.c b/drivers/extcon/extcon.c
index 8682efc0f57b..78298460d168 100644
--- a/drivers/extcon/extcon.c
+++ b/drivers/extcon/extcon.c
@@ -38,43 +38,159 @@
 #define SUPPORTED_CABLE_MAX	32
 #define CABLE_NAME_MAX		30
 
-static const char *extcon_name[] =  {
-	[EXTCON_NONE]			= "NONE",
+struct __extcon_info {
+	unsigned int type;
+	unsigned int id;
+	const char *name;
+
+} extcon_info[] = {
+	[EXTCON_NONE] = {
+		.type = EXTCON_TYPE_MISC,
+		.id = EXTCON_NONE,
+		.name = "NONE",
+	},
 
 	/* USB external connector */
-	[EXTCON_USB]			= "USB",
-	[EXTCON_USB_HOST]		= "USB-HOST",
+	[EXTCON_USB] = {
+		.type = EXTCON_TYPE_USB,
+		.id = EXTCON_USB,
+		.name = "USB",
+	},
+	[EXTCON_USB_HOST] = {
+		.type = EXTCON_TYPE_USB,
+		.id = EXTCON_USB_HOST,
+		.name = "USB_HOST",
+	},
 
 	/* Charging external connector */
-	[EXTCON_CHG_USB_SDP]		= "SDP",
-	[EXTCON_CHG_USB_DCP]		= "DCP",
-	[EXTCON_CHG_USB_CDP]		= "CDP",
-	[EXTCON_CHG_USB_ACA]		= "ACA",
-	[EXTCON_CHG_USB_FAST]		= "FAST-CHARGER",
-	[EXTCON_CHG_USB_SLOW]		= "SLOW-CHARGER",
+	[EXTCON_CHG_USB_SDP] = {
+		.type = EXTCON_TYPE_CHG | EXTCON_TYPE_USB,
+		.id = EXTCON_CHG_USB_SDP,
+		.name = "SDP",
+	},
+	[EXTCON_CHG_USB_DCP] = {
+		.type = EXTCON_TYPE_CHG | EXTCON_TYPE_USB,
+		.id = EXTCON_CHG_USB_DCP,
+		.name = "DCP",
+	},
+	[EXTCON_CHG_USB_CDP] = {
+		.type = EXTCON_TYPE_CHG | EXTCON_TYPE_USB,
+		.id = EXTCON_CHG_USB_CDP,
+		.name = "CDP",
+	},
+	[EXTCON_CHG_USB_ACA] = {
+		.type = EXTCON_TYPE_CHG | EXTCON_TYPE_USB,
+		.id = EXTCON_CHG_USB_ACA,
+		.name = "ACA",
+	},
+	[EXTCON_CHG_USB_FAST] = {
+		.type = EXTCON_TYPE_CHG | EXTCON_TYPE_USB,
+		.id = EXTCON_CHG_USB_FAST,
+		.name = "FAST-CHARGER",
+	},
+	[EXTCON_CHG_USB_SLOW] = {
+		.type = EXTCON_TYPE_CHG | EXTCON_TYPE_USB,
+		.id = EXTCON_CHG_USB_SLOW,
+		.name = "SLOW-CHARGER",
+	},
+	[EXTCON_CHG_WPT] = {
+		.type = EXTCON_TYPE_CHG,
+		.id = EXTCON_CHG_WPT,
+		.name = "WPT",
+	},
 
 	/* Jack external connector */
-	[EXTCON_JACK_MICROPHONE]	= "MICROPHONE",
-	[EXTCON_JACK_HEADPHONE]		= "HEADPHONE",
-	[EXTCON_JACK_LINE_IN]		= "LINE-IN",
-	[EXTCON_JACK_LINE_OUT]		= "LINE-OUT",
-	[EXTCON_JACK_VIDEO_IN]		= "VIDEO-IN",
-	[EXTCON_JACK_VIDEO_OUT]		= "VIDEO-OUT",
-	[EXTCON_JACK_SPDIF_IN]		= "SPDIF-IN",
-	[EXTCON_JACK_SPDIF_OUT]		= "SPDIF-OUT",
+	[EXTCON_JACK_MICROPHONE] = {
+		.type = EXTCON_TYPE_JACK,
+		.id = EXTCON_JACK_MICROPHONE,
+		.name = "MICROPHONE",
+	},
+	[EXTCON_JACK_HEADPHONE] = {
+		.type = EXTCON_TYPE_JACK,
+		.id = EXTCON_JACK_HEADPHONE,
+		.name = "HEADPHONE",
+	},
+	[EXTCON_JACK_LINE_IN] = {
+		.type = EXTCON_TYPE_JACK,
+		.id = EXTCON_JACK_LINE_IN,
+		.name = "LINE-IN",
+	},
+	[EXTCON_JACK_LINE_OUT] = {
+		.type = EXTCON_TYPE_JACK,
+		.id = EXTCON_JACK_LINE_OUT,
+		.name = "LINE-OUT",
+	},
+	[EXTCON_JACK_VIDEO_IN] = {
+		.type = EXTCON_TYPE_JACK,
+		.id = EXTCON_JACK_VIDEO_IN,
+		.name = "VIDEO-IN",
+	},
+	[EXTCON_JACK_VIDEO_OUT] = {
+		.type = EXTCON_TYPE_JACK,
+		.id = EXTCON_JACK_VIDEO_OUT,
+		.name = "VIDEO-OUT",
+	},
+	[EXTCON_JACK_SPDIF_IN] = {
+		.type = EXTCON_TYPE_JACK,
+		.id = EXTCON_JACK_SPDIF_IN,
+		.name = "SPDIF-IN",
+	},
+	[EXTCON_JACK_SPDIF_OUT] = {
+		.type = EXTCON_TYPE_JACK,
+		.id = EXTCON_JACK_SPDIF_OUT,
+		.name = "SPDIF-OUT",
+	},
 
 	/* Display external connector */
-	[EXTCON_DISP_HDMI]		= "HDMI",
-	[EXTCON_DISP_MHL]		= "MHL",
-	[EXTCON_DISP_DVI]		= "DVI",
-	[EXTCON_DISP_VGA]		= "VGA",
+	[EXTCON_DISP_HDMI] = {
+		.type = EXTCON_TYPE_DISP,
+		.id = EXTCON_DISP_HDMI,
+		.name = "HDMI",
+	},
+	[EXTCON_DISP_MHL] = {
+		.type = EXTCON_TYPE_DISP,
+		.id = EXTCON_DISP_MHL,
+		.name = "MHL",
+	},
+	[EXTCON_DISP_DVI] = {
+		.type = EXTCON_TYPE_DISP,
+		.id = EXTCON_DISP_DVI,
+		.name = "DVI",
+	},
+	[EXTCON_DISP_VGA] = {
+		.type = EXTCON_TYPE_DISP,
+		.id = EXTCON_DISP_VGA,
+		.name = "VGA",
+	},
+	[EXTCON_DISP_DP] = {
+		.type = EXTCON_TYPE_DISP | EXTCON_TYPE_USB,
+		.id = EXTCON_DISP_DP,
+		.name = "DP",
+	},
+	[EXTCON_DISP_HMD] = {
+		.type = EXTCON_TYPE_DISP | EXTCON_TYPE_USB,
+		.id = EXTCON_DISP_HMD,
+		.name = "HMD",
+	},
 
 	/* Miscellaneous external connector */
-	[EXTCON_DOCK]			= "DOCK",
-	[EXTCON_JIG]			= "JIG",
-	[EXTCON_MECHANICAL]		= "MECHANICAL",
-
-	NULL,
+	[EXTCON_DOCK] = {
+		.type = EXTCON_TYPE_MISC,
+		.id = EXTCON_DOCK,
+		.name = "DOCK",
+	},
+	[EXTCON_JIG] = {
+		.type = EXTCON_TYPE_MISC,
+		.id = EXTCON_JIG,
+		.name = "JIG",
+	},
+	[EXTCON_MECHANICAL] = {
+		.type = EXTCON_TYPE_MISC,
+		.id = EXTCON_MECHANICAL,
+		.name = "MECHANICAL",
+	},
+
+	{ /* sentinel */ }
 };
 
 /**
@@ -95,6 +211,16 @@ struct extcon_cable {
 	struct device_attribute attr_state;
 
 	struct attribute *attrs[3]; /* to be fed to attr_g.attrs */
+
+	union extcon_property_value usb_propval[EXTCON_PROP_USB_CNT];
+	union extcon_property_value chg_propval[EXTCON_PROP_CHG_CNT];
+	union extcon_property_value jack_propval[EXTCON_PROP_JACK_CNT];
+	union extcon_property_value disp_propval[EXTCON_PROP_DISP_CNT];
+
+	unsigned long usb_bits[BITS_TO_LONGS(EXTCON_PROP_USB_CNT)];
+	unsigned long chg_bits[BITS_TO_LONGS(EXTCON_PROP_CHG_CNT)];
+	unsigned long jack_bits[BITS_TO_LONGS(EXTCON_PROP_JACK_CNT)];
+	unsigned long disp_bits[BITS_TO_LONGS(EXTCON_PROP_DISP_CNT)];
 };
 
 static struct class *extcon_class;
@@ -147,14 +273,93 @@ static int find_cable_index_by_id(struct extcon_dev *edev, const unsigned int id
 	return -EINVAL;
 }
 
-static bool is_extcon_changed(u32 prev, u32 new, int idx, bool *attached)
+static int get_extcon_type(unsigned int prop)
 {
-	if (((prev >> idx) & 0x1) != ((new >> idx) & 0x1)) {
-		*attached = ((new >> idx) & 0x1) ? true : false;
-		return true;
+	switch (prop) {
+	case EXTCON_PROP_USB_MIN ... EXTCON_PROP_USB_MAX:
+		return EXTCON_TYPE_USB;
+	case EXTCON_PROP_CHG_MIN ... EXTCON_PROP_CHG_MAX:
+		return EXTCON_TYPE_CHG;
+	case EXTCON_PROP_JACK_MIN ... EXTCON_PROP_JACK_MAX:
+		return EXTCON_TYPE_JACK;
+	case EXTCON_PROP_DISP_MIN ... EXTCON_PROP_DISP_MAX:
+		return EXTCON_TYPE_DISP;
+	default:
+		return -EINVAL;
 	}
+}
+
+static bool is_extcon_attached(struct extcon_dev *edev, unsigned int index)
+{
+	return !!(edev->state & BIT(index));
+}
+
+static bool is_extcon_changed(struct extcon_dev *edev, int index,
+				bool new_state)
+{
+	int state = !!(edev->state & BIT(index));
+	return (state != new_state);
+}
+
+static bool is_extcon_property_supported(unsigned int id, unsigned int prop)
+{
+	int type;
+
+	/* Check whether the property is supported or not. */
+	type = get_extcon_type(prop);
+	if (type < 0)
+		return false;
 
-	return false;
+	/* Check whether a specific extcon id supports the property or not. */
+	return !!(extcon_info[id].type & type);
+}
+
+static int is_extcon_property_capability(struct extcon_dev *edev,
+				unsigned int id, int index,unsigned int prop)
+{
+	struct extcon_cable *cable;
+	int type, ret;
+
+	/* Check whether the property is supported or not. */
+	type = get_extcon_type(prop);
+	if (type < 0)
+		return type;
+
+	cable = &edev->cables[index];
+
+	switch (type) {
+	case EXTCON_TYPE_USB:
+		ret = test_bit(prop - EXTCON_PROP_USB_MIN, cable->usb_bits);
+		break;
+	case EXTCON_TYPE_CHG:
+		ret = test_bit(prop - EXTCON_PROP_CHG_MIN, cable->chg_bits);
+		break;
+	case EXTCON_TYPE_JACK:
+		ret = test_bit(prop - EXTCON_PROP_JACK_MIN, cable->jack_bits);
+		break;
+	case EXTCON_TYPE_DISP:
+		ret = test_bit(prop - EXTCON_PROP_DISP_MIN, cable->disp_bits);
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+static void init_property(struct extcon_dev *edev, unsigned int id, int index)
+{
+	unsigned int type = extcon_info[id].type;
+	struct extcon_cable *cable = &edev->cables[index];
+
+	if (EXTCON_TYPE_USB & type)
+		memset(cable->usb_propval, 0, sizeof(cable->usb_propval));
+	if (EXTCON_TYPE_CHG & type)
+		memset(cable->chg_propval, 0, sizeof(cable->chg_propval));
+	if (EXTCON_TYPE_JACK & type)
+		memset(cable->jack_propval, 0, sizeof(cable->jack_propval));
+	if (EXTCON_TYPE_DISP & type)
+		memset(cable->disp_propval, 0, sizeof(cable->disp_propval));
 }
 
 static ssize_t state_show(struct device *dev, struct device_attribute *attr,
@@ -168,32 +373,13 @@ static ssize_t state_show(struct device *dev, struct device_attribute *attr,
 
 	for (i = 0; i < edev->max_supported; i++) {
 		count += sprintf(buf + count, "%s=%d\n",
-				extcon_name[edev->supported_cable[i]],
+				extcon_info[edev->supported_cable[i]].name,
 				 !!(edev->state & (1 << i)));
 	}
 
 	return count;
 }
-
-static ssize_t state_store(struct device *dev, struct device_attribute *attr,
-			   const char *buf, size_t count)
-{
-	u32 state;
-	ssize_t ret = 0;
-	struct extcon_dev *edev = dev_get_drvdata(dev);
-
-	ret = sscanf(buf, "0x%x", &state);
-	if (ret == 0)
-		ret = -EINVAL;
-	else
-		ret = extcon_set_state(edev, state);
-
-	if (ret < 0)
-		return ret;
-
-	return count;
-}
-static DEVICE_ATTR_RW(state);
+static DEVICE_ATTR_RO(state);
 
 static ssize_t name_show(struct device *dev, struct device_attribute *attr,
 		char *buf)
@@ -212,7 +398,7 @@ static ssize_t cable_name_show(struct device *dev,
 	int i = cable->cable_index;
 
 	return sprintf(buf, "%s\n",
-			extcon_name[cable->edev->supported_cable[i]]);
+			extcon_info[cable->edev->supported_cable[i]].name);
 }
 
 static ssize_t cable_state_show(struct device *dev,
@@ -224,26 +410,17 @@ static ssize_t cable_state_show(struct device *dev,
 	int i = cable->cable_index;
 
 	return sprintf(buf, "%d\n",
-		       extcon_get_cable_state_(cable->edev,
-					       cable->edev->supported_cable[i]));
+		extcon_get_state(cable->edev, cable->edev->supported_cable[i]));
 }
 
 /**
- * extcon_update_state() - Update the cable attach states of the extcon device
- *			   only for the masked bits.
- * @edev:	the extcon device
- * @mask:	the bit mask to designate updated bits.
- * @state:	new cable attach status for @edev
- *
- * Changing the state sends uevent with environment variable containing
- * the name of extcon device (envp[0]) and the state output (envp[1]).
- * Tizen uses this format for extcon device to get events from ports.
- * Android uses this format as well.
+ * extcon_sync()	- Synchronize the states for both the attached/detached
+ * @edev:		the extcon device that has the cable.
  *
- * Note that the notifier provides which bits are changed in the state
- * variable with the val parameter (second) to the callback.
+ * This function send a notification to synchronize the all states of a
+ * specific external connector
  */
-int extcon_update_state(struct extcon_dev *edev, u32 mask, u32 state)
+int extcon_sync(struct extcon_dev *edev, unsigned int id)
 {
 	char name_buf[120];
 	char state_buf[120];
@@ -252,100 +429,102 @@ int extcon_update_state(struct extcon_dev *edev, u32 mask, u32 state)
 	int env_offset = 0;
 	int length;
 	int index;
+	int state;
 	unsigned long flags;
-	bool attached;
 
 	if (!edev)
 		return -EINVAL;
 
-	spin_lock_irqsave(&edev->lock, flags);
+	index = find_cable_index_by_id(edev, id);
+	if (index < 0)
+		return index;
 
-	if (edev->state != ((edev->state & ~mask) | (state & mask))) {
-		u32 old_state;
+	spin_lock_irqsave(&edev->lock, flags);
 
-		if (check_mutually_exclusive(edev, (edev->state & ~mask) |
-						   (state & mask))) {
-			spin_unlock_irqrestore(&edev->lock, flags);
-			return -EPERM;
-		}
+	state = !!(edev->state & BIT(index));
+	raw_notifier_call_chain(&edev->nh[index], state, edev);
 
-		old_state = edev->state;
-		edev->state &= ~mask;
-		edev->state |= state & mask;
+	/* This could be in interrupt handler */
+	prop_buf = (char *)get_zeroed_page(GFP_ATOMIC);
+	if (!prop_buf) {
+		/* Unlock early before uevent */
+		spin_unlock_irqrestore(&edev->lock, flags);
 
-		for (index = 0; index < edev->max_supported; index++) {
-			if (is_extcon_changed(old_state, edev->state, index,
-					      &attached))
-				raw_notifier_call_chain(&edev->nh[index],
-							attached, edev);
-		}
+		dev_err(&edev->dev, "out of memory in extcon_set_state\n");
+		kobject_uevent(&edev->dev.kobj, KOBJ_CHANGE);
 
-		/* This could be in interrupt handler */
-		prop_buf = (char *)get_zeroed_page(GFP_ATOMIC);
-		if (prop_buf) {
-			length = name_show(&edev->dev, NULL, prop_buf);
-			if (length > 0) {
-				if (prop_buf[length - 1] == '\n')
-					prop_buf[length - 1] = 0;
-				snprintf(name_buf, sizeof(name_buf),
-					"NAME=%s", prop_buf);
-				envp[env_offset++] = name_buf;
-			}
-			length = state_show(&edev->dev, NULL, prop_buf);
-			if (length > 0) {
-				if (prop_buf[length - 1] == '\n')
-					prop_buf[length - 1] = 0;
-				snprintf(state_buf, sizeof(state_buf),
-					"STATE=%s", prop_buf);
-				envp[env_offset++] = state_buf;
-			}
-			envp[env_offset] = NULL;
-			/* Unlock early before uevent */
-			spin_unlock_irqrestore(&edev->lock, flags);
+		return 0;
+	}
 
-			kobject_uevent_env(&edev->dev.kobj, KOBJ_CHANGE, envp);
-			free_page((unsigned long)prop_buf);
-		} else {
-			/* Unlock early before uevent */
-			spin_unlock_irqrestore(&edev->lock, flags);
+	length = name_show(&edev->dev, NULL, prop_buf);
+	if (length > 0) {
+		if (prop_buf[length - 1] == '\n')
+			prop_buf[length - 1] = 0;
+		snprintf(name_buf, sizeof(name_buf), "NAME=%s", prop_buf);
+		envp[env_offset++] = name_buf;
+	}
 
-			dev_err(&edev->dev, "out of memory in extcon_set_state\n");
-			kobject_uevent(&edev->dev.kobj, KOBJ_CHANGE);
-		}
-	} else {
-		/* No changes */
-		spin_unlock_irqrestore(&edev->lock, flags);
+	length = state_show(&edev->dev, NULL, prop_buf);
+	if (length > 0) {
+		if (prop_buf[length - 1] == '\n')
+			prop_buf[length - 1] = 0;
+		snprintf(state_buf, sizeof(state_buf), "STATE=%s", prop_buf);
+		envp[env_offset++] = state_buf;
 	}
+	envp[env_offset] = NULL;
+
+	/* Unlock early before uevent */
+	spin_unlock_irqrestore(&edev->lock, flags);
+	kobject_uevent_env(&edev->dev.kobj, KOBJ_CHANGE, envp);
+	free_page((unsigned long)prop_buf);
 
 	return 0;
 }
-EXPORT_SYMBOL_GPL(extcon_update_state);
+EXPORT_SYMBOL_GPL(extcon_sync);
 
 /**
- * extcon_set_state() - Set the cable attach states of the extcon device.
- * @edev:	the extcon device
- * @state:	new cable attach status for @edev
- *
- * Note that notifier provides which bits are changed in the state
- * variable with the val parameter (second) to the callback.
+ * extcon_get_state() - Get the state of a external connector.
+ * @edev:	the extcon device that has the cable.
+ * @id:		the unique id of each external connector in extcon enumeration.
  */
-int extcon_set_state(struct extcon_dev *edev, u32 state)
+int extcon_get_state(struct extcon_dev *edev, const unsigned int id)
 {
+	int index, state;
+	unsigned long flags;
+
 	if (!edev)
 		return -EINVAL;
 
-	return extcon_update_state(edev, 0xffffffff, state);
+	index = find_cable_index_by_id(edev, id);
+	if (index < 0)
+		return index;
+
+	spin_lock_irqsave(&edev->lock, flags);
+	state = is_extcon_attached(edev, index);
+	spin_unlock_irqrestore(&edev->lock, flags);
+
+	return state;
 }
-EXPORT_SYMBOL_GPL(extcon_set_state);
+EXPORT_SYMBOL_GPL(extcon_get_state);
 
 /**
- * extcon_get_cable_state_() - Get the status of a specific cable.
- * @edev:	the extcon device that has the cable.
- * @id:		the unique id of each external connector in extcon enumeration.
+ * extcon_set_state() - Set the state of a external connector.
+ *			without a notification.
+ * @edev:		the extcon device that has the cable.
+ * @id:			the unique id of each external connector
+ *			in extcon enumeration.
+ * @state:		the new cable status. The default semantics is
+ *			true: attached / false: detached.
+ *
+ * This function only set the state of a external connector without
+ * a notification. To synchronize the data of a external connector,
+ * use extcon_set_state_sync() and extcon_sync().
  */
-int extcon_get_cable_state_(struct extcon_dev *edev, const unsigned int id)
+int extcon_set_state(struct extcon_dev *edev, unsigned int id,
+				bool cable_state)
 {
-	int index;
+	unsigned long flags;
+	int index, ret = 0;
 
 	if (!edev)
 		return -EINVAL;
@@ -354,41 +533,338 @@ int extcon_get_cable_state_(struct extcon_dev *edev, const unsigned int id)
 	if (index < 0)
 		return index;
 
-	if (edev->max_supported && edev->max_supported <= index)
-		return -EINVAL;
+	spin_lock_irqsave(&edev->lock, flags);
+
+	/* Check whether the external connector's state is changed. */
+	if (!is_extcon_changed(edev, index, cable_state))
+		goto out;
+
+	if (check_mutually_exclusive(edev,
+		(edev->state & ~BIT(index)) | (cable_state & BIT(index)))) {
+		ret = -EPERM;
+		goto out;
+	}
+
+	/*
+	 * Initialize the value of extcon property before setting
+	 * the detached state for an external connector.
+	 */
+	if (!cable_state)
+		init_property(edev, id, index);
+
+	/* Update the state for a external connector. */
+	if (cable_state)
+		edev->state |= BIT(index);
+	else
+		edev->state &= ~(BIT(index));
+out:
+	spin_unlock_irqrestore(&edev->lock, flags);
 
-	return !!(edev->state & (1 << index));
+	return ret;
 }
-EXPORT_SYMBOL_GPL(extcon_get_cable_state_);
+EXPORT_SYMBOL_GPL(extcon_set_state);
 
 /**
- * extcon_set_cable_state_() - Set the status of a specific cable.
+ * extcon_set_state_sync() - Set the state of a external connector
+ *			with a notification.
  * @edev:		the extcon device that has the cable.
  * @id:			the unique id of each external connector
  *			in extcon enumeration.
  * @state:		the new cable status. The default semantics is
  *			true: attached / false: detached.
+ *
+ * This function set the state of external connector and synchronize the data
+ * by usning a notification.
  */
-int extcon_set_cable_state_(struct extcon_dev *edev, unsigned int id,
+int extcon_set_state_sync(struct extcon_dev *edev, unsigned int id,
 				bool cable_state)
 {
-	u32 state;
+	int ret, index;
+	unsigned long flags;
+
+	index = find_cable_index_by_id(edev, id);
+	if (index < 0)
+		return index;
+
+	/* Check whether the external connector's state is changed. */
+	spin_lock_irqsave(&edev->lock, flags);
+	ret = is_extcon_changed(edev, index, cable_state);
+	spin_unlock_irqrestore(&edev->lock, flags);
+	if (!ret)
+		return 0;
+
+	ret = extcon_set_state(edev, id, cable_state);
+	if (ret < 0)
+		return ret;
+
+	return extcon_sync(edev, id);
+}
+EXPORT_SYMBOL_GPL(extcon_set_state_sync);
+
+/**
+ * extcon_get_property() - Get the property value of a specific cable.
+ * @edev:		the extcon device that has the cable.
+ * @id:			the unique id of each external connector
+ *			in extcon enumeration.
+ * @prop:		the property id among enum extcon_property.
+ * @prop_val:		the pointer which store the value of property.
+ *
+ * When getting the property value of external connector, the external connector
+ * should be attached. If detached state, function just return 0 without
+ * property value. Also, the each property should be included in the list of
+ * supported properties according to the type of external connectors.
+ *
+ * Returns 0 if success or error number if fail
+ */
+int extcon_get_property(struct extcon_dev *edev, unsigned int id,
+				unsigned int prop,
+				union extcon_property_value *prop_val)
+{
+	struct extcon_cable *cable;
+	unsigned long flags;
+	int index, ret = 0;
+
+	*prop_val = (union extcon_property_value)(0);
+
+	if (!edev)
+		return -EINVAL;
+
+	/* Check whether the property is supported or not */
+	if (!is_extcon_property_supported(id, prop))
+		return -EINVAL;
+
+	/* Find the cable index of external connector by using id */
+	index = find_cable_index_by_id(edev, id);
+	if (index < 0)
+		return index;
+
+	spin_lock_irqsave(&edev->lock, flags);
+
+	/* Check whether the property is available or not. */
+	if (!is_extcon_property_capability(edev, id, index, prop)) {
+		spin_unlock_irqrestore(&edev->lock, flags);
+		return -EPERM;
+	}
+
+	/*
+	 * Check whether the external connector is attached.
+	 * If external connector is detached, the user can not
+	 * get the property value.
+	 */
+	if (!is_extcon_attached(edev, index)) {
+		spin_unlock_irqrestore(&edev->lock, flags);
+		return 0;
+	}
+
+	cable = &edev->cables[index];
+
+	/* Get the property value according to extcon type */
+	switch (prop) {
+	case EXTCON_PROP_USB_MIN ... EXTCON_PROP_USB_MAX:
+		*prop_val = cable->usb_propval[prop - EXTCON_PROP_USB_MIN];
+		break;
+	case EXTCON_PROP_CHG_MIN ... EXTCON_PROP_CHG_MAX:
+		*prop_val = cable->chg_propval[prop - EXTCON_PROP_CHG_MIN];
+		break;
+	case EXTCON_PROP_JACK_MIN ... EXTCON_PROP_JACK_MAX:
+		*prop_val = cable->jack_propval[prop - EXTCON_PROP_JACK_MIN];
+		break;
+	case EXTCON_PROP_DISP_MIN ... EXTCON_PROP_DISP_MAX:
+		*prop_val = cable->disp_propval[prop - EXTCON_PROP_DISP_MIN];
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	spin_unlock_irqrestore(&edev->lock, flags);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(extcon_get_property);
+
+/**
+ * extcon_set_property() - Set the property value of a specific cable.
+ * @edev:		the extcon device that has the cable.
+ * @id:			the unique id of each external connector
+ *			in extcon enumeration.
+ * @prop:		the property id among enum extcon_property.
+ * @prop_val:		the pointer including the new value of property.
+ *
+ * The each property should be included in the list of supported properties
+ * according to the type of external connectors.
+ *
+ * Returns 0 if success or error number if fail
+ */
+int extcon_set_property(struct extcon_dev *edev, unsigned int id,
+				unsigned int prop,
+				union extcon_property_value prop_val)
+{
+	struct extcon_cable *cable;
+	unsigned long flags;
+	int index, ret = 0;
+
+	if (!edev)
+		return -EINVAL;
+
+	/* Check whether the property is supported or not */
+	if (!is_extcon_property_supported(id, prop))
+		return -EINVAL;
+
+	/* Find the cable index of external connector by using id */
+	index = find_cable_index_by_id(edev, id);
+	if (index < 0)
+		return index;
+
+	spin_lock_irqsave(&edev->lock, flags);
+
+	/* Check whether the property is available or not. */
+	if (!is_extcon_property_capability(edev, id, index, prop)) {
+		spin_unlock_irqrestore(&edev->lock, flags);
+		return -EPERM;
+	}
+
+	cable = &edev->cables[index];
+
+	/* Set the property value according to extcon type */
+	switch (prop) {
+	case EXTCON_PROP_USB_MIN ... EXTCON_PROP_USB_MAX:
+		cable->usb_propval[prop - EXTCON_PROP_USB_MIN] = prop_val;
+		break;
+	case EXTCON_PROP_CHG_MIN ... EXTCON_PROP_CHG_MAX:
+		cable->chg_propval[prop - EXTCON_PROP_CHG_MIN] = prop_val;
+		break;
+	case EXTCON_PROP_JACK_MIN ... EXTCON_PROP_JACK_MAX:
+		cable->jack_propval[prop - EXTCON_PROP_JACK_MIN] = prop_val;
+		break;
+	case EXTCON_PROP_DISP_MIN ... EXTCON_PROP_DISP_MAX:
+		cable->disp_propval[prop - EXTCON_PROP_DISP_MIN] = prop_val;
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	spin_unlock_irqrestore(&edev->lock, flags);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(extcon_set_property);
+
+/**
+ * extcon_set_property_sync() - Set the property value of a specific cable
+			with a notification.
+ * @prop_val:		the pointer including the new value of property.
+ *
+ * When setting the property value of external connector, the external connector
+ * should be attached. The each property should be included in the list of
+ * supported properties according to the type of external connectors.
+ *
+ * Returns 0 if success or error number if fail
+ */
+int extcon_set_property_sync(struct extcon_dev *edev, unsigned int id,
+				unsigned int prop,
+				union extcon_property_value prop_val)
+{
+	int ret;
+
+	ret = extcon_set_property(edev, id, prop, prop_val);
+	if (ret < 0)
+		return ret;
+
+	return extcon_sync(edev, id);
+}
+EXPORT_SYMBOL_GPL(extcon_set_property_sync);
+
+/**
+ * extcon_get_property_capability() - Get the capability of property
+ *			of an external connector.
+ * @edev:		the extcon device that has the cable.
+ * @id:			the unique id of each external connector
+ *			in extcon enumeration.
+ * @prop:		the property id among enum extcon_property.
+ *
+ * Returns 1 if the property is available or 0 if not available.
+ */
+int extcon_get_property_capability(struct extcon_dev *edev, unsigned int id,
+					unsigned int prop)
+{
 	int index;
 
 	if (!edev)
 		return -EINVAL;
 
+	/* Check whether the property is supported or not */
+	if (!is_extcon_property_supported(id, prop))
+		return -EINVAL;
+
+	/* Find the cable index of external connector by using id */
 	index = find_cable_index_by_id(edev, id);
 	if (index < 0)
 		return index;
 
-	if (edev->max_supported && edev->max_supported <= index)
+	return is_extcon_property_capability(edev, id, index, prop);
+}
+EXPORT_SYMBOL_GPL(extcon_get_property_capability);
+
+/**
+ * extcon_set_property_capability() - Set the capability of a property
+ *			of an external connector.
+ * @edev:		the extcon device that has the cable.
+ * @id:			the unique id of each external connector
+ *			in extcon enumeration.
+ * @prop:		the property id among enum extcon_property.
+ *
+ * This function set the capability of a property for an external connector
+ * to mark the bit in capability bitmap which mean the available state of
+ * a property.
+ *
+ * Returns 0 if success or error number if fail
+ */
+int extcon_set_property_capability(struct extcon_dev *edev, unsigned int id,
+					unsigned int prop)
+{
+	struct extcon_cable *cable;
+	int index, type, ret = 0;
+
+	if (!edev)
 		return -EINVAL;
 
-	state = cable_state ? (1 << index) : 0;
-	return extcon_update_state(edev, 1 << index, state);
+	/* Check whether the property is supported or not. */
+	if (!is_extcon_property_supported(id, prop))
+		return -EINVAL;
+
+	/* Find the cable index of external connector by using id. */
+	index = find_cable_index_by_id(edev, id);
+	if (index < 0)
+		return index;
+
+	type = get_extcon_type(prop);
+	if (type < 0)
+		return type;
+
+	cable = &edev->cables[index];
+
+	switch (type) {
+	case EXTCON_TYPE_USB:
+		__set_bit(prop - EXTCON_PROP_USB_MIN, cable->usb_bits);
+		break;
+	case EXTCON_TYPE_CHG:
+		__set_bit(prop - EXTCON_PROP_CHG_MIN, cable->chg_bits);
+		break;
+	case EXTCON_TYPE_JACK:
+		__set_bit(prop - EXTCON_PROP_JACK_MIN, cable->jack_bits);
+		break;
+	case EXTCON_TYPE_DISP:
+		__set_bit(prop - EXTCON_PROP_DISP_MIN, cable->disp_bits);
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	return ret;
 }
-EXPORT_SYMBOL_GPL(extcon_set_cable_state_);
+EXPORT_SYMBOL_GPL(extcon_set_property_capability);
 
 /**
  * extcon_get_extcon_dev() - Get the extcon device instance from the name
@@ -428,7 +904,7 @@ int extcon_register_notifier(struct extcon_dev *edev, unsigned int id,
 			     struct notifier_block *nb)
 {
 	unsigned long flags;
-	int ret, idx;
+	int ret, idx = -EINVAL;
 
 	if (!nb)
 		return -EINVAL;
@@ -846,13 +1322,13 @@ struct extcon_dev *extcon_get_edev_by_phandle(struct device *dev, int index)
 		return ERR_PTR(-EINVAL);
 
 	if (!dev->of_node) {
-		dev_err(dev, "device does not have a device node entry\n");
+		dev_dbg(dev, "device does not have a device node entry\n");
 		return ERR_PTR(-EINVAL);
 	}
 
 	node = of_parse_phandle(dev->of_node, "extcon", index);
 	if (!node) {
-		dev_err(dev, "failed to get phandle in %s node\n",
+		dev_dbg(dev, "failed to get phandle in %s node\n",
 			dev->of_node->full_name);
 		return ERR_PTR(-ENODEV);
 	}
diff --git a/drivers/firewire/nosy.c b/drivers/firewire/nosy.c
index 631c977b0da5..180f0a96528c 100644
--- a/drivers/firewire/nosy.c
+++ b/drivers/firewire/nosy.c
@@ -566,6 +566,11 @@ add_card(struct pci_dev *dev, const struct pci_device_id *unused)
 
 	lynx->registers = ioremap_nocache(pci_resource_start(dev, 0),
 					  PCILYNX_MAX_REGISTER);
+	if (lynx->registers == NULL) {
+		dev_err(&dev->dev, "Failed to map registers\n");
+		ret = -ENOMEM;
+		goto fail_deallocate_lynx;
+	}
 
 	lynx->rcv_start_pcl = pci_alloc_consistent(lynx->pci_device,
 				sizeof(struct pcl), &lynx->rcv_start_pcl_bus);
@@ -578,7 +583,7 @@ add_card(struct pci_dev *dev, const struct pci_device_id *unused)
 	    lynx->rcv_buffer == NULL) {
 		dev_err(&dev->dev, "Failed to allocate receive buffer\n");
 		ret = -ENOMEM;
-		goto fail_deallocate;
+		goto fail_deallocate_buffers;
 	}
 	lynx->rcv_start_pcl->next	= cpu_to_le32(lynx->rcv_pcl_bus);
 	lynx->rcv_pcl->next		= cpu_to_le32(PCL_NEXT_INVALID);
@@ -641,7 +646,7 @@ add_card(struct pci_dev *dev, const struct pci_device_id *unused)
 		dev_err(&dev->dev,
 			"Failed to allocate shared interrupt %d\n", dev->irq);
 		ret = -EIO;
-		goto fail_deallocate;
+		goto fail_deallocate_buffers;
 	}
 
 	lynx->misc.parent = &dev->dev;
@@ -668,7 +673,7 @@ fail_free_irq:
 	reg_write(lynx, PCI_INT_ENABLE, 0);
 	free_irq(lynx->pci_device->irq, lynx);
 
-fail_deallocate:
+fail_deallocate_buffers:
 	if (lynx->rcv_start_pcl)
 		pci_free_consistent(lynx->pci_device, sizeof(struct pcl),
 				lynx->rcv_start_pcl, lynx->rcv_start_pcl_bus);
@@ -679,6 +684,8 @@ fail_deallocate:
 		pci_free_consistent(lynx->pci_device, PAGE_SIZE,
 				lynx->rcv_buffer, lynx->rcv_buffer_bus);
 	iounmap(lynx->registers);
+
+fail_deallocate_lynx:
 	kfree(lynx);
 
 fail_disable:
diff --git a/drivers/firmware/Kconfig b/drivers/firmware/Kconfig
index 0e22f241403b..bca172d42c74 100644
--- a/drivers/firmware/Kconfig
+++ b/drivers/firmware/Kconfig
@@ -209,5 +209,6 @@ config HAVE_ARM_SMCCC
 source "drivers/firmware/broadcom/Kconfig"
 source "drivers/firmware/google/Kconfig"
 source "drivers/firmware/efi/Kconfig"
+source "drivers/firmware/meson/Kconfig"
 
 endmenu
diff --git a/drivers/firmware/Makefile b/drivers/firmware/Makefile
index 44a59dcfc398..898ac41fa8b3 100644
--- a/drivers/firmware/Makefile
+++ b/drivers/firmware/Makefile
@@ -22,6 +22,7 @@ obj-$(CONFIG_QCOM_SCM_32)	+= qcom_scm-32.o
 CFLAGS_qcom_scm-32.o :=$(call as-instr,.arch armv7-a\n.arch_extension sec,-DREQUIRES_SEC=1) -march=armv7-a
 
 obj-y				+= broadcom/
+obj-y				+= meson/
 obj-$(CONFIG_GOOGLE_FIRMWARE)	+= google/
 obj-$(CONFIG_EFI)		+= efi/
 obj-$(CONFIG_UEFI_CPER)		+= efi/
diff --git a/drivers/firmware/dcdbas.c b/drivers/firmware/dcdbas.c
index 829eec8959f2..2fe1a130189f 100644
--- a/drivers/firmware/dcdbas.c
+++ b/drivers/firmware/dcdbas.c
@@ -23,6 +23,7 @@
 #include <linux/platform_device.h>
 #include <linux/dma-mapping.h>
 #include <linux/errno.h>
+#include <linux/cpu.h>
 #include <linux/gfp.h>
 #include <linux/init.h>
 #include <linux/kernel.h>
@@ -238,33 +239,14 @@ static ssize_t host_control_on_shutdown_store(struct device *dev,
 	return count;
 }
 
-/**
- * dcdbas_smi_request: generate SMI request
- *
- * Called with smi_data_lock.
- */
-int dcdbas_smi_request(struct smi_cmd *smi_cmd)
+static int raise_smi(void *par)
 {
-	cpumask_var_t old_mask;
-	int ret = 0;
-
-	if (smi_cmd->magic != SMI_CMD_MAGIC) {
-		dev_info(&dcdbas_pdev->dev, "%s: invalid magic value\n",
-			 __func__);
-		return -EBADR;
-	}
+	struct smi_cmd *smi_cmd = par;
 
-	/* SMI requires CPU 0 */
-	if (!alloc_cpumask_var(&old_mask, GFP_KERNEL))
-		return -ENOMEM;
-
-	cpumask_copy(old_mask, &current->cpus_allowed);
-	set_cpus_allowed_ptr(current, cpumask_of(0));
 	if (smp_processor_id() != 0) {
 		dev_dbg(&dcdbas_pdev->dev, "%s: failed to get CPU 0\n",
 			__func__);
-		ret = -EBUSY;
-		goto out;
+		return -EBUSY;
 	}
 
 	/* generate SMI */
@@ -280,9 +262,28 @@ int dcdbas_smi_request(struct smi_cmd *smi_cmd)
 		: "memory"
 	);
 
-out:
-	set_cpus_allowed_ptr(current, old_mask);
-	free_cpumask_var(old_mask);
+	return 0;
+}
+/**
+ * dcdbas_smi_request: generate SMI request
+ *
+ * Called with smi_data_lock.
+ */
+int dcdbas_smi_request(struct smi_cmd *smi_cmd)
+{
+	int ret;
+
+	if (smi_cmd->magic != SMI_CMD_MAGIC) {
+		dev_info(&dcdbas_pdev->dev, "%s: invalid magic value\n",
+			 __func__);
+		return -EBADR;
+	}
+
+	/* SMI requires CPU 0 */
+	get_online_cpus();
+	ret = smp_call_on_cpu(0, raise_smi, smi_cmd, true);
+	put_online_cpus();
+
 	return ret;
 }
 
diff --git a/drivers/firmware/efi/Kconfig b/drivers/firmware/efi/Kconfig
index 6394152f648f..c981be17d3c0 100644
--- a/drivers/firmware/efi/Kconfig
+++ b/drivers/firmware/efi/Kconfig
@@ -112,6 +112,23 @@ config EFI_CAPSULE_LOADER
 
 	  Most users should say N.
 
+config EFI_TEST
+	tristate "EFI Runtime Service Tests Support"
+	depends on EFI
+	default n
+	help
+	  This driver uses the efi.<service> function pointers directly instead
+	  of going through the efivar API, because it is not trying to test the
+	  kernel subsystem, just for testing the UEFI runtime service
+	  interfaces which are provided by the firmware. This driver is used
+	  by the Firmware Test Suite (FWTS) for testing the UEFI runtime
+	  interfaces readiness of the firmware.
+	  Details for FWTS are available from:
+	  <https://wiki.ubuntu.com/FirmwareTestSuite>
+
+	  Say Y here to enable the runtime services support via /dev/efi_test.
+	  If unsure, say N.
+
 endmenu
 
 config UEFI_CPER
diff --git a/drivers/firmware/efi/Makefile b/drivers/firmware/efi/Makefile
index a219640f881f..c8a439f6d715 100644
--- a/drivers/firmware/efi/Makefile
+++ b/drivers/firmware/efi/Makefile
@@ -10,7 +10,7 @@
 KASAN_SANITIZE_runtime-wrappers.o	:= n
 
 obj-$(CONFIG_EFI)			+= efi.o vars.o reboot.o memattr.o
-obj-$(CONFIG_EFI)			+= capsule.o
+obj-$(CONFIG_EFI)			+= capsule.o memmap.o
 obj-$(CONFIG_EFI_VARS)			+= efivars.o
 obj-$(CONFIG_EFI_ESRT)			+= esrt.o
 obj-$(CONFIG_EFI_VARS_PSTORE)		+= efi-pstore.o
@@ -20,6 +20,7 @@ obj-$(CONFIG_EFI_RUNTIME_WRAPPERS)	+= runtime-wrappers.o
 obj-$(CONFIG_EFI_STUB)			+= libstub/
 obj-$(CONFIG_EFI_FAKE_MEMMAP)		+= fake_mem.o
 obj-$(CONFIG_EFI_BOOTLOADER_CONTROL)	+= efibc.o
+obj-$(CONFIG_EFI_TEST)			+= test/
 
 arm-obj-$(CONFIG_EFI)			:= arm-init.o arm-runtime.o
 obj-$(CONFIG_ARM)			+= $(arm-obj-y)
diff --git a/drivers/firmware/efi/arm-init.c b/drivers/firmware/efi/arm-init.c
index c49d50e68aee..8efe13075c92 100644
--- a/drivers/firmware/efi/arm-init.c
+++ b/drivers/firmware/efi/arm-init.c
@@ -26,9 +26,9 @@
 
 u64 efi_system_table;
 
-static int __init is_normal_ram(efi_memory_desc_t *md)
+static int __init is_memory(efi_memory_desc_t *md)
 {
-	if (md->attribute & EFI_MEMORY_WB)
+	if (md->attribute & (EFI_MEMORY_WB|EFI_MEMORY_WT|EFI_MEMORY_WC))
 		return 1;
 	return 0;
 }
@@ -152,9 +152,9 @@ out:
 }
 
 /*
- * Return true for RAM regions we want to permanently reserve.
+ * Return true for regions that can be used as System RAM.
  */
-static __init int is_reserve_region(efi_memory_desc_t *md)
+static __init int is_usable_memory(efi_memory_desc_t *md)
 {
 	switch (md->type) {
 	case EFI_LOADER_CODE:
@@ -163,18 +163,22 @@ static __init int is_reserve_region(efi_memory_desc_t *md)
 	case EFI_BOOT_SERVICES_DATA:
 	case EFI_CONVENTIONAL_MEMORY:
 	case EFI_PERSISTENT_MEMORY:
-		return 0;
+		/*
+		 * According to the spec, these regions are no longer reserved
+		 * after calling ExitBootServices(). However, we can only use
+		 * them as System RAM if they can be mapped writeback cacheable.
+		 */
+		return (md->attribute & EFI_MEMORY_WB);
 	default:
 		break;
 	}
-	return is_normal_ram(md);
+	return false;
 }
 
 static __init void reserve_regions(void)
 {
 	efi_memory_desc_t *md;
 	u64 paddr, npages, size;
-	int resv;
 
 	if (efi_enabled(EFI_DBG))
 		pr_info("Processing EFI memory map:\n");
@@ -191,32 +195,29 @@ static __init void reserve_regions(void)
 		paddr = md->phys_addr;
 		npages = md->num_pages;
 
-		resv = is_reserve_region(md);
 		if (efi_enabled(EFI_DBG)) {
 			char buf[64];
 
-			pr_info("  0x%012llx-0x%012llx %s%s\n",
+			pr_info("  0x%012llx-0x%012llx %s\n",
 				paddr, paddr + (npages << EFI_PAGE_SHIFT) - 1,
-				efi_md_typeattr_format(buf, sizeof(buf), md),
-				resv ? "*" : "");
+				efi_md_typeattr_format(buf, sizeof(buf), md));
 		}
 
 		memrange_efi_to_native(&paddr, &npages);
 		size = npages << PAGE_SHIFT;
 
-		if (is_normal_ram(md))
+		if (is_memory(md)) {
 			early_init_dt_add_memory_arch(paddr, size);
 
-		if (resv)
-			memblock_mark_nomap(paddr, size);
-
+			if (!is_usable_memory(md))
+				memblock_mark_nomap(paddr, size);
+		}
 	}
-
-	set_bit(EFI_MEMMAP, &efi.flags);
 }
 
 void __init efi_init(void)
 {
+	struct efi_memory_map_data data;
 	struct efi_fdt_params params;
 
 	/* Grab UEFI information placed in FDT by stub */
@@ -225,9 +226,12 @@ void __init efi_init(void)
 
 	efi_system_table = params.system_table;
 
-	efi.memmap.phys_map = params.mmap;
-	efi.memmap.map = early_memremap_ro(params.mmap, params.mmap_size);
-	if (efi.memmap.map == NULL) {
+	data.desc_version = params.desc_ver;
+	data.desc_size = params.desc_size;
+	data.size = params.mmap_size;
+	data.phys_map = params.mmap;
+
+	if (efi_memmap_init_early(&data) < 0) {
 		/*
 		* If we are booting via UEFI, the UEFI memory map is the only
 		* description of memory we have, so there is little point in
@@ -235,9 +239,6 @@ void __init efi_init(void)
 		*/
 		panic("Unable to map EFI memory map.\n");
 	}
-	efi.memmap.map_end = efi.memmap.map + params.mmap_size;
-	efi.memmap.desc_size = params.desc_size;
-	efi.memmap.desc_version = params.desc_ver;
 
 	WARN(efi.memmap.desc_version != 1,
 	     "Unexpected EFI_MEMORY_DESCRIPTOR version %ld",
@@ -248,7 +249,8 @@ void __init efi_init(void)
 
 	reserve_regions();
 	efi_memattr_init();
-	early_memunmap(efi.memmap.map, params.mmap_size);
+	efi_esrt_init();
+	efi_memmap_unmap();
 
 	memblock_reserve(params.mmap & PAGE_MASK,
 			 PAGE_ALIGN(params.mmap_size +
diff --git a/drivers/firmware/efi/arm-runtime.c b/drivers/firmware/efi/arm-runtime.c
index c394b81fe452..7c75a8d9091a 100644
--- a/drivers/firmware/efi/arm-runtime.c
+++ b/drivers/firmware/efi/arm-runtime.c
@@ -39,6 +39,26 @@ static struct mm_struct efi_mm = {
 	.mmlist			= LIST_HEAD_INIT(efi_mm.mmlist),
 };
 
+#ifdef CONFIG_ARM64_PTDUMP
+#include <asm/ptdump.h>
+
+static struct ptdump_info efi_ptdump_info = {
+	.mm		= &efi_mm,
+	.markers	= (struct addr_marker[]){
+		{ 0,		"UEFI runtime start" },
+		{ TASK_SIZE_64,	"UEFI runtime end" }
+	},
+	.base_addr	= 0,
+};
+
+static int __init ptdump_init(void)
+{
+	return ptdump_register(&efi_ptdump_info, "efi_page_tables");
+}
+device_initcall(ptdump_init);
+
+#endif
+
 static bool __init efi_virtmap_init(void)
 {
 	efi_memory_desc_t *md;
@@ -114,14 +134,12 @@ static int __init arm_enable_runtime_services(void)
 
 	pr_info("Remapping and enabling EFI services.\n");
 
-	mapsize = efi.memmap.map_end - efi.memmap.map;
+	mapsize = efi.memmap.desc_size * efi.memmap.nr_map;
 
-	efi.memmap.map = memremap(efi.memmap.phys_map, mapsize, MEMREMAP_WB);
-	if (!efi.memmap.map) {
+	if (efi_memmap_init_late(efi.memmap.phys_map, mapsize)) {
 		pr_err("Failed to remap EFI memory map\n");
 		return -ENOMEM;
 	}
-	efi.memmap.map_end = efi.memmap.map + mapsize;
 
 	if (!efi_virtmap_init()) {
 		pr_err("UEFI virtual mapping missing or invalid -- runtime services will not be available\n");
diff --git a/drivers/firmware/efi/efi-pstore.c b/drivers/firmware/efi/efi-pstore.c
index 30a24d09ea6c..f402ba2eed46 100644
--- a/drivers/firmware/efi/efi-pstore.c
+++ b/drivers/firmware/efi/efi-pstore.c
@@ -125,16 +125,19 @@ static void efi_pstore_scan_sysfs_enter(struct efivar_entry *pos,
  * @entry: deleting entry
  * @turn_off_scanning: Check if a scanning flag should be turned off
  */
-static inline void __efi_pstore_scan_sysfs_exit(struct efivar_entry *entry,
+static inline int __efi_pstore_scan_sysfs_exit(struct efivar_entry *entry,
 						bool turn_off_scanning)
 {
 	if (entry->deleting) {
 		list_del(&entry->list);
 		efivar_entry_iter_end();
 		efivar_unregister(entry);
-		efivar_entry_iter_begin();
+		if (efivar_entry_iter_begin())
+			return -EINTR;
 	} else if (turn_off_scanning)
 		entry->scanning = false;
+
+	return 0;
 }
 
 /**
@@ -144,13 +147,18 @@ static inline void __efi_pstore_scan_sysfs_exit(struct efivar_entry *entry,
  * @head: list head
  * @stop: a flag checking if scanning will stop
  */
-static void efi_pstore_scan_sysfs_exit(struct efivar_entry *pos,
+static int efi_pstore_scan_sysfs_exit(struct efivar_entry *pos,
 				       struct efivar_entry *next,
 				       struct list_head *head, bool stop)
 {
-	__efi_pstore_scan_sysfs_exit(pos, true);
+	int ret = __efi_pstore_scan_sysfs_exit(pos, true);
+
+	if (ret)
+		return ret;
+
 	if (stop)
-		__efi_pstore_scan_sysfs_exit(next, &next->list != head);
+		ret = __efi_pstore_scan_sysfs_exit(next, &next->list != head);
+	return ret;
 }
 
 /**
@@ -172,13 +180,17 @@ static int efi_pstore_sysfs_entry_iter(void *data, struct efivar_entry **pos)
 	struct efivar_entry *entry, *n;
 	struct list_head *head = &efivar_sysfs_list;
 	int size = 0;
+	int ret;
 
 	if (!*pos) {
 		list_for_each_entry_safe(entry, n, head, list) {
 			efi_pstore_scan_sysfs_enter(entry, n, head);
 
 			size = efi_pstore_read_func(entry, data);
-			efi_pstore_scan_sysfs_exit(entry, n, head, size < 0);
+			ret = efi_pstore_scan_sysfs_exit(entry, n, head,
+							 size < 0);
+			if (ret)
+				return ret;
 			if (size)
 				break;
 		}
@@ -190,7 +202,9 @@ static int efi_pstore_sysfs_entry_iter(void *data, struct efivar_entry **pos)
 		efi_pstore_scan_sysfs_enter((*pos), n, head);
 
 		size = efi_pstore_read_func((*pos), data);
-		efi_pstore_scan_sysfs_exit((*pos), n, head, size < 0);
+		ret = efi_pstore_scan_sysfs_exit((*pos), n, head, size < 0);
+		if (ret)
+			return ret;
 		if (size)
 			break;
 	}
@@ -232,7 +246,10 @@ static ssize_t efi_pstore_read(u64 *id, enum pstore_type_id *type,
 	if (!*data.buf)
 		return -ENOMEM;
 
-	efivar_entry_iter_begin();
+	if (efivar_entry_iter_begin()) {
+		kfree(*data.buf);
+		return -EINTR;
+	}
 	size = efi_pstore_sysfs_entry_iter(&data,
 					   (struct efivar_entry **)&psi->data);
 	efivar_entry_iter_end();
@@ -347,7 +364,8 @@ static int efi_pstore_erase(enum pstore_type_id type, u64 id, int count,
 	edata.time = time;
 	edata.name = efi_name;
 
-	efivar_entry_iter_begin();
+	if (efivar_entry_iter_begin())
+		return -EINTR;
 	found = __efivar_entry_iter(efi_pstore_erase_func, &efivar_sysfs_list, &edata, &entry);
 
 	if (found && !entry->scanning) {
@@ -362,7 +380,7 @@ static int efi_pstore_erase(enum pstore_type_id type, u64 id, int count,
 static struct pstore_info efi_pstore_info = {
 	.owner		= THIS_MODULE,
 	.name		= "efi",
-	.flags		= PSTORE_FLAGS_FRAGILE,
+	.flags		= PSTORE_FLAGS_DMESG,
 	.open		= efi_pstore_open,
 	.close		= efi_pstore_close,
 	.read		= efi_pstore_read,
diff --git a/drivers/firmware/efi/efi.c b/drivers/firmware/efi/efi.c
index 7dd2e2d37231..1ac199cd75e7 100644
--- a/drivers/firmware/efi/efi.c
+++ b/drivers/firmware/efi/efi.c
@@ -27,6 +27,7 @@
 #include <linux/slab.h>
 #include <linux/acpi.h>
 #include <linux/ucs2_string.h>
+#include <linux/memblock.h>
 
 #include <asm/early_ioremap.h>
 
@@ -347,56 +348,31 @@ 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,
+ * physical 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;
-	phys_addr_t p, e;
+	efi_memory_desc_t *md;
 
 	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;
+	for_each_efi_memory_desc(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(p, sizeof (*md));
-		if (!md) {
-			pr_err_once("early_memremap(%pa, %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;
 		}
 
@@ -404,11 +380,8 @@ int __init efi_mem_desc_lookup(u64 phys_addr, efi_memory_desc_t *out_md)
 		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;
@@ -424,6 +397,35 @@ u64 __init efi_mem_desc_end(efi_memory_desc_t *md)
 	return end;
 }
 
+void __init __weak efi_arch_mem_reserve(phys_addr_t addr, u64 size) {}
+
+/**
+ * efi_mem_reserve - Reserve an EFI memory region
+ * @addr: Physical address to reserve
+ * @size: Size of reservation
+ *
+ * Mark a region as reserved from general kernel allocation and
+ * prevent it being released by efi_free_boot_services().
+ *
+ * This function should be called drivers once they've parsed EFI
+ * configuration tables to figure out where their data lives, e.g.
+ * efi_esrt_init().
+ */
+void __init efi_mem_reserve(phys_addr_t addr, u64 size)
+{
+	if (!memblock_is_region_reserved(addr, size))
+		memblock_reserve(addr, size);
+
+	/*
+	 * Some architectures (x86) reserve all boot services ranges
+	 * until efi_free_boot_services() because of buggy firmware
+	 * implementations. This means the above memblock_reserve() is
+	 * superfluous on x86 and instead what it needs to do is
+	 * ensure the @start, @size is not freed.
+	 */
+	efi_arch_mem_reserve(addr, size);
+}
+
 static __initdata efi_config_table_type_t common_tables[] = {
 	{ACPI_20_TABLE_GUID, "ACPI 2.0", &efi.acpi20},
 	{ACPI_TABLE_GUID, "ACPI", &efi.acpi},
@@ -811,6 +813,9 @@ int efi_status_to_err(efi_status_t status)
 	case EFI_NOT_FOUND:
 		err = -ENOENT;
 		break;
+	case EFI_ABORTED:
+		err = -EINTR;
+		break;
 	default:
 		err = -EINVAL;
 	}
diff --git a/drivers/firmware/efi/efivars.c b/drivers/firmware/efi/efivars.c
index 116b244dee68..3e626fd9bd4e 100644
--- a/drivers/firmware/efi/efivars.c
+++ b/drivers/firmware/efi/efivars.c
@@ -510,7 +510,8 @@ static ssize_t efivar_delete(struct file *filp, struct kobject *kobj,
 		vendor = del_var->VendorGuid;
 	}
 
-	efivar_entry_iter_begin();
+	if (efivar_entry_iter_begin())
+		return -EINTR;
 	entry = efivar_entry_find(name, vendor, &efivar_sysfs_list, true);
 	if (!entry)
 		err = -EINVAL;
@@ -575,7 +576,10 @@ efivar_create_sysfs_entry(struct efivar_entry *new_var)
 		return ret;
 
 	kobject_uevent(&new_var->kobj, KOBJ_ADD);
-	efivar_entry_add(new_var, &efivar_sysfs_list);
+	if (efivar_entry_add(new_var, &efivar_sysfs_list)) {
+		efivar_unregister(new_var);
+		return -EINTR;
+	}
 
 	return 0;
 }
@@ -690,7 +694,10 @@ static int efivars_sysfs_callback(efi_char16_t *name, efi_guid_t vendor,
 
 static int efivar_sysfs_destroy(struct efivar_entry *entry, void *data)
 {
-	efivar_entry_remove(entry);
+	int err = efivar_entry_remove(entry);
+
+	if (err)
+		return err;
 	efivar_unregister(entry);
 	return 0;
 }
@@ -698,7 +705,14 @@ static int efivar_sysfs_destroy(struct efivar_entry *entry, void *data)
 static void efivars_sysfs_exit(void)
 {
 	/* Remove all entries and destroy */
-	__efivar_entry_iter(efivar_sysfs_destroy, &efivar_sysfs_list, NULL, NULL);
+	int err;
+
+	err = __efivar_entry_iter(efivar_sysfs_destroy, &efivar_sysfs_list,
+				  NULL, NULL);
+	if (err) {
+		pr_err("efivars: Failed to destroy sysfs entries\n");
+		return;
+	}
 
 	if (efivars_new_var)
 		sysfs_remove_bin_file(&efivars_kset->kobj, efivars_new_var);
diff --git a/drivers/firmware/efi/esrt.c b/drivers/firmware/efi/esrt.c
index 75feb3f5829b..14914074f716 100644
--- a/drivers/firmware/efi/esrt.c
+++ b/drivers/firmware/efi/esrt.c
@@ -16,6 +16,7 @@
 #include <linux/device.h>
 #include <linux/efi.h>
 #include <linux/init.h>
+#include <linux/io.h>
 #include <linux/kernel.h>
 #include <linux/kobject.h>
 #include <linux/list.h>
@@ -235,7 +236,7 @@ static struct attribute_group esrt_attr_group = {
 };
 
 /*
- * remap the table, copy it to kmalloced pages, and unmap it.
+ * remap the table, validate it, mark it reserved and unmap it.
  */
 void __init efi_esrt_init(void)
 {
@@ -335,7 +336,7 @@ void __init efi_esrt_init(void)
 
 	end = esrt_data + size;
 	pr_info("Reserving ESRT space from %pa to %pa.\n", &esrt_data, &end);
-	memblock_reserve(esrt_data, esrt_data_size);
+	efi_mem_reserve(esrt_data, esrt_data_size);
 
 	pr_debug("esrt-init: loaded.\n");
 err_memunmap:
@@ -382,28 +383,18 @@ static void cleanup_entry_list(void)
 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);
+	esrt = memremap(esrt_data, esrt_data_size, MEMREMAP_WB);
 	if (!esrt) {
-		pr_err("kmalloc failed. (wanted %zu bytes)\n", esrt_data_size);
-		iounmap(ioesrt);
+		pr_err("memremap(%pa, %zu) failed.\n", &esrt_data,
+		       esrt_data_size);
 		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");
@@ -429,8 +420,6 @@ static int __init esrt_sysfs_init(void)
 	if (error)
 		goto err_cleanup_list;
 
-	memblock_remove(esrt_data, esrt_data_size);
-
 	pr_debug("esrt-sysfs: loaded.\n");
 
 	return 0;
diff --git a/drivers/firmware/efi/fake_mem.c b/drivers/firmware/efi/fake_mem.c
index 48430aba13c1..520a40e5e0e4 100644
--- a/drivers/firmware/efi/fake_mem.c
+++ b/drivers/firmware/efi/fake_mem.c
@@ -35,17 +35,13 @@
 
 #define EFI_MAX_FAKEMEM CONFIG_EFI_MAX_FAKE_MEM
 
-struct fake_mem {
-	struct range range;
-	u64 attribute;
-};
-static struct fake_mem fake_mems[EFI_MAX_FAKEMEM];
+static struct efi_mem_range fake_mems[EFI_MAX_FAKEMEM];
 static int nr_fake_mem;
 
 static int __init cmp_fake_mem(const void *x1, const void *x2)
 {
-	const struct fake_mem *m1 = x1;
-	const struct fake_mem *m2 = x2;
+	const struct efi_mem_range *m1 = x1;
+	const struct efi_mem_range *m2 = x2;
 
 	if (m1->range.start < m2->range.start)
 		return -1;
@@ -56,40 +52,21 @@ static int __init cmp_fake_mem(const void *x1, const void *x2)
 
 void __init efi_fake_memmap(void)
 {
-	u64 start, end, m_start, m_end, m_attr;
 	int new_nr_map = efi.memmap.nr_map;
 	efi_memory_desc_t *md;
 	phys_addr_t new_memmap_phy;
 	void *new_memmap;
-	void *old, *new;
 	int i;
 
-	if (!nr_fake_mem || !efi_enabled(EFI_MEMMAP))
+	if (!nr_fake_mem)
 		return;
 
 	/* count up the number of EFI memory descriptor */
-	for_each_efi_memory_desc(md) {
-		start = md->phys_addr;
-		end = start + (md->num_pages << EFI_PAGE_SHIFT) - 1;
-
-		for (i = 0; i < nr_fake_mem; i++) {
-			/* modifying range */
-			m_start = fake_mems[i].range.start;
-			m_end = fake_mems[i].range.end;
-
-			if (m_start <= start) {
-				/* split into 2 parts */
-				if (start < m_end && m_end < end)
-					new_nr_map++;
-			}
-			if (start < m_start && m_start < end) {
-				/* split into 3 parts */
-				if (m_end < end)
-					new_nr_map += 2;
-				/* split into 2 parts */
-				if (end <= m_end)
-					new_nr_map++;
-			}
+	for (i = 0; i < nr_fake_mem; i++) {
+		for_each_efi_memory_desc(md) {
+			struct range *r = &fake_mems[i].range;
+
+			new_nr_map += efi_memmap_split_count(md, r);
 		}
 	}
 
@@ -107,85 +84,13 @@ void __init efi_fake_memmap(void)
 		return;
 	}
 
-	for (old = efi.memmap.map, new = new_memmap;
-	     old < efi.memmap.map_end;
-	     old += efi.memmap.desc_size, new += efi.memmap.desc_size) {
-
-		/* copy original EFI memory descriptor */
-		memcpy(new, old, efi.memmap.desc_size);
-		md = new;
-		start = md->phys_addr;
-		end = md->phys_addr + (md->num_pages << EFI_PAGE_SHIFT) - 1;
-
-		for (i = 0; i < nr_fake_mem; i++) {
-			/* modifying range */
-			m_start = fake_mems[i].range.start;
-			m_end = fake_mems[i].range.end;
-			m_attr = fake_mems[i].attribute;
-
-			if (m_start <= start && end <= m_end)
-				md->attribute |= m_attr;
-
-			if (m_start <= start &&
-			    (start < m_end && m_end < end)) {
-				/* first part */
-				md->attribute |= m_attr;
-				md->num_pages = (m_end - md->phys_addr + 1) >>
-					EFI_PAGE_SHIFT;
-				/* latter part */
-				new += efi.memmap.desc_size;
-				memcpy(new, old, efi.memmap.desc_size);
-				md = new;
-				md->phys_addr = m_end + 1;
-				md->num_pages = (end - md->phys_addr + 1) >>
-					EFI_PAGE_SHIFT;
-			}
-
-			if ((start < m_start && m_start < end) && m_end < end) {
-				/* first part */
-				md->num_pages = (m_start - md->phys_addr) >>
-					EFI_PAGE_SHIFT;
-				/* middle part */
-				new += efi.memmap.desc_size;
-				memcpy(new, old, efi.memmap.desc_size);
-				md = new;
-				md->attribute |= m_attr;
-				md->phys_addr = m_start;
-				md->num_pages = (m_end - m_start + 1) >>
-					EFI_PAGE_SHIFT;
-				/* last part */
-				new += efi.memmap.desc_size;
-				memcpy(new, old, efi.memmap.desc_size);
-				md = new;
-				md->phys_addr = m_end + 1;
-				md->num_pages = (end - m_end) >>
-					EFI_PAGE_SHIFT;
-			}
-
-			if ((start < m_start && m_start < end) &&
-			    (end <= m_end)) {
-				/* first part */
-				md->num_pages = (m_start - md->phys_addr) >>
-					EFI_PAGE_SHIFT;
-				/* latter part */
-				new += efi.memmap.desc_size;
-				memcpy(new, old, efi.memmap.desc_size);
-				md = new;
-				md->phys_addr = m_start;
-				md->num_pages = (end - md->phys_addr + 1) >>
-					EFI_PAGE_SHIFT;
-				md->attribute |= m_attr;
-			}
-		}
-	}
+	for (i = 0; i < nr_fake_mem; i++)
+		efi_memmap_insert(&efi.memmap, new_memmap, &fake_mems[i]);
 
 	/* swap into new EFI memmap */
-	efi_unmap_memmap();
-	efi.memmap.map = new_memmap;
-	efi.memmap.phys_map = new_memmap_phy;
-	efi.memmap.nr_map = new_nr_map;
-	efi.memmap.map_end = efi.memmap.map + efi.memmap.nr_map * efi.memmap.desc_size;
-	set_bit(EFI_MEMMAP, &efi.flags);
+	early_memunmap(new_memmap, efi.memmap.desc_size * new_nr_map);
+
+	efi_memmap_install(new_memmap_phy, new_nr_map);
 
 	/* print new EFI memmap */
 	efi_print_memmap();
@@ -223,7 +128,7 @@ static int __init setup_fake_mem(char *p)
 			p++;
 	}
 
-	sort(fake_mems, nr_fake_mem, sizeof(struct fake_mem),
+	sort(fake_mems, nr_fake_mem, sizeof(struct efi_mem_range),
 	     cmp_fake_mem, NULL);
 
 	for (i = 0; i < nr_fake_mem; i++)
diff --git a/drivers/firmware/efi/libstub/Makefile b/drivers/firmware/efi/libstub/Makefile
index c06945160a41..5e23e2d305e7 100644
--- a/drivers/firmware/efi/libstub/Makefile
+++ b/drivers/firmware/efi/libstub/Makefile
@@ -11,7 +11,7 @@ cflags-$(CONFIG_X86)		+= -m$(BITS) -D__KERNEL__ $(LINUX_INCLUDE) -O2 \
 				   -mno-mmx -mno-sse
 
 cflags-$(CONFIG_ARM64)		:= $(subst -pg,,$(KBUILD_CFLAGS))
-cflags-$(CONFIG_ARM)		:= $(subst -pg,,$(KBUILD_CFLAGS)) \
+cflags-$(CONFIG_ARM)		:= $(subst -pg,,$(KBUILD_CFLAGS)) -g0 \
 				   -fno-builtin -fpic -mno-single-pic-base
 
 cflags-$(CONFIG_EFI_ARMSTUB)	+= -I$(srctree)/scripts/dtc/libfdt
@@ -79,5 +79,6 @@ quiet_cmd_stubcopy = STUBCPY $@
 # decompressor. So move our .data to .data.efistub, which is preserved
 # explicitly by the decompressor linker script.
 #
-STUBCOPY_FLAGS-$(CONFIG_ARM)	+= --rename-section .data=.data.efistub
+STUBCOPY_FLAGS-$(CONFIG_ARM)	+= --rename-section .data=.data.efistub \
+				   -R ___ksymtab+sort -R ___kcrctab+sort
 STUBCOPY_RELOC-$(CONFIG_ARM)	:= R_ARM_ABS
diff --git a/drivers/firmware/efi/memmap.c b/drivers/firmware/efi/memmap.c
new file mode 100644
index 000000000000..f03ddecd232b
--- /dev/null
+++ b/drivers/firmware/efi/memmap.c
@@ -0,0 +1,303 @@
+/*
+ * Common EFI memory map functions.
+ */
+
+#define pr_fmt(fmt) "efi: " fmt
+
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/efi.h>
+#include <linux/io.h>
+#include <asm/early_ioremap.h>
+
+/**
+ * __efi_memmap_init - Common code for mapping the EFI memory map
+ * @data: EFI memory map data
+ * @late: Use early or late mapping function?
+ *
+ * This function takes care of figuring out which function to use to
+ * map the EFI memory map in efi.memmap based on how far into the boot
+ * we are.
+ *
+ * During bootup @late should be %false since we only have access to
+ * the early_memremap*() functions as the vmalloc space isn't setup.
+ * Once the kernel is fully booted we can fallback to the more robust
+ * memremap*() API.
+ *
+ * Returns zero on success, a negative error code on failure.
+ */
+static int __init
+__efi_memmap_init(struct efi_memory_map_data *data, bool late)
+{
+	struct efi_memory_map map;
+	phys_addr_t phys_map;
+
+	if (efi_enabled(EFI_PARAVIRT))
+		return 0;
+
+	phys_map = data->phys_map;
+
+	if (late)
+		map.map = memremap(phys_map, data->size, MEMREMAP_WB);
+	else
+		map.map = early_memremap(phys_map, data->size);
+
+	if (!map.map) {
+		pr_err("Could not map the memory map!\n");
+		return -ENOMEM;
+	}
+
+	map.phys_map = data->phys_map;
+	map.nr_map = data->size / data->desc_size;
+	map.map_end = map.map + data->size;
+
+	map.desc_version = data->desc_version;
+	map.desc_size = data->desc_size;
+	map.late = late;
+
+	set_bit(EFI_MEMMAP, &efi.flags);
+
+	efi.memmap = map;
+
+	return 0;
+}
+
+/**
+ * efi_memmap_init_early - Map the EFI memory map data structure
+ * @data: EFI memory map data
+ *
+ * Use early_memremap() to map the passed in EFI memory map and assign
+ * it to efi.memmap.
+ */
+int __init efi_memmap_init_early(struct efi_memory_map_data *data)
+{
+	/* Cannot go backwards */
+	WARN_ON(efi.memmap.late);
+
+	return __efi_memmap_init(data, false);
+}
+
+void __init efi_memmap_unmap(void)
+{
+	if (!efi.memmap.late) {
+		unsigned long size;
+
+		size = efi.memmap.desc_size * efi.memmap.nr_map;
+		early_memunmap(efi.memmap.map, size);
+	} else {
+		memunmap(efi.memmap.map);
+	}
+
+	efi.memmap.map = NULL;
+	clear_bit(EFI_MEMMAP, &efi.flags);
+}
+
+/**
+ * efi_memmap_init_late - Map efi.memmap with memremap()
+ * @phys_addr: Physical address of the new EFI memory map
+ * @size: Size in bytes of the new EFI memory map
+ *
+ * Setup a mapping of the EFI memory map using ioremap_cache(). This
+ * function should only be called once the vmalloc space has been
+ * setup and is therefore not suitable for calling during early EFI
+ * initialise, e.g. in efi_init(). Additionally, it expects
+ * efi_memmap_init_early() to have already been called.
+ *
+ * The reason there are two EFI memmap initialisation
+ * (efi_memmap_init_early() and this late version) is because the
+ * early EFI memmap should be explicitly unmapped once EFI
+ * initialisation is complete as the fixmap space used to map the EFI
+ * memmap (via early_memremap()) is a scarce resource.
+ *
+ * This late mapping is intended to persist for the duration of
+ * runtime so that things like efi_mem_desc_lookup() and
+ * efi_mem_attributes() always work.
+ *
+ * Returns zero on success, a negative error code on failure.
+ */
+int __init efi_memmap_init_late(phys_addr_t addr, unsigned long size)
+{
+	struct efi_memory_map_data data = {
+		.phys_map = addr,
+		.size = size,
+	};
+
+	/* Did we forget to unmap the early EFI memmap? */
+	WARN_ON(efi.memmap.map);
+
+	/* Were we already called? */
+	WARN_ON(efi.memmap.late);
+
+	/*
+	 * It makes no sense to allow callers to register different
+	 * values for the following fields. Copy them out of the
+	 * existing early EFI memmap.
+	 */
+	data.desc_version = efi.memmap.desc_version;
+	data.desc_size = efi.memmap.desc_size;
+
+	return __efi_memmap_init(&data, true);
+}
+
+/**
+ * efi_memmap_install - Install a new EFI memory map in efi.memmap
+ * @addr: Physical address of the memory map
+ * @nr_map: Number of entries in the memory map
+ *
+ * Unlike efi_memmap_init_*(), this function does not allow the caller
+ * to switch from early to late mappings. It simply uses the existing
+ * mapping function and installs the new memmap.
+ *
+ * Returns zero on success, a negative error code on failure.
+ */
+int __init efi_memmap_install(phys_addr_t addr, unsigned int nr_map)
+{
+	struct efi_memory_map_data data;
+
+	efi_memmap_unmap();
+
+	data.phys_map = addr;
+	data.size = efi.memmap.desc_size * nr_map;
+	data.desc_version = efi.memmap.desc_version;
+	data.desc_size = efi.memmap.desc_size;
+
+	return __efi_memmap_init(&data, efi.memmap.late);
+}
+
+/**
+ * efi_memmap_split_count - Count number of additional EFI memmap entries
+ * @md: EFI memory descriptor to split
+ * @range: Address range (start, end) to split around
+ *
+ * Returns the number of additional EFI memmap entries required to
+ * accomodate @range.
+ */
+int __init efi_memmap_split_count(efi_memory_desc_t *md, struct range *range)
+{
+	u64 m_start, m_end;
+	u64 start, end;
+	int count = 0;
+
+	start = md->phys_addr;
+	end = start + (md->num_pages << EFI_PAGE_SHIFT) - 1;
+
+	/* modifying range */
+	m_start = range->start;
+	m_end = range->end;
+
+	if (m_start <= start) {
+		/* split into 2 parts */
+		if (start < m_end && m_end < end)
+			count++;
+	}
+
+	if (start < m_start && m_start < end) {
+		/* split into 3 parts */
+		if (m_end < end)
+			count += 2;
+		/* split into 2 parts */
+		if (end <= m_end)
+			count++;
+	}
+
+	return count;
+}
+
+/**
+ * efi_memmap_insert - Insert a memory region in an EFI memmap
+ * @old_memmap: The existing EFI memory map structure
+ * @buf: Address of buffer to store new map
+ * @mem: Memory map entry to insert
+ *
+ * It is suggested that you call efi_memmap_split_count() first
+ * to see how large @buf needs to be.
+ */
+void __init efi_memmap_insert(struct efi_memory_map *old_memmap, void *buf,
+			      struct efi_mem_range *mem)
+{
+	u64 m_start, m_end, m_attr;
+	efi_memory_desc_t *md;
+	u64 start, end;
+	void *old, *new;
+
+	/* modifying range */
+	m_start = mem->range.start;
+	m_end = mem->range.end;
+	m_attr = mem->attribute;
+
+	/*
+	 * The EFI memory map deals with regions in EFI_PAGE_SIZE
+	 * units. Ensure that the region described by 'mem' is aligned
+	 * correctly.
+	 */
+	if (!IS_ALIGNED(m_start, EFI_PAGE_SIZE) ||
+	    !IS_ALIGNED(m_end + 1, EFI_PAGE_SIZE)) {
+		WARN_ON(1);
+		return;
+	}
+
+	for (old = old_memmap->map, new = buf;
+	     old < old_memmap->map_end;
+	     old += old_memmap->desc_size, new += old_memmap->desc_size) {
+
+		/* copy original EFI memory descriptor */
+		memcpy(new, old, old_memmap->desc_size);
+		md = new;
+		start = md->phys_addr;
+		end = md->phys_addr + (md->num_pages << EFI_PAGE_SHIFT) - 1;
+
+		if (m_start <= start && end <= m_end)
+			md->attribute |= m_attr;
+
+		if (m_start <= start &&
+		    (start < m_end && m_end < end)) {
+			/* first part */
+			md->attribute |= m_attr;
+			md->num_pages = (m_end - md->phys_addr + 1) >>
+				EFI_PAGE_SHIFT;
+			/* latter part */
+			new += old_memmap->desc_size;
+			memcpy(new, old, old_memmap->desc_size);
+			md = new;
+			md->phys_addr = m_end + 1;
+			md->num_pages = (end - md->phys_addr + 1) >>
+				EFI_PAGE_SHIFT;
+		}
+
+		if ((start < m_start && m_start < end) && m_end < end) {
+			/* first part */
+			md->num_pages = (m_start - md->phys_addr) >>
+				EFI_PAGE_SHIFT;
+			/* middle part */
+			new += old_memmap->desc_size;
+			memcpy(new, old, old_memmap->desc_size);
+			md = new;
+			md->attribute |= m_attr;
+			md->phys_addr = m_start;
+			md->num_pages = (m_end - m_start + 1) >>
+				EFI_PAGE_SHIFT;
+			/* last part */
+			new += old_memmap->desc_size;
+			memcpy(new, old, old_memmap->desc_size);
+			md = new;
+			md->phys_addr = m_end + 1;
+			md->num_pages = (end - m_end) >>
+				EFI_PAGE_SHIFT;
+		}
+
+		if ((start < m_start && m_start < end) &&
+		    (end <= m_end)) {
+			/* first part */
+			md->num_pages = (m_start - md->phys_addr) >>
+				EFI_PAGE_SHIFT;
+			/* latter part */
+			new += old_memmap->desc_size;
+			memcpy(new, old, old_memmap->desc_size);
+			md = new;
+			md->phys_addr = m_start;
+			md->num_pages = (end - md->phys_addr + 1) >>
+				EFI_PAGE_SHIFT;
+			md->attribute |= m_attr;
+		}
+	}
+}
diff --git a/drivers/firmware/efi/runtime-map.c b/drivers/firmware/efi/runtime-map.c
index 5c55227a34c8..8e64b77aeac9 100644
--- a/drivers/firmware/efi/runtime-map.c
+++ b/drivers/firmware/efi/runtime-map.c
@@ -14,10 +14,6 @@
 
 #include <asm/setup.h>
 
-static void *efi_runtime_map;
-static int nr_efi_runtime_map;
-static u32 efi_memdesc_size;
-
 struct efi_runtime_map_entry {
 	efi_memory_desc_t md;
 	struct kobject kobj;   /* kobject for each entry */
@@ -106,7 +102,8 @@ static struct kobj_type __refdata map_ktype = {
 static struct kset *map_kset;
 
 static struct efi_runtime_map_entry *
-add_sysfs_runtime_map_entry(struct kobject *kobj, int nr)
+add_sysfs_runtime_map_entry(struct kobject *kobj, int nr,
+			    efi_memory_desc_t *md)
 {
 	int ret;
 	struct efi_runtime_map_entry *entry;
@@ -124,8 +121,7 @@ add_sysfs_runtime_map_entry(struct kobject *kobj, int nr)
 		return ERR_PTR(-ENOMEM);
 	}
 
-	memcpy(&entry->md, efi_runtime_map + nr * efi_memdesc_size,
-	       sizeof(efi_memory_desc_t));
+	memcpy(&entry->md, md, sizeof(efi_memory_desc_t));
 
 	kobject_init(&entry->kobj, &map_ktype);
 	entry->kobj.kset = map_kset;
@@ -142,12 +138,12 @@ add_sysfs_runtime_map_entry(struct kobject *kobj, int nr)
 
 int efi_get_runtime_map_size(void)
 {
-	return nr_efi_runtime_map * efi_memdesc_size;
+	return efi.memmap.nr_map * efi.memmap.desc_size;
 }
 
 int efi_get_runtime_map_desc_size(void)
 {
-	return efi_memdesc_size;
+	return efi.memmap.desc_size;
 }
 
 int efi_runtime_map_copy(void *buf, size_t bufsz)
@@ -157,38 +153,33 @@ int efi_runtime_map_copy(void *buf, size_t bufsz)
 	if (sz > bufsz)
 		sz = bufsz;
 
-	memcpy(buf, efi_runtime_map, sz);
+	memcpy(buf, efi.memmap.map, sz);
 	return 0;
 }
 
-void efi_runtime_map_setup(void *map, int nr_entries, u32 desc_size)
-{
-	efi_runtime_map = map;
-	nr_efi_runtime_map = nr_entries;
-	efi_memdesc_size = desc_size;
-}
-
 int __init efi_runtime_map_init(struct kobject *efi_kobj)
 {
 	int i, j, ret = 0;
 	struct efi_runtime_map_entry *entry;
+	efi_memory_desc_t *md;
 
-	if (!efi_runtime_map)
+	if (!efi_enabled(EFI_MEMMAP))
 		return 0;
 
-	map_entries = kzalloc(nr_efi_runtime_map * sizeof(entry), GFP_KERNEL);
+	map_entries = kzalloc(efi.memmap.nr_map * sizeof(entry), GFP_KERNEL);
 	if (!map_entries) {
 		ret = -ENOMEM;
 		goto out;
 	}
 
-	for (i = 0; i < nr_efi_runtime_map; i++) {
-		entry = add_sysfs_runtime_map_entry(efi_kobj, i);
+	i = 0;
+	for_each_efi_memory_desc(md) {
+		entry = add_sysfs_runtime_map_entry(efi_kobj, i, md);
 		if (IS_ERR(entry)) {
 			ret = PTR_ERR(entry);
 			goto out_add_entry;
 		}
-		*(map_entries + i) = entry;
+		*(map_entries + i++) = entry;
 	}
 
 	return 0;
diff --git a/drivers/firmware/efi/runtime-wrappers.c b/drivers/firmware/efi/runtime-wrappers.c
index 41958774cde3..ae54870b2788 100644
--- a/drivers/firmware/efi/runtime-wrappers.c
+++ b/drivers/firmware/efi/runtime-wrappers.c
@@ -14,11 +14,13 @@
  * This file is released under the GPLv2.
  */
 
+#define pr_fmt(fmt)	"efi: " fmt
+
 #include <linux/bug.h>
 #include <linux/efi.h>
 #include <linux/irqflags.h>
 #include <linux/mutex.h>
-#include <linux/spinlock.h>
+#include <linux/semaphore.h>
 #include <linux/stringify.h>
 #include <asm/efi.h>
 
@@ -81,20 +83,21 @@ void efi_call_virt_check_flags(unsigned long flags, const char *call)
  * +------------------------------------+-------------------------------+
  *
  * Due to the fact that the EFI pstore may write to the variable store in
- * interrupt context, we need to use a spinlock for at least the groups that
+ * interrupt context, we need to use a lock for at least the groups that
  * contain SetVariable() and QueryVariableInfo(). That leaves little else, as
  * none of the remaining functions are actually ever called at runtime.
- * So let's just use a single spinlock to serialize all Runtime Services calls.
+ * So let's just use a single lock to serialize all Runtime Services calls.
  */
-static DEFINE_SPINLOCK(efi_runtime_lock);
+static DEFINE_SEMAPHORE(efi_runtime_lock);
 
 static efi_status_t virt_efi_get_time(efi_time_t *tm, efi_time_cap_t *tc)
 {
 	efi_status_t status;
 
-	spin_lock(&efi_runtime_lock);
+	if (down_interruptible(&efi_runtime_lock))
+		return EFI_ABORTED;
 	status = efi_call_virt(get_time, tm, tc);
-	spin_unlock(&efi_runtime_lock);
+	up(&efi_runtime_lock);
 	return status;
 }
 
@@ -102,9 +105,10 @@ static efi_status_t virt_efi_set_time(efi_time_t *tm)
 {
 	efi_status_t status;
 
-	spin_lock(&efi_runtime_lock);
+	if (down_interruptible(&efi_runtime_lock))
+		return EFI_ABORTED;
 	status = efi_call_virt(set_time, tm);
-	spin_unlock(&efi_runtime_lock);
+	up(&efi_runtime_lock);
 	return status;
 }
 
@@ -114,9 +118,10 @@ static efi_status_t virt_efi_get_wakeup_time(efi_bool_t *enabled,
 {
 	efi_status_t status;
 
-	spin_lock(&efi_runtime_lock);
+	if (down_interruptible(&efi_runtime_lock))
+		return EFI_ABORTED;
 	status = efi_call_virt(get_wakeup_time, enabled, pending, tm);
-	spin_unlock(&efi_runtime_lock);
+	up(&efi_runtime_lock);
 	return status;
 }
 
@@ -124,9 +129,10 @@ static efi_status_t virt_efi_set_wakeup_time(efi_bool_t enabled, efi_time_t *tm)
 {
 	efi_status_t status;
 
-	spin_lock(&efi_runtime_lock);
+	if (down_interruptible(&efi_runtime_lock))
+		return EFI_ABORTED;
 	status = efi_call_virt(set_wakeup_time, enabled, tm);
-	spin_unlock(&efi_runtime_lock);
+	up(&efi_runtime_lock);
 	return status;
 }
 
@@ -138,10 +144,11 @@ static efi_status_t virt_efi_get_variable(efi_char16_t *name,
 {
 	efi_status_t status;
 
-	spin_lock(&efi_runtime_lock);
+	if (down_interruptible(&efi_runtime_lock))
+		return EFI_ABORTED;
 	status = efi_call_virt(get_variable, name, vendor, attr, data_size,
 			       data);
-	spin_unlock(&efi_runtime_lock);
+	up(&efi_runtime_lock);
 	return status;
 }
 
@@ -151,9 +158,10 @@ static efi_status_t virt_efi_get_next_variable(unsigned long *name_size,
 {
 	efi_status_t status;
 
-	spin_lock(&efi_runtime_lock);
+	if (down_interruptible(&efi_runtime_lock))
+		return EFI_ABORTED;
 	status = efi_call_virt(get_next_variable, name_size, name, vendor);
-	spin_unlock(&efi_runtime_lock);
+	up(&efi_runtime_lock);
 	return status;
 }
 
@@ -165,10 +173,11 @@ static efi_status_t virt_efi_set_variable(efi_char16_t *name,
 {
 	efi_status_t status;
 
-	spin_lock(&efi_runtime_lock);
+	if (down_interruptible(&efi_runtime_lock))
+		return EFI_ABORTED;
 	status = efi_call_virt(set_variable, name, vendor, attr, data_size,
 			       data);
-	spin_unlock(&efi_runtime_lock);
+	up(&efi_runtime_lock);
 	return status;
 }
 
@@ -179,12 +188,12 @@ virt_efi_set_variable_nonblocking(efi_char16_t *name, efi_guid_t *vendor,
 {
 	efi_status_t status;
 
-	if (!spin_trylock(&efi_runtime_lock))
+	if (down_trylock(&efi_runtime_lock))
 		return EFI_NOT_READY;
 
 	status = efi_call_virt(set_variable, name, vendor, attr, data_size,
 			       data);
-	spin_unlock(&efi_runtime_lock);
+	up(&efi_runtime_lock);
 	return status;
 }
 
@@ -199,10 +208,11 @@ static efi_status_t virt_efi_query_variable_info(u32 attr,
 	if (efi.runtime_version < EFI_2_00_SYSTEM_TABLE_REVISION)
 		return EFI_UNSUPPORTED;
 
-	spin_lock(&efi_runtime_lock);
+	if (down_interruptible(&efi_runtime_lock))
+		return EFI_ABORTED;
 	status = efi_call_virt(query_variable_info, attr, storage_space,
 			       remaining_space, max_variable_size);
-	spin_unlock(&efi_runtime_lock);
+	up(&efi_runtime_lock);
 	return status;
 }
 
@@ -217,12 +227,12 @@ virt_efi_query_variable_info_nonblocking(u32 attr,
 	if (efi.runtime_version < EFI_2_00_SYSTEM_TABLE_REVISION)
 		return EFI_UNSUPPORTED;
 
-	if (!spin_trylock(&efi_runtime_lock))
+	if (down_trylock(&efi_runtime_lock))
 		return EFI_NOT_READY;
 
 	status = efi_call_virt(query_variable_info, attr, storage_space,
 			       remaining_space, max_variable_size);
-	spin_unlock(&efi_runtime_lock);
+	up(&efi_runtime_lock);
 	return status;
 }
 
@@ -230,9 +240,10 @@ static efi_status_t virt_efi_get_next_high_mono_count(u32 *count)
 {
 	efi_status_t status;
 
-	spin_lock(&efi_runtime_lock);
+	if (down_interruptible(&efi_runtime_lock))
+		return EFI_ABORTED;
 	status = efi_call_virt(get_next_high_mono_count, count);
-	spin_unlock(&efi_runtime_lock);
+	up(&efi_runtime_lock);
 	return status;
 }
 
@@ -241,9 +252,13 @@ static void virt_efi_reset_system(int reset_type,
 				  unsigned long data_size,
 				  efi_char16_t *data)
 {
-	spin_lock(&efi_runtime_lock);
+	if (down_interruptible(&efi_runtime_lock)) {
+		pr_warn("failed to invoke the reset_system() runtime service:\n"
+			"could not get exclusive access to the firmware\n");
+		return;
+	}
 	__efi_call_virt(reset_system, reset_type, status, data_size, data);
-	spin_unlock(&efi_runtime_lock);
+	up(&efi_runtime_lock);
 }
 
 static efi_status_t virt_efi_update_capsule(efi_capsule_header_t **capsules,
@@ -255,9 +270,10 @@ static efi_status_t virt_efi_update_capsule(efi_capsule_header_t **capsules,
 	if (efi.runtime_version < EFI_2_00_SYSTEM_TABLE_REVISION)
 		return EFI_UNSUPPORTED;
 
-	spin_lock(&efi_runtime_lock);
+	if (down_interruptible(&efi_runtime_lock))
+		return EFI_ABORTED;
 	status = efi_call_virt(update_capsule, capsules, count, sg_list);
-	spin_unlock(&efi_runtime_lock);
+	up(&efi_runtime_lock);
 	return status;
 }
 
@@ -271,10 +287,11 @@ static efi_status_t virt_efi_query_capsule_caps(efi_capsule_header_t **capsules,
 	if (efi.runtime_version < EFI_2_00_SYSTEM_TABLE_REVISION)
 		return EFI_UNSUPPORTED;
 
-	spin_lock(&efi_runtime_lock);
+	if (down_interruptible(&efi_runtime_lock))
+		return EFI_ABORTED;
 	status = efi_call_virt(query_capsule_caps, capsules, count, max_size,
 			       reset_type);
-	spin_unlock(&efi_runtime_lock);
+	up(&efi_runtime_lock);
 	return status;
 }
 
diff --git a/drivers/firmware/efi/test/Makefile b/drivers/firmware/efi/test/Makefile
new file mode 100644
index 000000000000..bcd4577d40e6
--- /dev/null
+++ b/drivers/firmware/efi/test/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_EFI_TEST)			+= efi_test.o
diff --git a/drivers/firmware/efi/test/efi_test.c b/drivers/firmware/efi/test/efi_test.c
new file mode 100644
index 000000000000..f61bb52be318
--- /dev/null
+++ b/drivers/firmware/efi/test/efi_test.c
@@ -0,0 +1,749 @@
+/*
+ * EFI Test Driver for Runtime Services
+ *
+ * Copyright(C) 2012-2016 Canonical Ltd.
+ *
+ * This driver exports EFI runtime services interfaces into userspace, which
+ * allow to use and test UEFI runtime services provided by firmware.
+ *
+ */
+
+#include <linux/version.h>
+#include <linux/miscdevice.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/proc_fs.h>
+#include <linux/efi.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+
+#include "efi_test.h"
+
+MODULE_AUTHOR("Ivan Hu <ivan.hu@canonical.com>");
+MODULE_DESCRIPTION("EFI Test Driver");
+MODULE_LICENSE("GPL");
+
+/*
+ * Count the bytes in 'str', including the terminating NULL.
+ *
+ * Note this function returns the number of *bytes*, not the number of
+ * ucs2 characters.
+ */
+static inline size_t user_ucs2_strsize(efi_char16_t  __user *str)
+{
+	efi_char16_t *s = str, c;
+	size_t len;
+
+	if (!str)
+		return 0;
+
+	/* Include terminating NULL */
+	len = sizeof(efi_char16_t);
+
+	if (get_user(c, s++)) {
+		/* Can't read userspace memory for size */
+		return 0;
+	}
+
+	while (c != 0) {
+		if (get_user(c, s++)) {
+			/* Can't read userspace memory for size */
+			return 0;
+		}
+		len += sizeof(efi_char16_t);
+	}
+	return len;
+}
+
+/*
+ * Allocate a buffer and copy a ucs2 string from user space into it.
+ */
+static inline int
+copy_ucs2_from_user_len(efi_char16_t **dst, efi_char16_t __user *src,
+			size_t len)
+{
+	efi_char16_t *buf;
+
+	if (!src) {
+		*dst = NULL;
+		return 0;
+	}
+
+	if (!access_ok(VERIFY_READ, src, 1))
+		return -EFAULT;
+
+	buf = kmalloc(len, GFP_KERNEL);
+	if (!buf) {
+		*dst = NULL;
+		return -ENOMEM;
+	}
+	*dst = buf;
+
+	if (copy_from_user(*dst, src, len)) {
+		kfree(buf);
+		return -EFAULT;
+	}
+
+	return 0;
+}
+
+/*
+ * Count the bytes in 'str', including the terminating NULL.
+ *
+ * Just a wrap for user_ucs2_strsize
+ */
+static inline int
+get_ucs2_strsize_from_user(efi_char16_t __user *src, size_t *len)
+{
+	if (!access_ok(VERIFY_READ, src, 1))
+		return -EFAULT;
+
+	*len = user_ucs2_strsize(src);
+	if (*len == 0)
+		return -EFAULT;
+
+	return 0;
+}
+
+/*
+ * Calculate the required buffer allocation size and copy a ucs2 string
+ * from user space into it.
+ *
+ * This function differs from copy_ucs2_from_user_len() because it
+ * calculates the size of the buffer to allocate by taking the length of
+ * the string 'src'.
+ *
+ * If a non-zero value is returned, the caller MUST NOT access 'dst'.
+ *
+ * It is the caller's responsibility to free 'dst'.
+ */
+static inline int
+copy_ucs2_from_user(efi_char16_t **dst, efi_char16_t __user *src)
+{
+	size_t len;
+
+	if (!access_ok(VERIFY_READ, src, 1))
+		return -EFAULT;
+
+	len = user_ucs2_strsize(src);
+	if (len == 0)
+		return -EFAULT;
+	return copy_ucs2_from_user_len(dst, src, len);
+}
+
+/*
+ * Copy a ucs2 string to a user buffer.
+ *
+ * This function is a simple wrapper around copy_to_user() that does
+ * nothing if 'src' is NULL, which is useful for reducing the amount of
+ * NULL checking the caller has to do.
+ *
+ * 'len' specifies the number of bytes to copy.
+ */
+static inline int
+copy_ucs2_to_user_len(efi_char16_t __user *dst, efi_char16_t *src, size_t len)
+{
+	if (!src)
+		return 0;
+
+	if (!access_ok(VERIFY_WRITE, dst, 1))
+		return -EFAULT;
+
+	return copy_to_user(dst, src, len);
+}
+
+static long efi_runtime_get_variable(unsigned long arg)
+{
+	struct efi_getvariable __user *getvariable_user;
+	struct efi_getvariable getvariable;
+	unsigned long datasize, prev_datasize, *dz;
+	efi_guid_t vendor_guid, *vd = NULL;
+	efi_status_t status;
+	efi_char16_t *name = NULL;
+	u32 attr, *at;
+	void *data = NULL;
+	int rv = 0;
+
+	getvariable_user = (struct efi_getvariable __user *)arg;
+
+	if (copy_from_user(&getvariable, getvariable_user,
+			   sizeof(getvariable)))
+		return -EFAULT;
+	if (getvariable.data_size &&
+	    get_user(datasize, getvariable.data_size))
+		return -EFAULT;
+	if (getvariable.vendor_guid) {
+		if (copy_from_user(&vendor_guid, getvariable.vendor_guid,
+					sizeof(vendor_guid)))
+			return -EFAULT;
+		vd = &vendor_guid;
+	}
+
+	if (getvariable.variable_name) {
+		rv = copy_ucs2_from_user(&name, getvariable.variable_name);
+		if (rv)
+			return rv;
+	}
+
+	at = getvariable.attributes ? &attr : NULL;
+	dz = getvariable.data_size ? &datasize : NULL;
+
+	if (getvariable.data_size && getvariable.data) {
+		data = kmalloc(datasize, GFP_KERNEL);
+		if (!data) {
+			kfree(name);
+			return -ENOMEM;
+		}
+	}
+
+	prev_datasize = datasize;
+	status = efi.get_variable(name, vd, at, dz, data);
+	kfree(name);
+
+	if (put_user(status, getvariable.status)) {
+		rv = -EFAULT;
+		goto out;
+	}
+
+	if (status != EFI_SUCCESS) {
+		if (status == EFI_BUFFER_TOO_SMALL) {
+			if (dz && put_user(datasize, getvariable.data_size)) {
+				rv = -EFAULT;
+				goto out;
+			}
+		}
+		rv = -EINVAL;
+		goto out;
+	}
+
+	if (prev_datasize < datasize) {
+		rv = -EINVAL;
+		goto out;
+	}
+
+	if (data) {
+		if (copy_to_user(getvariable.data, data, datasize)) {
+			rv = -EFAULT;
+			goto out;
+		}
+	}
+
+	if (at && put_user(attr, getvariable.attributes)) {
+		rv = -EFAULT;
+		goto out;
+	}
+
+	if (dz && put_user(datasize, getvariable.data_size))
+		rv = -EFAULT;
+
+out:
+	kfree(data);
+	return rv;
+
+}
+
+static long efi_runtime_set_variable(unsigned long arg)
+{
+	struct efi_setvariable __user *setvariable_user;
+	struct efi_setvariable setvariable;
+	efi_guid_t vendor_guid;
+	efi_status_t status;
+	efi_char16_t *name = NULL;
+	void *data;
+	int rv = 0;
+
+	setvariable_user = (struct efi_setvariable __user *)arg;
+
+	if (copy_from_user(&setvariable, setvariable_user, sizeof(setvariable)))
+		return -EFAULT;
+	if (copy_from_user(&vendor_guid, setvariable.vendor_guid,
+				sizeof(vendor_guid)))
+		return -EFAULT;
+
+	if (setvariable.variable_name) {
+		rv = copy_ucs2_from_user(&name, setvariable.variable_name);
+		if (rv)
+			return rv;
+	}
+
+	data = kmalloc(setvariable.data_size, GFP_KERNEL);
+	if (!data) {
+		kfree(name);
+		return -ENOMEM;
+	}
+	if (copy_from_user(data, setvariable.data, setvariable.data_size)) {
+		rv = -EFAULT;
+		goto out;
+	}
+
+	status = efi.set_variable(name, &vendor_guid,
+				setvariable.attributes,
+				setvariable.data_size, data);
+
+	if (put_user(status, setvariable.status)) {
+		rv = -EFAULT;
+		goto out;
+	}
+
+	rv = status == EFI_SUCCESS ? 0 : -EINVAL;
+
+out:
+	kfree(data);
+	kfree(name);
+
+	return rv;
+}
+
+static long efi_runtime_get_time(unsigned long arg)
+{
+	struct efi_gettime __user *gettime_user;
+	struct efi_gettime  gettime;
+	efi_status_t status;
+	efi_time_cap_t cap;
+	efi_time_t efi_time;
+
+	gettime_user = (struct efi_gettime __user *)arg;
+	if (copy_from_user(&gettime, gettime_user, sizeof(gettime)))
+		return -EFAULT;
+
+	status = efi.get_time(gettime.time ? &efi_time : NULL,
+			      gettime.capabilities ? &cap : NULL);
+
+	if (put_user(status, gettime.status))
+		return -EFAULT;
+
+	if (status != EFI_SUCCESS)
+		return -EINVAL;
+
+	if (gettime.capabilities) {
+		efi_time_cap_t __user *cap_local;
+
+		cap_local = (efi_time_cap_t *)gettime.capabilities;
+		if (put_user(cap.resolution, &(cap_local->resolution)) ||
+			put_user(cap.accuracy, &(cap_local->accuracy)) ||
+			put_user(cap.sets_to_zero, &(cap_local->sets_to_zero)))
+			return -EFAULT;
+	}
+	if (gettime.time) {
+		if (copy_to_user(gettime.time, &efi_time, sizeof(efi_time_t)))
+			return -EFAULT;
+	}
+
+	return 0;
+}
+
+static long efi_runtime_set_time(unsigned long arg)
+{
+	struct efi_settime __user *settime_user;
+	struct efi_settime settime;
+	efi_status_t status;
+	efi_time_t efi_time;
+
+	settime_user = (struct efi_settime __user *)arg;
+	if (copy_from_user(&settime, settime_user, sizeof(settime)))
+		return -EFAULT;
+	if (copy_from_user(&efi_time, settime.time,
+					sizeof(efi_time_t)))
+		return -EFAULT;
+	status = efi.set_time(&efi_time);
+
+	if (put_user(status, settime.status))
+		return -EFAULT;
+
+	return status == EFI_SUCCESS ? 0 : -EINVAL;
+}
+
+static long efi_runtime_get_waketime(unsigned long arg)
+{
+	struct efi_getwakeuptime __user *getwakeuptime_user;
+	struct efi_getwakeuptime getwakeuptime;
+	efi_bool_t enabled, pending;
+	efi_status_t status;
+	efi_time_t efi_time;
+
+	getwakeuptime_user = (struct efi_getwakeuptime __user *)arg;
+	if (copy_from_user(&getwakeuptime, getwakeuptime_user,
+				sizeof(getwakeuptime)))
+		return -EFAULT;
+
+	status = efi.get_wakeup_time(
+		getwakeuptime.enabled ? (efi_bool_t *)&enabled : NULL,
+		getwakeuptime.pending ? (efi_bool_t *)&pending : NULL,
+		getwakeuptime.time ? &efi_time : NULL);
+
+	if (put_user(status, getwakeuptime.status))
+		return -EFAULT;
+
+	if (status != EFI_SUCCESS)
+		return -EINVAL;
+
+	if (getwakeuptime.enabled && put_user(enabled,
+						getwakeuptime.enabled))
+		return -EFAULT;
+
+	if (getwakeuptime.time) {
+		if (copy_to_user(getwakeuptime.time, &efi_time,
+				sizeof(efi_time_t)))
+			return -EFAULT;
+	}
+
+	return 0;
+}
+
+static long efi_runtime_set_waketime(unsigned long arg)
+{
+	struct efi_setwakeuptime __user *setwakeuptime_user;
+	struct efi_setwakeuptime setwakeuptime;
+	efi_bool_t enabled;
+	efi_status_t status;
+	efi_time_t efi_time;
+
+	setwakeuptime_user = (struct efi_setwakeuptime __user *)arg;
+
+	if (copy_from_user(&setwakeuptime, setwakeuptime_user,
+				sizeof(setwakeuptime)))
+		return -EFAULT;
+
+	enabled = setwakeuptime.enabled;
+	if (setwakeuptime.time) {
+		if (copy_from_user(&efi_time, setwakeuptime.time,
+					sizeof(efi_time_t)))
+			return -EFAULT;
+
+		status = efi.set_wakeup_time(enabled, &efi_time);
+	} else
+		status = efi.set_wakeup_time(enabled, NULL);
+
+	if (put_user(status, setwakeuptime.status))
+		return -EFAULT;
+
+	return status == EFI_SUCCESS ? 0 : -EINVAL;
+}
+
+static long efi_runtime_get_nextvariablename(unsigned long arg)
+{
+	struct efi_getnextvariablename __user *getnextvariablename_user;
+	struct efi_getnextvariablename getnextvariablename;
+	unsigned long name_size, prev_name_size = 0, *ns = NULL;
+	efi_status_t status;
+	efi_guid_t *vd = NULL;
+	efi_guid_t vendor_guid;
+	efi_char16_t *name = NULL;
+	int rv;
+
+	getnextvariablename_user = (struct efi_getnextvariablename __user *)arg;
+
+	if (copy_from_user(&getnextvariablename, getnextvariablename_user,
+			   sizeof(getnextvariablename)))
+		return -EFAULT;
+
+	if (getnextvariablename.variable_name_size) {
+		if (get_user(name_size, getnextvariablename.variable_name_size))
+			return -EFAULT;
+		ns = &name_size;
+		prev_name_size = name_size;
+	}
+
+	if (getnextvariablename.vendor_guid) {
+		if (copy_from_user(&vendor_guid,
+				getnextvariablename.vendor_guid,
+				sizeof(vendor_guid)))
+			return -EFAULT;
+		vd = &vendor_guid;
+	}
+
+	if (getnextvariablename.variable_name) {
+		size_t name_string_size = 0;
+
+		rv = get_ucs2_strsize_from_user(
+				getnextvariablename.variable_name,
+				&name_string_size);
+		if (rv)
+			return rv;
+		/*
+		 * The name_size may be smaller than the real buffer size where
+		 * variable name located in some use cases. The most typical
+		 * case is passing a 0 to get the required buffer size for the
+		 * 1st time call. So we need to copy the content from user
+		 * space for at least the string size of variable name, or else
+		 * the name passed to UEFI may not be terminated as we expected.
+		 */
+		rv = copy_ucs2_from_user_len(&name,
+				getnextvariablename.variable_name,
+				prev_name_size > name_string_size ?
+				prev_name_size : name_string_size);
+		if (rv)
+			return rv;
+	}
+
+	status = efi.get_next_variable(ns, name, vd);
+
+	if (put_user(status, getnextvariablename.status)) {
+		rv = -EFAULT;
+		goto out;
+	}
+
+	if (status != EFI_SUCCESS) {
+		if (status == EFI_BUFFER_TOO_SMALL) {
+			if (ns && put_user(*ns,
+				getnextvariablename.variable_name_size)) {
+				rv = -EFAULT;
+				goto out;
+			}
+		}
+		rv = -EINVAL;
+		goto out;
+	}
+
+	if (name) {
+		if (copy_ucs2_to_user_len(getnextvariablename.variable_name,
+						name, prev_name_size)) {
+			rv = -EFAULT;
+			goto out;
+		}
+	}
+
+	if (ns) {
+		if (put_user(*ns, getnextvariablename.variable_name_size)) {
+			rv = -EFAULT;
+			goto out;
+		}
+	}
+
+	if (vd) {
+		if (copy_to_user(getnextvariablename.vendor_guid, vd,
+							sizeof(efi_guid_t)))
+			rv = -EFAULT;
+	}
+
+out:
+	kfree(name);
+	return rv;
+}
+
+static long efi_runtime_get_nexthighmonocount(unsigned long arg)
+{
+	struct efi_getnexthighmonotoniccount __user *getnexthighmonocount_user;
+	struct efi_getnexthighmonotoniccount getnexthighmonocount;
+	efi_status_t status;
+	u32 count;
+
+	getnexthighmonocount_user = (struct
+			efi_getnexthighmonotoniccount __user *)arg;
+
+	if (copy_from_user(&getnexthighmonocount,
+			   getnexthighmonocount_user,
+			   sizeof(getnexthighmonocount)))
+		return -EFAULT;
+
+	status = efi.get_next_high_mono_count(
+		getnexthighmonocount.high_count ? &count : NULL);
+
+	if (put_user(status, getnexthighmonocount.status))
+		return -EFAULT;
+
+	if (status != EFI_SUCCESS)
+		return -EINVAL;
+
+	if (getnexthighmonocount.high_count &&
+	    put_user(count, getnexthighmonocount.high_count))
+		return -EFAULT;
+
+	return 0;
+}
+
+static long efi_runtime_query_variableinfo(unsigned long arg)
+{
+	struct efi_queryvariableinfo __user *queryvariableinfo_user;
+	struct efi_queryvariableinfo queryvariableinfo;
+	efi_status_t status;
+	u64 max_storage, remaining, max_size;
+
+	queryvariableinfo_user = (struct efi_queryvariableinfo __user *)arg;
+
+	if (copy_from_user(&queryvariableinfo, queryvariableinfo_user,
+			   sizeof(queryvariableinfo)))
+		return -EFAULT;
+
+	status = efi.query_variable_info(queryvariableinfo.attributes,
+					 &max_storage, &remaining, &max_size);
+
+	if (put_user(status, queryvariableinfo.status))
+		return -EFAULT;
+
+	if (status != EFI_SUCCESS)
+		return -EINVAL;
+
+	if (put_user(max_storage,
+		     queryvariableinfo.maximum_variable_storage_size))
+		return -EFAULT;
+
+	if (put_user(remaining,
+		     queryvariableinfo.remaining_variable_storage_size))
+		return -EFAULT;
+
+	if (put_user(max_size, queryvariableinfo.maximum_variable_size))
+		return -EFAULT;
+
+	return 0;
+}
+
+static long efi_runtime_query_capsulecaps(unsigned long arg)
+{
+	struct efi_querycapsulecapabilities __user *qcaps_user;
+	struct efi_querycapsulecapabilities qcaps;
+	efi_capsule_header_t *capsules;
+	efi_status_t status;
+	u64 max_size;
+	int i, reset_type;
+	int rv = 0;
+
+	qcaps_user = (struct efi_querycapsulecapabilities __user *)arg;
+
+	if (copy_from_user(&qcaps, qcaps_user, sizeof(qcaps)))
+		return -EFAULT;
+
+	capsules = kcalloc(qcaps.capsule_count + 1,
+			   sizeof(efi_capsule_header_t), GFP_KERNEL);
+	if (!capsules)
+		return -ENOMEM;
+
+	for (i = 0; i < qcaps.capsule_count; i++) {
+		efi_capsule_header_t *c;
+		/*
+		 * We cannot dereference qcaps.capsule_header_array directly to
+		 * obtain the address of the capsule as it resides in the
+		 * user space
+		 */
+		if (get_user(c, qcaps.capsule_header_array + i)) {
+			rv = -EFAULT;
+			goto out;
+		}
+		if (copy_from_user(&capsules[i], c,
+				sizeof(efi_capsule_header_t))) {
+			rv = -EFAULT;
+			goto out;
+		}
+	}
+
+	qcaps.capsule_header_array = &capsules;
+
+	status = efi.query_capsule_caps((efi_capsule_header_t **)
+					qcaps.capsule_header_array,
+					qcaps.capsule_count,
+					&max_size, &reset_type);
+
+	if (put_user(status, qcaps.status)) {
+		rv = -EFAULT;
+		goto out;
+	}
+
+	if (status != EFI_SUCCESS) {
+		rv = -EINVAL;
+		goto out;
+	}
+
+	if (put_user(max_size, qcaps.maximum_capsule_size)) {
+		rv = -EFAULT;
+		goto out;
+	}
+
+	if (put_user(reset_type, qcaps.reset_type))
+		rv = -EFAULT;
+
+out:
+	kfree(capsules);
+	return rv;
+}
+
+static long efi_test_ioctl(struct file *file, unsigned int cmd,
+							unsigned long arg)
+{
+	switch (cmd) {
+	case EFI_RUNTIME_GET_VARIABLE:
+		return efi_runtime_get_variable(arg);
+
+	case EFI_RUNTIME_SET_VARIABLE:
+		return efi_runtime_set_variable(arg);
+
+	case EFI_RUNTIME_GET_TIME:
+		return efi_runtime_get_time(arg);
+
+	case EFI_RUNTIME_SET_TIME:
+		return efi_runtime_set_time(arg);
+
+	case EFI_RUNTIME_GET_WAKETIME:
+		return efi_runtime_get_waketime(arg);
+
+	case EFI_RUNTIME_SET_WAKETIME:
+		return efi_runtime_set_waketime(arg);
+
+	case EFI_RUNTIME_GET_NEXTVARIABLENAME:
+		return efi_runtime_get_nextvariablename(arg);
+
+	case EFI_RUNTIME_GET_NEXTHIGHMONOTONICCOUNT:
+		return efi_runtime_get_nexthighmonocount(arg);
+
+	case EFI_RUNTIME_QUERY_VARIABLEINFO:
+		return efi_runtime_query_variableinfo(arg);
+
+	case EFI_RUNTIME_QUERY_CAPSULECAPABILITIES:
+		return efi_runtime_query_capsulecaps(arg);
+	}
+
+	return -ENOTTY;
+}
+
+static int efi_test_open(struct inode *inode, struct file *file)
+{
+	/*
+	 * nothing special to do here
+	 * We do accept multiple open files at the same time as we
+	 * synchronize on the per call operation.
+	 */
+	return 0;
+}
+
+static int efi_test_close(struct inode *inode, struct file *file)
+{
+	return 0;
+}
+
+/*
+ *	The various file operations we support.
+ */
+static const struct file_operations efi_test_fops = {
+	.owner		= THIS_MODULE,
+	.unlocked_ioctl	= efi_test_ioctl,
+	.open		= efi_test_open,
+	.release	= efi_test_close,
+	.llseek		= no_llseek,
+};
+
+static struct miscdevice efi_test_dev = {
+	MISC_DYNAMIC_MINOR,
+	"efi_test",
+	&efi_test_fops
+};
+
+static int __init efi_test_init(void)
+{
+	int ret;
+
+	ret = misc_register(&efi_test_dev);
+	if (ret) {
+		pr_err("efi_test: can't misc_register on minor=%d\n",
+			MISC_DYNAMIC_MINOR);
+		return ret;
+	}
+
+	return 0;
+}
+
+static void __exit efi_test_exit(void)
+{
+	misc_deregister(&efi_test_dev);
+}
+
+module_init(efi_test_init);
+module_exit(efi_test_exit);
diff --git a/drivers/firmware/efi/test/efi_test.h b/drivers/firmware/efi/test/efi_test.h
new file mode 100644
index 000000000000..a33a6c633852
--- /dev/null
+++ b/drivers/firmware/efi/test/efi_test.h
@@ -0,0 +1,110 @@
+/*
+ * EFI Test driver Header
+ *
+ * Copyright(C) 2012-2016 Canonical Ltd.
+ *
+ */
+
+#ifndef _DRIVERS_FIRMWARE_EFI_TEST_H_
+#define _DRIVERS_FIRMWARE_EFI_TEST_H_
+
+#include <linux/efi.h>
+
+struct efi_getvariable {
+	efi_char16_t	*variable_name;
+	efi_guid_t	*vendor_guid;
+	u32		*attributes;
+	unsigned long	*data_size;
+	void		*data;
+	efi_status_t	*status;
+} __packed;
+
+struct efi_setvariable {
+	efi_char16_t	*variable_name;
+	efi_guid_t	*vendor_guid;
+	u32		attributes;
+	unsigned long	data_size;
+	void		*data;
+	efi_status_t	*status;
+} __packed;
+
+struct efi_getnextvariablename {
+	unsigned long	*variable_name_size;
+	efi_char16_t	*variable_name;
+	efi_guid_t	*vendor_guid;
+	efi_status_t	*status;
+} __packed;
+
+struct efi_queryvariableinfo {
+	u32		attributes;
+	u64		*maximum_variable_storage_size;
+	u64		*remaining_variable_storage_size;
+	u64		*maximum_variable_size;
+	efi_status_t	*status;
+} __packed;
+
+struct efi_gettime {
+	efi_time_t	*time;
+	efi_time_cap_t	*capabilities;
+	efi_status_t	*status;
+} __packed;
+
+struct efi_settime {
+	efi_time_t	*time;
+	efi_status_t	*status;
+} __packed;
+
+struct efi_getwakeuptime {
+	efi_bool_t	*enabled;
+	efi_bool_t	*pending;
+	efi_time_t	*time;
+	efi_status_t	*status;
+} __packed;
+
+struct efi_setwakeuptime {
+	efi_bool_t	enabled;
+	efi_time_t	*time;
+	efi_status_t	*status;
+} __packed;
+
+struct efi_getnexthighmonotoniccount {
+	u32		*high_count;
+	efi_status_t	*status;
+} __packed;
+
+struct efi_querycapsulecapabilities {
+	efi_capsule_header_t	**capsule_header_array;
+	unsigned long		capsule_count;
+	u64			*maximum_capsule_size;
+	int			*reset_type;
+	efi_status_t		*status;
+} __packed;
+
+#define EFI_RUNTIME_GET_VARIABLE \
+	_IOWR('p', 0x01, struct efi_getvariable)
+#define EFI_RUNTIME_SET_VARIABLE \
+	_IOW('p', 0x02, struct efi_setvariable)
+
+#define EFI_RUNTIME_GET_TIME \
+	_IOR('p', 0x03, struct efi_gettime)
+#define EFI_RUNTIME_SET_TIME \
+	_IOW('p', 0x04, struct efi_settime)
+
+#define EFI_RUNTIME_GET_WAKETIME \
+	_IOR('p', 0x05, struct efi_getwakeuptime)
+#define EFI_RUNTIME_SET_WAKETIME \
+	_IOW('p', 0x06, struct efi_setwakeuptime)
+
+#define EFI_RUNTIME_GET_NEXTVARIABLENAME \
+	_IOWR('p', 0x07, struct efi_getnextvariablename)
+
+#define EFI_RUNTIME_QUERY_VARIABLEINFO \
+	_IOR('p', 0x08, struct efi_queryvariableinfo)
+
+#define EFI_RUNTIME_GET_NEXTHIGHMONOTONICCOUNT \
+	_IOR('p', 0x09, struct efi_getnexthighmonotoniccount)
+
+#define EFI_RUNTIME_QUERY_CAPSULECAPABILITIES \
+	_IOR('p', 0x0A, struct efi_querycapsulecapabilities)
+
+#endif /* _DRIVERS_FIRMWARE_EFI_TEST_H_ */
diff --git a/drivers/firmware/efi/vars.c b/drivers/firmware/efi/vars.c
index d3b751383286..9336ffdf6e2c 100644
--- a/drivers/firmware/efi/vars.c
+++ b/drivers/firmware/efi/vars.c
@@ -37,6 +37,14 @@
 /* Private pointer to registered efivars */
 static struct efivars *__efivars;
 
+/*
+ * efivars_lock protects three things:
+ * 1) efivarfs_list and efivars_sysfs_list
+ * 2) ->ops calls
+ * 3) (un)registration of __efivars
+ */
+static DEFINE_SEMAPHORE(efivars_lock);
+
 static bool efivar_wq_enabled = true;
 DECLARE_WORK(efivar_work, NULL);
 EXPORT_SYMBOL_GPL(efivar_work);
@@ -434,7 +442,10 @@ int efivar_init(int (*func)(efi_char16_t *, efi_guid_t, unsigned long, void *),
 		return -ENOMEM;
 	}
 
-	spin_lock_irq(&__efivars->lock);
+	if (down_interruptible(&efivars_lock)) {
+		err = -EINTR;
+		goto free;
+	}
 
 	/*
 	 * Per EFI spec, the maximum storage allocated for both
@@ -450,7 +461,7 @@ int efivar_init(int (*func)(efi_char16_t *, efi_guid_t, unsigned long, void *),
 		switch (status) {
 		case EFI_SUCCESS:
 			if (duplicates)
-				spin_unlock_irq(&__efivars->lock);
+				up(&efivars_lock);
 
 			variable_name_size = var_name_strnsize(variable_name,
 							       variable_name_size);
@@ -476,8 +487,12 @@ int efivar_init(int (*func)(efi_char16_t *, efi_guid_t, unsigned long, void *),
 					status = EFI_NOT_FOUND;
 			}
 
-			if (duplicates)
-				spin_lock_irq(&__efivars->lock);
+			if (duplicates) {
+				if (down_interruptible(&efivars_lock)) {
+					err = -EINTR;
+					goto free;
+				}
+			}
 
 			break;
 		case EFI_NOT_FOUND:
@@ -491,8 +506,8 @@ int efivar_init(int (*func)(efi_char16_t *, efi_guid_t, unsigned long, void *),
 
 	} while (status != EFI_NOT_FOUND);
 
-	spin_unlock_irq(&__efivars->lock);
-
+	up(&efivars_lock);
+free:
 	kfree(variable_name);
 
 	return err;
@@ -503,24 +518,34 @@ EXPORT_SYMBOL_GPL(efivar_init);
  * efivar_entry_add - add entry to variable list
  * @entry: entry to add to list
  * @head: list head
+ *
+ * Returns 0 on success, or a kernel error code on failure.
  */
-void efivar_entry_add(struct efivar_entry *entry, struct list_head *head)
+int efivar_entry_add(struct efivar_entry *entry, struct list_head *head)
 {
-	spin_lock_irq(&__efivars->lock);
+	if (down_interruptible(&efivars_lock))
+		return -EINTR;
 	list_add(&entry->list, head);
-	spin_unlock_irq(&__efivars->lock);
+	up(&efivars_lock);
+
+	return 0;
 }
 EXPORT_SYMBOL_GPL(efivar_entry_add);
 
 /**
  * efivar_entry_remove - remove entry from variable list
  * @entry: entry to remove from list
+ *
+ * Returns 0 on success, or a kernel error code on failure.
  */
-void efivar_entry_remove(struct efivar_entry *entry)
+int efivar_entry_remove(struct efivar_entry *entry)
 {
-	spin_lock_irq(&__efivars->lock);
+	if (down_interruptible(&efivars_lock))
+		return -EINTR;
 	list_del(&entry->list);
-	spin_unlock_irq(&__efivars->lock);
+	up(&efivars_lock);
+
+	return 0;
 }
 EXPORT_SYMBOL_GPL(efivar_entry_remove);
 
@@ -537,10 +562,8 @@ EXPORT_SYMBOL_GPL(efivar_entry_remove);
  */
 static void efivar_entry_list_del_unlock(struct efivar_entry *entry)
 {
-	lockdep_assert_held(&__efivars->lock);
-
 	list_del(&entry->list);
-	spin_unlock_irq(&__efivars->lock);
+	up(&efivars_lock);
 }
 
 /**
@@ -563,8 +586,6 @@ int __efivar_entry_delete(struct efivar_entry *entry)
 	const struct efivar_operations *ops = __efivars->ops;
 	efi_status_t status;
 
-	lockdep_assert_held(&__efivars->lock);
-
 	status = ops->set_variable(entry->var.VariableName,
 				   &entry->var.VendorGuid,
 				   0, 0, NULL);
@@ -581,20 +602,22 @@ EXPORT_SYMBOL_GPL(__efivar_entry_delete);
  * variable list. It is the caller's responsibility to free @entry
  * once we return.
  *
- * Returns 0 on success, or a converted EFI status code if
- * set_variable() fails.
+ * Returns 0 on success, -EINTR if we can't grab the semaphore,
+ * converted EFI status code if set_variable() fails.
  */
 int efivar_entry_delete(struct efivar_entry *entry)
 {
 	const struct efivar_operations *ops = __efivars->ops;
 	efi_status_t status;
 
-	spin_lock_irq(&__efivars->lock);
+	if (down_interruptible(&efivars_lock))
+		return -EINTR;
+
 	status = ops->set_variable(entry->var.VariableName,
 				   &entry->var.VendorGuid,
 				   0, 0, NULL);
 	if (!(status == EFI_SUCCESS || status == EFI_NOT_FOUND)) {
-		spin_unlock_irq(&__efivars->lock);
+		up(&efivars_lock);
 		return efi_status_to_err(status);
 	}
 
@@ -620,9 +643,9 @@ EXPORT_SYMBOL_GPL(efivar_entry_delete);
  * If @head is not NULL a lookup is performed to determine whether
  * the entry is already on the list.
  *
- * Returns 0 on success, -EEXIST if a lookup is performed and the entry
- * already exists on the list, or a converted EFI status code if
- * set_variable() fails.
+ * Returns 0 on success, -EINTR if we can't grab the semaphore,
+ * -EEXIST if a lookup is performed and the entry already exists on
+ * the list, or a converted EFI status code if set_variable() fails.
  */
 int efivar_entry_set(struct efivar_entry *entry, u32 attributes,
 		     unsigned long size, void *data, struct list_head *head)
@@ -632,10 +655,10 @@ int efivar_entry_set(struct efivar_entry *entry, u32 attributes,
 	efi_char16_t *name = entry->var.VariableName;
 	efi_guid_t vendor = entry->var.VendorGuid;
 
-	spin_lock_irq(&__efivars->lock);
-
+	if (down_interruptible(&efivars_lock))
+		return -EINTR;
 	if (head && efivar_entry_find(name, vendor, head, false)) {
-		spin_unlock_irq(&__efivars->lock);
+		up(&efivars_lock);
 		return -EEXIST;
 	}
 
@@ -644,7 +667,7 @@ int efivar_entry_set(struct efivar_entry *entry, u32 attributes,
 		status = ops->set_variable(name, &vendor,
 					   attributes, size, data);
 
-	spin_unlock_irq(&__efivars->lock);
+	up(&efivars_lock);
 
 	return efi_status_to_err(status);
 
@@ -658,30 +681,29 @@ EXPORT_SYMBOL_GPL(efivar_entry_set);
  * from crash/panic handlers.
  *
  * Crucially, this function will not block if it cannot acquire
- * __efivars->lock. Instead, it returns -EBUSY.
+ * efivars_lock. Instead, it returns -EBUSY.
  */
 static int
 efivar_entry_set_nonblocking(efi_char16_t *name, efi_guid_t vendor,
 			     u32 attributes, unsigned long size, void *data)
 {
 	const struct efivar_operations *ops = __efivars->ops;
-	unsigned long flags;
 	efi_status_t status;
 
-	if (!spin_trylock_irqsave(&__efivars->lock, flags))
+	if (down_trylock(&efivars_lock))
 		return -EBUSY;
 
 	status = check_var_size_nonblocking(attributes,
 					    size + ucs2_strsize(name, 1024));
 	if (status != EFI_SUCCESS) {
-		spin_unlock_irqrestore(&__efivars->lock, flags);
+		up(&efivars_lock);
 		return -ENOSPC;
 	}
 
 	status = ops->set_variable_nonblocking(name, &vendor, attributes,
 					       size, data);
 
-	spin_unlock_irqrestore(&__efivars->lock, flags);
+	up(&efivars_lock);
 	return efi_status_to_err(status);
 }
 
@@ -706,7 +728,6 @@ int efivar_entry_set_safe(efi_char16_t *name, efi_guid_t vendor, u32 attributes,
 			  bool block, unsigned long size, void *data)
 {
 	const struct efivar_operations *ops = __efivars->ops;
-	unsigned long flags;
 	efi_status_t status;
 
 	if (!ops->query_variable_store)
@@ -727,21 +748,22 @@ int efivar_entry_set_safe(efi_char16_t *name, efi_guid_t vendor, u32 attributes,
 						    size, data);
 
 	if (!block) {
-		if (!spin_trylock_irqsave(&__efivars->lock, flags))
+		if (down_trylock(&efivars_lock))
 			return -EBUSY;
 	} else {
-		spin_lock_irqsave(&__efivars->lock, flags);
+		if (down_interruptible(&efivars_lock))
+			return -EINTR;
 	}
 
 	status = check_var_size(attributes, size + ucs2_strsize(name, 1024));
 	if (status != EFI_SUCCESS) {
-		spin_unlock_irqrestore(&__efivars->lock, flags);
+		up(&efivars_lock);
 		return -ENOSPC;
 	}
 
 	status = ops->set_variable(name, &vendor, attributes, size, data);
 
-	spin_unlock_irqrestore(&__efivars->lock, flags);
+	up(&efivars_lock);
 
 	return efi_status_to_err(status);
 }
@@ -771,8 +793,6 @@ struct efivar_entry *efivar_entry_find(efi_char16_t *name, efi_guid_t guid,
 	int strsize1, strsize2;
 	bool found = false;
 
-	lockdep_assert_held(&__efivars->lock);
-
 	list_for_each_entry_safe(entry, n, head, list) {
 		strsize1 = ucs2_strsize(name, 1024);
 		strsize2 = ucs2_strsize(entry->var.VariableName, 1024);
@@ -814,10 +834,11 @@ int efivar_entry_size(struct efivar_entry *entry, unsigned long *size)
 
 	*size = 0;
 
-	spin_lock_irq(&__efivars->lock);
+	if (down_interruptible(&efivars_lock))
+		return -EINTR;
 	status = ops->get_variable(entry->var.VariableName,
 				   &entry->var.VendorGuid, NULL, size, NULL);
-	spin_unlock_irq(&__efivars->lock);
+	up(&efivars_lock);
 
 	if (status != EFI_BUFFER_TOO_SMALL)
 		return efi_status_to_err(status);
@@ -843,8 +864,6 @@ int __efivar_entry_get(struct efivar_entry *entry, u32 *attributes,
 	const struct efivar_operations *ops = __efivars->ops;
 	efi_status_t status;
 
-	lockdep_assert_held(&__efivars->lock);
-
 	status = ops->get_variable(entry->var.VariableName,
 				   &entry->var.VendorGuid,
 				   attributes, size, data);
@@ -866,11 +885,12 @@ int efivar_entry_get(struct efivar_entry *entry, u32 *attributes,
 	const struct efivar_operations *ops = __efivars->ops;
 	efi_status_t status;
 
-	spin_lock_irq(&__efivars->lock);
+	if (down_interruptible(&efivars_lock))
+		return -EINTR;
 	status = ops->get_variable(entry->var.VariableName,
 				   &entry->var.VendorGuid,
 				   attributes, size, data);
-	spin_unlock_irq(&__efivars->lock);
+	up(&efivars_lock);
 
 	return efi_status_to_err(status);
 }
@@ -917,7 +937,8 @@ int efivar_entry_set_get_size(struct efivar_entry *entry, u32 attributes,
 	 * set_variable call, and removal of the variable from the efivars
 	 * list (in the case of an authenticated delete).
 	 */
-	spin_lock_irq(&__efivars->lock);
+	if (down_interruptible(&efivars_lock))
+		return -EINTR;
 
 	/*
 	 * Ensure that the available space hasn't shrunk below the safe level
@@ -957,7 +978,7 @@ int efivar_entry_set_get_size(struct efivar_entry *entry, u32 attributes,
 	if (status == EFI_NOT_FOUND)
 		efivar_entry_list_del_unlock(entry);
 	else
-		spin_unlock_irq(&__efivars->lock);
+		up(&efivars_lock);
 
 	if (status && status != EFI_BUFFER_TOO_SMALL)
 		return efi_status_to_err(status);
@@ -965,7 +986,7 @@ int efivar_entry_set_get_size(struct efivar_entry *entry, u32 attributes,
 	return 0;
 
 out:
-	spin_unlock_irq(&__efivars->lock);
+	up(&efivars_lock);
 	return err;
 
 }
@@ -978,9 +999,9 @@ EXPORT_SYMBOL_GPL(efivar_entry_set_get_size);
  * efivar_entry_iter_end() is called. This function is usually used in
  * conjunction with __efivar_entry_iter() or efivar_entry_iter().
  */
-void efivar_entry_iter_begin(void)
+int efivar_entry_iter_begin(void)
 {
-	spin_lock_irq(&__efivars->lock);
+	return down_interruptible(&efivars_lock);
 }
 EXPORT_SYMBOL_GPL(efivar_entry_iter_begin);
 
@@ -991,7 +1012,7 @@ EXPORT_SYMBOL_GPL(efivar_entry_iter_begin);
  */
 void efivar_entry_iter_end(void)
 {
-	spin_unlock_irq(&__efivars->lock);
+	up(&efivars_lock);
 }
 EXPORT_SYMBOL_GPL(efivar_entry_iter_end);
 
@@ -1067,7 +1088,9 @@ int efivar_entry_iter(int (*func)(struct efivar_entry *, void *),
 {
 	int err = 0;
 
-	efivar_entry_iter_begin();
+	err = efivar_entry_iter_begin();
+	if (err)
+		return err;
 	err = __efivar_entry_iter(func, head, data, NULL);
 	efivar_entry_iter_end();
 
@@ -1112,12 +1135,18 @@ int efivars_register(struct efivars *efivars,
 		     const struct efivar_operations *ops,
 		     struct kobject *kobject)
 {
-	spin_lock_init(&efivars->lock);
+	if (down_interruptible(&efivars_lock))
+		return -EINTR;
+
 	efivars->ops = ops;
 	efivars->kobject = kobject;
 
 	__efivars = efivars;
 
+	pr_info("Registered efivars operations\n");
+
+	up(&efivars_lock);
+
 	return 0;
 }
 EXPORT_SYMBOL_GPL(efivars_register);
@@ -1133,6 +1162,9 @@ int efivars_unregister(struct efivars *efivars)
 {
 	int rv;
 
+	if (down_interruptible(&efivars_lock))
+		return -EINTR;
+
 	if (!__efivars) {
 		printk(KERN_ERR "efivars not registered\n");
 		rv = -EINVAL;
@@ -1144,10 +1176,12 @@ int efivars_unregister(struct efivars *efivars)
 		goto out;
 	}
 
+	pr_info("Unregistered efivars operations\n");
 	__efivars = NULL;
 
 	rv = 0;
 out:
+	up(&efivars_lock);
 	return rv;
 }
 EXPORT_SYMBOL_GPL(efivars_unregister);
diff --git a/drivers/firmware/google/gsmi.c b/drivers/firmware/google/gsmi.c
index f1ab05ea56bb..c46387160976 100644
--- a/drivers/firmware/google/gsmi.c
+++ b/drivers/firmware/google/gsmi.c
@@ -910,8 +910,7 @@ out_err:
 	gsmi_buf_free(gsmi_dev.param_buf);
 	gsmi_buf_free(gsmi_dev.data_buf);
 	gsmi_buf_free(gsmi_dev.name_buf);
-	if (gsmi_dev.dma_pool)
-		dma_pool_destroy(gsmi_dev.dma_pool);
+	dma_pool_destroy(gsmi_dev.dma_pool);
 	platform_device_unregister(gsmi_dev.pdev);
 	pr_info("gsmi: failed to load: %d\n", ret);
 	return ret;
diff --git a/drivers/firmware/meson/Kconfig b/drivers/firmware/meson/Kconfig
new file mode 100644
index 000000000000..170d7e8bcdfb
--- /dev/null
+++ b/drivers/firmware/meson/Kconfig
@@ -0,0 +1,9 @@
+#
+# Amlogic Secure Monitor driver
+#
+config MESON_SM
+	bool
+	default ARCH_MESON
+	depends on ARM64_4K_PAGES
+	help
+	  Say y here to enable the Amlogic secure monitor driver
diff --git a/drivers/firmware/meson/Makefile b/drivers/firmware/meson/Makefile
new file mode 100644
index 000000000000..9ab3884f96bc
--- /dev/null
+++ b/drivers/firmware/meson/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_MESON_SM) +=	meson_sm.o
diff --git a/drivers/firmware/meson/meson_sm.c b/drivers/firmware/meson/meson_sm.c
new file mode 100644
index 000000000000..b0d254930ed3
--- /dev/null
+++ b/drivers/firmware/meson/meson_sm.c
@@ -0,0 +1,248 @@
+/*
+ * Amlogic Secure Monitor driver
+ *
+ * Copyright (C) 2016 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 of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#define pr_fmt(fmt) "meson-sm: " fmt
+
+#include <linux/arm-smccc.h>
+#include <linux/bug.h>
+#include <linux/io.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/printk.h>
+#include <linux/types.h>
+#include <linux/sizes.h>
+
+#include <linux/firmware/meson/meson_sm.h>
+
+struct meson_sm_cmd {
+	unsigned int index;
+	u32 smc_id;
+};
+#define CMD(d, s) { .index = (d), .smc_id = (s), }
+
+struct meson_sm_chip {
+	unsigned int shmem_size;
+	u32 cmd_shmem_in_base;
+	u32 cmd_shmem_out_base;
+	struct meson_sm_cmd cmd[];
+};
+
+struct meson_sm_chip gxbb_chip = {
+	.shmem_size		= SZ_4K,
+	.cmd_shmem_in_base	= 0x82000020,
+	.cmd_shmem_out_base	= 0x82000021,
+	.cmd = {
+		CMD(SM_EFUSE_READ,	0x82000030),
+		CMD(SM_EFUSE_WRITE,	0x82000031),
+		CMD(SM_EFUSE_USER_MAX,	0x82000033),
+		{ /* sentinel */ },
+	},
+};
+
+struct meson_sm_firmware {
+	const struct meson_sm_chip *chip;
+	void __iomem *sm_shmem_in_base;
+	void __iomem *sm_shmem_out_base;
+};
+
+static struct meson_sm_firmware fw;
+
+static u32 meson_sm_get_cmd(const struct meson_sm_chip *chip,
+			    unsigned int cmd_index)
+{
+	const struct meson_sm_cmd *cmd = chip->cmd;
+
+	while (cmd->smc_id && cmd->index != cmd_index)
+		cmd++;
+
+	return cmd->smc_id;
+}
+
+static u32 __meson_sm_call(u32 cmd, u32 arg0, u32 arg1, u32 arg2,
+			   u32 arg3, u32 arg4)
+{
+	struct arm_smccc_res res;
+
+	arm_smccc_smc(cmd, arg0, arg1, arg2, arg3, arg4, 0, 0, &res);
+	return res.a0;
+}
+
+static void __iomem *meson_sm_map_shmem(u32 cmd_shmem, unsigned int size)
+{
+	u32 sm_phy_base;
+
+	sm_phy_base = __meson_sm_call(cmd_shmem, 0, 0, 0, 0, 0);
+	if (!sm_phy_base)
+		return 0;
+
+	return ioremap_cache(sm_phy_base, size);
+}
+
+/**
+ * meson_sm_call - generic SMC32 call to the secure-monitor
+ *
+ * @cmd_index:	Index of the SMC32 function ID
+ * @ret:	Returned value
+ * @arg0:	SMC32 Argument 0
+ * @arg1:	SMC32 Argument 1
+ * @arg2:	SMC32 Argument 2
+ * @arg3:	SMC32 Argument 3
+ * @arg4:	SMC32 Argument 4
+ *
+ * Return:	0 on success, a negative value on error
+ */
+int meson_sm_call(unsigned int cmd_index, u32 *ret, u32 arg0,
+		  u32 arg1, u32 arg2, u32 arg3, u32 arg4)
+{
+	u32 cmd, lret;
+
+	if (!fw.chip)
+		return -ENOENT;
+
+	cmd = meson_sm_get_cmd(fw.chip, cmd_index);
+	if (!cmd)
+		return -EINVAL;
+
+	lret = __meson_sm_call(cmd, arg0, arg1, arg2, arg3, arg4);
+
+	if (ret)
+		*ret = lret;
+
+	return 0;
+}
+EXPORT_SYMBOL(meson_sm_call);
+
+/**
+ * meson_sm_call_read - retrieve data from secure-monitor
+ *
+ * @buffer:	Buffer to store the retrieved data
+ * @cmd_index:	Index of the SMC32 function ID
+ * @arg0:	SMC32 Argument 0
+ * @arg1:	SMC32 Argument 1
+ * @arg2:	SMC32 Argument 2
+ * @arg3:	SMC32 Argument 3
+ * @arg4:	SMC32 Argument 4
+ *
+ * Return:	size of read data on success, a negative value on error
+ */
+int meson_sm_call_read(void *buffer, unsigned int cmd_index, u32 arg0,
+		       u32 arg1, u32 arg2, u32 arg3, u32 arg4)
+{
+	u32 size;
+
+	if (!fw.chip)
+		return -ENOENT;
+
+	if (!fw.chip->cmd_shmem_out_base)
+		return -EINVAL;
+
+	if (meson_sm_call(cmd_index, &size, arg0, arg1, arg2, arg3, arg4) < 0)
+		return -EINVAL;
+
+	if (!size || size > fw.chip->shmem_size)
+		return -EINVAL;
+
+	if (buffer)
+		memcpy(buffer, fw.sm_shmem_out_base, size);
+
+	return size;
+}
+EXPORT_SYMBOL(meson_sm_call_read);
+
+/**
+ * meson_sm_call_write - send data to secure-monitor
+ *
+ * @buffer:	Buffer containing data to send
+ * @size:	Size of the data to send
+ * @cmd_index:	Index of the SMC32 function ID
+ * @arg0:	SMC32 Argument 0
+ * @arg1:	SMC32 Argument 1
+ * @arg2:	SMC32 Argument 2
+ * @arg3:	SMC32 Argument 3
+ * @arg4:	SMC32 Argument 4
+ *
+ * Return:	size of sent data on success, a negative value on error
+ */
+int meson_sm_call_write(void *buffer, unsigned int size, unsigned int cmd_index,
+			u32 arg0, u32 arg1, u32 arg2, u32 arg3, u32 arg4)
+{
+	u32 written;
+
+	if (!fw.chip)
+		return -ENOENT;
+
+	if (size > fw.chip->shmem_size)
+		return -EINVAL;
+
+	if (!fw.chip->cmd_shmem_in_base)
+		return -EINVAL;
+
+	memcpy(fw.sm_shmem_in_base, buffer, size);
+
+	if (meson_sm_call(cmd_index, &written, arg0, arg1, arg2, arg3, arg4) < 0)
+		return -EINVAL;
+
+	if (!written)
+		return -EINVAL;
+
+	return written;
+}
+EXPORT_SYMBOL(meson_sm_call_write);
+
+static const struct of_device_id meson_sm_ids[] = {
+	{ .compatible = "amlogic,meson-gxbb-sm", .data = &gxbb_chip },
+	{ /* sentinel */ },
+};
+
+int __init meson_sm_init(void)
+{
+	const struct meson_sm_chip *chip;
+	const struct of_device_id *matched_np;
+	struct device_node *np;
+
+	np = of_find_matching_node_and_match(NULL, meson_sm_ids, &matched_np);
+	if (!np)
+		return -ENODEV;
+
+	chip = matched_np->data;
+	if (!chip) {
+		pr_err("unable to setup secure-monitor data\n");
+		goto out;
+	}
+
+	if (chip->cmd_shmem_in_base) {
+		fw.sm_shmem_in_base = meson_sm_map_shmem(chip->cmd_shmem_in_base,
+							 chip->shmem_size);
+		if (WARN_ON(!fw.sm_shmem_in_base))
+			goto out;
+	}
+
+	if (chip->cmd_shmem_out_base) {
+		fw.sm_shmem_out_base = meson_sm_map_shmem(chip->cmd_shmem_out_base,
+							  chip->shmem_size);
+		if (WARN_ON(!fw.sm_shmem_out_base))
+			goto out_in_base;
+	}
+
+	fw.chip = chip;
+	pr_info("secure-monitor enabled\n");
+
+	return 0;
+
+out_in_base:
+	iounmap(fw.sm_shmem_in_base);
+out:
+	return -EINVAL;
+}
+device_initcall(meson_sm_init);
diff --git a/drivers/firmware/qcom_scm.c b/drivers/firmware/qcom_scm.c
index e64a501adbf4..d95c70227c05 100644
--- a/drivers/firmware/qcom_scm.c
+++ b/drivers/firmware/qcom_scm.c
@@ -1,4 +1,7 @@
-/* Copyright (c) 2010,2015, The Linux Foundation. All rights reserved.
+/*
+ * Qualcomm SCM driver
+ *
+ * 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
@@ -12,7 +15,7 @@
  *
  */
 #include <linux/platform_device.h>
-#include <linux/module.h>
+#include <linux/init.h>
 #include <linux/cpumask.h>
 #include <linux/export.h>
 #include <linux/dma-mapping.h>
@@ -376,8 +379,6 @@ static const struct of_device_id qcom_scm_dt_match[] = {
 	{}
 };
 
-MODULE_DEVICE_TABLE(of, qcom_scm_dt_match);
-
 static struct platform_driver qcom_scm_driver = {
 	.driver = {
 		.name	= "qcom_scm",
@@ -414,14 +415,4 @@ static int __init qcom_scm_init(void)
 
 	return platform_driver_register(&qcom_scm_driver);
 }
-
 subsys_initcall(qcom_scm_init);
-
-static void __exit qcom_scm_exit(void)
-{
-	platform_driver_unregister(&qcom_scm_driver);
-}
-module_exit(qcom_scm_exit);
-
-MODULE_DESCRIPTION("Qualcomm SCM driver");
-MODULE_LICENSE("GPL v2");
diff --git a/drivers/fpga/Kconfig b/drivers/fpga/Kconfig
index d61410299ec0..cd84934774cc 100644
--- a/drivers/fpga/Kconfig
+++ b/drivers/fpga/Kconfig
@@ -21,6 +21,7 @@ config FPGA_MGR_SOCFPGA
 
 config FPGA_MGR_ZYNQ_FPGA
 	tristate "Xilinx Zynq FPGA"
+	depends on ARCH_ZYNQ || COMPILE_TEST
 	depends on HAS_DMA
 	help
 	  FPGA manager driver support for Xilinx Zynq FPGAs.
diff --git a/drivers/gpio/Kconfig b/drivers/gpio/Kconfig
index 24caedb00a7a..26ee00f6bd58 100644
--- a/drivers/gpio/Kconfig
+++ b/drivers/gpio/Kconfig
@@ -10,27 +10,6 @@ config ARCH_HAVE_CUSTOM_GPIO_H
 	  overriding the default implementations.  New uses of this are
 	  strongly discouraged.
 
-config ARCH_WANT_OPTIONAL_GPIOLIB
-	bool
-	help
-	  Select this config option from the architecture Kconfig, if
-	  it is possible to use gpiolib on the architecture, but let the
-	  user decide whether to actually build it or not.
-	  Select this instead of ARCH_REQUIRE_GPIOLIB, if your architecture does
-	  not depend on GPIOs being available, but rather let the user
-	  decide whether he needs it or not.
-
-config ARCH_REQUIRE_GPIOLIB
-	bool
-	select GPIOLIB
-	help
-	  Platforms select gpiolib if they use this infrastructure
-	  for all their GPIOs, usually starting with ones integrated
-	  into SOC processors.
-	  Selecting this from the architecture code will cause the gpiolib
-	  code to always get built in.
-
-
 menuconfig GPIOLIB
 	bool "GPIO Support"
 	select ANON_INODES
@@ -87,6 +66,7 @@ config GPIO_SYSFS
 	  exported to userspace; this can be useful when debugging.
 
 config GPIO_GENERIC
+	depends on HAS_IOMEM # Only for IOMEM drivers
 	tristate
 
 # put drivers in the right section, in alphabetical order
@@ -96,6 +76,7 @@ config GPIO_MAX730X
 	tristate
 
 menu "Memory mapped GPIO drivers"
+	depends on HAS_IOMEM
 
 config GPIO_74XX_MMIO
 	tristate "GPIO driver for 74xx-ICs with MMIO access"
@@ -128,6 +109,13 @@ config GPIO_AMDPT
 	  driver for GPIO functionality on Promontory IOHub
 	  Require ACPI ASL code to enumerate as a platform device.
 
+config GPIO_ASPEED
+	tristate "Aspeed GPIO support"
+	depends on (ARCH_ASPEED || COMPILE_TEST) && OF_GPIO
+	select GPIOLIB_IRQCHIP
+	help
+	  Say Y here to support Aspeed AST2400 and AST2500 GPIO controllers.
+
 config GPIO_ATH79
 	tristate "Atheros AR71XX/AR724X/AR913X GPIO support"
 	default y if ATH79
@@ -138,6 +126,12 @@ config GPIO_ATH79
 	  Select this option to enable GPIO driver for
 	  Atheros AR71XX/AR724X/AR913X SoC devices.
 
+config GPIO_AXP209
+	tristate "X-Powers AXP209 PMIC GPIO Support"
+	depends on MFD_AXP20X
+	help
+	  Say yes to enable GPIO support for the AXP209 PMIC
+
 config GPIO_BCM_KONA
 	bool "Broadcom Kona GPIO"
 	depends on OF_GPIO && (ARCH_BCM_MOBILE || COMPILE_TEST)
@@ -237,7 +231,8 @@ config GPIO_ICH
 
 config GPIO_IOP
 	tristate "Intel IOP GPIO"
-	depends on ARM && (ARCH_IOP32X || ARCH_IOP33X)
+	depends on ARCH_IOP32X || ARCH_IOP33X || COMPILE_TEST
+	select GPIO_GENERIC
 	help
 	  Say yes here to support the GPIO functionality of a number of Intel
 	  IOP32X or IOP33X.
@@ -287,6 +282,18 @@ config GPIO_MM_LANTIQ
 	  (EBU) found on Lantiq SoCs. The gpios are output only as they are
 	  created by attaching a 16bit latch to the bus.
 
+config GPIO_MOCKUP
+	tristate "GPIO Testing Driver"
+	depends on GPIOLIB
+	select GPIO_SYSFS
+	help
+	  This enables GPIO Testing driver, which provides a way to test GPIO
+	  subsystem through sysfs(or char device) and debugfs. GPIO_SYSFS
+	  must be selected for this test.
+	  User could use it through the script in
+	  tools/testing/selftests/gpio/gpio-mockup.sh. Reference the usage in
+	  it.
+
 config GPIO_MOXART
 	bool "MOXART GPIO support"
 	depends on ARCH_MOXART || COMPILE_TEST
@@ -574,6 +581,19 @@ config GPIO_F7188X
 	  To compile this driver as a module, choose M here: the module will
 	  be called f7188x-gpio.
 
+config GPIO_GPIO_MM
+	tristate "Diamond Systems GPIO-MM GPIO support"
+	depends on ISA_BUS_API
+	help
+	  Enables GPIO support for the Diamond Systems GPIO-MM and GPIO-MM-12.
+
+	  The Diamond Systems GPIO-MM device features 48 lines of digital I/O
+	  via the emulation of dual 82C55A PPI chips. This driver provides GPIO
+	  support for these 48 channels of digital I/O.
+
+	  The base port addresses for the devices may be configured via the base
+	  array module parameter.
+
 config GPIO_IT87
 	tristate "IT87xx GPIO support"
 	help
@@ -780,6 +800,13 @@ config GPIO_TPIC2810
 	  To compile this driver as a module, choose M here: the module will
 	  be called gpio-tpic2810.
 
+config GPIO_TS4900
+	tristate "Technologic Systems FPGA I2C GPIO"
+	select REGMAP_I2C
+	help
+	  Say yes here to enabled the GPIO driver for Technologic's FPGA core.
+	  Series supported include TS-4100, TS-4900, TS-7970 and TS-7990.
+
 endmenu
 
 menu "MFD GPIO expanders"
@@ -849,6 +876,14 @@ config GPIO_DLN2
 	  This driver can also be built as a module. If so, the module
 	  will be called gpio-dln2.
 
+config HTC_EGPIO
+	bool "HTC EGPIO support"
+	depends on GPIOLIB && ARM
+	help
+	    This driver supports the CPLD egpio chip present on
+	    several HTC phones.  It provides basic support for input
+	    pins, output pins, and irqs.
+
 config GPIO_JANZ_TTL
 	tristate "Janz VMOD-TTL Digital IO Module"
 	depends on MFD_JANZ_CMODIO
@@ -875,6 +910,16 @@ config GPIO_LP3943
 	  LP3943 can be used as a GPIO expander which provides up to 16 GPIOs.
 	  Open drain outputs are required for this usage.
 
+config GPIO_LP873X
+	tristate "TI LP873X GPO"
+	depends on MFD_TI_LP873X
+	help
+	  This driver supports the GPO on TI Lp873x PMICs. 2 GPOs are present
+	  on LP873X PMICs.
+
+	  This driver can also be built as a module. If so, the module will be
+          called gpio-lp873x.
+
 config GPIO_MAX77620
 	tristate "GPIO support for PMIC MAX77620 and MAX20024"
 	depends on MFD_MAX77620
@@ -985,6 +1030,19 @@ config GPIO_UCB1400
 	  This enables support for the Philips UCB1400 GPIO pins.
 	  The UCB1400 is an AC97 audio codec.
 
+config GPIO_WHISKEY_COVE
+	tristate "GPIO support for Whiskey Cove PMIC"
+	depends on INTEL_SOC_PMIC
+	select GPIOLIB_IRQCHIP
+	help
+	  Support for GPIO pins on Whiskey Cove PMIC.
+
+	  Say Yes if you have a Intel SoC based tablet with Whiskey Cove PMIC
+	  inside.
+
+	  This driver can also be built as a module. If so, the module will be
+	  called gpio-wcove.
+
 config GPIO_WM831X
 	tristate "WM831x GPIOs"
 	depends on MFD_WM831X
diff --git a/drivers/gpio/Makefile b/drivers/gpio/Makefile
index 2a035ed8f168..ab28a2daeacc 100644
--- a/drivers/gpio/Makefile
+++ b/drivers/gpio/Makefile
@@ -28,6 +28,8 @@ obj-$(CONFIG_GPIO_AMD8111)	+= gpio-amd8111.o
 obj-$(CONFIG_GPIO_AMDPT)	+= gpio-amdpt.o
 obj-$(CONFIG_GPIO_ARIZONA)	+= gpio-arizona.o
 obj-$(CONFIG_GPIO_ATH79)	+= gpio-ath79.o
+obj-$(CONFIG_GPIO_ASPEED)	+= gpio-aspeed.o
+obj-$(CONFIG_GPIO_AXP209)	+= gpio-axp209.o
 obj-$(CONFIG_GPIO_BCM_KONA)	+= gpio-bcm-kona.o
 obj-$(CONFIG_GPIO_BRCMSTB)	+= gpio-brcmstb.o
 obj-$(CONFIG_GPIO_BT8XX)	+= gpio-bt8xx.o
@@ -44,7 +46,9 @@ 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_GPIO_MM)	+= gpio-gpio-mm.o
 obj-$(CONFIG_GPIO_GRGPIO)	+= gpio-grgpio.o
+obj-$(CONFIG_HTC_EGPIO)		+= gpio-htc-egpio.o
 obj-$(CONFIG_GPIO_ICH)		+= gpio-ich.o
 obj-$(CONFIG_GPIO_IOP)		+= gpio-iop.o
 obj-$(CONFIG_GPIO_IT87)		+= gpio-it87.o
@@ -56,6 +60,7 @@ 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_LP873X)	+= gpio-lp873x.o
 obj-$(CONFIG_GPIO_LYNXPOINT)	+= gpio-lynxpoint.o
 obj-$(CONFIG_GPIO_MAX730X)	+= gpio-max730x.o
 obj-$(CONFIG_GPIO_MAX7300)	+= gpio-max7300.o
@@ -70,6 +75,7 @@ obj-$(CONFIG_GPIO_MC9S08DZ60)	+= gpio-mc9s08dz60.o
 obj-$(CONFIG_GPIO_MCP23S08)	+= gpio-mcp23s08.o
 obj-$(CONFIG_GPIO_ML_IOH)	+= gpio-ml-ioh.o
 obj-$(CONFIG_GPIO_MM_LANTIQ)	+= gpio-mm-lantiq.o
+obj-$(CONFIG_GPIO_MOCKUP)      += gpio-mockup.o
 obj-$(CONFIG_GPIO_MOXART)	+= gpio-moxart.o
 obj-$(CONFIG_GPIO_MPC5200)	+= gpio-mpc5200.o
 obj-$(CONFIG_GPIO_MPC8XXX)	+= gpio-mpc8xxx.o
@@ -110,6 +116,7 @@ obj-$(CONFIG_GPIO_TPS6586X)	+= gpio-tps6586x.o
 obj-$(CONFIG_GPIO_TPS65910)	+= gpio-tps65910.o
 obj-$(CONFIG_GPIO_TPS65912)	+= gpio-tps65912.o
 obj-$(CONFIG_GPIO_TS4800)	+= gpio-ts4800.o
+obj-$(CONFIG_GPIO_TS4900)	+= gpio-ts4900.o
 obj-$(CONFIG_GPIO_TS5500)	+= gpio-ts5500.o
 obj-$(CONFIG_GPIO_TWL4030)	+= gpio-twl4030.o
 obj-$(CONFIG_GPIO_TWL6040)	+= gpio-twl6040.o
@@ -120,6 +127,7 @@ obj-$(CONFIG_GPIO_VF610)	+= gpio-vf610.o
 obj-$(CONFIG_GPIO_VIPERBOARD)	+= gpio-viperboard.o
 obj-$(CONFIG_GPIO_VR41XX)	+= gpio-vr41xx.o
 obj-$(CONFIG_GPIO_VX855)	+= gpio-vx855.o
+obj-$(CONFIG_GPIO_WHISKEY_COVE)	+= gpio-wcove.o
 obj-$(CONFIG_GPIO_WM831X)	+= gpio-wm831x.o
 obj-$(CONFIG_GPIO_WM8350)	+= gpio-wm8350.o
 obj-$(CONFIG_GPIO_WM8994)	+= gpio-wm8994.o
diff --git a/drivers/gpio/gpio-altera.c b/drivers/gpio/gpio-altera.c
index 3f87a03abc22..5bddbd507ca9 100644
--- a/drivers/gpio/gpio-altera.c
+++ b/drivers/gpio/gpio-altera.c
@@ -17,6 +17,7 @@
  */
 
 #include <linux/io.h>
+#include <linux/module.h>
 #include <linux/of_gpio.h>
 #include <linux/platform_device.h>
 
diff --git a/drivers/gpio/gpio-arizona.c b/drivers/gpio/gpio-arizona.c
index 991370494922..482462889c8f 100644
--- a/drivers/gpio/gpio-arizona.c
+++ b/drivers/gpio/gpio-arizona.c
@@ -79,7 +79,7 @@ static void arizona_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 			   ARIZONA_GPN_LVL, value);
 }
 
-static struct gpio_chip template_chip = {
+static const struct gpio_chip template_chip = {
 	.label			= "arizona",
 	.owner			= THIS_MODULE,
 	.direction_input	= arizona_gpio_direction_in,
diff --git a/drivers/gpio/gpio-aspeed.c b/drivers/gpio/gpio-aspeed.c
new file mode 100644
index 000000000000..03a5925a423c
--- /dev/null
+++ b/drivers/gpio/gpio-aspeed.c
@@ -0,0 +1,455 @@
+/*
+ * Copyright 2015 IBM Corp.
+ *
+ * Joel Stanley <joel@jms.id.au>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version
+ * 2 of the License, or (at your option) any later version.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/spinlock.h>
+#include <linux/platform_device.h>
+#include <linux/gpio/driver.h>
+#include <linux/pinctrl/consumer.h>
+
+struct aspeed_gpio {
+	struct gpio_chip chip;
+	spinlock_t lock;
+	void __iomem *base;
+	int irq;
+};
+
+struct aspeed_gpio_bank {
+	uint16_t	val_regs;
+	uint16_t	irq_regs;
+	const char	names[4];
+};
+
+static const struct aspeed_gpio_bank aspeed_gpio_banks[] = {
+	{
+		.val_regs = 0x0000,
+		.irq_regs = 0x0008,
+		.names = { 'A', 'B', 'C', 'D' },
+	},
+	{
+		.val_regs = 0x0020,
+		.irq_regs = 0x0028,
+		.names = { 'E', 'F', 'G', 'H' },
+	},
+	{
+		.val_regs = 0x0070,
+		.irq_regs = 0x0098,
+		.names = { 'I', 'J', 'K', 'L' },
+	},
+	{
+		.val_regs = 0x0078,
+		.irq_regs = 0x00e8,
+		.names = { 'M', 'N', 'O', 'P' },
+	},
+	{
+		.val_regs = 0x0080,
+		.irq_regs = 0x0118,
+		.names = { 'Q', 'R', 'S', 'T' },
+	},
+	{
+		.val_regs = 0x0088,
+		.irq_regs = 0x0148,
+		.names = { 'U', 'V', 'W', 'X' },
+	},
+	/*
+	 * A bank exists for { 'Y', 'Z', "AA", "AB" }, but is not implemented.
+	 * Only half of GPIOs Y support interrupt configuration, and none of Z,
+	 * AA or AB do as they are output only.
+	 */
+};
+
+#define GPIO_BANK(x)	((x) >> 5)
+#define GPIO_OFFSET(x)	((x) & 0x1f)
+#define GPIO_BIT(x)	BIT(GPIO_OFFSET(x))
+
+#define GPIO_DATA	0x00
+#define GPIO_DIR	0x04
+
+#define GPIO_IRQ_ENABLE	0x00
+#define GPIO_IRQ_TYPE0	0x04
+#define GPIO_IRQ_TYPE1	0x08
+#define GPIO_IRQ_TYPE2	0x0c
+#define GPIO_IRQ_STATUS	0x10
+
+static const struct aspeed_gpio_bank *to_bank(unsigned int offset)
+{
+	unsigned int bank = GPIO_BANK(offset);
+
+	WARN_ON(bank > ARRAY_SIZE(aspeed_gpio_banks));
+	return &aspeed_gpio_banks[bank];
+}
+
+static void __iomem *bank_val_reg(struct aspeed_gpio *gpio,
+		const struct aspeed_gpio_bank *bank,
+		unsigned int reg)
+{
+	return gpio->base + bank->val_regs + reg;
+}
+
+static void __iomem *bank_irq_reg(struct aspeed_gpio *gpio,
+		const struct aspeed_gpio_bank *bank,
+		unsigned int reg)
+{
+	return gpio->base + bank->irq_regs + reg;
+}
+
+static int aspeed_gpio_get(struct gpio_chip *gc, unsigned int offset)
+{
+	struct aspeed_gpio *gpio = gpiochip_get_data(gc);
+	const struct aspeed_gpio_bank *bank = to_bank(offset);
+
+	return !!(ioread32(bank_val_reg(gpio, bank, GPIO_DATA))
+			& GPIO_BIT(offset));
+}
+
+static void __aspeed_gpio_set(struct gpio_chip *gc, unsigned int offset,
+			      int val)
+{
+	struct aspeed_gpio *gpio = gpiochip_get_data(gc);
+	const struct aspeed_gpio_bank *bank = to_bank(offset);
+	void __iomem *addr;
+	u32 reg;
+
+	addr = bank_val_reg(gpio, bank, GPIO_DATA);
+	reg = ioread32(addr);
+
+	if (val)
+		reg |= GPIO_BIT(offset);
+	else
+		reg &= ~GPIO_BIT(offset);
+
+	iowrite32(reg, addr);
+}
+
+static void aspeed_gpio_set(struct gpio_chip *gc, unsigned int offset,
+			    int val)
+{
+	struct aspeed_gpio *gpio = gpiochip_get_data(gc);
+	unsigned long flags;
+
+	spin_lock_irqsave(&gpio->lock, flags);
+
+	__aspeed_gpio_set(gc, offset, val);
+
+	spin_unlock_irqrestore(&gpio->lock, flags);
+}
+
+static int aspeed_gpio_dir_in(struct gpio_chip *gc, unsigned int offset)
+{
+	struct aspeed_gpio *gpio = gpiochip_get_data(gc);
+	const struct aspeed_gpio_bank *bank = to_bank(offset);
+	unsigned long flags;
+	u32 reg;
+
+	spin_lock_irqsave(&gpio->lock, flags);
+
+	reg = ioread32(bank_val_reg(gpio, bank, GPIO_DIR));
+	iowrite32(reg & ~GPIO_BIT(offset), bank_val_reg(gpio, bank, GPIO_DIR));
+
+	spin_unlock_irqrestore(&gpio->lock, flags);
+
+	return 0;
+}
+
+static int aspeed_gpio_dir_out(struct gpio_chip *gc,
+			       unsigned int offset, int val)
+{
+	struct aspeed_gpio *gpio = gpiochip_get_data(gc);
+	const struct aspeed_gpio_bank *bank = to_bank(offset);
+	unsigned long flags;
+	u32 reg;
+
+	spin_lock_irqsave(&gpio->lock, flags);
+
+	reg = ioread32(bank_val_reg(gpio, bank, GPIO_DIR));
+	iowrite32(reg | GPIO_BIT(offset), bank_val_reg(gpio, bank, GPIO_DIR));
+
+	__aspeed_gpio_set(gc, offset, val);
+
+	spin_unlock_irqrestore(&gpio->lock, flags);
+
+	return 0;
+}
+
+static int aspeed_gpio_get_direction(struct gpio_chip *gc, unsigned int offset)
+{
+	struct aspeed_gpio *gpio = gpiochip_get_data(gc);
+	const struct aspeed_gpio_bank *bank = to_bank(offset);
+	unsigned long flags;
+	u32 val;
+
+	spin_lock_irqsave(&gpio->lock, flags);
+
+	val = ioread32(bank_val_reg(gpio, bank, GPIO_DIR)) & GPIO_BIT(offset);
+
+	spin_unlock_irqrestore(&gpio->lock, flags);
+
+	return !val;
+
+}
+
+static inline int irqd_to_aspeed_gpio_data(struct irq_data *d,
+		struct aspeed_gpio **gpio,
+		const struct aspeed_gpio_bank **bank,
+		u32 *bit)
+{
+	int offset;
+
+	offset = irqd_to_hwirq(d);
+
+	*gpio = irq_data_get_irq_chip_data(d);
+	*bank = to_bank(offset);
+	*bit = GPIO_BIT(offset);
+
+	return 0;
+}
+
+static void aspeed_gpio_irq_ack(struct irq_data *d)
+{
+	const struct aspeed_gpio_bank *bank;
+	struct aspeed_gpio *gpio;
+	unsigned long flags;
+	void __iomem *status_addr;
+	u32 bit;
+	int rc;
+
+	rc = irqd_to_aspeed_gpio_data(d, &gpio, &bank, &bit);
+	if (rc)
+		return;
+
+	status_addr = bank_irq_reg(gpio, bank, GPIO_IRQ_STATUS);
+
+	spin_lock_irqsave(&gpio->lock, flags);
+	iowrite32(bit, status_addr);
+	spin_unlock_irqrestore(&gpio->lock, flags);
+}
+
+static void aspeed_gpio_irq_set_mask(struct irq_data *d, bool set)
+{
+	const struct aspeed_gpio_bank *bank;
+	struct aspeed_gpio *gpio;
+	unsigned long flags;
+	u32 reg, bit;
+	void __iomem *addr;
+	int rc;
+
+	rc = irqd_to_aspeed_gpio_data(d, &gpio, &bank, &bit);
+	if (rc)
+		return;
+
+	addr = bank_irq_reg(gpio, bank, GPIO_IRQ_ENABLE);
+
+	spin_lock_irqsave(&gpio->lock, flags);
+
+	reg = ioread32(addr);
+	if (set)
+		reg |= bit;
+	else
+		reg &= bit;
+	iowrite32(reg, addr);
+
+	spin_unlock_irqrestore(&gpio->lock, flags);
+}
+
+static void aspeed_gpio_irq_mask(struct irq_data *d)
+{
+	aspeed_gpio_irq_set_mask(d, false);
+}
+
+static void aspeed_gpio_irq_unmask(struct irq_data *d)
+{
+	aspeed_gpio_irq_set_mask(d, true);
+}
+
+static int aspeed_gpio_set_type(struct irq_data *d, unsigned int type)
+{
+	u32 type0 = 0;
+	u32 type1 = 0;
+	u32 type2 = 0;
+	u32 bit, reg;
+	const struct aspeed_gpio_bank *bank;
+	irq_flow_handler_t handler;
+	struct aspeed_gpio *gpio;
+	unsigned long flags;
+	void __iomem *addr;
+	int rc;
+
+	rc = irqd_to_aspeed_gpio_data(d, &gpio, &bank, &bit);
+	if (rc)
+		return -EINVAL;
+
+	switch (type & IRQ_TYPE_SENSE_MASK) {
+	case IRQ_TYPE_EDGE_BOTH:
+		type2 |= bit;
+	case IRQ_TYPE_EDGE_RISING:
+		type0 |= bit;
+	case IRQ_TYPE_EDGE_FALLING:
+		handler = handle_edge_irq;
+		break;
+	case IRQ_TYPE_LEVEL_HIGH:
+		type0 |= bit;
+	case IRQ_TYPE_LEVEL_LOW:
+		type1 |= bit;
+		handler = handle_level_irq;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	spin_lock_irqsave(&gpio->lock, flags);
+
+	addr = bank_irq_reg(gpio, bank, GPIO_IRQ_TYPE0);
+	reg = ioread32(addr);
+	reg = (reg & ~bit) | type0;
+	iowrite32(reg, addr);
+
+	addr = bank_irq_reg(gpio, bank, GPIO_IRQ_TYPE1);
+	reg = ioread32(addr);
+	reg = (reg & ~bit) | type1;
+	iowrite32(reg, addr);
+
+	addr = bank_irq_reg(gpio, bank, GPIO_IRQ_TYPE2);
+	reg = ioread32(addr);
+	reg = (reg & ~bit) | type2;
+	iowrite32(reg, addr);
+
+	spin_unlock_irqrestore(&gpio->lock, flags);
+
+	irq_set_handler_locked(d, handler);
+
+	return 0;
+}
+
+static void aspeed_gpio_irq_handler(struct irq_desc *desc)
+{
+	struct gpio_chip *gc = irq_desc_get_handler_data(desc);
+	struct irq_chip *ic = irq_desc_get_chip(desc);
+	struct aspeed_gpio *data = gpiochip_get_data(gc);
+	unsigned int i, p, girq;
+	unsigned long reg;
+
+	chained_irq_enter(ic, desc);
+
+	for (i = 0; i < ARRAY_SIZE(aspeed_gpio_banks); i++) {
+		const struct aspeed_gpio_bank *bank = &aspeed_gpio_banks[i];
+
+		reg = ioread32(bank_irq_reg(data, bank, GPIO_IRQ_STATUS));
+
+		for_each_set_bit(p, &reg, 32) {
+			girq = irq_find_mapping(gc->irqdomain, i * 32 + p);
+			generic_handle_irq(girq);
+		}
+
+	}
+
+	chained_irq_exit(ic, desc);
+}
+
+static struct irq_chip aspeed_gpio_irqchip = {
+	.name		= "aspeed-gpio",
+	.irq_ack	= aspeed_gpio_irq_ack,
+	.irq_mask	= aspeed_gpio_irq_mask,
+	.irq_unmask	= aspeed_gpio_irq_unmask,
+	.irq_set_type	= aspeed_gpio_set_type,
+};
+
+static int aspeed_gpio_setup_irqs(struct aspeed_gpio *gpio,
+		struct platform_device *pdev)
+{
+	int rc;
+
+	rc = platform_get_irq(pdev, 0);
+	if (rc < 0)
+		return rc;
+
+	gpio->irq = rc;
+
+	rc = gpiochip_irqchip_add(&gpio->chip, &aspeed_gpio_irqchip,
+			0, handle_bad_irq, IRQ_TYPE_NONE);
+	if (rc) {
+		dev_info(&pdev->dev, "Could not add irqchip\n");
+		return rc;
+	}
+
+	gpiochip_set_chained_irqchip(&gpio->chip, &aspeed_gpio_irqchip,
+				     gpio->irq, aspeed_gpio_irq_handler);
+
+	return 0;
+}
+
+static int aspeed_gpio_request(struct gpio_chip *chip, unsigned int offset)
+{
+	return pinctrl_request_gpio(chip->base + offset);
+}
+
+static void aspeed_gpio_free(struct gpio_chip *chip, unsigned int offset)
+{
+	pinctrl_free_gpio(chip->base + offset);
+}
+
+static int __init aspeed_gpio_probe(struct platform_device *pdev)
+{
+	struct aspeed_gpio *gpio;
+	struct resource *res;
+	int rc;
+
+	gpio = devm_kzalloc(&pdev->dev, sizeof(*gpio), GFP_KERNEL);
+	if (!gpio)
+		return -ENOMEM;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	gpio->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(gpio->base))
+		return PTR_ERR(gpio->base);
+
+	spin_lock_init(&gpio->lock);
+
+	gpio->chip.ngpio = ARRAY_SIZE(aspeed_gpio_banks) * 32;
+
+	gpio->chip.parent = &pdev->dev;
+	gpio->chip.direction_input = aspeed_gpio_dir_in;
+	gpio->chip.direction_output = aspeed_gpio_dir_out;
+	gpio->chip.get_direction = aspeed_gpio_get_direction;
+	gpio->chip.request = aspeed_gpio_request;
+	gpio->chip.free = aspeed_gpio_free;
+	gpio->chip.get = aspeed_gpio_get;
+	gpio->chip.set = aspeed_gpio_set;
+	gpio->chip.label = dev_name(&pdev->dev);
+	gpio->chip.base = -1;
+
+	rc = devm_gpiochip_add_data(&pdev->dev, &gpio->chip, gpio);
+	if (rc < 0)
+		return rc;
+
+	return aspeed_gpio_setup_irqs(gpio, pdev);
+}
+
+static const struct of_device_id aspeed_gpio_of_table[] = {
+	{ .compatible = "aspeed,ast2400-gpio" },
+	{ .compatible = "aspeed,ast2500-gpio" },
+	{}
+};
+MODULE_DEVICE_TABLE(of, aspeed_gpio_of_table);
+
+static struct platform_driver aspeed_gpio_driver = {
+	.driver = {
+		.name = KBUILD_MODNAME,
+		.of_match_table = aspeed_gpio_of_table,
+	},
+};
+
+module_platform_driver_probe(aspeed_gpio_driver, aspeed_gpio_probe);
+
+MODULE_DESCRIPTION("Aspeed GPIO Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/gpio/gpio-ath79.c b/drivers/gpio/gpio-ath79.c
index c4f4cddc7c1a..9457e2022bf6 100644
--- a/drivers/gpio/gpio-ath79.c
+++ b/drivers/gpio/gpio-ath79.c
@@ -15,6 +15,7 @@
 #include <linux/platform_data/gpio-ath79.h>
 #include <linux/of_device.h>
 #include <linux/interrupt.h>
+#include <linux/module.h>
 #include <linux/irq.h>
 
 #define AR71XX_GPIO_REG_OE		0x00
diff --git a/drivers/gpio/gpio-axp209.c b/drivers/gpio/gpio-axp209.c
new file mode 100644
index 000000000000..d9c2a517c6df
--- /dev/null
+++ b/drivers/gpio/gpio-axp209.c
@@ -0,0 +1,192 @@
+/*
+ * AXP20x GPIO driver
+ *
+ * Copyright (C) 2016 Maxime Ripard <maxime.ripard@free-electrons.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under  the terms of the GNU General  Public License 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/device.h>
+#include <linux/gpio/driver.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/mfd/axp20x.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+#include <linux/slab.h>
+
+#define AXP20X_GPIO_FUNCTIONS		0x7
+#define AXP20X_GPIO_FUNCTION_OUT_LOW	0
+#define AXP20X_GPIO_FUNCTION_OUT_HIGH	1
+#define AXP20X_GPIO_FUNCTION_INPUT	2
+
+struct axp20x_gpio {
+	struct gpio_chip	chip;
+	struct regmap		*regmap;
+};
+
+static int axp20x_gpio_get_reg(unsigned offset)
+{
+	switch (offset) {
+	case 0:
+		return AXP20X_GPIO0_CTRL;
+	case 1:
+		return AXP20X_GPIO1_CTRL;
+	case 2:
+		return AXP20X_GPIO2_CTRL;
+	}
+
+	return -EINVAL;
+}
+
+static int axp20x_gpio_input(struct gpio_chip *chip, unsigned offset)
+{
+	struct axp20x_gpio *gpio = gpiochip_get_data(chip);
+	int reg;
+
+	reg = axp20x_gpio_get_reg(offset);
+	if (reg < 0)
+		return reg;
+
+	return regmap_update_bits(gpio->regmap, reg,
+				  AXP20X_GPIO_FUNCTIONS,
+				  AXP20X_GPIO_FUNCTION_INPUT);
+}
+
+static int axp20x_gpio_get(struct gpio_chip *chip, unsigned offset)
+{
+	struct axp20x_gpio *gpio = gpiochip_get_data(chip);
+	unsigned int val;
+	int reg, ret;
+
+	reg = axp20x_gpio_get_reg(offset);
+	if (reg < 0)
+		return reg;
+
+	ret = regmap_read(gpio->regmap, reg, &val);
+	if (ret)
+		return ret;
+
+	return !!(val & BIT(offset + 4));
+}
+
+static int axp20x_gpio_get_direction(struct gpio_chip *chip, unsigned offset)
+{
+	struct axp20x_gpio *gpio = gpiochip_get_data(chip);
+	unsigned int val;
+	int reg, ret;
+
+	reg = axp20x_gpio_get_reg(offset);
+	if (reg < 0)
+		return reg;
+
+	ret = regmap_read(gpio->regmap, reg, &val);
+	if (ret)
+		return ret;
+
+	/*
+	 * This shouldn't really happen if the pin is in use already,
+	 * or if it's not in use yet, it doesn't matter since we're
+	 * going to change the value soon anyway. Default to output.
+	 */
+	if ((val & AXP20X_GPIO_FUNCTIONS) > 2)
+		return 0;
+
+	/*
+	 * The GPIO directions are the three lowest values.
+	 * 2 is input, 0 and 1 are output
+	 */
+	return val & 2;
+}
+
+static int axp20x_gpio_output(struct gpio_chip *chip, unsigned offset,
+			      int value)
+{
+	struct axp20x_gpio *gpio = gpiochip_get_data(chip);
+	int reg;
+
+	reg = axp20x_gpio_get_reg(offset);
+	if (reg < 0)
+		return reg;
+
+	return regmap_update_bits(gpio->regmap, reg,
+				  AXP20X_GPIO_FUNCTIONS,
+				  value ? AXP20X_GPIO_FUNCTION_OUT_HIGH
+				  : AXP20X_GPIO_FUNCTION_OUT_LOW);
+}
+
+static void axp20x_gpio_set(struct gpio_chip *chip, unsigned offset,
+			    int value)
+{
+	axp20x_gpio_output(chip, offset, value);
+}
+
+static int axp20x_gpio_probe(struct platform_device *pdev)
+{
+	struct axp20x_dev *axp20x = dev_get_drvdata(pdev->dev.parent);
+	struct axp20x_gpio *gpio;
+	int ret;
+
+	if (!of_device_is_available(pdev->dev.of_node))
+		return -ENODEV;
+
+	if (!axp20x) {
+		dev_err(&pdev->dev, "Parent drvdata not set\n");
+		return -EINVAL;
+	}
+
+	gpio = devm_kzalloc(&pdev->dev, sizeof(*gpio), GFP_KERNEL);
+	if (!gpio)
+		return -ENOMEM;
+
+	gpio->chip.base			= -1;
+	gpio->chip.can_sleep		= true;
+	gpio->chip.parent		= &pdev->dev;
+	gpio->chip.label		= dev_name(&pdev->dev);
+	gpio->chip.owner		= THIS_MODULE;
+	gpio->chip.get			= axp20x_gpio_get;
+	gpio->chip.get_direction	= axp20x_gpio_get_direction;
+	gpio->chip.set			= axp20x_gpio_set;
+	gpio->chip.direction_input	= axp20x_gpio_input;
+	gpio->chip.direction_output	= axp20x_gpio_output;
+	gpio->chip.ngpio		= 3;
+
+	gpio->regmap = axp20x->regmap;
+
+	ret = devm_gpiochip_add_data(&pdev->dev, &gpio->chip, gpio);
+	if (ret) {
+		dev_err(&pdev->dev, "Failed to register GPIO chip\n");
+		return ret;
+	}
+
+	dev_info(&pdev->dev, "AXP209 GPIO driver loaded\n");
+
+	return 0;
+}
+
+static const struct of_device_id axp20x_gpio_match[] = {
+	{ .compatible = "x-powers,axp209-gpio" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, axp20x_gpio_match);
+
+static struct platform_driver axp20x_gpio_driver = {
+	.probe		= axp20x_gpio_probe,
+	.driver = {
+		.name		= "axp20x-gpio",
+		.of_match_table	= axp20x_gpio_match,
+	},
+};
+
+module_platform_driver(axp20x_gpio_driver);
+
+MODULE_AUTHOR("Maxime Ripard <maxime.ripard@free-electrons.com>");
+MODULE_DESCRIPTION("AXP20x PMIC GPIO driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/gpio/gpio-bcm-kona.c b/drivers/gpio/gpio-bcm-kona.c
index 953e4b829e32..3d1cf018e8e7 100644
--- a/drivers/gpio/gpio-bcm-kona.c
+++ b/drivers/gpio/gpio-bcm-kona.c
@@ -308,7 +308,7 @@ static int bcm_kona_gpio_set_debounce(struct gpio_chip *chip, unsigned gpio,
 	return 0;
 }
 
-static struct gpio_chip template_chip = {
+static const struct gpio_chip template_chip = {
 	.label = "bcm-kona-gpio",
 	.owner = THIS_MODULE,
 	.request = bcm_kona_gpio_request,
diff --git a/drivers/gpio/gpio-da9052.c b/drivers/gpio/gpio-da9052.c
index e29553b7ccdb..dd8977cf3e85 100644
--- a/drivers/gpio/gpio-da9052.c
+++ b/drivers/gpio/gpio-da9052.c
@@ -184,7 +184,7 @@ static int da9052_gpio_to_irq(struct gpio_chip *gc, u32 offset)
 	return irq;
 }
 
-static struct gpio_chip reference_gp = {
+static const struct gpio_chip reference_gp = {
 	.label = "da9052-gpio",
 	.owner = THIS_MODULE,
 	.get = da9052_gpio_get,
diff --git a/drivers/gpio/gpio-da9055.c b/drivers/gpio/gpio-da9055.c
index 2c2c18dc6c4f..82053b52cba0 100644
--- a/drivers/gpio/gpio-da9055.c
+++ b/drivers/gpio/gpio-da9055.c
@@ -121,7 +121,7 @@ static int da9055_gpio_to_irq(struct gpio_chip *gc, u32 offset)
 				  DA9055_IRQ_GPI0 + offset);
 }
 
-static struct gpio_chip reference_gp = {
+static const struct gpio_chip reference_gp = {
 	.label = "da9055-gpio",
 	.owner = THIS_MODULE,
 	.get = da9055_gpio_get,
diff --git a/drivers/gpio/gpio-f7188x.c b/drivers/gpio/gpio-f7188x.c
index 600be8418707..e8accde62aa7 100644
--- a/drivers/gpio/gpio-f7188x.c
+++ b/drivers/gpio/gpio-f7188x.c
@@ -214,8 +214,7 @@ static struct f7188x_gpio_bank f81866_gpio_bank[] = {
 static int f7188x_gpio_get_direction(struct gpio_chip *chip, unsigned offset)
 {
 	int err;
-	struct f7188x_gpio_bank *bank =
-		container_of(chip, struct f7188x_gpio_bank, chip);
+	struct f7188x_gpio_bank *bank = gpiochip_get_data(chip);
 	struct f7188x_sio *sio = bank->data->sio;
 	u8 dir;
 
diff --git a/drivers/gpio/gpio-gpio-mm.c b/drivers/gpio/gpio-gpio-mm.c
new file mode 100644
index 000000000000..1e7def9449ce
--- /dev/null
+++ b/drivers/gpio/gpio-gpio-mm.c
@@ -0,0 +1,267 @@
+/*
+ * GPIO driver for the Diamond Systems GPIO-MM
+ * Copyright (C) 2016 William Breathitt Gray
+ *
+ * This program is free software; you can 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.
+ *
+ * This driver supports the following Diamond Systems devices: GPIO-MM and
+ * GPIO-MM-12.
+ */
+#include <linux/bitops.h>
+#include <linux/device.h>
+#include <linux/errno.h>
+#include <linux/gpio/driver.h>
+#include <linux/io.h>
+#include <linux/ioport.h>
+#include <linux/isa.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/spinlock.h>
+
+#define GPIOMM_EXTENT 8
+#define MAX_NUM_GPIOMM max_num_isa_dev(GPIOMM_EXTENT)
+
+static unsigned int base[MAX_NUM_GPIOMM];
+static unsigned int num_gpiomm;
+module_param_array(base, uint, &num_gpiomm, 0);
+MODULE_PARM_DESC(base, "Diamond Systems GPIO-MM base addresses");
+
+/**
+ * struct gpiomm_gpio - GPIO device private data structure
+ * @chip:	instance of the gpio_chip
+ * @io_state:	bit I/O state (whether bit is set to input or output)
+ * @out_state:	output bits state
+ * @control:	Control registers state
+ * @lock:	synchronization lock to prevent I/O race conditions
+ * @base:	base port address of the GPIO device
+ */
+struct gpiomm_gpio {
+	struct gpio_chip chip;
+	unsigned char io_state[6];
+	unsigned char out_state[6];
+	unsigned char control[2];
+	spinlock_t lock;
+	unsigned int base;
+};
+
+static int gpiomm_gpio_get_direction(struct gpio_chip *chip,
+	unsigned int offset)
+{
+	struct gpiomm_gpio *const gpiommgpio = gpiochip_get_data(chip);
+	const unsigned int port = offset / 8;
+	const unsigned int mask = BIT(offset % 8);
+
+	return !!(gpiommgpio->io_state[port] & mask);
+}
+
+static int gpiomm_gpio_direction_input(struct gpio_chip *chip,
+	unsigned int offset)
+{
+	struct gpiomm_gpio *const gpiommgpio = gpiochip_get_data(chip);
+	const unsigned int io_port = offset / 8;
+	const unsigned int control_port = io_port / 3;
+	const unsigned int control_addr = gpiommgpio->base + 3 + control_port*4;
+	unsigned long flags;
+	unsigned int control;
+
+	spin_lock_irqsave(&gpiommgpio->lock, flags);
+
+	/* Check if configuring Port C */
+	if (io_port == 2 || io_port == 5) {
+		/* Port C can be configured by nibble */
+		if (offset % 8 > 3) {
+			gpiommgpio->io_state[io_port] |= 0xF0;
+			gpiommgpio->control[control_port] |= BIT(3);
+		} else {
+			gpiommgpio->io_state[io_port] |= 0x0F;
+			gpiommgpio->control[control_port] |= BIT(0);
+		}
+	} else {
+		gpiommgpio->io_state[io_port] |= 0xFF;
+		if (io_port == 0 || io_port == 3)
+			gpiommgpio->control[control_port] |= BIT(4);
+		else
+			gpiommgpio->control[control_port] |= BIT(1);
+	}
+
+	control = BIT(7) | gpiommgpio->control[control_port];
+	outb(control, control_addr);
+
+	spin_unlock_irqrestore(&gpiommgpio->lock, flags);
+
+	return 0;
+}
+
+static int gpiomm_gpio_direction_output(struct gpio_chip *chip,
+	unsigned int offset, int value)
+{
+	struct gpiomm_gpio *const gpiommgpio = gpiochip_get_data(chip);
+	const unsigned int io_port = offset / 8;
+	const unsigned int control_port = io_port / 3;
+	const unsigned int mask = BIT(offset % 8);
+	const unsigned int control_addr = gpiommgpio->base + 3 + control_port*4;
+	const unsigned int out_port = (io_port > 2) ? io_port + 1 : io_port;
+	unsigned long flags;
+	unsigned int control;
+
+	spin_lock_irqsave(&gpiommgpio->lock, flags);
+
+	/* Check if configuring Port C */
+	if (io_port == 2 || io_port == 5) {
+		/* Port C can be configured by nibble */
+		if (offset % 8 > 3) {
+			gpiommgpio->io_state[io_port] &= 0x0F;
+			gpiommgpio->control[control_port] &= ~BIT(3);
+		} else {
+			gpiommgpio->io_state[io_port] &= 0xF0;
+			gpiommgpio->control[control_port] &= ~BIT(0);
+		}
+	} else {
+		gpiommgpio->io_state[io_port] &= 0x00;
+		if (io_port == 0 || io_port == 3)
+			gpiommgpio->control[control_port] &= ~BIT(4);
+		else
+			gpiommgpio->control[control_port] &= ~BIT(1);
+	}
+
+	if (value)
+		gpiommgpio->out_state[io_port] |= mask;
+	else
+		gpiommgpio->out_state[io_port] &= ~mask;
+
+	control = BIT(7) | gpiommgpio->control[control_port];
+	outb(control, control_addr);
+
+	outb(gpiommgpio->out_state[io_port], gpiommgpio->base + out_port);
+
+	spin_unlock_irqrestore(&gpiommgpio->lock, flags);
+
+	return 0;
+}
+
+static int gpiomm_gpio_get(struct gpio_chip *chip, unsigned int offset)
+{
+	struct gpiomm_gpio *const gpiommgpio = gpiochip_get_data(chip);
+	const unsigned int port = offset / 8;
+	const unsigned int mask = BIT(offset % 8);
+	const unsigned int in_port = (port > 2) ? port + 1 : port;
+	unsigned long flags;
+	unsigned int port_state;
+
+	spin_lock_irqsave(&gpiommgpio->lock, flags);
+
+	/* ensure that GPIO is set for input */
+	if (!(gpiommgpio->io_state[port] & mask)) {
+		spin_unlock_irqrestore(&gpiommgpio->lock, flags);
+		return -EINVAL;
+	}
+
+	port_state = inb(gpiommgpio->base + in_port);
+
+	spin_unlock_irqrestore(&gpiommgpio->lock, flags);
+
+	return !!(port_state & mask);
+}
+
+static void gpiomm_gpio_set(struct gpio_chip *chip, unsigned int offset,
+	int value)
+{
+	struct gpiomm_gpio *const gpiommgpio = gpiochip_get_data(chip);
+	const unsigned int port = offset / 8;
+	const unsigned int mask = BIT(offset % 8);
+	const unsigned int out_port = (port > 2) ? port + 1 : port;
+	unsigned long flags;
+
+	spin_lock_irqsave(&gpiommgpio->lock, flags);
+
+	if (value)
+		gpiommgpio->out_state[port] |= mask;
+	else
+		gpiommgpio->out_state[port] &= ~mask;
+
+	outb(gpiommgpio->out_state[port], gpiommgpio->base + out_port);
+
+	spin_unlock_irqrestore(&gpiommgpio->lock, flags);
+}
+
+static int gpiomm_probe(struct device *dev, unsigned int id)
+{
+	struct gpiomm_gpio *gpiommgpio;
+	const char *const name = dev_name(dev);
+	int err;
+
+	gpiommgpio = devm_kzalloc(dev, sizeof(*gpiommgpio), GFP_KERNEL);
+	if (!gpiommgpio)
+		return -ENOMEM;
+
+	if (!devm_request_region(dev, base[id], GPIOMM_EXTENT, name)) {
+		dev_err(dev, "Unable to lock port addresses (0x%X-0x%X)\n",
+			base[id], base[id] + GPIOMM_EXTENT);
+		return -EBUSY;
+	}
+
+	gpiommgpio->chip.label = name;
+	gpiommgpio->chip.parent = dev;
+	gpiommgpio->chip.owner = THIS_MODULE;
+	gpiommgpio->chip.base = -1;
+	gpiommgpio->chip.ngpio = 48;
+	gpiommgpio->chip.get_direction = gpiomm_gpio_get_direction;
+	gpiommgpio->chip.direction_input = gpiomm_gpio_direction_input;
+	gpiommgpio->chip.direction_output = gpiomm_gpio_direction_output;
+	gpiommgpio->chip.get = gpiomm_gpio_get;
+	gpiommgpio->chip.set = gpiomm_gpio_set;
+	gpiommgpio->base = base[id];
+
+	spin_lock_init(&gpiommgpio->lock);
+
+	dev_set_drvdata(dev, gpiommgpio);
+
+	err = gpiochip_add_data(&gpiommgpio->chip, gpiommgpio);
+	if (err) {
+		dev_err(dev, "GPIO registering failed (%d)\n", err);
+		return err;
+	}
+
+	/* initialize all GPIO as output */
+	outb(0x80, base[id] + 3);
+	outb(0x00, base[id]);
+	outb(0x00, base[id] + 1);
+	outb(0x00, base[id] + 2);
+	outb(0x80, base[id] + 7);
+	outb(0x00, base[id] + 4);
+	outb(0x00, base[id] + 5);
+	outb(0x00, base[id] + 6);
+
+	return 0;
+}
+
+static int gpiomm_remove(struct device *dev, unsigned int id)
+{
+	struct gpiomm_gpio *const gpiommgpio = dev_get_drvdata(dev);
+
+	gpiochip_remove(&gpiommgpio->chip);
+
+	return 0;
+}
+
+static struct isa_driver gpiomm_driver = {
+	.probe = gpiomm_probe,
+	.driver = {
+		.name = "gpio-mm"
+	},
+	.remove = gpiomm_remove
+};
+
+module_isa_driver(gpiomm_driver, num_gpiomm);
+
+MODULE_AUTHOR("William Breathitt Gray <vilhelm.gray@gmail.com>");
+MODULE_DESCRIPTION("Diamond Systems GPIO-MM GPIO driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/mfd/htc-egpio.c b/drivers/gpio/gpio-htc-egpio.c
index 513cfc5c8fb6..0b4df6051097 100644
--- a/drivers/mfd/htc-egpio.c
+++ b/drivers/gpio/gpio-htc-egpio.c
@@ -14,10 +14,10 @@
 #include <linux/irq.h>
 #include <linux/io.h>
 #include <linux/spinlock.h>
+#include <linux/platform_data/gpio-htc-egpio.h>
 #include <linux/platform_device.h>
 #include <linux/slab.h>
 #include <linux/module.h>
-#include <linux/mfd/htc-egpio.h>
 
 struct egpio_chip {
 	int              reg_start;
diff --git a/drivers/gpio/gpio-iop.c b/drivers/gpio/gpio-iop.c
index 860c535922fd..98c7ff2a76e7 100644
--- a/drivers/gpio/gpio-iop.c
+++ b/drivers/gpio/gpio-iop.c
@@ -10,111 +10,40 @@
  * your option) any later version.
  */
 
-#include <linux/device.h>
-#include <linux/init.h>
-#include <linux/types.h>
-#include <linux/errno.h>
-#include <linux/gpio.h>
-#include <linux/export.h>
+#include <linux/err.h>
+#include <linux/module.h>
+#include <linux/gpio/driver.h>
 #include <linux/platform_device.h>
-#include <linux/bitops.h>
-#include <linux/io.h>
 
-#define IOP3XX_N_GPIOS	8
-
-#define GPIO_IN			0
-#define GPIO_OUT		1
-#define GPIO_LOW		0
-#define GPIO_HIGH		1
-
-/* Memory base offset */
-static void __iomem *base;
-
-#define IOP3XX_GPIO_REG(reg)	(base + (reg))
-#define IOP3XX_GPOE		IOP3XX_GPIO_REG(0x0000)
-#define IOP3XX_GPID		IOP3XX_GPIO_REG(0x0004)
-#define IOP3XX_GPOD		IOP3XX_GPIO_REG(0x0008)
-
-static void gpio_line_config(int line, int direction)
-{
-	unsigned long flags;
-	u32 val;
-
-	local_irq_save(flags);
-	val = readl(IOP3XX_GPOE);
-	if (direction == GPIO_IN) {
-		val |= BIT(line);
-	} else if (direction == GPIO_OUT) {
-		val &= ~BIT(line);
-	}
-	writel(val, IOP3XX_GPOE);
-	local_irq_restore(flags);
-}
-
-static int gpio_line_get(int line)
-{
-	return !!(readl(IOP3XX_GPID) & BIT(line));
-}
-
-static void gpio_line_set(int line, int value)
-{
-	unsigned long flags;
-	u32 val;
-
-	local_irq_save(flags);
-	val = readl(IOP3XX_GPOD);
-	if (value == GPIO_LOW) {
-		val &= ~BIT(line);
-	} else if (value == GPIO_HIGH) {
-		val |= BIT(line);
-	}
-	writel(val, IOP3XX_GPOD);
-	local_irq_restore(flags);
-}
-
-static int iop3xx_gpio_direction_input(struct gpio_chip *chip, unsigned gpio)
-{
-	gpio_line_config(gpio, GPIO_IN);
-	return 0;
-}
-
-static int iop3xx_gpio_direction_output(struct gpio_chip *chip, unsigned gpio, int level)
-{
-	gpio_line_set(gpio, level);
-	gpio_line_config(gpio, GPIO_OUT);
-	return 0;
-}
-
-static int iop3xx_gpio_get_value(struct gpio_chip *chip, unsigned gpio)
-{
-	return gpio_line_get(gpio);
-}
-
-static void iop3xx_gpio_set_value(struct gpio_chip *chip, unsigned gpio, int value)
-{
-	gpio_line_set(gpio, value);
-}
-
-static struct gpio_chip iop3xx_chip = {
-	.label			= "iop3xx",
-	.direction_input	= iop3xx_gpio_direction_input,
-	.get			= iop3xx_gpio_get_value,
-	.direction_output	= iop3xx_gpio_direction_output,
-	.set			= iop3xx_gpio_set_value,
-	.base			= 0,
-	.ngpio			= IOP3XX_N_GPIOS,
-};
+#define IOP3XX_GPOE	0x0000
+#define IOP3XX_GPID	0x0004
+#define IOP3XX_GPOD	0x0008
 
 static int iop3xx_gpio_probe(struct platform_device *pdev)
 {
 	struct resource *res;
+	struct gpio_chip *gc;
+	void __iomem *base;
+	int err;
+
+	gc = devm_kzalloc(&pdev->dev, sizeof(*gc), GFP_KERNEL);
+	if (!gc)
+		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);
 
-	return devm_gpiochip_add_data(&pdev->dev, &iop3xx_chip, NULL);
+	err = bgpio_init(gc, &pdev->dev, 1, base + IOP3XX_GPID,
+			 base + IOP3XX_GPOD, NULL, NULL, base + IOP3XX_GPOE, 0);
+	if (err)
+		return err;
+
+	gc->base = 0;
+	gc->owner = THIS_MODULE;
+
+	return devm_gpiochip_add_data(&pdev->dev, gc, NULL);
 }
 
 static struct platform_driver iop3xx_gpio_driver = {
diff --git a/drivers/gpio/gpio-it87.c b/drivers/gpio/gpio-it87.c
index 63a962d18cd6..45d29e488dbb 100644
--- a/drivers/gpio/gpio-it87.c
+++ b/drivers/gpio/gpio-it87.c
@@ -273,7 +273,7 @@ exit:
 	return rc;
 }
 
-static struct gpio_chip it87_template_chip = {
+static const struct gpio_chip it87_template_chip = {
 	.label			= KBUILD_MODNAME,
 	.owner			= THIS_MODULE,
 	.request		= it87_gpio_request,
diff --git a/drivers/gpio/gpio-loongson1.c b/drivers/gpio/gpio-loongson1.c
index 10c09bdd8514..72b64039241a 100644
--- a/drivers/gpio/gpio-loongson1.c
+++ b/drivers/gpio/gpio-loongson1.c
@@ -8,6 +8,7 @@
  * warranty of any kind, whether express or implied.
  */
 
+#include <linux/module.h>
 #include <linux/gpio/driver.h>
 #include <linux/platform_device.h>
 
@@ -55,11 +56,6 @@ static int ls1x_gpio_probe(struct platform_device *pdev)
 		return -ENOMEM;
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		dev_err(dev, "failed to get I/O memory\n");
-		return -EINVAL;
-	}
-
 	gpio_reg_base = devm_ioremap_resource(dev, res);
 	if (IS_ERR(gpio_reg_base))
 		return PTR_ERR(gpio_reg_base);
diff --git a/drivers/gpio/gpio-lp873x.c b/drivers/gpio/gpio-lp873x.c
new file mode 100644
index 000000000000..218c706359aa
--- /dev/null
+++ b/drivers/gpio/gpio-lp873x.c
@@ -0,0 +1,183 @@
+/*
+ * Copyright (C) 2016 Texas Instruments Incorporated - http://www.ti.com/
+ *	Keerthy <j-keerthy@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 "as is" WITHOUT ANY WARRANTY of any
+ * kind, whether expressed or implied; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License version 2 for more details.
+ *
+ * Based on the TPS65218 driver
+ */
+
+#include <linux/gpio.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+
+#include <linux/mfd/lp873x.h>
+
+#define BITS_PER_GPO		0x4
+#define LP873X_GPO_CTRL_OD	0x2
+
+struct lp873x_gpio {
+	struct gpio_chip chip;
+	struct lp873x *lp873;
+};
+
+static int lp873x_gpio_get_direction(struct gpio_chip *chip,
+				     unsigned int offset)
+{
+	/* This device is output only */
+	return 0;
+}
+
+static int lp873x_gpio_direction_input(struct gpio_chip *chip,
+				       unsigned int offset)
+{
+	/* This device is output only */
+	return -EINVAL;
+}
+
+static int lp873x_gpio_direction_output(struct gpio_chip *chip,
+					unsigned int offset, int value)
+{
+	struct lp873x_gpio *gpio = gpiochip_get_data(chip);
+
+	/* Set the initial value */
+	return regmap_update_bits(gpio->lp873->regmap, LP873X_REG_GPO_CTRL,
+				  BIT(offset * BITS_PER_GPO),
+				  value ? BIT(offset * BITS_PER_GPO) : 0);
+}
+
+static int lp873x_gpio_get(struct gpio_chip *chip, unsigned int offset)
+{
+	struct lp873x_gpio *gpio = gpiochip_get_data(chip);
+	int ret, val;
+
+	ret = regmap_read(gpio->lp873->regmap, LP873X_REG_GPO_CTRL, &val);
+	if (ret < 0)
+		return ret;
+
+	return val & BIT(offset * BITS_PER_GPO);
+}
+
+static void lp873x_gpio_set(struct gpio_chip *chip, unsigned int offset,
+			    int value)
+{
+	struct lp873x_gpio *gpio = gpiochip_get_data(chip);
+
+	regmap_update_bits(gpio->lp873->regmap, LP873X_REG_GPO_CTRL,
+			   BIT(offset * BITS_PER_GPO),
+			   value ? BIT(offset * BITS_PER_GPO) : 0);
+}
+
+static int lp873x_gpio_request(struct gpio_chip *gc, unsigned int offset)
+{
+	struct lp873x_gpio *gpio = gpiochip_get_data(gc);
+	int ret;
+
+	switch (offset) {
+	case 0:
+		/* No MUX Set up Needed for GPO */
+		break;
+	case 1:
+		/* Setup the CLKIN_PIN_SEL MUX to GPO2 */
+		ret = regmap_update_bits(gpio->lp873->regmap, LP873X_REG_CONFIG,
+					 LP873X_CONFIG_CLKIN_PIN_SEL, 0);
+		if (ret)
+			return ret;
+
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int lp873x_gpio_set_single_ended(struct gpio_chip *gc,
+					unsigned int offset,
+					enum single_ended_mode mode)
+{
+	struct lp873x_gpio *gpio = gpiochip_get_data(gc);
+
+	switch (mode) {
+	case LINE_MODE_OPEN_DRAIN:
+		return regmap_update_bits(gpio->lp873->regmap,
+					  LP873X_REG_GPO_CTRL,
+					  BIT(offset * BITS_PER_GPO +
+					  LP873X_GPO_CTRL_OD),
+					  BIT(offset * BITS_PER_GPO +
+					  LP873X_GPO_CTRL_OD));
+	case LINE_MODE_PUSH_PULL:
+		return regmap_update_bits(gpio->lp873->regmap,
+					  LP873X_REG_GPO_CTRL,
+					  BIT(offset * BITS_PER_GPO +
+					  LP873X_GPO_CTRL_OD), 0);
+	default:
+		return -ENOTSUPP;
+	}
+}
+
+static const struct gpio_chip template_chip = {
+	.label			= "lp873x-gpio",
+	.owner			= THIS_MODULE,
+	.request		= lp873x_gpio_request,
+	.get_direction		= lp873x_gpio_get_direction,
+	.direction_input	= lp873x_gpio_direction_input,
+	.direction_output	= lp873x_gpio_direction_output,
+	.get			= lp873x_gpio_get,
+	.set			= lp873x_gpio_set,
+	.set_single_ended	= lp873x_gpio_set_single_ended,
+	.base			= -1,
+	.ngpio			= 2,
+	.can_sleep		= true,
+};
+
+static int lp873x_gpio_probe(struct platform_device *pdev)
+{
+	struct lp873x_gpio *gpio;
+	int ret;
+
+	gpio = devm_kzalloc(&pdev->dev, sizeof(*gpio), GFP_KERNEL);
+	if (!gpio)
+		return -ENOMEM;
+
+	platform_set_drvdata(pdev, gpio);
+
+	gpio->lp873 = dev_get_drvdata(pdev->dev.parent);
+	gpio->chip = template_chip;
+	gpio->chip.parent = gpio->lp873->dev;
+
+	ret = devm_gpiochip_add_data(&pdev->dev, &gpio->chip, gpio);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "Could not register gpiochip, %d\n", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static const struct platform_device_id lp873x_gpio_id_table[] = {
+	{ "lp873x-gpio", },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(platform, lp873x_gpio_id_table);
+
+static struct platform_driver lp873x_gpio_driver = {
+	.driver = {
+		.name = "lp873x-gpio",
+	},
+	.probe = lp873x_gpio_probe,
+	.id_table = lp873x_gpio_id_table,
+};
+module_platform_driver(lp873x_gpio_driver);
+
+MODULE_AUTHOR("Keerthy <j-keerthy@ti.com>");
+MODULE_DESCRIPTION("LP873X GPIO driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/gpio/gpio-lpc18xx.c b/drivers/gpio/gpio-lpc18xx.c
index 98832c9f614a..f12e02e1016d 100644
--- a/drivers/gpio/gpio-lpc18xx.c
+++ b/drivers/gpio/gpio-lpc18xx.c
@@ -78,7 +78,7 @@ static int lpc18xx_gpio_direction_output(struct gpio_chip *chip,
 	return lpc18xx_gpio_direction(chip, offset, true);
 }
 
-static struct gpio_chip lpc18xx_chip = {
+static const struct gpio_chip lpc18xx_chip = {
 	.label			= "lpc18xx/43xx-gpio",
 	.request		= gpiochip_generic_request,
 	.free			= gpiochip_generic_free,
diff --git a/drivers/gpio/gpio-lpc32xx.c b/drivers/gpio/gpio-lpc32xx.c
index fc5f197906ac..92b3ae2a6735 100644
--- a/drivers/gpio/gpio-lpc32xx.c
+++ b/drivers/gpio/gpio-lpc32xx.c
@@ -25,7 +25,6 @@
 #include <linux/of_gpio.h>
 #include <linux/platform_device.h>
 #include <linux/module.h>
-#include <linux/platform_data/gpio-lpc32xx.h>
 
 #include <mach/hardware.h>
 #include <mach/platform.h>
@@ -68,6 +67,20 @@
 #define GPI3_PIN_IN_SEL(x, y)			(((x) >> (y)) & 1)
 #define GPO3_PIN_IN_SEL(x, y)			(((x) >> (y)) & 1)
 
+#define LPC32XX_GPIO_P0_MAX	8
+#define LPC32XX_GPIO_P1_MAX	24
+#define LPC32XX_GPIO_P2_MAX	13
+#define LPC32XX_GPIO_P3_MAX	6
+#define LPC32XX_GPI_P3_MAX	29
+#define LPC32XX_GPO_P3_MAX	24
+
+#define LPC32XX_GPIO_P0_GRP	0
+#define LPC32XX_GPIO_P1_GRP	(LPC32XX_GPIO_P0_GRP + LPC32XX_GPIO_P0_MAX)
+#define LPC32XX_GPIO_P2_GRP	(LPC32XX_GPIO_P1_GRP + LPC32XX_GPIO_P1_MAX)
+#define LPC32XX_GPIO_P3_GRP	(LPC32XX_GPIO_P2_GRP + LPC32XX_GPIO_P2_MAX)
+#define LPC32XX_GPI_P3_GRP	(LPC32XX_GPIO_P3_GRP + LPC32XX_GPIO_P3_MAX)
+#define LPC32XX_GPO_P3_GRP	(LPC32XX_GPI_P3_GRP + LPC32XX_GPI_P3_MAX)
+
 struct gpio_regs {
 	void __iomem *inp_state;
 	void __iomem *outp_state;
diff --git a/drivers/gpio/gpio-mmio.c b/drivers/gpio/gpio-mmio.c
index 6ec144baeb11..d7d03ad052d0 100644
--- a/drivers/gpio/gpio-mmio.c
+++ b/drivers/gpio/gpio-mmio.c
@@ -573,6 +573,7 @@ static void __iomem *bgpio_map(struct platform_device *pdev,
 
 #ifdef CONFIG_OF
 static const struct of_device_id bgpio_of_match[] = {
+	{ .compatible = "brcm,bcm6345-gpio" },
 	{ .compatible = "wd,mbl-gpio" },
 	{ }
 };
@@ -593,6 +594,9 @@ static struct bgpio_pdata *bgpio_parse_dt(struct platform_device *pdev,
 
 	pdata->base = -1;
 
+	if (of_device_is_big_endian(pdev->dev.of_node))
+		*flags |= BGPIOF_BIG_ENDIAN_BYTE_ORDER;
+
 	if (of_property_read_bool(pdev->dev.of_node, "no-output"))
 		*flags |= BGPIOF_NO_OUTPUT;
 
diff --git a/drivers/gpio/gpio-mockup.c b/drivers/gpio/gpio-mockup.c
new file mode 100644
index 000000000000..1ef85b0c2b1f
--- /dev/null
+++ b/drivers/gpio/gpio-mockup.c
@@ -0,0 +1,214 @@
+/*
+ * GPIO Testing Device Driver
+ *
+ * Copyright (C) 2014  Kamlakant Patel <kamlakant.patel@broadcom.com>
+ * Copyright (C) 2015-2016  Bamvor Jian Zhang <bamvor.zhangjian@linaro.org>
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ *
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/gpio/driver.h>
+#include <linux/platform_device.h>
+
+#define GPIO_NAME	"gpio-mockup"
+#define	MAX_GC		10
+
+enum direction {
+	OUT,
+	IN
+};
+
+/*
+ * struct gpio_pin_status - structure describing a GPIO status
+ * @dir:       Configures direction of gpio as "in" or "out", 0=in, 1=out
+ * @value:     Configures status of the gpio as 0(low) or 1(high)
+ */
+struct gpio_pin_status {
+	enum direction dir;
+	bool value;
+};
+
+struct mockup_gpio_controller {
+	struct gpio_chip gc;
+	struct gpio_pin_status *stats;
+};
+
+static int gpio_mockup_ranges[MAX_GC << 1];
+static int gpio_mockup_params_nr;
+module_param_array(gpio_mockup_ranges, int, &gpio_mockup_params_nr, 0400);
+
+const char pins_name_start = 'A';
+
+static int mockup_gpio_get(struct gpio_chip *gc, unsigned int offset)
+{
+	struct mockup_gpio_controller *cntr = gpiochip_get_data(gc);
+
+	return cntr->stats[offset].value;
+}
+
+static void mockup_gpio_set(struct gpio_chip *gc, unsigned int offset,
+			    int value)
+{
+	struct mockup_gpio_controller *cntr = gpiochip_get_data(gc);
+
+	cntr->stats[offset].value = !!value;
+}
+
+static int mockup_gpio_dirout(struct gpio_chip *gc, unsigned int offset,
+			      int value)
+{
+	struct mockup_gpio_controller *cntr = gpiochip_get_data(gc);
+
+	mockup_gpio_set(gc, offset, value);
+	cntr->stats[offset].dir = OUT;
+	return 0;
+}
+
+static int mockup_gpio_dirin(struct gpio_chip *gc, unsigned int offset)
+{
+	struct mockup_gpio_controller *cntr = gpiochip_get_data(gc);
+
+	cntr->stats[offset].dir = IN;
+	return 0;
+}
+
+static int mockup_gpio_get_direction(struct gpio_chip *gc, unsigned int offset)
+{
+	struct mockup_gpio_controller *cntr = gpiochip_get_data(gc);
+
+	return cntr->stats[offset].dir;
+}
+
+static int mockup_gpio_add(struct device *dev,
+			   struct mockup_gpio_controller *cntr,
+			   const char *name, int base, int ngpio)
+{
+	int ret;
+
+	cntr->gc.base = base;
+	cntr->gc.ngpio = ngpio;
+	cntr->gc.label = name;
+	cntr->gc.owner = THIS_MODULE;
+	cntr->gc.parent = dev;
+	cntr->gc.get = mockup_gpio_get;
+	cntr->gc.set = mockup_gpio_set;
+	cntr->gc.direction_output = mockup_gpio_dirout;
+	cntr->gc.direction_input = mockup_gpio_dirin;
+	cntr->gc.get_direction = mockup_gpio_get_direction;
+	cntr->stats = devm_kzalloc(dev, sizeof(*cntr->stats) * cntr->gc.ngpio,
+				   GFP_KERNEL);
+	if (!cntr->stats) {
+		ret = -ENOMEM;
+		goto err;
+	}
+	ret = devm_gpiochip_add_data(dev, &cntr->gc, cntr);
+	if (ret)
+		goto err;
+
+	dev_info(dev, "gpio<%d..%d> add successful!", base, base + ngpio);
+	return 0;
+err:
+	dev_err(dev, "gpio<%d..%d> add failed!", base, base + ngpio);
+	return ret;
+}
+
+static int mockup_gpio_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct mockup_gpio_controller *cntr;
+	int ret;
+	int i;
+	int base;
+	int ngpio;
+	char chip_name[sizeof(GPIO_NAME) + 3];
+
+	if (gpio_mockup_params_nr < 2)
+		return -EINVAL;
+
+	cntr = devm_kzalloc(dev, sizeof(*cntr) * (gpio_mockup_params_nr >> 1),
+			    GFP_KERNEL);
+	if (!cntr)
+		return -ENOMEM;
+
+	platform_set_drvdata(pdev, cntr);
+
+	for (i = 0; i < gpio_mockup_params_nr >> 1; i++) {
+		base = gpio_mockup_ranges[i * 2];
+		if (base == -1)
+			ngpio = gpio_mockup_ranges[i * 2 + 1];
+		else
+			ngpio = gpio_mockup_ranges[i * 2 + 1] - base;
+
+		if (ngpio >= 0) {
+			sprintf(chip_name, "%s-%c", GPIO_NAME,
+				pins_name_start + i);
+			ret = mockup_gpio_add(dev, &cntr[i],
+					      chip_name, base, ngpio);
+		} else {
+			ret = -1;
+		}
+		if (ret) {
+			if (base < 0)
+				dev_err(dev, "gpio<%d..%d> add failed\n",
+					base, ngpio);
+			else
+				dev_err(dev, "gpio<%d..%d> add failed\n",
+					base, base + ngpio);
+
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+static struct platform_driver mockup_gpio_driver = {
+	.driver = {
+		   .name = GPIO_NAME,
+		   },
+	.probe = mockup_gpio_probe,
+};
+
+static struct platform_device *pdev;
+static int __init mock_device_init(void)
+{
+	int err;
+
+	pdev = platform_device_alloc(GPIO_NAME, -1);
+	if (!pdev)
+		return -ENOMEM;
+
+	err = platform_device_add(pdev);
+	if (err) {
+		platform_device_put(pdev);
+		return err;
+	}
+
+	err = platform_driver_register(&mockup_gpio_driver);
+	if (err) {
+		platform_device_unregister(pdev);
+		return err;
+	}
+
+	return 0;
+}
+
+static void __exit mock_device_exit(void)
+{
+	platform_driver_unregister(&mockup_gpio_driver);
+	platform_device_unregister(pdev);
+}
+
+module_init(mock_device_init);
+module_exit(mock_device_exit);
+
+MODULE_AUTHOR("Kamlakant Patel <kamlakant.patel@broadcom.com>");
+MODULE_AUTHOR("Bamvor Jian Zhang <bamvor.zhangjian@linaro.org>");
+MODULE_DESCRIPTION("GPIO Testing driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/gpio/gpio-msic.c b/drivers/gpio/gpio-msic.c
index d75649787e6c..1b7ce7f85886 100644
--- a/drivers/gpio/gpio-msic.c
+++ b/drivers/gpio/gpio-msic.c
@@ -20,7 +20,6 @@
  *
  */
 
-#include <linux/module.h>
 #include <linux/kernel.h>
 #include <linux/slab.h>
 #include <linux/interrupt.h>
@@ -328,9 +327,4 @@ static int __init platform_msic_gpio_init(void)
 {
 	return platform_driver_register(&platform_msic_gpio_driver);
 }
-
 subsys_initcall(platform_msic_gpio_init);
-
-MODULE_AUTHOR("Mathias Nyman <mathias.nyman@linux.intel.com>");
-MODULE_DESCRIPTION("Intel Medfield MSIC GPIO driver");
-MODULE_LICENSE("GPL v2");
diff --git a/drivers/gpio/gpio-mxc.c b/drivers/gpio/gpio-mxc.c
index 1b342a3842c8..c1a1e00b8cb0 100644
--- a/drivers/gpio/gpio-mxc.c
+++ b/drivers/gpio/gpio-mxc.c
@@ -2,7 +2,8 @@
  * MXC GPIO support. (c) 2008 Daniel Mack <daniel@caiaq.de>
  * Copyright 2008 Juergen Beisert, kernel@pengutronix.de
  *
- * Based on code from Freescale,
+ * Based on code from Freescale Semiconductor,
+ * Authors: Daniel Mack, Juergen Beisert.
  * Copyright (C) 2004-2010 Freescale Semiconductor, Inc. All Rights Reserved.
  *
  * This program is free software; you can redistribute it and/or
@@ -33,7 +34,6 @@
 #include <linux/gpio.h>
 #include <linux/of.h>
 #include <linux/of_device.h>
-#include <linux/module.h>
 #include <linux/bug.h>
 
 enum mxc_gpio_hwtype {
@@ -458,6 +458,11 @@ static int mxc_gpio_probe(struct platform_device *pdev)
 	if (err)
 		goto out_bgio;
 
+	if (of_property_read_bool(np, "gpio-ranges")) {
+		port->gc.request = gpiochip_generic_request;
+		port->gc.free = gpiochip_generic_free;
+	}
+
 	port->gc.to_irq = mxc_gpio_to_irq;
 	port->gc.base = (pdev->id < 0) ? of_alias_get_id(np, "gpio") * 32 :
 					     pdev->id * 32;
@@ -510,10 +515,4 @@ static int __init gpio_mxc_init(void)
 {
 	return platform_driver_register(&mxc_gpio_driver);
 }
-postcore_initcall(gpio_mxc_init);
-
-MODULE_AUTHOR("Freescale Semiconductor, "
-	      "Daniel Mack <danielncaiaq.de>, "
-	      "Juergen Beisert <kernel@pengutronix.de>");
-MODULE_DESCRIPTION("Freescale MXC GPIO");
-MODULE_LICENSE("GPL");
+subsys_initcall(gpio_mxc_init);
diff --git a/drivers/gpio/gpio-palmas.c b/drivers/gpio/gpio-palmas.c
index 839474430229..3d818195e351 100644
--- a/drivers/gpio/gpio-palmas.c
+++ b/drivers/gpio/gpio-palmas.c
@@ -152,7 +152,6 @@ static const struct of_device_id of_palmas_gpio_match[] = {
 	{ .compatible = "ti,tps80036-gpio", .data = &tps80036_dev_data,},
 	{ },
 };
-MODULE_DEVICE_TABLE(of, of_palmas_gpio_match);
 
 static int palmas_gpio_probe(struct platform_device *pdev)
 {
diff --git a/drivers/gpio/gpio-pca953x.c b/drivers/gpio/gpio-pca953x.c
index 02f2a5621bb0..e422568e14ad 100644
--- a/drivers/gpio/gpio-pca953x.c
+++ b/drivers/gpio/gpio-pca953x.c
@@ -21,6 +21,7 @@
 #include <asm/unaligned.h>
 #include <linux/of_platform.h>
 #include <linux/acpi.h>
+#include <linux/regulator/consumer.h>
 
 #define PCA953X_INPUT		0
 #define PCA953X_OUTPUT		1
@@ -94,6 +95,24 @@ MODULE_DEVICE_TABLE(acpi, pca953x_acpi_ids);
 
 #define NBANK(chip) DIV_ROUND_UP(chip->gpio_chip.ngpio, BANK_SZ)
 
+struct pca953x_reg_config {
+	int direction;
+	int output;
+	int input;
+};
+
+static const struct pca953x_reg_config pca953x_regs = {
+	.direction = PCA953X_DIRECTION,
+	.output = PCA953X_OUTPUT,
+	.input = PCA953X_INPUT,
+};
+
+static const struct pca953x_reg_config pca957x_regs = {
+	.direction = PCA957X_CFG,
+	.output = PCA957X_OUT,
+	.input = PCA957X_IN,
+};
+
 struct pca953x_chip {
 	unsigned gpio_start;
 	u8 reg_output[MAX_BANK];
@@ -111,8 +130,13 @@ struct pca953x_chip {
 	struct i2c_client *client;
 	struct gpio_chip gpio_chip;
 	const char *const *names;
-	int	chip_type;
 	unsigned long driver_data;
+	struct regulator *regulator;
+
+	const struct pca953x_reg_config *regs;
+
+	int (*write_regs)(struct pca953x_chip *, int, u8 *);
+	int (*read_regs)(struct pca953x_chip *, int, u8 *);
 };
 
 static int pca953x_read_single(struct pca953x_chip *chip, int reg, u32 *val,
@@ -152,38 +176,44 @@ static int pca953x_write_single(struct pca953x_chip *chip, int reg, u32 val,
 	return 0;
 }
 
-static int pca953x_write_regs(struct pca953x_chip *chip, int reg, u8 *val)
+static int pca953x_write_regs_8(struct pca953x_chip *chip, int reg, u8 *val)
 {
-	int ret = 0;
+	return i2c_smbus_write_byte_data(chip->client, reg, *val);
+}
 
-	if (chip->gpio_chip.ngpio <= 8)
-		ret = i2c_smbus_write_byte_data(chip->client, reg, *val);
-	else if (chip->gpio_chip.ngpio >= 24) {
-		int bank_shift = fls((chip->gpio_chip.ngpio - 1) / BANK_SZ);
-		ret = i2c_smbus_write_i2c_block_data(chip->client,
-					(reg << bank_shift) | REG_ADDR_AI,
-					NBANK(chip), val);
-	} else {
-		switch (chip->chip_type) {
-		case PCA953X_TYPE: {
-			__le16 word = cpu_to_le16(get_unaligned((u16 *)val));
+static int pca953x_write_regs_16(struct pca953x_chip *chip, int reg, u8 *val)
+{
+	__le16 word = cpu_to_le16(get_unaligned((u16 *)val));
 
-			ret = i2c_smbus_write_word_data(chip->client, reg << 1,
-							(__force u16)word);
-			break;
-		}
-		case PCA957X_TYPE:
-			ret = i2c_smbus_write_byte_data(chip->client, reg << 1,
-							val[0]);
-			if (ret < 0)
-				break;
-			ret = i2c_smbus_write_byte_data(chip->client,
-							(reg << 1) + 1,
-							val[1]);
-			break;
-		}
-	}
+	return i2c_smbus_write_word_data(chip->client,
+					 reg << 1, (__force u16)word);
+}
+
+static int pca957x_write_regs_16(struct pca953x_chip *chip, int reg, u8 *val)
+{
+	int ret;
+
+	ret = i2c_smbus_write_byte_data(chip->client, reg << 1, val[0]);
+	if (ret < 0)
+		return ret;
+
+	return i2c_smbus_write_byte_data(chip->client, (reg << 1) + 1, val[1]);
+}
+
+static int pca953x_write_regs_24(struct pca953x_chip *chip, int reg, u8 *val)
+{
+	int bank_shift = fls((chip->gpio_chip.ngpio - 1) / BANK_SZ);
+
+	return i2c_smbus_write_i2c_block_data(chip->client,
+					      (reg << bank_shift) | REG_ADDR_AI,
+					      NBANK(chip), val);
+}
 
+static int pca953x_write_regs(struct pca953x_chip *chip, int reg, u8 *val)
+{
+	int ret = 0;
+
+	ret = chip->write_regs(chip, reg, val);
 	if (ret < 0) {
 		dev_err(&chip->client->dev, "failed writing register\n");
 		return ret;
@@ -192,24 +222,41 @@ static int pca953x_write_regs(struct pca953x_chip *chip, int reg, u8 *val)
 	return 0;
 }
 
-static int pca953x_read_regs(struct pca953x_chip *chip, int reg, u8 *val)
+static int pca953x_read_regs_8(struct pca953x_chip *chip, int reg, u8 *val)
 {
 	int ret;
 
-	if (chip->gpio_chip.ngpio <= 8) {
-		ret = i2c_smbus_read_byte_data(chip->client, reg);
-		*val = ret;
-	} else if (chip->gpio_chip.ngpio >= 24) {
-		int bank_shift = fls((chip->gpio_chip.ngpio - 1) / BANK_SZ);
+	ret = i2c_smbus_read_byte_data(chip->client, reg);
+	*val = ret;
 
-		ret = i2c_smbus_read_i2c_block_data(chip->client,
-					(reg << bank_shift) | REG_ADDR_AI,
-					NBANK(chip), val);
-	} else {
-		ret = i2c_smbus_read_word_data(chip->client, reg << 1);
-		val[0] = (u16)ret & 0xFF;
-		val[1] = (u16)ret >> 8;
-	}
+	return ret;
+}
+
+static int pca953x_read_regs_16(struct pca953x_chip *chip, int reg, u8 *val)
+{
+	int ret;
+
+	ret = i2c_smbus_read_word_data(chip->client, reg << 1);
+	val[0] = (u16)ret & 0xFF;
+	val[1] = (u16)ret >> 8;
+
+	return ret;
+}
+
+static int pca953x_read_regs_24(struct pca953x_chip *chip, int reg, u8 *val)
+{
+	int bank_shift = fls((chip->gpio_chip.ngpio - 1) / BANK_SZ);
+
+	return i2c_smbus_read_i2c_block_data(chip->client,
+					     (reg << bank_shift) | REG_ADDR_AI,
+					     NBANK(chip), val);
+}
+
+static int pca953x_read_regs(struct pca953x_chip *chip, int reg, u8 *val)
+{
+	int ret;
+
+	ret = chip->read_regs(chip, reg, val);
 	if (ret < 0) {
 		dev_err(&chip->client->dev, "failed reading register\n");
 		return ret;
@@ -222,20 +269,12 @@ static int pca953x_gpio_direction_input(struct gpio_chip *gc, unsigned off)
 {
 	struct pca953x_chip *chip = gpiochip_get_data(gc);
 	u8 reg_val;
-	int ret, offset = 0;
+	int ret;
 
 	mutex_lock(&chip->i2c_lock);
 	reg_val = chip->reg_direction[off / BANK_SZ] | (1u << (off % BANK_SZ));
 
-	switch (chip->chip_type) {
-	case PCA953X_TYPE:
-		offset = PCA953X_DIRECTION;
-		break;
-	case PCA957X_TYPE:
-		offset = PCA957X_CFG;
-		break;
-	}
-	ret = pca953x_write_single(chip, offset, reg_val, off);
+	ret = pca953x_write_single(chip, chip->regs->direction, reg_val, off);
 	if (ret)
 		goto exit;
 
@@ -250,7 +289,7 @@ static int pca953x_gpio_direction_output(struct gpio_chip *gc,
 {
 	struct pca953x_chip *chip = gpiochip_get_data(gc);
 	u8 reg_val;
-	int ret, offset = 0;
+	int ret;
 
 	mutex_lock(&chip->i2c_lock);
 	/* set output level */
@@ -261,15 +300,7 @@ static int pca953x_gpio_direction_output(struct gpio_chip *gc,
 		reg_val = chip->reg_output[off / BANK_SZ]
 			& ~(1u << (off % BANK_SZ));
 
-	switch (chip->chip_type) {
-	case PCA953X_TYPE:
-		offset = PCA953X_OUTPUT;
-		break;
-	case PCA957X_TYPE:
-		offset = PCA957X_OUT;
-		break;
-	}
-	ret = pca953x_write_single(chip, offset, reg_val, off);
+	ret = pca953x_write_single(chip, chip->regs->output, reg_val, off);
 	if (ret)
 		goto exit;
 
@@ -277,15 +308,7 @@ static int pca953x_gpio_direction_output(struct gpio_chip *gc,
 
 	/* then direction */
 	reg_val = chip->reg_direction[off / BANK_SZ] & ~(1u << (off % BANK_SZ));
-	switch (chip->chip_type) {
-	case PCA953X_TYPE:
-		offset = PCA953X_DIRECTION;
-		break;
-	case PCA957X_TYPE:
-		offset = PCA957X_CFG;
-		break;
-	}
-	ret = pca953x_write_single(chip, offset, reg_val, off);
+	ret = pca953x_write_single(chip, chip->regs->direction, reg_val, off);
 	if (ret)
 		goto exit;
 
@@ -299,18 +322,10 @@ static int pca953x_gpio_get_value(struct gpio_chip *gc, unsigned off)
 {
 	struct pca953x_chip *chip = gpiochip_get_data(gc);
 	u32 reg_val;
-	int ret, offset = 0;
+	int ret;
 
 	mutex_lock(&chip->i2c_lock);
-	switch (chip->chip_type) {
-	case PCA953X_TYPE:
-		offset = PCA953X_INPUT;
-		break;
-	case PCA957X_TYPE:
-		offset = PCA957X_IN;
-		break;
-	}
-	ret = pca953x_read_single(chip, offset, &reg_val, off);
+	ret = pca953x_read_single(chip, chip->regs->input, &reg_val, off);
 	mutex_unlock(&chip->i2c_lock);
 	if (ret < 0) {
 		/* NOTE:  diagnostic already emitted; that's all we should
@@ -327,7 +342,7 @@ static void pca953x_gpio_set_value(struct gpio_chip *gc, unsigned off, int val)
 {
 	struct pca953x_chip *chip = gpiochip_get_data(gc);
 	u8 reg_val;
-	int ret, offset = 0;
+	int ret;
 
 	mutex_lock(&chip->i2c_lock);
 	if (val)
@@ -337,15 +352,7 @@ static void pca953x_gpio_set_value(struct gpio_chip *gc, unsigned off, int val)
 		reg_val = chip->reg_output[off / BANK_SZ]
 			& ~(1u << (off % BANK_SZ));
 
-	switch (chip->chip_type) {
-	case PCA953X_TYPE:
-		offset = PCA953X_OUTPUT;
-		break;
-	case PCA957X_TYPE:
-		offset = PCA957X_OUT;
-		break;
-	}
-	ret = pca953x_write_single(chip, offset, reg_val, off);
+	ret = pca953x_write_single(chip, chip->regs->output, reg_val, off);
 	if (ret)
 		goto exit;
 
@@ -355,35 +362,31 @@ exit:
 }
 
 static void pca953x_gpio_set_multiple(struct gpio_chip *gc,
-		unsigned long *mask, unsigned long *bits)
+				      unsigned long *mask, unsigned long *bits)
 {
 	struct pca953x_chip *chip = gpiochip_get_data(gc);
+	unsigned int bank_mask, bank_val;
+	int bank_shift, bank;
 	u8 reg_val[MAX_BANK];
-	int ret, offset = 0;
-	int bank_shift = fls((chip->gpio_chip.ngpio - 1) / BANK_SZ);
-	int bank;
-
-	switch (chip->chip_type) {
-	case PCA953X_TYPE:
-		offset = PCA953X_OUTPUT;
-		break;
-	case PCA957X_TYPE:
-		offset = PCA957X_OUT;
-		break;
-	}
+	int ret;
+
+	bank_shift = fls((chip->gpio_chip.ngpio - 1) / BANK_SZ);
 
 	memcpy(reg_val, chip->reg_output, NBANK(chip));
 	mutex_lock(&chip->i2c_lock);
-	for(bank=0; bank<NBANK(chip); bank++) {
-		unsigned bankmask = mask[bank / sizeof(*mask)] >>
-				    ((bank % sizeof(*mask)) * 8);
-		if(bankmask) {
-			unsigned bankval  = bits[bank / sizeof(*bits)] >>
-					    ((bank % sizeof(*bits)) * 8);
-			reg_val[bank] = (reg_val[bank] & ~bankmask) | bankval;
+	for (bank = 0; bank < NBANK(chip); bank++) {
+		bank_mask = mask[bank / sizeof(*mask)] >>
+			   ((bank % sizeof(*mask)) * 8);
+		if (bank_mask) {
+			bank_val = bits[bank / sizeof(*bits)] >>
+				  ((bank % sizeof(*bits)) * 8);
+			reg_val[bank] = (reg_val[bank] & ~bank_mask) | bank_val;
 		}
 	}
-	ret = i2c_smbus_write_i2c_block_data(chip->client, offset << bank_shift, NBANK(chip), reg_val);
+
+	ret = i2c_smbus_write_i2c_block_data(chip->client,
+					     chip->regs->output << bank_shift,
+					     NBANK(chip), reg_val);
 	if (ret)
 		goto exit;
 
@@ -515,7 +518,7 @@ static bool pca953x_irq_pending(struct pca953x_chip *chip, u8 *pending)
 	bool pending_seen = false;
 	bool trigger_seen = false;
 	u8 trigger[MAX_BANK];
-	int ret, i, offset = 0;
+	int ret, i;
 
 	if (chip->driver_data & PCA_PCAL) {
 		/* Read the current interrupt status from the device */
@@ -540,15 +543,7 @@ static bool pca953x_irq_pending(struct pca953x_chip *chip, u8 *pending)
 		return pending_seen;
 	}
 
-	switch (chip->chip_type) {
-	case PCA953X_TYPE:
-		offset = PCA953X_INPUT;
-		break;
-	case PCA957X_TYPE:
-		offset = PCA957X_IN;
-		break;
-	}
-	ret = pca953x_read_regs(chip, offset, cur_stat);
+	ret = pca953x_read_regs(chip, chip->regs->input, cur_stat);
 	if (ret)
 		return false;
 
@@ -608,20 +603,13 @@ static int pca953x_irq_setup(struct pca953x_chip *chip,
 			     int irq_base)
 {
 	struct i2c_client *client = chip->client;
-	int ret, i, offset = 0;
+	int ret, i;
 
 	if (client->irq && irq_base != -1
 			&& (chip->driver_data & PCA_INT)) {
 
-		switch (chip->chip_type) {
-		case PCA953X_TYPE:
-			offset = PCA953X_INPUT;
-			break;
-		case PCA957X_TYPE:
-			offset = PCA957X_IN;
-			break;
-		}
-		ret = pca953x_read_regs(chip, offset, chip->irq_stat);
+		ret = pca953x_read_regs(chip,
+					chip->regs->input, chip->irq_stat);
 		if (ret)
 			return ret;
 
@@ -684,12 +672,14 @@ static int device_pca953x_init(struct pca953x_chip *chip, u32 invert)
 	int ret;
 	u8 val[MAX_BANK];
 
-	ret = pca953x_read_regs(chip, PCA953X_OUTPUT, chip->reg_output);
+	chip->regs = &pca953x_regs;
+
+	ret = pca953x_read_regs(chip, chip->regs->output, chip->reg_output);
 	if (ret)
 		goto out;
 
-	ret = pca953x_read_regs(chip, PCA953X_DIRECTION,
-			       chip->reg_direction);
+	ret = pca953x_read_regs(chip, chip->regs->direction,
+				chip->reg_direction);
 	if (ret)
 		goto out;
 
@@ -709,10 +699,13 @@ static int device_pca957x_init(struct pca953x_chip *chip, u32 invert)
 	int ret;
 	u8 val[MAX_BANK];
 
-	ret = pca953x_read_regs(chip, PCA957X_OUT, chip->reg_output);
+	chip->regs = &pca957x_regs;
+
+	ret = pca953x_read_regs(chip, chip->regs->output, chip->reg_output);
 	if (ret)
 		goto out;
-	ret = pca953x_read_regs(chip, PCA957X_CFG, chip->reg_direction);
+	ret = pca953x_read_regs(chip, chip->regs->direction,
+				chip->reg_direction);
 	if (ret)
 		goto out;
 
@@ -739,13 +732,14 @@ out:
 static const struct of_device_id pca953x_dt_ids[];
 
 static int pca953x_probe(struct i2c_client *client,
-				   const struct i2c_device_id *id)
+				   const struct i2c_device_id *i2c_id)
 {
 	struct pca953x_platform_data *pdata;
 	struct pca953x_chip *chip;
 	int irq_base = 0;
 	int ret;
 	u32 invert = 0;
+	struct regulator *reg;
 
 	chip = devm_kzalloc(&client->dev,
 			sizeof(struct pca953x_chip), GFP_KERNEL);
@@ -765,47 +759,93 @@ static int pca953x_probe(struct i2c_client *client,
 
 	chip->client = client;
 
-	if (id) {
-		chip->driver_data = id->driver_data;
+	reg = devm_regulator_get(&client->dev, "vcc");
+	if (IS_ERR(reg)) {
+		ret = PTR_ERR(reg);
+		if (ret != -EPROBE_DEFER)
+			dev_err(&client->dev, "reg get err: %d\n", ret);
+		return ret;
+	}
+	ret = regulator_enable(reg);
+	if (ret) {
+		dev_err(&client->dev, "reg en err: %d\n", ret);
+		return ret;
+	}
+	chip->regulator = reg;
+
+	if (i2c_id) {
+		chip->driver_data = i2c_id->driver_data;
 	} else {
-		const struct acpi_device_id *id;
+		const struct acpi_device_id *acpi_id;
 		const struct of_device_id *match;
 
 		match = of_match_device(pca953x_dt_ids, &client->dev);
 		if (match) {
 			chip->driver_data = (int)(uintptr_t)match->data;
 		} else {
-			id = acpi_match_device(pca953x_acpi_ids, &client->dev);
-			if (!id)
-				return -ENODEV;
+			acpi_id = acpi_match_device(pca953x_acpi_ids, &client->dev);
+			if (!acpi_id) {
+				ret = -ENODEV;
+				goto err_exit;
+			}
 
-			chip->driver_data = id->driver_data;
+			chip->driver_data = acpi_id->driver_data;
 		}
 	}
 
-	chip->chip_type = PCA_CHIP_TYPE(chip->driver_data);
-
 	mutex_init(&chip->i2c_lock);
+	/*
+	 * In case we have an i2c-mux controlled by a GPIO provided by an
+	 * expander using the same driver higher on the device tree, read the
+	 * i2c adapter nesting depth and use the retrieved value as lockdep
+	 * subclass for chip->i2c_lock.
+	 *
+	 * REVISIT: This solution is not complete. It protects us from lockdep
+	 * false positives when the expander controlling the i2c-mux is on
+	 * a different level on the device tree, but not when it's on the same
+	 * level on a different branch (in which case the subclass number
+	 * would be the same).
+	 *
+	 * TODO: Once a correct solution is developed, a similar fix should be
+	 * applied to all other i2c-controlled GPIO expanders (and potentially
+	 * regmap-i2c).
+	 */
+	lockdep_set_subclass(&chip->i2c_lock,
+			     i2c_adapter_depth(client->adapter));
 
 	/* initialize cached registers from their original values.
 	 * we can't share this chip with another i2c master.
 	 */
 	pca953x_setup_gpio(chip, chip->driver_data & PCA_GPIO_MASK);
 
-	if (chip->chip_type == PCA953X_TYPE)
+	if (chip->gpio_chip.ngpio <= 8) {
+		chip->write_regs = pca953x_write_regs_8;
+		chip->read_regs = pca953x_read_regs_8;
+	} else if (chip->gpio_chip.ngpio >= 24) {
+		chip->write_regs = pca953x_write_regs_24;
+		chip->read_regs = pca953x_read_regs_24;
+	} else {
+		if (PCA_CHIP_TYPE(chip->driver_data) == PCA953X_TYPE)
+			chip->write_regs = pca953x_write_regs_16;
+		else
+			chip->write_regs = pca957x_write_regs_16;
+		chip->read_regs = pca953x_read_regs_16;
+	}
+
+	if (PCA_CHIP_TYPE(chip->driver_data) == PCA953X_TYPE)
 		ret = device_pca953x_init(chip, invert);
 	else
 		ret = device_pca957x_init(chip, invert);
 	if (ret)
-		return ret;
+		goto err_exit;
 
 	ret = devm_gpiochip_add_data(&client->dev, &chip->gpio_chip, chip);
 	if (ret)
-		return ret;
+		goto err_exit;
 
 	ret = pca953x_irq_setup(chip, irq_base);
 	if (ret)
-		return ret;
+		goto err_exit;
 
 	if (pdata && pdata->setup) {
 		ret = pdata->setup(client, chip->gpio_chip.base,
@@ -816,6 +856,10 @@ static int pca953x_probe(struct i2c_client *client,
 
 	i2c_set_clientdata(client, chip);
 	return 0;
+
+err_exit:
+	regulator_disable(chip->regulator);
+	return ret;
 }
 
 static int pca953x_remove(struct i2c_client *client)
@@ -827,14 +871,16 @@ static int pca953x_remove(struct i2c_client *client)
 	if (pdata && pdata->teardown) {
 		ret = pdata->teardown(client, chip->gpio_chip.base,
 				chip->gpio_chip.ngpio, pdata->context);
-		if (ret < 0) {
+		if (ret < 0)
 			dev_err(&client->dev, "%s failed, %d\n",
 					"teardown", ret);
-			return ret;
-		}
+	} else {
+		ret = 0;
 	}
 
-	return 0;
+	regulator_disable(chip->regulator);
+
+	return ret;
 }
 
 /* convenience to stop overlong match-table lines */
diff --git a/drivers/gpio/gpio-pisosr.c b/drivers/gpio/gpio-pisosr.c
index cb14b8d1d512..f5545049c187 100644
--- a/drivers/gpio/gpio-pisosr.c
+++ b/drivers/gpio/gpio-pisosr.c
@@ -90,7 +90,7 @@ static int pisosr_gpio_get(struct gpio_chip *chip, unsigned offset)
 	return (gpio->buffer[offset / 8] >> (offset % 8)) & 0x1;
 }
 
-static struct gpio_chip template_chip = {
+static const struct gpio_chip template_chip = {
 	.label			= "pisosr-gpio",
 	.owner			= THIS_MODULE,
 	.get_direction		= pisosr_gpio_get_direction,
diff --git a/drivers/gpio/gpio-rcar.c b/drivers/gpio/gpio-rcar.c
index b96e0b466f74..2be48f5eba36 100644
--- a/drivers/gpio/gpio-rcar.c
+++ b/drivers/gpio/gpio-rcar.c
@@ -348,6 +348,10 @@ static const struct of_device_id gpio_rcar_of_table[] = {
 		/* Gen3 GPIO is identical to Gen2. */
 		.data = &gpio_rcar_info_gen2,
 	}, {
+		.compatible = "renesas,gpio-r8a7796",
+		/* Gen3 GPIO is identical to Gen2. */
+		.data = &gpio_rcar_info_gen2,
+	}, {
 		.compatible = "renesas,gpio-rcar",
 		.data = &gpio_rcar_info_gen1,
 	}, {
diff --git a/drivers/gpio/gpio-sch.c b/drivers/gpio/gpio-sch.c
index eb43ae4835c1..545004445846 100644
--- a/drivers/gpio/gpio-sch.c
+++ b/drivers/gpio/gpio-sch.c
@@ -138,7 +138,7 @@ static int sch_gpio_direction_out(struct gpio_chip *gc, unsigned gpio_num,
 	return 0;
 }
 
-static struct gpio_chip sch_gpio_chip = {
+static const struct gpio_chip sch_gpio_chip = {
 	.label			= "sch_gpio",
 	.owner			= THIS_MODULE,
 	.direction_input	= sch_gpio_direction_in,
diff --git a/drivers/gpio/gpio-spear-spics.c b/drivers/gpio/gpio-spear-spics.c
index 7ffd16495286..22267479ba68 100644
--- a/drivers/gpio/gpio-spear-spics.c
+++ b/drivers/gpio/gpio-spear-spics.c
@@ -12,7 +12,7 @@
 #include <linux/err.h>
 #include <linux/gpio.h>
 #include <linux/io.h>
-#include <linux/module.h>
+#include <linux/init.h>
 #include <linux/of.h>
 #include <linux/platform_device.h>
 #include <linux/types.h>
@@ -183,7 +183,6 @@ static const struct of_device_id spics_gpio_of_match[] = {
 	{ .compatible = "st,spear-spics-gpio" },
 	{}
 };
-MODULE_DEVICE_TABLE(of, spics_gpio_of_match);
 
 static struct platform_driver spics_gpio_driver = {
 	.probe = spics_gpio_probe,
@@ -198,7 +197,3 @@ static int __init spics_gpio_init(void)
 	return platform_driver_register(&spics_gpio_driver);
 }
 subsys_initcall(spics_gpio_init);
-
-MODULE_AUTHOR("Shiraz Hashim <shiraz.linux.kernel@gmail.com>");
-MODULE_DESCRIPTION("STMicroelectronics SPEAr SPI Chip Select Abstraction");
-MODULE_LICENSE("GPL");
diff --git a/drivers/gpio/gpio-stmpe.c b/drivers/gpio/gpio-stmpe.c
index f675132de10e..e7d422a6b90b 100644
--- a/drivers/gpio/gpio-stmpe.c
+++ b/drivers/gpio/gpio-stmpe.c
@@ -13,6 +13,7 @@
 #include <linux/of.h>
 #include <linux/mfd/stmpe.h>
 #include <linux/seq_file.h>
+#include <linux/bitops.h>
 
 /*
  * These registers are modified under the irq bus lock and cached to avoid
@@ -20,6 +21,8 @@
  */
 enum { REG_RE, REG_FE, REG_IE };
 
+enum { LSB, CSB, MSB };
+
 #define CACHE_NR_REGS	3
 /* No variant has more than 24 GPIOs */
 #define CACHE_NR_BANKS	(24 / 8)
@@ -39,8 +42,8 @@ static int stmpe_gpio_get(struct gpio_chip *chip, unsigned offset)
 {
 	struct stmpe_gpio *stmpe_gpio = gpiochip_get_data(chip);
 	struct stmpe *stmpe = stmpe_gpio->stmpe;
-	u8 reg = stmpe->regs[STMPE_IDX_GPMR_LSB] - (offset / 8);
-	u8 mask = 1 << (offset % 8);
+	u8 reg = stmpe->regs[STMPE_IDX_GPMR_LSB + (offset / 8)];
+	u8 mask = BIT(offset % 8);
 	int ret;
 
 	ret = stmpe_reg_read(stmpe, reg);
@@ -55,8 +58,8 @@ static void stmpe_gpio_set(struct gpio_chip *chip, unsigned offset, int val)
 	struct stmpe_gpio *stmpe_gpio = gpiochip_get_data(chip);
 	struct stmpe *stmpe = stmpe_gpio->stmpe;
 	int which = val ? STMPE_IDX_GPSR_LSB : STMPE_IDX_GPCR_LSB;
-	u8 reg = stmpe->regs[which] - (offset / 8);
-	u8 mask = 1 << (offset % 8);
+	u8 reg = stmpe->regs[which + (offset / 8)];
+	u8 mask = BIT(offset % 8);
 
 	/*
 	 * Some variants have single register for gpio set/clear functionality.
@@ -74,7 +77,7 @@ static int stmpe_gpio_get_direction(struct gpio_chip *chip,
 	struct stmpe_gpio *stmpe_gpio = gpiochip_get_data(chip);
 	struct stmpe *stmpe = stmpe_gpio->stmpe;
 	u8 reg = stmpe->regs[STMPE_IDX_GPDR_LSB] - (offset / 8);
-	u8 mask = 1 << (offset % 8);
+	u8 mask = BIT(offset % 8);
 	int ret;
 
 	ret = stmpe_reg_read(stmpe, reg);
@@ -89,8 +92,8 @@ static int stmpe_gpio_direction_output(struct gpio_chip *chip,
 {
 	struct stmpe_gpio *stmpe_gpio = gpiochip_get_data(chip);
 	struct stmpe *stmpe = stmpe_gpio->stmpe;
-	u8 reg = stmpe->regs[STMPE_IDX_GPDR_LSB] - (offset / 8);
-	u8 mask = 1 << (offset % 8);
+	u8 reg = stmpe->regs[STMPE_IDX_GPDR_LSB + (offset / 8)];
+	u8 mask = BIT(offset % 8);
 
 	stmpe_gpio_set(chip, offset, val);
 
@@ -102,8 +105,8 @@ static int stmpe_gpio_direction_input(struct gpio_chip *chip,
 {
 	struct stmpe_gpio *stmpe_gpio = gpiochip_get_data(chip);
 	struct stmpe *stmpe = stmpe_gpio->stmpe;
-	u8 reg = stmpe->regs[STMPE_IDX_GPDR_LSB] - (offset / 8);
-	u8 mask = 1 << (offset % 8);
+	u8 reg = stmpe->regs[STMPE_IDX_GPDR_LSB + (offset / 8)];
+	u8 mask = BIT(offset % 8);
 
 	return stmpe_set_bits(stmpe, reg, mask, 0);
 }
@@ -113,13 +116,13 @@ static int stmpe_gpio_request(struct gpio_chip *chip, unsigned offset)
 	struct stmpe_gpio *stmpe_gpio = gpiochip_get_data(chip);
 	struct stmpe *stmpe = stmpe_gpio->stmpe;
 
-	if (stmpe_gpio->norequest_mask & (1 << offset))
+	if (stmpe_gpio->norequest_mask & BIT(offset))
 		return -EINVAL;
 
-	return stmpe_set_altfunc(stmpe, 1 << offset, STMPE_BLOCK_GPIO);
+	return stmpe_set_altfunc(stmpe, BIT(offset), STMPE_BLOCK_GPIO);
 }
 
-static struct gpio_chip template_chip = {
+static const struct gpio_chip template_chip = {
 	.label			= "stmpe",
 	.owner			= THIS_MODULE,
 	.get_direction		= stmpe_gpio_get_direction,
@@ -137,13 +140,14 @@ static int stmpe_gpio_irq_set_type(struct irq_data *d, unsigned int type)
 	struct stmpe_gpio *stmpe_gpio = gpiochip_get_data(gc);
 	int offset = d->hwirq;
 	int regoffset = offset / 8;
-	int mask = 1 << (offset % 8);
+	int mask = BIT(offset % 8);
 
 	if (type & IRQ_TYPE_LEVEL_LOW || type & IRQ_TYPE_LEVEL_HIGH)
 		return -EINVAL;
 
-	/* STMPE801 doesn't have RE and FE registers */
-	if (stmpe_gpio->stmpe->partnum == STMPE801)
+	/* STMPE801 and STMPE 1600 don't have RE and FE registers */
+	if (stmpe_gpio->stmpe->partnum == STMPE801 ||
+	    stmpe_gpio->stmpe->partnum == STMPE1600)
 		return 0;
 
 	if (type & IRQ_TYPE_EDGE_RISING)
@@ -173,17 +177,24 @@ static void stmpe_gpio_irq_sync_unlock(struct irq_data *d)
 	struct stmpe_gpio *stmpe_gpio = gpiochip_get_data(gc);
 	struct stmpe *stmpe = stmpe_gpio->stmpe;
 	int num_banks = DIV_ROUND_UP(stmpe->num_gpios, 8);
-	static const u8 regmap[] = {
-		[REG_RE]	= STMPE_IDX_GPRER_LSB,
-		[REG_FE]	= STMPE_IDX_GPFER_LSB,
-		[REG_IE]	= STMPE_IDX_IEGPIOR_LSB,
+	static const u8 regmap[CACHE_NR_REGS][CACHE_NR_BANKS] = {
+		[REG_RE][LSB] = STMPE_IDX_GPRER_LSB,
+		[REG_RE][CSB] = STMPE_IDX_GPRER_CSB,
+		[REG_RE][MSB] = STMPE_IDX_GPRER_MSB,
+		[REG_FE][LSB] = STMPE_IDX_GPFER_LSB,
+		[REG_FE][CSB] = STMPE_IDX_GPFER_CSB,
+		[REG_FE][MSB] = STMPE_IDX_GPFER_MSB,
+		[REG_IE][LSB] = STMPE_IDX_IEGPIOR_LSB,
+		[REG_IE][CSB] = STMPE_IDX_IEGPIOR_CSB,
+		[REG_IE][MSB] = STMPE_IDX_IEGPIOR_MSB,
 	};
 	int i, j;
 
 	for (i = 0; i < CACHE_NR_REGS; i++) {
-		/* STMPE801 doesn't have RE and FE registers */
-		if ((stmpe->partnum == STMPE801) &&
-				(i != REG_IE))
+		/* STMPE801 and STMPE1600 don't have RE and FE registers */
+		if ((stmpe->partnum == STMPE801 ||
+		     stmpe->partnum == STMPE1600) &&
+		     (i != REG_IE))
 			continue;
 
 		for (j = 0; j < num_banks; j++) {
@@ -194,7 +205,7 @@ static void stmpe_gpio_irq_sync_unlock(struct irq_data *d)
 				continue;
 
 			stmpe_gpio->oldregs[i][j] = new;
-			stmpe_reg_write(stmpe, stmpe->regs[regmap[i]] - j, new);
+			stmpe_reg_write(stmpe, stmpe->regs[regmap[i][j]], new);
 		}
 	}
 
@@ -207,7 +218,7 @@ static void stmpe_gpio_irq_mask(struct irq_data *d)
 	struct stmpe_gpio *stmpe_gpio = gpiochip_get_data(gc);
 	int offset = d->hwirq;
 	int regoffset = offset / 8;
-	int mask = 1 << (offset % 8);
+	int mask = BIT(offset % 8);
 
 	stmpe_gpio->regs[REG_IE][regoffset] &= ~mask;
 }
@@ -216,11 +227,21 @@ static void stmpe_gpio_irq_unmask(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
 	struct stmpe_gpio *stmpe_gpio = gpiochip_get_data(gc);
+	struct stmpe *stmpe = stmpe_gpio->stmpe;
 	int offset = d->hwirq;
 	int regoffset = offset / 8;
-	int mask = 1 << (offset % 8);
+	int mask = BIT(offset % 8);
 
 	stmpe_gpio->regs[REG_IE][regoffset] |= mask;
+
+	/*
+	 * STMPE1600 workaround: to be able to get IRQ from pins,
+	 * a read must be done on GPMR register, or a write in
+	 * GPSR or GPCR registers
+	 */
+	if (stmpe->partnum == STMPE1600)
+		stmpe_reg_read(stmpe,
+			       stmpe->regs[STMPE_IDX_GPMR_LSB + regoffset]);
 }
 
 static void stmpe_dbg_show_one(struct seq_file *s,
@@ -230,10 +251,10 @@ static void stmpe_dbg_show_one(struct seq_file *s,
 	struct stmpe_gpio *stmpe_gpio = gpiochip_get_data(gc);
 	struct stmpe *stmpe = stmpe_gpio->stmpe;
 	const char *label = gpiochip_is_requested(gc, offset);
-	int num_banks = DIV_ROUND_UP(stmpe->num_gpios, 8);
 	bool val = !!stmpe_gpio_get(gc, offset);
-	u8 dir_reg = stmpe->regs[STMPE_IDX_GPDR_LSB] - (offset / 8);
-	u8 mask = 1 << (offset % 8);
+	u8 bank = offset / 8;
+	u8 dir_reg = stmpe->regs[STMPE_IDX_GPDR_LSB + bank];
+	u8 mask = BIT(offset % 8);
 	int ret;
 	u8 dir;
 
@@ -247,39 +268,72 @@ static void stmpe_dbg_show_one(struct seq_file *s,
 			   gpio, label ?: "(none)",
 			   val ? "hi" : "lo");
 	} else {
-		u8 edge_det_reg = stmpe->regs[STMPE_IDX_GPEDR_MSB] + num_banks - 1 - (offset / 8);
-		u8 rise_reg = stmpe->regs[STMPE_IDX_GPRER_LSB] - (offset / 8);
-		u8 fall_reg = stmpe->regs[STMPE_IDX_GPFER_LSB] - (offset / 8);
-		u8 irqen_reg = stmpe->regs[STMPE_IDX_IEGPIOR_LSB] - (offset / 8);
-		bool edge_det;
-		bool rise;
-		bool fall;
+		u8 edge_det_reg;
+		u8 rise_reg;
+		u8 fall_reg;
+		u8 irqen_reg;
+
+		char *edge_det_values[] = {"edge-inactive",
+					   "edge-asserted",
+					   "not-supported"};
+		char *rise_values[] = {"no-rising-edge-detection",
+				       "rising-edge-detection",
+				       "not-supported"};
+		char *fall_values[] = {"no-falling-edge-detection",
+				       "falling-edge-detection",
+				       "not-supported"};
+		#define NOT_SUPPORTED_IDX 2
+		u8 edge_det = NOT_SUPPORTED_IDX;
+		u8 rise = NOT_SUPPORTED_IDX;
+		u8 fall = NOT_SUPPORTED_IDX;
 		bool irqen;
 
-		ret = stmpe_reg_read(stmpe, edge_det_reg);
-		if (ret < 0)
+		switch (stmpe->partnum) {
+		case STMPE610:
+		case STMPE811:
+		case STMPE1601:
+		case STMPE2401:
+		case STMPE2403:
+			edge_det_reg = stmpe->regs[STMPE_IDX_GPEDR_LSB + bank];
+			ret = stmpe_reg_read(stmpe, edge_det_reg);
+			if (ret < 0)
+				return;
+			edge_det = !!(ret & mask);
+
+		case STMPE1801:
+			rise_reg = stmpe->regs[STMPE_IDX_GPRER_LSB + bank];
+			fall_reg = stmpe->regs[STMPE_IDX_GPFER_LSB + bank];
+
+			ret = stmpe_reg_read(stmpe, rise_reg);
+			if (ret < 0)
+				return;
+			rise = !!(ret & mask);
+			ret = stmpe_reg_read(stmpe, fall_reg);
+			if (ret < 0)
+				return;
+			fall = !!(ret & mask);
+
+		case STMPE801:
+		case STMPE1600:
+			irqen_reg = stmpe->regs[STMPE_IDX_IEGPIOR_LSB + bank];
+			break;
+
+		default:
 			return;
-		edge_det = !!(ret & mask);
-		ret = stmpe_reg_read(stmpe, rise_reg);
-		if (ret < 0)
-			return;
-		rise = !!(ret & mask);
-		ret = stmpe_reg_read(stmpe, fall_reg);
-		if (ret < 0)
-			return;
-		fall = !!(ret & mask);
+		}
+
 		ret = stmpe_reg_read(stmpe, irqen_reg);
 		if (ret < 0)
 			return;
 		irqen = !!(ret & mask);
 
-		seq_printf(s, " gpio-%-3d (%-20.20s) in  %s %s %s%s%s",
+		seq_printf(s, " gpio-%-3d (%-20.20s) in  %s %13s %13s %25s %25s",
 			   gpio, label ?: "(none)",
 			   val ? "hi" : "lo",
-			   edge_det ? "edge-asserted" : "edge-inactive",
-			   irqen ? "IRQ-enabled" : "",
-			   rise ? " rising-edge-detection" : "",
-			   fall ? " falling-edge-detection" : "");
+			   edge_det_values[edge_det],
+			   irqen ? "IRQ-enabled" : "IRQ-disabled",
+			   rise_values[rise],
+			   fall_values[fall]);
 	}
 }
 
@@ -307,18 +361,32 @@ static irqreturn_t stmpe_gpio_irq(int irq, void *dev)
 {
 	struct stmpe_gpio *stmpe_gpio = dev;
 	struct stmpe *stmpe = stmpe_gpio->stmpe;
-	u8 statmsbreg = stmpe->regs[STMPE_IDX_ISGPIOR_MSB];
+	u8 statmsbreg;
 	int num_banks = DIV_ROUND_UP(stmpe->num_gpios, 8);
 	u8 status[num_banks];
 	int ret;
 	int i;
 
+	/*
+	 * the stmpe_block_read() call below, imposes to set statmsbreg
+	 * with the register located at the lowest address. As STMPE1600
+	 * variant is the only one which respect registers address's order
+	 * (LSB regs located at lowest address than MSB ones) whereas all
+	 * the others have a registers layout with MSB located before the
+	 * LSB regs.
+	 */
+	if (stmpe->partnum == STMPE1600)
+		statmsbreg = stmpe->regs[STMPE_IDX_ISGPIOR_LSB];
+	else
+		statmsbreg = stmpe->regs[STMPE_IDX_ISGPIOR_MSB];
+
 	ret = stmpe_block_read(stmpe, statmsbreg, num_banks, status);
 	if (ret < 0)
 		return IRQ_NONE;
 
 	for (i = 0; i < num_banks; i++) {
-		int bank = num_banks - i - 1;
+		int bank = (stmpe_gpio->stmpe->partnum == STMPE1600) ? i :
+			   num_banks - i - 1;
 		unsigned int enabled = stmpe_gpio->regs[REG_IE][bank];
 		unsigned int stat = status[i];
 
@@ -333,15 +401,21 @@ static irqreturn_t stmpe_gpio_irq(int irq, void *dev)
 							 line);
 
 			handle_nested_irq(child_irq);
-			stat &= ~(1 << bit);
+			stat &= ~BIT(bit);
 		}
 
-		stmpe_reg_write(stmpe, statmsbreg + i, status[i]);
-
-		/* Edge detect register is not present on 801 */
-		if (stmpe->partnum != STMPE801)
-			stmpe_reg_write(stmpe, stmpe->regs[STMPE_IDX_GPEDR_MSB]
-					+ i, status[i]);
+		/*
+		 * interrupt status register write has no effect on
+		 * 801/1801/1600, bits are cleared when read.
+		 * Edge detect register is not present on 801/1600/1801
+		 */
+		if (stmpe->partnum != STMPE801 || stmpe->partnum != STMPE1600 ||
+		    stmpe->partnum != STMPE1801) {
+			stmpe_reg_write(stmpe, statmsbreg + i, status[i]);
+			stmpe_reg_write(stmpe,
+					stmpe->regs[STMPE_IDX_GPEDR_LSB + i],
+					status[i]);
+		}
 	}
 
 	return IRQ_HANDLED;
@@ -376,6 +450,8 @@ static int stmpe_gpio_probe(struct platform_device *pdev)
 
 	of_property_read_u32(np, "st,norequest-mask",
 			&stmpe_gpio->norequest_mask);
+	if (stmpe_gpio->norequest_mask)
+		stmpe_gpio->chip.irq_need_valid_mask = true;
 
 	if (irq < 0)
 		dev_info(&pdev->dev,
@@ -400,6 +476,14 @@ static int stmpe_gpio_probe(struct platform_device *pdev)
 			dev_err(&pdev->dev, "unable to get irq: %d\n", ret);
 			goto out_disable;
 		}
+		if (stmpe_gpio->norequest_mask) {
+			int i;
+
+			/* Forbid unused lines to be mapped as IRQs */
+			for (i = 0; i < sizeof(u32); i++)
+				if (stmpe_gpio->norequest_mask & BIT(i))
+					clear_bit(i, stmpe_gpio->chip.irq_valid_mask);
+		}
 		ret =  gpiochip_irqchip_add(&stmpe_gpio->chip,
 					    &stmpe_gpio_irq_chip,
 					    0,
diff --git a/drivers/gpio/gpio-sx150x.c b/drivers/gpio/gpio-sx150x.c
index a177ebd921d5..af95de89db01 100644
--- a/drivers/gpio/gpio-sx150x.c
+++ b/drivers/gpio/gpio-sx150x.c
@@ -236,7 +236,6 @@ static const struct i2c_device_id sx150x_id[] = {
 	{"sx1502q", 3},
 	{}
 };
-MODULE_DEVICE_TABLE(i2c, sx150x_id);
 
 static const struct of_device_id sx150x_of_match[] = {
 	{ .compatible = "semtech,sx1508q" },
@@ -245,7 +244,6 @@ static const struct of_device_id sx150x_of_match[] = {
 	{ .compatible = "semtech,sx1502q" },
 	{},
 };
-MODULE_DEVICE_TABLE(of, sx150x_of_match);
 
 static s32 sx150x_i2c_write(struct i2c_client *client, u8 reg, u8 val)
 {
diff --git a/drivers/gpio/gpio-tc3589x.c b/drivers/gpio/gpio-tc3589x.c
index 8b3659352e49..5a5a6cb00eea 100644
--- a/drivers/gpio/gpio-tc3589x.c
+++ b/drivers/gpio/gpio-tc3589x.c
@@ -34,7 +34,7 @@ struct tc3589x_gpio {
 	u8 oldregs[CACHE_NR_REGS][CACHE_NR_BANKS];
 };
 
-static int tc3589x_gpio_get(struct gpio_chip *chip, unsigned offset)
+static int tc3589x_gpio_get(struct gpio_chip *chip, unsigned int offset)
 {
 	struct tc3589x_gpio *tc3589x_gpio = gpiochip_get_data(chip);
 	struct tc3589x *tc3589x = tc3589x_gpio->tc3589x;
@@ -49,24 +49,24 @@ static int tc3589x_gpio_get(struct gpio_chip *chip, unsigned offset)
 	return !!(ret & mask);
 }
 
-static void tc3589x_gpio_set(struct gpio_chip *chip, unsigned offset, int val)
+static void tc3589x_gpio_set(struct gpio_chip *chip, unsigned int offset, int val)
 {
 	struct tc3589x_gpio *tc3589x_gpio = gpiochip_get_data(chip);
 	struct tc3589x *tc3589x = tc3589x_gpio->tc3589x;
 	u8 reg = TC3589x_GPIODATA0 + (offset / 8) * 2;
-	unsigned pos = offset % 8;
+	unsigned int pos = offset % 8;
 	u8 data[] = {val ? BIT(pos) : 0, BIT(pos)};
 
 	tc3589x_block_write(tc3589x, reg, ARRAY_SIZE(data), data);
 }
 
 static int tc3589x_gpio_direction_output(struct gpio_chip *chip,
-					 unsigned offset, int val)
+					 unsigned int offset, int val)
 {
 	struct tc3589x_gpio *tc3589x_gpio = gpiochip_get_data(chip);
 	struct tc3589x *tc3589x = tc3589x_gpio->tc3589x;
 	u8 reg = TC3589x_GPIODIR0 + offset / 8;
-	unsigned pos = offset % 8;
+	unsigned int pos = offset % 8;
 
 	tc3589x_gpio_set(chip, offset, val);
 
@@ -74,19 +74,35 @@ static int tc3589x_gpio_direction_output(struct gpio_chip *chip,
 }
 
 static int tc3589x_gpio_direction_input(struct gpio_chip *chip,
-					unsigned offset)
+					unsigned int offset)
 {
 	struct tc3589x_gpio *tc3589x_gpio = gpiochip_get_data(chip);
 	struct tc3589x *tc3589x = tc3589x_gpio->tc3589x;
 	u8 reg = TC3589x_GPIODIR0 + offset / 8;
-	unsigned pos = offset % 8;
+	unsigned int pos = offset % 8;
 
 	return tc3589x_set_bits(tc3589x, reg, BIT(pos), 0);
 }
 
-static int tc3589x_gpio_single_ended(struct gpio_chip *chip,
-				     unsigned offset,
-				     enum single_ended_mode mode)
+static int tc3589x_gpio_get_direction(struct gpio_chip *chip,
+				      unsigned int offset)
+{
+	struct tc3589x_gpio *tc3589x_gpio = gpiochip_get_data(chip);
+	struct tc3589x *tc3589x = tc3589x_gpio->tc3589x;
+	u8 reg = TC3589x_GPIODIR0 + offset / 8;
+	unsigned int pos = offset % 8;
+	int ret;
+
+	ret = tc3589x_reg_read(tc3589x, reg);
+	if (ret < 0)
+		return ret;
+
+	return !!(ret & BIT(pos));
+}
+
+static int tc3589x_gpio_set_single_ended(struct gpio_chip *chip,
+					 unsigned int offset,
+					 enum single_ended_mode mode)
 {
 	struct tc3589x_gpio *tc3589x_gpio = gpiochip_get_data(chip);
 	struct tc3589x *tc3589x = tc3589x_gpio->tc3589x;
@@ -97,7 +113,7 @@ static int tc3589x_gpio_single_ended(struct gpio_chip *chip,
 	 */
 	u8 odmreg = TC3589x_GPIOODM0 + (offset / 8) * 2;
 	u8 odereg = TC3589x_GPIOODE0 + (offset / 8) * 2;
-	unsigned pos = offset % 8;
+	unsigned int pos = offset % 8;
 	int ret;
 
 	switch(mode) {
@@ -124,14 +140,15 @@ static int tc3589x_gpio_single_ended(struct gpio_chip *chip,
 	return -ENOTSUPP;
 }
 
-static struct gpio_chip template_chip = {
+static const struct gpio_chip template_chip = {
 	.label			= "tc3589x",
 	.owner			= THIS_MODULE,
-	.direction_input	= tc3589x_gpio_direction_input,
 	.get			= tc3589x_gpio_get,
-	.direction_output	= tc3589x_gpio_direction_output,
 	.set			= tc3589x_gpio_set,
-	.set_single_ended	= tc3589x_gpio_single_ended,
+	.direction_output	= tc3589x_gpio_direction_output,
+	.direction_input	= tc3589x_gpio_direction_input,
+	.get_direction		= tc3589x_gpio_get_direction,
+	.set_single_ended	= tc3589x_gpio_set_single_ended,
 	.can_sleep		= true,
 };
 
diff --git a/drivers/gpio/gpio-tpic2810.c b/drivers/gpio/gpio-tpic2810.c
index cace79c1b70a..c8b34d787eed 100644
--- a/drivers/gpio/gpio-tpic2810.c
+++ b/drivers/gpio/gpio-tpic2810.c
@@ -87,7 +87,7 @@ static void tpic2810_set_multiple(struct gpio_chip *chip, unsigned long *mask,
 	tpic2810_set_mask_bits(chip, *mask, *bits);
 }
 
-static struct gpio_chip template_chip = {
+static const struct gpio_chip template_chip = {
 	.label			= "tpic2810",
 	.owner			= THIS_MODULE,
 	.get_direction		= tpic2810_get_direction,
diff --git a/drivers/gpio/gpio-tps65086.c b/drivers/gpio/gpio-tps65086.c
index 8e25f01ac314..b23c4d2429be 100644
--- a/drivers/gpio/gpio-tps65086.c
+++ b/drivers/gpio/gpio-tps65086.c
@@ -72,7 +72,7 @@ static void tps65086_gpio_set(struct gpio_chip *chip, unsigned offset,
 			   BIT(4 + offset), value ? BIT(4 + offset) : 0);
 }
 
-static struct gpio_chip template_chip = {
+static const struct gpio_chip template_chip = {
 	.label			= "tps65086-gpio",
 	.owner			= THIS_MODULE,
 	.get_direction		= tps65086_gpio_get_direction,
diff --git a/drivers/gpio/gpio-tps65218.c b/drivers/gpio/gpio-tps65218.c
index 1c09a19ae10c..d779307a9685 100644
--- a/drivers/gpio/gpio-tps65218.c
+++ b/drivers/gpio/gpio-tps65218.c
@@ -172,7 +172,7 @@ static int tps65218_gpio_set_single_ended(struct gpio_chip *gc,
 	return -ENOTSUPP;
 }
 
-static struct gpio_chip template_chip = {
+static const struct gpio_chip template_chip = {
 	.label			= "gpio-tps65218",
 	.owner			= THIS_MODULE,
 	.request		= tps65218_gpio_request,
@@ -204,7 +204,8 @@ static int tps65218_gpio_probe(struct platform_device *pdev)
 	tps65218_gpio->gpio_chip.of_node = pdev->dev.of_node;
 #endif
 
-	ret = gpiochip_add_data(&tps65218_gpio->gpio_chip, tps65218_gpio);
+	ret = devm_gpiochip_add_data(&pdev->dev, &tps65218_gpio->gpio_chip,
+				     tps65218_gpio);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "Failed to register gpiochip, %d\n", ret);
 		return ret;
@@ -215,15 +216,6 @@ static int tps65218_gpio_probe(struct platform_device *pdev)
 	return ret;
 }
 
-static int tps65218_gpio_remove(struct platform_device *pdev)
-{
-	struct tps65218_gpio *tps65218_gpio = platform_get_drvdata(pdev);
-
-	gpiochip_remove(&tps65218_gpio->gpio_chip);
-
-	return 0;
-}
-
 static const struct of_device_id tps65218_dt_match[] = {
 	{ .compatible = "ti,tps65218-gpio" },
 	{  }
@@ -242,7 +234,6 @@ static struct platform_driver tps65218_gpio_driver = {
 		.of_match_table = of_match_ptr(tps65218_dt_match)
 	},
 	.probe = tps65218_gpio_probe,
-	.remove = tps65218_gpio_remove,
 	.id_table = tps65218_gpio_id_table,
 };
 
diff --git a/drivers/gpio/gpio-tps65912.c b/drivers/gpio/gpio-tps65912.c
index acfd30a13a56..abc0798ef843 100644
--- a/drivers/gpio/gpio-tps65912.c
+++ b/drivers/gpio/gpio-tps65912.c
@@ -90,7 +90,7 @@ static void tps65912_gpio_set(struct gpio_chip *gc, unsigned offset,
 			   GPIO_SET_MASK, value ? GPIO_SET_MASK : 0);
 }
 
-static struct gpio_chip template_chip = {
+static const struct gpio_chip template_chip = {
 	.label			= "tps65912-gpio",
 	.owner			= THIS_MODULE,
 	.get_direction		= tps65912_gpio_get_direction,
diff --git a/drivers/gpio/gpio-ts4800.c b/drivers/gpio/gpio-ts4800.c
index 0c144a72f9af..99256115bea5 100644
--- a/drivers/gpio/gpio-ts4800.c
+++ b/drivers/gpio/gpio-ts4800.c
@@ -9,6 +9,7 @@
  */
 
 #include <linux/gpio/driver.h>
+#include <linux/module.h>
 #include <linux/of_address.h>
 #include <linux/of_device.h>
 #include <linux/platform_device.h>
diff --git a/drivers/gpio/gpio-ts4900.c b/drivers/gpio/gpio-ts4900.c
new file mode 100644
index 000000000000..5bd21725e604
--- /dev/null
+++ b/drivers/gpio/gpio-ts4900.c
@@ -0,0 +1,190 @@
+/*
+ * Digital I/O driver for Technologic Systems I2C FPGA Core
+ *
+ * Copyright (C) 2015 Technologic Systems
+ * Copyright (C) 2016 Savoir-Faire Linux
+ *
+ * This program is free software; you can 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 expressed or implied; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License version 2 for more details.
+ */
+
+#include <linux/gpio/driver.h>
+#include <linux/i2c.h>
+#include <linux/of_device.h>
+#include <linux/module.h>
+#include <linux/regmap.h>
+
+#define DEFAULT_PIN_NUMBER	32
+/*
+ * Register bits used by the GPIO device
+ * Some boards, such as TS-7970 do not have a separate input bit
+ */
+#define TS4900_GPIO_OE		0x01
+#define TS4900_GPIO_OUT		0x02
+#define TS4900_GPIO_IN		0x04
+#define TS7970_GPIO_IN		0x02
+
+struct ts4900_gpio_priv {
+	struct regmap *regmap;
+	struct gpio_chip gpio_chip;
+	unsigned int input_bit;
+};
+
+static int ts4900_gpio_get_direction(struct gpio_chip *chip,
+				     unsigned int offset)
+{
+	struct ts4900_gpio_priv *priv = gpiochip_get_data(chip);
+	unsigned int reg;
+
+	regmap_read(priv->regmap, offset, &reg);
+
+	return !(reg & TS4900_GPIO_OE);
+}
+
+static int ts4900_gpio_direction_input(struct gpio_chip *chip,
+				       unsigned int offset)
+{
+	struct ts4900_gpio_priv *priv = gpiochip_get_data(chip);
+
+	/*
+	 * This will clear the output enable bit, the other bits are
+	 * dontcare when this is cleared
+	 */
+	return regmap_write(priv->regmap, offset, 0);
+}
+
+static int ts4900_gpio_direction_output(struct gpio_chip *chip,
+					unsigned int offset, int value)
+{
+	struct ts4900_gpio_priv *priv = gpiochip_get_data(chip);
+	int ret;
+
+	if (value)
+		ret = regmap_write(priv->regmap, offset, TS4900_GPIO_OE |
+							 TS4900_GPIO_OUT);
+	else
+		ret = regmap_write(priv->regmap, offset, TS4900_GPIO_OE);
+
+	return ret;
+}
+
+static int ts4900_gpio_get(struct gpio_chip *chip, unsigned int offset)
+{
+	struct ts4900_gpio_priv *priv = gpiochip_get_data(chip);
+	unsigned int reg;
+
+	regmap_read(priv->regmap, offset, &reg);
+
+	return !!(reg & priv->input_bit);
+}
+
+static void ts4900_gpio_set(struct gpio_chip *chip, unsigned int offset,
+			    int value)
+{
+	struct ts4900_gpio_priv *priv = gpiochip_get_data(chip);
+
+	if (value)
+		regmap_update_bits(priv->regmap, offset, TS4900_GPIO_OUT,
+				   TS4900_GPIO_OUT);
+	else
+		regmap_update_bits(priv->regmap, offset, TS4900_GPIO_OUT, 0);
+}
+
+static const struct regmap_config ts4900_regmap_config = {
+	.reg_bits = 16,
+	.val_bits = 8,
+};
+
+static const struct gpio_chip template_chip = {
+	.label			= "ts4900-gpio",
+	.owner			= THIS_MODULE,
+	.get_direction		= ts4900_gpio_get_direction,
+	.direction_input	= ts4900_gpio_direction_input,
+	.direction_output	= ts4900_gpio_direction_output,
+	.get			= ts4900_gpio_get,
+	.set			= ts4900_gpio_set,
+	.base			= -1,
+	.can_sleep		= true,
+};
+
+static const struct of_device_id ts4900_gpio_of_match_table[] = {
+	{
+		.compatible = "technologic,ts4900-gpio",
+		.data = (void *)TS4900_GPIO_IN,
+	}, {
+		.compatible = "technologic,ts7970-gpio",
+		.data = (void *)TS7970_GPIO_IN,
+	},
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, ts4900_gpio_of_match_table);
+
+static int ts4900_gpio_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	const struct of_device_id *match;
+	struct ts4900_gpio_priv *priv;
+	u32 ngpio;
+	int ret;
+
+	match = of_match_device(ts4900_gpio_of_match_table, &client->dev);
+	if (!match)
+		return -EINVAL;
+
+	if (of_property_read_u32(client->dev.of_node, "ngpios", &ngpio))
+		ngpio = DEFAULT_PIN_NUMBER;
+
+	priv = devm_kzalloc(&client->dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->gpio_chip = template_chip;
+	priv->gpio_chip.label = "ts4900-gpio";
+	priv->gpio_chip.ngpio = ngpio;
+	priv->gpio_chip.parent = &client->dev;
+	priv->input_bit = (uintptr_t)match->data;
+
+	priv->regmap = devm_regmap_init_i2c(client, &ts4900_regmap_config);
+	if (IS_ERR(priv->regmap)) {
+		ret = PTR_ERR(priv->regmap);
+		dev_err(&client->dev, "Failed to allocate register map: %d\n",
+			ret);
+		return ret;
+	}
+
+	ret = devm_gpiochip_add_data(&client->dev, &priv->gpio_chip, priv);
+	if (ret < 0) {
+		dev_err(&client->dev, "Unable to register gpiochip\n");
+		return ret;
+	}
+
+	i2c_set_clientdata(client, priv);
+
+	return 0;
+}
+
+static const struct i2c_device_id ts4900_gpio_id_table[] = {
+	{ "ts4900-gpio", },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(i2c, ts4900_gpio_id_table);
+
+static struct i2c_driver ts4900_gpio_driver = {
+	.driver = {
+		.name = "ts4900-gpio",
+		.of_match_table = ts4900_gpio_of_match_table,
+	},
+	.probe = ts4900_gpio_probe,
+	.id_table = ts4900_gpio_id_table,
+};
+module_i2c_driver(ts4900_gpio_driver);
+
+MODULE_AUTHOR("Technologic Systems");
+MODULE_DESCRIPTION("GPIO interface for Technologic Systems I2C-FPGA core");
+MODULE_LICENSE("GPL");
diff --git a/drivers/gpio/gpio-twl4030.c b/drivers/gpio/gpio-twl4030.c
index 4b807b0e0c8e..dfcfbba74416 100644
--- a/drivers/gpio/gpio-twl4030.c
+++ b/drivers/gpio/gpio-twl4030.c
@@ -381,7 +381,7 @@ static int twl_to_irq(struct gpio_chip *chip, unsigned offset)
 		: -EINVAL;
 }
 
-static struct gpio_chip template_chip = {
+static const struct gpio_chip template_chip = {
 	.label			= "twl4030",
 	.owner			= THIS_MODULE,
 	.request		= twl_request,
diff --git a/drivers/gpio/gpio-vf610.c b/drivers/gpio/gpio-vf610.c
index 6284bdbe1e0c..3edb09cb9ee0 100644
--- a/drivers/gpio/gpio-vf610.c
+++ b/drivers/gpio/gpio-vf610.c
@@ -1,5 +1,5 @@
 /*
- * vf610 GPIO support through PORT and GPIO module
+ * Freescale vf610 GPIO support through PORT and GPIO
  *
  * Copyright (c) 2014 Toradex AG.
  *
@@ -23,7 +23,6 @@
 #include <linux/io.h>
 #include <linux/ioport.h>
 #include <linux/irq.h>
-#include <linux/module.h>
 #include <linux/platform_device.h>
 #include <linux/of.h>
 #include <linux/of_device.h>
@@ -289,7 +288,3 @@ static int __init gpio_vf610_init(void)
 	return platform_driver_register(&vf610_gpio_driver);
 }
 device_initcall(gpio_vf610_init);
-
-MODULE_AUTHOR("Stefan Agner <stefan@agner.ch>");
-MODULE_DESCRIPTION("Freescale VF610 GPIO");
-MODULE_LICENSE("GPL v2");
diff --git a/drivers/gpio/gpio-wcove.c b/drivers/gpio/gpio-wcove.c
new file mode 100644
index 000000000000..d0ddba7a9d08
--- /dev/null
+++ b/drivers/gpio/gpio-wcove.c
@@ -0,0 +1,470 @@
+/*
+ * Intel Whiskey Cove PMIC GPIO Driver
+ *
+ * This driver is written based on gpio-crystalcove.c
+ *
+ * Copyright (C) 2016 Intel 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/bitops.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/gpio/driver.h>
+#include <linux/mfd/intel_soc_pmic.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+#include <linux/seq_file.h>
+
+/*
+ * Whiskey Cove PMIC has 13 physical GPIO pins divided into 3 banks:
+ * Bank 0: Pin 0 - 6
+ * Bank 1: Pin 7 - 10
+ * Bank 2: Pin 11 -12
+ * Each pin has one output control register and one input control register.
+ */
+#define BANK0_NR_PINS		7
+#define BANK1_NR_PINS		4
+#define BANK2_NR_PINS		2
+#define WCOVE_GPIO_NUM		(BANK0_NR_PINS + BANK1_NR_PINS + BANK2_NR_PINS)
+#define WCOVE_VGPIO_NUM		94
+/* GPIO output control registers (one per pin): 0x4e44 - 0x4e50 */
+#define GPIO_OUT_CTRL_BASE	0x4e44
+/* GPIO input control registers (one per pin): 0x4e51 - 0x4e5d */
+#define GPIO_IN_CTRL_BASE	0x4e51
+
+/*
+ * GPIO interrupts are organized in two groups:
+ * Group 0: Bank 0 pins (Pin 0 - 6)
+ * Group 1: Bank 1 and Bank 2 pins (Pin 7 - 12)
+ * Each group has two registers (one bit per pin): status and mask.
+ */
+#define GROUP0_NR_IRQS		7
+#define GROUP1_NR_IRQS		6
+#define IRQ_MASK_BASE		0x4e19
+#define IRQ_STATUS_BASE		0x4e0b
+#define UPDATE_IRQ_TYPE		BIT(0)
+#define UPDATE_IRQ_MASK		BIT(1)
+
+#define CTLI_INTCNT_DIS		(0 << 1)
+#define CTLI_INTCNT_NE		(1 << 1)
+#define CTLI_INTCNT_PE		(2 << 1)
+#define CTLI_INTCNT_BE		(3 << 1)
+
+#define CTLO_DIR_IN		(0 << 5)
+#define CTLO_DIR_OUT		(1 << 5)
+
+#define CTLO_DRV_MASK		(1 << 4)
+#define CTLO_DRV_OD		(0 << 4)
+#define CTLO_DRV_CMOS		(1 << 4)
+
+#define CTLO_DRV_REN		(1 << 3)
+
+#define CTLO_RVAL_2KDOWN	(0 << 1)
+#define CTLO_RVAL_2KUP		(1 << 1)
+#define CTLO_RVAL_50KDOWN	(2 << 1)
+#define CTLO_RVAL_50KUP		(3 << 1)
+
+#define CTLO_INPUT_SET	(CTLO_DRV_CMOS | CTLO_DRV_REN | CTLO_RVAL_2KUP)
+#define CTLO_OUTPUT_SET	(CTLO_DIR_OUT | CTLO_INPUT_SET)
+
+enum ctrl_register {
+	CTRL_IN,
+	CTRL_OUT,
+};
+
+/*
+ * struct wcove_gpio - Whiskey Cove GPIO controller
+ * @buslock: for bus lock/sync and unlock.
+ * @chip: the abstract gpio_chip structure.
+ * @dev: the gpio device
+ * @regmap: the regmap from the parent device.
+ * @regmap_irq_chip: the regmap of the gpio irq chip.
+ * @update: pending IRQ setting update, to be written to the chip upon unlock.
+ * @intcnt: the Interrupt Detect value to be written.
+ * @set_irq_mask: true if the IRQ mask needs to be set, false to clear.
+ */
+struct wcove_gpio {
+	struct mutex buslock;
+	struct gpio_chip chip;
+	struct device *dev;
+	struct regmap *regmap;
+	struct regmap_irq_chip_data *regmap_irq_chip;
+	int update;
+	int intcnt;
+	bool set_irq_mask;
+};
+
+static inline unsigned int to_reg(int gpio, enum ctrl_register reg_type)
+{
+	unsigned int reg;
+	int bank;
+
+	if (gpio < BANK0_NR_PINS)
+		bank = 0;
+	else if (gpio < BANK0_NR_PINS + BANK1_NR_PINS)
+		bank = 1;
+	else
+		bank = 2;
+
+	if (reg_type == CTRL_IN)
+		reg = GPIO_IN_CTRL_BASE + bank;
+	else
+		reg = GPIO_OUT_CTRL_BASE + bank;
+
+	return reg;
+}
+
+static void wcove_update_irq_mask(struct wcove_gpio *wg, int gpio)
+{
+	unsigned int reg, mask;
+
+	if (gpio < GROUP0_NR_IRQS) {
+		reg = IRQ_MASK_BASE;
+		mask = BIT(gpio % GROUP0_NR_IRQS);
+	} else {
+		reg = IRQ_MASK_BASE + 1;
+		mask = BIT((gpio - GROUP0_NR_IRQS) % GROUP1_NR_IRQS);
+	}
+
+	if (wg->set_irq_mask)
+		regmap_update_bits(wg->regmap, reg, mask, mask);
+	else
+		regmap_update_bits(wg->regmap, reg, mask, 0);
+}
+
+static void wcove_update_irq_ctrl(struct wcove_gpio *wg, int gpio)
+{
+	unsigned int reg = to_reg(gpio, CTRL_IN);
+
+	regmap_update_bits(wg->regmap, reg, CTLI_INTCNT_BE, wg->intcnt);
+}
+
+static int wcove_gpio_dir_in(struct gpio_chip *chip, unsigned int gpio)
+{
+	struct wcove_gpio *wg = gpiochip_get_data(chip);
+
+	return regmap_write(wg->regmap, to_reg(gpio, CTRL_OUT),
+			    CTLO_INPUT_SET);
+}
+
+static int wcove_gpio_dir_out(struct gpio_chip *chip, unsigned int gpio,
+				    int value)
+{
+	struct wcove_gpio *wg = gpiochip_get_data(chip);
+
+	return regmap_write(wg->regmap, to_reg(gpio, CTRL_OUT),
+			    CTLO_OUTPUT_SET | value);
+}
+
+static int wcove_gpio_get_direction(struct gpio_chip *chip, unsigned int gpio)
+{
+	struct wcove_gpio *wg = gpiochip_get_data(chip);
+	unsigned int val;
+	int ret;
+
+	ret = regmap_read(wg->regmap, to_reg(gpio, CTRL_OUT), &val);
+	if (ret)
+		return ret;
+
+	return !(val & CTLO_DIR_OUT);
+}
+
+static int wcove_gpio_get(struct gpio_chip *chip, unsigned int gpio)
+{
+	struct wcove_gpio *wg = gpiochip_get_data(chip);
+	unsigned int val;
+	int ret;
+
+	ret = regmap_read(wg->regmap, to_reg(gpio, CTRL_IN), &val);
+	if (ret)
+		return ret;
+
+	return val & 0x1;
+}
+
+static void wcove_gpio_set(struct gpio_chip *chip,
+				 unsigned int gpio, int value)
+{
+	struct wcove_gpio *wg = gpiochip_get_data(chip);
+
+	if (value)
+		regmap_update_bits(wg->regmap, to_reg(gpio, CTRL_OUT), 1, 1);
+	else
+		regmap_update_bits(wg->regmap, to_reg(gpio, CTRL_OUT), 1, 0);
+}
+
+static int wcove_gpio_set_single_ended(struct gpio_chip *chip,
+					unsigned int gpio,
+					enum single_ended_mode mode)
+{
+	struct wcove_gpio *wg = gpiochip_get_data(chip);
+
+	switch (mode) {
+	case LINE_MODE_OPEN_DRAIN:
+		return regmap_update_bits(wg->regmap, to_reg(gpio, CTRL_OUT),
+						CTLO_DRV_MASK, CTLO_DRV_OD);
+	case LINE_MODE_PUSH_PULL:
+		return regmap_update_bits(wg->regmap, to_reg(gpio, CTRL_OUT),
+						CTLO_DRV_MASK, CTLO_DRV_CMOS);
+	default:
+		break;
+	}
+
+	return -ENOTSUPP;
+}
+
+static int wcove_irq_type(struct irq_data *data, unsigned int type)
+{
+	struct gpio_chip *chip = irq_data_get_irq_chip_data(data);
+	struct wcove_gpio *wg = gpiochip_get_data(chip);
+
+	switch (type) {
+	case IRQ_TYPE_NONE:
+		wg->intcnt = CTLI_INTCNT_DIS;
+		break;
+	case IRQ_TYPE_EDGE_BOTH:
+		wg->intcnt = CTLI_INTCNT_BE;
+		break;
+	case IRQ_TYPE_EDGE_RISING:
+		wg->intcnt = CTLI_INTCNT_PE;
+		break;
+	case IRQ_TYPE_EDGE_FALLING:
+		wg->intcnt = CTLI_INTCNT_NE;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	wg->update |= UPDATE_IRQ_TYPE;
+
+	return 0;
+}
+
+static void wcove_bus_lock(struct irq_data *data)
+{
+	struct gpio_chip *chip = irq_data_get_irq_chip_data(data);
+	struct wcove_gpio *wg = gpiochip_get_data(chip);
+
+	mutex_lock(&wg->buslock);
+}
+
+static void wcove_bus_sync_unlock(struct irq_data *data)
+{
+	struct gpio_chip *chip = irq_data_get_irq_chip_data(data);
+	struct wcove_gpio *wg = gpiochip_get_data(chip);
+	int gpio = data->hwirq;
+
+	if (wg->update & UPDATE_IRQ_TYPE)
+		wcove_update_irq_ctrl(wg, gpio);
+	if (wg->update & UPDATE_IRQ_MASK)
+		wcove_update_irq_mask(wg, gpio);
+	wg->update = 0;
+
+	mutex_unlock(&wg->buslock);
+}
+
+static void wcove_irq_unmask(struct irq_data *data)
+{
+	struct gpio_chip *chip = irq_data_get_irq_chip_data(data);
+	struct wcove_gpio *wg = gpiochip_get_data(chip);
+
+	wg->set_irq_mask = false;
+	wg->update |= UPDATE_IRQ_MASK;
+}
+
+static void wcove_irq_mask(struct irq_data *data)
+{
+	struct gpio_chip *chip = irq_data_get_irq_chip_data(data);
+	struct wcove_gpio *wg = gpiochip_get_data(chip);
+
+	wg->set_irq_mask = true;
+	wg->update |= UPDATE_IRQ_MASK;
+}
+
+static struct irq_chip wcove_irqchip = {
+	.name			= "Whiskey Cove",
+	.irq_mask		= wcove_irq_mask,
+	.irq_unmask		= wcove_irq_unmask,
+	.irq_set_type		= wcove_irq_type,
+	.irq_bus_lock		= wcove_bus_lock,
+	.irq_bus_sync_unlock	= wcove_bus_sync_unlock,
+};
+
+static irqreturn_t wcove_gpio_irq_handler(int irq, void *data)
+{
+	struct wcove_gpio *wg = (struct wcove_gpio *)data;
+	unsigned int pending, virq, gpio, mask, offset;
+	u8 p[2];
+
+	if (regmap_bulk_read(wg->regmap, IRQ_STATUS_BASE, p, 2)) {
+		dev_err(wg->dev, "Failed to read irq status register\n");
+		return IRQ_NONE;
+	}
+
+	pending = p[0] | (p[1] << 8);
+	if (!pending)
+		return IRQ_NONE;
+
+	/* Iterate until no interrupt is pending */
+	while (pending) {
+		/* One iteration is for all pending bits */
+		for_each_set_bit(gpio, (const unsigned long *)&pending,
+						 GROUP0_NR_IRQS) {
+			offset = (gpio > GROUP0_NR_IRQS) ? 1 : 0;
+			mask = (offset == 1) ? BIT(gpio - GROUP0_NR_IRQS) :
+								BIT(gpio);
+			virq = irq_find_mapping(wg->chip.irqdomain, gpio);
+			handle_nested_irq(virq);
+			regmap_update_bits(wg->regmap, IRQ_STATUS_BASE + offset,
+								mask, mask);
+		}
+
+		/* Next iteration */
+		if (regmap_bulk_read(wg->regmap, IRQ_STATUS_BASE, p, 2)) {
+			dev_err(wg->dev, "Failed to read irq status\n");
+			break;
+		}
+
+		pending = p[0] | (p[1] << 8);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static void wcove_gpio_dbg_show(struct seq_file *s,
+				      struct gpio_chip *chip)
+{
+	unsigned int ctlo, ctli, irq_mask, irq_status;
+	struct wcove_gpio *wg = gpiochip_get_data(chip);
+	int gpio, offset, group, ret = 0;
+
+	for (gpio = 0; gpio < WCOVE_GPIO_NUM; gpio++) {
+		group = gpio < GROUP0_NR_IRQS ? 0 : 1;
+		ret += regmap_read(wg->regmap, to_reg(gpio, CTRL_OUT), &ctlo);
+		ret += regmap_read(wg->regmap, to_reg(gpio, CTRL_IN), &ctli);
+		ret += regmap_read(wg->regmap, IRQ_MASK_BASE + group,
+							&irq_mask);
+		ret += regmap_read(wg->regmap, IRQ_STATUS_BASE + group,
+							&irq_status);
+		if (ret) {
+			pr_err("Failed to read registers: ctrl out/in or irq status/mask\n");
+			break;
+		}
+
+		offset = gpio % 8;
+		seq_printf(s, " gpio-%-2d %s %s %s %s ctlo=%2x,%s %s\n",
+			   gpio, ctlo & CTLO_DIR_OUT ? "out" : "in ",
+			   ctli & 0x1 ? "hi" : "lo",
+			   ctli & CTLI_INTCNT_NE ? "fall" : "    ",
+			   ctli & CTLI_INTCNT_PE ? "rise" : "    ",
+			   ctlo,
+			   irq_mask & BIT(offset) ? "mask  " : "unmask",
+			   irq_status & BIT(offset) ? "pending" : "       ");
+	}
+}
+
+static int wcove_gpio_probe(struct platform_device *pdev)
+{
+	struct intel_soc_pmic *pmic;
+	struct wcove_gpio *wg;
+	int virq, ret, irq;
+	struct device *dev;
+
+	/*
+	 * This gpio platform device is created by a mfd device (see
+	 * drivers/mfd/intel_soc_pmic_bxtwc.c for details). Information
+	 * shared by all sub-devices created by the mfd device, the regmap
+	 * pointer for instance, is stored as driver data of the mfd device
+	 * driver.
+	 */
+	pmic = dev_get_drvdata(pdev->dev.parent);
+	if (!pmic)
+		return -ENODEV;
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0)
+		return irq;
+
+	dev = &pdev->dev;
+
+	wg = devm_kzalloc(dev, sizeof(*wg), GFP_KERNEL);
+	if (!wg)
+		return -ENOMEM;
+
+	wg->regmap_irq_chip = pmic->irq_chip_data_level2;
+
+	platform_set_drvdata(pdev, wg);
+
+	mutex_init(&wg->buslock);
+	wg->chip.label = KBUILD_MODNAME;
+	wg->chip.direction_input = wcove_gpio_dir_in;
+	wg->chip.direction_output = wcove_gpio_dir_out;
+	wg->chip.get_direction = wcove_gpio_get_direction;
+	wg->chip.get = wcove_gpio_get;
+	wg->chip.set = wcove_gpio_set;
+	wg->chip.set_single_ended = wcove_gpio_set_single_ended,
+	wg->chip.base = -1;
+	wg->chip.ngpio = WCOVE_VGPIO_NUM;
+	wg->chip.can_sleep = true;
+	wg->chip.parent = pdev->dev.parent;
+	wg->chip.dbg_show = wcove_gpio_dbg_show;
+	wg->dev = dev;
+	wg->regmap = pmic->regmap;
+
+	ret = devm_gpiochip_add_data(dev, &wg->chip, wg);
+	if (ret) {
+		dev_err(dev, "Failed to add gpiochip: %d\n", ret);
+		return ret;
+	}
+
+	ret = gpiochip_irqchip_add(&wg->chip, &wcove_irqchip, 0,
+			     handle_simple_irq, IRQ_TYPE_NONE);
+	if (ret) {
+		dev_err(dev, "Failed to add irqchip: %d\n", ret);
+		return ret;
+	}
+
+	virq = regmap_irq_get_virq(wg->regmap_irq_chip, irq);
+	if (virq < 0) {
+		dev_err(dev, "Failed to get virq by irq %d\n", irq);
+		return virq;
+	}
+
+	ret = devm_request_threaded_irq(dev, virq, NULL,
+		wcove_gpio_irq_handler, IRQF_ONESHOT, pdev->name, wg);
+	if (ret) {
+		dev_err(dev, "Failed to request irq %d\n", virq);
+		return ret;
+	}
+
+	return 0;
+}
+
+/*
+ * Whiskey Cove PMIC itself is a analog device(but with digital control
+ * interface) providing power management support for other devices in
+ * the accompanied SoC, so we have no .pm for Whiskey Cove GPIO driver.
+ */
+static struct platform_driver wcove_gpio_driver = {
+	.driver = {
+		.name = "bxt_wcove_gpio",
+	},
+	.probe = wcove_gpio_probe,
+};
+
+module_platform_driver(wcove_gpio_driver);
+
+MODULE_AUTHOR("Ajay Thomas <ajay.thomas.david.rajamanickam@intel.com>");
+MODULE_AUTHOR("Bin Gao <bin.gao@intel.com>");
+MODULE_DESCRIPTION("Intel Whiskey Cove GPIO Driver");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:bxt_wcove_gpio");
diff --git a/drivers/gpio/gpio-wm831x.c b/drivers/gpio/gpio-wm831x.c
index 21f97bcd0062..533707f943f4 100644
--- a/drivers/gpio/gpio-wm831x.c
+++ b/drivers/gpio/gpio-wm831x.c
@@ -247,7 +247,7 @@ static void wm831x_gpio_dbg_show(struct seq_file *s, struct gpio_chip *chip)
 #define wm831x_gpio_dbg_show NULL
 #endif
 
-static struct gpio_chip template_chip = {
+static const struct gpio_chip template_chip = {
 	.label			= "wm831x",
 	.owner			= THIS_MODULE,
 	.direction_input	= wm831x_gpio_direction_in,
diff --git a/drivers/gpio/gpio-wm8350.c b/drivers/gpio/gpio-wm8350.c
index e9765707d5c1..e46752e73dd9 100644
--- a/drivers/gpio/gpio-wm8350.c
+++ b/drivers/gpio/gpio-wm8350.c
@@ -93,7 +93,7 @@ static int wm8350_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
 	return wm8350->irq_base + WM8350_IRQ_GPIO(offset);
 }
 
-static struct gpio_chip template_chip = {
+static const struct gpio_chip template_chip = {
 	.label			= "wm8350",
 	.owner			= THIS_MODULE,
 	.direction_input	= wm8350_gpio_direction_in,
diff --git a/drivers/gpio/gpio-wm8994.c b/drivers/gpio/gpio-wm8994.c
index 2457aac8592e..68410fda6138 100644
--- a/drivers/gpio/gpio-wm8994.c
+++ b/drivers/gpio/gpio-wm8994.c
@@ -249,7 +249,7 @@ static void wm8994_gpio_dbg_show(struct seq_file *s, struct gpio_chip *chip)
 #define wm8994_gpio_dbg_show NULL
 #endif
 
-static struct gpio_chip template_chip = {
+static const struct gpio_chip template_chip = {
 	.label			= "wm8994",
 	.owner			= THIS_MODULE,
 	.request		= wm8994_gpio_request,
diff --git a/drivers/gpio/gpio-zynq.c b/drivers/gpio/gpio-zynq.c
index e72794e463aa..6b4d10d6e10f 100644
--- a/drivers/gpio/gpio-zynq.c
+++ b/drivers/gpio/gpio-zynq.c
@@ -96,6 +96,9 @@
 /* GPIO upper 16 bit mask */
 #define ZYNQ_GPIO_UPPER_MASK 0xFFFF0000
 
+/* For GPIO quirks */
+#define ZYNQ_GPIO_QUIRK_FOO	BIT(0)
+
 /**
  * struct zynq_gpio - gpio device private data structure
  * @chip:	instance of the gpio_chip
@@ -122,6 +125,7 @@ struct zynq_gpio {
 */
 struct zynq_platform_data {
 	const char *label;
+	u32 quirks;
 	u16 ngpio;
 	int max_bank;
 	int bank_min[ZYNQMP_GPIO_MAX_BANK];
@@ -238,13 +242,19 @@ static void zynq_gpio_set_value(struct gpio_chip *chip, unsigned int pin,
 static int zynq_gpio_dir_in(struct gpio_chip *chip, unsigned int pin)
 {
 	u32 reg;
+	bool is_zynq_gpio;
 	unsigned int bank_num, bank_pin_num;
 	struct zynq_gpio *gpio = gpiochip_get_data(chip);
 
+	is_zynq_gpio = gpio->p_data->quirks & ZYNQ_GPIO_QUIRK_FOO;
 	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))
+	/*
+	 * On zynq bank 0 pins 7 and 8 are special and cannot be used
+	 * as inputs.
+	 */
+	if (is_zynq_gpio && bank_num == 0 &&
+		(bank_pin_num == 7 || bank_pin_num == 8))
 		return -EINVAL;
 
 	/* clear the bit in direction mode reg to set the pin as input */
@@ -627,6 +637,7 @@ static const struct zynq_platform_data zynqmp_gpio_def = {
 
 static const struct zynq_platform_data zynq_gpio_def = {
 	.label = "zynq_gpio",
+	.quirks = ZYNQ_GPIO_QUIRK_FOO,
 	.ngpio = ZYNQ_GPIO_NR_GPIOS,
 	.max_bank = ZYNQ_GPIO_MAX_BANK,
 	.bank_min[0] = ZYNQ_GPIO_BANK0_PIN_MIN(),
diff --git a/drivers/gpio/gpiolib-acpi.c b/drivers/gpio/gpiolib-acpi.c
index af514618d7fb..58ece201b8e6 100644
--- a/drivers/gpio/gpiolib-acpi.c
+++ b/drivers/gpio/gpiolib-acpi.c
@@ -14,6 +14,7 @@
 #include <linux/gpio.h>
 #include <linux/gpio/consumer.h>
 #include <linux/gpio/driver.h>
+#include <linux/gpio/machine.h>
 #include <linux/export.h>
 #include <linux/acpi.h>
 #include <linux/interrupt.h>
@@ -395,7 +396,7 @@ struct acpi_gpio_lookup {
 	int n;
 };
 
-static int acpi_find_gpio(struct acpi_resource *ares, void *data)
+static int acpi_populate_gpio_lookup(struct acpi_resource *ares, void *data)
 {
 	struct acpi_gpio_lookup *lookup = data;
 
@@ -440,7 +441,8 @@ static int acpi_gpio_resource_lookup(struct acpi_gpio_lookup *lookup,
 
 	INIT_LIST_HEAD(&res_list);
 
-	ret = acpi_dev_get_resources(lookup->adev, &res_list, acpi_find_gpio,
+	ret = acpi_dev_get_resources(lookup->adev, &res_list,
+				     acpi_populate_gpio_lookup,
 				     lookup);
 	if (ret < 0)
 		return ret;
@@ -513,7 +515,7 @@ static int acpi_gpio_property_lookup(struct fwnode_handle *fwnode,
  * Note: if the GPIO resource has multiple entries in the pin list, this
  * function only returns the first.
  */
-struct gpio_desc *acpi_get_gpiod_by_index(struct acpi_device *adev,
+static struct gpio_desc *acpi_get_gpiod_by_index(struct acpi_device *adev,
 					  const char *propname, int index,
 					  struct acpi_gpio_info *info)
 {
@@ -546,6 +548,55 @@ struct gpio_desc *acpi_get_gpiod_by_index(struct acpi_device *adev,
 	return ret ? ERR_PTR(ret) : lookup.desc;
 }
 
+struct gpio_desc *acpi_find_gpio(struct device *dev,
+				 const char *con_id,
+				 unsigned int idx,
+				 enum gpiod_flags flags,
+				 enum gpio_lookup_flags *lookupflags)
+{
+	struct acpi_device *adev = ACPI_COMPANION(dev);
+	struct acpi_gpio_info info;
+	struct gpio_desc *desc;
+	char propname[32];
+	int i;
+
+	/* Try first from _DSD */
+	for (i = 0; i < ARRAY_SIZE(gpio_suffixes); i++) {
+		if (con_id && strcmp(con_id, "gpios")) {
+			snprintf(propname, sizeof(propname), "%s-%s",
+				 con_id, gpio_suffixes[i]);
+		} else {
+			snprintf(propname, sizeof(propname), "%s",
+				 gpio_suffixes[i]);
+		}
+
+		desc = acpi_get_gpiod_by_index(adev, propname, idx, &info);
+		if (!IS_ERR(desc) || (PTR_ERR(desc) == -EPROBE_DEFER))
+			break;
+	}
+
+	/* Then from plain _CRS GPIOs */
+	if (IS_ERR(desc)) {
+		if (!acpi_can_fallback_to_crs(adev, con_id))
+			return ERR_PTR(-ENOENT);
+
+		desc = acpi_get_gpiod_by_index(adev, NULL, idx, &info);
+		if (IS_ERR(desc))
+			return desc;
+
+		if ((flags == GPIOD_OUT_LOW || flags == GPIOD_OUT_HIGH) &&
+		    info.gpioint) {
+			dev_dbg(dev, "refusing GpioInt() entry when doing GPIOD_OUT_* lookup\n");
+			return ERR_PTR(-ENOENT);
+		}
+	}
+
+	if (info.polarity == GPIO_ACTIVE_LOW)
+		*lookupflags |= GPIO_ACTIVE_LOW;
+
+	return desc;
+}
+
 /**
  * acpi_node_get_gpiod() - get a GPIO descriptor from ACPI resources
  * @fwnode: pointer to an ACPI firmware node to get the GPIO information from
diff --git a/drivers/gpio/gpiolib-of.c b/drivers/gpio/gpiolib-of.c
index a28feb3edf33..ecad3f0e3b77 100644
--- a/drivers/gpio/gpiolib-of.c
+++ b/drivers/gpio/gpiolib-of.c
@@ -113,6 +113,45 @@ int of_get_named_gpio_flags(struct device_node *np, const char *list_name,
 }
 EXPORT_SYMBOL(of_get_named_gpio_flags);
 
+struct gpio_desc *of_find_gpio(struct device *dev, const char *con_id,
+			       unsigned int idx,
+			       enum gpio_lookup_flags *flags)
+{
+	char prop_name[32]; /* 32 is max size of property name */
+	enum of_gpio_flags of_flags;
+	struct gpio_desc *desc;
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(gpio_suffixes); i++) {
+		if (con_id)
+			snprintf(prop_name, sizeof(prop_name), "%s-%s", con_id,
+				 gpio_suffixes[i]);
+		else
+			snprintf(prop_name, sizeof(prop_name), "%s",
+				 gpio_suffixes[i]);
+
+		desc = of_get_named_gpiod_flags(dev->of_node, prop_name, idx,
+						&of_flags);
+		if (!IS_ERR(desc) || (PTR_ERR(desc) != -ENOENT))
+			break;
+	}
+
+	if (IS_ERR(desc))
+		return desc;
+
+	if (of_flags & OF_GPIO_ACTIVE_LOW)
+		*flags |= GPIO_ACTIVE_LOW;
+
+	if (of_flags & OF_GPIO_SINGLE_ENDED) {
+		if (of_flags & OF_GPIO_ACTIVE_LOW)
+			*flags |= GPIO_OPEN_DRAIN;
+		else
+			*flags |= GPIO_OPEN_SOURCE;
+	}
+
+	return desc;
+}
+
 /**
  * of_parse_own_gpio() - Get a GPIO hog descriptor, names and flags for GPIO API
  * @np:		device node to get GPIO from
diff --git a/drivers/gpio/gpiolib-sysfs.c b/drivers/gpio/gpiolib-sysfs.c
index 932e510aec50..4b44dd97c07f 100644
--- a/drivers/gpio/gpiolib-sysfs.c
+++ b/drivers/gpio/gpiolib-sysfs.c
@@ -670,10 +670,10 @@ int gpiod_export_link(struct device *dev, const char *name,
 EXPORT_SYMBOL_GPL(gpiod_export_link);
 
 /**
- * gpiod_unexport - reverse effect of gpio_export()
+ * gpiod_unexport - reverse effect of gpiod_export()
  * @gpio: gpio to make unavailable
  *
- * This is implicit on gpio_free().
+ * This is implicit on gpiod_free().
  */
 void gpiod_unexport(struct gpio_desc *desc)
 {
diff --git a/drivers/gpio/gpiolib.c b/drivers/gpio/gpiolib.c
index 53ff25ac66d8..f0fc3a0d37c8 100644
--- a/drivers/gpio/gpiolib.c
+++ b/drivers/gpio/gpiolib.c
@@ -71,6 +71,8 @@ LIST_HEAD(gpio_devices);
 
 static void gpiochip_free_hogs(struct gpio_chip *chip);
 static void gpiochip_irqchip_remove(struct gpio_chip *gpiochip);
+static int gpiochip_irqchip_init_valid_mask(struct gpio_chip *gpiochip);
+static void gpiochip_irqchip_free_valid_mask(struct gpio_chip *gpiochip);
 
 static bool gpiolib_initialized;
 
@@ -1167,6 +1169,10 @@ int gpiochip_add_data(struct gpio_chip *chip, void *data)
 	if (status)
 		goto err_remove_from_list;
 
+	status = gpiochip_irqchip_init_valid_mask(chip);
+	if (status)
+		goto err_remove_from_list;
+
 	status = of_gpiochip_add(chip);
 	if (status)
 		goto err_remove_chip;
@@ -1192,6 +1198,7 @@ err_remove_chip:
 	acpi_gpiochip_remove(chip);
 	gpiochip_free_hogs(chip);
 	of_gpiochip_remove(chip);
+	gpiochip_irqchip_free_valid_mask(chip);
 err_remove_from_list:
 	spin_lock_irqsave(&gpio_lock, flags);
 	list_del(&gdev->list);
@@ -1363,19 +1370,15 @@ struct gpio_chip *gpiochip_find(void *data,
 					     void *data))
 {
 	struct gpio_device *gdev;
-	struct gpio_chip *chip;
+	struct gpio_chip *chip = NULL;
 	unsigned long flags;
 
 	spin_lock_irqsave(&gpio_lock, flags);
 	list_for_each_entry(gdev, &gpio_devices, list)
-		if (gdev->chip && match(gdev->chip, data))
+		if (gdev->chip && match(gdev->chip, data)) {
+			chip = gdev->chip;
 			break;
-
-	/* No match? */
-	if (&gdev->list == &gpio_devices)
-		chip = NULL;
-	else
-		chip = gdev->chip;
+		}
 
 	spin_unlock_irqrestore(&gpio_lock, flags);
 
@@ -1401,6 +1404,40 @@ static struct gpio_chip *find_chip_by_name(const char *name)
  * The following is irqchip helper code for gpiochips.
  */
 
+static int gpiochip_irqchip_init_valid_mask(struct gpio_chip *gpiochip)
+{
+	int i;
+
+	if (!gpiochip->irq_need_valid_mask)
+		return 0;
+
+	gpiochip->irq_valid_mask = kcalloc(BITS_TO_LONGS(gpiochip->ngpio),
+					   sizeof(long), GFP_KERNEL);
+	if (!gpiochip->irq_valid_mask)
+		return -ENOMEM;
+
+	/* Assume by default all GPIOs are valid */
+	for (i = 0; i < gpiochip->ngpio; i++)
+		set_bit(i, gpiochip->irq_valid_mask);
+
+	return 0;
+}
+
+static void gpiochip_irqchip_free_valid_mask(struct gpio_chip *gpiochip)
+{
+	kfree(gpiochip->irq_valid_mask);
+	gpiochip->irq_valid_mask = NULL;
+}
+
+static bool gpiochip_irqchip_irq_valid(const struct gpio_chip *gpiochip,
+				       unsigned int offset)
+{
+	/* No mask means all valid */
+	if (likely(!gpiochip->irq_valid_mask))
+		return true;
+	return test_bit(offset, gpiochip->irq_valid_mask);
+}
+
 /**
  * gpiochip_set_chained_irqchip() - sets a chained irqchip to a gpiochip
  * @gpiochip: the gpiochip to set the irqchip chain to
@@ -1442,9 +1479,12 @@ void gpiochip_set_chained_irqchip(struct gpio_chip *gpiochip,
 	}
 
 	/* Set the parent IRQ for all affected IRQs */
-	for (offset = 0; offset < gpiochip->ngpio; offset++)
+	for (offset = 0; offset < gpiochip->ngpio; offset++) {
+		if (!gpiochip_irqchip_irq_valid(gpiochip, offset))
+			continue;
 		irq_set_parent(irq_find_mapping(gpiochip->irqdomain, offset),
 			       parent_irq);
+	}
 }
 EXPORT_SYMBOL_GPL(gpiochip_set_chained_irqchip);
 
@@ -1551,9 +1591,12 @@ static void gpiochip_irqchip_remove(struct gpio_chip *gpiochip)
 
 	/* Remove all IRQ mappings and delete the domain */
 	if (gpiochip->irqdomain) {
-		for (offset = 0; offset < gpiochip->ngpio; offset++)
+		for (offset = 0; offset < gpiochip->ngpio; offset++) {
+			if (!gpiochip_irqchip_irq_valid(gpiochip, offset))
+				continue;
 			irq_dispose_mapping(
 				irq_find_mapping(gpiochip->irqdomain, offset));
+		}
 		irq_domain_remove(gpiochip->irqdomain);
 	}
 
@@ -1562,6 +1605,8 @@ static void gpiochip_irqchip_remove(struct gpio_chip *gpiochip)
 		gpiochip->irqchip->irq_release_resources = NULL;
 		gpiochip->irqchip = NULL;
 	}
+
+	gpiochip_irqchip_free_valid_mask(gpiochip);
 }
 
 /**
@@ -1597,6 +1642,7 @@ int _gpiochip_irqchip_add(struct gpio_chip *gpiochip,
 			  struct lock_class_key *lock_key)
 {
 	struct device_node *of_node;
+	bool irq_base_set = false;
 	unsigned int offset;
 	unsigned irq_base = 0;
 
@@ -1617,6 +1663,20 @@ int _gpiochip_irqchip_add(struct gpio_chip *gpiochip,
 	if (gpiochip->of_node)
 		of_node = gpiochip->of_node;
 #endif
+	/*
+	 * Specifying a default trigger is a terrible idea if DT or ACPI is
+	 * used to configure the interrupts, as you may end-up with
+	 * conflicting triggers. Tell the user, and reset to NONE.
+	 */
+	if (WARN(of_node && type != IRQ_TYPE_NONE,
+		 "%s: Ignoring %d default trigger\n", of_node->full_name, type))
+		type = IRQ_TYPE_NONE;
+	if (has_acpi_companion(gpiochip->parent) && type != IRQ_TYPE_NONE) {
+		acpi_handle_warn(ACPI_HANDLE(gpiochip->parent),
+				 "Ignoring %d default trigger\n", type);
+		type = IRQ_TYPE_NONE;
+	}
+
 	gpiochip->irqchip = irqchip;
 	gpiochip->irq_handler = handler;
 	gpiochip->irq_default_type = type;
@@ -1646,13 +1706,17 @@ int _gpiochip_irqchip_add(struct gpio_chip *gpiochip,
 	 * necessary to allocate descriptors for all IRQs.
 	 */
 	for (offset = 0; offset < gpiochip->ngpio; offset++) {
+		if (!gpiochip_irqchip_irq_valid(gpiochip, offset))
+			continue;
 		irq_base = irq_create_mapping(gpiochip->irqdomain, offset);
-		if (offset == 0)
+		if (!irq_base_set) {
 			/*
 			 * Store the base into the gpiochip to be used when
 			 * unmapping the irqs.
 			 */
 			gpiochip->irq_base = irq_base;
+			irq_base_set = true;
+		}
 	}
 
 	acpi_gpiochip_request_interrupts(gpiochip);
@@ -1664,6 +1728,12 @@ EXPORT_SYMBOL_GPL(_gpiochip_irqchip_add);
 #else /* CONFIG_GPIOLIB_IRQCHIP */
 
 static void gpiochip_irqchip_remove(struct gpio_chip *gpiochip) {}
+static inline int gpiochip_irqchip_init_valid_mask(struct gpio_chip *gpiochip)
+{
+	return 0;
+}
+static inline void gpiochip_irqchip_free_valid_mask(struct gpio_chip *gpiochip)
+{ }
 
 #endif /* CONFIG_GPIOLIB_IRQCHIP */
 
@@ -2813,94 +2883,6 @@ void gpiod_remove_lookup_table(struct gpiod_lookup_table *table)
 	mutex_unlock(&gpio_lookup_lock);
 }
 
-static struct gpio_desc *of_find_gpio(struct device *dev, const char *con_id,
-				      unsigned int idx,
-				      enum gpio_lookup_flags *flags)
-{
-	char prop_name[32]; /* 32 is max size of property name */
-	enum of_gpio_flags of_flags;
-	struct gpio_desc *desc;
-	unsigned int i;
-
-	for (i = 0; i < ARRAY_SIZE(gpio_suffixes); i++) {
-		if (con_id)
-			snprintf(prop_name, sizeof(prop_name), "%s-%s", con_id,
-				 gpio_suffixes[i]);
-		else
-			snprintf(prop_name, sizeof(prop_name), "%s",
-				 gpio_suffixes[i]);
-
-		desc = of_get_named_gpiod_flags(dev->of_node, prop_name, idx,
-						&of_flags);
-		if (!IS_ERR(desc) || (PTR_ERR(desc) != -ENOENT))
-			break;
-	}
-
-	if (IS_ERR(desc))
-		return desc;
-
-	if (of_flags & OF_GPIO_ACTIVE_LOW)
-		*flags |= GPIO_ACTIVE_LOW;
-
-	if (of_flags & OF_GPIO_SINGLE_ENDED) {
-		if (of_flags & OF_GPIO_ACTIVE_LOW)
-			*flags |= GPIO_OPEN_DRAIN;
-		else
-			*flags |= GPIO_OPEN_SOURCE;
-	}
-
-	return desc;
-}
-
-static struct gpio_desc *acpi_find_gpio(struct device *dev,
-					const char *con_id,
-					unsigned int idx,
-					enum gpiod_flags flags,
-					enum gpio_lookup_flags *lookupflags)
-{
-	struct acpi_device *adev = ACPI_COMPANION(dev);
-	struct acpi_gpio_info info;
-	struct gpio_desc *desc;
-	char propname[32];
-	int i;
-
-	/* Try first from _DSD */
-	for (i = 0; i < ARRAY_SIZE(gpio_suffixes); i++) {
-		if (con_id && strcmp(con_id, "gpios")) {
-			snprintf(propname, sizeof(propname), "%s-%s",
-				 con_id, gpio_suffixes[i]);
-		} else {
-			snprintf(propname, sizeof(propname), "%s",
-				 gpio_suffixes[i]);
-		}
-
-		desc = acpi_get_gpiod_by_index(adev, propname, idx, &info);
-		if (!IS_ERR(desc) || (PTR_ERR(desc) == -EPROBE_DEFER))
-			break;
-	}
-
-	/* Then from plain _CRS GPIOs */
-	if (IS_ERR(desc)) {
-		if (!acpi_can_fallback_to_crs(adev, con_id))
-			return ERR_PTR(-ENOENT);
-
-		desc = acpi_get_gpiod_by_index(adev, NULL, idx, &info);
-		if (IS_ERR(desc))
-			return desc;
-
-		if ((flags == GPIOD_OUT_LOW || flags == GPIOD_OUT_HIGH) &&
-		    info.gpioint) {
-			dev_dbg(dev, "refusing GpioInt() entry when doing GPIOD_OUT_* lookup\n");
-			return ERR_PTR(-ENOENT);
-		}
-	}
-
-	if (info.polarity == GPIO_ACTIVE_LOW)
-		*lookupflags |= GPIO_ACTIVE_LOW;
-
-	return desc;
-}
-
 static struct gpiod_lookup_table *gpiod_find_lookup_table(struct device *dev)
 {
 	const char *dev_id = dev ? dev_name(dev) : NULL;
diff --git a/drivers/gpio/gpiolib.h b/drivers/gpio/gpiolib.h
index 2d9ea5e0cab3..346fbda39220 100644
--- a/drivers/gpio/gpiolib.h
+++ b/drivers/gpio/gpiolib.h
@@ -20,6 +20,7 @@
 
 enum of_gpio_flags;
 enum gpiod_flags;
+enum gpio_lookup_flags;
 struct acpi_device;
 
 /**
@@ -86,6 +87,32 @@ struct acpi_gpio_info {
 /* gpio suffixes used for ACPI and device tree lookup */
 static const char * const gpio_suffixes[] = { "gpios", "gpio" };
 
+#ifdef CONFIG_OF_GPIO
+struct gpio_desc *of_find_gpio(struct device *dev,
+			       const char *con_id,
+			       unsigned int idx,
+			       enum gpio_lookup_flags *flags);
+struct gpio_desc *of_get_named_gpiod_flags(struct device_node *np,
+		   const char *list_name, int index, enum of_gpio_flags *flags);
+int of_gpiochip_add(struct gpio_chip *gc);
+void of_gpiochip_remove(struct gpio_chip *gc);
+#else
+static inline struct gpio_desc *of_find_gpio(struct device *dev,
+					     const char *con_id,
+					     unsigned int idx,
+					     enum gpio_lookup_flags *flags)
+{
+	return ERR_PTR(-ENOENT);
+}
+static inline struct gpio_desc *of_get_named_gpiod_flags(struct device_node *np,
+		   const char *list_name, int index, enum of_gpio_flags *flags)
+{
+	return ERR_PTR(-ENOENT);
+}
+static inline int of_gpiochip_add(struct gpio_chip *gc) { return 0; }
+static inline void of_gpiochip_remove(struct gpio_chip *gc) { }
+#endif /* CONFIG_OF_GPIO */
+
 #ifdef CONFIG_ACPI
 void acpi_gpiochip_add(struct gpio_chip *chip);
 void acpi_gpiochip_remove(struct gpio_chip *chip);
@@ -93,9 +120,11 @@ void acpi_gpiochip_remove(struct gpio_chip *chip);
 void acpi_gpiochip_request_interrupts(struct gpio_chip *chip);
 void acpi_gpiochip_free_interrupts(struct gpio_chip *chip);
 
-struct gpio_desc *acpi_get_gpiod_by_index(struct acpi_device *adev,
-					  const char *propname, int index,
-					  struct acpi_gpio_info *info);
+struct gpio_desc *acpi_find_gpio(struct device *dev,
+				 const char *con_id,
+				 unsigned int idx,
+				 enum gpiod_flags flags,
+				 enum gpio_lookup_flags *lookupflags);
 struct gpio_desc *acpi_node_get_gpiod(struct fwnode_handle *fwnode,
 				      const char *propname, int index,
 				      struct acpi_gpio_info *info);
@@ -114,10 +143,11 @@ static inline void
 acpi_gpiochip_free_interrupts(struct gpio_chip *chip) { }
 
 static inline struct gpio_desc *
-acpi_get_gpiod_by_index(struct acpi_device *adev, const char *propname,
-			int index, struct acpi_gpio_info *info)
+acpi_find_gpio(struct device *dev, const char *con_id,
+	       unsigned int idx, enum gpiod_flags flags,
+	       enum gpio_lookup_flags *lookupflags)
 {
-	return ERR_PTR(-ENOSYS);
+	return ERR_PTR(-ENOENT);
 }
 static inline struct gpio_desc *
 acpi_node_get_gpiod(struct fwnode_handle *fwnode, const char *propname,
@@ -137,9 +167,6 @@ static inline bool acpi_can_fallback_to_crs(struct acpi_device *adev,
 }
 #endif
 
-struct gpio_desc *of_get_named_gpiod_flags(struct device_node *np,
-		   const char *list_name, int index, enum of_gpio_flags *flags);
-
 struct gpio_desc *gpiochip_get_desc(struct gpio_chip *chip, u16 hwnum);
 void gpiod_set_array_value_complex(bool raw, bool can_sleep,
 				   unsigned int array_size,
diff --git a/drivers/gpu/drm/amd/amdgpu/Kconfig b/drivers/gpu/drm/amd/amdgpu/Kconfig
index 53cf3971dfc3..61360e27715f 100644
--- a/drivers/gpu/drm/amd/amdgpu/Kconfig
+++ b/drivers/gpu/drm/amd/amdgpu/Kconfig
@@ -32,5 +32,4 @@ config DRM_AMDGPU_GART_DEBUGFS
 	  Selecting this option creates a debugfs file to inspect the mapped
 	  pages. Uses more memory for housekeeping, enable only for debugging.
 
-source "drivers/gpu/drm/amd/powerplay/Kconfig"
 source "drivers/gpu/drm/amd/acp/Kconfig"
diff --git a/drivers/gpu/drm/amd/amdgpu/Makefile b/drivers/gpu/drm/amd/amdgpu/Makefile
index 9ec262d4b8a2..248a05d02917 100644
--- a/drivers/gpu/drm/amd/amdgpu/Makefile
+++ b/drivers/gpu/drm/amd/amdgpu/Makefile
@@ -111,14 +111,10 @@ amdgpu-$(CONFIG_VGA_SWITCHEROO) += amdgpu_atpx_handler.o
 amdgpu-$(CONFIG_ACPI) += amdgpu_acpi.o
 amdgpu-$(CONFIG_MMU_NOTIFIER) += amdgpu_mn.o
 
-ifneq ($(CONFIG_DRM_AMD_POWERPLAY),)
-
 include $(FULL_AMD_PATH)/powerplay/Makefile
 
 amdgpu-y += $(AMD_POWERPLAY_FILES)
 
-endif
-
 obj-$(CONFIG_DRM_AMDGPU)+= amdgpu.o
 
 CFLAGS_amdgpu_trace_points.o := -I$(src)
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu.h b/drivers/gpu/drm/amd/amdgpu/amdgpu.h
index 235f3902643a..039b57e4644c 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu.h
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu.h
@@ -1943,6 +1943,7 @@ struct amdgpu_ip_block_status {
 	bool valid;
 	bool sw;
 	bool hw;
+	bool late_initialized;
 	bool hang;
 };
 
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_connectors.c b/drivers/gpu/drm/amd/amdgpu/amdgpu_connectors.c
index 22c11e7698c8..e3281d4e3e41 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_connectors.c
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_connectors.c
@@ -765,12 +765,20 @@ amdgpu_connector_lvds_detect(struct drm_connector *connector, bool force)
 	return ret;
 }
 
-static void amdgpu_connector_destroy(struct drm_connector *connector)
+static void amdgpu_connector_unregister(struct drm_connector *connector)
 {
 	struct amdgpu_connector *amdgpu_connector = to_amdgpu_connector(connector);
 
-	if (amdgpu_connector->ddc_bus->has_aux)
+	if (amdgpu_connector->ddc_bus->has_aux) {
 		drm_dp_aux_unregister(&amdgpu_connector->ddc_bus->aux);
+		amdgpu_connector->ddc_bus->has_aux = false;
+	}
+}
+
+static void amdgpu_connector_destroy(struct drm_connector *connector)
+{
+	struct amdgpu_connector *amdgpu_connector = to_amdgpu_connector(connector);
+
 	amdgpu_connector_free_edid(connector);
 	kfree(amdgpu_connector->con_priv);
 	drm_connector_unregister(connector);
@@ -824,6 +832,7 @@ static const struct drm_connector_funcs amdgpu_connector_lvds_funcs = {
 	.dpms = drm_helper_connector_dpms,
 	.detect = amdgpu_connector_lvds_detect,
 	.fill_modes = drm_helper_probe_single_connector_modes,
+	.early_unregister = amdgpu_connector_unregister,
 	.destroy = amdgpu_connector_destroy,
 	.set_property = amdgpu_connector_set_lcd_property,
 };
@@ -934,6 +943,7 @@ static const struct drm_connector_funcs amdgpu_connector_vga_funcs = {
 	.dpms = drm_helper_connector_dpms,
 	.detect = amdgpu_connector_vga_detect,
 	.fill_modes = drm_helper_probe_single_connector_modes,
+	.early_unregister = amdgpu_connector_unregister,
 	.destroy = amdgpu_connector_destroy,
 	.set_property = amdgpu_connector_set_property,
 };
@@ -1201,6 +1211,7 @@ static const struct drm_connector_funcs amdgpu_connector_dvi_funcs = {
 	.detect = amdgpu_connector_dvi_detect,
 	.fill_modes = drm_helper_probe_single_connector_modes,
 	.set_property = amdgpu_connector_set_property,
+	.early_unregister = amdgpu_connector_unregister,
 	.destroy = amdgpu_connector_destroy,
 	.force = amdgpu_connector_dvi_force,
 };
@@ -1491,6 +1502,7 @@ static const struct drm_connector_funcs amdgpu_connector_dp_funcs = {
 	.detect = amdgpu_connector_dp_detect,
 	.fill_modes = drm_helper_probe_single_connector_modes,
 	.set_property = amdgpu_connector_set_property,
+	.early_unregister = amdgpu_connector_unregister,
 	.destroy = amdgpu_connector_destroy,
 	.force = amdgpu_connector_dvi_force,
 };
@@ -1500,6 +1512,7 @@ static const struct drm_connector_funcs amdgpu_connector_edp_funcs = {
 	.detect = amdgpu_connector_dp_detect,
 	.fill_modes = drm_helper_probe_single_connector_modes,
 	.set_property = amdgpu_connector_set_lcd_property,
+	.early_unregister = amdgpu_connector_unregister,
 	.destroy = amdgpu_connector_destroy,
 	.force = amdgpu_connector_dvi_force,
 };
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_ctx.c b/drivers/gpu/drm/amd/amdgpu/amdgpu_ctx.c
index e203e5561107..a5e2fcbef0f0 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_ctx.c
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_ctx.c
@@ -43,6 +43,9 @@ static int amdgpu_ctx_init(struct amdgpu_device *adev, struct amdgpu_ctx *ctx)
 		ctx->rings[i].sequence = 1;
 		ctx->rings[i].fences = &ctx->fences[amdgpu_sched_jobs * i];
 	}
+
+	ctx->reset_counter = atomic_read(&adev->gpu_reset_counter);
+
 	/* create context entity for each ring */
 	for (i = 0; i < adev->num_rings; i++) {
 		struct amdgpu_ring *ring = adev->rings[i];
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_device.c b/drivers/gpu/drm/amd/amdgpu/amdgpu_device.c
index 99a15cad6789..b4f4a9239069 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_device.c
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_device.c
@@ -1408,22 +1408,25 @@ static int amdgpu_late_init(struct amdgpu_device *adev)
 	for (i = 0; i < adev->num_ip_blocks; i++) {
 		if (!adev->ip_block_status[i].valid)
 			continue;
-		if (adev->ip_blocks[i].type == AMD_IP_BLOCK_TYPE_UVD ||
-			adev->ip_blocks[i].type == AMD_IP_BLOCK_TYPE_VCE)
-			continue;
-		/* enable clockgating to save power */
-		r = adev->ip_blocks[i].funcs->set_clockgating_state((void *)adev,
-								    AMD_CG_STATE_GATE);
-		if (r) {
-			DRM_ERROR("set_clockgating_state(gate) of IP block <%s> failed %d\n", adev->ip_blocks[i].funcs->name, r);
-			return r;
-		}
 		if (adev->ip_blocks[i].funcs->late_init) {
 			r = adev->ip_blocks[i].funcs->late_init((void *)adev);
 			if (r) {
 				DRM_ERROR("late_init of IP block <%s> failed %d\n", adev->ip_blocks[i].funcs->name, r);
 				return r;
 			}
+			adev->ip_block_status[i].late_initialized = true;
+		}
+		/* skip CG for VCE/UVD, it's handled specially */
+		if (adev->ip_blocks[i].type != AMD_IP_BLOCK_TYPE_UVD &&
+		    adev->ip_blocks[i].type != AMD_IP_BLOCK_TYPE_VCE) {
+			/* enable clockgating to save power */
+			r = adev->ip_blocks[i].funcs->set_clockgating_state((void *)adev,
+									    AMD_CG_STATE_GATE);
+			if (r) {
+				DRM_ERROR("set_clockgating_state(gate) of IP block <%s> failed %d\n",
+					  adev->ip_blocks[i].funcs->name, r);
+				return r;
+			}
 		}
 	}
 
@@ -1434,6 +1437,30 @@ static int amdgpu_fini(struct amdgpu_device *adev)
 {
 	int i, r;
 
+	/* need to disable SMC first */
+	for (i = 0; i < adev->num_ip_blocks; i++) {
+		if (!adev->ip_block_status[i].hw)
+			continue;
+		if (adev->ip_blocks[i].type == AMD_IP_BLOCK_TYPE_SMC) {
+			/* ungate blocks before hw fini so that we can shutdown the blocks safely */
+			r = adev->ip_blocks[i].funcs->set_clockgating_state((void *)adev,
+									    AMD_CG_STATE_UNGATE);
+			if (r) {
+				DRM_ERROR("set_clockgating_state(ungate) of IP block <%s> failed %d\n",
+					  adev->ip_blocks[i].funcs->name, r);
+				return r;
+			}
+			r = adev->ip_blocks[i].funcs->hw_fini((void *)adev);
+			/* XXX handle errors */
+			if (r) {
+				DRM_DEBUG("hw_fini of IP block <%s> failed %d\n",
+					  adev->ip_blocks[i].funcs->name, r);
+			}
+			adev->ip_block_status[i].hw = false;
+			break;
+		}
+	}
+
 	for (i = adev->num_ip_blocks - 1; i >= 0; i--) {
 		if (!adev->ip_block_status[i].hw)
 			continue;
@@ -1469,8 +1496,11 @@ static int amdgpu_fini(struct amdgpu_device *adev)
 	}
 
 	for (i = adev->num_ip_blocks - 1; i >= 0; i--) {
+		if (!adev->ip_block_status[i].late_initialized)
+			continue;
 		if (adev->ip_blocks[i].funcs->late_fini)
 			adev->ip_blocks[i].funcs->late_fini((void *)adev);
+		adev->ip_block_status[i].late_initialized = false;
 	}
 
 	return 0;
@@ -1822,11 +1852,11 @@ void amdgpu_device_fini(struct amdgpu_device *adev)
 
 	DRM_INFO("amdgpu: finishing device.\n");
 	adev->shutdown = true;
+	drm_crtc_force_disable_all(adev->ddev);
 	/* evict vram memory */
 	amdgpu_bo_evict_vram(adev);
 	amdgpu_ib_pool_fini(adev);
 	amdgpu_fence_driver_fini(adev);
-	drm_crtc_force_disable_all(adev->ddev);
 	amdgpu_fbdev_fini(adev);
 	r = amdgpu_fini(adev);
 	kfree(adev->ip_block_status);
@@ -2069,7 +2099,8 @@ static bool amdgpu_check_soft_reset(struct amdgpu_device *adev)
 		if (!adev->ip_block_status[i].valid)
 			continue;
 		if (adev->ip_blocks[i].funcs->check_soft_reset)
-			adev->ip_blocks[i].funcs->check_soft_reset(adev);
+			adev->ip_block_status[i].hang =
+				adev->ip_blocks[i].funcs->check_soft_reset(adev);
 		if (adev->ip_block_status[i].hang) {
 			DRM_INFO("IP block:%d is hang!\n", i);
 			asic_hang = true;
@@ -2098,12 +2129,20 @@ static int amdgpu_pre_soft_reset(struct amdgpu_device *adev)
 
 static bool amdgpu_need_full_reset(struct amdgpu_device *adev)
 {
-	if (adev->ip_block_status[AMD_IP_BLOCK_TYPE_GMC].hang ||
-	    adev->ip_block_status[AMD_IP_BLOCK_TYPE_SMC].hang ||
-	    adev->ip_block_status[AMD_IP_BLOCK_TYPE_ACP].hang ||
-	    adev->ip_block_status[AMD_IP_BLOCK_TYPE_DCE].hang) {
-		DRM_INFO("Some block need full reset!\n");
-		return true;
+	int i;
+
+	for (i = 0; i < adev->num_ip_blocks; i++) {
+		if (!adev->ip_block_status[i].valid)
+			continue;
+		if ((adev->ip_blocks[i].type == AMD_IP_BLOCK_TYPE_GMC) ||
+		    (adev->ip_blocks[i].type == AMD_IP_BLOCK_TYPE_SMC) ||
+		    (adev->ip_blocks[i].type == AMD_IP_BLOCK_TYPE_ACP) ||
+		    (adev->ip_blocks[i].type == AMD_IP_BLOCK_TYPE_DCE)) {
+			if (adev->ip_block_status[i].hang) {
+				DRM_INFO("Some block need full reset!\n");
+				return true;
+			}
+		}
 	}
 	return false;
 }
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_dpm.c b/drivers/gpu/drm/amd/amdgpu/amdgpu_dpm.c
index fe36caf1b7d7..14f57d9915e3 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_dpm.c
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_dpm.c
@@ -113,24 +113,26 @@ void amdgpu_dpm_print_ps_status(struct amdgpu_device *adev,
 	printk("\n");
 }
 
+
 u32 amdgpu_dpm_get_vblank_time(struct amdgpu_device *adev)
 {
 	struct drm_device *dev = adev->ddev;
 	struct drm_crtc *crtc;
 	struct amdgpu_crtc *amdgpu_crtc;
-	u32 line_time_us, vblank_lines;
+	u32 vblank_in_pixels;
 	u32 vblank_time_us = 0xffffffff; /* if the displays are off, vblank time is max */
 
 	if (adev->mode_info.num_crtc && adev->mode_info.mode_config_initialized) {
 		list_for_each_entry(crtc, &dev->mode_config.crtc_list, head) {
 			amdgpu_crtc = to_amdgpu_crtc(crtc);
 			if (crtc->enabled && amdgpu_crtc->enabled && amdgpu_crtc->hw_mode.clock) {
-				line_time_us = (amdgpu_crtc->hw_mode.crtc_htotal * 1000) /
-					amdgpu_crtc->hw_mode.clock;
-				vblank_lines = amdgpu_crtc->hw_mode.crtc_vblank_end -
+				vblank_in_pixels =
+					amdgpu_crtc->hw_mode.crtc_htotal *
+					(amdgpu_crtc->hw_mode.crtc_vblank_end -
 					amdgpu_crtc->hw_mode.crtc_vdisplay +
-					(amdgpu_crtc->v_border * 2);
-				vblank_time_us = vblank_lines * line_time_us;
+					(amdgpu_crtc->v_border * 2));
+
+				vblank_time_us = vblank_in_pixels * 1000 / amdgpu_crtc->hw_mode.clock;
 				break;
 			}
 		}
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_drv.c b/drivers/gpu/drm/amd/amdgpu/amdgpu_drv.c
index dbe89fb25694..71ed27eb3dde 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_drv.c
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_drv.c
@@ -174,7 +174,6 @@ module_param_named(sched_jobs, amdgpu_sched_jobs, int, 0444);
 MODULE_PARM_DESC(sched_hw_submission, "the max number of HW submissions (default 2)");
 module_param_named(sched_hw_submission, amdgpu_sched_hw_submission, int, 0444);
 
-#ifdef CONFIG_DRM_AMD_POWERPLAY
 MODULE_PARM_DESC(powerplay, "Powerplay component (1 = enable, 0 = disable, -1 = auto (default))");
 module_param_named(powerplay, amdgpu_powerplay, int, 0444);
 
@@ -183,7 +182,6 @@ module_param_named(powercontainment, amdgpu_powercontainment, int, 0444);
 
 MODULE_PARM_DESC(ppfeaturemask, "all power features enabled (default))");
 module_param_named(ppfeaturemask, amdgpu_pp_feature_mask, int, 0444);
-#endif
 
 MODULE_PARM_DESC(sclkdeepsleep, "SCLK Deep Sleep (1 = enable (default), 0 = disable)");
 module_param_named(sclkdeepsleep, amdgpu_sclk_deep_sleep_en, int, 0444);
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_i2c.c b/drivers/gpu/drm/amd/amdgpu/amdgpu_i2c.c
index 34bab616588c..91d367399956 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_i2c.c
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_i2c.c
@@ -220,6 +220,7 @@ void amdgpu_i2c_destroy(struct amdgpu_i2c_chan *i2c)
 {
 	if (!i2c)
 		return;
+	WARN_ON(i2c->has_aux);
 	i2c_del_adapter(&i2c->adapter);
 	kfree(i2c);
 }
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_powerplay.c b/drivers/gpu/drm/amd/amdgpu/amdgpu_powerplay.c
index 68ad24101a36..7532ff822aa7 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_powerplay.c
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_powerplay.c
@@ -42,7 +42,6 @@ static int amdgpu_powerplay_init(struct amdgpu_device *adev)
 	amd_pp = &(adev->powerplay);
 
 	if (adev->pp_enabled) {
-#ifdef CONFIG_DRM_AMD_POWERPLAY
 		struct amd_pp_init *pp_init;
 
 		pp_init = kzalloc(sizeof(struct amd_pp_init), GFP_KERNEL);
@@ -55,7 +54,6 @@ static int amdgpu_powerplay_init(struct amdgpu_device *adev)
 		pp_init->device = amdgpu_cgs_create_device(adev);
 		ret = amd_powerplay_init(pp_init, amd_pp);
 		kfree(pp_init);
-#endif
 	} else {
 		amd_pp->pp_handle = (void *)adev;
 
@@ -97,7 +95,6 @@ static int amdgpu_pp_early_init(void *handle)
 	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
 	int ret = 0;
 
-#ifdef CONFIG_DRM_AMD_POWERPLAY
 	switch (adev->asic_type) {
 	case CHIP_POLARIS11:
 	case CHIP_POLARIS10:
@@ -120,9 +117,6 @@ static int amdgpu_pp_early_init(void *handle)
 		adev->pp_enabled = false;
 		break;
 	}
-#else
-	adev->pp_enabled = false;
-#endif
 
 	ret = amdgpu_powerplay_init(adev);
 	if (ret)
@@ -144,12 +138,11 @@ static int amdgpu_pp_late_init(void *handle)
 		ret = adev->powerplay.ip_funcs->late_init(
 					adev->powerplay.pp_handle);
 
-#ifdef CONFIG_DRM_AMD_POWERPLAY
 	if (adev->pp_enabled && adev->pm.dpm_enabled) {
 		amdgpu_pm_sysfs_init(adev);
 		amdgpu_dpm_dispatch_task(adev, AMD_PP_EVENT_COMPLETE_INIT, NULL, NULL);
 	}
-#endif
+
 	return ret;
 }
 
@@ -162,10 +155,8 @@ static int amdgpu_pp_sw_init(void *handle)
 		ret = adev->powerplay.ip_funcs->sw_init(
 					adev->powerplay.pp_handle);
 
-#ifdef CONFIG_DRM_AMD_POWERPLAY
 	if (adev->pp_enabled)
 		adev->pm.dpm_enabled = true;
-#endif
 
 	return ret;
 }
@@ -216,7 +207,6 @@ static int amdgpu_pp_hw_fini(void *handle)
 
 static void amdgpu_pp_late_fini(void *handle)
 {
-#ifdef CONFIG_DRM_AMD_POWERPLAY
 	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
 
 	if (adev->pp_enabled) {
@@ -227,7 +217,6 @@ static void amdgpu_pp_late_fini(void *handle)
 	if (adev->powerplay.ip_funcs->late_fini)
 		adev->powerplay.ip_funcs->late_fini(
 			  adev->powerplay.pp_handle);
-#endif
 }
 
 static int amdgpu_pp_suspend(void *handle)
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_ring.c b/drivers/gpu/drm/amd/amdgpu/amdgpu_ring.c
index e1fa8731d1e2..3cb5e903cd62 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_ring.c
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_ring.c
@@ -345,8 +345,8 @@ static int amdgpu_debugfs_ring_init(struct amdgpu_device *adev,
 	ent = debugfs_create_file(name,
 				  S_IFREG | S_IRUGO, root,
 				  ring, &amdgpu_debugfs_ring_fops);
-	if (IS_ERR(ent))
-		return PTR_ERR(ent);
+	if (!ent)
+		return -ENOMEM;
 
 	i_size_write(ent->d_inode, ring->ring_size + 12);
 	ring->ent = ent;
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_ttm.c b/drivers/gpu/drm/amd/amdgpu/amdgpu_ttm.c
index 887483b8b818..dcaf691f56b5 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_ttm.c
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_ttm.c
@@ -555,10 +555,13 @@ struct amdgpu_ttm_tt {
 int amdgpu_ttm_tt_get_user_pages(struct ttm_tt *ttm, struct page **pages)
 {
 	struct amdgpu_ttm_tt *gtt = (void *)ttm;
-	int write = !(gtt->userflags & AMDGPU_GEM_USERPTR_READONLY);
+	unsigned int flags = 0;
 	unsigned pinned = 0;
 	int r;
 
+	if (!(gtt->userflags & AMDGPU_GEM_USERPTR_READONLY))
+		flags |= FOLL_WRITE;
+
 	if (gtt->userflags & AMDGPU_GEM_USERPTR_ANONONLY) {
 		/* check that we only use anonymous memory
 		   to prevent problems with writeback */
@@ -581,7 +584,7 @@ int amdgpu_ttm_tt_get_user_pages(struct ttm_tt *ttm, struct page **pages)
 		list_add(&guptask.list, &gtt->guptasks);
 		spin_unlock(&gtt->guptasklock);
 
-		r = get_user_pages(userptr, num_pages, write, 0, p, NULL);
+		r = get_user_pages(userptr, num_pages, flags, p, NULL);
 
 		spin_lock(&gtt->guptasklock);
 		list_del(&guptask.list);
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_ucode.c b/drivers/gpu/drm/amd/amdgpu/amdgpu_ucode.c
index 7a05f79818f1..cb3d252f3c78 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_ucode.c
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_ucode.c
@@ -273,7 +273,6 @@ int amdgpu_ucode_init_bo(struct amdgpu_device *adev)
 
 	amdgpu_bo_unreserve(*bo);
 
-	fw_offset = 0;
 	for (i = 0; i < AMDGPU_UCODE_ID_MAXIMUM; i++) {
 		ucode = &adev->firmware.ucode[i];
 		if (ucode->fw) {
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_vce.c b/drivers/gpu/drm/amd/amdgpu/amdgpu_vce.c
index 3b03558ddb01..7fe8fd884f06 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_vce.c
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_vce.c
@@ -699,6 +699,20 @@ int amdgpu_vce_ring_parse_cs(struct amdgpu_cs_parser *p, uint32_t ib_idx)
 		case 0x05000009: /* clock table */
 			break;
 
+		case 0x0500000c: /* hw config */
+			switch (p->adev->asic_type) {
+#ifdef CONFIG_DRM_AMDGPU_CIK
+			case CHIP_KAVERI:
+			case CHIP_MULLINS:
+#endif
+			case CHIP_CARRIZO:
+				break;
+			default:
+				r = -EINVAL;
+				goto out;
+			}
+			break;
+
 		case 0x03000001: /* encode */
 			r = amdgpu_vce_cs_reloc(p, ib_idx, idx + 10, idx + 9,
 						*size, 0);
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_vm.c b/drivers/gpu/drm/amd/amdgpu/amdgpu_vm.c
index bc4b22c6fc08..06f24322e7c3 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_vm.c
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_vm.c
@@ -878,13 +878,13 @@ static void amdgpu_vm_frag_ptes(struct amdgpu_pte_update_params	*params,
 	 * allocation size to the fragment size.
 	 */
 
-	const uint64_t frag_align = 1 << AMDGPU_LOG2_PAGES_PER_FRAG;
+	/* SI and newer are optimized for 64KB */
+	uint64_t frag_flags = AMDGPU_PTE_FRAG(AMDGPU_LOG2_PAGES_PER_FRAG);
+	uint64_t frag_align = 1 << AMDGPU_LOG2_PAGES_PER_FRAG;
 
 	uint64_t frag_start = ALIGN(start, frag_align);
 	uint64_t frag_end = end & ~(frag_align - 1);
 
-	uint32_t frag;
-
 	/* system pages are non continuously */
 	if (params->src || !(flags & AMDGPU_PTE_VALID) ||
 	    (frag_start >= frag_end)) {
@@ -893,10 +893,6 @@ static void amdgpu_vm_frag_ptes(struct amdgpu_pte_update_params	*params,
 		return;
 	}
 
-	/* use more than 64KB fragment size if possible */
-	frag = lower_32_bits(frag_start | frag_end);
-	frag = likely(frag) ? __ffs(frag) : 31;
-
 	/* handle the 4K area at the beginning */
 	if (start != frag_start) {
 		amdgpu_vm_update_ptes(params, vm, start, frag_start,
@@ -906,7 +902,7 @@ static void amdgpu_vm_frag_ptes(struct amdgpu_pte_update_params	*params,
 
 	/* handle the area in the middle */
 	amdgpu_vm_update_ptes(params, vm, frag_start, frag_end, dst,
-			      flags | AMDGPU_PTE_FRAG(frag));
+			      flags | frag_flags);
 
 	/* handle the 4K area at the end */
 	if (frag_end != end) {
diff --git a/drivers/gpu/drm/amd/amdgpu/cz_dpm.c b/drivers/gpu/drm/amd/amdgpu/cz_dpm.c
index f80a0834e889..3c082e143730 100644
--- a/drivers/gpu/drm/amd/amdgpu/cz_dpm.c
+++ b/drivers/gpu/drm/amd/amdgpu/cz_dpm.c
@@ -1514,14 +1514,16 @@ static int cz_dpm_set_powergating_state(void *handle,
 	return 0;
 }
 
-/* borrowed from KV, need future unify */
 static int cz_dpm_get_temperature(struct amdgpu_device *adev)
 {
 	int actual_temp = 0;
-	uint32_t temp = RREG32_SMC(0xC0300E0C);
+	uint32_t val = RREG32_SMC(ixTHM_TCON_CUR_TMP);
+	uint32_t temp = REG_GET_FIELD(val, THM_TCON_CUR_TMP, CUR_TEMP);
 
-	if (temp)
+	if (REG_GET_FIELD(val, THM_TCON_CUR_TMP, CUR_TEMP_RANGE_SEL))
 		actual_temp = 1000 * ((temp / 8) - 49);
+	else
+		actual_temp = 1000 * (temp / 8);
 
 	return actual_temp;
 }
diff --git a/drivers/gpu/drm/amd/amdgpu/dce_v10_0.c b/drivers/gpu/drm/amd/amdgpu/dce_v10_0.c
index 613ebb7ed50f..4108c686aa7c 100644
--- a/drivers/gpu/drm/amd/amdgpu/dce_v10_0.c
+++ b/drivers/gpu/drm/amd/amdgpu/dce_v10_0.c
@@ -3188,16 +3188,11 @@ static int dce_v10_0_wait_for_idle(void *handle)
 	return 0;
 }
 
-static int dce_v10_0_check_soft_reset(void *handle)
+static bool dce_v10_0_check_soft_reset(void *handle)
 {
 	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
 
-	if (dce_v10_0_is_display_hung(adev))
-		adev->ip_block_status[AMD_IP_BLOCK_TYPE_DCE].hang = true;
-	else
-		adev->ip_block_status[AMD_IP_BLOCK_TYPE_DCE].hang = false;
-
-	return 0;
+	return dce_v10_0_is_display_hung(adev);
 }
 
 static int dce_v10_0_soft_reset(void *handle)
@@ -3205,9 +3200,6 @@ static int dce_v10_0_soft_reset(void *handle)
 	u32 srbm_soft_reset = 0, tmp;
 	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
 
-	if (!adev->ip_block_status[AMD_IP_BLOCK_TYPE_DCE].hang)
-		return 0;
-
 	if (dce_v10_0_is_display_hung(adev))
 		srbm_soft_reset |= SRBM_SOFT_RESET__SOFT_RESET_DC_MASK;
 
diff --git a/drivers/gpu/drm/amd/amdgpu/dce_v11_0.c b/drivers/gpu/drm/amd/amdgpu/dce_v11_0.c
index 678f5eb6cbc2..f264b8f17ad1 100644
--- a/drivers/gpu/drm/amd/amdgpu/dce_v11_0.c
+++ b/drivers/gpu/drm/amd/amdgpu/dce_v11_0.c
@@ -3159,6 +3159,7 @@ static int dce_v11_0_sw_fini(void *handle)
 
 	dce_v11_0_afmt_fini(adev);
 
+	drm_mode_config_cleanup(adev->ddev);
 	adev->mode_info.mode_config_initialized = false;
 
 	return 0;
diff --git a/drivers/gpu/drm/amd/amdgpu/dce_virtual.c b/drivers/gpu/drm/amd/amdgpu/dce_virtual.c
index a754f2522ba2..c2bd9f045532 100644
--- a/drivers/gpu/drm/amd/amdgpu/dce_virtual.c
+++ b/drivers/gpu/drm/amd/amdgpu/dce_virtual.c
@@ -99,15 +99,15 @@ static void dce_virtual_stop_mc_access(struct amdgpu_device *adev,
 			      struct amdgpu_mode_mc_save *save)
 {
 	switch (adev->asic_type) {
+#ifdef CONFIG_DRM_AMDGPU_CIK
 	case CHIP_BONAIRE:
 	case CHIP_HAWAII:
 	case CHIP_KAVERI:
 	case CHIP_KABINI:
 	case CHIP_MULLINS:
-#ifdef CONFIG_DRM_AMDGPU_CIK
 		dce_v8_0_disable_dce(adev);
-#endif
 		break;
+#endif
 	case CHIP_FIJI:
 	case CHIP_TONGA:
 		dce_v10_0_disable_dce(adev);
diff --git a/drivers/gpu/drm/amd/amdgpu/gfx_v8_0.c b/drivers/gpu/drm/amd/amdgpu/gfx_v8_0.c
index 6c6ff57b1c95..ee6a48a09214 100644
--- a/drivers/gpu/drm/amd/amdgpu/gfx_v8_0.c
+++ b/drivers/gpu/drm/amd/amdgpu/gfx_v8_0.c
@@ -4087,14 +4087,21 @@ static int gfx_v8_0_rlc_load_microcode(struct amdgpu_device *adev)
 static int gfx_v8_0_rlc_resume(struct amdgpu_device *adev)
 {
 	int r;
+	u32 tmp;
 
 	gfx_v8_0_rlc_stop(adev);
 
 	/* disable CG */
-	WREG32(mmRLC_CGCG_CGLS_CTRL, 0);
+	tmp = RREG32(mmRLC_CGCG_CGLS_CTRL);
+	tmp &= ~(RLC_CGCG_CGLS_CTRL__CGCG_EN_MASK |
+		 RLC_CGCG_CGLS_CTRL__CGLS_EN_MASK);
+	WREG32(mmRLC_CGCG_CGLS_CTRL, tmp);
 	if (adev->asic_type == CHIP_POLARIS11 ||
-	    adev->asic_type == CHIP_POLARIS10)
-		WREG32(mmRLC_CGCG_CGLS_CTRL_3D, 0);
+	    adev->asic_type == CHIP_POLARIS10) {
+		tmp = RREG32(mmRLC_CGCG_CGLS_CTRL_3D);
+		tmp &= ~0x3;
+		WREG32(mmRLC_CGCG_CGLS_CTRL_3D, tmp);
+	}
 
 	/* disable PG */
 	WREG32(mmRLC_PG_CNTL, 0);
@@ -5137,7 +5144,7 @@ static int gfx_v8_0_wait_for_idle(void *handle)
 	return -ETIMEDOUT;
 }
 
-static int gfx_v8_0_check_soft_reset(void *handle)
+static bool gfx_v8_0_check_soft_reset(void *handle)
 {
 	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
 	u32 grbm_soft_reset = 0, srbm_soft_reset = 0;
@@ -5189,16 +5196,14 @@ static int gfx_v8_0_check_soft_reset(void *handle)
 						SRBM_SOFT_RESET, SOFT_RESET_SEM, 1);
 
 	if (grbm_soft_reset || srbm_soft_reset) {
-		adev->ip_block_status[AMD_IP_BLOCK_TYPE_GFX].hang = true;
 		adev->gfx.grbm_soft_reset = grbm_soft_reset;
 		adev->gfx.srbm_soft_reset = srbm_soft_reset;
+		return true;
 	} else {
-		adev->ip_block_status[AMD_IP_BLOCK_TYPE_GFX].hang = false;
 		adev->gfx.grbm_soft_reset = 0;
 		adev->gfx.srbm_soft_reset = 0;
+		return false;
 	}
-
-	return 0;
 }
 
 static void gfx_v8_0_inactive_hqd(struct amdgpu_device *adev,
@@ -5226,7 +5231,8 @@ static int gfx_v8_0_pre_soft_reset(void *handle)
 	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
 	u32 grbm_soft_reset = 0, srbm_soft_reset = 0;
 
-	if (!adev->ip_block_status[AMD_IP_BLOCK_TYPE_GFX].hang)
+	if ((!adev->gfx.grbm_soft_reset) &&
+	    (!adev->gfx.srbm_soft_reset))
 		return 0;
 
 	grbm_soft_reset = adev->gfx.grbm_soft_reset;
@@ -5264,7 +5270,8 @@ static int gfx_v8_0_soft_reset(void *handle)
 	u32 grbm_soft_reset = 0, srbm_soft_reset = 0;
 	u32 tmp;
 
-	if (!adev->ip_block_status[AMD_IP_BLOCK_TYPE_GFX].hang)
+	if ((!adev->gfx.grbm_soft_reset) &&
+	    (!adev->gfx.srbm_soft_reset))
 		return 0;
 
 	grbm_soft_reset = adev->gfx.grbm_soft_reset;
@@ -5334,7 +5341,8 @@ static int gfx_v8_0_post_soft_reset(void *handle)
 	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
 	u32 grbm_soft_reset = 0, srbm_soft_reset = 0;
 
-	if (!adev->ip_block_status[AMD_IP_BLOCK_TYPE_GFX].hang)
+	if ((!adev->gfx.grbm_soft_reset) &&
+	    (!adev->gfx.srbm_soft_reset))
 		return 0;
 
 	grbm_soft_reset = adev->gfx.grbm_soft_reset;
diff --git a/drivers/gpu/drm/amd/amdgpu/gmc_v8_0.c b/drivers/gpu/drm/amd/amdgpu/gmc_v8_0.c
index 1b319f5bc696..c22ef140a542 100644
--- a/drivers/gpu/drm/amd/amdgpu/gmc_v8_0.c
+++ b/drivers/gpu/drm/amd/amdgpu/gmc_v8_0.c
@@ -1099,7 +1099,7 @@ static int gmc_v8_0_wait_for_idle(void *handle)
 
 }
 
-static int gmc_v8_0_check_soft_reset(void *handle)
+static bool gmc_v8_0_check_soft_reset(void *handle)
 {
 	u32 srbm_soft_reset = 0;
 	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
@@ -1116,20 +1116,19 @@ static int gmc_v8_0_check_soft_reset(void *handle)
 							SRBM_SOFT_RESET, SOFT_RESET_MC, 1);
 	}
 	if (srbm_soft_reset) {
-		adev->ip_block_status[AMD_IP_BLOCK_TYPE_GMC].hang = true;
 		adev->mc.srbm_soft_reset = srbm_soft_reset;
+		return true;
 	} else {
-		adev->ip_block_status[AMD_IP_BLOCK_TYPE_GMC].hang = false;
 		adev->mc.srbm_soft_reset = 0;
+		return false;
 	}
-	return 0;
 }
 
 static int gmc_v8_0_pre_soft_reset(void *handle)
 {
 	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
 
-	if (!adev->ip_block_status[AMD_IP_BLOCK_TYPE_GMC].hang)
+	if (!adev->mc.srbm_soft_reset)
 		return 0;
 
 	gmc_v8_0_mc_stop(adev, &adev->mc.save);
@@ -1145,7 +1144,7 @@ static int gmc_v8_0_soft_reset(void *handle)
 	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
 	u32 srbm_soft_reset;
 
-	if (!adev->ip_block_status[AMD_IP_BLOCK_TYPE_GMC].hang)
+	if (!adev->mc.srbm_soft_reset)
 		return 0;
 	srbm_soft_reset = adev->mc.srbm_soft_reset;
 
@@ -1175,7 +1174,7 @@ static int gmc_v8_0_post_soft_reset(void *handle)
 {
 	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
 
-	if (!adev->ip_block_status[AMD_IP_BLOCK_TYPE_GMC].hang)
+	if (!adev->mc.srbm_soft_reset)
 		return 0;
 
 	gmc_v8_0_mc_resume(adev, &adev->mc.save);
diff --git a/drivers/gpu/drm/amd/amdgpu/sdma_v3_0.c b/drivers/gpu/drm/amd/amdgpu/sdma_v3_0.c
index f325fd86430b..a9d10941fb53 100644
--- a/drivers/gpu/drm/amd/amdgpu/sdma_v3_0.c
+++ b/drivers/gpu/drm/amd/amdgpu/sdma_v3_0.c
@@ -1268,7 +1268,7 @@ static int sdma_v3_0_wait_for_idle(void *handle)
 	return -ETIMEDOUT;
 }
 
-static int sdma_v3_0_check_soft_reset(void *handle)
+static bool sdma_v3_0_check_soft_reset(void *handle)
 {
 	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
 	u32 srbm_soft_reset = 0;
@@ -1281,14 +1281,12 @@ static int sdma_v3_0_check_soft_reset(void *handle)
 	}
 
 	if (srbm_soft_reset) {
-		adev->ip_block_status[AMD_IP_BLOCK_TYPE_SDMA].hang = true;
 		adev->sdma.srbm_soft_reset = srbm_soft_reset;
+		return true;
 	} else {
-		adev->ip_block_status[AMD_IP_BLOCK_TYPE_SDMA].hang = false;
 		adev->sdma.srbm_soft_reset = 0;
+		return false;
 	}
-
-	return 0;
 }
 
 static int sdma_v3_0_pre_soft_reset(void *handle)
@@ -1296,7 +1294,7 @@ static int sdma_v3_0_pre_soft_reset(void *handle)
 	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
 	u32 srbm_soft_reset = 0;
 
-	if (!adev->ip_block_status[AMD_IP_BLOCK_TYPE_SDMA].hang)
+	if (!adev->sdma.srbm_soft_reset)
 		return 0;
 
 	srbm_soft_reset = adev->sdma.srbm_soft_reset;
@@ -1315,7 +1313,7 @@ static int sdma_v3_0_post_soft_reset(void *handle)
 	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
 	u32 srbm_soft_reset = 0;
 
-	if (!adev->ip_block_status[AMD_IP_BLOCK_TYPE_SDMA].hang)
+	if (!adev->sdma.srbm_soft_reset)
 		return 0;
 
 	srbm_soft_reset = adev->sdma.srbm_soft_reset;
@@ -1335,7 +1333,7 @@ static int sdma_v3_0_soft_reset(void *handle)
 	u32 srbm_soft_reset = 0;
 	u32 tmp;
 
-	if (!adev->ip_block_status[AMD_IP_BLOCK_TYPE_SDMA].hang)
+	if (!adev->sdma.srbm_soft_reset)
 		return 0;
 
 	srbm_soft_reset = adev->sdma.srbm_soft_reset;
diff --git a/drivers/gpu/drm/amd/amdgpu/si_dpm.c b/drivers/gpu/drm/amd/amdgpu/si_dpm.c
index 8bd08925b370..3de7bca5854b 100644
--- a/drivers/gpu/drm/amd/amdgpu/si_dpm.c
+++ b/drivers/gpu/drm/amd/amdgpu/si_dpm.c
@@ -3499,6 +3499,12 @@ static void si_apply_state_adjust_rules(struct amdgpu_device *adev,
 		max_sclk = 75000;
 		max_mclk = 80000;
 	}
+	/* Limit clocks for some HD8600 parts */
+	if (adev->pdev->device == 0x6660 &&
+	    adev->pdev->revision == 0x83) {
+		max_sclk = 75000;
+		max_mclk = 80000;
+	}
 
 	if (rps->vce_active) {
 		rps->evclk = adev->pm.dpm.vce_states[adev->pm.dpm.vce_level].evclk;
diff --git a/drivers/gpu/drm/amd/amdgpu/tonga_ih.c b/drivers/gpu/drm/amd/amdgpu/tonga_ih.c
index d127d59f953a..b4ea229bb449 100644
--- a/drivers/gpu/drm/amd/amdgpu/tonga_ih.c
+++ b/drivers/gpu/drm/amd/amdgpu/tonga_ih.c
@@ -373,7 +373,7 @@ static int tonga_ih_wait_for_idle(void *handle)
 	return -ETIMEDOUT;
 }
 
-static int tonga_ih_check_soft_reset(void *handle)
+static bool tonga_ih_check_soft_reset(void *handle)
 {
 	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
 	u32 srbm_soft_reset = 0;
@@ -384,21 +384,19 @@ static int tonga_ih_check_soft_reset(void *handle)
 						SOFT_RESET_IH, 1);
 
 	if (srbm_soft_reset) {
-		adev->ip_block_status[AMD_IP_BLOCK_TYPE_IH].hang = true;
 		adev->irq.srbm_soft_reset = srbm_soft_reset;
+		return true;
 	} else {
-		adev->ip_block_status[AMD_IP_BLOCK_TYPE_IH].hang = false;
 		adev->irq.srbm_soft_reset = 0;
+		return false;
 	}
-
-	return 0;
 }
 
 static int tonga_ih_pre_soft_reset(void *handle)
 {
 	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
 
-	if (!adev->ip_block_status[AMD_IP_BLOCK_TYPE_IH].hang)
+	if (!adev->irq.srbm_soft_reset)
 		return 0;
 
 	return tonga_ih_hw_fini(adev);
@@ -408,7 +406,7 @@ static int tonga_ih_post_soft_reset(void *handle)
 {
 	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
 
-	if (!adev->ip_block_status[AMD_IP_BLOCK_TYPE_IH].hang)
+	if (!adev->irq.srbm_soft_reset)
 		return 0;
 
 	return tonga_ih_hw_init(adev);
@@ -419,7 +417,7 @@ static int tonga_ih_soft_reset(void *handle)
 	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
 	u32 srbm_soft_reset;
 
-	if (!adev->ip_block_status[AMD_IP_BLOCK_TYPE_IH].hang)
+	if (!adev->irq.srbm_soft_reset)
 		return 0;
 	srbm_soft_reset = adev->irq.srbm_soft_reset;
 
diff --git a/drivers/gpu/drm/amd/amdgpu/uvd_v6_0.c b/drivers/gpu/drm/amd/amdgpu/uvd_v6_0.c
index e0fd9f21ed95..ab3df6d75656 100644
--- a/drivers/gpu/drm/amd/amdgpu/uvd_v6_0.c
+++ b/drivers/gpu/drm/amd/amdgpu/uvd_v6_0.c
@@ -770,7 +770,7 @@ static int uvd_v6_0_wait_for_idle(void *handle)
 }
 
 #define AMDGPU_UVD_STATUS_BUSY_MASK    0xfd
-static int uvd_v6_0_check_soft_reset(void *handle)
+static bool uvd_v6_0_check_soft_reset(void *handle)
 {
 	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
 	u32 srbm_soft_reset = 0;
@@ -782,19 +782,19 @@ static int uvd_v6_0_check_soft_reset(void *handle)
 		srbm_soft_reset = REG_SET_FIELD(srbm_soft_reset, SRBM_SOFT_RESET, SOFT_RESET_UVD, 1);
 
 	if (srbm_soft_reset) {
-		adev->ip_block_status[AMD_IP_BLOCK_TYPE_UVD].hang = true;
 		adev->uvd.srbm_soft_reset = srbm_soft_reset;
+		return true;
 	} else {
-		adev->ip_block_status[AMD_IP_BLOCK_TYPE_UVD].hang = false;
 		adev->uvd.srbm_soft_reset = 0;
+		return false;
 	}
-	return 0;
 }
+
 static int uvd_v6_0_pre_soft_reset(void *handle)
 {
 	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
 
-	if (!adev->ip_block_status[AMD_IP_BLOCK_TYPE_UVD].hang)
+	if (!adev->uvd.srbm_soft_reset)
 		return 0;
 
 	uvd_v6_0_stop(adev);
@@ -806,7 +806,7 @@ static int uvd_v6_0_soft_reset(void *handle)
 	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
 	u32 srbm_soft_reset;
 
-	if (!adev->ip_block_status[AMD_IP_BLOCK_TYPE_UVD].hang)
+	if (!adev->uvd.srbm_soft_reset)
 		return 0;
 	srbm_soft_reset = adev->uvd.srbm_soft_reset;
 
@@ -836,7 +836,7 @@ static int uvd_v6_0_post_soft_reset(void *handle)
 {
 	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
 
-	if (!adev->ip_block_status[AMD_IP_BLOCK_TYPE_UVD].hang)
+	if (!adev->uvd.srbm_soft_reset)
 		return 0;
 
 	mdelay(5);
diff --git a/drivers/gpu/drm/amd/amdgpu/vce_v3_0.c b/drivers/gpu/drm/amd/amdgpu/vce_v3_0.c
index 3f6db4ec0102..8533269ec160 100644
--- a/drivers/gpu/drm/amd/amdgpu/vce_v3_0.c
+++ b/drivers/gpu/drm/amd/amdgpu/vce_v3_0.c
@@ -561,7 +561,7 @@ static int vce_v3_0_wait_for_idle(void *handle)
 #define  AMDGPU_VCE_STATUS_BUSY_MASK (VCE_STATUS_VCPU_REPORT_AUTO_BUSY_MASK | \
 				      VCE_STATUS_VCPU_REPORT_RB0_BUSY_MASK)
 
-static int vce_v3_0_check_soft_reset(void *handle)
+static bool vce_v3_0_check_soft_reset(void *handle)
 {
 	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
 	u32 srbm_soft_reset = 0;
@@ -591,16 +591,15 @@ static int vce_v3_0_check_soft_reset(void *handle)
 		srbm_soft_reset = REG_SET_FIELD(srbm_soft_reset, SRBM_SOFT_RESET, SOFT_RESET_VCE1, 1);
 	}
 	WREG32_FIELD(GRBM_GFX_INDEX, INSTANCE_INDEX, 0);
+	mutex_unlock(&adev->grbm_idx_mutex);
 
 	if (srbm_soft_reset) {
-		adev->ip_block_status[AMD_IP_BLOCK_TYPE_VCE].hang = true;
 		adev->vce.srbm_soft_reset = srbm_soft_reset;
+		return true;
 	} else {
-		adev->ip_block_status[AMD_IP_BLOCK_TYPE_VCE].hang = false;
 		adev->vce.srbm_soft_reset = 0;
+		return false;
 	}
-	mutex_unlock(&adev->grbm_idx_mutex);
-	return 0;
 }
 
 static int vce_v3_0_soft_reset(void *handle)
@@ -608,7 +607,7 @@ static int vce_v3_0_soft_reset(void *handle)
 	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
 	u32 srbm_soft_reset;
 
-	if (!adev->ip_block_status[AMD_IP_BLOCK_TYPE_VCE].hang)
+	if (!adev->vce.srbm_soft_reset)
 		return 0;
 	srbm_soft_reset = adev->vce.srbm_soft_reset;
 
@@ -638,7 +637,7 @@ static int vce_v3_0_pre_soft_reset(void *handle)
 {
 	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
 
-	if (!adev->ip_block_status[AMD_IP_BLOCK_TYPE_VCE].hang)
+	if (!adev->vce.srbm_soft_reset)
 		return 0;
 
 	mdelay(5);
@@ -651,7 +650,7 @@ static int vce_v3_0_post_soft_reset(void *handle)
 {
 	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
 
-	if (!adev->ip_block_status[AMD_IP_BLOCK_TYPE_VCE].hang)
+	if (!adev->vce.srbm_soft_reset)
 		return 0;
 
 	mdelay(5);
diff --git a/drivers/gpu/drm/amd/include/amd_shared.h b/drivers/gpu/drm/amd/include/amd_shared.h
index c934b78c9e2f..bec8125bceb0 100644
--- a/drivers/gpu/drm/amd/include/amd_shared.h
+++ b/drivers/gpu/drm/amd/include/amd_shared.h
@@ -165,7 +165,7 @@ struct amd_ip_funcs {
 	/* poll for idle */
 	int (*wait_for_idle)(void *handle);
 	/* check soft reset the IP block */
-	int (*check_soft_reset)(void *handle);
+	bool (*check_soft_reset)(void *handle);
 	/* pre soft reset the IP block */
 	int (*pre_soft_reset)(void *handle);
 	/* soft reset the IP block */
diff --git a/drivers/gpu/drm/amd/powerplay/Kconfig b/drivers/gpu/drm/amd/powerplay/Kconfig
deleted file mode 100644
index af380335b425..000000000000
--- a/drivers/gpu/drm/amd/powerplay/Kconfig
+++ /dev/null
@@ -1,6 +0,0 @@
-config DRM_AMD_POWERPLAY
-	bool  "Enable AMD powerplay component"
-	depends on DRM_AMDGPU
-	default n
-	help
-	  select this option will enable AMD powerplay component.
diff --git a/drivers/gpu/drm/amd/powerplay/eventmgr/eventactionchains.c b/drivers/gpu/drm/amd/powerplay/eventmgr/eventactionchains.c
index 92b117843875..8cee4e0f9fde 100644
--- a/drivers/gpu/drm/amd/powerplay/eventmgr/eventactionchains.c
+++ b/drivers/gpu/drm/amd/powerplay/eventmgr/eventactionchains.c
@@ -49,6 +49,7 @@ static const pem_event_action * const uninitialize_event[] = {
 	uninitialize_display_phy_access_tasks,
 	disable_gfx_voltage_island_power_gating_tasks,
 	disable_gfx_clock_gating_tasks,
+	uninitialize_thermal_controller_tasks,
 	set_boot_state_tasks,
 	adjust_power_state_tasks,
 	disable_dynamic_state_management_tasks,
diff --git a/drivers/gpu/drm/amd/powerplay/hwmgr/cz_hwmgr.c b/drivers/gpu/drm/amd/powerplay/hwmgr/cz_hwmgr.c
index 7e4fcbbbe086..960424913496 100644
--- a/drivers/gpu/drm/amd/powerplay/hwmgr/cz_hwmgr.c
+++ b/drivers/gpu/drm/amd/powerplay/hwmgr/cz_hwmgr.c
@@ -1785,6 +1785,21 @@ static int cz_get_max_high_clocks(struct pp_hwmgr *hwmgr, struct amd_pp_simple_c
 	return 0;
 }
 
+static int cz_thermal_get_temperature(struct pp_hwmgr *hwmgr)
+{
+	int actual_temp = 0;
+	uint32_t val = cgs_read_ind_register(hwmgr->device,
+					     CGS_IND_REG__SMC, ixTHM_TCON_CUR_TMP);
+	uint32_t temp = PHM_GET_FIELD(val, THM_TCON_CUR_TMP, CUR_TEMP);
+
+	if (PHM_GET_FIELD(val, THM_TCON_CUR_TMP, CUR_TEMP_RANGE_SEL))
+		actual_temp = ((temp / 8) - 49) * PP_TEMPERATURE_UNITS_PER_CENTIGRADES;
+	else
+		actual_temp = (temp / 8) * PP_TEMPERATURE_UNITS_PER_CENTIGRADES;
+
+	return actual_temp;
+}
+
 static int cz_read_sensor(struct pp_hwmgr *hwmgr, int idx, int32_t *value)
 {
 	struct cz_hwmgr *cz_hwmgr = (struct cz_hwmgr *)(hwmgr->backend);
@@ -1881,6 +1896,9 @@ static int cz_read_sensor(struct pp_hwmgr *hwmgr, int idx, int32_t *value)
 	case AMDGPU_PP_SENSOR_VCE_POWER:
 		*value = cz_hwmgr->vce_power_gated ? 0 : 1;
 		return 0;
+	case AMDGPU_PP_SENSOR_GPU_TEMP:
+		*value = cz_thermal_get_temperature(hwmgr);
+		return 0;
 	default:
 		return -EINVAL;
 	}
diff --git a/drivers/gpu/drm/amd/powerplay/hwmgr/smu7_hwmgr.c b/drivers/gpu/drm/amd/powerplay/hwmgr/smu7_hwmgr.c
index 508245d49d33..609996c84ad5 100644
--- a/drivers/gpu/drm/amd/powerplay/hwmgr/smu7_hwmgr.c
+++ b/drivers/gpu/drm/amd/powerplay/hwmgr/smu7_hwmgr.c
@@ -1030,20 +1030,19 @@ static int smu7_disable_sclk_mclk_dpm(struct pp_hwmgr *hwmgr)
 	struct smu7_hwmgr *data = (struct smu7_hwmgr *)(hwmgr->backend);
 
 	/* disable SCLK dpm */
-	if (!data->sclk_dpm_key_disabled)
-		PP_ASSERT_WITH_CODE(
-				(smum_send_msg_to_smc(hwmgr->smumgr,
-						PPSMC_MSG_DPM_Disable) == 0),
-				"Failed to disable SCLK DPM!",
-				return -EINVAL);
+	if (!data->sclk_dpm_key_disabled) {
+		PP_ASSERT_WITH_CODE(true == smum_is_dpm_running(hwmgr),
+				"Trying to disable SCLK DPM when DPM is disabled",
+				return 0);
+		smum_send_msg_to_smc(hwmgr->smumgr, PPSMC_MSG_DPM_Disable);
+	}
 
 	/* disable MCLK dpm */
 	if (!data->mclk_dpm_key_disabled) {
-		PP_ASSERT_WITH_CODE(
-				(smum_send_msg_to_smc(hwmgr->smumgr,
-						PPSMC_MSG_MCLKDPM_Disable) == 0),
-				"Failed to disable MCLK DPM!",
-				return -EINVAL);
+		PP_ASSERT_WITH_CODE(true == smum_is_dpm_running(hwmgr),
+				"Trying to disable MCLK DPM when DPM is disabled",
+				return 0);
+		smum_send_msg_to_smc(hwmgr->smumgr, PPSMC_MSG_MCLKDPM_Disable);
 	}
 
 	return 0;
@@ -1069,10 +1068,13 @@ static int smu7_stop_dpm(struct pp_hwmgr *hwmgr)
 				return -EINVAL);
 	}
 
-	if (smu7_disable_sclk_mclk_dpm(hwmgr)) {
-		printk(KERN_ERR "Failed to disable Sclk DPM and Mclk DPM!");
-		return -EINVAL;
-	}
+	smu7_disable_sclk_mclk_dpm(hwmgr);
+
+	PP_ASSERT_WITH_CODE(true == smum_is_dpm_running(hwmgr),
+			"Trying to disable voltage DPM when DPM is disabled",
+			return 0);
+
+	smum_send_msg_to_smc(hwmgr->smumgr, PPSMC_MSG_Voltage_Cntl_Disable);
 
 	return 0;
 }
@@ -1226,7 +1228,7 @@ int smu7_enable_dpm_tasks(struct pp_hwmgr *hwmgr)
 	PP_ASSERT_WITH_CODE((0 == tmp_result),
 			"Failed to enable VR hot GPIO interrupt!", result = tmp_result);
 
-	smum_send_msg_to_smc(hwmgr->smumgr, (PPSMC_Msg)PPSMC_HasDisplay);
+	smum_send_msg_to_smc(hwmgr->smumgr, (PPSMC_Msg)PPSMC_NoDisplay);
 
 	tmp_result = smu7_enable_sclk_control(hwmgr);
 	PP_ASSERT_WITH_CODE((0 == tmp_result),
@@ -1306,6 +1308,12 @@ int smu7_disable_dpm_tasks(struct pp_hwmgr *hwmgr)
 	PP_ASSERT_WITH_CODE((tmp_result == 0),
 			"Failed to disable thermal auto throttle!", result = tmp_result);
 
+	if (1 == PHM_READ_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC, FEATURE_STATUS, AVS_ON)) {
+		PP_ASSERT_WITH_CODE((0 == smum_send_msg_to_smc(hwmgr->smumgr, PPSMC_MSG_DisableAvfs)),
+					"Failed to disable AVFS!",
+					return -EINVAL);
+	}
+
 	tmp_result = smu7_stop_dpm(hwmgr);
 	PP_ASSERT_WITH_CODE((tmp_result == 0),
 			"Failed to stop DPM!", result = tmp_result);
@@ -1452,8 +1460,10 @@ static int smu7_get_evv_voltages(struct pp_hwmgr *hwmgr)
 	struct phm_ppt_v1_clock_voltage_dependency_table *sclk_table = NULL;
 
 
-	if (table_info != NULL)
-		sclk_table = table_info->vdd_dep_on_sclk;
+	if (table_info == NULL)
+		return -EINVAL;
+
+	sclk_table = table_info->vdd_dep_on_sclk;
 
 	for (i = 0; i < SMU7_MAX_LEAKAGE_COUNT; i++) {
 		vv_id = ATOM_VIRTUAL_VOLTAGE_ID0 + i;
@@ -3802,13 +3812,15 @@ static inline bool smu7_are_power_levels_equal(const struct smu7_performance_lev
 
 int smu7_check_states_equal(struct pp_hwmgr *hwmgr, const struct pp_hw_power_state *pstate1, const struct pp_hw_power_state *pstate2, bool *equal)
 {
-	const struct smu7_power_state *psa = cast_const_phw_smu7_power_state(pstate1);
-	const struct smu7_power_state *psb = cast_const_phw_smu7_power_state(pstate2);
+	const struct smu7_power_state *psa;
+	const struct smu7_power_state *psb;
 	int i;
 
 	if (pstate1 == NULL || pstate2 == NULL || equal == NULL)
 		return -EINVAL;
 
+	psa = cast_const_phw_smu7_power_state(pstate1);
+	psb = cast_const_phw_smu7_power_state(pstate2);
 	/* If the two states don't even have the same number of performance levels they cannot be the same state. */
 	if (psa->performance_level_count != psb->performance_level_count) {
 		*equal = false;
@@ -4324,6 +4336,7 @@ static const struct pp_hwmgr_func smu7_hwmgr_funcs = {
 	.set_mclk_od = smu7_set_mclk_od,
 	.get_clock_by_type = smu7_get_clock_by_type,
 	.read_sensor = smu7_read_sensor,
+	.dynamic_state_management_disable = smu7_disable_dpm_tasks,
 };
 
 uint8_t smu7_get_sleep_divider_id_from_clock(uint32_t clock,
diff --git a/drivers/gpu/drm/amd/powerplay/smumgr/iceland_smc.c b/drivers/gpu/drm/amd/powerplay/smumgr/iceland_smc.c
index eda802bc63c8..8c889caba420 100644
--- a/drivers/gpu/drm/amd/powerplay/smumgr/iceland_smc.c
+++ b/drivers/gpu/drm/amd/powerplay/smumgr/iceland_smc.c
@@ -2458,7 +2458,7 @@ static int iceland_set_mc_special_registers(struct pp_hwmgr *hwmgr,
 			PP_ASSERT_WITH_CODE((j <= SMU71_DISCRETE_MC_REGISTER_ARRAY_SIZE),
 				"Invalid VramInfo table.", return -EINVAL);
 
-			if (!data->is_memory_gddr5) {
+			if (!data->is_memory_gddr5 && j < SMU71_DISCRETE_MC_REGISTER_ARRAY_SIZE) {
 				table->mc_reg_address[j].s1 = mmMC_PMG_AUTO_CMD;
 				table->mc_reg_address[j].s0 = mmMC_PMG_AUTO_CMD;
 				for (k = 0; k < table->num_entries; k++) {
diff --git a/drivers/gpu/drm/armada/armada_crtc.c b/drivers/gpu/drm/armada/armada_crtc.c
index 2f58e9e2a59c..a51f8cbcfe26 100644
--- a/drivers/gpu/drm/armada/armada_crtc.c
+++ b/drivers/gpu/drm/armada/armada_crtc.c
@@ -332,17 +332,19 @@ static void armada_drm_crtc_dpms(struct drm_crtc *crtc, int dpms)
 {
 	struct armada_crtc *dcrtc = drm_to_armada_crtc(crtc);
 
-	if (dcrtc->dpms != dpms) {
-		dcrtc->dpms = dpms;
-		if (!IS_ERR(dcrtc->clk) && !dpms_blanked(dpms))
-			WARN_ON(clk_prepare_enable(dcrtc->clk));
-		armada_drm_crtc_update(dcrtc);
-		if (!IS_ERR(dcrtc->clk) && dpms_blanked(dpms))
-			clk_disable_unprepare(dcrtc->clk);
+	if (dpms_blanked(dcrtc->dpms) != dpms_blanked(dpms)) {
 		if (dpms_blanked(dpms))
 			armada_drm_vblank_off(dcrtc);
-		else
+		else if (!IS_ERR(dcrtc->clk))
+			WARN_ON(clk_prepare_enable(dcrtc->clk));
+		dcrtc->dpms = dpms;
+		armada_drm_crtc_update(dcrtc);
+		if (!dpms_blanked(dpms))
 			drm_crtc_vblank_on(&dcrtc->crtc);
+		else if (!IS_ERR(dcrtc->clk))
+			clk_disable_unprepare(dcrtc->clk);
+	} else if (dcrtc->dpms != dpms) {
+		dcrtc->dpms = dpms;
 	}
 }
 
diff --git a/drivers/gpu/drm/armada/armada_gem.c b/drivers/gpu/drm/armada/armada_gem.c
index a5e428d27d2f..806791897304 100644
--- a/drivers/gpu/drm/armada/armada_gem.c
+++ b/drivers/gpu/drm/armada/armada_gem.c
@@ -387,7 +387,7 @@ int armada_gem_pwrite_ioctl(struct drm_device *dev, void *data,
 	if (!access_ok(VERIFY_READ, ptr, args->size))
 		return -EFAULT;
 
-	ret = fault_in_multipages_readable(ptr, args->size);
+	ret = fault_in_pages_readable(ptr, args->size);
 	if (ret)
 		return ret;
 
diff --git a/drivers/gpu/drm/drm_debugfs.c b/drivers/gpu/drm/drm_debugfs.c
index 8ea72d821525..800055c39cdb 100644
--- a/drivers/gpu/drm/drm_debugfs.c
+++ b/drivers/gpu/drm/drm_debugfs.c
@@ -104,8 +104,8 @@ int drm_debugfs_create_files(const struct drm_info_list *files, int count,
 		ent = debugfs_create_file(files[i].name, S_IFREG | S_IRUGO,
 					  root, tmp, &drm_debugfs_fops);
 		if (!ent) {
-			DRM_ERROR("Cannot create /sys/kernel/debug/dri/%s/%s\n",
-				  root->d_name.name, files[i].name);
+			DRM_ERROR("Cannot create /sys/kernel/debug/dri/%pd/%s\n",
+				  root, files[i].name);
 			kfree(tmp);
 			ret = -1;
 			goto fail;
diff --git a/drivers/gpu/drm/drm_dp_dual_mode_helper.c b/drivers/gpu/drm/drm_dp_dual_mode_helper.c
index a7b2a751f6fe..488355bdafb9 100644
--- a/drivers/gpu/drm/drm_dp_dual_mode_helper.c
+++ b/drivers/gpu/drm/drm_dp_dual_mode_helper.c
@@ -148,6 +148,14 @@ static bool is_type2_adaptor(uint8_t adaptor_id)
 			      DP_DUAL_MODE_REV_TYPE2);
 }
 
+static bool is_lspcon_adaptor(const char hdmi_id[DP_DUAL_MODE_HDMI_ID_LEN],
+			      const uint8_t adaptor_id)
+{
+	return is_hdmi_adaptor(hdmi_id) &&
+		(adaptor_id == (DP_DUAL_MODE_TYPE_TYPE2 |
+		 DP_DUAL_MODE_TYPE_HAS_DPCD));
+}
+
 /**
  * drm_dp_dual_mode_detect - Identify the DP dual mode adaptor
  * @adapter: I2C adapter for the DDC bus
@@ -203,6 +211,8 @@ enum drm_dp_dual_mode_type drm_dp_dual_mode_detect(struct i2c_adapter *adapter)
 	ret = drm_dp_dual_mode_read(adapter, DP_DUAL_MODE_ADAPTOR_ID,
 				    &adaptor_id, sizeof(adaptor_id));
 	if (ret == 0) {
+		if (is_lspcon_adaptor(hdmi_id, adaptor_id))
+			return DRM_DP_DUAL_MODE_LSPCON;
 		if (is_type2_adaptor(adaptor_id)) {
 			if (is_hdmi_adaptor(hdmi_id))
 				return DRM_DP_DUAL_MODE_TYPE2_HDMI;
@@ -364,3 +374,96 @@ const char *drm_dp_get_dual_mode_type_name(enum drm_dp_dual_mode_type type)
 	}
 }
 EXPORT_SYMBOL(drm_dp_get_dual_mode_type_name);
+
+/**
+ * drm_lspcon_get_mode: Get LSPCON's current mode of operation by
+ * reading offset (0x80, 0x41)
+ * @adapter: I2C-over-aux adapter
+ * @mode: current lspcon mode of operation output variable
+ *
+ * Returns:
+ * 0 on success, sets the current_mode value to appropriate mode
+ * -error on failure
+ */
+int drm_lspcon_get_mode(struct i2c_adapter *adapter,
+			enum drm_lspcon_mode *mode)
+{
+	u8 data;
+	int ret = 0;
+
+	if (!mode) {
+		DRM_ERROR("NULL input\n");
+		return -EINVAL;
+	}
+
+	/* Read Status: i2c over aux */
+	ret = drm_dp_dual_mode_read(adapter, DP_DUAL_MODE_LSPCON_CURRENT_MODE,
+				    &data, sizeof(data));
+	if (ret < 0) {
+		DRM_ERROR("LSPCON read(0x80, 0x41) failed\n");
+		return -EFAULT;
+	}
+
+	if (data & DP_DUAL_MODE_LSPCON_MODE_PCON)
+		*mode = DRM_LSPCON_MODE_PCON;
+	else
+		*mode = DRM_LSPCON_MODE_LS;
+	return 0;
+}
+EXPORT_SYMBOL(drm_lspcon_get_mode);
+
+/**
+ * drm_lspcon_set_mode: Change LSPCON's mode of operation by
+ * writing offset (0x80, 0x40)
+ * @adapter: I2C-over-aux adapter
+ * @mode: required mode of operation
+ *
+ * Returns:
+ * 0 on success, -error on failure/timeout
+ */
+int drm_lspcon_set_mode(struct i2c_adapter *adapter,
+			enum drm_lspcon_mode mode)
+{
+	u8 data = 0;
+	int ret;
+	int time_out = 200;
+	enum drm_lspcon_mode current_mode;
+
+	if (mode == DRM_LSPCON_MODE_PCON)
+		data = DP_DUAL_MODE_LSPCON_MODE_PCON;
+
+	/* Change mode */
+	ret = drm_dp_dual_mode_write(adapter, DP_DUAL_MODE_LSPCON_MODE_CHANGE,
+				     &data, sizeof(data));
+	if (ret < 0) {
+		DRM_ERROR("LSPCON mode change failed\n");
+		return ret;
+	}
+
+	/*
+	 * Confirm mode change by reading the status bit.
+	 * Sometimes, it takes a while to change the mode,
+	 * so wait and retry until time out or done.
+	 */
+	do {
+		ret = drm_lspcon_get_mode(adapter, &current_mode);
+		if (ret) {
+			DRM_ERROR("can't confirm LSPCON mode change\n");
+			return ret;
+		} else {
+			if (current_mode != mode) {
+				msleep(10);
+				time_out -= 10;
+			} else {
+				DRM_DEBUG_KMS("LSPCON mode changed to %s\n",
+						mode == DRM_LSPCON_MODE_LS ?
+						"LS" : "PCON");
+				return 0;
+			}
+		}
+	} while (time_out);
+
+	DRM_ERROR("LSPCON mode change timed out\n");
+	return -ETIMEDOUT;
+}
+EXPORT_SYMBOL(drm_lspcon_set_mode);
diff --git a/drivers/gpu/drm/drm_dp_helper.c b/drivers/gpu/drm/drm_dp_helper.c
index ac3924a877e0..3e6fe82c6d64 100644
--- a/drivers/gpu/drm/drm_dp_helper.c
+++ b/drivers/gpu/drm/drm_dp_helper.c
@@ -975,6 +975,12 @@ static void unlock_bus(struct i2c_adapter *i2c, unsigned int flags)
 	mutex_unlock(&i2c_to_aux(i2c)->hw_mutex);
 }
 
+static const struct i2c_lock_operations drm_dp_i2c_lock_ops = {
+	.lock_bus = lock_bus,
+	.trylock_bus = trylock_bus,
+	.unlock_bus = unlock_bus,
+};
+
 /**
  * drm_dp_aux_init() - minimally initialise an aux channel
  * @aux: DisplayPort AUX channel
@@ -992,9 +998,7 @@ void drm_dp_aux_init(struct drm_dp_aux *aux)
 	aux->ddc.algo_data = aux;
 	aux->ddc.retries = 3;
 
-	aux->ddc.lock_bus = lock_bus;
-	aux->ddc.trylock_bus = trylock_bus;
-	aux->ddc.unlock_bus = unlock_bus;
+	aux->ddc.lock_ops = &drm_dp_i2c_lock_ops;
 }
 EXPORT_SYMBOL(drm_dp_aux_init);
 
diff --git a/drivers/gpu/drm/drm_info.c b/drivers/gpu/drm/drm_info.c
index 1df2d33d0b40..ffb2ab389d1d 100644
--- a/drivers/gpu/drm/drm_info.c
+++ b/drivers/gpu/drm/drm_info.c
@@ -54,9 +54,6 @@ int drm_name_info(struct seq_file *m, void *data)
 
 	mutex_lock(&dev->master_mutex);
 	master = dev->master;
-	if (!master)
-		goto out_unlock;
-
 	seq_printf(m, "%s", dev->driver->name);
 	if (dev->dev)
 		seq_printf(m, " dev=%s", dev_name(dev->dev));
@@ -65,7 +62,6 @@ int drm_name_info(struct seq_file *m, void *data)
 	if (dev->unique)
 		seq_printf(m, " unique=%s", dev->unique);
 	seq_printf(m, "\n");
-out_unlock:
 	mutex_unlock(&dev->master_mutex);
 
 	return 0;
diff --git a/drivers/gpu/drm/etnaviv/etnaviv_buffer.c b/drivers/gpu/drm/etnaviv/etnaviv_buffer.c
index cb86c7e5495c..d9230132dfbc 100644
--- a/drivers/gpu/drm/etnaviv/etnaviv_buffer.c
+++ b/drivers/gpu/drm/etnaviv/etnaviv_buffer.c
@@ -329,20 +329,34 @@ void etnaviv_buffer_queue(struct etnaviv_gpu *gpu, unsigned int event,
 	/*
 	 * Append a LINK to the submitted command buffer to return to
 	 * the ring buffer.  return_target is the ring target address.
-	 * We need three dwords: event, wait, link.
+	 * We need at most 7 dwords in the return target: 2 cache flush +
+	 * 2 semaphore stall + 1 event + 1 wait + 1 link.
 	 */
-	return_dwords = 3;
+	return_dwords = 7;
 	return_target = etnaviv_buffer_reserve(gpu, buffer, return_dwords);
 	CMD_LINK(cmdbuf, return_dwords, return_target);
 
 	/*
-	 * Append event, wait and link pointing back to the wait
-	 * command to the ring buffer.
+	 * Append a cache flush, stall, event, wait and link pointing back to
+	 * the wait command to the ring buffer.
 	 */
+	if (gpu->exec_state == ETNA_PIPE_2D) {
+		CMD_LOAD_STATE(buffer, VIVS_GL_FLUSH_CACHE,
+				       VIVS_GL_FLUSH_CACHE_PE2D);
+	} else {
+		CMD_LOAD_STATE(buffer, VIVS_GL_FLUSH_CACHE,
+				       VIVS_GL_FLUSH_CACHE_DEPTH |
+				       VIVS_GL_FLUSH_CACHE_COLOR);
+		CMD_LOAD_STATE(buffer, VIVS_TS_FLUSH_CACHE,
+				       VIVS_TS_FLUSH_CACHE_FLUSH);
+	}
+	CMD_SEM(buffer, SYNC_RECIPIENT_FE, SYNC_RECIPIENT_PE);
+	CMD_STALL(buffer, SYNC_RECIPIENT_FE, SYNC_RECIPIENT_PE);
 	CMD_LOAD_STATE(buffer, VIVS_GL_EVENT, VIVS_GL_EVENT_EVENT_ID(event) |
 		       VIVS_GL_EVENT_FROM_PE);
 	CMD_WAIT(buffer);
-	CMD_LINK(buffer, 2, return_target + 8);
+	CMD_LINK(buffer, 2, etnaviv_iommu_get_cmdbuf_va(gpu, buffer) +
+			    buffer->user_size - 4);
 
 	if (drm_debug & DRM_UT_DRIVER)
 		pr_info("stream link to 0x%08x @ 0x%08x %p\n",
diff --git a/drivers/gpu/drm/etnaviv/etnaviv_gem.c b/drivers/gpu/drm/etnaviv/etnaviv_gem.c
index 9ffca2478e02..3755ef935af4 100644
--- a/drivers/gpu/drm/etnaviv/etnaviv_gem.c
+++ b/drivers/gpu/drm/etnaviv/etnaviv_gem.c
@@ -744,19 +744,22 @@ static struct page **etnaviv_gem_userptr_do_get_pages(
 	int ret = 0, pinned, npages = etnaviv_obj->base.size >> PAGE_SHIFT;
 	struct page **pvec;
 	uintptr_t ptr;
+	unsigned int flags = 0;
 
 	pvec = drm_malloc_ab(npages, sizeof(struct page *));
 	if (!pvec)
 		return ERR_PTR(-ENOMEM);
 
+	if (!etnaviv_obj->userptr.ro)
+		flags |= FOLL_WRITE;
+
 	pinned = 0;
 	ptr = etnaviv_obj->userptr.ptr;
 
 	down_read(&mm->mmap_sem);
 	while (pinned < npages) {
 		ret = get_user_pages_remote(task, mm, ptr, npages - pinned,
-					    !etnaviv_obj->userptr.ro, 0,
-					    pvec + pinned, NULL);
+					    flags, pvec + pinned, NULL);
 		if (ret < 0)
 			break;
 
diff --git a/drivers/gpu/drm/etnaviv/etnaviv_mmu.c b/drivers/gpu/drm/etnaviv/etnaviv_mmu.c
index d3796ed8d8c5..169ac96e8f08 100644
--- a/drivers/gpu/drm/etnaviv/etnaviv_mmu.c
+++ b/drivers/gpu/drm/etnaviv/etnaviv_mmu.c
@@ -330,7 +330,8 @@ u32 etnaviv_iommu_get_cmdbuf_va(struct etnaviv_gpu *gpu,
 			return (u32)buf->vram_node.start;
 
 		mutex_lock(&mmu->lock);
-		ret = etnaviv_iommu_find_iova(mmu, &buf->vram_node, buf->size);
+		ret = etnaviv_iommu_find_iova(mmu, &buf->vram_node,
+					      buf->size + SZ_64K);
 		if (ret < 0) {
 			mutex_unlock(&mmu->lock);
 			return 0;
diff --git a/drivers/gpu/drm/exynos/Kconfig b/drivers/gpu/drm/exynos/Kconfig
index 83f61c513b7e..465d344f3391 100644
--- a/drivers/gpu/drm/exynos/Kconfig
+++ b/drivers/gpu/drm/exynos/Kconfig
@@ -38,7 +38,6 @@ config DRM_EXYNOS7_DECON
 
 config DRM_EXYNOS_MIXER
 	bool "Mixer"
-	depends on !VIDEO_SAMSUNG_S5P_TV
 	help
 	  Choose this option if you want to use Exynos Mixer for DRM.
 
@@ -77,7 +76,7 @@ config DRM_EXYNOS_DP
 
 config DRM_EXYNOS_HDMI
 	bool "HDMI"
-	depends on !VIDEO_SAMSUNG_S5P_TV && (DRM_EXYNOS_MIXER || DRM_EXYNOS5433_DECON)
+	depends on DRM_EXYNOS_MIXER || DRM_EXYNOS5433_DECON
 	help
 	  Choose this option if you want to use Exynos HDMI for DRM.
 
diff --git a/drivers/gpu/drm/exynos/exynos_drm_g2d.c b/drivers/gpu/drm/exynos/exynos_drm_g2d.c
index aa92decf4233..fbd13fabdf2d 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_g2d.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_g2d.c
@@ -488,7 +488,8 @@ static dma_addr_t *g2d_userptr_get_dma_addr(struct drm_device *drm_dev,
 		goto err_free;
 	}
 
-	ret = get_vaddr_frames(start, npages, true, true, g2d_userptr->vec);
+	ret = get_vaddr_frames(start, npages, FOLL_FORCE | FOLL_WRITE,
+		g2d_userptr->vec);
 	if (ret != npages) {
 		DRM_ERROR("failed to get user pages from userptr.\n");
 		if (ret < 0)
diff --git a/drivers/gpu/drm/exynos/exynos_drm_iommu.h b/drivers/gpu/drm/exynos/exynos_drm_iommu.h
index c8de4913fdbe..87f6b5672e11 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_iommu.h
+++ b/drivers/gpu/drm/exynos/exynos_drm_iommu.h
@@ -66,7 +66,7 @@ static inline int __exynos_iommu_create_mapping(struct exynos_drm_private *priv,
 	if (ret)
 		goto free_domain;
 
-	ret = iommu_dma_init_domain(domain, start, size);
+	ret = iommu_dma_init_domain(domain, start, size, NULL);
 	if (ret)
 		goto put_cookie;
 
diff --git a/drivers/gpu/drm/fsl-dcu/fsl_dcu_drm_crtc.c b/drivers/gpu/drm/fsl-dcu/fsl_dcu_drm_crtc.c
index 3371635cd4d7..b2d5e188b1b8 100644
--- a/drivers/gpu/drm/fsl-dcu/fsl_dcu_drm_crtc.c
+++ b/drivers/gpu/drm/fsl-dcu/fsl_dcu_drm_crtc.c
@@ -51,6 +51,7 @@ static void fsl_dcu_drm_disable_crtc(struct drm_crtc *crtc)
 			   DCU_MODE_DCU_MODE(DCU_MODE_OFF));
 	regmap_write(fsl_dev->regmap, DCU_UPDATE_MODE,
 		     DCU_UPDATE_MODE_READREG);
+	clk_disable_unprepare(fsl_dev->pix_clk);
 }
 
 static void fsl_dcu_drm_crtc_enable(struct drm_crtc *crtc)
@@ -58,6 +59,7 @@ static void fsl_dcu_drm_crtc_enable(struct drm_crtc *crtc)
 	struct drm_device *dev = crtc->dev;
 	struct fsl_dcu_drm_device *fsl_dev = dev->dev_private;
 
+	clk_prepare_enable(fsl_dev->pix_clk);
 	regmap_update_bits(fsl_dev->regmap, DCU_DCU_MODE,
 			   DCU_MODE_DCU_MODE_MASK,
 			   DCU_MODE_DCU_MODE(DCU_MODE_NORMAL));
@@ -116,8 +118,6 @@ static void fsl_dcu_drm_crtc_mode_set_nofb(struct drm_crtc *crtc)
 		     DCU_THRESHOLD_LS_BF_VS(BF_VS_VAL) |
 		     DCU_THRESHOLD_OUT_BUF_HIGH(BUF_MAX_VAL) |
 		     DCU_THRESHOLD_OUT_BUF_LOW(BUF_MIN_VAL));
-	regmap_write(fsl_dev->regmap, DCU_UPDATE_MODE,
-		     DCU_UPDATE_MODE_READREG);
 	return;
 }
 
diff --git a/drivers/gpu/drm/fsl-dcu/fsl_dcu_drm_drv.c b/drivers/gpu/drm/fsl-dcu/fsl_dcu_drm_drv.c
index 0884c45aefe8..e04efbed1a54 100644
--- a/drivers/gpu/drm/fsl-dcu/fsl_dcu_drm_drv.c
+++ b/drivers/gpu/drm/fsl-dcu/fsl_dcu_drm_drv.c
@@ -267,12 +267,8 @@ static int fsl_dcu_drm_pm_resume(struct device *dev)
 		return ret;
 	}
 
-	ret = clk_prepare_enable(fsl_dev->pix_clk);
-	if (ret < 0) {
-		dev_err(dev, "failed to enable pix clk\n");
-		goto disable_dcu_clk;
-	}
-
+	if (fsl_dev->tcon)
+		fsl_tcon_bypass_enable(fsl_dev->tcon);
 	fsl_dcu_drm_init_planes(fsl_dev->drm);
 	drm_atomic_helper_resume(fsl_dev->drm, fsl_dev->state);
 
@@ -284,10 +280,6 @@ static int fsl_dcu_drm_pm_resume(struct device *dev)
 	enable_irq(fsl_dev->irq);
 
 	return 0;
-
-disable_dcu_clk:
-	clk_disable_unprepare(fsl_dev->clk);
-	return ret;
 }
 #endif
 
@@ -401,18 +393,12 @@ static int fsl_dcu_drm_probe(struct platform_device *pdev)
 		goto disable_clk;
 	}
 
-	ret = clk_prepare_enable(fsl_dev->pix_clk);
-	if (ret < 0) {
-		dev_err(dev, "failed to enable pix clk\n");
-		goto unregister_pix_clk;
-	}
-
 	fsl_dev->tcon = fsl_tcon_init(dev);
 
 	drm = drm_dev_alloc(driver, dev);
 	if (IS_ERR(drm)) {
 		ret = PTR_ERR(drm);
-		goto disable_pix_clk;
+		goto unregister_pix_clk;
 	}
 
 	fsl_dev->dev = dev;
@@ -433,8 +419,6 @@ static int fsl_dcu_drm_probe(struct platform_device *pdev)
 
 unref:
 	drm_dev_unref(drm);
-disable_pix_clk:
-	clk_disable_unprepare(fsl_dev->pix_clk);
 unregister_pix_clk:
 	clk_unregister(fsl_dev->pix_clk);
 disable_clk:
@@ -447,7 +431,6 @@ static int fsl_dcu_drm_remove(struct platform_device *pdev)
 	struct fsl_dcu_drm_device *fsl_dev = platform_get_drvdata(pdev);
 
 	clk_disable_unprepare(fsl_dev->clk);
-	clk_disable_unprepare(fsl_dev->pix_clk);
 	clk_unregister(fsl_dev->pix_clk);
 	drm_put_dev(fsl_dev->drm);
 
diff --git a/drivers/gpu/drm/fsl-dcu/fsl_dcu_drm_plane.c b/drivers/gpu/drm/fsl-dcu/fsl_dcu_drm_plane.c
index a7e5486bd1e9..9e6f7d8112b3 100644
--- a/drivers/gpu/drm/fsl-dcu/fsl_dcu_drm_plane.c
+++ b/drivers/gpu/drm/fsl-dcu/fsl_dcu_drm_plane.c
@@ -211,11 +211,6 @@ void fsl_dcu_drm_init_planes(struct drm_device *dev)
 		for (j = 1; j <= fsl_dev->soc->layer_regs; j++)
 			regmap_write(fsl_dev->regmap, DCU_CTRLDESCLN(i, j), 0);
 	}
-	regmap_update_bits(fsl_dev->regmap, DCU_DCU_MODE,
-			   DCU_MODE_DCU_MODE_MASK,
-			   DCU_MODE_DCU_MODE(DCU_MODE_OFF));
-	regmap_write(fsl_dev->regmap, DCU_UPDATE_MODE,
-		     DCU_UPDATE_MODE_READREG);
 }
 
 struct drm_plane *fsl_dcu_drm_primary_create_plane(struct drm_device *dev)
diff --git a/drivers/gpu/drm/fsl-dcu/fsl_dcu_drm_rgb.c b/drivers/gpu/drm/fsl-dcu/fsl_dcu_drm_rgb.c
index 26edcc899712..e1dd75b18118 100644
--- a/drivers/gpu/drm/fsl-dcu/fsl_dcu_drm_rgb.c
+++ b/drivers/gpu/drm/fsl-dcu/fsl_dcu_drm_rgb.c
@@ -20,38 +20,6 @@
 #include "fsl_dcu_drm_drv.h"
 #include "fsl_tcon.h"
 
-static int
-fsl_dcu_drm_encoder_atomic_check(struct drm_encoder *encoder,
-				 struct drm_crtc_state *crtc_state,
-				 struct drm_connector_state *conn_state)
-{
-	return 0;
-}
-
-static void fsl_dcu_drm_encoder_disable(struct drm_encoder *encoder)
-{
-	struct drm_device *dev = encoder->dev;
-	struct fsl_dcu_drm_device *fsl_dev = dev->dev_private;
-
-	if (fsl_dev->tcon)
-		fsl_tcon_bypass_disable(fsl_dev->tcon);
-}
-
-static void fsl_dcu_drm_encoder_enable(struct drm_encoder *encoder)
-{
-	struct drm_device *dev = encoder->dev;
-	struct fsl_dcu_drm_device *fsl_dev = dev->dev_private;
-
-	if (fsl_dev->tcon)
-		fsl_tcon_bypass_enable(fsl_dev->tcon);
-}
-
-static const struct drm_encoder_helper_funcs encoder_helper_funcs = {
-	.atomic_check = fsl_dcu_drm_encoder_atomic_check,
-	.disable = fsl_dcu_drm_encoder_disable,
-	.enable = fsl_dcu_drm_encoder_enable,
-};
-
 static void fsl_dcu_drm_encoder_destroy(struct drm_encoder *encoder)
 {
 	drm_encoder_cleanup(encoder);
@@ -68,13 +36,16 @@ int fsl_dcu_drm_encoder_create(struct fsl_dcu_drm_device *fsl_dev,
 	int ret;
 
 	encoder->possible_crtcs = 1;
+
+	/* Use bypass mode for parallel RGB/LVDS encoder */
+	if (fsl_dev->tcon)
+		fsl_tcon_bypass_enable(fsl_dev->tcon);
+
 	ret = drm_encoder_init(fsl_dev->drm, encoder, &encoder_funcs,
 			       DRM_MODE_ENCODER_LVDS, NULL);
 	if (ret < 0)
 		return ret;
 
-	drm_encoder_helper_add(encoder, &encoder_helper_funcs);
-
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/i915/Kconfig b/drivers/gpu/drm/i915/Kconfig
index 7769e469118f..1c1b19ccb92f 100644
--- a/drivers/gpu/drm/i915/Kconfig
+++ b/drivers/gpu/drm/i915/Kconfig
@@ -46,6 +46,31 @@ config DRM_I915_PRELIMINARY_HW_SUPPORT
 
 	  If in doubt, say "N".
 
+config DRM_I915_CAPTURE_ERROR
+	bool "Enable capturing GPU state following a hang"
+	depends on DRM_I915
+	default y
+	help
+	  This option enables capturing the GPU state when a hang is detected.
+	  This information is vital for triaging hangs and assists in debugging.
+	  Please report any hang to
+            https://bugs.freedesktop.org/enter_bug.cgi?product=DRI
+	  for triaging.
+
+	  If in doubt, say "Y".
+
+config DRM_I915_COMPRESS_ERROR
+	bool "Compress GPU error state"
+	depends on DRM_I915_CAPTURE_ERROR
+	select ZLIB_DEFLATE
+	default y
+	help
+	  This option selects ZLIB_DEFLATE if it isn't already
+	  selected and causes any error state captured upon a GPU hang
+	  to be compressed using zlib.
+
+	  If in doubt, say "Y".
+
 config DRM_I915_USERPTR
 	bool "Always enable userptr support"
 	depends on DRM_I915
diff --git a/drivers/gpu/drm/i915/Makefile b/drivers/gpu/drm/i915/Makefile
index a998c2bce70a..612340097f4b 100644
--- a/drivers/gpu/drm/i915/Makefile
+++ b/drivers/gpu/drm/i915/Makefile
@@ -42,7 +42,6 @@ i915-y += i915_cmd_parser.o \
 	  i915_gem_stolen.o \
 	  i915_gem_tiling.o \
 	  i915_gem_userptr.o \
-	  i915_gpu_error.o \
 	  i915_trace_points.o \
 	  intel_breadcrumbs.o \
 	  intel_engine_cs.o \
@@ -102,11 +101,15 @@ i915-y += dvo_ch7017.o \
 	  intel_dvo.o \
 	  intel_hdmi.o \
 	  intel_i2c.o \
+	  intel_lspcon.o \
 	  intel_lvds.o \
 	  intel_panel.o \
 	  intel_sdvo.o \
 	  intel_tv.o
 
+# Post-mortem debug and GPU hang state capture
+i915-$(CONFIG_DRM_I915_CAPTURE_ERROR) += i915_gpu_error.o
+
 # virtual gpu code
 i915-y += i915_vgpu.o
 
diff --git a/drivers/gpu/drm/i915/gvt/Makefile b/drivers/gpu/drm/i915/gvt/Makefile
index d0f21a6ad60d..34ea4776af70 100644
--- a/drivers/gpu/drm/i915/gvt/Makefile
+++ b/drivers/gpu/drm/i915/gvt/Makefile
@@ -1,5 +1,7 @@
 GVT_DIR := gvt
-GVT_SOURCE := gvt.o
+GVT_SOURCE := gvt.o aperture_gm.o handlers.o vgpu.o trace_points.o firmware.o \
+	interrupt.o gtt.o cfg_space.o opregion.o mmio.o display.o edid.o \
+	execlist.o scheduler.o sched_policy.o render.o cmd_parser.o
 
 ccflags-y                      += -I$(src) -I$(src)/$(GVT_DIR) -Wall
 i915-y			       += $(addprefix $(GVT_DIR)/, $(GVT_SOURCE))
diff --git a/drivers/gpu/drm/i915/gvt/aperture_gm.c b/drivers/gpu/drm/i915/gvt/aperture_gm.c
new file mode 100644
index 000000000000..0d41ebc4aea6
--- /dev/null
+++ b/drivers/gpu/drm/i915/gvt/aperture_gm.c
@@ -0,0 +1,352 @@
+/*
+ * Copyright(c) 2011-2016 Intel Corporation. All rights reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ * Authors:
+ *    Kevin Tian <kevin.tian@intel.com>
+ *    Dexuan Cui
+ *
+ * Contributors:
+ *    Pei Zhang <pei.zhang@intel.com>
+ *    Min He <min.he@intel.com>
+ *    Niu Bing <bing.niu@intel.com>
+ *    Yulei Zhang <yulei.zhang@intel.com>
+ *    Zhenyu Wang <zhenyuw@linux.intel.com>
+ *    Zhi Wang <zhi.a.wang@intel.com>
+ *
+ */
+
+#include "i915_drv.h"
+#include "gvt.h"
+
+#define MB_TO_BYTES(mb) ((mb) << 20ULL)
+#define BYTES_TO_MB(b) ((b) >> 20ULL)
+
+#define HOST_LOW_GM_SIZE MB_TO_BYTES(128)
+#define HOST_HIGH_GM_SIZE MB_TO_BYTES(384)
+#define HOST_FENCE 4
+
+static int alloc_gm(struct intel_vgpu *vgpu, bool high_gm)
+{
+	struct intel_gvt *gvt = vgpu->gvt;
+	struct drm_i915_private *dev_priv = gvt->dev_priv;
+	u32 alloc_flag, search_flag;
+	u64 start, end, size;
+	struct drm_mm_node *node;
+	int retried = 0;
+	int ret;
+
+	if (high_gm) {
+		search_flag = DRM_MM_SEARCH_BELOW;
+		alloc_flag = DRM_MM_CREATE_TOP;
+		node = &vgpu->gm.high_gm_node;
+		size = vgpu_hidden_sz(vgpu);
+		start = gvt_hidden_gmadr_base(gvt);
+		end = gvt_hidden_gmadr_end(gvt);
+	} else {
+		search_flag = DRM_MM_SEARCH_DEFAULT;
+		alloc_flag = DRM_MM_CREATE_DEFAULT;
+		node = &vgpu->gm.low_gm_node;
+		size = vgpu_aperture_sz(vgpu);
+		start = gvt_aperture_gmadr_base(gvt);
+		end = gvt_aperture_gmadr_end(gvt);
+	}
+
+	mutex_lock(&dev_priv->drm.struct_mutex);
+search_again:
+	ret = drm_mm_insert_node_in_range_generic(&dev_priv->ggtt.base.mm,
+						  node, size, 4096, 0,
+						  start, end, search_flag,
+						  alloc_flag);
+	if (ret) {
+		ret = i915_gem_evict_something(&dev_priv->ggtt.base,
+					       size, 4096, 0, start, end, 0);
+		if (ret == 0 && ++retried < 3)
+			goto search_again;
+
+		gvt_err("fail to alloc %s gm space from host, retried %d\n",
+				high_gm ? "high" : "low", retried);
+	}
+	mutex_unlock(&dev_priv->drm.struct_mutex);
+	return ret;
+}
+
+static int alloc_vgpu_gm(struct intel_vgpu *vgpu)
+{
+	struct intel_gvt *gvt = vgpu->gvt;
+	struct drm_i915_private *dev_priv = gvt->dev_priv;
+	int ret;
+
+	ret = alloc_gm(vgpu, false);
+	if (ret)
+		return ret;
+
+	ret = alloc_gm(vgpu, true);
+	if (ret)
+		goto out_free_aperture;
+
+	gvt_dbg_core("vgpu%d: alloc low GM start %llx size %llx\n", vgpu->id,
+		     vgpu_aperture_offset(vgpu), vgpu_aperture_sz(vgpu));
+
+	gvt_dbg_core("vgpu%d: alloc high GM start %llx size %llx\n", vgpu->id,
+		     vgpu_hidden_offset(vgpu), vgpu_hidden_sz(vgpu));
+
+	return 0;
+out_free_aperture:
+	mutex_lock(&dev_priv->drm.struct_mutex);
+	drm_mm_remove_node(&vgpu->gm.low_gm_node);
+	mutex_unlock(&dev_priv->drm.struct_mutex);
+	return ret;
+}
+
+static void free_vgpu_gm(struct intel_vgpu *vgpu)
+{
+	struct drm_i915_private *dev_priv = vgpu->gvt->dev_priv;
+
+	mutex_lock(&dev_priv->drm.struct_mutex);
+	drm_mm_remove_node(&vgpu->gm.low_gm_node);
+	drm_mm_remove_node(&vgpu->gm.high_gm_node);
+	mutex_unlock(&dev_priv->drm.struct_mutex);
+}
+
+/**
+ * intel_vgpu_write_fence - write fence registers owned by a vGPU
+ * @vgpu: vGPU instance
+ * @fence: vGPU fence register number
+ * @value: Fence register value to be written
+ *
+ * This function is used to write fence registers owned by a vGPU. The vGPU
+ * fence register number will be translated into HW fence register number.
+ *
+ */
+void intel_vgpu_write_fence(struct intel_vgpu *vgpu,
+		u32 fence, u64 value)
+{
+	struct intel_gvt *gvt = vgpu->gvt;
+	struct drm_i915_private *dev_priv = gvt->dev_priv;
+	struct drm_i915_fence_reg *reg;
+	i915_reg_t fence_reg_lo, fence_reg_hi;
+
+	assert_rpm_wakelock_held(dev_priv);
+
+	if (WARN_ON(fence > vgpu_fence_sz(vgpu)))
+		return;
+
+	reg = vgpu->fence.regs[fence];
+	if (WARN_ON(!reg))
+		return;
+
+	fence_reg_lo = FENCE_REG_GEN6_LO(reg->id);
+	fence_reg_hi = FENCE_REG_GEN6_HI(reg->id);
+
+	I915_WRITE(fence_reg_lo, 0);
+	POSTING_READ(fence_reg_lo);
+
+	I915_WRITE(fence_reg_hi, upper_32_bits(value));
+	I915_WRITE(fence_reg_lo, lower_32_bits(value));
+	POSTING_READ(fence_reg_lo);
+}
+
+static void free_vgpu_fence(struct intel_vgpu *vgpu)
+{
+	struct intel_gvt *gvt = vgpu->gvt;
+	struct drm_i915_private *dev_priv = gvt->dev_priv;
+	struct drm_i915_fence_reg *reg;
+	u32 i;
+
+	if (WARN_ON(!vgpu_fence_sz(vgpu)))
+		return;
+
+	intel_runtime_pm_get(dev_priv);
+
+	mutex_lock(&dev_priv->drm.struct_mutex);
+	for (i = 0; i < vgpu_fence_sz(vgpu); i++) {
+		reg = vgpu->fence.regs[i];
+		intel_vgpu_write_fence(vgpu, i, 0);
+		list_add_tail(&reg->link,
+			      &dev_priv->mm.fence_list);
+	}
+	mutex_unlock(&dev_priv->drm.struct_mutex);
+
+	intel_runtime_pm_put(dev_priv);
+}
+
+static int alloc_vgpu_fence(struct intel_vgpu *vgpu)
+{
+	struct intel_gvt *gvt = vgpu->gvt;
+	struct drm_i915_private *dev_priv = gvt->dev_priv;
+	struct drm_i915_fence_reg *reg;
+	int i;
+	struct list_head *pos, *q;
+
+	intel_runtime_pm_get(dev_priv);
+
+	/* Request fences from host */
+	mutex_lock(&dev_priv->drm.struct_mutex);
+	i = 0;
+	list_for_each_safe(pos, q, &dev_priv->mm.fence_list) {
+		reg = list_entry(pos, struct drm_i915_fence_reg, link);
+		if (reg->pin_count || reg->vma)
+			continue;
+		list_del(pos);
+		vgpu->fence.regs[i] = reg;
+		intel_vgpu_write_fence(vgpu, i, 0);
+		if (++i == vgpu_fence_sz(vgpu))
+			break;
+	}
+	if (i != vgpu_fence_sz(vgpu))
+		goto out_free_fence;
+
+	mutex_unlock(&dev_priv->drm.struct_mutex);
+	intel_runtime_pm_put(dev_priv);
+	return 0;
+out_free_fence:
+	/* Return fences to host, if fail */
+	for (i = 0; i < vgpu_fence_sz(vgpu); i++) {
+		reg = vgpu->fence.regs[i];
+		if (!reg)
+			continue;
+		list_add_tail(&reg->link,
+			      &dev_priv->mm.fence_list);
+	}
+	mutex_unlock(&dev_priv->drm.struct_mutex);
+	intel_runtime_pm_put(dev_priv);
+	return -ENOSPC;
+}
+
+static void free_resource(struct intel_vgpu *vgpu)
+{
+	struct intel_gvt *gvt = vgpu->gvt;
+
+	gvt->gm.vgpu_allocated_low_gm_size -= vgpu_aperture_sz(vgpu);
+	gvt->gm.vgpu_allocated_high_gm_size -= vgpu_hidden_sz(vgpu);
+	gvt->fence.vgpu_allocated_fence_num -= vgpu_fence_sz(vgpu);
+}
+
+static int alloc_resource(struct intel_vgpu *vgpu,
+		struct intel_vgpu_creation_params *param)
+{
+	struct intel_gvt *gvt = vgpu->gvt;
+	unsigned long request, avail, max, taken;
+	const char *item;
+
+	if (!param->low_gm_sz || !param->high_gm_sz || !param->fence_sz) {
+		gvt_err("Invalid vGPU creation params\n");
+		return -EINVAL;
+	}
+
+	item = "low GM space";
+	max = gvt_aperture_sz(gvt) - HOST_LOW_GM_SIZE;
+	taken = gvt->gm.vgpu_allocated_low_gm_size;
+	avail = max - taken;
+	request = MB_TO_BYTES(param->low_gm_sz);
+
+	if (request > avail)
+		goto no_enough_resource;
+
+	vgpu_aperture_sz(vgpu) = request;
+
+	item = "high GM space";
+	max = gvt_hidden_sz(gvt) - HOST_HIGH_GM_SIZE;
+	taken = gvt->gm.vgpu_allocated_high_gm_size;
+	avail = max - taken;
+	request = MB_TO_BYTES(param->high_gm_sz);
+
+	if (request > avail)
+		goto no_enough_resource;
+
+	vgpu_hidden_sz(vgpu) = request;
+
+	item = "fence";
+	max = gvt_fence_sz(gvt) - HOST_FENCE;
+	taken = gvt->fence.vgpu_allocated_fence_num;
+	avail = max - taken;
+	request = param->fence_sz;
+
+	if (request > avail)
+		goto no_enough_resource;
+
+	vgpu_fence_sz(vgpu) = request;
+
+	gvt->gm.vgpu_allocated_low_gm_size += MB_TO_BYTES(param->low_gm_sz);
+	gvt->gm.vgpu_allocated_high_gm_size += MB_TO_BYTES(param->high_gm_sz);
+	gvt->fence.vgpu_allocated_fence_num += param->fence_sz;
+	return 0;
+
+no_enough_resource:
+	gvt_err("vgpu%d: fail to allocate resource %s\n", vgpu->id, item);
+	gvt_err("vgpu%d: request %luMB avail %luMB max %luMB taken %luMB\n",
+		vgpu->id, BYTES_TO_MB(request), BYTES_TO_MB(avail),
+		BYTES_TO_MB(max), BYTES_TO_MB(taken));
+	return -ENOSPC;
+}
+
+/**
+ * inte_gvt_free_vgpu_resource - free HW resource owned by a vGPU
+ * @vgpu: a vGPU
+ *
+ * This function is used to free the HW resource owned by a vGPU.
+ *
+ */
+void intel_vgpu_free_resource(struct intel_vgpu *vgpu)
+{
+	free_vgpu_gm(vgpu);
+	free_vgpu_fence(vgpu);
+	free_resource(vgpu);
+}
+
+/**
+ * intel_alloc_vgpu_resource - allocate HW resource for a vGPU
+ * @vgpu: vGPU
+ * @param: vGPU creation params
+ *
+ * This function is used to allocate HW resource for a vGPU. User specifies
+ * the resource configuration through the creation params.
+ *
+ * Returns:
+ * zero on success, negative error code if failed.
+ *
+ */
+int intel_vgpu_alloc_resource(struct intel_vgpu *vgpu,
+		struct intel_vgpu_creation_params *param)
+{
+	int ret;
+
+	ret = alloc_resource(vgpu, param);
+	if (ret)
+		return ret;
+
+	ret = alloc_vgpu_gm(vgpu);
+	if (ret)
+		goto out_free_resource;
+
+	ret = alloc_vgpu_fence(vgpu);
+	if (ret)
+		goto out_free_vgpu_gm;
+
+	return 0;
+
+out_free_vgpu_gm:
+	free_vgpu_gm(vgpu);
+out_free_resource:
+	free_resource(vgpu);
+	return ret;
+}
diff --git a/drivers/gpu/drm/i915/gvt/cfg_space.c b/drivers/gpu/drm/i915/gvt/cfg_space.c
new file mode 100644
index 000000000000..4c687740f5f1
--- /dev/null
+++ b/drivers/gpu/drm/i915/gvt/cfg_space.c
@@ -0,0 +1,288 @@
+/*
+ * Copyright(c) 2011-2016 Intel Corporation. All rights reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ * Authors:
+ *    Eddie Dong <eddie.dong@intel.com>
+ *    Jike Song <jike.song@intel.com>
+ *
+ * Contributors:
+ *    Zhi Wang <zhi.a.wang@intel.com>
+ *    Min He <min.he@intel.com>
+ *    Bing Niu <bing.niu@intel.com>
+ *
+ */
+
+#include "i915_drv.h"
+#include "gvt.h"
+
+enum {
+	INTEL_GVT_PCI_BAR_GTTMMIO = 0,
+	INTEL_GVT_PCI_BAR_APERTURE,
+	INTEL_GVT_PCI_BAR_PIO,
+	INTEL_GVT_PCI_BAR_MAX,
+};
+
+/**
+ * intel_vgpu_emulate_cfg_read - emulate vGPU configuration space read
+ *
+ * Returns:
+ * Zero on success, negative error code if failed.
+ */
+int intel_vgpu_emulate_cfg_read(void *__vgpu, unsigned int offset,
+	void *p_data, unsigned int bytes)
+{
+	struct intel_vgpu *vgpu = __vgpu;
+
+	if (WARN_ON(bytes > 4))
+		return -EINVAL;
+
+	if (WARN_ON(offset + bytes > INTEL_GVT_MAX_CFG_SPACE_SZ))
+		return -EINVAL;
+
+	memcpy(p_data, vgpu_cfg_space(vgpu) + offset, bytes);
+	return 0;
+}
+
+static int map_aperture(struct intel_vgpu *vgpu, bool map)
+{
+	u64 first_gfn, first_mfn;
+	u64 val;
+	int ret;
+
+	if (map == vgpu->cfg_space.bar[INTEL_GVT_PCI_BAR_APERTURE].tracked)
+		return 0;
+
+	val = vgpu_cfg_space(vgpu)[PCI_BASE_ADDRESS_2];
+	if (val & PCI_BASE_ADDRESS_MEM_TYPE_64)
+		val = *(u64 *)(vgpu_cfg_space(vgpu) + PCI_BASE_ADDRESS_2);
+	else
+		val = *(u32 *)(vgpu_cfg_space(vgpu) + PCI_BASE_ADDRESS_2);
+
+	first_gfn = (val + vgpu_aperture_offset(vgpu)) >> PAGE_SHIFT;
+	first_mfn = vgpu_aperture_pa_base(vgpu) >> PAGE_SHIFT;
+
+	ret = intel_gvt_hypervisor_map_gfn_to_mfn(vgpu, first_gfn,
+						  first_mfn,
+						  vgpu_aperture_sz(vgpu)
+						  >> PAGE_SHIFT, map,
+						  GVT_MAP_APERTURE);
+	if (ret)
+		return ret;
+
+	vgpu->cfg_space.bar[INTEL_GVT_PCI_BAR_APERTURE].tracked = map;
+	return 0;
+}
+
+static int trap_gttmmio(struct intel_vgpu *vgpu, bool trap)
+{
+	u64 start, end;
+	u64 val;
+	int ret;
+
+	if (trap == vgpu->cfg_space.bar[INTEL_GVT_PCI_BAR_GTTMMIO].tracked)
+		return 0;
+
+	val = vgpu_cfg_space(vgpu)[PCI_BASE_ADDRESS_0];
+	if (val & PCI_BASE_ADDRESS_MEM_TYPE_64)
+		start = *(u64 *)(vgpu_cfg_space(vgpu) + PCI_BASE_ADDRESS_0);
+	else
+		start = *(u32 *)(vgpu_cfg_space(vgpu) + PCI_BASE_ADDRESS_0);
+
+	start &= ~GENMASK(3, 0);
+	end = start + vgpu->cfg_space.bar[INTEL_GVT_PCI_BAR_GTTMMIO].size - 1;
+
+	ret = intel_gvt_hypervisor_set_trap_area(vgpu, start, end, trap);
+	if (ret)
+		return ret;
+
+	vgpu->cfg_space.bar[INTEL_GVT_PCI_BAR_GTTMMIO].tracked = trap;
+	return 0;
+}
+
+static int emulate_pci_command_write(struct intel_vgpu *vgpu,
+	unsigned int offset, void *p_data, unsigned int bytes)
+{
+	u8 old = vgpu_cfg_space(vgpu)[offset];
+	u8 new = *(u8 *)p_data;
+	u8 changed = old ^ new;
+	int ret;
+
+	if (!(changed & PCI_COMMAND_MEMORY))
+		return 0;
+
+	if (old & PCI_COMMAND_MEMORY) {
+		ret = trap_gttmmio(vgpu, false);
+		if (ret)
+			return ret;
+		ret = map_aperture(vgpu, false);
+		if (ret)
+			return ret;
+	} else {
+		ret = trap_gttmmio(vgpu, true);
+		if (ret)
+			return ret;
+		ret = map_aperture(vgpu, true);
+		if (ret)
+			return ret;
+	}
+
+	memcpy(vgpu_cfg_space(vgpu) + offset, p_data, bytes);
+	return 0;
+}
+
+static int emulate_pci_bar_write(struct intel_vgpu *vgpu, unsigned int offset,
+	void *p_data, unsigned int bytes)
+{
+	unsigned int bar_index =
+		(rounddown(offset, 8) % PCI_BASE_ADDRESS_0) / 8;
+	u32 new = *(u32 *)(p_data);
+	bool lo = IS_ALIGNED(offset, 8);
+	u64 size;
+	int ret = 0;
+	bool mmio_enabled =
+		vgpu_cfg_space(vgpu)[PCI_COMMAND] & PCI_COMMAND_MEMORY;
+
+	if (WARN_ON(bar_index >= INTEL_GVT_PCI_BAR_MAX))
+		return -EINVAL;
+
+	if (new == 0xffffffff) {
+		/*
+		 * Power-up software can determine how much address
+		 * space the device requires by writing a value of
+		 * all 1's to the register and then reading the value
+		 * back. The device will return 0's in all don't-care
+		 * address bits.
+		 */
+		size = vgpu->cfg_space.bar[bar_index].size;
+		if (lo) {
+			new = rounddown(new, size);
+		} else {
+			u32 val = vgpu_cfg_space(vgpu)[rounddown(offset, 8)];
+			/* for 32bit mode bar it returns all-0 in upper 32
+			 * bit, for 64bit mode bar it will calculate the
+			 * size with lower 32bit and return the corresponding
+			 * value
+			 */
+			if (val & PCI_BASE_ADDRESS_MEM_TYPE_64)
+				new &= (~(size-1)) >> 32;
+			else
+				new = 0;
+		}
+		/*
+		 * Unmapp & untrap the BAR, since guest hasn't configured a
+		 * valid GPA
+		 */
+		switch (bar_index) {
+		case INTEL_GVT_PCI_BAR_GTTMMIO:
+			ret = trap_gttmmio(vgpu, false);
+			break;
+		case INTEL_GVT_PCI_BAR_APERTURE:
+			ret = map_aperture(vgpu, false);
+			break;
+		}
+		intel_vgpu_write_pci_bar(vgpu, offset, new, lo);
+	} else {
+		/*
+		 * Unmapp & untrap the old BAR first, since guest has
+		 * re-configured the BAR
+		 */
+		switch (bar_index) {
+		case INTEL_GVT_PCI_BAR_GTTMMIO:
+			ret = trap_gttmmio(vgpu, false);
+			break;
+		case INTEL_GVT_PCI_BAR_APERTURE:
+			ret = map_aperture(vgpu, false);
+			break;
+		}
+		intel_vgpu_write_pci_bar(vgpu, offset, new, lo);
+		/* Track the new BAR */
+		if (mmio_enabled) {
+			switch (bar_index) {
+			case INTEL_GVT_PCI_BAR_GTTMMIO:
+				ret = trap_gttmmio(vgpu, true);
+				break;
+			case INTEL_GVT_PCI_BAR_APERTURE:
+				ret = map_aperture(vgpu, true);
+				break;
+			}
+		}
+	}
+	return ret;
+}
+
+/**
+ * intel_vgpu_emulate_cfg_read - emulate vGPU configuration space write
+ *
+ * Returns:
+ * Zero on success, negative error code if failed.
+ */
+int intel_vgpu_emulate_cfg_write(void *__vgpu, unsigned int offset,
+	void *p_data, unsigned int bytes)
+{
+	struct intel_vgpu *vgpu = __vgpu;
+	int ret;
+
+	if (WARN_ON(bytes > 4))
+		return -EINVAL;
+
+	if (WARN_ON(offset + bytes >= INTEL_GVT_MAX_CFG_SPACE_SZ))
+		return -EINVAL;
+
+	/* First check if it's PCI_COMMAND */
+	if (IS_ALIGNED(offset, 2) && offset == PCI_COMMAND) {
+		if (WARN_ON(bytes > 2))
+			return -EINVAL;
+		return emulate_pci_command_write(vgpu, offset, p_data, bytes);
+	}
+
+	switch (rounddown(offset, 4)) {
+	case PCI_BASE_ADDRESS_0:
+	case PCI_BASE_ADDRESS_1:
+	case PCI_BASE_ADDRESS_2:
+	case PCI_BASE_ADDRESS_3:
+		if (WARN_ON(!IS_ALIGNED(offset, 4)))
+			return -EINVAL;
+		return emulate_pci_bar_write(vgpu, offset, p_data, bytes);
+
+	case INTEL_GVT_PCI_SWSCI:
+		if (WARN_ON(!IS_ALIGNED(offset, 4)))
+			return -EINVAL;
+		ret = intel_vgpu_emulate_opregion_request(vgpu, *(u32 *)p_data);
+		if (ret)
+			return ret;
+		break;
+
+	case INTEL_GVT_PCI_OPREGION:
+		if (WARN_ON(!IS_ALIGNED(offset, 4)))
+			return -EINVAL;
+		ret = intel_vgpu_init_opregion(vgpu, *(u32 *)p_data);
+		if (ret)
+			return ret;
+
+		memcpy(vgpu_cfg_space(vgpu) + offset, p_data, bytes);
+		break;
+	default:
+		memcpy(vgpu_cfg_space(vgpu) + offset, p_data, bytes);
+		break;
+	}
+	return 0;
+}
diff --git a/drivers/gpu/drm/i915/gvt/cmd_parser.c b/drivers/gpu/drm/i915/gvt/cmd_parser.c
new file mode 100644
index 000000000000..aafb57e26288
--- /dev/null
+++ b/drivers/gpu/drm/i915/gvt/cmd_parser.c
@@ -0,0 +1,2831 @@
+/*
+ * Copyright(c) 2011-2016 Intel Corporation. All rights reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ * Authors:
+ *    Ke Yu
+ *    Kevin Tian <kevin.tian@intel.com>
+ *    Zhiyuan Lv <zhiyuan.lv@intel.com>
+ *
+ * Contributors:
+ *    Min He <min.he@intel.com>
+ *    Ping Gao <ping.a.gao@intel.com>
+ *    Tina Zhang <tina.zhang@intel.com>
+ *    Yulei Zhang <yulei.zhang@intel.com>
+ *    Zhi Wang <zhi.a.wang@intel.com>
+ *
+ */
+
+#include <linux/slab.h>
+#include "i915_drv.h"
+#include "gvt.h"
+#include "i915_pvinfo.h"
+#include "trace.h"
+
+#define INVALID_OP    (~0U)
+
+#define OP_LEN_MI           9
+#define OP_LEN_2D           10
+#define OP_LEN_3D_MEDIA     16
+#define OP_LEN_MFX_VC       16
+#define OP_LEN_VEBOX	    16
+
+#define CMD_TYPE(cmd)	(((cmd) >> 29) & 7)
+
+struct sub_op_bits {
+	int hi;
+	int low;
+};
+struct decode_info {
+	char *name;
+	int op_len;
+	int nr_sub_op;
+	struct sub_op_bits *sub_op;
+};
+
+#define   MAX_CMD_BUDGET			0x7fffffff
+#define   MI_WAIT_FOR_PLANE_C_FLIP_PENDING      (1<<15)
+#define   MI_WAIT_FOR_PLANE_B_FLIP_PENDING      (1<<9)
+#define   MI_WAIT_FOR_PLANE_A_FLIP_PENDING      (1<<1)
+
+#define   MI_WAIT_FOR_SPRITE_C_FLIP_PENDING      (1<<20)
+#define   MI_WAIT_FOR_SPRITE_B_FLIP_PENDING      (1<<10)
+#define   MI_WAIT_FOR_SPRITE_A_FLIP_PENDING      (1<<2)
+
+/* Render Command Map */
+
+/* MI_* command Opcode (28:23) */
+#define OP_MI_NOOP                          0x0
+#define OP_MI_SET_PREDICATE                 0x1  /* HSW+ */
+#define OP_MI_USER_INTERRUPT                0x2
+#define OP_MI_WAIT_FOR_EVENT                0x3
+#define OP_MI_FLUSH                         0x4
+#define OP_MI_ARB_CHECK                     0x5
+#define OP_MI_RS_CONTROL                    0x6  /* HSW+ */
+#define OP_MI_REPORT_HEAD                   0x7
+#define OP_MI_ARB_ON_OFF                    0x8
+#define OP_MI_URB_ATOMIC_ALLOC              0x9  /* HSW+ */
+#define OP_MI_BATCH_BUFFER_END              0xA
+#define OP_MI_SUSPEND_FLUSH                 0xB
+#define OP_MI_PREDICATE                     0xC  /* IVB+ */
+#define OP_MI_TOPOLOGY_FILTER               0xD  /* IVB+ */
+#define OP_MI_SET_APPID                     0xE  /* IVB+ */
+#define OP_MI_RS_CONTEXT                    0xF  /* HSW+ */
+#define OP_MI_LOAD_SCAN_LINES_INCL          0x12 /* HSW+ */
+#define OP_MI_DISPLAY_FLIP                  0x14
+#define OP_MI_SEMAPHORE_MBOX                0x16
+#define OP_MI_SET_CONTEXT                   0x18
+#define OP_MI_MATH                          0x1A
+#define OP_MI_URB_CLEAR                     0x19
+#define OP_MI_SEMAPHORE_SIGNAL		    0x1B  /* BDW+ */
+#define OP_MI_SEMAPHORE_WAIT		    0x1C  /* BDW+ */
+
+#define OP_MI_STORE_DATA_IMM                0x20
+#define OP_MI_STORE_DATA_INDEX              0x21
+#define OP_MI_LOAD_REGISTER_IMM             0x22
+#define OP_MI_UPDATE_GTT                    0x23
+#define OP_MI_STORE_REGISTER_MEM            0x24
+#define OP_MI_FLUSH_DW                      0x26
+#define OP_MI_CLFLUSH                       0x27
+#define OP_MI_REPORT_PERF_COUNT             0x28
+#define OP_MI_LOAD_REGISTER_MEM             0x29  /* HSW+ */
+#define OP_MI_LOAD_REGISTER_REG             0x2A  /* HSW+ */
+#define OP_MI_RS_STORE_DATA_IMM             0x2B  /* HSW+ */
+#define OP_MI_LOAD_URB_MEM                  0x2C  /* HSW+ */
+#define OP_MI_STORE_URM_MEM                 0x2D  /* HSW+ */
+#define OP_MI_2E			    0x2E  /* BDW+ */
+#define OP_MI_2F			    0x2F  /* BDW+ */
+#define OP_MI_BATCH_BUFFER_START            0x31
+
+/* Bit definition for dword 0 */
+#define _CMDBIT_BB_START_IN_PPGTT	(1UL << 8)
+
+#define OP_MI_CONDITIONAL_BATCH_BUFFER_END  0x36
+
+#define BATCH_BUFFER_ADDR_MASK ((1UL << 32) - (1U << 2))
+#define BATCH_BUFFER_ADDR_HIGH_MASK ((1UL << 16) - (1U))
+#define BATCH_BUFFER_ADR_SPACE_BIT(x)	(((x) >> 8) & 1U)
+#define BATCH_BUFFER_2ND_LEVEL_BIT(x)   ((x) >> 22 & 1U)
+
+/* 2D command: Opcode (28:22) */
+#define OP_2D(x)    ((2<<7) | x)
+
+#define OP_XY_SETUP_BLT                             OP_2D(0x1)
+#define OP_XY_SETUP_CLIP_BLT                        OP_2D(0x3)
+#define OP_XY_SETUP_MONO_PATTERN_SL_BLT             OP_2D(0x11)
+#define OP_XY_PIXEL_BLT                             OP_2D(0x24)
+#define OP_XY_SCANLINES_BLT                         OP_2D(0x25)
+#define OP_XY_TEXT_BLT                              OP_2D(0x26)
+#define OP_XY_TEXT_IMMEDIATE_BLT                    OP_2D(0x31)
+#define OP_XY_COLOR_BLT                             OP_2D(0x50)
+#define OP_XY_PAT_BLT                               OP_2D(0x51)
+#define OP_XY_MONO_PAT_BLT                          OP_2D(0x52)
+#define OP_XY_SRC_COPY_BLT                          OP_2D(0x53)
+#define OP_XY_MONO_SRC_COPY_BLT                     OP_2D(0x54)
+#define OP_XY_FULL_BLT                              OP_2D(0x55)
+#define OP_XY_FULL_MONO_SRC_BLT                     OP_2D(0x56)
+#define OP_XY_FULL_MONO_PATTERN_BLT                 OP_2D(0x57)
+#define OP_XY_FULL_MONO_PATTERN_MONO_SRC_BLT        OP_2D(0x58)
+#define OP_XY_MONO_PAT_FIXED_BLT                    OP_2D(0x59)
+#define OP_XY_MONO_SRC_COPY_IMMEDIATE_BLT           OP_2D(0x71)
+#define OP_XY_PAT_BLT_IMMEDIATE                     OP_2D(0x72)
+#define OP_XY_SRC_COPY_CHROMA_BLT                   OP_2D(0x73)
+#define OP_XY_FULL_IMMEDIATE_PATTERN_BLT            OP_2D(0x74)
+#define OP_XY_FULL_MONO_SRC_IMMEDIATE_PATTERN_BLT   OP_2D(0x75)
+#define OP_XY_PAT_CHROMA_BLT                        OP_2D(0x76)
+#define OP_XY_PAT_CHROMA_BLT_IMMEDIATE              OP_2D(0x77)
+
+/* 3D/Media Command: Pipeline Type(28:27) Opcode(26:24) Sub Opcode(23:16) */
+#define OP_3D_MEDIA(sub_type, opcode, sub_opcode) \
+	((3 << 13) | ((sub_type) << 11) | ((opcode) << 8) | (sub_opcode))
+
+#define OP_STATE_PREFETCH                       OP_3D_MEDIA(0x0, 0x0, 0x03)
+
+#define OP_STATE_BASE_ADDRESS                   OP_3D_MEDIA(0x0, 0x1, 0x01)
+#define OP_STATE_SIP                            OP_3D_MEDIA(0x0, 0x1, 0x02)
+#define OP_3D_MEDIA_0_1_4			OP_3D_MEDIA(0x0, 0x1, 0x04)
+
+#define OP_3DSTATE_VF_STATISTICS_GM45           OP_3D_MEDIA(0x1, 0x0, 0x0B)
+
+#define OP_PIPELINE_SELECT                      OP_3D_MEDIA(0x1, 0x1, 0x04)
+
+#define OP_MEDIA_VFE_STATE                      OP_3D_MEDIA(0x2, 0x0, 0x0)
+#define OP_MEDIA_CURBE_LOAD                     OP_3D_MEDIA(0x2, 0x0, 0x1)
+#define OP_MEDIA_INTERFACE_DESCRIPTOR_LOAD      OP_3D_MEDIA(0x2, 0x0, 0x2)
+#define OP_MEDIA_GATEWAY_STATE                  OP_3D_MEDIA(0x2, 0x0, 0x3)
+#define OP_MEDIA_STATE_FLUSH                    OP_3D_MEDIA(0x2, 0x0, 0x4)
+
+#define OP_MEDIA_OBJECT                         OP_3D_MEDIA(0x2, 0x1, 0x0)
+#define OP_MEDIA_OBJECT_PRT                     OP_3D_MEDIA(0x2, 0x1, 0x2)
+#define OP_MEDIA_OBJECT_WALKER                  OP_3D_MEDIA(0x2, 0x1, 0x3)
+#define OP_GPGPU_WALKER                         OP_3D_MEDIA(0x2, 0x1, 0x5)
+
+#define OP_3DSTATE_CLEAR_PARAMS                 OP_3D_MEDIA(0x3, 0x0, 0x04) /* IVB+ */
+#define OP_3DSTATE_DEPTH_BUFFER                 OP_3D_MEDIA(0x3, 0x0, 0x05) /* IVB+ */
+#define OP_3DSTATE_STENCIL_BUFFER               OP_3D_MEDIA(0x3, 0x0, 0x06) /* IVB+ */
+#define OP_3DSTATE_HIER_DEPTH_BUFFER            OP_3D_MEDIA(0x3, 0x0, 0x07) /* IVB+ */
+#define OP_3DSTATE_VERTEX_BUFFERS               OP_3D_MEDIA(0x3, 0x0, 0x08)
+#define OP_3DSTATE_VERTEX_ELEMENTS              OP_3D_MEDIA(0x3, 0x0, 0x09)
+#define OP_3DSTATE_INDEX_BUFFER                 OP_3D_MEDIA(0x3, 0x0, 0x0A)
+#define OP_3DSTATE_VF_STATISTICS                OP_3D_MEDIA(0x3, 0x0, 0x0B)
+#define OP_3DSTATE_VF                           OP_3D_MEDIA(0x3, 0x0, 0x0C)  /* HSW+ */
+#define OP_3DSTATE_CC_STATE_POINTERS            OP_3D_MEDIA(0x3, 0x0, 0x0E)
+#define OP_3DSTATE_SCISSOR_STATE_POINTERS       OP_3D_MEDIA(0x3, 0x0, 0x0F)
+#define OP_3DSTATE_VS                           OP_3D_MEDIA(0x3, 0x0, 0x10)
+#define OP_3DSTATE_GS                           OP_3D_MEDIA(0x3, 0x0, 0x11)
+#define OP_3DSTATE_CLIP                         OP_3D_MEDIA(0x3, 0x0, 0x12)
+#define OP_3DSTATE_SF                           OP_3D_MEDIA(0x3, 0x0, 0x13)
+#define OP_3DSTATE_WM                           OP_3D_MEDIA(0x3, 0x0, 0x14)
+#define OP_3DSTATE_CONSTANT_VS                  OP_3D_MEDIA(0x3, 0x0, 0x15)
+#define OP_3DSTATE_CONSTANT_GS                  OP_3D_MEDIA(0x3, 0x0, 0x16)
+#define OP_3DSTATE_CONSTANT_PS                  OP_3D_MEDIA(0x3, 0x0, 0x17)
+#define OP_3DSTATE_SAMPLE_MASK                  OP_3D_MEDIA(0x3, 0x0, 0x18)
+#define OP_3DSTATE_CONSTANT_HS                  OP_3D_MEDIA(0x3, 0x0, 0x19) /* IVB+ */
+#define OP_3DSTATE_CONSTANT_DS                  OP_3D_MEDIA(0x3, 0x0, 0x1A) /* IVB+ */
+#define OP_3DSTATE_HS                           OP_3D_MEDIA(0x3, 0x0, 0x1B) /* IVB+ */
+#define OP_3DSTATE_TE                           OP_3D_MEDIA(0x3, 0x0, 0x1C) /* IVB+ */
+#define OP_3DSTATE_DS                           OP_3D_MEDIA(0x3, 0x0, 0x1D) /* IVB+ */
+#define OP_3DSTATE_STREAMOUT                    OP_3D_MEDIA(0x3, 0x0, 0x1E) /* IVB+ */
+#define OP_3DSTATE_SBE                          OP_3D_MEDIA(0x3, 0x0, 0x1F) /* IVB+ */
+#define OP_3DSTATE_PS                           OP_3D_MEDIA(0x3, 0x0, 0x20) /* IVB+ */
+#define OP_3DSTATE_VIEWPORT_STATE_POINTERS_SF_CLIP OP_3D_MEDIA(0x3, 0x0, 0x21) /* IVB+ */
+#define OP_3DSTATE_VIEWPORT_STATE_POINTERS_CC   OP_3D_MEDIA(0x3, 0x0, 0x23) /* IVB+ */
+#define OP_3DSTATE_BLEND_STATE_POINTERS         OP_3D_MEDIA(0x3, 0x0, 0x24) /* IVB+ */
+#define OP_3DSTATE_DEPTH_STENCIL_STATE_POINTERS OP_3D_MEDIA(0x3, 0x0, 0x25) /* IVB+ */
+#define OP_3DSTATE_BINDING_TABLE_POINTERS_VS    OP_3D_MEDIA(0x3, 0x0, 0x26) /* IVB+ */
+#define OP_3DSTATE_BINDING_TABLE_POINTERS_HS    OP_3D_MEDIA(0x3, 0x0, 0x27) /* IVB+ */
+#define OP_3DSTATE_BINDING_TABLE_POINTERS_DS    OP_3D_MEDIA(0x3, 0x0, 0x28) /* IVB+ */
+#define OP_3DSTATE_BINDING_TABLE_POINTERS_GS    OP_3D_MEDIA(0x3, 0x0, 0x29) /* IVB+ */
+#define OP_3DSTATE_BINDING_TABLE_POINTERS_PS    OP_3D_MEDIA(0x3, 0x0, 0x2A) /* IVB+ */
+#define OP_3DSTATE_SAMPLER_STATE_POINTERS_VS    OP_3D_MEDIA(0x3, 0x0, 0x2B) /* IVB+ */
+#define OP_3DSTATE_SAMPLER_STATE_POINTERS_HS    OP_3D_MEDIA(0x3, 0x0, 0x2C) /* IVB+ */
+#define OP_3DSTATE_SAMPLER_STATE_POINTERS_DS    OP_3D_MEDIA(0x3, 0x0, 0x2D) /* IVB+ */
+#define OP_3DSTATE_SAMPLER_STATE_POINTERS_GS    OP_3D_MEDIA(0x3, 0x0, 0x2E) /* IVB+ */
+#define OP_3DSTATE_SAMPLER_STATE_POINTERS_PS    OP_3D_MEDIA(0x3, 0x0, 0x2F) /* IVB+ */
+#define OP_3DSTATE_URB_VS                       OP_3D_MEDIA(0x3, 0x0, 0x30) /* IVB+ */
+#define OP_3DSTATE_URB_HS                       OP_3D_MEDIA(0x3, 0x0, 0x31) /* IVB+ */
+#define OP_3DSTATE_URB_DS                       OP_3D_MEDIA(0x3, 0x0, 0x32) /* IVB+ */
+#define OP_3DSTATE_URB_GS                       OP_3D_MEDIA(0x3, 0x0, 0x33) /* IVB+ */
+#define OP_3DSTATE_GATHER_CONSTANT_VS           OP_3D_MEDIA(0x3, 0x0, 0x34) /* HSW+ */
+#define OP_3DSTATE_GATHER_CONSTANT_GS           OP_3D_MEDIA(0x3, 0x0, 0x35) /* HSW+ */
+#define OP_3DSTATE_GATHER_CONSTANT_HS           OP_3D_MEDIA(0x3, 0x0, 0x36) /* HSW+ */
+#define OP_3DSTATE_GATHER_CONSTANT_DS           OP_3D_MEDIA(0x3, 0x0, 0x37) /* HSW+ */
+#define OP_3DSTATE_GATHER_CONSTANT_PS           OP_3D_MEDIA(0x3, 0x0, 0x38) /* HSW+ */
+#define OP_3DSTATE_DX9_CONSTANTF_VS             OP_3D_MEDIA(0x3, 0x0, 0x39) /* HSW+ */
+#define OP_3DSTATE_DX9_CONSTANTF_PS             OP_3D_MEDIA(0x3, 0x0, 0x3A) /* HSW+ */
+#define OP_3DSTATE_DX9_CONSTANTI_VS             OP_3D_MEDIA(0x3, 0x0, 0x3B) /* HSW+ */
+#define OP_3DSTATE_DX9_CONSTANTI_PS             OP_3D_MEDIA(0x3, 0x0, 0x3C) /* HSW+ */
+#define OP_3DSTATE_DX9_CONSTANTB_VS             OP_3D_MEDIA(0x3, 0x0, 0x3D) /* HSW+ */
+#define OP_3DSTATE_DX9_CONSTANTB_PS             OP_3D_MEDIA(0x3, 0x0, 0x3E) /* HSW+ */
+#define OP_3DSTATE_DX9_LOCAL_VALID_VS           OP_3D_MEDIA(0x3, 0x0, 0x3F) /* HSW+ */
+#define OP_3DSTATE_DX9_LOCAL_VALID_PS           OP_3D_MEDIA(0x3, 0x0, 0x40) /* HSW+ */
+#define OP_3DSTATE_DX9_GENERATE_ACTIVE_VS       OP_3D_MEDIA(0x3, 0x0, 0x41) /* HSW+ */
+#define OP_3DSTATE_DX9_GENERATE_ACTIVE_PS       OP_3D_MEDIA(0x3, 0x0, 0x42) /* HSW+ */
+#define OP_3DSTATE_BINDING_TABLE_EDIT_VS        OP_3D_MEDIA(0x3, 0x0, 0x43) /* HSW+ */
+#define OP_3DSTATE_BINDING_TABLE_EDIT_GS        OP_3D_MEDIA(0x3, 0x0, 0x44) /* HSW+ */
+#define OP_3DSTATE_BINDING_TABLE_EDIT_HS        OP_3D_MEDIA(0x3, 0x0, 0x45) /* HSW+ */
+#define OP_3DSTATE_BINDING_TABLE_EDIT_DS        OP_3D_MEDIA(0x3, 0x0, 0x46) /* HSW+ */
+#define OP_3DSTATE_BINDING_TABLE_EDIT_PS        OP_3D_MEDIA(0x3, 0x0, 0x47) /* HSW+ */
+
+#define OP_3DSTATE_VF_INSTANCING 		OP_3D_MEDIA(0x3, 0x0, 0x49) /* BDW+ */
+#define OP_3DSTATE_VF_SGVS  			OP_3D_MEDIA(0x3, 0x0, 0x4A) /* BDW+ */
+#define OP_3DSTATE_VF_TOPOLOGY   		OP_3D_MEDIA(0x3, 0x0, 0x4B) /* BDW+ */
+#define OP_3DSTATE_WM_CHROMAKEY   		OP_3D_MEDIA(0x3, 0x0, 0x4C) /* BDW+ */
+#define OP_3DSTATE_PS_BLEND   			OP_3D_MEDIA(0x3, 0x0, 0x4D) /* BDW+ */
+#define OP_3DSTATE_WM_DEPTH_STENCIL   		OP_3D_MEDIA(0x3, 0x0, 0x4E) /* BDW+ */
+#define OP_3DSTATE_PS_EXTRA   			OP_3D_MEDIA(0x3, 0x0, 0x4F) /* BDW+ */
+#define OP_3DSTATE_RASTER   			OP_3D_MEDIA(0x3, 0x0, 0x50) /* BDW+ */
+#define OP_3DSTATE_SBE_SWIZ   			OP_3D_MEDIA(0x3, 0x0, 0x51) /* BDW+ */
+#define OP_3DSTATE_WM_HZ_OP   			OP_3D_MEDIA(0x3, 0x0, 0x52) /* BDW+ */
+#define OP_3DSTATE_COMPONENT_PACKING		OP_3D_MEDIA(0x3, 0x0, 0x55) /* SKL+ */
+
+#define OP_3DSTATE_DRAWING_RECTANGLE            OP_3D_MEDIA(0x3, 0x1, 0x00)
+#define OP_3DSTATE_SAMPLER_PALETTE_LOAD0        OP_3D_MEDIA(0x3, 0x1, 0x02)
+#define OP_3DSTATE_CHROMA_KEY                   OP_3D_MEDIA(0x3, 0x1, 0x04)
+#define OP_SNB_3DSTATE_DEPTH_BUFFER             OP_3D_MEDIA(0x3, 0x1, 0x05)
+#define OP_3DSTATE_POLY_STIPPLE_OFFSET          OP_3D_MEDIA(0x3, 0x1, 0x06)
+#define OP_3DSTATE_POLY_STIPPLE_PATTERN         OP_3D_MEDIA(0x3, 0x1, 0x07)
+#define OP_3DSTATE_LINE_STIPPLE                 OP_3D_MEDIA(0x3, 0x1, 0x08)
+#define OP_3DSTATE_AA_LINE_PARAMS               OP_3D_MEDIA(0x3, 0x1, 0x0A)
+#define OP_3DSTATE_GS_SVB_INDEX                 OP_3D_MEDIA(0x3, 0x1, 0x0B)
+#define OP_3DSTATE_SAMPLER_PALETTE_LOAD1        OP_3D_MEDIA(0x3, 0x1, 0x0C)
+#define OP_3DSTATE_MULTISAMPLE_BDW		OP_3D_MEDIA(0x3, 0x0, 0x0D)
+#define OP_SNB_3DSTATE_STENCIL_BUFFER           OP_3D_MEDIA(0x3, 0x1, 0x0E)
+#define OP_SNB_3DSTATE_HIER_DEPTH_BUFFER        OP_3D_MEDIA(0x3, 0x1, 0x0F)
+#define OP_SNB_3DSTATE_CLEAR_PARAMS             OP_3D_MEDIA(0x3, 0x1, 0x10)
+#define OP_3DSTATE_MONOFILTER_SIZE              OP_3D_MEDIA(0x3, 0x1, 0x11)
+#define OP_3DSTATE_PUSH_CONSTANT_ALLOC_VS       OP_3D_MEDIA(0x3, 0x1, 0x12) /* IVB+ */
+#define OP_3DSTATE_PUSH_CONSTANT_ALLOC_HS       OP_3D_MEDIA(0x3, 0x1, 0x13) /* IVB+ */
+#define OP_3DSTATE_PUSH_CONSTANT_ALLOC_DS       OP_3D_MEDIA(0x3, 0x1, 0x14) /* IVB+ */
+#define OP_3DSTATE_PUSH_CONSTANT_ALLOC_GS       OP_3D_MEDIA(0x3, 0x1, 0x15) /* IVB+ */
+#define OP_3DSTATE_PUSH_CONSTANT_ALLOC_PS       OP_3D_MEDIA(0x3, 0x1, 0x16) /* IVB+ */
+#define OP_3DSTATE_SO_DECL_LIST                 OP_3D_MEDIA(0x3, 0x1, 0x17)
+#define OP_3DSTATE_SO_BUFFER                    OP_3D_MEDIA(0x3, 0x1, 0x18)
+#define OP_3DSTATE_BINDING_TABLE_POOL_ALLOC     OP_3D_MEDIA(0x3, 0x1, 0x19) /* HSW+ */
+#define OP_3DSTATE_GATHER_POOL_ALLOC            OP_3D_MEDIA(0x3, 0x1, 0x1A) /* HSW+ */
+#define OP_3DSTATE_DX9_CONSTANT_BUFFER_POOL_ALLOC OP_3D_MEDIA(0x3, 0x1, 0x1B) /* HSW+ */
+#define OP_3DSTATE_SAMPLE_PATTERN               OP_3D_MEDIA(0x3, 0x1, 0x1C)
+#define OP_PIPE_CONTROL                         OP_3D_MEDIA(0x3, 0x2, 0x00)
+#define OP_3DPRIMITIVE                          OP_3D_MEDIA(0x3, 0x3, 0x00)
+
+/* VCCP Command Parser */
+
+/*
+ * Below MFX and VBE cmd definition is from vaapi intel driver project (BSD License)
+ * git://anongit.freedesktop.org/vaapi/intel-driver
+ * src/i965_defines.h
+ *
+ */
+
+#define OP_MFX(pipeline, op, sub_opa, sub_opb)     \
+	(3 << 13 | \
+	 (pipeline) << 11 | \
+	 (op) << 8 | \
+	 (sub_opa) << 5 | \
+	 (sub_opb))
+
+#define OP_MFX_PIPE_MODE_SELECT                    OP_MFX(2, 0, 0, 0)  /* ALL */
+#define OP_MFX_SURFACE_STATE                       OP_MFX(2, 0, 0, 1)  /* ALL */
+#define OP_MFX_PIPE_BUF_ADDR_STATE                 OP_MFX(2, 0, 0, 2)  /* ALL */
+#define OP_MFX_IND_OBJ_BASE_ADDR_STATE             OP_MFX(2, 0, 0, 3)  /* ALL */
+#define OP_MFX_BSP_BUF_BASE_ADDR_STATE             OP_MFX(2, 0, 0, 4)  /* ALL */
+#define OP_2_0_0_5                                 OP_MFX(2, 0, 0, 5)  /* ALL */
+#define OP_MFX_STATE_POINTER                       OP_MFX(2, 0, 0, 6)  /* ALL */
+#define OP_MFX_QM_STATE                            OP_MFX(2, 0, 0, 7)  /* IVB+ */
+#define OP_MFX_FQM_STATE                           OP_MFX(2, 0, 0, 8)  /* IVB+ */
+#define OP_MFX_PAK_INSERT_OBJECT                   OP_MFX(2, 0, 2, 8)  /* IVB+ */
+#define OP_MFX_STITCH_OBJECT                       OP_MFX(2, 0, 2, 0xA)  /* IVB+ */
+
+#define OP_MFD_IT_OBJECT                           OP_MFX(2, 0, 1, 9) /* ALL */
+
+#define OP_MFX_WAIT                                OP_MFX(1, 0, 0, 0) /* IVB+ */
+#define OP_MFX_AVC_IMG_STATE                       OP_MFX(2, 1, 0, 0) /* ALL */
+#define OP_MFX_AVC_QM_STATE                        OP_MFX(2, 1, 0, 1) /* ALL */
+#define OP_MFX_AVC_DIRECTMODE_STATE                OP_MFX(2, 1, 0, 2) /* ALL */
+#define OP_MFX_AVC_SLICE_STATE                     OP_MFX(2, 1, 0, 3) /* ALL */
+#define OP_MFX_AVC_REF_IDX_STATE                   OP_MFX(2, 1, 0, 4) /* ALL */
+#define OP_MFX_AVC_WEIGHTOFFSET_STATE              OP_MFX(2, 1, 0, 5) /* ALL */
+#define OP_MFD_AVC_PICID_STATE                     OP_MFX(2, 1, 1, 5) /* HSW+ */
+#define OP_MFD_AVC_DPB_STATE			   OP_MFX(2, 1, 1, 6) /* IVB+ */
+#define OP_MFD_AVC_SLICEADDR                       OP_MFX(2, 1, 1, 7) /* IVB+ */
+#define OP_MFD_AVC_BSD_OBJECT                      OP_MFX(2, 1, 1, 8) /* ALL */
+#define OP_MFC_AVC_PAK_OBJECT                      OP_MFX(2, 1, 2, 9) /* ALL */
+
+#define OP_MFX_VC1_PRED_PIPE_STATE                 OP_MFX(2, 2, 0, 1) /* ALL */
+#define OP_MFX_VC1_DIRECTMODE_STATE                OP_MFX(2, 2, 0, 2) /* ALL */
+#define OP_MFD_VC1_SHORT_PIC_STATE                 OP_MFX(2, 2, 1, 0) /* IVB+ */
+#define OP_MFD_VC1_LONG_PIC_STATE                  OP_MFX(2, 2, 1, 1) /* IVB+ */
+#define OP_MFD_VC1_BSD_OBJECT                      OP_MFX(2, 2, 1, 8) /* ALL */
+
+#define OP_MFX_MPEG2_PIC_STATE                     OP_MFX(2, 3, 0, 0) /* ALL */
+#define OP_MFX_MPEG2_QM_STATE                      OP_MFX(2, 3, 0, 1) /* ALL */
+#define OP_MFD_MPEG2_BSD_OBJECT                    OP_MFX(2, 3, 1, 8) /* ALL */
+#define OP_MFC_MPEG2_SLICEGROUP_STATE              OP_MFX(2, 3, 2, 3) /* ALL */
+#define OP_MFC_MPEG2_PAK_OBJECT                    OP_MFX(2, 3, 2, 9) /* ALL */
+
+#define OP_MFX_2_6_0_0                             OP_MFX(2, 6, 0, 0) /* IVB+ */
+#define OP_MFX_2_6_0_8                             OP_MFX(2, 6, 0, 8) /* IVB+ */
+#define OP_MFX_2_6_0_9                             OP_MFX(2, 6, 0, 9) /* IVB+ */
+
+#define OP_MFX_JPEG_PIC_STATE                      OP_MFX(2, 7, 0, 0)
+#define OP_MFX_JPEG_HUFF_TABLE_STATE               OP_MFX(2, 7, 0, 2)
+#define OP_MFD_JPEG_BSD_OBJECT                     OP_MFX(2, 7, 1, 8)
+
+#define OP_VEB(pipeline, op, sub_opa, sub_opb) \
+	(3 << 13 | \
+	 (pipeline) << 11 | \
+	 (op) << 8 | \
+	 (sub_opa) << 5 | \
+	 (sub_opb))
+
+#define OP_VEB_SURFACE_STATE                       OP_VEB(2, 4, 0, 0)
+#define OP_VEB_STATE                               OP_VEB(2, 4, 0, 2)
+#define OP_VEB_DNDI_IECP_STATE                     OP_VEB(2, 4, 0, 3)
+
+struct parser_exec_state;
+
+typedef int (*parser_cmd_handler)(struct parser_exec_state *s);
+
+#define GVT_CMD_HASH_BITS   7
+
+/* which DWords need address fix */
+#define ADDR_FIX_1(x1)			(1 << (x1))
+#define ADDR_FIX_2(x1, x2)		(ADDR_FIX_1(x1) | ADDR_FIX_1(x2))
+#define ADDR_FIX_3(x1, x2, x3)		(ADDR_FIX_1(x1) | ADDR_FIX_2(x2, x3))
+#define ADDR_FIX_4(x1, x2, x3, x4)	(ADDR_FIX_1(x1) | ADDR_FIX_3(x2, x3, x4))
+#define ADDR_FIX_5(x1, x2, x3, x4, x5)  (ADDR_FIX_1(x1) | ADDR_FIX_4(x2, x3, x4, x5))
+
+struct cmd_info {
+	char *name;
+	u32 opcode;
+
+#define F_LEN_MASK	(1U<<0)
+#define F_LEN_CONST  1U
+#define F_LEN_VAR    0U
+
+/*
+ * command has its own ip advance logic
+ * e.g. MI_BATCH_START, MI_BATCH_END
+ */
+#define F_IP_ADVANCE_CUSTOM (1<<1)
+
+#define F_POST_HANDLE	(1<<2)
+	u32 flag;
+
+#define R_RCS	(1 << RCS)
+#define R_VCS1  (1 << VCS)
+#define R_VCS2  (1 << VCS2)
+#define R_VCS	(R_VCS1 | R_VCS2)
+#define R_BCS	(1 << BCS)
+#define R_VECS	(1 << VECS)
+#define R_ALL (R_RCS | R_VCS | R_BCS | R_VECS)
+	/* rings that support this cmd: BLT/RCS/VCS/VECS */
+	uint16_t rings;
+
+	/* devices that support this cmd: SNB/IVB/HSW/... */
+	uint16_t devices;
+
+	/* which DWords are address that need fix up.
+	 * bit 0 means a 32-bit non address operand in command
+	 * bit 1 means address operand, which could be 32-bit
+	 * or 64-bit depending on different architectures.(
+	 * defined by "gmadr_bytes_in_cmd" in intel_gvt.
+	 * No matter the address length, each address only takes
+	 * one bit in the bitmap.
+	 */
+	uint16_t addr_bitmap;
+
+	/* flag == F_LEN_CONST : command length
+	 * flag == F_LEN_VAR : length bias bits
+	 * Note: length is in DWord
+	 */
+	uint8_t	len;
+
+	parser_cmd_handler handler;
+};
+
+struct cmd_entry {
+	struct hlist_node hlist;
+	struct cmd_info *info;
+};
+
+enum {
+	RING_BUFFER_INSTRUCTION,
+	BATCH_BUFFER_INSTRUCTION,
+	BATCH_BUFFER_2ND_LEVEL,
+};
+
+enum {
+	GTT_BUFFER,
+	PPGTT_BUFFER
+};
+
+struct parser_exec_state {
+	struct intel_vgpu *vgpu;
+	int ring_id;
+
+	int buf_type;
+
+	/* batch buffer address type */
+	int buf_addr_type;
+
+	/* graphics memory address of ring buffer start */
+	unsigned long ring_start;
+	unsigned long ring_size;
+	unsigned long ring_head;
+	unsigned long ring_tail;
+
+	/* instruction graphics memory address */
+	unsigned long ip_gma;
+
+	/* mapped va of the instr_gma */
+	void *ip_va;
+	void *rb_va;
+
+	void *ret_bb_va;
+	/* next instruction when return from  batch buffer to ring buffer */
+	unsigned long ret_ip_gma_ring;
+
+	/* next instruction when return from 2nd batch buffer to batch buffer */
+	unsigned long ret_ip_gma_bb;
+
+	/* batch buffer address type (GTT or PPGTT)
+	 * used when ret from 2nd level batch buffer
+	 */
+	int saved_buf_addr_type;
+
+	struct cmd_info *info;
+
+	struct intel_vgpu_workload *workload;
+};
+
+#define gmadr_dw_number(s)	\
+	(s->vgpu->gvt->device_info.gmadr_bytes_in_cmd >> 2)
+
+static unsigned long bypass_scan_mask = 0;
+static bool bypass_batch_buffer_scan = true;
+
+/* ring ALL, type = 0 */
+static struct sub_op_bits sub_op_mi[] = {
+	{31, 29},
+	{28, 23},
+};
+
+static struct decode_info decode_info_mi = {
+	"MI",
+	OP_LEN_MI,
+	ARRAY_SIZE(sub_op_mi),
+	sub_op_mi,
+};
+
+/* ring RCS, command type 2 */
+static struct sub_op_bits sub_op_2d[] = {
+	{31, 29},
+	{28, 22},
+};
+
+static struct decode_info decode_info_2d = {
+	"2D",
+	OP_LEN_2D,
+	ARRAY_SIZE(sub_op_2d),
+	sub_op_2d,
+};
+
+/* ring RCS, command type 3 */
+static struct sub_op_bits sub_op_3d_media[] = {
+	{31, 29},
+	{28, 27},
+	{26, 24},
+	{23, 16},
+};
+
+static struct decode_info decode_info_3d_media = {
+	"3D_Media",
+	OP_LEN_3D_MEDIA,
+	ARRAY_SIZE(sub_op_3d_media),
+	sub_op_3d_media,
+};
+
+/* ring VCS, command type 3 */
+static struct sub_op_bits sub_op_mfx_vc[] = {
+	{31, 29},
+	{28, 27},
+	{26, 24},
+	{23, 21},
+	{20, 16},
+};
+
+static struct decode_info decode_info_mfx_vc = {
+	"MFX_VC",
+	OP_LEN_MFX_VC,
+	ARRAY_SIZE(sub_op_mfx_vc),
+	sub_op_mfx_vc,
+};
+
+/* ring VECS, command type 3 */
+static struct sub_op_bits sub_op_vebox[] = {
+	{31, 29},
+	{28, 27},
+	{26, 24},
+	{23, 21},
+	{20, 16},
+};
+
+static struct decode_info decode_info_vebox = {
+	"VEBOX",
+	OP_LEN_VEBOX,
+	ARRAY_SIZE(sub_op_vebox),
+	sub_op_vebox,
+};
+
+static struct decode_info *ring_decode_info[I915_NUM_ENGINES][8] = {
+	[RCS] = {
+		&decode_info_mi,
+		NULL,
+		NULL,
+		&decode_info_3d_media,
+		NULL,
+		NULL,
+		NULL,
+		NULL,
+	},
+
+	[VCS] = {
+		&decode_info_mi,
+		NULL,
+		NULL,
+		&decode_info_mfx_vc,
+		NULL,
+		NULL,
+		NULL,
+		NULL,
+	},
+
+	[BCS] = {
+		&decode_info_mi,
+		NULL,
+		&decode_info_2d,
+		NULL,
+		NULL,
+		NULL,
+		NULL,
+		NULL,
+	},
+
+	[VECS] = {
+		&decode_info_mi,
+		NULL,
+		NULL,
+		&decode_info_vebox,
+		NULL,
+		NULL,
+		NULL,
+		NULL,
+	},
+
+	[VCS2] = {
+		&decode_info_mi,
+		NULL,
+		NULL,
+		&decode_info_mfx_vc,
+		NULL,
+		NULL,
+		NULL,
+		NULL,
+	},
+};
+
+static inline u32 get_opcode(u32 cmd, int ring_id)
+{
+	struct decode_info *d_info;
+
+	if (ring_id >= I915_NUM_ENGINES)
+		return INVALID_OP;
+
+	d_info = ring_decode_info[ring_id][CMD_TYPE(cmd)];
+	if (d_info == NULL)
+		return INVALID_OP;
+
+	return cmd >> (32 - d_info->op_len);
+}
+
+static inline struct cmd_info *find_cmd_entry(struct intel_gvt *gvt,
+		unsigned int opcode, int ring_id)
+{
+	struct cmd_entry *e;
+
+	hash_for_each_possible(gvt->cmd_table, e, hlist, opcode) {
+		if ((opcode == e->info->opcode) &&
+				(e->info->rings & (1 << ring_id)))
+			return e->info;
+	}
+	return NULL;
+}
+
+static inline struct cmd_info *get_cmd_info(struct intel_gvt *gvt,
+		u32 cmd, int ring_id)
+{
+	u32 opcode;
+
+	opcode = get_opcode(cmd, ring_id);
+	if (opcode == INVALID_OP)
+		return NULL;
+
+	return find_cmd_entry(gvt, opcode, ring_id);
+}
+
+static inline u32 sub_op_val(u32 cmd, u32 hi, u32 low)
+{
+	return (cmd >> low) & ((1U << (hi - low + 1)) - 1);
+}
+
+static inline void print_opcode(u32 cmd, int ring_id)
+{
+	struct decode_info *d_info;
+	int i;
+
+	if (ring_id >= I915_NUM_ENGINES)
+		return;
+
+	d_info = ring_decode_info[ring_id][CMD_TYPE(cmd)];
+	if (d_info == NULL)
+		return;
+
+	gvt_err("opcode=0x%x %s sub_ops:",
+			cmd >> (32 - d_info->op_len), d_info->name);
+
+	for (i = 0; i < d_info->nr_sub_op; i++)
+		pr_err("0x%x ", sub_op_val(cmd, d_info->sub_op[i].hi,
+					d_info->sub_op[i].low));
+
+	pr_err("\n");
+}
+
+static inline u32 *cmd_ptr(struct parser_exec_state *s, int index)
+{
+	return s->ip_va + (index << 2);
+}
+
+static inline u32 cmd_val(struct parser_exec_state *s, int index)
+{
+	return *cmd_ptr(s, index);
+}
+
+static void parser_exec_state_dump(struct parser_exec_state *s)
+{
+	int cnt = 0;
+	int i;
+
+	gvt_err("  vgpu%d RING%d: ring_start(%08lx) ring_end(%08lx)"
+			" ring_head(%08lx) ring_tail(%08lx)\n", s->vgpu->id,
+			s->ring_id, s->ring_start, s->ring_start + s->ring_size,
+			s->ring_head, s->ring_tail);
+
+	gvt_err("  %s %s ip_gma(%08lx) ",
+			s->buf_type == RING_BUFFER_INSTRUCTION ?
+			"RING_BUFFER" : "BATCH_BUFFER",
+			s->buf_addr_type == GTT_BUFFER ?
+			"GTT" : "PPGTT", s->ip_gma);
+
+	if (s->ip_va == NULL) {
+		gvt_err(" ip_va(NULL)");
+		return;
+	}
+
+	gvt_err("  ip_va=%p: %08x %08x %08x %08x\n",
+			s->ip_va, cmd_val(s, 0), cmd_val(s, 1),
+			cmd_val(s, 2), cmd_val(s, 3));
+
+	print_opcode(cmd_val(s, 0), s->ring_id);
+
+	/* print the whole page to trace */
+	pr_err("    ip_va=%p: %08x %08x %08x %08x\n",
+			s->ip_va, cmd_val(s, 0), cmd_val(s, 1),
+			cmd_val(s, 2), cmd_val(s, 3));
+
+	s->ip_va = (u32 *)((((u64)s->ip_va) >> 12) << 12);
+
+	while (cnt < 1024) {
+		pr_err("ip_va=%p: ", s->ip_va);
+		for (i = 0; i < 8; i++)
+			pr_err("%08x ", cmd_val(s, i));
+		pr_err("\n");
+
+		s->ip_va += 8 * sizeof(u32);
+		cnt += 8;
+	}
+}
+
+static inline void update_ip_va(struct parser_exec_state *s)
+{
+	unsigned long len = 0;
+
+	if (WARN_ON(s->ring_head == s->ring_tail))
+		return;
+
+	if (s->buf_type == RING_BUFFER_INSTRUCTION) {
+		unsigned long ring_top = s->ring_start + s->ring_size;
+
+		if (s->ring_head > s->ring_tail) {
+			if (s->ip_gma >= s->ring_head && s->ip_gma < ring_top)
+				len = (s->ip_gma - s->ring_head);
+			else if (s->ip_gma >= s->ring_start &&
+					s->ip_gma <= s->ring_tail)
+				len = (ring_top - s->ring_head) +
+					(s->ip_gma - s->ring_start);
+		} else
+			len = (s->ip_gma - s->ring_head);
+
+		s->ip_va = s->rb_va + len;
+	} else {/* shadow batch buffer */
+		s->ip_va = s->ret_bb_va;
+	}
+}
+
+static inline int ip_gma_set(struct parser_exec_state *s,
+		unsigned long ip_gma)
+{
+	WARN_ON(!IS_ALIGNED(ip_gma, 4));
+
+	s->ip_gma = ip_gma;
+	update_ip_va(s);
+	return 0;
+}
+
+static inline int ip_gma_advance(struct parser_exec_state *s,
+		unsigned int dw_len)
+{
+	s->ip_gma += (dw_len << 2);
+
+	if (s->buf_type == RING_BUFFER_INSTRUCTION) {
+		if (s->ip_gma >= s->ring_start + s->ring_size)
+			s->ip_gma -= s->ring_size;
+		update_ip_va(s);
+	} else {
+		s->ip_va += (dw_len << 2);
+	}
+
+	return 0;
+}
+
+static inline int get_cmd_length(struct cmd_info *info, u32 cmd)
+{
+	if ((info->flag & F_LEN_MASK) == F_LEN_CONST)
+		return info->len;
+	else
+		return (cmd & ((1U << info->len) - 1)) + 2;
+	return 0;
+}
+
+static inline int cmd_length(struct parser_exec_state *s)
+{
+	return get_cmd_length(s->info, cmd_val(s, 0));
+}
+
+/* do not remove this, some platform may need clflush here */
+#define patch_value(s, addr, val) do { \
+	*addr = val; \
+} while (0)
+
+static bool is_shadowed_mmio(unsigned int offset)
+{
+	bool ret = false;
+
+	if ((offset == 0x2168) || /*BB current head register UDW */
+	    (offset == 0x2140) || /*BB current header register */
+	    (offset == 0x211c) || /*second BB header register UDW */
+	    (offset == 0x2114)) { /*second BB header register UDW */
+		ret = true;
+	}
+	return ret;
+}
+
+static int cmd_reg_handler(struct parser_exec_state *s,
+	unsigned int offset, unsigned int index, char *cmd)
+{
+	struct intel_vgpu *vgpu = s->vgpu;
+	struct intel_gvt *gvt = vgpu->gvt;
+
+	if (offset + 4 > gvt->device_info.mmio_size) {
+		gvt_err("%s access to (%x) outside of MMIO range\n",
+				cmd, offset);
+		return -EINVAL;
+	}
+
+	if (!intel_gvt_mmio_is_cmd_access(gvt, offset)) {
+		gvt_err("vgpu%d: %s access to non-render register (%x)\n",
+				s->vgpu->id, cmd, offset);
+		return 0;
+	}
+
+	if (is_shadowed_mmio(offset)) {
+		gvt_err("vgpu%d: found access of shadowed MMIO %x\n",
+				s->vgpu->id, offset);
+		return 0;
+	}
+
+	if (offset == i915_mmio_reg_offset(DERRMR) ||
+		offset == i915_mmio_reg_offset(FORCEWAKE_MT)) {
+		/* Writing to HW VGT_PVINFO_PAGE offset will be discarded */
+		patch_value(s, cmd_ptr(s, index), VGT_PVINFO_PAGE);
+	}
+
+	/* TODO: Update the global mask if this MMIO is a masked-MMIO */
+	intel_gvt_mmio_set_cmd_accessed(gvt, offset);
+	return 0;
+}
+
+#define cmd_reg(s, i) \
+	(cmd_val(s, i) & GENMASK(22, 2))
+
+#define cmd_reg_inhibit(s, i) \
+	(cmd_val(s, i) & GENMASK(22, 18))
+
+#define cmd_gma(s, i) \
+	(cmd_val(s, i) & GENMASK(31, 2))
+
+#define cmd_gma_hi(s, i) \
+	(cmd_val(s, i) & GENMASK(15, 0))
+
+static int cmd_handler_lri(struct parser_exec_state *s)
+{
+	int i, ret = 0;
+	int cmd_len = cmd_length(s);
+	struct intel_gvt *gvt = s->vgpu->gvt;
+
+	for (i = 1; i < cmd_len; i += 2) {
+		if (IS_BROADWELL(gvt->dev_priv) &&
+				(s->ring_id != RCS)) {
+			if (s->ring_id == BCS &&
+					cmd_reg(s, i) ==
+					i915_mmio_reg_offset(DERRMR))
+				ret |= 0;
+			else
+				ret |= (cmd_reg_inhibit(s, i)) ? -EINVAL : 0;
+		}
+		if (ret)
+			break;
+		ret |= cmd_reg_handler(s, cmd_reg(s, i), i, "lri");
+	}
+	return ret;
+}
+
+static int cmd_handler_lrr(struct parser_exec_state *s)
+{
+	int i, ret = 0;
+	int cmd_len = cmd_length(s);
+
+	for (i = 1; i < cmd_len; i += 2) {
+		if (IS_BROADWELL(s->vgpu->gvt->dev_priv))
+			ret |= ((cmd_reg_inhibit(s, i) ||
+					(cmd_reg_inhibit(s, i + 1)))) ?
+				-EINVAL : 0;
+		if (ret)
+			break;
+		ret |= cmd_reg_handler(s, cmd_reg(s, i), i, "lrr-src");
+		ret |= cmd_reg_handler(s, cmd_reg(s, i + 1), i, "lrr-dst");
+	}
+	return ret;
+}
+
+static inline int cmd_address_audit(struct parser_exec_state *s,
+		unsigned long guest_gma, int op_size, bool index_mode);
+
+static int cmd_handler_lrm(struct parser_exec_state *s)
+{
+	struct intel_gvt *gvt = s->vgpu->gvt;
+	int gmadr_bytes = gvt->device_info.gmadr_bytes_in_cmd;
+	unsigned long gma;
+	int i, ret = 0;
+	int cmd_len = cmd_length(s);
+
+	for (i = 1; i < cmd_len;) {
+		if (IS_BROADWELL(gvt->dev_priv))
+			ret |= (cmd_reg_inhibit(s, i)) ? -EINVAL : 0;
+		if (ret)
+			break;
+		ret |= cmd_reg_handler(s, cmd_reg(s, i), i, "lrm");
+		if (cmd_val(s, 0) & (1 << 22)) {
+			gma = cmd_gma(s, i + 1);
+			if (gmadr_bytes == 8)
+				gma |= (cmd_gma_hi(s, i + 2)) << 32;
+			ret |= cmd_address_audit(s, gma, sizeof(u32), false);
+		}
+		i += gmadr_dw_number(s) + 1;
+	}
+	return ret;
+}
+
+static int cmd_handler_srm(struct parser_exec_state *s)
+{
+	int gmadr_bytes = s->vgpu->gvt->device_info.gmadr_bytes_in_cmd;
+	unsigned long gma;
+	int i, ret = 0;
+	int cmd_len = cmd_length(s);
+
+	for (i = 1; i < cmd_len;) {
+		ret |= cmd_reg_handler(s, cmd_reg(s, i), i, "srm");
+		if (cmd_val(s, 0) & (1 << 22)) {
+			gma = cmd_gma(s, i + 1);
+			if (gmadr_bytes == 8)
+				gma |= (cmd_gma_hi(s, i + 2)) << 32;
+			ret |= cmd_address_audit(s, gma, sizeof(u32), false);
+		}
+		i += gmadr_dw_number(s) + 1;
+	}
+	return ret;
+}
+
+struct cmd_interrupt_event {
+	int pipe_control_notify;
+	int mi_flush_dw;
+	int mi_user_interrupt;
+};
+
+static struct cmd_interrupt_event cmd_interrupt_events[] = {
+	[RCS] = {
+		.pipe_control_notify = RCS_PIPE_CONTROL,
+		.mi_flush_dw = INTEL_GVT_EVENT_RESERVED,
+		.mi_user_interrupt = RCS_MI_USER_INTERRUPT,
+	},
+	[BCS] = {
+		.pipe_control_notify = INTEL_GVT_EVENT_RESERVED,
+		.mi_flush_dw = BCS_MI_FLUSH_DW,
+		.mi_user_interrupt = BCS_MI_USER_INTERRUPT,
+	},
+	[VCS] = {
+		.pipe_control_notify = INTEL_GVT_EVENT_RESERVED,
+		.mi_flush_dw = VCS_MI_FLUSH_DW,
+		.mi_user_interrupt = VCS_MI_USER_INTERRUPT,
+	},
+	[VCS2] = {
+		.pipe_control_notify = INTEL_GVT_EVENT_RESERVED,
+		.mi_flush_dw = VCS2_MI_FLUSH_DW,
+		.mi_user_interrupt = VCS2_MI_USER_INTERRUPT,
+	},
+	[VECS] = {
+		.pipe_control_notify = INTEL_GVT_EVENT_RESERVED,
+		.mi_flush_dw = VECS_MI_FLUSH_DW,
+		.mi_user_interrupt = VECS_MI_USER_INTERRUPT,
+	},
+};
+
+static int cmd_handler_pipe_control(struct parser_exec_state *s)
+{
+	int gmadr_bytes = s->vgpu->gvt->device_info.gmadr_bytes_in_cmd;
+	unsigned long gma;
+	bool index_mode = false;
+	unsigned int post_sync;
+	int ret = 0;
+
+	post_sync = (cmd_val(s, 1) & PIPE_CONTROL_POST_SYNC_OP_MASK) >> 14;
+
+	/* LRI post sync */
+	if (cmd_val(s, 1) & PIPE_CONTROL_MMIO_WRITE)
+		ret = cmd_reg_handler(s, cmd_reg(s, 2), 1, "pipe_ctrl");
+	/* post sync */
+	else if (post_sync) {
+		if (post_sync == 2)
+			ret = cmd_reg_handler(s, 0x2350, 1, "pipe_ctrl");
+		else if (post_sync == 3)
+			ret = cmd_reg_handler(s, 0x2358, 1, "pipe_ctrl");
+		else if (post_sync == 1) {
+			/* check ggtt*/
+			if ((cmd_val(s, 2) & (1 << 2))) {
+				gma = cmd_val(s, 2) & GENMASK(31, 3);
+				if (gmadr_bytes == 8)
+					gma |= (cmd_gma_hi(s, 3)) << 32;
+				/* Store Data Index */
+				if (cmd_val(s, 1) & (1 << 21))
+					index_mode = true;
+				ret |= cmd_address_audit(s, gma, sizeof(u64),
+						index_mode);
+			}
+		}
+	}
+
+	if (ret)
+		return ret;
+
+	if (cmd_val(s, 1) & PIPE_CONTROL_NOTIFY)
+		set_bit(cmd_interrupt_events[s->ring_id].pipe_control_notify,
+				s->workload->pending_events);
+	return 0;
+}
+
+static int cmd_handler_mi_user_interrupt(struct parser_exec_state *s)
+{
+	set_bit(cmd_interrupt_events[s->ring_id].mi_user_interrupt,
+			s->workload->pending_events);
+	return 0;
+}
+
+static int cmd_advance_default(struct parser_exec_state *s)
+{
+	return ip_gma_advance(s, cmd_length(s));
+}
+
+static int cmd_handler_mi_batch_buffer_end(struct parser_exec_state *s)
+{
+	int ret;
+
+	if (s->buf_type == BATCH_BUFFER_2ND_LEVEL) {
+		s->buf_type = BATCH_BUFFER_INSTRUCTION;
+		ret = ip_gma_set(s, s->ret_ip_gma_bb);
+		s->buf_addr_type = s->saved_buf_addr_type;
+	} else {
+		s->buf_type = RING_BUFFER_INSTRUCTION;
+		s->buf_addr_type = GTT_BUFFER;
+		if (s->ret_ip_gma_ring >= s->ring_start + s->ring_size)
+			s->ret_ip_gma_ring -= s->ring_size;
+		ret = ip_gma_set(s, s->ret_ip_gma_ring);
+	}
+	return ret;
+}
+
+struct mi_display_flip_command_info {
+	int pipe;
+	int plane;
+	int event;
+	i915_reg_t stride_reg;
+	i915_reg_t ctrl_reg;
+	i915_reg_t surf_reg;
+	u64 stride_val;
+	u64 tile_val;
+	u64 surf_val;
+	bool async_flip;
+};
+
+struct plane_code_mapping {
+	int pipe;
+	int plane;
+	int event;
+};
+
+static int gen8_decode_mi_display_flip(struct parser_exec_state *s,
+		struct mi_display_flip_command_info *info)
+{
+	struct drm_i915_private *dev_priv = s->vgpu->gvt->dev_priv;
+	struct plane_code_mapping gen8_plane_code[] = {
+		[0] = {PIPE_A, PLANE_A, PRIMARY_A_FLIP_DONE},
+		[1] = {PIPE_B, PLANE_A, PRIMARY_B_FLIP_DONE},
+		[2] = {PIPE_A, PLANE_B, SPRITE_A_FLIP_DONE},
+		[3] = {PIPE_B, PLANE_B, SPRITE_B_FLIP_DONE},
+		[4] = {PIPE_C, PLANE_A, PRIMARY_C_FLIP_DONE},
+		[5] = {PIPE_C, PLANE_B, SPRITE_C_FLIP_DONE},
+	};
+	u32 dword0, dword1, dword2;
+	u32 v;
+
+	dword0 = cmd_val(s, 0);
+	dword1 = cmd_val(s, 1);
+	dword2 = cmd_val(s, 2);
+
+	v = (dword0 & GENMASK(21, 19)) >> 19;
+	if (WARN_ON(v >= ARRAY_SIZE(gen8_plane_code)))
+		return -EINVAL;
+
+	info->pipe = gen8_plane_code[v].pipe;
+	info->plane = gen8_plane_code[v].plane;
+	info->event = gen8_plane_code[v].event;
+	info->stride_val = (dword1 & GENMASK(15, 6)) >> 6;
+	info->tile_val = (dword1 & 0x1);
+	info->surf_val = (dword2 & GENMASK(31, 12)) >> 12;
+	info->async_flip = ((dword2 & GENMASK(1, 0)) == 0x1);
+
+	if (info->plane == PLANE_A) {
+		info->ctrl_reg = DSPCNTR(info->pipe);
+		info->stride_reg = DSPSTRIDE(info->pipe);
+		info->surf_reg = DSPSURF(info->pipe);
+	} else if (info->plane == PLANE_B) {
+		info->ctrl_reg = SPRCTL(info->pipe);
+		info->stride_reg = SPRSTRIDE(info->pipe);
+		info->surf_reg = SPRSURF(info->pipe);
+	} else {
+		WARN_ON(1);
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int skl_decode_mi_display_flip(struct parser_exec_state *s,
+		struct mi_display_flip_command_info *info)
+{
+	struct drm_i915_private *dev_priv = s->vgpu->gvt->dev_priv;
+	u32 dword0 = cmd_val(s, 0);
+	u32 dword1 = cmd_val(s, 1);
+	u32 dword2 = cmd_val(s, 2);
+	u32 plane = (dword0 & GENMASK(12, 8)) >> 8;
+
+	switch (plane) {
+	case MI_DISPLAY_FLIP_SKL_PLANE_1_A:
+		info->pipe = PIPE_A;
+		info->event = PRIMARY_A_FLIP_DONE;
+		break;
+	case MI_DISPLAY_FLIP_SKL_PLANE_1_B:
+		info->pipe = PIPE_B;
+		info->event = PRIMARY_B_FLIP_DONE;
+		break;
+	case MI_DISPLAY_FLIP_SKL_PLANE_1_C:
+		info->pipe = PIPE_B;
+		info->event = PRIMARY_C_FLIP_DONE;
+		break;
+	default:
+		gvt_err("unknown plane code %d\n", plane);
+		return -EINVAL;
+	}
+
+	info->pipe = PRIMARY_PLANE;
+	info->stride_val = (dword1 & GENMASK(15, 6)) >> 6;
+	info->tile_val = (dword1 & GENMASK(2, 0));
+	info->surf_val = (dword2 & GENMASK(31, 12)) >> 12;
+	info->async_flip = ((dword2 & GENMASK(1, 0)) == 0x1);
+
+	info->ctrl_reg = DSPCNTR(info->pipe);
+	info->stride_reg = DSPSTRIDE(info->pipe);
+	info->surf_reg = DSPSURF(info->pipe);
+
+	return 0;
+}
+
+static int gen8_check_mi_display_flip(struct parser_exec_state *s,
+		struct mi_display_flip_command_info *info)
+{
+	struct drm_i915_private *dev_priv = s->vgpu->gvt->dev_priv;
+	u32 stride, tile;
+
+	if (!info->async_flip)
+		return 0;
+
+	if (IS_SKYLAKE(dev_priv)) {
+		stride = vgpu_vreg(s->vgpu, info->stride_reg) & GENMASK(9, 0);
+		tile = (vgpu_vreg(s->vgpu, info->ctrl_reg) &
+				GENMASK(12, 10)) >> 10;
+	} else {
+		stride = (vgpu_vreg(s->vgpu, info->stride_reg) &
+				GENMASK(15, 6)) >> 6;
+		tile = (vgpu_vreg(s->vgpu, info->ctrl_reg) & (1 << 10)) >> 10;
+	}
+
+	if (stride != info->stride_val)
+		gvt_dbg_cmd("cannot change stride during async flip\n");
+
+	if (tile != info->tile_val)
+		gvt_dbg_cmd("cannot change tile during async flip\n");
+
+	return 0;
+}
+
+static int gen8_update_plane_mmio_from_mi_display_flip(
+		struct parser_exec_state *s,
+		struct mi_display_flip_command_info *info)
+{
+	struct drm_i915_private *dev_priv = s->vgpu->gvt->dev_priv;
+	struct intel_vgpu *vgpu = s->vgpu;
+
+#define write_bits(reg, e, s, v) do { \
+	vgpu_vreg(vgpu, reg) &= ~GENMASK(e, s); \
+	vgpu_vreg(vgpu, reg) |= (v << s); \
+} while (0)
+
+	write_bits(info->surf_reg, 31, 12, info->surf_val);
+	if (IS_SKYLAKE(dev_priv))
+		write_bits(info->stride_reg, 9, 0, info->stride_val);
+	else
+		write_bits(info->stride_reg, 15, 6, info->stride_val);
+	write_bits(info->ctrl_reg, IS_SKYLAKE(dev_priv) ? 12 : 10,
+		   10, info->tile_val);
+
+#undef write_bits
+
+	vgpu_vreg(vgpu, PIPE_FRMCOUNT_G4X(info->pipe))++;
+	intel_vgpu_trigger_virtual_event(vgpu, info->event);
+	return 0;
+}
+
+static int decode_mi_display_flip(struct parser_exec_state *s,
+		struct mi_display_flip_command_info *info)
+{
+	struct drm_i915_private *dev_priv = s->vgpu->gvt->dev_priv;
+
+	if (IS_BROADWELL(dev_priv))
+		return gen8_decode_mi_display_flip(s, info);
+	if (IS_SKYLAKE(dev_priv))
+		return skl_decode_mi_display_flip(s, info);
+
+	return -ENODEV;
+}
+
+static int check_mi_display_flip(struct parser_exec_state *s,
+		struct mi_display_flip_command_info *info)
+{
+	struct drm_i915_private *dev_priv = s->vgpu->gvt->dev_priv;
+
+	if (IS_BROADWELL(dev_priv) || IS_SKYLAKE(dev_priv))
+		return gen8_check_mi_display_flip(s, info);
+	return -ENODEV;
+}
+
+static int update_plane_mmio_from_mi_display_flip(
+		struct parser_exec_state *s,
+		struct mi_display_flip_command_info *info)
+{
+	struct drm_i915_private *dev_priv = s->vgpu->gvt->dev_priv;
+
+	if (IS_BROADWELL(dev_priv) || IS_SKYLAKE(dev_priv))
+		return gen8_update_plane_mmio_from_mi_display_flip(s, info);
+	return -ENODEV;
+}
+
+static int cmd_handler_mi_display_flip(struct parser_exec_state *s)
+{
+	struct mi_display_flip_command_info info;
+	int ret;
+	int i;
+	int len = cmd_length(s);
+
+	ret = decode_mi_display_flip(s, &info);
+	if (ret) {
+		gvt_err("fail to decode MI display flip command\n");
+		return ret;
+	}
+
+	ret = check_mi_display_flip(s, &info);
+	if (ret) {
+		gvt_err("invalid MI display flip command\n");
+		return ret;
+	}
+
+	ret = update_plane_mmio_from_mi_display_flip(s, &info);
+	if (ret) {
+		gvt_err("fail to update plane mmio\n");
+		return ret;
+	}
+
+	for (i = 0; i < len; i++)
+		patch_value(s, cmd_ptr(s, i), MI_NOOP);
+	return 0;
+}
+
+static bool is_wait_for_flip_pending(u32 cmd)
+{
+	return cmd & (MI_WAIT_FOR_PLANE_A_FLIP_PENDING |
+			MI_WAIT_FOR_PLANE_B_FLIP_PENDING |
+			MI_WAIT_FOR_PLANE_C_FLIP_PENDING |
+			MI_WAIT_FOR_SPRITE_A_FLIP_PENDING |
+			MI_WAIT_FOR_SPRITE_B_FLIP_PENDING |
+			MI_WAIT_FOR_SPRITE_C_FLIP_PENDING);
+}
+
+static int cmd_handler_mi_wait_for_event(struct parser_exec_state *s)
+{
+	u32 cmd = cmd_val(s, 0);
+
+	if (!is_wait_for_flip_pending(cmd))
+		return 0;
+
+	patch_value(s, cmd_ptr(s, 0), MI_NOOP);
+	return 0;
+}
+
+static unsigned long get_gma_bb_from_cmd(struct parser_exec_state *s, int index)
+{
+	unsigned long addr;
+	unsigned long gma_high, gma_low;
+	int gmadr_bytes = s->vgpu->gvt->device_info.gmadr_bytes_in_cmd;
+
+	if (WARN_ON(gmadr_bytes != 4 && gmadr_bytes != 8))
+		return INTEL_GVT_INVALID_ADDR;
+
+	gma_low = cmd_val(s, index) & BATCH_BUFFER_ADDR_MASK;
+	if (gmadr_bytes == 4) {
+		addr = gma_low;
+	} else {
+		gma_high = cmd_val(s, index + 1) & BATCH_BUFFER_ADDR_HIGH_MASK;
+		addr = (((unsigned long)gma_high) << 32) | gma_low;
+	}
+	return addr;
+}
+
+static inline int cmd_address_audit(struct parser_exec_state *s,
+		unsigned long guest_gma, int op_size, bool index_mode)
+{
+	struct intel_vgpu *vgpu = s->vgpu;
+	u32 max_surface_size = vgpu->gvt->device_info.max_surface_size;
+	int i;
+	int ret;
+
+	if (op_size > max_surface_size) {
+		gvt_err("command address audit fail name %s\n", s->info->name);
+		return -EINVAL;
+	}
+
+	if (index_mode)	{
+		if (guest_gma >= GTT_PAGE_SIZE / sizeof(u64)) {
+			ret = -EINVAL;
+			goto err;
+		}
+	} else if ((!vgpu_gmadr_is_valid(s->vgpu, guest_gma)) ||
+			(!vgpu_gmadr_is_valid(s->vgpu,
+					      guest_gma + op_size - 1))) {
+		ret = -EINVAL;
+		goto err;
+	}
+	return 0;
+err:
+	gvt_err("cmd_parser: Malicious %s detected, addr=0x%lx, len=%d!\n",
+			s->info->name, guest_gma, op_size);
+
+	pr_err("cmd dump: ");
+	for (i = 0; i < cmd_length(s); i++) {
+		if (!(i % 4))
+			pr_err("\n%08x ", cmd_val(s, i));
+		else
+			pr_err("%08x ", cmd_val(s, i));
+	}
+	pr_err("\nvgpu%d: aperture 0x%llx - 0x%llx, hidden 0x%llx - 0x%llx\n",
+			vgpu->id,
+			vgpu_aperture_gmadr_base(vgpu),
+			vgpu_aperture_gmadr_end(vgpu),
+			vgpu_hidden_gmadr_base(vgpu),
+			vgpu_hidden_gmadr_end(vgpu));
+	return ret;
+}
+
+static int cmd_handler_mi_store_data_imm(struct parser_exec_state *s)
+{
+	int gmadr_bytes = s->vgpu->gvt->device_info.gmadr_bytes_in_cmd;
+	int op_size = (cmd_length(s) - 3) * sizeof(u32);
+	int core_id = (cmd_val(s, 2) & (1 << 0)) ? 1 : 0;
+	unsigned long gma, gma_low, gma_high;
+	int ret = 0;
+
+	/* check ppggt */
+	if (!(cmd_val(s, 0) & (1 << 22)))
+		return 0;
+
+	gma = cmd_val(s, 2) & GENMASK(31, 2);
+
+	if (gmadr_bytes == 8) {
+		gma_low = cmd_val(s, 1) & GENMASK(31, 2);
+		gma_high = cmd_val(s, 2) & GENMASK(15, 0);
+		gma = (gma_high << 32) | gma_low;
+		core_id = (cmd_val(s, 1) & (1 << 0)) ? 1 : 0;
+	}
+	ret = cmd_address_audit(s, gma + op_size * core_id, op_size, false);
+	return ret;
+}
+
+static inline int unexpected_cmd(struct parser_exec_state *s)
+{
+	gvt_err("vgpu%d: Unexpected %s in command buffer!\n",
+			s->vgpu->id, s->info->name);
+	return -EINVAL;
+}
+
+static int cmd_handler_mi_semaphore_wait(struct parser_exec_state *s)
+{
+	return unexpected_cmd(s);
+}
+
+static int cmd_handler_mi_report_perf_count(struct parser_exec_state *s)
+{
+	return unexpected_cmd(s);
+}
+
+static int cmd_handler_mi_op_2e(struct parser_exec_state *s)
+{
+	return unexpected_cmd(s);
+}
+
+static int cmd_handler_mi_op_2f(struct parser_exec_state *s)
+{
+	int gmadr_bytes = s->vgpu->gvt->device_info.gmadr_bytes_in_cmd;
+	int op_size = ((1 << (cmd_val(s, 0) & GENMASK(20, 19) >> 19)) *
+			sizeof(u32));
+	unsigned long gma, gma_high;
+	int ret = 0;
+
+	if (!(cmd_val(s, 0) & (1 << 22)))
+		return ret;
+
+	gma = cmd_val(s, 1) & GENMASK(31, 2);
+	if (gmadr_bytes == 8) {
+		gma_high = cmd_val(s, 2) & GENMASK(15, 0);
+		gma = (gma_high << 32) | gma;
+	}
+	ret = cmd_address_audit(s, gma, op_size, false);
+	return ret;
+}
+
+static int cmd_handler_mi_store_data_index(struct parser_exec_state *s)
+{
+	return unexpected_cmd(s);
+}
+
+static int cmd_handler_mi_clflush(struct parser_exec_state *s)
+{
+	return unexpected_cmd(s);
+}
+
+static int cmd_handler_mi_conditional_batch_buffer_end(
+		struct parser_exec_state *s)
+{
+	return unexpected_cmd(s);
+}
+
+static int cmd_handler_mi_update_gtt(struct parser_exec_state *s)
+{
+	return unexpected_cmd(s);
+}
+
+static int cmd_handler_mi_flush_dw(struct parser_exec_state *s)
+{
+	int gmadr_bytes = s->vgpu->gvt->device_info.gmadr_bytes_in_cmd;
+	unsigned long gma;
+	bool index_mode = false;
+	int ret = 0;
+
+	/* Check post-sync and ppgtt bit */
+	if (((cmd_val(s, 0) >> 14) & 0x3) && (cmd_val(s, 1) & (1 << 2))) {
+		gma = cmd_val(s, 1) & GENMASK(31, 3);
+		if (gmadr_bytes == 8)
+			gma |= (cmd_val(s, 2) & GENMASK(15, 0)) << 32;
+		/* Store Data Index */
+		if (cmd_val(s, 0) & (1 << 21))
+			index_mode = true;
+		ret = cmd_address_audit(s, gma, sizeof(u64), index_mode);
+	}
+	/* Check notify bit */
+	if ((cmd_val(s, 0) & (1 << 8)))
+		set_bit(cmd_interrupt_events[s->ring_id].mi_flush_dw,
+				s->workload->pending_events);
+	return ret;
+}
+
+static void addr_type_update_snb(struct parser_exec_state *s)
+{
+	if ((s->buf_type == RING_BUFFER_INSTRUCTION) &&
+			(BATCH_BUFFER_ADR_SPACE_BIT(cmd_val(s, 0)) == 1)) {
+		s->buf_addr_type = PPGTT_BUFFER;
+	}
+}
+
+
+static int copy_gma_to_hva(struct intel_vgpu *vgpu, struct intel_vgpu_mm *mm,
+		unsigned long gma, unsigned long end_gma, void *va)
+{
+	unsigned long copy_len, offset;
+	unsigned long len = 0;
+	unsigned long gpa;
+
+	while (gma != end_gma) {
+		gpa = intel_vgpu_gma_to_gpa(mm, gma);
+		if (gpa == INTEL_GVT_INVALID_ADDR) {
+			gvt_err("invalid gma address: %lx\n", gma);
+			return -EFAULT;
+		}
+
+		offset = gma & (GTT_PAGE_SIZE - 1);
+
+		copy_len = (end_gma - gma) >= (GTT_PAGE_SIZE - offset) ?
+			GTT_PAGE_SIZE - offset : end_gma - gma;
+
+		intel_gvt_hypervisor_read_gpa(vgpu, gpa, va + len, copy_len);
+
+		len += copy_len;
+		gma += copy_len;
+	}
+	return 0;
+}
+
+
+/*
+ * Check whether a batch buffer needs to be scanned. Currently
+ * the only criteria is based on privilege.
+ */
+static int batch_buffer_needs_scan(struct parser_exec_state *s)
+{
+	struct intel_gvt *gvt = s->vgpu->gvt;
+
+	if (bypass_batch_buffer_scan)
+		return 0;
+
+	if (IS_BROADWELL(gvt->dev_priv) || IS_SKYLAKE(gvt->dev_priv)) {
+		/* BDW decides privilege based on address space */
+		if (cmd_val(s, 0) & (1 << 8))
+			return 0;
+	}
+	return 1;
+}
+
+static uint32_t find_bb_size(struct parser_exec_state *s)
+{
+	unsigned long gma = 0;
+	struct cmd_info *info;
+	uint32_t bb_size = 0;
+	uint32_t cmd_len = 0;
+	bool met_bb_end = false;
+	u32 cmd;
+
+	/* get the start gm address of the batch buffer */
+	gma = get_gma_bb_from_cmd(s, 1);
+	cmd = cmd_val(s, 0);
+
+	info = get_cmd_info(s->vgpu->gvt, cmd, s->ring_id);
+	if (info == NULL) {
+		gvt_err("unknown cmd 0x%x, opcode=0x%x\n",
+				cmd, get_opcode(cmd, s->ring_id));
+		return -EINVAL;
+	}
+	do {
+		copy_gma_to_hva(s->vgpu, s->vgpu->gtt.ggtt_mm,
+				gma, gma + 4, &cmd);
+		info = get_cmd_info(s->vgpu->gvt, cmd, s->ring_id);
+		if (info == NULL) {
+			gvt_err("unknown cmd 0x%x, opcode=0x%x\n",
+				cmd, get_opcode(cmd, s->ring_id));
+			return -EINVAL;
+		}
+
+		if (info->opcode == OP_MI_BATCH_BUFFER_END) {
+			met_bb_end = true;
+		} else if (info->opcode == OP_MI_BATCH_BUFFER_START) {
+			if (BATCH_BUFFER_2ND_LEVEL_BIT(cmd) == 0) {
+				/* chained batch buffer */
+				met_bb_end = true;
+			}
+		}
+		cmd_len = get_cmd_length(info, cmd) << 2;
+		bb_size += cmd_len;
+		gma += cmd_len;
+
+	} while (!met_bb_end);
+
+	return bb_size;
+}
+
+static int perform_bb_shadow(struct parser_exec_state *s)
+{
+	struct intel_shadow_bb_entry *entry_obj;
+	unsigned long gma = 0;
+	uint32_t bb_size;
+	void *dst = NULL;
+	int ret = 0;
+
+	/* get the start gm address of the batch buffer */
+	gma = get_gma_bb_from_cmd(s, 1);
+
+	/* get the size of the batch buffer */
+	bb_size = find_bb_size(s);
+
+	/* allocate shadow batch buffer */
+	entry_obj = kmalloc(sizeof(*entry_obj), GFP_KERNEL);
+	if (entry_obj == NULL)
+		return -ENOMEM;
+
+	entry_obj->obj =
+		i915_gem_object_create(&(s->vgpu->gvt->dev_priv->drm),
+				       roundup(bb_size, PAGE_SIZE));
+	if (IS_ERR(entry_obj->obj)) {
+		ret = PTR_ERR(entry_obj->obj);
+		goto free_entry;
+	}
+	entry_obj->len = bb_size;
+	INIT_LIST_HEAD(&entry_obj->list);
+
+	dst = i915_gem_object_pin_map(entry_obj->obj, I915_MAP_WB);
+	if (IS_ERR(dst)) {
+		ret = PTR_ERR(dst);
+		goto put_obj;
+	}
+
+	ret = i915_gem_object_set_to_cpu_domain(entry_obj->obj, false);
+	if (ret) {
+		gvt_err("failed to set shadow batch to CPU\n");
+		goto unmap_src;
+	}
+
+	entry_obj->va = dst;
+	entry_obj->bb_start_cmd_va = s->ip_va;
+
+	/* copy batch buffer to shadow batch buffer*/
+	ret = copy_gma_to_hva(s->vgpu, s->vgpu->gtt.ggtt_mm,
+			      gma, gma + bb_size,
+			      dst);
+	if (ret) {
+		gvt_err("fail to copy guest ring buffer\n");
+		goto unmap_src;
+	}
+
+	list_add(&entry_obj->list, &s->workload->shadow_bb);
+	/*
+	 * ip_va saves the virtual address of the shadow batch buffer, while
+	 * ip_gma saves the graphics address of the original batch buffer.
+	 * As the shadow batch buffer is just a copy from the originial one,
+	 * it should be right to use shadow batch buffer'va and original batch
+	 * buffer's gma in pair. After all, we don't want to pin the shadow
+	 * buffer here (too early).
+	 */
+	s->ip_va = dst;
+	s->ip_gma = gma;
+
+	return 0;
+
+unmap_src:
+	i915_gem_object_unpin_map(entry_obj->obj);
+put_obj:
+	i915_gem_object_put(entry_obj->obj);
+free_entry:
+	kfree(entry_obj);
+	return ret;
+}
+
+static int cmd_handler_mi_batch_buffer_start(struct parser_exec_state *s)
+{
+	bool second_level;
+	int ret = 0;
+
+	if (s->buf_type == BATCH_BUFFER_2ND_LEVEL) {
+		gvt_err("Found MI_BATCH_BUFFER_START in 2nd level BB\n");
+		return -EINVAL;
+	}
+
+	second_level = BATCH_BUFFER_2ND_LEVEL_BIT(cmd_val(s, 0)) == 1;
+	if (second_level && (s->buf_type != BATCH_BUFFER_INSTRUCTION)) {
+		gvt_err("Jumping to 2nd level BB from RB is not allowed\n");
+		return -EINVAL;
+	}
+
+	s->saved_buf_addr_type = s->buf_addr_type;
+	addr_type_update_snb(s);
+	if (s->buf_type == RING_BUFFER_INSTRUCTION) {
+		s->ret_ip_gma_ring = s->ip_gma + cmd_length(s) * sizeof(u32);
+		s->buf_type = BATCH_BUFFER_INSTRUCTION;
+	} else if (second_level) {
+		s->buf_type = BATCH_BUFFER_2ND_LEVEL;
+		s->ret_ip_gma_bb = s->ip_gma + cmd_length(s) * sizeof(u32);
+		s->ret_bb_va = s->ip_va + cmd_length(s) * sizeof(u32);
+	}
+
+	if (batch_buffer_needs_scan(s)) {
+		ret = perform_bb_shadow(s);
+		if (ret < 0)
+			gvt_err("invalid shadow batch buffer\n");
+	} else {
+		/* emulate a batch buffer end to do return right */
+		ret = cmd_handler_mi_batch_buffer_end(s);
+		if (ret < 0)
+			return ret;
+	}
+
+	return ret;
+}
+
+static struct cmd_info cmd_info[] = {
+	{"MI_NOOP", OP_MI_NOOP, F_LEN_CONST, R_ALL, D_ALL, 0, 1, NULL},
+
+	{"MI_SET_PREDICATE", OP_MI_SET_PREDICATE, F_LEN_CONST, R_ALL, D_ALL,
+		0, 1, NULL},
+
+	{"MI_USER_INTERRUPT", OP_MI_USER_INTERRUPT, F_LEN_CONST, R_ALL, D_ALL,
+		0, 1, cmd_handler_mi_user_interrupt},
+
+	{"MI_WAIT_FOR_EVENT", OP_MI_WAIT_FOR_EVENT, F_LEN_CONST, R_RCS | R_BCS,
+		D_ALL, 0, 1, cmd_handler_mi_wait_for_event},
+
+	{"MI_FLUSH", OP_MI_FLUSH, F_LEN_CONST, R_ALL, D_ALL, 0, 1, NULL},
+
+	{"MI_ARB_CHECK", OP_MI_ARB_CHECK, F_LEN_CONST, R_ALL, D_ALL, 0, 1,
+		NULL},
+
+	{"MI_RS_CONTROL", OP_MI_RS_CONTROL, F_LEN_CONST, R_RCS, D_ALL, 0, 1,
+		NULL},
+
+	{"MI_REPORT_HEAD", OP_MI_REPORT_HEAD, F_LEN_CONST, R_ALL, D_ALL, 0, 1,
+		NULL},
+
+	{"MI_ARB_ON_OFF", OP_MI_ARB_ON_OFF, F_LEN_CONST, R_ALL, D_ALL, 0, 1,
+		NULL},
+
+	{"MI_URB_ATOMIC_ALLOC", OP_MI_URB_ATOMIC_ALLOC, F_LEN_CONST, R_RCS,
+		D_ALL, 0, 1, NULL},
+
+	{"MI_BATCH_BUFFER_END", OP_MI_BATCH_BUFFER_END,
+		F_IP_ADVANCE_CUSTOM | F_LEN_CONST, R_ALL, D_ALL, 0, 1,
+		cmd_handler_mi_batch_buffer_end},
+
+	{"MI_SUSPEND_FLUSH", OP_MI_SUSPEND_FLUSH, F_LEN_CONST, R_ALL, D_ALL,
+		0, 1, NULL},
+
+	{"MI_PREDICATE", OP_MI_PREDICATE, F_LEN_CONST, R_RCS, D_ALL, 0, 1,
+		NULL},
+
+	{"MI_TOPOLOGY_FILTER", OP_MI_TOPOLOGY_FILTER, F_LEN_CONST, R_ALL,
+		D_ALL, 0, 1, NULL},
+
+	{"MI_SET_APPID", OP_MI_SET_APPID, F_LEN_CONST, R_ALL, D_ALL, 0, 1,
+		NULL},
+
+	{"MI_RS_CONTEXT", OP_MI_RS_CONTEXT, F_LEN_CONST, R_RCS, D_ALL, 0, 1,
+		NULL},
+
+	{"MI_DISPLAY_FLIP", OP_MI_DISPLAY_FLIP, F_LEN_VAR | F_POST_HANDLE,
+		R_RCS | R_BCS, D_ALL, 0, 8, cmd_handler_mi_display_flip},
+
+	{"MI_SEMAPHORE_MBOX", OP_MI_SEMAPHORE_MBOX, F_LEN_VAR, R_ALL, D_ALL,
+		0, 8, NULL},
+
+	{"MI_MATH", OP_MI_MATH, F_LEN_VAR, R_ALL, D_ALL, 0, 8, NULL},
+
+	{"MI_URB_CLEAR", OP_MI_URB_CLEAR, F_LEN_VAR, R_RCS, D_ALL, 0, 8, NULL},
+
+	{"ME_SEMAPHORE_SIGNAL", OP_MI_SEMAPHORE_SIGNAL, F_LEN_VAR, R_ALL,
+		D_BDW_PLUS, 0, 8, NULL},
+
+	{"ME_SEMAPHORE_WAIT", OP_MI_SEMAPHORE_WAIT, F_LEN_VAR, R_ALL, D_BDW_PLUS,
+		ADDR_FIX_1(2), 8, cmd_handler_mi_semaphore_wait},
+
+	{"MI_STORE_DATA_IMM", OP_MI_STORE_DATA_IMM, F_LEN_VAR, R_ALL, D_BDW_PLUS,
+		ADDR_FIX_1(1), 10, cmd_handler_mi_store_data_imm},
+
+	{"MI_STORE_DATA_INDEX", OP_MI_STORE_DATA_INDEX, F_LEN_VAR, R_ALL, D_ALL,
+		0, 8, cmd_handler_mi_store_data_index},
+
+	{"MI_LOAD_REGISTER_IMM", OP_MI_LOAD_REGISTER_IMM, F_LEN_VAR, R_ALL,
+		D_ALL, 0, 8, cmd_handler_lri},
+
+	{"MI_UPDATE_GTT", OP_MI_UPDATE_GTT, F_LEN_VAR, R_ALL, D_BDW_PLUS, 0, 10,
+		cmd_handler_mi_update_gtt},
+
+	{"MI_STORE_REGISTER_MEM", OP_MI_STORE_REGISTER_MEM, F_LEN_VAR, R_ALL,
+		D_ALL, ADDR_FIX_1(2), 8, cmd_handler_srm},
+
+	{"MI_FLUSH_DW", OP_MI_FLUSH_DW, F_LEN_VAR, R_ALL, D_ALL, 0, 6,
+		cmd_handler_mi_flush_dw},
+
+	{"MI_CLFLUSH", OP_MI_CLFLUSH, F_LEN_VAR, R_ALL, D_ALL, ADDR_FIX_1(1),
+		10, cmd_handler_mi_clflush},
+
+	{"MI_REPORT_PERF_COUNT", OP_MI_REPORT_PERF_COUNT, F_LEN_VAR, R_ALL,
+		D_ALL, ADDR_FIX_1(1), 6, cmd_handler_mi_report_perf_count},
+
+	{"MI_LOAD_REGISTER_MEM", OP_MI_LOAD_REGISTER_MEM, F_LEN_VAR, R_ALL,
+		D_ALL, ADDR_FIX_1(2), 8, cmd_handler_lrm},
+
+	{"MI_LOAD_REGISTER_REG", OP_MI_LOAD_REGISTER_REG, F_LEN_VAR, R_ALL,
+		D_ALL, 0, 8, cmd_handler_lrr},
+
+	{"MI_RS_STORE_DATA_IMM", OP_MI_RS_STORE_DATA_IMM, F_LEN_VAR, R_RCS,
+		D_ALL, 0, 8, NULL},
+
+	{"MI_LOAD_URB_MEM", OP_MI_LOAD_URB_MEM, F_LEN_VAR, R_RCS, D_ALL,
+		ADDR_FIX_1(2), 8, NULL},
+
+	{"MI_STORE_URM_MEM", OP_MI_STORE_URM_MEM, F_LEN_VAR, R_RCS, D_ALL,
+		ADDR_FIX_1(2), 8, NULL},
+
+	{"MI_OP_2E", OP_MI_2E, F_LEN_VAR, R_ALL, D_BDW_PLUS, ADDR_FIX_2(1, 2),
+		8, cmd_handler_mi_op_2e},
+
+	{"MI_OP_2F", OP_MI_2F, F_LEN_VAR, R_ALL, D_BDW_PLUS, ADDR_FIX_1(1),
+		8, cmd_handler_mi_op_2f},
+
+	{"MI_BATCH_BUFFER_START", OP_MI_BATCH_BUFFER_START,
+		F_IP_ADVANCE_CUSTOM, R_ALL, D_ALL, 0, 8,
+		cmd_handler_mi_batch_buffer_start},
+
+	{"MI_CONDITIONAL_BATCH_BUFFER_END", OP_MI_CONDITIONAL_BATCH_BUFFER_END,
+		F_LEN_VAR, R_ALL, D_ALL, ADDR_FIX_1(2), 8,
+		cmd_handler_mi_conditional_batch_buffer_end},
+
+	{"MI_LOAD_SCAN_LINES_INCL", OP_MI_LOAD_SCAN_LINES_INCL, F_LEN_CONST,
+		R_RCS | R_BCS, D_ALL, 0, 2, NULL},
+
+	{"XY_SETUP_BLT", OP_XY_SETUP_BLT, F_LEN_VAR, R_BCS, D_ALL,
+		ADDR_FIX_2(4, 7), 8, NULL},
+
+	{"XY_SETUP_CLIP_BLT", OP_XY_SETUP_CLIP_BLT, F_LEN_VAR, R_BCS, D_ALL,
+		0, 8, NULL},
+
+	{"XY_SETUP_MONO_PATTERN_SL_BLT", OP_XY_SETUP_MONO_PATTERN_SL_BLT,
+		F_LEN_VAR, R_BCS, D_ALL, ADDR_FIX_1(4), 8, NULL},
+
+	{"XY_PIXEL_BLT", OP_XY_PIXEL_BLT, F_LEN_VAR, R_BCS, D_ALL, 0, 8, NULL},
+
+	{"XY_SCANLINES_BLT", OP_XY_SCANLINES_BLT, F_LEN_VAR, R_BCS, D_ALL,
+		0, 8, NULL},
+
+	{"XY_TEXT_BLT", OP_XY_TEXT_BLT, F_LEN_VAR, R_BCS, D_ALL,
+		ADDR_FIX_1(3), 8, NULL},
+
+	{"XY_TEXT_IMMEDIATE_BLT", OP_XY_TEXT_IMMEDIATE_BLT, F_LEN_VAR, R_BCS,
+		D_ALL, 0, 8, NULL},
+
+	{"XY_COLOR_BLT", OP_XY_COLOR_BLT, F_LEN_VAR, R_BCS, D_ALL,
+		ADDR_FIX_1(4), 8, NULL},
+
+	{"XY_PAT_BLT", OP_XY_PAT_BLT, F_LEN_VAR, R_BCS, D_ALL,
+		ADDR_FIX_2(4, 5), 8, NULL},
+
+	{"XY_MONO_PAT_BLT", OP_XY_MONO_PAT_BLT, F_LEN_VAR, R_BCS, D_ALL,
+		ADDR_FIX_1(4), 8, NULL},
+
+	{"XY_SRC_COPY_BLT", OP_XY_SRC_COPY_BLT, F_LEN_VAR, R_BCS, D_ALL,
+		ADDR_FIX_2(4, 7), 8, NULL},
+
+	{"XY_MONO_SRC_COPY_BLT", OP_XY_MONO_SRC_COPY_BLT, F_LEN_VAR, R_BCS,
+		D_ALL, ADDR_FIX_2(4, 5), 8, NULL},
+
+	{"XY_FULL_BLT", OP_XY_FULL_BLT, F_LEN_VAR, R_BCS, D_ALL, 0, 8, NULL},
+
+	{"XY_FULL_MONO_SRC_BLT", OP_XY_FULL_MONO_SRC_BLT, F_LEN_VAR, R_BCS,
+		D_ALL, ADDR_FIX_3(4, 5, 8), 8, NULL},
+
+	{"XY_FULL_MONO_PATTERN_BLT", OP_XY_FULL_MONO_PATTERN_BLT, F_LEN_VAR,
+		R_BCS, D_ALL, ADDR_FIX_2(4, 7), 8, NULL},
+
+	{"XY_FULL_MONO_PATTERN_MONO_SRC_BLT",
+		OP_XY_FULL_MONO_PATTERN_MONO_SRC_BLT,
+		F_LEN_VAR, R_BCS, D_ALL, ADDR_FIX_2(4, 5), 8, NULL},
+
+	{"XY_MONO_PAT_FIXED_BLT", OP_XY_MONO_PAT_FIXED_BLT, F_LEN_VAR, R_BCS,
+		D_ALL, ADDR_FIX_1(4), 8, NULL},
+
+	{"XY_MONO_SRC_COPY_IMMEDIATE_BLT", OP_XY_MONO_SRC_COPY_IMMEDIATE_BLT,
+		F_LEN_VAR, R_BCS, D_ALL, ADDR_FIX_1(4), 8, NULL},
+
+	{"XY_PAT_BLT_IMMEDIATE", OP_XY_PAT_BLT_IMMEDIATE, F_LEN_VAR, R_BCS,
+		D_ALL, ADDR_FIX_1(4), 8, NULL},
+
+	{"XY_SRC_COPY_CHROMA_BLT", OP_XY_SRC_COPY_CHROMA_BLT, F_LEN_VAR, R_BCS,
+		D_ALL, ADDR_FIX_2(4, 7), 8, NULL},
+
+	{"XY_FULL_IMMEDIATE_PATTERN_BLT", OP_XY_FULL_IMMEDIATE_PATTERN_BLT,
+		F_LEN_VAR, R_BCS, D_ALL, ADDR_FIX_2(4, 7), 8, NULL},
+
+	{"XY_FULL_MONO_SRC_IMMEDIATE_PATTERN_BLT",
+		OP_XY_FULL_MONO_SRC_IMMEDIATE_PATTERN_BLT,
+		F_LEN_VAR, R_BCS, D_ALL, ADDR_FIX_2(4, 5), 8, NULL},
+
+	{"XY_PAT_CHROMA_BLT", OP_XY_PAT_CHROMA_BLT, F_LEN_VAR, R_BCS, D_ALL,
+		ADDR_FIX_2(4, 5), 8, NULL},
+
+	{"XY_PAT_CHROMA_BLT_IMMEDIATE", OP_XY_PAT_CHROMA_BLT_IMMEDIATE,
+		F_LEN_VAR, R_BCS, D_ALL, ADDR_FIX_1(4), 8, NULL},
+
+	{"3DSTATE_VIEWPORT_STATE_POINTERS_SF_CLIP",
+		OP_3DSTATE_VIEWPORT_STATE_POINTERS_SF_CLIP,
+		F_LEN_VAR, R_RCS, D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_VIEWPORT_STATE_POINTERS_CC",
+		OP_3DSTATE_VIEWPORT_STATE_POINTERS_CC,
+		F_LEN_VAR, R_RCS, D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_BLEND_STATE_POINTERS",
+		OP_3DSTATE_BLEND_STATE_POINTERS,
+		F_LEN_VAR, R_RCS, D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_DEPTH_STENCIL_STATE_POINTERS",
+		OP_3DSTATE_DEPTH_STENCIL_STATE_POINTERS,
+		F_LEN_VAR, R_RCS, D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_BINDING_TABLE_POINTERS_VS",
+		OP_3DSTATE_BINDING_TABLE_POINTERS_VS,
+		F_LEN_VAR, R_RCS, D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_BINDING_TABLE_POINTERS_HS",
+		OP_3DSTATE_BINDING_TABLE_POINTERS_HS,
+		F_LEN_VAR, R_RCS, D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_BINDING_TABLE_POINTERS_DS",
+		OP_3DSTATE_BINDING_TABLE_POINTERS_DS,
+		F_LEN_VAR, R_RCS, D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_BINDING_TABLE_POINTERS_GS",
+		OP_3DSTATE_BINDING_TABLE_POINTERS_GS,
+		F_LEN_VAR, R_RCS, D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_BINDING_TABLE_POINTERS_PS",
+		OP_3DSTATE_BINDING_TABLE_POINTERS_PS,
+		F_LEN_VAR, R_RCS, D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_SAMPLER_STATE_POINTERS_VS",
+		OP_3DSTATE_SAMPLER_STATE_POINTERS_VS,
+		F_LEN_VAR, R_RCS, D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_SAMPLER_STATE_POINTERS_HS",
+		OP_3DSTATE_SAMPLER_STATE_POINTERS_HS,
+		F_LEN_VAR, R_RCS, D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_SAMPLER_STATE_POINTERS_DS",
+		OP_3DSTATE_SAMPLER_STATE_POINTERS_DS,
+		F_LEN_VAR, R_RCS, D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_SAMPLER_STATE_POINTERS_GS",
+		OP_3DSTATE_SAMPLER_STATE_POINTERS_GS,
+		F_LEN_VAR, R_RCS, D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_SAMPLER_STATE_POINTERS_PS",
+		OP_3DSTATE_SAMPLER_STATE_POINTERS_PS,
+		F_LEN_VAR, R_RCS, D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_URB_VS", OP_3DSTATE_URB_VS, F_LEN_VAR, R_RCS, D_ALL,
+		0, 8, NULL},
+
+	{"3DSTATE_URB_HS", OP_3DSTATE_URB_HS, F_LEN_VAR, R_RCS, D_ALL,
+		0, 8, NULL},
+
+	{"3DSTATE_URB_DS", OP_3DSTATE_URB_DS, F_LEN_VAR, R_RCS, D_ALL,
+		0, 8, NULL},
+
+	{"3DSTATE_URB_GS", OP_3DSTATE_URB_GS, F_LEN_VAR, R_RCS, D_ALL,
+		0, 8, NULL},
+
+	{"3DSTATE_GATHER_CONSTANT_VS", OP_3DSTATE_GATHER_CONSTANT_VS,
+		F_LEN_VAR, R_RCS, D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_GATHER_CONSTANT_GS", OP_3DSTATE_GATHER_CONSTANT_GS,
+		F_LEN_VAR, R_RCS, D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_GATHER_CONSTANT_HS", OP_3DSTATE_GATHER_CONSTANT_HS,
+		F_LEN_VAR, R_RCS, D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_GATHER_CONSTANT_DS", OP_3DSTATE_GATHER_CONSTANT_DS,
+		F_LEN_VAR, R_RCS, D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_GATHER_CONSTANT_PS", OP_3DSTATE_GATHER_CONSTANT_PS,
+		F_LEN_VAR, R_RCS, D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_DX9_CONSTANTF_VS", OP_3DSTATE_DX9_CONSTANTF_VS,
+		F_LEN_VAR, R_RCS, D_ALL, 0, 11, NULL},
+
+	{"3DSTATE_DX9_CONSTANTF_PS", OP_3DSTATE_DX9_CONSTANTF_PS,
+		F_LEN_VAR, R_RCS, D_ALL, 0, 11, NULL},
+
+	{"3DSTATE_DX9_CONSTANTI_VS", OP_3DSTATE_DX9_CONSTANTI_VS,
+		F_LEN_VAR, R_RCS, D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_DX9_CONSTANTI_PS", OP_3DSTATE_DX9_CONSTANTI_PS,
+		F_LEN_VAR, R_RCS, D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_DX9_CONSTANTB_VS", OP_3DSTATE_DX9_CONSTANTB_VS,
+		F_LEN_VAR, R_RCS, D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_DX9_CONSTANTB_PS", OP_3DSTATE_DX9_CONSTANTB_PS,
+		F_LEN_VAR, R_RCS, D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_DX9_LOCAL_VALID_VS", OP_3DSTATE_DX9_LOCAL_VALID_VS,
+		F_LEN_VAR, R_RCS, D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_DX9_LOCAL_VALID_PS", OP_3DSTATE_DX9_LOCAL_VALID_PS,
+		F_LEN_VAR, R_RCS, D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_DX9_GENERATE_ACTIVE_VS", OP_3DSTATE_DX9_GENERATE_ACTIVE_VS,
+		F_LEN_VAR, R_RCS, D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_DX9_GENERATE_ACTIVE_PS", OP_3DSTATE_DX9_GENERATE_ACTIVE_PS,
+		F_LEN_VAR, R_RCS, D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_BINDING_TABLE_EDIT_VS", OP_3DSTATE_BINDING_TABLE_EDIT_VS,
+		F_LEN_VAR, R_RCS, D_ALL, 0, 9, NULL},
+
+	{"3DSTATE_BINDING_TABLE_EDIT_GS", OP_3DSTATE_BINDING_TABLE_EDIT_GS,
+		F_LEN_VAR, R_RCS, D_ALL, 0, 9, NULL},
+
+	{"3DSTATE_BINDING_TABLE_EDIT_HS", OP_3DSTATE_BINDING_TABLE_EDIT_HS,
+		F_LEN_VAR, R_RCS, D_ALL, 0, 9, NULL},
+
+	{"3DSTATE_BINDING_TABLE_EDIT_DS", OP_3DSTATE_BINDING_TABLE_EDIT_DS,
+		F_LEN_VAR, R_RCS, D_ALL, 0, 9, NULL},
+
+	{"3DSTATE_BINDING_TABLE_EDIT_PS", OP_3DSTATE_BINDING_TABLE_EDIT_PS,
+		F_LEN_VAR, R_RCS, D_ALL, 0, 9, NULL},
+
+	{"3DSTATE_VF_INSTANCING", OP_3DSTATE_VF_INSTANCING, F_LEN_VAR, R_RCS,
+		D_BDW_PLUS, 0, 8, NULL},
+
+	{"3DSTATE_VF_SGVS", OP_3DSTATE_VF_SGVS, F_LEN_VAR, R_RCS, D_BDW_PLUS, 0, 8,
+		NULL},
+
+	{"3DSTATE_VF_TOPOLOGY", OP_3DSTATE_VF_TOPOLOGY, F_LEN_VAR, R_RCS,
+		D_BDW_PLUS, 0, 8, NULL},
+
+	{"3DSTATE_WM_CHROMAKEY", OP_3DSTATE_WM_CHROMAKEY, F_LEN_VAR, R_RCS,
+		D_BDW_PLUS, 0, 8, NULL},
+
+	{"3DSTATE_PS_BLEND", OP_3DSTATE_PS_BLEND, F_LEN_VAR, R_RCS, D_BDW_PLUS, 0,
+		8, NULL},
+
+	{"3DSTATE_WM_DEPTH_STENCIL", OP_3DSTATE_WM_DEPTH_STENCIL, F_LEN_VAR,
+		R_RCS, D_BDW_PLUS, 0, 8, NULL},
+
+	{"3DSTATE_PS_EXTRA", OP_3DSTATE_PS_EXTRA, F_LEN_VAR, R_RCS, D_BDW_PLUS, 0,
+		8, NULL},
+
+	{"3DSTATE_RASTER", OP_3DSTATE_RASTER, F_LEN_VAR, R_RCS, D_BDW_PLUS, 0, 8,
+		NULL},
+
+	{"3DSTATE_SBE_SWIZ", OP_3DSTATE_SBE_SWIZ, F_LEN_VAR, R_RCS, D_BDW_PLUS, 0, 8,
+		NULL},
+
+	{"3DSTATE_WM_HZ_OP", OP_3DSTATE_WM_HZ_OP, F_LEN_VAR, R_RCS, D_BDW_PLUS, 0, 8,
+		NULL},
+
+	{"3DSTATE_VERTEX_BUFFERS", OP_3DSTATE_VERTEX_BUFFERS, F_LEN_VAR, R_RCS,
+		D_BDW_PLUS, 0, 8, NULL},
+
+	{"3DSTATE_VERTEX_ELEMENTS", OP_3DSTATE_VERTEX_ELEMENTS, F_LEN_VAR,
+		R_RCS, D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_INDEX_BUFFER", OP_3DSTATE_INDEX_BUFFER, F_LEN_VAR, R_RCS,
+		D_BDW_PLUS, ADDR_FIX_1(2), 8, NULL},
+
+	{"3DSTATE_VF_STATISTICS", OP_3DSTATE_VF_STATISTICS, F_LEN_CONST,
+		R_RCS, D_ALL, 0, 1, NULL},
+
+	{"3DSTATE_VF", OP_3DSTATE_VF, F_LEN_VAR, R_RCS, D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_CC_STATE_POINTERS", OP_3DSTATE_CC_STATE_POINTERS, F_LEN_VAR,
+		R_RCS, D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_SCISSOR_STATE_POINTERS", OP_3DSTATE_SCISSOR_STATE_POINTERS,
+		F_LEN_VAR, R_RCS, D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_GS", OP_3DSTATE_GS, F_LEN_VAR, R_RCS, D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_CLIP", OP_3DSTATE_CLIP, F_LEN_VAR, R_RCS, D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_WM", OP_3DSTATE_WM, F_LEN_VAR, R_RCS, D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_CONSTANT_GS", OP_3DSTATE_CONSTANT_GS, F_LEN_VAR, R_RCS,
+		D_BDW_PLUS, 0, 8, NULL},
+
+	{"3DSTATE_CONSTANT_PS", OP_3DSTATE_CONSTANT_PS, F_LEN_VAR, R_RCS,
+		D_BDW_PLUS, 0, 8, NULL},
+
+	{"3DSTATE_SAMPLE_MASK", OP_3DSTATE_SAMPLE_MASK, F_LEN_VAR, R_RCS,
+		D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_CONSTANT_HS", OP_3DSTATE_CONSTANT_HS, F_LEN_VAR, R_RCS,
+		D_BDW_PLUS, 0, 8, NULL},
+
+	{"3DSTATE_CONSTANT_DS", OP_3DSTATE_CONSTANT_DS, F_LEN_VAR, R_RCS,
+		D_BDW_PLUS, 0, 8, NULL},
+
+	{"3DSTATE_HS", OP_3DSTATE_HS, F_LEN_VAR, R_RCS, D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_TE", OP_3DSTATE_TE, F_LEN_VAR, R_RCS, D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_DS", OP_3DSTATE_DS, F_LEN_VAR, R_RCS, D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_STREAMOUT", OP_3DSTATE_STREAMOUT, F_LEN_VAR, R_RCS,
+		D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_SBE", OP_3DSTATE_SBE, F_LEN_VAR, R_RCS, D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_PS", OP_3DSTATE_PS, F_LEN_VAR, R_RCS, D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_DRAWING_RECTANGLE", OP_3DSTATE_DRAWING_RECTANGLE, F_LEN_VAR,
+		R_RCS, D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_SAMPLER_PALETTE_LOAD0", OP_3DSTATE_SAMPLER_PALETTE_LOAD0,
+		F_LEN_VAR, R_RCS, D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_CHROMA_KEY", OP_3DSTATE_CHROMA_KEY, F_LEN_VAR, R_RCS, D_ALL,
+		0, 8, NULL},
+
+	{"3DSTATE_DEPTH_BUFFER", OP_3DSTATE_DEPTH_BUFFER, F_LEN_VAR, R_RCS,
+		D_ALL, ADDR_FIX_1(2), 8, NULL},
+
+	{"3DSTATE_POLY_STIPPLE_OFFSET", OP_3DSTATE_POLY_STIPPLE_OFFSET,
+		F_LEN_VAR, R_RCS, D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_POLY_STIPPLE_PATTERN", OP_3DSTATE_POLY_STIPPLE_PATTERN,
+		F_LEN_VAR, R_RCS, D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_LINE_STIPPLE", OP_3DSTATE_LINE_STIPPLE, F_LEN_VAR, R_RCS,
+		D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_AA_LINE_PARAMS", OP_3DSTATE_AA_LINE_PARAMS, F_LEN_VAR, R_RCS,
+		D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_GS_SVB_INDEX", OP_3DSTATE_GS_SVB_INDEX, F_LEN_VAR, R_RCS,
+		D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_SAMPLER_PALETTE_LOAD1", OP_3DSTATE_SAMPLER_PALETTE_LOAD1,
+		F_LEN_VAR, R_RCS, D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_MULTISAMPLE", OP_3DSTATE_MULTISAMPLE_BDW, F_LEN_VAR, R_RCS,
+		D_BDW_PLUS, 0, 8, NULL},
+
+	{"3DSTATE_STENCIL_BUFFER", OP_3DSTATE_STENCIL_BUFFER, F_LEN_VAR, R_RCS,
+		D_ALL, ADDR_FIX_1(2), 8, NULL},
+
+	{"3DSTATE_HIER_DEPTH_BUFFER", OP_3DSTATE_HIER_DEPTH_BUFFER, F_LEN_VAR,
+		R_RCS, D_ALL, ADDR_FIX_1(2), 8, NULL},
+
+	{"3DSTATE_CLEAR_PARAMS", OP_3DSTATE_CLEAR_PARAMS, F_LEN_VAR,
+		R_RCS, D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_PUSH_CONSTANT_ALLOC_VS", OP_3DSTATE_PUSH_CONSTANT_ALLOC_VS,
+		F_LEN_VAR, R_RCS, D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_PUSH_CONSTANT_ALLOC_HS", OP_3DSTATE_PUSH_CONSTANT_ALLOC_HS,
+		F_LEN_VAR, R_RCS, D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_PUSH_CONSTANT_ALLOC_DS", OP_3DSTATE_PUSH_CONSTANT_ALLOC_DS,
+		F_LEN_VAR, R_RCS, D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_PUSH_CONSTANT_ALLOC_GS", OP_3DSTATE_PUSH_CONSTANT_ALLOC_GS,
+		F_LEN_VAR, R_RCS, D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_PUSH_CONSTANT_ALLOC_PS", OP_3DSTATE_PUSH_CONSTANT_ALLOC_PS,
+		F_LEN_VAR, R_RCS, D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_MONOFILTER_SIZE", OP_3DSTATE_MONOFILTER_SIZE, F_LEN_VAR,
+		R_RCS, D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_SO_DECL_LIST", OP_3DSTATE_SO_DECL_LIST, F_LEN_VAR, R_RCS,
+		D_ALL, 0, 9, NULL},
+
+	{"3DSTATE_SO_BUFFER", OP_3DSTATE_SO_BUFFER, F_LEN_VAR, R_RCS, D_BDW_PLUS,
+		ADDR_FIX_2(2, 4), 8, NULL},
+
+	{"3DSTATE_BINDING_TABLE_POOL_ALLOC",
+		OP_3DSTATE_BINDING_TABLE_POOL_ALLOC,
+		F_LEN_VAR, R_RCS, D_BDW_PLUS, ADDR_FIX_1(1), 8, NULL},
+
+	{"3DSTATE_GATHER_POOL_ALLOC", OP_3DSTATE_GATHER_POOL_ALLOC,
+		F_LEN_VAR, R_RCS, D_BDW_PLUS, ADDR_FIX_1(1), 8, NULL},
+
+	{"3DSTATE_DX9_CONSTANT_BUFFER_POOL_ALLOC",
+		OP_3DSTATE_DX9_CONSTANT_BUFFER_POOL_ALLOC,
+		F_LEN_VAR, R_RCS, D_BDW_PLUS, ADDR_FIX_1(1), 8, NULL},
+
+	{"3DSTATE_SAMPLE_PATTERN", OP_3DSTATE_SAMPLE_PATTERN, F_LEN_VAR, R_RCS,
+		D_BDW_PLUS, 0, 8, NULL},
+
+	{"PIPE_CONTROL", OP_PIPE_CONTROL, F_LEN_VAR, R_RCS, D_ALL,
+		ADDR_FIX_1(2), 8, cmd_handler_pipe_control},
+
+	{"3DPRIMITIVE", OP_3DPRIMITIVE, F_LEN_VAR, R_RCS, D_ALL, 0, 8, NULL},
+
+	{"PIPELINE_SELECT", OP_PIPELINE_SELECT, F_LEN_CONST, R_RCS, D_ALL, 0,
+		1, NULL},
+
+	{"STATE_PREFETCH", OP_STATE_PREFETCH, F_LEN_VAR, R_RCS, D_ALL,
+		ADDR_FIX_1(1), 8, NULL},
+
+	{"STATE_SIP", OP_STATE_SIP, F_LEN_VAR, R_RCS, D_ALL, 0, 8, NULL},
+
+	{"STATE_BASE_ADDRESS", OP_STATE_BASE_ADDRESS, F_LEN_VAR, R_RCS, D_BDW_PLUS,
+		ADDR_FIX_5(1, 3, 4, 5, 6), 8, NULL},
+
+	{"OP_3D_MEDIA_0_1_4", OP_3D_MEDIA_0_1_4, F_LEN_VAR, R_RCS, D_ALL,
+		ADDR_FIX_1(1), 8, NULL},
+
+	{"3DSTATE_VS", OP_3DSTATE_VS, F_LEN_VAR, R_RCS, D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_SF", OP_3DSTATE_SF, F_LEN_VAR, R_RCS, D_ALL, 0, 8, NULL},
+
+	{"3DSTATE_CONSTANT_VS", OP_3DSTATE_CONSTANT_VS, F_LEN_VAR, R_RCS, D_BDW_PLUS,
+		0, 8, NULL},
+
+	{"3DSTATE_COMPONENT_PACKING", OP_3DSTATE_COMPONENT_PACKING, F_LEN_VAR, R_RCS,
+		D_SKL_PLUS, 0, 8, NULL},
+
+	{"MEDIA_INTERFACE_DESCRIPTOR_LOAD", OP_MEDIA_INTERFACE_DESCRIPTOR_LOAD,
+		F_LEN_VAR, R_RCS, D_ALL, 0, 16, NULL},
+
+	{"MEDIA_GATEWAY_STATE", OP_MEDIA_GATEWAY_STATE, F_LEN_VAR, R_RCS, D_ALL,
+		0, 16, NULL},
+
+	{"MEDIA_STATE_FLUSH", OP_MEDIA_STATE_FLUSH, F_LEN_VAR, R_RCS, D_ALL,
+		0, 16, NULL},
+
+	{"MEDIA_OBJECT", OP_MEDIA_OBJECT, F_LEN_VAR, R_RCS, D_ALL, 0, 16, NULL},
+
+	{"MEDIA_CURBE_LOAD", OP_MEDIA_CURBE_LOAD, F_LEN_VAR, R_RCS, D_ALL,
+		0, 16, NULL},
+
+	{"MEDIA_OBJECT_PRT", OP_MEDIA_OBJECT_PRT, F_LEN_VAR, R_RCS, D_ALL,
+		0, 16, NULL},
+
+	{"MEDIA_OBJECT_WALKER", OP_MEDIA_OBJECT_WALKER, F_LEN_VAR, R_RCS, D_ALL,
+		0, 16, NULL},
+
+	{"GPGPU_WALKER", OP_GPGPU_WALKER, F_LEN_VAR, R_RCS, D_ALL,
+		0, 8, NULL},
+
+	{"MEDIA_VFE_STATE", OP_MEDIA_VFE_STATE, F_LEN_VAR, R_RCS, D_ALL, 0, 16,
+		NULL},
+
+	{"3DSTATE_VF_STATISTICS_GM45", OP_3DSTATE_VF_STATISTICS_GM45,
+		F_LEN_CONST, R_ALL, D_ALL, 0, 1, NULL},
+
+	{"MFX_PIPE_MODE_SELECT", OP_MFX_PIPE_MODE_SELECT, F_LEN_VAR,
+		R_VCS, D_ALL, 0, 12, NULL},
+
+	{"MFX_SURFACE_STATE", OP_MFX_SURFACE_STATE, F_LEN_VAR,
+		R_VCS, D_ALL, 0, 12, NULL},
+
+	{"MFX_PIPE_BUF_ADDR_STATE", OP_MFX_PIPE_BUF_ADDR_STATE, F_LEN_VAR,
+		R_VCS, D_BDW_PLUS, 0, 12, NULL},
+
+	{"MFX_IND_OBJ_BASE_ADDR_STATE", OP_MFX_IND_OBJ_BASE_ADDR_STATE,
+		F_LEN_VAR, R_VCS, D_BDW_PLUS, 0, 12, NULL},
+
+	{"MFX_BSP_BUF_BASE_ADDR_STATE", OP_MFX_BSP_BUF_BASE_ADDR_STATE,
+		F_LEN_VAR, R_VCS, D_BDW_PLUS, ADDR_FIX_3(1, 3, 5), 12, NULL},
+
+	{"OP_2_0_0_5", OP_2_0_0_5, F_LEN_VAR, R_VCS, D_BDW_PLUS, 0, 12, NULL},
+
+	{"MFX_STATE_POINTER", OP_MFX_STATE_POINTER, F_LEN_VAR,
+		R_VCS, D_ALL, 0, 12, NULL},
+
+	{"MFX_QM_STATE", OP_MFX_QM_STATE, F_LEN_VAR,
+		R_VCS, D_ALL, 0, 12, NULL},
+
+	{"MFX_FQM_STATE", OP_MFX_FQM_STATE, F_LEN_VAR,
+		R_VCS, D_ALL, 0, 12, NULL},
+
+	{"MFX_PAK_INSERT_OBJECT", OP_MFX_PAK_INSERT_OBJECT, F_LEN_VAR,
+		R_VCS, D_ALL, 0, 12, NULL},
+
+	{"MFX_STITCH_OBJECT", OP_MFX_STITCH_OBJECT, F_LEN_VAR,
+		R_VCS, D_ALL, 0, 12, NULL},
+
+	{"MFD_IT_OBJECT", OP_MFD_IT_OBJECT, F_LEN_VAR,
+		R_VCS, D_ALL, 0, 12, NULL},
+
+	{"MFX_WAIT", OP_MFX_WAIT, F_LEN_VAR,
+		R_VCS, D_ALL, 0, 6, NULL},
+
+	{"MFX_AVC_IMG_STATE", OP_MFX_AVC_IMG_STATE, F_LEN_VAR,
+		R_VCS, D_ALL, 0, 12, NULL},
+
+	{"MFX_AVC_QM_STATE", OP_MFX_AVC_QM_STATE, F_LEN_VAR,
+		R_VCS, D_ALL, 0, 12, NULL},
+
+	{"MFX_AVC_DIRECTMODE_STATE", OP_MFX_AVC_DIRECTMODE_STATE, F_LEN_VAR,
+		R_VCS, D_ALL, 0, 12, NULL},
+
+	{"MFX_AVC_SLICE_STATE", OP_MFX_AVC_SLICE_STATE, F_LEN_VAR,
+		R_VCS, D_ALL, 0, 12, NULL},
+
+	{"MFX_AVC_REF_IDX_STATE", OP_MFX_AVC_REF_IDX_STATE, F_LEN_VAR,
+		R_VCS, D_ALL, 0, 12, NULL},
+
+	{"MFX_AVC_WEIGHTOFFSET_STATE", OP_MFX_AVC_WEIGHTOFFSET_STATE, F_LEN_VAR,
+		R_VCS, D_ALL, 0, 12, NULL},
+
+	{"MFD_AVC_PICID_STATE", OP_MFD_AVC_PICID_STATE, F_LEN_VAR,
+		R_VCS, D_ALL, 0, 12, NULL},
+	{"MFD_AVC_DPB_STATE", OP_MFD_AVC_DPB_STATE, F_LEN_VAR,
+		R_VCS, D_ALL, 0, 12, NULL},
+
+	{"MFD_AVC_BSD_OBJECT", OP_MFD_AVC_BSD_OBJECT, F_LEN_VAR,
+		R_VCS, D_ALL, 0, 12, NULL},
+
+	{"MFD_AVC_SLICEADDR", OP_MFD_AVC_SLICEADDR, F_LEN_VAR,
+		R_VCS, D_ALL, ADDR_FIX_1(2), 12, NULL},
+
+	{"MFC_AVC_PAK_OBJECT", OP_MFC_AVC_PAK_OBJECT, F_LEN_VAR,
+		R_VCS, D_ALL, 0, 12, NULL},
+
+	{"MFX_VC1_PRED_PIPE_STATE", OP_MFX_VC1_PRED_PIPE_STATE, F_LEN_VAR,
+		R_VCS, D_ALL, 0, 12, NULL},
+
+	{"MFX_VC1_DIRECTMODE_STATE", OP_MFX_VC1_DIRECTMODE_STATE, F_LEN_VAR,
+		R_VCS, D_ALL, 0, 12, NULL},
+
+	{"MFD_VC1_SHORT_PIC_STATE", OP_MFD_VC1_SHORT_PIC_STATE, F_LEN_VAR,
+		R_VCS, D_ALL, 0, 12, NULL},
+
+	{"MFD_VC1_LONG_PIC_STATE", OP_MFD_VC1_LONG_PIC_STATE, F_LEN_VAR,
+		R_VCS, D_ALL, 0, 12, NULL},
+
+	{"MFD_VC1_BSD_OBJECT", OP_MFD_VC1_BSD_OBJECT, F_LEN_VAR,
+		R_VCS, D_ALL, 0, 12, NULL},
+
+	{"MFC_MPEG2_SLICEGROUP_STATE", OP_MFC_MPEG2_SLICEGROUP_STATE, F_LEN_VAR,
+		R_VCS, D_ALL, 0, 12, NULL},
+
+	{"MFC_MPEG2_PAK_OBJECT", OP_MFC_MPEG2_PAK_OBJECT, F_LEN_VAR,
+		R_VCS, D_ALL, 0, 12, NULL},
+
+	{"MFX_MPEG2_PIC_STATE", OP_MFX_MPEG2_PIC_STATE, F_LEN_VAR,
+		R_VCS, D_ALL, 0, 12, NULL},
+
+	{"MFX_MPEG2_QM_STATE", OP_MFX_MPEG2_QM_STATE, F_LEN_VAR,
+		R_VCS, D_ALL, 0, 12, NULL},
+
+	{"MFD_MPEG2_BSD_OBJECT", OP_MFD_MPEG2_BSD_OBJECT, F_LEN_VAR,
+		R_VCS, D_ALL, 0, 12, NULL},
+
+	{"MFX_2_6_0_0", OP_MFX_2_6_0_0, F_LEN_VAR, R_VCS, D_ALL,
+		0, 16, NULL},
+
+	{"MFX_2_6_0_9", OP_MFX_2_6_0_9, F_LEN_VAR, R_VCS, D_ALL, 0, 16, NULL},
+
+	{"MFX_2_6_0_8", OP_MFX_2_6_0_8, F_LEN_VAR, R_VCS, D_ALL, 0, 16, NULL},
+
+	{"MFX_JPEG_PIC_STATE", OP_MFX_JPEG_PIC_STATE, F_LEN_VAR,
+		R_VCS, D_ALL, 0, 12, NULL},
+
+	{"MFX_JPEG_HUFF_TABLE_STATE", OP_MFX_JPEG_HUFF_TABLE_STATE, F_LEN_VAR,
+		R_VCS, D_ALL, 0, 12, NULL},
+
+	{"MFD_JPEG_BSD_OBJECT", OP_MFD_JPEG_BSD_OBJECT, F_LEN_VAR,
+		R_VCS, D_ALL, 0, 12, NULL},
+
+	{"VEBOX_STATE", OP_VEB_STATE, F_LEN_VAR, R_VECS, D_ALL, 0, 12, NULL},
+
+	{"VEBOX_SURFACE_STATE", OP_VEB_SURFACE_STATE, F_LEN_VAR, R_VECS, D_ALL,
+		0, 12, NULL},
+
+	{"VEB_DI_IECP", OP_VEB_DNDI_IECP_STATE, F_LEN_VAR, R_VECS, D_BDW_PLUS,
+		0, 20, NULL},
+};
+
+static void add_cmd_entry(struct intel_gvt *gvt, struct cmd_entry *e)
+{
+	hash_add(gvt->cmd_table, &e->hlist, e->info->opcode);
+}
+
+#define GVT_MAX_CMD_LENGTH     20  /* In Dword */
+
+static void trace_cs_command(struct parser_exec_state *s,
+		cycles_t cost_pre_cmd_handler, cycles_t cost_cmd_handler)
+{
+	/* This buffer is used by ftrace to store all commands copied from
+	 * guest gma space. Sometimes commands can cross pages, this should
+	 * not be handled in ftrace logic. So this is just used as a
+	 * 'bounce buffer'
+	 */
+	u32 cmd_trace_buf[GVT_MAX_CMD_LENGTH];
+	int i;
+	u32 cmd_len = cmd_length(s);
+	/* The chosen value of GVT_MAX_CMD_LENGTH are just based on
+	 * following two considerations:
+	 * 1) From observation, most common ring commands is not that long.
+	 *    But there are execeptions. So it indeed makes sence to observe
+	 *    longer commands.
+	 * 2) From the performance and debugging point of view, dumping all
+	 *    contents of very commands is not necessary.
+	 * We mgith shrink GVT_MAX_CMD_LENGTH or remove this trace event in
+	 * future for performance considerations.
+	 */
+	if (unlikely(cmd_len > GVT_MAX_CMD_LENGTH)) {
+		gvt_dbg_cmd("cmd length exceed tracing limitation!\n");
+		cmd_len = GVT_MAX_CMD_LENGTH;
+	}
+
+	for (i = 0; i < cmd_len; i++)
+		cmd_trace_buf[i] = cmd_val(s, i);
+
+	trace_gvt_command(s->vgpu->id, s->ring_id, s->ip_gma, cmd_trace_buf,
+			cmd_len, s->buf_type == RING_BUFFER_INSTRUCTION,
+			cost_pre_cmd_handler, cost_cmd_handler);
+}
+
+/* call the cmd handler, and advance ip */
+static int cmd_parser_exec(struct parser_exec_state *s)
+{
+	struct cmd_info *info;
+	u32 cmd;
+	int ret = 0;
+	cycles_t t0, t1, t2;
+	struct parser_exec_state s_before_advance_custom;
+
+	t0 = get_cycles();
+
+	cmd = cmd_val(s, 0);
+
+	info = get_cmd_info(s->vgpu->gvt, cmd, s->ring_id);
+	if (info == NULL) {
+		gvt_err("unknown cmd 0x%x, opcode=0x%x\n",
+				cmd, get_opcode(cmd, s->ring_id));
+		return -EINVAL;
+	}
+
+	gvt_dbg_cmd("%s\n", info->name);
+
+	s->info = info;
+
+	t1 = get_cycles();
+
+	memcpy(&s_before_advance_custom, s, sizeof(struct parser_exec_state));
+
+	if (info->handler) {
+		ret = info->handler(s);
+		if (ret < 0) {
+			gvt_err("%s handler error\n", info->name);
+			return ret;
+		}
+	}
+	t2 = get_cycles();
+
+	trace_cs_command(&s_before_advance_custom, t1 - t0, t2 - t1);
+
+	if (!(info->flag & F_IP_ADVANCE_CUSTOM)) {
+		ret = cmd_advance_default(s);
+		if (ret) {
+			gvt_err("%s IP advance error\n", info->name);
+			return ret;
+		}
+	}
+	return 0;
+}
+
+static inline bool gma_out_of_range(unsigned long gma,
+		unsigned long gma_head, unsigned int gma_tail)
+{
+	if (gma_tail >= gma_head)
+		return (gma < gma_head) || (gma > gma_tail);
+	else
+		return (gma > gma_tail) && (gma < gma_head);
+}
+
+static int command_scan(struct parser_exec_state *s,
+		unsigned long rb_head, unsigned long rb_tail,
+		unsigned long rb_start, unsigned long rb_len)
+{
+
+	unsigned long gma_head, gma_tail, gma_bottom;
+	int ret = 0;
+
+	gma_head = rb_start + rb_head;
+	gma_tail = rb_start + rb_tail;
+	gma_bottom = rb_start +  rb_len;
+
+	gvt_dbg_cmd("scan_start: start=%lx end=%lx\n", gma_head, gma_tail);
+
+	while (s->ip_gma != gma_tail) {
+		if (s->buf_type == RING_BUFFER_INSTRUCTION) {
+			if (!(s->ip_gma >= rb_start) ||
+				!(s->ip_gma < gma_bottom)) {
+				gvt_err("ip_gma %lx out of ring scope."
+					"(base:0x%lx, bottom: 0x%lx)\n",
+					s->ip_gma, rb_start,
+					gma_bottom);
+				parser_exec_state_dump(s);
+				return -EINVAL;
+			}
+			if (gma_out_of_range(s->ip_gma, gma_head, gma_tail)) {
+				gvt_err("ip_gma %lx out of range."
+					"base 0x%lx head 0x%lx tail 0x%lx\n",
+					s->ip_gma, rb_start,
+					rb_head, rb_tail);
+				parser_exec_state_dump(s);
+				break;
+			}
+		}
+		ret = cmd_parser_exec(s);
+		if (ret) {
+			gvt_err("cmd parser error\n");
+			parser_exec_state_dump(s);
+			break;
+		}
+	}
+
+	gvt_dbg_cmd("scan_end\n");
+
+	return ret;
+}
+
+static int scan_workload(struct intel_vgpu_workload *workload)
+{
+	unsigned long gma_head, gma_tail, gma_bottom;
+	struct parser_exec_state s;
+	int ret = 0;
+
+	/* ring base is page aligned */
+	if (WARN_ON(!IS_ALIGNED(workload->rb_start, GTT_PAGE_SIZE)))
+		return -EINVAL;
+
+	gma_head = workload->rb_start + workload->rb_head;
+	gma_tail = workload->rb_start + workload->rb_tail;
+	gma_bottom = workload->rb_start +  _RING_CTL_BUF_SIZE(workload->rb_ctl);
+
+	s.buf_type = RING_BUFFER_INSTRUCTION;
+	s.buf_addr_type = GTT_BUFFER;
+	s.vgpu = workload->vgpu;
+	s.ring_id = workload->ring_id;
+	s.ring_start = workload->rb_start;
+	s.ring_size = _RING_CTL_BUF_SIZE(workload->rb_ctl);
+	s.ring_head = gma_head;
+	s.ring_tail = gma_tail;
+	s.rb_va = workload->shadow_ring_buffer_va;
+	s.workload = workload;
+
+	if (bypass_scan_mask & (1 << workload->ring_id))
+		return 0;
+
+	ret = ip_gma_set(&s, gma_head);
+	if (ret)
+		goto out;
+
+	ret = command_scan(&s, workload->rb_head, workload->rb_tail,
+		workload->rb_start, _RING_CTL_BUF_SIZE(workload->rb_ctl));
+
+out:
+	return ret;
+}
+
+static int scan_wa_ctx(struct intel_shadow_wa_ctx *wa_ctx)
+{
+
+	unsigned long gma_head, gma_tail, gma_bottom, ring_size, ring_tail;
+	struct parser_exec_state s;
+	int ret = 0;
+
+	/* ring base is page aligned */
+	if (WARN_ON(!IS_ALIGNED(wa_ctx->indirect_ctx.guest_gma, GTT_PAGE_SIZE)))
+		return -EINVAL;
+
+	ring_tail = wa_ctx->indirect_ctx.size + 3 * sizeof(uint32_t);
+	ring_size = round_up(wa_ctx->indirect_ctx.size + CACHELINE_BYTES,
+			PAGE_SIZE);
+	gma_head = wa_ctx->indirect_ctx.guest_gma;
+	gma_tail = wa_ctx->indirect_ctx.guest_gma + ring_tail;
+	gma_bottom = wa_ctx->indirect_ctx.guest_gma + ring_size;
+
+	s.buf_type = RING_BUFFER_INSTRUCTION;
+	s.buf_addr_type = GTT_BUFFER;
+	s.vgpu = wa_ctx->workload->vgpu;
+	s.ring_id = wa_ctx->workload->ring_id;
+	s.ring_start = wa_ctx->indirect_ctx.guest_gma;
+	s.ring_size = ring_size;
+	s.ring_head = gma_head;
+	s.ring_tail = gma_tail;
+	s.rb_va = wa_ctx->indirect_ctx.shadow_va;
+	s.workload = wa_ctx->workload;
+
+	ret = ip_gma_set(&s, gma_head);
+	if (ret)
+		goto out;
+
+	ret = command_scan(&s, 0, ring_tail,
+		wa_ctx->indirect_ctx.guest_gma, ring_size);
+out:
+	return ret;
+}
+
+static int shadow_workload_ring_buffer(struct intel_vgpu_workload *workload)
+{
+	struct intel_vgpu *vgpu = workload->vgpu;
+	int ring_id = workload->ring_id;
+	struct i915_gem_context *shadow_ctx = vgpu->shadow_ctx;
+	struct intel_ring *ring = shadow_ctx->engine[ring_id].ring;
+	unsigned long gma_head, gma_tail, gma_top, guest_rb_size;
+	unsigned int copy_len = 0;
+	int ret;
+
+	guest_rb_size = _RING_CTL_BUF_SIZE(workload->rb_ctl);
+
+	/* calculate workload ring buffer size */
+	workload->rb_len = (workload->rb_tail + guest_rb_size -
+			workload->rb_head) % guest_rb_size;
+
+	gma_head = workload->rb_start + workload->rb_head;
+	gma_tail = workload->rb_start + workload->rb_tail;
+	gma_top = workload->rb_start + guest_rb_size;
+
+	/* allocate shadow ring buffer */
+	ret = intel_ring_begin(workload->req, workload->rb_len / 4);
+	if (ret)
+		return ret;
+
+	/* get shadow ring buffer va */
+	workload->shadow_ring_buffer_va = ring->vaddr + ring->tail;
+
+	/* head > tail --> copy head <-> top */
+	if (gma_head > gma_tail) {
+		ret = copy_gma_to_hva(vgpu, vgpu->gtt.ggtt_mm,
+				gma_head, gma_top,
+				workload->shadow_ring_buffer_va);
+		if (ret) {
+			gvt_err("fail to copy guest ring buffer\n");
+			return ret;
+		}
+		copy_len = gma_top - gma_head;
+		gma_head = workload->rb_start;
+	}
+
+	/* copy head or start <-> tail */
+	ret = copy_gma_to_hva(vgpu, vgpu->gtt.ggtt_mm,
+			gma_head, gma_tail,
+			workload->shadow_ring_buffer_va + copy_len);
+	if (ret) {
+		gvt_err("fail to copy guest ring buffer\n");
+		return ret;
+	}
+	ring->tail += workload->rb_len;
+	intel_ring_advance(ring);
+	return 0;
+}
+
+int intel_gvt_scan_and_shadow_workload(struct intel_vgpu_workload *workload)
+{
+	int ret;
+
+	ret = shadow_workload_ring_buffer(workload);
+	if (ret) {
+		gvt_err("fail to shadow workload ring_buffer\n");
+		return ret;
+	}
+
+	ret = scan_workload(workload);
+	if (ret) {
+		gvt_err("scan workload error\n");
+		return ret;
+	}
+	return 0;
+}
+
+static int shadow_indirect_ctx(struct intel_shadow_wa_ctx *wa_ctx)
+{
+	struct drm_device *dev = &wa_ctx->workload->vgpu->gvt->dev_priv->drm;
+	int ctx_size = wa_ctx->indirect_ctx.size;
+	unsigned long guest_gma = wa_ctx->indirect_ctx.guest_gma;
+	struct drm_i915_gem_object *obj;
+	int ret = 0;
+	void *map;
+
+	obj = i915_gem_object_create(dev,
+				     roundup(ctx_size + CACHELINE_BYTES,
+					     PAGE_SIZE));
+	if (IS_ERR(obj))
+		return PTR_ERR(obj);
+
+	/* get the va of the shadow batch buffer */
+	map = i915_gem_object_pin_map(obj, I915_MAP_WB);
+	if (IS_ERR(map)) {
+		gvt_err("failed to vmap shadow indirect ctx\n");
+		ret = PTR_ERR(map);
+		goto put_obj;
+	}
+
+	ret = i915_gem_object_set_to_cpu_domain(obj, false);
+	if (ret) {
+		gvt_err("failed to set shadow indirect ctx to CPU\n");
+		goto unmap_src;
+	}
+
+	ret = copy_gma_to_hva(wa_ctx->workload->vgpu,
+				wa_ctx->workload->vgpu->gtt.ggtt_mm,
+				guest_gma, guest_gma + ctx_size,
+				map);
+	if (ret) {
+		gvt_err("fail to copy guest indirect ctx\n");
+		goto unmap_src;
+	}
+
+	wa_ctx->indirect_ctx.obj = obj;
+	wa_ctx->indirect_ctx.shadow_va = map;
+	return 0;
+
+unmap_src:
+	i915_gem_object_unpin_map(obj);
+put_obj:
+	i915_gem_object_put(wa_ctx->indirect_ctx.obj);
+	return ret;
+}
+
+static int combine_wa_ctx(struct intel_shadow_wa_ctx *wa_ctx)
+{
+	uint32_t per_ctx_start[CACHELINE_DWORDS] = {0};
+	unsigned char *bb_start_sva;
+
+	per_ctx_start[0] = 0x18800001;
+	per_ctx_start[1] = wa_ctx->per_ctx.guest_gma;
+
+	bb_start_sva = (unsigned char *)wa_ctx->indirect_ctx.shadow_va +
+				wa_ctx->indirect_ctx.size;
+
+	memcpy(bb_start_sva, per_ctx_start, CACHELINE_BYTES);
+
+	return 0;
+}
+
+int intel_gvt_scan_and_shadow_wa_ctx(struct intel_shadow_wa_ctx *wa_ctx)
+{
+	int ret;
+
+	if (wa_ctx->indirect_ctx.size == 0)
+		return 0;
+
+	ret = shadow_indirect_ctx(wa_ctx);
+	if (ret) {
+		gvt_err("fail to shadow indirect ctx\n");
+		return ret;
+	}
+
+	combine_wa_ctx(wa_ctx);
+
+	ret = scan_wa_ctx(wa_ctx);
+	if (ret) {
+		gvt_err("scan wa ctx error\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static struct cmd_info *find_cmd_entry_any_ring(struct intel_gvt *gvt,
+		unsigned int opcode, int rings)
+{
+	struct cmd_info *info = NULL;
+	unsigned int ring;
+
+	for_each_set_bit(ring, (unsigned long *)&rings, I915_NUM_ENGINES) {
+		info = find_cmd_entry(gvt, opcode, ring);
+		if (info)
+			break;
+	}
+	return info;
+}
+
+static int init_cmd_table(struct intel_gvt *gvt)
+{
+	int i;
+	struct cmd_entry *e;
+	struct cmd_info	*info;
+	unsigned int gen_type;
+
+	gen_type = intel_gvt_get_device_type(gvt);
+
+	for (i = 0; i < ARRAY_SIZE(cmd_info); i++) {
+		if (!(cmd_info[i].devices & gen_type))
+			continue;
+
+		e = kzalloc(sizeof(*e), GFP_KERNEL);
+		if (!e)
+			return -ENOMEM;
+
+		e->info = &cmd_info[i];
+		info = find_cmd_entry_any_ring(gvt,
+				e->info->opcode, e->info->rings);
+		if (info) {
+			gvt_err("%s %s duplicated\n", e->info->name,
+					info->name);
+			return -EEXIST;
+		}
+
+		INIT_HLIST_NODE(&e->hlist);
+		add_cmd_entry(gvt, e);
+		gvt_dbg_cmd("add %-30s op %04x flag %x devs %02x rings %02x\n",
+				e->info->name, e->info->opcode, e->info->flag,
+				e->info->devices, e->info->rings);
+	}
+	return 0;
+}
+
+static void clean_cmd_table(struct intel_gvt *gvt)
+{
+	struct hlist_node *tmp;
+	struct cmd_entry *e;
+	int i;
+
+	hash_for_each_safe(gvt->cmd_table, i, tmp, e, hlist)
+		kfree(e);
+
+	hash_init(gvt->cmd_table);
+}
+
+void intel_gvt_clean_cmd_parser(struct intel_gvt *gvt)
+{
+	clean_cmd_table(gvt);
+}
+
+int intel_gvt_init_cmd_parser(struct intel_gvt *gvt)
+{
+	int ret;
+
+	ret = init_cmd_table(gvt);
+	if (ret) {
+		intel_gvt_clean_cmd_parser(gvt);
+		return ret;
+	}
+	return 0;
+}
diff --git a/drivers/gpu/drm/i915/gvt/cmd_parser.h b/drivers/gpu/drm/i915/gvt/cmd_parser.h
new file mode 100644
index 000000000000..bed33514103c
--- /dev/null
+++ b/drivers/gpu/drm/i915/gvt/cmd_parser.h
@@ -0,0 +1,49 @@
+/*
+ * Copyright(c) 2011-2016 Intel Corporation. All rights reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ * Authors:
+ *    Ke Yu
+ *    Kevin Tian <kevin.tian@intel.com>
+ *    Zhiyuan Lv <zhiyuan.lv@intel.com>
+ *
+ * Contributors:
+ *    Min He <min.he@intel.com>
+ *    Ping Gao <ping.a.gao@intel.com>
+ *    Tina Zhang <tina.zhang@intel.com>
+ *    Yulei Zhang <yulei.zhang@intel.com>
+ *    Zhi Wang <zhi.a.wang@intel.com>
+ *
+ */
+#ifndef _GVT_CMD_PARSER_H_
+#define _GVT_CMD_PARSER_H_
+
+#define GVT_CMD_HASH_BITS 7
+
+void intel_gvt_clean_cmd_parser(struct intel_gvt *gvt);
+
+int intel_gvt_init_cmd_parser(struct intel_gvt *gvt);
+
+int intel_gvt_scan_and_shadow_workload(struct intel_vgpu_workload *workload);
+
+int intel_gvt_scan_and_shadow_wa_ctx(struct intel_shadow_wa_ctx *wa_ctx);
+
+#endif
diff --git a/drivers/gpu/drm/i915/gvt/debug.h b/drivers/gpu/drm/i915/gvt/debug.h
index 7ef412be665f..68cba7bd980a 100644
--- a/drivers/gpu/drm/i915/gvt/debug.h
+++ b/drivers/gpu/drm/i915/gvt/debug.h
@@ -24,11 +24,34 @@
 #ifndef __GVT_DEBUG_H__
 #define __GVT_DEBUG_H__
 
+#define gvt_err(fmt, args...) \
+	DRM_ERROR("gvt: "fmt, ##args)
+
 #define gvt_dbg_core(fmt, args...) \
 	DRM_DEBUG_DRIVER("gvt: core: "fmt, ##args)
 
-/*
- * Other GVT debug stuff will be introduced in the GVT device model patches.
- */
+#define gvt_dbg_irq(fmt, args...) \
+	DRM_DEBUG_DRIVER("gvt: irq: "fmt, ##args)
+
+#define gvt_dbg_mm(fmt, args...) \
+	DRM_DEBUG_DRIVER("gvt: mm: "fmt, ##args)
+
+#define gvt_dbg_mmio(fmt, args...) \
+	DRM_DEBUG_DRIVER("gvt: mmio: "fmt, ##args)
+
+#define gvt_dbg_dpy(fmt, args...) \
+	DRM_DEBUG_DRIVER("gvt: dpy: "fmt, ##args)
+
+#define gvt_dbg_el(fmt, args...) \
+	DRM_DEBUG_DRIVER("gvt: el: "fmt, ##args)
+
+#define gvt_dbg_sched(fmt, args...) \
+	DRM_DEBUG_DRIVER("gvt: sched: "fmt, ##args)
+
+#define gvt_dbg_render(fmt, args...) \
+	DRM_DEBUG_DRIVER("gvt: render: "fmt, ##args)
+
+#define gvt_dbg_cmd(fmt, args...) \
+	DRM_DEBUG_DRIVER("gvt: cmd: "fmt, ##args)
 
 #endif
diff --git a/drivers/gpu/drm/i915/gvt/display.c b/drivers/gpu/drm/i915/gvt/display.c
new file mode 100644
index 000000000000..c0c884aeb30e
--- /dev/null
+++ b/drivers/gpu/drm/i915/gvt/display.c
@@ -0,0 +1,330 @@
+/*
+ * Copyright(c) 2011-2016 Intel Corporation. All rights reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ * Authors:
+ *    Ke Yu
+ *    Zhiyuan Lv <zhiyuan.lv@intel.com>
+ *
+ * Contributors:
+ *    Terrence Xu <terrence.xu@intel.com>
+ *    Changbin Du <changbin.du@intel.com>
+ *    Bing Niu <bing.niu@intel.com>
+ *    Zhi Wang <zhi.a.wang@intel.com>
+ *
+ */
+
+#include "i915_drv.h"
+#include "gvt.h"
+
+static int get_edp_pipe(struct intel_vgpu *vgpu)
+{
+	u32 data = vgpu_vreg(vgpu, _TRANS_DDI_FUNC_CTL_EDP);
+	int pipe = -1;
+
+	switch (data & TRANS_DDI_EDP_INPUT_MASK) {
+	case TRANS_DDI_EDP_INPUT_A_ON:
+	case TRANS_DDI_EDP_INPUT_A_ONOFF:
+		pipe = PIPE_A;
+		break;
+	case TRANS_DDI_EDP_INPUT_B_ONOFF:
+		pipe = PIPE_B;
+		break;
+	case TRANS_DDI_EDP_INPUT_C_ONOFF:
+		pipe = PIPE_C;
+		break;
+	}
+	return pipe;
+}
+
+static int edp_pipe_is_enabled(struct intel_vgpu *vgpu)
+{
+	struct drm_i915_private *dev_priv = vgpu->gvt->dev_priv;
+
+	if (!(vgpu_vreg(vgpu, PIPECONF(_PIPE_EDP)) & PIPECONF_ENABLE))
+		return 0;
+
+	if (!(vgpu_vreg(vgpu, _TRANS_DDI_FUNC_CTL_EDP) & TRANS_DDI_FUNC_ENABLE))
+		return 0;
+	return 1;
+}
+
+static int pipe_is_enabled(struct intel_vgpu *vgpu, int pipe)
+{
+	struct drm_i915_private *dev_priv = vgpu->gvt->dev_priv;
+
+	if (WARN_ON(pipe < PIPE_A || pipe >= I915_MAX_PIPES))
+		return -EINVAL;
+
+	if (vgpu_vreg(vgpu, PIPECONF(pipe)) & PIPECONF_ENABLE)
+		return 1;
+
+	if (edp_pipe_is_enabled(vgpu) &&
+			get_edp_pipe(vgpu) == pipe)
+		return 1;
+	return 0;
+}
+
+/* EDID with 1024x768 as its resolution */
+static unsigned char virtual_dp_monitor_edid[] = {
+	/*Header*/
+	0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00,
+	/* Vendor & Product Identification */
+	0x22, 0xf0, 0x54, 0x29, 0x00, 0x00, 0x00, 0x00, 0x04, 0x17,
+	/* Version & Revision */
+	0x01, 0x04,
+	/* Basic Display Parameters & Features */
+	0xa5, 0x34, 0x20, 0x78, 0x23,
+	/* Color Characteristics */
+	0xfc, 0x81, 0xa4, 0x55, 0x4d, 0x9d, 0x25, 0x12, 0x50, 0x54,
+	/* Established Timings: maximum resolution is 1024x768 */
+	0x21, 0x08, 0x00,
+	/* Standard Timings. All invalid */
+	0x00, 0xc0, 0x00, 0xc0, 0x00, 0x40, 0x00, 0x80, 0x00, 0x00,
+	0x00, 0x40, 0x00, 0x00, 0x00, 0x01,
+	/* 18 Byte Data Blocks 1: invalid */
+	0x00, 0x00, 0x80, 0xa0, 0x70, 0xb0,
+	0x23, 0x40, 0x30, 0x20, 0x36, 0x00, 0x06, 0x44, 0x21, 0x00, 0x00, 0x1a,
+	/* 18 Byte Data Blocks 2: invalid */
+	0x00, 0x00, 0x00, 0xfd, 0x00, 0x18, 0x3c, 0x18, 0x50, 0x11, 0x00, 0x0a,
+	0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
+	/* 18 Byte Data Blocks 3: invalid */
+	0x00, 0x00, 0x00, 0xfc, 0x00, 0x48,
+	0x50, 0x20, 0x5a, 0x52, 0x32, 0x34, 0x34, 0x30, 0x77, 0x0a, 0x20, 0x20,
+	/* 18 Byte Data Blocks 4: invalid */
+	0x00, 0x00, 0x00, 0xff, 0x00, 0x43, 0x4e, 0x34, 0x33, 0x30, 0x34, 0x30,
+	0x44, 0x58, 0x51, 0x0a, 0x20, 0x20,
+	/* Extension Block Count */
+	0x00,
+	/* Checksum */
+	0xef,
+};
+
+#define DPCD_HEADER_SIZE        0xb
+
+static u8 dpcd_fix_data[DPCD_HEADER_SIZE] = {
+	0x11, 0x0a, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
+};
+
+static void emulate_monitor_status_change(struct intel_vgpu *vgpu)
+{
+	struct drm_i915_private *dev_priv = vgpu->gvt->dev_priv;
+	vgpu_vreg(vgpu, SDEISR) &= ~(SDE_PORTB_HOTPLUG_CPT |
+			SDE_PORTC_HOTPLUG_CPT |
+			SDE_PORTD_HOTPLUG_CPT);
+
+	if (IS_SKYLAKE(dev_priv))
+		vgpu_vreg(vgpu, SDEISR) &= ~(SDE_PORTA_HOTPLUG_SPT |
+				SDE_PORTE_HOTPLUG_SPT);
+
+	if (intel_vgpu_has_monitor_on_port(vgpu, PORT_B))
+		vgpu_vreg(vgpu, SDEISR) |= SDE_PORTB_HOTPLUG_CPT;
+
+	if (intel_vgpu_has_monitor_on_port(vgpu, PORT_C))
+		vgpu_vreg(vgpu, SDEISR) |= SDE_PORTC_HOTPLUG_CPT;
+
+	if (intel_vgpu_has_monitor_on_port(vgpu, PORT_D))
+		vgpu_vreg(vgpu, SDEISR) |= SDE_PORTD_HOTPLUG_CPT;
+
+	if (IS_SKYLAKE(dev_priv) &&
+			intel_vgpu_has_monitor_on_port(vgpu, PORT_E)) {
+		vgpu_vreg(vgpu, SDEISR) |= SDE_PORTE_HOTPLUG_SPT;
+	}
+
+	if (intel_vgpu_has_monitor_on_port(vgpu, PORT_A)) {
+		if (IS_BROADWELL(dev_priv))
+			vgpu_vreg(vgpu, GEN8_DE_PORT_ISR) |=
+				GEN8_PORT_DP_A_HOTPLUG;
+		else
+			vgpu_vreg(vgpu, SDEISR) |= SDE_PORTA_HOTPLUG_SPT;
+	}
+}
+
+static void clean_virtual_dp_monitor(struct intel_vgpu *vgpu, int port_num)
+{
+	struct intel_vgpu_port *port = intel_vgpu_port(vgpu, port_num);
+
+	kfree(port->edid);
+	port->edid = NULL;
+
+	kfree(port->dpcd);
+	port->dpcd = NULL;
+}
+
+static int setup_virtual_dp_monitor(struct intel_vgpu *vgpu, int port_num,
+		int type)
+{
+	struct intel_vgpu_port *port = intel_vgpu_port(vgpu, port_num);
+
+	port->edid = kzalloc(sizeof(*(port->edid)), GFP_KERNEL);
+	if (!port->edid)
+		return -ENOMEM;
+
+	port->dpcd = kzalloc(sizeof(*(port->dpcd)), GFP_KERNEL);
+	if (!port->dpcd) {
+		kfree(port->edid);
+		return -ENOMEM;
+	}
+
+	memcpy(port->edid->edid_block, virtual_dp_monitor_edid,
+			EDID_SIZE);
+	port->edid->data_valid = true;
+
+	memcpy(port->dpcd->data, dpcd_fix_data, DPCD_HEADER_SIZE);
+	port->dpcd->data_valid = true;
+	port->dpcd->data[DPCD_SINK_COUNT] = 0x1;
+	port->type = type;
+
+	emulate_monitor_status_change(vgpu);
+	return 0;
+}
+
+/**
+ * intel_gvt_check_vblank_emulation - check if vblank emulation timer should
+ * be turned on/off when a virtual pipe is enabled/disabled.
+ * @gvt: a GVT device
+ *
+ * This function is used to turn on/off vblank timer according to currently
+ * enabled/disabled virtual pipes.
+ *
+ */
+void intel_gvt_check_vblank_emulation(struct intel_gvt *gvt)
+{
+	struct intel_gvt_irq *irq = &gvt->irq;
+	struct intel_vgpu *vgpu;
+	bool have_enabled_pipe = false;
+	int pipe, id;
+
+	if (WARN_ON(!mutex_is_locked(&gvt->lock)))
+		return;
+
+	hrtimer_cancel(&irq->vblank_timer.timer);
+
+	for_each_active_vgpu(gvt, vgpu, id) {
+		for (pipe = 0; pipe < I915_MAX_PIPES; pipe++) {
+			have_enabled_pipe =
+				pipe_is_enabled(vgpu, pipe);
+			if (have_enabled_pipe)
+				break;
+		}
+	}
+
+	if (have_enabled_pipe)
+		hrtimer_start(&irq->vblank_timer.timer,
+			ktime_add_ns(ktime_get(), irq->vblank_timer.period),
+			HRTIMER_MODE_ABS);
+}
+
+static void emulate_vblank_on_pipe(struct intel_vgpu *vgpu, int pipe)
+{
+	struct drm_i915_private *dev_priv = vgpu->gvt->dev_priv;
+	struct intel_vgpu_irq *irq = &vgpu->irq;
+	int vblank_event[] = {
+		[PIPE_A] = PIPE_A_VBLANK,
+		[PIPE_B] = PIPE_B_VBLANK,
+		[PIPE_C] = PIPE_C_VBLANK,
+	};
+	int event;
+
+	if (pipe < PIPE_A || pipe > PIPE_C)
+		return;
+
+	for_each_set_bit(event, irq->flip_done_event[pipe],
+			INTEL_GVT_EVENT_MAX) {
+		clear_bit(event, irq->flip_done_event[pipe]);
+		if (!pipe_is_enabled(vgpu, pipe))
+			continue;
+
+		vgpu_vreg(vgpu, PIPE_FLIPCOUNT_G4X(pipe))++;
+		intel_vgpu_trigger_virtual_event(vgpu, event);
+	}
+
+	if (pipe_is_enabled(vgpu, pipe)) {
+		vgpu_vreg(vgpu, PIPE_FRMCOUNT_G4X(pipe))++;
+		intel_vgpu_trigger_virtual_event(vgpu, vblank_event[pipe]);
+	}
+}
+
+static void emulate_vblank(struct intel_vgpu *vgpu)
+{
+	int pipe;
+
+	for_each_pipe(vgpu->gvt->dev_priv, pipe)
+		emulate_vblank_on_pipe(vgpu, pipe);
+}
+
+/**
+ * intel_gvt_emulate_vblank - trigger vblank events for vGPUs on GVT device
+ * @gvt: a GVT device
+ *
+ * This function is used to trigger vblank interrupts for vGPUs on GVT device
+ *
+ */
+void intel_gvt_emulate_vblank(struct intel_gvt *gvt)
+{
+	struct intel_vgpu *vgpu;
+	int id;
+
+	if (WARN_ON(!mutex_is_locked(&gvt->lock)))
+		return;
+
+	for_each_active_vgpu(gvt, vgpu, id)
+		emulate_vblank(vgpu);
+}
+
+/**
+ * intel_vgpu_clean_display - clean vGPU virtual display emulation
+ * @vgpu: a vGPU
+ *
+ * This function is used to clean vGPU virtual display emulation stuffs
+ *
+ */
+void intel_vgpu_clean_display(struct intel_vgpu *vgpu)
+{
+	struct drm_i915_private *dev_priv = vgpu->gvt->dev_priv;
+
+	if (IS_SKYLAKE(dev_priv))
+		clean_virtual_dp_monitor(vgpu, PORT_D);
+	else
+		clean_virtual_dp_monitor(vgpu, PORT_B);
+}
+
+/**
+ * intel_vgpu_init_display- initialize vGPU virtual display emulation
+ * @vgpu: a vGPU
+ *
+ * This function is used to initialize vGPU virtual display emulation stuffs
+ *
+ * Returns:
+ * Zero on success, negative error code if failed.
+ *
+ */
+int intel_vgpu_init_display(struct intel_vgpu *vgpu)
+{
+	struct drm_i915_private *dev_priv = vgpu->gvt->dev_priv;
+
+	intel_vgpu_init_i2c_edid(vgpu);
+
+	if (IS_SKYLAKE(dev_priv))
+		return setup_virtual_dp_monitor(vgpu, PORT_D, GVT_DP_D);
+	else
+		return setup_virtual_dp_monitor(vgpu, PORT_B, GVT_DP_B);
+}
diff --git a/drivers/gpu/drm/i915/gvt/display.h b/drivers/gpu/drm/i915/gvt/display.h
new file mode 100644
index 000000000000..7a60cb848268
--- /dev/null
+++ b/drivers/gpu/drm/i915/gvt/display.h
@@ -0,0 +1,163 @@
+/*
+ * Copyright(c) 2011-2016 Intel Corporation. All rights reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ * Authors:
+ *    Ke Yu
+ *    Zhiyuan Lv <zhiyuan.lv@intel.com>
+ *
+ * Contributors:
+ *    Terrence Xu <terrence.xu@intel.com>
+ *    Changbin Du <changbin.du@intel.com>
+ *    Bing Niu <bing.niu@intel.com>
+ *    Zhi Wang <zhi.a.wang@intel.com>
+ *
+ */
+
+#ifndef _GVT_DISPLAY_H_
+#define _GVT_DISPLAY_H_
+
+#define SBI_REG_MAX	20
+#define DPCD_SIZE	0x700
+
+#define intel_vgpu_port(vgpu, port) \
+	(&(vgpu->display.ports[port]))
+
+#define intel_vgpu_has_monitor_on_port(vgpu, port) \
+	(intel_vgpu_port(vgpu, port)->edid && \
+		intel_vgpu_port(vgpu, port)->edid->data_valid)
+
+#define intel_vgpu_port_is_dp(vgpu, port) \
+	((intel_vgpu_port(vgpu, port)->type == GVT_DP_A) || \
+	(intel_vgpu_port(vgpu, port)->type == GVT_DP_B) || \
+	(intel_vgpu_port(vgpu, port)->type == GVT_DP_C) || \
+	(intel_vgpu_port(vgpu, port)->type == GVT_DP_D))
+
+#define INTEL_GVT_MAX_UEVENT_VARS	3
+
+/* DPCD start */
+#define DPCD_SIZE	0x700
+
+/* DPCD */
+#define DP_SET_POWER            0x600
+#define DP_SET_POWER_D0         0x1
+#define AUX_NATIVE_WRITE        0x8
+#define AUX_NATIVE_READ         0x9
+
+#define AUX_NATIVE_REPLY_MASK   (0x3 << 4)
+#define AUX_NATIVE_REPLY_ACK    (0x0 << 4)
+#define AUX_NATIVE_REPLY_NAK    (0x1 << 4)
+#define AUX_NATIVE_REPLY_DEFER  (0x2 << 4)
+
+#define AUX_BURST_SIZE          16
+
+/* DPCD addresses */
+#define DPCD_REV			0x000
+#define DPCD_MAX_LINK_RATE		0x001
+#define DPCD_MAX_LANE_COUNT		0x002
+
+#define DPCD_TRAINING_PATTERN_SET	0x102
+#define	DPCD_SINK_COUNT			0x200
+#define DPCD_LANE0_1_STATUS		0x202
+#define DPCD_LANE2_3_STATUS		0x203
+#define DPCD_LANE_ALIGN_STATUS_UPDATED	0x204
+#define DPCD_SINK_STATUS		0x205
+
+/* link training */
+#define DPCD_TRAINING_PATTERN_SET_MASK	0x03
+#define DPCD_LINK_TRAINING_DISABLED	0x00
+#define DPCD_TRAINING_PATTERN_1		0x01
+#define DPCD_TRAINING_PATTERN_2		0x02
+
+#define DPCD_CP_READY_MASK		(1 << 6)
+
+/* lane status */
+#define DPCD_LANES_CR_DONE		0x11
+#define DPCD_LANES_EQ_DONE		0x22
+#define DPCD_SYMBOL_LOCKED		0x44
+
+#define DPCD_INTERLANE_ALIGN_DONE	0x01
+
+#define DPCD_SINK_IN_SYNC		0x03
+/* DPCD end */
+
+#define SBI_RESPONSE_MASK               0x3
+#define SBI_RESPONSE_SHIFT              0x1
+#define SBI_STAT_MASK                   0x1
+#define SBI_STAT_SHIFT                  0x0
+#define SBI_OPCODE_SHIFT                8
+#define SBI_OPCODE_MASK			(0xff << SBI_OPCODE_SHIFT)
+#define SBI_CMD_IORD                    2
+#define SBI_CMD_IOWR                    3
+#define SBI_CMD_CRRD                    6
+#define SBI_CMD_CRWR                    7
+#define SBI_ADDR_OFFSET_SHIFT           16
+#define SBI_ADDR_OFFSET_MASK            (0xffff << SBI_ADDR_OFFSET_SHIFT)
+
+struct intel_vgpu_sbi_register {
+	unsigned int offset;
+	u32 value;
+};
+
+struct intel_vgpu_sbi {
+	int number;
+	struct intel_vgpu_sbi_register registers[SBI_REG_MAX];
+};
+
+enum intel_gvt_plane_type {
+	PRIMARY_PLANE = 0,
+	CURSOR_PLANE,
+	SPRITE_PLANE,
+	MAX_PLANE
+};
+
+struct intel_vgpu_dpcd_data {
+	bool data_valid;
+	u8 data[DPCD_SIZE];
+};
+
+enum intel_vgpu_port_type {
+	GVT_CRT = 0,
+	GVT_DP_A,
+	GVT_DP_B,
+	GVT_DP_C,
+	GVT_DP_D,
+	GVT_HDMI_B,
+	GVT_HDMI_C,
+	GVT_HDMI_D,
+	GVT_PORT_MAX
+};
+
+struct intel_vgpu_port {
+	/* per display EDID information */
+	struct intel_vgpu_edid_data *edid;
+	/* per display DPCD information */
+	struct intel_vgpu_dpcd_data *dpcd;
+	int type;
+};
+
+void intel_gvt_emulate_vblank(struct intel_gvt *gvt);
+void intel_gvt_check_vblank_emulation(struct intel_gvt *gvt);
+
+int intel_vgpu_init_display(struct intel_vgpu *vgpu);
+void intel_vgpu_clean_display(struct intel_vgpu *vgpu);
+
+#endif
diff --git a/drivers/gpu/drm/i915/gvt/edid.c b/drivers/gpu/drm/i915/gvt/edid.c
new file mode 100644
index 000000000000..7e1da1c563ca
--- /dev/null
+++ b/drivers/gpu/drm/i915/gvt/edid.c
@@ -0,0 +1,532 @@
+/*
+ * Copyright(c) 2011-2016 Intel Corporation. All rights reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ * Authors:
+ *    Ke Yu
+ *    Zhiyuan Lv <zhiyuan.lv@intel.com>
+ *
+ * Contributors:
+ *    Terrence Xu <terrence.xu@intel.com>
+ *    Changbin Du <changbin.du@intel.com>
+ *    Bing Niu <bing.niu@intel.com>
+ *    Zhi Wang <zhi.a.wang@intel.com>
+ *
+ */
+
+#include "i915_drv.h"
+#include "gvt.h"
+
+#define GMBUS1_TOTAL_BYTES_SHIFT 16
+#define GMBUS1_TOTAL_BYTES_MASK 0x1ff
+#define gmbus1_total_byte_count(v) (((v) >> \
+	GMBUS1_TOTAL_BYTES_SHIFT) & GMBUS1_TOTAL_BYTES_MASK)
+#define gmbus1_slave_addr(v) (((v) & 0xff) >> 1)
+#define gmbus1_slave_index(v) (((v) >> 8) & 0xff)
+#define gmbus1_bus_cycle(v) (((v) >> 25) & 0x7)
+
+/* GMBUS0 bits definitions */
+#define _GMBUS_PIN_SEL_MASK     (0x7)
+
+static unsigned char edid_get_byte(struct intel_vgpu *vgpu)
+{
+	struct intel_vgpu_i2c_edid *edid = &vgpu->display.i2c_edid;
+	unsigned char chr = 0;
+
+	if (edid->state == I2C_NOT_SPECIFIED || !edid->slave_selected) {
+		gvt_err("Driver tries to read EDID without proper sequence!\n");
+		return 0;
+	}
+	if (edid->current_edid_read >= EDID_SIZE) {
+		gvt_err("edid_get_byte() exceeds the size of EDID!\n");
+		return 0;
+	}
+
+	if (!edid->edid_available) {
+		gvt_err("Reading EDID but EDID is not available!\n");
+		return 0;
+	}
+
+	if (intel_vgpu_has_monitor_on_port(vgpu, edid->port)) {
+		struct intel_vgpu_edid_data *edid_data =
+			intel_vgpu_port(vgpu, edid->port)->edid;
+
+		chr = edid_data->edid_block[edid->current_edid_read];
+		edid->current_edid_read++;
+	} else {
+		gvt_err("No EDID available during the reading?\n");
+	}
+	return chr;
+}
+
+static inline int get_port_from_gmbus0(u32 gmbus0)
+{
+	int port_select = gmbus0 & _GMBUS_PIN_SEL_MASK;
+	int port = -EINVAL;
+
+	if (port_select == 2)
+		port = PORT_E;
+	else if (port_select == 4)
+		port = PORT_C;
+	else if (port_select == 5)
+		port = PORT_B;
+	else if (port_select == 6)
+		port = PORT_D;
+	return port;
+}
+
+static void reset_gmbus_controller(struct intel_vgpu *vgpu)
+{
+	vgpu_vreg(vgpu, PCH_GMBUS2) = GMBUS_HW_RDY;
+	if (!vgpu->display.i2c_edid.edid_available)
+		vgpu_vreg(vgpu, PCH_GMBUS2) |= GMBUS_SATOER;
+	vgpu->display.i2c_edid.gmbus.phase = GMBUS_IDLE_PHASE;
+}
+
+/* GMBUS0 */
+static int gmbus0_mmio_write(struct intel_vgpu *vgpu,
+			unsigned int offset, void *p_data, unsigned int bytes)
+{
+	int port, pin_select;
+
+	memcpy(&vgpu_vreg(vgpu, offset), p_data, bytes);
+
+	pin_select = vgpu_vreg(vgpu, offset) & _GMBUS_PIN_SEL_MASK;
+
+	intel_vgpu_init_i2c_edid(vgpu);
+
+	if (pin_select == 0)
+		return 0;
+
+	port = get_port_from_gmbus0(pin_select);
+	if (WARN_ON(port < 0))
+		return 0;
+
+	vgpu->display.i2c_edid.state = I2C_GMBUS;
+	vgpu->display.i2c_edid.gmbus.phase = GMBUS_IDLE_PHASE;
+
+	vgpu_vreg(vgpu, PCH_GMBUS2) &= ~GMBUS_ACTIVE;
+	vgpu_vreg(vgpu, PCH_GMBUS2) |= GMBUS_HW_RDY | GMBUS_HW_WAIT_PHASE;
+
+	if (intel_vgpu_has_monitor_on_port(vgpu, port) &&
+			!intel_vgpu_port_is_dp(vgpu, port)) {
+		vgpu->display.i2c_edid.port = port;
+		vgpu->display.i2c_edid.edid_available = true;
+		vgpu_vreg(vgpu, PCH_GMBUS2) &= ~GMBUS_SATOER;
+	} else
+		vgpu_vreg(vgpu, PCH_GMBUS2) |= GMBUS_SATOER;
+	return 0;
+}
+
+static int gmbus1_mmio_write(struct intel_vgpu *vgpu, unsigned int offset,
+		void *p_data, unsigned int bytes)
+{
+	struct intel_vgpu_i2c_edid *i2c_edid = &vgpu->display.i2c_edid;
+	u32 slave_addr;
+	u32 wvalue = *(u32 *)p_data;
+
+	if (vgpu_vreg(vgpu, offset) & GMBUS_SW_CLR_INT) {
+		if (!(wvalue & GMBUS_SW_CLR_INT)) {
+			vgpu_vreg(vgpu, offset) &= ~GMBUS_SW_CLR_INT;
+			reset_gmbus_controller(vgpu);
+		}
+		/*
+		 * TODO: "This bit is cleared to zero when an event
+		 * causes the HW_RDY bit transition to occur "
+		 */
+	} else {
+		/*
+		 * per bspec setting this bit can cause:
+		 * 1) INT status bit cleared
+		 * 2) HW_RDY bit asserted
+		 */
+		if (wvalue & GMBUS_SW_CLR_INT) {
+			vgpu_vreg(vgpu, PCH_GMBUS2) &= ~GMBUS_INT;
+			vgpu_vreg(vgpu, PCH_GMBUS2) |= GMBUS_HW_RDY;
+		}
+
+		/* For virtualization, we suppose that HW is always ready,
+		 * so GMBUS_SW_RDY should always be cleared
+		 */
+		if (wvalue & GMBUS_SW_RDY)
+			wvalue &= ~GMBUS_SW_RDY;
+
+		i2c_edid->gmbus.total_byte_count =
+			gmbus1_total_byte_count(wvalue);
+		slave_addr = gmbus1_slave_addr(wvalue);
+
+		/* vgpu gmbus only support EDID */
+		if (slave_addr == EDID_ADDR) {
+			i2c_edid->slave_selected = true;
+		} else if (slave_addr != 0) {
+			gvt_dbg_dpy(
+				"vgpu%d: unsupported gmbus slave addr(0x%x)\n"
+				"	gmbus operations will be ignored.\n",
+					vgpu->id, slave_addr);
+		}
+
+		if (wvalue & GMBUS_CYCLE_INDEX)
+			i2c_edid->current_edid_read =
+				gmbus1_slave_index(wvalue);
+
+		i2c_edid->gmbus.cycle_type = gmbus1_bus_cycle(wvalue);
+		switch (gmbus1_bus_cycle(wvalue)) {
+		case GMBUS_NOCYCLE:
+			break;
+		case GMBUS_STOP:
+			/* From spec:
+			 * This can only cause a STOP to be generated
+			 * if a GMBUS cycle is generated, the GMBUS is
+			 * currently in a data/wait/idle phase, or it is in a
+			 * WAIT phase
+			 */
+			if (gmbus1_bus_cycle(vgpu_vreg(vgpu, offset))
+				!= GMBUS_NOCYCLE) {
+				intel_vgpu_init_i2c_edid(vgpu);
+				/* After the 'stop' cycle, hw state would become
+				 * 'stop phase' and then 'idle phase' after a
+				 * few milliseconds. In emulation, we just set
+				 * it as 'idle phase' ('stop phase' is not
+				 * visible in gmbus interface)
+				 */
+				i2c_edid->gmbus.phase = GMBUS_IDLE_PHASE;
+				vgpu_vreg(vgpu, PCH_GMBUS2) &= ~GMBUS_ACTIVE;
+			}
+			break;
+		case NIDX_NS_W:
+		case IDX_NS_W:
+		case NIDX_STOP:
+		case IDX_STOP:
+			/* From hw spec the GMBUS phase
+			 * transition like this:
+			 * START (-->INDEX) -->DATA
+			 */
+			i2c_edid->gmbus.phase = GMBUS_DATA_PHASE;
+			vgpu_vreg(vgpu, PCH_GMBUS2) |= GMBUS_ACTIVE;
+			break;
+		default:
+			gvt_err("Unknown/reserved GMBUS cycle detected!\n");
+			break;
+		}
+		/*
+		 * From hw spec the WAIT state will be
+		 * cleared:
+		 * (1) in a new GMBUS cycle
+		 * (2) by generating a stop
+		 */
+		vgpu_vreg(vgpu, offset) = wvalue;
+	}
+	return 0;
+}
+
+static int gmbus3_mmio_write(struct intel_vgpu *vgpu, unsigned int offset,
+	void *p_data, unsigned int bytes)
+{
+	WARN_ON(1);
+	return 0;
+}
+
+static int gmbus3_mmio_read(struct intel_vgpu *vgpu, unsigned int offset,
+		void *p_data, unsigned int bytes)
+{
+	int i;
+	unsigned char byte_data;
+	struct intel_vgpu_i2c_edid *i2c_edid = &vgpu->display.i2c_edid;
+	int byte_left = i2c_edid->gmbus.total_byte_count -
+				i2c_edid->current_edid_read;
+	int byte_count = byte_left;
+	u32 reg_data = 0;
+
+	/* Data can only be recevied if previous settings correct */
+	if (vgpu_vreg(vgpu, PCH_GMBUS1) & GMBUS_SLAVE_READ) {
+		if (byte_left <= 0) {
+			memcpy(p_data, &vgpu_vreg(vgpu, offset), bytes);
+			return 0;
+		}
+
+		if (byte_count > 4)
+			byte_count = 4;
+		for (i = 0; i < byte_count; i++) {
+			byte_data = edid_get_byte(vgpu);
+			reg_data |= (byte_data << (i << 3));
+		}
+
+		memcpy(&vgpu_vreg(vgpu, offset), &reg_data, byte_count);
+		memcpy(p_data, &vgpu_vreg(vgpu, offset), bytes);
+
+		if (byte_left <= 4) {
+			switch (i2c_edid->gmbus.cycle_type) {
+			case NIDX_STOP:
+			case IDX_STOP:
+				i2c_edid->gmbus.phase = GMBUS_IDLE_PHASE;
+				break;
+			case NIDX_NS_W:
+			case IDX_NS_W:
+			default:
+				i2c_edid->gmbus.phase = GMBUS_WAIT_PHASE;
+				break;
+			}
+			intel_vgpu_init_i2c_edid(vgpu);
+		}
+		/*
+		 * Read GMBUS3 during send operation,
+		 * return the latest written value
+		 */
+	} else {
+		memcpy(p_data, &vgpu_vreg(vgpu, offset), bytes);
+		gvt_err("vgpu%d: warning: gmbus3 read with nothing returned\n",
+				vgpu->id);
+	}
+	return 0;
+}
+
+static int gmbus2_mmio_read(struct intel_vgpu *vgpu, unsigned int offset,
+		void *p_data, unsigned int bytes)
+{
+	u32 value = vgpu_vreg(vgpu, offset);
+
+	if (!(vgpu_vreg(vgpu, offset) & GMBUS_INUSE))
+		vgpu_vreg(vgpu, offset) |= GMBUS_INUSE;
+	memcpy(p_data, (void *)&value, bytes);
+	return 0;
+}
+
+static int gmbus2_mmio_write(struct intel_vgpu *vgpu, unsigned int offset,
+		void *p_data, unsigned int bytes)
+{
+	u32 wvalue = *(u32 *)p_data;
+
+	if (wvalue & GMBUS_INUSE)
+		vgpu_vreg(vgpu, offset) &= ~GMBUS_INUSE;
+	/* All other bits are read-only */
+	return 0;
+}
+
+/**
+ * intel_gvt_i2c_handle_gmbus_read - emulate gmbus register mmio read
+ * @vgpu: a vGPU
+ *
+ * This function is used to emulate gmbus register mmio read
+ *
+ * Returns:
+ * Zero on success, negative error code if failed.
+ *
+ */
+int intel_gvt_i2c_handle_gmbus_read(struct intel_vgpu *vgpu,
+	unsigned int offset, void *p_data, unsigned int bytes)
+{
+	if (WARN_ON(bytes > 8 && (offset & (bytes - 1))))
+		return -EINVAL;
+
+	if (offset == i915_mmio_reg_offset(PCH_GMBUS2))
+		return gmbus2_mmio_read(vgpu, offset, p_data, bytes);
+	else if (offset == i915_mmio_reg_offset(PCH_GMBUS3))
+		return gmbus3_mmio_read(vgpu, offset, p_data, bytes);
+
+	memcpy(p_data, &vgpu_vreg(vgpu, offset), bytes);
+	return 0;
+}
+
+/**
+ * intel_gvt_i2c_handle_gmbus_write - emulate gmbus register mmio write
+ * @vgpu: a vGPU
+ *
+ * This function is used to emulate gmbus register mmio write
+ *
+ * Returns:
+ * Zero on success, negative error code if failed.
+ *
+ */
+int intel_gvt_i2c_handle_gmbus_write(struct intel_vgpu *vgpu,
+		unsigned int offset, void *p_data, unsigned int bytes)
+{
+	if (WARN_ON(bytes > 8 && (offset & (bytes - 1))))
+		return -EINVAL;
+
+	if (offset == i915_mmio_reg_offset(PCH_GMBUS0))
+		return gmbus0_mmio_write(vgpu, offset, p_data, bytes);
+	else if (offset == i915_mmio_reg_offset(PCH_GMBUS1))
+		return gmbus1_mmio_write(vgpu, offset, p_data, bytes);
+	else if (offset == i915_mmio_reg_offset(PCH_GMBUS2))
+		return gmbus2_mmio_write(vgpu, offset, p_data, bytes);
+	else if (offset == i915_mmio_reg_offset(PCH_GMBUS3))
+		return gmbus3_mmio_write(vgpu, offset, p_data, bytes);
+
+	memcpy(&vgpu_vreg(vgpu, offset), p_data, bytes);
+	return 0;
+}
+
+enum {
+	AUX_CH_CTL = 0,
+	AUX_CH_DATA1,
+	AUX_CH_DATA2,
+	AUX_CH_DATA3,
+	AUX_CH_DATA4,
+	AUX_CH_DATA5
+};
+
+static inline int get_aux_ch_reg(unsigned int offset)
+{
+	int reg;
+
+	switch (offset & 0xff) {
+	case 0x10:
+		reg = AUX_CH_CTL;
+		break;
+	case 0x14:
+		reg = AUX_CH_DATA1;
+		break;
+	case 0x18:
+		reg = AUX_CH_DATA2;
+		break;
+	case 0x1c:
+		reg = AUX_CH_DATA3;
+		break;
+	case 0x20:
+		reg = AUX_CH_DATA4;
+		break;
+	case 0x24:
+		reg = AUX_CH_DATA5;
+		break;
+	default:
+		reg = -1;
+		break;
+	}
+	return reg;
+}
+
+#define AUX_CTL_MSG_LENGTH(reg) \
+	((reg & DP_AUX_CH_CTL_MESSAGE_SIZE_MASK) >> \
+		DP_AUX_CH_CTL_MESSAGE_SIZE_SHIFT)
+
+/**
+ * intel_gvt_i2c_handle_aux_ch_write - emulate AUX channel register write
+ * @vgpu: a vGPU
+ *
+ * This function is used to emulate AUX channel register write
+ *
+ */
+void intel_gvt_i2c_handle_aux_ch_write(struct intel_vgpu *vgpu,
+				int port_idx,
+				unsigned int offset,
+				void *p_data)
+{
+	struct intel_vgpu_i2c_edid *i2c_edid = &vgpu->display.i2c_edid;
+	int msg_length, ret_msg_size;
+	int msg, addr, ctrl, op;
+	u32 value = *(u32 *)p_data;
+	int aux_data_for_write = 0;
+	int reg = get_aux_ch_reg(offset);
+
+	if (reg != AUX_CH_CTL) {
+		vgpu_vreg(vgpu, offset) = value;
+		return;
+	}
+
+	msg_length = AUX_CTL_MSG_LENGTH(value);
+	// check the msg in DATA register.
+	msg = vgpu_vreg(vgpu, offset + 4);
+	addr = (msg >> 8) & 0xffff;
+	ctrl = (msg >> 24) & 0xff;
+	op = ctrl >> 4;
+	if (!(value & DP_AUX_CH_CTL_SEND_BUSY)) {
+		/* The ctl write to clear some states */
+		return;
+	}
+
+	/* Always set the wanted value for vms. */
+	ret_msg_size = (((op & 0x1) == GVT_AUX_I2C_READ) ? 2 : 1);
+	vgpu_vreg(vgpu, offset) =
+		DP_AUX_CH_CTL_DONE |
+		((ret_msg_size << DP_AUX_CH_CTL_MESSAGE_SIZE_SHIFT) &
+		DP_AUX_CH_CTL_MESSAGE_SIZE_MASK);
+
+	if (msg_length == 3) {
+		if (!(op & GVT_AUX_I2C_MOT)) {
+			/* stop */
+			intel_vgpu_init_i2c_edid(vgpu);
+		} else {
+			/* start or restart */
+			i2c_edid->aux_ch.i2c_over_aux_ch = true;
+			i2c_edid->aux_ch.aux_ch_mot = true;
+			if (addr == 0) {
+				/* reset the address */
+				intel_vgpu_init_i2c_edid(vgpu);
+			} else if (addr == EDID_ADDR) {
+				i2c_edid->state = I2C_AUX_CH;
+				i2c_edid->port = port_idx;
+				i2c_edid->slave_selected = true;
+				if (intel_vgpu_has_monitor_on_port(vgpu,
+					port_idx) &&
+					intel_vgpu_port_is_dp(vgpu, port_idx))
+					i2c_edid->edid_available = true;
+			}
+		}
+	} else if ((op & 0x1) == GVT_AUX_I2C_WRITE) {
+		/* TODO
+		 * We only support EDID reading from I2C_over_AUX. And
+		 * we do not expect the index mode to be used. Right now
+		 * the WRITE operation is ignored. It is good enough to
+		 * support the gfx driver to do EDID access.
+		 */
+	} else {
+		if (WARN_ON((op & 0x1) != GVT_AUX_I2C_READ))
+			return;
+		if (WARN_ON(msg_length != 4))
+			return;
+		if (i2c_edid->edid_available && i2c_edid->slave_selected) {
+			unsigned char val = edid_get_byte(vgpu);
+
+			aux_data_for_write = (val << 16);
+		}
+	}
+	/* write the return value in AUX_CH_DATA reg which includes:
+	 * ACK of I2C_WRITE
+	 * returned byte if it is READ
+	 */
+
+	aux_data_for_write |= (GVT_AUX_I2C_REPLY_ACK & 0xff) << 24;
+	vgpu_vreg(vgpu, offset + 4) = aux_data_for_write;
+}
+
+/**
+ * intel_vgpu_init_i2c_edid - initialize vGPU i2c edid emulation
+ * @vgpu: a vGPU
+ *
+ * This function is used to initialize vGPU i2c edid emulation stuffs
+ *
+ */
+void intel_vgpu_init_i2c_edid(struct intel_vgpu *vgpu)
+{
+	struct intel_vgpu_i2c_edid *edid = &vgpu->display.i2c_edid;
+
+	edid->state = I2C_NOT_SPECIFIED;
+
+	edid->port = -1;
+	edid->slave_selected = false;
+	edid->edid_available = false;
+	edid->current_edid_read = 0;
+
+	memset(&edid->gmbus, 0, sizeof(struct intel_vgpu_i2c_gmbus));
+
+	edid->aux_ch.i2c_over_aux_ch = false;
+	edid->aux_ch.aux_ch_mot = false;
+}
diff --git a/drivers/gpu/drm/i915/gvt/edid.h b/drivers/gpu/drm/i915/gvt/edid.h
new file mode 100644
index 000000000000..de366b1d5196
--- /dev/null
+++ b/drivers/gpu/drm/i915/gvt/edid.h
@@ -0,0 +1,150 @@
+/*
+ * Copyright(c) 2011-2016 Intel Corporation. All rights reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ * Authors:
+ *    Ke Yu
+ *    Zhiyuan Lv <zhiyuan.lv@intel.com>
+ *
+ * Contributors:
+ *    Terrence Xu <terrence.xu@intel.com>
+ *    Changbin Du <changbin.du@intel.com>
+ *    Bing Niu <bing.niu@intel.com>
+ *    Zhi Wang <zhi.a.wang@intel.com>
+ *
+ */
+
+#ifndef _GVT_EDID_H_
+#define _GVT_EDID_H_
+
+#define EDID_SIZE		128
+#define EDID_ADDR		0x50 /* Linux hvm EDID addr */
+
+#define GVT_AUX_NATIVE_WRITE			0x8
+#define GVT_AUX_NATIVE_READ			0x9
+#define GVT_AUX_I2C_WRITE			0x0
+#define GVT_AUX_I2C_READ			0x1
+#define GVT_AUX_I2C_STATUS			0x2
+#define GVT_AUX_I2C_MOT				0x4
+#define GVT_AUX_I2C_REPLY_ACK			(0x0 << 6)
+
+struct intel_vgpu_edid_data {
+	bool data_valid;
+	unsigned char edid_block[EDID_SIZE];
+};
+
+enum gmbus_cycle_type {
+	GMBUS_NOCYCLE	= 0x0,
+	NIDX_NS_W	= 0x1,
+	IDX_NS_W	= 0x3,
+	GMBUS_STOP	= 0x4,
+	NIDX_STOP	= 0x5,
+	IDX_STOP	= 0x7
+};
+
+/*
+ * States of GMBUS
+ *
+ * GMBUS0-3 could be related to the EDID virtualization. Another two GMBUS
+ * registers, GMBUS4 (interrupt mask) and GMBUS5 (2 byte indes register), are
+ * not considered here. Below describes the usage of GMBUS registers that are
+ * cared by the EDID virtualization
+ *
+ * GMBUS0:
+ *      R/W
+ *      port selection. value of bit0 - bit2 corresponds to the GPIO registers.
+ *
+ * GMBUS1:
+ *      R/W Protect
+ *      Command and Status.
+ *      bit0 is the direction bit: 1 is read; 0 is write.
+ *      bit1 - bit7 is slave 7-bit address.
+ *      bit16 - bit24 total byte count (ignore?)
+ *
+ * GMBUS2:
+ *      Most of bits are read only except bit 15 (IN_USE)
+ *      Status register
+ *      bit0 - bit8 current byte count
+ *      bit 11: hardware ready;
+ *
+ * GMBUS3:
+ *      Read/Write
+ *      Data for transfer
+ */
+
+/* From hw specs, Other phases like START, ADDRESS, INDEX
+ * are invisible to GMBUS MMIO interface. So no definitions
+ * in below enum types
+ */
+enum gvt_gmbus_phase {
+	GMBUS_IDLE_PHASE = 0,
+	GMBUS_DATA_PHASE,
+	GMBUS_WAIT_PHASE,
+	//GMBUS_STOP_PHASE,
+	GMBUS_MAX_PHASE
+};
+
+struct intel_vgpu_i2c_gmbus {
+	unsigned int total_byte_count; /* from GMBUS1 */
+	enum gmbus_cycle_type cycle_type;
+	enum gvt_gmbus_phase phase;
+};
+
+struct intel_vgpu_i2c_aux_ch {
+	bool i2c_over_aux_ch;
+	bool aux_ch_mot;
+};
+
+enum i2c_state {
+	I2C_NOT_SPECIFIED = 0,
+	I2C_GMBUS = 1,
+	I2C_AUX_CH = 2
+};
+
+/* I2C sequences cannot interleave.
+ * GMBUS and AUX_CH sequences cannot interleave.
+ */
+struct intel_vgpu_i2c_edid {
+	enum i2c_state state;
+
+	unsigned int port;
+	bool slave_selected;
+	bool edid_available;
+	unsigned int current_edid_read;
+
+	struct intel_vgpu_i2c_gmbus gmbus;
+	struct intel_vgpu_i2c_aux_ch aux_ch;
+};
+
+void intel_vgpu_init_i2c_edid(struct intel_vgpu *vgpu);
+
+int intel_gvt_i2c_handle_gmbus_read(struct intel_vgpu *vgpu,
+		unsigned int offset, void *p_data, unsigned int bytes);
+
+int intel_gvt_i2c_handle_gmbus_write(struct intel_vgpu *vgpu,
+		unsigned int offset, void *p_data, unsigned int bytes);
+
+void intel_gvt_i2c_handle_aux_ch_write(struct intel_vgpu *vgpu,
+		int port_idx,
+		unsigned int offset,
+		void *p_data);
+
+#endif /*_GVT_EDID_H_*/
diff --git a/drivers/gpu/drm/i915/gvt/execlist.c b/drivers/gpu/drm/i915/gvt/execlist.c
new file mode 100644
index 000000000000..c1f6019d8895
--- /dev/null
+++ b/drivers/gpu/drm/i915/gvt/execlist.c
@@ -0,0 +1,860 @@
+/*
+ * Copyright(c) 2011-2016 Intel Corporation. All rights reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ * Authors:
+ *    Zhiyuan Lv <zhiyuan.lv@intel.com>
+ *    Zhi Wang <zhi.a.wang@intel.com>
+ *
+ * Contributors:
+ *    Min He <min.he@intel.com>
+ *    Bing Niu <bing.niu@intel.com>
+ *    Ping Gao <ping.a.gao@intel.com>
+ *    Tina Zhang <tina.zhang@intel.com>
+ *
+ */
+
+#include "i915_drv.h"
+#include "gvt.h"
+
+#define _EL_OFFSET_STATUS       0x234
+#define _EL_OFFSET_STATUS_BUF   0x370
+#define _EL_OFFSET_STATUS_PTR   0x3A0
+
+#define execlist_ring_mmio(gvt, ring_id, offset) \
+	(gvt->dev_priv->engine[ring_id]->mmio_base + (offset))
+
+#define valid_context(ctx) ((ctx)->valid)
+#define same_context(a, b) (((a)->context_id == (b)->context_id) && \
+		((a)->lrca == (b)->lrca))
+
+static int context_switch_events[] = {
+	[RCS] = RCS_AS_CONTEXT_SWITCH,
+	[BCS] = BCS_AS_CONTEXT_SWITCH,
+	[VCS] = VCS_AS_CONTEXT_SWITCH,
+	[VCS2] = VCS2_AS_CONTEXT_SWITCH,
+	[VECS] = VECS_AS_CONTEXT_SWITCH,
+};
+
+static int ring_id_to_context_switch_event(int ring_id)
+{
+	if (WARN_ON(ring_id < RCS && ring_id >
+				ARRAY_SIZE(context_switch_events)))
+		return -EINVAL;
+
+	return context_switch_events[ring_id];
+}
+
+static void switch_virtual_execlist_slot(struct intel_vgpu_execlist *execlist)
+{
+	gvt_dbg_el("[before] running slot %d/context %x pending slot %d\n",
+			execlist->running_slot ?
+			execlist->running_slot->index : -1,
+			execlist->running_context ?
+			execlist->running_context->context_id : 0,
+			execlist->pending_slot ?
+			execlist->pending_slot->index : -1);
+
+	execlist->running_slot = execlist->pending_slot;
+	execlist->pending_slot = NULL;
+	execlist->running_context = execlist->running_context ?
+		&execlist->running_slot->ctx[0] : NULL;
+
+	gvt_dbg_el("[after] running slot %d/context %x pending slot %d\n",
+			execlist->running_slot ?
+			execlist->running_slot->index : -1,
+			execlist->running_context ?
+			execlist->running_context->context_id : 0,
+			execlist->pending_slot ?
+			execlist->pending_slot->index : -1);
+}
+
+static void emulate_execlist_status(struct intel_vgpu_execlist *execlist)
+{
+	struct intel_vgpu_execlist_slot *running = execlist->running_slot;
+	struct intel_vgpu_execlist_slot *pending = execlist->pending_slot;
+	struct execlist_ctx_descriptor_format *desc = execlist->running_context;
+	struct intel_vgpu *vgpu = execlist->vgpu;
+	struct execlist_status_format status;
+	int ring_id = execlist->ring_id;
+	u32 status_reg = execlist_ring_mmio(vgpu->gvt,
+			ring_id, _EL_OFFSET_STATUS);
+
+	status.ldw = vgpu_vreg(vgpu, status_reg);
+	status.udw = vgpu_vreg(vgpu, status_reg + 4);
+
+	if (running) {
+		status.current_execlist_pointer = !!running->index;
+		status.execlist_write_pointer = !!!running->index;
+		status.execlist_0_active = status.execlist_0_valid =
+			!!!(running->index);
+		status.execlist_1_active = status.execlist_1_valid =
+			!!(running->index);
+	} else {
+		status.context_id = 0;
+		status.execlist_0_active = status.execlist_0_valid = 0;
+		status.execlist_1_active = status.execlist_1_valid = 0;
+	}
+
+	status.context_id = desc ? desc->context_id : 0;
+	status.execlist_queue_full = !!(pending);
+
+	vgpu_vreg(vgpu, status_reg) = status.ldw;
+	vgpu_vreg(vgpu, status_reg + 4) = status.udw;
+
+	gvt_dbg_el("vgpu%d: status reg offset %x ldw %x udw %x\n",
+		vgpu->id, status_reg, status.ldw, status.udw);
+}
+
+static void emulate_csb_update(struct intel_vgpu_execlist *execlist,
+		struct execlist_context_status_format *status,
+		bool trigger_interrupt_later)
+{
+	struct intel_vgpu *vgpu = execlist->vgpu;
+	int ring_id = execlist->ring_id;
+	struct execlist_context_status_pointer_format ctx_status_ptr;
+	u32 write_pointer;
+	u32 ctx_status_ptr_reg, ctx_status_buf_reg, offset;
+
+	ctx_status_ptr_reg = execlist_ring_mmio(vgpu->gvt, ring_id,
+			_EL_OFFSET_STATUS_PTR);
+	ctx_status_buf_reg = execlist_ring_mmio(vgpu->gvt, ring_id,
+			_EL_OFFSET_STATUS_BUF);
+
+	ctx_status_ptr.dw = vgpu_vreg(vgpu, ctx_status_ptr_reg);
+
+	write_pointer = ctx_status_ptr.write_ptr;
+
+	if (write_pointer == 0x7)
+		write_pointer = 0;
+	else {
+		++write_pointer;
+		write_pointer %= 0x6;
+	}
+
+	offset = ctx_status_buf_reg + write_pointer * 8;
+
+	vgpu_vreg(vgpu, offset) = status->ldw;
+	vgpu_vreg(vgpu, offset + 4) = status->udw;
+
+	ctx_status_ptr.write_ptr = write_pointer;
+	vgpu_vreg(vgpu, ctx_status_ptr_reg) = ctx_status_ptr.dw;
+
+	gvt_dbg_el("vgpu%d: w pointer %u reg %x csb l %x csb h %x\n",
+		vgpu->id, write_pointer, offset, status->ldw, status->udw);
+
+	if (trigger_interrupt_later)
+		return;
+
+	intel_vgpu_trigger_virtual_event(vgpu,
+			ring_id_to_context_switch_event(execlist->ring_id));
+}
+
+static int emulate_execlist_ctx_schedule_out(
+		struct intel_vgpu_execlist *execlist,
+		struct execlist_ctx_descriptor_format *ctx)
+{
+	struct intel_vgpu_execlist_slot *running = execlist->running_slot;
+	struct intel_vgpu_execlist_slot *pending = execlist->pending_slot;
+	struct execlist_ctx_descriptor_format *ctx0 = &running->ctx[0];
+	struct execlist_ctx_descriptor_format *ctx1 = &running->ctx[1];
+	struct execlist_context_status_format status;
+
+	memset(&status, 0, sizeof(status));
+
+	gvt_dbg_el("schedule out context id %x\n", ctx->context_id);
+
+	if (WARN_ON(!same_context(ctx, execlist->running_context))) {
+		gvt_err("schedule out context is not running context,"
+				"ctx id %x running ctx id %x\n",
+				ctx->context_id,
+				execlist->running_context->context_id);
+		return -EINVAL;
+	}
+
+	/* ctx1 is valid, ctx0/ctx is scheduled-out -> element switch */
+	if (valid_context(ctx1) && same_context(ctx0, ctx)) {
+		gvt_dbg_el("ctx 1 valid, ctx/ctx 0 is scheduled-out\n");
+
+		execlist->running_context = ctx1;
+
+		emulate_execlist_status(execlist);
+
+		status.context_complete = status.element_switch = 1;
+		status.context_id = ctx->context_id;
+
+		emulate_csb_update(execlist, &status, false);
+		/*
+		 * ctx1 is not valid, ctx == ctx0
+		 * ctx1 is valid, ctx1 == ctx
+		 *	--> last element is finished
+		 * emulate:
+		 *	active-to-idle if there is *no* pending execlist
+		 *	context-complete if there *is* pending execlist
+		 */
+	} else if ((!valid_context(ctx1) && same_context(ctx0, ctx))
+			|| (valid_context(ctx1) && same_context(ctx1, ctx))) {
+		gvt_dbg_el("need to switch virtual execlist slot\n");
+
+		switch_virtual_execlist_slot(execlist);
+
+		emulate_execlist_status(execlist);
+
+		status.context_complete = status.active_to_idle = 1;
+		status.context_id = ctx->context_id;
+
+		if (!pending) {
+			emulate_csb_update(execlist, &status, false);
+		} else {
+			emulate_csb_update(execlist, &status, true);
+
+			memset(&status, 0, sizeof(status));
+
+			status.idle_to_active = 1;
+			status.context_id = 0;
+
+			emulate_csb_update(execlist, &status, false);
+		}
+	} else {
+		WARN_ON(1);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static struct intel_vgpu_execlist_slot *get_next_execlist_slot(
+		struct intel_vgpu_execlist *execlist)
+{
+	struct intel_vgpu *vgpu = execlist->vgpu;
+	int ring_id = execlist->ring_id;
+	u32 status_reg = execlist_ring_mmio(vgpu->gvt, ring_id,
+			_EL_OFFSET_STATUS);
+	struct execlist_status_format status;
+
+	status.ldw = vgpu_vreg(vgpu, status_reg);
+	status.udw = vgpu_vreg(vgpu, status_reg + 4);
+
+	if (status.execlist_queue_full) {
+		gvt_err("virtual execlist slots are full\n");
+		return NULL;
+	}
+
+	return &execlist->slot[status.execlist_write_pointer];
+}
+
+static int emulate_execlist_schedule_in(struct intel_vgpu_execlist *execlist,
+		struct execlist_ctx_descriptor_format ctx[2])
+{
+	struct intel_vgpu_execlist_slot *running = execlist->running_slot;
+	struct intel_vgpu_execlist_slot *slot =
+		get_next_execlist_slot(execlist);
+
+	struct execlist_ctx_descriptor_format *ctx0, *ctx1;
+	struct execlist_context_status_format status;
+
+	gvt_dbg_el("emulate schedule-in\n");
+
+	if (!slot) {
+		gvt_err("no available execlist slot\n");
+		return -EINVAL;
+	}
+
+	memset(&status, 0, sizeof(status));
+	memset(slot->ctx, 0, sizeof(slot->ctx));
+
+	slot->ctx[0] = ctx[0];
+	slot->ctx[1] = ctx[1];
+
+	gvt_dbg_el("alloc slot index %d ctx 0 %x ctx 1 %x\n",
+			slot->index, ctx[0].context_id,
+			ctx[1].context_id);
+
+	/*
+	 * no running execlist, make this write bundle as running execlist
+	 * -> idle-to-active
+	 */
+	if (!running) {
+		gvt_dbg_el("no current running execlist\n");
+
+		execlist->running_slot = slot;
+		execlist->pending_slot = NULL;
+		execlist->running_context = &slot->ctx[0];
+
+		gvt_dbg_el("running slot index %d running context %x\n",
+				execlist->running_slot->index,
+				execlist->running_context->context_id);
+
+		emulate_execlist_status(execlist);
+
+		status.idle_to_active = 1;
+		status.context_id = 0;
+
+		emulate_csb_update(execlist, &status, false);
+		return 0;
+	}
+
+	ctx0 = &running->ctx[0];
+	ctx1 = &running->ctx[1];
+
+	gvt_dbg_el("current running slot index %d ctx 0 %x ctx 1 %x\n",
+		running->index, ctx0->context_id, ctx1->context_id);
+
+	/*
+	 * already has an running execlist
+	 *	a. running ctx1 is valid,
+	 *	   ctx0 is finished, and running ctx1 == new execlist ctx[0]
+	 *	b. running ctx1 is not valid,
+	 *	   ctx0 == new execlist ctx[0]
+	 * ----> lite-restore + preempted
+	 */
+	if ((valid_context(ctx1) && same_context(ctx1, &slot->ctx[0]) &&
+		/* condition a */
+		(!same_context(ctx0, execlist->running_context))) ||
+			(!valid_context(ctx1) &&
+			 same_context(ctx0, &slot->ctx[0]))) { /* condition b */
+		gvt_dbg_el("need to switch virtual execlist slot\n");
+
+		execlist->pending_slot = slot;
+		switch_virtual_execlist_slot(execlist);
+
+		emulate_execlist_status(execlist);
+
+		status.lite_restore = status.preempted = 1;
+		status.context_id = ctx[0].context_id;
+
+		emulate_csb_update(execlist, &status, false);
+	} else {
+		gvt_dbg_el("emulate as pending slot\n");
+		/*
+		 * otherwise
+		 * --> emulate pending execlist exist + but no preemption case
+		 */
+		execlist->pending_slot = slot;
+		emulate_execlist_status(execlist);
+	}
+	return 0;
+}
+
+static void free_workload(struct intel_vgpu_workload *workload)
+{
+	intel_vgpu_unpin_mm(workload->shadow_mm);
+	intel_gvt_mm_unreference(workload->shadow_mm);
+	kmem_cache_free(workload->vgpu->workloads, workload);
+}
+
+#define get_desc_from_elsp_dwords(ed, i) \
+	((struct execlist_ctx_descriptor_format *)&((ed)->data[i * 2]))
+
+
+#define BATCH_BUFFER_ADDR_MASK ((1UL << 32) - (1U << 2))
+#define BATCH_BUFFER_ADDR_HIGH_MASK ((1UL << 16) - (1U))
+static int set_gma_to_bb_cmd(struct intel_shadow_bb_entry *entry_obj,
+			     unsigned long add, int gmadr_bytes)
+{
+	if (WARN_ON(gmadr_bytes != 4 && gmadr_bytes != 8))
+		return -1;
+
+	*((u32 *)(entry_obj->bb_start_cmd_va + (1 << 2))) = add &
+		BATCH_BUFFER_ADDR_MASK;
+	if (gmadr_bytes == 8) {
+		*((u32 *)(entry_obj->bb_start_cmd_va + (2 << 2))) =
+			add & BATCH_BUFFER_ADDR_HIGH_MASK;
+	}
+
+	return 0;
+}
+
+static void prepare_shadow_batch_buffer(struct intel_vgpu_workload *workload)
+{
+	int gmadr_bytes = workload->vgpu->gvt->device_info.gmadr_bytes_in_cmd;
+
+	/* pin the gem object to ggtt */
+	if (!list_empty(&workload->shadow_bb)) {
+		struct intel_shadow_bb_entry *entry_obj =
+			list_first_entry(&workload->shadow_bb,
+					 struct intel_shadow_bb_entry,
+					 list);
+		struct intel_shadow_bb_entry *temp;
+
+		list_for_each_entry_safe(entry_obj, temp, &workload->shadow_bb,
+				list) {
+			struct i915_vma *vma;
+
+			vma = i915_gem_object_ggtt_pin(entry_obj->obj, NULL, 0,
+						       4, 0);
+			if (IS_ERR(vma)) {
+				gvt_err("Cannot pin\n");
+				return;
+			}
+
+			/* FIXME: we are not tracking our pinned VMA leaving it
+			 * up to the core to fix up the stray pin_count upon
+			 * free.
+			 */
+
+			/* update the relocate gma with shadow batch buffer*/
+			set_gma_to_bb_cmd(entry_obj,
+					  i915_ggtt_offset(vma),
+					  gmadr_bytes);
+		}
+	}
+}
+
+static int update_wa_ctx_2_shadow_ctx(struct intel_shadow_wa_ctx *wa_ctx)
+{
+	int ring_id = wa_ctx->workload->ring_id;
+	struct i915_gem_context *shadow_ctx =
+		wa_ctx->workload->vgpu->shadow_ctx;
+	struct drm_i915_gem_object *ctx_obj =
+		shadow_ctx->engine[ring_id].state->obj;
+	struct execlist_ring_context *shadow_ring_context;
+	struct page *page;
+
+	page = i915_gem_object_get_page(ctx_obj, LRC_STATE_PN);
+	shadow_ring_context = kmap_atomic(page);
+
+	shadow_ring_context->bb_per_ctx_ptr.val =
+		(shadow_ring_context->bb_per_ctx_ptr.val &
+		(~PER_CTX_ADDR_MASK)) | wa_ctx->per_ctx.shadow_gma;
+	shadow_ring_context->rcs_indirect_ctx.val =
+		(shadow_ring_context->rcs_indirect_ctx.val &
+		(~INDIRECT_CTX_ADDR_MASK)) | wa_ctx->indirect_ctx.shadow_gma;
+
+	kunmap_atomic(shadow_ring_context);
+	return 0;
+}
+
+static void prepare_shadow_wa_ctx(struct intel_shadow_wa_ctx *wa_ctx)
+{
+	struct i915_vma *vma;
+	unsigned char *per_ctx_va =
+		(unsigned char *)wa_ctx->indirect_ctx.shadow_va +
+		wa_ctx->indirect_ctx.size;
+
+	if (wa_ctx->indirect_ctx.size == 0)
+		return;
+
+	vma = i915_gem_object_ggtt_pin(wa_ctx->indirect_ctx.obj, NULL,
+				       0, CACHELINE_BYTES, 0);
+	if (IS_ERR(vma)) {
+		gvt_err("Cannot pin indirect ctx obj\n");
+		return;
+	}
+
+	/* FIXME: we are not tracking our pinned VMA leaving it
+	 * up to the core to fix up the stray pin_count upon
+	 * free.
+	 */
+
+	wa_ctx->indirect_ctx.shadow_gma = i915_ggtt_offset(vma);
+
+	wa_ctx->per_ctx.shadow_gma = *((unsigned int *)per_ctx_va + 1);
+	memset(per_ctx_va, 0, CACHELINE_BYTES);
+
+	update_wa_ctx_2_shadow_ctx(wa_ctx);
+}
+
+static int prepare_execlist_workload(struct intel_vgpu_workload *workload)
+{
+	struct intel_vgpu *vgpu = workload->vgpu;
+	struct execlist_ctx_descriptor_format ctx[2];
+	int ring_id = workload->ring_id;
+
+	intel_vgpu_pin_mm(workload->shadow_mm);
+	intel_vgpu_sync_oos_pages(workload->vgpu);
+	intel_vgpu_flush_post_shadow(workload->vgpu);
+	prepare_shadow_batch_buffer(workload);
+	prepare_shadow_wa_ctx(&workload->wa_ctx);
+	if (!workload->emulate_schedule_in)
+		return 0;
+
+	ctx[0] = *get_desc_from_elsp_dwords(&workload->elsp_dwords, 1);
+	ctx[1] = *get_desc_from_elsp_dwords(&workload->elsp_dwords, 0);
+
+	return emulate_execlist_schedule_in(&vgpu->execlist[ring_id], ctx);
+}
+
+static void release_shadow_batch_buffer(struct intel_vgpu_workload *workload)
+{
+	/* release all the shadow batch buffer */
+	if (!list_empty(&workload->shadow_bb)) {
+		struct intel_shadow_bb_entry *entry_obj =
+			list_first_entry(&workload->shadow_bb,
+					 struct intel_shadow_bb_entry,
+					 list);
+		struct intel_shadow_bb_entry *temp;
+
+		list_for_each_entry_safe(entry_obj, temp, &workload->shadow_bb,
+					 list) {
+			i915_gem_object_unpin_map(entry_obj->obj);
+			i915_gem_object_put(entry_obj->obj);
+			list_del(&entry_obj->list);
+			kfree(entry_obj);
+		}
+	}
+}
+
+static void release_shadow_wa_ctx(struct intel_shadow_wa_ctx *wa_ctx)
+{
+	if (wa_ctx->indirect_ctx.size == 0)
+		return;
+
+	i915_gem_object_unpin_map(wa_ctx->indirect_ctx.obj);
+	i915_gem_object_put(wa_ctx->indirect_ctx.obj);
+}
+
+static int complete_execlist_workload(struct intel_vgpu_workload *workload)
+{
+	struct intel_vgpu *vgpu = workload->vgpu;
+	struct intel_vgpu_execlist *execlist =
+		&vgpu->execlist[workload->ring_id];
+	struct intel_vgpu_workload *next_workload;
+	struct list_head *next = workload_q_head(vgpu, workload->ring_id)->next;
+	bool lite_restore = false;
+	int ret;
+
+	gvt_dbg_el("complete workload %p status %d\n", workload,
+			workload->status);
+
+	release_shadow_batch_buffer(workload);
+	release_shadow_wa_ctx(&workload->wa_ctx);
+
+	if (workload->status || vgpu->resetting)
+		goto out;
+
+	if (!list_empty(workload_q_head(vgpu, workload->ring_id))) {
+		struct execlist_ctx_descriptor_format *this_desc, *next_desc;
+
+		next_workload = container_of(next,
+				struct intel_vgpu_workload, list);
+		this_desc = &workload->ctx_desc;
+		next_desc = &next_workload->ctx_desc;
+
+		lite_restore = same_context(this_desc, next_desc);
+	}
+
+	if (lite_restore) {
+		gvt_dbg_el("next context == current - no schedule-out\n");
+		free_workload(workload);
+		return 0;
+	}
+
+	ret = emulate_execlist_ctx_schedule_out(execlist, &workload->ctx_desc);
+	if (ret)
+		goto err;
+out:
+	free_workload(workload);
+	return 0;
+err:
+	free_workload(workload);
+	return ret;
+}
+
+#define RING_CTX_OFF(x) \
+	offsetof(struct execlist_ring_context, x)
+
+static void read_guest_pdps(struct intel_vgpu *vgpu,
+		u64 ring_context_gpa, u32 pdp[8])
+{
+	u64 gpa;
+	int i;
+
+	gpa = ring_context_gpa + RING_CTX_OFF(pdp3_UDW.val);
+
+	for (i = 0; i < 8; i++)
+		intel_gvt_hypervisor_read_gpa(vgpu,
+				gpa + i * 8, &pdp[7 - i], 4);
+}
+
+static int prepare_mm(struct intel_vgpu_workload *workload)
+{
+	struct execlist_ctx_descriptor_format *desc = &workload->ctx_desc;
+	struct intel_vgpu_mm *mm;
+	int page_table_level;
+	u32 pdp[8];
+
+	if (desc->addressing_mode == 1) { /* legacy 32-bit */
+		page_table_level = 3;
+	} else if (desc->addressing_mode == 3) { /* legacy 64 bit */
+		page_table_level = 4;
+	} else {
+		gvt_err("Advanced Context mode(SVM) is not supported!\n");
+		return -EINVAL;
+	}
+
+	read_guest_pdps(workload->vgpu, workload->ring_context_gpa, pdp);
+
+	mm = intel_vgpu_find_ppgtt_mm(workload->vgpu, page_table_level, pdp);
+	if (mm) {
+		intel_gvt_mm_reference(mm);
+	} else {
+
+		mm = intel_vgpu_create_mm(workload->vgpu, INTEL_GVT_MM_PPGTT,
+				pdp, page_table_level, 0);
+		if (IS_ERR(mm)) {
+			gvt_err("fail to create mm object.\n");
+			return PTR_ERR(mm);
+		}
+	}
+	workload->shadow_mm = mm;
+	return 0;
+}
+
+#define get_last_workload(q) \
+	(list_empty(q) ? NULL : container_of(q->prev, \
+	struct intel_vgpu_workload, list))
+
+static int submit_context(struct intel_vgpu *vgpu, int ring_id,
+		struct execlist_ctx_descriptor_format *desc,
+		bool emulate_schedule_in)
+{
+	struct list_head *q = workload_q_head(vgpu, ring_id);
+	struct intel_vgpu_workload *last_workload = get_last_workload(q);
+	struct intel_vgpu_workload *workload = NULL;
+	u64 ring_context_gpa;
+	u32 head, tail, start, ctl, ctx_ctl, per_ctx, indirect_ctx;
+	int ret;
+
+	ring_context_gpa = intel_vgpu_gma_to_gpa(vgpu->gtt.ggtt_mm,
+			(u32)((desc->lrca + 1) << GTT_PAGE_SHIFT));
+	if (ring_context_gpa == INTEL_GVT_INVALID_ADDR) {
+		gvt_err("invalid guest context LRCA: %x\n", desc->lrca);
+		return -EINVAL;
+	}
+
+	intel_gvt_hypervisor_read_gpa(vgpu, ring_context_gpa +
+			RING_CTX_OFF(ring_header.val), &head, 4);
+
+	intel_gvt_hypervisor_read_gpa(vgpu, ring_context_gpa +
+			RING_CTX_OFF(ring_tail.val), &tail, 4);
+
+	head &= RB_HEAD_OFF_MASK;
+	tail &= RB_TAIL_OFF_MASK;
+
+	if (last_workload && same_context(&last_workload->ctx_desc, desc)) {
+		gvt_dbg_el("ring id %d cur workload == last\n", ring_id);
+		gvt_dbg_el("ctx head %x real head %lx\n", head,
+				last_workload->rb_tail);
+		/*
+		 * cannot use guest context head pointer here,
+		 * as it might not be updated at this time
+		 */
+		head = last_workload->rb_tail;
+	}
+
+	gvt_dbg_el("ring id %d begin a new workload\n", ring_id);
+
+	workload = kmem_cache_zalloc(vgpu->workloads, GFP_KERNEL);
+	if (!workload)
+		return -ENOMEM;
+
+	/* record some ring buffer register values for scan and shadow */
+	intel_gvt_hypervisor_read_gpa(vgpu, ring_context_gpa +
+			RING_CTX_OFF(rb_start.val), &start, 4);
+	intel_gvt_hypervisor_read_gpa(vgpu, ring_context_gpa +
+			RING_CTX_OFF(rb_ctrl.val), &ctl, 4);
+	intel_gvt_hypervisor_read_gpa(vgpu, ring_context_gpa +
+			RING_CTX_OFF(ctx_ctrl.val), &ctx_ctl, 4);
+
+	INIT_LIST_HEAD(&workload->list);
+	INIT_LIST_HEAD(&workload->shadow_bb);
+
+	init_waitqueue_head(&workload->shadow_ctx_status_wq);
+	atomic_set(&workload->shadow_ctx_active, 0);
+
+	workload->vgpu = vgpu;
+	workload->ring_id = ring_id;
+	workload->ctx_desc = *desc;
+	workload->ring_context_gpa = ring_context_gpa;
+	workload->rb_head = head;
+	workload->rb_tail = tail;
+	workload->rb_start = start;
+	workload->rb_ctl = ctl;
+	workload->prepare = prepare_execlist_workload;
+	workload->complete = complete_execlist_workload;
+	workload->status = -EINPROGRESS;
+	workload->emulate_schedule_in = emulate_schedule_in;
+
+	if (ring_id == RCS) {
+		intel_gvt_hypervisor_read_gpa(vgpu, ring_context_gpa +
+			RING_CTX_OFF(bb_per_ctx_ptr.val), &per_ctx, 4);
+		intel_gvt_hypervisor_read_gpa(vgpu, ring_context_gpa +
+			RING_CTX_OFF(rcs_indirect_ctx.val), &indirect_ctx, 4);
+
+		workload->wa_ctx.indirect_ctx.guest_gma =
+			indirect_ctx & INDIRECT_CTX_ADDR_MASK;
+		workload->wa_ctx.indirect_ctx.size =
+			(indirect_ctx & INDIRECT_CTX_SIZE_MASK) *
+			CACHELINE_BYTES;
+		workload->wa_ctx.per_ctx.guest_gma =
+			per_ctx & PER_CTX_ADDR_MASK;
+		workload->wa_ctx.workload = workload;
+
+		WARN_ON(workload->wa_ctx.indirect_ctx.size && !(per_ctx & 0x1));
+	}
+
+	if (emulate_schedule_in)
+		memcpy(&workload->elsp_dwords,
+				&vgpu->execlist[ring_id].elsp_dwords,
+				sizeof(workload->elsp_dwords));
+
+	gvt_dbg_el("workload %p ring id %d head %x tail %x start %x ctl %x\n",
+			workload, ring_id, head, tail, start, ctl);
+
+	gvt_dbg_el("workload %p emulate schedule_in %d\n", workload,
+			emulate_schedule_in);
+
+	ret = prepare_mm(workload);
+	if (ret) {
+		kmem_cache_free(vgpu->workloads, workload);
+		return ret;
+	}
+
+	queue_workload(workload);
+	return 0;
+}
+
+int intel_vgpu_submit_execlist(struct intel_vgpu *vgpu, int ring_id)
+{
+	struct intel_vgpu_execlist *execlist = &vgpu->execlist[ring_id];
+	struct execlist_ctx_descriptor_format *desc[2], valid_desc[2];
+	unsigned long valid_desc_bitmap = 0;
+	bool emulate_schedule_in = true;
+	int ret;
+	int i;
+
+	memset(valid_desc, 0, sizeof(valid_desc));
+
+	desc[0] = get_desc_from_elsp_dwords(&execlist->elsp_dwords, 1);
+	desc[1] = get_desc_from_elsp_dwords(&execlist->elsp_dwords, 0);
+
+	for (i = 0; i < 2; i++) {
+		if (!desc[i]->valid)
+			continue;
+
+		if (!desc[i]->privilege_access) {
+			gvt_err("vgpu%d: unexpected GGTT elsp submission\n",
+					vgpu->id);
+			return -EINVAL;
+		}
+
+		/* TODO: add another guest context checks here. */
+		set_bit(i, &valid_desc_bitmap);
+		valid_desc[i] = *desc[i];
+	}
+
+	if (!valid_desc_bitmap) {
+		gvt_err("vgpu%d: no valid desc in a elsp submission\n",
+				vgpu->id);
+		return -EINVAL;
+	}
+
+	if (!test_bit(0, (void *)&valid_desc_bitmap) &&
+			test_bit(1, (void *)&valid_desc_bitmap)) {
+		gvt_err("vgpu%d: weird elsp submission, desc 0 is not valid\n",
+				vgpu->id);
+		return -EINVAL;
+	}
+
+	/* submit workload */
+	for_each_set_bit(i, (void *)&valid_desc_bitmap, 2) {
+		ret = submit_context(vgpu, ring_id, &valid_desc[i],
+				emulate_schedule_in);
+		if (ret) {
+			gvt_err("vgpu%d: fail to schedule workload\n",
+					vgpu->id);
+			return ret;
+		}
+		emulate_schedule_in = false;
+	}
+	return 0;
+}
+
+static void init_vgpu_execlist(struct intel_vgpu *vgpu, int ring_id)
+{
+	struct intel_vgpu_execlist *execlist = &vgpu->execlist[ring_id];
+	struct execlist_context_status_pointer_format ctx_status_ptr;
+	u32 ctx_status_ptr_reg;
+
+	memset(execlist, 0, sizeof(*execlist));
+
+	execlist->vgpu = vgpu;
+	execlist->ring_id = ring_id;
+	execlist->slot[0].index = 0;
+	execlist->slot[1].index = 1;
+
+	ctx_status_ptr_reg = execlist_ring_mmio(vgpu->gvt, ring_id,
+			_EL_OFFSET_STATUS_PTR);
+
+	ctx_status_ptr.dw = vgpu_vreg(vgpu, ctx_status_ptr_reg);
+	ctx_status_ptr.read_ptr = ctx_status_ptr.write_ptr = 0x7;
+	vgpu_vreg(vgpu, ctx_status_ptr_reg) = ctx_status_ptr.dw;
+}
+
+void intel_vgpu_clean_execlist(struct intel_vgpu *vgpu)
+{
+	kmem_cache_destroy(vgpu->workloads);
+}
+
+int intel_vgpu_init_execlist(struct intel_vgpu *vgpu)
+{
+	enum intel_engine_id i;
+	struct intel_engine_cs *engine;
+
+	/* each ring has a virtual execlist engine */
+	for_each_engine(engine, vgpu->gvt->dev_priv, i) {
+		init_vgpu_execlist(vgpu, i);
+		INIT_LIST_HEAD(&vgpu->workload_q_head[i]);
+	}
+
+	vgpu->workloads = kmem_cache_create("gvt-g vgpu workload",
+			sizeof(struct intel_vgpu_workload), 0,
+			SLAB_HWCACHE_ALIGN,
+			NULL);
+
+	if (!vgpu->workloads)
+		return -ENOMEM;
+
+	return 0;
+}
+
+void intel_vgpu_reset_execlist(struct intel_vgpu *vgpu,
+		unsigned long ring_bitmap)
+{
+	int bit;
+	struct list_head *pos, *n;
+	struct intel_vgpu_workload *workload = NULL;
+
+	for_each_set_bit(bit, &ring_bitmap, sizeof(ring_bitmap) * 8) {
+		if (bit >= I915_NUM_ENGINES)
+			break;
+		/* free the unsubmited workload in the queue */
+		list_for_each_safe(pos, n, &vgpu->workload_q_head[bit]) {
+			workload = container_of(pos,
+					struct intel_vgpu_workload, list);
+			list_del_init(&workload->list);
+			free_workload(workload);
+		}
+
+		init_vgpu_execlist(vgpu, bit);
+	}
+}
diff --git a/drivers/gpu/drm/i915/gvt/execlist.h b/drivers/gpu/drm/i915/gvt/execlist.h
new file mode 100644
index 000000000000..635f31c6dcc1
--- /dev/null
+++ b/drivers/gpu/drm/i915/gvt/execlist.h
@@ -0,0 +1,188 @@
+/*
+ * Copyright(c) 2011-2016 Intel Corporation. All rights reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ * Authors:
+ *    Zhiyuan Lv <zhiyuan.lv@intel.com>
+ *    Zhi Wang <zhi.a.wang@intel.com>
+ *
+ * Contributors:
+ *    Min He <min.he@intel.com>
+ *    Bing Niu <bing.niu@intel.com>
+ *    Ping Gao <ping.a.gao@intel.com>
+ *    Tina Zhang <tina.zhang@intel.com>
+ *
+ */
+
+#ifndef _GVT_EXECLIST_H_
+#define _GVT_EXECLIST_H_
+
+struct execlist_ctx_descriptor_format {
+	union {
+		u32 udw;
+		u32 context_id;
+	};
+	union {
+		u32 ldw;
+		struct {
+			u32 valid                  : 1;
+			u32 force_pd_restore       : 1;
+			u32 force_restore          : 1;
+			u32 addressing_mode        : 2;
+			u32 llc_coherency          : 1;
+			u32 fault_handling         : 2;
+			u32 privilege_access       : 1;
+			u32 reserved               : 3;
+			u32 lrca                   : 20;
+		};
+	};
+};
+
+struct execlist_status_format {
+	union {
+		u32 ldw;
+		struct {
+			u32 current_execlist_pointer       :1;
+			u32 execlist_write_pointer         :1;
+			u32 execlist_queue_full            :1;
+			u32 execlist_1_valid               :1;
+			u32 execlist_0_valid               :1;
+			u32 last_ctx_switch_reason         :9;
+			u32 current_active_elm_status      :2;
+			u32 arbitration_enable             :1;
+			u32 execlist_1_active              :1;
+			u32 execlist_0_active              :1;
+			u32 reserved                       :13;
+		};
+	};
+	union {
+		u32 udw;
+		u32 context_id;
+	};
+};
+
+struct execlist_context_status_pointer_format {
+	union {
+		u32 dw;
+		struct {
+			u32 write_ptr              :3;
+			u32 reserved               :5;
+			u32 read_ptr               :3;
+			u32 reserved2              :5;
+			u32 mask                   :16;
+		};
+	};
+};
+
+struct execlist_context_status_format {
+	union {
+		u32 ldw;
+		struct {
+			u32 idle_to_active         :1;
+			u32 preempted              :1;
+			u32 element_switch         :1;
+			u32 active_to_idle         :1;
+			u32 context_complete       :1;
+			u32 wait_on_sync_flip      :1;
+			u32 wait_on_vblank         :1;
+			u32 wait_on_semaphore      :1;
+			u32 wait_on_scanline       :1;
+			u32 reserved               :2;
+			u32 semaphore_wait_mode    :1;
+			u32 display_plane          :3;
+			u32 lite_restore           :1;
+			u32 reserved_2             :16;
+		};
+	};
+	union {
+		u32 udw;
+		u32 context_id;
+	};
+};
+
+struct execlist_mmio_pair {
+	u32 addr;
+	u32 val;
+};
+
+/* The first 52 dwords in register state context */
+struct execlist_ring_context {
+	u32 nop1;
+	u32 lri_cmd_1;
+	struct execlist_mmio_pair ctx_ctrl;
+	struct execlist_mmio_pair ring_header;
+	struct execlist_mmio_pair ring_tail;
+	struct execlist_mmio_pair rb_start;
+	struct execlist_mmio_pair rb_ctrl;
+	struct execlist_mmio_pair bb_cur_head_UDW;
+	struct execlist_mmio_pair bb_cur_head_LDW;
+	struct execlist_mmio_pair bb_state;
+	struct execlist_mmio_pair second_bb_addr_UDW;
+	struct execlist_mmio_pair second_bb_addr_LDW;
+	struct execlist_mmio_pair second_bb_state;
+	struct execlist_mmio_pair bb_per_ctx_ptr;
+	struct execlist_mmio_pair rcs_indirect_ctx;
+	struct execlist_mmio_pair rcs_indirect_ctx_offset;
+	u32 nop2;
+	u32 nop3;
+	u32 nop4;
+	u32 lri_cmd_2;
+	struct execlist_mmio_pair ctx_timestamp;
+	struct execlist_mmio_pair pdp3_UDW;
+	struct execlist_mmio_pair pdp3_LDW;
+	struct execlist_mmio_pair pdp2_UDW;
+	struct execlist_mmio_pair pdp2_LDW;
+	struct execlist_mmio_pair pdp1_UDW;
+	struct execlist_mmio_pair pdp1_LDW;
+	struct execlist_mmio_pair pdp0_UDW;
+	struct execlist_mmio_pair pdp0_LDW;
+};
+
+struct intel_vgpu_elsp_dwords {
+	u32 data[4];
+	u32 index;
+};
+
+struct intel_vgpu_execlist_slot {
+	struct execlist_ctx_descriptor_format ctx[2];
+	u32 index;
+};
+
+struct intel_vgpu_execlist {
+	struct intel_vgpu_execlist_slot slot[2];
+	struct intel_vgpu_execlist_slot *running_slot;
+	struct intel_vgpu_execlist_slot *pending_slot;
+	struct execlist_ctx_descriptor_format *running_context;
+	int ring_id;
+	struct intel_vgpu *vgpu;
+	struct intel_vgpu_elsp_dwords elsp_dwords;
+};
+
+void intel_vgpu_clean_execlist(struct intel_vgpu *vgpu);
+
+int intel_vgpu_init_execlist(struct intel_vgpu *vgpu);
+
+int intel_vgpu_submit_execlist(struct intel_vgpu *vgpu, int ring_id);
+
+void intel_vgpu_reset_execlist(struct intel_vgpu *vgpu,
+		unsigned long ring_bitmap);
+
+#endif /*_GVT_EXECLIST_H_*/
diff --git a/drivers/gpu/drm/i915/gvt/firmware.c b/drivers/gpu/drm/i915/gvt/firmware.c
new file mode 100644
index 000000000000..2fae2a2ca96f
--- /dev/null
+++ b/drivers/gpu/drm/i915/gvt/firmware.c
@@ -0,0 +1,312 @@
+/*
+ * Copyright(c) 2011-2016 Intel Corporation. All rights reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ * Authors:
+ *    Zhi Wang <zhi.a.wang@intel.com>
+ *
+ * Contributors:
+ *    Changbin Du <changbin.du@intel.com>
+ *
+ */
+
+#include <linux/firmware.h>
+#include <linux/crc32.h>
+
+#include "i915_drv.h"
+#include "gvt.h"
+#include "i915_pvinfo.h"
+
+#define FIRMWARE_VERSION (0x0)
+
+struct gvt_firmware_header {
+	u64 magic;
+	u32 crc32;		/* protect the data after this field */
+	u32 version;
+	u64 cfg_space_size;
+	u64 cfg_space_offset;	/* offset in the file */
+	u64 mmio_size;
+	u64 mmio_offset;	/* offset in the file */
+	unsigned char data[1];
+};
+
+#define RD(offset) (readl(mmio + offset.reg))
+#define WR(v, offset) (writel(v, mmio + offset.reg))
+
+static void bdw_forcewake_get(void __iomem *mmio)
+{
+	WR(_MASKED_BIT_DISABLE(0xffff), FORCEWAKE_MT);
+
+	RD(ECOBUS);
+
+	if (wait_for((RD(FORCEWAKE_ACK_HSW) & FORCEWAKE_KERNEL) == 0, 50))
+		gvt_err("fail to wait forcewake idle\n");
+
+	WR(_MASKED_BIT_ENABLE(FORCEWAKE_KERNEL), FORCEWAKE_MT);
+
+	if (wait_for((RD(FORCEWAKE_ACK_HSW) & FORCEWAKE_KERNEL), 50))
+		gvt_err("fail to wait forcewake ack\n");
+
+	if (wait_for((RD(GEN6_GT_THREAD_STATUS_REG) &
+		      GEN6_GT_THREAD_STATUS_CORE_MASK) == 0, 50))
+		gvt_err("fail to wait c0 wake up\n");
+}
+
+#undef RD
+#undef WR
+
+#define dev_to_drm_minor(d) dev_get_drvdata((d))
+
+static ssize_t
+gvt_firmware_read(struct file *filp, struct kobject *kobj,
+	     struct bin_attribute *attr, char *buf,
+	     loff_t offset, size_t count)
+{
+	memcpy(buf, attr->private + offset, count);
+	return count;
+}
+
+static struct bin_attribute firmware_attr = {
+	.attr = {.name = "gvt_firmware", .mode = (S_IRUSR)},
+	.read = gvt_firmware_read,
+	.write = NULL,
+	.mmap = NULL,
+};
+
+static int expose_firmware_sysfs(struct intel_gvt *gvt,
+					void __iomem *mmio)
+{
+	struct intel_gvt_device_info *info = &gvt->device_info;
+	struct pci_dev *pdev = gvt->dev_priv->drm.pdev;
+	struct intel_gvt_mmio_info *e;
+	struct gvt_firmware_header *h;
+	void *firmware;
+	void *p;
+	unsigned long size;
+	int i;
+	int ret;
+
+	size = sizeof(*h) + info->mmio_size + info->cfg_space_size - 1;
+	firmware = vmalloc(size);
+	if (!firmware)
+		return -ENOMEM;
+
+	h = firmware;
+
+	h->magic = VGT_MAGIC;
+	h->version = FIRMWARE_VERSION;
+	h->cfg_space_size = info->cfg_space_size;
+	h->cfg_space_offset = offsetof(struct gvt_firmware_header, data);
+	h->mmio_size = info->mmio_size;
+	h->mmio_offset = h->cfg_space_offset + h->cfg_space_size;
+
+	p = firmware + h->cfg_space_offset;
+
+	for (i = 0; i < h->cfg_space_size; i += 4)
+		pci_read_config_dword(pdev, i, p + i);
+
+	memcpy(gvt->firmware.cfg_space, p, info->cfg_space_size);
+
+	p = firmware + h->mmio_offset;
+
+	hash_for_each(gvt->mmio.mmio_info_table, i, e, node) {
+		int j;
+
+		for (j = 0; j < e->length; j += 4)
+			*(u32 *)(p + e->offset + j) =
+				readl(mmio + e->offset + j);
+	}
+
+	memcpy(gvt->firmware.mmio, p, info->mmio_size);
+
+	firmware_attr.size = size;
+	firmware_attr.private = firmware;
+
+	ret = device_create_bin_file(&pdev->dev, &firmware_attr);
+	if (ret) {
+		vfree(firmware);
+		return ret;
+	}
+	return 0;
+}
+
+static void clean_firmware_sysfs(struct intel_gvt *gvt)
+{
+	struct pci_dev *pdev = gvt->dev_priv->drm.pdev;
+
+	device_remove_bin_file(&pdev->dev, &firmware_attr);
+	vfree(firmware_attr.private);
+}
+
+/**
+ * intel_gvt_free_firmware - free GVT firmware
+ * @gvt: intel gvt device
+ *
+ */
+void intel_gvt_free_firmware(struct intel_gvt *gvt)
+{
+	if (!gvt->firmware.firmware_loaded)
+		clean_firmware_sysfs(gvt);
+
+	kfree(gvt->firmware.cfg_space);
+	kfree(gvt->firmware.mmio);
+}
+
+static int verify_firmware(struct intel_gvt *gvt,
+			   const struct firmware *fw)
+{
+	struct intel_gvt_device_info *info = &gvt->device_info;
+	struct drm_i915_private *dev_priv = gvt->dev_priv;
+	struct pci_dev *pdev = dev_priv->drm.pdev;
+	struct gvt_firmware_header *h;
+	unsigned long id, crc32_start;
+	const void *mem;
+	const char *item;
+	u64 file, request;
+
+	h = (struct gvt_firmware_header *)fw->data;
+
+	crc32_start = offsetof(struct gvt_firmware_header, crc32) + 4;
+	mem = fw->data + crc32_start;
+
+#define VERIFY(s, a, b) do { \
+	item = (s); file = (u64)(a); request = (u64)(b); \
+	if ((a) != (b)) \
+		goto invalid_firmware; \
+} while (0)
+
+	VERIFY("magic number", h->magic, VGT_MAGIC);
+	VERIFY("version", h->version, FIRMWARE_VERSION);
+	VERIFY("crc32", h->crc32, crc32_le(0, mem, fw->size - crc32_start));
+	VERIFY("cfg space size", h->cfg_space_size, info->cfg_space_size);
+	VERIFY("mmio size", h->mmio_size, info->mmio_size);
+
+	mem = (fw->data + h->cfg_space_offset);
+
+	id = *(u16 *)(mem + PCI_VENDOR_ID);
+	VERIFY("vender id", id, pdev->vendor);
+
+	id = *(u16 *)(mem + PCI_DEVICE_ID);
+	VERIFY("device id", id, pdev->device);
+
+	id = *(u8 *)(mem + PCI_REVISION_ID);
+	VERIFY("revision id", id, pdev->revision);
+
+#undef VERIFY
+	return 0;
+
+invalid_firmware:
+	gvt_dbg_core("Invalid firmware: %s [file] 0x%llx [request] 0x%llx\n",
+		     item, file, request);
+	return -EINVAL;
+}
+
+#define GVT_FIRMWARE_PATH "i915/gvt"
+
+/**
+ * intel_gvt_load_firmware - load GVT firmware
+ * @gvt: intel gvt device
+ *
+ */
+int intel_gvt_load_firmware(struct intel_gvt *gvt)
+{
+	struct intel_gvt_device_info *info = &gvt->device_info;
+	struct drm_i915_private *dev_priv = gvt->dev_priv;
+	struct pci_dev *pdev = dev_priv->drm.pdev;
+	struct intel_gvt_firmware *firmware = &gvt->firmware;
+	struct gvt_firmware_header *h;
+	const struct firmware *fw;
+	char *path;
+	void __iomem *mmio;
+	void *mem;
+	int ret;
+
+	path = kmalloc(PATH_MAX, GFP_KERNEL);
+	if (!path)
+		return -ENOMEM;
+
+	mem = kmalloc(info->cfg_space_size, GFP_KERNEL);
+	if (!mem) {
+		kfree(path);
+		return -ENOMEM;
+	}
+
+	firmware->cfg_space = mem;
+
+	mem = kmalloc(info->mmio_size, GFP_KERNEL);
+	if (!mem) {
+		kfree(path);
+		kfree(firmware->cfg_space);
+		return -ENOMEM;
+	}
+
+	firmware->mmio = mem;
+
+	mmio = pci_iomap(pdev, info->mmio_bar, info->mmio_size);
+	if (!mmio) {
+		kfree(path);
+		kfree(firmware->cfg_space);
+		kfree(firmware->mmio);
+		return -EINVAL;
+	}
+
+	if (IS_BROADWELL(gvt->dev_priv) || IS_SKYLAKE(gvt->dev_priv))
+		bdw_forcewake_get(mmio);
+
+	sprintf(path, "%s/vid_0x%04x_did_0x%04x_rid_0x%04x.golden_hw_state",
+		 GVT_FIRMWARE_PATH, pdev->vendor, pdev->device,
+		 pdev->revision);
+
+	gvt_dbg_core("request hw state firmware %s...\n", path);
+
+	ret = request_firmware(&fw, path, &dev_priv->drm.pdev->dev);
+	kfree(path);
+
+	if (ret)
+		goto expose_firmware;
+
+	gvt_dbg_core("success.\n");
+
+	ret = verify_firmware(gvt, fw);
+	if (ret)
+		goto out_free_fw;
+
+	gvt_dbg_core("verified.\n");
+
+	h = (struct gvt_firmware_header *)fw->data;
+
+	memcpy(firmware->cfg_space, fw->data + h->cfg_space_offset,
+	       h->cfg_space_size);
+	memcpy(firmware->mmio, fw->data + h->mmio_offset,
+	       h->mmio_size);
+
+	release_firmware(fw);
+	firmware->firmware_loaded = true;
+	pci_iounmap(pdev, mmio);
+	return 0;
+
+out_free_fw:
+	release_firmware(fw);
+expose_firmware:
+	expose_firmware_sysfs(gvt, mmio);
+	pci_iounmap(pdev, mmio);
+	return 0;
+}
diff --git a/drivers/gpu/drm/i915/gvt/gtt.c b/drivers/gpu/drm/i915/gvt/gtt.c
new file mode 100644
index 000000000000..2cc761328569
--- /dev/null
+++ b/drivers/gpu/drm/i915/gvt/gtt.c
@@ -0,0 +1,2232 @@
+/*
+ * GTT virtualization
+ *
+ * Copyright(c) 2011-2016 Intel Corporation. All rights reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ * Authors:
+ *    Zhi Wang <zhi.a.wang@intel.com>
+ *    Zhenyu Wang <zhenyuw@linux.intel.com>
+ *    Xiao Zheng <xiao.zheng@intel.com>
+ *
+ * Contributors:
+ *    Min He <min.he@intel.com>
+ *    Bing Niu <bing.niu@intel.com>
+ *
+ */
+
+#include "i915_drv.h"
+#include "gvt.h"
+#include "i915_pvinfo.h"
+#include "trace.h"
+
+static bool enable_out_of_sync = false;
+static int preallocated_oos_pages = 8192;
+
+/*
+ * validate a gm address and related range size,
+ * translate it to host gm address
+ */
+bool intel_gvt_ggtt_validate_range(struct intel_vgpu *vgpu, u64 addr, u32 size)
+{
+	if ((!vgpu_gmadr_is_valid(vgpu, addr)) || (size
+			&& !vgpu_gmadr_is_valid(vgpu, addr + size - 1))) {
+		gvt_err("vgpu%d: invalid range gmadr 0x%llx size 0x%x\n",
+				vgpu->id, addr, size);
+		return false;
+	}
+	return true;
+}
+
+/* translate a guest gmadr to host gmadr */
+int intel_gvt_ggtt_gmadr_g2h(struct intel_vgpu *vgpu, u64 g_addr, u64 *h_addr)
+{
+	if (WARN(!vgpu_gmadr_is_valid(vgpu, g_addr),
+		 "invalid guest gmadr %llx\n", g_addr))
+		return -EACCES;
+
+	if (vgpu_gmadr_is_aperture(vgpu, g_addr))
+		*h_addr = vgpu_aperture_gmadr_base(vgpu)
+			  + (g_addr - vgpu_aperture_offset(vgpu));
+	else
+		*h_addr = vgpu_hidden_gmadr_base(vgpu)
+			  + (g_addr - vgpu_hidden_offset(vgpu));
+	return 0;
+}
+
+/* translate a host gmadr to guest gmadr */
+int intel_gvt_ggtt_gmadr_h2g(struct intel_vgpu *vgpu, u64 h_addr, u64 *g_addr)
+{
+	if (WARN(!gvt_gmadr_is_valid(vgpu->gvt, h_addr),
+		 "invalid host gmadr %llx\n", h_addr))
+		return -EACCES;
+
+	if (gvt_gmadr_is_aperture(vgpu->gvt, h_addr))
+		*g_addr = vgpu_aperture_gmadr_base(vgpu)
+			+ (h_addr - gvt_aperture_gmadr_base(vgpu->gvt));
+	else
+		*g_addr = vgpu_hidden_gmadr_base(vgpu)
+			+ (h_addr - gvt_hidden_gmadr_base(vgpu->gvt));
+	return 0;
+}
+
+int intel_gvt_ggtt_index_g2h(struct intel_vgpu *vgpu, unsigned long g_index,
+			     unsigned long *h_index)
+{
+	u64 h_addr;
+	int ret;
+
+	ret = intel_gvt_ggtt_gmadr_g2h(vgpu, g_index << GTT_PAGE_SHIFT,
+				       &h_addr);
+	if (ret)
+		return ret;
+
+	*h_index = h_addr >> GTT_PAGE_SHIFT;
+	return 0;
+}
+
+int intel_gvt_ggtt_h2g_index(struct intel_vgpu *vgpu, unsigned long h_index,
+			     unsigned long *g_index)
+{
+	u64 g_addr;
+	int ret;
+
+	ret = intel_gvt_ggtt_gmadr_h2g(vgpu, h_index << GTT_PAGE_SHIFT,
+				       &g_addr);
+	if (ret)
+		return ret;
+
+	*g_index = g_addr >> GTT_PAGE_SHIFT;
+	return 0;
+}
+
+#define gtt_type_is_entry(type) \
+	(type > GTT_TYPE_INVALID && type < GTT_TYPE_PPGTT_ENTRY \
+	 && type != GTT_TYPE_PPGTT_PTE_ENTRY \
+	 && type != GTT_TYPE_PPGTT_ROOT_ENTRY)
+
+#define gtt_type_is_pt(type) \
+	(type >= GTT_TYPE_PPGTT_PTE_PT && type < GTT_TYPE_MAX)
+
+#define gtt_type_is_pte_pt(type) \
+	(type == GTT_TYPE_PPGTT_PTE_PT)
+
+#define gtt_type_is_root_pointer(type) \
+	(gtt_type_is_entry(type) && type > GTT_TYPE_PPGTT_ROOT_ENTRY)
+
+#define gtt_init_entry(e, t, p, v) do { \
+	(e)->type = t; \
+	(e)->pdev = p; \
+	memcpy(&(e)->val64, &v, sizeof(v)); \
+} while (0)
+
+enum {
+	GTT_TYPE_INVALID = -1,
+
+	GTT_TYPE_GGTT_PTE,
+
+	GTT_TYPE_PPGTT_PTE_4K_ENTRY,
+	GTT_TYPE_PPGTT_PTE_2M_ENTRY,
+	GTT_TYPE_PPGTT_PTE_1G_ENTRY,
+
+	GTT_TYPE_PPGTT_PTE_ENTRY,
+
+	GTT_TYPE_PPGTT_PDE_ENTRY,
+	GTT_TYPE_PPGTT_PDP_ENTRY,
+	GTT_TYPE_PPGTT_PML4_ENTRY,
+
+	GTT_TYPE_PPGTT_ROOT_ENTRY,
+
+	GTT_TYPE_PPGTT_ROOT_L3_ENTRY,
+	GTT_TYPE_PPGTT_ROOT_L4_ENTRY,
+
+	GTT_TYPE_PPGTT_ENTRY,
+
+	GTT_TYPE_PPGTT_PTE_PT,
+	GTT_TYPE_PPGTT_PDE_PT,
+	GTT_TYPE_PPGTT_PDP_PT,
+	GTT_TYPE_PPGTT_PML4_PT,
+
+	GTT_TYPE_MAX,
+};
+
+/*
+ * Mappings between GTT_TYPE* enumerations.
+ * Following information can be found according to the given type:
+ * - type of next level page table
+ * - type of entry inside this level page table
+ * - type of entry with PSE set
+ *
+ * If the given type doesn't have such a kind of information,
+ * e.g. give a l4 root entry type, then request to get its PSE type,
+ * give a PTE page table type, then request to get its next level page
+ * table type, as we know l4 root entry doesn't have a PSE bit,
+ * and a PTE page table doesn't have a next level page table type,
+ * GTT_TYPE_INVALID will be returned. This is useful when traversing a
+ * page table.
+ */
+
+struct gtt_type_table_entry {
+	int entry_type;
+	int next_pt_type;
+	int pse_entry_type;
+};
+
+#define GTT_TYPE_TABLE_ENTRY(type, e_type, npt_type, pse_type) \
+	[type] = { \
+		.entry_type = e_type, \
+		.next_pt_type = npt_type, \
+		.pse_entry_type = pse_type, \
+	}
+
+static struct gtt_type_table_entry gtt_type_table[] = {
+	GTT_TYPE_TABLE_ENTRY(GTT_TYPE_PPGTT_ROOT_L4_ENTRY,
+			GTT_TYPE_PPGTT_ROOT_L4_ENTRY,
+			GTT_TYPE_PPGTT_PML4_PT,
+			GTT_TYPE_INVALID),
+	GTT_TYPE_TABLE_ENTRY(GTT_TYPE_PPGTT_PML4_PT,
+			GTT_TYPE_PPGTT_PML4_ENTRY,
+			GTT_TYPE_PPGTT_PDP_PT,
+			GTT_TYPE_INVALID),
+	GTT_TYPE_TABLE_ENTRY(GTT_TYPE_PPGTT_PML4_ENTRY,
+			GTT_TYPE_PPGTT_PML4_ENTRY,
+			GTT_TYPE_PPGTT_PDP_PT,
+			GTT_TYPE_INVALID),
+	GTT_TYPE_TABLE_ENTRY(GTT_TYPE_PPGTT_PDP_PT,
+			GTT_TYPE_PPGTT_PDP_ENTRY,
+			GTT_TYPE_PPGTT_PDE_PT,
+			GTT_TYPE_PPGTT_PTE_1G_ENTRY),
+	GTT_TYPE_TABLE_ENTRY(GTT_TYPE_PPGTT_ROOT_L3_ENTRY,
+			GTT_TYPE_PPGTT_ROOT_L3_ENTRY,
+			GTT_TYPE_PPGTT_PDE_PT,
+			GTT_TYPE_PPGTT_PTE_1G_ENTRY),
+	GTT_TYPE_TABLE_ENTRY(GTT_TYPE_PPGTT_PDP_ENTRY,
+			GTT_TYPE_PPGTT_PDP_ENTRY,
+			GTT_TYPE_PPGTT_PDE_PT,
+			GTT_TYPE_PPGTT_PTE_1G_ENTRY),
+	GTT_TYPE_TABLE_ENTRY(GTT_TYPE_PPGTT_PDE_PT,
+			GTT_TYPE_PPGTT_PDE_ENTRY,
+			GTT_TYPE_PPGTT_PTE_PT,
+			GTT_TYPE_PPGTT_PTE_2M_ENTRY),
+	GTT_TYPE_TABLE_ENTRY(GTT_TYPE_PPGTT_PDE_ENTRY,
+			GTT_TYPE_PPGTT_PDE_ENTRY,
+			GTT_TYPE_PPGTT_PTE_PT,
+			GTT_TYPE_PPGTT_PTE_2M_ENTRY),
+	GTT_TYPE_TABLE_ENTRY(GTT_TYPE_PPGTT_PTE_PT,
+			GTT_TYPE_PPGTT_PTE_4K_ENTRY,
+			GTT_TYPE_INVALID,
+			GTT_TYPE_INVALID),
+	GTT_TYPE_TABLE_ENTRY(GTT_TYPE_PPGTT_PTE_4K_ENTRY,
+			GTT_TYPE_PPGTT_PTE_4K_ENTRY,
+			GTT_TYPE_INVALID,
+			GTT_TYPE_INVALID),
+	GTT_TYPE_TABLE_ENTRY(GTT_TYPE_PPGTT_PTE_2M_ENTRY,
+			GTT_TYPE_PPGTT_PDE_ENTRY,
+			GTT_TYPE_INVALID,
+			GTT_TYPE_PPGTT_PTE_2M_ENTRY),
+	GTT_TYPE_TABLE_ENTRY(GTT_TYPE_PPGTT_PTE_1G_ENTRY,
+			GTT_TYPE_PPGTT_PDP_ENTRY,
+			GTT_TYPE_INVALID,
+			GTT_TYPE_PPGTT_PTE_1G_ENTRY),
+	GTT_TYPE_TABLE_ENTRY(GTT_TYPE_GGTT_PTE,
+			GTT_TYPE_GGTT_PTE,
+			GTT_TYPE_INVALID,
+			GTT_TYPE_INVALID),
+};
+
+static inline int get_next_pt_type(int type)
+{
+	return gtt_type_table[type].next_pt_type;
+}
+
+static inline int get_entry_type(int type)
+{
+	return gtt_type_table[type].entry_type;
+}
+
+static inline int get_pse_type(int type)
+{
+	return gtt_type_table[type].pse_entry_type;
+}
+
+static u64 read_pte64(struct drm_i915_private *dev_priv, unsigned long index)
+{
+	void __iomem *addr = (gen8_pte_t __iomem *)dev_priv->ggtt.gsm + index;
+	u64 pte;
+
+#ifdef readq
+	pte = readq(addr);
+#else
+	pte = ioread32(addr);
+	pte |= ioread32(addr + 4) << 32;
+#endif
+	return pte;
+}
+
+static void write_pte64(struct drm_i915_private *dev_priv,
+		unsigned long index, u64 pte)
+{
+	void __iomem *addr = (gen8_pte_t __iomem *)dev_priv->ggtt.gsm + index;
+
+#ifdef writeq
+	writeq(pte, addr);
+#else
+	iowrite32((u32)pte, addr);
+	iowrite32(pte >> 32, addr + 4);
+#endif
+	I915_WRITE(GFX_FLSH_CNTL_GEN6, GFX_FLSH_CNTL_EN);
+	POSTING_READ(GFX_FLSH_CNTL_GEN6);
+}
+
+static inline struct intel_gvt_gtt_entry *gtt_get_entry64(void *pt,
+		struct intel_gvt_gtt_entry *e,
+		unsigned long index, bool hypervisor_access, unsigned long gpa,
+		struct intel_vgpu *vgpu)
+{
+	const struct intel_gvt_device_info *info = &vgpu->gvt->device_info;
+	int ret;
+
+	if (WARN_ON(info->gtt_entry_size != 8))
+		return e;
+
+	if (hypervisor_access) {
+		ret = intel_gvt_hypervisor_read_gpa(vgpu, gpa +
+				(index << info->gtt_entry_size_shift),
+				&e->val64, 8);
+		WARN_ON(ret);
+	} else if (!pt) {
+		e->val64 = read_pte64(vgpu->gvt->dev_priv, index);
+	} else {
+		e->val64 = *((u64 *)pt + index);
+	}
+	return e;
+}
+
+static inline struct intel_gvt_gtt_entry *gtt_set_entry64(void *pt,
+		struct intel_gvt_gtt_entry *e,
+		unsigned long index, bool hypervisor_access, unsigned long gpa,
+		struct intel_vgpu *vgpu)
+{
+	const struct intel_gvt_device_info *info = &vgpu->gvt->device_info;
+	int ret;
+
+	if (WARN_ON(info->gtt_entry_size != 8))
+		return e;
+
+	if (hypervisor_access) {
+		ret = intel_gvt_hypervisor_write_gpa(vgpu, gpa +
+				(index << info->gtt_entry_size_shift),
+				&e->val64, 8);
+		WARN_ON(ret);
+	} else if (!pt) {
+		write_pte64(vgpu->gvt->dev_priv, index, e->val64);
+	} else {
+		*((u64 *)pt + index) = e->val64;
+	}
+	return e;
+}
+
+#define GTT_HAW 46
+
+#define ADDR_1G_MASK (((1UL << (GTT_HAW - 30 + 1)) - 1) << 30)
+#define ADDR_2M_MASK (((1UL << (GTT_HAW - 21 + 1)) - 1) << 21)
+#define ADDR_4K_MASK (((1UL << (GTT_HAW - 12 + 1)) - 1) << 12)
+
+static unsigned long gen8_gtt_get_pfn(struct intel_gvt_gtt_entry *e)
+{
+	unsigned long pfn;
+
+	if (e->type == GTT_TYPE_PPGTT_PTE_1G_ENTRY)
+		pfn = (e->val64 & ADDR_1G_MASK) >> 12;
+	else if (e->type == GTT_TYPE_PPGTT_PTE_2M_ENTRY)
+		pfn = (e->val64 & ADDR_2M_MASK) >> 12;
+	else
+		pfn = (e->val64 & ADDR_4K_MASK) >> 12;
+	return pfn;
+}
+
+static void gen8_gtt_set_pfn(struct intel_gvt_gtt_entry *e, unsigned long pfn)
+{
+	if (e->type == GTT_TYPE_PPGTT_PTE_1G_ENTRY) {
+		e->val64 &= ~ADDR_1G_MASK;
+		pfn &= (ADDR_1G_MASK >> 12);
+	} else if (e->type == GTT_TYPE_PPGTT_PTE_2M_ENTRY) {
+		e->val64 &= ~ADDR_2M_MASK;
+		pfn &= (ADDR_2M_MASK >> 12);
+	} else {
+		e->val64 &= ~ADDR_4K_MASK;
+		pfn &= (ADDR_4K_MASK >> 12);
+	}
+
+	e->val64 |= (pfn << 12);
+}
+
+static bool gen8_gtt_test_pse(struct intel_gvt_gtt_entry *e)
+{
+	/* Entry doesn't have PSE bit. */
+	if (get_pse_type(e->type) == GTT_TYPE_INVALID)
+		return false;
+
+	e->type = get_entry_type(e->type);
+	if (!(e->val64 & (1 << 7)))
+		return false;
+
+	e->type = get_pse_type(e->type);
+	return true;
+}
+
+static bool gen8_gtt_test_present(struct intel_gvt_gtt_entry *e)
+{
+	/*
+	 * i915 writes PDP root pointer registers without present bit,
+	 * it also works, so we need to treat root pointer entry
+	 * specifically.
+	 */
+	if (e->type == GTT_TYPE_PPGTT_ROOT_L3_ENTRY
+			|| e->type == GTT_TYPE_PPGTT_ROOT_L4_ENTRY)
+		return (e->val64 != 0);
+	else
+		return (e->val64 & (1 << 0));
+}
+
+static void gtt_entry_clear_present(struct intel_gvt_gtt_entry *e)
+{
+	e->val64 &= ~(1 << 0);
+}
+
+/*
+ * Per-platform GMA routines.
+ */
+static unsigned long gma_to_ggtt_pte_index(unsigned long gma)
+{
+	unsigned long x = (gma >> GTT_PAGE_SHIFT);
+
+	trace_gma_index(__func__, gma, x);
+	return x;
+}
+
+#define DEFINE_PPGTT_GMA_TO_INDEX(prefix, ename, exp) \
+static unsigned long prefix##_gma_to_##ename##_index(unsigned long gma) \
+{ \
+	unsigned long x = (exp); \
+	trace_gma_index(__func__, gma, x); \
+	return x; \
+}
+
+DEFINE_PPGTT_GMA_TO_INDEX(gen8, pte, (gma >> 12 & 0x1ff));
+DEFINE_PPGTT_GMA_TO_INDEX(gen8, pde, (gma >> 21 & 0x1ff));
+DEFINE_PPGTT_GMA_TO_INDEX(gen8, l3_pdp, (gma >> 30 & 0x3));
+DEFINE_PPGTT_GMA_TO_INDEX(gen8, l4_pdp, (gma >> 30 & 0x1ff));
+DEFINE_PPGTT_GMA_TO_INDEX(gen8, pml4, (gma >> 39 & 0x1ff));
+
+static struct intel_gvt_gtt_pte_ops gen8_gtt_pte_ops = {
+	.get_entry = gtt_get_entry64,
+	.set_entry = gtt_set_entry64,
+	.clear_present = gtt_entry_clear_present,
+	.test_present = gen8_gtt_test_present,
+	.test_pse = gen8_gtt_test_pse,
+	.get_pfn = gen8_gtt_get_pfn,
+	.set_pfn = gen8_gtt_set_pfn,
+};
+
+static struct intel_gvt_gtt_gma_ops gen8_gtt_gma_ops = {
+	.gma_to_ggtt_pte_index = gma_to_ggtt_pte_index,
+	.gma_to_pte_index = gen8_gma_to_pte_index,
+	.gma_to_pde_index = gen8_gma_to_pde_index,
+	.gma_to_l3_pdp_index = gen8_gma_to_l3_pdp_index,
+	.gma_to_l4_pdp_index = gen8_gma_to_l4_pdp_index,
+	.gma_to_pml4_index = gen8_gma_to_pml4_index,
+};
+
+static int gtt_entry_p2m(struct intel_vgpu *vgpu, struct intel_gvt_gtt_entry *p,
+		struct intel_gvt_gtt_entry *m)
+{
+	struct intel_gvt_gtt_pte_ops *ops = vgpu->gvt->gtt.pte_ops;
+	unsigned long gfn, mfn;
+
+	*m = *p;
+
+	if (!ops->test_present(p))
+		return 0;
+
+	gfn = ops->get_pfn(p);
+
+	mfn = intel_gvt_hypervisor_gfn_to_mfn(vgpu, gfn);
+	if (mfn == INTEL_GVT_INVALID_ADDR) {
+		gvt_err("fail to translate gfn: 0x%lx\n", gfn);
+		return -ENXIO;
+	}
+
+	ops->set_pfn(m, mfn);
+	return 0;
+}
+
+/*
+ * MM helpers.
+ */
+struct intel_gvt_gtt_entry *intel_vgpu_mm_get_entry(struct intel_vgpu_mm *mm,
+		void *page_table, struct intel_gvt_gtt_entry *e,
+		unsigned long index)
+{
+	struct intel_gvt *gvt = mm->vgpu->gvt;
+	struct intel_gvt_gtt_pte_ops *ops = gvt->gtt.pte_ops;
+
+	e->type = mm->page_table_entry_type;
+
+	ops->get_entry(page_table, e, index, false, 0, mm->vgpu);
+	ops->test_pse(e);
+	return e;
+}
+
+struct intel_gvt_gtt_entry *intel_vgpu_mm_set_entry(struct intel_vgpu_mm *mm,
+		void *page_table, struct intel_gvt_gtt_entry *e,
+		unsigned long index)
+{
+	struct intel_gvt *gvt = mm->vgpu->gvt;
+	struct intel_gvt_gtt_pte_ops *ops = gvt->gtt.pte_ops;
+
+	return ops->set_entry(page_table, e, index, false, 0, mm->vgpu);
+}
+
+/*
+ * PPGTT shadow page table helpers.
+ */
+static inline struct intel_gvt_gtt_entry *ppgtt_spt_get_entry(
+		struct intel_vgpu_ppgtt_spt *spt,
+		void *page_table, int type,
+		struct intel_gvt_gtt_entry *e, unsigned long index,
+		bool guest)
+{
+	struct intel_gvt *gvt = spt->vgpu->gvt;
+	struct intel_gvt_gtt_pte_ops *ops = gvt->gtt.pte_ops;
+
+	e->type = get_entry_type(type);
+
+	if (WARN(!gtt_type_is_entry(e->type), "invalid entry type\n"))
+		return e;
+
+	ops->get_entry(page_table, e, index, guest,
+			spt->guest_page.gfn << GTT_PAGE_SHIFT,
+			spt->vgpu);
+	ops->test_pse(e);
+	return e;
+}
+
+static inline struct intel_gvt_gtt_entry *ppgtt_spt_set_entry(
+		struct intel_vgpu_ppgtt_spt *spt,
+		void *page_table, int type,
+		struct intel_gvt_gtt_entry *e, unsigned long index,
+		bool guest)
+{
+	struct intel_gvt *gvt = spt->vgpu->gvt;
+	struct intel_gvt_gtt_pte_ops *ops = gvt->gtt.pte_ops;
+
+	if (WARN(!gtt_type_is_entry(e->type), "invalid entry type\n"))
+		return e;
+
+	return ops->set_entry(page_table, e, index, guest,
+			spt->guest_page.gfn << GTT_PAGE_SHIFT,
+			spt->vgpu);
+}
+
+#define ppgtt_get_guest_entry(spt, e, index) \
+	ppgtt_spt_get_entry(spt, NULL, \
+		spt->guest_page_type, e, index, true)
+
+#define ppgtt_set_guest_entry(spt, e, index) \
+	ppgtt_spt_set_entry(spt, NULL, \
+		spt->guest_page_type, e, index, true)
+
+#define ppgtt_get_shadow_entry(spt, e, index) \
+	ppgtt_spt_get_entry(spt, spt->shadow_page.vaddr, \
+		spt->shadow_page.type, e, index, false)
+
+#define ppgtt_set_shadow_entry(spt, e, index) \
+	ppgtt_spt_set_entry(spt, spt->shadow_page.vaddr, \
+		spt->shadow_page.type, e, index, false)
+
+/**
+ * intel_vgpu_init_guest_page - init a guest page data structure
+ * @vgpu: a vGPU
+ * @p: a guest page data structure
+ * @gfn: guest memory page frame number
+ * @handler: function will be called when target guest memory page has
+ * been modified.
+ *
+ * This function is called when user wants to track a guest memory page.
+ *
+ * Returns:
+ * Zero on success, negative error code if failed.
+ */
+int intel_vgpu_init_guest_page(struct intel_vgpu *vgpu,
+		struct intel_vgpu_guest_page *p,
+		unsigned long gfn,
+		int (*handler)(void *, u64, void *, int),
+		void *data)
+{
+	INIT_HLIST_NODE(&p->node);
+
+	p->writeprotection = false;
+	p->gfn = gfn;
+	p->handler = handler;
+	p->data = data;
+	p->oos_page = NULL;
+	p->write_cnt = 0;
+
+	hash_add(vgpu->gtt.guest_page_hash_table, &p->node, p->gfn);
+	return 0;
+}
+
+static int detach_oos_page(struct intel_vgpu *vgpu,
+		struct intel_vgpu_oos_page *oos_page);
+
+/**
+ * intel_vgpu_clean_guest_page - release the resource owned by guest page data
+ * structure
+ * @vgpu: a vGPU
+ * @p: a tracked guest page
+ *
+ * This function is called when user tries to stop tracking a guest memory
+ * page.
+ */
+void intel_vgpu_clean_guest_page(struct intel_vgpu *vgpu,
+		struct intel_vgpu_guest_page *p)
+{
+	if (!hlist_unhashed(&p->node))
+		hash_del(&p->node);
+
+	if (p->oos_page)
+		detach_oos_page(vgpu, p->oos_page);
+
+	if (p->writeprotection)
+		intel_gvt_hypervisor_unset_wp_page(vgpu, p);
+}
+
+/**
+ * intel_vgpu_find_guest_page - find a guest page data structure by GFN.
+ * @vgpu: a vGPU
+ * @gfn: guest memory page frame number
+ *
+ * This function is called when emulation logic wants to know if a trapped GFN
+ * is a tracked guest page.
+ *
+ * Returns:
+ * Pointer to guest page data structure, NULL if failed.
+ */
+struct intel_vgpu_guest_page *intel_vgpu_find_guest_page(
+		struct intel_vgpu *vgpu, unsigned long gfn)
+{
+	struct intel_vgpu_guest_page *p;
+
+	hash_for_each_possible(vgpu->gtt.guest_page_hash_table,
+		p, node, gfn) {
+		if (p->gfn == gfn)
+			return p;
+	}
+	return NULL;
+}
+
+static inline int init_shadow_page(struct intel_vgpu *vgpu,
+		struct intel_vgpu_shadow_page *p, int type)
+{
+	p->vaddr = page_address(p->page);
+	p->type = type;
+
+	INIT_HLIST_NODE(&p->node);
+
+	p->mfn = intel_gvt_hypervisor_virt_to_mfn(p->vaddr);
+	if (p->mfn == INTEL_GVT_INVALID_ADDR)
+		return -EFAULT;
+
+	hash_add(vgpu->gtt.shadow_page_hash_table, &p->node, p->mfn);
+	return 0;
+}
+
+static inline void clean_shadow_page(struct intel_vgpu_shadow_page *p)
+{
+	if (!hlist_unhashed(&p->node))
+		hash_del(&p->node);
+}
+
+static inline struct intel_vgpu_shadow_page *find_shadow_page(
+		struct intel_vgpu *vgpu, unsigned long mfn)
+{
+	struct intel_vgpu_shadow_page *p;
+
+	hash_for_each_possible(vgpu->gtt.shadow_page_hash_table,
+		p, node, mfn) {
+		if (p->mfn == mfn)
+			return p;
+	}
+	return NULL;
+}
+
+#define guest_page_to_ppgtt_spt(ptr) \
+	container_of(ptr, struct intel_vgpu_ppgtt_spt, guest_page)
+
+#define shadow_page_to_ppgtt_spt(ptr) \
+	container_of(ptr, struct intel_vgpu_ppgtt_spt, shadow_page)
+
+static void *alloc_spt(gfp_t gfp_mask)
+{
+	struct intel_vgpu_ppgtt_spt *spt;
+
+	spt = kzalloc(sizeof(*spt), gfp_mask);
+	if (!spt)
+		return NULL;
+
+	spt->shadow_page.page = alloc_page(gfp_mask);
+	if (!spt->shadow_page.page) {
+		kfree(spt);
+		return NULL;
+	}
+	return spt;
+}
+
+static void free_spt(struct intel_vgpu_ppgtt_spt *spt)
+{
+	__free_page(spt->shadow_page.page);
+	kfree(spt);
+}
+
+static void ppgtt_free_shadow_page(struct intel_vgpu_ppgtt_spt *spt)
+{
+	trace_spt_free(spt->vgpu->id, spt, spt->shadow_page.type);
+
+	clean_shadow_page(&spt->shadow_page);
+	intel_vgpu_clean_guest_page(spt->vgpu, &spt->guest_page);
+	list_del_init(&spt->post_shadow_list);
+
+	free_spt(spt);
+}
+
+static void ppgtt_free_all_shadow_page(struct intel_vgpu *vgpu)
+{
+	struct hlist_node *n;
+	struct intel_vgpu_shadow_page *sp;
+	int i;
+
+	hash_for_each_safe(vgpu->gtt.shadow_page_hash_table, i, n, sp, node)
+		ppgtt_free_shadow_page(shadow_page_to_ppgtt_spt(sp));
+}
+
+static int ppgtt_handle_guest_write_page_table_bytes(void *gp,
+		u64 pa, void *p_data, int bytes);
+
+static int ppgtt_write_protection_handler(void *gp, u64 pa,
+		void *p_data, int bytes)
+{
+	struct intel_vgpu_guest_page *gpt = (struct intel_vgpu_guest_page *)gp;
+	int ret;
+
+	if (bytes != 4 && bytes != 8)
+		return -EINVAL;
+
+	if (!gpt->writeprotection)
+		return -EINVAL;
+
+	ret = ppgtt_handle_guest_write_page_table_bytes(gp,
+		pa, p_data, bytes);
+	if (ret)
+		return ret;
+	return ret;
+}
+
+static int reclaim_one_mm(struct intel_gvt *gvt);
+
+static struct intel_vgpu_ppgtt_spt *ppgtt_alloc_shadow_page(
+		struct intel_vgpu *vgpu, int type, unsigned long gfn)
+{
+	struct intel_vgpu_ppgtt_spt *spt = NULL;
+	int ret;
+
+retry:
+	spt = alloc_spt(GFP_KERNEL | __GFP_ZERO);
+	if (!spt) {
+		if (reclaim_one_mm(vgpu->gvt))
+			goto retry;
+
+		gvt_err("fail to allocate ppgtt shadow page\n");
+		return ERR_PTR(-ENOMEM);
+	}
+
+	spt->vgpu = vgpu;
+	spt->guest_page_type = type;
+	atomic_set(&spt->refcount, 1);
+	INIT_LIST_HEAD(&spt->post_shadow_list);
+
+	/*
+	 * TODO: guest page type may be different with shadow page type,
+	 *	 when we support PSE page in future.
+	 */
+	ret = init_shadow_page(vgpu, &spt->shadow_page, type);
+	if (ret) {
+		gvt_err("fail to initialize shadow page for spt\n");
+		goto err;
+	}
+
+	ret = intel_vgpu_init_guest_page(vgpu, &spt->guest_page,
+			gfn, ppgtt_write_protection_handler, NULL);
+	if (ret) {
+		gvt_err("fail to initialize guest page for spt\n");
+		goto err;
+	}
+
+	trace_spt_alloc(vgpu->id, spt, type, spt->shadow_page.mfn, gfn);
+	return spt;
+err:
+	ppgtt_free_shadow_page(spt);
+	return ERR_PTR(ret);
+}
+
+static struct intel_vgpu_ppgtt_spt *ppgtt_find_shadow_page(
+		struct intel_vgpu *vgpu, unsigned long mfn)
+{
+	struct intel_vgpu_shadow_page *p = find_shadow_page(vgpu, mfn);
+
+	if (p)
+		return shadow_page_to_ppgtt_spt(p);
+
+	gvt_err("vgpu%d: fail to find ppgtt shadow page: 0x%lx\n",
+			vgpu->id, mfn);
+	return NULL;
+}
+
+#define pt_entry_size_shift(spt) \
+	((spt)->vgpu->gvt->device_info.gtt_entry_size_shift)
+
+#define pt_entries(spt) \
+	(GTT_PAGE_SIZE >> pt_entry_size_shift(spt))
+
+#define for_each_present_guest_entry(spt, e, i) \
+	for (i = 0; i < pt_entries(spt); i++) \
+	if (spt->vgpu->gvt->gtt.pte_ops->test_present( \
+		ppgtt_get_guest_entry(spt, e, i)))
+
+#define for_each_present_shadow_entry(spt, e, i) \
+	for (i = 0; i < pt_entries(spt); i++) \
+	if (spt->vgpu->gvt->gtt.pte_ops->test_present( \
+		ppgtt_get_shadow_entry(spt, e, i)))
+
+static void ppgtt_get_shadow_page(struct intel_vgpu_ppgtt_spt *spt)
+{
+	int v = atomic_read(&spt->refcount);
+
+	trace_spt_refcount(spt->vgpu->id, "inc", spt, v, (v + 1));
+
+	atomic_inc(&spt->refcount);
+}
+
+static int ppgtt_invalidate_shadow_page(struct intel_vgpu_ppgtt_spt *spt);
+
+static int ppgtt_invalidate_shadow_page_by_shadow_entry(struct intel_vgpu *vgpu,
+		struct intel_gvt_gtt_entry *e)
+{
+	struct intel_gvt_gtt_pte_ops *ops = vgpu->gvt->gtt.pte_ops;
+	struct intel_vgpu_ppgtt_spt *s;
+
+	if (WARN_ON(!gtt_type_is_pt(get_next_pt_type(e->type))))
+		return -EINVAL;
+
+	if (ops->get_pfn(e) == vgpu->gtt.scratch_page_mfn)
+		return 0;
+
+	s = ppgtt_find_shadow_page(vgpu, ops->get_pfn(e));
+	if (!s) {
+		gvt_err("vgpu%d: fail to find shadow page: mfn: 0x%lx\n",
+				vgpu->id, ops->get_pfn(e));
+		return -ENXIO;
+	}
+	return ppgtt_invalidate_shadow_page(s);
+}
+
+static int ppgtt_invalidate_shadow_page(struct intel_vgpu_ppgtt_spt *spt)
+{
+	struct intel_gvt_gtt_entry e;
+	unsigned long index;
+	int ret;
+	int v = atomic_read(&spt->refcount);
+
+	trace_spt_change(spt->vgpu->id, "die", spt,
+			spt->guest_page.gfn, spt->shadow_page.type);
+
+	trace_spt_refcount(spt->vgpu->id, "dec", spt, v, (v - 1));
+
+	if (atomic_dec_return(&spt->refcount) > 0)
+		return 0;
+
+	if (gtt_type_is_pte_pt(spt->shadow_page.type))
+		goto release;
+
+	for_each_present_shadow_entry(spt, &e, index) {
+		if (!gtt_type_is_pt(get_next_pt_type(e.type))) {
+			gvt_err("GVT doesn't support pse bit for now\n");
+			return -EINVAL;
+		}
+		ret = ppgtt_invalidate_shadow_page_by_shadow_entry(
+				spt->vgpu, &e);
+		if (ret)
+			goto fail;
+	}
+release:
+	trace_spt_change(spt->vgpu->id, "release", spt,
+			spt->guest_page.gfn, spt->shadow_page.type);
+	ppgtt_free_shadow_page(spt);
+	return 0;
+fail:
+	gvt_err("vgpu%d: fail: shadow page %p shadow entry 0x%llx type %d\n",
+			spt->vgpu->id, spt, e.val64, e.type);
+	return ret;
+}
+
+static int ppgtt_populate_shadow_page(struct intel_vgpu_ppgtt_spt *spt);
+
+static struct intel_vgpu_ppgtt_spt *ppgtt_populate_shadow_page_by_guest_entry(
+		struct intel_vgpu *vgpu, struct intel_gvt_gtt_entry *we)
+{
+	struct intel_gvt_gtt_pte_ops *ops = vgpu->gvt->gtt.pte_ops;
+	struct intel_vgpu_ppgtt_spt *s = NULL;
+	struct intel_vgpu_guest_page *g;
+	int ret;
+
+	if (WARN_ON(!gtt_type_is_pt(get_next_pt_type(we->type)))) {
+		ret = -EINVAL;
+		goto fail;
+	}
+
+	g = intel_vgpu_find_guest_page(vgpu, ops->get_pfn(we));
+	if (g) {
+		s = guest_page_to_ppgtt_spt(g);
+		ppgtt_get_shadow_page(s);
+	} else {
+		int type = get_next_pt_type(we->type);
+
+		s = ppgtt_alloc_shadow_page(vgpu, type, ops->get_pfn(we));
+		if (IS_ERR(s)) {
+			ret = PTR_ERR(s);
+			goto fail;
+		}
+
+		ret = intel_gvt_hypervisor_set_wp_page(vgpu, &s->guest_page);
+		if (ret)
+			goto fail;
+
+		ret = ppgtt_populate_shadow_page(s);
+		if (ret)
+			goto fail;
+
+		trace_spt_change(vgpu->id, "new", s, s->guest_page.gfn,
+			s->shadow_page.type);
+	}
+	return s;
+fail:
+	gvt_err("vgpu%d: fail: shadow page %p guest entry 0x%llx type %d\n",
+			vgpu->id, s, we->val64, we->type);
+	return ERR_PTR(ret);
+}
+
+static inline void ppgtt_generate_shadow_entry(struct intel_gvt_gtt_entry *se,
+		struct intel_vgpu_ppgtt_spt *s, struct intel_gvt_gtt_entry *ge)
+{
+	struct intel_gvt_gtt_pte_ops *ops = s->vgpu->gvt->gtt.pte_ops;
+
+	se->type = ge->type;
+	se->val64 = ge->val64;
+
+	ops->set_pfn(se, s->shadow_page.mfn);
+}
+
+static int ppgtt_populate_shadow_page(struct intel_vgpu_ppgtt_spt *spt)
+{
+	struct intel_vgpu *vgpu = spt->vgpu;
+	struct intel_vgpu_ppgtt_spt *s;
+	struct intel_gvt_gtt_entry se, ge;
+	unsigned long i;
+	int ret;
+
+	trace_spt_change(spt->vgpu->id, "born", spt,
+			spt->guest_page.gfn, spt->shadow_page.type);
+
+	if (gtt_type_is_pte_pt(spt->shadow_page.type)) {
+		for_each_present_guest_entry(spt, &ge, i) {
+			ret = gtt_entry_p2m(vgpu, &ge, &se);
+			if (ret)
+				goto fail;
+			ppgtt_set_shadow_entry(spt, &se, i);
+		}
+		return 0;
+	}
+
+	for_each_present_guest_entry(spt, &ge, i) {
+		if (!gtt_type_is_pt(get_next_pt_type(ge.type))) {
+			gvt_err("GVT doesn't support pse bit now\n");
+			ret = -EINVAL;
+			goto fail;
+		}
+
+		s = ppgtt_populate_shadow_page_by_guest_entry(vgpu, &ge);
+		if (IS_ERR(s)) {
+			ret = PTR_ERR(s);
+			goto fail;
+		}
+		ppgtt_get_shadow_entry(spt, &se, i);
+		ppgtt_generate_shadow_entry(&se, s, &ge);
+		ppgtt_set_shadow_entry(spt, &se, i);
+	}
+	return 0;
+fail:
+	gvt_err("vgpu%d: fail: shadow page %p guest entry 0x%llx type %d\n",
+			vgpu->id, spt, ge.val64, ge.type);
+	return ret;
+}
+
+static int ppgtt_handle_guest_entry_removal(struct intel_vgpu_guest_page *gpt,
+		struct intel_gvt_gtt_entry *we, unsigned long index)
+{
+	struct intel_vgpu_ppgtt_spt *spt = guest_page_to_ppgtt_spt(gpt);
+	struct intel_vgpu_shadow_page *sp = &spt->shadow_page;
+	struct intel_vgpu *vgpu = spt->vgpu;
+	struct intel_gvt_gtt_pte_ops *ops = vgpu->gvt->gtt.pte_ops;
+	struct intel_gvt_gtt_entry e;
+	int ret;
+
+	trace_gpt_change(spt->vgpu->id, "remove", spt, sp->type,
+		we->val64, index);
+
+	ppgtt_get_shadow_entry(spt, &e, index);
+	if (!ops->test_present(&e))
+		return 0;
+
+	if (ops->get_pfn(&e) == vgpu->gtt.scratch_page_mfn)
+		return 0;
+
+	if (gtt_type_is_pt(get_next_pt_type(we->type))) {
+		struct intel_vgpu_guest_page *g =
+			intel_vgpu_find_guest_page(vgpu, ops->get_pfn(we));
+		if (!g) {
+			gvt_err("fail to find guest page\n");
+			ret = -ENXIO;
+			goto fail;
+		}
+		ret = ppgtt_invalidate_shadow_page(guest_page_to_ppgtt_spt(g));
+		if (ret)
+			goto fail;
+	}
+	ops->set_pfn(&e, vgpu->gtt.scratch_page_mfn);
+	ppgtt_set_shadow_entry(spt, &e, index);
+	return 0;
+fail:
+	gvt_err("vgpu%d: fail: shadow page %p guest entry 0x%llx type %d\n",
+			vgpu->id, spt, we->val64, we->type);
+	return ret;
+}
+
+static int ppgtt_handle_guest_entry_add(struct intel_vgpu_guest_page *gpt,
+		struct intel_gvt_gtt_entry *we, unsigned long index)
+{
+	struct intel_vgpu_ppgtt_spt *spt = guest_page_to_ppgtt_spt(gpt);
+	struct intel_vgpu_shadow_page *sp = &spt->shadow_page;
+	struct intel_vgpu *vgpu = spt->vgpu;
+	struct intel_gvt_gtt_entry m;
+	struct intel_vgpu_ppgtt_spt *s;
+	int ret;
+
+	trace_gpt_change(spt->vgpu->id, "add", spt, sp->type,
+		we->val64, index);
+
+	if (gtt_type_is_pt(get_next_pt_type(we->type))) {
+		s = ppgtt_populate_shadow_page_by_guest_entry(vgpu, we);
+		if (IS_ERR(s)) {
+			ret = PTR_ERR(s);
+			goto fail;
+		}
+		ppgtt_get_shadow_entry(spt, &m, index);
+		ppgtt_generate_shadow_entry(&m, s, we);
+		ppgtt_set_shadow_entry(spt, &m, index);
+	} else {
+		ret = gtt_entry_p2m(vgpu, we, &m);
+		if (ret)
+			goto fail;
+		ppgtt_set_shadow_entry(spt, &m, index);
+	}
+	return 0;
+fail:
+	gvt_err("vgpu%d: fail: spt %p guest entry 0x%llx type %d\n", vgpu->id,
+			spt, we->val64, we->type);
+	return ret;
+}
+
+static int sync_oos_page(struct intel_vgpu *vgpu,
+		struct intel_vgpu_oos_page *oos_page)
+{
+	const struct intel_gvt_device_info *info = &vgpu->gvt->device_info;
+	struct intel_gvt *gvt = vgpu->gvt;
+	struct intel_gvt_gtt_pte_ops *ops = gvt->gtt.pte_ops;
+	struct intel_vgpu_ppgtt_spt *spt =
+		guest_page_to_ppgtt_spt(oos_page->guest_page);
+	struct intel_gvt_gtt_entry old, new, m;
+	int index;
+	int ret;
+
+	trace_oos_change(vgpu->id, "sync", oos_page->id,
+			oos_page->guest_page, spt->guest_page_type);
+
+	old.type = new.type = get_entry_type(spt->guest_page_type);
+	old.val64 = new.val64 = 0;
+
+	for (index = 0; index < (GTT_PAGE_SIZE >> info->gtt_entry_size_shift);
+		index++) {
+		ops->get_entry(oos_page->mem, &old, index, false, 0, vgpu);
+		ops->get_entry(NULL, &new, index, true,
+			oos_page->guest_page->gfn << PAGE_SHIFT, vgpu);
+
+		if (old.val64 == new.val64
+			&& !test_and_clear_bit(index, spt->post_shadow_bitmap))
+			continue;
+
+		trace_oos_sync(vgpu->id, oos_page->id,
+				oos_page->guest_page, spt->guest_page_type,
+				new.val64, index);
+
+		ret = gtt_entry_p2m(vgpu, &new, &m);
+		if (ret)
+			return ret;
+
+		ops->set_entry(oos_page->mem, &new, index, false, 0, vgpu);
+		ppgtt_set_shadow_entry(spt, &m, index);
+	}
+
+	oos_page->guest_page->write_cnt = 0;
+	list_del_init(&spt->post_shadow_list);
+	return 0;
+}
+
+static int detach_oos_page(struct intel_vgpu *vgpu,
+		struct intel_vgpu_oos_page *oos_page)
+{
+	struct intel_gvt *gvt = vgpu->gvt;
+	struct intel_vgpu_ppgtt_spt *spt =
+		guest_page_to_ppgtt_spt(oos_page->guest_page);
+
+	trace_oos_change(vgpu->id, "detach", oos_page->id,
+			oos_page->guest_page, spt->guest_page_type);
+
+	oos_page->guest_page->write_cnt = 0;
+	oos_page->guest_page->oos_page = NULL;
+	oos_page->guest_page = NULL;
+
+	list_del_init(&oos_page->vm_list);
+	list_move_tail(&oos_page->list, &gvt->gtt.oos_page_free_list_head);
+
+	return 0;
+}
+
+static int attach_oos_page(struct intel_vgpu *vgpu,
+		struct intel_vgpu_oos_page *oos_page,
+		struct intel_vgpu_guest_page *gpt)
+{
+	struct intel_gvt *gvt = vgpu->gvt;
+	int ret;
+
+	ret = intel_gvt_hypervisor_read_gpa(vgpu, gpt->gfn << GTT_PAGE_SHIFT,
+		oos_page->mem, GTT_PAGE_SIZE);
+	if (ret)
+		return ret;
+
+	oos_page->guest_page = gpt;
+	gpt->oos_page = oos_page;
+
+	list_move_tail(&oos_page->list, &gvt->gtt.oos_page_use_list_head);
+
+	trace_oos_change(vgpu->id, "attach", gpt->oos_page->id,
+			gpt, guest_page_to_ppgtt_spt(gpt)->guest_page_type);
+	return 0;
+}
+
+static int ppgtt_set_guest_page_sync(struct intel_vgpu *vgpu,
+		struct intel_vgpu_guest_page *gpt)
+{
+	int ret;
+
+	ret = intel_gvt_hypervisor_set_wp_page(vgpu, gpt);
+	if (ret)
+		return ret;
+
+	trace_oos_change(vgpu->id, "set page sync", gpt->oos_page->id,
+			gpt, guest_page_to_ppgtt_spt(gpt)->guest_page_type);
+
+	list_del_init(&gpt->oos_page->vm_list);
+	return sync_oos_page(vgpu, gpt->oos_page);
+}
+
+static int ppgtt_allocate_oos_page(struct intel_vgpu *vgpu,
+		struct intel_vgpu_guest_page *gpt)
+{
+	struct intel_gvt *gvt = vgpu->gvt;
+	struct intel_gvt_gtt *gtt = &gvt->gtt;
+	struct intel_vgpu_oos_page *oos_page = gpt->oos_page;
+	int ret;
+
+	WARN(oos_page, "shadow PPGTT page has already has a oos page\n");
+
+	if (list_empty(&gtt->oos_page_free_list_head)) {
+		oos_page = container_of(gtt->oos_page_use_list_head.next,
+			struct intel_vgpu_oos_page, list);
+		ret = ppgtt_set_guest_page_sync(vgpu, oos_page->guest_page);
+		if (ret)
+			return ret;
+		ret = detach_oos_page(vgpu, oos_page);
+		if (ret)
+			return ret;
+	} else
+		oos_page = container_of(gtt->oos_page_free_list_head.next,
+			struct intel_vgpu_oos_page, list);
+	return attach_oos_page(vgpu, oos_page, gpt);
+}
+
+static int ppgtt_set_guest_page_oos(struct intel_vgpu *vgpu,
+		struct intel_vgpu_guest_page *gpt)
+{
+	struct intel_vgpu_oos_page *oos_page = gpt->oos_page;
+
+	if (WARN(!oos_page, "shadow PPGTT page should have a oos page\n"))
+		return -EINVAL;
+
+	trace_oos_change(vgpu->id, "set page out of sync", gpt->oos_page->id,
+			gpt, guest_page_to_ppgtt_spt(gpt)->guest_page_type);
+
+	list_add_tail(&oos_page->vm_list, &vgpu->gtt.oos_page_list_head);
+	return intel_gvt_hypervisor_unset_wp_page(vgpu, gpt);
+}
+
+/**
+ * intel_vgpu_sync_oos_pages - sync all the out-of-synced shadow for vGPU
+ * @vgpu: a vGPU
+ *
+ * This function is called before submitting a guest workload to host,
+ * to sync all the out-of-synced shadow for vGPU
+ *
+ * Returns:
+ * Zero on success, negative error code if failed.
+ */
+int intel_vgpu_sync_oos_pages(struct intel_vgpu *vgpu)
+{
+	struct list_head *pos, *n;
+	struct intel_vgpu_oos_page *oos_page;
+	int ret;
+
+	if (!enable_out_of_sync)
+		return 0;
+
+	list_for_each_safe(pos, n, &vgpu->gtt.oos_page_list_head) {
+		oos_page = container_of(pos,
+				struct intel_vgpu_oos_page, vm_list);
+		ret = ppgtt_set_guest_page_sync(vgpu, oos_page->guest_page);
+		if (ret)
+			return ret;
+	}
+	return 0;
+}
+
+/*
+ * The heart of PPGTT shadow page table.
+ */
+static int ppgtt_handle_guest_write_page_table(
+		struct intel_vgpu_guest_page *gpt,
+		struct intel_gvt_gtt_entry *we, unsigned long index)
+{
+	struct intel_vgpu_ppgtt_spt *spt = guest_page_to_ppgtt_spt(gpt);
+	struct intel_vgpu *vgpu = spt->vgpu;
+	struct intel_gvt_gtt_pte_ops *ops = vgpu->gvt->gtt.pte_ops;
+	struct intel_gvt_gtt_entry ge;
+
+	int old_present, new_present;
+	int ret;
+
+	ppgtt_get_guest_entry(spt, &ge, index);
+
+	old_present = ops->test_present(&ge);
+	new_present = ops->test_present(we);
+
+	ppgtt_set_guest_entry(spt, we, index);
+
+	if (old_present) {
+		ret = ppgtt_handle_guest_entry_removal(gpt, &ge, index);
+		if (ret)
+			goto fail;
+	}
+	if (new_present) {
+		ret = ppgtt_handle_guest_entry_add(gpt, we, index);
+		if (ret)
+			goto fail;
+	}
+	return 0;
+fail:
+	gvt_err("vgpu%d: fail: shadow page %p guest entry 0x%llx type %d.\n",
+			vgpu->id, spt, we->val64, we->type);
+	return ret;
+}
+
+static inline bool can_do_out_of_sync(struct intel_vgpu_guest_page *gpt)
+{
+	return enable_out_of_sync
+		&& gtt_type_is_pte_pt(
+			guest_page_to_ppgtt_spt(gpt)->guest_page_type)
+		&& gpt->write_cnt >= 2;
+}
+
+static void ppgtt_set_post_shadow(struct intel_vgpu_ppgtt_spt *spt,
+		unsigned long index)
+{
+	set_bit(index, spt->post_shadow_bitmap);
+	if (!list_empty(&spt->post_shadow_list))
+		return;
+
+	list_add_tail(&spt->post_shadow_list,
+			&spt->vgpu->gtt.post_shadow_list_head);
+}
+
+/**
+ * intel_vgpu_flush_post_shadow - flush the post shadow transactions
+ * @vgpu: a vGPU
+ *
+ * This function is called before submitting a guest workload to host,
+ * to flush all the post shadows for a vGPU.
+ *
+ * Returns:
+ * Zero on success, negative error code if failed.
+ */
+int intel_vgpu_flush_post_shadow(struct intel_vgpu *vgpu)
+{
+	struct list_head *pos, *n;
+	struct intel_vgpu_ppgtt_spt *spt;
+	struct intel_gvt_gtt_entry ge, e;
+	unsigned long index;
+	int ret;
+
+	list_for_each_safe(pos, n, &vgpu->gtt.post_shadow_list_head) {
+		spt = container_of(pos, struct intel_vgpu_ppgtt_spt,
+				post_shadow_list);
+
+		for_each_set_bit(index, spt->post_shadow_bitmap,
+				GTT_ENTRY_NUM_IN_ONE_PAGE) {
+			ppgtt_get_guest_entry(spt, &ge, index);
+			e = ge;
+			e.val64 = 0;
+			ppgtt_set_guest_entry(spt, &e, index);
+
+			ret = ppgtt_handle_guest_write_page_table(
+					&spt->guest_page, &ge, index);
+			if (ret)
+				return ret;
+			clear_bit(index, spt->post_shadow_bitmap);
+		}
+		list_del_init(&spt->post_shadow_list);
+	}
+	return 0;
+}
+
+static int ppgtt_handle_guest_write_page_table_bytes(void *gp,
+		u64 pa, void *p_data, int bytes)
+{
+	struct intel_vgpu_guest_page *gpt = (struct intel_vgpu_guest_page *)gp;
+	struct intel_vgpu_ppgtt_spt *spt = guest_page_to_ppgtt_spt(gpt);
+	struct intel_vgpu *vgpu = spt->vgpu;
+	struct intel_gvt_gtt_pte_ops *ops = vgpu->gvt->gtt.pte_ops;
+	const struct intel_gvt_device_info *info = &vgpu->gvt->device_info;
+	struct intel_gvt_gtt_entry we;
+	unsigned long index;
+	int ret;
+
+	index = (pa & (PAGE_SIZE - 1)) >> info->gtt_entry_size_shift;
+
+	ppgtt_get_guest_entry(spt, &we, index);
+	memcpy((void *)&we.val64 + (pa & (info->gtt_entry_size - 1)),
+			p_data, bytes);
+
+	ops->test_pse(&we);
+
+	if (bytes == info->gtt_entry_size) {
+		ret = ppgtt_handle_guest_write_page_table(gpt, &we, index);
+		if (ret)
+			return ret;
+	} else {
+		struct intel_gvt_gtt_entry ge;
+
+		ppgtt_get_guest_entry(spt, &ge, index);
+
+		if (!test_bit(index, spt->post_shadow_bitmap)) {
+			ret = ppgtt_handle_guest_entry_removal(gpt,
+					&ge, index);
+			if (ret)
+				return ret;
+		}
+
+		ppgtt_set_post_shadow(spt, index);
+		ppgtt_set_guest_entry(spt, &we, index);
+	}
+
+	if (!enable_out_of_sync)
+		return 0;
+
+	gpt->write_cnt++;
+
+	if (gpt->oos_page)
+		ops->set_entry(gpt->oos_page->mem, &we, index,
+				false, 0, vgpu);
+
+	if (can_do_out_of_sync(gpt)) {
+		if (!gpt->oos_page)
+			ppgtt_allocate_oos_page(vgpu, gpt);
+
+		ret = ppgtt_set_guest_page_oos(vgpu, gpt);
+		if (ret < 0)
+			return ret;
+	}
+	return 0;
+}
+
+/*
+ * mm page table allocation policy for bdw+
+ *  - for ggtt, only virtual page table will be allocated.
+ *  - for ppgtt, dedicated virtual/shadow page table will be allocated.
+ */
+static int gen8_mm_alloc_page_table(struct intel_vgpu_mm *mm)
+{
+	struct intel_vgpu *vgpu = mm->vgpu;
+	struct intel_gvt *gvt = vgpu->gvt;
+	const struct intel_gvt_device_info *info = &gvt->device_info;
+	void *mem;
+
+	if (mm->type == INTEL_GVT_MM_PPGTT) {
+		mm->page_table_entry_cnt = 4;
+		mm->page_table_entry_size = mm->page_table_entry_cnt *
+			info->gtt_entry_size;
+		mem = kzalloc(mm->has_shadow_page_table ?
+			mm->page_table_entry_size * 2
+				: mm->page_table_entry_size,
+			GFP_ATOMIC);
+		if (!mem)
+			return -ENOMEM;
+		mm->virtual_page_table = mem;
+		if (!mm->has_shadow_page_table)
+			return 0;
+		mm->shadow_page_table = mem + mm->page_table_entry_size;
+	} else if (mm->type == INTEL_GVT_MM_GGTT) {
+		mm->page_table_entry_cnt =
+			(gvt_ggtt_gm_sz(gvt) >> GTT_PAGE_SHIFT);
+		mm->page_table_entry_size = mm->page_table_entry_cnt *
+			info->gtt_entry_size;
+		mem = vzalloc(mm->page_table_entry_size);
+		if (!mem)
+			return -ENOMEM;
+		mm->virtual_page_table = mem;
+	}
+	return 0;
+}
+
+static void gen8_mm_free_page_table(struct intel_vgpu_mm *mm)
+{
+	if (mm->type == INTEL_GVT_MM_PPGTT) {
+		kfree(mm->virtual_page_table);
+	} else if (mm->type == INTEL_GVT_MM_GGTT) {
+		if (mm->virtual_page_table)
+			vfree(mm->virtual_page_table);
+	}
+	mm->virtual_page_table = mm->shadow_page_table = NULL;
+}
+
+static void invalidate_mm(struct intel_vgpu_mm *mm)
+{
+	struct intel_vgpu *vgpu = mm->vgpu;
+	struct intel_gvt *gvt = vgpu->gvt;
+	struct intel_gvt_gtt *gtt = &gvt->gtt;
+	struct intel_gvt_gtt_pte_ops *ops = gtt->pte_ops;
+	struct intel_gvt_gtt_entry se;
+	int i;
+
+	if (WARN_ON(!mm->has_shadow_page_table || !mm->shadowed))
+		return;
+
+	for (i = 0; i < mm->page_table_entry_cnt; i++) {
+		ppgtt_get_shadow_root_entry(mm, &se, i);
+		if (!ops->test_present(&se))
+			continue;
+		ppgtt_invalidate_shadow_page_by_shadow_entry(
+				vgpu, &se);
+		se.val64 = 0;
+		ppgtt_set_shadow_root_entry(mm, &se, i);
+
+		trace_gpt_change(vgpu->id, "destroy root pointer",
+				NULL, se.type, se.val64, i);
+	}
+	mm->shadowed = false;
+}
+
+/**
+ * intel_vgpu_destroy_mm - destroy a mm object
+ * @mm: a kref object
+ *
+ * This function is used to destroy a mm object for vGPU
+ *
+ */
+void intel_vgpu_destroy_mm(struct kref *mm_ref)
+{
+	struct intel_vgpu_mm *mm = container_of(mm_ref, typeof(*mm), ref);
+	struct intel_vgpu *vgpu = mm->vgpu;
+	struct intel_gvt *gvt = vgpu->gvt;
+	struct intel_gvt_gtt *gtt = &gvt->gtt;
+
+	if (!mm->initialized)
+		goto out;
+
+	list_del(&mm->list);
+	list_del(&mm->lru_list);
+
+	if (mm->has_shadow_page_table)
+		invalidate_mm(mm);
+
+	gtt->mm_free_page_table(mm);
+out:
+	kfree(mm);
+}
+
+static int shadow_mm(struct intel_vgpu_mm *mm)
+{
+	struct intel_vgpu *vgpu = mm->vgpu;
+	struct intel_gvt *gvt = vgpu->gvt;
+	struct intel_gvt_gtt *gtt = &gvt->gtt;
+	struct intel_gvt_gtt_pte_ops *ops = gtt->pte_ops;
+	struct intel_vgpu_ppgtt_spt *spt;
+	struct intel_gvt_gtt_entry ge, se;
+	int i;
+	int ret;
+
+	if (WARN_ON(!mm->has_shadow_page_table || mm->shadowed))
+		return 0;
+
+	mm->shadowed = true;
+
+	for (i = 0; i < mm->page_table_entry_cnt; i++) {
+		ppgtt_get_guest_root_entry(mm, &ge, i);
+		if (!ops->test_present(&ge))
+			continue;
+
+		trace_gpt_change(vgpu->id, __func__, NULL,
+				ge.type, ge.val64, i);
+
+		spt = ppgtt_populate_shadow_page_by_guest_entry(vgpu, &ge);
+		if (IS_ERR(spt)) {
+			gvt_err("fail to populate guest root pointer\n");
+			ret = PTR_ERR(spt);
+			goto fail;
+		}
+		ppgtt_generate_shadow_entry(&se, spt, &ge);
+		ppgtt_set_shadow_root_entry(mm, &se, i);
+
+		trace_gpt_change(vgpu->id, "populate root pointer",
+				NULL, se.type, se.val64, i);
+	}
+	return 0;
+fail:
+	invalidate_mm(mm);
+	return ret;
+}
+
+/**
+ * intel_vgpu_create_mm - create a mm object for a vGPU
+ * @vgpu: a vGPU
+ * @mm_type: mm object type, should be PPGTT or GGTT
+ * @virtual_page_table: page table root pointers. Could be NULL if user wants
+ *	to populate shadow later.
+ * @page_table_level: describe the page table level of the mm object
+ * @pde_base_index: pde root pointer base in GGTT MMIO.
+ *
+ * This function is used to create a mm object for a vGPU.
+ *
+ * Returns:
+ * Zero on success, negative error code in pointer if failed.
+ */
+struct intel_vgpu_mm *intel_vgpu_create_mm(struct intel_vgpu *vgpu,
+		int mm_type, void *virtual_page_table, int page_table_level,
+		u32 pde_base_index)
+{
+	struct intel_gvt *gvt = vgpu->gvt;
+	struct intel_gvt_gtt *gtt = &gvt->gtt;
+	struct intel_vgpu_mm *mm;
+	int ret;
+
+	mm = kzalloc(sizeof(*mm), GFP_ATOMIC);
+	if (!mm) {
+		ret = -ENOMEM;
+		goto fail;
+	}
+
+	mm->type = mm_type;
+
+	if (page_table_level == 1)
+		mm->page_table_entry_type = GTT_TYPE_GGTT_PTE;
+	else if (page_table_level == 3)
+		mm->page_table_entry_type = GTT_TYPE_PPGTT_ROOT_L3_ENTRY;
+	else if (page_table_level == 4)
+		mm->page_table_entry_type = GTT_TYPE_PPGTT_ROOT_L4_ENTRY;
+	else {
+		WARN_ON(1);
+		ret = -EINVAL;
+		goto fail;
+	}
+
+	mm->page_table_level = page_table_level;
+	mm->pde_base_index = pde_base_index;
+
+	mm->vgpu = vgpu;
+	mm->has_shadow_page_table = !!(mm_type == INTEL_GVT_MM_PPGTT);
+
+	kref_init(&mm->ref);
+	atomic_set(&mm->pincount, 0);
+	INIT_LIST_HEAD(&mm->list);
+	INIT_LIST_HEAD(&mm->lru_list);
+	list_add_tail(&mm->list, &vgpu->gtt.mm_list_head);
+
+	ret = gtt->mm_alloc_page_table(mm);
+	if (ret) {
+		gvt_err("fail to allocate page table for mm\n");
+		goto fail;
+	}
+
+	mm->initialized = true;
+
+	if (virtual_page_table)
+		memcpy(mm->virtual_page_table, virtual_page_table,
+				mm->page_table_entry_size);
+
+	if (mm->has_shadow_page_table) {
+		ret = shadow_mm(mm);
+		if (ret)
+			goto fail;
+		list_add_tail(&mm->lru_list, &gvt->gtt.mm_lru_list_head);
+	}
+	return mm;
+fail:
+	gvt_err("fail to create mm\n");
+	if (mm)
+		intel_gvt_mm_unreference(mm);
+	return ERR_PTR(ret);
+}
+
+/**
+ * intel_vgpu_unpin_mm - decrease the pin count of a vGPU mm object
+ * @mm: a vGPU mm object
+ *
+ * This function is called when user doesn't want to use a vGPU mm object
+ */
+void intel_vgpu_unpin_mm(struct intel_vgpu_mm *mm)
+{
+	if (WARN_ON(mm->type != INTEL_GVT_MM_PPGTT))
+		return;
+
+	atomic_dec(&mm->pincount);
+}
+
+/**
+ * intel_vgpu_pin_mm - increase the pin count of a vGPU mm object
+ * @vgpu: a vGPU
+ *
+ * This function is called when user wants to use a vGPU mm object. If this
+ * mm object hasn't been shadowed yet, the shadow will be populated at this
+ * time.
+ *
+ * Returns:
+ * Zero on success, negative error code if failed.
+ */
+int intel_vgpu_pin_mm(struct intel_vgpu_mm *mm)
+{
+	int ret;
+
+	if (WARN_ON(mm->type != INTEL_GVT_MM_PPGTT))
+		return 0;
+
+	atomic_inc(&mm->pincount);
+
+	if (!mm->shadowed) {
+		ret = shadow_mm(mm);
+		if (ret)
+			return ret;
+	}
+
+	list_del_init(&mm->lru_list);
+	list_add_tail(&mm->lru_list, &mm->vgpu->gvt->gtt.mm_lru_list_head);
+	return 0;
+}
+
+static int reclaim_one_mm(struct intel_gvt *gvt)
+{
+	struct intel_vgpu_mm *mm;
+	struct list_head *pos, *n;
+
+	list_for_each_safe(pos, n, &gvt->gtt.mm_lru_list_head) {
+		mm = container_of(pos, struct intel_vgpu_mm, lru_list);
+
+		if (mm->type != INTEL_GVT_MM_PPGTT)
+			continue;
+		if (atomic_read(&mm->pincount))
+			continue;
+
+		list_del_init(&mm->lru_list);
+		invalidate_mm(mm);
+		return 1;
+	}
+	return 0;
+}
+
+/*
+ * GMA translation APIs.
+ */
+static inline int ppgtt_get_next_level_entry(struct intel_vgpu_mm *mm,
+		struct intel_gvt_gtt_entry *e, unsigned long index, bool guest)
+{
+	struct intel_vgpu *vgpu = mm->vgpu;
+	struct intel_gvt_gtt_pte_ops *ops = vgpu->gvt->gtt.pte_ops;
+	struct intel_vgpu_ppgtt_spt *s;
+
+	if (WARN_ON(!mm->has_shadow_page_table))
+		return -EINVAL;
+
+	s = ppgtt_find_shadow_page(vgpu, ops->get_pfn(e));
+	if (!s)
+		return -ENXIO;
+
+	if (!guest)
+		ppgtt_get_shadow_entry(s, e, index);
+	else
+		ppgtt_get_guest_entry(s, e, index);
+	return 0;
+}
+
+/**
+ * intel_vgpu_gma_to_gpa - translate a gma to GPA
+ * @mm: mm object. could be a PPGTT or GGTT mm object
+ * @gma: graphics memory address in this mm object
+ *
+ * This function is used to translate a graphics memory address in specific
+ * graphics memory space to guest physical address.
+ *
+ * Returns:
+ * Guest physical address on success, INTEL_GVT_INVALID_ADDR if failed.
+ */
+unsigned long intel_vgpu_gma_to_gpa(struct intel_vgpu_mm *mm, unsigned long gma)
+{
+	struct intel_vgpu *vgpu = mm->vgpu;
+	struct intel_gvt *gvt = vgpu->gvt;
+	struct intel_gvt_gtt_pte_ops *pte_ops = gvt->gtt.pte_ops;
+	struct intel_gvt_gtt_gma_ops *gma_ops = gvt->gtt.gma_ops;
+	unsigned long gpa = INTEL_GVT_INVALID_ADDR;
+	unsigned long gma_index[4];
+	struct intel_gvt_gtt_entry e;
+	int i, index;
+	int ret;
+
+	if (mm->type != INTEL_GVT_MM_GGTT && mm->type != INTEL_GVT_MM_PPGTT)
+		return INTEL_GVT_INVALID_ADDR;
+
+	if (mm->type == INTEL_GVT_MM_GGTT) {
+		if (!vgpu_gmadr_is_valid(vgpu, gma))
+			goto err;
+
+		ggtt_get_guest_entry(mm, &e,
+			gma_ops->gma_to_ggtt_pte_index(gma));
+		gpa = (pte_ops->get_pfn(&e) << GTT_PAGE_SHIFT)
+			+ (gma & ~GTT_PAGE_MASK);
+
+		trace_gma_translate(vgpu->id, "ggtt", 0, 0, gma, gpa);
+		return gpa;
+	}
+
+	switch (mm->page_table_level) {
+	case 4:
+		ppgtt_get_shadow_root_entry(mm, &e, 0);
+		gma_index[0] = gma_ops->gma_to_pml4_index(gma);
+		gma_index[1] = gma_ops->gma_to_l4_pdp_index(gma);
+		gma_index[2] = gma_ops->gma_to_pde_index(gma);
+		gma_index[3] = gma_ops->gma_to_pte_index(gma);
+		index = 4;
+		break;
+	case 3:
+		ppgtt_get_shadow_root_entry(mm, &e,
+				gma_ops->gma_to_l3_pdp_index(gma));
+		gma_index[0] = gma_ops->gma_to_pde_index(gma);
+		gma_index[1] = gma_ops->gma_to_pte_index(gma);
+		index = 2;
+		break;
+	case 2:
+		ppgtt_get_shadow_root_entry(mm, &e,
+				gma_ops->gma_to_pde_index(gma));
+		gma_index[0] = gma_ops->gma_to_pte_index(gma);
+		index = 1;
+		break;
+	default:
+		WARN_ON(1);
+		goto err;
+	}
+
+	/* walk into the shadow page table and get gpa from guest entry */
+	for (i = 0; i < index; i++) {
+		ret = ppgtt_get_next_level_entry(mm, &e, gma_index[i],
+			(i == index - 1));
+		if (ret)
+			goto err;
+	}
+
+	gpa = (pte_ops->get_pfn(&e) << GTT_PAGE_SHIFT)
+		+ (gma & ~GTT_PAGE_MASK);
+
+	trace_gma_translate(vgpu->id, "ppgtt", 0,
+			mm->page_table_level, gma, gpa);
+	return gpa;
+err:
+	gvt_err("invalid mm type: %d gma %lx\n", mm->type, gma);
+	return INTEL_GVT_INVALID_ADDR;
+}
+
+static int emulate_gtt_mmio_read(struct intel_vgpu *vgpu,
+	unsigned int off, void *p_data, unsigned int bytes)
+{
+	struct intel_vgpu_mm *ggtt_mm = vgpu->gtt.ggtt_mm;
+	const struct intel_gvt_device_info *info = &vgpu->gvt->device_info;
+	unsigned long index = off >> info->gtt_entry_size_shift;
+	struct intel_gvt_gtt_entry e;
+
+	if (bytes != 4 && bytes != 8)
+		return -EINVAL;
+
+	ggtt_get_guest_entry(ggtt_mm, &e, index);
+	memcpy(p_data, (void *)&e.val64 + (off & (info->gtt_entry_size - 1)),
+			bytes);
+	return 0;
+}
+
+/**
+ * intel_vgpu_emulate_gtt_mmio_read - emulate GTT MMIO register read
+ * @vgpu: a vGPU
+ * @off: register offset
+ * @p_data: data will be returned to guest
+ * @bytes: data length
+ *
+ * This function is used to emulate the GTT MMIO register read
+ *
+ * Returns:
+ * Zero on success, error code if failed.
+ */
+int intel_vgpu_emulate_gtt_mmio_read(struct intel_vgpu *vgpu, unsigned int off,
+	void *p_data, unsigned int bytes)
+{
+	const struct intel_gvt_device_info *info = &vgpu->gvt->device_info;
+	int ret;
+
+	if (bytes != 4 && bytes != 8)
+		return -EINVAL;
+
+	off -= info->gtt_start_offset;
+	ret = emulate_gtt_mmio_read(vgpu, off, p_data, bytes);
+	return ret;
+}
+
+static int emulate_gtt_mmio_write(struct intel_vgpu *vgpu, unsigned int off,
+	void *p_data, unsigned int bytes)
+{
+	struct intel_gvt *gvt = vgpu->gvt;
+	const struct intel_gvt_device_info *info = &gvt->device_info;
+	struct intel_vgpu_mm *ggtt_mm = vgpu->gtt.ggtt_mm;
+	struct intel_gvt_gtt_pte_ops *ops = gvt->gtt.pte_ops;
+	unsigned long g_gtt_index = off >> info->gtt_entry_size_shift;
+	unsigned long gma;
+	struct intel_gvt_gtt_entry e, m;
+	int ret;
+
+	if (bytes != 4 && bytes != 8)
+		return -EINVAL;
+
+	gma = g_gtt_index << GTT_PAGE_SHIFT;
+
+	/* the VM may configure the whole GM space when ballooning is used */
+	if (WARN_ONCE(!vgpu_gmadr_is_valid(vgpu, gma),
+				"vgpu%d: found oob ggtt write, offset %x\n",
+				vgpu->id, off)) {
+		return 0;
+	}
+
+	ggtt_get_guest_entry(ggtt_mm, &e, g_gtt_index);
+
+	memcpy((void *)&e.val64 + (off & (info->gtt_entry_size - 1)), p_data,
+			bytes);
+
+	if (ops->test_present(&e)) {
+		ret = gtt_entry_p2m(vgpu, &e, &m);
+		if (ret) {
+			gvt_err("vgpu%d: fail to translate guest gtt entry\n",
+					vgpu->id);
+			return ret;
+		}
+	} else {
+		m = e;
+		m.val64 = 0;
+	}
+
+	ggtt_set_shadow_entry(ggtt_mm, &m, g_gtt_index);
+	ggtt_set_guest_entry(ggtt_mm, &e, g_gtt_index);
+	return 0;
+}
+
+/*
+ * intel_vgpu_emulate_gtt_mmio_write - emulate GTT MMIO register write
+ * @vgpu: a vGPU
+ * @off: register offset
+ * @p_data: data from guest write
+ * @bytes: data length
+ *
+ * This function is used to emulate the GTT MMIO register write
+ *
+ * Returns:
+ * Zero on success, error code if failed.
+ */
+int intel_vgpu_emulate_gtt_mmio_write(struct intel_vgpu *vgpu, unsigned int off,
+	void *p_data, unsigned int bytes)
+{
+	const struct intel_gvt_device_info *info = &vgpu->gvt->device_info;
+	int ret;
+
+	if (bytes != 4 && bytes != 8)
+		return -EINVAL;
+
+	off -= info->gtt_start_offset;
+	ret = emulate_gtt_mmio_write(vgpu, off, p_data, bytes);
+	return ret;
+}
+
+static int create_scratch_page(struct intel_vgpu *vgpu)
+{
+	struct intel_vgpu_gtt *gtt = &vgpu->gtt;
+	void *p;
+	void *vaddr;
+	unsigned long mfn;
+
+	gtt->scratch_page = alloc_page(GFP_KERNEL);
+	if (!gtt->scratch_page) {
+		gvt_err("Failed to allocate scratch page.\n");
+		return -ENOMEM;
+	}
+
+	/* set to zero */
+	p = kmap_atomic(gtt->scratch_page);
+	memset(p, 0, PAGE_SIZE);
+	kunmap_atomic(p);
+
+	/* translate page to mfn */
+	vaddr = page_address(gtt->scratch_page);
+	mfn = intel_gvt_hypervisor_virt_to_mfn(vaddr);
+
+	if (mfn == INTEL_GVT_INVALID_ADDR) {
+		gvt_err("fail to translate vaddr:0x%llx\n", (u64)vaddr);
+		__free_page(gtt->scratch_page);
+		gtt->scratch_page = NULL;
+		return -ENXIO;
+	}
+
+	gtt->scratch_page_mfn = mfn;
+	gvt_dbg_core("vgpu%d create scratch page: mfn=0x%lx\n", vgpu->id, mfn);
+	return 0;
+}
+
+static void release_scratch_page(struct intel_vgpu *vgpu)
+{
+	if (vgpu->gtt.scratch_page != NULL) {
+		__free_page(vgpu->gtt.scratch_page);
+		vgpu->gtt.scratch_page = NULL;
+		vgpu->gtt.scratch_page_mfn = 0;
+	}
+}
+
+/**
+ * intel_vgpu_init_gtt - initialize per-vGPU graphics memory virulization
+ * @vgpu: a vGPU
+ *
+ * This function is used to initialize per-vGPU graphics memory virtualization
+ * components.
+ *
+ * Returns:
+ * Zero on success, error code if failed.
+ */
+int intel_vgpu_init_gtt(struct intel_vgpu *vgpu)
+{
+	struct intel_vgpu_gtt *gtt = &vgpu->gtt;
+	struct intel_vgpu_mm *ggtt_mm;
+
+	hash_init(gtt->guest_page_hash_table);
+	hash_init(gtt->shadow_page_hash_table);
+
+	INIT_LIST_HEAD(&gtt->mm_list_head);
+	INIT_LIST_HEAD(&gtt->oos_page_list_head);
+	INIT_LIST_HEAD(&gtt->post_shadow_list_head);
+
+	ggtt_mm = intel_vgpu_create_mm(vgpu, INTEL_GVT_MM_GGTT,
+			NULL, 1, 0);
+	if (IS_ERR(ggtt_mm)) {
+		gvt_err("fail to create mm for ggtt.\n");
+		return PTR_ERR(ggtt_mm);
+	}
+
+	gtt->ggtt_mm = ggtt_mm;
+
+	return create_scratch_page(vgpu);
+}
+
+/**
+ * intel_vgpu_clean_gtt - clean up per-vGPU graphics memory virulization
+ * @vgpu: a vGPU
+ *
+ * This function is used to clean up per-vGPU graphics memory virtualization
+ * components.
+ *
+ * Returns:
+ * Zero on success, error code if failed.
+ */
+void intel_vgpu_clean_gtt(struct intel_vgpu *vgpu)
+{
+	struct list_head *pos, *n;
+	struct intel_vgpu_mm *mm;
+
+	ppgtt_free_all_shadow_page(vgpu);
+	release_scratch_page(vgpu);
+
+	list_for_each_safe(pos, n, &vgpu->gtt.mm_list_head) {
+		mm = container_of(pos, struct intel_vgpu_mm, list);
+		vgpu->gvt->gtt.mm_free_page_table(mm);
+		list_del(&mm->list);
+		list_del(&mm->lru_list);
+		kfree(mm);
+	}
+}
+
+static void clean_spt_oos(struct intel_gvt *gvt)
+{
+	struct intel_gvt_gtt *gtt = &gvt->gtt;
+	struct list_head *pos, *n;
+	struct intel_vgpu_oos_page *oos_page;
+
+	WARN(!list_empty(&gtt->oos_page_use_list_head),
+		"someone is still using oos page\n");
+
+	list_for_each_safe(pos, n, &gtt->oos_page_free_list_head) {
+		oos_page = container_of(pos, struct intel_vgpu_oos_page, list);
+		list_del(&oos_page->list);
+		kfree(oos_page);
+	}
+}
+
+static int setup_spt_oos(struct intel_gvt *gvt)
+{
+	struct intel_gvt_gtt *gtt = &gvt->gtt;
+	struct intel_vgpu_oos_page *oos_page;
+	int i;
+	int ret;
+
+	INIT_LIST_HEAD(&gtt->oos_page_free_list_head);
+	INIT_LIST_HEAD(&gtt->oos_page_use_list_head);
+
+	for (i = 0; i < preallocated_oos_pages; i++) {
+		oos_page = kzalloc(sizeof(*oos_page), GFP_KERNEL);
+		if (!oos_page) {
+			gvt_err("fail to pre-allocate oos page\n");
+			ret = -ENOMEM;
+			goto fail;
+		}
+
+		INIT_LIST_HEAD(&oos_page->list);
+		INIT_LIST_HEAD(&oos_page->vm_list);
+		oos_page->id = i;
+		list_add_tail(&oos_page->list, &gtt->oos_page_free_list_head);
+	}
+
+	gvt_dbg_mm("%d oos pages preallocated\n", i);
+
+	return 0;
+fail:
+	clean_spt_oos(gvt);
+	return ret;
+}
+
+/**
+ * intel_vgpu_find_ppgtt_mm - find a PPGTT mm object
+ * @vgpu: a vGPU
+ * @page_table_level: PPGTT page table level
+ * @root_entry: PPGTT page table root pointers
+ *
+ * This function is used to find a PPGTT mm object from mm object pool
+ *
+ * Returns:
+ * pointer to mm object on success, NULL if failed.
+ */
+struct intel_vgpu_mm *intel_vgpu_find_ppgtt_mm(struct intel_vgpu *vgpu,
+		int page_table_level, void *root_entry)
+{
+	struct list_head *pos;
+	struct intel_vgpu_mm *mm;
+	u64 *src, *dst;
+
+	list_for_each(pos, &vgpu->gtt.mm_list_head) {
+		mm = container_of(pos, struct intel_vgpu_mm, list);
+		if (mm->type != INTEL_GVT_MM_PPGTT)
+			continue;
+
+		if (mm->page_table_level != page_table_level)
+			continue;
+
+		src = root_entry;
+		dst = mm->virtual_page_table;
+
+		if (page_table_level == 3) {
+			if (src[0] == dst[0]
+					&& src[1] == dst[1]
+					&& src[2] == dst[2]
+					&& src[3] == dst[3])
+				return mm;
+		} else {
+			if (src[0] == dst[0])
+				return mm;
+		}
+	}
+	return NULL;
+}
+
+/**
+ * intel_vgpu_g2v_create_ppgtt_mm - create a PPGTT mm object from
+ * g2v notification
+ * @vgpu: a vGPU
+ * @page_table_level: PPGTT page table level
+ *
+ * This function is used to create a PPGTT mm object from a guest to GVT-g
+ * notification.
+ *
+ * Returns:
+ * Zero on success, negative error code if failed.
+ */
+int intel_vgpu_g2v_create_ppgtt_mm(struct intel_vgpu *vgpu,
+		int page_table_level)
+{
+	u64 *pdp = (u64 *)&vgpu_vreg64(vgpu, vgtif_reg(pdp[0]));
+	struct intel_vgpu_mm *mm;
+
+	if (WARN_ON((page_table_level != 4) && (page_table_level != 3)))
+		return -EINVAL;
+
+	mm = intel_vgpu_find_ppgtt_mm(vgpu, page_table_level, pdp);
+	if (mm) {
+		intel_gvt_mm_reference(mm);
+	} else {
+		mm = intel_vgpu_create_mm(vgpu, INTEL_GVT_MM_PPGTT,
+				pdp, page_table_level, 0);
+		if (IS_ERR(mm)) {
+			gvt_err("fail to create mm\n");
+			return PTR_ERR(mm);
+		}
+	}
+	return 0;
+}
+
+/**
+ * intel_vgpu_g2v_destroy_ppgtt_mm - destroy a PPGTT mm object from
+ * g2v notification
+ * @vgpu: a vGPU
+ * @page_table_level: PPGTT page table level
+ *
+ * This function is used to create a PPGTT mm object from a guest to GVT-g
+ * notification.
+ *
+ * Returns:
+ * Zero on success, negative error code if failed.
+ */
+int intel_vgpu_g2v_destroy_ppgtt_mm(struct intel_vgpu *vgpu,
+		int page_table_level)
+{
+	u64 *pdp = (u64 *)&vgpu_vreg64(vgpu, vgtif_reg(pdp[0]));
+	struct intel_vgpu_mm *mm;
+
+	if (WARN_ON((page_table_level != 4) && (page_table_level != 3)))
+		return -EINVAL;
+
+	mm = intel_vgpu_find_ppgtt_mm(vgpu, page_table_level, pdp);
+	if (!mm) {
+		gvt_err("fail to find ppgtt instance.\n");
+		return -EINVAL;
+	}
+	intel_gvt_mm_unreference(mm);
+	return 0;
+}
+
+/**
+ * intel_gvt_init_gtt - initialize mm components of a GVT device
+ * @gvt: GVT device
+ *
+ * This function is called at the initialization stage, to initialize
+ * the mm components of a GVT device.
+ *
+ * Returns:
+ * zero on success, negative error code if failed.
+ */
+int intel_gvt_init_gtt(struct intel_gvt *gvt)
+{
+	int ret;
+
+	gvt_dbg_core("init gtt\n");
+
+	if (IS_BROADWELL(gvt->dev_priv) || IS_SKYLAKE(gvt->dev_priv)) {
+		gvt->gtt.pte_ops = &gen8_gtt_pte_ops;
+		gvt->gtt.gma_ops = &gen8_gtt_gma_ops;
+		gvt->gtt.mm_alloc_page_table = gen8_mm_alloc_page_table;
+		gvt->gtt.mm_free_page_table = gen8_mm_free_page_table;
+	} else {
+		return -ENODEV;
+	}
+
+	if (enable_out_of_sync) {
+		ret = setup_spt_oos(gvt);
+		if (ret) {
+			gvt_err("fail to initialize SPT oos\n");
+			return ret;
+		}
+	}
+	INIT_LIST_HEAD(&gvt->gtt.mm_lru_list_head);
+	return 0;
+}
+
+/**
+ * intel_gvt_clean_gtt - clean up mm components of a GVT device
+ * @gvt: GVT device
+ *
+ * This function is called at the driver unloading stage, to clean up the
+ * the mm components of a GVT device.
+ *
+ */
+void intel_gvt_clean_gtt(struct intel_gvt *gvt)
+{
+	if (enable_out_of_sync)
+		clean_spt_oos(gvt);
+}
diff --git a/drivers/gpu/drm/i915/gvt/gtt.h b/drivers/gpu/drm/i915/gvt/gtt.h
new file mode 100644
index 000000000000..e4dcde78f3f9
--- /dev/null
+++ b/drivers/gpu/drm/i915/gvt/gtt.h
@@ -0,0 +1,270 @@
+/*
+ * Copyright(c) 2011-2016 Intel Corporation. All rights reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ * Authors:
+ *    Zhi Wang <zhi.a.wang@intel.com>
+ *    Zhenyu Wang <zhenyuw@linux.intel.com>
+ *    Xiao Zheng <xiao.zheng@intel.com>
+ *
+ * Contributors:
+ *    Min He <min.he@intel.com>
+ *    Bing Niu <bing.niu@intel.com>
+ *
+ */
+
+#ifndef _GVT_GTT_H_
+#define _GVT_GTT_H_
+
+#define GTT_PAGE_SHIFT		12
+#define GTT_PAGE_SIZE		(1UL << GTT_PAGE_SHIFT)
+#define GTT_PAGE_MASK		(~(GTT_PAGE_SIZE-1))
+
+struct intel_vgpu_mm;
+
+#define INTEL_GVT_GTT_HASH_BITS 8
+#define INTEL_GVT_INVALID_ADDR (~0UL)
+
+struct intel_gvt_gtt_entry {
+	u64 val64;
+	int type;
+};
+
+struct intel_gvt_gtt_pte_ops {
+	struct intel_gvt_gtt_entry *(*get_entry)(void *pt,
+		struct intel_gvt_gtt_entry *e,
+		unsigned long index, bool hypervisor_access, unsigned long gpa,
+		struct intel_vgpu *vgpu);
+	struct intel_gvt_gtt_entry *(*set_entry)(void *pt,
+		struct intel_gvt_gtt_entry *e,
+		unsigned long index, bool hypervisor_access, unsigned long gpa,
+		struct intel_vgpu *vgpu);
+	bool (*test_present)(struct intel_gvt_gtt_entry *e);
+	void (*clear_present)(struct intel_gvt_gtt_entry *e);
+	bool (*test_pse)(struct intel_gvt_gtt_entry *e);
+	void (*set_pfn)(struct intel_gvt_gtt_entry *e, unsigned long pfn);
+	unsigned long (*get_pfn)(struct intel_gvt_gtt_entry *e);
+};
+
+struct intel_gvt_gtt_gma_ops {
+	unsigned long (*gma_to_ggtt_pte_index)(unsigned long gma);
+	unsigned long (*gma_to_pte_index)(unsigned long gma);
+	unsigned long (*gma_to_pde_index)(unsigned long gma);
+	unsigned long (*gma_to_l3_pdp_index)(unsigned long gma);
+	unsigned long (*gma_to_l4_pdp_index)(unsigned long gma);
+	unsigned long (*gma_to_pml4_index)(unsigned long gma);
+};
+
+struct intel_gvt_gtt {
+	struct intel_gvt_gtt_pte_ops *pte_ops;
+	struct intel_gvt_gtt_gma_ops *gma_ops;
+	int (*mm_alloc_page_table)(struct intel_vgpu_mm *mm);
+	void (*mm_free_page_table)(struct intel_vgpu_mm *mm);
+	struct list_head oos_page_use_list_head;
+	struct list_head oos_page_free_list_head;
+	struct list_head mm_lru_list_head;
+};
+
+enum {
+	INTEL_GVT_MM_GGTT = 0,
+	INTEL_GVT_MM_PPGTT,
+};
+
+struct intel_vgpu_mm {
+	int type;
+	bool initialized;
+	bool shadowed;
+
+	int page_table_entry_type;
+	u32 page_table_entry_size;
+	u32 page_table_entry_cnt;
+	void *virtual_page_table;
+	void *shadow_page_table;
+
+	int page_table_level;
+	bool has_shadow_page_table;
+	u32 pde_base_index;
+
+	struct list_head list;
+	struct kref ref;
+	atomic_t pincount;
+	struct list_head lru_list;
+	struct intel_vgpu *vgpu;
+};
+
+extern struct intel_gvt_gtt_entry *intel_vgpu_mm_get_entry(
+		struct intel_vgpu_mm *mm,
+		void *page_table, struct intel_gvt_gtt_entry *e,
+		unsigned long index);
+
+extern struct intel_gvt_gtt_entry *intel_vgpu_mm_set_entry(
+		struct intel_vgpu_mm *mm,
+		void *page_table, struct intel_gvt_gtt_entry *e,
+		unsigned long index);
+
+#define ggtt_get_guest_entry(mm, e, index) \
+	intel_vgpu_mm_get_entry(mm, mm->virtual_page_table, e, index)
+
+#define ggtt_set_guest_entry(mm, e, index) \
+	intel_vgpu_mm_set_entry(mm, mm->virtual_page_table, e, index)
+
+#define ggtt_get_shadow_entry(mm, e, index) \
+	intel_vgpu_mm_get_entry(mm, mm->shadow_page_table, e, index)
+
+#define ggtt_set_shadow_entry(mm, e, index) \
+	intel_vgpu_mm_set_entry(mm, mm->shadow_page_table, e, index)
+
+#define ppgtt_get_guest_root_entry(mm, e, index) \
+	intel_vgpu_mm_get_entry(mm, mm->virtual_page_table, e, index)
+
+#define ppgtt_set_guest_root_entry(mm, e, index) \
+	intel_vgpu_mm_set_entry(mm, mm->virtual_page_table, e, index)
+
+#define ppgtt_get_shadow_root_entry(mm, e, index) \
+	intel_vgpu_mm_get_entry(mm, mm->shadow_page_table, e, index)
+
+#define ppgtt_set_shadow_root_entry(mm, e, index) \
+	intel_vgpu_mm_set_entry(mm, mm->shadow_page_table, e, index)
+
+extern struct intel_vgpu_mm *intel_vgpu_create_mm(struct intel_vgpu *vgpu,
+		int mm_type, void *virtual_page_table, int page_table_level,
+		u32 pde_base_index);
+extern void intel_vgpu_destroy_mm(struct kref *mm_ref);
+
+struct intel_vgpu_guest_page;
+
+struct intel_vgpu_gtt {
+	struct intel_vgpu_mm *ggtt_mm;
+	unsigned long active_ppgtt_mm_bitmap;
+	struct list_head mm_list_head;
+	DECLARE_HASHTABLE(shadow_page_hash_table, INTEL_GVT_GTT_HASH_BITS);
+	DECLARE_HASHTABLE(guest_page_hash_table, INTEL_GVT_GTT_HASH_BITS);
+	atomic_t n_write_protected_guest_page;
+	struct list_head oos_page_list_head;
+	struct list_head post_shadow_list_head;
+	struct page *scratch_page;
+	unsigned long scratch_page_mfn;
+};
+
+extern int intel_vgpu_init_gtt(struct intel_vgpu *vgpu);
+extern void intel_vgpu_clean_gtt(struct intel_vgpu *vgpu);
+
+extern int intel_gvt_init_gtt(struct intel_gvt *gvt);
+extern void intel_gvt_clean_gtt(struct intel_gvt *gvt);
+
+extern struct intel_vgpu_mm *intel_gvt_find_ppgtt_mm(struct intel_vgpu *vgpu,
+		int page_table_level, void *root_entry);
+
+struct intel_vgpu_oos_page;
+
+struct intel_vgpu_shadow_page {
+	void *vaddr;
+	struct page *page;
+	int type;
+	struct hlist_node node;
+	unsigned long mfn;
+};
+
+struct intel_vgpu_guest_page {
+	struct hlist_node node;
+	bool writeprotection;
+	unsigned long gfn;
+	int (*handler)(void *, u64, void *, int);
+	void *data;
+	unsigned long write_cnt;
+	struct intel_vgpu_oos_page *oos_page;
+};
+
+struct intel_vgpu_oos_page {
+	struct intel_vgpu_guest_page *guest_page;
+	struct list_head list;
+	struct list_head vm_list;
+	int id;
+	unsigned char mem[GTT_PAGE_SIZE];
+};
+
+#define GTT_ENTRY_NUM_IN_ONE_PAGE 512
+
+struct intel_vgpu_ppgtt_spt {
+	struct intel_vgpu_shadow_page shadow_page;
+	struct intel_vgpu_guest_page guest_page;
+	int guest_page_type;
+	atomic_t refcount;
+	struct intel_vgpu *vgpu;
+	DECLARE_BITMAP(post_shadow_bitmap, GTT_ENTRY_NUM_IN_ONE_PAGE);
+	struct list_head post_shadow_list;
+};
+
+int intel_vgpu_init_guest_page(struct intel_vgpu *vgpu,
+		struct intel_vgpu_guest_page *guest_page,
+		unsigned long gfn,
+		int (*handler)(void *gp, u64, void *, int),
+		void *data);
+
+void intel_vgpu_clean_guest_page(struct intel_vgpu *vgpu,
+		struct intel_vgpu_guest_page *guest_page);
+
+int intel_vgpu_set_guest_page_writeprotection(struct intel_vgpu *vgpu,
+		struct intel_vgpu_guest_page *guest_page);
+
+void intel_vgpu_clear_guest_page_writeprotection(struct intel_vgpu *vgpu,
+		struct intel_vgpu_guest_page *guest_page);
+
+struct intel_vgpu_guest_page *intel_vgpu_find_guest_page(
+		struct intel_vgpu *vgpu, unsigned long gfn);
+
+int intel_vgpu_sync_oos_pages(struct intel_vgpu *vgpu);
+
+int intel_vgpu_flush_post_shadow(struct intel_vgpu *vgpu);
+
+static inline void intel_gvt_mm_reference(struct intel_vgpu_mm *mm)
+{
+	kref_get(&mm->ref);
+}
+
+static inline void intel_gvt_mm_unreference(struct intel_vgpu_mm *mm)
+{
+	kref_put(&mm->ref, intel_vgpu_destroy_mm);
+}
+
+int intel_vgpu_pin_mm(struct intel_vgpu_mm *mm);
+
+void intel_vgpu_unpin_mm(struct intel_vgpu_mm *mm);
+
+unsigned long intel_vgpu_gma_to_gpa(struct intel_vgpu_mm *mm,
+		unsigned long gma);
+
+struct intel_vgpu_mm *intel_vgpu_find_ppgtt_mm(struct intel_vgpu *vgpu,
+		int page_table_level, void *root_entry);
+
+int intel_vgpu_g2v_create_ppgtt_mm(struct intel_vgpu *vgpu,
+		int page_table_level);
+
+int intel_vgpu_g2v_destroy_ppgtt_mm(struct intel_vgpu *vgpu,
+		int page_table_level);
+
+int intel_vgpu_emulate_gtt_mmio_read(struct intel_vgpu *vgpu,
+	unsigned int off, void *p_data, unsigned int bytes);
+
+int intel_vgpu_emulate_gtt_mmio_write(struct intel_vgpu *vgpu,
+	unsigned int off, void *p_data, unsigned int bytes);
+
+#endif /* _GVT_GTT_H_ */
diff --git a/drivers/gpu/drm/i915/gvt/gvt.c b/drivers/gpu/drm/i915/gvt/gvt.c
index 927f4579f5b6..31b59d40f3fb 100644
--- a/drivers/gpu/drm/i915/gvt/gvt.c
+++ b/drivers/gpu/drm/i915/gvt/gvt.c
@@ -19,12 +19,23 @@
  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
  * SOFTWARE.
+ *
+ * Authors:
+ *    Kevin Tian <kevin.tian@intel.com>
+ *    Eddie Dong <eddie.dong@intel.com>
+ *
+ * Contributors:
+ *    Niu Bing <bing.niu@intel.com>
+ *    Zhi Wang <zhi.a.wang@intel.com>
+ *
  */
 
 #include <linux/types.h>
 #include <xen/xen.h>
+#include <linux/kthread.h>
 
 #include "i915_drv.h"
+#include "gvt.h"
 
 struct intel_gvt_host intel_gvt_host;
 
@@ -33,6 +44,13 @@ static const char * const supported_hypervisors[] = {
 	[INTEL_GVT_HYPERVISOR_KVM] = "KVM",
 };
 
+struct intel_gvt_io_emulation_ops intel_gvt_io_emulation_ops = {
+	.emulate_cfg_read = intel_vgpu_emulate_cfg_read,
+	.emulate_cfg_write = intel_vgpu_emulate_cfg_write,
+	.emulate_mmio_read = intel_vgpu_emulate_mmio_read,
+	.emulate_mmio_write = intel_vgpu_emulate_mmio_write,
+};
+
 /**
  * intel_gvt_init_host - Load MPT modules and detect if we're running in host
  * @gvt: intel gvt device
@@ -84,9 +102,66 @@ int intel_gvt_init_host(void)
 
 static void init_device_info(struct intel_gvt *gvt)
 {
-	if (IS_BROADWELL(gvt->dev_priv))
-		gvt->device_info.max_support_vgpus = 8;
-	/* This function will grow large in GVT device model patches. */
+	struct intel_gvt_device_info *info = &gvt->device_info;
+
+	if (IS_BROADWELL(gvt->dev_priv) || IS_SKYLAKE(gvt->dev_priv)) {
+		info->max_support_vgpus = 8;
+		info->cfg_space_size = 256;
+		info->mmio_size = 2 * 1024 * 1024;
+		info->mmio_bar = 0;
+		info->msi_cap_offset = IS_SKYLAKE(gvt->dev_priv) ? 0xac : 0x90;
+		info->gtt_start_offset = 8 * 1024 * 1024;
+		info->gtt_entry_size = 8;
+		info->gtt_entry_size_shift = 3;
+		info->gmadr_bytes_in_cmd = 8;
+		info->max_surface_size = 36 * 1024 * 1024;
+	}
+}
+
+static int gvt_service_thread(void *data)
+{
+	struct intel_gvt *gvt = (struct intel_gvt *)data;
+	int ret;
+
+	gvt_dbg_core("service thread start\n");
+
+	while (!kthread_should_stop()) {
+		ret = wait_event_interruptible(gvt->service_thread_wq,
+				kthread_should_stop() || gvt->service_request);
+
+		if (kthread_should_stop())
+			break;
+
+		if (WARN_ONCE(ret, "service thread is waken up by signal.\n"))
+			continue;
+
+		if (test_and_clear_bit(INTEL_GVT_REQUEST_EMULATE_VBLANK,
+					(void *)&gvt->service_request)) {
+			mutex_lock(&gvt->lock);
+			intel_gvt_emulate_vblank(gvt);
+			mutex_unlock(&gvt->lock);
+		}
+	}
+
+	return 0;
+}
+
+static void clean_service_thread(struct intel_gvt *gvt)
+{
+	kthread_stop(gvt->service_thread);
+}
+
+static int init_service_thread(struct intel_gvt *gvt)
+{
+	init_waitqueue_head(&gvt->service_thread_wq);
+
+	gvt->service_thread = kthread_run(gvt_service_thread,
+			gvt, "gvt_service_thread");
+	if (IS_ERR(gvt->service_thread)) {
+		gvt_err("fail to start service thread.\n");
+		return PTR_ERR(gvt->service_thread);
+	}
+	return 0;
 }
 
 /**
@@ -99,14 +174,23 @@ static void init_device_info(struct intel_gvt *gvt)
  */
 void intel_gvt_clean_device(struct drm_i915_private *dev_priv)
 {
-	struct intel_gvt *gvt = &dev_priv->gvt;
+	struct intel_gvt *gvt = to_gvt(dev_priv);
 
-	if (WARN_ON(!gvt->initialized))
+	if (WARN_ON(!gvt))
 		return;
 
-	/* Other de-initialization of GVT components will be introduced. */
+	clean_service_thread(gvt);
+	intel_gvt_clean_cmd_parser(gvt);
+	intel_gvt_clean_sched_policy(gvt);
+	intel_gvt_clean_workload_scheduler(gvt);
+	intel_gvt_clean_opregion(gvt);
+	intel_gvt_clean_gtt(gvt);
+	intel_gvt_clean_irq(gvt);
+	intel_gvt_clean_mmio_info(gvt);
+	intel_gvt_free_firmware(gvt);
 
-	gvt->initialized = false;
+	kfree(dev_priv->gvt);
+	dev_priv->gvt = NULL;
 }
 
 /**
@@ -122,7 +206,9 @@ void intel_gvt_clean_device(struct drm_i915_private *dev_priv)
  */
 int intel_gvt_init_device(struct drm_i915_private *dev_priv)
 {
-	struct intel_gvt *gvt = &dev_priv->gvt;
+	struct intel_gvt *gvt;
+	int ret;
+
 	/*
 	 * Cannot initialize GVT device without intel_gvt_host gets
 	 * initialized first.
@@ -130,16 +216,76 @@ int intel_gvt_init_device(struct drm_i915_private *dev_priv)
 	if (WARN_ON(!intel_gvt_host.initialized))
 		return -EINVAL;
 
-	if (WARN_ON(gvt->initialized))
+	if (WARN_ON(dev_priv->gvt))
 		return -EEXIST;
 
+	gvt = kzalloc(sizeof(struct intel_gvt), GFP_KERNEL);
+	if (!gvt)
+		return -ENOMEM;
+
 	gvt_dbg_core("init gvt device\n");
 
+	mutex_init(&gvt->lock);
+	gvt->dev_priv = dev_priv;
+
 	init_device_info(gvt);
-	/*
-	 * Other initialization of GVT components will be introduce here.
-	 */
+
+	ret = intel_gvt_setup_mmio_info(gvt);
+	if (ret)
+		return ret;
+
+	ret = intel_gvt_load_firmware(gvt);
+	if (ret)
+		goto out_clean_mmio_info;
+
+	ret = intel_gvt_init_irq(gvt);
+	if (ret)
+		goto out_free_firmware;
+
+	ret = intel_gvt_init_gtt(gvt);
+	if (ret)
+		goto out_clean_irq;
+
+	ret = intel_gvt_init_opregion(gvt);
+	if (ret)
+		goto out_clean_gtt;
+
+	ret = intel_gvt_init_workload_scheduler(gvt);
+	if (ret)
+		goto out_clean_opregion;
+
+	ret = intel_gvt_init_sched_policy(gvt);
+	if (ret)
+		goto out_clean_workload_scheduler;
+
+	ret = intel_gvt_init_cmd_parser(gvt);
+	if (ret)
+		goto out_clean_sched_policy;
+
+	ret = init_service_thread(gvt);
+	if (ret)
+		goto out_clean_cmd_parser;
+
 	gvt_dbg_core("gvt device creation is done\n");
-	gvt->initialized = true;
+	dev_priv->gvt = gvt;
 	return 0;
+
+out_clean_cmd_parser:
+	intel_gvt_clean_cmd_parser(gvt);
+out_clean_sched_policy:
+	intel_gvt_clean_sched_policy(gvt);
+out_clean_workload_scheduler:
+	intel_gvt_clean_workload_scheduler(gvt);
+out_clean_opregion:
+	intel_gvt_clean_opregion(gvt);
+out_clean_gtt:
+	intel_gvt_clean_gtt(gvt);
+out_clean_irq:
+	intel_gvt_clean_irq(gvt);
+out_free_firmware:
+	intel_gvt_free_firmware(gvt);
+out_clean_mmio_info:
+	intel_gvt_clean_mmio_info(gvt);
+	kfree(gvt);
+	return ret;
 }
diff --git a/drivers/gpu/drm/i915/gvt/gvt.h b/drivers/gpu/drm/i915/gvt/gvt.h
index fb619a6e519d..11df62b542b1 100644
--- a/drivers/gpu/drm/i915/gvt/gvt.h
+++ b/drivers/gpu/drm/i915/gvt/gvt.h
@@ -19,6 +19,15 @@
  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
  * SOFTWARE.
+ *
+ * Authors:
+ *    Kevin Tian <kevin.tian@intel.com>
+ *    Eddie Dong <eddie.dong@intel.com>
+ *
+ * Contributors:
+ *    Niu Bing <bing.niu@intel.com>
+ *    Zhi Wang <zhi.a.wang@intel.com>
+ *
  */
 
 #ifndef _GVT_H_
@@ -26,6 +35,17 @@
 
 #include "debug.h"
 #include "hypercall.h"
+#include "mmio.h"
+#include "reg.h"
+#include "interrupt.h"
+#include "gtt.h"
+#include "display.h"
+#include "edid.h"
+#include "execlist.h"
+#include "scheduler.h"
+#include "sched_policy.h"
+#include "render.h"
+#include "cmd_parser.h"
 
 #define GVT_MAX_VGPU 8
 
@@ -45,25 +65,324 @@ extern struct intel_gvt_host intel_gvt_host;
 /* Describe per-platform limitations. */
 struct intel_gvt_device_info {
 	u32 max_support_vgpus;
-	/* This data structure will grow bigger in GVT device model patches */
+	u32 cfg_space_size;
+	u32 mmio_size;
+	u32 mmio_bar;
+	unsigned long msi_cap_offset;
+	u32 gtt_start_offset;
+	u32 gtt_entry_size;
+	u32 gtt_entry_size_shift;
+	int gmadr_bytes_in_cmd;
+	u32 max_surface_size;
+};
+
+/* GM resources owned by a vGPU */
+struct intel_vgpu_gm {
+	u64 aperture_sz;
+	u64 hidden_sz;
+	struct drm_mm_node low_gm_node;
+	struct drm_mm_node high_gm_node;
+};
+
+#define INTEL_GVT_MAX_NUM_FENCES 32
+
+/* Fences owned by a vGPU */
+struct intel_vgpu_fence {
+	struct drm_i915_fence_reg *regs[INTEL_GVT_MAX_NUM_FENCES];
+	u32 base;
+	u32 size;
+};
+
+struct intel_vgpu_mmio {
+	void *vreg;
+	void *sreg;
+	bool disable_warn_untrack;
+};
+
+#define INTEL_GVT_MAX_CFG_SPACE_SZ 256
+#define INTEL_GVT_MAX_BAR_NUM 4
+
+struct intel_vgpu_pci_bar {
+	u64 size;
+	bool tracked;
+};
+
+struct intel_vgpu_cfg_space {
+	unsigned char virtual_cfg_space[INTEL_GVT_MAX_CFG_SPACE_SZ];
+	struct intel_vgpu_pci_bar bar[INTEL_GVT_MAX_BAR_NUM];
+};
+
+#define vgpu_cfg_space(vgpu) ((vgpu)->cfg_space.virtual_cfg_space)
+
+#define INTEL_GVT_MAX_PIPE 4
+
+struct intel_vgpu_irq {
+	bool irq_warn_once[INTEL_GVT_EVENT_MAX];
+	DECLARE_BITMAP(flip_done_event[INTEL_GVT_MAX_PIPE],
+		       INTEL_GVT_EVENT_MAX);
+};
+
+struct intel_vgpu_opregion {
+	void *va;
+	u32 gfn[INTEL_GVT_OPREGION_PAGES];
+	struct page *pages[INTEL_GVT_OPREGION_PAGES];
+};
+
+#define vgpu_opregion(vgpu) (&(vgpu->opregion))
+
+#define INTEL_GVT_MAX_PORT 5
+
+struct intel_vgpu_display {
+	struct intel_vgpu_i2c_edid i2c_edid;
+	struct intel_vgpu_port ports[INTEL_GVT_MAX_PORT];
+	struct intel_vgpu_sbi sbi;
 };
 
 struct intel_vgpu {
 	struct intel_gvt *gvt;
 	int id;
 	unsigned long handle; /* vGPU handle used by hypervisor MPT modules */
+	bool active;
+	bool resetting;
+	void *sched_data;
+
+	struct intel_vgpu_fence fence;
+	struct intel_vgpu_gm gm;
+	struct intel_vgpu_cfg_space cfg_space;
+	struct intel_vgpu_mmio mmio;
+	struct intel_vgpu_irq irq;
+	struct intel_vgpu_gtt gtt;
+	struct intel_vgpu_opregion opregion;
+	struct intel_vgpu_display display;
+	struct intel_vgpu_execlist execlist[I915_NUM_ENGINES];
+	struct list_head workload_q_head[I915_NUM_ENGINES];
+	struct kmem_cache *workloads;
+	atomic_t running_workload_num;
+	DECLARE_BITMAP(tlb_handle_pending, I915_NUM_ENGINES);
+	struct i915_gem_context *shadow_ctx;
+	struct notifier_block shadow_ctx_notifier_block;
+};
+
+struct intel_gvt_gm {
+	unsigned long vgpu_allocated_low_gm_size;
+	unsigned long vgpu_allocated_high_gm_size;
+};
+
+struct intel_gvt_fence {
+	unsigned long vgpu_allocated_fence_num;
+};
+
+#define INTEL_GVT_MMIO_HASH_BITS 9
+
+struct intel_gvt_mmio {
+	u32 *mmio_attribute;
+	DECLARE_HASHTABLE(mmio_info_table, INTEL_GVT_MMIO_HASH_BITS);
+};
+
+struct intel_gvt_firmware {
+	void *cfg_space;
+	void *mmio;
+	bool firmware_loaded;
+};
+
+struct intel_gvt_opregion {
+	void __iomem *opregion_va;
+	u32 opregion_pa;
 };
 
 struct intel_gvt {
 	struct mutex lock;
-	bool initialized;
-
 	struct drm_i915_private *dev_priv;
 	struct idr vgpu_idr;	/* vGPU IDR pool */
 
 	struct intel_gvt_device_info device_info;
+	struct intel_gvt_gm gm;
+	struct intel_gvt_fence fence;
+	struct intel_gvt_mmio mmio;
+	struct intel_gvt_firmware firmware;
+	struct intel_gvt_irq irq;
+	struct intel_gvt_gtt gtt;
+	struct intel_gvt_opregion opregion;
+	struct intel_gvt_workload_scheduler scheduler;
+	DECLARE_HASHTABLE(cmd_table, GVT_CMD_HASH_BITS);
+
+	struct task_struct *service_thread;
+	wait_queue_head_t service_thread_wq;
+	unsigned long service_request;
 };
 
+static inline struct intel_gvt *to_gvt(struct drm_i915_private *i915)
+{
+	return i915->gvt;
+}
+
+enum {
+	INTEL_GVT_REQUEST_EMULATE_VBLANK = 0,
+};
+
+static inline void intel_gvt_request_service(struct intel_gvt *gvt,
+		int service)
+{
+	set_bit(service, (void *)&gvt->service_request);
+	wake_up(&gvt->service_thread_wq);
+}
+
+void intel_gvt_free_firmware(struct intel_gvt *gvt);
+int intel_gvt_load_firmware(struct intel_gvt *gvt);
+
+/* Aperture/GM space definitions for GVT device */
+#define gvt_aperture_sz(gvt)	  (gvt->dev_priv->ggtt.mappable_end)
+#define gvt_aperture_pa_base(gvt) (gvt->dev_priv->ggtt.mappable_base)
+
+#define gvt_ggtt_gm_sz(gvt)	  (gvt->dev_priv->ggtt.base.total)
+#define gvt_ggtt_sz(gvt) \
+	((gvt->dev_priv->ggtt.base.total >> PAGE_SHIFT) << 3)
+#define gvt_hidden_sz(gvt)	  (gvt_ggtt_gm_sz(gvt) - gvt_aperture_sz(gvt))
+
+#define gvt_aperture_gmadr_base(gvt) (0)
+#define gvt_aperture_gmadr_end(gvt) (gvt_aperture_gmadr_base(gvt) \
+				     + gvt_aperture_sz(gvt) - 1)
+
+#define gvt_hidden_gmadr_base(gvt) (gvt_aperture_gmadr_base(gvt) \
+				    + gvt_aperture_sz(gvt))
+#define gvt_hidden_gmadr_end(gvt) (gvt_hidden_gmadr_base(gvt) \
+				   + gvt_hidden_sz(gvt) - 1)
+
+#define gvt_fence_sz(gvt) (gvt->dev_priv->num_fence_regs)
+
+/* Aperture/GM space definitions for vGPU */
+#define vgpu_aperture_offset(vgpu)	((vgpu)->gm.low_gm_node.start)
+#define vgpu_hidden_offset(vgpu)	((vgpu)->gm.high_gm_node.start)
+#define vgpu_aperture_sz(vgpu)		((vgpu)->gm.aperture_sz)
+#define vgpu_hidden_sz(vgpu)		((vgpu)->gm.hidden_sz)
+
+#define vgpu_aperture_pa_base(vgpu) \
+	(gvt_aperture_pa_base(vgpu->gvt) + vgpu_aperture_offset(vgpu))
+
+#define vgpu_ggtt_gm_sz(vgpu) ((vgpu)->gm.aperture_sz + (vgpu)->gm.hidden_sz)
+
+#define vgpu_aperture_pa_end(vgpu) \
+	(vgpu_aperture_pa_base(vgpu) + vgpu_aperture_sz(vgpu) - 1)
+
+#define vgpu_aperture_gmadr_base(vgpu) (vgpu_aperture_offset(vgpu))
+#define vgpu_aperture_gmadr_end(vgpu) \
+	(vgpu_aperture_gmadr_base(vgpu) + vgpu_aperture_sz(vgpu) - 1)
+
+#define vgpu_hidden_gmadr_base(vgpu) (vgpu_hidden_offset(vgpu))
+#define vgpu_hidden_gmadr_end(vgpu) \
+	(vgpu_hidden_gmadr_base(vgpu) + vgpu_hidden_sz(vgpu) - 1)
+
+#define vgpu_fence_base(vgpu) (vgpu->fence.base)
+#define vgpu_fence_sz(vgpu) (vgpu->fence.size)
+
+struct intel_vgpu_creation_params {
+	__u64 handle;
+	__u64 low_gm_sz;  /* in MB */
+	__u64 high_gm_sz; /* in MB */
+	__u64 fence_sz;
+	__s32 primary;
+	__u64 vgpu_id;
+};
+
+int intel_vgpu_alloc_resource(struct intel_vgpu *vgpu,
+			      struct intel_vgpu_creation_params *param);
+void intel_vgpu_free_resource(struct intel_vgpu *vgpu);
+void intel_vgpu_write_fence(struct intel_vgpu *vgpu,
+	u32 fence, u64 value);
+
+/* Macros for easily accessing vGPU virtual/shadow register */
+#define vgpu_vreg(vgpu, reg) \
+	(*(u32 *)(vgpu->mmio.vreg + INTEL_GVT_MMIO_OFFSET(reg)))
+#define vgpu_vreg8(vgpu, reg) \
+	(*(u8 *)(vgpu->mmio.vreg + INTEL_GVT_MMIO_OFFSET(reg)))
+#define vgpu_vreg16(vgpu, reg) \
+	(*(u16 *)(vgpu->mmio.vreg + INTEL_GVT_MMIO_OFFSET(reg)))
+#define vgpu_vreg64(vgpu, reg) \
+	(*(u64 *)(vgpu->mmio.vreg + INTEL_GVT_MMIO_OFFSET(reg)))
+#define vgpu_sreg(vgpu, reg) \
+	(*(u32 *)(vgpu->mmio.sreg + INTEL_GVT_MMIO_OFFSET(reg)))
+#define vgpu_sreg8(vgpu, reg) \
+	(*(u8 *)(vgpu->mmio.sreg + INTEL_GVT_MMIO_OFFSET(reg)))
+#define vgpu_sreg16(vgpu, reg) \
+	(*(u16 *)(vgpu->mmio.sreg + INTEL_GVT_MMIO_OFFSET(reg)))
+#define vgpu_sreg64(vgpu, reg) \
+	(*(u64 *)(vgpu->mmio.sreg + INTEL_GVT_MMIO_OFFSET(reg)))
+
+#define for_each_active_vgpu(gvt, vgpu, id) \
+	idr_for_each_entry((&(gvt)->vgpu_idr), (vgpu), (id)) \
+		for_each_if(vgpu->active)
+
+static inline void intel_vgpu_write_pci_bar(struct intel_vgpu *vgpu,
+					    u32 offset, u32 val, bool low)
+{
+	u32 *pval;
+
+	/* BAR offset should be 32 bits algiend */
+	offset = rounddown(offset, 4);
+	pval = (u32 *)(vgpu_cfg_space(vgpu) + offset);
+
+	if (low) {
+		/*
+		 * only update bit 31 - bit 4,
+		 * leave the bit 3 - bit 0 unchanged.
+		 */
+		*pval = (val & GENMASK(31, 4)) | (*pval & GENMASK(3, 0));
+	}
+}
+
+struct intel_vgpu *intel_gvt_create_vgpu(struct intel_gvt *gvt,
+					 struct intel_vgpu_creation_params *
+					 param);
+
+void intel_gvt_destroy_vgpu(struct intel_vgpu *vgpu);
+
+/* validating GM functions */
+#define vgpu_gmadr_is_aperture(vgpu, gmadr) \
+	((gmadr >= vgpu_aperture_gmadr_base(vgpu)) && \
+	 (gmadr <= vgpu_aperture_gmadr_end(vgpu)))
+
+#define vgpu_gmadr_is_hidden(vgpu, gmadr) \
+	((gmadr >= vgpu_hidden_gmadr_base(vgpu)) && \
+	 (gmadr <= vgpu_hidden_gmadr_end(vgpu)))
+
+#define vgpu_gmadr_is_valid(vgpu, gmadr) \
+	 ((vgpu_gmadr_is_aperture(vgpu, gmadr) || \
+	  (vgpu_gmadr_is_hidden(vgpu, gmadr))))
+
+#define gvt_gmadr_is_aperture(gvt, gmadr) \
+	 ((gmadr >= gvt_aperture_gmadr_base(gvt)) && \
+	  (gmadr <= gvt_aperture_gmadr_end(gvt)))
+
+#define gvt_gmadr_is_hidden(gvt, gmadr) \
+	  ((gmadr >= gvt_hidden_gmadr_base(gvt)) && \
+	   (gmadr <= gvt_hidden_gmadr_end(gvt)))
+
+#define gvt_gmadr_is_valid(gvt, gmadr) \
+	  (gvt_gmadr_is_aperture(gvt, gmadr) || \
+	    gvt_gmadr_is_hidden(gvt, gmadr))
+
+bool intel_gvt_ggtt_validate_range(struct intel_vgpu *vgpu, u64 addr, u32 size);
+int intel_gvt_ggtt_gmadr_g2h(struct intel_vgpu *vgpu, u64 g_addr, u64 *h_addr);
+int intel_gvt_ggtt_gmadr_h2g(struct intel_vgpu *vgpu, u64 h_addr, u64 *g_addr);
+int intel_gvt_ggtt_index_g2h(struct intel_vgpu *vgpu, unsigned long g_index,
+			     unsigned long *h_index);
+int intel_gvt_ggtt_h2g_index(struct intel_vgpu *vgpu, unsigned long h_index,
+			     unsigned long *g_index);
+
+int intel_vgpu_emulate_cfg_read(void *__vgpu, unsigned int offset,
+		void *p_data, unsigned int bytes);
+
+int intel_vgpu_emulate_cfg_write(void *__vgpu, unsigned int offset,
+		void *p_data, unsigned int bytes);
+
+void intel_gvt_clean_opregion(struct intel_gvt *gvt);
+int intel_gvt_init_opregion(struct intel_gvt *gvt);
+
+void intel_vgpu_clean_opregion(struct intel_vgpu *vgpu);
+int intel_vgpu_init_opregion(struct intel_vgpu *vgpu, u32 gpa);
+
+int intel_vgpu_emulate_opregion_request(struct intel_vgpu *vgpu, u32 swsci);
+
 #include "mpt.h"
 
 #endif
diff --git a/drivers/gpu/drm/i915/gvt/handlers.c b/drivers/gpu/drm/i915/gvt/handlers.c
new file mode 100644
index 000000000000..3e74fb3d4aa9
--- /dev/null
+++ b/drivers/gpu/drm/i915/gvt/handlers.c
@@ -0,0 +1,2797 @@
+/*
+ * Copyright(c) 2011-2016 Intel Corporation. All rights reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ * Authors:
+ *    Kevin Tian <kevin.tian@intel.com>
+ *    Eddie Dong <eddie.dong@intel.com>
+ *    Zhiyuan Lv <zhiyuan.lv@intel.com>
+ *
+ * Contributors:
+ *    Min He <min.he@intel.com>
+ *    Tina Zhang <tina.zhang@intel.com>
+ *    Pei Zhang <pei.zhang@intel.com>
+ *    Niu Bing <bing.niu@intel.com>
+ *    Ping Gao <ping.a.gao@intel.com>
+ *    Zhi Wang <zhi.a.wang@intel.com>
+ *
+
+ */
+
+#include "i915_drv.h"
+#include "gvt.h"
+#include "i915_pvinfo.h"
+
+/* XXX FIXME i915 has changed PP_XXX definition */
+#define PCH_PP_STATUS  _MMIO(0xc7200)
+#define PCH_PP_CONTROL _MMIO(0xc7204)
+#define PCH_PP_ON_DELAYS _MMIO(0xc7208)
+#define PCH_PP_OFF_DELAYS _MMIO(0xc720c)
+#define PCH_PP_DIVISOR _MMIO(0xc7210)
+
+/* Register contains RO bits */
+#define F_RO		(1 << 0)
+/* Register contains graphics address */
+#define F_GMADR		(1 << 1)
+/* Mode mask registers with high 16 bits as the mask bits */
+#define F_MODE_MASK	(1 << 2)
+/* This reg can be accessed by GPU commands */
+#define F_CMD_ACCESS	(1 << 3)
+/* This reg has been accessed by a VM */
+#define F_ACCESSED	(1 << 4)
+/* This reg has been accessed through GPU commands */
+#define F_CMD_ACCESSED	(1 << 5)
+/* This reg could be accessed by unaligned address */
+#define F_UNALIGN	(1 << 6)
+
+unsigned long intel_gvt_get_device_type(struct intel_gvt *gvt)
+{
+	if (IS_BROADWELL(gvt->dev_priv))
+		return D_BDW;
+	else if (IS_SKYLAKE(gvt->dev_priv))
+		return D_SKL;
+
+	return 0;
+}
+
+bool intel_gvt_match_device(struct intel_gvt *gvt,
+		unsigned long device)
+{
+	return intel_gvt_get_device_type(gvt) & device;
+}
+
+static void read_vreg(struct intel_vgpu *vgpu, unsigned int offset,
+	void *p_data, unsigned int bytes)
+{
+	memcpy(p_data, &vgpu_vreg(vgpu, offset), bytes);
+}
+
+static void write_vreg(struct intel_vgpu *vgpu, unsigned int offset,
+	void *p_data, unsigned int bytes)
+{
+	memcpy(&vgpu_vreg(vgpu, offset), p_data, bytes);
+}
+
+static int new_mmio_info(struct intel_gvt *gvt,
+		u32 offset, u32 flags, u32 size,
+		u32 addr_mask, u32 ro_mask, u32 device,
+		void *read, void *write)
+{
+	struct intel_gvt_mmio_info *info, *p;
+	u32 start, end, i;
+
+	if (!intel_gvt_match_device(gvt, device))
+		return 0;
+
+	if (WARN_ON(!IS_ALIGNED(offset, 4)))
+		return -EINVAL;
+
+	start = offset;
+	end = offset + size;
+
+	for (i = start; i < end; i += 4) {
+		info = kzalloc(sizeof(*info), GFP_KERNEL);
+		if (!info)
+			return -ENOMEM;
+
+		info->offset = i;
+		p = intel_gvt_find_mmio_info(gvt, info->offset);
+		if (p)
+			gvt_err("dup mmio definition offset %x\n",
+				info->offset);
+		info->size = size;
+		info->length = (i + 4) < end ? 4 : (end - i);
+		info->addr_mask = addr_mask;
+		info->device = device;
+		info->read = read ? read : intel_vgpu_default_mmio_read;
+		info->write = write ? write : intel_vgpu_default_mmio_write;
+		gvt->mmio.mmio_attribute[info->offset / 4] = flags;
+		INIT_HLIST_NODE(&info->node);
+		hash_add(gvt->mmio.mmio_info_table, &info->node, info->offset);
+	}
+	return 0;
+}
+
+static int render_mmio_to_ring_id(struct intel_gvt *gvt, unsigned int reg)
+{
+	enum intel_engine_id id;
+	struct intel_engine_cs *engine;
+
+	reg &= ~GENMASK(11, 0);
+	for_each_engine(engine, gvt->dev_priv, id) {
+		if (engine->mmio_base == reg)
+			return id;
+	}
+	return -1;
+}
+
+#define offset_to_fence_num(offset) \
+	((offset - i915_mmio_reg_offset(FENCE_REG_GEN6_LO(0))) >> 3)
+
+#define fence_num_to_offset(num) \
+	(num * 8 + i915_mmio_reg_offset(FENCE_REG_GEN6_LO(0)))
+
+static int sanitize_fence_mmio_access(struct intel_vgpu *vgpu,
+		unsigned int fence_num, void *p_data, unsigned int bytes)
+{
+	if (fence_num >= vgpu_fence_sz(vgpu)) {
+		gvt_err("vgpu%d: found oob fence register access\n",
+				vgpu->id);
+		gvt_err("vgpu%d: total fence num %d access fence num %d\n",
+				vgpu->id, vgpu_fence_sz(vgpu), fence_num);
+		memset(p_data, 0, bytes);
+	}
+	return 0;
+}
+
+static int fence_mmio_read(struct intel_vgpu *vgpu, unsigned int off,
+		void *p_data, unsigned int bytes)
+{
+	int ret;
+
+	ret = sanitize_fence_mmio_access(vgpu, offset_to_fence_num(off),
+			p_data, bytes);
+	if (ret)
+		return ret;
+	read_vreg(vgpu, off, p_data, bytes);
+	return 0;
+}
+
+static int fence_mmio_write(struct intel_vgpu *vgpu, unsigned int off,
+		void *p_data, unsigned int bytes)
+{
+	unsigned int fence_num = offset_to_fence_num(off);
+	int ret;
+
+	ret = sanitize_fence_mmio_access(vgpu, fence_num, p_data, bytes);
+	if (ret)
+		return ret;
+	write_vreg(vgpu, off, p_data, bytes);
+
+	intel_vgpu_write_fence(vgpu, fence_num,
+			vgpu_vreg64(vgpu, fence_num_to_offset(fence_num)));
+	return 0;
+}
+
+#define CALC_MODE_MASK_REG(old, new) \
+	(((new) & GENMASK(31, 16)) \
+	 | ((((old) & GENMASK(15, 0)) & ~((new) >> 16)) \
+	 | ((new) & ((new) >> 16))))
+
+static int mul_force_wake_write(struct intel_vgpu *vgpu,
+		unsigned int offset, void *p_data, unsigned int bytes)
+{
+	u32 old, new;
+	uint32_t ack_reg_offset;
+
+	old = vgpu_vreg(vgpu, offset);
+	new = CALC_MODE_MASK_REG(old, *(u32 *)p_data);
+
+	if (IS_SKYLAKE(vgpu->gvt->dev_priv)) {
+		switch (offset) {
+		case FORCEWAKE_RENDER_GEN9_REG:
+			ack_reg_offset = FORCEWAKE_ACK_RENDER_GEN9_REG;
+			break;
+		case FORCEWAKE_BLITTER_GEN9_REG:
+			ack_reg_offset = FORCEWAKE_ACK_BLITTER_GEN9_REG;
+			break;
+		case FORCEWAKE_MEDIA_GEN9_REG:
+			ack_reg_offset = FORCEWAKE_ACK_MEDIA_GEN9_REG;
+			break;
+		default:
+			/*should not hit here*/
+			gvt_err("invalid forcewake offset 0x%x\n", offset);
+			return 1;
+		}
+	} else {
+		ack_reg_offset = FORCEWAKE_ACK_HSW_REG;
+	}
+
+	vgpu_vreg(vgpu, offset) = new;
+	vgpu_vreg(vgpu, ack_reg_offset) = (new & GENMASK(15, 0));
+	return 0;
+}
+
+static int handle_device_reset(struct intel_vgpu *vgpu, unsigned int offset,
+		void *p_data, unsigned int bytes, unsigned long bitmap)
+{
+	struct intel_gvt_workload_scheduler *scheduler =
+		&vgpu->gvt->scheduler;
+
+	vgpu->resetting = true;
+
+	intel_vgpu_stop_schedule(vgpu);
+	if (scheduler->current_vgpu == vgpu) {
+		mutex_unlock(&vgpu->gvt->lock);
+		intel_gvt_wait_vgpu_idle(vgpu);
+		mutex_lock(&vgpu->gvt->lock);
+	}
+
+	intel_vgpu_reset_execlist(vgpu, bitmap);
+
+	vgpu->resetting = false;
+
+	return 0;
+}
+
+static int gdrst_mmio_write(struct intel_vgpu *vgpu, unsigned int offset,
+		void *p_data, unsigned int bytes)
+{
+	u32 data;
+	u64 bitmap = 0;
+
+	data = vgpu_vreg(vgpu, offset);
+
+	if (data & GEN6_GRDOM_FULL) {
+		gvt_dbg_mmio("vgpu%d: request full GPU reset\n", vgpu->id);
+		bitmap = 0xff;
+	}
+	if (data & GEN6_GRDOM_RENDER) {
+		gvt_dbg_mmio("vgpu%d: request RCS reset\n", vgpu->id);
+		bitmap |= (1 << RCS);
+	}
+	if (data & GEN6_GRDOM_MEDIA) {
+		gvt_dbg_mmio("vgpu%d: request VCS reset\n", vgpu->id);
+		bitmap |= (1 << VCS);
+	}
+	if (data & GEN6_GRDOM_BLT) {
+		gvt_dbg_mmio("vgpu%d: request BCS Reset\n", vgpu->id);
+		bitmap |= (1 << BCS);
+	}
+	if (data & GEN6_GRDOM_VECS) {
+		gvt_dbg_mmio("vgpu%d: request VECS Reset\n", vgpu->id);
+		bitmap |= (1 << VECS);
+	}
+	if (data & GEN8_GRDOM_MEDIA2) {
+		gvt_dbg_mmio("vgpu%d: request VCS2 Reset\n", vgpu->id);
+		if (HAS_BSD2(vgpu->gvt->dev_priv))
+			bitmap |= (1 << VCS2);
+	}
+	return handle_device_reset(vgpu, offset, p_data, bytes, bitmap);
+}
+
+static int gmbus_mmio_read(struct intel_vgpu *vgpu, unsigned int offset,
+		void *p_data, unsigned int bytes)
+{
+	return intel_gvt_i2c_handle_gmbus_read(vgpu, offset, p_data, bytes);
+}
+
+static int gmbus_mmio_write(struct intel_vgpu *vgpu, unsigned int offset,
+		void *p_data, unsigned int bytes)
+{
+	return intel_gvt_i2c_handle_gmbus_write(vgpu, offset, p_data, bytes);
+}
+
+static int pch_pp_control_mmio_write(struct intel_vgpu *vgpu,
+		unsigned int offset, void *p_data, unsigned int bytes)
+{
+	write_vreg(vgpu, offset, p_data, bytes);
+
+	if (vgpu_vreg(vgpu, offset) & PANEL_POWER_ON) {
+		vgpu_vreg(vgpu, PCH_PP_STATUS) |= PP_ON;
+		vgpu_vreg(vgpu, PCH_PP_STATUS) |= PP_SEQUENCE_STATE_ON_IDLE;
+		vgpu_vreg(vgpu, PCH_PP_STATUS) &= ~PP_SEQUENCE_POWER_DOWN;
+		vgpu_vreg(vgpu, PCH_PP_STATUS) &= ~PP_CYCLE_DELAY_ACTIVE;
+
+	} else
+		vgpu_vreg(vgpu, PCH_PP_STATUS) &=
+			~(PP_ON | PP_SEQUENCE_POWER_DOWN
+					| PP_CYCLE_DELAY_ACTIVE);
+	return 0;
+}
+
+static int transconf_mmio_write(struct intel_vgpu *vgpu,
+		unsigned int offset, void *p_data, unsigned int bytes)
+{
+	write_vreg(vgpu, offset, p_data, bytes);
+
+	if (vgpu_vreg(vgpu, offset) & TRANS_ENABLE)
+		vgpu_vreg(vgpu, offset) |= TRANS_STATE_ENABLE;
+	else
+		vgpu_vreg(vgpu, offset) &= ~TRANS_STATE_ENABLE;
+	return 0;
+}
+
+static int lcpll_ctl_mmio_write(struct intel_vgpu *vgpu, unsigned int offset,
+		void *p_data, unsigned int bytes)
+{
+	write_vreg(vgpu, offset, p_data, bytes);
+
+	if (vgpu_vreg(vgpu, offset) & LCPLL_PLL_DISABLE)
+		vgpu_vreg(vgpu, offset) &= ~LCPLL_PLL_LOCK;
+	else
+		vgpu_vreg(vgpu, offset) |= LCPLL_PLL_LOCK;
+
+	if (vgpu_vreg(vgpu, offset) & LCPLL_CD_SOURCE_FCLK)
+		vgpu_vreg(vgpu, offset) |= LCPLL_CD_SOURCE_FCLK_DONE;
+	else
+		vgpu_vreg(vgpu, offset) &= ~LCPLL_CD_SOURCE_FCLK_DONE;
+
+	return 0;
+}
+
+static int dpy_reg_mmio_read(struct intel_vgpu *vgpu, unsigned int offset,
+		void *p_data, unsigned int bytes)
+{
+	*(u32 *)p_data = (1 << 17);
+	return 0;
+}
+
+static int dpy_reg_mmio_read_2(struct intel_vgpu *vgpu, unsigned int offset,
+		void *p_data, unsigned int bytes)
+{
+	*(u32 *)p_data = 3;
+	return 0;
+}
+
+static int dpy_reg_mmio_read_3(struct intel_vgpu *vgpu, unsigned int offset,
+		void *p_data, unsigned int bytes)
+{
+	*(u32 *)p_data = (0x2f << 16);
+	return 0;
+}
+
+static int pipeconf_mmio_write(struct intel_vgpu *vgpu, unsigned int offset,
+		void *p_data, unsigned int bytes)
+{
+	u32 data;
+
+	write_vreg(vgpu, offset, p_data, bytes);
+	data = vgpu_vreg(vgpu, offset);
+
+	if (data & PIPECONF_ENABLE)
+		vgpu_vreg(vgpu, offset) |= I965_PIPECONF_ACTIVE;
+	else
+		vgpu_vreg(vgpu, offset) &= ~I965_PIPECONF_ACTIVE;
+	intel_gvt_check_vblank_emulation(vgpu->gvt);
+	return 0;
+}
+
+static int ddi_buf_ctl_mmio_write(struct intel_vgpu *vgpu, unsigned int offset,
+		void *p_data, unsigned int bytes)
+{
+	write_vreg(vgpu, offset, p_data, bytes);
+
+	if (vgpu_vreg(vgpu, offset) & DDI_BUF_CTL_ENABLE) {
+		vgpu_vreg(vgpu, offset) &= ~DDI_BUF_IS_IDLE;
+	} else {
+		vgpu_vreg(vgpu, offset) |= DDI_BUF_IS_IDLE;
+		if (offset == i915_mmio_reg_offset(DDI_BUF_CTL(PORT_E)))
+			vgpu_vreg(vgpu, DP_TP_STATUS(PORT_E))
+				&= ~DP_TP_STATUS_AUTOTRAIN_DONE;
+	}
+	return 0;
+}
+
+static int fdi_rx_iir_mmio_write(struct intel_vgpu *vgpu,
+		unsigned int offset, void *p_data, unsigned int bytes)
+{
+	vgpu_vreg(vgpu, offset) &= ~*(u32 *)p_data;
+	return 0;
+}
+
+#define FDI_LINK_TRAIN_PATTERN1         0
+#define FDI_LINK_TRAIN_PATTERN2         1
+
+static int fdi_auto_training_started(struct intel_vgpu *vgpu)
+{
+	u32 ddi_buf_ctl = vgpu_vreg(vgpu, DDI_BUF_CTL(PORT_E));
+	u32 rx_ctl = vgpu_vreg(vgpu, _FDI_RXA_CTL);
+	u32 tx_ctl = vgpu_vreg(vgpu, DP_TP_CTL(PORT_E));
+
+	if ((ddi_buf_ctl & DDI_BUF_CTL_ENABLE) &&
+			(rx_ctl & FDI_RX_ENABLE) &&
+			(rx_ctl & FDI_AUTO_TRAINING) &&
+			(tx_ctl & DP_TP_CTL_ENABLE) &&
+			(tx_ctl & DP_TP_CTL_FDI_AUTOTRAIN))
+		return 1;
+	else
+		return 0;
+}
+
+static int check_fdi_rx_train_status(struct intel_vgpu *vgpu,
+		enum pipe pipe, unsigned int train_pattern)
+{
+	i915_reg_t fdi_rx_imr, fdi_tx_ctl, fdi_rx_ctl;
+	unsigned int fdi_rx_check_bits, fdi_tx_check_bits;
+	unsigned int fdi_rx_train_bits, fdi_tx_train_bits;
+	unsigned int fdi_iir_check_bits;
+
+	fdi_rx_imr = FDI_RX_IMR(pipe);
+	fdi_tx_ctl = FDI_TX_CTL(pipe);
+	fdi_rx_ctl = FDI_RX_CTL(pipe);
+
+	if (train_pattern == FDI_LINK_TRAIN_PATTERN1) {
+		fdi_rx_train_bits = FDI_LINK_TRAIN_PATTERN_1_CPT;
+		fdi_tx_train_bits = FDI_LINK_TRAIN_PATTERN_1;
+		fdi_iir_check_bits = FDI_RX_BIT_LOCK;
+	} else if (train_pattern == FDI_LINK_TRAIN_PATTERN2) {
+		fdi_rx_train_bits = FDI_LINK_TRAIN_PATTERN_2_CPT;
+		fdi_tx_train_bits = FDI_LINK_TRAIN_PATTERN_2;
+		fdi_iir_check_bits = FDI_RX_SYMBOL_LOCK;
+	} else {
+		gvt_err("Invalid train pattern %d\n", train_pattern);
+		return -EINVAL;
+	}
+
+	fdi_rx_check_bits = FDI_RX_ENABLE | fdi_rx_train_bits;
+	fdi_tx_check_bits = FDI_TX_ENABLE | fdi_tx_train_bits;
+
+	/* If imr bit has been masked */
+	if (vgpu_vreg(vgpu, fdi_rx_imr) & fdi_iir_check_bits)
+		return 0;
+
+	if (((vgpu_vreg(vgpu, fdi_tx_ctl) & fdi_tx_check_bits)
+			== fdi_tx_check_bits)
+		&& ((vgpu_vreg(vgpu, fdi_rx_ctl) & fdi_rx_check_bits)
+			== fdi_rx_check_bits))
+		return 1;
+	else
+		return 0;
+}
+
+#define INVALID_INDEX (~0U)
+
+static unsigned int calc_index(unsigned int offset, unsigned int start,
+	unsigned int next, unsigned int end, i915_reg_t i915_end)
+{
+	unsigned int range = next - start;
+
+	if (!end)
+		end = i915_mmio_reg_offset(i915_end);
+	if (offset < start || offset > end)
+		return INVALID_INDEX;
+	offset -= start;
+	return offset / range;
+}
+
+#define FDI_RX_CTL_TO_PIPE(offset) \
+	calc_index(offset, _FDI_RXA_CTL, _FDI_RXB_CTL, 0, FDI_RX_CTL(PIPE_C))
+
+#define FDI_TX_CTL_TO_PIPE(offset) \
+	calc_index(offset, _FDI_TXA_CTL, _FDI_TXB_CTL, 0, FDI_TX_CTL(PIPE_C))
+
+#define FDI_RX_IMR_TO_PIPE(offset) \
+	calc_index(offset, _FDI_RXA_IMR, _FDI_RXB_IMR, 0, FDI_RX_IMR(PIPE_C))
+
+static int update_fdi_rx_iir_status(struct intel_vgpu *vgpu,
+		unsigned int offset, void *p_data, unsigned int bytes)
+{
+	i915_reg_t fdi_rx_iir;
+	unsigned int index;
+	int ret;
+
+	if (FDI_RX_CTL_TO_PIPE(offset) != INVALID_INDEX)
+		index = FDI_RX_CTL_TO_PIPE(offset);
+	else if (FDI_TX_CTL_TO_PIPE(offset) != INVALID_INDEX)
+		index = FDI_TX_CTL_TO_PIPE(offset);
+	else if (FDI_RX_IMR_TO_PIPE(offset) != INVALID_INDEX)
+		index = FDI_RX_IMR_TO_PIPE(offset);
+	else {
+		gvt_err("Unsupport registers %x\n", offset);
+		return -EINVAL;
+	}
+
+	write_vreg(vgpu, offset, p_data, bytes);
+
+	fdi_rx_iir = FDI_RX_IIR(index);
+
+	ret = check_fdi_rx_train_status(vgpu, index, FDI_LINK_TRAIN_PATTERN1);
+	if (ret < 0)
+		return ret;
+	if (ret)
+		vgpu_vreg(vgpu, fdi_rx_iir) |= FDI_RX_BIT_LOCK;
+
+	ret = check_fdi_rx_train_status(vgpu, index, FDI_LINK_TRAIN_PATTERN2);
+	if (ret < 0)
+		return ret;
+	if (ret)
+		vgpu_vreg(vgpu, fdi_rx_iir) |= FDI_RX_SYMBOL_LOCK;
+
+	if (offset == _FDI_RXA_CTL)
+		if (fdi_auto_training_started(vgpu))
+			vgpu_vreg(vgpu, DP_TP_STATUS(PORT_E)) |=
+				DP_TP_STATUS_AUTOTRAIN_DONE;
+	return 0;
+}
+
+#define DP_TP_CTL_TO_PORT(offset) \
+	calc_index(offset, _DP_TP_CTL_A, _DP_TP_CTL_B, 0, DP_TP_CTL(PORT_E))
+
+static int dp_tp_ctl_mmio_write(struct intel_vgpu *vgpu, unsigned int offset,
+		void *p_data, unsigned int bytes)
+{
+	i915_reg_t status_reg;
+	unsigned int index;
+	u32 data;
+
+	write_vreg(vgpu, offset, p_data, bytes);
+
+	index = DP_TP_CTL_TO_PORT(offset);
+	data = (vgpu_vreg(vgpu, offset) & GENMASK(10, 8)) >> 8;
+	if (data == 0x2) {
+		status_reg = DP_TP_STATUS(index);
+		vgpu_vreg(vgpu, status_reg) |= (1 << 25);
+	}
+	return 0;
+}
+
+static int dp_tp_status_mmio_write(struct intel_vgpu *vgpu,
+		unsigned int offset, void *p_data, unsigned int bytes)
+{
+	u32 reg_val;
+	u32 sticky_mask;
+
+	reg_val = *((u32 *)p_data);
+	sticky_mask = GENMASK(27, 26) | (1 << 24);
+
+	vgpu_vreg(vgpu, offset) = (reg_val & ~sticky_mask) |
+		(vgpu_vreg(vgpu, offset) & sticky_mask);
+	vgpu_vreg(vgpu, offset) &= ~(reg_val & sticky_mask);
+	return 0;
+}
+
+static int pch_adpa_mmio_write(struct intel_vgpu *vgpu,
+		unsigned int offset, void *p_data, unsigned int bytes)
+{
+	u32 data;
+
+	write_vreg(vgpu, offset, p_data, bytes);
+	data = vgpu_vreg(vgpu, offset);
+
+	if (data & ADPA_CRT_HOTPLUG_FORCE_TRIGGER)
+		vgpu_vreg(vgpu, offset) &= ~ADPA_CRT_HOTPLUG_FORCE_TRIGGER;
+	return 0;
+}
+
+static int south_chicken2_mmio_write(struct intel_vgpu *vgpu,
+		unsigned int offset, void *p_data, unsigned int bytes)
+{
+	u32 data;
+
+	write_vreg(vgpu, offset, p_data, bytes);
+	data = vgpu_vreg(vgpu, offset);
+
+	if (data & FDI_MPHY_IOSFSB_RESET_CTL)
+		vgpu_vreg(vgpu, offset) |= FDI_MPHY_IOSFSB_RESET_STATUS;
+	else
+		vgpu_vreg(vgpu, offset) &= ~FDI_MPHY_IOSFSB_RESET_STATUS;
+	return 0;
+}
+
+#define DSPSURF_TO_PIPE(offset) \
+	calc_index(offset, _DSPASURF, _DSPBSURF, 0, DSPSURF(PIPE_C))
+
+static int pri_surf_mmio_write(struct intel_vgpu *vgpu, unsigned int offset,
+		void *p_data, unsigned int bytes)
+{
+	struct drm_i915_private *dev_priv = vgpu->gvt->dev_priv;
+	unsigned int index = DSPSURF_TO_PIPE(offset);
+	i915_reg_t surflive_reg = DSPSURFLIVE(index);
+	int flip_event[] = {
+		[PIPE_A] = PRIMARY_A_FLIP_DONE,
+		[PIPE_B] = PRIMARY_B_FLIP_DONE,
+		[PIPE_C] = PRIMARY_C_FLIP_DONE,
+	};
+
+	write_vreg(vgpu, offset, p_data, bytes);
+	vgpu_vreg(vgpu, surflive_reg) = vgpu_vreg(vgpu, offset);
+
+	set_bit(flip_event[index], vgpu->irq.flip_done_event[index]);
+	return 0;
+}
+
+#define SPRSURF_TO_PIPE(offset) \
+	calc_index(offset, _SPRA_SURF, _SPRB_SURF, 0, SPRSURF(PIPE_C))
+
+static int spr_surf_mmio_write(struct intel_vgpu *vgpu, unsigned int offset,
+		void *p_data, unsigned int bytes)
+{
+	unsigned int index = SPRSURF_TO_PIPE(offset);
+	i915_reg_t surflive_reg = SPRSURFLIVE(index);
+	int flip_event[] = {
+		[PIPE_A] = SPRITE_A_FLIP_DONE,
+		[PIPE_B] = SPRITE_B_FLIP_DONE,
+		[PIPE_C] = SPRITE_C_FLIP_DONE,
+	};
+
+	write_vreg(vgpu, offset, p_data, bytes);
+	vgpu_vreg(vgpu, surflive_reg) = vgpu_vreg(vgpu, offset);
+
+	set_bit(flip_event[index], vgpu->irq.flip_done_event[index]);
+	return 0;
+}
+
+static int trigger_aux_channel_interrupt(struct intel_vgpu *vgpu,
+		unsigned int reg)
+{
+	struct drm_i915_private *dev_priv = vgpu->gvt->dev_priv;
+	enum intel_gvt_event_type event;
+
+	if (reg == _DPA_AUX_CH_CTL)
+		event = AUX_CHANNEL_A;
+	else if (reg == _PCH_DPB_AUX_CH_CTL || reg == _DPB_AUX_CH_CTL)
+		event = AUX_CHANNEL_B;
+	else if (reg == _PCH_DPC_AUX_CH_CTL || reg == _DPC_AUX_CH_CTL)
+		event = AUX_CHANNEL_C;
+	else if (reg == _PCH_DPD_AUX_CH_CTL || reg == _DPD_AUX_CH_CTL)
+		event = AUX_CHANNEL_D;
+	else {
+		WARN_ON(true);
+		return -EINVAL;
+	}
+
+	intel_vgpu_trigger_virtual_event(vgpu, event);
+	return 0;
+}
+
+static int dp_aux_ch_ctl_trans_done(struct intel_vgpu *vgpu, u32 value,
+		unsigned int reg, int len, bool data_valid)
+{
+	/* mark transaction done */
+	value |= DP_AUX_CH_CTL_DONE;
+	value &= ~DP_AUX_CH_CTL_SEND_BUSY;
+	value &= ~DP_AUX_CH_CTL_RECEIVE_ERROR;
+
+	if (data_valid)
+		value &= ~DP_AUX_CH_CTL_TIME_OUT_ERROR;
+	else
+		value |= DP_AUX_CH_CTL_TIME_OUT_ERROR;
+
+	/* message size */
+	value &= ~(0xf << 20);
+	value |= (len << 20);
+	vgpu_vreg(vgpu, reg) = value;
+
+	if (value & DP_AUX_CH_CTL_INTERRUPT)
+		return trigger_aux_channel_interrupt(vgpu, reg);
+	return 0;
+}
+
+static void dp_aux_ch_ctl_link_training(struct intel_vgpu_dpcd_data *dpcd,
+		uint8_t t)
+{
+	if ((t & DPCD_TRAINING_PATTERN_SET_MASK) == DPCD_TRAINING_PATTERN_1) {
+		/* training pattern 1 for CR */
+		/* set LANE0_CR_DONE, LANE1_CR_DONE */
+		dpcd->data[DPCD_LANE0_1_STATUS] |= DPCD_LANES_CR_DONE;
+		/* set LANE2_CR_DONE, LANE3_CR_DONE */
+		dpcd->data[DPCD_LANE2_3_STATUS] |= DPCD_LANES_CR_DONE;
+	} else if ((t & DPCD_TRAINING_PATTERN_SET_MASK) ==
+			DPCD_TRAINING_PATTERN_2) {
+		/* training pattern 2 for EQ */
+		/* Set CHANNEL_EQ_DONE and  SYMBOL_LOCKED for Lane0_1 */
+		dpcd->data[DPCD_LANE0_1_STATUS] |= DPCD_LANES_EQ_DONE;
+		dpcd->data[DPCD_LANE0_1_STATUS] |= DPCD_SYMBOL_LOCKED;
+		/* Set CHANNEL_EQ_DONE and  SYMBOL_LOCKED for Lane2_3 */
+		dpcd->data[DPCD_LANE2_3_STATUS] |= DPCD_LANES_EQ_DONE;
+		dpcd->data[DPCD_LANE2_3_STATUS] |= DPCD_SYMBOL_LOCKED;
+		/* set INTERLANE_ALIGN_DONE */
+		dpcd->data[DPCD_LANE_ALIGN_STATUS_UPDATED] |=
+			DPCD_INTERLANE_ALIGN_DONE;
+	} else if ((t & DPCD_TRAINING_PATTERN_SET_MASK) ==
+			DPCD_LINK_TRAINING_DISABLED) {
+		/* finish link training */
+		/* set sink status as synchronized */
+		dpcd->data[DPCD_SINK_STATUS] = DPCD_SINK_IN_SYNC;
+	}
+}
+
+#define _REG_HSW_DP_AUX_CH_CTL(dp) \
+	((dp) ? (_PCH_DPB_AUX_CH_CTL + ((dp)-1)*0x100) : 0x64010)
+
+#define _REG_SKL_DP_AUX_CH_CTL(dp) (0x64010 + (dp) * 0x100)
+
+#define OFFSET_TO_DP_AUX_PORT(offset) (((offset) & 0xF00) >> 8)
+
+#define dpy_is_valid_port(port)	\
+		(((port) >= PORT_A) && ((port) < I915_MAX_PORTS))
+
+static int dp_aux_ch_ctl_mmio_write(struct intel_vgpu *vgpu,
+		unsigned int offset, void *p_data, unsigned int bytes)
+{
+	struct intel_vgpu_display *display = &vgpu->display;
+	int msg, addr, ctrl, op, len;
+	int port_index = OFFSET_TO_DP_AUX_PORT(offset);
+	struct intel_vgpu_dpcd_data *dpcd = NULL;
+	struct intel_vgpu_port *port = NULL;
+	u32 data;
+
+	if (!dpy_is_valid_port(port_index)) {
+		gvt_err("GVT(%d): Unsupported DP port access!\n", vgpu->id);
+		return 0;
+	}
+
+	write_vreg(vgpu, offset, p_data, bytes);
+	data = vgpu_vreg(vgpu, offset);
+
+	if (IS_SKYLAKE(vgpu->gvt->dev_priv) &&
+	    offset != _REG_SKL_DP_AUX_CH_CTL(port_index)) {
+		/* SKL DPB/C/D aux ctl register changed */
+		return 0;
+	} else if (IS_BROADWELL(vgpu->gvt->dev_priv) &&
+		   offset != _REG_HSW_DP_AUX_CH_CTL(port_index)) {
+		/* write to the data registers */
+		return 0;
+	}
+
+	if (!(data & DP_AUX_CH_CTL_SEND_BUSY)) {
+		/* just want to clear the sticky bits */
+		vgpu_vreg(vgpu, offset) = 0;
+		return 0;
+	}
+
+	port = &display->ports[port_index];
+	dpcd = port->dpcd;
+
+	/* read out message from DATA1 register */
+	msg = vgpu_vreg(vgpu, offset + 4);
+	addr = (msg >> 8) & 0xffff;
+	ctrl = (msg >> 24) & 0xff;
+	len = msg & 0xff;
+	op = ctrl >> 4;
+
+	if (op == GVT_AUX_NATIVE_WRITE) {
+		int t;
+		uint8_t buf[16];
+
+		if ((addr + len + 1) >= DPCD_SIZE) {
+			/*
+			 * Write request exceeds what we supported,
+			 * DCPD spec: When a Source Device is writing a DPCD
+			 * address not supported by the Sink Device, the Sink
+			 * Device shall reply with AUX NACK and “M” equal to
+			 * zero.
+			 */
+
+			/* NAK the write */
+			vgpu_vreg(vgpu, offset + 4) = AUX_NATIVE_REPLY_NAK;
+			dp_aux_ch_ctl_trans_done(vgpu, data, offset, 2, true);
+			return 0;
+		}
+
+		/*
+		 * Write request format: (command + address) occupies
+		 * 3 bytes, followed by (len + 1) bytes of data.
+		 */
+		if (WARN_ON((len + 4) > AUX_BURST_SIZE))
+			return -EINVAL;
+
+		/* unpack data from vreg to buf */
+		for (t = 0; t < 4; t++) {
+			u32 r = vgpu_vreg(vgpu, offset + 8 + t * 4);
+
+			buf[t * 4] = (r >> 24) & 0xff;
+			buf[t * 4 + 1] = (r >> 16) & 0xff;
+			buf[t * 4 + 2] = (r >> 8) & 0xff;
+			buf[t * 4 + 3] = r & 0xff;
+		}
+
+		/* write to virtual DPCD */
+		if (dpcd && dpcd->data_valid) {
+			for (t = 0; t <= len; t++) {
+				int p = addr + t;
+
+				dpcd->data[p] = buf[t];
+				/* check for link training */
+				if (p == DPCD_TRAINING_PATTERN_SET)
+					dp_aux_ch_ctl_link_training(dpcd,
+							buf[t]);
+			}
+		}
+
+		/* ACK the write */
+		vgpu_vreg(vgpu, offset + 4) = 0;
+		dp_aux_ch_ctl_trans_done(vgpu, data, offset, 1,
+				dpcd && dpcd->data_valid);
+		return 0;
+	}
+
+	if (op == GVT_AUX_NATIVE_READ) {
+		int idx, i, ret = 0;
+
+		if ((addr + len + 1) >= DPCD_SIZE) {
+			/*
+			 * read request exceeds what we supported
+			 * DPCD spec: A Sink Device receiving a Native AUX CH
+			 * read request for an unsupported DPCD address must
+			 * reply with an AUX ACK and read data set equal to
+			 * zero instead of replying with AUX NACK.
+			 */
+
+			/* ACK the READ*/
+			vgpu_vreg(vgpu, offset + 4) = 0;
+			vgpu_vreg(vgpu, offset + 8) = 0;
+			vgpu_vreg(vgpu, offset + 12) = 0;
+			vgpu_vreg(vgpu, offset + 16) = 0;
+			vgpu_vreg(vgpu, offset + 20) = 0;
+
+			dp_aux_ch_ctl_trans_done(vgpu, data, offset, len + 2,
+					true);
+			return 0;
+		}
+
+		for (idx = 1; idx <= 5; idx++) {
+			/* clear the data registers */
+			vgpu_vreg(vgpu, offset + 4 * idx) = 0;
+		}
+
+		/*
+		 * Read reply format: ACK (1 byte) plus (len + 1) bytes of data.
+		 */
+		if (WARN_ON((len + 2) > AUX_BURST_SIZE))
+			return -EINVAL;
+
+		/* read from virtual DPCD to vreg */
+		/* first 4 bytes: [ACK][addr][addr+1][addr+2] */
+		if (dpcd && dpcd->data_valid) {
+			for (i = 1; i <= (len + 1); i++) {
+				int t;
+
+				t = dpcd->data[addr + i - 1];
+				t <<= (24 - 8 * (i % 4));
+				ret |= t;
+
+				if ((i % 4 == 3) || (i == (len + 1))) {
+					vgpu_vreg(vgpu, offset +
+							(i / 4 + 1) * 4) = ret;
+					ret = 0;
+				}
+			}
+		}
+		dp_aux_ch_ctl_trans_done(vgpu, data, offset, len + 2,
+				dpcd && dpcd->data_valid);
+		return 0;
+	}
+
+	/* i2c transaction starts */
+	intel_gvt_i2c_handle_aux_ch_write(vgpu, port_index, offset, p_data);
+
+	if (data & DP_AUX_CH_CTL_INTERRUPT)
+		trigger_aux_channel_interrupt(vgpu, offset);
+	return 0;
+}
+
+static int vga_control_mmio_write(struct intel_vgpu *vgpu, unsigned int offset,
+		void *p_data, unsigned int bytes)
+{
+	bool vga_disable;
+
+	write_vreg(vgpu, offset, p_data, bytes);
+	vga_disable = vgpu_vreg(vgpu, offset) & VGA_DISP_DISABLE;
+
+	gvt_dbg_core("vgpu%d: %s VGA mode\n", vgpu->id,
+			vga_disable ? "Disable" : "Enable");
+	return 0;
+}
+
+static u32 read_virtual_sbi_register(struct intel_vgpu *vgpu,
+		unsigned int sbi_offset)
+{
+	struct intel_vgpu_display *display = &vgpu->display;
+	int num = display->sbi.number;
+	int i;
+
+	for (i = 0; i < num; ++i)
+		if (display->sbi.registers[i].offset == sbi_offset)
+			break;
+
+	if (i == num)
+		return 0;
+
+	return display->sbi.registers[i].value;
+}
+
+static void write_virtual_sbi_register(struct intel_vgpu *vgpu,
+		unsigned int offset, u32 value)
+{
+	struct intel_vgpu_display *display = &vgpu->display;
+	int num = display->sbi.number;
+	int i;
+
+	for (i = 0; i < num; ++i) {
+		if (display->sbi.registers[i].offset == offset)
+			break;
+	}
+
+	if (i == num) {
+		if (num == SBI_REG_MAX) {
+			gvt_err("vgpu%d: SBI caching meets maximum limits\n",
+					vgpu->id);
+			return;
+		}
+		display->sbi.number++;
+	}
+
+	display->sbi.registers[i].offset = offset;
+	display->sbi.registers[i].value = value;
+}
+
+static int sbi_data_mmio_read(struct intel_vgpu *vgpu, unsigned int offset,
+		void *p_data, unsigned int bytes)
+{
+	if (((vgpu_vreg(vgpu, SBI_CTL_STAT) & SBI_OPCODE_MASK) >>
+				SBI_OPCODE_SHIFT) == SBI_CMD_CRRD) {
+		unsigned int sbi_offset = (vgpu_vreg(vgpu, SBI_ADDR) &
+				SBI_ADDR_OFFSET_MASK) >> SBI_ADDR_OFFSET_SHIFT;
+		vgpu_vreg(vgpu, offset) = read_virtual_sbi_register(vgpu,
+				sbi_offset);
+	}
+	read_vreg(vgpu, offset, p_data, bytes);
+	return 0;
+}
+
+static bool sbi_ctl_mmio_write(struct intel_vgpu *vgpu, unsigned int offset,
+		void *p_data, unsigned int bytes)
+{
+	u32 data;
+
+	write_vreg(vgpu, offset, p_data, bytes);
+	data = vgpu_vreg(vgpu, offset);
+
+	data &= ~(SBI_STAT_MASK << SBI_STAT_SHIFT);
+	data |= SBI_READY;
+
+	data &= ~(SBI_RESPONSE_MASK << SBI_RESPONSE_SHIFT);
+	data |= SBI_RESPONSE_SUCCESS;
+
+	vgpu_vreg(vgpu, offset) = data;
+
+	if (((vgpu_vreg(vgpu, SBI_CTL_STAT) & SBI_OPCODE_MASK) >>
+				SBI_OPCODE_SHIFT) == SBI_CMD_CRWR) {
+		unsigned int sbi_offset = (vgpu_vreg(vgpu, SBI_ADDR) &
+				SBI_ADDR_OFFSET_MASK) >> SBI_ADDR_OFFSET_SHIFT;
+
+		write_virtual_sbi_register(vgpu, sbi_offset,
+				vgpu_vreg(vgpu, SBI_DATA));
+	}
+	return 0;
+}
+
+#define _vgtif_reg(x) \
+	(VGT_PVINFO_PAGE + offsetof(struct vgt_if, x))
+
+static int pvinfo_mmio_read(struct intel_vgpu *vgpu, unsigned int offset,
+		void *p_data, unsigned int bytes)
+{
+	bool invalid_read = false;
+
+	read_vreg(vgpu, offset, p_data, bytes);
+
+	switch (offset) {
+	case _vgtif_reg(magic) ... _vgtif_reg(vgt_id):
+		if (offset + bytes > _vgtif_reg(vgt_id) + 4)
+			invalid_read = true;
+		break;
+	case _vgtif_reg(avail_rs.mappable_gmadr.base) ...
+		_vgtif_reg(avail_rs.fence_num):
+		if (offset + bytes >
+			_vgtif_reg(avail_rs.fence_num) + 4)
+			invalid_read = true;
+		break;
+	case 0x78010:	/* vgt_caps */
+	case 0x7881c:
+		break;
+	default:
+		invalid_read = true;
+		break;
+	}
+	if (invalid_read)
+		gvt_err("invalid pvinfo read: [%x:%x] = %x\n",
+				offset, bytes, *(u32 *)p_data);
+	return 0;
+}
+
+static int handle_g2v_notification(struct intel_vgpu *vgpu, int notification)
+{
+	int ret = 0;
+
+	switch (notification) {
+	case VGT_G2V_PPGTT_L3_PAGE_TABLE_CREATE:
+		ret = intel_vgpu_g2v_create_ppgtt_mm(vgpu, 3);
+		break;
+	case VGT_G2V_PPGTT_L3_PAGE_TABLE_DESTROY:
+		ret = intel_vgpu_g2v_destroy_ppgtt_mm(vgpu, 3);
+		break;
+	case VGT_G2V_PPGTT_L4_PAGE_TABLE_CREATE:
+		ret = intel_vgpu_g2v_create_ppgtt_mm(vgpu, 4);
+		break;
+	case VGT_G2V_PPGTT_L4_PAGE_TABLE_DESTROY:
+		ret = intel_vgpu_g2v_destroy_ppgtt_mm(vgpu, 4);
+		break;
+	case VGT_G2V_EXECLIST_CONTEXT_CREATE:
+	case VGT_G2V_EXECLIST_CONTEXT_DESTROY:
+	case 1:	/* Remove this in guest driver. */
+		break;
+	default:
+		gvt_err("Invalid PV notification %d\n", notification);
+	}
+	return ret;
+}
+
+static int send_display_ready_uevent(struct intel_vgpu *vgpu, int ready)
+{
+	struct drm_i915_private *dev_priv = vgpu->gvt->dev_priv;
+	struct kobject *kobj = &dev_priv->drm.primary->kdev->kobj;
+	char *env[3] = {NULL, NULL, NULL};
+	char vmid_str[20];
+	char display_ready_str[20];
+
+	snprintf(display_ready_str, 20, "GVT_DISPLAY_READY=%d\n", ready);
+	env[0] = display_ready_str;
+
+	snprintf(vmid_str, 20, "VMID=%d", vgpu->id);
+	env[1] = vmid_str;
+
+	return kobject_uevent_env(kobj, KOBJ_ADD, env);
+}
+
+static int pvinfo_mmio_write(struct intel_vgpu *vgpu, unsigned int offset,
+		void *p_data, unsigned int bytes)
+{
+	u32 data;
+	int ret;
+
+	write_vreg(vgpu, offset, p_data, bytes);
+	data = vgpu_vreg(vgpu, offset);
+
+	switch (offset) {
+	case _vgtif_reg(display_ready):
+		send_display_ready_uevent(vgpu, data ? 1 : 0);
+		break;
+	case _vgtif_reg(g2v_notify):
+		ret = handle_g2v_notification(vgpu, data);
+		break;
+	/* add xhot and yhot to handled list to avoid error log */
+	case 0x78830:
+	case 0x78834:
+	case _vgtif_reg(pdp[0].lo):
+	case _vgtif_reg(pdp[0].hi):
+	case _vgtif_reg(pdp[1].lo):
+	case _vgtif_reg(pdp[1].hi):
+	case _vgtif_reg(pdp[2].lo):
+	case _vgtif_reg(pdp[2].hi):
+	case _vgtif_reg(pdp[3].lo):
+	case _vgtif_reg(pdp[3].hi):
+	case _vgtif_reg(execlist_context_descriptor_lo):
+	case _vgtif_reg(execlist_context_descriptor_hi):
+		break;
+	default:
+		gvt_err("invalid pvinfo write offset %x bytes %x data %x\n",
+				offset, bytes, data);
+		break;
+	}
+	return 0;
+}
+
+static int pf_write(struct intel_vgpu *vgpu,
+		unsigned int offset, void *p_data, unsigned int bytes)
+{
+	u32 val = *(u32 *)p_data;
+
+	if ((offset == _PS_1A_CTRL || offset == _PS_2A_CTRL ||
+	   offset == _PS_1B_CTRL || offset == _PS_2B_CTRL ||
+	   offset == _PS_1C_CTRL) && (val & PS_PLANE_SEL_MASK) != 0) {
+		WARN_ONCE(true, "VM(%d): guest is trying to scaling a plane\n",
+			  vgpu->id);
+		return 0;
+	}
+
+	return intel_vgpu_default_mmio_write(vgpu, offset, p_data, bytes);
+}
+
+static int power_well_ctl_mmio_write(struct intel_vgpu *vgpu,
+		unsigned int offset, void *p_data, unsigned int bytes)
+{
+	write_vreg(vgpu, offset, p_data, bytes);
+
+	if (vgpu_vreg(vgpu, offset) & HSW_PWR_WELL_ENABLE_REQUEST)
+		vgpu_vreg(vgpu, offset) |= HSW_PWR_WELL_STATE_ENABLED;
+	else
+		vgpu_vreg(vgpu, offset) &= ~HSW_PWR_WELL_STATE_ENABLED;
+	return 0;
+}
+
+static int fpga_dbg_mmio_write(struct intel_vgpu *vgpu,
+	unsigned int offset, void *p_data, unsigned int bytes)
+{
+	write_vreg(vgpu, offset, p_data, bytes);
+
+	if (vgpu_vreg(vgpu, offset) & FPGA_DBG_RM_NOCLAIM)
+		vgpu_vreg(vgpu, offset) &= ~FPGA_DBG_RM_NOCLAIM;
+	return 0;
+}
+
+static int dma_ctrl_write(struct intel_vgpu *vgpu, unsigned int offset,
+		void *p_data, unsigned int bytes)
+{
+	u32 mode = *(u32 *)p_data;
+
+	if (GFX_MODE_BIT_SET_IN_MASK(mode, START_DMA)) {
+		WARN_ONCE(1, "VM(%d): iGVT-g doesn't supporte GuC\n",
+				vgpu->id);
+		return 0;
+	}
+
+	return 0;
+}
+
+static int gen9_trtte_write(struct intel_vgpu *vgpu, unsigned int offset,
+		void *p_data, unsigned int bytes)
+{
+	struct drm_i915_private *dev_priv = vgpu->gvt->dev_priv;
+	u32 trtte = *(u32 *)p_data;
+
+	if ((trtte & 1) && (trtte & (1 << 1)) == 0) {
+		WARN(1, "VM(%d): Use physical address for TRTT!\n",
+				vgpu->id);
+		return -EINVAL;
+	}
+	write_vreg(vgpu, offset, p_data, bytes);
+	/* TRTTE is not per-context */
+	I915_WRITE(_MMIO(offset), vgpu_vreg(vgpu, offset));
+
+	return 0;
+}
+
+static int gen9_trtt_chicken_write(struct intel_vgpu *vgpu, unsigned int offset,
+		void *p_data, unsigned int bytes)
+{
+	struct drm_i915_private *dev_priv = vgpu->gvt->dev_priv;
+	u32 val = *(u32 *)p_data;
+
+	if (val & 1) {
+		/* unblock hw logic */
+		I915_WRITE(_MMIO(offset), val);
+	}
+	write_vreg(vgpu, offset, p_data, bytes);
+	return 0;
+}
+
+static int dpll_status_read(struct intel_vgpu *vgpu, unsigned int offset,
+		void *p_data, unsigned int bytes)
+{
+	u32 v = 0;
+
+	if (vgpu_vreg(vgpu, 0x46010) & (1 << 31))
+		v |= (1 << 0);
+
+	if (vgpu_vreg(vgpu, 0x46014) & (1 << 31))
+		v |= (1 << 8);
+
+	if (vgpu_vreg(vgpu, 0x46040) & (1 << 31))
+		v |= (1 << 16);
+
+	if (vgpu_vreg(vgpu, 0x46060) & (1 << 31))
+		v |= (1 << 24);
+
+	vgpu_vreg(vgpu, offset) = v;
+
+	return intel_vgpu_default_mmio_read(vgpu, offset, p_data, bytes);
+}
+
+static int mailbox_write(struct intel_vgpu *vgpu, unsigned int offset,
+		void *p_data, unsigned int bytes)
+{
+	u32 value = *(u32 *)p_data;
+	u32 cmd = value & 0xff;
+	u32 *data0 = &vgpu_vreg(vgpu, GEN6_PCODE_DATA);
+
+	switch (cmd) {
+	case 0x6:
+		/**
+		 * "Read memory latency" command on gen9.
+		 * Below memory latency values are read
+		 * from skylake platform.
+		 */
+		if (!*data0)
+			*data0 = 0x1e1a1100;
+		else
+			*data0 = 0x61514b3d;
+		break;
+	case 0x5:
+		*data0 |= 0x1;
+		break;
+	}
+
+	gvt_dbg_core("VM(%d) write %x to mailbox, return data0 %x\n",
+		     vgpu->id, value, *data0);
+
+	value &= ~(1 << 31);
+	return intel_vgpu_default_mmio_write(vgpu, offset, &value, bytes);
+}
+
+static int skl_power_well_ctl_write(struct intel_vgpu *vgpu,
+		unsigned int offset, void *p_data, unsigned int bytes)
+{
+	u32 v = *(u32 *)p_data;
+
+	v &= (1 << 31) | (1 << 29) | (1 << 9) |
+	     (1 << 7) | (1 << 5) | (1 << 3) | (1 << 1);
+	v |= (v >> 1);
+
+	return intel_vgpu_default_mmio_write(vgpu, offset, &v, bytes);
+}
+
+static int skl_misc_ctl_write(struct intel_vgpu *vgpu, unsigned int offset,
+		void *p_data, unsigned int bytes)
+{
+	struct drm_i915_private *dev_priv = vgpu->gvt->dev_priv;
+	i915_reg_t reg = {.reg = offset};
+
+	switch (offset) {
+	case 0x4ddc:
+		vgpu_vreg(vgpu, offset) = 0x8000003c;
+		break;
+	case 0x42080:
+		vgpu_vreg(vgpu, offset) = 0x8000;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	/**
+	 * TODO: need detect stepping info after gvt contain such information
+	 * 0x4ddc enabled after C0, 0x42080 enabled after E0.
+	 */
+	I915_WRITE(reg, vgpu_vreg(vgpu, offset));
+	return 0;
+}
+
+static int skl_lcpll_write(struct intel_vgpu *vgpu, unsigned int offset,
+		void *p_data, unsigned int bytes)
+{
+	u32 v = *(u32 *)p_data;
+
+	/* other bits are MBZ. */
+	v &= (1 << 31) | (1 << 30);
+	v & (1 << 31) ? (v |= (1 << 30)) : (v &= ~(1 << 30));
+
+	vgpu_vreg(vgpu, offset) = v;
+
+	return 0;
+}
+
+static int ring_timestamp_mmio_read(struct intel_vgpu *vgpu,
+		unsigned int offset, void *p_data, unsigned int bytes)
+{
+	struct drm_i915_private *dev_priv = vgpu->gvt->dev_priv;
+
+	vgpu_vreg(vgpu, offset) = I915_READ(_MMIO(offset));
+	return intel_vgpu_default_mmio_read(vgpu, offset, p_data, bytes);
+}
+
+static int elsp_mmio_write(struct intel_vgpu *vgpu, unsigned int offset,
+		void *p_data, unsigned int bytes)
+{
+	int ring_id = render_mmio_to_ring_id(vgpu->gvt, offset);
+	struct intel_vgpu_execlist *execlist;
+	u32 data = *(u32 *)p_data;
+	int ret;
+
+	if (WARN_ON(ring_id < 0 || ring_id > I915_NUM_ENGINES - 1))
+		return -EINVAL;
+
+	execlist = &vgpu->execlist[ring_id];
+
+	execlist->elsp_dwords.data[execlist->elsp_dwords.index] = data;
+	if (execlist->elsp_dwords.index == 3)
+		ret = intel_vgpu_submit_execlist(vgpu, ring_id);
+
+	++execlist->elsp_dwords.index;
+	execlist->elsp_dwords.index &= 0x3;
+	return 0;
+}
+
+static int ring_mode_mmio_write(struct intel_vgpu *vgpu, unsigned int offset,
+		void *p_data, unsigned int bytes)
+{
+	u32 data = *(u32 *)p_data;
+	int ring_id = render_mmio_to_ring_id(vgpu->gvt, offset);
+	bool enable_execlist;
+
+	write_vreg(vgpu, offset, p_data, bytes);
+	if ((data & _MASKED_BIT_ENABLE(GFX_RUN_LIST_ENABLE))
+			|| (data & _MASKED_BIT_DISABLE(GFX_RUN_LIST_ENABLE))) {
+		enable_execlist = !!(data & GFX_RUN_LIST_ENABLE);
+
+		gvt_dbg_core("EXECLIST %s on ring %d\n",
+				(enable_execlist ? "enabling" : "disabling"),
+				ring_id);
+
+		if (enable_execlist)
+			intel_vgpu_start_schedule(vgpu);
+	}
+	return 0;
+}
+
+static int gvt_reg_tlb_control_handler(struct intel_vgpu *vgpu,
+		unsigned int offset, void *p_data, unsigned int bytes)
+{
+	int rc = 0;
+	unsigned int id = 0;
+
+	switch (offset) {
+	case 0x4260:
+		id = RCS;
+		break;
+	case 0x4264:
+		id = VCS;
+		break;
+	case 0x4268:
+		id = VCS2;
+		break;
+	case 0x426c:
+		id = BCS;
+		break;
+	case 0x4270:
+		id = VECS;
+		break;
+	default:
+		rc = -EINVAL;
+		break;
+	}
+	set_bit(id, (void *)vgpu->tlb_handle_pending);
+
+	return rc;
+}
+
+#define MMIO_F(reg, s, f, am, rm, d, r, w) do { \
+	ret = new_mmio_info(gvt, INTEL_GVT_MMIO_OFFSET(reg), \
+		f, s, am, rm, d, r, w); \
+	if (ret) \
+		return ret; \
+} while (0)
+
+#define MMIO_D(reg, d) \
+	MMIO_F(reg, 4, 0, 0, 0, d, NULL, NULL)
+
+#define MMIO_DH(reg, d, r, w) \
+	MMIO_F(reg, 4, 0, 0, 0, d, r, w)
+
+#define MMIO_DFH(reg, d, f, r, w) \
+	MMIO_F(reg, 4, f, 0, 0, d, r, w)
+
+#define MMIO_GM(reg, d, r, w) \
+	MMIO_F(reg, 4, F_GMADR, 0xFFFFF000, 0, d, r, w)
+
+#define MMIO_RO(reg, d, f, rm, r, w) \
+	MMIO_F(reg, 4, F_RO | f, 0, rm, d, r, w)
+
+#define MMIO_RING_F(prefix, s, f, am, rm, d, r, w) do { \
+	MMIO_F(prefix(RENDER_RING_BASE), s, f, am, rm, d, r, w); \
+	MMIO_F(prefix(BLT_RING_BASE), s, f, am, rm, d, r, w); \
+	MMIO_F(prefix(GEN6_BSD_RING_BASE), s, f, am, rm, d, r, w); \
+	MMIO_F(prefix(VEBOX_RING_BASE), s, f, am, rm, d, r, w); \
+} while (0)
+
+#define MMIO_RING_D(prefix, d) \
+	MMIO_RING_F(prefix, 4, 0, 0, 0, d, NULL, NULL)
+
+#define MMIO_RING_DFH(prefix, d, f, r, w) \
+	MMIO_RING_F(prefix, 4, f, 0, 0, d, r, w)
+
+#define MMIO_RING_GM(prefix, d, r, w) \
+	MMIO_RING_F(prefix, 4, F_GMADR, 0xFFFF0000, 0, d, r, w)
+
+#define MMIO_RING_RO(prefix, d, f, rm, r, w) \
+	MMIO_RING_F(prefix, 4, F_RO | f, 0, rm, d, r, w)
+
+static int init_generic_mmio_info(struct intel_gvt *gvt)
+{
+	struct drm_i915_private *dev_priv = gvt->dev_priv;
+	int ret;
+
+	MMIO_RING_DFH(RING_IMR, D_ALL, 0, NULL, intel_vgpu_reg_imr_handler);
+
+	MMIO_DFH(SDEIMR, D_ALL, 0, NULL, intel_vgpu_reg_imr_handler);
+	MMIO_DFH(SDEIER, D_ALL, 0, NULL, intel_vgpu_reg_ier_handler);
+	MMIO_DFH(SDEIIR, D_ALL, 0, NULL, intel_vgpu_reg_iir_handler);
+	MMIO_D(SDEISR, D_ALL);
+
+	MMIO_RING_D(RING_HWSTAM, D_ALL);
+
+	MMIO_GM(RENDER_HWS_PGA_GEN7, D_ALL, NULL, NULL);
+	MMIO_GM(BSD_HWS_PGA_GEN7, D_ALL, NULL, NULL);
+	MMIO_GM(BLT_HWS_PGA_GEN7, D_ALL, NULL, NULL);
+	MMIO_GM(VEBOX_HWS_PGA_GEN7, D_ALL, NULL, NULL);
+
+#define RING_REG(base) (base + 0x28)
+	MMIO_RING_D(RING_REG, D_ALL);
+#undef RING_REG
+
+#define RING_REG(base) (base + 0x134)
+	MMIO_RING_D(RING_REG, D_ALL);
+#undef RING_REG
+
+	MMIO_GM(0x2148, D_ALL, NULL, NULL);
+	MMIO_GM(CCID, D_ALL, NULL, NULL);
+	MMIO_GM(0x12198, D_ALL, NULL, NULL);
+	MMIO_D(GEN7_CXT_SIZE, D_ALL);
+
+	MMIO_RING_D(RING_TAIL, D_ALL);
+	MMIO_RING_D(RING_HEAD, D_ALL);
+	MMIO_RING_D(RING_CTL, D_ALL);
+	MMIO_RING_D(RING_ACTHD, D_ALL);
+	MMIO_RING_GM(RING_START, D_ALL, NULL, NULL);
+
+	/* RING MODE */
+#define RING_REG(base) (base + 0x29c)
+	MMIO_RING_DFH(RING_REG, D_ALL, F_MODE_MASK, NULL, ring_mode_mmio_write);
+#undef RING_REG
+
+	MMIO_RING_DFH(RING_MI_MODE, D_ALL, F_MODE_MASK, NULL, NULL);
+	MMIO_RING_DFH(RING_INSTPM, D_ALL, F_MODE_MASK, NULL, NULL);
+	MMIO_RING_DFH(RING_TIMESTAMP, D_ALL, F_CMD_ACCESS,
+			ring_timestamp_mmio_read, NULL);
+	MMIO_RING_DFH(RING_TIMESTAMP_UDW, D_ALL, F_CMD_ACCESS,
+			ring_timestamp_mmio_read, NULL);
+
+	MMIO_DFH(GEN7_GT_MODE, D_ALL, F_MODE_MASK, NULL, NULL);
+	MMIO_DFH(CACHE_MODE_0_GEN7, D_ALL, F_MODE_MASK, NULL, NULL);
+	MMIO_DFH(CACHE_MODE_1, D_ALL, F_MODE_MASK, NULL, NULL);
+
+	MMIO_DFH(0x20dc, D_ALL, F_MODE_MASK, NULL, NULL);
+	MMIO_DFH(_3D_CHICKEN3, D_ALL, F_MODE_MASK, NULL, NULL);
+	MMIO_DFH(0x2088, D_ALL, F_MODE_MASK, NULL, NULL);
+	MMIO_DFH(0x20e4, D_ALL, F_MODE_MASK, NULL, NULL);
+	MMIO_DFH(0x2470, D_ALL, F_MODE_MASK, NULL, NULL);
+	MMIO_D(GAM_ECOCHK, D_ALL);
+	MMIO_DFH(GEN7_COMMON_SLICE_CHICKEN1, D_ALL, F_MODE_MASK, NULL, NULL);
+	MMIO_DFH(COMMON_SLICE_CHICKEN2, D_ALL, F_MODE_MASK, NULL, NULL);
+	MMIO_D(0x9030, D_ALL);
+	MMIO_D(0x20a0, D_ALL);
+	MMIO_D(0x2420, D_ALL);
+	MMIO_D(0x2430, D_ALL);
+	MMIO_D(0x2434, D_ALL);
+	MMIO_D(0x2438, D_ALL);
+	MMIO_D(0x243c, D_ALL);
+	MMIO_DFH(0x7018, D_ALL, F_MODE_MASK, NULL, NULL);
+	MMIO_DFH(0xe184, D_ALL, F_MODE_MASK, NULL, NULL);
+	MMIO_DFH(0xe100, D_ALL, F_MODE_MASK, NULL, NULL);
+
+	/* display */
+	MMIO_F(0x60220, 0x20, 0, 0, 0, D_ALL, NULL, NULL);
+	MMIO_D(0x602a0, D_ALL);
+
+	MMIO_D(0x65050, D_ALL);
+	MMIO_D(0x650b4, D_ALL);
+
+	MMIO_D(0xc4040, D_ALL);
+	MMIO_D(DERRMR, D_ALL);
+
+	MMIO_D(PIPEDSL(PIPE_A), D_ALL);
+	MMIO_D(PIPEDSL(PIPE_B), D_ALL);
+	MMIO_D(PIPEDSL(PIPE_C), D_ALL);
+	MMIO_D(PIPEDSL(_PIPE_EDP), D_ALL);
+
+	MMIO_DH(PIPECONF(PIPE_A), D_ALL, NULL, pipeconf_mmio_write);
+	MMIO_DH(PIPECONF(PIPE_B), D_ALL, NULL, pipeconf_mmio_write);
+	MMIO_DH(PIPECONF(PIPE_C), D_ALL, NULL, pipeconf_mmio_write);
+	MMIO_DH(PIPECONF(_PIPE_EDP), D_ALL, NULL, pipeconf_mmio_write);
+
+	MMIO_D(PIPESTAT(PIPE_A), D_ALL);
+	MMIO_D(PIPESTAT(PIPE_B), D_ALL);
+	MMIO_D(PIPESTAT(PIPE_C), D_ALL);
+	MMIO_D(PIPESTAT(_PIPE_EDP), D_ALL);
+
+	MMIO_D(PIPE_FLIPCOUNT_G4X(PIPE_A), D_ALL);
+	MMIO_D(PIPE_FLIPCOUNT_G4X(PIPE_B), D_ALL);
+	MMIO_D(PIPE_FLIPCOUNT_G4X(PIPE_C), D_ALL);
+	MMIO_D(PIPE_FLIPCOUNT_G4X(_PIPE_EDP), D_ALL);
+
+	MMIO_D(PIPE_FRMCOUNT_G4X(PIPE_A), D_ALL);
+	MMIO_D(PIPE_FRMCOUNT_G4X(PIPE_B), D_ALL);
+	MMIO_D(PIPE_FRMCOUNT_G4X(PIPE_C), D_ALL);
+	MMIO_D(PIPE_FRMCOUNT_G4X(_PIPE_EDP), D_ALL);
+
+	MMIO_D(CURCNTR(PIPE_A), D_ALL);
+	MMIO_D(CURCNTR(PIPE_B), D_ALL);
+	MMIO_D(CURCNTR(PIPE_C), D_ALL);
+
+	MMIO_D(CURPOS(PIPE_A), D_ALL);
+	MMIO_D(CURPOS(PIPE_B), D_ALL);
+	MMIO_D(CURPOS(PIPE_C), D_ALL);
+
+	MMIO_D(CURBASE(PIPE_A), D_ALL);
+	MMIO_D(CURBASE(PIPE_B), D_ALL);
+	MMIO_D(CURBASE(PIPE_C), D_ALL);
+
+	MMIO_D(0x700ac, D_ALL);
+	MMIO_D(0x710ac, D_ALL);
+	MMIO_D(0x720ac, D_ALL);
+
+	MMIO_D(0x70090, D_ALL);
+	MMIO_D(0x70094, D_ALL);
+	MMIO_D(0x70098, D_ALL);
+	MMIO_D(0x7009c, D_ALL);
+
+	MMIO_D(DSPCNTR(PIPE_A), D_ALL);
+	MMIO_D(DSPADDR(PIPE_A), D_ALL);
+	MMIO_D(DSPSTRIDE(PIPE_A), D_ALL);
+	MMIO_D(DSPPOS(PIPE_A), D_ALL);
+	MMIO_D(DSPSIZE(PIPE_A), D_ALL);
+	MMIO_DH(DSPSURF(PIPE_A), D_ALL, NULL, pri_surf_mmio_write);
+	MMIO_D(DSPOFFSET(PIPE_A), D_ALL);
+	MMIO_D(DSPSURFLIVE(PIPE_A), D_ALL);
+
+	MMIO_D(DSPCNTR(PIPE_B), D_ALL);
+	MMIO_D(DSPADDR(PIPE_B), D_ALL);
+	MMIO_D(DSPSTRIDE(PIPE_B), D_ALL);
+	MMIO_D(DSPPOS(PIPE_B), D_ALL);
+	MMIO_D(DSPSIZE(PIPE_B), D_ALL);
+	MMIO_DH(DSPSURF(PIPE_B), D_ALL, NULL, pri_surf_mmio_write);
+	MMIO_D(DSPOFFSET(PIPE_B), D_ALL);
+	MMIO_D(DSPSURFLIVE(PIPE_B), D_ALL);
+
+	MMIO_D(DSPCNTR(PIPE_C), D_ALL);
+	MMIO_D(DSPADDR(PIPE_C), D_ALL);
+	MMIO_D(DSPSTRIDE(PIPE_C), D_ALL);
+	MMIO_D(DSPPOS(PIPE_C), D_ALL);
+	MMIO_D(DSPSIZE(PIPE_C), D_ALL);
+	MMIO_DH(DSPSURF(PIPE_C), D_ALL, NULL, pri_surf_mmio_write);
+	MMIO_D(DSPOFFSET(PIPE_C), D_ALL);
+	MMIO_D(DSPSURFLIVE(PIPE_C), D_ALL);
+
+	MMIO_D(SPRCTL(PIPE_A), D_ALL);
+	MMIO_D(SPRLINOFF(PIPE_A), D_ALL);
+	MMIO_D(SPRSTRIDE(PIPE_A), D_ALL);
+	MMIO_D(SPRPOS(PIPE_A), D_ALL);
+	MMIO_D(SPRSIZE(PIPE_A), D_ALL);
+	MMIO_D(SPRKEYVAL(PIPE_A), D_ALL);
+	MMIO_D(SPRKEYMSK(PIPE_A), D_ALL);
+	MMIO_DH(SPRSURF(PIPE_A), D_ALL, NULL, spr_surf_mmio_write);
+	MMIO_D(SPRKEYMAX(PIPE_A), D_ALL);
+	MMIO_D(SPROFFSET(PIPE_A), D_ALL);
+	MMIO_D(SPRSCALE(PIPE_A), D_ALL);
+	MMIO_D(SPRSURFLIVE(PIPE_A), D_ALL);
+
+	MMIO_D(SPRCTL(PIPE_B), D_ALL);
+	MMIO_D(SPRLINOFF(PIPE_B), D_ALL);
+	MMIO_D(SPRSTRIDE(PIPE_B), D_ALL);
+	MMIO_D(SPRPOS(PIPE_B), D_ALL);
+	MMIO_D(SPRSIZE(PIPE_B), D_ALL);
+	MMIO_D(SPRKEYVAL(PIPE_B), D_ALL);
+	MMIO_D(SPRKEYMSK(PIPE_B), D_ALL);
+	MMIO_DH(SPRSURF(PIPE_B), D_ALL, NULL, spr_surf_mmio_write);
+	MMIO_D(SPRKEYMAX(PIPE_B), D_ALL);
+	MMIO_D(SPROFFSET(PIPE_B), D_ALL);
+	MMIO_D(SPRSCALE(PIPE_B), D_ALL);
+	MMIO_D(SPRSURFLIVE(PIPE_B), D_ALL);
+
+	MMIO_D(SPRCTL(PIPE_C), D_ALL);
+	MMIO_D(SPRLINOFF(PIPE_C), D_ALL);
+	MMIO_D(SPRSTRIDE(PIPE_C), D_ALL);
+	MMIO_D(SPRPOS(PIPE_C), D_ALL);
+	MMIO_D(SPRSIZE(PIPE_C), D_ALL);
+	MMIO_D(SPRKEYVAL(PIPE_C), D_ALL);
+	MMIO_D(SPRKEYMSK(PIPE_C), D_ALL);
+	MMIO_DH(SPRSURF(PIPE_C), D_ALL, NULL, spr_surf_mmio_write);
+	MMIO_D(SPRKEYMAX(PIPE_C), D_ALL);
+	MMIO_D(SPROFFSET(PIPE_C), D_ALL);
+	MMIO_D(SPRSCALE(PIPE_C), D_ALL);
+	MMIO_D(SPRSURFLIVE(PIPE_C), D_ALL);
+
+	MMIO_F(LGC_PALETTE(PIPE_A, 0), 4 * 256, 0, 0, 0, D_ALL, NULL, NULL);
+	MMIO_F(LGC_PALETTE(PIPE_B, 0), 4 * 256, 0, 0, 0, D_ALL, NULL, NULL);
+	MMIO_F(LGC_PALETTE(PIPE_C, 0), 4 * 256, 0, 0, 0, D_ALL, NULL, NULL);
+
+	MMIO_D(HTOTAL(TRANSCODER_A), D_ALL);
+	MMIO_D(HBLANK(TRANSCODER_A), D_ALL);
+	MMIO_D(HSYNC(TRANSCODER_A), D_ALL);
+	MMIO_D(VTOTAL(TRANSCODER_A), D_ALL);
+	MMIO_D(VBLANK(TRANSCODER_A), D_ALL);
+	MMIO_D(VSYNC(TRANSCODER_A), D_ALL);
+	MMIO_D(BCLRPAT(TRANSCODER_A), D_ALL);
+	MMIO_D(VSYNCSHIFT(TRANSCODER_A), D_ALL);
+	MMIO_D(PIPESRC(TRANSCODER_A), D_ALL);
+
+	MMIO_D(HTOTAL(TRANSCODER_B), D_ALL);
+	MMIO_D(HBLANK(TRANSCODER_B), D_ALL);
+	MMIO_D(HSYNC(TRANSCODER_B), D_ALL);
+	MMIO_D(VTOTAL(TRANSCODER_B), D_ALL);
+	MMIO_D(VBLANK(TRANSCODER_B), D_ALL);
+	MMIO_D(VSYNC(TRANSCODER_B), D_ALL);
+	MMIO_D(BCLRPAT(TRANSCODER_B), D_ALL);
+	MMIO_D(VSYNCSHIFT(TRANSCODER_B), D_ALL);
+	MMIO_D(PIPESRC(TRANSCODER_B), D_ALL);
+
+	MMIO_D(HTOTAL(TRANSCODER_C), D_ALL);
+	MMIO_D(HBLANK(TRANSCODER_C), D_ALL);
+	MMIO_D(HSYNC(TRANSCODER_C), D_ALL);
+	MMIO_D(VTOTAL(TRANSCODER_C), D_ALL);
+	MMIO_D(VBLANK(TRANSCODER_C), D_ALL);
+	MMIO_D(VSYNC(TRANSCODER_C), D_ALL);
+	MMIO_D(BCLRPAT(TRANSCODER_C), D_ALL);
+	MMIO_D(VSYNCSHIFT(TRANSCODER_C), D_ALL);
+	MMIO_D(PIPESRC(TRANSCODER_C), D_ALL);
+
+	MMIO_D(HTOTAL(TRANSCODER_EDP), D_ALL);
+	MMIO_D(HBLANK(TRANSCODER_EDP), D_ALL);
+	MMIO_D(HSYNC(TRANSCODER_EDP), D_ALL);
+	MMIO_D(VTOTAL(TRANSCODER_EDP), D_ALL);
+	MMIO_D(VBLANK(TRANSCODER_EDP), D_ALL);
+	MMIO_D(VSYNC(TRANSCODER_EDP), D_ALL);
+	MMIO_D(BCLRPAT(TRANSCODER_EDP), D_ALL);
+	MMIO_D(VSYNCSHIFT(TRANSCODER_EDP), D_ALL);
+
+	MMIO_D(PIPE_DATA_M1(TRANSCODER_A), D_ALL);
+	MMIO_D(PIPE_DATA_N1(TRANSCODER_A), D_ALL);
+	MMIO_D(PIPE_DATA_M2(TRANSCODER_A), D_ALL);
+	MMIO_D(PIPE_DATA_N2(TRANSCODER_A), D_ALL);
+	MMIO_D(PIPE_LINK_M1(TRANSCODER_A), D_ALL);
+	MMIO_D(PIPE_LINK_N1(TRANSCODER_A), D_ALL);
+	MMIO_D(PIPE_LINK_M2(TRANSCODER_A), D_ALL);
+	MMIO_D(PIPE_LINK_N2(TRANSCODER_A), D_ALL);
+
+	MMIO_D(PIPE_DATA_M1(TRANSCODER_B), D_ALL);
+	MMIO_D(PIPE_DATA_N1(TRANSCODER_B), D_ALL);
+	MMIO_D(PIPE_DATA_M2(TRANSCODER_B), D_ALL);
+	MMIO_D(PIPE_DATA_N2(TRANSCODER_B), D_ALL);
+	MMIO_D(PIPE_LINK_M1(TRANSCODER_B), D_ALL);
+	MMIO_D(PIPE_LINK_N1(TRANSCODER_B), D_ALL);
+	MMIO_D(PIPE_LINK_M2(TRANSCODER_B), D_ALL);
+	MMIO_D(PIPE_LINK_N2(TRANSCODER_B), D_ALL);
+
+	MMIO_D(PIPE_DATA_M1(TRANSCODER_C), D_ALL);
+	MMIO_D(PIPE_DATA_N1(TRANSCODER_C), D_ALL);
+	MMIO_D(PIPE_DATA_M2(TRANSCODER_C), D_ALL);
+	MMIO_D(PIPE_DATA_N2(TRANSCODER_C), D_ALL);
+	MMIO_D(PIPE_LINK_M1(TRANSCODER_C), D_ALL);
+	MMIO_D(PIPE_LINK_N1(TRANSCODER_C), D_ALL);
+	MMIO_D(PIPE_LINK_M2(TRANSCODER_C), D_ALL);
+	MMIO_D(PIPE_LINK_N2(TRANSCODER_C), D_ALL);
+
+	MMIO_D(PIPE_DATA_M1(TRANSCODER_EDP), D_ALL);
+	MMIO_D(PIPE_DATA_N1(TRANSCODER_EDP), D_ALL);
+	MMIO_D(PIPE_DATA_M2(TRANSCODER_EDP), D_ALL);
+	MMIO_D(PIPE_DATA_N2(TRANSCODER_EDP), D_ALL);
+	MMIO_D(PIPE_LINK_M1(TRANSCODER_EDP), D_ALL);
+	MMIO_D(PIPE_LINK_N1(TRANSCODER_EDP), D_ALL);
+	MMIO_D(PIPE_LINK_M2(TRANSCODER_EDP), D_ALL);
+	MMIO_D(PIPE_LINK_N2(TRANSCODER_EDP), D_ALL);
+
+	MMIO_D(PF_CTL(PIPE_A), D_ALL);
+	MMIO_D(PF_WIN_SZ(PIPE_A), D_ALL);
+	MMIO_D(PF_WIN_POS(PIPE_A), D_ALL);
+	MMIO_D(PF_VSCALE(PIPE_A), D_ALL);
+	MMIO_D(PF_HSCALE(PIPE_A), D_ALL);
+
+	MMIO_D(PF_CTL(PIPE_B), D_ALL);
+	MMIO_D(PF_WIN_SZ(PIPE_B), D_ALL);
+	MMIO_D(PF_WIN_POS(PIPE_B), D_ALL);
+	MMIO_D(PF_VSCALE(PIPE_B), D_ALL);
+	MMIO_D(PF_HSCALE(PIPE_B), D_ALL);
+
+	MMIO_D(PF_CTL(PIPE_C), D_ALL);
+	MMIO_D(PF_WIN_SZ(PIPE_C), D_ALL);
+	MMIO_D(PF_WIN_POS(PIPE_C), D_ALL);
+	MMIO_D(PF_VSCALE(PIPE_C), D_ALL);
+	MMIO_D(PF_HSCALE(PIPE_C), D_ALL);
+
+	MMIO_D(WM0_PIPEA_ILK, D_ALL);
+	MMIO_D(WM0_PIPEB_ILK, D_ALL);
+	MMIO_D(WM0_PIPEC_IVB, D_ALL);
+	MMIO_D(WM1_LP_ILK, D_ALL);
+	MMIO_D(WM2_LP_ILK, D_ALL);
+	MMIO_D(WM3_LP_ILK, D_ALL);
+	MMIO_D(WM1S_LP_ILK, D_ALL);
+	MMIO_D(WM2S_LP_IVB, D_ALL);
+	MMIO_D(WM3S_LP_IVB, D_ALL);
+
+	MMIO_D(BLC_PWM_CPU_CTL2, D_ALL);
+	MMIO_D(BLC_PWM_CPU_CTL, D_ALL);
+	MMIO_D(BLC_PWM_PCH_CTL1, D_ALL);
+	MMIO_D(BLC_PWM_PCH_CTL2, D_ALL);
+
+	MMIO_D(0x48268, D_ALL);
+
+	MMIO_F(PCH_GMBUS0, 4 * 4, 0, 0, 0, D_ALL, gmbus_mmio_read,
+		gmbus_mmio_write);
+	MMIO_F(PCH_GPIOA, 6 * 4, F_UNALIGN, 0, 0, D_ALL, NULL, NULL);
+	MMIO_F(0xe4f00, 0x28, 0, 0, 0, D_ALL, NULL, NULL);
+
+	MMIO_F(_PCH_DPB_AUX_CH_CTL, 6 * 4, 0, 0, 0, D_PRE_SKL, NULL,
+		dp_aux_ch_ctl_mmio_write);
+	MMIO_F(_PCH_DPC_AUX_CH_CTL, 6 * 4, 0, 0, 0, D_PRE_SKL, NULL,
+		dp_aux_ch_ctl_mmio_write);
+	MMIO_F(_PCH_DPD_AUX_CH_CTL, 6 * 4, 0, 0, 0, D_PRE_SKL, NULL,
+		dp_aux_ch_ctl_mmio_write);
+
+	MMIO_RO(PCH_ADPA, D_ALL, 0, ADPA_CRT_HOTPLUG_MONITOR_MASK, NULL, pch_adpa_mmio_write);
+
+	MMIO_DH(_PCH_TRANSACONF, D_ALL, NULL, transconf_mmio_write);
+	MMIO_DH(_PCH_TRANSBCONF, D_ALL, NULL, transconf_mmio_write);
+
+	MMIO_DH(FDI_RX_IIR(PIPE_A), D_ALL, NULL, fdi_rx_iir_mmio_write);
+	MMIO_DH(FDI_RX_IIR(PIPE_B), D_ALL, NULL, fdi_rx_iir_mmio_write);
+	MMIO_DH(FDI_RX_IIR(PIPE_C), D_ALL, NULL, fdi_rx_iir_mmio_write);
+	MMIO_DH(FDI_RX_IMR(PIPE_A), D_ALL, NULL, update_fdi_rx_iir_status);
+	MMIO_DH(FDI_RX_IMR(PIPE_B), D_ALL, NULL, update_fdi_rx_iir_status);
+	MMIO_DH(FDI_RX_IMR(PIPE_C), D_ALL, NULL, update_fdi_rx_iir_status);
+	MMIO_DH(FDI_RX_CTL(PIPE_A), D_ALL, NULL, update_fdi_rx_iir_status);
+	MMIO_DH(FDI_RX_CTL(PIPE_B), D_ALL, NULL, update_fdi_rx_iir_status);
+	MMIO_DH(FDI_RX_CTL(PIPE_C), D_ALL, NULL, update_fdi_rx_iir_status);
+
+	MMIO_D(_PCH_TRANS_HTOTAL_A, D_ALL);
+	MMIO_D(_PCH_TRANS_HBLANK_A, D_ALL);
+	MMIO_D(_PCH_TRANS_HSYNC_A, D_ALL);
+	MMIO_D(_PCH_TRANS_VTOTAL_A, D_ALL);
+	MMIO_D(_PCH_TRANS_VBLANK_A, D_ALL);
+	MMIO_D(_PCH_TRANS_VSYNC_A, D_ALL);
+	MMIO_D(_PCH_TRANS_VSYNCSHIFT_A, D_ALL);
+
+	MMIO_D(_PCH_TRANS_HTOTAL_B, D_ALL);
+	MMIO_D(_PCH_TRANS_HBLANK_B, D_ALL);
+	MMIO_D(_PCH_TRANS_HSYNC_B, D_ALL);
+	MMIO_D(_PCH_TRANS_VTOTAL_B, D_ALL);
+	MMIO_D(_PCH_TRANS_VBLANK_B, D_ALL);
+	MMIO_D(_PCH_TRANS_VSYNC_B, D_ALL);
+	MMIO_D(_PCH_TRANS_VSYNCSHIFT_B, D_ALL);
+
+	MMIO_D(_PCH_TRANSA_DATA_M1, D_ALL);
+	MMIO_D(_PCH_TRANSA_DATA_N1, D_ALL);
+	MMIO_D(_PCH_TRANSA_DATA_M2, D_ALL);
+	MMIO_D(_PCH_TRANSA_DATA_N2, D_ALL);
+	MMIO_D(_PCH_TRANSA_LINK_M1, D_ALL);
+	MMIO_D(_PCH_TRANSA_LINK_N1, D_ALL);
+	MMIO_D(_PCH_TRANSA_LINK_M2, D_ALL);
+	MMIO_D(_PCH_TRANSA_LINK_N2, D_ALL);
+
+	MMIO_D(TRANS_DP_CTL(PIPE_A), D_ALL);
+	MMIO_D(TRANS_DP_CTL(PIPE_B), D_ALL);
+	MMIO_D(TRANS_DP_CTL(PIPE_C), D_ALL);
+
+	MMIO_D(TVIDEO_DIP_CTL(PIPE_A), D_ALL);
+	MMIO_D(TVIDEO_DIP_DATA(PIPE_A), D_ALL);
+	MMIO_D(TVIDEO_DIP_GCP(PIPE_A), D_ALL);
+
+	MMIO_D(TVIDEO_DIP_CTL(PIPE_B), D_ALL);
+	MMIO_D(TVIDEO_DIP_DATA(PIPE_B), D_ALL);
+	MMIO_D(TVIDEO_DIP_GCP(PIPE_B), D_ALL);
+
+	MMIO_D(TVIDEO_DIP_CTL(PIPE_C), D_ALL);
+	MMIO_D(TVIDEO_DIP_DATA(PIPE_C), D_ALL);
+	MMIO_D(TVIDEO_DIP_GCP(PIPE_C), D_ALL);
+
+	MMIO_D(_FDI_RXA_MISC, D_ALL);
+	MMIO_D(_FDI_RXB_MISC, D_ALL);
+	MMIO_D(_FDI_RXA_TUSIZE1, D_ALL);
+	MMIO_D(_FDI_RXA_TUSIZE2, D_ALL);
+	MMIO_D(_FDI_RXB_TUSIZE1, D_ALL);
+	MMIO_D(_FDI_RXB_TUSIZE2, D_ALL);
+
+	MMIO_DH(PCH_PP_CONTROL, D_ALL, NULL, pch_pp_control_mmio_write);
+	MMIO_D(PCH_PP_DIVISOR, D_ALL);
+	MMIO_D(PCH_PP_STATUS,  D_ALL);
+	MMIO_D(PCH_LVDS, D_ALL);
+	MMIO_D(_PCH_DPLL_A, D_ALL);
+	MMIO_D(_PCH_DPLL_B, D_ALL);
+	MMIO_D(_PCH_FPA0, D_ALL);
+	MMIO_D(_PCH_FPA1, D_ALL);
+	MMIO_D(_PCH_FPB0, D_ALL);
+	MMIO_D(_PCH_FPB1, D_ALL);
+	MMIO_D(PCH_DREF_CONTROL, D_ALL);
+	MMIO_D(PCH_RAWCLK_FREQ, D_ALL);
+	MMIO_D(PCH_DPLL_SEL, D_ALL);
+
+	MMIO_D(0x61208, D_ALL);
+	MMIO_D(0x6120c, D_ALL);
+	MMIO_D(PCH_PP_ON_DELAYS, D_ALL);
+	MMIO_D(PCH_PP_OFF_DELAYS, D_ALL);
+
+	MMIO_DH(0xe651c, D_ALL, dpy_reg_mmio_read, NULL);
+	MMIO_DH(0xe661c, D_ALL, dpy_reg_mmio_read, NULL);
+	MMIO_DH(0xe671c, D_ALL, dpy_reg_mmio_read, NULL);
+	MMIO_DH(0xe681c, D_ALL, dpy_reg_mmio_read, NULL);
+	MMIO_DH(0xe6c04, D_ALL, dpy_reg_mmio_read_2, NULL);
+	MMIO_DH(0xe6e1c, D_ALL, dpy_reg_mmio_read_3, NULL);
+
+	MMIO_RO(PCH_PORT_HOTPLUG, D_ALL, 0,
+		PORTA_HOTPLUG_STATUS_MASK
+		| PORTB_HOTPLUG_STATUS_MASK
+		| PORTC_HOTPLUG_STATUS_MASK
+		| PORTD_HOTPLUG_STATUS_MASK,
+		NULL, NULL);
+
+	MMIO_DH(LCPLL_CTL, D_ALL, NULL, lcpll_ctl_mmio_write);
+	MMIO_D(FUSE_STRAP, D_ALL);
+	MMIO_D(DIGITAL_PORT_HOTPLUG_CNTRL, D_ALL);
+
+	MMIO_D(DISP_ARB_CTL, D_ALL);
+	MMIO_D(DISP_ARB_CTL2, D_ALL);
+
+	MMIO_D(ILK_DISPLAY_CHICKEN1, D_ALL);
+	MMIO_D(ILK_DISPLAY_CHICKEN2, D_ALL);
+	MMIO_D(ILK_DSPCLK_GATE_D, D_ALL);
+
+	MMIO_D(SOUTH_CHICKEN1, D_ALL);
+	MMIO_DH(SOUTH_CHICKEN2, D_ALL, NULL, south_chicken2_mmio_write);
+	MMIO_D(_TRANSA_CHICKEN1, D_ALL);
+	MMIO_D(_TRANSB_CHICKEN1, D_ALL);
+	MMIO_D(SOUTH_DSPCLK_GATE_D, D_ALL);
+	MMIO_D(_TRANSA_CHICKEN2, D_ALL);
+	MMIO_D(_TRANSB_CHICKEN2, D_ALL);
+
+	MMIO_D(ILK_DPFC_CB_BASE, D_ALL);
+	MMIO_D(ILK_DPFC_CONTROL, D_ALL);
+	MMIO_D(ILK_DPFC_RECOMP_CTL, D_ALL);
+	MMIO_D(ILK_DPFC_STATUS, D_ALL);
+	MMIO_D(ILK_DPFC_FENCE_YOFF, D_ALL);
+	MMIO_D(ILK_DPFC_CHICKEN, D_ALL);
+	MMIO_D(ILK_FBC_RT_BASE, D_ALL);
+
+	MMIO_D(IPS_CTL, D_ALL);
+
+	MMIO_D(PIPE_CSC_COEFF_RY_GY(PIPE_A), D_ALL);
+	MMIO_D(PIPE_CSC_COEFF_BY(PIPE_A), D_ALL);
+	MMIO_D(PIPE_CSC_COEFF_RU_GU(PIPE_A), D_ALL);
+	MMIO_D(PIPE_CSC_COEFF_BU(PIPE_A), D_ALL);
+	MMIO_D(PIPE_CSC_COEFF_RV_GV(PIPE_A), D_ALL);
+	MMIO_D(PIPE_CSC_COEFF_BV(PIPE_A), D_ALL);
+	MMIO_D(PIPE_CSC_MODE(PIPE_A), D_ALL);
+	MMIO_D(PIPE_CSC_PREOFF_HI(PIPE_A), D_ALL);
+	MMIO_D(PIPE_CSC_PREOFF_ME(PIPE_A), D_ALL);
+	MMIO_D(PIPE_CSC_PREOFF_LO(PIPE_A), D_ALL);
+	MMIO_D(PIPE_CSC_POSTOFF_HI(PIPE_A), D_ALL);
+	MMIO_D(PIPE_CSC_POSTOFF_ME(PIPE_A), D_ALL);
+	MMIO_D(PIPE_CSC_POSTOFF_LO(PIPE_A), D_ALL);
+
+	MMIO_D(PIPE_CSC_COEFF_RY_GY(PIPE_B), D_ALL);
+	MMIO_D(PIPE_CSC_COEFF_BY(PIPE_B), D_ALL);
+	MMIO_D(PIPE_CSC_COEFF_RU_GU(PIPE_B), D_ALL);
+	MMIO_D(PIPE_CSC_COEFF_BU(PIPE_B), D_ALL);
+	MMIO_D(PIPE_CSC_COEFF_RV_GV(PIPE_B), D_ALL);
+	MMIO_D(PIPE_CSC_COEFF_BV(PIPE_B), D_ALL);
+	MMIO_D(PIPE_CSC_MODE(PIPE_B), D_ALL);
+	MMIO_D(PIPE_CSC_PREOFF_HI(PIPE_B), D_ALL);
+	MMIO_D(PIPE_CSC_PREOFF_ME(PIPE_B), D_ALL);
+	MMIO_D(PIPE_CSC_PREOFF_LO(PIPE_B), D_ALL);
+	MMIO_D(PIPE_CSC_POSTOFF_HI(PIPE_B), D_ALL);
+	MMIO_D(PIPE_CSC_POSTOFF_ME(PIPE_B), D_ALL);
+	MMIO_D(PIPE_CSC_POSTOFF_LO(PIPE_B), D_ALL);
+
+	MMIO_D(PIPE_CSC_COEFF_RY_GY(PIPE_C), D_ALL);
+	MMIO_D(PIPE_CSC_COEFF_BY(PIPE_C), D_ALL);
+	MMIO_D(PIPE_CSC_COEFF_RU_GU(PIPE_C), D_ALL);
+	MMIO_D(PIPE_CSC_COEFF_BU(PIPE_C), D_ALL);
+	MMIO_D(PIPE_CSC_COEFF_RV_GV(PIPE_C), D_ALL);
+	MMIO_D(PIPE_CSC_COEFF_BV(PIPE_C), D_ALL);
+	MMIO_D(PIPE_CSC_MODE(PIPE_C), D_ALL);
+	MMIO_D(PIPE_CSC_PREOFF_HI(PIPE_C), D_ALL);
+	MMIO_D(PIPE_CSC_PREOFF_ME(PIPE_C), D_ALL);
+	MMIO_D(PIPE_CSC_PREOFF_LO(PIPE_C), D_ALL);
+	MMIO_D(PIPE_CSC_POSTOFF_HI(PIPE_C), D_ALL);
+	MMIO_D(PIPE_CSC_POSTOFF_ME(PIPE_C), D_ALL);
+	MMIO_D(PIPE_CSC_POSTOFF_LO(PIPE_C), D_ALL);
+
+	MMIO_D(PREC_PAL_INDEX(PIPE_A), D_ALL);
+	MMIO_D(PREC_PAL_DATA(PIPE_A), D_ALL);
+	MMIO_F(PREC_PAL_GC_MAX(PIPE_A, 0), 4 * 3, 0, 0, 0, D_ALL, NULL, NULL);
+
+	MMIO_D(PREC_PAL_INDEX(PIPE_B), D_ALL);
+	MMIO_D(PREC_PAL_DATA(PIPE_B), D_ALL);
+	MMIO_F(PREC_PAL_GC_MAX(PIPE_B, 0), 4 * 3, 0, 0, 0, D_ALL, NULL, NULL);
+
+	MMIO_D(PREC_PAL_INDEX(PIPE_C), D_ALL);
+	MMIO_D(PREC_PAL_DATA(PIPE_C), D_ALL);
+	MMIO_F(PREC_PAL_GC_MAX(PIPE_C, 0), 4 * 3, 0, 0, 0, D_ALL, NULL, NULL);
+
+	MMIO_D(0x60110, D_ALL);
+	MMIO_D(0x61110, D_ALL);
+	MMIO_F(0x70400, 0x40, 0, 0, 0, D_ALL, NULL, NULL);
+	MMIO_F(0x71400, 0x40, 0, 0, 0, D_ALL, NULL, NULL);
+	MMIO_F(0x72400, 0x40, 0, 0, 0, D_ALL, NULL, NULL);
+	MMIO_F(0x70440, 0xc, 0, 0, 0, D_PRE_SKL, NULL, NULL);
+	MMIO_F(0x71440, 0xc, 0, 0, 0, D_PRE_SKL, NULL, NULL);
+	MMIO_F(0x72440, 0xc, 0, 0, 0, D_PRE_SKL, NULL, NULL);
+	MMIO_F(0x7044c, 0xc, 0, 0, 0, D_PRE_SKL, NULL, NULL);
+	MMIO_F(0x7144c, 0xc, 0, 0, 0, D_PRE_SKL, NULL, NULL);
+	MMIO_F(0x7244c, 0xc, 0, 0, 0, D_PRE_SKL, NULL, NULL);
+
+	MMIO_D(PIPE_WM_LINETIME(PIPE_A), D_ALL);
+	MMIO_D(PIPE_WM_LINETIME(PIPE_B), D_ALL);
+	MMIO_D(PIPE_WM_LINETIME(PIPE_C), D_ALL);
+	MMIO_D(SPLL_CTL, D_ALL);
+	MMIO_D(_WRPLL_CTL1, D_ALL);
+	MMIO_D(_WRPLL_CTL2, D_ALL);
+	MMIO_D(PORT_CLK_SEL(PORT_A), D_ALL);
+	MMIO_D(PORT_CLK_SEL(PORT_B), D_ALL);
+	MMIO_D(PORT_CLK_SEL(PORT_C), D_ALL);
+	MMIO_D(PORT_CLK_SEL(PORT_D), D_ALL);
+	MMIO_D(PORT_CLK_SEL(PORT_E), D_ALL);
+	MMIO_D(TRANS_CLK_SEL(TRANSCODER_A), D_ALL);
+	MMIO_D(TRANS_CLK_SEL(TRANSCODER_B), D_ALL);
+	MMIO_D(TRANS_CLK_SEL(TRANSCODER_C), D_ALL);
+
+	MMIO_D(HSW_NDE_RSTWRN_OPT, D_ALL);
+	MMIO_D(0x46508, D_ALL);
+
+	MMIO_D(0x49080, D_ALL);
+	MMIO_D(0x49180, D_ALL);
+	MMIO_D(0x49280, D_ALL);
+
+	MMIO_F(0x49090, 0x14, 0, 0, 0, D_ALL, NULL, NULL);
+	MMIO_F(0x49190, 0x14, 0, 0, 0, D_ALL, NULL, NULL);
+	MMIO_F(0x49290, 0x14, 0, 0, 0, D_ALL, NULL, NULL);
+
+	MMIO_D(GAMMA_MODE(PIPE_A), D_ALL);
+	MMIO_D(GAMMA_MODE(PIPE_B), D_ALL);
+	MMIO_D(GAMMA_MODE(PIPE_C), D_ALL);
+
+	MMIO_D(PIPE_MULT(PIPE_A), D_ALL);
+	MMIO_D(PIPE_MULT(PIPE_B), D_ALL);
+	MMIO_D(PIPE_MULT(PIPE_C), D_ALL);
+
+	MMIO_D(HSW_TVIDEO_DIP_CTL(TRANSCODER_A), D_ALL);
+	MMIO_D(HSW_TVIDEO_DIP_CTL(TRANSCODER_B), D_ALL);
+	MMIO_D(HSW_TVIDEO_DIP_CTL(TRANSCODER_C), D_ALL);
+
+	MMIO_DH(SFUSE_STRAP, D_ALL, NULL, NULL);
+	MMIO_D(SBI_ADDR, D_ALL);
+	MMIO_DH(SBI_DATA, D_ALL, sbi_data_mmio_read, NULL);
+	MMIO_DH(SBI_CTL_STAT, D_ALL, NULL, sbi_ctl_mmio_write);
+	MMIO_D(PIXCLK_GATE, D_ALL);
+
+	MMIO_F(_DPA_AUX_CH_CTL, 6 * 4, 0, 0, 0, D_ALL, NULL,
+		dp_aux_ch_ctl_mmio_write);
+
+	MMIO_DH(DDI_BUF_CTL(PORT_A), D_ALL, NULL, ddi_buf_ctl_mmio_write);
+	MMIO_DH(DDI_BUF_CTL(PORT_B), D_ALL, NULL, ddi_buf_ctl_mmio_write);
+	MMIO_DH(DDI_BUF_CTL(PORT_C), D_ALL, NULL, ddi_buf_ctl_mmio_write);
+	MMIO_DH(DDI_BUF_CTL(PORT_D), D_ALL, NULL, ddi_buf_ctl_mmio_write);
+	MMIO_DH(DDI_BUF_CTL(PORT_E), D_ALL, NULL, ddi_buf_ctl_mmio_write);
+
+	MMIO_DH(DP_TP_CTL(PORT_A), D_ALL, NULL, dp_tp_ctl_mmio_write);
+	MMIO_DH(DP_TP_CTL(PORT_B), D_ALL, NULL, dp_tp_ctl_mmio_write);
+	MMIO_DH(DP_TP_CTL(PORT_C), D_ALL, NULL, dp_tp_ctl_mmio_write);
+	MMIO_DH(DP_TP_CTL(PORT_D), D_ALL, NULL, dp_tp_ctl_mmio_write);
+	MMIO_DH(DP_TP_CTL(PORT_E), D_ALL, NULL, dp_tp_ctl_mmio_write);
+
+	MMIO_DH(DP_TP_STATUS(PORT_A), D_ALL, NULL, dp_tp_status_mmio_write);
+	MMIO_DH(DP_TP_STATUS(PORT_B), D_ALL, NULL, dp_tp_status_mmio_write);
+	MMIO_DH(DP_TP_STATUS(PORT_C), D_ALL, NULL, dp_tp_status_mmio_write);
+	MMIO_DH(DP_TP_STATUS(PORT_D), D_ALL, NULL, dp_tp_status_mmio_write);
+	MMIO_DH(DP_TP_STATUS(PORT_E), D_ALL, NULL, NULL);
+
+	MMIO_F(_DDI_BUF_TRANS_A, 0x50, 0, 0, 0, D_ALL, NULL, NULL);
+	MMIO_F(0x64e60, 0x50, 0, 0, 0, D_ALL, NULL, NULL);
+	MMIO_F(0x64eC0, 0x50, 0, 0, 0, D_ALL, NULL, NULL);
+	MMIO_F(0x64f20, 0x50, 0, 0, 0, D_ALL, NULL, NULL);
+	MMIO_F(0x64f80, 0x50, 0, 0, 0, D_ALL, NULL, NULL);
+
+	MMIO_D(HSW_AUD_CFG(PIPE_A), D_ALL);
+	MMIO_D(HSW_AUD_PIN_ELD_CP_VLD, D_ALL);
+
+	MMIO_DH(_TRANS_DDI_FUNC_CTL_A, D_ALL, NULL, NULL);
+	MMIO_DH(_TRANS_DDI_FUNC_CTL_B, D_ALL, NULL, NULL);
+	MMIO_DH(_TRANS_DDI_FUNC_CTL_C, D_ALL, NULL, NULL);
+	MMIO_DH(_TRANS_DDI_FUNC_CTL_EDP, D_ALL, NULL, NULL);
+
+	MMIO_D(_TRANSA_MSA_MISC, D_ALL);
+	MMIO_D(_TRANSB_MSA_MISC, D_ALL);
+	MMIO_D(_TRANSC_MSA_MISC, D_ALL);
+	MMIO_D(_TRANS_EDP_MSA_MISC, D_ALL);
+
+	MMIO_DH(FORCEWAKE, D_ALL, NULL, NULL);
+	MMIO_D(FORCEWAKE_ACK, D_ALL);
+	MMIO_D(GEN6_GT_CORE_STATUS, D_ALL);
+	MMIO_D(GEN6_GT_THREAD_STATUS_REG, D_ALL);
+	MMIO_D(GTFIFODBG, D_ALL);
+	MMIO_D(GTFIFOCTL, D_ALL);
+	MMIO_DH(FORCEWAKE_MT, D_PRE_SKL, NULL, mul_force_wake_write);
+	MMIO_DH(FORCEWAKE_ACK_HSW, D_HSW | D_BDW, NULL, NULL);
+	MMIO_D(ECOBUS, D_ALL);
+	MMIO_DH(GEN6_RC_CONTROL, D_ALL, NULL, NULL);
+	MMIO_DH(GEN6_RC_STATE, D_ALL, NULL, NULL);
+	MMIO_D(GEN6_RPNSWREQ, D_ALL);
+	MMIO_D(GEN6_RC_VIDEO_FREQ, D_ALL);
+	MMIO_D(GEN6_RP_DOWN_TIMEOUT, D_ALL);
+	MMIO_D(GEN6_RP_INTERRUPT_LIMITS, D_ALL);
+	MMIO_D(GEN6_RPSTAT1, D_ALL);
+	MMIO_D(GEN6_RP_CONTROL, D_ALL);
+	MMIO_D(GEN6_RP_UP_THRESHOLD, D_ALL);
+	MMIO_D(GEN6_RP_DOWN_THRESHOLD, D_ALL);
+	MMIO_D(GEN6_RP_CUR_UP_EI, D_ALL);
+	MMIO_D(GEN6_RP_CUR_UP, D_ALL);
+	MMIO_D(GEN6_RP_PREV_UP, D_ALL);
+	MMIO_D(GEN6_RP_CUR_DOWN_EI, D_ALL);
+	MMIO_D(GEN6_RP_CUR_DOWN, D_ALL);
+	MMIO_D(GEN6_RP_PREV_DOWN, D_ALL);
+	MMIO_D(GEN6_RP_UP_EI, D_ALL);
+	MMIO_D(GEN6_RP_DOWN_EI, D_ALL);
+	MMIO_D(GEN6_RP_IDLE_HYSTERSIS, D_ALL);
+	MMIO_D(GEN6_RC1_WAKE_RATE_LIMIT, D_ALL);
+	MMIO_D(GEN6_RC6_WAKE_RATE_LIMIT, D_ALL);
+	MMIO_D(GEN6_RC6pp_WAKE_RATE_LIMIT, D_ALL);
+	MMIO_D(GEN6_RC_EVALUATION_INTERVAL, D_ALL);
+	MMIO_D(GEN6_RC_IDLE_HYSTERSIS, D_ALL);
+	MMIO_D(GEN6_RC_SLEEP, D_ALL);
+	MMIO_D(GEN6_RC1e_THRESHOLD, D_ALL);
+	MMIO_D(GEN6_RC6_THRESHOLD, D_ALL);
+	MMIO_D(GEN6_RC6p_THRESHOLD, D_ALL);
+	MMIO_D(GEN6_RC6pp_THRESHOLD, D_ALL);
+	MMIO_D(GEN6_PMINTRMSK, D_ALL);
+	MMIO_DH(HSW_PWR_WELL_BIOS, D_HSW | D_BDW, NULL, power_well_ctl_mmio_write);
+	MMIO_DH(HSW_PWR_WELL_DRIVER, D_HSW | D_BDW, NULL, power_well_ctl_mmio_write);
+	MMIO_DH(HSW_PWR_WELL_KVMR, D_HSW | D_BDW, NULL, power_well_ctl_mmio_write);
+	MMIO_DH(HSW_PWR_WELL_DEBUG, D_HSW | D_BDW, NULL, power_well_ctl_mmio_write);
+	MMIO_DH(HSW_PWR_WELL_CTL5, D_HSW | D_BDW, NULL, power_well_ctl_mmio_write);
+	MMIO_DH(HSW_PWR_WELL_CTL6, D_HSW | D_BDW, NULL, power_well_ctl_mmio_write);
+
+	MMIO_D(RSTDBYCTL, D_ALL);
+
+	MMIO_DH(GEN6_GDRST, D_ALL, NULL, gdrst_mmio_write);
+	MMIO_F(FENCE_REG_GEN6_LO(0), 0x80, 0, 0, 0, D_ALL, fence_mmio_read, fence_mmio_write);
+	MMIO_F(VGT_PVINFO_PAGE, VGT_PVINFO_SIZE, F_UNALIGN, 0, 0, D_ALL, pvinfo_mmio_read, pvinfo_mmio_write);
+	MMIO_DH(CPU_VGACNTRL, D_ALL, NULL, vga_control_mmio_write);
+
+	MMIO_F(MCHBAR_MIRROR_BASE_SNB, 0x40000, 0, 0, 0, D_ALL, NULL, NULL);
+
+	MMIO_D(TILECTL, D_ALL);
+
+	MMIO_D(GEN6_UCGCTL1, D_ALL);
+	MMIO_D(GEN6_UCGCTL2, D_ALL);
+
+	MMIO_F(0x4f000, 0x90, 0, 0, 0, D_ALL, NULL, NULL);
+
+	MMIO_D(GEN6_PCODE_MAILBOX, D_PRE_SKL);
+	MMIO_D(GEN6_PCODE_DATA, D_ALL);
+	MMIO_D(0x13812c, D_ALL);
+	MMIO_DH(GEN7_ERR_INT, D_ALL, NULL, NULL);
+	MMIO_D(HSW_EDRAM_CAP, D_ALL);
+	MMIO_D(HSW_IDICR, D_ALL);
+	MMIO_DH(GFX_FLSH_CNTL_GEN6, D_ALL, NULL, NULL);
+
+	MMIO_D(0x3c, D_ALL);
+	MMIO_D(0x860, D_ALL);
+	MMIO_D(ECOSKPD, D_ALL);
+	MMIO_D(0x121d0, D_ALL);
+	MMIO_D(GEN6_BLITTER_ECOSKPD, D_ALL);
+	MMIO_D(0x41d0, D_ALL);
+	MMIO_D(GAC_ECO_BITS, D_ALL);
+	MMIO_D(0x6200, D_ALL);
+	MMIO_D(0x6204, D_ALL);
+	MMIO_D(0x6208, D_ALL);
+	MMIO_D(0x7118, D_ALL);
+	MMIO_D(0x7180, D_ALL);
+	MMIO_D(0x7408, D_ALL);
+	MMIO_D(0x7c00, D_ALL);
+	MMIO_D(GEN6_MBCTL, D_ALL);
+	MMIO_D(0x911c, D_ALL);
+	MMIO_D(0x9120, D_ALL);
+
+	MMIO_D(GAB_CTL, D_ALL);
+	MMIO_D(0x48800, D_ALL);
+	MMIO_D(0xce044, D_ALL);
+	MMIO_D(0xe6500, D_ALL);
+	MMIO_D(0xe6504, D_ALL);
+	MMIO_D(0xe6600, D_ALL);
+	MMIO_D(0xe6604, D_ALL);
+	MMIO_D(0xe6700, D_ALL);
+	MMIO_D(0xe6704, D_ALL);
+	MMIO_D(0xe6800, D_ALL);
+	MMIO_D(0xe6804, D_ALL);
+	MMIO_D(PCH_GMBUS4, D_ALL);
+	MMIO_D(PCH_GMBUS5, D_ALL);
+
+	MMIO_D(0x902c, D_ALL);
+	MMIO_D(0xec008, D_ALL);
+	MMIO_D(0xec00c, D_ALL);
+	MMIO_D(0xec008 + 0x18, D_ALL);
+	MMIO_D(0xec00c + 0x18, D_ALL);
+	MMIO_D(0xec008 + 0x18 * 2, D_ALL);
+	MMIO_D(0xec00c + 0x18 * 2, D_ALL);
+	MMIO_D(0xec008 + 0x18 * 3, D_ALL);
+	MMIO_D(0xec00c + 0x18 * 3, D_ALL);
+	MMIO_D(0xec408, D_ALL);
+	MMIO_D(0xec40c, D_ALL);
+	MMIO_D(0xec408 + 0x18, D_ALL);
+	MMIO_D(0xec40c + 0x18, D_ALL);
+	MMIO_D(0xec408 + 0x18 * 2, D_ALL);
+	MMIO_D(0xec40c + 0x18 * 2, D_ALL);
+	MMIO_D(0xec408 + 0x18 * 3, D_ALL);
+	MMIO_D(0xec40c + 0x18 * 3, D_ALL);
+	MMIO_D(0xfc810, D_ALL);
+	MMIO_D(0xfc81c, D_ALL);
+	MMIO_D(0xfc828, D_ALL);
+	MMIO_D(0xfc834, D_ALL);
+	MMIO_D(0xfcc00, D_ALL);
+	MMIO_D(0xfcc0c, D_ALL);
+	MMIO_D(0xfcc18, D_ALL);
+	MMIO_D(0xfcc24, D_ALL);
+	MMIO_D(0xfd000, D_ALL);
+	MMIO_D(0xfd00c, D_ALL);
+	MMIO_D(0xfd018, D_ALL);
+	MMIO_D(0xfd024, D_ALL);
+	MMIO_D(0xfd034, D_ALL);
+
+	MMIO_DH(FPGA_DBG, D_ALL, NULL, fpga_dbg_mmio_write);
+	MMIO_D(0x2054, D_ALL);
+	MMIO_D(0x12054, D_ALL);
+	MMIO_D(0x22054, D_ALL);
+	MMIO_D(0x1a054, D_ALL);
+
+	MMIO_D(0x44070, D_ALL);
+
+	MMIO_D(0x215c, D_HSW_PLUS);
+	MMIO_DFH(0x2178, D_ALL, F_CMD_ACCESS, NULL, NULL);
+	MMIO_DFH(0x217c, D_ALL, F_CMD_ACCESS, NULL, NULL);
+	MMIO_DFH(0x12178, D_ALL, F_CMD_ACCESS, NULL, NULL);
+	MMIO_DFH(0x1217c, D_ALL, F_CMD_ACCESS, NULL, NULL);
+
+	MMIO_F(0x2290, 8, 0, 0, 0, D_HSW_PLUS, NULL, NULL);
+	MMIO_D(OACONTROL, D_HSW);
+	MMIO_D(0x2b00, D_BDW_PLUS);
+	MMIO_D(0x2360, D_BDW_PLUS);
+	MMIO_F(0x5200, 32, 0, 0, 0, D_ALL, NULL, NULL);
+	MMIO_F(0x5240, 32, 0, 0, 0, D_ALL, NULL, NULL);
+	MMIO_F(0x5280, 16, 0, 0, 0, D_ALL, NULL, NULL);
+
+	MMIO_DFH(0x1c17c, D_BDW_PLUS, F_CMD_ACCESS, NULL, NULL);
+	MMIO_DFH(0x1c178, D_BDW_PLUS, F_CMD_ACCESS, NULL, NULL);
+	MMIO_D(BCS_SWCTRL, D_ALL);
+
+	MMIO_F(HS_INVOCATION_COUNT, 8, 0, 0, 0, D_ALL, NULL, NULL);
+	MMIO_F(DS_INVOCATION_COUNT, 8, 0, 0, 0, D_ALL, NULL, NULL);
+	MMIO_F(IA_VERTICES_COUNT, 8, 0, 0, 0, D_ALL, NULL, NULL);
+	MMIO_F(IA_PRIMITIVES_COUNT, 8, 0, 0, 0, D_ALL, NULL, NULL);
+	MMIO_F(VS_INVOCATION_COUNT, 8, 0, 0, 0, D_ALL, NULL, NULL);
+	MMIO_F(GS_INVOCATION_COUNT, 8, 0, 0, 0, D_ALL, NULL, NULL);
+	MMIO_F(GS_PRIMITIVES_COUNT, 8, 0, 0, 0, D_ALL, NULL, NULL);
+	MMIO_F(CL_INVOCATION_COUNT, 8, 0, 0, 0, D_ALL, NULL, NULL);
+	MMIO_F(CL_PRIMITIVES_COUNT, 8, 0, 0, 0, D_ALL, NULL, NULL);
+	MMIO_F(PS_INVOCATION_COUNT, 8, 0, 0, 0, D_ALL, NULL, NULL);
+	MMIO_F(PS_DEPTH_COUNT, 8, 0, 0, 0, D_ALL, NULL, NULL);
+	MMIO_DH(0x4260, D_BDW_PLUS, NULL, gvt_reg_tlb_control_handler);
+	MMIO_DH(0x4264, D_BDW_PLUS, NULL, gvt_reg_tlb_control_handler);
+	MMIO_DH(0x4268, D_BDW_PLUS, NULL, gvt_reg_tlb_control_handler);
+	MMIO_DH(0x426c, D_BDW_PLUS, NULL, gvt_reg_tlb_control_handler);
+	MMIO_DH(0x4270, D_BDW_PLUS, NULL, gvt_reg_tlb_control_handler);
+	MMIO_DFH(0x4094, D_BDW_PLUS, F_CMD_ACCESS, NULL, NULL);
+
+	return 0;
+}
+
+static int init_broadwell_mmio_info(struct intel_gvt *gvt)
+{
+	struct drm_i915_private *dev_priv = gvt->dev_priv;
+	int ret;
+
+	MMIO_DH(RING_IMR(GEN8_BSD2_RING_BASE), D_BDW_PLUS, NULL,
+			intel_vgpu_reg_imr_handler);
+
+	MMIO_DH(GEN8_GT_IMR(0), D_BDW_PLUS, NULL, intel_vgpu_reg_imr_handler);
+	MMIO_DH(GEN8_GT_IER(0), D_BDW_PLUS, NULL, intel_vgpu_reg_ier_handler);
+	MMIO_DH(GEN8_GT_IIR(0), D_BDW_PLUS, NULL, intel_vgpu_reg_iir_handler);
+	MMIO_D(GEN8_GT_ISR(0), D_BDW_PLUS);
+
+	MMIO_DH(GEN8_GT_IMR(1), D_BDW_PLUS, NULL, intel_vgpu_reg_imr_handler);
+	MMIO_DH(GEN8_GT_IER(1), D_BDW_PLUS, NULL, intel_vgpu_reg_ier_handler);
+	MMIO_DH(GEN8_GT_IIR(1), D_BDW_PLUS, NULL, intel_vgpu_reg_iir_handler);
+	MMIO_D(GEN8_GT_ISR(1), D_BDW_PLUS);
+
+	MMIO_DH(GEN8_GT_IMR(2), D_BDW_PLUS, NULL, intel_vgpu_reg_imr_handler);
+	MMIO_DH(GEN8_GT_IER(2), D_BDW_PLUS, NULL, intel_vgpu_reg_ier_handler);
+	MMIO_DH(GEN8_GT_IIR(2), D_BDW_PLUS, NULL, intel_vgpu_reg_iir_handler);
+	MMIO_D(GEN8_GT_ISR(2), D_BDW_PLUS);
+
+	MMIO_DH(GEN8_GT_IMR(3), D_BDW_PLUS, NULL, intel_vgpu_reg_imr_handler);
+	MMIO_DH(GEN8_GT_IER(3), D_BDW_PLUS, NULL, intel_vgpu_reg_ier_handler);
+	MMIO_DH(GEN8_GT_IIR(3), D_BDW_PLUS, NULL, intel_vgpu_reg_iir_handler);
+	MMIO_D(GEN8_GT_ISR(3), D_BDW_PLUS);
+
+	MMIO_DH(GEN8_DE_PIPE_IMR(PIPE_A), D_BDW_PLUS, NULL,
+		intel_vgpu_reg_imr_handler);
+	MMIO_DH(GEN8_DE_PIPE_IER(PIPE_A), D_BDW_PLUS, NULL,
+		intel_vgpu_reg_ier_handler);
+	MMIO_DH(GEN8_DE_PIPE_IIR(PIPE_A), D_BDW_PLUS, NULL,
+		intel_vgpu_reg_iir_handler);
+	MMIO_D(GEN8_DE_PIPE_ISR(PIPE_A), D_BDW_PLUS);
+
+	MMIO_DH(GEN8_DE_PIPE_IMR(PIPE_B), D_BDW_PLUS, NULL,
+		intel_vgpu_reg_imr_handler);
+	MMIO_DH(GEN8_DE_PIPE_IER(PIPE_B), D_BDW_PLUS, NULL,
+		intel_vgpu_reg_ier_handler);
+	MMIO_DH(GEN8_DE_PIPE_IIR(PIPE_B), D_BDW_PLUS, NULL,
+		intel_vgpu_reg_iir_handler);
+	MMIO_D(GEN8_DE_PIPE_ISR(PIPE_B), D_BDW_PLUS);
+
+	MMIO_DH(GEN8_DE_PIPE_IMR(PIPE_C), D_BDW_PLUS, NULL,
+		intel_vgpu_reg_imr_handler);
+	MMIO_DH(GEN8_DE_PIPE_IER(PIPE_C), D_BDW_PLUS, NULL,
+		intel_vgpu_reg_ier_handler);
+	MMIO_DH(GEN8_DE_PIPE_IIR(PIPE_C), D_BDW_PLUS, NULL,
+		intel_vgpu_reg_iir_handler);
+	MMIO_D(GEN8_DE_PIPE_ISR(PIPE_C), D_BDW_PLUS);
+
+	MMIO_DH(GEN8_DE_PORT_IMR, D_BDW_PLUS, NULL, intel_vgpu_reg_imr_handler);
+	MMIO_DH(GEN8_DE_PORT_IER, D_BDW_PLUS, NULL, intel_vgpu_reg_ier_handler);
+	MMIO_DH(GEN8_DE_PORT_IIR, D_BDW_PLUS, NULL, intel_vgpu_reg_iir_handler);
+	MMIO_D(GEN8_DE_PORT_ISR, D_BDW_PLUS);
+
+	MMIO_DH(GEN8_DE_MISC_IMR, D_BDW_PLUS, NULL, intel_vgpu_reg_imr_handler);
+	MMIO_DH(GEN8_DE_MISC_IER, D_BDW_PLUS, NULL, intel_vgpu_reg_ier_handler);
+	MMIO_DH(GEN8_DE_MISC_IIR, D_BDW_PLUS, NULL, intel_vgpu_reg_iir_handler);
+	MMIO_D(GEN8_DE_MISC_ISR, D_BDW_PLUS);
+
+	MMIO_DH(GEN8_PCU_IMR, D_BDW_PLUS, NULL, intel_vgpu_reg_imr_handler);
+	MMIO_DH(GEN8_PCU_IER, D_BDW_PLUS, NULL, intel_vgpu_reg_ier_handler);
+	MMIO_DH(GEN8_PCU_IIR, D_BDW_PLUS, NULL, intel_vgpu_reg_iir_handler);
+	MMIO_D(GEN8_PCU_ISR, D_BDW_PLUS);
+
+	MMIO_DH(GEN8_MASTER_IRQ, D_BDW_PLUS, NULL,
+		intel_vgpu_reg_master_irq_handler);
+
+	MMIO_D(RING_HWSTAM(GEN8_BSD2_RING_BASE), D_BDW_PLUS);
+	MMIO_D(0x1c134, D_BDW_PLUS);
+
+	MMIO_D(RING_TAIL(GEN8_BSD2_RING_BASE), D_BDW_PLUS);
+	MMIO_D(RING_HEAD(GEN8_BSD2_RING_BASE),  D_BDW_PLUS);
+	MMIO_GM(RING_START(GEN8_BSD2_RING_BASE), D_BDW_PLUS, NULL, NULL);
+	MMIO_D(RING_CTL(GEN8_BSD2_RING_BASE), D_BDW_PLUS);
+	MMIO_D(RING_ACTHD(GEN8_BSD2_RING_BASE), D_BDW_PLUS);
+	MMIO_D(RING_ACTHD_UDW(GEN8_BSD2_RING_BASE), D_BDW_PLUS);
+	MMIO_DFH(0x1c29c, D_BDW_PLUS, F_MODE_MASK, NULL, ring_mode_mmio_write);
+	MMIO_DFH(RING_MI_MODE(GEN8_BSD2_RING_BASE), D_BDW_PLUS, F_MODE_MASK,
+			NULL, NULL);
+	MMIO_DFH(RING_INSTPM(GEN8_BSD2_RING_BASE), D_BDW_PLUS, F_MODE_MASK,
+			NULL, NULL);
+	MMIO_DFH(RING_TIMESTAMP(GEN8_BSD2_RING_BASE), D_BDW_PLUS, F_CMD_ACCESS,
+			ring_timestamp_mmio_read, NULL);
+
+	MMIO_RING_D(RING_ACTHD_UDW, D_BDW_PLUS);
+
+#define RING_REG(base) (base + 0x230)
+	MMIO_RING_DFH(RING_REG, D_BDW_PLUS, 0, NULL, elsp_mmio_write);
+	MMIO_DH(RING_REG(GEN8_BSD2_RING_BASE), D_BDW_PLUS, NULL, elsp_mmio_write);
+#undef RING_REG
+
+#define RING_REG(base) (base + 0x234)
+	MMIO_RING_F(RING_REG, 8, F_RO, 0, ~0, D_BDW_PLUS, NULL, NULL);
+	MMIO_F(RING_REG(GEN8_BSD2_RING_BASE), 4, F_RO, 0, ~0LL, D_BDW_PLUS, NULL, NULL);
+#undef RING_REG
+
+#define RING_REG(base) (base + 0x244)
+	MMIO_RING_D(RING_REG, D_BDW_PLUS);
+	MMIO_D(RING_REG(GEN8_BSD2_RING_BASE), D_BDW_PLUS);
+#undef RING_REG
+
+#define RING_REG(base) (base + 0x370)
+	MMIO_RING_F(RING_REG, 48, F_RO, 0, ~0, D_BDW_PLUS, NULL, NULL);
+	MMIO_F(RING_REG(GEN8_BSD2_RING_BASE), 48, F_RO, 0, ~0, D_BDW_PLUS,
+			NULL, NULL);
+#undef RING_REG
+
+#define RING_REG(base) (base + 0x3a0)
+	MMIO_RING_DFH(RING_REG, D_BDW_PLUS, F_MODE_MASK, NULL, NULL);
+	MMIO_DFH(RING_REG(GEN8_BSD2_RING_BASE), D_BDW_PLUS, F_MODE_MASK, NULL, NULL);
+#undef RING_REG
+
+	MMIO_D(PIPEMISC(PIPE_A), D_BDW_PLUS);
+	MMIO_D(PIPEMISC(PIPE_B), D_BDW_PLUS);
+	MMIO_D(PIPEMISC(PIPE_C), D_BDW_PLUS);
+	MMIO_D(0x1c1d0, D_BDW_PLUS);
+	MMIO_D(GEN6_MBCUNIT_SNPCR, D_BDW_PLUS);
+	MMIO_D(GEN7_MISCCPCTL, D_BDW_PLUS);
+	MMIO_D(0x1c054, D_BDW_PLUS);
+
+	MMIO_D(GEN8_PRIVATE_PAT_LO, D_BDW_PLUS);
+	MMIO_D(GEN8_PRIVATE_PAT_HI, D_BDW_PLUS);
+
+	MMIO_D(GAMTARBMODE, D_BDW_PLUS);
+
+#define RING_REG(base) (base + 0x270)
+	MMIO_RING_F(RING_REG, 32, 0, 0, 0, D_BDW_PLUS, NULL, NULL);
+	MMIO_F(RING_REG(GEN8_BSD2_RING_BASE), 32, 0, 0, 0, D_BDW_PLUS, NULL, NULL);
+#undef RING_REG
+
+	MMIO_RING_GM(RING_HWS_PGA, D_BDW_PLUS, NULL, NULL);
+	MMIO_GM(0x1c080, D_BDW_PLUS, NULL, NULL);
+
+	MMIO_DFH(HDC_CHICKEN0, D_BDW_PLUS, F_MODE_MASK, NULL, NULL);
+
+	MMIO_D(CHICKEN_PIPESL_1(PIPE_A), D_BDW);
+	MMIO_D(CHICKEN_PIPESL_1(PIPE_B), D_BDW);
+	MMIO_D(CHICKEN_PIPESL_1(PIPE_C), D_BDW);
+
+	MMIO_D(WM_MISC, D_BDW);
+	MMIO_D(BDW_EDP_PSR_BASE, D_BDW);
+
+	MMIO_D(0x66c00, D_BDW_PLUS);
+	MMIO_D(0x66c04, D_BDW_PLUS);
+
+	MMIO_D(HSW_GTT_CACHE_EN, D_BDW_PLUS);
+
+	MMIO_D(GEN8_EU_DISABLE0, D_BDW_PLUS);
+	MMIO_D(GEN8_EU_DISABLE1, D_BDW_PLUS);
+	MMIO_D(GEN8_EU_DISABLE2, D_BDW_PLUS);
+
+	MMIO_D(0xfdc, D_BDW);
+	MMIO_D(GEN8_ROW_CHICKEN, D_BDW_PLUS);
+	MMIO_D(GEN7_ROW_CHICKEN2, D_BDW_PLUS);
+	MMIO_D(GEN8_UCGCTL6, D_BDW_PLUS);
+
+	MMIO_D(0xb1f0, D_BDW);
+	MMIO_D(0xb1c0, D_BDW);
+	MMIO_DFH(GEN8_L3SQCREG4, D_BDW_PLUS, F_CMD_ACCESS, NULL, NULL);
+	MMIO_D(0xb100, D_BDW);
+	MMIO_D(0xb10c, D_BDW);
+	MMIO_D(0xb110, D_BDW);
+
+	MMIO_DH(0x24d0, D_BDW_PLUS, NULL, NULL);
+	MMIO_DH(0x24d4, D_BDW_PLUS, NULL, NULL);
+	MMIO_DH(0x24d8, D_BDW_PLUS, NULL, NULL);
+	MMIO_DH(0x24dc, D_BDW_PLUS, NULL, NULL);
+
+	MMIO_D(0x83a4, D_BDW);
+	MMIO_D(GEN8_L3_LRA_1_GPGPU, D_BDW_PLUS);
+
+	MMIO_D(0x8430, D_BDW);
+
+	MMIO_D(0x110000, D_BDW_PLUS);
+
+	MMIO_D(0x48400, D_BDW_PLUS);
+
+	MMIO_D(0x6e570, D_BDW_PLUS);
+	MMIO_D(0x65f10, D_BDW_PLUS);
+
+	MMIO_DFH(0xe194, D_BDW_PLUS, F_MODE_MASK, NULL, NULL);
+	MMIO_DFH(0xe188, D_BDW_PLUS, F_MODE_MASK, NULL, NULL);
+	MMIO_DFH(0xe180, D_BDW_PLUS, F_MODE_MASK, NULL, NULL);
+	MMIO_DFH(0x2580, D_BDW_PLUS, F_MODE_MASK, NULL, NULL);
+
+	MMIO_D(0x2248, D_BDW);
+
+	return 0;
+}
+
+static int init_skl_mmio_info(struct intel_gvt *gvt)
+{
+	struct drm_i915_private *dev_priv = gvt->dev_priv;
+	int ret;
+
+	MMIO_DH(FORCEWAKE_RENDER_GEN9, D_SKL_PLUS, NULL, mul_force_wake_write);
+	MMIO_DH(FORCEWAKE_ACK_RENDER_GEN9, D_SKL_PLUS, NULL, NULL);
+	MMIO_DH(FORCEWAKE_BLITTER_GEN9, D_SKL_PLUS, NULL, mul_force_wake_write);
+	MMIO_DH(FORCEWAKE_ACK_BLITTER_GEN9, D_SKL_PLUS, NULL, NULL);
+	MMIO_DH(FORCEWAKE_MEDIA_GEN9, D_SKL_PLUS, NULL, mul_force_wake_write);
+	MMIO_DH(FORCEWAKE_ACK_MEDIA_GEN9, D_SKL_PLUS, NULL, NULL);
+
+	MMIO_F(_DPB_AUX_CH_CTL, 6 * 4, 0, 0, 0, D_SKL, NULL, dp_aux_ch_ctl_mmio_write);
+	MMIO_F(_DPC_AUX_CH_CTL, 6 * 4, 0, 0, 0, D_SKL, NULL, dp_aux_ch_ctl_mmio_write);
+	MMIO_F(_DPD_AUX_CH_CTL, 6 * 4, 0, 0, 0, D_SKL, NULL, dp_aux_ch_ctl_mmio_write);
+
+	MMIO_D(HSW_PWR_WELL_BIOS, D_SKL);
+	MMIO_DH(HSW_PWR_WELL_DRIVER, D_SKL, NULL, skl_power_well_ctl_write);
+
+	MMIO_DH(GEN6_PCODE_MAILBOX, D_SKL, NULL, mailbox_write);
+	MMIO_D(0xa210, D_SKL_PLUS);
+	MMIO_D(GEN9_MEDIA_PG_IDLE_HYSTERESIS, D_SKL_PLUS);
+	MMIO_D(GEN9_RENDER_PG_IDLE_HYSTERESIS, D_SKL_PLUS);
+	MMIO_DH(0x4ddc, D_SKL, NULL, skl_misc_ctl_write);
+	MMIO_DH(0x42080, D_SKL, NULL, skl_misc_ctl_write);
+	MMIO_D(0x45504, D_SKL);
+	MMIO_D(0x45520, D_SKL);
+	MMIO_D(0x46000, D_SKL);
+	MMIO_DH(0x46010, D_SKL, NULL, skl_lcpll_write);
+	MMIO_DH(0x46014, D_SKL, NULL, skl_lcpll_write);
+	MMIO_D(0x6C040, D_SKL);
+	MMIO_D(0x6C048, D_SKL);
+	MMIO_D(0x6C050, D_SKL);
+	MMIO_D(0x6C044, D_SKL);
+	MMIO_D(0x6C04C, D_SKL);
+	MMIO_D(0x6C054, D_SKL);
+	MMIO_D(0x6c058, D_SKL);
+	MMIO_D(0x6c05c, D_SKL);
+	MMIO_DH(0X6c060, D_SKL, dpll_status_read, NULL);
+
+	MMIO_DH(SKL_PS_WIN_POS(PIPE_A, 0), D_SKL, NULL, pf_write);
+	MMIO_DH(SKL_PS_WIN_POS(PIPE_A, 1), D_SKL, NULL, pf_write);
+	MMIO_DH(SKL_PS_WIN_POS(PIPE_B, 0), D_SKL, NULL, pf_write);
+	MMIO_DH(SKL_PS_WIN_POS(PIPE_B, 1), D_SKL, NULL, pf_write);
+	MMIO_DH(SKL_PS_WIN_POS(PIPE_C, 0), D_SKL, NULL, pf_write);
+	MMIO_DH(SKL_PS_WIN_POS(PIPE_C, 1), D_SKL, NULL, pf_write);
+
+	MMIO_DH(SKL_PS_WIN_SZ(PIPE_A, 0), D_SKL, NULL, pf_write);
+	MMIO_DH(SKL_PS_WIN_SZ(PIPE_A, 1), D_SKL, NULL, pf_write);
+	MMIO_DH(SKL_PS_WIN_SZ(PIPE_B, 0), D_SKL, NULL, pf_write);
+	MMIO_DH(SKL_PS_WIN_SZ(PIPE_B, 1), D_SKL, NULL, pf_write);
+	MMIO_DH(SKL_PS_WIN_SZ(PIPE_C, 0), D_SKL, NULL, pf_write);
+	MMIO_DH(SKL_PS_WIN_SZ(PIPE_C, 1), D_SKL, NULL, pf_write);
+
+	MMIO_DH(SKL_PS_CTRL(PIPE_A, 0), D_SKL, NULL, pf_write);
+	MMIO_DH(SKL_PS_CTRL(PIPE_A, 1), D_SKL, NULL, pf_write);
+	MMIO_DH(SKL_PS_CTRL(PIPE_B, 0), D_SKL, NULL, pf_write);
+	MMIO_DH(SKL_PS_CTRL(PIPE_B, 1), D_SKL, NULL, pf_write);
+	MMIO_DH(SKL_PS_CTRL(PIPE_C, 0), D_SKL, NULL, pf_write);
+	MMIO_DH(SKL_PS_CTRL(PIPE_C, 1), D_SKL, NULL, pf_write);
+
+	MMIO_DH(PLANE_BUF_CFG(PIPE_A, 0), D_SKL, NULL, NULL);
+	MMIO_DH(PLANE_BUF_CFG(PIPE_A, 1), D_SKL, NULL, NULL);
+	MMIO_DH(PLANE_BUF_CFG(PIPE_A, 2), D_SKL, NULL, NULL);
+	MMIO_DH(PLANE_BUF_CFG(PIPE_A, 3), D_SKL, NULL, NULL);
+
+	MMIO_DH(PLANE_BUF_CFG(PIPE_B, 0), D_SKL, NULL, NULL);
+	MMIO_DH(PLANE_BUF_CFG(PIPE_B, 1), D_SKL, NULL, NULL);
+	MMIO_DH(PLANE_BUF_CFG(PIPE_B, 2), D_SKL, NULL, NULL);
+	MMIO_DH(PLANE_BUF_CFG(PIPE_B, 3), D_SKL, NULL, NULL);
+
+	MMIO_DH(PLANE_BUF_CFG(PIPE_C, 0), D_SKL, NULL, NULL);
+	MMIO_DH(PLANE_BUF_CFG(PIPE_C, 1), D_SKL, NULL, NULL);
+	MMIO_DH(PLANE_BUF_CFG(PIPE_C, 2), D_SKL, NULL, NULL);
+	MMIO_DH(PLANE_BUF_CFG(PIPE_C, 3), D_SKL, NULL, NULL);
+
+	MMIO_DH(CUR_BUF_CFG(PIPE_A), D_SKL, NULL, NULL);
+	MMIO_DH(CUR_BUF_CFG(PIPE_B), D_SKL, NULL, NULL);
+	MMIO_DH(CUR_BUF_CFG(PIPE_C), D_SKL, NULL, NULL);
+
+	MMIO_F(PLANE_WM(PIPE_A, 0, 0), 4 * 8, 0, 0, 0, D_SKL, NULL, NULL);
+	MMIO_F(PLANE_WM(PIPE_A, 1, 0), 4 * 8, 0, 0, 0, D_SKL, NULL, NULL);
+	MMIO_F(PLANE_WM(PIPE_A, 2, 0), 4 * 8, 0, 0, 0, D_SKL, NULL, NULL);
+
+	MMIO_F(PLANE_WM(PIPE_B, 0, 0), 4 * 8, 0, 0, 0, D_SKL, NULL, NULL);
+	MMIO_F(PLANE_WM(PIPE_B, 1, 0), 4 * 8, 0, 0, 0, D_SKL, NULL, NULL);
+	MMIO_F(PLANE_WM(PIPE_B, 2, 0), 4 * 8, 0, 0, 0, D_SKL, NULL, NULL);
+
+	MMIO_F(PLANE_WM(PIPE_C, 0, 0), 4 * 8, 0, 0, 0, D_SKL, NULL, NULL);
+	MMIO_F(PLANE_WM(PIPE_C, 1, 0), 4 * 8, 0, 0, 0, D_SKL, NULL, NULL);
+	MMIO_F(PLANE_WM(PIPE_C, 2, 0), 4 * 8, 0, 0, 0, D_SKL, NULL, NULL);
+
+	MMIO_F(CUR_WM(PIPE_A, 0), 4 * 8, 0, 0, 0, D_SKL, NULL, NULL);
+	MMIO_F(CUR_WM(PIPE_B, 0), 4 * 8, 0, 0, 0, D_SKL, NULL, NULL);
+	MMIO_F(CUR_WM(PIPE_C, 0), 4 * 8, 0, 0, 0, D_SKL, NULL, NULL);
+
+	MMIO_DH(PLANE_WM_TRANS(PIPE_A, 0), D_SKL, NULL, NULL);
+	MMIO_DH(PLANE_WM_TRANS(PIPE_A, 1), D_SKL, NULL, NULL);
+	MMIO_DH(PLANE_WM_TRANS(PIPE_A, 2), D_SKL, NULL, NULL);
+
+	MMIO_DH(PLANE_WM_TRANS(PIPE_B, 0), D_SKL, NULL, NULL);
+	MMIO_DH(PLANE_WM_TRANS(PIPE_B, 1), D_SKL, NULL, NULL);
+	MMIO_DH(PLANE_WM_TRANS(PIPE_B, 2), D_SKL, NULL, NULL);
+
+	MMIO_DH(PLANE_WM_TRANS(PIPE_C, 0), D_SKL, NULL, NULL);
+	MMIO_DH(PLANE_WM_TRANS(PIPE_C, 1), D_SKL, NULL, NULL);
+	MMIO_DH(PLANE_WM_TRANS(PIPE_C, 2), D_SKL, NULL, NULL);
+
+	MMIO_DH(CUR_WM_TRANS(PIPE_A), D_SKL, NULL, NULL);
+	MMIO_DH(CUR_WM_TRANS(PIPE_B), D_SKL, NULL, NULL);
+	MMIO_DH(CUR_WM_TRANS(PIPE_C), D_SKL, NULL, NULL);
+
+	MMIO_DH(PLANE_NV12_BUF_CFG(PIPE_A, 0), D_SKL, NULL, NULL);
+	MMIO_DH(PLANE_NV12_BUF_CFG(PIPE_A, 1), D_SKL, NULL, NULL);
+	MMIO_DH(PLANE_NV12_BUF_CFG(PIPE_A, 2), D_SKL, NULL, NULL);
+	MMIO_DH(PLANE_NV12_BUF_CFG(PIPE_A, 3), D_SKL, NULL, NULL);
+
+	MMIO_DH(PLANE_NV12_BUF_CFG(PIPE_B, 0), D_SKL, NULL, NULL);
+	MMIO_DH(PLANE_NV12_BUF_CFG(PIPE_B, 1), D_SKL, NULL, NULL);
+	MMIO_DH(PLANE_NV12_BUF_CFG(PIPE_B, 2), D_SKL, NULL, NULL);
+	MMIO_DH(PLANE_NV12_BUF_CFG(PIPE_B, 3), D_SKL, NULL, NULL);
+
+	MMIO_DH(PLANE_NV12_BUF_CFG(PIPE_C, 0), D_SKL, NULL, NULL);
+	MMIO_DH(PLANE_NV12_BUF_CFG(PIPE_C, 1), D_SKL, NULL, NULL);
+	MMIO_DH(PLANE_NV12_BUF_CFG(PIPE_C, 2), D_SKL, NULL, NULL);
+	MMIO_DH(PLANE_NV12_BUF_CFG(PIPE_C, 3), D_SKL, NULL, NULL);
+
+	MMIO_DH(_REG_701C0(PIPE_A, 1), D_SKL, NULL, NULL);
+	MMIO_DH(_REG_701C0(PIPE_A, 2), D_SKL, NULL, NULL);
+	MMIO_DH(_REG_701C0(PIPE_A, 3), D_SKL, NULL, NULL);
+	MMIO_DH(_REG_701C0(PIPE_A, 4), D_SKL, NULL, NULL);
+
+	MMIO_DH(_REG_701C0(PIPE_B, 1), D_SKL, NULL, NULL);
+	MMIO_DH(_REG_701C0(PIPE_B, 2), D_SKL, NULL, NULL);
+	MMIO_DH(_REG_701C0(PIPE_B, 3), D_SKL, NULL, NULL);
+	MMIO_DH(_REG_701C0(PIPE_B, 4), D_SKL, NULL, NULL);
+
+	MMIO_DH(_REG_701C0(PIPE_C, 1), D_SKL, NULL, NULL);
+	MMIO_DH(_REG_701C0(PIPE_C, 2), D_SKL, NULL, NULL);
+	MMIO_DH(_REG_701C0(PIPE_C, 3), D_SKL, NULL, NULL);
+	MMIO_DH(_REG_701C0(PIPE_C, 4), D_SKL, NULL, NULL);
+
+	MMIO_DH(_REG_701C4(PIPE_A, 1), D_SKL, NULL, NULL);
+	MMIO_DH(_REG_701C4(PIPE_A, 2), D_SKL, NULL, NULL);
+	MMIO_DH(_REG_701C4(PIPE_A, 3), D_SKL, NULL, NULL);
+	MMIO_DH(_REG_701C4(PIPE_A, 4), D_SKL, NULL, NULL);
+
+	MMIO_DH(_REG_701C4(PIPE_B, 1), D_SKL, NULL, NULL);
+	MMIO_DH(_REG_701C4(PIPE_B, 2), D_SKL, NULL, NULL);
+	MMIO_DH(_REG_701C4(PIPE_B, 3), D_SKL, NULL, NULL);
+	MMIO_DH(_REG_701C4(PIPE_B, 4), D_SKL, NULL, NULL);
+
+	MMIO_DH(_REG_701C4(PIPE_C, 1), D_SKL, NULL, NULL);
+	MMIO_DH(_REG_701C4(PIPE_C, 2), D_SKL, NULL, NULL);
+	MMIO_DH(_REG_701C4(PIPE_C, 3), D_SKL, NULL, NULL);
+	MMIO_DH(_REG_701C4(PIPE_C, 4), D_SKL, NULL, NULL);
+
+	MMIO_D(0x70380, D_SKL);
+	MMIO_D(0x71380, D_SKL);
+	MMIO_D(0x72380, D_SKL);
+	MMIO_D(0x7039c, D_SKL);
+
+	MMIO_F(0x80000, 0x3000, 0, 0, 0, D_SKL, NULL, NULL);
+	MMIO_D(0x8f074, D_SKL);
+	MMIO_D(0x8f004, D_SKL);
+	MMIO_D(0x8f034, D_SKL);
+
+	MMIO_D(0xb11c, D_SKL);
+
+	MMIO_D(0x51000, D_SKL);
+	MMIO_D(0x6c00c, D_SKL);
+
+	MMIO_F(0xc800, 0x7f8, 0, 0, 0, D_SKL, NULL, NULL);
+	MMIO_F(0xb020, 0x80, 0, 0, 0, D_SKL, NULL, NULL);
+
+	MMIO_D(0xd08, D_SKL);
+	MMIO_D(0x20e0, D_SKL);
+	MMIO_D(0x20ec, D_SKL);
+
+	/* TRTT */
+	MMIO_D(0x4de0, D_SKL);
+	MMIO_D(0x4de4, D_SKL);
+	MMIO_D(0x4de8, D_SKL);
+	MMIO_D(0x4dec, D_SKL);
+	MMIO_D(0x4df0, D_SKL);
+	MMIO_DH(0x4df4, D_SKL, NULL, gen9_trtte_write);
+	MMIO_DH(0x4dfc, D_SKL, NULL, gen9_trtt_chicken_write);
+
+	MMIO_D(0x45008, D_SKL);
+
+	MMIO_D(0x46430, D_SKL);
+
+	MMIO_D(0x46520, D_SKL);
+
+	MMIO_D(0xc403c, D_SKL);
+	MMIO_D(0xb004, D_SKL);
+	MMIO_DH(DMA_CTRL, D_SKL_PLUS, NULL, dma_ctrl_write);
+
+	MMIO_D(0x65900, D_SKL);
+	MMIO_D(0x1082c0, D_SKL);
+	MMIO_D(0x4068, D_SKL);
+	MMIO_D(0x67054, D_SKL);
+	MMIO_D(0x6e560, D_SKL);
+	MMIO_D(0x6e554, D_SKL);
+	MMIO_D(0x2b20, D_SKL);
+	MMIO_D(0x65f00, D_SKL);
+	MMIO_D(0x65f08, D_SKL);
+	MMIO_D(0x320f0, D_SKL);
+
+	MMIO_D(_REG_VCS2_EXCC, D_SKL);
+	MMIO_D(0x70034, D_SKL);
+	MMIO_D(0x71034, D_SKL);
+	MMIO_D(0x72034, D_SKL);
+
+	MMIO_D(_PLANE_KEYVAL_1(PIPE_A), D_SKL);
+	MMIO_D(_PLANE_KEYVAL_1(PIPE_B), D_SKL);
+	MMIO_D(_PLANE_KEYVAL_1(PIPE_C), D_SKL);
+	MMIO_D(_PLANE_KEYMSK_1(PIPE_A), D_SKL);
+	MMIO_D(_PLANE_KEYMSK_1(PIPE_B), D_SKL);
+	MMIO_D(_PLANE_KEYMSK_1(PIPE_C), D_SKL);
+
+	MMIO_D(0x44500, D_SKL);
+	return 0;
+}
+
+/**
+ * intel_gvt_find_mmio_info - find MMIO information entry by aligned offset
+ * @gvt: GVT device
+ * @offset: register offset
+ *
+ * This function is used to find the MMIO information entry from hash table
+ *
+ * Returns:
+ * pointer to MMIO information entry, NULL if not exists
+ */
+struct intel_gvt_mmio_info *intel_gvt_find_mmio_info(struct intel_gvt *gvt,
+	unsigned int offset)
+{
+	struct intel_gvt_mmio_info *e;
+
+	WARN_ON(!IS_ALIGNED(offset, 4));
+
+	hash_for_each_possible(gvt->mmio.mmio_info_table, e, node, offset) {
+		if (e->offset == offset)
+			return e;
+	}
+	return NULL;
+}
+
+/**
+ * intel_gvt_clean_mmio_info - clean up MMIO information table for GVT device
+ * @gvt: GVT device
+ *
+ * This function is called at the driver unloading stage, to clean up the MMIO
+ * information table of GVT device
+ *
+ */
+void intel_gvt_clean_mmio_info(struct intel_gvt *gvt)
+{
+	struct hlist_node *tmp;
+	struct intel_gvt_mmio_info *e;
+	int i;
+
+	hash_for_each_safe(gvt->mmio.mmio_info_table, i, tmp, e, node)
+		kfree(e);
+
+	vfree(gvt->mmio.mmio_attribute);
+	gvt->mmio.mmio_attribute = NULL;
+}
+
+/**
+ * intel_gvt_setup_mmio_info - setup MMIO information table for GVT device
+ * @gvt: GVT device
+ *
+ * This function is called at the initialization stage, to setup the MMIO
+ * information table for GVT device
+ *
+ * Returns:
+ * zero on success, negative if failed.
+ */
+int intel_gvt_setup_mmio_info(struct intel_gvt *gvt)
+{
+	struct intel_gvt_device_info *info = &gvt->device_info;
+	struct drm_i915_private *dev_priv = gvt->dev_priv;
+	int ret;
+
+	gvt->mmio.mmio_attribute = vzalloc(info->mmio_size);
+	if (!gvt->mmio.mmio_attribute)
+		return -ENOMEM;
+
+	ret = init_generic_mmio_info(gvt);
+	if (ret)
+		goto err;
+
+	if (IS_BROADWELL(dev_priv)) {
+		ret = init_broadwell_mmio_info(gvt);
+		if (ret)
+			goto err;
+	} else if (IS_SKYLAKE(dev_priv)) {
+		ret = init_broadwell_mmio_info(gvt);
+		if (ret)
+			goto err;
+		ret = init_skl_mmio_info(gvt);
+		if (ret)
+			goto err;
+	}
+	return 0;
+err:
+	intel_gvt_clean_mmio_info(gvt);
+	return ret;
+}
+
+/**
+ * intel_gvt_mmio_set_accessed - mark a MMIO has been accessed
+ * @gvt: a GVT device
+ * @offset: register offset
+ *
+ */
+void intel_gvt_mmio_set_accessed(struct intel_gvt *gvt, unsigned int offset)
+{
+	gvt->mmio.mmio_attribute[offset >> 2] |=
+		F_ACCESSED;
+}
+
+/**
+ * intel_gvt_mmio_is_cmd_accessed - mark a MMIO could be accessed by command
+ * @gvt: a GVT device
+ * @offset: register offset
+ *
+ */
+bool intel_gvt_mmio_is_cmd_access(struct intel_gvt *gvt,
+		unsigned int offset)
+{
+	return gvt->mmio.mmio_attribute[offset >> 2] &
+		F_CMD_ACCESS;
+}
+
+/**
+ * intel_gvt_mmio_is_unalign - mark a MMIO could be accessed unaligned
+ * @gvt: a GVT device
+ * @offset: register offset
+ *
+ */
+bool intel_gvt_mmio_is_unalign(struct intel_gvt *gvt,
+		unsigned int offset)
+{
+	return gvt->mmio.mmio_attribute[offset >> 2] &
+		F_UNALIGN;
+}
+
+/**
+ * intel_gvt_mmio_set_cmd_accessed - mark a MMIO has been accessed by command
+ * @gvt: a GVT device
+ * @offset: register offset
+ *
+ */
+void intel_gvt_mmio_set_cmd_accessed(struct intel_gvt *gvt,
+		unsigned int offset)
+{
+	gvt->mmio.mmio_attribute[offset >> 2] |=
+		F_CMD_ACCESSED;
+}
+
+/**
+ * intel_gvt_mmio_has_mode_mask - if a MMIO has a mode mask
+ * @gvt: a GVT device
+ * @offset: register offset
+ *
+ * Returns:
+ * True if a MMIO has a mode mask in its higher 16 bits, false if it isn't.
+ *
+ */
+bool intel_gvt_mmio_has_mode_mask(struct intel_gvt *gvt, unsigned int offset)
+{
+	return gvt->mmio.mmio_attribute[offset >> 2] &
+		F_MODE_MASK;
+}
+
+/**
+ * intel_vgpu_default_mmio_read - default MMIO read handler
+ * @vgpu: a vGPU
+ * @offset: access offset
+ * @p_data: data return buffer
+ * @bytes: access data length
+ *
+ * Returns:
+ * Zero on success, negative error code if failed.
+ */
+int intel_vgpu_default_mmio_read(struct intel_vgpu *vgpu, unsigned int offset,
+		void *p_data, unsigned int bytes)
+{
+	read_vreg(vgpu, offset, p_data, bytes);
+	return 0;
+}
+
+/**
+ * intel_t_default_mmio_write - default MMIO write handler
+ * @vgpu: a vGPU
+ * @offset: access offset
+ * @p_data: write data buffer
+ * @bytes: access data length
+ *
+ * Returns:
+ * Zero on success, negative error code if failed.
+ */
+int intel_vgpu_default_mmio_write(struct intel_vgpu *vgpu, unsigned int offset,
+		void *p_data, unsigned int bytes)
+{
+	write_vreg(vgpu, offset, p_data, bytes);
+	return 0;
+}
diff --git a/drivers/gpu/drm/i915/gvt/hypercall.h b/drivers/gpu/drm/i915/gvt/hypercall.h
index 254df8bf1f35..027ef558d91c 100644
--- a/drivers/gpu/drm/i915/gvt/hypercall.h
+++ b/drivers/gpu/drm/i915/gvt/hypercall.h
@@ -19,17 +19,51 @@
  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
  * SOFTWARE.
+ *
+ * Authors:
+ *    Eddie Dong <eddie.dong@intel.com>
+ *    Dexuan Cui
+ *    Jike Song <jike.song@intel.com>
+ *
+ * Contributors:
+ *    Zhi Wang <zhi.a.wang@intel.com>
+ *
  */
 
 #ifndef _GVT_HYPERCALL_H_
 #define _GVT_HYPERCALL_H_
 
+struct intel_gvt_io_emulation_ops {
+	int (*emulate_cfg_read)(void *, unsigned int, void *, unsigned int);
+	int (*emulate_cfg_write)(void *, unsigned int, void *, unsigned int);
+	int (*emulate_mmio_read)(void *, u64, void *, unsigned int);
+	int (*emulate_mmio_write)(void *, u64, void *, unsigned int);
+};
+
+extern struct intel_gvt_io_emulation_ops intel_gvt_io_emulation_ops;
+
 /*
  * Specific GVT-g MPT modules function collections. Currently GVT-g supports
  * both Xen and KVM by providing dedicated hypervisor-related MPT modules.
  */
 struct intel_gvt_mpt {
 	int (*detect_host)(void);
+	int (*attach_vgpu)(void *vgpu, unsigned long *handle);
+	void (*detach_vgpu)(unsigned long handle);
+	int (*inject_msi)(unsigned long handle, u32 addr, u16 data);
+	unsigned long (*from_virt_to_mfn)(void *p);
+	int (*set_wp_page)(unsigned long handle, u64 gfn);
+	int (*unset_wp_page)(unsigned long handle, u64 gfn);
+	int (*read_gpa)(unsigned long handle, unsigned long gpa, void *buf,
+			unsigned long len);
+	int (*write_gpa)(unsigned long handle, unsigned long gpa, void *buf,
+			 unsigned long len);
+	unsigned long (*gfn_to_mfn)(unsigned long handle, unsigned long gfn);
+	int (*map_gfn_to_mfn)(unsigned long handle, unsigned long gfn,
+			      unsigned long mfn, unsigned int nr, bool map,
+			      int type);
+	int (*set_trap_area)(unsigned long handle, u64 start, u64 end,
+			     bool map);
 };
 
 extern struct intel_gvt_mpt xengt_mpt;
diff --git a/drivers/gpu/drm/i915/gvt/interrupt.c b/drivers/gpu/drm/i915/gvt/interrupt.c
new file mode 100644
index 000000000000..f7be02ac4be1
--- /dev/null
+++ b/drivers/gpu/drm/i915/gvt/interrupt.c
@@ -0,0 +1,741 @@
+/*
+ * Copyright(c) 2011-2016 Intel Corporation. All rights reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ * Authors:
+ *    Kevin Tian <kevin.tian@intel.com>
+ *    Zhi Wang <zhi.a.wang@intel.com>
+ *
+ * Contributors:
+ *    Min he <min.he@intel.com>
+ *
+ */
+
+#include "i915_drv.h"
+#include "gvt.h"
+
+/* common offset among interrupt control registers */
+#define regbase_to_isr(base)	(base)
+#define regbase_to_imr(base)	(base + 0x4)
+#define regbase_to_iir(base)	(base + 0x8)
+#define regbase_to_ier(base)	(base + 0xC)
+
+#define iir_to_regbase(iir)    (iir - 0x8)
+#define ier_to_regbase(ier)    (ier - 0xC)
+
+#define get_event_virt_handler(irq, e)	(irq->events[e].v_handler)
+#define get_irq_info(irq, e)		(irq->events[e].info)
+
+#define irq_to_gvt(irq) \
+	container_of(irq, struct intel_gvt, irq)
+
+static void update_upstream_irq(struct intel_vgpu *vgpu,
+		struct intel_gvt_irq_info *info);
+
+static const char * const irq_name[INTEL_GVT_EVENT_MAX] = {
+	[RCS_MI_USER_INTERRUPT] = "Render CS MI USER INTERRUPT",
+	[RCS_DEBUG] = "Render EU debug from SVG",
+	[RCS_MMIO_SYNC_FLUSH] = "Render MMIO sync flush status",
+	[RCS_CMD_STREAMER_ERR] = "Render CS error interrupt",
+	[RCS_PIPE_CONTROL] = "Render PIPE CONTROL notify",
+	[RCS_WATCHDOG_EXCEEDED] = "Render CS Watchdog counter exceeded",
+	[RCS_PAGE_DIRECTORY_FAULT] = "Render page directory faults",
+	[RCS_AS_CONTEXT_SWITCH] = "Render AS Context Switch Interrupt",
+
+	[VCS_MI_USER_INTERRUPT] = "Video CS MI USER INTERRUPT",
+	[VCS_MMIO_SYNC_FLUSH] = "Video MMIO sync flush status",
+	[VCS_CMD_STREAMER_ERR] = "Video CS error interrupt",
+	[VCS_MI_FLUSH_DW] = "Video MI FLUSH DW notify",
+	[VCS_WATCHDOG_EXCEEDED] = "Video CS Watchdog counter exceeded",
+	[VCS_PAGE_DIRECTORY_FAULT] = "Video page directory faults",
+	[VCS_AS_CONTEXT_SWITCH] = "Video AS Context Switch Interrupt",
+	[VCS2_MI_USER_INTERRUPT] = "VCS2 Video CS MI USER INTERRUPT",
+	[VCS2_MI_FLUSH_DW] = "VCS2 Video MI FLUSH DW notify",
+	[VCS2_AS_CONTEXT_SWITCH] = "VCS2 Context Switch Interrupt",
+
+	[BCS_MI_USER_INTERRUPT] = "Blitter CS MI USER INTERRUPT",
+	[BCS_MMIO_SYNC_FLUSH] = "Billter MMIO sync flush status",
+	[BCS_CMD_STREAMER_ERR] = "Blitter CS error interrupt",
+	[BCS_MI_FLUSH_DW] = "Blitter MI FLUSH DW notify",
+	[BCS_PAGE_DIRECTORY_FAULT] = "Blitter page directory faults",
+	[BCS_AS_CONTEXT_SWITCH] = "Blitter AS Context Switch Interrupt",
+
+	[VECS_MI_FLUSH_DW] = "Video Enhanced Streamer MI FLUSH DW notify",
+	[VECS_AS_CONTEXT_SWITCH] = "VECS Context Switch Interrupt",
+
+	[PIPE_A_FIFO_UNDERRUN] = "Pipe A FIFO underrun",
+	[PIPE_A_CRC_ERR] = "Pipe A CRC error",
+	[PIPE_A_CRC_DONE] = "Pipe A CRC done",
+	[PIPE_A_VSYNC] = "Pipe A vsync",
+	[PIPE_A_LINE_COMPARE] = "Pipe A line compare",
+	[PIPE_A_ODD_FIELD] = "Pipe A odd field",
+	[PIPE_A_EVEN_FIELD] = "Pipe A even field",
+	[PIPE_A_VBLANK] = "Pipe A vblank",
+	[PIPE_B_FIFO_UNDERRUN] = "Pipe B FIFO underrun",
+	[PIPE_B_CRC_ERR] = "Pipe B CRC error",
+	[PIPE_B_CRC_DONE] = "Pipe B CRC done",
+	[PIPE_B_VSYNC] = "Pipe B vsync",
+	[PIPE_B_LINE_COMPARE] = "Pipe B line compare",
+	[PIPE_B_ODD_FIELD] = "Pipe B odd field",
+	[PIPE_B_EVEN_FIELD] = "Pipe B even field",
+	[PIPE_B_VBLANK] = "Pipe B vblank",
+	[PIPE_C_VBLANK] = "Pipe C vblank",
+	[DPST_PHASE_IN] = "DPST phase in event",
+	[DPST_HISTOGRAM] = "DPST histogram event",
+	[GSE] = "GSE",
+	[DP_A_HOTPLUG] = "DP A Hotplug",
+	[AUX_CHANNEL_A] = "AUX Channel A",
+	[PERF_COUNTER] = "Performance counter",
+	[POISON] = "Poison",
+	[GTT_FAULT] = "GTT fault",
+	[PRIMARY_A_FLIP_DONE] = "Primary Plane A flip done",
+	[PRIMARY_B_FLIP_DONE] = "Primary Plane B flip done",
+	[PRIMARY_C_FLIP_DONE] = "Primary Plane C flip done",
+	[SPRITE_A_FLIP_DONE] = "Sprite Plane A flip done",
+	[SPRITE_B_FLIP_DONE] = "Sprite Plane B flip done",
+	[SPRITE_C_FLIP_DONE] = "Sprite Plane C flip done",
+
+	[PCU_THERMAL] = "PCU Thermal Event",
+	[PCU_PCODE2DRIVER_MAILBOX] = "PCU pcode2driver mailbox event",
+
+	[FDI_RX_INTERRUPTS_TRANSCODER_A] = "FDI RX Interrupts Combined A",
+	[AUDIO_CP_CHANGE_TRANSCODER_A] = "Audio CP Change Transcoder A",
+	[AUDIO_CP_REQUEST_TRANSCODER_A] = "Audio CP Request Transcoder A",
+	[FDI_RX_INTERRUPTS_TRANSCODER_B] = "FDI RX Interrupts Combined B",
+	[AUDIO_CP_CHANGE_TRANSCODER_B] = "Audio CP Change Transcoder B",
+	[AUDIO_CP_REQUEST_TRANSCODER_B] = "Audio CP Request Transcoder B",
+	[FDI_RX_INTERRUPTS_TRANSCODER_C] = "FDI RX Interrupts Combined C",
+	[AUDIO_CP_CHANGE_TRANSCODER_C] = "Audio CP Change Transcoder C",
+	[AUDIO_CP_REQUEST_TRANSCODER_C] = "Audio CP Request Transcoder C",
+	[ERR_AND_DBG] = "South Error and Debug Interupts Combined",
+	[GMBUS] = "Gmbus",
+	[SDVO_B_HOTPLUG] = "SDVO B hotplug",
+	[CRT_HOTPLUG] = "CRT Hotplug",
+	[DP_B_HOTPLUG] = "DisplayPort/HDMI/DVI B Hotplug",
+	[DP_C_HOTPLUG] = "DisplayPort/HDMI/DVI C Hotplug",
+	[DP_D_HOTPLUG] = "DisplayPort/HDMI/DVI D Hotplug",
+	[AUX_CHANNEL_B] = "AUX Channel B",
+	[AUX_CHANNEL_C] = "AUX Channel C",
+	[AUX_CHANNEL_D] = "AUX Channel D",
+	[AUDIO_POWER_STATE_CHANGE_B] = "Audio Power State change Port B",
+	[AUDIO_POWER_STATE_CHANGE_C] = "Audio Power State change Port C",
+	[AUDIO_POWER_STATE_CHANGE_D] = "Audio Power State change Port D",
+
+	[INTEL_GVT_EVENT_RESERVED] = "RESERVED EVENTS!!!",
+};
+
+static inline struct intel_gvt_irq_info *regbase_to_irq_info(
+		struct intel_gvt *gvt,
+		unsigned int reg)
+{
+	struct intel_gvt_irq *irq = &gvt->irq;
+	int i;
+
+	for_each_set_bit(i, irq->irq_info_bitmap, INTEL_GVT_IRQ_INFO_MAX) {
+		if (i915_mmio_reg_offset(irq->info[i]->reg_base) == reg)
+			return irq->info[i];
+	}
+
+	return NULL;
+}
+
+/**
+ * intel_vgpu_reg_imr_handler - Generic IMR register emulation write handler
+ * @vgpu: a vGPU
+ * @reg: register offset written by guest
+ * @p_data: register data written by guest
+ * @bytes: register data length
+ *
+ * This function is used to emulate the generic IMR register bit change
+ * behavior.
+ *
+ * Returns:
+ * Zero on success, negative error code if failed.
+ *
+ */
+int intel_vgpu_reg_imr_handler(struct intel_vgpu *vgpu,
+	unsigned int reg, void *p_data, unsigned int bytes)
+{
+	struct intel_gvt *gvt = vgpu->gvt;
+	struct intel_gvt_irq_ops *ops = gvt->irq.ops;
+	u32 changed, masked, unmasked;
+	u32 imr = *(u32 *)p_data;
+
+	gvt_dbg_irq("write IMR %x with val %x\n",
+		reg, imr);
+
+	gvt_dbg_irq("old vIMR %x\n", vgpu_vreg(vgpu, reg));
+
+	/* figure out newly masked/unmasked bits */
+	changed = vgpu_vreg(vgpu, reg) ^ imr;
+	masked = (vgpu_vreg(vgpu, reg) & changed) ^ changed;
+	unmasked = masked ^ changed;
+
+	gvt_dbg_irq("changed %x, masked %x, unmasked %x\n",
+		changed, masked, unmasked);
+
+	vgpu_vreg(vgpu, reg) = imr;
+
+	ops->check_pending_irq(vgpu);
+	gvt_dbg_irq("IRQ: new vIMR %x\n", vgpu_vreg(vgpu, reg));
+	return 0;
+}
+
+/**
+ * intel_vgpu_reg_master_irq_handler - master IRQ write emulation handler
+ * @vgpu: a vGPU
+ * @reg: register offset written by guest
+ * @p_data: register data written by guest
+ * @bytes: register data length
+ *
+ * This function is used to emulate the master IRQ register on gen8+.
+ *
+ * Returns:
+ * Zero on success, negative error code if failed.
+ *
+ */
+int intel_vgpu_reg_master_irq_handler(struct intel_vgpu *vgpu,
+	unsigned int reg, void *p_data, unsigned int bytes)
+{
+	struct intel_gvt *gvt = vgpu->gvt;
+	struct intel_gvt_irq_ops *ops = gvt->irq.ops;
+	u32 changed, enabled, disabled;
+	u32 ier = *(u32 *)p_data;
+	u32 virtual_ier = vgpu_vreg(vgpu, reg);
+
+	gvt_dbg_irq("write master irq reg %x with val %x\n",
+		reg, ier);
+
+	gvt_dbg_irq("old vreg %x\n", vgpu_vreg(vgpu, reg));
+
+	/*
+	 * GEN8_MASTER_IRQ is a special irq register,
+	 * only bit 31 is allowed to be modified
+	 * and treated as an IER bit.
+	 */
+	ier &= GEN8_MASTER_IRQ_CONTROL;
+	virtual_ier &= GEN8_MASTER_IRQ_CONTROL;
+	vgpu_vreg(vgpu, reg) &= ~GEN8_MASTER_IRQ_CONTROL;
+	vgpu_vreg(vgpu, reg) |= ier;
+
+	/* figure out newly enabled/disable bits */
+	changed = virtual_ier ^ ier;
+	enabled = (virtual_ier & changed) ^ changed;
+	disabled = enabled ^ changed;
+
+	gvt_dbg_irq("changed %x, enabled %x, disabled %x\n",
+			changed, enabled, disabled);
+
+	ops->check_pending_irq(vgpu);
+	gvt_dbg_irq("new vreg %x\n", vgpu_vreg(vgpu, reg));
+	return 0;
+}
+
+/**
+ * intel_vgpu_reg_ier_handler - Generic IER write emulation handler
+ * @vgpu: a vGPU
+ * @reg: register offset written by guest
+ * @p_data: register data written by guest
+ * @bytes: register data length
+ *
+ * This function is used to emulate the generic IER register behavior.
+ *
+ * Returns:
+ * Zero on success, negative error code if failed.
+ *
+ */
+int intel_vgpu_reg_ier_handler(struct intel_vgpu *vgpu,
+	unsigned int reg, void *p_data, unsigned int bytes)
+{
+	struct intel_gvt *gvt = vgpu->gvt;
+	struct intel_gvt_irq_ops *ops = gvt->irq.ops;
+	struct intel_gvt_irq_info *info;
+	u32 changed, enabled, disabled;
+	u32 ier = *(u32 *)p_data;
+
+	gvt_dbg_irq("write IER %x with val %x\n",
+		reg, ier);
+
+	gvt_dbg_irq("old vIER %x\n", vgpu_vreg(vgpu, reg));
+
+	/* figure out newly enabled/disable bits */
+	changed = vgpu_vreg(vgpu, reg) ^ ier;
+	enabled = (vgpu_vreg(vgpu, reg) & changed) ^ changed;
+	disabled = enabled ^ changed;
+
+	gvt_dbg_irq("changed %x, enabled %x, disabled %x\n",
+			changed, enabled, disabled);
+	vgpu_vreg(vgpu, reg) = ier;
+
+	info = regbase_to_irq_info(gvt, ier_to_regbase(reg));
+	if (WARN_ON(!info))
+		return -EINVAL;
+
+	if (info->has_upstream_irq)
+		update_upstream_irq(vgpu, info);
+
+	ops->check_pending_irq(vgpu);
+	gvt_dbg_irq("new vIER %x\n", vgpu_vreg(vgpu, reg));
+	return 0;
+}
+
+/**
+ * intel_vgpu_reg_iir_handler - Generic IIR write emulation handler
+ * @vgpu: a vGPU
+ * @reg: register offset written by guest
+ * @p_data: register data written by guest
+ * @bytes: register data length
+ *
+ * This function is used to emulate the generic IIR register behavior.
+ *
+ * Returns:
+ * Zero on success, negative error code if failed.
+ *
+ */
+int intel_vgpu_reg_iir_handler(struct intel_vgpu *vgpu, unsigned int reg,
+	void *p_data, unsigned int bytes)
+{
+	struct intel_gvt_irq_info *info = regbase_to_irq_info(vgpu->gvt,
+		iir_to_regbase(reg));
+	u32 iir = *(u32 *)p_data;
+
+	gvt_dbg_irq("write IIR %x with val %x\n", reg, iir);
+
+	if (WARN_ON(!info))
+		return -EINVAL;
+
+	vgpu_vreg(vgpu, reg) &= ~iir;
+
+	if (info->has_upstream_irq)
+		update_upstream_irq(vgpu, info);
+	return 0;
+}
+
+static struct intel_gvt_irq_map gen8_irq_map[] = {
+	{ INTEL_GVT_IRQ_INFO_MASTER, 0, INTEL_GVT_IRQ_INFO_GT0, 0xffff },
+	{ INTEL_GVT_IRQ_INFO_MASTER, 1, INTEL_GVT_IRQ_INFO_GT0, 0xffff0000 },
+	{ INTEL_GVT_IRQ_INFO_MASTER, 2, INTEL_GVT_IRQ_INFO_GT1, 0xffff },
+	{ INTEL_GVT_IRQ_INFO_MASTER, 3, INTEL_GVT_IRQ_INFO_GT1, 0xffff0000 },
+	{ INTEL_GVT_IRQ_INFO_MASTER, 4, INTEL_GVT_IRQ_INFO_GT2, 0xffff },
+	{ INTEL_GVT_IRQ_INFO_MASTER, 6, INTEL_GVT_IRQ_INFO_GT3, 0xffff },
+	{ INTEL_GVT_IRQ_INFO_MASTER, 16, INTEL_GVT_IRQ_INFO_DE_PIPE_A, ~0 },
+	{ INTEL_GVT_IRQ_INFO_MASTER, 17, INTEL_GVT_IRQ_INFO_DE_PIPE_B, ~0 },
+	{ INTEL_GVT_IRQ_INFO_MASTER, 18, INTEL_GVT_IRQ_INFO_DE_PIPE_C, ~0 },
+	{ INTEL_GVT_IRQ_INFO_MASTER, 20, INTEL_GVT_IRQ_INFO_DE_PORT, ~0 },
+	{ INTEL_GVT_IRQ_INFO_MASTER, 22, INTEL_GVT_IRQ_INFO_DE_MISC, ~0 },
+	{ INTEL_GVT_IRQ_INFO_MASTER, 23, INTEL_GVT_IRQ_INFO_PCH, ~0 },
+	{ INTEL_GVT_IRQ_INFO_MASTER, 30, INTEL_GVT_IRQ_INFO_PCU, ~0 },
+	{ -1, -1, ~0 },
+};
+
+static void update_upstream_irq(struct intel_vgpu *vgpu,
+		struct intel_gvt_irq_info *info)
+{
+	struct intel_gvt_irq *irq = &vgpu->gvt->irq;
+	struct intel_gvt_irq_map *map = irq->irq_map;
+	struct intel_gvt_irq_info *up_irq_info = NULL;
+	u32 set_bits = 0;
+	u32 clear_bits = 0;
+	int bit;
+	u32 val = vgpu_vreg(vgpu,
+			regbase_to_iir(i915_mmio_reg_offset(info->reg_base)))
+		& vgpu_vreg(vgpu,
+			regbase_to_ier(i915_mmio_reg_offset(info->reg_base)));
+
+	if (!info->has_upstream_irq)
+		return;
+
+	for (map = irq->irq_map; map->up_irq_bit != -1; map++) {
+		if (info->group != map->down_irq_group)
+			continue;
+
+		if (!up_irq_info)
+			up_irq_info = irq->info[map->up_irq_group];
+		else
+			WARN_ON(up_irq_info != irq->info[map->up_irq_group]);
+
+		bit = map->up_irq_bit;
+
+		if (val & map->down_irq_bitmask)
+			set_bits |= (1 << bit);
+		else
+			clear_bits |= (1 << bit);
+	}
+
+	WARN_ON(!up_irq_info);
+
+	if (up_irq_info->group == INTEL_GVT_IRQ_INFO_MASTER) {
+		u32 isr = i915_mmio_reg_offset(up_irq_info->reg_base);
+
+		vgpu_vreg(vgpu, isr) &= ~clear_bits;
+		vgpu_vreg(vgpu, isr) |= set_bits;
+	} else {
+		u32 iir = regbase_to_iir(
+			i915_mmio_reg_offset(up_irq_info->reg_base));
+		u32 imr = regbase_to_imr(
+			i915_mmio_reg_offset(up_irq_info->reg_base));
+
+		vgpu_vreg(vgpu, iir) |= (set_bits & ~vgpu_vreg(vgpu, imr));
+	}
+
+	if (up_irq_info->has_upstream_irq)
+		update_upstream_irq(vgpu, up_irq_info);
+}
+
+static void init_irq_map(struct intel_gvt_irq *irq)
+{
+	struct intel_gvt_irq_map *map;
+	struct intel_gvt_irq_info *up_info, *down_info;
+	int up_bit;
+
+	for (map = irq->irq_map; map->up_irq_bit != -1; map++) {
+		up_info = irq->info[map->up_irq_group];
+		up_bit = map->up_irq_bit;
+		down_info = irq->info[map->down_irq_group];
+
+		set_bit(up_bit, up_info->downstream_irq_bitmap);
+		down_info->has_upstream_irq = true;
+
+		gvt_dbg_irq("[up] grp %d bit %d -> [down] grp %d bitmask %x\n",
+			up_info->group, up_bit,
+			down_info->group, map->down_irq_bitmask);
+	}
+}
+
+/* =======================vEvent injection===================== */
+static int inject_virtual_interrupt(struct intel_vgpu *vgpu)
+{
+	return intel_gvt_hypervisor_inject_msi(vgpu);
+}
+
+static void propagate_event(struct intel_gvt_irq *irq,
+	enum intel_gvt_event_type event, struct intel_vgpu *vgpu)
+{
+	struct intel_gvt_irq_info *info;
+	unsigned int reg_base;
+	int bit;
+
+	info = get_irq_info(irq, event);
+	if (WARN_ON(!info))
+		return;
+
+	reg_base = i915_mmio_reg_offset(info->reg_base);
+	bit = irq->events[event].bit;
+
+	if (!test_bit(bit, (void *)&vgpu_vreg(vgpu,
+					regbase_to_imr(reg_base)))) {
+		gvt_dbg_irq("set bit (%d) for (%s) for vgpu (%d)\n",
+				bit, irq_name[event], vgpu->id);
+		set_bit(bit, (void *)&vgpu_vreg(vgpu,
+					regbase_to_iir(reg_base)));
+	}
+}
+
+/* =======================vEvent Handlers===================== */
+static void handle_default_event_virt(struct intel_gvt_irq *irq,
+	enum intel_gvt_event_type event, struct intel_vgpu *vgpu)
+{
+	if (!vgpu->irq.irq_warn_once[event]) {
+		gvt_dbg_core("vgpu%d: IRQ receive event %d (%s)\n",
+			vgpu->id, event, irq_name[event]);
+		vgpu->irq.irq_warn_once[event] = true;
+	}
+	propagate_event(irq, event, vgpu);
+}
+
+/* =====================GEN specific logic======================= */
+/* GEN8 interrupt routines. */
+
+#define DEFINE_GVT_GEN8_INTEL_GVT_IRQ_INFO(regname, regbase) \
+static struct intel_gvt_irq_info gen8_##regname##_info = { \
+	.name = #regname"-IRQ", \
+	.reg_base = (regbase), \
+	.bit_to_event = {[0 ... INTEL_GVT_IRQ_BITWIDTH-1] = \
+		INTEL_GVT_EVENT_RESERVED}, \
+}
+
+DEFINE_GVT_GEN8_INTEL_GVT_IRQ_INFO(gt0, GEN8_GT_ISR(0));
+DEFINE_GVT_GEN8_INTEL_GVT_IRQ_INFO(gt1, GEN8_GT_ISR(1));
+DEFINE_GVT_GEN8_INTEL_GVT_IRQ_INFO(gt2, GEN8_GT_ISR(2));
+DEFINE_GVT_GEN8_INTEL_GVT_IRQ_INFO(gt3, GEN8_GT_ISR(3));
+DEFINE_GVT_GEN8_INTEL_GVT_IRQ_INFO(de_pipe_a, GEN8_DE_PIPE_ISR(PIPE_A));
+DEFINE_GVT_GEN8_INTEL_GVT_IRQ_INFO(de_pipe_b, GEN8_DE_PIPE_ISR(PIPE_B));
+DEFINE_GVT_GEN8_INTEL_GVT_IRQ_INFO(de_pipe_c, GEN8_DE_PIPE_ISR(PIPE_C));
+DEFINE_GVT_GEN8_INTEL_GVT_IRQ_INFO(de_port, GEN8_DE_PORT_ISR);
+DEFINE_GVT_GEN8_INTEL_GVT_IRQ_INFO(de_misc, GEN8_DE_MISC_ISR);
+DEFINE_GVT_GEN8_INTEL_GVT_IRQ_INFO(pcu, GEN8_PCU_ISR);
+DEFINE_GVT_GEN8_INTEL_GVT_IRQ_INFO(master, GEN8_MASTER_IRQ);
+
+static struct intel_gvt_irq_info gvt_base_pch_info = {
+	.name = "PCH-IRQ",
+	.reg_base = SDEISR,
+	.bit_to_event = {[0 ... INTEL_GVT_IRQ_BITWIDTH-1] =
+		INTEL_GVT_EVENT_RESERVED},
+};
+
+static void gen8_check_pending_irq(struct intel_vgpu *vgpu)
+{
+	struct intel_gvt_irq *irq = &vgpu->gvt->irq;
+	int i;
+
+	if (!(vgpu_vreg(vgpu, i915_mmio_reg_offset(GEN8_MASTER_IRQ)) &
+				GEN8_MASTER_IRQ_CONTROL))
+		return;
+
+	for_each_set_bit(i, irq->irq_info_bitmap, INTEL_GVT_IRQ_INFO_MAX) {
+		struct intel_gvt_irq_info *info = irq->info[i];
+		u32 reg_base;
+
+		if (!info->has_upstream_irq)
+			continue;
+
+		reg_base = i915_mmio_reg_offset(info->reg_base);
+		if ((vgpu_vreg(vgpu, regbase_to_iir(reg_base))
+				& vgpu_vreg(vgpu, regbase_to_ier(reg_base))))
+			update_upstream_irq(vgpu, info);
+	}
+
+	if (vgpu_vreg(vgpu, i915_mmio_reg_offset(GEN8_MASTER_IRQ))
+			& ~GEN8_MASTER_IRQ_CONTROL)
+		inject_virtual_interrupt(vgpu);
+}
+
+static void gen8_init_irq(
+		struct intel_gvt_irq *irq)
+{
+	struct intel_gvt *gvt = irq_to_gvt(irq);
+
+#define SET_BIT_INFO(s, b, e, i)		\
+	do {					\
+		s->events[e].bit = b;		\
+		s->events[e].info = s->info[i];	\
+		s->info[i]->bit_to_event[b] = e;\
+	} while (0)
+
+#define SET_IRQ_GROUP(s, g, i) \
+	do { \
+		s->info[g] = i; \
+		(i)->group = g; \
+		set_bit(g, s->irq_info_bitmap); \
+	} while (0)
+
+	SET_IRQ_GROUP(irq, INTEL_GVT_IRQ_INFO_MASTER, &gen8_master_info);
+	SET_IRQ_GROUP(irq, INTEL_GVT_IRQ_INFO_GT0, &gen8_gt0_info);
+	SET_IRQ_GROUP(irq, INTEL_GVT_IRQ_INFO_GT1, &gen8_gt1_info);
+	SET_IRQ_GROUP(irq, INTEL_GVT_IRQ_INFO_GT2, &gen8_gt2_info);
+	SET_IRQ_GROUP(irq, INTEL_GVT_IRQ_INFO_GT3, &gen8_gt3_info);
+	SET_IRQ_GROUP(irq, INTEL_GVT_IRQ_INFO_DE_PIPE_A, &gen8_de_pipe_a_info);
+	SET_IRQ_GROUP(irq, INTEL_GVT_IRQ_INFO_DE_PIPE_B, &gen8_de_pipe_b_info);
+	SET_IRQ_GROUP(irq, INTEL_GVT_IRQ_INFO_DE_PIPE_C, &gen8_de_pipe_c_info);
+	SET_IRQ_GROUP(irq, INTEL_GVT_IRQ_INFO_DE_PORT, &gen8_de_port_info);
+	SET_IRQ_GROUP(irq, INTEL_GVT_IRQ_INFO_DE_MISC, &gen8_de_misc_info);
+	SET_IRQ_GROUP(irq, INTEL_GVT_IRQ_INFO_PCU, &gen8_pcu_info);
+	SET_IRQ_GROUP(irq, INTEL_GVT_IRQ_INFO_PCH, &gvt_base_pch_info);
+
+	/* GEN8 level 2 interrupts. */
+
+	/* GEN8 interrupt GT0 events */
+	SET_BIT_INFO(irq, 0, RCS_MI_USER_INTERRUPT, INTEL_GVT_IRQ_INFO_GT0);
+	SET_BIT_INFO(irq, 4, RCS_PIPE_CONTROL, INTEL_GVT_IRQ_INFO_GT0);
+	SET_BIT_INFO(irq, 8, RCS_AS_CONTEXT_SWITCH, INTEL_GVT_IRQ_INFO_GT0);
+
+	SET_BIT_INFO(irq, 16, BCS_MI_USER_INTERRUPT, INTEL_GVT_IRQ_INFO_GT0);
+	SET_BIT_INFO(irq, 20, BCS_MI_FLUSH_DW, INTEL_GVT_IRQ_INFO_GT0);
+	SET_BIT_INFO(irq, 24, BCS_AS_CONTEXT_SWITCH, INTEL_GVT_IRQ_INFO_GT0);
+
+	/* GEN8 interrupt GT1 events */
+	SET_BIT_INFO(irq, 0, VCS_MI_USER_INTERRUPT, INTEL_GVT_IRQ_INFO_GT1);
+	SET_BIT_INFO(irq, 4, VCS_MI_FLUSH_DW, INTEL_GVT_IRQ_INFO_GT1);
+	SET_BIT_INFO(irq, 8, VCS_AS_CONTEXT_SWITCH, INTEL_GVT_IRQ_INFO_GT1);
+
+	if (HAS_BSD2(gvt->dev_priv)) {
+		SET_BIT_INFO(irq, 16, VCS2_MI_USER_INTERRUPT,
+			INTEL_GVT_IRQ_INFO_GT1);
+		SET_BIT_INFO(irq, 20, VCS2_MI_FLUSH_DW,
+			INTEL_GVT_IRQ_INFO_GT1);
+		SET_BIT_INFO(irq, 24, VCS2_AS_CONTEXT_SWITCH,
+			INTEL_GVT_IRQ_INFO_GT1);
+	}
+
+	/* GEN8 interrupt GT3 events */
+	SET_BIT_INFO(irq, 0, VECS_MI_USER_INTERRUPT, INTEL_GVT_IRQ_INFO_GT3);
+	SET_BIT_INFO(irq, 4, VECS_MI_FLUSH_DW, INTEL_GVT_IRQ_INFO_GT3);
+	SET_BIT_INFO(irq, 8, VECS_AS_CONTEXT_SWITCH, INTEL_GVT_IRQ_INFO_GT3);
+
+	SET_BIT_INFO(irq, 0, PIPE_A_VBLANK, INTEL_GVT_IRQ_INFO_DE_PIPE_A);
+	SET_BIT_INFO(irq, 0, PIPE_B_VBLANK, INTEL_GVT_IRQ_INFO_DE_PIPE_B);
+	SET_BIT_INFO(irq, 0, PIPE_C_VBLANK, INTEL_GVT_IRQ_INFO_DE_PIPE_C);
+
+	/* GEN8 interrupt DE PORT events */
+	SET_BIT_INFO(irq, 0, AUX_CHANNEL_A, INTEL_GVT_IRQ_INFO_DE_PORT);
+	SET_BIT_INFO(irq, 3, DP_A_HOTPLUG, INTEL_GVT_IRQ_INFO_DE_PORT);
+
+	/* GEN8 interrupt DE MISC events */
+	SET_BIT_INFO(irq, 0, GSE, INTEL_GVT_IRQ_INFO_DE_MISC);
+
+	/* PCH events */
+	SET_BIT_INFO(irq, 17, GMBUS, INTEL_GVT_IRQ_INFO_PCH);
+	SET_BIT_INFO(irq, 19, CRT_HOTPLUG, INTEL_GVT_IRQ_INFO_PCH);
+	SET_BIT_INFO(irq, 21, DP_B_HOTPLUG, INTEL_GVT_IRQ_INFO_PCH);
+	SET_BIT_INFO(irq, 22, DP_C_HOTPLUG, INTEL_GVT_IRQ_INFO_PCH);
+	SET_BIT_INFO(irq, 23, DP_D_HOTPLUG, INTEL_GVT_IRQ_INFO_PCH);
+
+	if (IS_BROADWELL(gvt->dev_priv)) {
+		SET_BIT_INFO(irq, 25, AUX_CHANNEL_B, INTEL_GVT_IRQ_INFO_PCH);
+		SET_BIT_INFO(irq, 26, AUX_CHANNEL_C, INTEL_GVT_IRQ_INFO_PCH);
+		SET_BIT_INFO(irq, 27, AUX_CHANNEL_D, INTEL_GVT_IRQ_INFO_PCH);
+
+		SET_BIT_INFO(irq, 4, PRIMARY_A_FLIP_DONE, INTEL_GVT_IRQ_INFO_DE_PIPE_A);
+		SET_BIT_INFO(irq, 5, SPRITE_A_FLIP_DONE, INTEL_GVT_IRQ_INFO_DE_PIPE_A);
+
+		SET_BIT_INFO(irq, 4, PRIMARY_B_FLIP_DONE, INTEL_GVT_IRQ_INFO_DE_PIPE_B);
+		SET_BIT_INFO(irq, 5, SPRITE_B_FLIP_DONE, INTEL_GVT_IRQ_INFO_DE_PIPE_B);
+
+		SET_BIT_INFO(irq, 4, PRIMARY_C_FLIP_DONE, INTEL_GVT_IRQ_INFO_DE_PIPE_C);
+		SET_BIT_INFO(irq, 5, SPRITE_C_FLIP_DONE, INTEL_GVT_IRQ_INFO_DE_PIPE_C);
+	} else if (IS_SKYLAKE(gvt->dev_priv)) {
+		SET_BIT_INFO(irq, 25, AUX_CHANNEL_B, INTEL_GVT_IRQ_INFO_DE_PORT);
+		SET_BIT_INFO(irq, 26, AUX_CHANNEL_C, INTEL_GVT_IRQ_INFO_DE_PORT);
+		SET_BIT_INFO(irq, 27, AUX_CHANNEL_D, INTEL_GVT_IRQ_INFO_DE_PORT);
+
+		SET_BIT_INFO(irq, 3, PRIMARY_A_FLIP_DONE, INTEL_GVT_IRQ_INFO_DE_PIPE_A);
+		SET_BIT_INFO(irq, 3, PRIMARY_B_FLIP_DONE, INTEL_GVT_IRQ_INFO_DE_PIPE_B);
+		SET_BIT_INFO(irq, 3, PRIMARY_C_FLIP_DONE, INTEL_GVT_IRQ_INFO_DE_PIPE_C);
+	}
+
+	/* GEN8 interrupt PCU events */
+	SET_BIT_INFO(irq, 24, PCU_THERMAL, INTEL_GVT_IRQ_INFO_PCU);
+	SET_BIT_INFO(irq, 25, PCU_PCODE2DRIVER_MAILBOX, INTEL_GVT_IRQ_INFO_PCU);
+}
+
+static struct intel_gvt_irq_ops gen8_irq_ops = {
+	.init_irq = gen8_init_irq,
+	.check_pending_irq = gen8_check_pending_irq,
+};
+
+/**
+ * intel_vgpu_trigger_virtual_event - Trigger a virtual event for a vGPU
+ * @vgpu: a vGPU
+ * @event: interrupt event
+ *
+ * This function is used to trigger a virtual interrupt event for vGPU.
+ * The caller provides the event to be triggered, the framework itself
+ * will emulate the IRQ register bit change.
+ *
+ */
+void intel_vgpu_trigger_virtual_event(struct intel_vgpu *vgpu,
+	enum intel_gvt_event_type event)
+{
+	struct intel_gvt *gvt = vgpu->gvt;
+	struct intel_gvt_irq *irq = &gvt->irq;
+	gvt_event_virt_handler_t handler;
+	struct intel_gvt_irq_ops *ops = gvt->irq.ops;
+
+	handler = get_event_virt_handler(irq, event);
+	WARN_ON(!handler);
+
+	handler(irq, event, vgpu);
+
+	ops->check_pending_irq(vgpu);
+}
+
+static void init_events(
+	struct intel_gvt_irq *irq)
+{
+	int i;
+
+	for (i = 0; i < INTEL_GVT_EVENT_MAX; i++) {
+		irq->events[i].info = NULL;
+		irq->events[i].v_handler = handle_default_event_virt;
+	}
+}
+
+static enum hrtimer_restart vblank_timer_fn(struct hrtimer *data)
+{
+	struct intel_gvt_vblank_timer *vblank_timer;
+	struct intel_gvt_irq *irq;
+	struct intel_gvt *gvt;
+
+	vblank_timer = container_of(data, struct intel_gvt_vblank_timer, timer);
+	irq = container_of(vblank_timer, struct intel_gvt_irq, vblank_timer);
+	gvt = container_of(irq, struct intel_gvt, irq);
+
+	intel_gvt_request_service(gvt, INTEL_GVT_REQUEST_EMULATE_VBLANK);
+	hrtimer_add_expires_ns(&vblank_timer->timer, vblank_timer->period);
+	return HRTIMER_RESTART;
+}
+
+/**
+ * intel_gvt_clean_irq - clean up GVT-g IRQ emulation subsystem
+ * @gvt: a GVT device
+ *
+ * This function is called at driver unloading stage, to clean up GVT-g IRQ
+ * emulation subsystem.
+ *
+ */
+void intel_gvt_clean_irq(struct intel_gvt *gvt)
+{
+	struct intel_gvt_irq *irq = &gvt->irq;
+
+	hrtimer_cancel(&irq->vblank_timer.timer);
+}
+
+#define VBLNAK_TIMER_PERIOD 16000000
+
+/**
+ * intel_gvt_init_irq - initialize GVT-g IRQ emulation subsystem
+ * @gvt: a GVT device
+ *
+ * This function is called at driver loading stage, to initialize the GVT-g IRQ
+ * emulation subsystem.
+ *
+ * Returns:
+ * Zero on success, negative error code if failed.
+ */
+int intel_gvt_init_irq(struct intel_gvt *gvt)
+{
+	struct intel_gvt_irq *irq = &gvt->irq;
+	struct intel_gvt_vblank_timer *vblank_timer = &irq->vblank_timer;
+
+	gvt_dbg_core("init irq framework\n");
+
+	if (IS_BROADWELL(gvt->dev_priv) || IS_SKYLAKE(gvt->dev_priv)) {
+		irq->ops = &gen8_irq_ops;
+		irq->irq_map = gen8_irq_map;
+	} else {
+		WARN_ON(1);
+		return -ENODEV;
+	}
+
+	/* common event initialization */
+	init_events(irq);
+
+	/* gen specific initialization */
+	irq->ops->init_irq(irq);
+
+	init_irq_map(irq);
+
+	hrtimer_init(&vblank_timer->timer, CLOCK_MONOTONIC, HRTIMER_MODE_ABS);
+	vblank_timer->timer.function = vblank_timer_fn;
+	vblank_timer->period = VBLNAK_TIMER_PERIOD;
+
+	return 0;
+}
diff --git a/drivers/gpu/drm/i915/gvt/interrupt.h b/drivers/gpu/drm/i915/gvt/interrupt.h
new file mode 100644
index 000000000000..5313fb1b33e1
--- /dev/null
+++ b/drivers/gpu/drm/i915/gvt/interrupt.h
@@ -0,0 +1,233 @@
+/*
+ * Copyright(c) 2011-2016 Intel Corporation. All rights reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ * Authors:
+ *    Kevin Tian <kevin.tian@intel.com>
+ *    Zhi Wang <zhi.a.wang@intel.com>
+ *
+ * Contributors:
+ *    Min he <min.he@intel.com>
+ *
+ */
+
+#ifndef _GVT_INTERRUPT_H_
+#define _GVT_INTERRUPT_H_
+
+enum intel_gvt_event_type {
+	RCS_MI_USER_INTERRUPT = 0,
+	RCS_DEBUG,
+	RCS_MMIO_SYNC_FLUSH,
+	RCS_CMD_STREAMER_ERR,
+	RCS_PIPE_CONTROL,
+	RCS_L3_PARITY_ERR,
+	RCS_WATCHDOG_EXCEEDED,
+	RCS_PAGE_DIRECTORY_FAULT,
+	RCS_AS_CONTEXT_SWITCH,
+	RCS_MONITOR_BUFF_HALF_FULL,
+
+	VCS_MI_USER_INTERRUPT,
+	VCS_MMIO_SYNC_FLUSH,
+	VCS_CMD_STREAMER_ERR,
+	VCS_MI_FLUSH_DW,
+	VCS_WATCHDOG_EXCEEDED,
+	VCS_PAGE_DIRECTORY_FAULT,
+	VCS_AS_CONTEXT_SWITCH,
+
+	VCS2_MI_USER_INTERRUPT,
+	VCS2_MI_FLUSH_DW,
+	VCS2_AS_CONTEXT_SWITCH,
+
+	BCS_MI_USER_INTERRUPT,
+	BCS_MMIO_SYNC_FLUSH,
+	BCS_CMD_STREAMER_ERR,
+	BCS_MI_FLUSH_DW,
+	BCS_PAGE_DIRECTORY_FAULT,
+	BCS_AS_CONTEXT_SWITCH,
+
+	VECS_MI_USER_INTERRUPT,
+	VECS_MI_FLUSH_DW,
+	VECS_AS_CONTEXT_SWITCH,
+
+	PIPE_A_FIFO_UNDERRUN,
+	PIPE_B_FIFO_UNDERRUN,
+	PIPE_A_CRC_ERR,
+	PIPE_B_CRC_ERR,
+	PIPE_A_CRC_DONE,
+	PIPE_B_CRC_DONE,
+	PIPE_A_ODD_FIELD,
+	PIPE_B_ODD_FIELD,
+	PIPE_A_EVEN_FIELD,
+	PIPE_B_EVEN_FIELD,
+	PIPE_A_LINE_COMPARE,
+	PIPE_B_LINE_COMPARE,
+	PIPE_C_LINE_COMPARE,
+	PIPE_A_VBLANK,
+	PIPE_B_VBLANK,
+	PIPE_C_VBLANK,
+	PIPE_A_VSYNC,
+	PIPE_B_VSYNC,
+	PIPE_C_VSYNC,
+	PRIMARY_A_FLIP_DONE,
+	PRIMARY_B_FLIP_DONE,
+	PRIMARY_C_FLIP_DONE,
+	SPRITE_A_FLIP_DONE,
+	SPRITE_B_FLIP_DONE,
+	SPRITE_C_FLIP_DONE,
+
+	PCU_THERMAL,
+	PCU_PCODE2DRIVER_MAILBOX,
+
+	DPST_PHASE_IN,
+	DPST_HISTOGRAM,
+	GSE,
+	DP_A_HOTPLUG,
+	AUX_CHANNEL_A,
+	PERF_COUNTER,
+	POISON,
+	GTT_FAULT,
+	ERROR_INTERRUPT_COMBINED,
+
+	FDI_RX_INTERRUPTS_TRANSCODER_A,
+	AUDIO_CP_CHANGE_TRANSCODER_A,
+	AUDIO_CP_REQUEST_TRANSCODER_A,
+	FDI_RX_INTERRUPTS_TRANSCODER_B,
+	AUDIO_CP_CHANGE_TRANSCODER_B,
+	AUDIO_CP_REQUEST_TRANSCODER_B,
+	FDI_RX_INTERRUPTS_TRANSCODER_C,
+	AUDIO_CP_CHANGE_TRANSCODER_C,
+	AUDIO_CP_REQUEST_TRANSCODER_C,
+	ERR_AND_DBG,
+	GMBUS,
+	SDVO_B_HOTPLUG,
+	CRT_HOTPLUG,
+	DP_B_HOTPLUG,
+	DP_C_HOTPLUG,
+	DP_D_HOTPLUG,
+	AUX_CHANNEL_B,
+	AUX_CHANNEL_C,
+	AUX_CHANNEL_D,
+	AUDIO_POWER_STATE_CHANGE_B,
+	AUDIO_POWER_STATE_CHANGE_C,
+	AUDIO_POWER_STATE_CHANGE_D,
+
+	INTEL_GVT_EVENT_RESERVED,
+	INTEL_GVT_EVENT_MAX,
+};
+
+struct intel_gvt_irq;
+struct intel_gvt;
+
+typedef void (*gvt_event_virt_handler_t)(struct intel_gvt_irq *irq,
+	enum intel_gvt_event_type event, struct intel_vgpu *vgpu);
+
+struct intel_gvt_irq_ops {
+	void (*init_irq)(struct intel_gvt_irq *irq);
+	void (*check_pending_irq)(struct intel_vgpu *vgpu);
+};
+
+/* the list of physical interrupt control register groups */
+enum intel_gvt_irq_type {
+	INTEL_GVT_IRQ_INFO_GT,
+	INTEL_GVT_IRQ_INFO_DPY,
+	INTEL_GVT_IRQ_INFO_PCH,
+	INTEL_GVT_IRQ_INFO_PM,
+
+	INTEL_GVT_IRQ_INFO_MASTER,
+	INTEL_GVT_IRQ_INFO_GT0,
+	INTEL_GVT_IRQ_INFO_GT1,
+	INTEL_GVT_IRQ_INFO_GT2,
+	INTEL_GVT_IRQ_INFO_GT3,
+	INTEL_GVT_IRQ_INFO_DE_PIPE_A,
+	INTEL_GVT_IRQ_INFO_DE_PIPE_B,
+	INTEL_GVT_IRQ_INFO_DE_PIPE_C,
+	INTEL_GVT_IRQ_INFO_DE_PORT,
+	INTEL_GVT_IRQ_INFO_DE_MISC,
+	INTEL_GVT_IRQ_INFO_AUD,
+	INTEL_GVT_IRQ_INFO_PCU,
+
+	INTEL_GVT_IRQ_INFO_MAX,
+};
+
+#define INTEL_GVT_IRQ_BITWIDTH	32
+
+/* device specific interrupt bit definitions */
+struct intel_gvt_irq_info {
+	char *name;
+	i915_reg_t reg_base;
+	enum intel_gvt_event_type bit_to_event[INTEL_GVT_IRQ_BITWIDTH];
+	unsigned long warned;
+	int group;
+	DECLARE_BITMAP(downstream_irq_bitmap, INTEL_GVT_IRQ_BITWIDTH);
+	bool has_upstream_irq;
+};
+
+/* per-event information */
+struct intel_gvt_event_info {
+	int bit;				/* map to register bit */
+	int policy;				/* forwarding policy */
+	struct intel_gvt_irq_info *info;	/* register info */
+	gvt_event_virt_handler_t v_handler;	/* for v_event */
+};
+
+struct intel_gvt_irq_map {
+	int up_irq_group;
+	int up_irq_bit;
+	int down_irq_group;
+	u32 down_irq_bitmask;
+};
+
+struct intel_gvt_vblank_timer {
+	struct hrtimer timer;
+	u64 period;
+};
+
+/* structure containing device specific IRQ state */
+struct intel_gvt_irq {
+	struct intel_gvt_irq_ops *ops;
+	struct intel_gvt_irq_info *info[INTEL_GVT_IRQ_INFO_MAX];
+	DECLARE_BITMAP(irq_info_bitmap, INTEL_GVT_IRQ_INFO_MAX);
+	struct intel_gvt_event_info events[INTEL_GVT_EVENT_MAX];
+	DECLARE_BITMAP(pending_events, INTEL_GVT_EVENT_MAX);
+	struct intel_gvt_irq_map *irq_map;
+	struct intel_gvt_vblank_timer vblank_timer;
+};
+
+int intel_gvt_init_irq(struct intel_gvt *gvt);
+void intel_gvt_clean_irq(struct intel_gvt *gvt);
+
+void intel_vgpu_trigger_virtual_event(struct intel_vgpu *vgpu,
+	enum intel_gvt_event_type event);
+
+int intel_vgpu_reg_iir_handler(struct intel_vgpu *vgpu, unsigned int reg,
+	void *p_data, unsigned int bytes);
+int intel_vgpu_reg_ier_handler(struct intel_vgpu *vgpu,
+	unsigned int reg, void *p_data, unsigned int bytes);
+int intel_vgpu_reg_master_irq_handler(struct intel_vgpu *vgpu,
+	unsigned int reg, void *p_data, unsigned int bytes);
+int intel_vgpu_reg_imr_handler(struct intel_vgpu *vgpu,
+	unsigned int reg, void *p_data, unsigned int bytes);
+
+int gvt_ring_id_to_pipe_control_notify_event(int ring_id);
+int gvt_ring_id_to_mi_flush_dw_event(int ring_id);
+int gvt_ring_id_to_mi_user_interrupt_event(int ring_id);
+
+#endif /* _GVT_INTERRUPT_H_ */
diff --git a/drivers/gpu/drm/i915/gvt/mmio.c b/drivers/gpu/drm/i915/gvt/mmio.c
new file mode 100644
index 000000000000..585b01f63254
--- /dev/null
+++ b/drivers/gpu/drm/i915/gvt/mmio.c
@@ -0,0 +1,306 @@
+/*
+ * Copyright(c) 2011-2016 Intel Corporation. All rights reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ * Authors:
+ *    Ke Yu
+ *    Kevin Tian <kevin.tian@intel.com>
+ *    Dexuan Cui
+ *
+ * Contributors:
+ *    Tina Zhang <tina.zhang@intel.com>
+ *    Min He <min.he@intel.com>
+ *    Niu Bing <bing.niu@intel.com>
+ *    Zhi Wang <zhi.a.wang@intel.com>
+ *
+ */
+
+#include "i915_drv.h"
+#include "gvt.h"
+
+/**
+ * intel_vgpu_gpa_to_mmio_offset - translate a GPA to MMIO offset
+ * @vgpu: a vGPU
+ *
+ * Returns:
+ * Zero on success, negative error code if failed
+ */
+int intel_vgpu_gpa_to_mmio_offset(struct intel_vgpu *vgpu, u64 gpa)
+{
+	u64 gttmmio_gpa = *(u64 *)(vgpu_cfg_space(vgpu) + PCI_BASE_ADDRESS_0) &
+			  ~GENMASK(3, 0);
+	return gpa - gttmmio_gpa;
+}
+
+#define reg_is_mmio(gvt, reg)  \
+	(reg >= 0 && reg < gvt->device_info.mmio_size)
+
+#define reg_is_gtt(gvt, reg)   \
+	(reg >= gvt->device_info.gtt_start_offset \
+	 && reg < gvt->device_info.gtt_start_offset + gvt_ggtt_sz(gvt))
+
+/**
+ * intel_vgpu_emulate_mmio_read - emulate MMIO read
+ * @vgpu: a vGPU
+ * @pa: guest physical address
+ * @p_data: data return buffer
+ * @bytes: access data length
+ *
+ * Returns:
+ * Zero on success, negative error code if failed
+ */
+int intel_vgpu_emulate_mmio_read(void *__vgpu, uint64_t pa,
+		void *p_data, unsigned int bytes)
+{
+	struct intel_vgpu *vgpu = __vgpu;
+	struct intel_gvt *gvt = vgpu->gvt;
+	struct intel_gvt_mmio_info *mmio;
+	unsigned int offset = 0;
+	int ret = -EINVAL;
+
+	mutex_lock(&gvt->lock);
+
+	if (atomic_read(&vgpu->gtt.n_write_protected_guest_page)) {
+		struct intel_vgpu_guest_page *gp;
+
+		gp = intel_vgpu_find_guest_page(vgpu, pa >> PAGE_SHIFT);
+		if (gp) {
+			ret = intel_gvt_hypervisor_read_gpa(vgpu, pa,
+					p_data, bytes);
+			if (ret) {
+				gvt_err("vgpu%d: guest page read error %d, "
+					"gfn 0x%lx, pa 0x%llx, var 0x%x, len %d\n",
+					vgpu->id, ret,
+					gp->gfn, pa, *(u32 *)p_data, bytes);
+			}
+			mutex_unlock(&gvt->lock);
+			return ret;
+		}
+	}
+
+	offset = intel_vgpu_gpa_to_mmio_offset(vgpu, pa);
+
+	if (WARN_ON(bytes > 8))
+		goto err;
+
+	if (reg_is_gtt(gvt, offset)) {
+		if (WARN_ON(!IS_ALIGNED(offset, 4) && !IS_ALIGNED(offset, 8)))
+			goto err;
+		if (WARN_ON(bytes != 4 && bytes != 8))
+			goto err;
+		if (WARN_ON(!reg_is_gtt(gvt, offset + bytes - 1)))
+			goto err;
+
+		ret = intel_vgpu_emulate_gtt_mmio_read(vgpu, offset,
+				p_data, bytes);
+		if (ret)
+			goto err;
+		mutex_unlock(&gvt->lock);
+		return ret;
+	}
+
+	if (WARN_ON_ONCE(!reg_is_mmio(gvt, offset))) {
+		ret = intel_gvt_hypervisor_read_gpa(vgpu, pa, p_data, bytes);
+		mutex_unlock(&gvt->lock);
+		return ret;
+	}
+
+	if (WARN_ON(!reg_is_mmio(gvt, offset + bytes - 1)))
+		goto err;
+
+	mmio = intel_gvt_find_mmio_info(gvt, rounddown(offset, 4));
+	if (!mmio && !vgpu->mmio.disable_warn_untrack) {
+		gvt_err("vgpu%d: read untracked MMIO %x len %d val %x\n",
+				vgpu->id, offset, bytes, *(u32 *)p_data);
+
+		if (offset == 0x206c) {
+			gvt_err("------------------------------------------\n");
+			gvt_err("vgpu%d: likely triggers a gfx reset\n",
+			vgpu->id);
+			gvt_err("------------------------------------------\n");
+			vgpu->mmio.disable_warn_untrack = true;
+		}
+	}
+
+	if (!intel_gvt_mmio_is_unalign(gvt, offset)) {
+		if (WARN_ON(!IS_ALIGNED(offset, bytes)))
+			goto err;
+	}
+
+	if (mmio) {
+		if (!intel_gvt_mmio_is_unalign(gvt, mmio->offset)) {
+			if (WARN_ON(offset + bytes > mmio->offset + mmio->size))
+				goto err;
+			if (WARN_ON(mmio->offset != offset))
+				goto err;
+		}
+		ret = mmio->read(vgpu, offset, p_data, bytes);
+	} else
+		ret = intel_vgpu_default_mmio_read(vgpu, offset, p_data, bytes);
+
+	if (ret)
+		goto err;
+
+	intel_gvt_mmio_set_accessed(gvt, offset);
+	mutex_unlock(&gvt->lock);
+	return 0;
+err:
+	gvt_err("vgpu%d: fail to emulate MMIO read %08x len %d\n",
+			vgpu->id, offset, bytes);
+	mutex_unlock(&gvt->lock);
+	return ret;
+}
+
+/**
+ * intel_vgpu_emulate_mmio_write - emulate MMIO write
+ * @vgpu: a vGPU
+ * @pa: guest physical address
+ * @p_data: write data buffer
+ * @bytes: access data length
+ *
+ * Returns:
+ * Zero on success, negative error code if failed
+ */
+int intel_vgpu_emulate_mmio_write(void *__vgpu, uint64_t pa,
+		void *p_data, unsigned int bytes)
+{
+	struct intel_vgpu *vgpu = __vgpu;
+	struct intel_gvt *gvt = vgpu->gvt;
+	struct intel_gvt_mmio_info *mmio;
+	unsigned int offset = 0;
+	u32 old_vreg = 0, old_sreg = 0;
+	int ret = -EINVAL;
+
+	mutex_lock(&gvt->lock);
+
+	if (atomic_read(&vgpu->gtt.n_write_protected_guest_page)) {
+		struct intel_vgpu_guest_page *gp;
+
+		gp = intel_vgpu_find_guest_page(vgpu, pa >> PAGE_SHIFT);
+		if (gp) {
+			ret = gp->handler(gp, pa, p_data, bytes);
+			if (ret) {
+				gvt_err("vgpu%d: guest page write error %d, "
+					"gfn 0x%lx, pa 0x%llx, var 0x%x, len %d\n",
+					vgpu->id, ret,
+					gp->gfn, pa, *(u32 *)p_data, bytes);
+			}
+			mutex_unlock(&gvt->lock);
+			return ret;
+		}
+	}
+
+	offset = intel_vgpu_gpa_to_mmio_offset(vgpu, pa);
+
+	if (WARN_ON(bytes > 8))
+		goto err;
+
+	if (reg_is_gtt(gvt, offset)) {
+		if (WARN_ON(!IS_ALIGNED(offset, 4) && !IS_ALIGNED(offset, 8)))
+			goto err;
+		if (WARN_ON(bytes != 4 && bytes != 8))
+			goto err;
+		if (WARN_ON(!reg_is_gtt(gvt, offset + bytes - 1)))
+			goto err;
+
+		ret = intel_vgpu_emulate_gtt_mmio_write(vgpu, offset,
+				p_data, bytes);
+		if (ret)
+			goto err;
+		mutex_unlock(&gvt->lock);
+		return ret;
+	}
+
+	if (WARN_ON_ONCE(!reg_is_mmio(gvt, offset))) {
+		ret = intel_gvt_hypervisor_write_gpa(vgpu, pa, p_data, bytes);
+		mutex_unlock(&gvt->lock);
+		return ret;
+	}
+
+	mmio = intel_gvt_find_mmio_info(gvt, rounddown(offset, 4));
+	if (!mmio && !vgpu->mmio.disable_warn_untrack)
+		gvt_err("vgpu%d: write untracked MMIO %x len %d val %x\n",
+				vgpu->id, offset, bytes, *(u32 *)p_data);
+
+	if (!intel_gvt_mmio_is_unalign(gvt, offset)) {
+		if (WARN_ON(!IS_ALIGNED(offset, bytes)))
+			goto err;
+	}
+
+	if (mmio) {
+		u64 ro_mask = mmio->ro_mask;
+
+		if (!intel_gvt_mmio_is_unalign(gvt, mmio->offset)) {
+			if (WARN_ON(offset + bytes > mmio->offset + mmio->size))
+				goto err;
+			if (WARN_ON(mmio->offset != offset))
+				goto err;
+		}
+
+		if (intel_gvt_mmio_has_mode_mask(gvt, mmio->offset)) {
+			old_vreg = vgpu_vreg(vgpu, offset);
+			old_sreg = vgpu_sreg(vgpu, offset);
+		}
+
+		if (!ro_mask) {
+			ret = mmio->write(vgpu, offset, p_data, bytes);
+		} else {
+			/* Protect RO bits like HW */
+			u64 data = 0;
+
+			/* all register bits are RO. */
+			if (ro_mask == ~(u64)0) {
+				gvt_err("vgpu%d: try to write RO reg %x\n",
+						vgpu->id, offset);
+				ret = 0;
+				goto out;
+			}
+			/* keep the RO bits in the virtual register */
+			memcpy(&data, p_data, bytes);
+			data &= ~mmio->ro_mask;
+			data |= vgpu_vreg(vgpu, offset) & mmio->ro_mask;
+			ret = mmio->write(vgpu, offset, &data, bytes);
+		}
+
+		/* higher 16bits of mode ctl regs are mask bits for change */
+		if (intel_gvt_mmio_has_mode_mask(gvt, mmio->offset)) {
+			u32 mask = vgpu_vreg(vgpu, offset) >> 16;
+
+			vgpu_vreg(vgpu, offset) = (old_vreg & ~mask)
+				| (vgpu_vreg(vgpu, offset) & mask);
+			vgpu_sreg(vgpu, offset) = (old_sreg & ~mask)
+				| (vgpu_sreg(vgpu, offset) & mask);
+		}
+	} else
+		ret = intel_vgpu_default_mmio_write(vgpu, offset, p_data,
+				bytes);
+	if (ret)
+		goto err;
+out:
+	intel_gvt_mmio_set_accessed(gvt, offset);
+	mutex_unlock(&gvt->lock);
+	return 0;
+err:
+	gvt_err("vgpu%d: fail to emulate MMIO write %08x len %d\n",
+			vgpu->id, offset, bytes);
+	mutex_unlock(&gvt->lock);
+	return ret;
+}
diff --git a/drivers/gpu/drm/i915/gvt/mmio.h b/drivers/gpu/drm/i915/gvt/mmio.h
new file mode 100644
index 000000000000..9dc739a01892
--- /dev/null
+++ b/drivers/gpu/drm/i915/gvt/mmio.h
@@ -0,0 +1,105 @@
+/*
+ * Copyright(c) 2011-2016 Intel Corporation. All rights reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ * Authors:
+ *    Ke Yu
+ *    Kevin Tian <kevin.tian@intel.com>
+ *    Dexuan Cui
+ *
+ * Contributors:
+ *    Tina Zhang <tina.zhang@intel.com>
+ *    Min He <min.he@intel.com>
+ *    Niu Bing <bing.niu@intel.com>
+ *    Zhi Wang <zhi.a.wang@intel.com>
+ *
+ */
+
+#ifndef _GVT_MMIO_H_
+#define _GVT_MMIO_H_
+
+struct intel_gvt;
+struct intel_vgpu;
+
+#define D_SNB   (1 << 0)
+#define D_IVB   (1 << 1)
+#define D_HSW   (1 << 2)
+#define D_BDW   (1 << 3)
+#define D_SKL	(1 << 4)
+
+#define D_GEN9PLUS	(D_SKL)
+#define D_GEN8PLUS	(D_BDW | D_SKL)
+#define D_GEN75PLUS	(D_HSW | D_BDW | D_SKL)
+#define D_GEN7PLUS	(D_IVB | D_HSW | D_BDW | D_SKL)
+
+#define D_SKL_PLUS	(D_SKL)
+#define D_BDW_PLUS	(D_BDW | D_SKL)
+#define D_HSW_PLUS	(D_HSW | D_BDW | D_SKL)
+#define D_IVB_PLUS	(D_IVB | D_HSW | D_BDW | D_SKL)
+
+#define D_PRE_BDW	(D_SNB | D_IVB | D_HSW)
+#define D_PRE_SKL	(D_SNB | D_IVB | D_HSW | D_BDW)
+#define D_ALL		(D_SNB | D_IVB | D_HSW | D_BDW | D_SKL)
+
+struct intel_gvt_mmio_info {
+	u32 offset;
+	u32 size;
+	u32 length;
+	u32 addr_mask;
+	u64 ro_mask;
+	u32 device;
+	int (*read)(struct intel_vgpu *, unsigned int, void *, unsigned int);
+	int (*write)(struct intel_vgpu *, unsigned int, void *, unsigned int);
+	u32 addr_range;
+	struct hlist_node node;
+};
+
+unsigned long intel_gvt_get_device_type(struct intel_gvt *gvt);
+bool intel_gvt_match_device(struct intel_gvt *gvt, unsigned long device);
+
+int intel_gvt_setup_mmio_info(struct intel_gvt *gvt);
+void intel_gvt_clean_mmio_info(struct intel_gvt *gvt);
+
+struct intel_gvt_mmio_info *intel_gvt_find_mmio_info(struct intel_gvt *gvt,
+						     unsigned int offset);
+#define INTEL_GVT_MMIO_OFFSET(reg) ({ \
+	typeof(reg) __reg = reg; \
+	u32 *offset = (u32 *)&__reg; \
+	*offset; \
+})
+
+int intel_vgpu_gpa_to_mmio_offset(struct intel_vgpu *vgpu, u64 gpa);
+int intel_vgpu_emulate_mmio_read(void *__vgpu, u64 pa, void *p_data,
+				 unsigned int bytes);
+int intel_vgpu_emulate_mmio_write(void *__vgpu, u64 pa, void *p_data,
+				  unsigned int bytes);
+bool intel_gvt_mmio_is_cmd_access(struct intel_gvt *gvt,
+				  unsigned int offset);
+bool intel_gvt_mmio_is_unalign(struct intel_gvt *gvt, unsigned int offset);
+void intel_gvt_mmio_set_accessed(struct intel_gvt *gvt, unsigned int offset);
+void intel_gvt_mmio_set_cmd_accessed(struct intel_gvt *gvt,
+				     unsigned int offset);
+bool intel_gvt_mmio_has_mode_mask(struct intel_gvt *gvt, unsigned int offset);
+int intel_vgpu_default_mmio_read(struct intel_vgpu *vgpu, unsigned int offset,
+				 void *p_data, unsigned int bytes);
+int intel_vgpu_default_mmio_write(struct intel_vgpu *vgpu, unsigned int offset,
+				  void *p_data, unsigned int bytes);
+#endif
diff --git a/drivers/gpu/drm/i915/gvt/mpt.h b/drivers/gpu/drm/i915/gvt/mpt.h
index 03601e3ffa7c..67858782d327 100644
--- a/drivers/gpu/drm/i915/gvt/mpt.h
+++ b/drivers/gpu/drm/i915/gvt/mpt.h
@@ -19,6 +19,15 @@
  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
  * SOFTWARE.
+ *
+ * Authors:
+ *    Eddie Dong <eddie.dong@intel.com>
+ *    Dexuan Cui
+ *    Jike Song <jike.song@intel.com>
+ *
+ * Contributors:
+ *    Zhi Wang <zhi.a.wang@intel.com>
+ *
  */
 
 #ifndef _GVT_MPT_H_
@@ -46,4 +55,215 @@ static inline int intel_gvt_hypervisor_detect_host(void)
 	return intel_gvt_host.mpt->detect_host();
 }
 
+/**
+ * intel_gvt_hypervisor_attach_vgpu - call hypervisor to initialize vGPU
+ * related stuffs inside hypervisor.
+ *
+ * Returns:
+ * Zero on success, negative error code if failed.
+ */
+static inline int intel_gvt_hypervisor_attach_vgpu(struct intel_vgpu *vgpu)
+{
+	return intel_gvt_host.mpt->attach_vgpu(vgpu, &vgpu->handle);
+}
+
+/**
+ * intel_gvt_hypervisor_detach_vgpu - call hypervisor to release vGPU
+ * related stuffs inside hypervisor.
+ *
+ * Returns:
+ * Zero on success, negative error code if failed.
+ */
+static inline void intel_gvt_hypervisor_detach_vgpu(struct intel_vgpu *vgpu)
+{
+	intel_gvt_host.mpt->detach_vgpu(vgpu->handle);
+}
+
+#define MSI_CAP_CONTROL(offset) (offset + 2)
+#define MSI_CAP_ADDRESS(offset) (offset + 4)
+#define MSI_CAP_DATA(offset) (offset + 8)
+#define MSI_CAP_EN 0x1
+
+/**
+ * intel_gvt_hypervisor_inject_msi - inject a MSI interrupt into vGPU
+ *
+ * Returns:
+ * Zero on success, negative error code if failed.
+ */
+static inline int intel_gvt_hypervisor_inject_msi(struct intel_vgpu *vgpu)
+{
+	unsigned long offset = vgpu->gvt->device_info.msi_cap_offset;
+	u16 control, data;
+	u32 addr;
+	int ret;
+
+	control = *(u16 *)(vgpu_cfg_space(vgpu) + MSI_CAP_CONTROL(offset));
+	addr = *(u32 *)(vgpu_cfg_space(vgpu) + MSI_CAP_ADDRESS(offset));
+	data = *(u16 *)(vgpu_cfg_space(vgpu) + MSI_CAP_DATA(offset));
+
+	/* Do not generate MSI if MSIEN is disable */
+	if (!(control & MSI_CAP_EN))
+		return 0;
+
+	if (WARN(control & GENMASK(15, 1), "only support one MSI format\n"))
+		return -EINVAL;
+
+	gvt_dbg_irq("vgpu%d: inject msi address %x data%x\n", vgpu->id, addr,
+		    data);
+
+	ret = intel_gvt_host.mpt->inject_msi(vgpu->handle, addr, data);
+	if (ret)
+		return ret;
+	return 0;
+}
+
+/**
+ * intel_gvt_hypervisor_set_wp_page - translate a host VA into MFN
+ * @p: host kernel virtual address
+ *
+ * Returns:
+ * MFN on success, INTEL_GVT_INVALID_ADDR if failed.
+ */
+static inline unsigned long intel_gvt_hypervisor_virt_to_mfn(void *p)
+{
+	return intel_gvt_host.mpt->from_virt_to_mfn(p);
+}
+
+/**
+ * intel_gvt_hypervisor_set_wp_page - set a guest page to write-protected
+ * @vgpu: a vGPU
+ * @p: intel_vgpu_guest_page
+ *
+ * Returns:
+ * Zero on success, negative error code if failed.
+ */
+static inline int intel_gvt_hypervisor_set_wp_page(struct intel_vgpu *vgpu,
+		struct intel_vgpu_guest_page *p)
+{
+	int ret;
+
+	if (p->writeprotection)
+		return 0;
+
+	ret = intel_gvt_host.mpt->set_wp_page(vgpu->handle, p->gfn);
+	if (ret)
+		return ret;
+	p->writeprotection = true;
+	atomic_inc(&vgpu->gtt.n_write_protected_guest_page);
+	return 0;
+}
+
+/**
+ * intel_gvt_hypervisor_unset_wp_page - remove the write-protection of a
+ * guest page
+ * @vgpu: a vGPU
+ * @p: intel_vgpu_guest_page
+ *
+ * Returns:
+ * Zero on success, negative error code if failed.
+ */
+static inline int intel_gvt_hypervisor_unset_wp_page(struct intel_vgpu *vgpu,
+		struct intel_vgpu_guest_page *p)
+{
+	int ret;
+
+	if (!p->writeprotection)
+		return 0;
+
+	ret = intel_gvt_host.mpt->unset_wp_page(vgpu->handle, p->gfn);
+	if (ret)
+		return ret;
+	p->writeprotection = false;
+	atomic_dec(&vgpu->gtt.n_write_protected_guest_page);
+	return 0;
+}
+
+/**
+ * intel_gvt_hypervisor_read_gpa - copy data from GPA to host data buffer
+ * @vgpu: a vGPU
+ * @gpa: guest physical address
+ * @buf: host data buffer
+ * @len: data length
+ *
+ * Returns:
+ * Zero on success, negative error code if failed.
+ */
+static inline int intel_gvt_hypervisor_read_gpa(struct intel_vgpu *vgpu,
+		unsigned long gpa, void *buf, unsigned long len)
+{
+	return intel_gvt_host.mpt->read_gpa(vgpu->handle, gpa, buf, len);
+}
+
+/**
+ * intel_gvt_hypervisor_write_gpa - copy data from host data buffer to GPA
+ * @vgpu: a vGPU
+ * @gpa: guest physical address
+ * @buf: host data buffer
+ * @len: data length
+ *
+ * Returns:
+ * Zero on success, negative error code if failed.
+ */
+static inline int intel_gvt_hypervisor_write_gpa(struct intel_vgpu *vgpu,
+		unsigned long gpa, void *buf, unsigned long len)
+{
+	return intel_gvt_host.mpt->write_gpa(vgpu->handle, gpa, buf, len);
+}
+
+/**
+ * intel_gvt_hypervisor_gfn_to_mfn - translate a GFN to MFN
+ * @vgpu: a vGPU
+ * @gpfn: guest pfn
+ *
+ * Returns:
+ * MFN on success, INTEL_GVT_INVALID_ADDR if failed.
+ */
+static inline unsigned long intel_gvt_hypervisor_gfn_to_mfn(
+		struct intel_vgpu *vgpu, unsigned long gfn)
+{
+	return intel_gvt_host.mpt->gfn_to_mfn(vgpu->handle, gfn);
+}
+
+enum {
+	GVT_MAP_APERTURE = 0,
+	GVT_MAP_OPREGION,
+};
+
+/**
+ * intel_gvt_hypervisor_map_gfn_to_mfn - map a GFN region to MFN
+ * @vgpu: a vGPU
+ * @gfn: guest PFN
+ * @mfn: host PFN
+ * @nr: amount of PFNs
+ * @map: map or unmap
+ * @type: map type
+ *
+ * Returns:
+ * Zero on success, negative error code if failed.
+ */
+static inline int intel_gvt_hypervisor_map_gfn_to_mfn(
+		struct intel_vgpu *vgpu, unsigned long gfn,
+		unsigned long mfn, unsigned int nr,
+		bool map, int type)
+{
+	return intel_gvt_host.mpt->map_gfn_to_mfn(vgpu->handle, gfn, mfn, nr,
+						  map, type);
+}
+
+/**
+ * intel_gvt_hypervisor_set_trap_area - Trap a guest PA region
+ * @vgpu: a vGPU
+ * @start: the beginning of the guest physical address region
+ * @end: the end of the guest physical address region
+ * @map: map or unmap
+ *
+ * Returns:
+ * Zero on success, negative error code if failed.
+ */
+static inline int intel_gvt_hypervisor_set_trap_area(
+		struct intel_vgpu *vgpu, u64 start, u64 end, bool map)
+{
+	return intel_gvt_host.mpt->set_trap_area(vgpu->handle, start, end, map);
+}
+
 #endif /* _GVT_MPT_H_ */
diff --git a/drivers/gpu/drm/i915/gvt/opregion.c b/drivers/gpu/drm/i915/gvt/opregion.c
new file mode 100644
index 000000000000..973c8a9d0b15
--- /dev/null
+++ b/drivers/gpu/drm/i915/gvt/opregion.c
@@ -0,0 +1,344 @@
+/*
+ * Copyright(c) 2011-2016 Intel Corporation. All rights reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/acpi.h>
+#include "i915_drv.h"
+#include "gvt.h"
+
+static int init_vgpu_opregion(struct intel_vgpu *vgpu, u32 gpa)
+{
+	void __iomem *host_va = vgpu->gvt->opregion.opregion_va;
+	u8 *buf;
+	int i;
+
+	if (WARN((vgpu_opregion(vgpu)->va),
+			"vgpu%d: opregion has been initialized already.\n",
+			vgpu->id))
+		return -EINVAL;
+
+	vgpu_opregion(vgpu)->va = (void *)__get_free_pages(GFP_ATOMIC |
+			GFP_DMA32 | __GFP_ZERO,
+			INTEL_GVT_OPREGION_PORDER);
+
+	if (!vgpu_opregion(vgpu)->va)
+		return -ENOMEM;
+
+	memcpy_fromio(vgpu_opregion(vgpu)->va, host_va,
+			INTEL_GVT_OPREGION_SIZE);
+
+	for (i = 0; i < INTEL_GVT_OPREGION_PAGES; i++)
+		vgpu_opregion(vgpu)->gfn[i] = (gpa >> PAGE_SHIFT) + i;
+
+	/* for unknown reason, the value in LID field is incorrect
+	 * which block the windows guest, so workaround it by force
+	 * setting it to "OPEN"
+	 */
+	buf = (u8 *)vgpu_opregion(vgpu)->va;
+	buf[INTEL_GVT_OPREGION_CLID] = 0x3;
+
+	return 0;
+}
+
+static int map_vgpu_opregion(struct intel_vgpu *vgpu, bool map)
+{
+	u64 mfn;
+	int i, ret;
+
+	for (i = 0; i < INTEL_GVT_OPREGION_PAGES; i++) {
+		mfn = intel_gvt_hypervisor_virt_to_mfn(vgpu_opregion(vgpu)
+			+ i * PAGE_SIZE);
+		if (mfn == INTEL_GVT_INVALID_ADDR) {
+			gvt_err("fail to get MFN from VA\n");
+			return -EINVAL;
+		}
+		ret = intel_gvt_hypervisor_map_gfn_to_mfn(vgpu,
+				vgpu_opregion(vgpu)->gfn[i],
+				mfn, 1, map, GVT_MAP_OPREGION);
+		if (ret) {
+			gvt_err("fail to map GFN to MFN, errno: %d\n", ret);
+			return ret;
+		}
+	}
+	return 0;
+}
+
+/**
+ * intel_vgpu_clean_opregion - clean the stuff used to emulate opregion
+ * @vgpu: a vGPU
+ *
+ */
+void intel_vgpu_clean_opregion(struct intel_vgpu *vgpu)
+{
+	int i;
+
+	gvt_dbg_core("vgpu%d: clean vgpu opregion\n", vgpu->id);
+
+	if (!vgpu_opregion(vgpu)->va)
+		return;
+
+	if (intel_gvt_host.hypervisor_type == INTEL_GVT_HYPERVISOR_KVM) {
+		vunmap(vgpu_opregion(vgpu)->va);
+		for (i = 0; i < INTEL_GVT_OPREGION_PAGES; i++) {
+			if (vgpu_opregion(vgpu)->pages[i]) {
+				put_page(vgpu_opregion(vgpu)->pages[i]);
+				vgpu_opregion(vgpu)->pages[i] = NULL;
+			}
+		}
+	} else {
+		map_vgpu_opregion(vgpu, false);
+		free_pages((unsigned long)vgpu_opregion(vgpu)->va,
+				INTEL_GVT_OPREGION_PORDER);
+	}
+
+	vgpu_opregion(vgpu)->va = NULL;
+}
+
+/**
+ * intel_vgpu_init_opregion - initialize the stuff used to emulate opregion
+ * @vgpu: a vGPU
+ * @gpa: guest physical address of opregion
+ *
+ * Returns:
+ * Zero on success, negative error code if failed.
+ */
+int intel_vgpu_init_opregion(struct intel_vgpu *vgpu, u32 gpa)
+{
+	int ret;
+
+	gvt_dbg_core("vgpu%d: init vgpu opregion\n", vgpu->id);
+
+	if (intel_gvt_host.hypervisor_type == INTEL_GVT_HYPERVISOR_XEN) {
+		gvt_dbg_core("emulate opregion from kernel\n");
+
+		ret = init_vgpu_opregion(vgpu, gpa);
+		if (ret)
+			return ret;
+
+		ret = map_vgpu_opregion(vgpu, true);
+		if (ret)
+			return ret;
+	} else {
+		gvt_dbg_core("emulate opregion from userspace\n");
+
+		/*
+		 * If opregion pages are not allocated from host kenrel,
+		 * most of the params are meaningless
+		 */
+		ret = intel_gvt_hypervisor_map_gfn_to_mfn(vgpu,
+				0, /* not used */
+				0, /* not used */
+				2, /* not used */
+				1,
+				GVT_MAP_OPREGION);
+		if (ret)
+			return ret;
+	}
+	return 0;
+}
+
+/**
+ * intel_gvt_clean_opregion - clean host opergion related stuffs
+ * @gvt: a GVT device
+ *
+ */
+void intel_gvt_clean_opregion(struct intel_gvt *gvt)
+{
+	iounmap(gvt->opregion.opregion_va);
+	gvt->opregion.opregion_va = NULL;
+}
+
+/**
+ * intel_gvt_init_opregion - initialize host opergion related stuffs
+ * @gvt: a GVT device
+ *
+ * Returns:
+ * Zero on success, negative error code if failed.
+ */
+int intel_gvt_init_opregion(struct intel_gvt *gvt)
+{
+	gvt_dbg_core("init host opregion\n");
+
+	pci_read_config_dword(gvt->dev_priv->drm.pdev, INTEL_GVT_PCI_OPREGION,
+			&gvt->opregion.opregion_pa);
+
+	gvt->opregion.opregion_va = acpi_os_ioremap(gvt->opregion.opregion_pa,
+			INTEL_GVT_OPREGION_SIZE);
+	if (!gvt->opregion.opregion_va) {
+		gvt_err("fail to map host opregion\n");
+		return -EFAULT;
+	}
+	return 0;
+}
+
+#define GVT_OPREGION_FUNC(scic)					\
+	({							\
+	 u32 __ret;						\
+	 __ret = (scic & OPREGION_SCIC_FUNC_MASK) >>		\
+	 OPREGION_SCIC_FUNC_SHIFT;				\
+	 __ret;							\
+	 })
+
+#define GVT_OPREGION_SUBFUNC(scic)				\
+	({							\
+	 u32 __ret;						\
+	 __ret = (scic & OPREGION_SCIC_SUBFUNC_MASK) >>		\
+	 OPREGION_SCIC_SUBFUNC_SHIFT;				\
+	 __ret;							\
+	 })
+
+static const char *opregion_func_name(u32 func)
+{
+	const char *name = NULL;
+
+	switch (func) {
+	case 0 ... 3:
+	case 5:
+	case 7 ... 15:
+		name = "Reserved";
+		break;
+
+	case 4:
+		name = "Get BIOS Data";
+		break;
+
+	case 6:
+		name = "System BIOS Callbacks";
+		break;
+
+	default:
+		name = "Unknown";
+		break;
+	}
+	return name;
+}
+
+static const char *opregion_subfunc_name(u32 subfunc)
+{
+	const char *name = NULL;
+
+	switch (subfunc) {
+	case 0:
+		name = "Supported Calls";
+		break;
+
+	case 1:
+		name = "Requested Callbacks";
+		break;
+
+	case 2 ... 3:
+	case 8 ... 9:
+		name = "Reserved";
+		break;
+
+	case 5:
+		name = "Boot Display";
+		break;
+
+	case 6:
+		name = "TV-Standard/Video-Connector";
+		break;
+
+	case 7:
+		name = "Internal Graphics";
+		break;
+
+	case 10:
+		name = "Spread Spectrum Clocks";
+		break;
+
+	case 11:
+		name = "Get AKSV";
+		break;
+
+	default:
+		name = "Unknown";
+		break;
+	}
+	return name;
+};
+
+static bool querying_capabilities(u32 scic)
+{
+	u32 func, subfunc;
+
+	func = GVT_OPREGION_FUNC(scic);
+	subfunc = GVT_OPREGION_SUBFUNC(scic);
+
+	if ((func == INTEL_GVT_OPREGION_SCIC_F_GETBIOSDATA &&
+		subfunc == INTEL_GVT_OPREGION_SCIC_SF_SUPPRTEDCALLS)
+		|| (func == INTEL_GVT_OPREGION_SCIC_F_GETBIOSDATA &&
+		 subfunc == INTEL_GVT_OPREGION_SCIC_SF_REQEUSTEDCALLBACKS)
+		|| (func == INTEL_GVT_OPREGION_SCIC_F_GETBIOSCALLBACKS &&
+		 subfunc == INTEL_GVT_OPREGION_SCIC_SF_SUPPRTEDCALLS)) {
+		return true;
+	}
+	return false;
+}
+
+/**
+ * intel_vgpu_emulate_opregion_request - emulating OpRegion request
+ * @vgpu: a vGPU
+ * @swsci: SWSCI request
+ *
+ * Returns:
+ * Zero on success, negative error code if failed
+ */
+int intel_vgpu_emulate_opregion_request(struct intel_vgpu *vgpu, u32 swsci)
+{
+	u32 *scic, *parm;
+	u32 func, subfunc;
+
+	scic = vgpu_opregion(vgpu)->va + INTEL_GVT_OPREGION_SCIC;
+	parm = vgpu_opregion(vgpu)->va + INTEL_GVT_OPREGION_PARM;
+
+	if (!(swsci & SWSCI_SCI_SELECT)) {
+		gvt_err("vgpu%d: requesting SMI service\n", vgpu->id);
+		return 0;
+	}
+	/* ignore non 0->1 trasitions */
+	if ((vgpu_cfg_space(vgpu)[INTEL_GVT_PCI_SWSCI]
+				& SWSCI_SCI_TRIGGER) ||
+			!(swsci & SWSCI_SCI_TRIGGER)) {
+		return 0;
+	}
+
+	func = GVT_OPREGION_FUNC(*scic);
+	subfunc = GVT_OPREGION_SUBFUNC(*scic);
+	if (!querying_capabilities(*scic)) {
+		gvt_err("vgpu%d: requesting runtime service: func \"%s\","
+				" subfunc \"%s\"\n",
+				vgpu->id,
+				opregion_func_name(func),
+				opregion_subfunc_name(subfunc));
+		/*
+		 * emulate exit status of function call, '0' means
+		 * "failure, generic, unsupported or unknown cause"
+		 */
+		*scic &= ~OPREGION_SCIC_EXIT_MASK;
+		return 0;
+	}
+
+	*scic = 0;
+	*parm = 0;
+	return 0;
+}
diff --git a/drivers/gpu/drm/i915/gvt/reg.h b/drivers/gpu/drm/i915/gvt/reg.h
new file mode 100644
index 000000000000..0dfe789d8f02
--- /dev/null
+++ b/drivers/gpu/drm/i915/gvt/reg.h
@@ -0,0 +1,80 @@
+/*
+ * Copyright(c) 2011-2016 Intel Corporation. All rights reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _GVT_REG_H
+#define _GVT_REG_H
+
+#define INTEL_GVT_PCI_CLASS_VGA_OTHER   0x80
+
+#define INTEL_GVT_PCI_GMCH_CONTROL	0x50
+#define   BDW_GMCH_GMS_SHIFT		8
+#define   BDW_GMCH_GMS_MASK		0xff
+
+#define INTEL_GVT_PCI_SWSCI		0xe8
+#define   SWSCI_SCI_SELECT		(1 << 15)
+#define   SWSCI_SCI_TRIGGER		1
+
+#define INTEL_GVT_PCI_OPREGION		0xfc
+
+#define INTEL_GVT_OPREGION_CLID		0x1AC
+#define INTEL_GVT_OPREGION_SCIC		0x200
+#define   OPREGION_SCIC_FUNC_MASK	0x1E
+#define   OPREGION_SCIC_FUNC_SHIFT	1
+#define   OPREGION_SCIC_SUBFUNC_MASK	0xFF00
+#define   OPREGION_SCIC_SUBFUNC_SHIFT	8
+#define   OPREGION_SCIC_EXIT_MASK	0xE0
+#define INTEL_GVT_OPREGION_SCIC_F_GETBIOSDATA         4
+#define INTEL_GVT_OPREGION_SCIC_F_GETBIOSCALLBACKS    6
+#define INTEL_GVT_OPREGION_SCIC_SF_SUPPRTEDCALLS      0
+#define INTEL_GVT_OPREGION_SCIC_SF_REQEUSTEDCALLBACKS 1
+#define INTEL_GVT_OPREGION_PARM                   0x204
+
+#define INTEL_GVT_OPREGION_PAGES	2
+#define INTEL_GVT_OPREGION_PORDER	1
+#define INTEL_GVT_OPREGION_SIZE		(2 * 4096)
+
+#define VGT_SPRSTRIDE(pipe)	_PIPE(pipe, _SPRA_STRIDE, _PLANE_STRIDE_2_B)
+
+#define _REG_VECS_EXCC		0x1A028
+#define _REG_VCS2_EXCC		0x1c028
+
+#define _REG_701C0(pipe, plane) (0x701c0 + pipe * 0x1000 + (plane - 1) * 0x100)
+#define _REG_701C4(pipe, plane) (0x701c4 + pipe * 0x1000 + (plane - 1) * 0x100)
+
+#define GFX_MODE_BIT_SET_IN_MASK(val, bit) \
+		((((bit) & 0xffff0000) == 0) && !!((val) & (((bit) << 16))))
+
+#define FORCEWAKE_RENDER_GEN9_REG 0xa278
+#define FORCEWAKE_ACK_RENDER_GEN9_REG 0x0D84
+#define FORCEWAKE_BLITTER_GEN9_REG 0xa188
+#define FORCEWAKE_ACK_BLITTER_GEN9_REG 0x130044
+#define FORCEWAKE_MEDIA_GEN9_REG 0xa270
+#define FORCEWAKE_ACK_MEDIA_GEN9_REG 0x0D88
+#define FORCEWAKE_ACK_HSW_REG 0x130044
+
+#define RB_HEAD_OFF_MASK	((1U << 21) - (1U << 2))
+#define RB_TAIL_OFF_MASK	((1U << 21) - (1U << 3))
+#define RB_TAIL_SIZE_MASK	((1U << 21) - (1U << 12))
+#define _RING_CTL_BUF_SIZE(ctl) (((ctl) & RB_TAIL_SIZE_MASK) + GTT_PAGE_SIZE)
+
+#endif
diff --git a/drivers/gpu/drm/i915/gvt/render.c b/drivers/gpu/drm/i915/gvt/render.c
new file mode 100644
index 000000000000..feebb65ba641
--- /dev/null
+++ b/drivers/gpu/drm/i915/gvt/render.c
@@ -0,0 +1,291 @@
+/*
+ * Copyright(c) 2011-2016 Intel Corporation. All rights reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ * Authors:
+ *    Eddie Dong <eddie.dong@intel.com>
+ *    Kevin Tian <kevin.tian@intel.com>
+ *
+ * Contributors:
+ *    Zhi Wang <zhi.a.wang@intel.com>
+ *    Changbin Du <changbin.du@intel.com>
+ *    Zhenyu Wang <zhenyuw@linux.intel.com>
+ *    Tina Zhang <tina.zhang@intel.com>
+ *    Bing Niu <bing.niu@intel.com>
+ *
+ */
+
+#include "i915_drv.h"
+#include "gvt.h"
+
+struct render_mmio {
+	int ring_id;
+	i915_reg_t reg;
+	u32 mask;
+	bool in_context;
+	u32 value;
+};
+
+static struct render_mmio gen8_render_mmio_list[] = {
+	{RCS, _MMIO(0x229c), 0xffff, false},
+	{RCS, _MMIO(0x2248), 0x0, false},
+	{RCS, _MMIO(0x2098), 0x0, false},
+	{RCS, _MMIO(0x20c0), 0xffff, true},
+	{RCS, _MMIO(0x24d0), 0, false},
+	{RCS, _MMIO(0x24d4), 0, false},
+	{RCS, _MMIO(0x24d8), 0, false},
+	{RCS, _MMIO(0x24dc), 0, false},
+	{RCS, _MMIO(0x7004), 0xffff, true},
+	{RCS, _MMIO(0x7008), 0xffff, true},
+	{RCS, _MMIO(0x7000), 0xffff, true},
+	{RCS, _MMIO(0x7010), 0xffff, true},
+	{RCS, _MMIO(0x7300), 0xffff, true},
+	{RCS, _MMIO(0x83a4), 0xffff, true},
+
+	{BCS, _MMIO(0x2229c), 0xffff, false},
+	{BCS, _MMIO(0x2209c), 0xffff, false},
+	{BCS, _MMIO(0x220c0), 0xffff, false},
+	{BCS, _MMIO(0x22098), 0x0, false},
+	{BCS, _MMIO(0x22028), 0x0, false},
+};
+
+static struct render_mmio gen9_render_mmio_list[] = {
+	{RCS, _MMIO(0x229c), 0xffff, false},
+	{RCS, _MMIO(0x2248), 0x0, false},
+	{RCS, _MMIO(0x2098), 0x0, false},
+	{RCS, _MMIO(0x20c0), 0xffff, true},
+	{RCS, _MMIO(0x24d0), 0, false},
+	{RCS, _MMIO(0x24d4), 0, false},
+	{RCS, _MMIO(0x24d8), 0, false},
+	{RCS, _MMIO(0x24dc), 0, false},
+	{RCS, _MMIO(0x7004), 0xffff, true},
+	{RCS, _MMIO(0x7008), 0xffff, true},
+	{RCS, _MMIO(0x7000), 0xffff, true},
+	{RCS, _MMIO(0x7010), 0xffff, true},
+	{RCS, _MMIO(0x7300), 0xffff, true},
+	{RCS, _MMIO(0x83a4), 0xffff, true},
+
+	{RCS, _MMIO(0x40e0), 0, false},
+	{RCS, _MMIO(0x40e4), 0, false},
+	{RCS, _MMIO(0x2580), 0xffff, true},
+	{RCS, _MMIO(0x7014), 0xffff, true},
+	{RCS, _MMIO(0x20ec), 0xffff, false},
+	{RCS, _MMIO(0xb118), 0, false},
+	{RCS, _MMIO(0xe100), 0xffff, true},
+	{RCS, _MMIO(0xe180), 0xffff, true},
+	{RCS, _MMIO(0xe184), 0xffff, true},
+	{RCS, _MMIO(0xe188), 0xffff, true},
+	{RCS, _MMIO(0xe194), 0xffff, true},
+	{RCS, _MMIO(0x4de0), 0, false},
+	{RCS, _MMIO(0x4de4), 0, false},
+	{RCS, _MMIO(0x4de8), 0, false},
+	{RCS, _MMIO(0x4dec), 0, false},
+	{RCS, _MMIO(0x4df0), 0, false},
+	{RCS, _MMIO(0x4df4), 0, false},
+
+	{BCS, _MMIO(0x2229c), 0xffff, false},
+	{BCS, _MMIO(0x2209c), 0xffff, false},
+	{BCS, _MMIO(0x220c0), 0xffff, false},
+	{BCS, _MMIO(0x22098), 0x0, false},
+	{BCS, _MMIO(0x22028), 0x0, false},
+
+	{VCS2, _MMIO(0x1c028), 0xffff, false},
+
+	{VECS, _MMIO(0x1a028), 0xffff, false},
+};
+
+static u32 gen9_render_mocs[I915_NUM_ENGINES][64];
+static u32 gen9_render_mocs_L3[32];
+
+static void handle_tlb_pending_event(struct intel_vgpu *vgpu, int ring_id)
+{
+	struct drm_i915_private *dev_priv = vgpu->gvt->dev_priv;
+	i915_reg_t reg;
+	u32 regs[] = {
+		[RCS] = 0x4260,
+		[VCS] = 0x4264,
+		[VCS2] = 0x4268,
+		[BCS] = 0x426c,
+		[VECS] = 0x4270,
+	};
+
+	if (WARN_ON(ring_id >= ARRAY_SIZE(regs)))
+		return;
+
+	if (!test_and_clear_bit(ring_id, (void *)vgpu->tlb_handle_pending))
+		return;
+
+	reg = _MMIO(regs[ring_id]);
+
+	I915_WRITE(reg, 0x1);
+
+	if (wait_for_atomic((I915_READ(reg) == 0), 50))
+		gvt_err("timeout in invalidate ring (%d) tlb\n", ring_id);
+
+	gvt_dbg_core("invalidate TLB for ring %d\n", ring_id);
+}
+
+static void load_mocs(struct intel_vgpu *vgpu, int ring_id)
+{
+	struct drm_i915_private *dev_priv = vgpu->gvt->dev_priv;
+	i915_reg_t offset, l3_offset;
+	u32 regs[] = {
+		[RCS] = 0xc800,
+		[VCS] = 0xc900,
+		[VCS2] = 0xca00,
+		[BCS] = 0xcc00,
+		[VECS] = 0xcb00,
+	};
+	int i;
+
+	if (WARN_ON(ring_id >= ARRAY_SIZE(regs)))
+		return;
+
+	if (!IS_SKYLAKE(dev_priv))
+		return;
+
+	for (i = 0; i < 64; i++) {
+		gen9_render_mocs[ring_id][i] = I915_READ(offset);
+		I915_WRITE(offset, vgpu_vreg(vgpu, offset));
+		POSTING_READ(offset);
+		offset.reg += 4;
+	}
+
+	if (ring_id == RCS) {
+		l3_offset.reg = 0xb020;
+		for (i = 0; i < 32; i++) {
+			gen9_render_mocs_L3[i] = I915_READ(l3_offset);
+			I915_WRITE(l3_offset, vgpu_vreg(vgpu, offset));
+			POSTING_READ(l3_offset);
+			l3_offset.reg += 4;
+		}
+	}
+}
+
+static void restore_mocs(struct intel_vgpu *vgpu, int ring_id)
+{
+	struct drm_i915_private *dev_priv = vgpu->gvt->dev_priv;
+	i915_reg_t offset, l3_offset;
+	u32 regs[] = {
+		[RCS] = 0xc800,
+		[VCS] = 0xc900,
+		[VCS2] = 0xca00,
+		[BCS] = 0xcc00,
+		[VECS] = 0xcb00,
+	};
+	int i;
+
+	if (WARN_ON(ring_id >= ARRAY_SIZE(regs)))
+		return;
+
+	if (!IS_SKYLAKE(dev_priv))
+		return;
+
+	for (i = 0; i < 64; i++) {
+		vgpu_vreg(vgpu, offset) = I915_READ(offset);
+		I915_WRITE(offset, gen9_render_mocs[ring_id][i]);
+		POSTING_READ(offset);
+		offset.reg += 4;
+	}
+
+	if (ring_id == RCS) {
+		l3_offset.reg = 0xb020;
+		for (i = 0; i < 32; i++) {
+			vgpu_vreg(vgpu, l3_offset) = I915_READ(l3_offset);
+			I915_WRITE(l3_offset, gen9_render_mocs_L3[i]);
+			POSTING_READ(l3_offset);
+			l3_offset.reg += 4;
+		}
+	}
+}
+
+void intel_gvt_load_render_mmio(struct intel_vgpu *vgpu, int ring_id)
+{
+	struct drm_i915_private *dev_priv = vgpu->gvt->dev_priv;
+	struct render_mmio *mmio;
+	u32 v;
+	int i, array_size;
+
+	if (IS_SKYLAKE(vgpu->gvt->dev_priv)) {
+		mmio = gen9_render_mmio_list;
+		array_size = ARRAY_SIZE(gen9_render_mmio_list);
+		load_mocs(vgpu, ring_id);
+	} else {
+		mmio = gen8_render_mmio_list;
+		array_size = ARRAY_SIZE(gen8_render_mmio_list);
+	}
+
+	for (i = 0; i < array_size; i++, mmio++) {
+		if (mmio->ring_id != ring_id)
+			continue;
+
+		mmio->value = I915_READ(mmio->reg);
+		if (mmio->mask)
+			v = vgpu_vreg(vgpu, mmio->reg) | (mmio->mask << 16);
+		else
+			v = vgpu_vreg(vgpu, mmio->reg);
+
+		I915_WRITE(mmio->reg, v);
+		POSTING_READ(mmio->reg);
+
+		gvt_dbg_render("load reg %x old %x new %x\n",
+				i915_mmio_reg_offset(mmio->reg),
+				mmio->value, v);
+	}
+	handle_tlb_pending_event(vgpu, ring_id);
+}
+
+void intel_gvt_restore_render_mmio(struct intel_vgpu *vgpu, int ring_id)
+{
+	struct drm_i915_private *dev_priv = vgpu->gvt->dev_priv;
+	struct render_mmio *mmio;
+	u32 v;
+	int i, array_size;
+
+	if (IS_SKYLAKE(dev_priv)) {
+		mmio = gen9_render_mmio_list;
+		array_size = ARRAY_SIZE(gen9_render_mmio_list);
+		restore_mocs(vgpu, ring_id);
+	} else {
+		mmio = gen8_render_mmio_list;
+		array_size = ARRAY_SIZE(gen8_render_mmio_list);
+	}
+
+	for (i = 0; i < array_size; i++, mmio++) {
+		if (mmio->ring_id != ring_id)
+			continue;
+
+		vgpu_vreg(vgpu, mmio->reg) = I915_READ(mmio->reg);
+
+		if (mmio->mask) {
+			vgpu_vreg(vgpu, mmio->reg) &= ~(mmio->mask << 16);
+			v = mmio->value | (mmio->mask << 16);
+		} else
+			v = mmio->value;
+
+		I915_WRITE(mmio->reg, v);
+		POSTING_READ(mmio->reg);
+
+		gvt_dbg_render("restore reg %x old %x new %x\n",
+				i915_mmio_reg_offset(mmio->reg),
+				mmio->value, v);
+	}
+}
diff --git a/drivers/gpu/drm/i915/gvt/render.h b/drivers/gpu/drm/i915/gvt/render.h
new file mode 100644
index 000000000000..dac1a3cc458b
--- /dev/null
+++ b/drivers/gpu/drm/i915/gvt/render.h
@@ -0,0 +1,43 @@
+/*
+ * Copyright(c) 2011-2016 Intel Corporation. All rights reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ * Authors:
+ *    Eddie Dong <eddie.dong@intel.com>
+ *    Kevin Tian <kevin.tian@intel.com>
+ *
+ * Contributors:
+ *    Zhi Wang <zhi.a.wang@intel.com>
+ *    Changbin Du <changbin.du@intel.com>
+ *    Zhenyu Wang <zhenyuw@linux.intel.com>
+ *    Tina Zhang <tina.zhang@intel.com>
+ *    Bing Niu <bing.niu@intel.com>
+ *
+ */
+
+#ifndef __GVT_RENDER_H__
+#define __GVT_RENDER_H__
+
+void intel_gvt_load_render_mmio(struct intel_vgpu *vgpu, int ring_id);
+
+void intel_gvt_restore_render_mmio(struct intel_vgpu *vgpu, int ring_id);
+
+#endif
diff --git a/drivers/gpu/drm/i915/gvt/sched_policy.c b/drivers/gpu/drm/i915/gvt/sched_policy.c
new file mode 100644
index 000000000000..1df6a5460f3e
--- /dev/null
+++ b/drivers/gpu/drm/i915/gvt/sched_policy.c
@@ -0,0 +1,294 @@
+/*
+ * Copyright(c) 2011-2016 Intel Corporation. All rights reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ * Authors:
+ *    Anhua Xu
+ *    Kevin Tian <kevin.tian@intel.com>
+ *
+ * Contributors:
+ *    Min He <min.he@intel.com>
+ *    Bing Niu <bing.niu@intel.com>
+ *    Zhi Wang <zhi.a.wang@intel.com>
+ *
+ */
+
+#include "i915_drv.h"
+#include "gvt.h"
+
+static bool vgpu_has_pending_workload(struct intel_vgpu *vgpu)
+{
+	struct intel_vgpu_execlist *execlist;
+	enum intel_engine_id i;
+	struct intel_engine_cs *engine;
+
+	for_each_engine(engine, vgpu->gvt->dev_priv, i) {
+		execlist = &vgpu->execlist[i];
+		if (!list_empty(workload_q_head(vgpu, i)))
+			return true;
+	}
+
+	return false;
+}
+
+static void try_to_schedule_next_vgpu(struct intel_gvt *gvt)
+{
+	struct intel_gvt_workload_scheduler *scheduler = &gvt->scheduler;
+	enum intel_engine_id i;
+	struct intel_engine_cs *engine;
+
+	/* no target to schedule */
+	if (!scheduler->next_vgpu)
+		return;
+
+	gvt_dbg_sched("try to schedule next vgpu %d\n",
+			scheduler->next_vgpu->id);
+
+	/*
+	 * after the flag is set, workload dispatch thread will
+	 * stop dispatching workload for current vgpu
+	 */
+	scheduler->need_reschedule = true;
+
+	/* still have uncompleted workload? */
+	for_each_engine(engine, gvt->dev_priv, i) {
+		if (scheduler->current_workload[i]) {
+			gvt_dbg_sched("still have running workload\n");
+			return;
+		}
+	}
+
+	gvt_dbg_sched("switch to next vgpu %d\n",
+			scheduler->next_vgpu->id);
+
+	/* switch current vgpu */
+	scheduler->current_vgpu = scheduler->next_vgpu;
+	scheduler->next_vgpu = NULL;
+
+	scheduler->need_reschedule = false;
+
+	/* wake up workload dispatch thread */
+	for_each_engine(engine, gvt->dev_priv, i)
+		wake_up(&scheduler->waitq[i]);
+}
+
+struct tbs_vgpu_data {
+	struct list_head list;
+	struct intel_vgpu *vgpu;
+	/* put some per-vgpu sched stats here */
+};
+
+struct tbs_sched_data {
+	struct intel_gvt *gvt;
+	struct delayed_work work;
+	unsigned long period;
+	struct list_head runq_head;
+};
+
+#define GVT_DEFAULT_TIME_SLICE (1 * HZ / 1000)
+
+static void tbs_sched_func(struct work_struct *work)
+{
+	struct tbs_sched_data *sched_data = container_of(work,
+			struct tbs_sched_data, work.work);
+	struct tbs_vgpu_data *vgpu_data;
+
+	struct intel_gvt *gvt = sched_data->gvt;
+	struct intel_gvt_workload_scheduler *scheduler = &gvt->scheduler;
+
+	struct intel_vgpu *vgpu = NULL;
+	struct list_head *pos, *head;
+
+	mutex_lock(&gvt->lock);
+
+	/* no vgpu or has already had a target */
+	if (list_empty(&sched_data->runq_head) || scheduler->next_vgpu)
+		goto out;
+
+	if (scheduler->current_vgpu) {
+		vgpu_data = scheduler->current_vgpu->sched_data;
+		head = &vgpu_data->list;
+	} else {
+		gvt_dbg_sched("no current vgpu search from q head\n");
+		head = &sched_data->runq_head;
+	}
+
+	/* search a vgpu with pending workload */
+	list_for_each(pos, head) {
+		if (pos == &sched_data->runq_head)
+			continue;
+
+		vgpu_data = container_of(pos, struct tbs_vgpu_data, list);
+		if (!vgpu_has_pending_workload(vgpu_data->vgpu))
+			continue;
+
+		vgpu = vgpu_data->vgpu;
+		break;
+	}
+
+	if (vgpu) {
+		scheduler->next_vgpu = vgpu;
+		gvt_dbg_sched("pick next vgpu %d\n", vgpu->id);
+	}
+out:
+	if (scheduler->next_vgpu) {
+		gvt_dbg_sched("try to schedule next vgpu %d\n",
+				scheduler->next_vgpu->id);
+		try_to_schedule_next_vgpu(gvt);
+	}
+
+	/*
+	 * still have vgpu on runq
+	 * or last schedule haven't finished due to running workload
+	 */
+	if (!list_empty(&sched_data->runq_head) || scheduler->next_vgpu)
+		schedule_delayed_work(&sched_data->work, sched_data->period);
+
+	mutex_unlock(&gvt->lock);
+}
+
+static int tbs_sched_init(struct intel_gvt *gvt)
+{
+	struct intel_gvt_workload_scheduler *scheduler =
+		&gvt->scheduler;
+
+	struct tbs_sched_data *data;
+
+	data = kzalloc(sizeof(*data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	INIT_LIST_HEAD(&data->runq_head);
+	INIT_DELAYED_WORK(&data->work, tbs_sched_func);
+	data->period = GVT_DEFAULT_TIME_SLICE;
+	data->gvt = gvt;
+
+	scheduler->sched_data = data;
+	return 0;
+}
+
+static void tbs_sched_clean(struct intel_gvt *gvt)
+{
+	struct intel_gvt_workload_scheduler *scheduler =
+		&gvt->scheduler;
+	struct tbs_sched_data *data = scheduler->sched_data;
+
+	cancel_delayed_work(&data->work);
+	kfree(data);
+	scheduler->sched_data = NULL;
+}
+
+static int tbs_sched_init_vgpu(struct intel_vgpu *vgpu)
+{
+	struct tbs_vgpu_data *data;
+
+	data = kzalloc(sizeof(*data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	data->vgpu = vgpu;
+	INIT_LIST_HEAD(&data->list);
+
+	vgpu->sched_data = data;
+	return 0;
+}
+
+static void tbs_sched_clean_vgpu(struct intel_vgpu *vgpu)
+{
+	kfree(vgpu->sched_data);
+	vgpu->sched_data = NULL;
+}
+
+static void tbs_sched_start_schedule(struct intel_vgpu *vgpu)
+{
+	struct tbs_sched_data *sched_data = vgpu->gvt->scheduler.sched_data;
+	struct tbs_vgpu_data *vgpu_data = vgpu->sched_data;
+
+	if (!list_empty(&vgpu_data->list))
+		return;
+
+	list_add_tail(&vgpu_data->list, &sched_data->runq_head);
+	schedule_delayed_work(&sched_data->work, sched_data->period);
+}
+
+static void tbs_sched_stop_schedule(struct intel_vgpu *vgpu)
+{
+	struct tbs_vgpu_data *vgpu_data = vgpu->sched_data;
+
+	list_del_init(&vgpu_data->list);
+}
+
+static struct intel_gvt_sched_policy_ops tbs_schedule_ops = {
+	.init = tbs_sched_init,
+	.clean = tbs_sched_clean,
+	.init_vgpu = tbs_sched_init_vgpu,
+	.clean_vgpu = tbs_sched_clean_vgpu,
+	.start_schedule = tbs_sched_start_schedule,
+	.stop_schedule = tbs_sched_stop_schedule,
+};
+
+int intel_gvt_init_sched_policy(struct intel_gvt *gvt)
+{
+	gvt->scheduler.sched_ops = &tbs_schedule_ops;
+
+	return gvt->scheduler.sched_ops->init(gvt);
+}
+
+void intel_gvt_clean_sched_policy(struct intel_gvt *gvt)
+{
+	gvt->scheduler.sched_ops->clean(gvt);
+}
+
+int intel_vgpu_init_sched_policy(struct intel_vgpu *vgpu)
+{
+	return vgpu->gvt->scheduler.sched_ops->init_vgpu(vgpu);
+}
+
+void intel_vgpu_clean_sched_policy(struct intel_vgpu *vgpu)
+{
+	vgpu->gvt->scheduler.sched_ops->clean_vgpu(vgpu);
+}
+
+void intel_vgpu_start_schedule(struct intel_vgpu *vgpu)
+{
+	gvt_dbg_core("vgpu%d: start schedule\n", vgpu->id);
+
+	vgpu->gvt->scheduler.sched_ops->start_schedule(vgpu);
+}
+
+void intel_vgpu_stop_schedule(struct intel_vgpu *vgpu)
+{
+	struct intel_gvt_workload_scheduler *scheduler =
+		&vgpu->gvt->scheduler;
+
+	gvt_dbg_core("vgpu%d: stop schedule\n", vgpu->id);
+
+	scheduler->sched_ops->stop_schedule(vgpu);
+
+	if (scheduler->next_vgpu == vgpu)
+		scheduler->next_vgpu = NULL;
+
+	if (scheduler->current_vgpu == vgpu) {
+		/* stop workload dispatching */
+		scheduler->need_reschedule = true;
+		scheduler->current_vgpu = NULL;
+	}
+}
diff --git a/drivers/gpu/drm/i915/gvt/sched_policy.h b/drivers/gpu/drm/i915/gvt/sched_policy.h
new file mode 100644
index 000000000000..bb8b9097e41a
--- /dev/null
+++ b/drivers/gpu/drm/i915/gvt/sched_policy.h
@@ -0,0 +1,58 @@
+/*
+ * Copyright(c) 2011-2016 Intel Corporation. All rights reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ * Authors:
+ *    Anhua Xu
+ *    Kevin Tian <kevin.tian@intel.com>
+ *
+ * Contributors:
+ *    Min He <min.he@intel.com>
+ *    Bing Niu <bing.niu@intel.com>
+ *    Zhi Wang <zhi.a.wang@intel.com>
+ *
+ */
+
+#ifndef __GVT_SCHED_POLICY__
+#define __GVT_SCHED_POLICY__
+
+struct intel_gvt_sched_policy_ops {
+	int (*init)(struct intel_gvt *gvt);
+	void (*clean)(struct intel_gvt *gvt);
+	int (*init_vgpu)(struct intel_vgpu *vgpu);
+	void (*clean_vgpu)(struct intel_vgpu *vgpu);
+	void (*start_schedule)(struct intel_vgpu *vgpu);
+	void (*stop_schedule)(struct intel_vgpu *vgpu);
+};
+
+int intel_gvt_init_sched_policy(struct intel_gvt *gvt);
+
+void intel_gvt_clean_sched_policy(struct intel_gvt *gvt);
+
+int intel_vgpu_init_sched_policy(struct intel_vgpu *vgpu);
+
+void intel_vgpu_clean_sched_policy(struct intel_vgpu *vgpu);
+
+void intel_vgpu_start_schedule(struct intel_vgpu *vgpu);
+
+void intel_vgpu_stop_schedule(struct intel_vgpu *vgpu);
+
+#endif
diff --git a/drivers/gpu/drm/i915/gvt/scheduler.c b/drivers/gpu/drm/i915/gvt/scheduler.c
new file mode 100644
index 000000000000..e96eaeebeb0a
--- /dev/null
+++ b/drivers/gpu/drm/i915/gvt/scheduler.c
@@ -0,0 +1,578 @@
+/*
+ * Copyright(c) 2011-2016 Intel Corporation. All rights reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ * Authors:
+ *    Zhi Wang <zhi.a.wang@intel.com>
+ *
+ * Contributors:
+ *    Ping Gao <ping.a.gao@intel.com>
+ *    Tina Zhang <tina.zhang@intel.com>
+ *    Chanbin Du <changbin.du@intel.com>
+ *    Min He <min.he@intel.com>
+ *    Bing Niu <bing.niu@intel.com>
+ *    Zhenyu Wang <zhenyuw@linux.intel.com>
+ *
+ */
+
+#include <linux/kthread.h>
+
+#include "i915_drv.h"
+#include "gvt.h"
+
+#define RING_CTX_OFF(x) \
+	offsetof(struct execlist_ring_context, x)
+
+static void set_context_pdp_root_pointer(
+		struct execlist_ring_context *ring_context,
+		u32 pdp[8])
+{
+	struct execlist_mmio_pair *pdp_pair = &ring_context->pdp3_UDW;
+	int i;
+
+	for (i = 0; i < 8; i++)
+		pdp_pair[i].val = pdp[7 - i];
+}
+
+static int populate_shadow_context(struct intel_vgpu_workload *workload)
+{
+	struct intel_vgpu *vgpu = workload->vgpu;
+	struct intel_gvt *gvt = vgpu->gvt;
+	int ring_id = workload->ring_id;
+	struct i915_gem_context *shadow_ctx = workload->vgpu->shadow_ctx;
+	struct drm_i915_gem_object *ctx_obj =
+		shadow_ctx->engine[ring_id].state->obj;
+	struct execlist_ring_context *shadow_ring_context;
+	struct page *page;
+	void *dst;
+	unsigned long context_gpa, context_page_num;
+	int i;
+
+	gvt_dbg_sched("ring id %d workload lrca %x", ring_id,
+			workload->ctx_desc.lrca);
+
+	context_page_num = intel_lr_context_size(
+			gvt->dev_priv->engine[ring_id]);
+
+	context_page_num = context_page_num >> PAGE_SHIFT;
+
+	if (IS_BROADWELL(gvt->dev_priv) && ring_id == RCS)
+		context_page_num = 19;
+
+	i = 2;
+
+	while (i < context_page_num) {
+		context_gpa = intel_vgpu_gma_to_gpa(vgpu->gtt.ggtt_mm,
+				(u32)((workload->ctx_desc.lrca + i) <<
+				GTT_PAGE_SHIFT));
+		if (context_gpa == INTEL_GVT_INVALID_ADDR) {
+			gvt_err("Invalid guest context descriptor\n");
+			return -EINVAL;
+		}
+
+		page = i915_gem_object_get_page(ctx_obj, LRC_PPHWSP_PN + i);
+		dst = kmap_atomic(page);
+		intel_gvt_hypervisor_read_gpa(vgpu, context_gpa, dst,
+				GTT_PAGE_SIZE);
+		kunmap_atomic(dst);
+		i++;
+	}
+
+	page = i915_gem_object_get_page(ctx_obj, LRC_STATE_PN);
+	shadow_ring_context = kmap_atomic(page);
+
+#define COPY_REG(name) \
+	intel_gvt_hypervisor_read_gpa(vgpu, workload->ring_context_gpa \
+		+ RING_CTX_OFF(name.val), &shadow_ring_context->name.val, 4)
+
+	COPY_REG(ctx_ctrl);
+	COPY_REG(ctx_timestamp);
+
+	if (ring_id == RCS) {
+		COPY_REG(bb_per_ctx_ptr);
+		COPY_REG(rcs_indirect_ctx);
+		COPY_REG(rcs_indirect_ctx_offset);
+	}
+#undef COPY_REG
+
+	set_context_pdp_root_pointer(shadow_ring_context,
+				     workload->shadow_mm->shadow_page_table);
+
+	intel_gvt_hypervisor_read_gpa(vgpu,
+			workload->ring_context_gpa +
+			sizeof(*shadow_ring_context),
+			(void *)shadow_ring_context +
+			sizeof(*shadow_ring_context),
+			GTT_PAGE_SIZE - sizeof(*shadow_ring_context));
+
+	kunmap_atomic(shadow_ring_context);
+	return 0;
+}
+
+static int shadow_context_status_change(struct notifier_block *nb,
+		unsigned long action, void *data)
+{
+	struct intel_vgpu *vgpu = container_of(nb,
+			struct intel_vgpu, shadow_ctx_notifier_block);
+	struct drm_i915_gem_request *req =
+		(struct drm_i915_gem_request *)data;
+	struct intel_gvt_workload_scheduler *scheduler =
+		&vgpu->gvt->scheduler;
+	struct intel_vgpu_workload *workload =
+		scheduler->current_workload[req->engine->id];
+
+	switch (action) {
+	case INTEL_CONTEXT_SCHEDULE_IN:
+		intel_gvt_load_render_mmio(workload->vgpu,
+					   workload->ring_id);
+		atomic_set(&workload->shadow_ctx_active, 1);
+		break;
+	case INTEL_CONTEXT_SCHEDULE_OUT:
+		intel_gvt_restore_render_mmio(workload->vgpu,
+					      workload->ring_id);
+		atomic_set(&workload->shadow_ctx_active, 0);
+		break;
+	default:
+		WARN_ON(1);
+		return NOTIFY_OK;
+	}
+	wake_up(&workload->shadow_ctx_status_wq);
+	return NOTIFY_OK;
+}
+
+static int dispatch_workload(struct intel_vgpu_workload *workload)
+{
+	struct intel_vgpu *vgpu = workload->vgpu;
+	struct intel_gvt *gvt = vgpu->gvt;
+	int ring_id = workload->ring_id;
+	struct i915_gem_context *shadow_ctx = workload->vgpu->shadow_ctx;
+	struct drm_i915_private *dev_priv = workload->vgpu->gvt->dev_priv;
+	struct drm_i915_gem_request *rq;
+	int ret;
+
+	gvt_dbg_sched("ring id %d prepare to dispatch workload %p\n",
+		ring_id, workload);
+
+	shadow_ctx->desc_template = workload->ctx_desc.addressing_mode <<
+				    GEN8_CTX_ADDRESSING_MODE_SHIFT;
+
+	rq = i915_gem_request_alloc(dev_priv->engine[ring_id], shadow_ctx);
+	if (IS_ERR(rq)) {
+		gvt_err("fail to allocate gem request\n");
+		workload->status = PTR_ERR(rq);
+		return workload->status;
+	}
+
+	gvt_dbg_sched("ring id %d get i915 gem request %p\n", ring_id, rq);
+
+	workload->req = i915_gem_request_get(rq);
+
+	mutex_lock(&gvt->lock);
+
+	ret = intel_gvt_scan_and_shadow_workload(workload);
+	if (ret)
+		goto err;
+
+	ret = intel_gvt_scan_and_shadow_wa_ctx(&workload->wa_ctx);
+	if (ret)
+		goto err;
+
+	ret = populate_shadow_context(workload);
+	if (ret)
+		goto err;
+
+	if (workload->prepare) {
+		ret = workload->prepare(workload);
+		if (ret)
+			goto err;
+	}
+
+	mutex_unlock(&gvt->lock);
+
+	gvt_dbg_sched("ring id %d submit workload to i915 %p\n",
+			ring_id, workload->req);
+
+	i915_add_request_no_flush(rq);
+	workload->dispatched = true;
+	return 0;
+err:
+	workload->status = ret;
+
+	mutex_unlock(&gvt->lock);
+
+	i915_add_request_no_flush(rq);
+	return ret;
+}
+
+static struct intel_vgpu_workload *pick_next_workload(
+		struct intel_gvt *gvt, int ring_id)
+{
+	struct intel_gvt_workload_scheduler *scheduler = &gvt->scheduler;
+	struct intel_vgpu_workload *workload = NULL;
+
+	mutex_lock(&gvt->lock);
+
+	/*
+	 * no current vgpu / will be scheduled out / no workload
+	 * bail out
+	 */
+	if (!scheduler->current_vgpu) {
+		gvt_dbg_sched("ring id %d stop - no current vgpu\n", ring_id);
+		goto out;
+	}
+
+	if (scheduler->need_reschedule) {
+		gvt_dbg_sched("ring id %d stop - will reschedule\n", ring_id);
+		goto out;
+	}
+
+	if (list_empty(workload_q_head(scheduler->current_vgpu, ring_id))) {
+		gvt_dbg_sched("ring id %d stop - no available workload\n",
+				ring_id);
+		goto out;
+	}
+
+	/*
+	 * still have current workload, maybe the workload disptacher
+	 * fail to submit it for some reason, resubmit it.
+	 */
+	if (scheduler->current_workload[ring_id]) {
+		workload = scheduler->current_workload[ring_id];
+		gvt_dbg_sched("ring id %d still have current workload %p\n",
+				ring_id, workload);
+		goto out;
+	}
+
+	/*
+	 * pick a workload as current workload
+	 * once current workload is set, schedule policy routines
+	 * will wait the current workload is finished when trying to
+	 * schedule out a vgpu.
+	 */
+	scheduler->current_workload[ring_id] = container_of(
+			workload_q_head(scheduler->current_vgpu, ring_id)->next,
+			struct intel_vgpu_workload, list);
+
+	workload = scheduler->current_workload[ring_id];
+
+	gvt_dbg_sched("ring id %d pick new workload %p\n", ring_id, workload);
+
+	atomic_inc(&workload->vgpu->running_workload_num);
+out:
+	mutex_unlock(&gvt->lock);
+	return workload;
+}
+
+static void update_guest_context(struct intel_vgpu_workload *workload)
+{
+	struct intel_vgpu *vgpu = workload->vgpu;
+	struct intel_gvt *gvt = vgpu->gvt;
+	int ring_id = workload->ring_id;
+	struct i915_gem_context *shadow_ctx = workload->vgpu->shadow_ctx;
+	struct drm_i915_gem_object *ctx_obj =
+		shadow_ctx->engine[ring_id].state->obj;
+	struct execlist_ring_context *shadow_ring_context;
+	struct page *page;
+	void *src;
+	unsigned long context_gpa, context_page_num;
+	int i;
+
+	gvt_dbg_sched("ring id %d workload lrca %x\n", ring_id,
+			workload->ctx_desc.lrca);
+
+	context_page_num = intel_lr_context_size(
+			gvt->dev_priv->engine[ring_id]);
+
+	context_page_num = context_page_num >> PAGE_SHIFT;
+
+	if (IS_BROADWELL(gvt->dev_priv) && ring_id == RCS)
+		context_page_num = 19;
+
+	i = 2;
+
+	while (i < context_page_num) {
+		context_gpa = intel_vgpu_gma_to_gpa(vgpu->gtt.ggtt_mm,
+				(u32)((workload->ctx_desc.lrca + i) <<
+					GTT_PAGE_SHIFT));
+		if (context_gpa == INTEL_GVT_INVALID_ADDR) {
+			gvt_err("invalid guest context descriptor\n");
+			return;
+		}
+
+		page = i915_gem_object_get_page(ctx_obj, LRC_PPHWSP_PN + i);
+		src = kmap_atomic(page);
+		intel_gvt_hypervisor_write_gpa(vgpu, context_gpa, src,
+				GTT_PAGE_SIZE);
+		kunmap_atomic(src);
+		i++;
+	}
+
+	intel_gvt_hypervisor_write_gpa(vgpu, workload->ring_context_gpa +
+		RING_CTX_OFF(ring_header.val), &workload->rb_tail, 4);
+
+	page = i915_gem_object_get_page(ctx_obj, LRC_STATE_PN);
+	shadow_ring_context = kmap_atomic(page);
+
+#define COPY_REG(name) \
+	intel_gvt_hypervisor_write_gpa(vgpu, workload->ring_context_gpa + \
+		RING_CTX_OFF(name.val), &shadow_ring_context->name.val, 4)
+
+	COPY_REG(ctx_ctrl);
+	COPY_REG(ctx_timestamp);
+
+#undef COPY_REG
+
+	intel_gvt_hypervisor_write_gpa(vgpu,
+			workload->ring_context_gpa +
+			sizeof(*shadow_ring_context),
+			(void *)shadow_ring_context +
+			sizeof(*shadow_ring_context),
+			GTT_PAGE_SIZE - sizeof(*shadow_ring_context));
+
+	kunmap_atomic(shadow_ring_context);
+}
+
+static void complete_current_workload(struct intel_gvt *gvt, int ring_id)
+{
+	struct intel_gvt_workload_scheduler *scheduler = &gvt->scheduler;
+	struct intel_vgpu_workload *workload;
+	int event;
+
+	mutex_lock(&gvt->lock);
+
+	workload = scheduler->current_workload[ring_id];
+
+	if (!workload->status && !workload->vgpu->resetting) {
+		wait_event(workload->shadow_ctx_status_wq,
+			   !atomic_read(&workload->shadow_ctx_active));
+
+		update_guest_context(workload);
+
+		for_each_set_bit(event, workload->pending_events,
+				 INTEL_GVT_EVENT_MAX)
+			intel_vgpu_trigger_virtual_event(workload->vgpu,
+					event);
+	}
+
+	gvt_dbg_sched("ring id %d complete workload %p status %d\n",
+			ring_id, workload, workload->status);
+
+	scheduler->current_workload[ring_id] = NULL;
+
+	atomic_dec(&workload->vgpu->running_workload_num);
+
+	list_del_init(&workload->list);
+	workload->complete(workload);
+
+	wake_up(&scheduler->workload_complete_wq);
+	mutex_unlock(&gvt->lock);
+}
+
+struct workload_thread_param {
+	struct intel_gvt *gvt;
+	int ring_id;
+};
+
+static DEFINE_MUTEX(scheduler_mutex);
+
+static int workload_thread(void *priv)
+{
+	struct workload_thread_param *p = (struct workload_thread_param *)priv;
+	struct intel_gvt *gvt = p->gvt;
+	int ring_id = p->ring_id;
+	struct intel_gvt_workload_scheduler *scheduler = &gvt->scheduler;
+	struct intel_vgpu_workload *workload = NULL;
+	int ret;
+	bool need_force_wake = IS_SKYLAKE(gvt->dev_priv);
+
+	kfree(p);
+
+	gvt_dbg_core("workload thread for ring %d started\n", ring_id);
+
+	while (!kthread_should_stop()) {
+		ret = wait_event_interruptible(scheduler->waitq[ring_id],
+				kthread_should_stop() ||
+				(workload = pick_next_workload(gvt, ring_id)));
+
+		WARN_ON_ONCE(ret);
+
+		if (kthread_should_stop())
+			break;
+
+		mutex_lock(&scheduler_mutex);
+
+		gvt_dbg_sched("ring id %d next workload %p vgpu %d\n",
+				workload->ring_id, workload,
+				workload->vgpu->id);
+
+		intel_runtime_pm_get(gvt->dev_priv);
+
+		gvt_dbg_sched("ring id %d will dispatch workload %p\n",
+				workload->ring_id, workload);
+
+		if (need_force_wake)
+			intel_uncore_forcewake_get(gvt->dev_priv,
+					FORCEWAKE_ALL);
+
+		mutex_lock(&gvt->dev_priv->drm.struct_mutex);
+		ret = dispatch_workload(workload);
+		mutex_unlock(&gvt->dev_priv->drm.struct_mutex);
+
+		if (ret) {
+			gvt_err("fail to dispatch workload, skip\n");
+			goto complete;
+		}
+
+		gvt_dbg_sched("ring id %d wait workload %p\n",
+				workload->ring_id, workload);
+
+		workload->status = i915_wait_request(workload->req,
+						     0, NULL, NULL);
+		if (workload->status != 0)
+			gvt_err("fail to wait workload, skip\n");
+
+complete:
+		gvt_dbg_sched("will complete workload %p\n, status: %d\n",
+				workload, workload->status);
+
+		mutex_lock(&gvt->dev_priv->drm.struct_mutex);
+		complete_current_workload(gvt, ring_id);
+		mutex_unlock(&gvt->dev_priv->drm.struct_mutex);
+
+		i915_gem_request_put(fetch_and_zero(&workload->req));
+
+		if (need_force_wake)
+			intel_uncore_forcewake_put(gvt->dev_priv,
+					FORCEWAKE_ALL);
+
+		intel_runtime_pm_put(gvt->dev_priv);
+
+		mutex_unlock(&scheduler_mutex);
+
+	}
+	return 0;
+}
+
+void intel_gvt_wait_vgpu_idle(struct intel_vgpu *vgpu)
+{
+	struct intel_gvt *gvt = vgpu->gvt;
+	struct intel_gvt_workload_scheduler *scheduler = &gvt->scheduler;
+
+	if (atomic_read(&vgpu->running_workload_num)) {
+		gvt_dbg_sched("wait vgpu idle\n");
+
+		wait_event(scheduler->workload_complete_wq,
+				!atomic_read(&vgpu->running_workload_num));
+	}
+}
+
+void intel_gvt_clean_workload_scheduler(struct intel_gvt *gvt)
+{
+	struct intel_gvt_workload_scheduler *scheduler = &gvt->scheduler;
+	int i;
+
+	gvt_dbg_core("clean workload scheduler\n");
+
+	for (i = 0; i < I915_NUM_ENGINES; i++) {
+		if (scheduler->thread[i]) {
+			kthread_stop(scheduler->thread[i]);
+			scheduler->thread[i] = NULL;
+		}
+	}
+}
+
+int intel_gvt_init_workload_scheduler(struct intel_gvt *gvt)
+{
+	struct intel_gvt_workload_scheduler *scheduler = &gvt->scheduler;
+	struct workload_thread_param *param = NULL;
+	int ret;
+	int i;
+
+	gvt_dbg_core("init workload scheduler\n");
+
+	init_waitqueue_head(&scheduler->workload_complete_wq);
+
+	for (i = 0; i < I915_NUM_ENGINES; i++) {
+		/* check ring mask at init time */
+		if (!HAS_ENGINE(gvt->dev_priv, i))
+			continue;
+
+		init_waitqueue_head(&scheduler->waitq[i]);
+
+		param = kzalloc(sizeof(*param), GFP_KERNEL);
+		if (!param) {
+			ret = -ENOMEM;
+			goto err;
+		}
+
+		param->gvt = gvt;
+		param->ring_id = i;
+
+		scheduler->thread[i] = kthread_run(workload_thread, param,
+			"gvt workload %d", i);
+		if (IS_ERR(scheduler->thread[i])) {
+			gvt_err("fail to create workload thread\n");
+			ret = PTR_ERR(scheduler->thread[i]);
+			goto err;
+		}
+	}
+	return 0;
+err:
+	intel_gvt_clean_workload_scheduler(gvt);
+	kfree(param);
+	param = NULL;
+	return ret;
+}
+
+void intel_vgpu_clean_gvt_context(struct intel_vgpu *vgpu)
+{
+	struct drm_i915_private *dev_priv = vgpu->gvt->dev_priv;
+
+	atomic_notifier_chain_unregister(&vgpu->shadow_ctx->status_notifier,
+			&vgpu->shadow_ctx_notifier_block);
+
+	mutex_lock(&dev_priv->drm.struct_mutex);
+
+	/* a little hacky to mark as ctx closed */
+	vgpu->shadow_ctx->closed = true;
+	i915_gem_context_put(vgpu->shadow_ctx);
+
+	mutex_unlock(&dev_priv->drm.struct_mutex);
+}
+
+int intel_vgpu_init_gvt_context(struct intel_vgpu *vgpu)
+{
+	atomic_set(&vgpu->running_workload_num, 0);
+
+	vgpu->shadow_ctx = i915_gem_context_create_gvt(
+			&vgpu->gvt->dev_priv->drm);
+	if (IS_ERR(vgpu->shadow_ctx))
+		return PTR_ERR(vgpu->shadow_ctx);
+
+	vgpu->shadow_ctx->engine[RCS].initialised = true;
+
+	vgpu->shadow_ctx_notifier_block.notifier_call =
+		shadow_context_status_change;
+
+	atomic_notifier_chain_register(&vgpu->shadow_ctx->status_notifier,
+				       &vgpu->shadow_ctx_notifier_block);
+	return 0;
+}
diff --git a/drivers/gpu/drm/i915/gvt/scheduler.h b/drivers/gpu/drm/i915/gvt/scheduler.h
new file mode 100644
index 000000000000..3b30c28bff51
--- /dev/null
+++ b/drivers/gpu/drm/i915/gvt/scheduler.h
@@ -0,0 +1,139 @@
+/*
+ * Copyright(c) 2011-2016 Intel Corporation. All rights reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ * Authors:
+ *    Zhi Wang <zhi.a.wang@intel.com>
+ *
+ * Contributors:
+ *    Ping Gao <ping.a.gao@intel.com>
+ *    Tina Zhang <tina.zhang@intel.com>
+ *    Chanbin Du <changbin.du@intel.com>
+ *    Min He <min.he@intel.com>
+ *    Bing Niu <bing.niu@intel.com>
+ *    Zhenyu Wang <zhenyuw@linux.intel.com>
+ *
+ */
+
+#ifndef _GVT_SCHEDULER_H_
+#define _GVT_SCHEDULER_H_
+
+struct intel_gvt_workload_scheduler {
+	struct intel_vgpu *current_vgpu;
+	struct intel_vgpu *next_vgpu;
+	struct intel_vgpu_workload *current_workload[I915_NUM_ENGINES];
+	bool need_reschedule;
+
+	wait_queue_head_t workload_complete_wq;
+	struct task_struct *thread[I915_NUM_ENGINES];
+	wait_queue_head_t waitq[I915_NUM_ENGINES];
+
+	void *sched_data;
+	struct intel_gvt_sched_policy_ops *sched_ops;
+};
+
+#define INDIRECT_CTX_ADDR_MASK 0xffffffc0
+#define INDIRECT_CTX_SIZE_MASK 0x3f
+struct shadow_indirect_ctx {
+	struct drm_i915_gem_object *obj;
+	unsigned long guest_gma;
+	unsigned long shadow_gma;
+	void *shadow_va;
+	uint32_t size;
+};
+
+#define PER_CTX_ADDR_MASK 0xfffff000
+struct shadow_per_ctx {
+	unsigned long guest_gma;
+	unsigned long shadow_gma;
+};
+
+struct intel_shadow_wa_ctx {
+	struct intel_vgpu_workload *workload;
+	struct shadow_indirect_ctx indirect_ctx;
+	struct shadow_per_ctx per_ctx;
+
+};
+
+struct intel_vgpu_workload {
+	struct intel_vgpu *vgpu;
+	int ring_id;
+	struct drm_i915_gem_request *req;
+	/* if this workload has been dispatched to i915? */
+	bool dispatched;
+	int status;
+
+	struct intel_vgpu_mm *shadow_mm;
+
+	/* different submission model may need different handler */
+	int (*prepare)(struct intel_vgpu_workload *);
+	int (*complete)(struct intel_vgpu_workload *);
+	struct list_head list;
+
+	DECLARE_BITMAP(pending_events, INTEL_GVT_EVENT_MAX);
+	void *shadow_ring_buffer_va;
+
+	/* execlist context information */
+	struct execlist_ctx_descriptor_format ctx_desc;
+	struct execlist_ring_context *ring_context;
+	unsigned long rb_head, rb_tail, rb_ctl, rb_start, rb_len;
+	bool restore_inhibit;
+	struct intel_vgpu_elsp_dwords elsp_dwords;
+	bool emulate_schedule_in;
+	atomic_t shadow_ctx_active;
+	wait_queue_head_t shadow_ctx_status_wq;
+	u64 ring_context_gpa;
+
+	/* shadow batch buffer */
+	struct list_head shadow_bb;
+	struct intel_shadow_wa_ctx wa_ctx;
+};
+
+/* Intel shadow batch buffer is a i915 gem object */
+struct intel_shadow_bb_entry {
+	struct list_head list;
+	struct drm_i915_gem_object *obj;
+	void *va;
+	unsigned long len;
+	void *bb_start_cmd_va;
+};
+
+#define workload_q_head(vgpu, ring_id) \
+	(&(vgpu->workload_q_head[ring_id]))
+
+#define queue_workload(workload) do { \
+	list_add_tail(&workload->list, \
+	workload_q_head(workload->vgpu, workload->ring_id)); \
+	wake_up(&workload->vgpu->gvt-> \
+	scheduler.waitq[workload->ring_id]); \
+} while (0)
+
+int intel_gvt_init_workload_scheduler(struct intel_gvt *gvt);
+
+void intel_gvt_clean_workload_scheduler(struct intel_gvt *gvt);
+
+void intel_gvt_wait_vgpu_idle(struct intel_vgpu *vgpu);
+
+int intel_vgpu_init_gvt_context(struct intel_vgpu *vgpu);
+
+void intel_vgpu_clean_gvt_context(struct intel_vgpu *vgpu);
+
+#endif
diff --git a/drivers/gpu/drm/i915/gvt/trace.h b/drivers/gpu/drm/i915/gvt/trace.h
new file mode 100644
index 000000000000..53a2d10cf3f1
--- /dev/null
+++ b/drivers/gpu/drm/i915/gvt/trace.h
@@ -0,0 +1,286 @@
+/*
+ * Copyright © 2011-2016 Intel Corporation
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+ * IN THE SOFTWARE.
+ *
+ * Authors:
+ *    Jike Song <jike.song@intel.com>
+ *
+ * Contributors:
+ *    Zhi Wang <zhi.a.wang@intel.com>
+ *
+ */
+
+#if !defined(_GVT_TRACE_H_) || defined(TRACE_HEADER_MULTI_READ)
+#define _GVT_TRACE_H_
+
+#include <linux/types.h>
+#include <linux/stringify.h>
+#include <linux/tracepoint.h>
+#include <asm/tsc.h>
+
+#undef TRACE_SYSTEM
+#define TRACE_SYSTEM gvt
+
+TRACE_EVENT(spt_alloc,
+	TP_PROTO(int id, void *spt, int type, unsigned long mfn,
+		unsigned long gpt_gfn),
+
+	TP_ARGS(id, spt, type, mfn, gpt_gfn),
+
+	TP_STRUCT__entry(
+		__field(int, id)
+		__field(void *, spt)
+		__field(int, type)
+		__field(unsigned long, mfn)
+		__field(unsigned long, gpt_gfn)
+		),
+
+	TP_fast_assign(
+		__entry->id = id;
+		__entry->spt = spt;
+		__entry->type = type;
+		__entry->mfn = mfn;
+		__entry->gpt_gfn = gpt_gfn;
+	),
+
+	TP_printk("VM%d [alloc] spt %p type %d mfn 0x%lx gfn 0x%lx\n",
+		__entry->id,
+		__entry->spt,
+		__entry->type,
+		__entry->mfn,
+		__entry->gpt_gfn)
+);
+
+TRACE_EVENT(spt_free,
+	TP_PROTO(int id, void *spt, int type),
+
+	TP_ARGS(id, spt, type),
+
+	TP_STRUCT__entry(
+		__field(int, id)
+		__field(void *, spt)
+		__field(int, type)
+		),
+
+	TP_fast_assign(
+		__entry->id = id;
+		__entry->spt = spt;
+		__entry->type = type;
+	),
+
+	TP_printk("VM%u [free] spt %p type %d\n",
+		__entry->id,
+		__entry->spt,
+		__entry->type)
+);
+
+#define MAX_BUF_LEN 256
+
+TRACE_EVENT(gma_index,
+	TP_PROTO(const char *prefix, unsigned long gma,
+		unsigned long index),
+
+	TP_ARGS(prefix, gma, index),
+
+	TP_STRUCT__entry(
+		__array(char, buf, MAX_BUF_LEN)
+	),
+
+	TP_fast_assign(
+		snprintf(__entry->buf, MAX_BUF_LEN,
+			"%s gma 0x%lx index 0x%lx\n", prefix, gma, index);
+	),
+
+	TP_printk("%s", __entry->buf)
+);
+
+TRACE_EVENT(gma_translate,
+	TP_PROTO(int id, char *type, int ring_id, int pt_level,
+		unsigned long gma, unsigned long gpa),
+
+	TP_ARGS(id, type, ring_id, pt_level, gma, gpa),
+
+	TP_STRUCT__entry(
+		__array(char, buf, MAX_BUF_LEN)
+	),
+
+	TP_fast_assign(
+		snprintf(__entry->buf, MAX_BUF_LEN,
+			"VM%d %s ring %d pt_level %d gma 0x%lx -> gpa 0x%lx\n",
+				id, type, ring_id, pt_level, gma, gpa);
+	),
+
+	TP_printk("%s", __entry->buf)
+);
+
+TRACE_EVENT(spt_refcount,
+	TP_PROTO(int id, char *action, void *spt, int before, int after),
+
+	TP_ARGS(id, action, spt, before, after),
+
+	TP_STRUCT__entry(
+		__array(char, buf, MAX_BUF_LEN)
+	),
+
+	TP_fast_assign(
+		snprintf(__entry->buf, MAX_BUF_LEN,
+			"VM%d [%s] spt %p before %d -> after %d\n",
+				id, action, spt, before, after);
+	),
+
+	TP_printk("%s", __entry->buf)
+);
+
+TRACE_EVENT(spt_change,
+	TP_PROTO(int id, char *action, void *spt, unsigned long gfn,
+		int type),
+
+	TP_ARGS(id, action, spt, gfn, type),
+
+	TP_STRUCT__entry(
+		__array(char, buf, MAX_BUF_LEN)
+	),
+
+	TP_fast_assign(
+		snprintf(__entry->buf, MAX_BUF_LEN,
+			"VM%d [%s] spt %p gfn 0x%lx type %d\n",
+				id, action, spt, gfn, type);
+	),
+
+	TP_printk("%s", __entry->buf)
+);
+
+TRACE_EVENT(gpt_change,
+	TP_PROTO(int id, const char *tag, void *spt, int type, u64 v,
+		unsigned long index),
+
+	TP_ARGS(id, tag, spt, type, v, index),
+
+	TP_STRUCT__entry(
+		__array(char, buf, MAX_BUF_LEN)
+	),
+
+	TP_fast_assign(
+		snprintf(__entry->buf, MAX_BUF_LEN,
+		"VM%d [%s] spt %p type %d entry 0x%llx index 0x%lx\n",
+			id, tag, spt, type, v, index);
+	),
+
+	TP_printk("%s", __entry->buf)
+);
+
+TRACE_EVENT(oos_change,
+	TP_PROTO(int id, const char *tag, int page_id, void *gpt, int type),
+
+	TP_ARGS(id, tag, page_id, gpt, type),
+
+	TP_STRUCT__entry(
+		__array(char, buf, MAX_BUF_LEN)
+	),
+
+	TP_fast_assign(
+		snprintf(__entry->buf, MAX_BUF_LEN,
+		"VM%d [oos %s] page id %d gpt %p type %d\n",
+			id, tag, page_id, gpt, type);
+	),
+
+	TP_printk("%s", __entry->buf)
+);
+
+TRACE_EVENT(oos_sync,
+	TP_PROTO(int id, int page_id, void *gpt, int type, u64 v,
+		unsigned long index),
+
+	TP_ARGS(id, page_id, gpt, type, v, index),
+
+	TP_STRUCT__entry(
+		__array(char, buf, MAX_BUF_LEN)
+	),
+
+	TP_fast_assign(
+	snprintf(__entry->buf, MAX_BUF_LEN,
+	"VM%d [oos sync] page id %d gpt %p type %d entry 0x%llx index 0x%lx\n",
+				id, page_id, gpt, type, v, index);
+	),
+
+	TP_printk("%s", __entry->buf)
+);
+
+#define MAX_CMD_STR_LEN	256
+TRACE_EVENT(gvt_command,
+		TP_PROTO(u8 vm_id, u8 ring_id, u32 ip_gma, u32 *cmd_va, u32 cmd_len, bool ring_buffer_cmd, cycles_t cost_pre_cmd_handler, cycles_t cost_cmd_handler),
+
+		TP_ARGS(vm_id, ring_id, ip_gma, cmd_va, cmd_len, ring_buffer_cmd, cost_pre_cmd_handler, cost_cmd_handler),
+
+		TP_STRUCT__entry(
+			__field(u8, vm_id)
+			__field(u8, ring_id)
+			__field(int, i)
+			__array(char, tmp_buf, MAX_CMD_STR_LEN)
+			__array(char, cmd_str, MAX_CMD_STR_LEN)
+			),
+
+		TP_fast_assign(
+			__entry->vm_id = vm_id;
+			__entry->ring_id = ring_id;
+			__entry->cmd_str[0] = '\0';
+			snprintf(__entry->tmp_buf, MAX_CMD_STR_LEN, "VM(%d) Ring(%d): %s ip(%08x) pre handler cost (%llu), handler cost (%llu) ", vm_id, ring_id, ring_buffer_cmd ? "RB":"BB", ip_gma, cost_pre_cmd_handler, cost_cmd_handler);
+			strcat(__entry->cmd_str, __entry->tmp_buf);
+			entry->i = 0;
+			while (cmd_len > 0) {
+				if (cmd_len >= 8) {
+					snprintf(__entry->tmp_buf, MAX_CMD_STR_LEN, "%08x %08x %08x %08x %08x %08x %08x %08x ",
+						cmd_va[__entry->i], cmd_va[__entry->i+1], cmd_va[__entry->i+2], cmd_va[__entry->i+3],
+						cmd_va[__entry->i+4], cmd_va[__entry->i+5], cmd_va[__entry->i+6], cmd_va[__entry->i+7]);
+					__entry->i += 8;
+					cmd_len -= 8;
+					strcat(__entry->cmd_str, __entry->tmp_buf);
+				} else if (cmd_len >= 4) {
+					snprintf(__entry->tmp_buf, MAX_CMD_STR_LEN, "%08x %08x %08x %08x ",
+						cmd_va[__entry->i], cmd_va[__entry->i+1], cmd_va[__entry->i+2], cmd_va[__entry->i+3]);
+					__entry->i += 4;
+					cmd_len -= 4;
+					strcat(__entry->cmd_str, __entry->tmp_buf);
+				} else if (cmd_len >= 2) {
+					snprintf(__entry->tmp_buf, MAX_CMD_STR_LEN, "%08x %08x ", cmd_va[__entry->i], cmd_va[__entry->i+1]);
+					__entry->i += 2;
+					cmd_len -= 2;
+					strcat(__entry->cmd_str, __entry->tmp_buf);
+				} else if (cmd_len == 1) {
+					snprintf(__entry->tmp_buf, MAX_CMD_STR_LEN, "%08x ", cmd_va[__entry->i]);
+					__entry->i += 1;
+					cmd_len -= 1;
+					strcat(__entry->cmd_str, __entry->tmp_buf);
+				}
+			}
+			strcat(__entry->cmd_str, "\n");
+		),
+
+		TP_printk("%s", __entry->cmd_str)
+);
+#endif /* _GVT_TRACE_H_ */
+
+/* This part must be out of protection */
+#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/gpu/drm/i915/gvt/trace_points.c b/drivers/gpu/drm/i915/gvt/trace_points.c
new file mode 100644
index 000000000000..a3deed692b9c
--- /dev/null
+++ b/drivers/gpu/drm/i915/gvt/trace_points.c
@@ -0,0 +1,36 @@
+/*
+ * Copyright(c) 2011-2016 Intel Corporation. All rights reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ * Authors:
+ *    Jike Song <jike.song@intel.com>
+ *
+ * Contributors:
+ *    Zhi Wang <zhi.a.wang@intel.com>
+ *
+ */
+
+#include "trace.h"
+
+#ifndef __CHECKER__
+#define CREATE_TRACE_POINTS
+#include "trace.h"
+#endif
diff --git a/drivers/gpu/drm/i915/gvt/vgpu.c b/drivers/gpu/drm/i915/gvt/vgpu.c
new file mode 100644
index 000000000000..9401436d721f
--- /dev/null
+++ b/drivers/gpu/drm/i915/gvt/vgpu.c
@@ -0,0 +1,274 @@
+/*
+ * Copyright(c) 2011-2016 Intel Corporation. All rights reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ * Authors:
+ *    Eddie Dong <eddie.dong@intel.com>
+ *    Kevin Tian <kevin.tian@intel.com>
+ *
+ * Contributors:
+ *    Ping Gao <ping.a.gao@intel.com>
+ *    Zhi Wang <zhi.a.wang@intel.com>
+ *    Bing Niu <bing.niu@intel.com>
+ *
+ */
+
+#include "i915_drv.h"
+#include "gvt.h"
+#include "i915_pvinfo.h"
+
+static void clean_vgpu_mmio(struct intel_vgpu *vgpu)
+{
+	vfree(vgpu->mmio.vreg);
+	vgpu->mmio.vreg = vgpu->mmio.sreg = NULL;
+}
+
+static int setup_vgpu_mmio(struct intel_vgpu *vgpu)
+{
+	struct intel_gvt *gvt = vgpu->gvt;
+	const struct intel_gvt_device_info *info = &gvt->device_info;
+
+	vgpu->mmio.vreg = vzalloc(info->mmio_size * 2);
+	if (!vgpu->mmio.vreg)
+		return -ENOMEM;
+
+	vgpu->mmio.sreg = vgpu->mmio.vreg + info->mmio_size;
+
+	memcpy(vgpu->mmio.vreg, gvt->firmware.mmio, info->mmio_size);
+	memcpy(vgpu->mmio.sreg, gvt->firmware.mmio, info->mmio_size);
+
+	vgpu_vreg(vgpu, GEN6_GT_THREAD_STATUS_REG) = 0;
+
+	/* set the bit 0:2(Core C-State ) to C0 */
+	vgpu_vreg(vgpu, GEN6_GT_CORE_STATUS) = 0;
+	return 0;
+}
+
+static void setup_vgpu_cfg_space(struct intel_vgpu *vgpu,
+	struct intel_vgpu_creation_params *param)
+{
+	struct intel_gvt *gvt = vgpu->gvt;
+	const struct intel_gvt_device_info *info = &gvt->device_info;
+	u16 *gmch_ctl;
+	int i;
+
+	memcpy(vgpu_cfg_space(vgpu), gvt->firmware.cfg_space,
+	       info->cfg_space_size);
+
+	if (!param->primary) {
+		vgpu_cfg_space(vgpu)[PCI_CLASS_DEVICE] =
+			INTEL_GVT_PCI_CLASS_VGA_OTHER;
+		vgpu_cfg_space(vgpu)[PCI_CLASS_PROG] =
+			INTEL_GVT_PCI_CLASS_VGA_OTHER;
+	}
+
+	/* Show guest that there isn't any stolen memory.*/
+	gmch_ctl = (u16 *)(vgpu_cfg_space(vgpu) + INTEL_GVT_PCI_GMCH_CONTROL);
+	*gmch_ctl &= ~(BDW_GMCH_GMS_MASK << BDW_GMCH_GMS_SHIFT);
+
+	intel_vgpu_write_pci_bar(vgpu, PCI_BASE_ADDRESS_2,
+				 gvt_aperture_pa_base(gvt), true);
+
+	vgpu_cfg_space(vgpu)[PCI_COMMAND] &= ~(PCI_COMMAND_IO
+					     | PCI_COMMAND_MEMORY
+					     | PCI_COMMAND_MASTER);
+	/*
+	 * Clear the bar upper 32bit and let guest to assign the new value
+	 */
+	memset(vgpu_cfg_space(vgpu) + PCI_BASE_ADDRESS_1, 0, 4);
+	memset(vgpu_cfg_space(vgpu) + PCI_BASE_ADDRESS_3, 0, 4);
+
+	for (i = 0; i < INTEL_GVT_MAX_BAR_NUM; i++) {
+		vgpu->cfg_space.bar[i].size = pci_resource_len(
+					      gvt->dev_priv->drm.pdev, i * 2);
+		vgpu->cfg_space.bar[i].tracked = false;
+	}
+}
+
+static void populate_pvinfo_page(struct intel_vgpu *vgpu)
+{
+	/* setup the ballooning information */
+	vgpu_vreg64(vgpu, vgtif_reg(magic)) = VGT_MAGIC;
+	vgpu_vreg(vgpu, vgtif_reg(version_major)) = 1;
+	vgpu_vreg(vgpu, vgtif_reg(version_minor)) = 0;
+	vgpu_vreg(vgpu, vgtif_reg(display_ready)) = 0;
+	vgpu_vreg(vgpu, vgtif_reg(vgt_id)) = vgpu->id;
+	vgpu_vreg(vgpu, vgtif_reg(avail_rs.mappable_gmadr.base)) =
+		vgpu_aperture_gmadr_base(vgpu);
+	vgpu_vreg(vgpu, vgtif_reg(avail_rs.mappable_gmadr.size)) =
+		vgpu_aperture_sz(vgpu);
+	vgpu_vreg(vgpu, vgtif_reg(avail_rs.nonmappable_gmadr.base)) =
+		vgpu_hidden_gmadr_base(vgpu);
+	vgpu_vreg(vgpu, vgtif_reg(avail_rs.nonmappable_gmadr.size)) =
+		vgpu_hidden_sz(vgpu);
+
+	vgpu_vreg(vgpu, vgtif_reg(avail_rs.fence_num)) = vgpu_fence_sz(vgpu);
+
+	gvt_dbg_core("Populate PVINFO PAGE for vGPU %d\n", vgpu->id);
+	gvt_dbg_core("aperture base [GMADR] 0x%llx size 0x%llx\n",
+		vgpu_aperture_gmadr_base(vgpu), vgpu_aperture_sz(vgpu));
+	gvt_dbg_core("hidden base [GMADR] 0x%llx size=0x%llx\n",
+		vgpu_hidden_gmadr_base(vgpu), vgpu_hidden_sz(vgpu));
+	gvt_dbg_core("fence size %d\n", vgpu_fence_sz(vgpu));
+
+	WARN_ON(sizeof(struct vgt_if) != VGT_PVINFO_SIZE);
+}
+
+/**
+ * intel_gvt_destroy_vgpu - destroy a virtual GPU
+ * @vgpu: virtual GPU
+ *
+ * This function is called when user wants to destroy a virtual GPU.
+ *
+ */
+void intel_gvt_destroy_vgpu(struct intel_vgpu *vgpu)
+{
+	struct intel_gvt *gvt = vgpu->gvt;
+
+	mutex_lock(&gvt->lock);
+
+	vgpu->active = false;
+	idr_remove(&gvt->vgpu_idr, vgpu->id);
+
+	if (atomic_read(&vgpu->running_workload_num)) {
+		mutex_unlock(&gvt->lock);
+		intel_gvt_wait_vgpu_idle(vgpu);
+		mutex_lock(&gvt->lock);
+	}
+
+	intel_vgpu_stop_schedule(vgpu);
+	intel_vgpu_clean_sched_policy(vgpu);
+	intel_vgpu_clean_gvt_context(vgpu);
+	intel_vgpu_clean_execlist(vgpu);
+	intel_vgpu_clean_display(vgpu);
+	intel_vgpu_clean_opregion(vgpu);
+	intel_vgpu_clean_gtt(vgpu);
+	intel_gvt_hypervisor_detach_vgpu(vgpu);
+	intel_vgpu_free_resource(vgpu);
+	clean_vgpu_mmio(vgpu);
+	vfree(vgpu);
+
+	mutex_unlock(&gvt->lock);
+}
+
+/**
+ * intel_gvt_create_vgpu - create a virtual GPU
+ * @gvt: GVT device
+ * @param: vGPU creation parameters
+ *
+ * This function is called when user wants to create a virtual GPU.
+ *
+ * Returns:
+ * pointer to intel_vgpu, error pointer if failed.
+ */
+struct intel_vgpu *intel_gvt_create_vgpu(struct intel_gvt *gvt,
+		struct intel_vgpu_creation_params *param)
+{
+	struct intel_vgpu *vgpu;
+	int ret;
+
+	gvt_dbg_core("handle %llu low %llu MB high %llu MB fence %llu\n",
+			param->handle, param->low_gm_sz, param->high_gm_sz,
+			param->fence_sz);
+
+	vgpu = vzalloc(sizeof(*vgpu));
+	if (!vgpu)
+		return ERR_PTR(-ENOMEM);
+
+	mutex_lock(&gvt->lock);
+
+	ret = idr_alloc(&gvt->vgpu_idr, vgpu, 1, GVT_MAX_VGPU, GFP_KERNEL);
+	if (ret < 0)
+		goto out_free_vgpu;
+
+	vgpu->id = ret;
+	vgpu->handle = param->handle;
+	vgpu->gvt = gvt;
+	bitmap_zero(vgpu->tlb_handle_pending, I915_NUM_ENGINES);
+
+	setup_vgpu_cfg_space(vgpu, param);
+
+	ret = setup_vgpu_mmio(vgpu);
+	if (ret)
+		goto out_free_vgpu;
+
+	ret = intel_vgpu_alloc_resource(vgpu, param);
+	if (ret)
+		goto out_clean_vgpu_mmio;
+
+	populate_pvinfo_page(vgpu);
+
+	ret = intel_gvt_hypervisor_attach_vgpu(vgpu);
+	if (ret)
+		goto out_clean_vgpu_resource;
+
+	ret = intel_vgpu_init_gtt(vgpu);
+	if (ret)
+		goto out_detach_hypervisor_vgpu;
+
+	if (intel_gvt_host.hypervisor_type == INTEL_GVT_HYPERVISOR_KVM) {
+		ret = intel_vgpu_init_opregion(vgpu, 0);
+		if (ret)
+			goto out_clean_gtt;
+	}
+
+	ret = intel_vgpu_init_display(vgpu);
+	if (ret)
+		goto out_clean_opregion;
+
+	ret = intel_vgpu_init_execlist(vgpu);
+	if (ret)
+		goto out_clean_display;
+
+	ret = intel_vgpu_init_gvt_context(vgpu);
+	if (ret)
+		goto out_clean_execlist;
+
+	ret = intel_vgpu_init_sched_policy(vgpu);
+	if (ret)
+		goto out_clean_shadow_ctx;
+
+	vgpu->active = true;
+	mutex_unlock(&gvt->lock);
+
+	return vgpu;
+
+out_clean_shadow_ctx:
+	intel_vgpu_clean_gvt_context(vgpu);
+out_clean_execlist:
+	intel_vgpu_clean_execlist(vgpu);
+out_clean_display:
+	intel_vgpu_clean_display(vgpu);
+out_clean_opregion:
+	intel_vgpu_clean_opregion(vgpu);
+out_clean_gtt:
+	intel_vgpu_clean_gtt(vgpu);
+out_detach_hypervisor_vgpu:
+	intel_gvt_hypervisor_detach_vgpu(vgpu);
+out_clean_vgpu_resource:
+	intel_vgpu_free_resource(vgpu);
+out_clean_vgpu_mmio:
+	clean_vgpu_mmio(vgpu);
+out_free_vgpu:
+	vfree(vgpu);
+	mutex_unlock(&gvt->lock);
+	return ERR_PTR(ret);
+}
diff --git a/drivers/gpu/drm/i915/i915_cmd_parser.c b/drivers/gpu/drm/i915/i915_cmd_parser.c
index 70980f82a15b..f191d7b66b1d 100644
--- a/drivers/gpu/drm/i915/i915_cmd_parser.c
+++ b/drivers/gpu/drm/i915/i915_cmd_parser.c
@@ -1308,10 +1308,11 @@ int intel_engine_cmd_parser(struct intel_engine_cs *engine,
 int i915_cmd_parser_get_version(struct drm_i915_private *dev_priv)
 {
 	struct intel_engine_cs *engine;
+	enum intel_engine_id id;
 	bool active = false;
 
 	/* If the command parser is not enabled, report 0 - unsupported */
-	for_each_engine(engine, dev_priv) {
+	for_each_engine(engine, dev_priv, id) {
 		if (intel_engine_needs_cmd_parser(engine)) {
 			active = true;
 			break;
diff --git a/drivers/gpu/drm/i915/i915_debugfs.c b/drivers/gpu/drm/i915/i915_debugfs.c
index 6c7bb87f764e..20638d22bbad 100644
--- a/drivers/gpu/drm/i915/i915_debugfs.c
+++ b/drivers/gpu/drm/i915/i915_debugfs.c
@@ -79,10 +79,8 @@ static int i915_capabilities(struct seq_file *m, void *data)
 	seq_printf(m, "gen: %d\n", INTEL_GEN(dev_priv));
 	seq_printf(m, "pch: %d\n", INTEL_PCH_TYPE(dev_priv));
 #define PRINT_FLAG(x)  seq_printf(m, #x ": %s\n", yesno(info->x))
-#define SEP_SEMICOLON ;
-	DEV_INFO_FOR_EACH_FLAG(PRINT_FLAG, SEP_SEMICOLON);
+	DEV_INFO_FOR_EACH_FLAG(PRINT_FLAG);
 #undef PRINT_FLAG
-#undef SEP_SEMICOLON
 
 	return 0;
 }
@@ -109,7 +107,7 @@ static char get_tiling_flag(struct drm_i915_gem_object *obj)
 
 static char get_global_flag(struct drm_i915_gem_object *obj)
 {
-	return i915_gem_object_to_ggtt(obj, NULL) ?  'g' : ' ';
+	return obj->fault_mappable ? 'g' : ' ';
 }
 
 static char get_pin_mapped_flag(struct drm_i915_gem_object *obj)
@@ -152,7 +150,7 @@ describe_obj(struct seq_file *m, struct drm_i915_gem_object *obj)
 		   obj->base.size / 1024,
 		   obj->base.read_domains,
 		   obj->base.write_domain);
-	for_each_engine_id(engine, dev_priv, id)
+	for_each_engine(engine, dev_priv, id)
 		seq_printf(m, "%x ",
 			   i915_gem_active_get_seqno(&obj->last_read[id],
 						     &obj->base.dev->struct_mutex));
@@ -188,15 +186,6 @@ describe_obj(struct seq_file *m, struct drm_i915_gem_object *obj)
 	}
 	if (obj->stolen)
 		seq_printf(m, " (stolen: %08llx)", obj->stolen->start);
-	if (obj->pin_display || obj->fault_mappable) {
-		char s[3], *t = s;
-		if (obj->pin_display)
-			*t++ = 'p';
-		if (obj->fault_mappable)
-			*t++ = 'f';
-		*t = '\0';
-		seq_printf(m, " (%s mappable)", s);
-	}
 
 	engine = i915_gem_active_get_engine(&obj->last_write,
 					    &dev_priv->drm.struct_mutex);
@@ -334,11 +323,12 @@ static void print_batch_pool_stats(struct seq_file *m,
 	struct drm_i915_gem_object *obj;
 	struct file_stats stats;
 	struct intel_engine_cs *engine;
+	enum intel_engine_id id;
 	int j;
 
 	memset(&stats, 0, sizeof(stats));
 
-	for_each_engine(engine, dev_priv) {
+	for_each_engine(engine, dev_priv, id) {
 		for (j = 0; j < ARRAY_SIZE(engine->batch_pool.cache_list); j++) {
 			list_for_each_entry(obj,
 					    &engine->batch_pool.cache_list[j],
@@ -402,7 +392,7 @@ static int i915_gem_object_info(struct seq_file *m, void *data)
 	if (ret)
 		return ret;
 
-	seq_printf(m, "%u objects, %zu bytes\n",
+	seq_printf(m, "%u objects, %llu bytes\n",
 		   dev_priv->mm.object_count,
 		   dev_priv->mm.object_memory);
 
@@ -607,6 +597,7 @@ static int i915_gem_batch_pool_info(struct seq_file *m, void *data)
 	struct drm_device *dev = &dev_priv->drm;
 	struct drm_i915_gem_object *obj;
 	struct intel_engine_cs *engine;
+	enum intel_engine_id id;
 	int total = 0;
 	int ret, j;
 
@@ -614,7 +605,7 @@ static int i915_gem_batch_pool_info(struct seq_file *m, void *data)
 	if (ret)
 		return ret;
 
-	for_each_engine(engine, dev_priv) {
+	for_each_engine(engine, dev_priv, id) {
 		for (j = 0; j < ARRAY_SIZE(engine->batch_pool.cache_list); j++) {
 			int count;
 
@@ -645,12 +636,30 @@ static int i915_gem_batch_pool_info(struct seq_file *m, void *data)
 	return 0;
 }
 
+static void print_request(struct seq_file *m,
+			  struct drm_i915_gem_request *rq,
+			  const char *prefix)
+{
+	struct pid *pid = rq->ctx->pid;
+	struct task_struct *task;
+
+	rcu_read_lock();
+	task = pid ? pid_task(pid, PIDTYPE_PID) : NULL;
+	seq_printf(m, "%s%x [%x:%x] @ %d: %s [%d]\n", prefix,
+		   rq->fence.seqno, rq->ctx->hw_id, rq->fence.seqno,
+		   jiffies_to_msecs(jiffies - rq->emitted_jiffies),
+		   task ? task->comm : "<unknown>",
+		   task ? task->pid : -1);
+	rcu_read_unlock();
+}
+
 static int i915_gem_request_info(struct seq_file *m, void *data)
 {
 	struct drm_i915_private *dev_priv = node_to_i915(m->private);
 	struct drm_device *dev = &dev_priv->drm;
-	struct intel_engine_cs *engine;
 	struct drm_i915_gem_request *req;
+	struct intel_engine_cs *engine;
+	enum intel_engine_id id;
 	int ret, any;
 
 	ret = mutex_lock_interruptible(&dev->struct_mutex);
@@ -658,7 +667,7 @@ static int i915_gem_request_info(struct seq_file *m, void *data)
 		return ret;
 
 	any = 0;
-	for_each_engine(engine, dev_priv) {
+	for_each_engine(engine, dev_priv, id) {
 		int count;
 
 		count = 0;
@@ -668,19 +677,8 @@ static int i915_gem_request_info(struct seq_file *m, void *data)
 			continue;
 
 		seq_printf(m, "%s requests: %d\n", engine->name, count);
-		list_for_each_entry(req, &engine->request_list, link) {
-			struct pid *pid = req->ctx->pid;
-			struct task_struct *task;
-
-			rcu_read_lock();
-			task = pid ? pid_task(pid, PIDTYPE_PID) : NULL;
-			seq_printf(m, "    %x @ %d: %s [%d]\n",
-				   req->fence.seqno,
-				   (int) (jiffies - req->emitted_jiffies),
-				   task ? task->comm : "<unknown>",
-				   task ? task->pid : -1);
-			rcu_read_unlock();
-		}
+		list_for_each_entry(req, &engine->request_list, link)
+			print_request(m, req, "    ");
 
 		any++;
 	}
@@ -715,8 +713,9 @@ static int i915_gem_seqno_info(struct seq_file *m, void *data)
 {
 	struct drm_i915_private *dev_priv = node_to_i915(m->private);
 	struct intel_engine_cs *engine;
+	enum intel_engine_id id;
 
-	for_each_engine(engine, dev_priv)
+	for_each_engine(engine, dev_priv, id)
 		i915_ring_seqno_info(m, engine);
 
 	return 0;
@@ -727,6 +726,7 @@ static int i915_interrupt_info(struct seq_file *m, void *data)
 {
 	struct drm_i915_private *dev_priv = node_to_i915(m->private);
 	struct intel_engine_cs *engine;
+	enum intel_engine_id id;
 	int i, pipe;
 
 	intel_runtime_pm_get(dev_priv);
@@ -895,7 +895,7 @@ static int i915_interrupt_info(struct seq_file *m, void *data)
 		seq_printf(m, "Graphics Interrupt mask:		%08x\n",
 			   I915_READ(GTIMR));
 	}
-	for_each_engine(engine, dev_priv) {
+	for_each_engine(engine, dev_priv, id) {
 		if (INTEL_GEN(dev_priv) >= 6) {
 			seq_printf(m,
 				   "Graphics Interrupt mask (%s):	%08x\n",
@@ -943,7 +943,7 @@ static int i915_hws_info(struct seq_file *m, void *data)
 	const u32 *hws;
 	int i;
 
-	engine = &dev_priv->engine[(uintptr_t)node->info_ent->data];
+	engine = dev_priv->engine[(uintptr_t)node->info_ent->data];
 	hws = engine->status_page.page_addr;
 	if (hws == NULL)
 		return 0;
@@ -956,6 +956,8 @@ static int i915_hws_info(struct seq_file *m, void *data)
 	return 0;
 }
 
+#if IS_ENABLED(CONFIG_DRM_I915_CAPTURE_ERROR)
+
 static ssize_t
 i915_error_state_write(struct file *filp,
 		       const char __user *ubuf,
@@ -1038,6 +1040,8 @@ static const struct file_operations i915_error_state_fops = {
 	.release = i915_error_state_release,
 };
 
+#endif
+
 static int
 i915_next_seqno_get(void *data, u64 *val)
 {
@@ -1277,15 +1281,42 @@ out:
 	return ret;
 }
 
+static void i915_instdone_info(struct drm_i915_private *dev_priv,
+			       struct seq_file *m,
+			       struct intel_instdone *instdone)
+{
+	int slice;
+	int subslice;
+
+	seq_printf(m, "\t\tINSTDONE: 0x%08x\n",
+		   instdone->instdone);
+
+	if (INTEL_GEN(dev_priv) <= 3)
+		return;
+
+	seq_printf(m, "\t\tSC_INSTDONE: 0x%08x\n",
+		   instdone->slice_common);
+
+	if (INTEL_GEN(dev_priv) <= 6)
+		return;
+
+	for_each_instdone_slice_subslice(dev_priv, slice, subslice)
+		seq_printf(m, "\t\tSAMPLER_INSTDONE[%d][%d]: 0x%08x\n",
+			   slice, subslice, instdone->sampler[slice][subslice]);
+
+	for_each_instdone_slice_subslice(dev_priv, slice, subslice)
+		seq_printf(m, "\t\tROW_INSTDONE[%d][%d]: 0x%08x\n",
+			   slice, subslice, instdone->row[slice][subslice]);
+}
+
 static int i915_hangcheck_info(struct seq_file *m, void *unused)
 {
 	struct drm_i915_private *dev_priv = node_to_i915(m->private);
 	struct intel_engine_cs *engine;
 	u64 acthd[I915_NUM_ENGINES];
 	u32 seqno[I915_NUM_ENGINES];
-	u32 instdone[I915_NUM_INSTDONE_REG];
+	struct intel_instdone instdone;
 	enum intel_engine_id id;
-	int j;
 
 	if (test_bit(I915_WEDGED, &dev_priv->gpu_error.flags))
 		seq_printf(m, "Wedged\n");
@@ -1303,12 +1334,12 @@ static int i915_hangcheck_info(struct seq_file *m, void *unused)
 
 	intel_runtime_pm_get(dev_priv);
 
-	for_each_engine_id(engine, dev_priv, id) {
+	for_each_engine(engine, dev_priv, id) {
 		acthd[id] = intel_engine_get_active_head(engine);
 		seqno[id] = intel_engine_get_seqno(engine);
 	}
 
-	i915_get_extra_instdone(dev_priv, instdone);
+	intel_engine_get_instdone(dev_priv->engine[RCS], &instdone);
 
 	intel_runtime_pm_put(dev_priv);
 
@@ -1319,7 +1350,10 @@ static int i915_hangcheck_info(struct seq_file *m, void *unused)
 	} else
 		seq_printf(m, "Hangcheck inactive\n");
 
-	for_each_engine_id(engine, dev_priv, id) {
+	for_each_engine(engine, dev_priv, id) {
+		struct intel_breadcrumbs *b = &engine->breadcrumbs;
+		struct rb_node *rb;
+
 		seq_printf(m, "%s:\n", engine->name);
 		seq_printf(m, "\tseqno = %x [current %x, last %x]\n",
 			   engine->hangcheck.seqno,
@@ -1329,6 +1363,15 @@ static int i915_hangcheck_info(struct seq_file *m, void *unused)
 			   yesno(intel_engine_has_waiter(engine)),
 			   yesno(test_bit(engine->id,
 					  &dev_priv->gpu_error.missed_irq_rings)));
+		spin_lock(&b->lock);
+		for (rb = rb_first(&b->waiters); rb; rb = rb_next(rb)) {
+			struct intel_wait *w = container_of(rb, typeof(*w), node);
+
+			seq_printf(m, "\t%s [%d] waiting for %x\n",
+				   w->tsk->comm, w->tsk->pid, w->seqno);
+		}
+		spin_unlock(&b->lock);
+
 		seq_printf(m, "\tACTHD = 0x%08llx [current 0x%08llx]\n",
 			   (long long)engine->hangcheck.acthd,
 			   (long long)acthd[id]);
@@ -1336,18 +1379,14 @@ static int i915_hangcheck_info(struct seq_file *m, void *unused)
 		seq_printf(m, "\taction = %d\n", engine->hangcheck.action);
 
 		if (engine->id == RCS) {
-			seq_puts(m, "\tinstdone read =");
-
-			for (j = 0; j < I915_NUM_INSTDONE_REG; j++)
-				seq_printf(m, " 0x%08x", instdone[j]);
+			seq_puts(m, "\tinstdone read =\n");
 
-			seq_puts(m, "\n\tinstdone accu =");
+			i915_instdone_info(dev_priv, m, &instdone);
 
-			for (j = 0; j < I915_NUM_INSTDONE_REG; j++)
-				seq_printf(m, " 0x%08x",
-					   engine->hangcheck.instdone[j]);
+			seq_puts(m, "\tinstdone accu =\n");
 
-			seq_puts(m, "\n");
+			i915_instdone_info(dev_priv, m,
+					   &engine->hangcheck.instdone);
 		}
 	}
 
@@ -1635,7 +1674,8 @@ static int i915_fbc_status(struct seq_file *m, void *unused)
 		seq_printf(m, "FBC disabled: %s\n",
 			   dev_priv->fbc.no_fbc_reason);
 
-	if (INTEL_GEN(dev_priv) >= 7)
+	if (intel_fbc_is_active(dev_priv) &&
+	    INTEL_GEN(dev_priv) >= 7)
 		seq_printf(m, "Compressing: %s\n",
 			   yesno(I915_READ(FBC_STATUS2) &
 				 FBC_COMPRESSION_MASK));
@@ -1909,6 +1949,7 @@ static int i915_context_status(struct seq_file *m, void *unused)
 	struct drm_device *dev = &dev_priv->drm;
 	struct intel_engine_cs *engine;
 	struct i915_gem_context *ctx;
+	enum intel_engine_id id;
 	int ret;
 
 	ret = mutex_lock_interruptible(&dev->struct_mutex);
@@ -1935,7 +1976,7 @@ static int i915_context_status(struct seq_file *m, void *unused)
 		seq_putc(m, ctx->remap_slice ? 'R' : 'r');
 		seq_putc(m, '\n');
 
-		for_each_engine(engine, dev_priv) {
+		for_each_engine(engine, dev_priv, id) {
 			struct intel_context *ce = &ctx->engine[engine->id];
 
 			seq_printf(m, "%s: ", engine->name);
@@ -2002,6 +2043,7 @@ static int i915_dump_lrc(struct seq_file *m, void *unused)
 	struct drm_device *dev = &dev_priv->drm;
 	struct intel_engine_cs *engine;
 	struct i915_gem_context *ctx;
+	enum intel_engine_id id;
 	int ret;
 
 	if (!i915.enable_execlists) {
@@ -2014,7 +2056,7 @@ static int i915_dump_lrc(struct seq_file *m, void *unused)
 		return ret;
 
 	list_for_each_entry(ctx, &dev_priv->context_list, link)
-		for_each_engine(engine, dev_priv)
+		for_each_engine(engine, dev_priv, id)
 			i915_dump_lrc_obj(m, ctx, engine);
 
 	mutex_unlock(&dev->struct_mutex);
@@ -2022,84 +2064,6 @@ static int i915_dump_lrc(struct seq_file *m, void *unused)
 	return 0;
 }
 
-static int i915_execlists(struct seq_file *m, void *data)
-{
-	struct drm_i915_private *dev_priv = node_to_i915(m->private);
-	struct drm_device *dev = &dev_priv->drm;
-	struct intel_engine_cs *engine;
-	u32 status_pointer;
-	u8 read_pointer;
-	u8 write_pointer;
-	u32 status;
-	u32 ctx_id;
-	struct list_head *cursor;
-	int i, ret;
-
-	if (!i915.enable_execlists) {
-		seq_puts(m, "Logical Ring Contexts are disabled\n");
-		return 0;
-	}
-
-	ret = mutex_lock_interruptible(&dev->struct_mutex);
-	if (ret)
-		return ret;
-
-	intel_runtime_pm_get(dev_priv);
-
-	for_each_engine(engine, dev_priv) {
-		struct drm_i915_gem_request *head_req = NULL;
-		int count = 0;
-
-		seq_printf(m, "%s\n", engine->name);
-
-		status = I915_READ(RING_EXECLIST_STATUS_LO(engine));
-		ctx_id = I915_READ(RING_EXECLIST_STATUS_HI(engine));
-		seq_printf(m, "\tExeclist status: 0x%08X, context: %u\n",
-			   status, ctx_id);
-
-		status_pointer = I915_READ(RING_CONTEXT_STATUS_PTR(engine));
-		seq_printf(m, "\tStatus pointer: 0x%08X\n", status_pointer);
-
-		read_pointer = GEN8_CSB_READ_PTR(status_pointer);
-		write_pointer = GEN8_CSB_WRITE_PTR(status_pointer);
-		if (read_pointer > write_pointer)
-			write_pointer += GEN8_CSB_ENTRIES;
-		seq_printf(m, "\tRead pointer: 0x%08X, write pointer 0x%08X\n",
-			   read_pointer, write_pointer);
-
-		for (i = 0; i < GEN8_CSB_ENTRIES; i++) {
-			status = I915_READ(RING_CONTEXT_STATUS_BUF_LO(engine, i));
-			ctx_id = I915_READ(RING_CONTEXT_STATUS_BUF_HI(engine, i));
-
-			seq_printf(m, "\tStatus buffer %d: 0x%08X, context: %u\n",
-				   i, status, ctx_id);
-		}
-
-		spin_lock_bh(&engine->execlist_lock);
-		list_for_each(cursor, &engine->execlist_queue)
-			count++;
-		head_req = list_first_entry_or_null(&engine->execlist_queue,
-						    struct drm_i915_gem_request,
-						    execlist_link);
-		spin_unlock_bh(&engine->execlist_lock);
-
-		seq_printf(m, "\t%d requests in queue\n", count);
-		if (head_req) {
-			seq_printf(m, "\tHead request context: %u\n",
-				   head_req->ctx->hw_id);
-			seq_printf(m, "\tHead request tail: %u\n",
-				   head_req->tail);
-		}
-
-		seq_putc(m, '\n');
-	}
-
-	intel_runtime_pm_put(dev_priv);
-	mutex_unlock(&dev->struct_mutex);
-
-	return 0;
-}
-
 static const char *swizzle_string(unsigned swizzle)
 {
 	switch (swizzle) {
@@ -2201,14 +2165,15 @@ static int per_file_ctx(int id, void *ptr, void *data)
 static void gen8_ppgtt_info(struct seq_file *m,
 			    struct drm_i915_private *dev_priv)
 {
-	struct intel_engine_cs *engine;
 	struct i915_hw_ppgtt *ppgtt = dev_priv->mm.aliasing_ppgtt;
+	struct intel_engine_cs *engine;
+	enum intel_engine_id id;
 	int i;
 
 	if (!ppgtt)
 		return;
 
-	for_each_engine(engine, dev_priv) {
+	for_each_engine(engine, dev_priv, id) {
 		seq_printf(m, "%s\n", engine->name);
 		for (i = 0; i < 4; i++) {
 			u64 pdp = I915_READ(GEN8_RING_PDP_UDW(engine, i));
@@ -2223,11 +2188,12 @@ static void gen6_ppgtt_info(struct seq_file *m,
 			    struct drm_i915_private *dev_priv)
 {
 	struct intel_engine_cs *engine;
+	enum intel_engine_id id;
 
 	if (IS_GEN6(dev_priv))
 		seq_printf(m, "GFX_MODE: 0x%08x\n", I915_READ(GFX_MODE));
 
-	for_each_engine(engine, dev_priv) {
+	for_each_engine(engine, dev_priv, id) {
 		seq_printf(m, "%s\n", engine->name);
 		if (IS_GEN7(dev_priv))
 			seq_printf(m, "GFX_MODE: 0x%08x\n",
@@ -2296,9 +2262,10 @@ out_unlock:
 static int count_irq_waiters(struct drm_i915_private *i915)
 {
 	struct intel_engine_cs *engine;
+	enum intel_engine_id id;
 	int count = 0;
 
-	for_each_engine(engine, i915)
+	for_each_engine(engine, i915, id)
 		count += intel_engine_has_waiter(engine);
 
 	return count;
@@ -2461,7 +2428,7 @@ static void i915_guc_client_info(struct seq_file *m,
 	seq_printf(m, "\tFailed doorbell: %u\n", client->b_fail);
 	seq_printf(m, "\tLast submission result: %d\n", client->retcode);
 
-	for_each_engine_id(engine, dev_priv, id) {
+	for_each_engine(engine, dev_priv, id) {
 		u64 submissions = client->submissions[id];
 		tot += submissions;
 		seq_printf(m, "\tSubmissions: %llu %s\n",
@@ -2504,7 +2471,7 @@ static int i915_guc_info(struct seq_file *m, void *data)
 	seq_printf(m, "GuC last action error code: %d\n", guc.action_err);
 
 	seq_printf(m, "\nGuC submissions:\n");
-	for_each_engine_id(engine, dev_priv, id) {
+	for_each_engine(engine, dev_priv, id) {
 		u64 submissions = guc.submissions[id];
 		total += submissions;
 		seq_printf(m, "\t%-24s: %10llu, last seqno 0x%08x\n",
@@ -3121,6 +3088,134 @@ static int i915_display_info(struct seq_file *m, void *unused)
 	return 0;
 }
 
+static int i915_engine_info(struct seq_file *m, void *unused)
+{
+	struct drm_i915_private *dev_priv = node_to_i915(m->private);
+	struct intel_engine_cs *engine;
+	enum intel_engine_id id;
+
+	for_each_engine(engine, dev_priv, id) {
+		struct intel_breadcrumbs *b = &engine->breadcrumbs;
+		struct drm_i915_gem_request *rq;
+		struct rb_node *rb;
+		u64 addr;
+
+		seq_printf(m, "%s\n", engine->name);
+		seq_printf(m, "\tcurrent seqno %x, last %x, hangcheck %x [score %d]\n",
+			   intel_engine_get_seqno(engine),
+			   engine->last_submitted_seqno,
+			   engine->hangcheck.seqno,
+			   engine->hangcheck.score);
+
+		rcu_read_lock();
+
+		seq_printf(m, "\tRequests:\n");
+
+		rq = list_first_entry(&engine->request_list,
+				struct drm_i915_gem_request, link);
+		if (&rq->link != &engine->request_list)
+			print_request(m, rq, "\t\tfirst  ");
+
+		rq = list_last_entry(&engine->request_list,
+				struct drm_i915_gem_request, link);
+		if (&rq->link != &engine->request_list)
+			print_request(m, rq, "\t\tlast   ");
+
+		rq = i915_gem_find_active_request(engine);
+		if (rq) {
+			print_request(m, rq, "\t\tactive ");
+			seq_printf(m,
+				   "\t\t[head %04x, postfix %04x, tail %04x, batch 0x%08x_%08x]\n",
+				   rq->head, rq->postfix, rq->tail,
+				   rq->batch ? upper_32_bits(rq->batch->node.start) : ~0u,
+				   rq->batch ? lower_32_bits(rq->batch->node.start) : ~0u);
+		}
+
+		seq_printf(m, "\tRING_START: 0x%08x [0x%08x]\n",
+			   I915_READ(RING_START(engine->mmio_base)),
+			   rq ? i915_ggtt_offset(rq->ring->vma) : 0);
+		seq_printf(m, "\tRING_HEAD:  0x%08x [0x%08x]\n",
+			   I915_READ(RING_HEAD(engine->mmio_base)) & HEAD_ADDR,
+			   rq ? rq->ring->head : 0);
+		seq_printf(m, "\tRING_TAIL:  0x%08x [0x%08x]\n",
+			   I915_READ(RING_TAIL(engine->mmio_base)) & TAIL_ADDR,
+			   rq ? rq->ring->tail : 0);
+		seq_printf(m, "\tRING_CTL:   0x%08x [%s]\n",
+			   I915_READ(RING_CTL(engine->mmio_base)),
+			   I915_READ(RING_CTL(engine->mmio_base)) & (RING_WAIT | RING_WAIT_SEMAPHORE) ? "waiting" : "");
+
+		rcu_read_unlock();
+
+		addr = intel_engine_get_active_head(engine);
+		seq_printf(m, "\tACTHD:  0x%08x_%08x\n",
+			   upper_32_bits(addr), lower_32_bits(addr));
+		addr = intel_engine_get_last_batch_head(engine);
+		seq_printf(m, "\tBBADDR: 0x%08x_%08x\n",
+			   upper_32_bits(addr), lower_32_bits(addr));
+
+		if (i915.enable_execlists) {
+			u32 ptr, read, write;
+
+			seq_printf(m, "\tExeclist status: 0x%08x %08x\n",
+				   I915_READ(RING_EXECLIST_STATUS_LO(engine)),
+				   I915_READ(RING_EXECLIST_STATUS_HI(engine)));
+
+			ptr = I915_READ(RING_CONTEXT_STATUS_PTR(engine));
+			read = GEN8_CSB_READ_PTR(ptr);
+			write = GEN8_CSB_WRITE_PTR(ptr);
+			seq_printf(m, "\tExeclist CSB read %d, write %d\n",
+				   read, write);
+			if (read >= GEN8_CSB_ENTRIES)
+				read = 0;
+			if (write >= GEN8_CSB_ENTRIES)
+				write = 0;
+			if (read > write)
+				write += GEN8_CSB_ENTRIES;
+			while (read < write) {
+				unsigned int idx = ++read % GEN8_CSB_ENTRIES;
+
+				seq_printf(m, "\tExeclist CSB[%d]: 0x%08x, context: %d\n",
+					   idx,
+					   I915_READ(RING_CONTEXT_STATUS_BUF_LO(engine, idx)),
+					   I915_READ(RING_CONTEXT_STATUS_BUF_HI(engine, idx)));
+			}
+
+			rcu_read_lock();
+			rq = READ_ONCE(engine->execlist_port[0].request);
+			if (rq)
+				print_request(m, rq, "\t\tELSP[0] ");
+			else
+				seq_printf(m, "\t\tELSP[0] idle\n");
+			rq = READ_ONCE(engine->execlist_port[1].request);
+			if (rq)
+				print_request(m, rq, "\t\tELSP[1] ");
+			else
+				seq_printf(m, "\t\tELSP[1] idle\n");
+			rcu_read_unlock();
+		} else if (INTEL_GEN(dev_priv) > 6) {
+			seq_printf(m, "\tPP_DIR_BASE: 0x%08x\n",
+				   I915_READ(RING_PP_DIR_BASE(engine)));
+			seq_printf(m, "\tPP_DIR_BASE_READ: 0x%08x\n",
+				   I915_READ(RING_PP_DIR_BASE_READ(engine)));
+			seq_printf(m, "\tPP_DIR_DCLV: 0x%08x\n",
+				   I915_READ(RING_PP_DIR_DCLV(engine)));
+		}
+
+		spin_lock(&b->lock);
+		for (rb = rb_first(&b->waiters); rb; rb = rb_next(rb)) {
+			struct intel_wait *w = container_of(rb, typeof(*w), node);
+
+			seq_printf(m, "\t%s [%d] waiting for %x\n",
+				   w->tsk->comm, w->tsk->pid, w->seqno);
+		}
+		spin_unlock(&b->lock);
+
+		seq_puts(m, "\n");
+	}
+
+	return 0;
+}
+
 static int i915_semaphore_status(struct seq_file *m, void *unused)
 {
 	struct drm_i915_private *dev_priv = node_to_i915(m->private);
@@ -3147,7 +3242,7 @@ static int i915_semaphore_status(struct seq_file *m, void *unused)
 		page = i915_gem_object_get_page(dev_priv->semaphore->obj, 0);
 
 		seqno = (uint64_t *)kmap_atomic(page);
-		for_each_engine_id(engine, dev_priv, id) {
+		for_each_engine(engine, dev_priv, id) {
 			uint64_t offset;
 
 			seq_printf(m, "%s\n", engine->name);
@@ -3172,7 +3267,7 @@ static int i915_semaphore_status(struct seq_file *m, void *unused)
 		kunmap_atomic(seqno);
 	} else {
 		seq_puts(m, "  Last signal:");
-		for_each_engine(engine, dev_priv)
+		for_each_engine(engine, dev_priv, id)
 			for (j = 0; j < num_rings; j++)
 				seq_printf(m, "0x%08x\n",
 					   I915_READ(engine->semaphore.mbox.signal[j]));
@@ -3180,7 +3275,7 @@ static int i915_semaphore_status(struct seq_file *m, void *unused)
 	}
 
 	seq_puts(m, "\nSync seqno:\n");
-	for_each_engine(engine, dev_priv) {
+	for_each_engine(engine, dev_priv, id) {
 		for (j = 0; j < num_rings; j++)
 			seq_printf(m, "  0x%08x ",
 				   engine->semaphore.sync_seqno[j]);
@@ -3236,7 +3331,7 @@ static int i915_wa_registers(struct seq_file *m, void *unused)
 	intel_runtime_pm_get(dev_priv);
 
 	seq_printf(m, "Workarounds applied: %d\n", workarounds->count);
-	for_each_engine_id(engine, dev_priv, id)
+	for_each_engine(engine, dev_priv, id)
 		seq_printf(m, "HW whitelist count for %s: %d\n",
 			   engine->name, workarounds->hw_whitelist_count[id]);
 	for (i = 0; i < workarounds->count; ++i) {
@@ -4462,7 +4557,7 @@ static void wm_latency_show(struct seq_file *m, const uint16_t wm[8])
 	else if (IS_VALLEYVIEW(dev_priv))
 		num_levels = 1;
 	else
-		num_levels = ilk_wm_max_level(dev) + 1;
+		num_levels = ilk_wm_max_level(dev_priv) + 1;
 
 	drm_modeset_lock_all(dev);
 
@@ -4578,7 +4673,7 @@ static ssize_t wm_latency_write(struct file *file, const char __user *ubuf,
 	else if (IS_VALLEYVIEW(dev_priv))
 		num_levels = 1;
 	else
-		num_levels = ilk_wm_max_level(dev) + 1;
+		num_levels = ilk_wm_max_level(dev_priv) + 1;
 
 	if (len >= sizeof(tmp))
 		return -EINVAL;
@@ -5274,7 +5369,6 @@ static const struct drm_info_list i915_debugfs_list[] = {
 	{"i915_gem_framebuffer", i915_gem_framebuffer_info, 0},
 	{"i915_context_status", i915_context_status, 0},
 	{"i915_dump_lrc", i915_dump_lrc, 0},
-	{"i915_execlists", i915_execlists, 0},
 	{"i915_forcewake_domains", i915_forcewake_domains, 0},
 	{"i915_swizzle_info", i915_swizzle_info, 0},
 	{"i915_ppgtt_info", i915_ppgtt_info, 0},
@@ -5286,6 +5380,7 @@ static const struct drm_info_list i915_debugfs_list[] = {
 	{"i915_power_domain_info", i915_power_domain_info, 0},
 	{"i915_dmc_info", i915_dmc_info, 0},
 	{"i915_display_info", i915_display_info, 0},
+	{"i915_engine_info", i915_engine_info, 0},
 	{"i915_semaphore_status", i915_semaphore_status, 0},
 	{"i915_shared_dplls_info", i915_shared_dplls_info, 0},
 	{"i915_dp_mst_info", i915_dp_mst_info, 0},
@@ -5308,7 +5403,9 @@ static const struct i915_debugfs_files {
 	{"i915_ring_missed_irq", &i915_ring_missed_irq_fops},
 	{"i915_ring_test_irq", &i915_ring_test_irq_fops},
 	{"i915_gem_drop_caches", &i915_drop_caches_fops},
+#if IS_ENABLED(CONFIG_DRM_I915_CAPTURE_ERROR)
 	{"i915_error_state", &i915_error_state_fops},
+#endif
 	{"i915_next_seqno", &i915_next_seqno_fops},
 	{"i915_display_crc_ctl", &i915_display_crc_ctl_fops},
 	{"i915_pri_wm_latency", &i915_pri_wm_latency_fops},
diff --git a/drivers/gpu/drm/i915/i915_drv.c b/drivers/gpu/drm/i915/i915_drv.c
index 7f4e8adec8a8..912d5348e3e7 100644
--- a/drivers/gpu/drm/i915/i915_drv.c
+++ b/drivers/gpu/drm/i915/i915_drv.c
@@ -114,7 +114,7 @@ static bool i915_error_injected(struct drm_i915_private *dev_priv)
 		      fmt, ##__VA_ARGS__)
 
 
-static enum intel_pch intel_virt_detect_pch(struct drm_device *dev)
+static enum intel_pch intel_virt_detect_pch(struct drm_i915_private *dev_priv)
 {
 	enum intel_pch ret = PCH_NOP;
 
@@ -125,16 +125,16 @@ static enum intel_pch intel_virt_detect_pch(struct drm_device *dev)
 	 * make an educated guess as to which PCH is really there.
 	 */
 
-	if (IS_GEN5(dev)) {
+	if (IS_GEN5(dev_priv)) {
 		ret = PCH_IBX;
 		DRM_DEBUG_KMS("Assuming Ibex Peak PCH\n");
-	} else if (IS_GEN6(dev) || IS_IVYBRIDGE(dev)) {
+	} else if (IS_GEN6(dev_priv) || IS_IVYBRIDGE(dev_priv)) {
 		ret = PCH_CPT;
 		DRM_DEBUG_KMS("Assuming CouarPoint PCH\n");
-	} else if (IS_HASWELL(dev) || IS_BROADWELL(dev)) {
+	} else if (IS_HASWELL(dev_priv) || IS_BROADWELL(dev_priv)) {
 		ret = PCH_LPT;
 		DRM_DEBUG_KMS("Assuming LynxPoint PCH\n");
-	} else if (IS_SKYLAKE(dev) || IS_KABYLAKE(dev)) {
+	} else if (IS_SKYLAKE(dev_priv) || IS_KABYLAKE(dev_priv)) {
 		ret = PCH_SPT;
 		DRM_DEBUG_KMS("Assuming SunrisePoint PCH\n");
 	}
@@ -174,40 +174,46 @@ static void intel_detect_pch(struct drm_device *dev)
 			if (id == INTEL_PCH_IBX_DEVICE_ID_TYPE) {
 				dev_priv->pch_type = PCH_IBX;
 				DRM_DEBUG_KMS("Found Ibex Peak PCH\n");
-				WARN_ON(!IS_GEN5(dev));
+				WARN_ON(!IS_GEN5(dev_priv));
 			} else if (id == INTEL_PCH_CPT_DEVICE_ID_TYPE) {
 				dev_priv->pch_type = PCH_CPT;
 				DRM_DEBUG_KMS("Found CougarPoint PCH\n");
-				WARN_ON(!(IS_GEN6(dev) || IS_IVYBRIDGE(dev)));
+				WARN_ON(!(IS_GEN6(dev_priv) ||
+					IS_IVYBRIDGE(dev_priv)));
 			} else if (id == INTEL_PCH_PPT_DEVICE_ID_TYPE) {
 				/* PantherPoint is CPT compatible */
 				dev_priv->pch_type = PCH_CPT;
 				DRM_DEBUG_KMS("Found PantherPoint PCH\n");
-				WARN_ON(!(IS_GEN6(dev) || IS_IVYBRIDGE(dev)));
+				WARN_ON(!(IS_GEN6(dev_priv) ||
+					IS_IVYBRIDGE(dev_priv)));
 			} else if (id == INTEL_PCH_LPT_DEVICE_ID_TYPE) {
 				dev_priv->pch_type = PCH_LPT;
 				DRM_DEBUG_KMS("Found LynxPoint PCH\n");
-				WARN_ON(!IS_HASWELL(dev) && !IS_BROADWELL(dev));
-				WARN_ON(IS_HSW_ULT(dev) || IS_BDW_ULT(dev));
+				WARN_ON(!IS_HASWELL(dev_priv) &&
+					!IS_BROADWELL(dev_priv));
+				WARN_ON(IS_HSW_ULT(dev_priv) ||
+					IS_BDW_ULT(dev_priv));
 			} else if (id == INTEL_PCH_LPT_LP_DEVICE_ID_TYPE) {
 				dev_priv->pch_type = PCH_LPT;
 				DRM_DEBUG_KMS("Found LynxPoint LP PCH\n");
-				WARN_ON(!IS_HASWELL(dev) && !IS_BROADWELL(dev));
-				WARN_ON(!IS_HSW_ULT(dev) && !IS_BDW_ULT(dev));
+				WARN_ON(!IS_HASWELL(dev_priv) &&
+					!IS_BROADWELL(dev_priv));
+				WARN_ON(!IS_HSW_ULT(dev_priv) &&
+					!IS_BDW_ULT(dev_priv));
 			} else if (id == INTEL_PCH_SPT_DEVICE_ID_TYPE) {
 				dev_priv->pch_type = PCH_SPT;
 				DRM_DEBUG_KMS("Found SunrisePoint PCH\n");
-				WARN_ON(!IS_SKYLAKE(dev) &&
-					!IS_KABYLAKE(dev));
+				WARN_ON(!IS_SKYLAKE(dev_priv) &&
+					!IS_KABYLAKE(dev_priv));
 			} else if (id == INTEL_PCH_SPT_LP_DEVICE_ID_TYPE) {
 				dev_priv->pch_type = PCH_SPT;
 				DRM_DEBUG_KMS("Found SunrisePoint LP PCH\n");
-				WARN_ON(!IS_SKYLAKE(dev) &&
-					!IS_KABYLAKE(dev));
+				WARN_ON(!IS_SKYLAKE(dev_priv) &&
+					!IS_KABYLAKE(dev_priv));
 			} else if (id == INTEL_PCH_KBP_DEVICE_ID_TYPE) {
 				dev_priv->pch_type = PCH_KBP;
 				DRM_DEBUG_KMS("Found KabyPoint PCH\n");
-				WARN_ON(!IS_KABYLAKE(dev));
+				WARN_ON(!IS_KABYLAKE(dev_priv));
 			} else if ((id == INTEL_PCH_P2X_DEVICE_ID_TYPE) ||
 				   (id == INTEL_PCH_P3X_DEVICE_ID_TYPE) ||
 				   ((id == INTEL_PCH_QEMU_DEVICE_ID_TYPE) &&
@@ -215,7 +221,8 @@ static void intel_detect_pch(struct drm_device *dev)
 					    PCI_SUBVENDOR_ID_REDHAT_QUMRANET &&
 				    pch->subsystem_device ==
 					    PCI_SUBDEVICE_ID_QEMU)) {
-				dev_priv->pch_type = intel_virt_detect_pch(dev);
+				dev_priv->pch_type =
+					intel_virt_detect_pch(dev_priv);
 			} else
 				continue;
 
@@ -255,16 +262,16 @@ static int i915_getparam(struct drm_device *dev, void *data,
 		value = dev_priv->overlay ? 1 : 0;
 		break;
 	case I915_PARAM_HAS_BSD:
-		value = intel_engine_initialized(&dev_priv->engine[VCS]);
+		value = !!dev_priv->engine[VCS];
 		break;
 	case I915_PARAM_HAS_BLT:
-		value = intel_engine_initialized(&dev_priv->engine[BCS]);
+		value = !!dev_priv->engine[BCS];
 		break;
 	case I915_PARAM_HAS_VEBOX:
-		value = intel_engine_initialized(&dev_priv->engine[VECS]);
+		value = !!dev_priv->engine[VECS];
 		break;
 	case I915_PARAM_HAS_BSD2:
-		value = intel_engine_initialized(&dev_priv->engine[VCS2]);
+		value = !!dev_priv->engine[VCS2];
 		break;
 	case I915_PARAM_HAS_EXEC_CONSTANTS:
 		value = INTEL_GEN(dev_priv) >= 4;
@@ -417,12 +424,12 @@ intel_setup_mchbar(struct drm_device *dev)
 	u32 temp;
 	bool enabled;
 
-	if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev))
+	if (IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv))
 		return;
 
 	dev_priv->mchbar_need_disable = false;
 
-	if (IS_I915G(dev) || IS_I915GM(dev)) {
+	if (IS_I915G(dev_priv) || IS_I915GM(dev_priv)) {
 		pci_read_config_dword(dev_priv->bridge_dev, DEVEN, &temp);
 		enabled = !!(temp & DEVEN_MCHBAR_EN);
 	} else {
@@ -440,7 +447,7 @@ intel_setup_mchbar(struct drm_device *dev)
 	dev_priv->mchbar_need_disable = true;
 
 	/* Space is allocated or reserved, so enable it. */
-	if (IS_I915G(dev) || IS_I915GM(dev)) {
+	if (IS_I915G(dev_priv) || IS_I915GM(dev_priv)) {
 		pci_write_config_dword(dev_priv->bridge_dev, DEVEN,
 				       temp | DEVEN_MCHBAR_EN);
 	} else {
@@ -456,7 +463,7 @@ intel_teardown_mchbar(struct drm_device *dev)
 	int mchbar_reg = INTEL_INFO(dev)->gen >= 4 ? MCHBAR_I965 : MCHBAR_I915;
 
 	if (dev_priv->mchbar_need_disable) {
-		if (IS_I915G(dev) || IS_I915GM(dev)) {
+		if (IS_I915G(dev_priv) || IS_I915GM(dev_priv)) {
 			u32 deven_val;
 
 			pci_read_config_dword(dev_priv->bridge_dev, DEVEN,
@@ -532,32 +539,6 @@ static const struct vga_switcheroo_client_ops i915_switcheroo_ops = {
 
 static void i915_gem_fini(struct drm_device *dev)
 {
-	struct drm_i915_private *dev_priv = to_i915(dev);
-
-	/*
-	 * Neither the BIOS, ourselves or any other kernel
-	 * expects the system to be in execlists mode on startup,
-	 * so we need to reset the GPU back to legacy mode. And the only
-	 * known way to disable logical contexts is through a GPU reset.
-	 *
-	 * So in order to leave the system in a known default configuration,
-	 * always reset the GPU upon unload. Afterwards we then clean up the
-	 * GEM state tracking, flushing off the requests and leaving the
-	 * system in a known idle state.
-	 *
-	 * Note that is of the upmost importance that the GPU is idle and
-	 * all stray writes are flushed *before* we dismantle the backing
-	 * storage for the pinned objects.
-	 *
-	 * However, since we are uncertain that reseting the GPU on older
-	 * machines is a good idea, we don't - just in case it leaves the
-	 * machine in an unusable condition.
-	 */
-	if (HAS_HW_CONTEXTS(dev)) {
-		int reset = intel_gpu_reset(dev_priv, ALL_ENGINES);
-		WARN_ON(reset && reset != -ENODEV);
-	}
-
 	mutex_lock(&dev->struct_mutex);
 	i915_gem_cleanup_engines(dev);
 	i915_gem_context_fini(dev);
@@ -636,6 +617,8 @@ static int i915_load_modeset_init(struct drm_device *dev)
 	return 0;
 
 cleanup_gem:
+	if (i915_gem_suspend(dev))
+		DRM_ERROR("failed to idle hardware; continuing to unload!\n");
 	i915_gem_fini(dev);
 cleanup_irq:
 	intel_guc_fini(dev);
@@ -771,6 +754,19 @@ static void i915_workqueues_cleanup(struct drm_i915_private *dev_priv)
 	destroy_workqueue(dev_priv->wq);
 }
 
+/*
+ * We don't keep the workarounds for pre-production hardware, so we expect our
+ * driver to fail on these machines in one way or another. A little warning on
+ * dmesg may help both the user and the bug triagers.
+ */
+static void intel_detect_preproduction_hw(struct drm_i915_private *dev_priv)
+{
+	if (IS_HSW_EARLY_SDV(dev_priv) ||
+	    IS_SKL_REVID(dev_priv, 0, SKL_REVID_F0))
+		DRM_ERROR("This is a pre-production stepping. "
+			  "It may not be fully functional.\n");
+}
+
 /**
  * i915_driver_init_early - setup state not requiring device access
  * @dev_priv: device private
@@ -838,13 +834,7 @@ static int i915_driver_init_early(struct drm_i915_private *dev_priv,
 
 	intel_device_info_dump(dev_priv);
 
-	/* Not all pre-production machines fall into this category, only the
-	 * very first ones. Almost everything should work, except for maybe
-	 * suspend/resume. And we don't implement workarounds that affect only
-	 * pre-production machines. */
-	if (IS_HSW_EARLY_SDV(dev_priv))
-		DRM_INFO("This is an early pre-production Haswell machine. "
-			 "It may not be fully functional.\n");
+	intel_detect_preproduction_hw(dev_priv);
 
 	return 0;
 
@@ -870,7 +860,7 @@ static int i915_mmio_setup(struct drm_device *dev)
 	int mmio_bar;
 	int mmio_size;
 
-	mmio_bar = IS_GEN2(dev) ? 1 : 0;
+	mmio_bar = IS_GEN2(dev_priv) ? 1 : 0;
 	/*
 	 * Before gen4, the registers and the GTT are behind different BARs.
 	 * However, from gen4 onwards, the registers and the GTT are shared
@@ -1023,7 +1013,7 @@ static int i915_driver_init_hw(struct drm_i915_private *dev_priv)
 	pci_set_master(pdev);
 
 	/* overlay on gen2 is broken and can't address above 1G */
-	if (IS_GEN2(dev)) {
+	if (IS_GEN2(dev_priv)) {
 		ret = dma_set_coherent_mask(&pdev->dev, DMA_BIT_MASK(30));
 		if (ret) {
 			DRM_ERROR("failed to set DMA mask\n");
@@ -1070,7 +1060,7 @@ static int i915_driver_init_hw(struct drm_i915_private *dev_priv)
 	 * be lost or delayed, but we use them anyways to avoid
 	 * stuck interrupts on some machines.
 	 */
-	if (!IS_I945G(dev) && !IS_I945GM(dev)) {
+	if (!IS_I945G(dev_priv) && !IS_I945GM(dev_priv)) {
 		if (pci_enable_msi(pdev) < 0)
 			DRM_DEBUG_DRIVER("can't enable MSI");
 	}
@@ -1242,6 +1232,10 @@ int i915_driver_load(struct pci_dev *pdev, const struct pci_device_id *ent)
 	DRM_INFO("Initialized %s %d.%d.%d %s for %s on minor %d\n",
 		 driver.name, driver.major, driver.minor, driver.patchlevel,
 		 driver.date, pci_name(pdev), dev_priv->drm.primary->index);
+	if (IS_ENABLED(CONFIG_DRM_I915_DEBUG))
+		DRM_INFO("DRM_I915_DEBUG enabled\n");
+	if (IS_ENABLED(CONFIG_DRM_I915_DEBUG_GEM))
+		DRM_INFO("DRM_I915_DEBUG_GEM enabled\n");
 
 	intel_runtime_pm_put(dev_priv);
 
@@ -1447,8 +1441,6 @@ static int i915_drm_suspend(struct drm_device *dev)
 
 	dev_priv->suspend_count++;
 
-	intel_display_set_init_power(dev_priv, false);
-
 	intel_csr_ucode_suspend(dev_priv);
 
 out:
@@ -1466,6 +1458,8 @@ static int i915_drm_suspend_late(struct drm_device *dev, bool hibernation)
 
 	disable_rpm_wakeref_asserts(dev_priv);
 
+	intel_display_set_init_power(dev_priv, false);
+
 	fw_csr = !IS_BROXTON(dev_priv) &&
 		suspend_to_idle(dev_priv) && dev_priv->csr.dmc_payload;
 	/*
@@ -1721,6 +1715,22 @@ int i915_resume_switcheroo(struct drm_device *dev)
 	return i915_drm_resume(dev);
 }
 
+static void disable_engines_irq(struct drm_i915_private *dev_priv)
+{
+	struct intel_engine_cs *engine;
+	enum intel_engine_id id;
+
+	/* Ensure irq handler finishes, and not run again. */
+	disable_irq(dev_priv->drm.irq);
+	for_each_engine(engine, dev_priv, id)
+		tasklet_kill(&engine->irq_tasklet);
+}
+
+static void enable_engines_irq(struct drm_i915_private *dev_priv)
+{
+	enable_irq(dev_priv->drm.irq);
+}
+
 /**
  * i915_reset - reset chip after a hang
  * @dev: drm device to reset
@@ -1754,7 +1764,11 @@ void i915_reset(struct drm_i915_private *dev_priv)
 	error->reset_count++;
 
 	pr_notice("drm/i915: Resetting chip after gpu hang\n");
+
+	disable_engines_irq(dev_priv);
 	ret = intel_gpu_reset(dev_priv, ALL_ENGINES);
+	enable_engines_irq(dev_priv);
+
 	if (ret) {
 		if (ret != -ENODEV)
 			DRM_ERROR("Failed to reset chip: %i\n", ret);
@@ -1786,15 +1800,6 @@ void i915_reset(struct drm_i915_private *dev_priv)
 		goto error;
 	}
 
-	/*
-	 * rps/rc6 re-init is necessary to restore state lost after the
-	 * reset and the re-install of gt irqs. Skip for ironlake per
-	 * previous concerns that it doesn't respond well to some forms
-	 * of re-init after reset.
-	 */
-	intel_sanitize_gt_powersave(dev_priv);
-	intel_autoenable_gt_powersave(dev_priv);
-
 wakeup:
 	wake_up_bit(&error->flags, I915_RESET_IN_PROGRESS);
 	return;
@@ -1872,7 +1877,17 @@ static int i915_pm_resume(struct device *kdev)
 /* freeze: before creating the hibernation_image */
 static int i915_pm_freeze(struct device *kdev)
 {
-	return i915_pm_suspend(kdev);
+	int ret;
+
+	ret = i915_pm_suspend(kdev);
+	if (ret)
+		return ret;
+
+	ret = i915_gem_freeze(kdev_to_i915(kdev));
+	if (ret)
+		return ret;
+
+	return 0;
 }
 
 static int i915_pm_freeze_late(struct device *kdev)
@@ -2281,7 +2296,7 @@ static int intel_runtime_suspend(struct device *kdev)
 	if (WARN_ON_ONCE(!(dev_priv->rps.enabled && intel_enable_rc6())))
 		return -ENODEV;
 
-	if (WARN_ON_ONCE(!HAS_RUNTIME_PM(dev)))
+	if (WARN_ON_ONCE(!HAS_RUNTIME_PM(dev_priv)))
 		return -ENODEV;
 
 	DRM_DEBUG_KMS("Suspending device\n");
@@ -2385,7 +2400,7 @@ static int intel_runtime_resume(struct device *kdev)
 	struct drm_i915_private *dev_priv = to_i915(dev);
 	int ret = 0;
 
-	if (WARN_ON_ONCE(!HAS_RUNTIME_PM(dev)))
+	if (WARN_ON_ONCE(!HAS_RUNTIME_PM(dev_priv)))
 		return -ENODEV;
 
 	DRM_DEBUG_KMS("Resuming device\n");
@@ -2403,7 +2418,7 @@ static int intel_runtime_resume(struct device *kdev)
 	if (IS_GEN6(dev_priv))
 		intel_init_pch_refclk(dev);
 
-	if (IS_BROXTON(dev)) {
+	if (IS_BROXTON(dev_priv)) {
 		bxt_disable_dc9(dev_priv);
 		bxt_display_core_init(dev_priv, true);
 		if (dev_priv->csr.dmc_payload &&
diff --git a/drivers/gpu/drm/i915/i915_drv.h b/drivers/gpu/drm/i915/i915_drv.h
index 4dd307ed4336..f022f438e5b9 100644
--- a/drivers/gpu/drm/i915/i915_drv.h
+++ b/drivers/gpu/drm/i915/i915_drv.h
@@ -70,7 +70,8 @@
 
 #define DRIVER_NAME		"i915"
 #define DRIVER_DESC		"Intel Graphics"
-#define DRIVER_DATE		"20160919"
+#define DRIVER_DATE		"20161024"
+#define DRIVER_TIMESTAMP	1477290335
 
 #undef WARN_ON
 /* Many gcc seem to no see through this and fall over :( */
@@ -185,6 +186,7 @@ enum plane {
 #define sprite_name(p, s) ((p) * INTEL_INFO(dev)->num_sprites[(p)] + (s) + 'A')
 
 enum port {
+	PORT_NONE = -1,
 	PORT_A = 0,
 	PORT_B,
 	PORT_C,
@@ -581,13 +583,25 @@ struct intel_uncore_funcs {
 				uint32_t val, bool trace);
 };
 
+struct intel_forcewake_range {
+	u32 start;
+	u32 end;
+
+	enum forcewake_domains domains;
+};
+
 struct intel_uncore {
 	spinlock_t lock; /** lock is also taken in irq contexts. */
 
+	const struct intel_forcewake_range *fw_domains_table;
+	unsigned int fw_domains_table_entries;
+
 	struct intel_uncore_funcs funcs;
 
 	unsigned fifo_count;
+
 	enum forcewake_domains fw_domains;
+	enum forcewake_domains fw_domains_active;
 
 	struct intel_uncore_forcewake_domain {
 		struct drm_i915_private *i915;
@@ -633,54 +647,53 @@ struct intel_csr {
 	uint32_t allowed_dc_mask;
 };
 
-#define DEV_INFO_FOR_EACH_FLAG(func, sep) \
-	func(is_mobile) sep \
-	func(is_i85x) sep \
-	func(is_i915g) sep \
-	func(is_i945gm) sep \
-	func(is_g33) sep \
-	func(hws_needs_physical) sep \
-	func(is_g4x) sep \
-	func(is_pineview) sep \
-	func(is_broadwater) sep \
-	func(is_crestline) sep \
-	func(is_ivybridge) sep \
-	func(is_valleyview) sep \
-	func(is_cherryview) sep \
-	func(is_haswell) sep \
-	func(is_broadwell) sep \
-	func(is_skylake) sep \
-	func(is_broxton) sep \
-	func(is_kabylake) sep \
-	func(is_preliminary) sep \
-	func(has_fbc) sep \
-	func(has_psr) sep \
-	func(has_runtime_pm) sep \
-	func(has_csr) sep \
-	func(has_resource_streamer) sep \
-	func(has_rc6) sep \
-	func(has_rc6p) sep \
-	func(has_dp_mst) sep \
-	func(has_gmbus_irq) sep \
-	func(has_hw_contexts) sep \
-	func(has_logical_ring_contexts) sep \
-	func(has_l3_dpf) sep \
-	func(has_gmch_display) sep \
-	func(has_guc) sep \
-	func(has_pipe_cxsr) sep \
-	func(has_hotplug) sep \
-	func(cursor_needs_physical) sep \
-	func(has_overlay) sep \
-	func(overlay_needs_physical) sep \
-	func(supports_tv) sep \
-	func(has_llc) sep \
-	func(has_snoop) sep \
-	func(has_ddi) sep \
-	func(has_fpga_dbg) sep \
-	func(has_pooled_eu)
-
-#define DEFINE_FLAG(name) u8 name:1
-#define SEP_SEMICOLON ;
+#define DEV_INFO_FOR_EACH_FLAG(func) \
+	/* Keep is_* in chronological order */ \
+	func(is_mobile); \
+	func(is_i85x); \
+	func(is_i915g); \
+	func(is_i945gm); \
+	func(is_g33); \
+	func(is_g4x); \
+	func(is_pineview); \
+	func(is_broadwater); \
+	func(is_crestline); \
+	func(is_ivybridge); \
+	func(is_valleyview); \
+	func(is_cherryview); \
+	func(is_haswell); \
+	func(is_broadwell); \
+	func(is_skylake); \
+	func(is_broxton); \
+	func(is_kabylake); \
+	func(is_preliminary); \
+	/* Keep has_* in alphabetical order */ \
+	func(has_csr); \
+	func(has_ddi); \
+	func(has_dp_mst); \
+	func(has_fbc); \
+	func(has_fpga_dbg); \
+	func(has_gmbus_irq); \
+	func(has_gmch_display); \
+	func(has_guc); \
+	func(has_hotplug); \
+	func(has_hw_contexts); \
+	func(has_l3_dpf); \
+	func(has_llc); \
+	func(has_logical_ring_contexts); \
+	func(has_overlay); \
+	func(has_pipe_cxsr); \
+	func(has_pooled_eu); \
+	func(has_psr); \
+	func(has_rc6); \
+	func(has_rc6p); \
+	func(has_resource_streamer); \
+	func(has_runtime_pm); \
+	func(has_snoop); \
+	func(cursor_needs_physical); \
+	func(hws_needs_physical); \
+	func(overlay_needs_physical); \
+	func(supports_tv)
 
 struct sseu_dev_info {
 	u8 slice_mask;
@@ -709,7 +722,9 @@ struct intel_device_info {
 	u16 gen_mask;
 	u8 ring_mask; /* Rings supported by the HW */
 	u8 num_rings;
-	DEV_INFO_FOR_EACH_FLAG(DEFINE_FLAG, SEP_SEMICOLON);
+#define DEFINE_FLAG(name) u8 name:1
+	DEV_INFO_FOR_EACH_FLAG(DEFINE_FLAG);
+#undef DEFINE_FLAG
 	u16 ddb_size; /* in blocks */
 	/* Register offsets for the various display pipes and transcoders */
 	int pipe_offsets[I915_MAX_TRANSCODERS];
@@ -726,15 +741,14 @@ struct intel_device_info {
 	} color;
 };
 
-#undef DEFINE_FLAG
-#undef SEP_SEMICOLON
-
 struct intel_display_error_state;
 
 struct drm_i915_error_state {
 	struct kref ref;
 	struct timeval time;
 
+	struct drm_i915_private *i915;
+
 	char error_msg[128];
 	bool simulated;
 	int iommu;
@@ -759,7 +773,7 @@ struct drm_i915_error_state {
 	u32 gam_ecochk;
 	u32 gab_ctl;
 	u32 gfx_mode;
-	u32 extra_instdone[I915_NUM_INSTDONE_REG];
+
 	u64 fence[I915_MAX_NUM_FENCES];
 	struct intel_overlay_error_state *overlay;
 	struct intel_display_error_state *display;
@@ -775,6 +789,9 @@ struct drm_i915_error_state {
 		struct i915_address_space *vm;
 		int num_requests;
 
+		/* position of active request inside the ring */
+		u32 rq_head, rq_post, rq_tail;
+
 		/* our own tracking of ring head and tail */
 		u32 cpu_ring_head;
 		u32 cpu_ring_tail;
@@ -791,7 +808,6 @@ struct drm_i915_error_state {
 		u32 hws;
 		u32 ipeir;
 		u32 ipehr;
-		u32 instdone;
 		u32 bbstate;
 		u32 instpm;
 		u32 instps;
@@ -802,11 +818,13 @@ struct drm_i915_error_state {
 		u64 faddr;
 		u32 rc_psmi; /* sleep state */
 		u32 semaphore_mboxes[I915_NUM_ENGINES - 1];
+		struct intel_instdone instdone;
 
 		struct drm_i915_error_object {
-			int page_count;
 			u64 gtt_offset;
 			u64 gtt_size;
+			int page_count;
+			int unused;
 			u32 *pages[0];
 		} *ringbuffer, *batchbuffer, *wa_batchbuffer, *ctx, *hws_page;
 
@@ -815,10 +833,11 @@ struct drm_i915_error_state {
 		struct drm_i915_error_request {
 			long jiffies;
 			pid_t pid;
+			u32 context;
 			u32 seqno;
 			u32 head;
 			u32 tail;
-		} *requests;
+		} *requests, execlist[2];
 
 		struct drm_i915_error_waiter {
 			char comm[TASK_COMM_LEN];
@@ -972,6 +991,9 @@ struct intel_fbc {
 	bool enabled;
 	bool active;
 
+	bool underrun_detected;
+	struct work_struct underrun_work;
+
 	struct intel_fbc_state_cache {
 		struct {
 			unsigned int mode_flags;
@@ -1368,7 +1390,7 @@ struct i915_gem_mm {
 
 	/* accounting, useful for userland debugging */
 	spinlock_t object_stat_lock;
-	size_t object_memory;
+	u64 object_memory;
 	u32 object_count;
 };
 
@@ -1620,7 +1642,6 @@ static inline bool skl_ddb_entry_equal(const struct skl_ddb_entry *e1,
 }
 
 struct skl_ddb_allocation {
-	struct skl_ddb_entry pipe[I915_MAX_PIPES];
 	struct skl_ddb_entry plane[I915_MAX_PIPES][I915_MAX_PLANES]; /* packed/uv */
 	struct skl_ddb_entry y_plane[I915_MAX_PIPES][I915_MAX_PLANES];
 };
@@ -1628,15 +1649,12 @@ struct skl_ddb_allocation {
 struct skl_wm_values {
 	unsigned dirty_pipes;
 	struct skl_ddb_allocation ddb;
-	uint32_t wm_linetime[I915_MAX_PIPES];
-	uint32_t plane[I915_MAX_PIPES][I915_MAX_PLANES][8];
-	uint32_t plane_trans[I915_MAX_PIPES][I915_MAX_PLANES];
 };
 
 struct skl_wm_level {
-	bool plane_en[I915_MAX_PLANES];
-	uint16_t plane_res_b[I915_MAX_PLANES];
-	uint8_t plane_res_l[I915_MAX_PLANES];
+	bool plane_en;
+	uint16_t plane_res_b;
+	uint8_t plane_res_l;
 };
 
 /*
@@ -1759,7 +1777,7 @@ struct drm_i915_private {
 
 	struct i915_virtual_gpu vgpu;
 
-	struct intel_gvt gvt;
+	struct intel_gvt *gvt;
 
 	struct intel_guc guc;
 
@@ -1787,7 +1805,7 @@ struct drm_i915_private {
 
 	struct pci_dev *bridge_dev;
 	struct i915_gem_context *kernel_context;
-	struct intel_engine_cs engine[I915_NUM_ENGINES];
+	struct intel_engine_cs *engine[I915_NUM_ENGINES];
 	struct i915_vma *semaphore;
 	u32 next_seqno;
 
@@ -1984,11 +2002,11 @@ struct drm_i915_private {
 	struct vlv_s0ix_state vlv_s0ix_state;
 
 	enum {
-		I915_SKL_SAGV_UNKNOWN = 0,
-		I915_SKL_SAGV_DISABLED,
-		I915_SKL_SAGV_ENABLED,
-		I915_SKL_SAGV_NOT_CONTROLLED
-	} skl_sagv_status;
+		I915_SAGV_UNKNOWN = 0,
+		I915_SAGV_DISABLED,
+		I915_SAGV_ENABLED,
+		I915_SAGV_NOT_CONTROLLED
+	} sagv_status;
 
 	struct {
 		/*
@@ -2079,7 +2097,8 @@ struct drm_i915_private {
 	/* perform PHY state sanity checks? */
 	bool chv_phy_assert[2];
 
-	struct intel_encoder *dig_port_map[I915_MAX_PORTS];
+	/* Used to save the pipe-to-encoder mapping for audio */
+	struct intel_encoder *av_enc_map[I915_MAX_PIPES];
 
 	/*
 	 * NOTE: This is the dri1/ums dungeon, don't add stuff here. Your patch
@@ -2103,19 +2122,11 @@ static inline struct drm_i915_private *guc_to_i915(struct intel_guc *guc)
 }
 
 /* Simple iterator over all initialised engines */
-#define for_each_engine(engine__, dev_priv__) \
-	for ((engine__) = &(dev_priv__)->engine[0]; \
-	     (engine__) < &(dev_priv__)->engine[I915_NUM_ENGINES]; \
-	     (engine__)++) \
-		for_each_if (intel_engine_initialized(engine__))
-
-/* Iterator with engine_id */
-#define for_each_engine_id(engine__, dev_priv__, id__) \
-	for ((engine__) = &(dev_priv__)->engine[0], (id__) = 0; \
-	     (engine__) < &(dev_priv__)->engine[I915_NUM_ENGINES]; \
-	     (engine__)++) \
-		for_each_if (((id__) = (engine__)->id, \
-			      intel_engine_initialized(engine__)))
+#define for_each_engine(engine__, dev_priv__, id__) \
+	for ((id__) = 0; \
+	     (id__) < I915_NUM_ENGINES; \
+	     (id__)++) \
+		for_each_if ((engine__) = (dev_priv__)->engine[(id__)])
 
 #define __mask_next_bit(mask) ({					\
 	int __idx = ffs(mask) - 1;					\
@@ -2126,7 +2137,7 @@ static inline struct drm_i915_private *guc_to_i915(struct intel_guc *guc)
 /* Iterator over subset of engines selected by mask */
 #define for_each_engine_masked(engine__, dev_priv__, mask__, tmp__) \
 	for (tmp__ = mask__ & INTEL_INFO(dev_priv__)->ring_mask;	\
-	     tmp__ ? (engine__ = &(dev_priv__)->engine[__mask_next_bit(tmp__)]), 1 : 0; )
+	     tmp__ ? (engine__ = (dev_priv__)->engine[__mask_next_bit(tmp__)]), 1 : 0; )
 
 enum hdmi_force_audio {
 	HDMI_AUDIO_OFF_DVI = -2,	/* no aux data for HDMI-DVI converter */
@@ -2276,21 +2287,19 @@ struct drm_i915_gem_object {
 	/** Record of address bit 17 of each page at last unbind. */
 	unsigned long *bit_17;
 
-	union {
-		/** for phy allocated objects */
-		struct drm_dma_handle *phys_handle;
-
-		struct i915_gem_userptr {
-			uintptr_t ptr;
-			unsigned read_only :1;
-			unsigned workers :4;
+	struct i915_gem_userptr {
+		uintptr_t ptr;
+		unsigned read_only :1;
+		unsigned workers :4;
 #define I915_GEM_USERPTR_MAX_WORKERS 15
 
-			struct i915_mm_struct *mm;
-			struct i915_mmu_object *mmu_object;
-			struct work_struct *work;
-		} userptr;
-	};
+		struct i915_mm_struct *mm;
+		struct i915_mmu_object *mmu_object;
+		struct work_struct *work;
+	} userptr;
+
+	/** for phys allocated objects */
+	struct drm_dma_handle *phys_handle;
 };
 
 static inline struct drm_i915_gem_object *
@@ -2588,8 +2597,9 @@ struct drm_i915_cmd_table {
 	__p; \
 })
 #define INTEL_INFO(p)	(&__I915__(p)->info)
-#define INTEL_GEN(p)	(INTEL_INFO(p)->gen)
-#define INTEL_DEVID(p)	(INTEL_INFO(p)->device_id)
+
+#define INTEL_GEN(dev_priv)	((dev_priv)->info.gen)
+#define INTEL_DEVID(dev_priv)	((dev_priv)->info.device_id)
 
 #define REVID_FOREVER		0xff
 #define INTEL_REVID(p)	(__I915__(p)->drm.pdev->revision)
@@ -2600,7 +2610,7 @@ struct drm_i915_cmd_table {
  *
  * Use GEN_FOREVER for unbound start and or end.
  */
-#define IS_GEN(p, s, e) ({ \
+#define IS_GEN(dev_priv, s, e) ({ \
 	unsigned int __s = (s), __e = (e); \
 	BUILD_BUG_ON(!__builtin_constant_p(s)); \
 	BUILD_BUG_ON(!__builtin_constant_p(e)); \
@@ -2610,7 +2620,7 @@ struct drm_i915_cmd_table {
 		__e = BITS_PER_LONG - 1; \
 	else \
 		__e = (e) - 1; \
-	!!(INTEL_INFO(p)->gen_mask & GENMASK((__e), (__s))); \
+	!!((dev_priv)->info.gen_mask & GENMASK((__e), (__s))); \
 })
 
 /*
@@ -2621,73 +2631,73 @@ struct drm_i915_cmd_table {
 #define IS_REVID(p, since, until) \
 	(INTEL_REVID(p) >= (since) && INTEL_REVID(p) <= (until))
 
-#define IS_I830(dev)		(INTEL_DEVID(dev) == 0x3577)
-#define IS_845G(dev)		(INTEL_DEVID(dev) == 0x2562)
+#define IS_I830(dev_priv)	(INTEL_DEVID(dev_priv) == 0x3577)
+#define IS_845G(dev_priv)	(INTEL_DEVID(dev_priv) == 0x2562)
 #define IS_I85X(dev)		(INTEL_INFO(dev)->is_i85x)
-#define IS_I865G(dev)		(INTEL_DEVID(dev) == 0x2572)
+#define IS_I865G(dev_priv)	(INTEL_DEVID(dev_priv) == 0x2572)
 #define IS_I915G(dev)		(INTEL_INFO(dev)->is_i915g)
-#define IS_I915GM(dev)		(INTEL_DEVID(dev) == 0x2592)
-#define IS_I945G(dev)		(INTEL_DEVID(dev) == 0x2772)
+#define IS_I915GM(dev_priv)	(INTEL_DEVID(dev_priv) == 0x2592)
+#define IS_I945G(dev_priv)	(INTEL_DEVID(dev_priv) == 0x2772)
 #define IS_I945GM(dev)		(INTEL_INFO(dev)->is_i945gm)
 #define IS_BROADWATER(dev)	(INTEL_INFO(dev)->is_broadwater)
 #define IS_CRESTLINE(dev)	(INTEL_INFO(dev)->is_crestline)
-#define IS_GM45(dev)		(INTEL_DEVID(dev) == 0x2A42)
-#define IS_G4X(dev)		(INTEL_INFO(dev)->is_g4x)
-#define IS_PINEVIEW_G(dev)	(INTEL_DEVID(dev) == 0xa001)
-#define IS_PINEVIEW_M(dev)	(INTEL_DEVID(dev) == 0xa011)
+#define IS_GM45(dev_priv)	(INTEL_DEVID(dev_priv) == 0x2A42)
+#define IS_G4X(dev_priv)	((dev_priv)->info.is_g4x)
+#define IS_PINEVIEW_G(dev_priv)	(INTEL_DEVID(dev_priv) == 0xa001)
+#define IS_PINEVIEW_M(dev_priv)	(INTEL_DEVID(dev_priv) == 0xa011)
 #define IS_PINEVIEW(dev)	(INTEL_INFO(dev)->is_pineview)
 #define IS_G33(dev)		(INTEL_INFO(dev)->is_g33)
-#define IS_IRONLAKE_M(dev)	(INTEL_DEVID(dev) == 0x0046)
-#define IS_IVYBRIDGE(dev)	(INTEL_INFO(dev)->is_ivybridge)
-#define IS_IVB_GT1(dev)		(INTEL_DEVID(dev) == 0x0156 || \
-				 INTEL_DEVID(dev) == 0x0152 || \
-				 INTEL_DEVID(dev) == 0x015a)
-#define IS_VALLEYVIEW(dev)	(INTEL_INFO(dev)->is_valleyview)
-#define IS_CHERRYVIEW(dev)	(INTEL_INFO(dev)->is_cherryview)
-#define IS_HASWELL(dev)	(INTEL_INFO(dev)->is_haswell)
-#define IS_BROADWELL(dev)	(INTEL_INFO(dev)->is_broadwell)
-#define IS_SKYLAKE(dev)	(INTEL_INFO(dev)->is_skylake)
-#define IS_BROXTON(dev)		(INTEL_INFO(dev)->is_broxton)
-#define IS_KABYLAKE(dev)	(INTEL_INFO(dev)->is_kabylake)
+#define IS_IRONLAKE_M(dev_priv)	(INTEL_DEVID(dev_priv) == 0x0046)
+#define IS_IVYBRIDGE(dev_priv)	((dev_priv)->info.is_ivybridge)
+#define IS_IVB_GT1(dev_priv)	(INTEL_DEVID(dev_priv) == 0x0156 || \
+				 INTEL_DEVID(dev_priv) == 0x0152 || \
+				 INTEL_DEVID(dev_priv) == 0x015a)
+#define IS_VALLEYVIEW(dev_priv)	((dev_priv)->info.is_valleyview)
+#define IS_CHERRYVIEW(dev_priv)	((dev_priv)->info.is_cherryview)
+#define IS_HASWELL(dev_priv)	((dev_priv)->info.is_haswell)
+#define IS_BROADWELL(dev_priv)	((dev_priv)->info.is_broadwell)
+#define IS_SKYLAKE(dev_priv)	((dev_priv)->info.is_skylake)
+#define IS_BROXTON(dev_priv)	((dev_priv)->info.is_broxton)
+#define IS_KABYLAKE(dev_priv)	((dev_priv)->info.is_kabylake)
 #define IS_MOBILE(dev)		(INTEL_INFO(dev)->is_mobile)
-#define IS_HSW_EARLY_SDV(dev)	(IS_HASWELL(dev) && \
-				 (INTEL_DEVID(dev) & 0xFF00) == 0x0C00)
-#define IS_BDW_ULT(dev)		(IS_BROADWELL(dev) && \
-				 ((INTEL_DEVID(dev) & 0xf) == 0x6 ||	\
-				 (INTEL_DEVID(dev) & 0xf) == 0xb ||	\
-				 (INTEL_DEVID(dev) & 0xf) == 0xe))
+#define IS_HSW_EARLY_SDV(dev_priv) (IS_HASWELL(dev_priv) && \
+				    (INTEL_DEVID(dev_priv) & 0xFF00) == 0x0C00)
+#define IS_BDW_ULT(dev_priv)	(IS_BROADWELL(dev_priv) && \
+				 ((INTEL_DEVID(dev_priv) & 0xf) == 0x6 ||	\
+				 (INTEL_DEVID(dev_priv) & 0xf) == 0xb ||	\
+				 (INTEL_DEVID(dev_priv) & 0xf) == 0xe))
 /* ULX machines are also considered ULT. */
-#define IS_BDW_ULX(dev)		(IS_BROADWELL(dev) && \
-				 (INTEL_DEVID(dev) & 0xf) == 0xe)
-#define IS_BDW_GT3(dev)		(IS_BROADWELL(dev) && \
-				 (INTEL_DEVID(dev) & 0x00F0) == 0x0020)
-#define IS_HSW_ULT(dev)		(IS_HASWELL(dev) && \
-				 (INTEL_DEVID(dev) & 0xFF00) == 0x0A00)
-#define IS_HSW_GT3(dev)		(IS_HASWELL(dev) && \
-				 (INTEL_DEVID(dev) & 0x00F0) == 0x0020)
+#define IS_BDW_ULX(dev_priv)	(IS_BROADWELL(dev_priv) && \
+				 (INTEL_DEVID(dev_priv) & 0xf) == 0xe)
+#define IS_BDW_GT3(dev_priv)	(IS_BROADWELL(dev_priv) && \
+				 (INTEL_DEVID(dev_priv) & 0x00F0) == 0x0020)
+#define IS_HSW_ULT(dev_priv)	(IS_HASWELL(dev_priv) && \
+				 (INTEL_DEVID(dev_priv) & 0xFF00) == 0x0A00)
+#define IS_HSW_GT3(dev_priv)	(IS_HASWELL(dev_priv) && \
+				 (INTEL_DEVID(dev_priv) & 0x00F0) == 0x0020)
 /* ULX machines are also considered ULT. */
-#define IS_HSW_ULX(dev)		(INTEL_DEVID(dev) == 0x0A0E || \
-				 INTEL_DEVID(dev) == 0x0A1E)
-#define IS_SKL_ULT(dev)		(INTEL_DEVID(dev) == 0x1906 || \
-				 INTEL_DEVID(dev) == 0x1913 || \
-				 INTEL_DEVID(dev) == 0x1916 || \
-				 INTEL_DEVID(dev) == 0x1921 || \
-				 INTEL_DEVID(dev) == 0x1926)
-#define IS_SKL_ULX(dev)		(INTEL_DEVID(dev) == 0x190E || \
-				 INTEL_DEVID(dev) == 0x1915 || \
-				 INTEL_DEVID(dev) == 0x191E)
-#define IS_KBL_ULT(dev)		(INTEL_DEVID(dev) == 0x5906 || \
-				 INTEL_DEVID(dev) == 0x5913 || \
-				 INTEL_DEVID(dev) == 0x5916 || \
-				 INTEL_DEVID(dev) == 0x5921 || \
-				 INTEL_DEVID(dev) == 0x5926)
-#define IS_KBL_ULX(dev)		(INTEL_DEVID(dev) == 0x590E || \
-				 INTEL_DEVID(dev) == 0x5915 || \
-				 INTEL_DEVID(dev) == 0x591E)
-#define IS_SKL_GT3(dev)		(IS_SKYLAKE(dev) && \
-				 (INTEL_DEVID(dev) & 0x00F0) == 0x0020)
-#define IS_SKL_GT4(dev)		(IS_SKYLAKE(dev) && \
-				 (INTEL_DEVID(dev) & 0x00F0) == 0x0030)
+#define IS_HSW_ULX(dev_priv)	(INTEL_DEVID(dev_priv) == 0x0A0E || \
+				 INTEL_DEVID(dev_priv) == 0x0A1E)
+#define IS_SKL_ULT(dev_priv)	(INTEL_DEVID(dev_priv) == 0x1906 || \
+				 INTEL_DEVID(dev_priv) == 0x1913 || \
+				 INTEL_DEVID(dev_priv) == 0x1916 || \
+				 INTEL_DEVID(dev_priv) == 0x1921 || \
+				 INTEL_DEVID(dev_priv) == 0x1926)
+#define IS_SKL_ULX(dev_priv)	(INTEL_DEVID(dev_priv) == 0x190E || \
+				 INTEL_DEVID(dev_priv) == 0x1915 || \
+				 INTEL_DEVID(dev_priv) == 0x191E)
+#define IS_KBL_ULT(dev_priv)	(INTEL_DEVID(dev_priv) == 0x5906 || \
+				 INTEL_DEVID(dev_priv) == 0x5913 || \
+				 INTEL_DEVID(dev_priv) == 0x5916 || \
+				 INTEL_DEVID(dev_priv) == 0x5921 || \
+				 INTEL_DEVID(dev_priv) == 0x5926)
+#define IS_KBL_ULX(dev_priv)	(INTEL_DEVID(dev_priv) == 0x590E || \
+				 INTEL_DEVID(dev_priv) == 0x5915 || \
+				 INTEL_DEVID(dev_priv) == 0x591E)
+#define IS_SKL_GT3(dev_priv)	(IS_SKYLAKE(dev_priv) && \
+				 (INTEL_DEVID(dev_priv) & 0x00F0) == 0x0020)
+#define IS_SKL_GT4(dev_priv)	(IS_SKYLAKE(dev_priv) && \
+				 (INTEL_DEVID(dev_priv) & 0x00F0) == 0x0030)
 
 #define IS_PRELIMINARY_HW(intel_info) ((intel_info)->is_preliminary)
 
@@ -2707,7 +2717,8 @@ struct drm_i915_cmd_table {
 #define BXT_REVID_B0		0x3
 #define BXT_REVID_C0		0x9
 
-#define IS_BXT_REVID(p, since, until) (IS_BROXTON(p) && IS_REVID(p, since, until))
+#define IS_BXT_REVID(dev_priv, since, until) \
+	(IS_BROXTON(dev_priv) && IS_REVID(dev_priv, since, until))
 
 #define KBL_REVID_A0		0x0
 #define KBL_REVID_B0		0x1
@@ -2715,8 +2726,8 @@ struct drm_i915_cmd_table {
 #define KBL_REVID_D0		0x3
 #define KBL_REVID_E0		0x4
 
-#define IS_KBL_REVID(p, since, until) \
-	(IS_KABYLAKE(p) && IS_REVID(p, since, until))
+#define IS_KBL_REVID(dev_priv, since, until) \
+	(IS_KABYLAKE(dev_priv) && IS_REVID(dev_priv, since, until))
 
 /*
  * The genX designation typically refers to the render engine, so render
@@ -2724,14 +2735,14 @@ struct drm_i915_cmd_table {
  * have their own (e.g. HAS_PCH_SPLIT for ILK+ display, IS_foo for particular
  * chips, etc.).
  */
-#define IS_GEN2(dev)	(!!(INTEL_INFO(dev)->gen_mask & BIT(1)))
-#define IS_GEN3(dev)	(!!(INTEL_INFO(dev)->gen_mask & BIT(2)))
-#define IS_GEN4(dev)	(!!(INTEL_INFO(dev)->gen_mask & BIT(3)))
-#define IS_GEN5(dev)	(!!(INTEL_INFO(dev)->gen_mask & BIT(4)))
-#define IS_GEN6(dev)	(!!(INTEL_INFO(dev)->gen_mask & BIT(5)))
-#define IS_GEN7(dev)	(!!(INTEL_INFO(dev)->gen_mask & BIT(6)))
-#define IS_GEN8(dev)	(!!(INTEL_INFO(dev)->gen_mask & BIT(7)))
-#define IS_GEN9(dev)	(!!(INTEL_INFO(dev)->gen_mask & BIT(8)))
+#define IS_GEN2(dev_priv)	(!!((dev_priv)->info.gen_mask & BIT(1)))
+#define IS_GEN3(dev_priv)	(!!((dev_priv)->info.gen_mask & BIT(2)))
+#define IS_GEN4(dev_priv)	(!!((dev_priv)->info.gen_mask & BIT(3)))
+#define IS_GEN5(dev_priv)	(!!((dev_priv)->info.gen_mask & BIT(4)))
+#define IS_GEN6(dev_priv)	(!!((dev_priv)->info.gen_mask & BIT(5)))
+#define IS_GEN7(dev_priv)	(!!((dev_priv)->info.gen_mask & BIT(6)))
+#define IS_GEN8(dev_priv)	(!!((dev_priv)->info.gen_mask & BIT(7)))
+#define IS_GEN9(dev_priv)	(!!((dev_priv)->info.gen_mask & BIT(8)))
 
 #define ENGINE_MASK(id)	BIT(id)
 #define RENDER_RING	ENGINE_MASK(RCS)
@@ -2752,8 +2763,8 @@ struct drm_i915_cmd_table {
 #define HAS_LLC(dev)		(INTEL_INFO(dev)->has_llc)
 #define HAS_SNOOP(dev)		(INTEL_INFO(dev)->has_snoop)
 #define HAS_EDRAM(dev)		(!!(__I915__(dev)->edram_cap & EDRAM_ENABLED))
-#define HAS_WT(dev)		((IS_HASWELL(dev) || IS_BROADWELL(dev)) && \
-				 HAS_EDRAM(dev))
+#define HAS_WT(dev_priv)	((IS_HASWELL(dev_priv) || \
+				 IS_BROADWELL(dev_priv)) && HAS_EDRAM(dev_priv))
 #define HWS_NEEDS_PHYSICAL(dev)	(INTEL_INFO(dev)->hws_needs_physical)
 
 #define HAS_HW_CONTEXTS(dev)	(INTEL_INFO(dev)->has_hw_contexts)
@@ -2766,7 +2777,7 @@ struct drm_i915_cmd_table {
 #define OVERLAY_NEEDS_PHYSICAL(dev)	(INTEL_INFO(dev)->overlay_needs_physical)
 
 /* Early gen2 have a totally busted CS tlb and require pinned batches. */
-#define HAS_BROKEN_CS_TLB(dev)		(IS_I830(dev) || IS_845G(dev))
+#define HAS_BROKEN_CS_TLB(dev_priv)	(IS_I830(dev_priv) || IS_845G(dev_priv))
 
 /* WaRsDisableCoarsePowerGating:skl,bxt */
 #define NEEDS_WaRsDisableCoarsePowerGating(dev_priv) \
@@ -2786,8 +2797,9 @@ struct drm_i915_cmd_table {
 /* With the 945 and later, Y tiling got adjusted so that it was 32 128-byte
  * rows, which changed the alignment requirements and fence programming.
  */
-#define HAS_128_BYTE_Y_TILING(dev) (!IS_GEN2(dev) && !(IS_I915G(dev) || \
-						      IS_I915GM(dev)))
+#define HAS_128_BYTE_Y_TILING(dev_priv) (!IS_GEN2(dev_priv) && \
+					 !(IS_I915G(dev_priv) || \
+					 IS_I915GM(dev_priv)))
 #define SUPPORTS_TV(dev)		(INTEL_INFO(dev)->supports_tv)
 #define I915_HAS_HOTPLUG(dev)		 (INTEL_INFO(dev)->has_hotplug)
 
@@ -2795,19 +2807,19 @@ struct drm_i915_cmd_table {
 #define HAS_PIPE_CXSR(dev) (INTEL_INFO(dev)->has_pipe_cxsr)
 #define HAS_FBC(dev) (INTEL_INFO(dev)->has_fbc)
 
-#define HAS_IPS(dev)		(IS_HSW_ULT(dev) || IS_BROADWELL(dev))
+#define HAS_IPS(dev_priv)	(IS_HSW_ULT(dev_priv) || IS_BROADWELL(dev_priv))
 
 #define HAS_DP_MST(dev)	(INTEL_INFO(dev)->has_dp_mst)
 
-#define HAS_DDI(dev)		(INTEL_INFO(dev)->has_ddi)
+#define HAS_DDI(dev_priv)	((dev_priv)->info.has_ddi)
 #define HAS_FPGA_DBG_UNCLAIMED(dev)	(INTEL_INFO(dev)->has_fpga_dbg)
 #define HAS_PSR(dev)		(INTEL_INFO(dev)->has_psr)
-#define HAS_RUNTIME_PM(dev)	(INTEL_INFO(dev)->has_runtime_pm)
 #define HAS_RC6(dev)		(INTEL_INFO(dev)->has_rc6)
 #define HAS_RC6p(dev)		(INTEL_INFO(dev)->has_rc6p)
 
 #define HAS_CSR(dev)	(INTEL_INFO(dev)->has_csr)
 
+#define HAS_RUNTIME_PM(dev_priv) ((dev_priv)->info.has_runtime_pm)
 /*
  * For now, anything with a GuC requires uCode loading, and then supports
  * command submission once loaded. But these are logically independent
@@ -2834,22 +2846,27 @@ struct drm_i915_cmd_table {
 #define INTEL_PCH_P3X_DEVICE_ID_TYPE		0x7000
 #define INTEL_PCH_QEMU_DEVICE_ID_TYPE		0x2900 /* qemu q35 has 2918 */
 
-#define INTEL_PCH_TYPE(dev) (__I915__(dev)->pch_type)
-#define HAS_PCH_KBP(dev) (INTEL_PCH_TYPE(dev) == PCH_KBP)
-#define HAS_PCH_SPT(dev) (INTEL_PCH_TYPE(dev) == PCH_SPT)
-#define HAS_PCH_LPT(dev) (INTEL_PCH_TYPE(dev) == PCH_LPT)
-#define HAS_PCH_LPT_LP(dev) (__I915__(dev)->pch_id == INTEL_PCH_LPT_LP_DEVICE_ID_TYPE)
-#define HAS_PCH_LPT_H(dev) (__I915__(dev)->pch_id == INTEL_PCH_LPT_DEVICE_ID_TYPE)
-#define HAS_PCH_CPT(dev) (INTEL_PCH_TYPE(dev) == PCH_CPT)
-#define HAS_PCH_IBX(dev) (INTEL_PCH_TYPE(dev) == PCH_IBX)
-#define HAS_PCH_NOP(dev) (INTEL_PCH_TYPE(dev) == PCH_NOP)
-#define HAS_PCH_SPLIT(dev) (INTEL_PCH_TYPE(dev) != PCH_NONE)
+#define INTEL_PCH_TYPE(dev_priv) ((dev_priv)->pch_type)
+#define HAS_PCH_KBP(dev_priv) (INTEL_PCH_TYPE(dev_priv) == PCH_KBP)
+#define HAS_PCH_SPT(dev_priv) (INTEL_PCH_TYPE(dev_priv) == PCH_SPT)
+#define HAS_PCH_LPT(dev_priv) (INTEL_PCH_TYPE(dev_priv) == PCH_LPT)
+#define HAS_PCH_LPT_LP(dev_priv) \
+	((dev_priv)->pch_id == INTEL_PCH_LPT_LP_DEVICE_ID_TYPE)
+#define HAS_PCH_LPT_H(dev_priv) \
+	((dev_priv)->pch_id == INTEL_PCH_LPT_DEVICE_ID_TYPE)
+#define HAS_PCH_CPT(dev_priv) (INTEL_PCH_TYPE(dev_priv) == PCH_CPT)
+#define HAS_PCH_IBX(dev_priv) (INTEL_PCH_TYPE(dev_priv) == PCH_IBX)
+#define HAS_PCH_NOP(dev_priv) (INTEL_PCH_TYPE(dev_priv) == PCH_NOP)
+#define HAS_PCH_SPLIT(dev_priv) (INTEL_PCH_TYPE(dev_priv) != PCH_NONE)
+
+#define HAS_GMCH_DISPLAY(dev_priv) ((dev_priv)->info.has_gmch_display)
 
-#define HAS_GMCH_DISPLAY(dev) (INTEL_INFO(dev)->has_gmch_display)
+#define HAS_LSPCON(dev_priv) (IS_GEN9(dev_priv))
 
 /* DPF == dynamic parity feature */
-#define HAS_L3_DPF(dev) (INTEL_INFO(dev)->has_l3_dpf)
-#define NUM_L3_SLICES(dev) (IS_HSW_GT3(dev) ? 2 : HAS_L3_DPF(dev))
+#define HAS_L3_DPF(dev_priv) ((dev_priv)->info.has_l3_dpf)
+#define NUM_L3_SLICES(dev_priv) (IS_HSW_GT3(dev_priv) ? \
+				 2 : HAS_L3_DPF(dev_priv))
 
 #define GT_FREQUENCY_MULTIPLIER 50
 #define GEN9_FREQ_SCALER 3
@@ -2885,6 +2902,11 @@ __i915_printk(struct drm_i915_private *dev_priv, const char *level,
 extern long i915_compat_ioctl(struct file *filp, unsigned int cmd,
 			      unsigned long arg);
 #endif
+extern const struct dev_pm_ops i915_pm_ops;
+
+extern int i915_driver_load(struct pci_dev *pdev,
+			    const struct pci_device_id *ent);
+extern void i915_driver_unload(struct drm_device *dev);
 extern int intel_gpu_reset(struct drm_i915_private *dev_priv, u32 engine_mask);
 extern bool intel_has_gpu_reset(struct drm_i915_private *dev_priv);
 extern void i915_reset(struct drm_i915_private *dev_priv);
@@ -2971,7 +2993,7 @@ int intel_wait_for_register_fw(struct drm_i915_private *dev_priv,
 
 static inline bool intel_gvt_active(struct drm_i915_private *dev_priv)
 {
-	return dev_priv->gvt.initialized;
+	return dev_priv->gvt;
 }
 
 static inline bool intel_vgpu_active(struct drm_i915_private *dev_priv)
@@ -3076,6 +3098,7 @@ int i915_gem_wait_ioctl(struct drm_device *dev, void *data,
 void i915_gem_load_init(struct drm_device *dev);
 void i915_gem_load_cleanup(struct drm_device *dev);
 void i915_gem_load_init_fences(struct drm_i915_private *dev_priv);
+int i915_gem_freeze(struct drm_i915_private *dev_priv);
 int i915_gem_freeze_late(struct drm_i915_private *dev_priv);
 
 void *i915_gem_object_alloc(struct drm_device *dev);
@@ -3083,7 +3106,7 @@ void i915_gem_object_free(struct drm_i915_gem_object *obj);
 void i915_gem_object_init(struct drm_i915_gem_object *obj,
 			 const struct drm_i915_gem_object_ops *ops);
 struct drm_i915_gem_object *i915_gem_object_create(struct drm_device *dev,
-						  size_t size);
+						   u64 size);
 struct drm_i915_gem_object *i915_gem_object_create_from_data(
 		struct drm_device *dev, const void *data, size_t size);
 void i915_gem_close_object(struct drm_gem_object *gem, struct drm_file *file);
@@ -3157,14 +3180,15 @@ i915_gem_object_get_page(struct drm_i915_gem_object *obj, int n)
 
 static inline void i915_gem_object_pin_pages(struct drm_i915_gem_object *obj)
 {
-	BUG_ON(obj->pages == NULL);
+	GEM_BUG_ON(obj->pages == NULL);
 	obj->pages_pin_count++;
 }
 
 static inline void i915_gem_object_unpin_pages(struct drm_i915_gem_object *obj)
 {
-	BUG_ON(obj->pages_pin_count == 0);
+	GEM_BUG_ON(obj->pages_pin_count == 0);
 	obj->pages_pin_count--;
+	GEM_BUG_ON(obj->pages_pin_count < obj->bind_count);
 }
 
 enum i915_map_type {
@@ -3522,6 +3546,8 @@ static inline void intel_display_crc_init(struct drm_i915_private *dev_priv) {}
 #endif
 
 /* i915_gpu_error.c */
+#if IS_ENABLED(CONFIG_DRM_I915_CAPTURE_ERROR)
+
 __printf(2, 3)
 void i915_error_printf(struct drm_i915_error_state_buf *e, const char *f, ...);
 int i915_error_state_to_str(struct drm_i915_error_state_buf *estr,
@@ -3542,7 +3568,20 @@ void i915_error_state_get(struct drm_device *dev,
 void i915_error_state_put(struct i915_error_state_file_priv *error_priv);
 void i915_destroy_error_state(struct drm_device *dev);
 
-void i915_get_extra_instdone(struct drm_i915_private *dev_priv, uint32_t *instdone);
+#else
+
+static inline void i915_capture_error_state(struct drm_i915_private *dev_priv,
+					    u32 engine_mask,
+					    const char *error_msg)
+{
+}
+
+static inline void i915_destroy_error_state(struct drm_device *dev)
+{
+}
+
+#endif
+
 const char *i915_cache_level_str(struct drm_i915_private *i915, int type);
 
 /* i915_cmd_parser.c */
@@ -3592,6 +3631,9 @@ bool intel_bios_is_port_dp_dual_mode(struct drm_i915_private *dev_priv, enum por
 bool intel_bios_is_dsi_present(struct drm_i915_private *dev_priv, enum port *port);
 bool intel_bios_is_port_hpd_inverted(struct drm_i915_private *dev_priv,
 				     enum port port);
+bool intel_bios_is_lspcon_present(struct drm_i915_private *dev_priv,
+				enum port port);
+
 
 /* intel_opregion.c */
 #ifdef CONFIG_ACPI
@@ -3808,11 +3850,11 @@ __raw_write(64, q)
 #define INTEL_BROADCAST_RGB_FULL 1
 #define INTEL_BROADCAST_RGB_LIMITED 2
 
-static inline i915_reg_t i915_vgacntrl_reg(struct drm_device *dev)
+static inline i915_reg_t i915_vgacntrl_reg(struct drm_i915_private *dev_priv)
 {
-	if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev))
+	if (IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv))
 		return VLV_VGACNTRL;
-	else if (INTEL_INFO(dev)->gen >= 5)
+	else if (INTEL_GEN(dev_priv) >= 5)
 		return CPU_VGACNTRL;
 	else
 		return VGACNTRL;
diff --git a/drivers/gpu/drm/i915/i915_gem.c b/drivers/gpu/drm/i915/i915_gem.c
index 2c8106758922..8ed8e24025ac 100644
--- a/drivers/gpu/drm/i915/i915_gem.c
+++ b/drivers/gpu/drm/i915/i915_gem.c
@@ -82,7 +82,7 @@ remove_mappable_node(struct drm_mm_node *node)
 
 /* some bookkeeping */
 static void i915_gem_info_add_obj(struct drm_i915_private *dev_priv,
-				  size_t size)
+				  u64 size)
 {
 	spin_lock(&dev_priv->mm.object_stat_lock);
 	dev_priv->mm.object_count++;
@@ -91,7 +91,7 @@ static void i915_gem_info_add_obj(struct drm_i915_private *dev_priv,
 }
 
 static void i915_gem_info_remove_obj(struct drm_i915_private *dev_priv,
-				     size_t size)
+				     u64 size)
 {
 	spin_lock(&dev_priv->mm.object_stat_lock);
 	dev_priv->mm.object_count--;
@@ -860,7 +860,7 @@ i915_gem_gtt_pread(struct drm_device *dev,
 
 	mutex_unlock(&dev->struct_mutex);
 	if (likely(!i915.prefault_disable)) {
-		ret = fault_in_multipages_writeable(user_data, remain);
+		ret = fault_in_pages_writeable(user_data, remain);
 		if (ret) {
 			mutex_lock(&dev->struct_mutex);
 			goto out_unpin;
@@ -919,8 +919,7 @@ out_unpin:
 	if (node.allocated) {
 		wmb();
 		ggtt->base.clear_range(&ggtt->base,
-				       node.start, node.size,
-				       true);
+				       node.start, node.size);
 		i915_gem_object_unpin_pages(obj);
 		remove_mappable_node(&node);
 	} else {
@@ -983,7 +982,7 @@ i915_gem_shmem_pread(struct drm_device *dev,
 		mutex_unlock(&dev->struct_mutex);
 
 		if (likely(!i915.prefault_disable) && !prefaulted) {
-			ret = fault_in_multipages_writeable(user_data, remain);
+			ret = fault_in_pages_writeable(user_data, remain);
 			/* Userspace is tricking us, but we've already clobbered
 			 * its pages with the prefault and promised to write the
 			 * data up to the first fault. Hence ignore any errors
@@ -1228,8 +1227,7 @@ out_unpin:
 	if (node.allocated) {
 		wmb();
 		ggtt->base.clear_range(&ggtt->base,
-				       node.start, node.size,
-				       true);
+				       node.start, node.size);
 		i915_gem_object_unpin_pages(obj);
 		remove_mappable_node(&node);
 	} else {
@@ -1431,7 +1429,7 @@ i915_gem_pwrite_ioctl(struct drm_device *dev, void *data,
 		return -EFAULT;
 
 	if (likely(!i915.prefault_disable)) {
-		ret = fault_in_multipages_readable(u64_to_user_ptr(args->data_ptr),
+		ret = fault_in_pages_readable(u64_to_user_ptr(args->data_ptr),
 						   args->size);
 		if (ret)
 			return -EFAULT;
@@ -1813,8 +1811,7 @@ int i915_gem_fault(struct vm_area_struct *area, struct vm_fault *vmf)
 		view.params.partial.offset = rounddown(page_offset, chunk_size);
 		view.params.partial.size =
 			min_t(unsigned int, chunk_size,
-			      (area->vm_end - area->vm_start) / PAGE_SIZE -
-			      view.params.partial.offset);
+			      vma_pages(area) - view.params.partial.offset);
 
 		/* If the partial covers the entire object, just create a
 		 * normal VMA.
@@ -2208,6 +2205,15 @@ i915_gem_object_put_pages(struct drm_i915_gem_object *obj)
 	return 0;
 }
 
+static unsigned int swiotlb_max_size(void)
+{
+#if IS_ENABLED(CONFIG_SWIOTLB)
+	return rounddown(swiotlb_nr_tbl() << IO_TLB_SHIFT, PAGE_SIZE);
+#else
+	return 0;
+#endif
+}
+
 static int
 i915_gem_object_get_pages_gtt(struct drm_i915_gem_object *obj)
 {
@@ -2219,6 +2225,7 @@ i915_gem_object_get_pages_gtt(struct drm_i915_gem_object *obj)
 	struct sgt_iter sgt_iter;
 	struct page *page;
 	unsigned long last_pfn = 0;	/* suppress gcc warning */
+	unsigned int max_segment;
 	int ret;
 	gfp_t gfp;
 
@@ -2229,6 +2236,10 @@ i915_gem_object_get_pages_gtt(struct drm_i915_gem_object *obj)
 	BUG_ON(obj->base.read_domains & I915_GEM_GPU_DOMAINS);
 	BUG_ON(obj->base.write_domain & I915_GEM_GPU_DOMAINS);
 
+	max_segment = swiotlb_max_size();
+	if (!max_segment)
+		max_segment = rounddown(UINT_MAX, PAGE_SIZE);
+
 	st = kmalloc(sizeof(*st), GFP_KERNEL);
 	if (st == NULL)
 		return -ENOMEM;
@@ -2264,22 +2275,15 @@ i915_gem_object_get_pages_gtt(struct drm_i915_gem_object *obj)
 			 * our own buffer, now let the real VM do its job and
 			 * go down in flames if truly OOM.
 			 */
-			i915_gem_shrink_all(dev_priv);
 			page = shmem_read_mapping_page(mapping, i);
 			if (IS_ERR(page)) {
 				ret = PTR_ERR(page);
 				goto err_pages;
 			}
 		}
-#ifdef CONFIG_SWIOTLB
-		if (swiotlb_nr_tbl()) {
-			st->nents++;
-			sg_set_page(sg, page, PAGE_SIZE, 0);
-			sg = sg_next(sg);
-			continue;
-		}
-#endif
-		if (!i || page_to_pfn(page) != last_pfn + 1) {
+		if (!i ||
+		    sg->length >= max_segment ||
+		    page_to_pfn(page) != last_pfn + 1) {
 			if (i)
 				sg = sg_next(sg);
 			st->nents++;
@@ -2292,9 +2296,7 @@ i915_gem_object_get_pages_gtt(struct drm_i915_gem_object *obj)
 		/* Check that the i965g/gm workaround works. */
 		WARN_ON((gfp & __GFP_DMA32) && (last_pfn >= 0x00100000UL));
 	}
-#ifdef CONFIG_SWIOTLB
-	if (!swiotlb_nr_tbl())
-#endif
+	if (sg) /* loop terminated early; short sg table */
 		sg_mark_end(sg);
 	obj->pages = st;
 
@@ -2581,8 +2583,6 @@ static void i915_gem_reset_engine(struct intel_engine_cs *engine)
 	struct i915_gem_context *incomplete_ctx;
 	bool ring_hung;
 
-	/* Ensure irq handler finishes, and not run again. */
-	tasklet_kill(&engine->irq_tasklet);
 	if (engine->irq_seqno_barrier)
 		engine->irq_seqno_barrier(engine);
 
@@ -2591,6 +2591,9 @@ static void i915_gem_reset_engine(struct intel_engine_cs *engine)
 		return;
 
 	ring_hung = engine->hangcheck.score >= HANGCHECK_SCORE_RING_HUNG;
+	if (engine->hangcheck.seqno != intel_engine_get_seqno(engine))
+		ring_hung = false;
+
 	i915_set_reset_status(request->ctx, ring_hung);
 	if (!ring_hung)
 		return;
@@ -2616,21 +2619,26 @@ static void i915_gem_reset_engine(struct intel_engine_cs *engine)
 	list_for_each_entry_continue(request, &engine->request_list, link)
 		if (request->ctx == incomplete_ctx)
 			reset_request(request);
-
-	engine->i915->gt.active_engines &= ~intel_engine_flag(engine);
 }
 
 void i915_gem_reset(struct drm_i915_private *dev_priv)
 {
 	struct intel_engine_cs *engine;
+	enum intel_engine_id id;
 
 	i915_gem_retire_requests(dev_priv);
 
-	for_each_engine(engine, dev_priv)
+	for_each_engine(engine, dev_priv, id)
 		i915_gem_reset_engine(engine);
-	mod_delayed_work(dev_priv->wq, &dev_priv->gt.idle_work, 0);
 
 	i915_gem_restore_fences(&dev_priv->drm);
+
+	if (dev_priv->gt.awake) {
+		intel_sanitize_gt_powersave(dev_priv);
+		intel_enable_gt_powersave(dev_priv);
+		if (INTEL_GEN(dev_priv) >= 6)
+			gen6_rps_busy(dev_priv);
+	}
 }
 
 static void nop_submit_request(struct drm_i915_gem_request *request)
@@ -2668,12 +2676,13 @@ static void i915_gem_cleanup_engine(struct intel_engine_cs *engine)
 void i915_gem_set_wedged(struct drm_i915_private *dev_priv)
 {
 	struct intel_engine_cs *engine;
+	enum intel_engine_id id;
 
 	lockdep_assert_held(&dev_priv->drm.struct_mutex);
 	set_bit(I915_WEDGED, &dev_priv->gpu_error.flags);
 
 	i915_gem_context_lost(dev_priv);
-	for_each_engine(engine, dev_priv)
+	for_each_engine(engine, dev_priv, id)
 		i915_gem_cleanup_engine(engine);
 	mod_delayed_work(dev_priv->wq, &dev_priv->gt.idle_work, 0);
 
@@ -2712,6 +2721,7 @@ i915_gem_idle_work_handler(struct work_struct *work)
 		container_of(work, typeof(*dev_priv), gt.idle_work.work);
 	struct drm_device *dev = &dev_priv->drm;
 	struct intel_engine_cs *engine;
+	enum intel_engine_id id;
 	bool rearm_hangcheck;
 
 	if (!READ_ONCE(dev_priv->gt.awake))
@@ -2734,7 +2744,7 @@ i915_gem_idle_work_handler(struct work_struct *work)
 	if (dev_priv->gt.active_engines)
 		goto out_unlock;
 
-	for_each_engine(engine, dev_priv)
+	for_each_engine(engine, dev_priv, id)
 		i915_gem_batch_pool_fini(&engine->batch_pool);
 
 	GEM_BUG_ON(!dev_priv->gt.awake);
@@ -2927,9 +2937,10 @@ int i915_gem_wait_for_idle(struct drm_i915_private *dev_priv,
 			   unsigned int flags)
 {
 	struct intel_engine_cs *engine;
+	enum intel_engine_id id;
 	int ret;
 
-	for_each_engine(engine, dev_priv) {
+	for_each_engine(engine, dev_priv, id) {
 		if (engine->last_context == NULL)
 			continue;
 
@@ -3091,6 +3102,9 @@ search_free:
 
 			goto err_unpin;
 		}
+
+		GEM_BUG_ON(vma->node.start < start);
+		GEM_BUG_ON(vma->node.start + vma->node.size > end);
 	}
 	GEM_BUG_ON(!i915_gem_valid_gtt_space(vma, obj->cache_level));
 
@@ -3169,7 +3183,7 @@ i915_gem_object_flush_gtt_write_domain(struct drm_i915_gem_object *obj)
 	 */
 	wmb();
 	if (INTEL_GEN(dev_priv) >= 6 && !HAS_LLC(dev_priv))
-		POSTING_READ(RING_ACTHD(dev_priv->engine[RCS].mmio_base));
+		POSTING_READ(RING_ACTHD(dev_priv->engine[RCS]->mmio_base));
 
 	intel_fb_obj_flush(obj, false, write_origin(obj, I915_GEM_DOMAIN_GTT));
 
@@ -3464,7 +3478,7 @@ int i915_gem_set_caching_ioctl(struct drm_device *dev, void *data,
 		level = I915_CACHE_LLC;
 		break;
 	case I915_CACHING_DISPLAY:
-		level = HAS_WT(dev) ? I915_CACHE_WT : I915_CACHE_NONE;
+		level = HAS_WT(dev_priv) ? I915_CACHE_WT : I915_CACHE_NONE;
 		break;
 	default:
 		return -EINVAL;
@@ -3522,7 +3536,8 @@ i915_gem_object_pin_to_display_plane(struct drm_i915_gem_object *obj,
 	 * with that bit in the PTE to main memory with just one PIPE_CONTROL.
 	 */
 	ret = i915_gem_object_set_cache_level(obj,
-					      HAS_WT(obj->base.dev) ? I915_CACHE_WT : I915_CACHE_NONE);
+					      HAS_WT(to_i915(obj->base.dev)) ?
+					      I915_CACHE_WT : I915_CACHE_NONE);
 	if (ret) {
 		vma = ERR_PTR(ret);
 		goto err_unpin_display;
@@ -3789,7 +3804,8 @@ i915_gem_object_ggtt_pin(struct drm_i915_gem_object *obj,
 			 u64 alignment,
 			 u64 flags)
 {
-	struct i915_address_space *vm = &to_i915(obj->base.dev)->ggtt.base;
+	struct drm_i915_private *dev_priv = to_i915(obj->base.dev);
+	struct i915_address_space *vm = &dev_priv->ggtt.base;
 	struct i915_vma *vma;
 	int ret;
 
@@ -3802,6 +3818,41 @@ i915_gem_object_ggtt_pin(struct drm_i915_gem_object *obj,
 		    (i915_vma_is_pinned(vma) || i915_vma_is_active(vma)))
 			return ERR_PTR(-ENOSPC);
 
+		if (flags & PIN_MAPPABLE) {
+			u32 fence_size;
+
+			fence_size = i915_gem_get_ggtt_size(dev_priv, vma->size,
+							    i915_gem_object_get_tiling(obj));
+			/* If the required space is larger than the available
+			 * aperture, we will not able to find a slot for the
+			 * object and unbinding the object now will be in
+			 * vain. Worse, doing so may cause us to ping-pong
+			 * the object in and out of the Global GTT and
+			 * waste a lot of cycles under the mutex.
+			 */
+			if (fence_size > dev_priv->ggtt.mappable_end)
+				return ERR_PTR(-E2BIG);
+
+			/* If NONBLOCK is set the caller is optimistically
+			 * trying to cache the full object within the mappable
+			 * aperture, and *must* have a fallback in place for
+			 * situations where we cannot bind the object. We
+			 * can be a little more lax here and use the fallback
+			 * more often to avoid costly migrations of ourselves
+			 * and other objects within the aperture.
+			 *
+			 * Half-the-aperture is used as a simple heuristic.
+			 * More interesting would to do search for a free
+			 * block prior to making the commitment to unbind.
+			 * That caters for the self-harm case, and with a
+			 * little more heuristics (e.g. NOFAULT, NOEVICT)
+			 * we could try to minimise harm to others.
+			 */
+			if (flags & PIN_NONBLOCK &&
+			    fence_size > dev_priv->ggtt.mappable_end / 2)
+				return ERR_PTR(-ENOSPC);
+		}
+
 		WARN(i915_vma_is_pinned(vma),
 		     "bo is already pinned in ggtt with incorrect alignment:"
 		     " offset=%08x, req.alignment=%llx,"
@@ -4080,14 +4131,29 @@ static const struct drm_i915_gem_object_ops i915_gem_object_ops = {
 	.put_pages = i915_gem_object_put_pages_gtt,
 };
 
-struct drm_i915_gem_object *i915_gem_object_create(struct drm_device *dev,
-						  size_t size)
+/* Note we don't consider signbits :| */
+#define overflows_type(x, T) \
+	(sizeof(x) > sizeof(T) && (x) >> (sizeof(T) * BITS_PER_BYTE))
+
+struct drm_i915_gem_object *
+i915_gem_object_create(struct drm_device *dev, u64 size)
 {
 	struct drm_i915_gem_object *obj;
 	struct address_space *mapping;
 	gfp_t mask;
 	int ret;
 
+	/* There is a prevalence of the assumption that we fit the object's
+	 * page count inside a 32bit _signed_ variable. Let's document this and
+	 * catch if we ever need to fix it. In the meantime, if you do spot
+	 * such a local variable, please consider fixing!
+	 */
+	if (WARN_ON(size >> PAGE_SHIFT > INT_MAX))
+		return ERR_PTR(-E2BIG);
+
+	if (overflows_type(size, obj->base.size))
+		return ERR_PTR(-E2BIG);
+
 	obj = i915_gem_object_alloc(dev);
 	if (obj == NULL)
 		return ERR_PTR(-ENOMEM);
@@ -4264,6 +4330,30 @@ int i915_gem_suspend(struct drm_device *dev)
 	 */
 	WARN_ON(dev_priv->gt.awake);
 
+	/*
+	 * Neither the BIOS, ourselves or any other kernel
+	 * expects the system to be in execlists mode on startup,
+	 * so we need to reset the GPU back to legacy mode. And the only
+	 * known way to disable logical contexts is through a GPU reset.
+	 *
+	 * So in order to leave the system in a known default configuration,
+	 * always reset the GPU upon unload and suspend. Afterwards we then
+	 * clean up the GEM state tracking, flushing off the requests and
+	 * leaving the system in a known idle state.
+	 *
+	 * Note that is of the upmost importance that the GPU is idle and
+	 * all stray writes are flushed *before* we dismantle the backing
+	 * storage for the pinned objects.
+	 *
+	 * However, since we are uncertain that resetting the GPU on older
+	 * machines is a good idea, we don't - just in case it leaves the
+	 * machine in an unusable condition.
+	 */
+	if (HAS_HW_CONTEXTS(dev)) {
+		int reset = intel_gpu_reset(dev_priv, ALL_ENGINES);
+		WARN_ON(reset && reset != -ENODEV);
+	}
+
 	return 0;
 
 err:
@@ -4298,44 +4388,42 @@ void i915_gem_init_swizzling(struct drm_device *dev)
 	I915_WRITE(DISP_ARB_CTL, I915_READ(DISP_ARB_CTL) |
 				 DISP_TILE_SURFACE_SWIZZLING);
 
-	if (IS_GEN5(dev))
+	if (IS_GEN5(dev_priv))
 		return;
 
 	I915_WRITE(TILECTL, I915_READ(TILECTL) | TILECTL_SWZCTL);
-	if (IS_GEN6(dev))
+	if (IS_GEN6(dev_priv))
 		I915_WRITE(ARB_MODE, _MASKED_BIT_ENABLE(ARB_MODE_SWIZZLE_SNB));
-	else if (IS_GEN7(dev))
+	else if (IS_GEN7(dev_priv))
 		I915_WRITE(ARB_MODE, _MASKED_BIT_ENABLE(ARB_MODE_SWIZZLE_IVB));
-	else if (IS_GEN8(dev))
+	else if (IS_GEN8(dev_priv))
 		I915_WRITE(GAMTARBMODE, _MASKED_BIT_ENABLE(ARB_MODE_SWIZZLE_BDW));
 	else
 		BUG();
 }
 
-static void init_unused_ring(struct drm_device *dev, u32 base)
+static void init_unused_ring(struct drm_i915_private *dev_priv, u32 base)
 {
-	struct drm_i915_private *dev_priv = to_i915(dev);
-
 	I915_WRITE(RING_CTL(base), 0);
 	I915_WRITE(RING_HEAD(base), 0);
 	I915_WRITE(RING_TAIL(base), 0);
 	I915_WRITE(RING_START(base), 0);
 }
 
-static void init_unused_rings(struct drm_device *dev)
-{
-	if (IS_I830(dev)) {
-		init_unused_ring(dev, PRB1_BASE);
-		init_unused_ring(dev, SRB0_BASE);
-		init_unused_ring(dev, SRB1_BASE);
-		init_unused_ring(dev, SRB2_BASE);
-		init_unused_ring(dev, SRB3_BASE);
-	} else if (IS_GEN2(dev)) {
-		init_unused_ring(dev, SRB0_BASE);
-		init_unused_ring(dev, SRB1_BASE);
-	} else if (IS_GEN3(dev)) {
-		init_unused_ring(dev, PRB1_BASE);
-		init_unused_ring(dev, PRB2_BASE);
+static void init_unused_rings(struct drm_i915_private *dev_priv)
+{
+	if (IS_I830(dev_priv)) {
+		init_unused_ring(dev_priv, PRB1_BASE);
+		init_unused_ring(dev_priv, SRB0_BASE);
+		init_unused_ring(dev_priv, SRB1_BASE);
+		init_unused_ring(dev_priv, SRB2_BASE);
+		init_unused_ring(dev_priv, SRB3_BASE);
+	} else if (IS_GEN2(dev_priv)) {
+		init_unused_ring(dev_priv, SRB0_BASE);
+		init_unused_ring(dev_priv, SRB1_BASE);
+	} else if (IS_GEN3(dev_priv)) {
+		init_unused_ring(dev_priv, PRB1_BASE);
+		init_unused_ring(dev_priv, PRB2_BASE);
 	}
 }
 
@@ -4344,6 +4432,7 @@ i915_gem_init_hw(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = to_i915(dev);
 	struct intel_engine_cs *engine;
+	enum intel_engine_id id;
 	int ret;
 
 	/* Double layer security blanket, see i915_gem_init() */
@@ -4352,12 +4441,12 @@ i915_gem_init_hw(struct drm_device *dev)
 	if (HAS_EDRAM(dev) && INTEL_GEN(dev_priv) < 9)
 		I915_WRITE(HSW_IDICR, I915_READ(HSW_IDICR) | IDIHASHMSK(0xf));
 
-	if (IS_HASWELL(dev))
-		I915_WRITE(MI_PREDICATE_RESULT_2, IS_HSW_GT3(dev) ?
+	if (IS_HASWELL(dev_priv))
+		I915_WRITE(MI_PREDICATE_RESULT_2, IS_HSW_GT3(dev_priv) ?
 			   LOWER_SLICE_ENABLED : LOWER_SLICE_DISABLED);
 
-	if (HAS_PCH_NOP(dev)) {
-		if (IS_IVYBRIDGE(dev)) {
+	if (HAS_PCH_NOP(dev_priv)) {
+		if (IS_IVYBRIDGE(dev_priv)) {
 			u32 temp = I915_READ(GEN7_MSG_CTL);
 			temp &= ~(WAIT_FOR_PCH_FLR_ACK | WAIT_FOR_PCH_RESET_ACK);
 			I915_WRITE(GEN7_MSG_CTL, temp);
@@ -4376,7 +4465,7 @@ i915_gem_init_hw(struct drm_device *dev)
 	 * will prevent c3 entry. Makes sure all unused rings
 	 * are totally idle.
 	 */
-	init_unused_rings(dev);
+	init_unused_rings(dev_priv);
 
 	BUG_ON(!dev_priv->kernel_context);
 
@@ -4387,7 +4476,7 @@ i915_gem_init_hw(struct drm_device *dev)
 	}
 
 	/* Need to do basic initialisation of all rings first: */
-	for_each_engine(engine, dev_priv) {
+	for_each_engine(engine, dev_priv, id) {
 		ret = engine->init_hw(engine);
 		if (ret)
 			goto out;
@@ -4486,17 +4575,12 @@ i915_gem_cleanup_engines(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = to_i915(dev);
 	struct intel_engine_cs *engine;
+	enum intel_engine_id id;
 
-	for_each_engine(engine, dev_priv)
+	for_each_engine(engine, dev_priv, id)
 		dev_priv->gt.cleanup_engine(engine);
 }
 
-static void
-init_engine_lists(struct intel_engine_cs *engine)
-{
-	INIT_LIST_HEAD(&engine->request_list);
-}
-
 void
 i915_gem_load_init_fences(struct drm_i915_private *dev_priv)
 {
@@ -4533,7 +4617,6 @@ void
 i915_gem_load_init(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = to_i915(dev);
-	int i;
 
 	dev_priv->objects =
 		kmem_cache_create("i915_gem_object",
@@ -4557,8 +4640,6 @@ i915_gem_load_init(struct drm_device *dev)
 	INIT_LIST_HEAD(&dev_priv->mm.unbound_list);
 	INIT_LIST_HEAD(&dev_priv->mm.bound_list);
 	INIT_LIST_HEAD(&dev_priv->mm.fence_list);
-	for (i = 0; i < I915_NUM_ENGINES; i++)
-		init_engine_lists(&dev_priv->engine[i]);
 	INIT_DELAYED_WORK(&dev_priv->gt.retire_work,
 			  i915_gem_retire_work_handler);
 	INIT_DELAYED_WORK(&dev_priv->gt.idle_work,
@@ -4589,6 +4670,19 @@ void i915_gem_load_cleanup(struct drm_device *dev)
 	rcu_barrier();
 }
 
+int i915_gem_freeze(struct drm_i915_private *dev_priv)
+{
+	intel_runtime_pm_get(dev_priv);
+
+	mutex_lock(&dev_priv->drm.struct_mutex);
+	i915_gem_shrink_all(dev_priv);
+	mutex_unlock(&dev_priv->drm.struct_mutex);
+
+	intel_runtime_pm_put(dev_priv);
+
+	return 0;
+}
+
 int i915_gem_freeze_late(struct drm_i915_private *dev_priv)
 {
 	struct drm_i915_gem_object *obj;
@@ -4612,7 +4706,8 @@ int i915_gem_freeze_late(struct drm_i915_private *dev_priv)
 	 * the objects as well.
 	 */
 
-	i915_gem_shrink_all(dev_priv);
+	mutex_lock(&dev_priv->drm.struct_mutex);
+	i915_gem_shrink(dev_priv, -1UL, I915_SHRINK_UNBOUND);
 
 	for (p = phases; *p; p++) {
 		list_for_each_entry(obj, *p, global_list) {
@@ -4620,6 +4715,7 @@ int i915_gem_freeze_late(struct drm_i915_private *dev_priv)
 			obj->base.write_domain = I915_GEM_DOMAIN_CPU;
 		}
 	}
+	mutex_unlock(&dev_priv->drm.struct_mutex);
 
 	return 0;
 }
diff --git a/drivers/gpu/drm/i915/i915_gem_context.c b/drivers/gpu/drm/i915/i915_gem_context.c
index df10f4e95736..5dca32ac1c67 100644
--- a/drivers/gpu/drm/i915/i915_gem_context.c
+++ b/drivers/gpu/drm/i915/i915_gem_context.c
@@ -192,7 +192,7 @@ i915_gem_alloc_context_obj(struct drm_device *dev, size_t size)
 	 * This is only applicable for Ivy Bridge devices since
 	 * later platforms don't have L3 control bits in the PTE.
 	 */
-	if (IS_IVYBRIDGE(dev)) {
+	if (IS_IVYBRIDGE(to_i915(dev))) {
 		ret = i915_gem_object_set_cache_level(obj, I915_CACHE_L3_LLC);
 		/* Failure shouldn't ever happen this early */
 		if (WARN_ON(ret)) {
@@ -474,10 +474,11 @@ int i915_gem_context_init(struct drm_device *dev)
 void i915_gem_context_lost(struct drm_i915_private *dev_priv)
 {
 	struct intel_engine_cs *engine;
+	enum intel_engine_id id;
 
 	lockdep_assert_held(&dev_priv->drm.struct_mutex);
 
-	for_each_engine(engine, dev_priv) {
+	for_each_engine(engine, dev_priv, id) {
 		if (engine->last_context) {
 			i915_gem_context_unpin(engine->last_context, engine);
 			engine->last_context = NULL;
@@ -492,13 +493,13 @@ void i915_gem_context_lost(struct drm_i915_private *dev_priv)
 			if (!i915_gem_context_is_default(ctx))
 				continue;
 
-			for_each_engine(engine, dev_priv)
+			for_each_engine(engine, dev_priv, id)
 				ctx->engine[engine->id].initialised = false;
 
 			ctx->remap_slice = ALL_L3_SLICES(dev_priv);
 		}
 
-		for_each_engine(engine, dev_priv) {
+		for_each_engine(engine, dev_priv, id) {
 			struct intel_context *kce =
 				&dev_priv->kernel_context->engine[engine->id];
 
@@ -563,6 +564,7 @@ mi_set_context(struct drm_i915_gem_request *req, u32 hw_flags)
 	struct drm_i915_private *dev_priv = req->i915;
 	struct intel_ring *ring = req->ring;
 	struct intel_engine_cs *engine = req->engine;
+	enum intel_engine_id id;
 	u32 flags = hw_flags | MI_MM_SPACE_GTT;
 	const int num_rings =
 		/* Use an extended w/a on ivb+ if signalling from other rings */
@@ -605,7 +607,7 @@ mi_set_context(struct drm_i915_gem_request *req, u32 hw_flags)
 
 			intel_ring_emit(ring,
 					MI_LOAD_REGISTER_IMM(num_rings));
-			for_each_engine(signaller, dev_priv) {
+			for_each_engine(signaller, dev_priv, id) {
 				if (signaller == engine)
 					continue;
 
@@ -634,7 +636,7 @@ mi_set_context(struct drm_i915_gem_request *req, u32 hw_flags)
 
 			intel_ring_emit(ring,
 					MI_LOAD_REGISTER_IMM(num_rings));
-			for_each_engine(signaller, dev_priv) {
+			for_each_engine(signaller, dev_priv, id) {
 				if (signaller == engine)
 					continue;
 
@@ -929,8 +931,9 @@ int i915_switch_context(struct drm_i915_gem_request *req)
 int i915_gem_switch_to_kernel_context(struct drm_i915_private *dev_priv)
 {
 	struct intel_engine_cs *engine;
+	enum intel_engine_id id;
 
-	for_each_engine(engine, dev_priv) {
+	for_each_engine(engine, dev_priv, id) {
 		struct drm_i915_gem_request *req;
 		int ret;
 
diff --git a/drivers/gpu/drm/i915/i915_gem_evict.c b/drivers/gpu/drm/i915/i915_gem_evict.c
index 5b6f81c1dbca..b5e9e669f50f 100644
--- a/drivers/gpu/drm/i915/i915_gem_evict.c
+++ b/drivers/gpu/drm/i915/i915_gem_evict.c
@@ -37,8 +37,9 @@ static bool
 gpu_is_idle(struct drm_i915_private *dev_priv)
 {
 	struct intel_engine_cs *engine;
+	enum intel_engine_id id;
 
-	for_each_engine(engine, dev_priv) {
+	for_each_engine(engine, dev_priv, id) {
 		if (intel_engine_is_active(engine))
 			return false;
 	}
diff --git a/drivers/gpu/drm/i915/i915_gem_execbuffer.c b/drivers/gpu/drm/i915/i915_gem_execbuffer.c
index 33c85227643d..e52affdcc125 100644
--- a/drivers/gpu/drm/i915/i915_gem_execbuffer.c
+++ b/drivers/gpu/drm/i915/i915_gem_execbuffer.c
@@ -370,8 +370,7 @@ static void reloc_cache_fini(struct reloc_cache *cache)
 
 			ggtt->base.clear_range(&ggtt->base,
 					       cache->node.start,
-					       cache->node.size,
-					       true);
+					       cache->node.size);
 			drm_mm_remove_node(&cache->node);
 		} else {
 			i915_vma_unpin((struct i915_vma *)cache->node.mm);
@@ -429,7 +428,7 @@ static void *reloc_iomap(struct drm_i915_gem_object *obj,
 	}
 
 	if (cache->vaddr) {
-		io_mapping_unmap_atomic(unmask_page(cache->vaddr));
+		io_mapping_unmap_atomic((void __force __iomem *) unmask_page(cache->vaddr));
 	} else {
 		struct i915_vma *vma;
 		int ret;
@@ -451,8 +450,8 @@ static void *reloc_iomap(struct drm_i915_gem_object *obj,
 				 0, ggtt->mappable_end,
 				 DRM_MM_SEARCH_DEFAULT,
 				 DRM_MM_CREATE_DEFAULT);
-			if (ret)
-				return ERR_PTR(ret);
+			if (ret) /* no inactive aperture space, use cpu reloc */
+				return NULL;
 		} else {
 			ret = i915_vma_put_fence(vma);
 			if (ret) {
@@ -474,7 +473,7 @@ static void *reloc_iomap(struct drm_i915_gem_object *obj,
 		offset += page << PAGE_SHIFT;
 	}
 
-	vaddr = io_mapping_map_atomic_wc(&cache->i915->ggtt.mappable, offset);
+	vaddr = (void __force *) io_mapping_map_atomic_wc(&cache->i915->ggtt.mappable, offset);
 	cache->page = page;
 	cache->vaddr = (unsigned long)vaddr;
 
@@ -552,27 +551,13 @@ repeat:
 	return 0;
 }
 
-static bool object_is_idle(struct drm_i915_gem_object *obj)
-{
-	unsigned long active = i915_gem_object_get_active(obj);
-	int idx;
-
-	for_each_active(active, idx) {
-		if (!i915_gem_active_is_idle(&obj->last_read[idx],
-					     &obj->base.dev->struct_mutex))
-			return false;
-	}
-
-	return true;
-}
-
 static int
 i915_gem_execbuffer_relocate_entry(struct drm_i915_gem_object *obj,
 				   struct eb_vmas *eb,
 				   struct drm_i915_gem_relocation_entry *reloc,
 				   struct reloc_cache *cache)
 {
-	struct drm_device *dev = obj->base.dev;
+	struct drm_i915_private *dev_priv = to_i915(obj->base.dev);
 	struct drm_gem_object *target_obj;
 	struct drm_i915_gem_object *target_i915_obj;
 	struct i915_vma *target_vma;
@@ -591,7 +576,7 @@ i915_gem_execbuffer_relocate_entry(struct drm_i915_gem_object *obj,
 	/* Sandybridge PPGTT errata: We need a global gtt mapping for MI and
 	 * pipe_control writes because the gpu doesn't properly redirect them
 	 * through the ppgtt for non_secure batchbuffers. */
-	if (unlikely(IS_GEN6(dev) &&
+	if (unlikely(IS_GEN6(dev_priv) &&
 	    reloc->write_domain == I915_GEM_DOMAIN_INSTRUCTION)) {
 		ret = i915_vma_bind(target_vma, target_i915_obj->cache_level,
 				    PIN_GLOBAL);
@@ -649,10 +634,6 @@ i915_gem_execbuffer_relocate_entry(struct drm_i915_gem_object *obj,
 		return -EINVAL;
 	}
 
-	/* We can't wait for rendering with pagefaults disabled */
-	if (pagefault_disabled() && !object_is_idle(obj))
-		return -EFAULT;
-
 	ret = relocate_entry(obj, reloc, cache, target_offset);
 	if (ret)
 		return ret;
@@ -679,12 +660,23 @@ i915_gem_execbuffer_relocate_vma(struct i915_vma *vma,
 	remain = entry->relocation_count;
 	while (remain) {
 		struct drm_i915_gem_relocation_entry *r = stack_reloc;
-		int count = remain;
-		if (count > ARRAY_SIZE(stack_reloc))
-			count = ARRAY_SIZE(stack_reloc);
+		unsigned long unwritten;
+		unsigned int count;
+
+		count = min_t(unsigned int, remain, ARRAY_SIZE(stack_reloc));
 		remain -= count;
 
-		if (__copy_from_user_inatomic(r, user_relocs, count*sizeof(r[0]))) {
+		/* This is the fast path and we cannot handle a pagefault
+		 * whilst holding the struct mutex lest the user pass in the
+		 * relocations contained within a mmaped bo. For in such a case
+		 * we, the page fault handler would call i915_gem_fault() and
+		 * we would try to acquire the struct mutex again. Obviously
+		 * this is bad and so lockdep complains vehemently.
+		 */
+		pagefault_disable();
+		unwritten = __copy_from_user_inatomic(r, user_relocs, count*sizeof(r[0]));
+		pagefault_enable();
+		if (unlikely(unwritten)) {
 			ret = -EFAULT;
 			goto out;
 		}
@@ -696,11 +688,26 @@ i915_gem_execbuffer_relocate_vma(struct i915_vma *vma,
 			if (ret)
 				goto out;
 
-			if (r->presumed_offset != offset &&
-			    __put_user(r->presumed_offset,
-				       &user_relocs->presumed_offset)) {
-				ret = -EFAULT;
-				goto out;
+			if (r->presumed_offset != offset) {
+				pagefault_disable();
+				unwritten = __put_user(r->presumed_offset,
+						       &user_relocs->presumed_offset);
+				pagefault_enable();
+				if (unlikely(unwritten)) {
+					/* Note that reporting an error now
+					 * leaves everything in an inconsistent
+					 * state as we have *already* changed
+					 * the relocation value inside the
+					 * object. As we have not changed the
+					 * reloc.presumed_offset or will not
+					 * change the execobject.offset, on the
+					 * call we may not rewrite the value
+					 * inside the object, leaving it
+					 * dangling and causing a GPU hang.
+					 */
+					ret = -EFAULT;
+					goto out;
+				}
 			}
 
 			user_relocs++;
@@ -740,20 +747,11 @@ i915_gem_execbuffer_relocate(struct eb_vmas *eb)
 	struct i915_vma *vma;
 	int ret = 0;
 
-	/* This is the fast path and we cannot handle a pagefault whilst
-	 * holding the struct mutex lest the user pass in the relocations
-	 * contained within a mmaped bo. For in such a case we, the page
-	 * fault handler would call i915_gem_fault() and we would try to
-	 * acquire the struct mutex again. Obviously this is bad and so
-	 * lockdep complains vehemently.
-	 */
-	pagefault_disable();
 	list_for_each_entry(vma, &eb->vmas, exec_list) {
 		ret = i915_gem_execbuffer_relocate_vma(vma, eb);
 		if (ret)
 			break;
 	}
-	pagefault_enable();
 
 	return ret;
 }
@@ -1253,7 +1251,7 @@ validate_exec_list(struct drm_device *dev,
 			return -EFAULT;
 
 		if (likely(!i915.prefault_disable)) {
-			if (fault_in_multipages_readable(ptr, length))
+			if (fault_in_pages_readable(ptr, length))
 				return -EFAULT;
 		}
 	}
@@ -1599,12 +1597,12 @@ eb_select_engine(struct drm_i915_private *dev_priv,
 			return NULL;
 		}
 
-		engine = &dev_priv->engine[_VCS(bsd_idx)];
+		engine = dev_priv->engine[_VCS(bsd_idx)];
 	} else {
-		engine = &dev_priv->engine[user_ring_map[user_ring_id]];
+		engine = dev_priv->engine[user_ring_map[user_ring_id]];
 	}
 
-	if (!intel_engine_initialized(engine)) {
+	if (!engine) {
 		DRM_DEBUG("execbuf with invalid ring: %u\n", user_ring_id);
 		return NULL;
 	}
diff --git a/drivers/gpu/drm/i915/i915_gem_fence.c b/drivers/gpu/drm/i915/i915_gem_fence.c
index 8df1fa7234e8..a6daf2deab74 100644
--- a/drivers/gpu/drm/i915/i915_gem_fence.c
+++ b/drivers/gpu/drm/i915/i915_gem_fence.c
@@ -290,6 +290,8 @@ i915_vma_put_fence(struct i915_vma *vma)
 {
 	struct drm_i915_fence_reg *fence = vma->fence;
 
+	assert_rpm_wakelock_held(to_i915(vma->vm->dev));
+
 	if (!fence)
 		return 0;
 
@@ -341,6 +343,8 @@ i915_vma_get_fence(struct i915_vma *vma)
 	struct drm_i915_fence_reg *fence;
 	struct i915_vma *set = i915_gem_object_is_tiled(vma->obj) ? vma : NULL;
 
+	assert_rpm_wakelock_held(to_i915(vma->vm->dev));
+
 	/* Just update our place in the LRU if our fence is getting reused. */
 	if (vma->fence) {
 		fence = vma->fence;
@@ -371,6 +375,12 @@ void i915_gem_restore_fences(struct drm_device *dev)
 	struct drm_i915_private *dev_priv = to_i915(dev);
 	int i;
 
+	/* Note that this may be called outside of struct_mutex, by
+	 * runtime suspend/resume. The barrier we require is enforced by
+	 * rpm itself - all access to fences/GTT are only within an rpm
+	 * wakeref, and to acquire that wakeref you must pass through here.
+	 */
+
 	for (i = 0; i < dev_priv->num_fence_regs; i++) {
 		struct drm_i915_fence_reg *reg = &dev_priv->fence_regs[i];
 		struct i915_vma *vma = reg->vma;
@@ -379,10 +389,17 @@ void i915_gem_restore_fences(struct drm_device *dev)
 		 * Commit delayed tiling changes if we have an object still
 		 * attached to the fence, otherwise just clear the fence.
 		 */
-		if (vma && !i915_gem_object_is_tiled(vma->obj))
+		if (vma && !i915_gem_object_is_tiled(vma->obj)) {
+			GEM_BUG_ON(!reg->dirty);
+			GEM_BUG_ON(vma->obj->fault_mappable);
+
+			list_move(&reg->link, &dev_priv->mm.fence_list);
+			vma->fence = NULL;
 			vma = NULL;
+		}
 
-		fence_update(reg, vma);
+		fence_write(reg, vma);
+		reg->vma = vma;
 	}
 }
 
@@ -448,7 +465,7 @@ i915_gem_detect_bit_6_swizzle(struct drm_device *dev)
 	uint32_t swizzle_x = I915_BIT_6_SWIZZLE_UNKNOWN;
 	uint32_t swizzle_y = I915_BIT_6_SWIZZLE_UNKNOWN;
 
-	if (INTEL_INFO(dev)->gen >= 8 || IS_VALLEYVIEW(dev)) {
+	if (INTEL_GEN(dev_priv) >= 8 || IS_VALLEYVIEW(dev_priv)) {
 		/*
 		 * On BDW+, swizzling is not used. We leave the CPU memory
 		 * controller in charge of optimizing memory accesses without
@@ -487,19 +504,20 @@ i915_gem_detect_bit_6_swizzle(struct drm_device *dev)
 				swizzle_y = I915_BIT_6_SWIZZLE_NONE;
 			}
 		}
-	} else if (IS_GEN5(dev)) {
+	} else if (IS_GEN5(dev_priv)) {
 		/* On Ironlake whatever DRAM config, GPU always do
 		 * same swizzling setup.
 		 */
 		swizzle_x = I915_BIT_6_SWIZZLE_9_10;
 		swizzle_y = I915_BIT_6_SWIZZLE_9;
-	} else if (IS_GEN2(dev)) {
+	} else if (IS_GEN2(dev_priv)) {
 		/* As far as we know, the 865 doesn't have these bit 6
 		 * swizzling issues.
 		 */
 		swizzle_x = I915_BIT_6_SWIZZLE_NONE;
 		swizzle_y = I915_BIT_6_SWIZZLE_NONE;
-	} else if (IS_MOBILE(dev) || (IS_GEN3(dev) && !IS_G33(dev))) {
+	} else if (IS_MOBILE(dev_priv) || (IS_GEN3(dev_priv) &&
+		   !IS_G33(dev_priv))) {
 		uint32_t dcc;
 
 		/* On 9xx chipsets, channel interleave by the CPU is
@@ -537,7 +555,7 @@ i915_gem_detect_bit_6_swizzle(struct drm_device *dev)
 		}
 
 		/* check for L-shaped memory aka modified enhanced addressing */
-		if (IS_GEN4(dev) &&
+		if (IS_GEN4(dev_priv) &&
 		    !(I915_READ(DCC2) & DCC2_MODIFIED_ENHANCED_DISABLE)) {
 			swizzle_x = I915_BIT_6_SWIZZLE_UNKNOWN;
 			swizzle_y = I915_BIT_6_SWIZZLE_UNKNOWN;
diff --git a/drivers/gpu/drm/i915/i915_gem_gtt.c b/drivers/gpu/drm/i915/i915_gem_gtt.c
index 0bb4232f66bc..062fb0ad75da 100644
--- a/drivers/gpu/drm/i915/i915_gem_gtt.c
+++ b/drivers/gpu/drm/i915/i915_gem_gtt.c
@@ -191,15 +191,13 @@ static void ppgtt_unbind_vma(struct i915_vma *vma)
 {
 	vma->vm->clear_range(vma->vm,
 			     vma->node.start,
-			     vma->size,
-			     true);
+			     vma->size);
 }
 
 static gen8_pte_t gen8_pte_encode(dma_addr_t addr,
-				  enum i915_cache_level level,
-				  bool valid)
+				  enum i915_cache_level level)
 {
-	gen8_pte_t pte = valid ? _PAGE_PRESENT | _PAGE_RW : 0;
+	gen8_pte_t pte = _PAGE_PRESENT | _PAGE_RW;
 	pte |= addr;
 
 	switch (level) {
@@ -234,9 +232,9 @@ static gen8_pde_t gen8_pde_encode(const dma_addr_t addr,
 
 static gen6_pte_t snb_pte_encode(dma_addr_t addr,
 				 enum i915_cache_level level,
-				 bool valid, u32 unused)
+				 u32 unused)
 {
-	gen6_pte_t pte = valid ? GEN6_PTE_VALID : 0;
+	gen6_pte_t pte = GEN6_PTE_VALID;
 	pte |= GEN6_PTE_ADDR_ENCODE(addr);
 
 	switch (level) {
@@ -256,9 +254,9 @@ static gen6_pte_t snb_pte_encode(dma_addr_t addr,
 
 static gen6_pte_t ivb_pte_encode(dma_addr_t addr,
 				 enum i915_cache_level level,
-				 bool valid, u32 unused)
+				 u32 unused)
 {
-	gen6_pte_t pte = valid ? GEN6_PTE_VALID : 0;
+	gen6_pte_t pte = GEN6_PTE_VALID;
 	pte |= GEN6_PTE_ADDR_ENCODE(addr);
 
 	switch (level) {
@@ -280,9 +278,9 @@ static gen6_pte_t ivb_pte_encode(dma_addr_t addr,
 
 static gen6_pte_t byt_pte_encode(dma_addr_t addr,
 				 enum i915_cache_level level,
-				 bool valid, u32 flags)
+				 u32 flags)
 {
-	gen6_pte_t pte = valid ? GEN6_PTE_VALID : 0;
+	gen6_pte_t pte = GEN6_PTE_VALID;
 	pte |= GEN6_PTE_ADDR_ENCODE(addr);
 
 	if (!(flags & PTE_READ_ONLY))
@@ -296,9 +294,9 @@ static gen6_pte_t byt_pte_encode(dma_addr_t addr,
 
 static gen6_pte_t hsw_pte_encode(dma_addr_t addr,
 				 enum i915_cache_level level,
-				 bool valid, u32 unused)
+				 u32 unused)
 {
-	gen6_pte_t pte = valid ? GEN6_PTE_VALID : 0;
+	gen6_pte_t pte = GEN6_PTE_VALID;
 	pte |= HSW_PTE_ADDR_ENCODE(addr);
 
 	if (level != I915_CACHE_NONE)
@@ -309,9 +307,9 @@ static gen6_pte_t hsw_pte_encode(dma_addr_t addr,
 
 static gen6_pte_t iris_pte_encode(dma_addr_t addr,
 				  enum i915_cache_level level,
-				  bool valid, u32 unused)
+				  u32 unused)
 {
-	gen6_pte_t pte = valid ? GEN6_PTE_VALID : 0;
+	gen6_pte_t pte = GEN6_PTE_VALID;
 	pte |= HSW_PTE_ADDR_ENCODE(addr);
 
 	switch (level) {
@@ -373,27 +371,29 @@ static void *kmap_page_dma(struct i915_page_dma *p)
 /* We use the flushing unmap only with ppgtt structures:
  * page directories, page tables and scratch pages.
  */
-static void kunmap_page_dma(struct drm_device *dev, void *vaddr)
+static void kunmap_page_dma(struct drm_i915_private *dev_priv, void *vaddr)
 {
 	/* There are only few exceptions for gen >=6. chv and bxt.
 	 * And we are not sure about the latter so play safe for now.
 	 */
-	if (IS_CHERRYVIEW(dev) || IS_BROXTON(dev))
+	if (IS_CHERRYVIEW(dev_priv) || IS_BROXTON(dev_priv))
 		drm_clflush_virt_range(vaddr, PAGE_SIZE);
 
 	kunmap_atomic(vaddr);
 }
 
 #define kmap_px(px) kmap_page_dma(px_base(px))
-#define kunmap_px(ppgtt, vaddr) kunmap_page_dma((ppgtt)->base.dev, (vaddr))
+#define kunmap_px(ppgtt, vaddr) \
+		kunmap_page_dma(to_i915((ppgtt)->base.dev), (vaddr))
 
 #define setup_px(dev, px) setup_page_dma((dev), px_base(px))
 #define cleanup_px(dev, px) cleanup_page_dma((dev), px_base(px))
-#define fill_px(dev, px, v) fill_page_dma((dev), px_base(px), (v))
-#define fill32_px(dev, px, v) fill_page_dma_32((dev), px_base(px), (v))
+#define fill_px(dev_priv, px, v) fill_page_dma((dev_priv), px_base(px), (v))
+#define fill32_px(dev_priv, px, v) \
+		fill_page_dma_32((dev_priv), px_base(px), (v))
 
-static void fill_page_dma(struct drm_device *dev, struct i915_page_dma *p,
-			  const uint64_t val)
+static void fill_page_dma(struct drm_i915_private *dev_priv,
+			  struct i915_page_dma *p, const uint64_t val)
 {
 	int i;
 	uint64_t * const vaddr = kmap_page_dma(p);
@@ -401,17 +401,17 @@ static void fill_page_dma(struct drm_device *dev, struct i915_page_dma *p,
 	for (i = 0; i < 512; i++)
 		vaddr[i] = val;
 
-	kunmap_page_dma(dev, vaddr);
+	kunmap_page_dma(dev_priv, vaddr);
 }
 
-static void fill_page_dma_32(struct drm_device *dev, struct i915_page_dma *p,
-			     const uint32_t val32)
+static void fill_page_dma_32(struct drm_i915_private *dev_priv,
+			     struct i915_page_dma *p, const uint32_t val32)
 {
 	uint64_t v = val32;
 
 	v = v << 32 | val32;
 
-	fill_page_dma(dev, p, v);
+	fill_page_dma(dev_priv, p, v);
 }
 
 static int
@@ -472,9 +472,9 @@ static void gen8_initialize_pt(struct i915_address_space *vm,
 	gen8_pte_t scratch_pte;
 
 	scratch_pte = gen8_pte_encode(vm->scratch_page.daddr,
-				      I915_CACHE_LLC, true);
+				      I915_CACHE_LLC);
 
-	fill_px(vm->dev, pt, scratch_pte);
+	fill_px(to_i915(vm->dev), pt, scratch_pte);
 }
 
 static void gen6_initialize_pt(struct i915_address_space *vm,
@@ -485,9 +485,9 @@ static void gen6_initialize_pt(struct i915_address_space *vm,
 	WARN_ON(vm->scratch_page.daddr == 0);
 
 	scratch_pte = vm->pte_encode(vm->scratch_page.daddr,
-				     I915_CACHE_LLC, true, 0);
+				     I915_CACHE_LLC, 0);
 
-	fill32_px(vm->dev, pt, scratch_pte);
+	fill32_px(to_i915(vm->dev), pt, scratch_pte);
 }
 
 static struct i915_page_directory *alloc_pd(struct drm_device *dev)
@@ -534,7 +534,7 @@ static void gen8_initialize_pd(struct i915_address_space *vm,
 
 	scratch_pde = gen8_pde_encode(px_dma(vm->scratch_pt), I915_CACHE_LLC);
 
-	fill_px(vm->dev, pd, scratch_pde);
+	fill_px(to_i915(vm->dev), pd, scratch_pde);
 }
 
 static int __pdp_init(struct drm_device *dev,
@@ -615,7 +615,7 @@ static void gen8_initialize_pdp(struct i915_address_space *vm,
 
 	scratch_pdpe = gen8_pdpe_encode(px_dma(vm->scratch_pd), I915_CACHE_LLC);
 
-	fill_px(vm->dev, pdp, scratch_pdpe);
+	fill_px(to_i915(vm->dev), pdp, scratch_pdpe);
 }
 
 static void gen8_initialize_pml4(struct i915_address_space *vm,
@@ -626,7 +626,7 @@ static void gen8_initialize_pml4(struct i915_address_space *vm,
 	scratch_pml4e = gen8_pml4e_encode(px_dma(vm->scratch_pdp),
 					  I915_CACHE_LLC);
 
-	fill_px(vm->dev, pml4, scratch_pml4e);
+	fill_px(to_i915(vm->dev), pml4, scratch_pml4e);
 }
 
 static void
@@ -706,85 +706,157 @@ static int gen8_48b_mm_switch(struct i915_hw_ppgtt *ppgtt,
 	return gen8_write_pdp(req, 0, px_dma(&ppgtt->pml4));
 }
 
-static void gen8_ppgtt_clear_pte_range(struct i915_address_space *vm,
-				       struct i915_page_directory_pointer *pdp,
-				       uint64_t start,
-				       uint64_t length,
-				       gen8_pte_t scratch_pte)
+/* Removes entries from a single page table, releasing it if it's empty.
+ * Caller can use the return value to update higher-level entries.
+ */
+static bool gen8_ppgtt_clear_pt(struct i915_address_space *vm,
+				struct i915_page_table *pt,
+				uint64_t start,
+				uint64_t length)
 {
 	struct i915_hw_ppgtt *ppgtt = i915_vm_to_ppgtt(vm);
+	unsigned int pte_start = gen8_pte_index(start);
+	unsigned int num_entries = gen8_pte_count(start, length);
+	uint64_t pte;
 	gen8_pte_t *pt_vaddr;
-	unsigned pdpe = gen8_pdpe_index(start);
-	unsigned pde = gen8_pde_index(start);
-	unsigned pte = gen8_pte_index(start);
-	unsigned num_entries = length >> PAGE_SHIFT;
-	unsigned last_pte, i;
+	gen8_pte_t scratch_pte = gen8_pte_encode(vm->scratch_page.daddr,
+						 I915_CACHE_LLC);
 
-	if (WARN_ON(!pdp))
-		return;
+	if (WARN_ON(!px_page(pt)))
+		return false;
 
-	while (num_entries) {
-		struct i915_page_directory *pd;
-		struct i915_page_table *pt;
+	bitmap_clear(pt->used_ptes, pte_start, num_entries);
 
-		if (WARN_ON(!pdp->page_directory[pdpe]))
-			break;
+	if (bitmap_empty(pt->used_ptes, GEN8_PTES)) {
+		free_pt(vm->dev, pt);
+		return true;
+	}
 
-		pd = pdp->page_directory[pdpe];
+	pt_vaddr = kmap_px(pt);
 
-		if (WARN_ON(!pd->page_table[pde]))
-			break;
+	for (pte = pte_start; pte < num_entries; pte++)
+		pt_vaddr[pte] = scratch_pte;
 
-		pt = pd->page_table[pde];
+	kunmap_px(ppgtt, pt_vaddr);
 
-		if (WARN_ON(!px_page(pt)))
-			break;
+	return false;
+}
 
-		last_pte = pte + num_entries;
-		if (last_pte > GEN8_PTES)
-			last_pte = GEN8_PTES;
+/* Removes entries from a single page dir, releasing it if it's empty.
+ * Caller can use the return value to update higher-level entries
+ */
+static bool gen8_ppgtt_clear_pd(struct i915_address_space *vm,
+				struct i915_page_directory *pd,
+				uint64_t start,
+				uint64_t length)
+{
+	struct i915_hw_ppgtt *ppgtt = i915_vm_to_ppgtt(vm);
+	struct i915_page_table *pt;
+	uint64_t pde;
+	gen8_pde_t *pde_vaddr;
+	gen8_pde_t scratch_pde = gen8_pde_encode(px_dma(vm->scratch_pt),
+						 I915_CACHE_LLC);
 
-		pt_vaddr = kmap_px(pt);
+	gen8_for_each_pde(pt, pd, start, length, pde) {
+		if (WARN_ON(!pd->page_table[pde]))
+			break;
 
-		for (i = pte; i < last_pte; i++) {
-			pt_vaddr[i] = scratch_pte;
-			num_entries--;
+		if (gen8_ppgtt_clear_pt(vm, pt, start, length)) {
+			__clear_bit(pde, pd->used_pdes);
+			pde_vaddr = kmap_px(pd);
+			pde_vaddr[pde] = scratch_pde;
+			kunmap_px(ppgtt, pde_vaddr);
 		}
+	}
 
-		kunmap_px(ppgtt, pt_vaddr);
+	if (bitmap_empty(pd->used_pdes, I915_PDES)) {
+		free_pd(vm->dev, pd);
+		return true;
+	}
+
+	return false;
+}
 
-		pte = 0;
-		if (++pde == I915_PDES) {
-			if (++pdpe == I915_PDPES_PER_PDP(vm->dev))
-				break;
-			pde = 0;
+/* Removes entries from a single page dir pointer, releasing it if it's empty.
+ * Caller can use the return value to update higher-level entries
+ */
+static bool gen8_ppgtt_clear_pdp(struct i915_address_space *vm,
+				 struct i915_page_directory_pointer *pdp,
+				 uint64_t start,
+				 uint64_t length)
+{
+	struct i915_hw_ppgtt *ppgtt = i915_vm_to_ppgtt(vm);
+	struct i915_page_directory *pd;
+	uint64_t pdpe;
+	gen8_ppgtt_pdpe_t *pdpe_vaddr;
+	gen8_ppgtt_pdpe_t scratch_pdpe =
+		gen8_pdpe_encode(px_dma(vm->scratch_pd), I915_CACHE_LLC);
+
+	gen8_for_each_pdpe(pd, pdp, start, length, pdpe) {
+		if (WARN_ON(!pdp->page_directory[pdpe]))
+			break;
+
+		if (gen8_ppgtt_clear_pd(vm, pd, start, length)) {
+			__clear_bit(pdpe, pdp->used_pdpes);
+			if (USES_FULL_48BIT_PPGTT(vm->dev)) {
+				pdpe_vaddr = kmap_px(pdp);
+				pdpe_vaddr[pdpe] = scratch_pdpe;
+				kunmap_px(ppgtt, pdpe_vaddr);
+			}
 		}
 	}
+
+	if (USES_FULL_48BIT_PPGTT(vm->dev) &&
+	    bitmap_empty(pdp->used_pdpes, I915_PDPES_PER_PDP(vm->dev))) {
+		free_pdp(vm->dev, pdp);
+		return true;
+	}
+
+	return false;
 }
 
-static void gen8_ppgtt_clear_range(struct i915_address_space *vm,
-				   uint64_t start,
-				   uint64_t length,
-				   bool use_scratch)
+/* Removes entries from a single pml4.
+ * This is the top-level structure in 4-level page tables used on gen8+.
+ * Empty entries are always scratch pml4e.
+ */
+static void gen8_ppgtt_clear_pml4(struct i915_address_space *vm,
+				  struct i915_pml4 *pml4,
+				  uint64_t start,
+				  uint64_t length)
 {
 	struct i915_hw_ppgtt *ppgtt = i915_vm_to_ppgtt(vm);
-	gen8_pte_t scratch_pte = gen8_pte_encode(vm->scratch_page.daddr,
-						 I915_CACHE_LLC, use_scratch);
+	struct i915_page_directory_pointer *pdp;
+	uint64_t pml4e;
+	gen8_ppgtt_pml4e_t *pml4e_vaddr;
+	gen8_ppgtt_pml4e_t scratch_pml4e =
+		gen8_pml4e_encode(px_dma(vm->scratch_pdp), I915_CACHE_LLC);
 
-	if (!USES_FULL_48BIT_PPGTT(vm->dev)) {
-		gen8_ppgtt_clear_pte_range(vm, &ppgtt->pdp, start, length,
-					   scratch_pte);
-	} else {
-		uint64_t pml4e;
-		struct i915_page_directory_pointer *pdp;
+	GEM_BUG_ON(!USES_FULL_48BIT_PPGTT(vm->dev));
 
-		gen8_for_each_pml4e(pdp, &ppgtt->pml4, start, length, pml4e) {
-			gen8_ppgtt_clear_pte_range(vm, pdp, start, length,
-						   scratch_pte);
+	gen8_for_each_pml4e(pdp, pml4, start, length, pml4e) {
+		if (WARN_ON(!pml4->pdps[pml4e]))
+			break;
+
+		if (gen8_ppgtt_clear_pdp(vm, pdp, start, length)) {
+			__clear_bit(pml4e, pml4->used_pml4es);
+			pml4e_vaddr = kmap_px(pml4);
+			pml4e_vaddr[pml4e] = scratch_pml4e;
+			kunmap_px(ppgtt, pml4e_vaddr);
 		}
 	}
 }
 
+static void gen8_ppgtt_clear_range(struct i915_address_space *vm,
+				   uint64_t start, uint64_t length)
+{
+	struct i915_hw_ppgtt *ppgtt = i915_vm_to_ppgtt(vm);
+
+	if (USES_FULL_48BIT_PPGTT(vm->dev))
+		gen8_ppgtt_clear_pml4(vm, &ppgtt->pml4, start, length);
+	else
+		gen8_ppgtt_clear_pdp(vm, &ppgtt->pdp, start, length);
+}
+
 static void
 gen8_ppgtt_insert_pte_entries(struct i915_address_space *vm,
 			      struct i915_page_directory_pointer *pdp,
@@ -809,7 +881,7 @@ gen8_ppgtt_insert_pte_entries(struct i915_address_space *vm,
 
 		pt_vaddr[pte] =
 			gen8_pte_encode(sg_page_iter_dma_address(sg_iter),
-					cache_level, true);
+					cache_level);
 		if (++pte == GEN8_PTES) {
 			kunmap_px(ppgtt, pt_vaddr);
 			pt_vaddr = NULL;
@@ -1452,7 +1524,7 @@ static void gen8_dump_ppgtt(struct i915_hw_ppgtt *ppgtt, struct seq_file *m)
 	uint64_t start = ppgtt->base.start;
 	uint64_t length = ppgtt->base.total;
 	gen8_pte_t scratch_pte = gen8_pte_encode(vm->scratch_page.daddr,
-						 I915_CACHE_LLC, true);
+						 I915_CACHE_LLC);
 
 	if (!USES_FULL_48BIT_PPGTT(vm->dev)) {
 		gen8_dump_pdp(&ppgtt->pdp, start, length, scratch_pte, m);
@@ -1569,7 +1641,7 @@ static void gen6_dump_ppgtt(struct i915_hw_ppgtt *ppgtt, struct seq_file *m)
 	uint32_t start = ppgtt->base.start, length = ppgtt->base.total;
 
 	scratch_pte = vm->pte_encode(vm->scratch_page.daddr,
-				     I915_CACHE_LLC, true, 0);
+				     I915_CACHE_LLC, 0);
 
 	gen6_for_each_pde(unused, &ppgtt->pd, start, length, pde) {
 		u32 expected;
@@ -1728,8 +1800,9 @@ static void gen8_ppgtt_enable(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = to_i915(dev);
 	struct intel_engine_cs *engine;
+	enum intel_engine_id id;
 
-	for_each_engine(engine, dev_priv) {
+	for_each_engine(engine, dev_priv, id) {
 		u32 four_level = USES_FULL_48BIT_PPGTT(dev) ? GEN8_GFX_PPGTT_48B : 0;
 		I915_WRITE(RING_MODE_GEN7(engine),
 			   _MASKED_BIT_ENABLE(GFX_PPGTT_ENABLE | four_level));
@@ -1741,12 +1814,13 @@ static void gen7_ppgtt_enable(struct drm_device *dev)
 	struct drm_i915_private *dev_priv = to_i915(dev);
 	struct intel_engine_cs *engine;
 	uint32_t ecochk, ecobits;
+	enum intel_engine_id id;
 
 	ecobits = I915_READ(GAC_ECO_BITS);
 	I915_WRITE(GAC_ECO_BITS, ecobits | ECOBITS_PPGTT_CACHE64B);
 
 	ecochk = I915_READ(GAM_ECOCHK);
-	if (IS_HASWELL(dev)) {
+	if (IS_HASWELL(dev_priv)) {
 		ecochk |= ECOCHK_PPGTT_WB_HSW;
 	} else {
 		ecochk |= ECOCHK_PPGTT_LLC_IVB;
@@ -1754,7 +1828,7 @@ static void gen7_ppgtt_enable(struct drm_device *dev)
 	}
 	I915_WRITE(GAM_ECOCHK, ecochk);
 
-	for_each_engine(engine, dev_priv) {
+	for_each_engine(engine, dev_priv, id) {
 		/* GFX_MODE is per-ring on gen7+ */
 		I915_WRITE(RING_MODE_GEN7(engine),
 			   _MASKED_BIT_ENABLE(GFX_PPGTT_ENABLE));
@@ -1782,8 +1856,7 @@ static void gen6_ppgtt_enable(struct drm_device *dev)
 /* PPGTT support for Sandybdrige/Gen6 and later */
 static void gen6_ppgtt_clear_range(struct i915_address_space *vm,
 				   uint64_t start,
-				   uint64_t length,
-				   bool use_scratch)
+				   uint64_t length)
 {
 	struct i915_hw_ppgtt *ppgtt = i915_vm_to_ppgtt(vm);
 	gen6_pte_t *pt_vaddr, scratch_pte;
@@ -1794,7 +1867,7 @@ static void gen6_ppgtt_clear_range(struct i915_address_space *vm,
 	unsigned last_pte, i;
 
 	scratch_pte = vm->pte_encode(vm->scratch_page.daddr,
-				     I915_CACHE_LLC, true, 0);
+				     I915_CACHE_LLC, 0);
 
 	while (num_entries) {
 		last_pte = first_pte + num_entries;
@@ -1832,7 +1905,7 @@ static void gen6_ppgtt_insert_entries(struct i915_address_space *vm,
 			pt_vaddr = kmap_px(ppgtt->pd.page_table[act_pt]);
 
 		pt_vaddr[act_pte] =
-			vm->pte_encode(addr, cache_level, true, flags);
+			vm->pte_encode(addr, cache_level, flags);
 
 		if (++act_pte == GEN6_PTES) {
 			kunmap_px(ppgtt, pt_vaddr);
@@ -2056,11 +2129,11 @@ static int gen6_ppgtt_init(struct i915_hw_ppgtt *ppgtt)
 	int ret;
 
 	ppgtt->base.pte_encode = ggtt->base.pte_encode;
-	if (intel_vgpu_active(dev_priv) || IS_GEN6(dev))
+	if (intel_vgpu_active(dev_priv) || IS_GEN6(dev_priv))
 		ppgtt->switch_mm = gen6_mm_switch;
-	else if (IS_HASWELL(dev))
+	else if (IS_HASWELL(dev_priv))
 		ppgtt->switch_mm = hsw_mm_switch;
-	else if (IS_GEN7(dev))
+	else if (IS_GEN7(dev_priv))
 		ppgtt->switch_mm = gen7_mm_switch;
 	else
 		BUG();
@@ -2129,13 +2202,13 @@ static void gtt_write_workarounds(struct drm_device *dev)
 	 * workarounds here even if they get overwritten by GPU reset.
 	 */
 	/* WaIncreaseDefaultTLBEntries:chv,bdw,skl,bxt */
-	if (IS_BROADWELL(dev))
+	if (IS_BROADWELL(dev_priv))
 		I915_WRITE(GEN8_L3_LRA_1_GPGPU, GEN8_L3_LRA_1_GPGPU_DEFAULT_VALUE_BDW);
-	else if (IS_CHERRYVIEW(dev))
+	else if (IS_CHERRYVIEW(dev_priv))
 		I915_WRITE(GEN8_L3_LRA_1_GPGPU, GEN8_L3_LRA_1_GPGPU_DEFAULT_VALUE_CHV);
-	else if (IS_SKYLAKE(dev))
+	else if (IS_SKYLAKE(dev_priv))
 		I915_WRITE(GEN8_L3_LRA_1_GPGPU, GEN9_L3_LRA_1_GPGPU_DEFAULT_VALUE_SKL);
-	else if (IS_BROXTON(dev))
+	else if (IS_BROXTON(dev_priv))
 		I915_WRITE(GEN8_L3_LRA_1_GPGPU, GEN9_L3_LRA_1_GPGPU_DEFAULT_VALUE_BXT);
 }
 
@@ -2157,6 +2230,8 @@ static int i915_ppgtt_init(struct i915_hw_ppgtt *ppgtt,
 
 int i915_ppgtt_init_hw(struct drm_device *dev)
 {
+	struct drm_i915_private *dev_priv = to_i915(dev);
+
 	gtt_write_workarounds(dev);
 
 	/* In the case of execlists, PPGTT is enabled by the context descriptor
@@ -2168,9 +2243,9 @@ int i915_ppgtt_init_hw(struct drm_device *dev)
 	if (!USES_PPGTT(dev))
 		return 0;
 
-	if (IS_GEN6(dev))
+	if (IS_GEN6(dev_priv))
 		gen6_ppgtt_enable(dev);
-	else if (IS_GEN7(dev))
+	else if (IS_GEN7(dev_priv))
 		gen7_ppgtt_enable(dev);
 	else if (INTEL_INFO(dev)->gen >= 8)
 		gen8_ppgtt_enable(dev);
@@ -2239,11 +2314,12 @@ static bool needs_idle_maps(struct drm_i915_private *dev_priv)
 void i915_check_and_clear_faults(struct drm_i915_private *dev_priv)
 {
 	struct intel_engine_cs *engine;
+	enum intel_engine_id id;
 
 	if (INTEL_INFO(dev_priv)->gen < 6)
 		return;
 
-	for_each_engine(engine, dev_priv) {
+	for_each_engine(engine, dev_priv, id) {
 		u32 fault_reg;
 		fault_reg = I915_READ(RING_FAULT_REG(engine));
 		if (fault_reg & RING_FAULT_VALID) {
@@ -2260,7 +2336,10 @@ void i915_check_and_clear_faults(struct drm_i915_private *dev_priv)
 				   fault_reg & ~RING_FAULT_VALID);
 		}
 	}
-	POSTING_READ(RING_FAULT_REG(&dev_priv->engine[RCS]));
+
+	/* Engine specific init may not have been done till this point. */
+	if (dev_priv->engine[RCS])
+		POSTING_READ(RING_FAULT_REG(dev_priv->engine[RCS]));
 }
 
 static void i915_ggtt_flush(struct drm_i915_private *dev_priv)
@@ -2286,8 +2365,7 @@ void i915_gem_suspend_gtt_mappings(struct drm_device *dev)
 
 	i915_check_and_clear_faults(dev_priv);
 
-	ggtt->base.clear_range(&ggtt->base, ggtt->base.start, ggtt->base.total,
-			     true);
+	ggtt->base.clear_range(&ggtt->base, ggtt->base.start, ggtt->base.total);
 
 	i915_ggtt_flush(dev_priv);
 }
@@ -2321,7 +2399,7 @@ static void gen8_ggtt_insert_page(struct i915_address_space *vm,
 
 	rpm_atomic_seq = assert_rpm_atomic_begin(dev_priv);
 
-	gen8_set_pte(pte, gen8_pte_encode(addr, level, true));
+	gen8_set_pte(pte, gen8_pte_encode(addr, level));
 
 	I915_WRITE(GFX_FLSH_CNTL_GEN6, GFX_FLSH_CNTL_EN);
 	POSTING_READ(GFX_FLSH_CNTL_GEN6);
@@ -2348,7 +2426,7 @@ static void gen8_ggtt_insert_entries(struct i915_address_space *vm,
 	gtt_entries = (gen8_pte_t __iomem *)ggtt->gsm + (start >> PAGE_SHIFT);
 
 	for_each_sgt_dma(addr, sgt_iter, st) {
-		gtt_entry = gen8_pte_encode(addr, level, true);
+		gtt_entry = gen8_pte_encode(addr, level);
 		gen8_set_pte(&gtt_entries[i++], gtt_entry);
 	}
 
@@ -2412,7 +2490,7 @@ static void gen6_ggtt_insert_page(struct i915_address_space *vm,
 
 	rpm_atomic_seq = assert_rpm_atomic_begin(dev_priv);
 
-	iowrite32(vm->pte_encode(addr, level, true, flags), pte);
+	iowrite32(vm->pte_encode(addr, level, flags), pte);
 
 	I915_WRITE(GFX_FLSH_CNTL_GEN6, GFX_FLSH_CNTL_EN);
 	POSTING_READ(GFX_FLSH_CNTL_GEN6);
@@ -2445,7 +2523,7 @@ static void gen6_ggtt_insert_entries(struct i915_address_space *vm,
 	gtt_entries = (gen6_pte_t __iomem *)ggtt->gsm + (start >> PAGE_SHIFT);
 
 	for_each_sgt_dma(addr, sgt_iter, st) {
-		gtt_entry = vm->pte_encode(addr, level, true, flags);
+		gtt_entry = vm->pte_encode(addr, level, flags);
 		iowrite32(gtt_entry, &gtt_entries[i++]);
 	}
 
@@ -2469,16 +2547,12 @@ static void gen6_ggtt_insert_entries(struct i915_address_space *vm,
 }
 
 static void nop_clear_range(struct i915_address_space *vm,
-			    uint64_t start,
-			    uint64_t length,
-			    bool use_scratch)
+			    uint64_t start, uint64_t length)
 {
 }
 
 static void gen8_ggtt_clear_range(struct i915_address_space *vm,
-				  uint64_t start,
-				  uint64_t length,
-				  bool use_scratch)
+				  uint64_t start, uint64_t length)
 {
 	struct drm_i915_private *dev_priv = to_i915(vm->dev);
 	struct i915_ggtt *ggtt = i915_vm_to_ggtt(vm);
@@ -2498,8 +2572,7 @@ static void gen8_ggtt_clear_range(struct i915_address_space *vm,
 		num_entries = max_entries;
 
 	scratch_pte = gen8_pte_encode(vm->scratch_page.daddr,
-				      I915_CACHE_LLC,
-				      use_scratch);
+				      I915_CACHE_LLC);
 	for (i = 0; i < num_entries; i++)
 		gen8_set_pte(&gtt_base[i], scratch_pte);
 	readl(gtt_base);
@@ -2509,8 +2582,7 @@ static void gen8_ggtt_clear_range(struct i915_address_space *vm,
 
 static void gen6_ggtt_clear_range(struct i915_address_space *vm,
 				  uint64_t start,
-				  uint64_t length,
-				  bool use_scratch)
+				  uint64_t length)
 {
 	struct drm_i915_private *dev_priv = to_i915(vm->dev);
 	struct i915_ggtt *ggtt = i915_vm_to_ggtt(vm);
@@ -2530,7 +2602,7 @@ static void gen6_ggtt_clear_range(struct i915_address_space *vm,
 		num_entries = max_entries;
 
 	scratch_pte = vm->pte_encode(vm->scratch_page.daddr,
-				     I915_CACHE_LLC, use_scratch, 0);
+				     I915_CACHE_LLC, 0);
 
 	for (i = 0; i < num_entries; i++)
 		iowrite32(scratch_pte, &gtt_base[i]);
@@ -2577,8 +2649,7 @@ static void i915_ggtt_insert_entries(struct i915_address_space *vm,
 
 static void i915_ggtt_clear_range(struct i915_address_space *vm,
 				  uint64_t start,
-				  uint64_t length,
-				  bool unused)
+				  uint64_t length)
 {
 	struct drm_i915_private *dev_priv = to_i915(vm->dev);
 	unsigned first_entry = start >> PAGE_SHIFT;
@@ -2662,13 +2733,11 @@ static void ggtt_unbind_vma(struct i915_vma *vma)
 
 	if (vma->flags & I915_VMA_GLOBAL_BIND)
 		vma->vm->clear_range(vma->vm,
-				     vma->node.start, size,
-				     true);
+				     vma->node.start, size);
 
 	if (vma->flags & I915_VMA_LOCAL_BIND && appgtt)
 		appgtt->base.clear_range(&appgtt->base,
-					 vma->node.start, size,
-					 true);
+					 vma->node.start, size);
 }
 
 void i915_gem_gtt_finish_object(struct drm_i915_gem_object *obj)
@@ -2717,6 +2786,7 @@ int i915_gem_init_ggtt(struct drm_i915_private *dev_priv)
 	 */
 	struct i915_ggtt *ggtt = &dev_priv->ggtt;
 	unsigned long hole_start, hole_end;
+	struct i915_hw_ppgtt *ppgtt;
 	struct drm_mm_node *entry;
 	int ret;
 
@@ -2724,45 +2794,48 @@ int i915_gem_init_ggtt(struct drm_i915_private *dev_priv)
 	if (ret)
 		return ret;
 
+	/* Reserve a mappable slot for our lockless error capture */
+	ret = drm_mm_insert_node_in_range_generic(&ggtt->base.mm,
+						  &ggtt->error_capture,
+						  4096, 0, -1,
+						  0, ggtt->mappable_end,
+						  0, 0);
+	if (ret)
+		return ret;
+
 	/* Clear any non-preallocated blocks */
 	drm_mm_for_each_hole(entry, &ggtt->base.mm, hole_start, hole_end) {
 		DRM_DEBUG_KMS("clearing unused GTT space: [%lx, %lx]\n",
 			      hole_start, hole_end);
 		ggtt->base.clear_range(&ggtt->base, hole_start,
-				     hole_end - hole_start, true);
+				       hole_end - hole_start);
 	}
 
 	/* And finally clear the reserved guard page */
 	ggtt->base.clear_range(&ggtt->base,
-			       ggtt->base.total - PAGE_SIZE, PAGE_SIZE,
-			       true);
+			       ggtt->base.total - PAGE_SIZE, PAGE_SIZE);
 
 	if (USES_PPGTT(dev_priv) && !USES_FULL_PPGTT(dev_priv)) {
-		struct i915_hw_ppgtt *ppgtt;
-
 		ppgtt = kzalloc(sizeof(*ppgtt), GFP_KERNEL);
-		if (!ppgtt)
-			return -ENOMEM;
+		if (!ppgtt) {
+			ret = -ENOMEM;
+			goto err;
+		}
 
 		ret = __hw_ppgtt_init(ppgtt, dev_priv);
-		if (ret) {
-			kfree(ppgtt);
-			return ret;
-		}
+		if (ret)
+			goto err_ppgtt;
 
-		if (ppgtt->base.allocate_va_range)
+		if (ppgtt->base.allocate_va_range) {
 			ret = ppgtt->base.allocate_va_range(&ppgtt->base, 0,
 							    ppgtt->base.total);
-		if (ret) {
-			ppgtt->base.cleanup(&ppgtt->base);
-			kfree(ppgtt);
-			return ret;
+			if (ret)
+				goto err_ppgtt_cleanup;
 		}
 
 		ppgtt->base.clear_range(&ppgtt->base,
 					ppgtt->base.start,
-					ppgtt->base.total,
-					true);
+					ppgtt->base.total);
 
 		dev_priv->mm.aliasing_ppgtt = ppgtt;
 		WARN_ON(ggtt->base.bind_vma != ggtt_bind_vma);
@@ -2770,6 +2843,14 @@ int i915_gem_init_ggtt(struct drm_i915_private *dev_priv)
 	}
 
 	return 0;
+
+err_ppgtt_cleanup:
+	ppgtt->base.cleanup(&ppgtt->base);
+err_ppgtt:
+	kfree(ppgtt);
+err:
+	drm_mm_remove_node(&ggtt->error_capture);
+	return ret;
 }
 
 /**
@@ -2788,6 +2869,9 @@ void i915_ggtt_cleanup_hw(struct drm_i915_private *dev_priv)
 
 	i915_gem_cleanup_stolen(&dev_priv->drm);
 
+	if (drm_mm_node_allocated(&ggtt->error_capture))
+		drm_mm_remove_node(&ggtt->error_capture);
+
 	if (drm_mm_initialized(&ggtt->base.mm)) {
 		intel_vgt_deballoon(dev_priv);
 
@@ -2895,7 +2979,7 @@ static int ggtt_probe_common(struct i915_ggtt *ggtt, u64 size)
 	 * resort to an uncached mapping. The WC issue is easily caught by the
 	 * readback check when writing GTT PTE entries.
 	 */
-	if (IS_BROXTON(ggtt->base.dev))
+	if (IS_BROXTON(to_i915(ggtt->base.dev)))
 		ggtt->gsm = ioremap_nocache(phys_addr, size);
 	else
 		ggtt->gsm = ioremap_wc(phys_addr, size);
@@ -3237,8 +3321,7 @@ void i915_gem_restore_gtt_mappings(struct drm_device *dev)
 	i915_check_and_clear_faults(dev_priv);
 
 	/* First fill our portion of the GTT with scratch pages */
-	ggtt->base.clear_range(&ggtt->base, ggtt->base.start, ggtt->base.total,
-			       true);
+	ggtt->base.clear_range(&ggtt->base, ggtt->base.start, ggtt->base.total);
 
 	ggtt->base.closed = true; /* skip rewriting PTE on VMA unbind */
 
@@ -3267,7 +3350,7 @@ void i915_gem_restore_gtt_mappings(struct drm_device *dev)
 	ggtt->base.closed = false;
 
 	if (INTEL_INFO(dev)->gen >= 8) {
-		if (IS_CHERRYVIEW(dev) || IS_BROXTON(dev))
+		if (IS_CHERRYVIEW(dev_priv) || IS_BROXTON(dev_priv))
 			chv_setup_private_ppat(dev_priv);
 		else
 			bdw_setup_private_ppat(dev_priv);
diff --git a/drivers/gpu/drm/i915/i915_gem_gtt.h b/drivers/gpu/drm/i915/i915_gem_gtt.h
index ec78be2f8c77..c241d8143255 100644
--- a/drivers/gpu/drm/i915/i915_gem_gtt.h
+++ b/drivers/gpu/drm/i915/i915_gem_gtt.h
@@ -395,7 +395,7 @@ struct i915_address_space {
 	/* FIXME: Need a more generic return type */
 	gen6_pte_t (*pte_encode)(dma_addr_t addr,
 				 enum i915_cache_level level,
-				 bool valid, u32 flags); /* Create a valid PTE */
+				 u32 flags); /* Create a valid PTE */
 	/* flags for pte_encode */
 #define PTE_READ_ONLY	(1<<0)
 	int (*allocate_va_range)(struct i915_address_space *vm,
@@ -403,8 +403,7 @@ struct i915_address_space {
 				 uint64_t length);
 	void (*clear_range)(struct i915_address_space *vm,
 			    uint64_t start,
-			    uint64_t length,
-			    bool use_scratch);
+			    uint64_t length);
 	void (*insert_page)(struct i915_address_space *vm,
 			    dma_addr_t addr,
 			    uint64_t offset,
@@ -450,6 +449,8 @@ struct i915_ggtt {
 	bool do_idle_maps;
 
 	int mtrr;
+
+	struct drm_mm_node error_capture;
 };
 
 struct i915_hw_ppgtt {
diff --git a/drivers/gpu/drm/i915/i915_gem_render_state.c b/drivers/gpu/drm/i915/i915_gem_render_state.c
index 95b7e9afd5f8..a98c0f42badd 100644
--- a/drivers/gpu/drm/i915/i915_gem_render_state.c
+++ b/drivers/gpu/drm/i915/i915_gem_render_state.c
@@ -72,9 +72,9 @@ render_state_get_rodata(const struct drm_i915_gem_request *req)
 
 static int render_state_setup(struct render_state *so)
 {
-	struct drm_device *dev = so->vma->vm->dev;
+	struct drm_i915_private *dev_priv = to_i915(so->vma->vm->dev);
 	const struct intel_renderstate_rodata *rodata = so->rodata;
-	const bool has_64bit_reloc = INTEL_GEN(dev) >= 8;
+	const bool has_64bit_reloc = INTEL_GEN(dev_priv) >= 8;
 	unsigned int i = 0, reloc_index = 0;
 	struct page *page;
 	u32 *d;
@@ -115,7 +115,7 @@ static int render_state_setup(struct render_state *so)
 
 	so->aux_batch_offset = i * sizeof(u32);
 
-	if (HAS_POOLED_EU(dev)) {
+	if (HAS_POOLED_EU(dev_priv)) {
 		/*
 		 * We always program 3x6 pool config but depending upon which
 		 * subslice is disabled HW drops down to appropriate config
diff --git a/drivers/gpu/drm/i915/i915_gem_request.c b/drivers/gpu/drm/i915/i915_gem_request.c
index 40978bc12ceb..74ede1f53372 100644
--- a/drivers/gpu/drm/i915/i915_gem_request.c
+++ b/drivers/gpu/drm/i915/i915_gem_request.c
@@ -256,10 +256,11 @@ static int i915_gem_check_wedge(struct drm_i915_private *dev_priv)
 static int i915_gem_init_seqno(struct drm_i915_private *dev_priv, u32 seqno)
 {
 	struct intel_engine_cs *engine;
+	enum intel_engine_id id;
 	int ret;
 
 	/* Carefully retire all requests without writing to the rings */
-	for_each_engine(engine, dev_priv) {
+	for_each_engine(engine, dev_priv, id) {
 		ret = intel_engine_idle(engine,
 					I915_WAIT_INTERRUPTIBLE |
 					I915_WAIT_LOCKED);
@@ -276,7 +277,7 @@ static int i915_gem_init_seqno(struct drm_i915_private *dev_priv, u32 seqno)
 	}
 
 	/* Finally reset hw state */
-	for_each_engine(engine, dev_priv)
+	for_each_engine(engine, dev_priv, id)
 		intel_engine_init_seqno(engine, seqno);
 
 	return 0;
@@ -328,6 +329,7 @@ submit_notify(struct i915_sw_fence *fence, enum i915_sw_fence_notify state)
 
 	switch (state) {
 	case FENCE_COMPLETE:
+		request->engine->last_submitted_seqno = request->fence.seqno;
 		request->engine->submit_request(request);
 		break;
 
@@ -641,8 +643,8 @@ void __i915_add_request(struct drm_i915_gem_request *request, bool flush_caches)
 					     &request->submitq);
 
 	request->emitted_jiffies = jiffies;
-	request->previous_seqno = engine->last_submitted_seqno;
-	engine->last_submitted_seqno = request->fence.seqno;
+	request->previous_seqno = engine->last_pending_seqno;
+	engine->last_pending_seqno = request->fence.seqno;
 	i915_gem_active_set(&engine->last_request, request);
 	list_add_tail(&request->link, &engine->request_list);
 	list_add_tail(&request->ring_link, &ring->request_list);
diff --git a/drivers/gpu/drm/i915/i915_gem_shrinker.c b/drivers/gpu/drm/i915/i915_gem_shrinker.c
index 1c237d02f30b..de25b6e0a101 100644
--- a/drivers/gpu/drm/i915/i915_gem_shrinker.c
+++ b/drivers/gpu/drm/i915/i915_gem_shrinker.c
@@ -182,8 +182,9 @@ i915_gem_shrink(struct drm_i915_private *dev_priv,
 			    !is_vmalloc_addr(obj->mapping))
 				continue;
 
-			if ((flags & I915_SHRINK_ACTIVE) == 0 &&
-			    i915_gem_object_is_active(obj))
+			if (!(flags & I915_SHRINK_ACTIVE) &&
+			    (i915_gem_object_is_active(obj) ||
+			     obj->framebuffer_references))
 				continue;
 
 			if (!can_release_pages(obj))
diff --git a/drivers/gpu/drm/i915/i915_gem_stolen.c b/drivers/gpu/drm/i915/i915_gem_stolen.c
index 59989e8ee5dc..f4f6d3a48b05 100644
--- a/drivers/gpu/drm/i915/i915_gem_stolen.c
+++ b/drivers/gpu/drm/i915/i915_gem_stolen.c
@@ -115,7 +115,7 @@ static unsigned long i915_stolen_to_physical(struct drm_device *dev)
 		pci_read_config_dword(pdev, INTEL_BSM, &bsm);
 
 		base = bsm & INTEL_BSM_MASK;
-	} else if (IS_I865G(dev)) {
+	} else if (IS_I865G(dev_priv)) {
 		u32 tseg_size = 0;
 		u16 toud = 0;
 		u8 tmp;
@@ -154,7 +154,7 @@ static unsigned long i915_stolen_to_physical(struct drm_device *dev)
 		tom = tmp * MB(32);
 
 		base = tom - tseg_size - ggtt->stolen_size;
-	} else if (IS_845G(dev)) {
+	} else if (IS_845G(dev_priv)) {
 		u32 tseg_size = 0;
 		u32 tom;
 		u8 tmp;
@@ -178,7 +178,7 @@ static unsigned long i915_stolen_to_physical(struct drm_device *dev)
 		tom = tmp * MB(32);
 
 		base = tom - tseg_size - ggtt->stolen_size;
-	} else if (IS_I830(dev)) {
+	} else if (IS_I830(dev_priv)) {
 		u32 tseg_size = 0;
 		u32 tom;
 		u8 tmp;
@@ -204,7 +204,8 @@ static unsigned long i915_stolen_to_physical(struct drm_device *dev)
 		return 0;
 
 	/* make sure we don't clobber the GTT if it's within stolen memory */
-	if (INTEL_INFO(dev)->gen <= 4 && !IS_G33(dev) && !IS_G4X(dev)) {
+	if (INTEL_GEN(dev_priv) <= 4 && !IS_G33(dev_priv) &&
+	    !IS_G4X(dev_priv)) {
 		struct {
 			u32 start, end;
 		} stolen[2] = {
@@ -214,7 +215,7 @@ static unsigned long i915_stolen_to_physical(struct drm_device *dev)
 		u64 ggtt_start, ggtt_end;
 
 		ggtt_start = I915_READ(PGTBL_CTL);
-		if (IS_GEN4(dev))
+		if (IS_GEN4(dev_priv))
 			ggtt_start = (ggtt_start & PGTBL_ADDRESS_LO_MASK) |
 				     (ggtt_start & PGTBL_ADDRESS_HI_MASK) << 28;
 		else
@@ -270,7 +271,7 @@ static unsigned long i915_stolen_to_physical(struct drm_device *dev)
 		 * GEN3 firmware likes to smash pci bridges into the stolen
 		 * range. Apparently this works.
 		 */
-		if (r == NULL && !IS_GEN3(dev)) {
+		if (r == NULL && !IS_GEN3(dev_priv)) {
 			DRM_ERROR("conflict detected with stolen region: [0x%08x - 0x%08x]\n",
 				  base, base + (uint32_t)ggtt->stolen_size);
 			base = 0;
@@ -437,7 +438,7 @@ int i915_gem_init_stolen(struct drm_device *dev)
 	case 3:
 		break;
 	case 4:
-		if (IS_G4X(dev))
+		if (IS_G4X(dev_priv))
 			g4x_get_stolen_reserved(dev_priv, &reserved_base,
 						&reserved_size);
 		break;
@@ -456,7 +457,7 @@ int i915_gem_init_stolen(struct drm_device *dev)
 		break;
 	default:
 		if (IS_BROADWELL(dev_priv) ||
-		    IS_SKYLAKE(dev_priv) || IS_KABYLAKE(dev))
+		    IS_SKYLAKE(dev_priv) || IS_KABYLAKE(dev_priv))
 			bdw_get_stolen_reserved(dev_priv, &reserved_base,
 						&reserved_size);
 		else
diff --git a/drivers/gpu/drm/i915/i915_gem_tiling.c b/drivers/gpu/drm/i915/i915_gem_tiling.c
index a14b1e3d4c78..c21bc0068d20 100644
--- a/drivers/gpu/drm/i915/i915_gem_tiling.c
+++ b/drivers/gpu/drm/i915/i915_gem_tiling.c
@@ -62,6 +62,7 @@
 static bool
 i915_tiling_ok(struct drm_device *dev, int stride, int size, int tiling_mode)
 {
+	struct drm_i915_private *dev_priv = to_i915(dev);
 	int tile_width;
 
 	/* Linear is always fine */
@@ -71,8 +72,8 @@ i915_tiling_ok(struct drm_device *dev, int stride, int size, int tiling_mode)
 	if (tiling_mode > I915_TILING_LAST)
 		return false;
 
-	if (IS_GEN2(dev) ||
-	    (tiling_mode == I915_TILING_Y && HAS_128_BYTE_Y_TILING(dev)))
+	if (IS_GEN2(dev_priv) ||
+	    (tiling_mode == I915_TILING_Y && HAS_128_BYTE_Y_TILING(dev_priv)))
 		tile_width = 128;
 	else
 		tile_width = 512;
@@ -90,7 +91,7 @@ i915_tiling_ok(struct drm_device *dev, int stride, int size, int tiling_mode)
 		if (stride > 8192)
 			return false;
 
-		if (IS_GEN3(dev)) {
+		if (IS_GEN3(dev_priv)) {
 			if (size > I830_FENCE_MAX_SIZE_VAL << 20)
 				return false;
 		} else {
diff --git a/drivers/gpu/drm/i915/i915_gem_userptr.c b/drivers/gpu/drm/i915/i915_gem_userptr.c
index e537930c64b5..c6f780f5abc9 100644
--- a/drivers/gpu/drm/i915/i915_gem_userptr.c
+++ b/drivers/gpu/drm/i915/i915_gem_userptr.c
@@ -508,6 +508,10 @@ __i915_gem_userptr_get_pages_worker(struct work_struct *_work)
 	pvec = drm_malloc_gfp(npages, sizeof(struct page *), GFP_TEMPORARY);
 	if (pvec != NULL) {
 		struct mm_struct *mm = obj->userptr.mm->mm;
+		unsigned int flags = 0;
+
+		if (!obj->userptr.read_only)
+			flags |= FOLL_WRITE;
 
 		ret = -EFAULT;
 		if (atomic_inc_not_zero(&mm->mm_users)) {
@@ -517,7 +521,7 @@ __i915_gem_userptr_get_pages_worker(struct work_struct *_work)
 					(work->task, mm,
 					 obj->userptr.ptr + pinned * PAGE_SIZE,
 					 npages - pinned,
-					 !obj->userptr.read_only, 0,
+					 flags,
 					 pvec + pinned, NULL);
 				if (ret < 0)
 					break;
diff --git a/drivers/gpu/drm/i915/i915_gpu_error.c b/drivers/gpu/drm/i915/i915_gpu_error.c
index 334f15df7c8d..242b9a927899 100644
--- a/drivers/gpu/drm/i915/i915_gpu_error.c
+++ b/drivers/gpu/drm/i915/i915_gpu_error.c
@@ -28,6 +28,8 @@
  */
 
 #include <generated/utsrelease.h>
+#include <linux/stop_machine.h>
+#include <linux/zlib.h>
 #include "i915_drv.h"
 
 static const char *engine_str(int engine)
@@ -172,6 +174,110 @@ static void i915_error_puts(struct drm_i915_error_state_buf *e,
 #define err_printf(e, ...) i915_error_printf(e, __VA_ARGS__)
 #define err_puts(e, s) i915_error_puts(e, s)
 
+#ifdef CONFIG_DRM_I915_COMPRESS_ERROR
+
+static bool compress_init(struct z_stream_s *zstream)
+{
+	memset(zstream, 0, sizeof(*zstream));
+
+	zstream->workspace =
+		kmalloc(zlib_deflate_workspacesize(MAX_WBITS, MAX_MEM_LEVEL),
+			GFP_ATOMIC | __GFP_NOWARN);
+	if (!zstream->workspace)
+		return false;
+
+	if (zlib_deflateInit(zstream, Z_DEFAULT_COMPRESSION) != Z_OK) {
+		kfree(zstream->workspace);
+		return false;
+	}
+
+	return true;
+}
+
+static int compress_page(struct z_stream_s *zstream,
+			 void *src,
+			 struct drm_i915_error_object *dst)
+{
+	zstream->next_in = src;
+	zstream->avail_in = PAGE_SIZE;
+
+	do {
+		if (zstream->avail_out == 0) {
+			unsigned long page;
+
+			page = __get_free_page(GFP_ATOMIC | __GFP_NOWARN);
+			if (!page)
+				return -ENOMEM;
+
+			dst->pages[dst->page_count++] = (void *)page;
+
+			zstream->next_out = (void *)page;
+			zstream->avail_out = PAGE_SIZE;
+		}
+
+		if (zlib_deflate(zstream, Z_SYNC_FLUSH) != Z_OK)
+			return -EIO;
+	} while (zstream->avail_in);
+
+	/* Fallback to uncompressed if we increase size? */
+	if (0 && zstream->total_out > zstream->total_in)
+		return -E2BIG;
+
+	return 0;
+}
+
+static void compress_fini(struct z_stream_s *zstream,
+			  struct drm_i915_error_object *dst)
+{
+	if (dst) {
+		zlib_deflate(zstream, Z_FINISH);
+		dst->unused = zstream->avail_out;
+	}
+
+	zlib_deflateEnd(zstream);
+	kfree(zstream->workspace);
+}
+
+static void err_compression_marker(struct drm_i915_error_state_buf *m)
+{
+	err_puts(m, ":");
+}
+
+#else
+
+static bool compress_init(struct z_stream_s *zstream)
+{
+	return true;
+}
+
+static int compress_page(struct z_stream_s *zstream,
+			 void *src,
+			 struct drm_i915_error_object *dst)
+{
+	unsigned long page;
+
+	page = __get_free_page(GFP_ATOMIC | __GFP_NOWARN);
+	if (!page)
+		return -ENOMEM;
+
+	dst->pages[dst->page_count++] =
+		memcpy((void *)page, src, PAGE_SIZE);
+
+	return 0;
+}
+
+static void compress_fini(struct z_stream_s *zstream,
+			  struct drm_i915_error_object *dst)
+{
+}
+
+static void err_compression_marker(struct drm_i915_error_state_buf *m)
+{
+	err_puts(m, "~");
+}
+
+#endif
+
 static void print_error_buffers(struct drm_i915_error_state_buf *m,
 				const char *name,
 				struct drm_i915_error_buffer *err,
@@ -228,13 +334,57 @@ static const char *hangcheck_action_to_str(enum intel_engine_hangcheck_action a)
 	return "unknown";
 }
 
+static void error_print_instdone(struct drm_i915_error_state_buf *m,
+				 struct drm_i915_error_engine *ee)
+{
+	int slice;
+	int subslice;
+
+	err_printf(m, "  INSTDONE: 0x%08x\n",
+		   ee->instdone.instdone);
+
+	if (ee->engine_id != RCS || INTEL_GEN(m->i915) <= 3)
+		return;
+
+	err_printf(m, "  SC_INSTDONE: 0x%08x\n",
+		   ee->instdone.slice_common);
+
+	if (INTEL_GEN(m->i915) <= 6)
+		return;
+
+	for_each_instdone_slice_subslice(m->i915, slice, subslice)
+		err_printf(m, "  SAMPLER_INSTDONE[%d][%d]: 0x%08x\n",
+			   slice, subslice,
+			   ee->instdone.sampler[slice][subslice]);
+
+	for_each_instdone_slice_subslice(m->i915, slice, subslice)
+		err_printf(m, "  ROW_INSTDONE[%d][%d]: 0x%08x\n",
+			   slice, subslice,
+			   ee->instdone.row[slice][subslice]);
+}
+
+static void error_print_request(struct drm_i915_error_state_buf *m,
+				const char *prefix,
+				struct drm_i915_error_request *erq)
+{
+	if (!erq->seqno)
+		return;
+
+	err_printf(m, "%s pid %d, seqno %8x:%08x, emitted %dms ago, head %08x, tail %08x\n",
+		   prefix, erq->pid,
+		   erq->context, erq->seqno,
+		   jiffies_to_msecs(jiffies - erq->jiffies),
+		   erq->head, erq->tail);
+}
+
 static void error_print_engine(struct drm_i915_error_state_buf *m,
 			       struct drm_i915_error_engine *ee)
 {
 	err_printf(m, "%s command stream:\n", engine_str(ee->engine_id));
 	err_printf(m, "  START: 0x%08x\n", ee->start);
-	err_printf(m, "  HEAD:  0x%08x\n", ee->head);
-	err_printf(m, "  TAIL:  0x%08x\n", ee->tail);
+	err_printf(m, "  HEAD:  0x%08x [0x%08x]\n", ee->head, ee->rq_head);
+	err_printf(m, "  TAIL:  0x%08x [0x%08x, 0x%08x]\n",
+		   ee->tail, ee->rq_post, ee->rq_tail);
 	err_printf(m, "  CTL:   0x%08x\n", ee->ctl);
 	err_printf(m, "  MODE:  0x%08x\n", ee->mode);
 	err_printf(m, "  HWS:   0x%08x\n", ee->hws);
@@ -242,7 +392,9 @@ static void error_print_engine(struct drm_i915_error_state_buf *m,
 		   (u32)(ee->acthd>>32), (u32)ee->acthd);
 	err_printf(m, "  IPEIR: 0x%08x\n", ee->ipeir);
 	err_printf(m, "  IPEHR: 0x%08x\n", ee->ipehr);
-	err_printf(m, "  INSTDONE: 0x%08x\n", ee->instdone);
+
+	error_print_instdone(m, ee);
+
 	if (ee->batchbuffer) {
 		u64 start = ee->batchbuffer->gtt_offset;
 		u64 end = start + ee->batchbuffer->gtt_size;
@@ -296,6 +448,8 @@ static void error_print_engine(struct drm_i915_error_state_buf *m,
 	err_printf(m, "  hangcheck: %s [%d]\n",
 		   hangcheck_action_to_str(ee->hangcheck_action),
 		   ee->hangcheck_score);
+	error_print_request(m, "  ELSP[0]: ", &ee->execlist[0]);
+	error_print_request(m, "  ELSP[1]: ", &ee->execlist[1]);
 }
 
 void i915_error_printf(struct drm_i915_error_state_buf *e, const char *f, ...)
@@ -307,28 +461,72 @@ void i915_error_printf(struct drm_i915_error_state_buf *e, const char *f, ...)
 	va_end(args);
 }
 
+static int
+ascii85_encode_len(int len)
+{
+	return DIV_ROUND_UP(len, 4);
+}
+
+static bool
+ascii85_encode(u32 in, char *out)
+{
+	int i;
+
+	if (in == 0)
+		return false;
+
+	out[5] = '\0';
+	for (i = 5; i--; ) {
+		out[i] = '!' + in % 85;
+		in /= 85;
+	}
+
+	return true;
+}
+
 static void print_error_obj(struct drm_i915_error_state_buf *m,
+			    struct intel_engine_cs *engine,
+			    const char *name,
 			    struct drm_i915_error_object *obj)
 {
-	int page, offset, elt;
+	char out[6];
+	int page;
+
+	if (!obj)
+		return;
+
+	if (name) {
+		err_printf(m, "%s --- %s = 0x%08x %08x\n",
+			   engine ? engine->name : "global", name,
+			   upper_32_bits(obj->gtt_offset),
+			   lower_32_bits(obj->gtt_offset));
+	}
+
+	err_compression_marker(m);
+	for (page = 0; page < obj->page_count; page++) {
+		int i, len;
+
+		len = PAGE_SIZE;
+		if (page == obj->page_count - 1)
+			len -= obj->unused;
+		len = ascii85_encode_len(len);
 
-	for (page = offset = 0; page < obj->page_count; page++) {
-		for (elt = 0; elt < PAGE_SIZE/4; elt++) {
-			err_printf(m, "%08x :  %08x\n", offset,
-				   obj->pages[page][elt]);
-			offset += 4;
+		for (i = 0; i < len; i++) {
+			if (ascii85_encode(obj->pages[page][i], out))
+				err_puts(m, out);
+			else
+				err_puts(m, "z");
 		}
 	}
+	err_puts(m, "\n");
 }
 
 static void err_print_capabilities(struct drm_i915_error_state_buf *m,
 				   const struct intel_device_info *info)
 {
 #define PRINT_FLAG(x)  err_printf(m, #x ": %s\n", yesno(info->x))
-#define SEP_SEMICOLON ;
-	DEV_INFO_FOR_EACH_FLAG(PRINT_FLAG, SEP_SEMICOLON);
+	DEV_INFO_FOR_EACH_FLAG(PRINT_FLAG);
 #undef PRINT_FLAG
-#undef SEP_SEMICOLON
 }
 
 int i915_error_state_to_str(struct drm_i915_error_state_buf *m,
@@ -339,8 +537,8 @@ int i915_error_state_to_str(struct drm_i915_error_state_buf *m,
 	struct pci_dev *pdev = dev_priv->drm.pdev;
 	struct drm_i915_error_state *error = error_priv->error;
 	struct drm_i915_error_object *obj;
-	int i, j, offset, elt;
 	int max_hangcheck_score;
+	int i, j;
 
 	if (!error) {
 		err_printf(m, "no error state collected\n");
@@ -391,7 +589,7 @@ int i915_error_state_to_str(struct drm_i915_error_state_buf *m,
 		for (i = 0; i < 4; i++)
 			err_printf(m, "GTIER gt %d: 0x%08x\n", i,
 				   error->gtier[i]);
-	} else if (HAS_PCH_SPLIT(dev) || IS_VALLEYVIEW(dev))
+	} else if (HAS_PCH_SPLIT(dev_priv) || IS_VALLEYVIEW(dev_priv))
 		err_printf(m, "GTIER: 0x%08x\n", error->gtier[0]);
 	err_printf(m, "PGTBL_ER: 0x%08x\n", error->pgtbl_er);
 	err_printf(m, "FORCEWAKE: 0x%08x\n", error->forcewake);
@@ -402,10 +600,6 @@ int i915_error_state_to_str(struct drm_i915_error_state_buf *m,
 	for (i = 0; i < dev_priv->num_fence_regs; i++)
 		err_printf(m, "  fence[%d] = %08llx\n", i, error->fence[i]);
 
-	for (i = 0; i < ARRAY_SIZE(error->extra_instdone); i++)
-		err_printf(m, "  INSTDONE_%d: 0x%08x\n", i,
-			   error->extra_instdone[i]);
-
 	if (INTEL_INFO(dev)->gen >= 6) {
 		err_printf(m, "ERROR: 0x%08x\n", error->error);
 
@@ -416,7 +610,7 @@ int i915_error_state_to_str(struct drm_i915_error_state_buf *m,
 		err_printf(m, "DONE_REG: 0x%08x\n", error->done_reg);
 	}
 
-	if (IS_GEN7(dev))
+	if (IS_GEN7(dev_priv))
 		err_printf(m, "ERR_INT: 0x%08x\n", error->err_int);
 
 	for (i = 0; i < ARRAY_SIZE(error->engine); i++) {
@@ -438,7 +632,7 @@ int i915_error_state_to_str(struct drm_i915_error_state_buf *m,
 
 			len += scnprintf(buf + len, sizeof(buf), "%s%s",
 					 first ? "" : ", ",
-					 dev_priv->engine[j].name);
+					 dev_priv->engine[j]->name);
 			first = 0;
 		}
 		scnprintf(buf + len, sizeof(buf), ")");
@@ -456,7 +650,7 @@ int i915_error_state_to_str(struct drm_i915_error_state_buf *m,
 
 		obj = ee->batchbuffer;
 		if (obj) {
-			err_puts(m, dev_priv->engine[i].name);
+			err_puts(m, dev_priv->engine[i]->name);
 			if (ee->pid != -1)
 				err_printf(m, " (submitted by %s [%d])",
 					   ee->comm,
@@ -464,37 +658,23 @@ int i915_error_state_to_str(struct drm_i915_error_state_buf *m,
 			err_printf(m, " --- gtt_offset = 0x%08x %08x\n",
 				   upper_32_bits(obj->gtt_offset),
 				   lower_32_bits(obj->gtt_offset));
-			print_error_obj(m, obj);
-		}
-
-		obj = ee->wa_batchbuffer;
-		if (obj) {
-			err_printf(m, "%s (w/a) --- gtt_offset = 0x%08x\n",
-				   dev_priv->engine[i].name,
-				   lower_32_bits(obj->gtt_offset));
-			print_error_obj(m, obj);
+			print_error_obj(m, dev_priv->engine[i], NULL, obj);
 		}
 
 		if (ee->num_requests) {
 			err_printf(m, "%s --- %d requests\n",
-				   dev_priv->engine[i].name,
+				   dev_priv->engine[i]->name,
 				   ee->num_requests);
-			for (j = 0; j < ee->num_requests; j++) {
-				err_printf(m, "  pid %d, seqno 0x%08x, emitted %ld, head 0x%08x, tail 0x%08x\n",
-					   ee->requests[j].pid,
-					   ee->requests[j].seqno,
-					   ee->requests[j].jiffies,
-					   ee->requests[j].head,
-					   ee->requests[j].tail);
-			}
+			for (j = 0; j < ee->num_requests; j++)
+				error_print_request(m, " ", &ee->requests[j]);
 		}
 
 		if (IS_ERR(ee->waiters)) {
 			err_printf(m, "%s --- ? waiters [unable to acquire spinlock]\n",
-				   dev_priv->engine[i].name);
+				   dev_priv->engine[i]->name);
 		} else if (ee->num_waiters) {
 			err_printf(m, "%s --- %d waiters\n",
-				   dev_priv->engine[i].name,
+				   dev_priv->engine[i]->name,
 				   ee->num_waiters);
 			for (j = 0; j < ee->num_waiters; j++) {
 				err_printf(m, " seqno 0x%08x for %s [%d]\n",
@@ -504,77 +684,23 @@ int i915_error_state_to_str(struct drm_i915_error_state_buf *m,
 			}
 		}
 
-		if ((obj = ee->ringbuffer)) {
-			err_printf(m, "%s --- ringbuffer = 0x%08x\n",
-				   dev_priv->engine[i].name,
-				   lower_32_bits(obj->gtt_offset));
-			print_error_obj(m, obj);
-		}
+		print_error_obj(m, dev_priv->engine[i],
+				"ringbuffer", ee->ringbuffer);
 
-		if ((obj = ee->hws_page)) {
-			u64 hws_offset = obj->gtt_offset;
-			u32 *hws_page = &obj->pages[0][0];
+		print_error_obj(m, dev_priv->engine[i],
+				"HW Status", ee->hws_page);
 
-			if (i915.enable_execlists) {
-				hws_offset += LRC_PPHWSP_PN * PAGE_SIZE;
-				hws_page = &obj->pages[LRC_PPHWSP_PN][0];
-			}
-			err_printf(m, "%s --- HW Status = 0x%08llx\n",
-				   dev_priv->engine[i].name, hws_offset);
-			offset = 0;
-			for (elt = 0; elt < PAGE_SIZE/16; elt += 4) {
-				err_printf(m, "[%04x] %08x %08x %08x %08x\n",
-					   offset,
-					   hws_page[elt],
-					   hws_page[elt+1],
-					   hws_page[elt+2],
-					   hws_page[elt+3]);
-				offset += 16;
-			}
-		}
+		print_error_obj(m, dev_priv->engine[i],
+				"HW context", ee->ctx);
 
-		obj = ee->wa_ctx;
-		if (obj) {
-			u64 wa_ctx_offset = obj->gtt_offset;
-			u32 *wa_ctx_page = &obj->pages[0][0];
-			struct intel_engine_cs *engine = &dev_priv->engine[RCS];
-			u32 wa_ctx_size = (engine->wa_ctx.indirect_ctx.size +
-					   engine->wa_ctx.per_ctx.size);
-
-			err_printf(m, "%s --- WA ctx batch buffer = 0x%08llx\n",
-				   dev_priv->engine[i].name, wa_ctx_offset);
-			offset = 0;
-			for (elt = 0; elt < wa_ctx_size; elt += 4) {
-				err_printf(m, "[%04x] %08x %08x %08x %08x\n",
-					   offset,
-					   wa_ctx_page[elt + 0],
-					   wa_ctx_page[elt + 1],
-					   wa_ctx_page[elt + 2],
-					   wa_ctx_page[elt + 3]);
-				offset += 16;
-			}
-		}
+		print_error_obj(m, dev_priv->engine[i],
+				"WA context", ee->wa_ctx);
 
-		if ((obj = ee->ctx)) {
-			err_printf(m, "%s --- HW Context = 0x%08x\n",
-				   dev_priv->engine[i].name,
-				   lower_32_bits(obj->gtt_offset));
-			print_error_obj(m, obj);
-		}
+		print_error_obj(m, dev_priv->engine[i],
+				"WA batchbuffer", ee->wa_batchbuffer);
 	}
 
-	if ((obj = error->semaphore)) {
-		err_printf(m, "Semaphore page = 0x%08x\n",
-			   lower_32_bits(obj->gtt_offset));
-		for (elt = 0; elt < PAGE_SIZE/16; elt += 4) {
-			err_printf(m, "[%04x] %08x %08x %08x %08x\n",
-				   elt * 4,
-				   obj->pages[0][elt],
-				   obj->pages[0][elt+1],
-				   obj->pages[0][elt+2],
-				   obj->pages[0][elt+3]);
-		}
-	}
+	print_error_obj(m, NULL, "Semaphores", error->semaphore);
 
 	if (error->overlay)
 		intel_overlay_print_error_state(m, error->overlay);
@@ -629,7 +755,7 @@ static void i915_error_object_free(struct drm_i915_error_object *obj)
 		return;
 
 	for (page = 0; page < obj->page_count; page++)
-		kfree(obj->pages[page]);
+		free_page((unsigned long)obj->pages[page]);
 
 	kfree(obj);
 }
@@ -667,104 +793,63 @@ static void i915_error_state_free(struct kref *error_ref)
 }
 
 static struct drm_i915_error_object *
-i915_error_object_create(struct drm_i915_private *dev_priv,
+i915_error_object_create(struct drm_i915_private *i915,
 			 struct i915_vma *vma)
 {
-	struct i915_ggtt *ggtt = &dev_priv->ggtt;
-	struct drm_i915_gem_object *src;
+	struct i915_ggtt *ggtt = &i915->ggtt;
+	const u64 slot = ggtt->error_capture.start;
 	struct drm_i915_error_object *dst;
-	int num_pages;
-	bool use_ggtt;
-	int i = 0;
-	u64 reloc_offset;
+	struct z_stream_s zstream;
+	unsigned long num_pages;
+	struct sgt_iter iter;
+	dma_addr_t dma;
 
 	if (!vma)
 		return NULL;
 
-	src = vma->obj;
-	if (!src->pages)
-		return NULL;
-
-	num_pages = src->base.size >> PAGE_SHIFT;
-
-	dst = kmalloc(sizeof(*dst) + num_pages * sizeof(u32 *), GFP_ATOMIC);
+	num_pages = min_t(u64, vma->size, vma->obj->base.size) >> PAGE_SHIFT;
+	num_pages = DIV_ROUND_UP(10 * num_pages, 8); /* worstcase zlib growth */
+	dst = kmalloc(sizeof(*dst) + num_pages * sizeof(u32 *),
+		      GFP_ATOMIC | __GFP_NOWARN);
 	if (!dst)
 		return NULL;
 
 	dst->gtt_offset = vma->node.start;
 	dst->gtt_size = vma->node.size;
+	dst->page_count = 0;
+	dst->unused = 0;
 
-	reloc_offset = dst->gtt_offset;
-	use_ggtt = (src->cache_level == I915_CACHE_NONE &&
-		   (vma->flags & I915_VMA_GLOBAL_BIND) &&
-		   reloc_offset + num_pages * PAGE_SIZE <= ggtt->mappable_end);
-
-	/* Cannot access stolen address directly, try to use the aperture */
-	if (src->stolen) {
-		use_ggtt = true;
-
-		if (!(vma->flags & I915_VMA_GLOBAL_BIND))
-			goto unwind;
-
-		reloc_offset = vma->node.start;
-		if (reloc_offset + num_pages * PAGE_SIZE > ggtt->mappable_end)
-			goto unwind;
+	if (!compress_init(&zstream)) {
+		kfree(dst);
+		return NULL;
 	}
 
-	/* Cannot access snooped pages through the aperture */
-	if (use_ggtt && src->cache_level != I915_CACHE_NONE &&
-	    !HAS_LLC(dev_priv))
-		goto unwind;
+	for_each_sgt_dma(dma, iter, vma->pages) {
+		void __iomem *s;
+		int ret;
 
-	dst->page_count = num_pages;
-	while (num_pages--) {
-		unsigned long flags;
-		void *d;
+		ggtt->base.insert_page(&ggtt->base, dma, slot,
+				       I915_CACHE_NONE, 0);
 
-		d = kmalloc(PAGE_SIZE, GFP_ATOMIC);
-		if (d == NULL)
-			goto unwind;
-
-		local_irq_save(flags);
-		if (use_ggtt) {
-			void __iomem *s;
-
-			/* Simply ignore tiling or any overlapping fence.
-			 * It's part of the error state, and this hopefully
-			 * captures what the GPU read.
-			 */
-
-			s = io_mapping_map_atomic_wc(&ggtt->mappable,
-						     reloc_offset);
-			memcpy_fromio(d, s, PAGE_SIZE);
-			io_mapping_unmap_atomic(s);
-		} else {
-			struct page *page;
-			void *s;
-
-			page = i915_gem_object_get_page(src, i);
-
-			drm_clflush_pages(&page, 1);
-
-			s = kmap_atomic(page);
-			memcpy(d, s, PAGE_SIZE);
-			kunmap_atomic(s);
-
-			drm_clflush_pages(&page, 1);
-		}
-		local_irq_restore(flags);
+		s = io_mapping_map_atomic_wc(&ggtt->mappable, slot);
+		ret = compress_page(&zstream, (void  __force *)s, dst);
+		io_mapping_unmap_atomic(s);
 
-		dst->pages[i++] = d;
-		reloc_offset += PAGE_SIZE;
+		if (ret)
+			goto unwind;
 	}
-
-	return dst;
+	goto out;
 
 unwind:
-	while (i--)
-		kfree(dst->pages[i]);
+	while (dst->page_count--)
+		free_page((unsigned long)dst->pages[dst->page_count]);
 	kfree(dst);
-	return NULL;
+	dst = NULL;
+
+out:
+	compress_fini(&zstream, dst);
+	ggtt->base.clear_range(&ggtt->base, slot, PAGE_SIZE);
+	return dst;
 }
 
 /* The error capture is special as tries to run underneath the normal
@@ -855,7 +940,8 @@ static uint32_t i915_error_generate_code(struct drm_i915_private *dev_priv,
 			if (engine_id)
 				*engine_id = i;
 
-			return error->engine[i].ipehr ^ error->engine[i].instdone;
+			return error->engine[i].ipehr ^
+			       error->engine[i].instdone.instdone;
 		}
 	}
 
@@ -891,7 +977,7 @@ static void gen8_record_semaphore_state(struct drm_i915_error_state *error,
 	if (!error->semaphore)
 		return;
 
-	for_each_engine_id(to, dev_priv, id) {
+	for_each_engine(to, dev_priv, id) {
 		int idx;
 		u16 signal_offset;
 		u32 *tmp;
@@ -998,7 +1084,6 @@ static void error_record_engine_registers(struct drm_i915_error_state *error,
 		ee->faddr = I915_READ(RING_DMA_FADD(engine->mmio_base));
 		ee->ipeir = I915_READ(RING_IPEIR(engine->mmio_base));
 		ee->ipehr = I915_READ(RING_IPEHR(engine->mmio_base));
-		ee->instdone = I915_READ(RING_INSTDONE(engine->mmio_base));
 		ee->instps = I915_READ(RING_INSTPS(engine->mmio_base));
 		ee->bbaddr = I915_READ(RING_BBADDR(engine->mmio_base));
 		if (INTEL_GEN(dev_priv) >= 8) {
@@ -1010,9 +1095,10 @@ static void error_record_engine_registers(struct drm_i915_error_state *error,
 		ee->faddr = I915_READ(DMA_FADD_I8XX);
 		ee->ipeir = I915_READ(IPEIR);
 		ee->ipehr = I915_READ(IPEHR);
-		ee->instdone = I915_READ(GEN2_INSTDONE);
 	}
 
+	intel_engine_get_instdone(engine, &ee->instdone);
+
 	ee->waiting = intel_engine_has_waiter(engine);
 	ee->instpm = I915_READ(RING_INSTPM(engine->mmio_base));
 	ee->acthd = intel_engine_get_active_head(engine);
@@ -1079,6 +1165,20 @@ static void error_record_engine_registers(struct drm_i915_error_state *error,
 	}
 }
 
+static void record_request(struct drm_i915_gem_request *request,
+			   struct drm_i915_error_request *erq)
+{
+	erq->context = request->ctx->hw_id;
+	erq->seqno = request->fence.seqno;
+	erq->jiffies = request->emitted_jiffies;
+	erq->head = request->head;
+	erq->tail = request->tail;
+
+	rcu_read_lock();
+	erq->pid = request->ctx->pid ? pid_nr(request->ctx->pid) : 0;
+	rcu_read_unlock();
+}
+
 static void engine_record_requests(struct intel_engine_cs *engine,
 				   struct drm_i915_gem_request *first,
 				   struct drm_i915_error_engine *ee)
@@ -1102,8 +1202,6 @@ static void engine_record_requests(struct intel_engine_cs *engine,
 	count = 0;
 	request = first;
 	list_for_each_entry_from(request, &engine->request_list, link) {
-		struct drm_i915_error_request *erq;
-
 		if (count >= ee->num_requests) {
 			/*
 			 * If the ring request list was changed in
@@ -1123,19 +1221,22 @@ static void engine_record_requests(struct intel_engine_cs *engine,
 			break;
 		}
 
-		erq = &ee->requests[count++];
-		erq->seqno = request->fence.seqno;
-		erq->jiffies = request->emitted_jiffies;
-		erq->head = request->head;
-		erq->tail = request->tail;
-
-		rcu_read_lock();
-		erq->pid = request->ctx->pid ? pid_nr(request->ctx->pid) : 0;
-		rcu_read_unlock();
+		record_request(request, &ee->requests[count++]);
 	}
 	ee->num_requests = count;
 }
 
+static void error_record_engine_execlists(struct intel_engine_cs *engine,
+					  struct drm_i915_error_engine *ee)
+{
+	unsigned int n;
+
+	for (n = 0; n < ARRAY_SIZE(engine->execlist_port); n++)
+		if (engine->execlist_port[n].request)
+			record_request(engine->execlist_port[n].request,
+				       &ee->execlist[n]);
+}
+
 static void i915_gem_record_rings(struct drm_i915_private *dev_priv,
 				  struct drm_i915_error_state *error)
 {
@@ -1146,20 +1247,21 @@ static void i915_gem_record_rings(struct drm_i915_private *dev_priv,
 		i915_error_object_create(dev_priv, dev_priv->semaphore);
 
 	for (i = 0; i < I915_NUM_ENGINES; i++) {
-		struct intel_engine_cs *engine = &dev_priv->engine[i];
+		struct intel_engine_cs *engine = dev_priv->engine[i];
 		struct drm_i915_error_engine *ee = &error->engine[i];
 		struct drm_i915_gem_request *request;
 
 		ee->pid = -1;
 		ee->engine_id = -1;
 
-		if (!intel_engine_initialized(engine))
+		if (!engine)
 			continue;
 
 		ee->engine_id = i;
 
 		error_record_engine_registers(error, engine, ee);
 		error_record_engine_waiters(engine, ee);
+		error_record_engine_execlists(engine, ee);
 
 		request = i915_gem_find_active_request(engine);
 		if (request) {
@@ -1202,6 +1304,10 @@ static void i915_gem_record_rings(struct drm_i915_private *dev_priv,
 			error->simulated |=
 				request->ctx->flags & CONTEXT_NO_ERROR_CAPTURE;
 
+			ee->rq_head = request->head;
+			ee->rq_post = request->postfix;
+			ee->rq_tail = request->tail;
+
 			ring = request->ring;
 			ee->cpu_ring_head = ring->head;
 			ee->cpu_ring_tail = ring->tail;
@@ -1318,13 +1424,13 @@ static void i915_capture_reg_state(struct drm_i915_private *dev_priv,
 	 */
 
 	/* 1: Registers specific to a single generation */
-	if (IS_VALLEYVIEW(dev)) {
+	if (IS_VALLEYVIEW(dev_priv)) {
 		error->gtier[0] = I915_READ(GTIER);
 		error->ier = I915_READ(VLV_IER);
 		error->forcewake = I915_READ_FW(FORCEWAKE_VLV);
 	}
 
-	if (IS_GEN7(dev))
+	if (IS_GEN7(dev_priv))
 		error->err_int = I915_READ(GEN7_ERR_INT);
 
 	if (INTEL_INFO(dev)->gen >= 8) {
@@ -1332,7 +1438,7 @@ static void i915_capture_reg_state(struct drm_i915_private *dev_priv,
 		error->fault_data1 = I915_READ(GEN8_FAULT_TLB_DATA1);
 	}
 
-	if (IS_GEN6(dev)) {
+	if (IS_GEN6(dev_priv)) {
 		error->forcewake = I915_READ_FW(FORCEWAKE);
 		error->gab_ctl = I915_READ(GAB_CTL);
 		error->gfx_mode = I915_READ(GFX_MODE);
@@ -1349,7 +1455,7 @@ static void i915_capture_reg_state(struct drm_i915_private *dev_priv,
 	}
 
 	/* 3: Feature specific registers */
-	if (IS_GEN6(dev) || IS_GEN7(dev)) {
+	if (IS_GEN6(dev_priv) || IS_GEN7(dev_priv)) {
 		error->gam_ecochk = I915_READ(GAM_ECOCHK);
 		error->gac_eco = I915_READ(GAC_ECO_BITS);
 	}
@@ -1362,18 +1468,16 @@ static void i915_capture_reg_state(struct drm_i915_private *dev_priv,
 		error->ier = I915_READ(GEN8_DE_MISC_IER);
 		for (i = 0; i < 4; i++)
 			error->gtier[i] = I915_READ(GEN8_GT_IER(i));
-	} else if (HAS_PCH_SPLIT(dev)) {
+	} else if (HAS_PCH_SPLIT(dev_priv)) {
 		error->ier = I915_READ(DEIER);
 		error->gtier[0] = I915_READ(GTIER);
-	} else if (IS_GEN2(dev)) {
+	} else if (IS_GEN2(dev_priv)) {
 		error->ier = I915_READ16(IER);
-	} else if (!IS_VALLEYVIEW(dev)) {
+	} else if (!IS_VALLEYVIEW(dev_priv)) {
 		error->ier = I915_READ(IER);
 	}
 	error->eir = I915_READ(EIR);
 	error->pgtbl_er = I915_READ(PGTBL_ER);
-
-	i915_get_extra_instdone(dev_priv, error->extra_instdone);
 }
 
 static void i915_error_capture_msg(struct drm_i915_private *dev_priv,
@@ -1418,6 +1522,27 @@ static void i915_capture_gen_state(struct drm_i915_private *dev_priv,
 	       sizeof(error->device_info));
 }
 
+static int capture(void *data)
+{
+	struct drm_i915_error_state *error = data;
+
+	i915_capture_gen_state(error->i915, error);
+	i915_capture_reg_state(error->i915, error);
+	i915_gem_record_fences(error->i915, error);
+	i915_gem_record_rings(error->i915, error);
+	i915_capture_active_buffers(error->i915, error);
+	i915_capture_pinned_buffers(error->i915, error);
+
+	do_gettimeofday(&error->time);
+
+	error->overlay = intel_overlay_capture_error_state(error->i915);
+	error->display = intel_display_capture_error_state(error->i915);
+
+	return 0;
+}
+
+#define DAY_AS_SECONDS(x) (24 * 60 * 60 * (x))
+
 /**
  * i915_capture_error_state - capture an error record for later analysis
  * @dev: drm device
@@ -1435,6 +1560,9 @@ void i915_capture_error_state(struct drm_i915_private *dev_priv,
 	struct drm_i915_error_state *error;
 	unsigned long flags;
 
+	if (!i915.error_capture)
+		return;
+
 	if (READ_ONCE(dev_priv->gpu_error.first_error))
 		return;
 
@@ -1446,18 +1574,9 @@ void i915_capture_error_state(struct drm_i915_private *dev_priv,
 	}
 
 	kref_init(&error->ref);
+	error->i915 = dev_priv;
 
-	i915_capture_gen_state(dev_priv, error);
-	i915_capture_reg_state(dev_priv, error);
-	i915_gem_record_fences(dev_priv, error);
-	i915_gem_record_rings(dev_priv, error);
-	i915_capture_active_buffers(dev_priv, error);
-	i915_capture_pinned_buffers(dev_priv, error);
-
-	do_gettimeofday(&error->time);
-
-	error->overlay = intel_overlay_capture_error_state(dev_priv);
-	error->display = intel_display_capture_error_state(dev_priv);
+	stop_machine(capture, error, NULL);
 
 	i915_error_capture_msg(dev_priv, error, engine_mask, error_msg);
 	DRM_INFO("%s\n", error->error_msg);
@@ -1476,7 +1595,8 @@ void i915_capture_error_state(struct drm_i915_private *dev_priv,
 		return;
 	}
 
-	if (!warned) {
+	if (!warned &&
+	    ktime_get_real_seconds() - DRIVER_TIMESTAMP < DAY_AS_SECONDS(180)) {
 		DRM_INFO("GPU hangs can indicate a bug anywhere in the entire gfx stack, including userspace.\n");
 		DRM_INFO("Please file a _new_ bug report on bugs.freedesktop.org against DRI -> DRM/Intel\n");
 		DRM_INFO("drm/i915 developers can then reassign to the right component if it's not a kernel issue.\n");
@@ -1497,7 +1617,6 @@ void i915_error_state_get(struct drm_device *dev,
 	if (error_priv->error)
 		kref_get(&error_priv->error->ref);
 	spin_unlock_irq(&dev_priv->gpu_error.lock);
-
 }
 
 void i915_error_state_put(struct i915_error_state_file_priv *error_priv)
@@ -1519,33 +1638,3 @@ void i915_destroy_error_state(struct drm_device *dev)
 	if (error)
 		kref_put(&error->ref, i915_error_state_free);
 }
-
-const char *i915_cache_level_str(struct drm_i915_private *i915, int type)
-{
-	switch (type) {
-	case I915_CACHE_NONE: return " uncached";
-	case I915_CACHE_LLC: return HAS_LLC(i915) ? " LLC" : " snooped";
-	case I915_CACHE_L3_LLC: return " L3+LLC";
-	case I915_CACHE_WT: return " WT";
-	default: return "";
-	}
-}
-
-/* NB: please notice the memset */
-void i915_get_extra_instdone(struct drm_i915_private *dev_priv,
-			     uint32_t *instdone)
-{
-	memset(instdone, 0, sizeof(*instdone) * I915_NUM_INSTDONE_REG);
-
-	if (IS_GEN2(dev_priv) || IS_GEN3(dev_priv))
-		instdone[0] = I915_READ(GEN2_INSTDONE);
-	else if (IS_GEN4(dev_priv) || IS_GEN5(dev_priv) || IS_GEN6(dev_priv)) {
-		instdone[0] = I915_READ(RING_INSTDONE(RENDER_RING_BASE));
-		instdone[1] = I915_READ(GEN4_INSTDONE1);
-	} else if (INTEL_GEN(dev_priv) >= 7) {
-		instdone[0] = I915_READ(RING_INSTDONE(RENDER_RING_BASE));
-		instdone[1] = I915_READ(GEN7_SC_INSTDONE);
-		instdone[2] = I915_READ(GEN7_SAMPLER_INSTDONE);
-		instdone[3] = I915_READ(GEN7_ROW_INSTDONE);
-	}
-}
diff --git a/drivers/gpu/drm/i915/i915_guc_submission.c b/drivers/gpu/drm/i915/i915_guc_submission.c
index 43358e18d34c..a1f76c8f8cde 100644
--- a/drivers/gpu/drm/i915/i915_guc_submission.c
+++ b/drivers/gpu/drm/i915/i915_guc_submission.c
@@ -451,6 +451,18 @@ int i915_guc_wq_reserve(struct drm_i915_gem_request *request)
 	return ret;
 }
 
+void i915_guc_wq_unreserve(struct drm_i915_gem_request *request)
+{
+	const size_t wqi_size = sizeof(struct guc_wq_item);
+	struct i915_guc_client *gc = request->i915->guc.execbuf_client;
+
+	GEM_BUG_ON(READ_ONCE(gc->wq_rsvd) < wqi_size);
+
+	spin_lock(&gc->wq_lock);
+	gc->wq_rsvd -= wqi_size;
+	spin_unlock(&gc->wq_lock);
+}
+
 /* Construct a Work Item and append it to the GuC's Work Queue */
 static void guc_wq_item_append(struct i915_guc_client *gc,
 			       struct drm_i915_gem_request *rq)
@@ -905,6 +917,7 @@ static void guc_addon_create(struct intel_guc *guc)
 	struct guc_policies *policies;
 	struct guc_mmio_reg_state *reg_state;
 	struct intel_engine_cs *engine;
+	enum intel_engine_id id;
 	struct page *page;
 	u32 size;
 
@@ -932,10 +945,10 @@ static void guc_addon_create(struct intel_guc *guc)
 	 * so its address won't change after we've told the GuC where
 	 * to find it.
 	 */
-	engine = &dev_priv->engine[RCS];
+	engine = dev_priv->engine[RCS];
 	ads->golden_context_lrca = engine->status_page.ggtt_offset;
 
-	for_each_engine(engine, dev_priv)
+	for_each_engine(engine, dev_priv, id)
 		ads->eng_state_size[engine->guc_id] = intel_lr_context_size(engine);
 
 	/* GuC scheduling policies */
@@ -948,7 +961,7 @@ static void guc_addon_create(struct intel_guc *guc)
 	/* MMIO reg state */
 	reg_state = (void *)policies + sizeof(struct guc_policies);
 
-	for_each_engine(engine, dev_priv) {
+	for_each_engine(engine, dev_priv, id) {
 		reg_state->mmio_white_list[engine->guc_id].mmio_start =
 			engine->mmio_base + GUC_MMIO_WHITE_LIST_START;
 
@@ -1002,9 +1015,10 @@ int i915_guc_submission_init(struct drm_i915_private *dev_priv)
 int i915_guc_submission_enable(struct drm_i915_private *dev_priv)
 {
 	struct intel_guc *guc = &dev_priv->guc;
+	struct drm_i915_gem_request *request;
 	struct i915_guc_client *client;
 	struct intel_engine_cs *engine;
-	struct drm_i915_gem_request *request;
+	enum intel_engine_id id;
 
 	/* client for execbuf submission */
 	client = guc_client_alloc(dev_priv,
@@ -1021,7 +1035,7 @@ int i915_guc_submission_enable(struct drm_i915_private *dev_priv)
 	guc_init_doorbell_hw(guc);
 
 	/* Take over from manual control of ELSP (execlists) */
-	for_each_engine(engine, dev_priv) {
+	for_each_engine(engine, dev_priv, id) {
 		engine->submit_request = i915_guc_submit;
 
 		/* Replay the current set of previously submitted requests */
diff --git a/drivers/gpu/drm/i915/i915_irq.c b/drivers/gpu/drm/i915/i915_irq.c
index c128fdbd24e4..23315e5461bf 100644
--- a/drivers/gpu/drm/i915/i915_irq.c
+++ b/drivers/gpu/drm/i915/i915_irq.c
@@ -350,6 +350,9 @@ void gen6_reset_rps_interrupts(struct drm_i915_private *dev_priv)
 
 void gen6_enable_rps_interrupts(struct drm_i915_private *dev_priv)
 {
+	if (READ_ONCE(dev_priv->rps.interrupts_enabled))
+		return;
+
 	spin_lock_irq(&dev_priv->irq_lock);
 	WARN_ON_ONCE(dev_priv->rps.pm_iir);
 	WARN_ON_ONCE(I915_READ(gen6_pm_iir(dev_priv)) & dev_priv->pm_rps_events);
@@ -368,6 +371,9 @@ u32 gen6_sanitize_rps_pm_mask(struct drm_i915_private *dev_priv, u32 mask)
 
 void gen6_disable_rps_interrupts(struct drm_i915_private *dev_priv)
 {
+	if (!READ_ONCE(dev_priv->rps.interrupts_enabled))
+		return;
+
 	spin_lock_irq(&dev_priv->irq_lock);
 	dev_priv->rps.interrupts_enabled = false;
 
@@ -1052,8 +1058,9 @@ static u32 vlv_wa_c0_ei(struct drm_i915_private *dev_priv, u32 pm_iir)
 static bool any_waiters(struct drm_i915_private *dev_priv)
 {
 	struct intel_engine_cs *engine;
+	enum intel_engine_id id;
 
-	for_each_engine(engine, dev_priv)
+	for_each_engine(engine, dev_priv, id)
 		if (intel_engine_has_waiter(engine))
 			return true;
 
@@ -1251,20 +1258,20 @@ static void ilk_gt_irq_handler(struct drm_i915_private *dev_priv,
 			       u32 gt_iir)
 {
 	if (gt_iir & GT_RENDER_USER_INTERRUPT)
-		notify_ring(&dev_priv->engine[RCS]);
+		notify_ring(dev_priv->engine[RCS]);
 	if (gt_iir & ILK_BSD_USER_INTERRUPT)
-		notify_ring(&dev_priv->engine[VCS]);
+		notify_ring(dev_priv->engine[VCS]);
 }
 
 static void snb_gt_irq_handler(struct drm_i915_private *dev_priv,
 			       u32 gt_iir)
 {
 	if (gt_iir & GT_RENDER_USER_INTERRUPT)
-		notify_ring(&dev_priv->engine[RCS]);
+		notify_ring(dev_priv->engine[RCS]);
 	if (gt_iir & GT_BSD_USER_INTERRUPT)
-		notify_ring(&dev_priv->engine[VCS]);
+		notify_ring(dev_priv->engine[VCS]);
 	if (gt_iir & GT_BLT_USER_INTERRUPT)
-		notify_ring(&dev_priv->engine[BCS]);
+		notify_ring(dev_priv->engine[BCS]);
 
 	if (gt_iir & (GT_BLT_CS_ERROR_INTERRUPT |
 		      GT_BSD_CS_ERROR_INTERRUPT |
@@ -1334,21 +1341,21 @@ static void gen8_gt_irq_handler(struct drm_i915_private *dev_priv,
 				u32 gt_iir[4])
 {
 	if (gt_iir[0]) {
-		gen8_cs_irq_handler(&dev_priv->engine[RCS],
+		gen8_cs_irq_handler(dev_priv->engine[RCS],
 				    gt_iir[0], GEN8_RCS_IRQ_SHIFT);
-		gen8_cs_irq_handler(&dev_priv->engine[BCS],
+		gen8_cs_irq_handler(dev_priv->engine[BCS],
 				    gt_iir[0], GEN8_BCS_IRQ_SHIFT);
 	}
 
 	if (gt_iir[1]) {
-		gen8_cs_irq_handler(&dev_priv->engine[VCS],
+		gen8_cs_irq_handler(dev_priv->engine[VCS],
 				    gt_iir[1], GEN8_VCS1_IRQ_SHIFT);
-		gen8_cs_irq_handler(&dev_priv->engine[VCS2],
+		gen8_cs_irq_handler(dev_priv->engine[VCS2],
 				    gt_iir[1], GEN8_VCS2_IRQ_SHIFT);
 	}
 
 	if (gt_iir[3])
-		gen8_cs_irq_handler(&dev_priv->engine[VECS],
+		gen8_cs_irq_handler(dev_priv->engine[VECS],
 				    gt_iir[3], GEN8_VECS_IRQ_SHIFT);
 
 	if (gt_iir[2] & dev_priv->pm_rps_events)
@@ -1592,7 +1599,7 @@ static void gen6_rps_irq_handler(struct drm_i915_private *dev_priv, u32 pm_iir)
 
 	if (HAS_VEBOX(dev_priv)) {
 		if (pm_iir & PM_VEBOX_USER_INTERRUPT)
-			notify_ring(&dev_priv->engine[VECS]);
+			notify_ring(dev_priv->engine[VECS]);
 
 		if (pm_iir & PM_VEBOX_CS_ERROR_INTERRUPT)
 			DRM_DEBUG("Command parser error, pm_iir 0x%08x\n", pm_iir);
@@ -2545,92 +2552,52 @@ static void i915_reset_and_wakeup(struct drm_i915_private *dev_priv)
 	wake_up_all(&dev_priv->gpu_error.reset_queue);
 }
 
-static void i915_report_and_clear_eir(struct drm_i915_private *dev_priv)
+static inline void
+i915_err_print_instdone(struct drm_i915_private *dev_priv,
+			struct intel_instdone *instdone)
 {
-	uint32_t instdone[I915_NUM_INSTDONE_REG];
-	u32 eir = I915_READ(EIR);
-	int pipe, i;
+	int slice;
+	int subslice;
+
+	pr_err("  INSTDONE: 0x%08x\n", instdone->instdone);
 
-	if (!eir)
+	if (INTEL_GEN(dev_priv) <= 3)
 		return;
 
-	pr_err("render error detected, EIR: 0x%08x\n", eir);
+	pr_err("  SC_INSTDONE: 0x%08x\n", instdone->slice_common);
 
-	i915_get_extra_instdone(dev_priv, instdone);
+	if (INTEL_GEN(dev_priv) <= 6)
+		return;
 
-	if (IS_G4X(dev_priv)) {
-		if (eir & (GM45_ERROR_MEM_PRIV | GM45_ERROR_CP_PRIV)) {
-			u32 ipeir = I915_READ(IPEIR_I965);
-
-			pr_err("  IPEIR: 0x%08x\n", I915_READ(IPEIR_I965));
-			pr_err("  IPEHR: 0x%08x\n", I915_READ(IPEHR_I965));
-			for (i = 0; i < ARRAY_SIZE(instdone); i++)
-				pr_err("  INSTDONE_%d: 0x%08x\n", i, instdone[i]);
-			pr_err("  INSTPS: 0x%08x\n", I915_READ(INSTPS));
-			pr_err("  ACTHD: 0x%08x\n", I915_READ(ACTHD_I965));
-			I915_WRITE(IPEIR_I965, ipeir);
-			POSTING_READ(IPEIR_I965);
-		}
-		if (eir & GM45_ERROR_PAGE_TABLE) {
-			u32 pgtbl_err = I915_READ(PGTBL_ER);
-			pr_err("page table error\n");
-			pr_err("  PGTBL_ER: 0x%08x\n", pgtbl_err);
-			I915_WRITE(PGTBL_ER, pgtbl_err);
-			POSTING_READ(PGTBL_ER);
-		}
-	}
+	for_each_instdone_slice_subslice(dev_priv, slice, subslice)
+		pr_err("  SAMPLER_INSTDONE[%d][%d]: 0x%08x\n",
+		       slice, subslice, instdone->sampler[slice][subslice]);
 
-	if (!IS_GEN2(dev_priv)) {
-		if (eir & I915_ERROR_PAGE_TABLE) {
-			u32 pgtbl_err = I915_READ(PGTBL_ER);
-			pr_err("page table error\n");
-			pr_err("  PGTBL_ER: 0x%08x\n", pgtbl_err);
-			I915_WRITE(PGTBL_ER, pgtbl_err);
-			POSTING_READ(PGTBL_ER);
-		}
-	}
+	for_each_instdone_slice_subslice(dev_priv, slice, subslice)
+		pr_err("  ROW_INSTDONE[%d][%d]: 0x%08x\n",
+		       slice, subslice, instdone->row[slice][subslice]);
+}
 
-	if (eir & I915_ERROR_MEMORY_REFRESH) {
-		pr_err("memory refresh error:\n");
-		for_each_pipe(dev_priv, pipe)
-			pr_err("pipe %c stat: 0x%08x\n",
-			       pipe_name(pipe), I915_READ(PIPESTAT(pipe)));
-		/* pipestat has already been acked */
-	}
-	if (eir & I915_ERROR_INSTRUCTION) {
-		pr_err("instruction error\n");
-		pr_err("  INSTPM: 0x%08x\n", I915_READ(INSTPM));
-		for (i = 0; i < ARRAY_SIZE(instdone); i++)
-			pr_err("  INSTDONE_%d: 0x%08x\n", i, instdone[i]);
-		if (INTEL_GEN(dev_priv) < 4) {
-			u32 ipeir = I915_READ(IPEIR);
-
-			pr_err("  IPEIR: 0x%08x\n", I915_READ(IPEIR));
-			pr_err("  IPEHR: 0x%08x\n", I915_READ(IPEHR));
-			pr_err("  ACTHD: 0x%08x\n", I915_READ(ACTHD));
-			I915_WRITE(IPEIR, ipeir);
-			POSTING_READ(IPEIR);
-		} else {
-			u32 ipeir = I915_READ(IPEIR_I965);
-
-			pr_err("  IPEIR: 0x%08x\n", I915_READ(IPEIR_I965));
-			pr_err("  IPEHR: 0x%08x\n", I915_READ(IPEHR_I965));
-			pr_err("  INSTPS: 0x%08x\n", I915_READ(INSTPS));
-			pr_err("  ACTHD: 0x%08x\n", I915_READ(ACTHD_I965));
-			I915_WRITE(IPEIR_I965, ipeir);
-			POSTING_READ(IPEIR_I965);
-		}
-	}
+static void i915_clear_error_registers(struct drm_i915_private *dev_priv)
+{
+	u32 eir;
 
-	I915_WRITE(EIR, eir);
-	POSTING_READ(EIR);
+	if (!IS_GEN2(dev_priv))
+		I915_WRITE(PGTBL_ER, I915_READ(PGTBL_ER));
+
+	if (INTEL_GEN(dev_priv) < 4)
+		I915_WRITE(IPEIR, I915_READ(IPEIR));
+	else
+		I915_WRITE(IPEIR_I965, I915_READ(IPEIR_I965));
+
+	I915_WRITE(EIR, I915_READ(EIR));
 	eir = I915_READ(EIR);
 	if (eir) {
 		/*
 		 * some errors might have become stuck,
 		 * mask them.
 		 */
-		DRM_ERROR("EIR stuck: 0x%08x, masking\n", eir);
+		DRM_DEBUG_DRIVER("EIR stuck: 0x%08x, masking\n", eir);
 		I915_WRITE(EMR, I915_READ(EMR) | eir);
 		I915_WRITE(IIR, I915_RENDER_COMMAND_PARSER_ERROR_INTERRUPT);
 	}
@@ -2659,7 +2626,7 @@ void i915_handle_error(struct drm_i915_private *dev_priv,
 	va_end(args);
 
 	i915_capture_error_state(dev_priv, engine_mask, error_msg);
-	i915_report_and_clear_eir(dev_priv);
+	i915_clear_error_registers(dev_priv);
 
 	if (!engine_mask)
 		return;
@@ -2688,45 +2655,40 @@ void i915_handle_error(struct drm_i915_private *dev_priv,
 /* Called from drm generic code, passed 'crtc' which
  * we use as a pipe index
  */
-static int i915_enable_vblank(struct drm_device *dev, unsigned int pipe)
+static int i8xx_enable_vblank(struct drm_device *dev, unsigned int pipe)
 {
 	struct drm_i915_private *dev_priv = to_i915(dev);
 	unsigned long irqflags;
 
 	spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
-	if (INTEL_INFO(dev)->gen >= 4)
-		i915_enable_pipestat(dev_priv, pipe,
-				     PIPE_START_VBLANK_INTERRUPT_STATUS);
-	else
-		i915_enable_pipestat(dev_priv, pipe,
-				     PIPE_VBLANK_INTERRUPT_STATUS);
+	i915_enable_pipestat(dev_priv, pipe, PIPE_VBLANK_INTERRUPT_STATUS);
 	spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
 
 	return 0;
 }
 
-static int ironlake_enable_vblank(struct drm_device *dev, unsigned int pipe)
+static int i965_enable_vblank(struct drm_device *dev, unsigned int pipe)
 {
 	struct drm_i915_private *dev_priv = to_i915(dev);
 	unsigned long irqflags;
-	uint32_t bit = (INTEL_INFO(dev)->gen >= 7) ? DE_PIPE_VBLANK_IVB(pipe) :
-						     DE_PIPE_VBLANK(pipe);
 
 	spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
-	ilk_enable_display_irq(dev_priv, bit);
+	i915_enable_pipestat(dev_priv, pipe,
+			     PIPE_START_VBLANK_INTERRUPT_STATUS);
 	spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
 
 	return 0;
 }
 
-static int valleyview_enable_vblank(struct drm_device *dev, unsigned int pipe)
+static int ironlake_enable_vblank(struct drm_device *dev, unsigned int pipe)
 {
 	struct drm_i915_private *dev_priv = to_i915(dev);
 	unsigned long irqflags;
+	uint32_t bit = INTEL_GEN(dev_priv) >= 7 ?
+		DE_PIPE_VBLANK_IVB(pipe) : DE_PIPE_VBLANK(pipe);
 
 	spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
-	i915_enable_pipestat(dev_priv, pipe,
-			     PIPE_START_VBLANK_INTERRUPT_STATUS);
+	ilk_enable_display_irq(dev_priv, bit);
 	spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
 
 	return 0;
@@ -2747,38 +2709,36 @@ static int gen8_enable_vblank(struct drm_device *dev, unsigned int pipe)
 /* Called from drm generic code, passed 'crtc' which
  * we use as a pipe index
  */
-static void i915_disable_vblank(struct drm_device *dev, unsigned int pipe)
+static void i8xx_disable_vblank(struct drm_device *dev, unsigned int pipe)
 {
 	struct drm_i915_private *dev_priv = to_i915(dev);
 	unsigned long irqflags;
 
 	spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
-	i915_disable_pipestat(dev_priv, pipe,
-			      PIPE_VBLANK_INTERRUPT_STATUS |
-			      PIPE_START_VBLANK_INTERRUPT_STATUS);
+	i915_disable_pipestat(dev_priv, pipe, PIPE_VBLANK_INTERRUPT_STATUS);
 	spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
 }
 
-static void ironlake_disable_vblank(struct drm_device *dev, unsigned int pipe)
+static void i965_disable_vblank(struct drm_device *dev, unsigned int pipe)
 {
 	struct drm_i915_private *dev_priv = to_i915(dev);
 	unsigned long irqflags;
-	uint32_t bit = (INTEL_INFO(dev)->gen >= 7) ? DE_PIPE_VBLANK_IVB(pipe) :
-						     DE_PIPE_VBLANK(pipe);
 
 	spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
-	ilk_disable_display_irq(dev_priv, bit);
+	i915_disable_pipestat(dev_priv, pipe,
+			      PIPE_START_VBLANK_INTERRUPT_STATUS);
 	spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
 }
 
-static void valleyview_disable_vblank(struct drm_device *dev, unsigned int pipe)
+static void ironlake_disable_vblank(struct drm_device *dev, unsigned int pipe)
 {
 	struct drm_i915_private *dev_priv = to_i915(dev);
 	unsigned long irqflags;
+	uint32_t bit = INTEL_GEN(dev_priv) >= 7 ?
+		DE_PIPE_VBLANK_IVB(pipe) : DE_PIPE_VBLANK(pipe);
 
 	spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
-	i915_disable_pipestat(dev_priv, pipe,
-			      PIPE_START_VBLANK_INTERRUPT_STATUS);
+	ilk_disable_display_irq(dev_priv, bit);
 	spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
 }
 
@@ -2810,29 +2770,30 @@ semaphore_wait_to_signaller_ring(struct intel_engine_cs *engine, u32 ipehr,
 {
 	struct drm_i915_private *dev_priv = engine->i915;
 	struct intel_engine_cs *signaller;
+	enum intel_engine_id id;
 
 	if (INTEL_GEN(dev_priv) >= 8) {
-		for_each_engine(signaller, dev_priv) {
+		for_each_engine(signaller, dev_priv, id) {
 			if (engine == signaller)
 				continue;
 
-			if (offset == signaller->semaphore.signal_ggtt[engine->id])
+			if (offset == signaller->semaphore.signal_ggtt[engine->hw_id])
 				return signaller;
 		}
 	} else {
 		u32 sync_bits = ipehr & MI_SEMAPHORE_SYNC_MASK;
 
-		for_each_engine(signaller, dev_priv) {
+		for_each_engine(signaller, dev_priv, id) {
 			if(engine == signaller)
 				continue;
 
-			if (sync_bits == signaller->semaphore.mbox.wait[engine->id])
+			if (sync_bits == signaller->semaphore.mbox.wait[engine->hw_id])
 				return signaller;
 		}
 	}
 
-	DRM_DEBUG_DRIVER("No signaller ring found for ring %i, ipehr 0x%08x, offset 0x%016llx\n",
-			 engine->id, ipehr, offset);
+	DRM_DEBUG_DRIVER("No signaller ring found for %s, ipehr 0x%08x, offset 0x%016llx\n",
+			 engine->name, ipehr, offset);
 
 	return ERR_PTR(-ENODEV);
 }
@@ -2943,35 +2904,52 @@ static int semaphore_passed(struct intel_engine_cs *engine)
 static void semaphore_clear_deadlocks(struct drm_i915_private *dev_priv)
 {
 	struct intel_engine_cs *engine;
+	enum intel_engine_id id;
 
-	for_each_engine(engine, dev_priv)
+	for_each_engine(engine, dev_priv, id)
 		engine->hangcheck.deadlock = 0;
 }
 
+static bool instdone_unchanged(u32 current_instdone, u32 *old_instdone)
+{
+	u32 tmp = current_instdone | *old_instdone;
+	bool unchanged;
+
+	unchanged = tmp == *old_instdone;
+	*old_instdone |= tmp;
+
+	return unchanged;
+}
+
 static bool subunits_stuck(struct intel_engine_cs *engine)
 {
-	u32 instdone[I915_NUM_INSTDONE_REG];
+	struct drm_i915_private *dev_priv = engine->i915;
+	struct intel_instdone instdone;
+	struct intel_instdone *accu_instdone = &engine->hangcheck.instdone;
 	bool stuck;
-	int i;
+	int slice;
+	int subslice;
 
 	if (engine->id != RCS)
 		return true;
 
-	i915_get_extra_instdone(engine->i915, instdone);
+	intel_engine_get_instdone(engine, &instdone);
 
 	/* There might be unstable subunit states even when
 	 * actual head is not moving. Filter out the unstable ones by
 	 * accumulating the undone -> done transitions and only
 	 * consider those as progress.
 	 */
-	stuck = true;
-	for (i = 0; i < I915_NUM_INSTDONE_REG; i++) {
-		const u32 tmp = instdone[i] | engine->hangcheck.instdone[i];
-
-		if (tmp != engine->hangcheck.instdone[i])
-			stuck = false;
-
-		engine->hangcheck.instdone[i] |= tmp;
+	stuck = instdone_unchanged(instdone.instdone,
+				   &accu_instdone->instdone);
+	stuck &= instdone_unchanged(instdone.slice_common,
+				    &accu_instdone->slice_common);
+
+	for_each_instdone_slice_subslice(dev_priv, slice, subslice) {
+		stuck &= instdone_unchanged(instdone.sampler[slice][subslice],
+					    &accu_instdone->sampler[slice][subslice]);
+		stuck &= instdone_unchanged(instdone.row[slice][subslice],
+					    &accu_instdone->row[slice][subslice]);
 	}
 
 	return stuck;
@@ -2983,7 +2961,7 @@ head_stuck(struct intel_engine_cs *engine, u64 acthd)
 	if (acthd != engine->hangcheck.acthd) {
 
 		/* Clear subunit states on head movement */
-		memset(engine->hangcheck.instdone, 0,
+		memset(&engine->hangcheck.instdone, 0,
 		       sizeof(engine->hangcheck.instdone));
 
 		return HANGCHECK_ACTIVE;
@@ -3055,6 +3033,7 @@ static void i915_hangcheck_elapsed(struct work_struct *work)
 		container_of(work, typeof(*dev_priv),
 			     gpu_error.hangcheck_work.work);
 	struct intel_engine_cs *engine;
+	enum intel_engine_id id;
 	unsigned int hung = 0, stuck = 0;
 	int busy_count = 0;
 #define BUSY 1
@@ -3074,7 +3053,7 @@ static void i915_hangcheck_elapsed(struct work_struct *work)
 	 */
 	intel_uncore_arm_unclaimed_mmio_detection(dev_priv);
 
-	for_each_engine(engine, dev_priv) {
+	for_each_engine(engine, dev_priv, id) {
 		bool busy = intel_engine_has_waiter(engine);
 		u64 acthd;
 		u32 seqno;
@@ -3153,7 +3132,7 @@ static void i915_hangcheck_elapsed(struct work_struct *work)
 			/* Clear head and subunit states on seqno movement */
 			acthd = 0;
 
-			memset(engine->hangcheck.instdone, 0,
+			memset(&engine->hangcheck.instdone, 0,
 			       sizeof(engine->hangcheck.instdone));
 		}
 
@@ -3191,12 +3170,12 @@ static void ibx_irq_reset(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = to_i915(dev);
 
-	if (HAS_PCH_NOP(dev))
+	if (HAS_PCH_NOP(dev_priv))
 		return;
 
 	GEN5_IRQ_RESET(SDE);
 
-	if (HAS_PCH_CPT(dev) || HAS_PCH_LPT(dev))
+	if (HAS_PCH_CPT(dev_priv) || HAS_PCH_LPT(dev_priv))
 		I915_WRITE(SERR_INT, 0xffffffff);
 }
 
@@ -3212,7 +3191,7 @@ static void ibx_irq_pre_postinstall(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = to_i915(dev);
 
-	if (HAS_PCH_NOP(dev))
+	if (HAS_PCH_NOP(dev_priv))
 		return;
 
 	WARN_ON(I915_READ(SDEIER) != 0);
@@ -3287,7 +3266,7 @@ static void ironlake_irq_reset(struct drm_device *dev)
 	I915_WRITE(HWSTAM, 0xffffffff);
 
 	GEN5_IRQ_RESET(DE);
-	if (IS_GEN7(dev))
+	if (IS_GEN7(dev_priv))
 		I915_WRITE(GEN7_ERR_INT, 0xffffffff);
 
 	gen5_gt_irq_reset(dev);
@@ -3337,7 +3316,7 @@ static void gen8_irq_reset(struct drm_device *dev)
 	GEN5_IRQ_RESET(GEN8_DE_MISC_);
 	GEN5_IRQ_RESET(GEN8_PCU_);
 
-	if (HAS_PCH_SPLIT(dev))
+	if (HAS_PCH_SPLIT(dev_priv))
 		ibx_irq_reset(dev);
 }
 
@@ -3526,10 +3505,10 @@ static void ibx_irq_postinstall(struct drm_device *dev)
 	struct drm_i915_private *dev_priv = to_i915(dev);
 	u32 mask;
 
-	if (HAS_PCH_NOP(dev))
+	if (HAS_PCH_NOP(dev_priv))
 		return;
 
-	if (HAS_PCH_IBX(dev))
+	if (HAS_PCH_IBX(dev_priv))
 		mask = SDE_GMBUS | SDE_AUX_MASK | SDE_POISON;
 	else
 		mask = SDE_GMBUS_CPT | SDE_AUX_MASK_CPT;
@@ -3546,14 +3525,14 @@ static void gen5_gt_irq_postinstall(struct drm_device *dev)
 	pm_irqs = gt_irqs = 0;
 
 	dev_priv->gt_irq_mask = ~0;
-	if (HAS_L3_DPF(dev)) {
+	if (HAS_L3_DPF(dev_priv)) {
 		/* L3 parity interrupt is always unmasked. */
-		dev_priv->gt_irq_mask = ~GT_PARITY_ERROR(dev);
-		gt_irqs |= GT_PARITY_ERROR(dev);
+		dev_priv->gt_irq_mask = ~GT_PARITY_ERROR(dev_priv);
+		gt_irqs |= GT_PARITY_ERROR(dev_priv);
 	}
 
 	gt_irqs |= GT_RENDER_USER_INTERRUPT;
-	if (IS_GEN5(dev)) {
+	if (IS_GEN5(dev_priv)) {
 		gt_irqs |= ILK_BSD_USER_INTERRUPT;
 	} else {
 		gt_irqs |= GT_BLT_USER_INTERRUPT | GT_BSD_USER_INTERRUPT;
@@ -3610,7 +3589,7 @@ static int ironlake_irq_postinstall(struct drm_device *dev)
 
 	ibx_irq_postinstall(dev);
 
-	if (IS_IRONLAKE_M(dev)) {
+	if (IS_IRONLAKE_M(dev_priv)) {
 		/* Enable PCU event interrupts
 		 *
 		 * spinlocking not required here for correctness since interrupt
@@ -3750,13 +3729,13 @@ static int gen8_irq_postinstall(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = to_i915(dev);
 
-	if (HAS_PCH_SPLIT(dev))
+	if (HAS_PCH_SPLIT(dev_priv))
 		ibx_irq_pre_postinstall(dev);
 
 	gen8_gt_irq_postinstall(dev_priv);
 	gen8_de_irq_postinstall(dev_priv);
 
-	if (HAS_PCH_SPLIT(dev))
+	if (HAS_PCH_SPLIT(dev_priv))
 		ibx_irq_postinstall(dev);
 
 	I915_WRITE(GEN8_MASTER_IRQ, GEN8_MASTER_IRQ_CONTROL);
@@ -3965,7 +3944,7 @@ static irqreturn_t i8xx_irq_handler(int irq, void *arg)
 		new_iir = I915_READ16(IIR); /* Flush posted writes */
 
 		if (iir & I915_USER_INTERRUPT)
-			notify_ring(&dev_priv->engine[RCS]);
+			notify_ring(dev_priv->engine[RCS]);
 
 		for_each_pipe(dev_priv, pipe) {
 			int plane = pipe;
@@ -4162,7 +4141,7 @@ static irqreturn_t i915_irq_handler(int irq, void *arg)
 		new_iir = I915_READ(IIR); /* Flush posted writes */
 
 		if (iir & I915_USER_INTERRUPT)
-			notify_ring(&dev_priv->engine[RCS]);
+			notify_ring(dev_priv->engine[RCS]);
 
 		for_each_pipe(dev_priv, pipe) {
 			int plane = pipe;
@@ -4394,9 +4373,9 @@ static irqreturn_t i965_irq_handler(int irq, void *arg)
 		new_iir = I915_READ(IIR); /* Flush posted writes */
 
 		if (iir & I915_USER_INTERRUPT)
-			notify_ring(&dev_priv->engine[RCS]);
+			notify_ring(dev_priv->engine[RCS]);
 		if (iir & I915_BSD_USER_INTERRUPT)
-			notify_ring(&dev_priv->engine[VCS]);
+			notify_ring(dev_priv->engine[VCS]);
 
 		for_each_pipe(dev_priv, pipe) {
 			if (pipe_stats[pipe] & PIPE_START_VBLANK_INTERRUPT_STATUS &&
@@ -4533,16 +4512,16 @@ void intel_irq_init(struct drm_i915_private *dev_priv)
 		dev->driver->irq_preinstall = cherryview_irq_preinstall;
 		dev->driver->irq_postinstall = cherryview_irq_postinstall;
 		dev->driver->irq_uninstall = cherryview_irq_uninstall;
-		dev->driver->enable_vblank = valleyview_enable_vblank;
-		dev->driver->disable_vblank = valleyview_disable_vblank;
+		dev->driver->enable_vblank = i965_enable_vblank;
+		dev->driver->disable_vblank = i965_disable_vblank;
 		dev_priv->display.hpd_irq_setup = i915_hpd_irq_setup;
 	} else if (IS_VALLEYVIEW(dev_priv)) {
 		dev->driver->irq_handler = valleyview_irq_handler;
 		dev->driver->irq_preinstall = valleyview_irq_preinstall;
 		dev->driver->irq_postinstall = valleyview_irq_postinstall;
 		dev->driver->irq_uninstall = valleyview_irq_uninstall;
-		dev->driver->enable_vblank = valleyview_enable_vblank;
-		dev->driver->disable_vblank = valleyview_disable_vblank;
+		dev->driver->enable_vblank = i965_enable_vblank;
+		dev->driver->disable_vblank = i965_disable_vblank;
 		dev_priv->display.hpd_irq_setup = i915_hpd_irq_setup;
 	} else if (INTEL_INFO(dev_priv)->gen >= 8) {
 		dev->driver->irq_handler = gen8_irq_handler;
@@ -4551,13 +4530,13 @@ void intel_irq_init(struct drm_i915_private *dev_priv)
 		dev->driver->irq_uninstall = gen8_irq_uninstall;
 		dev->driver->enable_vblank = gen8_enable_vblank;
 		dev->driver->disable_vblank = gen8_disable_vblank;
-		if (IS_BROXTON(dev))
+		if (IS_BROXTON(dev_priv))
 			dev_priv->display.hpd_irq_setup = bxt_hpd_irq_setup;
-		else if (HAS_PCH_SPT(dev) || HAS_PCH_KBP(dev))
+		else if (HAS_PCH_SPT(dev_priv) || HAS_PCH_KBP(dev_priv))
 			dev_priv->display.hpd_irq_setup = spt_hpd_irq_setup;
 		else
 			dev_priv->display.hpd_irq_setup = ilk_hpd_irq_setup;
-	} else if (HAS_PCH_SPLIT(dev)) {
+	} else if (HAS_PCH_SPLIT(dev_priv)) {
 		dev->driver->irq_handler = ironlake_irq_handler;
 		dev->driver->irq_preinstall = ironlake_irq_reset;
 		dev->driver->irq_postinstall = ironlake_irq_postinstall;
@@ -4571,21 +4550,25 @@ void intel_irq_init(struct drm_i915_private *dev_priv)
 			dev->driver->irq_postinstall = i8xx_irq_postinstall;
 			dev->driver->irq_handler = i8xx_irq_handler;
 			dev->driver->irq_uninstall = i8xx_irq_uninstall;
+			dev->driver->enable_vblank = i8xx_enable_vblank;
+			dev->driver->disable_vblank = i8xx_disable_vblank;
 		} else if (IS_GEN3(dev_priv)) {
 			dev->driver->irq_preinstall = i915_irq_preinstall;
 			dev->driver->irq_postinstall = i915_irq_postinstall;
 			dev->driver->irq_uninstall = i915_irq_uninstall;
 			dev->driver->irq_handler = i915_irq_handler;
+			dev->driver->enable_vblank = i8xx_enable_vblank;
+			dev->driver->disable_vblank = i8xx_disable_vblank;
 		} else {
 			dev->driver->irq_preinstall = i965_irq_preinstall;
 			dev->driver->irq_postinstall = i965_irq_postinstall;
 			dev->driver->irq_uninstall = i965_irq_uninstall;
 			dev->driver->irq_handler = i965_irq_handler;
+			dev->driver->enable_vblank = i965_enable_vblank;
+			dev->driver->disable_vblank = i965_disable_vblank;
 		}
 		if (I915_HAS_HOTPLUG(dev_priv))
 			dev_priv->display.hpd_irq_setup = i915_hpd_irq_setup;
-		dev->driver->enable_vblank = i915_enable_vblank;
-		dev->driver->disable_vblank = i915_disable_vblank;
 	}
 }
 
diff --git a/drivers/gpu/drm/i915/i915_params.c b/drivers/gpu/drm/i915/i915_params.c
index 768ad89d9cd4..629e4334719c 100644
--- a/drivers/gpu/drm/i915/i915_params.c
+++ b/drivers/gpu/drm/i915/i915_params.c
@@ -47,6 +47,7 @@ struct i915_params i915 __read_mostly = {
 	.load_detect_test = 0,
 	.force_reset_modeset_test = 0,
 	.reset = true,
+	.error_capture = true,
 	.invert_brightness = 0,
 	.disable_display = 0,
 	.enable_cmd_parser = 1,
@@ -115,6 +116,14 @@ MODULE_PARM_DESC(vbt_sdvo_panel_type,
 module_param_named_unsafe(reset, i915.reset, bool, 0600);
 MODULE_PARM_DESC(reset, "Attempt GPU resets (default: true)");
 
+#if IS_ENABLED(CONFIG_DRM_I915_CAPTURE_ERROR)
+module_param_named(error_capture, i915.error_capture, bool, 0600);
+MODULE_PARM_DESC(error_capture,
+	"Record the GPU state following a hang. "
+	"This information in /sys/class/drm/card<N>/error is vital for "
+	"triaging and debugging hangs.");
+#endif
+
 module_param_named_unsafe(enable_hangcheck, i915.enable_hangcheck, bool, 0644);
 MODULE_PARM_DESC(enable_hangcheck,
 	"Periodically check GPU activity for detecting hangs. "
diff --git a/drivers/gpu/drm/i915/i915_params.h b/drivers/gpu/drm/i915/i915_params.h
index 3a0dd78ddb38..94efc899c1ef 100644
--- a/drivers/gpu/drm/i915/i915_params.h
+++ b/drivers/gpu/drm/i915/i915_params.h
@@ -59,6 +59,7 @@ struct i915_params {
 	bool load_detect_test;
 	bool force_reset_modeset_test;
 	bool reset;
+	bool error_capture;
 	bool disable_display;
 	bool verbose_state_checks;
 	bool nuclear_pageflip;
diff --git a/drivers/gpu/drm/i915/i915_pci.c b/drivers/gpu/drm/i915/i915_pci.c
index 687c768833b3..31e6edd08dd0 100644
--- a/drivers/gpu/drm/i915/i915_pci.c
+++ b/drivers/gpu/drm/i915/i915_pci.c
@@ -431,9 +431,6 @@ static const struct pci_device_id pciidlist[] = {
 };
 MODULE_DEVICE_TABLE(pci, pciidlist);
 
-extern int i915_driver_load(struct pci_dev *pdev,
-			    const struct pci_device_id *ent);
-
 static int i915_pci_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 {
 	struct intel_device_info *intel_info =
@@ -463,8 +460,6 @@ static int i915_pci_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	return i915_driver_load(pdev, ent);
 }
 
-extern void i915_driver_unload(struct drm_device *dev);
-
 static void i915_pci_remove(struct pci_dev *pdev)
 {
 	struct drm_device *dev = pci_get_drvdata(pdev);
@@ -473,8 +468,6 @@ static void i915_pci_remove(struct pci_dev *pdev)
 	drm_dev_unref(dev);
 }
 
-extern const struct dev_pm_ops i915_pm_ops;
-
 static struct pci_driver i915_pci_driver = {
 	.name = DRIVER_NAME,
 	.id_table = pciidlist,
diff --git a/drivers/gpu/drm/i915/i915_reg.h b/drivers/gpu/drm/i915/i915_reg.h
index 70d96162def6..00efaa13974d 100644
--- a/drivers/gpu/drm/i915/i915_reg.h
+++ b/drivers/gpu/drm/i915/i915_reg.h
@@ -86,8 +86,7 @@ static inline bool i915_mmio_reg_valid(i915_reg_t reg)
 #define DEVEN 0x54
 #define   DEVEN_MCHBAR_EN (1 << 28)
 
-#define BSM 0x5c
-#define   BSM_MASK (0xFFFF << 20)
+/* BSM in include/drm/i915_drm.h */
 
 #define HPLLCC	0xc0 /* 85x only */
 #define   GC_CLOCK_CONTROL_MASK		(0x7 << 0)
@@ -1605,6 +1604,7 @@ enum skl_disp_power_wells {
 #define RING_HEAD(base)		_MMIO((base)+0x34)
 #define RING_START(base)	_MMIO((base)+0x38)
 #define RING_CTL(base)		_MMIO((base)+0x3c)
+#define   RING_CTL_SIZE(size)	((size) - PAGE_SIZE) /* in bytes -> pages */
 #define RING_SYNC_0(base)	_MMIO((base)+0x40)
 #define RING_SYNC_1(base)	_MMIO((base)+0x44)
 #define RING_SYNC_2(base)	_MMIO((base)+0x48)
@@ -1708,7 +1708,11 @@ enum skl_disp_power_wells {
 #define GEN7_SC_INSTDONE	_MMIO(0x7100)
 #define GEN7_SAMPLER_INSTDONE	_MMIO(0xe160)
 #define GEN7_ROW_INSTDONE	_MMIO(0xe164)
-#define I915_NUM_INSTDONE_REG	4
+#define GEN8_MCR_SELECTOR		_MMIO(0xfdc)
+#define   GEN8_MCR_SLICE(slice)		(((slice) & 3) << 26)
+#define   GEN8_MCR_SLICE_MASK		GEN8_MCR_SLICE(3)
+#define   GEN8_MCR_SUBSLICE(subslice)	(((subslice) & 3) << 24)
+#define   GEN8_MCR_SUBSLICE_MASK	GEN8_MCR_SUBSLICE(3)
 #define RING_IPEIR(base)	_MMIO((base)+0x64)
 #define RING_IPEHR(base)	_MMIO((base)+0x68)
 /*
@@ -2089,9 +2093,9 @@ enum skl_disp_power_wells {
 #define PM_VEBOX_CS_ERROR_INTERRUPT		(1 << 12) /* hsw+ */
 #define PM_VEBOX_USER_INTERRUPT			(1 << 10) /* hsw+ */
 
-#define GT_PARITY_ERROR(dev) \
+#define GT_PARITY_ERROR(dev_priv) \
 	(GT_RENDER_L3_PARITY_ERROR_INTERRUPT | \
-	 (IS_HASWELL(dev) ? GT_RENDER_L3_PARITY_ERROR_INTERRUPT_S1 : 0))
+	 (IS_HASWELL(dev_priv) ? GT_RENDER_L3_PARITY_ERROR_INTERRUPT_S1 : 0))
 
 /* These are all the "old" interrupts */
 #define ILK_BSD_USER_INTERRUPT				(1<<5)
@@ -7327,6 +7331,10 @@ enum {
 #define   AUD_CONFIG_UPPER_N_MASK		(0xff << 20)
 #define   AUD_CONFIG_LOWER_N_SHIFT		4
 #define   AUD_CONFIG_LOWER_N_MASK		(0xfff << 4)
+#define   AUD_CONFIG_N_MASK			(AUD_CONFIG_UPPER_N_MASK | AUD_CONFIG_LOWER_N_MASK)
+#define   AUD_CONFIG_N(n) \
+	(((((n) >> 12) & 0xff) << AUD_CONFIG_UPPER_N_SHIFT) |	\
+	 (((n) & 0xfff) << AUD_CONFIG_LOWER_N_SHIFT))
 #define   AUD_CONFIG_PIXEL_CLOCK_HDMI_SHIFT	16
 #define   AUD_CONFIG_PIXEL_CLOCK_HDMI_MASK	(0xf << 16)
 #define   AUD_CONFIG_PIXEL_CLOCK_HDMI_25175	(0 << 16)
diff --git a/drivers/gpu/drm/i915/i915_suspend.c b/drivers/gpu/drm/i915/i915_suspend.c
index a0af170062b1..344cbf39cfa9 100644
--- a/drivers/gpu/drm/i915/i915_suspend.c
+++ b/drivers/gpu/drm/i915/i915_suspend.c
@@ -38,7 +38,7 @@ static void i915_save_display(struct drm_device *dev)
 		dev_priv->regfile.saveDSPARB = I915_READ(DSPARB);
 
 	/* save FBC interval */
-	if (HAS_FBC(dev) && INTEL_INFO(dev)->gen <= 4 && !IS_G4X(dev))
+	if (HAS_FBC(dev_priv) && INTEL_GEN(dev_priv) <= 4 && !IS_G4X(dev_priv))
 		dev_priv->regfile.saveFBC_CONTROL = I915_READ(FBC_CONTROL);
 }
 
@@ -54,7 +54,7 @@ static void i915_restore_display(struct drm_device *dev)
 	intel_fbc_global_disable(dev_priv);
 
 	/* restore FBC interval */
-	if (HAS_FBC(dev) && INTEL_INFO(dev)->gen <= 4 && !IS_G4X(dev))
+	if (HAS_FBC(dev_priv) && INTEL_GEN(dev_priv) <= 4 && !IS_G4X(dev_priv))
 		I915_WRITE(FBC_CONTROL, dev_priv->regfile.saveFBC_CONTROL);
 
 	i915_redisable_vga(dev);
@@ -70,7 +70,7 @@ int i915_save_state(struct drm_device *dev)
 
 	i915_save_display(dev);
 
-	if (IS_GEN4(dev))
+	if (IS_GEN4(dev_priv))
 		pci_read_config_word(pdev, GCDGMBUS,
 				     &dev_priv->regfile.saveGCDGMBUS);
 
@@ -116,7 +116,7 @@ int i915_restore_state(struct drm_device *dev)
 
 	i915_gem_restore_fences(dev);
 
-	if (IS_GEN4(dev))
+	if (IS_GEN4(dev_priv))
 		pci_write_config_word(pdev, GCDGMBUS,
 				      dev_priv->regfile.saveGCDGMBUS);
 	i915_restore_display(dev);
diff --git a/drivers/gpu/drm/i915/i915_sysfs.c b/drivers/gpu/drm/i915/i915_sysfs.c
index 1012eeea1324..47590ab08d7e 100644
--- a/drivers/gpu/drm/i915/i915_sysfs.c
+++ b/drivers/gpu/drm/i915/i915_sysfs.c
@@ -514,6 +514,8 @@ static const struct attribute *vlv_attrs[] = {
 	NULL,
 };
 
+#if IS_ENABLED(CONFIG_DRM_I915_CAPTURE_ERROR)
+
 static ssize_t error_state_read(struct file *filp, struct kobject *kobj,
 				struct bin_attribute *attr, char *buf,
 				loff_t off, size_t count)
@@ -571,6 +573,21 @@ static struct bin_attribute error_state_attr = {
 	.write = error_state_write,
 };
 
+static void i915_setup_error_capture(struct device *kdev)
+{
+	if (sysfs_create_bin_file(&kdev->kobj, &error_state_attr))
+		DRM_ERROR("error_state sysfs setup failed\n");
+}
+
+static void i915_teardown_error_capture(struct device *kdev)
+{
+	sysfs_remove_bin_file(&kdev->kobj, &error_state_attr);
+}
+#else
+static void i915_setup_error_capture(struct device *kdev) {}
+static void i915_teardown_error_capture(struct device *kdev) {}
+#endif
+
 void i915_setup_sysfs(struct drm_i915_private *dev_priv)
 {
 	struct device *kdev = dev_priv->drm.primary->kdev;
@@ -617,17 +634,15 @@ void i915_setup_sysfs(struct drm_i915_private *dev_priv)
 	if (ret)
 		DRM_ERROR("RPS sysfs setup failed\n");
 
-	ret = sysfs_create_bin_file(&kdev->kobj,
-				    &error_state_attr);
-	if (ret)
-		DRM_ERROR("error_state sysfs setup failed\n");
+	i915_setup_error_capture(kdev);
 }
 
 void i915_teardown_sysfs(struct drm_i915_private *dev_priv)
 {
 	struct device *kdev = dev_priv->drm.primary->kdev;
 
-	sysfs_remove_bin_file(&kdev->kobj, &error_state_attr);
+	i915_teardown_error_capture(kdev);
+
 	if (IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv))
 		sysfs_remove_files(&kdev->kobj, vlv_attrs);
 	else
diff --git a/drivers/gpu/drm/i915/intel_audio.c b/drivers/gpu/drm/i915/intel_audio.c
index 6c70a5bfd7d8..7093cfbb62b1 100644
--- a/drivers/gpu/drm/i915/intel_audio.c
+++ b/drivers/gpu/drm/i915/intel_audio.c
@@ -81,7 +81,7 @@ static const struct {
 	int clock;
 	int n;
 	int cts;
-} aud_ncts[] = {
+} hdmi_aud_ncts[] = {
 	{ 44100, TMDS_296M, 4459, 234375 },
 	{ 44100, TMDS_297M, 4704, 247500 },
 	{ 48000, TMDS_296M, 5824, 281250 },
@@ -121,45 +121,20 @@ static u32 audio_config_hdmi_pixel_clock(const struct drm_display_mode *adjusted
 	return hdmi_audio_clock[i].config;
 }
 
-static int audio_config_get_n(const struct drm_display_mode *mode, int rate)
+static int audio_config_hdmi_get_n(const struct drm_display_mode *adjusted_mode,
+				   int rate)
 {
 	int i;
 
-	for (i = 0; i < ARRAY_SIZE(aud_ncts); i++) {
-		if ((rate == aud_ncts[i].sample_rate) &&
-			(mode->clock == aud_ncts[i].clock)) {
-			return aud_ncts[i].n;
+	for (i = 0; i < ARRAY_SIZE(hdmi_aud_ncts); i++) {
+		if (rate == hdmi_aud_ncts[i].sample_rate &&
+		    adjusted_mode->crtc_clock == hdmi_aud_ncts[i].clock) {
+			return hdmi_aud_ncts[i].n;
 		}
 	}
 	return 0;
 }
 
-static uint32_t audio_config_setup_n_reg(int n, uint32_t val)
-{
-	int n_low, n_up;
-	uint32_t tmp = val;
-
-	n_low = n & 0xfff;
-	n_up = (n >> 12) & 0xff;
-	tmp &= ~(AUD_CONFIG_UPPER_N_MASK | AUD_CONFIG_LOWER_N_MASK);
-	tmp |= ((n_up << AUD_CONFIG_UPPER_N_SHIFT) |
-			(n_low << AUD_CONFIG_LOWER_N_SHIFT) |
-			AUD_CONFIG_N_PROG_ENABLE);
-	return tmp;
-}
-
-/* check whether N/CTS/M need be set manually */
-static bool audio_rate_need_prog(struct intel_crtc *crtc,
-				 const struct drm_display_mode *mode)
-{
-	if (((mode->clock == TMDS_297M) ||
-		 (mode->clock == TMDS_296M)) &&
-		intel_crtc_has_type(crtc->config, INTEL_OUTPUT_HDMI))
-		return true;
-	else
-		return false;
-}
-
 static bool intel_eld_uptodate(struct drm_connector *connector,
 			       i915_reg_t reg_eldv, uint32_t bits_eldv,
 			       i915_reg_t reg_elda, uint32_t bits_elda,
@@ -245,6 +220,65 @@ static void g4x_audio_codec_enable(struct drm_connector *connector,
 	I915_WRITE(G4X_AUD_CNTL_ST, tmp);
 }
 
+static void
+hsw_dp_audio_config_update(struct intel_crtc *intel_crtc, enum port port,
+			   const struct drm_display_mode *adjusted_mode)
+{
+	struct drm_i915_private *dev_priv = to_i915(intel_crtc->base.dev);
+	enum pipe pipe = intel_crtc->pipe;
+	u32 tmp;
+
+	tmp = I915_READ(HSW_AUD_CFG(pipe));
+	tmp &= ~AUD_CONFIG_N_VALUE_INDEX;
+	tmp &= ~AUD_CONFIG_PIXEL_CLOCK_HDMI_MASK;
+	tmp &= ~AUD_CONFIG_N_PROG_ENABLE;
+	tmp |= AUD_CONFIG_N_VALUE_INDEX;
+
+	I915_WRITE(HSW_AUD_CFG(pipe), tmp);
+}
+
+static void
+hsw_hdmi_audio_config_update(struct intel_crtc *intel_crtc, enum port port,
+			     const struct drm_display_mode *adjusted_mode)
+{
+	struct drm_i915_private *dev_priv = to_i915(intel_crtc->base.dev);
+	struct i915_audio_component *acomp = dev_priv->audio_component;
+	int rate = acomp ? acomp->aud_sample_rate[port] : 0;
+	enum pipe pipe = intel_crtc->pipe;
+	int n;
+	u32 tmp;
+
+	tmp = I915_READ(HSW_AUD_CFG(pipe));
+	tmp &= ~AUD_CONFIG_N_VALUE_INDEX;
+	tmp &= ~AUD_CONFIG_PIXEL_CLOCK_HDMI_MASK;
+	tmp &= ~AUD_CONFIG_N_PROG_ENABLE;
+	tmp |= audio_config_hdmi_pixel_clock(adjusted_mode);
+
+	if (adjusted_mode->crtc_clock == TMDS_296M ||
+	    adjusted_mode->crtc_clock == TMDS_297M) {
+		n = audio_config_hdmi_get_n(adjusted_mode, rate);
+		if (n != 0) {
+			tmp &= ~AUD_CONFIG_N_MASK;
+			tmp |= AUD_CONFIG_N(n);
+			tmp |= AUD_CONFIG_N_PROG_ENABLE;
+		} else {
+			DRM_DEBUG_KMS("no suitable N value is found\n");
+		}
+	}
+
+	I915_WRITE(HSW_AUD_CFG(pipe), tmp);
+}
+
+static void
+hsw_audio_config_update(struct intel_crtc *intel_crtc, enum port port,
+			const struct drm_display_mode *adjusted_mode)
+{
+	if (intel_crtc_has_dp_encoder(intel_crtc->config))
+		hsw_dp_audio_config_update(intel_crtc, port, adjusted_mode);
+	else
+		hsw_hdmi_audio_config_update(intel_crtc, port, adjusted_mode);
+}
+
 static void hsw_audio_codec_disable(struct intel_encoder *encoder)
 {
 	struct drm_i915_private *dev_priv = to_i915(encoder->base.dev);
@@ -276,20 +310,16 @@ static void hsw_audio_codec_disable(struct intel_encoder *encoder)
 }
 
 static void hsw_audio_codec_enable(struct drm_connector *connector,
-				   struct intel_encoder *encoder,
+				   struct intel_encoder *intel_encoder,
 				   const struct drm_display_mode *adjusted_mode)
 {
 	struct drm_i915_private *dev_priv = to_i915(connector->dev);
-	struct intel_crtc *intel_crtc = to_intel_crtc(encoder->base.crtc);
+	struct intel_crtc *intel_crtc = to_intel_crtc(intel_encoder->base.crtc);
 	enum pipe pipe = intel_crtc->pipe;
-	struct i915_audio_component *acomp = dev_priv->audio_component;
+	enum port port = intel_encoder->port;
 	const uint8_t *eld = connector->eld;
-	struct intel_digital_port *intel_dig_port =
-		enc_to_dig_port(&encoder->base);
-	enum port port = intel_dig_port->port;
 	uint32_t tmp;
 	int len, i;
-	int n, rate;
 
 	DRM_DEBUG_KMS("Enable audio codec on pipe %c, %u bytes ELD\n",
 		      pipe_name(pipe), drm_eld_size(eld));
@@ -325,42 +355,17 @@ static void hsw_audio_codec_enable(struct drm_connector *connector,
 	I915_WRITE(HSW_AUD_PIN_ELD_CP_VLD, tmp);
 
 	/* Enable timestamps */
-	tmp = I915_READ(HSW_AUD_CFG(pipe));
-	tmp &= ~AUD_CONFIG_N_VALUE_INDEX;
-	tmp &= ~AUD_CONFIG_PIXEL_CLOCK_HDMI_MASK;
-	if (intel_crtc_has_dp_encoder(intel_crtc->config))
-		tmp |= AUD_CONFIG_N_VALUE_INDEX;
-	else
-		tmp |= audio_config_hdmi_pixel_clock(adjusted_mode);
-
-	tmp &= ~AUD_CONFIG_N_PROG_ENABLE;
-	if (audio_rate_need_prog(intel_crtc, adjusted_mode)) {
-		if (!acomp)
-			rate = 0;
-		else if (port >= PORT_A && port <= PORT_E)
-			rate = acomp->aud_sample_rate[port];
-		else {
-			DRM_ERROR("invalid port: %d\n", port);
-			rate = 0;
-		}
-		n = audio_config_get_n(adjusted_mode, rate);
-		if (n != 0)
-			tmp = audio_config_setup_n_reg(n, tmp);
-		else
-			DRM_DEBUG_KMS("no suitable N value is found\n");
-	}
-
-	I915_WRITE(HSW_AUD_CFG(pipe), tmp);
+	hsw_audio_config_update(intel_crtc, port, adjusted_mode);
 
 	mutex_unlock(&dev_priv->av_mutex);
 }
 
-static void ilk_audio_codec_disable(struct intel_encoder *encoder)
+static void ilk_audio_codec_disable(struct intel_encoder *intel_encoder)
 {
-	struct drm_i915_private *dev_priv = to_i915(encoder->base.dev);
-	struct intel_crtc *intel_crtc = to_intel_crtc(encoder->base.crtc);
-	enum port port = enc_to_dig_port(&encoder->base)->port;
+	struct drm_i915_private *dev_priv = to_i915(intel_encoder->base.dev);
+	struct intel_crtc *intel_crtc = to_intel_crtc(intel_encoder->base.crtc);
 	enum pipe pipe = intel_crtc->pipe;
+	enum port port = intel_encoder->port;
 	uint32_t tmp, eldv;
 	i915_reg_t aud_config, aud_cntrl_st2;
 
@@ -400,13 +405,13 @@ static void ilk_audio_codec_disable(struct intel_encoder *encoder)
 }
 
 static void ilk_audio_codec_enable(struct drm_connector *connector,
-				   struct intel_encoder *encoder,
+				   struct intel_encoder *intel_encoder,
 				   const struct drm_display_mode *adjusted_mode)
 {
 	struct drm_i915_private *dev_priv = to_i915(connector->dev);
-	struct intel_crtc *intel_crtc = to_intel_crtc(encoder->base.crtc);
-	enum port port = enc_to_dig_port(&encoder->base)->port;
+	struct intel_crtc *intel_crtc = to_intel_crtc(intel_encoder->base.crtc);
 	enum pipe pipe = intel_crtc->pipe;
+	enum port port = intel_encoder->port;
 	uint8_t *eld = connector->eld;
 	uint32_t tmp, eldv;
 	int len, i;
@@ -425,13 +430,13 @@ static void ilk_audio_codec_enable(struct drm_connector *connector,
 	 * infrastructure is not there yet.
 	 */
 
-	if (HAS_PCH_IBX(connector->dev)) {
+	if (HAS_PCH_IBX(dev_priv)) {
 		hdmiw_hdmiedid = IBX_HDMIW_HDMIEDID(pipe);
 		aud_config = IBX_AUD_CFG(pipe);
 		aud_cntl_st = IBX_AUD_CNTL_ST(pipe);
 		aud_cntrl_st2 = IBX_AUD_CNTL_ST2;
-	} else if (IS_VALLEYVIEW(connector->dev) ||
-		   IS_CHERRYVIEW(connector->dev)) {
+	} else if (IS_VALLEYVIEW(dev_priv) ||
+		   IS_CHERRYVIEW(dev_priv)) {
 		hdmiw_hdmiedid = VLV_HDMIW_HDMIEDID(pipe);
 		aud_config = VLV_AUD_CFG(pipe);
 		aud_cntl_st = VLV_AUD_CNTL_ST(pipe);
@@ -490,11 +495,10 @@ void intel_audio_codec_enable(struct intel_encoder *intel_encoder)
 	struct intel_crtc *crtc = to_intel_crtc(encoder->crtc);
 	const struct drm_display_mode *adjusted_mode = &crtc->config->base.adjusted_mode;
 	struct drm_connector *connector;
-	struct drm_device *dev = encoder->dev;
-	struct drm_i915_private *dev_priv = to_i915(dev);
+	struct drm_i915_private *dev_priv = to_i915(encoder->dev);
 	struct i915_audio_component *acomp = dev_priv->audio_component;
-	struct intel_digital_port *intel_dig_port = enc_to_dig_port(encoder);
-	enum port port = intel_dig_port->port;
+	enum port port = intel_encoder->port;
+	enum pipe pipe = crtc->pipe;
 
 	connector = drm_select_eld(encoder);
 	if (!connector)
@@ -518,13 +522,19 @@ void intel_audio_codec_enable(struct intel_encoder *intel_encoder)
 						     adjusted_mode);
 
 	mutex_lock(&dev_priv->av_mutex);
-	intel_dig_port->audio_connector = connector;
+	intel_encoder->audio_connector = connector;
+
 	/* referred in audio callbacks */
-	dev_priv->dig_port_map[port] = intel_encoder;
+	dev_priv->av_enc_map[pipe] = intel_encoder;
 	mutex_unlock(&dev_priv->av_mutex);
 
+	/* audio drivers expect pipe = -1 to indicate Non-MST cases */
+	if (intel_encoder->type != INTEL_OUTPUT_DP_MST)
+		pipe = -1;
+
 	if (acomp && acomp->audio_ops && acomp->audio_ops->pin_eld_notify)
-		acomp->audio_ops->pin_eld_notify(acomp->audio_ops->audio_ptr, (int) port);
+		acomp->audio_ops->pin_eld_notify(acomp->audio_ops->audio_ptr,
+						 (int) port, (int) pipe);
 }
 
 /**
@@ -537,22 +547,27 @@ void intel_audio_codec_enable(struct intel_encoder *intel_encoder)
 void intel_audio_codec_disable(struct intel_encoder *intel_encoder)
 {
 	struct drm_encoder *encoder = &intel_encoder->base;
-	struct drm_device *dev = encoder->dev;
-	struct drm_i915_private *dev_priv = to_i915(dev);
+	struct drm_i915_private *dev_priv = to_i915(encoder->dev);
 	struct i915_audio_component *acomp = dev_priv->audio_component;
-	struct intel_digital_port *intel_dig_port = enc_to_dig_port(encoder);
-	enum port port = intel_dig_port->port;
+	enum port port = intel_encoder->port;
+	struct intel_crtc *crtc = to_intel_crtc(encoder->crtc);
+	enum pipe pipe = crtc->pipe;
 
 	if (dev_priv->display.audio_codec_disable)
 		dev_priv->display.audio_codec_disable(intel_encoder);
 
 	mutex_lock(&dev_priv->av_mutex);
-	intel_dig_port->audio_connector = NULL;
-	dev_priv->dig_port_map[port] = NULL;
+	intel_encoder->audio_connector = NULL;
+	dev_priv->av_enc_map[pipe] = NULL;
 	mutex_unlock(&dev_priv->av_mutex);
 
+	/* audio drivers expect pipe = -1 to indicate Non-MST cases */
+	if (intel_encoder->type != INTEL_OUTPUT_DP_MST)
+		pipe = -1;
+
 	if (acomp && acomp->audio_ops && acomp->audio_ops->pin_eld_notify)
-		acomp->audio_ops->pin_eld_notify(acomp->audio_ops->audio_ptr, (int) port);
+		acomp->audio_ops->pin_eld_notify(acomp->audio_ops->audio_ptr,
+						 (int) port, (int) pipe);
 }
 
 /**
@@ -627,74 +642,67 @@ static int i915_audio_component_get_cdclk_freq(struct device *kdev)
 	return dev_priv->cdclk_freq;
 }
 
-static int i915_audio_component_sync_audio_rate(struct device *kdev,
-						int port, int rate)
+static struct intel_encoder *get_saved_enc(struct drm_i915_private *dev_priv,
+					       int port, int pipe)
+{
+
+	if (WARN_ON(pipe >= I915_MAX_PIPES))
+		return NULL;
+
+	/* MST */
+	if (pipe >= 0)
+		return dev_priv->av_enc_map[pipe];
+
+	/* Non-MST */
+	for_each_pipe(dev_priv, pipe) {
+		struct intel_encoder *encoder;
+
+		encoder = dev_priv->av_enc_map[pipe];
+		if (encoder == NULL)
+			continue;
+
+		if (port == encoder->port)
+			return encoder;
+	}
+
+	return NULL;
+}
+
+static int i915_audio_component_sync_audio_rate(struct device *kdev, int port,
+						int pipe, int rate)
 {
 	struct drm_i915_private *dev_priv = kdev_to_i915(kdev);
 	struct intel_encoder *intel_encoder;
 	struct intel_crtc *crtc;
-	struct drm_display_mode *mode;
+	struct drm_display_mode *adjusted_mode;
 	struct i915_audio_component *acomp = dev_priv->audio_component;
-	enum pipe pipe = INVALID_PIPE;
-	u32 tmp;
-	int n;
 	int err = 0;
 
-	/* HSW, BDW, SKL, KBL need this fix */
-	if (!IS_SKYLAKE(dev_priv) &&
-	    !IS_KABYLAKE(dev_priv) &&
-	    !IS_BROADWELL(dev_priv) &&
-	    !IS_HASWELL(dev_priv))
+	if (!HAS_DDI(dev_priv))
 		return 0;
 
 	i915_audio_component_get_power(kdev);
 	mutex_lock(&dev_priv->av_mutex);
+
 	/* 1. get the pipe */
-	intel_encoder = dev_priv->dig_port_map[port];
-	/* intel_encoder might be NULL for DP MST */
+	intel_encoder = get_saved_enc(dev_priv, port, pipe);
 	if (!intel_encoder || !intel_encoder->base.crtc ||
 	    intel_encoder->type != INTEL_OUTPUT_HDMI) {
-		DRM_DEBUG_KMS("no valid port %c\n", port_name(port));
+		DRM_DEBUG_KMS("Not valid for port %c\n", port_name(port));
 		err = -ENODEV;
 		goto unlock;
 	}
+
+	/* pipe passed from the audio driver will be -1 for Non-MST case */
 	crtc = to_intel_crtc(intel_encoder->base.crtc);
 	pipe = crtc->pipe;
-	if (pipe == INVALID_PIPE) {
-		DRM_DEBUG_KMS("no pipe for the port %c\n", port_name(port));
-		err = -ENODEV;
-		goto unlock;
-	}
 
-	DRM_DEBUG_KMS("pipe %c connects port %c\n",
-				  pipe_name(pipe), port_name(port));
-	mode = &crtc->config->base.adjusted_mode;
+	adjusted_mode = &crtc->config->base.adjusted_mode;
 
 	/* port must be valid now, otherwise the pipe will be invalid */
 	acomp->aud_sample_rate[port] = rate;
 
-	/* 2. check whether to set the N/CTS/M manually or not */
-	if (!audio_rate_need_prog(crtc, mode)) {
-		tmp = I915_READ(HSW_AUD_CFG(pipe));
-		tmp &= ~AUD_CONFIG_N_PROG_ENABLE;
-		I915_WRITE(HSW_AUD_CFG(pipe), tmp);
-		goto unlock;
-	}
-
-	n = audio_config_get_n(mode, rate);
-	if (n == 0) {
-		DRM_DEBUG_KMS("Using automatic mode for N value on port %c\n",
-					  port_name(port));
-		tmp = I915_READ(HSW_AUD_CFG(pipe));
-		tmp &= ~AUD_CONFIG_N_PROG_ENABLE;
-		I915_WRITE(HSW_AUD_CFG(pipe), tmp);
-		goto unlock;
-	}
-
-	/* 3. set the N/CTS/M */
-	tmp = I915_READ(HSW_AUD_CFG(pipe));
-	tmp = audio_config_setup_n_reg(n, tmp);
-	I915_WRITE(HSW_AUD_CFG(pipe), tmp);
+	hsw_audio_config_update(crtc, port, adjusted_mode);
 
  unlock:
 	mutex_unlock(&dev_priv->av_mutex);
@@ -703,27 +711,29 @@ static int i915_audio_component_sync_audio_rate(struct device *kdev,
 }
 
 static int i915_audio_component_get_eld(struct device *kdev, int port,
-					bool *enabled,
+					int pipe, bool *enabled,
 					unsigned char *buf, int max_bytes)
 {
 	struct drm_i915_private *dev_priv = kdev_to_i915(kdev);
 	struct intel_encoder *intel_encoder;
-	struct intel_digital_port *intel_dig_port;
 	const u8 *eld;
 	int ret = -EINVAL;
 
 	mutex_lock(&dev_priv->av_mutex);
-	intel_encoder = dev_priv->dig_port_map[port];
-	/* intel_encoder might be NULL for DP MST */
-	if (intel_encoder) {
-		ret = 0;
-		intel_dig_port = enc_to_dig_port(&intel_encoder->base);
-		*enabled = intel_dig_port->audio_connector != NULL;
-		if (*enabled) {
-			eld = intel_dig_port->audio_connector->eld;
-			ret = drm_eld_size(eld);
-			memcpy(buf, eld, min(max_bytes, ret));
-		}
+
+	intel_encoder = get_saved_enc(dev_priv, port, pipe);
+	if (!intel_encoder) {
+		DRM_DEBUG_KMS("Not valid for port %c\n", port_name(port));
+		mutex_unlock(&dev_priv->av_mutex);
+		return ret;
+	}
+
+	ret = 0;
+	*enabled = intel_encoder->audio_connector != NULL;
+	if (*enabled) {
+		eld = intel_encoder->audio_connector->eld;
+		ret = drm_eld_size(eld);
+		memcpy(buf, eld, min(max_bytes, ret));
 	}
 
 	mutex_unlock(&dev_priv->av_mutex);
diff --git a/drivers/gpu/drm/i915/intel_bios.c b/drivers/gpu/drm/i915/intel_bios.c
index c6e69e4cfa83..5ab646ef8c9f 100644
--- a/drivers/gpu/drm/i915/intel_bios.c
+++ b/drivers/gpu/drm/i915/intel_bios.c
@@ -996,6 +996,10 @@ parse_mipi_sequence(struct drm_i915_private *dev_priv,
 			goto err;
 		}
 
+		/* Log about presence of sequences we won't run. */
+		if (seq_id == MIPI_SEQ_TEAR_ON || seq_id == MIPI_SEQ_TEAR_OFF)
+			DRM_DEBUG_KMS("Unsupported sequence %u\n", seq_id);
+
 		dev_priv->vbt.dsi.sequence[seq_id] = data + index;
 
 		if (sequence->version >= 3)
@@ -1031,6 +1035,77 @@ static u8 translate_iboost(u8 val)
 	return mapping[val];
 }
 
+static void sanitize_ddc_pin(struct drm_i915_private *dev_priv,
+			     enum port port)
+{
+	const struct ddi_vbt_port_info *info =
+		&dev_priv->vbt.ddi_port_info[port];
+	enum port p;
+
+	if (!info->alternate_ddc_pin)
+		return;
+
+	for_each_port_masked(p, (1 << port) - 1) {
+		struct ddi_vbt_port_info *i = &dev_priv->vbt.ddi_port_info[p];
+
+		if (info->alternate_ddc_pin != i->alternate_ddc_pin)
+			continue;
+
+		DRM_DEBUG_KMS("port %c trying to use the same DDC pin (0x%x) as port %c, "
+			      "disabling port %c DVI/HDMI support\n",
+			      port_name(p), i->alternate_ddc_pin,
+			      port_name(port), port_name(p));
+
+		/*
+		 * If we have multiple ports supposedly sharing the
+		 * pin, then dvi/hdmi couldn't exist on the shared
+		 * port. Otherwise they share the same ddc bin and
+		 * system couldn't communicate with them separately.
+		 *
+		 * Due to parsing the ports in alphabetical order,
+		 * a higher port will always clobber a lower one.
+		 */
+		i->supports_dvi = false;
+		i->supports_hdmi = false;
+		i->alternate_ddc_pin = 0;
+	}
+}
+
+static void sanitize_aux_ch(struct drm_i915_private *dev_priv,
+			    enum port port)
+{
+	const struct ddi_vbt_port_info *info =
+		&dev_priv->vbt.ddi_port_info[port];
+	enum port p;
+
+	if (!info->alternate_aux_channel)
+		return;
+
+	for_each_port_masked(p, (1 << port) - 1) {
+		struct ddi_vbt_port_info *i = &dev_priv->vbt.ddi_port_info[p];
+
+		if (info->alternate_aux_channel != i->alternate_aux_channel)
+			continue;
+
+		DRM_DEBUG_KMS("port %c trying to use the same AUX CH (0x%x) as port %c, "
+			      "disabling port %c DP support\n",
+			      port_name(p), i->alternate_aux_channel,
+			      port_name(port), port_name(p));
+
+		/*
+		 * If we have multiple ports supposedlt sharing the
+		 * aux channel, then DP couldn't exist on the shared
+		 * port. Otherwise they share the same aux channel
+		 * and system couldn't communicate with them separately.
+		 *
+		 * Due to parsing the ports in alphabetical order,
+		 * a higher port will always clobber a lower one.
+		 */
+		i->supports_dp = false;
+		i->alternate_aux_channel = 0;
+	}
+}
+
 static void parse_ddi_port(struct drm_i915_private *dev_priv, enum port port,
 			   const struct bdb_header *bdb)
 {
@@ -1105,54 +1180,15 @@ static void parse_ddi_port(struct drm_i915_private *dev_priv, enum port port,
 		DRM_DEBUG_KMS("Port %c is internal DP\n", port_name(port));
 
 	if (is_dvi) {
-		if (port == PORT_E) {
-			info->alternate_ddc_pin = ddc_pin;
-			/* if DDIE share ddc pin with other port, then
-			 * dvi/hdmi couldn't exist on the shared port.
-			 * Otherwise they share the same ddc bin and system
-			 * couldn't communicate with them seperately. */
-			if (ddc_pin == DDC_PIN_B) {
-				dev_priv->vbt.ddi_port_info[PORT_B].supports_dvi = 0;
-				dev_priv->vbt.ddi_port_info[PORT_B].supports_hdmi = 0;
-			} else if (ddc_pin == DDC_PIN_C) {
-				dev_priv->vbt.ddi_port_info[PORT_C].supports_dvi = 0;
-				dev_priv->vbt.ddi_port_info[PORT_C].supports_hdmi = 0;
-			} else if (ddc_pin == DDC_PIN_D) {
-				dev_priv->vbt.ddi_port_info[PORT_D].supports_dvi = 0;
-				dev_priv->vbt.ddi_port_info[PORT_D].supports_hdmi = 0;
-			}
-		} else if (ddc_pin == DDC_PIN_B && port != PORT_B)
-			DRM_DEBUG_KMS("Unexpected DDC pin for port B\n");
-		else if (ddc_pin == DDC_PIN_C && port != PORT_C)
-			DRM_DEBUG_KMS("Unexpected DDC pin for port C\n");
-		else if (ddc_pin == DDC_PIN_D && port != PORT_D)
-			DRM_DEBUG_KMS("Unexpected DDC pin for port D\n");
+		info->alternate_ddc_pin = ddc_pin;
+
+		sanitize_ddc_pin(dev_priv, port);
 	}
 
 	if (is_dp) {
-		if (port == PORT_E) {
-			info->alternate_aux_channel = aux_channel;
-			/* if DDIE share aux channel with other port, then
-			 * DP couldn't exist on the shared port. Otherwise
-			 * they share the same aux channel and system
-			 * couldn't communicate with them seperately. */
-			if (aux_channel == DP_AUX_A)
-				dev_priv->vbt.ddi_port_info[PORT_A].supports_dp = 0;
-			else if (aux_channel == DP_AUX_B)
-				dev_priv->vbt.ddi_port_info[PORT_B].supports_dp = 0;
-			else if (aux_channel == DP_AUX_C)
-				dev_priv->vbt.ddi_port_info[PORT_C].supports_dp = 0;
-			else if (aux_channel == DP_AUX_D)
-				dev_priv->vbt.ddi_port_info[PORT_D].supports_dp = 0;
-		}
-		else if (aux_channel == DP_AUX_A && port != PORT_A)
-			DRM_DEBUG_KMS("Unexpected AUX channel for port A\n");
-		else if (aux_channel == DP_AUX_B && port != PORT_B)
-			DRM_DEBUG_KMS("Unexpected AUX channel for port B\n");
-		else if (aux_channel == DP_AUX_C && port != PORT_C)
-			DRM_DEBUG_KMS("Unexpected AUX channel for port C\n");
-		else if (aux_channel == DP_AUX_D && port != PORT_D)
-			DRM_DEBUG_KMS("Unexpected AUX channel for port D\n");
+		info->alternate_aux_channel = aux_channel;
+
+		sanitize_aux_ch(dev_priv, port);
 	}
 
 	if (bdb->version >= 158) {
@@ -1759,3 +1795,52 @@ intel_bios_is_port_hpd_inverted(struct drm_i915_private *dev_priv,
 
 	return false;
 }
+
+/**
+ * intel_bios_is_lspcon_present - if LSPCON is attached on %port
+ * @dev_priv:	i915 device instance
+ * @port:	port to check
+ *
+ * Return true if LSPCON is present on this port
+ */
+bool
+intel_bios_is_lspcon_present(struct drm_i915_private *dev_priv,
+				enum port port)
+{
+	int i;
+
+	if (!HAS_LSPCON(dev_priv))
+		return false;
+
+	for (i = 0; i < dev_priv->vbt.child_dev_num; i++) {
+		if (!dev_priv->vbt.child_dev[i].common.lspcon)
+			continue;
+
+		switch (dev_priv->vbt.child_dev[i].common.dvo_port) {
+		case DVO_PORT_DPA:
+		case DVO_PORT_HDMIA:
+			if (port == PORT_A)
+				return true;
+			break;
+		case DVO_PORT_DPB:
+		case DVO_PORT_HDMIB:
+			if (port == PORT_B)
+				return true;
+			break;
+		case DVO_PORT_DPC:
+		case DVO_PORT_HDMIC:
+			if (port == PORT_C)
+				return true;
+			break;
+		case DVO_PORT_DPD:
+		case DVO_PORT_HDMID:
+			if (port == PORT_D)
+				return true;
+			break;
+		default:
+			break;
+		}
+	}
+
+	return false;
+}
diff --git a/drivers/gpu/drm/i915/intel_breadcrumbs.c b/drivers/gpu/drm/i915/intel_breadcrumbs.c
index 9bad14d22c95..23fc1042fed4 100644
--- a/drivers/gpu/drm/i915/intel_breadcrumbs.c
+++ b/drivers/gpu/drm/i915/intel_breadcrumbs.c
@@ -578,6 +578,36 @@ int intel_engine_init_breadcrumbs(struct intel_engine_cs *engine)
 	return 0;
 }
 
+static void cancel_fake_irq(struct intel_engine_cs *engine)
+{
+	struct intel_breadcrumbs *b = &engine->breadcrumbs;
+
+	del_timer_sync(&b->hangcheck);
+	del_timer_sync(&b->fake_irq);
+	clear_bit(engine->id, &engine->i915->gpu_error.missed_irq_rings);
+}
+
+void intel_engine_reset_breadcrumbs(struct intel_engine_cs *engine)
+{
+	struct intel_breadcrumbs *b = &engine->breadcrumbs;
+
+	cancel_fake_irq(engine);
+	spin_lock(&b->lock);
+
+	__intel_breadcrumbs_disable_irq(b);
+	if (intel_engine_has_waiter(engine)) {
+		b->timeout = wait_timeout();
+		__intel_breadcrumbs_enable_irq(b);
+		if (READ_ONCE(b->irq_posted))
+			wake_up_process(b->first_wait->tsk);
+	} else {
+		/* sanitize the IMR and unmask any auxiliary interrupts */
+		irq_disable(engine);
+	}
+
+	spin_unlock(&b->lock);
+}
+
 void intel_engine_fini_breadcrumbs(struct intel_engine_cs *engine)
 {
 	struct intel_breadcrumbs *b = &engine->breadcrumbs;
@@ -585,13 +615,13 @@ void intel_engine_fini_breadcrumbs(struct intel_engine_cs *engine)
 	if (!IS_ERR_OR_NULL(b->signaler))
 		kthread_stop(b->signaler);
 
-	del_timer_sync(&b->hangcheck);
-	del_timer_sync(&b->fake_irq);
+	cancel_fake_irq(engine);
 }
 
 unsigned int intel_kick_waiters(struct drm_i915_private *i915)
 {
 	struct intel_engine_cs *engine;
+	enum intel_engine_id id;
 	unsigned int mask = 0;
 
 	/* To avoid the task_struct disappearing beneath us as we wake up
@@ -599,7 +629,7 @@ unsigned int intel_kick_waiters(struct drm_i915_private *i915)
 	 * RCU lock, i.e. as we call wake_up_process() we must be holding the
 	 * rcu_read_lock().
 	 */
-	for_each_engine(engine, i915)
+	for_each_engine(engine, i915, id)
 		if (unlikely(intel_engine_wakeup(engine)))
 			mask |= intel_engine_flag(engine);
 
@@ -609,9 +639,10 @@ unsigned int intel_kick_waiters(struct drm_i915_private *i915)
 unsigned int intel_kick_signalers(struct drm_i915_private *i915)
 {
 	struct intel_engine_cs *engine;
+	enum intel_engine_id id;
 	unsigned int mask = 0;
 
-	for_each_engine(engine, i915) {
+	for_each_engine(engine, i915, id) {
 		if (unlikely(READ_ONCE(engine->breadcrumbs.first_signal))) {
 			wake_up_process(engine->breadcrumbs.signaler);
 			mask |= intel_engine_flag(engine);
diff --git a/drivers/gpu/drm/i915/intel_color.c b/drivers/gpu/drm/i915/intel_color.c
index 95a72771eea6..445108855275 100644
--- a/drivers/gpu/drm/i915/intel_color.c
+++ b/drivers/gpu/drm/i915/intel_color.c
@@ -273,7 +273,7 @@ static void i9xx_load_luts_internal(struct drm_crtc *crtc,
 	enum pipe pipe = intel_crtc->pipe;
 	int i;
 
-	if (HAS_GMCH_DISPLAY(dev)) {
+	if (HAS_GMCH_DISPLAY(dev_priv)) {
 		if (intel_crtc_has_type(crtc_state, INTEL_OUTPUT_DSI))
 			assert_dsi_pll_enabled(dev_priv);
 		else
@@ -288,7 +288,7 @@ static void i9xx_load_luts_internal(struct drm_crtc *crtc,
 				(drm_color_lut_extract(lut[i].green, 8) << 8) |
 				drm_color_lut_extract(lut[i].blue, 8);
 
-			if (HAS_GMCH_DISPLAY(dev))
+			if (HAS_GMCH_DISPLAY(dev_priv))
 				I915_WRITE(PALETTE(pipe, i), word);
 			else
 				I915_WRITE(LGC_PALETTE(pipe, i), word);
@@ -297,7 +297,7 @@ static void i9xx_load_luts_internal(struct drm_crtc *crtc,
 		for (i = 0; i < 256; i++) {
 			uint32_t word = (i << 16) | (i << 8) | i;
 
-			if (HAS_GMCH_DISPLAY(dev))
+			if (HAS_GMCH_DISPLAY(dev_priv))
 				I915_WRITE(PALETTE(pipe, i), word);
 			else
 				I915_WRITE(LGC_PALETTE(pipe, i), word);
@@ -326,7 +326,7 @@ static void haswell_load_luts(struct drm_crtc_state *crtc_state)
 	 * Workaround : Do not read or write the pipe palette/gamma data while
 	 * GAMMA_MODE is configured for split gamma and IPS_CTL has IPS enabled.
 	 */
-	if (IS_HASWELL(dev) && intel_crtc_state->ips_enabled &&
+	if (IS_HASWELL(dev_priv) && intel_crtc_state->ips_enabled &&
 	    (intel_crtc_state->gamma_mode == GAMMA_MODE_MODE_SPLIT)) {
 		hsw_disable_ips(intel_crtc);
 		reenable_ips = true;
@@ -534,14 +534,14 @@ void intel_color_init(struct drm_crtc *crtc)
 
 	drm_mode_crtc_set_gamma_size(crtc, 256);
 
-	if (IS_CHERRYVIEW(dev)) {
+	if (IS_CHERRYVIEW(dev_priv)) {
 		dev_priv->display.load_csc_matrix = cherryview_load_csc_matrix;
 		dev_priv->display.load_luts = cherryview_load_luts;
-	} else if (IS_HASWELL(dev)) {
+	} else if (IS_HASWELL(dev_priv)) {
 		dev_priv->display.load_csc_matrix = i9xx_load_csc_matrix;
 		dev_priv->display.load_luts = haswell_load_luts;
-	} else if (IS_BROADWELL(dev) || IS_SKYLAKE(dev) ||
-		   IS_BROXTON(dev) || IS_KABYLAKE(dev)) {
+	} else if (IS_BROADWELL(dev_priv) || IS_SKYLAKE(dev_priv) ||
+		   IS_BROXTON(dev_priv) || IS_KABYLAKE(dev_priv)) {
 		dev_priv->display.load_csc_matrix = i9xx_load_csc_matrix;
 		dev_priv->display.load_luts = broadwell_load_luts;
 	} else {
diff --git a/drivers/gpu/drm/i915/intel_crt.c b/drivers/gpu/drm/i915/intel_crt.c
index dfbcf16b41df..a97151fcb9f4 100644
--- a/drivers/gpu/drm/i915/intel_crt.c
+++ b/drivers/gpu/drm/i915/intel_crt.c
@@ -84,7 +84,7 @@ static bool intel_crt_get_hw_state(struct intel_encoder *encoder,
 	if (!(tmp & ADPA_DAC_ENABLE))
 		goto out;
 
-	if (HAS_PCH_CPT(dev))
+	if (HAS_PCH_CPT(dev_priv))
 		*pipe = PORT_TO_PIPE_CPT(tmp);
 	else
 		*pipe = PORT_TO_PIPE(tmp);
@@ -165,16 +165,16 @@ static void intel_crt_set_dpms(struct intel_encoder *encoder,
 		adpa |= ADPA_VSYNC_ACTIVE_HIGH;
 
 	/* For CPT allow 3 pipe config, for others just use A or B */
-	if (HAS_PCH_LPT(dev))
+	if (HAS_PCH_LPT(dev_priv))
 		; /* Those bits don't exist here */
-	else if (HAS_PCH_CPT(dev))
+	else if (HAS_PCH_CPT(dev_priv))
 		adpa |= PORT_TRANS_SEL_CPT(crtc->pipe);
 	else if (crtc->pipe == 0)
 		adpa |= ADPA_PIPE_A_SELECT;
 	else
 		adpa |= ADPA_PIPE_B_SELECT;
 
-	if (!HAS_PCH_SPLIT(dev))
+	if (!HAS_PCH_SPLIT(dev_priv))
 		I915_WRITE(BCLRPAT(crtc->pipe), 0);
 
 	switch (mode) {
@@ -241,7 +241,8 @@ intel_crt_mode_valid(struct drm_connector *connector,
 		     struct drm_display_mode *mode)
 {
 	struct drm_device *dev = connector->dev;
-	int max_dotclk = to_i915(dev)->max_dotclk_freq;
+	struct drm_i915_private *dev_priv = to_i915(dev);
+	int max_dotclk = dev_priv->max_dotclk_freq;
 	int max_clock;
 
 	if (mode->flags & DRM_MODE_FLAG_DBLSCAN)
@@ -250,15 +251,15 @@ intel_crt_mode_valid(struct drm_connector *connector,
 	if (mode->clock < 25000)
 		return MODE_CLOCK_LOW;
 
-	if (HAS_PCH_LPT(dev))
+	if (HAS_PCH_LPT(dev_priv))
 		max_clock = 180000;
-	else if (IS_VALLEYVIEW(dev))
+	else if (IS_VALLEYVIEW(dev_priv))
 		/*
 		 * 270 MHz due to current DPLL limits,
 		 * DAC limit supposedly 355 MHz.
 		 */
 		max_clock = 270000;
-	else if (IS_GEN3(dev) || IS_GEN4(dev))
+	else if (IS_GEN3(dev_priv) || IS_GEN4(dev_priv))
 		max_clock = 400000;
 	else
 		max_clock = 350000;
@@ -269,7 +270,7 @@ intel_crt_mode_valid(struct drm_connector *connector,
 		return MODE_CLOCK_HIGH;
 
 	/* The FDI receiver on LPT only supports 8bpc and only has 2 lanes. */
-	if (HAS_PCH_LPT(dev) &&
+	if (HAS_PCH_LPT(dev_priv) &&
 	    (ironlake_get_lanes_required(mode->clock, 270000, 24) > 2))
 		return MODE_CLOCK_HIGH;
 
@@ -280,13 +281,13 @@ static bool intel_crt_compute_config(struct intel_encoder *encoder,
 				     struct intel_crtc_state *pipe_config,
 				     struct drm_connector_state *conn_state)
 {
-	struct drm_device *dev = encoder->base.dev;
+	struct drm_i915_private *dev_priv = to_i915(encoder->base.dev);
 
-	if (HAS_PCH_SPLIT(dev))
+	if (HAS_PCH_SPLIT(dev_priv))
 		pipe_config->has_pch_encoder = true;
 
 	/* LPT FDI RX only supports 8bpc. */
-	if (HAS_PCH_LPT(dev)) {
+	if (HAS_PCH_LPT(dev_priv)) {
 		if (pipe_config->bw_constrained && pipe_config->pipe_bpp < 24) {
 			DRM_DEBUG_KMS("LPT only supports 24bpp\n");
 			return false;
@@ -296,7 +297,7 @@ static bool intel_crt_compute_config(struct intel_encoder *encoder,
 	}
 
 	/* FDI must always be 2.7 GHz */
-	if (HAS_DDI(dev))
+	if (HAS_DDI(dev_priv))
 		pipe_config->port_clock = 135000 * 2;
 
 	return true;
@@ -312,7 +313,7 @@ static bool intel_ironlake_crt_detect_hotplug(struct drm_connector *connector)
 
 	/* The first time through, trigger an explicit detection cycle */
 	if (crt->force_hotplug_required) {
-		bool turn_off_dac = HAS_PCH_SPLIT(dev);
+		bool turn_off_dac = HAS_PCH_SPLIT(dev_priv);
 		u32 save_adpa;
 
 		crt->force_hotplug_required = 0;
@@ -419,10 +420,10 @@ static bool intel_crt_detect_hotplug(struct drm_connector *connector)
 	bool ret = false;
 	int i, tries = 0;
 
-	if (HAS_PCH_SPLIT(dev))
+	if (HAS_PCH_SPLIT(dev_priv))
 		return intel_ironlake_crt_detect_hotplug(connector);
 
-	if (IS_VALLEYVIEW(dev))
+	if (IS_VALLEYVIEW(dev_priv))
 		return valleyview_crt_detect_hotplug(connector);
 
 	/*
@@ -430,7 +431,7 @@ static bool intel_crt_detect_hotplug(struct drm_connector *connector)
 	 * to get a reliable result.
 	 */
 
-	if (IS_G4X(dev) && !IS_GM45(dev))
+	if (IS_G4X(dev_priv) && !IS_GM45(dev_priv))
 		tries = 2;
 	else
 		tries = 1;
@@ -566,7 +567,7 @@ intel_crt_load_detect(struct intel_crt *crt, uint32_t pipe)
 	/* Set the border color to purple. */
 	I915_WRITE(bclrpat_reg, 0x500050);
 
-	if (!IS_GEN2(dev)) {
+	if (!IS_GEN2(dev_priv)) {
 		uint32_t pipeconf = I915_READ(pipeconf_reg);
 		I915_WRITE(pipeconf_reg, pipeconf | PIPECONF_FORCE_BORDER);
 		POSTING_READ(pipeconf_reg);
@@ -643,6 +644,32 @@ intel_crt_load_detect(struct intel_crt *crt, uint32_t pipe)
 	return status;
 }
 
+static int intel_spurious_crt_detect_dmi_callback(const struct dmi_system_id *id)
+{
+	DRM_DEBUG_DRIVER("Skipping CRT detection for %s\n", id->ident);
+	return 1;
+}
+
+static const struct dmi_system_id intel_spurious_crt_detect[] = {
+	{
+		.callback = intel_spurious_crt_detect_dmi_callback,
+		.ident = "ACER ZGB",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "ACER"),
+			DMI_MATCH(DMI_PRODUCT_NAME, "ZGB"),
+		},
+	},
+	{
+		.callback = intel_spurious_crt_detect_dmi_callback,
+		.ident = "Intel DZ77BH-55K",
+		.matches = {
+			DMI_MATCH(DMI_BOARD_VENDOR, "Intel Corporation"),
+			DMI_MATCH(DMI_BOARD_NAME, "DZ77BH-55K"),
+		},
+	},
+	{ }
+};
+
 static enum drm_connector_status
 intel_crt_detect(struct drm_connector *connector, bool force)
 {
@@ -659,6 +686,10 @@ intel_crt_detect(struct drm_connector *connector, bool force)
 		      connector->base.id, connector->name,
 		      force);
 
+	/* Skip machines without VGA that falsely report hotplug events */
+	if (dmi_check_system(intel_spurious_crt_detect))
+		return connector_status_disconnected;
+
 	power_domain = intel_display_port_power_domain(intel_encoder);
 	intel_display_power_get(dev_priv, power_domain);
 
@@ -740,7 +771,7 @@ static int intel_crt_get_modes(struct drm_connector *connector)
 
 	i2c = intel_gmbus_get_adapter(dev_priv, dev_priv->vbt.crt_ddc_pin);
 	ret = intel_crt_ddc_get_modes(connector, i2c);
-	if (ret || !IS_G4X(dev))
+	if (ret || !IS_G4X(dev_priv))
 		goto out;
 
 	/* Try to probe digital port for output in DVI-I -> VGA mode. */
@@ -808,32 +839,6 @@ static const struct drm_encoder_funcs intel_crt_enc_funcs = {
 	.destroy = intel_encoder_destroy,
 };
 
-static int intel_no_crt_dmi_callback(const struct dmi_system_id *id)
-{
-	DRM_INFO("Skipping CRT initialization for %s\n", id->ident);
-	return 1;
-}
-
-static const struct dmi_system_id intel_no_crt[] = {
-	{
-		.callback = intel_no_crt_dmi_callback,
-		.ident = "ACER ZGB",
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "ACER"),
-			DMI_MATCH(DMI_PRODUCT_NAME, "ZGB"),
-		},
-	},
-	{
-		.callback = intel_no_crt_dmi_callback,
-		.ident = "DELL XPS 8700",
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "Dell Inc."),
-			DMI_MATCH(DMI_PRODUCT_NAME, "XPS 8700"),
-		},
-	},
-	{ }
-};
-
 void intel_crt_init(struct drm_device *dev)
 {
 	struct drm_connector *connector;
@@ -843,13 +848,9 @@ void intel_crt_init(struct drm_device *dev)
 	i915_reg_t adpa_reg;
 	u32 adpa;
 
-	/* Skip machines without VGA that falsely report hotplug events */
-	if (dmi_check_system(intel_no_crt))
-		return;
-
-	if (HAS_PCH_SPLIT(dev))
+	if (HAS_PCH_SPLIT(dev_priv))
 		adpa_reg = PCH_ADPA;
-	else if (IS_VALLEYVIEW(dev))
+	else if (IS_VALLEYVIEW(dev_priv))
 		adpa_reg = VLV_ADPA;
 	else
 		adpa_reg = ADPA;
@@ -893,12 +894,12 @@ void intel_crt_init(struct drm_device *dev)
 
 	crt->base.type = INTEL_OUTPUT_ANALOG;
 	crt->base.cloneable = (1 << INTEL_OUTPUT_DVO) | (1 << INTEL_OUTPUT_HDMI);
-	if (IS_I830(dev))
+	if (IS_I830(dev_priv))
 		crt->base.crtc_mask = (1 << 0);
 	else
 		crt->base.crtc_mask = (1 << 0) | (1 << 1) | (1 << 2);
 
-	if (IS_GEN2(dev))
+	if (IS_GEN2(dev_priv))
 		connector->interlace_allowed = 0;
 	else
 		connector->interlace_allowed = 1;
@@ -907,20 +908,23 @@ void intel_crt_init(struct drm_device *dev)
 	crt->adpa_reg = adpa_reg;
 
 	crt->base.compute_config = intel_crt_compute_config;
-	if (HAS_PCH_SPLIT(dev)) {
+	if (HAS_PCH_SPLIT(dev_priv)) {
 		crt->base.disable = pch_disable_crt;
 		crt->base.post_disable = pch_post_disable_crt;
 	} else {
 		crt->base.disable = intel_disable_crt;
 	}
 	crt->base.enable = intel_enable_crt;
-	if (I915_HAS_HOTPLUG(dev))
+	if (I915_HAS_HOTPLUG(dev) &&
+	    !dmi_check_system(intel_spurious_crt_detect))
 		crt->base.hpd_pin = HPD_CRT;
-	if (HAS_DDI(dev)) {
+	if (HAS_DDI(dev_priv)) {
+		crt->base.port = PORT_E;
 		crt->base.get_config = hsw_crt_get_config;
 		crt->base.get_hw_state = intel_ddi_get_hw_state;
 		crt->base.post_disable = hsw_post_disable_crt;
 	} else {
+		crt->base.port = PORT_NONE;
 		crt->base.get_config = intel_crt_get_config;
 		crt->base.get_hw_state = intel_crt_get_hw_state;
 	}
@@ -941,7 +945,7 @@ void intel_crt_init(struct drm_device *dev)
 	 * polarity and link reversal bits or not, instead of relying on the
 	 * BIOS.
 	 */
-	if (HAS_PCH_LPT(dev)) {
+	if (HAS_PCH_LPT(dev_priv)) {
 		u32 fdi_config = FDI_RX_POLARITY_REVERSED_LPT |
 				 FDI_RX_LINK_REVERSAL_OVERRIDE;
 
diff --git a/drivers/gpu/drm/i915/intel_ddi.c b/drivers/gpu/drm/i915/intel_ddi.c
index 15d47c87def6..fb18d699ce10 100644
--- a/drivers/gpu/drm/i915/intel_ddi.c
+++ b/drivers/gpu/drm/i915/intel_ddi.c
@@ -167,8 +167,47 @@ static const struct ddi_buf_trans skl_y_ddi_translations_dp[] = {
 	{ 0x80005012, 0x000000C0, 0x3 },
 };
 
+/* Kabylake H and S */
+static const struct ddi_buf_trans kbl_ddi_translations_dp[] = {
+	{ 0x00002016, 0x000000A0, 0x0 },
+	{ 0x00005012, 0x0000009B, 0x0 },
+	{ 0x00007011, 0x00000088, 0x0 },
+	{ 0x80009010, 0x000000C0, 0x1 },
+	{ 0x00002016, 0x0000009B, 0x0 },
+	{ 0x00005012, 0x00000088, 0x0 },
+	{ 0x80007011, 0x000000C0, 0x1 },
+	{ 0x00002016, 0x00000097, 0x0 },
+	{ 0x80005012, 0x000000C0, 0x1 },
+};
+
+/* Kabylake U */
+static const struct ddi_buf_trans kbl_u_ddi_translations_dp[] = {
+	{ 0x0000201B, 0x000000A1, 0x0 },
+	{ 0x00005012, 0x00000088, 0x0 },
+	{ 0x80007011, 0x000000CD, 0x3 },
+	{ 0x80009010, 0x000000C0, 0x3 },
+	{ 0x0000201B, 0x0000009D, 0x0 },
+	{ 0x80005012, 0x000000C0, 0x3 },
+	{ 0x80007011, 0x000000C0, 0x3 },
+	{ 0x00002016, 0x0000004F, 0x0 },
+	{ 0x80005012, 0x000000C0, 0x3 },
+};
+
+/* Kabylake Y */
+static const struct ddi_buf_trans kbl_y_ddi_translations_dp[] = {
+	{ 0x00001017, 0x000000A1, 0x0 },
+	{ 0x00005012, 0x00000088, 0x0 },
+	{ 0x80007011, 0x000000CD, 0x3 },
+	{ 0x8000800F, 0x000000C0, 0x3 },
+	{ 0x00001017, 0x0000009D, 0x0 },
+	{ 0x80005012, 0x000000C0, 0x3 },
+	{ 0x80007011, 0x000000C0, 0x3 },
+	{ 0x00001017, 0x0000004C, 0x0 },
+	{ 0x80005012, 0x000000C0, 0x3 },
+};
+
 /*
- * Skylake H and S
+ * Skylake/Kabylake H and S
  * eDP 1.4 low vswing translation parameters
  */
 static const struct ddi_buf_trans skl_ddi_translations_edp[] = {
@@ -185,7 +224,7 @@ static const struct ddi_buf_trans skl_ddi_translations_edp[] = {
 };
 
 /*
- * Skylake U
+ * Skylake/Kabylake U
  * eDP 1.4 low vswing translation parameters
  */
 static const struct ddi_buf_trans skl_u_ddi_translations_edp[] = {
@@ -202,7 +241,7 @@ static const struct ddi_buf_trans skl_u_ddi_translations_edp[] = {
 };
 
 /*
- * Skylake Y
+ * Skylake/Kabylake Y
  * eDP 1.4 low vswing translation parameters
  */
 static const struct ddi_buf_trans skl_y_ddi_translations_edp[] = {
@@ -218,7 +257,7 @@ static const struct ddi_buf_trans skl_y_ddi_translations_edp[] = {
 	{ 0x00000018, 0x0000008A, 0x0 },
 };
 
-/* Skylake U, H and S */
+/* Skylake/Kabylake U, H and S */
 static const struct ddi_buf_trans skl_ddi_translations_hdmi[] = {
 	{ 0x00000018, 0x000000AC, 0x0 },
 	{ 0x00005012, 0x0000009D, 0x0 },
@@ -233,7 +272,7 @@ static const struct ddi_buf_trans skl_ddi_translations_hdmi[] = {
 	{ 0x80000018, 0x000000C0, 0x1 },
 };
 
-/* Skylake Y */
+/* Skylake/Kabylake Y */
 static const struct ddi_buf_trans skl_y_ddi_translations_hdmi[] = {
 	{ 0x00000018, 0x000000A1, 0x0 },
 	{ 0x00005012, 0x000000DF, 0x0 },
@@ -334,10 +373,10 @@ bdw_get_buf_trans_edp(struct drm_i915_private *dev_priv, int *n_entries)
 static const struct ddi_buf_trans *
 skl_get_buf_trans_dp(struct drm_i915_private *dev_priv, int *n_entries)
 {
-	if (IS_SKL_ULX(dev_priv) || IS_KBL_ULX(dev_priv)) {
+	if (IS_SKL_ULX(dev_priv)) {
 		*n_entries = ARRAY_SIZE(skl_y_ddi_translations_dp);
 		return skl_y_ddi_translations_dp;
-	} else if (IS_SKL_ULT(dev_priv) || IS_KBL_ULT(dev_priv)) {
+	} else if (IS_SKL_ULT(dev_priv)) {
 		*n_entries = ARRAY_SIZE(skl_u_ddi_translations_dp);
 		return skl_u_ddi_translations_dp;
 	} else {
@@ -347,6 +386,21 @@ skl_get_buf_trans_dp(struct drm_i915_private *dev_priv, int *n_entries)
 }
 
 static const struct ddi_buf_trans *
+kbl_get_buf_trans_dp(struct drm_i915_private *dev_priv, int *n_entries)
+{
+	if (IS_KBL_ULX(dev_priv)) {
+		*n_entries = ARRAY_SIZE(kbl_y_ddi_translations_dp);
+		return kbl_y_ddi_translations_dp;
+	} else if (IS_KBL_ULT(dev_priv)) {
+		*n_entries = ARRAY_SIZE(kbl_u_ddi_translations_dp);
+		return kbl_u_ddi_translations_dp;
+	} else {
+		*n_entries = ARRAY_SIZE(kbl_ddi_translations_dp);
+		return kbl_ddi_translations_dp;
+	}
+}
+
+static const struct ddi_buf_trans *
 skl_get_buf_trans_edp(struct drm_i915_private *dev_priv, int *n_entries)
 {
 	if (dev_priv->vbt.edp.low_vswing) {
@@ -362,7 +416,10 @@ skl_get_buf_trans_edp(struct drm_i915_private *dev_priv, int *n_entries)
 		}
 	}
 
-	return skl_get_buf_trans_dp(dev_priv, n_entries);
+	if (IS_KABYLAKE(dev_priv))
+		return kbl_get_buf_trans_dp(dev_priv, n_entries);
+	else
+		return skl_get_buf_trans_dp(dev_priv, n_entries);
 }
 
 static const struct ddi_buf_trans *
@@ -430,21 +487,18 @@ void intel_prepare_dp_ddi_buffers(struct intel_encoder *encoder)
 	if (IS_BROXTON(dev_priv))
 		return;
 
-	if (IS_SKYLAKE(dev_priv) || IS_KABYLAKE(dev_priv)) {
+	if (IS_KABYLAKE(dev_priv)) {
+		ddi_translations_fdi = NULL;
+		ddi_translations_dp =
+				kbl_get_buf_trans_dp(dev_priv, &n_dp_entries);
+		ddi_translations_edp =
+				skl_get_buf_trans_edp(dev_priv, &n_edp_entries);
+	} else if (IS_SKYLAKE(dev_priv)) {
 		ddi_translations_fdi = NULL;
 		ddi_translations_dp =
 				skl_get_buf_trans_dp(dev_priv, &n_dp_entries);
 		ddi_translations_edp =
 				skl_get_buf_trans_edp(dev_priv, &n_edp_entries);
-
-		/* If we're boosting the current, set bit 31 of trans1 */
-		if (dev_priv->vbt.ddi_port_info[port].dp_boost_level)
-			iboost_bit = DDI_BUF_BALANCE_LEG_ENABLE;
-
-		if (WARN_ON(encoder->type == INTEL_OUTPUT_EDP &&
-			    port != PORT_A && port != PORT_E &&
-			    n_edp_entries > 9))
-			n_edp_entries = 9;
 	} else if (IS_BROADWELL(dev_priv)) {
 		ddi_translations_fdi = bdw_ddi_translations_fdi;
 		ddi_translations_dp = bdw_ddi_translations_dp;
@@ -464,6 +518,17 @@ void intel_prepare_dp_ddi_buffers(struct intel_encoder *encoder)
 		n_dp_entries = ARRAY_SIZE(bdw_ddi_translations_dp);
 	}
 
+	if (IS_SKYLAKE(dev_priv) || IS_KABYLAKE(dev_priv)) {
+		/* If we're boosting the current, set bit 31 of trans1 */
+		if (dev_priv->vbt.ddi_port_info[port].dp_boost_level)
+			iboost_bit = DDI_BUF_BALANCE_LEG_ENABLE;
+
+		if (WARN_ON(encoder->type == INTEL_OUTPUT_EDP &&
+			    port != PORT_A && port != PORT_E &&
+			    n_edp_entries > 9))
+			n_edp_entries = 9;
+	}
+
 	switch (encoder->type) {
 	case INTEL_OUTPUT_EDP:
 		ddi_translations = ddi_translations_edp;
@@ -1020,13 +1085,13 @@ static void bxt_ddi_clock_get(struct intel_encoder *encoder,
 void intel_ddi_clock_get(struct intel_encoder *encoder,
 			 struct intel_crtc_state *pipe_config)
 {
-	struct drm_device *dev = encoder->base.dev;
+	struct drm_i915_private *dev_priv = to_i915(encoder->base.dev);
 
-	if (INTEL_INFO(dev)->gen <= 8)
+	if (INTEL_GEN(dev_priv) <= 8)
 		hsw_ddi_clock_get(encoder, pipe_config);
-	else if (IS_SKYLAKE(dev) || IS_KABYLAKE(dev))
+	else if (IS_SKYLAKE(dev_priv) || IS_KABYLAKE(dev_priv))
 		skl_ddi_clock_get(encoder, pipe_config);
-	else if (IS_BROXTON(dev))
+	else if (IS_BROXTON(dev_priv))
 		bxt_ddi_clock_get(encoder, pipe_config);
 }
 
@@ -1081,14 +1146,14 @@ bxt_ddi_pll_select(struct intel_crtc *intel_crtc,
 bool intel_ddi_pll_select(struct intel_crtc *intel_crtc,
 			  struct intel_crtc_state *crtc_state)
 {
-	struct drm_device *dev = intel_crtc->base.dev;
+	struct drm_i915_private *dev_priv = to_i915(intel_crtc->base.dev);
 	struct intel_encoder *intel_encoder =
 		intel_ddi_get_crtc_new_encoder(crtc_state);
 
-	if (IS_SKYLAKE(dev) || IS_KABYLAKE(dev))
+	if (IS_SKYLAKE(dev_priv) || IS_KABYLAKE(dev_priv))
 		return skl_ddi_pll_select(intel_crtc, crtc_state,
 					  intel_encoder);
-	else if (IS_BROXTON(dev))
+	else if (IS_BROXTON(dev_priv))
 		return bxt_ddi_pll_select(intel_crtc, crtc_state,
 					  intel_encoder);
 	else
@@ -1189,7 +1254,7 @@ void intel_ddi_enable_transcoder_func(struct drm_crtc *crtc)
 			 * eDP when not using the panel fitter, and when not
 			 * using motion blur mitigation (which we don't
 			 * support). */
-			if (IS_HASWELL(dev) &&
+			if (IS_HASWELL(dev_priv) &&
 			    (intel_crtc->config->pch_pfit.enabled ||
 			     intel_crtc->config->pch_pfit.force_thru))
 				temp |= TRANS_DDI_EDP_INPUT_A_ONOFF;
@@ -1434,7 +1499,12 @@ static void skl_ddi_set_iboost(struct intel_encoder *encoder, u32 level)
 		if (dp_iboost) {
 			iboost = dp_iboost;
 		} else {
-			ddi_translations = skl_get_buf_trans_dp(dev_priv, &n_entries);
+			if (IS_KABYLAKE(dev_priv))
+				ddi_translations = kbl_get_buf_trans_dp(dev_priv,
+									&n_entries);
+			else
+				ddi_translations = skl_get_buf_trans_dp(dev_priv,
+									&n_entries);
 			iboost = ddi_translations[level].i_boost;
 		}
 	} else if (type == INTEL_OUTPUT_EDP) {
@@ -1742,7 +1812,7 @@ static void intel_ddi_post_disable(struct intel_encoder *intel_encoder,
 		intel_edp_panel_off(intel_dp);
 	}
 
-	if (IS_SKYLAKE(dev) || IS_KABYLAKE(dev))
+	if (IS_SKYLAKE(dev_priv) || IS_KABYLAKE(dev_priv))
 		I915_WRITE(DPLL_CTRL2, (I915_READ(DPLL_CTRL2) |
 					DPLL_CTRL2_DDI_CLK_OFF(port)));
 	else if (INTEL_INFO(dev)->gen < 9)
@@ -2438,7 +2508,7 @@ void intel_ddi_init(struct drm_device *dev, enum port port)
 	struct intel_digital_port *intel_dig_port;
 	struct intel_encoder *intel_encoder;
 	struct drm_encoder *encoder;
-	bool init_hdmi, init_dp;
+	bool init_hdmi, init_dp, init_lspcon = false;
 	int max_lanes;
 
 	if (I915_READ(DDI_BUF_CTL(PORT_A)) & DDI_A_4_LANES) {
@@ -2470,6 +2540,19 @@ void intel_ddi_init(struct drm_device *dev, enum port port)
 	init_hdmi = (dev_priv->vbt.ddi_port_info[port].supports_dvi ||
 		     dev_priv->vbt.ddi_port_info[port].supports_hdmi);
 	init_dp = dev_priv->vbt.ddi_port_info[port].supports_dp;
+
+	if (intel_bios_is_lspcon_present(dev_priv, port)) {
+		/*
+		 * Lspcon device needs to be driven with DP connector
+		 * with special detection sequence. So make sure DP
+		 * is initialized before lspcon.
+		 */
+		init_dp = true;
+		init_lspcon = true;
+		init_hdmi = false;
+		DRM_DEBUG_KMS("VBT says port %c has lspcon\n", port_name(port));
+	}
+
 	if (!init_dp && !init_hdmi) {
 		DRM_DEBUG_KMS("VBT says port %c is not DVI/HDMI/DP compatible, respect it\n",
 			      port_name(port));
@@ -2509,7 +2592,7 @@ void intel_ddi_init(struct drm_device *dev, enum port port)
 	 * configuration so that we use the proper lane count for our
 	 * calculations.
 	 */
-	if (IS_BROXTON(dev) && port == PORT_A) {
+	if (IS_BROXTON(dev_priv) && port == PORT_A) {
 		if (!(intel_dig_port->saved_port_bits & DDI_A_4_LANES)) {
 			DRM_DEBUG_KMS("BXT BIOS forgot to set DDI_A_4_LANES for port A; fixing\n");
 			intel_dig_port->saved_port_bits |= DDI_A_4_LANES;
@@ -2520,6 +2603,7 @@ void intel_ddi_init(struct drm_device *dev, enum port port)
 	intel_dig_port->max_lanes = max_lanes;
 
 	intel_encoder->type = INTEL_OUTPUT_UNKNOWN;
+	intel_encoder->port = port;
 	intel_encoder->crtc_mask = (1 << 0) | (1 << 1) | (1 << 2);
 	intel_encoder->cloneable = 0;
 
@@ -2532,7 +2616,7 @@ void intel_ddi_init(struct drm_device *dev, enum port port)
 		 * On BXT A0/A1, sw needs to activate DDIA HPD logic and
 		 * interrupts to check the external panel connection.
 		 */
-		if (IS_BXT_REVID(dev, 0, BXT_REVID_A1) && port == PORT_B)
+		if (IS_BXT_REVID(dev_priv, 0, BXT_REVID_A1) && port == PORT_B)
 			dev_priv->hotplug.irq_port[PORT_A] = intel_dig_port;
 		else
 			dev_priv->hotplug.irq_port[port] = intel_dig_port;
@@ -2545,6 +2629,20 @@ void intel_ddi_init(struct drm_device *dev, enum port port)
 			goto err;
 	}
 
+	if (init_lspcon) {
+		if (lspcon_init(intel_dig_port))
+			/* TODO: handle hdmi info frame part */
+			DRM_DEBUG_KMS("LSPCON init success on port %c\n",
+				port_name(port));
+		else
+			/*
+			 * LSPCON init faied, but DP init was success, so
+			 * lets try to drive as DP++ port.
+			 */
+			DRM_ERROR("LSPCON init failed on port %c\n",
+				port_name(port));
+	}
+
 	return;
 
 err:
diff --git a/drivers/gpu/drm/i915/intel_device_info.c b/drivers/gpu/drm/i915/intel_device_info.c
index 73b6858600ac..d6a8f11813d5 100644
--- a/drivers/gpu/drm/i915/intel_device_info.c
+++ b/drivers/gpu/drm/i915/intel_device_info.c
@@ -28,20 +28,14 @@ void intel_device_info_dump(struct drm_i915_private *dev_priv)
 {
 	const struct intel_device_info *info = &dev_priv->info;
 
-#define PRINT_S(name) "%s"
-#define SEP_EMPTY
-#define PRINT_FLAG(name) info->name ? #name "," : ""
-#define SEP_COMMA ,
-	DRM_DEBUG_DRIVER("i915 device info: gen=%i, pciid=0x%04x rev=0x%02x flags="
-			 DEV_INFO_FOR_EACH_FLAG(PRINT_S, SEP_EMPTY),
+	DRM_DEBUG_DRIVER("i915 device info: gen=%i, pciid=0x%04x rev=0x%02x",
 			 info->gen,
 			 dev_priv->drm.pdev->device,
-			 dev_priv->drm.pdev->revision,
-			 DEV_INFO_FOR_EACH_FLAG(PRINT_FLAG, SEP_COMMA));
-#undef PRINT_S
-#undef SEP_EMPTY
+			 dev_priv->drm.pdev->revision);
+#define PRINT_FLAG(name) \
+	DRM_DEBUG_DRIVER("i915 device info: " #name ": %s", yesno(info->name))
+	DEV_INFO_FOR_EACH_FLAG(PRINT_FLAG);
 #undef PRINT_FLAG
-#undef SEP_COMMA
 }
 
 static void cherryview_sseu_info_init(struct drm_i915_private *dev_priv)
@@ -192,7 +186,7 @@ static void broadwell_sseu_info_init(struct drm_i915_private *dev_priv)
 	struct sseu_dev_info *sseu = &mkwrite_device_info(dev_priv)->sseu;
 	const int s_max = 3, ss_max = 3, eu_max = 8;
 	int s, ss;
-	u32 fuse2, eu_disable[s_max];
+	u32 fuse2, eu_disable[3]; /* s_max */
 
 	fuse2 = I915_READ(GEN8_FUSE2);
 	sseu->slice_mask = (fuse2 & GEN8_F2_S_ENA_MASK) >> GEN8_F2_S_ENA_SHIFT;
diff --git a/drivers/gpu/drm/i915/intel_display.c b/drivers/gpu/drm/i915/intel_display.c
index 6c5c36eba6cb..6f8f6ec5b27a 100644
--- a/drivers/gpu/drm/i915/intel_display.c
+++ b/drivers/gpu/drm/i915/intel_display.c
@@ -600,7 +600,7 @@ int chv_calc_dpll_params(int refclk, struct dpll *clock)
  * the given connectors.
  */
 
-static bool intel_PLL_is_valid(struct drm_device *dev,
+static bool intel_PLL_is_valid(struct drm_i915_private *dev_priv,
 			       const struct intel_limit *limit,
 			       const struct dpll *clock)
 {
@@ -613,12 +613,13 @@ static bool intel_PLL_is_valid(struct drm_device *dev,
 	if (clock->m1  < limit->m1.min  || limit->m1.max  < clock->m1)
 		INTELPllInvalid("m1 out of range\n");
 
-	if (!IS_PINEVIEW(dev) && !IS_VALLEYVIEW(dev) &&
-	    !IS_CHERRYVIEW(dev) && !IS_BROXTON(dev))
+	if (!IS_PINEVIEW(dev_priv) && !IS_VALLEYVIEW(dev_priv) &&
+	    !IS_CHERRYVIEW(dev_priv) && !IS_BROXTON(dev_priv))
 		if (clock->m1 <= clock->m2)
 			INTELPllInvalid("m1 <= m2\n");
 
-	if (!IS_VALLEYVIEW(dev) && !IS_CHERRYVIEW(dev) && !IS_BROXTON(dev)) {
+	if (!IS_VALLEYVIEW(dev_priv) && !IS_CHERRYVIEW(dev_priv) &&
+	    !IS_BROXTON(dev_priv)) {
 		if (clock->p < limit->p.min || limit->p.max < clock->p)
 			INTELPllInvalid("p out of range\n");
 		if (clock->m < limit->m.min || limit->m.max < clock->m)
@@ -698,7 +699,8 @@ i9xx_find_best_dpll(const struct intel_limit *limit,
 					int this_err;
 
 					i9xx_calc_dpll_params(refclk, &clock);
-					if (!intel_PLL_is_valid(dev, limit,
+					if (!intel_PLL_is_valid(to_i915(dev),
+								limit,
 								&clock))
 						continue;
 					if (match_clock &&
@@ -753,7 +755,8 @@ pnv_find_best_dpll(const struct intel_limit *limit,
 					int this_err;
 
 					pnv_calc_dpll_params(refclk, &clock);
-					if (!intel_PLL_is_valid(dev, limit,
+					if (!intel_PLL_is_valid(to_i915(dev),
+								limit,
 								&clock))
 						continue;
 					if (match_clock &&
@@ -813,7 +816,8 @@ g4x_find_best_dpll(const struct intel_limit *limit,
 					int this_err;
 
 					i9xx_calc_dpll_params(refclk, &clock);
-					if (!intel_PLL_is_valid(dev, limit,
+					if (!intel_PLL_is_valid(to_i915(dev),
+								limit,
 								&clock))
 						continue;
 
@@ -845,7 +849,7 @@ static bool vlv_PLL_is_optimal(struct drm_device *dev, int target_freq,
 	 * For CHV ignore the error and consider only the P value.
 	 * Prefer a bigger P value based on HW requirements.
 	 */
-	if (IS_CHERRYVIEW(dev)) {
+	if (IS_CHERRYVIEW(to_i915(dev))) {
 		*error_ppm = 0;
 
 		return calculated_clock->p > best_clock->p;
@@ -909,7 +913,8 @@ vlv_find_best_dpll(const struct intel_limit *limit,
 
 					vlv_calc_dpll_params(refclk, &clock);
 
-					if (!intel_PLL_is_valid(dev, limit,
+					if (!intel_PLL_is_valid(to_i915(dev),
+								limit,
 								&clock))
 						continue;
 
@@ -977,7 +982,7 @@ chv_find_best_dpll(const struct intel_limit *limit,
 
 			chv_calc_dpll_params(refclk, &clock);
 
-			if (!intel_PLL_is_valid(dev, limit, &clock))
+			if (!intel_PLL_is_valid(to_i915(dev), limit, &clock))
 				continue;
 
 			if (!vlv_PLL_is_optimal(dev, target, &clock, best_clock,
@@ -1040,7 +1045,7 @@ static bool pipe_dsl_stopped(struct drm_device *dev, enum pipe pipe)
 	u32 line1, line2;
 	u32 line_mask;
 
-	if (IS_GEN2(dev))
+	if (IS_GEN2(dev_priv))
 		line_mask = DSL_LINEMASK_GEN2;
 	else
 		line_mask = DSL_LINEMASK_GEN3;
@@ -1187,19 +1192,17 @@ void assert_fdi_rx_pll(struct drm_i915_private *dev_priv,
 			onoff(state), onoff(cur_state));
 }
 
-void assert_panel_unlocked(struct drm_i915_private *dev_priv,
-			   enum pipe pipe)
+void assert_panel_unlocked(struct drm_i915_private *dev_priv, enum pipe pipe)
 {
-	struct drm_device *dev = &dev_priv->drm;
 	i915_reg_t pp_reg;
 	u32 val;
 	enum pipe panel_pipe = PIPE_A;
 	bool locked = true;
 
-	if (WARN_ON(HAS_DDI(dev)))
+	if (WARN_ON(HAS_DDI(dev_priv)))
 		return;
 
-	if (HAS_PCH_SPLIT(dev)) {
+	if (HAS_PCH_SPLIT(dev_priv)) {
 		u32 port_sel;
 
 		pp_reg = PP_CONTROL(0);
@@ -1209,7 +1212,7 @@ void assert_panel_unlocked(struct drm_i915_private *dev_priv,
 		    I915_READ(PCH_LVDS) & LVDS_PIPEB_SELECT)
 			panel_pipe = PIPE_B;
 		/* XXX: else fix for eDP */
-	} else if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) {
+	} else if (IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv)) {
 		/* presumably write lock depends on pipe, not port select */
 		pp_reg = PP_CONTROL(pipe);
 		panel_pipe = pipe;
@@ -1232,10 +1235,9 @@ void assert_panel_unlocked(struct drm_i915_private *dev_priv,
 static void assert_cursor(struct drm_i915_private *dev_priv,
 			  enum pipe pipe, bool state)
 {
-	struct drm_device *dev = &dev_priv->drm;
 	bool cur_state;
 
-	if (IS_845G(dev) || IS_I865G(dev))
+	if (IS_845G(dev_priv) || IS_I865G(dev_priv))
 		cur_state = I915_READ(CURCNTR(PIPE_A)) & CURSOR_ENABLE;
 	else
 		cur_state = I915_READ(CURCNTR(pipe)) & CURSOR_MODE;
@@ -1330,7 +1332,7 @@ static void assert_sprites_disabled(struct drm_i915_private *dev_priv,
 			     "plane %d assertion failure, should be off on pipe %c but is still active\n",
 			     sprite, pipe_name(pipe));
 		}
-	} else if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) {
+	} else if (IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv)) {
 		for_each_sprite(dev_priv, pipe, sprite) {
 			u32 val = I915_READ(SPCNTR(pipe, sprite));
 			I915_STATE_WARN(val & SP_ENABLE,
@@ -1619,11 +1621,11 @@ static void i9xx_enable_pll(struct intel_crtc *crtc)
 	assert_pipe_disabled(dev_priv, crtc->pipe);
 
 	/* PLL is protected by panel, make sure we can write it */
-	if (IS_MOBILE(dev) && !IS_I830(dev))
+	if (IS_MOBILE(dev_priv) && !IS_I830(dev_priv))
 		assert_panel_unlocked(dev_priv, crtc->pipe);
 
 	/* Enable DVO 2x clock on both PLLs if necessary */
-	if (IS_I830(dev) && intel_num_dvo_pipes(dev) > 0) {
+	if (IS_I830(dev_priv) && intel_num_dvo_pipes(dev) > 0) {
 		/*
 		 * It appears to be important that we don't enable this
 		 * for the current pipe before otherwise configuring the
@@ -1688,7 +1690,7 @@ static void i9xx_disable_pll(struct intel_crtc *crtc)
 	enum pipe pipe = crtc->pipe;
 
 	/* Disable DVO 2x clock on both PLLs if necessary */
-	if (IS_I830(dev) &&
+	if (IS_I830(dev_priv) &&
 	    intel_crtc_has_type(crtc->config, INTEL_OUTPUT_DVO) &&
 	    !intel_num_dvo_pipes(dev)) {
 		I915_WRITE(DPLL(PIPE_B),
@@ -1786,7 +1788,6 @@ void vlv_wait_port_ready(struct drm_i915_private *dev_priv,
 static void ironlake_enable_pch_transcoder(struct drm_i915_private *dev_priv,
 					   enum pipe pipe)
 {
-	struct drm_device *dev = &dev_priv->drm;
 	struct drm_crtc *crtc = dev_priv->pipe_to_crtc_mapping[pipe];
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	i915_reg_t reg;
@@ -1799,7 +1800,7 @@ static void ironlake_enable_pch_transcoder(struct drm_i915_private *dev_priv,
 	assert_fdi_tx_enabled(dev_priv, pipe);
 	assert_fdi_rx_enabled(dev_priv, pipe);
 
-	if (HAS_PCH_CPT(dev)) {
+	if (HAS_PCH_CPT(dev_priv)) {
 		/* Workaround: Set the timing override bit before enabling the
 		 * pch transcoder. */
 		reg = TRANS_CHICKEN2(pipe);
@@ -1877,7 +1878,6 @@ static void lpt_enable_pch_transcoder(struct drm_i915_private *dev_priv,
 static void ironlake_disable_pch_transcoder(struct drm_i915_private *dev_priv,
 					    enum pipe pipe)
 {
-	struct drm_device *dev = &dev_priv->drm;
 	i915_reg_t reg;
 	uint32_t val;
 
@@ -1898,7 +1898,7 @@ static void ironlake_disable_pch_transcoder(struct drm_i915_private *dev_priv,
 				    50))
 		DRM_ERROR("failed to disable transcoder %c\n", pipe_name(pipe));
 
-	if (HAS_PCH_CPT(dev)) {
+	if (HAS_PCH_CPT(dev_priv)) {
 		/* Workaround: Clear the timing override chicken bit again. */
 		reg = TRANS_CHICKEN2(pipe);
 		val = I915_READ(reg);
@@ -1926,6 +1926,18 @@ void lpt_disable_pch_transcoder(struct drm_i915_private *dev_priv)
 	I915_WRITE(TRANS_CHICKEN2(PIPE_A), val);
 }
 
+enum transcoder intel_crtc_pch_transcoder(struct intel_crtc *crtc)
+{
+	struct drm_i915_private *dev_priv = to_i915(crtc->base.dev);
+
+	WARN_ON(!crtc->config->has_pch_encoder);
+
+	if (HAS_PCH_LPT(dev_priv))
+		return TRANSCODER_A;
+	else
+		return (enum transcoder) crtc->pipe;
+}
+
 /**
  * intel_enable_pipe - enable a pipe, asserting requirements
  * @crtc: crtc responsible for the pipe
@@ -1939,7 +1951,6 @@ static void intel_enable_pipe(struct intel_crtc *crtc)
 	struct drm_i915_private *dev_priv = to_i915(dev);
 	enum pipe pipe = crtc->pipe;
 	enum transcoder cpu_transcoder = crtc->config->cpu_transcoder;
-	enum pipe pch_transcoder;
 	i915_reg_t reg;
 	u32 val;
 
@@ -1949,11 +1960,6 @@ static void intel_enable_pipe(struct intel_crtc *crtc)
 	assert_cursor_disabled(dev_priv, pipe);
 	assert_sprites_disabled(dev_priv, pipe);
 
-	if (HAS_PCH_LPT(dev_priv))
-		pch_transcoder = TRANSCODER_A;
-	else
-		pch_transcoder = pipe;
-
 	/*
 	 * A pipe without a PLL won't actually be able to drive bits from
 	 * a plane.  On ILK+ the pipe PLLs are integrated, so we don't
@@ -1967,7 +1973,8 @@ static void intel_enable_pipe(struct intel_crtc *crtc)
 	} else {
 		if (crtc->config->has_pch_encoder) {
 			/* if driving the PCH, we need FDI enabled */
-			assert_fdi_rx_pll_enabled(dev_priv, pch_transcoder);
+			assert_fdi_rx_pll_enabled(dev_priv,
+						  (enum pipe) intel_crtc_pch_transcoder(crtc));
 			assert_fdi_tx_pll_enabled(dev_priv,
 						  (enum pipe) cpu_transcoder);
 		}
@@ -3033,7 +3040,7 @@ static void i9xx_update_primary_plane(struct drm_plane *primary,
 			   ((crtc_state->pipe_src_h - 1) << 16) |
 			   (crtc_state->pipe_src_w - 1));
 		I915_WRITE(DSPPOS(plane), 0);
-	} else if (IS_CHERRYVIEW(dev) && plane == PLANE_B) {
+	} else if (IS_CHERRYVIEW(dev_priv) && plane == PLANE_B) {
 		I915_WRITE(PRIMSIZE(plane),
 			   ((crtc_state->pipe_src_h - 1) << 16) |
 			   (crtc_state->pipe_src_w - 1));
@@ -3071,7 +3078,7 @@ static void i9xx_update_primary_plane(struct drm_plane *primary,
 	    fb->modifier[0] == I915_FORMAT_MOD_X_TILED)
 		dspcntr |= DISPPLANE_TILED;
 
-	if (IS_G4X(dev))
+	if (IS_G4X(dev_priv))
 		dspcntr |= DISPPLANE_TRICKLE_FEED_DISABLE;
 
 	intel_add_fb_offsets(&x, &y, plane_state, 0);
@@ -3144,7 +3151,7 @@ static void ironlake_update_primary_plane(struct drm_plane *primary,
 	dspcntr = DISPPLANE_GAMMA_ENABLE;
 	dspcntr |= DISPLAY_PLANE_ENABLE;
 
-	if (IS_HASWELL(dev) || IS_BROADWELL(dev))
+	if (IS_HASWELL(dev_priv) || IS_BROADWELL(dev_priv))
 		dspcntr |= DISPPLANE_PIPE_CSC_ENABLE;
 
 	switch (fb->pixel_format) {
@@ -3173,7 +3180,7 @@ static void ironlake_update_primary_plane(struct drm_plane *primary,
 	if (fb->modifier[0] == I915_FORMAT_MOD_X_TILED)
 		dspcntr |= DISPPLANE_TILED;
 
-	if (!IS_HASWELL(dev) && !IS_BROADWELL(dev))
+	if (!IS_HASWELL(dev_priv) && !IS_BROADWELL(dev_priv))
 		dspcntr |= DISPPLANE_TRICKLE_FEED_DISABLE;
 
 	intel_add_fb_offsets(&x, &y, plane_state, 0);
@@ -3184,7 +3191,7 @@ static void ironlake_update_primary_plane(struct drm_plane *primary,
 	if (rotation == DRM_ROTATE_180) {
 		dspcntr |= DISPPLANE_ROTATE_180;
 
-		if (!IS_HASWELL(dev) && !IS_BROADWELL(dev)) {
+		if (!IS_HASWELL(dev_priv) && !IS_BROADWELL(dev_priv)) {
 			x += (crtc_state->pipe_src_w - 1);
 			y += (crtc_state->pipe_src_h - 1);
 		}
@@ -3201,7 +3208,7 @@ static void ironlake_update_primary_plane(struct drm_plane *primary,
 	I915_WRITE(DSPSURF(plane),
 		   intel_fb_gtt_offset(fb, rotation) +
 		   intel_crtc->dspaddr_offset);
-	if (IS_HASWELL(dev) || IS_BROADWELL(dev)) {
+	if (IS_HASWELL(dev_priv) || IS_BROADWELL(dev_priv)) {
 		I915_WRITE(DSPOFFSET(plane), (y << 16) | x);
 	} else {
 		I915_WRITE(DSPTILEOFF(plane), (y << 16) | x);
@@ -3378,6 +3385,8 @@ static void skylake_update_primary_plane(struct drm_plane *plane,
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc_state->base.crtc);
 	struct drm_framebuffer *fb = plane_state->base.fb;
 	const struct skl_wm_values *wm = &dev_priv->wm.skl_results;
+	const struct skl_plane_wm *p_wm =
+		&crtc_state->wm.skl.optimal.planes[0];
 	int pipe = intel_crtc->pipe;
 	u32 plane_ctl;
 	unsigned int rotation = plane_state->base.rotation;
@@ -3408,11 +3417,13 @@ static void skylake_update_primary_plane(struct drm_plane *plane,
 	dst_w--;
 	dst_h--;
 
+	intel_crtc->dspaddr_offset = surf_addr;
+
 	intel_crtc->adjusted_x = src_x;
 	intel_crtc->adjusted_y = src_y;
 
 	if (wm->dirty_pipes & drm_crtc_mask(&intel_crtc->base))
-		skl_write_plane_wm(intel_crtc, wm, 0);
+		skl_write_plane_wm(intel_crtc, p_wm, &wm->ddb, 0);
 
 	I915_WRITE(PLANE_CTL(pipe, 0), plane_ctl);
 	I915_WRITE(PLANE_OFFSET(pipe, 0), (src_y << 16) | src_x);
@@ -3446,6 +3457,8 @@ static void skylake_disable_primary_plane(struct drm_plane *primary,
 	struct drm_device *dev = crtc->dev;
 	struct drm_i915_private *dev_priv = to_i915(dev);
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	struct intel_crtc_state *cstate = to_intel_crtc_state(crtc->state);
+	const struct skl_plane_wm *p_wm = &cstate->wm.skl.optimal.planes[0];
 	int pipe = intel_crtc->pipe;
 
 	/*
@@ -3453,7 +3466,8 @@ static void skylake_disable_primary_plane(struct drm_plane *primary,
 	 * plane's visiblity isn't actually changing neither is its watermarks.
 	 */
 	if (!crtc->primary->state->visible)
-		skl_write_plane_wm(intel_crtc, &dev_priv->wm.skl_results, 0);
+		skl_write_plane_wm(intel_crtc, p_wm,
+				   &dev_priv->wm.skl_results.ddb, 0);
 
 	I915_WRITE(PLANE_CTL(pipe, 0), 0);
 	I915_WRITE(PLANE_SURF(pipe, 0), 0);
@@ -3601,8 +3615,6 @@ void intel_finish_reset(struct drm_i915_private *dev_priv)
 
 	dev_priv->modeset_restore_state = NULL;
 
-	dev_priv->modeset_restore_state = NULL;
-
 	/* reset doesn't touch the display */
 	if (!gpu_reset_clobbers_display(dev_priv)) {
 		if (!state) {
@@ -3629,6 +3641,7 @@ void intel_finish_reset(struct drm_i915_private *dev_priv)
 		intel_runtime_pm_disable_interrupts(dev_priv);
 		intel_runtime_pm_enable_interrupts(dev_priv);
 
+		intel_pps_unlock_regs_wa(dev_priv);
 		intel_modeset_init_hw(dev);
 
 		spin_lock_irq(&dev_priv->irq_lock);
@@ -3713,7 +3726,7 @@ static void intel_update_pipe_config(struct intel_crtc *crtc,
 
 		if (pipe_config->pch_pfit.enabled)
 			skylake_pfit_enable(crtc);
-	} else if (HAS_PCH_SPLIT(dev)) {
+	} else if (HAS_PCH_SPLIT(dev_priv)) {
 		if (pipe_config->pch_pfit.enabled)
 			ironlake_pfit_enable(crtc);
 		else if (old_crtc_state->pch_pfit.enabled)
@@ -3733,7 +3746,7 @@ static void intel_fdi_normal_train(struct drm_crtc *crtc)
 	/* enable normal train */
 	reg = FDI_TX_CTL(pipe);
 	temp = I915_READ(reg);
-	if (IS_IVYBRIDGE(dev)) {
+	if (IS_IVYBRIDGE(dev_priv)) {
 		temp &= ~FDI_LINK_TRAIN_NONE_IVB;
 		temp |= FDI_LINK_TRAIN_NONE_IVB | FDI_TX_ENHANCE_FRAME_ENABLE;
 	} else {
@@ -3744,7 +3757,7 @@ static void intel_fdi_normal_train(struct drm_crtc *crtc)
 
 	reg = FDI_RX_CTL(pipe);
 	temp = I915_READ(reg);
-	if (HAS_PCH_CPT(dev)) {
+	if (HAS_PCH_CPT(dev_priv)) {
 		temp &= ~FDI_LINK_TRAIN_PATTERN_MASK_CPT;
 		temp |= FDI_LINK_TRAIN_NORMAL_CPT;
 	} else {
@@ -3758,7 +3771,7 @@ static void intel_fdi_normal_train(struct drm_crtc *crtc)
 	udelay(1000);
 
 	/* IVB wants error correction enabled */
-	if (IS_IVYBRIDGE(dev))
+	if (IS_IVYBRIDGE(dev_priv))
 		I915_WRITE(reg, I915_READ(reg) | FDI_FS_ERRC_ENABLE |
 			   FDI_FE_ERRC_ENABLE);
 }
@@ -3902,7 +3915,7 @@ static void gen6_fdi_link_train(struct drm_crtc *crtc)
 
 	reg = FDI_RX_CTL(pipe);
 	temp = I915_READ(reg);
-	if (HAS_PCH_CPT(dev)) {
+	if (HAS_PCH_CPT(dev_priv)) {
 		temp &= ~FDI_LINK_TRAIN_PATTERN_MASK_CPT;
 		temp |= FDI_LINK_TRAIN_PATTERN_1_CPT;
 	} else {
@@ -3946,7 +3959,7 @@ static void gen6_fdi_link_train(struct drm_crtc *crtc)
 	temp = I915_READ(reg);
 	temp &= ~FDI_LINK_TRAIN_NONE;
 	temp |= FDI_LINK_TRAIN_PATTERN_2;
-	if (IS_GEN6(dev)) {
+	if (IS_GEN6(dev_priv)) {
 		temp &= ~FDI_LINK_TRAIN_VOL_EMP_MASK;
 		/* SNB-B */
 		temp |= FDI_LINK_TRAIN_400MV_0DB_SNB_B;
@@ -3955,7 +3968,7 @@ static void gen6_fdi_link_train(struct drm_crtc *crtc)
 
 	reg = FDI_RX_CTL(pipe);
 	temp = I915_READ(reg);
-	if (HAS_PCH_CPT(dev)) {
+	if (HAS_PCH_CPT(dev_priv)) {
 		temp &= ~FDI_LINK_TRAIN_PATTERN_MASK_CPT;
 		temp |= FDI_LINK_TRAIN_PATTERN_2_CPT;
 	} else {
@@ -4209,7 +4222,7 @@ static void ironlake_fdi_disable(struct drm_crtc *crtc)
 	udelay(100);
 
 	/* Ironlake workaround, disable clock pointer after downing FDI */
-	if (HAS_PCH_IBX(dev))
+	if (HAS_PCH_IBX(dev_priv))
 		I915_WRITE(FDI_RX_CHICKEN(pipe), FDI_RX_PHASE_SYNC_POINTER_OVR);
 
 	/* still set train pattern 1 */
@@ -4221,7 +4234,7 @@ static void ironlake_fdi_disable(struct drm_crtc *crtc)
 
 	reg = FDI_RX_CTL(pipe);
 	temp = I915_READ(reg);
-	if (HAS_PCH_CPT(dev)) {
+	if (HAS_PCH_CPT(dev_priv)) {
 		temp &= ~FDI_LINK_TRAIN_PATTERN_MASK_CPT;
 		temp |= FDI_LINK_TRAIN_PATTERN_1_CPT;
 	} else {
@@ -4544,7 +4557,7 @@ static void ironlake_pch_enable(struct drm_crtc *crtc)
 
 	assert_pch_transcoder_disabled(dev_priv, pipe);
 
-	if (IS_IVYBRIDGE(dev))
+	if (IS_IVYBRIDGE(dev_priv))
 		ivybridge_update_fdi_bc_bifurcation(intel_crtc);
 
 	/* Write the TU size bits before fdi link training, so that error
@@ -4557,7 +4570,7 @@ static void ironlake_pch_enable(struct drm_crtc *crtc)
 
 	/* We need to program the right clock selection before writing the pixel
 	 * mutliplier into the DPLL. */
-	if (HAS_PCH_CPT(dev)) {
+	if (HAS_PCH_CPT(dev_priv)) {
 		u32 sel;
 
 		temp = I915_READ(PCH_DPLL_SEL);
@@ -4587,7 +4600,8 @@ static void ironlake_pch_enable(struct drm_crtc *crtc)
 	intel_fdi_normal_train(crtc);
 
 	/* For PCH DP, enable TRANS_DP_CTL */
-	if (HAS_PCH_CPT(dev) && intel_crtc_has_dp_encoder(intel_crtc->config)) {
+	if (HAS_PCH_CPT(dev_priv) &&
+	    intel_crtc_has_dp_encoder(intel_crtc->config)) {
 		const struct drm_display_mode *adjusted_mode =
 			&intel_crtc->config->base.adjusted_mode;
 		u32 bpc = (I915_READ(PIPECONF(pipe)) & PIPECONF_BPC_MASK) >> 5;
@@ -4857,7 +4871,7 @@ static void ironlake_pfit_enable(struct intel_crtc *crtc)
 		 * as some pre-programmed values are broken,
 		 * e.g. x201.
 		 */
-		if (IS_IVYBRIDGE(dev) || IS_HASWELL(dev))
+		if (IS_IVYBRIDGE(dev_priv) || IS_HASWELL(dev_priv))
 			I915_WRITE(PF_CTL(pipe), PF_ENABLE | PF_FILTER_MED_3x3 |
 						 PF_PIPE_SEL_IVB(pipe));
 		else
@@ -4882,7 +4896,7 @@ void hsw_enable_ips(struct intel_crtc *crtc)
 	 */
 
 	assert_plane_enabled(dev_priv, crtc->plane);
-	if (IS_BROADWELL(dev)) {
+	if (IS_BROADWELL(dev_priv)) {
 		mutex_lock(&dev_priv->rps.hw_lock);
 		WARN_ON(sandybridge_pcode_write(dev_priv, DISPLAY_IPS_CONTROL, 0xc0000000));
 		mutex_unlock(&dev_priv->rps.hw_lock);
@@ -4914,7 +4928,7 @@ void hsw_disable_ips(struct intel_crtc *crtc)
 		return;
 
 	assert_plane_enabled(dev_priv, crtc->plane);
-	if (IS_BROADWELL(dev)) {
+	if (IS_BROADWELL(dev_priv)) {
 		mutex_lock(&dev_priv->rps.hw_lock);
 		WARN_ON(sandybridge_pcode_write(dev_priv, DISPLAY_IPS_CONTROL, 0));
 		mutex_unlock(&dev_priv->rps.hw_lock);
@@ -4983,7 +4997,7 @@ intel_post_enable_primary(struct drm_crtc *crtc)
 	 * FIXME: Need to fix the logic to work when we turn off all planes
 	 * but leave the pipe running.
 	 */
-	if (IS_GEN2(dev))
+	if (IS_GEN2(dev_priv))
 		intel_set_cpu_fifo_underrun_reporting(dev_priv, pipe, true);
 
 	/* Underruns don't always raise interrupts, so check manually. */
@@ -5006,7 +5020,7 @@ intel_pre_disable_primary(struct drm_crtc *crtc)
 	 * FIXME: Need to fix the logic to work when we turn off all planes
 	 * but leave the pipe running.
 	 */
-	if (IS_GEN2(dev))
+	if (IS_GEN2(dev_priv))
 		intel_set_cpu_fifo_underrun_reporting(dev_priv, pipe, false);
 
 	/*
@@ -5038,7 +5052,7 @@ intel_pre_disable_primary_noatomic(struct drm_crtc *crtc)
 	 * event which is after the vblank start event, so we need to have a
 	 * wait-for-vblank between disabling the plane and the pipe.
 	 */
-	if (HAS_GMCH_DISPLAY(dev)) {
+	if (HAS_GMCH_DISPLAY(dev_priv)) {
 		intel_set_memory_cxsr(dev_priv, false);
 		dev_priv->wm.vlv.cxsr = false;
 		intel_wait_for_vblank(dev, pipe);
@@ -5103,7 +5117,7 @@ static void intel_pre_plane_update(struct intel_crtc_state *old_crtc_state)
 			intel_pre_disable_primary(&crtc->base);
 	}
 
-	if (pipe_config->disable_cxsr && HAS_GMCH_DISPLAY(dev)) {
+	if (pipe_config->disable_cxsr && HAS_GMCH_DISPLAY(dev_priv)) {
 		crtc->wm.cxsr_allowed = false;
 
 		/*
@@ -5381,7 +5395,7 @@ static void ironlake_crtc_enable(struct intel_crtc_state *pipe_config,
 
 	intel_encoders_enable(crtc, pipe_config, old_state);
 
-	if (HAS_PCH_CPT(dev))
+	if (HAS_PCH_CPT(dev_priv))
 		cpt_verify_modeset(dev, intel_crtc->pipe);
 
 	/* Must wait for vblank to avoid spurious PCH FIFO underruns */
@@ -5394,7 +5408,7 @@ static void ironlake_crtc_enable(struct intel_crtc_state *pipe_config,
 /* IPS only exists on ULT machines and is tied to pipe A. */
 static bool hsw_crtc_supports_ips(struct intel_crtc *crtc)
 {
-	return HAS_IPS(crtc->base.dev) && crtc->pipe == PIPE_A;
+	return HAS_IPS(to_i915(crtc->base.dev)) && crtc->pipe == PIPE_A;
 }
 
 static void haswell_crtc_enable(struct intel_crtc_state *pipe_config,
@@ -5506,7 +5520,7 @@ static void haswell_crtc_enable(struct intel_crtc_state *pipe_config,
 	/* If we change the relative order between pipe/planes enabling, we need
 	 * to change the workaround. */
 	hsw_workaround_pipe = pipe_config->hsw_workaround_pipe;
-	if (IS_HASWELL(dev) && hsw_workaround_pipe != INVALID_PIPE) {
+	if (IS_HASWELL(dev_priv) && hsw_workaround_pipe != INVALID_PIPE) {
 		intel_wait_for_vblank(dev, hsw_workaround_pipe);
 		intel_wait_for_vblank(dev, hsw_workaround_pipe);
 	}
@@ -5563,7 +5577,7 @@ static void ironlake_crtc_disable(struct intel_crtc_state *old_crtc_state,
 	if (intel_crtc->config->has_pch_encoder) {
 		ironlake_disable_pch_transcoder(dev_priv, pipe);
 
-		if (HAS_PCH_CPT(dev)) {
+		if (HAS_PCH_CPT(dev_priv)) {
 			i915_reg_t reg;
 			u32 temp;
 
@@ -5697,13 +5711,13 @@ static enum intel_display_power_domain port_to_aux_power_domain(enum port port)
 enum intel_display_power_domain
 intel_display_port_power_domain(struct intel_encoder *intel_encoder)
 {
-	struct drm_device *dev = intel_encoder->base.dev;
+	struct drm_i915_private *dev_priv = to_i915(intel_encoder->base.dev);
 	struct intel_digital_port *intel_dig_port;
 
 	switch (intel_encoder->type) {
 	case INTEL_OUTPUT_UNKNOWN:
 		/* Only DDI platforms should ever use this output type */
-		WARN_ON_ONCE(!HAS_DDI(dev));
+		WARN_ON_ONCE(!HAS_DDI(dev_priv));
 	case INTEL_OUTPUT_DP:
 	case INTEL_OUTPUT_HDMI:
 	case INTEL_OUTPUT_EDP:
@@ -5724,7 +5738,7 @@ intel_display_port_power_domain(struct intel_encoder *intel_encoder)
 enum intel_display_power_domain
 intel_display_port_aux_power_domain(struct intel_encoder *intel_encoder)
 {
-	struct drm_device *dev = intel_encoder->base.dev;
+	struct drm_i915_private *dev_priv = to_i915(intel_encoder->base.dev);
 	struct intel_digital_port *intel_dig_port;
 
 	switch (intel_encoder->type) {
@@ -5737,7 +5751,7 @@ intel_display_port_aux_power_domain(struct intel_encoder *intel_encoder)
 		 * what's the status of the given connectors, play safe and
 		 * run the DP detection too.
 		 */
-		WARN_ON_ONCE(!HAS_DDI(dev));
+		WARN_ON_ONCE(!HAS_DDI(dev_priv));
 	case INTEL_OUTPUT_DP:
 	case INTEL_OUTPUT_EDP:
 		intel_dig_port = enc_to_dig_port(&intel_encoder->base);
@@ -5833,7 +5847,7 @@ static void intel_update_max_cdclk(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = to_i915(dev);
 
-	if (IS_SKYLAKE(dev) || IS_KABYLAKE(dev)) {
+	if (IS_SKYLAKE(dev_priv) || IS_KABYLAKE(dev_priv)) {
 		u32 limit = I915_READ(SKL_DFSM) & SKL_DFSM_CDCLK_LIMIT_MASK;
 		int max_cdclk, vco;
 
@@ -5855,9 +5869,9 @@ static void intel_update_max_cdclk(struct drm_device *dev)
 			max_cdclk = 308571;
 
 		dev_priv->max_cdclk_freq = skl_calc_cdclk(max_cdclk, vco);
-	} else if (IS_BROXTON(dev)) {
+	} else if (IS_BROXTON(dev_priv)) {
 		dev_priv->max_cdclk_freq = 624000;
-	} else if (IS_BROADWELL(dev))  {
+	} else if (IS_BROADWELL(dev_priv))  {
 		/*
 		 * FIXME with extra cooling we can allow
 		 * 540 MHz for ULX and 675 Mhz for ULT.
@@ -5866,15 +5880,15 @@ static void intel_update_max_cdclk(struct drm_device *dev)
 		 */
 		if (I915_READ(FUSE_STRAP) & HSW_CDCLK_LIMIT)
 			dev_priv->max_cdclk_freq = 450000;
-		else if (IS_BDW_ULX(dev))
+		else if (IS_BDW_ULX(dev_priv))
 			dev_priv->max_cdclk_freq = 450000;
-		else if (IS_BDW_ULT(dev))
+		else if (IS_BDW_ULT(dev_priv))
 			dev_priv->max_cdclk_freq = 540000;
 		else
 			dev_priv->max_cdclk_freq = 675000;
-	} else if (IS_CHERRYVIEW(dev)) {
+	} else if (IS_CHERRYVIEW(dev_priv)) {
 		dev_priv->max_cdclk_freq = 320000;
-	} else if (IS_VALLEYVIEW(dev)) {
+	} else if (IS_VALLEYVIEW(dev_priv)) {
 		dev_priv->max_cdclk_freq = 400000;
 	} else {
 		/* otherwise assume cdclk is fixed */
@@ -6674,7 +6688,7 @@ static void valleyview_modeset_commit_cdclk(struct drm_atomic_state *old_state)
 	 */
 	intel_display_power_get(dev_priv, POWER_DOMAIN_PIPE_A);
 
-	if (IS_CHERRYVIEW(dev))
+	if (IS_CHERRYVIEW(dev_priv))
 		cherryview_set_cdclk(dev, req_cdclk);
 	else
 		valleyview_set_cdclk(dev, req_cdclk);
@@ -6702,7 +6716,7 @@ static void valleyview_crtc_enable(struct intel_crtc_state *pipe_config,
 	intel_set_pipe_timings(intel_crtc);
 	intel_set_pipe_src_size(intel_crtc);
 
-	if (IS_CHERRYVIEW(dev) && pipe == PIPE_B) {
+	if (IS_CHERRYVIEW(dev_priv) && pipe == PIPE_B) {
 		struct drm_i915_private *dev_priv = to_i915(dev);
 
 		I915_WRITE(CHV_BLEND(pipe), CHV_BLEND_LEGACY);
@@ -6717,7 +6731,7 @@ static void valleyview_crtc_enable(struct intel_crtc_state *pipe_config,
 
 	intel_encoders_pre_pll_enable(crtc, pipe_config, old_state);
 
-	if (IS_CHERRYVIEW(dev)) {
+	if (IS_CHERRYVIEW(dev_priv)) {
 		chv_prepare_pll(intel_crtc, intel_crtc->config);
 		chv_enable_pll(intel_crtc, intel_crtc->config);
 	} else {
@@ -6773,7 +6787,7 @@ static void i9xx_crtc_enable(struct intel_crtc_state *pipe_config,
 
 	intel_crtc->active = true;
 
-	if (!IS_GEN2(dev))
+	if (!IS_GEN2(dev_priv))
 		intel_set_cpu_fifo_underrun_reporting(dev_priv, pipe, true);
 
 	intel_encoders_pre_enable(crtc, pipe_config, old_state);
@@ -6821,7 +6835,7 @@ static void i9xx_crtc_disable(struct intel_crtc_state *old_crtc_state,
 	 * On gen2 planes are double buffered but the pipe isn't, so we must
 	 * wait for planes to fully turn off before disabling the pipe.
 	 */
-	if (IS_GEN2(dev))
+	if (IS_GEN2(dev_priv))
 		intel_wait_for_vblank(dev, pipe);
 
 	intel_encoders_disable(crtc, old_crtc_state, old_state);
@@ -6836,9 +6850,9 @@ static void i9xx_crtc_disable(struct intel_crtc_state *old_crtc_state,
 	intel_encoders_post_disable(crtc, old_crtc_state, old_state);
 
 	if (!intel_crtc_has_type(intel_crtc->config, INTEL_OUTPUT_DSI)) {
-		if (IS_CHERRYVIEW(dev))
+		if (IS_CHERRYVIEW(dev_priv))
 			chv_disable_pll(dev_priv, pipe);
-		else if (IS_VALLEYVIEW(dev))
+		else if (IS_VALLEYVIEW(dev_priv))
 			vlv_disable_pll(dev_priv, pipe);
 		else
 			i9xx_disable_pll(intel_crtc);
@@ -6846,7 +6860,7 @@ static void i9xx_crtc_disable(struct intel_crtc_state *old_crtc_state,
 
 	intel_encoders_post_pll_disable(crtc, old_crtc_state, old_state);
 
-	if (!IS_GEN2(dev))
+	if (!IS_GEN2(dev_priv))
 		intel_set_cpu_fifo_underrun_reporting(dev_priv, pipe, false);
 }
 
@@ -7026,6 +7040,7 @@ static int pipe_required_fdi_lanes(struct intel_crtc_state *crtc_state)
 static int ironlake_check_fdi_lanes(struct drm_device *dev, enum pipe pipe,
 				     struct intel_crtc_state *pipe_config)
 {
+	struct drm_i915_private *dev_priv = to_i915(dev);
 	struct drm_atomic_state *state = pipe_config->base.state;
 	struct intel_crtc *other_crtc;
 	struct intel_crtc_state *other_crtc_state;
@@ -7038,7 +7053,7 @@ static int ironlake_check_fdi_lanes(struct drm_device *dev, enum pipe pipe,
 		return -EINVAL;
 	}
 
-	if (IS_HASWELL(dev) || IS_BROADWELL(dev)) {
+	if (IS_HASWELL(dev_priv) || IS_BROADWELL(dev_priv)) {
 		if (pipe_config->fdi_lanes > 2) {
 			DRM_DEBUG_KMS("only 2 lanes on haswell, required: %i lanes\n",
 				      pipe_config->fdi_lanes);
@@ -7223,11 +7238,11 @@ static int intel_crtc_compute_config(struct intel_crtc *crtc,
 	/* Cantiga+ cannot handle modes with a hsync front porch of 0.
 	 * WaPruneModeWithIncorrectHsyncOffset:ctg,elk,ilk,snb,ivb,vlv,hsw.
 	 */
-	if ((INTEL_INFO(dev)->gen > 4 || IS_G4X(dev)) &&
+	if ((INTEL_GEN(dev_priv) > 4 || IS_G4X(dev_priv)) &&
 		adjusted_mode->crtc_hsync_start == adjusted_mode->crtc_hdisplay)
 		return -EINVAL;
 
-	if (HAS_IPS(dev))
+	if (HAS_IPS(dev_priv))
 		hsw_compute_ips_config(crtc, pipe_config);
 
 	if (pipe_config->has_pch_encoder)
@@ -7365,7 +7380,7 @@ static int haswell_get_display_clock_speed(struct drm_device *dev)
 		return 450000;
 	else if (freq == LCPLL_CLK_FREQ_450)
 		return 450000;
-	else if (IS_HSW_ULT(dev))
+	else if (IS_HSW_ULT(dev_priv))
 		return 337500;
 	else
 		return 540000;
@@ -7535,9 +7550,9 @@ static unsigned int intel_hpll_vco(struct drm_device *dev)
 	uint8_t tmp = 0;
 
 	/* FIXME other chipsets? */
-	if (IS_GM45(dev))
+	if (IS_GM45(dev_priv))
 		vco_table = ctg_vco;
-	else if (IS_G4X(dev))
+	else if (IS_G4X(dev_priv))
 		vco_table = elk_vco;
 	else if (IS_CRESTLINE(dev))
 		vco_table = cl_vco;
@@ -7802,8 +7817,8 @@ static void intel_cpu_transcoder_set_m_n(struct intel_crtc *crtc,
 		 * for gen < 8) and if DRRS is supported (to make sure the
 		 * registers are not unnecessarily accessed).
 		 */
-		if (m2_n2 && (IS_CHERRYVIEW(dev) || INTEL_INFO(dev)->gen < 8) &&
-			crtc->config->has_drrs) {
+		if (m2_n2 && (IS_CHERRYVIEW(dev_priv) ||
+		    INTEL_GEN(dev_priv) < 8) && crtc->config->has_drrs) {
 			I915_WRITE(PIPE_DATA_M2(transcoder),
 					TU_SIZE(m2_n2->tu) | m2_n2->gmch_m);
 			I915_WRITE(PIPE_DATA_N2(transcoder), m2_n2->gmch_n);
@@ -8105,7 +8120,7 @@ int vlv_force_pll_on(struct drm_device *dev, enum pipe pipe,
 	pipe_config->pixel_multiplier = 1;
 	pipe_config->dpll = *dpll;
 
-	if (IS_CHERRYVIEW(dev)) {
+	if (IS_CHERRYVIEW(to_i915(dev))) {
 		chv_compute_dpll(crtc, pipe_config);
 		chv_prepare_pll(crtc, pipe_config);
 		chv_enable_pll(crtc, pipe_config);
@@ -8130,7 +8145,7 @@ int vlv_force_pll_on(struct drm_device *dev, enum pipe pipe,
  */
 void vlv_force_pll_off(struct drm_device *dev, enum pipe pipe)
 {
-	if (IS_CHERRYVIEW(dev))
+	if (IS_CHERRYVIEW(to_i915(dev)))
 		chv_disable_pll(to_i915(dev), pipe);
 	else
 		vlv_disable_pll(to_i915(dev), pipe);
@@ -8154,7 +8169,7 @@ static void i9xx_compute_dpll(struct intel_crtc *crtc,
 	else
 		dpll |= DPLLB_MODE_DAC_SERIAL;
 
-	if (IS_I945G(dev) || IS_I945GM(dev) || IS_G33(dev)) {
+	if (IS_I945G(dev_priv) || IS_I945GM(dev_priv) || IS_G33(dev_priv)) {
 		dpll |= (crtc_state->pixel_multiplier - 1)
 			<< SDVO_MULTIPLIER_SHIFT_HIRES;
 	}
@@ -8171,7 +8186,7 @@ static void i9xx_compute_dpll(struct intel_crtc *crtc,
 		dpll |= (1 << (clock->p1 - 1)) << DPLL_FPA01_P1_POST_DIV_SHIFT_PINEVIEW;
 	else {
 		dpll |= (1 << (clock->p1 - 1)) << DPLL_FPA01_P1_POST_DIV_SHIFT;
-		if (IS_G4X(dev) && reduced_clock)
+		if (IS_G4X(dev_priv) && reduced_clock)
 			dpll |= (1 << (reduced_clock->p1 - 1)) << DPLL_FPA1_P1_POST_DIV_SHIFT;
 	}
 	switch (clock->p2) {
@@ -8233,7 +8248,8 @@ static void i8xx_compute_dpll(struct intel_crtc *crtc,
 			dpll |= PLL_P2_DIVIDE_BY_4;
 	}
 
-	if (!IS_I830(dev) && intel_crtc_has_type(crtc_state, INTEL_OUTPUT_DVO))
+	if (!IS_I830(dev_priv) &&
+	    intel_crtc_has_type(crtc_state, INTEL_OUTPUT_DVO))
 		dpll |= DPLL_DVO_2X_MODE;
 
 	if (intel_crtc_has_type(crtc_state, INTEL_OUTPUT_LVDS) &&
@@ -8302,7 +8318,7 @@ static void intel_set_pipe_timings(struct intel_crtc *intel_crtc)
 	 * programmed with the VTOTAL_EDP value. Same for VTOTAL_C. This is
 	 * documented on the DDI_FUNC_CTL register description, EDP Input Select
 	 * bits. */
-	if (IS_HASWELL(dev) && cpu_transcoder == TRANSCODER_EDP &&
+	if (IS_HASWELL(dev_priv) && cpu_transcoder == TRANSCODER_EDP &&
 	    (pipe == PIPE_B || pipe == PIPE_C))
 		I915_WRITE(VTOTAL(pipe), I915_READ(VTOTAL(cpu_transcoder)));
 
@@ -8412,7 +8428,8 @@ static void i9xx_set_pipeconf(struct intel_crtc *intel_crtc)
 		pipeconf |= PIPECONF_DOUBLE_WIDE;
 
 	/* only g4x and later have fancy bpc/dither controls */
-	if (IS_G4X(dev) || IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) {
+	if (IS_G4X(dev_priv) || IS_VALLEYVIEW(dev_priv) ||
+	    IS_CHERRYVIEW(dev_priv)) {
 		/* Bspec claims that we can't use dithering for 30bpp pipes. */
 		if (intel_crtc->config->dither && intel_crtc->config->pipe_bpp != 30)
 			pipeconf |= PIPECONF_DITHER_EN |
@@ -8452,7 +8469,7 @@ static void i9xx_set_pipeconf(struct intel_crtc *intel_crtc)
 	} else
 		pipeconf |= PIPECONF_PROGRESSIVE;
 
-	if ((IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) &&
+	if ((IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv)) &&
 	     intel_crtc->config->limited_color_range)
 		pipeconf |= PIPECONF_COLOR_RANGE_SELECT;
 
@@ -8656,7 +8673,8 @@ static void i9xx_get_pfit_config(struct intel_crtc *crtc,
 	struct drm_i915_private *dev_priv = to_i915(dev);
 	uint32_t tmp;
 
-	if (INTEL_INFO(dev)->gen <= 3 && (IS_I830(dev) || !IS_MOBILE(dev)))
+	if (INTEL_GEN(dev_priv) <= 3 &&
+	    (IS_I830(dev_priv) || !IS_MOBILE(dev_priv)))
 		return;
 
 	tmp = I915_READ(PFIT_CONTROL);
@@ -8828,7 +8846,8 @@ static bool i9xx_get_pipe_config(struct intel_crtc *crtc,
 	if (!(tmp & PIPECONF_ENABLE))
 		goto out;
 
-	if (IS_G4X(dev) || IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) {
+	if (IS_G4X(dev_priv) || IS_VALLEYVIEW(dev_priv) ||
+	    IS_CHERRYVIEW(dev_priv)) {
 		switch (tmp & PIPECONF_BPC_MASK) {
 		case PIPECONF_6BPC:
 			pipe_config->pipe_bpp = 18;
@@ -8844,7 +8863,7 @@ static bool i9xx_get_pipe_config(struct intel_crtc *crtc,
 		}
 	}
 
-	if ((IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) &&
+	if ((IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv)) &&
 	    (tmp & PIPECONF_COLOR_RANGE_SELECT))
 		pipe_config->limited_color_range = true;
 
@@ -8858,7 +8877,7 @@ static bool i9xx_get_pipe_config(struct intel_crtc *crtc,
 
 	if (INTEL_INFO(dev)->gen >= 4) {
 		/* No way to read it out on pipes B and C */
-		if (IS_CHERRYVIEW(dev) && crtc->pipe != PIPE_A)
+		if (IS_CHERRYVIEW(dev_priv) && crtc->pipe != PIPE_A)
 			tmp = dev_priv->chv_dpll_md[crtc->pipe];
 		else
 			tmp = I915_READ(DPLL_MD(crtc->pipe));
@@ -8866,7 +8885,8 @@ static bool i9xx_get_pipe_config(struct intel_crtc *crtc,
 			((tmp & DPLL_MD_UDI_MULTIPLIER_MASK)
 			 >> DPLL_MD_UDI_MULTIPLIER_SHIFT) + 1;
 		pipe_config->dpll_hw_state.dpll_md = tmp;
-	} else if (IS_I945G(dev) || IS_I945GM(dev) || IS_G33(dev)) {
+	} else if (IS_I945G(dev_priv) || IS_I945GM(dev_priv) ||
+		   IS_G33(dev_priv)) {
 		tmp = I915_READ(DPLL(crtc->pipe));
 		pipe_config->pixel_multiplier =
 			((tmp & SDVO_MULTIPLIER_MASK)
@@ -8878,13 +8898,13 @@ static bool i9xx_get_pipe_config(struct intel_crtc *crtc,
 		pipe_config->pixel_multiplier = 1;
 	}
 	pipe_config->dpll_hw_state.dpll = I915_READ(DPLL(crtc->pipe));
-	if (!IS_VALLEYVIEW(dev) && !IS_CHERRYVIEW(dev)) {
+	if (!IS_VALLEYVIEW(dev_priv) && !IS_CHERRYVIEW(dev_priv)) {
 		/*
 		 * DPLL_DVO_2X_MODE must be enabled for both DPLLs
 		 * on 830. Filter it out here so that we don't
 		 * report errors due to that.
 		 */
-		if (IS_I830(dev))
+		if (IS_I830(dev_priv))
 			pipe_config->dpll_hw_state.dpll &= ~DPLL_DVO_2X_MODE;
 
 		pipe_config->dpll_hw_state.fp0 = I915_READ(FP0(crtc->pipe));
@@ -8896,9 +8916,9 @@ static bool i9xx_get_pipe_config(struct intel_crtc *crtc,
 						     DPLL_PORTB_READY_MASK);
 	}
 
-	if (IS_CHERRYVIEW(dev))
+	if (IS_CHERRYVIEW(dev_priv))
 		chv_crtc_clock_get(crtc, pipe_config);
-	else if (IS_VALLEYVIEW(dev))
+	else if (IS_VALLEYVIEW(dev_priv))
 		vlv_crtc_clock_get(crtc, pipe_config);
 	else
 		i9xx_crtc_clock_get(crtc, pipe_config);
@@ -8949,7 +8969,7 @@ static void ironlake_init_pch_refclk(struct drm_device *dev)
 		}
 	}
 
-	if (HAS_PCH_IBX(dev)) {
+	if (HAS_PCH_IBX(dev_priv)) {
 		has_ck505 = dev_priv->vbt.display_clock_mode;
 		can_ssc = has_ck505;
 	} else {
@@ -9197,7 +9217,8 @@ static void lpt_enable_clkout_dp(struct drm_device *dev, bool with_spread,
 
 	if (WARN(with_fdi && !with_spread, "FDI requires downspread\n"))
 		with_spread = true;
-	if (WARN(HAS_PCH_LPT_LP(dev) && with_fdi, "LP PCH doesn't have FDI\n"))
+	if (WARN(HAS_PCH_LPT_LP(dev_priv) &&
+	    with_fdi, "LP PCH doesn't have FDI\n"))
 		with_fdi = false;
 
 	mutex_lock(&dev_priv->sb_lock);
@@ -9220,7 +9241,7 @@ static void lpt_enable_clkout_dp(struct drm_device *dev, bool with_spread,
 		}
 	}
 
-	reg = HAS_PCH_LPT_LP(dev) ? SBI_GEN0 : SBI_DBUFF0;
+	reg = HAS_PCH_LPT_LP(dev_priv) ? SBI_GEN0 : SBI_DBUFF0;
 	tmp = intel_sbi_read(dev_priv, reg, SBI_ICLK);
 	tmp |= SBI_GEN0_CFG_BUFFENABLE_DISABLE;
 	intel_sbi_write(dev_priv, reg, tmp, SBI_ICLK);
@@ -9236,7 +9257,7 @@ static void lpt_disable_clkout_dp(struct drm_device *dev)
 
 	mutex_lock(&dev_priv->sb_lock);
 
-	reg = HAS_PCH_LPT_LP(dev) ? SBI_GEN0 : SBI_DBUFF0;
+	reg = HAS_PCH_LPT_LP(dev_priv) ? SBI_GEN0 : SBI_DBUFF0;
 	tmp = intel_sbi_read(dev_priv, reg, SBI_ICLK);
 	tmp &= ~SBI_GEN0_CFG_BUFFENABLE_DISABLE;
 	intel_sbi_write(dev_priv, reg, tmp, SBI_ICLK);
@@ -9344,9 +9365,11 @@ static void lpt_init_pch_refclk(struct drm_device *dev)
  */
 void intel_init_pch_refclk(struct drm_device *dev)
 {
-	if (HAS_PCH_IBX(dev) || HAS_PCH_CPT(dev))
+	struct drm_i915_private *dev_priv = to_i915(dev);
+
+	if (HAS_PCH_IBX(dev_priv) || HAS_PCH_CPT(dev_priv))
 		ironlake_init_pch_refclk(dev);
-	else if (HAS_PCH_LPT(dev))
+	else if (HAS_PCH_LPT(dev_priv))
 		lpt_init_pch_refclk(dev);
 }
 
@@ -9475,7 +9498,7 @@ static void ironlake_compute_dpll(struct intel_crtc *intel_crtc,
 	if (intel_crtc_has_type(crtc_state, INTEL_OUTPUT_LVDS)) {
 		if ((intel_panel_use_ssc(dev_priv) &&
 		     dev_priv->vbt.lvds_ssc_freq == 100000) ||
-		    (HAS_PCH_IBX(dev) && intel_is_dual_link_lvds(dev)))
+		    (HAS_PCH_IBX(dev_priv) && intel_is_dual_link_lvds(dev)))
 			factor = 25;
 	} else if (crtc_state->sdvo_tv_clock)
 		factor = 20;
@@ -9511,6 +9534,24 @@ static void ironlake_compute_dpll(struct intel_crtc *intel_crtc,
 	if (intel_crtc_has_dp_encoder(crtc_state))
 		dpll |= DPLL_SDVO_HIGH_SPEED;
 
+	/*
+	 * The high speed IO clock is only really required for
+	 * SDVO/HDMI/DP, but we also enable it for CRT to make it
+	 * possible to share the DPLL between CRT and HDMI. Enabling
+	 * the clock needlessly does no real harm, except use up a
+	 * bit of power potentially.
+	 *
+	 * We'll limit this to IVB with 3 pipes, since it has only two
+	 * DPLLs and so DPLL sharing is the only way to get three pipes
+	 * driving PCH ports at the same time. On SNB we could do this,
+	 * and potentially avoid enabling the second DPLL, but it's not
+	 * clear if it''s a win or loss power wise. No point in doing
+	 * this on ILK at all since it has a fixed DPLL<->pipe mapping.
+	 */
+	if (INTEL_INFO(dev_priv)->num_pipes == 3 &&
+	    intel_crtc_has_type(crtc_state, INTEL_OUTPUT_ANALOG))
+		dpll |= DPLL_SDVO_HIGH_SPEED;
+
 	/* compute bitmask from p1 value */
 	dpll |= (1 << (crtc_state->dpll.p1 - 1)) << DPLL_FPA01_P1_POST_DIV_SHIFT;
 	/* also FPA1 */
@@ -9817,7 +9858,7 @@ static void ironlake_get_pfit_config(struct intel_crtc *crtc,
 		/* We currently do not free assignements of panel fitters on
 		 * ivb/hsw (since we don't use the higher upscaling modes which
 		 * differentiates them) so just WARN about this case for now. */
-		if (IS_GEN7(dev)) {
+		if (IS_GEN7(dev_priv)) {
 			WARN_ON((tmp & PF_PIPE_SEL_MASK_IVB) !=
 				PF_PIPE_SEL_IVB(crtc->pipe));
 		}
@@ -9862,7 +9903,7 @@ ironlake_get_initial_plane_config(struct intel_crtc *crtc,
 	fb->bits_per_pixel = drm_format_plane_cpp(fourcc, 0) * 8;
 
 	base = I915_READ(DSPSURF(pipe)) & 0xfffff000;
-	if (IS_HASWELL(dev) || IS_BROADWELL(dev)) {
+	if (IS_HASWELL(dev_priv) || IS_BROADWELL(dev_priv)) {
 		offset = I915_READ(DSPOFFSET(pipe));
 	} else {
 		if (plane_config->tiling)
@@ -10006,7 +10047,7 @@ static void assert_can_disable_lcpll(struct drm_i915_private *dev_priv)
 	I915_STATE_WARN(I915_READ(PP_STATUS(0)) & PP_ON, "Panel power on\n");
 	I915_STATE_WARN(I915_READ(BLC_PWM_CPU_CTL2) & BLM_PWM_ENABLE,
 	     "CPU PWM1 enabled\n");
-	if (IS_HASWELL(dev))
+	if (IS_HASWELL(dev_priv))
 		I915_STATE_WARN(I915_READ(HSW_BLC_PWM2_CTL) & BLM_PWM_ENABLE,
 		     "CPU PWM2 enabled\n");
 	I915_STATE_WARN(I915_READ(BLC_PWM_PCH_CTL1) & BLM_PCH_PWM_ENABLE,
@@ -10026,9 +10067,7 @@ static void assert_can_disable_lcpll(struct drm_i915_private *dev_priv)
 
 static uint32_t hsw_read_dcomp(struct drm_i915_private *dev_priv)
 {
-	struct drm_device *dev = &dev_priv->drm;
-
-	if (IS_HASWELL(dev))
+	if (IS_HASWELL(dev_priv))
 		return I915_READ(D_COMP_HSW);
 	else
 		return I915_READ(D_COMP_BDW);
@@ -10036,9 +10075,7 @@ static uint32_t hsw_read_dcomp(struct drm_i915_private *dev_priv)
 
 static void hsw_write_dcomp(struct drm_i915_private *dev_priv, uint32_t val)
 {
-	struct drm_device *dev = &dev_priv->drm;
-
-	if (IS_HASWELL(dev)) {
+	if (IS_HASWELL(dev_priv)) {
 		mutex_lock(&dev_priv->rps.hw_lock);
 		if (sandybridge_pcode_write(dev_priv, GEN6_PCODE_WRITE_D_COMP,
 					    val))
@@ -10186,7 +10223,7 @@ void hsw_enable_pc8(struct drm_i915_private *dev_priv)
 
 	DRM_DEBUG_KMS("Enabling package C8+\n");
 
-	if (HAS_PCH_LPT_LP(dev)) {
+	if (HAS_PCH_LPT_LP(dev_priv)) {
 		val = I915_READ(SOUTH_DSPCLK_GATE_D);
 		val &= ~PCH_LP_PARTITION_LEVEL_DISABLE;
 		I915_WRITE(SOUTH_DSPCLK_GATE_D, val);
@@ -10206,7 +10243,7 @@ void hsw_disable_pc8(struct drm_i915_private *dev_priv)
 	hsw_restore_lcpll(dev_priv);
 	lpt_init_pch_refclk(dev);
 
-	if (HAS_PCH_LPT_LP(dev)) {
+	if (HAS_PCH_LPT_LP(dev_priv)) {
 		val = I915_READ(SOUTH_DSPCLK_GATE_D);
 		val |= PCH_LP_PARTITION_LEVEL_DISABLE;
 		I915_WRITE(SOUTH_DSPCLK_GATE_D, val);
@@ -10630,9 +10667,9 @@ static void haswell_get_ddi_port_state(struct intel_crtc *crtc,
 
 	port = (tmp & TRANS_DDI_PORT_MASK) >> TRANS_DDI_PORT_SHIFT;
 
-	if (IS_SKYLAKE(dev) || IS_KABYLAKE(dev))
+	if (IS_SKYLAKE(dev_priv) || IS_KABYLAKE(dev_priv))
 		skylake_get_ddi_pll(dev_priv, port, pipe_config);
-	else if (IS_BROXTON(dev))
+	else if (IS_BROXTON(dev_priv))
 		bxt_get_ddi_pll(dev_priv, port, pipe_config);
 	else
 		haswell_get_ddi_pll(dev_priv, port, pipe_config);
@@ -10715,7 +10752,7 @@ static bool haswell_get_pipe_config(struct intel_crtc *crtc,
 			ironlake_get_pfit_config(crtc, pipe_config);
 	}
 
-	if (IS_HASWELL(dev))
+	if (IS_HASWELL(dev_priv))
 		pipe_config->ips_enabled = hsw_crtc_supports_ips(crtc) &&
 			(I915_READ(IPS_CTL) & IPS_ENABLE);
 
@@ -10803,12 +10840,15 @@ static void i9xx_update_cursor(struct drm_crtc *crtc, u32 base,
 	struct drm_device *dev = crtc->dev;
 	struct drm_i915_private *dev_priv = to_i915(dev);
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	struct intel_crtc_state *cstate = to_intel_crtc_state(crtc->state);
 	const struct skl_wm_values *wm = &dev_priv->wm.skl_results;
+	const struct skl_plane_wm *p_wm =
+		&cstate->wm.skl.optimal.planes[PLANE_CURSOR];
 	int pipe = intel_crtc->pipe;
 	uint32_t cntl = 0;
 
 	if (INTEL_GEN(dev_priv) >= 9 && wm->dirty_pipes & drm_crtc_mask(crtc))
-		skl_write_cursor_wm(intel_crtc, wm);
+		skl_write_cursor_wm(intel_crtc, p_wm, &wm->ddb);
 
 	if (plane_state && plane_state->base.visible) {
 		cntl = MCURSOR_GAMMA_ENABLE;
@@ -10828,7 +10868,7 @@ static void i9xx_update_cursor(struct drm_crtc *crtc, u32 base,
 		}
 		cntl |= pipe << 28; /* Connect to correct pipe */
 
-		if (HAS_DDI(dev))
+		if (HAS_DDI(dev_priv))
 			cntl |= CURSOR_PIPE_CSC_ENABLE;
 
 		if (plane_state->base.rotation == DRM_ROTATE_180)
@@ -10876,7 +10916,7 @@ static void intel_crtc_update_cursor(struct drm_crtc *crtc,
 		pos |= y << CURSOR_Y_SHIFT;
 
 		/* ILK+ do this automagically */
-		if (HAS_GMCH_DISPLAY(dev) &&
+		if (HAS_GMCH_DISPLAY(dev_priv) &&
 		    plane_state->base.rotation == DRM_ROTATE_180) {
 			base += (plane_state->base.crtc_h *
 				 plane_state->base.crtc_w - 1) * 4;
@@ -10885,13 +10925,13 @@ static void intel_crtc_update_cursor(struct drm_crtc *crtc,
 
 	I915_WRITE(CURPOS(pipe), pos);
 
-	if (IS_845G(dev) || IS_I865G(dev))
+	if (IS_845G(dev_priv) || IS_I865G(dev_priv))
 		i845_update_cursor(crtc, base, plane_state);
 	else
 		i9xx_update_cursor(crtc, base, plane_state);
 }
 
-static bool cursor_size_ok(struct drm_device *dev,
+static bool cursor_size_ok(struct drm_i915_private *dev_priv,
 			   uint32_t width, uint32_t height)
 {
 	if (width == 0 || height == 0)
@@ -10903,11 +10943,11 @@ static bool cursor_size_ok(struct drm_device *dev,
 	 * the precision of the register. Everything else requires
 	 * square cursors, limited to a few power-of-two sizes.
 	 */
-	if (IS_845G(dev) || IS_I865G(dev)) {
+	if (IS_845G(dev_priv) || IS_I865G(dev_priv)) {
 		if ((width & 63) != 0)
 			return false;
 
-		if (width > (IS_845G(dev) ? 64 : 512))
+		if (width > (IS_845G(dev_priv) ? 64 : 512))
 			return false;
 
 		if (height > 1023)
@@ -10916,7 +10956,7 @@ static bool cursor_size_ok(struct drm_device *dev,
 		switch (width | height) {
 		case 256:
 		case 128:
-			if (IS_GEN2(dev))
+			if (IS_GEN2(dev_priv))
 				return false;
 		case 64:
 			break;
@@ -11299,9 +11339,9 @@ static int i9xx_pll_refclk(struct drm_device *dev,
 
 	if ((dpll & PLL_REF_INPUT_MASK) == PLLB_REF_INPUT_SPREADSPECTRUMIN)
 		return dev_priv->vbt.lvds_ssc_freq;
-	else if (HAS_PCH_SPLIT(dev))
+	else if (HAS_PCH_SPLIT(dev_priv))
 		return 120000;
-	else if (!IS_GEN2(dev))
+	else if (!IS_GEN2(dev_priv))
 		return 96000;
 	else
 		return 48000;
@@ -11334,7 +11374,7 @@ static void i9xx_crtc_clock_get(struct intel_crtc *crtc,
 		clock.m2 = (fp & FP_M2_DIV_MASK) >> FP_M2_DIV_SHIFT;
 	}
 
-	if (!IS_GEN2(dev)) {
+	if (!IS_GEN2(dev_priv)) {
 		if (IS_PINEVIEW(dev))
 			clock.p1 = ffs((dpll & DPLL_FPA01_P1_POST_DIV_MASK_PINEVIEW) >>
 				DPLL_FPA01_P1_POST_DIV_SHIFT_PINEVIEW);
@@ -11362,7 +11402,7 @@ static void i9xx_crtc_clock_get(struct intel_crtc *crtc,
 		else
 			port_clock = i9xx_calc_dpll_params(refclk, &clock);
 	} else {
-		u32 lvds = IS_I830(dev) ? 0 : I915_READ(LVDS);
+		u32 lvds = IS_I830(dev_priv) ? 0 : I915_READ(LVDS);
 		bool is_lvds = (pipe == 1) && (lvds & LVDS_PORT_EN);
 
 		if (is_lvds) {
@@ -11563,7 +11603,7 @@ static bool __pageflip_finished_cs(struct intel_crtc *crtc,
 	 * really needed there. But since ctg has the registers,
 	 * include it in the check anyway.
 	 */
-	if (INTEL_INFO(dev)->gen < 5 && !IS_G4X(dev))
+	if (INTEL_GEN(dev_priv) < 5 && !IS_G4X(dev_priv))
 		return true;
 
 	/*
@@ -11833,6 +11873,7 @@ static int intel_gen7_queue_flip(struct drm_device *dev,
 				 struct drm_i915_gem_request *req,
 				 uint32_t flags)
 {
+	struct drm_i915_private *dev_priv = to_i915(dev);
 	struct intel_ring *ring = req->ring;
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	uint32_t plane_bit = 0;
@@ -11861,7 +11902,7 @@ static int intel_gen7_queue_flip(struct drm_device *dev,
 		 * 48bits addresses, and we need a NOOP for the batch size to
 		 * stay even.
 		 */
-		if (IS_GEN8(dev))
+		if (IS_GEN8(dev_priv))
 			len += 2;
 	}
 
@@ -11898,7 +11939,7 @@ static int intel_gen7_queue_flip(struct drm_device *dev,
 		intel_ring_emit(ring, ~(DERRMR_PIPEA_PRI_FLIP_DONE |
 					  DERRMR_PIPEB_PRI_FLIP_DONE |
 					  DERRMR_PIPEC_PRI_FLIP_DONE));
-		if (IS_GEN8(dev))
+		if (IS_GEN8(dev_priv))
 			intel_ring_emit(ring, MI_STORE_REGISTER_MEM_GEN8 |
 					      MI_SRM_LRM_GLOBAL_GTT);
 		else
@@ -11907,7 +11948,7 @@ static int intel_gen7_queue_flip(struct drm_device *dev,
 		intel_ring_emit_reg(ring, DERRMR);
 		intel_ring_emit(ring,
 				i915_ggtt_offset(req->engine->scratch) + 256);
-		if (IS_GEN8(dev)) {
+		if (IS_GEN8(dev_priv)) {
 			intel_ring_emit(ring, 0);
 			intel_ring_emit(ring, MI_NOOP);
 		}
@@ -12226,23 +12267,23 @@ static int intel_crtc_page_flip(struct drm_crtc *crtc,
 
 	atomic_inc(&intel_crtc->unpin_work_count);
 
-	if (INTEL_INFO(dev)->gen >= 5 || IS_G4X(dev))
+	if (INTEL_GEN(dev_priv) >= 5 || IS_G4X(dev_priv))
 		work->flip_count = I915_READ(PIPE_FLIPCOUNT_G4X(pipe)) + 1;
 
-	if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) {
-		engine = &dev_priv->engine[BCS];
+	if (IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv)) {
+		engine = dev_priv->engine[BCS];
 		if (fb->modifier[0] != old_fb->modifier[0])
 			/* vlv: DISPLAY_FLIP fails to change tiling */
 			engine = NULL;
-	} else if (IS_IVYBRIDGE(dev) || IS_HASWELL(dev)) {
-		engine = &dev_priv->engine[BCS];
+	} else if (IS_IVYBRIDGE(dev_priv) || IS_HASWELL(dev_priv)) {
+		engine = dev_priv->engine[BCS];
 	} else if (INTEL_INFO(dev)->gen >= 7) {
 		engine = i915_gem_active_get_engine(&obj->last_write,
 						    &obj->base.dev->struct_mutex);
 		if (engine == NULL || engine->id != RCS)
-			engine = &dev_priv->engine[BCS];
+			engine = dev_priv->engine[BCS];
 	} else {
-		engine = &dev_priv->engine[RCS];
+		engine = dev_priv->engine[RCS];
 	}
 
 	mmio_flip = use_mmio_flip(engine, obj);
@@ -12273,7 +12314,7 @@ static int intel_crtc_page_flip(struct drm_crtc *crtc,
 
 		work->flip_queued_req = i915_gem_active_get(&obj->last_write,
 							    &obj->base.dev->struct_mutex);
-		schedule_work(&work->mmio_work);
+		queue_work(system_unbound_wq, &work->mmio_work);
 	} else {
 		request = i915_gem_request_alloc(engine, engine->last_context);
 		if (IS_ERR(request)) {
@@ -12430,7 +12471,7 @@ int intel_plane_atomic_calc_changes(struct drm_crtc_state *crtc_state,
 	struct drm_framebuffer *fb = plane_state->fb;
 	int ret;
 
-	if (INTEL_GEN(dev) >= 9 && plane->type != DRM_PLANE_TYPE_CURSOR) {
+	if (INTEL_GEN(dev_priv) >= 9 && plane->type != DRM_PLANE_TYPE_CURSOR) {
 		ret = skl_update_scaler_plane(
 			to_intel_crtc_state(crtc_state),
 			to_intel_plane_state(plane_state));
@@ -12509,7 +12550,7 @@ int intel_plane_atomic_calc_changes(struct drm_crtc_state *crtc_state,
 	 * cstate->update_wm was already set above, so this flag will
 	 * take effect when we commit and program watermarks.
 	 */
-	if (plane->type == DRM_PLANE_TYPE_OVERLAY && IS_IVYBRIDGE(dev) &&
+	if (plane->type == DRM_PLANE_TYPE_OVERLAY && IS_IVYBRIDGE(dev_priv) &&
 	    needs_scaling(to_intel_plane_state(plane_state)) &&
 	    !needs_scaling(old_plane_state))
 		pipe_config->disable_lp_wm = true;
@@ -12685,15 +12726,16 @@ static int
 compute_baseline_pipe_bpp(struct intel_crtc *crtc,
 			  struct intel_crtc_state *pipe_config)
 {
-	struct drm_device *dev = crtc->base.dev;
+	struct drm_i915_private *dev_priv = to_i915(crtc->base.dev);
 	struct drm_atomic_state *state;
 	struct drm_connector *connector;
 	struct drm_connector_state *connector_state;
 	int bpp, i;
 
-	if ((IS_G4X(dev) || IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)))
+	if ((IS_G4X(dev_priv) || IS_VALLEYVIEW(dev_priv) ||
+	    IS_CHERRYVIEW(dev_priv)))
 		bpp = 10*3;
-	else if (INTEL_INFO(dev)->gen >= 5)
+	else if (INTEL_GEN(dev_priv) >= 5)
 		bpp = 12*3;
 	else
 		bpp = 8*3;
@@ -12731,6 +12773,7 @@ static void intel_dump_pipe_config(struct intel_crtc *crtc,
 				   const char *context)
 {
 	struct drm_device *dev = crtc->base.dev;
+	struct drm_i915_private *dev_priv = to_i915(dev);
 	struct drm_plane *plane;
 	struct intel_plane *intel_plane;
 	struct intel_plane_state *state;
@@ -12792,7 +12835,7 @@ static void intel_dump_pipe_config(struct intel_crtc *crtc,
 	DRM_DEBUG_KMS("ips: %i\n", pipe_config->ips_enabled);
 	DRM_DEBUG_KMS("double wide: %i\n", pipe_config->double_wide);
 
-	if (IS_BROXTON(dev)) {
+	if (IS_BROXTON(dev_priv)) {
 		DRM_DEBUG_KMS("dpll_hw_state: ebb0: 0x%x, ebb4: 0x%x,"
 			      "pll0: 0x%x, pll1: 0x%x, pll2: 0x%x, pll3: 0x%x, "
 			      "pll6: 0x%x, pll8: 0x%x, pll9: 0x%x, pll10: 0x%x, pcsdw12: 0x%x\n",
@@ -12807,13 +12850,13 @@ static void intel_dump_pipe_config(struct intel_crtc *crtc,
 			      pipe_config->dpll_hw_state.pll9,
 			      pipe_config->dpll_hw_state.pll10,
 			      pipe_config->dpll_hw_state.pcsdw12);
-	} else if (IS_SKYLAKE(dev) || IS_KABYLAKE(dev)) {
+	} else if (IS_SKYLAKE(dev_priv) || IS_KABYLAKE(dev_priv)) {
 		DRM_DEBUG_KMS("dpll_hw_state: "
 			      "ctrl1: 0x%x, cfgcr1: 0x%x, cfgcr2: 0x%x\n",
 			      pipe_config->dpll_hw_state.ctrl1,
 			      pipe_config->dpll_hw_state.cfgcr1,
 			      pipe_config->dpll_hw_state.cfgcr2);
-	} else if (HAS_DDI(dev)) {
+	} else if (HAS_DDI(dev_priv)) {
 		DRM_DEBUG_KMS("dpll_hw_state: wrpll: 0x%x spll: 0x%x\n",
 			      pipe_config->dpll_hw_state.wrpll,
 			      pipe_config->dpll_hw_state.spll);
@@ -12891,7 +12934,7 @@ static bool check_digital_port_conflicts(struct drm_atomic_state *state)
 		switch (encoder->type) {
 			unsigned int port_mask;
 		case INTEL_OUTPUT_UNKNOWN:
-			if (WARN_ON(!HAS_DDI(dev)))
+			if (WARN_ON(!HAS_DDI(to_i915(dev))))
 				break;
 		case INTEL_OUTPUT_DP:
 		case INTEL_OUTPUT_HDMI:
@@ -13177,6 +13220,7 @@ intel_pipe_config_compare(struct drm_device *dev,
 			  struct intel_crtc_state *pipe_config,
 			  bool adjust)
 {
+	struct drm_i915_private *dev_priv = to_i915(dev);
 	bool ret = true;
 
 #define INTEL_ERR_OR_DBG_KMS(fmt, ...) \
@@ -13322,8 +13366,8 @@ intel_pipe_config_compare(struct drm_device *dev,
 
 	PIPE_CONF_CHECK_I(pixel_multiplier);
 	PIPE_CONF_CHECK_I(has_hdmi_sink);
-	if ((INTEL_INFO(dev)->gen < 8 && !IS_HASWELL(dev)) ||
-	    IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev))
+	if ((INTEL_GEN(dev_priv) < 8 && !IS_HASWELL(dev_priv)) ||
+	    IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv))
 		PIPE_CONF_CHECK_I(limited_color_range);
 	PIPE_CONF_CHECK_I(has_infoframe);
 
@@ -13363,7 +13407,7 @@ intel_pipe_config_compare(struct drm_device *dev,
 	}
 
 	/* BDW+ don't expose a synchronous way to read the state */
-	if (IS_HASWELL(dev))
+	if (IS_HASWELL(dev_priv))
 		PIPE_CONF_CHECK_I(ips_enabled);
 
 	PIPE_CONF_CHECK_I(double_wide);
@@ -13382,7 +13426,7 @@ intel_pipe_config_compare(struct drm_device *dev,
 	PIPE_CONF_CHECK_X(dsi_pll.ctrl);
 	PIPE_CONF_CHECK_X(dsi_pll.div);
 
-	if (IS_G4X(dev) || INTEL_INFO(dev)->gen >= 5)
+	if (IS_G4X(dev_priv) || INTEL_GEN(dev_priv) >= 5)
 		PIPE_CONF_CHECK_I(pipe_bpp);
 
 	PIPE_CONF_CHECK_CLOCK_FUZZY(base.adjusted_mode.crtc_clock);
@@ -13423,30 +13467,65 @@ static void verify_wm_state(struct drm_crtc *crtc,
 	struct drm_device *dev = crtc->dev;
 	struct drm_i915_private *dev_priv = to_i915(dev);
 	struct skl_ddb_allocation hw_ddb, *sw_ddb;
-	struct skl_ddb_entry *hw_entry, *sw_entry;
+	struct skl_pipe_wm hw_wm, *sw_wm;
+	struct skl_plane_wm *hw_plane_wm, *sw_plane_wm;
+	struct skl_ddb_entry *hw_ddb_entry, *sw_ddb_entry;
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	const enum pipe pipe = intel_crtc->pipe;
-	int plane;
+	int plane, level, max_level = ilk_wm_max_level(dev_priv);
 
 	if (INTEL_INFO(dev)->gen < 9 || !new_state->active)
 		return;
 
+	skl_pipe_wm_get_hw_state(crtc, &hw_wm);
+	sw_wm = &intel_crtc->wm.active.skl;
+
 	skl_ddb_get_hw_state(dev_priv, &hw_ddb);
 	sw_ddb = &dev_priv->wm.skl_hw.ddb;
 
 	/* planes */
 	for_each_plane(dev_priv, pipe, plane) {
-		hw_entry = &hw_ddb.plane[pipe][plane];
-		sw_entry = &sw_ddb->plane[pipe][plane];
+		hw_plane_wm = &hw_wm.planes[plane];
+		sw_plane_wm = &sw_wm->planes[plane];
 
-		if (skl_ddb_entry_equal(hw_entry, sw_entry))
-			continue;
+		/* Watermarks */
+		for (level = 0; level <= max_level; level++) {
+			if (skl_wm_level_equals(&hw_plane_wm->wm[level],
+						&sw_plane_wm->wm[level]))
+				continue;
 
-		DRM_ERROR("mismatch in DDB state pipe %c plane %d "
-			  "(expected (%u,%u), found (%u,%u))\n",
-			  pipe_name(pipe), plane + 1,
-			  sw_entry->start, sw_entry->end,
-			  hw_entry->start, hw_entry->end);
+			DRM_ERROR("mismatch in WM pipe %c plane %d level %d (expected e=%d b=%u l=%u, got e=%d b=%u l=%u)\n",
+				  pipe_name(pipe), plane + 1, level,
+				  sw_plane_wm->wm[level].plane_en,
+				  sw_plane_wm->wm[level].plane_res_b,
+				  sw_plane_wm->wm[level].plane_res_l,
+				  hw_plane_wm->wm[level].plane_en,
+				  hw_plane_wm->wm[level].plane_res_b,
+				  hw_plane_wm->wm[level].plane_res_l);
+		}
+
+		if (!skl_wm_level_equals(&hw_plane_wm->trans_wm,
+					 &sw_plane_wm->trans_wm)) {
+			DRM_ERROR("mismatch in trans WM pipe %c plane %d (expected e=%d b=%u l=%u, got e=%d b=%u l=%u)\n",
+				  pipe_name(pipe), plane + 1,
+				  sw_plane_wm->trans_wm.plane_en,
+				  sw_plane_wm->trans_wm.plane_res_b,
+				  sw_plane_wm->trans_wm.plane_res_l,
+				  hw_plane_wm->trans_wm.plane_en,
+				  hw_plane_wm->trans_wm.plane_res_b,
+				  hw_plane_wm->trans_wm.plane_res_l);
+		}
+
+		/* DDB */
+		hw_ddb_entry = &hw_ddb.plane[pipe][plane];
+		sw_ddb_entry = &sw_ddb->plane[pipe][plane];
+
+		if (!skl_ddb_entry_equal(hw_ddb_entry, sw_ddb_entry)) {
+			DRM_ERROR("mismatch in DDB state pipe %c plane %d (expected (%u,%u), found (%u,%u))\n",
+				  pipe_name(pipe), plane + 1,
+				  sw_ddb_entry->start, sw_ddb_entry->end,
+				  hw_ddb_entry->start, hw_ddb_entry->end);
+		}
 	}
 
 	/*
@@ -13456,15 +13535,46 @@ static void verify_wm_state(struct drm_crtc *crtc,
 	 * once the plane becomes visible, we can skip this check
 	 */
 	if (intel_crtc->cursor_addr) {
-		hw_entry = &hw_ddb.plane[pipe][PLANE_CURSOR];
-		sw_entry = &sw_ddb->plane[pipe][PLANE_CURSOR];
+		hw_plane_wm = &hw_wm.planes[PLANE_CURSOR];
+		sw_plane_wm = &sw_wm->planes[PLANE_CURSOR];
+
+		/* Watermarks */
+		for (level = 0; level <= max_level; level++) {
+			if (skl_wm_level_equals(&hw_plane_wm->wm[level],
+						&sw_plane_wm->wm[level]))
+				continue;
+
+			DRM_ERROR("mismatch in WM pipe %c cursor level %d (expected e=%d b=%u l=%u, got e=%d b=%u l=%u)\n",
+				  pipe_name(pipe), level,
+				  sw_plane_wm->wm[level].plane_en,
+				  sw_plane_wm->wm[level].plane_res_b,
+				  sw_plane_wm->wm[level].plane_res_l,
+				  hw_plane_wm->wm[level].plane_en,
+				  hw_plane_wm->wm[level].plane_res_b,
+				  hw_plane_wm->wm[level].plane_res_l);
+		}
+
+		if (!skl_wm_level_equals(&hw_plane_wm->trans_wm,
+					 &sw_plane_wm->trans_wm)) {
+			DRM_ERROR("mismatch in trans WM pipe %c cursor (expected e=%d b=%u l=%u, got e=%d b=%u l=%u)\n",
+				  pipe_name(pipe),
+				  sw_plane_wm->trans_wm.plane_en,
+				  sw_plane_wm->trans_wm.plane_res_b,
+				  sw_plane_wm->trans_wm.plane_res_l,
+				  hw_plane_wm->trans_wm.plane_en,
+				  hw_plane_wm->trans_wm.plane_res_b,
+				  hw_plane_wm->trans_wm.plane_res_l);
+		}
 
-		if (!skl_ddb_entry_equal(hw_entry, sw_entry)) {
-			DRM_ERROR("mismatch in DDB state pipe %c cursor "
-				  "(expected (%u,%u), found (%u,%u))\n",
+		/* DDB */
+		hw_ddb_entry = &hw_ddb.plane[pipe][PLANE_CURSOR];
+		sw_ddb_entry = &sw_ddb->plane[pipe][PLANE_CURSOR];
+
+		if (!skl_ddb_entry_equal(hw_ddb_entry, sw_ddb_entry)) {
+			DRM_ERROR("mismatch in DDB state pipe %c cursor (expected (%u,%u), found (%u,%u))\n",
 				  pipe_name(pipe),
-				  sw_entry->start, sw_entry->end,
-				  hw_entry->start, hw_entry->end);
+				  sw_ddb_entry->start, sw_ddb_entry->end,
+				  hw_ddb_entry->start, hw_ddb_entry->end);
 		}
 	}
 }
@@ -13715,7 +13825,7 @@ intel_modeset_verify_disabled(struct drm_device *dev)
 
 static void update_scanline_offset(struct intel_crtc *crtc)
 {
-	struct drm_device *dev = crtc->base.dev;
+	struct drm_i915_private *dev_priv = to_i915(crtc->base.dev);
 
 	/*
 	 * The scanline counter increments at the leading edge of hsync.
@@ -13735,7 +13845,7 @@ static void update_scanline_offset(struct intel_crtc *crtc)
 	 * there's an extra 1 line difference. So we need to add two instead of
 	 * one to the value.
 	 */
-	if (IS_GEN2(dev)) {
+	if (IS_GEN2(dev_priv)) {
 		const struct drm_display_mode *adjusted_mode = &crtc->config->base.adjusted_mode;
 		int vtotal;
 
@@ -13744,7 +13854,7 @@ static void update_scanline_offset(struct intel_crtc *crtc)
 			vtotal /= 2;
 
 		crtc->scanline_offset = vtotal - 1;
-	} else if (HAS_DDI(dev) &&
+	} else if (HAS_DDI(dev_priv) &&
 		   intel_crtc_has_type(crtc->config, INTEL_OUTPUT_HDMI)) {
 		crtc->scanline_offset = 2;
 	} else
@@ -14222,12 +14332,11 @@ static void skl_update_crtcs(struct drm_atomic_state *state,
 			     unsigned int *crtc_vblank_mask)
 {
 	struct drm_device *dev = state->dev;
-	struct drm_i915_private *dev_priv = to_i915(dev);
 	struct intel_atomic_state *intel_state = to_intel_atomic_state(state);
 	struct drm_crtc *crtc;
+	struct intel_crtc *intel_crtc;
 	struct drm_crtc_state *old_crtc_state;
-	struct skl_ddb_allocation *new_ddb = &intel_state->wm_results.ddb;
-	struct skl_ddb_allocation *cur_ddb = &dev_priv->wm.skl_hw.ddb;
+	struct intel_crtc_state *cstate;
 	unsigned int updated = 0;
 	bool progress;
 	enum pipe pipe;
@@ -14245,12 +14354,14 @@ static void skl_update_crtcs(struct drm_atomic_state *state,
 		for_each_crtc_in_state(state, crtc, old_crtc_state, i) {
 			bool vbl_wait = false;
 			unsigned int cmask = drm_crtc_mask(crtc);
-			pipe = to_intel_crtc(crtc)->pipe;
+
+			intel_crtc = to_intel_crtc(crtc);
+			cstate = to_intel_crtc_state(crtc->state);
+			pipe = intel_crtc->pipe;
 
 			if (updated & cmask || !crtc->state->active)
 				continue;
-			if (skl_ddb_allocation_overlaps(state, cur_ddb, new_ddb,
-							pipe))
+			if (skl_ddb_allocation_overlaps(state, intel_crtc))
 				continue;
 
 			updated |= cmask;
@@ -14261,7 +14372,8 @@ static void skl_update_crtcs(struct drm_atomic_state *state,
 			 * then we need to wait for a vblank to pass for the
 			 * new ddb allocation to take effect.
 			 */
-			if (!skl_ddb_allocation_equals(cur_ddb, new_ddb, pipe) &&
+			if (!skl_ddb_entry_equal(&cstate->wm.skl.ddb,
+						 &intel_crtc->hw_ddb) &&
 			    !crtc->state->active_changed &&
 			    intel_state->wm_results.dirty_pipes != updated)
 				vbl_wait = true;
@@ -14369,8 +14481,8 @@ static void intel_atomic_commit_tail(struct drm_atomic_state *state)
 		 * SKL workaround: bspec recommends we disable the SAGV when we
 		 * have more then one pipe enabled
 		 */
-		if (IS_SKYLAKE(dev_priv) && !skl_can_enable_sagv(state))
-			skl_disable_sagv(dev_priv);
+		if (!intel_can_enable_sagv(state))
+			intel_disable_sagv(dev_priv);
 
 		intel_modeset_verify_disabled(dev);
 	}
@@ -14427,9 +14539,8 @@ static void intel_atomic_commit_tail(struct drm_atomic_state *state)
 		intel_modeset_verify_crtc(crtc, old_crtc_state, crtc->state);
 	}
 
-	if (IS_SKYLAKE(dev_priv) && intel_state->modeset &&
-	    skl_can_enable_sagv(state))
-		skl_enable_sagv(dev_priv);
+	if (intel_state->modeset && intel_can_enable_sagv(state))
+		intel_enable_sagv(dev_priv);
 
 	drm_atomic_helper_commit_hw_done(state);
 
@@ -14642,6 +14753,7 @@ intel_prepare_plane_fb(struct drm_plane *plane,
 		       struct drm_plane_state *new_state)
 {
 	struct drm_device *dev = plane->dev;
+	struct drm_i915_private *dev_priv = to_i915(dev);
 	struct drm_framebuffer *fb = new_state->fb;
 	struct drm_i915_gem_object *obj = intel_fb_obj(fb);
 	struct drm_i915_gem_object *old_obj = intel_fb_obj(plane->state->fb);
@@ -14693,7 +14805,7 @@ intel_prepare_plane_fb(struct drm_plane *plane,
 
 	if (plane->type == DRM_PLANE_TYPE_CURSOR &&
 	    INTEL_INFO(dev)->cursor_needs_physical) {
-		int align = IS_I830(dev) ? 16 * 1024 : 256;
+		int align = IS_I830(dev_priv) ? 16 * 1024 : 256;
 		ret = i915_gem_object_attach_phys(obj, align);
 		if (ret)
 			DRM_DEBUG_KMS("failed to attach phys object\n");
@@ -14818,6 +14930,8 @@ static void intel_begin_crtc_commit(struct drm_crtc *crtc,
 	struct drm_device *dev = crtc->dev;
 	struct drm_i915_private *dev_priv = to_i915(dev);
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	struct intel_crtc_state *intel_cstate =
+		to_intel_crtc_state(crtc->state);
 	struct intel_crtc_state *old_intel_state =
 		to_intel_crtc_state(old_crtc_state);
 	bool modeset = needs_modeset(crtc->state);
@@ -14834,13 +14948,13 @@ static void intel_begin_crtc_commit(struct drm_crtc *crtc,
 		intel_color_load_luts(crtc->state);
 	}
 
-	if (to_intel_crtc_state(crtc->state)->update_pipe)
+	if (intel_cstate->update_pipe) {
 		intel_update_pipe_config(intel_crtc, old_intel_state);
-	else if (INTEL_GEN(dev_priv) >= 9) {
+	} else if (INTEL_GEN(dev_priv) >= 9) {
 		skl_detach_scalers(intel_crtc);
 
 		I915_WRITE(PIPE_WM_LINETIME(pipe),
-			   dev_priv->wm.skl_hw.wm_linetime[pipe]);
+			   intel_cstate->wm.skl.optimal.linetime);
 	}
 }
 
@@ -14883,6 +14997,7 @@ const struct drm_plane_funcs intel_plane_funcs = {
 static struct drm_plane *intel_primary_plane_create(struct drm_device *dev,
 						    int pipe)
 {
+	struct drm_i915_private *dev_priv = to_i915(dev);
 	struct intel_plane *primary = NULL;
 	struct intel_plane_state *state = NULL;
 	const uint32_t *intel_primary_formats;
@@ -14918,7 +15033,7 @@ static struct drm_plane *intel_primary_plane_create(struct drm_device *dev,
 
 		primary->update_plane = skylake_update_primary_plane;
 		primary->disable_plane = skylake_disable_primary_plane;
-	} else if (HAS_PCH_SPLIT(dev)) {
+	} else if (HAS_PCH_SPLIT(dev_priv)) {
 		intel_primary_formats = i965_primary_formats;
 		num_formats = ARRAY_SIZE(i965_primary_formats);
 
@@ -14944,7 +15059,7 @@ static struct drm_plane *intel_primary_plane_create(struct drm_device *dev,
 					       intel_primary_formats, num_formats,
 					       DRM_PLANE_TYPE_PRIMARY,
 					       "plane 1%c", pipe_name(pipe));
-	else if (INTEL_INFO(dev)->gen >= 5 || IS_G4X(dev))
+	else if (INTEL_GEN(dev_priv) >= 5 || IS_G4X(dev_priv))
 		ret = drm_universal_plane_init(dev, &primary->base, 0,
 					       &intel_plane_funcs,
 					       intel_primary_formats, num_formats,
@@ -14959,18 +15074,18 @@ static struct drm_plane *intel_primary_plane_create(struct drm_device *dev,
 	if (ret)
 		goto fail;
 
-	if (INTEL_GEN(dev) >= 9) {
+	if (INTEL_GEN(dev_priv) >= 9) {
 		supported_rotations =
 			DRM_ROTATE_0 | DRM_ROTATE_90 |
 			DRM_ROTATE_180 | DRM_ROTATE_270;
-	} else if (INTEL_GEN(dev) >= 4) {
+	} else if (INTEL_GEN(dev_priv) >= 4) {
 		supported_rotations =
 			DRM_ROTATE_0 | DRM_ROTATE_180;
 	} else {
 		supported_rotations = DRM_ROTATE_0;
 	}
 
-	if (INTEL_GEN(dev) >= 4)
+	if (INTEL_GEN(dev_priv) >= 4)
 		drm_plane_create_rotation_property(&primary->base,
 						   DRM_ROTATE_0,
 						   supported_rotations);
@@ -15010,7 +15125,8 @@ intel_check_cursor_plane(struct drm_plane *plane,
 		return 0;
 
 	/* Check for which cursor types we support */
-	if (!cursor_size_ok(plane->dev, state->base.crtc_w, state->base.crtc_h)) {
+	if (!cursor_size_ok(to_i915(plane->dev), state->base.crtc_w,
+			    state->base.crtc_h)) {
 		DRM_DEBUG("Cursor dimension %dx%d not supported\n",
 			  state->base.crtc_w, state->base.crtc_h);
 		return -EINVAL;
@@ -15037,7 +15153,7 @@ intel_check_cursor_plane(struct drm_plane *plane,
 	 * display power well must be turned off and on again.
 	 * Refuse the put the cursor into that compromised position.
 	 */
-	if (IS_CHERRYVIEW(plane->dev) && pipe == PIPE_C &&
+	if (IS_CHERRYVIEW(to_i915(plane->dev)) && pipe == PIPE_C &&
 	    state->base.visible && state->base.crtc_x < 0) {
 		DRM_DEBUG_KMS("CHV cursor C not allowed to straddle the left screen edge\n");
 		return -EINVAL;
@@ -15081,6 +15197,7 @@ intel_update_cursor_plane(struct drm_plane *plane,
 static struct drm_plane *intel_cursor_plane_create(struct drm_device *dev,
 						   int pipe)
 {
+	struct drm_i915_private *dev_priv = to_i915(dev);
 	struct intel_plane *cursor = NULL;
 	struct intel_plane_state *state = NULL;
 	int ret;
@@ -15112,7 +15229,7 @@ static struct drm_plane *intel_cursor_plane_create(struct drm_device *dev,
 	if (ret)
 		goto fail;
 
-	if (INTEL_GEN(dev) >= 4)
+	if (INTEL_GEN(dev_priv) >= 4)
 		drm_plane_create_rotation_property(&cursor->base,
 						   DRM_ROTATE_0,
 						   DRM_ROTATE_0 |
@@ -15285,7 +15402,7 @@ static bool has_edp_a(struct drm_device *dev)
 	if ((I915_READ(DP_A) & DP_DETECTED) == 0)
 		return false;
 
-	if (IS_GEN5(dev) && (I915_READ(FUSE_STRAP) & ILK_eDP_A_DISABLE))
+	if (IS_GEN5(dev_priv) && (I915_READ(FUSE_STRAP) & ILK_eDP_A_DISABLE))
 		return false;
 
 	return true;
@@ -15298,17 +15415,18 @@ static bool intel_crt_present(struct drm_device *dev)
 	if (INTEL_INFO(dev)->gen >= 9)
 		return false;
 
-	if (IS_HSW_ULT(dev) || IS_BDW_ULT(dev))
+	if (IS_HSW_ULT(dev_priv) || IS_BDW_ULT(dev_priv))
 		return false;
 
-	if (IS_CHERRYVIEW(dev))
+	if (IS_CHERRYVIEW(dev_priv))
 		return false;
 
-	if (HAS_PCH_LPT_H(dev) && I915_READ(SFUSE_STRAP) & SFUSE_STRAP_CRT_DISABLED)
+	if (HAS_PCH_LPT_H(dev_priv) &&
+	    I915_READ(SFUSE_STRAP) & SFUSE_STRAP_CRT_DISABLED)
 		return false;
 
 	/* DDI E can't be used if DDI A requires 4 lanes */
-	if (HAS_DDI(dev) && I915_READ(DDI_BUF_CTL(PORT_A)) & DDI_A_4_LANES)
+	if (HAS_DDI(dev_priv) && I915_READ(DDI_BUF_CTL(PORT_A)) & DDI_A_4_LANES)
 		return false;
 
 	if (!dev_priv->vbt.int_crt_support)
@@ -15371,7 +15489,7 @@ static void intel_setup_outputs(struct drm_device *dev)
 	if (intel_crt_present(dev))
 		intel_crt_init(dev);
 
-	if (IS_BROXTON(dev)) {
+	if (IS_BROXTON(dev_priv)) {
 		/*
 		 * FIXME: Broxton doesn't support port detection via the
 		 * DDI_BUF_CTL_A or SFUSE_STRAP registers, find another way to
@@ -15382,7 +15500,7 @@ static void intel_setup_outputs(struct drm_device *dev)
 		intel_ddi_init(dev, PORT_C);
 
 		intel_dsi_init(dev);
-	} else if (HAS_DDI(dev)) {
+	} else if (HAS_DDI(dev_priv)) {
 		int found;
 
 		/*
@@ -15392,7 +15510,7 @@ static void intel_setup_outputs(struct drm_device *dev)
 		 */
 		found = I915_READ(DDI_BUF_CTL(PORT_A)) & DDI_INIT_DISPLAY_DETECTED;
 		/* WaIgnoreDDIAStrap: skl */
-		if (found || IS_SKYLAKE(dev) || IS_KABYLAKE(dev))
+		if (found || IS_SKYLAKE(dev_priv) || IS_KABYLAKE(dev_priv))
 			intel_ddi_init(dev, PORT_A);
 
 		/* DDI B, C and D detection is indicated by the SFUSE_STRAP
@@ -15408,13 +15526,13 @@ static void intel_setup_outputs(struct drm_device *dev)
 		/*
 		 * On SKL we don't have a way to detect DDI-E so we rely on VBT.
 		 */
-		if ((IS_SKYLAKE(dev) || IS_KABYLAKE(dev)) &&
+		if ((IS_SKYLAKE(dev_priv) || IS_KABYLAKE(dev_priv)) &&
 		    (dev_priv->vbt.ddi_port_info[PORT_E].supports_dp ||
 		     dev_priv->vbt.ddi_port_info[PORT_E].supports_dvi ||
 		     dev_priv->vbt.ddi_port_info[PORT_E].supports_hdmi))
 			intel_ddi_init(dev, PORT_E);
 
-	} else if (HAS_PCH_SPLIT(dev)) {
+	} else if (HAS_PCH_SPLIT(dev_priv)) {
 		int found;
 		dpd_is_edp = intel_dp_is_edp(dev, PORT_D);
 
@@ -15441,7 +15559,7 @@ static void intel_setup_outputs(struct drm_device *dev)
 
 		if (I915_READ(PCH_DP_D) & DP_DETECTED)
 			intel_dp_init(dev, PCH_DP_D, PORT_D);
-	} else if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) {
+	} else if (IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv)) {
 		bool has_edp, has_port;
 
 		/*
@@ -15473,7 +15591,7 @@ static void intel_setup_outputs(struct drm_device *dev)
 		if ((I915_READ(VLV_HDMIC) & SDVO_DETECTED || has_port) && !has_edp)
 			intel_hdmi_init(dev, VLV_HDMIC, PORT_C);
 
-		if (IS_CHERRYVIEW(dev)) {
+		if (IS_CHERRYVIEW(dev_priv)) {
 			/*
 			 * eDP not supported on port D,
 			 * so no need to worry about it
@@ -15486,18 +15604,18 @@ static void intel_setup_outputs(struct drm_device *dev)
 		}
 
 		intel_dsi_init(dev);
-	} else if (!IS_GEN2(dev) && !IS_PINEVIEW(dev)) {
+	} else if (!IS_GEN2(dev_priv) && !IS_PINEVIEW(dev_priv)) {
 		bool found = false;
 
 		if (I915_READ(GEN3_SDVOB) & SDVO_DETECTED) {
 			DRM_DEBUG_KMS("probing SDVOB\n");
 			found = intel_sdvo_init(dev, GEN3_SDVOB, PORT_B);
-			if (!found && IS_G4X(dev)) {
+			if (!found && IS_G4X(dev_priv)) {
 				DRM_DEBUG_KMS("probing HDMI on SDVOB\n");
 				intel_hdmi_init(dev, GEN4_HDMIB, PORT_B);
 			}
 
-			if (!found && IS_G4X(dev))
+			if (!found && IS_G4X(dev_priv))
 				intel_dp_init(dev, DP_B, PORT_B);
 		}
 
@@ -15510,18 +15628,17 @@ static void intel_setup_outputs(struct drm_device *dev)
 
 		if (!found && (I915_READ(GEN3_SDVOC) & SDVO_DETECTED)) {
 
-			if (IS_G4X(dev)) {
+			if (IS_G4X(dev_priv)) {
 				DRM_DEBUG_KMS("probing HDMI on SDVOC\n");
 				intel_hdmi_init(dev, GEN4_HDMIC, PORT_C);
 			}
-			if (IS_G4X(dev))
+			if (IS_G4X(dev_priv))
 				intel_dp_init(dev, DP_C, PORT_C);
 		}
 
-		if (IS_G4X(dev) &&
-		    (I915_READ(DP_D) & DP_DETECTED))
+		if (IS_G4X(dev_priv) && (I915_READ(DP_D) & DP_DETECTED))
 			intel_dp_init(dev, DP_D, PORT_D);
-	} else if (IS_GEN2(dev))
+	} else if (IS_GEN2(dev_priv))
 		intel_dvo_init(dev);
 
 	if (SUPPORTS_TV(dev))
@@ -15592,10 +15709,10 @@ static const struct drm_framebuffer_funcs intel_fb_funcs = {
 };
 
 static
-u32 intel_fb_pitch_limit(struct drm_device *dev, uint64_t fb_modifier,
-			 uint32_t pixel_format)
+u32 intel_fb_pitch_limit(struct drm_i915_private *dev_priv,
+			 uint64_t fb_modifier, uint32_t pixel_format)
 {
-	u32 gen = INTEL_INFO(dev)->gen;
+	u32 gen = INTEL_INFO(dev_priv)->gen;
 
 	if (gen >= 9) {
 		int cpp = drm_format_plane_cpp(pixel_format, 0);
@@ -15604,7 +15721,8 @@ u32 intel_fb_pitch_limit(struct drm_device *dev, uint64_t fb_modifier,
 		 *  pixels and 32K bytes."
 		 */
 		return min(8192 * cpp, 32768);
-	} else if (gen >= 5 && !IS_VALLEYVIEW(dev) && !IS_CHERRYVIEW(dev)) {
+	} else if (gen >= 5 && !IS_VALLEYVIEW(dev_priv) &&
+		   !IS_CHERRYVIEW(dev_priv)) {
 		return 32*1024;
 	} else if (gen >= 4) {
 		if (fb_modifier == I915_FORMAT_MOD_X_TILED)
@@ -15691,7 +15809,7 @@ static int intel_framebuffer_init(struct drm_device *dev,
 		return -EINVAL;
 	}
 
-	pitch_limit = intel_fb_pitch_limit(dev, mode_cmd->modifier[0],
+	pitch_limit = intel_fb_pitch_limit(dev_priv, mode_cmd->modifier[0],
 					   mode_cmd->pixel_format);
 	if (mode_cmd->pitches[0] > pitch_limit) {
 		DRM_DEBUG("%s pitch (%u) must be at less than %d\n",
@@ -15729,7 +15847,7 @@ static int intel_framebuffer_init(struct drm_device *dev,
 		}
 		break;
 	case DRM_FORMAT_ABGR8888:
-		if (!IS_VALLEYVIEW(dev) && !IS_CHERRYVIEW(dev) &&
+		if (!IS_VALLEYVIEW(dev_priv) && !IS_CHERRYVIEW(dev_priv) &&
 		    INTEL_INFO(dev)->gen < 9) {
 			format_name = drm_get_format_name(mode_cmd->pixel_format);
 			DRM_DEBUG("unsupported pixel format: %s\n", format_name);
@@ -15748,7 +15866,7 @@ static int intel_framebuffer_init(struct drm_device *dev,
 		}
 		break;
 	case DRM_FORMAT_ABGR2101010:
-		if (!IS_VALLEYVIEW(dev) && !IS_CHERRYVIEW(dev)) {
+		if (!IS_VALLEYVIEW(dev_priv) && !IS_CHERRYVIEW(dev_priv)) {
 			format_name = drm_get_format_name(mode_cmd->pixel_format);
 			DRM_DEBUG("unsupported pixel format: %s\n", format_name);
 			kfree(format_name);
@@ -15815,12 +15933,6 @@ intel_user_framebuffer_create(struct drm_device *dev,
 	return fb;
 }
 
-#ifndef CONFIG_DRM_FBDEV_EMULATION
-static inline void intel_fbdev_output_poll_changed(struct drm_device *dev)
-{
-}
-#endif
-
 static const struct drm_mode_config_funcs intel_mode_funcs = {
 	.fb_create = intel_user_framebuffer_create,
 	.output_poll_changed = intel_fbdev_output_poll_changed,
@@ -16201,7 +16313,7 @@ static void i915_disable_vga(struct drm_device *dev)
 	struct drm_i915_private *dev_priv = to_i915(dev);
 	struct pci_dev *pdev = dev_priv->drm.pdev;
 	u8 sr1;
-	i915_reg_t vga_reg = i915_vgacntrl_reg(dev);
+	i915_reg_t vga_reg = i915_vgacntrl_reg(dev_priv);
 
 	/* WaEnableVGAAccessThroughIOPort:ctg,elk,ilk,snb,ivb,vlv,hsw */
 	vga_get_uninterruptible(pdev, VGA_RSRC_LEGACY_IO);
@@ -16340,7 +16452,7 @@ void intel_modeset_init(struct drm_device *dev)
 	 * BIOS isn't using it, don't assume it will work even if the VBT
 	 * indicates as much.
 	 */
-	if (HAS_PCH_IBX(dev) || HAS_PCH_CPT(dev)) {
+	if (HAS_PCH_IBX(dev_priv) || HAS_PCH_CPT(dev_priv)) {
 		bool bios_lvds_use_ssc = !!(I915_READ(PCH_DREF_CONTROL) &
 					    DREF_SSC1_ENABLE);
 
@@ -16352,10 +16464,10 @@ void intel_modeset_init(struct drm_device *dev)
 		}
 	}
 
-	if (IS_GEN2(dev)) {
+	if (IS_GEN2(dev_priv)) {
 		dev->mode_config.max_width = 2048;
 		dev->mode_config.max_height = 2048;
-	} else if (IS_GEN3(dev)) {
+	} else if (IS_GEN3(dev_priv)) {
 		dev->mode_config.max_width = 4096;
 		dev->mode_config.max_height = 4096;
 	} else {
@@ -16363,10 +16475,10 @@ void intel_modeset_init(struct drm_device *dev)
 		dev->mode_config.max_height = 8192;
 	}
 
-	if (IS_845G(dev) || IS_I865G(dev)) {
-		dev->mode_config.cursor_width = IS_845G(dev) ? 64 : 512;
+	if (IS_845G(dev_priv) || IS_I865G(dev_priv)) {
+		dev->mode_config.cursor_width = IS_845G(dev_priv) ? 64 : 512;
 		dev->mode_config.cursor_height = 1023;
-	} else if (IS_GEN2(dev)) {
+	} else if (IS_GEN2(dev_priv)) {
 		dev->mode_config.cursor_width = GEN2_CURSOR_WIDTH;
 		dev->mode_config.cursor_height = GEN2_CURSOR_HEIGHT;
 	} else {
@@ -16572,7 +16684,7 @@ static void intel_sanitize_crtc(struct intel_crtc *crtc)
 	if (crtc->active && !intel_crtc_has_encoders(crtc))
 		intel_crtc_disable_noatomic(&crtc->base);
 
-	if (crtc->active || HAS_GMCH_DISPLAY(dev)) {
+	if (crtc->active || HAS_GMCH_DISPLAY(dev_priv)) {
 		/*
 		 * We start out with underrun reporting disabled to avoid races.
 		 * For correct bookkeeping mark this on active crtcs.
@@ -16647,7 +16759,7 @@ static void intel_sanitize_encoder(struct intel_encoder *encoder)
 void i915_redisable_vga_power_on(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = to_i915(dev);
-	i915_reg_t vga_reg = i915_vgacntrl_reg(dev);
+	i915_reg_t vga_reg = i915_vgacntrl_reg(dev_priv);
 
 	if (!(I915_READ(vga_reg) & VGA_DISP_DISABLE)) {
 		DRM_DEBUG_KMS("Something enabled VGA plane, disabling it\n");
@@ -16885,11 +16997,11 @@ intel_modeset_setup_hw_state(struct drm_device *dev)
 		pll->on = false;
 	}
 
-	if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev))
+	if (IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv))
 		vlv_wm_get_hw_state(dev);
-	else if (IS_GEN9(dev))
+	else if (IS_GEN9(dev_priv))
 		skl_wm_get_hw_state(dev);
-	else if (HAS_PCH_SPLIT(dev))
+	else if (HAS_PCH_SPLIT(dev_priv))
 		ilk_wm_get_hw_state(dev);
 
 	for_each_intel_crtc(dev, crtc) {
@@ -17080,6 +17192,8 @@ int intel_modeset_vga_set_state(struct drm_device *dev, bool state)
 	return 0;
 }
 
+#if IS_ENABLED(CONFIG_DRM_I915_CAPTURE_ERROR)
+
 struct intel_display_error_state {
 
 	u32 power_well_driver;
@@ -17218,7 +17332,7 @@ intel_display_print_error_state(struct drm_i915_error_state_buf *m,
 		return;
 
 	err_printf(m, "Num Pipes: %d\n", INTEL_INFO(dev)->num_pipes);
-	if (IS_HASWELL(dev) || IS_BROADWELL(dev))
+	if (IS_HASWELL(dev_priv) || IS_BROADWELL(dev_priv))
 		err_printf(m, "PWR_WELL_CTL2: %08x\n",
 			   error->power_well_driver);
 	for_each_pipe(dev_priv, i) {
@@ -17235,7 +17349,7 @@ intel_display_print_error_state(struct drm_i915_error_state_buf *m,
 			err_printf(m, "  SIZE: %08x\n", error->plane[i].size);
 			err_printf(m, "  POS: %08x\n", error->plane[i].pos);
 		}
-		if (INTEL_INFO(dev)->gen <= 7 && !IS_HASWELL(dev))
+		if (INTEL_GEN(dev_priv) <= 7 && !IS_HASWELL(dev_priv))
 			err_printf(m, "  ADDR: %08x\n", error->plane[i].addr);
 		if (INTEL_INFO(dev)->gen >= 4) {
 			err_printf(m, "  SURF: %08x\n", error->plane[i].surface);
@@ -17262,3 +17376,5 @@ intel_display_print_error_state(struct drm_i915_error_state_buf *m,
 		err_printf(m, "  VSYNC: %08x\n", error->transcoder[i].vsync);
 	}
 }
+
+#endif
diff --git a/drivers/gpu/drm/i915/intel_dp.c b/drivers/gpu/drm/i915/intel_dp.c
index 01bc67194dba..3c2293bd24bf 100644
--- a/drivers/gpu/drm/i915/intel_dp.c
+++ b/drivers/gpu/drm/i915/intel_dp.c
@@ -344,7 +344,7 @@ vlv_power_sequencer_kick(struct intel_dp *intel_dp)
 	DP |= DP_PORT_WIDTH(1);
 	DP |= DP_LINK_TRAIN_PAT_1;
 
-	if (IS_CHERRYVIEW(dev))
+	if (IS_CHERRYVIEW(dev_priv))
 		DP |= DP_PIPE_SELECT_CHV(pipe);
 	else if (pipe == PIPE_B)
 		DP |= DP_PIPEB_SELECT;
@@ -356,10 +356,10 @@ vlv_power_sequencer_kick(struct intel_dp *intel_dp)
 	 * So enable temporarily it if it's not already enabled.
 	 */
 	if (!pll_enabled) {
-		release_cl_override = IS_CHERRYVIEW(dev) &&
+		release_cl_override = IS_CHERRYVIEW(dev_priv) &&
 			!chv_phy_powergate_ch(dev_priv, phy, ch, true);
 
-		if (vlv_force_pll_on(dev, pipe, IS_CHERRYVIEW(dev) ?
+		if (vlv_force_pll_on(dev, pipe, IS_CHERRYVIEW(dev_priv) ?
 				     &chv_dpll[0].dpll : &vlv_dpll[0].dpll)) {
 			DRM_ERROR("Failed to force on pll for pipe %c!\n",
 				  pipe_name(pipe));
@@ -570,8 +570,8 @@ void intel_power_sequencer_reset(struct drm_i915_private *dev_priv)
 	struct drm_device *dev = &dev_priv->drm;
 	struct intel_encoder *encoder;
 
-	if (WARN_ON(!IS_VALLEYVIEW(dev) && !IS_CHERRYVIEW(dev) &&
-		    !IS_BROXTON(dev)))
+	if (WARN_ON(!IS_VALLEYVIEW(dev_priv) && !IS_CHERRYVIEW(dev_priv) &&
+		    !IS_BROXTON(dev_priv)))
 		return;
 
 	/*
@@ -591,7 +591,7 @@ void intel_power_sequencer_reset(struct drm_i915_private *dev_priv)
 			continue;
 
 		intel_dp = enc_to_intel_dp(&encoder->base);
-		if (IS_BROXTON(dev))
+		if (IS_BROXTON(dev_priv))
 			intel_dp->pps_reset = true;
 		else
 			intel_dp->pps_pipe = INVALID_PIPE;
@@ -664,7 +664,7 @@ static int edp_notify_handler(struct notifier_block *this, unsigned long code,
 
 	pps_lock(intel_dp);
 
-	if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) {
+	if (IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv)) {
 		enum pipe pipe = vlv_power_sequencer_pipe(intel_dp);
 		i915_reg_t pp_ctrl_reg, pp_div_reg;
 		u32 pp_div;
@@ -692,7 +692,7 @@ static bool edp_have_panel_power(struct intel_dp *intel_dp)
 
 	lockdep_assert_held(&dev_priv->pps_mutex);
 
-	if ((IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) &&
+	if ((IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv)) &&
 	    intel_dp->pps_pipe == INVALID_PIPE)
 		return false;
 
@@ -706,7 +706,7 @@ static bool edp_have_panel_vdd(struct intel_dp *intel_dp)
 
 	lockdep_assert_held(&dev_priv->pps_mutex);
 
-	if ((IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) &&
+	if ((IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv)) &&
 	    intel_dp->pps_pipe == INVALID_PIPE)
 		return false;
 
@@ -821,15 +821,16 @@ static uint32_t g4x_get_aux_send_ctl(struct intel_dp *intel_dp,
 				     uint32_t aux_clock_divider)
 {
 	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
-	struct drm_device *dev = intel_dig_port->base.base.dev;
+	struct drm_i915_private *dev_priv =
+			to_i915(intel_dig_port->base.base.dev);
 	uint32_t precharge, timeout;
 
-	if (IS_GEN6(dev))
+	if (IS_GEN6(dev_priv))
 		precharge = 3;
 	else
 		precharge = 5;
 
-	if (IS_BROADWELL(dev) && intel_dig_port->port == PORT_A)
+	if (IS_BROADWELL(dev_priv) && intel_dig_port->port == PORT_A)
 		timeout = DP_AUX_CH_CTL_TIME_OUT_600us;
 	else
 		timeout = DP_AUX_CH_CTL_TIME_OUT_400us;
@@ -1108,8 +1109,46 @@ intel_dp_aux_transfer(struct drm_dp_aux *aux, struct drm_dp_aux_msg *msg)
 	return ret;
 }
 
+static enum port intel_aux_port(struct drm_i915_private *dev_priv,
+				enum port port)
+{
+	const struct ddi_vbt_port_info *info =
+		&dev_priv->vbt.ddi_port_info[port];
+	enum port aux_port;
+
+	if (!info->alternate_aux_channel) {
+		DRM_DEBUG_KMS("using AUX %c for port %c (platform default)\n",
+			      port_name(port), port_name(port));
+		return port;
+	}
+
+	switch (info->alternate_aux_channel) {
+	case DP_AUX_A:
+		aux_port = PORT_A;
+		break;
+	case DP_AUX_B:
+		aux_port = PORT_B;
+		break;
+	case DP_AUX_C:
+		aux_port = PORT_C;
+		break;
+	case DP_AUX_D:
+		aux_port = PORT_D;
+		break;
+	default:
+		MISSING_CASE(info->alternate_aux_channel);
+		aux_port = PORT_A;
+		break;
+	}
+
+	DRM_DEBUG_KMS("using AUX %c for port %c (VBT)\n",
+		      port_name(aux_port), port_name(port));
+
+	return aux_port;
+}
+
 static i915_reg_t g4x_aux_ctl_reg(struct drm_i915_private *dev_priv,
-				       enum port port)
+				  enum port port)
 {
 	switch (port) {
 	case PORT_B:
@@ -1123,7 +1162,7 @@ static i915_reg_t g4x_aux_ctl_reg(struct drm_i915_private *dev_priv,
 }
 
 static i915_reg_t g4x_aux_data_reg(struct drm_i915_private *dev_priv,
-					enum port port, int index)
+				   enum port port, int index)
 {
 	switch (port) {
 	case PORT_B:
@@ -1137,7 +1176,7 @@ static i915_reg_t g4x_aux_data_reg(struct drm_i915_private *dev_priv,
 }
 
 static i915_reg_t ilk_aux_ctl_reg(struct drm_i915_private *dev_priv,
-				       enum port port)
+				  enum port port)
 {
 	switch (port) {
 	case PORT_A:
@@ -1153,7 +1192,7 @@ static i915_reg_t ilk_aux_ctl_reg(struct drm_i915_private *dev_priv,
 }
 
 static i915_reg_t ilk_aux_data_reg(struct drm_i915_private *dev_priv,
-					enum port port, int index)
+				   enum port port, int index)
 {
 	switch (port) {
 	case PORT_A:
@@ -1168,36 +1207,9 @@ static i915_reg_t ilk_aux_data_reg(struct drm_i915_private *dev_priv,
 	}
 }
 
-/*
- * On SKL we don't have Aux for port E so we rely
- * on VBT to set a proper alternate aux channel.
- */
-static enum port skl_porte_aux_port(struct drm_i915_private *dev_priv)
-{
-	const struct ddi_vbt_port_info *info =
-		&dev_priv->vbt.ddi_port_info[PORT_E];
-
-	switch (info->alternate_aux_channel) {
-	case DP_AUX_A:
-		return PORT_A;
-	case DP_AUX_B:
-		return PORT_B;
-	case DP_AUX_C:
-		return PORT_C;
-	case DP_AUX_D:
-		return PORT_D;
-	default:
-		MISSING_CASE(info->alternate_aux_channel);
-		return PORT_A;
-	}
-}
-
 static i915_reg_t skl_aux_ctl_reg(struct drm_i915_private *dev_priv,
-				       enum port port)
+				  enum port port)
 {
-	if (port == PORT_E)
-		port = skl_porte_aux_port(dev_priv);
-
 	switch (port) {
 	case PORT_A:
 	case PORT_B:
@@ -1211,11 +1223,8 @@ static i915_reg_t skl_aux_ctl_reg(struct drm_i915_private *dev_priv,
 }
 
 static i915_reg_t skl_aux_data_reg(struct drm_i915_private *dev_priv,
-					enum port port, int index)
+				   enum port port, int index)
 {
-	if (port == PORT_E)
-		port = skl_porte_aux_port(dev_priv);
-
 	switch (port) {
 	case PORT_A:
 	case PORT_B:
@@ -1229,7 +1238,7 @@ static i915_reg_t skl_aux_data_reg(struct drm_i915_private *dev_priv,
 }
 
 static i915_reg_t intel_aux_ctl_reg(struct drm_i915_private *dev_priv,
-					 enum port port)
+				    enum port port)
 {
 	if (INTEL_INFO(dev_priv)->gen >= 9)
 		return skl_aux_ctl_reg(dev_priv, port);
@@ -1240,7 +1249,7 @@ static i915_reg_t intel_aux_ctl_reg(struct drm_i915_private *dev_priv,
 }
 
 static i915_reg_t intel_aux_data_reg(struct drm_i915_private *dev_priv,
-					  enum port port, int index)
+				     enum port port, int index)
 {
 	if (INTEL_INFO(dev_priv)->gen >= 9)
 		return skl_aux_data_reg(dev_priv, port, index);
@@ -1253,7 +1262,8 @@ static i915_reg_t intel_aux_data_reg(struct drm_i915_private *dev_priv,
 static void intel_aux_reg_init(struct intel_dp *intel_dp)
 {
 	struct drm_i915_private *dev_priv = to_i915(intel_dp_to_dev(intel_dp));
-	enum port port = dp_to_dig_port(intel_dp)->port;
+	enum port port = intel_aux_port(dev_priv,
+					dp_to_dig_port(intel_dp)->port);
 	int i;
 
 	intel_dp->aux_ch_ctl_reg = intel_aux_ctl_reg(dev_priv, port);
@@ -1297,14 +1307,10 @@ intel_dp_sink_rates(struct intel_dp *intel_dp, const int **sink_rates)
 bool intel_dp_source_supports_hbr2(struct intel_dp *intel_dp)
 {
 	struct intel_digital_port *dig_port = dp_to_dig_port(intel_dp);
-	struct drm_device *dev = dig_port->base.base.dev;
-
-	/* WaDisableHBR2:skl */
-	if (IS_SKL_REVID(dev, 0, SKL_REVID_B0))
-		return false;
+	struct drm_i915_private *dev_priv = to_i915(dig_port->base.base.dev);
 
-	if ((IS_HASWELL(dev) && !IS_HSW_ULX(dev)) || IS_BROADWELL(dev) ||
-	    (INTEL_INFO(dev)->gen >= 9))
+	if ((IS_HASWELL(dev_priv) && !IS_HSW_ULX(dev_priv)) ||
+	    IS_BROADWELL(dev_priv) || (INTEL_GEN(dev_priv) >= 9))
 		return true;
 	else
 		return false;
@@ -1314,13 +1320,13 @@ static int
 intel_dp_source_rates(struct intel_dp *intel_dp, const int **source_rates)
 {
 	struct intel_digital_port *dig_port = dp_to_dig_port(intel_dp);
-	struct drm_device *dev = dig_port->base.base.dev;
+	struct drm_i915_private *dev_priv = to_i915(dig_port->base.base.dev);
 	int size;
 
-	if (IS_BROXTON(dev)) {
+	if (IS_BROXTON(dev_priv)) {
 		*source_rates = bxt_rates;
 		size = ARRAY_SIZE(bxt_rates);
-	} else if (IS_SKYLAKE(dev) || IS_KABYLAKE(dev)) {
+	} else if (IS_SKYLAKE(dev_priv) || IS_KABYLAKE(dev_priv)) {
 		*source_rates = skl_rates;
 		size = ARRAY_SIZE(skl_rates);
 	} else {
@@ -1340,19 +1346,20 @@ intel_dp_set_clock(struct intel_encoder *encoder,
 		   struct intel_crtc_state *pipe_config)
 {
 	struct drm_device *dev = encoder->base.dev;
+	struct drm_i915_private *dev_priv = to_i915(dev);
 	const struct dp_link_dpll *divisor = NULL;
 	int i, count = 0;
 
-	if (IS_G4X(dev)) {
+	if (IS_G4X(dev_priv)) {
 		divisor = gen4_dpll;
 		count = ARRAY_SIZE(gen4_dpll);
-	} else if (HAS_PCH_SPLIT(dev)) {
+	} else if (HAS_PCH_SPLIT(dev_priv)) {
 		divisor = pch_dpll;
 		count = ARRAY_SIZE(pch_dpll);
-	} else if (IS_CHERRYVIEW(dev)) {
+	} else if (IS_CHERRYVIEW(dev_priv)) {
 		divisor = chv_dpll;
 		count = ARRAY_SIZE(chv_dpll);
-	} else if (IS_VALLEYVIEW(dev)) {
+	} else if (IS_VALLEYVIEW(dev_priv)) {
 		divisor = vlv_dpll;
 		count = ARRAY_SIZE(vlv_dpll);
 	}
@@ -1567,7 +1574,7 @@ intel_dp_compute_config(struct intel_encoder *encoder,
 
 	max_clock = common_len - 1;
 
-	if (HAS_PCH_SPLIT(dev) && !HAS_DDI(dev) && port != PORT_A)
+	if (HAS_PCH_SPLIT(dev_priv) && !HAS_DDI(dev_priv) && port != PORT_A)
 		pipe_config->has_pch_encoder = true;
 
 	pipe_config->has_drrs = false;
@@ -1584,7 +1591,7 @@ intel_dp_compute_config(struct intel_encoder *encoder,
 				return ret;
 		}
 
-		if (HAS_GMCH_DISPLAY(dev))
+		if (HAS_GMCH_DISPLAY(dev_priv))
 			intel_gmch_panel_fitting(intel_crtc, pipe_config,
 						 intel_connector->panel.fitting_mode);
 		else
@@ -1709,7 +1716,7 @@ found:
 		to_intel_atomic_state(pipe_config->base.state)->cdclk_pll_vco = vco;
 	}
 
-	if (!HAS_DDI(dev))
+	if (!HAS_DDI(dev_priv))
 		intel_dp_set_clock(encoder, pipe_config);
 
 	return true;
@@ -1767,7 +1774,7 @@ static void intel_dp_prepare(struct intel_encoder *encoder,
 
 	/* Split out the IBX/CPU vs CPT settings */
 
-	if (IS_GEN7(dev) && port == PORT_A) {
+	if (IS_GEN7(dev_priv) && port == PORT_A) {
 		if (adjusted_mode->flags & DRM_MODE_FLAG_PHSYNC)
 			intel_dp->DP |= DP_SYNC_HS_HIGH;
 		if (adjusted_mode->flags & DRM_MODE_FLAG_PVSYNC)
@@ -1778,7 +1785,7 @@ static void intel_dp_prepare(struct intel_encoder *encoder,
 			intel_dp->DP |= DP_ENHANCED_FRAMING;
 
 		intel_dp->DP |= crtc->pipe << 29;
-	} else if (HAS_PCH_CPT(dev) && port != PORT_A) {
+	} else if (HAS_PCH_CPT(dev_priv) && port != PORT_A) {
 		u32 trans_dp;
 
 		intel_dp->DP |= DP_LINK_TRAIN_OFF_CPT;
@@ -1790,8 +1797,9 @@ static void intel_dp_prepare(struct intel_encoder *encoder,
 			trans_dp &= ~TRANS_DP_ENH_FRAMING;
 		I915_WRITE(TRANS_DP_CTL(crtc->pipe), trans_dp);
 	} else {
-		if (!HAS_PCH_SPLIT(dev) && !IS_VALLEYVIEW(dev) &&
-		    !IS_CHERRYVIEW(dev) && pipe_config->limited_color_range)
+		if (!HAS_PCH_SPLIT(dev_priv) && !IS_VALLEYVIEW(dev_priv) &&
+		    !IS_CHERRYVIEW(dev_priv) &&
+		    pipe_config->limited_color_range)
 			intel_dp->DP |= DP_COLOR_RANGE_16_235;
 
 		if (adjusted_mode->flags & DRM_MODE_FLAG_PHSYNC)
@@ -1803,7 +1811,7 @@ static void intel_dp_prepare(struct intel_encoder *encoder,
 		if (drm_dp_enhanced_frame_cap(intel_dp->dpcd))
 			intel_dp->DP |= DP_ENHANCED_FRAMING;
 
-		if (IS_CHERRYVIEW(dev))
+		if (IS_CHERRYVIEW(dev_priv))
 			intel_dp->DP |= DP_PIPE_SELECT_CHV(crtc->pipe);
 		else if (crtc->pipe == PIPE_B)
 			intel_dp->DP |= DP_PIPEB_SELECT;
@@ -2112,7 +2120,7 @@ static void edp_panel_on(struct intel_dp *intel_dp)
 
 	pp_ctrl_reg = _pp_ctrl_reg(intel_dp);
 	pp = ironlake_get_pp_control(intel_dp);
-	if (IS_GEN5(dev)) {
+	if (IS_GEN5(dev_priv)) {
 		/* ILK workaround: disable reset around power sequence */
 		pp &= ~PANEL_POWER_RESET;
 		I915_WRITE(pp_ctrl_reg, pp);
@@ -2120,7 +2128,7 @@ static void edp_panel_on(struct intel_dp *intel_dp)
 	}
 
 	pp |= PANEL_POWER_ON;
-	if (!IS_GEN5(dev))
+	if (!IS_GEN5(dev_priv))
 		pp |= PANEL_POWER_RESET;
 
 	I915_WRITE(pp_ctrl_reg, pp);
@@ -2129,7 +2137,7 @@ static void edp_panel_on(struct intel_dp *intel_dp)
 	wait_panel_on(intel_dp);
 	intel_dp->last_power_on = jiffies;
 
-	if (IS_GEN5(dev)) {
+	if (IS_GEN5(dev_priv)) {
 		pp |= PANEL_POWER_RESET; /* restore panel reset bit */
 		I915_WRITE(pp_ctrl_reg, pp);
 		POSTING_READ(pp_ctrl_reg);
@@ -2442,9 +2450,9 @@ static bool intel_dp_get_hw_state(struct intel_encoder *encoder,
 	if (!(tmp & DP_PORT_EN))
 		goto out;
 
-	if (IS_GEN7(dev) && port == PORT_A) {
+	if (IS_GEN7(dev_priv) && port == PORT_A) {
 		*pipe = PORT_TO_PIPE_CPT(tmp);
-	} else if (HAS_PCH_CPT(dev) && port != PORT_A) {
+	} else if (HAS_PCH_CPT(dev_priv) && port != PORT_A) {
 		enum pipe p;
 
 		for_each_pipe(dev_priv, p) {
@@ -2459,7 +2467,7 @@ static bool intel_dp_get_hw_state(struct intel_encoder *encoder,
 
 		DRM_DEBUG_KMS("No pipe for dp port 0x%x found\n",
 			      i915_mmio_reg_offset(intel_dp->output_reg));
-	} else if (IS_CHERRYVIEW(dev)) {
+	} else if (IS_CHERRYVIEW(dev_priv)) {
 		*pipe = DP_PORT_TO_PIPE_CHV(tmp);
 	} else {
 		*pipe = PORT_TO_PIPE(tmp);
@@ -2487,7 +2495,7 @@ static void intel_dp_get_config(struct intel_encoder *encoder,
 
 	pipe_config->has_audio = tmp & DP_AUDIO_OUTPUT_ENABLE && port != PORT_A;
 
-	if (HAS_PCH_CPT(dev) && port != PORT_A) {
+	if (HAS_PCH_CPT(dev_priv) && port != PORT_A) {
 		u32 trans_dp = I915_READ(TRANS_DP_CTL(crtc->pipe));
 
 		if (trans_dp & TRANS_DP_HSYNC_ACTIVE_HIGH)
@@ -2513,8 +2521,8 @@ static void intel_dp_get_config(struct intel_encoder *encoder,
 
 	pipe_config->base.adjusted_mode.flags |= flags;
 
-	if (!HAS_PCH_SPLIT(dev) && !IS_VALLEYVIEW(dev) &&
-	    !IS_CHERRYVIEW(dev) && tmp & DP_COLOR_RANGE_16_235)
+	if (!HAS_PCH_SPLIT(dev_priv) && !IS_VALLEYVIEW(dev_priv) &&
+	    !IS_CHERRYVIEW(dev_priv) && tmp & DP_COLOR_RANGE_16_235)
 		pipe_config->limited_color_range = true;
 
 	pipe_config->lane_count =
@@ -2634,7 +2642,7 @@ _intel_dp_set_link_train(struct intel_dp *intel_dp,
 		DRM_DEBUG_KMS("Using DP training pattern TPS%d\n",
 			      dp_train_pat & DP_TRAINING_PATTERN_MASK);
 
-	if (HAS_DDI(dev)) {
+	if (HAS_DDI(dev_priv)) {
 		uint32_t temp = I915_READ(DP_TP_CTL(port));
 
 		if (dp_train_pat & DP_LINK_SCRAMBLING_DISABLE)
@@ -2660,8 +2668,8 @@ _intel_dp_set_link_train(struct intel_dp *intel_dp,
 		}
 		I915_WRITE(DP_TP_CTL(port), temp);
 
-	} else if ((IS_GEN7(dev) && port == PORT_A) ||
-		   (HAS_PCH_CPT(dev) && port != PORT_A)) {
+	} else if ((IS_GEN7(dev_priv) && port == PORT_A) ||
+		   (HAS_PCH_CPT(dev_priv) && port != PORT_A)) {
 		*DP &= ~DP_LINK_TRAIN_MASK_CPT;
 
 		switch (dp_train_pat & DP_TRAINING_PATTERN_MASK) {
@@ -2681,7 +2689,7 @@ _intel_dp_set_link_train(struct intel_dp *intel_dp,
 		}
 
 	} else {
-		if (IS_CHERRYVIEW(dev))
+		if (IS_CHERRYVIEW(dev_priv))
 			*DP &= ~DP_LINK_TRAIN_MASK_CHV;
 		else
 			*DP &= ~DP_LINK_TRAIN_MASK;
@@ -2697,7 +2705,7 @@ _intel_dp_set_link_train(struct intel_dp *intel_dp,
 			*DP |= DP_LINK_TRAIN_PAT_2;
 			break;
 		case DP_TRAINING_PATTERN_3:
-			if (IS_CHERRYVIEW(dev)) {
+			if (IS_CHERRYVIEW(dev_priv)) {
 				*DP |= DP_LINK_TRAIN_PAT_3_CHV;
 			} else {
 				DRM_DEBUG_KMS("TPS3 not supported, using TPS2 instead\n");
@@ -2747,7 +2755,7 @@ static void intel_enable_dp(struct intel_encoder *encoder,
 
 	pps_lock(intel_dp);
 
-	if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev))
+	if (IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv))
 		vlv_init_panel_power_sequencer(intel_dp);
 
 	intel_dp_enable_port(intel_dp, pipe_config);
@@ -2758,10 +2766,10 @@ static void intel_enable_dp(struct intel_encoder *encoder,
 
 	pps_unlock(intel_dp);
 
-	if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) {
+	if (IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv)) {
 		unsigned int lane_mask = 0x0;
 
-		if (IS_CHERRYVIEW(dev))
+		if (IS_CHERRYVIEW(dev_priv))
 			lane_mask = intel_dp_unused_lane_mask(pipe_config->lane_count);
 
 		vlv_wait_port_ready(dev_priv, dp_to_dig_port(intel_dp),
@@ -2981,17 +2989,17 @@ intel_dp_voltage_max(struct intel_dp *intel_dp)
 	struct drm_i915_private *dev_priv = to_i915(dev);
 	enum port port = dp_to_dig_port(intel_dp)->port;
 
-	if (IS_BROXTON(dev))
+	if (IS_BROXTON(dev_priv))
 		return DP_TRAIN_VOLTAGE_SWING_LEVEL_3;
 	else if (INTEL_INFO(dev)->gen >= 9) {
 		if (dev_priv->vbt.edp.low_vswing && port == PORT_A)
 			return DP_TRAIN_VOLTAGE_SWING_LEVEL_3;
 		return DP_TRAIN_VOLTAGE_SWING_LEVEL_2;
-	} else if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev))
+	} else if (IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv))
 		return DP_TRAIN_VOLTAGE_SWING_LEVEL_3;
-	else if (IS_GEN7(dev) && port == PORT_A)
+	else if (IS_GEN7(dev_priv) && port == PORT_A)
 		return DP_TRAIN_VOLTAGE_SWING_LEVEL_2;
-	else if (HAS_PCH_CPT(dev) && port != PORT_A)
+	else if (HAS_PCH_CPT(dev_priv) && port != PORT_A)
 		return DP_TRAIN_VOLTAGE_SWING_LEVEL_3;
 	else
 		return DP_TRAIN_VOLTAGE_SWING_LEVEL_2;
@@ -3000,10 +3008,10 @@ intel_dp_voltage_max(struct intel_dp *intel_dp)
 uint8_t
 intel_dp_pre_emphasis_max(struct intel_dp *intel_dp, uint8_t voltage_swing)
 {
-	struct drm_device *dev = intel_dp_to_dev(intel_dp);
+	struct drm_i915_private *dev_priv = to_i915(intel_dp_to_dev(intel_dp));
 	enum port port = dp_to_dig_port(intel_dp)->port;
 
-	if (INTEL_INFO(dev)->gen >= 9) {
+	if (INTEL_GEN(dev_priv) >= 9) {
 		switch (voltage_swing & DP_TRAIN_VOLTAGE_SWING_MASK) {
 		case DP_TRAIN_VOLTAGE_SWING_LEVEL_0:
 			return DP_TRAIN_PRE_EMPH_LEVEL_3;
@@ -3016,7 +3024,7 @@ intel_dp_pre_emphasis_max(struct intel_dp *intel_dp, uint8_t voltage_swing)
 		default:
 			return DP_TRAIN_PRE_EMPH_LEVEL_0;
 		}
-	} else if (IS_HASWELL(dev) || IS_BROADWELL(dev)) {
+	} else if (IS_HASWELL(dev_priv) || IS_BROADWELL(dev_priv)) {
 		switch (voltage_swing & DP_TRAIN_VOLTAGE_SWING_MASK) {
 		case DP_TRAIN_VOLTAGE_SWING_LEVEL_0:
 			return DP_TRAIN_PRE_EMPH_LEVEL_3;
@@ -3028,7 +3036,7 @@ intel_dp_pre_emphasis_max(struct intel_dp *intel_dp, uint8_t voltage_swing)
 		default:
 			return DP_TRAIN_PRE_EMPH_LEVEL_0;
 		}
-	} else if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) {
+	} else if (IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv)) {
 		switch (voltage_swing & DP_TRAIN_VOLTAGE_SWING_MASK) {
 		case DP_TRAIN_VOLTAGE_SWING_LEVEL_0:
 			return DP_TRAIN_PRE_EMPH_LEVEL_3;
@@ -3040,7 +3048,7 @@ intel_dp_pre_emphasis_max(struct intel_dp *intel_dp, uint8_t voltage_swing)
 		default:
 			return DP_TRAIN_PRE_EMPH_LEVEL_0;
 		}
-	} else if (IS_GEN7(dev) && port == PORT_A) {
+	} else if (IS_GEN7(dev_priv) && port == PORT_A) {
 		switch (voltage_swing & DP_TRAIN_VOLTAGE_SWING_MASK) {
 		case DP_TRAIN_VOLTAGE_SWING_LEVEL_0:
 			return DP_TRAIN_PRE_EMPH_LEVEL_2;
@@ -3341,21 +3349,21 @@ intel_dp_set_signal_levels(struct intel_dp *intel_dp)
 	uint32_t signal_levels, mask = 0;
 	uint8_t train_set = intel_dp->train_set[0];
 
-	if (HAS_DDI(dev)) {
+	if (HAS_DDI(dev_priv)) {
 		signal_levels = ddi_signal_levels(intel_dp);
 
-		if (IS_BROXTON(dev))
+		if (IS_BROXTON(dev_priv))
 			signal_levels = 0;
 		else
 			mask = DDI_BUF_EMP_MASK;
-	} else if (IS_CHERRYVIEW(dev)) {
+	} else if (IS_CHERRYVIEW(dev_priv)) {
 		signal_levels = chv_signal_levels(intel_dp);
-	} else if (IS_VALLEYVIEW(dev)) {
+	} else if (IS_VALLEYVIEW(dev_priv)) {
 		signal_levels = vlv_signal_levels(intel_dp);
-	} else if (IS_GEN7(dev) && port == PORT_A) {
+	} else if (IS_GEN7(dev_priv) && port == PORT_A) {
 		signal_levels = gen7_edp_signal_levels(train_set);
 		mask = EDP_LINK_TRAIN_VOL_EMP_MASK_IVB;
-	} else if (IS_GEN6(dev) && port == PORT_A) {
+	} else if (IS_GEN6(dev_priv) && port == PORT_A) {
 		signal_levels = gen6_edp_signal_levels(train_set);
 		mask = EDP_LINK_TRAIN_VOL_EMP_MASK_SNB;
 	} else {
@@ -3400,7 +3408,7 @@ void intel_dp_set_idle_link_train(struct intel_dp *intel_dp)
 	enum port port = intel_dig_port->port;
 	uint32_t val;
 
-	if (!HAS_DDI(dev))
+	if (!HAS_DDI(dev_priv))
 		return;
 
 	val = I915_READ(DP_TP_CTL(port));
@@ -3435,7 +3443,7 @@ intel_dp_link_down(struct intel_dp *intel_dp)
 	struct drm_i915_private *dev_priv = to_i915(dev);
 	uint32_t DP = intel_dp->DP;
 
-	if (WARN_ON(HAS_DDI(dev)))
+	if (WARN_ON(HAS_DDI(dev_priv)))
 		return;
 
 	if (WARN_ON((I915_READ(intel_dp->output_reg) & DP_PORT_EN) == 0))
@@ -3443,12 +3451,12 @@ intel_dp_link_down(struct intel_dp *intel_dp)
 
 	DRM_DEBUG_KMS("\n");
 
-	if ((IS_GEN7(dev) && port == PORT_A) ||
-	    (HAS_PCH_CPT(dev) && port != PORT_A)) {
+	if ((IS_GEN7(dev_priv) && port == PORT_A) ||
+	    (HAS_PCH_CPT(dev_priv) && port != PORT_A)) {
 		DP &= ~DP_LINK_TRAIN_MASK_CPT;
 		DP |= DP_LINK_TRAIN_PAT_IDLE_CPT;
 	} else {
-		if (IS_CHERRYVIEW(dev))
+		if (IS_CHERRYVIEW(dev_priv))
 			DP &= ~DP_LINK_TRAIN_MASK_CHV;
 		else
 			DP &= ~DP_LINK_TRAIN_MASK;
@@ -3466,7 +3474,7 @@ intel_dp_link_down(struct intel_dp *intel_dp)
 	 * to transcoder A after disabling it to allow the
 	 * matching HDMI port to be enabled on transcoder A.
 	 */
-	if (HAS_PCH_IBX(dev) && crtc->pipe == PIPE_B && port != PORT_A) {
+	if (HAS_PCH_IBX(dev_priv) && crtc->pipe == PIPE_B && port != PORT_A) {
 		/*
 		 * We get CPU/PCH FIFO underruns on the other pipe when
 		 * doing the workaround. Sweep them under the rug.
@@ -3549,8 +3557,8 @@ intel_edp_init_dpcd(struct intel_dp *intel_dp)
 	/* Read the eDP Display control capabilities registers */
 	if ((intel_dp->dpcd[DP_EDP_CONFIGURATION_CAP] & DP_DPCD_DISPLAY_CONTROL_CAPABLE) &&
 	    drm_dp_dpcd_read(&intel_dp->aux, DP_EDP_DPCD_REV,
-			     intel_dp->edp_dpcd, sizeof(intel_dp->edp_dpcd) ==
-			     sizeof(intel_dp->edp_dpcd)))
+			     intel_dp->edp_dpcd, sizeof(intel_dp->edp_dpcd)) ==
+			     sizeof(intel_dp->edp_dpcd))
 		DRM_DEBUG_KMS("EDP DPCD : %*ph\n", (int) sizeof(intel_dp->edp_dpcd),
 			      intel_dp->edp_dpcd);
 
@@ -3986,6 +3994,31 @@ go_again:
 }
 
 static void
+intel_dp_retrain_link(struct intel_dp *intel_dp)
+{
+	struct intel_encoder *encoder = &dp_to_dig_port(intel_dp)->base;
+	struct drm_i915_private *dev_priv = to_i915(encoder->base.dev);
+	struct intel_crtc *crtc = to_intel_crtc(encoder->base.crtc);
+
+	/* Suppress underruns caused by re-training */
+	intel_set_cpu_fifo_underrun_reporting(dev_priv, crtc->pipe, false);
+	if (crtc->config->has_pch_encoder)
+		intel_set_pch_fifo_underrun_reporting(dev_priv,
+						      intel_crtc_pch_transcoder(crtc), false);
+
+	intel_dp_start_link_train(intel_dp);
+	intel_dp_stop_link_train(intel_dp);
+
+	/* Keep underrun reporting disabled until things are stable */
+	intel_wait_for_vblank(&dev_priv->drm, crtc->pipe);
+
+	intel_set_cpu_fifo_underrun_reporting(dev_priv, crtc->pipe, true);
+	if (crtc->config->has_pch_encoder)
+		intel_set_pch_fifo_underrun_reporting(dev_priv,
+						      intel_crtc_pch_transcoder(crtc), true);
+}
+
+static void
 intel_dp_check_link_status(struct intel_dp *intel_dp)
 {
 	struct intel_encoder *intel_encoder = &dp_to_dig_port(intel_dp)->base;
@@ -4005,13 +4038,18 @@ intel_dp_check_link_status(struct intel_dp *intel_dp)
 	if (!to_intel_crtc(intel_encoder->base.crtc)->active)
 		return;
 
+	/* FIXME: we need to synchronize this sort of stuff with hardware
+	 * readout */
+	if (WARN_ON_ONCE(!intel_dp->lane_count))
+		return;
+
 	/* if link training is requested we should perform it always */
 	if ((intel_dp->compliance_test_type == DP_TEST_LINK_TRAINING) ||
 	    (!drm_dp_channel_eq_ok(link_status, intel_dp->lane_count))) {
 		DRM_DEBUG_KMS("%s: channel EQ not ok, retraining\n",
 			      intel_encoder->base.name);
-		intel_dp_start_link_train(intel_dp);
-		intel_dp_stop_link_train(intel_dp);
+
+		intel_dp_retrain_link(intel_dp);
 	}
 }
 
@@ -4334,7 +4372,7 @@ intel_dp_unset_edid(struct intel_dp *intel_dp)
 	intel_dp->has_audio = false;
 }
 
-static void
+static enum drm_connector_status
 intel_dp_long_pulse(struct intel_connector *intel_connector)
 {
 	struct drm_connector *connector = &intel_connector->base;
@@ -4358,7 +4396,7 @@ intel_dp_long_pulse(struct intel_connector *intel_connector)
 	else
 		status = connector_status_disconnected;
 
-	if (status != connector_status_connected) {
+	if (status == connector_status_disconnected) {
 		intel_dp->compliance_test_active = 0;
 		intel_dp->compliance_test_type = 0;
 		intel_dp->compliance_test_data = 0;
@@ -4420,8 +4458,8 @@ intel_dp_long_pulse(struct intel_connector *intel_connector)
 	intel_dp->aux.i2c_defer_count = 0;
 
 	intel_dp_set_edid(intel_dp);
-
-	status = connector_status_connected;
+	if (is_edp(intel_dp) || intel_connector->detect_edid)
+		status = connector_status_connected;
 	intel_dp->detect_done = true;
 
 	/* Try to read the source of the interrupt */
@@ -4440,12 +4478,11 @@ intel_dp_long_pulse(struct intel_connector *intel_connector)
 	}
 
 out:
-	if ((status != connector_status_connected) &&
-	    (intel_dp->is_mst == false))
+	if (status != connector_status_connected && !intel_dp->is_mst)
 		intel_dp_unset_edid(intel_dp);
 
 	intel_display_power_put(to_i915(dev), power_domain);
-	return;
+	return status;
 }
 
 static enum drm_connector_status
@@ -4454,7 +4491,7 @@ intel_dp_detect(struct drm_connector *connector, bool force)
 	struct intel_dp *intel_dp = intel_attached_dp(connector);
 	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
 	struct intel_encoder *intel_encoder = &intel_dig_port->base;
-	struct intel_connector *intel_connector = to_intel_connector(connector);
+	enum drm_connector_status status = connector->status;
 
 	DRM_DEBUG_KMS("[CONNECTOR:%d:%s]\n",
 		      connector->base.id, connector->name);
@@ -4469,14 +4506,11 @@ intel_dp_detect(struct drm_connector *connector, bool force)
 
 	/* If full detect is not performed yet, do a full detect */
 	if (!intel_dp->detect_done)
-		intel_dp_long_pulse(intel_dp->attached_connector);
+		status = intel_dp_long_pulse(intel_dp->attached_connector);
 
 	intel_dp->detect_done = false;
 
-	if (is_edp(intel_dp) || intel_connector->detect_edid)
-		return connector_status_connected;
-	else
-		return connector_status_disconnected;
+	return status;
 }
 
 static void
@@ -4757,11 +4791,16 @@ static void intel_edp_panel_vdd_sanitize(struct intel_dp *intel_dp)
 void intel_dp_encoder_reset(struct drm_encoder *encoder)
 {
 	struct drm_i915_private *dev_priv = to_i915(encoder->dev);
-	struct intel_dp *intel_dp = enc_to_intel_dp(encoder);
+	struct intel_digital_port *intel_dig_port = enc_to_dig_port(encoder);
+	struct intel_lspcon *lspcon = &intel_dig_port->lspcon;
+	struct intel_dp *intel_dp = &intel_dig_port->dp;
 
 	if (!HAS_DDI(dev_priv))
 		intel_dp->DP = I915_READ(intel_dp->output_reg);
 
+	if (IS_GEN9(dev_priv) && lspcon->active)
+		lspcon_resume(lspcon);
+
 	if (to_intel_encoder(encoder)->type != INTEL_OUTPUT_EDP)
 		return;
 
@@ -4828,36 +4867,34 @@ intel_dp_hpd_pulse(struct intel_digital_port *intel_dig_port, bool long_hpd)
 		      port_name(intel_dig_port->port),
 		      long_hpd ? "long" : "short");
 
+	if (long_hpd) {
+		intel_dp->detect_done = false;
+		return IRQ_NONE;
+	}
+
 	power_domain = intel_display_port_aux_power_domain(intel_encoder);
 	intel_display_power_get(dev_priv, power_domain);
 
-	if (long_hpd) {
-		intel_dp_long_pulse(intel_dp->attached_connector);
-		if (intel_dp->is_mst)
-			ret = IRQ_HANDLED;
-		goto put_power;
-
-	} else {
-		if (intel_dp->is_mst) {
-			if (intel_dp_check_mst_status(intel_dp) == -EINVAL) {
-				/*
-				 * If we were in MST mode, and device is not
-				 * there, get out of MST mode
-				 */
-				DRM_DEBUG_KMS("MST device may have disappeared %d vs %d\n",
-					      intel_dp->is_mst, intel_dp->mst_mgr.mst_state);
-				intel_dp->is_mst = false;
-				drm_dp_mst_topology_mgr_set_mst(&intel_dp->mst_mgr,
-								intel_dp->is_mst);
-				goto put_power;
-			}
+	if (intel_dp->is_mst) {
+		if (intel_dp_check_mst_status(intel_dp) == -EINVAL) {
+			/*
+			 * If we were in MST mode, and device is not
+			 * there, get out of MST mode
+			 */
+			DRM_DEBUG_KMS("MST device may have disappeared %d vs %d\n",
+				      intel_dp->is_mst, intel_dp->mst_mgr.mst_state);
+			intel_dp->is_mst = false;
+			drm_dp_mst_topology_mgr_set_mst(&intel_dp->mst_mgr,
+							intel_dp->is_mst);
+			intel_dp->detect_done = false;
+			goto put_power;
 		}
+	}
 
-		if (!intel_dp->is_mst) {
-			if (!intel_dp_short_pulse(intel_dp)) {
-				intel_dp_long_pulse(intel_dp->attached_connector);
-				goto put_power;
-			}
+	if (!intel_dp->is_mst) {
+		if (!intel_dp_short_pulse(intel_dp)) {
+			intel_dp->detect_done = false;
+			goto put_power;
 		}
 	}
 
@@ -5077,7 +5114,7 @@ intel_dp_init_panel_power_sequencer_registers(struct drm_device *dev,
 		 (seq->t10 << PANEL_POWER_DOWN_DELAY_SHIFT);
 	/* Compute the divisor for the pp clock, simply match the Bspec
 	 * formula. */
-	if (IS_BROXTON(dev)) {
+	if (IS_BROXTON(dev_priv)) {
 		pp_div = I915_READ(regs.pp_ctrl);
 		pp_div &= ~BXT_POWER_CYCLE_DELAY_MASK;
 		pp_div |= (DIV_ROUND_UP((seq->t11_t12 + 1), 1000)
@@ -5090,9 +5127,9 @@ intel_dp_init_panel_power_sequencer_registers(struct drm_device *dev,
 
 	/* Haswell doesn't have any port selection bits for the panel
 	 * power sequencer any more. */
-	if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) {
+	if (IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv)) {
 		port_sel = PANEL_PORT_SELECT_VLV(port);
-	} else if (HAS_PCH_IBX(dev) || HAS_PCH_CPT(dev)) {
+	} else if (HAS_PCH_IBX(dev_priv) || HAS_PCH_CPT(dev_priv)) {
 		if (port == PORT_A)
 			port_sel = PANEL_PORT_SELECT_DPA;
 		else
@@ -5103,7 +5140,7 @@ intel_dp_init_panel_power_sequencer_registers(struct drm_device *dev,
 
 	I915_WRITE(regs.pp_on, pp_on);
 	I915_WRITE(regs.pp_off, pp_off);
-	if (IS_BROXTON(dev))
+	if (IS_BROXTON(dev_priv))
 		I915_WRITE(regs.pp_ctrl, pp_div);
 	else
 		I915_WRITE(regs.pp_div, pp_div);
@@ -5111,7 +5148,7 @@ intel_dp_init_panel_power_sequencer_registers(struct drm_device *dev,
 	DRM_DEBUG_KMS("panel power sequencer register settings: PP_ON %#x, PP_OFF %#x, PP_DIV %#x\n",
 		      I915_READ(regs.pp_on),
 		      I915_READ(regs.pp_off),
-		      IS_BROXTON(dev) ?
+		      IS_BROXTON(dev_priv) ?
 		      (I915_READ(regs.pp_ctrl) & BXT_POWER_CYCLE_DELAY_MASK) :
 		      I915_READ(regs.pp_div));
 }
@@ -5119,7 +5156,9 @@ intel_dp_init_panel_power_sequencer_registers(struct drm_device *dev,
 static void intel_dp_pps_init(struct drm_device *dev,
 			      struct intel_dp *intel_dp)
 {
-	if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) {
+	struct drm_i915_private *dev_priv = to_i915(dev);
+
+	if (IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv)) {
 		vlv_initial_power_sequencer_setup(intel_dp);
 	} else {
 		intel_dp_init_panel_power_sequencer(dev, intel_dp);
@@ -5589,7 +5628,7 @@ static bool intel_edp_init_connector(struct intel_dp *intel_dp,
 	}
 	mutex_unlock(&dev->mode_config.mutex);
 
-	if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) {
+	if (IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv)) {
 		intel_dp->edp_notifier.notifier_call = edp_notify_handler;
 		register_reboot_notifier(&intel_dp->edp_notifier);
 
@@ -5598,7 +5637,7 @@ static bool intel_edp_init_connector(struct intel_dp *intel_dp,
 		 * If the current pipe isn't valid, try the PPS pipe, and if that
 		 * fails just assume pipe A.
 		 */
-		if (IS_CHERRYVIEW(dev))
+		if (IS_CHERRYVIEW(dev_priv))
 			pipe = DP_PORT_TO_PIPE_CHV(intel_dp->DP);
 		else
 			pipe = PORT_TO_PIPE(intel_dp->DP);
@@ -5654,9 +5693,9 @@ intel_dp_init_connector(struct intel_digital_port *intel_dig_port,
 	/* intel_dp vfuncs */
 	if (INTEL_INFO(dev)->gen >= 9)
 		intel_dp->get_aux_clock_divider = skl_get_aux_clock_divider;
-	else if (IS_HASWELL(dev) || IS_BROADWELL(dev))
+	else if (IS_HASWELL(dev_priv) || IS_BROADWELL(dev_priv))
 		intel_dp->get_aux_clock_divider = hsw_get_aux_clock_divider;
-	else if (HAS_PCH_SPLIT(dev))
+	else if (HAS_PCH_SPLIT(dev_priv))
 		intel_dp->get_aux_clock_divider = ilk_get_aux_clock_divider;
 	else
 		intel_dp->get_aux_clock_divider = g4x_get_aux_clock_divider;
@@ -5666,7 +5705,7 @@ intel_dp_init_connector(struct intel_digital_port *intel_dig_port,
 	else
 		intel_dp->get_aux_send_ctl = g4x_get_aux_send_ctl;
 
-	if (HAS_DDI(dev))
+	if (HAS_DDI(dev_priv))
 		intel_dp->prepare_link_retrain = intel_ddi_prepare_link_retrain;
 
 	/* Preserve the current hw state. */
@@ -5687,7 +5726,7 @@ intel_dp_init_connector(struct intel_digital_port *intel_dig_port,
 		intel_encoder->type = INTEL_OUTPUT_EDP;
 
 	/* eDP only on port B and/or C on vlv/chv */
-	if (WARN_ON((IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) &&
+	if (WARN_ON((IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv)) &&
 		    is_edp(intel_dp) && port != PORT_B && port != PORT_C))
 		return false;
 
@@ -5708,7 +5747,7 @@ intel_dp_init_connector(struct intel_digital_port *intel_dig_port,
 
 	intel_connector_attach_encoder(intel_connector, intel_encoder);
 
-	if (HAS_DDI(dev))
+	if (HAS_DDI(dev_priv))
 		intel_connector->get_hw_state = intel_ddi_connector_get_hw_state;
 	else
 		intel_connector->get_hw_state = intel_connector_get_hw_state;
@@ -5720,7 +5759,7 @@ intel_dp_init_connector(struct intel_digital_port *intel_dig_port,
 		break;
 	case PORT_B:
 		intel_encoder->hpd_pin = HPD_PORT_B;
-		if (IS_BXT_REVID(dev, 0, BXT_REVID_A1))
+		if (IS_BXT_REVID(dev_priv, 0, BXT_REVID_A1))
 			intel_encoder->hpd_pin = HPD_PORT_A;
 		break;
 	case PORT_C:
@@ -5754,7 +5793,7 @@ intel_dp_init_connector(struct intel_digital_port *intel_dig_port,
 	 * 0xd.  Failure to do so will result in spurious interrupts being
 	 * generated on the port when a cable is not attached.
 	 */
-	if (IS_G4X(dev) && !IS_GM45(dev)) {
+	if (IS_G4X(dev_priv) && !IS_GM45(dev_priv)) {
 		u32 temp = I915_READ(PEG_BAND_GAP_DATA);
 		I915_WRITE(PEG_BAND_GAP_DATA, (temp & ~0xf) | 0xd);
 	}
@@ -5797,13 +5836,13 @@ bool intel_dp_init(struct drm_device *dev,
 	intel_encoder->get_hw_state = intel_dp_get_hw_state;
 	intel_encoder->get_config = intel_dp_get_config;
 	intel_encoder->suspend = intel_dp_encoder_suspend;
-	if (IS_CHERRYVIEW(dev)) {
+	if (IS_CHERRYVIEW(dev_priv)) {
 		intel_encoder->pre_pll_enable = chv_dp_pre_pll_enable;
 		intel_encoder->pre_enable = chv_pre_enable_dp;
 		intel_encoder->enable = vlv_enable_dp;
 		intel_encoder->post_disable = chv_post_disable_dp;
 		intel_encoder->post_pll_disable = chv_dp_post_pll_disable;
-	} else if (IS_VALLEYVIEW(dev)) {
+	} else if (IS_VALLEYVIEW(dev_priv)) {
 		intel_encoder->pre_pll_enable = vlv_dp_pre_pll_enable;
 		intel_encoder->pre_enable = vlv_pre_enable_dp;
 		intel_encoder->enable = vlv_enable_dp;
@@ -5820,7 +5859,7 @@ bool intel_dp_init(struct drm_device *dev,
 	intel_dig_port->max_lanes = 4;
 
 	intel_encoder->type = INTEL_OUTPUT_DP;
-	if (IS_CHERRYVIEW(dev)) {
+	if (IS_CHERRYVIEW(dev_priv)) {
 		if (port == PORT_D)
 			intel_encoder->crtc_mask = 1 << 2;
 		else
@@ -5829,6 +5868,7 @@ bool intel_dp_init(struct drm_device *dev,
 		intel_encoder->crtc_mask = (1 << 0) | (1 << 1) | (1 << 2);
 	}
 	intel_encoder->cloneable = 0;
+	intel_encoder->port = port;
 
 	intel_dig_port->hpd_pulse = intel_dp_hpd_pulse;
 	dev_priv->hotplug.irq_port[port] = intel_dig_port;
diff --git a/drivers/gpu/drm/i915/intel_dp_link_training.c b/drivers/gpu/drm/i915/intel_dp_link_training.c
index c438b02184cb..0048b520baf7 100644
--- a/drivers/gpu/drm/i915/intel_dp_link_training.c
+++ b/drivers/gpu/drm/i915/intel_dp_link_training.c
@@ -225,9 +225,6 @@ static u32 intel_dp_training_pattern(struct intel_dp *intel_dp)
 	 * Intel platforms that support HBR2 also support TPS3. TPS3 support is
 	 * also mandatory for downstream devices that support HBR2. However, not
 	 * all sinks follow the spec.
-	 *
-	 * Due to WaDisableHBR2 SKL < B0 is the only exception where TPS3 is
-	 * supported in source but still not enabled.
 	 */
 	source_tps3 = intel_dp_source_supports_hbr2(intel_dp);
 	sink_tps3 = drm_dp_tps3_supported(intel_dp->dpcd);
diff --git a/drivers/gpu/drm/i915/intel_dp_mst.c b/drivers/gpu/drm/i915/intel_dp_mst.c
index 54a9d7610d8f..3ffbd69e4551 100644
--- a/drivers/gpu/drm/i915/intel_dp_mst.c
+++ b/drivers/gpu/drm/i915/intel_dp_mst.c
@@ -523,6 +523,7 @@ intel_dp_create_fake_mst_encoder(struct intel_digital_port *intel_dig_port, enum
 			 DRM_MODE_ENCODER_DPMST, "DP-MST %c", pipe_name(pipe));
 
 	intel_encoder->type = INTEL_OUTPUT_DP_MST;
+	intel_encoder->port = intel_dig_port->port;
 	intel_encoder->crtc_mask = 0x7;
 	intel_encoder->cloneable = 0;
 
diff --git a/drivers/gpu/drm/i915/intel_dpll_mgr.c b/drivers/gpu/drm/i915/intel_dpll_mgr.c
index c26d18a574b6..605d0b509f24 100644
--- a/drivers/gpu/drm/i915/intel_dpll_mgr.c
+++ b/drivers/gpu/drm/i915/intel_dpll_mgr.c
@@ -1694,21 +1694,32 @@ bool bxt_ddi_dp_set_dpll_hw_state(int clock,
 	return bxt_ddi_set_dpll_hw_state(clock, &clk_div, dpll_hw_state);
 }
 
+static bool
+bxt_ddi_hdmi_set_dpll_hw_state(struct intel_crtc *intel_crtc,
+			       struct intel_crtc_state *crtc_state, int clock,
+			       struct intel_dpll_hw_state *dpll_hw_state)
+{
+	struct bxt_clk_div clk_div = { };
+
+	bxt_ddi_hdmi_pll_dividers(intel_crtc, crtc_state, clock, &clk_div);
+
+	return bxt_ddi_set_dpll_hw_state(clock, &clk_div, dpll_hw_state);
+}
+
 static struct intel_shared_dpll *
 bxt_get_dpll(struct intel_crtc *crtc,
 		struct intel_crtc_state *crtc_state,
 		struct intel_encoder *encoder)
 {
-	struct bxt_clk_div clk_div = {0};
-	struct intel_dpll_hw_state dpll_hw_state = {0};
+	struct intel_dpll_hw_state dpll_hw_state = { };
 	struct drm_i915_private *dev_priv = to_i915(crtc->base.dev);
 	struct intel_digital_port *intel_dig_port;
 	struct intel_shared_dpll *pll;
 	int i, clock = crtc_state->port_clock;
 
-	if (encoder->type == INTEL_OUTPUT_HDMI
-	    && !bxt_ddi_hdmi_pll_dividers(crtc, crtc_state,
-					  clock, &clk_div))
+	if (encoder->type == INTEL_OUTPUT_HDMI &&
+	    !bxt_ddi_hdmi_set_dpll_hw_state(crtc, crtc_state, clock,
+					    &dpll_hw_state))
 		return NULL;
 
 	if ((encoder->type == INTEL_OUTPUT_DP ||
@@ -1840,13 +1851,13 @@ void intel_shared_dpll_init(struct drm_device *dev)
 	const struct dpll_info *dpll_info;
 	int i;
 
-	if (IS_SKYLAKE(dev) || IS_KABYLAKE(dev))
+	if (IS_SKYLAKE(dev_priv) || IS_KABYLAKE(dev_priv))
 		dpll_mgr = &skl_pll_mgr;
-	else if (IS_BROXTON(dev))
+	else if (IS_BROXTON(dev_priv))
 		dpll_mgr = &bxt_pll_mgr;
-	else if (HAS_DDI(dev))
+	else if (HAS_DDI(dev_priv))
 		dpll_mgr = &hsw_pll_mgr;
-	else if (HAS_PCH_IBX(dev) || HAS_PCH_CPT(dev))
+	else if (HAS_PCH_IBX(dev_priv) || HAS_PCH_CPT(dev_priv))
 		dpll_mgr = &pch_pll_mgr;
 
 	if (!dpll_mgr) {
@@ -1872,7 +1883,7 @@ void intel_shared_dpll_init(struct drm_device *dev)
 	BUG_ON(dev_priv->num_shared_dpll > I915_NUM_PLLS);
 
 	/* FIXME: Move this to a more suitable place */
-	if (HAS_DDI(dev))
+	if (HAS_DDI(dev_priv))
 		intel_ddi_pll_init(dev);
 }
 
diff --git a/drivers/gpu/drm/i915/intel_drv.h b/drivers/gpu/drm/i915/intel_drv.h
index 5145ff264c8e..4e90b0760f3f 100644
--- a/drivers/gpu/drm/i915/intel_drv.h
+++ b/drivers/gpu/drm/i915/intel_drv.h
@@ -206,6 +206,7 @@ struct intel_encoder {
 	struct drm_encoder base;
 
 	enum intel_output_type type;
+	enum port port;
 	unsigned int cloneable;
 	void (*hot_plug)(struct intel_encoder *);
 	bool (*compute_config)(struct intel_encoder *,
@@ -247,6 +248,8 @@ struct intel_encoder {
 	void (*suspend)(struct intel_encoder *);
 	int crtc_mask;
 	enum hpd_pin hpd_pin;
+	/* for communication with audio component; protected by av_mutex */
+	const struct drm_connector *audio_connector;
 };
 
 struct intel_panel {
@@ -263,6 +266,7 @@ struct intel_panel {
 		bool enabled;
 		bool combination_mode;	/* gen 2/4 only */
 		bool active_low_pwm;
+		bool alternate_pwm_increment;	/* lpt+ */
 
 		/* PWM chip */
 		bool util_pin_active_low;	/* bxt+ */
@@ -464,9 +468,13 @@ struct intel_pipe_wm {
 	bool sprites_scaled;
 };
 
-struct skl_pipe_wm {
+struct skl_plane_wm {
 	struct skl_wm_level wm[8];
 	struct skl_wm_level trans_wm;
+};
+
+struct skl_pipe_wm {
+	struct skl_plane_wm planes[I915_MAX_PLANES];
 	uint32_t linetime;
 };
 
@@ -492,6 +500,7 @@ struct intel_crtc_wm_state {
 		struct {
 			/* gen9+ only needs 1-step wm programming */
 			struct skl_pipe_wm optimal;
+			struct skl_ddb_entry ddb;
 
 			/* cached plane data rate */
 			unsigned plane_data_rate[I915_MAX_PLANES];
@@ -729,6 +738,9 @@ struct intel_crtc {
 		bool cxsr_allowed;
 	} wm;
 
+	/* gen9+: ddb allocation currently being used */
+	struct skl_ddb_entry hw_ddb;
+
 	int scanline_offset;
 
 	struct {
@@ -795,22 +807,22 @@ struct intel_plane {
 };
 
 struct intel_watermark_params {
-	unsigned long fifo_size;
-	unsigned long max_wm;
-	unsigned long default_wm;
-	unsigned long guard_size;
-	unsigned long cacheline_size;
+	u16 fifo_size;
+	u16 max_wm;
+	u8 default_wm;
+	u8 guard_size;
+	u8 cacheline_size;
 };
 
 struct cxsr_latency {
-	int is_desktop;
-	int is_ddr3;
-	unsigned long fsb_freq;
-	unsigned long mem_freq;
-	unsigned long display_sr;
-	unsigned long display_hpll_disable;
-	unsigned long cursor_sr;
-	unsigned long cursor_hpll_disable;
+	bool is_desktop : 1;
+	bool is_ddr3 : 1;
+	u16 fsb_freq;
+	u16 mem_freq;
+	u16 display_sr;
+	u16 display_hpll_disable;
+	u16 cursor_sr;
+	u16 cursor_hpll_disable;
 };
 
 #define to_intel_atomic_state(x) container_of(x, struct intel_atomic_state, base)
@@ -949,17 +961,22 @@ struct intel_dp {
 	bool compliance_test_active;
 };
 
+struct intel_lspcon {
+	bool active;
+	enum drm_lspcon_mode mode;
+	struct drm_dp_aux *aux;
+};
+
 struct intel_digital_port {
 	struct intel_encoder base;
 	enum port port;
 	u32 saved_port_bits;
 	struct intel_dp dp;
 	struct intel_hdmi hdmi;
+	struct intel_lspcon lspcon;
 	enum irqreturn (*hpd_pulse)(struct intel_digital_port *, bool);
 	bool release_cl2_override;
 	uint8_t max_lanes;
-	/* for communication with audio component; protected by av_mutex */
-	const struct drm_connector *audio_connector;
 };
 
 struct intel_dp_mst_encoder {
@@ -1181,6 +1198,7 @@ void i915_audio_component_init(struct drm_i915_private *dev_priv);
 void i915_audio_component_cleanup(struct drm_i915_private *dev_priv);
 
 /* intel_display.c */
+enum transcoder intel_crtc_pch_transcoder(struct intel_crtc *crtc);
 void skl_set_preferred_cdclk_vco(struct drm_i915_private *dev_priv, int vco);
 void intel_update_rawclk(struct drm_i915_private *dev_priv);
 int vlv_get_cck_clock(struct drm_i915_private *dev_priv,
@@ -1477,6 +1495,10 @@ static inline void intel_fbdev_set_suspend(struct drm_device *dev, int state, bo
 {
 }
 
+static inline void intel_fbdev_output_poll_changed(struct drm_device *dev)
+{
+}
+
 static inline void intel_fbdev_restore_mode(struct drm_device *dev)
 {
 }
@@ -1503,6 +1525,7 @@ void intel_fbc_invalidate(struct drm_i915_private *dev_priv,
 void intel_fbc_flush(struct drm_i915_private *dev_priv,
 		     unsigned int frontbuffer_bits, enum fb_op_origin origin);
 void intel_fbc_cleanup_cfb(struct drm_i915_private *dev_priv);
+void intel_fbc_handle_fifo_underrun_irq(struct drm_i915_private *dev_priv);
 
 /* intel_hdmi.c */
 void intel_hdmi_init(struct drm_device *dev, i915_reg_t hdmi_reg, enum port port);
@@ -1706,7 +1729,7 @@ bool chv_phy_powergate_ch(struct drm_i915_private *dev_priv, enum dpio_phy phy,
 /* intel_pm.c */
 void intel_init_clock_gating(struct drm_device *dev);
 void intel_suspend_hw(struct drm_device *dev);
-int ilk_wm_max_level(const struct drm_device *dev);
+int ilk_wm_max_level(const struct drm_i915_private *dev_priv);
 void intel_update_watermarks(struct drm_crtc *crtc);
 void intel_init_pm(struct drm_device *dev);
 void intel_init_clock_gating_hooks(struct drm_i915_private *dev_priv);
@@ -1732,20 +1755,24 @@ void ilk_wm_get_hw_state(struct drm_device *dev);
 void skl_wm_get_hw_state(struct drm_device *dev);
 void skl_ddb_get_hw_state(struct drm_i915_private *dev_priv,
 			  struct skl_ddb_allocation *ddb /* out */);
-bool skl_can_enable_sagv(struct drm_atomic_state *state);
-int skl_enable_sagv(struct drm_i915_private *dev_priv);
-int skl_disable_sagv(struct drm_i915_private *dev_priv);
+void skl_pipe_wm_get_hw_state(struct drm_crtc *crtc,
+			      struct skl_pipe_wm *out);
+bool intel_can_enable_sagv(struct drm_atomic_state *state);
+int intel_enable_sagv(struct drm_i915_private *dev_priv);
+int intel_disable_sagv(struct drm_i915_private *dev_priv);
+bool skl_wm_level_equals(const struct skl_wm_level *l1,
+			 const struct skl_wm_level *l2);
 bool skl_ddb_allocation_equals(const struct skl_ddb_allocation *old,
 			       const struct skl_ddb_allocation *new,
 			       enum pipe pipe);
 bool skl_ddb_allocation_overlaps(struct drm_atomic_state *state,
-				 const struct skl_ddb_allocation *old,
-				 const struct skl_ddb_allocation *new,
-				 enum pipe pipe);
+				 struct intel_crtc *intel_crtc);
 void skl_write_cursor_wm(struct intel_crtc *intel_crtc,
-			 const struct skl_wm_values *wm);
+			 const struct skl_plane_wm *wm,
+			 const struct skl_ddb_allocation *ddb);
 void skl_write_plane_wm(struct intel_crtc *intel_crtc,
-			const struct skl_wm_values *wm,
+			const struct skl_plane_wm *wm,
+			const struct skl_ddb_allocation *ddb,
 			int plane);
 uint32_t ilk_pipe_pixel_rate(const struct intel_crtc_state *pipe_config);
 bool ilk_disable_lp_wm(struct drm_device *dev);
@@ -1825,4 +1852,7 @@ int intel_color_check(struct drm_crtc *crtc, struct drm_crtc_state *state);
 void intel_color_set_csc(struct drm_crtc_state *crtc_state);
 void intel_color_load_luts(struct drm_crtc_state *crtc_state);
 
+/* intel_lspcon.c */
+bool lspcon_init(struct intel_digital_port *intel_dig_port);
+void lspcon_resume(struct intel_lspcon *lspcon);
 #endif /* __INTEL_DRV_H__ */
diff --git a/drivers/gpu/drm/i915/intel_dsi.c b/drivers/gpu/drm/i915/intel_dsi.c
index b2e3d3a334f7..4e0d025490a3 100644
--- a/drivers/gpu/drm/i915/intel_dsi.c
+++ b/drivers/gpu/drm/i915/intel_dsi.c
@@ -437,11 +437,11 @@ static void vlv_dsi_device_ready(struct intel_encoder *encoder)
 
 static void intel_dsi_device_ready(struct intel_encoder *encoder)
 {
-	struct drm_device *dev = encoder->base.dev;
+	struct drm_i915_private *dev_priv = to_i915(encoder->base.dev);
 
-	if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev))
+	if (IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv))
 		vlv_dsi_device_ready(encoder);
-	else if (IS_BROXTON(dev))
+	else if (IS_BROXTON(dev_priv))
 		bxt_dsi_device_ready(encoder);
 }
 
@@ -464,7 +464,7 @@ static void intel_dsi_port_enable(struct intel_encoder *encoder)
 	}
 
 	for_each_dsi_port(port, intel_dsi->ports) {
-		i915_reg_t port_ctrl = IS_BROXTON(dev) ?
+		i915_reg_t port_ctrl = IS_BROXTON(dev_priv) ?
 			BXT_MIPI_PORT_CTRL(port) : MIPI_PORT_CTRL(port);
 		u32 temp;
 
@@ -494,7 +494,7 @@ static void intel_dsi_port_disable(struct intel_encoder *encoder)
 	enum port port;
 
 	for_each_dsi_port(port, intel_dsi->ports) {
-		i915_reg_t port_ctrl = IS_BROXTON(dev) ?
+		i915_reg_t port_ctrl = IS_BROXTON(dev_priv) ?
 			BXT_MIPI_PORT_CTRL(port) : MIPI_PORT_CTRL(port);
 		u32 temp;
 
@@ -656,7 +656,6 @@ static void intel_dsi_disable(struct intel_encoder *encoder)
 
 static void intel_dsi_clear_device_ready(struct intel_encoder *encoder)
 {
-	struct drm_device *dev = encoder->base.dev;
 	struct drm_i915_private *dev_priv = to_i915(encoder->base.dev);
 	struct intel_dsi *intel_dsi = enc_to_intel_dsi(&encoder->base);
 	enum port port;
@@ -664,7 +663,7 @@ static void intel_dsi_clear_device_ready(struct intel_encoder *encoder)
 	DRM_DEBUG_KMS("\n");
 	for_each_dsi_port(port, intel_dsi->ports) {
 		/* Common bit for both MIPI Port A & MIPI Port C on VLV/CHV */
-		i915_reg_t port_ctrl = IS_BROXTON(dev) ?
+		i915_reg_t port_ctrl = IS_BROXTON(dev_priv) ?
 			BXT_MIPI_PORT_CTRL(port) : MIPI_PORT_CTRL(PORT_A);
 		u32 val;
 
@@ -741,7 +740,6 @@ static bool intel_dsi_get_hw_state(struct intel_encoder *encoder,
 {
 	struct drm_i915_private *dev_priv = to_i915(encoder->base.dev);
 	struct intel_dsi *intel_dsi = enc_to_intel_dsi(&encoder->base);
-	struct drm_device *dev = encoder->base.dev;
 	enum intel_display_power_domain power_domain;
 	enum port port;
 	bool active = false;
@@ -762,7 +760,7 @@ static bool intel_dsi_get_hw_state(struct intel_encoder *encoder,
 
 	/* XXX: this only works for one DSI output */
 	for_each_dsi_port(port, intel_dsi->ports) {
-		i915_reg_t ctrl_reg = IS_BROXTON(dev) ?
+		i915_reg_t ctrl_reg = IS_BROXTON(dev_priv) ?
 			BXT_MIPI_PORT_CTRL(port) : MIPI_PORT_CTRL(port);
 		bool enabled = I915_READ(ctrl_reg) & DPI_ENABLE;
 
@@ -771,7 +769,8 @@ static bool intel_dsi_get_hw_state(struct intel_encoder *encoder,
 		 * bit in port C control register does not get set. As a
 		 * workaround, check pipe B conf instead.
 		 */
-		if ((IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) && port == PORT_C)
+		if ((IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv)) &&
+		    port == PORT_C)
 			enabled = I915_READ(PIPECONF(PIPE_B)) & PIPECONF_ENABLE;
 
 		/* Try command mode if video mode not enabled */
@@ -970,11 +969,11 @@ static void bxt_dsi_get_pipe_config(struct intel_encoder *encoder,
 static void intel_dsi_get_config(struct intel_encoder *encoder,
 				 struct intel_crtc_state *pipe_config)
 {
-	struct drm_device *dev = encoder->base.dev;
+	struct drm_i915_private *dev_priv = to_i915(encoder->base.dev);
 	u32 pclk;
 	DRM_DEBUG_KMS("\n");
 
-	if (IS_BROXTON(dev))
+	if (IS_BROXTON(dev_priv))
 		bxt_dsi_get_pipe_config(encoder, pipe_config);
 
 	pclk = intel_dsi_get_pclk(encoder, pipe_config->pipe_bpp,
@@ -1066,7 +1065,7 @@ static void set_dsi_timings(struct drm_encoder *encoder,
 	hbp = txbyteclkhs(hbp, bpp, lane_count, intel_dsi->burst_mode_ratio);
 
 	for_each_dsi_port(port, intel_dsi->ports) {
-		if (IS_BROXTON(dev)) {
+		if (IS_BROXTON(dev_priv)) {
 			/*
 			 * Program hdisplay and vdisplay on MIPI transcoder.
 			 * This is different from calculated hactive and
@@ -1138,7 +1137,7 @@ static void intel_dsi_prepare(struct intel_encoder *intel_encoder,
 	}
 
 	for_each_dsi_port(port, intel_dsi->ports) {
-		if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) {
+		if (IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv)) {
 			/*
 			 * escape clock divider, 20MHz, shared for A and C.
 			 * device ready must be off when doing this! txclkesc?
@@ -1153,7 +1152,7 @@ static void intel_dsi_prepare(struct intel_encoder *intel_encoder,
 			tmp &= ~READ_REQUEST_PRIORITY_MASK;
 			I915_WRITE(MIPI_CTRL(port), tmp |
 					READ_REQUEST_PRIORITY_HIGH);
-		} else if (IS_BROXTON(dev)) {
+		} else if (IS_BROXTON(dev_priv)) {
 			enum pipe pipe = intel_crtc->pipe;
 
 			tmp = I915_READ(MIPI_CTRL(port));
@@ -1242,7 +1241,7 @@ static void intel_dsi_prepare(struct intel_encoder *intel_encoder,
 		I915_WRITE(MIPI_INIT_COUNT(port),
 				txclkesc(intel_dsi->escape_clk_div, 100));
 
-		if (IS_BROXTON(dev) && (!intel_dsi->dual_link)) {
+		if (IS_BROXTON(dev_priv) && (!intel_dsi->dual_link)) {
 			/*
 			 * BXT spec says write MIPI_INIT_COUNT for
 			 * both the ports, even if only one is
@@ -1346,7 +1345,7 @@ static int intel_dsi_set_property(struct drm_connector *connector,
 			DRM_DEBUG_KMS("no scaling not supported\n");
 			return -EINVAL;
 		}
-		if (HAS_GMCH_DISPLAY(dev) &&
+		if (HAS_GMCH_DISPLAY(to_i915(dev)) &&
 		    val == DRM_MODE_SCALE_CENTER) {
 			DRM_DEBUG_KMS("centering not supported\n");
 			return -EINVAL;
@@ -1450,9 +1449,9 @@ void intel_dsi_init(struct drm_device *dev)
 	if (!intel_bios_is_dsi_present(dev_priv, &port))
 		return;
 
-	if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) {
+	if (IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv)) {
 		dev_priv->mipi_mmio_base = VLV_MIPI_BASE;
-	} else if (IS_BROXTON(dev)) {
+	} else if (IS_BROXTON(dev_priv)) {
 		dev_priv->mipi_mmio_base = BXT_MIPI_BASE;
 	} else {
 		DRM_ERROR("Unsupported Mipi device to reg base");
@@ -1488,6 +1487,7 @@ void intel_dsi_init(struct drm_device *dev)
 
 	intel_connector->get_hw_state = intel_connector_get_hw_state;
 
+	intel_encoder->port = port;
 	/*
 	 * On BYT/CHV, pipe A maps to MIPI DSI port A, pipe B maps to MIPI DSI
 	 * port C. BXT isn't limited like this.
diff --git a/drivers/gpu/drm/i915/intel_dsi_panel_vbt.c b/drivers/gpu/drm/i915/intel_dsi_panel_vbt.c
index cd154ce6b6c1..9f279a3d0f74 100644
--- a/drivers/gpu/drm/i915/intel_dsi_panel_vbt.c
+++ b/drivers/gpu/drm/i915/intel_dsi_panel_vbt.c
@@ -126,6 +126,8 @@ static const u8 *mipi_exec_send_packet(struct intel_dsi *intel_dsi,
 	u16 len;
 	enum port port;
 
+	DRM_DEBUG_KMS("\n");
+
 	flags = *data++;
 	type = *data++;
 
@@ -199,6 +201,8 @@ static const u8 *mipi_exec_delay(struct intel_dsi *intel_dsi, const u8 *data)
 {
 	u32 delay = *((const u32 *) data);
 
+	DRM_DEBUG_KMS("\n");
+
 	usleep_range(delay, delay + 10);
 	data += 4;
 
@@ -307,6 +311,8 @@ static const u8 *mipi_exec_gpio(struct intel_dsi *intel_dsi, const u8 *data)
 	u8 gpio_source, gpio_index;
 	bool value;
 
+	DRM_DEBUG_KMS("\n");
+
 	if (dev_priv->vbt.dsi.seq_version >= 3)
 		data++;
 
@@ -331,18 +337,36 @@ static const u8 *mipi_exec_gpio(struct intel_dsi *intel_dsi, const u8 *data)
 	return data;
 }
 
-static const u8 *mipi_exec_i2c_skip(struct intel_dsi *intel_dsi, const u8 *data)
+static const u8 *mipi_exec_i2c(struct intel_dsi *intel_dsi, const u8 *data)
 {
+	DRM_DEBUG_KMS("Skipping I2C element execution\n");
+
 	return data + *(data + 6) + 7;
 }
 
+static const u8 *mipi_exec_spi(struct intel_dsi *intel_dsi, const u8 *data)
+{
+	DRM_DEBUG_KMS("Skipping SPI element execution\n");
+
+	return data + *(data + 5) + 6;
+}
+
+static const u8 *mipi_exec_pmic(struct intel_dsi *intel_dsi, const u8 *data)
+{
+	DRM_DEBUG_KMS("Skipping PMIC element execution\n");
+
+	return data + 15;
+}
+
 typedef const u8 * (*fn_mipi_elem_exec)(struct intel_dsi *intel_dsi,
 					const u8 *data);
 static const fn_mipi_elem_exec exec_elem[] = {
 	[MIPI_SEQ_ELEM_SEND_PKT] = mipi_exec_send_packet,
 	[MIPI_SEQ_ELEM_DELAY] = mipi_exec_delay,
 	[MIPI_SEQ_ELEM_GPIO] = mipi_exec_gpio,
-	[MIPI_SEQ_ELEM_I2C] = mipi_exec_i2c_skip,
+	[MIPI_SEQ_ELEM_I2C] = mipi_exec_i2c,
+	[MIPI_SEQ_ELEM_SPI] = mipi_exec_spi,
+	[MIPI_SEQ_ELEM_PMIC] = mipi_exec_pmic,
 };
 
 /*
@@ -385,11 +409,8 @@ static void generic_exec_sequence(struct drm_panel *panel, enum mipi_seq seq_id)
 		return;
 
 	data = dev_priv->vbt.dsi.sequence[seq_id];
-	if (!data) {
-		DRM_DEBUG_KMS("MIPI sequence %d - %s not available\n",
-			      seq_id, sequence_name(seq_id));
+	if (!data)
 		return;
-	}
 
 	WARN_ON(*data != seq_id);
 
@@ -420,7 +441,15 @@ static void generic_exec_sequence(struct drm_panel *panel, enum mipi_seq seq_id)
 			operation_size = *data++;
 
 		if (mipi_elem_exec) {
+			const u8 *next = data + operation_size;
+
 			data = mipi_elem_exec(intel_dsi, data);
+
+			/* Consistency check if we have size. */
+			if (operation_size && data != next) {
+				DRM_ERROR("Inconsistent operation size\n");
+				return;
+			}
 		} else if (operation_size) {
 			/* We have size, skip. */
 			DRM_DEBUG_KMS("Unsupported MIPI operation byte %u\n",
@@ -438,6 +467,8 @@ static void generic_exec_sequence(struct drm_panel *panel, enum mipi_seq seq_id)
 static int vbt_panel_prepare(struct drm_panel *panel)
 {
 	generic_exec_sequence(panel, MIPI_SEQ_ASSERT_RESET);
+	generic_exec_sequence(panel, MIPI_SEQ_POWER_ON);
+	generic_exec_sequence(panel, MIPI_SEQ_DEASSERT_RESET);
 	generic_exec_sequence(panel, MIPI_SEQ_INIT_OTP);
 
 	return 0;
@@ -445,7 +476,8 @@ static int vbt_panel_prepare(struct drm_panel *panel)
 
 static int vbt_panel_unprepare(struct drm_panel *panel)
 {
-	generic_exec_sequence(panel, MIPI_SEQ_DEASSERT_RESET);
+	generic_exec_sequence(panel, MIPI_SEQ_ASSERT_RESET);
+	generic_exec_sequence(panel, MIPI_SEQ_POWER_OFF);
 
 	return 0;
 }
@@ -453,12 +485,14 @@ static int vbt_panel_unprepare(struct drm_panel *panel)
 static int vbt_panel_enable(struct drm_panel *panel)
 {
 	generic_exec_sequence(panel, MIPI_SEQ_DISPLAY_ON);
+	generic_exec_sequence(panel, MIPI_SEQ_BACKLIGHT_ON);
 
 	return 0;
 }
 
 static int vbt_panel_disable(struct drm_panel *panel)
 {
+	generic_exec_sequence(panel, MIPI_SEQ_BACKLIGHT_OFF);
 	generic_exec_sequence(panel, MIPI_SEQ_DISPLAY_OFF);
 
 	return 0;
diff --git a/drivers/gpu/drm/i915/intel_dsi_pll.c b/drivers/gpu/drm/i915/intel_dsi_pll.c
index 6ab58a01b18e..56eff6004bc0 100644
--- a/drivers/gpu/drm/i915/intel_dsi_pll.c
+++ b/drivers/gpu/drm/i915/intel_dsi_pll.c
@@ -351,7 +351,7 @@ static u32 bxt_dsi_get_pclk(struct intel_encoder *encoder, int pipe_bpp,
 u32 intel_dsi_get_pclk(struct intel_encoder *encoder, int pipe_bpp,
 		       struct intel_crtc_state *config)
 {
-	if (IS_BROXTON(encoder->base.dev))
+	if (IS_BROXTON(to_i915(encoder->base.dev)))
 		return bxt_dsi_get_pclk(encoder, pipe_bpp, config);
 	else
 		return vlv_dsi_get_pclk(encoder, pipe_bpp, config);
@@ -515,11 +515,11 @@ bool intel_dsi_pll_is_enabled(struct drm_i915_private *dev_priv)
 int intel_compute_dsi_pll(struct intel_encoder *encoder,
 			  struct intel_crtc_state *config)
 {
-	struct drm_device *dev = encoder->base.dev;
+	struct drm_i915_private *dev_priv = to_i915(encoder->base.dev);
 
-	if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev))
+	if (IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv))
 		return vlv_compute_dsi_pll(encoder, config);
-	else if (IS_BROXTON(dev))
+	else if (IS_BROXTON(dev_priv))
 		return bxt_compute_dsi_pll(encoder, config);
 
 	return -ENODEV;
@@ -528,21 +528,21 @@ int intel_compute_dsi_pll(struct intel_encoder *encoder,
 void intel_enable_dsi_pll(struct intel_encoder *encoder,
 			  const struct intel_crtc_state *config)
 {
-	struct drm_device *dev = encoder->base.dev;
+	struct drm_i915_private *dev_priv = to_i915(encoder->base.dev);
 
-	if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev))
+	if (IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv))
 		vlv_enable_dsi_pll(encoder, config);
-	else if (IS_BROXTON(dev))
+	else if (IS_BROXTON(dev_priv))
 		bxt_enable_dsi_pll(encoder, config);
 }
 
 void intel_disable_dsi_pll(struct intel_encoder *encoder)
 {
-	struct drm_device *dev = encoder->base.dev;
+	struct drm_i915_private *dev_priv = to_i915(encoder->base.dev);
 
-	if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev))
+	if (IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv))
 		vlv_disable_dsi_pll(encoder);
-	else if (IS_BROXTON(dev))
+	else if (IS_BROXTON(dev_priv))
 		bxt_disable_dsi_pll(encoder);
 }
 
@@ -564,10 +564,10 @@ static void bxt_dsi_reset_clocks(struct intel_encoder *encoder, enum port port)
 
 void intel_dsi_reset_clocks(struct intel_encoder *encoder, enum port port)
 {
-	struct drm_device *dev = encoder->base.dev;
+	struct drm_i915_private *dev_priv = to_i915(encoder->base.dev);
 
-	if (IS_BROXTON(dev))
+	if (IS_BROXTON(dev_priv))
 		bxt_dsi_reset_clocks(encoder, port);
-	else if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev))
+	else if (IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv))
 		vlv_dsi_reset_clocks(encoder, port);
 }
diff --git a/drivers/gpu/drm/i915/intel_dvo.c b/drivers/gpu/drm/i915/intel_dvo.c
index 2e452c505e7e..cd574900cd8d 100644
--- a/drivers/gpu/drm/i915/intel_dvo.c
+++ b/drivers/gpu/drm/i915/intel_dvo.c
@@ -412,16 +412,14 @@ intel_dvo_get_current_mode(struct drm_connector *connector)
 	return mode;
 }
 
-static char intel_dvo_port_name(i915_reg_t dvo_reg)
+static enum port intel_dvo_port(i915_reg_t dvo_reg)
 {
 	if (i915_mmio_reg_equal(dvo_reg, DVOA))
-		return 'A';
+		return PORT_A;
 	else if (i915_mmio_reg_equal(dvo_reg, DVOB))
-		return 'B';
-	else if (i915_mmio_reg_equal(dvo_reg, DVOC))
-		return 'C';
+		return PORT_B;
 	else
-		return '?';
+		return PORT_C;
 }
 
 void intel_dvo_init(struct drm_device *dev)
@@ -464,6 +462,7 @@ void intel_dvo_init(struct drm_device *dev)
 		bool dvoinit;
 		enum pipe pipe;
 		uint32_t dpll[I915_MAX_PIPES];
+		enum port port;
 
 		/* Allow the I2C driver info to specify the GPIO to be used in
 		 * special cases, but otherwise default to what's defined
@@ -511,12 +510,15 @@ void intel_dvo_init(struct drm_device *dev)
 		if (!dvoinit)
 			continue;
 
+		port = intel_dvo_port(dvo->dvo_reg);
 		drm_encoder_init(dev, &intel_encoder->base,
 				 &intel_dvo_enc_funcs, encoder_type,
-				 "DVO %c", intel_dvo_port_name(dvo->dvo_reg));
+				 "DVO %c", port_name(port));
 
 		intel_encoder->type = INTEL_OUTPUT_DVO;
+		intel_encoder->port = port;
 		intel_encoder->crtc_mask = (1 << 0) | (1 << 1);
+
 		switch (dvo->type) {
 		case INTEL_DVO_CHIP_TMDS:
 			intel_encoder->cloneable = (1 << INTEL_OUTPUT_ANALOG) |
diff --git a/drivers/gpu/drm/i915/intel_engine_cs.c b/drivers/gpu/drm/i915/intel_engine_cs.c
index e405f1080296..2dc94812bea5 100644
--- a/drivers/gpu/drm/i915/intel_engine_cs.c
+++ b/drivers/gpu/drm/i915/intel_engine_cs.c
@@ -82,12 +82,17 @@ static const struct engine_info {
 	},
 };
 
-static struct intel_engine_cs *
+static int
 intel_engine_setup(struct drm_i915_private *dev_priv,
 		   enum intel_engine_id id)
 {
 	const struct engine_info *info = &intel_engines[id];
-	struct intel_engine_cs *engine = &dev_priv->engine[id];
+	struct intel_engine_cs *engine;
+
+	GEM_BUG_ON(dev_priv->engine[id]);
+	engine = kzalloc(sizeof(*engine), GFP_KERNEL);
+	if (!engine)
+		return -ENOMEM;
 
 	engine->id = id;
 	engine->i915 = dev_priv;
@@ -97,7 +102,8 @@ intel_engine_setup(struct drm_i915_private *dev_priv,
 	engine->mmio_base = info->mmio_base;
 	engine->irq_shift = info->irq_shift;
 
-	return engine;
+	dev_priv->engine[id] = engine;
+	return 0;
 }
 
 /**
@@ -110,13 +116,16 @@ int intel_engines_init(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = to_i915(dev);
 	struct intel_device_info *device_info = mkwrite_device_info(dev_priv);
+	unsigned int ring_mask = INTEL_INFO(dev_priv)->ring_mask;
 	unsigned int mask = 0;
 	int (*init)(struct intel_engine_cs *engine);
+	struct intel_engine_cs *engine;
+	enum intel_engine_id id;
 	unsigned int i;
 	int ret;
 
-	WARN_ON(INTEL_INFO(dev_priv)->ring_mask == 0);
-	WARN_ON(INTEL_INFO(dev_priv)->ring_mask &
+	WARN_ON(ring_mask == 0);
+	WARN_ON(ring_mask &
 		GENMASK(sizeof(mask) * BITS_PER_BYTE - 1, I915_NUM_ENGINES));
 
 	for (i = 0; i < ARRAY_SIZE(intel_engines); i++) {
@@ -131,7 +140,11 @@ int intel_engines_init(struct drm_device *dev)
 		if (!init)
 			continue;
 
-		ret = init(intel_engine_setup(dev_priv, i));
+		ret = intel_engine_setup(dev_priv, i);
+		if (ret)
+			goto cleanup;
+
+		ret = init(dev_priv->engine[i]);
 		if (ret)
 			goto cleanup;
 
@@ -143,7 +156,7 @@ int intel_engines_init(struct drm_device *dev)
 	 * are added to the driver by a warning and disabling the forgotten
 	 * engines.
 	 */
-	if (WARN_ON(mask != INTEL_INFO(dev_priv)->ring_mask))
+	if (WARN_ON(mask != ring_mask))
 		device_info->ring_mask = mask;
 
 	device_info->num_rings = hweight32(mask);
@@ -151,11 +164,11 @@ int intel_engines_init(struct drm_device *dev)
 	return 0;
 
 cleanup:
-	for (i = 0; i < I915_NUM_ENGINES; i++) {
+	for_each_engine(engine, dev_priv, id) {
 		if (i915.enable_execlists)
-			intel_logical_ring_cleanup(&dev_priv->engine[i]);
+			intel_logical_ring_cleanup(engine);
 		else
-			intel_engine_cleanup(&dev_priv->engine[i]);
+			intel_engine_cleanup(engine);
 	}
 
 	return ret;
@@ -210,9 +223,6 @@ void intel_engine_init_seqno(struct intel_engine_cs *engine, u32 seqno)
 void intel_engine_init_hangcheck(struct intel_engine_cs *engine)
 {
 	memset(&engine->hangcheck, 0, sizeof(engine->hangcheck));
-	clear_bit(engine->id, &engine->i915->gpu_error.missed_irq_rings);
-	if (intel_engine_has_waiter(engine))
-		i915_queue_hangcheck(engine->i915);
 }
 
 static void intel_engine_init_requests(struct intel_engine_cs *engine)
@@ -307,18 +317,6 @@ int intel_engine_init_common(struct intel_engine_cs *engine)
 	return 0;
 }
 
-void intel_engine_reset_irq(struct intel_engine_cs *engine)
-{
-	struct drm_i915_private *dev_priv = engine->i915;
-
-	spin_lock_irq(&dev_priv->irq_lock);
-	if (intel_engine_has_waiter(engine))
-		engine->irq_enable(engine);
-	else
-		engine->irq_disable(engine);
-	spin_unlock_irq(&dev_priv->irq_lock);
-}
-
 /**
  * intel_engines_cleanup_common - cleans up the engine state created by
  *                                the common initiailizers.
@@ -334,3 +332,137 @@ void intel_engine_cleanup_common(struct intel_engine_cs *engine)
 	intel_engine_cleanup_cmd_parser(engine);
 	i915_gem_batch_pool_fini(&engine->batch_pool);
 }
+
+u64 intel_engine_get_active_head(struct intel_engine_cs *engine)
+{
+	struct drm_i915_private *dev_priv = engine->i915;
+	u64 acthd;
+
+	if (INTEL_GEN(dev_priv) >= 8)
+		acthd = I915_READ64_2x32(RING_ACTHD(engine->mmio_base),
+					 RING_ACTHD_UDW(engine->mmio_base));
+	else if (INTEL_GEN(dev_priv) >= 4)
+		acthd = I915_READ(RING_ACTHD(engine->mmio_base));
+	else
+		acthd = I915_READ(ACTHD);
+
+	return acthd;
+}
+
+u64 intel_engine_get_last_batch_head(struct intel_engine_cs *engine)
+{
+	struct drm_i915_private *dev_priv = engine->i915;
+	u64 bbaddr;
+
+	if (INTEL_GEN(dev_priv) >= 8)
+		bbaddr = I915_READ64_2x32(RING_BBADDR(engine->mmio_base),
+					  RING_BBADDR_UDW(engine->mmio_base));
+	else
+		bbaddr = I915_READ(RING_BBADDR(engine->mmio_base));
+
+	return bbaddr;
+}
+
+const char *i915_cache_level_str(struct drm_i915_private *i915, int type)
+{
+	switch (type) {
+	case I915_CACHE_NONE: return " uncached";
+	case I915_CACHE_LLC: return HAS_LLC(i915) ? " LLC" : " snooped";
+	case I915_CACHE_L3_LLC: return " L3+LLC";
+	case I915_CACHE_WT: return " WT";
+	default: return "";
+	}
+}
+
+static inline uint32_t
+read_subslice_reg(struct drm_i915_private *dev_priv, int slice,
+		  int subslice, i915_reg_t reg)
+{
+	uint32_t mcr;
+	uint32_t ret;
+	enum forcewake_domains fw_domains;
+
+	fw_domains = intel_uncore_forcewake_for_reg(dev_priv, reg,
+						    FW_REG_READ);
+	fw_domains |= intel_uncore_forcewake_for_reg(dev_priv,
+						     GEN8_MCR_SELECTOR,
+						     FW_REG_READ | FW_REG_WRITE);
+
+	spin_lock_irq(&dev_priv->uncore.lock);
+	intel_uncore_forcewake_get__locked(dev_priv, fw_domains);
+
+	mcr = I915_READ_FW(GEN8_MCR_SELECTOR);
+	/*
+	 * The HW expects the slice and sublice selectors to be reset to 0
+	 * after reading out the registers.
+	 */
+	WARN_ON_ONCE(mcr & (GEN8_MCR_SLICE_MASK | GEN8_MCR_SUBSLICE_MASK));
+	mcr &= ~(GEN8_MCR_SLICE_MASK | GEN8_MCR_SUBSLICE_MASK);
+	mcr |= GEN8_MCR_SLICE(slice) | GEN8_MCR_SUBSLICE(subslice);
+	I915_WRITE_FW(GEN8_MCR_SELECTOR, mcr);
+
+	ret = I915_READ_FW(reg);
+
+	mcr &= ~(GEN8_MCR_SLICE_MASK | GEN8_MCR_SUBSLICE_MASK);
+	I915_WRITE_FW(GEN8_MCR_SELECTOR, mcr);
+
+	intel_uncore_forcewake_put__locked(dev_priv, fw_domains);
+	spin_unlock_irq(&dev_priv->uncore.lock);
+
+	return ret;
+}
+
+/* NB: please notice the memset */
+void intel_engine_get_instdone(struct intel_engine_cs *engine,
+			       struct intel_instdone *instdone)
+{
+	struct drm_i915_private *dev_priv = engine->i915;
+	u32 mmio_base = engine->mmio_base;
+	int slice;
+	int subslice;
+
+	memset(instdone, 0, sizeof(*instdone));
+
+	switch (INTEL_GEN(dev_priv)) {
+	default:
+		instdone->instdone = I915_READ(RING_INSTDONE(mmio_base));
+
+		if (engine->id != RCS)
+			break;
+
+		instdone->slice_common = I915_READ(GEN7_SC_INSTDONE);
+		for_each_instdone_slice_subslice(dev_priv, slice, subslice) {
+			instdone->sampler[slice][subslice] =
+				read_subslice_reg(dev_priv, slice, subslice,
+						  GEN7_SAMPLER_INSTDONE);
+			instdone->row[slice][subslice] =
+				read_subslice_reg(dev_priv, slice, subslice,
+						  GEN7_ROW_INSTDONE);
+		}
+		break;
+	case 7:
+		instdone->instdone = I915_READ(RING_INSTDONE(mmio_base));
+
+		if (engine->id != RCS)
+			break;
+
+		instdone->slice_common = I915_READ(GEN7_SC_INSTDONE);
+		instdone->sampler[0][0] = I915_READ(GEN7_SAMPLER_INSTDONE);
+		instdone->row[0][0] = I915_READ(GEN7_ROW_INSTDONE);
+
+		break;
+	case 6:
+	case 5:
+	case 4:
+		instdone->instdone = I915_READ(RING_INSTDONE(mmio_base));
+
+		if (engine->id == RCS)
+			/* HACK: Using the wrong struct member */
+			instdone->slice_common = I915_READ(GEN4_INSTDONE1);
+		break;
+	case 3:
+	case 2:
+		instdone->instdone = I915_READ(GEN2_INSTDONE);
+		break;
+	}
+}
diff --git a/drivers/gpu/drm/i915/intel_fbc.c b/drivers/gpu/drm/i915/intel_fbc.c
index afc040be1172..49048d9d7895 100644
--- a/drivers/gpu/drm/i915/intel_fbc.c
+++ b/drivers/gpu/drm/i915/intel_fbc.c
@@ -774,6 +774,14 @@ static bool intel_fbc_can_activate(struct intel_crtc *crtc)
 	struct intel_fbc *fbc = &dev_priv->fbc;
 	struct intel_fbc_state_cache *cache = &fbc->state_cache;
 
+	/* We don't need to use a state cache here since this information is
+	 * global for all CRTC.
+	 */
+	if (fbc->underrun_detected) {
+		fbc->no_fbc_reason = "underrun detected";
+		return false;
+	}
+
 	if (!cache->plane.visible) {
 		fbc->no_fbc_reason = "primary plane not visible";
 		return false;
@@ -859,6 +867,11 @@ static bool intel_fbc_can_choose(struct intel_crtc *crtc)
 		return false;
 	}
 
+	if (fbc->underrun_detected) {
+		fbc->no_fbc_reason = "underrun detected";
+		return false;
+	}
+
 	if (fbc_on_pipe_a_only(dev_priv) && crtc->pipe != PIPE_A) {
 		fbc->no_fbc_reason = "no enabled pipes can have FBC";
 		return false;
@@ -1221,6 +1234,59 @@ void intel_fbc_global_disable(struct drm_i915_private *dev_priv)
 	cancel_work_sync(&fbc->work.work);
 }
 
+static void intel_fbc_underrun_work_fn(struct work_struct *work)
+{
+	struct drm_i915_private *dev_priv =
+		container_of(work, struct drm_i915_private, fbc.underrun_work);
+	struct intel_fbc *fbc = &dev_priv->fbc;
+
+	mutex_lock(&fbc->lock);
+
+	/* Maybe we were scheduled twice. */
+	if (fbc->underrun_detected)
+		goto out;
+
+	DRM_DEBUG_KMS("Disabling FBC due to FIFO underrun.\n");
+	fbc->underrun_detected = true;
+
+	intel_fbc_deactivate(dev_priv);
+out:
+	mutex_unlock(&fbc->lock);
+}
+
+/**
+ * intel_fbc_handle_fifo_underrun_irq - disable FBC when we get a FIFO underrun
+ * @dev_priv: i915 device instance
+ *
+ * Without FBC, most underruns are harmless and don't really cause too many
+ * problems, except for an annoying message on dmesg. With FBC, underruns can
+ * become black screens or even worse, especially when paired with bad
+ * watermarks. So in order for us to be on the safe side, completely disable FBC
+ * in case we ever detect a FIFO underrun on any pipe. An underrun on any pipe
+ * already suggests that watermarks may be bad, so try to be as safe as
+ * possible.
+ *
+ * This function is called from the IRQ handler.
+ */
+void intel_fbc_handle_fifo_underrun_irq(struct drm_i915_private *dev_priv)
+{
+	struct intel_fbc *fbc = &dev_priv->fbc;
+
+	if (!fbc_supported(dev_priv))
+		return;
+
+	/* There's no guarantee that underrun_detected won't be set to true
+	 * right after this check and before the work is scheduled, but that's
+	 * not a problem since we'll check it again under the work function
+	 * while FBC is locked. This check here is just to prevent us from
+	 * unnecessarily scheduling the work, and it relies on the fact that we
+	 * never switch underrun_detect back to false after it's true. */
+	if (READ_ONCE(fbc->underrun_detected))
+		return;
+
+	schedule_work(&fbc->underrun_work);
+}
+
 /**
  * intel_fbc_init_pipe_state - initialize FBC's CRTC visibility tracking
  * @dev_priv: i915 device instance
@@ -1292,6 +1358,7 @@ void intel_fbc_init(struct drm_i915_private *dev_priv)
 	enum pipe pipe;
 
 	INIT_WORK(&fbc->work.work, intel_fbc_work_fn);
+	INIT_WORK(&fbc->underrun_work, intel_fbc_underrun_work_fn);
 	mutex_init(&fbc->lock);
 	fbc->enabled = false;
 	fbc->active = false;
diff --git a/drivers/gpu/drm/i915/intel_fifo_underrun.c b/drivers/gpu/drm/i915/intel_fifo_underrun.c
index 2aa744081f09..3018f4f589c8 100644
--- a/drivers/gpu/drm/i915/intel_fifo_underrun.c
+++ b/drivers/gpu/drm/i915/intel_fifo_underrun.c
@@ -254,13 +254,13 @@ static bool __intel_set_cpu_fifo_underrun_reporting(struct drm_device *dev,
 	old = !intel_crtc->cpu_fifo_underrun_disabled;
 	intel_crtc->cpu_fifo_underrun_disabled = !enable;
 
-	if (HAS_GMCH_DISPLAY(dev))
+	if (HAS_GMCH_DISPLAY(dev_priv))
 		i9xx_set_fifo_underrun_reporting(dev, pipe, enable, old);
-	else if (IS_GEN5(dev) || IS_GEN6(dev))
+	else if (IS_GEN5(dev_priv) || IS_GEN6(dev_priv))
 		ironlake_set_fifo_underrun_reporting(dev, pipe, enable);
-	else if (IS_GEN7(dev))
+	else if (IS_GEN7(dev_priv))
 		ivybridge_set_fifo_underrun_reporting(dev, pipe, enable, old);
-	else if (IS_GEN8(dev) || IS_GEN9(dev))
+	else if (IS_GEN8(dev_priv) || IS_GEN9(dev_priv))
 		broadwell_set_fifo_underrun_reporting(dev, pipe, enable);
 
 	return old;
@@ -372,6 +372,8 @@ void intel_cpu_fifo_underrun_irq_handler(struct drm_i915_private *dev_priv,
 	if (intel_set_cpu_fifo_underrun_reporting(dev_priv, pipe, false))
 		DRM_ERROR("CPU pipe %c FIFO underrun\n",
 			  pipe_name(pipe));
+
+	intel_fbc_handle_fifo_underrun_irq(dev_priv);
 }
 
 /**
diff --git a/drivers/gpu/drm/i915/intel_guc.h b/drivers/gpu/drm/i915/intel_guc.h
index b1ba86958811..5cdf7aa75be5 100644
--- a/drivers/gpu/drm/i915/intel_guc.h
+++ b/drivers/gpu/drm/i915/intel_guc.h
@@ -160,6 +160,7 @@ extern int intel_guc_resume(struct drm_device *dev);
 int i915_guc_submission_init(struct drm_i915_private *dev_priv);
 int i915_guc_submission_enable(struct drm_i915_private *dev_priv);
 int i915_guc_wq_reserve(struct drm_i915_gem_request *rq);
+void i915_guc_wq_unreserve(struct drm_i915_gem_request *request);
 void i915_guc_submission_disable(struct drm_i915_private *dev_priv);
 void i915_guc_submission_fini(struct drm_i915_private *dev_priv);
 
diff --git a/drivers/gpu/drm/i915/intel_guc_loader.c b/drivers/gpu/drm/i915/intel_guc_loader.c
index 6fd39efb7894..3c8eaae13732 100644
--- a/drivers/gpu/drm/i915/intel_guc_loader.c
+++ b/drivers/gpu/drm/i915/intel_guc_loader.c
@@ -100,12 +100,13 @@ const char *intel_guc_fw_status_repr(enum intel_guc_fw_status status)
 static void guc_interrupts_release(struct drm_i915_private *dev_priv)
 {
 	struct intel_engine_cs *engine;
+	enum intel_engine_id id;
 	int irqs;
 
 	/* tell all command streamers NOT to forward interrupts or vblank to GuC */
 	irqs = _MASKED_FIELD(GFX_FORWARD_VBLANK_MASK, GFX_FORWARD_VBLANK_NEVER);
 	irqs |= _MASKED_BIT_DISABLE(GFX_INTERRUPT_STEERING);
-	for_each_engine(engine, dev_priv)
+	for_each_engine(engine, dev_priv, id)
 		I915_WRITE(RING_MODE_GEN7(engine), irqs);
 
 	/* route all GT interrupts to the host */
@@ -117,12 +118,13 @@ static void guc_interrupts_release(struct drm_i915_private *dev_priv)
 static void guc_interrupts_capture(struct drm_i915_private *dev_priv)
 {
 	struct intel_engine_cs *engine;
+	enum intel_engine_id id;
 	int irqs;
 	u32 tmp;
 
 	/* tell all command streamers to forward interrupts (but not vblank) to GuC */
 	irqs = _MASKED_BIT_ENABLE(GFX_INTERRUPT_STEERING);
-	for_each_engine(engine, dev_priv)
+	for_each_engine(engine, dev_priv, id)
 		I915_WRITE(RING_MODE_GEN7(engine), irqs);
 
 	/* route USER_INTERRUPT to Host, all others are sent to GuC. */
@@ -347,7 +349,6 @@ static u32 guc_wopcm_size(struct drm_i915_private *dev_priv)
 static int guc_ucode_xfer(struct drm_i915_private *dev_priv)
 {
 	struct intel_guc_fw *guc_fw = &dev_priv->guc.guc_fw;
-	struct drm_device *dev = &dev_priv->drm;
 	struct i915_vma *vma;
 	int ret;
 
@@ -375,24 +376,22 @@ static int guc_ucode_xfer(struct drm_i915_private *dev_priv)
 	/* Enable MIA caching. GuC clock gating is disabled. */
 	I915_WRITE(GUC_SHIM_CONTROL, GUC_SHIM_CONTROL_VALUE);
 
-	/* WaDisableMinuteIaClockGating:skl,bxt */
-	if (IS_SKL_REVID(dev, 0, SKL_REVID_B0) ||
-	    IS_BXT_REVID(dev, 0, BXT_REVID_A1)) {
+	/* WaDisableMinuteIaClockGating:bxt */
+	if (IS_BXT_REVID(dev_priv, 0, BXT_REVID_A1)) {
 		I915_WRITE(GUC_SHIM_CONTROL, (I915_READ(GUC_SHIM_CONTROL) &
 					      ~GUC_ENABLE_MIA_CLOCK_GATING));
 	}
 
-	/* WaC6DisallowByGfxPause*/
-	if (IS_SKL_REVID(dev, 0, SKL_REVID_C0) ||
-	    IS_BXT_REVID(dev, 0, BXT_REVID_B0))
+	/* WaC6DisallowByGfxPause:bxt */
+	if (IS_BXT_REVID(dev_priv, 0, BXT_REVID_B0))
 		I915_WRITE(GEN6_GFXPAUSE, 0x30FFF);
 
-	if (IS_BROXTON(dev))
+	if (IS_BROXTON(dev_priv))
 		I915_WRITE(GEN9LP_GT_PM_CONFIG, GT_DOORBELL_ENABLE);
 	else
 		I915_WRITE(GEN9_GT_PM_CONFIG, GT_DOORBELL_ENABLE);
 
-	if (IS_GEN9(dev)) {
+	if (IS_GEN9(dev_priv)) {
 		/* DOP Clock Gating Enable for GuC clocks */
 		I915_WRITE(GEN7_MISCCPCTL, (GEN8_DOP_CLOCK_GATE_GUC_ENABLE |
 					    I915_READ(GEN7_MISCCPCTL)));
@@ -720,23 +719,28 @@ void intel_guc_init(struct drm_device *dev)
 	struct intel_guc_fw *guc_fw = &dev_priv->guc.guc_fw;
 	const char *fw_path;
 
-	/* A negative value means "use platform default" */
-	if (i915.enable_guc_loading < 0)
-		i915.enable_guc_loading = HAS_GUC_UCODE(dev);
-	if (i915.enable_guc_submission < 0)
-		i915.enable_guc_submission = HAS_GUC_SCHED(dev);
+	if (!HAS_GUC(dev)) {
+		i915.enable_guc_loading = 0;
+		i915.enable_guc_submission = 0;
+	} else {
+		/* A negative value means "use platform default" */
+		if (i915.enable_guc_loading < 0)
+			i915.enable_guc_loading = HAS_GUC_UCODE(dev);
+		if (i915.enable_guc_submission < 0)
+			i915.enable_guc_submission = HAS_GUC_SCHED(dev);
+	}
 
 	if (!HAS_GUC_UCODE(dev)) {
 		fw_path = NULL;
-	} else if (IS_SKYLAKE(dev)) {
+	} else if (IS_SKYLAKE(dev_priv)) {
 		fw_path = I915_SKL_GUC_UCODE;
 		guc_fw->guc_fw_major_wanted = SKL_FW_MAJOR;
 		guc_fw->guc_fw_minor_wanted = SKL_FW_MINOR;
-	} else if (IS_BROXTON(dev)) {
+	} else if (IS_BROXTON(dev_priv)) {
 		fw_path = I915_BXT_GUC_UCODE;
 		guc_fw->guc_fw_major_wanted = BXT_FW_MAJOR;
 		guc_fw->guc_fw_minor_wanted = BXT_FW_MINOR;
-	} else if (IS_KABYLAKE(dev)) {
+	} else if (IS_KABYLAKE(dev_priv)) {
 		fw_path = I915_KBL_GUC_UCODE;
 		guc_fw->guc_fw_major_wanted = KBL_FW_MAJOR;
 		guc_fw->guc_fw_minor_wanted = KBL_FW_MINOR;
diff --git a/drivers/gpu/drm/i915/intel_gvt.c b/drivers/gpu/drm/i915/intel_gvt.c
index 434f4d5c553d..290384e86c63 100644
--- a/drivers/gpu/drm/i915/intel_gvt.c
+++ b/drivers/gpu/drm/i915/intel_gvt.c
@@ -31,14 +31,20 @@
  * GPU among multiple virtual machines on a time-sharing basis. Each
  * virtual machine is presented a virtual GPU (vGPU), which has equivalent
  * features as the underlying physical GPU (pGPU), so i915 driver can run
- * seamlessly in a virtual machine. This file provides the englightments
- * of GVT and the necessary components used by GVT in i915 driver.
+ * seamlessly in a virtual machine.
+ *
+ * To virtualize GPU resources GVT-g driver depends on hypervisor technology
+ * e.g KVM/VFIO/mdev, Xen, etc. to provide resource access trapping capability
+ * and be virtualized within GVT-g device module. More architectural design
+ * doc is available on https://01.org/group/2230/documentation-list.
  */
 
 static bool is_supported_device(struct drm_i915_private *dev_priv)
 {
 	if (IS_BROADWELL(dev_priv))
 		return true;
+	if (IS_SKYLAKE(dev_priv))
+		return true;
 	return false;
 }
 
diff --git a/drivers/gpu/drm/i915/intel_gvt.h b/drivers/gpu/drm/i915/intel_gvt.h
index 960211df74db..25df2d65b985 100644
--- a/drivers/gpu/drm/i915/intel_gvt.h
+++ b/drivers/gpu/drm/i915/intel_gvt.h
@@ -24,7 +24,7 @@
 #ifndef _INTEL_GVT_H_
 #define _INTEL_GVT_H_
 
-#include "gvt/gvt.h"
+struct intel_gvt;
 
 #ifdef CONFIG_DRM_I915_GVT
 int intel_gvt_init(struct drm_i915_private *dev_priv);
diff --git a/drivers/gpu/drm/i915/intel_hdmi.c b/drivers/gpu/drm/i915/intel_hdmi.c
index f40a35f2913a..af8715f31807 100644
--- a/drivers/gpu/drm/i915/intel_hdmi.c
+++ b/drivers/gpu/drm/i915/intel_hdmi.c
@@ -50,7 +50,7 @@ assert_hdmi_port_disabled(struct intel_hdmi *intel_hdmi)
 	struct drm_i915_private *dev_priv = to_i915(dev);
 	uint32_t enabled_bits;
 
-	enabled_bits = HAS_DDI(dev) ? DDI_BUF_CTL_ENABLE : SDVO_ENABLE;
+	enabled_bits = HAS_DDI(dev_priv) ? DDI_BUF_CTL_ENABLE : SDVO_ENABLE;
 
 	WARN(I915_READ(intel_hdmi->hdmi_reg) & enabled_bits,
 	     "HDMI port enabled, expecting disabled\n");
@@ -864,7 +864,7 @@ static void intel_hdmi_prepare(struct intel_encoder *encoder)
 	intel_dp_dual_mode_set_tmds_output(intel_hdmi, true);
 
 	hdmi_val = SDVO_ENCODING_HDMI;
-	if (!HAS_PCH_SPLIT(dev) && crtc->config->limited_color_range)
+	if (!HAS_PCH_SPLIT(dev_priv) && crtc->config->limited_color_range)
 		hdmi_val |= HDMI_COLOR_RANGE_16_235;
 	if (adjusted_mode->flags & DRM_MODE_FLAG_PVSYNC)
 		hdmi_val |= SDVO_VSYNC_ACTIVE_HIGH;
@@ -879,9 +879,9 @@ static void intel_hdmi_prepare(struct intel_encoder *encoder)
 	if (crtc->config->has_hdmi_sink)
 		hdmi_val |= HDMI_MODE_SELECT_HDMI;
 
-	if (HAS_PCH_CPT(dev))
+	if (HAS_PCH_CPT(dev_priv))
 		hdmi_val |= SDVO_PIPE_SEL_CPT(crtc->pipe);
-	else if (IS_CHERRYVIEW(dev))
+	else if (IS_CHERRYVIEW(dev_priv))
 		hdmi_val |= SDVO_PIPE_SEL_CHV(crtc->pipe);
 	else
 		hdmi_val |= SDVO_PIPE_SEL(crtc->pipe);
@@ -911,9 +911,9 @@ static bool intel_hdmi_get_hw_state(struct intel_encoder *encoder,
 	if (!(tmp & SDVO_ENABLE))
 		goto out;
 
-	if (HAS_PCH_CPT(dev))
+	if (HAS_PCH_CPT(dev_priv))
 		*pipe = PORT_TO_PIPE_CPT(tmp);
-	else if (IS_CHERRYVIEW(dev))
+	else if (IS_CHERRYVIEW(dev_priv))
 		*pipe = SDVO_PORT_TO_PIPE_CHV(tmp);
 	else
 		*pipe = PORT_TO_PIPE(tmp);
@@ -956,7 +956,7 @@ static void intel_hdmi_get_config(struct intel_encoder *encoder,
 	if (tmp & SDVO_AUDIO_ENABLE)
 		pipe_config->has_audio = true;
 
-	if (!HAS_PCH_SPLIT(dev) &&
+	if (!HAS_PCH_SPLIT(dev_priv) &&
 	    tmp & HDMI_COLOR_RANGE_16_235)
 		pipe_config->limited_color_range = true;
 
@@ -1141,7 +1141,7 @@ static void intel_disable_hdmi(struct intel_encoder *encoder,
 	 * to transcoder A after disabling it to allow the
 	 * matching DP port to be enabled on transcoder A.
 	 */
-	if (HAS_PCH_IBX(dev) && crtc->pipe == PIPE_B) {
+	if (HAS_PCH_IBX(dev_priv) && crtc->pipe == PIPE_B) {
 		/*
 		 * We get CPU/PCH FIFO underruns on the other pipe when
 		 * doing the workaround. Sweep them under the rug.
@@ -1241,7 +1241,7 @@ static enum drm_mode_status
 hdmi_port_clock_valid(struct intel_hdmi *hdmi,
 		      int clock, bool respect_downstream_limits)
 {
-	struct drm_device *dev = intel_hdmi_to_dev(hdmi);
+	struct drm_i915_private *dev_priv = to_i915(intel_hdmi_to_dev(hdmi));
 
 	if (clock < 25000)
 		return MODE_CLOCK_LOW;
@@ -1249,11 +1249,11 @@ hdmi_port_clock_valid(struct intel_hdmi *hdmi,
 		return MODE_CLOCK_HIGH;
 
 	/* BXT DPLL can't generate 223-240 MHz */
-	if (IS_BROXTON(dev) && clock > 223333 && clock < 240000)
+	if (IS_BROXTON(dev_priv) && clock > 223333 && clock < 240000)
 		return MODE_CLOCK_RANGE;
 
 	/* CHV DPLL can't generate 216-240 MHz */
-	if (IS_CHERRYVIEW(dev) && clock > 216000 && clock < 240000)
+	if (IS_CHERRYVIEW(dev_priv) && clock > 216000 && clock < 240000)
 		return MODE_CLOCK_RANGE;
 
 	return MODE_OK;
@@ -1265,6 +1265,7 @@ intel_hdmi_mode_valid(struct drm_connector *connector,
 {
 	struct intel_hdmi *hdmi = intel_attached_hdmi(connector);
 	struct drm_device *dev = intel_hdmi_to_dev(hdmi);
+	struct drm_i915_private *dev_priv = to_i915(dev);
 	enum drm_mode_status status;
 	int clock;
 	int max_dotclk = to_i915(connector->dev)->max_dotclk_freq;
@@ -1287,7 +1288,7 @@ intel_hdmi_mode_valid(struct drm_connector *connector,
 	status = hdmi_port_clock_valid(hdmi, clock, true);
 
 	/* if we can't do 8bpc we may still be able to do 12bpc */
-	if (!HAS_GMCH_DISPLAY(dev) && status != MODE_OK)
+	if (!HAS_GMCH_DISPLAY(dev_priv) && status != MODE_OK)
 		status = hdmi_port_clock_valid(hdmi, clock * 3 / 2, true);
 
 	return status;
@@ -1297,7 +1298,7 @@ static bool hdmi_12bpc_possible(struct intel_crtc_state *crtc_state)
 {
 	struct drm_device *dev = crtc_state->base.crtc->dev;
 
-	if (HAS_GMCH_DISPLAY(dev))
+	if (HAS_GMCH_DISPLAY(to_i915(dev)))
 		return false;
 
 	/*
@@ -1312,7 +1313,7 @@ bool intel_hdmi_compute_config(struct intel_encoder *encoder,
 			       struct drm_connector_state *conn_state)
 {
 	struct intel_hdmi *intel_hdmi = enc_to_intel_hdmi(&encoder->base);
-	struct drm_device *dev = encoder->base.dev;
+	struct drm_i915_private *dev_priv = to_i915(encoder->base.dev);
 	struct drm_display_mode *adjusted_mode = &pipe_config->base.adjusted_mode;
 	int clock_8bpc = pipe_config->base.adjusted_mode.crtc_clock;
 	int clock_12bpc = clock_8bpc * 3 / 2;
@@ -1339,7 +1340,7 @@ bool intel_hdmi_compute_config(struct intel_encoder *encoder,
 		clock_12bpc *= 2;
 	}
 
-	if (HAS_PCH_SPLIT(dev) && !HAS_DDI(dev))
+	if (HAS_PCH_SPLIT(dev_priv) && !HAS_DDI(dev_priv))
 		pipe_config->has_pch_encoder = true;
 
 	if (pipe_config->has_hdmi_sink && intel_hdmi->has_audio)
@@ -1799,6 +1800,50 @@ intel_hdmi_add_properties(struct intel_hdmi *intel_hdmi, struct drm_connector *c
 	intel_hdmi->aspect_ratio = HDMI_PICTURE_ASPECT_NONE;
 }
 
+static u8 intel_hdmi_ddc_pin(struct drm_i915_private *dev_priv,
+			     enum port port)
+{
+	const struct ddi_vbt_port_info *info =
+		&dev_priv->vbt.ddi_port_info[port];
+	u8 ddc_pin;
+
+	if (info->alternate_ddc_pin) {
+		DRM_DEBUG_KMS("Using DDC pin 0x%x for port %c (VBT)\n",
+			      info->alternate_ddc_pin, port_name(port));
+		return info->alternate_ddc_pin;
+	}
+
+	switch (port) {
+	case PORT_B:
+		if (IS_BROXTON(dev_priv))
+			ddc_pin = GMBUS_PIN_1_BXT;
+		else
+			ddc_pin = GMBUS_PIN_DPB;
+		break;
+	case PORT_C:
+		if (IS_BROXTON(dev_priv))
+			ddc_pin = GMBUS_PIN_2_BXT;
+		else
+			ddc_pin = GMBUS_PIN_DPC;
+		break;
+	case PORT_D:
+		if (IS_CHERRYVIEW(dev_priv))
+			ddc_pin = GMBUS_PIN_DPD_CHV;
+		else
+			ddc_pin = GMBUS_PIN_DPD;
+		break;
+	default:
+		MISSING_CASE(port);
+		ddc_pin = GMBUS_PIN_DPB;
+		break;
+	}
+
+	DRM_DEBUG_KMS("Using DDC pin 0x%x for port %c (platform default)\n",
+		      ddc_pin, port_name(port));
+
+	return ddc_pin;
+}
+
 void intel_hdmi_init_connector(struct intel_digital_port *intel_dig_port,
 			       struct intel_connector *intel_connector)
 {
@@ -1808,7 +1853,6 @@ void intel_hdmi_init_connector(struct intel_digital_port *intel_dig_port,
 	struct drm_device *dev = intel_encoder->base.dev;
 	struct drm_i915_private *dev_priv = to_i915(dev);
 	enum port port = intel_dig_port->port;
-	uint8_t alternate_ddc_pin;
 
 	DRM_DEBUG_KMS("Adding HDMI connector on port %c\n",
 		      port_name(port));
@@ -1826,12 +1870,10 @@ void intel_hdmi_init_connector(struct intel_digital_port *intel_dig_port,
 	connector->doublescan_allowed = 0;
 	connector->stereo_allowed = 1;
 
+	intel_hdmi->ddc_bus = intel_hdmi_ddc_pin(dev_priv, port);
+
 	switch (port) {
 	case PORT_B:
-		if (IS_BROXTON(dev_priv))
-			intel_hdmi->ddc_bus = GMBUS_PIN_1_BXT;
-		else
-			intel_hdmi->ddc_bus = GMBUS_PIN_DPB;
 		/*
 		 * On BXT A0/A1, sw needs to activate DDIA HPD logic and
 		 * interrupts to check the external panel connection.
@@ -1842,61 +1884,32 @@ void intel_hdmi_init_connector(struct intel_digital_port *intel_dig_port,
 			intel_encoder->hpd_pin = HPD_PORT_B;
 		break;
 	case PORT_C:
-		if (IS_BROXTON(dev_priv))
-			intel_hdmi->ddc_bus = GMBUS_PIN_2_BXT;
-		else
-			intel_hdmi->ddc_bus = GMBUS_PIN_DPC;
 		intel_encoder->hpd_pin = HPD_PORT_C;
 		break;
 	case PORT_D:
-		if (WARN_ON(IS_BROXTON(dev_priv)))
-			intel_hdmi->ddc_bus = GMBUS_PIN_DISABLED;
-		else if (IS_CHERRYVIEW(dev_priv))
-			intel_hdmi->ddc_bus = GMBUS_PIN_DPD_CHV;
-		else
-			intel_hdmi->ddc_bus = GMBUS_PIN_DPD;
 		intel_encoder->hpd_pin = HPD_PORT_D;
 		break;
 	case PORT_E:
-		/* On SKL PORT E doesn't have seperate GMBUS pin
-		 *  We rely on VBT to set a proper alternate GMBUS pin. */
-		alternate_ddc_pin =
-			dev_priv->vbt.ddi_port_info[PORT_E].alternate_ddc_pin;
-		switch (alternate_ddc_pin) {
-		case DDC_PIN_B:
-			intel_hdmi->ddc_bus = GMBUS_PIN_DPB;
-			break;
-		case DDC_PIN_C:
-			intel_hdmi->ddc_bus = GMBUS_PIN_DPC;
-			break;
-		case DDC_PIN_D:
-			intel_hdmi->ddc_bus = GMBUS_PIN_DPD;
-			break;
-		default:
-			MISSING_CASE(alternate_ddc_pin);
-		}
 		intel_encoder->hpd_pin = HPD_PORT_E;
 		break;
-	case PORT_A:
-		intel_encoder->hpd_pin = HPD_PORT_A;
-		/* Internal port only for eDP. */
 	default:
-		BUG();
+		MISSING_CASE(port);
+		return;
 	}
 
-	if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) {
+	if (IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv)) {
 		intel_hdmi->write_infoframe = vlv_write_infoframe;
 		intel_hdmi->set_infoframes = vlv_set_infoframes;
 		intel_hdmi->infoframe_enabled = vlv_infoframe_enabled;
-	} else if (IS_G4X(dev)) {
+	} else if (IS_G4X(dev_priv)) {
 		intel_hdmi->write_infoframe = g4x_write_infoframe;
 		intel_hdmi->set_infoframes = g4x_set_infoframes;
 		intel_hdmi->infoframe_enabled = g4x_infoframe_enabled;
-	} else if (HAS_DDI(dev)) {
+	} else if (HAS_DDI(dev_priv)) {
 		intel_hdmi->write_infoframe = hsw_write_infoframe;
 		intel_hdmi->set_infoframes = hsw_set_infoframes;
 		intel_hdmi->infoframe_enabled = hsw_infoframe_enabled;
-	} else if (HAS_PCH_IBX(dev)) {
+	} else if (HAS_PCH_IBX(dev_priv)) {
 		intel_hdmi->write_infoframe = ibx_write_infoframe;
 		intel_hdmi->set_infoframes = ibx_set_infoframes;
 		intel_hdmi->infoframe_enabled = ibx_infoframe_enabled;
@@ -1906,7 +1919,7 @@ void intel_hdmi_init_connector(struct intel_digital_port *intel_dig_port,
 		intel_hdmi->infoframe_enabled = cpt_infoframe_enabled;
 	}
 
-	if (HAS_DDI(dev))
+	if (HAS_DDI(dev_priv))
 		intel_connector->get_hw_state = intel_ddi_connector_get_hw_state;
 	else
 		intel_connector->get_hw_state = intel_connector_get_hw_state;
@@ -1920,7 +1933,7 @@ void intel_hdmi_init_connector(struct intel_digital_port *intel_dig_port,
 	 * 0xd.  Failure to do so will result in spurious interrupts being
 	 * generated on the port when a cable is not attached.
 	 */
-	if (IS_G4X(dev) && !IS_GM45(dev)) {
+	if (IS_G4X(dev_priv) && !IS_GM45(dev_priv)) {
 		u32 temp = I915_READ(PEG_BAND_GAP_DATA);
 		I915_WRITE(PEG_BAND_GAP_DATA, (temp & ~0xf) | 0xd);
 	}
@@ -1929,6 +1942,7 @@ void intel_hdmi_init_connector(struct intel_digital_port *intel_dig_port,
 void intel_hdmi_init(struct drm_device *dev,
 		     i915_reg_t hdmi_reg, enum port port)
 {
+	struct drm_i915_private *dev_priv = to_i915(dev);
 	struct intel_digital_port *intel_dig_port;
 	struct intel_encoder *intel_encoder;
 	struct intel_connector *intel_connector;
@@ -1949,7 +1963,7 @@ void intel_hdmi_init(struct drm_device *dev,
 			 DRM_MODE_ENCODER_TMDS, "HDMI %c", port_name(port));
 
 	intel_encoder->compute_config = intel_hdmi_compute_config;
-	if (HAS_PCH_SPLIT(dev)) {
+	if (HAS_PCH_SPLIT(dev_priv)) {
 		intel_encoder->disable = pch_disable_hdmi;
 		intel_encoder->post_disable = pch_post_disable_hdmi;
 	} else {
@@ -1957,29 +1971,30 @@ void intel_hdmi_init(struct drm_device *dev,
 	}
 	intel_encoder->get_hw_state = intel_hdmi_get_hw_state;
 	intel_encoder->get_config = intel_hdmi_get_config;
-	if (IS_CHERRYVIEW(dev)) {
+	if (IS_CHERRYVIEW(dev_priv)) {
 		intel_encoder->pre_pll_enable = chv_hdmi_pre_pll_enable;
 		intel_encoder->pre_enable = chv_hdmi_pre_enable;
 		intel_encoder->enable = vlv_enable_hdmi;
 		intel_encoder->post_disable = chv_hdmi_post_disable;
 		intel_encoder->post_pll_disable = chv_hdmi_post_pll_disable;
-	} else if (IS_VALLEYVIEW(dev)) {
+	} else if (IS_VALLEYVIEW(dev_priv)) {
 		intel_encoder->pre_pll_enable = vlv_hdmi_pre_pll_enable;
 		intel_encoder->pre_enable = vlv_hdmi_pre_enable;
 		intel_encoder->enable = vlv_enable_hdmi;
 		intel_encoder->post_disable = vlv_hdmi_post_disable;
 	} else {
 		intel_encoder->pre_enable = intel_hdmi_pre_enable;
-		if (HAS_PCH_CPT(dev))
+		if (HAS_PCH_CPT(dev_priv))
 			intel_encoder->enable = cpt_enable_hdmi;
-		else if (HAS_PCH_IBX(dev))
+		else if (HAS_PCH_IBX(dev_priv))
 			intel_encoder->enable = ibx_enable_hdmi;
 		else
 			intel_encoder->enable = g4x_enable_hdmi;
 	}
 
 	intel_encoder->type = INTEL_OUTPUT_HDMI;
-	if (IS_CHERRYVIEW(dev)) {
+	intel_encoder->port = port;
+	if (IS_CHERRYVIEW(dev_priv)) {
 		if (port == PORT_D)
 			intel_encoder->crtc_mask = 1 << 2;
 		else
@@ -1993,7 +2008,7 @@ void intel_hdmi_init(struct drm_device *dev,
 	 * to work on real hardware. And since g4x can send infoframes to
 	 * only one port anyway, nothing is lost by allowing it.
 	 */
-	if (IS_G4X(dev))
+	if (IS_G4X(dev_priv))
 		intel_encoder->cloneable |= 1 << INTEL_OUTPUT_HDMI;
 
 	intel_dig_port->port = port;
diff --git a/drivers/gpu/drm/i915/intel_i2c.c b/drivers/gpu/drm/i915/intel_i2c.c
index 79aab9ad6faa..83f260bb4eef 100644
--- a/drivers/gpu/drm/i915/intel_i2c.c
+++ b/drivers/gpu/drm/i915/intel_i2c.c
@@ -138,11 +138,10 @@ static void intel_i2c_quirk_set(struct drm_i915_private *dev_priv, bool enable)
 static u32 get_reserved(struct intel_gmbus *bus)
 {
 	struct drm_i915_private *dev_priv = bus->dev_priv;
-	struct drm_device *dev = &dev_priv->drm;
 	u32 reserved = 0;
 
 	/* On most chips, these bits must be preserved in software. */
-	if (!IS_I830(dev) && !IS_845G(dev))
+	if (!IS_I830(dev_priv) && !IS_845G(dev_priv))
 		reserved = I915_READ_NOTRACE(bus->gpio_reg) &
 					     (GPIO_DATA_PULLUP_DISABLE |
 					      GPIO_CLOCK_PULLUP_DISABLE);
@@ -468,13 +467,9 @@ do_gmbus_xfer(struct i2c_adapter *adapter, struct i2c_msg *msgs, int num)
 					       struct intel_gmbus,
 					       adapter);
 	struct drm_i915_private *dev_priv = bus->dev_priv;
-	const unsigned int fw =
-		intel_uncore_forcewake_for_reg(dev_priv, GMBUS0,
-					       FW_REG_READ | FW_REG_WRITE);
 	int i = 0, inc, try = 0;
 	int ret = 0;
 
-	intel_uncore_forcewake_get(dev_priv, fw);
 retry:
 	I915_WRITE_FW(GMBUS0, bus->reg0);
 
@@ -576,7 +571,6 @@ timeout:
 	ret = -EAGAIN;
 
 out:
-	intel_uncore_forcewake_put(dev_priv, fw);
 	return ret;
 }
 
@@ -633,10 +627,10 @@ int intel_setup_gmbus(struct drm_device *dev)
 	unsigned int pin;
 	int ret;
 
-	if (HAS_PCH_NOP(dev))
+	if (HAS_PCH_NOP(dev_priv))
 		return 0;
 
-	if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev))
+	if (IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv))
 		dev_priv->gpio_mmio_base = VLV_DISPLAY_BASE;
 	else if (!HAS_GMCH_DISPLAY(dev_priv))
 		dev_priv->gpio_mmio_base =
@@ -674,7 +668,7 @@ int intel_setup_gmbus(struct drm_device *dev)
 		bus->reg0 = pin | GMBUS_RATE_100KHZ;
 
 		/* gmbus seems to be broken on i830 */
-		if (IS_I830(dev))
+		if (IS_I830(dev_priv))
 			bus->force_bit = 1;
 
 		intel_gpio_setup(bus, pin);
diff --git a/drivers/gpu/drm/i915/intel_lrc.c b/drivers/gpu/drm/i915/intel_lrc.c
index 251143361f31..bc86585b9fbb 100644
--- a/drivers/gpu/drm/i915/intel_lrc.c
+++ b/drivers/gpu/drm/i915/intel_lrc.c
@@ -226,10 +226,16 @@ enum {
 /* Typical size of the average request (2 pipecontrols and a MI_BB) */
 #define EXECLISTS_REQUEST_SIZE 64 /* bytes */
 
+#define WA_TAIL_DWORDS 2
+
 static int execlists_context_deferred_alloc(struct i915_gem_context *ctx,
 					    struct intel_engine_cs *engine);
 static int intel_lr_context_pin(struct i915_gem_context *ctx,
 				struct intel_engine_cs *engine);
+static void execlists_init_reg_state(u32 *reg_state,
+				     struct i915_gem_context *ctx,
+				     struct intel_engine_cs *engine,
+				     struct intel_ring *ring);
 
 /**
  * intel_sanitize_enable_execlists() - sanitize i915.enable_execlists
@@ -269,8 +275,7 @@ logical_ring_init_platform_invariants(struct intel_engine_cs *engine)
 	struct drm_i915_private *dev_priv = engine->i915;
 
 	engine->disable_lite_restore_wa =
-		(IS_SKL_REVID(dev_priv, 0, SKL_REVID_B0) ||
-		 IS_BXT_REVID(dev_priv, 0, BXT_REVID_A1)) &&
+		IS_BXT_REVID(dev_priv, 0, BXT_REVID_A1) &&
 		(engine->id == VCS || engine->id == VCS2);
 
 	engine->ctx_desc_template = GEN8_CTX_VALID;
@@ -621,6 +626,10 @@ int intel_logical_ring_alloc_request_extras(struct drm_i915_gem_request *request
 
 	request->ring = ce->ring;
 
+	ret = intel_lr_context_pin(request->ctx, engine);
+	if (ret)
+		return ret;
+
 	if (i915.enable_guc_submission) {
 		/*
 		 * Check that the GuC has space for the request before
@@ -629,21 +638,17 @@ int intel_logical_ring_alloc_request_extras(struct drm_i915_gem_request *request
 		 */
 		ret = i915_guc_wq_reserve(request);
 		if (ret)
-			return ret;
+			goto err_unpin;
 	}
 
-	ret = intel_lr_context_pin(request->ctx, engine);
-	if (ret)
-		return ret;
-
 	ret = intel_ring_begin(request, 0);
 	if (ret)
-		goto err_unpin;
+		goto err_unreserve;
 
 	if (!ce->initialised) {
 		ret = engine->init_context(request);
 		if (ret)
-			goto err_unpin;
+			goto err_unreserve;
 
 		ce->initialised = true;
 	}
@@ -658,6 +663,9 @@ int intel_logical_ring_alloc_request_extras(struct drm_i915_gem_request *request
 	request->reserved_space -= EXECLISTS_REQUEST_SIZE;
 	return 0;
 
+err_unreserve:
+	if (i915.enable_guc_submission)
+		i915_guc_wq_unreserve(request);
 err_unpin:
 	intel_lr_context_unpin(request->ctx, engine);
 	return ret;
@@ -708,7 +716,6 @@ static int intel_lr_context_pin(struct i915_gem_context *ctx,
 {
 	struct intel_context *ce = &ctx->engine[engine->id];
 	void *vaddr;
-	u32 *lrc_reg_state;
 	int ret;
 
 	lockdep_assert_held(&ctx->i915->drm.struct_mutex);
@@ -727,17 +734,16 @@ static int intel_lr_context_pin(struct i915_gem_context *ctx,
 		goto unpin_vma;
 	}
 
-	lrc_reg_state = vaddr + LRC_STATE_PN * PAGE_SIZE;
-
 	ret = intel_ring_pin(ce->ring);
 	if (ret)
 		goto unpin_map;
 
 	intel_lr_context_descriptor_update(ctx, engine);
 
-	lrc_reg_state[CTX_RING_BUFFER_START+1] =
+	ce->lrc_reg_state = vaddr + LRC_STATE_PN * PAGE_SIZE;
+	ce->lrc_reg_state[CTX_RING_BUFFER_START+1] =
 		i915_ggtt_offset(ce->ring->vma);
-	ce->lrc_reg_state = lrc_reg_state;
+
 	ce->state->obj->dirty = true;
 
 	/* Invalidate GuC TLB. */
@@ -846,13 +852,12 @@ static inline int gen8_emit_flush_coherentl3_wa(struct intel_engine_cs *engine,
 	uint32_t l3sqc4_flush = (0x40400000 | GEN8_LQSC_FLUSH_COHERENT_LINES);
 
 	/*
-	 * WaDisableLSQCROPERFforOCL:skl,kbl
+	 * WaDisableLSQCROPERFforOCL:kbl
 	 * This WA is implemented in skl_init_clock_gating() but since
 	 * this batch updates GEN8_L3SQCREG4 with default value we need to
 	 * set this bit here to retain the WA during flush.
 	 */
-	if (IS_SKL_REVID(dev_priv, 0, SKL_REVID_E0) ||
-	    IS_KBL_REVID(dev_priv, 0, KBL_REVID_E0))
+	if (IS_KBL_REVID(dev_priv, 0, KBL_REVID_E0))
 		l3sqc4_flush |= GEN8_LQSC_RO_PERF_DIS;
 
 	wa_ctx_emit(batch, index, (MI_STORE_REGISTER_MEM_GEN8 |
@@ -995,9 +1000,8 @@ static int gen9_init_indirectctx_bb(struct intel_engine_cs *engine,
 	struct drm_i915_private *dev_priv = engine->i915;
 	uint32_t index = wa_ctx_start(wa_ctx, *offset, CACHELINE_DWORDS);
 
-	/* WaDisableCtxRestoreArbitration:skl,bxt */
-	if (IS_SKL_REVID(dev_priv, 0, SKL_REVID_D0) ||
-	    IS_BXT_REVID(dev_priv, 0, BXT_REVID_A1))
+	/* WaDisableCtxRestoreArbitration:bxt */
+	if (IS_BXT_REVID(dev_priv, 0, BXT_REVID_A1))
 		wa_ctx_emit(batch, index, MI_ARB_ON_OFF | MI_ARB_DISABLE);
 
 	/* WaFlushCoherentL3CacheLinesAtContextSwitch:skl,bxt */
@@ -1068,9 +1072,8 @@ static int gen9_init_perctx_bb(struct intel_engine_cs *engine,
 {
 	uint32_t index = wa_ctx_start(wa_ctx, *offset, CACHELINE_DWORDS);
 
-	/* WaSetDisablePixMaskCammingAndRhwoInCommonSliceChicken:skl,bxt */
-	if (IS_SKL_REVID(engine->i915, 0, SKL_REVID_B0) ||
-	    IS_BXT_REVID(engine->i915, 0, BXT_REVID_A1)) {
+	/* WaSetDisablePixMaskCammingAndRhwoInCommonSliceChicken:bxt */
+	if (IS_BXT_REVID(engine->i915, 0, BXT_REVID_A1)) {
 		wa_ctx_emit(batch, index, MI_LOAD_REGISTER_IMM(1));
 		wa_ctx_emit_reg(batch, index, GEN9_SLICE_COMMON_ECO_CHICKEN0);
 		wa_ctx_emit(batch, index,
@@ -1097,9 +1100,8 @@ static int gen9_init_perctx_bb(struct intel_engine_cs *engine,
 		wa_ctx_emit(batch, index, MI_NOOP);
 	}
 
-	/* WaDisableCtxRestoreArbitration:skl,bxt */
-	if (IS_SKL_REVID(engine->i915, 0, SKL_REVID_D0) ||
-	    IS_BXT_REVID(engine->i915, 0, BXT_REVID_A1))
+	/* WaDisableCtxRestoreArbitration:bxt */
+	if (IS_BXT_REVID(engine->i915, 0, BXT_REVID_A1))
 		wa_ctx_emit(batch, index, MI_ARB_ON_OFF | MI_ARB_ENABLE);
 
 	wa_ctx_emit(batch, index, MI_BATCH_BUFFER_END);
@@ -1231,7 +1233,7 @@ static int gen8_init_common_ring(struct intel_engine_cs *engine)
 
 	lrc_init_hws(engine);
 
-	intel_engine_reset_irq(engine);
+	intel_engine_reset_breadcrumbs(engine);
 
 	I915_WRITE(RING_HWSTAM(engine->mmio_base), 0xffffffff);
 
@@ -1243,8 +1245,12 @@ static int gen8_init_common_ring(struct intel_engine_cs *engine)
 
 	intel_engine_init_hangcheck(engine);
 
-	if (!execlists_elsp_idle(engine))
+	/* After a GPU reset, we may have requests to replay */
+	if (!execlists_elsp_idle(engine)) {
+		engine->execlist_port[0].count = 0;
+		engine->execlist_port[1].count = 0;
 		execlists_submit_ports(engine);
+	}
 
 	return 0;
 }
@@ -1289,8 +1295,21 @@ static void reset_common_ring(struct intel_engine_cs *engine,
 	struct execlist_port *port = engine->execlist_port;
 	struct intel_context *ce = &request->ctx->engine[engine->id];
 
+	/* We want a simple context + ring to execute the breadcrumb update.
+	 * We cannot rely on the context being intact across the GPU hang,
+	 * so clear it and rebuild just what we need for the breadcrumb.
+	 * All pending requests for this context will be zapped, and any
+	 * future request will be after userspace has had the opportunity
+	 * to recreate its own state.
+	 */
+	execlists_init_reg_state(ce->lrc_reg_state,
+				 request->ctx, engine, ce->ring);
+
 	/* Move the RING_HEAD onto the breadcrumb, past the hanging batch */
+	ce->lrc_reg_state[CTX_RING_BUFFER_START+1] =
+		i915_ggtt_offset(ce->ring->vma);
 	ce->lrc_reg_state[CTX_RING_HEAD+1] = request->postfix;
+
 	request->ring->head = request->postfix;
 	request->ring->last_retired_head = -1;
 	intel_ring_update_space(request->ring);
@@ -1306,10 +1325,10 @@ static void reset_common_ring(struct intel_engine_cs *engine,
 		memset(&port[1], 0, sizeof(port[1]));
 	}
 
-	/* CS is stopped, and we will resubmit both ports on resume */
 	GEM_BUG_ON(request->ctx != port[0].request->ctx);
-	port[0].count = 0;
-	port[1].count = 0;
+
+	/* Reset WaIdleLiteRestore:bdw,skl as well */
+	request->tail = request->wa_tail - WA_TAIL_DWORDS * sizeof(u32);
 }
 
 static int intel_logical_ring_emit_pdps(struct drm_i915_gem_request *req)
@@ -1547,7 +1566,6 @@ static void bxt_a_seqno_barrier(struct intel_engine_cs *engine)
  * used as a workaround for not being allowed to do lite
  * restore with HEAD==TAIL (WaIdleLiteRestore).
  */
-#define WA_TAIL_DWORDS 2
 
 static int gen8_emit_request(struct drm_i915_gem_request *request)
 {
@@ -1630,9 +1648,6 @@ void intel_logical_ring_cleanup(struct intel_engine_cs *engine)
 {
 	struct drm_i915_private *dev_priv;
 
-	if (!intel_engine_initialized(engine))
-		return;
-
 	/*
 	 * Tasklet cannot be active at this point due intel_mark_active/idle
 	 * so this is just for documentation.
@@ -1659,13 +1674,16 @@ void intel_logical_ring_cleanup(struct intel_engine_cs *engine)
 
 	lrc_destroy_wa_ctx_obj(engine);
 	engine->i915 = NULL;
+	dev_priv->engine[engine->id] = NULL;
+	kfree(engine);
 }
 
 void intel_execlists_enable_submission(struct drm_i915_private *dev_priv)
 {
 	struct intel_engine_cs *engine;
+	enum intel_engine_id id;
 
-	for_each_engine(engine, dev_priv)
+	for_each_engine(engine, dev_priv, id)
 		engine->submit_request = execlists_submit_request;
 }
 
@@ -1894,38 +1912,13 @@ static u32 intel_lr_indirect_ctx_offset(struct intel_engine_cs *engine)
 	return indirect_ctx_offset;
 }
 
-static int
-populate_lr_context(struct i915_gem_context *ctx,
-		    struct drm_i915_gem_object *ctx_obj,
-		    struct intel_engine_cs *engine,
-		    struct intel_ring *ring)
+static void execlists_init_reg_state(u32 *reg_state,
+				     struct i915_gem_context *ctx,
+				     struct intel_engine_cs *engine,
+				     struct intel_ring *ring)
 {
-	struct drm_i915_private *dev_priv = ctx->i915;
-	struct i915_hw_ppgtt *ppgtt = ctx->ppgtt;
-	void *vaddr;
-	u32 *reg_state;
-	int ret;
-
-	if (!ppgtt)
-		ppgtt = dev_priv->mm.aliasing_ppgtt;
-
-	ret = i915_gem_object_set_to_cpu_domain(ctx_obj, true);
-	if (ret) {
-		DRM_DEBUG_DRIVER("Could not set to CPU domain\n");
-		return ret;
-	}
-
-	vaddr = i915_gem_object_pin_map(ctx_obj, I915_MAP_WB);
-	if (IS_ERR(vaddr)) {
-		ret = PTR_ERR(vaddr);
-		DRM_DEBUG_DRIVER("Could not map object pages! (%d)\n", ret);
-		return ret;
-	}
-	ctx_obj->dirty = true;
-
-	/* The second page of the context object contains some fields which must
-	 * be set up prior to the first execution. */
-	reg_state = vaddr + LRC_STATE_PN * PAGE_SIZE;
+	struct drm_i915_private *dev_priv = engine->i915;
+	struct i915_hw_ppgtt *ppgtt = ctx->ppgtt ?: dev_priv->mm.aliasing_ppgtt;
 
 	/* A context is actually a big batch buffer with several MI_LOAD_REGISTER_IMM
 	 * commands followed by (reg, value) pairs. The values we are setting here are
@@ -1939,19 +1932,16 @@ populate_lr_context(struct i915_gem_context *ctx,
 		       _MASKED_BIT_ENABLE(CTX_CTRL_INHIBIT_SYN_CTX_SWITCH |
 					  CTX_CTRL_ENGINE_CTX_RESTORE_INHIBIT |
 					  (HAS_RESOURCE_STREAMER(dev_priv) ?
-					    CTX_CTRL_RS_CTX_ENABLE : 0)));
+					   CTX_CTRL_RS_CTX_ENABLE : 0)));
 	ASSIGN_CTX_REG(reg_state, CTX_RING_HEAD, RING_HEAD(engine->mmio_base),
 		       0);
 	ASSIGN_CTX_REG(reg_state, CTX_RING_TAIL, RING_TAIL(engine->mmio_base),
 		       0);
-	/* Ring buffer start address is not known until the buffer is pinned.
-	 * It is written to the context image in execlists_update_context()
-	 */
 	ASSIGN_CTX_REG(reg_state, CTX_RING_BUFFER_START,
 		       RING_START(engine->mmio_base), 0);
 	ASSIGN_CTX_REG(reg_state, CTX_RING_BUFFER_CONTROL,
 		       RING_CTL(engine->mmio_base),
-		       ((ring->size - PAGE_SIZE) & RING_NR_PAGES) | RING_VALID);
+		       RING_CTL_SIZE(ring->size) | RING_VALID);
 	ASSIGN_CTX_REG(reg_state, CTX_BB_HEAD_U,
 		       RING_BBADDR_UDW(engine->mmio_base), 0);
 	ASSIGN_CTX_REG(reg_state, CTX_BB_HEAD_L,
@@ -2029,6 +2019,36 @@ populate_lr_context(struct i915_gem_context *ctx,
 		ASSIGN_CTX_REG(reg_state, CTX_R_PWR_CLK_STATE, GEN8_R_PWR_CLK_STATE,
 			       make_rpcs(dev_priv));
 	}
+}
+
+static int
+populate_lr_context(struct i915_gem_context *ctx,
+		    struct drm_i915_gem_object *ctx_obj,
+		    struct intel_engine_cs *engine,
+		    struct intel_ring *ring)
+{
+	void *vaddr;
+	int ret;
+
+	ret = i915_gem_object_set_to_cpu_domain(ctx_obj, true);
+	if (ret) {
+		DRM_DEBUG_DRIVER("Could not set to CPU domain\n");
+		return ret;
+	}
+
+	vaddr = i915_gem_object_pin_map(ctx_obj, I915_MAP_WB);
+	if (IS_ERR(vaddr)) {
+		ret = PTR_ERR(vaddr);
+		DRM_DEBUG_DRIVER("Could not map object pages! (%d)\n", ret);
+		return ret;
+	}
+	ctx_obj->dirty = true;
+
+	/* The second page of the context object contains some fields which must
+	 * be set up prior to the first execution. */
+
+	execlists_init_reg_state(vaddr + LRC_STATE_PN * PAGE_SIZE,
+				 ctx, engine, ring);
 
 	i915_gem_object_unpin_map(ctx_obj);
 
@@ -2129,30 +2149,43 @@ error_deref_obj:
 
 void intel_lr_context_resume(struct drm_i915_private *dev_priv)
 {
-	struct i915_gem_context *ctx = dev_priv->kernel_context;
 	struct intel_engine_cs *engine;
+	struct i915_gem_context *ctx;
+	enum intel_engine_id id;
+
+	/* Because we emit WA_TAIL_DWORDS there may be a disparity
+	 * between our bookkeeping in ce->ring->head and ce->ring->tail and
+	 * that stored in context. As we only write new commands from
+	 * ce->ring->tail onwards, everything before that is junk. If the GPU
+	 * starts reading from its RING_HEAD from the context, it may try to
+	 * execute that junk and die.
+	 *
+	 * So to avoid that we reset the context images upon resume. For
+	 * simplicity, we just zero everything out.
+	 */
+	list_for_each_entry(ctx, &dev_priv->context_list, link) {
+		for_each_engine(engine, dev_priv, id) {
+			struct intel_context *ce = &ctx->engine[engine->id];
+			u32 *reg;
 
-	for_each_engine(engine, dev_priv) {
-		struct intel_context *ce = &ctx->engine[engine->id];
-		void *vaddr;
-		uint32_t *reg_state;
-
-		if (!ce->state)
-			continue;
-
-		vaddr = i915_gem_object_pin_map(ce->state->obj, I915_MAP_WB);
-		if (WARN_ON(IS_ERR(vaddr)))
-			continue;
+			if (!ce->state)
+				continue;
 
-		reg_state = vaddr + LRC_STATE_PN * PAGE_SIZE;
+			reg = i915_gem_object_pin_map(ce->state->obj,
+						      I915_MAP_WB);
+			if (WARN_ON(IS_ERR(reg)))
+				continue;
 
-		reg_state[CTX_RING_HEAD+1] = 0;
-		reg_state[CTX_RING_TAIL+1] = 0;
+			reg += LRC_STATE_PN * PAGE_SIZE / sizeof(*reg);
+			reg[CTX_RING_HEAD+1] = 0;
+			reg[CTX_RING_TAIL+1] = 0;
 
-		ce->state->obj->dirty = true;
-		i915_gem_object_unpin_map(ce->state->obj);
+			ce->state->obj->dirty = true;
+			i915_gem_object_unpin_map(ce->state->obj);
 
-		ce->ring->head = 0;
-		ce->ring->tail = 0;
+			ce->ring->head = ce->ring->tail = 0;
+			ce->ring->last_retired_head = -1;
+			intel_ring_update_space(ce->ring);
+		}
 	}
 }
diff --git a/drivers/gpu/drm/i915/intel_lspcon.c b/drivers/gpu/drm/i915/intel_lspcon.c
new file mode 100644
index 000000000000..632149c6b3ad
--- /dev/null
+++ b/drivers/gpu/drm/i915/intel_lspcon.c
@@ -0,0 +1,136 @@
+/*
+ * Copyright © 2016 Intel Corporation
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+ *
+ *
+ */
+#include <drm/drm_edid.h>
+#include <drm/drm_atomic_helper.h>
+#include <drm/drm_dp_dual_mode_helper.h>
+#include "intel_drv.h"
+
+static enum drm_lspcon_mode lspcon_get_current_mode(struct intel_lspcon *lspcon)
+{
+	enum drm_lspcon_mode current_mode = DRM_LSPCON_MODE_INVALID;
+	struct i2c_adapter *adapter = &lspcon->aux->ddc;
+
+	if (drm_lspcon_get_mode(adapter, &current_mode))
+		DRM_ERROR("Error reading LSPCON mode\n");
+	else
+		DRM_DEBUG_KMS("Current LSPCON mode %s\n",
+			current_mode == DRM_LSPCON_MODE_PCON ? "PCON" : "LS");
+	return current_mode;
+}
+
+static int lspcon_change_mode(struct intel_lspcon *lspcon,
+	enum drm_lspcon_mode mode, bool force)
+{
+	int err;
+	enum drm_lspcon_mode current_mode;
+	struct i2c_adapter *adapter = &lspcon->aux->ddc;
+
+	err = drm_lspcon_get_mode(adapter, &current_mode);
+	if (err) {
+		DRM_ERROR("Error reading LSPCON mode\n");
+		return err;
+	}
+
+	if (current_mode == mode) {
+		DRM_DEBUG_KMS("Current mode = desired LSPCON mode\n");
+		return 0;
+	}
+
+	err = drm_lspcon_set_mode(adapter, mode);
+	if (err < 0) {
+		DRM_ERROR("LSPCON mode change failed\n");
+		return err;
+	}
+
+	lspcon->mode = mode;
+	DRM_DEBUG_KMS("LSPCON mode changed done\n");
+	return 0;
+}
+
+static bool lspcon_probe(struct intel_lspcon *lspcon)
+{
+	enum drm_dp_dual_mode_type adaptor_type;
+	struct i2c_adapter *adapter = &lspcon->aux->ddc;
+
+	/* Lets probe the adaptor and check its type */
+	adaptor_type = drm_dp_dual_mode_detect(adapter);
+	if (adaptor_type != DRM_DP_DUAL_MODE_LSPCON) {
+		DRM_DEBUG_KMS("No LSPCON detected, found %s\n",
+			drm_dp_get_dual_mode_type_name(adaptor_type));
+		return false;
+	}
+
+	/* Yay ... got a LSPCON device */
+	DRM_DEBUG_KMS("LSPCON detected\n");
+	lspcon->mode = lspcon_get_current_mode(lspcon);
+	lspcon->active = true;
+	return true;
+}
+
+void lspcon_resume(struct intel_lspcon *lspcon)
+{
+	if (lspcon_change_mode(lspcon, DRM_LSPCON_MODE_PCON, true))
+		DRM_ERROR("LSPCON resume failed\n");
+	else
+		DRM_DEBUG_KMS("LSPCON resume success\n");
+}
+
+bool lspcon_init(struct intel_digital_port *intel_dig_port)
+{
+	struct intel_dp *dp = &intel_dig_port->dp;
+	struct intel_lspcon *lspcon = &intel_dig_port->lspcon;
+	struct drm_device *dev = intel_dig_port->base.base.dev;
+	struct drm_i915_private *dev_priv = to_i915(dev);
+
+	if (!IS_GEN9(dev_priv)) {
+		DRM_ERROR("LSPCON is supported on GEN9 only\n");
+		return false;
+	}
+
+	lspcon->active = false;
+	lspcon->mode = DRM_LSPCON_MODE_INVALID;
+	lspcon->aux = &dp->aux;
+
+	if (!lspcon_probe(lspcon)) {
+		DRM_ERROR("Failed to probe lspcon\n");
+		return false;
+	}
+
+	/*
+	* In the SW state machine, lets Put LSPCON in PCON mode only.
+	* In this way, it will work with both HDMI 1.4 sinks as well as HDMI
+	* 2.0 sinks.
+	*/
+	if (lspcon->active && lspcon->mode != DRM_LSPCON_MODE_PCON) {
+		if (lspcon_change_mode(lspcon, DRM_LSPCON_MODE_PCON,
+			true) < 0) {
+			DRM_ERROR("LSPCON mode change to PCON failed\n");
+			return false;
+		}
+	}
+
+	DRM_DEBUG_KMS("Success: LSPCON init\n");
+	return true;
+}
diff --git a/drivers/gpu/drm/i915/intel_lvds.c b/drivers/gpu/drm/i915/intel_lvds.c
index e1d47d51ea47..199b90c7907a 100644
--- a/drivers/gpu/drm/i915/intel_lvds.c
+++ b/drivers/gpu/drm/i915/intel_lvds.c
@@ -106,7 +106,7 @@ static bool intel_lvds_get_hw_state(struct intel_encoder *encoder,
 	if (!(tmp & LVDS_PORT_EN))
 		goto out;
 
-	if (HAS_PCH_CPT(dev))
+	if (HAS_PCH_CPT(dev_priv))
 		*pipe = PORT_TO_PIPE_CPT(tmp);
 	else
 		*pipe = PORT_TO_PIPE(tmp);
@@ -396,7 +396,7 @@ static bool intel_lvds_compute_config(struct intel_encoder *intel_encoder,
 				      struct intel_crtc_state *pipe_config,
 				      struct drm_connector_state *conn_state)
 {
-	struct drm_device *dev = intel_encoder->base.dev;
+	struct drm_i915_private *dev_priv = to_i915(intel_encoder->base.dev);
 	struct intel_lvds_encoder *lvds_encoder =
 		to_lvds_encoder(&intel_encoder->base);
 	struct intel_connector *intel_connector =
@@ -406,7 +406,7 @@ static bool intel_lvds_compute_config(struct intel_encoder *intel_encoder,
 	unsigned int lvds_bpp;
 
 	/* Should never happen!! */
-	if (INTEL_INFO(dev)->gen < 4 && intel_crtc->pipe == 0) {
+	if (INTEL_GEN(dev_priv) < 4 && intel_crtc->pipe == 0) {
 		DRM_ERROR("Can't support LVDS on pipe A\n");
 		return false;
 	}
@@ -431,7 +431,7 @@ static bool intel_lvds_compute_config(struct intel_encoder *intel_encoder,
 	intel_fixed_panel_mode(intel_connector->panel.fixed_mode,
 			       adjusted_mode);
 
-	if (HAS_PCH_SPLIT(dev)) {
+	if (HAS_PCH_SPLIT(dev_priv)) {
 		pipe_config->has_pch_encoder = true;
 
 		intel_pch_panel_fitting(intel_crtc, pipe_config,
@@ -566,7 +566,7 @@ static int intel_lid_notify(struct notifier_block *nb, unsigned long val,
 	 * and as part of the cleanup in the hw state restore we also redisable
 	 * the vga plane.
 	 */
-	if (!HAS_PCH_SPLIT(dev))
+	if (!HAS_PCH_SPLIT(dev_priv))
 		intel_display_resume(dev);
 
 	dev_priv->modeset_restore = MODESET_DONE;
@@ -949,16 +949,17 @@ static bool compute_is_dual_link_lvds(struct intel_lvds_encoder *lvds_encoder)
 	return (val & LVDS_CLKB_POWER_MASK) == LVDS_CLKB_POWER_UP;
 }
 
-static bool intel_lvds_supported(struct drm_device *dev)
+static bool intel_lvds_supported(struct drm_i915_private *dev_priv)
 {
 	/* With the introduction of the PCH we gained a dedicated
 	 * LVDS presence pin, use it. */
-	if (HAS_PCH_IBX(dev) || HAS_PCH_CPT(dev))
+	if (HAS_PCH_IBX(dev_priv) || HAS_PCH_CPT(dev_priv))
 		return true;
 
 	/* Otherwise LVDS was only attached to mobile products,
 	 * except for the inglorious 830gm */
-	if (INTEL_INFO(dev)->gen <= 4 && IS_MOBILE(dev) && !IS_I830(dev))
+	if (INTEL_GEN(dev_priv) <= 4 &&
+	    IS_MOBILE(dev_priv) && !IS_I830(dev_priv))
 		return true;
 
 	return false;
@@ -990,21 +991,21 @@ void intel_lvds_init(struct drm_device *dev)
 	int pipe;
 	u8 pin;
 
-	if (!intel_lvds_supported(dev))
+	if (!intel_lvds_supported(dev_priv))
 		return;
 
 	/* Skip init on machines we know falsely report LVDS */
 	if (dmi_check_system(intel_no_lvds))
 		return;
 
-	if (HAS_PCH_SPLIT(dev))
+	if (HAS_PCH_SPLIT(dev_priv))
 		lvds_reg = PCH_LVDS;
 	else
 		lvds_reg = LVDS;
 
 	lvds = I915_READ(lvds_reg);
 
-	if (HAS_PCH_SPLIT(dev)) {
+	if (HAS_PCH_SPLIT(dev_priv)) {
 		if ((lvds & LVDS_DETECTED) == 0)
 			return;
 		if (dev_priv->vbt.edp.support) {
@@ -1064,12 +1065,13 @@ void intel_lvds_init(struct drm_device *dev)
 	intel_connector->get_hw_state = intel_connector_get_hw_state;
 
 	intel_connector_attach_encoder(intel_connector, intel_encoder);
-	intel_encoder->type = INTEL_OUTPUT_LVDS;
 
+	intel_encoder->type = INTEL_OUTPUT_LVDS;
+	intel_encoder->port = PORT_NONE;
 	intel_encoder->cloneable = 0;
-	if (HAS_PCH_SPLIT(dev))
+	if (HAS_PCH_SPLIT(dev_priv))
 		intel_encoder->crtc_mask = (1 << 0) | (1 << 1) | (1 << 2);
-	else if (IS_GEN4(dev))
+	else if (IS_GEN4(dev_priv))
 		intel_encoder->crtc_mask = (1 << 0) | (1 << 1);
 	else
 		intel_encoder->crtc_mask = (1 << 1);
@@ -1157,7 +1159,7 @@ void intel_lvds_init(struct drm_device *dev)
 	 */
 
 	/* Ironlake: FIXME if still fail, not try pipe mode now */
-	if (HAS_PCH_SPLIT(dev))
+	if (HAS_PCH_SPLIT(dev_priv))
 		goto failed;
 
 	pipe = (lvds & LVDS_PIPEB_SELECT) ? 1 : 0;
diff --git a/drivers/gpu/drm/i915/intel_overlay.c b/drivers/gpu/drm/i915/intel_overlay.c
index a24bc8c7889f..25bcd4a178d3 100644
--- a/drivers/gpu/drm/i915/intel_overlay.c
+++ b/drivers/gpu/drm/i915/intel_overlay.c
@@ -233,7 +233,7 @@ static int intel_overlay_do_wait_request(struct intel_overlay *overlay,
 static struct drm_i915_gem_request *alloc_request(struct intel_overlay *overlay)
 {
 	struct drm_i915_private *dev_priv = overlay->i915;
-	struct intel_engine_cs *engine = &dev_priv->engine[RCS];
+	struct intel_engine_cs *engine = dev_priv->engine[RCS];
 
 	return i915_gem_request_alloc(engine, dev_priv->kernel_context);
 }
@@ -1470,6 +1470,8 @@ void intel_cleanup_overlay(struct drm_i915_private *dev_priv)
 	kfree(dev_priv->overlay);
 }
 
+#if IS_ENABLED(CONFIG_DRM_I915_CAPTURE_ERROR)
+
 struct intel_overlay_error_state {
 	struct overlay_registers regs;
 	unsigned long base;
@@ -1587,3 +1589,5 @@ intel_overlay_print_error_state(struct drm_i915_error_state_buf *m,
 	P(UVSCALEV);
 #undef P
 }
+
+#endif
diff --git a/drivers/gpu/drm/i915/intel_panel.c b/drivers/gpu/drm/i915/intel_panel.c
index c10e9b0405e8..be4b4d546fd9 100644
--- a/drivers/gpu/drm/i915/intel_panel.c
+++ b/drivers/gpu/drm/i915/intel_panel.c
@@ -841,7 +841,7 @@ static void lpt_enable_backlight(struct intel_connector *connector)
 {
 	struct drm_i915_private *dev_priv = to_i915(connector->base.dev);
 	struct intel_panel *panel = &connector->panel;
-	u32 pch_ctl1, pch_ctl2;
+	u32 pch_ctl1, pch_ctl2, schicken;
 
 	pch_ctl1 = I915_READ(BLC_PWM_PCH_CTL1);
 	if (pch_ctl1 & BLM_PCH_PWM_ENABLE) {
@@ -850,6 +850,22 @@ static void lpt_enable_backlight(struct intel_connector *connector)
 		I915_WRITE(BLC_PWM_PCH_CTL1, pch_ctl1);
 	}
 
+	if (HAS_PCH_LPT(dev_priv)) {
+		schicken = I915_READ(SOUTH_CHICKEN2);
+		if (panel->backlight.alternate_pwm_increment)
+			schicken |= LPT_PWM_GRANULARITY;
+		else
+			schicken &= ~LPT_PWM_GRANULARITY;
+		I915_WRITE(SOUTH_CHICKEN2, schicken);
+	} else {
+		schicken = I915_READ(SOUTH_CHICKEN1);
+		if (panel->backlight.alternate_pwm_increment)
+			schicken |= SPT_PWM_GRANULARITY;
+		else
+			schicken &= ~SPT_PWM_GRANULARITY;
+		I915_WRITE(SOUTH_CHICKEN1, schicken);
+	}
+
 	pch_ctl2 = panel->backlight.max << 16;
 	I915_WRITE(BLC_PWM_PCH_CTL2, pch_ctl2);
 
@@ -1242,10 +1258,10 @@ static u32 bxt_hz_to_pwm(struct intel_connector *connector, u32 pwm_freq_hz)
  */
 static u32 spt_hz_to_pwm(struct intel_connector *connector, u32 pwm_freq_hz)
 {
-	struct drm_i915_private *dev_priv = to_i915(connector->base.dev);
+	struct intel_panel *panel = &connector->panel;
 	u32 mul;
 
-	if (I915_READ(SOUTH_CHICKEN1) & SPT_PWM_GRANULARITY)
+	if (panel->backlight.alternate_pwm_increment)
 		mul = 128;
 	else
 		mul = 16;
@@ -1261,9 +1277,10 @@ static u32 spt_hz_to_pwm(struct intel_connector *connector, u32 pwm_freq_hz)
 static u32 lpt_hz_to_pwm(struct intel_connector *connector, u32 pwm_freq_hz)
 {
 	struct drm_i915_private *dev_priv = to_i915(connector->base.dev);
+	struct intel_panel *panel = &connector->panel;
 	u32 mul, clock;
 
-	if (I915_READ(SOUTH_CHICKEN2) & LPT_PWM_GRANULARITY)
+	if (panel->backlight.alternate_pwm_increment)
 		mul = 16;
 	else
 		mul = 128;
@@ -1414,6 +1431,13 @@ static int lpt_setup_backlight(struct intel_connector *connector, enum pipe unus
 	struct drm_i915_private *dev_priv = to_i915(connector->base.dev);
 	struct intel_panel *panel = &connector->panel;
 	u32 pch_ctl1, pch_ctl2, val;
+	bool alt;
+
+	if (HAS_PCH_LPT(dev_priv))
+		alt = I915_READ(SOUTH_CHICKEN2) & LPT_PWM_GRANULARITY;
+	else
+		alt = I915_READ(SOUTH_CHICKEN1) & SPT_PWM_GRANULARITY;
+	panel->backlight.alternate_pwm_increment = alt;
 
 	pch_ctl1 = I915_READ(BLC_PWM_PCH_CTL1);
 	panel->backlight.active_low_pwm = pch_ctl1 & BLM_PCH_POLARITY;
diff --git a/drivers/gpu/drm/i915/intel_pm.c b/drivers/gpu/drm/i915/intel_pm.c
index 1472400ddce3..560fc7af8267 100644
--- a/drivers/gpu/drm/i915/intel_pm.c
+++ b/drivers/gpu/drm/i915/intel_pm.c
@@ -252,8 +252,8 @@ static const struct cxsr_latency cxsr_latency_table[] = {
 	{0, 1, 400, 800, 6042, 36042, 6584, 36584},    /* DDR3-800 SC */
 };
 
-static const struct cxsr_latency *intel_get_cxsr_latency(int is_desktop,
-							 int is_ddr3,
+static const struct cxsr_latency *intel_get_cxsr_latency(bool is_desktop,
+							 bool is_ddr3,
 							 int fsb,
 							 int mem)
 {
@@ -322,11 +322,11 @@ void intel_set_memory_cxsr(struct drm_i915_private *dev_priv, bool enable)
 	struct drm_device *dev = &dev_priv->drm;
 	u32 val;
 
-	if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) {
+	if (IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv)) {
 		I915_WRITE(FW_BLC_SELF_VLV, enable ? FW_CSPWRDWNEN : 0);
 		POSTING_READ(FW_BLC_SELF_VLV);
 		dev_priv->wm.vlv.cxsr = enable;
-	} else if (IS_G4X(dev) || IS_CRESTLINE(dev)) {
+	} else if (IS_G4X(dev_priv) || IS_CRESTLINE(dev_priv)) {
 		I915_WRITE(FW_BLC_SELF, enable ? FW_BLC_SELF_EN : 0);
 		POSTING_READ(FW_BLC_SELF);
 	} else if (IS_PINEVIEW(dev)) {
@@ -334,12 +334,12 @@ void intel_set_memory_cxsr(struct drm_i915_private *dev_priv, bool enable)
 		val |= enable ? PINEVIEW_SELF_REFRESH_EN : 0;
 		I915_WRITE(DSPFW3, val);
 		POSTING_READ(DSPFW3);
-	} else if (IS_I945G(dev) || IS_I945GM(dev)) {
+	} else if (IS_I945G(dev_priv) || IS_I945GM(dev_priv)) {
 		val = enable ? _MASKED_BIT_ENABLE(FW_BLC_SELF_EN) :
 			       _MASKED_BIT_DISABLE(FW_BLC_SELF_EN);
 		I915_WRITE(FW_BLC_SELF, val);
 		POSTING_READ(FW_BLC_SELF);
-	} else if (IS_I915GM(dev)) {
+	} else if (IS_I915GM(dev_priv)) {
 		/*
 		 * FIXME can't find a bit like this for 915G, and
 		 * and yet it does have the related watermark in
@@ -648,8 +648,10 @@ static void pineview_update_wm(struct drm_crtc *unused_crtc)
 	u32 reg;
 	unsigned long wm;
 
-	latency = intel_get_cxsr_latency(IS_PINEVIEW_G(dev), dev_priv->is_ddr3,
-					 dev_priv->fsb_freq, dev_priv->mem_freq);
+	latency = intel_get_cxsr_latency(IS_PINEVIEW_G(dev_priv),
+					 dev_priv->is_ddr3,
+					 dev_priv->fsb_freq,
+					 dev_priv->mem_freq);
 	if (!latency) {
 		DRM_DEBUG_KMS("Unknown FSB/MEM found, disable CxSR\n");
 		intel_set_memory_cxsr(dev_priv, false);
@@ -775,13 +777,13 @@ static bool g4x_check_srwm(struct drm_device *dev,
 		      display_wm, cursor_wm);
 
 	if (display_wm > display->max_wm) {
-		DRM_DEBUG_KMS("display watermark is too large(%d/%ld), disabling\n",
+		DRM_DEBUG_KMS("display watermark is too large(%d/%u), disabling\n",
 			      display_wm, display->max_wm);
 		return false;
 	}
 
 	if (cursor_wm > cursor->max_wm) {
-		DRM_DEBUG_KMS("cursor watermark is too large(%d/%ld), disabling\n",
+		DRM_DEBUG_KMS("cursor watermark is too large(%d/%u), disabling\n",
 			      cursor_wm, cursor->max_wm);
 		return false;
 	}
@@ -1528,7 +1530,7 @@ static void i9xx_update_wm(struct drm_crtc *unused_crtc)
 
 	if (IS_I945GM(dev))
 		wm_info = &i945_wm_info;
-	else if (!IS_GEN2(dev))
+	else if (!IS_GEN2(dev_priv))
 		wm_info = &i915_wm_info;
 	else
 		wm_info = &i830_a_wm_info;
@@ -1538,7 +1540,7 @@ static void i9xx_update_wm(struct drm_crtc *unused_crtc)
 	if (intel_crtc_active(crtc)) {
 		const struct drm_display_mode *adjusted_mode;
 		int cpp = drm_format_plane_cpp(crtc->primary->state->fb->pixel_format, 0);
-		if (IS_GEN2(dev))
+		if (IS_GEN2(dev_priv))
 			cpp = 4;
 
 		adjusted_mode = &to_intel_crtc(crtc)->config->base.adjusted_mode;
@@ -1552,7 +1554,7 @@ static void i9xx_update_wm(struct drm_crtc *unused_crtc)
 			planea_wm = wm_info->max_wm;
 	}
 
-	if (IS_GEN2(dev))
+	if (IS_GEN2(dev_priv))
 		wm_info = &i830_bc_wm_info;
 
 	fifo_size = dev_priv->display.get_fifo_size(dev, 1);
@@ -1560,7 +1562,7 @@ static void i9xx_update_wm(struct drm_crtc *unused_crtc)
 	if (intel_crtc_active(crtc)) {
 		const struct drm_display_mode *adjusted_mode;
 		int cpp = drm_format_plane_cpp(crtc->primary->state->fb->pixel_format, 0);
-		if (IS_GEN2(dev))
+		if (IS_GEN2(dev_priv))
 			cpp = 4;
 
 		adjusted_mode = &to_intel_crtc(crtc)->config->base.adjusted_mode;
@@ -1579,7 +1581,7 @@ static void i9xx_update_wm(struct drm_crtc *unused_crtc)
 
 	DRM_DEBUG_KMS("FIFO watermarks - A: %d, B: %d\n", planea_wm, planeb_wm);
 
-	if (IS_I915GM(dev) && enabled) {
+	if (IS_I915GM(dev_priv) && enabled) {
 		struct drm_i915_gem_object *obj;
 
 		obj = intel_fb_obj(enabled->primary->state->fb);
@@ -1609,7 +1611,7 @@ static void i9xx_update_wm(struct drm_crtc *unused_crtc)
 		unsigned long line_time_us;
 		int entries;
 
-		if (IS_I915GM(dev) || IS_I945GM(dev))
+		if (IS_I915GM(dev_priv) || IS_I945GM(dev_priv))
 			cpp = 4;
 
 		line_time_us = max(htotal * 1000 / clock, 1);
@@ -1623,7 +1625,7 @@ static void i9xx_update_wm(struct drm_crtc *unused_crtc)
 		if (srwm < 0)
 			srwm = 1;
 
-		if (IS_I945G(dev) || IS_I945GM(dev))
+		if (IS_I945G(dev_priv) || IS_I945GM(dev_priv))
 			I915_WRITE(FW_BLC_SELF,
 				   FW_BLC_SELF_FIFO_MASK | (srwm & 0xff));
 		else
@@ -2080,10 +2082,10 @@ static void intel_read_wm_latency(struct drm_device *dev, uint16_t wm[8])
 {
 	struct drm_i915_private *dev_priv = to_i915(dev);
 
-	if (IS_GEN9(dev)) {
+	if (IS_GEN9(dev_priv)) {
 		uint32_t val;
 		int ret, i;
-		int level, max_level = ilk_wm_max_level(dev);
+		int level, max_level = ilk_wm_max_level(dev_priv);
 
 		/* read the first set of memory latencies[0:3] */
 		val = 0; /* data0 to be programmed to 0 for first set */
@@ -2127,33 +2129,35 @@ static void intel_read_wm_latency(struct drm_device *dev, uint16_t wm[8])
 				GEN9_MEM_LATENCY_LEVEL_MASK;
 
 		/*
+		 * If a level n (n > 1) has a 0us latency, all levels m (m >= n)
+		 * need to be disabled. We make sure to sanitize the values out
+		 * of the punit to satisfy this requirement.
+		 */
+		for (level = 1; level <= max_level; level++) {
+			if (wm[level] == 0) {
+				for (i = level + 1; i <= max_level; i++)
+					wm[i] = 0;
+				break;
+			}
+		}
+
+		/*
 		 * WaWmMemoryReadLatency:skl
 		 *
 		 * punit doesn't take into account the read latency so we need
-		 * to add 2us to the various latency levels we retrieve from
-		 * the punit.
-		 *   - W0 is a bit special in that it's the only level that
-		 *   can't be disabled if we want to have display working, so
-		 *   we always add 2us there.
-		 *   - For levels >=1, punit returns 0us latency when they are
-		 *   disabled, so we respect that and don't add 2us then
-		 *
-		 * Additionally, if a level n (n > 1) has a 0us latency, all
-		 * levels m (m >= n) need to be disabled. We make sure to
-		 * sanitize the values out of the punit to satisfy this
-		 * requirement.
+		 * to add 2us to the various latency levels we retrieve from the
+		 * punit when level 0 response data us 0us.
 		 */
-		wm[0] += 2;
-		for (level = 1; level <= max_level; level++)
-			if (wm[level] != 0)
+		if (wm[0] == 0) {
+			wm[0] += 2;
+			for (level = 1; level <= max_level; level++) {
+				if (wm[level] == 0)
+					break;
 				wm[level] += 2;
-			else {
-				for (i = level + 1; i <= max_level; i++)
-					wm[i] = 0;
-
-				break;
 			}
-	} else if (IS_HASWELL(dev) || IS_BROADWELL(dev)) {
+		}
+
+	} else if (IS_HASWELL(dev_priv) || IS_BROADWELL(dev_priv)) {
 		uint64_t sskpd = I915_READ64(MCH_SSKPD);
 
 		wm[0] = (sskpd >> 56) & 0xFF;
@@ -2180,42 +2184,44 @@ static void intel_read_wm_latency(struct drm_device *dev, uint16_t wm[8])
 	}
 }
 
-static void intel_fixup_spr_wm_latency(struct drm_device *dev, uint16_t wm[5])
+static void intel_fixup_spr_wm_latency(struct drm_i915_private *dev_priv,
+				       uint16_t wm[5])
 {
 	/* ILK sprite LP0 latency is 1300 ns */
-	if (IS_GEN5(dev))
+	if (IS_GEN5(dev_priv))
 		wm[0] = 13;
 }
 
-static void intel_fixup_cur_wm_latency(struct drm_device *dev, uint16_t wm[5])
+static void intel_fixup_cur_wm_latency(struct drm_i915_private *dev_priv,
+				       uint16_t wm[5])
 {
 	/* ILK cursor LP0 latency is 1300 ns */
-	if (IS_GEN5(dev))
+	if (IS_GEN5(dev_priv))
 		wm[0] = 13;
 
 	/* WaDoubleCursorLP3Latency:ivb */
-	if (IS_IVYBRIDGE(dev))
+	if (IS_IVYBRIDGE(dev_priv))
 		wm[3] *= 2;
 }
 
-int ilk_wm_max_level(const struct drm_device *dev)
+int ilk_wm_max_level(const struct drm_i915_private *dev_priv)
 {
 	/* how many WM levels are we expecting */
-	if (INTEL_INFO(dev)->gen >= 9)
+	if (INTEL_GEN(dev_priv) >= 9)
 		return 7;
-	else if (IS_HASWELL(dev) || IS_BROADWELL(dev))
+	else if (IS_HASWELL(dev_priv) || IS_BROADWELL(dev_priv))
 		return 4;
-	else if (INTEL_INFO(dev)->gen >= 6)
+	else if (INTEL_GEN(dev_priv) >= 6)
 		return 3;
 	else
 		return 2;
 }
 
-static void intel_print_wm_latency(struct drm_device *dev,
+static void intel_print_wm_latency(struct drm_i915_private *dev_priv,
 				   const char *name,
 				   const uint16_t wm[8])
 {
-	int level, max_level = ilk_wm_max_level(dev);
+	int level, max_level = ilk_wm_max_level(dev_priv);
 
 	for (level = 0; level <= max_level; level++) {
 		unsigned int latency = wm[level];
@@ -2230,7 +2236,7 @@ static void intel_print_wm_latency(struct drm_device *dev,
 		 * - latencies are in us on gen9.
 		 * - before then, WM1+ latency values are in 0.5us units
 		 */
-		if (IS_GEN9(dev))
+		if (IS_GEN9(dev_priv))
 			latency *= 10;
 		else if (level > 0)
 			latency *= 5;
@@ -2244,7 +2250,7 @@ static void intel_print_wm_latency(struct drm_device *dev,
 static bool ilk_increase_wm_latency(struct drm_i915_private *dev_priv,
 				    uint16_t wm[5], uint16_t min)
 {
-	int level, max_level = ilk_wm_max_level(&dev_priv->drm);
+	int level, max_level = ilk_wm_max_level(dev_priv);
 
 	if (wm[0] >= min)
 		return false;
@@ -2273,9 +2279,9 @@ static void snb_wm_latency_quirk(struct drm_device *dev)
 		return;
 
 	DRM_DEBUG_KMS("WM latency values increased to avoid potential underruns\n");
-	intel_print_wm_latency(dev, "Primary", dev_priv->wm.pri_latency);
-	intel_print_wm_latency(dev, "Sprite", dev_priv->wm.spr_latency);
-	intel_print_wm_latency(dev, "Cursor", dev_priv->wm.cur_latency);
+	intel_print_wm_latency(dev_priv, "Primary", dev_priv->wm.pri_latency);
+	intel_print_wm_latency(dev_priv, "Sprite", dev_priv->wm.spr_latency);
+	intel_print_wm_latency(dev_priv, "Cursor", dev_priv->wm.cur_latency);
 }
 
 static void ilk_setup_wm_latency(struct drm_device *dev)
@@ -2289,14 +2295,14 @@ static void ilk_setup_wm_latency(struct drm_device *dev)
 	memcpy(dev_priv->wm.cur_latency, dev_priv->wm.pri_latency,
 	       sizeof(dev_priv->wm.pri_latency));
 
-	intel_fixup_spr_wm_latency(dev, dev_priv->wm.spr_latency);
-	intel_fixup_cur_wm_latency(dev, dev_priv->wm.cur_latency);
+	intel_fixup_spr_wm_latency(dev_priv, dev_priv->wm.spr_latency);
+	intel_fixup_cur_wm_latency(dev_priv, dev_priv->wm.cur_latency);
 
-	intel_print_wm_latency(dev, "Primary", dev_priv->wm.pri_latency);
-	intel_print_wm_latency(dev, "Sprite", dev_priv->wm.spr_latency);
-	intel_print_wm_latency(dev, "Cursor", dev_priv->wm.cur_latency);
+	intel_print_wm_latency(dev_priv, "Primary", dev_priv->wm.pri_latency);
+	intel_print_wm_latency(dev_priv, "Sprite", dev_priv->wm.spr_latency);
+	intel_print_wm_latency(dev_priv, "Cursor", dev_priv->wm.cur_latency);
 
-	if (IS_GEN6(dev))
+	if (IS_GEN6(dev_priv))
 		snb_wm_latency_quirk(dev);
 }
 
@@ -2305,7 +2311,7 @@ static void skl_setup_wm_latency(struct drm_device *dev)
 	struct drm_i915_private *dev_priv = to_i915(dev);
 
 	intel_read_wm_latency(dev, dev_priv->wm.skl_latency);
-	intel_print_wm_latency(dev, "Gen9 Plane", dev_priv->wm.skl_latency);
+	intel_print_wm_latency(dev_priv, "Gen9 Plane", dev_priv->wm.skl_latency);
 }
 
 static bool ilk_validate_pipe_wm(struct drm_device *dev,
@@ -2343,7 +2349,7 @@ static int ilk_compute_pipe_wm(struct intel_crtc_state *cstate)
 	struct intel_plane_state *pristate = NULL;
 	struct intel_plane_state *sprstate = NULL;
 	struct intel_plane_state *curstate = NULL;
-	int level, max_level = ilk_wm_max_level(dev), usable_level;
+	int level, max_level = ilk_wm_max_level(dev_priv), usable_level;
 	struct ilk_wm_maximums max;
 
 	pipe_wm = &cstate->wm.ilk.optimal;
@@ -2388,7 +2394,7 @@ static int ilk_compute_pipe_wm(struct intel_crtc_state *cstate)
 	memset(&pipe_wm->wm, 0, sizeof(pipe_wm->wm));
 	pipe_wm->wm[0] = pipe_wm->raw_wm[0];
 
-	if (IS_HASWELL(dev) || IS_BROADWELL(dev))
+	if (IS_HASWELL(dev_priv) || IS_BROADWELL(dev_priv))
 		pipe_wm->linetime = hsw_compute_linetime_wm(cstate);
 
 	if (!ilk_validate_pipe_wm(dev, pipe_wm))
@@ -2430,7 +2436,7 @@ static int ilk_compute_intermediate_wm(struct drm_device *dev,
 {
 	struct intel_pipe_wm *a = &newstate->wm.ilk.intermediate;
 	struct intel_pipe_wm *b = &intel_crtc->wm.active.ilk;
-	int level, max_level = ilk_wm_max_level(dev);
+	int level, max_level = ilk_wm_max_level(to_i915(dev));
 
 	/*
 	 * Start with the final, target watermarks, then combine with the
@@ -2514,11 +2520,11 @@ static void ilk_wm_merge(struct drm_device *dev,
 			 struct intel_pipe_wm *merged)
 {
 	struct drm_i915_private *dev_priv = to_i915(dev);
-	int level, max_level = ilk_wm_max_level(dev);
+	int level, max_level = ilk_wm_max_level(dev_priv);
 	int last_enabled_level = max_level;
 
 	/* ILK/SNB/IVB: LP1+ watermarks only w/ single pipe */
-	if ((INTEL_INFO(dev)->gen <= 6 || IS_IVYBRIDGE(dev)) &&
+	if ((INTEL_GEN(dev_priv) <= 6 || IS_IVYBRIDGE(dev_priv)) &&
 	    config->num_pipes_active > 1)
 		last_enabled_level = 0;
 
@@ -2554,7 +2560,7 @@ static void ilk_wm_merge(struct drm_device *dev,
 	 * What we should check here is whether FBC can be
 	 * enabled sometime later.
 	 */
-	if (IS_GEN5(dev) && !merged->fbc_wm_enabled &&
+	if (IS_GEN5(dev_priv) && !merged->fbc_wm_enabled &&
 	    intel_fbc_is_active(dev_priv)) {
 		for (level = 2; level <= max_level; level++) {
 			struct intel_wm_level *wm = &merged->wm[level];
@@ -2575,7 +2581,7 @@ static unsigned int ilk_wm_lp_latency(struct drm_device *dev, int level)
 {
 	struct drm_i915_private *dev_priv = to_i915(dev);
 
-	if (IS_HASWELL(dev) || IS_BROADWELL(dev))
+	if (IS_HASWELL(dev_priv) || IS_BROADWELL(dev_priv))
 		return 2 * level;
 	else
 		return dev_priv->wm.pri_latency[level];
@@ -2654,7 +2660,7 @@ static struct intel_pipe_wm *ilk_find_best_result(struct drm_device *dev,
 						  struct intel_pipe_wm *r1,
 						  struct intel_pipe_wm *r2)
 {
-	int level, max_level = ilk_wm_max_level(dev);
+	int level, max_level = ilk_wm_max_level(to_i915(dev));
 	int level1 = 0, level2 = 0;
 
 	for (level = 1; level <= max_level; level++) {
@@ -2799,7 +2805,7 @@ static void ilk_write_wm_values(struct drm_i915_private *dev_priv,
 		I915_WRITE(PIPE_WM_LINETIME(PIPE_C), results->wm_linetime[2]);
 
 	if (dirty & WM_DIRTY_DDB) {
-		if (IS_HASWELL(dev) || IS_BROADWELL(dev)) {
+		if (IS_HASWELL(dev_priv) || IS_BROADWELL(dev_priv)) {
 			val = I915_READ(WM_MISC);
 			if (results->partitioning == INTEL_DDB_PART_1_2)
 				val &= ~WM_MISC_DATA_PARTITION_5_6;
@@ -2878,6 +2884,34 @@ skl_wm_plane_id(const struct intel_plane *plane)
 }
 
 /*
+ * FIXME: We still don't have the proper code detect if we need to apply the WA,
+ * so assume we'll always need it in order to avoid underruns.
+ */
+static bool skl_needs_memory_bw_wa(struct intel_atomic_state *state)
+{
+	struct drm_i915_private *dev_priv = to_i915(state->base.dev);
+
+	if (IS_SKYLAKE(dev_priv) || IS_BROXTON(dev_priv) ||
+	    IS_KABYLAKE(dev_priv))
+		return true;
+
+	return false;
+}
+
+static bool
+intel_has_sagv(struct drm_i915_private *dev_priv)
+{
+	if (IS_KABYLAKE(dev_priv))
+		return true;
+
+	if (IS_SKYLAKE(dev_priv) &&
+	    dev_priv->sagv_status != I915_SAGV_NOT_CONTROLLED)
+		return true;
+
+	return false;
+}
+
+/*
  * SAGV dynamically adjusts the system agent voltage and clock frequencies
  * depending on power and performance requirements. The display engine access
  * to system memory is blocked during the adjustment time. Because of the
@@ -2889,12 +2923,14 @@ skl_wm_plane_id(const struct intel_plane *plane)
  *  - We're not using an interlaced display configuration
  */
 int
-skl_enable_sagv(struct drm_i915_private *dev_priv)
+intel_enable_sagv(struct drm_i915_private *dev_priv)
 {
 	int ret;
 
-	if (dev_priv->skl_sagv_status == I915_SKL_SAGV_NOT_CONTROLLED ||
-	    dev_priv->skl_sagv_status == I915_SKL_SAGV_ENABLED)
+	if (!intel_has_sagv(dev_priv))
+		return 0;
+
+	if (dev_priv->sagv_status == I915_SAGV_ENABLED)
 		return 0;
 
 	DRM_DEBUG_KMS("Enabling the SAGV\n");
@@ -2910,21 +2946,21 @@ skl_enable_sagv(struct drm_i915_private *dev_priv)
 	 * Some skl systems, pre-release machines in particular,
 	 * don't actually have an SAGV.
 	 */
-	if (ret == -ENXIO) {
+	if (IS_SKYLAKE(dev_priv) && ret == -ENXIO) {
 		DRM_DEBUG_DRIVER("No SAGV found on system, ignoring\n");
-		dev_priv->skl_sagv_status = I915_SKL_SAGV_NOT_CONTROLLED;
+		dev_priv->sagv_status = I915_SAGV_NOT_CONTROLLED;
 		return 0;
 	} else if (ret < 0) {
 		DRM_ERROR("Failed to enable the SAGV\n");
 		return ret;
 	}
 
-	dev_priv->skl_sagv_status = I915_SKL_SAGV_ENABLED;
+	dev_priv->sagv_status = I915_SAGV_ENABLED;
 	return 0;
 }
 
 static int
-skl_do_sagv_disable(struct drm_i915_private *dev_priv)
+intel_do_sagv_disable(struct drm_i915_private *dev_priv)
 {
 	int ret;
 	uint32_t temp = GEN9_SAGV_DISABLE;
@@ -2938,19 +2974,21 @@ skl_do_sagv_disable(struct drm_i915_private *dev_priv)
 }
 
 int
-skl_disable_sagv(struct drm_i915_private *dev_priv)
+intel_disable_sagv(struct drm_i915_private *dev_priv)
 {
 	int ret, result;
 
-	if (dev_priv->skl_sagv_status == I915_SKL_SAGV_NOT_CONTROLLED ||
-	    dev_priv->skl_sagv_status == I915_SKL_SAGV_DISABLED)
+	if (!intel_has_sagv(dev_priv))
+		return 0;
+
+	if (dev_priv->sagv_status == I915_SAGV_DISABLED)
 		return 0;
 
 	DRM_DEBUG_KMS("Disabling the SAGV\n");
 	mutex_lock(&dev_priv->rps.hw_lock);
 
 	/* bspec says to keep retrying for at least 1 ms */
-	ret = wait_for(result = skl_do_sagv_disable(dev_priv), 1);
+	ret = wait_for(result = intel_do_sagv_disable(dev_priv), 1);
 	mutex_unlock(&dev_priv->rps.hw_lock);
 
 	if (ret == -ETIMEDOUT) {
@@ -2962,27 +3000,33 @@ skl_disable_sagv(struct drm_i915_private *dev_priv)
 	 * Some skl systems, pre-release machines in particular,
 	 * don't actually have an SAGV.
 	 */
-	if (result == -ENXIO) {
+	if (IS_SKYLAKE(dev_priv) && result == -ENXIO) {
 		DRM_DEBUG_DRIVER("No SAGV found on system, ignoring\n");
-		dev_priv->skl_sagv_status = I915_SKL_SAGV_NOT_CONTROLLED;
+		dev_priv->sagv_status = I915_SAGV_NOT_CONTROLLED;
 		return 0;
 	} else if (result < 0) {
 		DRM_ERROR("Failed to disable the SAGV\n");
 		return result;
 	}
 
-	dev_priv->skl_sagv_status = I915_SKL_SAGV_DISABLED;
+	dev_priv->sagv_status = I915_SAGV_DISABLED;
 	return 0;
 }
 
-bool skl_can_enable_sagv(struct drm_atomic_state *state)
+bool intel_can_enable_sagv(struct drm_atomic_state *state)
 {
 	struct drm_device *dev = state->dev;
 	struct drm_i915_private *dev_priv = to_i915(dev);
 	struct intel_atomic_state *intel_state = to_intel_atomic_state(state);
-	struct drm_crtc *crtc;
+	struct intel_crtc *crtc;
+	struct intel_plane *plane;
+	struct intel_crtc_state *cstate;
+	struct skl_plane_wm *wm;
 	enum pipe pipe;
-	int level, plane;
+	int level, latency;
+
+	if (!intel_has_sagv(dev_priv))
+		return false;
 
 	/*
 	 * SKL workaround: bspec recommends we disable the SAGV when we have
@@ -2997,27 +3041,37 @@ bool skl_can_enable_sagv(struct drm_atomic_state *state)
 
 	/* Since we're now guaranteed to only have one active CRTC... */
 	pipe = ffs(intel_state->active_crtcs) - 1;
-	crtc = dev_priv->pipe_to_crtc_mapping[pipe];
+	crtc = to_intel_crtc(dev_priv->pipe_to_crtc_mapping[pipe]);
+	cstate = to_intel_crtc_state(crtc->base.state);
 
-	if (crtc->state->mode.flags & DRM_MODE_FLAG_INTERLACE)
+	if (crtc->base.state->adjusted_mode.flags & DRM_MODE_FLAG_INTERLACE)
 		return false;
 
-	for_each_plane(dev_priv, pipe, plane) {
+	for_each_intel_plane_on_crtc(dev, crtc, plane) {
+		wm = &cstate->wm.skl.optimal.planes[skl_wm_plane_id(plane)];
+
 		/* Skip this plane if it's not enabled */
-		if (intel_state->wm_results.plane[pipe][plane][0] == 0)
+		if (!wm->wm[0].plane_en)
 			continue;
 
 		/* Find the highest enabled wm level for this plane */
-		for (level = ilk_wm_max_level(dev);
-		     intel_state->wm_results.plane[pipe][plane][level] == 0; --level)
+		for (level = ilk_wm_max_level(dev_priv);
+		     !wm->wm[level].plane_en; --level)
 		     { }
 
+		latency = dev_priv->wm.skl_latency[level];
+
+		if (skl_needs_memory_bw_wa(intel_state) &&
+		    plane->base.state->fb->modifier[0] ==
+		    I915_FORMAT_MOD_X_TILED)
+			latency += 15;
+
 		/*
 		 * If any of the planes on this pipe don't enable wm levels
 		 * that incur memory latencies higher then 30µs we can't enable
 		 * the SAGV
 		 */
-		if (dev_priv->wm.skl_latency[level] < SKL_SAGV_BLOCK_TIME)
+		if (latency < SKL_SAGV_BLOCK_TIME)
 			return false;
 	}
 
@@ -3036,7 +3090,6 @@ skl_ddb_get_pipe_allocation_limits(struct drm_device *dev,
 	struct drm_crtc *for_crtc = cstate->base.crtc;
 	unsigned int pipe_size, ddb_size;
 	int nth_active_pipe;
-	int pipe = to_intel_crtc(for_crtc)->pipe;
 
 	if (WARN_ON(!state) || !cstate->base.active) {
 		alloc->start = 0;
@@ -3064,7 +3117,7 @@ skl_ddb_get_pipe_allocation_limits(struct drm_device *dev,
 	 * we currently hold.
 	 */
 	if (!intel_state->active_pipe_changes) {
-		*alloc = dev_priv->wm.skl_hw.ddb.pipe[pipe];
+		*alloc = to_intel_crtc(for_crtc)->hw_ddb;
 		return;
 	}
 
@@ -3332,7 +3385,7 @@ skl_allocate_pipe_ddb(struct intel_crtc_state *cstate,
 	struct drm_plane *plane;
 	struct drm_plane_state *pstate;
 	enum pipe pipe = intel_crtc->pipe;
-	struct skl_ddb_entry *alloc = &ddb->pipe[pipe];
+	struct skl_ddb_entry *alloc = &cstate->wm.skl.ddb;
 	uint16_t alloc_size, start, cursor_blocks;
 	uint16_t *minimum = cstate->wm.skl.minimum_blocks;
 	uint16_t *y_minimum = cstate->wm.skl.minimum_y_blocks;
@@ -3340,13 +3393,15 @@ skl_allocate_pipe_ddb(struct intel_crtc_state *cstate,
 	int num_active;
 	int id, i;
 
+	/* Clear the partitioning for disabled planes. */
+	memset(ddb->plane[pipe], 0, sizeof(ddb->plane[pipe]));
+	memset(ddb->y_plane[pipe], 0, sizeof(ddb->y_plane[pipe]));
+
 	if (WARN_ON(!state))
 		return 0;
 
 	if (!cstate->base.active) {
-		ddb->pipe[pipe].start = ddb->pipe[pipe].end = 0;
-		memset(ddb->plane[pipe], 0, sizeof(ddb->plane[pipe]));
-		memset(ddb->y_plane[pipe], 0, sizeof(ddb->y_plane[pipe]));
+		alloc->start = alloc->end = 0;
 		return 0;
 	}
 
@@ -3446,12 +3501,6 @@ skl_allocate_pipe_ddb(struct intel_crtc_state *cstate,
 	return 0;
 }
 
-static uint32_t skl_pipe_pixel_rate(const struct intel_crtc_state *config)
-{
-	/* TODO: Take into account the scalers once we support them */
-	return config->base.adjusted_mode.crtc_clock;
-}
-
 /*
  * The max latency should be 257 (max the punit can code is 255 and we add 2us
  * for the read latency) and cpp should always be <= 8, so that
@@ -3472,29 +3521,14 @@ static uint32_t skl_wm_method1(uint32_t pixel_rate, uint8_t cpp, uint32_t latenc
 }
 
 static uint32_t skl_wm_method2(uint32_t pixel_rate, uint32_t pipe_htotal,
-			       uint32_t horiz_pixels, uint8_t cpp,
-			       uint64_t tiling, uint32_t latency)
+			       uint32_t latency, uint32_t plane_blocks_per_line)
 {
 	uint32_t ret;
-	uint32_t plane_bytes_per_line, plane_blocks_per_line;
 	uint32_t wm_intermediate_val;
 
 	if (latency == 0)
 		return UINT_MAX;
 
-	plane_bytes_per_line = horiz_pixels * cpp;
-
-	if (tiling == I915_FORMAT_MOD_Y_TILED ||
-	    tiling == I915_FORMAT_MOD_Yf_TILED) {
-		plane_bytes_per_line *= 4;
-		plane_blocks_per_line = DIV_ROUND_UP(plane_bytes_per_line, 512);
-		plane_blocks_per_line /= 4;
-	} else if (tiling == DRM_FORMAT_MOD_NONE) {
-		plane_blocks_per_line = DIV_ROUND_UP(plane_bytes_per_line, 512) + 1;
-	} else {
-		plane_blocks_per_line = DIV_ROUND_UP(plane_bytes_per_line, 512);
-	}
-
 	wm_intermediate_val = latency * pixel_rate;
 	ret = DIV_ROUND_UP(wm_intermediate_val, pipe_htotal * 1000) *
 				plane_blocks_per_line;
@@ -3517,7 +3551,7 @@ static uint32_t skl_adjusted_plane_pixel_rate(const struct intel_crtc_state *cst
 	 * Adjusted plane pixel rate is just the pipe's adjusted pixel rate
 	 * with additional adjustments for plane-specific scaling.
 	 */
-	adjusted_pixel_rate = skl_pipe_pixel_rate(cstate);
+	adjusted_pixel_rate = ilk_pipe_pixel_rate(cstate);
 	downscale_amount = skl_plane_downscale_amount(pstate);
 
 	pixel_rate = adjusted_pixel_rate * downscale_amount >> 16;
@@ -3545,12 +3579,19 @@ static int skl_compute_plane_wm(const struct drm_i915_private *dev_priv,
 	uint8_t cpp;
 	uint32_t width = 0, height = 0;
 	uint32_t plane_pixel_rate;
+	uint32_t y_tile_minimum, y_min_scanlines;
+	struct intel_atomic_state *state =
+		to_intel_atomic_state(cstate->base.state);
+	bool apply_memory_bw_wa = skl_needs_memory_bw_wa(state);
 
 	if (latency == 0 || !cstate->base.active || !intel_pstate->base.visible) {
 		*enabled = false;
 		return 0;
 	}
 
+	if (apply_memory_bw_wa && fb->modifier[0] == I915_FORMAT_MOD_X_TILED)
+		latency += 15;
+
 	width = drm_rect_width(&intel_pstate->base.src) >> 16;
 	height = drm_rect_height(&intel_pstate->base.src) >> 16;
 
@@ -3560,41 +3601,60 @@ static int skl_compute_plane_wm(const struct drm_i915_private *dev_priv,
 	cpp = drm_format_plane_cpp(fb->pixel_format, 0);
 	plane_pixel_rate = skl_adjusted_plane_pixel_rate(cstate, intel_pstate);
 
+	if (drm_rotation_90_or_270(pstate->rotation)) {
+		int cpp = (fb->pixel_format == DRM_FORMAT_NV12) ?
+			drm_format_plane_cpp(fb->pixel_format, 1) :
+			drm_format_plane_cpp(fb->pixel_format, 0);
+
+		switch (cpp) {
+		case 1:
+			y_min_scanlines = 16;
+			break;
+		case 2:
+			y_min_scanlines = 8;
+			break;
+		case 4:
+			y_min_scanlines = 4;
+			break;
+		default:
+			MISSING_CASE(cpp);
+			return -EINVAL;
+		}
+	} else {
+		y_min_scanlines = 4;
+	}
+
+	plane_bytes_per_line = width * cpp;
+	if (fb->modifier[0] == I915_FORMAT_MOD_Y_TILED ||
+	    fb->modifier[0] == I915_FORMAT_MOD_Yf_TILED) {
+		plane_blocks_per_line =
+		      DIV_ROUND_UP(plane_bytes_per_line * y_min_scanlines, 512);
+		plane_blocks_per_line /= y_min_scanlines;
+	} else if (fb->modifier[0] == DRM_FORMAT_MOD_NONE) {
+		plane_blocks_per_line = DIV_ROUND_UP(plane_bytes_per_line, 512)
+					+ 1;
+	} else {
+		plane_blocks_per_line = DIV_ROUND_UP(plane_bytes_per_line, 512);
+	}
+
 	method1 = skl_wm_method1(plane_pixel_rate, cpp, latency);
 	method2 = skl_wm_method2(plane_pixel_rate,
 				 cstate->base.adjusted_mode.crtc_htotal,
-				 width,
-				 cpp,
-				 fb->modifier[0],
-				 latency);
+				 latency,
+				 plane_blocks_per_line);
 
-	plane_bytes_per_line = width * cpp;
-	plane_blocks_per_line = DIV_ROUND_UP(plane_bytes_per_line, 512);
+	y_tile_minimum = plane_blocks_per_line * y_min_scanlines;
+	if (apply_memory_bw_wa)
+		y_tile_minimum *= 2;
 
 	if (fb->modifier[0] == I915_FORMAT_MOD_Y_TILED ||
 	    fb->modifier[0] == I915_FORMAT_MOD_Yf_TILED) {
-		uint32_t min_scanlines = 4;
-		uint32_t y_tile_minimum;
-		if (drm_rotation_90_or_270(pstate->rotation)) {
-			int cpp = (fb->pixel_format == DRM_FORMAT_NV12) ?
-				drm_format_plane_cpp(fb->pixel_format, 1) :
-				drm_format_plane_cpp(fb->pixel_format, 0);
-
-			switch (cpp) {
-			case 1:
-				min_scanlines = 16;
-				break;
-			case 2:
-				min_scanlines = 8;
-				break;
-			case 8:
-				WARN(1, "Unsupported pixel depth for rotation");
-			}
-		}
-		y_tile_minimum = plane_blocks_per_line * min_scanlines;
 		selected_result = max(method2, y_tile_minimum);
 	} else {
-		if ((ddb_allocation / plane_blocks_per_line) >= 1)
+		if ((cpp * cstate->base.adjusted_mode.crtc_htotal / 512 < 1) &&
+		    (plane_bytes_per_line / 512 < 1))
+			selected_result = method2;
+		else if ((ddb_allocation / plane_blocks_per_line) >= 1)
 			selected_result = min(method1, method2);
 		else
 			selected_result = method1;
@@ -3605,10 +3665,12 @@ static int skl_compute_plane_wm(const struct drm_i915_private *dev_priv,
 
 	if (level >= 1 && level <= 7) {
 		if (fb->modifier[0] == I915_FORMAT_MOD_Y_TILED ||
-		    fb->modifier[0] == I915_FORMAT_MOD_Yf_TILED)
-			res_lines += 4;
-		else
+		    fb->modifier[0] == I915_FORMAT_MOD_Yf_TILED) {
+			res_blocks += y_tile_minimum;
+			res_lines += y_min_scanlines;
+		} else {
 			res_blocks++;
+		}
 	}
 
 	if (res_blocks >= ddb_allocation || res_lines > 31) {
@@ -3642,67 +3704,52 @@ static int
 skl_compute_wm_level(const struct drm_i915_private *dev_priv,
 		     struct skl_ddb_allocation *ddb,
 		     struct intel_crtc_state *cstate,
+		     struct intel_plane *intel_plane,
 		     int level,
 		     struct skl_wm_level *result)
 {
 	struct drm_atomic_state *state = cstate->base.state;
 	struct intel_crtc *intel_crtc = to_intel_crtc(cstate->base.crtc);
-	struct drm_plane *plane;
-	struct intel_plane *intel_plane;
-	struct intel_plane_state *intel_pstate;
+	struct drm_plane *plane = &intel_plane->base;
+	struct intel_plane_state *intel_pstate = NULL;
 	uint16_t ddb_blocks;
 	enum pipe pipe = intel_crtc->pipe;
 	int ret;
+	int i = skl_wm_plane_id(intel_plane);
+
+	if (state)
+		intel_pstate =
+			intel_atomic_get_existing_plane_state(state,
+							      intel_plane);
 
 	/*
-	 * We'll only calculate watermarks for planes that are actually
-	 * enabled, so make sure all other planes are set as disabled.
+	 * Note: If we start supporting multiple pending atomic commits against
+	 * the same planes/CRTC's in the future, plane->state will no longer be
+	 * the correct pre-state to use for the calculations here and we'll
+	 * need to change where we get the 'unchanged' plane data from.
+	 *
+	 * For now this is fine because we only allow one queued commit against
+	 * a CRTC.  Even if the plane isn't modified by this transaction and we
+	 * don't have a plane lock, we still have the CRTC's lock, so we know
+	 * that no other transactions are racing with us to update it.
 	 */
-	memset(result, 0, sizeof(*result));
+	if (!intel_pstate)
+		intel_pstate = to_intel_plane_state(plane->state);
 
-	for_each_intel_plane_mask(&dev_priv->drm,
-				  intel_plane,
-				  cstate->base.plane_mask) {
-		int i = skl_wm_plane_id(intel_plane);
+	WARN_ON(!intel_pstate->base.fb);
 
-		plane = &intel_plane->base;
-		intel_pstate = NULL;
-		if (state)
-			intel_pstate =
-				intel_atomic_get_existing_plane_state(state,
-								      intel_plane);
+	ddb_blocks = skl_ddb_entry_size(&ddb->plane[pipe][i]);
 
-		/*
-		 * Note: If we start supporting multiple pending atomic commits
-		 * against the same planes/CRTC's in the future, plane->state
-		 * will no longer be the correct pre-state to use for the
-		 * calculations here and we'll need to change where we get the
-		 * 'unchanged' plane data from.
-		 *
-		 * For now this is fine because we only allow one queued commit
-		 * against a CRTC.  Even if the plane isn't modified by this
-		 * transaction and we don't have a plane lock, we still have
-		 * the CRTC's lock, so we know that no other transactions are
-		 * racing with us to update it.
-		 */
-		if (!intel_pstate)
-			intel_pstate = to_intel_plane_state(plane->state);
-
-		WARN_ON(!intel_pstate->base.fb);
-
-		ddb_blocks = skl_ddb_entry_size(&ddb->plane[pipe][i]);
-
-		ret = skl_compute_plane_wm(dev_priv,
-					   cstate,
-					   intel_pstate,
-					   ddb_blocks,
-					   level,
-					   &result->plane_res_b[i],
-					   &result->plane_res_l[i],
-					   &result->plane_en[i]);
-		if (ret)
-			return ret;
-	}
+	ret = skl_compute_plane_wm(dev_priv,
+				   cstate,
+				   intel_pstate,
+				   ddb_blocks,
+				   level,
+				   &result->plane_res_b,
+				   &result->plane_res_l,
+				   &result->plane_en);
+	if (ret)
+		return ret;
 
 	return 0;
 }
@@ -3710,32 +3757,28 @@ skl_compute_wm_level(const struct drm_i915_private *dev_priv,
 static uint32_t
 skl_compute_linetime_wm(struct intel_crtc_state *cstate)
 {
+	uint32_t pixel_rate;
+
 	if (!cstate->base.active)
 		return 0;
 
-	if (WARN_ON(skl_pipe_pixel_rate(cstate) == 0))
+	pixel_rate = ilk_pipe_pixel_rate(cstate);
+
+	if (WARN_ON(pixel_rate == 0))
 		return 0;
 
 	return DIV_ROUND_UP(8 * cstate->base.adjusted_mode.crtc_htotal * 1000,
-			    skl_pipe_pixel_rate(cstate));
+			    pixel_rate);
 }
 
 static void skl_compute_transition_wm(struct intel_crtc_state *cstate,
 				      struct skl_wm_level *trans_wm /* out */)
 {
-	struct drm_crtc *crtc = cstate->base.crtc;
-	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
-	struct intel_plane *intel_plane;
-
 	if (!cstate->base.active)
 		return;
 
 	/* Until we know more, just disable transition WMs */
-	for_each_intel_plane_on_crtc(crtc->dev, intel_crtc, intel_plane) {
-		int i = skl_wm_plane_id(intel_plane);
-
-		trans_wm->plane_en[i] = false;
-	}
+	trans_wm->plane_en = false;
 }
 
 static int skl_build_pipe_wm(struct intel_crtc_state *cstate,
@@ -3744,77 +3787,34 @@ static int skl_build_pipe_wm(struct intel_crtc_state *cstate,
 {
 	struct drm_device *dev = cstate->base.crtc->dev;
 	const struct drm_i915_private *dev_priv = to_i915(dev);
-	int level, max_level = ilk_wm_max_level(dev);
+	struct intel_plane *intel_plane;
+	struct skl_plane_wm *wm;
+	int level, max_level = ilk_wm_max_level(dev_priv);
 	int ret;
 
-	for (level = 0; level <= max_level; level++) {
-		ret = skl_compute_wm_level(dev_priv, ddb, cstate,
-					   level, &pipe_wm->wm[level]);
-		if (ret)
-			return ret;
-	}
-	pipe_wm->linetime = skl_compute_linetime_wm(cstate);
-
-	skl_compute_transition_wm(cstate, &pipe_wm->trans_wm);
-
-	return 0;
-}
-
-static void skl_compute_wm_results(struct drm_device *dev,
-				   struct skl_pipe_wm *p_wm,
-				   struct skl_wm_values *r,
-				   struct intel_crtc *intel_crtc)
-{
-	int level, max_level = ilk_wm_max_level(dev);
-	enum pipe pipe = intel_crtc->pipe;
-	uint32_t temp;
-	int i;
-
-	for (level = 0; level <= max_level; level++) {
-		for (i = 0; i < intel_num_planes(intel_crtc); i++) {
-			temp = 0;
-
-			temp |= p_wm->wm[level].plane_res_l[i] <<
-					PLANE_WM_LINES_SHIFT;
-			temp |= p_wm->wm[level].plane_res_b[i];
-			if (p_wm->wm[level].plane_en[i])
-				temp |= PLANE_WM_EN;
+	/*
+	 * We'll only calculate watermarks for planes that are actually
+	 * enabled, so make sure all other planes are set as disabled.
+	 */
+	memset(pipe_wm->planes, 0, sizeof(pipe_wm->planes));
 
-			r->plane[pipe][i][level] = temp;
+	for_each_intel_plane_mask(&dev_priv->drm,
+				  intel_plane,
+				  cstate->base.plane_mask) {
+		wm = &pipe_wm->planes[skl_wm_plane_id(intel_plane)];
+
+		for (level = 0; level <= max_level; level++) {
+			ret = skl_compute_wm_level(dev_priv, ddb, cstate,
+						   intel_plane, level,
+						   &wm->wm[level]);
+			if (ret)
+				return ret;
 		}
-
-		temp = 0;
-
-		temp |= p_wm->wm[level].plane_res_l[PLANE_CURSOR] << PLANE_WM_LINES_SHIFT;
-		temp |= p_wm->wm[level].plane_res_b[PLANE_CURSOR];
-
-		if (p_wm->wm[level].plane_en[PLANE_CURSOR])
-			temp |= PLANE_WM_EN;
-
-		r->plane[pipe][PLANE_CURSOR][level] = temp;
-
-	}
-
-	/* transition WMs */
-	for (i = 0; i < intel_num_planes(intel_crtc); i++) {
-		temp = 0;
-		temp |= p_wm->trans_wm.plane_res_l[i] << PLANE_WM_LINES_SHIFT;
-		temp |= p_wm->trans_wm.plane_res_b[i];
-		if (p_wm->trans_wm.plane_en[i])
-			temp |= PLANE_WM_EN;
-
-		r->plane_trans[pipe][i] = temp;
+		skl_compute_transition_wm(cstate, &wm->trans_wm);
 	}
+	pipe_wm->linetime = skl_compute_linetime_wm(cstate);
 
-	temp = 0;
-	temp |= p_wm->trans_wm.plane_res_l[PLANE_CURSOR] << PLANE_WM_LINES_SHIFT;
-	temp |= p_wm->trans_wm.plane_res_b[PLANE_CURSOR];
-	if (p_wm->trans_wm.plane_en[PLANE_CURSOR])
-		temp |= PLANE_WM_EN;
-
-	r->plane_trans[pipe][PLANE_CURSOR] = temp;
-
-	r->wm_linetime[pipe] = p_wm->linetime;
+	return 0;
 }
 
 static void skl_ddb_entry_write(struct drm_i915_private *dev_priv,
@@ -3827,53 +3827,77 @@ static void skl_ddb_entry_write(struct drm_i915_private *dev_priv,
 		I915_WRITE(reg, 0);
 }
 
+static void skl_write_wm_level(struct drm_i915_private *dev_priv,
+			       i915_reg_t reg,
+			       const struct skl_wm_level *level)
+{
+	uint32_t val = 0;
+
+	if (level->plane_en) {
+		val |= PLANE_WM_EN;
+		val |= level->plane_res_b;
+		val |= level->plane_res_l << PLANE_WM_LINES_SHIFT;
+	}
+
+	I915_WRITE(reg, val);
+}
+
 void skl_write_plane_wm(struct intel_crtc *intel_crtc,
-			const struct skl_wm_values *wm,
+			const struct skl_plane_wm *wm,
+			const struct skl_ddb_allocation *ddb,
 			int plane)
 {
 	struct drm_crtc *crtc = &intel_crtc->base;
 	struct drm_device *dev = crtc->dev;
 	struct drm_i915_private *dev_priv = to_i915(dev);
-	int level, max_level = ilk_wm_max_level(dev);
+	int level, max_level = ilk_wm_max_level(dev_priv);
 	enum pipe pipe = intel_crtc->pipe;
 
 	for (level = 0; level <= max_level; level++) {
-		I915_WRITE(PLANE_WM(pipe, plane, level),
-			   wm->plane[pipe][plane][level]);
+		skl_write_wm_level(dev_priv, PLANE_WM(pipe, plane, level),
+				   &wm->wm[level]);
 	}
-	I915_WRITE(PLANE_WM_TRANS(pipe, plane), wm->plane_trans[pipe][plane]);
+	skl_write_wm_level(dev_priv, PLANE_WM_TRANS(pipe, plane),
+			   &wm->trans_wm);
 
 	skl_ddb_entry_write(dev_priv, PLANE_BUF_CFG(pipe, plane),
-			    &wm->ddb.plane[pipe][plane]);
+			    &ddb->plane[pipe][plane]);
 	skl_ddb_entry_write(dev_priv, PLANE_NV12_BUF_CFG(pipe, plane),
-			    &wm->ddb.y_plane[pipe][plane]);
+			    &ddb->y_plane[pipe][plane]);
 }
 
 void skl_write_cursor_wm(struct intel_crtc *intel_crtc,
-			 const struct skl_wm_values *wm)
+			 const struct skl_plane_wm *wm,
+			 const struct skl_ddb_allocation *ddb)
 {
 	struct drm_crtc *crtc = &intel_crtc->base;
 	struct drm_device *dev = crtc->dev;
 	struct drm_i915_private *dev_priv = to_i915(dev);
-	int level, max_level = ilk_wm_max_level(dev);
+	int level, max_level = ilk_wm_max_level(dev_priv);
 	enum pipe pipe = intel_crtc->pipe;
 
 	for (level = 0; level <= max_level; level++) {
-		I915_WRITE(CUR_WM(pipe, level),
-			   wm->plane[pipe][PLANE_CURSOR][level]);
+		skl_write_wm_level(dev_priv, CUR_WM(pipe, level),
+				   &wm->wm[level]);
 	}
-	I915_WRITE(CUR_WM_TRANS(pipe), wm->plane_trans[pipe][PLANE_CURSOR]);
+	skl_write_wm_level(dev_priv, CUR_WM_TRANS(pipe), &wm->trans_wm);
 
 	skl_ddb_entry_write(dev_priv, CUR_BUF_CFG(pipe),
-			    &wm->ddb.plane[pipe][PLANE_CURSOR]);
+			    &ddb->plane[pipe][PLANE_CURSOR]);
 }
 
-bool skl_ddb_allocation_equals(const struct skl_ddb_allocation *old,
-			       const struct skl_ddb_allocation *new,
-			       enum pipe pipe)
+bool skl_wm_level_equals(const struct skl_wm_level *l1,
+			 const struct skl_wm_level *l2)
 {
-	return new->pipe[pipe].start == old->pipe[pipe].start &&
-	       new->pipe[pipe].end == old->pipe[pipe].end;
+	if (l1->plane_en != l2->plane_en)
+		return false;
+
+	/* If both planes aren't enabled, the rest shouldn't matter */
+	if (!l1->plane_en)
+		return true;
+
+	return (l1->plane_res_l == l2->plane_res_l &&
+		l1->plane_res_b == l2->plane_res_b);
 }
 
 static inline bool skl_ddb_entries_overlap(const struct skl_ddb_entry *a,
@@ -3883,22 +3907,22 @@ static inline bool skl_ddb_entries_overlap(const struct skl_ddb_entry *a,
 }
 
 bool skl_ddb_allocation_overlaps(struct drm_atomic_state *state,
-				 const struct skl_ddb_allocation *old,
-				 const struct skl_ddb_allocation *new,
-				 enum pipe pipe)
+				 struct intel_crtc *intel_crtc)
 {
-	struct drm_device *dev = state->dev;
-	struct intel_crtc *intel_crtc;
-	enum pipe otherp;
+	struct drm_crtc *other_crtc;
+	struct drm_crtc_state *other_cstate;
+	struct intel_crtc *other_intel_crtc;
+	const struct skl_ddb_entry *ddb =
+		&to_intel_crtc_state(intel_crtc->base.state)->wm.skl.ddb;
+	int i;
 
-	for_each_intel_crtc(dev, intel_crtc) {
-		otherp = intel_crtc->pipe;
+	for_each_crtc_in_state(state, other_crtc, other_cstate, i) {
+		other_intel_crtc = to_intel_crtc(other_crtc);
 
-		if (otherp == pipe)
+		if (other_intel_crtc == intel_crtc)
 			continue;
 
-		if (skl_ddb_entries_overlap(&new->pipe[pipe],
-					    &old->pipe[otherp]))
+		if (skl_ddb_entries_overlap(ddb, &other_intel_crtc->hw_ddb))
 			return true;
 	}
 
@@ -3940,6 +3964,41 @@ pipes_modified(struct drm_atomic_state *state)
 }
 
 static int
+skl_ddb_add_affected_planes(struct intel_crtc_state *cstate)
+{
+	struct drm_atomic_state *state = cstate->base.state;
+	struct drm_device *dev = state->dev;
+	struct drm_crtc *crtc = cstate->base.crtc;
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	struct drm_i915_private *dev_priv = to_i915(dev);
+	struct intel_atomic_state *intel_state = to_intel_atomic_state(state);
+	struct skl_ddb_allocation *new_ddb = &intel_state->wm_results.ddb;
+	struct skl_ddb_allocation *cur_ddb = &dev_priv->wm.skl_hw.ddb;
+	struct drm_plane_state *plane_state;
+	struct drm_plane *plane;
+	enum pipe pipe = intel_crtc->pipe;
+	int id;
+
+	WARN_ON(!drm_atomic_get_existing_crtc_state(state, crtc));
+
+	drm_for_each_plane_mask(plane, dev, crtc->state->plane_mask) {
+		id = skl_wm_plane_id(to_intel_plane(plane));
+
+		if (skl_ddb_entry_equal(&cur_ddb->plane[pipe][id],
+					&new_ddb->plane[pipe][id]) &&
+		    skl_ddb_entry_equal(&cur_ddb->y_plane[pipe][id],
+					&new_ddb->y_plane[pipe][id]))
+			continue;
+
+		plane_state = drm_atomic_get_plane_state(state, plane);
+		if (IS_ERR(plane_state))
+			return PTR_ERR(plane_state);
+	}
+
+	return 0;
+}
+
+static int
 skl_compute_ddb(struct drm_atomic_state *state)
 {
 	struct drm_device *dev = state->dev;
@@ -3992,6 +4051,12 @@ skl_compute_ddb(struct drm_atomic_state *state)
 		intel_state->wm_results.dirty_pipes = ~0;
 	}
 
+	/*
+	 * We're not recomputing for the pipes not included in the commit, so
+	 * make sure we start with the current state.
+	 */
+	memcpy(ddb, &dev_priv->wm.skl_hw.ddb, sizeof(*ddb));
+
 	for_each_intel_crtc_mask(dev, intel_crtc, realloc_pipes) {
 		struct intel_crtc_state *cstate;
 
@@ -4003,7 +4068,7 @@ skl_compute_ddb(struct drm_atomic_state *state)
 		if (ret)
 			return ret;
 
-		ret = drm_atomic_add_affected_planes(state, &intel_crtc->base);
+		ret = skl_ddb_add_affected_planes(cstate);
 		if (ret)
 			return ret;
 	}
@@ -4016,19 +4081,64 @@ skl_copy_wm_for_pipe(struct skl_wm_values *dst,
 		     struct skl_wm_values *src,
 		     enum pipe pipe)
 {
-	dst->wm_linetime[pipe] = src->wm_linetime[pipe];
-	memcpy(dst->plane[pipe], src->plane[pipe],
-	       sizeof(dst->plane[pipe]));
-	memcpy(dst->plane_trans[pipe], src->plane_trans[pipe],
-	       sizeof(dst->plane_trans[pipe]));
-
-	dst->ddb.pipe[pipe] = src->ddb.pipe[pipe];
 	memcpy(dst->ddb.y_plane[pipe], src->ddb.y_plane[pipe],
 	       sizeof(dst->ddb.y_plane[pipe]));
 	memcpy(dst->ddb.plane[pipe], src->ddb.plane[pipe],
 	       sizeof(dst->ddb.plane[pipe]));
 }
 
+static void
+skl_print_wm_changes(const struct drm_atomic_state *state)
+{
+	const struct drm_device *dev = state->dev;
+	const struct drm_i915_private *dev_priv = to_i915(dev);
+	const struct intel_atomic_state *intel_state =
+		to_intel_atomic_state(state);
+	const struct drm_crtc *crtc;
+	const struct drm_crtc_state *cstate;
+	const struct drm_plane *plane;
+	const struct intel_plane *intel_plane;
+	const struct drm_plane_state *pstate;
+	const struct skl_ddb_allocation *old_ddb = &dev_priv->wm.skl_hw.ddb;
+	const struct skl_ddb_allocation *new_ddb = &intel_state->wm_results.ddb;
+	enum pipe pipe;
+	int id;
+	int i, j;
+
+	for_each_crtc_in_state(state, crtc, cstate, i) {
+		pipe = to_intel_crtc(crtc)->pipe;
+
+		for_each_plane_in_state(state, plane, pstate, j) {
+			const struct skl_ddb_entry *old, *new;
+
+			intel_plane = to_intel_plane(plane);
+			id = skl_wm_plane_id(intel_plane);
+			old = &old_ddb->plane[pipe][id];
+			new = &new_ddb->plane[pipe][id];
+
+			if (intel_plane->pipe != pipe)
+				continue;
+
+			if (skl_ddb_entry_equal(old, new))
+				continue;
+
+			if (id != PLANE_CURSOR) {
+				DRM_DEBUG_ATOMIC("[PLANE:%d:plane %d%c] ddb (%d - %d) -> (%d - %d)\n",
+						 plane->base.id, id + 1,
+						 pipe_name(pipe),
+						 old->start, old->end,
+						 new->start, new->end);
+			} else {
+				DRM_DEBUG_ATOMIC("[PLANE:%d:cursor %c] ddb (%d - %d) -> (%d - %d)\n",
+						 plane->base.id,
+						 pipe_name(pipe),
+						 old->start, old->end,
+						 new->start, new->end);
+			}
+		}
+	}
+}
+
 static int
 skl_compute_wm(struct drm_atomic_state *state)
 {
@@ -4071,7 +4181,6 @@ skl_compute_wm(struct drm_atomic_state *state)
 	 * no suitable watermark values can be found.
 	 */
 	for_each_crtc_in_state(state, crtc, cstate, i) {
-		struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 		struct intel_crtc_state *intel_cstate =
 			to_intel_crtc_state(cstate);
 
@@ -4089,9 +4198,10 @@ skl_compute_wm(struct drm_atomic_state *state)
 			continue;
 
 		intel_cstate->update_wm_pre = true;
-		skl_compute_wm_results(crtc->dev, pipe_wm, results, intel_crtc);
 	}
 
+	skl_print_wm_changes(state);
+
 	return 0;
 }
 
@@ -4123,13 +4233,17 @@ static void skl_update_wm(struct drm_crtc *crtc)
 		int plane;
 
 		for (plane = 0; plane < intel_num_planes(intel_crtc); plane++)
-			skl_write_plane_wm(intel_crtc, results, plane);
+			skl_write_plane_wm(intel_crtc, &pipe_wm->planes[plane],
+					   &results->ddb, plane);
 
-		skl_write_cursor_wm(intel_crtc, results);
+		skl_write_cursor_wm(intel_crtc, &pipe_wm->planes[PLANE_CURSOR],
+				    &results->ddb);
 	}
 
 	skl_copy_wm_for_pipe(hw_vals, results, pipe);
 
+	intel_crtc->hw_ddb = cstate->wm.skl.ddb;
+
 	mutex_unlock(&dev_priv->wm.wm_mutex);
 }
 
@@ -4208,114 +4322,77 @@ static void ilk_optimize_watermarks(struct intel_crtc_state *cstate)
 	mutex_unlock(&dev_priv->wm.wm_mutex);
 }
 
-static void skl_pipe_wm_active_state(uint32_t val,
-				     struct skl_pipe_wm *active,
-				     bool is_transwm,
-				     bool is_cursor,
-				     int i,
-				     int level)
+static inline void skl_wm_level_from_reg_val(uint32_t val,
+					     struct skl_wm_level *level)
 {
-	bool is_enabled = (val & PLANE_WM_EN) != 0;
-
-	if (!is_transwm) {
-		if (!is_cursor) {
-			active->wm[level].plane_en[i] = is_enabled;
-			active->wm[level].plane_res_b[i] =
-					val & PLANE_WM_BLOCKS_MASK;
-			active->wm[level].plane_res_l[i] =
-					(val >> PLANE_WM_LINES_SHIFT) &
-						PLANE_WM_LINES_MASK;
-		} else {
-			active->wm[level].plane_en[PLANE_CURSOR] = is_enabled;
-			active->wm[level].plane_res_b[PLANE_CURSOR] =
-					val & PLANE_WM_BLOCKS_MASK;
-			active->wm[level].plane_res_l[PLANE_CURSOR] =
-					(val >> PLANE_WM_LINES_SHIFT) &
-						PLANE_WM_LINES_MASK;
-		}
-	} else {
-		if (!is_cursor) {
-			active->trans_wm.plane_en[i] = is_enabled;
-			active->trans_wm.plane_res_b[i] =
-					val & PLANE_WM_BLOCKS_MASK;
-			active->trans_wm.plane_res_l[i] =
-					(val >> PLANE_WM_LINES_SHIFT) &
-						PLANE_WM_LINES_MASK;
-		} else {
-			active->trans_wm.plane_en[PLANE_CURSOR] = is_enabled;
-			active->trans_wm.plane_res_b[PLANE_CURSOR] =
-					val & PLANE_WM_BLOCKS_MASK;
-			active->trans_wm.plane_res_l[PLANE_CURSOR] =
-					(val >> PLANE_WM_LINES_SHIFT) &
-						PLANE_WM_LINES_MASK;
-		}
-	}
+	level->plane_en = val & PLANE_WM_EN;
+	level->plane_res_b = val & PLANE_WM_BLOCKS_MASK;
+	level->plane_res_l = (val >> PLANE_WM_LINES_SHIFT) &
+		PLANE_WM_LINES_MASK;
 }
 
-static void skl_pipe_wm_get_hw_state(struct drm_crtc *crtc)
+void skl_pipe_wm_get_hw_state(struct drm_crtc *crtc,
+			      struct skl_pipe_wm *out)
 {
 	struct drm_device *dev = crtc->dev;
 	struct drm_i915_private *dev_priv = to_i915(dev);
-	struct skl_wm_values *hw = &dev_priv->wm.skl_hw;
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
-	struct intel_crtc_state *cstate = to_intel_crtc_state(crtc->state);
-	struct skl_pipe_wm *active = &cstate->wm.skl.optimal;
+	struct intel_plane *intel_plane;
+	struct skl_plane_wm *wm;
 	enum pipe pipe = intel_crtc->pipe;
-	int level, i, max_level;
-	uint32_t temp;
-
-	max_level = ilk_wm_max_level(dev);
-
-	hw->wm_linetime[pipe] = I915_READ(PIPE_WM_LINETIME(pipe));
-
-	for (level = 0; level <= max_level; level++) {
-		for (i = 0; i < intel_num_planes(intel_crtc); i++)
-			hw->plane[pipe][i][level] =
-					I915_READ(PLANE_WM(pipe, i, level));
-		hw->plane[pipe][PLANE_CURSOR][level] = I915_READ(CUR_WM(pipe, level));
-	}
-
-	for (i = 0; i < intel_num_planes(intel_crtc); i++)
-		hw->plane_trans[pipe][i] = I915_READ(PLANE_WM_TRANS(pipe, i));
-	hw->plane_trans[pipe][PLANE_CURSOR] = I915_READ(CUR_WM_TRANS(pipe));
+	int level, id, max_level;
+	uint32_t val;
 
-	if (!intel_crtc->active)
-		return;
+	max_level = ilk_wm_max_level(dev_priv);
 
-	hw->dirty_pipes |= drm_crtc_mask(crtc);
+	for_each_intel_plane_on_crtc(dev, intel_crtc, intel_plane) {
+		id = skl_wm_plane_id(intel_plane);
+		wm = &out->planes[id];
 
-	active->linetime = hw->wm_linetime[pipe];
+		for (level = 0; level <= max_level; level++) {
+			if (id != PLANE_CURSOR)
+				val = I915_READ(PLANE_WM(pipe, id, level));
+			else
+				val = I915_READ(CUR_WM(pipe, level));
 
-	for (level = 0; level <= max_level; level++) {
-		for (i = 0; i < intel_num_planes(intel_crtc); i++) {
-			temp = hw->plane[pipe][i][level];
-			skl_pipe_wm_active_state(temp, active, false,
-						false, i, level);
+			skl_wm_level_from_reg_val(val, &wm->wm[level]);
 		}
-		temp = hw->plane[pipe][PLANE_CURSOR][level];
-		skl_pipe_wm_active_state(temp, active, false, true, i, level);
-	}
 
-	for (i = 0; i < intel_num_planes(intel_crtc); i++) {
-		temp = hw->plane_trans[pipe][i];
-		skl_pipe_wm_active_state(temp, active, true, false, i, 0);
+		if (id != PLANE_CURSOR)
+			val = I915_READ(PLANE_WM_TRANS(pipe, id));
+		else
+			val = I915_READ(CUR_WM_TRANS(pipe));
+
+		skl_wm_level_from_reg_val(val, &wm->trans_wm);
 	}
 
-	temp = hw->plane_trans[pipe][PLANE_CURSOR];
-	skl_pipe_wm_active_state(temp, active, true, true, i, 0);
+	if (!intel_crtc->active)
+		return;
 
-	intel_crtc->wm.active.skl = *active;
+	out->linetime = I915_READ(PIPE_WM_LINETIME(pipe));
 }
 
 void skl_wm_get_hw_state(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = to_i915(dev);
+	struct skl_wm_values *hw = &dev_priv->wm.skl_hw;
 	struct skl_ddb_allocation *ddb = &dev_priv->wm.skl_hw.ddb;
 	struct drm_crtc *crtc;
+	struct intel_crtc *intel_crtc;
+	struct intel_crtc_state *cstate;
 
 	skl_ddb_get_hw_state(dev_priv, ddb);
-	list_for_each_entry(crtc, &dev->mode_config.crtc_list, head)
-		skl_pipe_wm_get_hw_state(crtc);
+	list_for_each_entry(crtc, &dev->mode_config.crtc_list, head) {
+		intel_crtc = to_intel_crtc(crtc);
+		cstate = to_intel_crtc_state(crtc->state);
+
+		skl_pipe_wm_get_hw_state(crtc, &cstate->wm.skl.optimal);
+
+		if (intel_crtc->active) {
+			hw->dirty_pipes |= drm_crtc_mask(crtc);
+			intel_crtc->wm.active.skl = cstate->wm.skl.optimal;
+		}
+	}
 
 	if (dev_priv->active_crtcs) {
 		/* Fully recompute DDB on first atomic commit */
@@ -4342,7 +4419,7 @@ static void ilk_pipe_wm_get_hw_state(struct drm_crtc *crtc)
 	};
 
 	hw->wm_pipe[pipe] = I915_READ(wm0_pipe_reg[pipe]);
-	if (IS_HASWELL(dev) || IS_BROADWELL(dev))
+	if (IS_HASWELL(dev_priv) || IS_BROADWELL(dev_priv))
 		hw->wm_linetime[pipe] = I915_READ(PIPE_WM_LINETIME(pipe));
 
 	memset(active, 0, sizeof(*active));
@@ -4364,7 +4441,7 @@ static void ilk_pipe_wm_get_hw_state(struct drm_crtc *crtc)
 		active->wm[0].cur_val = tmp & WM0_PIPE_CURSOR_MASK;
 		active->linetime = hw->wm_linetime[pipe];
 	} else {
-		int level, max_level = ilk_wm_max_level(dev);
+		int level, max_level = ilk_wm_max_level(dev_priv);
 
 		/*
 		 * For inactive pipes, all watermark levels
@@ -4550,10 +4627,10 @@ void ilk_wm_get_hw_state(struct drm_device *dev)
 		hw->wm_lp_spr[2] = I915_READ(WM3S_LP_IVB);
 	}
 
-	if (IS_HASWELL(dev) || IS_BROADWELL(dev))
+	if (IS_HASWELL(dev_priv) || IS_BROADWELL(dev_priv))
 		hw->partitioning = (I915_READ(WM_MISC) & WM_MISC_DATA_PARTITION_5_6) ?
 			INTEL_DDB_PART_5_6 : INTEL_DDB_PART_1_2;
-	else if (IS_IVYBRIDGE(dev))
+	else if (IS_IVYBRIDGE(dev_priv))
 		hw->partitioning = (I915_READ(DISP_ARB_CTL2) & DISP_DATA_PARTITION_5_6) ?
 			INTEL_DDB_PART_5_6 : INTEL_DDB_PART_1_2;
 
@@ -5297,6 +5374,7 @@ static void gen9_enable_rps(struct drm_i915_private *dev_priv)
 static void gen9_enable_rc6(struct drm_i915_private *dev_priv)
 {
 	struct intel_engine_cs *engine;
+	enum intel_engine_id id;
 	uint32_t rc6_mask = 0;
 
 	/* 1a: Software RC state - RC0 */
@@ -5318,7 +5396,7 @@ static void gen9_enable_rc6(struct drm_i915_private *dev_priv)
 		I915_WRITE(GEN6_RC6_WAKE_RATE_LIMIT, 54 << 16);
 	I915_WRITE(GEN6_RC_EVALUATION_INTERVAL, 125000); /* 12500 * 1280ns */
 	I915_WRITE(GEN6_RC_IDLE_HYSTERSIS, 25); /* 25 * 1280ns */
-	for_each_engine(engine, dev_priv)
+	for_each_engine(engine, dev_priv, id)
 		I915_WRITE(RING_MAX_IDLE(engine->mmio_base), 10);
 
 	if (HAS_GUC(dev_priv))
@@ -5334,9 +5412,8 @@ static void gen9_enable_rc6(struct drm_i915_private *dev_priv)
 	if (intel_enable_rc6() & INTEL_RC6_ENABLE)
 		rc6_mask = GEN6_RC_CTL_RC6_ENABLE;
 	DRM_INFO("RC6 %s\n", onoff(rc6_mask & GEN6_RC_CTL_RC6_ENABLE));
-	/* WaRsUseTimeoutMode */
-	if (IS_SKL_REVID(dev_priv, 0, SKL_REVID_D0) ||
-	    IS_BXT_REVID(dev_priv, 0, BXT_REVID_A1)) {
+	/* WaRsUseTimeoutMode:bxt */
+	if (IS_BXT_REVID(dev_priv, 0, BXT_REVID_A1)) {
 		I915_WRITE(GEN6_RC6_THRESHOLD, 625); /* 800us */
 		I915_WRITE(GEN6_RC_CONTROL, GEN6_RC_CTL_HW_ENABLE |
 			   GEN7_RC_CTL_TO_MODE |
@@ -5364,6 +5441,7 @@ static void gen9_enable_rc6(struct drm_i915_private *dev_priv)
 static void gen8_enable_rps(struct drm_i915_private *dev_priv)
 {
 	struct intel_engine_cs *engine;
+	enum intel_engine_id id;
 	uint32_t rc6_mask = 0;
 
 	/* 1a: Software RC state - RC0 */
@@ -5380,7 +5458,7 @@ static void gen8_enable_rps(struct drm_i915_private *dev_priv)
 	I915_WRITE(GEN6_RC6_WAKE_RATE_LIMIT, 40 << 16);
 	I915_WRITE(GEN6_RC_EVALUATION_INTERVAL, 125000); /* 12500 * 1280ns */
 	I915_WRITE(GEN6_RC_IDLE_HYSTERSIS, 25); /* 25 * 1280ns */
-	for_each_engine(engine, dev_priv)
+	for_each_engine(engine, dev_priv, id)
 		I915_WRITE(RING_MAX_IDLE(engine->mmio_base), 10);
 	I915_WRITE(GEN6_RC_SLEEP, 0);
 	if (IS_BROADWELL(dev_priv))
@@ -5440,6 +5518,7 @@ static void gen8_enable_rps(struct drm_i915_private *dev_priv)
 static void gen6_enable_rps(struct drm_i915_private *dev_priv)
 {
 	struct intel_engine_cs *engine;
+	enum intel_engine_id id;
 	u32 rc6vids, rc6_mask = 0;
 	u32 gtfifodbg;
 	int rc6_mode;
@@ -5473,7 +5552,7 @@ static void gen6_enable_rps(struct drm_i915_private *dev_priv)
 	I915_WRITE(GEN6_RC_EVALUATION_INTERVAL, 125000);
 	I915_WRITE(GEN6_RC_IDLE_HYSTERSIS, 25);
 
-	for_each_engine(engine, dev_priv)
+	for_each_engine(engine, dev_priv, id)
 		I915_WRITE(RING_MAX_IDLE(engine->mmio_base), 10);
 
 	I915_WRITE(GEN6_RC_SLEEP, 0);
@@ -5510,10 +5589,6 @@ static void gen6_enable_rps(struct drm_i915_private *dev_priv)
 	I915_WRITE(GEN6_RP_DOWN_TIMEOUT, 50000);
 	I915_WRITE(GEN6_RP_IDLE_HYSTERSIS, 10);
 
-	ret = sandybridge_pcode_write(dev_priv, GEN6_PCODE_WRITE_MIN_FREQ_TABLE, 0);
-	if (ret)
-		DRM_DEBUG_DRIVER("Failed to set the min frequency\n");
-
 	reset_rps(dev_priv, gen6_set_rps);
 
 	rc6vids = 0;
@@ -5922,6 +5997,7 @@ static void valleyview_cleanup_gt_powersave(struct drm_i915_private *dev_priv)
 static void cherryview_enable_rps(struct drm_i915_private *dev_priv)
 {
 	struct intel_engine_cs *engine;
+	enum intel_engine_id id;
 	u32 gtfifodbg, val, rc6_mode = 0, pcbr;
 
 	WARN_ON(!mutex_is_locked(&dev_priv->rps.hw_lock));
@@ -5948,7 +6024,7 @@ static void cherryview_enable_rps(struct drm_i915_private *dev_priv)
 	I915_WRITE(GEN6_RC_EVALUATION_INTERVAL, 125000); /* 12500 * 1280ns */
 	I915_WRITE(GEN6_RC_IDLE_HYSTERSIS, 25); /* 25 * 1280ns */
 
-	for_each_engine(engine, dev_priv)
+	for_each_engine(engine, dev_priv, id)
 		I915_WRITE(RING_MAX_IDLE(engine->mmio_base), 10);
 	I915_WRITE(GEN6_RC_SLEEP, 0);
 
@@ -6010,6 +6086,7 @@ static void cherryview_enable_rps(struct drm_i915_private *dev_priv)
 static void valleyview_enable_rps(struct drm_i915_private *dev_priv)
 {
 	struct intel_engine_cs *engine;
+	enum intel_engine_id id;
 	u32 gtfifodbg, val, rc6_mode = 0;
 
 	WARN_ON(!mutex_is_locked(&dev_priv->rps.hw_lock));
@@ -6049,7 +6126,7 @@ static void valleyview_enable_rps(struct drm_i915_private *dev_priv)
 	I915_WRITE(GEN6_RC_EVALUATION_INTERVAL, 125000);
 	I915_WRITE(GEN6_RC_IDLE_HYSTERSIS, 25);
 
-	for_each_engine(engine, dev_priv)
+	for_each_engine(engine, dev_priv, id)
 		I915_WRITE(RING_MAX_IDLE(engine->mmio_base), 10);
 
 	I915_WRITE(GEN6_RC6_THRESHOLD, 0x557);
@@ -6732,7 +6809,7 @@ static void __intel_autoenable_gt_powersave(struct work_struct *work)
 	if (READ_ONCE(dev_priv->rps.enabled))
 		goto out;
 
-	rcs = &dev_priv->engine[RCS];
+	rcs = dev_priv->engine[RCS];
 	if (rcs->last_context)
 		goto out;
 
@@ -6869,7 +6946,7 @@ static void ironlake_init_clock_gating(struct drm_device *dev)
 	 * The bit 22 of 0x42004
 	 * The bit 7,8,9 of 0x42020.
 	 */
-	if (IS_IRONLAKE_M(dev)) {
+	if (IS_IRONLAKE_M(dev_priv)) {
 		/* WaFbcAsynchFlipDisableFbcQueue:ilk */
 		I915_WRITE(ILK_DISPLAY_CHICKEN1,
 			   I915_READ(ILK_DISPLAY_CHICKEN1) |
@@ -7071,7 +7148,7 @@ static void lpt_init_clock_gating(struct drm_device *dev)
 	 * TODO: this bit should only be enabled when really needed, then
 	 * disabled when not needed anymore in order to save power.
 	 */
-	if (HAS_PCH_LPT_LP(dev))
+	if (HAS_PCH_LPT_LP(dev_priv))
 		I915_WRITE(SOUTH_DSPCLK_GATE_D,
 			   I915_READ(SOUTH_DSPCLK_GATE_D) |
 			   PCH_LP_PARTITION_LEVEL_DISABLE);
@@ -7086,7 +7163,7 @@ static void lpt_suspend_hw(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = to_i915(dev);
 
-	if (HAS_PCH_LPT_LP(dev)) {
+	if (HAS_PCH_LPT_LP(dev_priv)) {
 		uint32_t val = I915_READ(SOUTH_DSPCLK_GATE_D);
 
 		val &= ~PCH_LP_PARTITION_LEVEL_DISABLE;
@@ -7279,7 +7356,7 @@ static void ivybridge_init_clock_gating(struct drm_device *dev)
 		   CHICKEN3_DGMG_DONE_FIX_DISABLE);
 
 	/* WaDisablePSDDualDispatchEnable:ivb */
-	if (IS_IVB_GT1(dev))
+	if (IS_IVB_GT1(dev_priv))
 		I915_WRITE(GEN7_HALF_SLICE_CHICKEN1,
 			   _MASKED_BIT_ENABLE(GEN7_PSD_SINGLE_PORT_DISPATCH_ENABLE));
 
@@ -7295,7 +7372,7 @@ static void ivybridge_init_clock_gating(struct drm_device *dev)
 			GEN7_WA_FOR_GEN7_L3_CONTROL);
 	I915_WRITE(GEN7_L3_CHICKEN_MODE_REGISTER,
 		   GEN7_WA_L3_CHICKEN_MODE);
-	if (IS_IVB_GT1(dev))
+	if (IS_IVB_GT1(dev_priv))
 		I915_WRITE(GEN7_ROW_CHICKEN2,
 			   _MASKED_BIT_ENABLE(DOP_CLOCK_GATING_DISABLE));
 	else {
@@ -7352,7 +7429,7 @@ static void ivybridge_init_clock_gating(struct drm_device *dev)
 	snpcr |= GEN6_MBC_SNPCR_MED;
 	I915_WRITE(GEN6_MBCUNIT_SNPCR, snpcr);
 
-	if (!HAS_PCH_NOP(dev))
+	if (!HAS_PCH_NOP(dev_priv))
 		cpt_init_clock_gating(dev);
 
 	gen6_check_mch_setup(dev);
@@ -7489,7 +7566,7 @@ static void g4x_init_clock_gating(struct drm_device *dev)
 	dspclk_gate = VRHUNIT_CLOCK_GATE_DISABLE |
 		OVRUNIT_CLOCK_GATE_DISABLE |
 		OVCUNIT_CLOCK_GATE_DISABLE;
-	if (IS_GM45(dev))
+	if (IS_GM45(dev_priv))
 		dspclk_gate |= DSSUNIT_CLOCK_GATE_DISABLE;
 	I915_WRITE(DSPCLK_GATE_D, dspclk_gate);
 
@@ -7595,7 +7672,7 @@ void intel_init_clock_gating(struct drm_device *dev)
 
 void intel_suspend_hw(struct drm_device *dev)
 {
-	if (HAS_PCH_LPT(dev))
+	if (HAS_PCH_LPT(to_i915(dev)))
 		lpt_suspend_hw(dev);
 }
 
@@ -7663,7 +7740,7 @@ void intel_init_pm(struct drm_device *dev)
 	/* For cxsr */
 	if (IS_PINEVIEW(dev))
 		i915_pineview_get_mem_freq(dev);
-	else if (IS_GEN5(dev))
+	else if (IS_GEN5(dev_priv))
 		i915_ironlake_get_mem_freq(dev);
 
 	/* For FIFO watermark updates */
@@ -7671,12 +7748,12 @@ void intel_init_pm(struct drm_device *dev)
 		skl_setup_wm_latency(dev);
 		dev_priv->display.update_wm = skl_update_wm;
 		dev_priv->display.compute_global_watermarks = skl_compute_wm;
-	} else if (HAS_PCH_SPLIT(dev)) {
+	} else if (HAS_PCH_SPLIT(dev_priv)) {
 		ilk_setup_wm_latency(dev);
 
-		if ((IS_GEN5(dev) && dev_priv->wm.pri_latency[1] &&
+		if ((IS_GEN5(dev_priv) && dev_priv->wm.pri_latency[1] &&
 		     dev_priv->wm.spr_latency[1] && dev_priv->wm.cur_latency[1]) ||
-		    (!IS_GEN5(dev) && dev_priv->wm.pri_latency[0] &&
+		    (!IS_GEN5(dev_priv) && dev_priv->wm.pri_latency[0] &&
 		     dev_priv->wm.spr_latency[0] && dev_priv->wm.cur_latency[0])) {
 			dev_priv->display.compute_pipe_wm = ilk_compute_pipe_wm;
 			dev_priv->display.compute_intermediate_wm =
@@ -7689,14 +7766,14 @@ void intel_init_pm(struct drm_device *dev)
 			DRM_DEBUG_KMS("Failed to read display plane latency. "
 				      "Disable CxSR\n");
 		}
-	} else if (IS_CHERRYVIEW(dev)) {
+	} else if (IS_CHERRYVIEW(dev_priv)) {
 		vlv_setup_wm_latency(dev);
 		dev_priv->display.update_wm = vlv_update_wm;
-	} else if (IS_VALLEYVIEW(dev)) {
+	} else if (IS_VALLEYVIEW(dev_priv)) {
 		vlv_setup_wm_latency(dev);
 		dev_priv->display.update_wm = vlv_update_wm;
 	} else if (IS_PINEVIEW(dev)) {
-		if (!intel_get_cxsr_latency(IS_PINEVIEW_G(dev),
+		if (!intel_get_cxsr_latency(IS_PINEVIEW_G(dev_priv),
 					    dev_priv->is_ddr3,
 					    dev_priv->fsb_freq,
 					    dev_priv->mem_freq)) {
@@ -7710,14 +7787,14 @@ void intel_init_pm(struct drm_device *dev)
 			dev_priv->display.update_wm = NULL;
 		} else
 			dev_priv->display.update_wm = pineview_update_wm;
-	} else if (IS_G4X(dev)) {
+	} else if (IS_G4X(dev_priv)) {
 		dev_priv->display.update_wm = g4x_update_wm;
-	} else if (IS_GEN4(dev)) {
+	} else if (IS_GEN4(dev_priv)) {
 		dev_priv->display.update_wm = i965_update_wm;
-	} else if (IS_GEN3(dev)) {
+	} else if (IS_GEN3(dev_priv)) {
 		dev_priv->display.update_wm = i9xx_update_wm;
 		dev_priv->display.get_fifo_size = i9xx_get_fifo_size;
-	} else if (IS_GEN2(dev)) {
+	} else if (IS_GEN2(dev_priv)) {
 		if (INTEL_INFO(dev)->num_pipes == 1) {
 			dev_priv->display.update_wm = i845_update_wm;
 			dev_priv->display.get_fifo_size = i845_get_fifo_size;
diff --git a/drivers/gpu/drm/i915/intel_psr.c b/drivers/gpu/drm/i915/intel_psr.c
index 108ba1e5d658..271a3e29ff23 100644
--- a/drivers/gpu/drm/i915/intel_psr.c
+++ b/drivers/gpu/drm/i915/intel_psr.c
@@ -268,7 +268,7 @@ static void hsw_psr_enable_source(struct intel_dp *intel_dp)
 	val |= max_sleep_time << EDP_PSR_MAX_SLEEP_TIME_SHIFT;
 	val |= idle_frames << EDP_PSR_IDLE_FRAME_SHIFT;
 
-	if (IS_HASWELL(dev))
+	if (IS_HASWELL(dev_priv))
 		val |= EDP_PSR_MIN_LINK_ENTRY_TIME_8_LINES;
 
 	if (dev_priv->psr.link_standby)
@@ -344,7 +344,7 @@ static bool intel_psr_match_conditions(struct intel_dp *intel_dp)
 	 * ones. Since by Display design transcoder EDP is tied to port A
 	 * we can safely escape based on the port A.
 	 */
-	if (HAS_DDI(dev) && dig_port->port != PORT_A) {
+	if (HAS_DDI(dev_priv) && dig_port->port != PORT_A) {
 		DRM_DEBUG_KMS("PSR condition failed: Port not supported\n");
 		return false;
 	}
@@ -354,20 +354,20 @@ static bool intel_psr_match_conditions(struct intel_dp *intel_dp)
 		return false;
 	}
 
-	if ((IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) &&
+	if ((IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv)) &&
 	    !dev_priv->psr.link_standby) {
 		DRM_ERROR("PSR condition failed: Link off requested but not supported on this platform\n");
 		return false;
 	}
 
-	if (IS_HASWELL(dev) &&
+	if (IS_HASWELL(dev_priv) &&
 	    I915_READ(HSW_STEREO_3D_CTL(intel_crtc->config->cpu_transcoder)) &
 		      S3D_ENABLE) {
 		DRM_DEBUG_KMS("PSR condition failed: Stereo 3D is Enabled\n");
 		return false;
 	}
 
-	if (IS_HASWELL(dev) &&
+	if (IS_HASWELL(dev_priv) &&
 	    adjusted_mode->flags & DRM_MODE_FLAG_INTERLACE) {
 		DRM_DEBUG_KMS("PSR condition failed: Interlaced is Enabled\n");
 		return false;
@@ -402,7 +402,7 @@ static void intel_psr_activate(struct intel_dp *intel_dp)
 	lockdep_assert_held(&dev_priv->psr.lock);
 
 	/* Enable/Re-enable PSR on the host */
-	if (HAS_DDI(dev))
+	if (HAS_DDI(dev_priv))
 		/* On HSW+ after we enable PSR on source it will activate it
 		 * as soon as it match configure idle_frame count. So
 		 * we just actually enable it here on activation time.
@@ -448,7 +448,7 @@ void intel_psr_enable(struct intel_dp *intel_dp)
 
 	dev_priv->psr.busy_frontbuffer_bits = 0;
 
-	if (HAS_DDI(dev)) {
+	if (HAS_DDI(dev_priv)) {
 		hsw_psr_setup_vsc(intel_dp);
 
 		if (dev_priv->psr.psr2_support) {
@@ -580,7 +580,7 @@ void intel_psr_disable(struct intel_dp *intel_dp)
 	}
 
 	/* Disable PSR on Source */
-	if (HAS_DDI(dev))
+	if (HAS_DDI(dev_priv))
 		hsw_psr_disable(intel_dp);
 	else
 		vlv_psr_disable(intel_dp);
@@ -827,17 +827,17 @@ void intel_psr_init(struct drm_device *dev)
 
 	/* Per platform default */
 	if (i915.enable_psr == -1) {
-		if (IS_HASWELL(dev) || IS_BROADWELL(dev))
+		if (IS_HASWELL(dev_priv) || IS_BROADWELL(dev_priv))
 			i915.enable_psr = 1;
 		else
 			i915.enable_psr = 0;
 	}
 
 	/* Set link_standby x link_off defaults */
-	if (IS_HASWELL(dev) || IS_BROADWELL(dev))
+	if (IS_HASWELL(dev_priv) || IS_BROADWELL(dev_priv))
 		/* HSW and BDW require workarounds that we don't implement. */
 		dev_priv->psr.link_standby = false;
-	else if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev))
+	else if (IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv))
 		/* On VLV and CHV only standby mode is supported. */
 		dev_priv->psr.link_standby = true;
 	else
diff --git a/drivers/gpu/drm/i915/intel_ringbuffer.c b/drivers/gpu/drm/i915/intel_ringbuffer.c
index 7a74750076c5..32786ba199b9 100644
--- a/drivers/gpu/drm/i915/intel_ringbuffer.c
+++ b/drivers/gpu/drm/i915/intel_ringbuffer.c
@@ -405,22 +405,6 @@ gen8_render_ring_flush(struct drm_i915_gem_request *req, u32 mode)
 	return gen8_emit_pipe_control(req, flags, scratch_addr);
 }
 
-u64 intel_engine_get_active_head(struct intel_engine_cs *engine)
-{
-	struct drm_i915_private *dev_priv = engine->i915;
-	u64 acthd;
-
-	if (INTEL_GEN(dev_priv) >= 8)
-		acthd = I915_READ64_2x32(RING_ACTHD(engine->mmio_base),
-					 RING_ACTHD_UDW(engine->mmio_base));
-	else if (INTEL_GEN(dev_priv) >= 4)
-		acthd = I915_READ(RING_ACTHD(engine->mmio_base));
-	else
-		acthd = I915_READ(ACTHD);
-
-	return acthd;
-}
-
 static void ring_setup_phys_status_page(struct intel_engine_cs *engine)
 {
 	struct drm_i915_private *dev_priv = engine->i915;
@@ -564,7 +548,7 @@ static int init_ring_common(struct intel_engine_cs *engine)
 	else
 		intel_ring_setup_status_page(engine);
 
-	intel_engine_reset_irq(engine);
+	intel_engine_reset_breadcrumbs(engine);
 
 	/* Enforce ordering by reading HEAD register back */
 	I915_READ_HEAD(engine);
@@ -585,9 +569,7 @@ static int init_ring_common(struct intel_engine_cs *engine)
 	I915_WRITE_TAIL(engine, ring->tail);
 	(void)I915_READ_TAIL(engine);
 
-	I915_WRITE_CTL(engine,
-			((ring->size - PAGE_SIZE) & RING_NR_PAGES)
-			| RING_VALID);
+	I915_WRITE_CTL(engine, RING_CTL_SIZE(ring->size) | RING_VALID);
 
 	/* If the head is still not zero, the ring is dead */
 	if (intel_wait_for_register_fw(dev_priv, RING_CTL(engine->mmio_base),
@@ -851,15 +833,13 @@ static int gen9_init_workarounds(struct intel_engine_cs *engine)
 	WA_SET_BIT_MASKED(HALF_SLICE_CHICKEN3,
 			  GEN9_DISABLE_OCL_OOB_SUPPRESS_LOGIC);
 
-	/* WaDisableDgMirrorFixInHalfSliceChicken5:skl,bxt */
-	if (IS_SKL_REVID(dev_priv, 0, SKL_REVID_B0) ||
-	    IS_BXT_REVID(dev_priv, 0, BXT_REVID_A1))
+	/* WaDisableDgMirrorFixInHalfSliceChicken5:bxt */
+	if (IS_BXT_REVID(dev_priv, 0, BXT_REVID_A1))
 		WA_CLR_BIT_MASKED(GEN9_HALF_SLICE_CHICKEN5,
 				  GEN9_DG_MIRROR_FIX_ENABLE);
 
-	/* WaSetDisablePixMaskCammingAndRhwoInCommonSliceChicken:skl,bxt */
-	if (IS_SKL_REVID(dev_priv, 0, SKL_REVID_B0) ||
-	    IS_BXT_REVID(dev_priv, 0, BXT_REVID_A1)) {
+	/* WaSetDisablePixMaskCammingAndRhwoInCommonSliceChicken:bxt */
+	if (IS_BXT_REVID(dev_priv, 0, BXT_REVID_A1)) {
 		WA_SET_BIT_MASKED(GEN7_COMMON_SLICE_CHICKEN1,
 				  GEN9_RHWO_OPTIMIZATION_DISABLE);
 		/*
@@ -869,10 +849,8 @@ static int gen9_init_workarounds(struct intel_engine_cs *engine)
 		 */
 	}
 
-	/* WaEnableYV12BugFixInHalfSliceChicken7:skl,bxt,kbl */
 	/* WaEnableSamplerGPGPUPreemptionSupport:skl,bxt,kbl */
 	WA_SET_BIT_MASKED(GEN9_HALF_SLICE_CHICKEN7,
-			  GEN9_ENABLE_YV12_BUGFIX |
 			  GEN9_ENABLE_GPGPU_PREEMPTION);
 
 	/* Wa4x4STCOptimizationDisable:skl,bxt,kbl */
@@ -884,9 +862,8 @@ static int gen9_init_workarounds(struct intel_engine_cs *engine)
 	WA_CLR_BIT_MASKED(GEN9_HALF_SLICE_CHICKEN5,
 			  GEN9_CCS_TLB_PREFETCH_ENABLE);
 
-	/* WaDisableMaskBasedCammingInRCC:skl,bxt */
-	if (IS_SKL_REVID(dev_priv, SKL_REVID_C0, SKL_REVID_C0) ||
-	    IS_BXT_REVID(dev_priv, 0, BXT_REVID_A1))
+	/* WaDisableMaskBasedCammingInRCC:bxt */
+	if (IS_BXT_REVID(dev_priv, 0, BXT_REVID_A1))
 		WA_SET_BIT_MASKED(SLICE_ECO_CHICKEN0,
 				  PIXEL_MASK_CAMMING_DISABLE);
 
@@ -1003,47 +980,12 @@ static int skl_init_workarounds(struct intel_engine_cs *engine)
 	 * until D0 which is the default case so this is equivalent to
 	 * !WaDisablePerCtxtPreemptionGranularityControl:skl
 	 */
-	if (IS_SKL_REVID(dev_priv, SKL_REVID_E0, REVID_FOREVER)) {
-		I915_WRITE(GEN7_FF_SLICE_CS_CHICKEN1,
-			   _MASKED_BIT_ENABLE(GEN9_FFSC_PERCTX_PREEMPT_CTRL));
-	}
-
-	if (IS_SKL_REVID(dev_priv, 0, SKL_REVID_E0)) {
-		/* WaDisableChickenBitTSGBarrierAckForFFSliceCS:skl */
-		I915_WRITE(FF_SLICE_CS_CHICKEN2,
-			   _MASKED_BIT_ENABLE(GEN9_TSG_BARRIER_ACK_DISABLE));
-	}
-
-	/* GEN8_L3SQCREG4 has a dependency with WA batch so any new changes
-	 * involving this register should also be added to WA batch as required.
-	 */
-	if (IS_SKL_REVID(dev_priv, 0, SKL_REVID_E0))
-		/* WaDisableLSQCROPERFforOCL:skl */
-		I915_WRITE(GEN8_L3SQCREG4, I915_READ(GEN8_L3SQCREG4) |
-			   GEN8_LQSC_RO_PERF_DIS);
+	I915_WRITE(GEN7_FF_SLICE_CS_CHICKEN1,
+		   _MASKED_BIT_ENABLE(GEN9_FFSC_PERCTX_PREEMPT_CTRL));
 
 	/* WaEnableGapsTsvCreditFix:skl */
-	if (IS_SKL_REVID(dev_priv, SKL_REVID_C0, REVID_FOREVER)) {
-		I915_WRITE(GEN8_GARBCNTL, (I915_READ(GEN8_GARBCNTL) |
-					   GEN9_GAPS_TSV_CREDIT_DISABLE));
-	}
-
-	/* WaDisablePowerCompilerClockGating:skl */
-	if (IS_SKL_REVID(dev_priv, SKL_REVID_B0, SKL_REVID_B0))
-		WA_SET_BIT_MASKED(HIZ_CHICKEN,
-				  BDW_HIZ_POWER_COMPILER_CLOCK_GATING_DISABLE);
-
-	/* WaBarrierPerformanceFixDisable:skl */
-	if (IS_SKL_REVID(dev_priv, SKL_REVID_C0, SKL_REVID_D0))
-		WA_SET_BIT_MASKED(HDC_CHICKEN0,
-				  HDC_FENCE_DEST_SLM_DISABLE |
-				  HDC_BARRIER_PERFORMANCE_DISABLE);
-
-	/* WaDisableSbeCacheDispatchPortSharing:skl */
-	if (IS_SKL_REVID(dev_priv, 0, SKL_REVID_F0))
-		WA_SET_BIT_MASKED(
-			GEN7_HALF_SLICE_CHICKEN1,
-			GEN7_SBE_SS_CACHE_DISPATCH_PORT_SHARING_DISABLE);
+	I915_WRITE(GEN8_GARBCNTL, (I915_READ(GEN8_GARBCNTL) |
+				   GEN9_GAPS_TSV_CREDIT_DISABLE));
 
 	/* WaDisableGafsUnitClkGating:skl */
 	WA_SET_BIT(GEN7_UCGCTL4, GEN8_EU_GAUNIT_CLOCK_GATE_DISABLE);
@@ -1284,7 +1226,7 @@ static int gen8_rcs_signal(struct drm_i915_gem_request *req)
 	if (ret)
 		return ret;
 
-	for_each_engine_id(waiter, dev_priv, id) {
+	for_each_engine(waiter, dev_priv, id) {
 		u64 gtt_offset = req->engine->semaphore.signal_ggtt[id];
 		if (gtt_offset == MI_SEMAPHORE_SYNC_INVALID)
 			continue;
@@ -1321,7 +1263,7 @@ static int gen8_xcs_signal(struct drm_i915_gem_request *req)
 	if (ret)
 		return ret;
 
-	for_each_engine_id(waiter, dev_priv, id) {
+	for_each_engine(waiter, dev_priv, id) {
 		u64 gtt_offset = req->engine->semaphore.signal_ggtt[id];
 		if (gtt_offset == MI_SEMAPHORE_SYNC_INVALID)
 			continue;
@@ -1348,6 +1290,7 @@ static int gen6_signal(struct drm_i915_gem_request *req)
 	struct intel_ring *ring = req->ring;
 	struct drm_i915_private *dev_priv = req->i915;
 	struct intel_engine_cs *engine;
+	enum intel_engine_id id;
 	int ret, num_rings;
 
 	num_rings = INTEL_INFO(dev_priv)->num_rings;
@@ -1355,7 +1298,7 @@ static int gen6_signal(struct drm_i915_gem_request *req)
 	if (ret)
 		return ret;
 
-	for_each_engine(engine, dev_priv) {
+	for_each_engine(engine, dev_priv, id) {
 		i915_reg_t mbox_reg;
 
 		if (!(BIT(engine->hw_id) & GEN6_SEMAPHORES_MASK))
@@ -1989,6 +1932,7 @@ intel_engine_create_ring(struct intel_engine_cs *engine, int size)
 	struct i915_vma *vma;
 
 	GEM_BUG_ON(!is_power_of_2(size));
+	GEM_BUG_ON(RING_CTL_SIZE(size) & ~RING_NR_PAGES);
 
 	ring = kzalloc(sizeof(*ring), GFP_KERNEL);
 	if (!ring)
@@ -2146,9 +2090,6 @@ void intel_engine_cleanup(struct intel_engine_cs *engine)
 {
 	struct drm_i915_private *dev_priv;
 
-	if (!intel_engine_initialized(engine))
-		return;
-
 	dev_priv = engine->i915;
 
 	if (engine->buffer) {
@@ -2175,13 +2116,16 @@ void intel_engine_cleanup(struct intel_engine_cs *engine)
 	intel_ring_context_unpin(dev_priv->kernel_context, engine);
 
 	engine->i915 = NULL;
+	dev_priv->engine[engine->id] = NULL;
+	kfree(engine);
 }
 
 void intel_legacy_submission_resume(struct drm_i915_private *dev_priv)
 {
 	struct intel_engine_cs *engine;
+	enum intel_engine_id id;
 
-	for_each_engine(engine, dev_priv) {
+	for_each_engine(engine, dev_priv, id) {
 		engine->buffer->head = engine->buffer->tail;
 		engine->buffer->last_retired_head = -1;
 	}
diff --git a/drivers/gpu/drm/i915/intel_ringbuffer.h b/drivers/gpu/drm/i915/intel_ringbuffer.h
index 7f64d611159b..32b2e6332ccf 100644
--- a/drivers/gpu/drm/i915/intel_ringbuffer.h
+++ b/drivers/gpu/drm/i915/intel_ringbuffer.h
@@ -73,13 +73,40 @@ enum intel_engine_hangcheck_action {
 
 #define HANGCHECK_SCORE_RING_HUNG 31
 
+#define I915_MAX_SLICES	3
+#define I915_MAX_SUBSLICES 3
+
+#define instdone_slice_mask(dev_priv__) \
+	(INTEL_GEN(dev_priv__) == 7 ? \
+	 1 : INTEL_INFO(dev_priv__)->sseu.slice_mask)
+
+#define instdone_subslice_mask(dev_priv__) \
+	(INTEL_GEN(dev_priv__) == 7 ? \
+	 1 : INTEL_INFO(dev_priv__)->sseu.subslice_mask)
+
+#define for_each_instdone_slice_subslice(dev_priv__, slice__, subslice__) \
+	for ((slice__) = 0, (subslice__) = 0; \
+	     (slice__) < I915_MAX_SLICES; \
+	     (subslice__) = ((subslice__) + 1) < I915_MAX_SUBSLICES ? (subslice__) + 1 : 0, \
+	       (slice__) += ((subslice__) == 0)) \
+		for_each_if((BIT(slice__) & instdone_slice_mask(dev_priv__)) && \
+			    (BIT(subslice__) & instdone_subslice_mask(dev_priv__)))
+
+struct intel_instdone {
+	u32 instdone;
+	/* The following exist only in the RCS engine */
+	u32 slice_common;
+	u32 sampler[I915_MAX_SLICES][I915_MAX_SUBSLICES];
+	u32 row[I915_MAX_SLICES][I915_MAX_SUBSLICES];
+};
+
 struct intel_engine_hangcheck {
 	u64 acthd;
 	u32 seqno;
 	int score;
 	enum intel_engine_hangcheck_action action;
 	int deadlock;
-	u32 instdone[I915_NUM_INSTDONE_REG];
+	struct intel_instdone instdone;
 };
 
 struct intel_ring {
@@ -328,6 +355,7 @@ struct intel_engine_cs {
 	 * inspecting request list.
 	 */
 	u32 last_submitted_seqno;
+	u32 last_pending_seqno;
 
 	/* An RCU guarded pointer to the last request. No reference is
 	 * held to the request, users must carefully acquire a reference to
@@ -367,12 +395,6 @@ struct intel_engine_cs {
 	u32 (*get_cmd_length_mask)(u32 cmd_header);
 };
 
-static inline bool
-intel_engine_initialized(const struct intel_engine_cs *engine)
-{
-	return engine->i915 != NULL;
-}
-
 static inline unsigned
 intel_engine_flag(const struct intel_engine_cs *engine)
 {
@@ -393,7 +415,7 @@ intel_engine_sync_index(struct intel_engine_cs *engine,
 	 * vcs2 -> 0 = rcs, 1 = vcs, 2 = bcs, 3 = vecs;
 	 */
 
-	idx = (other - engine) - 1;
+	idx = (other->id - engine->id) - 1;
 	if (idx < 0)
 		idx += I915_NUM_ENGINES;
 
@@ -492,7 +514,6 @@ int __intel_ring_space(int head, int tail, int size);
 void intel_ring_update_space(struct intel_ring *ring);
 
 void intel_engine_init_seqno(struct intel_engine_cs *engine, u32 seqno);
-void intel_engine_reset_irq(struct intel_engine_cs *engine);
 
 void intel_engine_setup_common(struct intel_engine_cs *engine);
 int intel_engine_init_common(struct intel_engine_cs *engine);
@@ -514,6 +535,8 @@ int intel_init_blt_ring_buffer(struct intel_engine_cs *engine);
 int intel_init_vebox_ring_buffer(struct intel_engine_cs *engine);
 
 u64 intel_engine_get_active_head(struct intel_engine_cs *engine);
+u64 intel_engine_get_last_batch_head(struct intel_engine_cs *engine);
+
 static inline u32 intel_engine_get_seqno(struct intel_engine_cs *engine)
 {
 	return intel_read_status_page(engine, I915_GEM_HWS_INDEX);
@@ -521,6 +544,9 @@ static inline u32 intel_engine_get_seqno(struct intel_engine_cs *engine)
 
 int init_workarounds_ring(struct intel_engine_cs *engine);
 
+void intel_engine_get_instdone(struct intel_engine_cs *engine,
+			       struct intel_instdone *instdone);
+
 /*
  * Arbitrary size for largest possible 'add request' sequence. The code paths
  * are complex and variable. Empirical measurement shows that the worst case
@@ -584,6 +610,7 @@ static inline bool intel_engine_wakeup(const struct intel_engine_cs *engine)
 	return wakeup;
 }
 
+void intel_engine_reset_breadcrumbs(struct intel_engine_cs *engine);
 void intel_engine_fini_breadcrumbs(struct intel_engine_cs *engine);
 unsigned int intel_kick_waiters(struct drm_i915_private *i915);
 unsigned int intel_kick_signalers(struct drm_i915_private *i915);
diff --git a/drivers/gpu/drm/i915/intel_runtime_pm.c b/drivers/gpu/drm/i915/intel_runtime_pm.c
index 6c11168facd6..ee56a8756c07 100644
--- a/drivers/gpu/drm/i915/intel_runtime_pm.c
+++ b/drivers/gpu/drm/i915/intel_runtime_pm.c
@@ -288,7 +288,6 @@ void intel_display_set_init_power(struct drm_i915_private *dev_priv,
 static void hsw_power_well_post_enable(struct drm_i915_private *dev_priv)
 {
 	struct pci_dev *pdev = dev_priv->drm.pdev;
-	struct drm_device *dev = &dev_priv->drm;
 
 	/*
 	 * After we re-enable the power well, if we touch VGA register 0x3d5
@@ -304,7 +303,7 @@ static void hsw_power_well_post_enable(struct drm_i915_private *dev_priv)
 	outb(inb(VGA_MSR_READ), VGA_MSR_WRITE);
 	vga_put(pdev, VGA_RSRC_LEGACY_IO);
 
-	if (IS_BROADWELL(dev))
+	if (IS_BROADWELL(dev_priv))
 		gen8_irq_power_well_post_enable(dev_priv,
 						1 << PIPE_C | 1 << PIPE_B);
 }
@@ -2590,20 +2589,19 @@ static void vlv_cmnlane_wa(struct drm_i915_private *dev_priv)
  */
 void intel_power_domains_init_hw(struct drm_i915_private *dev_priv, bool resume)
 {
-	struct drm_device *dev = &dev_priv->drm;
 	struct i915_power_domains *power_domains = &dev_priv->power_domains;
 
 	power_domains->initializing = true;
 
-	if (IS_SKYLAKE(dev) || IS_KABYLAKE(dev)) {
+	if (IS_SKYLAKE(dev_priv) || IS_KABYLAKE(dev_priv)) {
 		skl_display_core_init(dev_priv, resume);
-	} else if (IS_BROXTON(dev)) {
+	} else if (IS_BROXTON(dev_priv)) {
 		bxt_display_core_init(dev_priv, resume);
-	} else if (IS_CHERRYVIEW(dev)) {
+	} else if (IS_CHERRYVIEW(dev_priv)) {
 		mutex_lock(&power_domains->lock);
 		chv_phy_control_init(dev_priv);
 		mutex_unlock(&power_domains->lock);
-	} else if (IS_VALLEYVIEW(dev)) {
+	} else if (IS_VALLEYVIEW(dev_priv)) {
 		mutex_lock(&power_domains->lock);
 		vlv_cmnlane_wa(dev_priv);
 		mutex_unlock(&power_domains->lock);
@@ -2758,7 +2756,6 @@ void intel_runtime_pm_put(struct drm_i915_private *dev_priv)
 void intel_runtime_pm_enable(struct drm_i915_private *dev_priv)
 {
 	struct pci_dev *pdev = dev_priv->drm.pdev;
-	struct drm_device *dev = &dev_priv->drm;
 	struct device *kdev = &pdev->dev;
 
 	pm_runtime_set_autosuspend_delay(kdev, 10000); /* 10s */
@@ -2770,7 +2767,7 @@ void intel_runtime_pm_enable(struct drm_i915_private *dev_priv)
 	 * so the driver's own RPM reference tracking asserts also work on
 	 * platforms without RPM support.
 	 */
-	if (!HAS_RUNTIME_PM(dev)) {
+	if (!HAS_RUNTIME_PM(dev_priv)) {
 		pm_runtime_dont_use_autosuspend(kdev);
 		pm_runtime_get_sync(kdev);
 	} else {
diff --git a/drivers/gpu/drm/i915/intel_sdvo.c b/drivers/gpu/drm/i915/intel_sdvo.c
index c551024d4871..49fb95d03d74 100644
--- a/drivers/gpu/drm/i915/intel_sdvo.c
+++ b/drivers/gpu/drm/i915/intel_sdvo.c
@@ -251,7 +251,7 @@ static void intel_sdvo_write_sdvox(struct intel_sdvo *intel_sdvo, u32 val)
 		 * HW workaround, need to write this twice for issue
 		 * that may result in first write getting masked.
 		 */
-		if (HAS_PCH_IBX(dev)) {
+		if (HAS_PCH_IBX(dev_priv)) {
 			I915_WRITE(intel_sdvo->sdvo_reg, val);
 			POSTING_READ(intel_sdvo->sdvo_reg);
 		}
@@ -307,7 +307,7 @@ static bool intel_sdvo_read_byte(struct intel_sdvo *intel_sdvo, u8 addr, u8 *ch)
 static const struct _sdvo_cmd_name {
 	u8 cmd;
 	const char *name;
-} sdvo_cmd_names[] = {
+} __attribute__ ((packed)) sdvo_cmd_names[] = {
 	SDVO_CMD_NAME_ENTRY(SDVO_CMD_RESET),
 	SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_DEVICE_CAPS),
 	SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_FIRMWARE_REV),
@@ -1133,7 +1133,7 @@ static bool intel_sdvo_compute_config(struct intel_encoder *encoder,
 	DRM_DEBUG_KMS("forcing bpc to 8 for SDVO\n");
 	pipe_config->pipe_bpp = 8*3;
 
-	if (HAS_PCH_SPLIT(encoder->base.dev))
+	if (HAS_PCH_SPLIT(to_i915(encoder->base.dev)))
 		pipe_config->has_pch_encoder = true;
 
 	/* We need to construct preferred input timings based on our
@@ -1273,7 +1273,7 @@ static void intel_sdvo_pre_enable(struct intel_encoder *intel_encoder,
 		/* The real mode polarity is set by the SDVO commands, using
 		 * struct intel_sdvo_dtd. */
 		sdvox = SDVO_VSYNC_ACTIVE_HIGH | SDVO_HSYNC_ACTIVE_HIGH;
-		if (!HAS_PCH_SPLIT(dev) && crtc_state->limited_color_range)
+		if (!HAS_PCH_SPLIT(dev_priv) && crtc_state->limited_color_range)
 			sdvox |= HDMI_COLOR_RANGE_16_235;
 		if (INTEL_INFO(dev)->gen < 5)
 			sdvox |= SDVO_BORDER_ENABLE;
@@ -1286,7 +1286,7 @@ static void intel_sdvo_pre_enable(struct intel_encoder *intel_encoder,
 		sdvox |= (9 << 19) | SDVO_BORDER_ENABLE;
 	}
 
-	if (INTEL_PCH_TYPE(dev) >= PCH_CPT)
+	if (INTEL_PCH_TYPE(dev_priv) >= PCH_CPT)
 		sdvox |= SDVO_PIPE_SEL_CPT(crtc->pipe);
 	else
 		sdvox |= SDVO_PIPE_SEL(crtc->pipe);
@@ -1296,7 +1296,8 @@ static void intel_sdvo_pre_enable(struct intel_encoder *intel_encoder,
 
 	if (INTEL_INFO(dev)->gen >= 4) {
 		/* done in crtc_mode_set as the dpll_md reg must be written early */
-	} else if (IS_I945G(dev) || IS_I945GM(dev) || IS_G33(dev)) {
+	} else if (IS_I945G(dev_priv) || IS_I945GM(dev_priv) ||
+		   IS_G33(dev_priv)) {
 		/* done in crtc_mode_set as it lives inside the dpll register */
 	} else {
 		sdvox |= (crtc_state->pixel_multiplier - 1)
@@ -1339,7 +1340,7 @@ static bool intel_sdvo_get_hw_state(struct intel_encoder *encoder,
 	if (!(tmp & SDVO_ENABLE) && (active_outputs == 0))
 		return false;
 
-	if (HAS_PCH_CPT(dev))
+	if (HAS_PCH_CPT(dev_priv))
 		*pipe = PORT_TO_PIPE_CPT(tmp);
 	else
 		*pipe = PORT_TO_PIPE(tmp);
@@ -1389,7 +1390,7 @@ static void intel_sdvo_get_config(struct intel_encoder *encoder,
 	 * encoder->get_config we so already have a valid pixel multplier on all
 	 * other platfroms.
 	 */
-	if (IS_I915G(dev) || IS_I915GM(dev)) {
+	if (IS_I915G(dev_priv) || IS_I915GM(dev_priv)) {
 		pipe_config->pixel_multiplier =
 			((sdvox & SDVO_PORT_MULTIPLY_MASK)
 			 >> SDVO_PORT_MULTIPLY_SHIFT) + 1;
@@ -1595,15 +1596,15 @@ static bool intel_sdvo_get_capabilities(struct intel_sdvo *intel_sdvo, struct in
 
 static uint16_t intel_sdvo_get_hotplug_support(struct intel_sdvo *intel_sdvo)
 {
-	struct drm_device *dev = intel_sdvo->base.base.dev;
+	struct drm_i915_private *dev_priv = to_i915(intel_sdvo->base.base.dev);
 	uint16_t hotplug;
 
-	if (!I915_HAS_HOTPLUG(dev))
+	if (!I915_HAS_HOTPLUG(dev_priv))
 		return 0;
 
 	/* HW Erratum: SDVO Hotplug is broken on all i945G chips, there's noise
 	 * on the line. */
-	if (IS_I945G(dev) || IS_I945GM(dev))
+	if (IS_I945G(dev_priv) || IS_I945GM(dev_priv))
 		return 0;
 
 	if (!intel_sdvo_get_value(intel_sdvo, SDVO_CMD_GET_HOT_PLUG_SUPPORT,
@@ -2981,6 +2982,7 @@ bool intel_sdvo_init(struct drm_device *dev,
 	/* encoder type will be decided later */
 	intel_encoder = &intel_sdvo->base;
 	intel_encoder->type = INTEL_OUTPUT_SDVO;
+	intel_encoder->port = port;
 	drm_encoder_init(dev, &intel_encoder->base, &intel_sdvo_enc_funcs, 0,
 			 "SDVO %c", port_name(port));
 
@@ -2996,7 +2998,7 @@ bool intel_sdvo_init(struct drm_device *dev,
 	}
 
 	intel_encoder->compute_config = intel_sdvo_compute_config;
-	if (HAS_PCH_SPLIT(dev)) {
+	if (HAS_PCH_SPLIT(dev_priv)) {
 		intel_encoder->disable = pch_disable_sdvo;
 		intel_encoder->post_disable = pch_post_disable_sdvo;
 	} else {
diff --git a/drivers/gpu/drm/i915/intel_sprite.c b/drivers/gpu/drm/i915/intel_sprite.c
index 3ea6419e18b9..43d0350856e7 100644
--- a/drivers/gpu/drm/i915/intel_sprite.c
+++ b/drivers/gpu/drm/i915/intel_sprite.c
@@ -208,6 +208,8 @@ skl_update_plane(struct drm_plane *drm_plane,
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	const int pipe = intel_plane->pipe;
 	const int plane = intel_plane->plane + 1;
+	const struct skl_plane_wm *p_wm =
+		&crtc_state->wm.skl.optimal.planes[plane];
 	u32 plane_ctl;
 	const struct drm_intel_sprite_colorkey *key = &plane_state->ckey;
 	u32 surf_addr = plane_state->main.offset;
@@ -232,7 +234,7 @@ skl_update_plane(struct drm_plane *drm_plane,
 	plane_ctl |= skl_plane_ctl_rotation(rotation);
 
 	if (wm->dirty_pipes & drm_crtc_mask(crtc))
-		skl_write_plane_wm(intel_crtc, wm, plane);
+		skl_write_plane_wm(intel_crtc, p_wm, &wm->ddb, plane);
 
 	if (key->flags) {
 		I915_WRITE(PLANE_KEYVAL(pipe, plane), key->min_value);
@@ -289,6 +291,7 @@ skl_disable_plane(struct drm_plane *dplane, struct drm_crtc *crtc)
 	struct drm_device *dev = dplane->dev;
 	struct drm_i915_private *dev_priv = to_i915(dev);
 	struct intel_plane *intel_plane = to_intel_plane(dplane);
+	struct intel_crtc_state *cstate = to_intel_crtc_state(crtc->state);
 	const int pipe = intel_plane->pipe;
 	const int plane = intel_plane->plane + 1;
 
@@ -298,7 +301,8 @@ skl_disable_plane(struct drm_plane *dplane, struct drm_crtc *crtc)
 	 */
 	if (!dplane->state->visible)
 		skl_write_plane_wm(to_intel_crtc(crtc),
-				   &dev_priv->wm.skl_results, plane);
+				   &cstate->wm.skl.optimal.planes[plane],
+				   &dev_priv->wm.skl_results.ddb, plane);
 
 	I915_WRITE(PLANE_CTL(pipe, plane), 0);
 
@@ -450,7 +454,7 @@ vlv_update_plane(struct drm_plane *dplane,
 	if (key->flags & I915_SET_COLORKEY_SOURCE)
 		sprctl |= SP_SOURCE_KEY;
 
-	if (IS_CHERRYVIEW(dev) && pipe == PIPE_B)
+	if (IS_CHERRYVIEW(dev_priv) && pipe == PIPE_B)
 		chv_update_csc(intel_plane, fb->pixel_format);
 
 	I915_WRITE(SPSTRIDE(pipe, plane), fb->pitches[0]);
@@ -542,12 +546,12 @@ ivb_update_plane(struct drm_plane *plane,
 	if (fb->modifier[0] == I915_FORMAT_MOD_X_TILED)
 		sprctl |= SPRITE_TILED;
 
-	if (IS_HASWELL(dev) || IS_BROADWELL(dev))
+	if (IS_HASWELL(dev_priv) || IS_BROADWELL(dev_priv))
 		sprctl &= ~SPRITE_TRICKLE_FEED_DISABLE;
 	else
 		sprctl |= SPRITE_TRICKLE_FEED_DISABLE;
 
-	if (IS_HASWELL(dev) || IS_BROADWELL(dev))
+	if (IS_HASWELL(dev_priv) || IS_BROADWELL(dev_priv))
 		sprctl |= SPRITE_PIPE_CSC_ENABLE;
 
 	/* Sizes are 0 based */
@@ -566,7 +570,7 @@ ivb_update_plane(struct drm_plane *plane,
 		sprctl |= SPRITE_ROTATE_180;
 
 		/* HSW and BDW does this automagically in hardware */
-		if (!IS_HASWELL(dev) && !IS_BROADWELL(dev)) {
+		if (!IS_HASWELL(dev_priv) && !IS_BROADWELL(dev_priv)) {
 			x += src_w;
 			y += src_h;
 		}
@@ -590,7 +594,7 @@ ivb_update_plane(struct drm_plane *plane,
 
 	/* HSW consolidates SPRTILEOFF and SPRLINOFF into a single SPROFFSET
 	 * register */
-	if (IS_HASWELL(dev) || IS_BROADWELL(dev))
+	if (IS_HASWELL(dev_priv) || IS_BROADWELL(dev_priv))
 		I915_WRITE(SPROFFSET(pipe), (y << 16) | x);
 	else if (fb->modifier[0] == I915_FORMAT_MOD_X_TILED)
 		I915_WRITE(SPRTILEOFF(pipe), (y << 16) | x);
@@ -680,7 +684,7 @@ ilk_update_plane(struct drm_plane *plane,
 	if (fb->modifier[0] == I915_FORMAT_MOD_X_TILED)
 		dvscntr |= DVS_TILED;
 
-	if (IS_GEN6(dev))
+	if (IS_GEN6(dev_priv))
 		dvscntr |= DVS_TRICKLE_FEED_DISABLE; /* must disable */
 
 	/* Sizes are 0 based */
@@ -753,7 +757,7 @@ intel_check_sprite_plane(struct drm_plane *plane,
 			 struct intel_crtc_state *crtc_state,
 			 struct intel_plane_state *state)
 {
-	struct drm_device *dev = plane->dev;
+	struct drm_i915_private *dev_priv = to_i915(plane->dev);
 	struct drm_crtc *crtc = state->base.crtc;
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	struct intel_plane *intel_plane = to_intel_plane(plane);
@@ -797,7 +801,7 @@ intel_check_sprite_plane(struct drm_plane *plane,
 	}
 
 	/* setup can_scale, min_scale, max_scale */
-	if (INTEL_INFO(dev)->gen >= 9) {
+	if (INTEL_GEN(dev_priv) >= 9) {
 		/* use scaler when colorkey is not required */
 		if (state->ckey.flags == I915_SET_COLORKEY_NONE) {
 			can_scale = 1;
@@ -913,7 +917,7 @@ intel_check_sprite_plane(struct drm_plane *plane,
 
 		width_bytes = ((src_x * cpp) & 63) + src_w * cpp;
 
-		if (INTEL_INFO(dev)->gen < 9 && (src_w > 2048 || src_h > 2048 ||
+		if (INTEL_GEN(dev_priv) < 9 && (src_w > 2048 || src_h > 2048 ||
 		    width_bytes > 4096 || fb->pitches[0] > 4096)) {
 			DRM_DEBUG_KMS("Source dimensions exceed hardware limits\n");
 			return -EINVAL;
@@ -932,7 +936,7 @@ intel_check_sprite_plane(struct drm_plane *plane,
 	dst->y1 = crtc_y;
 	dst->y2 = crtc_y + crtc_h;
 
-	if (INTEL_GEN(dev) >= 9) {
+	if (INTEL_GEN(dev_priv) >= 9) {
 		ret = skl_check_plane_surface(state);
 		if (ret)
 			return ret;
@@ -944,6 +948,7 @@ intel_check_sprite_plane(struct drm_plane *plane,
 int intel_sprite_set_colorkey(struct drm_device *dev, void *data,
 			      struct drm_file *file_priv)
 {
+	struct drm_i915_private *dev_priv = to_i915(dev);
 	struct drm_intel_sprite_colorkey *set = data;
 	struct drm_plane *plane;
 	struct drm_plane_state *plane_state;
@@ -955,7 +960,7 @@ int intel_sprite_set_colorkey(struct drm_device *dev, void *data,
 	if ((set->flags & (I915_SET_COLORKEY_DESTINATION | I915_SET_COLORKEY_SOURCE)) == (I915_SET_COLORKEY_DESTINATION | I915_SET_COLORKEY_SOURCE))
 		return -EINVAL;
 
-	if ((IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) &&
+	if ((IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv)) &&
 	    set->flags & I915_SET_COLORKEY_DESTINATION)
 		return -EINVAL;
 
@@ -1040,6 +1045,7 @@ static uint32_t skl_plane_formats[] = {
 int
 intel_plane_init(struct drm_device *dev, enum pipe pipe, int plane)
 {
+	struct drm_i915_private *dev_priv = to_i915(dev);
 	struct intel_plane *intel_plane = NULL;
 	struct intel_plane_state *state = NULL;
 	unsigned long possible_crtcs;
@@ -1072,7 +1078,7 @@ intel_plane_init(struct drm_device *dev, enum pipe pipe, int plane)
 		intel_plane->update_plane = ilk_update_plane;
 		intel_plane->disable_plane = ilk_disable_plane;
 
-		if (IS_GEN6(dev)) {
+		if (IS_GEN6(dev_priv)) {
 			plane_formats = snb_plane_formats;
 			num_plane_formats = ARRAY_SIZE(snb_plane_formats);
 		} else {
@@ -1083,7 +1089,7 @@ intel_plane_init(struct drm_device *dev, enum pipe pipe, int plane)
 
 	case 7:
 	case 8:
-		if (IS_IVYBRIDGE(dev)) {
+		if (IS_IVYBRIDGE(dev_priv)) {
 			intel_plane->can_scale = true;
 			intel_plane->max_downscale = 2;
 		} else {
@@ -1091,7 +1097,7 @@ intel_plane_init(struct drm_device *dev, enum pipe pipe, int plane)
 			intel_plane->max_downscale = 1;
 		}
 
-		if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) {
+		if (IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv)) {
 			intel_plane->update_plane = vlv_update_plane;
 			intel_plane->disable_plane = vlv_disable_plane;
 
@@ -1120,7 +1126,7 @@ intel_plane_init(struct drm_device *dev, enum pipe pipe, int plane)
 		goto fail;
 	}
 
-	if (INTEL_GEN(dev) >= 9) {
+	if (INTEL_GEN(dev_priv) >= 9) {
 		supported_rotations =
 			DRM_ROTATE_0 | DRM_ROTATE_90 |
 			DRM_ROTATE_180 | DRM_ROTATE_270;
diff --git a/drivers/gpu/drm/i915/intel_tv.c b/drivers/gpu/drm/i915/intel_tv.c
index d960e4866595..7118fb55f57f 100644
--- a/drivers/gpu/drm/i915/intel_tv.c
+++ b/drivers/gpu/drm/i915/intel_tv.c
@@ -86,7 +86,8 @@ struct intel_tv {
 };
 
 struct video_levels {
-	int blank, black, burst;
+	u16 blank, black;
+	u8 burst;
 };
 
 struct color_conversion {
@@ -339,34 +340,43 @@ static const struct video_levels component_levels = {
 
 struct tv_mode {
 	const char *name;
-	int clock;
-	int refresh; /* in millihertz (for precision) */
+
+	u32 clock;
+	u16 refresh; /* in millihertz (for precision) */
 	u32 oversample;
-	int hsync_end, hblank_start, hblank_end, htotal;
-	bool progressive, trilevel_sync, component_only;
-	int vsync_start_f1, vsync_start_f2, vsync_len;
-	bool veq_ena;
-	int veq_start_f1, veq_start_f2, veq_len;
-	int vi_end_f1, vi_end_f2, nbr_end;
-	bool burst_ena;
-	int hburst_start, hburst_len;
-	int vburst_start_f1, vburst_end_f1;
-	int vburst_start_f2, vburst_end_f2;
-	int vburst_start_f3, vburst_end_f3;
-	int vburst_start_f4, vburst_end_f4;
+	u8 hsync_end;
+	u16 hblank_start, hblank_end, htotal;
+	bool progressive : 1, trilevel_sync : 1, component_only : 1;
+	u8 vsync_start_f1, vsync_start_f2, vsync_len;
+	bool veq_ena : 1;
+	u8 veq_start_f1, veq_start_f2, veq_len;
+	u8 vi_end_f1, vi_end_f2;
+	u16 nbr_end;
+	bool burst_ena : 1;
+	u8 hburst_start, hburst_len;
+	u8 vburst_start_f1;
+	u16 vburst_end_f1;
+	u8 vburst_start_f2;
+	u16 vburst_end_f2;
+	u8 vburst_start_f3;
+	u16 vburst_end_f3;
+	u8 vburst_start_f4;
+	u16 vburst_end_f4;
 	/*
 	 * subcarrier programming
 	 */
-	int dda2_size, dda3_size, dda1_inc, dda2_inc, dda3_inc;
+	u16 dda2_size, dda3_size;
+	u8 dda1_inc;
+	u16 dda2_inc, dda3_inc;
 	u32 sc_reset;
-	bool pal_burst;
+	bool pal_burst : 1;
 	/*
 	 * blank/black levels
 	 */
 	const struct video_levels *composite_levels, *svideo_levels;
 	const struct color_conversion *composite_color, *svideo_color;
 	const u32 *filter_table;
-	int max_srcw;
+	u16 max_srcw;
 };
 
 
@@ -1095,7 +1105,7 @@ static void intel_tv_pre_enable(struct intel_encoder *encoder,
 		tv_mode->dda3_inc << TV_SCDDA3_INC_SHIFT;
 
 	/* Enable two fixes for the chips that need them. */
-	if (IS_I915GM(dev))
+	if (IS_I915GM(dev_priv))
 		tv_ctl |= TV_ENC_C0_FIX | TV_ENC_SDP_FIX;
 
 	set_tv_mode_timings(dev_priv, tv_mode, burst_ena);
@@ -1220,7 +1230,7 @@ intel_tv_detect_type(struct intel_tv *intel_tv,
 	 * The TV sense state should be cleared to zero on cantiga platform. Otherwise
 	 * the TV is misdetected. This is hardware requirement.
 	 */
-	if (IS_GM45(dev))
+	if (IS_GM45(dev_priv))
 		tv_dac &= ~(TVDAC_STATE_CHG_EN | TVDAC_A_SENSE_CTL |
 			    TVDAC_B_SENSE_CTL | TVDAC_C_SENSE_CTL);
 
@@ -1610,7 +1620,9 @@ intel_tv_init(struct drm_device *dev)
 	intel_connector->get_hw_state = intel_connector_get_hw_state;
 
 	intel_connector_attach_encoder(intel_connector, intel_encoder);
+
 	intel_encoder->type = INTEL_OUTPUT_TVOUT;
+	intel_encoder->port = PORT_NONE;
 	intel_encoder->crtc_mask = (1 << 0) | (1 << 1);
 	intel_encoder->cloneable = 0;
 	intel_encoder->base.possible_crtcs = ((1 << 0) | (1 << 1));
diff --git a/drivers/gpu/drm/i915/intel_uncore.c b/drivers/gpu/drm/i915/intel_uncore.c
index a9b6c936aadd..e2b188dcf908 100644
--- a/drivers/gpu/drm/i915/intel_uncore.c
+++ b/drivers/gpu/drm/i915/intel_uncore.c
@@ -231,19 +231,21 @@ intel_uncore_fw_release_timer(struct hrtimer *timer)
 {
 	struct intel_uncore_forcewake_domain *domain =
 	       container_of(timer, struct intel_uncore_forcewake_domain, timer);
+	struct drm_i915_private *dev_priv = domain->i915;
 	unsigned long irqflags;
 
-	assert_rpm_device_not_suspended(domain->i915);
+	assert_rpm_device_not_suspended(dev_priv);
 
-	spin_lock_irqsave(&domain->i915->uncore.lock, irqflags);
+	spin_lock_irqsave(&dev_priv->uncore.lock, irqflags);
 	if (WARN_ON(domain->wake_count == 0))
 		domain->wake_count++;
 
-	if (--domain->wake_count == 0)
-		domain->i915->uncore.funcs.force_wake_put(domain->i915,
-							  1 << domain->id);
+	if (--domain->wake_count == 0) {
+		dev_priv->uncore.funcs.force_wake_put(dev_priv, domain->mask);
+		dev_priv->uncore.fw_domains_active &= ~domain->mask;
+	}
 
-	spin_unlock_irqrestore(&domain->i915->uncore.lock, irqflags);
+	spin_unlock_irqrestore(&dev_priv->uncore.lock, irqflags);
 
 	return HRTIMER_NORESTART;
 }
@@ -254,7 +256,7 @@ void intel_uncore_forcewake_reset(struct drm_i915_private *dev_priv,
 	unsigned long irqflags;
 	struct intel_uncore_forcewake_domain *domain;
 	int retry_count = 100;
-	enum forcewake_domains fw = 0, active_domains;
+	enum forcewake_domains fw, active_domains;
 
 	/* Hold uncore.lock across reset to prevent any register access
 	 * with forcewake not set correctly. Wait until all pending
@@ -291,10 +293,7 @@ void intel_uncore_forcewake_reset(struct drm_i915_private *dev_priv,
 
 	WARN_ON(active_domains);
 
-	for_each_fw_domain(domain, dev_priv)
-		if (domain->wake_count)
-			fw |= domain->mask;
-
+	fw = dev_priv->uncore.fw_domains_active;
 	if (fw)
 		dev_priv->uncore.funcs.force_wake_put(dev_priv, fw);
 
@@ -443,9 +442,6 @@ static void __intel_uncore_forcewake_get(struct drm_i915_private *dev_priv,
 {
 	struct intel_uncore_forcewake_domain *domain;
 
-	if (!dev_priv->uncore.funcs.force_wake_get)
-		return;
-
 	fw_domains &= dev_priv->uncore.fw_domains;
 
 	for_each_fw_domain_masked(domain, fw_domains, dev_priv) {
@@ -453,8 +449,10 @@ static void __intel_uncore_forcewake_get(struct drm_i915_private *dev_priv,
 			fw_domains &= ~domain->mask;
 	}
 
-	if (fw_domains)
+	if (fw_domains) {
 		dev_priv->uncore.funcs.force_wake_get(dev_priv, fw_domains);
+		dev_priv->uncore.fw_domains_active |= fw_domains;
+	}
 }
 
 /**
@@ -509,9 +507,6 @@ static void __intel_uncore_forcewake_put(struct drm_i915_private *dev_priv,
 {
 	struct intel_uncore_forcewake_domain *domain;
 
-	if (!dev_priv->uncore.funcs.force_wake_put)
-		return;
-
 	fw_domains &= dev_priv->uncore.fw_domains;
 
 	for_each_fw_domain_masked(domain, fw_domains, dev_priv) {
@@ -567,13 +562,10 @@ void intel_uncore_forcewake_put__locked(struct drm_i915_private *dev_priv,
 
 void assert_forcewakes_inactive(struct drm_i915_private *dev_priv)
 {
-	struct intel_uncore_forcewake_domain *domain;
-
 	if (!dev_priv->uncore.funcs.force_wake_get)
 		return;
 
-	for_each_fw_domain(domain, dev_priv)
-		WARN_ON(domain->wake_count);
+	WARN_ON(dev_priv->uncore.fw_domains_active);
 }
 
 /* We give fast paths for the really cool registers */
@@ -589,49 +581,146 @@ void assert_forcewakes_inactive(struct drm_i915_private *dev_priv)
 	__fwd; \
 })
 
-#define REG_RANGE(reg, start, end) ((reg) >= (start) && (reg) < (end))
+static int fw_range_cmp(u32 offset, const struct intel_forcewake_range *entry)
+{
+	if (offset < entry->start)
+		return -1;
+	else if (offset > entry->end)
+		return 1;
+	else
+		return 0;
+}
+
+/* Copied and "macroized" from lib/bsearch.c */
+#define BSEARCH(key, base, num, cmp) ({                                 \
+	unsigned int start__ = 0, end__ = (num);                        \
+	typeof(base) result__ = NULL;                                   \
+	while (start__ < end__) {                                       \
+		unsigned int mid__ = start__ + (end__ - start__) / 2;   \
+		int ret__ = (cmp)((key), (base) + mid__);               \
+		if (ret__ < 0) {                                        \
+			end__ = mid__;                                  \
+		} else if (ret__ > 0) {                                 \
+			start__ = mid__ + 1;                            \
+		} else {                                                \
+			result__ = (base) + mid__;                      \
+			break;                                          \
+		}                                                       \
+	}                                                               \
+	result__;                                                       \
+})
+
+static enum forcewake_domains
+find_fw_domain(struct drm_i915_private *dev_priv, u32 offset)
+{
+	const struct intel_forcewake_range *entry;
+
+	entry = BSEARCH(offset,
+			dev_priv->uncore.fw_domains_table,
+			dev_priv->uncore.fw_domains_table_entries,
+			fw_range_cmp);
 
-#define FORCEWAKE_VLV_RENDER_RANGE_OFFSET(reg) \
-	(REG_RANGE((reg), 0x2000, 0x4000) || \
-	 REG_RANGE((reg), 0x5000, 0x8000) || \
-	 REG_RANGE((reg), 0xB000, 0x12000) || \
-	 REG_RANGE((reg), 0x2E000, 0x30000))
+	return entry ? entry->domains : 0;
+}
+
+static void
+intel_fw_table_check(struct drm_i915_private *dev_priv)
+{
+	const struct intel_forcewake_range *ranges;
+	unsigned int num_ranges;
+	s32 prev;
+	unsigned int i;
+
+	if (!IS_ENABLED(CONFIG_DRM_I915_DEBUG))
+		return;
+
+	ranges = dev_priv->uncore.fw_domains_table;
+	if (!ranges)
+		return;
+
+	num_ranges = dev_priv->uncore.fw_domains_table_entries;
+
+	for (i = 0, prev = -1; i < num_ranges; i++, ranges++) {
+		WARN_ON_ONCE(prev >= (s32)ranges->start);
+		prev = ranges->start;
+		WARN_ON_ONCE(prev >= (s32)ranges->end);
+		prev = ranges->end;
+	}
+}
 
-#define FORCEWAKE_VLV_MEDIA_RANGE_OFFSET(reg) \
-	(REG_RANGE((reg), 0x12000, 0x14000) || \
-	 REG_RANGE((reg), 0x22000, 0x24000) || \
-	 REG_RANGE((reg), 0x30000, 0x40000))
+#define GEN_FW_RANGE(s, e, d) \
+	{ .start = (s), .end = (e), .domains = (d) }
 
-#define __vlv_reg_read_fw_domains(offset) \
+#define HAS_FWTABLE(dev_priv) \
+	(IS_GEN9(dev_priv) || \
+	 IS_CHERRYVIEW(dev_priv) || \
+	 IS_VALLEYVIEW(dev_priv))
+
+/* *Must* be sorted by offset ranges! See intel_fw_table_check(). */
+static const struct intel_forcewake_range __vlv_fw_ranges[] = {
+	GEN_FW_RANGE(0x2000, 0x3fff, FORCEWAKE_RENDER),
+	GEN_FW_RANGE(0x5000, 0x7fff, FORCEWAKE_RENDER),
+	GEN_FW_RANGE(0xb000, 0x11fff, FORCEWAKE_RENDER),
+	GEN_FW_RANGE(0x12000, 0x13fff, FORCEWAKE_MEDIA),
+	GEN_FW_RANGE(0x22000, 0x23fff, FORCEWAKE_MEDIA),
+	GEN_FW_RANGE(0x2e000, 0x2ffff, FORCEWAKE_RENDER),
+	GEN_FW_RANGE(0x30000, 0x3ffff, FORCEWAKE_MEDIA),
+};
+
+#define __fwtable_reg_read_fw_domains(offset) \
 ({ \
 	enum forcewake_domains __fwd = 0; \
-	if (!NEEDS_FORCE_WAKE(offset)) \
-		__fwd = 0; \
-	else if (FORCEWAKE_VLV_RENDER_RANGE_OFFSET(offset)) \
-		__fwd = FORCEWAKE_RENDER; \
-	else if (FORCEWAKE_VLV_MEDIA_RANGE_OFFSET(offset)) \
-		__fwd = FORCEWAKE_MEDIA; \
+	if (NEEDS_FORCE_WAKE((offset))) \
+		__fwd = find_fw_domain(dev_priv, offset); \
 	__fwd; \
 })
 
+/* *Must* be sorted by offset! See intel_shadow_table_check(). */
 static const i915_reg_t gen8_shadowed_regs[] = {
-	GEN6_RPNSWREQ,
-	GEN6_RC_VIDEO_FREQ,
-	RING_TAIL(RENDER_RING_BASE),
-	RING_TAIL(GEN6_BSD_RING_BASE),
-	RING_TAIL(VEBOX_RING_BASE),
-	RING_TAIL(BLT_RING_BASE),
+	RING_TAIL(RENDER_RING_BASE),	/* 0x2000 (base) */
+	GEN6_RPNSWREQ,			/* 0xA008 */
+	GEN6_RC_VIDEO_FREQ,		/* 0xA00C */
+	RING_TAIL(GEN6_BSD_RING_BASE),	/* 0x12000 (base) */
+	RING_TAIL(VEBOX_RING_BASE),	/* 0x1a000 (base) */
+	RING_TAIL(BLT_RING_BASE),	/* 0x22000 (base) */
 	/* TODO: Other registers are not yet used */
 };
 
+static void intel_shadow_table_check(void)
+{
+	const i915_reg_t *reg = gen8_shadowed_regs;
+	s32 prev;
+	u32 offset;
+	unsigned int i;
+
+	if (!IS_ENABLED(CONFIG_DRM_I915_DEBUG))
+		return;
+
+	for (i = 0, prev = -1; i < ARRAY_SIZE(gen8_shadowed_regs); i++, reg++) {
+		offset = i915_mmio_reg_offset(*reg);
+		WARN_ON_ONCE(prev >= (s32)offset);
+		prev = offset;
+	}
+}
+
+static int mmio_reg_cmp(u32 key, const i915_reg_t *reg)
+{
+	u32 offset = i915_mmio_reg_offset(*reg);
+
+	if (key < offset)
+		return -1;
+	else if (key > offset)
+		return 1;
+	else
+		return 0;
+}
+
 static bool is_gen8_shadowed(u32 offset)
 {
-	int i;
-	for (i = 0; i < ARRAY_SIZE(gen8_shadowed_regs); i++)
-		if (offset == gen8_shadowed_regs[i].reg)
-			return true;
+	const i915_reg_t *regs = gen8_shadowed_regs;
 
-	return false;
+	return BSEARCH(offset, regs, ARRAY_SIZE(gen8_shadowed_regs),
+		       mmio_reg_cmp);
 }
 
 #define __gen8_reg_write_fw_domains(offset) \
@@ -644,143 +733,70 @@ static bool is_gen8_shadowed(u32 offset)
 	__fwd; \
 })
 
-#define FORCEWAKE_CHV_RENDER_RANGE_OFFSET(reg) \
-	(REG_RANGE((reg), 0x2000, 0x4000) || \
-	 REG_RANGE((reg), 0x5200, 0x8000) || \
-	 REG_RANGE((reg), 0x8300, 0x8500) || \
-	 REG_RANGE((reg), 0xB000, 0xB480) || \
-	 REG_RANGE((reg), 0xE000, 0xE800))
-
-#define FORCEWAKE_CHV_MEDIA_RANGE_OFFSET(reg) \
-	(REG_RANGE((reg), 0x8800, 0x8900) || \
-	 REG_RANGE((reg), 0xD000, 0xD800) || \
-	 REG_RANGE((reg), 0x12000, 0x14000) || \
-	 REG_RANGE((reg), 0x1A000, 0x1C000) || \
-	 REG_RANGE((reg), 0x1E800, 0x1EA00) || \
-	 REG_RANGE((reg), 0x30000, 0x38000))
-
-#define FORCEWAKE_CHV_COMMON_RANGE_OFFSET(reg) \
-	(REG_RANGE((reg), 0x4000, 0x5000) || \
-	 REG_RANGE((reg), 0x8000, 0x8300) || \
-	 REG_RANGE((reg), 0x8500, 0x8600) || \
-	 REG_RANGE((reg), 0x9000, 0xB000) || \
-	 REG_RANGE((reg), 0xF000, 0x10000))
-
-#define __chv_reg_read_fw_domains(offset) \
-({ \
-	enum forcewake_domains __fwd = 0; \
-	if (!NEEDS_FORCE_WAKE(offset)) \
-		__fwd = 0; \
-	else if (FORCEWAKE_CHV_RENDER_RANGE_OFFSET(offset)) \
-		__fwd = FORCEWAKE_RENDER; \
-	else if (FORCEWAKE_CHV_MEDIA_RANGE_OFFSET(offset)) \
-		__fwd = FORCEWAKE_MEDIA; \
-	else if (FORCEWAKE_CHV_COMMON_RANGE_OFFSET(offset)) \
-		__fwd = FORCEWAKE_RENDER | FORCEWAKE_MEDIA; \
-	__fwd; \
-})
+/* *Must* be sorted by offset ranges! See intel_fw_table_check(). */
+static const struct intel_forcewake_range __chv_fw_ranges[] = {
+	GEN_FW_RANGE(0x2000, 0x3fff, FORCEWAKE_RENDER),
+	GEN_FW_RANGE(0x4000, 0x4fff, FORCEWAKE_RENDER | FORCEWAKE_MEDIA),
+	GEN_FW_RANGE(0x5200, 0x7fff, FORCEWAKE_RENDER),
+	GEN_FW_RANGE(0x8000, 0x82ff, FORCEWAKE_RENDER | FORCEWAKE_MEDIA),
+	GEN_FW_RANGE(0x8300, 0x84ff, FORCEWAKE_RENDER),
+	GEN_FW_RANGE(0x8500, 0x85ff, FORCEWAKE_RENDER | FORCEWAKE_MEDIA),
+	GEN_FW_RANGE(0x8800, 0x88ff, FORCEWAKE_MEDIA),
+	GEN_FW_RANGE(0x9000, 0xafff, FORCEWAKE_RENDER | FORCEWAKE_MEDIA),
+	GEN_FW_RANGE(0xb000, 0xb47f, FORCEWAKE_RENDER),
+	GEN_FW_RANGE(0xd000, 0xd7ff, FORCEWAKE_MEDIA),
+	GEN_FW_RANGE(0xe000, 0xe7ff, FORCEWAKE_RENDER),
+	GEN_FW_RANGE(0xf000, 0xffff, FORCEWAKE_RENDER | FORCEWAKE_MEDIA),
+	GEN_FW_RANGE(0x12000, 0x13fff, FORCEWAKE_MEDIA),
+	GEN_FW_RANGE(0x1a000, 0x1bfff, FORCEWAKE_MEDIA),
+	GEN_FW_RANGE(0x1e800, 0x1e9ff, FORCEWAKE_MEDIA),
+	GEN_FW_RANGE(0x30000, 0x37fff, FORCEWAKE_MEDIA),
+};
 
-#define __chv_reg_write_fw_domains(offset) \
+#define __fwtable_reg_write_fw_domains(offset) \
 ({ \
 	enum forcewake_domains __fwd = 0; \
-	if (!NEEDS_FORCE_WAKE(offset) || is_gen8_shadowed(offset)) \
-		__fwd = 0; \
-	else if (FORCEWAKE_CHV_RENDER_RANGE_OFFSET(offset)) \
-		__fwd = FORCEWAKE_RENDER; \
-	else if (FORCEWAKE_CHV_MEDIA_RANGE_OFFSET(offset)) \
-		__fwd = FORCEWAKE_MEDIA; \
-	else if (FORCEWAKE_CHV_COMMON_RANGE_OFFSET(offset)) \
-		__fwd = FORCEWAKE_RENDER | FORCEWAKE_MEDIA; \
-	__fwd; \
-})
-
-#define FORCEWAKE_GEN9_UNCORE_RANGE_OFFSET(reg) \
-	REG_RANGE((reg), 0xB00,  0x2000)
-
-#define FORCEWAKE_GEN9_RENDER_RANGE_OFFSET(reg) \
-	(REG_RANGE((reg), 0x2000, 0x2700) || \
-	 REG_RANGE((reg), 0x3000, 0x4000) || \
-	 REG_RANGE((reg), 0x5200, 0x8000) || \
-	 REG_RANGE((reg), 0x8140, 0x8160) || \
-	 REG_RANGE((reg), 0x8300, 0x8500) || \
-	 REG_RANGE((reg), 0x8C00, 0x8D00) || \
-	 REG_RANGE((reg), 0xB000, 0xB480) || \
-	 REG_RANGE((reg), 0xE000, 0xE900) || \
-	 REG_RANGE((reg), 0x24400, 0x24800))
-
-#define FORCEWAKE_GEN9_MEDIA_RANGE_OFFSET(reg) \
-	(REG_RANGE((reg), 0x8130, 0x8140) || \
-	 REG_RANGE((reg), 0x8800, 0x8A00) || \
-	 REG_RANGE((reg), 0xD000, 0xD800) || \
-	 REG_RANGE((reg), 0x12000, 0x14000) || \
-	 REG_RANGE((reg), 0x1A000, 0x1EA00) || \
-	 REG_RANGE((reg), 0x30000, 0x40000))
-
-#define FORCEWAKE_GEN9_COMMON_RANGE_OFFSET(reg) \
-	REG_RANGE((reg), 0x9400, 0x9800)
-
-#define FORCEWAKE_GEN9_BLITTER_RANGE_OFFSET(reg) \
-	((reg) < 0x40000 && \
-	 !FORCEWAKE_GEN9_UNCORE_RANGE_OFFSET(reg) && \
-	 !FORCEWAKE_GEN9_RENDER_RANGE_OFFSET(reg) && \
-	 !FORCEWAKE_GEN9_MEDIA_RANGE_OFFSET(reg) && \
-	 !FORCEWAKE_GEN9_COMMON_RANGE_OFFSET(reg))
-
-#define SKL_NEEDS_FORCE_WAKE(reg) \
-	((reg) < 0x40000 && !FORCEWAKE_GEN9_UNCORE_RANGE_OFFSET(reg))
-
-#define __gen9_reg_read_fw_domains(offset) \
-({ \
-	enum forcewake_domains __fwd; \
-	if (!SKL_NEEDS_FORCE_WAKE(offset)) \
-		__fwd = 0; \
-	else if (FORCEWAKE_GEN9_RENDER_RANGE_OFFSET(offset)) \
-		__fwd = FORCEWAKE_RENDER; \
-	else if (FORCEWAKE_GEN9_MEDIA_RANGE_OFFSET(offset)) \
-		__fwd = FORCEWAKE_MEDIA; \
-	else if (FORCEWAKE_GEN9_COMMON_RANGE_OFFSET(offset)) \
-		__fwd = FORCEWAKE_RENDER | FORCEWAKE_MEDIA; \
-	else \
-		__fwd = FORCEWAKE_BLITTER; \
+	if (NEEDS_FORCE_WAKE((offset)) && !is_gen8_shadowed(offset)) \
+		__fwd = find_fw_domain(dev_priv, offset); \
 	__fwd; \
 })
 
-static const i915_reg_t gen9_shadowed_regs[] = {
-	RING_TAIL(RENDER_RING_BASE),
-	RING_TAIL(GEN6_BSD_RING_BASE),
-	RING_TAIL(VEBOX_RING_BASE),
-	RING_TAIL(BLT_RING_BASE),
-	GEN6_RPNSWREQ,
-	GEN6_RC_VIDEO_FREQ,
-	/* TODO: Other registers are not yet used */
+/* *Must* be sorted by offset ranges! See intel_fw_table_check(). */
+static const struct intel_forcewake_range __gen9_fw_ranges[] = {
+	GEN_FW_RANGE(0x0, 0xaff, FORCEWAKE_BLITTER),
+	GEN_FW_RANGE(0xb00, 0x1fff, 0), /* uncore range */
+	GEN_FW_RANGE(0x2000, 0x26ff, FORCEWAKE_RENDER),
+	GEN_FW_RANGE(0x2700, 0x2fff, FORCEWAKE_BLITTER),
+	GEN_FW_RANGE(0x3000, 0x3fff, FORCEWAKE_RENDER),
+	GEN_FW_RANGE(0x4000, 0x51ff, FORCEWAKE_BLITTER),
+	GEN_FW_RANGE(0x5200, 0x7fff, FORCEWAKE_RENDER),
+	GEN_FW_RANGE(0x8000, 0x812f, FORCEWAKE_BLITTER),
+	GEN_FW_RANGE(0x8130, 0x813f, FORCEWAKE_MEDIA),
+	GEN_FW_RANGE(0x8140, 0x815f, FORCEWAKE_RENDER),
+	GEN_FW_RANGE(0x8160, 0x82ff, FORCEWAKE_BLITTER),
+	GEN_FW_RANGE(0x8300, 0x84ff, FORCEWAKE_RENDER),
+	GEN_FW_RANGE(0x8500, 0x87ff, FORCEWAKE_BLITTER),
+	GEN_FW_RANGE(0x8800, 0x89ff, FORCEWAKE_MEDIA),
+	GEN_FW_RANGE(0x8a00, 0x8bff, FORCEWAKE_BLITTER),
+	GEN_FW_RANGE(0x8c00, 0x8cff, FORCEWAKE_RENDER),
+	GEN_FW_RANGE(0x8d00, 0x93ff, FORCEWAKE_BLITTER),
+	GEN_FW_RANGE(0x9400, 0x97ff, FORCEWAKE_RENDER | FORCEWAKE_MEDIA),
+	GEN_FW_RANGE(0x9800, 0xafff, FORCEWAKE_BLITTER),
+	GEN_FW_RANGE(0xb000, 0xb47f, FORCEWAKE_RENDER),
+	GEN_FW_RANGE(0xb480, 0xbfff, FORCEWAKE_BLITTER),
+	GEN_FW_RANGE(0xd000, 0xd7ff, FORCEWAKE_MEDIA),
+	GEN_FW_RANGE(0xd800, 0xdfff, FORCEWAKE_BLITTER),
+	GEN_FW_RANGE(0xe000, 0xe8ff, FORCEWAKE_RENDER),
+	GEN_FW_RANGE(0xe900, 0x11fff, FORCEWAKE_BLITTER),
+	GEN_FW_RANGE(0x12000, 0x13fff, FORCEWAKE_MEDIA),
+	GEN_FW_RANGE(0x14000, 0x19fff, FORCEWAKE_BLITTER),
+	GEN_FW_RANGE(0x1a000, 0x1e9ff, FORCEWAKE_MEDIA),
+	GEN_FW_RANGE(0x1ea00, 0x243ff, FORCEWAKE_BLITTER),
+	GEN_FW_RANGE(0x24400, 0x247ff, FORCEWAKE_RENDER),
+	GEN_FW_RANGE(0x24800, 0x2ffff, FORCEWAKE_BLITTER),
+	GEN_FW_RANGE(0x30000, 0x3ffff, FORCEWAKE_MEDIA),
 };
 
-static bool is_gen9_shadowed(u32 offset)
-{
-	int i;
-	for (i = 0; i < ARRAY_SIZE(gen9_shadowed_regs); i++)
-		if (offset == gen9_shadowed_regs[i].reg)
-			return true;
-
-	return false;
-}
-
-#define __gen9_reg_write_fw_domains(offset) \
-({ \
-	enum forcewake_domains __fwd; \
-	if (!SKL_NEEDS_FORCE_WAKE(offset) || is_gen9_shadowed(offset)) \
-		__fwd = 0; \
-	else if (FORCEWAKE_GEN9_RENDER_RANGE_OFFSET(offset)) \
-		__fwd = FORCEWAKE_RENDER; \
-	else if (FORCEWAKE_GEN9_MEDIA_RANGE_OFFSET(offset)) \
-		__fwd = FORCEWAKE_MEDIA; \
-	else if (FORCEWAKE_GEN9_COMMON_RANGE_OFFSET(offset)) \
-		__fwd = FORCEWAKE_RENDER | FORCEWAKE_MEDIA; \
-	else \
-		__fwd = FORCEWAKE_BLITTER; \
-	__fwd; \
-})
-
 static void
 ilk_dummy_write(struct drm_i915_private *dev_priv)
 {
@@ -796,10 +812,9 @@ __unclaimed_reg_debug(struct drm_i915_private *dev_priv,
 		      const bool read,
 		      const bool before)
 {
-	if (WARN(check_for_unclaimed_mmio(dev_priv),
-		 "Unclaimed register detected %s %s register 0x%x\n",
-		 before ? "before" : "after",
-		 read ? "reading" : "writing to",
+	if (WARN(check_for_unclaimed_mmio(dev_priv) && !before,
+		 "Unclaimed %s register 0x%x\n",
+		 read ? "read from" : "write to",
 		 i915_mmio_reg_offset(reg)))
 		i915.mmio_debug--; /* Only report the first N failures */
 }
@@ -870,26 +885,30 @@ __gen2_read(64)
 	trace_i915_reg_rw(false, reg, val, sizeof(val), trace); \
 	return val
 
-static inline void __force_wake_auto(struct drm_i915_private *dev_priv,
-				     enum forcewake_domains fw_domains)
+static noinline void ___force_wake_auto(struct drm_i915_private *dev_priv,
+					enum forcewake_domains fw_domains)
 {
 	struct intel_uncore_forcewake_domain *domain;
 
+	for_each_fw_domain_masked(domain, fw_domains, dev_priv)
+		fw_domain_arm_timer(domain);
+
+	dev_priv->uncore.funcs.force_wake_get(dev_priv, fw_domains);
+	dev_priv->uncore.fw_domains_active |= fw_domains;
+}
+
+static inline void __force_wake_auto(struct drm_i915_private *dev_priv,
+				     enum forcewake_domains fw_domains)
+{
 	if (WARN_ON(!fw_domains))
 		return;
 
-	/* Ideally GCC would be constant-fold and eliminate this loop */
-	for_each_fw_domain_masked(domain, fw_domains, dev_priv) {
-		if (domain->wake_count) {
-			fw_domains &= ~domain->mask;
-			continue;
-		}
-
-		fw_domain_arm_timer(domain);
-	}
+	/* Turn on all requested but inactive supported forcewake domains. */
+	fw_domains &= dev_priv->uncore.fw_domains;
+	fw_domains &= ~dev_priv->uncore.fw_domains_active;
 
 	if (fw_domains)
-		dev_priv->uncore.funcs.force_wake_get(dev_priv, fw_domains);
+		___force_wake_auto(dev_priv, fw_domains);
 }
 
 #define __gen6_read(x) \
@@ -904,62 +923,28 @@ gen6_read##x(struct drm_i915_private *dev_priv, i915_reg_t reg, bool trace) { \
 	GEN6_READ_FOOTER; \
 }
 
-#define __vlv_read(x) \
-static u##x \
-vlv_read##x(struct drm_i915_private *dev_priv, i915_reg_t reg, bool trace) { \
-	enum forcewake_domains fw_engine; \
-	GEN6_READ_HEADER(x); \
-	fw_engine = __vlv_reg_read_fw_domains(offset); \
-	if (fw_engine) \
-		__force_wake_auto(dev_priv, fw_engine); \
-	val = __raw_i915_read##x(dev_priv, reg); \
-	GEN6_READ_FOOTER; \
-}
-
-#define __chv_read(x) \
+#define __fwtable_read(x) \
 static u##x \
-chv_read##x(struct drm_i915_private *dev_priv, i915_reg_t reg, bool trace) { \
+fwtable_read##x(struct drm_i915_private *dev_priv, i915_reg_t reg, bool trace) { \
 	enum forcewake_domains fw_engine; \
 	GEN6_READ_HEADER(x); \
-	fw_engine = __chv_reg_read_fw_domains(offset); \
+	fw_engine = __fwtable_reg_read_fw_domains(offset); \
 	if (fw_engine) \
 		__force_wake_auto(dev_priv, fw_engine); \
 	val = __raw_i915_read##x(dev_priv, reg); \
 	GEN6_READ_FOOTER; \
 }
 
-#define __gen9_read(x) \
-static u##x \
-gen9_read##x(struct drm_i915_private *dev_priv, i915_reg_t reg, bool trace) { \
-	enum forcewake_domains fw_engine; \
-	GEN6_READ_HEADER(x); \
-	fw_engine = __gen9_reg_read_fw_domains(offset); \
-	if (fw_engine) \
-		__force_wake_auto(dev_priv, fw_engine); \
-	val = __raw_i915_read##x(dev_priv, reg); \
-	GEN6_READ_FOOTER; \
-}
-
-__gen9_read(8)
-__gen9_read(16)
-__gen9_read(32)
-__gen9_read(64)
-__chv_read(8)
-__chv_read(16)
-__chv_read(32)
-__chv_read(64)
-__vlv_read(8)
-__vlv_read(16)
-__vlv_read(32)
-__vlv_read(64)
+__fwtable_read(8)
+__fwtable_read(16)
+__fwtable_read(32)
+__fwtable_read(64)
 __gen6_read(8)
 __gen6_read(16)
 __gen6_read(32)
 __gen6_read(64)
 
-#undef __gen9_read
-#undef __chv_read
-#undef __vlv_read
+#undef __fwtable_read
 #undef __gen6_read
 #undef GEN6_READ_FOOTER
 #undef GEN6_READ_HEADER
@@ -1055,21 +1040,6 @@ gen6_write##x(struct drm_i915_private *dev_priv, i915_reg_t reg, u##x val, bool
 	GEN6_WRITE_FOOTER; \
 }
 
-#define __hsw_write(x) \
-static void \
-hsw_write##x(struct drm_i915_private *dev_priv, i915_reg_t reg, u##x val, bool trace) { \
-	u32 __fifo_ret = 0; \
-	GEN6_WRITE_HEADER; \
-	if (NEEDS_FORCE_WAKE(offset)) { \
-		__fifo_ret = __gen6_gt_wait_for_fifo(dev_priv); \
-	} \
-	__raw_i915_write##x(dev_priv, reg, val); \
-	if (unlikely(__fifo_ret)) { \
-		gen6_gt_check_fifodbg(dev_priv); \
-	} \
-	GEN6_WRITE_FOOTER; \
-}
-
 #define __gen8_write(x) \
 static void \
 gen8_write##x(struct drm_i915_private *dev_priv, i915_reg_t reg, u##x val, bool trace) { \
@@ -1082,51 +1052,30 @@ gen8_write##x(struct drm_i915_private *dev_priv, i915_reg_t reg, u##x val, bool
 	GEN6_WRITE_FOOTER; \
 }
 
-#define __chv_write(x) \
-static void \
-chv_write##x(struct drm_i915_private *dev_priv, i915_reg_t reg, u##x val, bool trace) { \
-	enum forcewake_domains fw_engine; \
-	GEN6_WRITE_HEADER; \
-	fw_engine = __chv_reg_write_fw_domains(offset); \
-	if (fw_engine) \
-		__force_wake_auto(dev_priv, fw_engine); \
-	__raw_i915_write##x(dev_priv, reg, val); \
-	GEN6_WRITE_FOOTER; \
-}
-
-#define __gen9_write(x) \
+#define __fwtable_write(x) \
 static void \
-gen9_write##x(struct drm_i915_private *dev_priv, i915_reg_t reg, u##x val, \
-		bool trace) { \
+fwtable_write##x(struct drm_i915_private *dev_priv, i915_reg_t reg, u##x val, bool trace) { \
 	enum forcewake_domains fw_engine; \
 	GEN6_WRITE_HEADER; \
-	fw_engine = __gen9_reg_write_fw_domains(offset); \
+	fw_engine = __fwtable_reg_write_fw_domains(offset); \
 	if (fw_engine) \
 		__force_wake_auto(dev_priv, fw_engine); \
 	__raw_i915_write##x(dev_priv, reg, val); \
 	GEN6_WRITE_FOOTER; \
 }
 
-__gen9_write(8)
-__gen9_write(16)
-__gen9_write(32)
-__chv_write(8)
-__chv_write(16)
-__chv_write(32)
+__fwtable_write(8)
+__fwtable_write(16)
+__fwtable_write(32)
 __gen8_write(8)
 __gen8_write(16)
 __gen8_write(32)
-__hsw_write(8)
-__hsw_write(16)
-__hsw_write(32)
 __gen6_write(8)
 __gen6_write(16)
 __gen6_write(32)
 
-#undef __gen9_write
-#undef __chv_write
+#undef __fwtable_write
 #undef __gen8_write
-#undef __hsw_write
 #undef __gen6_write
 #undef GEN6_WRITE_FOOTER
 #undef GEN6_WRITE_HEADER
@@ -1315,6 +1264,13 @@ static void intel_uncore_fw_domains_init(struct drm_i915_private *dev_priv)
 	WARN_ON(dev_priv->uncore.fw_domains == 0);
 }
 
+#define ASSIGN_FW_DOMAINS_TABLE(d) \
+{ \
+	dev_priv->uncore.fw_domains_table = \
+			(struct intel_forcewake_range *)(d); \
+	dev_priv->uncore.fw_domains_table_entries = ARRAY_SIZE((d)); \
+}
+
 void intel_uncore_init(struct drm_i915_private *dev_priv)
 {
 	i915_check_vgpu(dev_priv);
@@ -1328,13 +1284,15 @@ void intel_uncore_init(struct drm_i915_private *dev_priv)
 	switch (INTEL_INFO(dev_priv)->gen) {
 	default:
 	case 9:
-		ASSIGN_WRITE_MMIO_VFUNCS(gen9);
-		ASSIGN_READ_MMIO_VFUNCS(gen9);
+		ASSIGN_FW_DOMAINS_TABLE(__gen9_fw_ranges);
+		ASSIGN_WRITE_MMIO_VFUNCS(fwtable);
+		ASSIGN_READ_MMIO_VFUNCS(fwtable);
 		break;
 	case 8:
 		if (IS_CHERRYVIEW(dev_priv)) {
-			ASSIGN_WRITE_MMIO_VFUNCS(chv);
-			ASSIGN_READ_MMIO_VFUNCS(chv);
+			ASSIGN_FW_DOMAINS_TABLE(__chv_fw_ranges);
+			ASSIGN_WRITE_MMIO_VFUNCS(fwtable);
+			ASSIGN_READ_MMIO_VFUNCS(fwtable);
 
 		} else {
 			ASSIGN_WRITE_MMIO_VFUNCS(gen8);
@@ -1343,14 +1301,11 @@ void intel_uncore_init(struct drm_i915_private *dev_priv)
 		break;
 	case 7:
 	case 6:
-		if (IS_HASWELL(dev_priv)) {
-			ASSIGN_WRITE_MMIO_VFUNCS(hsw);
-		} else {
-			ASSIGN_WRITE_MMIO_VFUNCS(gen6);
-		}
+		ASSIGN_WRITE_MMIO_VFUNCS(gen6);
 
 		if (IS_VALLEYVIEW(dev_priv)) {
-			ASSIGN_READ_MMIO_VFUNCS(vlv);
+			ASSIGN_FW_DOMAINS_TABLE(__vlv_fw_ranges);
+			ASSIGN_READ_MMIO_VFUNCS(fwtable);
 		} else {
 			ASSIGN_READ_MMIO_VFUNCS(gen6);
 		}
@@ -1367,6 +1322,10 @@ void intel_uncore_init(struct drm_i915_private *dev_priv)
 		break;
 	}
 
+	intel_fw_table_check(dev_priv);
+	if (INTEL_GEN(dev_priv) >= 8)
+		intel_shadow_table_check();
+
 	if (intel_vgpu_active(dev_priv)) {
 		ASSIGN_WRITE_MMIO_VFUNCS(vgpu);
 		ASSIGN_READ_MMIO_VFUNCS(vgpu);
@@ -1816,35 +1775,16 @@ static enum forcewake_domains
 intel_uncore_forcewake_for_read(struct drm_i915_private *dev_priv,
 				i915_reg_t reg)
 {
+	u32 offset = i915_mmio_reg_offset(reg);
 	enum forcewake_domains fw_domains;
 
-	if (intel_vgpu_active(dev_priv))
-		return 0;
-
-	switch (INTEL_GEN(dev_priv)) {
-	case 9:
-		fw_domains = __gen9_reg_read_fw_domains(i915_mmio_reg_offset(reg));
-		break;
-	case 8:
-		if (IS_CHERRYVIEW(dev_priv))
-			fw_domains = __chv_reg_read_fw_domains(i915_mmio_reg_offset(reg));
-		else
-			fw_domains = __gen6_reg_read_fw_domains(i915_mmio_reg_offset(reg));
-		break;
-	case 7:
-	case 6:
-		if (IS_VALLEYVIEW(dev_priv))
-			fw_domains = __vlv_reg_read_fw_domains(i915_mmio_reg_offset(reg));
-		else
-			fw_domains = __gen6_reg_read_fw_domains(i915_mmio_reg_offset(reg));
-		break;
-	default:
-		MISSING_CASE(INTEL_INFO(dev_priv)->gen);
-	case 5: /* forcewake was introduced with gen6 */
-	case 4:
-	case 3:
-	case 2:
-		return 0;
+	if (HAS_FWTABLE(dev_priv)) {
+		fw_domains = __fwtable_reg_read_fw_domains(offset);
+	} else if (INTEL_GEN(dev_priv) >= 6) {
+		fw_domains = __gen6_reg_read_fw_domains(offset);
+	} else {
+		WARN_ON(!IS_GEN(dev_priv, 2, 5));
+		fw_domains = 0;
 	}
 
 	WARN_ON(fw_domains & ~dev_priv->uncore.fw_domains);
@@ -1856,32 +1796,18 @@ static enum forcewake_domains
 intel_uncore_forcewake_for_write(struct drm_i915_private *dev_priv,
 				 i915_reg_t reg)
 {
+	u32 offset = i915_mmio_reg_offset(reg);
 	enum forcewake_domains fw_domains;
 
-	if (intel_vgpu_active(dev_priv))
-		return 0;
-
-	switch (INTEL_GEN(dev_priv)) {
-	case 9:
-		fw_domains = __gen9_reg_write_fw_domains(i915_mmio_reg_offset(reg));
-		break;
-	case 8:
-		if (IS_CHERRYVIEW(dev_priv))
-			fw_domains = __chv_reg_write_fw_domains(i915_mmio_reg_offset(reg));
-		else
-			fw_domains = __gen8_reg_write_fw_domains(i915_mmio_reg_offset(reg));
-		break;
-	case 7:
-	case 6:
+	if (HAS_FWTABLE(dev_priv) && !IS_VALLEYVIEW(dev_priv)) {
+		fw_domains = __fwtable_reg_write_fw_domains(offset);
+	} else if (IS_GEN8(dev_priv)) {
+		fw_domains = __gen8_reg_write_fw_domains(offset);
+	} else if (IS_GEN(dev_priv, 6, 7)) {
 		fw_domains = FORCEWAKE_RENDER;
-		break;
-	default:
-		MISSING_CASE(INTEL_INFO(dev_priv)->gen);
-	case 5:
-	case 4:
-	case 3:
-	case 2:
-		return 0;
+	} else {
+		WARN_ON(!IS_GEN(dev_priv, 2, 5));
+		fw_domains = 0;
 	}
 
 	WARN_ON(fw_domains & ~dev_priv->uncore.fw_domains);
@@ -1911,6 +1837,9 @@ intel_uncore_forcewake_for_reg(struct drm_i915_private *dev_priv,
 
 	WARN_ON(!op);
 
+	if (intel_vgpu_active(dev_priv))
+		return 0;
+
 	if (op & FW_REG_READ)
 		fw_domains = intel_uncore_forcewake_for_read(dev_priv, reg);
 
diff --git a/drivers/gpu/drm/msm/msm_perf.c b/drivers/gpu/drm/msm/msm_perf.c
index 17fe4e53e0d1..1627294575cb 100644
--- a/drivers/gpu/drm/msm/msm_perf.c
+++ b/drivers/gpu/drm/msm/msm_perf.c
@@ -229,8 +229,8 @@ int msm_perf_debugfs_init(struct drm_minor *minor)
 	perf->ent = debugfs_create_file("perf", S_IFREG | S_IRUGO,
 			minor->debugfs_root, perf, &perf_debugfs_fops);
 	if (!perf->ent) {
-		DRM_ERROR("Cannot create /sys/kernel/debug/dri/%s/perf\n",
-				minor->debugfs_root->d_name.name);
+		DRM_ERROR("Cannot create /sys/kernel/debug/dri/%pd/perf\n",
+				minor->debugfs_root);
 		goto fail;
 	}
 
diff --git a/drivers/gpu/drm/msm/msm_rd.c b/drivers/gpu/drm/msm/msm_rd.c
index 3a5fdfcd67ae..8487f461f05f 100644
--- a/drivers/gpu/drm/msm/msm_rd.c
+++ b/drivers/gpu/drm/msm/msm_rd.c
@@ -243,8 +243,8 @@ int msm_rd_debugfs_init(struct drm_minor *minor)
 	rd->ent = debugfs_create_file("rd", S_IFREG | S_IRUGO,
 			minor->debugfs_root, rd, &rd_debugfs_fops);
 	if (!rd->ent) {
-		DRM_ERROR("Cannot create /sys/kernel/debug/dri/%s/rd\n",
-				minor->debugfs_root->d_name.name);
+		DRM_ERROR("Cannot create /sys/kernel/debug/dri/%pd/rd\n",
+				minor->debugfs_root);
 		goto fail;
 	}
 
diff --git a/drivers/gpu/drm/nouveau/include/nvkm/core/device.h b/drivers/gpu/drm/nouveau/include/nvkm/core/device.h
index 7ea8aa7ca408..6bc712f32c8b 100644
--- a/drivers/gpu/drm/nouveau/include/nvkm/core/device.h
+++ b/drivers/gpu/drm/nouveau/include/nvkm/core/device.h
@@ -175,6 +175,7 @@ struct nvkm_device_func {
 	void (*fini)(struct nvkm_device *, bool suspend);
 	resource_size_t (*resource_addr)(struct nvkm_device *, unsigned bar);
 	resource_size_t (*resource_size)(struct nvkm_device *, unsigned bar);
+	bool cpu_coherent;
 };
 
 struct nvkm_device_quirk {
diff --git a/drivers/gpu/drm/nouveau/nouveau_bo.c b/drivers/gpu/drm/nouveau/nouveau_bo.c
index 66f31c3eb8ba..343b8659472c 100644
--- a/drivers/gpu/drm/nouveau/nouveau_bo.c
+++ b/drivers/gpu/drm/nouveau/nouveau_bo.c
@@ -209,7 +209,8 @@ nouveau_bo_new(struct drm_device *dev, int size, int align,
 	nvbo->tile_flags = tile_flags;
 	nvbo->bo.bdev = &drm->ttm.bdev;
 
-	nvbo->force_coherent = flags & TTM_PL_FLAG_UNCACHED;
+	if (!nvxx_device(&drm->device)->func->cpu_coherent)
+		nvbo->force_coherent = flags & TTM_PL_FLAG_UNCACHED;
 
 	nvbo->page_shift = 12;
 	if (drm->client.vm) {
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/device/pci.c b/drivers/gpu/drm/nouveau/nvkm/engine/device/pci.c
index b1b693219db3..62ad0300cfa5 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/device/pci.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/device/pci.c
@@ -1614,6 +1614,7 @@ nvkm_device_pci_func = {
 	.fini = nvkm_device_pci_fini,
 	.resource_addr = nvkm_device_pci_resource_addr,
 	.resource_size = nvkm_device_pci_resource_size,
+	.cpu_coherent = !IS_ENABLED(CONFIG_ARM),
 };
 
 int
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/device/tegra.c b/drivers/gpu/drm/nouveau/nvkm/engine/device/tegra.c
index 939682f18788..9b638bd905ff 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/device/tegra.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/device/tegra.c
@@ -245,6 +245,7 @@ nvkm_device_tegra_func = {
 	.fini = nvkm_device_tegra_fini,
 	.resource_addr = nvkm_device_tegra_resource_addr,
 	.resource_size = nvkm_device_tegra_resource_size,
+	.cpu_coherent = false,
 };
 
 int
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/fifo/dmanv04.c b/drivers/gpu/drm/nouveau/nvkm/engine/fifo/dmanv04.c
index edec30fd3ecd..0a7b6ed5ed28 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/fifo/dmanv04.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/fifo/dmanv04.c
@@ -37,7 +37,10 @@ nv04_fifo_dma_object_dtor(struct nvkm_fifo_chan *base, int cookie)
 {
 	struct nv04_fifo_chan *chan = nv04_fifo_chan(base);
 	struct nvkm_instmem *imem = chan->fifo->base.engine.subdev.device->imem;
+
+	mutex_lock(&chan->fifo->base.engine.subdev.mutex);
 	nvkm_ramht_remove(imem->ramht, cookie);
+	mutex_unlock(&chan->fifo->base.engine.subdev.mutex);
 }
 
 static int
diff --git a/drivers/gpu/drm/radeon/atombios_crtc.c b/drivers/gpu/drm/radeon/atombios_crtc.c
index a4e9f35da3a2..74f99bac08b1 100644
--- a/drivers/gpu/drm/radeon/atombios_crtc.c
+++ b/drivers/gpu/drm/radeon/atombios_crtc.c
@@ -1638,8 +1638,8 @@ static int avivo_crtc_do_set_base(struct drm_crtc *crtc,
 	WREG32(AVIVO_D1MODE_VIEWPORT_SIZE + radeon_crtc->crtc_offset,
 	       (viewport_w << 16) | viewport_h);
 
-	/* set pageflip to happen anywhere in vblank interval */
-	WREG32(AVIVO_D1MODE_MASTER_UPDATE_MODE + radeon_crtc->crtc_offset, 0);
+	/* set pageflip to happen only at start of vblank interval (front porch) */
+	WREG32(AVIVO_D1MODE_MASTER_UPDATE_MODE + radeon_crtc->crtc_offset, 3);
 
 	if (!atomic && fb && fb != crtc->primary->fb) {
 		radeon_fb = to_radeon_framebuffer(fb);
diff --git a/drivers/gpu/drm/radeon/r600_dpm.c b/drivers/gpu/drm/radeon/r600_dpm.c
index 6a4b020dd0b4..5a26eb4545aa 100644
--- a/drivers/gpu/drm/radeon/r600_dpm.c
+++ b/drivers/gpu/drm/radeon/r600_dpm.c
@@ -156,19 +156,20 @@ u32 r600_dpm_get_vblank_time(struct radeon_device *rdev)
 	struct drm_device *dev = rdev->ddev;
 	struct drm_crtc *crtc;
 	struct radeon_crtc *radeon_crtc;
-	u32 line_time_us, vblank_lines;
+	u32 vblank_in_pixels;
 	u32 vblank_time_us = 0xffffffff; /* if the displays are off, vblank time is max */
 
 	if (rdev->num_crtc && rdev->mode_info.mode_config_initialized) {
 		list_for_each_entry(crtc, &dev->mode_config.crtc_list, head) {
 			radeon_crtc = to_radeon_crtc(crtc);
 			if (crtc->enabled && radeon_crtc->enabled && radeon_crtc->hw_mode.clock) {
-				line_time_us = (radeon_crtc->hw_mode.crtc_htotal * 1000) /
-					radeon_crtc->hw_mode.clock;
-				vblank_lines = radeon_crtc->hw_mode.crtc_vblank_end -
-					radeon_crtc->hw_mode.crtc_vdisplay +
-					(radeon_crtc->v_border * 2);
-				vblank_time_us = vblank_lines * line_time_us;
+				vblank_in_pixels =
+					radeon_crtc->hw_mode.crtc_htotal *
+					(radeon_crtc->hw_mode.crtc_vblank_end -
+					 radeon_crtc->hw_mode.crtc_vdisplay +
+					 (radeon_crtc->v_border * 2));
+
+				vblank_time_us = vblank_in_pixels * 1000 / radeon_crtc->hw_mode.clock;
 				break;
 			}
 		}
diff --git a/drivers/gpu/drm/radeon/radeon_connectors.c b/drivers/gpu/drm/radeon/radeon_connectors.c
index 50e96d2c593d..e18839d52e3e 100644
--- a/drivers/gpu/drm/radeon/radeon_connectors.c
+++ b/drivers/gpu/drm/radeon/radeon_connectors.c
@@ -927,6 +927,16 @@ radeon_lvds_detect(struct drm_connector *connector, bool force)
 	return ret;
 }
 
+static void radeon_connector_unregister(struct drm_connector *connector)
+{
+	struct radeon_connector *radeon_connector = to_radeon_connector(connector);
+
+	if (radeon_connector->ddc_bus->has_aux) {
+		drm_dp_aux_unregister(&radeon_connector->ddc_bus->aux);
+		radeon_connector->ddc_bus->has_aux = false;
+	}
+}
+
 static void radeon_connector_destroy(struct drm_connector *connector)
 {
 	struct radeon_connector *radeon_connector = to_radeon_connector(connector);
@@ -984,6 +994,7 @@ static const struct drm_connector_funcs radeon_lvds_connector_funcs = {
 	.dpms = drm_helper_connector_dpms,
 	.detect = radeon_lvds_detect,
 	.fill_modes = drm_helper_probe_single_connector_modes,
+	.early_unregister = radeon_connector_unregister,
 	.destroy = radeon_connector_destroy,
 	.set_property = radeon_lvds_set_property,
 };
@@ -1111,6 +1122,7 @@ static const struct drm_connector_funcs radeon_vga_connector_funcs = {
 	.dpms = drm_helper_connector_dpms,
 	.detect = radeon_vga_detect,
 	.fill_modes = drm_helper_probe_single_connector_modes,
+	.early_unregister = radeon_connector_unregister,
 	.destroy = radeon_connector_destroy,
 	.set_property = radeon_connector_set_property,
 };
@@ -1188,6 +1200,7 @@ static const struct drm_connector_funcs radeon_tv_connector_funcs = {
 	.dpms = drm_helper_connector_dpms,
 	.detect = radeon_tv_detect,
 	.fill_modes = drm_helper_probe_single_connector_modes,
+	.early_unregister = radeon_connector_unregister,
 	.destroy = radeon_connector_destroy,
 	.set_property = radeon_connector_set_property,
 };
@@ -1519,6 +1532,7 @@ static const struct drm_connector_funcs radeon_dvi_connector_funcs = {
 	.detect = radeon_dvi_detect,
 	.fill_modes = drm_helper_probe_single_connector_modes,
 	.set_property = radeon_connector_set_property,
+	.early_unregister = radeon_connector_unregister,
 	.destroy = radeon_connector_destroy,
 	.force = radeon_dvi_force,
 };
@@ -1832,6 +1846,7 @@ static const struct drm_connector_funcs radeon_dp_connector_funcs = {
 	.detect = radeon_dp_detect,
 	.fill_modes = drm_helper_probe_single_connector_modes,
 	.set_property = radeon_connector_set_property,
+	.early_unregister = radeon_connector_unregister,
 	.destroy = radeon_connector_destroy,
 	.force = radeon_dvi_force,
 };
@@ -1841,6 +1856,7 @@ static const struct drm_connector_funcs radeon_edp_connector_funcs = {
 	.detect = radeon_dp_detect,
 	.fill_modes = drm_helper_probe_single_connector_modes,
 	.set_property = radeon_lvds_set_property,
+	.early_unregister = radeon_connector_unregister,
 	.destroy = radeon_connector_destroy,
 	.force = radeon_dvi_force,
 };
@@ -1850,6 +1866,7 @@ static const struct drm_connector_funcs radeon_lvds_bridge_connector_funcs = {
 	.detect = radeon_dp_detect,
 	.fill_modes = drm_helper_probe_single_connector_modes,
 	.set_property = radeon_lvds_set_property,
+	.early_unregister = radeon_connector_unregister,
 	.destroy = radeon_connector_destroy,
 	.force = radeon_dvi_force,
 };
diff --git a/drivers/gpu/drm/radeon/radeon_display.c b/drivers/gpu/drm/radeon/radeon_display.c
index 890171f08987..cdb8cb568c15 100644
--- a/drivers/gpu/drm/radeon/radeon_display.c
+++ b/drivers/gpu/drm/radeon/radeon_display.c
@@ -321,16 +321,30 @@ void radeon_crtc_handle_vblank(struct radeon_device *rdev, int crtc_id)
 	update_pending = radeon_page_flip_pending(rdev, crtc_id);
 
 	/* Has the pageflip already completed in crtc, or is it certain
-	 * to complete in this vblank?
+	 * to complete in this vblank? GET_DISTANCE_TO_VBLANKSTART provides
+	 * distance to start of "fudged earlier" vblank in vpos, distance to
+	 * start of real vblank in hpos. vpos >= 0 && hpos < 0 means we are in
+	 * the last few scanlines before start of real vblank, where the vblank
+	 * irq can fire, so we have sampled update_pending a bit too early and
+	 * know the flip will complete at leading edge of the upcoming real
+	 * vblank. On pre-AVIVO hardware, flips also complete inside the real
+	 * vblank, not only at leading edge, so if update_pending for hpos >= 0
+	 *  == inside real vblank, the flip will complete almost immediately.
+	 * Note that this method of completion handling is still not 100% race
+	 * free, as we could execute before the radeon_flip_work_func managed
+	 * to run and set the RADEON_FLIP_SUBMITTED status, thereby we no-op,
+	 * but the flip still gets programmed into hw and completed during
+	 * vblank, leading to a delayed emission of the flip completion event.
+	 * This applies at least to pre-AVIVO hardware, where flips are always
+	 * completing inside vblank, not only at leading edge of vblank.
 	 */
 	if (update_pending &&
-	    (DRM_SCANOUTPOS_VALID & radeon_get_crtc_scanoutpos(rdev->ddev,
-							       crtc_id,
-							       USE_REAL_VBLANKSTART,
-							       &vpos, &hpos, NULL, NULL,
-							       &rdev->mode_info.crtcs[crtc_id]->base.hwmode)) &&
-	    ((vpos >= (99 * rdev->mode_info.crtcs[crtc_id]->base.hwmode.crtc_vdisplay)/100) ||
-	     (vpos < 0 && !ASIC_IS_AVIVO(rdev)))) {
+	    (DRM_SCANOUTPOS_VALID &
+	     radeon_get_crtc_scanoutpos(rdev->ddev, crtc_id,
+					GET_DISTANCE_TO_VBLANKSTART,
+					&vpos, &hpos, NULL, NULL,
+					&rdev->mode_info.crtcs[crtc_id]->base.hwmode)) &&
+	    ((vpos >= 0 && hpos < 0) || (hpos >= 0 && !ASIC_IS_AVIVO(rdev)))) {
 		/* crtc didn't flip in this target vblank interval,
 		 * but flip is pending in crtc. Based on the current
 		 * scanout position we know that the current frame is
@@ -438,16 +452,19 @@ static void radeon_flip_work_func(struct work_struct *__work)
 	}
 
 	/* Wait until we're out of the vertical blank period before the one
-	 * targeted by the flip
+	 * targeted by the flip. Always wait on pre DCE4 to avoid races with
+	 * flip completion handling from vblank irq, as these old asics don't
+	 * have reliable pageflip completion interrupts.
 	 */
 	while (radeon_crtc->enabled &&
-	       (radeon_get_crtc_scanoutpos(dev, work->crtc_id, 0,
-					   &vpos, &hpos, NULL, NULL,
-					   &crtc->hwmode)
+		(radeon_get_crtc_scanoutpos(dev, work->crtc_id, 0,
+					    &vpos, &hpos, NULL, NULL,
+					    &crtc->hwmode)
 		& (DRM_SCANOUTPOS_VALID | DRM_SCANOUTPOS_IN_VBLANK)) ==
-	       (DRM_SCANOUTPOS_VALID | DRM_SCANOUTPOS_IN_VBLANK) &&
-	       (int)(work->target_vblank -
-		     dev->driver->get_vblank_counter(dev, work->crtc_id)) > 0)
+		(DRM_SCANOUTPOS_VALID | DRM_SCANOUTPOS_IN_VBLANK) &&
+		(!ASIC_IS_AVIVO(rdev) ||
+		((int) (work->target_vblank -
+		dev->driver->get_vblank_counter(dev, work->crtc_id)) > 0)))
 		usleep_range(1000, 2000);
 
 	/* We borrow the event spin lock for protecting flip_status */
@@ -1658,20 +1675,20 @@ int radeon_modeset_init(struct radeon_device *rdev)
 
 void radeon_modeset_fini(struct radeon_device *rdev)
 {
-	radeon_fbdev_fini(rdev);
-	kfree(rdev->mode_info.bios_hardcoded_edid);
-
-	/* free i2c buses */
-	radeon_i2c_fini(rdev);
-
 	if (rdev->mode_info.mode_config_initialized) {
-		radeon_afmt_fini(rdev);
 		drm_kms_helper_poll_fini(rdev->ddev);
 		radeon_hpd_fini(rdev);
 		drm_crtc_force_disable_all(rdev->ddev);
+		radeon_fbdev_fini(rdev);
+		radeon_afmt_fini(rdev);
 		drm_mode_config_cleanup(rdev->ddev);
 		rdev->mode_info.mode_config_initialized = false;
 	}
+
+	kfree(rdev->mode_info.bios_hardcoded_edid);
+
+	/* free i2c buses */
+	radeon_i2c_fini(rdev);
 }
 
 static bool is_hdtv_mode(const struct drm_display_mode *mode)
diff --git a/drivers/gpu/drm/radeon/radeon_drv.c b/drivers/gpu/drm/radeon/radeon_drv.c
index 91c8f4339566..00ea0002b539 100644
--- a/drivers/gpu/drm/radeon/radeon_drv.c
+++ b/drivers/gpu/drm/radeon/radeon_drv.c
@@ -96,9 +96,10 @@
  *   2.45.0 - Allow setting shader registers using DMA/COPY packet3 on SI
  *   2.46.0 - Add PFP_SYNC_ME support on evergreen
  *   2.47.0 - Add UVD_NO_OP register support
+ *   2.48.0 - TA_CS_BC_BASE_ADDR allowed on SI
  */
 #define KMS_DRIVER_MAJOR	2
-#define KMS_DRIVER_MINOR	47
+#define KMS_DRIVER_MINOR	48
 #define KMS_DRIVER_PATCHLEVEL	0
 int radeon_driver_load_kms(struct drm_device *dev, unsigned long flags);
 int radeon_driver_unload_kms(struct drm_device *dev);
diff --git a/drivers/gpu/drm/radeon/radeon_i2c.c b/drivers/gpu/drm/radeon/radeon_i2c.c
index 021aa005623f..29f7817af821 100644
--- a/drivers/gpu/drm/radeon/radeon_i2c.c
+++ b/drivers/gpu/drm/radeon/radeon_i2c.c
@@ -982,9 +982,8 @@ void radeon_i2c_destroy(struct radeon_i2c_chan *i2c)
 {
 	if (!i2c)
 		return;
+	WARN_ON(i2c->has_aux);
 	i2c_del_adapter(&i2c->adapter);
-	if (i2c->has_aux)
-		drm_dp_aux_unregister(&i2c->aux);
 	kfree(i2c);
 }
 
diff --git a/drivers/gpu/drm/radeon/radeon_ttm.c b/drivers/gpu/drm/radeon/radeon_ttm.c
index 455268214b89..3de5e6e21662 100644
--- a/drivers/gpu/drm/radeon/radeon_ttm.c
+++ b/drivers/gpu/drm/radeon/radeon_ttm.c
@@ -566,7 +566,8 @@ static int radeon_ttm_tt_pin_userptr(struct ttm_tt *ttm)
 		uint64_t userptr = gtt->userptr + pinned * PAGE_SIZE;
 		struct page **pages = ttm->pages + pinned;
 
-		r = get_user_pages(userptr, num_pages, write, 0, pages, NULL);
+		r = get_user_pages(userptr, num_pages, write ? FOLL_WRITE : 0,
+				   pages, NULL);
 		if (r < 0)
 			goto release_pages;
 
diff --git a/drivers/gpu/drm/radeon/rv515.c b/drivers/gpu/drm/radeon/rv515.c
index 76c55c5d11ec..c55d653aaf5f 100644
--- a/drivers/gpu/drm/radeon/rv515.c
+++ b/drivers/gpu/drm/radeon/rv515.c
@@ -406,8 +406,9 @@ void rv515_mc_resume(struct radeon_device *rdev, struct rv515_mc_save *save)
 	for (i = 0; i < rdev->num_crtc; i++) {
 		if (save->crtc_enabled[i]) {
 			tmp = RREG32(AVIVO_D1MODE_MASTER_UPDATE_MODE + crtc_offsets[i]);
-			if ((tmp & 0x7) != 0) {
+			if ((tmp & 0x7) != 3) {
 				tmp &= ~0x7;
+				tmp |= 0x3;
 				WREG32(AVIVO_D1MODE_MASTER_UPDATE_MODE + crtc_offsets[i], tmp);
 			}
 			tmp = RREG32(AVIVO_D1GRPH_UPDATE + crtc_offsets[i]);
diff --git a/drivers/gpu/drm/radeon/si.c b/drivers/gpu/drm/radeon/si.c
index 7ee9aafbdf74..e402be8821c4 100644
--- a/drivers/gpu/drm/radeon/si.c
+++ b/drivers/gpu/drm/radeon/si.c
@@ -4431,6 +4431,7 @@ static bool si_vm_reg_valid(u32 reg)
 	case SPI_CONFIG_CNTL:
 	case SPI_CONFIG_CNTL_1:
 	case TA_CNTL_AUX:
+	case TA_CS_BC_BASE_ADDR:
 		return true;
 	default:
 		DRM_ERROR("Invalid register 0x%x in CS\n", reg);
diff --git a/drivers/gpu/drm/radeon/si_dpm.c b/drivers/gpu/drm/radeon/si_dpm.c
index ba7e3d19b38d..89bdf20344ae 100644
--- a/drivers/gpu/drm/radeon/si_dpm.c
+++ b/drivers/gpu/drm/radeon/si_dpm.c
@@ -3015,6 +3015,12 @@ static void si_apply_state_adjust_rules(struct radeon_device *rdev,
 	if (rdev->pdev->device == 0x6811 &&
 	    rdev->pdev->revision == 0x81)
 		max_mclk = 120000;
+	/* limit sclk/mclk on Jet parts for stability */
+	if (rdev->pdev->device == 0x6665 &&
+	    rdev->pdev->revision == 0xc3) {
+		max_sclk = 75000;
+		max_mclk = 80000;
+	}
 
 	if (rps->vce_active) {
 		rps->evclk = rdev->pm.dpm.vce_states[rdev->pm.dpm.vce_level].evclk;
diff --git a/drivers/gpu/drm/radeon/sid.h b/drivers/gpu/drm/radeon/sid.h
index eb220eecba78..65a911ddd509 100644
--- a/drivers/gpu/drm/radeon/sid.h
+++ b/drivers/gpu/drm/radeon/sid.h
@@ -1145,6 +1145,7 @@
 #define	SPI_LB_CU_MASK					0x9354
 
 #define	TA_CNTL_AUX					0x9508
+#define	TA_CS_BC_BASE_ADDR				0x950C
 
 #define CC_RB_BACKEND_DISABLE				0x98F4
 #define		BACKEND_DISABLE(x)     			((x) << 16)
diff --git a/drivers/gpu/drm/udl/udl_fb.c b/drivers/gpu/drm/udl/udl_fb.c
index 9688bfa92ccd..611b6b9bb3cb 100644
--- a/drivers/gpu/drm/udl/udl_fb.c
+++ b/drivers/gpu/drm/udl/udl_fb.c
@@ -122,7 +122,7 @@ int udl_handle_damage(struct udl_framebuffer *fb, int x, int y,
 		return 0;
 	cmd = urb->transfer_buffer;
 
-	for (i = y; i < height ; i++) {
+	for (i = y; i < y + height ; i++) {
 		const int line_offset = fb->base.pitches[0] * i;
 		const int byte_offset = line_offset + (x * bpp);
 		const int dev_byte_offset = (fb->base.width * bpp * i) + (x * bpp);
diff --git a/drivers/gpu/drm/vc4/vc4_crtc.c b/drivers/gpu/drm/vc4/vc4_crtc.c
index 2682f07d8f1e..7f08d681a74b 100644
--- a/drivers/gpu/drm/vc4/vc4_crtc.c
+++ b/drivers/gpu/drm/vc4/vc4_crtc.c
@@ -229,7 +229,7 @@ int vc4_crtc_get_scanoutpos(struct drm_device *dev, unsigned int crtc_id,
 	 * and need to make things up in a approximative but consistent way.
 	 */
 	ret |= DRM_SCANOUTPOS_IN_VBLANK;
-	vblank_lines = mode->crtc_vtotal - mode->crtc_vdisplay;
+	vblank_lines = mode->vtotal - mode->vdisplay;
 
 	if (flags & DRM_CALLED_FROM_VBLIRQ) {
 		/*
@@ -378,7 +378,7 @@ static void vc4_crtc_mode_set_nofb(struct drm_crtc *crtc)
 	struct drm_crtc_state *state = crtc->state;
 	struct drm_display_mode *mode = &state->adjusted_mode;
 	bool interlace = mode->flags & DRM_MODE_FLAG_INTERLACE;
-	u32 vactive = (mode->vdisplay >> (interlace ? 1 : 0));
+	u32 pixel_rep = (mode->flags & DRM_MODE_FLAG_DBLCLK) ? 2 : 1;
 	u32 format = PV_CONTROL_FORMAT_24;
 	bool debug_dump_regs = false;
 	int clock_select = vc4_get_clock_select(crtc);
@@ -394,47 +394,65 @@ static void vc4_crtc_mode_set_nofb(struct drm_crtc *crtc)
 	CRTC_WRITE(PV_CONTROL, 0);
 
 	CRTC_WRITE(PV_HORZA,
-		   VC4_SET_FIELD(mode->htotal - mode->hsync_end,
+		   VC4_SET_FIELD((mode->htotal -
+				  mode->hsync_end) * pixel_rep,
 				 PV_HORZA_HBP) |
-		   VC4_SET_FIELD(mode->hsync_end - mode->hsync_start,
+		   VC4_SET_FIELD((mode->hsync_end -
+				  mode->hsync_start) * pixel_rep,
 				 PV_HORZA_HSYNC));
 	CRTC_WRITE(PV_HORZB,
-		   VC4_SET_FIELD(mode->hsync_start - mode->hdisplay,
+		   VC4_SET_FIELD((mode->hsync_start -
+				  mode->hdisplay) * pixel_rep,
 				 PV_HORZB_HFP) |
-		   VC4_SET_FIELD(mode->hdisplay, PV_HORZB_HACTIVE));
+		   VC4_SET_FIELD(mode->hdisplay * pixel_rep, PV_HORZB_HACTIVE));
 
 	CRTC_WRITE(PV_VERTA,
-		   VC4_SET_FIELD(mode->vtotal - mode->vsync_end,
+		   VC4_SET_FIELD(mode->crtc_vtotal - mode->crtc_vsync_end,
 				 PV_VERTA_VBP) |
-		   VC4_SET_FIELD(mode->vsync_end - mode->vsync_start,
+		   VC4_SET_FIELD(mode->crtc_vsync_end - mode->crtc_vsync_start,
 				 PV_VERTA_VSYNC));
 	CRTC_WRITE(PV_VERTB,
-		   VC4_SET_FIELD(mode->vsync_start - mode->vdisplay,
+		   VC4_SET_FIELD(mode->crtc_vsync_start - mode->crtc_vdisplay,
 				 PV_VERTB_VFP) |
-		   VC4_SET_FIELD(vactive, PV_VERTB_VACTIVE));
+		   VC4_SET_FIELD(mode->crtc_vdisplay, PV_VERTB_VACTIVE));
 
 	if (interlace) {
 		CRTC_WRITE(PV_VERTA_EVEN,
-			   VC4_SET_FIELD(mode->vtotal - mode->vsync_end - 1,
+			   VC4_SET_FIELD(mode->crtc_vtotal -
+					 mode->crtc_vsync_end - 1,
 					 PV_VERTA_VBP) |
-			   VC4_SET_FIELD(mode->vsync_end - mode->vsync_start,
+			   VC4_SET_FIELD(mode->crtc_vsync_end -
+					 mode->crtc_vsync_start,
 					 PV_VERTA_VSYNC));
 		CRTC_WRITE(PV_VERTB_EVEN,
-			   VC4_SET_FIELD(mode->vsync_start - mode->vdisplay,
+			   VC4_SET_FIELD(mode->crtc_vsync_start -
+					 mode->crtc_vdisplay,
 					 PV_VERTB_VFP) |
-			   VC4_SET_FIELD(vactive, PV_VERTB_VACTIVE));
+			   VC4_SET_FIELD(mode->crtc_vdisplay, PV_VERTB_VACTIVE));
+
+		/* We set up first field even mode for HDMI.  VEC's
+		 * NTSC mode would want first field odd instead, once
+		 * we support it (to do so, set ODD_FIRST and put the
+		 * delay in VSYNCD_EVEN instead).
+		 */
+		CRTC_WRITE(PV_V_CONTROL,
+			   PV_VCONTROL_CONTINUOUS |
+			   PV_VCONTROL_INTERLACE |
+			   VC4_SET_FIELD(mode->htotal * pixel_rep / 2,
+					 PV_VCONTROL_ODD_DELAY));
+		CRTC_WRITE(PV_VSYNCD_EVEN, 0);
+	} else {
+		CRTC_WRITE(PV_V_CONTROL, PV_VCONTROL_CONTINUOUS);
 	}
 
-	CRTC_WRITE(PV_HACT_ACT, mode->hdisplay);
+	CRTC_WRITE(PV_HACT_ACT, mode->hdisplay * pixel_rep);
 
-	CRTC_WRITE(PV_V_CONTROL,
-		   PV_VCONTROL_CONTINUOUS |
-		   (interlace ? PV_VCONTROL_INTERLACE : 0));
 
 	CRTC_WRITE(PV_CONTROL,
 		   VC4_SET_FIELD(format, PV_CONTROL_FORMAT) |
 		   VC4_SET_FIELD(vc4_get_fifo_full_level(format),
 				 PV_CONTROL_FIFO_LEVEL) |
+		   VC4_SET_FIELD(pixel_rep - 1, PV_CONTROL_PIXEL_REP) |
 		   PV_CONTROL_CLR_AT_START |
 		   PV_CONTROL_TRIGGER_UNDERFLOW |
 		   PV_CONTROL_WAIT_HSTART |
@@ -544,16 +562,6 @@ static bool vc4_crtc_mode_fixup(struct drm_crtc *crtc,
 		return false;
 	}
 
-	/*
-	 * Interlaced video modes got CRTC_INTERLACE_HALVE_V applied when
-	 * coming from user space. We don't want this, as it screws up
-	 * vblank timestamping, so fix it up.
-	 */
-	drm_mode_set_crtcinfo(adjusted_mode, 0);
-
-	DRM_DEBUG_KMS("[CRTC:%d] adjusted_mode :\n", crtc->base.id);
-	drm_mode_debug_printmodeline(adjusted_mode);
-
 	return true;
 }
 
diff --git a/drivers/gpu/drm/vc4/vc4_drv.h b/drivers/gpu/drm/vc4/vc4_drv.h
index 428e24919ef1..7c1e4d97486f 100644
--- a/drivers/gpu/drm/vc4/vc4_drv.h
+++ b/drivers/gpu/drm/vc4/vc4_drv.h
@@ -122,9 +122,16 @@ to_vc4_dev(struct drm_device *dev)
 struct vc4_bo {
 	struct drm_gem_cma_object base;
 
-	/* seqno of the last job to render to this BO. */
+	/* seqno of the last job to render using this BO. */
 	uint64_t seqno;
 
+	/* seqno of the last job to use the RCL to write to this BO.
+	 *
+	 * Note that this doesn't include binner overflow memory
+	 * writes.
+	 */
+	uint64_t write_seqno;
+
 	/* List entry for the BO's position in either
 	 * vc4_exec_info->unref_list or vc4_dev->bo_cache.time_list
 	 */
@@ -216,6 +223,9 @@ struct vc4_exec_info {
 	/* Sequence number for this bin/render job. */
 	uint64_t seqno;
 
+	/* Latest write_seqno of any BO that binning depends on. */
+	uint64_t bin_dep_seqno;
+
 	/* Last current addresses the hardware was processing when the
 	 * hangcheck timer checked on us.
 	 */
@@ -230,6 +240,13 @@ struct vc4_exec_info {
 	struct drm_gem_cma_object **bo;
 	uint32_t bo_count;
 
+	/* List of BOs that are being written by the RCL.  Other than
+	 * the binner temporary storage, this is all the BOs written
+	 * by the job.
+	 */
+	struct drm_gem_cma_object *rcl_write_bo[4];
+	uint32_t rcl_write_bo_count;
+
 	/* Pointers for our position in vc4->job_list */
 	struct list_head head;
 
@@ -307,18 +324,15 @@ struct vc4_exec_info {
 static inline struct vc4_exec_info *
 vc4_first_bin_job(struct vc4_dev *vc4)
 {
-	if (list_empty(&vc4->bin_job_list))
-		return NULL;
-	return list_first_entry(&vc4->bin_job_list, struct vc4_exec_info, head);
+	return list_first_entry_or_null(&vc4->bin_job_list,
+					struct vc4_exec_info, head);
 }
 
 static inline struct vc4_exec_info *
 vc4_first_render_job(struct vc4_dev *vc4)
 {
-	if (list_empty(&vc4->render_job_list))
-		return NULL;
-	return list_first_entry(&vc4->render_job_list,
-				struct vc4_exec_info, head);
+	return list_first_entry_or_null(&vc4->render_job_list,
+					struct vc4_exec_info, head);
 }
 
 static inline struct vc4_exec_info *
diff --git a/drivers/gpu/drm/vc4/vc4_gem.c b/drivers/gpu/drm/vc4/vc4_gem.c
index 77daea6cb866..47a095f392f8 100644
--- a/drivers/gpu/drm/vc4/vc4_gem.c
+++ b/drivers/gpu/drm/vc4/vc4_gem.c
@@ -467,6 +467,11 @@ vc4_update_bo_seqnos(struct vc4_exec_info *exec, uint64_t seqno)
 	list_for_each_entry(bo, &exec->unref_list, unref_head) {
 		bo->seqno = seqno;
 	}
+
+	for (i = 0; i < exec->rcl_write_bo_count; i++) {
+		bo = to_vc4_bo(&exec->rcl_write_bo[i]->base);
+		bo->write_seqno = seqno;
+	}
 }
 
 /* Queues a struct vc4_exec_info for execution.  If no job is
@@ -669,6 +674,14 @@ vc4_get_bcl(struct drm_device *dev, struct vc4_exec_info *exec)
 		goto fail;
 
 	ret = vc4_validate_shader_recs(dev, exec);
+	if (ret)
+		goto fail;
+
+	/* Block waiting on any previous rendering into the CS's VBO,
+	 * IB, or textures, so that pixels are actually written by the
+	 * time we try to read them.
+	 */
+	ret = vc4_wait_for_seqno(dev, exec->bin_dep_seqno, ~0ull, true);
 
 fail:
 	drm_free_large(temp);
diff --git a/drivers/gpu/drm/vc4/vc4_hdmi.c b/drivers/gpu/drm/vc4/vc4_hdmi.c
index 68ad10634b29..c4cb2e26de32 100644
--- a/drivers/gpu/drm/vc4/vc4_hdmi.c
+++ b/drivers/gpu/drm/vc4/vc4_hdmi.c
@@ -62,6 +62,8 @@ struct vc4_hdmi {
 struct vc4_hdmi_encoder {
 	struct vc4_encoder base;
 	bool hdmi_monitor;
+	bool limited_rgb_range;
+	bool rgb_range_selectable;
 };
 
 static inline struct vc4_hdmi_encoder *
@@ -174,6 +176,9 @@ vc4_hdmi_connector_detect(struct drm_connector *connector, bool force)
 			return connector_status_disconnected;
 	}
 
+	if (drm_probe_ddc(vc4->hdmi->ddc))
+		return connector_status_connected;
+
 	if (HDMI_READ(VC4_HDMI_HOTPLUG) & VC4_HDMI_HOTPLUG_CONNECTED)
 		return connector_status_connected;
 	else
@@ -202,41 +207,22 @@ static int vc4_hdmi_connector_get_modes(struct drm_connector *connector)
 		return -ENODEV;
 
 	vc4_encoder->hdmi_monitor = drm_detect_hdmi_monitor(edid);
+
+	if (edid && edid->input & DRM_EDID_INPUT_DIGITAL) {
+		vc4_encoder->rgb_range_selectable =
+			drm_rgb_quant_range_selectable(edid);
+	}
+
 	drm_mode_connector_update_edid_property(connector, edid);
 	ret = drm_add_edid_modes(connector, edid);
 
 	return ret;
 }
 
-/*
- * drm_helper_probe_single_connector_modes() applies drm_mode_set_crtcinfo to
- * all modes with flag CRTC_INTERLACE_HALVE_V. We don't want this, as it
- * screws up vblank timestamping for interlaced modes, so fix it up.
- */
-static int vc4_hdmi_connector_probe_modes(struct drm_connector *connector,
-					  uint32_t maxX, uint32_t maxY)
-{
-	struct drm_display_mode *mode;
-	int count;
-
-	count = drm_helper_probe_single_connector_modes(connector, maxX, maxY);
-	if (count == 0)
-		return 0;
-
-	DRM_DEBUG_KMS("[CONNECTOR:%d:%s] probed adapted modes :\n",
-		      connector->base.id, connector->name);
-	list_for_each_entry(mode, &connector->modes, head) {
-		drm_mode_set_crtcinfo(mode, 0);
-		drm_mode_debug_printmodeline(mode);
-	}
-
-	return count;
-}
-
 static const struct drm_connector_funcs vc4_hdmi_connector_funcs = {
 	.dpms = drm_atomic_helper_connector_dpms,
 	.detect = vc4_hdmi_connector_detect,
-	.fill_modes = vc4_hdmi_connector_probe_modes,
+	.fill_modes = drm_helper_probe_single_connector_modes,
 	.destroy = vc4_hdmi_connector_destroy,
 	.reset = drm_atomic_helper_connector_reset,
 	.atomic_duplicate_state = drm_atomic_helper_connector_duplicate_state,
@@ -294,25 +280,143 @@ static const struct drm_encoder_funcs vc4_hdmi_encoder_funcs = {
 	.destroy = vc4_hdmi_encoder_destroy,
 };
 
+static int vc4_hdmi_stop_packet(struct drm_encoder *encoder,
+				enum hdmi_infoframe_type type)
+{
+	struct drm_device *dev = encoder->dev;
+	struct vc4_dev *vc4 = to_vc4_dev(dev);
+	u32 packet_id = type - 0x80;
+
+	HDMI_WRITE(VC4_HDMI_RAM_PACKET_CONFIG,
+		   HDMI_READ(VC4_HDMI_RAM_PACKET_CONFIG) & ~BIT(packet_id));
+
+	return wait_for(!(HDMI_READ(VC4_HDMI_RAM_PACKET_STATUS) &
+			  BIT(packet_id)), 100);
+}
+
+static void vc4_hdmi_write_infoframe(struct drm_encoder *encoder,
+				     union hdmi_infoframe *frame)
+{
+	struct drm_device *dev = encoder->dev;
+	struct vc4_dev *vc4 = to_vc4_dev(dev);
+	u32 packet_id = frame->any.type - 0x80;
+	u32 packet_reg = VC4_HDMI_GCP_0 + VC4_HDMI_PACKET_STRIDE * packet_id;
+	uint8_t buffer[VC4_HDMI_PACKET_STRIDE];
+	ssize_t len, i;
+	int ret;
+
+	WARN_ONCE(!(HDMI_READ(VC4_HDMI_RAM_PACKET_CONFIG) &
+		    VC4_HDMI_RAM_PACKET_ENABLE),
+		  "Packet RAM has to be on to store the packet.");
+
+	len = hdmi_infoframe_pack(frame, buffer, sizeof(buffer));
+	if (len < 0)
+		return;
+
+	ret = vc4_hdmi_stop_packet(encoder, frame->any.type);
+	if (ret) {
+		DRM_ERROR("Failed to wait for infoframe to go idle: %d\n", ret);
+		return;
+	}
+
+	for (i = 0; i < len; i += 7) {
+		HDMI_WRITE(packet_reg,
+			   buffer[i + 0] << 0 |
+			   buffer[i + 1] << 8 |
+			   buffer[i + 2] << 16);
+		packet_reg += 4;
+
+		HDMI_WRITE(packet_reg,
+			   buffer[i + 3] << 0 |
+			   buffer[i + 4] << 8 |
+			   buffer[i + 5] << 16 |
+			   buffer[i + 6] << 24);
+		packet_reg += 4;
+	}
+
+	HDMI_WRITE(VC4_HDMI_RAM_PACKET_CONFIG,
+		   HDMI_READ(VC4_HDMI_RAM_PACKET_CONFIG) | BIT(packet_id));
+	ret = wait_for((HDMI_READ(VC4_HDMI_RAM_PACKET_STATUS) &
+			BIT(packet_id)), 100);
+	if (ret)
+		DRM_ERROR("Failed to wait for infoframe to start: %d\n", ret);
+}
+
+static void vc4_hdmi_set_avi_infoframe(struct drm_encoder *encoder)
+{
+	struct vc4_hdmi_encoder *vc4_encoder = to_vc4_hdmi_encoder(encoder);
+	struct drm_crtc *crtc = encoder->crtc;
+	const struct drm_display_mode *mode = &crtc->state->adjusted_mode;
+	union hdmi_infoframe frame;
+	int ret;
+
+	ret = drm_hdmi_avi_infoframe_from_display_mode(&frame.avi, mode);
+	if (ret < 0) {
+		DRM_ERROR("couldn't fill AVI infoframe\n");
+		return;
+	}
+
+	if (vc4_encoder->rgb_range_selectable) {
+		if (vc4_encoder->limited_rgb_range) {
+			frame.avi.quantization_range =
+				HDMI_QUANTIZATION_RANGE_LIMITED;
+		} else {
+			frame.avi.quantization_range =
+				HDMI_QUANTIZATION_RANGE_FULL;
+		}
+	}
+
+	vc4_hdmi_write_infoframe(encoder, &frame);
+}
+
+static void vc4_hdmi_set_spd_infoframe(struct drm_encoder *encoder)
+{
+	union hdmi_infoframe frame;
+	int ret;
+
+	ret = hdmi_spd_infoframe_init(&frame.spd, "Broadcom", "Videocore");
+	if (ret < 0) {
+		DRM_ERROR("couldn't fill SPD infoframe\n");
+		return;
+	}
+
+	frame.spd.sdi = HDMI_SPD_SDI_PC;
+
+	vc4_hdmi_write_infoframe(encoder, &frame);
+}
+
+static void vc4_hdmi_set_infoframes(struct drm_encoder *encoder)
+{
+	vc4_hdmi_set_avi_infoframe(encoder);
+	vc4_hdmi_set_spd_infoframe(encoder);
+}
+
 static void vc4_hdmi_encoder_mode_set(struct drm_encoder *encoder,
 				      struct drm_display_mode *unadjusted_mode,
 				      struct drm_display_mode *mode)
 {
+	struct vc4_hdmi_encoder *vc4_encoder = to_vc4_hdmi_encoder(encoder);
 	struct drm_device *dev = encoder->dev;
 	struct vc4_dev *vc4 = to_vc4_dev(dev);
 	bool debug_dump_regs = false;
 	bool hsync_pos = mode->flags & DRM_MODE_FLAG_PHSYNC;
 	bool vsync_pos = mode->flags & DRM_MODE_FLAG_PVSYNC;
-	u32 vactive = (mode->vdisplay >>
-		       ((mode->flags & DRM_MODE_FLAG_INTERLACE) ? 1 : 0));
-	u32 verta = (VC4_SET_FIELD(mode->vsync_end - mode->vsync_start,
+	bool interlaced = mode->flags & DRM_MODE_FLAG_INTERLACE;
+	u32 pixel_rep = (mode->flags & DRM_MODE_FLAG_DBLCLK) ? 2 : 1;
+	u32 verta = (VC4_SET_FIELD(mode->crtc_vsync_end - mode->crtc_vsync_start,
 				   VC4_HDMI_VERTA_VSP) |
-		     VC4_SET_FIELD(mode->vsync_start - mode->vdisplay,
+		     VC4_SET_FIELD(mode->crtc_vsync_start - mode->crtc_vdisplay,
 				   VC4_HDMI_VERTA_VFP) |
-		     VC4_SET_FIELD(vactive, VC4_HDMI_VERTA_VAL));
+		     VC4_SET_FIELD(mode->crtc_vdisplay, VC4_HDMI_VERTA_VAL));
 	u32 vertb = (VC4_SET_FIELD(0, VC4_HDMI_VERTB_VSPO) |
-		     VC4_SET_FIELD(mode->vtotal - mode->vsync_end,
+		     VC4_SET_FIELD(mode->crtc_vtotal - mode->crtc_vsync_end,
 				   VC4_HDMI_VERTB_VBP));
+	u32 vertb_even = (VC4_SET_FIELD(0, VC4_HDMI_VERTB_VSPO) |
+			  VC4_SET_FIELD(mode->crtc_vtotal -
+					mode->crtc_vsync_end -
+					interlaced,
+					VC4_HDMI_VERTB_VBP));
+	u32 csc_ctl;
 
 	if (debug_dump_regs) {
 		DRM_INFO("HDMI regs before:\n");
@@ -321,7 +425,8 @@ static void vc4_hdmi_encoder_mode_set(struct drm_encoder *encoder,
 
 	HD_WRITE(VC4_HD_VID_CTL, 0);
 
-	clk_set_rate(vc4->hdmi->pixel_clock, mode->clock * 1000);
+	clk_set_rate(vc4->hdmi->pixel_clock, mode->clock * 1000 *
+		     ((mode->flags & DRM_MODE_FLAG_DBLCLK) ? 2 : 1));
 
 	HDMI_WRITE(VC4_HDMI_SCHEDULER_CONTROL,
 		   HDMI_READ(VC4_HDMI_SCHEDULER_CONTROL) |
@@ -331,29 +436,62 @@ static void vc4_hdmi_encoder_mode_set(struct drm_encoder *encoder,
 	HDMI_WRITE(VC4_HDMI_HORZA,
 		   (vsync_pos ? VC4_HDMI_HORZA_VPOS : 0) |
 		   (hsync_pos ? VC4_HDMI_HORZA_HPOS : 0) |
-		   VC4_SET_FIELD(mode->hdisplay, VC4_HDMI_HORZA_HAP));
+		   VC4_SET_FIELD(mode->hdisplay * pixel_rep,
+				 VC4_HDMI_HORZA_HAP));
 
 	HDMI_WRITE(VC4_HDMI_HORZB,
-		   VC4_SET_FIELD(mode->htotal - mode->hsync_end,
+		   VC4_SET_FIELD((mode->htotal -
+				  mode->hsync_end) * pixel_rep,
 				 VC4_HDMI_HORZB_HBP) |
-		   VC4_SET_FIELD(mode->hsync_end - mode->hsync_start,
+		   VC4_SET_FIELD((mode->hsync_end -
+				  mode->hsync_start) * pixel_rep,
 				 VC4_HDMI_HORZB_HSP) |
-		   VC4_SET_FIELD(mode->hsync_start - mode->hdisplay,
+		   VC4_SET_FIELD((mode->hsync_start -
+				  mode->hdisplay) * pixel_rep,
 				 VC4_HDMI_HORZB_HFP));
 
 	HDMI_WRITE(VC4_HDMI_VERTA0, verta);
 	HDMI_WRITE(VC4_HDMI_VERTA1, verta);
 
-	HDMI_WRITE(VC4_HDMI_VERTB0, vertb);
+	HDMI_WRITE(VC4_HDMI_VERTB0, vertb_even);
 	HDMI_WRITE(VC4_HDMI_VERTB1, vertb);
 
 	HD_WRITE(VC4_HD_VID_CTL,
 		 (vsync_pos ? 0 : VC4_HD_VID_CTL_VSYNC_LOW) |
 		 (hsync_pos ? 0 : VC4_HD_VID_CTL_HSYNC_LOW));
 
+	csc_ctl = VC4_SET_FIELD(VC4_HD_CSC_CTL_ORDER_BGR,
+				VC4_HD_CSC_CTL_ORDER);
+
+	if (vc4_encoder->hdmi_monitor && drm_match_cea_mode(mode) > 1) {
+		/* CEA VICs other than #1 requre limited range RGB
+		 * output unless overridden by an AVI infoframe.
+		 * Apply a colorspace conversion to squash 0-255 down
+		 * to 16-235.  The matrix here is:
+		 *
+		 * [ 0      0      0.8594 16]
+		 * [ 0      0.8594 0      16]
+		 * [ 0.8594 0      0      16]
+		 * [ 0      0      0       1]
+		 */
+		csc_ctl |= VC4_HD_CSC_CTL_ENABLE;
+		csc_ctl |= VC4_HD_CSC_CTL_RGB2YCC;
+		csc_ctl |= VC4_SET_FIELD(VC4_HD_CSC_CTL_MODE_CUSTOM,
+					 VC4_HD_CSC_CTL_MODE);
+
+		HD_WRITE(VC4_HD_CSC_12_11, (0x000 << 16) | 0x000);
+		HD_WRITE(VC4_HD_CSC_14_13, (0x100 << 16) | 0x6e0);
+		HD_WRITE(VC4_HD_CSC_22_21, (0x6e0 << 16) | 0x000);
+		HD_WRITE(VC4_HD_CSC_24_23, (0x100 << 16) | 0x000);
+		HD_WRITE(VC4_HD_CSC_32_31, (0x000 << 16) | 0x6e0);
+		HD_WRITE(VC4_HD_CSC_34_33, (0x100 << 16) | 0x000);
+		vc4_encoder->limited_rgb_range = true;
+	} else {
+		vc4_encoder->limited_rgb_range = false;
+	}
+
 	/* The RGB order applies even when CSC is disabled. */
-	HD_WRITE(VC4_HD_CSC_CTL, VC4_SET_FIELD(VC4_HD_CSC_CTL_ORDER_BGR,
-					       VC4_HD_CSC_CTL_ORDER));
+	HD_WRITE(VC4_HD_CSC_CTL, csc_ctl);
 
 	HDMI_WRITE(VC4_HDMI_FIFO_CTL, VC4_HDMI_FIFO_CTL_MASTER_SLAVE_N);
 
@@ -368,6 +506,8 @@ static void vc4_hdmi_encoder_disable(struct drm_encoder *encoder)
 	struct drm_device *dev = encoder->dev;
 	struct vc4_dev *vc4 = to_vc4_dev(dev);
 
+	HDMI_WRITE(VC4_HDMI_RAM_PACKET_CONFIG, 0);
+
 	HDMI_WRITE(VC4_HDMI_TX_PHY_RESET_CTL, 0xf << 16);
 	HD_WRITE(VC4_HD_VID_CTL,
 		 HD_READ(VC4_HD_VID_CTL) & ~VC4_HD_VID_CTL_ENABLE);
@@ -394,7 +534,7 @@ static void vc4_hdmi_encoder_enable(struct drm_encoder *encoder)
 			   VC4_HDMI_SCHEDULER_CONTROL_MODE_HDMI);
 
 		ret = wait_for(HDMI_READ(VC4_HDMI_SCHEDULER_CONTROL) &
-			       VC4_HDMI_SCHEDULER_CONTROL_HDMI_ACTIVE, 1);
+			       VC4_HDMI_SCHEDULER_CONTROL_HDMI_ACTIVE, 1000);
 		WARN_ONCE(ret, "Timeout waiting for "
 			  "VC4_HDMI_SCHEDULER_CONTROL_HDMI_ACTIVE\n");
 	} else {
@@ -406,7 +546,7 @@ static void vc4_hdmi_encoder_enable(struct drm_encoder *encoder)
 			   ~VC4_HDMI_SCHEDULER_CONTROL_MODE_HDMI);
 
 		ret = wait_for(!(HDMI_READ(VC4_HDMI_SCHEDULER_CONTROL) &
-				 VC4_HDMI_SCHEDULER_CONTROL_HDMI_ACTIVE), 1);
+				 VC4_HDMI_SCHEDULER_CONTROL_HDMI_ACTIVE), 1000);
 		WARN_ONCE(ret, "Timeout waiting for "
 			  "!VC4_HDMI_SCHEDULER_CONTROL_HDMI_ACTIVE\n");
 	}
@@ -420,9 +560,10 @@ static void vc4_hdmi_encoder_enable(struct drm_encoder *encoder)
 			   HDMI_READ(VC4_HDMI_SCHEDULER_CONTROL) |
 			   VC4_HDMI_SCHEDULER_CONTROL_VERT_ALWAYS_KEEPOUT);
 
-		/* XXX: Set HDMI_RAM_PACKET_CONFIG (1 << 16) and set
-		 * up the infoframe.
-		 */
+		HDMI_WRITE(VC4_HDMI_RAM_PACKET_CONFIG,
+			   VC4_HDMI_RAM_PACKET_ENABLE);
+
+		vc4_hdmi_set_infoframes(encoder);
 
 		drift = HDMI_READ(VC4_HDMI_FIFO_CTL);
 		drift &= VC4_HDMI_FIFO_VALID_WRITE_MASK;
diff --git a/drivers/gpu/drm/vc4/vc4_regs.h b/drivers/gpu/drm/vc4/vc4_regs.h
index 160942a9180e..1aa44c2db556 100644
--- a/drivers/gpu/drm/vc4/vc4_regs.h
+++ b/drivers/gpu/drm/vc4/vc4_regs.h
@@ -175,6 +175,8 @@
 # define PV_CONTROL_CLR_AT_START		BIT(14)
 # define PV_CONTROL_TRIGGER_UNDERFLOW		BIT(13)
 # define PV_CONTROL_WAIT_HSTART			BIT(12)
+# define PV_CONTROL_PIXEL_REP_MASK		VC4_MASK(5, 4)
+# define PV_CONTROL_PIXEL_REP_SHIFT		4
 # define PV_CONTROL_CLK_SELECT_DSI_VEC		0
 # define PV_CONTROL_CLK_SELECT_DPI_SMI_HDMI	1
 # define PV_CONTROL_CLK_SELECT_MASK		VC4_MASK(3, 2)
@@ -183,6 +185,9 @@
 # define PV_CONTROL_EN				BIT(0)
 
 #define PV_V_CONTROL				0x04
+# define PV_VCONTROL_ODD_DELAY_MASK		VC4_MASK(22, 6)
+# define PV_VCONTROL_ODD_DELAY_SHIFT		6
+# define PV_VCONTROL_ODD_FIRST			BIT(5)
 # define PV_VCONTROL_INTERLACE			BIT(4)
 # define PV_VCONTROL_CONTINUOUS			BIT(1)
 # define PV_VCONTROL_VIDEN			BIT(0)
@@ -438,6 +443,8 @@
 #define VC4_HDMI_RAM_PACKET_CONFIG		0x0a0
 # define VC4_HDMI_RAM_PACKET_ENABLE		BIT(16)
 
+#define VC4_HDMI_RAM_PACKET_STATUS		0x0a4
+
 #define VC4_HDMI_HORZA				0x0c4
 # define VC4_HDMI_HORZA_VPOS			BIT(14)
 # define VC4_HDMI_HORZA_HPOS			BIT(13)
@@ -499,6 +506,9 @@
 
 #define VC4_HDMI_TX_PHY_RESET_CTL		0x2c0
 
+#define VC4_HDMI_GCP_0				0x400
+#define VC4_HDMI_PACKET_STRIDE			0x24
+
 #define VC4_HD_M_CTL				0x00c
 # define VC4_HD_M_REGISTER_FILE_STANDBY		(3 << 6)
 # define VC4_HD_M_RAM_STANDBY			(3 << 4)
@@ -528,10 +538,17 @@
 # define VC4_HD_CSC_CTL_MODE_SHIFT		2
 # define VC4_HD_CSC_CTL_MODE_RGB_TO_SD_YPRPB	0
 # define VC4_HD_CSC_CTL_MODE_RGB_TO_HD_YPRPB	1
-# define VC4_HD_CSC_CTL_MODE_CUSTOM		2
+# define VC4_HD_CSC_CTL_MODE_CUSTOM		3
 # define VC4_HD_CSC_CTL_RGB2YCC			BIT(1)
 # define VC4_HD_CSC_CTL_ENABLE			BIT(0)
 
+#define VC4_HD_CSC_12_11			0x044
+#define VC4_HD_CSC_14_13			0x048
+#define VC4_HD_CSC_22_21			0x04c
+#define VC4_HD_CSC_24_23			0x050
+#define VC4_HD_CSC_32_31			0x054
+#define VC4_HD_CSC_34_33			0x058
+
 #define VC4_HD_FRAME_COUNT			0x068
 
 /* HVS display list information. */
diff --git a/drivers/gpu/drm/vc4/vc4_render_cl.c b/drivers/gpu/drm/vc4/vc4_render_cl.c
index 0f12418725e5..08886a309757 100644
--- a/drivers/gpu/drm/vc4/vc4_render_cl.c
+++ b/drivers/gpu/drm/vc4/vc4_render_cl.c
@@ -45,6 +45,8 @@ struct vc4_rcl_setup {
 
 	struct drm_gem_cma_object *rcl;
 	u32 next_offset;
+
+	u32 next_write_bo_index;
 };
 
 static inline void rcl_u8(struct vc4_rcl_setup *setup, u8 val)
@@ -407,6 +409,8 @@ static int vc4_rcl_msaa_surface_setup(struct vc4_exec_info *exec,
 	if (!*obj)
 		return -EINVAL;
 
+	exec->rcl_write_bo[exec->rcl_write_bo_count++] = *obj;
+
 	if (surf->offset & 0xf) {
 		DRM_ERROR("MSAA write must be 16b aligned.\n");
 		return -EINVAL;
@@ -417,7 +421,8 @@ static int vc4_rcl_msaa_surface_setup(struct vc4_exec_info *exec,
 
 static int vc4_rcl_surface_setup(struct vc4_exec_info *exec,
 				 struct drm_gem_cma_object **obj,
-				 struct drm_vc4_submit_rcl_surface *surf)
+				 struct drm_vc4_submit_rcl_surface *surf,
+				 bool is_write)
 {
 	uint8_t tiling = VC4_GET_FIELD(surf->bits,
 				       VC4_LOADSTORE_TILE_BUFFER_TILING);
@@ -440,6 +445,9 @@ static int vc4_rcl_surface_setup(struct vc4_exec_info *exec,
 	if (!*obj)
 		return -EINVAL;
 
+	if (is_write)
+		exec->rcl_write_bo[exec->rcl_write_bo_count++] = *obj;
+
 	if (surf->flags & VC4_SUBMIT_RCL_SURFACE_READ_IS_FULL_RES) {
 		if (surf == &exec->args->zs_write) {
 			DRM_ERROR("general zs write may not be a full-res.\n");
@@ -542,6 +550,8 @@ vc4_rcl_render_config_surface_setup(struct vc4_exec_info *exec,
 	if (!*obj)
 		return -EINVAL;
 
+	exec->rcl_write_bo[exec->rcl_write_bo_count++] = *obj;
+
 	if (tiling > VC4_TILING_FORMAT_LT) {
 		DRM_ERROR("Bad tiling format\n");
 		return -EINVAL;
@@ -599,15 +609,18 @@ int vc4_get_rcl(struct drm_device *dev, struct vc4_exec_info *exec)
 	if (ret)
 		return ret;
 
-	ret = vc4_rcl_surface_setup(exec, &setup.color_read, &args->color_read);
+	ret = vc4_rcl_surface_setup(exec, &setup.color_read, &args->color_read,
+				    false);
 	if (ret)
 		return ret;
 
-	ret = vc4_rcl_surface_setup(exec, &setup.zs_read, &args->zs_read);
+	ret = vc4_rcl_surface_setup(exec, &setup.zs_read, &args->zs_read,
+				    false);
 	if (ret)
 		return ret;
 
-	ret = vc4_rcl_surface_setup(exec, &setup.zs_write, &args->zs_write);
+	ret = vc4_rcl_surface_setup(exec, &setup.zs_write, &args->zs_write,
+				    true);
 	if (ret)
 		return ret;
 
diff --git a/drivers/gpu/drm/vc4/vc4_validate.c b/drivers/gpu/drm/vc4/vc4_validate.c
index 9ce1d0adf882..26503e307438 100644
--- a/drivers/gpu/drm/vc4/vc4_validate.c
+++ b/drivers/gpu/drm/vc4/vc4_validate.c
@@ -267,6 +267,9 @@ validate_indexed_prim_list(VALIDATE_ARGS)
 	if (!ib)
 		return -EINVAL;
 
+	exec->bin_dep_seqno = max(exec->bin_dep_seqno,
+				  to_vc4_bo(&ib->base)->write_seqno);
+
 	if (offset > ib->base.size ||
 	    (ib->base.size - offset) / index_size < length) {
 		DRM_ERROR("IB access overflow (%d + %d*%d > %zd)\n",
@@ -555,8 +558,7 @@ static bool
 reloc_tex(struct vc4_exec_info *exec,
 	  void *uniform_data_u,
 	  struct vc4_texture_sample_info *sample,
-	  uint32_t texture_handle_index)
-
+	  uint32_t texture_handle_index, bool is_cs)
 {
 	struct drm_gem_cma_object *tex;
 	uint32_t p0 = *(uint32_t *)(uniform_data_u + sample->p_offset[0]);
@@ -714,6 +716,11 @@ reloc_tex(struct vc4_exec_info *exec,
 
 	*validated_p0 = tex->paddr + p0;
 
+	if (is_cs) {
+		exec->bin_dep_seqno = max(exec->bin_dep_seqno,
+					  to_vc4_bo(&tex->base)->write_seqno);
+	}
+
 	return true;
  fail:
 	DRM_INFO("Texture p0 at %d: 0x%08x\n", sample->p_offset[0], p0);
@@ -835,7 +842,8 @@ validate_gl_shader_rec(struct drm_device *dev,
 			if (!reloc_tex(exec,
 				       uniform_data_u,
 				       &validated_shader->texture_samples[tex],
-				       texture_handles_u[tex])) {
+				       texture_handles_u[tex],
+				       i == 2)) {
 				return -EINVAL;
 			}
 		}
@@ -867,6 +875,9 @@ validate_gl_shader_rec(struct drm_device *dev,
 		uint32_t stride = *(uint8_t *)(pkt_u + o + 5);
 		uint32_t max_index;
 
+		exec->bin_dep_seqno = max(exec->bin_dep_seqno,
+					  to_vc4_bo(&vbo->base)->write_seqno);
+
 		if (state->addr & 0x8)
 			stride |= (*(uint32_t *)(pkt_u + 100 + i * 4)) & ~0xff;
 
diff --git a/drivers/gpu/drm/via/via_dmablit.c b/drivers/gpu/drm/via/via_dmablit.c
index 7e2a12c4fed2..1a3ad769f8c8 100644
--- a/drivers/gpu/drm/via/via_dmablit.c
+++ b/drivers/gpu/drm/via/via_dmablit.c
@@ -241,8 +241,8 @@ via_lock_all_dma_pages(drm_via_sg_info_t *vsg,  drm_via_dmablit_t *xfer)
 	down_read(&current->mm->mmap_sem);
 	ret = get_user_pages((unsigned long)xfer->mem_addr,
 			     vsg->num_pages,
-			     (vsg->direction == DMA_FROM_DEVICE),
-			     0, vsg->pages, NULL);
+			     (vsg->direction == DMA_FROM_DEVICE) ? FOLL_WRITE : 0,
+			     vsg->pages, NULL);
 
 	up_read(&current->mm->mmap_sem);
 	if (ret != vsg->num_pages) {
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_drv.c b/drivers/gpu/drm/vmwgfx/vmwgfx_drv.c
index e8ae3dc476d1..18061a4bc2f2 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_drv.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_drv.c
@@ -241,15 +241,15 @@ static int vmwgfx_pm_notifier(struct notifier_block *nb, unsigned long val,
 			      void *ptr);
 
 MODULE_PARM_DESC(enable_fbdev, "Enable vmwgfx fbdev");
-module_param_named(enable_fbdev, enable_fbdev, int, 0600);
+module_param_named(enable_fbdev, enable_fbdev, int, S_IRUSR | S_IWUSR);
 MODULE_PARM_DESC(force_dma_api, "Force using the DMA API for TTM pages");
-module_param_named(force_dma_api, vmw_force_iommu, int, 0600);
+module_param_named(force_dma_api, vmw_force_iommu, int, S_IRUSR | S_IWUSR);
 MODULE_PARM_DESC(restrict_iommu, "Try to limit IOMMU usage for TTM pages");
-module_param_named(restrict_iommu, vmw_restrict_iommu, int, 0600);
+module_param_named(restrict_iommu, vmw_restrict_iommu, int, S_IRUSR | S_IWUSR);
 MODULE_PARM_DESC(force_coherent, "Force coherent TTM pages");
-module_param_named(force_coherent, vmw_force_coherent, int, 0600);
+module_param_named(force_coherent, vmw_force_coherent, int, S_IRUSR | S_IWUSR);
 MODULE_PARM_DESC(restrict_dma_mask, "Restrict DMA mask to 44 bits with IOMMU");
-module_param_named(restrict_dma_mask, vmw_restrict_dma_mask, int, 0600);
+module_param_named(restrict_dma_mask, vmw_restrict_dma_mask, int, S_IRUSR | S_IWUSR);
 MODULE_PARM_DESC(assume_16bpp, "Assume 16-bpp when filtering modes");
 module_param_named(assume_16bpp, vmw_assume_16bpp, int, 0600);
 
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_drv.h b/drivers/gpu/drm/vmwgfx/vmwgfx_drv.h
index 070d750af16d..1e59a486bba8 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_drv.h
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_drv.h
@@ -43,7 +43,7 @@
 
 #define VMWGFX_DRIVER_DATE "20160210"
 #define VMWGFX_DRIVER_MAJOR 2
-#define VMWGFX_DRIVER_MINOR 10
+#define VMWGFX_DRIVER_MINOR 11
 #define VMWGFX_DRIVER_PATCHLEVEL 0
 #define VMWGFX_FILE_PAGE_OFFSET 0x00100000
 #define VMWGFX_FIFO_STATIC_SIZE (1024*1024)
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_execbuf.c b/drivers/gpu/drm/vmwgfx/vmwgfx_execbuf.c
index dc5beff2b4aa..c7b53d987f06 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_execbuf.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_execbuf.c
@@ -35,17 +35,37 @@
 #define VMW_RES_HT_ORDER 12
 
 /**
+ * enum vmw_resource_relocation_type - Relocation type for resources
+ *
+ * @vmw_res_rel_normal: Traditional relocation. The resource id in the
+ * command stream is replaced with the actual id after validation.
+ * @vmw_res_rel_nop: NOP relocation. The command is unconditionally replaced
+ * with a NOP.
+ * @vmw_res_rel_cond_nop: Conditional NOP relocation. If the resource id
+ * after validation is -1, the command is replaced with a NOP. Otherwise no
+ * action.
+ */
+enum vmw_resource_relocation_type {
+	vmw_res_rel_normal,
+	vmw_res_rel_nop,
+	vmw_res_rel_cond_nop,
+	vmw_res_rel_max
+};
+
+/**
  * struct vmw_resource_relocation - Relocation info for resources
  *
  * @head: List head for the software context's relocation list.
  * @res: Non-ref-counted pointer to the resource.
- * @offset: Offset of 4 byte entries into the command buffer where the
+ * @offset: Offset of single byte entries into the command buffer where the
  * id that needs fixup is located.
+ * @rel_type: Type of relocation.
  */
 struct vmw_resource_relocation {
 	struct list_head head;
 	const struct vmw_resource *res;
-	unsigned long offset;
+	u32 offset:29;
+	enum vmw_resource_relocation_type rel_type:3;
 };
 
 /**
@@ -109,7 +129,18 @@ static int vmw_bo_to_validate_list(struct vmw_sw_context *sw_context,
 				   struct vmw_dma_buffer *vbo,
 				   bool validate_as_mob,
 				   uint32_t *p_val_node);
-
+/**
+ * vmw_ptr_diff - Compute the offset from a to b in bytes
+ *
+ * @a: A starting pointer.
+ * @b: A pointer offset in the same address space.
+ *
+ * Returns: The offset in bytes between the two pointers.
+ */
+static size_t vmw_ptr_diff(void *a, void *b)
+{
+	return (unsigned long) b - (unsigned long) a;
+}
 
 /**
  * vmw_resources_unreserve - unreserve resources previously reserved for
@@ -409,11 +440,14 @@ static int vmw_resource_context_res_add(struct vmw_private *dev_priv,
  * @list: Pointer to head of relocation list.
  * @res: The resource.
  * @offset: Offset into the command buffer currently being parsed where the
- * id that needs fixup is located. Granularity is 4 bytes.
+ * id that needs fixup is located. Granularity is one byte.
+ * @rel_type: Relocation type.
  */
 static int vmw_resource_relocation_add(struct list_head *list,
 				       const struct vmw_resource *res,
-				       unsigned long offset)
+				       unsigned long offset,
+				       enum vmw_resource_relocation_type
+				       rel_type)
 {
 	struct vmw_resource_relocation *rel;
 
@@ -425,6 +459,7 @@ static int vmw_resource_relocation_add(struct list_head *list,
 
 	rel->res = res;
 	rel->offset = offset;
+	rel->rel_type = rel_type;
 	list_add_tail(&rel->head, list);
 
 	return 0;
@@ -459,11 +494,24 @@ static void vmw_resource_relocations_apply(uint32_t *cb,
 {
 	struct vmw_resource_relocation *rel;
 
+	/* Validate the struct vmw_resource_relocation member size */
+	BUILD_BUG_ON(SVGA_CB_MAX_SIZE >= (1 << 29));
+	BUILD_BUG_ON(vmw_res_rel_max >= (1 << 3));
+
 	list_for_each_entry(rel, list, head) {
-		if (likely(rel->res != NULL))
-			cb[rel->offset] = rel->res->id;
-		else
-			cb[rel->offset] = SVGA_3D_CMD_NOP;
+		u32 *addr = (u32 *)((unsigned long) cb + rel->offset);
+		switch (rel->rel_type) {
+		case vmw_res_rel_normal:
+			*addr = rel->res->id;
+			break;
+		case vmw_res_rel_nop:
+			*addr = SVGA_3D_CMD_NOP;
+			break;
+		default:
+			if (rel->res->id == -1)
+				*addr = SVGA_3D_CMD_NOP;
+			break;
+		}
 	}
 }
 
@@ -655,7 +703,9 @@ static int vmw_cmd_res_reloc_add(struct vmw_private *dev_priv,
 	*p_val = NULL;
 	ret = vmw_resource_relocation_add(&sw_context->res_relocations,
 					  res,
-					  id_loc - sw_context->buf_start);
+					  vmw_ptr_diff(sw_context->buf_start,
+						       id_loc),
+					  vmw_res_rel_normal);
 	if (unlikely(ret != 0))
 		return ret;
 
@@ -721,7 +771,8 @@ vmw_cmd_res_check(struct vmw_private *dev_priv,
 
 		return vmw_resource_relocation_add
 			(&sw_context->res_relocations, res,
-			 id_loc - sw_context->buf_start);
+			 vmw_ptr_diff(sw_context->buf_start, id_loc),
+			 vmw_res_rel_normal);
 	}
 
 	ret = vmw_user_resource_lookup_handle(dev_priv,
@@ -2143,10 +2194,10 @@ static int vmw_cmd_shader_define(struct vmw_private *dev_priv,
 		return ret;
 
 	return vmw_resource_relocation_add(&sw_context->res_relocations,
-					   NULL, &cmd->header.id -
-					   sw_context->buf_start);
-
-	return 0;
+					   NULL,
+					   vmw_ptr_diff(sw_context->buf_start,
+							&cmd->header.id),
+					   vmw_res_rel_nop);
 }
 
 /**
@@ -2188,10 +2239,10 @@ static int vmw_cmd_shader_destroy(struct vmw_private *dev_priv,
 		return ret;
 
 	return vmw_resource_relocation_add(&sw_context->res_relocations,
-					   NULL, &cmd->header.id -
-					   sw_context->buf_start);
-
-	return 0;
+					   NULL,
+					   vmw_ptr_diff(sw_context->buf_start,
+							&cmd->header.id),
+					   vmw_res_rel_nop);
 }
 
 /**
@@ -2848,8 +2899,7 @@ static int vmw_cmd_dx_cid_check(struct vmw_private *dev_priv,
  * @header: Pointer to the command header in the command stream.
  *
  * Check that the view exists, and if it was not created using this
- * command batch, make sure it's validated (present in the device) so that
- * the remove command will not confuse the device.
+ * command batch, conditionally make this command a NOP.
  */
 static int vmw_cmd_dx_view_remove(struct vmw_private *dev_priv,
 				  struct vmw_sw_context *sw_context,
@@ -2877,10 +2927,16 @@ static int vmw_cmd_dx_view_remove(struct vmw_private *dev_priv,
 		return ret;
 
 	/*
-	 * Add view to the validate list iff it was not created using this
-	 * command batch.
+	 * If the view wasn't created during this command batch, it might
+	 * have been removed due to a context swapout, so add a
+	 * relocation to conditionally make this command a NOP to avoid
+	 * device errors.
 	 */
-	return vmw_view_res_val_add(sw_context, view);
+	return vmw_resource_relocation_add(&sw_context->res_relocations,
+					   view,
+					   vmw_ptr_diff(sw_context->buf_start,
+							&cmd->header.id),
+					   vmw_res_rel_cond_nop);
 }
 
 /**
@@ -3029,6 +3085,35 @@ static int vmw_cmd_dx_genmips(struct vmw_private *dev_priv,
 				   cmd->body.shaderResourceViewId);
 }
 
+/**
+ * vmw_cmd_dx_transfer_from_buffer -
+ * Validate an SVGA_3D_CMD_DX_TRANSFER_FROM_BUFFER command
+ *
+ * @dev_priv: Pointer to a device private struct.
+ * @sw_context: The software context being used for this batch.
+ * @header: Pointer to the command header in the command stream.
+ */
+static int vmw_cmd_dx_transfer_from_buffer(struct vmw_private *dev_priv,
+					   struct vmw_sw_context *sw_context,
+					   SVGA3dCmdHeader *header)
+{
+	struct {
+		SVGA3dCmdHeader header;
+		SVGA3dCmdDXTransferFromBuffer body;
+	} *cmd = container_of(header, typeof(*cmd), header);
+	int ret;
+
+	ret = vmw_cmd_res_check(dev_priv, sw_context, vmw_res_surface,
+				user_surface_converter,
+				&cmd->body.srcSid, NULL);
+	if (ret != 0)
+		return ret;
+
+	return vmw_cmd_res_check(dev_priv, sw_context, vmw_res_surface,
+				 user_surface_converter,
+				 &cmd->body.destSid, NULL);
+}
+
 static int vmw_cmd_check_not_3d(struct vmw_private *dev_priv,
 				struct vmw_sw_context *sw_context,
 				void *buf, uint32_t *size)
@@ -3379,6 +3464,9 @@ static const struct vmw_cmd_entry vmw_cmd_entries[SVGA_3D_CMD_MAX] = {
 		    &vmw_cmd_buffer_copy_check, true, false, true),
 	VMW_CMD_DEF(SVGA_3D_CMD_DX_PRED_COPY_REGION,
 		    &vmw_cmd_pred_copy_check, true, false, true),
+	VMW_CMD_DEF(SVGA_3D_CMD_DX_TRANSFER_FROM_BUFFER,
+		    &vmw_cmd_dx_transfer_from_buffer,
+		    true, false, true),
 };
 
 static int vmw_cmd_check(struct vmw_private *dev_priv,
@@ -3848,14 +3936,14 @@ static void *vmw_execbuf_cmdbuf(struct vmw_private *dev_priv,
 	int ret;
 
 	*header = NULL;
-	if (!dev_priv->cman || kernel_commands)
-		return kernel_commands;
-
 	if (command_size > SVGA_CB_MAX_SIZE) {
 		DRM_ERROR("Command buffer is too large.\n");
 		return ERR_PTR(-EINVAL);
 	}
 
+	if (!dev_priv->cman || kernel_commands)
+		return kernel_commands;
+
 	/* If possible, add a little space for fencing. */
 	cmdbuf_size = command_size + 512;
 	cmdbuf_size = min_t(size_t, cmdbuf_size, SVGA_CB_MAX_SIZE);
@@ -4232,9 +4320,6 @@ void __vmw_execbuf_release_pinned_bo(struct vmw_private *dev_priv,
 	ttm_bo_unref(&query_val.bo);
 	ttm_bo_unref(&pinned_val.bo);
 	vmw_dmabuf_unreference(&dev_priv->pinned_bo);
-	DRM_INFO("Dummy query bo pin count: %d\n",
-		 dev_priv->dummy_query_bo->pin_count);
-
 out_unlock:
 	return;
 
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_surface.c b/drivers/gpu/drm/vmwgfx/vmwgfx_surface.c
index c2a721a8cef9..b445ce9b9757 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_surface.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_surface.c
@@ -324,7 +324,7 @@ static void vmw_hw_surface_destroy(struct vmw_resource *res)
 	if (res->id != -1) {
 
 		cmd = vmw_fifo_reserve(dev_priv, vmw_surface_destroy_size());
-		if (unlikely(cmd == NULL)) {
+		if (unlikely(!cmd)) {
 			DRM_ERROR("Failed reserving FIFO space for surface "
 				  "destruction.\n");
 			return;
@@ -397,7 +397,7 @@ static int vmw_legacy_srf_create(struct vmw_resource *res)
 
 	submit_size = vmw_surface_define_size(srf);
 	cmd = vmw_fifo_reserve(dev_priv, submit_size);
-	if (unlikely(cmd == NULL)) {
+	if (unlikely(!cmd)) {
 		DRM_ERROR("Failed reserving FIFO space for surface "
 			  "creation.\n");
 		ret = -ENOMEM;
@@ -446,11 +446,10 @@ static int vmw_legacy_srf_dma(struct vmw_resource *res,
 	uint8_t *cmd;
 	struct vmw_private *dev_priv = res->dev_priv;
 
-	BUG_ON(val_buf->bo == NULL);
-
+	BUG_ON(!val_buf->bo);
 	submit_size = vmw_surface_dma_size(srf);
 	cmd = vmw_fifo_reserve(dev_priv, submit_size);
-	if (unlikely(cmd == NULL)) {
+	if (unlikely(!cmd)) {
 		DRM_ERROR("Failed reserving FIFO space for surface "
 			  "DMA.\n");
 		return -ENOMEM;
@@ -538,7 +537,7 @@ static int vmw_legacy_srf_destroy(struct vmw_resource *res)
 
 	submit_size = vmw_surface_destroy_size();
 	cmd = vmw_fifo_reserve(dev_priv, submit_size);
-	if (unlikely(cmd == NULL)) {
+	if (unlikely(!cmd)) {
 		DRM_ERROR("Failed reserving FIFO space for surface "
 			  "eviction.\n");
 		return -ENOMEM;
@@ -578,7 +577,7 @@ static int vmw_surface_init(struct vmw_private *dev_priv,
 	int ret;
 	struct vmw_resource *res = &srf->res;
 
-	BUG_ON(res_free == NULL);
+	BUG_ON(!res_free);
 	if (!dev_priv->has_mob)
 		vmw_fifo_resource_inc(dev_priv);
 	ret = vmw_resource_init(dev_priv, res, true, res_free,
@@ -700,7 +699,6 @@ int vmw_surface_define_ioctl(struct drm_device *dev, void *data,
 	struct drm_vmw_surface_create_req *req = &arg->req;
 	struct drm_vmw_surface_arg *rep = &arg->rep;
 	struct ttm_object_file *tfile = vmw_fpriv(file_priv)->tfile;
-	struct drm_vmw_size __user *user_sizes;
 	int ret;
 	int i, j;
 	uint32_t cur_bo_offset;
@@ -748,7 +746,7 @@ int vmw_surface_define_ioctl(struct drm_device *dev, void *data,
 	}
 
 	user_srf = kzalloc(sizeof(*user_srf), GFP_KERNEL);
-	if (unlikely(user_srf == NULL)) {
+	if (unlikely(!user_srf)) {
 		ret = -ENOMEM;
 		goto out_no_user_srf;
 	}
@@ -763,29 +761,21 @@ int vmw_surface_define_ioctl(struct drm_device *dev, void *data,
 	memcpy(srf->mip_levels, req->mip_levels, sizeof(srf->mip_levels));
 	srf->num_sizes = num_sizes;
 	user_srf->size = size;
-
-	srf->sizes = kmalloc(srf->num_sizes * sizeof(*srf->sizes), GFP_KERNEL);
-	if (unlikely(srf->sizes == NULL)) {
-		ret = -ENOMEM;
+	srf->sizes = memdup_user((struct drm_vmw_size __user *)(unsigned long)
+				 req->size_addr,
+				 sizeof(*srf->sizes) * srf->num_sizes);
+	if (IS_ERR(srf->sizes)) {
+		ret = PTR_ERR(srf->sizes);
 		goto out_no_sizes;
 	}
-	srf->offsets = kmalloc(srf->num_sizes * sizeof(*srf->offsets),
-			       GFP_KERNEL);
-	if (unlikely(srf->offsets == NULL)) {
+	srf->offsets = kmalloc_array(srf->num_sizes,
+				     sizeof(*srf->offsets),
+				     GFP_KERNEL);
+	if (unlikely(!srf->offsets)) {
 		ret = -ENOMEM;
 		goto out_no_offsets;
 	}
 
-	user_sizes = (struct drm_vmw_size __user *)(unsigned long)
-	    req->size_addr;
-
-	ret = copy_from_user(srf->sizes, user_sizes,
-			     srf->num_sizes * sizeof(*srf->sizes));
-	if (unlikely(ret != 0)) {
-		ret = -EFAULT;
-		goto out_no_copy;
-	}
-
 	srf->base_size = *srf->sizes;
 	srf->autogen_filter = SVGA3D_TEX_FILTER_NONE;
 	srf->multisample_count = 0;
@@ -923,7 +913,7 @@ vmw_surface_handle_reference(struct vmw_private *dev_priv,
 
 	ret = -EINVAL;
 	base = ttm_base_object_lookup_for_ref(dev_priv->tdev, handle);
-	if (unlikely(base == NULL)) {
+	if (unlikely(!base)) {
 		DRM_ERROR("Could not find surface to reference.\n");
 		goto out_no_lookup;
 	}
@@ -1069,7 +1059,7 @@ static int vmw_gb_surface_create(struct vmw_resource *res)
 
 	cmd = vmw_fifo_reserve(dev_priv, submit_len);
 	cmd2 = (typeof(cmd2))cmd;
-	if (unlikely(cmd == NULL)) {
+	if (unlikely(!cmd)) {
 		DRM_ERROR("Failed reserving FIFO space for surface "
 			  "creation.\n");
 		ret = -ENOMEM;
@@ -1135,7 +1125,7 @@ static int vmw_gb_surface_bind(struct vmw_resource *res,
 	submit_size = sizeof(*cmd1) + (res->backup_dirty ? sizeof(*cmd2) : 0);
 
 	cmd1 = vmw_fifo_reserve(dev_priv, submit_size);
-	if (unlikely(cmd1 == NULL)) {
+	if (unlikely(!cmd1)) {
 		DRM_ERROR("Failed reserving FIFO space for surface "
 			  "binding.\n");
 		return -ENOMEM;
@@ -1185,7 +1175,7 @@ static int vmw_gb_surface_unbind(struct vmw_resource *res,
 
 	submit_size = sizeof(*cmd3) + (readback ? sizeof(*cmd1) : sizeof(*cmd2));
 	cmd = vmw_fifo_reserve(dev_priv, submit_size);
-	if (unlikely(cmd == NULL)) {
+	if (unlikely(!cmd)) {
 		DRM_ERROR("Failed reserving FIFO space for surface "
 			  "unbinding.\n");
 		return -ENOMEM;
@@ -1244,7 +1234,7 @@ static int vmw_gb_surface_destroy(struct vmw_resource *res)
 	vmw_binding_res_list_scrub(&res->binding_head);
 
 	cmd = vmw_fifo_reserve(dev_priv, sizeof(*cmd));
-	if (unlikely(cmd == NULL)) {
+	if (unlikely(!cmd)) {
 		DRM_ERROR("Failed reserving FIFO space for surface "
 			  "destruction.\n");
 		mutex_unlock(&dev_priv->binding_mutex);
@@ -1410,7 +1400,7 @@ int vmw_gb_surface_reference_ioctl(struct drm_device *dev, void *data,
 
 	user_srf = container_of(base, struct vmw_user_surface, prime.base);
 	srf = &user_srf->srf;
-	if (srf->res.backup == NULL) {
+	if (!srf->res.backup) {
 		DRM_ERROR("Shared GB surface is missing a backup buffer.\n");
 		goto out_bad_resource;
 	}
@@ -1524,7 +1514,7 @@ int vmw_surface_gb_priv_define(struct drm_device *dev,
 	}
 
 	user_srf = kzalloc(sizeof(*user_srf), GFP_KERNEL);
-	if (unlikely(user_srf == NULL)) {
+	if (unlikely(!user_srf)) {
 		ret = -ENOMEM;
 		goto out_no_user_srf;
 	}
diff --git a/drivers/hid/Kconfig b/drivers/hid/Kconfig
index 78ac4811bd3c..cd4599c0523b 100644
--- a/drivers/hid/Kconfig
+++ b/drivers/hid/Kconfig
@@ -457,8 +457,6 @@ config LOGITECH_FF
 	  - Logitech WingMan Cordless RumblePad
 	  - Logitech WingMan Cordless RumblePad 2
 	  - Logitech WingMan Force 3D
-	  - Logitech Formula Force EX
-	  - Logitech WingMan Formula Force GP
 
 	  and if you want to enable force feedback for them.
 	  Note: if you say N here, this device will still be supported, but without
@@ -488,15 +486,22 @@ config LOGIWHEELS_FF
 	select INPUT_FF_MEMLESS
 	default LOGITECH_FF
 	help
-	  Say Y here if you want to enable force feedback and range setting
+	  Say Y here if you want to enable force feedback and range setting(*)
 	  support for following Logitech wheels:
+	  - Logitech G25 (*)
+	  - Logitech G27 (*)
+	  - Logitech G29 (*)
 	  - Logitech Driving Force
-	  - Logitech Driving Force Pro
-	  - Logitech Driving Force GT
-	  - Logitech G25
-	  - Logitech G27
-	  - Logitech MOMO/MOMO 2
-	  - Logitech Formula Force EX
+	  - Logitech Driving Force Pro (*)
+	  - Logitech Driving Force GT (*)
+	  - Logitech Driving Force EX/RX
+	  - Logitech Driving Force Wireless
+	  - Logitech Speed Force Wireless
+	  - Logitech MOMO Force
+	  - Logitech MOMO Racing Force
+	  - Logitech Formula Force GP
+	  - Logitech Formula Force EX/RX
+	  - Logitech Wingman Formula Force GP
 
 config HID_MAGICMOUSE
 	tristate "Apple Magic Mouse/Trackpad multi-touch support"
@@ -862,6 +867,7 @@ config HID_WACOM
 	select POWER_SUPPLY
 	select NEW_LEDS
 	select LEDS_CLASS
+	select LEDS_TRIGGERS
 	help
 	  Say Y here if you want to use the USB or BT version of the Wacom Intuos
 	  or Graphire tablet.
@@ -967,4 +973,6 @@ source "drivers/hid/usbhid/Kconfig"
 
 source "drivers/hid/i2c-hid/Kconfig"
 
+source "drivers/hid/intel-ish-hid/Kconfig"
+
 endmenu
diff --git a/drivers/hid/Makefile b/drivers/hid/Makefile
index fc4b2aa47f2e..86b2b5785fd2 100644
--- a/drivers/hid/Makefile
+++ b/drivers/hid/Makefile
@@ -113,3 +113,5 @@ obj-$(CONFIG_USB_MOUSE)		+= usbhid/
 obj-$(CONFIG_USB_KBD)		+= usbhid/
 
 obj-$(CONFIG_I2C_HID)		+= i2c-hid/
+
+obj-$(CONFIG_INTEL_ISH_HID)	+= intel-ish-hid/
diff --git a/drivers/hid/hid-alps.c b/drivers/hid/hid-alps.c
index 048befde295a..ed9c0ea5b026 100644
--- a/drivers/hid/hid-alps.c
+++ b/drivers/hid/hid-alps.c
@@ -139,8 +139,8 @@ static int u1_read_write_register(struct hid_device *hdev, u32 address,
 	if (read_flag) {
 		readbuf = kzalloc(U1_FEATURE_REPORT_LEN, GFP_KERNEL);
 		if (!readbuf) {
-			kfree(input);
-			return -ENOMEM;
+			ret = -ENOMEM;
+			goto exit;
 		}
 
 		ret = hid_hw_raw_request(hdev, U1_FEATURE_REPORT_ID, readbuf,
@@ -149,6 +149,7 @@ static int u1_read_write_register(struct hid_device *hdev, u32 address,
 
 		if (ret < 0) {
 			dev_err(&hdev->dev, "failed read register (%d)\n", ret);
+			kfree(readbuf);
 			goto exit;
 		}
 
@@ -190,16 +191,16 @@ static int alps_raw_event(struct hid_device *hdev,
 			if (z != 0) {
 				input_mt_report_slot_state(hdata->input,
 					MT_TOOL_FINGER, 1);
+				input_report_abs(hdata->input,
+					ABS_MT_POSITION_X, x);
+				input_report_abs(hdata->input,
+					ABS_MT_POSITION_Y, y);
+				input_report_abs(hdata->input,
+					ABS_MT_PRESSURE, z);
 			} else {
 				input_mt_report_slot_state(hdata->input,
 					MT_TOOL_FINGER, 0);
-				break;
 			}
-
-			input_report_abs(hdata->input, ABS_MT_POSITION_X, x);
-			input_report_abs(hdata->input, ABS_MT_POSITION_Y, y);
-			input_report_abs(hdata->input, ABS_MT_PRESSURE, z);
-
 		}
 
 		input_mt_sync_frame(hdata->input);
@@ -244,13 +245,13 @@ static int alps_raw_event(struct hid_device *hdev,
 static int alps_post_reset(struct hid_device *hdev)
 {
 	return u1_read_write_register(hdev, ADDRESS_U1_DEV_CTRL_1,
-				NULL, U1_TP_ABS_MODE, false);
+				NULL, U1_TP_ABS_MODE | U1_SP_ABS_MODE, false);
 }
 
 static int alps_post_resume(struct hid_device *hdev)
 {
 	return u1_read_write_register(hdev, ADDRESS_U1_DEV_CTRL_1,
-				NULL, U1_TP_ABS_MODE, false);
+				NULL, U1_TP_ABS_MODE | U1_SP_ABS_MODE, false);
 }
 #endif /* CONFIG_PM */
 
@@ -383,7 +384,7 @@ static int alps_input_configured(struct hid_device *hdev, struct hid_input *hi)
 
 		input2 = input_allocate_device();
 		if (!input2) {
-			input_free_device(input2);
+			ret = -ENOMEM;
 			goto exit;
 		}
 
@@ -425,7 +426,8 @@ static int alps_input_configured(struct hid_device *hdev, struct hid_input *hi)
 		__set_bit(INPUT_PROP_POINTER, input2->propbit);
 		__set_bit(INPUT_PROP_POINTING_STICK, input2->propbit);
 
-		if (input_register_device(data->input2)) {
+		ret = input_register_device(data->input2);
+		if (ret) {
 			input_free_device(input2);
 			goto exit;
 		}
diff --git a/drivers/hid/hid-core.c b/drivers/hid/hid-core.c
index 08f53c7fd513..2b89c701076f 100644
--- a/drivers/hid/hid-core.c
+++ b/drivers/hid/hid-core.c
@@ -727,6 +727,7 @@ static void hid_scan_collection(struct hid_parser *parser, unsigned type)
 	    (hid->product == USB_DEVICE_ID_MS_TYPE_COVER_PRO_3 ||
 	     hid->product == USB_DEVICE_ID_MS_TYPE_COVER_PRO_3_2 ||
 	     hid->product == USB_DEVICE_ID_MS_TYPE_COVER_PRO_3_JP ||
+	     hid->product == USB_DEVICE_ID_MS_TYPE_COVER_PRO_4_JP ||
 	     hid->product == USB_DEVICE_ID_MS_TYPE_COVER_3 ||
 	     hid->product == USB_DEVICE_ID_MS_POWER_COVER) &&
 	    hid->group == HID_GROUP_MULTITOUCH)
@@ -1916,7 +1917,7 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_KYE, USB_DEVICE_ID_KYE_ERGO_525V) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_KYE, USB_DEVICE_ID_KYE_EASYPEN_I405X) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_KYE, USB_DEVICE_ID_KYE_MOUSEPEN_I608X) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_KYE, USB_DEVICE_ID_KYE_MOUSEPEN_I608X_2) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_KYE, USB_DEVICE_ID_KYE_MOUSEPEN_I608X_V2) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_KYE, USB_DEVICE_ID_KYE_EASYPEN_M610X) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_KYE, USB_DEVICE_ID_KYE_PENSKETCH_M912) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LABTEC, USB_DEVICE_ID_LABTEC_WIRELESS_KEYBOARD) },
@@ -1982,6 +1983,7 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_TYPE_COVER_PRO_3) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_TYPE_COVER_PRO_3_2) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_TYPE_COVER_PRO_3_JP) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_TYPE_COVER_PRO_4_JP) },
 	{ 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_DIGITAL_MEDIA_7K) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_DIGITAL_MEDIA_600) },
@@ -2037,6 +2039,7 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_SAITEK, USB_DEVICE_ID_SAITEK_PS1000) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_SAITEK, USB_DEVICE_ID_SAITEK_RAT7_OLD) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_SAITEK, USB_DEVICE_ID_SAITEK_RAT7) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_SAITEK, USB_DEVICE_ID_SAITEK_RAT9) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_SAITEK, USB_DEVICE_ID_SAITEK_MMO7) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_MADCATZ, USB_DEVICE_ID_MADCATZ_RAT5) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_MADCATZ, USB_DEVICE_ID_MADCATZ_RAT9) },
@@ -2083,6 +2086,11 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_UCLOGIC, USB_DEVICE_ID_UCLOGIC_TABLET_WP1062) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_UCLOGIC, USB_DEVICE_ID_UCLOGIC_WIRELESS_TABLET_TWHL850) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_UCLOGIC, USB_DEVICE_ID_UCLOGIC_TABLET_TWHA60) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_UCLOGIC, USB_DEVICE_ID_YIYNOVA_TABLET) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_UCLOGIC, USB_DEVICE_ID_UGEE_TABLET_81) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_UCLOGIC, USB_DEVICE_ID_UGEE_TABLET_45) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_UCLOGIC, USB_DEVICE_ID_UCLOGIC_DRAWIMAGE_G3) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_UGTIZER, USB_DEVICE_ID_UGTIZER_TABLET_GP0610) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_WISEGROUP, USB_DEVICE_ID_SMARTJOY_PLUS) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_WISEGROUP, USB_DEVICE_ID_SUPER_JOY_BOX_3) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_WISEGROUP, USB_DEVICE_ID_DUAL_USB_JOYPAD) },
@@ -2480,7 +2488,7 @@ static const struct hid_device_id hid_ignore_list[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_PANJIT, 0x0004) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_PHILIPS, USB_DEVICE_ID_PHILIPS_IEEE802154_DONGLE) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_POWERCOM, USB_DEVICE_ID_POWERCOM_UPS) },
-#if defined(CONFIG_MOUSE_SYNAPTICS_USB) || defined(CONFIG_MOUSE_SYNAPTICS_USB_MODULE)
+#if IS_ENABLED(CONFIG_MOUSE_SYNAPTICS_USB)
 	{ HID_USB_DEVICE(USB_VENDOR_ID_SYNAPTICS, USB_DEVICE_ID_SYNAPTICS_TP) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_SYNAPTICS, USB_DEVICE_ID_SYNAPTICS_INT_TP) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_SYNAPTICS, USB_DEVICE_ID_SYNAPTICS_CPAD) },
diff --git a/drivers/hid/hid-dr.c b/drivers/hid/hid-dr.c
index 8fd4bf77f264..818ea7d93533 100644
--- a/drivers/hid/hid-dr.c
+++ b/drivers/hid/hid-dr.c
@@ -234,58 +234,6 @@ static __u8 pid0011_rdesc_fixed[] = {
 	0xC0                /*  End Collection                  */
 };
 
-static __u8 pid0006_rdesc_fixed[] = {
-	0x05, 0x01,        /* Usage Page (Generic Desktop)	*/
-	0x09, 0x04,        /* Usage (Joystick)			*/
-	0xA1, 0x01,        /* Collection (Application)		*/
-	0xA1, 0x02,        /*   Collection (Logical)		*/
-	0x75, 0x08,        /*     Report Size (8)		*/
-	0x95, 0x05,        /*     Report Count (5)		*/
-	0x15, 0x00,        /*     Logical Minimum (0)		*/
-	0x26, 0xFF, 0x00,  /*     Logical Maximum (255)		*/
-	0x35, 0x00,        /*     Physical Minimum (0)		*/
-	0x46, 0xFF, 0x00,  /*     Physical Maximum (255)	*/
-	0x09, 0x30,        /*     Usage (X)			*/
-	0x09, 0x33,        /*     Usage (Ry)			*/
-	0x09, 0x32,        /*     Usage (Z)			*/
-	0x09, 0x31,        /*     Usage (Y)			*/
-	0x09, 0x34,        /*     Usage (Ry)			*/
-	0x81, 0x02,        /*     Input (Variable)		*/
-	0x75, 0x04,        /*     Report Size (4)		*/
-	0x95, 0x01,        /*     Report Count (1)		*/
-	0x25, 0x07,        /*     Logical Maximum (7)		*/
-	0x46, 0x3B, 0x01,  /*     Physical Maximum (315)	*/
-	0x65, 0x14,        /*     Unit (Centimeter)		*/
-	0x09, 0x39,        /*     Usage (Hat switch)		*/
-	0x81, 0x42,        /*     Input (Variable)		*/
-	0x65, 0x00,        /*     Unit (None)			*/
-	0x75, 0x01,        /*     Report Size (1)		*/
-	0x95, 0x0C,        /*     Report Count (12)		*/
-	0x25, 0x01,        /*     Logical Maximum (1)		*/
-	0x45, 0x01,        /*     Physical Maximum (1)		*/
-	0x05, 0x09,        /*     Usage Page (Button)		*/
-	0x19, 0x01,        /*     Usage Minimum (0x01)		*/
-	0x29, 0x0C,        /*     Usage Maximum (0x0C)		*/
-	0x81, 0x02,        /*     Input (Variable)		*/
-	0x06, 0x00, 0xFF,  /*     Usage Page (Vendor Defined)	*/
-	0x75, 0x01,        /*     Report Size (1)		*/
-	0x95, 0x08,        /*     Report Count (8)		*/
-	0x25, 0x01,        /*     Logical Maximum (1)		*/
-	0x45, 0x01,        /*     Physical Maximum (1)		*/
-	0x09, 0x01,        /*     Usage (0x01)			*/
-	0x81, 0x02,        /*     Input (Variable)		*/
-	0xC0,              /*   End Collection			*/
-	0xA1, 0x02,        /*   Collection (Logical)		*/
-	0x75, 0x08,        /*     Report Size (8)		*/
-	0x95, 0x07,        /*     Report Count (7)		*/
-	0x46, 0xFF, 0x00,  /*     Physical Maximum (255)	*/
-	0x26, 0xFF, 0x00,  /*     Logical Maximum (255)		*/
-	0x09, 0x02,        /*     Usage (0x02)			*/
-	0x91, 0x02,        /*     Output (Variable)		*/
-	0xC0,              /*   End Collection			*/
-	0xC0               /* End Collection			*/
-};
-
 static __u8 *dr_report_fixup(struct hid_device *hdev, __u8 *rdesc,
 				unsigned int *rsize)
 {
@@ -296,16 +244,34 @@ static __u8 *dr_report_fixup(struct hid_device *hdev, __u8 *rdesc,
 			*rsize = sizeof(pid0011_rdesc_fixed);
 		}
 		break;
-	case 0x0006:
-		if (*rsize == sizeof(pid0006_rdesc_fixed)) {
-			rdesc = pid0006_rdesc_fixed;
-			*rsize = sizeof(pid0006_rdesc_fixed);
-		}
-		break;
 	}
 	return rdesc;
 }
 
+#define map_abs(c)      hid_map_usage(hi, usage, bit, max, EV_ABS, (c))
+#define map_rel(c)      hid_map_usage(hi, usage, bit, max, EV_REL, (c))
+
+static int dr_input_mapping(struct hid_device *hdev, struct hid_input *hi,
+			    struct hid_field *field, struct hid_usage *usage,
+			    unsigned long **bit, int *max)
+{
+	switch (usage->hid) {
+	/*
+	 * revert to the old hid-input behavior where axes
+	 * can be randomly assigned when hid->usage is reused.
+	 */
+	case HID_GD_X: case HID_GD_Y: case HID_GD_Z:
+	case HID_GD_RX: case HID_GD_RY: case HID_GD_RZ:
+		if (field->flags & HID_MAIN_ITEM_RELATIVE)
+			map_rel(usage->hid & 0xf);
+		else
+			map_abs(usage->hid & 0xf);
+		return 1;
+	}
+
+	return 0;
+}
+
 static int dr_probe(struct hid_device *hdev, const struct hid_device_id *id)
 {
 	int ret;
@@ -352,6 +318,7 @@ static struct hid_driver dr_driver = {
 	.id_table = dr_devices,
 	.report_fixup = dr_report_fixup,
 	.probe = dr_probe,
+	.input_mapping = dr_input_mapping,
 };
 module_hid_driver(dr_driver);
 
diff --git a/drivers/hid/hid-ids.h b/drivers/hid/hid-ids.h
index 4ed9a4fdfea7..6cfb5cacc253 100644
--- a/drivers/hid/hid-ids.h
+++ b/drivers/hid/hid-ids.h
@@ -64,6 +64,9 @@
 #define USB_VENDOR_ID_AKAI		0x2011
 #define USB_DEVICE_ID_AKAI_MPKMINI2	0x0715
 
+#define USB_VENDOR_ID_AKAI_09E8		0x09E8
+#define USB_DEVICE_ID_AKAI_09E8_MIDIMIX	0x0031
+
 #define USB_VENDOR_ID_ALCOR		0x058f
 #define USB_DEVICE_ID_ALCOR_USBRS232	0x9720
 
@@ -268,6 +271,7 @@
 #define USB_DEVICE_ID_CORSAIR_K95RGB    0x1b11
 #define USB_DEVICE_ID_CORSAIR_M65RGB    0x1b12
 #define USB_DEVICE_ID_CORSAIR_K70RGB    0x1b13
+#define USB_DEVICE_ID_CORSAIR_STRAFE    0x1b15
 #define USB_DEVICE_ID_CORSAIR_K65RGB    0x1b17
 
 #define USB_VENDOR_ID_CREATIVELABS	0x041e
@@ -565,7 +569,7 @@
 #define USB_DEVICE_ID_KYE_GPEN_560	0x5003
 #define USB_DEVICE_ID_KYE_EASYPEN_I405X	0x5010
 #define USB_DEVICE_ID_KYE_MOUSEPEN_I608X	0x5011
-#define USB_DEVICE_ID_KYE_MOUSEPEN_I608X_2	0x501a
+#define USB_DEVICE_ID_KYE_MOUSEPEN_I608X_V2	0x501a
 #define USB_DEVICE_ID_KYE_EASYPEN_M610X	0x5013
 #define USB_DEVICE_ID_KYE_PENSKETCH_M912	0x5015
 
@@ -713,6 +717,7 @@
 #define USB_DEVICE_ID_MS_TYPE_COVER_PRO_3    0x07dc
 #define USB_DEVICE_ID_MS_TYPE_COVER_PRO_3_2  0x07e2
 #define USB_DEVICE_ID_MS_TYPE_COVER_PRO_3_JP 0x07dd
+#define USB_DEVICE_ID_MS_TYPE_COVER_PRO_4_JP 0x07e9
 #define USB_DEVICE_ID_MS_TYPE_COVER_3    0x07de
 #define USB_DEVICE_ID_MS_POWER_COVER     0x07da
 
@@ -859,6 +864,7 @@
 #define USB_DEVICE_ID_SAITEK_PS1000	0x0621
 #define USB_DEVICE_ID_SAITEK_RAT7_OLD	0x0ccb
 #define USB_DEVICE_ID_SAITEK_RAT7	0x0cd7
+#define USB_DEVICE_ID_SAITEK_RAT9	0x0cfa
 #define USB_DEVICE_ID_SAITEK_MMO7	0x0cd0
 
 #define USB_VENDOR_ID_SAMSUNG		0x0419
@@ -996,6 +1002,10 @@
 #define USB_DEVICE_ID_UCLOGIC_TABLET_WP1062	0x0064
 #define USB_DEVICE_ID_UCLOGIC_WIRELESS_TABLET_TWHL850	0x0522
 #define USB_DEVICE_ID_UCLOGIC_TABLET_TWHA60	0x0781
+#define USB_DEVICE_ID_UCLOGIC_DRAWIMAGE_G3	0x3031
+#define USB_DEVICE_ID_UGEE_TABLET_81		0x0081
+#define USB_DEVICE_ID_UGEE_TABLET_45		0x0045
+#define USB_DEVICE_ID_YIYNOVA_TABLET		0x004d
 
 #define USB_VENDOR_ID_UNITEC	0x227d
 #define USB_DEVICE_ID_UNITEC_USB_TOUCH_0709	0x0709
@@ -1085,4 +1095,7 @@
 #define USB_DEVICE_ID_RAPHNET_2NES2SNES	0x0002
 #define USB_DEVICE_ID_RAPHNET_4NES4SNES	0x0003
 
+#define USB_VENDOR_ID_UGTIZER			0x2179
+#define USB_DEVICE_ID_UGTIZER_TABLET_GP0610	0x0053
+
 #endif
diff --git a/drivers/hid/hid-input.c b/drivers/hid/hid-input.c
index bcfaf32d9e5e..fb9ace1cef8b 100644
--- a/drivers/hid/hid-input.c
+++ b/drivers/hid/hid-input.c
@@ -604,6 +604,15 @@ static void hidinput_configure_usage(struct hid_input *hidinput, struct hid_fiel
 			break;
 		}
 
+		/*
+		 * Some lazy vendors declare 255 usages for System Control,
+		 * leading to the creation of ABS_X|Y axis and too many others.
+		 * It wouldn't be a problem if joydev doesn't consider the
+		 * device as a joystick then.
+		 */
+		if (field->application == HID_GD_SYSTEM_CONTROL)
+			goto ignore;
+
 		if ((usage->hid & 0xf0) == 0x90) {	/* D-pad */
 			switch (usage->hid) {
 			case HID_GD_UP:	   usage->hat_dir = 1; break;
@@ -953,6 +962,7 @@ static void hidinput_configure_usage(struct hid_input *hidinput, struct hid_fiel
 	case HID_UP_HPVENDOR2:
 		set_bit(EV_REP, input->evbit);
 		switch (usage->hid & HID_USAGE) {
+		case 0x001: map_key_clear(KEY_MICMUTE);		break;
 		case 0x003: map_key_clear(KEY_BRIGHTNESSDOWN);	break;
 		case 0x004: map_key_clear(KEY_BRIGHTNESSUP);	break;
 		default:    goto ignore;
diff --git a/drivers/hid/hid-kye.c b/drivers/hid/hid-kye.c
index 32e6d8d9ded0..0dd1167b2c9b 100644
--- a/drivers/hid/hid-kye.c
+++ b/drivers/hid/hid-kye.c
@@ -19,11 +19,6 @@
 
 #include "hid-ids.h"
 
-/*
- * See EasyPen i405X description, device and HID report descriptors at
- * http://sf.net/apps/mediawiki/digimend/?title=KYE_EasyPen_i405X
- */
-
 /* Original EasyPen i405X report descriptor size */
 #define EASYPEN_I405X_RDESC_ORIG_SIZE	476
 
@@ -82,11 +77,6 @@ static __u8 easypen_i405x_rdesc_fixed[] = {
 	0xC0              /*  End Collection                  */
 };
 
-/*
- * See MousePen i608X description, device and HID report descriptors at
- * http://sf.net/apps/mediawiki/digimend/?title=KYE_MousePen_i608X
- */
-
 /* Original MousePen i608X report descriptor size */
 #define MOUSEPEN_I608X_RDESC_ORIG_SIZE	476
 
@@ -186,10 +176,104 @@ static __u8 mousepen_i608x_rdesc_fixed[] = {
 	0xC0              /*  End Collection                  */
 };
 
-/*
- * See EasyPen M610X description, device and HID report descriptors at
- * http://sf.net/apps/mediawiki/digimend/?title=KYE_EasyPen_M610X
- */
+/* Original MousePen i608X v2 report descriptor size */
+#define MOUSEPEN_I608X_V2_RDESC_ORIG_SIZE	482
+
+/* Fixed MousePen i608X v2 report descriptor */
+static __u8 mousepen_i608x_v2_rdesc_fixed[] = {
+	0x06, 0x00, 0xFF,             /*  Usage Page (FF00h),             */
+	0x09, 0x01,                   /*  Usage (01h),                    */
+	0xA1, 0x01,                   /*  Collection (Application),       */
+	0x85, 0x05,                   /*    Report ID (5),                */
+	0x09, 0x01,                   /*    Usage (01h),                  */
+	0x15, 0x80,                   /*    Logical Minimum (-128),       */
+	0x25, 0x7F,                   /*    Logical Maximum (127),        */
+	0x75, 0x08,                   /*    Report Size (8),              */
+	0x95, 0x07,                   /*    Report Count (7),             */
+	0xB1, 0x02,                   /*    Feature (Variable),           */
+	0xC0,                         /*  End Collection,                 */
+	0x05, 0x0D,                   /*  Usage Page (Digitizer),         */
+	0x09, 0x02,                   /*  Usage (Pen),                    */
+	0xA1, 0x01,                   /*  Collection (Application),       */
+	0x85, 0x10,                   /*    Report ID (16),               */
+	0x09, 0x20,                   /*    Usage (Stylus),               */
+	0xA0,                         /*    Collection (Physical),        */
+	0x14,                         /*      Logical Minimum (0),        */
+	0x25, 0x01,                   /*      Logical Maximum (1),        */
+	0x75, 0x01,                   /*      Report Size (1),            */
+	0x09, 0x42,                   /*      Usage (Tip Switch),         */
+	0x09, 0x44,                   /*      Usage (Barrel Switch),      */
+	0x09, 0x46,                   /*      Usage (Tablet Pick),        */
+	0x95, 0x03,                   /*      Report Count (3),           */
+	0x81, 0x02,                   /*      Input (Variable),           */
+	0x95, 0x04,                   /*      Report Count (4),           */
+	0x81, 0x03,                   /*      Input (Constant, Variable), */
+	0x09, 0x32,                   /*      Usage (In Range),           */
+	0x95, 0x01,                   /*      Report Count (1),           */
+	0x81, 0x02,                   /*      Input (Variable),           */
+	0x75, 0x10,                   /*      Report Size (16),           */
+	0x95, 0x01,                   /*      Report Count (1),           */
+	0xA4,                         /*      Push,                       */
+	0x05, 0x01,                   /*      Usage Page (Desktop),       */
+	0x55, 0xFD,                   /*      Unit Exponent (-3),         */
+	0x65, 0x13,                   /*      Unit (Inch),                */
+	0x34,                         /*      Physical Minimum (0),       */
+	0x09, 0x30,                   /*      Usage (X),                  */
+	0x46, 0x40, 0x1F,             /*      Physical Maximum (8000),    */
+	0x27, 0x00, 0xA0, 0x00, 0x00, /*      Logical Maximum (40960),    */
+	0x81, 0x02,                   /*      Input (Variable),           */
+	0x09, 0x31,                   /*      Usage (Y),                  */
+	0x46, 0x70, 0x17,             /*      Physical Maximum (6000),    */
+	0x26, 0x00, 0x78,             /*      Logical Maximum (30720),    */
+	0x81, 0x02,                   /*      Input (Variable),           */
+	0xB4,                         /*      Pop,                        */
+	0x09, 0x30,                   /*      Usage (Tip Pressure),       */
+	0x26, 0xFF, 0x07,             /*      Logical Maximum (2047),     */
+	0x81, 0x02,                   /*      Input (Variable),           */
+	0xC0,                         /*    End Collection,               */
+	0xC0,                         /*  End Collection,                 */
+	0x05, 0x01,                   /*  Usage Page (Desktop),           */
+	0x09, 0x02,                   /*  Usage (Mouse),                  */
+	0xA1, 0x01,                   /*  Collection (Application),       */
+	0x85, 0x11,                   /*    Report ID (17),               */
+	0x09, 0x01,                   /*    Usage (Pointer),              */
+	0xA0,                         /*    Collection (Physical),        */
+	0x14,                         /*      Logical Minimum (0),        */
+	0xA4,                         /*      Push,                       */
+	0x05, 0x09,                   /*      Usage Page (Button),        */
+	0x75, 0x01,                   /*      Report Size (1),            */
+	0x19, 0x01,                   /*      Usage Minimum (01h),        */
+	0x29, 0x03,                   /*      Usage Maximum (03h),        */
+	0x25, 0x01,                   /*      Logical Maximum (1),        */
+	0x95, 0x03,                   /*      Report Count (3),           */
+	0x81, 0x02,                   /*      Input (Variable),           */
+	0x95, 0x05,                   /*      Report Count (5),           */
+	0x81, 0x01,                   /*      Input (Constant),           */
+	0xB4,                         /*      Pop,                        */
+	0x95, 0x01,                   /*      Report Count (1),           */
+	0xA4,                         /*      Push,                       */
+	0x55, 0xFD,                   /*      Unit Exponent (-3),         */
+	0x65, 0x13,                   /*      Unit (Inch),                */
+	0x34,                         /*      Physical Minimum (0),       */
+	0x75, 0x10,                   /*      Report Size (16),           */
+	0x09, 0x30,                   /*      Usage (X),                  */
+	0x46, 0x40, 0x1F,             /*      Physical Maximum (8000),    */
+	0x27, 0x00, 0xA0, 0x00, 0x00, /*      Logical Maximum (40960),    */
+	0x81, 0x02,                   /*      Input (Variable),           */
+	0x09, 0x31,                   /*      Usage (Y),                  */
+	0x46, 0x70, 0x17,             /*      Physical Maximum (6000),    */
+	0x26, 0x00, 0x78,             /*      Logical Maximum (30720),    */
+	0x81, 0x02,                   /*      Input (Variable),           */
+	0xB4,                         /*      Pop,                        */
+	0x75, 0x08,                   /*      Report Size (8),            */
+	0x09, 0x38,                   /*      Usage (Wheel),              */
+	0x15, 0xFF,                   /*      Logical Minimum (-1),       */
+	0x25, 0x01,                   /*      Logical Maximum (1),        */
+	0x81, 0x06,                   /*      Input (Variable, Relative), */
+	0x81, 0x01,                   /*      Input (Constant),           */
+	0xC0,                         /*    End Collection,               */
+	0xC0                          /*  End Collection                  */
+};
 
 /* Original EasyPen M610X report descriptor size */
 #define EASYPEN_M610X_RDESC_ORIG_SIZE	476
@@ -454,12 +538,17 @@ static __u8 *kye_report_fixup(struct hid_device *hdev, __u8 *rdesc,
 		}
 		break;
 	case USB_DEVICE_ID_KYE_MOUSEPEN_I608X:
-	case USB_DEVICE_ID_KYE_MOUSEPEN_I608X_2:
 		if (*rsize == MOUSEPEN_I608X_RDESC_ORIG_SIZE) {
 			rdesc = mousepen_i608x_rdesc_fixed;
 			*rsize = sizeof(mousepen_i608x_rdesc_fixed);
 		}
 		break;
+	case USB_DEVICE_ID_KYE_MOUSEPEN_I608X_V2:
+		if (*rsize == MOUSEPEN_I608X_V2_RDESC_ORIG_SIZE) {
+			rdesc = mousepen_i608x_v2_rdesc_fixed;
+			*rsize = sizeof(mousepen_i608x_v2_rdesc_fixed);
+		}
+		break;
 	case USB_DEVICE_ID_KYE_EASYPEN_M610X:
 		if (*rsize == EASYPEN_M610X_RDESC_ORIG_SIZE) {
 			rdesc = easypen_m610x_rdesc_fixed;
@@ -553,7 +642,7 @@ static int kye_probe(struct hid_device *hdev, const struct hid_device_id *id)
 	switch (id->product) {
 	case USB_DEVICE_ID_KYE_EASYPEN_I405X:
 	case USB_DEVICE_ID_KYE_MOUSEPEN_I608X:
-	case USB_DEVICE_ID_KYE_MOUSEPEN_I608X_2:
+	case USB_DEVICE_ID_KYE_MOUSEPEN_I608X_V2:
 	case USB_DEVICE_ID_KYE_EASYPEN_M610X:
 	case USB_DEVICE_ID_KYE_PENSKETCH_M912:
 		ret = kye_tablet_enable(hdev);
@@ -586,7 +675,7 @@ static const struct hid_device_id kye_devices[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_KYE,
 				USB_DEVICE_ID_KYE_MOUSEPEN_I608X) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_KYE,
-				USB_DEVICE_ID_KYE_MOUSEPEN_I608X_2) },
+				USB_DEVICE_ID_KYE_MOUSEPEN_I608X_V2) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_KYE,
 				USB_DEVICE_ID_KYE_EASYPEN_M610X) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_KYE,
diff --git a/drivers/hid/hid-led.c b/drivers/hid/hid-led.c
index d8d55f37b4f5..d3e1ab162f7c 100644
--- a/drivers/hid/hid-led.c
+++ b/drivers/hid/hid-led.c
@@ -100,6 +100,7 @@ struct hidled_device {
 	const struct hidled_config *config;
 	struct hid_device       *hdev;
 	struct hidled_rgb	*rgb;
+	u8			*buf;
 	struct mutex		lock;
 };
 
@@ -118,13 +119,19 @@ static int hidled_send(struct hidled_device *ldev, __u8 *buf)
 
 	mutex_lock(&ldev->lock);
 
+	/*
+	 * buffer provided to hid_hw_raw_request must not be on the stack
+	 * and must not be part of a data structure
+	 */
+	memcpy(ldev->buf, buf, ldev->config->report_size);
+
 	if (ldev->config->report_type == RAW_REQUEST)
-		ret = hid_hw_raw_request(ldev->hdev, buf[0], buf,
+		ret = hid_hw_raw_request(ldev->hdev, buf[0], ldev->buf,
 					 ldev->config->report_size,
 					 HID_FEATURE_REPORT,
 					 HID_REQ_SET_REPORT);
 	else if (ldev->config->report_type == OUTPUT_REPORT)
-		ret = hid_hw_output_report(ldev->hdev, buf,
+		ret = hid_hw_output_report(ldev->hdev, ldev->buf,
 					   ldev->config->report_size);
 	else
 		ret = -EINVAL;
@@ -147,17 +154,21 @@ static int hidled_recv(struct hidled_device *ldev, __u8 *buf)
 
 	mutex_lock(&ldev->lock);
 
-	ret = hid_hw_raw_request(ldev->hdev, buf[0], buf,
+	memcpy(ldev->buf, buf, ldev->config->report_size);
+
+	ret = hid_hw_raw_request(ldev->hdev, buf[0], ldev->buf,
 				 ldev->config->report_size,
 				 HID_FEATURE_REPORT,
 				 HID_REQ_SET_REPORT);
 	if (ret < 0)
 		goto err;
 
-	ret = hid_hw_raw_request(ldev->hdev, buf[0], buf,
+	ret = hid_hw_raw_request(ldev->hdev, buf[0], ldev->buf,
 				 ldev->config->report_size,
 				 HID_FEATURE_REPORT,
 				 HID_REQ_GET_REPORT);
+
+	memcpy(buf, ldev->buf, ldev->config->report_size);
 err:
 	mutex_unlock(&ldev->lock);
 
@@ -447,6 +458,10 @@ static int hidled_probe(struct hid_device *hdev, const struct hid_device_id *id)
 	if (!ldev)
 		return -ENOMEM;
 
+	ldev->buf = devm_kmalloc(&hdev->dev, MAX_REPORT_SIZE, GFP_KERNEL);
+	if (!ldev->buf)
+		return -ENOMEM;
+
 	ret = hid_parse(hdev);
 	if (ret)
 		return ret;
diff --git a/drivers/hid/hid-lg.c b/drivers/hid/hid-lg.c
index feb2be71f77c..76f644deb0a7 100644
--- a/drivers/hid/hid-lg.c
+++ b/drivers/hid/hid-lg.c
@@ -49,6 +49,7 @@
 #define FV_RDESC_ORIG_SIZE	130
 #define MOMO_RDESC_ORIG_SIZE	87
 #define MOMO2_RDESC_ORIG_SIZE	87
+#define FFG_RDESC_ORIG_SIZE	85
 
 /* Fixed report descriptors for Logitech Driving Force (and Pro)
  * wheel controllers
@@ -334,6 +335,52 @@ static __u8 momo2_rdesc_fixed[] = {
 0xC0                /*  End Collection                      */
 };
 
+static __u8 ffg_rdesc_fixed[] = {
+0x05, 0x01,         /*  Usage Page (Desktop),               */
+0x09, 0x04,         /*  Usage (Joystik),                    */
+0xA1, 0x01,         /*  Collection (Application),           */
+0xA1, 0x02,         /*      Collection (Logical),           */
+0x95, 0x01,         /*          Report Count (1),           */
+0x75, 0x0A,         /*          Report Size (10),           */
+0x15, 0x00,         /*          Logical Minimum (0),        */
+0x26, 0xFF, 0x03,   /*          Logical Maximum (1023),     */
+0x35, 0x00,         /*          Physical Minimum (0),       */
+0x46, 0xFF, 0x03,   /*          Physical Maximum (1023),    */
+0x09, 0x30,         /*          Usage (X),                  */
+0x81, 0x02,         /*          Input (Variable),           */
+0x95, 0x06,         /*          Report Count (6),           */
+0x75, 0x01,         /*          Report Size (1),            */
+0x25, 0x01,         /*          Logical Maximum (1),        */
+0x45, 0x01,         /*          Physical Maximum (1),       */
+0x05, 0x09,         /*          Usage Page (Button),        */
+0x19, 0x01,         /*          Usage Minimum (01h),        */
+0x29, 0x06,         /*          Usage Maximum (06h),        */
+0x81, 0x02,         /*          Input (Variable),           */
+0x95, 0x01,         /*          Report Count (1),           */
+0x75, 0x08,         /*          Report Size (8),            */
+0x26, 0xFF, 0x00,   /*          Logical Maximum (255),      */
+0x46, 0xFF, 0x00,   /*          Physical Maximum (255),     */
+0x06, 0x00, 0xFF,   /*          Usage Page (FF00h),         */
+0x09, 0x01,         /*          Usage (01h),                */
+0x81, 0x02,         /*          Input (Variable),           */
+0x05, 0x01,         /*          Usage Page (Desktop),       */
+0x81, 0x01,         /*          Input (Constant),           */
+0x09, 0x31,         /*          Usage (Y),                  */
+0x81, 0x02,         /*          Input (Variable),           */
+0x09, 0x32,         /*          Usage (Z),                  */
+0x81, 0x02,         /*          Input (Variable),           */
+0x06, 0x00, 0xFF,   /*          Usage Page (FF00h),         */
+0x09, 0x01,         /*          Usage (01h),                */
+0x81, 0x02,         /*          Input (Variable),           */
+0xC0,               /*      End Collection,                 */
+0xA1, 0x02,         /*      Collection (Logical),           */
+0x09, 0x02,         /*          Usage (02h),                */
+0x95, 0x07,         /*          Report Count (7),           */
+0x91, 0x02,         /*          Output (Variable),          */
+0xC0,               /*      End Collection,                 */
+0xC0                /*  End Collection                      */
+};
+
 /*
  * Certain Logitech keyboards send in report #3 keys which are far
  * above the logical maximum described in descriptor. This extends
@@ -343,8 +390,6 @@ static __u8 *lg_report_fixup(struct hid_device *hdev, __u8 *rdesc,
 		unsigned int *rsize)
 {
 	struct lg_drv_data *drv_data = hid_get_drvdata(hdev);
-	struct usb_device_descriptor *udesc;
-	__u16 bcdDevice, rev_maj, rev_min;
 
 	if ((drv_data->quirks & LG_RDESC) && *rsize >= 91 && rdesc[83] == 0x26 &&
 			rdesc[84] == 0x8c && rdesc[85] == 0x02) {
@@ -363,20 +408,18 @@ static __u8 *lg_report_fixup(struct hid_device *hdev, __u8 *rdesc,
 
 	switch (hdev->product) {
 
-	/* Several wheels report as this id when operating in emulation mode. */
-	case USB_DEVICE_ID_LOGITECH_WHEEL:
-		udesc = &(hid_to_usb_dev(hdev)->descriptor);
-		if (!udesc) {
-			hid_err(hdev, "NULL USB device descriptor\n");
-			break;
+	case USB_DEVICE_ID_LOGITECH_WINGMAN_FFG:
+		if (*rsize == FFG_RDESC_ORIG_SIZE) {
+			hid_info(hdev,
+				"fixing up Logitech Wingman Formula Force GP report descriptor\n");
+			rdesc = ffg_rdesc_fixed;
+			*rsize = sizeof(ffg_rdesc_fixed);
 		}
-		bcdDevice = le16_to_cpu(udesc->bcdDevice);
-		rev_maj = bcdDevice >> 8;
-		rev_min = bcdDevice & 0xff;
+		break;
 
-		/* Update the report descriptor for only the Driving Force wheel */
-		if (rev_maj == 1 && rev_min == 2 &&
-				*rsize == DF_RDESC_ORIG_SIZE) {
+	/* Several wheels report as this id when operating in emulation mode. */
+	case USB_DEVICE_ID_LOGITECH_WHEEL:
+		if (*rsize == DF_RDESC_ORIG_SIZE) {
 			hid_info(hdev,
 				"fixing up Logitech Driving Force report descriptor\n");
 			rdesc = df_rdesc_fixed;
@@ -621,6 +664,7 @@ static int lg_input_mapped(struct hid_device *hdev, struct hid_input *hi,
 			usage->code == ABS_RZ)) {
 		switch (hdev->product) {
 		case USB_DEVICE_ID_LOGITECH_G29_WHEEL:
+		case USB_DEVICE_ID_LOGITECH_WINGMAN_FFG:
 		case USB_DEVICE_ID_LOGITECH_WHEEL:
 		case USB_DEVICE_ID_LOGITECH_MOMO_WHEEL:
 		case USB_DEVICE_ID_LOGITECH_DFP_WHEEL:
@@ -657,6 +701,17 @@ static int lg_event(struct hid_device *hdev, struct hid_field *field,
 	return 0;
 }
 
+static int lg_raw_event(struct hid_device *hdev, struct hid_report *report,
+		u8 *rd, int size)
+{
+	struct lg_drv_data *drv_data = hid_get_drvdata(hdev);
+
+	if (drv_data->quirks & LG_FF4)
+		return lg4ff_raw_event(hdev, report, rd, size, drv_data);
+
+	return 0;
+}
+
 static int lg_probe(struct hid_device *hdev, const struct hid_device_id *id)
 {
 	struct usb_interface *iface = to_usb_interface(hdev->dev.parent);
@@ -809,7 +864,7 @@ static const struct hid_device_id lg_devices[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_LOGITECH_WII_WHEEL),
 		.driver_data = LG_FF4 },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_LOGITECH_WINGMAN_FFG),
-		.driver_data = LG_FF },
+		.driver_data = LG_NOGET | LG_FF4 },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_LOGITECH_RUMBLEPAD2),
 		.driver_data = LG_FF2 },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_LOGITECH_FLIGHT_SYSTEM_G940),
@@ -830,6 +885,7 @@ static struct hid_driver lg_driver = {
 	.input_mapping = lg_input_mapping,
 	.input_mapped = lg_input_mapped,
 	.event = lg_event,
+	.raw_event = lg_raw_event,
 	.probe = lg_probe,
 	.remove = lg_remove,
 };
diff --git a/drivers/hid/hid-lg4ff.c b/drivers/hid/hid-lg4ff.c
index af3a8ec8a746..1fc12e357035 100644
--- a/drivers/hid/hid-lg4ff.c
+++ b/drivers/hid/hid-lg4ff.c
@@ -75,6 +75,7 @@ static void lg4ff_set_range_g25(struct hid_device *hid, u16 range);
 
 struct lg4ff_wheel_data {
 	const u32 product_id;
+	u16 combine;
 	u16 range;
 	const u16 min_range;
 	const u16 max_range;
@@ -136,6 +137,7 @@ struct lg4ff_alternate_mode {
 };
 
 static const struct lg4ff_wheel lg4ff_devices[] = {
+	{USB_DEVICE_ID_LOGITECH_WINGMAN_FFG, lg4ff_wheel_effects, 40, 180, NULL},
 	{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, lg4ff_set_range_dfp},
@@ -328,6 +330,56 @@ int lg4ff_adjust_input_event(struct hid_device *hid, struct hid_field *field,
 	}
 }
 
+int lg4ff_raw_event(struct hid_device *hdev, struct hid_report *report,
+		u8 *rd, int size, struct lg_drv_data *drv_data)
+{
+	int offset;
+	struct lg4ff_device_entry *entry = drv_data->device_props;
+
+	if (!entry)
+		return 0;
+
+	/* adjust HID report present combined pedals data */
+	if (entry->wdata.combine) {
+		switch (entry->wdata.product_id) {
+		case USB_DEVICE_ID_LOGITECH_WHEEL:
+			rd[5] = rd[3];
+			rd[6] = 0x7F;
+			return 1;
+		case USB_DEVICE_ID_LOGITECH_WINGMAN_FFG:
+		case USB_DEVICE_ID_LOGITECH_MOMO_WHEEL:
+		case USB_DEVICE_ID_LOGITECH_MOMO_WHEEL2:
+			rd[4] = rd[3];
+			rd[5] = 0x7F;
+			return 1;
+		case USB_DEVICE_ID_LOGITECH_DFP_WHEEL:
+			rd[5] = rd[4];
+			rd[6] = 0x7F;
+			return 1;
+		case USB_DEVICE_ID_LOGITECH_G25_WHEEL:
+		case USB_DEVICE_ID_LOGITECH_G27_WHEEL:
+			offset = 5;
+			break;
+		case USB_DEVICE_ID_LOGITECH_DFGT_WHEEL:
+		case USB_DEVICE_ID_LOGITECH_G29_WHEEL:
+			offset = 6;
+			break;
+		case USB_DEVICE_ID_LOGITECH_WII_WHEEL:
+			offset = 3;
+			break;
+		default:
+			return 0;
+		}
+
+		/* Compute a combined axis when wheel does not supply it */
+		rd[offset] = (0xFF + rd[offset] - rd[offset+1]) >> 1;
+		rd[offset+1] = 0x7F;
+		return 1;
+	}
+
+	return 0;
+}
+
 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)
@@ -345,6 +397,7 @@ static void lg4ff_init_wheel_data(struct lg4ff_wheel_data * const wdata, const s
 	{
 		struct lg4ff_wheel_data t_wdata =  { .product_id = wheel->product_id,
 						     .real_product_id = real_product_id,
+						     .combine = 0,
 						     .min_range = wheel->min_range,
 						     .max_range = wheel->max_range,
 						     .set_range = wheel->set_range,
@@ -885,6 +938,58 @@ 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);
 
+static ssize_t lg4ff_combine_show(struct device *dev, struct device_attribute *attr,
+				char *buf)
+{
+	struct hid_device *hid = to_hid_device(dev);
+	struct lg4ff_device_entry *entry;
+	struct lg_drv_data *drv_data;
+	size_t count;
+
+	drv_data = hid_get_drvdata(hid);
+	if (!drv_data) {
+		hid_err(hid, "Private driver data not found!\n");
+		return 0;
+	}
+
+	entry = drv_data->device_props;
+	if (!entry) {
+		hid_err(hid, "Device properties not found!\n");
+		return 0;
+	}
+
+	count = scnprintf(buf, PAGE_SIZE, "%u\n", entry->wdata.combine);
+	return count;
+}
+
+static ssize_t lg4ff_combine_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 combine = simple_strtoul(buf, NULL, 10);
+
+	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;
+	}
+
+	if (combine > 1)
+		combine = 1;
+
+	entry->wdata.combine = combine;
+	return count;
+}
+static DEVICE_ATTR(combine_pedals, S_IRUSR | S_IWUSR | S_IRGRP | S_IWGRP | S_IROTH, lg4ff_combine_show, lg4ff_combine_store);
+
 /* Export the currently set range of the wheel */
 static ssize_t lg4ff_range_show(struct device *dev, struct device_attribute *attr,
 				char *buf)
@@ -1259,6 +1364,9 @@ int lg4ff_init(struct hid_device *hid)
 	}
 
 	/* Create sysfs interface */
+	error = device_create_file(&hid->dev, &dev_attr_combine_pedals);
+	if (error)
+		hid_warn(hid, "Unable to create sysfs interface for \"combine\", errno %d\n", error);
 	error = device_create_file(&hid->dev, &dev_attr_range);
 	if (error)
 		hid_warn(hid, "Unable to create sysfs interface for \"range\", errno %d\n", error);
@@ -1358,6 +1466,7 @@ int lg4ff_deinit(struct hid_device *hid)
 		device_remove_file(&hid->dev, &dev_attr_alternate_modes);
 	}
 
+	device_remove_file(&hid->dev, &dev_attr_combine_pedals);
 	device_remove_file(&hid->dev, &dev_attr_range);
 #ifdef CONFIG_LEDS_CLASS
 	{
diff --git a/drivers/hid/hid-lg4ff.h b/drivers/hid/hid-lg4ff.h
index 66201af44da3..de1f350e0bd3 100644
--- a/drivers/hid/hid-lg4ff.h
+++ b/drivers/hid/hid-lg4ff.h
@@ -6,11 +6,15 @@ 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);
+int lg4ff_raw_event(struct hid_device *hdev, struct hid_report *report,
+		u8 *rd, int size, 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; }
+static inline int lg4ff_raw_event(struct hid_device *hdev, struct hid_report *report,
+		u8 *rd, int size, 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-microsoft.c b/drivers/hid/hid-microsoft.c
index e924d555536c..c6cd392e9f99 100644
--- a/drivers/hid/hid-microsoft.c
+++ b/drivers/hid/hid-microsoft.c
@@ -28,7 +28,6 @@
 #define MS_RDESC		0x08
 #define MS_NOGET		0x10
 #define MS_DUPLICATE_USAGES	0x20
-#define MS_RDESC_3K		0x40
 
 static __u8 *ms_report_fixup(struct hid_device *hdev, __u8 *rdesc,
 		unsigned int *rsize)
@@ -45,13 +44,6 @@ static __u8 *ms_report_fixup(struct hid_device *hdev, __u8 *rdesc,
 		rdesc[557] = 0x35;
 		rdesc[559] = 0x45;
 	}
-	/* the same as above (s/usage/physical/) */
-	if ((quirks & MS_RDESC_3K) && *rsize == 106 && rdesc[94] == 0x19 &&
-			rdesc[95] == 0x00 && rdesc[96] == 0x29 &&
-			rdesc[97] == 0xff) {
-		rdesc[94] = 0x35;
-		rdesc[96] = 0x45;
-	}
 	return rdesc;
 }
 
@@ -271,7 +263,7 @@ static const struct hid_device_id ms_devices[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_PRESENTER_8K_USB),
 		.driver_data = MS_PRESENTER },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_DIGITAL_MEDIA_3K),
-		.driver_data = MS_ERGONOMY | MS_RDESC_3K },
+		.driver_data = MS_ERGONOMY },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_DIGITAL_MEDIA_7K),
 		.driver_data = MS_ERGONOMY },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_DIGITAL_MEDIA_600),
@@ -288,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_PRO_3_JP),
 		.driver_data = MS_HIDINPUT },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_TYPE_COVER_PRO_4_JP),
+		.driver_data = MS_HIDINPUT },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_TYPE_COVER_3),
 		.driver_data = MS_HIDINPUT },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_POWER_COVER),
diff --git a/drivers/hid/hid-saitek.c b/drivers/hid/hid-saitek.c
index 2f84b26f1167..39e642686ff0 100644
--- a/drivers/hid/hid-saitek.c
+++ b/drivers/hid/hid-saitek.c
@@ -183,6 +183,8 @@ static const struct hid_device_id saitek_devices[] = {
 		.driver_data = SAITEK_RELEASE_MODE_RAT7 },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_SAITEK, USB_DEVICE_ID_SAITEK_RAT7),
 		.driver_data = SAITEK_RELEASE_MODE_RAT7 },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_SAITEK, USB_DEVICE_ID_SAITEK_RAT9),
+		.driver_data = SAITEK_RELEASE_MODE_RAT7 },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_MADCATZ, USB_DEVICE_ID_MADCATZ_RAT9),
 		.driver_data = SAITEK_RELEASE_MODE_RAT7 },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_SAITEK, USB_DEVICE_ID_SAITEK_MMO7),
diff --git a/drivers/hid/hid-sensor-hub.c b/drivers/hid/hid-sensor-hub.c
index 3d5ba5b51af3..658a607dc6d9 100644
--- a/drivers/hid/hid-sensor-hub.c
+++ b/drivers/hid/hid-sensor-hub.c
@@ -16,6 +16,7 @@
  * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
  *
  */
+
 #include <linux/device.h>
 #include <linux/hid.h>
 #include <linux/module.h>
@@ -798,6 +799,9 @@ static const struct hid_device_id sensor_hub_devices[] = {
 	{ HID_DEVICE(HID_BUS_ANY, HID_GROUP_SENSOR_HUB, USB_VENDOR_ID_ITE,
 			USB_DEVICE_ID_ITE_LENOVO_YOGA900),
 			.driver_data = HID_SENSOR_HUB_ENUM_QUIRK},
+	{ HID_DEVICE(HID_BUS_ANY, HID_GROUP_SENSOR_HUB, USB_VENDOR_ID_INTEL_0,
+			0x22D8),
+			.driver_data = HID_SENSOR_HUB_ENUM_QUIRK},
 	{ HID_DEVICE(HID_BUS_ANY, HID_GROUP_SENSOR_HUB, HID_ANY_ID,
 		     HID_ANY_ID) },
 	{ }
diff --git a/drivers/hid/hid-sony.c b/drivers/hid/hid-sony.c
index 310436a54a3f..b0bb99a821bd 100644
--- a/drivers/hid/hid-sony.c
+++ b/drivers/hid/hid-sony.c
@@ -8,7 +8,7 @@
  *  Copyright (c) 2012 David Dillow <dave@thedillows.org>
  *  Copyright (c) 2006-2013 Jiri Kosina
  *  Copyright (c) 2013 Colin Leitner <colin.leitner@gmail.com>
- *  Copyright (c) 2014 Frank Praznik <frank.praznik@gmail.com>
+ *  Copyright (c) 2014-2016 Frank Praznik <frank.praznik@gmail.com>
  */
 
 /*
@@ -51,6 +51,7 @@
 #define NAVIGATION_CONTROLLER_USB BIT(9)
 #define NAVIGATION_CONTROLLER_BT  BIT(10)
 #define SINO_LITE_CONTROLLER      BIT(11)
+#define FUTUREMAX_DANCE_MAT       BIT(12)
 
 #define SIXAXIS_CONTROLLER (SIXAXIS_CONTROLLER_USB | SIXAXIS_CONTROLLER_BT)
 #define MOTION_CONTROLLER (MOTION_CONTROLLER_USB | MOTION_CONTROLLER_BT)
@@ -65,6 +66,8 @@
 				MOTION_CONTROLLER_BT | NAVIGATION_CONTROLLER)
 #define SONY_FF_SUPPORT (SIXAXIS_CONTROLLER | DUALSHOCK4_CONTROLLER |\
 				MOTION_CONTROLLER)
+#define SONY_BT_DEVICE (SIXAXIS_CONTROLLER_BT | DUALSHOCK4_CONTROLLER_BT |\
+			MOTION_CONTROLLER_BT | NAVIGATION_CONTROLLER_BT)
 
 #define MAX_LEDS 4
 
@@ -1048,6 +1051,7 @@ struct sony_sc {
 
 	u8 mac_address[6];
 	u8 worker_initialized;
+	u8 defer_initialization;
 	u8 cable_state;
 	u8 battery_charging;
 	u8 battery_capacity;
@@ -1058,6 +1062,12 @@ struct sony_sc {
 	u8 led_count;
 };
 
+static inline void sony_schedule_work(struct sony_sc *sc)
+{
+	if (!sc->defer_initialization)
+		schedule_work(&sc->state_worker);
+}
+
 static u8 *sixaxis_fixup(struct hid_device *hdev, u8 *rdesc,
 			     unsigned int *rsize)
 {
@@ -1125,7 +1135,7 @@ static u8 *sony_report_fixup(struct hid_device *hdev, u8 *rdesc,
 {
 	struct sony_sc *sc = hid_get_drvdata(hdev);
 
-	if (sc->quirks & SINO_LITE_CONTROLLER)
+	if (sc->quirks & (SINO_LITE_CONTROLLER | FUTUREMAX_DANCE_MAT))
 		return rdesc;
 
 	/*
@@ -1317,6 +1327,11 @@ static int sony_raw_event(struct hid_device *hdev, struct hid_report *report,
 		dualshock4_parse_report(sc, rd, size);
 	}
 
+	if (sc->defer_initialization) {
+		sc->defer_initialization = 0;
+		sony_schedule_work(sc);
+	}
+
 	return 0;
 }
 
@@ -1554,7 +1569,7 @@ static void buzz_set_leds(struct sony_sc *sc)
 static void sony_set_leds(struct sony_sc *sc)
 {
 	if (!(sc->quirks & BUZZ_CONTROLLER))
-		schedule_work(&sc->state_worker);
+		sony_schedule_work(sc);
 	else
 		buzz_set_leds(sc);
 }
@@ -1665,7 +1680,7 @@ static int sony_led_blink_set(struct led_classdev *led, unsigned long *delay_on,
 		new_off != drv_data->led_delay_off[n]) {
 		drv_data->led_delay_on[n] = new_on;
 		drv_data->led_delay_off[n] = new_off;
-		schedule_work(&drv_data->state_worker);
+		sony_schedule_work(drv_data);
 	}
 
 	return 0;
@@ -1865,6 +1880,17 @@ static void dualshock4_send_output_report(struct sony_sc *sc)
 	u8 *buf = sc->output_report_dmabuf;
 	int offset;
 
+	/*
+	 * NOTE: The buf[1] field of the Bluetooth report controls
+	 * the Dualshock 4 reporting rate.
+	 *
+	 * Known values include:
+	 *
+	 * 0x80 - 1000hz (full speed)
+	 * 0xA0 - 31hz
+	 * 0xB0 - 20hz
+	 * 0xD0 - 66hz
+	 */
 	if (sc->quirks & DUALSHOCK4_CONTROLLER_USB) {
 		memset(buf, 0, DS4_REPORT_0x05_SIZE);
 		buf[0] = 0x05;
@@ -1976,7 +2002,7 @@ static int sony_play_effect(struct input_dev *dev, void *data,
 	sc->left = effect->u.rumble.strong_magnitude / 256;
 	sc->right = effect->u.rumble.weak_magnitude / 256;
 
-	schedule_work(&sc->state_worker);
+	sony_schedule_work(sc);
 	return 0;
 }
 
@@ -2039,8 +2065,11 @@ static int sony_battery_get_property(struct power_supply *psy,
 	return ret;
 }
 
-static int sony_battery_probe(struct sony_sc *sc)
+static int sony_battery_probe(struct sony_sc *sc, int append_dev_id)
 {
+	const char *battery_str_fmt = append_dev_id ?
+		"sony_controller_battery_%pMR_%i" :
+		"sony_controller_battery_%pMR";
 	struct power_supply_config psy_cfg = { .drv_data = sc, };
 	struct hid_device *hdev = sc->hdev;
 	int ret;
@@ -2056,9 +2085,8 @@ static int sony_battery_probe(struct sony_sc *sc)
 	sc->battery_desc.get_property = sony_battery_get_property;
 	sc->battery_desc.type = POWER_SUPPLY_TYPE_BATTERY;
 	sc->battery_desc.use_for_apm = 0;
-	sc->battery_desc.name = kasprintf(GFP_KERNEL,
-					  "sony_controller_battery_%pMR",
-					  sc->mac_address);
+	sc->battery_desc.name = kasprintf(GFP_KERNEL, battery_str_fmt,
+					  sc->mac_address, sc->device_id);
 	if (!sc->battery_desc.name)
 		return -ENOMEM;
 
@@ -2094,7 +2122,21 @@ static void sony_battery_remove(struct sony_sc *sc)
  * it will show up as two devices. A global list of connected controllers and
  * their MAC addresses is maintained to ensure that a device is only connected
  * once.
+ *
+ * Some USB-only devices masquerade as Sixaxis controllers and all have the
+ * same dummy Bluetooth address, so a comparison of the connection type is
+ * required.  Devices are only rejected in the case where two devices have
+ * matching Bluetooth addresses on different bus types.
  */
+static inline int sony_compare_connection_type(struct sony_sc *sc0,
+						struct sony_sc *sc1)
+{
+	const int sc0_not_bt = !(sc0->quirks & SONY_BT_DEVICE);
+	const int sc1_not_bt = !(sc1->quirks & SONY_BT_DEVICE);
+
+	return sc0_not_bt == sc1_not_bt;
+}
+
 static int sony_check_add_dev_list(struct sony_sc *sc)
 {
 	struct sony_sc *entry;
@@ -2107,9 +2149,14 @@ static int sony_check_add_dev_list(struct sony_sc *sc)
 		ret = memcmp(sc->mac_address, entry->mac_address,
 				sizeof(sc->mac_address));
 		if (!ret) {
-			ret = -EEXIST;
-			hid_info(sc->hdev, "controller with MAC address %pMR already connected\n",
+			if (sony_compare_connection_type(sc, entry)) {
+				ret = 1;
+			} else {
+				ret = -EEXIST;
+				hid_info(sc->hdev,
+				"controller with MAC address %pMR already connected\n",
 				sc->mac_address);
+			}
 			goto unlock;
 		}
 	}
@@ -2285,10 +2332,14 @@ static inline void sony_cancel_work_sync(struct sony_sc *sc)
 static int sony_probe(struct hid_device *hdev, const struct hid_device_id *id)
 {
 	int ret;
+	int append_dev_id;
 	unsigned long quirks = id->driver_data;
 	struct sony_sc *sc;
 	unsigned int connect_mask = HID_CONNECT_DEFAULT;
 
+	if (!strcmp(hdev->name, "FutureMax Dance Mat"))
+		quirks |= FUTUREMAX_DANCE_MAT;
+
 	sc = devm_kzalloc(&hdev->dev, sizeof(*sc), GFP_KERNEL);
 	if (sc == NULL) {
 		hid_err(hdev, "can't alloc sony descriptor\n");
@@ -2341,9 +2392,16 @@ static int sony_probe(struct hid_device *hdev, const struct hid_device_id *id)
 		 * the Sixaxis does not want the report_id as part of the data
 		 * packet, so we have to discard buf[0] when sending the actual
 		 * control message, even for numbered reports, humpf!
+		 *
+		 * Additionally, the Sixaxis on USB isn't properly initialized
+		 * until the PS logo button is pressed and as such won't retain
+		 * any state set by an output report, so the initial
+		 * configuration report is deferred until the first input
+		 * report arrives.
 		 */
 		hdev->quirks |= HID_QUIRK_NO_OUTPUT_REPORTS_ON_INTR_EP;
 		hdev->quirks |= HID_QUIRK_SKIP_OUTPUT_REPORT_ID;
+		sc->defer_initialization = 1;
 		ret = sixaxis_set_operational_usb(hdev);
 		sony_init_output_report(sc, sixaxis_send_output_report);
 	} else if ((sc->quirks & SIXAXIS_CONTROLLER_BT) ||
@@ -2379,7 +2437,7 @@ static int sony_probe(struct hid_device *hdev, const struct hid_device_id *id)
 	if (ret < 0)
 		goto err_stop;
 
-	ret = sony_check_add(sc);
+	ret = append_dev_id = sony_check_add(sc);
 	if (ret < 0)
 		goto err_stop;
 
@@ -2390,7 +2448,7 @@ static int sony_probe(struct hid_device *hdev, const struct hid_device_id *id)
 	}
 
 	if (sc->quirks & SONY_BATTERY_SUPPORT) {
-		ret = sony_battery_probe(sc);
+		ret = sony_battery_probe(sc, append_dev_id);
 		if (ret < 0)
 			goto err_stop;
 
@@ -2486,8 +2544,10 @@ static int sony_resume(struct hid_device *hdev)
 		 * reinitialized on resume or they won't behave properly.
 		 */
 		if ((sc->quirks & SIXAXIS_CONTROLLER_USB) ||
-			(sc->quirks & NAVIGATION_CONTROLLER_USB))
+			(sc->quirks & NAVIGATION_CONTROLLER_USB)) {
 			sixaxis_set_operational_usb(sc->hdev);
+			sc->defer_initialization = 1;
+		}
 
 		sony_set_leds(sc);
 	}
diff --git a/drivers/hid/hid-uclogic.c b/drivers/hid/hid-uclogic.c
index 85ac43517e3f..1509d7287ff3 100644
--- a/drivers/hid/hid-uclogic.c
+++ b/drivers/hid/hid-uclogic.c
@@ -21,13 +21,6 @@
 
 #include "hid-ids.h"
 
-/*
- * See WPXXXXU model descriptions, device and HID report descriptors at
- * http://sf.net/apps/mediawiki/digimend/?title=UC-Logic_Tablet_WP4030U
- * http://sf.net/apps/mediawiki/digimend/?title=UC-Logic_Tablet_WP5540U
- * http://sf.net/apps/mediawiki/digimend/?title=UC-Logic_Tablet_WP8060U
- */
-
 /* Size of the original descriptor of WPXXXXU tablets */
 #define WPXXXXU_RDESC_ORIG_SIZE	212
 
@@ -221,11 +214,6 @@ static __u8 wp8060u_rdesc_fixed[] = {
 	0xC0                /*  End Collection                      */
 };
 
-/*
- * See WP1062 description, device and HID report descriptors at
- * http://sf.net/apps/mediawiki/digimend/?title=UC-Logic_Tablet_WP1062
- */
-
 /* Size of the original descriptor of WP1062 tablet */
 #define WP1062_RDESC_ORIG_SIZE	254
 
@@ -274,11 +262,6 @@ static __u8 wp1062_rdesc_fixed[] = {
 	0xC0                /*  End Collection                      */
 };
 
-/*
- * See PF1209 description, device and HID report descriptors at
- * http://sf.net/apps/mediawiki/digimend/?title=UC-Logic_Tablet_PF1209
- */
-
 /* Size of the original descriptor of PF1209 tablet */
 #define PF1209_RDESC_ORIG_SIZE	234
 
@@ -356,11 +339,6 @@ static __u8 pf1209_rdesc_fixed[] = {
 	0xC0                /*  End Collection                      */
 };
 
-/*
- * See TWHL850 description, device and HID report descriptors at
- * http://sf.net/apps/mediawiki/digimend/?title=UC-Logic_Wireless_Tablet_TWHL850
- */
-
 /* Size of the original descriptors of TWHL850 tablet */
 #define TWHL850_RDESC_ORIG_SIZE0	182
 #define TWHL850_RDESC_ORIG_SIZE1	161
@@ -469,11 +447,6 @@ static __u8 twhl850_rdesc_fixed2[] = {
 	0xC0                /*  End Collection                      */
 };
 
-/*
- * See TWHA60 description, device and HID report descriptors at
- * http://sf.net/apps/mediawiki/digimend/?title=UC-Logic_Tablet_TWHA60
- */
-
 /* Size of the original descriptors of TWHA60 tablet */
 #define TWHA60_RDESC_ORIG_SIZE0 254
 #define TWHA60_RDESC_ORIG_SIZE1 139
@@ -613,6 +586,27 @@ static const __u8 uclogic_tablet_rdesc_template[] = {
 	0xC0                    /*  End Collection                          */
 };
 
+/* Fixed virtual pad report descriptor */
+static const __u8 uclogic_buttonpad_rdesc[] = {
+	0x05, 0x01,             /*  Usage Page (Desktop),                   */
+	0x09, 0x07,             /*  Usage (Keypad),                         */
+	0xA1, 0x01,             /*  Collection (Application),               */
+	0x85, 0xF7,             /*      Report ID (247),                    */
+	0x05, 0x0D,             /*      Usage Page (Digitizers),            */
+	0x09, 0x39,             /*      Usage (Tablet Function Keys),       */
+	0xA0,                   /*      Collection (Physical),              */
+	0x05, 0x09,             /*          Usage Page (Button),            */
+	0x75, 0x01,             /*          Report Size (1),                */
+	0x95, 0x18,             /*          Report Count (24),              */
+	0x81, 0x03,             /*          Input (Constant, Variable),     */
+	0x19, 0x01,             /*          Usage Minimum (01h),            */
+	0x29, 0x08,             /*          Usage Maximum (08h),            */
+	0x95, 0x08,             /*          Report Count (8),               */
+	0x81, 0x02,             /*          Input (Variable),               */
+	0xC0,                   /*      End Collection                      */
+	0xC0                    /*  End Collection                          */
+};
+
 /* Parameter indices */
 enum uclogic_prm {
 	UCLOGIC_PRM_X_LM	= 1,
@@ -628,6 +622,7 @@ struct uclogic_drvdata {
 	unsigned int rsize;
 	bool invert_pen_inrange;
 	bool ignore_pen_usage;
+	bool has_virtual_pad_interface;
 };
 
 static __u8 *uclogic_report_fixup(struct hid_device *hdev, __u8 *rdesc,
@@ -637,6 +632,12 @@ static __u8 *uclogic_report_fixup(struct hid_device *hdev, __u8 *rdesc,
 	__u8 iface_num = iface->cur_altsetting->desc.bInterfaceNumber;
 	struct uclogic_drvdata *drvdata = hid_get_drvdata(hdev);
 
+	if (drvdata->rdesc != NULL) {
+		rdesc = drvdata->rdesc;
+		*rsize = drvdata->rsize;
+		return rdesc;
+	}
+
 	switch (hdev->product) {
 	case USB_DEVICE_ID_UCLOGIC_TABLET_PF1209:
 		if (*rsize == PF1209_RDESC_ORIG_SIZE) {
@@ -706,11 +707,6 @@ static __u8 *uclogic_report_fixup(struct hid_device *hdev, __u8 *rdesc,
 			break;
 		}
 		break;
-	default:
-		if (drvdata->rdesc != NULL) {
-			rdesc = drvdata->rdesc;
-			*rsize = drvdata->rsize;
-		}
 	}
 
 	return rdesc;
@@ -804,7 +800,6 @@ static int uclogic_tablet_enable(struct hid_device *hdev)
 	len = UCLOGIC_PRM_NUM * sizeof(*buf);
 	buf = kmalloc(len, GFP_KERNEL);
 	if (buf == NULL) {
-		hid_err(hdev, "failed to allocate parameter buffer\n");
 		rc = -ENOMEM;
 		goto cleanup;
 	}
@@ -848,7 +843,6 @@ static int uclogic_tablet_enable(struct hid_device *hdev)
 				sizeof(uclogic_tablet_rdesc_template),
 				GFP_KERNEL);
 	if (drvdata->rdesc == NULL) {
-		hid_err(hdev, "failed to allocate fixed rdesc\n");
 		rc = -ENOMEM;
 		goto cleanup;
 	}
@@ -876,11 +870,75 @@ cleanup:
 	return rc;
 }
 
+/**
+ * Enable actual button mode.
+ *
+ * @hdev:	HID device
+ */
+static int uclogic_button_enable(struct hid_device *hdev)
+{
+	int rc;
+	struct usb_device *usb_dev = hid_to_usb_dev(hdev);
+	struct uclogic_drvdata *drvdata = hid_get_drvdata(hdev);
+	char *str_buf;
+	size_t str_len = 16;
+	unsigned char *rdesc;
+	size_t rdesc_len;
+
+	str_buf = kzalloc(str_len, GFP_KERNEL);
+	if (str_buf == NULL) {
+		rc = -ENOMEM;
+		goto cleanup;
+	}
+
+	/* Enable abstract keyboard mode */
+	rc = usb_string(usb_dev, 0x7b, str_buf, str_len);
+	if (rc == -EPIPE) {
+		hid_info(hdev, "button mode setting not found\n");
+		rc = 0;
+		goto cleanup;
+	} else if (rc < 0) {
+		hid_err(hdev, "failed to enable abstract keyboard\n");
+		goto cleanup;
+	} else if (strncmp(str_buf, "HK On", rc)) {
+		hid_info(hdev, "invalid answer when requesting buttons: '%s'\n",
+			str_buf);
+		rc = -EINVAL;
+		goto cleanup;
+	}
+
+	/* Re-allocate fixed report descriptor */
+	rdesc_len = drvdata->rsize + sizeof(uclogic_buttonpad_rdesc);
+	rdesc = devm_kzalloc(&hdev->dev, rdesc_len, GFP_KERNEL);
+	if (!rdesc) {
+		rc = -ENOMEM;
+		goto cleanup;
+	}
+
+	memcpy(rdesc, drvdata->rdesc, drvdata->rsize);
+
+	/* Append the buttonpad descriptor */
+	memcpy(rdesc + drvdata->rsize, uclogic_buttonpad_rdesc,
+	       sizeof(uclogic_buttonpad_rdesc));
+
+	/* clean up old rdesc and use the new one */
+	drvdata->rsize = rdesc_len;
+	devm_kfree(&hdev->dev, drvdata->rdesc);
+	drvdata->rdesc = rdesc;
+
+	rc = 0;
+
+cleanup:
+	kfree(str_buf);
+	return rc;
+}
+
 static int uclogic_probe(struct hid_device *hdev,
 		const struct hid_device_id *id)
 {
 	int rc;
 	struct usb_interface *intf = to_usb_interface(hdev->dev.parent);
+	struct usb_device *udev = hid_to_usb_dev(hdev);
 	struct uclogic_drvdata *drvdata;
 
 	/*
@@ -899,6 +957,10 @@ static int uclogic_probe(struct hid_device *hdev,
 
 	switch (id->product) {
 	case USB_DEVICE_ID_HUION_TABLET:
+	case USB_DEVICE_ID_YIYNOVA_TABLET:
+	case USB_DEVICE_ID_UGEE_TABLET_81:
+	case USB_DEVICE_ID_UCLOGIC_DRAWIMAGE_G3:
+	case USB_DEVICE_ID_UGEE_TABLET_45:
 		/* If this is the pen interface */
 		if (intf->cur_altsetting->desc.bInterfaceNumber == 0) {
 			rc = uclogic_tablet_enable(hdev);
@@ -907,10 +969,48 @@ static int uclogic_probe(struct hid_device *hdev,
 				return rc;
 			}
 			drvdata->invert_pen_inrange = true;
+
+			rc = uclogic_button_enable(hdev);
+			drvdata->has_virtual_pad_interface = !rc;
 		} else {
 			drvdata->ignore_pen_usage = true;
 		}
 		break;
+	case USB_DEVICE_ID_UGTIZER_TABLET_GP0610:
+		/* If this is the pen interface */
+		if (intf->cur_altsetting->desc.bInterfaceNumber == 1) {
+			rc = uclogic_tablet_enable(hdev);
+			if (rc) {
+				hid_err(hdev, "tablet enabling failed\n");
+				return rc;
+			}
+			drvdata->invert_pen_inrange = true;
+		} else {
+			drvdata->ignore_pen_usage = true;
+		}
+		break;
+	case USB_DEVICE_ID_UCLOGIC_TABLET_TWHA60:
+		/*
+		 * If it is the three-interface version, which is known to
+		 * respond to initialization.
+		 */
+		if (udev->config->desc.bNumInterfaces == 3) {
+			/* If it is the pen interface */
+			if (intf->cur_altsetting->desc.bInterfaceNumber == 0) {
+				rc = uclogic_tablet_enable(hdev);
+				if (rc) {
+					hid_err(hdev, "tablet enabling failed\n");
+					return rc;
+				}
+				drvdata->invert_pen_inrange = true;
+
+				rc = uclogic_button_enable(hdev);
+				drvdata->has_virtual_pad_interface = !rc;
+			} else {
+				drvdata->ignore_pen_usage = true;
+			}
+		}
+		break;
 	}
 
 	rc = hid_parse(hdev);
@@ -933,12 +1033,16 @@ static int uclogic_raw_event(struct hid_device *hdev, struct hid_report *report,
 {
 	struct uclogic_drvdata *drvdata = hid_get_drvdata(hdev);
 
-	if ((drvdata->invert_pen_inrange) &&
-	    (report->type == HID_INPUT_REPORT) &&
+	if ((report->type == HID_INPUT_REPORT) &&
 	    (report->id == UCLOGIC_PEN_REPORT_ID) &&
-	    (size >= 2))
-		/* Invert the in-range bit */
-		data[1] ^= 0x40;
+	    (size >= 2)) {
+		if (drvdata->has_virtual_pad_interface && (data[1] & 0x20))
+			/* Change to virtual frame button report ID */
+			data[0] = 0xf7;
+		else if (drvdata->invert_pen_inrange)
+			/* Invert the in-range bit */
+			data[1] ^= 0x40;
+	}
 
 	return 0;
 }
@@ -960,6 +1064,11 @@ static const struct hid_device_id uclogic_devices[] = {
 				USB_DEVICE_ID_UCLOGIC_TABLET_TWHA60) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_HUION, USB_DEVICE_ID_HUION_TABLET) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_UCLOGIC, USB_DEVICE_ID_HUION_TABLET) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_UCLOGIC, USB_DEVICE_ID_YIYNOVA_TABLET) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_UCLOGIC, USB_DEVICE_ID_UGEE_TABLET_81) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_UCLOGIC, USB_DEVICE_ID_UGEE_TABLET_45) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_UCLOGIC, USB_DEVICE_ID_UCLOGIC_DRAWIMAGE_G3) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_UGTIZER, USB_DEVICE_ID_UGTIZER_TABLET_GP0610) },
 	{ }
 };
 MODULE_DEVICE_TABLE(hid, uclogic_devices);
diff --git a/drivers/hid/hid-waltop.c b/drivers/hid/hid-waltop.c
index 059931d7b392..a91aabe4a70a 100644
--- a/drivers/hid/hid-waltop.c
+++ b/drivers/hid/hid-waltop.c
@@ -42,11 +42,6 @@
  * 02 16 02     ink
  */
 
-/*
- * See Slim Tablet 5.8 inch description, device and HID report descriptors at
- * http://sf.net/apps/mediawiki/digimend/?title=Waltop_Slim_Tablet_5.8%22
- */
-
 /* Size of the original report descriptor of Slim Tablet 5.8 inch */
 #define SLIM_TABLET_5_8_INCH_RDESC_ORIG_SIZE	222
 
@@ -98,11 +93,6 @@ static __u8 slim_tablet_5_8_inch_rdesc_fixed[] = {
 	0xC0                /*  End Collection                      */
 };
 
-/*
- * See Slim Tablet 12.1 inch description, device and HID report descriptors at
- * http://sf.net/apps/mediawiki/digimend/?title=Waltop_Slim_Tablet_12.1%22
- */
-
 /* Size of the original report descriptor of Slim Tablet 12.1 inch */
 #define SLIM_TABLET_12_1_INCH_RDESC_ORIG_SIZE	269
 
@@ -154,11 +144,6 @@ static __u8 slim_tablet_12_1_inch_rdesc_fixed[] = {
 	0xC0                /*  End Collection                      */
 };
 
-/*
- * See Q Pad description, device and HID report descriptors at
- * http://sf.net/apps/mediawiki/digimend/?title=Waltop_Q_Pad
- */
-
 /* Size of the original report descriptor of Q Pad */
 #define Q_PAD_RDESC_ORIG_SIZE	241
 
@@ -210,11 +195,6 @@ static __u8 q_pad_rdesc_fixed[] = {
 	0xC0                /*  End Collection                      */
 };
 
-/*
- * See description, device and HID report descriptors of tablet with PID 0038 at
- * http://sf.net/apps/mediawiki/digimend/?title=Waltop_PID_0038
- */
-
 /* Size of the original report descriptor of tablet with PID 0038 */
 #define PID_0038_RDESC_ORIG_SIZE	241
 
@@ -268,11 +248,6 @@ static __u8 pid_0038_rdesc_fixed[] = {
 	0xC0                /*  End Collection                      */
 };
 
-/*
- * See Media Tablet 10.6 inch description, device and HID report descriptors at
- * http://sf.net/apps/mediawiki/digimend/?title=Waltop_Media_Tablet_10.6%22
- */
-
 /* Size of the original report descriptor of Media Tablet 10.6 inch */
 #define MEDIA_TABLET_10_6_INCH_RDESC_ORIG_SIZE	300
 
@@ -386,11 +361,6 @@ static __u8 media_tablet_10_6_inch_rdesc_fixed[] = {
 	0xC0                /*  End Collection                      */
 };
 
-/*
- * See Media Tablet 14.1 inch description, device and HID report descriptors at
- * http://sf.net/apps/mediawiki/digimend/?title=Waltop_Media_Tablet_14.1%22
- */
-
 /* Size of the original report descriptor of Media Tablet 14.1 inch */
 #define MEDIA_TABLET_14_1_INCH_RDESC_ORIG_SIZE	309
 
@@ -502,12 +472,6 @@ static __u8 media_tablet_14_1_inch_rdesc_fixed[] = {
 	0xC0                /*  End Collection                      */
 };
 
-/*
- * See Sirius Battery Free Tablet description, device and HID report descriptors
- * at
- * http://sf.net/apps/mediawiki/digimend/?title=Waltop_Sirius_Battery_Free_Tablet
- */
-
 /* Size of the original report descriptor of Sirius Battery Free Tablet */
 #define SIRIUS_BATTERY_FREE_TABLET_RDESC_ORIG_SIZE	335
 
diff --git a/drivers/hid/intel-ish-hid/Kconfig b/drivers/hid/intel-ish-hid/Kconfig
new file mode 100644
index 000000000000..ea065b3684a2
--- /dev/null
+++ b/drivers/hid/intel-ish-hid/Kconfig
@@ -0,0 +1,17 @@
+menu "Intel ISH HID support"
+	depends on X86_64 && PCI
+
+config INTEL_ISH_HID
+	tristate "Intel Integrated Sensor Hub"
+	default n
+	select HID
+	help
+	  The Integrated Sensor Hub (ISH) enables the ability to offload
+	  sensor polling and algorithm processing to a dedicated low power
+	  processor in the chipset. This allows the core processor to go into
+	  low power modes more often, resulting in the increased battery life.
+	  The current processors that support ISH are: Cherrytrail, Skylake,
+	  Broxton and Kaby Lake.
+
+	  Say Y here if you want to support Intel ISH. If unsure, say N.
+endmenu
diff --git a/drivers/hid/intel-ish-hid/Makefile b/drivers/hid/intel-ish-hid/Makefile
new file mode 100644
index 000000000000..8c08b0b358b1
--- /dev/null
+++ b/drivers/hid/intel-ish-hid/Makefile
@@ -0,0 +1,22 @@
+#
+# Makefile - Intel ISH HID drivers
+# Copyright (c) 2014-2016, Intel Corporation.
+#
+#
+obj-$(CONFIG_INTEL_ISH_HID) += intel-ishtp.o
+intel-ishtp-objs := ishtp/init.o
+intel-ishtp-objs += ishtp/hbm.o
+intel-ishtp-objs += ishtp/client.o
+intel-ishtp-objs += ishtp/bus.o
+intel-ishtp-objs += ishtp/dma-if.o
+intel-ishtp-objs += ishtp/client-buffers.o
+
+obj-$(CONFIG_INTEL_ISH_HID) += intel-ish-ipc.o
+intel-ish-ipc-objs := ipc/ipc.o
+intel-ish-ipc-objs += ipc/pci-ish.o
+
+obj-$(CONFIG_INTEL_ISH_HID) += intel-ishtp-hid.o
+intel-ishtp-hid-objs := ishtp-hid.o
+intel-ishtp-hid-objs += ishtp-hid-client.o
+
+ccflags-y += -Idrivers/hid/intel-ish-hid/ishtp
diff --git a/drivers/hid/intel-ish-hid/ipc/hw-ish-regs.h b/drivers/hid/intel-ish-hid/ipc/hw-ish-regs.h
new file mode 100644
index 000000000000..ab68afcba2a2
--- /dev/null
+++ b/drivers/hid/intel-ish-hid/ipc/hw-ish-regs.h
@@ -0,0 +1,220 @@
+/*
+ * ISH registers definitions
+ *
+ * Copyright (c) 2012-2016, 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 _ISHTP_ISH_REGS_H_
+#define _ISHTP_ISH_REGS_H_
+
+
+/*** IPC PCI Offsets and sizes ***/
+/* ISH IPC Base Address */
+#define IPC_REG_BASE		0x0000
+/* Peripheral Interrupt Status Register */
+#define IPC_REG_PISR_CHV_AB      (IPC_REG_BASE + 0x00)
+/* Peripheral Interrupt Mask Register */
+#define IPC_REG_PIMR_CHV_AB      (IPC_REG_BASE + 0x04)
+/*BXT, CHV_K0*/
+/*Peripheral Interrupt Status Register */
+#define IPC_REG_PISR_BXT	 (IPC_REG_BASE + 0x0C)
+/*Peripheral Interrupt Mask Register */
+#define IPC_REG_PIMR_BXT	 (IPC_REG_BASE + 0x08)
+/***********************************/
+/* ISH Host Firmware status Register */
+#define IPC_REG_ISH_HOST_FWSTS	(IPC_REG_BASE + 0x34)
+/* Host Communication Register */
+#define IPC_REG_HOST_COMM	(IPC_REG_BASE + 0x38)
+/* Reset register */
+#define IPC_REG_ISH_RST		(IPC_REG_BASE + 0x44)
+
+/* Inbound doorbell register Host to ISH */
+#define IPC_REG_HOST2ISH_DRBL	(IPC_REG_BASE + 0x48)
+/* Outbound doorbell register ISH to Host */
+#define IPC_REG_ISH2HOST_DRBL	(IPC_REG_BASE + 0x54)
+/* ISH to HOST message registers */
+#define IPC_REG_ISH2HOST_MSG	(IPC_REG_BASE + 0x60)
+/* HOST to ISH message registers */
+#define IPC_REG_HOST2ISH_MSG	(IPC_REG_BASE + 0xE0)
+/* REMAP2 to enable DMA (D3 RCR) */
+#define	IPC_REG_ISH_RMP2	(IPC_REG_BASE + 0x368)
+
+#define	IPC_REG_MAX		(IPC_REG_BASE + 0x400)
+
+/*** register bits - HISR ***/
+/* bit corresponds HOST2ISH interrupt in PISR and PIMR registers */
+#define IPC_INT_HOST2ISH_BIT            (1<<0)
+/***********************************/
+/*CHV_A0, CHV_B0*/
+/* bit corresponds ISH2HOST interrupt in PISR and PIMR registers */
+#define IPC_INT_ISH2HOST_BIT_CHV_AB	(1<<3)
+/*BXT, CHV_K0*/
+/* bit corresponds ISH2HOST interrupt in PISR and PIMR registers */
+#define IPC_INT_ISH2HOST_BIT_BXT	(1<<0)
+/***********************************/
+
+/* bit corresponds ISH2HOST busy clear interrupt in PIMR register */
+#define IPC_INT_ISH2HOST_CLR_MASK_BIT	(1<<11)
+
+/* offset of ISH2HOST busy clear interrupt in IPC_BUSY_CLR register */
+#define IPC_INT_ISH2HOST_CLR_OFFS	(0)
+
+/* bit corresponds ISH2HOST busy clear interrupt in IPC_BUSY_CLR register */
+#define IPC_INT_ISH2HOST_CLR_BIT	(1<<IPC_INT_ISH2HOST_CLR_OFFS)
+
+/* bit corresponds busy bit in doorbell registers */
+#define IPC_DRBL_BUSY_OFFS		(31)
+#define IPC_DRBL_BUSY_BIT		(1<<IPC_DRBL_BUSY_OFFS)
+
+#define	IPC_HOST_OWNS_MSG_OFFS		(30)
+
+/*
+ * A0: bit means that host owns MSGnn registers and is reading them.
+ * ISH FW may not write to them
+ */
+#define	IPC_HOST_OWNS_MSG_BIT		(1<<IPC_HOST_OWNS_MSG_OFFS)
+
+/*
+ * Host status bits (HOSTCOMM)
+ */
+/* bit corresponds host ready bit in Host Status Register (HOST_COMM) */
+#define IPC_HOSTCOMM_READY_OFFS		(7)
+#define IPC_HOSTCOMM_READY_BIT		(1<<IPC_HOSTCOMM_READY_OFFS)
+
+/***********************************/
+/*CHV_A0, CHV_B0*/
+#define	IPC_HOSTCOMM_INT_EN_OFFS_CHV_AB	(31)
+#define	IPC_HOSTCOMM_INT_EN_BIT_CHV_AB		\
+	(1<<IPC_HOSTCOMM_INT_EN_OFFS_CHV_AB)
+/*BXT, CHV_K0*/
+#define IPC_PIMR_INT_EN_OFFS_BXT	(0)
+#define IPC_PIMR_INT_EN_BIT_BXT		(1<<IPC_PIMR_INT_EN_OFFS_BXT)
+
+#define IPC_HOST2ISH_BUSYCLEAR_MASK_OFFS_BXT	(8)
+#define IPC_HOST2ISH_BUSYCLEAR_MASK_BIT		\
+	(1<<IPC_HOST2ISH_BUSYCLEAR_MASK_OFFS_BXT)
+/***********************************/
+/*
+ * both Host and ISH have ILUP at bit 0
+ * bit corresponds host ready bit in both status registers
+ */
+#define IPC_ILUP_OFFS			(0)
+#define IPC_ILUP_BIT			(1<<IPC_ILUP_OFFS)
+
+/*
+ * FW status bits (relevant)
+ */
+#define	IPC_FWSTS_ILUP		0x1
+#define	IPC_FWSTS_ISHTP_UP	(1<<1)
+#define	IPC_FWSTS_DMA0		(1<<16)
+#define	IPC_FWSTS_DMA1		(1<<17)
+#define	IPC_FWSTS_DMA2		(1<<18)
+#define	IPC_FWSTS_DMA3		(1<<19)
+
+#define	IPC_ISH_IN_DMA		\
+	(IPC_FWSTS_DMA0 | IPC_FWSTS_DMA1 | IPC_FWSTS_DMA2 | IPC_FWSTS_DMA3)
+
+/* bit corresponds host ready bit in ISH FW Status Register */
+#define IPC_ISH_ISHTP_READY_OFFS		(1)
+#define IPC_ISH_ISHTP_READY_BIT		(1<<IPC_ISH_ISHTP_READY_OFFS)
+
+#define	IPC_RMP2_DMA_ENABLED	0x1	/* Value to enable DMA, per D3 RCR */
+
+#define IPC_MSG_MAX_SIZE	0x80
+
+
+#define IPC_HEADER_LENGTH_MASK		0x03FF
+#define IPC_HEADER_PROTOCOL_MASK	0x0F
+#define IPC_HEADER_MNG_CMD_MASK		0x0F
+
+#define IPC_HEADER_LENGTH_OFFSET	0
+#define IPC_HEADER_PROTOCOL_OFFSET	10
+#define IPC_HEADER_MNG_CMD_OFFSET	16
+
+#define IPC_HEADER_GET_LENGTH(drbl_reg)		\
+	(((drbl_reg) >> IPC_HEADER_LENGTH_OFFSET)&IPC_HEADER_LENGTH_MASK)
+#define IPC_HEADER_GET_PROTOCOL(drbl_reg)	\
+	(((drbl_reg) >> IPC_HEADER_PROTOCOL_OFFSET)&IPC_HEADER_PROTOCOL_MASK)
+#define IPC_HEADER_GET_MNG_CMD(drbl_reg)	\
+	(((drbl_reg) >> IPC_HEADER_MNG_CMD_OFFSET)&IPC_HEADER_MNG_CMD_MASK)
+
+#define IPC_IS_BUSY(drbl_reg)			\
+	(((drbl_reg)&IPC_DRBL_BUSY_BIT) == ((uint32_t)IPC_DRBL_BUSY_BIT))
+
+/***********************************/
+/*CHV_A0, CHV_B0*/
+#define IPC_INT_FROM_ISH_TO_HOST_CHV_AB(drbl_reg) \
+	(((drbl_reg)&IPC_INT_ISH2HOST_BIT_CHV_AB) == \
+	((u32)IPC_INT_ISH2HOST_BIT_CHV_AB))
+/*BXT, CHV_K0*/
+#define IPC_INT_FROM_ISH_TO_HOST_BXT(drbl_reg) \
+	(((drbl_reg)&IPC_INT_ISH2HOST_BIT_BXT) == \
+	((u32)IPC_INT_ISH2HOST_BIT_BXT))
+/***********************************/
+
+#define IPC_BUILD_HEADER(length, protocol, busy)		\
+	(((busy)<<IPC_DRBL_BUSY_OFFS) |				\
+	((protocol) << IPC_HEADER_PROTOCOL_OFFSET) |		\
+	((length)<<IPC_HEADER_LENGTH_OFFSET))
+
+#define IPC_BUILD_MNG_MSG(cmd, length)				\
+	(((1)<<IPC_DRBL_BUSY_OFFS)|				\
+	((IPC_PROTOCOL_MNG)<<IPC_HEADER_PROTOCOL_OFFSET)|	\
+	((cmd)<<IPC_HEADER_MNG_CMD_OFFSET)|			\
+	 ((length)<<IPC_HEADER_LENGTH_OFFSET))
+
+
+#define IPC_SET_HOST_READY(host_status)		\
+				((host_status) |= (IPC_HOSTCOMM_READY_BIT))
+
+#define IPC_SET_HOST_ILUP(host_status)		\
+				((host_status) |= (IPC_ILUP_BIT))
+
+#define IPC_CLEAR_HOST_READY(host_status)	\
+				((host_status) ^= (IPC_HOSTCOMM_READY_BIT))
+
+#define IPC_CLEAR_HOST_ILUP(host_status)	\
+				((host_status) ^= (IPC_ILUP_BIT))
+
+/* todo - temp until PIMR HW ready */
+#define IPC_HOST_BUSY_READING_OFFS	6
+
+/* bit corresponds host ready bit in Host Status Register (HOST_COMM) */
+#define IPC_HOST_BUSY_READING_BIT	(1<<IPC_HOST_BUSY_READING_OFFS)
+
+#define IPC_SET_HOST_BUSY_READING(host_status)	\
+				((host_status) |= (IPC_HOST_BUSY_READING_BIT))
+
+#define IPC_CLEAR_HOST_BUSY_READING(host_status)\
+				((host_status) ^= (IPC_HOST_BUSY_READING_BIT))
+
+
+#define IPC_IS_ISH_ISHTP_READY(ish_status)	\
+		(((ish_status) & IPC_ISH_ISHTP_READY_BIT) ==	\
+			((uint32_t)IPC_ISH_ISHTP_READY_BIT))
+
+#define IPC_IS_ISH_ILUP(ish_status)		\
+		(((ish_status) & IPC_ILUP_BIT) == ((uint32_t)IPC_ILUP_BIT))
+
+
+#define IPC_PROTOCOL_ISHTP		1
+#define IPC_PROTOCOL_MNG		3
+
+#define MNG_RX_CMPL_ENABLE		0
+#define MNG_RX_CMPL_DISABLE		1
+#define MNG_RX_CMPL_INDICATION		2
+#define MNG_RESET_NOTIFY		3
+#define MNG_RESET_NOTIFY_ACK		4
+#define MNG_SYNC_FW_CLOCK		5
+#define MNG_ILLEGAL_CMD			0xFF
+
+#endif /* _ISHTP_ISH_REGS_H_ */
diff --git a/drivers/hid/intel-ish-hid/ipc/hw-ish.h b/drivers/hid/intel-ish-hid/ipc/hw-ish.h
new file mode 100644
index 000000000000..46615a03e78f
--- /dev/null
+++ b/drivers/hid/intel-ish-hid/ipc/hw-ish.h
@@ -0,0 +1,71 @@
+/*
+ * H/W layer of ISHTP provider device (ISH)
+ *
+ * Copyright (c) 2014-2016, 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 _ISHTP_HW_ISH_H_
+#define _ISHTP_HW_ISH_H_
+
+#include <linux/pci.h>
+#include <linux/interrupt.h>
+#include "hw-ish-regs.h"
+#include "ishtp-dev.h"
+
+#define CHV_DEVICE_ID		0x22D8
+#define BXT_Ax_DEVICE_ID	0x0AA2
+#define BXT_Bx_DEVICE_ID	0x1AA2
+#define APL_Ax_DEVICE_ID	0x5AA2
+#define SPT_Ax_DEVICE_ID	0x9D35
+
+#define	REVISION_ID_CHT_A0	0x6
+#define	REVISION_ID_CHT_Ax_SI	0x0
+#define	REVISION_ID_CHT_Bx_SI	0x10
+#define	REVISION_ID_CHT_Kx_SI	0x20
+#define	REVISION_ID_CHT_Dx_SI	0x30
+#define	REVISION_ID_CHT_B0	0xB0
+#define	REVISION_ID_SI_MASK	0x70
+
+struct ipc_rst_payload_type {
+	uint16_t	reset_id;
+	uint16_t	reserved;
+};
+
+struct time_sync_format {
+	uint8_t ts1_source;
+	uint8_t ts2_source;
+	uint16_t reserved;
+} __packed;
+
+struct ipc_time_update_msg {
+	uint64_t primary_host_time;
+	struct time_sync_format sync_info;
+	uint64_t secondary_host_time;
+} __packed;
+
+enum {
+	HOST_UTC_TIME_USEC = 0,
+	HOST_SYSTEM_TIME_USEC = 1
+};
+
+struct ish_hw {
+	void __iomem *mem_addr;
+};
+
+#define to_ish_hw(dev) (struct ish_hw *)((dev)->hw)
+
+irqreturn_t ish_irq_handler(int irq, void *dev_id);
+struct ishtp_device *ish_dev_init(struct pci_dev *pdev);
+int ish_hw_start(struct ishtp_device *dev);
+void ish_device_disable(struct ishtp_device *dev);
+
+#endif /* _ISHTP_HW_ISH_H_ */
diff --git a/drivers/hid/intel-ish-hid/ipc/ipc.c b/drivers/hid/intel-ish-hid/ipc/ipc.c
new file mode 100644
index 000000000000..e2517c11e0ee
--- /dev/null
+++ b/drivers/hid/intel-ish-hid/ipc/ipc.c
@@ -0,0 +1,881 @@
+/*
+ * H/W layer of ISHTP provider device (ISH)
+ *
+ * Copyright (c) 2014-2016, 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/sched.h>
+#include <linux/spinlock.h>
+#include <linux/delay.h>
+#include <linux/jiffies.h>
+#include "client.h"
+#include "hw-ish.h"
+#include "utils.h"
+#include "hbm.h"
+
+/* For FW reset flow */
+static struct work_struct fw_reset_work;
+static struct ishtp_device *ishtp_dev;
+
+/**
+ * ish_reg_read() - Read register
+ * @dev: ISHTP device pointer
+ * @offset: Register offset
+ *
+ * Read 32 bit register at a given offset
+ *
+ * Return: Read register value
+ */
+static inline uint32_t ish_reg_read(const struct ishtp_device *dev,
+	unsigned long offset)
+{
+	struct ish_hw *hw = to_ish_hw(dev);
+
+	return readl(hw->mem_addr + offset);
+}
+
+/**
+ * ish_reg_write() - Write register
+ * @dev: ISHTP device pointer
+ * @offset: Register offset
+ * @value: Value to write
+ *
+ * Writes 32 bit register at a give offset
+ */
+static inline void ish_reg_write(struct ishtp_device *dev,
+				 unsigned long offset,
+				 uint32_t value)
+{
+	struct ish_hw *hw = to_ish_hw(dev);
+
+	writel(value, hw->mem_addr + offset);
+}
+
+/**
+ * _ish_read_fw_sts_reg() - Read FW status register
+ * @dev: ISHTP device pointer
+ *
+ * Read FW status register
+ *
+ * Return: Read register value
+ */
+static inline uint32_t _ish_read_fw_sts_reg(struct ishtp_device *dev)
+{
+	return ish_reg_read(dev, IPC_REG_ISH_HOST_FWSTS);
+}
+
+/**
+ * check_generated_interrupt() - Check if ISH interrupt
+ * @dev: ISHTP device pointer
+ *
+ * Check if an interrupt was generated for ISH
+ *
+ * Return: Read true or false
+ */
+static bool check_generated_interrupt(struct ishtp_device *dev)
+{
+	bool interrupt_generated = true;
+	uint32_t pisr_val = 0;
+
+	if (dev->pdev->device == CHV_DEVICE_ID) {
+		pisr_val = ish_reg_read(dev, IPC_REG_PISR_CHV_AB);
+		interrupt_generated =
+			IPC_INT_FROM_ISH_TO_HOST_CHV_AB(pisr_val);
+	} else {
+		pisr_val = ish_reg_read(dev, IPC_REG_PISR_BXT);
+		interrupt_generated = IPC_INT_FROM_ISH_TO_HOST_BXT(pisr_val);
+	}
+
+	return interrupt_generated;
+}
+
+/**
+ * ish_is_input_ready() - Check if FW ready for RX
+ * @dev: ISHTP device pointer
+ *
+ * Check if ISH FW is ready for receiving data
+ *
+ * Return: Read true or false
+ */
+static bool ish_is_input_ready(struct ishtp_device *dev)
+{
+	uint32_t doorbell_val;
+
+	doorbell_val = ish_reg_read(dev, IPC_REG_HOST2ISH_DRBL);
+	return !IPC_IS_BUSY(doorbell_val);
+}
+
+/**
+ * set_host_ready() - Indicate host ready
+ * @dev: ISHTP device pointer
+ *
+ * Set host ready indication to FW
+ */
+static void set_host_ready(struct ishtp_device *dev)
+{
+	if (dev->pdev->device == CHV_DEVICE_ID) {
+		if (dev->pdev->revision == REVISION_ID_CHT_A0 ||
+				(dev->pdev->revision & REVISION_ID_SI_MASK) ==
+				REVISION_ID_CHT_Ax_SI)
+			ish_reg_write(dev, IPC_REG_HOST_COMM, 0x81);
+		else if (dev->pdev->revision == REVISION_ID_CHT_B0 ||
+				(dev->pdev->revision & REVISION_ID_SI_MASK) ==
+				REVISION_ID_CHT_Bx_SI ||
+				(dev->pdev->revision & REVISION_ID_SI_MASK) ==
+				REVISION_ID_CHT_Kx_SI ||
+				(dev->pdev->revision & REVISION_ID_SI_MASK) ==
+				REVISION_ID_CHT_Dx_SI) {
+			uint32_t host_comm_val;
+
+			host_comm_val = ish_reg_read(dev, IPC_REG_HOST_COMM);
+			host_comm_val |= IPC_HOSTCOMM_INT_EN_BIT_CHV_AB | 0x81;
+			ish_reg_write(dev, IPC_REG_HOST_COMM, host_comm_val);
+		}
+	} else {
+			uint32_t host_pimr_val;
+
+			host_pimr_val = ish_reg_read(dev, IPC_REG_PIMR_BXT);
+			host_pimr_val |= IPC_PIMR_INT_EN_BIT_BXT;
+			/*
+			 * disable interrupt generated instead of
+			 * RX_complete_msg
+			 */
+			host_pimr_val &= ~IPC_HOST2ISH_BUSYCLEAR_MASK_BIT;
+
+			ish_reg_write(dev, IPC_REG_PIMR_BXT, host_pimr_val);
+	}
+}
+
+/**
+ * ishtp_fw_is_ready() - Check if FW ready
+ * @dev: ISHTP device pointer
+ *
+ * Check if ISH FW is ready
+ *
+ * Return: Read true or false
+ */
+static bool ishtp_fw_is_ready(struct ishtp_device *dev)
+{
+	uint32_t ish_status = _ish_read_fw_sts_reg(dev);
+
+	return IPC_IS_ISH_ILUP(ish_status) &&
+		IPC_IS_ISH_ISHTP_READY(ish_status);
+}
+
+/**
+ * ish_set_host_rdy() - Indicate host ready
+ * @dev: ISHTP device pointer
+ *
+ * Set host ready indication to FW
+ */
+static void ish_set_host_rdy(struct ishtp_device *dev)
+{
+	uint32_t host_status = ish_reg_read(dev, IPC_REG_HOST_COMM);
+
+	IPC_SET_HOST_READY(host_status);
+	ish_reg_write(dev, IPC_REG_HOST_COMM, host_status);
+}
+
+/**
+ * ish_clr_host_rdy() - Indicate host not ready
+ * @dev: ISHTP device pointer
+ *
+ * Send host not ready indication to FW
+ */
+static void ish_clr_host_rdy(struct ishtp_device *dev)
+{
+	uint32_t host_status = ish_reg_read(dev, IPC_REG_HOST_COMM);
+
+	IPC_CLEAR_HOST_READY(host_status);
+	ish_reg_write(dev, IPC_REG_HOST_COMM, host_status);
+}
+
+/**
+ * _ishtp_read_hdr() - Read message header
+ * @dev: ISHTP device pointer
+ *
+ * Read header of 32bit length
+ *
+ * Return: Read register value
+ */
+static uint32_t _ishtp_read_hdr(const struct ishtp_device *dev)
+{
+	return ish_reg_read(dev, IPC_REG_ISH2HOST_MSG);
+}
+
+/**
+ * _ishtp_read - Read message
+ * @dev: ISHTP device pointer
+ * @buffer: message buffer
+ * @buffer_length: length of message buffer
+ *
+ * Read message from FW
+ *
+ * Return: Always 0
+ */
+static int _ishtp_read(struct ishtp_device *dev, unsigned char *buffer,
+	unsigned long buffer_length)
+{
+	uint32_t	i;
+	uint32_t	*r_buf = (uint32_t *)buffer;
+	uint32_t	msg_offs;
+
+	msg_offs = IPC_REG_ISH2HOST_MSG + sizeof(struct ishtp_msg_hdr);
+	for (i = 0; i < buffer_length; i += sizeof(uint32_t))
+		*r_buf++ = ish_reg_read(dev, msg_offs + i);
+
+	return 0;
+}
+
+/**
+ * write_ipc_from_queue() - try to write ipc msg from Tx queue to device
+ * @dev: ishtp device pointer
+ *
+ * Check if DRBL is cleared. if it is - write the first IPC msg,  then call
+ * the callback function (unless it's NULL)
+ *
+ * Return: 0 for success else failure code
+ */
+static int write_ipc_from_queue(struct ishtp_device *dev)
+{
+	struct wr_msg_ctl_info	*ipc_link;
+	unsigned long	length;
+	unsigned long	rem;
+	unsigned long	flags;
+	uint32_t	doorbell_val;
+	uint32_t	*r_buf;
+	uint32_t	reg_addr;
+	int	i;
+	void	(*ipc_send_compl)(void *);
+	void	*ipc_send_compl_prm;
+	static int	out_ipc_locked;
+	unsigned long	out_ipc_flags;
+
+	if (dev->dev_state == ISHTP_DEV_DISABLED)
+		return	-EINVAL;
+
+	spin_lock_irqsave(&dev->out_ipc_spinlock, out_ipc_flags);
+	if (out_ipc_locked) {
+		spin_unlock_irqrestore(&dev->out_ipc_spinlock, out_ipc_flags);
+		return -EBUSY;
+	}
+	out_ipc_locked = 1;
+	if (!ish_is_input_ready(dev)) {
+		out_ipc_locked = 0;
+		spin_unlock_irqrestore(&dev->out_ipc_spinlock, out_ipc_flags);
+		return -EBUSY;
+	}
+	spin_unlock_irqrestore(&dev->out_ipc_spinlock, out_ipc_flags);
+
+	spin_lock_irqsave(&dev->wr_processing_spinlock, flags);
+	/*
+	 * if tx send list is empty - return 0;
+	 * may happen, as RX_COMPLETE handler doesn't check list emptiness.
+	 */
+	if (list_empty(&dev->wr_processing_list_head.link)) {
+		spin_unlock_irqrestore(&dev->wr_processing_spinlock, flags);
+		out_ipc_locked = 0;
+		return	0;
+	}
+
+	ipc_link = list_entry(dev->wr_processing_list_head.link.next,
+			      struct wr_msg_ctl_info, link);
+	/* first 4 bytes of the data is the doorbell value (IPC header) */
+	length = ipc_link->length - sizeof(uint32_t);
+	doorbell_val = *(uint32_t *)ipc_link->inline_data;
+	r_buf = (uint32_t *)(ipc_link->inline_data + sizeof(uint32_t));
+
+	/* If sending MNG_SYNC_FW_CLOCK, update clock again */
+	if (IPC_HEADER_GET_PROTOCOL(doorbell_val) == IPC_PROTOCOL_MNG &&
+		IPC_HEADER_GET_MNG_CMD(doorbell_val) == MNG_SYNC_FW_CLOCK) {
+		struct timespec ts_system;
+		struct timeval tv_utc;
+		uint64_t        usec_system, usec_utc;
+		struct ipc_time_update_msg time_update;
+		struct time_sync_format ts_format;
+
+		get_monotonic_boottime(&ts_system);
+		do_gettimeofday(&tv_utc);
+		usec_system = (timespec_to_ns(&ts_system)) / NSEC_PER_USEC;
+		usec_utc = (uint64_t)tv_utc.tv_sec * 1000000 +
+						((uint32_t)tv_utc.tv_usec);
+		ts_format.ts1_source = HOST_SYSTEM_TIME_USEC;
+		ts_format.ts2_source = HOST_UTC_TIME_USEC;
+
+		time_update.primary_host_time = usec_system;
+		time_update.secondary_host_time = usec_utc;
+		time_update.sync_info = ts_format;
+
+		memcpy(r_buf, &time_update,
+		       sizeof(struct ipc_time_update_msg));
+	}
+
+	for (i = 0, reg_addr = IPC_REG_HOST2ISH_MSG; i < length >> 2; i++,
+			reg_addr += 4)
+		ish_reg_write(dev, reg_addr, r_buf[i]);
+
+	rem = length & 0x3;
+	if (rem > 0) {
+		uint32_t reg = 0;
+
+		memcpy(&reg, &r_buf[length >> 2], rem);
+		ish_reg_write(dev, reg_addr, reg);
+	}
+	/* Flush writes to msg registers and doorbell */
+	ish_reg_read(dev, IPC_REG_ISH_HOST_FWSTS);
+
+	/* Update IPC counters */
+	++dev->ipc_tx_cnt;
+	dev->ipc_tx_bytes_cnt += IPC_HEADER_GET_LENGTH(doorbell_val);
+
+	ish_reg_write(dev, IPC_REG_HOST2ISH_DRBL, doorbell_val);
+	out_ipc_locked = 0;
+
+	ipc_send_compl = ipc_link->ipc_send_compl;
+	ipc_send_compl_prm = ipc_link->ipc_send_compl_prm;
+	list_del_init(&ipc_link->link);
+	list_add_tail(&ipc_link->link, &dev->wr_free_list_head.link);
+	spin_unlock_irqrestore(&dev->wr_processing_spinlock, flags);
+
+	/*
+	 * callback will be called out of spinlock,
+	 * after ipc_link returned to free list
+	 */
+	if (ipc_send_compl)
+		ipc_send_compl(ipc_send_compl_prm);
+
+	return 0;
+}
+
+/**
+ * write_ipc_to_queue() - write ipc msg to Tx queue
+ * @dev: ishtp device instance
+ * @ipc_send_compl: Send complete callback
+ * @ipc_send_compl_prm:	Parameter to send in complete callback
+ * @msg: Pointer to message
+ * @length: Length of message
+ *
+ * Recived msg with IPC (and upper protocol) header  and add it to the device
+ *  Tx-to-write list then try to send the first IPC waiting msg
+ *  (if DRBL is cleared)
+ * This function returns negative value for failure (means free list
+ *  is empty, or msg too long) and 0 for success.
+ *
+ * Return: 0 for success else failure code
+ */
+static int write_ipc_to_queue(struct ishtp_device *dev,
+	void (*ipc_send_compl)(void *), void *ipc_send_compl_prm,
+	unsigned char *msg, int length)
+{
+	struct wr_msg_ctl_info *ipc_link;
+	unsigned long	flags;
+
+	if (length > IPC_FULL_MSG_SIZE)
+		return -EMSGSIZE;
+
+	spin_lock_irqsave(&dev->wr_processing_spinlock, flags);
+	if (list_empty(&dev->wr_free_list_head.link)) {
+		spin_unlock_irqrestore(&dev->wr_processing_spinlock, flags);
+		return -ENOMEM;
+	}
+	ipc_link = list_entry(dev->wr_free_list_head.link.next,
+		struct wr_msg_ctl_info, link);
+	list_del_init(&ipc_link->link);
+
+	ipc_link->ipc_send_compl = ipc_send_compl;
+	ipc_link->ipc_send_compl_prm = ipc_send_compl_prm;
+	ipc_link->length = length;
+	memcpy(ipc_link->inline_data, msg, length);
+
+	list_add_tail(&ipc_link->link, &dev->wr_processing_list_head.link);
+	spin_unlock_irqrestore(&dev->wr_processing_spinlock, flags);
+
+	write_ipc_from_queue(dev);
+
+	return 0;
+}
+
+/**
+ * ipc_send_mng_msg() - Send management message
+ * @dev: ishtp device instance
+ * @msg_code: Message code
+ * @msg: Pointer to message
+ * @size: Length of message
+ *
+ * Send management message to FW
+ *
+ * Return: 0 for success else failure code
+ */
+static int ipc_send_mng_msg(struct ishtp_device *dev, uint32_t msg_code,
+	void *msg, size_t size)
+{
+	unsigned char	ipc_msg[IPC_FULL_MSG_SIZE];
+	uint32_t	drbl_val = IPC_BUILD_MNG_MSG(msg_code, size);
+
+	memcpy(ipc_msg, &drbl_val, sizeof(uint32_t));
+	memcpy(ipc_msg + sizeof(uint32_t), msg, size);
+	return	write_ipc_to_queue(dev, NULL, NULL, ipc_msg,
+		sizeof(uint32_t) + size);
+}
+
+/**
+ * ish_fw_reset_handler() - FW reset handler
+ * @dev: ishtp device pointer
+ *
+ * Handle FW reset
+ *
+ * Return: 0 for success else failure code
+ */
+static int ish_fw_reset_handler(struct ishtp_device *dev)
+{
+	uint32_t	reset_id;
+	unsigned long	flags;
+	struct wr_msg_ctl_info *processing, *next;
+
+	/* Read reset ID */
+	reset_id = ish_reg_read(dev, IPC_REG_ISH2HOST_MSG) & 0xFFFF;
+
+	/* Clear IPC output queue */
+	spin_lock_irqsave(&dev->wr_processing_spinlock, flags);
+	list_for_each_entry_safe(processing, next,
+			&dev->wr_processing_list_head.link, link) {
+		list_move_tail(&processing->link, &dev->wr_free_list_head.link);
+	}
+	spin_unlock_irqrestore(&dev->wr_processing_spinlock, flags);
+
+	/* ISHTP notification in IPC_RESET */
+	ishtp_reset_handler(dev);
+
+	if (!ish_is_input_ready(dev))
+		timed_wait_for_timeout(WAIT_FOR_SEND_SLICE,
+			ish_is_input_ready(dev), (2 * HZ));
+
+	/* ISH FW is dead */
+	if (!ish_is_input_ready(dev))
+		return	-EPIPE;
+	/*
+	 * Set HOST2ISH.ILUP. Apparently we need this BEFORE sending
+	 * RESET_NOTIFY_ACK - FW will be checking for it
+	 */
+	ish_set_host_rdy(dev);
+	/* Send RESET_NOTIFY_ACK (with reset_id) */
+	ipc_send_mng_msg(dev, MNG_RESET_NOTIFY_ACK, &reset_id,
+			 sizeof(uint32_t));
+
+	/* Wait for ISH FW'es ILUP and ISHTP_READY */
+	timed_wait_for_timeout(WAIT_FOR_SEND_SLICE, ishtp_fw_is_ready(dev),
+		(2 * HZ));
+	if (!ishtp_fw_is_ready(dev)) {
+		/* ISH FW is dead */
+		uint32_t	ish_status;
+
+		ish_status = _ish_read_fw_sts_reg(dev);
+		dev_err(dev->devc,
+			"[ishtp-ish]: completed reset, ISH is dead (FWSTS = %08X)\n",
+			ish_status);
+		return -ENODEV;
+	}
+	return	0;
+}
+
+/**
+ * ish_fw_reset_work_fn() - FW reset worker function
+ * @unused: not used
+ *
+ * Call ish_fw_reset_handler to complete FW reset
+ */
+static void fw_reset_work_fn(struct work_struct *unused)
+{
+	int	rv;
+
+	rv = ish_fw_reset_handler(ishtp_dev);
+	if (!rv) {
+		/* ISH is ILUP & ISHTP-ready. Restart ISHTP */
+		schedule_timeout(HZ / 3);
+		ishtp_dev->recvd_hw_ready = 1;
+		wake_up_interruptible(&ishtp_dev->wait_hw_ready);
+
+		/* ISHTP notification in IPC_RESET sequence completion */
+		ishtp_reset_compl_handler(ishtp_dev);
+	} else
+		dev_err(ishtp_dev->devc, "[ishtp-ish]: FW reset failed (%d)\n",
+			rv);
+}
+
+/**
+ * _ish_sync_fw_clock() -Sync FW clock with the OS clock
+ * @dev: ishtp device pointer
+ *
+ * Sync FW and OS time
+ */
+static void _ish_sync_fw_clock(struct ishtp_device *dev)
+{
+	static unsigned long	prev_sync;
+	struct timespec	ts;
+	uint64_t	usec;
+
+	if (prev_sync && jiffies - prev_sync < 20 * HZ)
+		return;
+
+	prev_sync = jiffies;
+	get_monotonic_boottime(&ts);
+	usec = (timespec_to_ns(&ts)) / NSEC_PER_USEC;
+	ipc_send_mng_msg(dev, MNG_SYNC_FW_CLOCK, &usec, sizeof(uint64_t));
+}
+
+/**
+ * recv_ipc() - Receive and process IPC management messages
+ * @dev: ishtp device instance
+ * @doorbell_val: doorbell value
+ *
+ * This function runs in ISR context.
+ * NOTE: Any other mng command than reset_notify and reset_notify_ack
+ * won't wake BH handler
+ */
+static void	recv_ipc(struct ishtp_device *dev, uint32_t doorbell_val)
+{
+	uint32_t	mng_cmd;
+
+	mng_cmd = IPC_HEADER_GET_MNG_CMD(doorbell_val);
+
+	switch (mng_cmd) {
+	default:
+		break;
+
+	case MNG_RX_CMPL_INDICATION:
+		if (dev->suspend_flag) {
+			dev->suspend_flag = 0;
+			wake_up_interruptible(&dev->suspend_wait);
+		}
+		if (dev->resume_flag) {
+			dev->resume_flag = 0;
+			wake_up_interruptible(&dev->resume_wait);
+		}
+
+		write_ipc_from_queue(dev);
+		break;
+
+	case MNG_RESET_NOTIFY:
+		if (!ishtp_dev) {
+			ishtp_dev = dev;
+			INIT_WORK(&fw_reset_work, fw_reset_work_fn);
+		}
+		schedule_work(&fw_reset_work);
+		break;
+
+	case MNG_RESET_NOTIFY_ACK:
+		dev->recvd_hw_ready = 1;
+		wake_up_interruptible(&dev->wait_hw_ready);
+		break;
+	}
+}
+
+/**
+ * ish_irq_handler() - ISH IRQ handler
+ * @irq: irq number
+ * @dev_id: ishtp device pointer
+ *
+ * ISH IRQ handler. If interrupt is generated and is for ISH it will process
+ * the interrupt.
+ */
+irqreturn_t ish_irq_handler(int irq, void *dev_id)
+{
+	struct ishtp_device	*dev = dev_id;
+	uint32_t	doorbell_val;
+	bool	interrupt_generated;
+
+	/* Check that it's interrupt from ISH (may be shared) */
+	interrupt_generated = check_generated_interrupt(dev);
+
+	if (!interrupt_generated)
+		return IRQ_NONE;
+
+	doorbell_val = ish_reg_read(dev, IPC_REG_ISH2HOST_DRBL);
+	if (!IPC_IS_BUSY(doorbell_val))
+		return IRQ_HANDLED;
+
+	if (dev->dev_state == ISHTP_DEV_DISABLED)
+		return	IRQ_HANDLED;
+
+	/* Sanity check: IPC dgram length in header */
+	if (IPC_HEADER_GET_LENGTH(doorbell_val) > IPC_PAYLOAD_SIZE) {
+		dev_err(dev->devc,
+			"IPC hdr - bad length: %u; dropped\n",
+			(unsigned int)IPC_HEADER_GET_LENGTH(doorbell_val));
+		goto	eoi;
+	}
+
+	switch (IPC_HEADER_GET_PROTOCOL(doorbell_val)) {
+	default:
+		break;
+	case IPC_PROTOCOL_MNG:
+		recv_ipc(dev, doorbell_val);
+		break;
+	case IPC_PROTOCOL_ISHTP:
+		ishtp_recv(dev);
+		break;
+	}
+
+eoi:
+	/* Update IPC counters */
+	++dev->ipc_rx_cnt;
+	dev->ipc_rx_bytes_cnt += IPC_HEADER_GET_LENGTH(doorbell_val);
+
+	ish_reg_write(dev, IPC_REG_ISH2HOST_DRBL, 0);
+	/* Flush write to doorbell */
+	ish_reg_read(dev, IPC_REG_ISH_HOST_FWSTS);
+
+	return	IRQ_HANDLED;
+}
+
+/**
+ * _ish_hw_reset() - HW reset
+ * @dev: ishtp device pointer
+ *
+ * Reset ISH HW to recover if any error
+ *
+ * Return: 0 for success else error fault code
+ */
+static int _ish_hw_reset(struct ishtp_device *dev)
+{
+	struct pci_dev *pdev = dev->pdev;
+	int	rv;
+	unsigned int	dma_delay;
+	uint16_t csr;
+
+	if (!pdev)
+		return	-ENODEV;
+
+	rv = pci_reset_function(pdev);
+	if (!rv)
+		dev->dev_state = ISHTP_DEV_RESETTING;
+
+	if (!pdev->pm_cap) {
+		dev_err(&pdev->dev, "Can't reset - no PM caps\n");
+		return	-EINVAL;
+	}
+
+	/* Now trigger reset to FW */
+	ish_reg_write(dev, IPC_REG_ISH_RMP2, 0);
+
+	for (dma_delay = 0; dma_delay < MAX_DMA_DELAY &&
+		_ish_read_fw_sts_reg(dev) & (IPC_ISH_IN_DMA);
+		dma_delay += 5)
+		mdelay(5);
+
+	if (dma_delay >= MAX_DMA_DELAY) {
+		dev_err(&pdev->dev,
+			"Can't reset - stuck with DMA in-progress\n");
+		return	-EBUSY;
+	}
+
+	pci_read_config_word(pdev, pdev->pm_cap + PCI_PM_CTRL, &csr);
+
+	csr &= ~PCI_PM_CTRL_STATE_MASK;
+	csr |= PCI_D3hot;
+	pci_write_config_word(pdev, pdev->pm_cap + PCI_PM_CTRL, csr);
+
+	mdelay(pdev->d3_delay);
+
+	csr &= ~PCI_PM_CTRL_STATE_MASK;
+	csr |= PCI_D0;
+	pci_write_config_word(pdev, pdev->pm_cap + PCI_PM_CTRL, csr);
+
+	ish_reg_write(dev, IPC_REG_ISH_RMP2, IPC_RMP2_DMA_ENABLED);
+
+	/*
+	 * Send 0 IPC message so that ISH FW wakes up if it was already
+	 * asleep
+	 */
+	ish_reg_write(dev, IPC_REG_HOST2ISH_DRBL, IPC_DRBL_BUSY_BIT);
+
+	/* Flush writes to doorbell and REMAP2 */
+	ish_reg_read(dev, IPC_REG_ISH_HOST_FWSTS);
+
+	return	0;
+}
+
+/**
+ * _ish_ipc_reset() - IPC reset
+ * @dev: ishtp device pointer
+ *
+ * Resets host and fw IPC and upper layers
+ *
+ * Return: 0 for success else error fault code
+ */
+static int _ish_ipc_reset(struct ishtp_device *dev)
+{
+	struct ipc_rst_payload_type ipc_mng_msg;
+	int	rv = 0;
+
+	ipc_mng_msg.reset_id = 1;
+	ipc_mng_msg.reserved = 0;
+
+	set_host_ready(dev);
+
+	/* Clear the incoming doorbell */
+	ish_reg_write(dev, IPC_REG_ISH2HOST_DRBL, 0);
+	/* Flush write to doorbell */
+	ish_reg_read(dev, IPC_REG_ISH_HOST_FWSTS);
+
+	dev->recvd_hw_ready = 0;
+
+	/* send message */
+	rv = ipc_send_mng_msg(dev, MNG_RESET_NOTIFY, &ipc_mng_msg,
+		sizeof(struct ipc_rst_payload_type));
+	if (rv) {
+		dev_err(dev->devc, "Failed to send IPC MNG_RESET_NOTIFY\n");
+		return	rv;
+	}
+
+	wait_event_interruptible_timeout(dev->wait_hw_ready,
+					 dev->recvd_hw_ready, 2 * HZ);
+	if (!dev->recvd_hw_ready) {
+		dev_err(dev->devc, "Timed out waiting for HW ready\n");
+		rv = -ENODEV;
+	}
+
+	return rv;
+}
+
+/**
+ * ish_hw_start() -Start ISH HW
+ * @dev: ishtp device pointer
+ *
+ * Set host to ready state and wait for FW reset
+ *
+ * Return: 0 for success else error fault code
+ */
+int ish_hw_start(struct ishtp_device *dev)
+{
+	ish_set_host_rdy(dev);
+	/* After that we can enable ISH DMA operation */
+	ish_reg_write(dev, IPC_REG_ISH_RMP2, IPC_RMP2_DMA_ENABLED);
+
+	/*
+	 * Send 0 IPC message so that ISH FW wakes up if it was already
+	 * asleep
+	 */
+	ish_reg_write(dev, IPC_REG_HOST2ISH_DRBL, IPC_DRBL_BUSY_BIT);
+	/* Flush write to doorbell */
+	ish_reg_read(dev, IPC_REG_ISH_HOST_FWSTS);
+
+	set_host_ready(dev);
+
+	/* wait for FW-initiated reset flow */
+	if (!dev->recvd_hw_ready)
+		wait_event_interruptible_timeout(dev->wait_hw_ready,
+						 dev->recvd_hw_ready,
+						 10 * HZ);
+
+	if (!dev->recvd_hw_ready) {
+		dev_err(dev->devc,
+			"[ishtp-ish]: Timed out waiting for FW-initiated reset\n");
+		return	-ENODEV;
+	}
+
+	return 0;
+}
+
+/**
+ * ish_ipc_get_header() -Get doorbell value
+ * @dev: ishtp device pointer
+ * @length: length of message
+ * @busy: busy status
+ *
+ * Get door bell value from message header
+ *
+ * Return: door bell value
+ */
+static uint32_t ish_ipc_get_header(struct ishtp_device *dev, int length,
+				   int busy)
+{
+	uint32_t drbl_val;
+
+	drbl_val = IPC_BUILD_HEADER(length, IPC_PROTOCOL_ISHTP, busy);
+
+	return drbl_val;
+}
+
+static const struct ishtp_hw_ops ish_hw_ops = {
+	.hw_reset = _ish_hw_reset,
+	.ipc_reset = _ish_ipc_reset,
+	.ipc_get_header = ish_ipc_get_header,
+	.ishtp_read = _ishtp_read,
+	.write = write_ipc_to_queue,
+	.get_fw_status = _ish_read_fw_sts_reg,
+	.sync_fw_clock = _ish_sync_fw_clock,
+	.ishtp_read_hdr = _ishtp_read_hdr
+};
+
+/**
+ * ish_dev_init() -Initialize ISH devoce
+ * @pdev: PCI device
+ *
+ * Allocate ISHTP device and initialize IPC processing
+ *
+ * Return: ISHTP device instance on success else NULL
+ */
+struct ishtp_device *ish_dev_init(struct pci_dev *pdev)
+{
+	struct ishtp_device *dev;
+	int	i;
+
+	dev = kzalloc(sizeof(struct ishtp_device) + sizeof(struct ish_hw),
+		GFP_KERNEL);
+	if (!dev)
+		return NULL;
+
+	ishtp_device_init(dev);
+
+	init_waitqueue_head(&dev->wait_hw_ready);
+
+	spin_lock_init(&dev->wr_processing_spinlock);
+	spin_lock_init(&dev->out_ipc_spinlock);
+
+	/* Init IPC processing and free lists */
+	INIT_LIST_HEAD(&dev->wr_processing_list_head.link);
+	INIT_LIST_HEAD(&dev->wr_free_list_head.link);
+	for (i = 0; i < IPC_TX_FIFO_SIZE; ++i) {
+		struct wr_msg_ctl_info	*tx_buf;
+
+		tx_buf = kzalloc(sizeof(struct wr_msg_ctl_info), GFP_KERNEL);
+		if (!tx_buf) {
+			/*
+			 * IPC buffers may be limited or not available
+			 * at all - although this shouldn't happen
+			 */
+			dev_err(dev->devc,
+				"[ishtp-ish]: failure in Tx FIFO allocations (%d)\n",
+				i);
+			break;
+		}
+		list_add_tail(&tx_buf->link, &dev->wr_free_list_head.link);
+	}
+
+	dev->ops = &ish_hw_ops;
+	dev->devc = &pdev->dev;
+	dev->mtu = IPC_PAYLOAD_SIZE - sizeof(struct ishtp_msg_hdr);
+	return dev;
+}
+
+/**
+ * ish_device_disable() - Disable ISH device
+ * @dev: ISHTP device pointer
+ *
+ * Disable ISH by clearing host ready to inform firmware.
+ */
+void	ish_device_disable(struct ishtp_device *dev)
+{
+	dev->dev_state = ISHTP_DEV_DISABLED;
+	ish_clr_host_rdy(dev);
+}
diff --git a/drivers/hid/intel-ish-hid/ipc/pci-ish.c b/drivers/hid/intel-ish-hid/ipc/pci-ish.c
new file mode 100644
index 000000000000..42f0beeb09fd
--- /dev/null
+++ b/drivers/hid/intel-ish-hid/ipc/pci-ish.c
@@ -0,0 +1,322 @@
+/*
+ * PCI glue for ISHTP provider device (ISH) driver
+ *
+ * Copyright (c) 2014-2016, 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/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/device.h>
+#include <linux/fs.h>
+#include <linux/errno.h>
+#include <linux/types.h>
+#include <linux/pci.h>
+#include <linux/sched.h>
+#include <linux/interrupt.h>
+#include <linux/workqueue.h>
+#include <linux/miscdevice.h>
+#define CREATE_TRACE_POINTS
+#include <trace/events/intel_ish.h>
+#include "ishtp-dev.h"
+#include "hw-ish.h"
+
+static const struct pci_device_id ish_pci_tbl[] = {
+	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, CHV_DEVICE_ID)},
+	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, BXT_Ax_DEVICE_ID)},
+	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, BXT_Bx_DEVICE_ID)},
+	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, APL_Ax_DEVICE_ID)},
+	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, SPT_Ax_DEVICE_ID)},
+	{0, }
+};
+MODULE_DEVICE_TABLE(pci, ish_pci_tbl);
+
+/**
+ * ish_event_tracer() - Callback function to dump trace messages
+ * @dev:	ishtp device
+ * @format:	printf style format
+ *
+ * Callback to direct log messages to Linux trace buffers
+ */
+static void ish_event_tracer(struct ishtp_device *dev, char *format, ...)
+{
+	if (trace_ishtp_dump_enabled()) {
+		va_list args;
+		char tmp_buf[100];
+
+		va_start(args, format);
+		vsnprintf(tmp_buf, sizeof(tmp_buf), format, args);
+		va_end(args);
+
+		trace_ishtp_dump(tmp_buf);
+	}
+}
+
+/**
+ * ish_init() - Init function
+ * @dev:	ishtp device
+ *
+ * This function initialize wait queues for suspend/resume and call
+ * calls hadware initialization function. This will initiate
+ * startup sequence
+ *
+ * Return: 0 for success or error code for failure
+ */
+static int ish_init(struct ishtp_device *dev)
+{
+	int ret;
+
+	/* Set the state of ISH HW to start */
+	ret = ish_hw_start(dev);
+	if (ret) {
+		dev_err(dev->devc, "ISH: hw start failed.\n");
+		return ret;
+	}
+
+	/* Start the inter process communication to ISH processor */
+	ret = ishtp_start(dev);
+	if (ret) {
+		dev_err(dev->devc, "ISHTP: Protocol init failed.\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+/**
+ * ish_probe() - PCI driver probe callback
+ * @pdev:	pci device
+ * @ent:	pci device id
+ *
+ * Initialize PCI function, setup interrupt and call for ISH initialization
+ *
+ * Return: 0 for success or error code for failure
+ */
+static int ish_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
+{
+	struct ishtp_device *dev;
+	struct ish_hw *hw;
+	int	ret;
+
+	/* enable pci dev */
+	ret = pci_enable_device(pdev);
+	if (ret) {
+		dev_err(&pdev->dev, "ISH: Failed to enable PCI device\n");
+		return ret;
+	}
+
+	/* set PCI host mastering */
+	pci_set_master(pdev);
+
+	/* pci request regions for ISH driver */
+	ret = pci_request_regions(pdev, KBUILD_MODNAME);
+	if (ret) {
+		dev_err(&pdev->dev, "ISH: Failed to get PCI regions\n");
+		goto disable_device;
+	}
+
+	/* allocates and initializes the ISH dev structure */
+	dev = ish_dev_init(pdev);
+	if (!dev) {
+		ret = -ENOMEM;
+		goto release_regions;
+	}
+	hw = to_ish_hw(dev);
+	dev->print_log = ish_event_tracer;
+
+	/* mapping IO device memory */
+	hw->mem_addr = pci_iomap(pdev, 0, 0);
+	if (!hw->mem_addr) {
+		dev_err(&pdev->dev, "ISH: mapping I/O range failure\n");
+		ret = -ENOMEM;
+		goto free_device;
+	}
+
+	dev->pdev = pdev;
+
+	pdev->dev_flags |= PCI_DEV_FLAGS_NO_D3;
+
+	/* request and enable interrupt */
+	ret = request_irq(pdev->irq, ish_irq_handler, IRQF_NO_SUSPEND,
+			  KBUILD_MODNAME, dev);
+	if (ret) {
+		dev_err(&pdev->dev, "ISH: request IRQ failure (%d)\n",
+			pdev->irq);
+		goto free_device;
+	}
+
+	dev_set_drvdata(dev->devc, dev);
+
+	init_waitqueue_head(&dev->suspend_wait);
+	init_waitqueue_head(&dev->resume_wait);
+
+	ret = ish_init(dev);
+	if (ret)
+		goto free_irq;
+
+	return 0;
+
+free_irq:
+	free_irq(pdev->irq, dev);
+free_device:
+	pci_iounmap(pdev, hw->mem_addr);
+	kfree(dev);
+release_regions:
+	pci_release_regions(pdev);
+disable_device:
+	pci_clear_master(pdev);
+	pci_disable_device(pdev);
+	dev_err(&pdev->dev, "ISH: PCI driver initialization failed.\n");
+
+	return ret;
+}
+
+/**
+ * ish_remove() - PCI driver remove callback
+ * @pdev:	pci device
+ *
+ * This function does cleanup of ISH on pci remove callback
+ */
+static void ish_remove(struct pci_dev *pdev)
+{
+	struct ishtp_device *ishtp_dev = pci_get_drvdata(pdev);
+	struct ish_hw *hw = to_ish_hw(ishtp_dev);
+
+	ishtp_bus_remove_all_clients(ishtp_dev, false);
+	ish_device_disable(ishtp_dev);
+
+	free_irq(pdev->irq, ishtp_dev);
+	pci_iounmap(pdev, hw->mem_addr);
+	pci_release_regions(pdev);
+	pci_clear_master(pdev);
+	pci_disable_device(pdev);
+	kfree(ishtp_dev);
+}
+
+static struct device *ish_resume_device;
+
+/**
+ * ish_resume_handler() - Work function to complete resume
+ * @work:	work struct
+ *
+ * The resume work function to complete resume function asynchronously.
+ * There are two types of platforms, one where ISH is not powered off,
+ * in that case a simple resume message is enough, others we need
+ * a reset sequence.
+ */
+static void ish_resume_handler(struct work_struct *work)
+{
+	struct pci_dev *pdev = to_pci_dev(ish_resume_device);
+	struct ishtp_device *dev = pci_get_drvdata(pdev);
+	int ret;
+
+	ishtp_send_resume(dev);
+
+	/* 50 ms to get resume response */
+	if (dev->resume_flag)
+		ret = wait_event_interruptible_timeout(dev->resume_wait,
+						       !dev->resume_flag,
+						       msecs_to_jiffies(50));
+
+	/*
+	 * If no resume response. This platform  is not S0ix compatible
+	 * So on resume full reboot of ISH processor will happen, so
+	 * need to go through init sequence again
+	 */
+	if (dev->resume_flag)
+		ish_init(dev);
+}
+
+/**
+ * ish_suspend() - ISH suspend callback
+ * @device:	device pointer
+ *
+ * ISH suspend callback
+ *
+ * Return: 0 to the pm core
+ */
+static int ish_suspend(struct device *device)
+{
+	struct pci_dev *pdev = to_pci_dev(device);
+	struct ishtp_device *dev = pci_get_drvdata(pdev);
+
+	enable_irq_wake(pdev->irq);
+	/*
+	 * If previous suspend hasn't been asnwered then ISH is likely dead,
+	 * don't attempt nested notification
+	 */
+	if (dev->suspend_flag)
+		return	0;
+
+	dev->resume_flag = 0;
+	dev->suspend_flag = 1;
+	ishtp_send_suspend(dev);
+
+	/* 25 ms should be enough for live ISH to flush all IPC buf */
+	if (dev->suspend_flag)
+		wait_event_interruptible_timeout(dev->suspend_wait,
+						 !dev->suspend_flag,
+						  msecs_to_jiffies(25));
+
+	return 0;
+}
+
+static DECLARE_WORK(resume_work, ish_resume_handler);
+/**
+ * ish_resume() - ISH resume callback
+ * @device:	device pointer
+ *
+ * ISH resume callback
+ *
+ * Return: 0 to the pm core
+ */
+static int ish_resume(struct device *device)
+{
+	struct pci_dev *pdev = to_pci_dev(device);
+	struct ishtp_device *dev = pci_get_drvdata(pdev);
+
+	ish_resume_device = device;
+	dev->resume_flag = 1;
+
+	disable_irq_wake(pdev->irq);
+	schedule_work(&resume_work);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static const struct dev_pm_ops ish_pm_ops = {
+	.suspend = ish_suspend,
+	.resume = ish_resume,
+};
+#define ISHTP_ISH_PM_OPS	(&ish_pm_ops)
+#else
+#define ISHTP_ISH_PM_OPS	NULL
+#endif
+
+static struct pci_driver ish_driver = {
+	.name = KBUILD_MODNAME,
+	.id_table = ish_pci_tbl,
+	.probe = ish_probe,
+	.remove = ish_remove,
+	.driver.pm = ISHTP_ISH_PM_OPS,
+};
+
+module_pci_driver(ish_driver);
+
+/* Original author */
+MODULE_AUTHOR("Daniel Drubin <daniel.drubin@intel.com>");
+/* Adoption to upstream Linux kernel */
+MODULE_AUTHOR("Srinivas Pandruvada <srinivas.pandruvada@linux.intel.com>");
+
+MODULE_DESCRIPTION("Intel(R) Integrated Sensor Hub PCI Device Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/hid/intel-ish-hid/ipc/utils.h b/drivers/hid/intel-ish-hid/ipc/utils.h
new file mode 100644
index 000000000000..5a82123dc7b4
--- /dev/null
+++ b/drivers/hid/intel-ish-hid/ipc/utils.h
@@ -0,0 +1,64 @@
+/*
+ * Utility macros of ISH
+ *
+ * Copyright (c) 2014-2016, 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 UTILS__H
+#define UTILS__H
+
+#define	WAIT_FOR_SEND_SLICE	(HZ / 10)
+#define	WAIT_FOR_CONNECT_SLICE	(HZ / 10)
+
+/*
+ * Waits for specified event when a thread that triggers event can't signal
+ * Also, waits *at_least* `timeinc` after condition is satisfied
+ */
+#define	timed_wait_for(timeinc, condition)			\
+	do {							\
+		int completed = 0;				\
+		do {						\
+			unsigned long	j;			\
+			int	done = 0;			\
+								\
+			completed = (condition);		\
+			for (j = jiffies, done = 0; !done; ) {	\
+				schedule_timeout(timeinc);	\
+				if (time_is_before_eq_jiffies(j + timeinc)) \
+					done = 1;		\
+			}					\
+		} while (!(completed));				\
+	} while (0)
+
+
+/*
+ * Waits for specified event when a thread that triggers event
+ * can't signal with timeout (use whenever we may hang)
+ */
+#define	timed_wait_for_timeout(timeinc, condition, timeout)	\
+	do {							\
+		int	t = timeout;				\
+		do {						\
+			unsigned long	j;			\
+			int	done = 0;			\
+								\
+			for (j = jiffies, done = 0; !done; ) {	\
+				schedule_timeout(timeinc);	\
+				if (time_is_before_eq_jiffies(j + timeinc)) \
+					done = 1;		\
+			} \
+			t -= timeinc;				\
+			if (t <= 0)				\
+				break;				\
+		} while (!(condition));				\
+	} while (0)
+
+#endif /* UTILS__H */
diff --git a/drivers/hid/intel-ish-hid/ishtp-hid-client.c b/drivers/hid/intel-ish-hid/ishtp-hid-client.c
new file mode 100644
index 000000000000..5c643d7a07b2
--- /dev/null
+++ b/drivers/hid/intel-ish-hid/ishtp-hid-client.c
@@ -0,0 +1,978 @@
+/*
+ * ISHTP client driver for HID (ISH)
+ *
+ * Copyright (c) 2014-2016, 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/module.h>
+#include <linux/hid.h>
+#include <linux/sched.h>
+#include "ishtp/ishtp-dev.h"
+#include "ishtp/client.h"
+#include "ishtp-hid.h"
+
+/* Rx ring buffer pool size */
+#define HID_CL_RX_RING_SIZE	32
+#define HID_CL_TX_RING_SIZE	16
+
+/**
+ * report_bad_packets() - Report bad packets
+ * @hid_ishtp_cl:	Client instance to get stats
+ * @recv_buf:		Raw received host interface message
+ * @cur_pos:		Current position index in payload
+ * @payload_len:	Length of payload expected
+ *
+ * Dumps error in case bad packet is received
+ */
+static void report_bad_packet(struct ishtp_cl *hid_ishtp_cl, void *recv_buf,
+			      size_t cur_pos,  size_t payload_len)
+{
+	struct hostif_msg *recv_msg = recv_buf;
+	struct ishtp_cl_data *client_data = hid_ishtp_cl->client_data;
+
+	dev_err(&client_data->cl_device->dev, "[hid-ish]: BAD packet %02X\n"
+		"total_bad=%u cur_pos=%u\n"
+		"[%02X %02X %02X %02X]\n"
+		"payload_len=%u\n"
+		"multi_packet_cnt=%u\n"
+		"is_response=%02X\n",
+		recv_msg->hdr.command, client_data->bad_recv_cnt,
+		(unsigned int)cur_pos,
+		((unsigned char *)recv_msg)[0], ((unsigned char *)recv_msg)[1],
+		((unsigned char *)recv_msg)[2], ((unsigned char *)recv_msg)[3],
+		(unsigned int)payload_len, client_data->multi_packet_cnt,
+		recv_msg->hdr.command & ~CMD_MASK);
+}
+
+/**
+ * process_recv() - Received and parse incoming packet
+ * @hid_ishtp_cl:	Client instance to get stats
+ * @recv_buf:		Raw received host interface message
+ * @data_len:		length of the message
+ *
+ * Parse the incoming packet. If it is a response packet then it will update
+ * per instance flags and wake up the caller waiting to for the response.
+ */
+static void process_recv(struct ishtp_cl *hid_ishtp_cl, void *recv_buf,
+			 size_t data_len)
+{
+	struct hostif_msg *recv_msg;
+	unsigned char *payload;
+	struct device_info *dev_info;
+	int i, j;
+	size_t	payload_len, total_len, cur_pos;
+	int report_type;
+	struct report_list *reports_list;
+	char *reports;
+	size_t report_len;
+	struct ishtp_cl_data *client_data = hid_ishtp_cl->client_data;
+	int curr_hid_dev = client_data->cur_hid_dev;
+
+	if (data_len < sizeof(struct hostif_msg_hdr)) {
+		dev_err(&client_data->cl_device->dev,
+			"[hid-ish]: error, received %u which is less than data header %u\n",
+			(unsigned int)data_len,
+			(unsigned int)sizeof(struct hostif_msg_hdr));
+		++client_data->bad_recv_cnt;
+		ish_hw_reset(hid_ishtp_cl->dev);
+		return;
+	}
+
+	payload = recv_buf + sizeof(struct hostif_msg_hdr);
+	total_len = data_len;
+	cur_pos = 0;
+
+	do {
+		recv_msg = (struct hostif_msg *)(recv_buf + cur_pos);
+		payload_len = recv_msg->hdr.size;
+
+		/* Sanity checks */
+		if (cur_pos + payload_len + sizeof(struct hostif_msg) >
+				total_len) {
+			++client_data->bad_recv_cnt;
+			report_bad_packet(hid_ishtp_cl, recv_msg, cur_pos,
+					  payload_len);
+			ish_hw_reset(hid_ishtp_cl->dev);
+			break;
+		}
+
+		hid_ishtp_trace(client_data,  "%s %d\n",
+				__func__, recv_msg->hdr.command & CMD_MASK);
+
+		switch (recv_msg->hdr.command & CMD_MASK) {
+		case HOSTIF_DM_ENUM_DEVICES:
+			if ((!(recv_msg->hdr.command & ~CMD_MASK) ||
+					client_data->init_done)) {
+				++client_data->bad_recv_cnt;
+				report_bad_packet(hid_ishtp_cl, recv_msg,
+						  cur_pos,
+						  payload_len);
+				ish_hw_reset(hid_ishtp_cl->dev);
+				break;
+			}
+			client_data->hid_dev_count = (unsigned int)*payload;
+			if (!client_data->hid_devices)
+				client_data->hid_devices = devm_kzalloc(
+						&client_data->cl_device->dev,
+						client_data->hid_dev_count *
+						sizeof(struct device_info),
+						GFP_KERNEL);
+			if (!client_data->hid_devices) {
+				dev_err(&client_data->cl_device->dev,
+				"Mem alloc failed for hid device info\n");
+				wake_up_interruptible(&client_data->init_wait);
+				break;
+			}
+			for (i = 0; i < client_data->hid_dev_count; ++i) {
+				if (1 + sizeof(struct device_info) * i >=
+						payload_len) {
+					dev_err(&client_data->cl_device->dev,
+						"[hid-ish]: [ENUM_DEVICES]: content size %lu is bigger than payload_len %u\n",
+						1 + sizeof(struct device_info)
+						* i,
+						(unsigned int)payload_len);
+				}
+
+				if (1 + sizeof(struct device_info) * i >=
+						data_len)
+					break;
+
+				dev_info = (struct device_info *)(payload + 1 +
+					sizeof(struct device_info) * i);
+				if (client_data->hid_devices)
+					memcpy(client_data->hid_devices + i,
+					       dev_info,
+					       sizeof(struct device_info));
+			}
+
+			client_data->enum_devices_done = true;
+			wake_up_interruptible(&client_data->init_wait);
+
+			break;
+
+		case HOSTIF_GET_HID_DESCRIPTOR:
+			if ((!(recv_msg->hdr.command & ~CMD_MASK) ||
+					client_data->init_done)) {
+				++client_data->bad_recv_cnt;
+				report_bad_packet(hid_ishtp_cl, recv_msg,
+						  cur_pos,
+						  payload_len);
+				ish_hw_reset(hid_ishtp_cl->dev);
+				break;
+			}
+			if (!client_data->hid_descr[curr_hid_dev])
+				client_data->hid_descr[curr_hid_dev] =
+				devm_kmalloc(&client_data->cl_device->dev,
+					     payload_len, GFP_KERNEL);
+			if (client_data->hid_descr[curr_hid_dev]) {
+				memcpy(client_data->hid_descr[curr_hid_dev],
+				       payload, payload_len);
+				client_data->hid_descr_size[curr_hid_dev] =
+					payload_len;
+				client_data->hid_descr_done = true;
+			}
+			wake_up_interruptible(&client_data->init_wait);
+
+			break;
+
+		case HOSTIF_GET_REPORT_DESCRIPTOR:
+			if ((!(recv_msg->hdr.command & ~CMD_MASK) ||
+					client_data->init_done)) {
+				++client_data->bad_recv_cnt;
+				report_bad_packet(hid_ishtp_cl, recv_msg,
+						  cur_pos,
+						  payload_len);
+				ish_hw_reset(hid_ishtp_cl->dev);
+				break;
+			}
+			if (!client_data->report_descr[curr_hid_dev])
+				client_data->report_descr[curr_hid_dev] =
+				devm_kmalloc(&client_data->cl_device->dev,
+					     payload_len, GFP_KERNEL);
+			if (client_data->report_descr[curr_hid_dev])  {
+				memcpy(client_data->report_descr[curr_hid_dev],
+				       payload,
+				       payload_len);
+				client_data->report_descr_size[curr_hid_dev] =
+					payload_len;
+				client_data->report_descr_done = true;
+			}
+			wake_up_interruptible(&client_data->init_wait);
+
+			break;
+
+		case HOSTIF_GET_FEATURE_REPORT:
+			report_type = HID_FEATURE_REPORT;
+			goto	do_get_report;
+
+		case HOSTIF_GET_INPUT_REPORT:
+			report_type = HID_INPUT_REPORT;
+do_get_report:
+			/* Get index of device that matches this id */
+			for (i = 0; i < client_data->num_hid_devices; ++i) {
+				if (recv_msg->hdr.device_id ==
+					client_data->hid_devices[i].dev_id)
+					if (client_data->hid_sensor_hubs[i]) {
+						hid_input_report(
+						client_data->hid_sensor_hubs[
+									i],
+						report_type, payload,
+						payload_len, 0);
+						ishtp_hid_wakeup(
+						client_data->hid_sensor_hubs[
+							i]);
+						break;
+					}
+			}
+			break;
+
+		case HOSTIF_SET_FEATURE_REPORT:
+			/* Get index of device that matches this id */
+			for (i = 0; i < client_data->num_hid_devices; ++i) {
+				if (recv_msg->hdr.device_id ==
+					client_data->hid_devices[i].dev_id)
+					if (client_data->hid_sensor_hubs[i]) {
+						ishtp_hid_wakeup(
+						client_data->hid_sensor_hubs[
+							i]);
+						break;
+					}
+			}
+			break;
+
+		case HOSTIF_PUBLISH_INPUT_REPORT:
+			report_type = HID_INPUT_REPORT;
+			for (i = 0; i < client_data->num_hid_devices; ++i)
+				if (recv_msg->hdr.device_id ==
+					client_data->hid_devices[i].dev_id)
+					if (client_data->hid_sensor_hubs[i])
+						hid_input_report(
+						client_data->hid_sensor_hubs[
+									i],
+						report_type, payload,
+						payload_len, 0);
+			break;
+
+		case HOSTIF_PUBLISH_INPUT_REPORT_LIST:
+			report_type = HID_INPUT_REPORT;
+			reports_list = (struct report_list *)payload;
+			reports = (char *)reports_list->reports;
+
+			for (j = 0; j < reports_list->num_of_reports; j++) {
+				recv_msg = (struct hostif_msg *)(reports +
+					sizeof(uint16_t));
+				report_len = *(uint16_t *)reports;
+				payload = reports + sizeof(uint16_t) +
+					sizeof(struct hostif_msg_hdr);
+				payload_len = report_len -
+					sizeof(struct hostif_msg_hdr);
+
+				for (i = 0; i < client_data->num_hid_devices;
+				     ++i)
+					if (recv_msg->hdr.device_id ==
+					client_data->hid_devices[i].dev_id &&
+					client_data->hid_sensor_hubs[i]) {
+						hid_input_report(
+						client_data->hid_sensor_hubs[
+									i],
+						report_type,
+						payload, payload_len,
+						0);
+					}
+
+				reports += sizeof(uint16_t) + report_len;
+			}
+			break;
+		default:
+			++client_data->bad_recv_cnt;
+			report_bad_packet(hid_ishtp_cl, recv_msg, cur_pos,
+					  payload_len);
+			ish_hw_reset(hid_ishtp_cl->dev);
+			break;
+
+		}
+
+		if (!cur_pos && cur_pos + payload_len +
+				sizeof(struct hostif_msg) < total_len)
+			++client_data->multi_packet_cnt;
+
+		cur_pos += payload_len + sizeof(struct hostif_msg);
+		payload += payload_len + sizeof(struct hostif_msg);
+
+	} while (cur_pos < total_len);
+}
+
+/**
+ * ish_cl_event_cb() - bus driver callback for incoming message/packet
+ * @device:	Pointer to the the ishtp client device for which this message
+ *		is targeted
+ *
+ * Remove the packet from the list and process the message by calling
+ * process_recv
+ */
+static void ish_cl_event_cb(struct ishtp_cl_device *device)
+{
+	struct ishtp_cl	*hid_ishtp_cl = device->driver_data;
+	struct ishtp_cl_rb *rb_in_proc;
+	size_t r_length;
+	unsigned long flags;
+
+	if (!hid_ishtp_cl)
+		return;
+
+	spin_lock_irqsave(&hid_ishtp_cl->in_process_spinlock, flags);
+	while (!list_empty(&hid_ishtp_cl->in_process_list.list)) {
+		rb_in_proc = list_entry(
+			hid_ishtp_cl->in_process_list.list.next,
+			struct ishtp_cl_rb, list);
+		list_del_init(&rb_in_proc->list);
+		spin_unlock_irqrestore(&hid_ishtp_cl->in_process_spinlock,
+			flags);
+
+		if (!rb_in_proc->buffer.data)
+			return;
+
+		r_length = rb_in_proc->buf_idx;
+
+		/* decide what to do with received data */
+		process_recv(hid_ishtp_cl, rb_in_proc->buffer.data, r_length);
+
+		ishtp_cl_io_rb_recycle(rb_in_proc);
+		spin_lock_irqsave(&hid_ishtp_cl->in_process_spinlock, flags);
+	}
+	spin_unlock_irqrestore(&hid_ishtp_cl->in_process_spinlock, flags);
+}
+
+/**
+ * hid_ishtp_set_feature() - send request to ISH FW to set a feature request
+ * @hid:	hid device instance for this request
+ * @buf:	feature buffer
+ * @len:	Length of feature buffer
+ * @report_id:	Report id for the feature set request
+ *
+ * This is called from hid core .request() callback. This function doesn't wait
+ * for response.
+ */
+void hid_ishtp_set_feature(struct hid_device *hid, char *buf, unsigned int len,
+			   int report_id)
+{
+	struct ishtp_hid_data *hid_data =  hid->driver_data;
+	struct ishtp_cl_data *client_data = hid_data->client_data;
+	struct hostif_msg *msg = (struct hostif_msg *)buf;
+	int	rv;
+	int	i;
+
+	hid_ishtp_trace(client_data,  "%s hid %p\n", __func__, hid);
+
+	rv = ishtp_hid_link_ready_wait(client_data);
+	if (rv) {
+		hid_ishtp_trace(client_data,  "%s hid %p link not ready\n",
+				__func__, hid);
+		return;
+	}
+
+	memset(msg, 0, sizeof(struct hostif_msg));
+	msg->hdr.command = HOSTIF_SET_FEATURE_REPORT;
+	for (i = 0; i < client_data->num_hid_devices; ++i) {
+		if (hid == client_data->hid_sensor_hubs[i]) {
+			msg->hdr.device_id =
+				client_data->hid_devices[i].dev_id;
+			break;
+		}
+	}
+
+	if (i == client_data->num_hid_devices)
+		return;
+
+	rv = ishtp_cl_send(client_data->hid_ishtp_cl, buf, len);
+	if (rv)
+		hid_ishtp_trace(client_data,  "%s hid %p send failed\n",
+				__func__, hid);
+}
+
+/**
+ * hid_ishtp_get_report() - request to get feature/input report
+ * @hid:	hid device instance for this request
+ * @report_id:	Report id for the get request
+ * @report_type:	Report type for the this request
+ *
+ * This is called from hid core .request() callback. This function will send
+ * request to FW and return without waiting for response.
+ */
+void hid_ishtp_get_report(struct hid_device *hid, int report_id,
+			  int report_type)
+{
+	struct ishtp_hid_data *hid_data =  hid->driver_data;
+	struct ishtp_cl_data *client_data = hid_data->client_data;
+	static unsigned char	buf[10];
+	unsigned int	len;
+	struct hostif_msg_to_sensor *msg = (struct hostif_msg_to_sensor *)buf;
+	int	rv;
+	int	i;
+
+	hid_ishtp_trace(client_data,  "%s hid %p\n", __func__, hid);
+	rv = ishtp_hid_link_ready_wait(client_data);
+	if (rv) {
+		hid_ishtp_trace(client_data,  "%s hid %p link not ready\n",
+				__func__, hid);
+		return;
+	}
+
+	len = sizeof(struct hostif_msg_to_sensor);
+
+	memset(msg, 0, sizeof(struct hostif_msg_to_sensor));
+	msg->hdr.command = (report_type == HID_FEATURE_REPORT) ?
+		HOSTIF_GET_FEATURE_REPORT : HOSTIF_GET_INPUT_REPORT;
+	for (i = 0; i < client_data->num_hid_devices; ++i) {
+		if (hid == client_data->hid_sensor_hubs[i]) {
+			msg->hdr.device_id =
+				client_data->hid_devices[i].dev_id;
+			break;
+		}
+	}
+
+	if (i == client_data->num_hid_devices)
+		return;
+
+	msg->report_id = report_id;
+	rv = ishtp_cl_send(client_data->hid_ishtp_cl, buf, len);
+	if (rv)
+		hid_ishtp_trace(client_data,  "%s hid %p send failed\n",
+				__func__, hid);
+}
+
+/**
+ * ishtp_hid_link_ready_wait() - Wait for link ready
+ * @client_data:	client data instance
+ *
+ * If the transport link started suspend process, then wait, till either
+ * resumed or timeout
+ *
+ * Return: 0 on success, non zero on error
+ */
+int ishtp_hid_link_ready_wait(struct ishtp_cl_data *client_data)
+{
+	int rc;
+
+	if (client_data->suspended) {
+		hid_ishtp_trace(client_data,  "wait for link ready\n");
+		rc = wait_event_interruptible_timeout(
+					client_data->ishtp_resume_wait,
+					!client_data->suspended,
+					5 * HZ);
+
+		if (rc == 0) {
+			hid_ishtp_trace(client_data,  "link not ready\n");
+			return -EIO;
+		}
+		hid_ishtp_trace(client_data,  "link ready\n");
+	}
+
+	return 0;
+}
+
+/**
+ * ishtp_enum_enum_devices() - Enumerate hid devices
+ * @hid_ishtp_cl:	client instance
+ *
+ * Helper function to send request to firmware to enumerate HID devices
+ *
+ * Return: 0 on success, non zero on error
+ */
+static int ishtp_enum_enum_devices(struct ishtp_cl *hid_ishtp_cl)
+{
+	struct hostif_msg msg;
+	struct ishtp_cl_data *client_data = hid_ishtp_cl->client_data;
+	int retry_count;
+	int rv;
+
+	/* Send HOSTIF_DM_ENUM_DEVICES */
+	memset(&msg, 0, sizeof(struct hostif_msg));
+	msg.hdr.command = HOSTIF_DM_ENUM_DEVICES;
+	rv = ishtp_cl_send(hid_ishtp_cl, (unsigned char *)&msg,
+			   sizeof(struct hostif_msg));
+	if (rv)
+		return rv;
+
+	retry_count = 0;
+	while (!client_data->enum_devices_done &&
+	       retry_count < 10) {
+		wait_event_interruptible_timeout(client_data->init_wait,
+					 client_data->enum_devices_done,
+					 3 * HZ);
+		++retry_count;
+		if (!client_data->enum_devices_done)
+			/* Send HOSTIF_DM_ENUM_DEVICES */
+			rv = ishtp_cl_send(hid_ishtp_cl,
+					   (unsigned char *) &msg,
+					   sizeof(struct hostif_msg));
+	}
+	if (!client_data->enum_devices_done) {
+		dev_err(&client_data->cl_device->dev,
+			"[hid-ish]: timed out waiting for enum_devices\n");
+		return -ETIMEDOUT;
+	}
+	if (!client_data->hid_devices) {
+		dev_err(&client_data->cl_device->dev,
+			"[hid-ish]: failed to allocate HID dev structures\n");
+		return -ENOMEM;
+	}
+
+	client_data->num_hid_devices = client_data->hid_dev_count;
+	dev_info(&hid_ishtp_cl->device->dev,
+		"[hid-ish]: enum_devices_done OK, num_hid_devices=%d\n",
+		client_data->num_hid_devices);
+
+	return	0;
+}
+
+/**
+ * ishtp_get_hid_descriptor() - Get hid descriptor
+ * @hid_ishtp_cl:	client instance
+ * @index:		Index into the hid_descr array
+ *
+ * Helper function to send request to firmware get HID descriptor of a device
+ *
+ * Return: 0 on success, non zero on error
+ */
+static int ishtp_get_hid_descriptor(struct ishtp_cl *hid_ishtp_cl, int index)
+{
+	struct hostif_msg msg;
+	struct ishtp_cl_data *client_data = hid_ishtp_cl->client_data;
+	int rv;
+
+	/* Get HID descriptor */
+	client_data->hid_descr_done = false;
+	memset(&msg, 0, sizeof(struct hostif_msg));
+	msg.hdr.command = HOSTIF_GET_HID_DESCRIPTOR;
+	msg.hdr.device_id = client_data->hid_devices[index].dev_id;
+	rv = ishtp_cl_send(hid_ishtp_cl, (unsigned char *) &msg,
+			   sizeof(struct hostif_msg));
+	if (rv)
+		return rv;
+
+	if (!client_data->hid_descr_done) {
+		wait_event_interruptible_timeout(client_data->init_wait,
+						 client_data->hid_descr_done,
+						 3 * HZ);
+		if (!client_data->hid_descr_done) {
+			dev_err(&client_data->cl_device->dev,
+				"[hid-ish]: timed out for hid_descr_done\n");
+			return -EIO;
+		}
+
+		if (!client_data->hid_descr[index]) {
+			dev_err(&client_data->cl_device->dev,
+				"[hid-ish]: allocation HID desc fail\n");
+			return -ENOMEM;
+		}
+	}
+
+	return 0;
+}
+
+/**
+ * ishtp_get_report_descriptor() - Get report descriptor
+ * @hid_ishtp_cl:	client instance
+ * @index:		Index into the hid_descr array
+ *
+ * Helper function to send request to firmware get HID report descriptor of
+ * a device
+ *
+ * Return: 0 on success, non zero on error
+ */
+static int ishtp_get_report_descriptor(struct ishtp_cl *hid_ishtp_cl,
+				       int index)
+{
+	struct hostif_msg msg;
+	struct ishtp_cl_data *client_data = hid_ishtp_cl->client_data;
+	int rv;
+
+	/* Get report descriptor */
+	client_data->report_descr_done = false;
+	memset(&msg, 0, sizeof(struct hostif_msg));
+	msg.hdr.command = HOSTIF_GET_REPORT_DESCRIPTOR;
+	msg.hdr.device_id = client_data->hid_devices[index].dev_id;
+	rv = ishtp_cl_send(hid_ishtp_cl, (unsigned char *) &msg,
+			   sizeof(struct hostif_msg));
+	if (rv)
+		return rv;
+
+	if (!client_data->report_descr_done)
+		wait_event_interruptible_timeout(client_data->init_wait,
+					 client_data->report_descr_done,
+					 3 * HZ);
+	if (!client_data->report_descr_done) {
+		dev_err(&client_data->cl_device->dev,
+				"[hid-ish]: timed out for report descr\n");
+		return -EIO;
+	}
+	if (!client_data->report_descr[index]) {
+		dev_err(&client_data->cl_device->dev,
+			"[hid-ish]: failed to alloc report descr\n");
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+
+/**
+ * hid_ishtp_cl_init() - Init function for ISHTP client
+ * @hid_ishtp_cl:	ISHTP client instance
+ * @reset:		true if called for init after reset
+ *
+ * This function complete the initializtion of the client. The summary of
+ * processing:
+ * - Send request to enumerate the hid clients
+ *	Get the HID descriptor for each enumearated device
+ *	Get report description of each device
+ *	Register each device wik hid core by calling ishtp_hid_probe
+ *
+ * Return: 0 on success, non zero on error
+ */
+static int hid_ishtp_cl_init(struct ishtp_cl *hid_ishtp_cl, int reset)
+{
+	struct ishtp_device *dev;
+	unsigned long flags;
+	struct ishtp_cl_data *client_data = hid_ishtp_cl->client_data;
+	int i;
+	int rv;
+
+	dev_dbg(&client_data->cl_device->dev, "%s\n", __func__);
+	hid_ishtp_trace(client_data,  "%s reset flag: %d\n", __func__, reset);
+
+	rv = ishtp_cl_link(hid_ishtp_cl, ISHTP_HOST_CLIENT_ID_ANY);
+	if (rv) {
+		dev_err(&client_data->cl_device->dev,
+			"ishtp_cl_link failed\n");
+		return	-ENOMEM;
+	}
+
+	client_data->init_done = 0;
+
+	dev = hid_ishtp_cl->dev;
+
+	/* Connect to FW client */
+	hid_ishtp_cl->rx_ring_size = HID_CL_RX_RING_SIZE;
+	hid_ishtp_cl->tx_ring_size = HID_CL_TX_RING_SIZE;
+
+	spin_lock_irqsave(&dev->fw_clients_lock, flags);
+	i = ishtp_fw_cl_by_uuid(dev, &hid_ishtp_guid);
+	if (i < 0) {
+		spin_unlock_irqrestore(&dev->fw_clients_lock, flags);
+		dev_err(&client_data->cl_device->dev,
+			"ish client uuid not found\n");
+		return i;
+	}
+	hid_ishtp_cl->fw_client_id = dev->fw_clients[i].client_id;
+	spin_unlock_irqrestore(&dev->fw_clients_lock, flags);
+	hid_ishtp_cl->state = ISHTP_CL_CONNECTING;
+
+	rv = ishtp_cl_connect(hid_ishtp_cl);
+	if (rv) {
+		dev_err(&client_data->cl_device->dev,
+			"client connect fail\n");
+		goto err_cl_unlink;
+	}
+
+	hid_ishtp_trace(client_data,  "%s client connected\n", __func__);
+
+	/* Register read callback */
+	ishtp_register_event_cb(hid_ishtp_cl->device, ish_cl_event_cb);
+
+	rv = ishtp_enum_enum_devices(hid_ishtp_cl);
+	if (rv)
+		goto err_cl_disconnect;
+
+	hid_ishtp_trace(client_data,  "%s enumerated device count %d\n",
+			__func__, client_data->num_hid_devices);
+
+	for (i = 0; i < client_data->num_hid_devices; ++i) {
+		client_data->cur_hid_dev = i;
+
+		rv = ishtp_get_hid_descriptor(hid_ishtp_cl, i);
+		if (rv)
+			goto err_cl_disconnect;
+
+		rv = ishtp_get_report_descriptor(hid_ishtp_cl, i);
+		if (rv)
+			goto err_cl_disconnect;
+
+		if (!reset) {
+			rv = ishtp_hid_probe(i, client_data);
+			if (rv) {
+				dev_err(&client_data->cl_device->dev,
+				"[hid-ish]: HID probe for #%u failed: %d\n",
+				i, rv);
+				goto err_cl_disconnect;
+			}
+		}
+	} /* for() on all hid devices */
+
+	client_data->init_done = 1;
+	client_data->suspended = false;
+	wake_up_interruptible(&client_data->ishtp_resume_wait);
+	hid_ishtp_trace(client_data,  "%s successful init\n", __func__);
+	return 0;
+
+err_cl_disconnect:
+	hid_ishtp_cl->state = ISHTP_CL_DISCONNECTING;
+	ishtp_cl_disconnect(hid_ishtp_cl);
+err_cl_unlink:
+	ishtp_cl_unlink(hid_ishtp_cl);
+	return rv;
+}
+
+/**
+ * hid_ishtp_cl_deinit() - Deinit function for ISHTP client
+ * @hid_ishtp_cl:	ISHTP client instance
+ *
+ * Unlink and free hid client
+ */
+static void hid_ishtp_cl_deinit(struct ishtp_cl *hid_ishtp_cl)
+{
+	ishtp_cl_unlink(hid_ishtp_cl);
+	ishtp_cl_flush_queues(hid_ishtp_cl);
+
+	/* disband and free all Tx and Rx client-level rings */
+	ishtp_cl_free(hid_ishtp_cl);
+}
+
+static void hid_ishtp_cl_reset_handler(struct work_struct *work)
+{
+	struct ishtp_cl_data *client_data;
+	struct ishtp_cl *hid_ishtp_cl;
+	struct ishtp_cl_device *cl_device;
+	int retry;
+	int rv;
+
+	client_data = container_of(work, struct ishtp_cl_data, work);
+
+	hid_ishtp_cl = client_data->hid_ishtp_cl;
+	cl_device = client_data->cl_device;
+
+	hid_ishtp_trace(client_data, "%s hid_ishtp_cl %p\n", __func__,
+			hid_ishtp_cl);
+	dev_dbg(&cl_device->dev, "%s\n", __func__);
+
+	hid_ishtp_cl_deinit(hid_ishtp_cl);
+
+	hid_ishtp_cl = ishtp_cl_allocate(cl_device->ishtp_dev);
+	if (!hid_ishtp_cl)
+		return;
+
+	cl_device->driver_data = hid_ishtp_cl;
+	hid_ishtp_cl->client_data = client_data;
+	client_data->hid_ishtp_cl = hid_ishtp_cl;
+
+	client_data->num_hid_devices = 0;
+
+	for (retry = 0; retry < 3; ++retry) {
+		rv = hid_ishtp_cl_init(hid_ishtp_cl, 1);
+		if (!rv)
+			break;
+		dev_err(&client_data->cl_device->dev, "Retry reset init\n");
+	}
+	if (rv) {
+		dev_err(&client_data->cl_device->dev, "Reset Failed\n");
+		hid_ishtp_trace(client_data, "%s Failed hid_ishtp_cl %p\n",
+				__func__, hid_ishtp_cl);
+	}
+}
+
+/**
+ * hid_ishtp_cl_probe() - ISHTP client driver probe
+ * @cl_device:		ISHTP client device instance
+ *
+ * This function gets called on device create on ISHTP bus
+ *
+ * Return: 0 on success, non zero on error
+ */
+static int hid_ishtp_cl_probe(struct ishtp_cl_device *cl_device)
+{
+	struct ishtp_cl *hid_ishtp_cl;
+	struct ishtp_cl_data *client_data;
+	int rv;
+
+	if (!cl_device)
+		return	-ENODEV;
+
+	if (uuid_le_cmp(hid_ishtp_guid,
+			cl_device->fw_client->props.protocol_name) != 0)
+		return	-ENODEV;
+
+	client_data = devm_kzalloc(&cl_device->dev, sizeof(*client_data),
+				   GFP_KERNEL);
+	if (!client_data)
+		return -ENOMEM;
+
+	hid_ishtp_cl = ishtp_cl_allocate(cl_device->ishtp_dev);
+	if (!hid_ishtp_cl)
+		return -ENOMEM;
+
+	cl_device->driver_data = hid_ishtp_cl;
+	hid_ishtp_cl->client_data = client_data;
+	client_data->hid_ishtp_cl = hid_ishtp_cl;
+	client_data->cl_device = cl_device;
+
+	init_waitqueue_head(&client_data->init_wait);
+	init_waitqueue_head(&client_data->ishtp_resume_wait);
+
+	INIT_WORK(&client_data->work, hid_ishtp_cl_reset_handler);
+
+	rv = hid_ishtp_cl_init(hid_ishtp_cl, 0);
+	if (rv) {
+		ishtp_cl_free(hid_ishtp_cl);
+		return rv;
+	}
+	ishtp_get_device(cl_device);
+
+	return 0;
+}
+
+/**
+ * hid_ishtp_cl_remove() - ISHTP client driver remove
+ * @cl_device:		ISHTP client device instance
+ *
+ * This function gets called on device remove on ISHTP bus
+ *
+ * Return: 0
+ */
+static int hid_ishtp_cl_remove(struct ishtp_cl_device *cl_device)
+{
+	struct ishtp_cl *hid_ishtp_cl = cl_device->driver_data;
+	struct ishtp_cl_data *client_data = hid_ishtp_cl->client_data;
+
+	hid_ishtp_trace(client_data, "%s hid_ishtp_cl %p\n", __func__,
+			hid_ishtp_cl);
+
+	dev_dbg(&cl_device->dev, "%s\n", __func__);
+	hid_ishtp_cl->state = ISHTP_CL_DISCONNECTING;
+	ishtp_cl_disconnect(hid_ishtp_cl);
+	ishtp_put_device(cl_device);
+	ishtp_hid_remove(client_data);
+	hid_ishtp_cl_deinit(hid_ishtp_cl);
+
+	hid_ishtp_cl = NULL;
+
+	client_data->num_hid_devices = 0;
+
+	return 0;
+}
+
+/**
+ * hid_ishtp_cl_reset() - ISHTP client driver reset
+ * @cl_device:		ISHTP client device instance
+ *
+ * This function gets called on device reset on ISHTP bus
+ *
+ * Return: 0
+ */
+static int hid_ishtp_cl_reset(struct ishtp_cl_device *cl_device)
+{
+	struct ishtp_cl *hid_ishtp_cl = cl_device->driver_data;
+	struct ishtp_cl_data *client_data = hid_ishtp_cl->client_data;
+
+	hid_ishtp_trace(client_data, "%s hid_ishtp_cl %p\n", __func__,
+			hid_ishtp_cl);
+
+	schedule_work(&client_data->work);
+
+	return 0;
+}
+
+#define to_ishtp_cl_device(d) container_of(d, struct ishtp_cl_device, dev)
+
+/**
+ * hid_ishtp_cl_suspend() - ISHTP client driver suspend
+ * @device:	device instance
+ *
+ * This function gets called on system suspend
+ *
+ * Return: 0
+ */
+static int hid_ishtp_cl_suspend(struct device *device)
+{
+	struct ishtp_cl_device *cl_device = to_ishtp_cl_device(device);
+	struct ishtp_cl *hid_ishtp_cl = cl_device->driver_data;
+	struct ishtp_cl_data *client_data = hid_ishtp_cl->client_data;
+
+	hid_ishtp_trace(client_data, "%s hid_ishtp_cl %p\n", __func__,
+			hid_ishtp_cl);
+	client_data->suspended = true;
+
+	return 0;
+}
+
+/**
+ * hid_ishtp_cl_resume() - ISHTP client driver resume
+ * @device:	device instance
+ *
+ * This function gets called on system resume
+ *
+ * Return: 0
+ */
+static int hid_ishtp_cl_resume(struct device *device)
+{
+	struct ishtp_cl_device *cl_device = to_ishtp_cl_device(device);
+	struct ishtp_cl *hid_ishtp_cl = cl_device->driver_data;
+	struct ishtp_cl_data *client_data = hid_ishtp_cl->client_data;
+
+	hid_ishtp_trace(client_data, "%s hid_ishtp_cl %p\n", __func__,
+			hid_ishtp_cl);
+	client_data->suspended = false;
+	return 0;
+}
+
+static const struct dev_pm_ops hid_ishtp_pm_ops = {
+	.suspend = hid_ishtp_cl_suspend,
+	.resume = hid_ishtp_cl_resume,
+};
+
+static struct ishtp_cl_driver	hid_ishtp_cl_driver = {
+	.name = "ish-hid",
+	.probe = hid_ishtp_cl_probe,
+	.remove = hid_ishtp_cl_remove,
+	.reset = hid_ishtp_cl_reset,
+	.driver.pm = &hid_ishtp_pm_ops,
+};
+
+static int __init ish_hid_init(void)
+{
+	int	rv;
+
+	/* Register ISHTP client device driver with ISHTP Bus */
+	rv = ishtp_cl_driver_register(&hid_ishtp_cl_driver);
+
+	return rv;
+
+}
+
+static void __exit ish_hid_exit(void)
+{
+	ishtp_cl_driver_unregister(&hid_ishtp_cl_driver);
+}
+
+late_initcall(ish_hid_init);
+module_exit(ish_hid_exit);
+
+MODULE_DESCRIPTION("ISH ISHTP HID client driver");
+/* Primary author */
+MODULE_AUTHOR("Daniel Drubin <daniel.drubin@intel.com>");
+/*
+ * Several modification for multi instance support
+ * suspend/resume and clean up
+ */
+MODULE_AUTHOR("Srinivas Pandruvada <srinivas.pandruvada@linux.intel.com>");
+
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("ishtp:*");
diff --git a/drivers/hid/intel-ish-hid/ishtp-hid.c b/drivers/hid/intel-ish-hid/ishtp-hid.c
new file mode 100644
index 000000000000..277983aa1d90
--- /dev/null
+++ b/drivers/hid/intel-ish-hid/ishtp-hid.c
@@ -0,0 +1,246 @@
+/*
+ * ISHTP-HID glue driver.
+ *
+ * Copyright (c) 2012-2016, 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/hid.h>
+#include <uapi/linux/input.h>
+#include "ishtp/client.h"
+#include "ishtp-hid.h"
+
+/**
+ * ishtp_hid_parse() - hid-core .parse() callback
+ * @hid:	hid device instance
+ *
+ * This function gets called during call to hid_add_device
+ *
+ * Return: 0 on success and non zero on error
+ */
+static int ishtp_hid_parse(struct hid_device *hid)
+{
+	struct ishtp_hid_data *hid_data =  hid->driver_data;
+	struct ishtp_cl_data *client_data = hid_data->client_data;
+	int rv;
+
+	rv = hid_parse_report(hid, client_data->report_descr[hid_data->index],
+			      client_data->report_descr_size[hid_data->index]);
+	if (rv)
+		return	rv;
+
+	return 0;
+}
+
+/* Empty callbacks with success return code */
+static int ishtp_hid_start(struct hid_device *hid)
+{
+	return 0;
+}
+
+static void ishtp_hid_stop(struct hid_device *hid)
+{
+}
+
+static int ishtp_hid_open(struct hid_device *hid)
+{
+	return 0;
+}
+
+static void ishtp_hid_close(struct hid_device *hid)
+{
+}
+
+static int ishtp_raw_request(struct hid_device *hdev, unsigned char reportnum,
+	__u8 *buf, size_t len, unsigned char rtype, int reqtype)
+{
+	return 0;
+}
+
+/**
+ * ishtp_hid_request() - hid-core .request() callback
+ * @hid:	hid device instance
+ * @rep:	pointer to hid_report
+ * @reqtype:	type of req. [GET|SET]_REPORT
+ *
+ * This function is used to set/get feaure/input report.
+ */
+static void ishtp_hid_request(struct hid_device *hid, struct hid_report *rep,
+	int reqtype)
+{
+	struct ishtp_hid_data *hid_data =  hid->driver_data;
+	/* the specific report length, just HID part of it */
+	unsigned int len = ((rep->size - 1) >> 3) + 1 + (rep->id > 0);
+	char *buf;
+	unsigned int header_size = sizeof(struct hostif_msg);
+
+	len += header_size;
+
+	hid_data->request_done = false;
+	switch (reqtype) {
+	case HID_REQ_GET_REPORT:
+		hid_ishtp_get_report(hid, rep->id, rep->type);
+		break;
+	case HID_REQ_SET_REPORT:
+		/*
+		 * Spare 7 bytes for 64b accesses through
+		 * get/put_unaligned_le64()
+		 */
+		buf = kzalloc(len + 7, GFP_KERNEL);
+		if (!buf)
+			return;
+
+		hid_output_report(rep, buf + header_size);
+		hid_ishtp_set_feature(hid, buf, len, rep->id);
+		kfree(buf);
+		break;
+	}
+}
+
+/**
+ * ishtp_wait_for_response() - hid-core .wait() callback
+ * @hid:	hid device instance
+ *
+ * This function is used to wait after get feaure/input report.
+ *
+ * Return: 0 on success and non zero on error
+ */
+static int ishtp_wait_for_response(struct hid_device *hid)
+{
+	struct ishtp_hid_data *hid_data =  hid->driver_data;
+	struct ishtp_cl_data *client_data = hid_data->client_data;
+	int rv;
+
+	hid_ishtp_trace(client_data,  "%s hid %p\n", __func__, hid);
+
+	rv = ishtp_hid_link_ready_wait(hid_data->client_data);
+	if (rv)
+		return rv;
+
+	if (!hid_data->request_done)
+		wait_event_interruptible_timeout(hid_data->hid_wait,
+					hid_data->request_done, 3 * HZ);
+
+	if (!hid_data->request_done) {
+		hid_err(hid,
+			"timeout waiting for response from ISHTP device\n");
+		return -ETIMEDOUT;
+	}
+	hid_ishtp_trace(client_data,  "%s hid %p done\n", __func__, hid);
+
+	hid_data->request_done = false;
+
+	return 0;
+}
+
+/**
+ * ishtp_hid_wakeup() - Wakeup caller
+ * @hid:	hid device instance
+ *
+ * This function will wakeup caller waiting for Get/Set feature report
+ */
+void ishtp_hid_wakeup(struct hid_device *hid)
+{
+	struct ishtp_hid_data *hid_data = hid->driver_data;
+
+	hid_data->request_done = true;
+	wake_up_interruptible(&hid_data->hid_wait);
+}
+
+static struct hid_ll_driver ishtp_hid_ll_driver = {
+	.parse = ishtp_hid_parse,
+	.start = ishtp_hid_start,
+	.stop = ishtp_hid_stop,
+	.open = ishtp_hid_open,
+	.close = ishtp_hid_close,
+	.request = ishtp_hid_request,
+	.wait = ishtp_wait_for_response,
+	.raw_request = ishtp_raw_request
+};
+
+/**
+ * ishtp_hid_probe() - hid register ll driver
+ * @cur_hid_dev:	Index of hid device calling to register
+ * @client_data:	Client data pointer
+ *
+ * This function is used to allocate and add HID device.
+ *
+ * Return: 0 on success, non zero on error
+ */
+int ishtp_hid_probe(unsigned int cur_hid_dev,
+		    struct ishtp_cl_data *client_data)
+{
+	int rv;
+	struct hid_device *hid;
+	struct ishtp_hid_data *hid_data;
+
+	hid = hid_allocate_device();
+	if (IS_ERR(hid)) {
+		rv = PTR_ERR(hid);
+		return	-ENOMEM;
+	}
+
+	hid_data = kzalloc(sizeof(*hid_data), GFP_KERNEL);
+	if (!hid_data) {
+		rv = -ENOMEM;
+		goto err_hid_data;
+	}
+
+	hid_data->index = cur_hid_dev;
+	hid_data->client_data = client_data;
+	init_waitqueue_head(&hid_data->hid_wait);
+
+	hid->driver_data = hid_data;
+
+	client_data->hid_sensor_hubs[cur_hid_dev] = hid;
+
+	hid->ll_driver = &ishtp_hid_ll_driver;
+	hid->bus = BUS_INTEL_ISHTP;
+	hid->dev.parent = &client_data->cl_device->dev;
+	hid->version = le16_to_cpu(ISH_HID_VERSION);
+	hid->vendor = le16_to_cpu(ISH_HID_VENDOR);
+	hid->product = le16_to_cpu(ISH_HID_PRODUCT);
+	snprintf(hid->name, sizeof(hid->name), "%s %04hX:%04hX", "hid-ishtp",
+		hid->vendor, hid->product);
+
+	rv = hid_add_device(hid);
+	if (rv)
+		goto err_hid_device;
+
+	hid_ishtp_trace(client_data,  "%s allocated hid %p\n", __func__, hid);
+
+	return 0;
+
+err_hid_device:
+	kfree(hid_data);
+err_hid_data:
+	kfree(hid);
+	return rv;
+}
+
+/**
+ * ishtp_hid_probe() - Remove registered hid device
+ * @client_data:	client data pointer
+ *
+ * This function is used to destroy allocatd HID device.
+ */
+void ishtp_hid_remove(struct ishtp_cl_data *client_data)
+{
+	int i;
+
+	for (i = 0; i < client_data->num_hid_devices; ++i) {
+		if (client_data->hid_sensor_hubs[i]) {
+			kfree(client_data->hid_sensor_hubs[i]->driver_data);
+			hid_destroy_device(client_data->hid_sensor_hubs[i]);
+			client_data->hid_sensor_hubs[i] = NULL;
+		}
+	}
+}
diff --git a/drivers/hid/intel-ish-hid/ishtp-hid.h b/drivers/hid/intel-ish-hid/ishtp-hid.h
new file mode 100644
index 000000000000..f5c7eb79b7b5
--- /dev/null
+++ b/drivers/hid/intel-ish-hid/ishtp-hid.h
@@ -0,0 +1,182 @@
+/*
+ * ISHTP-HID glue driver's definitions.
+ *
+ * Copyright (c) 2014-2016, 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 ISHTP_HID__H
+#define	ISHTP_HID__H
+
+/* The fixed ISH product and vendor id */
+#define	ISH_HID_VENDOR	0x8086
+#define	ISH_HID_PRODUCT	0x22D8
+#define	ISH_HID_VERSION	0x0200
+
+#define	CMD_MASK	0x7F
+#define	IS_RESPONSE	0x80
+
+/* Used to dump to Linux trace buffer, if enabled */
+#define hid_ishtp_trace(client, ...)	\
+	client->cl_device->ishtp_dev->print_log(\
+		client->cl_device->ishtp_dev, __VA_ARGS__)
+
+/* ISH Transport protocol (ISHTP in short) GUID */
+static const uuid_le hid_ishtp_guid = UUID_LE(0x33AECD58, 0xB679, 0x4E54,
+					      0x9B, 0xD9, 0xA0, 0x4D, 0x34,
+					      0xF0, 0xC2, 0x26);
+
+/* ISH HID message structure */
+struct hostif_msg_hdr {
+	uint8_t	command; /* Bit 7: is_response */
+	uint8_t	device_id;
+	uint8_t	status;
+	uint8_t	flags;
+	uint16_t size;
+} __packed;
+
+struct hostif_msg {
+	struct hostif_msg_hdr	hdr;
+} __packed;
+
+struct hostif_msg_to_sensor {
+	struct hostif_msg_hdr	hdr;
+	uint8_t	report_id;
+} __packed;
+
+struct device_info {
+	uint32_t dev_id;
+	uint8_t dev_class;
+	uint16_t pid;
+	uint16_t vid;
+} __packed;
+
+struct ishtp_version {
+	uint8_t	major;
+	uint8_t	minor;
+	uint8_t	hotfix;
+	uint16_t build;
+} __packed;
+
+/* struct for ISHTP aggregated input data */
+struct report_list {
+	uint16_t total_size;
+	uint8_t	num_of_reports;
+	uint8_t	flags;
+	struct {
+		uint16_t	size_of_report;
+		uint8_t report[1];
+	} __packed reports[1];
+} __packed;
+
+/* HOSTIF commands */
+#define	HOSTIF_HID_COMMAND_BASE			0
+#define	HOSTIF_GET_HID_DESCRIPTOR		0
+#define	HOSTIF_GET_REPORT_DESCRIPTOR		1
+#define HOSTIF_GET_FEATURE_REPORT		2
+#define	HOSTIF_SET_FEATURE_REPORT		3
+#define	HOSTIF_GET_INPUT_REPORT			4
+#define	HOSTIF_PUBLISH_INPUT_REPORT		5
+#define	HOSTIF_PUBLISH_INPUT_REPORT_LIST	6
+#define	HOSTIF_DM_COMMAND_BASE			32
+#define	HOSTIF_DM_ENUM_DEVICES			33
+#define	HOSTIF_DM_ADD_DEVICE			34
+
+#define	MAX_HID_DEVICES				32
+
+/**
+ * struct ishtp_cl_data - Encapsulate per ISH TP HID Client
+ * @enum_device_done:	Enum devices response complete flag
+ * @hid_descr_done:	HID descriptor complete flag
+ * @report_descr_done:	Get report descriptor complete flag
+ * @init_done:		Init process completed successfully
+ * @suspended:		System is under suspend state or in progress
+ * @num_hid_devices:	Number of HID devices enumerated in this client
+ * @cur_hid_dev:	This keeps track of the device index for which
+ *			initialization and registration with HID core
+ *			in progress.
+ * @hid_devices:	Store vid/pid/devid for each enumerated HID device
+ * @report_descr:	Stores the raw report descriptors for each HID device
+ * @report_descr_size:	Report description of size of above repo_descr[]
+ * @hid_sensor_hubs:	Pointer to hid_device for all HID device, so that
+ *			when clients are removed, they can be freed
+ * @hid_descr:		Pointer to hid descriptor for each enumerated hid
+ *			device
+ * @hid_descr_size:	Size of each above report descriptor
+ * @init_wait:		Wait queue to wait during initialization, where the
+ *			client send message to ISH FW and wait for response
+ * @ishtp_hid_wait:	The wait for get report during wait callback from hid
+ *			core
+ * @bad_recv_cnt:	Running count of packets received with error
+ * @multi_packet_cnt:	Count of fragmented packet count
+ *
+ * This structure is used to store completion flags and per client data like
+ * like report description, number of HID devices etc.
+ */
+struct ishtp_cl_data {
+	/* completion flags */
+	bool enum_devices_done;
+	bool hid_descr_done;
+	bool report_descr_done;
+	bool init_done;
+	bool suspended;
+
+	unsigned int num_hid_devices;
+	unsigned int cur_hid_dev;
+	unsigned int hid_dev_count;
+
+	struct device_info *hid_devices;
+	unsigned char *report_descr[MAX_HID_DEVICES];
+	int report_descr_size[MAX_HID_DEVICES];
+	struct hid_device *hid_sensor_hubs[MAX_HID_DEVICES];
+	unsigned char *hid_descr[MAX_HID_DEVICES];
+	int hid_descr_size[MAX_HID_DEVICES];
+
+	wait_queue_head_t init_wait;
+	wait_queue_head_t ishtp_resume_wait;
+	struct ishtp_cl *hid_ishtp_cl;
+
+	/* Statistics */
+	unsigned int bad_recv_cnt;
+	int multi_packet_cnt;
+
+	struct work_struct work;
+	struct ishtp_cl_device *cl_device;
+};
+
+/**
+ * struct ishtp_hid_data - Per instance HID data
+ * @index:		Device index in the order of enumeration
+ * @request_done:	Get Feature/Input report complete flag
+ *			used during get/set request from hid core
+ * @client_data:	Link to the client instance
+ * @hid_wait:		Completion waitq
+ *
+ * Used to tie hid hid->driver data to driver client instance
+ */
+struct ishtp_hid_data {
+	int index;
+	bool request_done;
+	struct ishtp_cl_data *client_data;
+	wait_queue_head_t hid_wait;
+};
+
+/* Interface functions between HID LL driver and ISH TP client */
+void hid_ishtp_set_feature(struct hid_device *hid, char *buf, unsigned int len,
+			   int report_id);
+void hid_ishtp_get_report(struct hid_device *hid, int report_id,
+			  int report_type);
+int ishtp_hid_probe(unsigned int cur_hid_dev,
+		    struct ishtp_cl_data *client_data);
+void ishtp_hid_remove(struct ishtp_cl_data *client_data);
+int ishtp_hid_link_ready_wait(struct ishtp_cl_data *client_data);
+void ishtp_hid_wakeup(struct hid_device *hid);
+
+#endif	/* ISHTP_HID__H */
diff --git a/drivers/hid/intel-ish-hid/ishtp/bus.c b/drivers/hid/intel-ish-hid/ishtp/bus.c
new file mode 100644
index 000000000000..256521509d20
--- /dev/null
+++ b/drivers/hid/intel-ish-hid/ishtp/bus.c
@@ -0,0 +1,788 @@
+/*
+ * ISHTP bus driver
+ *
+ * Copyright (c) 2012-2016, 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/module.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/device.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include "bus.h"
+#include "ishtp-dev.h"
+#include "client.h"
+#include "hbm.h"
+
+static int ishtp_use_dma;
+module_param_named(ishtp_use_dma, ishtp_use_dma, int, 0600);
+MODULE_PARM_DESC(ishtp_use_dma, "Use DMA to send messages");
+
+#define to_ishtp_cl_driver(d) container_of(d, struct ishtp_cl_driver, driver)
+#define to_ishtp_cl_device(d) container_of(d, struct ishtp_cl_device, dev)
+static bool ishtp_device_ready;
+
+/**
+ * ishtp_recv() - process ishtp message
+ * @dev: ishtp device
+ *
+ * If a message with valid header and size is received, then
+ * this function calls appropriate handler. The host or firmware
+ * address is zero, then they are host bus management message,
+ * otherwise they are message fo clients.
+ */
+void ishtp_recv(struct ishtp_device *dev)
+{
+	uint32_t	msg_hdr;
+	struct ishtp_msg_hdr	*ishtp_hdr;
+
+	/* Read ISHTP header dword */
+	msg_hdr = dev->ops->ishtp_read_hdr(dev);
+	if (!msg_hdr)
+		return;
+
+	dev->ops->sync_fw_clock(dev);
+
+	ishtp_hdr = (struct ishtp_msg_hdr *)&msg_hdr;
+	dev->ishtp_msg_hdr = msg_hdr;
+
+	/* Sanity check: ISHTP frag. length in header */
+	if (ishtp_hdr->length > dev->mtu) {
+		dev_err(dev->devc,
+			"ISHTP hdr - bad length: %u; dropped [%08X]\n",
+			(unsigned int)ishtp_hdr->length, msg_hdr);
+		return;
+	}
+
+	/* ISHTP bus message */
+	if (!ishtp_hdr->host_addr && !ishtp_hdr->fw_addr)
+		recv_hbm(dev, ishtp_hdr);
+	/* ISHTP fixed-client message */
+	else if (!ishtp_hdr->host_addr)
+		recv_fixed_cl_msg(dev, ishtp_hdr);
+	else
+		/* ISHTP client message */
+		recv_ishtp_cl_msg(dev, ishtp_hdr);
+}
+EXPORT_SYMBOL(ishtp_recv);
+
+/**
+ * ishtp_send_msg() - Send ishtp message
+ * @dev: ishtp device
+ * @hdr: Message header
+ * @msg: Message contents
+ * @ipc_send_compl: completion callback
+ * @ipc_send_compl_prm: completion callback parameter
+ *
+ * Send a multi fragment message via IPC. After sending the first fragment
+ * the completion callback is called to schedule transmit of next fragment.
+ *
+ * Return: This returns IPC send message status.
+ */
+int ishtp_send_msg(struct ishtp_device *dev, struct ishtp_msg_hdr *hdr,
+		       void *msg, void(*ipc_send_compl)(void *),
+		       void *ipc_send_compl_prm)
+{
+	unsigned char	ipc_msg[IPC_FULL_MSG_SIZE];
+	uint32_t	drbl_val;
+
+	drbl_val = dev->ops->ipc_get_header(dev, hdr->length +
+					    sizeof(struct ishtp_msg_hdr),
+					    1);
+
+	memcpy(ipc_msg, &drbl_val, sizeof(uint32_t));
+	memcpy(ipc_msg + sizeof(uint32_t), hdr, sizeof(uint32_t));
+	memcpy(ipc_msg + 2 * sizeof(uint32_t), msg, hdr->length);
+	return	dev->ops->write(dev, ipc_send_compl, ipc_send_compl_prm,
+				ipc_msg, 2 * sizeof(uint32_t) + hdr->length);
+}
+
+/**
+ * ishtp_write_message() - Send ishtp single fragment message
+ * @dev: ishtp device
+ * @hdr: Message header
+ * @buf: message data
+ *
+ * Send a single fragment message via IPC.  This returns IPC send message
+ * status.
+ *
+ * Return: This returns IPC send message status.
+ */
+int ishtp_write_message(struct ishtp_device *dev, struct ishtp_msg_hdr *hdr,
+			unsigned char *buf)
+{
+	return ishtp_send_msg(dev, hdr, buf, NULL, NULL);
+}
+
+/**
+ * ishtp_fw_cl_by_uuid() - locate index of fw client
+ * @dev: ishtp device
+ * @uuid: uuid of the client to search
+ *
+ * Search firmware client using UUID.
+ *
+ * Return: fw client index or -ENOENT if not found
+ */
+int ishtp_fw_cl_by_uuid(struct ishtp_device *dev, const uuid_le *uuid)
+{
+	int i, res = -ENOENT;
+
+	for (i = 0; i < dev->fw_clients_num; ++i) {
+		if (uuid_le_cmp(*uuid, dev->fw_clients[i].props.protocol_name)
+				== 0) {
+			res = i;
+			break;
+		}
+	}
+	return res;
+}
+EXPORT_SYMBOL(ishtp_fw_cl_by_uuid);
+
+/**
+ * ishtp_fw_cl_by_id() - return index to fw_clients for client_id
+ * @dev: the ishtp device structure
+ * @client_id: fw client id to search
+ *
+ * Search firmware client using client id.
+ *
+ * Return: index on success, -ENOENT on failure.
+ */
+int ishtp_fw_cl_by_id(struct ishtp_device *dev, uint8_t client_id)
+{
+	int i, res = -ENOENT;
+	unsigned long	flags;
+
+	spin_lock_irqsave(&dev->fw_clients_lock, flags);
+	for (i = 0; i < dev->fw_clients_num; i++) {
+		if (dev->fw_clients[i].client_id == client_id) {
+			res = i;
+			break;
+		}
+	}
+	spin_unlock_irqrestore(&dev->fw_clients_lock, flags);
+
+	return res;
+}
+
+/**
+ * ishtp_cl_device_probe() - Bus probe() callback
+ * @dev: the device structure
+ *
+ * This is a bus probe callback and calls the drive probe function.
+ *
+ * Return: Return value from driver probe() call.
+ */
+static int ishtp_cl_device_probe(struct device *dev)
+{
+	struct ishtp_cl_device *device = to_ishtp_cl_device(dev);
+	struct ishtp_cl_driver *driver;
+
+	if (!device)
+		return 0;
+
+	driver = to_ishtp_cl_driver(dev->driver);
+	if (!driver || !driver->probe)
+		return -ENODEV;
+
+	return driver->probe(device);
+}
+
+/**
+ * ishtp_cl_device_remove() - Bus remove() callback
+ * @dev: the device structure
+ *
+ * This is a bus remove callback and calls the drive remove function.
+ * Since the ISH driver model supports only built in, this is
+ * primarily can be called during pci driver init failure.
+ *
+ * Return: Return value from driver remove() call.
+ */
+static int ishtp_cl_device_remove(struct device *dev)
+{
+	struct ishtp_cl_device *device = to_ishtp_cl_device(dev);
+	struct ishtp_cl_driver *driver;
+
+	if (!device || !dev->driver)
+		return 0;
+
+	if (device->event_cb) {
+		device->event_cb = NULL;
+		cancel_work_sync(&device->event_work);
+	}
+
+	driver = to_ishtp_cl_driver(dev->driver);
+	if (!driver->remove) {
+		dev->driver = NULL;
+
+		return 0;
+	}
+
+	return driver->remove(device);
+}
+
+/**
+ * ishtp_cl_device_suspend() - Bus suspend callback
+ * @dev:	device
+ *
+ * Called during device suspend process.
+ *
+ * Return: Return value from driver suspend() call.
+ */
+static int ishtp_cl_device_suspend(struct device *dev)
+{
+	struct ishtp_cl_device *device = to_ishtp_cl_device(dev);
+	struct ishtp_cl_driver *driver;
+	int ret = 0;
+
+	if (!device)
+		return 0;
+
+	driver = to_ishtp_cl_driver(dev->driver);
+	if (driver && driver->driver.pm) {
+		if (driver->driver.pm->suspend)
+			ret = driver->driver.pm->suspend(dev);
+	}
+
+	return ret;
+}
+
+/**
+ * ishtp_cl_device_resume() - Bus resume callback
+ * @dev:	device
+ *
+ * Called during device resume process.
+ *
+ * Return: Return value from driver resume() call.
+ */
+static int ishtp_cl_device_resume(struct device *dev)
+{
+	struct ishtp_cl_device *device = to_ishtp_cl_device(dev);
+	struct ishtp_cl_driver *driver;
+	int ret = 0;
+
+	if (!device)
+		return 0;
+
+	/*
+	 * When ISH needs hard reset, it is done asynchrnously, hence bus
+	 * resume will  be called before full ISH resume
+	 */
+	if (device->ishtp_dev->resume_flag)
+		return 0;
+
+	driver = to_ishtp_cl_driver(dev->driver);
+	if (driver && driver->driver.pm) {
+		if (driver->driver.pm->resume)
+			ret = driver->driver.pm->resume(dev);
+	}
+
+	return ret;
+}
+
+/**
+ * ishtp_cl_device_reset() - Reset callback
+ * @device:	ishtp client device instance
+ *
+ * This is a callback when HW reset is done and the device need
+ * reinit.
+ *
+ * Return: Return value from driver reset() call.
+ */
+static int ishtp_cl_device_reset(struct ishtp_cl_device *device)
+{
+	struct ishtp_cl_driver *driver;
+	int ret = 0;
+
+	device->event_cb = NULL;
+	cancel_work_sync(&device->event_work);
+
+	driver = to_ishtp_cl_driver(device->dev.driver);
+	if (driver && driver->reset)
+		ret = driver->reset(device);
+
+	return ret;
+}
+
+static ssize_t modalias_show(struct device *dev, struct device_attribute *a,
+	char *buf)
+{
+	int len;
+
+	len = snprintf(buf, PAGE_SIZE, "ishtp:%s\n", dev_name(dev));
+	return (len >= PAGE_SIZE) ? (PAGE_SIZE - 1) : len;
+}
+
+static struct device_attribute ishtp_cl_dev_attrs[] = {
+	__ATTR_RO(modalias),
+	__ATTR_NULL,
+};
+
+static int ishtp_cl_uevent(struct device *dev, struct kobj_uevent_env *env)
+{
+	if (add_uevent_var(env, "MODALIAS=ishtp:%s", dev_name(dev)))
+		return -ENOMEM;
+	return 0;
+}
+
+static const struct dev_pm_ops ishtp_cl_bus_dev_pm_ops = {
+	/* Suspend callbacks */
+	.suspend = ishtp_cl_device_suspend,
+	.resume = ishtp_cl_device_resume,
+	/* Hibernate callbacks */
+	.freeze = ishtp_cl_device_suspend,
+	.thaw = ishtp_cl_device_resume,
+	.restore = ishtp_cl_device_resume,
+};
+
+static struct bus_type ishtp_cl_bus_type = {
+	.name		= "ishtp",
+	.dev_attrs	= ishtp_cl_dev_attrs,
+	.probe		= ishtp_cl_device_probe,
+	.remove		= ishtp_cl_device_remove,
+	.pm		= &ishtp_cl_bus_dev_pm_ops,
+	.uevent		= ishtp_cl_uevent,
+};
+
+static void ishtp_cl_dev_release(struct device *dev)
+{
+	kfree(to_ishtp_cl_device(dev));
+}
+
+static struct device_type ishtp_cl_device_type = {
+	.release	= ishtp_cl_dev_release,
+};
+
+/**
+ * ishtp_bus_add_device() - Function to create device on bus
+ * @dev:	ishtp device
+ * @uuid:	uuid of the client
+ * @name:	Name of the client
+ *
+ * Allocate ISHTP bus client device, attach it to uuid
+ * and register with ISHTP bus.
+ *
+ * Return: ishtp_cl_device pointer or NULL on failure
+ */
+static struct ishtp_cl_device *ishtp_bus_add_device(struct ishtp_device *dev,
+						    uuid_le uuid, char *name)
+{
+	struct ishtp_cl_device *device;
+	int status;
+	unsigned long flags;
+
+	spin_lock_irqsave(&dev->device_list_lock, flags);
+	list_for_each_entry(device, &dev->device_list, device_link) {
+		if (!strcmp(name, dev_name(&device->dev))) {
+			device->fw_client = &dev->fw_clients[
+				dev->fw_client_presentation_num - 1];
+			spin_unlock_irqrestore(&dev->device_list_lock, flags);
+			ishtp_cl_device_reset(device);
+			return device;
+		}
+	}
+	spin_unlock_irqrestore(&dev->device_list_lock, flags);
+
+	device = kzalloc(sizeof(struct ishtp_cl_device), GFP_KERNEL);
+	if (!device)
+		return NULL;
+
+	device->dev.parent = dev->devc;
+	device->dev.bus = &ishtp_cl_bus_type;
+	device->dev.type = &ishtp_cl_device_type;
+	device->ishtp_dev = dev;
+
+	device->fw_client =
+		&dev->fw_clients[dev->fw_client_presentation_num - 1];
+
+	dev_set_name(&device->dev, "%s", name);
+
+	spin_lock_irqsave(&dev->device_list_lock, flags);
+	list_add_tail(&device->device_link, &dev->device_list);
+	spin_unlock_irqrestore(&dev->device_list_lock, flags);
+
+	status = device_register(&device->dev);
+	if (status) {
+		spin_lock_irqsave(&dev->device_list_lock, flags);
+		list_del(&device->device_link);
+		spin_unlock_irqrestore(&dev->device_list_lock, flags);
+		dev_err(dev->devc, "Failed to register ISHTP client device\n");
+		kfree(device);
+		return NULL;
+	}
+
+	ishtp_device_ready = true;
+
+	return device;
+}
+
+/**
+ * ishtp_bus_remove_device() - Function to relase device on bus
+ * @device:	client device instance
+ *
+ * This is a counterpart of ishtp_bus_add_device.
+ * Device is unregistered.
+ * the device structure is freed in 'ishtp_cl_dev_release' function
+ * Called only during error in pci driver init path.
+ */
+static void ishtp_bus_remove_device(struct ishtp_cl_device *device)
+{
+	device_unregister(&device->dev);
+}
+
+/**
+ * __ishtp_cl_driver_register() - Client driver register
+ * @driver:	the client driver instance
+ * @owner:	Owner of this driver module
+ *
+ * Once a client driver is probed, it created a client
+ * instance and registers with the bus.
+ *
+ * Return: Return value of driver_register or -ENODEV if not ready
+ */
+int __ishtp_cl_driver_register(struct ishtp_cl_driver *driver,
+			       struct module *owner)
+{
+	int err;
+
+	if (!ishtp_device_ready)
+		return -ENODEV;
+
+	driver->driver.name = driver->name;
+	driver->driver.owner = owner;
+	driver->driver.bus = &ishtp_cl_bus_type;
+
+	err = driver_register(&driver->driver);
+	if (err)
+		return err;
+
+	return 0;
+}
+EXPORT_SYMBOL(__ishtp_cl_driver_register);
+
+/**
+ * ishtp_cl_driver_unregister() - Client driver unregister
+ * @driver:	the client driver instance
+ *
+ * Unregister client during device removal process.
+ */
+void ishtp_cl_driver_unregister(struct ishtp_cl_driver *driver)
+{
+	driver_unregister(&driver->driver);
+}
+EXPORT_SYMBOL(ishtp_cl_driver_unregister);
+
+/**
+ * ishtp_bus_event_work() - event work function
+ * @work:	work struct pointer
+ *
+ * Once an event is received for a client this work
+ * function is called. If the device has registered a
+ * callback then the callback is called.
+ */
+static void ishtp_bus_event_work(struct work_struct *work)
+{
+	struct ishtp_cl_device *device;
+
+	device = container_of(work, struct ishtp_cl_device, event_work);
+
+	if (device->event_cb)
+		device->event_cb(device);
+}
+
+/**
+ * ishtp_cl_bus_rx_event() - schedule event work
+ * @device:	client device instance
+ *
+ * Once an event is received for a client this schedules
+ * a work function to process.
+ */
+void ishtp_cl_bus_rx_event(struct ishtp_cl_device *device)
+{
+	if (!device || !device->event_cb)
+		return;
+
+	if (device->event_cb)
+		schedule_work(&device->event_work);
+}
+
+/**
+ * ishtp_register_event_cb() - Register callback
+ * @device:	client device instance
+ * @event_cb:	Event processor for an client
+ *
+ * Register a callback for events, called from client driver
+ *
+ * Return: Return 0 or -EALREADY if already registered
+ */
+int ishtp_register_event_cb(struct ishtp_cl_device *device,
+	void (*event_cb)(struct ishtp_cl_device *))
+{
+	if (device->event_cb)
+		return -EALREADY;
+
+	device->event_cb = event_cb;
+	INIT_WORK(&device->event_work, ishtp_bus_event_work);
+
+	return 0;
+}
+EXPORT_SYMBOL(ishtp_register_event_cb);
+
+/**
+ * ishtp_get_device() - update usage count for the device
+ * @cl_device:	client device instance
+ *
+ * Increment the usage count. The device can't be deleted
+ */
+void ishtp_get_device(struct ishtp_cl_device *cl_device)
+{
+	cl_device->reference_count++;
+}
+EXPORT_SYMBOL(ishtp_get_device);
+
+/**
+ * ishtp_put_device() - decrement usage count for the device
+ * @cl_device:	client device instance
+ *
+ * Decrement the usage count. The device can be deleted is count = 0
+ */
+void ishtp_put_device(struct ishtp_cl_device *cl_device)
+{
+	cl_device->reference_count--;
+}
+EXPORT_SYMBOL(ishtp_put_device);
+
+/**
+ * ishtp_bus_new_client() - Create a new client
+ * @dev:	ISHTP device instance
+ *
+ * Once bus protocol enumerates a client, this is called
+ * to add a device for the client.
+ *
+ * Return: 0 on success or error code on failure
+ */
+int ishtp_bus_new_client(struct ishtp_device *dev)
+{
+	int	i;
+	char	*dev_name;
+	struct ishtp_cl_device	*cl_device;
+	uuid_le	device_uuid;
+
+	/*
+	 * For all reported clients, create an unconnected client and add its
+	 * device to ISHTP bus.
+	 * If appropriate driver has loaded, this will trigger its probe().
+	 * Otherwise, probe() will be called when driver is loaded
+	 */
+	i = dev->fw_client_presentation_num - 1;
+	device_uuid = dev->fw_clients[i].props.protocol_name;
+	dev_name = kasprintf(GFP_KERNEL,
+		"{%02X%02X%02X%02X-%02X%02X-%02X%02X-%02X%02X-%02X%02X%02X%02X%02X%02X}",
+		device_uuid.b[3], device_uuid.b[2], device_uuid.b[1],
+		device_uuid.b[0], device_uuid.b[5], device_uuid.b[4],
+		device_uuid.b[7], device_uuid.b[6], device_uuid.b[8],
+		device_uuid.b[9], device_uuid.b[10], device_uuid.b[11],
+		device_uuid.b[12], device_uuid.b[13], device_uuid.b[14],
+		device_uuid.b[15]);
+	if (!dev_name)
+		return	-ENOMEM;
+
+	cl_device = ishtp_bus_add_device(dev, device_uuid, dev_name);
+	if (!cl_device) {
+		kfree(dev_name);
+		return	-ENOENT;
+	}
+
+	kfree(dev_name);
+
+	return	0;
+}
+
+/**
+ * ishtp_cl_device_bind() - bind a device
+ * @cl:		ishtp client device
+ *
+ * Binds connected ishtp_cl to ISHTP bus device
+ *
+ * Return: 0 on success or fault code
+ */
+int ishtp_cl_device_bind(struct ishtp_cl *cl)
+{
+	struct ishtp_cl_device	*cl_device;
+	unsigned long flags;
+	int	rv;
+
+	if (!cl->fw_client_id || cl->state != ISHTP_CL_CONNECTED)
+		return	-EFAULT;
+
+	rv = -ENOENT;
+	spin_lock_irqsave(&cl->dev->device_list_lock, flags);
+	list_for_each_entry(cl_device, &cl->dev->device_list,
+			device_link) {
+		if (cl_device->fw_client->client_id == cl->fw_client_id) {
+			cl->device = cl_device;
+			rv = 0;
+			break;
+		}
+	}
+	spin_unlock_irqrestore(&cl->dev->device_list_lock, flags);
+	return	rv;
+}
+
+/**
+ * ishtp_bus_remove_all_clients() - Remove all clients
+ * @ishtp_dev:		ishtp device
+ * @warm_reset:		Reset due to FW reset dure to errors or S3 suspend
+ *
+ * This is part of reset/remove flow. This function the main processing
+ * only targets error processing, if the FW has forced reset or
+ * error to remove connected clients. When warm reset the client devices are
+ * not removed.
+ */
+void ishtp_bus_remove_all_clients(struct ishtp_device *ishtp_dev,
+				  bool warm_reset)
+{
+	struct ishtp_cl_device	*cl_device, *n;
+	struct ishtp_cl	*cl;
+	unsigned long	flags;
+
+	spin_lock_irqsave(&ishtp_dev->cl_list_lock, flags);
+	list_for_each_entry(cl, &ishtp_dev->cl_list, link) {
+		cl->state = ISHTP_CL_DISCONNECTED;
+
+		/*
+		 * Wake any pending process. The waiter would check dev->state
+		 * and determine that it's not enabled already,
+		 * and will return error to its caller
+		 */
+		wake_up_interruptible(&cl->wait_ctrl_res);
+
+		/* Disband any pending read/write requests and free rb */
+		ishtp_cl_flush_queues(cl);
+
+		/* Remove all free and in_process rings, both Rx and Tx */
+		ishtp_cl_free_rx_ring(cl);
+		ishtp_cl_free_tx_ring(cl);
+
+		/*
+		 * Free client and ISHTP bus client device structures
+		 * don't free host client because it is part of the OS fd
+		 * structure
+		 */
+	}
+	spin_unlock_irqrestore(&ishtp_dev->cl_list_lock, flags);
+
+	/* Release DMA buffers for client messages */
+	ishtp_cl_free_dma_buf(ishtp_dev);
+
+	/* remove bus clients */
+	spin_lock_irqsave(&ishtp_dev->device_list_lock, flags);
+	list_for_each_entry_safe(cl_device, n, &ishtp_dev->device_list,
+				 device_link) {
+		if (warm_reset && cl_device->reference_count)
+			continue;
+
+		list_del(&cl_device->device_link);
+		spin_unlock_irqrestore(&ishtp_dev->device_list_lock, flags);
+		ishtp_bus_remove_device(cl_device);
+		spin_lock_irqsave(&ishtp_dev->device_list_lock, flags);
+	}
+	spin_unlock_irqrestore(&ishtp_dev->device_list_lock, flags);
+
+	/* Free all client structures */
+	spin_lock_irqsave(&ishtp_dev->fw_clients_lock, flags);
+	kfree(ishtp_dev->fw_clients);
+	ishtp_dev->fw_clients = NULL;
+	ishtp_dev->fw_clients_num = 0;
+	ishtp_dev->fw_client_presentation_num = 0;
+	ishtp_dev->fw_client_index = 0;
+	bitmap_zero(ishtp_dev->fw_clients_map, ISHTP_CLIENTS_MAX);
+	spin_unlock_irqrestore(&ishtp_dev->fw_clients_lock, flags);
+}
+EXPORT_SYMBOL(ishtp_bus_remove_all_clients);
+
+/**
+ * ishtp_reset_handler() - IPC reset handler
+ * @dev:	ishtp device
+ *
+ * ISHTP Handler for IPC_RESET notification
+ */
+void ishtp_reset_handler(struct ishtp_device *dev)
+{
+	unsigned long	flags;
+
+	/* Handle FW-initiated reset */
+	dev->dev_state = ISHTP_DEV_RESETTING;
+
+	/* Clear BH processing queue - no further HBMs */
+	spin_lock_irqsave(&dev->rd_msg_spinlock, flags);
+	dev->rd_msg_fifo_head = dev->rd_msg_fifo_tail = 0;
+	spin_unlock_irqrestore(&dev->rd_msg_spinlock, flags);
+
+	/* Handle ISH FW reset against upper layers */
+	ishtp_bus_remove_all_clients(dev, true);
+}
+EXPORT_SYMBOL(ishtp_reset_handler);
+
+/**
+ * ishtp_reset_compl_handler() - Reset completion handler
+ * @dev:	ishtp device
+ *
+ * ISHTP handler for IPC_RESET sequence completion to start
+ * host message bus start protocol sequence.
+ */
+void ishtp_reset_compl_handler(struct ishtp_device *dev)
+{
+	dev->dev_state = ISHTP_DEV_INIT_CLIENTS;
+	dev->hbm_state = ISHTP_HBM_START;
+	ishtp_hbm_start_req(dev);
+}
+EXPORT_SYMBOL(ishtp_reset_compl_handler);
+
+/**
+ * ishtp_use_dma_transfer() - Function to use DMA
+ *
+ * This interface is used to enable usage of DMA
+ *
+ * Return non zero if DMA can be enabled
+ */
+int ishtp_use_dma_transfer(void)
+{
+	return ishtp_use_dma;
+}
+
+/**
+ * ishtp_bus_register() - Function to register bus
+ *
+ * This register ishtp bus
+ *
+ * Return: Return output of bus_register
+ */
+static int  __init ishtp_bus_register(void)
+{
+	return bus_register(&ishtp_cl_bus_type);
+}
+
+/**
+ * ishtp_bus_unregister() - Function to unregister bus
+ *
+ * This unregister ishtp bus
+ */
+static void __exit ishtp_bus_unregister(void)
+{
+	bus_unregister(&ishtp_cl_bus_type);
+}
+
+module_init(ishtp_bus_register);
+module_exit(ishtp_bus_unregister);
+
+MODULE_LICENSE("GPL");
diff --git a/drivers/hid/intel-ish-hid/ishtp/bus.h b/drivers/hid/intel-ish-hid/ishtp/bus.h
new file mode 100644
index 000000000000..a1ffae7f26ad
--- /dev/null
+++ b/drivers/hid/intel-ish-hid/ishtp/bus.h
@@ -0,0 +1,114 @@
+/*
+ * ISHTP bus definitions
+ *
+ * Copyright (c) 2014-2016, 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_ISHTP_CL_BUS_H
+#define _LINUX_ISHTP_CL_BUS_H
+
+#include <linux/device.h>
+#include <linux/mod_devicetable.h>
+
+struct ishtp_cl;
+struct ishtp_cl_device;
+struct ishtp_device;
+struct ishtp_msg_hdr;
+
+/**
+ * struct ishtp_cl_device - ISHTP device handle
+ * @dev:	device pointer
+ * @ishtp_dev:	pointer to ishtp device structure to primarily to access
+ *		hw device operation callbacks and properties
+ * @fw_client:	fw_client pointer to get fw information like protocol name
+ *		max message length etc.
+ * @device_link: Link to next client in the list on a bus
+ * @event_work:	Used to schedule rx event for client
+ * @driver_data: Storage driver private data
+ * @reference_count:	Used for get/put device
+ * @event_cb:	Callback to driver to send events
+ *
+ * An ishtp_cl_device pointer is returned from ishtp_add_device()
+ * and links ISHTP bus clients to their actual host client pointer.
+ * Drivers for ISHTP devices will get an ishtp_cl_device pointer
+ * when being probed and shall use it for doing bus I/O.
+ */
+struct ishtp_cl_device {
+	struct device		dev;
+	struct ishtp_device	*ishtp_dev;
+	struct ishtp_fw_client	*fw_client;
+	struct list_head	device_link;
+	struct work_struct	event_work;
+	void			*driver_data;
+	int			reference_count;
+	void (*event_cb)(struct ishtp_cl_device *device);
+};
+
+/**
+ * struct ishtp_cl_device - ISHTP device handle
+ * @driver:	driver instance on a bus
+ * @name:	Name of the device for probe
+ * @probe:	driver callback for device probe
+ * @remove:	driver callback on device removal
+ *
+ * Client drivers defines to get probed/removed for ISHTP client device.
+ */
+struct ishtp_cl_driver {
+	struct device_driver driver;
+	const char *name;
+	int (*probe)(struct ishtp_cl_device *dev);
+	int (*remove)(struct ishtp_cl_device *dev);
+	int (*reset)(struct ishtp_cl_device *dev);
+	const struct dev_pm_ops *pm;
+};
+
+
+int	ishtp_bus_new_client(struct ishtp_device *dev);
+void	ishtp_remove_all_clients(struct ishtp_device *dev);
+int	ishtp_cl_device_bind(struct ishtp_cl *cl);
+void	ishtp_cl_bus_rx_event(struct ishtp_cl_device *device);
+
+/* Write a multi-fragment message */
+int	ishtp_send_msg(struct ishtp_device *dev,
+		       struct ishtp_msg_hdr *hdr, void *msg,
+		       void (*ipc_send_compl)(void *),
+		       void *ipc_send_compl_prm);
+
+/* Write a single-fragment message */
+int	ishtp_write_message(struct ishtp_device *dev,
+			    struct ishtp_msg_hdr *hdr,
+			    unsigned char *buf);
+
+/* Use DMA to send/receive messages */
+int ishtp_use_dma_transfer(void);
+
+/* Exported functions */
+void	ishtp_bus_remove_all_clients(struct ishtp_device *ishtp_dev,
+				     bool warm_reset);
+
+void	ishtp_recv(struct ishtp_device *dev);
+void	ishtp_reset_handler(struct ishtp_device *dev);
+void	ishtp_reset_compl_handler(struct ishtp_device *dev);
+
+void	ishtp_put_device(struct ishtp_cl_device *);
+void	ishtp_get_device(struct ishtp_cl_device *);
+
+int	__ishtp_cl_driver_register(struct ishtp_cl_driver *driver,
+				   struct module *owner);
+#define ishtp_cl_driver_register(driver)		\
+	__ishtp_cl_driver_register(driver, THIS_MODULE)
+void	ishtp_cl_driver_unregister(struct ishtp_cl_driver *driver);
+
+int	ishtp_register_event_cb(struct ishtp_cl_device *device,
+				void (*read_cb)(struct ishtp_cl_device *));
+int	ishtp_fw_cl_by_uuid(struct ishtp_device *dev, const uuid_le *cuuid);
+
+#endif /* _LINUX_ISHTP_CL_BUS_H */
diff --git a/drivers/hid/intel-ish-hid/ishtp/client-buffers.c b/drivers/hid/intel-ish-hid/ishtp/client-buffers.c
new file mode 100644
index 000000000000..b9b917d2d50d
--- /dev/null
+++ b/drivers/hid/intel-ish-hid/ishtp/client-buffers.c
@@ -0,0 +1,257 @@
+/*
+ * ISHTP Ring Buffers
+ *
+ * Copyright (c) 2003-2016, 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/slab.h>
+#include "client.h"
+
+/**
+ * ishtp_cl_alloc_rx_ring() - Allocate RX ring buffers
+ * @cl: client device instance
+ *
+ * Allocate and initialize RX ring buffers
+ *
+ * Return: 0 on success else -ENOMEM
+ */
+int ishtp_cl_alloc_rx_ring(struct ishtp_cl *cl)
+{
+	size_t	len = cl->device->fw_client->props.max_msg_length;
+	int	j;
+	struct ishtp_cl_rb *rb;
+	int	ret = 0;
+	unsigned long	flags;
+
+	for (j = 0; j < cl->rx_ring_size; ++j) {
+		rb = ishtp_io_rb_init(cl);
+		if (!rb) {
+			ret = -ENOMEM;
+			goto out;
+		}
+		ret = ishtp_io_rb_alloc_buf(rb, len);
+		if (ret)
+			goto out;
+		spin_lock_irqsave(&cl->free_list_spinlock, flags);
+		list_add_tail(&rb->list, &cl->free_rb_list.list);
+		spin_unlock_irqrestore(&cl->free_list_spinlock, flags);
+	}
+
+	return	0;
+
+out:
+	dev_err(&cl->device->dev, "error in allocating Rx buffers\n");
+	ishtp_cl_free_rx_ring(cl);
+	return	ret;
+}
+
+/**
+ * ishtp_cl_alloc_tx_ring() - Allocate TX ring buffers
+ * @cl: client device instance
+ *
+ * Allocate and initialize TX ring buffers
+ *
+ * Return: 0 on success else -ENOMEM
+ */
+int ishtp_cl_alloc_tx_ring(struct ishtp_cl *cl)
+{
+	size_t	len = cl->device->fw_client->props.max_msg_length;
+	int	j;
+	unsigned long	flags;
+
+	/* Allocate pool to free Tx bufs */
+	for (j = 0; j < cl->tx_ring_size; ++j) {
+		struct ishtp_cl_tx_ring	*tx_buf;
+
+		tx_buf = kzalloc(sizeof(struct ishtp_cl_tx_ring), GFP_KERNEL);
+		if (!tx_buf)
+			goto	out;
+
+		tx_buf->send_buf.data = kmalloc(len, GFP_KERNEL);
+		if (!tx_buf->send_buf.data) {
+			kfree(tx_buf);
+			goto	out;
+		}
+
+		spin_lock_irqsave(&cl->tx_free_list_spinlock, flags);
+		list_add_tail(&tx_buf->list, &cl->tx_free_list.list);
+		spin_unlock_irqrestore(&cl->tx_free_list_spinlock, flags);
+	}
+	return	0;
+out:
+	dev_err(&cl->device->dev, "error in allocating Tx pool\n");
+	ishtp_cl_free_rx_ring(cl);
+	return	-ENOMEM;
+}
+
+/**
+ * ishtp_cl_free_rx_ring() - Free RX ring buffers
+ * @cl: client device instance
+ *
+ * Free RX ring buffers
+ */
+void ishtp_cl_free_rx_ring(struct ishtp_cl *cl)
+{
+	struct ishtp_cl_rb *rb;
+	unsigned long	flags;
+
+	/* release allocated memory - pass over free_rb_list */
+	spin_lock_irqsave(&cl->free_list_spinlock, flags);
+	while (!list_empty(&cl->free_rb_list.list)) {
+		rb = list_entry(cl->free_rb_list.list.next, struct ishtp_cl_rb,
+				list);
+		list_del(&rb->list);
+		kfree(rb->buffer.data);
+		kfree(rb);
+	}
+	spin_unlock_irqrestore(&cl->free_list_spinlock, flags);
+	/* release allocated memory - pass over in_process_list */
+	spin_lock_irqsave(&cl->in_process_spinlock, flags);
+	while (!list_empty(&cl->in_process_list.list)) {
+		rb = list_entry(cl->in_process_list.list.next,
+				struct ishtp_cl_rb, list);
+		list_del(&rb->list);
+		kfree(rb->buffer.data);
+		kfree(rb);
+	}
+	spin_unlock_irqrestore(&cl->in_process_spinlock, flags);
+}
+
+/**
+ * ishtp_cl_free_tx_ring() - Free TX ring buffers
+ * @cl: client device instance
+ *
+ * Free TX ring buffers
+ */
+void ishtp_cl_free_tx_ring(struct ishtp_cl *cl)
+{
+	struct ishtp_cl_tx_ring	*tx_buf;
+	unsigned long	flags;
+
+	spin_lock_irqsave(&cl->tx_free_list_spinlock, flags);
+	/* release allocated memory - pass over tx_free_list */
+	while (!list_empty(&cl->tx_free_list.list)) {
+		tx_buf = list_entry(cl->tx_free_list.list.next,
+				    struct ishtp_cl_tx_ring, list);
+		list_del(&tx_buf->list);
+		kfree(tx_buf->send_buf.data);
+		kfree(tx_buf);
+	}
+	spin_unlock_irqrestore(&cl->tx_free_list_spinlock, flags);
+
+	spin_lock_irqsave(&cl->tx_list_spinlock, flags);
+	/* release allocated memory - pass over tx_list */
+	while (!list_empty(&cl->tx_list.list)) {
+		tx_buf = list_entry(cl->tx_list.list.next,
+				    struct ishtp_cl_tx_ring, list);
+		list_del(&tx_buf->list);
+		kfree(tx_buf->send_buf.data);
+		kfree(tx_buf);
+	}
+	spin_unlock_irqrestore(&cl->tx_list_spinlock, flags);
+}
+
+/**
+ * ishtp_io_rb_free() - Free IO request block
+ * @rb: IO request block
+ *
+ * Free io request block memory
+ */
+void ishtp_io_rb_free(struct ishtp_cl_rb *rb)
+{
+	if (rb == NULL)
+		return;
+
+	kfree(rb->buffer.data);
+	kfree(rb);
+}
+
+/**
+ * ishtp_io_rb_init() - Allocate and init IO request block
+ * @cl: client device instance
+ *
+ * Allocate and initialize request block
+ *
+ * Return: Allocted IO request block pointer
+ */
+struct ishtp_cl_rb *ishtp_io_rb_init(struct ishtp_cl *cl)
+{
+	struct ishtp_cl_rb *rb;
+
+	rb = kzalloc(sizeof(struct ishtp_cl_rb), GFP_KERNEL);
+	if (!rb)
+		return NULL;
+
+	INIT_LIST_HEAD(&rb->list);
+	rb->cl = cl;
+	rb->buf_idx = 0;
+	return rb;
+}
+
+/**
+ * ishtp_io_rb_alloc_buf() - Allocate and init response buffer
+ * @rb: IO request block
+ * @length: length of response buffer
+ *
+ * Allocate respose buffer
+ *
+ * Return: 0 on success else -ENOMEM
+ */
+int ishtp_io_rb_alloc_buf(struct ishtp_cl_rb *rb, size_t length)
+{
+	if (!rb)
+		return -EINVAL;
+
+	if (length == 0)
+		return 0;
+
+	rb->buffer.data = kmalloc(length, GFP_KERNEL);
+	if (!rb->buffer.data)
+		return -ENOMEM;
+
+	rb->buffer.size = length;
+	return 0;
+}
+
+/**
+ * ishtp_cl_io_rb_recycle() - Recycle IO request blocks
+ * @rb: IO request block
+ *
+ * Re-append rb to its client's free list and send flow control if needed
+ *
+ * Return: 0 on success else -EFAULT
+ */
+int ishtp_cl_io_rb_recycle(struct ishtp_cl_rb *rb)
+{
+	struct ishtp_cl *cl;
+	int	rets = 0;
+	unsigned long	flags;
+
+	if (!rb || !rb->cl)
+		return	-EFAULT;
+
+	cl = rb->cl;
+	spin_lock_irqsave(&cl->free_list_spinlock, flags);
+	list_add_tail(&rb->list, &cl->free_rb_list.list);
+	spin_unlock_irqrestore(&cl->free_list_spinlock, flags);
+
+	/*
+	 * If we returned the first buffer to empty 'free' list,
+	 * send flow control
+	 */
+	if (!cl->out_flow_ctrl_creds)
+		rets = ishtp_cl_read_start(cl);
+
+	return	rets;
+}
+EXPORT_SYMBOL(ishtp_cl_io_rb_recycle);
diff --git a/drivers/hid/intel-ish-hid/ishtp/client.c b/drivers/hid/intel-ish-hid/ishtp/client.c
new file mode 100644
index 000000000000..aad61328f282
--- /dev/null
+++ b/drivers/hid/intel-ish-hid/ishtp/client.c
@@ -0,0 +1,1054 @@
+/*
+ * ISHTP client logic
+ *
+ * Copyright (c) 2003-2016, 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/slab.h>
+#include <linux/sched.h>
+#include <linux/wait.h>
+#include <linux/delay.h>
+#include <linux/dma-mapping.h>
+#include "hbm.h"
+#include "client.h"
+
+/**
+ * ishtp_read_list_flush() - Flush read queue
+ * @cl: ishtp client instance
+ *
+ * Used to remove all entries from read queue for a client
+ */
+static void ishtp_read_list_flush(struct ishtp_cl *cl)
+{
+	struct ishtp_cl_rb *rb;
+	struct ishtp_cl_rb *next;
+	unsigned long	flags;
+
+	spin_lock_irqsave(&cl->dev->read_list_spinlock, flags);
+	list_for_each_entry_safe(rb, next, &cl->dev->read_list.list, list)
+		if (rb->cl && ishtp_cl_cmp_id(cl, rb->cl)) {
+			list_del(&rb->list);
+			ishtp_io_rb_free(rb);
+		}
+	spin_unlock_irqrestore(&cl->dev->read_list_spinlock, flags);
+}
+
+/**
+ * ishtp_cl_flush_queues() - Flush all queues for a client
+ * @cl: ishtp client instance
+ *
+ * Used to remove all queues for a client. This is called when a client device
+ * needs reset due to error, S3 resume or during module removal
+ *
+ * Return: 0 on success else -EINVAL if device is NULL
+ */
+int ishtp_cl_flush_queues(struct ishtp_cl *cl)
+{
+	if (WARN_ON(!cl || !cl->dev))
+		return -EINVAL;
+
+	ishtp_read_list_flush(cl);
+
+	return 0;
+}
+EXPORT_SYMBOL(ishtp_cl_flush_queues);
+
+/**
+ * ishtp_cl_init() - Initialize all fields of a client device
+ * @cl: ishtp client instance
+ * @dev: ishtp device
+ *
+ * Initializes a client device fields: Init spinlocks, init queues etc.
+ * This function is called during new client creation
+ */
+static void ishtp_cl_init(struct ishtp_cl *cl, struct ishtp_device *dev)
+{
+	memset(cl, 0, sizeof(struct ishtp_cl));
+	init_waitqueue_head(&cl->wait_ctrl_res);
+	spin_lock_init(&cl->free_list_spinlock);
+	spin_lock_init(&cl->in_process_spinlock);
+	spin_lock_init(&cl->tx_list_spinlock);
+	spin_lock_init(&cl->tx_free_list_spinlock);
+	spin_lock_init(&cl->fc_spinlock);
+	INIT_LIST_HEAD(&cl->link);
+	cl->dev = dev;
+
+	INIT_LIST_HEAD(&cl->free_rb_list.list);
+	INIT_LIST_HEAD(&cl->tx_list.list);
+	INIT_LIST_HEAD(&cl->tx_free_list.list);
+	INIT_LIST_HEAD(&cl->in_process_list.list);
+
+	cl->rx_ring_size = CL_DEF_RX_RING_SIZE;
+	cl->tx_ring_size = CL_DEF_TX_RING_SIZE;
+
+	/* dma */
+	cl->last_tx_path = CL_TX_PATH_IPC;
+	cl->last_dma_acked = 1;
+	cl->last_dma_addr = NULL;
+	cl->last_ipc_acked = 1;
+}
+
+/**
+ * ishtp_cl_allocate() - allocates client structure and sets it up.
+ * @dev: ishtp device
+ *
+ * Allocate memory for new client device and call to initialize each field.
+ *
+ * Return: The allocated client instance or NULL on failure
+ */
+struct ishtp_cl *ishtp_cl_allocate(struct ishtp_device *dev)
+{
+	struct ishtp_cl *cl;
+
+	cl = kmalloc(sizeof(struct ishtp_cl), GFP_KERNEL);
+	if (!cl)
+		return NULL;
+
+	ishtp_cl_init(cl, dev);
+	return cl;
+}
+EXPORT_SYMBOL(ishtp_cl_allocate);
+
+/**
+ * ishtp_cl_free() - Frees a client device
+ * @cl: client device instance
+ *
+ * Frees a client device
+ */
+void	ishtp_cl_free(struct ishtp_cl *cl)
+{
+	struct ishtp_device *dev;
+	unsigned long flags;
+
+	if (!cl)
+		return;
+
+	dev = cl->dev;
+	if (!dev)
+		return;
+
+	spin_lock_irqsave(&dev->cl_list_lock, flags);
+	ishtp_cl_free_rx_ring(cl);
+	ishtp_cl_free_tx_ring(cl);
+	kfree(cl);
+	spin_unlock_irqrestore(&dev->cl_list_lock, flags);
+}
+EXPORT_SYMBOL(ishtp_cl_free);
+
+/**
+ * ishtp_cl_link() - Reserve a host id and link the client instance
+ * @cl: client device instance
+ * @id: host client id to use. It can be ISHTP_HOST_CLIENT_ID_ANY if any
+ *	id from the available can be used
+ *
+ *
+ * This allocates a single bit in the hostmap. This function will make sure
+ * that not many client sessions are opened at the same time. Once allocated
+ * the client device instance is added to the ishtp device in the current
+ * client list
+ *
+ * Return: 0 or error code on failure
+ */
+int ishtp_cl_link(struct ishtp_cl *cl, int id)
+{
+	struct ishtp_device *dev;
+	unsigned long	flags, flags_cl;
+	int	ret = 0;
+
+	if (WARN_ON(!cl || !cl->dev))
+		return -EINVAL;
+
+	dev = cl->dev;
+
+	spin_lock_irqsave(&dev->device_lock, flags);
+
+	if (dev->open_handle_count >= ISHTP_MAX_OPEN_HANDLE_COUNT) {
+		ret = -EMFILE;
+		goto unlock_dev;
+	}
+
+	/* If Id is not assigned get one*/
+	if (id == ISHTP_HOST_CLIENT_ID_ANY)
+		id = find_first_zero_bit(dev->host_clients_map,
+			ISHTP_CLIENTS_MAX);
+
+	if (id >= ISHTP_CLIENTS_MAX) {
+		spin_unlock_irqrestore(&dev->device_lock, flags);
+		dev_err(&cl->device->dev, "id exceeded %d", ISHTP_CLIENTS_MAX);
+		return -ENOENT;
+	}
+
+	dev->open_handle_count++;
+	cl->host_client_id = id;
+	spin_lock_irqsave(&dev->cl_list_lock, flags_cl);
+	if (dev->dev_state != ISHTP_DEV_ENABLED) {
+		ret = -ENODEV;
+		goto unlock_cl;
+	}
+	list_add_tail(&cl->link, &dev->cl_list);
+	set_bit(id, dev->host_clients_map);
+	cl->state = ISHTP_CL_INITIALIZING;
+
+unlock_cl:
+	spin_unlock_irqrestore(&dev->cl_list_lock, flags_cl);
+unlock_dev:
+	spin_unlock_irqrestore(&dev->device_lock, flags);
+	return ret;
+}
+EXPORT_SYMBOL(ishtp_cl_link);
+
+/**
+ * ishtp_cl_unlink() - remove fw_cl from the client device list
+ * @cl: client device instance
+ *
+ * Remove a previously linked device to a ishtp device
+ */
+void ishtp_cl_unlink(struct ishtp_cl *cl)
+{
+	struct ishtp_device *dev;
+	struct ishtp_cl *pos;
+	unsigned long	flags;
+
+	/* don't shout on error exit path */
+	if (!cl || !cl->dev)
+		return;
+
+	dev = cl->dev;
+
+	spin_lock_irqsave(&dev->device_lock, flags);
+	if (dev->open_handle_count > 0) {
+		clear_bit(cl->host_client_id, dev->host_clients_map);
+		dev->open_handle_count--;
+	}
+	spin_unlock_irqrestore(&dev->device_lock, flags);
+
+	/*
+	 * This checks that 'cl' is actually linked into device's structure,
+	 * before attempting 'list_del'
+	 */
+	spin_lock_irqsave(&dev->cl_list_lock, flags);
+	list_for_each_entry(pos, &dev->cl_list, link)
+		if (cl->host_client_id == pos->host_client_id) {
+			list_del_init(&pos->link);
+			break;
+		}
+	spin_unlock_irqrestore(&dev->cl_list_lock, flags);
+}
+EXPORT_SYMBOL(ishtp_cl_unlink);
+
+/**
+ * ishtp_cl_disconnect() - Send disconnect request to firmware
+ * @cl: client device instance
+ *
+ * Send a disconnect request for a client to firmware.
+ *
+ * Return: 0 if successful disconnect response from the firmware or error
+ * code on failure
+ */
+int ishtp_cl_disconnect(struct ishtp_cl *cl)
+{
+	struct ishtp_device *dev;
+	int err;
+
+	if (WARN_ON(!cl || !cl->dev))
+		return -ENODEV;
+
+	dev = cl->dev;
+
+	dev->print_log(dev, "%s() state %d\n", __func__, cl->state);
+
+	if (cl->state != ISHTP_CL_DISCONNECTING) {
+		dev->print_log(dev, "%s() Disconnect in progress\n", __func__);
+		return 0;
+	}
+
+	if (ishtp_hbm_cl_disconnect_req(dev, cl)) {
+		dev->print_log(dev, "%s() Failed to disconnect\n", __func__);
+		dev_err(&cl->device->dev, "failed to disconnect.\n");
+		return -ENODEV;
+	}
+
+	err = wait_event_interruptible_timeout(cl->wait_ctrl_res,
+			(dev->dev_state != ISHTP_DEV_ENABLED ||
+			cl->state == ISHTP_CL_DISCONNECTED),
+			ishtp_secs_to_jiffies(ISHTP_CL_CONNECT_TIMEOUT));
+
+	/*
+	 * If FW reset arrived, this will happen. Don't check cl->,
+	 * as 'cl' may be freed already
+	 */
+	if (dev->dev_state != ISHTP_DEV_ENABLED) {
+		dev->print_log(dev, "%s() dev_state != ISHTP_DEV_ENABLED\n",
+			       __func__);
+		return -ENODEV;
+	}
+
+	if (cl->state == ISHTP_CL_DISCONNECTED) {
+		dev->print_log(dev, "%s() successful\n", __func__);
+		return 0;
+	}
+
+	return -ENODEV;
+}
+EXPORT_SYMBOL(ishtp_cl_disconnect);
+
+/**
+ * ishtp_cl_is_other_connecting() - Check other client is connecting
+ * @cl: client device instance
+ *
+ * Checks if other client with the same fw client id is connecting
+ *
+ * Return: true if other client is connected else false
+ */
+static bool ishtp_cl_is_other_connecting(struct ishtp_cl *cl)
+{
+	struct ishtp_device *dev;
+	struct ishtp_cl *pos;
+	unsigned long	flags;
+
+	if (WARN_ON(!cl || !cl->dev))
+		return false;
+
+	dev = cl->dev;
+	spin_lock_irqsave(&dev->cl_list_lock, flags);
+	list_for_each_entry(pos, &dev->cl_list, link) {
+		if ((pos->state == ISHTP_CL_CONNECTING) && (pos != cl) &&
+				cl->fw_client_id == pos->fw_client_id) {
+			spin_unlock_irqrestore(&dev->cl_list_lock, flags);
+			return true;
+		}
+	}
+	spin_unlock_irqrestore(&dev->cl_list_lock, flags);
+
+	return false;
+}
+
+/**
+ * ishtp_cl_connect() - Send connect request to firmware
+ * @cl: client device instance
+ *
+ * Send a connect request for a client to firmware. If successful it will
+ * RX and TX ring buffers
+ *
+ * Return: 0 if successful connect response from the firmware and able
+ * to bind and allocate ring buffers or error code on failure
+ */
+int ishtp_cl_connect(struct ishtp_cl *cl)
+{
+	struct ishtp_device *dev;
+	int rets;
+
+	if (WARN_ON(!cl || !cl->dev))
+		return -ENODEV;
+
+	dev = cl->dev;
+
+	dev->print_log(dev, "%s() current_state = %d\n", __func__, cl->state);
+
+	if (ishtp_cl_is_other_connecting(cl)) {
+		dev->print_log(dev, "%s() Busy\n", __func__);
+		return	-EBUSY;
+	}
+
+	if (ishtp_hbm_cl_connect_req(dev, cl)) {
+		dev->print_log(dev, "%s() HBM connect req fail\n", __func__);
+		return -ENODEV;
+	}
+
+	rets = wait_event_interruptible_timeout(cl->wait_ctrl_res,
+				(dev->dev_state == ISHTP_DEV_ENABLED &&
+				(cl->state == ISHTP_CL_CONNECTED ||
+				 cl->state == ISHTP_CL_DISCONNECTED)),
+				ishtp_secs_to_jiffies(
+					ISHTP_CL_CONNECT_TIMEOUT));
+	/*
+	 * If FW reset arrived, this will happen. Don't check cl->,
+	 * as 'cl' may be freed already
+	 */
+	if (dev->dev_state != ISHTP_DEV_ENABLED) {
+		dev->print_log(dev, "%s() dev_state != ISHTP_DEV_ENABLED\n",
+			       __func__);
+		return -EFAULT;
+	}
+
+	if (cl->state != ISHTP_CL_CONNECTED) {
+		dev->print_log(dev, "%s() state != ISHTP_CL_CONNECTED\n",
+			       __func__);
+		return -EFAULT;
+	}
+
+	rets = cl->status;
+	if (rets) {
+		dev->print_log(dev, "%s() Invalid status\n", __func__);
+		return rets;
+	}
+
+	rets = ishtp_cl_device_bind(cl);
+	if (rets) {
+		dev->print_log(dev, "%s() Bind error\n", __func__);
+		ishtp_cl_disconnect(cl);
+		return rets;
+	}
+
+	rets = ishtp_cl_alloc_rx_ring(cl);
+	if (rets) {
+		dev->print_log(dev, "%s() Alloc RX ring failed\n", __func__);
+		/* if failed allocation, disconnect */
+		ishtp_cl_disconnect(cl);
+		return rets;
+	}
+
+	rets = ishtp_cl_alloc_tx_ring(cl);
+	if (rets) {
+		dev->print_log(dev, "%s() Alloc TX ring failed\n", __func__);
+		/* if failed allocation, disconnect */
+		ishtp_cl_free_rx_ring(cl);
+		ishtp_cl_disconnect(cl);
+		return rets;
+	}
+
+	/* Upon successful connection and allocation, emit flow-control */
+	rets = ishtp_cl_read_start(cl);
+
+	dev->print_log(dev, "%s() successful\n", __func__);
+
+	return rets;
+}
+EXPORT_SYMBOL(ishtp_cl_connect);
+
+/**
+ * ishtp_cl_read_start() - Prepare to read client message
+ * @cl: client device instance
+ *
+ * Get a free buffer from pool of free read buffers and add to read buffer
+ * pool to add contents. Send a flow control request to firmware to be able
+ * send next message.
+ *
+ * Return: 0 if successful or error code on failure
+ */
+int ishtp_cl_read_start(struct ishtp_cl *cl)
+{
+	struct ishtp_device *dev;
+	struct ishtp_cl_rb *rb;
+	int rets;
+	int i;
+	unsigned long	flags;
+	unsigned long	dev_flags;
+
+	if (WARN_ON(!cl || !cl->dev))
+		return -ENODEV;
+
+	dev = cl->dev;
+
+	if (cl->state != ISHTP_CL_CONNECTED)
+		return -ENODEV;
+
+	if (dev->dev_state != ISHTP_DEV_ENABLED)
+		return -ENODEV;
+
+	i = ishtp_fw_cl_by_id(dev, cl->fw_client_id);
+	if (i < 0) {
+		dev_err(&cl->device->dev, "no such fw client %d\n",
+			cl->fw_client_id);
+		return -ENODEV;
+	}
+
+	/* The current rb is the head of the free rb list */
+	spin_lock_irqsave(&cl->free_list_spinlock, flags);
+	if (list_empty(&cl->free_rb_list.list)) {
+		dev_warn(&cl->device->dev,
+			 "[ishtp-ish] Rx buffers pool is empty\n");
+		rets = -ENOMEM;
+		rb = NULL;
+		spin_unlock_irqrestore(&cl->free_list_spinlock, flags);
+		goto out;
+	}
+	rb = list_entry(cl->free_rb_list.list.next, struct ishtp_cl_rb, list);
+	list_del_init(&rb->list);
+	spin_unlock_irqrestore(&cl->free_list_spinlock, flags);
+
+	rb->cl = cl;
+	rb->buf_idx = 0;
+
+	INIT_LIST_HEAD(&rb->list);
+	rets = 0;
+
+	/*
+	 * This must be BEFORE sending flow control -
+	 * response in ISR may come too fast...
+	 */
+	spin_lock_irqsave(&dev->read_list_spinlock, dev_flags);
+	list_add_tail(&rb->list, &dev->read_list.list);
+	spin_unlock_irqrestore(&dev->read_list_spinlock, dev_flags);
+	if (ishtp_hbm_cl_flow_control_req(dev, cl)) {
+		rets = -ENODEV;
+		goto out;
+	}
+out:
+	/* if ishtp_hbm_cl_flow_control_req failed, return rb to free list */
+	if (rets && rb) {
+		spin_lock_irqsave(&dev->read_list_spinlock, dev_flags);
+		list_del(&rb->list);
+		spin_unlock_irqrestore(&dev->read_list_spinlock, dev_flags);
+
+		spin_lock_irqsave(&cl->free_list_spinlock, flags);
+		list_add_tail(&rb->list, &cl->free_rb_list.list);
+		spin_unlock_irqrestore(&cl->free_list_spinlock, flags);
+	}
+	return rets;
+}
+
+/**
+ * ishtp_cl_send() - Send a message to firmware
+ * @cl: client device instance
+ * @buf: message buffer
+ * @length: length of message
+ *
+ * If the client is correct state to send message, this function gets a buffer
+ * from tx ring buffers, copy the message data and call to send the message
+ * using ishtp_cl_send_msg()
+ *
+ * Return: 0 if successful or error code on failure
+ */
+int ishtp_cl_send(struct ishtp_cl *cl, uint8_t *buf, size_t length)
+{
+	struct ishtp_device	*dev;
+	int	id;
+	struct ishtp_cl_tx_ring	*cl_msg;
+	int	have_msg_to_send = 0;
+	unsigned long	tx_flags, tx_free_flags;
+
+	if (WARN_ON(!cl || !cl->dev))
+		return -ENODEV;
+
+	dev = cl->dev;
+
+	if (cl->state != ISHTP_CL_CONNECTED) {
+		++cl->err_send_msg;
+		return -EPIPE;
+	}
+
+	if (dev->dev_state != ISHTP_DEV_ENABLED) {
+		++cl->err_send_msg;
+		return -ENODEV;
+	}
+
+	/* Check if we have fw client device */
+	id = ishtp_fw_cl_by_id(dev, cl->fw_client_id);
+	if (id < 0) {
+		++cl->err_send_msg;
+		return -ENOENT;
+	}
+
+	if (length > dev->fw_clients[id].props.max_msg_length) {
+		++cl->err_send_msg;
+		return -EMSGSIZE;
+	}
+
+	/* No free bufs */
+	spin_lock_irqsave(&cl->tx_free_list_spinlock, tx_free_flags);
+	if (list_empty(&cl->tx_free_list.list)) {
+		spin_unlock_irqrestore(&cl->tx_free_list_spinlock,
+			tx_free_flags);
+		++cl->err_send_msg;
+		return	-ENOMEM;
+	}
+
+	cl_msg = list_first_entry(&cl->tx_free_list.list,
+		struct ishtp_cl_tx_ring, list);
+	if (!cl_msg->send_buf.data) {
+		spin_unlock_irqrestore(&cl->tx_free_list_spinlock,
+			tx_free_flags);
+		return	-EIO;
+		/* Should not happen, as free list is pre-allocated */
+	}
+	/*
+	 * This is safe, as 'length' is already checked for not exceeding
+	 * max ISHTP message size per client
+	 */
+	list_del_init(&cl_msg->list);
+	spin_unlock_irqrestore(&cl->tx_free_list_spinlock, tx_free_flags);
+	memcpy(cl_msg->send_buf.data, buf, length);
+	cl_msg->send_buf.size = length;
+	spin_lock_irqsave(&cl->tx_list_spinlock, tx_flags);
+	have_msg_to_send = !list_empty(&cl->tx_list.list);
+	list_add_tail(&cl_msg->list, &cl->tx_list.list);
+	spin_unlock_irqrestore(&cl->tx_list_spinlock, tx_flags);
+
+	if (!have_msg_to_send && cl->ishtp_flow_ctrl_creds > 0)
+		ishtp_cl_send_msg(dev, cl);
+
+	return	0;
+}
+EXPORT_SYMBOL(ishtp_cl_send);
+
+/**
+ * ishtp_cl_read_complete() - read complete
+ * @rb: Pointer to client request block
+ *
+ * If the message is completely received call ishtp_cl_bus_rx_event()
+ * to process message
+ */
+static void ishtp_cl_read_complete(struct ishtp_cl_rb *rb)
+{
+	unsigned long	flags;
+	int	schedule_work_flag = 0;
+	struct ishtp_cl	*cl = rb->cl;
+
+	spin_lock_irqsave(&cl->in_process_spinlock, flags);
+	/*
+	 * if in-process list is empty, then need to schedule
+	 * the processing thread
+	 */
+	schedule_work_flag = list_empty(&cl->in_process_list.list);
+	list_add_tail(&rb->list, &cl->in_process_list.list);
+	spin_unlock_irqrestore(&cl->in_process_spinlock, flags);
+
+	if (schedule_work_flag)
+		ishtp_cl_bus_rx_event(cl->device);
+}
+
+/**
+ * ipc_tx_callback() - IPC tx callback function
+ * @prm: Pointer to client device instance
+ *
+ * Send message over IPC either first time or on callback on previous message
+ * completion
+ */
+static void ipc_tx_callback(void *prm)
+{
+	struct ishtp_cl	*cl = prm;
+	struct ishtp_cl_tx_ring	*cl_msg;
+	size_t	rem;
+	struct ishtp_device	*dev = (cl ? cl->dev : NULL);
+	struct ishtp_msg_hdr	ishtp_hdr;
+	unsigned long	tx_flags, tx_free_flags;
+	unsigned char	*pmsg;
+
+	if (!dev)
+		return;
+
+	/*
+	 * Other conditions if some critical error has
+	 * occurred before this callback is called
+	 */
+	if (dev->dev_state != ISHTP_DEV_ENABLED)
+		return;
+
+	if (cl->state != ISHTP_CL_CONNECTED)
+		return;
+
+	spin_lock_irqsave(&cl->tx_list_spinlock, tx_flags);
+	if (list_empty(&cl->tx_list.list)) {
+		spin_unlock_irqrestore(&cl->tx_list_spinlock, tx_flags);
+		return;
+	}
+
+	if (cl->ishtp_flow_ctrl_creds != 1 && !cl->sending) {
+		spin_unlock_irqrestore(&cl->tx_list_spinlock, tx_flags);
+		return;
+	}
+
+	if (!cl->sending) {
+		--cl->ishtp_flow_ctrl_creds;
+		cl->last_ipc_acked = 0;
+		cl->last_tx_path = CL_TX_PATH_IPC;
+		cl->sending = 1;
+	}
+
+	cl_msg = list_entry(cl->tx_list.list.next, struct ishtp_cl_tx_ring,
+			    list);
+	rem = cl_msg->send_buf.size - cl->tx_offs;
+
+	ishtp_hdr.host_addr = cl->host_client_id;
+	ishtp_hdr.fw_addr = cl->fw_client_id;
+	ishtp_hdr.reserved = 0;
+	pmsg = cl_msg->send_buf.data + cl->tx_offs;
+
+	if (rem <= dev->mtu) {
+		ishtp_hdr.length = rem;
+		ishtp_hdr.msg_complete = 1;
+		cl->sending = 0;
+		list_del_init(&cl_msg->list);	/* Must be before write */
+		spin_unlock_irqrestore(&cl->tx_list_spinlock, tx_flags);
+		/* Submit to IPC queue with no callback */
+		ishtp_write_message(dev, &ishtp_hdr, pmsg);
+		spin_lock_irqsave(&cl->tx_free_list_spinlock, tx_free_flags);
+		list_add_tail(&cl_msg->list, &cl->tx_free_list.list);
+		spin_unlock_irqrestore(&cl->tx_free_list_spinlock,
+			tx_free_flags);
+	} else {
+		/* Send IPC fragment */
+		spin_unlock_irqrestore(&cl->tx_list_spinlock, tx_flags);
+		cl->tx_offs += dev->mtu;
+		ishtp_hdr.length = dev->mtu;
+		ishtp_hdr.msg_complete = 0;
+		ishtp_send_msg(dev, &ishtp_hdr, pmsg, ipc_tx_callback, cl);
+	}
+}
+
+/**
+ * ishtp_cl_send_msg_ipc() -Send message using IPC
+ * @dev: ISHTP device instance
+ * @cl: Pointer to client device instance
+ *
+ * Send message over IPC not using DMA
+ */
+static void ishtp_cl_send_msg_ipc(struct ishtp_device *dev,
+				  struct ishtp_cl *cl)
+{
+	/* If last DMA message wasn't acked yet, leave this one in Tx queue */
+	if (cl->last_tx_path == CL_TX_PATH_DMA && cl->last_dma_acked == 0)
+		return;
+
+	cl->tx_offs = 0;
+	ipc_tx_callback(cl);
+	++cl->send_msg_cnt_ipc;
+}
+
+/**
+ * ishtp_cl_send_msg_dma() -Send message using DMA
+ * @dev: ISHTP device instance
+ * @cl: Pointer to client device instance
+ *
+ * Send message using DMA
+ */
+static void ishtp_cl_send_msg_dma(struct ishtp_device *dev,
+	struct ishtp_cl *cl)
+{
+	struct ishtp_msg_hdr	hdr;
+	struct dma_xfer_hbm	dma_xfer;
+	unsigned char	*msg_addr;
+	int off;
+	struct ishtp_cl_tx_ring	*cl_msg;
+	unsigned long tx_flags, tx_free_flags;
+
+	/* If last IPC message wasn't acked yet, leave this one in Tx queue */
+	if (cl->last_tx_path == CL_TX_PATH_IPC && cl->last_ipc_acked == 0)
+		return;
+
+	spin_lock_irqsave(&cl->tx_list_spinlock, tx_flags);
+	if (list_empty(&cl->tx_list.list)) {
+		spin_unlock_irqrestore(&cl->tx_list_spinlock, tx_flags);
+		return;
+	}
+
+	cl_msg = list_entry(cl->tx_list.list.next, struct ishtp_cl_tx_ring,
+		list);
+
+	msg_addr = ishtp_cl_get_dma_send_buf(dev, cl_msg->send_buf.size);
+	if (!msg_addr) {
+		spin_unlock_irqrestore(&cl->tx_list_spinlock, tx_flags);
+		if (dev->transfer_path == CL_TX_PATH_DEFAULT)
+			ishtp_cl_send_msg_ipc(dev, cl);
+		return;
+	}
+
+	list_del_init(&cl_msg->list);	/* Must be before write */
+	spin_unlock_irqrestore(&cl->tx_list_spinlock, tx_flags);
+
+	--cl->ishtp_flow_ctrl_creds;
+	cl->last_dma_acked = 0;
+	cl->last_dma_addr = msg_addr;
+	cl->last_tx_path = CL_TX_PATH_DMA;
+
+	/* write msg to dma buf */
+	memcpy(msg_addr, cl_msg->send_buf.data, cl_msg->send_buf.size);
+
+	/* send dma_xfer hbm msg */
+	off = msg_addr - (unsigned char *)dev->ishtp_host_dma_tx_buf;
+	ishtp_hbm_hdr(&hdr, sizeof(struct dma_xfer_hbm));
+	dma_xfer.hbm = DMA_XFER;
+	dma_xfer.fw_client_id = cl->fw_client_id;
+	dma_xfer.host_client_id = cl->host_client_id;
+	dma_xfer.reserved = 0;
+	dma_xfer.msg_addr = dev->ishtp_host_dma_tx_buf_phys + off;
+	dma_xfer.msg_length = cl_msg->send_buf.size;
+	dma_xfer.reserved2 = 0;
+	ishtp_write_message(dev, &hdr, (unsigned char *)&dma_xfer);
+	spin_lock_irqsave(&cl->tx_free_list_spinlock, tx_free_flags);
+	list_add_tail(&cl_msg->list, &cl->tx_free_list.list);
+	spin_unlock_irqrestore(&cl->tx_free_list_spinlock, tx_free_flags);
+	++cl->send_msg_cnt_dma;
+}
+
+/**
+ * ishtp_cl_send_msg() -Send message using DMA or IPC
+ * @dev: ISHTP device instance
+ * @cl: Pointer to client device instance
+ *
+ * Send message using DMA or IPC based on transfer_path
+ */
+void ishtp_cl_send_msg(struct ishtp_device *dev, struct ishtp_cl *cl)
+{
+	if (dev->transfer_path == CL_TX_PATH_DMA)
+		ishtp_cl_send_msg_dma(dev, cl);
+	else
+		ishtp_cl_send_msg_ipc(dev, cl);
+}
+
+/**
+ * recv_ishtp_cl_msg() -Receive client message
+ * @dev: ISHTP device instance
+ * @ishtp_hdr: Pointer to message header
+ *
+ * Receive and dispatch ISHTP client messages. This function executes in ISR
+ * context
+ */
+void recv_ishtp_cl_msg(struct ishtp_device *dev,
+		       struct ishtp_msg_hdr *ishtp_hdr)
+{
+	struct ishtp_cl *cl;
+	struct ishtp_cl_rb *rb;
+	struct ishtp_cl_rb *new_rb;
+	unsigned char *buffer = NULL;
+	struct ishtp_cl_rb *complete_rb = NULL;
+	unsigned long	dev_flags;
+	unsigned long	flags;
+	int	rb_count;
+
+	if (ishtp_hdr->reserved) {
+		dev_err(dev->devc, "corrupted message header.\n");
+		goto	eoi;
+	}
+
+	if (ishtp_hdr->length > IPC_PAYLOAD_SIZE) {
+		dev_err(dev->devc,
+			"ISHTP message length in hdr exceeds IPC MTU\n");
+		goto	eoi;
+	}
+
+	spin_lock_irqsave(&dev->read_list_spinlock, dev_flags);
+	rb_count = -1;
+	list_for_each_entry(rb, &dev->read_list.list, list) {
+		++rb_count;
+		cl = rb->cl;
+		if (!cl || !(cl->host_client_id == ishtp_hdr->host_addr &&
+				cl->fw_client_id == ishtp_hdr->fw_addr) ||
+				!(cl->state == ISHTP_CL_CONNECTED))
+			continue;
+
+		 /* If no Rx buffer is allocated, disband the rb */
+		if (rb->buffer.size == 0 || rb->buffer.data == NULL) {
+			spin_unlock_irqrestore(&dev->read_list_spinlock,
+				dev_flags);
+			dev_err(&cl->device->dev,
+				"Rx buffer is not allocated.\n");
+			list_del(&rb->list);
+			ishtp_io_rb_free(rb);
+			cl->status = -ENOMEM;
+			goto	eoi;
+		}
+
+		/*
+		 * If message buffer overflown (exceeds max. client msg
+		 * size, drop message and return to free buffer.
+		 * Do we need to disconnect such a client? (We don't send
+		 * back FC, so communication will be stuck anyway)
+		 */
+		if (rb->buffer.size < ishtp_hdr->length + rb->buf_idx) {
+			spin_unlock_irqrestore(&dev->read_list_spinlock,
+				dev_flags);
+			dev_err(&cl->device->dev,
+				"message overflow. size %d len %d idx %ld\n",
+				rb->buffer.size, ishtp_hdr->length,
+				rb->buf_idx);
+			list_del(&rb->list);
+			ishtp_cl_io_rb_recycle(rb);
+			cl->status = -EIO;
+			goto	eoi;
+		}
+
+		buffer = rb->buffer.data + rb->buf_idx;
+		dev->ops->ishtp_read(dev, buffer, ishtp_hdr->length);
+
+		rb->buf_idx += ishtp_hdr->length;
+		if (ishtp_hdr->msg_complete) {
+			/* Last fragment in message - it's complete */
+			cl->status = 0;
+			list_del(&rb->list);
+			complete_rb = rb;
+
+			--cl->out_flow_ctrl_creds;
+			/*
+			 * the whole msg arrived, send a new FC, and add a new
+			 * rb buffer for the next coming msg
+			 */
+			spin_lock_irqsave(&cl->free_list_spinlock, flags);
+
+			if (!list_empty(&cl->free_rb_list.list)) {
+				new_rb = list_entry(cl->free_rb_list.list.next,
+					struct ishtp_cl_rb, list);
+				list_del_init(&new_rb->list);
+				spin_unlock_irqrestore(&cl->free_list_spinlock,
+					flags);
+				new_rb->cl = cl;
+				new_rb->buf_idx = 0;
+				INIT_LIST_HEAD(&new_rb->list);
+				list_add_tail(&new_rb->list,
+					&dev->read_list.list);
+
+				ishtp_hbm_cl_flow_control_req(dev, cl);
+			} else {
+				spin_unlock_irqrestore(&cl->free_list_spinlock,
+					flags);
+			}
+		}
+		/* One more fragment in message (even if this was last) */
+		++cl->recv_msg_num_frags;
+
+		/*
+		 * We can safely break here (and in BH too),
+		 * a single input message can go only to a single request!
+		 */
+		break;
+	}
+
+	spin_unlock_irqrestore(&dev->read_list_spinlock, dev_flags);
+	/* If it's nobody's message, just read and discard it */
+	if (!buffer) {
+		uint8_t	rd_msg_buf[ISHTP_RD_MSG_BUF_SIZE];
+
+		dev_err(dev->devc, "Dropped Rx msg - no request\n");
+		dev->ops->ishtp_read(dev, rd_msg_buf, ishtp_hdr->length);
+		goto	eoi;
+	}
+
+	if (complete_rb) {
+		getnstimeofday(&cl->ts_rx);
+		++cl->recv_msg_cnt_ipc;
+		ishtp_cl_read_complete(complete_rb);
+	}
+eoi:
+	return;
+}
+
+/**
+ * recv_ishtp_cl_msg_dma() -Receive client message
+ * @dev: ISHTP device instance
+ * @msg: message pointer
+ * @hbm: hbm buffer
+ *
+ * Receive and dispatch ISHTP client messages using DMA. This function executes
+ * in ISR context
+ */
+void recv_ishtp_cl_msg_dma(struct ishtp_device *dev, void *msg,
+			   struct dma_xfer_hbm *hbm)
+{
+	struct ishtp_cl *cl;
+	struct ishtp_cl_rb *rb;
+	struct ishtp_cl_rb *new_rb;
+	unsigned char *buffer = NULL;
+	struct ishtp_cl_rb *complete_rb = NULL;
+	unsigned long	dev_flags;
+	unsigned long	flags;
+
+	spin_lock_irqsave(&dev->read_list_spinlock, dev_flags);
+	list_for_each_entry(rb, &dev->read_list.list, list) {
+		cl = rb->cl;
+		if (!cl || !(cl->host_client_id == hbm->host_client_id &&
+				cl->fw_client_id == hbm->fw_client_id) ||
+				!(cl->state == ISHTP_CL_CONNECTED))
+			continue;
+
+		/*
+		 * If no Rx buffer is allocated, disband the rb
+		 */
+		if (rb->buffer.size == 0 || rb->buffer.data == NULL) {
+			spin_unlock_irqrestore(&dev->read_list_spinlock,
+				dev_flags);
+			dev_err(&cl->device->dev,
+				"response buffer is not allocated.\n");
+			list_del(&rb->list);
+			ishtp_io_rb_free(rb);
+			cl->status = -ENOMEM;
+			goto	eoi;
+		}
+
+		/*
+		 * If message buffer overflown (exceeds max. client msg
+		 * size, drop message and return to free buffer.
+		 * Do we need to disconnect such a client? (We don't send
+		 * back FC, so communication will be stuck anyway)
+		 */
+		if (rb->buffer.size < hbm->msg_length) {
+			spin_unlock_irqrestore(&dev->read_list_spinlock,
+				dev_flags);
+			dev_err(&cl->device->dev,
+				"message overflow. size %d len %d idx %ld\n",
+				rb->buffer.size, hbm->msg_length, rb->buf_idx);
+			list_del(&rb->list);
+			ishtp_cl_io_rb_recycle(rb);
+			cl->status = -EIO;
+			goto	eoi;
+		}
+
+		buffer = rb->buffer.data;
+		memcpy(buffer, msg, hbm->msg_length);
+		rb->buf_idx = hbm->msg_length;
+
+		/* Last fragment in message - it's complete */
+		cl->status = 0;
+		list_del(&rb->list);
+		complete_rb = rb;
+
+		--cl->out_flow_ctrl_creds;
+		/*
+		 * the whole msg arrived, send a new FC, and add a new
+		 * rb buffer for the next coming msg
+		 */
+		spin_lock_irqsave(&cl->free_list_spinlock, flags);
+
+		if (!list_empty(&cl->free_rb_list.list)) {
+			new_rb = list_entry(cl->free_rb_list.list.next,
+				struct ishtp_cl_rb, list);
+			list_del_init(&new_rb->list);
+			spin_unlock_irqrestore(&cl->free_list_spinlock,
+				flags);
+			new_rb->cl = cl;
+			new_rb->buf_idx = 0;
+			INIT_LIST_HEAD(&new_rb->list);
+			list_add_tail(&new_rb->list,
+				&dev->read_list.list);
+
+			ishtp_hbm_cl_flow_control_req(dev, cl);
+		} else {
+			spin_unlock_irqrestore(&cl->free_list_spinlock,
+				flags);
+		}
+
+		/* One more fragment in message (this is always last) */
+		++cl->recv_msg_num_frags;
+
+		/*
+		 * We can safely break here (and in BH too),
+		 * a single input message can go only to a single request!
+		 */
+		break;
+	}
+
+	spin_unlock_irqrestore(&dev->read_list_spinlock, dev_flags);
+	/* If it's nobody's message, just read and discard it */
+	if (!buffer) {
+		dev_err(dev->devc, "Dropped Rx (DMA) msg - no request\n");
+		goto	eoi;
+	}
+
+	if (complete_rb) {
+		getnstimeofday(&cl->ts_rx);
+		++cl->recv_msg_cnt_dma;
+		ishtp_cl_read_complete(complete_rb);
+	}
+eoi:
+	return;
+}
diff --git a/drivers/hid/intel-ish-hid/ishtp/client.h b/drivers/hid/intel-ish-hid/ishtp/client.h
new file mode 100644
index 000000000000..444d069c2ed4
--- /dev/null
+++ b/drivers/hid/intel-ish-hid/ishtp/client.h
@@ -0,0 +1,182 @@
+/*
+ * ISHTP client logic
+ *
+ * Copyright (c) 2003-2016, 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 _ISHTP_CLIENT_H_
+#define _ISHTP_CLIENT_H_
+
+#include <linux/types.h>
+#include "ishtp-dev.h"
+
+/* Client state */
+enum cl_state {
+	ISHTP_CL_INITIALIZING = 0,
+	ISHTP_CL_CONNECTING,
+	ISHTP_CL_CONNECTED,
+	ISHTP_CL_DISCONNECTING,
+	ISHTP_CL_DISCONNECTED
+};
+
+/* Tx and Rx ring size */
+#define	CL_DEF_RX_RING_SIZE	2
+#define	CL_DEF_TX_RING_SIZE	2
+#define	CL_MAX_RX_RING_SIZE	32
+#define	CL_MAX_TX_RING_SIZE	32
+
+#define DMA_SLOT_SIZE		4096
+/* Number of IPC fragments after which it's worth sending via DMA */
+#define	DMA_WORTH_THRESHOLD	3
+
+/* DMA/IPC Tx paths. Other the default means enforcement */
+#define	CL_TX_PATH_DEFAULT	0
+#define	CL_TX_PATH_IPC		1
+#define	CL_TX_PATH_DMA		2
+
+/* Client Tx buffer list entry */
+struct ishtp_cl_tx_ring {
+	struct list_head	list;
+	struct ishtp_msg_data	send_buf;
+};
+
+/* ISHTP client instance */
+struct ishtp_cl {
+	struct list_head	link;
+	struct ishtp_device	*dev;
+	enum cl_state		state;
+	int			status;
+
+	/* Link to ISHTP bus device */
+	struct ishtp_cl_device	*device;
+
+	/* ID of client connected */
+	uint8_t	host_client_id;
+	uint8_t	fw_client_id;
+	uint8_t	ishtp_flow_ctrl_creds;
+	uint8_t	out_flow_ctrl_creds;
+
+	/* dma */
+	int	last_tx_path;
+	/* 0: ack wasn't received,1:ack was received */
+	int	last_dma_acked;
+	unsigned char	*last_dma_addr;
+	/* 0: ack wasn't received,1:ack was received */
+	int	last_ipc_acked;
+
+	/* Rx ring buffer pool */
+	unsigned int	rx_ring_size;
+	struct ishtp_cl_rb	free_rb_list;
+	spinlock_t	free_list_spinlock;
+	/* Rx in-process list */
+	struct ishtp_cl_rb	in_process_list;
+	spinlock_t	in_process_spinlock;
+
+	/* Client Tx buffers list */
+	unsigned int	tx_ring_size;
+	struct ishtp_cl_tx_ring	tx_list, tx_free_list;
+	spinlock_t	tx_list_spinlock;
+	spinlock_t	tx_free_list_spinlock;
+	size_t	tx_offs;	/* Offset in buffer at head of 'tx_list' */
+
+	/**
+	 * if we get a FC, and the list is not empty, we must know whether we
+	 * are at the middle of sending.
+	 * if so -need to increase FC counter, otherwise, need to start sending
+	 * the first msg in list
+	 * (!)This is for counting-FC implementation only. Within single-FC the
+	 * other party may NOT send FC until it receives complete message
+	 */
+	int	sending;
+
+	/* Send FC spinlock */
+	spinlock_t	fc_spinlock;
+
+	/* wait queue for connect and disconnect response from FW */
+	wait_queue_head_t	wait_ctrl_res;
+
+	/* Error stats */
+	unsigned int	err_send_msg;
+	unsigned int	err_send_fc;
+
+	/* Send/recv stats */
+	unsigned int	send_msg_cnt_ipc;
+	unsigned int	send_msg_cnt_dma;
+	unsigned int	recv_msg_cnt_ipc;
+	unsigned int	recv_msg_cnt_dma;
+	unsigned int	recv_msg_num_frags;
+	unsigned int	ishtp_flow_ctrl_cnt;
+	unsigned int	out_flow_ctrl_cnt;
+
+	/* Rx msg ... out FC timing */
+	struct timespec ts_rx;
+	struct timespec ts_out_fc;
+	struct timespec ts_max_fc_delay;
+	void *client_data;
+};
+
+/* Client connection managenment internal functions */
+int ishtp_can_client_connect(struct ishtp_device *ishtp_dev, uuid_le *uuid);
+int ishtp_fw_cl_by_id(struct ishtp_device *dev, uint8_t client_id);
+void ishtp_cl_send_msg(struct ishtp_device *dev, struct ishtp_cl *cl);
+void recv_ishtp_cl_msg(struct ishtp_device *dev,
+		       struct ishtp_msg_hdr *ishtp_hdr);
+int ishtp_cl_read_start(struct ishtp_cl *cl);
+
+/* Ring Buffer I/F */
+int ishtp_cl_alloc_rx_ring(struct ishtp_cl *cl);
+int ishtp_cl_alloc_tx_ring(struct ishtp_cl *cl);
+void ishtp_cl_free_rx_ring(struct ishtp_cl *cl);
+void ishtp_cl_free_tx_ring(struct ishtp_cl *cl);
+
+/* DMA I/F functions */
+void recv_ishtp_cl_msg_dma(struct ishtp_device *dev, void *msg,
+			   struct dma_xfer_hbm *hbm);
+void ishtp_cl_alloc_dma_buf(struct ishtp_device *dev);
+void ishtp_cl_free_dma_buf(struct ishtp_device *dev);
+void *ishtp_cl_get_dma_send_buf(struct ishtp_device *dev,
+				uint32_t size);
+void ishtp_cl_release_dma_acked_mem(struct ishtp_device *dev,
+				    void *msg_addr,
+				    uint8_t size);
+
+/* Request blocks alloc/free I/F */
+struct ishtp_cl_rb *ishtp_io_rb_init(struct ishtp_cl *cl);
+void ishtp_io_rb_free(struct ishtp_cl_rb *priv_rb);
+int ishtp_io_rb_alloc_buf(struct ishtp_cl_rb *rb, size_t length);
+
+/**
+ * ishtp_cl_cmp_id - tells if file private data have same id
+ * returns true  - if ids are the same and not NULL
+ */
+static inline bool ishtp_cl_cmp_id(const struct ishtp_cl *cl1,
+				   const struct ishtp_cl *cl2)
+{
+	return cl1 && cl2 &&
+		(cl1->host_client_id == cl2->host_client_id) &&
+		(cl1->fw_client_id == cl2->fw_client_id);
+}
+
+/* exported functions from ISHTP under client management scope */
+struct ishtp_cl	*ishtp_cl_allocate(struct ishtp_device *dev);
+void ishtp_cl_free(struct ishtp_cl *cl);
+int ishtp_cl_link(struct ishtp_cl *cl, int id);
+void ishtp_cl_unlink(struct ishtp_cl *cl);
+int ishtp_cl_disconnect(struct ishtp_cl *cl);
+int ishtp_cl_connect(struct ishtp_cl *cl);
+int ishtp_cl_send(struct ishtp_cl *cl, uint8_t *buf, size_t length);
+int ishtp_cl_flush_queues(struct ishtp_cl *cl);
+
+/* exported functions from ISHTP client buffer management scope */
+int ishtp_cl_io_rb_recycle(struct ishtp_cl_rb *rb);
+
+#endif /* _ISHTP_CLIENT_H_ */
diff --git a/drivers/hid/intel-ish-hid/ishtp/dma-if.c b/drivers/hid/intel-ish-hid/ishtp/dma-if.c
new file mode 100644
index 000000000000..2783f3666114
--- /dev/null
+++ b/drivers/hid/intel-ish-hid/ishtp/dma-if.c
@@ -0,0 +1,175 @@
+/*
+ * ISHTP DMA I/F functions
+ *
+ * Copyright (c) 2003-2016, 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/slab.h>
+#include <linux/sched.h>
+#include <linux/wait.h>
+#include <linux/delay.h>
+#include <linux/dma-mapping.h>
+#include "ishtp-dev.h"
+#include "client.h"
+
+/**
+ * ishtp_cl_alloc_dma_buf() - Allocate DMA RX and TX buffer
+ * @dev: ishtp device
+ *
+ * Allocate RX and TX DMA buffer once during bus setup.
+ * It allocates 1MB, RX and TX DMA buffer, which are divided
+ * into slots.
+ */
+void	ishtp_cl_alloc_dma_buf(struct ishtp_device *dev)
+{
+	dma_addr_t	h;
+
+	if (dev->ishtp_host_dma_tx_buf)
+		return;
+
+	dev->ishtp_host_dma_tx_buf_size = 1024*1024;
+	dev->ishtp_host_dma_rx_buf_size = 1024*1024;
+
+	/* Allocate Tx buffer and init usage bitmap */
+	dev->ishtp_host_dma_tx_buf = dma_alloc_coherent(dev->devc,
+					dev->ishtp_host_dma_tx_buf_size,
+					&h, GFP_KERNEL);
+	if (dev->ishtp_host_dma_tx_buf)
+		dev->ishtp_host_dma_tx_buf_phys = h;
+
+	dev->ishtp_dma_num_slots = dev->ishtp_host_dma_tx_buf_size /
+						DMA_SLOT_SIZE;
+
+	dev->ishtp_dma_tx_map = kcalloc(dev->ishtp_dma_num_slots,
+					sizeof(uint8_t),
+					GFP_KERNEL);
+	spin_lock_init(&dev->ishtp_dma_tx_lock);
+
+	/* Allocate Rx buffer */
+	dev->ishtp_host_dma_rx_buf = dma_alloc_coherent(dev->devc,
+					dev->ishtp_host_dma_rx_buf_size,
+					 &h, GFP_KERNEL);
+
+	if (dev->ishtp_host_dma_rx_buf)
+		dev->ishtp_host_dma_rx_buf_phys = h;
+}
+
+/**
+ * ishtp_cl_free_dma_buf() - Free DMA RX and TX buffer
+ * @dev: ishtp device
+ *
+ * Free DMA buffer when all clients are released. This is
+ * only happens during error path in ISH built in driver
+ * model
+ */
+void	ishtp_cl_free_dma_buf(struct ishtp_device *dev)
+{
+	dma_addr_t	h;
+
+	if (dev->ishtp_host_dma_tx_buf) {
+		h = dev->ishtp_host_dma_tx_buf_phys;
+		dma_free_coherent(dev->devc, dev->ishtp_host_dma_tx_buf_size,
+				  dev->ishtp_host_dma_tx_buf, h);
+	}
+
+	if (dev->ishtp_host_dma_rx_buf) {
+		h = dev->ishtp_host_dma_rx_buf_phys;
+		dma_free_coherent(dev->devc, dev->ishtp_host_dma_rx_buf_size,
+				  dev->ishtp_host_dma_rx_buf, h);
+	}
+
+	kfree(dev->ishtp_dma_tx_map);
+	dev->ishtp_host_dma_tx_buf = NULL;
+	dev->ishtp_host_dma_rx_buf = NULL;
+	dev->ishtp_dma_tx_map = NULL;
+}
+
+/*
+ * ishtp_cl_get_dma_send_buf() - Get a DMA memory slot
+ * @dev:	ishtp device
+ * @size:	Size of memory to get
+ *
+ * Find and return free address of "size" bytes in dma tx buffer.
+ * the function will mark this address as "in-used" memory.
+ *
+ * Return: NULL when no free buffer else a buffer to copy
+ */
+void *ishtp_cl_get_dma_send_buf(struct ishtp_device *dev,
+				uint32_t size)
+{
+	unsigned long	flags;
+	int i, j, free;
+	/* additional slot is needed if there is rem */
+	int required_slots = (size / DMA_SLOT_SIZE)
+		+ 1 * (size % DMA_SLOT_SIZE != 0);
+
+	spin_lock_irqsave(&dev->ishtp_dma_tx_lock, flags);
+	for (i = 0; i <= (dev->ishtp_dma_num_slots - required_slots); i++) {
+		free = 1;
+		for (j = 0; j < required_slots; j++)
+			if (dev->ishtp_dma_tx_map[i+j]) {
+				free = 0;
+				i += j;
+				break;
+			}
+		if (free) {
+			/* mark memory as "caught" */
+			for (j = 0; j < required_slots; j++)
+				dev->ishtp_dma_tx_map[i+j] = 1;
+			spin_unlock_irqrestore(&dev->ishtp_dma_tx_lock, flags);
+			return (i * DMA_SLOT_SIZE) +
+				(unsigned char *)dev->ishtp_host_dma_tx_buf;
+		}
+	}
+	spin_unlock_irqrestore(&dev->ishtp_dma_tx_lock, flags);
+	dev_err(dev->devc, "No free DMA buffer to send msg\n");
+	return NULL;
+}
+
+/*
+ * ishtp_cl_release_dma_acked_mem() - Release DMA memory slot
+ * @dev:	ishtp device
+ * @msg_addr:	message address of slot
+ * @size:	Size of memory to get
+ *
+ * Release_dma_acked_mem - returnes the acked memory to free list.
+ * (from msg_addr, size bytes long)
+ */
+void ishtp_cl_release_dma_acked_mem(struct ishtp_device *dev,
+				    void *msg_addr,
+				    uint8_t size)
+{
+	unsigned long	flags;
+	int acked_slots = (size / DMA_SLOT_SIZE)
+		+ 1 * (size % DMA_SLOT_SIZE != 0);
+	int i, j;
+
+	if ((msg_addr - dev->ishtp_host_dma_tx_buf) % DMA_SLOT_SIZE) {
+		dev_err(dev->devc, "Bad DMA Tx ack address\n");
+		return;
+	}
+
+	i = (msg_addr - dev->ishtp_host_dma_tx_buf) / DMA_SLOT_SIZE;
+	spin_lock_irqsave(&dev->ishtp_dma_tx_lock, flags);
+	for (j = 0; j < acked_slots; j++) {
+		if ((i + j) >= dev->ishtp_dma_num_slots ||
+					!dev->ishtp_dma_tx_map[i+j]) {
+			/* no such slot, or memory is already free */
+			spin_unlock_irqrestore(&dev->ishtp_dma_tx_lock, flags);
+			dev_err(dev->devc, "Bad DMA Tx ack address\n");
+			return;
+		}
+		dev->ishtp_dma_tx_map[i+j] = 0;
+	}
+	spin_unlock_irqrestore(&dev->ishtp_dma_tx_lock, flags);
+}
diff --git a/drivers/hid/intel-ish-hid/ishtp/hbm.c b/drivers/hid/intel-ish-hid/ishtp/hbm.c
new file mode 100644
index 000000000000..74bffee60774
--- /dev/null
+++ b/drivers/hid/intel-ish-hid/ishtp/hbm.c
@@ -0,0 +1,1032 @@
+/*
+ * ISHTP bus layer messages handling
+ *
+ * Copyright (c) 2003-2016, 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/export.h>
+#include <linux/slab.h>
+#include <linux/sched.h>
+#include <linux/wait.h>
+#include <linux/spinlock.h>
+#include <linux/miscdevice.h>
+#include "ishtp-dev.h"
+#include "hbm.h"
+#include "client.h"
+
+/**
+ * ishtp_hbm_fw_cl_allocate() - Allocate FW clients
+ * @dev: ISHTP device instance
+ *
+ * Allocates storage for fw clients
+ */
+static void ishtp_hbm_fw_cl_allocate(struct ishtp_device *dev)
+{
+	struct ishtp_fw_client *clients;
+	int b;
+
+	/* count how many ISH clients we have */
+	for_each_set_bit(b, dev->fw_clients_map, ISHTP_CLIENTS_MAX)
+		dev->fw_clients_num++;
+
+	if (dev->fw_clients_num <= 0)
+		return;
+
+	/* allocate storage for fw clients representation */
+	clients = kcalloc(dev->fw_clients_num, sizeof(struct ishtp_fw_client),
+			  GFP_KERNEL);
+	if (!clients) {
+		dev->dev_state = ISHTP_DEV_RESETTING;
+		ish_hw_reset(dev);
+		return;
+	}
+	dev->fw_clients = clients;
+}
+
+/**
+ * ishtp_hbm_cl_hdr() - construct client hbm header
+ * @cl: client
+ * @hbm_cmd: host bus message command
+ * @buf: buffer for cl header
+ * @len: buffer length
+ *
+ * Initialize HBM buffer
+ */
+static inline void ishtp_hbm_cl_hdr(struct ishtp_cl *cl, uint8_t hbm_cmd,
+	void *buf, size_t len)
+{
+	struct ishtp_hbm_cl_cmd *cmd = buf;
+
+	memset(cmd, 0, len);
+
+	cmd->hbm_cmd = hbm_cmd;
+	cmd->host_addr = cl->host_client_id;
+	cmd->fw_addr = cl->fw_client_id;
+}
+
+/**
+ * ishtp_hbm_cl_addr_equal() - Compare client address
+ * @cl: client
+ * @buf: Client command buffer
+ *
+ * Compare client address with the address in command buffer
+ *
+ * Return: True if they have the same address
+ */
+static inline bool ishtp_hbm_cl_addr_equal(struct ishtp_cl *cl, void *buf)
+{
+	struct ishtp_hbm_cl_cmd *cmd = buf;
+
+	return cl->host_client_id == cmd->host_addr &&
+		cl->fw_client_id == cmd->fw_addr;
+}
+
+/**
+ * ishtp_hbm_start_wait() - Wait for HBM start message
+ * @dev: ISHTP device instance
+ *
+ * Wait for HBM start message from firmware
+ *
+ * Return: 0 if HBM start is/was received else timeout error
+ */
+int ishtp_hbm_start_wait(struct ishtp_device *dev)
+{
+	int ret;
+
+	if (dev->hbm_state > ISHTP_HBM_START)
+		return 0;
+
+	dev_dbg(dev->devc, "Going to wait for ishtp start. hbm_state=%08X\n",
+		dev->hbm_state);
+	ret = wait_event_interruptible_timeout(dev->wait_hbm_recvd_msg,
+					dev->hbm_state >= ISHTP_HBM_STARTED,
+					(ISHTP_INTEROP_TIMEOUT * HZ));
+
+	dev_dbg(dev->devc,
+		"Woke up from waiting for ishtp start. hbm_state=%08X\n",
+		dev->hbm_state);
+
+	if (ret <= 0 && (dev->hbm_state <= ISHTP_HBM_START)) {
+		dev->hbm_state = ISHTP_HBM_IDLE;
+		dev_err(dev->devc,
+		"waiting for ishtp start failed. ret=%d hbm_state=%08X\n",
+			ret, dev->hbm_state);
+		return -ETIMEDOUT;
+	}
+	return 0;
+}
+
+/**
+ * ishtp_hbm_start_req() - Send HBM start message
+ * @dev: ISHTP device instance
+ *
+ * Send HBM start message to firmware
+ *
+ * Return: 0 if success else error code
+ */
+int ishtp_hbm_start_req(struct ishtp_device *dev)
+{
+	struct ishtp_msg_hdr hdr;
+	unsigned char data[128];
+	struct ishtp_msg_hdr *ishtp_hdr = &hdr;
+	struct hbm_host_version_request *start_req;
+	const size_t len = sizeof(struct hbm_host_version_request);
+
+	ishtp_hbm_hdr(ishtp_hdr, len);
+
+	/* host start message */
+	start_req = (struct hbm_host_version_request *)data;
+	memset(start_req, 0, len);
+	start_req->hbm_cmd = HOST_START_REQ_CMD;
+	start_req->host_version.major_version = HBM_MAJOR_VERSION;
+	start_req->host_version.minor_version = HBM_MINOR_VERSION;
+
+	/*
+	 * (!) Response to HBM start may be so quick that this thread would get
+	 * preempted BEFORE managing to set hbm_state = ISHTP_HBM_START.
+	 * So set it at first, change back to ISHTP_HBM_IDLE upon failure
+	 */
+	dev->hbm_state = ISHTP_HBM_START;
+	if (ishtp_write_message(dev, ishtp_hdr, data)) {
+		dev_err(dev->devc, "version message send failed\n");
+		dev->dev_state = ISHTP_DEV_RESETTING;
+		dev->hbm_state = ISHTP_HBM_IDLE;
+		ish_hw_reset(dev);
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+/**
+ * ishtp_hbm_enum_clients_req() - Send client enum req
+ * @dev: ISHTP device instance
+ *
+ * Send enumeration client request message
+ *
+ * Return: 0 if success else error code
+ */
+void ishtp_hbm_enum_clients_req(struct ishtp_device *dev)
+{
+	struct ishtp_msg_hdr hdr;
+	unsigned char data[128];
+	struct ishtp_msg_hdr *ishtp_hdr = &hdr;
+	struct hbm_host_enum_request *enum_req;
+	const size_t len = sizeof(struct hbm_host_enum_request);
+
+	/* enumerate clients */
+	ishtp_hbm_hdr(ishtp_hdr, len);
+
+	enum_req = (struct hbm_host_enum_request *)data;
+	memset(enum_req, 0, len);
+	enum_req->hbm_cmd = HOST_ENUM_REQ_CMD;
+
+	if (ishtp_write_message(dev, ishtp_hdr, data)) {
+		dev->dev_state = ISHTP_DEV_RESETTING;
+		dev_err(dev->devc, "enumeration request send failed\n");
+		ish_hw_reset(dev);
+	}
+	dev->hbm_state = ISHTP_HBM_ENUM_CLIENTS;
+}
+
+/**
+ * ishtp_hbm_prop_req() - Request property
+ * @dev: ISHTP device instance
+ *
+ * Request property for a single client
+ *
+ * Return: 0 if success else error code
+ */
+static int ishtp_hbm_prop_req(struct ishtp_device *dev)
+{
+
+	struct ishtp_msg_hdr hdr;
+	unsigned char data[128];
+	struct ishtp_msg_hdr *ishtp_hdr = &hdr;
+	struct hbm_props_request *prop_req;
+	const size_t len = sizeof(struct hbm_props_request);
+	unsigned long next_client_index;
+	uint8_t client_num;
+
+	client_num = dev->fw_client_presentation_num;
+
+	next_client_index = find_next_bit(dev->fw_clients_map,
+		ISHTP_CLIENTS_MAX, dev->fw_client_index);
+
+	/* We got all client properties */
+	if (next_client_index == ISHTP_CLIENTS_MAX) {
+		dev->hbm_state = ISHTP_HBM_WORKING;
+		dev->dev_state = ISHTP_DEV_ENABLED;
+
+		for (dev->fw_client_presentation_num = 1;
+			dev->fw_client_presentation_num < client_num + 1;
+				++dev->fw_client_presentation_num)
+			/* Add new client device */
+			ishtp_bus_new_client(dev);
+		return 0;
+	}
+
+	dev->fw_clients[client_num].client_id = next_client_index;
+
+	ishtp_hbm_hdr(ishtp_hdr, len);
+	prop_req = (struct hbm_props_request *)data;
+
+	memset(prop_req, 0, sizeof(struct hbm_props_request));
+
+	prop_req->hbm_cmd = HOST_CLIENT_PROPERTIES_REQ_CMD;
+	prop_req->address = next_client_index;
+
+	if (ishtp_write_message(dev, ishtp_hdr, data)) {
+		dev->dev_state = ISHTP_DEV_RESETTING;
+		dev_err(dev->devc, "properties request send failed\n");
+		ish_hw_reset(dev);
+		return -EIO;
+	}
+
+	dev->fw_client_index = next_client_index;
+
+	return 0;
+}
+
+/**
+ * ishtp_hbm_stop_req() - Send HBM stop
+ * @dev: ISHTP device instance
+ *
+ * Send stop request message
+ */
+static void ishtp_hbm_stop_req(struct ishtp_device *dev)
+{
+	struct ishtp_msg_hdr hdr;
+	unsigned char data[128];
+	struct ishtp_msg_hdr *ishtp_hdr = &hdr;
+	struct hbm_host_stop_request *req;
+	const size_t len = sizeof(struct hbm_host_stop_request);
+
+	ishtp_hbm_hdr(ishtp_hdr, len);
+	req = (struct hbm_host_stop_request *)data;
+
+	memset(req, 0, sizeof(struct hbm_host_stop_request));
+	req->hbm_cmd = HOST_STOP_REQ_CMD;
+	req->reason = DRIVER_STOP_REQUEST;
+
+	ishtp_write_message(dev, ishtp_hdr, data);
+}
+
+/**
+ * ishtp_hbm_cl_flow_control_req() - Send flow control request
+ * @dev: ISHTP device instance
+ * @cl: ISHTP client instance
+ *
+ * Send flow control request
+ *
+ * Return: 0 if success else error code
+ */
+int ishtp_hbm_cl_flow_control_req(struct ishtp_device *dev,
+				  struct ishtp_cl *cl)
+{
+	struct ishtp_msg_hdr hdr;
+	unsigned char data[128];
+	struct ishtp_msg_hdr *ishtp_hdr = &hdr;
+	const size_t len = sizeof(struct hbm_flow_control);
+	int	rv;
+	unsigned int	num_frags;
+	unsigned long	flags;
+
+	spin_lock_irqsave(&cl->fc_spinlock, flags);
+	ishtp_hbm_hdr(ishtp_hdr, len);
+	ishtp_hbm_cl_hdr(cl, ISHTP_FLOW_CONTROL_CMD, data, len);
+
+	/*
+	 * Sync possible race when RB recycle and packet receive paths
+	 * both try to send an out FC
+	 */
+	if (cl->out_flow_ctrl_creds) {
+		spin_unlock_irqrestore(&cl->fc_spinlock, flags);
+		return	0;
+	}
+
+	num_frags = cl->recv_msg_num_frags;
+	cl->recv_msg_num_frags = 0;
+
+	rv = ishtp_write_message(dev, ishtp_hdr, data);
+	if (!rv) {
+		++cl->out_flow_ctrl_creds;
+		++cl->out_flow_ctrl_cnt;
+		getnstimeofday(&cl->ts_out_fc);
+		if (cl->ts_rx.tv_sec && cl->ts_rx.tv_nsec) {
+			struct timespec ts_diff;
+
+			ts_diff = timespec_sub(cl->ts_out_fc, cl->ts_rx);
+			if (timespec_compare(&ts_diff, &cl->ts_max_fc_delay)
+					> 0)
+				cl->ts_max_fc_delay = ts_diff;
+		}
+	} else {
+		++cl->err_send_fc;
+	}
+
+	spin_unlock_irqrestore(&cl->fc_spinlock, flags);
+	return	rv;
+}
+
+/**
+ * ishtp_hbm_cl_disconnect_req() - Send disconnect request
+ * @dev: ISHTP device instance
+ * @cl: ISHTP client instance
+ *
+ * Send disconnect message to fw
+ *
+ * Return: 0 if success else error code
+ */
+int ishtp_hbm_cl_disconnect_req(struct ishtp_device *dev, struct ishtp_cl *cl)
+{
+	struct ishtp_msg_hdr hdr;
+	unsigned char data[128];
+	struct ishtp_msg_hdr *ishtp_hdr = &hdr;
+	const size_t len = sizeof(struct hbm_client_connect_request);
+
+	ishtp_hbm_hdr(ishtp_hdr, len);
+	ishtp_hbm_cl_hdr(cl, CLIENT_DISCONNECT_REQ_CMD, data, len);
+
+	return ishtp_write_message(dev, ishtp_hdr, data);
+}
+
+/**
+ * ishtp_hbm_cl_disconnect_res() - Get disconnect response
+ * @dev: ISHTP device instance
+ * @rs: Response message
+ *
+ * Received disconnect response from fw
+ */
+static void ishtp_hbm_cl_disconnect_res(struct ishtp_device *dev,
+	struct hbm_client_connect_response *rs)
+{
+	struct ishtp_cl *cl = NULL;
+	unsigned long	flags;
+
+	spin_lock_irqsave(&dev->cl_list_lock, flags);
+	list_for_each_entry(cl, &dev->cl_list, link) {
+		if (!rs->status && ishtp_hbm_cl_addr_equal(cl, rs)) {
+			cl->state = ISHTP_CL_DISCONNECTED;
+			break;
+		}
+	}
+	if (cl)
+		wake_up_interruptible(&cl->wait_ctrl_res);
+	spin_unlock_irqrestore(&dev->cl_list_lock, flags);
+}
+
+/**
+ * ishtp_hbm_cl_connect_req() - Send connect request
+ * @dev: ISHTP device instance
+ * @cl: client device instance
+ *
+ * Send connection request to specific fw client
+ *
+ * Return: 0 if success else error code
+ */
+int ishtp_hbm_cl_connect_req(struct ishtp_device *dev, struct ishtp_cl *cl)
+{
+	struct ishtp_msg_hdr hdr;
+	unsigned char data[128];
+	struct ishtp_msg_hdr *ishtp_hdr = &hdr;
+	const size_t len = sizeof(struct hbm_client_connect_request);
+
+	ishtp_hbm_hdr(ishtp_hdr, len);
+	ishtp_hbm_cl_hdr(cl, CLIENT_CONNECT_REQ_CMD, data, len);
+
+	return ishtp_write_message(dev, ishtp_hdr, data);
+}
+
+/**
+ * ishtp_hbm_cl_connect_res() - Get connect response
+ * @dev: ISHTP device instance
+ * @rs: Response message
+ *
+ * Received connect response from fw
+ */
+static void ishtp_hbm_cl_connect_res(struct ishtp_device *dev,
+	struct hbm_client_connect_response *rs)
+{
+	struct ishtp_cl *cl = NULL;
+	unsigned long	flags;
+
+	spin_lock_irqsave(&dev->cl_list_lock, flags);
+	list_for_each_entry(cl, &dev->cl_list, link) {
+		if (ishtp_hbm_cl_addr_equal(cl, rs)) {
+			if (!rs->status) {
+				cl->state = ISHTP_CL_CONNECTED;
+				cl->status = 0;
+			} else {
+				cl->state = ISHTP_CL_DISCONNECTED;
+				cl->status = -ENODEV;
+			}
+			break;
+		}
+	}
+	if (cl)
+		wake_up_interruptible(&cl->wait_ctrl_res);
+	spin_unlock_irqrestore(&dev->cl_list_lock, flags);
+}
+
+/**
+ * ishtp_client_disconnect_request() - Receive disconnect request
+ * @dev: ISHTP device instance
+ * @disconnect_req: disconnect request structure
+ *
+ * Disconnect request bus message from the fw. Send diconnect response.
+ */
+static void ishtp_hbm_fw_disconnect_req(struct ishtp_device *dev,
+	struct hbm_client_connect_request *disconnect_req)
+{
+	struct ishtp_cl *cl;
+	const size_t len = sizeof(struct hbm_client_connect_response);
+	unsigned long	flags;
+	struct ishtp_msg_hdr hdr;
+	unsigned char data[4];	/* All HBM messages are 4 bytes */
+
+	spin_lock_irqsave(&dev->cl_list_lock, flags);
+	list_for_each_entry(cl, &dev->cl_list, link) {
+		if (ishtp_hbm_cl_addr_equal(cl, disconnect_req)) {
+			cl->state = ISHTP_CL_DISCONNECTED;
+
+			/* send disconnect response */
+			ishtp_hbm_hdr(&hdr, len);
+			ishtp_hbm_cl_hdr(cl, CLIENT_DISCONNECT_RES_CMD, data,
+				len);
+			ishtp_write_message(dev, &hdr, data);
+			break;
+		}
+	}
+	spin_unlock_irqrestore(&dev->cl_list_lock, flags);
+}
+
+/**
+ * ishtp_hbm_dma_xfer_ack(() - Receive transfer ACK
+ * @dev: ISHTP device instance
+ * @dma_xfer: HBM transfer message
+ *
+ * Receive ack for ISHTP-over-DMA client message
+ */
+static void ishtp_hbm_dma_xfer_ack(struct ishtp_device *dev,
+				   struct dma_xfer_hbm *dma_xfer)
+{
+	void	*msg;
+	uint64_t	offs;
+	struct ishtp_msg_hdr	*ishtp_hdr =
+		(struct ishtp_msg_hdr *)&dev->ishtp_msg_hdr;
+	unsigned int	msg_offs;
+	struct ishtp_cl *cl;
+
+	for (msg_offs = 0; msg_offs < ishtp_hdr->length;
+		msg_offs += sizeof(struct dma_xfer_hbm)) {
+		offs = dma_xfer->msg_addr - dev->ishtp_host_dma_tx_buf_phys;
+		if (offs > dev->ishtp_host_dma_tx_buf_size) {
+			dev_err(dev->devc, "Bad DMA Tx ack message address\n");
+			return;
+		}
+		if (dma_xfer->msg_length >
+				dev->ishtp_host_dma_tx_buf_size - offs) {
+			dev_err(dev->devc, "Bad DMA Tx ack message size\n");
+			return;
+		}
+
+		/* logical address of the acked mem */
+		msg = (unsigned char *)dev->ishtp_host_dma_tx_buf + offs;
+		ishtp_cl_release_dma_acked_mem(dev, msg, dma_xfer->msg_length);
+
+		list_for_each_entry(cl, &dev->cl_list, link) {
+			if (cl->fw_client_id == dma_xfer->fw_client_id &&
+			    cl->host_client_id == dma_xfer->host_client_id)
+				/*
+				 * in case that a single ack may be sent
+				 * over several dma transfers, and the last msg
+				 * addr was inside the acked memory, but not in
+				 * its start
+				 */
+				if (cl->last_dma_addr >=
+							(unsigned char *)msg &&
+						cl->last_dma_addr <
+						(unsigned char *)msg +
+						dma_xfer->msg_length) {
+					cl->last_dma_acked = 1;
+
+					if (!list_empty(&cl->tx_list.list) &&
+						cl->ishtp_flow_ctrl_creds) {
+						/*
+						 * start sending the first msg
+						 */
+						ishtp_cl_send_msg(dev, cl);
+					}
+				}
+		}
+		++dma_xfer;
+	}
+}
+
+/**
+ * ishtp_hbm_dma_xfer() - Receive DMA transfer message
+ * @dev: ISHTP device instance
+ * @dma_xfer: HBM transfer message
+ *
+ * Receive ISHTP-over-DMA client message
+ */
+static void ishtp_hbm_dma_xfer(struct ishtp_device *dev,
+			       struct dma_xfer_hbm *dma_xfer)
+{
+	void	*msg;
+	uint64_t	offs;
+	struct ishtp_msg_hdr	hdr;
+	struct ishtp_msg_hdr	*ishtp_hdr =
+		(struct ishtp_msg_hdr *) &dev->ishtp_msg_hdr;
+	struct dma_xfer_hbm	*prm = dma_xfer;
+	unsigned int	msg_offs;
+
+	for (msg_offs = 0; msg_offs < ishtp_hdr->length;
+		msg_offs += sizeof(struct dma_xfer_hbm)) {
+
+		offs = dma_xfer->msg_addr - dev->ishtp_host_dma_rx_buf_phys;
+		if (offs > dev->ishtp_host_dma_rx_buf_size) {
+			dev_err(dev->devc, "Bad DMA Rx message address\n");
+			return;
+		}
+		if (dma_xfer->msg_length >
+				dev->ishtp_host_dma_rx_buf_size - offs) {
+			dev_err(dev->devc, "Bad DMA Rx message size\n");
+			return;
+		}
+		msg = dev->ishtp_host_dma_rx_buf + offs;
+		recv_ishtp_cl_msg_dma(dev, msg, dma_xfer);
+		dma_xfer->hbm = DMA_XFER_ACK;	/* Prepare for response */
+		++dma_xfer;
+	}
+
+	/* Send DMA_XFER_ACK [...] */
+	ishtp_hbm_hdr(&hdr, ishtp_hdr->length);
+	ishtp_write_message(dev, &hdr, (unsigned char *)prm);
+}
+
+/**
+ * ishtp_hbm_dispatch() - HBM dispatch function
+ * @dev: ISHTP device instance
+ * @hdr: bus message
+ *
+ * Bottom half read routine after ISR to handle the read bus message cmd
+ * processing
+ */
+void ishtp_hbm_dispatch(struct ishtp_device *dev,
+			struct ishtp_bus_message *hdr)
+{
+	struct ishtp_bus_message *ishtp_msg;
+	struct ishtp_fw_client *fw_client;
+	struct hbm_host_version_response *version_res;
+	struct hbm_client_connect_response *connect_res;
+	struct hbm_client_connect_response *disconnect_res;
+	struct hbm_client_connect_request *disconnect_req;
+	struct hbm_props_response *props_res;
+	struct hbm_host_enum_response *enum_res;
+	struct ishtp_msg_hdr ishtp_hdr;
+	struct dma_alloc_notify	dma_alloc_notify;
+	struct dma_xfer_hbm	*dma_xfer;
+
+	ishtp_msg = hdr;
+
+	switch (ishtp_msg->hbm_cmd) {
+	case HOST_START_RES_CMD:
+		version_res = (struct hbm_host_version_response *)ishtp_msg;
+		if (!version_res->host_version_supported) {
+			dev->version = version_res->fw_max_version;
+
+			dev->hbm_state = ISHTP_HBM_STOPPED;
+			ishtp_hbm_stop_req(dev);
+			return;
+		}
+
+		dev->version.major_version = HBM_MAJOR_VERSION;
+		dev->version.minor_version = HBM_MINOR_VERSION;
+		if (dev->dev_state == ISHTP_DEV_INIT_CLIENTS &&
+				dev->hbm_state == ISHTP_HBM_START) {
+			dev->hbm_state = ISHTP_HBM_STARTED;
+			ishtp_hbm_enum_clients_req(dev);
+		} else {
+			dev_err(dev->devc,
+				"reset: wrong host start response\n");
+			/* BUG: why do we arrive here? */
+			ish_hw_reset(dev);
+			return;
+		}
+
+		wake_up_interruptible(&dev->wait_hbm_recvd_msg);
+		break;
+
+	case CLIENT_CONNECT_RES_CMD:
+		connect_res = (struct hbm_client_connect_response *)ishtp_msg;
+		ishtp_hbm_cl_connect_res(dev, connect_res);
+		break;
+
+	case CLIENT_DISCONNECT_RES_CMD:
+		disconnect_res =
+			(struct hbm_client_connect_response *)ishtp_msg;
+		ishtp_hbm_cl_disconnect_res(dev, disconnect_res);
+		break;
+
+	case HOST_CLIENT_PROPERTIES_RES_CMD:
+		props_res = (struct hbm_props_response *)ishtp_msg;
+		fw_client = &dev->fw_clients[dev->fw_client_presentation_num];
+
+		if (props_res->status || !dev->fw_clients) {
+			dev_err(dev->devc,
+			"reset: properties response hbm wrong status\n");
+			ish_hw_reset(dev);
+			return;
+		}
+
+		if (fw_client->client_id != props_res->address) {
+			dev_err(dev->devc,
+				"reset: host properties response address mismatch [%02X %02X]\n",
+				fw_client->client_id, props_res->address);
+			ish_hw_reset(dev);
+			return;
+		}
+
+		if (dev->dev_state != ISHTP_DEV_INIT_CLIENTS ||
+			dev->hbm_state != ISHTP_HBM_CLIENT_PROPERTIES) {
+			dev_err(dev->devc,
+				"reset: unexpected properties response\n");
+			ish_hw_reset(dev);
+			return;
+		}
+
+		fw_client->props = props_res->client_properties;
+		dev->fw_client_index++;
+		dev->fw_client_presentation_num++;
+
+		/* request property for the next client */
+		ishtp_hbm_prop_req(dev);
+
+		if (dev->dev_state != ISHTP_DEV_ENABLED)
+			break;
+
+		if (!ishtp_use_dma_transfer())
+			break;
+
+		dev_dbg(dev->devc, "Requesting to use DMA\n");
+		ishtp_cl_alloc_dma_buf(dev);
+		if (dev->ishtp_host_dma_rx_buf) {
+			const size_t len = sizeof(dma_alloc_notify);
+
+			memset(&dma_alloc_notify, 0, sizeof(dma_alloc_notify));
+			dma_alloc_notify.hbm = DMA_BUFFER_ALLOC_NOTIFY;
+			dma_alloc_notify.buf_size =
+					dev->ishtp_host_dma_rx_buf_size;
+			dma_alloc_notify.buf_address =
+					dev->ishtp_host_dma_rx_buf_phys;
+			ishtp_hbm_hdr(&ishtp_hdr, len);
+			ishtp_write_message(dev, &ishtp_hdr,
+				(unsigned char *)&dma_alloc_notify);
+		}
+
+		break;
+
+	case HOST_ENUM_RES_CMD:
+		enum_res = (struct hbm_host_enum_response *) ishtp_msg;
+		memcpy(dev->fw_clients_map, enum_res->valid_addresses, 32);
+		if (dev->dev_state == ISHTP_DEV_INIT_CLIENTS &&
+			dev->hbm_state == ISHTP_HBM_ENUM_CLIENTS) {
+			dev->fw_client_presentation_num = 0;
+			dev->fw_client_index = 0;
+
+			ishtp_hbm_fw_cl_allocate(dev);
+			dev->hbm_state = ISHTP_HBM_CLIENT_PROPERTIES;
+
+			/* first property request */
+			ishtp_hbm_prop_req(dev);
+		} else {
+			dev_err(dev->devc,
+			      "reset: unexpected enumeration response hbm\n");
+			ish_hw_reset(dev);
+			return;
+		}
+		break;
+
+	case HOST_STOP_RES_CMD:
+		if (dev->hbm_state != ISHTP_HBM_STOPPED)
+			dev_err(dev->devc, "unexpected stop response\n");
+
+		dev->dev_state = ISHTP_DEV_DISABLED;
+		dev_info(dev->devc, "reset: FW stop response\n");
+		ish_hw_reset(dev);
+		break;
+
+	case CLIENT_DISCONNECT_REQ_CMD:
+		/* search for client */
+		disconnect_req =
+			(struct hbm_client_connect_request *)ishtp_msg;
+		ishtp_hbm_fw_disconnect_req(dev, disconnect_req);
+		break;
+
+	case FW_STOP_REQ_CMD:
+		dev->hbm_state = ISHTP_HBM_STOPPED;
+		break;
+
+	case DMA_BUFFER_ALLOC_RESPONSE:
+		dev->ishtp_host_dma_enabled = 1;
+		break;
+
+	case DMA_XFER:
+		dma_xfer = (struct dma_xfer_hbm *)ishtp_msg;
+		if (!dev->ishtp_host_dma_enabled) {
+			dev_err(dev->devc,
+				"DMA XFER requested but DMA is not enabled\n");
+			break;
+		}
+		ishtp_hbm_dma_xfer(dev, dma_xfer);
+		break;
+
+	case DMA_XFER_ACK:
+		dma_xfer = (struct dma_xfer_hbm *)ishtp_msg;
+		if (!dev->ishtp_host_dma_enabled ||
+		    !dev->ishtp_host_dma_tx_buf) {
+			dev_err(dev->devc,
+				"DMA XFER acked but DMA Tx is not enabled\n");
+			break;
+		}
+		ishtp_hbm_dma_xfer_ack(dev, dma_xfer);
+		break;
+
+	default:
+		dev_err(dev->devc, "unknown HBM: %u\n",
+			(unsigned int)ishtp_msg->hbm_cmd);
+
+		break;
+	}
+}
+
+/**
+ * bh_hbm_work_fn() - HBM work function
+ * @work: work struct
+ *
+ * Bottom half processing work function (instead of thread handler)
+ * for processing hbm messages
+ */
+void	bh_hbm_work_fn(struct work_struct *work)
+{
+	unsigned long	flags;
+	struct ishtp_device	*dev;
+	unsigned char	hbm[IPC_PAYLOAD_SIZE];
+
+	dev = container_of(work, struct ishtp_device, bh_hbm_work);
+	spin_lock_irqsave(&dev->rd_msg_spinlock, flags);
+	if (dev->rd_msg_fifo_head != dev->rd_msg_fifo_tail) {
+		memcpy(hbm, dev->rd_msg_fifo + dev->rd_msg_fifo_head,
+			IPC_PAYLOAD_SIZE);
+		dev->rd_msg_fifo_head =
+			(dev->rd_msg_fifo_head + IPC_PAYLOAD_SIZE) %
+			(RD_INT_FIFO_SIZE * IPC_PAYLOAD_SIZE);
+		spin_unlock_irqrestore(&dev->rd_msg_spinlock, flags);
+		ishtp_hbm_dispatch(dev, (struct ishtp_bus_message *)hbm);
+	} else {
+		spin_unlock_irqrestore(&dev->rd_msg_spinlock, flags);
+	}
+}
+
+/**
+ * recv_hbm() - Receive HBM message
+ * @dev: ISHTP device instance
+ * @ishtp_hdr: received bus message
+ *
+ * Receive and process ISHTP bus messages in ISR context. This will schedule
+ * work function to process message
+ */
+void	recv_hbm(struct ishtp_device *dev, struct ishtp_msg_hdr *ishtp_hdr)
+{
+	uint8_t	rd_msg_buf[ISHTP_RD_MSG_BUF_SIZE];
+	struct ishtp_bus_message	*ishtp_msg =
+		(struct ishtp_bus_message *)rd_msg_buf;
+	unsigned long	flags;
+
+	dev->ops->ishtp_read(dev, rd_msg_buf, ishtp_hdr->length);
+
+	/* Flow control - handle in place */
+	if (ishtp_msg->hbm_cmd == ISHTP_FLOW_CONTROL_CMD) {
+		struct hbm_flow_control *flow_control =
+			(struct hbm_flow_control *)ishtp_msg;
+		struct ishtp_cl *cl = NULL;
+		unsigned long	flags, tx_flags;
+
+		spin_lock_irqsave(&dev->cl_list_lock, flags);
+		list_for_each_entry(cl, &dev->cl_list, link) {
+			if (cl->host_client_id == flow_control->host_addr &&
+					cl->fw_client_id ==
+					flow_control->fw_addr) {
+				/*
+				 * NOTE: It's valid only for counting
+				 * flow-control implementation to receive a
+				 * FC in the middle of sending. Meanwhile not
+				 * supported
+				 */
+				if (cl->ishtp_flow_ctrl_creds)
+					dev_err(dev->devc,
+					 "recv extra FC from FW client %u (host client %u) (FC count was %d)\n",
+					 (unsigned int)cl->fw_client_id,
+					 (unsigned int)cl->host_client_id,
+					 cl->ishtp_flow_ctrl_creds);
+				else {
+					++cl->ishtp_flow_ctrl_creds;
+					++cl->ishtp_flow_ctrl_cnt;
+					cl->last_ipc_acked = 1;
+					spin_lock_irqsave(
+							&cl->tx_list_spinlock,
+							tx_flags);
+					if (!list_empty(&cl->tx_list.list)) {
+						/*
+						 * start sending the first msg
+						 *	= the callback function
+						 */
+						spin_unlock_irqrestore(
+							&cl->tx_list_spinlock,
+							tx_flags);
+						ishtp_cl_send_msg(dev, cl);
+					} else {
+						spin_unlock_irqrestore(
+							&cl->tx_list_spinlock,
+							tx_flags);
+					}
+				}
+				break;
+			}
+		}
+		spin_unlock_irqrestore(&dev->cl_list_lock, flags);
+		goto	eoi;
+	}
+
+	/*
+	 * Some messages that are safe for ISR processing and important
+	 * to be done "quickly" and in-order, go here
+	 */
+	if (ishtp_msg->hbm_cmd == CLIENT_CONNECT_RES_CMD ||
+			ishtp_msg->hbm_cmd == CLIENT_DISCONNECT_RES_CMD ||
+			ishtp_msg->hbm_cmd == CLIENT_DISCONNECT_REQ_CMD ||
+			ishtp_msg->hbm_cmd == DMA_XFER) {
+		ishtp_hbm_dispatch(dev, ishtp_msg);
+		goto	eoi;
+	}
+
+	/*
+	 * All other HBMs go here.
+	 * We schedule HBMs for processing serially by using system wq,
+	 * possibly there will be multiple HBMs scheduled at the same time.
+	 */
+	spin_lock_irqsave(&dev->rd_msg_spinlock, flags);
+	if ((dev->rd_msg_fifo_tail + IPC_PAYLOAD_SIZE) %
+			(RD_INT_FIFO_SIZE * IPC_PAYLOAD_SIZE) ==
+			dev->rd_msg_fifo_head) {
+		spin_unlock_irqrestore(&dev->rd_msg_spinlock, flags);
+		dev_err(dev->devc, "BH buffer overflow, dropping HBM %u\n",
+			(unsigned int)ishtp_msg->hbm_cmd);
+		goto	eoi;
+	}
+	memcpy(dev->rd_msg_fifo + dev->rd_msg_fifo_tail, ishtp_msg,
+		ishtp_hdr->length);
+	dev->rd_msg_fifo_tail = (dev->rd_msg_fifo_tail + IPC_PAYLOAD_SIZE) %
+		(RD_INT_FIFO_SIZE * IPC_PAYLOAD_SIZE);
+	spin_unlock_irqrestore(&dev->rd_msg_spinlock, flags);
+	schedule_work(&dev->bh_hbm_work);
+eoi:
+	return;
+}
+
+/**
+ * recv_fixed_cl_msg() - Receive fixed client message
+ * @dev: ISHTP device instance
+ * @ishtp_hdr: received bus message
+ *
+ * Receive and process ISHTP fixed client messages (address == 0)
+ * in ISR context
+ */
+void recv_fixed_cl_msg(struct ishtp_device *dev,
+	struct ishtp_msg_hdr *ishtp_hdr)
+{
+	uint8_t rd_msg_buf[ISHTP_RD_MSG_BUF_SIZE];
+
+	dev->print_log(dev,
+		"%s() got fixed client msg from client #%d\n",
+		__func__, ishtp_hdr->fw_addr);
+	dev->ops->ishtp_read(dev, rd_msg_buf, ishtp_hdr->length);
+	if (ishtp_hdr->fw_addr == ISHTP_SYSTEM_STATE_CLIENT_ADDR) {
+		struct ish_system_states_header *msg_hdr =
+			(struct ish_system_states_header *)rd_msg_buf;
+		if (msg_hdr->cmd == SYSTEM_STATE_SUBSCRIBE)
+			ishtp_send_resume(dev);
+		/* if FW request arrived here, the system is not suspended */
+		else
+			dev_err(dev->devc, "unknown fixed client msg [%02X]\n",
+				msg_hdr->cmd);
+	}
+}
+
+/**
+ * fix_cl_hdr() - Initialize fixed client header
+ * @hdr: message header
+ * @length: length of message
+ * @cl_addr: Client address
+ *
+ * Initialize message header for fixed client
+ */
+static inline void fix_cl_hdr(struct ishtp_msg_hdr *hdr, size_t length,
+	uint8_t cl_addr)
+{
+	hdr->host_addr = 0;
+	hdr->fw_addr = cl_addr;
+	hdr->length = length;
+	hdr->msg_complete = 1;
+	hdr->reserved = 0;
+}
+
+/*** Suspend and resume notification ***/
+
+static uint32_t current_state;
+static uint32_t supported_states = 0 | SUSPEND_STATE_BIT;
+
+/**
+ * ishtp_send_suspend() - Send suspend message to FW
+ * @dev: ISHTP device instance
+ *
+ * Send suspend message to FW. This is useful for system freeze (non S3) case
+ */
+void ishtp_send_suspend(struct ishtp_device *dev)
+{
+	struct ishtp_msg_hdr	ishtp_hdr;
+	struct ish_system_states_status state_status_msg;
+	const size_t len = sizeof(struct ish_system_states_status);
+
+	fix_cl_hdr(&ishtp_hdr, len, ISHTP_SYSTEM_STATE_CLIENT_ADDR);
+
+	memset(&state_status_msg, 0, len);
+	state_status_msg.hdr.cmd = SYSTEM_STATE_STATUS;
+	state_status_msg.supported_states = supported_states;
+	current_state |= SUSPEND_STATE_BIT;
+	dev->print_log(dev, "%s() sends SUSPEND notification\n", __func__);
+	state_status_msg.states_status = current_state;
+
+	ishtp_write_message(dev, &ishtp_hdr,
+		(unsigned char *)&state_status_msg);
+}
+EXPORT_SYMBOL(ishtp_send_suspend);
+
+/**
+ * ishtp_send_resume() - Send resume message to FW
+ * @dev: ISHTP device instance
+ *
+ * Send resume message to FW. This is useful for system freeze (non S3) case
+ */
+void ishtp_send_resume(struct ishtp_device *dev)
+{
+	struct ishtp_msg_hdr	ishtp_hdr;
+	struct ish_system_states_status state_status_msg;
+	const size_t len = sizeof(struct ish_system_states_status);
+
+	fix_cl_hdr(&ishtp_hdr, len, ISHTP_SYSTEM_STATE_CLIENT_ADDR);
+
+	memset(&state_status_msg, 0, len);
+	state_status_msg.hdr.cmd = SYSTEM_STATE_STATUS;
+	state_status_msg.supported_states = supported_states;
+	current_state &= ~SUSPEND_STATE_BIT;
+	dev->print_log(dev, "%s() sends RESUME notification\n", __func__);
+	state_status_msg.states_status = current_state;
+
+	ishtp_write_message(dev, &ishtp_hdr,
+		(unsigned char *)&state_status_msg);
+}
+EXPORT_SYMBOL(ishtp_send_resume);
+
+/**
+ * ishtp_query_subscribers() - Send query subscribers message
+ * @dev: ISHTP device instance
+ *
+ * Send message to query subscribers
+ */
+void ishtp_query_subscribers(struct ishtp_device *dev)
+{
+	struct ishtp_msg_hdr	ishtp_hdr;
+	struct ish_system_states_query_subscribers query_subscribers_msg;
+	const size_t len = sizeof(struct ish_system_states_query_subscribers);
+
+	fix_cl_hdr(&ishtp_hdr, len, ISHTP_SYSTEM_STATE_CLIENT_ADDR);
+
+	memset(&query_subscribers_msg, 0, len);
+	query_subscribers_msg.hdr.cmd = SYSTEM_STATE_QUERY_SUBSCRIBERS;
+
+	ishtp_write_message(dev, &ishtp_hdr,
+		(unsigned char *)&query_subscribers_msg);
+}
diff --git a/drivers/hid/intel-ish-hid/ishtp/hbm.h b/drivers/hid/intel-ish-hid/ishtp/hbm.h
new file mode 100644
index 000000000000..d96111cef7f8
--- /dev/null
+++ b/drivers/hid/intel-ish-hid/ishtp/hbm.h
@@ -0,0 +1,321 @@
+/*
+ * ISHTP bus layer messages handling
+ *
+ * Copyright (c) 2003-2016, 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 _ISHTP_HBM_H_
+#define _ISHTP_HBM_H_
+
+#include <linux/uuid.h>
+
+struct ishtp_device;
+struct ishtp_msg_hdr;
+struct ishtp_cl;
+
+/*
+ * Timeouts in Seconds
+ */
+#define ISHTP_INTEROP_TIMEOUT		7 /* Timeout on ready message */
+
+#define ISHTP_CL_CONNECT_TIMEOUT	15 /* HPS: Client Connect Timeout */
+
+/*
+ * ISHTP Version
+ */
+#define HBM_MINOR_VERSION		0
+#define HBM_MAJOR_VERSION		1
+
+/* Host bus message command opcode */
+#define ISHTP_HBM_CMD_OP_MSK		0x7f
+/* Host bus message command RESPONSE */
+#define ISHTP_HBM_CMD_RES_MSK		0x80
+
+/*
+ * ISHTP Bus Message Command IDs
+ */
+#define HOST_START_REQ_CMD		0x01
+#define HOST_START_RES_CMD		0x81
+
+#define HOST_STOP_REQ_CMD		0x02
+#define HOST_STOP_RES_CMD		0x82
+
+#define FW_STOP_REQ_CMD			0x03
+
+#define HOST_ENUM_REQ_CMD		0x04
+#define HOST_ENUM_RES_CMD		0x84
+
+#define HOST_CLIENT_PROPERTIES_REQ_CMD	0x05
+#define HOST_CLIENT_PROPERTIES_RES_CMD	0x85
+
+#define CLIENT_CONNECT_REQ_CMD		0x06
+#define CLIENT_CONNECT_RES_CMD		0x86
+
+#define CLIENT_DISCONNECT_REQ_CMD	0x07
+#define CLIENT_DISCONNECT_RES_CMD	0x87
+
+#define ISHTP_FLOW_CONTROL_CMD		0x08
+
+#define DMA_BUFFER_ALLOC_NOTIFY		0x11
+#define DMA_BUFFER_ALLOC_RESPONSE	0x91
+
+#define DMA_XFER			0x12
+#define DMA_XFER_ACK			0x92
+
+/*
+ * ISHTP Stop Reason
+ * used by hbm_host_stop_request.reason
+ */
+#define	DRIVER_STOP_REQUEST		0x00
+
+/*
+ * ISHTP BUS Interface Section
+ */
+struct ishtp_msg_hdr {
+	uint32_t fw_addr:8;
+	uint32_t host_addr:8;
+	uint32_t length:9;
+	uint32_t reserved:6;
+	uint32_t msg_complete:1;
+} __packed;
+
+struct ishtp_bus_message {
+	uint8_t hbm_cmd;
+	uint8_t data[0];
+} __packed;
+
+/**
+ * struct hbm_cl_cmd - client specific host bus command
+ *	CONNECT, DISCONNECT, and FlOW CONTROL
+ *
+ * @hbm_cmd - bus message command header
+ * @fw_addr - address of the fw client
+ * @host_addr - address of the client in the driver
+ * @data
+ */
+struct ishtp_hbm_cl_cmd {
+	uint8_t hbm_cmd;
+	uint8_t fw_addr;
+	uint8_t host_addr;
+	uint8_t data;
+};
+
+struct hbm_version {
+	uint8_t minor_version;
+	uint8_t major_version;
+} __packed;
+
+struct hbm_host_version_request {
+	uint8_t hbm_cmd;
+	uint8_t reserved;
+	struct hbm_version host_version;
+} __packed;
+
+struct hbm_host_version_response {
+	uint8_t hbm_cmd;
+	uint8_t host_version_supported;
+	struct hbm_version fw_max_version;
+} __packed;
+
+struct hbm_host_stop_request {
+	uint8_t hbm_cmd;
+	uint8_t reason;
+	uint8_t reserved[2];
+} __packed;
+
+struct hbm_host_stop_response {
+	uint8_t hbm_cmd;
+	uint8_t reserved[3];
+} __packed;
+
+struct hbm_host_enum_request {
+	uint8_t hbm_cmd;
+	uint8_t reserved[3];
+} __packed;
+
+struct hbm_host_enum_response {
+	uint8_t hbm_cmd;
+	uint8_t reserved[3];
+	uint8_t valid_addresses[32];
+} __packed;
+
+struct ishtp_client_properties {
+	uuid_le protocol_name;
+	uint8_t protocol_version;
+	uint8_t max_number_of_connections;
+	uint8_t fixed_address;
+	uint8_t single_recv_buf;
+	uint32_t max_msg_length;
+	uint8_t dma_hdr_len;
+#define	ISHTP_CLIENT_DMA_ENABLED	0x80
+	uint8_t reserved4;
+	uint8_t reserved5;
+	uint8_t reserved6;
+} __packed;
+
+struct hbm_props_request {
+	uint8_t hbm_cmd;
+	uint8_t address;
+	uint8_t reserved[2];
+} __packed;
+
+struct hbm_props_response {
+	uint8_t hbm_cmd;
+	uint8_t address;
+	uint8_t status;
+	uint8_t reserved[1];
+	struct ishtp_client_properties client_properties;
+} __packed;
+
+/**
+ * struct hbm_client_connect_request - connect/disconnect request
+ *
+ * @hbm_cmd - bus message command header
+ * @fw_addr - address of the fw client
+ * @host_addr - address of the client in the driver
+ * @reserved
+ */
+struct hbm_client_connect_request {
+	uint8_t hbm_cmd;
+	uint8_t fw_addr;
+	uint8_t host_addr;
+	uint8_t reserved;
+} __packed;
+
+/**
+ * struct hbm_client_connect_response - connect/disconnect response
+ *
+ * @hbm_cmd - bus message command header
+ * @fw_addr - address of the fw client
+ * @host_addr - address of the client in the driver
+ * @status - status of the request
+ */
+struct hbm_client_connect_response {
+	uint8_t hbm_cmd;
+	uint8_t fw_addr;
+	uint8_t host_addr;
+	uint8_t status;
+} __packed;
+
+
+#define ISHTP_FC_MESSAGE_RESERVED_LENGTH		5
+
+struct hbm_flow_control {
+	uint8_t hbm_cmd;
+	uint8_t fw_addr;
+	uint8_t host_addr;
+	uint8_t reserved[ISHTP_FC_MESSAGE_RESERVED_LENGTH];
+} __packed;
+
+struct dma_alloc_notify {
+	uint8_t hbm;
+	uint8_t status;
+	uint8_t reserved[2];
+	uint32_t buf_size;
+	uint64_t buf_address;
+	/* [...] May come more size/address pairs */
+} __packed;
+
+struct dma_xfer_hbm {
+	uint8_t hbm;
+	uint8_t fw_client_id;
+	uint8_t host_client_id;
+	uint8_t reserved;
+	uint64_t msg_addr;
+	uint32_t msg_length;
+	uint32_t reserved2;
+} __packed;
+
+/* System state */
+#define ISHTP_SYSTEM_STATE_CLIENT_ADDR		13
+
+#define SYSTEM_STATE_SUBSCRIBE			0x1
+#define SYSTEM_STATE_STATUS			0x2
+#define SYSTEM_STATE_QUERY_SUBSCRIBERS		0x3
+#define SYSTEM_STATE_STATE_CHANGE_REQ		0x4
+/*indicates suspend and resume states*/
+#define SUSPEND_STATE_BIT			(1<<1)
+
+struct ish_system_states_header {
+	uint32_t cmd;
+	uint32_t cmd_status;	/*responses will have this set*/
+} __packed;
+
+struct ish_system_states_subscribe {
+	struct ish_system_states_header hdr;
+	uint32_t states;
+} __packed;
+
+struct ish_system_states_status {
+	struct ish_system_states_header hdr;
+	uint32_t supported_states;
+	uint32_t states_status;
+} __packed;
+
+struct ish_system_states_query_subscribers {
+	struct ish_system_states_header hdr;
+} __packed;
+
+struct ish_system_states_state_change_req {
+	struct ish_system_states_header hdr;
+	uint32_t requested_states;
+	uint32_t states_status;
+} __packed;
+
+/**
+ * enum ishtp_hbm_state - host bus message protocol state
+ *
+ * @ISHTP_HBM_IDLE : protocol not started
+ * @ISHTP_HBM_START : start request message was sent
+ * @ISHTP_HBM_ENUM_CLIENTS : enumeration request was sent
+ * @ISHTP_HBM_CLIENT_PROPERTIES : acquiring clients properties
+ */
+enum ishtp_hbm_state {
+	ISHTP_HBM_IDLE = 0,
+	ISHTP_HBM_START,
+	ISHTP_HBM_STARTED,
+	ISHTP_HBM_ENUM_CLIENTS,
+	ISHTP_HBM_CLIENT_PROPERTIES,
+	ISHTP_HBM_WORKING,
+	ISHTP_HBM_STOPPED,
+};
+
+static inline void ishtp_hbm_hdr(struct ishtp_msg_hdr *hdr, size_t length)
+{
+	hdr->host_addr = 0;
+	hdr->fw_addr = 0;
+	hdr->length = length;
+	hdr->msg_complete = 1;
+	hdr->reserved = 0;
+}
+
+int ishtp_hbm_start_req(struct ishtp_device *dev);
+int ishtp_hbm_start_wait(struct ishtp_device *dev);
+int ishtp_hbm_cl_flow_control_req(struct ishtp_device *dev,
+				  struct ishtp_cl *cl);
+int ishtp_hbm_cl_disconnect_req(struct ishtp_device *dev, struct ishtp_cl *cl);
+int ishtp_hbm_cl_connect_req(struct ishtp_device *dev, struct ishtp_cl *cl);
+void ishtp_hbm_enum_clients_req(struct ishtp_device *dev);
+void bh_hbm_work_fn(struct work_struct *work);
+void recv_hbm(struct ishtp_device *dev, struct ishtp_msg_hdr *ishtp_hdr);
+void recv_fixed_cl_msg(struct ishtp_device *dev,
+	struct ishtp_msg_hdr *ishtp_hdr);
+void ishtp_hbm_dispatch(struct ishtp_device *dev,
+	struct ishtp_bus_message *hdr);
+
+void ishtp_query_subscribers(struct ishtp_device *dev);
+
+/* Exported I/F */
+void ishtp_send_suspend(struct ishtp_device *dev);
+void ishtp_send_resume(struct ishtp_device *dev);
+
+#endif /* _ISHTP_HBM_H_ */
diff --git a/drivers/hid/intel-ish-hid/ishtp/init.c b/drivers/hid/intel-ish-hid/ishtp/init.c
new file mode 100644
index 000000000000..ac364418e17c
--- /dev/null
+++ b/drivers/hid/intel-ish-hid/ishtp/init.c
@@ -0,0 +1,115 @@
+/*
+ * Initialization protocol for ISHTP driver
+ *
+ * Copyright (c) 2003-2016, 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/export.h>
+#include <linux/slab.h>
+#include <linux/sched.h>
+#include <linux/miscdevice.h>
+#include "ishtp-dev.h"
+#include "hbm.h"
+#include "client.h"
+
+/**
+ * ishtp_dev_state_str() -Convert to string format
+ * @state: state to convert
+ *
+ * Convert state to string for prints
+ *
+ * Return: character pointer to converted string
+ */
+const char *ishtp_dev_state_str(int state)
+{
+	switch (state) {
+	case ISHTP_DEV_INITIALIZING:
+		return	"INITIALIZING";
+	case ISHTP_DEV_INIT_CLIENTS:
+		return	"INIT_CLIENTS";
+	case ISHTP_DEV_ENABLED:
+		return	"ENABLED";
+	case ISHTP_DEV_RESETTING:
+		return	"RESETTING";
+	case ISHTP_DEV_DISABLED:
+		return	"DISABLED";
+	case ISHTP_DEV_POWER_DOWN:
+		return	"POWER_DOWN";
+	case ISHTP_DEV_POWER_UP:
+		return	"POWER_UP";
+	default:
+		return "unknown";
+	}
+}
+
+/**
+ * ishtp_device_init() - ishtp device init
+ * @dev: ISHTP device instance
+ *
+ * After ISHTP device is alloacted, this function is used to initialize
+ * each field which includes spin lock, work struct and lists
+ */
+void ishtp_device_init(struct ishtp_device *dev)
+{
+	dev->dev_state = ISHTP_DEV_INITIALIZING;
+	INIT_LIST_HEAD(&dev->cl_list);
+	INIT_LIST_HEAD(&dev->device_list);
+	dev->rd_msg_fifo_head = 0;
+	dev->rd_msg_fifo_tail = 0;
+	spin_lock_init(&dev->rd_msg_spinlock);
+
+	init_waitqueue_head(&dev->wait_hbm_recvd_msg);
+	spin_lock_init(&dev->read_list_spinlock);
+	spin_lock_init(&dev->device_lock);
+	spin_lock_init(&dev->device_list_lock);
+	spin_lock_init(&dev->cl_list_lock);
+	spin_lock_init(&dev->fw_clients_lock);
+	INIT_WORK(&dev->bh_hbm_work, bh_hbm_work_fn);
+
+	bitmap_zero(dev->host_clients_map, ISHTP_CLIENTS_MAX);
+	dev->open_handle_count = 0;
+
+	/*
+	 * Reserving client ID 0 for ISHTP Bus Message communications
+	 */
+	bitmap_set(dev->host_clients_map, 0, 1);
+
+	INIT_LIST_HEAD(&dev->read_list.list);
+
+}
+EXPORT_SYMBOL(ishtp_device_init);
+
+/**
+ * ishtp_start() - Start ISH processing
+ * @dev: ISHTP device instance
+ *
+ * Start ISHTP processing by sending query subscriber message
+ *
+ * Return: 0 on success else -ENODEV
+ */
+int ishtp_start(struct ishtp_device *dev)
+{
+	if (ishtp_hbm_start_wait(dev)) {
+		dev_err(dev->devc, "HBM haven't started");
+		goto err;
+	}
+
+	/* suspend & resume notification - send QUERY_SUBSCRIBERS msg */
+	ishtp_query_subscribers(dev);
+
+	return 0;
+err:
+	dev_err(dev->devc, "link layer initialization failed.\n");
+	dev->dev_state = ISHTP_DEV_DISABLED;
+	return -ENODEV;
+}
+EXPORT_SYMBOL(ishtp_start);
diff --git a/drivers/hid/intel-ish-hid/ishtp/ishtp-dev.h b/drivers/hid/intel-ish-hid/ishtp/ishtp-dev.h
new file mode 100644
index 000000000000..a94f9a8a96a0
--- /dev/null
+++ b/drivers/hid/intel-ish-hid/ishtp/ishtp-dev.h
@@ -0,0 +1,277 @@
+/*
+ * Most ISHTP provider device and ISHTP logic declarations
+ *
+ * Copyright (c) 2003-2016, 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 _ISHTP_DEV_H_
+#define _ISHTP_DEV_H_
+
+#include <linux/types.h>
+#include <linux/spinlock.h>
+#include "bus.h"
+#include "hbm.h"
+
+#define	IPC_PAYLOAD_SIZE	128
+#define ISHTP_RD_MSG_BUF_SIZE	IPC_PAYLOAD_SIZE
+#define	IPC_FULL_MSG_SIZE	132
+
+/* Number of messages to be held in ISR->BH FIFO */
+#define	RD_INT_FIFO_SIZE	64
+
+/*
+ * Number of IPC messages to be held in Tx FIFO, to be sent by ISR -
+ * Tx complete interrupt or RX_COMPLETE handler
+ */
+#define	IPC_TX_FIFO_SIZE	512
+
+/*
+ * Number of Maximum ISHTP Clients
+ */
+#define ISHTP_CLIENTS_MAX 256
+
+/*
+ * Number of File descriptors/handles
+ * that can be opened to the driver.
+ *
+ * Limit to 255: 256 Total Clients
+ * minus internal client for ISHTP Bus Messages
+ */
+#define ISHTP_MAX_OPEN_HANDLE_COUNT (ISHTP_CLIENTS_MAX - 1)
+
+/* Internal Clients Number */
+#define ISHTP_HOST_CLIENT_ID_ANY		(-1)
+#define ISHTP_HBM_HOST_CLIENT_ID		0
+
+#define	MAX_DMA_DELAY	20
+
+/* ISHTP device states */
+enum ishtp_dev_state {
+	ISHTP_DEV_INITIALIZING = 0,
+	ISHTP_DEV_INIT_CLIENTS,
+	ISHTP_DEV_ENABLED,
+	ISHTP_DEV_RESETTING,
+	ISHTP_DEV_DISABLED,
+	ISHTP_DEV_POWER_DOWN,
+	ISHTP_DEV_POWER_UP
+};
+const char *ishtp_dev_state_str(int state);
+
+struct ishtp_cl;
+
+/**
+ * struct ishtp_fw_client - representation of fw client
+ *
+ * @props - client properties
+ * @client_id - fw client id
+ */
+struct ishtp_fw_client {
+	struct ishtp_client_properties props;
+	uint8_t client_id;
+};
+
+/**
+ * struct ishtp_msg_data - ISHTP message data struct
+ * @size:	Size of data in the *data
+ * @data:	Pointer to data
+ */
+struct ishtp_msg_data {
+	uint32_t size;
+	unsigned char *data;
+};
+
+/*
+ * struct ishtp_cl_rb - request block structure
+ * @list:	Link to list members
+ * @cl:		ISHTP client instance
+ * @buffer:	message header
+ * @buf_idx:	Index into buffer
+ * @read_time:	 unused at this time
+ */
+struct ishtp_cl_rb {
+	struct list_head list;
+	struct ishtp_cl *cl;
+	struct ishtp_msg_data buffer;
+	unsigned long buf_idx;
+	unsigned long read_time;
+};
+
+/*
+ * Control info for IPC messages ISHTP/IPC sending FIFO -
+ * list with inline data buffer
+ * This structure will be filled with parameters submitted
+ * by the caller glue layer
+ * 'buf' may be pointing to the external buffer or to 'inline_data'
+ * 'offset' will be initialized to 0 by submitting
+ *
+ * 'ipc_send_compl' is intended for use by clients that send fragmented
+ * messages. When a fragment is sent down to IPC msg regs,
+ * it will be called.
+ * If it has more fragments to send, it will do it. With last fragment
+ * it will send appropriate ISHTP "message-complete" flag.
+ * It will remove the outstanding message
+ * (mark outstanding buffer as available).
+ * If counting flow control is in work and there are more flow control
+ * credits, it can put the next client message queued in cl.
+ * structure for IPC processing.
+ *
+ */
+struct wr_msg_ctl_info {
+	/* Will be called with 'ipc_send_compl_prm' as parameter */
+	void (*ipc_send_compl)(void *);
+
+	void *ipc_send_compl_prm;
+	size_t length;
+	struct list_head	link;
+	unsigned char	inline_data[IPC_FULL_MSG_SIZE];
+};
+
+/*
+ * The ISHTP layer talks to hardware IPC message using the following
+ * callbacks
+ */
+struct ishtp_hw_ops {
+	int	(*hw_reset)(struct ishtp_device *dev);
+	int	(*ipc_reset)(struct ishtp_device *dev);
+	uint32_t (*ipc_get_header)(struct ishtp_device *dev, int length,
+				   int busy);
+	int	(*write)(struct ishtp_device *dev,
+		void (*ipc_send_compl)(void *), void *ipc_send_compl_prm,
+		unsigned char *msg, int length);
+	uint32_t	(*ishtp_read_hdr)(const struct ishtp_device *dev);
+	int	(*ishtp_read)(struct ishtp_device *dev, unsigned char *buffer,
+			unsigned long buffer_length);
+	uint32_t	(*get_fw_status)(struct ishtp_device *dev);
+	void	(*sync_fw_clock)(struct ishtp_device *dev);
+};
+
+/**
+ * struct ishtp_device - ISHTP private device struct
+ */
+struct ishtp_device {
+	struct device *devc;	/* pointer to lowest device */
+	struct pci_dev *pdev;	/* PCI device to get device ids */
+
+	/* waitq for waiting for suspend response */
+	wait_queue_head_t suspend_wait;
+	bool suspend_flag;	/* Suspend is active */
+
+	/* waitq for waiting for resume response */
+	wait_queue_head_t resume_wait;
+	bool resume_flag;	/*Resume is active */
+
+	/*
+	 * lock for the device, for everything that doesn't have
+	 * a dedicated spinlock
+	 */
+	spinlock_t device_lock;
+
+	bool recvd_hw_ready;
+	struct hbm_version version;
+	int transfer_path; /* Choice of transfer path: IPC or DMA */
+
+	/* ishtp device states */
+	enum ishtp_dev_state dev_state;
+	enum ishtp_hbm_state hbm_state;
+
+	/* driver read queue */
+	struct ishtp_cl_rb read_list;
+	spinlock_t read_list_spinlock;
+
+	/* list of ishtp_cl's */
+	struct list_head cl_list;
+	spinlock_t cl_list_lock;
+	long open_handle_count;
+
+	/* List of bus devices */
+	struct list_head device_list;
+	spinlock_t device_list_lock;
+
+	/* waiting queues for receive message from FW */
+	wait_queue_head_t wait_hw_ready;
+	wait_queue_head_t wait_hbm_recvd_msg;
+
+	/* FIFO for input messages for BH processing */
+	unsigned char rd_msg_fifo[RD_INT_FIFO_SIZE * IPC_PAYLOAD_SIZE];
+	unsigned int rd_msg_fifo_head, rd_msg_fifo_tail;
+	spinlock_t rd_msg_spinlock;
+	struct work_struct bh_hbm_work;
+
+	/* IPC write queue */
+	struct wr_msg_ctl_info wr_processing_list_head, wr_free_list_head;
+	/* For both processing list  and free list */
+	spinlock_t wr_processing_spinlock;
+
+	spinlock_t out_ipc_spinlock;
+
+	struct ishtp_fw_client *fw_clients; /*Note:memory has to be allocated*/
+	DECLARE_BITMAP(fw_clients_map, ISHTP_CLIENTS_MAX);
+	DECLARE_BITMAP(host_clients_map, ISHTP_CLIENTS_MAX);
+	uint8_t fw_clients_num;
+	uint8_t fw_client_presentation_num;
+	uint8_t fw_client_index;
+	spinlock_t fw_clients_lock;
+
+	/* TX DMA buffers and slots */
+	int ishtp_host_dma_enabled;
+	void *ishtp_host_dma_tx_buf;
+	unsigned int ishtp_host_dma_tx_buf_size;
+	uint64_t ishtp_host_dma_tx_buf_phys;
+	int ishtp_dma_num_slots;
+
+	/* map of 4k blocks in Tx dma buf: 0-free, 1-used */
+	uint8_t *ishtp_dma_tx_map;
+	spinlock_t ishtp_dma_tx_lock;
+
+	/* RX DMA buffers and slots */
+	void *ishtp_host_dma_rx_buf;
+	unsigned int ishtp_host_dma_rx_buf_size;
+	uint64_t ishtp_host_dma_rx_buf_phys;
+
+	/* Dump to trace buffers if enabled*/
+	void (*print_log)(struct ishtp_device *dev, char *format, ...);
+
+	/* Debug stats */
+	unsigned int	ipc_rx_cnt;
+	unsigned long long	ipc_rx_bytes_cnt;
+	unsigned int	ipc_tx_cnt;
+	unsigned long long	ipc_tx_bytes_cnt;
+
+	const struct ishtp_hw_ops *ops;
+	size_t	mtu;
+	uint32_t	ishtp_msg_hdr;
+	char hw[0] __aligned(sizeof(void *));
+};
+
+static inline unsigned long ishtp_secs_to_jiffies(unsigned long sec)
+{
+	return msecs_to_jiffies(sec * MSEC_PER_SEC);
+}
+
+/*
+ * Register Access Function
+ */
+static inline int ish_ipc_reset(struct ishtp_device *dev)
+{
+	return dev->ops->ipc_reset(dev);
+}
+
+static inline int ish_hw_reset(struct ishtp_device *dev)
+{
+	return dev->ops->hw_reset(dev);
+}
+
+/* Exported function */
+void	ishtp_device_init(struct ishtp_device *dev);
+int	ishtp_start(struct ishtp_device *dev);
+
+#endif /*_ISHTP_DEV_H_*/
diff --git a/drivers/hid/uhid.c b/drivers/hid/uhid.c
index 99ec3ff7563b..7f8ff39ed44b 100644
--- a/drivers/hid/uhid.c
+++ b/drivers/hid/uhid.c
@@ -779,19 +779,8 @@ static struct miscdevice uhid_misc = {
 	.minor		= UHID_MINOR,
 	.name		= UHID_NAME,
 };
+module_misc_device(uhid_misc);
 
-static int __init uhid_init(void)
-{
-	return misc_register(&uhid_misc);
-}
-
-static void __exit uhid_exit(void)
-{
-	misc_deregister(&uhid_misc);
-}
-
-module_init(uhid_init);
-module_exit(uhid_exit);
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("David Herrmann <dh.herrmann@gmail.com>");
 MODULE_DESCRIPTION("User-space I/O driver support for HID subsystem");
diff --git a/drivers/hid/usbhid/hid-quirks.c b/drivers/hid/usbhid/hid-quirks.c
index b4b8c6abb03e..354d49ea36dd 100644
--- a/drivers/hid/usbhid/hid-quirks.c
+++ b/drivers/hid/usbhid/hid-quirks.c
@@ -56,6 +56,7 @@ static const struct hid_blacklist {
 
 	{ USB_VENDOR_ID_AIREN, USB_DEVICE_ID_AIREN_SLIMPLUS, HID_QUIRK_NOGET },
 	{ USB_VENDOR_ID_AKAI, USB_DEVICE_ID_AKAI_MPKMINI2, HID_QUIRK_NO_INIT_REPORTS },
+	{ USB_VENDOR_ID_AKAI_09E8, USB_DEVICE_ID_AKAI_09E8_MIDIMIX, HID_QUIRK_NO_INIT_REPORTS },
 	{ USB_VENDOR_ID_ATEN, USB_DEVICE_ID_ATEN_UC100KM, HID_QUIRK_NOGET },
 	{ USB_VENDOR_ID_ATEN, USB_DEVICE_ID_ATEN_CS124U, HID_QUIRK_NOGET },
 	{ USB_VENDOR_ID_ATEN, USB_DEVICE_ID_ATEN_2PORTKVM, HID_QUIRK_NOGET },
@@ -76,6 +77,7 @@ static const struct hid_blacklist {
 	{ USB_VENDOR_ID_CORSAIR, USB_DEVICE_ID_CORSAIR_K95RGB, HID_QUIRK_NO_INIT_REPORTS | HID_QUIRK_ALWAYS_POLL },
 	{ USB_VENDOR_ID_CORSAIR, USB_DEVICE_ID_CORSAIR_K70RGB, HID_QUIRK_NO_INIT_REPORTS },
 	{ USB_VENDOR_ID_CORSAIR, USB_DEVICE_ID_CORSAIR_K65RGB, HID_QUIRK_NO_INIT_REPORTS },
+	{ USB_VENDOR_ID_CORSAIR, USB_DEVICE_ID_CORSAIR_STRAFE, HID_QUIRK_NO_INIT_REPORTS | HID_QUIRK_ALWAYS_POLL },
 	{ USB_VENDOR_ID_CREATIVELABS, USB_DEVICE_ID_CREATIVE_SB_OMNI_SURROUND_51, HID_QUIRK_NOGET },
 	{ USB_VENDOR_ID_DMI, USB_DEVICE_ID_DMI_ENC, HID_QUIRK_NOGET },
 	{ USB_VENDOR_ID_DRAGONRISE, USB_DEVICE_ID_DRAGONRISE_WIIU, HID_QUIRK_MULTI_INPUT },
@@ -98,6 +100,7 @@ static const struct hid_blacklist {
 	{ USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_TYPE_COVER_PRO_3, HID_QUIRK_NO_INIT_REPORTS },
 	{ USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_TYPE_COVER_PRO_3_2, HID_QUIRK_NO_INIT_REPORTS },
 	{ USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_TYPE_COVER_PRO_3_JP, HID_QUIRK_NO_INIT_REPORTS },
+	{ USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_TYPE_COVER_PRO_4_JP, HID_QUIRK_NO_INIT_REPORTS },
 	{ USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_TYPE_COVER_3, 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 },
@@ -143,7 +146,7 @@ static const struct hid_blacklist {
 	{ USB_VENDOR_ID_CHICONY, USB_DEVICE_ID_CHICONY_WIRELESS, HID_QUIRK_MULTI_INPUT },
 	{ USB_VENDOR_ID_SIGMA_MICRO, USB_DEVICE_ID_SIGMA_MICRO_KEYBOARD, HID_QUIRK_NO_INIT_REPORTS },
 	{ USB_VENDOR_ID_KYE, USB_DEVICE_ID_KYE_MOUSEPEN_I608X, HID_QUIRK_MULTI_INPUT },
-	{ USB_VENDOR_ID_KYE, USB_DEVICE_ID_KYE_MOUSEPEN_I608X_2, HID_QUIRK_MULTI_INPUT },
+	{ USB_VENDOR_ID_KYE, USB_DEVICE_ID_KYE_MOUSEPEN_I608X_V2, HID_QUIRK_MULTI_INPUT },
 	{ USB_VENDOR_ID_KYE, USB_DEVICE_ID_KYE_EASYPEN_M610X, HID_QUIRK_MULTI_INPUT },
 	{ USB_VENDOR_ID_KYE, USB_DEVICE_ID_KYE_PENSKETCH_M912, HID_QUIRK_MULTI_INPUT },
 	{ USB_VENDOR_ID_NTRIG, USB_DEVICE_ID_NTRIG_DUOSENSE, HID_QUIRK_NO_INIT_REPORTS },
diff --git a/drivers/hid/wacom.h b/drivers/hid/wacom.h
index 4681a65a4579..b4800ea891cb 100644
--- a/drivers/hid/wacom.h
+++ b/drivers/hid/wacom.h
@@ -90,6 +90,8 @@
 #include <linux/module.h>
 #include <linux/mod_devicetable.h>
 #include <linux/hid.h>
+#include <linux/kfifo.h>
+#include <linux/leds.h>
 #include <linux/usb/input.h>
 #include <linux/power_supply.h>
 #include <asm/unaligned.h>
@@ -105,32 +107,95 @@
 #define USB_VENDOR_ID_WACOM	0x056a
 #define USB_VENDOR_ID_LENOVO	0x17ef
 
+enum wacom_worker {
+	WACOM_WORKER_WIRELESS,
+	WACOM_WORKER_BATTERY,
+	WACOM_WORKER_REMOTE,
+};
+
+struct wacom;
+
+struct wacom_led {
+	struct led_classdev cdev;
+	struct led_trigger trigger;
+	struct wacom *wacom;
+	unsigned int group;
+	unsigned int id;
+	u8 llv;
+	u8 hlv;
+	bool held;
+};
+
+struct wacom_group_leds {
+	u8 select; /* status led selector (0..3) */
+	struct wacom_led *leds;
+	unsigned int count;
+	struct device *dev;
+};
+
+struct wacom_battery {
+	struct wacom *wacom;
+	struct power_supply_desc bat_desc;
+	struct power_supply *battery;
+	char bat_name[WACOM_NAME_MAX];
+	int battery_capacity;
+	int bat_charging;
+	int bat_connected;
+	int ps_connected;
+};
+
+struct wacom_remote {
+	spinlock_t remote_lock;
+	struct kfifo remote_fifo;
+	struct kobject *remote_dir;
+	struct {
+		struct attribute_group group;
+		u32 serial;
+		struct input_dev *input;
+		bool registered;
+		struct wacom_battery battery;
+	} remotes[WACOM_MAX_REMOTES];
+};
+
 struct wacom {
 	struct usb_device *usbdev;
 	struct usb_interface *intf;
 	struct wacom_wac wacom_wac;
 	struct hid_device *hdev;
 	struct mutex lock;
-	struct work_struct work;
-	struct wacom_led {
-		u8 select[5]; /* status led selector (0..3) */
+	struct work_struct wireless_work;
+	struct work_struct battery_work;
+	struct work_struct remote_work;
+	struct wacom_remote *remote;
+	struct wacom_leds {
+		struct wacom_group_leds *groups;
+		unsigned int count;
 		u8 llv;       /* status led brightness no button (1..127) */
 		u8 hlv;       /* status led brightness button pressed (1..127) */
 		u8 img_lum;   /* OLED matrix display brightness */
+		u8 max_llv;   /* maximum brightness of LED (llv) */
+		u8 max_hlv;   /* maximum brightness of LED (hlv) */
 	} led;
-	bool led_initialized;
-	struct power_supply *battery;
-	struct power_supply *ac;
-	struct power_supply_desc battery_desc;
-	struct power_supply_desc ac_desc;
-	struct kobject *remote_dir;
-	struct attribute_group remote_group[5];
+	struct wacom_battery battery;
+	bool resources;
 };
 
-static inline void wacom_schedule_work(struct wacom_wac *wacom_wac)
+static inline void wacom_schedule_work(struct wacom_wac *wacom_wac,
+				       enum wacom_worker which)
 {
 	struct wacom *wacom = container_of(wacom_wac, struct wacom, wacom_wac);
-	schedule_work(&wacom->work);
+
+	switch (which) {
+	case WACOM_WORKER_WIRELESS:
+		schedule_work(&wacom->wireless_work);
+		break;
+	case WACOM_WORKER_BATTERY:
+		schedule_work(&wacom->battery_work);
+		break;
+	case WACOM_WORKER_REMOTE:
+		schedule_work(&wacom->remote_work);
+		break;
+	}
 }
 
 extern const struct hid_device_id wacom_ids[];
@@ -149,7 +214,8 @@ int wacom_wac_event(struct hid_device *hdev, struct hid_field *field,
 		struct hid_usage *usage, __s32 value);
 void wacom_wac_report(struct hid_device *hdev, struct hid_report *report);
 void wacom_battery_work(struct work_struct *work);
-int wacom_remote_create_attr_group(struct wacom *wacom, __u32 serial,
-				   int index);
-void wacom_remote_destroy_attr_group(struct wacom *wacom, __u32 serial);
+enum led_brightness wacom_leds_brightness_get(struct wacom_led *led);
+struct wacom_led *wacom_led_find(struct wacom *wacom, unsigned int group,
+				 unsigned int id);
+struct wacom_led *wacom_led_next(struct wacom *wacom, struct wacom_led *cur);
 #endif
diff --git a/drivers/hid/wacom_sys.c b/drivers/hid/wacom_sys.c
index 499cc8213cfe..5e7a5648e708 100644
--- a/drivers/hid/wacom_sys.c
+++ b/drivers/hid/wacom_sys.c
@@ -25,7 +25,6 @@
 #define WAC_CMD_RETRIES		10
 #define WAC_CMD_DELETE_PAIRING	0x20
 #define WAC_CMD_UNPAIR_ALL	0xFF
-#define WAC_REMOTE_SERIAL_MAX_STRLEN	9
 
 #define DEV_ATTR_RW_PERM (S_IRUGO | S_IWUSR | S_IWGRP)
 #define DEV_ATTR_WO_PERM (S_IWUSR | S_IWGRP)
@@ -91,7 +90,12 @@ static void wacom_close(struct input_dev *dev)
 {
 	struct wacom *wacom = input_get_drvdata(dev);
 
-	hid_hw_close(wacom->hdev);
+	/*
+	 * wacom->hdev should never be null, but surprisingly, I had the case
+	 * once while unplugging the Wacom Wireless Receiver.
+	 */
+	if (wacom->hdev)
+		hid_hw_close(wacom->hdev);
 }
 
 /*
@@ -523,36 +527,95 @@ struct wacom_hdev_data {
 static LIST_HEAD(wacom_udev_list);
 static DEFINE_MUTEX(wacom_udev_list_lock);
 
+static bool compare_device_paths(struct hid_device *hdev_a,
+		struct hid_device *hdev_b, char separator)
+{
+	int n1 = strrchr(hdev_a->phys, separator) - hdev_a->phys;
+	int n2 = strrchr(hdev_b->phys, separator) - hdev_b->phys;
+
+	if (n1 != n2 || n1 <= 0 || n2 <= 0)
+		return false;
+
+	return !strncmp(hdev_a->phys, hdev_b->phys, n1);
+}
+
 static bool wacom_are_sibling(struct hid_device *hdev,
 		struct hid_device *sibling)
 {
 	struct wacom *wacom = hid_get_drvdata(hdev);
 	struct wacom_features *features = &wacom->wacom_wac.features;
-	int vid = features->oVid;
-	int pid = features->oPid;
-	int n1,n2;
+	struct wacom *sibling_wacom = hid_get_drvdata(sibling);
+	struct wacom_features *sibling_features = &sibling_wacom->wacom_wac.features;
+	__u32 oVid = features->oVid ? features->oVid : hdev->vendor;
+	__u32 oPid = features->oPid ? features->oPid : hdev->product;
+
+	/* The defined oVid/oPid must match that of the sibling */
+	if (features->oVid != HID_ANY_ID && sibling->vendor != oVid)
+		return false;
+	if (features->oPid != HID_ANY_ID && sibling->product != oPid)
+		return false;
 
-	if (vid == 0 && pid == 0) {
-		vid = hdev->vendor;
-		pid = hdev->product;
+	/*
+	 * Devices with the same VID/PID must share the same physical
+	 * device path, while those with different VID/PID must share
+	 * the same physical parent device path.
+	 */
+	if (hdev->vendor == sibling->vendor && hdev->product == sibling->product) {
+		if (!compare_device_paths(hdev, sibling, '/'))
+			return false;
+	} else {
+		if (!compare_device_paths(hdev, sibling, '.'))
+			return false;
 	}
 
-	if (vid != sibling->vendor || pid != sibling->product)
+	/* Skip the remaining heuristics unless you are a HID_GENERIC device */
+	if (features->type != HID_GENERIC)
+		return true;
+
+	/*
+	 * Direct-input devices may not be siblings of indirect-input
+	 * devices.
+	 */
+	if ((features->device_type & WACOM_DEVICETYPE_DIRECT) &&
+	    !(sibling_features->device_type & WACOM_DEVICETYPE_DIRECT))
 		return false;
 
-	/* Compare the physical path. */
-	n1 = strrchr(hdev->phys, '.') - hdev->phys;
-	n2 = strrchr(sibling->phys, '.') - sibling->phys;
-	if (n1 != n2 || n1 <= 0 || n2 <= 0)
+	/*
+	 * Indirect-input devices may not be siblings of direct-input
+	 * devices.
+	 */
+	if (!(features->device_type & WACOM_DEVICETYPE_DIRECT) &&
+	    (sibling_features->device_type & WACOM_DEVICETYPE_DIRECT))
+		return false;
+
+	/* Pen devices may only be siblings of touch devices */
+	if ((features->device_type & WACOM_DEVICETYPE_PEN) &&
+	    !(sibling_features->device_type & WACOM_DEVICETYPE_TOUCH))
 		return false;
 
-	return !strncmp(hdev->phys, sibling->phys, n1);
+	/* Touch devices may only be siblings of pen devices */
+	if ((features->device_type & WACOM_DEVICETYPE_TOUCH) &&
+	    !(sibling_features->device_type & WACOM_DEVICETYPE_PEN))
+		return false;
+
+	/*
+	 * No reason could be found for these two devices to NOT be
+	 * siblings, so there's a good chance they ARE siblings
+	 */
+	return true;
 }
 
 static struct wacom_hdev_data *wacom_get_hdev_data(struct hid_device *hdev)
 {
 	struct wacom_hdev_data *data;
 
+	/* Try to find an already-probed interface from the same device */
+	list_for_each_entry(data, &wacom_udev_list, list) {
+		if (compare_device_paths(hdev, data->dev, '/'))
+			return data;
+	}
+
+	/* Fallback to finding devices that appear to be "siblings" */
 	list_for_each_entry(data, &wacom_udev_list, list) {
 		if (wacom_are_sibling(hdev, data->dev)) {
 			kref_get(&data->kref);
@@ -563,6 +626,38 @@ static struct wacom_hdev_data *wacom_get_hdev_data(struct hid_device *hdev)
 	return NULL;
 }
 
+static void wacom_release_shared_data(struct kref *kref)
+{
+	struct wacom_hdev_data *data =
+		container_of(kref, struct wacom_hdev_data, kref);
+
+	mutex_lock(&wacom_udev_list_lock);
+	list_del(&data->list);
+	mutex_unlock(&wacom_udev_list_lock);
+
+	kfree(data);
+}
+
+static void wacom_remove_shared_data(void *res)
+{
+	struct wacom *wacom = res;
+	struct wacom_hdev_data *data;
+	struct wacom_wac *wacom_wac = &wacom->wacom_wac;
+
+	if (wacom_wac->shared) {
+		data = container_of(wacom_wac->shared, struct wacom_hdev_data,
+				    shared);
+
+		if (wacom_wac->shared->touch == wacom->hdev)
+			wacom_wac->shared->touch = NULL;
+		else if (wacom_wac->shared->pen == wacom->hdev)
+			wacom_wac->shared->pen = NULL;
+
+		kref_put(&data->kref, wacom_release_shared_data);
+		wacom_wac->shared = NULL;
+	}
+}
+
 static int wacom_add_shared_data(struct hid_device *hdev)
 {
 	struct wacom *wacom = hid_get_drvdata(hdev);
@@ -587,6 +682,13 @@ static int wacom_add_shared_data(struct hid_device *hdev)
 
 	wacom_wac->shared = &data->shared;
 
+	retval = devm_add_action(&hdev->dev, wacom_remove_shared_data, wacom);
+	if (retval) {
+		mutex_unlock(&wacom_udev_list_lock);
+		wacom_remove_shared_data(wacom);
+		return retval;
+	}
+
 	if (wacom_wac->features.device_type & WACOM_DEVICETYPE_TOUCH)
 		wacom_wac->shared->touch = hdev;
 	else if (wacom_wac->features.device_type & WACOM_DEVICETYPE_PEN)
@@ -597,37 +699,6 @@ out:
 	return retval;
 }
 
-static void wacom_release_shared_data(struct kref *kref)
-{
-	struct wacom_hdev_data *data =
-		container_of(kref, struct wacom_hdev_data, kref);
-
-	mutex_lock(&wacom_udev_list_lock);
-	list_del(&data->list);
-	mutex_unlock(&wacom_udev_list_lock);
-
-	kfree(data);
-}
-
-static void wacom_remove_shared_data(struct wacom *wacom)
-{
-	struct wacom_hdev_data *data;
-	struct wacom_wac *wacom_wac = &wacom->wacom_wac;
-
-	if (wacom_wac->shared) {
-		data = container_of(wacom_wac->shared, struct wacom_hdev_data,
-				    shared);
-
-		if (wacom_wac->shared->touch == wacom->hdev)
-			wacom_wac->shared->touch = NULL;
-		else if (wacom_wac->shared->pen == wacom->hdev)
-			wacom_wac->shared->pen = NULL;
-
-		kref_put(&data->kref, wacom_release_shared_data);
-		wacom_wac->shared = NULL;
-	}
-}
-
 static int wacom_led_control(struct wacom *wacom)
 {
 	unsigned char *buf;
@@ -635,6 +706,12 @@ static int wacom_led_control(struct wacom *wacom)
 	unsigned char report_id = WAC_CMD_LED_CONTROL;
 	int buf_size = 9;
 
+	if (!hid_get_drvdata(wacom->hdev))
+		return -ENODEV;
+
+	if (!wacom->led.groups)
+		return -ENOTSUPP;
+
 	if (wacom->wacom_wac.pid) { /* wireless connected */
 		report_id = WAC_CMD_WL_LED_CONTROL;
 		buf_size = 13;
@@ -650,7 +727,7 @@ static int wacom_led_control(struct wacom *wacom)
 		 * one of four values:
 		 *    0 = Low; 1 = Medium; 2 = High; 3 = Off
 		 */
-		int ring_led = wacom->led.select[0] & 0x03;
+		int ring_led = wacom->led.groups[0].select & 0x03;
 		int ring_lum = (((wacom->led.llv & 0x60) >> 5) - 1) & 0x03;
 		int crop_lum = 0;
 		unsigned char led_bits = (crop_lum << 4) | (ring_lum << 2) | (ring_led);
@@ -665,11 +742,11 @@ static int wacom_led_control(struct wacom *wacom)
 			buf[1] = led_bits;
 	}
 	else {
-		int led = wacom->led.select[0] | 0x4;
+		int led = wacom->led.groups[0].select | 0x4;
 
 		if (wacom->wacom_wac.features.type == WACOM_21UX2 ||
 		    wacom->wacom_wac.features.type == WACOM_24HD)
-			led |= (wacom->led.select[1] << 4) | 0x40;
+			led |= (wacom->led.groups[1].select << 4) | 0x40;
 
 		buf[0] = report_id;
 		buf[1] = led;
@@ -741,7 +818,7 @@ static ssize_t wacom_led_select_store(struct device *dev, int set_id,
 
 	mutex_lock(&wacom->lock);
 
-	wacom->led.select[set_id] = id & 0x3;
+	wacom->led.groups[set_id].select = id & 0x3;
 	err = wacom_led_control(wacom);
 
 	mutex_unlock(&wacom->lock);
@@ -761,7 +838,7 @@ static ssize_t wacom_led##SET_ID##_select_show(struct device *dev,	\
 	struct hid_device *hdev = to_hid_device(dev);\
 	struct wacom *wacom = hid_get_drvdata(hdev);			\
 	return scnprintf(buf, PAGE_SIZE, "%d\n",			\
-			 wacom->led.select[SET_ID]);			\
+			 wacom->led.groups[SET_ID].select);		\
 }									\
 static DEVICE_ATTR(status_led##SET_ID##_select, DEV_ATTR_RW_PERM,	\
 		    wacom_led##SET_ID##_select_show,			\
@@ -904,6 +981,327 @@ static struct attribute_group intuos5_led_attr_group = {
 	.attrs = intuos5_led_attrs,
 };
 
+struct wacom_sysfs_group_devres {
+	struct attribute_group *group;
+	struct kobject *root;
+};
+
+static void wacom_devm_sysfs_group_release(struct device *dev, void *res)
+{
+	struct wacom_sysfs_group_devres *devres = res;
+	struct kobject *kobj = devres->root;
+
+	dev_dbg(dev, "%s: dropping reference to %s\n",
+		__func__, devres->group->name);
+	sysfs_remove_group(kobj, devres->group);
+}
+
+static int __wacom_devm_sysfs_create_group(struct wacom *wacom,
+					   struct kobject *root,
+					   struct attribute_group *group)
+{
+	struct wacom_sysfs_group_devres *devres;
+	int error;
+
+	devres = devres_alloc(wacom_devm_sysfs_group_release,
+			      sizeof(struct wacom_sysfs_group_devres),
+			      GFP_KERNEL);
+	if (!devres)
+		return -ENOMEM;
+
+	devres->group = group;
+	devres->root = root;
+
+	error = sysfs_create_group(devres->root, group);
+	if (error)
+		return error;
+
+	devres_add(&wacom->hdev->dev, devres);
+
+	return 0;
+}
+
+static int wacom_devm_sysfs_create_group(struct wacom *wacom,
+					 struct attribute_group *group)
+{
+	return __wacom_devm_sysfs_create_group(wacom, &wacom->hdev->dev.kobj,
+					       group);
+}
+
+enum led_brightness wacom_leds_brightness_get(struct wacom_led *led)
+{
+	struct wacom *wacom = led->wacom;
+
+	if (wacom->led.max_hlv)
+		return led->hlv * LED_FULL / wacom->led.max_hlv;
+
+	if (wacom->led.max_llv)
+		return led->llv * LED_FULL / wacom->led.max_llv;
+
+	/* device doesn't support brightness tuning */
+	return LED_FULL;
+}
+
+static enum led_brightness __wacom_led_brightness_get(struct led_classdev *cdev)
+{
+	struct wacom_led *led = container_of(cdev, struct wacom_led, cdev);
+	struct wacom *wacom = led->wacom;
+
+	if (wacom->led.groups[led->group].select != led->id)
+		return LED_OFF;
+
+	return wacom_leds_brightness_get(led);
+}
+
+static int wacom_led_brightness_set(struct led_classdev *cdev,
+				    enum led_brightness brightness)
+{
+	struct wacom_led *led = container_of(cdev, struct wacom_led, cdev);
+	struct wacom *wacom = led->wacom;
+	int error;
+
+	mutex_lock(&wacom->lock);
+
+	if (!wacom->led.groups || (brightness == LED_OFF &&
+	    wacom->led.groups[led->group].select != led->id)) {
+		error = 0;
+		goto out;
+	}
+
+	led->llv = wacom->led.llv = wacom->led.max_llv * brightness / LED_FULL;
+	led->hlv = wacom->led.hlv = wacom->led.max_hlv * brightness / LED_FULL;
+
+	wacom->led.groups[led->group].select = led->id;
+
+	error = wacom_led_control(wacom);
+
+out:
+	mutex_unlock(&wacom->lock);
+
+	return error;
+}
+
+static void wacom_led_readonly_brightness_set(struct led_classdev *cdev,
+					       enum led_brightness brightness)
+{
+}
+
+static int wacom_led_register_one(struct device *dev, struct wacom *wacom,
+				  struct wacom_led *led, unsigned int group,
+				  unsigned int id, bool read_only)
+{
+	int error;
+	char *name;
+
+	name = devm_kasprintf(dev, GFP_KERNEL,
+			      "%s::wacom-%d.%d",
+			      dev_name(dev),
+			      group,
+			      id);
+	if (!name)
+		return -ENOMEM;
+
+	if (!read_only) {
+		led->trigger.name = name;
+		error = devm_led_trigger_register(dev, &led->trigger);
+		if (error) {
+			hid_err(wacom->hdev,
+				"failed to register LED trigger %s: %d\n",
+				led->cdev.name, error);
+			return error;
+		}
+	}
+
+	led->group = group;
+	led->id = id;
+	led->wacom = wacom;
+	led->llv = wacom->led.llv;
+	led->hlv = wacom->led.hlv;
+	led->cdev.name = name;
+	led->cdev.max_brightness = LED_FULL;
+	led->cdev.flags = LED_HW_PLUGGABLE;
+	led->cdev.brightness_get = __wacom_led_brightness_get;
+	if (!read_only) {
+		led->cdev.brightness_set_blocking = wacom_led_brightness_set;
+		led->cdev.default_trigger = led->cdev.name;
+	} else {
+		led->cdev.brightness_set = wacom_led_readonly_brightness_set;
+	}
+
+	error = devm_led_classdev_register(dev, &led->cdev);
+	if (error) {
+		hid_err(wacom->hdev,
+			"failed to register LED %s: %d\n",
+			led->cdev.name, error);
+		led->cdev.name = NULL;
+		return error;
+	}
+
+	return 0;
+}
+
+static void wacom_led_groups_release_one(void *data)
+{
+	struct wacom_group_leds *group = data;
+
+	devres_release_group(group->dev, group);
+}
+
+static int wacom_led_groups_alloc_and_register_one(struct device *dev,
+						   struct wacom *wacom,
+						   int group_id, int count,
+						   bool read_only)
+{
+	struct wacom_led *leds;
+	int i, error;
+
+	if (group_id >= wacom->led.count || count <= 0)
+		return -EINVAL;
+
+	if (!devres_open_group(dev, &wacom->led.groups[group_id], GFP_KERNEL))
+		return -ENOMEM;
+
+	leds = devm_kzalloc(dev, sizeof(struct wacom_led) * count, GFP_KERNEL);
+	if (!leds) {
+		error = -ENOMEM;
+		goto err;
+	}
+
+	wacom->led.groups[group_id].leds = leds;
+	wacom->led.groups[group_id].count = count;
+
+	for (i = 0; i < count; i++) {
+		error = wacom_led_register_one(dev, wacom, &leds[i],
+					       group_id, i, read_only);
+		if (error)
+			goto err;
+	}
+
+	wacom->led.groups[group_id].dev = dev;
+
+	devres_close_group(dev, &wacom->led.groups[group_id]);
+
+	/*
+	 * There is a bug (?) in devm_led_classdev_register() in which its
+	 * increments the refcount of the parent. If the parent is an input
+	 * device, that means the ref count never reaches 0 when
+	 * devm_input_device_release() gets called.
+	 * This means that the LEDs are still there after disconnect.
+	 * Manually force the release of the group so that the leds are released
+	 * once we are done using them.
+	 */
+	error = devm_add_action_or_reset(&wacom->hdev->dev,
+					 wacom_led_groups_release_one,
+					 &wacom->led.groups[group_id]);
+	if (error)
+		return error;
+
+	return 0;
+
+err:
+	devres_release_group(dev, &wacom->led.groups[group_id]);
+	return error;
+}
+
+struct wacom_led *wacom_led_find(struct wacom *wacom, unsigned int group_id,
+				 unsigned int id)
+{
+	struct wacom_group_leds *group;
+
+	if (group_id >= wacom->led.count)
+		return NULL;
+
+	group = &wacom->led.groups[group_id];
+
+	if (!group->leds)
+		return NULL;
+
+	id %= group->count;
+
+	return &group->leds[id];
+}
+
+/**
+ * wacom_led_next: gives the next available led with a wacom trigger.
+ *
+ * returns the next available struct wacom_led which has its default trigger
+ * or the current one if none is available.
+ */
+struct wacom_led *wacom_led_next(struct wacom *wacom, struct wacom_led *cur)
+{
+	struct wacom_led *next_led;
+	int group, next;
+
+	if (!wacom || !cur)
+		return NULL;
+
+	group = cur->group;
+	next = cur->id;
+
+	do {
+		next_led = wacom_led_find(wacom, group, ++next);
+		if (!next_led || next_led == cur)
+			return next_led;
+	} while (next_led->cdev.trigger != &next_led->trigger);
+
+	return next_led;
+}
+
+static void wacom_led_groups_release(void *data)
+{
+	struct wacom *wacom = data;
+
+	wacom->led.groups = NULL;
+	wacom->led.count = 0;
+}
+
+static int wacom_led_groups_allocate(struct wacom *wacom, int count)
+{
+	struct device *dev = &wacom->hdev->dev;
+	struct wacom_group_leds *groups;
+	int error;
+
+	groups = devm_kzalloc(dev, sizeof(struct wacom_group_leds) * count,
+			      GFP_KERNEL);
+	if (!groups)
+		return -ENOMEM;
+
+	error = devm_add_action_or_reset(dev, wacom_led_groups_release, wacom);
+	if (error)
+		return error;
+
+	wacom->led.groups = groups;
+	wacom->led.count = count;
+
+	return 0;
+}
+
+static int wacom_leds_alloc_and_register(struct wacom *wacom, int group_count,
+					 int led_per_group, bool read_only)
+{
+	struct device *dev;
+	int i, error;
+
+	if (!wacom->wacom_wac.pad_input)
+		return -EINVAL;
+
+	dev = &wacom->wacom_wac.pad_input->dev;
+
+	error = wacom_led_groups_allocate(wacom, group_count);
+	if (error)
+		return error;
+
+	for (i = 0; i < group_count; i++) {
+		error = wacom_led_groups_alloc_and_register_one(dev, wacom, i,
+								led_per_group,
+								read_only);
+		if (error)
+			return error;
+	}
+
+	return 0;
+}
+
 static int wacom_initialize_leds(struct wacom *wacom)
 {
 	int error;
@@ -917,25 +1315,38 @@ static int wacom_initialize_leds(struct wacom *wacom)
 	case INTUOS4:
 	case INTUOS4WL:
 	case INTUOS4L:
-		wacom->led.select[0] = 0;
-		wacom->led.select[1] = 0;
 		wacom->led.llv = 10;
 		wacom->led.hlv = 20;
+		wacom->led.max_llv = 127;
+		wacom->led.max_hlv = 127;
 		wacom->led.img_lum = 10;
-		error = sysfs_create_group(&wacom->hdev->dev.kobj,
-					   &intuos4_led_attr_group);
+
+		error = wacom_leds_alloc_and_register(wacom, 1, 4, false);
+		if (error) {
+			hid_err(wacom->hdev,
+				"cannot create leds err: %d\n", error);
+			return error;
+		}
+
+		error = wacom_devm_sysfs_create_group(wacom,
+						      &intuos4_led_attr_group);
 		break;
 
 	case WACOM_24HD:
 	case WACOM_21UX2:
-		wacom->led.select[0] = 0;
-		wacom->led.select[1] = 0;
 		wacom->led.llv = 0;
 		wacom->led.hlv = 0;
 		wacom->led.img_lum = 0;
 
-		error = sysfs_create_group(&wacom->hdev->dev.kobj,
-					   &cintiq_led_attr_group);
+		error = wacom_leds_alloc_and_register(wacom, 2, 4, false);
+		if (error) {
+			hid_err(wacom->hdev,
+				"cannot create leds err: %d\n", error);
+			return error;
+		}
+
+		error = wacom_devm_sysfs_create_group(wacom,
+						      &cintiq_led_attr_group);
 		break;
 
 	case INTUOS5S:
@@ -944,16 +1355,31 @@ static int wacom_initialize_leds(struct wacom *wacom)
 	case INTUOSPS:
 	case INTUOSPM:
 	case INTUOSPL:
-		wacom->led.select[0] = 0;
-		wacom->led.select[1] = 0;
 		wacom->led.llv = 32;
-		wacom->led.hlv = 0;
-		wacom->led.img_lum = 0;
+		wacom->led.max_llv = 96;
+
+		error = wacom_leds_alloc_and_register(wacom, 1, 4, false);
+		if (error) {
+			hid_err(wacom->hdev,
+				"cannot create leds err: %d\n", error);
+			return error;
+		}
 
-		error = sysfs_create_group(&wacom->hdev->dev.kobj,
-					  &intuos5_led_attr_group);
+		error = wacom_devm_sysfs_create_group(wacom,
+						      &intuos5_led_attr_group);
 		break;
 
+	case REMOTE:
+		wacom->led.llv = 255;
+		wacom->led.max_llv = 255;
+		error = wacom_led_groups_allocate(wacom, 5);
+		if (error) {
+			hid_err(wacom->hdev,
+				"cannot create leds err: %d\n", error);
+			return error;
+		}
+		return 0;
+
 	default:
 		return 0;
 	}
@@ -964,86 +1390,45 @@ static int wacom_initialize_leds(struct wacom *wacom)
 		return error;
 	}
 	wacom_led_control(wacom);
-	wacom->led_initialized = true;
 
 	return 0;
 }
 
-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) {
-	case INTUOS4S:
-	case INTUOS4:
-	case INTUOS4WL:
-	case INTUOS4L:
-		sysfs_remove_group(&wacom->hdev->dev.kobj,
-				   &intuos4_led_attr_group);
-		break;
-
-	case WACOM_24HD:
-	case WACOM_21UX2:
-		sysfs_remove_group(&wacom->hdev->dev.kobj,
-				   &cintiq_led_attr_group);
-		break;
-
-	case INTUOS5S:
-	case INTUOS5:
-	case INTUOS5L:
-	case INTUOSPS:
-	case INTUOSPM:
-	case INTUOSPL:
-		sysfs_remove_group(&wacom->hdev->dev.kobj,
-				   &intuos5_led_attr_group);
-		break;
-	}
-}
-
 static enum power_supply_property wacom_battery_props[] = {
+	POWER_SUPPLY_PROP_MODEL_NAME,
 	POWER_SUPPLY_PROP_PRESENT,
 	POWER_SUPPLY_PROP_STATUS,
 	POWER_SUPPLY_PROP_SCOPE,
 	POWER_SUPPLY_PROP_CAPACITY
 };
 
-static enum power_supply_property wacom_ac_props[] = {
-	POWER_SUPPLY_PROP_PRESENT,
-	POWER_SUPPLY_PROP_ONLINE,
-	POWER_SUPPLY_PROP_SCOPE,
-};
-
 static int wacom_battery_get_property(struct power_supply *psy,
 				      enum power_supply_property psp,
 				      union power_supply_propval *val)
 {
-	struct wacom *wacom = power_supply_get_drvdata(psy);
+	struct wacom_battery *battery = power_supply_get_drvdata(psy);
 	int ret = 0;
 
 	switch (psp) {
+		case POWER_SUPPLY_PROP_MODEL_NAME:
+			val->strval = battery->wacom->wacom_wac.name;
+			break;
 		case POWER_SUPPLY_PROP_PRESENT:
-			val->intval = wacom->wacom_wac.bat_connected;
+			val->intval = battery->bat_connected;
 			break;
 		case POWER_SUPPLY_PROP_SCOPE:
 			val->intval = POWER_SUPPLY_SCOPE_DEVICE;
 			break;
 		case POWER_SUPPLY_PROP_CAPACITY:
-			val->intval =
-				wacom->wacom_wac.battery_capacity;
+			val->intval = battery->battery_capacity;
 			break;
 		case POWER_SUPPLY_PROP_STATUS:
-			if (wacom->wacom_wac.bat_charging)
+			if (battery->bat_charging)
 				val->intval = POWER_SUPPLY_STATUS_CHARGING;
-			else if (wacom->wacom_wac.battery_capacity == 100 &&
-				    wacom->wacom_wac.ps_connected)
+			else if (battery->battery_capacity == 100 &&
+				    battery->ps_connected)
 				val->intval = POWER_SUPPLY_STATUS_FULL;
-			else if (wacom->wacom_wac.ps_connected)
+			else if (battery->ps_connected)
 				val->intval = POWER_SUPPLY_STATUS_NOT_CHARGING;
 			else
 				val->intval = POWER_SUPPLY_STATUS_DISCHARGING;
@@ -1056,84 +1441,64 @@ static int wacom_battery_get_property(struct power_supply *psy,
 	return ret;
 }
 
-static int wacom_ac_get_property(struct power_supply *psy,
-				enum power_supply_property psp,
-				union power_supply_propval *val)
+static int __wacom_initialize_battery(struct wacom *wacom,
+				      struct wacom_battery *battery)
 {
-	struct wacom *wacom = power_supply_get_drvdata(psy);
-	int ret = 0;
+	static atomic_t battery_no = ATOMIC_INIT(0);
+	struct device *dev = &wacom->hdev->dev;
+	struct power_supply_config psy_cfg = { .drv_data = battery, };
+	struct power_supply *ps_bat;
+	struct power_supply_desc *bat_desc = &battery->bat_desc;
+	unsigned long n;
+	int error;
 
-	switch (psp) {
-	case POWER_SUPPLY_PROP_PRESENT:
-		/* fall through */
-	case POWER_SUPPLY_PROP_ONLINE:
-		val->intval = wacom->wacom_wac.ps_connected;
-		break;
-	case POWER_SUPPLY_PROP_SCOPE:
-		val->intval = POWER_SUPPLY_SCOPE_DEVICE;
-		break;
-	default:
-		ret = -EINVAL;
-		break;
+	if (!devres_open_group(dev, bat_desc, GFP_KERNEL))
+		return -ENOMEM;
+
+	battery->wacom = wacom;
+
+	n = atomic_inc_return(&battery_no) - 1;
+
+	bat_desc->properties = wacom_battery_props;
+	bat_desc->num_properties = ARRAY_SIZE(wacom_battery_props);
+	bat_desc->get_property = wacom_battery_get_property;
+	sprintf(battery->bat_name, "wacom_battery_%ld", n);
+	bat_desc->name = battery->bat_name;
+	bat_desc->type = POWER_SUPPLY_TYPE_USB;
+	bat_desc->use_for_apm = 0;
+
+	ps_bat = devm_power_supply_register(dev, bat_desc, &psy_cfg);
+	if (IS_ERR(ps_bat)) {
+		error = PTR_ERR(ps_bat);
+		goto err;
 	}
-	return ret;
+
+	power_supply_powers(ps_bat, &wacom->hdev->dev);
+
+	battery->battery = ps_bat;
+
+	devres_close_group(dev, bat_desc);
+	return 0;
+
+err:
+	devres_release_group(dev, bat_desc);
+	return error;
 }
 
 static int wacom_initialize_battery(struct wacom *wacom)
 {
-	static atomic_t battery_no = ATOMIC_INIT(0);
-	struct power_supply_config psy_cfg = { .drv_data = wacom, };
-	unsigned long n;
-
-	if (wacom->wacom_wac.features.quirks & WACOM_QUIRK_BATTERY) {
-		struct power_supply_desc *bat_desc = &wacom->battery_desc;
-		struct power_supply_desc *ac_desc = &wacom->ac_desc;
-		n = atomic_inc_return(&battery_no) - 1;
-
-		bat_desc->properties = wacom_battery_props;
-		bat_desc->num_properties = ARRAY_SIZE(wacom_battery_props);
-		bat_desc->get_property = wacom_battery_get_property;
-		sprintf(wacom->wacom_wac.bat_name, "wacom_battery_%ld", n);
-		bat_desc->name = wacom->wacom_wac.bat_name;
-		bat_desc->type = POWER_SUPPLY_TYPE_BATTERY;
-		bat_desc->use_for_apm = 0;
-
-		ac_desc->properties = wacom_ac_props;
-		ac_desc->num_properties = ARRAY_SIZE(wacom_ac_props);
-		ac_desc->get_property = wacom_ac_get_property;
-		sprintf(wacom->wacom_wac.ac_name, "wacom_ac_%ld", n);
-		ac_desc->name = wacom->wacom_wac.ac_name;
-		ac_desc->type = POWER_SUPPLY_TYPE_MAINS;
-		ac_desc->use_for_apm = 0;
-
-		wacom->battery = power_supply_register(&wacom->hdev->dev,
-					      &wacom->battery_desc, &psy_cfg);
-		if (IS_ERR(wacom->battery))
-			return PTR_ERR(wacom->battery);
-
-		power_supply_powers(wacom->battery, &wacom->hdev->dev);
-
-		wacom->ac = power_supply_register(&wacom->hdev->dev,
-						  &wacom->ac_desc,
-						  &psy_cfg);
-		if (IS_ERR(wacom->ac)) {
-			power_supply_unregister(wacom->battery);
-			return PTR_ERR(wacom->ac);
-		}
-
-		power_supply_powers(wacom->ac, &wacom->hdev->dev);
-	}
+	if (wacom->wacom_wac.features.quirks & WACOM_QUIRK_BATTERY)
+		return __wacom_initialize_battery(wacom, &wacom->battery);
 
 	return 0;
 }
 
 static void wacom_destroy_battery(struct wacom *wacom)
 {
-	if (wacom->battery) {
-		power_supply_unregister(wacom->battery);
-		wacom->battery = NULL;
-		power_supply_unregister(wacom->ac);
-		wacom->ac = NULL;
+	if (wacom->battery.battery) {
+		devres_release_group(&wacom->hdev->dev,
+				     &wacom->battery.bat_desc);
+		wacom->battery.battery = NULL;
 	}
 }
 
@@ -1179,7 +1544,7 @@ static ssize_t wacom_show_remote_mode(struct kobject *kobj,
 	struct wacom *wacom = hid_get_drvdata(hdev);
 	u8 mode;
 
-	mode = wacom->led.select[index];
+	mode = wacom->led.groups[index].select;
 	if (mode >= 0 && mode < 3)
 		return snprintf(buf, PAGE_SIZE, "%d\n", mode);
 	else
@@ -1212,54 +1577,30 @@ DEVICE_EKR_ATTR_GROUP(2);
 DEVICE_EKR_ATTR_GROUP(3);
 DEVICE_EKR_ATTR_GROUP(4);
 
-int wacom_remote_create_attr_group(struct wacom *wacom, __u32 serial, int index)
+static int wacom_remote_create_attr_group(struct wacom *wacom, __u32 serial,
+					  int index)
 {
 	int error = 0;
-	char *buf;
-	struct wacom_wac *wacom_wac = &wacom->wacom_wac;
+	struct wacom_remote *remote = wacom->remote;
 
-	wacom_wac->serial[index] = serial;
-
-	buf = kzalloc(WAC_REMOTE_SERIAL_MAX_STRLEN, GFP_KERNEL);
-	if (!buf)
+	remote->remotes[index].group.name = devm_kasprintf(&wacom->hdev->dev,
+							  GFP_KERNEL,
+							  "%d", serial);
+	if (!remote->remotes[index].group.name)
 		return -ENOMEM;
-	snprintf(buf, WAC_REMOTE_SERIAL_MAX_STRLEN, "%d", serial);
-	wacom->remote_group[index].name = buf;
 
-	error = sysfs_create_group(wacom->remote_dir,
-				   &wacom->remote_group[index]);
+	error = __wacom_devm_sysfs_create_group(wacom, remote->remote_dir,
+						&remote->remotes[index].group);
 	if (error) {
+		remote->remotes[index].group.name = NULL;
 		hid_err(wacom->hdev,
 			"cannot create sysfs group err: %d\n", error);
-		kobject_put(wacom->remote_dir);
 		return error;
 	}
 
 	return 0;
 }
 
-void wacom_remote_destroy_attr_group(struct wacom *wacom, __u32 serial)
-{
-	struct wacom_wac *wacom_wac = &wacom->wacom_wac;
-	int i;
-
-	if (!serial)
-		return;
-
-	for (i = 0; i < WACOM_MAX_REMOTES; i++) {
-		if (wacom_wac->serial[i] == serial) {
-			wacom_wac->serial[i] = 0;
-			wacom->led.select[i] = WACOM_STATUS_UNKNOWN;
-			if (wacom->remote_group[i].name) {
-				sysfs_remove_group(wacom->remote_dir,
-						   &wacom->remote_group[i]);
-				kfree(wacom->remote_group[i].name);
-				wacom->remote_group[i].name = NULL;
-			}
-		}
-	}
-}
-
 static int wacom_cmd_unpair_remote(struct wacom *wacom, unsigned char selector)
 {
 	const size_t buf_size = 2;
@@ -1316,27 +1657,57 @@ static const struct attribute *remote_unpair_attrs[] = {
 	NULL
 };
 
-static int wacom_initialize_remote(struct wacom *wacom)
+static void wacom_remotes_destroy(void *data)
+{
+	struct wacom *wacom = data;
+	struct wacom_remote *remote = wacom->remote;
+
+	if (!remote)
+		return;
+
+	kobject_put(remote->remote_dir);
+	kfifo_free(&remote->remote_fifo);
+	wacom->remote = NULL;
+}
+
+static int wacom_initialize_remotes(struct wacom *wacom)
 {
 	int error = 0;
-	struct wacom_wac *wacom_wac = &(wacom->wacom_wac);
+	struct wacom_remote *remote;
 	int i;
 
 	if (wacom->wacom_wac.features.type != REMOTE)
 		return 0;
 
-	wacom->remote_group[0] = remote0_serial_group;
-	wacom->remote_group[1] = remote1_serial_group;
-	wacom->remote_group[2] = remote2_serial_group;
-	wacom->remote_group[3] = remote3_serial_group;
-	wacom->remote_group[4] = remote4_serial_group;
+	remote = devm_kzalloc(&wacom->hdev->dev, sizeof(*wacom->remote),
+			      GFP_KERNEL);
+	if (!remote)
+		return -ENOMEM;
+
+	wacom->remote = remote;
 
-	wacom->remote_dir = kobject_create_and_add("wacom_remote",
-						   &wacom->hdev->dev.kobj);
-	if (!wacom->remote_dir)
+	spin_lock_init(&remote->remote_lock);
+
+	error = kfifo_alloc(&remote->remote_fifo,
+			5 * sizeof(struct wacom_remote_data),
+			GFP_KERNEL);
+	if (error) {
+		hid_err(wacom->hdev, "failed allocating remote_fifo\n");
 		return -ENOMEM;
+	}
 
-	error = sysfs_create_files(wacom->remote_dir, remote_unpair_attrs);
+	remote->remotes[0].group = remote0_serial_group;
+	remote->remotes[1].group = remote1_serial_group;
+	remote->remotes[2].group = remote2_serial_group;
+	remote->remotes[3].group = remote3_serial_group;
+	remote->remotes[4].group = remote4_serial_group;
+
+	remote->remote_dir = kobject_create_and_add("wacom_remote",
+						    &wacom->hdev->dev.kobj);
+	if (!remote->remote_dir)
+		return -ENOMEM;
+
+	error = sysfs_create_files(remote->remote_dir, remote_unpair_attrs);
 
 	if (error) {
 		hid_err(wacom->hdev,
@@ -1345,10 +1716,15 @@ static int wacom_initialize_remote(struct wacom *wacom)
 	}
 
 	for (i = 0; i < WACOM_MAX_REMOTES; i++) {
-		wacom->led.select[i] = WACOM_STATUS_UNKNOWN;
-		wacom_wac->serial[i] = 0;
+		wacom->led.groups[i].select = WACOM_STATUS_UNKNOWN;
+		remote->remotes[i].serial = 0;
 	}
 
+	error = devm_add_action_or_reset(&wacom->hdev->dev,
+					 wacom_remotes_destroy, wacom);
+	if (error)
+		return error;
+
 	return 0;
 }
 
@@ -1358,7 +1734,7 @@ static struct input_dev *wacom_allocate_input(struct wacom *wacom)
 	struct hid_device *hdev = wacom->hdev;
 	struct wacom_wac *wacom_wac = &(wacom->wacom_wac);
 
-	input_dev = input_allocate_device();
+	input_dev = devm_input_allocate_device(&hdev->dev);
 	if (!input_dev)
 		return NULL;
 
@@ -1377,36 +1753,6 @@ static struct input_dev *wacom_allocate_input(struct wacom *wacom)
 	return input_dev;
 }
 
-static void wacom_clean_inputs(struct wacom *wacom)
-{
-	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.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)
-			input_unregister_device(wacom->wacom_wac.pad_input);
-		else
-			input_free_device(wacom->wacom_wac.pad_input);
-	}
-	kobject_put(wacom->remote_dir);
-	wacom->wacom_wac.pen_input = NULL;
-	wacom->wacom_wac.touch_input = NULL;
-	wacom->wacom_wac.pad_input = NULL;
-	wacom->wacom_wac.pen_registered = false;
-	wacom->wacom_wac.touch_registered = false;
-	wacom->wacom_wac.pad_registered = false;
-	wacom_destroy_leds(wacom);
-}
-
 static int wacom_allocate_inputs(struct wacom *wacom)
 {
 	struct wacom_wac *wacom_wac = &(wacom->wacom_wac);
@@ -1414,10 +1760,10 @@ static int wacom_allocate_inputs(struct wacom *wacom)
 	wacom_wac->pen_input = wacom_allocate_input(wacom);
 	wacom_wac->touch_input = wacom_allocate_input(wacom);
 	wacom_wac->pad_input = wacom_allocate_input(wacom);
-	if (!wacom_wac->pen_input || !wacom_wac->touch_input || !wacom_wac->pad_input) {
-		wacom_clean_inputs(wacom);
+	if (!wacom_wac->pen_input ||
+	    !wacom_wac->touch_input ||
+	    !wacom_wac->pad_input)
 		return -ENOMEM;
-	}
 
 	wacom_wac->pen_input->name = wacom_wac->pen_name;
 	wacom_wac->touch_input->name = wacom_wac->touch_name;
@@ -1448,8 +1794,7 @@ static int wacom_register_inputs(struct wacom *wacom)
 	} else {
 		error = input_register_device(pen_input_dev);
 		if (error)
-			goto fail_register_pen_input;
-		wacom_wac->pen_registered = true;
+			goto fail;
 	}
 
 	error = wacom_setup_touch_input_capabilities(touch_input_dev, wacom_wac);
@@ -1461,8 +1806,7 @@ static int wacom_register_inputs(struct wacom *wacom)
 	} else {
 		error = input_register_device(touch_input_dev);
 		if (error)
-			goto fail_register_touch_input;
-		wacom_wac->touch_registered = true;
+			goto fail;
 	}
 
 	error = wacom_setup_pad_input_capabilities(pad_input_dev, wacom_wac);
@@ -1474,37 +1818,15 @@ static int wacom_register_inputs(struct wacom *wacom)
 	} else {
 		error = input_register_device(pad_input_dev);
 		if (error)
-			goto fail_register_pad_input;
-		wacom_wac->pad_registered = true;
-
-		error = wacom_initialize_leds(wacom);
-		if (error)
-			goto fail_leds;
-
-		error = wacom_initialize_remote(wacom);
-		if (error)
-			goto fail_remote;
+			goto fail;
 	}
 
 	return 0;
 
-fail_remote:
-	wacom_destroy_leds(wacom);
-fail_leds:
-	input_unregister_device(pad_input_dev);
-	pad_input_dev = NULL;
-	wacom_wac->pad_registered = false;
-fail_register_pad_input:
-	if (touch_input_dev)
-		input_unregister_device(touch_input_dev);
+fail:
+	wacom_wac->pad_input = NULL;
 	wacom_wac->touch_input = NULL;
-	wacom_wac->touch_registered = false;
-fail_register_touch_input:
-	if (pen_input_dev)
-		input_unregister_device(pen_input_dev);
 	wacom_wac->pen_input = NULL;
-	wacom_wac->pen_registered = false;
-fail_register_pen_input:
 	return error;
 }
 
@@ -1543,14 +1865,14 @@ static void wacom_calculate_res(struct wacom_features *features)
 
 void wacom_battery_work(struct work_struct *work)
 {
-	struct wacom *wacom = container_of(work, struct wacom, work);
+	struct wacom *wacom = container_of(work, struct wacom, battery_work);
 
 	if ((wacom->wacom_wac.features.quirks & WACOM_QUIRK_BATTERY) &&
-	     !wacom->battery) {
+	     !wacom->battery.battery) {
 		wacom_initialize_battery(wacom);
 	}
 	else if (!(wacom->wacom_wac.features.quirks & WACOM_QUIRK_BATTERY) &&
-		 wacom->battery) {
+		 wacom->battery.battery) {
 		wacom_destroy_battery(wacom);
 	}
 }
@@ -1606,6 +1928,9 @@ static void wacom_update_name(struct wacom *wacom, const char *suffix)
 		strlcpy(name, features->name, sizeof(name));
 	}
 
+	snprintf(wacom_wac->name, sizeof(wacom_wac->name), "%s%s",
+		 name, suffix);
+
 	/* Append the device type to the name */
 	snprintf(wacom_wac->pen_name, sizeof(wacom_wac->pen_name),
 		"%s%s Pen", name, suffix);
@@ -1615,6 +1940,22 @@ static void wacom_update_name(struct wacom *wacom, const char *suffix)
 		"%s%s Pad", name, suffix);
 }
 
+static void wacom_release_resources(struct wacom *wacom)
+{
+	struct hid_device *hdev = wacom->hdev;
+
+	if (!wacom->resources)
+		return;
+
+	devres_release_group(&hdev->dev, wacom);
+
+	wacom->resources = false;
+
+	wacom->wacom_wac.pen_input = NULL;
+	wacom->wacom_wac.touch_input = NULL;
+	wacom->wacom_wac.pad_input = NULL;
+}
+
 static int wacom_parse_and_register(struct wacom *wacom, bool wireless)
 {
 	struct wacom_wac *wacom_wac = &wacom->wacom_wac;
@@ -1627,9 +1968,14 @@ static int wacom_parse_and_register(struct wacom *wacom, bool wireless)
 	if (features->pktlen > WACOM_PKGLEN_MAX)
 		return -EINVAL;
 
+	if (!devres_open_group(&hdev->dev, wacom, GFP_KERNEL))
+		return -ENOMEM;
+
+	wacom->resources = true;
+
 	error = wacom_allocate_inputs(wacom);
 	if (error)
-		return error;
+		goto fail;
 
 	/*
 	 * Bamboo Pad has a generic hid handling for the Pen, and we switch it
@@ -1642,7 +1988,7 @@ static int wacom_parse_and_register(struct wacom *wacom, bool wireless)
 		} else if ((features->pktlen != WACOM_PKGLEN_BPAD_TOUCH) &&
 			   (features->pktlen != WACOM_PKGLEN_BPAD_TOUCH_USB)) {
 			error = -ENODEV;
-			goto fail_allocate_inputs;
+			goto fail;
 		}
 	}
 
@@ -1662,7 +2008,7 @@ static int wacom_parse_and_register(struct wacom *wacom, bool wireless)
 			 error ? "Ignoring" : "Assuming pen");
 
 		if (error)
-			goto fail_parsed;
+			goto fail;
 
 		features->device_type |= WACOM_DEVICETYPE_PEN;
 	}
@@ -1673,18 +2019,28 @@ static int wacom_parse_and_register(struct wacom *wacom, bool wireless)
 
 	error = wacom_add_shared_data(hdev);
 	if (error)
-		goto fail_shared_data;
+		goto fail;
 
 	if (!(features->device_type & WACOM_DEVICETYPE_WL_MONITOR) &&
 	     (features->quirks & WACOM_QUIRK_BATTERY)) {
 		error = wacom_initialize_battery(wacom);
 		if (error)
-			goto fail_battery;
+			goto fail;
 	}
 
 	error = wacom_register_inputs(wacom);
 	if (error)
-		goto fail_register_inputs;
+		goto fail;
+
+	if (wacom->wacom_wac.features.device_type & WACOM_DEVICETYPE_PAD) {
+		error = wacom_initialize_leds(wacom);
+		if (error)
+			goto fail;
+
+		error = wacom_initialize_remotes(wacom);
+		if (error)
+			goto fail;
+	}
 
 	if (features->type == HID_GENERIC)
 		connect_mask |= HID_CONNECT_DRIVER;
@@ -1693,7 +2049,7 @@ static int wacom_parse_and_register(struct wacom *wacom, bool wireless)
 	error = hid_hw_start(hdev, connect_mask);
 	if (error) {
 		hid_err(hdev, "hw start failed\n");
-		goto fail_hw_start;
+		goto fail;
 	}
 
 	if (!wireless) {
@@ -1705,7 +2061,7 @@ static int wacom_parse_and_register(struct wacom *wacom, bool wireless)
 	if ((features->type == BAMBOO_TOUCH) &&
 	    (features->device_type & WACOM_DEVICETYPE_PEN)) {
 		error = -ENODEV;
-		goto fail_hw_start;
+		goto fail_quirks;
 	}
 
 	/* pen only Bamboo neither support touch nor pad */
@@ -1713,37 +2069,33 @@ static int wacom_parse_and_register(struct wacom *wacom, bool wireless)
 	    ((features->device_type & WACOM_DEVICETYPE_TOUCH) ||
 	    (features->device_type & WACOM_DEVICETYPE_PAD))) {
 		error = -ENODEV;
-		goto fail_hw_start;
+		goto fail_quirks;
 	}
 
 	if (features->device_type & WACOM_DEVICETYPE_WL_MONITOR)
 		error = hid_hw_open(hdev);
 
 	if ((wacom_wac->features.type == INTUOSHT ||
-	    wacom_wac->features.type == INTUOSHT2) &&
+	     wacom_wac->features.type == INTUOSHT2) &&
 	    (wacom_wac->features.device_type & WACOM_DEVICETYPE_TOUCH)) {
-			wacom_wac->shared->touch_input = wacom_wac->touch_input;
+		wacom_wac->shared->type = wacom_wac->features.type;
+		wacom_wac->shared->touch_input = wacom_wac->touch_input;
 	}
 
+	devres_close_group(&hdev->dev, wacom);
+
 	return 0;
 
-fail_hw_start:
+fail_quirks:
 	hid_hw_stop(hdev);
-fail_register_inputs:
-	wacom_clean_inputs(wacom);
-	wacom_destroy_battery(wacom);
-fail_battery:
-	wacom_remove_shared_data(wacom);
-fail_shared_data:
-fail_parsed:
-fail_allocate_inputs:
-	wacom_clean_inputs(wacom);
+fail:
+	wacom_release_resources(wacom);
 	return error;
 }
 
 static void wacom_wireless_work(struct work_struct *work)
 {
-	struct wacom *wacom = container_of(work, struct wacom, work);
+	struct wacom *wacom = container_of(work, struct wacom, wireless_work);
 	struct usb_device *usbdev = wacom->usbdev;
 	struct wacom_wac *wacom_wac = &wacom->wacom_wac;
 	struct hid_device *hdev1, *hdev2;
@@ -1762,17 +2114,16 @@ static void wacom_wireless_work(struct work_struct *work)
 	hdev1 = usb_get_intfdata(usbdev->config->interface[1]);
 	wacom1 = hid_get_drvdata(hdev1);
 	wacom_wac1 = &(wacom1->wacom_wac);
-	wacom_clean_inputs(wacom1);
+	wacom_release_resources(wacom1);
 
 	/* Touch interface */
 	hdev2 = usb_get_intfdata(usbdev->config->interface[2]);
 	wacom2 = hid_get_drvdata(hdev2);
 	wacom_wac2 = &(wacom2->wacom_wac);
-	wacom_clean_inputs(wacom2);
+	wacom_release_resources(wacom2);
 
 	if (wacom_wac->pid == 0) {
 		hid_info(wacom->hdev, "wireless tablet disconnected\n");
-		wacom_wac1->shared->type = 0;
 	} else {
 		const struct hid_device_id *id = wacom_ids;
 
@@ -1814,6 +2165,8 @@ static void wacom_wireless_work(struct work_struct *work)
 				goto fail;
 		}
 
+		strlcpy(wacom_wac->name, wacom_wac1->name,
+			sizeof(wacom_wac->name));
 		error = wacom_initialize_battery(wacom);
 		if (error)
 			goto fail;
@@ -1822,11 +2175,177 @@ static void wacom_wireless_work(struct work_struct *work)
 	return;
 
 fail:
-	wacom_clean_inputs(wacom1);
-	wacom_clean_inputs(wacom2);
+	wacom_release_resources(wacom1);
+	wacom_release_resources(wacom2);
 	return;
 }
 
+static void wacom_remote_destroy_one(struct wacom *wacom, unsigned int index)
+{
+	struct wacom_remote *remote = wacom->remote;
+	u32 serial = remote->remotes[index].serial;
+	int i;
+	unsigned long flags;
+
+	spin_lock_irqsave(&remote->remote_lock, flags);
+	remote->remotes[index].registered = false;
+	spin_unlock_irqrestore(&remote->remote_lock, flags);
+
+	if (remote->remotes[index].battery.battery)
+		devres_release_group(&wacom->hdev->dev,
+				     &remote->remotes[index].battery.bat_desc);
+
+	if (remote->remotes[index].group.name)
+		devres_release_group(&wacom->hdev->dev,
+				     &remote->remotes[index]);
+
+	for (i = 0; i < WACOM_MAX_REMOTES; i++) {
+		if (remote->remotes[i].serial == serial) {
+			remote->remotes[i].serial = 0;
+			remote->remotes[i].group.name = NULL;
+			remote->remotes[i].registered = false;
+			remote->remotes[i].battery.battery = NULL;
+			wacom->led.groups[i].select = WACOM_STATUS_UNKNOWN;
+		}
+	}
+}
+
+static int wacom_remote_create_one(struct wacom *wacom, u32 serial,
+				   unsigned int index)
+{
+	struct wacom_remote *remote = wacom->remote;
+	struct device *dev = &wacom->hdev->dev;
+	int error, k;
+
+	/* A remote can pair more than once with an EKR,
+	 * check to make sure this serial isn't already paired.
+	 */
+	for (k = 0; k < WACOM_MAX_REMOTES; k++) {
+		if (remote->remotes[k].serial == serial)
+			break;
+	}
+
+	if (k < WACOM_MAX_REMOTES) {
+		remote->remotes[index].serial = serial;
+		return 0;
+	}
+
+	if (!devres_open_group(dev, &remote->remotes[index], GFP_KERNEL))
+		return -ENOMEM;
+
+	error = wacom_remote_create_attr_group(wacom, serial, index);
+	if (error)
+		goto fail;
+
+	remote->remotes[index].input = wacom_allocate_input(wacom);
+	if (!remote->remotes[index].input) {
+		error = -ENOMEM;
+		goto fail;
+	}
+	remote->remotes[index].input->uniq = remote->remotes[index].group.name;
+	remote->remotes[index].input->name = wacom->wacom_wac.pad_name;
+
+	if (!remote->remotes[index].input->name) {
+		error = -EINVAL;
+		goto fail;
+	}
+
+	error = wacom_setup_pad_input_capabilities(remote->remotes[index].input,
+						   &wacom->wacom_wac);
+	if (error)
+		goto fail;
+
+	remote->remotes[index].serial = serial;
+
+	error = input_register_device(remote->remotes[index].input);
+	if (error)
+		goto fail;
+
+	error = wacom_led_groups_alloc_and_register_one(
+					&remote->remotes[index].input->dev,
+					wacom, index, 3, true);
+	if (error)
+		goto fail;
+
+	remote->remotes[index].registered = true;
+
+	devres_close_group(dev, &remote->remotes[index]);
+	return 0;
+
+fail:
+	devres_release_group(dev, &remote->remotes[index]);
+	remote->remotes[index].serial = 0;
+	return error;
+}
+
+static int wacom_remote_attach_battery(struct wacom *wacom, int index)
+{
+	struct wacom_remote *remote = wacom->remote;
+	int error;
+
+	if (!remote->remotes[index].registered)
+		return 0;
+
+	if (remote->remotes[index].battery.battery)
+		return 0;
+
+	if (wacom->led.groups[index].select == WACOM_STATUS_UNKNOWN)
+		return 0;
+
+	error = __wacom_initialize_battery(wacom,
+					&wacom->remote->remotes[index].battery);
+	if (error)
+		return error;
+
+	return 0;
+}
+
+static void wacom_remote_work(struct work_struct *work)
+{
+	struct wacom *wacom = container_of(work, struct wacom, remote_work);
+	struct wacom_remote *remote = wacom->remote;
+	struct wacom_remote_data data;
+	unsigned long flags;
+	unsigned int count;
+	u32 serial;
+	int i;
+
+	spin_lock_irqsave(&remote->remote_lock, flags);
+
+	count = kfifo_out(&remote->remote_fifo, &data, sizeof(data));
+
+	if (count != sizeof(data)) {
+		hid_err(wacom->hdev,
+			"workitem triggered without status available\n");
+		spin_unlock_irqrestore(&remote->remote_lock, flags);
+		return;
+	}
+
+	if (!kfifo_is_empty(&remote->remote_fifo))
+		wacom_schedule_work(&wacom->wacom_wac, WACOM_WORKER_REMOTE);
+
+	spin_unlock_irqrestore(&remote->remote_lock, flags);
+
+	for (i = 0; i < WACOM_MAX_REMOTES; i++) {
+		serial = data.remote[i].serial;
+		if (data.remote[i].connected) {
+
+			if (remote->remotes[i].serial == serial) {
+				wacom_remote_attach_battery(wacom, i);
+				continue;
+			}
+
+			if (remote->remotes[i].serial)
+				wacom_remote_destroy_one(wacom, i);
+
+			wacom_remote_create_one(wacom, serial, i);
+
+		} else if (remote->remotes[i].serial) {
+			wacom_remote_destroy_one(wacom, i);
+		}
+	}
+}
+
 static int wacom_probe(struct hid_device *hdev,
 		const struct hid_device_id *id)
 {
@@ -1845,7 +2364,7 @@ static int wacom_probe(struct hid_device *hdev,
 	/* hid-core sets this quirk for the boot interface */
 	hdev->quirks &= ~HID_QUIRK_NOGET;
 
-	wacom = kzalloc(sizeof(struct wacom), GFP_KERNEL);
+	wacom = devm_kzalloc(&hdev->dev, sizeof(struct wacom), GFP_KERNEL);
 	if (!wacom)
 		return -ENOMEM;
 
@@ -1858,7 +2377,7 @@ static int wacom_probe(struct hid_device *hdev,
 
 	if (features->check_for_hid_type && features->hid_type != hdev->type) {
 		error = -ENODEV;
-		goto fail_type;
+		goto fail;
 	}
 
 	wacom_wac->hid_data.inputmode = -1;
@@ -1867,18 +2386,20 @@ static int wacom_probe(struct hid_device *hdev,
 	wacom->usbdev = dev;
 	wacom->intf = intf;
 	mutex_init(&wacom->lock);
-	INIT_WORK(&wacom->work, wacom_wireless_work);
+	INIT_WORK(&wacom->wireless_work, wacom_wireless_work);
+	INIT_WORK(&wacom->battery_work, wacom_battery_work);
+	INIT_WORK(&wacom->remote_work, wacom_remote_work);
 
 	/* ask for the report descriptor to be loaded by HID */
 	error = hid_parse(hdev);
 	if (error) {
 		hid_err(hdev, "parse failed\n");
-		goto fail_parse;
+		goto fail;
 	}
 
 	error = wacom_parse_and_register(wacom, false);
 	if (error)
-		goto fail_parse;
+		goto fail;
 
 	if (hdev->bus == BUS_BLUETOOTH) {
 		error = device_create_file(&hdev->dev, &dev_attr_speed);
@@ -1890,9 +2411,7 @@ static int wacom_probe(struct hid_device *hdev,
 
 	return 0;
 
-fail_type:
-fail_parse:
-	kfree(wacom);
+fail:
 	hid_set_drvdata(hdev, NULL);
 	return error;
 }
@@ -1908,15 +2427,13 @@ static void wacom_remove(struct hid_device *hdev)
 
 	hid_hw_stop(hdev);
 
-	cancel_work_sync(&wacom->work);
-	wacom_clean_inputs(wacom);
+	cancel_work_sync(&wacom->wireless_work);
+	cancel_work_sync(&wacom->battery_work);
+	cancel_work_sync(&wacom->remote_work);
 	if (hdev->bus == BUS_BLUETOOTH)
 		device_remove_file(&hdev->dev, &dev_attr_speed);
-	wacom_destroy_battery(wacom);
-	wacom_remove_shared_data(wacom);
 
 	hid_set_drvdata(hdev, NULL);
-	kfree(wacom);
 }
 
 #ifdef CONFIG_PM
diff --git a/drivers/hid/wacom_wac.c b/drivers/hid/wacom_wac.c
index 1eae13cdc502..1cb79925730d 100644
--- a/drivers/hid/wacom_wac.c
+++ b/drivers/hid/wacom_wac.c
@@ -34,6 +34,10 @@
  */
 #define WACOM_CONTACT_AREA_SCALE 2607
 
+static bool touch_arbitration = 1;
+module_param(touch_arbitration, bool, 0644);
+MODULE_PARM_DESC(touch_arbitration, " on (Y) off (N)");
+
 static void wacom_report_numbered_buttons(struct input_dev *input_dev,
 				int button_count, int mask);
 
@@ -48,25 +52,34 @@ static unsigned short batcap_gr[8] = { 1, 15, 25, 35, 50, 70, 100, 100 };
  */
 static unsigned short batcap_i4[8] = { 1, 15, 30, 45, 60, 70, 85, 100 };
 
+static void __wacom_notify_battery(struct wacom_battery *battery,
+				   int bat_capacity, bool bat_charging,
+				   bool bat_connected, bool ps_connected)
+{
+	bool changed = battery->battery_capacity != bat_capacity  ||
+		       battery->bat_charging     != bat_charging  ||
+		       battery->bat_connected    != bat_connected ||
+		       battery->ps_connected     != ps_connected;
+
+	if (changed) {
+		battery->battery_capacity = bat_capacity;
+		battery->bat_charging = bat_charging;
+		battery->bat_connected = bat_connected;
+		battery->ps_connected = ps_connected;
+
+		if (battery->battery)
+			power_supply_changed(battery->battery);
+	}
+}
+
 static void wacom_notify_battery(struct wacom_wac *wacom_wac,
 	int bat_capacity, bool bat_charging, bool bat_connected,
 	bool ps_connected)
 {
 	struct wacom *wacom = container_of(wacom_wac, struct wacom, wacom_wac);
-	bool changed = wacom_wac->battery_capacity != bat_capacity  ||
-		       wacom_wac->bat_charging     != bat_charging  ||
-		       wacom_wac->bat_connected    != bat_connected ||
-		       wacom_wac->ps_connected     != ps_connected;
 
-	if (changed) {
-		wacom_wac->battery_capacity = bat_capacity;
-		wacom_wac->bat_charging = bat_charging;
-		wacom_wac->bat_connected = bat_connected;
-		wacom_wac->ps_connected = ps_connected;
-
-		if (wacom->battery)
-			power_supply_changed(wacom->battery);
-	}
+	__wacom_notify_battery(&wacom->battery, bat_capacity, bat_charging,
+			       bat_connected, ps_connected);
 }
 
 static int wacom_penpartner_irq(struct wacom_wac *wacom)
@@ -751,22 +764,37 @@ static int wacom_intuos_inout(struct wacom_wac *wacom)
 static int wacom_remote_irq(struct wacom_wac *wacom_wac, size_t len)
 {
 	unsigned char *data = wacom_wac->data;
-	struct input_dev *input = wacom_wac->pad_input;
+	struct input_dev *input;
 	struct wacom *wacom = container_of(wacom_wac, struct wacom, wacom_wac);
-	struct wacom_features *features = &wacom_wac->features;
+	struct wacom_remote *remote = wacom->remote;
 	int bat_charging, bat_percent, touch_ring_mode;
 	__u32 serial;
-	int i;
+	int i, index = -1;
+	unsigned long flags;
 
 	if (data[0] != WACOM_REPORT_REMOTE) {
-		dev_dbg(input->dev.parent,
-			"%s: received unknown report #%d", __func__, data[0]);
+		hid_dbg(wacom->hdev, "%s: received unknown report #%d",
+			__func__, data[0]);
 		return 0;
 	}
 
 	serial = data[3] + (data[4] << 8) + (data[5] << 16);
 	wacom_wac->id[0] = PAD_DEVICE_ID;
 
+	spin_lock_irqsave(&remote->remote_lock, flags);
+
+	for (i = 0; i < WACOM_MAX_REMOTES; i++) {
+		if (remote->remotes[i].serial == serial) {
+			index = i;
+			break;
+		}
+	}
+
+	if (index < 0 || !remote->remotes[index].registered)
+		goto out;
+
+	input = remote->remotes[index].input;
+
 	input_report_key(input, BTN_0, (data[9] & 0x01));
 	input_report_key(input, BTN_1, (data[9] & 0x02));
 	input_report_key(input, BTN_2, (data[9] & 0x04));
@@ -803,73 +831,69 @@ static int wacom_remote_irq(struct wacom_wac *wacom_wac, size_t len)
 
 	input_event(input, EV_MSC, MSC_SERIAL, serial);
 
+	input_sync(input);
+
 	/*Which mode select (LED light) is currently on?*/
 	touch_ring_mode = (data[11] & 0xC0) >> 6;
 
 	for (i = 0; i < WACOM_MAX_REMOTES; i++) {
-		if (wacom_wac->serial[i] == serial)
-			wacom->led.select[i] = touch_ring_mode;
-	}
-
-	if (!wacom->battery &&
-	    !(features->quirks & WACOM_QUIRK_BATTERY)) {
-		features->quirks |= WACOM_QUIRK_BATTERY;
-		INIT_WORK(&wacom->work, wacom_battery_work);
-		wacom_schedule_work(wacom_wac);
+		if (remote->remotes[i].serial == serial)
+			wacom->led.groups[i].select = touch_ring_mode;
 	}
 
-	wacom_notify_battery(wacom_wac, bat_percent, bat_charging, 1,
-			     bat_charging);
+	__wacom_notify_battery(&remote->remotes[index].battery, bat_percent,
+				bat_charging, 1, bat_charging);
 
-	return 1;
+out:
+	spin_unlock_irqrestore(&remote->remote_lock, flags);
+	return 0;
 }
 
-static int wacom_remote_status_irq(struct wacom_wac *wacom_wac, size_t len)
+static void wacom_remote_status_irq(struct wacom_wac *wacom_wac, size_t len)
 {
 	struct wacom *wacom = container_of(wacom_wac, struct wacom, wacom_wac);
 	unsigned char *data = wacom_wac->data;
-	int i;
+	struct wacom_remote *remote = wacom->remote;
+	struct wacom_remote_data remote_data;
+	unsigned long flags;
+	int i, ret;
 
 	if (data[0] != WACOM_REPORT_DEVICE_LIST)
-		return 0;
+		return;
+
+	memset(&remote_data, 0, sizeof(struct wacom_remote_data));
 
 	for (i = 0; i < WACOM_MAX_REMOTES; i++) {
 		int j = i * 6;
 		int serial = (data[j+6] << 16) + (data[j+5] << 8) + data[j+4];
 		bool connected = data[j+2];
 
-		if (connected) {
-			int k;
+		remote_data.remote[i].serial = serial;
+		remote_data.remote[i].connected = connected;
+	}
 
-			if (wacom_wac->serial[i] == serial)
-				continue;
+	spin_lock_irqsave(&remote->remote_lock, flags);
 
-			if (wacom_wac->serial[i]) {
-				wacom_remote_destroy_attr_group(wacom,
-							wacom_wac->serial[i]);
-			}
+	ret = kfifo_in(&remote->remote_fifo, &remote_data, sizeof(remote_data));
+	if (ret != sizeof(remote_data)) {
+		spin_unlock_irqrestore(&remote->remote_lock, flags);
+		hid_err(wacom->hdev, "Can't queue Remote status event.\n");
+		return;
+	}
 
-			/* A remote can pair more than once with an EKR,
-			 * check to make sure this serial isn't already paired.
-			 */
-			for (k = 0; k < WACOM_MAX_REMOTES; k++) {
-				if (wacom_wac->serial[k] == serial)
-					break;
-			}
+	spin_unlock_irqrestore(&remote->remote_lock, flags);
 
-			if (k < WACOM_MAX_REMOTES) {
-				wacom_wac->serial[i] = serial;
-				continue;
-			}
-			wacom_remote_create_attr_group(wacom, serial, i);
+	wacom_schedule_work(wacom_wac, WACOM_WORKER_REMOTE);
+}
 
-		} else if (wacom_wac->serial[i]) {
-			wacom_remote_destroy_attr_group(wacom,
-							wacom_wac->serial[i]);
-		}
-	}
+static inline bool report_touch_events(struct wacom_wac *wacom)
+{
+	return (touch_arbitration ? !wacom->shared->stylus_in_proximity : 1);
+}
 
-	return 0;
+static inline bool delay_pen_events(struct wacom_wac *wacom)
+{
+	return (wacom->shared->touch_down && touch_arbitration);
 }
 
 static int wacom_intuos_general(struct wacom_wac *wacom)
@@ -885,7 +909,7 @@ static int wacom_intuos_general(struct wacom_wac *wacom)
 		data[0] != WACOM_REPORT_INTUOS_PEN)
 		return 0;
 
-	if (wacom->shared->touch_down)
+	if (delay_pen_events(wacom))
 		return 1;
 
 	/* don't report events if we don't know the tool ID */
@@ -1145,7 +1169,7 @@ static int wacom_wac_finger_count_touches(struct wacom_wac *wacom)
 
 	if (touch_max == 1)
 		return test_bit(BTN_TOUCH, input->key) &&
-		       !wacom->shared->stylus_in_proximity;
+			report_touch_events(wacom);
 
 	for (i = 0; i < input->mt->num_slots; i++) {
 		struct input_mt_slot *ps = &input->mt->slots[i];
@@ -1186,7 +1210,7 @@ static int wacom_24hdt_irq(struct wacom_wac *wacom)
 
 	for (i = 0; i < contacts_to_send; i++) {
 		int offset = (byte_per_packet * i) + 1;
-		bool touch = (data[offset] & 0x1) && !wacom->shared->stylus_in_proximity;
+		bool touch = (data[offset] & 0x1) && report_touch_events(wacom);
 		int slot = input_mt_get_slot_by_key(input, data[offset + 1]);
 
 		if (slot < 0)
@@ -1250,7 +1274,7 @@ static int wacom_mt_touch(struct wacom_wac *wacom)
 
 	for (i = 0; i < contacts_to_send; i++) {
 		int offset = (WACOM_BYTES_PER_MT_PACKET + x_offset) * i + 3;
-		bool touch = (data[offset] & 0x1) && !wacom->shared->stylus_in_proximity;
+		bool touch = (data[offset] & 0x1) && report_touch_events(wacom);
 		int id = get_unaligned_le16(&data[offset + 1]);
 		int slot = input_mt_get_slot_by_key(input, id);
 
@@ -1284,7 +1308,7 @@ static int wacom_tpc_mt_touch(struct wacom_wac *wacom)
 
 	for (i = 0; i < 2; i++) {
 		int p = data[1] & (1 << i);
-		bool touch = p && !wacom->shared->stylus_in_proximity;
+		bool touch = p && report_touch_events(wacom);
 
 		input_mt_slot(input, i);
 		input_mt_report_slot_state(input, MT_TOOL_FINGER, touch);
@@ -1308,7 +1332,7 @@ static int wacom_tpc_single_touch(struct wacom_wac *wacom, size_t len)
 {
 	unsigned char *data = wacom->data;
 	struct input_dev *input = wacom->touch_input;
-	bool prox = !wacom->shared->stylus_in_proximity;
+	bool prox = report_touch_events(wacom);
 	int x = 0, y = 0;
 
 	if (wacom->features.touch_max > 1 || len > WACOM_PKGLEN_TPC2FG)
@@ -1353,8 +1377,10 @@ static int wacom_tpc_pen(struct wacom_wac *wacom)
 	/* keep pen state for touch events */
 	wacom->shared->stylus_in_proximity = prox;
 
-	/* send pen events only when touch is up or forced out */
-	if (!wacom->shared->touch_down) {
+	/* send pen events only when touch is up or forced out
+	 * or touch arbitration is off
+	 */
+	if (!delay_pen_events(wacom)) {
 		input_report_key(input, BTN_STYLUS, data[1] & 0x02);
 		input_report_key(input, BTN_STYLUS2, data[1] & 0x10);
 		input_report_abs(input, ABS_X, le16_to_cpup((__le16 *)&data[2]));
@@ -1496,8 +1522,10 @@ static int wacom_wac_pen_event(struct hid_device *hdev, struct hid_field *field,
 		return 0;
 	}
 
-	/* send pen events only when touch is up or forced out */
-	if (!usage->type || wacom_wac->shared->touch_down)
+	/* send pen events only when touch is up or forced out
+	 * or touch arbitration is off
+	 */
+	if (!usage->type || delay_pen_events(wacom_wac))
 		return 0;
 
 	input_event(input, usage->type, usage->code, value);
@@ -1527,8 +1555,7 @@ static void wacom_wac_pen_report(struct hid_device *hdev,
 	/* keep pen state for touch events */
 	wacom_wac->shared->stylus_in_proximity = prox;
 
-	/* send pen events only when touch is up or forced out */
-	if (!wacom_wac->shared->touch_down) {
+	if (!delay_pen_events(wacom_wac)) {
 		input_report_key(input, BTN_TOUCH,
 				wacom_wac->hid_data.tipswitch);
 		input_report_key(input, wacom_wac->tool[0], prox);
@@ -1544,13 +1571,11 @@ 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(input, usage, field, EV_ABS, ABS_X, 4);
 		else
@@ -1558,7 +1583,6 @@ static void wacom_wac_finger_usage_mapping(struct hid_device *hdev,
 					ABS_MT_POSITION_X, 4);
 		break;
 	case HID_GD_Y:
-		features->last_slot_field = usage->hid;
 		if (touch_max == 1)
 			wacom_map_usage(input, usage, field, EV_ABS, ABS_Y, 4);
 		else
@@ -1567,22 +1591,11 @@ static void wacom_wac_finger_usage_mapping(struct hid_device *hdev,
 		break;
 	case HID_DG_WIDTH:
 	case HID_DG_HEIGHT:
-		features->last_slot_field = usage->hid;
 		wacom_map_usage(input, usage, field, EV_ABS, ABS_MT_TOUCH_MAJOR, 0);
 		wacom_map_usage(input, usage, field, EV_ABS, ABS_MT_TOUCH_MINOR, 0);
 		input_set_abs_params(input, ABS_MT_ORIENTATION, 0, 1, 0, 0);
 		break;
-	case HID_DG_CONTACTID:
-		features->last_slot_field = usage->hid;
-		break;
-	case HID_DG_INRANGE:
-		features->last_slot_field = usage->hid;
-		break;
-	case HID_DG_INVERT:
-		features->last_slot_field = usage->hid;
-		break;
 	case HID_DG_TIPSWITCH:
-		features->last_slot_field = usage->hid;
 		wacom_map_usage(input, usage, field, EV_KEY, BTN_TOUCH, 0);
 		break;
 	case HID_DG_CONTACTCOUNT:
@@ -1599,7 +1612,7 @@ static void wacom_wac_finger_slot(struct wacom_wac *wacom_wac,
 	struct hid_data *hid_data = &wacom_wac->hid_data;
 	bool mt = wacom_wac->features.touch_max > 1;
 	bool prox = hid_data->tipswitch &&
-		    !wacom_wac->shared->stylus_in_proximity;
+		    report_touch_events(wacom_wac);
 
 	wacom_wac->hid_data.num_received++;
 	if (wacom_wac->hid_data.num_received > wacom_wac->hid_data.num_expected)
@@ -1660,7 +1673,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)
+		if (usage->hid == wacom_wac->hid_data.last_slot_field)
 			wacom_wac_finger_slot(wacom_wac, wacom_wac->touch_input);
 	}
 
@@ -1673,31 +1686,35 @@ static void wacom_wac_finger_pre_report(struct hid_device *hdev,
 	struct wacom *wacom = hid_get_drvdata(hdev);
 	struct wacom_wac *wacom_wac = &wacom->wacom_wac;
 	struct hid_data* hid_data = &wacom_wac->hid_data;
+	int i;
 
-	if (hid_data->cc_report != 0 &&
-	    hid_data->cc_report != report->id) {
-		int i;
-
-		hid_data->cc_report = report->id;
-		hid_data->cc_index = -1;
-		hid_data->cc_value_index = -1;
-
-		for (i = 0; i < report->maxfield; i++) {
-			struct hid_field *field = report->field[i];
-			int j;
-
-			for (j = 0; j < field->maxusage; j++) {
-				if (field->usage[j].hid == HID_DG_CONTACTCOUNT) {
-					hid_data->cc_index = i;
-					hid_data->cc_value_index = j;
-
-					/* break */
-					i = report->maxfield;
-					j = field->maxusage;
-				}
+	for (i = 0; i < report->maxfield; i++) {
+		struct hid_field *field = report->field[i];
+		int j;
+
+		for (j = 0; j < field->maxusage; j++) {
+			struct hid_usage *usage = &field->usage[j];
+
+			switch (usage->hid) {
+			case HID_GD_X:
+			case HID_GD_Y:
+			case HID_DG_WIDTH:
+			case HID_DG_HEIGHT:
+			case HID_DG_CONTACTID:
+			case HID_DG_INRANGE:
+			case HID_DG_INVERT:
+			case HID_DG_TIPSWITCH:
+				hid_data->last_slot_field = usage->hid;
+				break;
+			case HID_DG_CONTACTCOUNT:
+				hid_data->cc_report = report->id;
+				hid_data->cc_index = i;
+				hid_data->cc_value_index = j;
+				break;
 			}
 		}
 	}
+
 	if (hid_data->cc_report != 0 &&
 	    hid_data->cc_index >= 0) {
 		struct hid_field *field = report->field[hid_data->cc_index];
@@ -1740,10 +1757,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 wacom_features *features = &wacom_wac->features;
 
 	/* currently, only direct devices have proper hid report descriptors */
-	__set_bit(INPUT_PROP_DIRECT, wacom_wac->pen_input->propbit);
-	__set_bit(INPUT_PROP_DIRECT, wacom_wac->touch_input->propbit);
+	features->device_type |= WACOM_DEVICETYPE_DIRECT;
 
 	if (WACOM_PEN_FIELD(field))
 		return wacom_wac_pen_usage_mapping(hdev, field, usage);
@@ -1825,15 +1842,8 @@ static int wacom_bpt_touch(struct wacom_wac *wacom)
 
 	for (i = 0; i < 2; i++) {
 		int offset = (data[1] & 0x80) ? (8 * i) : (9 * i);
-		bool touch = data[offset + 3] & 0x80;
-
-		/*
-		 * Touch events need to be disabled while stylus is
-		 * in proximity because user's hand is resting on touchpad
-		 * and sending unwanted events.  User expects tablet buttons
-		 * to continue working though.
-		 */
-		touch = touch && !wacom->shared->stylus_in_proximity;
+		bool touch = report_touch_events(wacom)
+			   && (data[offset + 3] & 0x80);
 
 		input_mt_slot(input, i);
 		input_mt_report_slot_state(input, MT_TOOL_FINGER, touch);
@@ -1870,7 +1880,7 @@ static void wacom_bpt3_touch_msg(struct wacom_wac *wacom, unsigned char *data)
 	if (slot < 0)
 		return;
 
-	touch = touch && !wacom->shared->stylus_in_proximity;
+	touch = touch && report_touch_events(wacom);
 
 	input_mt_slot(input, slot);
 	input_mt_report_slot_state(input, MT_TOOL_FINGER, touch);
@@ -1942,7 +1952,7 @@ static int wacom_bpt3_touch(struct wacom_wac *wacom)
 	}
 
 	/* only update touch if we actually have a touchpad and touch data changed */
-	if (wacom->touch_registered && touch_changed) {
+	if (wacom->touch_input && touch_changed) {
 		input_mt_sync_frame(wacom->touch_input);
 		wacom->shared->touch_down = wacom_wac_finger_count_touches(wacom);
 	}
@@ -1983,7 +1993,7 @@ static int wacom_bpt_pen(struct wacom_wac *wacom)
 	}
 
 	wacom->shared->stylus_in_proximity = prox;
-	if (wacom->shared->touch_down)
+	if (delay_pen_events(wacom))
 		return 0;
 
 	if (prox) {
@@ -2077,7 +2087,7 @@ static int wacom_bamboo_pad_touch_event(struct wacom_wac *wacom,
 
 	for (id = 0; id < wacom->features.touch_max; id++) {
 		valid = !!(prefix & BIT(id)) &&
-			!wacom->shared->stylus_in_proximity;
+			report_touch_events(wacom);
 
 		input_mt_slot(input, id);
 		input_mt_report_slot_state(input, MT_TOOL_FINGER, valid);
@@ -2099,8 +2109,7 @@ static int wacom_bamboo_pad_touch_event(struct wacom_wac *wacom,
 	input_report_key(input, BTN_RIGHT, prefix & 0x80);
 
 	/* keep touch state for pen event */
-	wacom->shared->touch_down = !!prefix &&
-				    !wacom->shared->stylus_in_proximity;
+	wacom->shared->touch_down = !!prefix && report_touch_events(wacom);
 
 	return 1;
 }
@@ -2149,16 +2158,15 @@ static int wacom_wireless_irq(struct wacom_wac *wacom, size_t len)
 		charging = !!(data[5] & 0x80);
 		if (wacom->pid != pid) {
 			wacom->pid = pid;
-			wacom_schedule_work(wacom);
+			wacom_schedule_work(wacom, WACOM_WORKER_WIRELESS);
 		}
 
-		if (wacom->shared->type)
-			wacom_notify_battery(wacom, battery, charging, 1, 0);
+		wacom_notify_battery(wacom, battery, charging, 1, 0);
 
 	} else if (wacom->pid != 0) {
 		/* disconnected while previously connected */
 		wacom->pid = 0;
-		wacom_schedule_work(wacom);
+		wacom_schedule_work(wacom, WACOM_WORKER_WIRELESS);
 		wacom_notify_battery(wacom, 0, 0, 0, 0);
 	}
 
@@ -2190,18 +2198,16 @@ static int wacom_status_irq(struct wacom_wac *wacom_wac, size_t len)
 		wacom_notify_battery(wacom_wac, battery, charging,
 				     battery || charging, 1);
 
-		if (!wacom->battery &&
+		if (!wacom->battery.battery &&
 		    !(features->quirks & WACOM_QUIRK_BATTERY)) {
 			features->quirks |= WACOM_QUIRK_BATTERY;
-			INIT_WORK(&wacom->work, wacom_battery_work);
-			wacom_schedule_work(wacom_wac);
+			wacom_schedule_work(wacom_wac, WACOM_WORKER_BATTERY);
 		}
 	}
 	else if ((features->quirks & WACOM_QUIRK_BATTERY) &&
-		 wacom->battery) {
+		 wacom->battery.battery) {
 		features->quirks &= ~WACOM_QUIRK_BATTERY;
-		INIT_WORK(&wacom->work, wacom_battery_work);
-		wacom_schedule_work(wacom_wac);
+		wacom_schedule_work(wacom_wac, WACOM_WORKER_BATTERY);
 		wacom_notify_battery(wacom_wac, 0, 0, 0, 0);
 	}
 	return 0;
@@ -2312,8 +2318,9 @@ void wacom_wac_irq(struct wacom_wac *wacom_wac, size_t len)
 		break;
 
 	case REMOTE:
+		sync = false;
 		if (wacom_wac->data[0] == WACOM_REPORT_DEVICE_LIST)
-			sync = wacom_remote_status_irq(wacom_wac, len);
+			wacom_remote_status_irq(wacom_wac, len);
 		else
 			sync = wacom_remote_irq(wacom_wac, len);
 		break;
@@ -2451,6 +2458,33 @@ void wacom_setup_device_quirks(struct wacom *wacom)
 	if (features->type == REMOTE)
 		features->device_type = WACOM_DEVICETYPE_PAD;
 
+	switch (features->type) {
+	case PL:
+	case DTU:
+	case DTUS:
+	case DTUSX:
+	case WACOM_21UX2:
+	case WACOM_22HD:
+	case DTK:
+	case WACOM_24HD:
+	case WACOM_27QHD:
+	case CINTIQ_HYBRID:
+	case CINTIQ_COMPANION_2:
+	case CINTIQ:
+	case WACOM_BEE:
+	case WACOM_13HD:
+	case WACOM_24HDT:
+	case WACOM_27QHDT:
+	case TABLETPC:
+	case TABLETPCE:
+	case TABLETPC2FG:
+	case MTSCREEN:
+	case MTTPC:
+	case MTTPC_B:
+		features->device_type |= WACOM_DEVICETYPE_DIRECT;
+		break;
+	}
+
 	if (wacom->hdev->bus == BUS_BLUETOOTH)
 		features->quirks |= WACOM_QUIRK_BATTERY;
 
@@ -2469,6 +2503,9 @@ void wacom_setup_device_quirks(struct wacom *wacom)
 			features->quirks |= WACOM_QUIRK_BATTERY;
 		}
 	}
+
+	if (features->type == REMOTE)
+		features->device_type |= WACOM_DEVICETYPE_WL_MONITOR;
 }
 
 int wacom_setup_pen_input_capabilities(struct input_dev *input_dev,
@@ -2481,6 +2518,11 @@ int wacom_setup_pen_input_capabilities(struct input_dev *input_dev,
 	if (!(features->device_type & WACOM_DEVICETYPE_PEN))
 		return -ENODEV;
 
+	if (features->device_type & WACOM_DEVICETYPE_DIRECT)
+		__set_bit(INPUT_PROP_DIRECT, input_dev->propbit);
+	else
+		__set_bit(INPUT_PROP_POINTER, input_dev->propbit);
+
 	if (features->type == HID_GENERIC)
 		/* setup has already been done */
 		return 0;
@@ -2499,7 +2541,6 @@ int wacom_setup_pen_input_capabilities(struct input_dev *input_dev,
 	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:
 		__clear_bit(ABS_MISC, input_dev->absbit);
@@ -2523,8 +2564,6 @@ int wacom_setup_pen_input_capabilities(struct input_dev *input_dev,
 		__set_bit(BTN_TOOL_MOUSE, input_dev->keybit);
 		__set_bit(BTN_STYLUS, input_dev->keybit);
 		__set_bit(BTN_STYLUS2, input_dev->keybit);
-
-		__set_bit(INPUT_PROP_POINTER, input_dev->propbit);
 		break;
 
 	case WACOM_27QHD:
@@ -2539,7 +2578,6 @@ int wacom_setup_pen_input_capabilities(struct input_dev *input_dev,
 	case CINTIQ_COMPANION_2:
 		input_set_abs_params(input_dev, ABS_Z, -900, 899, 0, 0);
 		input_abs_set_res(input_dev, ABS_Z, 287);
-		__set_bit(INPUT_PROP_DIRECT, input_dev->propbit);
 		wacom_setup_cintiq(wacom_wac);
 		break;
 
@@ -2555,8 +2593,6 @@ int wacom_setup_pen_input_capabilities(struct input_dev *input_dev,
 		/* fall through */
 
 	case INTUOS:
-		__set_bit(INPUT_PROP_POINTER, input_dev->propbit);
-
 		wacom_setup_intuos(wacom_wac);
 		break;
 
@@ -2566,8 +2602,6 @@ int wacom_setup_pen_input_capabilities(struct input_dev *input_dev,
 	case INTUOSPL:
 	case INTUOS5S:
 	case INTUOSPS:
-		__set_bit(INPUT_PROP_POINTER, input_dev->propbit);
-
 		input_set_abs_params(input_dev, ABS_DISTANCE, 0,
 				      features->distance_max,
 				      features->distance_fuzz, 0);
@@ -2597,8 +2631,6 @@ int wacom_setup_pen_input_capabilities(struct input_dev *input_dev,
 		__set_bit(BTN_TOOL_RUBBER, input_dev->keybit);
 		__set_bit(BTN_STYLUS, input_dev->keybit);
 		__set_bit(BTN_STYLUS2, input_dev->keybit);
-
-		__set_bit(INPUT_PROP_DIRECT, input_dev->propbit);
 		break;
 
 	case PTU:
@@ -2609,16 +2641,12 @@ int wacom_setup_pen_input_capabilities(struct input_dev *input_dev,
 		__set_bit(BTN_TOOL_PEN, input_dev->keybit);
 		__set_bit(BTN_TOOL_RUBBER, input_dev->keybit);
 		__set_bit(BTN_STYLUS, input_dev->keybit);
-
-		__set_bit(INPUT_PROP_POINTER, input_dev->propbit);
 		break;
 
 	case INTUOSHT:
 	case BAMBOO_PT:
 	case BAMBOO_PEN:
 	case INTUOSHT2:
-		__set_bit(INPUT_PROP_POINTER, input_dev->propbit);
-
 		if (features->type == INTUOSHT2) {
 			wacom_setup_basic_pro_pen(wacom_wac);
 		} else {
@@ -2649,6 +2677,11 @@ int wacom_setup_touch_input_capabilities(struct input_dev *input_dev,
 	if (!(features->device_type & WACOM_DEVICETYPE_TOUCH))
 		return -ENODEV;
 
+	if (features->device_type & WACOM_DEVICETYPE_DIRECT)
+		__set_bit(INPUT_PROP_DIRECT, input_dev->propbit);
+	else
+		__set_bit(INPUT_PROP_POINTER, input_dev->propbit);
+
 	if (features->type == HID_GENERIC)
 		/* setup has already been done */
 		return 0;
@@ -2683,8 +2716,6 @@ int wacom_setup_touch_input_capabilities(struct input_dev *input_dev,
 	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);
@@ -2707,7 +2738,6 @@ int wacom_setup_touch_input_capabilities(struct input_dev *input_dev,
 
 	case TABLETPC:
 	case TABLETPCE:
-		__set_bit(INPUT_PROP_DIRECT, input_dev->propbit);
 		break;
 
 	case INTUOSHT:
@@ -2752,11 +2782,105 @@ static void wacom_setup_numbered_buttons(struct input_dev *input_dev,
 		__set_bit(BTN_BASE + (i-16), input_dev->keybit);
 }
 
+static void wacom_24hd_update_leds(struct wacom *wacom, int mask, int group)
+{
+	struct wacom_led *led;
+	int i;
+	bool updated = false;
+
+	/*
+	 * 24HD has LED group 1 to the left and LED group 0 to the right.
+	 * So group 0 matches the second half of the buttons and thus the mask
+	 * needs to be shifted.
+	 */
+	if (group == 0)
+		mask >>= 8;
+
+	for (i = 0; i < 3; i++) {
+		led = wacom_led_find(wacom, group, i);
+		if (!led) {
+			hid_err(wacom->hdev, "can't find LED %d in group %d\n",
+				i, group);
+			continue;
+		}
+		if (!updated && mask & BIT(i)) {
+			led->held = true;
+			led_trigger_event(&led->trigger, LED_FULL);
+		} else {
+			led->held = false;
+		}
+	}
+}
+
+static bool wacom_is_led_toggled(struct wacom *wacom, int button_count,
+				 int mask, int group)
+{
+	int button_per_group;
+
+	/*
+	 * 21UX2 has LED group 1 to the left and LED group 0
+	 * to the right. We need to reverse the group to match this
+	 * historical behavior.
+	 */
+	if (wacom->wacom_wac.features.type == WACOM_21UX2)
+		group = 1 - group;
+
+	button_per_group = button_count/wacom->led.count;
+
+	return mask & (1 << (group * button_per_group));
+}
+
+static void wacom_update_led(struct wacom *wacom, int button_count, int mask,
+			     int group)
+{
+	struct wacom_led *led, *next_led;
+	int cur;
+	bool pressed;
+
+	if (wacom->wacom_wac.features.type == WACOM_24HD)
+		return wacom_24hd_update_leds(wacom, mask, group);
+
+	pressed = wacom_is_led_toggled(wacom, button_count, mask, group);
+	cur = wacom->led.groups[group].select;
+
+	led = wacom_led_find(wacom, group, cur);
+	if (!led) {
+		hid_err(wacom->hdev, "can't find current LED %d in group %d\n",
+			cur, group);
+		return;
+	}
+
+	if (!pressed) {
+		led->held = false;
+		return;
+	}
+
+	if (led->held && pressed)
+		return;
+
+	next_led = wacom_led_next(wacom, led);
+	if (!next_led) {
+		hid_err(wacom->hdev, "can't find next LED in group %d\n",
+			group);
+		return;
+	}
+	if (next_led == led)
+		return;
+
+	next_led->held = true;
+	led_trigger_event(&next_led->trigger,
+			  wacom_leds_brightness_get(next_led));
+}
+
 static void wacom_report_numbered_buttons(struct input_dev *input_dev,
 				int button_count, int mask)
 {
+	struct wacom *wacom = input_get_drvdata(input_dev);
 	int i;
 
+	for (i = 0; i < wacom->led.count; i++)
+		wacom_update_led(wacom,  button_count, mask, i);
+
 	for (i = 0; i < button_count && i < 10; i++)
 		input_report_key(input_dev, BTN_0 + i, mask & (1 << i));
 	for (i = 10; i < button_count && i < 16; i++)
@@ -2773,6 +2897,9 @@ int wacom_setup_pad_input_capabilities(struct input_dev *input_dev,
 	if (!(features->device_type & WACOM_DEVICETYPE_PAD))
 		return -ENODEV;
 
+	if (features->type == REMOTE && input_dev == wacom_wac->pad_input)
+		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 */
@@ -3403,7 +3530,7 @@ static const struct wacom_features wacom_features_0x343 =
 	  WACOM_DTU_OFFSET, WACOM_DTU_OFFSET };
 
 static const struct wacom_features wacom_features_HID_ANY_ID =
-	{ "Wacom HID", .type = HID_GENERIC };
+	{ "Wacom HID", .type = HID_GENERIC, .oVid = HID_ANY_ID, .oPid = HID_ANY_ID };
 
 #define USB_DEVICE_WACOM(prod)						\
 	HID_DEVICE(BUS_USB, HID_GROUP_WACOM, USB_VENDOR_ID_WACOM, prod),\
diff --git a/drivers/hid/wacom_wac.h b/drivers/hid/wacom_wac.h
index 53d16537fd2a..324c40b0c119 100644
--- a/drivers/hid/wacom_wac.h
+++ b/drivers/hid/wacom_wac.h
@@ -82,6 +82,7 @@
 #define WACOM_DEVICETYPE_TOUCH          0x0002
 #define WACOM_DEVICETYPE_PAD            0x0004
 #define WACOM_DEVICETYPE_WL_MONITOR     0x0008
+#define WACOM_DEVICETYPE_DIRECT         0x0010
 
 #define WACOM_VENDORDEFINED_PEN		0xff0d0001
 #define WACOM_G9_PAGE			0xff090000
@@ -185,7 +186,6 @@ struct wacom_features {
 	int pktlen;
 	bool check_for_hid_type;
 	int hid_type;
-	int last_slot_field;
 };
 
 struct wacom_shared {
@@ -214,35 +214,35 @@ struct hid_data {
 	int cc_report;
 	int cc_index;
 	int cc_value_index;
+	int last_slot_field;
 	int num_expected;
 	int num_received;
 };
 
+struct wacom_remote_data {
+	struct {
+		u32 serial;
+		bool connected;
+	} remote[WACOM_MAX_REMOTES];
+};
+
 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];
 	unsigned char data[WACOM_PKGLEN_MAX];
 	int tool[2];
 	int id[2];
-	__u32 serial[5];
+	__u32 serial[2];
 	bool reporting_data;
 	struct wacom_features features;
 	struct wacom_shared *shared;
 	struct input_dev *pen_input;
 	struct input_dev *touch_input;
 	struct input_dev *pad_input;
-	bool pen_registered;
-	bool touch_registered;
-	bool pad_registered;
 	int pid;
-	int battery_capacity;
 	int num_contacts_left;
-	int bat_charging;
-	int bat_connected;
-	int ps_connected;
 	u8 bt_features;
 	u8 bt_high_speed;
 	int mode_report;
diff --git a/drivers/hv/channel.c b/drivers/hv/channel.c
index 56dd261f7142..16f91c8490fe 100644
--- a/drivers/hv/channel.c
+++ b/drivers/hv/channel.c
@@ -43,7 +43,12 @@ static void vmbus_setevent(struct vmbus_channel *channel)
 {
 	struct hv_monitor_page *monitorpage;
 
-	if (channel->offermsg.monitor_allocated) {
+	/*
+	 * For channels marked as in "low latency" mode
+	 * bypass the monitor page mechanism.
+	 */
+	if ((channel->offermsg.monitor_allocated) &&
+	    (!channel->low_latency)) {
 		/* Each u32 represents 32 channels */
 		sync_set_bit(channel->offermsg.child_relid & 31,
 			(unsigned long *) vmbus_connection.send_int_page +
@@ -70,12 +75,14 @@ int vmbus_open(struct vmbus_channel *newchannel, u32 send_ringbuffer_size,
 {
 	struct vmbus_channel_open_channel *open_msg;
 	struct vmbus_channel_msginfo *open_info = NULL;
-	void *in, *out;
 	unsigned long flags;
 	int ret, err = 0;
-	unsigned long t;
 	struct page *page;
 
+	if (send_ringbuffer_size % PAGE_SIZE ||
+	    recv_ringbuffer_size % PAGE_SIZE)
+		return -EINVAL;
+
 	spin_lock_irqsave(&newchannel->lock, flags);
 	if (newchannel->state == CHANNEL_OPEN_STATE) {
 		newchannel->state = CHANNEL_OPENING_STATE;
@@ -95,36 +102,33 @@ int vmbus_open(struct vmbus_channel *newchannel, u32 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);
+		page = alloc_pages(GFP_KERNEL|__GFP_ZERO,
+				   get_order(send_ringbuffer_size +
+					     recv_ringbuffer_size));
 
-	if (!out) {
+	if (!page) {
 		err = -ENOMEM;
-		goto error0;
+		goto error_set_chnstate;
 	}
 
-	in = (void *)((unsigned long)out + send_ringbuffer_size);
-
-	newchannel->ringbuffer_pages = out;
+	newchannel->ringbuffer_pages = page_address(page);
 	newchannel->ringbuffer_pagecount = (send_ringbuffer_size +
 					   recv_ringbuffer_size) >> PAGE_SHIFT;
 
-	ret = hv_ringbuffer_init(
-		&newchannel->outbound, out, send_ringbuffer_size);
+	ret = hv_ringbuffer_init(&newchannel->outbound, page,
+				 send_ringbuffer_size >> PAGE_SHIFT);
 
 	if (ret != 0) {
 		err = ret;
-		goto error0;
+		goto error_free_pages;
 	}
 
-	ret = hv_ringbuffer_init(
-		&newchannel->inbound, in, recv_ringbuffer_size);
+	ret = hv_ringbuffer_init(&newchannel->inbound,
+				 &page[send_ringbuffer_size >> PAGE_SHIFT],
+				 recv_ringbuffer_size >> PAGE_SHIFT);
 	if (ret != 0) {
 		err = ret;
-		goto error0;
+		goto error_free_pages;
 	}
 
 
@@ -132,14 +136,14 @@ int vmbus_open(struct vmbus_channel *newchannel, u32 send_ringbuffer_size,
 	newchannel->ringbuffer_gpadlhandle = 0;
 
 	ret = vmbus_establish_gpadl(newchannel,
-					 newchannel->outbound.ring_buffer,
-					 send_ringbuffer_size +
-					 recv_ringbuffer_size,
-					 &newchannel->ringbuffer_gpadlhandle);
+				    page_address(page),
+				    send_ringbuffer_size +
+				    recv_ringbuffer_size,
+				    &newchannel->ringbuffer_gpadlhandle);
 
 	if (ret != 0) {
 		err = ret;
-		goto error0;
+		goto error_free_pages;
 	}
 
 	/* Create and init the channel open message */
@@ -148,7 +152,7 @@ int vmbus_open(struct vmbus_channel *newchannel, u32 send_ringbuffer_size,
 			   GFP_KERNEL);
 	if (!open_info) {
 		err = -ENOMEM;
-		goto error_gpadl;
+		goto error_free_gpadl;
 	}
 
 	init_completion(&open_info->waitevent);
@@ -164,7 +168,7 @@ int vmbus_open(struct vmbus_channel *newchannel, u32 send_ringbuffer_size,
 
 	if (userdatalen > MAX_USER_DEFINED_BYTES) {
 		err = -EINVAL;
-		goto error_gpadl;
+		goto error_free_gpadl;
 	}
 
 	if (userdatalen)
@@ -180,14 +184,10 @@ int vmbus_open(struct vmbus_channel *newchannel, u32 send_ringbuffer_size,
 
 	if (ret != 0) {
 		err = ret;
-		goto error1;
+		goto error_clean_msglist;
 	}
 
-	t = wait_for_completion_timeout(&open_info->waitevent, 5*HZ);
-	if (t == 0) {
-		err = -ETIMEDOUT;
-		goto error1;
-	}
+	wait_for_completion(&open_info->waitevent);
 
 	spin_lock_irqsave(&vmbus_connection.channelmsg_lock, flags);
 	list_del(&open_info->msglistentry);
@@ -195,25 +195,27 @@ int vmbus_open(struct vmbus_channel *newchannel, u32 send_ringbuffer_size,
 
 	if (open_info->response.open_result.status) {
 		err = -EAGAIN;
-		goto error_gpadl;
+		goto error_free_gpadl;
 	}
 
 	newchannel->state = CHANNEL_OPENED_STATE;
 	kfree(open_info);
 	return 0;
 
-error1:
+error_clean_msglist:
 	spin_lock_irqsave(&vmbus_connection.channelmsg_lock, flags);
 	list_del(&open_info->msglistentry);
 	spin_unlock_irqrestore(&vmbus_connection.channelmsg_lock, flags);
 
-error_gpadl:
+error_free_gpadl:
 	vmbus_teardown_gpadl(newchannel, newchannel->ringbuffer_gpadlhandle);
-
-error0:
-	free_pages((unsigned long)out,
-		get_order(send_ringbuffer_size + recv_ringbuffer_size));
 	kfree(open_info);
+error_free_pages:
+	hv_ringbuffer_cleanup(&newchannel->outbound);
+	hv_ringbuffer_cleanup(&newchannel->inbound);
+	__free_pages(page,
+		     get_order(send_ringbuffer_size + recv_ringbuffer_size));
+error_set_chnstate:
 	newchannel->state = CHANNEL_OPEN_STATE;
 	return err;
 }
@@ -238,8 +240,7 @@ EXPORT_SYMBOL_GPL(vmbus_send_tl_connect_request);
  * create_gpadl_header - Creates a gpadl for the specified buffer
  */
 static int create_gpadl_header(void *kbuffer, u32 size,
-					 struct vmbus_channel_msginfo **msginfo,
-					 u32 *messagecount)
+			       struct vmbus_channel_msginfo **msginfo)
 {
 	int i;
 	int pagecount;
@@ -283,7 +284,6 @@ static int create_gpadl_header(void *kbuffer, u32 size,
 			gpadl_header->range[0].pfn_array[i] = slow_virt_to_phys(
 				kbuffer + PAGE_SIZE * i) >> PAGE_SHIFT;
 		*msginfo = msgheader;
-		*messagecount = 1;
 
 		pfnsum = pfncount;
 		pfnleft = pagecount - pfncount;
@@ -323,7 +323,6 @@ static int create_gpadl_header(void *kbuffer, u32 size,
 			}
 
 			msgbody->msgsize = msgsize;
-			(*messagecount)++;
 			gpadl_body =
 				(struct vmbus_channel_gpadl_body *)msgbody->msg;
 
@@ -352,6 +351,8 @@ static int create_gpadl_header(void *kbuffer, u32 size,
 		msgheader = kzalloc(msgsize, GFP_KERNEL);
 		if (msgheader == NULL)
 			goto nomem;
+
+		INIT_LIST_HEAD(&msgheader->submsglist);
 		msgheader->msgsize = msgsize;
 
 		gpadl_header = (struct vmbus_channel_gpadl_header *)
@@ -366,7 +367,6 @@ static int create_gpadl_header(void *kbuffer, u32 size,
 				kbuffer + PAGE_SIZE * i) >> PAGE_SHIFT;
 
 		*msginfo = msgheader;
-		*messagecount = 1;
 	}
 
 	return 0;
@@ -390,8 +390,7 @@ int vmbus_establish_gpadl(struct vmbus_channel *channel, void *kbuffer,
 	struct vmbus_channel_gpadl_header *gpadlmsg;
 	struct vmbus_channel_gpadl_body *gpadl_body;
 	struct vmbus_channel_msginfo *msginfo = NULL;
-	struct vmbus_channel_msginfo *submsginfo;
-	u32 msgcount;
+	struct vmbus_channel_msginfo *submsginfo, *tmp;
 	struct list_head *curr;
 	u32 next_gpadl_handle;
 	unsigned long flags;
@@ -400,7 +399,7 @@ int vmbus_establish_gpadl(struct vmbus_channel *channel, void *kbuffer,
 	next_gpadl_handle =
 		(atomic_inc_return(&vmbus_connection.next_gpadl_handle) - 1);
 
-	ret = create_gpadl_header(kbuffer, size, &msginfo, &msgcount);
+	ret = create_gpadl_header(kbuffer, size, &msginfo);
 	if (ret)
 		return ret;
 
@@ -423,24 +422,21 @@ int vmbus_establish_gpadl(struct vmbus_channel *channel, void *kbuffer,
 	if (ret != 0)
 		goto cleanup;
 
-	if (msgcount > 1) {
-		list_for_each(curr, &msginfo->submsglist) {
-
-			submsginfo = (struct vmbus_channel_msginfo *)curr;
-			gpadl_body =
-			     (struct vmbus_channel_gpadl_body *)submsginfo->msg;
+	list_for_each(curr, &msginfo->submsglist) {
+		submsginfo = (struct vmbus_channel_msginfo *)curr;
+		gpadl_body =
+			(struct vmbus_channel_gpadl_body *)submsginfo->msg;
 
-			gpadl_body->header.msgtype =
-				CHANNELMSG_GPADL_BODY;
-			gpadl_body->gpadl = next_gpadl_handle;
+		gpadl_body->header.msgtype =
+			CHANNELMSG_GPADL_BODY;
+		gpadl_body->gpadl = next_gpadl_handle;
 
-			ret = vmbus_post_msg(gpadl_body,
-					       submsginfo->msgsize -
-					       sizeof(*submsginfo));
-			if (ret != 0)
-				goto cleanup;
+		ret = vmbus_post_msg(gpadl_body,
+				     submsginfo->msgsize -
+				     sizeof(*submsginfo));
+		if (ret != 0)
+			goto cleanup;
 
-		}
 	}
 	wait_for_completion(&msginfo->waitevent);
 
@@ -451,6 +447,10 @@ cleanup:
 	spin_lock_irqsave(&vmbus_connection.channelmsg_lock, flags);
 	list_del(&msginfo->msglistentry);
 	spin_unlock_irqrestore(&vmbus_connection.channelmsg_lock, flags);
+	list_for_each_entry_safe(submsginfo, tmp, &msginfo->submsglist,
+				 msglistentry) {
+		kfree(submsginfo);
+	}
 
 	kfree(msginfo);
 	return ret;
@@ -512,7 +512,6 @@ static void reset_channel_cb(void *arg)
 static int vmbus_close_internal(struct vmbus_channel *channel)
 {
 	struct vmbus_channel_close_channel *msg;
-	struct tasklet_struct *tasklet;
 	int ret;
 
 	/*
@@ -524,8 +523,7 @@ static int vmbus_close_internal(struct vmbus_channel *channel)
 	 * To resolve the race, we can serialize them by disabling the
 	 * tasklet when the latter is running here.
 	 */
-	tasklet = hv_context.event_dpc[channel->target_cpu];
-	tasklet_disable(tasklet);
+	hv_event_tasklet_disable(channel);
 
 	/*
 	 * In case a device driver's probe() fails (e.g.,
@@ -591,7 +589,7 @@ static int vmbus_close_internal(struct vmbus_channel *channel)
 		get_order(channel->ringbuffer_pagecount * PAGE_SIZE));
 
 out:
-	tasklet_enable(tasklet);
+	hv_event_tasklet_enable(channel);
 
 	return ret;
 }
@@ -659,7 +657,7 @@ int vmbus_sendpacket_ctl(struct vmbus_channel *channel, void *buffer,
 	bufferlist[2].iov_len = (packetlen_aligned - packetlen);
 
 	ret = hv_ringbuffer_write(&channel->outbound, bufferlist, num_vecs,
-				  &signal, lock);
+				  &signal, lock, channel->signal_policy);
 
 	/*
 	 * Signalling the host is conditional on many factors:
@@ -680,11 +678,6 @@ int vmbus_sendpacket_ctl(struct vmbus_channel *channel, void *buffer,
 	 * mechanism which can hurt the performance otherwise.
 	 */
 
-	if (channel->signal_policy)
-		signal = true;
-	else
-		kick_q = true;
-
 	if (((ret == 0) && kick_q && signal) ||
 	    (ret && !is_hvsock_channel(channel)))
 		vmbus_setevent(channel);
@@ -777,7 +770,7 @@ int vmbus_sendpacket_pagebuffer_ctl(struct vmbus_channel *channel,
 	bufferlist[2].iov_len = (packetlen_aligned - packetlen);
 
 	ret = hv_ringbuffer_write(&channel->outbound, bufferlist, 3,
-				  &signal, lock);
+				  &signal, lock, channel->signal_policy);
 
 	/*
 	 * Signalling the host is conditional on many factors:
@@ -795,11 +788,6 @@ int vmbus_sendpacket_pagebuffer_ctl(struct vmbus_channel *channel,
 	 * enough condition that it should not matter.
 	 */
 
-	if (channel->signal_policy)
-		signal = true;
-	else
-		kick_q = true;
-
 	if (((ret == 0) && kick_q && signal) || (ret))
 		vmbus_setevent(channel);
 
@@ -861,7 +849,7 @@ int vmbus_sendpacket_mpb_desc(struct vmbus_channel *channel,
 	bufferlist[2].iov_len = (packetlen_aligned - packetlen);
 
 	ret = hv_ringbuffer_write(&channel->outbound, bufferlist, 3,
-				  &signal, lock);
+				  &signal, lock, channel->signal_policy);
 
 	if (ret == 0 && signal)
 		vmbus_setevent(channel);
@@ -926,7 +914,7 @@ int vmbus_sendpacket_multipagebuffer(struct vmbus_channel *channel,
 	bufferlist[2].iov_len = (packetlen_aligned - packetlen);
 
 	ret = hv_ringbuffer_write(&channel->outbound, bufferlist, 3,
-				  &signal, lock);
+				  &signal, lock, channel->signal_policy);
 
 	if (ret == 0 && signal)
 		vmbus_setevent(channel);
diff --git a/drivers/hv/channel_mgmt.c b/drivers/hv/channel_mgmt.c
index b6c1211b4df7..96a85cd39580 100644
--- a/drivers/hv/channel_mgmt.c
+++ b/drivers/hv/channel_mgmt.c
@@ -21,6 +21,7 @@
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
 #include <linux/kernel.h>
+#include <linux/interrupt.h>
 #include <linux/sched.h>
 #include <linux/wait.h>
 #include <linux/mm.h>
@@ -138,10 +139,32 @@ static const struct vmbus_device vmbus_devs[] = {
 	},
 };
 
-static u16 hv_get_dev_type(const uuid_le *guid)
+static const struct {
+	uuid_le guid;
+} vmbus_unsupported_devs[] = {
+	{ HV_AVMA1_GUID },
+	{ HV_AVMA2_GUID },
+	{ HV_RDV_GUID	},
+};
+
+static bool is_unsupported_vmbus_devs(const uuid_le *guid)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(vmbus_unsupported_devs); i++)
+		if (!uuid_le_cmp(*guid, vmbus_unsupported_devs[i].guid))
+			return true;
+	return false;
+}
+
+static u16 hv_get_dev_type(const struct vmbus_channel *channel)
 {
+	const uuid_le *guid = &channel->offermsg.offer.if_type;
 	u16 i;
 
+	if (is_hvsock_channel(channel) || is_unsupported_vmbus_devs(guid))
+		return HV_UNKOWN;
+
 	for (i = HV_IDE; i < HV_UNKOWN; i++) {
 		if (!uuid_le_cmp(*guid, vmbus_devs[i].guid))
 			return i;
@@ -251,14 +274,12 @@ EXPORT_SYMBOL_GPL(vmbus_prep_negotiate_resp);
  */
 static struct vmbus_channel *alloc_channel(void)
 {
-	static atomic_t chan_num = ATOMIC_INIT(0);
 	struct vmbus_channel *channel;
 
 	channel = kzalloc(sizeof(*channel), GFP_ATOMIC);
 	if (!channel)
 		return NULL;
 
-	channel->id = atomic_inc_return(&chan_num);
 	channel->acquire_ring_lock = true;
 	spin_lock_init(&channel->inbound_lock);
 	spin_lock_init(&channel->lock);
@@ -303,16 +324,32 @@ static void vmbus_release_relid(u32 relid)
 	vmbus_post_msg(&msg, sizeof(struct vmbus_channel_relid_released));
 }
 
+void hv_event_tasklet_disable(struct vmbus_channel *channel)
+{
+	struct tasklet_struct *tasklet;
+	tasklet = hv_context.event_dpc[channel->target_cpu];
+	tasklet_disable(tasklet);
+}
+
+void hv_event_tasklet_enable(struct vmbus_channel *channel)
+{
+	struct tasklet_struct *tasklet;
+	tasklet = hv_context.event_dpc[channel->target_cpu];
+	tasklet_enable(tasklet);
+
+	/* In case there is any pending event */
+	tasklet_schedule(tasklet);
+}
+
 void hv_process_channel_removal(struct vmbus_channel *channel, u32 relid)
 {
 	unsigned long flags;
 	struct vmbus_channel *primary_channel;
 
-	vmbus_release_relid(relid);
-
 	BUG_ON(!channel->rescind);
 	BUG_ON(!mutex_is_locked(&vmbus_connection.channel_mutex));
 
+	hv_event_tasklet_disable(channel);
 	if (channel->target_cpu != get_cpu()) {
 		put_cpu();
 		smp_call_function_single(channel->target_cpu,
@@ -321,6 +358,7 @@ void hv_process_channel_removal(struct vmbus_channel *channel, u32 relid)
 		percpu_channel_deq(channel);
 		put_cpu();
 	}
+	hv_event_tasklet_enable(channel);
 
 	if (channel->primary_channel == NULL) {
 		list_del(&channel->listentry);
@@ -338,8 +376,11 @@ void hv_process_channel_removal(struct vmbus_channel *channel, u32 relid)
 	 * We need to free the bit for init_vp_index() to work in the case
 	 * of sub-channel, when we reload drivers like hv_netvsc.
 	 */
-	cpumask_clear_cpu(channel->target_cpu,
-			  &primary_channel->alloced_cpus_in_node);
+	if (channel->affinity_policy == HV_LOCALIZED)
+		cpumask_clear_cpu(channel->target_cpu,
+				  &primary_channel->alloced_cpus_in_node);
+
+	vmbus_release_relid(relid);
 
 	free_channel(channel);
 }
@@ -405,10 +446,13 @@ static void vmbus_process_offer(struct vmbus_channel *newchannel)
 			goto err_free_chan;
 	}
 
-	dev_type = hv_get_dev_type(&newchannel->offermsg.offer.if_type);
+	dev_type = hv_get_dev_type(newchannel);
+	if (dev_type == HV_NIC)
+		set_channel_signal_state(newchannel, HV_SIGNAL_POLICY_EXPLICIT);
 
 	init_vp_index(newchannel, dev_type);
 
+	hv_event_tasklet_disable(newchannel);
 	if (newchannel->target_cpu != get_cpu()) {
 		put_cpu();
 		smp_call_function_single(newchannel->target_cpu,
@@ -418,6 +462,7 @@ static void vmbus_process_offer(struct vmbus_channel *newchannel)
 		percpu_channel_enq(newchannel);
 		put_cpu();
 	}
+	hv_event_tasklet_enable(newchannel);
 
 	/*
 	 * This state is used to indicate a successful open
@@ -463,12 +508,11 @@ static void vmbus_process_offer(struct vmbus_channel *newchannel)
 	return;
 
 err_deq_chan:
-	vmbus_release_relid(newchannel->offermsg.child_relid);
-
 	mutex_lock(&vmbus_connection.channel_mutex);
 	list_del(&newchannel->listentry);
 	mutex_unlock(&vmbus_connection.channel_mutex);
 
+	hv_event_tasklet_disable(newchannel);
 	if (newchannel->target_cpu != get_cpu()) {
 		put_cpu();
 		smp_call_function_single(newchannel->target_cpu,
@@ -477,6 +521,9 @@ err_deq_chan:
 		percpu_channel_deq(newchannel);
 		put_cpu();
 	}
+	hv_event_tasklet_enable(newchannel);
+
+	vmbus_release_relid(newchannel->offermsg.child_relid);
 
 err_free_chan:
 	free_channel(newchannel);
@@ -522,17 +569,17 @@ static void init_vp_index(struct vmbus_channel *channel, u16 dev_type)
 	}
 
 	/*
-	 * 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.
+	 * Based on the channel affinity policy, we will assign the NUMA
+	 * nodes.
 	 */
-	if (!primary) {
+
+	if ((channel->affinity_policy == HV_BALANCED) || (!primary)) {
 		while (true) {
 			next_node = next_numa_node_id++;
-			if (next_node == nr_node_ids)
+			if (next_node == nr_node_ids) {
 				next_node = next_numa_node_id = 0;
+				continue;
+			}
 			if (cpumask_empty(cpumask_of_node(next_node)))
 				continue;
 			break;
@@ -556,15 +603,17 @@ static void init_vp_index(struct vmbus_channel *channel, u16 dev_type)
 
 	cur_cpu = -1;
 
-	/*
-	 * Normally Hyper-V host doesn't create more subchannels than there
-	 * are VCPUs on the node but it is possible when not all present VCPUs
-	 * on the node are initialized by guest. Clear the alloced_cpus_in_node
-	 * to start over.
-	 */
-	if (cpumask_equal(&primary->alloced_cpus_in_node,
-			  cpumask_of_node(primary->numa_node)))
-		cpumask_clear(&primary->alloced_cpus_in_node);
+	if (primary->affinity_policy == HV_LOCALIZED) {
+		/*
+		 * Normally Hyper-V host doesn't create more subchannels
+		 * than there are VCPUs on the node but it is possible when not
+		 * all present VCPUs on the node are initialized by guest.
+		 * Clear the alloced_cpus_in_node to start over.
+		 */
+		if (cpumask_equal(&primary->alloced_cpus_in_node,
+				  cpumask_of_node(primary->numa_node)))
+			cpumask_clear(&primary->alloced_cpus_in_node);
+	}
 
 	while (true) {
 		cur_cpu = cpumask_next(cur_cpu, &available_mask);
@@ -575,17 +624,24 @@ static void init_vp_index(struct vmbus_channel *channel, u16 dev_type)
 			continue;
 		}
 
-		/*
-		 * NOTE: in the case of sub-channel, we clear the sub-channel
-		 * related bit(s) in primary->alloced_cpus_in_node in
-		 * hv_process_channel_removal(), so when we reload drivers
-		 * like hv_netvsc in SMP guest, here we're able to re-allocate
-		 * bit from primary->alloced_cpus_in_node.
-		 */
-		if (!cpumask_test_cpu(cur_cpu,
-				&primary->alloced_cpus_in_node)) {
-			cpumask_set_cpu(cur_cpu,
-					&primary->alloced_cpus_in_node);
+		if (primary->affinity_policy == HV_LOCALIZED) {
+			/*
+			 * NOTE: in the case of sub-channel, we clear the
+			 * sub-channel related bit(s) in
+			 * primary->alloced_cpus_in_node in
+			 * hv_process_channel_removal(), so when we
+			 * reload drivers like hv_netvsc in SMP guest, here
+			 * we're able to re-allocate
+			 * bit from primary->alloced_cpus_in_node.
+			 */
+			if (!cpumask_test_cpu(cur_cpu,
+					      &primary->alloced_cpus_in_node)) {
+				cpumask_set_cpu(cur_cpu,
+						&primary->alloced_cpus_in_node);
+				cpumask_set_cpu(cur_cpu, alloced_mask);
+				break;
+			}
+		} else {
 			cpumask_set_cpu(cur_cpu, alloced_mask);
 			break;
 		}
diff --git a/drivers/hv/connection.c b/drivers/hv/connection.c
index fcf8a02dc0ea..78e6368a4423 100644
--- a/drivers/hv/connection.c
+++ b/drivers/hv/connection.c
@@ -439,7 +439,7 @@ int vmbus_post_msg(void *buffer, size_t buflen)
 	union hv_connection_id conn_id;
 	int ret = 0;
 	int retries = 0;
-	u32 msec = 1;
+	u32 usec = 1;
 
 	conn_id.asu32 = 0;
 	conn_id.u.id = VMBUS_MESSAGE_CONNECTION_ID;
@@ -472,9 +472,9 @@ int vmbus_post_msg(void *buffer, size_t buflen)
 		}
 
 		retries++;
-		msleep(msec);
-		if (msec < 2048)
-			msec *= 2;
+		udelay(usec);
+		if (usec < 2048)
+			usec *= 2;
 	}
 	return ret;
 }
diff --git a/drivers/hv/hv.c b/drivers/hv/hv.c
index a1c086ba3b9a..60dbd6cb4640 100644
--- a/drivers/hv/hv.c
+++ b/drivers/hv/hv.c
@@ -278,7 +278,7 @@ cleanup:
  *
  * This routine is called normally during driver unloading or exiting.
  */
-void hv_cleanup(void)
+void hv_cleanup(bool crash)
 {
 	union hv_x64_msr_hypercall_contents hypercall_msr;
 
@@ -288,7 +288,8 @@ void hv_cleanup(void)
 	if (hv_context.hypercall_page) {
 		hypercall_msr.as_uint64 = 0;
 		wrmsrl(HV_X64_MSR_HYPERCALL, hypercall_msr.as_uint64);
-		vfree(hv_context.hypercall_page);
+		if (!crash)
+			vfree(hv_context.hypercall_page);
 		hv_context.hypercall_page = NULL;
 	}
 
@@ -308,7 +309,8 @@ void hv_cleanup(void)
 
 		hypercall_msr.as_uint64 = 0;
 		wrmsrl(HV_X64_MSR_REFERENCE_TSC, hypercall_msr.as_uint64);
-		vfree(hv_context.tsc_page);
+		if (!crash)
+			vfree(hv_context.tsc_page);
 		hv_context.tsc_page = NULL;
 	}
 #endif
diff --git a/drivers/hv/hv_balloon.c b/drivers/hv/hv_balloon.c
index df35fb7ed5df..fdf8da929cbe 100644
--- a/drivers/hv/hv_balloon.c
+++ b/drivers/hv/hv_balloon.c
@@ -430,16 +430,27 @@ struct dm_info_msg {
  * currently hot added. We hot add in multiples of 128M
  * chunks; it is possible that we may not be able to bring
  * online all the pages in the region. The range
- * covered_end_pfn defines the pages that can
+ * covered_start_pfn:covered_end_pfn defines the pages that can
  * be brough online.
  */
 
 struct hv_hotadd_state {
 	struct list_head list;
 	unsigned long start_pfn;
+	unsigned long covered_start_pfn;
 	unsigned long covered_end_pfn;
 	unsigned long ha_end_pfn;
 	unsigned long end_pfn;
+	/*
+	 * A list of gaps.
+	 */
+	struct list_head gap_list;
+};
+
+struct hv_hotadd_gap {
+	struct list_head list;
+	unsigned long start_pfn;
+	unsigned long end_pfn;
 };
 
 struct balloon_state {
@@ -536,7 +547,11 @@ struct hv_dynmem_device {
 	 */
 	struct task_struct *thread;
 
-	struct mutex ha_region_mutex;
+	/*
+	 * Protects ha_region_list, num_pages_onlined counter and individual
+	 * regions from ha_region_list.
+	 */
+	spinlock_t ha_lock;
 
 	/*
 	 * A list of hot-add regions.
@@ -560,18 +575,14 @@ static int hv_memory_notifier(struct notifier_block *nb, unsigned long val,
 			      void *v)
 {
 	struct memory_notify *mem = (struct memory_notify *)v;
+	unsigned long flags;
 
 	switch (val) {
-	case MEM_GOING_ONLINE:
-		mutex_lock(&dm_device.ha_region_mutex);
-		break;
-
 	case MEM_ONLINE:
+		spin_lock_irqsave(&dm_device.ha_lock, flags);
 		dm_device.num_pages_onlined += mem->nr_pages;
+		spin_unlock_irqrestore(&dm_device.ha_lock, flags);
 	case MEM_CANCEL_ONLINE:
-		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);
@@ -579,10 +590,11 @@ static int hv_memory_notifier(struct notifier_block *nb, unsigned long val,
 		break;
 
 	case MEM_OFFLINE:
-		mutex_lock(&dm_device.ha_region_mutex);
+		spin_lock_irqsave(&dm_device.ha_lock, flags);
 		dm_device.num_pages_onlined -= mem->nr_pages;
-		mutex_unlock(&dm_device.ha_region_mutex);
+		spin_unlock_irqrestore(&dm_device.ha_lock, flags);
 		break;
+	case MEM_GOING_ONLINE:
 	case MEM_GOING_OFFLINE:
 	case MEM_CANCEL_OFFLINE:
 		break;
@@ -595,18 +607,46 @@ static struct notifier_block hv_memory_nb = {
 	.priority = 0
 };
 
+/* Check if the particular page is backed and can be onlined and online it. */
+static void hv_page_online_one(struct hv_hotadd_state *has, struct page *pg)
+{
+	unsigned long cur_start_pgp;
+	unsigned long cur_end_pgp;
+	struct hv_hotadd_gap *gap;
+
+	cur_start_pgp = (unsigned long)pfn_to_page(has->covered_start_pfn);
+	cur_end_pgp = (unsigned long)pfn_to_page(has->covered_end_pfn);
+
+	/* The page is not backed. */
+	if (((unsigned long)pg < cur_start_pgp) ||
+	    ((unsigned long)pg >= cur_end_pgp))
+		return;
+
+	/* Check for gaps. */
+	list_for_each_entry(gap, &has->gap_list, list) {
+		cur_start_pgp = (unsigned long)
+			pfn_to_page(gap->start_pfn);
+		cur_end_pgp = (unsigned long)
+			pfn_to_page(gap->end_pfn);
+		if (((unsigned long)pg >= cur_start_pgp) &&
+		    ((unsigned long)pg < cur_end_pgp)) {
+			return;
+		}
+	}
+
+	/* This frame is currently backed; online the page. */
+	__online_page_set_limits(pg);
+	__online_page_increment_counters(pg);
+	__online_page_free(pg);
+}
 
-static void hv_bring_pgs_online(unsigned long start_pfn, unsigned long size)
+static void hv_bring_pgs_online(struct hv_hotadd_state *has,
+				unsigned long start_pfn, unsigned long size)
 {
 	int i;
 
-	for (i = 0; i < size; i++) {
-		struct page *pg;
-		pg = pfn_to_page(start_pfn + i);
-		__online_page_set_limits(pg);
-		__online_page_increment_counters(pg);
-		__online_page_free(pg);
-	}
+	for (i = 0; i < size; i++)
+		hv_page_online_one(has, pfn_to_page(start_pfn + i));
 }
 
 static void hv_mem_hot_add(unsigned long start, unsigned long size,
@@ -618,9 +658,12 @@ static void hv_mem_hot_add(unsigned long start, unsigned long size,
 	unsigned long start_pfn;
 	unsigned long processed_pfn;
 	unsigned long total_pfn = pfn_count;
+	unsigned long flags;
 
 	for (i = 0; i < (size/HA_CHUNK); i++) {
 		start_pfn = start + (i * HA_CHUNK);
+
+		spin_lock_irqsave(&dm_device.ha_lock, flags);
 		has->ha_end_pfn +=  HA_CHUNK;
 
 		if (total_pfn > HA_CHUNK) {
@@ -632,11 +675,11 @@ static void hv_mem_hot_add(unsigned long start, unsigned long size,
 		}
 
 		has->covered_end_pfn +=  processed_pfn;
+		spin_unlock_irqrestore(&dm_device.ha_lock, flags);
 
 		init_completion(&dm_device.ol_waitevent);
-		dm_device.ha_waiting = true;
+		dm_device.ha_waiting = !memhp_auto_online;
 
-		mutex_unlock(&dm_device.ha_region_mutex);
 		nid = memory_add_physaddr_to_nid(PFN_PHYS(start_pfn));
 		ret = add_memory(nid, PFN_PHYS((start_pfn)),
 				(HA_CHUNK << PAGE_SHIFT));
@@ -653,20 +696,23 @@ static void hv_mem_hot_add(unsigned long start, unsigned long size,
 				 */
 				do_hot_add = false;
 			}
+			spin_lock_irqsave(&dm_device.ha_lock, flags);
 			has->ha_end_pfn -= HA_CHUNK;
 			has->covered_end_pfn -=  processed_pfn;
-			mutex_lock(&dm_device.ha_region_mutex);
+			spin_unlock_irqrestore(&dm_device.ha_lock, flags);
 			break;
 		}
 
 		/*
-		 * Wait for the memory block to be onlined.
-		 * Since the hot add has succeeded, it is ok to
-		 * proceed even if the pages in the hot added region
-		 * have not been "onlined" within the allowed time.
+		 * Wait for the memory block to be onlined when memory onlining
+		 * is done outside of kernel (memhp_auto_online). Since the hot
+		 * add has succeeded, it is ok to proceed even if the pages in
+		 * the hot added region have not been "onlined" within the
+		 * allowed time.
 		 */
-		wait_for_completion_timeout(&dm_device.ol_waitevent, 5*HZ);
-		mutex_lock(&dm_device.ha_region_mutex);
+		if (dm_device.ha_waiting)
+			wait_for_completion_timeout(&dm_device.ol_waitevent,
+						    5*HZ);
 		post_status(&dm_device);
 	}
 
@@ -675,47 +721,64 @@ static void hv_mem_hot_add(unsigned long start, unsigned long size,
 
 static void hv_online_page(struct page *pg)
 {
-	struct list_head *cur;
 	struct hv_hotadd_state *has;
 	unsigned long cur_start_pgp;
 	unsigned long cur_end_pgp;
+	unsigned long flags;
 
-	list_for_each(cur, &dm_device.ha_region_list) {
-		has = list_entry(cur, struct hv_hotadd_state, list);
-		cur_start_pgp = (unsigned long)pfn_to_page(has->start_pfn);
-		cur_end_pgp = (unsigned long)pfn_to_page(has->covered_end_pfn);
+	spin_lock_irqsave(&dm_device.ha_lock, flags);
+	list_for_each_entry(has, &dm_device.ha_region_list, list) {
+		cur_start_pgp = (unsigned long)
+			pfn_to_page(has->start_pfn);
+		cur_end_pgp = (unsigned long)pfn_to_page(has->end_pfn);
 
-		if (((unsigned long)pg >= cur_start_pgp) &&
-			((unsigned long)pg < cur_end_pgp)) {
-			/*
-			 * This frame is currently backed; online the
-			 * page.
-			 */
-			__online_page_set_limits(pg);
-			__online_page_increment_counters(pg);
-			__online_page_free(pg);
-		}
+		/* The page belongs to a different HAS. */
+		if (((unsigned long)pg < cur_start_pgp) ||
+		    ((unsigned long)pg >= cur_end_pgp))
+			continue;
+
+		hv_page_online_one(has, pg);
+		break;
 	}
+	spin_unlock_irqrestore(&dm_device.ha_lock, flags);
 }
 
-static bool pfn_covered(unsigned long start_pfn, unsigned long pfn_cnt)
+static int pfn_covered(unsigned long start_pfn, unsigned long pfn_cnt)
 {
-	struct list_head *cur;
 	struct hv_hotadd_state *has;
+	struct hv_hotadd_gap *gap;
 	unsigned long residual, new_inc;
+	int ret = 0;
+	unsigned long flags;
 
-	if (list_empty(&dm_device.ha_region_list))
-		return false;
-
-	list_for_each(cur, &dm_device.ha_region_list) {
-		has = list_entry(cur, struct hv_hotadd_state, list);
-
+	spin_lock_irqsave(&dm_device.ha_lock, flags);
+	list_for_each_entry(has, &dm_device.ha_region_list, list) {
 		/*
 		 * If the pfn range we are dealing with is not in the current
 		 * "hot add block", move on.
 		 */
 		if (start_pfn < has->start_pfn || start_pfn >= has->end_pfn)
 			continue;
+
+		/*
+		 * If the current start pfn is not where the covered_end
+		 * is, create a gap and update covered_end_pfn.
+		 */
+		if (has->covered_end_pfn != start_pfn) {
+			gap = kzalloc(sizeof(struct hv_hotadd_gap), GFP_ATOMIC);
+			if (!gap) {
+				ret = -ENOMEM;
+				break;
+			}
+
+			INIT_LIST_HEAD(&gap->list);
+			gap->start_pfn = has->covered_end_pfn;
+			gap->end_pfn = start_pfn;
+			list_add_tail(&gap->list, &has->gap_list);
+
+			has->covered_end_pfn = start_pfn;
+		}
+
 		/*
 		 * If the current hot add-request extends beyond
 		 * our current limit; extend it.
@@ -732,19 +795,12 @@ static bool pfn_covered(unsigned long start_pfn, unsigned long pfn_cnt)
 			has->end_pfn += new_inc;
 		}
 
-		/*
-		 * If the current start pfn is not where the covered_end
-		 * is, update it.
-		 */
-
-		if (has->covered_end_pfn != start_pfn)
-			has->covered_end_pfn = start_pfn;
-
-		return true;
-
+		ret = 1;
+		break;
 	}
+	spin_unlock_irqrestore(&dm_device.ha_lock, flags);
 
-	return false;
+	return ret;
 }
 
 static unsigned long handle_pg_range(unsigned long pg_start,
@@ -753,17 +809,13 @@ static unsigned long handle_pg_range(unsigned long pg_start,
 	unsigned long start_pfn = pg_start;
 	unsigned long pfn_cnt = pg_count;
 	unsigned long size;
-	struct list_head *cur;
 	struct hv_hotadd_state *has;
 	unsigned long pgs_ol = 0;
 	unsigned long old_covered_state;
+	unsigned long res = 0, flags;
 
-	if (list_empty(&dm_device.ha_region_list))
-		return 0;
-
-	list_for_each(cur, &dm_device.ha_region_list) {
-		has = list_entry(cur, struct hv_hotadd_state, list);
-
+	spin_lock_irqsave(&dm_device.ha_lock, flags);
+	list_for_each_entry(has, &dm_device.ha_region_list, list) {
 		/*
 		 * If the pfn range we are dealing with is not in the current
 		 * "hot add block", move on.
@@ -783,6 +835,8 @@ static unsigned long handle_pg_range(unsigned long pg_start,
 			if (pgs_ol > pfn_cnt)
 				pgs_ol = pfn_cnt;
 
+			has->covered_end_pfn +=  pgs_ol;
+			pfn_cnt -= pgs_ol;
 			/*
 			 * Check if the corresponding memory block is already
 			 * online by checking its last previously backed page.
@@ -791,10 +845,8 @@ static unsigned long handle_pg_range(unsigned long pg_start,
 			 */
 			if (start_pfn > has->start_pfn &&
 			    !PageReserved(pfn_to_page(start_pfn - 1)))
-				hv_bring_pgs_online(start_pfn, pgs_ol);
+				hv_bring_pgs_online(has, start_pfn, pgs_ol);
 
-			has->covered_end_pfn +=  pgs_ol;
-			pfn_cnt -= pgs_ol;
 		}
 
 		if ((has->ha_end_pfn < has->end_pfn) && (pfn_cnt > 0)) {
@@ -813,17 +865,20 @@ static unsigned long handle_pg_range(unsigned long pg_start,
 			} else {
 				pfn_cnt = size;
 			}
+			spin_unlock_irqrestore(&dm_device.ha_lock, flags);
 			hv_mem_hot_add(has->ha_end_pfn, size, pfn_cnt, has);
+			spin_lock_irqsave(&dm_device.ha_lock, flags);
 		}
 		/*
 		 * If we managed to online any pages that were given to us,
 		 * we declare success.
 		 */
-		return has->covered_end_pfn - old_covered_state;
-
+		res = has->covered_end_pfn - old_covered_state;
+		break;
 	}
+	spin_unlock_irqrestore(&dm_device.ha_lock, flags);
 
-	return 0;
+	return res;
 }
 
 static unsigned long process_hot_add(unsigned long pg_start,
@@ -832,13 +887,20 @@ static unsigned long process_hot_add(unsigned long pg_start,
 					unsigned long rg_size)
 {
 	struct hv_hotadd_state *ha_region = NULL;
+	int covered;
+	unsigned long flags;
 
 	if (pfn_cnt == 0)
 		return 0;
 
-	if (!dm_device.host_specified_ha_region)
-		if (pfn_covered(pg_start, pfn_cnt))
+	if (!dm_device.host_specified_ha_region) {
+		covered = pfn_covered(pg_start, pfn_cnt);
+		if (covered < 0)
+			return 0;
+
+		if (covered)
 			goto do_pg_range;
+	}
 
 	/*
 	 * If the host has specified a hot-add range; deal with it first.
@@ -850,12 +912,17 @@ static unsigned long process_hot_add(unsigned long pg_start,
 			return 0;
 
 		INIT_LIST_HEAD(&ha_region->list);
+		INIT_LIST_HEAD(&ha_region->gap_list);
 
-		list_add_tail(&ha_region->list, &dm_device.ha_region_list);
 		ha_region->start_pfn = rg_start;
 		ha_region->ha_end_pfn = rg_start;
+		ha_region->covered_start_pfn = pg_start;
 		ha_region->covered_end_pfn = pg_start;
 		ha_region->end_pfn = rg_start + rg_size;
+
+		spin_lock_irqsave(&dm_device.ha_lock, flags);
+		list_add_tail(&ha_region->list, &dm_device.ha_region_list);
+		spin_unlock_irqrestore(&dm_device.ha_lock, flags);
 	}
 
 do_pg_range:
@@ -882,7 +949,6 @@ static void hot_add_req(struct work_struct *dummy)
 	resp.hdr.size = sizeof(struct dm_hot_add_response);
 
 #ifdef CONFIG_MEMORY_HOTPLUG
-	mutex_lock(&dm_device.ha_region_mutex);
 	pg_start = dm->ha_wrk.ha_page_range.finfo.start_page;
 	pfn_cnt = dm->ha_wrk.ha_page_range.finfo.page_cnt;
 
@@ -916,7 +982,6 @@ static void hot_add_req(struct work_struct *dummy)
 						rg_start, rg_sz);
 
 	dm->num_pages_added += resp.page_count;
-	mutex_unlock(&dm_device.ha_region_mutex);
 #endif
 	/*
 	 * The result field of the response structure has the
@@ -1010,7 +1075,6 @@ static unsigned long compute_balloon_floor(void)
 static void post_status(struct hv_dynmem_device *dm)
 {
 	struct dm_status status;
-	struct sysinfo val;
 	unsigned long now = jiffies;
 	unsigned long last_post = last_post_time;
 
@@ -1022,7 +1086,6 @@ static void post_status(struct hv_dynmem_device *dm)
 	if (!time_after(now, (last_post_time + HZ)))
 		return;
 
-	si_meminfo(&val);
 	memset(&status, 0, sizeof(struct dm_status));
 	status.hdr.type = DM_STATUS_REPORT;
 	status.hdr.size = sizeof(struct dm_status);
@@ -1038,7 +1101,7 @@ static void post_status(struct hv_dynmem_device *dm)
 	 * num_pages_onlined) as committed to the host, otherwise it can try
 	 * asking us to balloon them out.
 	 */
-	status.num_avail = val.freeram;
+	status.num_avail = si_mem_available();
 	status.num_committed = vm_memory_committed() +
 		dm->num_pages_ballooned +
 		(dm->num_pages_added > dm->num_pages_onlined ?
@@ -1144,7 +1207,7 @@ static void balloon_up(struct work_struct *dummy)
 	int ret;
 	bool done = false;
 	int i;
-	struct sysinfo val;
+	long avail_pages;
 	unsigned long floor;
 
 	/* The host balloons pages in 2M granularity. */
@@ -1156,12 +1219,12 @@ static void balloon_up(struct work_struct *dummy)
 	 */
 	alloc_unit = 512;
 
-	si_meminfo(&val);
+	avail_pages = si_mem_available();
 	floor = compute_balloon_floor();
 
 	/* Refuse to balloon below the floor, keep the 2M granularity. */
-	if (val.freeram < num_pages || val.freeram - num_pages < floor) {
-		num_pages = val.freeram > floor ? (val.freeram - floor) : 0;
+	if (avail_pages < num_pages || avail_pages - num_pages < floor) {
+		num_pages = avail_pages > floor ? (avail_pages - floor) : 0;
 		num_pages -= num_pages % PAGES_IN_2M;
 	}
 
@@ -1172,7 +1235,6 @@ static void balloon_up(struct work_struct *dummy)
 		bl_resp->hdr.size = sizeof(struct dm_balloon_response);
 		bl_resp->more_pages = 1;
 
-
 		num_pages -= num_ballooned;
 		num_ballooned = alloc_balloon_pages(&dm_device, num_pages,
 						    bl_resp, alloc_unit);
@@ -1461,7 +1523,7 @@ static int balloon_probe(struct hv_device *dev,
 	init_completion(&dm_device.host_event);
 	init_completion(&dm_device.config_event);
 	INIT_LIST_HEAD(&dm_device.ha_region_list);
-	mutex_init(&dm_device.ha_region_mutex);
+	spin_lock_init(&dm_device.ha_lock);
 	INIT_WORK(&dm_device.balloon_wrk.wrk, balloon_up);
 	INIT_WORK(&dm_device.ha_wrk.wrk, hot_add_req);
 	dm_device.host_specified_ha_region = false;
@@ -1580,8 +1642,9 @@ probe_error0:
 static int balloon_remove(struct hv_device *dev)
 {
 	struct hv_dynmem_device *dm = hv_get_drvdata(dev);
-	struct list_head *cur, *tmp;
-	struct hv_hotadd_state *has;
+	struct hv_hotadd_state *has, *tmp;
+	struct hv_hotadd_gap *gap, *tmp_gap;
+	unsigned long flags;
 
 	if (dm->num_pages_ballooned != 0)
 		pr_warn("Ballooned pages: %d\n", dm->num_pages_ballooned);
@@ -1596,11 +1659,16 @@ static int balloon_remove(struct hv_device *dev)
 	restore_online_page_callback(&hv_online_page);
 	unregister_memory_notifier(&hv_memory_nb);
 #endif
-	list_for_each_safe(cur, tmp, &dm->ha_region_list) {
-		has = list_entry(cur, struct hv_hotadd_state, list);
+	spin_lock_irqsave(&dm_device.ha_lock, flags);
+	list_for_each_entry_safe(has, tmp, &dm->ha_region_list, list) {
+		list_for_each_entry_safe(gap, tmp_gap, &has->gap_list, list) {
+			list_del(&gap->list);
+			kfree(gap);
+		}
 		list_del(&has->list);
 		kfree(has);
 	}
+	spin_unlock_irqrestore(&dm_device.ha_lock, flags);
 
 	return 0;
 }
diff --git a/drivers/hv/hv_fcopy.c b/drivers/hv/hv_fcopy.c
index 23c70799ad8a..8b2ba98831ec 100644
--- a/drivers/hv/hv_fcopy.c
+++ b/drivers/hv/hv_fcopy.c
@@ -83,6 +83,12 @@ static void fcopy_timeout_func(struct work_struct *dummy)
 	hv_poll_channel(fcopy_transaction.recv_channel, fcopy_poll_wrapper);
 }
 
+static void fcopy_register_done(void)
+{
+	pr_debug("FCP: userspace daemon registered\n");
+	hv_poll_channel(fcopy_transaction.recv_channel, fcopy_poll_wrapper);
+}
+
 static int fcopy_handle_handshake(u32 version)
 {
 	u32 our_ver = FCOPY_CURRENT_VERSION;
@@ -94,7 +100,8 @@ static int fcopy_handle_handshake(u32 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)))
+		if (hvutil_transport_send(hvt, &our_ver, sizeof(our_ver),
+		    fcopy_register_done))
 			return -EFAULT;
 		dm_reg_value = version;
 		break;
@@ -107,8 +114,7 @@ static int fcopy_handle_handshake(u32 version)
 		 */
 		return -EINVAL;
 	}
-	pr_debug("FCP: userspace daemon ver. %d registered\n", version);
-	hv_poll_channel(fcopy_transaction.recv_channel, fcopy_poll_wrapper);
+	pr_debug("FCP: userspace daemon ver. %d connected\n", version);
 	return 0;
 }
 
@@ -161,7 +167,7 @@ static void fcopy_send_data(struct work_struct *dummy)
 	}
 
 	fcopy_transaction.state = HVUTIL_USERSPACE_REQ;
-	rc = hvutil_transport_send(hvt, out_src, out_len);
+	rc = hvutil_transport_send(hvt, out_src, out_len, NULL);
 	if (rc) {
 		pr_debug("FCP: failed to communicate to the daemon: %d\n", rc);
 		if (cancel_delayed_work_sync(&fcopy_timeout_work)) {
diff --git a/drivers/hv/hv_kvp.c b/drivers/hv/hv_kvp.c
index cb1a9160aab1..5e1fdc8d32ab 100644
--- a/drivers/hv/hv_kvp.c
+++ b/drivers/hv/hv_kvp.c
@@ -102,6 +102,17 @@ static void kvp_poll_wrapper(void *channel)
 	hv_kvp_onchannelcallback(channel);
 }
 
+static void kvp_register_done(void)
+{
+	/*
+	 * If we're still negotiating with the host cancel the timeout
+	 * work to not poll the channel twice.
+	 */
+	pr_debug("KVP: userspace daemon registered\n");
+	cancel_delayed_work_sync(&kvp_host_handshake_work);
+	hv_poll_channel(kvp_transaction.recv_channel, kvp_poll_wrapper);
+}
+
 static void
 kvp_register(int reg_value)
 {
@@ -116,7 +127,8 @@ kvp_register(int reg_value)
 		kvp_msg->kvp_hdr.operation = reg_value;
 		strcpy(version, HV_DRV_VERSION);
 
-		hvutil_transport_send(hvt, kvp_msg, sizeof(*kvp_msg));
+		hvutil_transport_send(hvt, kvp_msg, sizeof(*kvp_msg),
+				      kvp_register_done);
 		kfree(kvp_msg);
 	}
 }
@@ -158,17 +170,10 @@ static int kvp_handle_handshake(struct hv_kvp_msg *msg)
 	/*
 	 * We have a compatible daemon; complete the handshake.
 	 */
-	pr_debug("KVP: userspace daemon ver. %d registered\n",
-		 KVP_OP_REGISTER);
+	pr_debug("KVP: userspace daemon ver. %d connected\n",
+		 msg->kvp_hdr.operation);
 	kvp_register(dm_reg_value);
 
-	/*
-	 * If we're still negotiating with the host cancel the timeout
-	 * work to not poll the channel twice.
-	 */
-	cancel_delayed_work_sync(&kvp_host_handshake_work);
-	hv_poll_channel(kvp_transaction.recv_channel, kvp_poll_wrapper);
-
 	return 0;
 }
 
@@ -455,7 +460,7 @@ kvp_send_key(struct work_struct *dummy)
 	}
 
 	kvp_transaction.state = HVUTIL_USERSPACE_REQ;
-	rc = hvutil_transport_send(hvt, message, sizeof(*message));
+	rc = hvutil_transport_send(hvt, message, sizeof(*message), NULL);
 	if (rc) {
 		pr_debug("KVP: failed to communicate to the daemon: %d\n", rc);
 		if (cancel_delayed_work_sync(&kvp_timeout_work)) {
diff --git a/drivers/hv/hv_snapshot.c b/drivers/hv/hv_snapshot.c
index 3fba14e88f03..a6707133c297 100644
--- a/drivers/hv/hv_snapshot.c
+++ b/drivers/hv/hv_snapshot.c
@@ -67,11 +67,11 @@ 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);
+static void vss_handle_request(struct work_struct *dummy);
 
 static DECLARE_DELAYED_WORK(vss_timeout_work, vss_timeout_func);
-static DECLARE_WORK(vss_send_op_work, vss_send_op);
+static DECLARE_WORK(vss_handle_request_work, vss_handle_request);
 
 static void vss_poll_wrapper(void *channel)
 {
@@ -95,6 +95,12 @@ static void vss_timeout_func(struct work_struct *dummy)
 	hv_poll_channel(vss_transaction.recv_channel, vss_poll_wrapper);
 }
 
+static void vss_register_done(void)
+{
+	hv_poll_channel(vss_transaction.recv_channel, vss_poll_wrapper);
+	pr_debug("VSS: userspace daemon registered\n");
+}
+
 static int vss_handle_handshake(struct hv_vss_msg *vss_msg)
 {
 	u32 our_ver = VSS_OP_REGISTER1;
@@ -105,16 +111,16 @@ static int vss_handle_handshake(struct hv_vss_msg *vss_msg)
 		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)))
+		/* Daemon expects us to reply with our own version */
+		if (hvutil_transport_send(hvt, &our_ver, sizeof(our_ver),
+					  vss_register_done))
 			return -EFAULT;
 		dm_reg_value = VSS_OP_REGISTER1;
 		break;
 	default:
 		return -EINVAL;
 	}
-	hv_poll_channel(vss_transaction.recv_channel, vss_poll_wrapper);
-	pr_debug("VSS: userspace daemon ver. %d registered\n", dm_reg_value);
+	pr_debug("VSS: userspace daemon ver. %d connected\n", dm_reg_value);
 	return 0;
 }
 
@@ -136,6 +142,11 @@ static int vss_on_msg(void *msg, int len)
 		return vss_handle_handshake(vss_msg);
 	} else if (vss_transaction.state == HVUTIL_USERSPACE_REQ) {
 		vss_transaction.state = HVUTIL_USERSPACE_RECV;
+
+		if (vss_msg->vss_hdr.operation == VSS_OP_HOT_BACKUP)
+			vss_transaction.msg->vss_cf.flags =
+				VSS_HBU_NO_AUTO_RECOVERY;
+
 		if (cancel_delayed_work_sync(&vss_timeout_work)) {
 			vss_respond_to_host(vss_msg->error);
 			/* Transaction is finished, reset the state. */
@@ -150,8 +161,7 @@ static int vss_on_msg(void *msg, int len)
 	return 0;
 }
 
-
-static void vss_send_op(struct work_struct *dummy)
+static void vss_send_op(void)
 {
 	int op = vss_transaction.msg->vss_hdr.operation;
 	int rc;
@@ -168,7 +178,10 @@ static void vss_send_op(struct work_struct *dummy)
 	vss_msg->vss_hdr.operation = op;
 
 	vss_transaction.state = HVUTIL_USERSPACE_REQ;
-	rc = hvutil_transport_send(hvt, vss_msg, sizeof(*vss_msg));
+
+	schedule_delayed_work(&vss_timeout_work, VSS_USERSPACE_TIMEOUT);
+
+	rc = hvutil_transport_send(hvt, vss_msg, sizeof(*vss_msg), NULL);
 	if (rc) {
 		pr_warn("VSS: failed to communicate to the daemon: %d\n", rc);
 		if (cancel_delayed_work_sync(&vss_timeout_work)) {
@@ -182,6 +195,38 @@ static void vss_send_op(struct work_struct *dummy)
 	return;
 }
 
+static void vss_handle_request(struct work_struct *dummy)
+{
+	switch (vss_transaction.msg->vss_hdr.operation) {
+	/*
+	 * Initiate a "freeze/thaw" operation in the guest.
+	 * We respond to the host once the operation is complete.
+	 *
+	 * We send the message to the user space daemon and the operation is
+	 * performed in the daemon.
+	 */
+	case VSS_OP_THAW:
+	case VSS_OP_FREEZE:
+	case VSS_OP_HOT_BACKUP:
+		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;
+		vss_send_op();
+		return;
+	case VSS_OP_GET_DM_INFO:
+		vss_transaction.msg->dm_info.flags = 0;
+		break;
+	default:
+		break;
+	}
+
+	vss_respond_to_host(0);
+	hv_poll_channel(vss_transaction.recv_channel, vss_poll_wrapper);
+}
+
 /*
  * Send a response back to the host.
  */
@@ -266,48 +311,8 @@ void hv_vss_onchannelcallback(void *context)
 			vss_transaction.recv_req_id = requestid;
 			vss_transaction.msg = (struct hv_vss_msg *)vss_msg;
 
-			switch (vss_msg->vss_hdr.operation) {
-				/*
-				 * Initiate a "freeze/thaw"
-				 * operation in the guest.
-				 * We respond to the host once
-				 * the operation is complete.
-				 *
-				 * We send the message to the
-				 * user space daemon and the
-				 * operation is performed in
-				 * the daemon.
-				 */
-			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);
-				return;
-
-			case VSS_OP_HOT_BACKUP:
-				vss_msg->vss_cf.flags =
-					 VSS_HBU_NO_AUTO_RECOVERY;
-				vss_respond_to_host(0);
-				return;
-
-			case VSS_OP_GET_DM_INFO:
-				vss_msg->dm_info.flags = 0;
-				vss_respond_to_host(0);
-				return;
-
-			default:
-				vss_respond_to_host(0);
-				return;
-
-			}
-
+			schedule_work(&vss_handle_request_work);
+			return;
 		}
 
 		icmsghdrp->icflags = ICMSGHDRFLAG_TRANSACTION
@@ -358,6 +363,6 @@ void hv_vss_deinit(void)
 {
 	vss_transaction.state = HVUTIL_DEVICE_DYING;
 	cancel_delayed_work_sync(&vss_timeout_work);
-	cancel_work_sync(&vss_send_op_work);
+	cancel_work_sync(&vss_handle_request_work);
 	hvutil_transport_destroy(hvt);
 }
diff --git a/drivers/hv/hv_util.c b/drivers/hv/hv_util.c
index d5acaa2d8e61..4aa3cb63fd41 100644
--- a/drivers/hv/hv_util.c
+++ b/drivers/hv/hv_util.c
@@ -34,22 +34,25 @@
 #define SD_MINOR	0
 #define SD_VERSION	(SD_MAJOR << 16 | SD_MINOR)
 
-#define SD_WS2008_MAJOR		1
-#define SD_WS2008_VERSION	(SD_WS2008_MAJOR << 16 | SD_MINOR)
+#define SD_MAJOR_1	1
+#define SD_VERSION_1	(SD_MAJOR_1 << 16 | SD_MINOR)
 
-#define TS_MAJOR	3
+#define TS_MAJOR	4
 #define TS_MINOR	0
 #define TS_VERSION	(TS_MAJOR << 16 | TS_MINOR)
 
-#define TS_WS2008_MAJOR		1
-#define TS_WS2008_VERSION	(TS_WS2008_MAJOR << 16 | TS_MINOR)
+#define TS_MAJOR_1	1
+#define TS_VERSION_1	(TS_MAJOR_1 << 16 | TS_MINOR)
+
+#define TS_MAJOR_3	3
+#define TS_VERSION_3	(TS_MAJOR_3 << 16 | TS_MINOR)
 
 #define HB_MAJOR	3
-#define HB_MINOR 0
+#define HB_MINOR	0
 #define HB_VERSION	(HB_MAJOR << 16 | HB_MINOR)
 
-#define HB_WS2008_MAJOR	1
-#define HB_WS2008_VERSION	(HB_WS2008_MAJOR << 16 | HB_MINOR)
+#define HB_MAJOR_1	1
+#define HB_VERSION_1	(HB_MAJOR_1 << 16 | HB_MINOR)
 
 static int sd_srv_version;
 static int ts_srv_version;
@@ -61,9 +64,14 @@ static struct hv_util_service util_shutdown = {
 	.util_cb = shutdown_onchannelcallback,
 };
 
+static int hv_timesync_init(struct hv_util_service *srv);
+static void hv_timesync_deinit(void);
+
 static void timesync_onchannelcallback(void *context);
 static struct hv_util_service util_timesynch = {
 	.util_cb = timesync_onchannelcallback,
+	.util_init = hv_timesync_init,
+	.util_deinit = hv_timesync_deinit,
 };
 
 static void heartbeat_onchannelcallback(void *context);
@@ -161,35 +169,43 @@ static void shutdown_onchannelcallback(void *context)
 }
 
 /*
- * Set guest time to host UTC time.
- */
-static inline void do_adj_guesttime(u64 hosttime)
-{
-	s64 host_tns;
-	struct timespec host_ts;
-
-	host_tns = (hosttime - WLTIMEDELTA) * 100;
-	host_ts = ns_to_timespec(host_tns);
-
-	do_settimeofday(&host_ts);
-}
-
-/*
  * Set the host time in a process context.
  */
 
 struct adj_time_work {
 	struct work_struct work;
 	u64	host_time;
+	u64	ref_time;
+	u8	flags;
 };
 
 static void hv_set_host_time(struct work_struct *work)
 {
 	struct adj_time_work	*wrk;
+	s64 host_tns;
+	u64 newtime;
+	struct timespec host_ts;
 
 	wrk = container_of(work, struct adj_time_work, work);
-	do_adj_guesttime(wrk->host_time);
-	kfree(wrk);
+
+	newtime = wrk->host_time;
+	if (ts_srv_version > TS_VERSION_3) {
+		/*
+		 * Some latency has been introduced since Hyper-V generated
+		 * its time sample. Take that latency into account before
+		 * using TSC reference time sample from Hyper-V.
+		 *
+		 * This sample is given by TimeSync v4 and above hosts.
+		 */
+		u64 current_tick;
+
+		rdmsrl(HV_X64_MSR_TIME_REF_COUNT, current_tick);
+		newtime += (current_tick - wrk->ref_time);
+	}
+	host_tns = (newtime - WLTIMEDELTA) * 100;
+	host_ts = ns_to_timespec(host_tns);
+
+	do_settimeofday(&host_ts);
 }
 
 /*
@@ -198,33 +214,31 @@ static void hv_set_host_time(struct work_struct *work)
  * ICTIMESYNCFLAG_SYNC flag bit indicates reboot, restore events of the VM.
  * After reboot the flag ICTIMESYNCFLAG_SYNC is included in the first time
  * message after the timesync channel is opened. Since the hv_utils module is
- * loaded after hv_vmbus, the first message is usually missed. The other
- * thing is, systime is automatically set to emulated hardware clock which may
- * not be UTC time or in the same time zone. So, to override these effects, we
- * use the first 50 time samples for initial system time setting.
+ * loaded after hv_vmbus, the first message is usually missed. This bit is
+ * considered a hard request to discipline the clock.
+ *
+ * ICTIMESYNCFLAG_SAMPLE bit indicates a time sample from host. This is
+ * typically used as a hint to the guest. The guest is under no obligation
+ * to discipline the clock.
  */
-static inline void adj_guesttime(u64 hosttime, u8 flags)
+static struct adj_time_work  wrk;
+static inline void adj_guesttime(u64 hosttime, u64 reftime, u8 flags)
 {
-	struct adj_time_work    *wrk;
-	static s32 scnt = 50;
 
-	wrk = kmalloc(sizeof(struct adj_time_work), GFP_ATOMIC);
-	if (wrk == NULL)
+	/*
+	 * This check is safe since we are executing in the
+	 * interrupt context and time synch messages arre always
+	 * delivered on the same CPU.
+	 */
+	if (work_pending(&wrk.work))
 		return;
 
-	wrk->host_time = hosttime;
-	if ((flags & ICTIMESYNCFLAG_SYNC) != 0) {
-		INIT_WORK(&wrk->work, hv_set_host_time);
-		schedule_work(&wrk->work);
-		return;
+	wrk.host_time = hosttime;
+	wrk.ref_time = reftime;
+	wrk.flags = flags;
+	if ((flags & (ICTIMESYNCFLAG_SYNC | ICTIMESYNCFLAG_SAMPLE)) != 0) {
+		schedule_work(&wrk.work);
 	}
-
-	if ((flags & ICTIMESYNCFLAG_SAMPLE) != 0 && scnt > 0) {
-		scnt--;
-		INIT_WORK(&wrk->work, hv_set_host_time);
-		schedule_work(&wrk->work);
-	} else
-		kfree(wrk);
 }
 
 /*
@@ -237,6 +251,7 @@ static void timesync_onchannelcallback(void *context)
 	u64 requestid;
 	struct icmsg_hdr *icmsghdrp;
 	struct ictimesync_data *timedatap;
+	struct ictimesync_ref_data *refdata;
 	u8 *time_txf_buf = util_timesynch.recv_buffer;
 	struct icmsg_negotiate *negop = NULL;
 
@@ -252,11 +267,27 @@ static void timesync_onchannelcallback(void *context)
 						time_txf_buf,
 						util_fw_version,
 						ts_srv_version);
+			pr_info("Using TimeSync version %d.%d\n",
+				ts_srv_version >> 16, ts_srv_version & 0xFFFF);
 		} else {
-			timedatap = (struct ictimesync_data *)&time_txf_buf[
-				sizeof(struct vmbuspipe_hdr) +
-				sizeof(struct icmsg_hdr)];
-			adj_guesttime(timedatap->parenttime, timedatap->flags);
+			if (ts_srv_version > TS_VERSION_3) {
+				refdata = (struct ictimesync_ref_data *)
+					&time_txf_buf[
+					sizeof(struct vmbuspipe_hdr) +
+					sizeof(struct icmsg_hdr)];
+
+				adj_guesttime(refdata->parenttime,
+						refdata->vmreferencetime,
+						refdata->flags);
+			} else {
+				timedatap = (struct ictimesync_data *)
+					&time_txf_buf[
+					sizeof(struct vmbuspipe_hdr) +
+					sizeof(struct icmsg_hdr)];
+				adj_guesttime(timedatap->parenttime,
+						0,
+						timedatap->flags);
+			}
 		}
 
 		icmsghdrp->icflags = ICMSGHDRFLAG_TRANSACTION
@@ -350,16 +381,21 @@ static int util_probe(struct hv_device *dev,
 	switch (vmbus_proto_version) {
 	case (VERSION_WS2008):
 		util_fw_version = UTIL_WS2K8_FW_VERSION;
-		sd_srv_version = SD_WS2008_VERSION;
-		ts_srv_version = TS_WS2008_VERSION;
-		hb_srv_version = HB_WS2008_VERSION;
+		sd_srv_version = SD_VERSION_1;
+		ts_srv_version = TS_VERSION_1;
+		hb_srv_version = HB_VERSION_1;
 		break;
-
-	default:
+	case(VERSION_WIN10):
 		util_fw_version = UTIL_FW_VERSION;
 		sd_srv_version = SD_VERSION;
 		ts_srv_version = TS_VERSION;
 		hb_srv_version = HB_VERSION;
+		break;
+	default:
+		util_fw_version = UTIL_FW_VERSION;
+		sd_srv_version = SD_VERSION;
+		ts_srv_version = TS_VERSION_3;
+		hb_srv_version = HB_VERSION;
 	}
 
 	ret = vmbus_open(dev->channel, 4 * PAGE_SIZE, 4 * PAGE_SIZE, NULL, 0,
@@ -427,6 +463,17 @@ static  struct hv_driver util_drv = {
 	.remove =  util_remove,
 };
 
+static int hv_timesync_init(struct hv_util_service *srv)
+{
+	INIT_WORK(&wrk.work, hv_set_host_time);
+	return 0;
+}
+
+static void hv_timesync_deinit(void)
+{
+	cancel_work_sync(&wrk.work);
+}
+
 static int __init init_hyperv_utils(void)
 {
 	pr_info("Registering HyperV Utility Driver\n");
diff --git a/drivers/hv/hv_utils_transport.c b/drivers/hv/hv_utils_transport.c
index 9a9983fa4531..c235a9515267 100644
--- a/drivers/hv/hv_utils_transport.c
+++ b/drivers/hv/hv_utils_transport.c
@@ -72,6 +72,10 @@ static ssize_t hvt_op_read(struct file *file, char __user *buf,
 	hvt->outmsg = NULL;
 	hvt->outmsg_len = 0;
 
+	if (hvt->on_read)
+		hvt->on_read();
+	hvt->on_read = NULL;
+
 out_unlock:
 	mutex_unlock(&hvt->lock);
 	return ret;
@@ -219,7 +223,8 @@ static void hvt_cn_callback(struct cn_msg *msg, struct netlink_skb_parms *nsp)
 	mutex_unlock(&hvt->lock);
 }
 
-int hvutil_transport_send(struct hvutil_transport *hvt, void *msg, int len)
+int hvutil_transport_send(struct hvutil_transport *hvt, void *msg, int len,
+			  void (*on_read_cb)(void))
 {
 	struct cn_msg *cn_msg;
 	int ret = 0;
@@ -237,6 +242,13 @@ int hvutil_transport_send(struct hvutil_transport *hvt, void *msg, int len)
 		memcpy(cn_msg->data, msg, len);
 		ret = cn_netlink_send(cn_msg, 0, 0, GFP_ATOMIC);
 		kfree(cn_msg);
+		/*
+		 * We don't know when netlink messages are delivered but unlike
+		 * in CHARDEV mode we're not blocked and we can send next
+		 * messages right away.
+		 */
+		if (on_read_cb)
+			on_read_cb();
 		return ret;
 	}
 	/* HVUTIL_TRANSPORT_CHARDEV */
@@ -255,6 +267,7 @@ int hvutil_transport_send(struct hvutil_transport *hvt, void *msg, int len)
 	if (hvt->outmsg) {
 		memcpy(hvt->outmsg, msg, len);
 		hvt->outmsg_len = len;
+		hvt->on_read = on_read_cb;
 		wake_up_interruptible(&hvt->outmsg_q);
 	} else
 		ret = -ENOMEM;
diff --git a/drivers/hv/hv_utils_transport.h b/drivers/hv/hv_utils_transport.h
index 06254a165a18..d98f5225c3e6 100644
--- a/drivers/hv/hv_utils_transport.h
+++ b/drivers/hv/hv_utils_transport.h
@@ -36,6 +36,7 @@ struct hvutil_transport {
 	struct list_head list;              /* hvt_list */
 	int (*on_msg)(void *, int);         /* callback on new user message */
 	void (*on_reset)(void);             /* callback when userspace drops */
+	void (*on_read)(void);              /* callback on message read */
 	u8 *outmsg;                         /* message to the userspace */
 	int outmsg_len;                     /* its length */
 	wait_queue_head_t outmsg_q;         /* poll/read wait queue */
@@ -46,7 +47,8 @@ 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);
+int hvutil_transport_send(struct hvutil_transport *hvt, void *msg, int len,
+			  void (*on_read_cb)(void));
 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 718b5c72f0c8..a5b4442433c8 100644
--- a/drivers/hv/hyperv_vmbus.h
+++ b/drivers/hv/hyperv_vmbus.h
@@ -495,7 +495,7 @@ struct hv_ring_buffer_debug_info {
 
 extern int hv_init(void);
 
-extern void hv_cleanup(void);
+extern void hv_cleanup(bool crash);
 
 extern int hv_post_message(union hv_connection_id connection_id,
 			 enum hv_message_type message_type,
@@ -522,14 +522,15 @@ extern unsigned int host_info_edx;
 /* Interface */
 
 
-int hv_ringbuffer_init(struct hv_ring_buffer_info *ring_info, void *buffer,
-		   u32 buflen);
+int hv_ringbuffer_init(struct hv_ring_buffer_info *ring_info,
+		       struct page *pages, u32 pagecnt);
 
 void hv_ringbuffer_cleanup(struct hv_ring_buffer_info *ring_info);
 
 int hv_ringbuffer_write(struct hv_ring_buffer_info *ring_info,
 		    struct kvec *kv_list,
-		    u32 kv_count, bool *signal, bool lock);
+		    u32 kv_count, bool *signal, bool lock,
+		    enum hv_signal_policy policy);
 
 int hv_ringbuffer_read(struct hv_ring_buffer_info *inring_info,
 		       void *buffer, u32 buflen, u32 *buffer_actual_len,
diff --git a/drivers/hv/ring_buffer.c b/drivers/hv/ring_buffer.c
index fe586bf74e17..08043da1a61c 100644
--- a/drivers/hv/ring_buffer.c
+++ b/drivers/hv/ring_buffer.c
@@ -27,6 +27,8 @@
 #include <linux/mm.h>
 #include <linux/hyperv.h>
 #include <linux/uio.h>
+#include <linux/vmalloc.h>
+#include <linux/slab.h>
 
 #include "hyperv_vmbus.h"
 
@@ -66,12 +68,20 @@ u32 hv_end_read(struct hv_ring_buffer_info *rbi)
  *	   arrived.
  */
 
-static bool hv_need_to_signal(u32 old_write, struct hv_ring_buffer_info *rbi)
+static bool hv_need_to_signal(u32 old_write, struct hv_ring_buffer_info *rbi,
+			      enum hv_signal_policy policy)
 {
 	virt_mb();
 	if (READ_ONCE(rbi->ring_buffer->interrupt_mask))
 		return false;
 
+	/*
+	 * When the client wants to control signaling,
+	 * we only honour the host interrupt mask.
+	 */
+	if (policy == HV_SIGNAL_POLICY_EXPLICIT)
+		return true;
+
 	/* check interrupt_mask before read_index */
 	virt_rmb();
 	/*
@@ -162,18 +172,7 @@ static u32 hv_copyfrom_ringbuffer(
 	void *ring_buffer = hv_get_ring_buffer(ring_info);
 	u32 ring_buffer_size = hv_get_ring_buffersize(ring_info);
 
-	u32 frag_len;
-
-	/* wrap-around detected at the src */
-	if (destlen > ring_buffer_size - start_read_offset) {
-		frag_len = ring_buffer_size - start_read_offset;
-
-		memcpy(dest, ring_buffer + start_read_offset, frag_len);
-		memcpy(dest + frag_len, ring_buffer, destlen - frag_len);
-	} else
-
-		memcpy(dest, ring_buffer + start_read_offset, destlen);
-
+	memcpy(dest, ring_buffer + start_read_offset, destlen);
 
 	start_read_offset += destlen;
 	start_read_offset %= ring_buffer_size;
@@ -194,15 +193,8 @@ static u32 hv_copyto_ringbuffer(
 {
 	void *ring_buffer = hv_get_ring_buffer(ring_info);
 	u32 ring_buffer_size = hv_get_ring_buffersize(ring_info);
-	u32 frag_len;
 
-	/* wrap-around detected! */
-	if (srclen > ring_buffer_size - start_write_offset) {
-		frag_len = ring_buffer_size - start_write_offset;
-		memcpy(ring_buffer + start_write_offset, src, frag_len);
-		memcpy(ring_buffer, src + frag_len, srclen - frag_len);
-	} else
-		memcpy(ring_buffer + start_write_offset, src, srclen);
+	memcpy(ring_buffer + start_write_offset, src, srclen);
 
 	start_write_offset += srclen;
 	start_write_offset %= ring_buffer_size;
@@ -235,22 +227,46 @@ void hv_ringbuffer_get_debuginfo(struct hv_ring_buffer_info *ring_info,
 
 /* Initialize the ring buffer. */
 int hv_ringbuffer_init(struct hv_ring_buffer_info *ring_info,
-		   void *buffer, u32 buflen)
+		       struct page *pages, u32 page_cnt)
 {
-	if (sizeof(struct hv_ring_buffer) != PAGE_SIZE)
-		return -EINVAL;
+	int i;
+	struct page **pages_wraparound;
+
+	BUILD_BUG_ON((sizeof(struct hv_ring_buffer) != PAGE_SIZE));
 
 	memset(ring_info, 0, sizeof(struct hv_ring_buffer_info));
 
-	ring_info->ring_buffer = (struct hv_ring_buffer *)buffer;
+	/*
+	 * First page holds struct hv_ring_buffer, do wraparound mapping for
+	 * the rest.
+	 */
+	pages_wraparound = kzalloc(sizeof(struct page *) * (page_cnt * 2 - 1),
+				   GFP_KERNEL);
+	if (!pages_wraparound)
+		return -ENOMEM;
+
+	pages_wraparound[0] = pages;
+	for (i = 0; i < 2 * (page_cnt - 1); i++)
+		pages_wraparound[i + 1] = &pages[i % (page_cnt - 1) + 1];
+
+	ring_info->ring_buffer = (struct hv_ring_buffer *)
+		vmap(pages_wraparound, page_cnt * 2 - 1, VM_MAP, PAGE_KERNEL);
+
+	kfree(pages_wraparound);
+
+
+	if (!ring_info->ring_buffer)
+		return -ENOMEM;
+
 	ring_info->ring_buffer->read_index =
 		ring_info->ring_buffer->write_index = 0;
 
 	/* Set the feature bit for enabling flow control. */
 	ring_info->ring_buffer->feature_bits.value = 1;
 
-	ring_info->ring_size = buflen;
-	ring_info->ring_datasize = buflen - sizeof(struct hv_ring_buffer);
+	ring_info->ring_size = page_cnt << PAGE_SHIFT;
+	ring_info->ring_datasize = ring_info->ring_size -
+		sizeof(struct hv_ring_buffer);
 
 	spin_lock_init(&ring_info->ring_lock);
 
@@ -260,11 +276,13 @@ int hv_ringbuffer_init(struct hv_ring_buffer_info *ring_info,
 /* Cleanup the ring buffer. */
 void hv_ringbuffer_cleanup(struct hv_ring_buffer_info *ring_info)
 {
+	vunmap(ring_info->ring_buffer);
 }
 
 /* Write to the ring buffer. */
 int hv_ringbuffer_write(struct hv_ring_buffer_info *outring_info,
-		    struct kvec *kv_list, u32 kv_count, bool *signal, bool lock)
+		    struct kvec *kv_list, u32 kv_count, bool *signal, bool lock,
+		    enum hv_signal_policy policy)
 {
 	int i = 0;
 	u32 bytes_avail_towrite;
@@ -326,7 +344,7 @@ int hv_ringbuffer_write(struct hv_ring_buffer_info *outring_info,
 	if (lock)
 		spin_unlock_irqrestore(&outring_info->ring_lock, flags);
 
-	*signal = hv_need_to_signal(old_write, outring_info);
+	*signal = hv_need_to_signal(old_write, outring_info, policy);
 	return 0;
 }
 
diff --git a/drivers/hv/vmbus_drv.c b/drivers/hv/vmbus_drv.c
index e82f7e1c217c..a259e18d22d5 100644
--- a/drivers/hv/vmbus_drv.c
+++ b/drivers/hv/vmbus_drv.c
@@ -105,8 +105,8 @@ static struct notifier_block hyperv_panic_block = {
 
 static const char *fb_mmio_name = "fb_range";
 static struct resource *fb_mmio;
-struct resource *hyperv_mmio;
-DEFINE_SEMAPHORE(hyperv_mmio_lock);
+static struct resource *hyperv_mmio;
+static DEFINE_SEMAPHORE(hyperv_mmio_lock);
 
 static int vmbus_exists(void)
 {
@@ -874,7 +874,7 @@ err_alloc:
 	bus_unregister(&hv_bus);
 
 err_cleanup:
-	hv_cleanup();
+	hv_cleanup(false);
 
 	return ret;
 }
@@ -961,8 +961,8 @@ int vmbus_device_register(struct hv_device *child_device_obj)
 {
 	int ret = 0;
 
-	dev_set_name(&child_device_obj->device, "vmbus_%d",
-		     child_device_obj->channel->id);
+	dev_set_name(&child_device_obj->device, "vmbus-%pUl",
+		     child_device_obj->channel->offermsg.offer.if_instance.b);
 
 	child_device_obj->device.bus = &hv_bus;
 	child_device_obj->device.parent = &hv_acpi_dev->dev;
@@ -1326,7 +1326,7 @@ static void hv_kexec_handler(void)
 	vmbus_initiate_unload(false);
 	for_each_online_cpu(cpu)
 		smp_call_function_single(cpu, hv_synic_cleanup, NULL, 1);
-	hv_cleanup();
+	hv_cleanup(false);
 };
 
 static void hv_crash_handler(struct pt_regs *regs)
@@ -1338,7 +1338,7 @@ static void hv_crash_handler(struct pt_regs *regs)
 	 * for kdump.
 	 */
 	hv_synic_cleanup(NULL);
-	hv_cleanup();
+	hv_cleanup(true);
 };
 
 static int __init hv_acpi_init(void)
@@ -1398,7 +1398,7 @@ static void __exit vmbus_exit(void)
 						 &hyperv_panic_block);
 	}
 	bus_unregister(&hv_bus);
-	hv_cleanup();
+	hv_cleanup(false);
 	for_each_online_cpu(cpu) {
 		tasklet_kill(hv_context.event_dpc[cpu]);
 		smp_call_function_single(cpu, hv_synic_cleanup, NULL, 1);
diff --git a/drivers/hwmon/Kconfig b/drivers/hwmon/Kconfig
index eaf2f916d48c..45cef3d2c75c 100644
--- a/drivers/hwmon/Kconfig
+++ b/drivers/hwmon/Kconfig
@@ -969,7 +969,6 @@ config SENSORS_LM73
 config SENSORS_LM75
 	tristate "National Semiconductor LM75 and compatibles"
 	depends on I2C
-	depends on THERMAL || !THERMAL_OF
 	select REGMAP_I2C
 	help
 	  If you say yes here you get support for one common type of
@@ -1119,6 +1118,7 @@ config SENSORS_LM95241
 config SENSORS_LM95245
 	tristate "National Semiconductor LM95245 and compatibles"
 	depends on I2C
+	select REGMAP_I2C
 	help
 	  If you say yes here you get support for LM95235 and LM95245
 	  temperature sensor chips.
@@ -1572,7 +1572,6 @@ config SENSORS_THMC50
 config SENSORS_TMP102
 	tristate "Texas Instruments TMP102"
 	depends on I2C
-	depends on THERMAL || !THERMAL_OF
 	select REGMAP_I2C
 	help
 	  If you say yes here you get support for Texas Instruments TMP102
@@ -1823,6 +1822,13 @@ config SENSORS_ULTRA45
 	  This driver provides support for the Ultra45 workstation environmental
 	  sensors.
 
+config SENSORS_XGENE
+	tristate "APM X-Gene SoC hardware monitoring driver"
+	depends on XGENE_SLIMPRO_MBOX || PCC
+	help
+	  If you say yes here you get support for the temperature
+	  and power sensors for APM X-Gene SoC.
+
 if ACPI
 
 comment "ACPI drivers"
diff --git a/drivers/hwmon/Makefile b/drivers/hwmon/Makefile
index fe87d2895a97..aecf4ba17460 100644
--- a/drivers/hwmon/Makefile
+++ b/drivers/hwmon/Makefile
@@ -165,6 +165,7 @@ obj-$(CONFIG_SENSORS_W83L785TS)	+= w83l785ts.o
 obj-$(CONFIG_SENSORS_W83L786NG)	+= w83l786ng.o
 obj-$(CONFIG_SENSORS_WM831X)	+= wm831x-hwmon.o
 obj-$(CONFIG_SENSORS_WM8350)	+= wm8350-hwmon.o
+obj-$(CONFIG_SENSORS_XGENE)	+= xgene-hwmon.o
 
 obj-$(CONFIG_PMBUS)		+= pmbus/
 
diff --git a/drivers/hwmon/adm9240.c b/drivers/hwmon/adm9240.c
index 98114cef1e43..2fe1828bd10b 100644
--- a/drivers/hwmon/adm9240.c
+++ b/drivers/hwmon/adm9240.c
@@ -194,10 +194,10 @@ static struct adm9240_data *adm9240_update_device(struct device *dev)
 		 * 0.5'C per two measurement cycles thus ignore possible
 		 * but unlikely aliasing error on lsb reading. --Grant
 		 */
-		data->temp = ((i2c_smbus_read_byte_data(client,
+		data->temp = (i2c_smbus_read_byte_data(client,
 					ADM9240_REG_TEMP) << 8) |
 					i2c_smbus_read_byte_data(client,
-					ADM9240_REG_TEMP_CONF)) / 128;
+					ADM9240_REG_TEMP_CONF);
 
 		for (i = 0; i < 2; i++) { /* read fans */
 			data->fan[i] = i2c_smbus_read_byte_data(client,
@@ -263,7 +263,7 @@ static ssize_t show_temp(struct device *dev, struct device_attribute *dummy,
 		char *buf)
 {
 	struct adm9240_data *data = adm9240_update_device(dev);
-	return sprintf(buf, "%d\n", data->temp * 500); /* 9-bit value */
+	return sprintf(buf, "%d\n", data->temp / 128 * 500); /* 9-bit value */
 }
 
 static ssize_t show_max(struct device *dev, struct device_attribute *devattr,
diff --git a/drivers/hwmon/adt7411.c b/drivers/hwmon/adt7411.c
index fc1e65a263a4..812fbc00f693 100644
--- a/drivers/hwmon/adt7411.c
+++ b/drivers/hwmon/adt7411.c
@@ -7,8 +7,7 @@
  *  it under the terms of the GNU General Public License version 2 as
  *  published by the Free Software Foundation.
  *
- *  TODO: SPI, support for external temperature sensor
- *	  use power-down mode for suspend?, interrupt handling?
+ *  TODO: SPI, use power-down mode for suspend?, interrupt handling?
  */
 
 #include <linux/kernel.h>
@@ -31,6 +30,7 @@
 #define ADT7411_REG_CFG1			0x18
 #define ADT7411_CFG1_START_MONITOR		(1 << 0)
 #define ADT7411_CFG1_RESERVED_BIT1		(1 << 1)
+#define ADT7411_CFG1_EXT_TDM			(1 << 2)
 #define ADT7411_CFG1_RESERVED_BIT3		(1 << 3)
 
 #define ADT7411_REG_CFG2			0x19
@@ -57,6 +57,7 @@ struct adt7411_data {
 	unsigned long next_update;
 	int vref_cached;
 	struct i2c_client *client;
+	bool use_ext_temp;
 };
 
 /*
@@ -127,11 +128,20 @@ static ssize_t adt7411_show_vdd(struct device *dev,
 static ssize_t adt7411_show_temp(struct device *dev,
 			struct device_attribute *attr, char *buf)
 {
+	int nr = to_sensor_dev_attr(attr)->index;
 	struct adt7411_data *data = dev_get_drvdata(dev);
 	struct i2c_client *client = data->client;
-	int val = adt7411_read_10_bit(client, ADT7411_REG_INT_TEMP_VDD_LSB,
-			ADT7411_REG_INT_TEMP_MSB, 0);
-
+	int val;
+	struct {
+		u8 low;
+		u8 high;
+	} reg[2] = {
+		{ ADT7411_REG_INT_TEMP_VDD_LSB, ADT7411_REG_INT_TEMP_MSB },
+		{ ADT7411_REG_EXT_TEMP_AIN14_LSB,
+		  ADT7411_REG_EXT_TEMP_AIN1_MSB },
+	};
+
+	val = adt7411_read_10_bit(client, reg[nr].low, reg[nr].high, 0);
 	if (val < 0)
 		return val;
 
@@ -218,11 +228,13 @@ static ssize_t adt7411_set_bit(struct device *dev,
 	return ret < 0 ? ret : count;
 }
 
+
 #define ADT7411_BIT_ATTR(__name, __reg, __bit) \
 	SENSOR_DEVICE_ATTR_2(__name, S_IRUGO | S_IWUSR, adt7411_show_bit, \
 	adt7411_set_bit, __bit, __reg)
 
-static DEVICE_ATTR(temp1_input, S_IRUGO, adt7411_show_temp, NULL);
+static SENSOR_DEVICE_ATTR(temp1_input, S_IRUGO, adt7411_show_temp, NULL, 0);
+static SENSOR_DEVICE_ATTR(temp2_input, S_IRUGO, adt7411_show_temp, NULL, 1);
 static DEVICE_ATTR(in0_input, S_IRUGO, adt7411_show_vdd, NULL);
 static SENSOR_DEVICE_ATTR(in1_input, S_IRUGO, adt7411_show_input, NULL, 0);
 static SENSOR_DEVICE_ATTR(in2_input, S_IRUGO, adt7411_show_input, NULL, 1);
@@ -237,7 +249,8 @@ static ADT7411_BIT_ATTR(fast_sampling, ADT7411_REG_CFG3, ADT7411_CFG3_ADC_CLK_22
 static ADT7411_BIT_ATTR(adc_ref_vdd, ADT7411_REG_CFG3, ADT7411_CFG3_REF_VDD);
 
 static struct attribute *adt7411_attrs[] = {
-	&dev_attr_temp1_input.attr,
+	&sensor_dev_attr_temp1_input.dev_attr.attr,
+	&sensor_dev_attr_temp2_input.dev_attr.attr,
 	&dev_attr_in0_input.attr,
 	&sensor_dev_attr_in1_input.dev_attr.attr,
 	&sensor_dev_attr_in2_input.dev_attr.attr,
@@ -253,7 +266,27 @@ static struct attribute *adt7411_attrs[] = {
 	NULL
 };
 
-ATTRIBUTE_GROUPS(adt7411);
+static umode_t adt7411_attrs_visible(struct kobject *kobj,
+				     struct attribute *attr, int index)
+{
+	struct device *dev = container_of(kobj, struct device, kobj);
+	struct adt7411_data *data = dev_get_drvdata(dev);
+	bool visible = true;
+
+	if (attr == &sensor_dev_attr_temp2_input.dev_attr.attr)
+		visible = data->use_ext_temp;
+	else if (attr == &sensor_dev_attr_in1_input.dev_attr.attr ||
+		 attr == &sensor_dev_attr_in2_input.dev_attr.attr)
+		visible = !data->use_ext_temp;
+
+	return visible ? attr->mode : 0;
+}
+
+static const struct attribute_group adt7411_group = {
+	.attrs = adt7411_attrs,
+	.is_visible = adt7411_attrs_visible,
+};
+__ATTRIBUTE_GROUPS(adt7411);
 
 static int adt7411_detect(struct i2c_client *client,
 			  struct i2c_board_info *info)
@@ -309,6 +342,8 @@ static int adt7411_init_device(struct adt7411_data *data)
 	if (ret < 0)
 		return ret;
 
+	data->use_ext_temp = ret & ADT7411_CFG1_EXT_TDM;
+
 	/*
 	 * We must only write zero to bit 1 and only one to bit 3 according to
 	 * the datasheet.
diff --git a/drivers/hwmon/adt7470.c b/drivers/hwmon/adt7470.c
index f5da39a68929..6e60ca53406e 100644
--- a/drivers/hwmon/adt7470.c
+++ b/drivers/hwmon/adt7470.c
@@ -32,6 +32,7 @@
 #include <linux/log2.h>
 #include <linux/kthread.h>
 #include <linux/slab.h>
+#include <linux/util_macros.h>
 
 /* Addresses to scan */
 static const unsigned short normal_i2c[] = { 0x2C, 0x2E, 0x2F, I2C_CLIENT_END };
@@ -83,6 +84,7 @@ static const unsigned short normal_i2c[] = { 0x2C, 0x2E, 0x2F, I2C_CLIENT_END };
 #define ADT7470_REG_PWM_MIN_MAX_ADDR		0x6D
 #define ADT7470_REG_PWM_TEMP_MIN_BASE_ADDR	0x6E
 #define ADT7470_REG_PWM_TEMP_MIN_MAX_ADDR	0x71
+#define ADT7470_REG_CFG_2			0x74
 #define ADT7470_REG_ACOUSTICS12			0x75
 #define ADT7470_REG_ACOUSTICS34			0x76
 #define ADT7470_REG_DEVICE			0x3D
@@ -142,6 +144,11 @@ static const unsigned short normal_i2c[] = { 0x2C, 0x2E, 0x2F, I2C_CLIENT_END };
 #define FAN_PERIOD_INVALID	65535
 #define FAN_DATA_VALID(x)	((x) && (x) != FAN_PERIOD_INVALID)
 
+/* Config registers 1 and 2 include fields for selecting the PWM frequency */
+#define ADT7470_CFG_LF		0x40
+#define ADT7470_FREQ_MASK	0x70
+#define ADT7470_FREQ_SHIFT	4
+
 struct adt7470_data {
 	struct i2c_client	*client;
 	struct mutex		lock;
@@ -170,7 +177,6 @@ struct adt7470_data {
 	u8			pwm_auto_temp[ADT7470_PWM_COUNT];
 
 	struct task_struct	*auto_update;
-	struct completion	auto_update_stop;
 	unsigned int		auto_update_interval;
 };
 
@@ -266,12 +272,14 @@ static int adt7470_update_thread(void *p)
 		mutex_lock(&data->lock);
 		adt7470_read_temperatures(client, data);
 		mutex_unlock(&data->lock);
+
+		set_current_state(TASK_INTERRUPTIBLE);
 		if (kthread_should_stop())
 			break;
-		msleep_interruptible(data->auto_update_interval);
+
+		schedule_timeout(msecs_to_jiffies(data->auto_update_interval));
 	}
 
-	complete_all(&data->auto_update_stop);
 	return 0;
 }
 
@@ -538,6 +546,28 @@ static ssize_t show_alarm_mask(struct device *dev,
 	return sprintf(buf, "%x\n", data->alarms_mask);
 }
 
+static ssize_t set_alarm_mask(struct device *dev,
+			      struct device_attribute *devattr,
+			      const char *buf,
+			      size_t count)
+{
+	struct adt7470_data *data = dev_get_drvdata(dev);
+	long mask;
+
+	if (kstrtoul(buf, 0, &mask))
+		return -EINVAL;
+
+	if (mask & ~0xffff)
+		return -EINVAL;
+
+	mutex_lock(&data->lock);
+	data->alarms_mask = mask;
+	adt7470_write_word_data(data->client, ADT7470_REG_ALARM1_MASK, mask);
+	mutex_unlock(&data->lock);
+
+	return count;
+}
+
 static ssize_t show_fan_max(struct device *dev,
 			    struct device_attribute *devattr,
 			    char *buf)
@@ -688,6 +718,70 @@ static ssize_t set_pwm(struct device *dev, struct device_attribute *devattr,
 	return count;
 }
 
+/* These are the valid PWM frequencies to the nearest Hz */
+static const int adt7470_freq_map[] = {
+	11, 15, 22, 29, 35, 44, 59, 88, 1400, 22500
+};
+
+static ssize_t show_pwm_freq(struct device *dev,
+			     struct device_attribute *devattr, char *buf)
+{
+	struct adt7470_data *data = adt7470_update_device(dev);
+	unsigned char cfg_reg_1;
+	unsigned char cfg_reg_2;
+	int index;
+
+	mutex_lock(&data->lock);
+	cfg_reg_1 = i2c_smbus_read_byte_data(data->client, ADT7470_REG_CFG);
+	cfg_reg_2 = i2c_smbus_read_byte_data(data->client, ADT7470_REG_CFG_2);
+	mutex_unlock(&data->lock);
+
+	index = (cfg_reg_2 & ADT7470_FREQ_MASK) >> ADT7470_FREQ_SHIFT;
+	if (!(cfg_reg_1 & ADT7470_CFG_LF))
+		index += 8;
+	if (index >= ARRAY_SIZE(adt7470_freq_map))
+		index = ARRAY_SIZE(adt7470_freq_map) - 1;
+
+	return scnprintf(buf, PAGE_SIZE, "%d\n", adt7470_freq_map[index]);
+}
+
+static ssize_t set_pwm_freq(struct device *dev,
+			    struct device_attribute *devattr,
+			    const char *buf, size_t count)
+{
+	struct adt7470_data *data = dev_get_drvdata(dev);
+	struct i2c_client *client = data->client;
+	long freq;
+	int index;
+	int low_freq = ADT7470_CFG_LF;
+	unsigned char val;
+
+	if (kstrtol(buf, 10, &freq))
+		return -EINVAL;
+
+	/* Round the user value given to the closest available frequency */
+	index = find_closest(freq, adt7470_freq_map,
+			     ARRAY_SIZE(adt7470_freq_map));
+
+	if (index >= 8) {
+		index -= 8;
+		low_freq = 0;
+	}
+
+	mutex_lock(&data->lock);
+	/* Configuration Register 1 */
+	val = i2c_smbus_read_byte_data(client, ADT7470_REG_CFG);
+	i2c_smbus_write_byte_data(client, ADT7470_REG_CFG,
+				  (val & ~ADT7470_CFG_LF) | low_freq);
+	/* Configuration Register 2 */
+	val = i2c_smbus_read_byte_data(client, ADT7470_REG_CFG_2);
+	i2c_smbus_write_byte_data(client, ADT7470_REG_CFG_2,
+		(val & ~ADT7470_FREQ_MASK) | (index << ADT7470_FREQ_SHIFT));
+	mutex_unlock(&data->lock);
+
+	return count;
+}
+
 static ssize_t show_pwm_max(struct device *dev,
 			    struct device_attribute *devattr,
 			    char *buf)
@@ -918,7 +1012,8 @@ static ssize_t show_alarm(struct device *dev,
 		return sprintf(buf, "0\n");
 }
 
-static DEVICE_ATTR(alarm_mask, S_IRUGO, show_alarm_mask, NULL);
+static DEVICE_ATTR(alarm_mask, S_IWUSR | S_IRUGO, show_alarm_mask,
+		   set_alarm_mask);
 static DEVICE_ATTR(num_temp_sensors, S_IWUSR | S_IRUGO, show_num_temp_sensors,
 		   set_num_temp_sensors);
 static DEVICE_ATTR(auto_update_interval, S_IWUSR | S_IRUGO,
@@ -1038,6 +1133,8 @@ static SENSOR_DEVICE_ATTR(pwm2, S_IWUSR | S_IRUGO, show_pwm, set_pwm, 1);
 static SENSOR_DEVICE_ATTR(pwm3, S_IWUSR | S_IRUGO, show_pwm, set_pwm, 2);
 static SENSOR_DEVICE_ATTR(pwm4, S_IWUSR | S_IRUGO, show_pwm, set_pwm, 3);
 
+static DEVICE_ATTR(pwm1_freq, S_IWUSR | S_IRUGO, show_pwm_freq, set_pwm_freq);
+
 static SENSOR_DEVICE_ATTR(pwm1_auto_point1_pwm, S_IWUSR | S_IRUGO,
 		    show_pwm_min, set_pwm_min, 0);
 static SENSOR_DEVICE_ATTR(pwm2_auto_point1_pwm, S_IWUSR | S_IRUGO,
@@ -1154,6 +1251,7 @@ static struct attribute *adt7470_attrs[] = {
 	&sensor_dev_attr_fan4_alarm.dev_attr.attr,
 	&sensor_dev_attr_force_pwm_max.dev_attr.attr,
 	&sensor_dev_attr_pwm1.dev_attr.attr,
+	&dev_attr_pwm1_freq.attr,
 	&sensor_dev_attr_pwm2.dev_attr.attr,
 	&sensor_dev_attr_pwm3.dev_attr.attr,
 	&sensor_dev_attr_pwm4.dev_attr.attr,
@@ -1256,7 +1354,6 @@ static int adt7470_probe(struct i2c_client *client,
 	if (IS_ERR(hwmon_dev))
 		return PTR_ERR(hwmon_dev);
 
-	init_completion(&data->auto_update_stop);
 	data->auto_update = kthread_run(adt7470_update_thread, client, "%s",
 					dev_name(hwmon_dev));
 	if (IS_ERR(data->auto_update)) {
@@ -1271,7 +1368,6 @@ static int adt7470_remove(struct i2c_client *client)
 	struct adt7470_data *data = i2c_get_clientdata(client);
 
 	kthread_stop(data->auto_update);
-	wait_for_completion(&data->auto_update_stop);
 	return 0;
 }
 
diff --git a/drivers/hwmon/dell-smm-hwmon.c b/drivers/hwmon/dell-smm-hwmon.c
index acf9c0361d9f..34704b0451b4 100644
--- a/drivers/hwmon/dell-smm-hwmon.c
+++ b/drivers/hwmon/dell-smm-hwmon.c
@@ -21,6 +21,7 @@
 
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
+#include <linux/cpu.h>
 #include <linux/delay.h>
 #include <linux/module.h>
 #include <linux/types.h>
@@ -36,6 +37,7 @@
 #include <linux/io.h>
 #include <linux/sched.h>
 #include <linux/ctype.h>
+#include <linux/smp.h>
 
 #include <linux/i8k.h>
 
@@ -134,11 +136,11 @@ static inline const char *i8k_get_dmi_data(int field)
 /*
  * Call the System Management Mode BIOS. Code provided by Jonathan Buzzard.
  */
-static int i8k_smm(struct smm_regs *regs)
+static int i8k_smm_func(void *par)
 {
 	int rc;
+	struct smm_regs *regs = par;
 	int eax = regs->eax;
-	cpumask_var_t old_mask;
 
 #ifdef DEBUG
 	int ebx = regs->ebx;
@@ -149,16 +151,8 @@ static int i8k_smm(struct smm_regs *regs)
 #endif
 
 	/* SMM requires CPU 0 */
-	if (!alloc_cpumask_var(&old_mask, GFP_KERNEL))
-		return -ENOMEM;
-	cpumask_copy(old_mask, &current->cpus_allowed);
-	rc = set_cpus_allowed_ptr(current, cpumask_of(0));
-	if (rc)
-		goto out;
-	if (smp_processor_id() != 0) {
-		rc = -EBUSY;
-		goto out;
-	}
+	if (smp_processor_id() != 0)
+		return -EBUSY;
 
 #if defined(CONFIG_X86_64)
 	asm volatile("pushq %%rax\n\t"
@@ -216,10 +210,6 @@ static int i8k_smm(struct smm_regs *regs)
 	if (rc != 0 || (regs->eax & 0xffff) == 0xffff || regs->eax == eax)
 		rc = -EINVAL;
 
-out:
-	set_cpus_allowed_ptr(current, old_mask);
-	free_cpumask_var(old_mask);
-
 #ifdef DEBUG
 	rettime = ktime_get();
 	delta = ktime_sub(rettime, calltime);
@@ -232,6 +222,20 @@ out:
 }
 
 /*
+ * Call the System Management Mode BIOS.
+ */
+static int i8k_smm(struct smm_regs *regs)
+{
+	int ret;
+
+	get_online_cpus();
+	ret = smp_call_on_cpu(0, i8k_smm_func, regs, true);
+	put_online_cpus();
+
+	return ret;
+}
+
+/*
  * Read the fan status.
  */
 static int i8k_get_fan_status(int fan)
diff --git a/drivers/hwmon/ftsteutates.c b/drivers/hwmon/ftsteutates.c
index 48633e541dc3..0f0277e7aae5 100644
--- a/drivers/hwmon/ftsteutates.c
+++ b/drivers/hwmon/ftsteutates.c
@@ -36,6 +36,10 @@
 #define FTS_EVENT_STATUS_REG		0x0006
 #define FTS_GLOBAL_CONTROL_REG		0x0007
 
+#define FTS_DEVICE_DETECT_REG_1		0x0C
+#define FTS_DEVICE_DETECT_REG_2		0x0D
+#define FTS_DEVICE_DETECT_REG_3		0x0E
+
 #define FTS_SENSOR_EVENT_REG		0x0010
 
 #define FTS_FAN_EVENT_REG		0x0014
@@ -54,6 +58,8 @@
 #define FTS_NO_TEMP_SENSORS		0x10
 #define FTS_NO_VOLT_SENSORS		0x04
 
+static const unsigned short normal_i2c[] = { 0x73, I2C_CLIENT_END };
+
 static struct i2c_device_id fts_id[] = {
 	{ "ftsteutates", 0 },
 	{ }
@@ -734,6 +740,42 @@ static const struct attribute_group *fts_attr_groups[] = {
 /*****************************************************************************/
 /* Module initialization / remove functions				     */
 /*****************************************************************************/
+static int fts_detect(struct i2c_client *client,
+		      struct i2c_board_info *info)
+{
+	int val;
+
+	/* detection works with revsion greater or equal to 0x2b */
+	val = i2c_smbus_read_byte_data(client, FTS_DEVICE_REVISION_REG);
+	if (val < 0x2b)
+		return -ENODEV;
+
+	/* Device Detect Regs must have 0x17 0x34 and 0x54 */
+	val = i2c_smbus_read_byte_data(client, FTS_DEVICE_DETECT_REG_1);
+	if (val != 0x17)
+		return -ENODEV;
+
+	val = i2c_smbus_read_byte_data(client, FTS_DEVICE_DETECT_REG_2);
+	if (val != 0x34)
+		return -ENODEV;
+
+	val = i2c_smbus_read_byte_data(client, FTS_DEVICE_DETECT_REG_3);
+	if (val != 0x54)
+		return -ENODEV;
+
+	/*
+	 * 0x10 == Baseboard Management Controller, 0x01 == Teutates
+	 * Device ID Reg needs to be 0x11
+	 */
+	val = i2c_smbus_read_byte_data(client, FTS_DEVICE_ID_REG);
+	if (val != 0x11)
+		return -ENODEV;
+
+	strlcpy(info->type, fts_id[0].name, I2C_NAME_SIZE);
+	info->flags = 0;
+	return 0;
+}
+
 static int fts_remove(struct i2c_client *client)
 {
 	struct fts_data *data = dev_get_drvdata(&client->dev);
@@ -804,12 +846,15 @@ static int fts_probe(struct i2c_client *client, const struct i2c_device_id *id)
 /* Module Details							     */
 /*****************************************************************************/
 static struct i2c_driver fts_driver = {
+	.class = I2C_CLASS_HWMON,
 	.driver = {
 		.name = "ftsteutates",
 	},
 	.id_table = fts_id,
 	.probe = fts_probe,
 	.remove = fts_remove,
+	.detect = fts_detect,
+	.address_list = normal_i2c,
 };
 
 module_i2c_driver(fts_driver);
diff --git a/drivers/hwmon/hwmon.c b/drivers/hwmon/hwmon.c
index a26c385a435b..adae6848ffb2 100644
--- a/drivers/hwmon/hwmon.c
+++ b/drivers/hwmon/hwmon.c
@@ -12,17 +12,17 @@
 
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
-#include <linux/module.h>
+#include <linux/bitops.h>
 #include <linux/device.h>
 #include <linux/err.h>
-#include <linux/slab.h>
-#include <linux/kdev_t.h>
-#include <linux/idr.h>
-#include <linux/hwmon.h>
 #include <linux/gfp.h>
-#include <linux/spinlock.h>
+#include <linux/hwmon.h>
+#include <linux/idr.h>
+#include <linux/module.h>
 #include <linux/pci.h>
+#include <linux/slab.h>
 #include <linux/string.h>
+#include <linux/thermal.h>
 
 #define HWMON_ID_PREFIX "hwmon"
 #define HWMON_ID_FORMAT HWMON_ID_PREFIX "%d"
@@ -30,9 +30,35 @@
 struct hwmon_device {
 	const char *name;
 	struct device dev;
+	const struct hwmon_chip_info *chip;
+
+	struct attribute_group group;
+	const struct attribute_group **groups;
 };
+
 #define to_hwmon_device(d) container_of(d, struct hwmon_device, dev)
 
+struct hwmon_device_attribute {
+	struct device_attribute dev_attr;
+	const struct hwmon_ops *ops;
+	enum hwmon_sensor_types type;
+	u32 attr;
+	int index;
+};
+
+#define to_hwmon_attr(d) \
+	container_of(d, struct hwmon_device_attribute, dev_attr)
+
+/*
+ * Thermal zone information
+ * In addition to the reference to the hwmon device,
+ * also provides the sensor index.
+ */
+struct hwmon_thermal_data {
+	struct hwmon_device *hwdev;	/* Reference to hwmon device */
+	int index;			/* sensor index */
+};
+
 static ssize_t
 show_name(struct device *dev, struct device_attribute *attr, char *buf)
 {
@@ -80,25 +106,409 @@ static struct class hwmon_class = {
 
 static DEFINE_IDA(hwmon_ida);
 
-/**
- * hwmon_device_register_with_groups - register w/ hwmon
- * @dev: the parent device
- * @name: hwmon name attribute
- * @drvdata: driver data to attach to created device
- * @groups: List of attribute groups to create
- *
- * hwmon_device_unregister() must be called when the device is no
- * longer needed.
- *
- * Returns the pointer to the new device.
+/* Thermal zone handling */
+
+/*
+ * The complex conditional is necessary to avoid a cyclic dependency
+ * between hwmon and thermal_sys modules.
  */
-struct device *
-hwmon_device_register_with_groups(struct device *dev, const char *name,
-				  void *drvdata,
-				  const struct attribute_group **groups)
+#if IS_REACHABLE(CONFIG_THERMAL) && defined(CONFIG_THERMAL_OF) && \
+	(!defined(CONFIG_THERMAL_HWMON) || \
+	 !(defined(MODULE) && IS_MODULE(CONFIG_THERMAL)))
+static int hwmon_thermal_get_temp(void *data, int *temp)
+{
+	struct hwmon_thermal_data *tdata = data;
+	struct hwmon_device *hwdev = tdata->hwdev;
+	int ret;
+	long t;
+
+	ret = hwdev->chip->ops->read(&hwdev->dev, hwmon_temp, hwmon_temp_input,
+				     tdata->index, &t);
+	if (ret < 0)
+		return ret;
+
+	*temp = t;
+
+	return 0;
+}
+
+static struct thermal_zone_of_device_ops hwmon_thermal_ops = {
+	.get_temp = hwmon_thermal_get_temp,
+};
+
+static int hwmon_thermal_add_sensor(struct device *dev,
+				    struct hwmon_device *hwdev, int index)
+{
+	struct hwmon_thermal_data *tdata;
+
+	tdata = devm_kzalloc(dev, sizeof(*tdata), GFP_KERNEL);
+	if (!tdata)
+		return -ENOMEM;
+
+	tdata->hwdev = hwdev;
+	tdata->index = index;
+
+	devm_thermal_zone_of_sensor_register(&hwdev->dev, index, tdata,
+					     &hwmon_thermal_ops);
+
+	return 0;
+}
+#else
+static int hwmon_thermal_add_sensor(struct device *dev,
+				    struct hwmon_device *hwdev, int index)
+{
+	return 0;
+}
+#endif /* IS_REACHABLE(CONFIG_THERMAL) && ... */
+
+/* sysfs attribute management */
+
+static ssize_t hwmon_attr_show(struct device *dev,
+			       struct device_attribute *devattr, char *buf)
+{
+	struct hwmon_device_attribute *hattr = to_hwmon_attr(devattr);
+	long val;
+	int ret;
+
+	ret = hattr->ops->read(dev, hattr->type, hattr->attr, hattr->index,
+			       &val);
+	if (ret < 0)
+		return ret;
+
+	return sprintf(buf, "%ld\n", val);
+}
+
+static ssize_t hwmon_attr_store(struct device *dev,
+				struct device_attribute *devattr,
+				const char *buf, size_t count)
+{
+	struct hwmon_device_attribute *hattr = to_hwmon_attr(devattr);
+	long val;
+	int ret;
+
+	ret = kstrtol(buf, 10, &val);
+	if (ret < 0)
+		return ret;
+
+	ret = hattr->ops->write(dev, hattr->type, hattr->attr, hattr->index,
+				val);
+	if (ret < 0)
+		return ret;
+
+	return count;
+}
+
+static int hwmon_attr_base(enum hwmon_sensor_types type)
+{
+	if (type == hwmon_in)
+		return 0;
+	return 1;
+}
+
+static struct attribute *hwmon_genattr(struct device *dev,
+				       const void *drvdata,
+				       enum hwmon_sensor_types type,
+				       u32 attr,
+				       int index,
+				       const char *template,
+				       const struct hwmon_ops *ops)
+{
+	struct hwmon_device_attribute *hattr;
+	struct device_attribute *dattr;
+	struct attribute *a;
+	umode_t mode;
+	char *name;
+
+	/* The attribute is invisible if there is no template string */
+	if (!template)
+		return ERR_PTR(-ENOENT);
+
+	mode = ops->is_visible(drvdata, type, attr, index);
+	if (!mode)
+		return ERR_PTR(-ENOENT);
+
+	if ((mode & S_IRUGO) && !ops->read)
+		return ERR_PTR(-EINVAL);
+	if ((mode & S_IWUGO) && !ops->write)
+		return ERR_PTR(-EINVAL);
+
+	if (type == hwmon_chip) {
+		name = (char *)template;
+	} else {
+		name = devm_kzalloc(dev, strlen(template) + 16, GFP_KERNEL);
+		if (!name)
+			return ERR_PTR(-ENOMEM);
+		scnprintf(name, strlen(template) + 16, template,
+			  index + hwmon_attr_base(type));
+	}
+
+	hattr = devm_kzalloc(dev, sizeof(*hattr), GFP_KERNEL);
+	if (!hattr)
+		return ERR_PTR(-ENOMEM);
+
+	hattr->type = type;
+	hattr->attr = attr;
+	hattr->index = index;
+	hattr->ops = ops;
+
+	dattr = &hattr->dev_attr;
+	dattr->show = hwmon_attr_show;
+	dattr->store = hwmon_attr_store;
+
+	a = &dattr->attr;
+	sysfs_attr_init(a);
+	a->name = name;
+	a->mode = mode;
+
+	return a;
+}
+
+static const char * const hwmon_chip_attr_templates[] = {
+	[hwmon_chip_temp_reset_history] = "temp_reset_history",
+	[hwmon_chip_in_reset_history] = "in_reset_history",
+	[hwmon_chip_curr_reset_history] = "curr_reset_history",
+	[hwmon_chip_power_reset_history] = "power_reset_history",
+	[hwmon_chip_update_interval] = "update_interval",
+	[hwmon_chip_alarms] = "alarms",
+};
+
+static const char * const hwmon_temp_attr_templates[] = {
+	[hwmon_temp_input] = "temp%d_input",
+	[hwmon_temp_type] = "temp%d_type",
+	[hwmon_temp_lcrit] = "temp%d_lcrit",
+	[hwmon_temp_lcrit_hyst] = "temp%d_lcrit_hyst",
+	[hwmon_temp_min] = "temp%d_min",
+	[hwmon_temp_min_hyst] = "temp%d_min_hyst",
+	[hwmon_temp_max] = "temp%d_max",
+	[hwmon_temp_max_hyst] = "temp%d_max_hyst",
+	[hwmon_temp_crit] = "temp%d_crit",
+	[hwmon_temp_crit_hyst] = "temp%d_crit_hyst",
+	[hwmon_temp_emergency] = "temp%d_emergency",
+	[hwmon_temp_emergency_hyst] = "temp%d_emergency_hyst",
+	[hwmon_temp_alarm] = "temp%d_alarm",
+	[hwmon_temp_lcrit_alarm] = "temp%d_lcrit_alarm",
+	[hwmon_temp_min_alarm] = "temp%d_min_alarm",
+	[hwmon_temp_max_alarm] = "temp%d_max_alarm",
+	[hwmon_temp_crit_alarm] = "temp%d_crit_alarm",
+	[hwmon_temp_emergency_alarm] = "temp%d_emergency_alarm",
+	[hwmon_temp_fault] = "temp%d_fault",
+	[hwmon_temp_offset] = "temp%d_offset",
+	[hwmon_temp_label] = "temp%d_label",
+	[hwmon_temp_lowest] = "temp%d_lowest",
+	[hwmon_temp_highest] = "temp%d_highest",
+	[hwmon_temp_reset_history] = "temp%d_reset_history",
+};
+
+static const char * const hwmon_in_attr_templates[] = {
+	[hwmon_in_input] = "in%d_input",
+	[hwmon_in_min] = "in%d_min",
+	[hwmon_in_max] = "in%d_max",
+	[hwmon_in_lcrit] = "in%d_lcrit",
+	[hwmon_in_crit] = "in%d_crit",
+	[hwmon_in_average] = "in%d_average",
+	[hwmon_in_lowest] = "in%d_lowest",
+	[hwmon_in_highest] = "in%d_highest",
+	[hwmon_in_reset_history] = "in%d_reset_history",
+	[hwmon_in_label] = "in%d_label",
+	[hwmon_in_alarm] = "in%d_alarm",
+	[hwmon_in_min_alarm] = "in%d_min_alarm",
+	[hwmon_in_max_alarm] = "in%d_max_alarm",
+	[hwmon_in_lcrit_alarm] = "in%d_lcrit_alarm",
+	[hwmon_in_crit_alarm] = "in%d_crit_alarm",
+};
+
+static const char * const hwmon_curr_attr_templates[] = {
+	[hwmon_curr_input] = "curr%d_input",
+	[hwmon_curr_min] = "curr%d_min",
+	[hwmon_curr_max] = "curr%d_max",
+	[hwmon_curr_lcrit] = "curr%d_lcrit",
+	[hwmon_curr_crit] = "curr%d_crit",
+	[hwmon_curr_average] = "curr%d_average",
+	[hwmon_curr_lowest] = "curr%d_lowest",
+	[hwmon_curr_highest] = "curr%d_highest",
+	[hwmon_curr_reset_history] = "curr%d_reset_history",
+	[hwmon_curr_label] = "curr%d_label",
+	[hwmon_curr_alarm] = "curr%d_alarm",
+	[hwmon_curr_min_alarm] = "curr%d_min_alarm",
+	[hwmon_curr_max_alarm] = "curr%d_max_alarm",
+	[hwmon_curr_lcrit_alarm] = "curr%d_lcrit_alarm",
+	[hwmon_curr_crit_alarm] = "curr%d_crit_alarm",
+};
+
+static const char * const hwmon_power_attr_templates[] = {
+	[hwmon_power_average] = "power%d_average",
+	[hwmon_power_average_interval] = "power%d_average_interval",
+	[hwmon_power_average_interval_max] = "power%d_interval_max",
+	[hwmon_power_average_interval_min] = "power%d_interval_min",
+	[hwmon_power_average_highest] = "power%d_average_highest",
+	[hwmon_power_average_lowest] = "power%d_average_lowest",
+	[hwmon_power_average_max] = "power%d_average_max",
+	[hwmon_power_average_min] = "power%d_average_min",
+	[hwmon_power_input] = "power%d_input",
+	[hwmon_power_input_highest] = "power%d_input_highest",
+	[hwmon_power_input_lowest] = "power%d_input_lowest",
+	[hwmon_power_reset_history] = "power%d_reset_history",
+	[hwmon_power_accuracy] = "power%d_accuracy",
+	[hwmon_power_cap] = "power%d_cap",
+	[hwmon_power_cap_hyst] = "power%d_cap_hyst",
+	[hwmon_power_cap_max] = "power%d_cap_max",
+	[hwmon_power_cap_min] = "power%d_cap_min",
+	[hwmon_power_max] = "power%d_max",
+	[hwmon_power_crit] = "power%d_crit",
+	[hwmon_power_label] = "power%d_label",
+	[hwmon_power_alarm] = "power%d_alarm",
+	[hwmon_power_cap_alarm] = "power%d_cap_alarm",
+	[hwmon_power_max_alarm] = "power%d_max_alarm",
+	[hwmon_power_crit_alarm] = "power%d_crit_alarm",
+};
+
+static const char * const hwmon_energy_attr_templates[] = {
+	[hwmon_energy_input] = "energy%d_input",
+	[hwmon_energy_label] = "energy%d_label",
+};
+
+static const char * const hwmon_humidity_attr_templates[] = {
+	[hwmon_humidity_input] = "humidity%d_input",
+	[hwmon_humidity_label] = "humidity%d_label",
+	[hwmon_humidity_min] = "humidity%d_min",
+	[hwmon_humidity_min_hyst] = "humidity%d_min_hyst",
+	[hwmon_humidity_max] = "humidity%d_max",
+	[hwmon_humidity_max_hyst] = "humidity%d_max_hyst",
+	[hwmon_humidity_alarm] = "humidity%d_alarm",
+	[hwmon_humidity_fault] = "humidity%d_fault",
+};
+
+static const char * const hwmon_fan_attr_templates[] = {
+	[hwmon_fan_input] = "fan%d_input",
+	[hwmon_fan_label] = "fan%d_label",
+	[hwmon_fan_min] = "fan%d_min",
+	[hwmon_fan_max] = "fan%d_max",
+	[hwmon_fan_div] = "fan%d_div",
+	[hwmon_fan_pulses] = "fan%d_pulses",
+	[hwmon_fan_target] = "fan%d_target",
+	[hwmon_fan_alarm] = "fan%d_alarm",
+	[hwmon_fan_min_alarm] = "fan%d_min_alarm",
+	[hwmon_fan_max_alarm] = "fan%d_max_alarm",
+	[hwmon_fan_fault] = "fan%d_fault",
+};
+
+static const char * const hwmon_pwm_attr_templates[] = {
+	[hwmon_pwm_input] = "pwm%d",
+	[hwmon_pwm_enable] = "pwm%d_enable",
+	[hwmon_pwm_mode] = "pwm%d_mode",
+	[hwmon_pwm_freq] = "pwm%d_freq",
+};
+
+static const char * const *__templates[] = {
+	[hwmon_chip] = hwmon_chip_attr_templates,
+	[hwmon_temp] = hwmon_temp_attr_templates,
+	[hwmon_in] = hwmon_in_attr_templates,
+	[hwmon_curr] = hwmon_curr_attr_templates,
+	[hwmon_power] = hwmon_power_attr_templates,
+	[hwmon_energy] = hwmon_energy_attr_templates,
+	[hwmon_humidity] = hwmon_humidity_attr_templates,
+	[hwmon_fan] = hwmon_fan_attr_templates,
+	[hwmon_pwm] = hwmon_pwm_attr_templates,
+};
+
+static const int __templates_size[] = {
+	[hwmon_chip] = ARRAY_SIZE(hwmon_chip_attr_templates),
+	[hwmon_temp] = ARRAY_SIZE(hwmon_temp_attr_templates),
+	[hwmon_in] = ARRAY_SIZE(hwmon_in_attr_templates),
+	[hwmon_curr] = ARRAY_SIZE(hwmon_curr_attr_templates),
+	[hwmon_power] = ARRAY_SIZE(hwmon_power_attr_templates),
+	[hwmon_energy] = ARRAY_SIZE(hwmon_energy_attr_templates),
+	[hwmon_humidity] = ARRAY_SIZE(hwmon_humidity_attr_templates),
+	[hwmon_fan] = ARRAY_SIZE(hwmon_fan_attr_templates),
+	[hwmon_pwm] = ARRAY_SIZE(hwmon_pwm_attr_templates),
+};
+
+static int hwmon_num_channel_attrs(const struct hwmon_channel_info *info)
+{
+	int i, n;
+
+	for (i = n = 0; info->config[i]; i++)
+		n += hweight32(info->config[i]);
+
+	return n;
+}
+
+static int hwmon_genattrs(struct device *dev,
+			  const void *drvdata,
+			  struct attribute **attrs,
+			  const struct hwmon_ops *ops,
+			  const struct hwmon_channel_info *info)
+{
+	const char * const *templates;
+	int template_size;
+	int i, aindex = 0;
+
+	if (info->type >= ARRAY_SIZE(__templates))
+		return -EINVAL;
+
+	templates = __templates[info->type];
+	template_size = __templates_size[info->type];
+
+	for (i = 0; info->config[i]; i++) {
+		u32 attr_mask = info->config[i];
+		u32 attr;
+
+		while (attr_mask) {
+			struct attribute *a;
+
+			attr = __ffs(attr_mask);
+			attr_mask &= ~BIT(attr);
+			if (attr >= template_size)
+				return -EINVAL;
+			a = hwmon_genattr(dev, drvdata, info->type, attr, i,
+					  templates[attr], ops);
+			if (IS_ERR(a)) {
+				if (PTR_ERR(a) != -ENOENT)
+					return PTR_ERR(a);
+				continue;
+			}
+			attrs[aindex++] = a;
+		}
+	}
+	return aindex;
+}
+
+static struct attribute **
+__hwmon_create_attrs(struct device *dev, const void *drvdata,
+		     const struct hwmon_chip_info *chip)
+{
+	int ret, i, aindex = 0, nattrs = 0;
+	struct attribute **attrs;
+
+	for (i = 0; chip->info[i]; i++)
+		nattrs += hwmon_num_channel_attrs(chip->info[i]);
+
+	if (nattrs == 0)
+		return ERR_PTR(-EINVAL);
+
+	attrs = devm_kcalloc(dev, nattrs + 1, sizeof(*attrs), GFP_KERNEL);
+	if (!attrs)
+		return ERR_PTR(-ENOMEM);
+
+	for (i = 0; chip->info[i]; i++) {
+		ret = hwmon_genattrs(dev, drvdata, &attrs[aindex], chip->ops,
+				     chip->info[i]);
+		if (ret < 0)
+			return ERR_PTR(ret);
+		aindex += ret;
+	}
+
+	return attrs;
+}
+
+static struct device *
+__hwmon_device_register(struct device *dev, const char *name, void *drvdata,
+			const struct hwmon_chip_info *chip,
+			const struct attribute_group **groups)
 {
 	struct hwmon_device *hwdev;
-	int err, id;
+	struct device *hdev;
+	int i, j, err, id;
 
 	/* Do not accept invalid characters in hwmon name attribute */
 	if (name && (!strlen(name) || strpbrk(name, "-* \t\n")))
@@ -114,28 +524,128 @@ hwmon_device_register_with_groups(struct device *dev, const char *name,
 		goto ida_remove;
 	}
 
+	hdev = &hwdev->dev;
+
+	if (chip && chip->ops->is_visible) {
+		struct attribute **attrs;
+		int ngroups = 2;
+
+		if (groups)
+			for (i = 0; groups[i]; i++)
+				ngroups++;
+
+		hwdev->groups = devm_kcalloc(dev, ngroups, sizeof(*groups),
+					     GFP_KERNEL);
+		if (!hwdev->groups)
+			return ERR_PTR(-ENOMEM);
+
+		attrs = __hwmon_create_attrs(dev, drvdata, chip);
+		if (IS_ERR(attrs)) {
+			err = PTR_ERR(attrs);
+			goto free_hwmon;
+		}
+
+		hwdev->group.attrs = attrs;
+		ngroups = 0;
+		hwdev->groups[ngroups++] = &hwdev->group;
+
+		if (groups) {
+			for (i = 0; groups[i]; i++)
+				hwdev->groups[ngroups++] = groups[i];
+		}
+
+		hdev->groups = hwdev->groups;
+	} else {
+		hdev->groups = groups;
+	}
+
 	hwdev->name = name;
-	hwdev->dev.class = &hwmon_class;
-	hwdev->dev.parent = dev;
-	hwdev->dev.groups = groups;
-	hwdev->dev.of_node = dev ? dev->of_node : NULL;
-	dev_set_drvdata(&hwdev->dev, drvdata);
-	dev_set_name(&hwdev->dev, HWMON_ID_FORMAT, id);
-	err = device_register(&hwdev->dev);
+	hdev->class = &hwmon_class;
+	hdev->parent = dev;
+	hdev->of_node = dev ? dev->of_node : NULL;
+	hwdev->chip = chip;
+	dev_set_drvdata(hdev, drvdata);
+	dev_set_name(hdev, HWMON_ID_FORMAT, id);
+	err = device_register(hdev);
 	if (err)
-		goto free;
+		goto free_hwmon;
+
+	if (chip && chip->ops->is_visible && chip->ops->read &&
+	    chip->info[0]->type == hwmon_chip &&
+	    (chip->info[0]->config[0] & HWMON_C_REGISTER_TZ)) {
+		const struct hwmon_channel_info **info = chip->info;
+
+		for (i = 1; info[i]; i++) {
+			if (info[i]->type != hwmon_temp)
+				continue;
+
+			for (j = 0; info[i]->config[j]; j++) {
+				if (!chip->ops->is_visible(drvdata, hwmon_temp,
+							   hwmon_temp_input, j))
+					continue;
+				if (info[i]->config[j] & HWMON_T_INPUT)
+					hwmon_thermal_add_sensor(dev, hwdev, j);
+			}
+		}
+	}
 
-	return &hwdev->dev;
+	return hdev;
 
-free:
+free_hwmon:
 	kfree(hwdev);
 ida_remove:
 	ida_simple_remove(&hwmon_ida, id);
 	return ERR_PTR(err);
 }
+
+/**
+ * hwmon_device_register_with_groups - register w/ hwmon
+ * @dev: the parent device
+ * @name: hwmon name attribute
+ * @drvdata: driver data to attach to created device
+ * @groups: List of attribute groups to create
+ *
+ * hwmon_device_unregister() must be called when the device is no
+ * longer needed.
+ *
+ * Returns the pointer to the new device.
+ */
+struct device *
+hwmon_device_register_with_groups(struct device *dev, const char *name,
+				  void *drvdata,
+				  const struct attribute_group **groups)
+{
+	return __hwmon_device_register(dev, name, drvdata, NULL, groups);
+}
 EXPORT_SYMBOL_GPL(hwmon_device_register_with_groups);
 
 /**
+ * hwmon_device_register_with_info - register w/ hwmon
+ * @dev: the parent device
+ * @name: hwmon name attribute
+ * @drvdata: driver data to attach to created device
+ * @info: Pointer to hwmon chip information
+ * @groups - pointer to list of driver specific attribute groups
+ *
+ * hwmon_device_unregister() must be called when the device is no
+ * longer needed.
+ *
+ * Returns the pointer to the new device.
+ */
+struct device *
+hwmon_device_register_with_info(struct device *dev, const char *name,
+				void *drvdata,
+				const struct hwmon_chip_info *chip,
+				const struct attribute_group **groups)
+{
+	if (chip && (!chip->ops || !chip->info))
+		return ERR_PTR(-EINVAL);
+
+	return __hwmon_device_register(dev, name, drvdata, chip, groups);
+}
+EXPORT_SYMBOL_GPL(hwmon_device_register_with_info);
+
+/**
  * hwmon_device_register - register w/ hwmon
  * @dev: the device to register
  *
@@ -213,6 +723,48 @@ error:
 }
 EXPORT_SYMBOL_GPL(devm_hwmon_device_register_with_groups);
 
+/**
+ * devm_hwmon_device_register_with_info - register w/ hwmon
+ * @dev: the parent device
+ * @name: hwmon name attribute
+ * @drvdata: driver data to attach to created device
+ * @info: Pointer to hwmon chip information
+ * @groups - pointer to list of driver specific attribute groups
+ *
+ * Returns the pointer to the new device. The new device is automatically
+ * unregistered with the parent device.
+ */
+struct device *
+devm_hwmon_device_register_with_info(struct device *dev, const char *name,
+				     void *drvdata,
+				     const struct hwmon_chip_info *chip,
+				     const struct attribute_group **groups)
+{
+	struct device **ptr, *hwdev;
+
+	if (!dev)
+		return ERR_PTR(-EINVAL);
+
+	ptr = devres_alloc(devm_hwmon_release, sizeof(*ptr), GFP_KERNEL);
+	if (!ptr)
+		return ERR_PTR(-ENOMEM);
+
+	hwdev = hwmon_device_register_with_info(dev, name, drvdata, chip,
+						groups);
+	if (IS_ERR(hwdev))
+		goto error;
+
+	*ptr = hwdev;
+	devres_add(dev, ptr);
+
+	return hwdev;
+
+error:
+	devres_free(ptr);
+	return hwdev;
+}
+EXPORT_SYMBOL_GPL(devm_hwmon_device_register_with_info);
+
 static int devm_hwmon_match(struct device *dev, void *res, void *data)
 {
 	struct device **hwdev = res;
diff --git a/drivers/hwmon/ibmpowernv.c b/drivers/hwmon/ibmpowernv.c
index 55b5a8ff1cfe..6d2e6605751c 100644
--- a/drivers/hwmon/ibmpowernv.c
+++ b/drivers/hwmon/ibmpowernv.c
@@ -143,13 +143,11 @@ static void __init make_sensor_label(struct device_node *np,
 		if (cpuid >= 0)
 			/*
 			 * The digital thermal sensors are associated
-			 * with a core. Let's print out the range of
-			 * cpu ids corresponding to the hardware
-			 * threads of the core.
+			 * with a core.
 			 */
 			n += snprintf(sdata->label + n,
-				      sizeof(sdata->label) - n, " %d-%d",
-				      cpuid, cpuid + threads_per_core - 1);
+				      sizeof(sdata->label) - n, " %d",
+				      cpuid);
 		else
 			n += snprintf(sdata->label + n,
 				      sizeof(sdata->label) - n, " phy%d", id);
diff --git a/drivers/hwmon/iio_hwmon.c b/drivers/hwmon/iio_hwmon.c
index 89449871bca7..f6a76679c650 100644
--- a/drivers/hwmon/iio_hwmon.c
+++ b/drivers/hwmon/iio_hwmon.c
@@ -73,8 +73,11 @@ static int iio_hwmon_probe(struct platform_device *pdev)
 		name = dev->of_node->name;
 
 	channels = iio_channel_get_all(dev);
-	if (IS_ERR(channels))
+	if (IS_ERR(channels)) {
+		if (PTR_ERR(channels) == -ENODEV)
+			return -EPROBE_DEFER;
 		return PTR_ERR(channels);
+	}
 
 	st = devm_kzalloc(dev, sizeof(*st), GFP_KERNEL);
 	if (st == NULL) {
diff --git a/drivers/hwmon/it87.c b/drivers/hwmon/it87.c
index 4667012b46b7..ad82cb28d87a 100644
--- a/drivers/hwmon/it87.c
+++ b/drivers/hwmon/it87.c
@@ -2011,10 +2011,10 @@ static struct attribute *it87_attributes_in[] = {
 	&sensor_dev_attr_in7_beep.dev_attr.attr,	/* 39 */
 
 	&sensor_dev_attr_in8_input.dev_attr.attr,	/* 40 */
-	&sensor_dev_attr_in9_input.dev_attr.attr,	/* 41 */
-	&sensor_dev_attr_in10_input.dev_attr.attr,	/* 41 */
-	&sensor_dev_attr_in11_input.dev_attr.attr,	/* 41 */
-	&sensor_dev_attr_in12_input.dev_attr.attr,	/* 41 */
+	&sensor_dev_attr_in9_input.dev_attr.attr,
+	&sensor_dev_attr_in10_input.dev_attr.attr,
+	&sensor_dev_attr_in11_input.dev_attr.attr,
+	&sensor_dev_attr_in12_input.dev_attr.attr,
 	NULL
 };
 
diff --git a/drivers/hwmon/jc42.c b/drivers/hwmon/jc42.c
index 9d5f85f3384f..1bf22eff0b08 100644
--- a/drivers/hwmon/jc42.c
+++ b/drivers/hwmon/jc42.c
@@ -28,7 +28,6 @@
 #include <linux/jiffies.h>
 #include <linux/i2c.h>
 #include <linux/hwmon.h>
-#include <linux/hwmon-sysfs.h>
 #include <linux/err.h>
 #include <linux/mutex.h>
 #include <linux/of.h>
@@ -254,170 +253,148 @@ abort:
 	return ret;
 }
 
-/* sysfs functions */
-
-static ssize_t show_temp(struct device *dev, struct device_attribute *devattr,
-			 char *buf)
-{
-	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
-	struct jc42_data *data = jc42_update_device(dev);
-	if (IS_ERR(data))
-		return PTR_ERR(data);
-	return sprintf(buf, "%d\n",
-		       jc42_temp_from_reg(data->temp[attr->index]));
-}
-
-static ssize_t show_temp_hyst(struct device *dev,
-			      struct device_attribute *devattr, char *buf)
+static int jc42_read(struct device *dev, enum hwmon_sensor_types type,
+		     u32 attr, int channel, long *val)
 {
-	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
 	struct jc42_data *data = jc42_update_device(dev);
 	int temp, hyst;
 
 	if (IS_ERR(data))
 		return PTR_ERR(data);
 
-	temp = jc42_temp_from_reg(data->temp[attr->index]);
-	hyst = jc42_hysteresis[(data->config & JC42_CFG_HYST_MASK)
-			       >> JC42_CFG_HYST_SHIFT];
-	return sprintf(buf, "%d\n", temp - hyst);
+	switch (attr) {
+	case hwmon_temp_input:
+		*val = jc42_temp_from_reg(data->temp[t_input]);
+		return 0;
+	case hwmon_temp_min:
+		*val = jc42_temp_from_reg(data->temp[t_min]);
+		return 0;
+	case hwmon_temp_max:
+		*val = jc42_temp_from_reg(data->temp[t_max]);
+		return 0;
+	case hwmon_temp_crit:
+		*val = jc42_temp_from_reg(data->temp[t_crit]);
+		return 0;
+	case hwmon_temp_max_hyst:
+		temp = jc42_temp_from_reg(data->temp[t_max]);
+		hyst = jc42_hysteresis[(data->config & JC42_CFG_HYST_MASK)
+						>> JC42_CFG_HYST_SHIFT];
+		*val = temp - hyst;
+		return 0;
+	case hwmon_temp_crit_hyst:
+		temp = jc42_temp_from_reg(data->temp[t_crit]);
+		hyst = jc42_hysteresis[(data->config & JC42_CFG_HYST_MASK)
+						>> JC42_CFG_HYST_SHIFT];
+		*val = temp - hyst;
+		return 0;
+	case hwmon_temp_min_alarm:
+		*val = (data->temp[t_input] >> JC42_ALARM_MIN_BIT) & 1;
+		return 0;
+	case hwmon_temp_max_alarm:
+		*val = (data->temp[t_input] >> JC42_ALARM_MAX_BIT) & 1;
+		return 0;
+	case hwmon_temp_crit_alarm:
+		*val = (data->temp[t_input] >> JC42_ALARM_CRIT_BIT) & 1;
+		return 0;
+	default:
+		return -EOPNOTSUPP;
+	}
 }
 
-static ssize_t set_temp(struct device *dev, struct device_attribute *devattr,
-			const char *buf, size_t count)
+static int jc42_write(struct device *dev, enum hwmon_sensor_types type,
+		      u32 attr, int channel, long val)
 {
-	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
 	struct jc42_data *data = dev_get_drvdata(dev);
-	int err, ret = count;
-	int nr = attr->index;
-	long val;
+	struct i2c_client *client = data->client;
+	int diff, hyst;
+	int ret;
 
-	if (kstrtol(buf, 10, &val) < 0)
-		return -EINVAL;
 	mutex_lock(&data->update_lock);
-	data->temp[nr] = jc42_temp_to_reg(val, data->extended);
-	err = i2c_smbus_write_word_swapped(data->client, temp_regs[nr],
-					   data->temp[nr]);
-	if (err < 0)
-		ret = err;
-	mutex_unlock(&data->update_lock);
-	return ret;
-}
 
-/*
- * JC42.4 compliant chips only support four hysteresis values.
- * Pick best choice and go from there.
- */
-static ssize_t set_temp_crit_hyst(struct device *dev,
-				  struct device_attribute *attr,
-				  const char *buf, size_t count)
-{
-	struct jc42_data *data = dev_get_drvdata(dev);
-	long val;
-	int diff, hyst;
-	int err;
-	int ret = count;
-
-	if (kstrtol(buf, 10, &val) < 0)
-		return -EINVAL;
-
-	val = clamp_val(val, (data->extended ? JC42_TEMP_MIN_EXTENDED :
-			      JC42_TEMP_MIN) - 6000, JC42_TEMP_MAX);
-	diff = jc42_temp_from_reg(data->temp[t_crit]) - val;
-
-	hyst = 0;
-	if (diff > 0) {
-		if (diff < 2250)
-			hyst = 1;	/* 1.5 degrees C */
-		else if (diff < 4500)
-			hyst = 2;	/* 3.0 degrees C */
-		else
-			hyst = 3;	/* 6.0 degrees C */
+	switch (attr) {
+	case hwmon_temp_min:
+		data->temp[t_min] = jc42_temp_to_reg(val, data->extended);
+		ret = i2c_smbus_write_word_swapped(client, temp_regs[t_min],
+						   data->temp[t_min]);
+		break;
+	case hwmon_temp_max:
+		data->temp[t_max] = jc42_temp_to_reg(val, data->extended);
+		ret = i2c_smbus_write_word_swapped(client, temp_regs[t_max],
+						   data->temp[t_max]);
+		break;
+	case hwmon_temp_crit:
+		data->temp[t_crit] = jc42_temp_to_reg(val, data->extended);
+		ret = i2c_smbus_write_word_swapped(client, temp_regs[t_crit],
+						   data->temp[t_crit]);
+		break;
+	case hwmon_temp_crit_hyst:
+		/*
+		 * JC42.4 compliant chips only support four hysteresis values.
+		 * Pick best choice and go from there.
+		 */
+		val = clamp_val(val, (data->extended ? JC42_TEMP_MIN_EXTENDED
+						     : JC42_TEMP_MIN) - 6000,
+				JC42_TEMP_MAX);
+		diff = jc42_temp_from_reg(data->temp[t_crit]) - val;
+		hyst = 0;
+		if (diff > 0) {
+			if (diff < 2250)
+				hyst = 1;	/* 1.5 degrees C */
+			else if (diff < 4500)
+				hyst = 2;	/* 3.0 degrees C */
+			else
+				hyst = 3;	/* 6.0 degrees C */
+		}
+		data->config = (data->config & ~JC42_CFG_HYST_MASK) |
+				(hyst << JC42_CFG_HYST_SHIFT);
+		ret = i2c_smbus_write_word_swapped(data->client,
+						   JC42_REG_CONFIG,
+						   data->config);
+		break;
+	default:
+		ret = -EOPNOTSUPP;
+		break;
 	}
 
-	mutex_lock(&data->update_lock);
-	data->config = (data->config & ~JC42_CFG_HYST_MASK)
-	  | (hyst << JC42_CFG_HYST_SHIFT);
-	err = i2c_smbus_write_word_swapped(data->client, JC42_REG_CONFIG,
-					   data->config);
-	if (err < 0)
-		ret = err;
 	mutex_unlock(&data->update_lock);
-	return ret;
-}
 
-static ssize_t show_alarm(struct device *dev,
-			  struct device_attribute *attr, char *buf)
-{
-	u16 bit = to_sensor_dev_attr(attr)->index;
-	struct jc42_data *data = jc42_update_device(dev);
-	u16 val;
-
-	if (IS_ERR(data))
-		return PTR_ERR(data);
-
-	val = data->temp[t_input];
-	if (bit != JC42_ALARM_CRIT_BIT && (data->config & JC42_CFG_CRIT_ONLY))
-		val = 0;
-	return sprintf(buf, "%u\n", (val >> bit) & 1);
+	return ret;
 }
 
-static SENSOR_DEVICE_ATTR(temp1_input, S_IRUGO, show_temp, NULL, t_input);
-static SENSOR_DEVICE_ATTR(temp1_crit, S_IRUGO, show_temp, set_temp, t_crit);
-static SENSOR_DEVICE_ATTR(temp1_min, S_IRUGO, show_temp, set_temp, t_min);
-static SENSOR_DEVICE_ATTR(temp1_max, S_IRUGO, show_temp, set_temp, t_max);
-
-static SENSOR_DEVICE_ATTR(temp1_crit_hyst, S_IRUGO, show_temp_hyst,
-			  set_temp_crit_hyst, t_crit);
-static SENSOR_DEVICE_ATTR(temp1_max_hyst, S_IRUGO, show_temp_hyst, NULL, t_max);
-
-static SENSOR_DEVICE_ATTR(temp1_crit_alarm, S_IRUGO, show_alarm, NULL,
-			  JC42_ALARM_CRIT_BIT);
-static SENSOR_DEVICE_ATTR(temp1_min_alarm, S_IRUGO, show_alarm, NULL,
-			  JC42_ALARM_MIN_BIT);
-static SENSOR_DEVICE_ATTR(temp1_max_alarm, S_IRUGO, show_alarm, NULL,
-			  JC42_ALARM_MAX_BIT);
-
-static struct attribute *jc42_attributes[] = {
-	&sensor_dev_attr_temp1_input.dev_attr.attr,
-	&sensor_dev_attr_temp1_crit.dev_attr.attr,
-	&sensor_dev_attr_temp1_min.dev_attr.attr,
-	&sensor_dev_attr_temp1_max.dev_attr.attr,
-	&sensor_dev_attr_temp1_crit_hyst.dev_attr.attr,
-	&sensor_dev_attr_temp1_max_hyst.dev_attr.attr,
-	&sensor_dev_attr_temp1_crit_alarm.dev_attr.attr,
-	&sensor_dev_attr_temp1_min_alarm.dev_attr.attr,
-	&sensor_dev_attr_temp1_max_alarm.dev_attr.attr,
-	NULL
-};
-
-static umode_t jc42_attribute_mode(struct kobject *kobj,
-				  struct attribute *attr, int index)
+static umode_t jc42_is_visible(const void *_data, enum hwmon_sensor_types type,
+			       u32 attr, int channel)
 {
-	struct device *dev = container_of(kobj, struct device, kobj);
-	struct jc42_data *data = dev_get_drvdata(dev);
+	const struct jc42_data *data = _data;
 	unsigned int config = data->config;
-	bool readonly;
-
-	if (attr == &sensor_dev_attr_temp1_crit.dev_attr.attr)
-		readonly = config & JC42_CFG_TCRIT_LOCK;
-	else if (attr == &sensor_dev_attr_temp1_min.dev_attr.attr ||
-		 attr == &sensor_dev_attr_temp1_max.dev_attr.attr)
-		readonly = config & JC42_CFG_EVENT_LOCK;
-	else if (attr == &sensor_dev_attr_temp1_crit_hyst.dev_attr.attr)
-		readonly = config & (JC42_CFG_EVENT_LOCK | JC42_CFG_TCRIT_LOCK);
-	else
-		readonly = true;
-
-	return S_IRUGO | (readonly ? 0 : S_IWUSR);
+	umode_t mode = S_IRUGO;
+
+	switch (attr) {
+	case hwmon_temp_min:
+	case hwmon_temp_max:
+		if (!(config & JC42_CFG_EVENT_LOCK))
+			mode |= S_IWUSR;
+		break;
+	case hwmon_temp_crit:
+		if (!(config & JC42_CFG_TCRIT_LOCK))
+			mode |= S_IWUSR;
+		break;
+	case hwmon_temp_crit_hyst:
+		if (!(config & (JC42_CFG_EVENT_LOCK | JC42_CFG_TCRIT_LOCK)))
+			mode |= S_IWUSR;
+		break;
+	case hwmon_temp_input:
+	case hwmon_temp_max_hyst:
+	case hwmon_temp_min_alarm:
+	case hwmon_temp_max_alarm:
+	case hwmon_temp_crit_alarm:
+		break;
+	default:
+		mode = 0;
+		break;
+	}
+	return mode;
 }
 
-static const struct attribute_group jc42_group = {
-	.attrs = jc42_attributes,
-	.is_visible = jc42_attribute_mode,
-};
-__ATTRIBUTE_GROUPS(jc42);
-
 /* Return 0 if detection is successful, -ENODEV otherwise */
 static int jc42_detect(struct i2c_client *client, struct i2c_board_info *info)
 {
@@ -450,6 +427,34 @@ static int jc42_detect(struct i2c_client *client, struct i2c_board_info *info)
 	return -ENODEV;
 }
 
+static const u32 jc42_temp_config[] = {
+	HWMON_T_INPUT | HWMON_T_MIN | HWMON_T_MAX | HWMON_T_CRIT |
+	HWMON_T_MAX_HYST | HWMON_T_CRIT_HYST |
+	HWMON_T_MIN_ALARM | HWMON_T_MAX_ALARM | HWMON_T_CRIT_ALARM,
+	0
+};
+
+static const struct hwmon_channel_info jc42_temp = {
+	.type = hwmon_temp,
+	.config = jc42_temp_config,
+};
+
+static const struct hwmon_channel_info *jc42_info[] = {
+	&jc42_temp,
+	NULL
+};
+
+static const struct hwmon_ops jc42_hwmon_ops = {
+	.is_visible = jc42_is_visible,
+	.read = jc42_read,
+	.write = jc42_write,
+};
+
+static const struct hwmon_chip_info jc42_chip_info = {
+	.ops = &jc42_hwmon_ops,
+	.info = jc42_info,
+};
+
 static int jc42_probe(struct i2c_client *client, const struct i2c_device_id *id)
 {
 	struct device *dev = &client->dev;
@@ -482,9 +487,9 @@ static int jc42_probe(struct i2c_client *client, const struct i2c_device_id *id)
 	}
 	data->config = config;
 
-	hwmon_dev = devm_hwmon_device_register_with_groups(dev, client->name,
-							   data,
-							   jc42_groups);
+	hwmon_dev = devm_hwmon_device_register_with_info(dev, client->name,
+							 data, &jc42_chip_info,
+							 NULL);
 	return PTR_ERR_OR_ZERO(hwmon_dev);
 }
 
diff --git a/drivers/hwmon/lm75.c b/drivers/hwmon/lm75.c
index 92f9d4bbf597..eff3b24d8473 100644
--- a/drivers/hwmon/lm75.c
+++ b/drivers/hwmon/lm75.c
@@ -28,7 +28,6 @@
 #include <linux/err.h>
 #include <linux/of.h>
 #include <linux/regmap.h>
-#include <linux/thermal.h>
 #include "lm75.h"
 
 
@@ -88,56 +87,75 @@ static inline long lm75_reg_to_mc(s16 temp, u8 resolution)
 	return ((temp >> (16 - resolution)) * 1000) >> (resolution - 8);
 }
 
-/* sysfs attributes for hwmon */
-
-static int lm75_read_temp(void *dev, int *temp)
+static int lm75_read(struct device *dev, enum hwmon_sensor_types type,
+		     u32 attr, int channel, long *val)
 {
 	struct lm75_data *data = dev_get_drvdata(dev);
-	unsigned int _temp;
-	int err;
-
-	err = regmap_read(data->regmap, LM75_REG_TEMP, &_temp);
-	if (err < 0)
-		return err;
-
-	*temp = lm75_reg_to_mc(_temp, data->resolution);
-
+	unsigned int regval;
+	int err, reg;
+
+	switch (type) {
+	case hwmon_chip:
+		switch (attr) {
+		case hwmon_chip_update_interval:
+			*val = data->sample_time;
+			break;;
+		default:
+			return -EINVAL;
+		}
+		break;
+	case hwmon_temp:
+		switch (attr) {
+		case hwmon_temp_input:
+			reg = LM75_REG_TEMP;
+			break;
+		case hwmon_temp_max:
+			reg = LM75_REG_MAX;
+			break;
+		case hwmon_temp_max_hyst:
+			reg = LM75_REG_HYST;
+			break;
+		default:
+			return -EINVAL;
+		}
+		err = regmap_read(data->regmap, reg, &regval);
+		if (err < 0)
+			return err;
+
+		*val = lm75_reg_to_mc(regval, data->resolution);
+		break;
+	default:
+		return -EINVAL;
+	}
 	return 0;
 }
 
-static ssize_t show_temp(struct device *dev, struct device_attribute *da,
-			 char *buf)
+static int lm75_write(struct device *dev, enum hwmon_sensor_types type,
+		      u32 attr, int channel, long temp)
 {
-	struct sensor_device_attribute *attr = to_sensor_dev_attr(da);
 	struct lm75_data *data = dev_get_drvdata(dev);
-	unsigned int temp = 0;
-	int err;
-
-	err = regmap_read(data->regmap, attr->index, &temp);
-	if (err < 0)
-		return err;
-
-	return sprintf(buf, "%ld\n", lm75_reg_to_mc(temp, data->resolution));
-}
-
-static ssize_t set_temp(struct device *dev, struct device_attribute *da,
-			const char *buf, size_t count)
-{
-	struct sensor_device_attribute *attr = to_sensor_dev_attr(da);
-	struct lm75_data *data = dev_get_drvdata(dev);
-	long temp;
-	int error;
 	u8 resolution;
+	int reg;
+
+	if (type != hwmon_temp)
+		return -EINVAL;
 
-	error = kstrtol(buf, 10, &temp);
-	if (error)
-		return error;
+	switch (attr) {
+	case hwmon_temp_max:
+		reg = LM75_REG_MAX;
+		break;
+	case hwmon_temp_max_hyst:
+		reg = LM75_REG_HYST;
+		break;
+	default:
+		return -EINVAL;
+	}
 
 	/*
 	 * Resolution of limit registers is assumed to be the same as the
 	 * temperature input register resolution unless given explicitly.
 	 */
-	if (attr->index && data->resolution_limits)
+	if (data->resolution_limits)
 		resolution = data->resolution_limits;
 	else
 		resolution = data->resolution;
@@ -145,45 +163,77 @@ static ssize_t set_temp(struct device *dev, struct device_attribute *da,
 	temp = clamp_val(temp, LM75_TEMP_MIN, LM75_TEMP_MAX);
 	temp = DIV_ROUND_CLOSEST(temp  << (resolution - 8),
 				 1000) << (16 - resolution);
-	error = regmap_write(data->regmap, attr->index, temp);
-	if (error < 0)
-		return error;
 
-	return count;
+	return regmap_write(data->regmap, reg, temp);
 }
 
-static ssize_t show_update_interval(struct device *dev,
-				    struct device_attribute *da, char *buf)
+static umode_t lm75_is_visible(const void *data, enum hwmon_sensor_types type,
+			       u32 attr, int channel)
 {
-	struct lm75_data *data = dev_get_drvdata(dev);
-
-	return sprintf(buf, "%u\n", data->sample_time);
+	switch (type) {
+	case hwmon_chip:
+		switch (attr) {
+		case hwmon_chip_update_interval:
+			return S_IRUGO;
+		}
+		break;
+	case hwmon_temp:
+		switch (attr) {
+		case hwmon_temp_input:
+			return S_IRUGO;
+		case hwmon_temp_max:
+		case hwmon_temp_max_hyst:
+			return S_IRUGO | S_IWUSR;
+		}
+		break;
+	default:
+		break;
+	}
+	return 0;
 }
 
-static SENSOR_DEVICE_ATTR(temp1_max, S_IWUSR | S_IRUGO,
-			show_temp, set_temp, LM75_REG_MAX);
-static SENSOR_DEVICE_ATTR(temp1_max_hyst, S_IWUSR | S_IRUGO,
-			show_temp, set_temp, LM75_REG_HYST);
-static SENSOR_DEVICE_ATTR(temp1_input, S_IRUGO, show_temp, NULL, LM75_REG_TEMP);
-static DEVICE_ATTR(update_interval, S_IRUGO, show_update_interval, NULL);
+/*-----------------------------------------------------------------------*/
 
-static struct attribute *lm75_attrs[] = {
-	&sensor_dev_attr_temp1_input.dev_attr.attr,
-	&sensor_dev_attr_temp1_max.dev_attr.attr,
-	&sensor_dev_attr_temp1_max_hyst.dev_attr.attr,
-	&dev_attr_update_interval.attr,
+/* device probe and removal */
 
-	NULL
+/* chip configuration */
+
+static const u32 lm75_chip_config[] = {
+	HWMON_C_REGISTER_TZ | HWMON_C_UPDATE_INTERVAL,
+	0
 };
-ATTRIBUTE_GROUPS(lm75);
 
-static const struct thermal_zone_of_device_ops lm75_of_thermal_ops = {
-	.get_temp = lm75_read_temp,
+static const struct hwmon_channel_info lm75_chip = {
+	.type = hwmon_chip,
+	.config = lm75_chip_config,
 };
 
-/*-----------------------------------------------------------------------*/
+static const u32 lm75_temp_config[] = {
+	HWMON_T_INPUT | HWMON_T_MAX | HWMON_T_MAX_HYST,
+	0
+};
 
-/* device probe and removal */
+static const struct hwmon_channel_info lm75_temp = {
+	.type = hwmon_temp,
+	.config = lm75_temp_config,
+};
+
+static const struct hwmon_channel_info *lm75_info[] = {
+	&lm75_chip,
+	&lm75_temp,
+	NULL
+};
+
+static const struct hwmon_ops lm75_hwmon_ops = {
+	.is_visible = lm75_is_visible,
+	.read = lm75_read,
+	.write = lm75_write,
+};
+
+static const struct hwmon_chip_info lm75_chip_info = {
+	.ops = &lm75_hwmon_ops,
+	.info = lm75_info,
+};
 
 static bool lm75_is_writeable_reg(struct device *dev, unsigned int reg)
 {
@@ -337,15 +387,12 @@ lm75_probe(struct i2c_client *client, const struct i2c_device_id *id)
 
 	dev_dbg(dev, "Config %02x\n", new);
 
-	hwmon_dev = devm_hwmon_device_register_with_groups(dev, client->name,
-							   data, lm75_groups);
+	hwmon_dev = devm_hwmon_device_register_with_info(dev, client->name,
+							 data, &lm75_chip_info,
+							 NULL);
 	if (IS_ERR(hwmon_dev))
 		return PTR_ERR(hwmon_dev);
 
-	devm_thermal_zone_of_sensor_register(hwmon_dev, 0,
-					     hwmon_dev,
-					     &lm75_of_thermal_ops);
-
 	dev_info(dev, "%s: sensor '%s'\n", dev_name(hwmon_dev), client->name);
 
 	return 0;
diff --git a/drivers/hwmon/lm90.c b/drivers/hwmon/lm90.c
index 496e771b363f..322ed9272811 100644
--- a/drivers/hwmon/lm90.c
+++ b/drivers/hwmon/lm90.c
@@ -89,7 +89,6 @@
 #include <linux/slab.h>
 #include <linux/jiffies.h>
 #include <linux/i2c.h>
-#include <linux/hwmon-sysfs.h>
 #include <linux/hwmon.h>
 #include <linux/err.h>
 #include <linux/mutex.h>
@@ -326,7 +325,7 @@ static const struct lm90_params lm90_params[] = {
 		.alert_alarms = 0x7c,
 		.max_convrate = 9,
 		.reg_local_ext = TMP451_REG_R_LOCAL_TEMPL,
-	}
+	},
 };
 
 /*
@@ -365,7 +364,10 @@ enum lm90_temp11_reg_index {
 
 struct lm90_data {
 	struct i2c_client *client;
-	const struct attribute_group *groups[6];
+	u32 channel_config[4];
+	struct hwmon_channel_info temp_info;
+	const struct hwmon_channel_info *info[3];
+	struct hwmon_chip_info chip;
 	struct mutex update_lock;
 	bool valid;		/* true if register values are valid */
 	unsigned long last_updated; /* in jiffies */
@@ -489,11 +491,11 @@ static inline int lm90_select_remote_channel(struct i2c_client *client,
  * client->update_lock must be held when calling this function (unless we are
  * in detection or initialization steps).
  */
-static void lm90_set_convrate(struct i2c_client *client, struct lm90_data *data,
-			      unsigned int interval)
+static int lm90_set_convrate(struct i2c_client *client, struct lm90_data *data,
+			     unsigned int interval)
 {
-	int i;
 	unsigned int update_interval;
+	int i, err;
 
 	/* Shift calculations to avoid rounding errors */
 	interval <<= 6;
@@ -504,8 +506,9 @@ static void lm90_set_convrate(struct i2c_client *client, struct lm90_data *data,
 		if (interval >= update_interval * 3 / 4)
 			break;
 
-	i2c_smbus_write_byte_data(client, LM90_REG_W_CONVRATE, i);
+	err = i2c_smbus_write_byte_data(client, LM90_REG_W_CONVRATE, i);
 	data->update_interval = DIV_ROUND_CLOSEST(update_interval, 64);
+	return err;
 }
 
 static int lm90_update_limits(struct device *dev)
@@ -604,19 +607,17 @@ static int lm90_update_limits(struct device *dev)
 	return 0;
 }
 
-static struct lm90_data *lm90_update_device(struct device *dev)
+static int lm90_update_device(struct device *dev)
 {
 	struct lm90_data *data = dev_get_drvdata(dev);
 	struct i2c_client *client = data->client;
 	unsigned long next_update;
-	int val = 0;
-
-	mutex_lock(&data->update_lock);
+	int val;
 
 	if (!data->valid) {
 		val = lm90_update_limits(dev);
 		if (val < 0)
-			goto error;
+			return val;
 	}
 
 	next_update = data->last_updated +
@@ -628,53 +629,55 @@ static struct lm90_data *lm90_update_device(struct device *dev)
 
 		val = lm90_read_reg(client, LM90_REG_R_LOCAL_LOW);
 		if (val < 0)
-			goto error;
+			return val;
 		data->temp8[LOCAL_LOW] = val;
 
 		val = lm90_read_reg(client, LM90_REG_R_LOCAL_HIGH);
 		if (val < 0)
-			goto error;
+			return val;
 		data->temp8[LOCAL_HIGH] = val;
 
 		if (data->reg_local_ext) {
 			val = lm90_read16(client, LM90_REG_R_LOCAL_TEMP,
 					  data->reg_local_ext);
 			if (val < 0)
-				goto error;
+				return val;
 			data->temp11[LOCAL_TEMP] = val;
 		} else {
 			val = lm90_read_reg(client, LM90_REG_R_LOCAL_TEMP);
 			if (val < 0)
-				goto error;
+				return val;
 			data->temp11[LOCAL_TEMP] = val << 8;
 		}
 		val = lm90_read16(client, LM90_REG_R_REMOTE_TEMPH,
 				  LM90_REG_R_REMOTE_TEMPL);
 		if (val < 0)
-			goto error;
+			return val;
 		data->temp11[REMOTE_TEMP] = val;
 
 		val = lm90_read_reg(client, LM90_REG_R_STATUS);
 		if (val < 0)
-			goto error;
+			return val;
 		data->alarms = val;	/* lower 8 bit of alarms */
 
 		if (data->kind == max6696) {
 			val = lm90_select_remote_channel(client, data, 1);
 			if (val < 0)
-				goto error;
+				return val;
 
 			val = lm90_read16(client, LM90_REG_R_REMOTE_TEMPH,
 					  LM90_REG_R_REMOTE_TEMPL);
-			if (val < 0)
-				goto error;
+			if (val < 0) {
+				lm90_select_remote_channel(client, data, 0);
+				return val;
+			}
 			data->temp11[REMOTE2_TEMP] = val;
 
 			lm90_select_remote_channel(client, data, 0);
 
 			val = lm90_read_reg(client, MAX6696_REG_R_STATUS2);
 			if (val < 0)
-				goto error;
+				return val;
 			data->alarms |= val << 8;
 		}
 
@@ -686,7 +689,7 @@ static struct lm90_data *lm90_update_device(struct device *dev)
 		    !(data->alarms & data->alert_alarms)) {
 			val = lm90_read_reg(client, LM90_REG_R_CONFIG1);
 			if (val < 0)
-				goto error;
+				return val;
 
 			if (val & 0x80) {
 				dev_dbg(&client->dev, "Re-enabling ALERT#\n");
@@ -700,13 +703,7 @@ static struct lm90_data *lm90_update_device(struct device *dev)
 		data->valid = true;
 	}
 
-error:
-	mutex_unlock(&data->update_lock);
-
-	if (val < 0)
-		return ERR_PTR(val);
-
-	return data;
+	return 0;
 }
 
 /*
@@ -832,52 +829,19 @@ static u16 temp_to_u16_adt7461(struct lm90_data *data, long val)
 	return (val + 125) / 250 * 64;
 }
 
-/*
- * Sysfs stuff
- */
-
-static ssize_t show_temp8(struct device *dev, struct device_attribute *devattr,
-			  char *buf)
+/* pec used for ADM1032 only */
+static ssize_t show_pec(struct device *dev, struct device_attribute *dummy,
+			char *buf)
 {
-	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
-	struct lm90_data *data = lm90_update_device(dev);
-	int temp;
-
-	if (IS_ERR(data))
-		return PTR_ERR(data);
-
-	if (data->kind == adt7461 || data->kind == tmp451)
-		temp = temp_from_u8_adt7461(data, data->temp8[attr->index]);
-	else if (data->kind == max6646)
-		temp = temp_from_u8(data->temp8[attr->index]);
-	else
-		temp = temp_from_s8(data->temp8[attr->index]);
-
-	/* +16 degrees offset for temp2 for the LM99 */
-	if (data->kind == lm99 && attr->index == 3)
-		temp += 16000;
+	struct i2c_client *client = to_i2c_client(dev);
 
-	return sprintf(buf, "%d\n", temp);
+	return sprintf(buf, "%d\n", !!(client->flags & I2C_CLIENT_PEC));
 }
 
-static ssize_t set_temp8(struct device *dev, struct device_attribute *devattr,
-			 const char *buf, size_t count)
+static ssize_t set_pec(struct device *dev, struct device_attribute *dummy,
+		       const char *buf, size_t count)
 {
-	static const u8 reg[TEMP8_REG_NUM] = {
-		LM90_REG_W_LOCAL_LOW,
-		LM90_REG_W_LOCAL_HIGH,
-		LM90_REG_W_LOCAL_CRIT,
-		LM90_REG_W_REMOTE_CRIT,
-		MAX6659_REG_W_LOCAL_EMERG,
-		MAX6659_REG_W_REMOTE_EMERG,
-		LM90_REG_W_REMOTE_CRIT,
-		MAX6659_REG_W_REMOTE_EMERG,
-	};
-
-	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
-	struct lm90_data *data = dev_get_drvdata(dev);
-	struct i2c_client *client = data->client;
-	int nr = attr->index;
+	struct i2c_client *client = to_i2c_client(dev);
 	long val;
 	int err;
 
@@ -885,82 +849,61 @@ static ssize_t set_temp8(struct device *dev, struct device_attribute *devattr,
 	if (err < 0)
 		return err;
 
-	/* +16 degrees offset for temp2 for the LM99 */
-	if (data->kind == lm99 && attr->index == 3)
-		val -= 16000;
-
-	mutex_lock(&data->update_lock);
-	if (data->kind == adt7461 || data->kind == tmp451)
-		data->temp8[nr] = temp_to_u8_adt7461(data, val);
-	else if (data->kind == max6646)
-		data->temp8[nr] = temp_to_u8(val);
-	else
-		data->temp8[nr] = temp_to_s8(val);
-
-	lm90_select_remote_channel(client, data, nr >= 6);
-	i2c_smbus_write_byte_data(client, reg[nr], data->temp8[nr]);
-	lm90_select_remote_channel(client, data, 0);
+	switch (val) {
+	case 0:
+		client->flags &= ~I2C_CLIENT_PEC;
+		break;
+	case 1:
+		client->flags |= I2C_CLIENT_PEC;
+		break;
+	default:
+		return -EINVAL;
+	}
 
-	mutex_unlock(&data->update_lock);
 	return count;
 }
 
-static ssize_t show_temp11(struct device *dev, struct device_attribute *devattr,
-			   char *buf)
+static DEVICE_ATTR(pec, S_IWUSR | S_IRUGO, show_pec, set_pec);
+
+static int lm90_get_temp11(struct lm90_data *data, int index)
 {
-	struct sensor_device_attribute_2 *attr = to_sensor_dev_attr_2(devattr);
-	struct lm90_data *data = lm90_update_device(dev);
+	s16 temp11 = data->temp11[index];
 	int temp;
 
-	if (IS_ERR(data))
-		return PTR_ERR(data);
-
 	if (data->kind == adt7461 || data->kind == tmp451)
-		temp = temp_from_u16_adt7461(data, data->temp11[attr->index]);
+		temp = temp_from_u16_adt7461(data, temp11);
 	else if (data->kind == max6646)
-		temp = temp_from_u16(data->temp11[attr->index]);
+		temp = temp_from_u16(temp11);
 	else
-		temp = temp_from_s16(data->temp11[attr->index]);
+		temp = temp_from_s16(temp11);
 
 	/* +16 degrees offset for temp2 for the LM99 */
-	if (data->kind == lm99 &&  attr->index <= 2)
+	if (data->kind == lm99 && index <= 2)
 		temp += 16000;
 
-	return sprintf(buf, "%d\n", temp);
+	return temp;
 }
 
-static ssize_t set_temp11(struct device *dev, struct device_attribute *devattr,
-			  const char *buf, size_t count)
+static int lm90_set_temp11(struct lm90_data *data, int index, long val)
 {
-	struct {
+	static struct reg {
 		u8 high;
 		u8 low;
-		int channel;
-	} reg[5] = {
-		{ LM90_REG_W_REMOTE_LOWH, LM90_REG_W_REMOTE_LOWL, 0 },
-		{ LM90_REG_W_REMOTE_HIGHH, LM90_REG_W_REMOTE_HIGHL, 0 },
-		{ LM90_REG_W_REMOTE_OFFSH, LM90_REG_W_REMOTE_OFFSL, 0 },
-		{ LM90_REG_W_REMOTE_LOWH, LM90_REG_W_REMOTE_LOWL, 1 },
-		{ LM90_REG_W_REMOTE_HIGHH, LM90_REG_W_REMOTE_HIGHL, 1 }
+	} reg[] = {
+	[REMOTE_LOW] = { LM90_REG_W_REMOTE_LOWH, LM90_REG_W_REMOTE_LOWL },
+	[REMOTE_HIGH] = { LM90_REG_W_REMOTE_HIGHH, LM90_REG_W_REMOTE_HIGHL },
+	[REMOTE_OFFSET] = { LM90_REG_W_REMOTE_OFFSH, LM90_REG_W_REMOTE_OFFSL },
+	[REMOTE2_LOW] = { LM90_REG_W_REMOTE_LOWH, LM90_REG_W_REMOTE_LOWL },
+	[REMOTE2_HIGH] = { LM90_REG_W_REMOTE_HIGHH, LM90_REG_W_REMOTE_HIGHL }
 	};
-
-	struct sensor_device_attribute_2 *attr = to_sensor_dev_attr_2(devattr);
-	struct lm90_data *data = dev_get_drvdata(dev);
 	struct i2c_client *client = data->client;
-	int nr = attr->nr;
-	int index = attr->index;
-	long val;
+	struct reg *regp = &reg[index];
 	int err;
 
-	err = kstrtol(buf, 10, &val);
-	if (err < 0)
-		return err;
-
 	/* +16 degrees offset for temp2 for the LM99 */
 	if (data->kind == lm99 && index <= 2)
 		val -= 16000;
 
-	mutex_lock(&data->update_lock);
 	if (data->kind == adt7461 || data->kind == tmp451)
 		data->temp11[index] = temp_to_u16_adt7461(data, val);
 	else if (data->kind == max6646)
@@ -970,317 +913,383 @@ static ssize_t set_temp11(struct device *dev, struct device_attribute *devattr,
 	else
 		data->temp11[index] = temp_to_s8(val) << 8;
 
-	lm90_select_remote_channel(client, data, reg[nr].channel);
-	i2c_smbus_write_byte_data(client, reg[nr].high,
+	lm90_select_remote_channel(client, data, index >= 3);
+	err = i2c_smbus_write_byte_data(client, regp->high,
 				  data->temp11[index] >> 8);
+	if (err < 0)
+		return err;
 	if (data->flags & LM90_HAVE_REM_LIMIT_EXT)
-		i2c_smbus_write_byte_data(client, reg[nr].low,
-					  data->temp11[index] & 0xff);
-	lm90_select_remote_channel(client, data, 0);
+		err = i2c_smbus_write_byte_data(client, regp->low,
+						data->temp11[index] & 0xff);
 
-	mutex_unlock(&data->update_lock);
-	return count;
+	lm90_select_remote_channel(client, data, 0);
+	return err;
 }
 
-static ssize_t show_temphyst(struct device *dev,
-			     struct device_attribute *devattr,
-			     char *buf)
+static int lm90_get_temp8(struct lm90_data *data, int index)
 {
-	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
-	struct lm90_data *data = lm90_update_device(dev);
+	s8 temp8 = data->temp8[index];
 	int temp;
 
-	if (IS_ERR(data))
-		return PTR_ERR(data);
-
 	if (data->kind == adt7461 || data->kind == tmp451)
-		temp = temp_from_u8_adt7461(data, data->temp8[attr->index]);
+		temp = temp_from_u8_adt7461(data, temp8);
 	else if (data->kind == max6646)
-		temp = temp_from_u8(data->temp8[attr->index]);
+		temp = temp_from_u8(temp8);
 	else
-		temp = temp_from_s8(data->temp8[attr->index]);
+		temp = temp_from_s8(temp8);
 
 	/* +16 degrees offset for temp2 for the LM99 */
-	if (data->kind == lm99 && attr->index == 3)
+	if (data->kind == lm99 && index == 3)
 		temp += 16000;
 
-	return sprintf(buf, "%d\n", temp - temp_from_s8(data->temp_hyst));
+	return temp;
 }
 
-static ssize_t set_temphyst(struct device *dev, struct device_attribute *dummy,
-			    const char *buf, size_t count)
+static int lm90_set_temp8(struct lm90_data *data, int index, long val)
 {
-	struct lm90_data *data = dev_get_drvdata(dev);
+	static const u8 reg[TEMP8_REG_NUM] = {
+		LM90_REG_W_LOCAL_LOW,
+		LM90_REG_W_LOCAL_HIGH,
+		LM90_REG_W_LOCAL_CRIT,
+		LM90_REG_W_REMOTE_CRIT,
+		MAX6659_REG_W_LOCAL_EMERG,
+		MAX6659_REG_W_REMOTE_EMERG,
+		LM90_REG_W_REMOTE_CRIT,
+		MAX6659_REG_W_REMOTE_EMERG,
+	};
 	struct i2c_client *client = data->client;
-	long val;
 	int err;
-	int temp;
 
-	err = kstrtol(buf, 10, &val);
-	if (err < 0)
-		return err;
+	/* +16 degrees offset for temp2 for the LM99 */
+	if (data->kind == lm99 && index == 3)
+		val -= 16000;
 
-	mutex_lock(&data->update_lock);
 	if (data->kind == adt7461 || data->kind == tmp451)
-		temp = temp_from_u8_adt7461(data, data->temp8[LOCAL_CRIT]);
+		data->temp8[index] = temp_to_u8_adt7461(data, val);
 	else if (data->kind == max6646)
-		temp = temp_from_u8(data->temp8[LOCAL_CRIT]);
+		data->temp8[index] = temp_to_u8(val);
 	else
-		temp = temp_from_s8(data->temp8[LOCAL_CRIT]);
+		data->temp8[index] = temp_to_s8(val);
 
-	data->temp_hyst = hyst_to_reg(temp - val);
-	i2c_smbus_write_byte_data(client, LM90_REG_W_TCRIT_HYST,
-				  data->temp_hyst);
-	mutex_unlock(&data->update_lock);
-	return count;
-}
-
-static ssize_t show_alarms(struct device *dev, struct device_attribute *dummy,
-			   char *buf)
-{
-	struct lm90_data *data = lm90_update_device(dev);
-
-	if (IS_ERR(data))
-		return PTR_ERR(data);
+	lm90_select_remote_channel(client, data, index >= 6);
+	err = i2c_smbus_write_byte_data(client, reg[index], data->temp8[index]);
+	lm90_select_remote_channel(client, data, 0);
 
-	return sprintf(buf, "%d\n", data->alarms);
+	return err;
 }
 
-static ssize_t show_alarm(struct device *dev, struct device_attribute
-			  *devattr, char *buf)
+static int lm90_get_temphyst(struct lm90_data *data, int index)
 {
-	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
-	struct lm90_data *data = lm90_update_device(dev);
-	int bitnr = attr->index;
-
-	if (IS_ERR(data))
-		return PTR_ERR(data);
+	int temp;
 
-	return sprintf(buf, "%d\n", (data->alarms >> bitnr) & 1);
-}
+	if (data->kind == adt7461 || data->kind == tmp451)
+		temp = temp_from_u8_adt7461(data, data->temp8[index]);
+	else if (data->kind == max6646)
+		temp = temp_from_u8(data->temp8[index]);
+	else
+		temp = temp_from_s8(data->temp8[index]);
 
-static ssize_t show_update_interval(struct device *dev,
-				    struct device_attribute *attr, char *buf)
-{
-	struct lm90_data *data = dev_get_drvdata(dev);
+	/* +16 degrees offset for temp2 for the LM99 */
+	if (data->kind == lm99 && index == 3)
+		temp += 16000;
 
-	return sprintf(buf, "%u\n", data->update_interval);
+	return temp - temp_from_s8(data->temp_hyst);
 }
 
-static ssize_t set_update_interval(struct device *dev,
-				   struct device_attribute *attr,
-				   const char *buf, size_t count)
+static int lm90_set_temphyst(struct lm90_data *data, long val)
 {
-	struct lm90_data *data = dev_get_drvdata(dev);
 	struct i2c_client *client = data->client;
-	unsigned long val;
+	int temp;
 	int err;
 
-	err = kstrtoul(buf, 10, &val);
-	if (err)
-		return err;
-
-	mutex_lock(&data->update_lock);
-	lm90_set_convrate(client, data, clamp_val(val, 0, 100000));
-	mutex_unlock(&data->update_lock);
+	if (data->kind == adt7461 || data->kind == tmp451)
+		temp = temp_from_u8_adt7461(data, data->temp8[LOCAL_CRIT]);
+	else if (data->kind == max6646)
+		temp = temp_from_u8(data->temp8[LOCAL_CRIT]);
+	else
+		temp = temp_from_s8(data->temp8[LOCAL_CRIT]);
 
-	return count;
+	data->temp_hyst = hyst_to_reg(temp - val);
+	err = i2c_smbus_write_byte_data(client, LM90_REG_W_TCRIT_HYST,
+					data->temp_hyst);
+	return err;
 }
 
-static SENSOR_DEVICE_ATTR_2(temp1_input, S_IRUGO, show_temp11, NULL,
-	0, LOCAL_TEMP);
-static SENSOR_DEVICE_ATTR_2(temp2_input, S_IRUGO, show_temp11, NULL,
-	0, REMOTE_TEMP);
-static SENSOR_DEVICE_ATTR(temp1_min, S_IWUSR | S_IRUGO, show_temp8,
-	set_temp8, LOCAL_LOW);
-static SENSOR_DEVICE_ATTR_2(temp2_min, S_IWUSR | S_IRUGO, show_temp11,
-	set_temp11, 0, REMOTE_LOW);
-static SENSOR_DEVICE_ATTR(temp1_max, S_IWUSR | S_IRUGO, show_temp8,
-	set_temp8, LOCAL_HIGH);
-static SENSOR_DEVICE_ATTR_2(temp2_max, S_IWUSR | S_IRUGO, show_temp11,
-	set_temp11, 1, REMOTE_HIGH);
-static SENSOR_DEVICE_ATTR(temp1_crit, S_IWUSR | S_IRUGO, show_temp8,
-	set_temp8, LOCAL_CRIT);
-static SENSOR_DEVICE_ATTR(temp2_crit, S_IWUSR | S_IRUGO, show_temp8,
-	set_temp8, REMOTE_CRIT);
-static SENSOR_DEVICE_ATTR(temp1_crit_hyst, S_IWUSR | S_IRUGO, show_temphyst,
-	set_temphyst, LOCAL_CRIT);
-static SENSOR_DEVICE_ATTR(temp2_crit_hyst, S_IRUGO, show_temphyst, NULL,
-	REMOTE_CRIT);
-static SENSOR_DEVICE_ATTR_2(temp2_offset, S_IWUSR | S_IRUGO, show_temp11,
-	set_temp11, 2, REMOTE_OFFSET);
-
-/* Individual alarm files */
-static SENSOR_DEVICE_ATTR(temp1_crit_alarm, S_IRUGO, show_alarm, NULL, 0);
-static SENSOR_DEVICE_ATTR(temp2_crit_alarm, S_IRUGO, show_alarm, NULL, 1);
-static SENSOR_DEVICE_ATTR(temp2_fault, S_IRUGO, show_alarm, NULL, 2);
-static SENSOR_DEVICE_ATTR(temp2_min_alarm, S_IRUGO, show_alarm, NULL, 3);
-static SENSOR_DEVICE_ATTR(temp2_max_alarm, S_IRUGO, show_alarm, NULL, 4);
-static SENSOR_DEVICE_ATTR(temp1_min_alarm, S_IRUGO, show_alarm, NULL, 5);
-static SENSOR_DEVICE_ATTR(temp1_max_alarm, S_IRUGO, show_alarm, NULL, 6);
-/* Raw alarm file for compatibility */
-static DEVICE_ATTR(alarms, S_IRUGO, show_alarms, NULL);
-
-static DEVICE_ATTR(update_interval, S_IRUGO | S_IWUSR, show_update_interval,
-		   set_update_interval);
-
-static struct attribute *lm90_attributes[] = {
-	&sensor_dev_attr_temp1_input.dev_attr.attr,
-	&sensor_dev_attr_temp2_input.dev_attr.attr,
-	&sensor_dev_attr_temp1_min.dev_attr.attr,
-	&sensor_dev_attr_temp2_min.dev_attr.attr,
-	&sensor_dev_attr_temp1_max.dev_attr.attr,
-	&sensor_dev_attr_temp2_max.dev_attr.attr,
-	&sensor_dev_attr_temp1_crit.dev_attr.attr,
-	&sensor_dev_attr_temp2_crit.dev_attr.attr,
-	&sensor_dev_attr_temp1_crit_hyst.dev_attr.attr,
-	&sensor_dev_attr_temp2_crit_hyst.dev_attr.attr,
-
-	&sensor_dev_attr_temp1_crit_alarm.dev_attr.attr,
-	&sensor_dev_attr_temp2_crit_alarm.dev_attr.attr,
-	&sensor_dev_attr_temp2_fault.dev_attr.attr,
-	&sensor_dev_attr_temp2_min_alarm.dev_attr.attr,
-	&sensor_dev_attr_temp2_max_alarm.dev_attr.attr,
-	&sensor_dev_attr_temp1_min_alarm.dev_attr.attr,
-	&sensor_dev_attr_temp1_max_alarm.dev_attr.attr,
-	&dev_attr_alarms.attr,
-	&dev_attr_update_interval.attr,
-	NULL
+static const u8 lm90_temp_index[3] = {
+	LOCAL_TEMP, REMOTE_TEMP, REMOTE2_TEMP
 };
 
-static const struct attribute_group lm90_group = {
-	.attrs = lm90_attributes,
+static const u8 lm90_temp_min_index[3] = {
+	LOCAL_LOW, REMOTE_LOW, REMOTE2_LOW
 };
 
-static struct attribute *lm90_temp2_offset_attributes[] = {
-	&sensor_dev_attr_temp2_offset.dev_attr.attr,
-	NULL
+static const u8 lm90_temp_max_index[3] = {
+	LOCAL_HIGH, REMOTE_HIGH, REMOTE2_HIGH
 };
 
-static const struct attribute_group lm90_temp2_offset_group = {
-	.attrs = lm90_temp2_offset_attributes,
+static const u8 lm90_temp_crit_index[3] = {
+	LOCAL_CRIT, REMOTE_CRIT, REMOTE2_CRIT
 };
 
-/*
- * Additional attributes for devices with emergency sensors
- */
-static SENSOR_DEVICE_ATTR(temp1_emergency, S_IWUSR | S_IRUGO, show_temp8,
-	set_temp8, LOCAL_EMERG);
-static SENSOR_DEVICE_ATTR(temp2_emergency, S_IWUSR | S_IRUGO, show_temp8,
-	set_temp8, REMOTE_EMERG);
-static SENSOR_DEVICE_ATTR(temp1_emergency_hyst, S_IRUGO, show_temphyst,
-			  NULL, LOCAL_EMERG);
-static SENSOR_DEVICE_ATTR(temp2_emergency_hyst, S_IRUGO, show_temphyst,
-			  NULL, REMOTE_EMERG);
-
-static struct attribute *lm90_emergency_attributes[] = {
-	&sensor_dev_attr_temp1_emergency.dev_attr.attr,
-	&sensor_dev_attr_temp2_emergency.dev_attr.attr,
-	&sensor_dev_attr_temp1_emergency_hyst.dev_attr.attr,
-	&sensor_dev_attr_temp2_emergency_hyst.dev_attr.attr,
-	NULL
+static const u8 lm90_temp_emerg_index[3] = {
+	LOCAL_EMERG, REMOTE_EMERG, REMOTE2_EMERG
 };
 
-static const struct attribute_group lm90_emergency_group = {
-	.attrs = lm90_emergency_attributes,
-};
+static const u8 lm90_min_alarm_bits[3] = { 5, 3, 11 };
+static const u8 lm90_max_alarm_bits[3] = { 0, 4, 12 };
+static const u8 lm90_crit_alarm_bits[3] = { 0, 1, 9 };
+static const u8 lm90_emergency_alarm_bits[3] = { 15, 13, 14 };
+static const u8 lm90_fault_bits[3] = { 0, 2, 10 };
 
-static SENSOR_DEVICE_ATTR(temp1_emergency_alarm, S_IRUGO, show_alarm, NULL, 15);
-static SENSOR_DEVICE_ATTR(temp2_emergency_alarm, S_IRUGO, show_alarm, NULL, 13);
+static int lm90_temp_read(struct device *dev, u32 attr, int channel, long *val)
+{
+	struct lm90_data *data = dev_get_drvdata(dev);
+	int err;
 
-static struct attribute *lm90_emergency_alarm_attributes[] = {
-	&sensor_dev_attr_temp1_emergency_alarm.dev_attr.attr,
-	&sensor_dev_attr_temp2_emergency_alarm.dev_attr.attr,
-	NULL
-};
+	mutex_lock(&data->update_lock);
+	err = lm90_update_device(dev);
+	mutex_unlock(&data->update_lock);
+	if (err)
+		return err;
 
-static const struct attribute_group lm90_emergency_alarm_group = {
-	.attrs = lm90_emergency_alarm_attributes,
-};
+	switch (attr) {
+	case hwmon_temp_input:
+		*val = lm90_get_temp11(data, lm90_temp_index[channel]);
+		break;
+	case hwmon_temp_min_alarm:
+		*val = (data->alarms >> lm90_min_alarm_bits[channel]) & 1;
+		break;
+	case hwmon_temp_max_alarm:
+		*val = (data->alarms >> lm90_max_alarm_bits[channel]) & 1;
+		break;
+	case hwmon_temp_crit_alarm:
+		*val = (data->alarms >> lm90_crit_alarm_bits[channel]) & 1;
+		break;
+	case hwmon_temp_emergency_alarm:
+		*val = (data->alarms >> lm90_emergency_alarm_bits[channel]) & 1;
+		break;
+	case hwmon_temp_fault:
+		*val = (data->alarms >> lm90_fault_bits[channel]) & 1;
+		break;
+	case hwmon_temp_min:
+		if (channel == 0)
+			*val = lm90_get_temp8(data,
+					      lm90_temp_min_index[channel]);
+		else
+			*val = lm90_get_temp11(data,
+					       lm90_temp_min_index[channel]);
+		break;
+	case hwmon_temp_max:
+		if (channel == 0)
+			*val = lm90_get_temp8(data,
+					      lm90_temp_max_index[channel]);
+		else
+			*val = lm90_get_temp11(data,
+					       lm90_temp_max_index[channel]);
+		break;
+	case hwmon_temp_crit:
+		*val = lm90_get_temp8(data, lm90_temp_crit_index[channel]);
+		break;
+	case hwmon_temp_crit_hyst:
+		*val = lm90_get_temphyst(data, lm90_temp_crit_index[channel]);
+		break;
+	case hwmon_temp_emergency:
+		*val = lm90_get_temp8(data, lm90_temp_emerg_index[channel]);
+		break;
+	case hwmon_temp_emergency_hyst:
+		*val = lm90_get_temphyst(data, lm90_temp_emerg_index[channel]);
+		break;
+	case hwmon_temp_offset:
+		*val = lm90_get_temp11(data, REMOTE_OFFSET);
+		break;
+	default:
+		return -EOPNOTSUPP;
+	}
+	return 0;
+}
 
-/*
- * Additional attributes for devices with 3 temperature sensors
- */
-static SENSOR_DEVICE_ATTR_2(temp3_input, S_IRUGO, show_temp11, NULL,
-	0, REMOTE2_TEMP);
-static SENSOR_DEVICE_ATTR_2(temp3_min, S_IWUSR | S_IRUGO, show_temp11,
-	set_temp11, 3, REMOTE2_LOW);
-static SENSOR_DEVICE_ATTR_2(temp3_max, S_IWUSR | S_IRUGO, show_temp11,
-	set_temp11, 4, REMOTE2_HIGH);
-static SENSOR_DEVICE_ATTR(temp3_crit, S_IWUSR | S_IRUGO, show_temp8,
-	set_temp8, REMOTE2_CRIT);
-static SENSOR_DEVICE_ATTR(temp3_crit_hyst, S_IRUGO, show_temphyst, NULL,
-	REMOTE2_CRIT);
-static SENSOR_DEVICE_ATTR(temp3_emergency, S_IWUSR | S_IRUGO, show_temp8,
-	set_temp8, REMOTE2_EMERG);
-static SENSOR_DEVICE_ATTR(temp3_emergency_hyst, S_IRUGO, show_temphyst,
-			  NULL, REMOTE2_EMERG);
-
-static SENSOR_DEVICE_ATTR(temp3_crit_alarm, S_IRUGO, show_alarm, NULL, 9);
-static SENSOR_DEVICE_ATTR(temp3_fault, S_IRUGO, show_alarm, NULL, 10);
-static SENSOR_DEVICE_ATTR(temp3_min_alarm, S_IRUGO, show_alarm, NULL, 11);
-static SENSOR_DEVICE_ATTR(temp3_max_alarm, S_IRUGO, show_alarm, NULL, 12);
-static SENSOR_DEVICE_ATTR(temp3_emergency_alarm, S_IRUGO, show_alarm, NULL, 14);
-
-static struct attribute *lm90_temp3_attributes[] = {
-	&sensor_dev_attr_temp3_input.dev_attr.attr,
-	&sensor_dev_attr_temp3_min.dev_attr.attr,
-	&sensor_dev_attr_temp3_max.dev_attr.attr,
-	&sensor_dev_attr_temp3_crit.dev_attr.attr,
-	&sensor_dev_attr_temp3_crit_hyst.dev_attr.attr,
-	&sensor_dev_attr_temp3_emergency.dev_attr.attr,
-	&sensor_dev_attr_temp3_emergency_hyst.dev_attr.attr,
-
-	&sensor_dev_attr_temp3_fault.dev_attr.attr,
-	&sensor_dev_attr_temp3_min_alarm.dev_attr.attr,
-	&sensor_dev_attr_temp3_max_alarm.dev_attr.attr,
-	&sensor_dev_attr_temp3_crit_alarm.dev_attr.attr,
-	&sensor_dev_attr_temp3_emergency_alarm.dev_attr.attr,
-	NULL
-};
+static int lm90_temp_write(struct device *dev, u32 attr, int channel, long val)
+{
+	struct lm90_data *data = dev_get_drvdata(dev);
+	int err;
 
-static const struct attribute_group lm90_temp3_group = {
-	.attrs = lm90_temp3_attributes,
-};
+	mutex_lock(&data->update_lock);
 
-/* pec used for ADM1032 only */
-static ssize_t show_pec(struct device *dev, struct device_attribute *dummy,
-			char *buf)
+	err = lm90_update_device(dev);
+	if (err)
+		goto error;
+
+	switch (attr) {
+	case hwmon_temp_min:
+		if (channel == 0)
+			err = lm90_set_temp8(data,
+					      lm90_temp_min_index[channel],
+					      val);
+		else
+			err = lm90_set_temp11(data,
+					      lm90_temp_min_index[channel],
+					      val);
+		break;
+	case hwmon_temp_max:
+		if (channel == 0)
+			err = lm90_set_temp8(data,
+					     lm90_temp_max_index[channel],
+					     val);
+		else
+			err = lm90_set_temp11(data,
+					      lm90_temp_max_index[channel],
+					      val);
+		break;
+	case hwmon_temp_crit:
+		err = lm90_set_temp8(data, lm90_temp_crit_index[channel], val);
+		break;
+	case hwmon_temp_crit_hyst:
+		err = lm90_set_temphyst(data, val);
+		break;
+	case hwmon_temp_emergency:
+		err = lm90_set_temp8(data, lm90_temp_emerg_index[channel], val);
+		break;
+	case hwmon_temp_offset:
+		err = lm90_set_temp11(data, REMOTE_OFFSET, val);
+		break;
+	default:
+		err = -EOPNOTSUPP;
+		break;
+	}
+error:
+	mutex_unlock(&data->update_lock);
+
+	return err;
+}
+
+static umode_t lm90_temp_is_visible(const void *data, u32 attr, int channel)
 {
-	struct i2c_client *client = to_i2c_client(dev);
-	return sprintf(buf, "%d\n", !!(client->flags & I2C_CLIENT_PEC));
+	switch (attr) {
+	case hwmon_temp_input:
+	case hwmon_temp_min_alarm:
+	case hwmon_temp_max_alarm:
+	case hwmon_temp_crit_alarm:
+	case hwmon_temp_emergency_alarm:
+	case hwmon_temp_emergency_hyst:
+	case hwmon_temp_fault:
+		return S_IRUGO;
+	case hwmon_temp_min:
+	case hwmon_temp_max:
+	case hwmon_temp_crit:
+	case hwmon_temp_emergency:
+	case hwmon_temp_offset:
+		return S_IRUGO | S_IWUSR;
+	case hwmon_temp_crit_hyst:
+		if (channel == 0)
+			return S_IRUGO | S_IWUSR;
+		return S_IRUGO;
+	default:
+		return 0;
+	}
 }
 
-static ssize_t set_pec(struct device *dev, struct device_attribute *dummy,
-		       const char *buf, size_t count)
+static int lm90_chip_read(struct device *dev, u32 attr, int channel, long *val)
 {
-	struct i2c_client *client = to_i2c_client(dev);
-	long val;
+	struct lm90_data *data = dev_get_drvdata(dev);
 	int err;
 
-	err = kstrtol(buf, 10, &val);
-	if (err < 0)
+	mutex_lock(&data->update_lock);
+	err = lm90_update_device(dev);
+	mutex_unlock(&data->update_lock);
+	if (err)
 		return err;
 
-	switch (val) {
-	case 0:
-		client->flags &= ~I2C_CLIENT_PEC;
+	switch (attr) {
+	case hwmon_chip_update_interval:
+		*val = data->update_interval;
 		break;
-	case 1:
-		client->flags |= I2C_CLIENT_PEC;
+	case hwmon_chip_alarms:
+		*val = data->alarms;
 		break;
 	default:
-		return -EINVAL;
+		return -EOPNOTSUPP;
 	}
 
-	return count;
+	return 0;
 }
 
-static DEVICE_ATTR(pec, S_IWUSR | S_IRUGO, show_pec, set_pec);
+static int lm90_chip_write(struct device *dev, u32 attr, int channel, long val)
+{
+	struct lm90_data *data = dev_get_drvdata(dev);
+	struct i2c_client *client = data->client;
+	int err;
 
-/*
- * Real code
- */
+	mutex_lock(&data->update_lock);
+
+	err = lm90_update_device(dev);
+	if (err)
+		goto error;
+
+	switch (attr) {
+	case hwmon_chip_update_interval:
+		err = lm90_set_convrate(client, data,
+					clamp_val(val, 0, 100000));
+		break;
+	default:
+		err = -EOPNOTSUPP;
+		break;
+	}
+error:
+	mutex_unlock(&data->update_lock);
+
+	return err;
+}
+
+static umode_t lm90_chip_is_visible(const void *data, u32 attr, int channel)
+{
+	switch (attr) {
+	case hwmon_chip_update_interval:
+		return S_IRUGO | S_IWUSR;
+	case hwmon_chip_alarms:
+		return S_IRUGO;
+	default:
+		return 0;
+	}
+}
+
+static int lm90_read(struct device *dev, enum hwmon_sensor_types type,
+		     u32 attr, int channel, long *val)
+{
+	switch (type) {
+	case hwmon_chip:
+		return lm90_chip_read(dev, attr, channel, val);
+	case hwmon_temp:
+		return lm90_temp_read(dev, attr, channel, val);
+	default:
+		return -EOPNOTSUPP;
+	}
+}
+
+static int lm90_write(struct device *dev, enum hwmon_sensor_types type,
+		      u32 attr, int channel, long val)
+{
+	switch (type) {
+	case hwmon_chip:
+		return lm90_chip_write(dev, attr, channel, val);
+	case hwmon_temp:
+		return lm90_temp_write(dev, attr, channel, val);
+	default:
+		return -EOPNOTSUPP;
+	}
+}
+
+static umode_t lm90_is_visible(const void *data, enum hwmon_sensor_types type,
+			       u32 attr, int channel)
+{
+	switch (type) {
+	case hwmon_chip:
+		return lm90_chip_is_visible(data, attr, channel);
+	case hwmon_temp:
+		return lm90_temp_is_visible(data, attr, channel);
+	default:
+		return 0;
+	}
+}
 
 /* Return 0 if detection is successful, -ENODEV otherwise */
 static int lm90_detect(struct i2c_client *client,
@@ -1617,15 +1626,32 @@ static void lm90_regulator_disable(void *regulator)
 	regulator_disable(regulator);
 }
 
+static const u32 lm90_chip_config[] = {
+	HWMON_C_REGISTER_TZ | HWMON_C_UPDATE_INTERVAL | HWMON_C_ALARMS,
+	0
+};
+
+static const struct hwmon_channel_info lm90_chip_info = {
+	.type = hwmon_chip,
+	.config = lm90_chip_config,
+};
+
+
+static const struct hwmon_ops lm90_ops = {
+	.is_visible = lm90_is_visible,
+	.read = lm90_read,
+	.write = lm90_write,
+};
+
 static int lm90_probe(struct i2c_client *client,
 		      const struct i2c_device_id *id)
 {
 	struct device *dev = &client->dev;
 	struct i2c_adapter *adapter = to_i2c_adapter(dev->parent);
-	struct lm90_data *data;
+	struct hwmon_channel_info *info;
 	struct regulator *regulator;
 	struct device *hwmon_dev;
-	int groups = 0;
+	struct lm90_data *data;
 	int err;
 
 	regulator = devm_regulator_get(dev, "vcc");
@@ -1665,6 +1691,49 @@ static int lm90_probe(struct i2c_client *client,
 
 	/* Set chip capabilities */
 	data->flags = lm90_params[data->kind].flags;
+
+	data->chip.ops = &lm90_ops;
+	data->chip.info = data->info;
+
+	data->info[0] = &lm90_chip_info;
+	data->info[1] = &data->temp_info;
+
+	info = &data->temp_info;
+	info->type = hwmon_temp;
+	info->config = data->channel_config;
+
+	data->channel_config[0] = HWMON_T_INPUT | HWMON_T_MIN | HWMON_T_MAX |
+		HWMON_T_CRIT | HWMON_T_CRIT_HYST | HWMON_T_MIN_ALARM |
+		HWMON_T_MAX_ALARM | HWMON_T_CRIT_ALARM;
+	data->channel_config[1] = HWMON_T_INPUT | HWMON_T_MIN | HWMON_T_MAX |
+		HWMON_T_CRIT | HWMON_T_CRIT_HYST | HWMON_T_MIN_ALARM |
+		HWMON_T_MAX_ALARM | HWMON_T_CRIT_ALARM | HWMON_T_FAULT;
+
+	if (data->flags & LM90_HAVE_OFFSET)
+		data->channel_config[1] |= HWMON_T_OFFSET;
+
+	if (data->flags & LM90_HAVE_EMERGENCY) {
+		data->channel_config[0] |= HWMON_T_EMERGENCY |
+			HWMON_T_EMERGENCY_HYST;
+		data->channel_config[1] |= HWMON_T_EMERGENCY |
+			HWMON_T_EMERGENCY_HYST;
+	}
+
+	if (data->flags & LM90_HAVE_EMERGENCY_ALARM) {
+		data->channel_config[0] |= HWMON_T_EMERGENCY_ALARM;
+		data->channel_config[1] |= HWMON_T_EMERGENCY_ALARM;
+	}
+
+	if (data->flags & LM90_HAVE_TEMP3) {
+		data->channel_config[2] = HWMON_T_INPUT |
+			HWMON_T_MIN | HWMON_T_MAX |
+			HWMON_T_CRIT | HWMON_T_CRIT_HYST |
+			HWMON_T_EMERGENCY | HWMON_T_EMERGENCY_HYST |
+			HWMON_T_MIN_ALARM | HWMON_T_MAX_ALARM |
+			HWMON_T_CRIT_ALARM | HWMON_T_EMERGENCY_ALARM |
+			HWMON_T_FAULT;
+	}
+
 	data->reg_local_ext = lm90_params[data->kind].reg_local_ext;
 
 	/* Set maximum conversion rate */
@@ -1677,21 +1746,10 @@ static int lm90_probe(struct i2c_client *client,
 		return err;
 	}
 
-	/* Register sysfs hooks */
-	data->groups[groups++] = &lm90_group;
-
-	if (data->flags & LM90_HAVE_OFFSET)
-		data->groups[groups++] = &lm90_temp2_offset_group;
-
-	if (data->flags & LM90_HAVE_EMERGENCY)
-		data->groups[groups++] = &lm90_emergency_group;
-
-	if (data->flags & LM90_HAVE_EMERGENCY_ALARM)
-		data->groups[groups++] = &lm90_emergency_alarm_group;
-
-	if (data->flags & LM90_HAVE_TEMP3)
-		data->groups[groups++] = &lm90_temp3_group;
-
+	/*
+	 * The 'pec' attribute is attached to the i2c device and thus created
+	 * separately.
+	 */
 	if (client->flags & I2C_CLIENT_PEC) {
 		err = device_create_file(dev, &dev_attr_pec);
 		if (err)
@@ -1701,8 +1759,9 @@ static int lm90_probe(struct i2c_client *client,
 			return err;
 	}
 
-	hwmon_dev = devm_hwmon_device_register_with_groups(dev, client->name,
-							   data, data->groups);
+	hwmon_dev = devm_hwmon_device_register_with_info(dev, client->name,
+							 data, &data->chip,
+							 NULL);
 	if (IS_ERR(hwmon_dev))
 		return PTR_ERR(hwmon_dev);
 
diff --git a/drivers/hwmon/lm95241.c b/drivers/hwmon/lm95241.c
index cdf19adaec79..8c573e6e9726 100644
--- a/drivers/hwmon/lm95241.c
+++ b/drivers/hwmon/lm95241.c
@@ -15,22 +15,17 @@
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 
-#include <linux/module.h>
+#include <linux/bitops.h>
+#include <linux/err.h>
+#include <linux/i2c.h>
 #include <linux/init.h>
-#include <linux/slab.h>
 #include <linux/jiffies.h>
-#include <linux/i2c.h>
 #include <linux/hwmon.h>
-#include <linux/hwmon-sysfs.h>
-#include <linux/err.h>
+#include <linux/module.h>
 #include <linux/mutex.h>
-#include <linux/sysfs.h>
+#include <linux/slab.h>
 
 #define DEVNAME "lm95241"
 
@@ -54,26 +49,25 @@ static const unsigned short normal_i2c[] = {
 #define LM95241_REG_RW_REMOTE_MODEL	0x30
 
 /* LM95241 specific bitfields */
-#define CFG_STOP 0x40
-#define CFG_CR0076 0x00
-#define CFG_CR0182 0x10
-#define CFG_CR1000 0x20
-#define CFG_CR2700 0x30
-#define R1MS_SHIFT 0
-#define R2MS_SHIFT 2
-#define R1MS_MASK (0x01 << (R1MS_SHIFT))
-#define R2MS_MASK (0x01 << (R2MS_SHIFT))
-#define R1DF_SHIFT 1
-#define R2DF_SHIFT 2
-#define R1DF_MASK (0x01 << (R1DF_SHIFT))
-#define R2DF_MASK (0x01 << (R2DF_SHIFT))
-#define R1FE_MASK 0x01
-#define R2FE_MASK 0x05
-#define TT1_SHIFT 0
-#define TT2_SHIFT 4
-#define TT_OFF 0
-#define TT_ON 1
-#define TT_MASK 7
+#define CFG_STOP	BIT(6)
+#define CFG_CR0076	0x00
+#define CFG_CR0182	BIT(4)
+#define CFG_CR1000	BIT(5)
+#define CFG_CR2700	(BIT(4) | BIT(5))
+#define CFG_CRMASK	(BIT(4) | BIT(5))
+#define R1MS_MASK	BIT(0)
+#define R2MS_MASK	BIT(2)
+#define R1DF_MASK	BIT(1)
+#define R2DF_MASK	BIT(2)
+#define R1FE_MASK	BIT(0)
+#define R2FE_MASK	BIT(2)
+#define R1DM		BIT(0)
+#define R2DM		BIT(1)
+#define TT1_SHIFT	0
+#define TT2_SHIFT	4
+#define TT_OFF		0
+#define TT_ON		1
+#define TT_MASK		7
 #define NATSEMI_MAN_ID	0x01
 #define LM95231_CHIP_ID	0xA1
 #define LM95241_CHIP_ID	0xA4
@@ -91,11 +85,12 @@ static const u8 lm95241_reg_address[] = {
 struct lm95241_data {
 	struct i2c_client *client;
 	struct mutex update_lock;
-	unsigned long last_updated, interval;	/* in jiffies */
+	unsigned long last_updated;	/* in jiffies */
+	unsigned long interval;		/* in milli-seconds */
 	char valid;		/* zero until following fields are valid */
 	/* registers values */
 	u8 temp[ARRAY_SIZE(lm95241_reg_address)];
-	u8 config, model, trutherm;
+	u8 status, config, model, trutherm;
 };
 
 /* Conversions */
@@ -118,7 +113,8 @@ static struct lm95241_data *lm95241_update_device(struct device *dev)
 
 	mutex_lock(&data->update_lock);
 
-	if (time_after(jiffies, data->last_updated + data->interval) ||
+	if (time_after(jiffies, data->last_updated
+		       + msecs_to_jiffies(data->interval)) ||
 	    !data->valid) {
 		int i;
 
@@ -127,6 +123,9 @@ static struct lm95241_data *lm95241_update_device(struct device *dev)
 			data->temp[i]
 			  = i2c_smbus_read_byte_data(client,
 						     lm95241_reg_address[i]);
+
+		data->status = i2c_smbus_read_byte_data(client,
+							LM95241_REG_R_STATUS);
 		data->last_updated = jiffies;
 		data->valid = 1;
 	}
@@ -136,197 +135,241 @@ static struct lm95241_data *lm95241_update_device(struct device *dev)
 	return data;
 }
 
-/* Sysfs stuff */
-static ssize_t show_input(struct device *dev, struct device_attribute *attr,
-			  char *buf)
-{
-	struct lm95241_data *data = lm95241_update_device(dev);
-	int index = to_sensor_dev_attr(attr)->index;
-
-	return snprintf(buf, PAGE_SIZE - 1, "%d\n",
-			index == 0 || (data->config & (1 << (index / 2))) ?
-		temp_from_reg_signed(data->temp[index], data->temp[index + 1]) :
-		temp_from_reg_unsigned(data->temp[index],
-				       data->temp[index + 1]));
-}
-
-static ssize_t show_type(struct device *dev, struct device_attribute *attr,
-			 char *buf)
+static int lm95241_read_chip(struct device *dev, u32 attr, int channel,
+			     long *val)
 {
 	struct lm95241_data *data = dev_get_drvdata(dev);
 
-	return snprintf(buf, PAGE_SIZE - 1,
-		data->model & to_sensor_dev_attr(attr)->index ? "1\n" : "2\n");
+	switch (attr) {
+	case hwmon_chip_update_interval:
+		*val = data->interval;
+		return 0;
+	default:
+		return -EOPNOTSUPP;
+	}
 }
 
-static ssize_t set_type(struct device *dev, struct device_attribute *attr,
-			const char *buf, size_t count)
+static int lm95241_read_temp(struct device *dev, u32 attr, int channel,
+			     long *val)
 {
-	struct lm95241_data *data = dev_get_drvdata(dev);
-	struct i2c_client *client = data->client;
-	unsigned long val;
-	int shift;
-	u8 mask = to_sensor_dev_attr(attr)->index;
-
-	if (kstrtoul(buf, 10, &val) < 0)
-		return -EINVAL;
-	if (val != 1 && val != 2)
-		return -EINVAL;
-
-	shift = mask == R1MS_MASK ? TT1_SHIFT : TT2_SHIFT;
-
-	mutex_lock(&data->update_lock);
+	struct lm95241_data *data = lm95241_update_device(dev);
 
-	data->trutherm &= ~(TT_MASK << shift);
-	if (val == 1) {
-		data->model |= mask;
-		data->trutherm |= (TT_ON << shift);
-	} else {
-		data->model &= ~mask;
-		data->trutherm |= (TT_OFF << shift);
+	switch (attr) {
+	case hwmon_temp_input:
+		if (!channel || (data->config & BIT(channel - 1)))
+			*val = temp_from_reg_signed(data->temp[channel * 2],
+						data->temp[channel * 2 + 1]);
+		else
+			*val = temp_from_reg_unsigned(data->temp[channel * 2],
+						data->temp[channel * 2 + 1]);
+		return 0;
+	case hwmon_temp_min:
+		if (channel == 1)
+			*val = (data->config & R1DF_MASK) ? -128000 : 0;
+		else
+			*val = (data->config & R2DF_MASK) ? -128000 : 0;
+		return 0;
+	case hwmon_temp_max:
+		if (channel == 1)
+			*val = (data->config & R1DF_MASK) ? 127875 : 255875;
+		else
+			*val = (data->config & R2DF_MASK) ? 127875 : 255875;
+		return 0;
+	case hwmon_temp_type:
+		if (channel == 1)
+			*val = (data->model & R1MS_MASK) ? 1 : 2;
+		else
+			*val = (data->model & R2MS_MASK) ? 1 : 2;
+		return 0;
+	case hwmon_temp_fault:
+		if (channel == 1)
+			*val = !!(data->status & R1DM);
+		else
+			*val = !!(data->status & R2DM);
+		return 0;
+	default:
+		return -EOPNOTSUPP;
 	}
-	data->valid = 0;
-
-	i2c_smbus_write_byte_data(client, LM95241_REG_RW_REMOTE_MODEL,
-				  data->model);
-	i2c_smbus_write_byte_data(client, LM95241_REG_RW_TRUTHERM,
-				  data->trutherm);
-
-	mutex_unlock(&data->update_lock);
-
-	return count;
 }
 
-static ssize_t show_min(struct device *dev, struct device_attribute *attr,
-			char *buf)
+static int lm95241_read(struct device *dev, enum hwmon_sensor_types type,
+			u32 attr, int channel, long *val)
 {
-	struct lm95241_data *data = dev_get_drvdata(dev);
-
-	return snprintf(buf, PAGE_SIZE - 1,
-			data->config & to_sensor_dev_attr(attr)->index ?
-			"-127000\n" : "0\n");
+	switch (type) {
+	case hwmon_chip:
+		return lm95241_read_chip(dev, attr, channel, val);
+	case hwmon_temp:
+		return lm95241_read_temp(dev, attr, channel, val);
+	default:
+		return -EOPNOTSUPP;
+	}
 }
 
-static ssize_t set_min(struct device *dev, struct device_attribute *attr,
-		       const char *buf, size_t count)
+static int lm95241_write_chip(struct device *dev, u32 attr, int channel,
+			      long val)
 {
 	struct lm95241_data *data = dev_get_drvdata(dev);
-	long val;
-
-	if (kstrtol(buf, 10, &val) < 0)
-		return -EINVAL;
-	if (val < -128000)
-		return -EINVAL;
+	int convrate;
+	u8 config;
+	int ret;
 
 	mutex_lock(&data->update_lock);
 
-	if (val < 0)
-		data->config |= to_sensor_dev_attr(attr)->index;
-	else
-		data->config &= ~to_sensor_dev_attr(attr)->index;
-	data->valid = 0;
-
-	i2c_smbus_write_byte_data(data->client, LM95241_REG_RW_CONFIG,
-				  data->config);
-
+	switch (attr) {
+	case hwmon_chip_update_interval:
+		config = data->config & ~CFG_CRMASK;
+		if (val < 130) {
+			convrate = 76;
+			config |= CFG_CR0076;
+		} else if (val < 590) {
+			convrate = 182;
+			config |= CFG_CR0182;
+		} else if (val < 1850) {
+			convrate = 1000;
+			config |= CFG_CR1000;
+		} else {
+			convrate = 2700;
+			config |= CFG_CR2700;
+		}
+		data->interval = convrate;
+		data->config = config;
+		ret = i2c_smbus_write_byte_data(data->client,
+						LM95241_REG_RW_CONFIG, config);
+		break;
+	default:
+		ret = -EOPNOTSUPP;
+		break;
+	}
 	mutex_unlock(&data->update_lock);
-
-	return count;
+	return ret;
 }
 
-static ssize_t show_max(struct device *dev, struct device_attribute *attr,
-			char *buf)
+static int lm95241_write_temp(struct device *dev, u32 attr, int channel,
+			      long val)
 {
 	struct lm95241_data *data = dev_get_drvdata(dev);
-
-	return snprintf(buf, PAGE_SIZE - 1,
-			data->config & to_sensor_dev_attr(attr)->index ?
-			"127000\n" : "255000\n");
-}
-
-static ssize_t set_max(struct device *dev, struct device_attribute *attr,
-		       const char *buf, size_t count)
-{
-	struct lm95241_data *data = dev_get_drvdata(dev);
-	long val;
-
-	if (kstrtol(buf, 10, &val) < 0)
-		return -EINVAL;
-	if (val >= 256000)
-		return -EINVAL;
+	struct i2c_client *client = data->client;
+	int ret;
 
 	mutex_lock(&data->update_lock);
 
-	if (val <= 127000)
-		data->config |= to_sensor_dev_attr(attr)->index;
-	else
-		data->config &= ~to_sensor_dev_attr(attr)->index;
-	data->valid = 0;
-
-	i2c_smbus_write_byte_data(data->client, LM95241_REG_RW_CONFIG,
-				  data->config);
+	switch (attr) {
+	case hwmon_temp_min:
+		if (channel == 1) {
+			if (val < 0)
+				data->config |= R1DF_MASK;
+			else
+				data->config &= ~R1DF_MASK;
+		} else {
+			if (val < 0)
+				data->config |= R2DF_MASK;
+			else
+				data->config &= ~R2DF_MASK;
+		}
+		data->valid = 0;
+		ret = i2c_smbus_write_byte_data(client, LM95241_REG_RW_CONFIG,
+						data->config);
+		break;
+	case hwmon_temp_max:
+		if (channel == 1) {
+			if (val <= 127875)
+				data->config |= R1DF_MASK;
+			else
+				data->config &= ~R1DF_MASK;
+		} else {
+			if (val <= 127875)
+				data->config |= R2DF_MASK;
+			else
+				data->config &= ~R2DF_MASK;
+		}
+		data->valid = 0;
+		ret = i2c_smbus_write_byte_data(client, LM95241_REG_RW_CONFIG,
+						data->config);
+		break;
+	case hwmon_temp_type:
+		if (val != 1 && val != 2) {
+			ret = -EINVAL;
+			break;
+		}
+		if (channel == 1) {
+			data->trutherm &= ~(TT_MASK << TT1_SHIFT);
+			if (val == 1) {
+				data->model |= R1MS_MASK;
+				data->trutherm |= (TT_ON << TT1_SHIFT);
+			} else {
+				data->model &= ~R1MS_MASK;
+				data->trutherm |= (TT_OFF << TT1_SHIFT);
+			}
+		} else {
+			data->trutherm &= ~(TT_MASK << TT2_SHIFT);
+			if (val == 1) {
+				data->model |= R2MS_MASK;
+				data->trutherm |= (TT_ON << TT2_SHIFT);
+			} else {
+				data->model &= ~R2MS_MASK;
+				data->trutherm |= (TT_OFF << TT2_SHIFT);
+			}
+		}
+		ret = i2c_smbus_write_byte_data(client,
+						LM95241_REG_RW_REMOTE_MODEL,
+						data->model);
+		if (ret < 0)
+			break;
+		ret = i2c_smbus_write_byte_data(client, LM95241_REG_RW_TRUTHERM,
+						data->trutherm);
+		break;
+	default:
+		ret = -EOPNOTSUPP;
+		break;
+	}
 
 	mutex_unlock(&data->update_lock);
 
-	return count;
+	return ret;
 }
 
-static ssize_t show_interval(struct device *dev, struct device_attribute *attr,
-			     char *buf)
+static int lm95241_write(struct device *dev, enum hwmon_sensor_types type,
+			 u32 attr, int channel, long val)
 {
-	struct lm95241_data *data = lm95241_update_device(dev);
-
-	return snprintf(buf, PAGE_SIZE - 1, "%lu\n", 1000 * data->interval
-			/ HZ);
+	switch (type) {
+	case hwmon_chip:
+		return lm95241_write_chip(dev, attr, channel, val);
+	case hwmon_temp:
+		return lm95241_write_temp(dev, attr, channel, val);
+	default:
+		return -EOPNOTSUPP;
+	}
 }
 
-static ssize_t set_interval(struct device *dev, struct device_attribute *attr,
-			    const char *buf, size_t count)
+static umode_t lm95241_is_visible(const void *data,
+				  enum hwmon_sensor_types type,
+				  u32 attr, int channel)
 {
-	struct lm95241_data *data = dev_get_drvdata(dev);
-	unsigned long val;
-
-	if (kstrtoul(buf, 10, &val) < 0)
-		return -EINVAL;
-
-	data->interval = val * HZ / 1000;
-
-	return count;
+	switch (type) {
+	case hwmon_chip:
+		switch (attr) {
+		case hwmon_chip_update_interval:
+			return S_IRUGO | S_IWUSR;
+		}
+		break;
+	case hwmon_temp:
+		switch (attr) {
+		case hwmon_temp_input:
+			return S_IRUGO;
+		case hwmon_temp_fault:
+			return S_IRUGO;
+		case hwmon_temp_min:
+		case hwmon_temp_max:
+		case hwmon_temp_type:
+			return S_IRUGO | S_IWUSR;
+		}
+		break;
+	default:
+		break;
+	}
+	return 0;
 }
 
-static SENSOR_DEVICE_ATTR(temp1_input, S_IRUGO, show_input, NULL, 0);
-static SENSOR_DEVICE_ATTR(temp2_input, S_IRUGO, show_input, NULL, 2);
-static SENSOR_DEVICE_ATTR(temp3_input, S_IRUGO, show_input, NULL, 4);
-static SENSOR_DEVICE_ATTR(temp2_type, S_IWUSR | S_IRUGO, show_type, set_type,
-			  R1MS_MASK);
-static SENSOR_DEVICE_ATTR(temp3_type, S_IWUSR | S_IRUGO, show_type, set_type,
-			  R2MS_MASK);
-static SENSOR_DEVICE_ATTR(temp2_min, S_IWUSR | S_IRUGO, show_min, set_min,
-			  R1DF_MASK);
-static SENSOR_DEVICE_ATTR(temp3_min, S_IWUSR | S_IRUGO, show_min, set_min,
-			  R2DF_MASK);
-static SENSOR_DEVICE_ATTR(temp2_max, S_IWUSR | S_IRUGO, show_max, set_max,
-			  R1DF_MASK);
-static SENSOR_DEVICE_ATTR(temp3_max, S_IWUSR | S_IRUGO, show_max, set_max,
-			  R2DF_MASK);
-static DEVICE_ATTR(update_interval, S_IWUSR | S_IRUGO, show_interval,
-		   set_interval);
-
-static struct attribute *lm95241_attrs[] = {
-	&sensor_dev_attr_temp1_input.dev_attr.attr,
-	&sensor_dev_attr_temp2_input.dev_attr.attr,
-	&sensor_dev_attr_temp3_input.dev_attr.attr,
-	&sensor_dev_attr_temp2_type.dev_attr.attr,
-	&sensor_dev_attr_temp3_type.dev_attr.attr,
-	&sensor_dev_attr_temp2_min.dev_attr.attr,
-	&sensor_dev_attr_temp3_min.dev_attr.attr,
-	&sensor_dev_attr_temp2_max.dev_attr.attr,
-	&sensor_dev_attr_temp3_max.dev_attr.attr,
-	&dev_attr_update_interval.attr,
-	NULL
-};
-ATTRIBUTE_GROUPS(lm95241);
-
 /* Return 0 if detection is successful, -ENODEV otherwise */
 static int lm95241_detect(struct i2c_client *new_client,
 			  struct i2c_board_info *info)
@@ -362,8 +405,8 @@ static int lm95241_detect(struct i2c_client *new_client,
 static void lm95241_init_client(struct i2c_client *client,
 				struct lm95241_data *data)
 {
-	data->interval = HZ;	/* 1 sec default */
-	data->config = CFG_CR0076;
+	data->interval = 1000;
+	data->config = CFG_CR1000;
 	data->trutherm = (TT_OFF << TT1_SHIFT) | (TT_OFF << TT2_SHIFT);
 
 	i2c_smbus_write_byte_data(client, LM95241_REG_RW_CONFIG, data->config);
@@ -375,6 +418,47 @@ static void lm95241_init_client(struct i2c_client *client,
 				  data->model);
 }
 
+static const u32 lm95241_chip_config[] = {
+	HWMON_C_UPDATE_INTERVAL,
+	0
+};
+
+static const struct hwmon_channel_info lm95241_chip = {
+	.type = hwmon_chip,
+	.config = lm95241_chip_config,
+};
+
+static const u32 lm95241_temp_config[] = {
+	HWMON_T_INPUT,
+	HWMON_T_INPUT | HWMON_T_MAX | HWMON_T_MIN | HWMON_T_TYPE |
+		HWMON_T_FAULT,
+	HWMON_T_INPUT | HWMON_T_MAX | HWMON_T_MIN | HWMON_T_TYPE |
+		HWMON_T_FAULT,
+	0
+};
+
+static const struct hwmon_channel_info lm95241_temp = {
+	.type = hwmon_temp,
+	.config = lm95241_temp_config,
+};
+
+static const struct hwmon_channel_info *lm95241_info[] = {
+	&lm95241_chip,
+	&lm95241_temp,
+	NULL
+};
+
+static const struct hwmon_ops lm95241_hwmon_ops = {
+	.is_visible = lm95241_is_visible,
+	.read = lm95241_read,
+	.write = lm95241_write,
+};
+
+static const struct hwmon_chip_info lm95241_chip_info = {
+	.ops = &lm95241_hwmon_ops,
+	.info = lm95241_info,
+};
+
 static int lm95241_probe(struct i2c_client *client,
 			 const struct i2c_device_id *id)
 {
@@ -392,9 +476,10 @@ static int lm95241_probe(struct i2c_client *client,
 	/* Initialize the LM95241 chip */
 	lm95241_init_client(client, data);
 
-	hwmon_dev = devm_hwmon_device_register_with_groups(dev, client->name,
+	hwmon_dev = devm_hwmon_device_register_with_info(dev, client->name,
 							   data,
-							   lm95241_groups);
+							   &lm95241_chip_info,
+							   NULL);
 	return PTR_ERR_OR_ZERO(hwmon_dev);
 }
 
@@ -420,5 +505,5 @@ static struct i2c_driver lm95241_driver = {
 module_i2c_driver(lm95241_driver);
 
 MODULE_AUTHOR("Davide Rizzo <elpa.rizzo@gmail.com>");
-MODULE_DESCRIPTION("LM95241 sensor driver");
+MODULE_DESCRIPTION("LM95231/LM95241 sensor driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/hwmon/lm95245.c b/drivers/hwmon/lm95245.c
index e7aef4561c83..a3bfd88752ca 100644
--- a/drivers/hwmon/lm95245.c
+++ b/drivers/hwmon/lm95245.c
@@ -15,22 +15,16 @@
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 
-#include <linux/module.h>
+#include <linux/err.h>
 #include <linux/init.h>
-#include <linux/slab.h>
-#include <linux/jiffies.h>
-#include <linux/i2c.h>
 #include <linux/hwmon.h>
-#include <linux/hwmon-sysfs.h>
-#include <linux/err.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
 #include <linux/mutex.h>
-#include <linux/sysfs.h>
+#include <linux/regmap.h>
+#include <linux/slab.h>
 
 static const unsigned short normal_i2c[] = {
 	0x18, 0x19, 0x29, 0x4c, 0x4d, I2C_CLIENT_END };
@@ -89,6 +83,7 @@ static const unsigned short normal_i2c[] = {
 #define RATE_CR1000	0x02
 #define RATE_CR2500	0x03
 
+#define STATUS1_ROS		0x10
 #define STATUS1_DIODE_FAULT	0x04
 #define STATUS1_RTCRIT		0x02
 #define STATUS1_LOC		0x01
@@ -112,14 +107,9 @@ static const u8 lm95245_reg_address[] = {
 
 /* Client data (each client gets its own) */
 struct lm95245_data {
-	struct i2c_client *client;
+	struct regmap *regmap;
 	struct mutex update_lock;
-	unsigned long last_updated;	/* in jiffies */
-	unsigned long interval;	/* in msecs */
-	bool valid;		/* zero until following fields are valid */
-	/* registers values */
-	u8 regs[ARRAY_SIZE(lm95245_reg_address)];
-	u8 config1, config2;
+	int interval;	/* in msecs */
 };
 
 /* Conversions */
@@ -135,60 +125,36 @@ static int temp_from_reg_signed(u8 val_h, u8 val_l)
 	return temp_from_reg_unsigned(val_h, val_l);
 }
 
-static struct lm95245_data *lm95245_update_device(struct device *dev)
+static int lm95245_read_conversion_rate(struct lm95245_data *data)
 {
-	struct lm95245_data *data = dev_get_drvdata(dev);
-	struct i2c_client *client = data->client;
+	unsigned int rate;
+	int ret;
 
-	mutex_lock(&data->update_lock);
-
-	if (time_after(jiffies, data->last_updated
-		+ msecs_to_jiffies(data->interval)) || !data->valid) {
-		int i;
-
-		for (i = 0; i < ARRAY_SIZE(lm95245_reg_address); i++)
-			data->regs[i]
-			  = i2c_smbus_read_byte_data(client,
-						     lm95245_reg_address[i]);
-		data->last_updated = jiffies;
-		data->valid = 1;
-	}
-
-	mutex_unlock(&data->update_lock);
-
-	return data;
-}
-
-static unsigned long lm95245_read_conversion_rate(struct i2c_client *client)
-{
-	int rate;
-	unsigned long interval;
-
-	rate = i2c_smbus_read_byte_data(client, LM95245_REG_RW_CONVERS_RATE);
+	ret = regmap_read(data->regmap, LM95245_REG_RW_CONVERS_RATE, &rate);
+	if (ret < 0)
+		return ret;
 
 	switch (rate) {
 	case RATE_CR0063:
-		interval = 63;
+		data->interval = 63;
 		break;
 	case RATE_CR0364:
-		interval = 364;
+		data->interval = 364;
 		break;
 	case RATE_CR1000:
-		interval = 1000;
+		data->interval = 1000;
 		break;
 	case RATE_CR2500:
 	default:
-		interval = 2500;
+		data->interval = 2500;
 		break;
 	}
-
-	return interval;
+	return 0;
 }
 
-static unsigned long lm95245_set_conversion_rate(struct i2c_client *client,
-			unsigned long interval)
+static int lm95245_set_conversion_rate(struct lm95245_data *data, long interval)
 {
-	int rate;
+	int ret, rate;
 
 	if (interval <= 63) {
 		interval = 63;
@@ -204,221 +170,289 @@ static unsigned long lm95245_set_conversion_rate(struct i2c_client *client,
 		rate = RATE_CR2500;
 	}
 
-	i2c_smbus_write_byte_data(client, LM95245_REG_RW_CONVERS_RATE, rate);
+	ret = regmap_write(data->regmap, LM95245_REG_RW_CONVERS_RATE, rate);
+	if (ret < 0)
+		return ret;
 
-	return interval;
-}
-
-/* Sysfs stuff */
-static ssize_t show_input(struct device *dev, struct device_attribute *attr,
-			  char *buf)
-{
-	struct lm95245_data *data = lm95245_update_device(dev);
-	int temp;
-	int index = to_sensor_dev_attr(attr)->index;
-
-	/*
-	 * Index 0 (Local temp) is always signed
-	 * Index 2 (Remote temp) has both signed and unsigned data
-	 * use signed calculation for remote if signed bit is set
-	 */
-	if (index == 0 || data->regs[index] & 0x80)
-		temp = temp_from_reg_signed(data->regs[index],
-			    data->regs[index + 1]);
-	else
-		temp = temp_from_reg_unsigned(data->regs[index + 2],
-			    data->regs[index + 3]);
-
-	return snprintf(buf, PAGE_SIZE - 1, "%d\n", temp);
-}
-
-static ssize_t show_limit(struct device *dev, struct device_attribute *attr,
-			 char *buf)
-{
-	struct lm95245_data *data = lm95245_update_device(dev);
-	int index = to_sensor_dev_attr(attr)->index;
-
-	return snprintf(buf, PAGE_SIZE - 1, "%d\n",
-			data->regs[index] * 1000);
+	data->interval = interval;
+	return 0;
 }
 
-static ssize_t set_limit(struct device *dev, struct device_attribute *attr,
-			const char *buf, size_t count)
+static int lm95245_read_temp(struct device *dev, u32 attr, int channel,
+			     long *val)
 {
 	struct lm95245_data *data = dev_get_drvdata(dev);
-	int index = to_sensor_dev_attr(attr)->index;
-	struct i2c_client *client = data->client;
-	unsigned long val;
-
-	if (kstrtoul(buf, 10, &val) < 0)
-		return -EINVAL;
-
-	val /= 1000;
-
-	val = clamp_val(val, 0, (index == 6 ? 127 : 255));
-
-	mutex_lock(&data->update_lock);
-
-	data->valid = 0;
-
-	i2c_smbus_write_byte_data(client, lm95245_reg_address[index], val);
-
-	mutex_unlock(&data->update_lock);
-
-	return count;
+	struct regmap *regmap = data->regmap;
+	int ret, regl, regh, regvall, regvalh;
+
+	switch (attr) {
+	case hwmon_temp_input:
+		regl = channel ? LM95245_REG_R_REMOTE_TEMPL_S :
+				 LM95245_REG_R_LOCAL_TEMPL_S;
+		regh = channel ? LM95245_REG_R_REMOTE_TEMPH_S :
+				 LM95245_REG_R_LOCAL_TEMPH_S;
+		ret = regmap_read(regmap, regl, &regvall);
+		if (ret < 0)
+			return ret;
+		ret = regmap_read(regmap, regh, &regvalh);
+		if (ret < 0)
+			return ret;
+		/*
+		 * Local temp is always signed.
+		 * Remote temp has both signed and unsigned data.
+		 * Use signed calculation for remote if signed bit is set
+		 * or if reported temperature is below signed limit.
+		 */
+		if (!channel || (regvalh & 0x80) || regvalh < 0x7f) {
+			*val = temp_from_reg_signed(regvalh, regvall);
+			return 0;
+		}
+		ret = regmap_read(regmap, LM95245_REG_R_REMOTE_TEMPL_U,
+				  &regvall);
+		if (ret < 0)
+			return ret;
+		ret = regmap_read(regmap, LM95245_REG_R_REMOTE_TEMPH_U,
+				  &regvalh);
+		if (ret < 0)
+			return ret;
+		*val = temp_from_reg_unsigned(regvalh, regvall);
+		return 0;
+	case hwmon_temp_max:
+		ret = regmap_read(regmap, LM95245_REG_RW_REMOTE_OS_LIMIT,
+				  &regvalh);
+		if (ret < 0)
+			return ret;
+		*val = regvalh * 1000;
+		return 0;
+	case hwmon_temp_crit:
+		regh = channel ? LM95245_REG_RW_REMOTE_TCRIT_LIMIT :
+				 LM95245_REG_RW_LOCAL_OS_TCRIT_LIMIT;
+		ret = regmap_read(regmap, regh, &regvalh);
+		if (ret < 0)
+			return ret;
+		*val = regvalh * 1000;
+		return 0;
+	case hwmon_temp_max_hyst:
+		ret = regmap_read(regmap, LM95245_REG_RW_REMOTE_OS_LIMIT,
+				  &regvalh);
+		if (ret < 0)
+			return ret;
+		ret = regmap_read(regmap, LM95245_REG_RW_COMMON_HYSTERESIS,
+				  &regvall);
+		if (ret < 0)
+			return ret;
+		*val = (regvalh - regvall) * 1000;
+		return 0;
+	case hwmon_temp_crit_hyst:
+		regh = channel ? LM95245_REG_RW_REMOTE_TCRIT_LIMIT :
+				 LM95245_REG_RW_LOCAL_OS_TCRIT_LIMIT;
+		ret = regmap_read(regmap, regh, &regvalh);
+		if (ret < 0)
+			return ret;
+		ret = regmap_read(regmap, LM95245_REG_RW_COMMON_HYSTERESIS,
+				  &regvall);
+		if (ret < 0)
+			return ret;
+		*val = (regvalh - regvall) * 1000;
+		return 0;
+	case hwmon_temp_type:
+		ret = regmap_read(regmap, LM95245_REG_RW_CONFIG2, &regvalh);
+		if (ret < 0)
+			return ret;
+		*val = (regvalh & CFG2_REMOTE_TT) ? 1 : 2;
+		return 0;
+	case hwmon_temp_offset:
+		ret = regmap_read(regmap, LM95245_REG_RW_REMOTE_OFFL,
+				  &regvall);
+		if (ret < 0)
+			return ret;
+		ret = regmap_read(regmap, LM95245_REG_RW_REMOTE_OFFH,
+				  &regvalh);
+		if (ret < 0)
+			return ret;
+		*val = temp_from_reg_signed(regvalh, regvall);
+		return 0;
+	case hwmon_temp_max_alarm:
+		ret = regmap_read(regmap, LM95245_REG_R_STATUS1, &regvalh);
+		if (ret < 0)
+			return ret;
+		*val = !!(regvalh & STATUS1_ROS);
+		return 0;
+	case hwmon_temp_crit_alarm:
+		ret = regmap_read(regmap, LM95245_REG_R_STATUS1, &regvalh);
+		if (ret < 0)
+			return ret;
+		*val = !!(regvalh & (channel ? STATUS1_RTCRIT : STATUS1_LOC));
+		return 0;
+	case hwmon_temp_fault:
+		ret = regmap_read(regmap, LM95245_REG_R_STATUS1, &regvalh);
+		if (ret < 0)
+			return ret;
+		*val = !!(regvalh & STATUS1_DIODE_FAULT);
+		return 0;
+	default:
+		return -EOPNOTSUPP;
+	}
 }
 
-static ssize_t show_crit_hyst(struct device *dev, struct device_attribute *attr,
-			char *buf)
+static int lm95245_write_temp(struct device *dev, u32 attr, int channel,
+			      long val)
 {
-	struct lm95245_data *data = lm95245_update_device(dev);
-	int index = to_sensor_dev_attr(attr)->index;
-	int hyst = data->regs[index] - data->regs[8];
-
-	return snprintf(buf, PAGE_SIZE - 1, "%d\n", hyst * 1000);
+	struct lm95245_data *data = dev_get_drvdata(dev);
+	struct regmap *regmap = data->regmap;
+	unsigned int regval;
+	int ret, reg;
+
+	switch (attr) {
+	case hwmon_temp_max:
+		val = clamp_val(val / 1000, 0, 255);
+		ret = regmap_write(regmap, LM95245_REG_RW_REMOTE_OS_LIMIT, val);
+		return ret;
+	case hwmon_temp_crit:
+		reg = channel ? LM95245_REG_RW_REMOTE_TCRIT_LIMIT :
+				LM95245_REG_RW_LOCAL_OS_TCRIT_LIMIT;
+		val = clamp_val(val / 1000, 0, channel ? 255 : 127);
+		ret = regmap_write(regmap, reg, val);
+		return ret;
+	case hwmon_temp_crit_hyst:
+		mutex_lock(&data->update_lock);
+		ret = regmap_read(regmap, LM95245_REG_RW_LOCAL_OS_TCRIT_LIMIT,
+				  &regval);
+		if (ret < 0) {
+			mutex_unlock(&data->update_lock);
+			return ret;
+		}
+		/* Clamp to reasonable range to prevent overflow */
+		val = clamp_val(val, -1000000, 1000000);
+		val = regval - val / 1000;
+		val = clamp_val(val, 0, 31);
+		ret = regmap_write(regmap, LM95245_REG_RW_COMMON_HYSTERESIS,
+				   val);
+		mutex_unlock(&data->update_lock);
+		return ret;
+	case hwmon_temp_offset:
+		val = clamp_val(val, -128000, 127875);
+		val = val * 256 / 1000;
+		mutex_lock(&data->update_lock);
+		ret = regmap_write(regmap, LM95245_REG_RW_REMOTE_OFFL,
+				   val & 0xe0);
+		if (ret < 0) {
+			mutex_unlock(&data->update_lock);
+			return ret;
+		}
+		ret = regmap_write(regmap, LM95245_REG_RW_REMOTE_OFFH,
+				   (val >> 8) & 0xff);
+		mutex_unlock(&data->update_lock);
+		return ret;
+	case hwmon_temp_type:
+		if (val != 1 && val != 2)
+			return -EINVAL;
+		ret = regmap_update_bits(regmap, LM95245_REG_RW_CONFIG2,
+					 CFG2_REMOTE_TT,
+					 val == 1 ? CFG2_REMOTE_TT : 0);
+		return ret;
+	default:
+		return -EOPNOTSUPP;
+	}
 }
 
-static ssize_t set_crit_hyst(struct device *dev, struct device_attribute *attr,
-			const char *buf, size_t count)
+static int lm95245_read_chip(struct device *dev, u32 attr, int channel,
+			     long *val)
 {
 	struct lm95245_data *data = dev_get_drvdata(dev);
-	int index = to_sensor_dev_attr(attr)->index;
-	struct i2c_client *client = data->client;
-	unsigned long val;
-	int hyst, limit;
-
-	if (kstrtoul(buf, 10, &val) < 0)
-		return -EINVAL;
-
-	mutex_lock(&data->update_lock);
-
-	limit = i2c_smbus_read_byte_data(client, lm95245_reg_address[index]);
-	hyst = limit - val / 1000;
-	hyst = clamp_val(hyst, 0, 31);
-	data->regs[8] = hyst;
 
-	/* shared crit hysteresis */
-	i2c_smbus_write_byte_data(client, LM95245_REG_RW_COMMON_HYSTERESIS,
-		hyst);
-
-	mutex_unlock(&data->update_lock);
-
-	return count;
+	switch (attr) {
+	case hwmon_chip_update_interval:
+		*val = data->interval;
+		return 0;
+	default:
+		return -EOPNOTSUPP;
+	}
 }
 
-static ssize_t show_type(struct device *dev, struct device_attribute *attr,
-			 char *buf)
+static int lm95245_write_chip(struct device *dev, u32 attr, int channel,
+			      long val)
 {
 	struct lm95245_data *data = dev_get_drvdata(dev);
-
-	return snprintf(buf, PAGE_SIZE - 1,
-		data->config2 & CFG2_REMOTE_TT ? "1\n" : "2\n");
+	int ret;
+
+	switch (attr) {
+	case hwmon_chip_update_interval:
+		mutex_lock(&data->update_lock);
+		ret = lm95245_set_conversion_rate(data, val);
+		mutex_unlock(&data->update_lock);
+		return ret;
+	default:
+		return -EOPNOTSUPP;
+	}
 }
 
-static ssize_t set_type(struct device *dev, struct device_attribute *attr,
-			const char *buf, size_t count)
+static int lm95245_read(struct device *dev, enum hwmon_sensor_types type,
+			u32 attr, int channel, long *val)
 {
-	struct lm95245_data *data = dev_get_drvdata(dev);
-	struct i2c_client *client = data->client;
-	unsigned long val;
-
-	if (kstrtoul(buf, 10, &val) < 0)
-		return -EINVAL;
-	if (val != 1 && val != 2)
-		return -EINVAL;
-
-	mutex_lock(&data->update_lock);
-
-	if (val == 1)
-		data->config2 |= CFG2_REMOTE_TT;
-	else
-		data->config2 &= ~CFG2_REMOTE_TT;
-
-	data->valid = 0;
-
-	i2c_smbus_write_byte_data(client, LM95245_REG_RW_CONFIG2,
-				  data->config2);
-
-	mutex_unlock(&data->update_lock);
-
-	return count;
+	switch (type) {
+	case hwmon_chip:
+		return lm95245_read_chip(dev, attr, channel, val);
+	case hwmon_temp:
+		return lm95245_read_temp(dev, attr, channel, val);
+	default:
+		return -EOPNOTSUPP;
+	}
 }
 
-static ssize_t show_alarm(struct device *dev, struct device_attribute *attr,
-			 char *buf)
+static int lm95245_write(struct device *dev, enum hwmon_sensor_types type,
+			 u32 attr, int channel, long val)
 {
-	struct lm95245_data *data = lm95245_update_device(dev);
-	int index = to_sensor_dev_attr(attr)->index;
-
-	return snprintf(buf, PAGE_SIZE - 1, "%d\n",
-			!!(data->regs[9] & index));
+	switch (type) {
+	case hwmon_chip:
+		return lm95245_write_chip(dev, attr, channel, val);
+	case hwmon_temp:
+		return lm95245_write_temp(dev, attr, channel, val);
+	default:
+		return -EOPNOTSUPP;
+	}
 }
 
-static ssize_t show_interval(struct device *dev, struct device_attribute *attr,
-			     char *buf)
+static umode_t lm95245_temp_is_visible(const void *data, u32 attr, int channel)
 {
-	struct lm95245_data *data = lm95245_update_device(dev);
-
-	return snprintf(buf, PAGE_SIZE - 1, "%lu\n", data->interval);
+	switch (attr) {
+	case hwmon_temp_input:
+	case hwmon_temp_max_alarm:
+	case hwmon_temp_max_hyst:
+	case hwmon_temp_crit_alarm:
+	case hwmon_temp_fault:
+		return S_IRUGO;
+	case hwmon_temp_type:
+	case hwmon_temp_max:
+	case hwmon_temp_crit:
+	case hwmon_temp_offset:
+		return S_IRUGO | S_IWUSR;
+	case hwmon_temp_crit_hyst:
+		return (channel == 0) ? S_IRUGO | S_IWUSR : S_IRUGO;
+	default:
+		return 0;
+	}
 }
 
-static ssize_t set_interval(struct device *dev, struct device_attribute *attr,
-			    const char *buf, size_t count)
+static umode_t lm95245_is_visible(const void *data,
+				  enum hwmon_sensor_types type,
+				  u32 attr, int channel)
 {
-	struct lm95245_data *data = dev_get_drvdata(dev);
-	struct i2c_client *client = data->client;
-	unsigned long val;
-
-	if (kstrtoul(buf, 10, &val) < 0)
-		return -EINVAL;
-
-	mutex_lock(&data->update_lock);
-
-	data->interval = lm95245_set_conversion_rate(client, val);
-
-	mutex_unlock(&data->update_lock);
-
-	return count;
+	switch (type) {
+	case hwmon_chip:
+		switch (attr) {
+		case hwmon_chip_update_interval:
+			return S_IRUGO | S_IWUSR;
+		default:
+			return 0;
+		}
+	case hwmon_temp:
+		return lm95245_temp_is_visible(data, attr, channel);
+	default:
+		return 0;
+	}
 }
 
-static SENSOR_DEVICE_ATTR(temp1_input, S_IRUGO, show_input, NULL, 0);
-static SENSOR_DEVICE_ATTR(temp1_crit, S_IWUSR | S_IRUGO, show_limit,
-		set_limit, 6);
-static SENSOR_DEVICE_ATTR(temp1_crit_hyst, S_IWUSR | S_IRUGO, show_crit_hyst,
-		set_crit_hyst, 6);
-static SENSOR_DEVICE_ATTR(temp1_crit_alarm, S_IRUGO, show_alarm, NULL,
-		STATUS1_LOC);
-
-static SENSOR_DEVICE_ATTR(temp2_input, S_IRUGO, show_input, NULL, 2);
-static SENSOR_DEVICE_ATTR(temp2_crit, S_IWUSR | S_IRUGO, show_limit,
-		set_limit, 7);
-static SENSOR_DEVICE_ATTR(temp2_crit_hyst, S_IRUGO, show_crit_hyst, NULL, 7);
-static SENSOR_DEVICE_ATTR(temp2_crit_alarm, S_IRUGO, show_alarm, NULL,
-		STATUS1_RTCRIT);
-static SENSOR_DEVICE_ATTR(temp2_type, S_IWUSR | S_IRUGO, show_type,
-		set_type, 0);
-static SENSOR_DEVICE_ATTR(temp2_fault, S_IRUGO, show_alarm, NULL,
-		STATUS1_DIODE_FAULT);
-
-static DEVICE_ATTR(update_interval, S_IWUSR | S_IRUGO, show_interval,
-		set_interval);
-
-static struct attribute *lm95245_attrs[] = {
-	&sensor_dev_attr_temp1_input.dev_attr.attr,
-	&sensor_dev_attr_temp1_crit.dev_attr.attr,
-	&sensor_dev_attr_temp1_crit_hyst.dev_attr.attr,
-	&sensor_dev_attr_temp1_crit_alarm.dev_attr.attr,
-	&sensor_dev_attr_temp2_input.dev_attr.attr,
-	&sensor_dev_attr_temp2_crit.dev_attr.attr,
-	&sensor_dev_attr_temp2_crit_hyst.dev_attr.attr,
-	&sensor_dev_attr_temp2_crit_alarm.dev_attr.attr,
-	&sensor_dev_attr_temp2_type.dev_attr.attr,
-	&sensor_dev_attr_temp2_fault.dev_attr.attr,
-	&dev_attr_update_interval.attr,
-	NULL
-};
-ATTRIBUTE_GROUPS(lm95245);
-
 /* Return 0 if detection is successful, -ENODEV otherwise */
 static int lm95245_detect(struct i2c_client *new_client,
 			  struct i2c_board_info *info)
@@ -453,44 +487,130 @@ static int lm95245_detect(struct i2c_client *new_client,
 	return 0;
 }
 
-static void lm95245_init_client(struct i2c_client *client,
-				struct lm95245_data *data)
+static int lm95245_init_client(struct lm95245_data *data)
 {
-	data->interval = lm95245_read_conversion_rate(client);
-
-	data->config1 = i2c_smbus_read_byte_data(client,
-		LM95245_REG_RW_CONFIG1);
-	data->config2 = i2c_smbus_read_byte_data(client,
-		LM95245_REG_RW_CONFIG2);
-
-	if (data->config1 & CFG_STOP) {
-		/* Clear the standby bit */
-		data->config1 &= ~CFG_STOP;
-		i2c_smbus_write_byte_data(client, LM95245_REG_RW_CONFIG1,
-			data->config1);
+	int ret;
+
+	ret = lm95245_read_conversion_rate(data);
+	if (ret < 0)
+		return ret;
+
+	return regmap_update_bits(data->regmap, LM95245_REG_RW_CONFIG1,
+				  CFG_STOP, 0);
+}
+
+static bool lm95245_is_writeable_reg(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case LM95245_REG_RW_CONFIG1:
+	case LM95245_REG_RW_CONVERS_RATE:
+	case LM95245_REG_W_ONE_SHOT:
+	case LM95245_REG_RW_CONFIG2:
+	case LM95245_REG_RW_REMOTE_OFFH:
+	case LM95245_REG_RW_REMOTE_OFFL:
+	case LM95245_REG_RW_REMOTE_OS_LIMIT:
+	case LM95245_REG_RW_LOCAL_OS_TCRIT_LIMIT:
+	case LM95245_REG_RW_REMOTE_TCRIT_LIMIT:
+	case LM95245_REG_RW_COMMON_HYSTERESIS:
+		return true;
+	default:
+		return false;
+	}
+}
+
+static bool lm95245_is_volatile_reg(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case LM95245_REG_R_STATUS1:
+	case LM95245_REG_R_STATUS2:
+	case LM95245_REG_R_LOCAL_TEMPH_S:
+	case LM95245_REG_R_LOCAL_TEMPL_S:
+	case LM95245_REG_R_REMOTE_TEMPH_S:
+	case LM95245_REG_R_REMOTE_TEMPL_S:
+	case LM95245_REG_R_REMOTE_TEMPH_U:
+	case LM95245_REG_R_REMOTE_TEMPL_U:
+		return true;
+	default:
+		return false;
 	}
 }
 
+static const struct regmap_config lm95245_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
+	.writeable_reg = lm95245_is_writeable_reg,
+	.volatile_reg = lm95245_is_volatile_reg,
+	.cache_type = REGCACHE_RBTREE,
+	.use_single_rw = true,
+};
+
+static const u32 lm95245_chip_config[] = {
+	HWMON_C_UPDATE_INTERVAL,
+	0
+};
+
+static const struct hwmon_channel_info lm95245_chip = {
+	.type = hwmon_chip,
+	.config = lm95245_chip_config,
+};
+
+static const u32 lm95245_temp_config[] = {
+	HWMON_T_INPUT | HWMON_T_CRIT | HWMON_T_CRIT_HYST | HWMON_T_CRIT_ALARM,
+	HWMON_T_INPUT | HWMON_T_MAX | HWMON_T_MAX_HYST | HWMON_T_CRIT |
+		HWMON_T_CRIT_HYST | HWMON_T_FAULT | HWMON_T_MAX_ALARM |
+		HWMON_T_CRIT_ALARM | HWMON_T_TYPE | HWMON_T_OFFSET,
+	0
+};
+
+static const struct hwmon_channel_info lm95245_temp = {
+	.type = hwmon_temp,
+	.config = lm95245_temp_config,
+};
+
+static const struct hwmon_channel_info *lm95245_info[] = {
+	&lm95245_chip,
+	&lm95245_temp,
+	NULL
+};
+
+static const struct hwmon_ops lm95245_hwmon_ops = {
+	.is_visible = lm95245_is_visible,
+	.read = lm95245_read,
+	.write = lm95245_write,
+};
+
+static const struct hwmon_chip_info lm95245_chip_info = {
+	.ops = &lm95245_hwmon_ops,
+	.info = lm95245_info,
+};
+
 static int lm95245_probe(struct i2c_client *client,
 			 const struct i2c_device_id *id)
 {
 	struct device *dev = &client->dev;
 	struct lm95245_data *data;
 	struct device *hwmon_dev;
+	int ret;
 
 	data = devm_kzalloc(dev, sizeof(struct lm95245_data), GFP_KERNEL);
 	if (!data)
 		return -ENOMEM;
 
-	data->client = client;
+	data->regmap = devm_regmap_init_i2c(client, &lm95245_regmap_config);
+	if (IS_ERR(data->regmap))
+		return PTR_ERR(data->regmap);
+
 	mutex_init(&data->update_lock);
 
 	/* Initialize the LM95245 chip */
-	lm95245_init_client(client, data);
-
-	hwmon_dev = devm_hwmon_device_register_with_groups(dev, client->name,
-							   data,
-							   lm95245_groups);
+	ret = lm95245_init_client(data);
+	if (ret < 0)
+		return ret;
+
+	hwmon_dev = devm_hwmon_device_register_with_info(dev, client->name,
+							 data,
+							 &lm95245_chip_info,
+							 NULL);
 	return PTR_ERR_OR_ZERO(hwmon_dev);
 }
 
diff --git a/drivers/hwmon/ltc4151.c b/drivers/hwmon/ltc4151.c
index c86a18402496..8445c9fd946b 100644
--- a/drivers/hwmon/ltc4151.c
+++ b/drivers/hwmon/ltc4151.c
@@ -30,6 +30,7 @@
 
 #include <linux/kernel.h>
 #include <linux/module.h>
+#include <linux/of.h>
 #include <linux/init.h>
 #include <linux/err.h>
 #include <linux/slab.h>
@@ -52,6 +53,7 @@ struct ltc4151_data {
 	struct mutex update_lock;
 	bool valid;
 	unsigned long last_updated; /* in jiffies */
+	unsigned int shunt; /* in micro ohms */
 
 	/* Registers */
 	u8 regs[6];
@@ -111,9 +113,9 @@ static int ltc4151_get_value(struct ltc4151_data *data, u8 reg)
 	case LTC4151_SENSE_H:
 		/*
 		 * 20uV resolution. Convert to current as measured with
-		 * an 1 mOhm sense resistor, in mA.
+		 * a given sense resistor, in mA.
 		 */
-		val = val * 20;
+		val = val * 20 * 1000 / data->shunt;
 		break;
 	case LTC4151_VIN_H:
 		/* 25 mV per increment */
@@ -176,6 +178,7 @@ static int ltc4151_probe(struct i2c_client *client,
 	struct device *dev = &client->dev;
 	struct ltc4151_data *data;
 	struct device *hwmon_dev;
+	u32 shunt;
 
 	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE_DATA))
 		return -ENODEV;
@@ -184,6 +187,15 @@ static int ltc4151_probe(struct i2c_client *client,
 	if (!data)
 		return -ENOMEM;
 
+	if (of_property_read_u32(client->dev.of_node,
+				 "shunt-resistor-micro-ohms", &shunt))
+		shunt = 1000; /* 1 mOhm if not set via DT */
+
+	if (shunt == 0)
+		return -EINVAL;
+
+	data->shunt = shunt;
+
 	data->client = client;
 	mutex_init(&data->update_lock);
 
@@ -199,10 +211,16 @@ static const struct i2c_device_id ltc4151_id[] = {
 };
 MODULE_DEVICE_TABLE(i2c, ltc4151_id);
 
+static const struct of_device_id ltc4151_match[] = {
+	{ .compatible = "lltc,ltc4151" },
+	{},
+};
+
 /* This is the driver that will be inserted */
 static struct i2c_driver ltc4151_driver = {
 	.driver = {
 		.name	= "ltc4151",
+		.of_match_table = of_match_ptr(ltc4151_match),
 	},
 	.probe		= ltc4151_probe,
 	.id_table	= ltc4151_id,
diff --git a/drivers/hwmon/ltc4245.c b/drivers/hwmon/ltc4245.c
index 681b5b7b3c3b..4680d89556ce 100644
--- a/drivers/hwmon/ltc4245.c
+++ b/drivers/hwmon/ltc4245.c
@@ -16,6 +16,7 @@
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/init.h>
+#include <linux/bitops.h>
 #include <linux/err.h>
 #include <linux/slab.h>
 #include <linux/i2c.h>
@@ -53,8 +54,6 @@ enum ltc4245_cmd {
 struct ltc4245_data {
 	struct i2c_client *client;
 
-	const struct attribute_group *groups[3];
-
 	struct mutex update_lock;
 	bool valid;
 	unsigned long last_updated; /* in jiffies */
@@ -162,7 +161,7 @@ static struct ltc4245_data *ltc4245_update_device(struct device *dev)
 		ltc4245_update_gpios(dev);
 
 		data->last_updated = jiffies;
-		data->valid = 1;
+		data->valid = true;
 	}
 
 	mutex_unlock(&data->update_lock);
@@ -256,213 +255,204 @@ static unsigned int ltc4245_get_current(struct device *dev, u8 reg)
 	return curr;
 }
 
-static ssize_t ltc4245_show_voltage(struct device *dev,
-				    struct device_attribute *da,
-				    char *buf)
-{
-	struct sensor_device_attribute *attr = to_sensor_dev_attr(da);
-	const int voltage = ltc4245_get_voltage(dev, attr->index);
+/* Map from voltage channel index to voltage register */
 
-	return snprintf(buf, PAGE_SIZE, "%d\n", voltage);
-}
+static const s8 ltc4245_in_regs[] = {
+	LTC4245_12VIN, LTC4245_5VIN, LTC4245_3VIN, LTC4245_VEEIN,
+	LTC4245_12VOUT, LTC4245_5VOUT, LTC4245_3VOUT, LTC4245_VEEOUT,
+};
+
+/* Map from current channel index to current register */
 
-static ssize_t ltc4245_show_current(struct device *dev,
-				    struct device_attribute *da,
-				    char *buf)
+static const s8 ltc4245_curr_regs[] = {
+	LTC4245_12VSENSE, LTC4245_5VSENSE, LTC4245_3VSENSE, LTC4245_VEESENSE,
+};
+
+static int ltc4245_read_curr(struct device *dev, u32 attr, int channel,
+			     long *val)
 {
-	struct sensor_device_attribute *attr = to_sensor_dev_attr(da);
-	const unsigned int curr = ltc4245_get_current(dev, attr->index);
+	struct ltc4245_data *data = ltc4245_update_device(dev);
 
-	return snprintf(buf, PAGE_SIZE, "%u\n", curr);
+	switch (attr) {
+	case hwmon_curr_input:
+		*val = ltc4245_get_current(dev, ltc4245_curr_regs[channel]);
+		return 0;
+	case hwmon_curr_max_alarm:
+		*val = !!(data->cregs[LTC4245_FAULT1] & BIT(channel + 4));
+		return 0;
+	default:
+		return -EOPNOTSUPP;
+	}
 }
 
-static ssize_t ltc4245_show_power(struct device *dev,
-				  struct device_attribute *da,
-				  char *buf)
+static int ltc4245_read_in(struct device *dev, u32 attr, int channel, long *val)
 {
-	struct sensor_device_attribute *attr = to_sensor_dev_attr(da);
-	const unsigned int curr = ltc4245_get_current(dev, attr->index);
-	const int output_voltage = ltc4245_get_voltage(dev, attr->index+1);
+	struct ltc4245_data *data = ltc4245_update_device(dev);
 
-	/* current in mA * voltage in mV == power in uW */
-	const unsigned int power = abs(output_voltage * curr);
+	switch (attr) {
+	case hwmon_in_input:
+		if (channel < 8) {
+			*val = ltc4245_get_voltage(dev,
+						ltc4245_in_regs[channel]);
+		} else {
+			int regval = data->gpios[channel - 8];
+
+			if (regval < 0)
+				return regval;
+			*val = regval * 10;
+		}
+		return 0;
+	case hwmon_in_min_alarm:
+		if (channel < 4)
+			*val = !!(data->cregs[LTC4245_FAULT1] & BIT(channel));
+		else
+			*val = !!(data->cregs[LTC4245_FAULT2] &
+				  BIT(channel - 4));
+		return 0;
+	default:
+		return -EOPNOTSUPP;
+	}
+}
 
-	return snprintf(buf, PAGE_SIZE, "%u\n", power);
+static int ltc4245_read_power(struct device *dev, u32 attr, int channel,
+			      long *val)
+{
+	unsigned long curr;
+	long voltage;
+
+	switch (attr) {
+	case hwmon_power_input:
+		(void)ltc4245_update_device(dev);
+		curr = ltc4245_get_current(dev, ltc4245_curr_regs[channel]);
+		voltage = ltc4245_get_voltage(dev, ltc4245_in_regs[channel]);
+		*val = abs(curr * voltage);
+		return 0;
+	default:
+		return -EOPNOTSUPP;
+	}
 }
 
-static ssize_t ltc4245_show_alarm(struct device *dev,
-					  struct device_attribute *da,
-					  char *buf)
+static int ltc4245_read(struct device *dev, enum hwmon_sensor_types type,
+			u32 attr, int channel, long *val)
 {
-	struct sensor_device_attribute_2 *attr = to_sensor_dev_attr_2(da);
-	struct ltc4245_data *data = ltc4245_update_device(dev);
-	const u8 reg = data->cregs[attr->index];
-	const u32 mask = attr->nr;
 
-	return snprintf(buf, PAGE_SIZE, "%u\n", (reg & mask) ? 1 : 0);
+	switch (type) {
+	case hwmon_curr:
+		return ltc4245_read_curr(dev, attr, channel, val);
+	case hwmon_power:
+		return ltc4245_read_power(dev, attr, channel, val);
+	case hwmon_in:
+		return ltc4245_read_in(dev, attr, channel - 1, val);
+	default:
+		return -EOPNOTSUPP;
+	}
 }
 
-static ssize_t ltc4245_show_gpio(struct device *dev,
-				 struct device_attribute *da,
-				 char *buf)
+static umode_t ltc4245_is_visible(const void *_data,
+				  enum hwmon_sensor_types type,
+				  u32 attr, int channel)
 {
-	struct sensor_device_attribute *attr = to_sensor_dev_attr(da);
-	struct ltc4245_data *data = ltc4245_update_device(dev);
-	int val = data->gpios[attr->index];
+	const struct ltc4245_data *data = _data;
+
+	switch (type) {
+	case hwmon_in:
+		if (channel == 0)
+			return 0;
+		switch (attr) {
+		case hwmon_in_input:
+			if (channel > 9 && !data->use_extra_gpios)
+				return 0;
+			return S_IRUGO;
+		case hwmon_in_min_alarm:
+			if (channel > 8)
+				return 0;
+			return S_IRUGO;
+		default:
+			return 0;
+		}
+	case hwmon_curr:
+		switch (attr) {
+		case hwmon_curr_input:
+		case hwmon_curr_max_alarm:
+			return S_IRUGO;
+		default:
+			return 0;
+		}
+	case hwmon_power:
+		switch (attr) {
+		case hwmon_power_input:
+			return S_IRUGO;
+		default:
+			return 0;
+		}
+	default:
+		return 0;
+	}
+}
 
-	/* handle stale GPIO's */
-	if (val < 0)
-		return val;
+static const u32 ltc4245_in_config[] = {
+	HWMON_I_INPUT,			/* dummy, skipped in is_visible */
+	HWMON_I_INPUT | HWMON_I_MIN_ALARM,
+	HWMON_I_INPUT | HWMON_I_MIN_ALARM,
+	HWMON_I_INPUT | HWMON_I_MIN_ALARM,
+	HWMON_I_INPUT | HWMON_I_MIN_ALARM,
+	HWMON_I_INPUT | HWMON_I_MIN_ALARM,
+	HWMON_I_INPUT | HWMON_I_MIN_ALARM,
+	HWMON_I_INPUT | HWMON_I_MIN_ALARM,
+	HWMON_I_INPUT | HWMON_I_MIN_ALARM,
+	HWMON_I_INPUT,
+	HWMON_I_INPUT,
+	HWMON_I_INPUT,
+	0
+};
 
-	/* Convert to millivolts and print */
-	return snprintf(buf, PAGE_SIZE, "%u\n", val * 10);
-}
+static const struct hwmon_channel_info ltc4245_in = {
+	.type = hwmon_in,
+	.config = ltc4245_in_config,
+};
 
-/* Construct a sensor_device_attribute structure for each register */
-
-/* Input voltages */
-static SENSOR_DEVICE_ATTR(in1_input, S_IRUGO, ltc4245_show_voltage, NULL,
-			  LTC4245_12VIN);
-static SENSOR_DEVICE_ATTR(in2_input, S_IRUGO, ltc4245_show_voltage, NULL,
-			  LTC4245_5VIN);
-static SENSOR_DEVICE_ATTR(in3_input, S_IRUGO, ltc4245_show_voltage, NULL,
-			  LTC4245_3VIN);
-static SENSOR_DEVICE_ATTR(in4_input, S_IRUGO, ltc4245_show_voltage, NULL,
-			  LTC4245_VEEIN);
-
-/* Input undervoltage alarms */
-static SENSOR_DEVICE_ATTR_2(in1_min_alarm, S_IRUGO, ltc4245_show_alarm, NULL,
-			    1 << 0, LTC4245_FAULT1);
-static SENSOR_DEVICE_ATTR_2(in2_min_alarm, S_IRUGO, ltc4245_show_alarm, NULL,
-			    1 << 1, LTC4245_FAULT1);
-static SENSOR_DEVICE_ATTR_2(in3_min_alarm, S_IRUGO, ltc4245_show_alarm, NULL,
-			    1 << 2, LTC4245_FAULT1);
-static SENSOR_DEVICE_ATTR_2(in4_min_alarm, S_IRUGO, ltc4245_show_alarm, NULL,
-			    1 << 3, LTC4245_FAULT1);
-
-/* Currents (via sense resistor) */
-static SENSOR_DEVICE_ATTR(curr1_input, S_IRUGO, ltc4245_show_current, NULL,
-			  LTC4245_12VSENSE);
-static SENSOR_DEVICE_ATTR(curr2_input, S_IRUGO, ltc4245_show_current, NULL,
-			  LTC4245_5VSENSE);
-static SENSOR_DEVICE_ATTR(curr3_input, S_IRUGO, ltc4245_show_current, NULL,
-			  LTC4245_3VSENSE);
-static SENSOR_DEVICE_ATTR(curr4_input, S_IRUGO, ltc4245_show_current, NULL,
-			  LTC4245_VEESENSE);
-
-/* Overcurrent alarms */
-static SENSOR_DEVICE_ATTR_2(curr1_max_alarm, S_IRUGO, ltc4245_show_alarm, NULL,
-			    1 << 4, LTC4245_FAULT1);
-static SENSOR_DEVICE_ATTR_2(curr2_max_alarm, S_IRUGO, ltc4245_show_alarm, NULL,
-			    1 << 5, LTC4245_FAULT1);
-static SENSOR_DEVICE_ATTR_2(curr3_max_alarm, S_IRUGO, ltc4245_show_alarm, NULL,
-			    1 << 6, LTC4245_FAULT1);
-static SENSOR_DEVICE_ATTR_2(curr4_max_alarm, S_IRUGO, ltc4245_show_alarm, NULL,
-			    1 << 7, LTC4245_FAULT1);
-
-/* Output voltages */
-static SENSOR_DEVICE_ATTR(in5_input, S_IRUGO, ltc4245_show_voltage, NULL,
-			  LTC4245_12VOUT);
-static SENSOR_DEVICE_ATTR(in6_input, S_IRUGO, ltc4245_show_voltage, NULL,
-			  LTC4245_5VOUT);
-static SENSOR_DEVICE_ATTR(in7_input, S_IRUGO, ltc4245_show_voltage, NULL,
-			  LTC4245_3VOUT);
-static SENSOR_DEVICE_ATTR(in8_input, S_IRUGO, ltc4245_show_voltage, NULL,
-			  LTC4245_VEEOUT);
-
-/* Power Bad alarms */
-static SENSOR_DEVICE_ATTR_2(in5_min_alarm, S_IRUGO, ltc4245_show_alarm, NULL,
-			    1 << 0, LTC4245_FAULT2);
-static SENSOR_DEVICE_ATTR_2(in6_min_alarm, S_IRUGO, ltc4245_show_alarm, NULL,
-			    1 << 1, LTC4245_FAULT2);
-static SENSOR_DEVICE_ATTR_2(in7_min_alarm, S_IRUGO, ltc4245_show_alarm, NULL,
-			    1 << 2, LTC4245_FAULT2);
-static SENSOR_DEVICE_ATTR_2(in8_min_alarm, S_IRUGO, ltc4245_show_alarm, NULL,
-			    1 << 3, LTC4245_FAULT2);
-
-/* GPIO voltages */
-static SENSOR_DEVICE_ATTR(in9_input, S_IRUGO, ltc4245_show_gpio, NULL, 0);
-static SENSOR_DEVICE_ATTR(in10_input, S_IRUGO, ltc4245_show_gpio, NULL, 1);
-static SENSOR_DEVICE_ATTR(in11_input, S_IRUGO, ltc4245_show_gpio, NULL, 2);
-
-/* Power Consumption (virtual) */
-static SENSOR_DEVICE_ATTR(power1_input, S_IRUGO, ltc4245_show_power, NULL,
-			  LTC4245_12VSENSE);
-static SENSOR_DEVICE_ATTR(power2_input, S_IRUGO, ltc4245_show_power, NULL,
-			  LTC4245_5VSENSE);
-static SENSOR_DEVICE_ATTR(power3_input, S_IRUGO, ltc4245_show_power, NULL,
-			  LTC4245_3VSENSE);
-static SENSOR_DEVICE_ATTR(power4_input, S_IRUGO, ltc4245_show_power, NULL,
-			  LTC4245_VEESENSE);
+static const u32 ltc4245_curr_config[] = {
+	HWMON_C_INPUT | HWMON_C_MAX_ALARM,
+	HWMON_C_INPUT | HWMON_C_MAX_ALARM,
+	HWMON_C_INPUT | HWMON_C_MAX_ALARM,
+	HWMON_C_INPUT | HWMON_C_MAX_ALARM,
+	0
+};
 
-/*
- * Finally, construct an array of pointers to members of the above objects,
- * as required for sysfs_create_group()
- */
-static struct attribute *ltc4245_std_attributes[] = {
-	&sensor_dev_attr_in1_input.dev_attr.attr,
-	&sensor_dev_attr_in2_input.dev_attr.attr,
-	&sensor_dev_attr_in3_input.dev_attr.attr,
-	&sensor_dev_attr_in4_input.dev_attr.attr,
-
-	&sensor_dev_attr_in1_min_alarm.dev_attr.attr,
-	&sensor_dev_attr_in2_min_alarm.dev_attr.attr,
-	&sensor_dev_attr_in3_min_alarm.dev_attr.attr,
-	&sensor_dev_attr_in4_min_alarm.dev_attr.attr,
-
-	&sensor_dev_attr_curr1_input.dev_attr.attr,
-	&sensor_dev_attr_curr2_input.dev_attr.attr,
-	&sensor_dev_attr_curr3_input.dev_attr.attr,
-	&sensor_dev_attr_curr4_input.dev_attr.attr,
-
-	&sensor_dev_attr_curr1_max_alarm.dev_attr.attr,
-	&sensor_dev_attr_curr2_max_alarm.dev_attr.attr,
-	&sensor_dev_attr_curr3_max_alarm.dev_attr.attr,
-	&sensor_dev_attr_curr4_max_alarm.dev_attr.attr,
-
-	&sensor_dev_attr_in5_input.dev_attr.attr,
-	&sensor_dev_attr_in6_input.dev_attr.attr,
-	&sensor_dev_attr_in7_input.dev_attr.attr,
-	&sensor_dev_attr_in8_input.dev_attr.attr,
-
-	&sensor_dev_attr_in5_min_alarm.dev_attr.attr,
-	&sensor_dev_attr_in6_min_alarm.dev_attr.attr,
-	&sensor_dev_attr_in7_min_alarm.dev_attr.attr,
-	&sensor_dev_attr_in8_min_alarm.dev_attr.attr,
-
-	&sensor_dev_attr_in9_input.dev_attr.attr,
-
-	&sensor_dev_attr_power1_input.dev_attr.attr,
-	&sensor_dev_attr_power2_input.dev_attr.attr,
-	&sensor_dev_attr_power3_input.dev_attr.attr,
-	&sensor_dev_attr_power4_input.dev_attr.attr,
-
-	NULL,
+static const struct hwmon_channel_info ltc4245_curr = {
+	.type = hwmon_curr,
+	.config = ltc4245_curr_config,
 };
 
-static struct attribute *ltc4245_gpio_attributes[] = {
-	&sensor_dev_attr_in10_input.dev_attr.attr,
-	&sensor_dev_attr_in11_input.dev_attr.attr,
-	NULL,
+static const u32 ltc4245_power_config[] = {
+	HWMON_P_INPUT,
+	HWMON_P_INPUT,
+	HWMON_P_INPUT,
+	HWMON_P_INPUT,
+	0
 };
 
-static const struct attribute_group ltc4245_std_group = {
-	.attrs = ltc4245_std_attributes,
+static const struct hwmon_channel_info ltc4245_power = {
+	.type = hwmon_power,
+	.config = ltc4245_power_config,
 };
 
-static const struct attribute_group ltc4245_gpio_group = {
-	.attrs = ltc4245_gpio_attributes,
+static const struct hwmon_channel_info *ltc4245_info[] = {
+	&ltc4245_in,
+	&ltc4245_curr,
+	&ltc4245_power,
+	NULL
 };
 
-static void ltc4245_sysfs_add_groups(struct ltc4245_data *data)
-{
-	/* standard sysfs attributes */
-	data->groups[0] = &ltc4245_std_group;
+static const struct hwmon_ops ltc4245_hwmon_ops = {
+	.is_visible = ltc4245_is_visible,
+	.read = ltc4245_read,
+};
 
-	/* if we're using the extra gpio support, register it's attributes */
-	if (data->use_extra_gpios)
-		data->groups[1] = &ltc4245_gpio_group;
-}
+static const struct hwmon_chip_info ltc4245_chip_info = {
+	.ops = &ltc4245_hwmon_ops,
+	.info = ltc4245_info,
+};
 
 static bool ltc4245_use_extra_gpios(struct i2c_client *client)
 {
@@ -502,12 +492,10 @@ static int ltc4245_probe(struct i2c_client *client,
 	i2c_smbus_write_byte_data(client, LTC4245_FAULT1, 0x00);
 	i2c_smbus_write_byte_data(client, LTC4245_FAULT2, 0x00);
 
-	/* Add sysfs hooks */
-	ltc4245_sysfs_add_groups(data);
-
-	hwmon_dev = devm_hwmon_device_register_with_groups(&client->dev,
-							   client->name, data,
-							   data->groups);
+	hwmon_dev = devm_hwmon_device_register_with_info(&client->dev,
+							 client->name, data,
+							 &ltc4245_chip_info,
+							 NULL);
 	return PTR_ERR_OR_ZERO(hwmon_dev);
 }
 
diff --git a/drivers/hwmon/max31790.c b/drivers/hwmon/max31790.c
index 69c0ac80a946..c1b9275978f9 100644
--- a/drivers/hwmon/max31790.c
+++ b/drivers/hwmon/max31790.c
@@ -17,7 +17,6 @@
 
 #include <linux/err.h>
 #include <linux/hwmon.h>
-#include <linux/hwmon-sysfs.h>
 #include <linux/i2c.h>
 #include <linux/init.h>
 #include <linux/jiffies.h>
@@ -169,362 +168,290 @@ static u8 bits_for_tach_period(int rpm)
 	return bits;
 }
 
-static ssize_t get_fan(struct device *dev,
-		       struct device_attribute *devattr, char *buf)
+static int max31790_read_fan(struct device *dev, u32 attr, int channel,
+			     long *val)
 {
-	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
 	struct max31790_data *data = max31790_update_device(dev);
 	int sr, rpm;
 
 	if (IS_ERR(data))
 		return PTR_ERR(data);
 
-	sr = get_tach_period(data->fan_dynamics[attr->index]);
-	rpm = RPM_FROM_REG(data->tach[attr->index], sr);
-
-	return sprintf(buf, "%d\n", rpm);
-}
-
-static ssize_t get_fan_target(struct device *dev,
-			      struct device_attribute *devattr, char *buf)
-{
-	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
-	struct max31790_data *data = max31790_update_device(dev);
-	int sr, rpm;
-
-	if (IS_ERR(data))
-		return PTR_ERR(data);
-
-	sr = get_tach_period(data->fan_dynamics[attr->index]);
-	rpm = RPM_FROM_REG(data->target_count[attr->index], sr);
-
-	return sprintf(buf, "%d\n", rpm);
+	switch (attr) {
+	case hwmon_fan_input:
+		sr = get_tach_period(data->fan_dynamics[channel]);
+		rpm = RPM_FROM_REG(data->tach[channel], sr);
+		*val = rpm;
+		return 0;
+	case hwmon_fan_target:
+		sr = get_tach_period(data->fan_dynamics[channel]);
+		rpm = RPM_FROM_REG(data->target_count[channel], sr);
+		*val = rpm;
+		return 0;
+	case hwmon_fan_fault:
+		*val = !!(data->fault_status & (1 << channel));
+		return 0;
+	default:
+		return -EOPNOTSUPP;
+	}
 }
 
-static ssize_t set_fan_target(struct device *dev,
-			      struct device_attribute *devattr,
-			      const char *buf, size_t count)
+static int max31790_write_fan(struct device *dev, u32 attr, int channel,
+			      long val)
 {
-	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
 	struct max31790_data *data = dev_get_drvdata(dev);
 	struct i2c_client *client = data->client;
+	int target_count;
+	int err = 0;
 	u8 bits;
 	int sr;
-	int target_count;
-	unsigned long rpm;
-	int err;
-
-	err = kstrtoul(buf, 10, &rpm);
-	if (err)
-		return err;
 
 	mutex_lock(&data->update_lock);
 
-	rpm = clamp_val(rpm, FAN_RPM_MIN, FAN_RPM_MAX);
-	bits = bits_for_tach_period(rpm);
-	data->fan_dynamics[attr->index] =
-			((data->fan_dynamics[attr->index]
-			  & ~MAX31790_FAN_DYN_SR_MASK)
-			 | (bits << MAX31790_FAN_DYN_SR_SHIFT));
-	err = i2c_smbus_write_byte_data(client,
-			MAX31790_REG_FAN_DYNAMICS(attr->index),
-			data->fan_dynamics[attr->index]);
-
-	if (err < 0) {
-		mutex_unlock(&data->update_lock);
-		return err;
+	switch (attr) {
+	case hwmon_fan_target:
+		val = clamp_val(val, FAN_RPM_MIN, FAN_RPM_MAX);
+		bits = bits_for_tach_period(val);
+		data->fan_dynamics[channel] =
+			((data->fan_dynamics[channel] &
+			  ~MAX31790_FAN_DYN_SR_MASK) |
+			 (bits << MAX31790_FAN_DYN_SR_SHIFT));
+		err = i2c_smbus_write_byte_data(client,
+					MAX31790_REG_FAN_DYNAMICS(channel),
+					data->fan_dynamics[channel]);
+		if (err < 0)
+			break;
+
+		sr = get_tach_period(data->fan_dynamics[channel]);
+		target_count = RPM_TO_REG(val, sr);
+		target_count = clamp_val(target_count, 0x1, 0x7FF);
+
+		data->target_count[channel] = target_count << 5;
+
+		err = i2c_smbus_write_word_swapped(client,
+					MAX31790_REG_TARGET_COUNT(channel),
+					data->target_count[channel]);
+		break;
+	default:
+		err = -EOPNOTSUPP;
+		break;
 	}
 
-	sr = get_tach_period(data->fan_dynamics[attr->index]);
-	target_count = RPM_TO_REG(rpm, sr);
-	target_count = clamp_val(target_count, 0x1, 0x7FF);
-
-	data->target_count[attr->index] = target_count << 5;
-
-	err = i2c_smbus_write_word_swapped(client,
-			MAX31790_REG_TARGET_COUNT(attr->index),
-			data->target_count[attr->index]);
-
 	mutex_unlock(&data->update_lock);
 
-	if (err < 0)
-		return err;
+	return err;
+}
 
-	return count;
+static umode_t max31790_fan_is_visible(const void *_data, u32 attr, int channel)
+{
+	const struct max31790_data *data = _data;
+	u8 fan_config = data->fan_config[channel % NR_CHANNEL];
+
+	switch (attr) {
+	case hwmon_fan_input:
+	case hwmon_fan_fault:
+		if (channel < NR_CHANNEL ||
+		    (fan_config & MAX31790_FAN_CFG_TACH_INPUT))
+			return S_IRUGO;
+		return 0;
+	case hwmon_fan_target:
+		if (channel < NR_CHANNEL &&
+		    !(fan_config & MAX31790_FAN_CFG_TACH_INPUT))
+			return S_IRUGO | S_IWUSR;
+		return 0;
+	default:
+		return 0;
+	}
 }
 
-static ssize_t get_pwm(struct device *dev,
-		       struct device_attribute *devattr, char *buf)
+static int max31790_read_pwm(struct device *dev, u32 attr, int channel,
+			     long *val)
 {
-	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
 	struct max31790_data *data = max31790_update_device(dev);
-	int pwm;
+	u8 fan_config;
 
 	if (IS_ERR(data))
 		return PTR_ERR(data);
 
-	pwm = data->pwm[attr->index] >> 8;
+	fan_config = data->fan_config[channel];
 
-	return sprintf(buf, "%d\n", pwm);
+	switch (attr) {
+	case hwmon_pwm_input:
+		*val = data->pwm[channel] >> 8;
+		return 0;
+	case hwmon_pwm_enable:
+		if (fan_config & MAX31790_FAN_CFG_RPM_MODE)
+			*val = 2;
+		else if (fan_config & MAX31790_FAN_CFG_TACH_INPUT_EN)
+			*val = 1;
+		else
+			*val = 0;
+		return 0;
+	default:
+		return -EOPNOTSUPP;
+	}
 }
 
-static ssize_t set_pwm(struct device *dev,
-		       struct device_attribute *devattr,
-		       const char *buf, size_t count)
+static int max31790_write_pwm(struct device *dev, u32 attr, int channel,
+			      long val)
 {
-	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
 	struct max31790_data *data = dev_get_drvdata(dev);
 	struct i2c_client *client = data->client;
-	unsigned long pwm;
-	int err;
-
-	err = kstrtoul(buf, 10, &pwm);
-	if (err)
-		return err;
-
-	if (pwm > 255)
-		return -EINVAL;
+	u8 fan_config;
+	int err = 0;
 
 	mutex_lock(&data->update_lock);
 
-	data->pwm[attr->index] = pwm << 8;
-	err = i2c_smbus_write_word_swapped(client,
-			MAX31790_REG_PWMOUT(attr->index),
-			data->pwm[attr->index]);
+	switch (attr) {
+	case hwmon_pwm_input:
+		if (val < 0 || val > 255) {
+			err = -EINVAL;
+			break;
+		}
+		data->pwm[channel] = val << 8;
+		err = i2c_smbus_write_word_swapped(client,
+						   MAX31790_REG_PWMOUT(channel),
+						   val);
+		break;
+	case hwmon_pwm_enable:
+		fan_config = data->fan_config[channel];
+		if (val == 0) {
+			fan_config &= ~(MAX31790_FAN_CFG_TACH_INPUT_EN |
+					MAX31790_FAN_CFG_RPM_MODE);
+		} else if (val == 1) {
+			fan_config = (fan_config |
+				      MAX31790_FAN_CFG_TACH_INPUT_EN) &
+				     ~MAX31790_FAN_CFG_RPM_MODE;
+		} else if (val == 2) {
+			fan_config |= MAX31790_FAN_CFG_TACH_INPUT_EN |
+				      MAX31790_FAN_CFG_RPM_MODE;
+		} else {
+			err = -EINVAL;
+			break;
+		}
+		data->fan_config[channel] = fan_config;
+		err = i2c_smbus_write_byte_data(client,
+					MAX31790_REG_FAN_CONFIG(channel),
+					fan_config);
+		break;
+	default:
+		err = -EOPNOTSUPP;
+		break;
+	}
 
 	mutex_unlock(&data->update_lock);
 
-	if (err < 0)
-		return err;
-
-	return count;
+	return err;
 }
 
-static ssize_t get_pwm_enable(struct device *dev,
-			      struct device_attribute *devattr, char *buf)
+static umode_t max31790_pwm_is_visible(const void *_data, u32 attr, int channel)
 {
-	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
-	struct max31790_data *data = max31790_update_device(dev);
-	int mode;
-
-	if (IS_ERR(data))
-		return PTR_ERR(data);
-
-	if (data->fan_config[attr->index] & MAX31790_FAN_CFG_RPM_MODE)
-		mode = 2;
-	else if (data->fan_config[attr->index] & MAX31790_FAN_CFG_TACH_INPUT_EN)
-		mode = 1;
-	else
-		mode = 0;
-
-	return sprintf(buf, "%d\n", mode);
+	const struct max31790_data *data = _data;
+	u8 fan_config = data->fan_config[channel];
+
+	switch (attr) {
+	case hwmon_pwm_input:
+	case hwmon_pwm_enable:
+		if (!(fan_config & MAX31790_FAN_CFG_TACH_INPUT))
+			return S_IRUGO | S_IWUSR;
+		return 0;
+	default:
+		return 0;
+	}
 }
 
-static ssize_t set_pwm_enable(struct device *dev,
-			      struct device_attribute *devattr,
-			      const char *buf, size_t count)
+static int max31790_read(struct device *dev, enum hwmon_sensor_types type,
+			 u32 attr, int channel, long *val)
 {
-	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
-	struct max31790_data *data = dev_get_drvdata(dev);
-	struct i2c_client *client = data->client;
-	unsigned long mode;
-	int err;
-
-	err = kstrtoul(buf, 10, &mode);
-	if (err)
-		return err;
-
-	switch (mode) {
-	case 0:
-		data->fan_config[attr->index] =
-			data->fan_config[attr->index]
-			& ~(MAX31790_FAN_CFG_TACH_INPUT_EN
-			    | MAX31790_FAN_CFG_RPM_MODE);
-		break;
-	case 1:
-		data->fan_config[attr->index] =
-			(data->fan_config[attr->index]
-			 | MAX31790_FAN_CFG_TACH_INPUT_EN)
-			& ~MAX31790_FAN_CFG_RPM_MODE;
-		break;
-	case 2:
-		data->fan_config[attr->index] =
-			data->fan_config[attr->index]
-			| MAX31790_FAN_CFG_TACH_INPUT_EN
-			| MAX31790_FAN_CFG_RPM_MODE;
-		break;
+	switch (type) {
+	case hwmon_fan:
+		return max31790_read_fan(dev, attr, channel, val);
+	case hwmon_pwm:
+		return max31790_read_pwm(dev, attr, channel, val);
 	default:
-		return -EINVAL;
+		return -EOPNOTSUPP;
 	}
-
-	mutex_lock(&data->update_lock);
-
-	err = i2c_smbus_write_byte_data(client,
-			MAX31790_REG_FAN_CONFIG(attr->index),
-			data->fan_config[attr->index]);
-
-	mutex_unlock(&data->update_lock);
-
-	if (err < 0)
-		return err;
-
-	return count;
 }
 
-static ssize_t get_fan_fault(struct device *dev,
-			     struct device_attribute *devattr, char *buf)
+static int max31790_write(struct device *dev, enum hwmon_sensor_types type,
+			  u32 attr, int channel, long val)
 {
-	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
-	struct max31790_data *data = max31790_update_device(dev);
-	int fault;
-
-	if (IS_ERR(data))
-		return PTR_ERR(data);
-
-	fault = !!(data->fault_status & (1 << attr->index));
+	switch (type) {
+	case hwmon_fan:
+		return max31790_write_fan(dev, attr, channel, val);
+	case hwmon_pwm:
+		return max31790_write_pwm(dev, attr, channel, val);
+	default:
+		return -EOPNOTSUPP;
+	}
+}
 
-	return sprintf(buf, "%d\n", fault);
+static umode_t max31790_is_visible(const void *data,
+				   enum hwmon_sensor_types type,
+				   u32 attr, int channel)
+{
+	switch (type) {
+	case hwmon_fan:
+		return max31790_fan_is_visible(data, attr, channel);
+	case hwmon_pwm:
+		return max31790_pwm_is_visible(data, attr, channel);
+	default:
+		return 0;
+	}
 }
 
-static SENSOR_DEVICE_ATTR(fan1_input, S_IRUGO, get_fan, NULL, 0);
-static SENSOR_DEVICE_ATTR(fan2_input, S_IRUGO, get_fan, NULL, 1);
-static SENSOR_DEVICE_ATTR(fan3_input, S_IRUGO, get_fan, NULL, 2);
-static SENSOR_DEVICE_ATTR(fan4_input, S_IRUGO, get_fan, NULL, 3);
-static SENSOR_DEVICE_ATTR(fan5_input, S_IRUGO, get_fan, NULL, 4);
-static SENSOR_DEVICE_ATTR(fan6_input, S_IRUGO, get_fan, NULL, 5);
-
-static SENSOR_DEVICE_ATTR(fan1_fault, S_IRUGO, get_fan_fault, NULL, 0);
-static SENSOR_DEVICE_ATTR(fan2_fault, S_IRUGO, get_fan_fault, NULL, 1);
-static SENSOR_DEVICE_ATTR(fan3_fault, S_IRUGO, get_fan_fault, NULL, 2);
-static SENSOR_DEVICE_ATTR(fan4_fault, S_IRUGO, get_fan_fault, NULL, 3);
-static SENSOR_DEVICE_ATTR(fan5_fault, S_IRUGO, get_fan_fault, NULL, 4);
-static SENSOR_DEVICE_ATTR(fan6_fault, S_IRUGO, get_fan_fault, NULL, 5);
-
-static SENSOR_DEVICE_ATTR(fan7_input, S_IRUGO, get_fan, NULL, 6);
-static SENSOR_DEVICE_ATTR(fan8_input, S_IRUGO, get_fan, NULL, 7);
-static SENSOR_DEVICE_ATTR(fan9_input, S_IRUGO, get_fan, NULL, 8);
-static SENSOR_DEVICE_ATTR(fan10_input, S_IRUGO, get_fan, NULL, 9);
-static SENSOR_DEVICE_ATTR(fan11_input, S_IRUGO, get_fan, NULL, 10);
-static SENSOR_DEVICE_ATTR(fan12_input, S_IRUGO, get_fan, NULL, 11);
-
-static SENSOR_DEVICE_ATTR(fan7_fault, S_IRUGO, get_fan_fault, NULL, 6);
-static SENSOR_DEVICE_ATTR(fan8_fault, S_IRUGO, get_fan_fault, NULL, 7);
-static SENSOR_DEVICE_ATTR(fan9_fault, S_IRUGO, get_fan_fault, NULL, 8);
-static SENSOR_DEVICE_ATTR(fan10_fault, S_IRUGO, get_fan_fault, NULL, 9);
-static SENSOR_DEVICE_ATTR(fan11_fault, S_IRUGO, get_fan_fault, NULL, 10);
-static SENSOR_DEVICE_ATTR(fan12_fault, S_IRUGO, get_fan_fault, NULL, 11);
-
-static SENSOR_DEVICE_ATTR(fan1_target, S_IWUSR | S_IRUGO,
-		get_fan_target, set_fan_target, 0);
-static SENSOR_DEVICE_ATTR(fan2_target, S_IWUSR | S_IRUGO,
-		get_fan_target, set_fan_target, 1);
-static SENSOR_DEVICE_ATTR(fan3_target, S_IWUSR | S_IRUGO,
-		get_fan_target, set_fan_target, 2);
-static SENSOR_DEVICE_ATTR(fan4_target, S_IWUSR | S_IRUGO,
-		get_fan_target, set_fan_target, 3);
-static SENSOR_DEVICE_ATTR(fan5_target, S_IWUSR | S_IRUGO,
-		get_fan_target, set_fan_target, 4);
-static SENSOR_DEVICE_ATTR(fan6_target, S_IWUSR | S_IRUGO,
-		get_fan_target, set_fan_target, 5);
-
-static SENSOR_DEVICE_ATTR(pwm1, S_IWUSR | S_IRUGO, get_pwm, set_pwm, 0);
-static SENSOR_DEVICE_ATTR(pwm2, S_IWUSR | S_IRUGO, get_pwm, set_pwm, 1);
-static SENSOR_DEVICE_ATTR(pwm3, S_IWUSR | S_IRUGO, get_pwm, set_pwm, 2);
-static SENSOR_DEVICE_ATTR(pwm4, S_IWUSR | S_IRUGO, get_pwm, set_pwm, 3);
-static SENSOR_DEVICE_ATTR(pwm5, S_IWUSR | S_IRUGO, get_pwm, set_pwm, 4);
-static SENSOR_DEVICE_ATTR(pwm6, S_IWUSR | S_IRUGO, get_pwm, set_pwm, 5);
-
-static SENSOR_DEVICE_ATTR(pwm1_enable, S_IWUSR | S_IRUGO,
-		get_pwm_enable, set_pwm_enable, 0);
-static SENSOR_DEVICE_ATTR(pwm2_enable, S_IWUSR | S_IRUGO,
-		get_pwm_enable, set_pwm_enable, 1);
-static SENSOR_DEVICE_ATTR(pwm3_enable, S_IWUSR | S_IRUGO,
-		get_pwm_enable, set_pwm_enable, 2);
-static SENSOR_DEVICE_ATTR(pwm4_enable, S_IWUSR | S_IRUGO,
-		get_pwm_enable, set_pwm_enable, 3);
-static SENSOR_DEVICE_ATTR(pwm5_enable, S_IWUSR | S_IRUGO,
-		get_pwm_enable, set_pwm_enable, 4);
-static SENSOR_DEVICE_ATTR(pwm6_enable, S_IWUSR | S_IRUGO,
-		get_pwm_enable, set_pwm_enable, 5);
-
-static struct attribute *max31790_attrs[] = {
-	&sensor_dev_attr_fan1_input.dev_attr.attr,
-	&sensor_dev_attr_fan2_input.dev_attr.attr,
-	&sensor_dev_attr_fan3_input.dev_attr.attr,
-	&sensor_dev_attr_fan4_input.dev_attr.attr,
-	&sensor_dev_attr_fan5_input.dev_attr.attr,
-	&sensor_dev_attr_fan6_input.dev_attr.attr,
-
-	&sensor_dev_attr_fan1_fault.dev_attr.attr,
-	&sensor_dev_attr_fan2_fault.dev_attr.attr,
-	&sensor_dev_attr_fan3_fault.dev_attr.attr,
-	&sensor_dev_attr_fan4_fault.dev_attr.attr,
-	&sensor_dev_attr_fan5_fault.dev_attr.attr,
-	&sensor_dev_attr_fan6_fault.dev_attr.attr,
-
-	&sensor_dev_attr_fan7_input.dev_attr.attr,
-	&sensor_dev_attr_fan8_input.dev_attr.attr,
-	&sensor_dev_attr_fan9_input.dev_attr.attr,
-	&sensor_dev_attr_fan10_input.dev_attr.attr,
-	&sensor_dev_attr_fan11_input.dev_attr.attr,
-	&sensor_dev_attr_fan12_input.dev_attr.attr,
-
-	&sensor_dev_attr_fan7_fault.dev_attr.attr,
-	&sensor_dev_attr_fan8_fault.dev_attr.attr,
-	&sensor_dev_attr_fan9_fault.dev_attr.attr,
-	&sensor_dev_attr_fan10_fault.dev_attr.attr,
-	&sensor_dev_attr_fan11_fault.dev_attr.attr,
-	&sensor_dev_attr_fan12_fault.dev_attr.attr,
-
-	&sensor_dev_attr_fan1_target.dev_attr.attr,
-	&sensor_dev_attr_fan2_target.dev_attr.attr,
-	&sensor_dev_attr_fan3_target.dev_attr.attr,
-	&sensor_dev_attr_fan4_target.dev_attr.attr,
-	&sensor_dev_attr_fan5_target.dev_attr.attr,
-	&sensor_dev_attr_fan6_target.dev_attr.attr,
-
-	&sensor_dev_attr_pwm1.dev_attr.attr,
-	&sensor_dev_attr_pwm2.dev_attr.attr,
-	&sensor_dev_attr_pwm3.dev_attr.attr,
-	&sensor_dev_attr_pwm4.dev_attr.attr,
-	&sensor_dev_attr_pwm5.dev_attr.attr,
-	&sensor_dev_attr_pwm6.dev_attr.attr,
-
-	&sensor_dev_attr_pwm1_enable.dev_attr.attr,
-	&sensor_dev_attr_pwm2_enable.dev_attr.attr,
-	&sensor_dev_attr_pwm3_enable.dev_attr.attr,
-	&sensor_dev_attr_pwm4_enable.dev_attr.attr,
-	&sensor_dev_attr_pwm5_enable.dev_attr.attr,
-	&sensor_dev_attr_pwm6_enable.dev_attr.attr,
-	NULL
+static const u32 max31790_fan_config[] = {
+	HWMON_F_INPUT | HWMON_F_TARGET | HWMON_F_FAULT,
+	HWMON_F_INPUT | HWMON_F_TARGET | HWMON_F_FAULT,
+	HWMON_F_INPUT | HWMON_F_TARGET | HWMON_F_FAULT,
+	HWMON_F_INPUT | HWMON_F_TARGET | HWMON_F_FAULT,
+	HWMON_F_INPUT | HWMON_F_TARGET | HWMON_F_FAULT,
+	HWMON_F_INPUT | HWMON_F_TARGET | HWMON_F_FAULT,
+	HWMON_F_INPUT | HWMON_F_FAULT,
+	HWMON_F_INPUT | HWMON_F_FAULT,
+	HWMON_F_INPUT | HWMON_F_FAULT,
+	HWMON_F_INPUT | HWMON_F_FAULT,
+	HWMON_F_INPUT | HWMON_F_FAULT,
+	HWMON_F_INPUT | HWMON_F_FAULT,
+	0
 };
 
-static umode_t max31790_attrs_visible(struct kobject *kobj,
-				     struct attribute *a, int n)
-{
-	struct device *dev = container_of(kobj, struct device, kobj);
-	struct max31790_data *data = dev_get_drvdata(dev);
-	struct device_attribute *devattr =
-			container_of(a, struct device_attribute, attr);
-	int index = to_sensor_dev_attr(devattr)->index % NR_CHANNEL;
-	u8 fan_config;
+static const struct hwmon_channel_info max31790_fan = {
+	.type = hwmon_fan,
+	.config = max31790_fan_config,
+};
 
-	fan_config = data->fan_config[index];
+static const u32 max31790_pwm_config[] = {
+	HWMON_PWM_INPUT | HWMON_PWM_ENABLE,
+	HWMON_PWM_INPUT | HWMON_PWM_ENABLE,
+	HWMON_PWM_INPUT | HWMON_PWM_ENABLE,
+	HWMON_PWM_INPUT | HWMON_PWM_ENABLE,
+	HWMON_PWM_INPUT | HWMON_PWM_ENABLE,
+	HWMON_PWM_INPUT | HWMON_PWM_ENABLE,
+	0
+};
 
-	if (n >= NR_CHANNEL * 2 && n < NR_CHANNEL * 4 &&
-	    !(fan_config & MAX31790_FAN_CFG_TACH_INPUT))
-		return 0;
-	if (n >= NR_CHANNEL * 4 && (fan_config & MAX31790_FAN_CFG_TACH_INPUT))
-		return 0;
+static const struct hwmon_channel_info max31790_pwm = {
+	.type = hwmon_pwm,
+	.config = max31790_pwm_config,
+};
 
-	return a->mode;
-}
+static const struct hwmon_channel_info *max31790_info[] = {
+	&max31790_fan,
+	&max31790_pwm,
+	NULL
+};
+
+static const struct hwmon_ops max31790_hwmon_ops = {
+	.is_visible = max31790_is_visible,
+	.read = max31790_read,
+	.write = max31790_write,
+};
 
-static const struct attribute_group max31790_group = {
-	.attrs = max31790_attrs,
-	.is_visible = max31790_attrs_visible,
+static const struct hwmon_chip_info max31790_chip_info = {
+	.ops = &max31790_hwmon_ops,
+	.info = max31790_info,
 };
-__ATTRIBUTE_GROUPS(max31790);
 
 static int max31790_init_client(struct i2c_client *client,
 				struct max31790_data *data)
@@ -575,8 +502,10 @@ static int max31790_probe(struct i2c_client *client,
 	if (err)
 		return err;
 
-	hwmon_dev = devm_hwmon_device_register_with_groups(dev,
-			client->name, data, max31790_groups);
+	hwmon_dev = devm_hwmon_device_register_with_info(dev, client->name,
+							 data,
+							 &max31790_chip_info,
+							 NULL);
 
 	return PTR_ERR_OR_ZERO(hwmon_dev);
 }
diff --git a/drivers/hwmon/max6650.c b/drivers/hwmon/max6650.c
index 162a520f4bd6..a993b44ed538 100644
--- a/drivers/hwmon/max6650.c
+++ b/drivers/hwmon/max6650.c
@@ -39,6 +39,7 @@
 #include <linux/hwmon.h>
 #include <linux/hwmon-sysfs.h>
 #include <linux/err.h>
+#include <linux/of_device.h>
 
 /*
  * Insmod parameters
@@ -48,7 +49,7 @@
 static int fan_voltage;
 /* prescaler: Possible values are 1, 2, 4, 8, 16 or 0 for don't change */
 static int prescaler;
-/* clock: The clock frequency of the chip the driver should assume */
+/* clock: The clock frequency of the chip (max6651 can be clocked externally) */
 static int clock = 254000;
 
 module_param(fan_voltage, int, S_IRUGO);
@@ -133,6 +134,19 @@ static const u8 tach_reg[] = {
 	MAX6650_REG_TACH3,
 };
 
+static const struct of_device_id max6650_dt_match[] = {
+	{
+		.compatible = "maxim,max6650",
+		.data = (void *)1
+	},
+	{
+		.compatible = "maxim,max6651",
+		.data = (void *)4
+	},
+	{ },
+};
+MODULE_DEVICE_TABLE(of, max6650_dt_match);
+
 static struct max6650_data *max6650_update_device(struct device *dev)
 {
 	struct max6650_data *data = dev_get_drvdata(dev);
@@ -171,6 +185,30 @@ static struct max6650_data *max6650_update_device(struct device *dev)
 	return data;
 }
 
+/*
+ * Change the operating mode of the chip (if needed).
+ * mode is one of the MAX6650_CFG_MODE_* values.
+ */
+static int max6650_set_operating_mode(struct max6650_data *data, u8 mode)
+{
+	int result;
+	u8 config = data->config;
+
+	if (mode == (config & MAX6650_CFG_MODE_MASK))
+		return 0;
+
+	config = (config & ~MAX6650_CFG_MODE_MASK) | mode;
+
+	result = i2c_smbus_write_byte_data(data->client, MAX6650_REG_CONFIG,
+					   config);
+	if (result < 0)
+		return result;
+
+	data->config = config;
+
+	return 0;
+}
+
 static ssize_t get_fan(struct device *dev, struct device_attribute *devattr,
 		       char *buf)
 {
@@ -252,18 +290,12 @@ static ssize_t get_target(struct device *dev, struct device_attribute *devattr,
 	return sprintf(buf, "%d\n", rpm);
 }
 
-static ssize_t set_target(struct device *dev, struct device_attribute *devattr,
-			 const char *buf, size_t count)
+static int max6650_set_target(struct max6650_data *data, unsigned long rpm)
 {
-	struct max6650_data *data = dev_get_drvdata(dev);
-	struct i2c_client *client = data->client;
 	int kscale, ktach;
-	unsigned long rpm;
-	int err;
 
-	err = kstrtoul(buf, 10, &rpm);
-	if (err)
-		return err;
+	if (rpm == 0)
+		return max6650_set_operating_mode(data, MAX6650_CFG_MODE_OFF);
 
 	rpm = clamp_val(rpm, FAN_RPM_MIN, FAN_RPM_MAX);
 
@@ -274,8 +306,6 @@ static ssize_t set_target(struct device *dev, struct device_attribute *devattr,
 	 *     KTACH = [(fCLK x KSCALE) / (256 x FanSpeed)] - 1
 	 */
 
-	mutex_lock(&data->update_lock);
-
 	kscale = DIV_FROM_REG(data->config);
 	ktach = ((clock * kscale) / (256 * rpm / 60)) - 1;
 	if (ktach < 0)
@@ -284,10 +314,30 @@ static ssize_t set_target(struct device *dev, struct device_attribute *devattr,
 		ktach = 255;
 	data->speed = ktach;
 
-	i2c_smbus_write_byte_data(client, MAX6650_REG_SPEED, data->speed);
+	return i2c_smbus_write_byte_data(data->client, MAX6650_REG_SPEED,
+					 data->speed);
+}
+
+static ssize_t set_target(struct device *dev, struct device_attribute *devattr,
+			 const char *buf, size_t count)
+{
+	struct max6650_data *data = dev_get_drvdata(dev);
+	unsigned long rpm;
+	int err;
+
+	err = kstrtoul(buf, 10, &rpm);
+	if (err)
+		return err;
+
+	mutex_lock(&data->update_lock);
+
+	err = max6650_set_target(data, rpm);
 
 	mutex_unlock(&data->update_lock);
 
+	if (err < 0)
+		return err;
+
 	return count;
 }
 
@@ -341,12 +391,11 @@ static ssize_t set_pwm(struct device *dev, struct device_attribute *devattr,
 		data->dac = 180 - (180 * pwm)/255;
 	else
 		data->dac = 76 - (76 * pwm)/255;
-
-	i2c_smbus_write_byte_data(client, MAX6650_REG_DAC, data->dac);
+	err = i2c_smbus_write_byte_data(client, MAX6650_REG_DAC, data->dac);
 
 	mutex_unlock(&data->update_lock);
 
-	return count;
+	return err < 0 ? err : count;
 }
 
 /*
@@ -355,14 +404,14 @@ static ssize_t set_pwm(struct device *dev, struct device_attribute *devattr,
  * 0 = Fan always on
  * 1 = Open loop, Voltage is set according to speed, not regulated.
  * 2 = Closed loop, RPM for all fans regulated by fan1 tachometer
+ * 3 = Fan off
  */
-
 static ssize_t get_enable(struct device *dev, struct device_attribute *devattr,
 			  char *buf)
 {
 	struct max6650_data *data = max6650_update_device(dev);
 	int mode = (data->config & MAX6650_CFG_MODE_MASK) >> 4;
-	int sysfs_modes[4] = {0, 1, 2, 1};
+	int sysfs_modes[4] = {0, 3, 2, 1};
 
 	return sprintf(buf, "%d\n", sysfs_modes[mode]);
 }
@@ -371,25 +420,25 @@ static ssize_t set_enable(struct device *dev, struct device_attribute *devattr,
 			  const char *buf, size_t count)
 {
 	struct max6650_data *data = dev_get_drvdata(dev);
-	struct i2c_client *client = data->client;
-	int max6650_modes[3] = {0, 3, 2};
 	unsigned long mode;
 	int err;
+	const u8 max6650_modes[] = {
+		MAX6650_CFG_MODE_ON,
+		MAX6650_CFG_MODE_OPEN_LOOP,
+		MAX6650_CFG_MODE_CLOSED_LOOP,
+		MAX6650_CFG_MODE_OFF,
+		};
 
 	err = kstrtoul(buf, 10, &mode);
 	if (err)
 		return err;
 
-	if (mode > 2)
+	if (mode >= ARRAY_SIZE(max6650_modes))
 		return -EINVAL;
 
 	mutex_lock(&data->update_lock);
 
-	data->config = i2c_smbus_read_byte_data(client, MAX6650_REG_CONFIG);
-	data->config = (data->config & ~MAX6650_CFG_MODE_MASK)
-		       | (max6650_modes[mode] << 4);
-
-	i2c_smbus_write_byte_data(client, MAX6650_REG_CONFIG, data->config);
+	max6650_set_operating_mode(data, max6650_modes[mode]);
 
 	mutex_unlock(&data->update_lock);
 
@@ -566,6 +615,18 @@ static int max6650_init_client(struct max6650_data *data,
 	struct device *dev = &client->dev;
 	int config;
 	int err = -EIO;
+	u32 voltage;
+	u32 prescale;
+	u32 target_rpm;
+
+	if (of_property_read_u32(dev->of_node, "maxim,fan-microvolt",
+				 &voltage))
+		voltage = fan_voltage;
+	else
+		voltage /= 1000000; /* Microvolts to volts */
+	if (of_property_read_u32(dev->of_node, "maxim,fan-prescale",
+				 &prescale))
+		prescale = prescaler;
 
 	config = i2c_smbus_read_byte_data(client, MAX6650_REG_CONFIG);
 
@@ -574,7 +635,7 @@ static int max6650_init_client(struct max6650_data *data,
 		return err;
 	}
 
-	switch (fan_voltage) {
+	switch (voltage) {
 	case 0:
 		break;
 	case 5:
@@ -584,14 +645,10 @@ static int max6650_init_client(struct max6650_data *data,
 		config |= MAX6650_CFG_V12;
 		break;
 	default:
-		dev_err(dev, "illegal value for fan_voltage (%d)\n",
-			fan_voltage);
+		dev_err(dev, "illegal value for fan_voltage (%d)\n", voltage);
 	}
 
-	dev_info(dev, "Fan voltage is set to %dV.\n",
-		 (config & MAX6650_CFG_V12) ? 12 : 5);
-
-	switch (prescaler) {
+	switch (prescale) {
 	case 0:
 		break;
 	case 1:
@@ -614,28 +671,13 @@ static int max6650_init_client(struct max6650_data *data,
 			 | MAX6650_CFG_PRESCALER_16;
 		break;
 	default:
-		dev_err(dev, "illegal value for prescaler (%d)\n", prescaler);
+		dev_err(dev, "illegal value for prescaler (%d)\n", prescale);
 	}
 
-	dev_info(dev, "Prescaler is set to %d.\n",
+	dev_info(dev, "Fan voltage: %dV, prescaler: %d.\n",
+		 (config & MAX6650_CFG_V12) ? 12 : 5,
 		 1 << (config & MAX6650_CFG_PRESCALER_MASK));
 
-	/*
-	 * If mode is set to "full off", we change it to "open loop" and
-	 * set DAC to 255, which has the same effect. We do this because
-	 * there's no "full off" mode defined in hwmon specifications.
-	 */
-
-	if ((config & MAX6650_CFG_MODE_MASK) == MAX6650_CFG_MODE_OFF) {
-		dev_dbg(dev, "Change mode to open loop, full off.\n");
-		config = (config & ~MAX6650_CFG_MODE_MASK)
-			 | MAX6650_CFG_MODE_OPEN_LOOP;
-		if (i2c_smbus_write_byte_data(client, MAX6650_REG_DAC, 255)) {
-			dev_err(dev, "DAC write error, aborting.\n");
-			return err;
-		}
-	}
-
 	if (i2c_smbus_write_byte_data(client, MAX6650_REG_CONFIG, config)) {
 		dev_err(dev, "Config write error, aborting.\n");
 		return err;
@@ -644,6 +686,12 @@ static int max6650_init_client(struct max6650_data *data,
 	data->config = config;
 	data->count = i2c_smbus_read_byte_data(client, MAX6650_REG_COUNT);
 
+	if (!of_property_read_u32(client->dev.of_node, "maxim,fan-target-rpm",
+				  &target_rpm)) {
+		max6650_set_target(data, target_rpm);
+		max6650_set_operating_mode(data, MAX6650_CFG_MODE_CLOSED_LOOP);
+	}
+
 	return 0;
 }
 
@@ -651,6 +699,8 @@ static int max6650_probe(struct i2c_client *client,
 			 const struct i2c_device_id *id)
 {
 	struct device *dev = &client->dev;
+	const struct of_device_id *of_id =
+		of_match_device(of_match_ptr(max6650_dt_match), dev);
 	struct max6650_data *data;
 	struct device *hwmon_dev;
 	int err;
@@ -661,7 +711,7 @@ static int max6650_probe(struct i2c_client *client,
 
 	data->client = client;
 	mutex_init(&data->update_lock);
-	data->nr_fans = id->driver_data;
+	data->nr_fans = of_id ? (int)(uintptr_t)of_id->data : id->driver_data;
 
 	/*
 	 * Initialize the max6650 chip
@@ -691,6 +741,7 @@ MODULE_DEVICE_TABLE(i2c, max6650_id);
 static struct i2c_driver max6650_driver = {
 	.driver = {
 		.name	= "max6650",
+		.of_match_table = of_match_ptr(max6650_dt_match),
 	},
 	.probe		= max6650_probe,
 	.id_table	= max6650_id,
diff --git a/drivers/hwmon/nct6775.c b/drivers/hwmon/nct6775.c
index d087a8e00cf5..ce75dd4db7eb 100644
--- a/drivers/hwmon/nct6775.c
+++ b/drivers/hwmon/nct6775.c
@@ -195,6 +195,8 @@ superio_exit(int ioreg)
 
 #define NUM_FAN		6
 
+#define TEMP_SOURCE_VIRTUAL	0x1f
+
 /* Common and NCT6775 specific data */
 
 /* Voltage min/max registers for nr=7..14 are in bank 5 */
@@ -3940,7 +3942,7 @@ static int nct6775_probe(struct platform_device *pdev)
 			continue;
 
 		src = nct6775_read_value(data, data->REG_TEMP_SEL[i]) & 0x1f;
-		if (!src || (mask & (1 << src)))
+		if (!src)
 			continue;
 
 		if (src >= data->temp_label_num ||
@@ -3952,7 +3954,16 @@ static int nct6775_probe(struct platform_device *pdev)
 			continue;
 		}
 
-		mask |= 1 << src;
+		/*
+		 * For virtual temperature sources, the 'virtual' temperature
+		 * for each fan reflects a different temperature, and there
+		 * are no duplicates.
+		 */
+		if (src != TEMP_SOURCE_VIRTUAL) {
+			if (mask & (1 << src))
+				continue;
+			mask |= 1 << src;
+		}
 
 		/* Use fixed index for SYSTIN(1), CPUTIN(2), AUXTIN(3) */
 		if (src <= data->temp_fixed_num) {
@@ -4232,11 +4243,11 @@ static int __init nct6775_find(int sioaddr, struct nct6775_sio_data *sio_data)
 	if (err)
 		return err;
 
-	if (force_id)
+	val = (superio_inb(sioaddr, SIO_REG_DEVID) << 8) |
+		superio_inb(sioaddr, SIO_REG_DEVID + 1);
+	if (force_id && val != 0xffff)
 		val = force_id;
-	else
-		val = (superio_inb(sioaddr, SIO_REG_DEVID) << 8)
-		    | superio_inb(sioaddr, SIO_REG_DEVID + 1);
+
 	switch (val & SIO_ID_MASK) {
 	case SIO_NCT6106_ID:
 		sio_data->kind = nct6106;
diff --git a/drivers/hwmon/nct7904.c b/drivers/hwmon/nct7904.c
index 08ff89d222e5..95a68ab175c7 100644
--- a/drivers/hwmon/nct7904.c
+++ b/drivers/hwmon/nct7904.c
@@ -21,7 +21,6 @@
 #include <linux/i2c.h>
 #include <linux/mutex.h>
 #include <linux/hwmon.h>
-#include <linux/hwmon-sysfs.h>
 
 #define VENDOR_ID_REG		0x7A	/* Any bank */
 #define NUVOTON_ID		0x50
@@ -153,341 +152,230 @@ static int nct7904_write_reg(struct nct7904_data *data,
 	return ret;
 }
 
-/* FANIN ATTR */
-static ssize_t show_fan(struct device *dev,
-			struct device_attribute *devattr, char *buf)
+static int nct7904_read_fan(struct device *dev, u32 attr, int channel,
+			    long *val)
 {
-	int index = to_sensor_dev_attr(devattr)->index;
 	struct nct7904_data *data = dev_get_drvdata(dev);
+	unsigned int cnt, rpm;
 	int ret;
-	unsigned cnt, rpm;
 
-	ret = nct7904_read_reg16(data, BANK_0, FANIN1_HV_REG + index * 2);
-	if (ret < 0)
-		return ret;
-	cnt = ((ret & 0xff00) >> 3) | (ret & 0x1f);
-	if (cnt == 0x1fff)
-		rpm = 0;
-	else
-		rpm = 1350000 / cnt;
-	return sprintf(buf, "%u\n", rpm);
+	switch(attr) {
+	case hwmon_fan_input:
+		ret = nct7904_read_reg16(data, BANK_0,
+					 FANIN1_HV_REG + channel * 2);
+		if (ret < 0)
+			return ret;
+		cnt = ((ret & 0xff00) >> 3) | (ret & 0x1f);
+		if (cnt == 0x1fff)
+			rpm = 0;
+		else
+			rpm = 1350000 / cnt;
+		*val = rpm;
+		return 0;
+	default:
+		return -EOPNOTSUPP;
+	}
 }
 
-static umode_t nct7904_fanin_is_visible(struct kobject *kobj,
-					struct attribute *a, int n)
+static umode_t nct7904_fan_is_visible(const void *_data, u32 attr, int channel)
 {
-	struct device *dev = container_of(kobj, struct device, kobj);
-	struct nct7904_data *data = dev_get_drvdata(dev);
+	const struct nct7904_data *data = _data;
 
-	if (data->fanin_mask & (1 << n))
-		return a->mode;
+	if (attr == hwmon_fan_input && data->fanin_mask & (1 << channel))
+		return S_IRUGO;
 	return 0;
 }
 
-static SENSOR_DEVICE_ATTR(fan1_input, S_IRUGO, show_fan, NULL, 0);
-static SENSOR_DEVICE_ATTR(fan2_input, S_IRUGO, show_fan, NULL, 1);
-static SENSOR_DEVICE_ATTR(fan3_input, S_IRUGO, show_fan, NULL, 2);
-static SENSOR_DEVICE_ATTR(fan4_input, S_IRUGO, show_fan, NULL, 3);
-static SENSOR_DEVICE_ATTR(fan5_input, S_IRUGO, show_fan, NULL, 4);
-static SENSOR_DEVICE_ATTR(fan6_input, S_IRUGO, show_fan, NULL, 5);
-static SENSOR_DEVICE_ATTR(fan7_input, S_IRUGO, show_fan, NULL, 6);
-static SENSOR_DEVICE_ATTR(fan8_input, S_IRUGO, show_fan, NULL, 7);
-static SENSOR_DEVICE_ATTR(fan9_input, S_IRUGO, show_fan, NULL, 8);
-static SENSOR_DEVICE_ATTR(fan10_input, S_IRUGO, show_fan, NULL, 9);
-static SENSOR_DEVICE_ATTR(fan11_input, S_IRUGO, show_fan, NULL, 10);
-static SENSOR_DEVICE_ATTR(fan12_input, S_IRUGO, show_fan, NULL, 11);
-
-static struct attribute *nct7904_fanin_attrs[] = {
-	&sensor_dev_attr_fan1_input.dev_attr.attr,
-	&sensor_dev_attr_fan2_input.dev_attr.attr,
-	&sensor_dev_attr_fan3_input.dev_attr.attr,
-	&sensor_dev_attr_fan4_input.dev_attr.attr,
-	&sensor_dev_attr_fan5_input.dev_attr.attr,
-	&sensor_dev_attr_fan6_input.dev_attr.attr,
-	&sensor_dev_attr_fan7_input.dev_attr.attr,
-	&sensor_dev_attr_fan8_input.dev_attr.attr,
-	&sensor_dev_attr_fan9_input.dev_attr.attr,
-	&sensor_dev_attr_fan10_input.dev_attr.attr,
-	&sensor_dev_attr_fan11_input.dev_attr.attr,
-	&sensor_dev_attr_fan12_input.dev_attr.attr,
-	NULL
-};
-
-static const struct attribute_group nct7904_fanin_group = {
-	.attrs = nct7904_fanin_attrs,
-	.is_visible = nct7904_fanin_is_visible,
+static u8 nct7904_chan_to_index[] = {
+	0,	/* Not used */
+	0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15,
+	18, 19, 20, 16
 };
 
-/* VSEN ATTR */
-static ssize_t show_voltage(struct device *dev,
-			    struct device_attribute *devattr, char *buf)
+static int nct7904_read_in(struct device *dev, u32 attr, int channel,
+			   long *val)
 {
-	int index = to_sensor_dev_attr(devattr)->index;
 	struct nct7904_data *data = dev_get_drvdata(dev);
-	int ret;
-	int volt;
+	int ret, volt, index;
 
-	ret = nct7904_read_reg16(data, BANK_0, VSEN1_HV_REG + index * 2);
-	if (ret < 0)
-		return ret;
-	volt = ((ret & 0xff00) >> 5) | (ret & 0x7);
-	if (index < 14)
-		volt *= 2; /* 0.002V scale */
-	else
-		volt *= 6; /* 0.006V scale */
+	index = nct7904_chan_to_index[channel];
 
-	return sprintf(buf, "%d\n", volt);
+	switch(attr) {
+	case hwmon_in_input:
+		ret = nct7904_read_reg16(data, BANK_0,
+					 VSEN1_HV_REG + index * 2);
+		if (ret < 0)
+			return ret;
+		volt = ((ret & 0xff00) >> 5) | (ret & 0x7);
+		if (index < 14)
+			volt *= 2; /* 0.002V scale */
+		else
+			volt *= 6; /* 0.006V scale */
+		*val = volt;
+		return 0;
+	default:
+		return -EOPNOTSUPP;
+	}
 }
 
-static ssize_t show_ltemp(struct device *dev,
-			  struct device_attribute *devattr, char *buf)
+static umode_t nct7904_in_is_visible(const void *_data, u32 attr, int channel)
 {
-	struct nct7904_data *data = dev_get_drvdata(dev);
-	int ret;
-	int temp;
+	const struct nct7904_data *data = _data;
+	int index = nct7904_chan_to_index[channel];
 
-	ret = nct7904_read_reg16(data, BANK_0, LTD_HV_REG);
-	if (ret < 0)
-		return ret;
-	temp = ((ret & 0xff00) >> 5) | (ret & 0x7);
-	temp = sign_extend32(temp, 10) * 125;
-
-	return sprintf(buf, "%d\n", temp);
-}
-
-static umode_t nct7904_vsen_is_visible(struct kobject *kobj,
-				       struct attribute *a, int n)
-{
-	struct device *dev = container_of(kobj, struct device, kobj);
-	struct nct7904_data *data = dev_get_drvdata(dev);
+	if (channel > 0 && attr == hwmon_in_input &&
+	    (data->vsen_mask & BIT(index)))
+		return S_IRUGO;
 
-	if (data->vsen_mask & (1 << n))
-		return a->mode;
 	return 0;
 }
 
-static SENSOR_DEVICE_ATTR(in1_input, S_IRUGO, show_voltage, NULL, 0);
-static SENSOR_DEVICE_ATTR(in2_input, S_IRUGO, show_voltage, NULL, 1);
-static SENSOR_DEVICE_ATTR(in3_input, S_IRUGO, show_voltage, NULL, 2);
-static SENSOR_DEVICE_ATTR(in4_input, S_IRUGO, show_voltage, NULL, 3);
-static SENSOR_DEVICE_ATTR(in5_input, S_IRUGO, show_voltage, NULL, 4);
-static SENSOR_DEVICE_ATTR(in6_input, S_IRUGO, show_voltage, NULL, 5);
-static SENSOR_DEVICE_ATTR(in7_input, S_IRUGO, show_voltage, NULL, 6);
-static SENSOR_DEVICE_ATTR(in8_input, S_IRUGO, show_voltage, NULL, 7);
-static SENSOR_DEVICE_ATTR(in9_input, S_IRUGO, show_voltage, NULL, 8);
-static SENSOR_DEVICE_ATTR(in10_input, S_IRUGO, show_voltage, NULL, 9);
-static SENSOR_DEVICE_ATTR(in11_input, S_IRUGO, show_voltage, NULL, 10);
-static SENSOR_DEVICE_ATTR(in12_input, S_IRUGO, show_voltage, NULL, 11);
-static SENSOR_DEVICE_ATTR(in13_input, S_IRUGO, show_voltage, NULL, 12);
-static SENSOR_DEVICE_ATTR(in14_input, S_IRUGO, show_voltage, NULL, 13);
-/*
- * Next 3 voltage sensors have specific names in the Nuvoton doc
- * (3VDD, VBAT, 3VSB) but we use vacant numbers for them.
- */
-static SENSOR_DEVICE_ATTR(in15_input, S_IRUGO, show_voltage, NULL, 14);
-static SENSOR_DEVICE_ATTR(in16_input, S_IRUGO, show_voltage, NULL, 15);
-static SENSOR_DEVICE_ATTR(in20_input, S_IRUGO, show_voltage, NULL, 16);
-/* This is not a voltage, but a local temperature sensor. */
-static SENSOR_DEVICE_ATTR(temp1_input, S_IRUGO, show_ltemp, NULL, 0);
-static SENSOR_DEVICE_ATTR(in17_input, S_IRUGO, show_voltage, NULL, 18);
-static SENSOR_DEVICE_ATTR(in18_input, S_IRUGO, show_voltage, NULL, 19);
-static SENSOR_DEVICE_ATTR(in19_input, S_IRUGO, show_voltage, NULL, 20);
-
-static struct attribute *nct7904_vsen_attrs[] = {
-	&sensor_dev_attr_in1_input.dev_attr.attr,
-	&sensor_dev_attr_in2_input.dev_attr.attr,
-	&sensor_dev_attr_in3_input.dev_attr.attr,
-	&sensor_dev_attr_in4_input.dev_attr.attr,
-	&sensor_dev_attr_in5_input.dev_attr.attr,
-	&sensor_dev_attr_in6_input.dev_attr.attr,
-	&sensor_dev_attr_in7_input.dev_attr.attr,
-	&sensor_dev_attr_in8_input.dev_attr.attr,
-	&sensor_dev_attr_in9_input.dev_attr.attr,
-	&sensor_dev_attr_in10_input.dev_attr.attr,
-	&sensor_dev_attr_in11_input.dev_attr.attr,
-	&sensor_dev_attr_in12_input.dev_attr.attr,
-	&sensor_dev_attr_in13_input.dev_attr.attr,
-	&sensor_dev_attr_in14_input.dev_attr.attr,
-	&sensor_dev_attr_in15_input.dev_attr.attr,
-	&sensor_dev_attr_in16_input.dev_attr.attr,
-	&sensor_dev_attr_in20_input.dev_attr.attr,
-	&sensor_dev_attr_temp1_input.dev_attr.attr,
-	&sensor_dev_attr_in17_input.dev_attr.attr,
-	&sensor_dev_attr_in18_input.dev_attr.attr,
-	&sensor_dev_attr_in19_input.dev_attr.attr,
-	NULL
-};
-
-static const struct attribute_group nct7904_vsen_group = {
-	.attrs = nct7904_vsen_attrs,
-	.is_visible = nct7904_vsen_is_visible,
-};
-
-/* CPU_TEMP ATTR */
-static ssize_t show_tcpu(struct device *dev,
-			 struct device_attribute *devattr, char *buf)
+static int nct7904_read_temp(struct device *dev, u32 attr, int channel,
+			     long *val)
 {
-	int index = to_sensor_dev_attr(devattr)->index;
 	struct nct7904_data *data = dev_get_drvdata(dev);
-	int ret;
-	int temp;
-
-	ret = nct7904_read_reg16(data, BANK_0, T_CPU1_HV_REG + index * 2);
-	if (ret < 0)
-		return ret;
-
-	temp = ((ret & 0xff00) >> 5) | (ret & 0x7);
-	temp = sign_extend32(temp, 10) * 125;
-	return sprintf(buf, "%d\n", temp);
+	int ret, temp;
+
+	switch(attr) {
+	case hwmon_temp_input:
+		if (channel == 0)
+			ret = nct7904_read_reg16(data, BANK_0, LTD_HV_REG);
+		else
+			ret = nct7904_read_reg16(data, BANK_0,
+					T_CPU1_HV_REG + (channel - 1) * 2);
+		if (ret < 0)
+			return ret;
+		temp = ((ret & 0xff00) >> 5) | (ret & 0x7);
+		*val = sign_extend32(temp, 10) * 125;
+		return 0;
+	default:
+		return -EOPNOTSUPP;
+	}
 }
 
-static umode_t nct7904_tcpu_is_visible(struct kobject *kobj,
-				       struct attribute *a, int n)
+static umode_t nct7904_temp_is_visible(const void *_data, u32 attr, int channel)
 {
-	struct device *dev = container_of(kobj, struct device, kobj);
-	struct nct7904_data *data = dev_get_drvdata(dev);
+	const struct nct7904_data *data = _data;
+
+	if (attr == hwmon_temp_input) {
+		if (channel == 0) {
+			if (data->vsen_mask & BIT(17))
+				return S_IRUGO;
+		} else {
+			if (data->tcpu_mask & BIT(channel - 1))
+				return S_IRUGO;
+		}
+	}
 
-	if (data->tcpu_mask & (1 << n))
-		return a->mode;
 	return 0;
 }
 
-/* "temp1_input" reserved for local temp */
-static SENSOR_DEVICE_ATTR(temp2_input, S_IRUGO, show_tcpu, NULL, 0);
-static SENSOR_DEVICE_ATTR(temp3_input, S_IRUGO, show_tcpu, NULL, 1);
-static SENSOR_DEVICE_ATTR(temp4_input, S_IRUGO, show_tcpu, NULL, 2);
-static SENSOR_DEVICE_ATTR(temp5_input, S_IRUGO, show_tcpu, NULL, 3);
-static SENSOR_DEVICE_ATTR(temp6_input, S_IRUGO, show_tcpu, NULL, 4);
-static SENSOR_DEVICE_ATTR(temp7_input, S_IRUGO, show_tcpu, NULL, 5);
-static SENSOR_DEVICE_ATTR(temp8_input, S_IRUGO, show_tcpu, NULL, 6);
-static SENSOR_DEVICE_ATTR(temp9_input, S_IRUGO, show_tcpu, NULL, 7);
-
-static struct attribute *nct7904_tcpu_attrs[] = {
-	&sensor_dev_attr_temp2_input.dev_attr.attr,
-	&sensor_dev_attr_temp3_input.dev_attr.attr,
-	&sensor_dev_attr_temp4_input.dev_attr.attr,
-	&sensor_dev_attr_temp5_input.dev_attr.attr,
-	&sensor_dev_attr_temp6_input.dev_attr.attr,
-	&sensor_dev_attr_temp7_input.dev_attr.attr,
-	&sensor_dev_attr_temp8_input.dev_attr.attr,
-	&sensor_dev_attr_temp9_input.dev_attr.attr,
-	NULL
-};
-
-static const struct attribute_group nct7904_tcpu_group = {
-	.attrs = nct7904_tcpu_attrs,
-	.is_visible = nct7904_tcpu_is_visible,
-};
-
-/* PWM ATTR */
-static ssize_t store_pwm(struct device *dev, struct device_attribute *devattr,
-			 const char *buf, size_t count)
+static int nct7904_read_pwm(struct device *dev, u32 attr, int channel,
+			    long *val)
 {
-	int index = to_sensor_dev_attr(devattr)->index;
 	struct nct7904_data *data = dev_get_drvdata(dev);
-	unsigned long val;
 	int ret;
 
-	if (kstrtoul(buf, 10, &val) < 0)
-		return -EINVAL;
-	if (val > 255)
-		return -EINVAL;
-
-	ret = nct7904_write_reg(data, BANK_3, FANCTL1_OUT_REG + index, val);
+	switch(attr) {
+	case hwmon_pwm_input:
+		ret = nct7904_read_reg(data, BANK_3, FANCTL1_OUT_REG + channel);
+		if (ret < 0)
+			return ret;
+		*val = ret;
+		return 0;
+	case hwmon_pwm_enable:
+		ret = nct7904_read_reg(data, BANK_3, FANCTL1_FMR_REG + channel);
+		if (ret < 0)
+			return ret;
 
-	return ret ? ret : count;
+		*val = ret ? 2 : 1;
+		return 0;
+	default:
+		return -EOPNOTSUPP;
+	}
 }
 
-static ssize_t show_pwm(struct device *dev,
-			struct device_attribute *devattr, char *buf)
+static int nct7904_write_pwm(struct device *dev, u32 attr, int channel,
+			     long val)
 {
-	int index = to_sensor_dev_attr(devattr)->index;
 	struct nct7904_data *data = dev_get_drvdata(dev);
-	int val;
-
-	val = nct7904_read_reg(data, BANK_3, FANCTL1_OUT_REG + index);
-	if (val < 0)
-		return val;
+	int ret;
 
-	return sprintf(buf, "%d\n", val);
+	switch(attr) {
+	case hwmon_pwm_input:
+		if (val < 0 || val > 255)
+			return -EINVAL;
+		ret = nct7904_write_reg(data, BANK_3, FANCTL1_OUT_REG + channel,
+					val);
+		return ret;
+	case hwmon_pwm_enable:
+		if (val < 1 || val > 2 ||
+		    (val == 2 && !data->fan_mode[channel]))
+			return -EINVAL;
+		ret = nct7904_write_reg(data, BANK_3, FANCTL1_FMR_REG + channel,
+					val == 2 ? data->fan_mode[channel] : 0);
+		return ret;
+	default:
+		return -EOPNOTSUPP;
+	}
 }
 
-static ssize_t store_enable(struct device *dev,
-			    struct device_attribute *devattr,
-			    const char *buf, size_t count)
+static umode_t nct7904_pwm_is_visible(const void *_data, u32 attr, int channel)
 {
-	int index = to_sensor_dev_attr(devattr)->index;
-	struct nct7904_data *data = dev_get_drvdata(dev);
-	unsigned long val;
-	int ret;
-
-	if (kstrtoul(buf, 10, &val) < 0)
-		return -EINVAL;
-	if (val < 1 || val > 2 || (val == 2 && !data->fan_mode[index]))
-		return -EINVAL;
-
-	ret = nct7904_write_reg(data, BANK_3, FANCTL1_FMR_REG + index,
-				val == 2 ? data->fan_mode[index] : 0);
-
-	return ret ? ret : count;
+	switch(attr) {
+	case hwmon_pwm_input:
+	case hwmon_pwm_enable:
+		return S_IRUGO | S_IWUSR;
+	default:
+		return 0;
+	}
 }
 
-/* Return 1 for manual mode or 2 for SmartFan mode */
-static ssize_t show_enable(struct device *dev,
-			   struct device_attribute *devattr, char *buf)
+static int nct7904_read(struct device *dev, enum hwmon_sensor_types type,
+			u32 attr, int channel, long *val)
 {
-	int index = to_sensor_dev_attr(devattr)->index;
-	struct nct7904_data *data = dev_get_drvdata(dev);
-	int val;
-
-	val = nct7904_read_reg(data, BANK_3, FANCTL1_FMR_REG + index);
-	if (val < 0)
-		return val;
-
-	return sprintf(buf, "%d\n", val ? 2 : 1);
+	switch (type) {
+	case hwmon_in:
+		return nct7904_read_in(dev, attr, channel, val);
+	case hwmon_fan:
+		return nct7904_read_fan(dev, attr, channel, val);
+	case hwmon_pwm:
+		return nct7904_read_pwm(dev, attr, channel, val);
+	case hwmon_temp:
+		return nct7904_read_temp(dev, attr, channel, val);
+	default:
+		return -EOPNOTSUPP;
+	}
 }
 
-/* 2 attributes per channel: pwm and mode */
-static SENSOR_DEVICE_ATTR(pwm1, S_IRUGO | S_IWUSR,
-			show_pwm, store_pwm, 0);
-static SENSOR_DEVICE_ATTR(pwm1_enable, S_IRUGO | S_IWUSR,
-			show_enable, store_enable, 0);
-static SENSOR_DEVICE_ATTR(pwm2, S_IRUGO | S_IWUSR,
-			show_pwm, store_pwm, 1);
-static SENSOR_DEVICE_ATTR(pwm2_enable, S_IRUGO | S_IWUSR,
-			show_enable, store_enable, 1);
-static SENSOR_DEVICE_ATTR(pwm3, S_IRUGO | S_IWUSR,
-			show_pwm, store_pwm, 2);
-static SENSOR_DEVICE_ATTR(pwm3_enable, S_IRUGO | S_IWUSR,
-			show_enable, store_enable, 2);
-static SENSOR_DEVICE_ATTR(pwm4, S_IRUGO | S_IWUSR,
-			show_pwm, store_pwm, 3);
-static SENSOR_DEVICE_ATTR(pwm4_enable, S_IRUGO | S_IWUSR,
-			show_enable, store_enable, 3);
-
-static struct attribute *nct7904_fanctl_attrs[] = {
-	&sensor_dev_attr_pwm1.dev_attr.attr,
-	&sensor_dev_attr_pwm1_enable.dev_attr.attr,
-	&sensor_dev_attr_pwm2.dev_attr.attr,
-	&sensor_dev_attr_pwm2_enable.dev_attr.attr,
-	&sensor_dev_attr_pwm3.dev_attr.attr,
-	&sensor_dev_attr_pwm3_enable.dev_attr.attr,
-	&sensor_dev_attr_pwm4.dev_attr.attr,
-	&sensor_dev_attr_pwm4_enable.dev_attr.attr,
-	NULL
-};
-
-static const struct attribute_group nct7904_fanctl_group = {
-	.attrs = nct7904_fanctl_attrs,
-};
+static int nct7904_write(struct device *dev, enum hwmon_sensor_types type,
+			 u32 attr, int channel, long val)
+{
+	switch (type) {
+	case hwmon_pwm:
+		return nct7904_write_pwm(dev, attr, channel, val);
+	default:
+		return -EOPNOTSUPP;
+	}
+}
 
-static const struct attribute_group *nct7904_groups[] = {
-	&nct7904_fanin_group,
-	&nct7904_vsen_group,
-	&nct7904_tcpu_group,
-	&nct7904_fanctl_group,
-	NULL
-};
+static umode_t nct7904_is_visible(const void *data,
+				  enum hwmon_sensor_types type,
+				  u32 attr, int channel)
+{
+	switch (type) {
+	case hwmon_in:
+		return nct7904_in_is_visible(data, attr, channel);
+	case hwmon_fan:
+		return nct7904_fan_is_visible(data, attr, channel);
+	case hwmon_pwm:
+		return nct7904_pwm_is_visible(data, attr, channel);
+	case hwmon_temp:
+		return nct7904_temp_is_visible(data, attr, channel);
+	default:
+		return 0;
+	}
+}
 
 /* Return 0 if detection is successful, -ENODEV otherwise */
 static int nct7904_detect(struct i2c_client *client,
@@ -512,6 +400,103 @@ static int nct7904_detect(struct i2c_client *client,
 	return 0;
 }
 
+static const u32 nct7904_in_config[] = {
+	HWMON_I_INPUT,                  /* dummy, skipped in is_visible */
+	HWMON_I_INPUT,
+	HWMON_I_INPUT,
+	HWMON_I_INPUT,
+	HWMON_I_INPUT,
+	HWMON_I_INPUT,
+	HWMON_I_INPUT,
+	HWMON_I_INPUT,
+	HWMON_I_INPUT,
+	HWMON_I_INPUT,
+	HWMON_I_INPUT,
+	HWMON_I_INPUT,
+	HWMON_I_INPUT,
+	HWMON_I_INPUT,
+	HWMON_I_INPUT,
+	HWMON_I_INPUT,
+	HWMON_I_INPUT,
+	HWMON_I_INPUT,
+	HWMON_I_INPUT,
+	HWMON_I_INPUT,
+	HWMON_I_INPUT,
+	0
+};
+
+static const struct hwmon_channel_info nct7904_in = {
+	.type = hwmon_in,
+	.config = nct7904_in_config,
+};
+
+static const u32 nct7904_fan_config[] = {
+            HWMON_F_INPUT,
+            HWMON_F_INPUT,
+            HWMON_F_INPUT,
+            HWMON_F_INPUT,
+            HWMON_F_INPUT,
+            HWMON_F_INPUT,
+            HWMON_F_INPUT,
+            HWMON_F_INPUT,
+	    0
+};
+
+static const struct hwmon_channel_info nct7904_fan = {
+	.type = hwmon_fan,
+	.config = nct7904_fan_config,
+};
+
+static const u32 nct7904_pwm_config[] = {
+            HWMON_PWM_INPUT | HWMON_PWM_ENABLE,
+            HWMON_PWM_INPUT | HWMON_PWM_ENABLE,
+            HWMON_PWM_INPUT | HWMON_PWM_ENABLE,
+            HWMON_PWM_INPUT | HWMON_PWM_ENABLE,
+	    0
+};
+
+static const struct hwmon_channel_info nct7904_pwm = {
+	.type = hwmon_pwm,
+	.config = nct7904_pwm_config,
+};
+
+static const u32 nct7904_temp_config[] = {
+            HWMON_T_INPUT,
+            HWMON_T_INPUT,
+            HWMON_T_INPUT,
+            HWMON_T_INPUT,
+            HWMON_T_INPUT,
+            HWMON_T_INPUT,
+            HWMON_T_INPUT,
+            HWMON_T_INPUT,
+            HWMON_T_INPUT,
+	    0
+};
+
+static const struct hwmon_channel_info nct7904_temp = {
+	.type = hwmon_temp,
+	.config = nct7904_temp_config,
+};
+
+static const struct hwmon_channel_info *nct7904_info[] = {
+	&nct7904_in,
+	&nct7904_fan,
+	&nct7904_pwm,
+	&nct7904_temp,
+	NULL
+};
+
+static const struct hwmon_ops nct7904_hwmon_ops = {
+	.is_visible = nct7904_is_visible,
+	.read = nct7904_read,
+	.write = nct7904_write,
+};
+
+static const struct hwmon_chip_info nct7904_chip_info = {
+	.ops = &nct7904_hwmon_ops,
+	.info = nct7904_info,
+};
+
 static int nct7904_probe(struct i2c_client *client,
 			 const struct i2c_device_id *id)
 {
@@ -566,8 +551,8 @@ static int nct7904_probe(struct i2c_client *client,
 	}
 
 	hwmon_dev =
-		devm_hwmon_device_register_with_groups(dev, client->name, data,
-						       nct7904_groups);
+		devm_hwmon_device_register_with_info(dev, client->name, data,
+						     &nct7904_chip_info, NULL);
 	return PTR_ERR_OR_ZERO(hwmon_dev);
 }
 
diff --git a/drivers/hwmon/ntc_thermistor.c b/drivers/hwmon/ntc_thermistor.c
index 8ef7b713cb1a..c52d07c6b49f 100644
--- a/drivers/hwmon/ntc_thermistor.c
+++ b/drivers/hwmon/ntc_thermistor.c
@@ -253,12 +253,9 @@ static const struct ntc_compensation b57330v2103[] = {
 };
 
 struct ntc_data {
-	struct device *hwmon_dev;
 	struct ntc_thermistor_platform_data *pdata;
 	const struct ntc_compensation *comp;
-	struct device *dev;
 	int n_comp;
-	char name[PLATFORM_NAME_SIZE];
 };
 
 #if defined(CONFIG_OF) && IS_ENABLED(CONFIG_IIO)
@@ -316,22 +313,22 @@ static const struct of_device_id ntc_match[] = {
 MODULE_DEVICE_TABLE(of, ntc_match);
 
 static struct ntc_thermistor_platform_data *
-ntc_thermistor_parse_dt(struct platform_device *pdev)
+ntc_thermistor_parse_dt(struct device *dev)
 {
 	struct iio_channel *chan;
 	enum iio_chan_type type;
-	struct device_node *np = pdev->dev.of_node;
+	struct device_node *np = dev->of_node;
 	struct ntc_thermistor_platform_data *pdata;
 	int ret;
 
 	if (!np)
 		return NULL;
 
-	pdata = devm_kzalloc(&pdev->dev, sizeof(*pdata), GFP_KERNEL);
+	pdata = devm_kzalloc(dev, sizeof(*pdata), GFP_KERNEL);
 	if (!pdata)
 		return ERR_PTR(-ENOMEM);
 
-	chan = iio_channel_get(&pdev->dev, NULL);
+	chan = devm_iio_channel_get(dev, NULL);
 	if (IS_ERR(chan))
 		return ERR_CAST(chan);
 
@@ -359,22 +356,15 @@ ntc_thermistor_parse_dt(struct platform_device *pdev)
 
 	return pdata;
 }
-static void ntc_iio_channel_release(struct ntc_thermistor_platform_data *pdata)
-{
-	if (pdata->chan)
-		iio_channel_release(pdata->chan);
-}
 #else
 static struct ntc_thermistor_platform_data *
-ntc_thermistor_parse_dt(struct platform_device *pdev)
+ntc_thermistor_parse_dt(struct device *dev)
 {
 	return NULL;
 }
 
 #define ntc_match	NULL
 
-static void ntc_iio_channel_release(struct ntc_thermistor_platform_data *pdata)
-{ }
 #endif
 
 static inline u64 div64_u64_safe(u64 dividend, u64 divisor)
@@ -516,9 +506,8 @@ static int ntc_thermistor_get_ohm(struct ntc_data *data)
 	return -EINVAL;
 }
 
-static int ntc_read_temp(void *dev, int *temp)
+static int ntc_read_temp(void *data, int *temp)
 {
-	struct ntc_data *data = dev_get_drvdata(dev);
 	int ohm;
 
 	ohm = ntc_thermistor_get_ohm(data);
@@ -530,14 +519,6 @@ static int ntc_read_temp(void *dev, int *temp)
 	return 0;
 }
 
-static ssize_t ntc_show_name(struct device *dev,
-		struct device_attribute *attr, char *buf)
-{
-	struct ntc_data *data = dev_get_drvdata(dev);
-
-	return sprintf(buf, "%s\n", data->name);
-}
-
 static ssize_t ntc_show_type(struct device *dev,
 		struct device_attribute *attr, char *buf)
 {
@@ -559,18 +540,13 @@ static ssize_t ntc_show_temp(struct device *dev,
 
 static SENSOR_DEVICE_ATTR(temp1_type, S_IRUGO, ntc_show_type, NULL, 0);
 static SENSOR_DEVICE_ATTR(temp1_input, S_IRUGO, ntc_show_temp, NULL, 0);
-static DEVICE_ATTR(name, S_IRUGO, ntc_show_name, NULL);
 
-static struct attribute *ntc_attributes[] = {
-	&dev_attr_name.attr,
+static struct attribute *ntc_attrs[] = {
 	&sensor_dev_attr_temp1_type.dev_attr.attr,
 	&sensor_dev_attr_temp1_input.dev_attr.attr,
 	NULL,
 };
-
-static const struct attribute_group ntc_attr_group = {
-	.attrs = ntc_attributes,
-};
+ATTRIBUTE_GROUPS(ntc);
 
 static const struct thermal_zone_of_device_ops ntc_of_thermal_ops = {
 	.get_temp = ntc_read_temp,
@@ -579,33 +555,34 @@ static const struct thermal_zone_of_device_ops ntc_of_thermal_ops = {
 static int ntc_thermistor_probe(struct platform_device *pdev)
 {
 	struct thermal_zone_device *tz;
+	struct device *dev = &pdev->dev;
 	const struct of_device_id *of_id =
-			of_match_device(of_match_ptr(ntc_match), &pdev->dev);
+			of_match_device(of_match_ptr(ntc_match), dev);
 	const struct platform_device_id *pdev_id;
 	struct ntc_thermistor_platform_data *pdata;
+	struct device *hwmon_dev;
 	struct ntc_data *data;
-	int ret;
 
-	pdata = ntc_thermistor_parse_dt(pdev);
+	pdata = ntc_thermistor_parse_dt(dev);
 	if (IS_ERR(pdata))
 		return PTR_ERR(pdata);
 	else if (pdata == NULL)
-		pdata = dev_get_platdata(&pdev->dev);
+		pdata = dev_get_platdata(dev);
 
 	if (!pdata) {
-		dev_err(&pdev->dev, "No platform init data supplied.\n");
+		dev_err(dev, "No platform init data supplied.\n");
 		return -ENODEV;
 	}
 
 	/* Either one of the two is required. */
 	if (!pdata->read_uv && !pdata->read_ohm) {
-		dev_err(&pdev->dev,
+		dev_err(dev,
 			"Both read_uv and read_ohm missing. Need either one of the two.\n");
 		return -EINVAL;
 	}
 
 	if (pdata->read_uv && pdata->read_ohm) {
-		dev_warn(&pdev->dev,
+		dev_warn(dev,
 			 "Only one of read_uv and read_ohm is needed; ignoring read_uv.\n");
 		pdata->read_uv = NULL;
 	}
@@ -617,20 +594,17 @@ static int ntc_thermistor_probe(struct platform_device *pdev)
 				 NTC_CONNECTED_POSITIVE) ||
 				(pdata->connect != NTC_CONNECTED_POSITIVE &&
 				 pdata->connect != NTC_CONNECTED_GROUND))) {
-		dev_err(&pdev->dev,
-			"Required data to use read_uv not supplied.\n");
+		dev_err(dev, "Required data to use read_uv not supplied.\n");
 		return -EINVAL;
 	}
 
-	data = devm_kzalloc(&pdev->dev, sizeof(struct ntc_data), GFP_KERNEL);
+	data = devm_kzalloc(dev, sizeof(struct ntc_data), GFP_KERNEL);
 	if (!data)
 		return -ENOMEM;
 
 	pdev_id = of_id ? of_id->data : platform_get_device_id(pdev);
 
-	data->dev = &pdev->dev;
 	data->pdata = pdata;
-	strlcpy(data->name, pdev_id->name, sizeof(data->name));
 
 	switch (pdev_id->driver_data) {
 	case TYPE_NCPXXWB473:
@@ -654,49 +628,25 @@ static int ntc_thermistor_probe(struct platform_device *pdev)
 		data->n_comp = ARRAY_SIZE(ncpXXxh103);
 		break;
 	default:
-		dev_err(&pdev->dev, "Unknown device type: %lu(%s)\n",
+		dev_err(dev, "Unknown device type: %lu(%s)\n",
 				pdev_id->driver_data, pdev_id->name);
 		return -EINVAL;
 	}
 
-	platform_set_drvdata(pdev, data);
-
-	ret = sysfs_create_group(&data->dev->kobj, &ntc_attr_group);
-	if (ret) {
-		dev_err(data->dev, "unable to create sysfs files\n");
-		return ret;
-	}
-
-	data->hwmon_dev = hwmon_device_register(data->dev);
-	if (IS_ERR(data->hwmon_dev)) {
-		dev_err(data->dev, "unable to register as hwmon device.\n");
-		ret = PTR_ERR(data->hwmon_dev);
-		goto err_after_sysfs;
+	hwmon_dev = devm_hwmon_device_register_with_groups(dev, pdev_id->name,
+							   data, ntc_groups);
+	if (IS_ERR(hwmon_dev)) {
+		dev_err(dev, "unable to register as hwmon device.\n");
+		return PTR_ERR(hwmon_dev);
 	}
 
-	dev_info(&pdev->dev, "Thermistor type: %s successfully probed.\n",
-								pdev_id->name);
+	dev_info(dev, "Thermistor type: %s successfully probed.\n",
+		 pdev_id->name);
 
-	tz = devm_thermal_zone_of_sensor_register(data->dev, 0, data->dev,
+	tz = devm_thermal_zone_of_sensor_register(dev, 0, data,
 						  &ntc_of_thermal_ops);
 	if (IS_ERR(tz))
-		dev_dbg(&pdev->dev, "Failed to register to thermal fw.\n");
-
-	return 0;
-err_after_sysfs:
-	sysfs_remove_group(&data->dev->kobj, &ntc_attr_group);
-	ntc_iio_channel_release(pdata);
-	return ret;
-}
-
-static int ntc_thermistor_remove(struct platform_device *pdev)
-{
-	struct ntc_data *data = platform_get_drvdata(pdev);
-	struct ntc_thermistor_platform_data *pdata = data->pdata;
-
-	hwmon_device_unregister(data->hwmon_dev);
-	sysfs_remove_group(&data->dev->kobj, &ntc_attr_group);
-	ntc_iio_channel_release(pdata);
+		dev_dbg(dev, "Failed to register to thermal fw.\n");
 
 	return 0;
 }
@@ -707,7 +657,6 @@ static struct platform_driver ntc_thermistor_driver = {
 		.of_match_table = of_match_ptr(ntc_match),
 	},
 	.probe = ntc_thermistor_probe,
-	.remove = ntc_thermistor_remove,
 	.id_table = ntc_thermistor_id,
 };
 
diff --git a/drivers/hwmon/pmbus/Kconfig b/drivers/hwmon/pmbus/Kconfig
index 054d3d863802..cad1229b7e17 100644
--- a/drivers/hwmon/pmbus/Kconfig
+++ b/drivers/hwmon/pmbus/Kconfig
@@ -126,12 +126,12 @@ config SENSORS_TPS40422
 	  be called tps40422.
 
 config SENSORS_UCD9000
-	tristate "TI UCD90120, UCD90124, UCD9090, UCD90910"
+	tristate "TI UCD90120, UCD90124, UCD90160, UCD9090, UCD90910"
 	default n
 	help
 	  If you say yes here you get hardware monitoring support for TI
-	  UCD90120, UCD90124, UCD9090, UCD90910 Sequencer and System Health
-	  Controllers.
+	  UCD90120, UCD90124, UCD90160, UCD9090, UCD90910, Sequencer and System
+	  Health Controllers.
 
 	  This driver can also be built as a module. If so, the module will
 	  be called ucd9000.
diff --git a/drivers/hwmon/pmbus/pmbus.c b/drivers/hwmon/pmbus/pmbus.c
index 0a74991a60f0..44ca8a94873d 100644
--- a/drivers/hwmon/pmbus/pmbus.c
+++ b/drivers/hwmon/pmbus/pmbus.c
@@ -25,6 +25,7 @@
 #include <linux/slab.h>
 #include <linux/mutex.h>
 #include <linux/i2c.h>
+#include <linux/i2c/pmbus.h>
 #include "pmbus.h"
 
 /*
@@ -167,14 +168,26 @@ static int pmbus_probe(struct i2c_client *client,
 		       const struct i2c_device_id *id)
 {
 	struct pmbus_driver_info *info;
+	struct pmbus_platform_data *pdata = NULL;
+	struct device *dev = &client->dev;
 
-	info = devm_kzalloc(&client->dev, sizeof(struct pmbus_driver_info),
-			    GFP_KERNEL);
+	info = devm_kzalloc(dev, sizeof(struct pmbus_driver_info), GFP_KERNEL);
 	if (!info)
 		return -ENOMEM;
 
+	if (!strcmp(id->name, "dps460") || !strcmp(id->name, "dps800") ||
+	    !strcmp(id->name, "sgd009")) {
+		pdata = devm_kzalloc(dev, sizeof(struct pmbus_platform_data),
+				     GFP_KERNEL);
+		if (!pdata)
+			return -ENOMEM;
+
+		pdata->flags = PMBUS_SKIP_STATUS_CHECK;
+	}
+
 	info->pages = id->driver_data;
 	info->identify = pmbus_identify;
+	dev->platform_data = pdata;
 
 	return pmbus_do_probe(client, id, info);
 }
@@ -186,6 +199,8 @@ static const struct i2c_device_id pmbus_id[] = {
 	{"adp4000", 1},
 	{"bmr453", 1},
 	{"bmr454", 1},
+	{"dps460", 1},
+	{"dps800", 1},
 	{"mdt040", 1},
 	{"ncp4200", 1},
 	{"ncp4208", 1},
@@ -193,6 +208,7 @@ static const struct i2c_device_id pmbus_id[] = {
 	{"pdt006", 1},
 	{"pdt012", 1},
 	{"pmbus", 0},
+	{"sgd009", 1},
 	{"tps40400", 1},
 	{"tps544b20", 1},
 	{"tps544b25", 1},
diff --git a/drivers/hwmon/pmbus/ucd9000.c b/drivers/hwmon/pmbus/ucd9000.c
index fbb1479d3ad4..3e3aa950277f 100644
--- a/drivers/hwmon/pmbus/ucd9000.c
+++ b/drivers/hwmon/pmbus/ucd9000.c
@@ -28,7 +28,7 @@
 #include <linux/i2c/pmbus.h>
 #include "pmbus.h"
 
-enum chips { ucd9000, ucd90120, ucd90124, ucd9090, ucd90910 };
+enum chips { ucd9000, ucd90120, ucd90124, ucd90160, ucd9090, ucd90910 };
 
 #define UCD9000_MONITOR_CONFIG		0xd5
 #define UCD9000_NUM_PAGES		0xd6
@@ -112,6 +112,7 @@ static const struct i2c_device_id ucd9000_id[] = {
 	{"ucd9000", ucd9000},
 	{"ucd90120", ucd90120},
 	{"ucd90124", ucd90124},
+	{"ucd90160", ucd90160},
 	{"ucd9090", ucd9090},
 	{"ucd90910", ucd90910},
 	{}
diff --git a/drivers/hwmon/scpi-hwmon.c b/drivers/hwmon/scpi-hwmon.c
index 25b44e68926d..559a3dcd64d8 100644
--- a/drivers/hwmon/scpi-hwmon.c
+++ b/drivers/hwmon/scpi-hwmon.c
@@ -255,7 +255,6 @@ static const struct of_device_id scpi_of_match[] = {
 static struct platform_driver scpi_hwmon_platdrv = {
 	.driver = {
 		.name	= "scpi-hwmon",
-		.owner	= THIS_MODULE,
 		.of_match_table = scpi_of_match,
 	},
 	.probe		= scpi_hwmon_probe,
diff --git a/drivers/hwmon/tmp102.c b/drivers/hwmon/tmp102.c
index 8479ac5eb853..36bba2a816a4 100644
--- a/drivers/hwmon/tmp102.c
+++ b/drivers/hwmon/tmp102.c
@@ -25,7 +25,6 @@
 #include <linux/device.h>
 #include <linux/jiffies.h>
 #include <linux/regmap.h>
-#include <linux/thermal.h>
 #include <linux/of.h>
 
 #define	DRIVER_NAME "tmp102"
@@ -79,84 +78,113 @@ static inline u16 tmp102_mC_to_reg(int val)
 	return (val * 128) / 1000;
 }
 
-static int tmp102_read_temp(void *dev, int *temp)
+static int tmp102_read(struct device *dev, enum hwmon_sensor_types type,
+		       u32 attr, int channel, long *temp)
 {
 	struct tmp102 *tmp102 = dev_get_drvdata(dev);
-	unsigned int reg;
-	int ret;
-
-	if (time_before(jiffies, tmp102->ready_time)) {
-		dev_dbg(dev, "%s: Conversion not ready yet..\n", __func__);
-		return -EAGAIN;
+	unsigned int regval;
+	int err, reg;
+
+	switch (attr) {
+	case hwmon_temp_input:
+		/* Is it too early to return a conversion ? */
+		if (time_before(jiffies, tmp102->ready_time)) {
+			dev_dbg(dev, "%s: Conversion not ready yet..\n", __func__);
+			return -EAGAIN;
+		}
+		reg = TMP102_TEMP_REG;
+		break;
+	case hwmon_temp_max_hyst:
+		reg = TMP102_TLOW_REG;
+		break;
+	case hwmon_temp_max:
+		reg = TMP102_THIGH_REG;
+		break;
+	default:
+		return -EOPNOTSUPP;
 	}
 
-	ret = regmap_read(tmp102->regmap, TMP102_TEMP_REG, &reg);
-	if (ret < 0)
-		return ret;
-
-	*temp = tmp102_reg_to_mC(reg);
+	err = regmap_read(tmp102->regmap, reg, &regval);
+	if (err < 0)
+		return err;
+	*temp = tmp102_reg_to_mC(regval);
 
 	return 0;
 }
 
-static ssize_t tmp102_show_temp(struct device *dev,
-				struct device_attribute *attr,
-				char *buf)
+static int tmp102_write(struct device *dev, enum hwmon_sensor_types type,
+			u32 attr, int channel, long temp)
 {
-	struct sensor_device_attribute *sda = to_sensor_dev_attr(attr);
 	struct tmp102 *tmp102 = dev_get_drvdata(dev);
-	int regaddr = sda->index;
-	unsigned int reg;
-	int err;
-
-	if (regaddr == TMP102_TEMP_REG &&
-	    time_before(jiffies, tmp102->ready_time))
-		return -EAGAIN;
-
-	err = regmap_read(tmp102->regmap, regaddr, &reg);
-	if (err < 0)
-		return err;
+	int reg;
+
+	switch (attr) {
+	case hwmon_temp_max_hyst:
+		reg = TMP102_TLOW_REG;
+		break;
+	case hwmon_temp_max:
+		reg = TMP102_THIGH_REG;
+		break;
+	default:
+		return -EOPNOTSUPP;
+	}
 
-	return sprintf(buf, "%d\n", tmp102_reg_to_mC(reg));
+	temp = clamp_val(temp, -256000, 255000);
+	return regmap_write(tmp102->regmap, reg, tmp102_mC_to_reg(temp));
 }
 
-static ssize_t tmp102_set_temp(struct device *dev,
-			       struct device_attribute *attr,
-			       const char *buf, size_t count)
+static umode_t tmp102_is_visible(const void *data, enum hwmon_sensor_types type,
+				 u32 attr, int channel)
 {
-	struct sensor_device_attribute *sda = to_sensor_dev_attr(attr);
-	struct tmp102 *tmp102 = dev_get_drvdata(dev);
-	int reg = sda->index;
-	long val;
-	int err;
-
-	if (kstrtol(buf, 10, &val) < 0)
-		return -EINVAL;
-	val = clamp_val(val, -256000, 255000);
-
-	err = regmap_write(tmp102->regmap, reg, tmp102_mC_to_reg(val));
-	return err ? : count;
+	if (type != hwmon_temp)
+		return 0;
+
+	switch (attr) {
+	case hwmon_temp_input:
+		return S_IRUGO;
+	case hwmon_temp_max_hyst:
+	case hwmon_temp_max:
+		return S_IRUGO | S_IWUSR;
+	default:
+		return 0;
+	}
 }
 
-static SENSOR_DEVICE_ATTR(temp1_input, S_IRUGO, tmp102_show_temp, NULL,
-			  TMP102_TEMP_REG);
+static u32 tmp102_chip_config[] = {
+	HWMON_C_REGISTER_TZ,
+	0
+};
+
+static const struct hwmon_channel_info tmp102_chip = {
+	.type = hwmon_chip,
+	.config = tmp102_chip_config,
+};
 
-static SENSOR_DEVICE_ATTR(temp1_max_hyst, S_IWUSR | S_IRUGO, tmp102_show_temp,
-			  tmp102_set_temp, TMP102_TLOW_REG);
+static u32 tmp102_temp_config[] = {
+	HWMON_T_INPUT | HWMON_T_MAX | HWMON_T_MAX_HYST,
+	0
+};
 
-static SENSOR_DEVICE_ATTR(temp1_max, S_IWUSR | S_IRUGO, tmp102_show_temp,
-			  tmp102_set_temp, TMP102_THIGH_REG);
+static const struct hwmon_channel_info tmp102_temp = {
+	.type = hwmon_temp,
+	.config = tmp102_temp_config,
+};
 
-static struct attribute *tmp102_attrs[] = {
-	&sensor_dev_attr_temp1_input.dev_attr.attr,
-	&sensor_dev_attr_temp1_max_hyst.dev_attr.attr,
-	&sensor_dev_attr_temp1_max.dev_attr.attr,
+static const struct hwmon_channel_info *tmp102_info[] = {
+	&tmp102_chip,
+	&tmp102_temp,
 	NULL
 };
-ATTRIBUTE_GROUPS(tmp102);
 
-static const struct thermal_zone_of_device_ops tmp102_of_thermal_ops = {
-	.get_temp = tmp102_read_temp,
+static const struct hwmon_ops tmp102_hwmon_ops = {
+	.is_visible = tmp102_is_visible,
+	.read = tmp102_read,
+	.write = tmp102_write,
+};
+
+static const struct hwmon_chip_info tmp102_chip_info = {
+	.ops = &tmp102_hwmon_ops,
+	.info = tmp102_info,
 };
 
 static void tmp102_restore_config(void *data)
@@ -188,7 +216,7 @@ static const struct regmap_config tmp102_regmap_config = {
 };
 
 static int tmp102_probe(struct i2c_client *client,
-				  const struct i2c_device_id *id)
+			const struct i2c_device_id *id)
 {
 	struct device *dev = &client->dev;
 	struct device *hwmon_dev;
@@ -249,16 +277,14 @@ static int tmp102_probe(struct i2c_client *client,
 		tmp102->ready_time += msecs_to_jiffies(CONVERSION_TIME_MS);
 	}
 
-	hwmon_dev = devm_hwmon_device_register_with_groups(dev, client->name,
-							   tmp102,
-							   tmp102_groups);
+	hwmon_dev = devm_hwmon_device_register_with_info(dev, client->name,
+							 tmp102,
+							 &tmp102_chip_info,
+							 NULL);
 	if (IS_ERR(hwmon_dev)) {
 		dev_dbg(dev, "unable to register hwmon device\n");
 		return PTR_ERR(hwmon_dev);
 	}
-	devm_thermal_zone_of_sensor_register(hwmon_dev, 0, hwmon_dev,
-					     &tmp102_of_thermal_ops);
-
 	dev_info(dev, "initialized\n");
 
 	return 0;
diff --git a/drivers/hwmon/tmp421.c b/drivers/hwmon/tmp421.c
index 85d48d80822a..bfb98b96c781 100644
--- a/drivers/hwmon/tmp421.c
+++ b/drivers/hwmon/tmp421.c
@@ -72,6 +72,10 @@ MODULE_DEVICE_TABLE(i2c, tmp421_id);
 struct tmp421_data {
 	struct i2c_client *client;
 	struct mutex update_lock;
+	u32 temp_config[5];
+	struct hwmon_channel_info temp_info;
+	const struct hwmon_channel_info *info[2];
+	struct hwmon_chip_info chip;
 	char valid;
 	unsigned long last_updated;
 	int channels;
@@ -125,85 +129,46 @@ static struct tmp421_data *tmp421_update_device(struct device *dev)
 	return data;
 }
 
-static ssize_t show_temp_value(struct device *dev,
-			       struct device_attribute *devattr, char *buf)
+static int tmp421_read(struct device *dev, enum hwmon_sensor_types type,
+		       u32 attr, int channel, long *val)
 {
-	int index = to_sensor_dev_attr(devattr)->index;
-	struct tmp421_data *data = tmp421_update_device(dev);
-	int temp;
-
-	mutex_lock(&data->update_lock);
-	if (data->config & TMP421_CONFIG_RANGE)
-		temp = temp_from_u16(data->temp[index]);
-	else
-		temp = temp_from_s16(data->temp[index]);
-	mutex_unlock(&data->update_lock);
-
-	return sprintf(buf, "%d\n", temp);
-}
+	struct tmp421_data *tmp421 = tmp421_update_device(dev);
+
+	switch (attr) {
+	case hwmon_temp_input:
+		if (tmp421->config & TMP421_CONFIG_RANGE)
+			*val = temp_from_u16(tmp421->temp[channel]);
+		else
+			*val = temp_from_s16(tmp421->temp[channel]);
+		return 0;
+	case hwmon_temp_fault:
+		/*
+		 * The OPEN bit signals a fault. This is bit 0 of the temperature
+		 * register (low byte).
+		 */
+		*val = tmp421->temp[channel] & 0x01;
+		return 0;
+	default:
+		return -EOPNOTSUPP;
+	}
 
-static ssize_t show_fault(struct device *dev,
-			  struct device_attribute *devattr, char *buf)
-{
-	int index = to_sensor_dev_attr(devattr)->index;
-	struct tmp421_data *data = tmp421_update_device(dev);
-
-	/*
-	 * The OPEN bit signals a fault. This is bit 0 of the temperature
-	 * register (low byte).
-	 */
-	if (data->temp[index] & 0x01)
-		return sprintf(buf, "1\n");
-	else
-		return sprintf(buf, "0\n");
 }
 
-static umode_t tmp421_is_visible(struct kobject *kobj, struct attribute *a,
-				int n)
+static umode_t tmp421_is_visible(const void *data, enum hwmon_sensor_types type,
+				 u32 attr, int channel)
 {
-	struct device *dev = container_of(kobj, struct device, kobj);
-	struct tmp421_data *data = dev_get_drvdata(dev);
-	struct device_attribute *devattr;
-	unsigned int index;
-
-	devattr = container_of(a, struct device_attribute, attr);
-	index = to_sensor_dev_attr(devattr)->index;
-
-	if (index < data->channels)
-		return a->mode;
-
-	return 0;
+	switch (attr) {
+	case hwmon_temp_fault:
+		if (channel == 0)
+			return 0;
+		return S_IRUGO;
+	case hwmon_temp_input:
+		return S_IRUGO;
+	default:
+		return 0;
+	}
 }
 
-static SENSOR_DEVICE_ATTR(temp1_input, S_IRUGO, show_temp_value, NULL, 0);
-static SENSOR_DEVICE_ATTR(temp2_input, S_IRUGO, show_temp_value, NULL, 1);
-static SENSOR_DEVICE_ATTR(temp2_fault, S_IRUGO, show_fault, NULL, 1);
-static SENSOR_DEVICE_ATTR(temp3_input, S_IRUGO, show_temp_value, NULL, 2);
-static SENSOR_DEVICE_ATTR(temp3_fault, S_IRUGO, show_fault, NULL, 2);
-static SENSOR_DEVICE_ATTR(temp4_input, S_IRUGO, show_temp_value, NULL, 3);
-static SENSOR_DEVICE_ATTR(temp4_fault, S_IRUGO, show_fault, NULL, 3);
-
-static struct attribute *tmp421_attr[] = {
-	&sensor_dev_attr_temp1_input.dev_attr.attr,
-	&sensor_dev_attr_temp2_input.dev_attr.attr,
-	&sensor_dev_attr_temp2_fault.dev_attr.attr,
-	&sensor_dev_attr_temp3_input.dev_attr.attr,
-	&sensor_dev_attr_temp3_fault.dev_attr.attr,
-	&sensor_dev_attr_temp4_input.dev_attr.attr,
-	&sensor_dev_attr_temp4_fault.dev_attr.attr,
-	NULL
-};
-
-static const struct attribute_group tmp421_group = {
-	.attrs = tmp421_attr,
-	.is_visible = tmp421_is_visible,
-};
-
-static const struct attribute_group *tmp421_groups[] = {
-	&tmp421_group,
-	NULL
-};
-
 static int tmp421_init_client(struct i2c_client *client)
 {
 	int config, config_orig;
@@ -289,13 +254,18 @@ static int tmp421_detect(struct i2c_client *client,
 	return 0;
 }
 
+static const struct hwmon_ops tmp421_ops = {
+	.is_visible = tmp421_is_visible,
+	.read = tmp421_read,
+};
+
 static int tmp421_probe(struct i2c_client *client,
 			const struct i2c_device_id *id)
 {
 	struct device *dev = &client->dev;
 	struct device *hwmon_dev;
 	struct tmp421_data *data;
-	int err;
+	int i, err;
 
 	data = devm_kzalloc(dev, sizeof(struct tmp421_data), GFP_KERNEL);
 	if (!data)
@@ -309,8 +279,21 @@ static int tmp421_probe(struct i2c_client *client,
 	if (err)
 		return err;
 
-	hwmon_dev = devm_hwmon_device_register_with_groups(dev, client->name,
-							   data, tmp421_groups);
+	for (i = 0; i < data->channels; i++)
+		data->temp_config[i] = HWMON_T_INPUT | HWMON_T_FAULT;
+
+	data->chip.ops = &tmp421_ops;
+	data->chip.info = data->info;
+
+	data->info[0] = &data->temp_info;
+
+	data->temp_info.type = hwmon_temp;
+	data->temp_info.config = data->temp_config;
+
+	hwmon_dev = devm_hwmon_device_register_with_info(dev, client->name,
+							 data,
+							 &data->chip,
+							 NULL);
 	return PTR_ERR_OR_ZERO(hwmon_dev);
 }
 
diff --git a/drivers/hwmon/xgene-hwmon.c b/drivers/hwmon/xgene-hwmon.c
new file mode 100644
index 000000000000..9c0dbb8191ad
--- /dev/null
+++ b/drivers/hwmon/xgene-hwmon.c
@@ -0,0 +1,787 @@
+/*
+ * APM X-Gene SoC Hardware Monitoring Driver
+ *
+ * Copyright (c) 2016, Applied Micro Circuits Corporation
+ * Author: Loc Ho <lho@apm.com>
+ *         Hoan Tran <hotran@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 the following features:
+ *  - Retrieve CPU total power (uW)
+ *  - Retrieve IO total power (uW)
+ *  - Retrieve SoC temperature (milli-degree C) and alarm
+ */
+#include <linux/acpi.h>
+#include <linux/dma-mapping.h>
+#include <linux/hwmon.h>
+#include <linux/hwmon-sysfs.h>
+#include <linux/io.h>
+#include <linux/interrupt.h>
+#include <linux/kfifo.h>
+#include <linux/mailbox_controller.h>
+#include <linux/mailbox_client.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+
+#include <acpi/pcc.h>
+
+/* SLIMpro message defines */
+#define MSG_TYPE_DBG			0
+#define MSG_TYPE_ERR			7
+#define MSG_TYPE_PWRMGMT		9
+
+#define MSG_TYPE(v)			(((v) & 0xF0000000) >> 28)
+#define MSG_TYPE_SET(v)			(((v) << 28) & 0xF0000000)
+#define MSG_SUBTYPE(v)			(((v) & 0x0F000000) >> 24)
+#define MSG_SUBTYPE_SET(v)		(((v) << 24) & 0x0F000000)
+
+#define DBG_SUBTYPE_SENSOR_READ		4
+#define SENSOR_RD_MSG			0x04FFE902
+#define SENSOR_RD_EN_ADDR(a)		((a) & 0x000FFFFF)
+#define PMD_PWR_REG			0x20
+#define PMD_PWR_MW_REG			0x26
+#define SOC_PWR_REG			0x21
+#define SOC_PWR_MW_REG			0x27
+#define SOC_TEMP_REG			0x10
+
+#define TEMP_NEGATIVE_BIT		8
+#define SENSOR_INVALID_DATA		BIT(15)
+
+#define PWRMGMT_SUBTYPE_TPC		1
+#define TPC_ALARM			2
+#define TPC_GET_ALARM			3
+#define TPC_CMD(v)			(((v) & 0x00FF0000) >> 16)
+#define TPC_CMD_SET(v)			(((v) << 16) & 0x00FF0000)
+#define TPC_EN_MSG(hndl, cmd, type) \
+	(MSG_TYPE_SET(MSG_TYPE_PWRMGMT) | \
+	MSG_SUBTYPE_SET(hndl) | TPC_CMD_SET(cmd) | type)
+
+/* PCC defines */
+#define PCC_SIGNATURE_MASK		0x50424300
+#define PCCC_GENERATE_DB_INT		BIT(15)
+#define PCCS_CMD_COMPLETE		BIT(0)
+#define PCCS_SCI_DOORBEL		BIT(1)
+#define PCCS_PLATFORM_NOTIFICATION	BIT(3)
+/*
+ * Arbitrary retries in case the remote processor is slow to respond
+ * to PCC commands
+ */
+#define PCC_NUM_RETRIES			500
+
+#define ASYNC_MSG_FIFO_SIZE		16
+#define MBOX_OP_TIMEOUTMS		1000
+
+#define WATT_TO_mWATT(x)		((x) * 1000)
+#define mWATT_TO_uWATT(x)		((x) * 1000)
+#define CELSIUS_TO_mCELSIUS(x)		((x) * 1000)
+
+#define to_xgene_hwmon_dev(cl)		\
+	container_of(cl, struct xgene_hwmon_dev, mbox_client)
+
+struct slimpro_resp_msg {
+	u32 msg;
+	u32 param1;
+	u32 param2;
+} __packed;
+
+struct xgene_hwmon_dev {
+	struct device		*dev;
+	struct mbox_chan	*mbox_chan;
+	struct mbox_client	mbox_client;
+	int			mbox_idx;
+
+	spinlock_t		kfifo_lock;
+	struct mutex		rd_mutex;
+	struct completion	rd_complete;
+	int			resp_pending;
+	struct slimpro_resp_msg sync_msg;
+
+	struct work_struct	workq;
+	struct kfifo_rec_ptr_1	async_msg_fifo;
+
+	struct device		*hwmon_dev;
+	bool			temp_critical_alarm;
+
+	phys_addr_t		comm_base_addr;
+	void			*pcc_comm_addr;
+	u64			usecs_lat;
+};
+
+/*
+ * This function tests and clears a bitmask then returns its old value
+ */
+static u16 xgene_word_tst_and_clr(u16 *addr, u16 mask)
+{
+	u16 ret, val;
+
+	val = le16_to_cpu(READ_ONCE(*addr));
+	ret = val & mask;
+	val &= ~mask;
+	WRITE_ONCE(*addr, cpu_to_le16(val));
+
+	return ret;
+}
+
+static int xgene_hwmon_pcc_rd(struct xgene_hwmon_dev *ctx, u32 *msg)
+{
+	struct acpi_pcct_shared_memory *generic_comm_base = ctx->pcc_comm_addr;
+	u32 *ptr = (void *)(generic_comm_base + 1);
+	int rc, i;
+	u16 val;
+
+	mutex_lock(&ctx->rd_mutex);
+	init_completion(&ctx->rd_complete);
+	ctx->resp_pending = true;
+
+	/* Write signature for subspace */
+	WRITE_ONCE(generic_comm_base->signature,
+		   cpu_to_le32(PCC_SIGNATURE_MASK | ctx->mbox_idx));
+
+	/* Write to the shared command region */
+	WRITE_ONCE(generic_comm_base->command,
+		   cpu_to_le16(MSG_TYPE(msg[0]) | PCCC_GENERATE_DB_INT));
+
+	/* Flip CMD COMPLETE bit */
+	val = le16_to_cpu(READ_ONCE(generic_comm_base->status));
+	val &= ~PCCS_CMD_COMPLETE;
+	WRITE_ONCE(generic_comm_base->status, cpu_to_le16(val));
+
+	/* Copy the message to the PCC comm space */
+	for (i = 0; i < sizeof(struct slimpro_resp_msg) / 4; i++)
+		WRITE_ONCE(ptr[i], cpu_to_le32(msg[i]));
+
+	/* Ring the doorbell */
+	rc = mbox_send_message(ctx->mbox_chan, msg);
+	if (rc < 0) {
+		dev_err(ctx->dev, "Mailbox send error %d\n", rc);
+		goto err;
+	}
+	if (!wait_for_completion_timeout(&ctx->rd_complete,
+					 usecs_to_jiffies(ctx->usecs_lat))) {
+		dev_err(ctx->dev, "Mailbox operation timed out\n");
+		rc = -ETIMEDOUT;
+		goto err;
+	}
+
+	/* Check for error message */
+	if (MSG_TYPE(ctx->sync_msg.msg) == MSG_TYPE_ERR) {
+		rc = -EINVAL;
+		goto err;
+	}
+
+	msg[0] = ctx->sync_msg.msg;
+	msg[1] = ctx->sync_msg.param1;
+	msg[2] = ctx->sync_msg.param2;
+
+err:
+	mbox_chan_txdone(ctx->mbox_chan, 0);
+	ctx->resp_pending = false;
+	mutex_unlock(&ctx->rd_mutex);
+	return rc;
+}
+
+static int xgene_hwmon_rd(struct xgene_hwmon_dev *ctx, u32 *msg)
+{
+	int rc;
+
+	mutex_lock(&ctx->rd_mutex);
+	init_completion(&ctx->rd_complete);
+	ctx->resp_pending = true;
+
+	rc = mbox_send_message(ctx->mbox_chan, msg);
+	if (rc < 0) {
+		dev_err(ctx->dev, "Mailbox send error %d\n", rc);
+		goto err;
+	}
+
+	if (!wait_for_completion_timeout(&ctx->rd_complete,
+					 msecs_to_jiffies(MBOX_OP_TIMEOUTMS))) {
+		dev_err(ctx->dev, "Mailbox operation timed out\n");
+		rc = -ETIMEDOUT;
+		goto err;
+	}
+
+	/* Check for error message */
+	if (MSG_TYPE(ctx->sync_msg.msg) == MSG_TYPE_ERR) {
+		rc = -EINVAL;
+		goto err;
+	}
+
+	msg[0] = ctx->sync_msg.msg;
+	msg[1] = ctx->sync_msg.param1;
+	msg[2] = ctx->sync_msg.param2;
+
+err:
+	ctx->resp_pending = false;
+	mutex_unlock(&ctx->rd_mutex);
+	return rc;
+}
+
+static int xgene_hwmon_reg_map_rd(struct xgene_hwmon_dev *ctx, u32 addr,
+				  u32 *data)
+{
+	u32 msg[3];
+	int rc;
+
+	msg[0] = SENSOR_RD_MSG;
+	msg[1] = SENSOR_RD_EN_ADDR(addr);
+	msg[2] = 0;
+
+	if (acpi_disabled)
+		rc = xgene_hwmon_rd(ctx, msg);
+	else
+		rc = xgene_hwmon_pcc_rd(ctx, msg);
+
+	if (rc < 0)
+		return rc;
+
+	/*
+	 * Check if sensor data is valid.
+	 */
+	if (msg[1] & SENSOR_INVALID_DATA)
+		return -ENODATA;
+
+	*data = msg[1];
+
+	return rc;
+}
+
+static int xgene_hwmon_get_notification_msg(struct xgene_hwmon_dev *ctx,
+					    u32 *amsg)
+{
+	u32 msg[3];
+	int rc;
+
+	msg[0] = TPC_EN_MSG(PWRMGMT_SUBTYPE_TPC, TPC_GET_ALARM, 0);
+	msg[1] = 0;
+	msg[2] = 0;
+
+	rc = xgene_hwmon_pcc_rd(ctx, msg);
+	if (rc < 0)
+		return rc;
+
+	amsg[0] = msg[0];
+	amsg[1] = msg[1];
+	amsg[2] = msg[2];
+
+	return rc;
+}
+
+static int xgene_hwmon_get_cpu_pwr(struct xgene_hwmon_dev *ctx, u32 *val)
+{
+	u32 watt, mwatt;
+	int rc;
+
+	rc = xgene_hwmon_reg_map_rd(ctx, PMD_PWR_REG, &watt);
+	if (rc < 0)
+		return rc;
+
+	rc = xgene_hwmon_reg_map_rd(ctx, PMD_PWR_MW_REG, &mwatt);
+	if (rc < 0)
+		return rc;
+
+	*val = WATT_TO_mWATT(watt) + mwatt;
+	return 0;
+}
+
+static int xgene_hwmon_get_io_pwr(struct xgene_hwmon_dev *ctx, u32 *val)
+{
+	u32 watt, mwatt;
+	int rc;
+
+	rc = xgene_hwmon_reg_map_rd(ctx, SOC_PWR_REG, &watt);
+	if (rc < 0)
+		return rc;
+
+	rc = xgene_hwmon_reg_map_rd(ctx, SOC_PWR_MW_REG, &mwatt);
+	if (rc < 0)
+		return rc;
+
+	*val = WATT_TO_mWATT(watt) + mwatt;
+	return 0;
+}
+
+static int xgene_hwmon_get_temp(struct xgene_hwmon_dev *ctx, u32 *val)
+{
+	return xgene_hwmon_reg_map_rd(ctx, SOC_TEMP_REG, val);
+}
+
+/*
+ * Sensor temperature/power functions
+ */
+static ssize_t temp1_input_show(struct device *dev,
+				struct device_attribute *attr,
+				char *buf)
+{
+	struct xgene_hwmon_dev *ctx = dev_get_drvdata(dev);
+	int rc, temp;
+	u32 val;
+
+	rc = xgene_hwmon_get_temp(ctx, &val);
+	if (rc < 0)
+		return rc;
+
+	temp = sign_extend32(val, TEMP_NEGATIVE_BIT);
+
+	return snprintf(buf, PAGE_SIZE, "%d\n", CELSIUS_TO_mCELSIUS(temp));
+}
+
+static ssize_t temp1_label_show(struct device *dev,
+				struct device_attribute *attr,
+				char *buf)
+{
+	return snprintf(buf, PAGE_SIZE, "SoC Temperature\n");
+}
+
+static ssize_t temp1_critical_alarm_show(struct device *dev,
+					 struct device_attribute *devattr,
+					 char *buf)
+{
+	struct xgene_hwmon_dev *ctx = dev_get_drvdata(dev);
+
+	return snprintf(buf, PAGE_SIZE, "%d\n", ctx->temp_critical_alarm);
+}
+
+static ssize_t power1_label_show(struct device *dev,
+				 struct device_attribute *attr,
+				 char *buf)
+{
+	return snprintf(buf, PAGE_SIZE, "CPU power\n");
+}
+
+static ssize_t power2_label_show(struct device *dev,
+				 struct device_attribute *attr,
+				 char *buf)
+{
+	return snprintf(buf, PAGE_SIZE, "IO power\n");
+}
+
+static ssize_t power1_input_show(struct device *dev,
+				 struct device_attribute *attr,
+				 char *buf)
+{
+	struct xgene_hwmon_dev *ctx = dev_get_drvdata(dev);
+	u32 val;
+	int rc;
+
+	rc = xgene_hwmon_get_cpu_pwr(ctx, &val);
+	if (rc < 0)
+		return rc;
+
+	return snprintf(buf, PAGE_SIZE, "%u\n", mWATT_TO_uWATT(val));
+}
+
+static ssize_t power2_input_show(struct device *dev,
+				 struct device_attribute *attr,
+				 char *buf)
+{
+	struct xgene_hwmon_dev *ctx = dev_get_drvdata(dev);
+	u32 val;
+	int rc;
+
+	rc = xgene_hwmon_get_io_pwr(ctx, &val);
+	if (rc < 0)
+		return rc;
+
+	return snprintf(buf, PAGE_SIZE, "%u\n", mWATT_TO_uWATT(val));
+}
+
+static DEVICE_ATTR_RO(temp1_label);
+static DEVICE_ATTR_RO(temp1_input);
+static DEVICE_ATTR_RO(temp1_critical_alarm);
+static DEVICE_ATTR_RO(power1_label);
+static DEVICE_ATTR_RO(power1_input);
+static DEVICE_ATTR_RO(power2_label);
+static DEVICE_ATTR_RO(power2_input);
+
+static struct attribute *xgene_hwmon_attrs[] = {
+	&dev_attr_temp1_label.attr,
+	&dev_attr_temp1_input.attr,
+	&dev_attr_temp1_critical_alarm.attr,
+	&dev_attr_power1_label.attr,
+	&dev_attr_power1_input.attr,
+	&dev_attr_power2_label.attr,
+	&dev_attr_power2_input.attr,
+	NULL,
+};
+
+ATTRIBUTE_GROUPS(xgene_hwmon);
+
+static int xgene_hwmon_tpc_alarm(struct xgene_hwmon_dev *ctx,
+				 struct slimpro_resp_msg *amsg)
+{
+	ctx->temp_critical_alarm = !!amsg->param2;
+	sysfs_notify(&ctx->dev->kobj, NULL, "temp1_critical_alarm");
+
+	return 0;
+}
+
+static void xgene_hwmon_process_pwrmsg(struct xgene_hwmon_dev *ctx,
+				       struct slimpro_resp_msg *amsg)
+{
+	if ((MSG_SUBTYPE(amsg->msg) == PWRMGMT_SUBTYPE_TPC) &&
+	    (TPC_CMD(amsg->msg) == TPC_ALARM))
+		xgene_hwmon_tpc_alarm(ctx, amsg);
+}
+
+/*
+ * This function is called to process async work queue
+ */
+static void xgene_hwmon_evt_work(struct work_struct *work)
+{
+	struct slimpro_resp_msg amsg;
+	struct xgene_hwmon_dev *ctx;
+	int ret;
+
+	ctx = container_of(work, struct xgene_hwmon_dev, workq);
+	while (kfifo_out_spinlocked(&ctx->async_msg_fifo, &amsg,
+				    sizeof(struct slimpro_resp_msg),
+				    &ctx->kfifo_lock)) {
+		/*
+		 * If PCC, send a consumer command to Platform to get info
+		 * If Slimpro Mailbox, get message from specific FIFO
+		 */
+		if (!acpi_disabled) {
+			ret = xgene_hwmon_get_notification_msg(ctx,
+							       (u32 *)&amsg);
+			if (ret < 0)
+				continue;
+		}
+
+		if (MSG_TYPE(amsg.msg) == MSG_TYPE_PWRMGMT)
+			xgene_hwmon_process_pwrmsg(ctx, &amsg);
+	}
+}
+
+static int xgene_hwmon_rx_ready(struct xgene_hwmon_dev *ctx, void *msg)
+{
+	if (IS_ERR_OR_NULL(ctx->hwmon_dev) && !ctx->resp_pending) {
+		/* Enqueue to the FIFO */
+		kfifo_in_spinlocked(&ctx->async_msg_fifo, msg,
+				    sizeof(struct slimpro_resp_msg),
+				    &ctx->kfifo_lock);
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+/*
+ * This function is called when the SLIMpro Mailbox received a message
+ */
+static void xgene_hwmon_rx_cb(struct mbox_client *cl, void *msg)
+{
+	struct xgene_hwmon_dev *ctx = to_xgene_hwmon_dev(cl);
+
+	/*
+	 * While the driver registers with the mailbox framework, an interrupt
+	 * can be pending before the probe function completes its
+	 * initialization. If such condition occurs, just queue up the message
+	 * as the driver is not ready for servicing the callback.
+	 */
+	if (xgene_hwmon_rx_ready(ctx, msg) < 0)
+		return;
+
+	/*
+	 * Response message format:
+	 * msg[0] is the return code of the operation
+	 * msg[1] is the first parameter word
+	 * msg[2] is the second parameter word
+	 *
+	 * As message only supports dword size, just assign it.
+	 */
+
+	/* Check for sync query */
+	if (ctx->resp_pending &&
+	    ((MSG_TYPE(((u32 *)msg)[0]) == MSG_TYPE_ERR) ||
+	     (MSG_TYPE(((u32 *)msg)[0]) == MSG_TYPE_DBG &&
+	      MSG_SUBTYPE(((u32 *)msg)[0]) == DBG_SUBTYPE_SENSOR_READ) ||
+	     (MSG_TYPE(((u32 *)msg)[0]) == MSG_TYPE_PWRMGMT &&
+	      MSG_SUBTYPE(((u32 *)msg)[0]) == PWRMGMT_SUBTYPE_TPC &&
+	      TPC_CMD(((u32 *)msg)[0]) == TPC_ALARM))) {
+		ctx->sync_msg.msg = ((u32 *)msg)[0];
+		ctx->sync_msg.param1 = ((u32 *)msg)[1];
+		ctx->sync_msg.param2 = ((u32 *)msg)[2];
+
+		/* Operation waiting for response */
+		complete(&ctx->rd_complete);
+
+		return;
+	}
+
+	/* Enqueue to the FIFO */
+	kfifo_in_spinlocked(&ctx->async_msg_fifo, msg,
+			    sizeof(struct slimpro_resp_msg), &ctx->kfifo_lock);
+	/* Schedule the bottom handler */
+	schedule_work(&ctx->workq);
+}
+
+/*
+ * This function is called when the PCC Mailbox received a message
+ */
+static void xgene_hwmon_pcc_rx_cb(struct mbox_client *cl, void *msg)
+{
+	struct xgene_hwmon_dev *ctx = to_xgene_hwmon_dev(cl);
+	struct acpi_pcct_shared_memory *generic_comm_base = ctx->pcc_comm_addr;
+	struct slimpro_resp_msg amsg;
+
+	/*
+	 * While the driver registers with the mailbox framework, an interrupt
+	 * can be pending before the probe function completes its
+	 * initialization. If such condition occurs, just queue up the message
+	 * as the driver is not ready for servicing the callback.
+	 */
+	if (xgene_hwmon_rx_ready(ctx, &amsg) < 0)
+		return;
+
+	msg = generic_comm_base + 1;
+	/* Check if platform sends interrupt */
+	if (!xgene_word_tst_and_clr(&generic_comm_base->status,
+				    PCCS_SCI_DOORBEL))
+		return;
+
+	/*
+	 * Response message format:
+	 * msg[0] is the return code of the operation
+	 * msg[1] is the first parameter word
+	 * msg[2] is the second parameter word
+	 *
+	 * As message only supports dword size, just assign it.
+	 */
+
+	/* Check for sync query */
+	if (ctx->resp_pending &&
+	    ((MSG_TYPE(((u32 *)msg)[0]) == MSG_TYPE_ERR) ||
+	     (MSG_TYPE(((u32 *)msg)[0]) == MSG_TYPE_DBG &&
+	      MSG_SUBTYPE(((u32 *)msg)[0]) == DBG_SUBTYPE_SENSOR_READ) ||
+	     (MSG_TYPE(((u32 *)msg)[0]) == MSG_TYPE_PWRMGMT &&
+	      MSG_SUBTYPE(((u32 *)msg)[0]) == PWRMGMT_SUBTYPE_TPC &&
+	      TPC_CMD(((u32 *)msg)[0]) == TPC_ALARM))) {
+		/* Check if platform completes command */
+		if (xgene_word_tst_and_clr(&generic_comm_base->status,
+					   PCCS_CMD_COMPLETE)) {
+			ctx->sync_msg.msg = ((u32 *)msg)[0];
+			ctx->sync_msg.param1 = ((u32 *)msg)[1];
+			ctx->sync_msg.param2 = ((u32 *)msg)[2];
+
+			/* Operation waiting for response */
+			complete(&ctx->rd_complete);
+
+			return;
+		}
+	}
+
+	/*
+	 * Platform notifies interrupt to OSPM.
+	 * OPSM schedules a consumer command to get this information
+	 * in a workqueue. Platform must wait until OSPM has issued
+	 * a consumer command that serves this notification.
+	 */
+
+	/* Enqueue to the FIFO */
+	kfifo_in_spinlocked(&ctx->async_msg_fifo, &amsg,
+			    sizeof(struct slimpro_resp_msg), &ctx->kfifo_lock);
+	/* Schedule the bottom handler */
+	schedule_work(&ctx->workq);
+}
+
+static void xgene_hwmon_tx_done(struct mbox_client *cl, void *msg, int ret)
+{
+	if (ret) {
+		dev_dbg(cl->dev, "TX did not complete: CMD sent:%x, ret:%d\n",
+			*(u16 *)msg, ret);
+	} else {
+		dev_dbg(cl->dev, "TX completed. CMD sent:%x, ret:%d\n",
+			*(u16 *)msg, ret);
+	}
+}
+
+static int xgene_hwmon_probe(struct platform_device *pdev)
+{
+	struct xgene_hwmon_dev *ctx;
+	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;
+
+	spin_lock_init(&ctx->kfifo_lock);
+	mutex_init(&ctx->rd_mutex);
+
+	rc = kfifo_alloc(&ctx->async_msg_fifo,
+			 sizeof(struct slimpro_resp_msg) * ASYNC_MSG_FIFO_SIZE,
+			 GFP_KERNEL);
+	if (rc)
+		goto out_mbox_free;
+
+	INIT_WORK(&ctx->workq, xgene_hwmon_evt_work);
+
+	/* Request mailbox channel */
+	cl->dev = &pdev->dev;
+	cl->tx_done = xgene_hwmon_tx_done;
+	cl->tx_block = false;
+	cl->tx_tout = MBOX_OP_TIMEOUTMS;
+	cl->knows_txdone = false;
+	if (acpi_disabled) {
+		cl->rx_callback = xgene_hwmon_rx_cb;
+		ctx->mbox_chan = mbox_request_channel(cl, 0);
+		if (IS_ERR(ctx->mbox_chan)) {
+			dev_err(&pdev->dev,
+				"SLIMpro mailbox channel request failed\n");
+			return -ENODEV;
+		}
+	} else {
+		struct acpi_pcct_hw_reduced *cppc_ss;
+
+		if (device_property_read_u32(&pdev->dev, "pcc-channel",
+					     &ctx->mbox_idx)) {
+			dev_err(&pdev->dev, "no pcc-channel property\n");
+			return -ENODEV;
+		}
+
+		cl->rx_callback = xgene_hwmon_pcc_rx_cb;
+		ctx->mbox_chan = pcc_mbox_request_channel(cl, ctx->mbox_idx);
+		if (IS_ERR(ctx->mbox_chan)) {
+			dev_err(&pdev->dev,
+				"PPC channel request failed\n");
+			return -ENODEV;
+		}
+
+		/*
+		 * The PCC mailbox controller driver should
+		 * have parsed the PCCT (global table of all
+		 * PCC channels) and stored pointers to the
+		 * subspace communication region in con_priv.
+		 */
+		cppc_ss = ctx->mbox_chan->con_priv;
+		if (!cppc_ss) {
+			dev_err(&pdev->dev, "PPC subspace not found\n");
+			rc = -ENODEV;
+			goto out_mbox_free;
+		}
+
+		if (!ctx->mbox_chan->mbox->txdone_irq) {
+			dev_err(&pdev->dev, "PCC IRQ not supported\n");
+			rc = -ENODEV;
+			goto out_mbox_free;
+		}
+
+		/*
+		 * This is the shared communication region
+		 * for the OS and Platform to communicate over.
+		 */
+		ctx->comm_base_addr = cppc_ss->base_address;
+		if (ctx->comm_base_addr) {
+			ctx->pcc_comm_addr = memremap(ctx->comm_base_addr,
+							cppc_ss->length,
+							MEMREMAP_WB);
+		} else {
+			dev_err(&pdev->dev, "Failed to get PCC comm region\n");
+			rc = -ENODEV;
+			goto out_mbox_free;
+		}
+
+		if (!ctx->pcc_comm_addr) {
+			dev_err(&pdev->dev,
+				"Failed to ioremap PCC comm region\n");
+			rc = -ENOMEM;
+			goto out_mbox_free;
+		}
+
+		/*
+		 * cppc_ss->latency is just a Nominal value. In reality
+		 * the remote processor could be much slower to reply.
+		 * So add an arbitrary amount of wait on top of Nominal.
+		 */
+		ctx->usecs_lat = PCC_NUM_RETRIES * cppc_ss->latency;
+	}
+
+	ctx->hwmon_dev = hwmon_device_register_with_groups(ctx->dev,
+							   "apm_xgene",
+							   ctx,
+							   xgene_hwmon_groups);
+	if (IS_ERR(ctx->hwmon_dev)) {
+		dev_err(&pdev->dev, "Failed to register HW monitor device\n");
+		rc = PTR_ERR(ctx->hwmon_dev);
+		goto out;
+	}
+
+	/*
+	 * Schedule the bottom handler if there is a pending message.
+	 */
+	schedule_work(&ctx->workq);
+
+	dev_info(&pdev->dev, "APM X-Gene SoC HW monitor driver registered\n");
+
+	return 0;
+
+out:
+	if (acpi_disabled)
+		mbox_free_channel(ctx->mbox_chan);
+	else
+		pcc_mbox_free_channel(ctx->mbox_chan);
+out_mbox_free:
+	kfifo_free(&ctx->async_msg_fifo);
+
+	return rc;
+}
+
+static int xgene_hwmon_remove(struct platform_device *pdev)
+{
+	struct xgene_hwmon_dev *ctx = platform_get_drvdata(pdev);
+
+	hwmon_device_unregister(ctx->hwmon_dev);
+	kfifo_free(&ctx->async_msg_fifo);
+	if (acpi_disabled)
+		mbox_free_channel(ctx->mbox_chan);
+	else
+		pcc_mbox_free_channel(ctx->mbox_chan);
+
+	return 0;
+}
+
+#ifdef CONFIG_ACPI
+static const struct acpi_device_id xgene_hwmon_acpi_match[] = {
+	{"APMC0D29", 0},
+	{},
+};
+MODULE_DEVICE_TABLE(acpi, xgene_hwmon_acpi_match);
+#endif
+
+static const struct of_device_id xgene_hwmon_of_match[] = {
+	{.compatible = "apm,xgene-slimpro-hwmon"},
+	{}
+};
+MODULE_DEVICE_TABLE(of, xgene_hwmon_of_match);
+
+static struct platform_driver xgene_hwmon_driver __refdata = {
+	.probe = xgene_hwmon_probe,
+	.remove = xgene_hwmon_remove,
+	.driver = {
+		.name = "xgene-slimpro-hwmon",
+		.of_match_table = xgene_hwmon_of_match,
+		.acpi_match_table = ACPI_PTR(xgene_hwmon_acpi_match),
+	},
+};
+module_platform_driver(xgene_hwmon_driver);
+
+MODULE_DESCRIPTION("APM X-Gene SoC hardware monitor");
+MODULE_LICENSE("GPL");
diff --git a/drivers/hwtracing/coresight/coresight-etb10.c b/drivers/hwtracing/coresight/coresight-etb10.c
index 4d20b0be0c0b..d7325c6534ad 100644
--- a/drivers/hwtracing/coresight/coresight-etb10.c
+++ b/drivers/hwtracing/coresight/coresight-etb10.c
@@ -184,8 +184,7 @@ static void etb_disable_hw(struct etb_drvdata *drvdata)
 
 	if (coresight_timeout(drvdata->base, ETB_FFCR, ETB_FFCR_BIT, 0)) {
 		dev_err(drvdata->dev,
-			"timeout observed when probing at offset %#x\n",
-			ETB_FFCR);
+		"timeout while waiting for completion of Manual Flush\n");
 	}
 
 	/* disable trace capture */
@@ -193,8 +192,7 @@ static void etb_disable_hw(struct etb_drvdata *drvdata)
 
 	if (coresight_timeout(drvdata->base, ETB_FFSR, ETB_FFSR_BIT, 1)) {
 		dev_err(drvdata->dev,
-			"timeout observed when probing at offset %#x\n",
-			ETB_FFCR);
+			"timeout while waiting for Formatter to Stop\n");
 	}
 
 	CS_LOCK(drvdata->base);
@@ -561,7 +559,7 @@ static const struct file_operations etb_fops = {
 };
 
 #define coresight_etb10_simple_func(name, offset)                       \
-	coresight_simple_func(struct etb_drvdata, name, offset)
+	coresight_simple_func(struct etb_drvdata, NULL, name, offset)
 
 coresight_etb10_simple_func(rdp, ETB_RAM_DEPTH_REG);
 coresight_etb10_simple_func(sts, ETB_STATUS_REG);
@@ -638,7 +636,7 @@ static int etb_probe(struct amba_device *adev, const struct amba_id *id)
 	struct coresight_platform_data *pdata = NULL;
 	struct etb_drvdata *drvdata;
 	struct resource *res = &adev->res;
-	struct coresight_desc *desc;
+	struct coresight_desc desc = { 0 };
 	struct device_node *np = adev->dev.of_node;
 
 	if (np) {
@@ -684,17 +682,13 @@ static int etb_probe(struct amba_device *adev, const struct amba_id *id)
 		return -ENOMEM;
 	}
 
-	desc = devm_kzalloc(dev, sizeof(*desc), GFP_KERNEL);
-	if (!desc)
-		return -ENOMEM;
-
-	desc->type = CORESIGHT_DEV_TYPE_SINK;
-	desc->subtype.sink_subtype = CORESIGHT_DEV_SUBTYPE_SINK_BUFFER;
-	desc->ops = &etb_cs_ops;
-	desc->pdata = pdata;
-	desc->dev = dev;
-	desc->groups = coresight_etb_groups;
-	drvdata->csdev = coresight_register(desc);
+	desc.type = CORESIGHT_DEV_TYPE_SINK;
+	desc.subtype.sink_subtype = CORESIGHT_DEV_SUBTYPE_SINK_BUFFER;
+	desc.ops = &etb_cs_ops;
+	desc.pdata = pdata;
+	desc.dev = dev;
+	desc.groups = coresight_etb_groups;
+	drvdata->csdev = coresight_register(&desc);
 	if (IS_ERR(drvdata->csdev))
 		return PTR_ERR(drvdata->csdev);
 
diff --git a/drivers/hwtracing/coresight/coresight-etm-perf.c b/drivers/hwtracing/coresight/coresight-etm-perf.c
index 755125f7917f..2cd7c718198a 100644
--- a/drivers/hwtracing/coresight/coresight-etm-perf.c
+++ b/drivers/hwtracing/coresight/coresight-etm-perf.c
@@ -27,6 +27,7 @@
 #include <linux/types.h>
 #include <linux/workqueue.h>
 
+#include "coresight-etm-perf.h"
 #include "coresight-priv.h"
 
 static struct pmu etm_pmu;
@@ -71,14 +72,48 @@ static const struct attribute_group *etm_pmu_attr_groups[] = {
 
 static void etm_event_read(struct perf_event *event) {}
 
-static int etm_event_init(struct perf_event *event)
+static int etm_addr_filters_alloc(struct perf_event *event)
 {
-	if (event->attr.type != etm_pmu.type)
-		return -ENOENT;
+	struct etm_filters *filters;
+	int node = event->cpu == -1 ? -1 : cpu_to_node(event->cpu);
+
+	filters = kzalloc_node(sizeof(struct etm_filters), GFP_KERNEL, node);
+	if (!filters)
+		return -ENOMEM;
+
+	if (event->parent)
+		memcpy(filters, event->parent->hw.addr_filters,
+		       sizeof(*filters));
+
+	event->hw.addr_filters = filters;
 
 	return 0;
 }
 
+static void etm_event_destroy(struct perf_event *event)
+{
+	kfree(event->hw.addr_filters);
+	event->hw.addr_filters = NULL;
+}
+
+static int etm_event_init(struct perf_event *event)
+{
+	int ret = 0;
+
+	if (event->attr.type != etm_pmu.type) {
+		ret = -ENOENT;
+		goto out;
+	}
+
+	ret = etm_addr_filters_alloc(event);
+	if (ret)
+		goto out;
+
+	event->destroy = etm_event_destroy;
+out:
+	return ret;
+}
+
 static void free_event_data(struct work_struct *work)
 {
 	int cpu;
@@ -100,7 +135,7 @@ static void free_event_data(struct work_struct *work)
 	}
 
 	for_each_cpu(cpu, mask) {
-		if (event_data->path[cpu])
+		if (!(IS_ERR_OR_NULL(event_data->path[cpu])))
 			coresight_release_path(event_data->path[cpu]);
 	}
 
@@ -185,7 +220,7 @@ static void *etm_setup_aux(int event_cpu, void **pages,
 		 * referenced later when the path is actually needed.
 		 */
 		event_data->path[cpu] = coresight_build_path(csdev);
-		if (!event_data->path[cpu])
+		if (IS_ERR(event_data->path[cpu]))
 			goto err;
 	}
 
@@ -258,7 +293,7 @@ static void etm_event_start(struct perf_event *event, int flags)
 	event->hw.state = 0;
 
 	/* Finally enable the tracer */
-	if (source_ops(csdev)->enable(csdev, &event->attr, CS_MODE_PERF))
+	if (source_ops(csdev)->enable(csdev, event, CS_MODE_PERF))
 		goto fail_end_stop;
 
 out:
@@ -291,7 +326,7 @@ static void etm_event_stop(struct perf_event *event, int mode)
 		return;
 
 	/* stop tracer */
-	source_ops(csdev)->disable(csdev);
+	source_ops(csdev)->disable(csdev, event);
 
 	/* tell the core */
 	event->hw.state = PERF_HES_STOPPED;
@@ -342,6 +377,87 @@ static void etm_event_del(struct perf_event *event, int mode)
 	etm_event_stop(event, PERF_EF_UPDATE);
 }
 
+static int etm_addr_filters_validate(struct list_head *filters)
+{
+	bool range = false, address = false;
+	int index = 0;
+	struct perf_addr_filter *filter;
+
+	list_for_each_entry(filter, filters, entry) {
+		/*
+		 * No need to go further if there's no more
+		 * room for filters.
+		 */
+		if (++index > ETM_ADDR_CMP_MAX)
+			return -EOPNOTSUPP;
+
+		/*
+		 * As taken from the struct perf_addr_filter documentation:
+		 *	@range:	1: range, 0: address
+		 *
+		 * At this time we don't allow range and start/stop filtering
+		 * to cohabitate, they have to be mutually exclusive.
+		 */
+		if ((filter->range == 1) && address)
+			return -EOPNOTSUPP;
+
+		if ((filter->range == 0) && range)
+			return -EOPNOTSUPP;
+
+		/*
+		 * For range filtering, the second address in the address
+		 * range comparator needs to be higher than the first.
+		 * Invalid otherwise.
+		 */
+		if (filter->range && filter->size == 0)
+			return -EINVAL;
+
+		/*
+		 * Everything checks out with this filter, record what we've
+		 * received before moving on to the next one.
+		 */
+		if (filter->range)
+			range = true;
+		else
+			address = true;
+	}
+
+	return 0;
+}
+
+static void etm_addr_filters_sync(struct perf_event *event)
+{
+	struct perf_addr_filters_head *head = perf_event_addr_filters(event);
+	unsigned long start, stop, *offs = event->addr_filters_offs;
+	struct etm_filters *filters = event->hw.addr_filters;
+	struct etm_filter *etm_filter;
+	struct perf_addr_filter *filter;
+	int i = 0;
+
+	list_for_each_entry(filter, &head->list, entry) {
+		start = filter->offset + offs[i];
+		stop = start + filter->size;
+		etm_filter = &filters->etm_filter[i];
+
+		if (filter->range == 1) {
+			etm_filter->start_addr = start;
+			etm_filter->stop_addr = stop;
+			etm_filter->type = ETM_ADDR_TYPE_RANGE;
+		} else {
+			if (filter->filter == 1) {
+				etm_filter->start_addr = start;
+				etm_filter->type = ETM_ADDR_TYPE_START;
+			} else {
+				etm_filter->stop_addr = stop;
+				etm_filter->type = ETM_ADDR_TYPE_STOP;
+			}
+		}
+		i++;
+	}
+
+	filters->nr_filters = i;
+}
+
 int etm_perf_symlink(struct coresight_device *csdev, bool link)
 {
 	char entry[sizeof("cpu9999999")];
@@ -371,18 +487,21 @@ static int __init etm_perf_init(void)
 {
 	int ret;
 
-	etm_pmu.capabilities	= PERF_PMU_CAP_EXCLUSIVE;
-
-	etm_pmu.attr_groups	= etm_pmu_attr_groups;
-	etm_pmu.task_ctx_nr	= perf_sw_context;
-	etm_pmu.read		= etm_event_read;
-	etm_pmu.event_init	= etm_event_init;
-	etm_pmu.setup_aux	= etm_setup_aux;
-	etm_pmu.free_aux	= etm_free_aux;
-	etm_pmu.start		= etm_event_start;
-	etm_pmu.stop		= etm_event_stop;
-	etm_pmu.add		= etm_event_add;
-	etm_pmu.del		= etm_event_del;
+	etm_pmu.capabilities		= PERF_PMU_CAP_EXCLUSIVE;
+
+	etm_pmu.attr_groups		= etm_pmu_attr_groups;
+	etm_pmu.task_ctx_nr		= perf_sw_context;
+	etm_pmu.read			= etm_event_read;
+	etm_pmu.event_init		= etm_event_init;
+	etm_pmu.setup_aux		= etm_setup_aux;
+	etm_pmu.free_aux		= etm_free_aux;
+	etm_pmu.start			= etm_event_start;
+	etm_pmu.stop			= etm_event_stop;
+	etm_pmu.add			= etm_event_add;
+	etm_pmu.del			= etm_event_del;
+	etm_pmu.addr_filters_sync	= etm_addr_filters_sync;
+	etm_pmu.addr_filters_validate	= etm_addr_filters_validate;
+	etm_pmu.nr_addr_filters		= ETM_ADDR_CMP_MAX;
 
 	ret = perf_pmu_register(&etm_pmu, CORESIGHT_ETM_PMU_NAME, -1);
 	if (ret == 0)
diff --git a/drivers/hwtracing/coresight/coresight-etm-perf.h b/drivers/hwtracing/coresight/coresight-etm-perf.h
index 87f5a134eb6f..3ffc9feb2d64 100644
--- a/drivers/hwtracing/coresight/coresight-etm-perf.h
+++ b/drivers/hwtracing/coresight/coresight-etm-perf.h
@@ -18,8 +18,42 @@
 #ifndef _CORESIGHT_ETM_PERF_H
 #define _CORESIGHT_ETM_PERF_H
 
+#include "coresight-priv.h"
+
 struct coresight_device;
 
+/*
+ * In both ETMv3 and v4 the maximum number of address comparator implentable
+ * is 8.  The actual number is implementation specific and will be checked
+ * when filters are applied.
+ */
+#define ETM_ADDR_CMP_MAX	8
+
+/**
+ * struct etm_filter - single instruction range or start/stop configuration.
+ * @start_addr:	The address to start tracing on.
+ * @stop_addr:	The address to stop tracing on.
+ * @type:	Is this a range or start/stop filter.
+ */
+struct etm_filter {
+	unsigned long start_addr;
+	unsigned long stop_addr;
+	enum etm_addr_type type;
+};
+
+/**
+ * struct etm_filters - set of filters for a session
+ * @etm_filter:	All the filters for this session.
+ * @nr_filters:	Number of filters
+ * @ssstatus:	Status of the start/stop logic.
+ */
+struct etm_filters {
+	struct etm_filter	etm_filter[ETM_ADDR_CMP_MAX];
+	unsigned int		nr_filters;
+	bool			ssstatus;
+};
+
+
 #ifdef CONFIG_CORESIGHT
 int etm_perf_symlink(struct coresight_device *csdev, bool link);
 
diff --git a/drivers/hwtracing/coresight/coresight-etm.h b/drivers/hwtracing/coresight/coresight-etm.h
index 51597cb2c08a..4a18ee499965 100644
--- a/drivers/hwtracing/coresight/coresight-etm.h
+++ b/drivers/hwtracing/coresight/coresight-etm.h
@@ -259,14 +259,6 @@ struct etm_drvdata {
 	struct etm_config		config;
 };
 
-enum etm_addr_type {
-	ETM_ADDR_TYPE_NONE,
-	ETM_ADDR_TYPE_SINGLE,
-	ETM_ADDR_TYPE_RANGE,
-	ETM_ADDR_TYPE_START,
-	ETM_ADDR_TYPE_STOP,
-};
-
 static inline void etm_writel(struct etm_drvdata *drvdata,
 			      u32 val, u32 off)
 {
diff --git a/drivers/hwtracing/coresight/coresight-etm3x-sysfs.c b/drivers/hwtracing/coresight/coresight-etm3x-sysfs.c
index 02d4b629891f..e9b071953f80 100644
--- a/drivers/hwtracing/coresight/coresight-etm3x-sysfs.c
+++ b/drivers/hwtracing/coresight/coresight-etm3x-sysfs.c
@@ -18,6 +18,7 @@
 #include <linux/pm_runtime.h>
 #include <linux/sysfs.h>
 #include "coresight-etm.h"
+#include "coresight-priv.h"
 
 static ssize_t nr_addr_cmp_show(struct device *dev,
 				struct device_attribute *attr, char *buf)
@@ -1222,7 +1223,7 @@ static struct attribute *coresight_etm_attrs[] = {
 };
 
 #define coresight_etm3x_simple_func(name, offset)			\
-	coresight_simple_func(struct etm_drvdata, name, offset)
+	coresight_simple_func(struct etm_drvdata, NULL, name, offset)
 
 coresight_etm3x_simple_func(etmccr, ETMCCR);
 coresight_etm3x_simple_func(etmccer, ETMCCER);
diff --git a/drivers/hwtracing/coresight/coresight-etm3x.c b/drivers/hwtracing/coresight/coresight-etm3x.c
index 2de4cad9c5ed..3fe368b23d15 100644
--- a/drivers/hwtracing/coresight/coresight-etm3x.c
+++ b/drivers/hwtracing/coresight/coresight-etm3x.c
@@ -311,9 +311,10 @@ void etm_config_trace_mode(struct etm_config *config)
 #define ETM3X_SUPPORTED_OPTIONS (ETMCR_CYC_ACC | ETMCR_TIMESTAMP_EN)
 
 static int etm_parse_event_config(struct etm_drvdata *drvdata,
-				  struct perf_event_attr *attr)
+				  struct perf_event *event)
 {
 	struct etm_config *config = &drvdata->config;
+	struct perf_event_attr *attr = &event->attr;
 
 	if (!attr)
 		return -EINVAL;
@@ -459,7 +460,7 @@ static int etm_trace_id(struct coresight_device *csdev)
 }
 
 static int etm_enable_perf(struct coresight_device *csdev,
-			   struct perf_event_attr *attr)
+			   struct perf_event *event)
 {
 	struct etm_drvdata *drvdata = dev_get_drvdata(csdev->dev.parent);
 
@@ -467,7 +468,7 @@ static int etm_enable_perf(struct coresight_device *csdev,
 		return -EINVAL;
 
 	/* Configure the tracer based on the session's specifics */
-	etm_parse_event_config(drvdata, attr);
+	etm_parse_event_config(drvdata, event);
 	/* And enable it */
 	etm_enable_hw(drvdata);
 
@@ -504,7 +505,7 @@ err:
 }
 
 static int etm_enable(struct coresight_device *csdev,
-		      struct perf_event_attr *attr, u32 mode)
+		      struct perf_event *event, u32 mode)
 {
 	int ret;
 	u32 val;
@@ -521,7 +522,7 @@ static int etm_enable(struct coresight_device *csdev,
 		ret = etm_enable_sysfs(csdev);
 		break;
 	case CS_MODE_PERF:
-		ret = etm_enable_perf(csdev, attr);
+		ret = etm_enable_perf(csdev, event);
 		break;
 	default:
 		ret = -EINVAL;
@@ -601,7 +602,8 @@ static void etm_disable_sysfs(struct coresight_device *csdev)
 	dev_info(drvdata->dev, "ETM tracing disabled\n");
 }
 
-static void etm_disable(struct coresight_device *csdev)
+static void etm_disable(struct coresight_device *csdev,
+			struct perf_event *event)
 {
 	u32 mode;
 	struct etm_drvdata *drvdata = dev_get_drvdata(csdev->dev.parent);
@@ -756,13 +758,9 @@ static int etm_probe(struct amba_device *adev, const struct amba_id *id)
 	struct coresight_platform_data *pdata = NULL;
 	struct etm_drvdata *drvdata;
 	struct resource *res = &adev->res;
-	struct coresight_desc *desc;
+	struct coresight_desc desc = { 0 };
 	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;
@@ -825,13 +823,13 @@ static int etm_probe(struct amba_device *adev, const struct amba_id *id)
 	etm_init_trace_id(drvdata);
 	etm_set_default(&drvdata->config);
 
-	desc->type = CORESIGHT_DEV_TYPE_SOURCE;
-	desc->subtype.source_subtype = CORESIGHT_DEV_SUBTYPE_SOURCE_PROC;
-	desc->ops = &etm_cs_ops;
-	desc->pdata = pdata;
-	desc->dev = dev;
-	desc->groups = coresight_etm_groups;
-	drvdata->csdev = coresight_register(desc);
+	desc.type = CORESIGHT_DEV_TYPE_SOURCE;
+	desc.subtype.source_subtype = CORESIGHT_DEV_SUBTYPE_SOURCE_PROC;
+	desc.ops = &etm_cs_ops;
+	desc.pdata = pdata;
+	desc.dev = dev;
+	desc.groups = coresight_etm_groups;
+	drvdata->csdev = coresight_register(&desc);
 	if (IS_ERR(drvdata->csdev)) {
 		ret = PTR_ERR(drvdata->csdev);
 		goto err_arch_supported;
@@ -893,6 +891,11 @@ static struct amba_id etm_ids[] = {
 		.mask	= 0x0003ffff,
 		.data	= "ETM 3.3",
 	},
+	{	/* ETM 3.5 - Cortex-A5 */
+		.id	= 0x0003b955,
+		.mask	= 0x0003ffff,
+		.data	= "ETM 3.5",
+	},
 	{	/* ETM 3.5 */
 		.id	= 0x0003b956,
 		.mask	= 0x0003ffff,
diff --git a/drivers/hwtracing/coresight/coresight-etm4x-sysfs.c b/drivers/hwtracing/coresight/coresight-etm4x-sysfs.c
index 7c84308c5564..b9b1e9c8f4c4 100644
--- a/drivers/hwtracing/coresight/coresight-etm4x-sysfs.c
+++ b/drivers/hwtracing/coresight/coresight-etm4x-sysfs.c
@@ -18,6 +18,7 @@
 #include <linux/pm_runtime.h>
 #include <linux/sysfs.h>
 #include "coresight-etm4x.h"
+#include "coresight-priv.h"
 
 static int etm4_set_mode_exclude(struct etmv4_drvdata *drvdata, bool exclude)
 {
@@ -2039,15 +2040,42 @@ static struct attribute *coresight_etmv4_attrs[] = {
 	NULL,
 };
 
+struct etmv4_reg {
+	void __iomem *addr;
+	u32 data;
+};
+
+static void do_smp_cross_read(void *data)
+{
+	struct etmv4_reg *reg = data;
+
+	reg->data = readl_relaxed(reg->addr);
+}
+
+static u32 etmv4_cross_read(const struct device *dev, u32 offset)
+{
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev);
+	struct etmv4_reg reg;
+
+	reg.addr = drvdata->base + offset;
+	/*
+	 * smp cross call ensures the CPU will be powered up before
+	 * accessing the ETMv4 trace core registers
+	 */
+	smp_call_function_single(drvdata->cpu, do_smp_cross_read, &reg, 1);
+	return reg.data;
+}
+
 #define coresight_etm4x_simple_func(name, offset)			\
-	coresight_simple_func(struct etmv4_drvdata, name, offset)
+	coresight_simple_func(struct etmv4_drvdata, NULL, name, offset)
+
+#define coresight_etm4x_cross_read(name, offset)			\
+	coresight_simple_func(struct etmv4_drvdata, etmv4_cross_read,	\
+			      name, offset)
 
-coresight_etm4x_simple_func(trcoslsr, TRCOSLSR);
 coresight_etm4x_simple_func(trcpdcr, TRCPDCR);
 coresight_etm4x_simple_func(trcpdsr, TRCPDSR);
 coresight_etm4x_simple_func(trclsr, TRCLSR);
-coresight_etm4x_simple_func(trcconfig, TRCCONFIGR);
-coresight_etm4x_simple_func(trctraceid, TRCTRACEIDR);
 coresight_etm4x_simple_func(trcauthstatus, TRCAUTHSTATUS);
 coresight_etm4x_simple_func(trcdevid, TRCDEVID);
 coresight_etm4x_simple_func(trcdevtype, TRCDEVTYPE);
@@ -2055,6 +2083,9 @@ coresight_etm4x_simple_func(trcpidr0, TRCPIDR0);
 coresight_etm4x_simple_func(trcpidr1, TRCPIDR1);
 coresight_etm4x_simple_func(trcpidr2, TRCPIDR2);
 coresight_etm4x_simple_func(trcpidr3, TRCPIDR3);
+coresight_etm4x_cross_read(trcoslsr, TRCOSLSR);
+coresight_etm4x_cross_read(trcconfig, TRCCONFIGR);
+coresight_etm4x_cross_read(trctraceid, TRCTRACEIDR);
 
 static struct attribute *coresight_etmv4_mgmt_attrs[] = {
 	&dev_attr_trcoslsr.attr,
@@ -2073,19 +2104,19 @@ static struct attribute *coresight_etmv4_mgmt_attrs[] = {
 	NULL,
 };
 
-coresight_etm4x_simple_func(trcidr0, TRCIDR0);
-coresight_etm4x_simple_func(trcidr1, TRCIDR1);
-coresight_etm4x_simple_func(trcidr2, TRCIDR2);
-coresight_etm4x_simple_func(trcidr3, TRCIDR3);
-coresight_etm4x_simple_func(trcidr4, TRCIDR4);
-coresight_etm4x_simple_func(trcidr5, TRCIDR5);
+coresight_etm4x_cross_read(trcidr0, TRCIDR0);
+coresight_etm4x_cross_read(trcidr1, TRCIDR1);
+coresight_etm4x_cross_read(trcidr2, TRCIDR2);
+coresight_etm4x_cross_read(trcidr3, TRCIDR3);
+coresight_etm4x_cross_read(trcidr4, TRCIDR4);
+coresight_etm4x_cross_read(trcidr5, TRCIDR5);
 /* trcidr[6,7] are reserved */
-coresight_etm4x_simple_func(trcidr8, TRCIDR8);
-coresight_etm4x_simple_func(trcidr9, TRCIDR9);
-coresight_etm4x_simple_func(trcidr10, TRCIDR10);
-coresight_etm4x_simple_func(trcidr11, TRCIDR11);
-coresight_etm4x_simple_func(trcidr12, TRCIDR12);
-coresight_etm4x_simple_func(trcidr13, TRCIDR13);
+coresight_etm4x_cross_read(trcidr8, TRCIDR8);
+coresight_etm4x_cross_read(trcidr9, TRCIDR9);
+coresight_etm4x_cross_read(trcidr10, TRCIDR10);
+coresight_etm4x_cross_read(trcidr11, TRCIDR11);
+coresight_etm4x_cross_read(trcidr12, TRCIDR12);
+coresight_etm4x_cross_read(trcidr13, TRCIDR13);
 
 static struct attribute *coresight_etmv4_trcidr_attrs[] = {
 	&dev_attr_trcidr0.attr,
diff --git a/drivers/hwtracing/coresight/coresight-etm4x.c b/drivers/hwtracing/coresight/coresight-etm4x.c
index 1a5e0d14c1dd..4db8d6a4d0cb 100644
--- a/drivers/hwtracing/coresight/coresight-etm4x.c
+++ b/drivers/hwtracing/coresight/coresight-etm4x.c
@@ -33,7 +33,6 @@
 #include <linux/uaccess.h>
 #include <linux/perf_event.h>
 #include <linux/pm_runtime.h>
-#include <linux/perf_event.h>
 #include <asm/sections.h>
 #include <asm/local.h>
 
@@ -46,7 +45,9 @@ 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_set_default(struct etmv4_config *config);
+static void etm4_set_default_config(struct etmv4_config *config);
+static int etm4_set_event_filters(struct etmv4_drvdata *drvdata,
+				  struct perf_event *event);
 
 static enum cpuhp_state hp_online;
 
@@ -79,22 +80,8 @@ static int etm4_cpu_id(struct coresight_device *csdev)
 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 (!local_read(&drvdata->mode))
-		return drvdata->trcid;
-
-	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);
-
-	return trace_id;
+	return drvdata->trcid;
 }
 
 static void etm4_enable_hw(void *info)
@@ -113,8 +100,7 @@ static void etm4_enable_hw(void *info)
 	/* 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);
+			"timeout while waiting for Idle Trace Status\n");
 
 	writel_relaxed(config->pe_sel, drvdata->base + TRCPROCSELR);
 	writel_relaxed(config->cfg, drvdata->base + TRCCONFIGR);
@@ -180,14 +166,20 @@ static void etm4_enable_hw(void *info)
 	writel_relaxed(config->vmid_mask0, drvdata->base + TRCVMIDCCTLR0);
 	writel_relaxed(config->vmid_mask1, drvdata->base + TRCVMIDCCTLR1);
 
+	/*
+	 * Request to keep the trace unit powered and also
+	 * emulation of powerdown
+	 */
+	writel_relaxed(readl_relaxed(drvdata->base + TRCPDCR) | TRCPDCR_PU,
+		       drvdata->base + TRCPDCR);
+
 	/* 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);
+			"timeout while waiting for Idle Trace Status\n");
 
 	CS_LOCK(drvdata->base);
 
@@ -195,12 +187,16 @@ static void etm4_enable_hw(void *info)
 }
 
 static int etm4_parse_event_config(struct etmv4_drvdata *drvdata,
-				   struct perf_event_attr *attr)
+				   struct perf_event *event)
 {
+	int ret = 0;
 	struct etmv4_config *config = &drvdata->config;
+	struct perf_event_attr *attr = &event->attr;
 
-	if (!attr)
-		return -EINVAL;
+	if (!attr) {
+		ret = -EINVAL;
+		goto out;
+	}
 
 	/* Clear configuration from previous run */
 	memset(config, 0, sizeof(struct etmv4_config));
@@ -212,14 +208,12 @@ static int etm4_parse_event_config(struct etmv4_drvdata *drvdata,
 		config->mode = ETM_MODE_EXCL_USER;
 
 	/* Always start from the default config */
-	etm4_set_default(config);
+	etm4_set_default_config(config);
 
-	/*
-	 * By default the tracers are configured to trace the whole address
-	 * range.  Narrow the field only if requested by user space.
-	 */
-	if (config->mode)
-		etm4_config_trace_mode(config);
+	/* Configure filters specified on the perf cmd line, if any. */
+	ret = etm4_set_event_filters(drvdata, event);
+	if (ret)
+		goto out;
 
 	/* Go from generic option to ETMv4 specifics */
 	if (attr->config & BIT(ETM_OPT_CYCACC))
@@ -227,23 +221,30 @@ static int etm4_parse_event_config(struct etmv4_drvdata *drvdata,
 	if (attr->config & BIT(ETM_OPT_TS))
 		config->cfg |= ETMv4_MODE_TIMESTAMP;
 
-	return 0;
+out:
+	return ret;
 }
 
 static int etm4_enable_perf(struct coresight_device *csdev,
-			    struct perf_event_attr *attr)
+			    struct perf_event *event)
 {
+	int ret = 0;
 	struct etmv4_drvdata *drvdata = dev_get_drvdata(csdev->dev.parent);
 
-	if (WARN_ON_ONCE(drvdata->cpu != smp_processor_id()))
-		return -EINVAL;
+	if (WARN_ON_ONCE(drvdata->cpu != smp_processor_id())) {
+		ret = -EINVAL;
+		goto out;
+	}
 
 	/* Configure the tracer based on the session's specifics */
-	etm4_parse_event_config(drvdata, attr);
+	ret = etm4_parse_event_config(drvdata, event);
+	if (ret)
+		goto out;
 	/* And enable it */
 	etm4_enable_hw(drvdata);
 
-	return 0;
+out:
+	return ret;
 }
 
 static int etm4_enable_sysfs(struct coresight_device *csdev)
@@ -274,7 +275,7 @@ err:
 }
 
 static int etm4_enable(struct coresight_device *csdev,
-		       struct perf_event_attr *attr, u32 mode)
+		       struct perf_event *event, u32 mode)
 {
 	int ret;
 	u32 val;
@@ -291,7 +292,7 @@ static int etm4_enable(struct coresight_device *csdev,
 		ret = etm4_enable_sysfs(csdev);
 		break;
 	case CS_MODE_PERF:
-		ret = etm4_enable_perf(csdev, attr);
+		ret = etm4_enable_perf(csdev, event);
 		break;
 	default:
 		ret = -EINVAL;
@@ -311,6 +312,11 @@ static void etm4_disable_hw(void *info)
 
 	CS_UNLOCK(drvdata->base);
 
+	/* power can be removed from the trace unit now */
+	control = readl_relaxed(drvdata->base + TRCPDCR);
+	control &= ~TRCPDCR_PU;
+	writel_relaxed(control, drvdata->base + TRCPDCR);
+
 	control = readl_relaxed(drvdata->base + TRCPRGCTLR);
 
 	/* EN, bit[0] Trace unit enable bit */
@@ -326,14 +332,28 @@ static void etm4_disable_hw(void *info)
 	dev_dbg(drvdata->dev, "cpu: %d disable smp call done\n", drvdata->cpu);
 }
 
-static int etm4_disable_perf(struct coresight_device *csdev)
+static int etm4_disable_perf(struct coresight_device *csdev,
+			     struct perf_event *event)
 {
+	u32 control;
+	struct etm_filters *filters = event->hw.addr_filters;
 	struct etmv4_drvdata *drvdata = dev_get_drvdata(csdev->dev.parent);
 
 	if (WARN_ON_ONCE(drvdata->cpu != smp_processor_id()))
 		return -EINVAL;
 
 	etm4_disable_hw(drvdata);
+
+	/*
+	 * Check if the start/stop logic was active when the unit was stopped.
+	 * That way we can re-enable the start/stop logic when the process is
+	 * scheduled again.  Configuration of the start/stop logic happens in
+	 * function etm4_set_event_filters().
+	 */
+	control = readl_relaxed(drvdata->base + TRCVICTLR);
+	/* TRCVICTLR::SSSTATUS, bit[9] */
+	filters->ssstatus = (control & BIT(9));
+
 	return 0;
 }
 
@@ -362,7 +382,8 @@ static void etm4_disable_sysfs(struct coresight_device *csdev)
 	dev_info(drvdata->dev, "ETM tracing disabled\n");
 }
 
-static void etm4_disable(struct coresight_device *csdev)
+static void etm4_disable(struct coresight_device *csdev,
+			 struct perf_event *event)
 {
 	u32 mode;
 	struct etmv4_drvdata *drvdata = dev_get_drvdata(csdev->dev.parent);
@@ -381,7 +402,7 @@ static void etm4_disable(struct coresight_device *csdev)
 		etm4_disable_sysfs(csdev);
 		break;
 	case CS_MODE_PERF:
-		etm4_disable_perf(csdev);
+		etm4_disable_perf(csdev, event);
 		break;
 	}
 
@@ -564,21 +585,8 @@ static void etm4_init_arch_data(void *info)
 	CS_LOCK(drvdata->base);
 }
 
-static void etm4_set_default(struct etmv4_config *config)
+static void etm4_set_default_config(struct etmv4_config *config)
 {
-	if (WARN_ON_ONCE(!config))
-		return;
-
-	/*
-	 * Make default initialisation trace everything
-	 *
-	 * Select the "always true" resource selector on the
-	 * "Enablign Event" line and configure address range comparator
-	 * '0' to trace all the possible address range.  From there
-	 * configure the "include/exclude" engine to include address
-	 * range comparator '0'.
-	 */
-
 	/* disable all events tracing */
 	config->eventctrl0 = 0x0;
 	config->eventctrl1 = 0x0;
@@ -594,6 +602,108 @@ static void etm4_set_default(struct etmv4_config *config)
 
 	/* TRCVICTLR::EVENT = 0x01, select the always on logic */
 	config->vinst_ctrl |= BIT(0);
+}
+
+static u64 etm4_get_access_type(struct etmv4_config *config)
+{
+	u64 access_type = 0;
+
+	/*
+	 * EXLEVEL_NS, bits[15:12]
+	 * The Exception levels are:
+	 *   Bit[12] Exception level 0 - Application
+	 *   Bit[13] Exception level 1 - OS
+	 *   Bit[14] Exception level 2 - Hypervisor
+	 *   Bit[15] Never implemented
+	 *
+	 * Always stay away from hypervisor mode.
+	 */
+	access_type = ETM_EXLEVEL_NS_HYP;
+
+	if (config->mode & ETM_MODE_EXCL_KERN)
+		access_type |= ETM_EXLEVEL_NS_OS;
+
+	if (config->mode & ETM_MODE_EXCL_USER)
+		access_type |= ETM_EXLEVEL_NS_APP;
+
+	/*
+	 * EXLEVEL_S, bits[11:8], don't trace anything happening
+	 * in secure state.
+	 */
+	access_type |= (ETM_EXLEVEL_S_APP	|
+			ETM_EXLEVEL_S_OS	|
+			ETM_EXLEVEL_S_HYP);
+
+	return access_type;
+}
+
+static void etm4_set_comparator_filter(struct etmv4_config *config,
+				       u64 start, u64 stop, int comparator)
+{
+	u64 access_type = etm4_get_access_type(config);
+
+	/* First half of default address comparator */
+	config->addr_val[comparator] = start;
+	config->addr_acc[comparator] = access_type;
+	config->addr_type[comparator] = ETM_ADDR_TYPE_RANGE;
+
+	/* Second half of default address comparator */
+	config->addr_val[comparator + 1] = stop;
+	config->addr_acc[comparator + 1] = access_type;
+	config->addr_type[comparator + 1] = ETM_ADDR_TYPE_RANGE;
+
+	/*
+	 * Configure the ViewInst function to include this address range
+	 * comparator.
+	 *
+	 * @comparator is divided by two since it is the index in the
+	 * etmv4_config::addr_val array but register TRCVIIECTLR deals with
+	 * address range comparator _pairs_.
+	 *
+	 * Therefore:
+	 *	index 0 -> compatator pair 0
+	 *	index 2 -> comparator pair 1
+	 *	index 4 -> comparator pair 2
+	 *	...
+	 *	index 14 -> comparator pair 7
+	 */
+	config->viiectlr |= BIT(comparator / 2);
+}
+
+static void etm4_set_start_stop_filter(struct etmv4_config *config,
+				       u64 address, int comparator,
+				       enum etm_addr_type type)
+{
+	int shift;
+	u64 access_type = etm4_get_access_type(config);
+
+	/* Configure the comparator */
+	config->addr_val[comparator] = address;
+	config->addr_acc[comparator] = access_type;
+	config->addr_type[comparator] = type;
+
+	/*
+	 * Configure ViewInst Start-Stop control register.
+	 * Addresses configured to start tracing go from bit 0 to n-1,
+	 * while those configured to stop tracing from 16 to 16 + n-1.
+	 */
+	shift = (type == ETM_ADDR_TYPE_START ? 0 : 16);
+	config->vissctlr |= BIT(shift + comparator);
+}
+
+static void etm4_set_default_filter(struct etmv4_config *config)
+{
+	u64 start, stop;
+
+	/*
+	 * Configure address range comparator '0' to encompass all
+	 * possible addresses.
+	 */
+	start = 0x0;
+	stop = ~0x0;
+
+	etm4_set_comparator_filter(config, start, stop,
+				   ETM_DEFAULT_ADDR_COMP);
 
 	/*
 	 * TRCVICTLR::SSSTATUS == 1, the start-stop logic is
@@ -601,43 +711,156 @@ static void etm4_set_default(struct etmv4_config *config)
 	 */
 	config->vinst_ctrl |= BIT(9);
 
+	/* No start-stop filtering for ViewInst */
+	config->vissctlr = 0x0;
+}
+
+static void etm4_set_default(struct etmv4_config *config)
+{
+	if (WARN_ON_ONCE(!config))
+		return;
+
 	/*
-	 * Configure address range comparator '0' to encompass all
-	 * possible addresses.
+	 * Make default initialisation trace everything
+	 *
+	 * Select the "always true" resource selector on the
+	 * "Enablign Event" line and configure address range comparator
+	 * '0' to trace all the possible address range.  From there
+	 * configure the "include/exclude" engine to include address
+	 * range comparator '0'.
 	 */
+	etm4_set_default_config(config);
+	etm4_set_default_filter(config);
+}
 
-	/* First half of default address comparator: start at address 0 */
-	config->addr_val[ETM_DEFAULT_ADDR_COMP] = 0x0;
-	/* trace instruction addresses */
-	config->addr_acc[ETM_DEFAULT_ADDR_COMP] &= ~(BIT(0) | BIT(1));
-	/* EXLEVEL_NS, bits[12:15], only trace application and kernel space */
-	config->addr_acc[ETM_DEFAULT_ADDR_COMP] |= ETM_EXLEVEL_NS_HYP;
-	/* EXLEVEL_S, bits[11:8], don't trace anything in secure state */
-	config->addr_acc[ETM_DEFAULT_ADDR_COMP] |= (ETM_EXLEVEL_S_APP |
-						    ETM_EXLEVEL_S_OS |
-						    ETM_EXLEVEL_S_HYP);
-	config->addr_type[ETM_DEFAULT_ADDR_COMP] = ETM_ADDR_TYPE_RANGE;
+static int etm4_get_next_comparator(struct etmv4_drvdata *drvdata, u32 type)
+{
+	int nr_comparator, index = 0;
+	struct etmv4_config *config = &drvdata->config;
 
 	/*
-	 * Second half of default address comparator: go all
-	 * the way to the top.
-	*/
-	config->addr_val[ETM_DEFAULT_ADDR_COMP + 1] = ~0x0;
-	/* trace instruction addresses */
-	config->addr_acc[ETM_DEFAULT_ADDR_COMP + 1] &= ~(BIT(0) | BIT(1));
-	/* Address comparator type must be equal for both halves */
-	config->addr_acc[ETM_DEFAULT_ADDR_COMP + 1] =
-					config->addr_acc[ETM_DEFAULT_ADDR_COMP];
-	config->addr_type[ETM_DEFAULT_ADDR_COMP + 1] = ETM_ADDR_TYPE_RANGE;
+	 * nr_addr_cmp holds the number of comparator _pair_, so time 2
+	 * for the total number of comparators.
+	 */
+	nr_comparator = drvdata->nr_addr_cmp * 2;
+
+	/* Go through the tally of comparators looking for a free one. */
+	while (index < nr_comparator) {
+		switch (type) {
+		case ETM_ADDR_TYPE_RANGE:
+			if (config->addr_type[index] == ETM_ADDR_TYPE_NONE &&
+			    config->addr_type[index + 1] == ETM_ADDR_TYPE_NONE)
+				return index;
+
+			/* Address range comparators go in pairs */
+			index += 2;
+			break;
+		case ETM_ADDR_TYPE_START:
+		case ETM_ADDR_TYPE_STOP:
+			if (config->addr_type[index] == ETM_ADDR_TYPE_NONE)
+				return index;
+
+			/* Start/stop address can have odd indexes */
+			index += 1;
+			break;
+		default:
+			return -EINVAL;
+		}
+	}
+
+	/* If we are here all the comparators have been used. */
+	return -ENOSPC;
+}
+
+static int etm4_set_event_filters(struct etmv4_drvdata *drvdata,
+				  struct perf_event *event)
+{
+	int i, comparator, ret = 0;
+	u64 address;
+	struct etmv4_config *config = &drvdata->config;
+	struct etm_filters *filters = event->hw.addr_filters;
+
+	if (!filters)
+		goto default_filter;
+
+	/* Sync events with what Perf got */
+	perf_event_addr_filters_sync(event);
 
 	/*
-	 * Configure the ViewInst function to filter on address range
-	 * comparator '0'.
+	 * If there are no filters to deal with simply go ahead with
+	 * the default filter, i.e the entire address range.
 	 */
-	config->viiectlr = BIT(0);
+	if (!filters->nr_filters)
+		goto default_filter;
+
+	for (i = 0; i < filters->nr_filters; i++) {
+		struct etm_filter *filter = &filters->etm_filter[i];
+		enum etm_addr_type type = filter->type;
+
+		/* See if a comparator is free. */
+		comparator = etm4_get_next_comparator(drvdata, type);
+		if (comparator < 0) {
+			ret = comparator;
+			goto out;
+		}
+
+		switch (type) {
+		case ETM_ADDR_TYPE_RANGE:
+			etm4_set_comparator_filter(config,
+						   filter->start_addr,
+						   filter->stop_addr,
+						   comparator);
+			/*
+			 * TRCVICTLR::SSSTATUS == 1, the start-stop logic is
+			 * in the started state
+			 */
+			config->vinst_ctrl |= BIT(9);
+
+			/* No start-stop filtering for ViewInst */
+			config->vissctlr = 0x0;
+			break;
+		case ETM_ADDR_TYPE_START:
+		case ETM_ADDR_TYPE_STOP:
+			/* Get the right start or stop address */
+			address = (type == ETM_ADDR_TYPE_START ?
+				   filter->start_addr :
+				   filter->stop_addr);
+
+			/* Configure comparator */
+			etm4_set_start_stop_filter(config, address,
+						   comparator, type);
+
+			/*
+			 * If filters::ssstatus == 1, trace acquisition was
+			 * started but the process was yanked away before the
+			 * the stop address was hit.  As such the start/stop
+			 * logic needs to be re-started so that tracing can
+			 * resume where it left.
+			 *
+			 * The start/stop logic status when a process is
+			 * scheduled out is checked in function
+			 * etm4_disable_perf().
+			 */
+			if (filters->ssstatus)
+				config->vinst_ctrl |= BIT(9);
+
+			/* No include/exclude filtering for ViewInst */
+			config->viiectlr = 0x0;
+			break;
+		default:
+			ret = -EINVAL;
+			goto out;
+		}
+	}
 
-	/* no start-stop filtering for ViewInst */
-	config->vissctlr = 0x0;
+	goto out;
+
+
+default_filter:
+	etm4_set_default_filter(config);
+
+out:
+	return ret;
 }
 
 void etm4_config_trace_mode(struct etmv4_config *config)
@@ -727,13 +950,9 @@ static int etm4_probe(struct amba_device *adev, const struct amba_id *id)
 	struct coresight_platform_data *pdata = NULL;
 	struct etmv4_drvdata *drvdata;
 	struct resource *res = &adev->res;
-	struct coresight_desc *desc;
+	struct coresight_desc desc = { 0 };
 	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;
@@ -788,13 +1007,13 @@ static int etm4_probe(struct amba_device *adev, const struct amba_id *id)
 	etm4_init_trace_id(drvdata);
 	etm4_set_default(&drvdata->config);
 
-	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);
+	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_arch_supported;
@@ -826,12 +1045,12 @@ err_arch_supported:
 }
 
 static struct amba_id etm4_ids[] = {
-	{       /* ETM 4.0 - Qualcomm */
-		.id	= 0x0003b95d,
-		.mask	= 0x0003ffff,
+	{       /* ETM 4.0 - Cortex-A53  */
+		.id	= 0x000bb95d,
+		.mask	= 0x000fffff,
 		.data	= "ETM 4.0",
 	},
-	{       /* ETM 4.0 - Juno board */
+	{       /* ETM 4.0 - Cortex-A57 */
 		.id	= 0x000bb95e,
 		.mask	= 0x000fffff,
 		.data	= "ETM 4.0",
diff --git a/drivers/hwtracing/coresight/coresight-etm4x.h b/drivers/hwtracing/coresight/coresight-etm4x.h
index 5359c5197c1d..ba8d3f86de21 100644
--- a/drivers/hwtracing/coresight/coresight-etm4x.h
+++ b/drivers/hwtracing/coresight/coresight-etm4x.h
@@ -183,6 +183,9 @@
 #define TRCSTATR_IDLE_BIT		0
 #define ETM_DEFAULT_ADDR_COMP		0
 
+/* PowerDown Control Register bits */
+#define TRCPDCR_PU			BIT(3)
+
 /* secure state access levels */
 #define ETM_EXLEVEL_S_APP		BIT(8)
 #define ETM_EXLEVEL_S_OS		BIT(9)
@@ -407,14 +410,6 @@ enum etm_addr_ctxtype {
 	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,
-};
-
 extern const struct attribute_group *coresight_etmv4_groups[];
 void etm4_config_trace_mode(struct etmv4_config *config);
 #endif
diff --git a/drivers/hwtracing/coresight/coresight-funnel.c b/drivers/hwtracing/coresight/coresight-funnel.c
index 05df789056cc..860fe6ef5632 100644
--- a/drivers/hwtracing/coresight/coresight-funnel.c
+++ b/drivers/hwtracing/coresight/coresight-funnel.c
@@ -176,7 +176,7 @@ static int funnel_probe(struct amba_device *adev, const struct amba_id *id)
 	struct coresight_platform_data *pdata = NULL;
 	struct funnel_drvdata *drvdata;
 	struct resource *res = &adev->res;
-	struct coresight_desc *desc;
+	struct coresight_desc desc = { 0 };
 	struct device_node *np = adev->dev.of_node;
 
 	if (np) {
@@ -207,17 +207,13 @@ static int funnel_probe(struct amba_device *adev, const struct amba_id *id)
 	drvdata->base = base;
 	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_MERG;
-	desc->ops = &funnel_cs_ops;
-	desc->pdata = pdata;
-	desc->dev = dev;
-	desc->groups = coresight_funnel_groups;
-	drvdata->csdev = coresight_register(desc);
+	desc.type = CORESIGHT_DEV_TYPE_LINK;
+	desc.subtype.link_subtype = CORESIGHT_DEV_SUBTYPE_LINK_MERG;
+	desc.ops = &funnel_cs_ops;
+	desc.pdata = pdata;
+	desc.dev = dev;
+	desc.groups = coresight_funnel_groups;
+	drvdata->csdev = coresight_register(&desc);
 	if (IS_ERR(drvdata->csdev))
 		return PTR_ERR(drvdata->csdev);
 
diff --git a/drivers/hwtracing/coresight/coresight-priv.h b/drivers/hwtracing/coresight/coresight-priv.h
index ad975c58080d..196a14be4b3d 100644
--- a/drivers/hwtracing/coresight/coresight-priv.h
+++ b/drivers/hwtracing/coresight/coresight-priv.h
@@ -16,6 +16,7 @@
 #include <linux/bitops.h>
 #include <linux/io.h>
 #include <linux/coresight.h>
+#include <linux/pm_runtime.h>
 
 /*
  * Coresight management registers (0xf00-0xfcc)
@@ -37,16 +38,32 @@
 #define ETM_MODE_EXCL_KERN	BIT(30)
 #define ETM_MODE_EXCL_USER	BIT(31)
 
-#define coresight_simple_func(type, name, offset)			\
+typedef u32 (*coresight_read_fn)(const struct device *, u32 offset);
+#define coresight_simple_func(type, func, name, offset)			\
 static ssize_t name##_show(struct device *_dev,				\
 			   struct device_attribute *attr, char *buf)	\
 {									\
 	type *drvdata = dev_get_drvdata(_dev->parent);			\
-	return scnprintf(buf, PAGE_SIZE, "0x%x\n",			\
-			 readl_relaxed(drvdata->base + offset));	\
+	coresight_read_fn fn = func;					\
+	u32 val;							\
+	pm_runtime_get_sync(_dev->parent);				\
+	if (fn)								\
+		val = fn(_dev->parent, offset);				\
+	else								\
+		val = readl_relaxed(drvdata->base + offset);		\
+	pm_runtime_put_sync(_dev->parent);				\
+	return scnprintf(buf, PAGE_SIZE, "0x%x\n", val);		\
 }									\
 static DEVICE_ATTR_RO(name)
 
+enum etm_addr_type {
+	ETM_ADDR_TYPE_NONE,
+	ETM_ADDR_TYPE_SINGLE,
+	ETM_ADDR_TYPE_RANGE,
+	ETM_ADDR_TYPE_START,
+	ETM_ADDR_TYPE_STOP,
+};
+
 enum cs_mode {
 	CS_MODE_DISABLED,
 	CS_MODE_SYSFS,
diff --git a/drivers/hwtracing/coresight/coresight-replicator-qcom.c b/drivers/hwtracing/coresight/coresight-replicator-qcom.c
index 700f710e4bfa..0a3d15f0b009 100644
--- a/drivers/hwtracing/coresight/coresight-replicator-qcom.c
+++ b/drivers/hwtracing/coresight/coresight-replicator-qcom.c
@@ -102,7 +102,7 @@ static int replicator_probe(struct amba_device *adev, const struct amba_id *id)
 	struct resource *res = &adev->res;
 	struct coresight_platform_data *pdata = NULL;
 	struct replicator_state *drvdata;
-	struct coresight_desc *desc;
+	struct coresight_desc desc = { 0 };
 	struct device_node *np = adev->dev.of_node;
 	void __iomem *base;
 
@@ -134,16 +134,12 @@ static int replicator_probe(struct amba_device *adev, const struct amba_id *id)
 	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);
+	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);
 
diff --git a/drivers/hwtracing/coresight/coresight-replicator.c b/drivers/hwtracing/coresight/coresight-replicator.c
index c6982e312e15..3756e71cb8f5 100644
--- a/drivers/hwtracing/coresight/coresight-replicator.c
+++ b/drivers/hwtracing/coresight/coresight-replicator.c
@@ -69,7 +69,7 @@ static int replicator_probe(struct platform_device *pdev)
 	struct device *dev = &pdev->dev;
 	struct coresight_platform_data *pdata = NULL;
 	struct replicator_drvdata *drvdata;
-	struct coresight_desc *desc;
+	struct coresight_desc desc = { 0 };
 	struct device_node *np = pdev->dev.of_node;
 
 	if (np) {
@@ -95,18 +95,12 @@ static int replicator_probe(struct platform_device *pdev)
 	pm_runtime_enable(&pdev->dev);
 	platform_set_drvdata(pdev, drvdata);
 
-	desc = devm_kzalloc(dev, sizeof(*desc), GFP_KERNEL);
-	if (!desc) {
-		ret = -ENOMEM;
-		goto out_disable_pm;
-	}
-
-	desc->type = CORESIGHT_DEV_TYPE_LINK;
-	desc->subtype.link_subtype = CORESIGHT_DEV_SUBTYPE_LINK_SPLIT;
-	desc->ops = &replicator_cs_ops;
-	desc->pdata = pdev->dev.platform_data;
-	desc->dev = &pdev->dev;
-	drvdata->csdev = coresight_register(desc);
+	desc.type = CORESIGHT_DEV_TYPE_LINK;
+	desc.subtype.link_subtype = CORESIGHT_DEV_SUBTYPE_LINK_SPLIT;
+	desc.ops = &replicator_cs_ops;
+	desc.pdata = pdev->dev.platform_data;
+	desc.dev = &pdev->dev;
+	drvdata->csdev = coresight_register(&desc);
 	if (IS_ERR(drvdata->csdev)) {
 		ret = PTR_ERR(drvdata->csdev);
 		goto out_disable_pm;
diff --git a/drivers/hwtracing/coresight/coresight-stm.c b/drivers/hwtracing/coresight/coresight-stm.c
index 73be58a11e4f..49e0f1b925a5 100644
--- a/drivers/hwtracing/coresight/coresight-stm.c
+++ b/drivers/hwtracing/coresight/coresight-stm.c
@@ -105,10 +105,12 @@ module_param_named(
 /**
  * struct channel_space - central management entity for extended ports
  * @base:		memory mapped base address where channels start.
+ * @phys:		physical base address of channel region.
  * @guaraneed:		is the channel delivery guaranteed.
  */
 struct channel_space {
 	void __iomem		*base;
+	phys_addr_t		phys;
 	unsigned long		*guaranteed;
 };
 
@@ -196,7 +198,7 @@ static void stm_enable_hw(struct stm_drvdata *drvdata)
 }
 
 static int stm_enable(struct coresight_device *csdev,
-		      struct perf_event_attr *attr, u32 mode)
+		      struct perf_event *event, u32 mode)
 {
 	u32 val;
 	struct stm_drvdata *drvdata = dev_get_drvdata(csdev->dev.parent);
@@ -258,7 +260,8 @@ static void stm_disable_hw(struct stm_drvdata *drvdata)
 		stm_hwevent_disable_hw(drvdata);
 }
 
-static void stm_disable(struct coresight_device *csdev)
+static void stm_disable(struct coresight_device *csdev,
+			struct perf_event *event)
 {
 	struct stm_drvdata *drvdata = dev_get_drvdata(csdev->dev.parent);
 
@@ -353,7 +356,24 @@ static void stm_generic_unlink(struct stm_data *stm_data,
 	if (!drvdata || !drvdata->csdev)
 		return;
 
-	stm_disable(drvdata->csdev);
+	stm_disable(drvdata->csdev, NULL);
+}
+
+static phys_addr_t
+stm_mmio_addr(struct stm_data *stm_data, unsigned int master,
+	      unsigned int channel, unsigned int nr_chans)
+{
+	struct stm_drvdata *drvdata = container_of(stm_data,
+						   struct stm_drvdata, stm);
+	phys_addr_t addr;
+
+	addr = drvdata->chs.phys + channel * BYTES_PER_CHANNEL;
+
+	if (offset_in_page(addr) ||
+	    offset_in_page(nr_chans * BYTES_PER_CHANNEL))
+		return 0;
+
+	return addr;
 }
 
 static long stm_generic_set_options(struct stm_data *stm_data,
@@ -616,7 +636,7 @@ static ssize_t traceid_store(struct device *dev,
 static DEVICE_ATTR_RW(traceid);
 
 #define coresight_stm_simple_func(name, offset)	\
-	coresight_simple_func(struct stm_drvdata, name, offset)
+	coresight_simple_func(struct stm_drvdata, NULL, name, offset)
 
 coresight_stm_simple_func(tcsr, STMTCSR);
 coresight_stm_simple_func(tsfreqr, STMTSFREQR);
@@ -761,7 +781,9 @@ static void stm_init_generic_data(struct stm_drvdata *drvdata)
 	drvdata->stm.sw_end = 1;
 	drvdata->stm.hw_override = true;
 	drvdata->stm.sw_nchannels = drvdata->numsp;
+	drvdata->stm.sw_mmiosz = BYTES_PER_CHANNEL;
 	drvdata->stm.packet = stm_generic_packet;
+	drvdata->stm.mmio_addr = stm_mmio_addr;
 	drvdata->stm.link = stm_generic_link;
 	drvdata->stm.unlink = stm_generic_unlink;
 	drvdata->stm.set_options = stm_generic_set_options;
@@ -778,7 +800,7 @@ static int stm_probe(struct amba_device *adev, const struct amba_id *id)
 	struct resource *res = &adev->res;
 	struct resource ch_res;
 	size_t res_size, bitmap_size;
-	struct coresight_desc *desc;
+	struct coresight_desc desc = { 0 };
 	struct device_node *np = adev->dev.of_node;
 
 	if (np) {
@@ -808,6 +830,7 @@ static int stm_probe(struct amba_device *adev, const struct amba_id *id)
 	ret = stm_get_resource_byname(np, "stm-stimulus-base", &ch_res);
 	if (ret)
 		return ret;
+	drvdata->chs.phys = ch_res.start;
 
 	base = devm_ioremap_resource(dev, &ch_res);
 	if (IS_ERR(base))
@@ -843,19 +866,13 @@ static int stm_probe(struct amba_device *adev, const struct amba_id *id)
 		return -EPROBE_DEFER;
 	}
 
-	desc = devm_kzalloc(dev, sizeof(*desc), GFP_KERNEL);
-	if (!desc) {
-		ret = -ENOMEM;
-		goto stm_unregister;
-	}
-
-	desc->type = CORESIGHT_DEV_TYPE_SOURCE;
-	desc->subtype.source_subtype = CORESIGHT_DEV_SUBTYPE_SOURCE_SOFTWARE;
-	desc->ops = &stm_cs_ops;
-	desc->pdata = pdata;
-	desc->dev = dev;
-	desc->groups = coresight_stm_groups;
-	drvdata->csdev = coresight_register(desc);
+	desc.type = CORESIGHT_DEV_TYPE_SOURCE;
+	desc.subtype.source_subtype = CORESIGHT_DEV_SUBTYPE_SOURCE_SOFTWARE;
+	desc.ops = &stm_cs_ops;
+	desc.pdata = pdata;
+	desc.dev = dev;
+	desc.groups = coresight_stm_groups;
+	drvdata->csdev = coresight_register(&desc);
 	if (IS_ERR(drvdata->csdev)) {
 		ret = PTR_ERR(drvdata->csdev);
 		goto stm_unregister;
diff --git a/drivers/hwtracing/coresight/coresight-tmc-etf.c b/drivers/hwtracing/coresight/coresight-tmc-etf.c
index 466af86fd76f..d6941ea24d8d 100644
--- a/drivers/hwtracing/coresight/coresight-tmc-etf.c
+++ b/drivers/hwtracing/coresight/coresight-tmc-etf.c
@@ -22,7 +22,7 @@
 #include "coresight-priv.h"
 #include "coresight-tmc.h"
 
-void tmc_etb_enable_hw(struct tmc_drvdata *drvdata)
+static void tmc_etb_enable_hw(struct tmc_drvdata *drvdata)
 {
 	CS_UNLOCK(drvdata->base);
 
@@ -48,6 +48,7 @@ static void tmc_etb_dump_hw(struct tmc_drvdata *drvdata)
 	int i;
 
 	bufp = drvdata->buf;
+	drvdata->len = 0;
 	while (1) {
 		for (i = 0; i < drvdata->memwidth; i++) {
 			read_data = readl_relaxed(drvdata->base + TMC_RRD);
@@ -55,6 +56,7 @@ static void tmc_etb_dump_hw(struct tmc_drvdata *drvdata)
 				return;
 			memcpy(bufp, &read_data, 4);
 			bufp += 4;
+			drvdata->len += 4;
 		}
 	}
 }
@@ -166,7 +168,7 @@ out:
 	spin_unlock_irqrestore(&drvdata->spinlock, flags);
 
 	/* Free memory outside the spinlock if need be */
-	if (!used && buf)
+	if (!used)
 		kfree(buf);
 
 	if (!ret)
diff --git a/drivers/hwtracing/coresight/coresight-tmc-etr.c b/drivers/hwtracing/coresight/coresight-tmc-etr.c
index 688be9e060fc..886ea83c68e0 100644
--- a/drivers/hwtracing/coresight/coresight-tmc-etr.c
+++ b/drivers/hwtracing/coresight/coresight-tmc-etr.c
@@ -20,7 +20,7 @@
 #include "coresight-priv.h"
 #include "coresight-tmc.h"
 
-void tmc_etr_enable_hw(struct tmc_drvdata *drvdata)
+static void tmc_etr_enable_hw(struct tmc_drvdata *drvdata)
 {
 	u32 axictl;
 
@@ -64,11 +64,17 @@ static void tmc_etr_dump_hw(struct tmc_drvdata *drvdata)
 	rwp = readl_relaxed(drvdata->base + TMC_RWP);
 	val = readl_relaxed(drvdata->base + TMC_STS);
 
-	/* How much memory do we still have */
-	if (val & BIT(0))
+	/*
+	 * Adjust the buffer to point to the beginning of the trace data
+	 * and update the available trace data.
+	 */
+	if (val & TMC_STS_FULL) {
 		drvdata->buf = drvdata->vaddr + rwp - drvdata->paddr;
-	else
+		drvdata->len = drvdata->size;
+	} else {
 		drvdata->buf = drvdata->vaddr;
+		drvdata->len = rwp - drvdata->paddr;
+	}
 }
 
 static void tmc_etr_disable_hw(struct tmc_drvdata *drvdata)
diff --git a/drivers/hwtracing/coresight/coresight-tmc.c b/drivers/hwtracing/coresight/coresight-tmc.c
index 9e02ac963cd0..d8517d2a968c 100644
--- a/drivers/hwtracing/coresight/coresight-tmc.c
+++ b/drivers/hwtracing/coresight/coresight-tmc.c
@@ -38,8 +38,7 @@ void tmc_wait_for_tmcready(struct tmc_drvdata *drvdata)
 	if (coresight_timeout(drvdata->base,
 			      TMC_STS, TMC_STS_TMCREADY_BIT, 1)) {
 		dev_err(drvdata->dev,
-			"timeout observed when probing at offset %#x\n",
-			TMC_STS);
+			"timeout while waiting for TMC to be Ready\n");
 	}
 }
 
@@ -56,8 +55,7 @@ void tmc_flush_and_stop(struct tmc_drvdata *drvdata)
 	if (coresight_timeout(drvdata->base,
 			      TMC_FFCR, TMC_FFCR_FLUSHMAN_BIT, 0)) {
 		dev_err(drvdata->dev,
-			"timeout observed when probing at offset %#x\n",
-			TMC_FFCR);
+		"timeout while waiting for completion of Manual Flush\n");
 	}
 
 	tmc_wait_for_tmcready(drvdata);
@@ -140,8 +138,8 @@ static ssize_t tmc_read(struct file *file, char __user *data, size_t len,
 						   struct tmc_drvdata, miscdev);
 	char *bufp = drvdata->buf + *ppos;
 
-	if (*ppos + len > drvdata->size)
-		len = drvdata->size - *ppos;
+	if (*ppos + len > drvdata->len)
+		len = drvdata->len - *ppos;
 
 	if (drvdata->config_type == TMC_CONFIG_TYPE_ETR) {
 		if (bufp == (char *)(drvdata->vaddr + drvdata->size))
@@ -160,7 +158,7 @@ static ssize_t tmc_read(struct file *file, char __user *data, size_t len,
 	*ppos += len;
 
 	dev_dbg(drvdata->dev, "%s: %zu bytes copied, %d bytes left\n",
-		__func__, len, (int)(drvdata->size - *ppos));
+		__func__, len, (int)(drvdata->len - *ppos));
 	return len;
 }
 
@@ -220,7 +218,7 @@ static enum tmc_mem_intf_width tmc_get_memwidth(u32 devid)
 }
 
 #define coresight_tmc_simple_func(name, offset)			\
-	coresight_simple_func(struct tmc_drvdata, name, offset)
+	coresight_simple_func(struct tmc_drvdata, NULL, name, offset)
 
 coresight_tmc_simple_func(rsz, TMC_RSZ);
 coresight_tmc_simple_func(sts, TMC_STS);
@@ -249,8 +247,8 @@ static struct attribute *coresight_tmc_mgmt_attrs[] = {
 	NULL,
 };
 
-ssize_t trigger_cntr_show(struct device *dev,
-			  struct device_attribute *attr, char *buf)
+static ssize_t trigger_cntr_show(struct device *dev,
+				 struct device_attribute *attr, char *buf)
 {
 	struct tmc_drvdata *drvdata = dev_get_drvdata(dev->parent);
 	unsigned long val = drvdata->trigger_cntr;
@@ -304,27 +302,32 @@ static int tmc_probe(struct amba_device *adev, const struct amba_id *id)
 	struct coresight_platform_data *pdata = NULL;
 	struct tmc_drvdata *drvdata;
 	struct resource *res = &adev->res;
-	struct coresight_desc *desc;
+	struct coresight_desc desc = { 0 };
 	struct device_node *np = adev->dev.of_node;
 
 	if (np) {
 		pdata = of_get_coresight_platform_data(dev, np);
-		if (IS_ERR(pdata))
-			return PTR_ERR(pdata);
+		if (IS_ERR(pdata)) {
+			ret = PTR_ERR(pdata);
+			goto out;
+		}
 		adev->dev.platform_data = pdata;
 	}
 
+	ret = -ENOMEM;
 	drvdata = devm_kzalloc(dev, sizeof(*drvdata), GFP_KERNEL);
 	if (!drvdata)
-		return -ENOMEM;
+		goto out;
 
 	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);
+	if (IS_ERR(base)) {
+		ret = PTR_ERR(base);
+		goto out;
+	}
 
 	drvdata->base = base;
 
@@ -347,33 +350,28 @@ static int tmc_probe(struct amba_device *adev, const struct amba_id *id)
 
 	pm_runtime_put(&adev->dev);
 
-	desc = devm_kzalloc(dev, sizeof(*desc), GFP_KERNEL);
-	if (!desc) {
-		ret = -ENOMEM;
-		goto err_devm_kzalloc;
-	}
-
-	desc->pdata = pdata;
-	desc->dev = dev;
-	desc->subtype.sink_subtype = CORESIGHT_DEV_SUBTYPE_SINK_BUFFER;
-	desc->groups = coresight_tmc_groups;
+	desc.pdata = pdata;
+	desc.dev = dev;
+	desc.groups = coresight_tmc_groups;
 
 	if (drvdata->config_type == TMC_CONFIG_TYPE_ETB) {
-		desc->type = CORESIGHT_DEV_TYPE_SINK;
-		desc->ops = &tmc_etb_cs_ops;
+		desc.type = CORESIGHT_DEV_TYPE_SINK;
+		desc.subtype.sink_subtype = CORESIGHT_DEV_SUBTYPE_SINK_BUFFER;
+		desc.ops = &tmc_etb_cs_ops;
 	} else if (drvdata->config_type == TMC_CONFIG_TYPE_ETR) {
-		desc->type = CORESIGHT_DEV_TYPE_SINK;
-		desc->ops = &tmc_etr_cs_ops;
+		desc.type = CORESIGHT_DEV_TYPE_SINK;
+		desc.subtype.sink_subtype = CORESIGHT_DEV_SUBTYPE_SINK_BUFFER;
+		desc.ops = &tmc_etr_cs_ops;
 	} else {
-		desc->type = CORESIGHT_DEV_TYPE_LINKSINK;
-		desc->subtype.link_subtype = CORESIGHT_DEV_SUBTYPE_LINK_FIFO;
-		desc->ops = &tmc_etf_cs_ops;
+		desc.type = CORESIGHT_DEV_TYPE_LINKSINK;
+		desc.subtype.link_subtype = CORESIGHT_DEV_SUBTYPE_LINK_FIFO;
+		desc.ops = &tmc_etf_cs_ops;
 	}
 
-	drvdata->csdev = coresight_register(desc);
+	drvdata->csdev = coresight_register(&desc);
 	if (IS_ERR(drvdata->csdev)) {
 		ret = PTR_ERR(drvdata->csdev);
-		goto err_devm_kzalloc;
+		goto out;
 	}
 
 	drvdata->miscdev.name = pdata->name;
@@ -381,16 +379,8 @@ static int tmc_probe(struct amba_device *adev, const struct amba_id *id)
 	drvdata->miscdev.fops = &tmc_fops;
 	ret = misc_register(&drvdata->miscdev);
 	if (ret)
-		goto err_misc_register;
-
-	return 0;
-
-err_misc_register:
-	coresight_unregister(drvdata->csdev);
-err_devm_kzalloc:
-	if (drvdata->config_type == TMC_CONFIG_TYPE_ETR)
-		dma_free_coherent(dev, drvdata->size,
-				drvdata->vaddr, drvdata->paddr);
+		coresight_unregister(drvdata->csdev);
+out:
 	return ret;
 }
 
diff --git a/drivers/hwtracing/coresight/coresight-tmc.h b/drivers/hwtracing/coresight/coresight-tmc.h
index 5c5fe2ad2ca7..44b3ae346118 100644
--- a/drivers/hwtracing/coresight/coresight-tmc.h
+++ b/drivers/hwtracing/coresight/coresight-tmc.h
@@ -98,7 +98,8 @@ enum tmc_mem_intf_width {
  * @buf:	area of memory where trace data get sent.
  * @paddr:	DMA start location in RAM.
  * @vaddr:	virtual representation of @paddr.
- * @size:	@buf size.
+ * @size:	trace buffer size.
+ * @len:	size of the available trace.
  * @mode:	how this TMC is being used.
  * @config_type: TMC variant, must be of type @tmc_config_type.
  * @memwidth:	width of the memory interface databus, in bytes.
@@ -115,6 +116,7 @@ struct tmc_drvdata {
 	dma_addr_t		paddr;
 	void __iomem		*vaddr;
 	u32			size;
+	u32			len;
 	local_t			mode;
 	enum tmc_config_type	config_type;
 	enum tmc_mem_intf_width	memwidth;
diff --git a/drivers/hwtracing/coresight/coresight-tpiu.c b/drivers/hwtracing/coresight/coresight-tpiu.c
index 4e471e2e9d89..0673baf0f2f5 100644
--- a/drivers/hwtracing/coresight/coresight-tpiu.c
+++ b/drivers/hwtracing/coresight/coresight-tpiu.c
@@ -119,7 +119,7 @@ static int tpiu_probe(struct amba_device *adev, const struct amba_id *id)
 	struct coresight_platform_data *pdata = NULL;
 	struct tpiu_drvdata *drvdata;
 	struct resource *res = &adev->res;
-	struct coresight_desc *desc;
+	struct coresight_desc desc = { 0 };
 	struct device_node *np = adev->dev.of_node;
 
 	if (np) {
@@ -154,16 +154,12 @@ static int tpiu_probe(struct amba_device *adev, const struct amba_id *id)
 
 	pm_runtime_put(&adev->dev);
 
-	desc = devm_kzalloc(dev, sizeof(*desc), GFP_KERNEL);
-	if (!desc)
-		return -ENOMEM;
-
-	desc->type = CORESIGHT_DEV_TYPE_SINK;
-	desc->subtype.sink_subtype = CORESIGHT_DEV_SUBTYPE_SINK_PORT;
-	desc->ops = &tpiu_cs_ops;
-	desc->pdata = pdata;
-	desc->dev = dev;
-	drvdata->csdev = coresight_register(desc);
+	desc.type = CORESIGHT_DEV_TYPE_SINK;
+	desc.subtype.sink_subtype = CORESIGHT_DEV_SUBTYPE_SINK_PORT;
+	desc.ops = &tpiu_cs_ops;
+	desc.pdata = pdata;
+	desc.dev = dev;
+	drvdata->csdev = coresight_register(&desc);
 	if (IS_ERR(drvdata->csdev))
 		return PTR_ERR(drvdata->csdev);
 
diff --git a/drivers/hwtracing/coresight/coresight.c b/drivers/hwtracing/coresight/coresight.c
index d08d1ab9bba5..7bf00a0beb6f 100644
--- a/drivers/hwtracing/coresight/coresight.c
+++ b/drivers/hwtracing/coresight/coresight.c
@@ -257,7 +257,7 @@ static void coresight_disable_source(struct coresight_device *csdev)
 {
 	if (atomic_dec_return(csdev->refcnt) == 0) {
 		if (source_ops(csdev)->disable) {
-			source_ops(csdev)->disable(csdev);
+			source_ops(csdev)->disable(csdev, NULL);
 			csdev->enable = false;
 		}
 	}
@@ -429,7 +429,7 @@ struct list_head *coresight_build_path(struct coresight_device *csdev)
 
 	path = kzalloc(sizeof(struct list_head), GFP_KERNEL);
 	if (!path)
-		return NULL;
+		return ERR_PTR(-ENOMEM);
 
 	INIT_LIST_HEAD(path);
 
@@ -725,7 +725,8 @@ static int coresight_orphan_match(struct device *dev, void *data)
 		/* We have found at least one orphan connection */
 		if (conn->child_dev == NULL) {
 			/* Does it match this newly added device? */
-			if (!strcmp(dev_name(&csdev->dev), conn->child_name)) {
+			if (conn->child_name &&
+			    !strcmp(dev_name(&csdev->dev), conn->child_name)) {
 				conn->child_dev = csdev;
 			} else {
 				/* This component still has an orphan */
@@ -893,7 +894,7 @@ struct coresight_device *coresight_register(struct coresight_desc *desc)
 	int nr_refcnts = 1;
 	atomic_t *refcnts = NULL;
 	struct coresight_device *csdev;
-	struct coresight_connection *conns;
+	struct coresight_connection *conns = NULL;
 
 	csdev = kzalloc(sizeof(*csdev), GFP_KERNEL);
 	if (!csdev) {
@@ -921,16 +922,20 @@ struct coresight_device *coresight_register(struct coresight_desc *desc)
 
 	csdev->nr_inport = desc->pdata->nr_inport;
 	csdev->nr_outport = desc->pdata->nr_outport;
-	conns = kcalloc(csdev->nr_outport, sizeof(*conns), GFP_KERNEL);
-	if (!conns) {
-		ret = -ENOMEM;
-		goto err_kzalloc_conns;
-	}
 
-	for (i = 0; i < csdev->nr_outport; i++) {
-		conns[i].outport = desc->pdata->outports[i];
-		conns[i].child_name = desc->pdata->child_names[i];
-		conns[i].child_port = desc->pdata->child_ports[i];
+	/* Initialise connections if there is at least one outport */
+	if (csdev->nr_outport) {
+		conns = kcalloc(csdev->nr_outport, sizeof(*conns), GFP_KERNEL);
+		if (!conns) {
+			ret = -ENOMEM;
+			goto err_kzalloc_conns;
+		}
+
+		for (i = 0; i < csdev->nr_outport; i++) {
+			conns[i].outport = desc->pdata->outports[i];
+			conns[i].child_name = desc->pdata->child_names[i];
+			conns[i].child_port = desc->pdata->child_ports[i];
+		}
 	}
 
 	csdev->conns = conns;
diff --git a/drivers/hwtracing/coresight/of_coresight.c b/drivers/hwtracing/coresight/of_coresight.c
index b68da1888fd5..629e031b7456 100644
--- a/drivers/hwtracing/coresight/of_coresight.c
+++ b/drivers/hwtracing/coresight/of_coresight.c
@@ -166,7 +166,7 @@ struct coresight_platform_data *of_get_coresight_platform_data(
 
 			rdev = of_coresight_get_endpoint_device(rparent);
 			if (!rdev)
-				continue;
+				return ERR_PTR(-EPROBE_DEFER);
 
 			pdata->child_names[i] = dev_name(rdev);
 			pdata->child_ports[i] = rendpoint.id;
@@ -184,6 +184,7 @@ struct coresight_platform_data *of_get_coresight_platform_data(
 			break;
 		}
 	}
+	of_node_put(dn);
 
 	return pdata;
 }
diff --git a/drivers/i2c/busses/Kconfig b/drivers/i2c/busses/Kconfig
index 5c3993b26129..6d94e2ec5b4f 100644
--- a/drivers/i2c/busses/Kconfig
+++ b/drivers/i2c/busses/Kconfig
@@ -836,7 +836,7 @@ config I2C_SH7760
 config I2C_SH_MOBILE
 	tristate "SuperH Mobile I2C Controller"
 	depends on HAS_DMA
-	depends on SUPERH || ARCH_RENESAS || COMPILE_TEST
+	depends on ARCH_SHMOBILE || ARCH_RENESAS || COMPILE_TEST
 	help
 	  If you say yes to this option, support will be included for the
 	  built-in I2C interface on the Renesas SH-Mobile processor.
@@ -956,6 +956,17 @@ config I2C_OCTEON
 	  This driver can also be built as a module.  If so, the module
 	  will be called i2c-octeon.
 
+config I2C_THUNDERX
+	tristate "Cavium ThunderX I2C bus support"
+	depends on 64BIT && PCI && (ARM64 || COMPILE_TEST)
+	select I2C_SMBUS
+	help
+	  Say yes if you want to support the I2C serial bus on Cavium
+	  ThunderX SOC.
+
+	  This driver can also be built as a module.  If so, the module
+	  will be called i2c-thunderx.
+
 config I2C_XILINX
 	tristate "Xilinx I2C Controller"
 	depends on HAS_IOMEM
diff --git a/drivers/i2c/busses/Makefile b/drivers/i2c/busses/Makefile
index 37f2819b4560..29764cc20a44 100644
--- a/drivers/i2c/busses/Makefile
+++ b/drivers/i2c/busses/Makefile
@@ -91,7 +91,10 @@ obj-$(CONFIG_I2C_UNIPHIER)	+= i2c-uniphier.o
 obj-$(CONFIG_I2C_UNIPHIER_F)	+= i2c-uniphier-f.o
 obj-$(CONFIG_I2C_VERSATILE)	+= i2c-versatile.o
 obj-$(CONFIG_I2C_WMT)		+= i2c-wmt.o
+i2c-octeon-objs := i2c-octeon-core.o i2c-octeon-platdrv.o
 obj-$(CONFIG_I2C_OCTEON)	+= i2c-octeon.o
+i2c-thunderx-objs := i2c-octeon-core.o i2c-thunderx-pcidrv.o
+obj-$(CONFIG_I2C_THUNDERX)	+= i2c-thunderx.o
 obj-$(CONFIG_I2C_XILINX)	+= i2c-xiic.o
 obj-$(CONFIG_I2C_XLR)		+= i2c-xlr.o
 obj-$(CONFIG_I2C_XLP9XX)	+= i2c-xlp9xx.o
diff --git a/drivers/i2c/busses/i2c-amd756.c b/drivers/i2c/busses/i2c-amd756.c
index 6c7113d990f8..274908cd1fde 100644
--- a/drivers/i2c/busses/i2c-amd756.c
+++ b/drivers/i2c/busses/i2c-amd756.c
@@ -378,11 +378,8 @@ static int amd756_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 		 amd756_ioport);
 
 	error = i2c_add_adapter(&amd756_smbus);
-	if (error) {
-		dev_err(&pdev->dev,
-			"Adapter registration failed, module not inserted\n");
+	if (error)
 		goto out_err;
-	}
 
 	return 0;
 
diff --git a/drivers/i2c/busses/i2c-at91.c b/drivers/i2c/busses/i2c-at91.c
index 1bb97f658b47..0b86c6173e07 100644
--- a/drivers/i2c/busses/i2c-at91.c
+++ b/drivers/i2c/busses/i2c-at91.c
@@ -1122,8 +1122,6 @@ static int at91_twi_probe(struct platform_device *pdev)
 
 	rc = i2c_add_numbered_adapter(&dev->adapter);
 	if (rc) {
-		dev_err(dev->dev, "Adapter %s registration failed\n",
-			dev->adapter.name);
 		clk_disable_unprepare(dev->clk);
 
 		pm_runtime_disable(dev->dev);
diff --git a/drivers/i2c/busses/i2c-axxia.c b/drivers/i2c/busses/i2c-axxia.c
index c335cc7852f9..4351a9343058 100644
--- a/drivers/i2c/busses/i2c-axxia.c
+++ b/drivers/i2c/busses/i2c-axxia.c
@@ -545,7 +545,11 @@ static int axxia_i2c_probe(struct platform_device *pdev)
 		return ret;
 	}
 
-	clk_prepare_enable(idev->i2c_clk);
+	ret = clk_prepare_enable(idev->i2c_clk);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to enable clock\n");
+		return ret;
+	}
 
 	i2c_set_adapdata(&idev->adapter, idev);
 	strlcpy(idev->adapter.name, pdev->name, sizeof(idev->adapter.name));
@@ -560,7 +564,7 @@ static int axxia_i2c_probe(struct platform_device *pdev)
 
 	ret = i2c_add_adapter(&idev->adapter);
 	if (ret) {
-		dev_err(&pdev->dev, "failed to add adapter\n");
+		clk_disable_unprepare(idev->i2c_clk);
 		return ret;
 	}
 
diff --git a/drivers/i2c/busses/i2c-bcm-iproc.c b/drivers/i2c/busses/i2c-bcm-iproc.c
index 95f7cac76f89..326b3db02c48 100644
--- a/drivers/i2c/busses/i2c-bcm-iproc.c
+++ b/drivers/i2c/busses/i2c-bcm-iproc.c
@@ -488,13 +488,7 @@ static int bcm_iproc_i2c_probe(struct platform_device *pdev)
 	adap->dev.parent = &pdev->dev;
 	adap->dev.of_node = pdev->dev.of_node;
 
-	ret = i2c_add_adapter(adap);
-	if (ret) {
-		dev_err(iproc_i2c->device, "failed to add adapter\n");
-		return ret;
-	}
-
-	return 0;
+	return i2c_add_adapter(adap);
 }
 
 static int bcm_iproc_i2c_remove(struct platform_device *pdev)
diff --git a/drivers/i2c/busses/i2c-bcm-kona.c b/drivers/i2c/busses/i2c-bcm-kona.c
index 258cb9a40ab3..4e489a9d16fb 100644
--- a/drivers/i2c/busses/i2c-bcm-kona.c
+++ b/drivers/i2c/busses/i2c-bcm-kona.c
@@ -858,10 +858,8 @@ static int bcm_kona_i2c_probe(struct platform_device *pdev)
 	adap->dev.of_node = pdev->dev.of_node;
 
 	rc = i2c_add_adapter(adap);
-	if (rc) {
-		dev_err(dev->device, "failed to add adapter\n");
+	if (rc)
 		return rc;
-	}
 
 	dev_info(dev->device, "device registered successfully\n");
 
diff --git a/drivers/i2c/busses/i2c-bfin-twi.c b/drivers/i2c/busses/i2c-bfin-twi.c
index 025686d41640..29d00c4f7824 100644
--- a/drivers/i2c/busses/i2c-bfin-twi.c
+++ b/drivers/i2c/busses/i2c-bfin-twi.c
@@ -685,10 +685,8 @@ static int i2c_bfin_twi_probe(struct platform_device *pdev)
 	write_CONTROL(iface, read_CONTROL(iface) | TWI_ENA);
 
 	rc = i2c_add_numbered_adapter(p_adap);
-	if (rc < 0) {
-		dev_err(&pdev->dev, "Can't add i2c adapter!\n");
+	if (rc < 0)
 		goto out_error;
-	}
 
 	platform_set_drvdata(pdev, iface);
 
diff --git a/drivers/i2c/busses/i2c-brcmstb.c b/drivers/i2c/busses/i2c-brcmstb.c
index 385b57bfcb38..0652281662a8 100644
--- a/drivers/i2c/busses/i2c-brcmstb.c
+++ b/drivers/i2c/busses/i2c-brcmstb.c
@@ -648,10 +648,8 @@ static int brcmstb_i2c_probe(struct platform_device *pdev)
 	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");
+	if (rc)
 		goto probe_errorout;
-	}
 
 	dev_info(dev->device, "%s@%dhz registered in %s mode\n",
 		 int_name ? int_name : " ", dev->clk_freq_hz,
diff --git a/drivers/i2c/busses/i2c-cadence.c b/drivers/i2c/busses/i2c-cadence.c
index 3c16a2f7c673..686971263bef 100644
--- a/drivers/i2c/busses/i2c-cadence.c
+++ b/drivers/i2c/busses/i2c-cadence.c
@@ -963,10 +963,8 @@ static int cdns_i2c_probe(struct platform_device *pdev)
 	}
 
 	ret = i2c_add_adapter(&id->adap);
-	if (ret < 0) {
-		dev_err(&pdev->dev, "reg adap failed: %d\n", ret);
+	if (ret < 0)
 		goto err_clk_dis;
-	}
 
 	/*
 	 * Cadence I2C controller has a bug wherein it generates
diff --git a/drivers/i2c/busses/i2c-cpm.c b/drivers/i2c/busses/i2c-cpm.c
index ee57c1e865e2..d89bde2c5da2 100644
--- a/drivers/i2c/busses/i2c-cpm.c
+++ b/drivers/i2c/busses/i2c-cpm.c
@@ -665,10 +665,8 @@ static int cpm_i2c_probe(struct platform_device *ofdev)
 	cpm->adap.nr = (data && len == 4) ? be32_to_cpup(data) : -1;
 	result = i2c_add_numbered_adapter(&cpm->adap);
 
-	if (result < 0) {
-		dev_err(&ofdev->dev, "Unable to register with I2C\n");
+	if (result < 0)
 		goto out_shut;
-	}
 
 	dev_dbg(&ofdev->dev, "hw routines for %s registered.\n",
 		cpm->adap.name);
diff --git a/drivers/i2c/busses/i2c-cros-ec-tunnel.c b/drivers/i2c/busses/i2c-cros-ec-tunnel.c
index 2d5ff86398d0..9b36a7b3befd 100644
--- a/drivers/i2c/busses/i2c-cros-ec-tunnel.c
+++ b/drivers/i2c/busses/i2c-cros-ec-tunnel.c
@@ -281,10 +281,8 @@ static int ec_i2c_probe(struct platform_device *pdev)
 	bus->adap.retries = I2C_MAX_RETRIES;
 
 	err = i2c_add_adapter(&bus->adap);
-	if (err) {
-		dev_err(dev, "cannot register i2c adapter\n");
+	if (err)
 		return err;
-	}
 	platform_set_drvdata(pdev, bus);
 
 	return err;
diff --git a/drivers/i2c/busses/i2c-davinci.c b/drivers/i2c/busses/i2c-davinci.c
index a8bdcb5292f5..9e7ef5cf5d49 100644
--- a/drivers/i2c/busses/i2c-davinci.c
+++ b/drivers/i2c/busses/i2c-davinci.c
@@ -846,10 +846,8 @@ static int davinci_i2c_probe(struct platform_device *pdev)
 
 	adap->nr = pdev->id;
 	r = i2c_add_numbered_adapter(adap);
-	if (r) {
-		dev_err(&pdev->dev, "failure adding adapter\n");
+	if (r)
 		goto err_unuse_clocks;
-	}
 
 	return 0;
 
diff --git a/drivers/i2c/busses/i2c-designware-core.c b/drivers/i2c/busses/i2c-designware-core.c
index fcd973d5131e..1fe93c43215c 100644
--- a/drivers/i2c/busses/i2c-designware-core.c
+++ b/drivers/i2c/busses/i2c-designware-core.c
@@ -42,6 +42,8 @@
 #define DW_IC_SS_SCL_LCNT	0x18
 #define DW_IC_FS_SCL_HCNT	0x1c
 #define DW_IC_FS_SCL_LCNT	0x20
+#define DW_IC_HS_SCL_HCNT	0x24
+#define DW_IC_HS_SCL_LCNT	0x28
 #define DW_IC_INTR_STAT		0x2c
 #define DW_IC_INTR_MASK		0x30
 #define DW_IC_RAW_INTR_STAT	0x34
@@ -89,12 +91,17 @@
 					 DW_IC_INTR_TX_ABRT | \
 					 DW_IC_INTR_STOP_DET)
 
-#define DW_IC_STATUS_ACTIVITY	0x1
+#define DW_IC_STATUS_ACTIVITY		0x1
+#define DW_IC_STATUS_TFE		BIT(2)
+#define DW_IC_STATUS_MST_ACTIVITY	BIT(5)
 
 #define DW_IC_ERR_TX_ABRT	0x1
 
 #define DW_IC_TAR_10BITADDR_MASTER BIT(12)
 
+#define DW_IC_COMP_PARAM_1_SPEED_MODE_HIGH	(BIT(2) | BIT(3))
+#define DW_IC_COMP_PARAM_1_SPEED_MODE_MASK	GENMASK(3, 2)
+
 /*
  * status codes
  */
@@ -252,10 +259,15 @@ static u32 i2c_dw_scl_lcnt(u32 ic_clk, u32 tLOW, u32 tf, int offset)
 
 static void __i2c_dw_enable(struct dw_i2c_dev *dev, bool enable)
 {
+	dw_writel(dev, enable, DW_IC_ENABLE);
+}
+
+static void __i2c_dw_enable_and_wait(struct dw_i2c_dev *dev, bool enable)
+{
 	int timeout = 100;
 
 	do {
-		dw_writel(dev, enable, DW_IC_ENABLE);
+		__i2c_dw_enable(dev, enable);
 		if ((dw_readl(dev, DW_IC_ENABLE_STATUS) & 1) == enable)
 			return;
 
@@ -282,6 +294,28 @@ static unsigned long i2c_dw_clk_rate(struct dw_i2c_dev *dev)
 	return dev->get_clk_rate_khz(dev);
 }
 
+static int i2c_dw_acquire_lock(struct dw_i2c_dev *dev)
+{
+	int ret;
+
+	if (!dev->acquire_lock)
+		return 0;
+
+	ret = dev->acquire_lock(dev);
+	if (!ret)
+		return 0;
+
+	dev_err(dev->dev, "couldn't acquire bus ownership\n");
+
+	return ret;
+}
+
+static void i2c_dw_release_lock(struct dw_i2c_dev *dev)
+{
+	if (dev->release_lock)
+		dev->release_lock(dev);
+}
+
 /**
  * i2c_dw_init() - initialize the designware i2c master hardware
  * @dev: device private data
@@ -293,17 +327,13 @@ static unsigned long i2c_dw_clk_rate(struct dw_i2c_dev *dev)
 int i2c_dw_init(struct dw_i2c_dev *dev)
 {
 	u32 hcnt, lcnt;
-	u32 reg;
+	u32 reg, comp_param1;
 	u32 sda_falling_time, scl_falling_time;
 	int ret;
 
-	if (dev->acquire_lock) {
-		ret = dev->acquire_lock(dev);
-		if (ret) {
-			dev_err(dev->dev, "couldn't acquire bus ownership\n");
-			return ret;
-		}
-	}
+	ret = i2c_dw_acquire_lock(dev);
+	if (ret)
+		return ret;
 
 	reg = dw_readl(dev, DW_IC_COMP_TYPE);
 	if (reg == ___constant_swab32(DW_IC_COMP_TYPE_VALUE)) {
@@ -315,13 +345,14 @@ int i2c_dw_init(struct dw_i2c_dev *dev)
 	} else if (reg != DW_IC_COMP_TYPE_VALUE) {
 		dev_err(dev->dev, "Unknown Synopsys component type: "
 			"0x%08x\n", reg);
-		if (dev->release_lock)
-			dev->release_lock(dev);
+		i2c_dw_release_lock(dev);
 		return -ENODEV;
 	}
 
+	comp_param1 = dw_readl(dev, DW_IC_COMP_PARAM_1);
+
 	/* Disable the adapter */
-	__i2c_dw_enable(dev, false);
+	__i2c_dw_enable_and_wait(dev, false);
 
 	/* set standard and fast speed deviders for high/low periods */
 
@@ -347,8 +378,11 @@ int i2c_dw_init(struct dw_i2c_dev *dev)
 	dw_writel(dev, lcnt, DW_IC_SS_SCL_LCNT);
 	dev_dbg(dev->dev, "Standard-mode HCNT:LCNT = %d:%d\n", hcnt, lcnt);
 
-	/* Set SCL timing parameters for fast-mode */
-	if (dev->fs_hcnt && dev->fs_lcnt) {
+	/* Set SCL timing parameters for fast-mode or fast-mode plus */
+	if ((dev->clk_freq == 1000000) && dev->fp_hcnt && dev->fp_lcnt) {
+		hcnt = dev->fp_hcnt;
+		lcnt = dev->fp_lcnt;
+	} else if (dev->fs_hcnt && dev->fs_lcnt) {
 		hcnt = dev->fs_hcnt;
 		lcnt = dev->fs_lcnt;
 	} else {
@@ -366,6 +400,23 @@ int i2c_dw_init(struct dw_i2c_dev *dev)
 	dw_writel(dev, lcnt, DW_IC_FS_SCL_LCNT);
 	dev_dbg(dev->dev, "Fast-mode HCNT:LCNT = %d:%d\n", hcnt, lcnt);
 
+	if ((dev->master_cfg & DW_IC_CON_SPEED_MASK) ==
+		DW_IC_CON_SPEED_HIGH) {
+		if ((comp_param1 & DW_IC_COMP_PARAM_1_SPEED_MODE_MASK)
+			!= DW_IC_COMP_PARAM_1_SPEED_MODE_HIGH) {
+			dev_err(dev->dev, "High Speed not supported!\n");
+			dev->master_cfg &= ~DW_IC_CON_SPEED_MASK;
+			dev->master_cfg |= DW_IC_CON_SPEED_FAST;
+		} else if (dev->hs_hcnt && dev->hs_lcnt) {
+			hcnt = dev->hs_hcnt;
+			lcnt = dev->hs_lcnt;
+			dw_writel(dev, hcnt, DW_IC_HS_SCL_HCNT);
+			dw_writel(dev, lcnt, DW_IC_HS_SCL_LCNT);
+			dev_dbg(dev->dev, "HighSpeed-mode HCNT:LCNT = %d:%d\n",
+				hcnt, lcnt);
+		}
+	}
+
 	/* Configure SDA Hold Time if required */
 	reg = dw_readl(dev, DW_IC_COMP_VERSION);
 	if (reg >= DW_IC_SDA_HOLD_MIN_VERS) {
@@ -387,8 +438,8 @@ int i2c_dw_init(struct dw_i2c_dev *dev)
 	/* configure the i2c master */
 	dw_writel(dev, dev->master_cfg , DW_IC_CON);
 
-	if (dev->release_lock)
-		dev->release_lock(dev);
+	i2c_dw_release_lock(dev);
+
 	return 0;
 }
 EXPORT_SYMBOL_GPL(i2c_dw_init);
@@ -415,27 +466,45 @@ static int i2c_dw_wait_bus_not_busy(struct dw_i2c_dev *dev)
 static void i2c_dw_xfer_init(struct dw_i2c_dev *dev)
 {
 	struct i2c_msg *msgs = dev->msgs;
-	u32 ic_con, ic_tar = 0;
+	u32 ic_tar = 0;
+	bool enabled;
 
-	/* Disable the adapter */
-	__i2c_dw_enable(dev, false);
+	enabled = dw_readl(dev, DW_IC_ENABLE_STATUS) & 1;
+
+	if (enabled) {
+		u32 ic_status;
+
+		/*
+		 * Only disable adapter if ic_tar and ic_con can't be
+		 * dynamically updated
+		 */
+		ic_status = dw_readl(dev, DW_IC_STATUS);
+		if (!dev->dynamic_tar_update_enabled ||
+		    (ic_status & DW_IC_STATUS_MST_ACTIVITY) ||
+		    !(ic_status & DW_IC_STATUS_TFE)) {
+			__i2c_dw_enable_and_wait(dev, false);
+			enabled = false;
+		}
+	}
 
 	/* if the slave address is ten bit address, enable 10BITADDR */
-	ic_con = dw_readl(dev, DW_IC_CON);
-	if (msgs[dev->msg_write_idx].flags & I2C_M_TEN) {
-		ic_con |= DW_IC_CON_10BITADDR_MASTER;
+	if (dev->dynamic_tar_update_enabled) {
 		/*
 		 * If I2C_DYNAMIC_TAR_UPDATE is set, the 10-bit addressing
-		 * mode has to be enabled via bit 12 of IC_TAR register.
-		 * We set it always as I2C_DYNAMIC_TAR_UPDATE can't be
-		 * detected from registers.
+		 * mode has to be enabled via bit 12 of IC_TAR register,
+		 * otherwise bit 4 of IC_CON is used.
 		 */
-		ic_tar = DW_IC_TAR_10BITADDR_MASTER;
+		if (msgs[dev->msg_write_idx].flags & I2C_M_TEN)
+			ic_tar = DW_IC_TAR_10BITADDR_MASTER;
 	} else {
-		ic_con &= ~DW_IC_CON_10BITADDR_MASTER;
-	}
+		u32 ic_con = dw_readl(dev, DW_IC_CON);
 
-	dw_writel(dev, ic_con, DW_IC_CON);
+		if (msgs[dev->msg_write_idx].flags & I2C_M_TEN)
+			ic_con |= DW_IC_CON_10BITADDR_MASTER;
+		else
+			ic_con &= ~DW_IC_CON_10BITADDR_MASTER;
+		dw_writel(dev, ic_con, DW_IC_CON);
+	}
 
 	/*
 	 * Set the slave (target) address and enable 10-bit addressing mode
@@ -446,8 +515,8 @@ static void i2c_dw_xfer_init(struct dw_i2c_dev *dev)
 	/* enforce disabled interrupts (due to HW issues) */
 	i2c_dw_disable_int(dev);
 
-	/* Enable the adapter */
-	__i2c_dw_enable(dev, true);
+	if (!enabled)
+		__i2c_dw_enable(dev, true);
 
 	/* Clear and enable interrupts */
 	dw_readl(dev, DW_IC_CLR_INTR);
@@ -628,7 +697,8 @@ static int i2c_dw_handle_tx_abort(struct dw_i2c_dev *dev)
 }
 
 /*
- * Prepare controller for a transaction and call i2c_dw_xfer_msg
+ * Prepare controller for a transaction and start transfer by calling
+ * i2c_dw_xfer_init()
  */
 static int
 i2c_dw_xfer(struct i2c_adapter *adap, struct i2c_msg msgs[], int num)
@@ -651,13 +721,9 @@ i2c_dw_xfer(struct i2c_adapter *adap, struct i2c_msg msgs[], int num)
 	dev->abort_source = 0;
 	dev->rx_outstanding = 0;
 
-	if (dev->acquire_lock) {
-		ret = dev->acquire_lock(dev);
-		if (ret) {
-			dev_err(dev->dev, "couldn't acquire bus ownership\n");
-			goto done_nolock;
-		}
-	}
+	ret = i2c_dw_acquire_lock(dev);
+	if (ret)
+		goto done_nolock;
 
 	ret = i2c_dw_wait_bus_not_busy(dev);
 	if (ret < 0)
@@ -675,16 +741,6 @@ i2c_dw_xfer(struct i2c_adapter *adap, struct i2c_msg msgs[], int num)
 		goto done;
 	}
 
-	/*
-	 * We must disable the adapter before returning and signaling the end
-	 * of the current transfer. Otherwise the hardware might continue
-	 * generating interrupts which in turn causes a race condition with
-	 * the following transfer.  Needs some more investigation if the
-	 * additional interrupts are a hardware bug or this driver doesn't
-	 * handle them correctly yet.
-	 */
-	__i2c_dw_enable(dev, false);
-
 	if (dev->msg_err) {
 		ret = dev->msg_err;
 		goto done;
@@ -704,8 +760,7 @@ i2c_dw_xfer(struct i2c_adapter *adap, struct i2c_msg msgs[], int num)
 	ret = -EIO;
 
 done:
-	if (dev->release_lock)
-		dev->release_lock(dev);
+	i2c_dw_release_lock(dev);
 
 done_nolock:
 	pm_runtime_mark_last_busy(dev->dev);
@@ -822,9 +877,19 @@ static irqreturn_t i2c_dw_isr(int this_irq, void *dev_id)
 	 */
 
 tx_aborted:
-	if ((stat & (DW_IC_INTR_TX_ABRT | DW_IC_INTR_STOP_DET)) || dev->msg_err)
+	if ((stat & (DW_IC_INTR_TX_ABRT | DW_IC_INTR_STOP_DET))
+			|| dev->msg_err) {
+		/*
+		 * We must disable interruts before returning and signaling
+		 * the end of the current transfer. Otherwise the hardware
+		 * might continue generating interrupts for non-existent
+		 * transfers.
+		 */
+		i2c_dw_disable_int(dev);
+		dw_readl(dev, DW_IC_CLR_INTR);
+
 		complete(&dev->cmd_complete);
-	else if (unlikely(dev->accessor_flags & ACCESS_INTR_MASK)) {
+	} else if (unlikely(dev->accessor_flags & ACCESS_INTR_MASK)) {
 		/* workaround to trigger pending interrupt */
 		stat = dw_readl(dev, DW_IC_INTR_MASK);
 		i2c_dw_disable_int(dev);
@@ -837,7 +902,7 @@ tx_aborted:
 void i2c_dw_disable(struct dw_i2c_dev *dev)
 {
 	/* Disable controller */
-	__i2c_dw_enable(dev, false);
+	__i2c_dw_enable_and_wait(dev, false);
 
 	/* Disable all interupts */
 	dw_writel(dev, 0, DW_IC_INTR_MASK);
@@ -861,6 +926,7 @@ int i2c_dw_probe(struct dw_i2c_dev *dev)
 {
 	struct i2c_adapter *adap = &dev->adapter;
 	int r;
+	u32 reg;
 
 	init_completion(&dev->cmd_complete);
 
@@ -868,6 +934,26 @@ int i2c_dw_probe(struct dw_i2c_dev *dev)
 	if (r)
 		return r;
 
+	r = i2c_dw_acquire_lock(dev);
+	if (r)
+		return r;
+
+	/*
+	 * Test if dynamic TAR update is enabled in this controller by writing
+	 * to IC_10BITADDR_MASTER field in IC_CON: when it is enabled this
+	 * field is read-only so it should not succeed
+	 */
+	reg = dw_readl(dev, DW_IC_CON);
+	dw_writel(dev, reg ^ DW_IC_CON_10BITADDR_MASTER, DW_IC_CON);
+
+	if ((dw_readl(dev, DW_IC_CON) & DW_IC_CON_10BITADDR_MASTER) ==
+	    (reg & DW_IC_CON_10BITADDR_MASTER)) {
+		dev->dynamic_tar_update_enabled = true;
+		dev_dbg(dev->dev, "Dynamic TAR update enabled");
+	}
+
+	i2c_dw_release_lock(dev);
+
 	snprintf(adap->name, sizeof(adap->name),
 		 "Synopsys DesignWare I2C adapter");
 	adap->retries = 3;
diff --git a/drivers/i2c/busses/i2c-designware-core.h b/drivers/i2c/busses/i2c-designware-core.h
index 38493a7142ad..0d44d2ae7d4c 100644
--- a/drivers/i2c/busses/i2c-designware-core.h
+++ b/drivers/i2c/busses/i2c-designware-core.h
@@ -26,6 +26,7 @@
 #define DW_IC_CON_MASTER		0x1
 #define DW_IC_CON_SPEED_STD		0x2
 #define DW_IC_CON_SPEED_FAST		0x4
+#define DW_IC_CON_SPEED_HIGH		0x6
 #define DW_IC_CON_SPEED_MASK		0x6
 #define DW_IC_CON_10BITADDR_MASTER	0x10
 #define DW_IC_CON_RESTART_EN		0x20
@@ -57,10 +58,15 @@
  * @tx_fifo_depth: depth of the hardware tx fifo
  * @rx_fifo_depth: depth of the hardware rx fifo
  * @rx_outstanding: current master-rx elements in tx fifo
+ * @clk_freq: bus clock frequency
  * @ss_hcnt: standard speed HCNT value
  * @ss_lcnt: standard speed LCNT value
  * @fs_hcnt: fast speed HCNT value
  * @fs_lcnt: fast speed LCNT value
+ * @fp_hcnt: fast plus HCNT value
+ * @fp_lcnt: fast plus LCNT value
+ * @hs_hcnt: high speed HCNT value
+ * @hs_lcnt: high speed LCNT value
  * @acquire_lock: function to acquire a hardware lock on the bus
  * @release_lock: function to release a hardware lock on the bus
  * @pm_runtime_disabled: true if pm runtime is disabled
@@ -96,6 +102,7 @@ struct dw_i2c_dev {
 	unsigned int		tx_fifo_depth;
 	unsigned int		rx_fifo_depth;
 	int			rx_outstanding;
+	u32			clk_freq;
 	u32			sda_hold_time;
 	u32			sda_falling_time;
 	u32			scl_falling_time;
@@ -103,9 +110,14 @@ struct dw_i2c_dev {
 	u16			ss_lcnt;
 	u16			fs_hcnt;
 	u16			fs_lcnt;
+	u16			fp_hcnt;
+	u16			fp_lcnt;
+	u16			hs_hcnt;
+	u16			hs_lcnt;
 	int			(*acquire_lock)(struct dw_i2c_dev *dev);
 	void			(*release_lock)(struct dw_i2c_dev *dev);
 	bool			pm_runtime_disabled;
+	bool			dynamic_tar_update_enabled;
 };
 
 #define ACCESS_SWAP		0x00000001
diff --git a/drivers/i2c/busses/i2c-designware-platdrv.c b/drivers/i2c/busses/i2c-designware-platdrv.c
index d656657b805c..0b42a12171f3 100644
--- a/drivers/i2c/busses/i2c-designware-platdrv.c
+++ b/drivers/i2c/busses/i2c-designware-platdrv.c
@@ -107,6 +107,8 @@ static int dw_i2c_acpi_configure(struct platform_device *pdev)
 	dw_i2c_acpi_params(pdev, "SSCN", &dev->ss_hcnt, &dev->ss_lcnt, NULL);
 	dw_i2c_acpi_params(pdev, "FMCN", &dev->fs_hcnt, &dev->fs_lcnt,
 			   &dev->sda_hold_time);
+	dw_i2c_acpi_params(pdev, "FPCN", &dev->fp_hcnt, &dev->fp_lcnt, NULL);
+	dw_i2c_acpi_params(pdev, "HSCN", &dev->hs_hcnt, &dev->hs_lcnt, NULL);
 
 	id = acpi_match_device(pdev->dev.driver->acpi_match_table, &pdev->dev);
 	if (id && id->driver_data)
@@ -155,7 +157,7 @@ static int dw_i2c_plat_probe(struct platform_device *pdev)
 	struct i2c_adapter *adap;
 	struct resource *mem;
 	int irq, r;
-	u32 clk_freq, ht = 0;
+	u32 acpi_speed, ht = 0;
 
 	irq = platform_get_irq(pdev, 0);
 	if (irq < 0)
@@ -175,10 +177,10 @@ static int dw_i2c_plat_probe(struct platform_device *pdev)
 	platform_set_drvdata(pdev, dev);
 
 	/* fast mode by default because of legacy reasons */
-	clk_freq = 400000;
+	dev->clk_freq = 400000;
 
 	if (pdata) {
-		clk_freq = pdata->i2c_scl_freq;
+		dev->clk_freq = pdata->i2c_scl_freq;
 	} else {
 		device_property_read_u32(&pdev->dev, "i2c-sda-hold-time-ns",
 					 &ht);
@@ -187,17 +189,24 @@ static int dw_i2c_plat_probe(struct platform_device *pdev)
 		device_property_read_u32(&pdev->dev, "i2c-scl-falling-time-ns",
 					 &dev->scl_falling_time);
 		device_property_read_u32(&pdev->dev, "clock-frequency",
-					 &clk_freq);
+					 &dev->clk_freq);
 	}
 
+	acpi_speed = i2c_acpi_find_bus_speed(&pdev->dev);
+	if (acpi_speed)
+		dev->clk_freq = acpi_speed;
+
 	if (has_acpi_companion(&pdev->dev))
 		dw_i2c_acpi_configure(pdev);
 
 	/*
-	 * Only standard mode at 100kHz and fast mode at 400kHz are supported.
+	 * Only standard mode at 100kHz, fast mode at 400kHz,
+	 * fast mode plus at 1MHz and high speed mode at 3.4MHz are supported.
 	 */
-	if (clk_freq != 100000 && clk_freq != 400000) {
-		dev_err(&pdev->dev, "Only 100kHz and 400kHz supported");
+	if (dev->clk_freq != 100000 && dev->clk_freq != 400000
+	    && dev->clk_freq != 1000000 && dev->clk_freq != 3400000) {
+		dev_err(&pdev->dev,
+			"Only 100kHz, 400kHz, 1MHz and 3.4MHz supported");
 		return -EINVAL;
 	}
 
@@ -212,12 +221,20 @@ static int dw_i2c_plat_probe(struct platform_device *pdev)
 		I2C_FUNC_SMBUS_BYTE_DATA |
 		I2C_FUNC_SMBUS_WORD_DATA |
 		I2C_FUNC_SMBUS_I2C_BLOCK;
-	if (clk_freq == 100000)
-		dev->master_cfg =  DW_IC_CON_MASTER | DW_IC_CON_SLAVE_DISABLE |
-			DW_IC_CON_RESTART_EN | DW_IC_CON_SPEED_STD;
-	else
-		dev->master_cfg =  DW_IC_CON_MASTER | DW_IC_CON_SLAVE_DISABLE |
-			DW_IC_CON_RESTART_EN | DW_IC_CON_SPEED_FAST;
+
+	dev->master_cfg = DW_IC_CON_MASTER | DW_IC_CON_SLAVE_DISABLE |
+			  DW_IC_CON_RESTART_EN;
+
+	switch (dev->clk_freq) {
+	case 100000:
+		dev->master_cfg |= DW_IC_CON_SPEED_STD;
+		break;
+	case 3400000:
+		dev->master_cfg |= DW_IC_CON_SPEED_HIGH;
+		break;
+	default:
+		dev->master_cfg |= DW_IC_CON_SPEED_FAST;
+	}
 
 	dev->clk = devm_clk_get(&pdev->dev, NULL);
 	if (!i2c_dw_plat_prepare_clk(dev, true)) {
diff --git a/drivers/i2c/busses/i2c-diolan-u2c.c b/drivers/i2c/busses/i2c-diolan-u2c.c
index b19a310bf9b3..f718ee4e3332 100644
--- a/drivers/i2c/busses/i2c-diolan-u2c.c
+++ b/drivers/i2c/busses/i2c-diolan-u2c.c
@@ -487,10 +487,8 @@ static int diolan_u2c_probe(struct usb_interface *interface,
 
 	/* and finally attach to i2c layer */
 	ret = i2c_add_adapter(&dev->adapter);
-	if (ret < 0) {
-		dev_err(&interface->dev, "failed to add I2C adapter\n");
+	if (ret < 0)
 		goto error_free;
-	}
 
 	dev_dbg(&interface->dev, "connected " DRIVER_NAME "\n");
 
diff --git a/drivers/i2c/busses/i2c-dln2.c b/drivers/i2c/busses/i2c-dln2.c
index f2eb4f76591f..8acda2aa1558 100644
--- a/drivers/i2c/busses/i2c-dln2.c
+++ b/drivers/i2c/busses/i2c-dln2.c
@@ -228,10 +228,8 @@ static int dln2_i2c_probe(struct platform_device *pdev)
 
 	/* and finally attach to i2c layer */
 	ret = i2c_add_adapter(&dln2->adapter);
-	if (ret < 0) {
-		dev_err(dev, "failed to add I2C adapter: %d\n", ret);
+	if (ret < 0)
 		goto out_disable;
-	}
 
 	return 0;
 
diff --git a/drivers/i2c/busses/i2c-efm32.c b/drivers/i2c/busses/i2c-efm32.c
index e253598d764c..aa336ba89aa3 100644
--- a/drivers/i2c/busses/i2c-efm32.c
+++ b/drivers/i2c/busses/i2c-efm32.c
@@ -438,7 +438,6 @@ static int efm32_i2c_probe(struct platform_device *pdev)
 
 	ret = i2c_add_adapter(&ddata->adapter);
 	if (ret) {
-		dev_err(&pdev->dev, "failed to add i2c adapter (%d)\n", ret);
 		free_irq(ddata->irq, ddata);
 
 err_disable_clk:
diff --git a/drivers/i2c/busses/i2c-exynos5.c b/drivers/i2c/busses/i2c-exynos5.c
index c0e3ada02876..bea607149972 100644
--- a/drivers/i2c/busses/i2c-exynos5.c
+++ b/drivers/i2c/busses/i2c-exynos5.c
@@ -796,10 +796,8 @@ static int exynos5_i2c_probe(struct platform_device *pdev)
 	exynos5_i2c_reset(i2c);
 
 	ret = i2c_add_adapter(&i2c->adap);
-	if (ret < 0) {
-		dev_err(&pdev->dev, "failed to add bus to i2c core\n");
+	if (ret < 0)
 		goto err_clk;
-	}
 
 	platform_set_drvdata(pdev, i2c);
 
diff --git a/drivers/i2c/busses/i2c-hix5hd2.c b/drivers/i2c/busses/i2c-hix5hd2.c
index 7c6966434ee7..ae7f3180f7e8 100644
--- a/drivers/i2c/busses/i2c-hix5hd2.c
+++ b/drivers/i2c/busses/i2c-hix5hd2.c
@@ -478,10 +478,8 @@ static int hix5hd2_i2c_probe(struct platform_device *pdev)
 	pm_runtime_enable(priv->dev);
 
 	ret = i2c_add_adapter(&priv->adap);
-	if (ret < 0) {
-		dev_err(&pdev->dev, "failed to add bus to i2c core\n");
+	if (ret < 0)
 		goto err_runtime;
-	}
 
 	return ret;
 
diff --git a/drivers/i2c/busses/i2c-i801.c b/drivers/i2c/busses/i2c-i801.c
index 5ef9b733d153..08847e8b8998 100644
--- a/drivers/i2c/busses/i2c-i801.c
+++ b/drivers/i2c/busses/i2c-i801.c
@@ -64,6 +64,7 @@
  * Broxton (SOC)		0x5ad4	32	hard	yes	yes	yes
  * Lewisburg (PCH)		0xa1a3	32	hard	yes	yes	yes
  * Lewisburg Supersku (PCH)	0xa223	32	hard	yes	yes	yes
+ * Kaby Lake PCH-H (PCH)	0xa2a3	32	hard	yes	yes	yes
  *
  * Features supported by this driver:
  * Software PEC				no
@@ -226,6 +227,7 @@
 #define PCI_DEVICE_ID_INTEL_SUNRISEPOINT_H_SMBUS	0xa123
 #define PCI_DEVICE_ID_INTEL_LEWISBURG_SMBUS		0xa1a3
 #define PCI_DEVICE_ID_INTEL_LEWISBURG_SSKU_SMBUS	0xa223
+#define PCI_DEVICE_ID_INTEL_KABYLAKE_PCH_H_SMBUS	0xa2a3
 
 struct i801_mux_config {
 	char *gpio_chip;
@@ -1006,6 +1008,7 @@ static const struct pci_device_id i801_ids[] = {
 	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_BROXTON_SMBUS) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_LEWISBURG_SMBUS) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_LEWISBURG_SSKU_SMBUS) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_KABYLAKE_PCH_H_SMBUS) },
 	{ 0, }
 };
 
@@ -1482,11 +1485,14 @@ static int i801_probe(struct pci_dev *dev, const struct pci_device_id *id)
 	case PCI_DEVICE_ID_INTEL_LEWISBURG_SMBUS:
 	case PCI_DEVICE_ID_INTEL_LEWISBURG_SSKU_SMBUS:
 	case PCI_DEVICE_ID_INTEL_DNV_SMBUS:
+	case PCI_DEVICE_ID_INTEL_KABYLAKE_PCH_H_SMBUS:
 		priv->features |= FEATURE_I2C_BLOCK_READ;
 		priv->features |= FEATURE_IRQ;
 		priv->features |= FEATURE_SMBUS_PEC;
 		priv->features |= FEATURE_BLOCK_BUFFER;
-		priv->features |= FEATURE_TCO;
+		/* If we have ACPI based watchdog use that instead */
+		if (!acpi_has_watchdog())
+			priv->features |= FEATURE_TCO;
 		priv->features |= FEATURE_HOST_NOTIFY;
 		break;
 
@@ -1613,7 +1619,6 @@ static int i801_probe(struct pci_dev *dev, const struct pci_device_id *id)
 		"SMBus I801 adapter at %04lx", priv->smba);
 	err = i2c_add_adapter(&priv->adapter);
 	if (err) {
-		dev_err(&dev->dev, "Failed to add SMBus adapter\n");
 		i801_acpi_remove(priv);
 		return err;
 	}
diff --git a/drivers/i2c/busses/i2c-ibm_iic.c b/drivers/i2c/busses/i2c-ibm_iic.c
index cdaa7be2cd1b..412b91d255ad 100644
--- a/drivers/i2c/busses/i2c-ibm_iic.c
+++ b/drivers/i2c/busses/i2c-ibm_iic.c
@@ -751,10 +751,8 @@ static int iic_probe(struct platform_device *ofdev)
 	adap->timeout = HZ;
 
 	ret = i2c_add_adapter(adap);
-	if (ret  < 0) {
-		dev_err(&ofdev->dev, "failed to register i2c adapter\n");
+	if (ret  < 0)
 		goto error_cleanup;
-	}
 
 	dev_info(&ofdev->dev, "using %s mode\n",
 		 dev->fast_mode ? "fast (400 kHz)" : "standard (100 kHz)");
diff --git a/drivers/i2c/busses/i2c-img-scb.c b/drivers/i2c/busses/i2c-img-scb.c
index ea20425b6972..db8e8b40569d 100644
--- a/drivers/i2c/busses/i2c-img-scb.c
+++ b/drivers/i2c/busses/i2c-img-scb.c
@@ -1394,10 +1394,8 @@ static int img_i2c_probe(struct platform_device *pdev)
 		goto disable_clk;
 
 	ret = i2c_add_numbered_adapter(&i2c->adap);
-	if (ret < 0) {
-		dev_err(&pdev->dev, "failed to add adapter\n");
+	if (ret < 0)
 		goto disable_clk;
-	}
 
 	return 0;
 
diff --git a/drivers/i2c/busses/i2c-imx.c b/drivers/i2c/busses/i2c-imx.c
index 1844bc9f7cd5..592a8f26a708 100644
--- a/drivers/i2c/busses/i2c-imx.c
+++ b/drivers/i2c/busses/i2c-imx.c
@@ -984,11 +984,24 @@ static void i2c_imx_unprepare_recovery(struct i2c_adapter *adap)
 	pinctrl_select_state(i2c_imx->pinctrl, i2c_imx->pinctrl_pins_default);
 }
 
-static void i2c_imx_init_recovery_info(struct imx_i2c_struct *i2c_imx,
+/*
+ * We switch SCL and SDA to their GPIO function and do some bitbanging
+ * for bus recovery. These alternative pinmux settings can be
+ * described in the device tree by a separate pinctrl state "gpio". If
+ * this is missing this is not a big problem, the only implication is
+ * that we can't do bus recovery.
+ */
+static int i2c_imx_init_recovery_info(struct imx_i2c_struct *i2c_imx,
 		struct platform_device *pdev)
 {
 	struct i2c_bus_recovery_info *rinfo = &i2c_imx->rinfo;
 
+	i2c_imx->pinctrl = devm_pinctrl_get(&pdev->dev);
+	if (!i2c_imx->pinctrl || IS_ERR(i2c_imx->pinctrl)) {
+		dev_info(&pdev->dev, "can't get pinctrl, bus recovery not supported\n");
+		return PTR_ERR(i2c_imx->pinctrl);
+	}
+
 	i2c_imx->pinctrl_pins_default = pinctrl_lookup_state(i2c_imx->pinctrl,
 			PINCTRL_STATE_DEFAULT);
 	i2c_imx->pinctrl_pins_gpio = pinctrl_lookup_state(i2c_imx->pinctrl,
@@ -1001,7 +1014,7 @@ static void i2c_imx_init_recovery_info(struct imx_i2c_struct *i2c_imx,
 	    IS_ERR(i2c_imx->pinctrl_pins_default) ||
 	    IS_ERR(i2c_imx->pinctrl_pins_gpio)) {
 		dev_dbg(&pdev->dev, "recovery information incomplete\n");
-		return;
+		return 0;
 	}
 
 	dev_dbg(&pdev->dev, "using scl-gpio %d and sda-gpio %d for recovery\n",
@@ -1011,6 +1024,8 @@ static void i2c_imx_init_recovery_info(struct imx_i2c_struct *i2c_imx,
 	rinfo->unprepare_recovery = i2c_imx_unprepare_recovery;
 	rinfo->recover_bus = i2c_generic_gpio_recovery;
 	i2c_imx->adapter.bus_recovery_info = rinfo;
+
+	return 0;
 }
 
 static u32 i2c_imx_func(struct i2c_adapter *adapter)
@@ -1081,12 +1096,6 @@ static int i2c_imx_probe(struct platform_device *pdev)
 		return ret;
 	}
 
-	i2c_imx->pinctrl = devm_pinctrl_get(&pdev->dev);
-	if (IS_ERR(i2c_imx->pinctrl)) {
-		ret = PTR_ERR(i2c_imx->pinctrl);
-		goto clk_disable;
-	}
-
 	/* Request IRQ */
 	ret = devm_request_irq(&pdev->dev, irq, i2c_imx_isr, 0,
 				pdev->name, i2c_imx);
@@ -1125,14 +1134,16 @@ static int i2c_imx_probe(struct platform_device *pdev)
 			i2c_imx, IMX_I2C_I2CR);
 	imx_i2c_write_reg(i2c_imx->hwdata->i2sr_clr_opcode, i2c_imx, IMX_I2C_I2SR);
 
-	i2c_imx_init_recovery_info(i2c_imx, pdev);
+	/* Init optional bus recovery function */
+	ret = i2c_imx_init_recovery_info(i2c_imx, pdev);
+	/* Give it another chance if pinctrl used is not ready yet */
+	if (ret == -EPROBE_DEFER)
+		goto rpm_disable;
 
 	/* Add I2C adapter */
 	ret = i2c_add_numbered_adapter(&i2c_imx->adapter);
-	if (ret < 0) {
-		dev_err(&pdev->dev, "registration failed\n");
+	if (ret < 0)
 		goto rpm_disable;
-	}
 
 	pm_runtime_mark_last_busy(&pdev->dev);
 	pm_runtime_put_autosuspend(&pdev->dev);
diff --git a/drivers/i2c/busses/i2c-isch.c b/drivers/i2c/busses/i2c-isch.c
index c2f25f19d76f..0cf1379f4e80 100644
--- a/drivers/i2c/busses/i2c-isch.c
+++ b/drivers/i2c/busses/i2c-isch.c
@@ -288,10 +288,8 @@ static int smbus_sch_probe(struct platform_device *dev)
 		"SMBus SCH adapter at %04x", sch_smba);
 
 	retval = i2c_add_adapter(&sch_adapter);
-	if (retval) {
-		dev_err(&dev->dev, "Couldn't register adapter!\n");
+	if (retval)
 		sch_smba = 0;
-	}
 
 	return retval;
 }
diff --git a/drivers/i2c/busses/i2c-ismt.c b/drivers/i2c/busses/i2c-ismt.c
index 1c8707710098..f573448d2132 100644
--- a/drivers/i2c/busses/i2c-ismt.c
+++ b/drivers/i2c/busses/i2c-ismt.c
@@ -922,10 +922,8 @@ ismt_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 		return err;
 
 	err = i2c_add_adapter(&priv->adapter);
-	if (err) {
-		dev_err(&pdev->dev, "Failed to add SMBus iSMT adapter\n");
+	if (err)
 		return -ENODEV;
-	}
 	return 0;
 }
 
diff --git a/drivers/i2c/busses/i2c-jz4780.c b/drivers/i2c/busses/i2c-jz4780.c
index cd9872594fe2..b8ea62105f42 100644
--- a/drivers/i2c/busses/i2c-jz4780.c
+++ b/drivers/i2c/busses/i2c-jz4780.c
@@ -798,10 +798,8 @@ static int jz4780_i2c_probe(struct platform_device *pdev)
 		goto err;
 
 	ret = i2c_add_adapter(&i2c->adap);
-	if (ret < 0) {
-		dev_err(&pdev->dev, "Failed to add bus\n");
+	if (ret < 0)
 		goto err;
-	}
 
 	return 0;
 
diff --git a/drivers/i2c/busses/i2c-lpc2k.c b/drivers/i2c/busses/i2c-lpc2k.c
index 586a15205e61..9b1fef455a89 100644
--- a/drivers/i2c/busses/i2c-lpc2k.c
+++ b/drivers/i2c/busses/i2c-lpc2k.c
@@ -432,10 +432,8 @@ static int i2c_lpc2k_probe(struct platform_device *pdev)
 	i2c->adap.dev.of_node = pdev->dev.of_node;
 
 	ret = i2c_add_adapter(&i2c->adap);
-	if (ret < 0) {
-		dev_err(&pdev->dev, "failed to add adapter!\n");
+	if (ret < 0)
 		goto fail_clk;
-	}
 
 	dev_info(&pdev->dev, "LPC2K I2C adapter\n");
 
diff --git a/drivers/i2c/busses/i2c-meson.c b/drivers/i2c/busses/i2c-meson.c
index 76e28980904f..2aa61bbbd307 100644
--- a/drivers/i2c/busses/i2c-meson.c
+++ b/drivers/i2c/busses/i2c-meson.c
@@ -453,7 +453,6 @@ static int meson_i2c_probe(struct platform_device *pdev)
 
 	ret = i2c_add_adapter(&i2c->adap);
 	if (ret < 0) {
-		dev_err(&pdev->dev, "can't register adapter\n");
 		clk_unprepare(i2c->clk);
 		return ret;
 	}
@@ -473,6 +472,7 @@ static int meson_i2c_remove(struct platform_device *pdev)
 
 static const struct of_device_id meson_i2c_match[] = {
 	{ .compatible = "amlogic,meson6-i2c" },
+	{ .compatible = "amlogic,meson-gxbb-i2c" },
 	{ },
 };
 MODULE_DEVICE_TABLE(of, meson_i2c_match);
diff --git a/drivers/i2c/busses/i2c-mpc.c b/drivers/i2c/busses/i2c-mpc.c
index 48ecffecc0ed..565a49a0c564 100644
--- a/drivers/i2c/busses/i2c-mpc.c
+++ b/drivers/i2c/busses/i2c-mpc.c
@@ -737,10 +737,8 @@ static int fsl_i2c_probe(struct platform_device *op)
 	i2c->adap.dev.of_node = of_node_get(op->dev.of_node);
 
 	result = i2c_add_adapter(&i2c->adap);
-	if (result < 0) {
-		dev_err(i2c->dev, "failed to add adapter\n");
+	if (result < 0)
 		goto fail_add;
-	}
 
 	return result;
 
diff --git a/drivers/i2c/busses/i2c-mt65xx.c b/drivers/i2c/busses/i2c-mt65xx.c
index d9373e60be8a..4a7d9bc2142b 100644
--- a/drivers/i2c/busses/i2c-mt65xx.c
+++ b/drivers/i2c/busses/i2c-mt65xx.c
@@ -786,10 +786,8 @@ static int mtk_i2c_probe(struct platform_device *pdev)
 
 	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");
+	if (ret)
 		return ret;
-	}
 
 	platform_set_drvdata(pdev, i2c);
 
diff --git a/drivers/i2c/busses/i2c-mxs.c b/drivers/i2c/busses/i2c-mxs.c
index 033846cdf266..5738556b6aac 100644
--- a/drivers/i2c/busses/i2c-mxs.c
+++ b/drivers/i2c/busses/i2c-mxs.c
@@ -868,7 +868,6 @@ static int mxs_i2c_probe(struct platform_device *pdev)
 	i2c_set_adapdata(adap, i2c);
 	err = i2c_add_numbered_adapter(adap);
 	if (err) {
-		dev_err(dev, "Failed to add adapter (%d)\n", err);
 		writel(MXS_I2C_CTRL0_SFTRST,
 				i2c->regs + MXS_I2C_CTRL0_SET);
 		return err;
diff --git a/drivers/i2c/busses/i2c-nforce2.c b/drivers/i2c/busses/i2c-nforce2.c
index 42fcc9458432..374b35e7e450 100644
--- a/drivers/i2c/busses/i2c-nforce2.c
+++ b/drivers/i2c/busses/i2c-nforce2.c
@@ -366,7 +366,6 @@ static int nforce2_probe_smb(struct pci_dev *dev, int bar, int alt_reg,
 
 	error = i2c_add_adapter(&smbus->adapter);
 	if (error) {
-		dev_err(&smbus->adapter.dev, "Failed to register adapter.\n");
 		release_region(smbus->base, smbus->size);
 		return error;
 	}
diff --git a/drivers/i2c/busses/i2c-nomadik.c b/drivers/i2c/busses/i2c-nomadik.c
index bcd17e8cbcb4..da6609d62848 100644
--- a/drivers/i2c/busses/i2c-nomadik.c
+++ b/drivers/i2c/busses/i2c-nomadik.c
@@ -1046,10 +1046,8 @@ static int nmk_i2c_probe(struct amba_device *adev, const struct amba_id *id)
 		 adap->name, dev->virtbase);
 
 	ret = i2c_add_adapter(adap);
-	if (ret) {
-		dev_err(&adev->dev, "failed to add adapter\n");
+	if (ret)
 		goto err_no_adap;
-	}
 
 	pm_runtime_put(&adev->dev);
 
diff --git a/drivers/i2c/busses/i2c-ocores.c b/drivers/i2c/busses/i2c-ocores.c
index ac88a524143e..34f1889a4073 100644
--- a/drivers/i2c/busses/i2c-ocores.c
+++ b/drivers/i2c/busses/i2c-ocores.c
@@ -494,10 +494,8 @@ static int ocores_i2c_probe(struct platform_device *pdev)
 
 	/* add i2c adapter to i2c tree */
 	ret = i2c_add_adapter(&i2c->adap);
-	if (ret) {
-		dev_err(&pdev->dev, "Failed to add adapter\n");
+	if (ret)
 		goto err_clk;
-	}
 
 	/* add in known devices to the bus */
 	if (pdata) {
diff --git a/drivers/i2c/busses/i2c-octeon.c b/drivers/i2c/busses/i2c-octeon-core.c
index 30ae35146723..419b54bfc7c7 100644
--- a/drivers/i2c/busses/i2c-octeon.c
+++ b/drivers/i2c/busses/i2c-octeon-core.c
@@ -4,280 +4,127 @@
  *
  * Portions Copyright (C) 2010 - 2016 Cavium, Inc.
  *
- * This is a driver for the i2c adapter in Cavium Networks' OCTEON processors.
+ * This file contains the shared part of the driver for the i2c adapter in
+ * Cavium Networks' OCTEON processors and ThunderX SOCs.
  *
  * 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/atomic.h>
-#include <linux/platform_device.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
 #include <linux/interrupt.h>
 #include <linux/kernel.h>
 #include <linux/module.h>
-#include <linux/delay.h>
-#include <linux/sched.h>
-#include <linux/slab.h>
-#include <linux/i2c.h>
-#include <linux/io.h>
-#include <linux/of.h>
-
-#include <asm/octeon/octeon.h>
-
-#define DRV_NAME "i2c-octeon"
-
-/* Register offsets */
-#define SW_TWSI			0x00
-#define TWSI_INT		0x10
-#define SW_TWSI_EXT		0x18
-
-/* Controller command patterns */
-#define SW_TWSI_V		BIT_ULL(63)	/* Valid bit */
-#define SW_TWSI_EIA		BIT_ULL(61)	/* Extended internal address */
-#define SW_TWSI_R		BIT_ULL(56)	/* Result or read bit */
-#define SW_TWSI_SOVR		BIT_ULL(55)	/* Size override */
-#define SW_TWSI_SIZE_SHIFT	52
-#define SW_TWSI_ADDR_SHIFT	40
-#define SW_TWSI_IA_SHIFT	32		/* Internal address */
-
-/* Controller opcode word (bits 60:57) */
-#define SW_TWSI_OP_SHIFT	57
-#define SW_TWSI_OP_7		(0ULL << SW_TWSI_OP_SHIFT)
-#define SW_TWSI_OP_7_IA		(1ULL << SW_TWSI_OP_SHIFT)
-#define SW_TWSI_OP_10		(2ULL << SW_TWSI_OP_SHIFT)
-#define SW_TWSI_OP_10_IA	(3ULL << SW_TWSI_OP_SHIFT)
-#define SW_TWSI_OP_TWSI_CLK	(4ULL << SW_TWSI_OP_SHIFT)
-#define SW_TWSI_OP_EOP		(6ULL << SW_TWSI_OP_SHIFT) /* Extended opcode */
-
-/* Controller extended opcode word (bits 34:32) */
-#define SW_TWSI_EOP_SHIFT	32
-#define SW_TWSI_EOP_TWSI_DATA	(SW_TWSI_OP_EOP | 1ULL << SW_TWSI_EOP_SHIFT)
-#define SW_TWSI_EOP_TWSI_CTL	(SW_TWSI_OP_EOP | 2ULL << SW_TWSI_EOP_SHIFT)
-#define SW_TWSI_EOP_TWSI_CLKCTL	(SW_TWSI_OP_EOP | 3ULL << SW_TWSI_EOP_SHIFT)
-#define SW_TWSI_EOP_TWSI_STAT	(SW_TWSI_OP_EOP | 3ULL << SW_TWSI_EOP_SHIFT)
-#define SW_TWSI_EOP_TWSI_RST	(SW_TWSI_OP_EOP | 7ULL << SW_TWSI_EOP_SHIFT)
-
-/* Controller command and status bits */
-#define TWSI_CTL_CE		0x80	/* High level controller enable */
-#define TWSI_CTL_ENAB		0x40	/* Bus enable */
-#define TWSI_CTL_STA		0x20	/* Master-mode start, HW clears when done */
-#define TWSI_CTL_STP		0x10	/* Master-mode stop, HW clears when done */
-#define TWSI_CTL_IFLG		0x08	/* HW event, SW writes 0 to ACK */
-#define TWSI_CTL_AAK		0x04	/* Assert ACK */
-
-/* Status values */
-#define STAT_ERROR		0x00
-#define STAT_START		0x08
-#define STAT_REP_START		0x10
-#define STAT_TXADDR_ACK		0x18
-#define STAT_TXADDR_NAK		0x20
-#define STAT_TXDATA_ACK		0x28
-#define STAT_TXDATA_NAK		0x30
-#define STAT_LOST_ARB_38	0x38
-#define STAT_RXADDR_ACK		0x40
-#define STAT_RXADDR_NAK		0x48
-#define STAT_RXDATA_ACK		0x50
-#define STAT_RXDATA_NAK		0x58
-#define STAT_SLAVE_60		0x60
-#define STAT_LOST_ARB_68	0x68
-#define STAT_SLAVE_70		0x70
-#define STAT_LOST_ARB_78	0x78
-#define STAT_SLAVE_80		0x80
-#define STAT_SLAVE_88		0x88
-#define STAT_GENDATA_ACK	0x90
-#define STAT_GENDATA_NAK	0x98
-#define STAT_SLAVE_A0		0xA0
-#define STAT_SLAVE_A8		0xA8
-#define STAT_LOST_ARB_B0	0xB0
-#define STAT_SLAVE_LOST		0xB8
-#define STAT_SLAVE_NAK		0xC0
-#define STAT_SLAVE_ACK		0xC8
-#define STAT_AD2W_ACK		0xD0
-#define STAT_AD2W_NAK		0xD8
-#define STAT_IDLE		0xF8
-
-/* TWSI_INT values */
-#define TWSI_INT_ST_INT		BIT_ULL(0)
-#define TWSI_INT_TS_INT		BIT_ULL(1)
-#define TWSI_INT_CORE_INT	BIT_ULL(2)
-#define TWSI_INT_ST_EN		BIT_ULL(4)
-#define TWSI_INT_TS_EN		BIT_ULL(5)
-#define TWSI_INT_CORE_EN	BIT_ULL(6)
-#define TWSI_INT_SDA_OVR	BIT_ULL(8)
-#define TWSI_INT_SCL_OVR	BIT_ULL(9)
-#define TWSI_INT_SDA		BIT_ULL(10)
-#define TWSI_INT_SCL		BIT_ULL(11)
-
-#define I2C_OCTEON_EVENT_WAIT 80 /* microseconds */
-
-struct octeon_i2c {
-	wait_queue_head_t queue;
-	struct i2c_adapter adap;
-	int irq;
-	int hlc_irq;		/* For cn7890 only */
-	u32 twsi_freq;
-	int sys_freq;
-	void __iomem *twsi_base;
-	struct device *dev;
-	bool hlc_enabled;
-	bool broken_irq_mode;
-	bool broken_irq_check;
-	void (*int_enable)(struct octeon_i2c *);
-	void (*int_disable)(struct octeon_i2c *);
-	void (*hlc_int_enable)(struct octeon_i2c *);
-	void (*hlc_int_disable)(struct octeon_i2c *);
-	atomic_t int_enable_cnt;
-	atomic_t hlc_int_enable_cnt;
-};
-
-static void octeon_i2c_writeq_flush(u64 val, void __iomem *addr)
-{
-	__raw_writeq(val, addr);
-	__raw_readq(addr);	/* wait for write to land */
-}
-
-/**
- * octeon_i2c_reg_write - write an I2C core register
- * @i2c: The struct octeon_i2c
- * @eop_reg: Register selector
- * @data: Value to be written
- *
- * The I2C core registers are accessed indirectly via the SW_TWSI CSR.
- */
-static void octeon_i2c_reg_write(struct octeon_i2c *i2c, u64 eop_reg, u8 data)
-{
-	u64 tmp;
 
-	__raw_writeq(SW_TWSI_V | eop_reg | data, i2c->twsi_base + SW_TWSI);
-	do {
-		tmp = __raw_readq(i2c->twsi_base + SW_TWSI);
-	} while ((tmp & SW_TWSI_V) != 0);
-}
-
-#define octeon_i2c_ctl_write(i2c, val)					\
-	octeon_i2c_reg_write(i2c, SW_TWSI_EOP_TWSI_CTL, val)
-#define octeon_i2c_data_write(i2c, val)					\
-	octeon_i2c_reg_write(i2c, SW_TWSI_EOP_TWSI_DATA, val)
+#include "i2c-octeon-core.h"
 
-/**
- * octeon_i2c_reg_read - read lower bits of an I2C core register
- * @i2c: The struct octeon_i2c
- * @eop_reg: Register selector
- *
- * Returns the data.
- *
- * The I2C core registers are accessed indirectly via the SW_TWSI CSR.
- */
-static u8 octeon_i2c_reg_read(struct octeon_i2c *i2c, u64 eop_reg)
+/* interrupt service routine */
+irqreturn_t octeon_i2c_isr(int irq, void *dev_id)
 {
-	u64 tmp;
+	struct octeon_i2c *i2c = dev_id;
 
-	__raw_writeq(SW_TWSI_V | eop_reg | SW_TWSI_R, i2c->twsi_base + SW_TWSI);
-	do {
-		tmp = __raw_readq(i2c->twsi_base + SW_TWSI);
-	} while ((tmp & SW_TWSI_V) != 0);
+	i2c->int_disable(i2c);
+	wake_up(&i2c->queue);
 
-	return tmp & 0xFF;
+	return IRQ_HANDLED;
 }
 
-#define octeon_i2c_ctl_read(i2c)					\
-	octeon_i2c_reg_read(i2c, SW_TWSI_EOP_TWSI_CTL)
-#define octeon_i2c_data_read(i2c)					\
-	octeon_i2c_reg_read(i2c, SW_TWSI_EOP_TWSI_DATA)
-#define octeon_i2c_stat_read(i2c)					\
-	octeon_i2c_reg_read(i2c, SW_TWSI_EOP_TWSI_STAT)
-
-/**
- * octeon_i2c_read_int - read the TWSI_INT register
- * @i2c: The struct octeon_i2c
- *
- * Returns the value of the register.
- */
-static u64 octeon_i2c_read_int(struct octeon_i2c *i2c)
+static bool octeon_i2c_test_iflg(struct octeon_i2c *i2c)
 {
-	return __raw_readq(i2c->twsi_base + TWSI_INT);
+	return (octeon_i2c_ctl_read(i2c) & TWSI_CTL_IFLG);
 }
 
-/**
- * octeon_i2c_write_int - write the TWSI_INT register
- * @i2c: The struct octeon_i2c
- * @data: Value to be written
- */
-static void octeon_i2c_write_int(struct octeon_i2c *i2c, u64 data)
+static bool octeon_i2c_test_ready(struct octeon_i2c *i2c, bool *first)
 {
-	octeon_i2c_writeq_flush(data, i2c->twsi_base + TWSI_INT);
-}
+	if (octeon_i2c_test_iflg(i2c))
+		return true;
 
-/**
- * octeon_i2c_int_enable - enable the CORE interrupt
- * @i2c: The struct octeon_i2c
- *
- * The interrupt will be asserted when there is non-STAT_IDLE state in
- * the SW_TWSI_EOP_TWSI_STAT register.
- */
-static void octeon_i2c_int_enable(struct octeon_i2c *i2c)
-{
-	octeon_i2c_write_int(i2c, TWSI_INT_CORE_EN);
-}
+	if (*first) {
+		*first = false;
+		return false;
+	}
 
-/* disable the CORE interrupt */
-static void octeon_i2c_int_disable(struct octeon_i2c *i2c)
-{
-	/* clear TS/ST/IFLG events */
-	octeon_i2c_write_int(i2c, 0);
+	/*
+	 * IRQ has signaled an event but IFLG hasn't changed.
+	 * Sleep and retry once.
+	 */
+	usleep_range(I2C_OCTEON_EVENT_WAIT, 2 * I2C_OCTEON_EVENT_WAIT);
+	return octeon_i2c_test_iflg(i2c);
 }
 
 /**
- * octeon_i2c_int_enable78 - enable the CORE interrupt
+ * octeon_i2c_wait - wait for the IFLG to be set
  * @i2c: The struct octeon_i2c
  *
- * The interrupt will be asserted when there is non-STAT_IDLE state in the
- * SW_TWSI_EOP_TWSI_STAT register.
+ * Returns 0 on success, otherwise a negative errno.
  */
-static void octeon_i2c_int_enable78(struct octeon_i2c *i2c)
-{
-	atomic_inc_return(&i2c->int_enable_cnt);
-	enable_irq(i2c->irq);
-}
-
-static void __octeon_i2c_irq_disable(atomic_t *cnt, int irq)
+static int octeon_i2c_wait(struct octeon_i2c *i2c)
 {
-	int count;
+	long time_left;
+	bool first = true;
 
 	/*
-	 * The interrupt can be disabled in two places, but we only
-	 * want to make the disable_irq_nosync() call once, so keep
-	 * track with the atomic variable.
+	 * Some chip revisions don't assert the irq in the interrupt
+	 * controller. So we must poll for the IFLG change.
 	 */
-	count = atomic_dec_if_positive(cnt);
-	if (count >= 0)
-		disable_irq_nosync(irq);
+	if (i2c->broken_irq_mode) {
+		u64 end = get_jiffies_64() + i2c->adap.timeout;
+
+		while (!octeon_i2c_test_iflg(i2c) &&
+		       time_before64(get_jiffies_64(), end))
+			usleep_range(I2C_OCTEON_EVENT_WAIT / 2, I2C_OCTEON_EVENT_WAIT);
+
+		return octeon_i2c_test_iflg(i2c) ? 0 : -ETIMEDOUT;
+	}
+
+	i2c->int_enable(i2c);
+	time_left = wait_event_timeout(i2c->queue, octeon_i2c_test_ready(i2c, &first),
+				       i2c->adap.timeout);
+	i2c->int_disable(i2c);
+
+	if (i2c->broken_irq_check && !time_left &&
+	    octeon_i2c_test_iflg(i2c)) {
+		dev_err(i2c->dev, "broken irq connection detected, switching to polling mode.\n");
+		i2c->broken_irq_mode = true;
+		return 0;
+	}
+
+	if (!time_left)
+		return -ETIMEDOUT;
+
+	return 0;
 }
 
-/* disable the CORE interrupt */
-static void octeon_i2c_int_disable78(struct octeon_i2c *i2c)
+static bool octeon_i2c_hlc_test_valid(struct octeon_i2c *i2c)
 {
-	__octeon_i2c_irq_disable(&i2c->int_enable_cnt, i2c->irq);
+	return (__raw_readq(i2c->twsi_base + SW_TWSI(i2c)) & SW_TWSI_V) == 0;
 }
 
-/**
- * octeon_i2c_hlc_int_enable78 - enable the ST interrupt
- * @i2c: The struct octeon_i2c
- *
- * The interrupt will be asserted when there is non-STAT_IDLE state in
- * the SW_TWSI_EOP_TWSI_STAT register.
- */
-static void octeon_i2c_hlc_int_enable78(struct octeon_i2c *i2c)
+static bool octeon_i2c_hlc_test_ready(struct octeon_i2c *i2c, bool *first)
 {
-	atomic_inc_return(&i2c->hlc_int_enable_cnt);
-	enable_irq(i2c->hlc_irq);
+	/* check if valid bit is cleared */
+	if (octeon_i2c_hlc_test_valid(i2c))
+		return true;
+
+	if (*first) {
+		*first = false;
+		return false;
+	}
+
+	/*
+	 * IRQ has signaled an event but valid bit isn't cleared.
+	 * Sleep and retry once.
+	 */
+	usleep_range(I2C_OCTEON_EVENT_WAIT, 2 * I2C_OCTEON_EVENT_WAIT);
+	return octeon_i2c_hlc_test_valid(i2c);
 }
 
-/* disable the ST interrupt */
-static void octeon_i2c_hlc_int_disable78(struct octeon_i2c *i2c)
+static void octeon_i2c_hlc_int_clear(struct octeon_i2c *i2c)
 {
-	__octeon_i2c_irq_disable(&i2c->hlc_int_enable_cnt, i2c->hlc_irq);
+	/* clear ST/TS events, listen for neither */
+	octeon_i2c_write_int(i2c, TWSI_INT_ST_INT | TWSI_INT_TS_INT);
 }
 
 /*
@@ -321,83 +168,41 @@ static void octeon_i2c_hlc_disable(struct octeon_i2c *i2c)
 	octeon_i2c_ctl_write(i2c, TWSI_CTL_ENAB);
 }
 
-/* interrupt service routine */
-static irqreturn_t octeon_i2c_isr(int irq, void *dev_id)
-{
-	struct octeon_i2c *i2c = dev_id;
-
-	i2c->int_disable(i2c);
-	wake_up(&i2c->queue);
-
-	return IRQ_HANDLED;
-}
-
-/* HLC interrupt service routine */
-static irqreturn_t octeon_i2c_hlc_isr78(int irq, void *dev_id)
-{
-	struct octeon_i2c *i2c = dev_id;
-
-	i2c->hlc_int_disable(i2c);
-	wake_up(&i2c->queue);
-
-	return IRQ_HANDLED;
-}
-
-static bool octeon_i2c_test_iflg(struct octeon_i2c *i2c)
-{
-	return (octeon_i2c_ctl_read(i2c) & TWSI_CTL_IFLG);
-}
-
-static bool octeon_i2c_test_ready(struct octeon_i2c *i2c, bool *first)
-{
-	if (octeon_i2c_test_iflg(i2c))
-		return true;
-
-	if (*first) {
-		*first = false;
-		return false;
-	}
-
-	/*
-	 * IRQ has signaled an event but IFLG hasn't changed.
-	 * Sleep and retry once.
-	 */
-	usleep_range(I2C_OCTEON_EVENT_WAIT, 2 * I2C_OCTEON_EVENT_WAIT);
-	return octeon_i2c_test_iflg(i2c);
-}
-
 /**
- * octeon_i2c_wait - wait for the IFLG to be set
+ * octeon_i2c_hlc_wait - wait for an HLC operation to complete
  * @i2c: The struct octeon_i2c
  *
- * Returns 0 on success, otherwise a negative errno.
+ * Returns 0 on success, otherwise -ETIMEDOUT.
  */
-static int octeon_i2c_wait(struct octeon_i2c *i2c)
+static int octeon_i2c_hlc_wait(struct octeon_i2c *i2c)
 {
-	long time_left;
-	bool first = 1;
+	bool first = true;
+	int time_left;
 
 	/*
-	 * Some chip revisions don't assert the irq in the interrupt
-	 * controller. So we must poll for the IFLG change.
+	 * Some cn38xx boards don't assert the irq in the interrupt
+	 * controller. So we must poll for the valid bit change.
 	 */
 	if (i2c->broken_irq_mode) {
 		u64 end = get_jiffies_64() + i2c->adap.timeout;
 
-		while (!octeon_i2c_test_iflg(i2c) &&
+		while (!octeon_i2c_hlc_test_valid(i2c) &&
 		       time_before64(get_jiffies_64(), end))
 			usleep_range(I2C_OCTEON_EVENT_WAIT / 2, I2C_OCTEON_EVENT_WAIT);
 
-		return octeon_i2c_test_iflg(i2c) ? 0 : -ETIMEDOUT;
+		return octeon_i2c_hlc_test_valid(i2c) ? 0 : -ETIMEDOUT;
 	}
 
-	i2c->int_enable(i2c);
-	time_left = wait_event_timeout(i2c->queue, octeon_i2c_test_ready(i2c, &first),
+	i2c->hlc_int_enable(i2c);
+	time_left = wait_event_timeout(i2c->queue,
+				       octeon_i2c_hlc_test_ready(i2c, &first),
 				       i2c->adap.timeout);
-	i2c->int_disable(i2c);
+	i2c->hlc_int_disable(i2c);
+	if (!time_left)
+		octeon_i2c_hlc_int_clear(i2c);
 
 	if (i2c->broken_irq_check && !time_left &&
-	    octeon_i2c_test_iflg(i2c)) {
+	    octeon_i2c_hlc_test_valid(i2c)) {
 		dev_err(i2c->dev, "broken irq connection detected, switching to polling mode.\n");
 		i2c->broken_irq_mode = true;
 		return 0;
@@ -405,13 +210,21 @@ static int octeon_i2c_wait(struct octeon_i2c *i2c)
 
 	if (!time_left)
 		return -ETIMEDOUT;
-
 	return 0;
 }
 
 static int octeon_i2c_check_status(struct octeon_i2c *i2c, int final_read)
 {
-	u8 stat = octeon_i2c_stat_read(i2c);
+	u8 stat;
+
+	/*
+	 * This is ugly... in HLC mode the status is not in the status register
+	 * but in the lower 8 bits of SW_TWSI.
+	 */
+	if (i2c->hlc_enabled)
+		stat = __raw_readq(i2c->twsi_base + SW_TWSI(i2c));
+	else
+		stat = octeon_i2c_stat_read(i2c);
 
 	switch (stat) {
 	/* Everything is fine */
@@ -470,83 +283,157 @@ static int octeon_i2c_check_status(struct octeon_i2c *i2c, int final_read)
 	}
 }
 
-static bool octeon_i2c_hlc_test_valid(struct octeon_i2c *i2c)
+static int octeon_i2c_recovery(struct octeon_i2c *i2c)
 {
-	return (__raw_readq(i2c->twsi_base + SW_TWSI) & SW_TWSI_V) == 0;
+	int ret;
+
+	ret = i2c_recover_bus(&i2c->adap);
+	if (ret)
+		/* recover failed, try hardware re-init */
+		ret = octeon_i2c_init_lowlevel(i2c);
+	return ret;
 }
 
-static bool octeon_i2c_hlc_test_ready(struct octeon_i2c *i2c, bool *first)
+/**
+ * octeon_i2c_start - send START to the bus
+ * @i2c: The struct octeon_i2c
+ *
+ * Returns 0 on success, otherwise a negative errno.
+ */
+static int octeon_i2c_start(struct octeon_i2c *i2c)
 {
-	/* check if valid bit is cleared */
-	if (octeon_i2c_hlc_test_valid(i2c))
-		return true;
+	int ret;
+	u8 stat;
 
-	if (*first) {
-		*first = false;
-		return false;
-	}
+	octeon_i2c_hlc_disable(i2c);
 
-	/*
-	 * IRQ has signaled an event but valid bit isn't cleared.
-	 * Sleep and retry once.
-	 */
-	usleep_range(I2C_OCTEON_EVENT_WAIT, 2 * I2C_OCTEON_EVENT_WAIT);
-	return octeon_i2c_hlc_test_valid(i2c);
+	octeon_i2c_ctl_write(i2c, TWSI_CTL_ENAB | TWSI_CTL_STA);
+	ret = octeon_i2c_wait(i2c);
+	if (ret)
+		goto error;
+
+	stat = octeon_i2c_stat_read(i2c);
+	if (stat == STAT_START || stat == STAT_REP_START)
+		/* START successful, bail out */
+		return 0;
+
+error:
+	/* START failed, try to recover */
+	ret = octeon_i2c_recovery(i2c);
+	return (ret) ? ret : -EAGAIN;
 }
 
-static void octeon_i2c_hlc_int_enable(struct octeon_i2c *i2c)
+/* send STOP to the bus */
+static void octeon_i2c_stop(struct octeon_i2c *i2c)
 {
-	octeon_i2c_write_int(i2c, TWSI_INT_ST_EN);
+	octeon_i2c_ctl_write(i2c, TWSI_CTL_ENAB | TWSI_CTL_STP);
 }
 
-static void octeon_i2c_hlc_int_clear(struct octeon_i2c *i2c)
+/**
+ * octeon_i2c_read - receive data from the bus via low-level controller
+ * @i2c: The struct octeon_i2c
+ * @target: Target address
+ * @data: Pointer to the location to store the data
+ * @rlength: Length of the data
+ * @recv_len: flag for length byte
+ *
+ * The address is sent over the bus, then the data is read.
+ *
+ * Returns 0 on success, otherwise a negative errno.
+ */
+static int octeon_i2c_read(struct octeon_i2c *i2c, int target,
+			   u8 *data, u16 *rlength, bool recv_len)
 {
-	/* clear ST/TS events, listen for neither */
-	octeon_i2c_write_int(i2c, TWSI_INT_ST_INT | TWSI_INT_TS_INT);
+	int i, result, length = *rlength;
+	bool final_read = false;
+
+	octeon_i2c_data_write(i2c, (target << 1) | 1);
+	octeon_i2c_ctl_write(i2c, TWSI_CTL_ENAB);
+
+	result = octeon_i2c_wait(i2c);
+	if (result)
+		return result;
+
+	/* address OK ? */
+	result = octeon_i2c_check_status(i2c, false);
+	if (result)
+		return result;
+
+	for (i = 0; i < length; i++) {
+		/*
+		 * For the last byte to receive TWSI_CTL_AAK must not be set.
+		 *
+		 * A special case is I2C_M_RECV_LEN where we don't know the
+		 * additional length yet. If recv_len is set we assume we're
+		 * not reading the final byte and therefore need to set
+		 * TWSI_CTL_AAK.
+		 */
+		if ((i + 1 == length) && !(recv_len && i == 0))
+			final_read = true;
+
+		/* clear iflg to allow next event */
+		if (final_read)
+			octeon_i2c_ctl_write(i2c, TWSI_CTL_ENAB);
+		else
+			octeon_i2c_ctl_write(i2c, TWSI_CTL_ENAB | TWSI_CTL_AAK);
+
+		result = octeon_i2c_wait(i2c);
+		if (result)
+			return result;
+
+		data[i] = octeon_i2c_data_read(i2c, &result);
+		if (result)
+			return result;
+		if (recv_len && i == 0) {
+			if (data[i] > I2C_SMBUS_BLOCK_MAX + 1)
+				return -EPROTO;
+			length += data[i];
+		}
+
+		result = octeon_i2c_check_status(i2c, final_read);
+		if (result)
+			return result;
+	}
+	*rlength = length;
+	return 0;
 }
 
 /**
- * octeon_i2c_hlc_wait - wait for an HLC operation to complete
+ * octeon_i2c_write - send data to the bus via low-level controller
  * @i2c: The struct octeon_i2c
+ * @target: Target address
+ * @data: Pointer to the data to be sent
+ * @length: Length of the data
  *
- * Returns 0 on success, otherwise -ETIMEDOUT.
+ * The address is sent over the bus, then the data.
+ *
+ * Returns 0 on success, otherwise a negative errno.
  */
-static int octeon_i2c_hlc_wait(struct octeon_i2c *i2c)
+static int octeon_i2c_write(struct octeon_i2c *i2c, int target,
+			    const u8 *data, int length)
 {
-	bool first = 1;
-	int time_left;
+	int i, result;
 
-	/*
-	 * Some cn38xx boards don't assert the irq in the interrupt
-	 * controller. So we must poll for the valid bit change.
-	 */
-	if (i2c->broken_irq_mode) {
-		u64 end = get_jiffies_64() + i2c->adap.timeout;
+	octeon_i2c_data_write(i2c, target << 1);
+	octeon_i2c_ctl_write(i2c, TWSI_CTL_ENAB);
 
-		while (!octeon_i2c_hlc_test_valid(i2c) &&
-		       time_before64(get_jiffies_64(), end))
-			usleep_range(I2C_OCTEON_EVENT_WAIT / 2, I2C_OCTEON_EVENT_WAIT);
+	result = octeon_i2c_wait(i2c);
+	if (result)
+		return result;
 
-		return octeon_i2c_hlc_test_valid(i2c) ? 0 : -ETIMEDOUT;
-	}
+	for (i = 0; i < length; i++) {
+		result = octeon_i2c_check_status(i2c, false);
+		if (result)
+			return result;
 
-	i2c->hlc_int_enable(i2c);
-	time_left = wait_event_timeout(i2c->queue,
-				       octeon_i2c_hlc_test_ready(i2c, &first),
-				       i2c->adap.timeout);
-	i2c->hlc_int_disable(i2c);
-	if (!time_left)
-		octeon_i2c_hlc_int_clear(i2c);
+		octeon_i2c_data_write(i2c, data[i]);
+		octeon_i2c_ctl_write(i2c, TWSI_CTL_ENAB);
 
-	if (i2c->broken_irq_check && !time_left &&
-	    octeon_i2c_hlc_test_valid(i2c)) {
-		dev_err(i2c->dev, "broken irq connection detected, switching to polling mode.\n");
-		i2c->broken_irq_mode = true;
-		return 0;
+		result = octeon_i2c_wait(i2c);
+		if (result)
+			return result;
 	}
 
-	if (!time_left)
-		return -ETIMEDOUT;
 	return 0;
 }
 
@@ -570,20 +457,20 @@ static int octeon_i2c_hlc_read(struct octeon_i2c *i2c, struct i2c_msg *msgs)
 	else
 		cmd |= SW_TWSI_OP_7;
 
-	octeon_i2c_writeq_flush(cmd, i2c->twsi_base + SW_TWSI);
+	octeon_i2c_writeq_flush(cmd, i2c->twsi_base + SW_TWSI(i2c));
 	ret = octeon_i2c_hlc_wait(i2c);
 	if (ret)
 		goto err;
 
-	cmd = __raw_readq(i2c->twsi_base + SW_TWSI);
+	cmd = __raw_readq(i2c->twsi_base + SW_TWSI(i2c));
 	if ((cmd & SW_TWSI_R) == 0)
-		return -EAGAIN;
+		return octeon_i2c_check_status(i2c, false);
 
 	for (i = 0, j = msgs[0].len - 1; i  < msgs[0].len && i < 4; i++, j--)
 		msgs[0].buf[j] = (cmd >> (8 * i)) & 0xff;
 
 	if (msgs[0].len > 4) {
-		cmd = __raw_readq(i2c->twsi_base + SW_TWSI_EXT);
+		cmd = __raw_readq(i2c->twsi_base + SW_TWSI_EXT(i2c));
 		for (i = 0; i  < msgs[0].len - 4 && i < 4; i++, j--)
 			msgs[0].buf[j] = (cmd >> (8 * i)) & 0xff;
 	}
@@ -620,19 +507,17 @@ static int octeon_i2c_hlc_write(struct octeon_i2c *i2c, struct i2c_msg *msgs)
 
 		for (i = 0; i < msgs[0].len - 4 && i < 4; i++, j--)
 			ext |= (u64)msgs[0].buf[j] << (8 * i);
-		octeon_i2c_writeq_flush(ext, i2c->twsi_base + SW_TWSI_EXT);
+		octeon_i2c_writeq_flush(ext, i2c->twsi_base + SW_TWSI_EXT(i2c));
 	}
 
-	octeon_i2c_writeq_flush(cmd, i2c->twsi_base + SW_TWSI);
+	octeon_i2c_writeq_flush(cmd, i2c->twsi_base + SW_TWSI(i2c));
 	ret = octeon_i2c_hlc_wait(i2c);
 	if (ret)
 		goto err;
 
-	cmd = __raw_readq(i2c->twsi_base + SW_TWSI);
+	cmd = __raw_readq(i2c->twsi_base + SW_TWSI(i2c));
 	if ((cmd & SW_TWSI_R) == 0)
-		return -EAGAIN;
-
-	ret = octeon_i2c_check_status(i2c, false);
+		return octeon_i2c_check_status(i2c, false);
 
 err:
 	return ret;
@@ -663,27 +548,27 @@ static int octeon_i2c_hlc_comp_read(struct octeon_i2c *i2c, struct i2c_msg *msgs
 		cmd |= SW_TWSI_EIA;
 		ext = (u64)msgs[0].buf[0] << SW_TWSI_IA_SHIFT;
 		cmd |= (u64)msgs[0].buf[1] << SW_TWSI_IA_SHIFT;
-		octeon_i2c_writeq_flush(ext, i2c->twsi_base + SW_TWSI_EXT);
+		octeon_i2c_writeq_flush(ext, i2c->twsi_base + SW_TWSI_EXT(i2c));
 	} else {
 		cmd |= (u64)msgs[0].buf[0] << SW_TWSI_IA_SHIFT;
 	}
 
 	octeon_i2c_hlc_int_clear(i2c);
-	octeon_i2c_writeq_flush(cmd, i2c->twsi_base + SW_TWSI);
+	octeon_i2c_writeq_flush(cmd, i2c->twsi_base + SW_TWSI(i2c));
 
 	ret = octeon_i2c_hlc_wait(i2c);
 	if (ret)
 		goto err;
 
-	cmd = __raw_readq(i2c->twsi_base + SW_TWSI);
+	cmd = __raw_readq(i2c->twsi_base + SW_TWSI(i2c));
 	if ((cmd & SW_TWSI_R) == 0)
-		return -EAGAIN;
+		return octeon_i2c_check_status(i2c, false);
 
 	for (i = 0, j = msgs[1].len - 1; i  < msgs[1].len && i < 4; i++, j--)
 		msgs[1].buf[j] = (cmd >> (8 * i)) & 0xff;
 
 	if (msgs[1].len > 4) {
-		cmd = __raw_readq(i2c->twsi_base + SW_TWSI_EXT);
+		cmd = __raw_readq(i2c->twsi_base + SW_TWSI_EXT(i2c));
 		for (i = 0; i  < msgs[1].len - 4 && i < 4; i++, j--)
 			msgs[1].buf[j] = (cmd >> (8 * i)) & 0xff;
 	}
@@ -730,27 +615,85 @@ static int octeon_i2c_hlc_comp_write(struct octeon_i2c *i2c, struct i2c_msg *msg
 		set_ext = true;
 	}
 	if (set_ext)
-		octeon_i2c_writeq_flush(ext, i2c->twsi_base + SW_TWSI_EXT);
+		octeon_i2c_writeq_flush(ext, i2c->twsi_base + SW_TWSI_EXT(i2c));
 
 	octeon_i2c_hlc_int_clear(i2c);
-	octeon_i2c_writeq_flush(cmd, i2c->twsi_base + SW_TWSI);
+	octeon_i2c_writeq_flush(cmd, i2c->twsi_base + SW_TWSI(i2c));
 
 	ret = octeon_i2c_hlc_wait(i2c);
 	if (ret)
 		goto err;
 
-	cmd = __raw_readq(i2c->twsi_base + SW_TWSI);
+	cmd = __raw_readq(i2c->twsi_base + SW_TWSI(i2c));
 	if ((cmd & SW_TWSI_R) == 0)
-		return -EAGAIN;
-
-	ret = octeon_i2c_check_status(i2c, false);
+		return octeon_i2c_check_status(i2c, false);
 
 err:
 	return ret;
 }
 
+/**
+ * octeon_i2c_xfer - The driver's master_xfer function
+ * @adap: Pointer to the i2c_adapter structure
+ * @msgs: Pointer to the messages to be processed
+ * @num: Length of the MSGS array
+ *
+ * Returns the number of messages processed, or a negative errno on failure.
+ */
+int octeon_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg *msgs, int num)
+{
+	struct octeon_i2c *i2c = i2c_get_adapdata(adap);
+	int i, ret = 0;
+
+	if (num == 1) {
+		if (msgs[0].len > 0 && msgs[0].len <= 8) {
+			if (msgs[0].flags & I2C_M_RD)
+				ret = octeon_i2c_hlc_read(i2c, msgs);
+			else
+				ret = octeon_i2c_hlc_write(i2c, msgs);
+			goto out;
+		}
+	} else if (num == 2) {
+		if ((msgs[0].flags & I2C_M_RD) == 0 &&
+		    (msgs[1].flags & I2C_M_RECV_LEN) == 0 &&
+		    msgs[0].len > 0 && msgs[0].len <= 2 &&
+		    msgs[1].len > 0 && msgs[1].len <= 8 &&
+		    msgs[0].addr == msgs[1].addr) {
+			if (msgs[1].flags & I2C_M_RD)
+				ret = octeon_i2c_hlc_comp_read(i2c, msgs);
+			else
+				ret = octeon_i2c_hlc_comp_write(i2c, msgs);
+			goto out;
+		}
+	}
+
+	for (i = 0; ret == 0 && i < num; i++) {
+		struct i2c_msg *pmsg = &msgs[i];
+
+		/* zero-length messages are not supported */
+		if (!pmsg->len) {
+			ret = -EOPNOTSUPP;
+			break;
+		}
+
+		ret = octeon_i2c_start(i2c);
+		if (ret)
+			return ret;
+
+		if (pmsg->flags & I2C_M_RD)
+			ret = octeon_i2c_read(i2c, pmsg->addr, pmsg->buf,
+					      &pmsg->len, pmsg->flags & I2C_M_RECV_LEN);
+		else
+			ret = octeon_i2c_write(i2c, pmsg->addr, pmsg->buf,
+					       pmsg->len);
+	}
+	octeon_i2c_stop(i2c);
+out:
+	return (ret != 0) ? ret : num;
+}
+
 /* calculate and set clock divisors */
-static void octeon_i2c_set_clock(struct octeon_i2c *i2c)
+void octeon_i2c_set_clock(struct octeon_i2c *i2c)
 {
 	int tclk, thp_base, inc, thp_idx, mdiv_idx, ndiv_idx, foscl, diff;
 	int thp = 0x18, mdiv = 2, ndiv = 0, delta_hz = 1000000;
@@ -791,7 +734,7 @@ static void octeon_i2c_set_clock(struct octeon_i2c *i2c)
 	octeon_i2c_reg_write(i2c, SW_TWSI_EOP_TWSI_CLKCTL, (mdiv << 3) | ndiv);
 }
 
-static int octeon_i2c_init_lowlevel(struct octeon_i2c *i2c)
+int octeon_i2c_init_lowlevel(struct octeon_i2c *i2c)
 {
 	u8 status = 0;
 	int tries;
@@ -818,219 +761,6 @@ static int octeon_i2c_init_lowlevel(struct octeon_i2c *i2c)
 	return 0;
 }
 
-static int octeon_i2c_recovery(struct octeon_i2c *i2c)
-{
-	int ret;
-
-	ret = i2c_recover_bus(&i2c->adap);
-	if (ret)
-		/* recover failed, try hardware re-init */
-		ret = octeon_i2c_init_lowlevel(i2c);
-	return ret;
-}
-
-/**
- * octeon_i2c_start - send START to the bus
- * @i2c: The struct octeon_i2c
- *
- * Returns 0 on success, otherwise a negative errno.
- */
-static int octeon_i2c_start(struct octeon_i2c *i2c)
-{
-	int ret;
-	u8 stat;
-
-	octeon_i2c_hlc_disable(i2c);
-
-	octeon_i2c_ctl_write(i2c, TWSI_CTL_ENAB | TWSI_CTL_STA);
-	ret = octeon_i2c_wait(i2c);
-	if (ret)
-		goto error;
-
-	stat = octeon_i2c_stat_read(i2c);
-	if (stat == STAT_START || stat == STAT_REP_START)
-		/* START successful, bail out */
-		return 0;
-
-error:
-	/* START failed, try to recover */
-	ret = octeon_i2c_recovery(i2c);
-	return (ret) ? ret : -EAGAIN;
-}
-
-/* send STOP to the bus */
-static void octeon_i2c_stop(struct octeon_i2c *i2c)
-{
-	octeon_i2c_ctl_write(i2c, TWSI_CTL_ENAB | TWSI_CTL_STP);
-}
-
-/**
- * octeon_i2c_write - send data to the bus via low-level controller
- * @i2c: The struct octeon_i2c
- * @target: Target address
- * @data: Pointer to the data to be sent
- * @length: Length of the data
- *
- * The address is sent over the bus, then the data.
- *
- * Returns 0 on success, otherwise a negative errno.
- */
-static int octeon_i2c_write(struct octeon_i2c *i2c, int target,
-			    const u8 *data, int length)
-{
-	int i, result;
-
-	octeon_i2c_data_write(i2c, target << 1);
-	octeon_i2c_ctl_write(i2c, TWSI_CTL_ENAB);
-
-	result = octeon_i2c_wait(i2c);
-	if (result)
-		return result;
-
-	for (i = 0; i < length; i++) {
-		result = octeon_i2c_check_status(i2c, false);
-		if (result)
-			return result;
-
-		octeon_i2c_data_write(i2c, data[i]);
-		octeon_i2c_ctl_write(i2c, TWSI_CTL_ENAB);
-
-		result = octeon_i2c_wait(i2c);
-		if (result)
-			return result;
-	}
-
-	return 0;
-}
-
-/**
- * octeon_i2c_read - receive data from the bus via low-level controller
- * @i2c: The struct octeon_i2c
- * @target: Target address
- * @data: Pointer to the location to store the data
- * @rlength: Length of the data
- * @recv_len: flag for length byte
- *
- * The address is sent over the bus, then the data is read.
- *
- * Returns 0 on success, otherwise a negative errno.
- */
-static int octeon_i2c_read(struct octeon_i2c *i2c, int target,
-			   u8 *data, u16 *rlength, bool recv_len)
-{
-	int i, result, length = *rlength;
-	bool final_read = false;
-
-	octeon_i2c_data_write(i2c, (target << 1) | 1);
-	octeon_i2c_ctl_write(i2c, TWSI_CTL_ENAB);
-
-	result = octeon_i2c_wait(i2c);
-	if (result)
-		return result;
-
-	/* address OK ? */
-	result = octeon_i2c_check_status(i2c, false);
-	if (result)
-		return result;
-
-	for (i = 0; i < length; i++) {
-		/*
-		 * For the last byte to receive TWSI_CTL_AAK must not be set.
-		 *
-		 * A special case is I2C_M_RECV_LEN where we don't know the
-		 * additional length yet. If recv_len is set we assume we're
-		 * not reading the final byte and therefore need to set
-		 * TWSI_CTL_AAK.
-		 */
-		if ((i + 1 == length) && !(recv_len && i == 0))
-			final_read = true;
-
-		/* clear iflg to allow next event */
-		if (final_read)
-			octeon_i2c_ctl_write(i2c, TWSI_CTL_ENAB);
-		else
-			octeon_i2c_ctl_write(i2c, TWSI_CTL_ENAB | TWSI_CTL_AAK);
-
-		result = octeon_i2c_wait(i2c);
-		if (result)
-			return result;
-
-		data[i] = octeon_i2c_data_read(i2c);
-		if (recv_len && i == 0) {
-			if (data[i] > I2C_SMBUS_BLOCK_MAX + 1)
-				return -EPROTO;
-			length += data[i];
-		}
-
-		result = octeon_i2c_check_status(i2c, final_read);
-		if (result)
-			return result;
-	}
-	*rlength = length;
-	return 0;
-}
-
-/**
- * octeon_i2c_xfer - The driver's master_xfer function
- * @adap: Pointer to the i2c_adapter structure
- * @msgs: Pointer to the messages to be processed
- * @num: Length of the MSGS array
- *
- * Returns the number of messages processed, or a negative errno on failure.
- */
-static int octeon_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg *msgs,
-			   int num)
-{
-	struct octeon_i2c *i2c = i2c_get_adapdata(adap);
-	int i, ret = 0;
-
-	if (num == 1) {
-		if (msgs[0].len > 0 && msgs[0].len <= 8) {
-			if (msgs[0].flags & I2C_M_RD)
-				ret = octeon_i2c_hlc_read(i2c, msgs);
-			else
-				ret = octeon_i2c_hlc_write(i2c, msgs);
-			goto out;
-		}
-	} else if (num == 2) {
-		if ((msgs[0].flags & I2C_M_RD) == 0 &&
-		    (msgs[1].flags & I2C_M_RECV_LEN) == 0 &&
-		    msgs[0].len > 0 && msgs[0].len <= 2 &&
-		    msgs[1].len > 0 && msgs[1].len <= 8 &&
-		    msgs[0].addr == msgs[1].addr) {
-			if (msgs[1].flags & I2C_M_RD)
-				ret = octeon_i2c_hlc_comp_read(i2c, msgs);
-			else
-				ret = octeon_i2c_hlc_comp_write(i2c, msgs);
-			goto out;
-		}
-	}
-
-	for (i = 0; ret == 0 && i < num; i++) {
-		struct i2c_msg *pmsg = &msgs[i];
-
-		/* zero-length messages are not supported */
-		if (!pmsg->len) {
-			ret = -EOPNOTSUPP;
-			break;
-		}
-
-		ret = octeon_i2c_start(i2c);
-		if (ret)
-			return ret;
-
-		if (pmsg->flags & I2C_M_RD)
-			ret = octeon_i2c_read(i2c, pmsg->addr, pmsg->buf,
-					      &pmsg->len, pmsg->flags & I2C_M_RECV_LEN);
-		else
-			ret = octeon_i2c_write(i2c, pmsg->addr, pmsg->buf,
-					       pmsg->len);
-	}
-	octeon_i2c_stop(i2c);
-out:
-	return (ret != 0) ? ret : num;
-}
-
 static int octeon_i2c_get_scl(struct i2c_adapter *adap)
 {
 	struct octeon_i2c *i2c = i2c_get_adapdata(adap);
@@ -1044,7 +774,7 @@ static void octeon_i2c_set_scl(struct i2c_adapter *adap, int val)
 {
 	struct octeon_i2c *i2c = i2c_get_adapdata(adap);
 
-	octeon_i2c_write_int(i2c, TWSI_INT_SCL_OVR);
+	octeon_i2c_write_int(i2c, val ? 0 : TWSI_INT_SCL_OVR);
 }
 
 static int octeon_i2c_get_sda(struct i2c_adapter *adap)
@@ -1060,13 +790,14 @@ static void octeon_i2c_prepare_recovery(struct i2c_adapter *adap)
 {
 	struct octeon_i2c *i2c = i2c_get_adapdata(adap);
 
+	octeon_i2c_hlc_disable(i2c);
+
 	/*
-	 * The stop resets the state machine, does not _transmit_ STOP unless
-	 * engine was active.
+	 * Bring control register to a good state regardless
+	 * of HLC state.
 	 */
-	octeon_i2c_stop(i2c);
+	octeon_i2c_ctl_write(i2c, TWSI_CTL_ENAB);
 
-	octeon_i2c_hlc_disable(i2c);
 	octeon_i2c_write_int(i2c, 0);
 }
 
@@ -1074,10 +805,19 @@ static void octeon_i2c_unprepare_recovery(struct i2c_adapter *adap)
 {
 	struct octeon_i2c *i2c = i2c_get_adapdata(adap);
 
+	/*
+	 * Generate STOP to finish the unfinished transaction.
+	 * Can't generate STOP via the TWSI CTL register
+	 * since it could bring the TWSI controller into an inoperable state.
+	 */
+	octeon_i2c_write_int(i2c, TWSI_INT_SDA_OVR | TWSI_INT_SCL_OVR);
+	udelay(5);
+	octeon_i2c_write_int(i2c, TWSI_INT_SDA_OVR);
+	udelay(5);
 	octeon_i2c_write_int(i2c, 0);
 }
 
-static struct i2c_bus_recovery_info octeon_i2c_recovery_info = {
+struct i2c_bus_recovery_info octeon_i2c_recovery_info = {
 	.recover_bus = i2c_generic_scl_recovery,
 	.get_scl = octeon_i2c_get_scl,
 	.set_scl = octeon_i2c_set_scl,
@@ -1085,171 +825,3 @@ static struct i2c_bus_recovery_info octeon_i2c_recovery_info = {
 	.prepare_recovery = octeon_i2c_prepare_recovery,
 	.unprepare_recovery = octeon_i2c_unprepare_recovery,
 };
-
-static u32 octeon_i2c_functionality(struct i2c_adapter *adap)
-{
-	return I2C_FUNC_I2C | (I2C_FUNC_SMBUS_EMUL & ~I2C_FUNC_SMBUS_QUICK) |
-	       I2C_FUNC_SMBUS_READ_BLOCK_DATA | I2C_SMBUS_BLOCK_PROC_CALL;
-}
-
-static const struct i2c_algorithm octeon_i2c_algo = {
-	.master_xfer = octeon_i2c_xfer,
-	.functionality = octeon_i2c_functionality,
-};
-
-static struct i2c_adapter octeon_i2c_ops = {
-	.owner = THIS_MODULE,
-	.name = "OCTEON adapter",
-	.algo = &octeon_i2c_algo,
-};
-
-static int octeon_i2c_probe(struct platform_device *pdev)
-{
-	struct device_node *node = pdev->dev.of_node;
-	int irq, result = 0, hlc_irq = 0;
-	struct resource *res_mem;
-	struct octeon_i2c *i2c;
-	bool cn78xx_style;
-
-	cn78xx_style = of_device_is_compatible(node, "cavium,octeon-7890-twsi");
-	if (cn78xx_style) {
-		hlc_irq = platform_get_irq(pdev, 0);
-		if (hlc_irq < 0)
-			return hlc_irq;
-
-		irq = platform_get_irq(pdev, 2);
-		if (irq < 0)
-			return irq;
-	} else {
-		/* All adaptors have an irq.  */
-		irq = platform_get_irq(pdev, 0);
-		if (irq < 0)
-			return irq;
-	}
-
-	i2c = devm_kzalloc(&pdev->dev, sizeof(*i2c), GFP_KERNEL);
-	if (!i2c) {
-		result = -ENOMEM;
-		goto out;
-	}
-	i2c->dev = &pdev->dev;
-
-	res_mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	i2c->twsi_base = devm_ioremap_resource(&pdev->dev, res_mem);
-	if (IS_ERR(i2c->twsi_base)) {
-		result = PTR_ERR(i2c->twsi_base);
-		goto out;
-	}
-
-	/*
-	 * "clock-rate" is a legacy binding, the official binding is
-	 * "clock-frequency".  Try the official one first and then
-	 * fall back if it doesn't exist.
-	 */
-	if (of_property_read_u32(node, "clock-frequency", &i2c->twsi_freq) &&
-	    of_property_read_u32(node, "clock-rate", &i2c->twsi_freq)) {
-		dev_err(i2c->dev,
-			"no I2C 'clock-rate' or 'clock-frequency' property\n");
-		result = -ENXIO;
-		goto out;
-	}
-
-	i2c->sys_freq = octeon_get_io_clock_rate();
-
-	init_waitqueue_head(&i2c->queue);
-
-	i2c->irq = irq;
-
-	if (cn78xx_style) {
-		i2c->hlc_irq = hlc_irq;
-
-		i2c->int_enable = octeon_i2c_int_enable78;
-		i2c->int_disable = octeon_i2c_int_disable78;
-		i2c->hlc_int_enable = octeon_i2c_hlc_int_enable78;
-		i2c->hlc_int_disable = octeon_i2c_hlc_int_disable78;
-
-		irq_set_status_flags(i2c->irq, IRQ_NOAUTOEN);
-		irq_set_status_flags(i2c->hlc_irq, IRQ_NOAUTOEN);
-
-		result = devm_request_irq(&pdev->dev, i2c->hlc_irq,
-					  octeon_i2c_hlc_isr78, 0,
-					  DRV_NAME, i2c);
-		if (result < 0) {
-			dev_err(i2c->dev, "failed to attach interrupt\n");
-			goto out;
-		}
-	} else {
-		i2c->int_enable = octeon_i2c_int_enable;
-		i2c->int_disable = octeon_i2c_int_disable;
-		i2c->hlc_int_enable = octeon_i2c_hlc_int_enable;
-		i2c->hlc_int_disable = octeon_i2c_int_disable;
-	}
-
-	result = devm_request_irq(&pdev->dev, i2c->irq,
-				  octeon_i2c_isr, 0, DRV_NAME, i2c);
-	if (result < 0) {
-		dev_err(i2c->dev, "failed to attach interrupt\n");
-		goto out;
-	}
-
-	if (OCTEON_IS_MODEL(OCTEON_CN38XX))
-		i2c->broken_irq_check = true;
-
-	result = octeon_i2c_init_lowlevel(i2c);
-	if (result) {
-		dev_err(i2c->dev, "init low level failed\n");
-		goto  out;
-	}
-
-	octeon_i2c_set_clock(i2c);
-
-	i2c->adap = octeon_i2c_ops;
-	i2c->adap.timeout = msecs_to_jiffies(2);
-	i2c->adap.retries = 5;
-	i2c->adap.bus_recovery_info = &octeon_i2c_recovery_info;
-	i2c->adap.dev.parent = &pdev->dev;
-	i2c->adap.dev.of_node = node;
-	i2c_set_adapdata(&i2c->adap, i2c);
-	platform_set_drvdata(pdev, i2c);
-
-	result = i2c_add_adapter(&i2c->adap);
-	if (result < 0) {
-		dev_err(i2c->dev, "failed to add adapter\n");
-		goto out;
-	}
-	dev_info(i2c->dev, "probed\n");
-	return 0;
-
-out:
-	return result;
-};
-
-static int octeon_i2c_remove(struct platform_device *pdev)
-{
-	struct octeon_i2c *i2c = platform_get_drvdata(pdev);
-
-	i2c_del_adapter(&i2c->adap);
-	return 0;
-};
-
-static const struct of_device_id octeon_i2c_match[] = {
-	{ .compatible = "cavium,octeon-3860-twsi", },
-	{ .compatible = "cavium,octeon-7890-twsi", },
-	{},
-};
-MODULE_DEVICE_TABLE(of, octeon_i2c_match);
-
-static struct platform_driver octeon_i2c_driver = {
-	.probe		= octeon_i2c_probe,
-	.remove		= octeon_i2c_remove,
-	.driver		= {
-		.name	= DRV_NAME,
-		.of_match_table = octeon_i2c_match,
-	},
-};
-
-module_platform_driver(octeon_i2c_driver);
-
-MODULE_AUTHOR("Michael Lawnick <michael.lawnick.ext@nsn.com>");
-MODULE_DESCRIPTION("I2C-Bus adapter for Cavium OCTEON processors");
-MODULE_LICENSE("GPL");
diff --git a/drivers/i2c/busses/i2c-octeon-core.h b/drivers/i2c/busses/i2c-octeon-core.h
new file mode 100644
index 000000000000..1db7c835a454
--- /dev/null
+++ b/drivers/i2c/busses/i2c-octeon-core.h
@@ -0,0 +1,216 @@
+#include <linux/atomic.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/i2c.h>
+#include <linux/i2c-smbus.h>
+#include <linux/io.h>
+#include <linux/iopoll.h>
+#include <linux/kernel.h>
+#include <linux/pci.h>
+
+/* Controller command patterns */
+#define SW_TWSI_V		BIT_ULL(63)	/* Valid bit */
+#define SW_TWSI_EIA		BIT_ULL(61)	/* Extended internal address */
+#define SW_TWSI_R		BIT_ULL(56)	/* Result or read bit */
+#define SW_TWSI_SOVR		BIT_ULL(55)	/* Size override */
+#define SW_TWSI_SIZE_SHIFT	52
+#define SW_TWSI_ADDR_SHIFT	40
+#define SW_TWSI_IA_SHIFT	32		/* Internal address */
+
+/* Controller opcode word (bits 60:57) */
+#define SW_TWSI_OP_SHIFT	57
+#define SW_TWSI_OP_7		(0ULL << SW_TWSI_OP_SHIFT)
+#define SW_TWSI_OP_7_IA		(1ULL << SW_TWSI_OP_SHIFT)
+#define SW_TWSI_OP_10		(2ULL << SW_TWSI_OP_SHIFT)
+#define SW_TWSI_OP_10_IA	(3ULL << SW_TWSI_OP_SHIFT)
+#define SW_TWSI_OP_TWSI_CLK	(4ULL << SW_TWSI_OP_SHIFT)
+#define SW_TWSI_OP_EOP		(6ULL << SW_TWSI_OP_SHIFT) /* Extended opcode */
+
+/* Controller extended opcode word (bits 34:32) */
+#define SW_TWSI_EOP_SHIFT	32
+#define SW_TWSI_EOP_TWSI_DATA	(SW_TWSI_OP_EOP | 1ULL << SW_TWSI_EOP_SHIFT)
+#define SW_TWSI_EOP_TWSI_CTL	(SW_TWSI_OP_EOP | 2ULL << SW_TWSI_EOP_SHIFT)
+#define SW_TWSI_EOP_TWSI_CLKCTL	(SW_TWSI_OP_EOP | 3ULL << SW_TWSI_EOP_SHIFT)
+#define SW_TWSI_EOP_TWSI_STAT	(SW_TWSI_OP_EOP | 3ULL << SW_TWSI_EOP_SHIFT)
+#define SW_TWSI_EOP_TWSI_RST	(SW_TWSI_OP_EOP | 7ULL << SW_TWSI_EOP_SHIFT)
+
+/* Controller command and status bits */
+#define TWSI_CTL_CE		0x80	/* High level controller enable */
+#define TWSI_CTL_ENAB		0x40	/* Bus enable */
+#define TWSI_CTL_STA		0x20	/* Master-mode start, HW clears when done */
+#define TWSI_CTL_STP		0x10	/* Master-mode stop, HW clears when done */
+#define TWSI_CTL_IFLG		0x08	/* HW event, SW writes 0 to ACK */
+#define TWSI_CTL_AAK		0x04	/* Assert ACK */
+
+/* Status values */
+#define STAT_ERROR		0x00
+#define STAT_START		0x08
+#define STAT_REP_START		0x10
+#define STAT_TXADDR_ACK		0x18
+#define STAT_TXADDR_NAK		0x20
+#define STAT_TXDATA_ACK		0x28
+#define STAT_TXDATA_NAK		0x30
+#define STAT_LOST_ARB_38	0x38
+#define STAT_RXADDR_ACK		0x40
+#define STAT_RXADDR_NAK		0x48
+#define STAT_RXDATA_ACK		0x50
+#define STAT_RXDATA_NAK		0x58
+#define STAT_SLAVE_60		0x60
+#define STAT_LOST_ARB_68	0x68
+#define STAT_SLAVE_70		0x70
+#define STAT_LOST_ARB_78	0x78
+#define STAT_SLAVE_80		0x80
+#define STAT_SLAVE_88		0x88
+#define STAT_GENDATA_ACK	0x90
+#define STAT_GENDATA_NAK	0x98
+#define STAT_SLAVE_A0		0xA0
+#define STAT_SLAVE_A8		0xA8
+#define STAT_LOST_ARB_B0	0xB0
+#define STAT_SLAVE_LOST		0xB8
+#define STAT_SLAVE_NAK		0xC0
+#define STAT_SLAVE_ACK		0xC8
+#define STAT_AD2W_ACK		0xD0
+#define STAT_AD2W_NAK		0xD8
+#define STAT_IDLE		0xF8
+
+/* TWSI_INT values */
+#define TWSI_INT_ST_INT		BIT_ULL(0)
+#define TWSI_INT_TS_INT		BIT_ULL(1)
+#define TWSI_INT_CORE_INT	BIT_ULL(2)
+#define TWSI_INT_ST_EN		BIT_ULL(4)
+#define TWSI_INT_TS_EN		BIT_ULL(5)
+#define TWSI_INT_CORE_EN	BIT_ULL(6)
+#define TWSI_INT_SDA_OVR	BIT_ULL(8)
+#define TWSI_INT_SCL_OVR	BIT_ULL(9)
+#define TWSI_INT_SDA		BIT_ULL(10)
+#define TWSI_INT_SCL		BIT_ULL(11)
+
+#define I2C_OCTEON_EVENT_WAIT 80 /* microseconds */
+
+/* Register offsets */
+struct octeon_i2c_reg_offset {
+	unsigned int sw_twsi;
+	unsigned int twsi_int;
+	unsigned int sw_twsi_ext;
+};
+
+#define SW_TWSI(x)	(x->roff.sw_twsi)
+#define TWSI_INT(x)	(x->roff.twsi_int)
+#define SW_TWSI_EXT(x)	(x->roff.sw_twsi_ext)
+
+struct octeon_i2c {
+	wait_queue_head_t queue;
+	struct i2c_adapter adap;
+	struct octeon_i2c_reg_offset roff;
+	struct clk *clk;
+	int irq;
+	int hlc_irq;		/* For cn7890 only */
+	u32 twsi_freq;
+	int sys_freq;
+	void __iomem *twsi_base;
+	struct device *dev;
+	bool hlc_enabled;
+	bool broken_irq_mode;
+	bool broken_irq_check;
+	void (*int_enable)(struct octeon_i2c *);
+	void (*int_disable)(struct octeon_i2c *);
+	void (*hlc_int_enable)(struct octeon_i2c *);
+	void (*hlc_int_disable)(struct octeon_i2c *);
+	atomic_t int_enable_cnt;
+	atomic_t hlc_int_enable_cnt;
+#if IS_ENABLED(CONFIG_I2C_THUNDERX)
+	struct msix_entry i2c_msix;
+#endif
+	struct i2c_smbus_alert_setup alert_data;
+	struct i2c_client *ara;
+};
+
+static inline void octeon_i2c_writeq_flush(u64 val, void __iomem *addr)
+{
+	__raw_writeq(val, addr);
+	__raw_readq(addr);	/* wait for write to land */
+}
+
+/**
+ * octeon_i2c_reg_write - write an I2C core register
+ * @i2c: The struct octeon_i2c
+ * @eop_reg: Register selector
+ * @data: Value to be written
+ *
+ * The I2C core registers are accessed indirectly via the SW_TWSI CSR.
+ */
+static inline void octeon_i2c_reg_write(struct octeon_i2c *i2c, u64 eop_reg, u8 data)
+{
+	u64 tmp;
+
+	__raw_writeq(SW_TWSI_V | eop_reg | data, i2c->twsi_base + SW_TWSI(i2c));
+
+	readq_poll_timeout(i2c->twsi_base + SW_TWSI(i2c), tmp, tmp & SW_TWSI_V,
+			   I2C_OCTEON_EVENT_WAIT, i2c->adap.timeout);
+}
+
+#define octeon_i2c_ctl_write(i2c, val)					\
+	octeon_i2c_reg_write(i2c, SW_TWSI_EOP_TWSI_CTL, val)
+#define octeon_i2c_data_write(i2c, val)					\
+	octeon_i2c_reg_write(i2c, SW_TWSI_EOP_TWSI_DATA, val)
+
+/**
+ * octeon_i2c_reg_read - read lower bits of an I2C core register
+ * @i2c: The struct octeon_i2c
+ * @eop_reg: Register selector
+ *
+ * Returns the data.
+ *
+ * The I2C core registers are accessed indirectly via the SW_TWSI CSR.
+ */
+static inline int octeon_i2c_reg_read(struct octeon_i2c *i2c, u64 eop_reg,
+				      int *error)
+{
+	u64 tmp;
+	int ret;
+
+	__raw_writeq(SW_TWSI_V | eop_reg | SW_TWSI_R, i2c->twsi_base + SW_TWSI(i2c));
+
+	ret = readq_poll_timeout(i2c->twsi_base + SW_TWSI(i2c), tmp,
+				 tmp & SW_TWSI_V, I2C_OCTEON_EVENT_WAIT,
+				 i2c->adap.timeout);
+	if (error)
+		*error = ret;
+	return tmp & 0xFF;
+}
+
+#define octeon_i2c_ctl_read(i2c)					\
+	octeon_i2c_reg_read(i2c, SW_TWSI_EOP_TWSI_CTL, NULL)
+#define octeon_i2c_data_read(i2c, error)				\
+	octeon_i2c_reg_read(i2c, SW_TWSI_EOP_TWSI_DATA, error)
+#define octeon_i2c_stat_read(i2c)					\
+	octeon_i2c_reg_read(i2c, SW_TWSI_EOP_TWSI_STAT, NULL)
+
+/**
+ * octeon_i2c_read_int - read the TWSI_INT register
+ * @i2c: The struct octeon_i2c
+ *
+ * Returns the value of the register.
+ */
+static inline u64 octeon_i2c_read_int(struct octeon_i2c *i2c)
+{
+	return __raw_readq(i2c->twsi_base + TWSI_INT(i2c));
+}
+
+/**
+ * octeon_i2c_write_int - write the TWSI_INT register
+ * @i2c: The struct octeon_i2c
+ * @data: Value to be written
+ */
+static inline void octeon_i2c_write_int(struct octeon_i2c *i2c, u64 data)
+{
+	octeon_i2c_writeq_flush(data, i2c->twsi_base + TWSI_INT(i2c));
+}
+
+/* Prototypes */
+irqreturn_t octeon_i2c_isr(int irq, void *dev_id);
+int octeon_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg *msgs, int num);
+int octeon_i2c_init_lowlevel(struct octeon_i2c *i2c);
+void octeon_i2c_set_clock(struct octeon_i2c *i2c);
+extern struct i2c_bus_recovery_info octeon_i2c_recovery_info;
diff --git a/drivers/i2c/busses/i2c-octeon-platdrv.c b/drivers/i2c/busses/i2c-octeon-platdrv.c
new file mode 100644
index 000000000000..917524ce6890
--- /dev/null
+++ b/drivers/i2c/busses/i2c-octeon-platdrv.c
@@ -0,0 +1,286 @@
+/*
+ * (C) Copyright 2009-2010
+ * Nokia Siemens Networks, michael.lawnick.ext@nsn.com
+ *
+ * Portions Copyright (C) 2010 - 2016 Cavium, Inc.
+ *
+ * This is a driver for the i2c adapter in Cavium Networks' OCTEON processors.
+ *
+ * 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/atomic.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+
+#include <asm/octeon/octeon.h>
+#include "i2c-octeon-core.h"
+
+#define DRV_NAME "i2c-octeon"
+
+/**
+ * octeon_i2c_int_enable - enable the CORE interrupt
+ * @i2c: The struct octeon_i2c
+ *
+ * The interrupt will be asserted when there is non-STAT_IDLE state in
+ * the SW_TWSI_EOP_TWSI_STAT register.
+ */
+static void octeon_i2c_int_enable(struct octeon_i2c *i2c)
+{
+	octeon_i2c_write_int(i2c, TWSI_INT_CORE_EN);
+}
+
+/* disable the CORE interrupt */
+static void octeon_i2c_int_disable(struct octeon_i2c *i2c)
+{
+	/* clear TS/ST/IFLG events */
+	octeon_i2c_write_int(i2c, 0);
+}
+
+/**
+ * octeon_i2c_int_enable78 - enable the CORE interrupt
+ * @i2c: The struct octeon_i2c
+ *
+ * The interrupt will be asserted when there is non-STAT_IDLE state in the
+ * SW_TWSI_EOP_TWSI_STAT register.
+ */
+static void octeon_i2c_int_enable78(struct octeon_i2c *i2c)
+{
+	atomic_inc_return(&i2c->int_enable_cnt);
+	enable_irq(i2c->irq);
+}
+
+static void __octeon_i2c_irq_disable(atomic_t *cnt, int irq)
+{
+	int count;
+
+	/*
+	 * The interrupt can be disabled in two places, but we only
+	 * want to make the disable_irq_nosync() call once, so keep
+	 * track with the atomic variable.
+	 */
+	count = atomic_dec_if_positive(cnt);
+	if (count >= 0)
+		disable_irq_nosync(irq);
+}
+
+/* disable the CORE interrupt */
+static void octeon_i2c_int_disable78(struct octeon_i2c *i2c)
+{
+	__octeon_i2c_irq_disable(&i2c->int_enable_cnt, i2c->irq);
+}
+
+/**
+ * octeon_i2c_hlc_int_enable78 - enable the ST interrupt
+ * @i2c: The struct octeon_i2c
+ *
+ * The interrupt will be asserted when there is non-STAT_IDLE state in
+ * the SW_TWSI_EOP_TWSI_STAT register.
+ */
+static void octeon_i2c_hlc_int_enable78(struct octeon_i2c *i2c)
+{
+	atomic_inc_return(&i2c->hlc_int_enable_cnt);
+	enable_irq(i2c->hlc_irq);
+}
+
+/* disable the ST interrupt */
+static void octeon_i2c_hlc_int_disable78(struct octeon_i2c *i2c)
+{
+	__octeon_i2c_irq_disable(&i2c->hlc_int_enable_cnt, i2c->hlc_irq);
+}
+
+/* HLC interrupt service routine */
+static irqreturn_t octeon_i2c_hlc_isr78(int irq, void *dev_id)
+{
+	struct octeon_i2c *i2c = dev_id;
+
+	i2c->hlc_int_disable(i2c);
+	wake_up(&i2c->queue);
+
+	return IRQ_HANDLED;
+}
+
+static void octeon_i2c_hlc_int_enable(struct octeon_i2c *i2c)
+{
+	octeon_i2c_write_int(i2c, TWSI_INT_ST_EN);
+}
+
+static u32 octeon_i2c_functionality(struct i2c_adapter *adap)
+{
+	return I2C_FUNC_I2C | (I2C_FUNC_SMBUS_EMUL & ~I2C_FUNC_SMBUS_QUICK) |
+	       I2C_FUNC_SMBUS_READ_BLOCK_DATA | I2C_SMBUS_BLOCK_PROC_CALL;
+}
+
+static const struct i2c_algorithm octeon_i2c_algo = {
+	.master_xfer = octeon_i2c_xfer,
+	.functionality = octeon_i2c_functionality,
+};
+
+static struct i2c_adapter octeon_i2c_ops = {
+	.owner = THIS_MODULE,
+	.name = "OCTEON adapter",
+	.algo = &octeon_i2c_algo,
+};
+
+static int octeon_i2c_probe(struct platform_device *pdev)
+{
+	struct device_node *node = pdev->dev.of_node;
+	int irq, result = 0, hlc_irq = 0;
+	struct resource *res_mem;
+	struct octeon_i2c *i2c;
+	bool cn78xx_style;
+
+	cn78xx_style = of_device_is_compatible(node, "cavium,octeon-7890-twsi");
+	if (cn78xx_style) {
+		hlc_irq = platform_get_irq(pdev, 0);
+		if (hlc_irq < 0)
+			return hlc_irq;
+
+		irq = platform_get_irq(pdev, 2);
+		if (irq < 0)
+			return irq;
+	} else {
+		/* All adaptors have an irq.  */
+		irq = platform_get_irq(pdev, 0);
+		if (irq < 0)
+			return irq;
+	}
+
+	i2c = devm_kzalloc(&pdev->dev, sizeof(*i2c), GFP_KERNEL);
+	if (!i2c) {
+		result = -ENOMEM;
+		goto out;
+	}
+	i2c->dev = &pdev->dev;
+
+	i2c->roff.sw_twsi = 0x00;
+	i2c->roff.twsi_int = 0x10;
+	i2c->roff.sw_twsi_ext = 0x18;
+
+	res_mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	i2c->twsi_base = devm_ioremap_resource(&pdev->dev, res_mem);
+	if (IS_ERR(i2c->twsi_base)) {
+		result = PTR_ERR(i2c->twsi_base);
+		goto out;
+	}
+
+	/*
+	 * "clock-rate" is a legacy binding, the official binding is
+	 * "clock-frequency".  Try the official one first and then
+	 * fall back if it doesn't exist.
+	 */
+	if (of_property_read_u32(node, "clock-frequency", &i2c->twsi_freq) &&
+	    of_property_read_u32(node, "clock-rate", &i2c->twsi_freq)) {
+		dev_err(i2c->dev,
+			"no I2C 'clock-rate' or 'clock-frequency' property\n");
+		result = -ENXIO;
+		goto out;
+	}
+
+	i2c->sys_freq = octeon_get_io_clock_rate();
+
+	init_waitqueue_head(&i2c->queue);
+
+	i2c->irq = irq;
+
+	if (cn78xx_style) {
+		i2c->hlc_irq = hlc_irq;
+
+		i2c->int_enable = octeon_i2c_int_enable78;
+		i2c->int_disable = octeon_i2c_int_disable78;
+		i2c->hlc_int_enable = octeon_i2c_hlc_int_enable78;
+		i2c->hlc_int_disable = octeon_i2c_hlc_int_disable78;
+
+		irq_set_status_flags(i2c->irq, IRQ_NOAUTOEN);
+		irq_set_status_flags(i2c->hlc_irq, IRQ_NOAUTOEN);
+
+		result = devm_request_irq(&pdev->dev, i2c->hlc_irq,
+					  octeon_i2c_hlc_isr78, 0,
+					  DRV_NAME, i2c);
+		if (result < 0) {
+			dev_err(i2c->dev, "failed to attach interrupt\n");
+			goto out;
+		}
+	} else {
+		i2c->int_enable = octeon_i2c_int_enable;
+		i2c->int_disable = octeon_i2c_int_disable;
+		i2c->hlc_int_enable = octeon_i2c_hlc_int_enable;
+		i2c->hlc_int_disable = octeon_i2c_int_disable;
+	}
+
+	result = devm_request_irq(&pdev->dev, i2c->irq,
+				  octeon_i2c_isr, 0, DRV_NAME, i2c);
+	if (result < 0) {
+		dev_err(i2c->dev, "failed to attach interrupt\n");
+		goto out;
+	}
+
+	if (OCTEON_IS_MODEL(OCTEON_CN38XX))
+		i2c->broken_irq_check = true;
+
+	result = octeon_i2c_init_lowlevel(i2c);
+	if (result) {
+		dev_err(i2c->dev, "init low level failed\n");
+		goto  out;
+	}
+
+	octeon_i2c_set_clock(i2c);
+
+	i2c->adap = octeon_i2c_ops;
+	i2c->adap.timeout = msecs_to_jiffies(2);
+	i2c->adap.retries = 5;
+	i2c->adap.bus_recovery_info = &octeon_i2c_recovery_info;
+	i2c->adap.dev.parent = &pdev->dev;
+	i2c->adap.dev.of_node = node;
+	i2c_set_adapdata(&i2c->adap, i2c);
+	platform_set_drvdata(pdev, i2c);
+
+	result = i2c_add_adapter(&i2c->adap);
+	if (result < 0)
+		goto out;
+	dev_info(i2c->dev, "probed\n");
+	return 0;
+
+out:
+	return result;
+};
+
+static int octeon_i2c_remove(struct platform_device *pdev)
+{
+	struct octeon_i2c *i2c = platform_get_drvdata(pdev);
+
+	i2c_del_adapter(&i2c->adap);
+	return 0;
+};
+
+static const struct of_device_id octeon_i2c_match[] = {
+	{ .compatible = "cavium,octeon-3860-twsi", },
+	{ .compatible = "cavium,octeon-7890-twsi", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, octeon_i2c_match);
+
+static struct platform_driver octeon_i2c_driver = {
+	.probe		= octeon_i2c_probe,
+	.remove		= octeon_i2c_remove,
+	.driver		= {
+		.name	= DRV_NAME,
+		.of_match_table = octeon_i2c_match,
+	},
+};
+
+module_platform_driver(octeon_i2c_driver);
+
+MODULE_AUTHOR("Michael Lawnick <michael.lawnick.ext@nsn.com>");
+MODULE_DESCRIPTION("I2C-Bus adapter for Cavium OCTEON processors");
+MODULE_LICENSE("GPL");
diff --git a/drivers/i2c/busses/i2c-omap.c b/drivers/i2c/busses/i2c-omap.c
index ab1279b8e240..c7da0c42baee 100644
--- a/drivers/i2c/busses/i2c-omap.c
+++ b/drivers/i2c/busses/i2c-omap.c
@@ -1425,10 +1425,8 @@ omap_i2c_probe(struct platform_device *pdev)
 	/* i2c device drivers may be active on return from add_adapter() */
 	adap->nr = pdev->id;
 	r = i2c_add_numbered_adapter(adap);
-	if (r) {
-		dev_err(omap->dev, "failure adding adapter\n");
+	if (r)
 		goto err_unuse_clocks;
-	}
 
 	dev_info(omap->dev, "bus %d rev%d.%d at %d kHz\n", adap->nr,
 		 major, minor, omap->speed);
diff --git a/drivers/i2c/busses/i2c-piix4.c b/drivers/i2c/busses/i2c-piix4.c
index 23d1c167b5d7..c2268cdf38e8 100644
--- a/drivers/i2c/busses/i2c-piix4.c
+++ b/drivers/i2c/busses/i2c-piix4.c
@@ -694,7 +694,6 @@ static int piix4_add_adapter(struct pci_dev *dev, unsigned short smba,
 
 	retval = i2c_add_adapter(adap);
 	if (retval) {
-		dev_err(&dev->dev, "Couldn't register adapter!\n");
 		kfree(adapdata);
 		kfree(adap);
 		release_region(smba, SMBIOSIZE);
diff --git a/drivers/i2c/busses/i2c-pmcmsp.c b/drivers/i2c/busses/i2c-pmcmsp.c
index 2c40edbf6224..217c78711d65 100644
--- a/drivers/i2c/busses/i2c-pmcmsp.c
+++ b/drivers/i2c/busses/i2c-pmcmsp.c
@@ -329,10 +329,8 @@ static int pmcmsptwi_probe(struct platform_device *pldev)
 	i2c_set_adapdata(&pmcmsptwi_adapter, &pmcmsptwi_data);
 
 	rc = i2c_add_adapter(&pmcmsptwi_adapter);
-	if (rc) {
-		dev_err(&pldev->dev, "Unable to register I2C adapter\n");
+	if (rc)
 		goto ret_unmap;
-	}
 
 	return 0;
 
diff --git a/drivers/i2c/busses/i2c-pnx.c b/drivers/i2c/busses/i2c-pnx.c
index 7ea67aa46fb7..fd5f9d2bf6d9 100644
--- a/drivers/i2c/busses/i2c-pnx.c
+++ b/drivers/i2c/busses/i2c-pnx.c
@@ -714,10 +714,8 @@ static int i2c_pnx_probe(struct platform_device *pdev)
 
 	/* Register this adapter with the I2C subsystem */
 	ret = i2c_add_numbered_adapter(&alg_data->adapter);
-	if (ret < 0) {
-		dev_err(&pdev->dev, "I2C: Failed to add bus\n");
+	if (ret < 0)
 		goto out_clock;
-	}
 
 	dev_dbg(&pdev->dev, "%s: Master at %#8x, irq %d.\n",
 		alg_data->adapter.name, res->start, alg_data->irq);
diff --git a/drivers/i2c/busses/i2c-puv3.c b/drivers/i2c/busses/i2c-puv3.c
index 82b6f02544da..0c8b1571886d 100644
--- a/drivers/i2c/busses/i2c-puv3.c
+++ b/drivers/i2c/busses/i2c-puv3.c
@@ -212,11 +212,8 @@ static int puv3_i2c_probe(struct platform_device *pdev)
 
 	adapter->nr = pdev->id;
 	rc = i2c_add_numbered_adapter(adapter);
-	if (rc) {
-		dev_err(&pdev->dev, "Adapter '%s' registration failed\n",
-				adapter->name);
+	if (rc)
 		goto fail_add_adapter;
-	}
 
 	dev_info(&pdev->dev, "PKUnity v3 i2c bus adapter.\n");
 	return 0;
diff --git a/drivers/i2c/busses/i2c-pxa.c b/drivers/i2c/busses/i2c-pxa.c
index 0d351954db02..e28b825b0433 100644
--- a/drivers/i2c/busses/i2c-pxa.c
+++ b/drivers/i2c/busses/i2c-pxa.c
@@ -1292,10 +1292,8 @@ static int i2c_pxa_probe(struct platform_device *dev)
 #endif
 
 	ret = i2c_add_numbered_adapter(&i2c->adap);
-	if (ret < 0) {
-		dev_err(&dev->dev, "failed to add bus: %d\n", ret);
+	if (ret < 0)
 		goto ereqirq;
-	}
 
 	platform_set_drvdata(dev, i2c);
 
diff --git a/drivers/i2c/busses/i2c-rcar.c b/drivers/i2c/busses/i2c-rcar.c
index 9bd849dacee8..726615e54f2a 100644
--- a/drivers/i2c/busses/i2c-rcar.c
+++ b/drivers/i2c/busses/i2c-rcar.c
@@ -802,6 +802,7 @@ static const struct of_device_id rcar_i2c_dt_ids[] = {
 	{ .compatible = "renesas,i2c-r8a7793", .data = (void *)I2C_RCAR_GEN2 },
 	{ .compatible = "renesas,i2c-r8a7794", .data = (void *)I2C_RCAR_GEN2 },
 	{ .compatible = "renesas,i2c-r8a7795", .data = (void *)I2C_RCAR_GEN3 },
+	{ .compatible = "renesas,i2c-r8a7796", .data = (void *)I2C_RCAR_GEN3 },
 	{},
 };
 MODULE_DEVICE_TABLE(of, rcar_i2c_dt_ids);
@@ -875,10 +876,8 @@ static int rcar_i2c_probe(struct platform_device *pdev)
 	platform_set_drvdata(pdev, priv);
 
 	ret = i2c_add_numbered_adapter(adap);
-	if (ret < 0) {
-		dev_err(dev, "reg adap failed: %d\n", ret);
+	if (ret < 0)
 		goto out_pm_disable;
-	}
 
 	dev_info(dev, "probed\n");
 
diff --git a/drivers/i2c/busses/i2c-riic.c b/drivers/i2c/busses/i2c-riic.c
index d7e3af671543..6263ea82d6ac 100644
--- a/drivers/i2c/busses/i2c-riic.c
+++ b/drivers/i2c/busses/i2c-riic.c
@@ -383,10 +383,8 @@ static int riic_i2c_probe(struct platform_device *pdev)
 
 
 	ret = i2c_add_adapter(adap);
-	if (ret) {
-		dev_err(&pdev->dev, "failed to add adapter\n");
+	if (ret)
 		return ret;
-	}
 
 	platform_set_drvdata(pdev, riic);
 
diff --git a/drivers/i2c/busses/i2c-rk3x.c b/drivers/i2c/busses/i2c-rk3x.c
index 5c5b7cada8be..50702c7bb244 100644
--- a/drivers/i2c/busses/i2c-rk3x.c
+++ b/drivers/i2c/busses/i2c-rk3x.c
@@ -58,7 +58,7 @@ enum {
 #define REG_CON_LASTACK   BIT(5) /* 1: send NACK after last received byte */
 #define REG_CON_ACTACK    BIT(6) /* 1: stop if NACK is received */
 
-#define REG_CON_TUNING_MASK GENMASK(15, 8)
+#define REG_CON_TUNING_MASK GENMASK_ULL(15, 8)
 
 #define REG_CON_SDA_CFG(cfg) ((cfg) << 8)
 #define REG_CON_STA_CFG(cfg) ((cfg) << 12)
@@ -742,7 +742,7 @@ static int rk3x_i2c_v1_calc_timings(unsigned long clk_rate,
 				    struct i2c_timings *t,
 				    struct rk3x_i2c_calced_timings *t_calc)
 {
-	unsigned long min_low_ns, min_high_ns, min_total_ns;
+	unsigned long min_low_ns, min_high_ns;
 	unsigned long min_setup_start_ns, min_setup_data_ns;
 	unsigned long min_setup_stop_ns, max_hold_data_ns;
 
@@ -793,7 +793,6 @@ static int rk3x_i2c_v1_calc_timings(unsigned long clk_rate,
 
 	/* These are the min dividers needed for min hold times. */
 	min_div_for_hold = (min_low_div + min_high_div);
-	min_total_ns = min_low_ns + min_high_ns;
 
 	/*
 	 * This is the maximum divider so we don't go over the maximum.
@@ -1312,10 +1311,8 @@ static int rk3x_i2c_probe(struct platform_device *pdev)
 	rk3x_i2c_adapt_div(i2c, clk_rate);
 
 	ret = i2c_add_adapter(&i2c->adap);
-	if (ret < 0) {
-		dev_err(&pdev->dev, "Could not register adapter\n");
+	if (ret < 0)
 		goto err_clk_notifier;
-	}
 
 	dev_info(&pdev->dev, "Initialized RK3xxx I2C bus at %p\n", i2c->regs);
 
diff --git a/drivers/i2c/busses/i2c-s3c2410.c b/drivers/i2c/busses/i2c-s3c2410.c
index 38dc1cacfd8b..499af26e736e 100644
--- a/drivers/i2c/busses/i2c-s3c2410.c
+++ b/drivers/i2c/busses/i2c-s3c2410.c
@@ -1215,7 +1215,6 @@ static int s3c24xx_i2c_probe(struct platform_device *pdev)
 
 	ret = i2c_add_numbered_adapter(&i2c->adap);
 	if (ret < 0) {
-		dev_err(&pdev->dev, "failed to add bus to i2c core\n");
 		pm_runtime_disable(&pdev->dev);
 		s3c24xx_i2c_deregister_cpufreq(i2c);
 		clk_unprepare(i2c->clk);
diff --git a/drivers/i2c/busses/i2c-sh7760.c b/drivers/i2c/busses/i2c-sh7760.c
index 24968384b401..c2005c789d2b 100644
--- a/drivers/i2c/busses/i2c-sh7760.c
+++ b/drivers/i2c/busses/i2c-sh7760.c
@@ -510,10 +510,8 @@ static int sh7760_i2c_probe(struct platform_device *pdev)
 	}
 
 	ret = i2c_add_numbered_adapter(&id->adap);
-	if (ret < 0) {
-		dev_err(&pdev->dev, "reg adap failed: %d\n", ret);
+	if (ret < 0)
 		goto out4;
-	}
 
 	platform_set_drvdata(pdev, id);
 
diff --git a/drivers/i2c/busses/i2c-sh_mobile.c b/drivers/i2c/busses/i2c-sh_mobile.c
index 05b1eeab9cf5..192f36f00e4d 100644
--- a/drivers/i2c/busses/i2c-sh_mobile.c
+++ b/drivers/i2c/busses/i2c-sh_mobile.c
@@ -981,7 +981,6 @@ static int sh_mobile_i2c_probe(struct platform_device *dev)
 	ret = i2c_add_numbered_adapter(adap);
 	if (ret < 0) {
 		sh_mobile_i2c_release_dma(pd);
-		dev_err(&dev->dev, "cannot add numbered adapter\n");
 		return ret;
 	}
 
diff --git a/drivers/i2c/busses/i2c-sirf.c b/drivers/i2c/busses/i2c-sirf.c
index 792a42bdd335..95e81d0f72b4 100644
--- a/drivers/i2c/busses/i2c-sirf.c
+++ b/drivers/i2c/busses/i2c-sirf.c
@@ -387,10 +387,8 @@ static int i2c_sirfsoc_probe(struct platform_device *pdev)
 		writel(regval, siic->base + SIRFSOC_I2C_SDA_DELAY);
 
 	err = i2c_add_numbered_adapter(adap);
-	if (err < 0) {
-		dev_err(&pdev->dev, "Can't add new i2c adapter\n");
+	if (err < 0)
 		goto out;
-	}
 
 	clk_disable(clk);
 
diff --git a/drivers/i2c/busses/i2c-st.c b/drivers/i2c/busses/i2c-st.c
index 944ec4205084..1371547ce1a3 100644
--- a/drivers/i2c/busses/i2c-st.c
+++ b/drivers/i2c/busses/i2c-st.c
@@ -874,10 +874,8 @@ static int st_i2c_probe(struct platform_device *pdev)
 	init_completion(&i2c_dev->complete);
 
 	ret = i2c_add_adapter(adap);
-	if (ret) {
-		dev_err(&pdev->dev, "Failed to add adapter\n");
+	if (ret)
 		return ret;
-	}
 
 	platform_set_drvdata(pdev, i2c_dev);
 
diff --git a/drivers/i2c/busses/i2c-stu300.c b/drivers/i2c/busses/i2c-stu300.c
index 460c134832ac..dc63236b45b2 100644
--- a/drivers/i2c/busses/i2c-stu300.c
+++ b/drivers/i2c/busses/i2c-stu300.c
@@ -920,11 +920,8 @@ static int stu300_probe(struct platform_device *pdev)
 
 	/* i2c device drivers may be active on return from add_adapter() */
 	ret = i2c_add_numbered_adapter(adap);
-	if (ret) {
-		dev_err(&pdev->dev, "failure adding ST Micro DDC "
-		       "I2C adapter\n");
+	if (ret)
 		return ret;
-	}
 
 	platform_set_drvdata(pdev, dev);
 	dev_info(&pdev->dev, "ST DDC I2C @ %p, irq %d\n",
diff --git a/drivers/i2c/busses/i2c-tegra.c b/drivers/i2c/busses/i2c-tegra.c
index b126dbaa47e3..4af9bbae20df 100644
--- a/drivers/i2c/busses/i2c-tegra.c
+++ b/drivers/i2c/busses/i2c-tegra.c
@@ -28,6 +28,9 @@
 #include <linux/of_device.h>
 #include <linux/module.h>
 #include <linux/reset.h>
+#include <linux/pinctrl/consumer.h>
+#include <linux/pm_runtime.h>
+#include <linux/iopoll.h>
 
 #include <asm/unaligned.h>
 
@@ -36,21 +39,21 @@
 
 #define I2C_CNFG				0x000
 #define I2C_CNFG_DEBOUNCE_CNT_SHIFT		12
-#define I2C_CNFG_PACKET_MODE_EN			(1<<10)
-#define I2C_CNFG_NEW_MASTER_FSM			(1<<11)
-#define I2C_CNFG_MULTI_MASTER_MODE		(1<<17)
+#define I2C_CNFG_PACKET_MODE_EN			BIT(10)
+#define I2C_CNFG_NEW_MASTER_FSM			BIT(11)
+#define I2C_CNFG_MULTI_MASTER_MODE		BIT(17)
 #define I2C_STATUS				0x01C
 #define I2C_SL_CNFG				0x020
-#define I2C_SL_CNFG_NACK			(1<<1)
-#define I2C_SL_CNFG_NEWSL			(1<<2)
+#define I2C_SL_CNFG_NACK			BIT(1)
+#define I2C_SL_CNFG_NEWSL			BIT(2)
 #define I2C_SL_ADDR1				0x02c
 #define I2C_SL_ADDR2				0x030
 #define I2C_TX_FIFO				0x050
 #define I2C_RX_FIFO				0x054
 #define I2C_PACKET_TRANSFER_STATUS		0x058
 #define I2C_FIFO_CONTROL			0x05c
-#define I2C_FIFO_CONTROL_TX_FLUSH		(1<<1)
-#define I2C_FIFO_CONTROL_RX_FLUSH		(1<<0)
+#define I2C_FIFO_CONTROL_TX_FLUSH		BIT(1)
+#define I2C_FIFO_CONTROL_RX_FLUSH		BIT(0)
 #define I2C_FIFO_CONTROL_TX_TRIG_SHIFT		5
 #define I2C_FIFO_CONTROL_RX_TRIG_SHIFT		2
 #define I2C_FIFO_STATUS				0x060
@@ -60,26 +63,26 @@
 #define I2C_FIFO_STATUS_RX_SHIFT		0
 #define I2C_INT_MASK				0x064
 #define I2C_INT_STATUS				0x068
-#define I2C_INT_PACKET_XFER_COMPLETE		(1<<7)
-#define I2C_INT_ALL_PACKETS_XFER_COMPLETE	(1<<6)
-#define I2C_INT_TX_FIFO_OVERFLOW		(1<<5)
-#define I2C_INT_RX_FIFO_UNDERFLOW		(1<<4)
-#define I2C_INT_NO_ACK				(1<<3)
-#define I2C_INT_ARBITRATION_LOST		(1<<2)
-#define I2C_INT_TX_FIFO_DATA_REQ		(1<<1)
-#define I2C_INT_RX_FIFO_DATA_REQ		(1<<0)
+#define I2C_INT_PACKET_XFER_COMPLETE		BIT(7)
+#define I2C_INT_ALL_PACKETS_XFER_COMPLETE	BIT(6)
+#define I2C_INT_TX_FIFO_OVERFLOW		BIT(5)
+#define I2C_INT_RX_FIFO_UNDERFLOW		BIT(4)
+#define I2C_INT_NO_ACK				BIT(3)
+#define I2C_INT_ARBITRATION_LOST		BIT(2)
+#define I2C_INT_TX_FIFO_DATA_REQ		BIT(1)
+#define I2C_INT_RX_FIFO_DATA_REQ		BIT(0)
 #define I2C_CLK_DIVISOR				0x06c
 #define I2C_CLK_DIVISOR_STD_FAST_MODE_SHIFT	16
 #define I2C_CLK_MULTIPLIER_STD_FAST_MODE	8
 
 #define DVC_CTRL_REG1				0x000
-#define DVC_CTRL_REG1_INTR_EN			(1<<10)
+#define DVC_CTRL_REG1_INTR_EN			BIT(10)
 #define DVC_CTRL_REG2				0x004
 #define DVC_CTRL_REG3				0x008
-#define DVC_CTRL_REG3_SW_PROG			(1<<26)
-#define DVC_CTRL_REG3_I2C_DONE_INTR_EN		(1<<30)
+#define DVC_CTRL_REG3_SW_PROG			BIT(26)
+#define DVC_CTRL_REG3_I2C_DONE_INTR_EN		BIT(30)
 #define DVC_STATUS				0x00c
-#define DVC_STATUS_I2C_DONE_INTR		(1<<30)
+#define DVC_STATUS_I2C_DONE_INTR		BIT(30)
 
 #define I2C_ERR_NONE				0x00
 #define I2C_ERR_NO_ACK				0x01
@@ -89,26 +92,28 @@
 #define PACKET_HEADER0_HEADER_SIZE_SHIFT	28
 #define PACKET_HEADER0_PACKET_ID_SHIFT		16
 #define PACKET_HEADER0_CONT_ID_SHIFT		12
-#define PACKET_HEADER0_PROTOCOL_I2C		(1<<4)
-
-#define I2C_HEADER_HIGHSPEED_MODE		(1<<22)
-#define I2C_HEADER_CONT_ON_NAK			(1<<21)
-#define I2C_HEADER_SEND_START_BYTE		(1<<20)
-#define I2C_HEADER_READ				(1<<19)
-#define I2C_HEADER_10BIT_ADDR			(1<<18)
-#define I2C_HEADER_IE_ENABLE			(1<<17)
-#define I2C_HEADER_REPEAT_START			(1<<16)
-#define I2C_HEADER_CONTINUE_XFER		(1<<15)
+#define PACKET_HEADER0_PROTOCOL_I2C		BIT(4)
+
+#define I2C_HEADER_HIGHSPEED_MODE		BIT(22)
+#define I2C_HEADER_CONT_ON_NAK			BIT(21)
+#define I2C_HEADER_SEND_START_BYTE		BIT(20)
+#define I2C_HEADER_READ				BIT(19)
+#define I2C_HEADER_10BIT_ADDR			BIT(18)
+#define I2C_HEADER_IE_ENABLE			BIT(17)
+#define I2C_HEADER_REPEAT_START			BIT(16)
+#define I2C_HEADER_CONTINUE_XFER		BIT(15)
 #define I2C_HEADER_MASTER_ADDR_SHIFT		12
 #define I2C_HEADER_SLAVE_ADDR_SHIFT		1
 
 #define I2C_CONFIG_LOAD				0x08C
-#define I2C_MSTR_CONFIG_LOAD			(1 << 0)
-#define I2C_SLV_CONFIG_LOAD			(1 << 1)
-#define I2C_TIMEOUT_CONFIG_LOAD			(1 << 2)
+#define I2C_MSTR_CONFIG_LOAD			BIT(0)
+#define I2C_SLV_CONFIG_LOAD			BIT(1)
+#define I2C_TIMEOUT_CONFIG_LOAD			BIT(2)
 
 #define I2C_CLKEN_OVERRIDE			0x090
-#define I2C_MST_CORE_CLKEN_OVR			(1 << 0)
+#define I2C_MST_CORE_CLKEN_OVR			BIT(0)
+
+#define I2C_CONFIG_LOAD_TIMEOUT			1000000
 
 /*
  * msg_end_type: The bus control which need to be send at end of transfer.
@@ -191,9 +196,11 @@ struct tegra_i2c_dev {
 	u16 clk_divisor_non_hs_mode;
 	bool is_suspended;
 	bool is_multimaster_mode;
+	spinlock_t xfer_lock;
 };
 
-static void dvc_writel(struct tegra_i2c_dev *i2c_dev, u32 val, unsigned long reg)
+static void dvc_writel(struct tegra_i2c_dev *i2c_dev, u32 val,
+		       unsigned long reg)
 {
 	writel(val, i2c_dev->base + reg);
 }
@@ -244,15 +251,17 @@ static void i2c_readsl(struct tegra_i2c_dev *i2c_dev, void *data,
 
 static void tegra_i2c_mask_irq(struct tegra_i2c_dev *i2c_dev, u32 mask)
 {
-	u32 int_mask = i2c_readl(i2c_dev, I2C_INT_MASK);
-	int_mask &= ~mask;
+	u32 int_mask;
+
+	int_mask = i2c_readl(i2c_dev, I2C_INT_MASK) & ~mask;
 	i2c_writel(i2c_dev, int_mask, I2C_INT_MASK);
 }
 
 static void tegra_i2c_unmask_irq(struct tegra_i2c_dev *i2c_dev, u32 mask)
 {
-	u32 int_mask = i2c_readl(i2c_dev, I2C_INT_MASK);
-	int_mask |= mask;
+	u32 int_mask;
+
+	int_mask = i2c_readl(i2c_dev, I2C_INT_MASK) | mask;
 	i2c_writel(i2c_dev, int_mask, I2C_INT_MASK);
 }
 
@@ -260,6 +269,7 @@ static int tegra_i2c_flush_fifos(struct tegra_i2c_dev *i2c_dev)
 {
 	unsigned long timeout = jiffies + HZ;
 	u32 val = i2c_readl(i2c_dev, I2C_FIFO_CONTROL);
+
 	val |= I2C_FIFO_CONTROL_TX_FLUSH | I2C_FIFO_CONTROL_RX_FLUSH;
 	i2c_writel(i2c_dev, val, I2C_FIFO_CONTROL);
 
@@ -385,7 +395,8 @@ static int tegra_i2c_fill_tx_fifo(struct tegra_i2c_dev *i2c_dev)
  */
 static void tegra_dvc_init(struct tegra_i2c_dev *i2c_dev)
 {
-	u32 val = 0;
+	u32 val;
+
 	val = dvc_readl(i2c_dev, DVC_CTRL_REG3);
 	val |= DVC_CTRL_REG3_SW_PROG;
 	val |= DVC_CTRL_REG3_I2C_DONE_INTR_EN;
@@ -396,9 +407,15 @@ static void tegra_dvc_init(struct tegra_i2c_dev *i2c_dev)
 	dvc_writel(i2c_dev, val, DVC_CTRL_REG1);
 }
 
-static inline int tegra_i2c_clock_enable(struct tegra_i2c_dev *i2c_dev)
+static int tegra_i2c_runtime_resume(struct device *dev)
 {
+	struct tegra_i2c_dev *i2c_dev = dev_get_drvdata(dev);
 	int ret;
+
+	ret = pinctrl_pm_select_default_state(i2c_dev->dev);
+	if (ret)
+		return ret;
+
 	if (!i2c_dev->hw->has_single_clk_source) {
 		ret = clk_enable(i2c_dev->fast_clk);
 		if (ret < 0) {
@@ -407,32 +424,66 @@ static inline int tegra_i2c_clock_enable(struct tegra_i2c_dev *i2c_dev)
 			return ret;
 		}
 	}
+
 	ret = clk_enable(i2c_dev->div_clk);
 	if (ret < 0) {
 		dev_err(i2c_dev->dev,
 			"Enabling div clk failed, err %d\n", ret);
 		clk_disable(i2c_dev->fast_clk);
+		return ret;
 	}
-	return ret;
+
+	return 0;
 }
 
-static inline void tegra_i2c_clock_disable(struct tegra_i2c_dev *i2c_dev)
+static int tegra_i2c_runtime_suspend(struct device *dev)
 {
+	struct tegra_i2c_dev *i2c_dev = dev_get_drvdata(dev);
+
 	clk_disable(i2c_dev->div_clk);
 	if (!i2c_dev->hw->has_single_clk_source)
 		clk_disable(i2c_dev->fast_clk);
+
+	return pinctrl_pm_select_idle_state(i2c_dev->dev);
+}
+
+static int tegra_i2c_wait_for_config_load(struct tegra_i2c_dev *i2c_dev)
+{
+	unsigned long reg_offset;
+	void __iomem *addr;
+	u32 val;
+	int err;
+
+	if (i2c_dev->hw->has_config_load_reg) {
+		reg_offset = tegra_i2c_reg_addr(i2c_dev, I2C_CONFIG_LOAD);
+		addr = i2c_dev->base + reg_offset;
+		i2c_writel(i2c_dev, I2C_MSTR_CONFIG_LOAD, I2C_CONFIG_LOAD);
+		if (in_interrupt())
+			err = readl_poll_timeout_atomic(addr, val, val == 0,
+					1000, I2C_CONFIG_LOAD_TIMEOUT);
+		else
+			err = readl_poll_timeout(addr, val, val == 0,
+					1000, I2C_CONFIG_LOAD_TIMEOUT);
+
+		if (err) {
+			dev_warn(i2c_dev->dev,
+				 "timeout waiting for config load\n");
+			return err;
+		}
+	}
+
+	return 0;
 }
 
 static int tegra_i2c_init(struct tegra_i2c_dev *i2c_dev)
 {
 	u32 val;
-	int err = 0;
+	int err;
 	u32 clk_divisor;
-	unsigned long timeout = jiffies + HZ;
 
-	err = tegra_i2c_clock_enable(i2c_dev);
+	err = pm_runtime_get_sync(i2c_dev->dev);
 	if (err < 0) {
-		dev_err(i2c_dev->dev, "Clock enable failed %d\n", err);
+		dev_err(i2c_dev->dev, "runtime resume failed %d\n", err);
 		return err;
 	}
 
@@ -460,54 +511,59 @@ static int tegra_i2c_init(struct tegra_i2c_dev *i2c_dev)
 
 	if (!i2c_dev->is_dvc) {
 		u32 sl_cfg = i2c_readl(i2c_dev, I2C_SL_CNFG);
+
 		sl_cfg |= I2C_SL_CNFG_NACK | I2C_SL_CNFG_NEWSL;
 		i2c_writel(i2c_dev, sl_cfg, I2C_SL_CNFG);
 		i2c_writel(i2c_dev, 0xfc, I2C_SL_ADDR1);
 		i2c_writel(i2c_dev, 0x00, I2C_SL_ADDR2);
-
 	}
 
 	val = 7 << I2C_FIFO_CONTROL_TX_TRIG_SHIFT |
 		0 << I2C_FIFO_CONTROL_RX_TRIG_SHIFT;
 	i2c_writel(i2c_dev, val, I2C_FIFO_CONTROL);
 
-	if (tegra_i2c_flush_fifos(i2c_dev))
-		err = -ETIMEDOUT;
+	err = tegra_i2c_flush_fifos(i2c_dev);
+	if (err)
+		goto err;
 
 	if (i2c_dev->is_multimaster_mode && i2c_dev->hw->has_slcg_override_reg)
 		i2c_writel(i2c_dev, I2C_MST_CORE_CLKEN_OVR, I2C_CLKEN_OVERRIDE);
 
-	if (i2c_dev->hw->has_config_load_reg) {
-		i2c_writel(i2c_dev, I2C_MSTR_CONFIG_LOAD, I2C_CONFIG_LOAD);
-		while (i2c_readl(i2c_dev, I2C_CONFIG_LOAD) != 0) {
-			if (time_after(jiffies, timeout)) {
-				dev_warn(i2c_dev->dev,
-					"timeout waiting for config load\n");
-				err = -ETIMEDOUT;
-				goto err;
-			}
-			msleep(1);
-		}
-	}
+	err = tegra_i2c_wait_for_config_load(i2c_dev);
+	if (err)
+		goto err;
 
 	if (i2c_dev->irq_disabled) {
-		i2c_dev->irq_disabled = 0;
+		i2c_dev->irq_disabled = false;
 		enable_irq(i2c_dev->irq);
 	}
 
 err:
-	tegra_i2c_clock_disable(i2c_dev);
+	pm_runtime_put(i2c_dev->dev);
 	return err;
 }
 
+static int tegra_i2c_disable_packet_mode(struct tegra_i2c_dev *i2c_dev)
+{
+	u32 cnfg;
+
+	cnfg = i2c_readl(i2c_dev, I2C_CNFG);
+	if (cnfg & I2C_CNFG_PACKET_MODE_EN)
+		i2c_writel(i2c_dev, cnfg & ~I2C_CNFG_PACKET_MODE_EN, I2C_CNFG);
+
+	return tegra_i2c_wait_for_config_load(i2c_dev);
+}
+
 static irqreturn_t tegra_i2c_isr(int irq, void *dev_id)
 {
 	u32 status;
 	const u32 status_err = I2C_INT_NO_ACK | I2C_INT_ARBITRATION_LOST;
 	struct tegra_i2c_dev *i2c_dev = dev_id;
+	unsigned long flags;
 
 	status = i2c_readl(i2c_dev, I2C_INT_STATUS);
 
+	spin_lock_irqsave(&i2c_dev->xfer_lock, flags);
 	if (status == 0) {
 		dev_warn(i2c_dev->dev, "irq status 0 %08x %08x %08x\n",
 			 i2c_readl(i2c_dev, I2C_PACKET_TRANSFER_STATUS),
@@ -517,12 +573,13 @@ static irqreturn_t tegra_i2c_isr(int irq, void *dev_id)
 
 		if (!i2c_dev->irq_disabled) {
 			disable_irq_nosync(i2c_dev->irq);
-			i2c_dev->irq_disabled = 1;
+			i2c_dev->irq_disabled = true;
 		}
 		goto err;
 	}
 
 	if (unlikely(status & status_err)) {
+		tegra_i2c_disable_packet_mode(i2c_dev);
 		if (status & I2C_INT_NO_ACK)
 			i2c_dev->msg_err |= I2C_ERR_NO_ACK;
 		if (status & I2C_INT_ARBITRATION_LOST)
@@ -552,7 +609,7 @@ static irqreturn_t tegra_i2c_isr(int irq, void *dev_id)
 		BUG_ON(i2c_dev->msg_buf_remaining);
 		complete(&i2c_dev->msg_complete);
 	}
-	return IRQ_HANDLED;
+	goto done;
 err:
 	/* An error occurred, mask all interrupts */
 	tegra_i2c_mask_irq(i2c_dev, I2C_INT_NO_ACK | I2C_INT_ARBITRATION_LOST |
@@ -563,6 +620,8 @@ err:
 		dvc_writel(i2c_dev, DVC_STATUS_I2C_DONE_INTR, DVC_STATUS);
 
 	complete(&i2c_dev->msg_complete);
+done:
+	spin_unlock_irqrestore(&i2c_dev->xfer_lock, flags);
 	return IRQ_HANDLED;
 }
 
@@ -572,6 +631,7 @@ static int tegra_i2c_xfer_msg(struct tegra_i2c_dev *i2c_dev,
 	u32 packet_header;
 	u32 int_mask;
 	unsigned long time_left;
+	unsigned long flags;
 
 	tegra_i2c_flush_fifos(i2c_dev);
 
@@ -584,6 +644,11 @@ static int tegra_i2c_xfer_msg(struct tegra_i2c_dev *i2c_dev,
 	i2c_dev->msg_read = (msg->flags & I2C_M_RD);
 	reinit_completion(&i2c_dev->msg_complete);
 
+	spin_lock_irqsave(&i2c_dev->xfer_lock, flags);
+
+	int_mask = I2C_INT_NO_ACK | I2C_INT_ARBITRATION_LOST;
+	tegra_i2c_unmask_irq(i2c_dev, int_mask);
+
 	packet_header = (0 << PACKET_HEADER0_HEADER_SIZE_SHIFT) |
 			PACKET_HEADER0_PROTOCOL_I2C |
 			(i2c_dev->cont_id << PACKET_HEADER0_CONT_ID_SHIFT) |
@@ -613,14 +678,15 @@ static int tegra_i2c_xfer_msg(struct tegra_i2c_dev *i2c_dev,
 	if (!(msg->flags & I2C_M_RD))
 		tegra_i2c_fill_tx_fifo(i2c_dev);
 
-	int_mask = I2C_INT_NO_ACK | I2C_INT_ARBITRATION_LOST;
 	if (i2c_dev->hw->has_per_pkt_xfer_complete_irq)
 		int_mask |= I2C_INT_PACKET_XFER_COMPLETE;
 	if (msg->flags & I2C_M_RD)
 		int_mask |= I2C_INT_RX_FIFO_DATA_REQ;
 	else if (i2c_dev->msg_buf_remaining)
 		int_mask |= I2C_INT_TX_FIFO_DATA_REQ;
+
 	tegra_i2c_unmask_irq(i2c_dev, int_mask);
+	spin_unlock_irqrestore(&i2c_dev->xfer_lock, flags);
 	dev_dbg(i2c_dev->dev, "unmasked irq: %02x\n",
 		i2c_readl(i2c_dev, I2C_INT_MASK));
 
@@ -643,9 +709,10 @@ static int tegra_i2c_xfer_msg(struct tegra_i2c_dev *i2c_dev,
 		return 0;
 
 	/*
-	 * NACK interrupt is generated before the I2C controller generates the
-	 * STOP condition on the bus. So wait for 2 clock periods before resetting
-	 * the controller so that STOP condition has been delivered properly.
+	 * NACK interrupt is generated before the I2C controller generates
+	 * the STOP condition on the bus. So wait for 2 clock periods
+	 * before resetting the controller so that the STOP condition has
+	 * been delivered properly.
 	 */
 	if (i2c_dev->msg_err == I2C_ERR_NO_ACK)
 		udelay(DIV_ROUND_UP(2 * 1000000, i2c_dev->bus_clk_rate));
@@ -670,14 +737,15 @@ static int tegra_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg msgs[],
 	if (i2c_dev->is_suspended)
 		return -EBUSY;
 
-	ret = tegra_i2c_clock_enable(i2c_dev);
+	ret = pm_runtime_get_sync(i2c_dev->dev);
 	if (ret < 0) {
-		dev_err(i2c_dev->dev, "Clock enable failed %d\n", ret);
+		dev_err(i2c_dev->dev, "runtime resume failed %d\n", ret);
 		return ret;
 	}
 
 	for (i = 0; i < num; i++) {
 		enum msg_end_type end_type = MSG_END_STOP;
+
 		if (i < (num - 1)) {
 			if (msgs[i + 1].flags & I2C_M_NOSTART)
 				end_type = MSG_END_CONTINUE;
@@ -688,7 +756,9 @@ static int tegra_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg msgs[],
 		if (ret)
 			break;
 	}
-	tegra_i2c_clock_disable(i2c_dev);
+
+	pm_runtime_put(i2c_dev->dev);
+
 	return ret ?: i;
 }
 
@@ -825,7 +895,7 @@ static int tegra_i2c_probe(struct platform_device *pdev)
 
 	div_clk = devm_clk_get(&pdev->dev, "div-clk");
 	if (IS_ERR(div_clk)) {
-		dev_err(&pdev->dev, "missing controller clock");
+		dev_err(&pdev->dev, "missing controller clock\n");
 		return PTR_ERR(div_clk);
 	}
 
@@ -843,27 +913,22 @@ static int tegra_i2c_probe(struct platform_device *pdev)
 
 	i2c_dev->rst = devm_reset_control_get(&pdev->dev, "i2c");
 	if (IS_ERR(i2c_dev->rst)) {
-		dev_err(&pdev->dev, "missing controller reset");
+		dev_err(&pdev->dev, "missing controller reset\n");
 		return PTR_ERR(i2c_dev->rst);
 	}
 
 	tegra_i2c_parse_dt(i2c_dev);
 
-	i2c_dev->hw = &tegra20_i2c_hw;
-
-	if (pdev->dev.of_node) {
-		i2c_dev->hw = of_device_get_match_data(&pdev->dev);
-		i2c_dev->is_dvc = of_device_is_compatible(pdev->dev.of_node,
-						"nvidia,tegra20-i2c-dvc");
-	} else if (pdev->id == 3) {
-		i2c_dev->is_dvc = 1;
-	}
+	i2c_dev->hw = of_device_get_match_data(&pdev->dev);
+	i2c_dev->is_dvc = of_device_is_compatible(pdev->dev.of_node,
+						  "nvidia,tegra20-i2c-dvc");
 	init_completion(&i2c_dev->msg_complete);
+	spin_lock_init(&i2c_dev->xfer_lock);
 
 	if (!i2c_dev->hw->has_single_clk_source) {
 		fast_clk = devm_clk_get(&pdev->dev, "fast-clk");
 		if (IS_ERR(fast_clk)) {
-			dev_err(&pdev->dev, "missing fast clock");
+			dev_err(&pdev->dev, "missing fast clock\n");
 			return PTR_ERR(fast_clk);
 		}
 		i2c_dev->fast_clk = fast_clk;
@@ -900,18 +965,27 @@ static int tegra_i2c_probe(struct platform_device *pdev)
 		goto unprepare_fast_clk;
 	}
 
+	pm_runtime_enable(&pdev->dev);
+	if (!pm_runtime_enabled(&pdev->dev)) {
+		ret = tegra_i2c_runtime_resume(&pdev->dev);
+		if (ret < 0) {
+			dev_err(&pdev->dev, "runtime resume failed\n");
+			goto unprepare_div_clk;
+		}
+	}
+
 	if (i2c_dev->is_multimaster_mode) {
 		ret = clk_enable(i2c_dev->div_clk);
 		if (ret < 0) {
 			dev_err(i2c_dev->dev, "div_clk enable failed %d\n",
 				ret);
-			goto unprepare_div_clk;
+			goto disable_rpm;
 		}
 	}
 
 	ret = tegra_i2c_init(i2c_dev);
 	if (ret) {
-		dev_err(&pdev->dev, "Failed to initialize i2c controller");
+		dev_err(&pdev->dev, "Failed to initialize i2c controller\n");
 		goto disable_div_clk;
 	}
 
@@ -925,17 +999,15 @@ static int tegra_i2c_probe(struct platform_device *pdev)
 	i2c_set_adapdata(&i2c_dev->adapter, i2c_dev);
 	i2c_dev->adapter.owner = THIS_MODULE;
 	i2c_dev->adapter.class = I2C_CLASS_DEPRECATED;
-	strlcpy(i2c_dev->adapter.name, "Tegra I2C adapter",
+	strlcpy(i2c_dev->adapter.name, dev_name(&pdev->dev),
 		sizeof(i2c_dev->adapter.name));
 	i2c_dev->adapter.dev.parent = &pdev->dev;
 	i2c_dev->adapter.nr = pdev->id;
 	i2c_dev->adapter.dev.of_node = pdev->dev.of_node;
 
 	ret = i2c_add_numbered_adapter(&i2c_dev->adapter);
-	if (ret) {
-		dev_err(&pdev->dev, "Failed to add I2C adapter\n");
+	if (ret)
 		goto disable_div_clk;
-	}
 
 	return 0;
 
@@ -943,6 +1015,11 @@ disable_div_clk:
 	if (i2c_dev->is_multimaster_mode)
 		clk_disable(i2c_dev->div_clk);
 
+disable_rpm:
+	pm_runtime_disable(&pdev->dev);
+	if (!pm_runtime_status_suspended(&pdev->dev))
+		tegra_i2c_runtime_suspend(&pdev->dev);
+
 unprepare_div_clk:
 	clk_unprepare(i2c_dev->div_clk);
 
@@ -956,11 +1033,16 @@ unprepare_fast_clk:
 static int tegra_i2c_remove(struct platform_device *pdev)
 {
 	struct tegra_i2c_dev *i2c_dev = platform_get_drvdata(pdev);
+
 	i2c_del_adapter(&i2c_dev->adapter);
 
 	if (i2c_dev->is_multimaster_mode)
 		clk_disable(i2c_dev->div_clk);
 
+	pm_runtime_disable(&pdev->dev);
+	if (!pm_runtime_status_suspended(&pdev->dev))
+		tegra_i2c_runtime_suspend(&pdev->dev);
+
 	clk_unprepare(i2c_dev->div_clk);
 	if (!i2c_dev->hw->has_single_clk_source)
 		clk_unprepare(i2c_dev->fast_clk);
@@ -988,20 +1070,19 @@ static int tegra_i2c_resume(struct device *dev)
 	i2c_lock_adapter(&i2c_dev->adapter);
 
 	ret = tegra_i2c_init(i2c_dev);
-
-	if (ret) {
-		i2c_unlock_adapter(&i2c_dev->adapter);
-		return ret;
-	}
-
-	i2c_dev->is_suspended = false;
+	if (!ret)
+		i2c_dev->is_suspended = false;
 
 	i2c_unlock_adapter(&i2c_dev->adapter);
 
-	return 0;
+	return ret;
 }
 
-static SIMPLE_DEV_PM_OPS(tegra_i2c_pm, tegra_i2c_suspend, tegra_i2c_resume);
+static const struct dev_pm_ops tegra_i2c_pm = {
+	SET_RUNTIME_PM_OPS(tegra_i2c_runtime_suspend, tegra_i2c_runtime_resume,
+			   NULL)
+	SET_SYSTEM_SLEEP_PM_OPS(tegra_i2c_suspend, tegra_i2c_resume)
+};
 #define TEGRA_I2C_PM	(&tegra_i2c_pm)
 #else
 #define TEGRA_I2C_PM	NULL
diff --git a/drivers/i2c/busses/i2c-thunderx-pcidrv.c b/drivers/i2c/busses/i2c-thunderx-pcidrv.c
new file mode 100644
index 000000000000..bba5b429f69c
--- /dev/null
+++ b/drivers/i2c/busses/i2c-thunderx-pcidrv.c
@@ -0,0 +1,259 @@
+/*
+ * Cavium ThunderX i2c driver.
+ *
+ * Copyright (C) 2015,2016 Cavium Inc.
+ * Authors: Fred Martin <fmartin@caviumnetworks.com>
+ *	    Jan Glauber <jglauber@cavium.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/acpi.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/i2c-smbus.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of_irq.h>
+#include <linux/pci.h>
+
+#include "i2c-octeon-core.h"
+
+#define DRV_NAME "i2c-thunderx"
+
+#define PCI_DEVICE_ID_THUNDER_TWSI	0xa012
+
+#define SYS_FREQ_DEFAULT		700000000
+
+#define TWSI_INT_ENA_W1C		0x1028
+#define TWSI_INT_ENA_W1S		0x1030
+
+/*
+ * Enable the CORE interrupt.
+ * The interrupt will be asserted when there is non-STAT_IDLE state in the
+ * SW_TWSI_EOP_TWSI_STAT register.
+ */
+static void thunder_i2c_int_enable(struct octeon_i2c *i2c)
+{
+	octeon_i2c_writeq_flush(TWSI_INT_CORE_INT,
+				i2c->twsi_base + TWSI_INT_ENA_W1S);
+}
+
+/*
+ * Disable the CORE interrupt.
+ */
+static void thunder_i2c_int_disable(struct octeon_i2c *i2c)
+{
+	octeon_i2c_writeq_flush(TWSI_INT_CORE_INT,
+				i2c->twsi_base + TWSI_INT_ENA_W1C);
+}
+
+static void thunder_i2c_hlc_int_enable(struct octeon_i2c *i2c)
+{
+	octeon_i2c_writeq_flush(TWSI_INT_ST_INT | TWSI_INT_TS_INT,
+				i2c->twsi_base + TWSI_INT_ENA_W1S);
+}
+
+static void thunder_i2c_hlc_int_disable(struct octeon_i2c *i2c)
+{
+	octeon_i2c_writeq_flush(TWSI_INT_ST_INT | TWSI_INT_TS_INT,
+				i2c->twsi_base + TWSI_INT_ENA_W1C);
+}
+
+static u32 thunderx_i2c_functionality(struct i2c_adapter *adap)
+{
+	return I2C_FUNC_I2C | (I2C_FUNC_SMBUS_EMUL & ~I2C_FUNC_SMBUS_QUICK) |
+	       I2C_FUNC_SMBUS_READ_BLOCK_DATA | I2C_SMBUS_BLOCK_PROC_CALL;
+}
+
+static const struct i2c_algorithm thunderx_i2c_algo = {
+	.master_xfer = octeon_i2c_xfer,
+	.functionality = thunderx_i2c_functionality,
+};
+
+static struct i2c_adapter thunderx_i2c_ops = {
+	.owner	= THIS_MODULE,
+	.name	= "ThunderX adapter",
+	.algo	= &thunderx_i2c_algo,
+};
+
+static void thunder_i2c_clock_enable(struct device *dev, struct octeon_i2c *i2c)
+{
+	int ret;
+
+	i2c->clk = clk_get(dev, NULL);
+	if (IS_ERR(i2c->clk)) {
+		i2c->clk = NULL;
+		goto skip;
+	}
+
+	ret = clk_prepare_enable(i2c->clk);
+	if (ret)
+		goto skip;
+	i2c->sys_freq = clk_get_rate(i2c->clk);
+
+skip:
+	if (!i2c->sys_freq)
+		i2c->sys_freq = SYS_FREQ_DEFAULT;
+}
+
+static void thunder_i2c_clock_disable(struct device *dev, struct clk *clk)
+{
+	if (!clk)
+		return;
+	clk_disable_unprepare(clk);
+	clk_put(clk);
+}
+
+static int thunder_i2c_smbus_setup_of(struct octeon_i2c *i2c,
+				      struct device_node *node)
+{
+	u32 type;
+
+	if (!node)
+		return -EINVAL;
+
+	i2c->alert_data.irq = irq_of_parse_and_map(node, 0);
+	if (!i2c->alert_data.irq)
+		return -EINVAL;
+
+	type = irqd_get_trigger_type(irq_get_irq_data(i2c->alert_data.irq));
+	i2c->alert_data.alert_edge_triggered =
+		(type & IRQ_TYPE_LEVEL_MASK) ? 1 : 0;
+
+	i2c->ara = i2c_setup_smbus_alert(&i2c->adap, &i2c->alert_data);
+	if (!i2c->ara)
+		return -ENODEV;
+	return 0;
+}
+
+static int thunder_i2c_smbus_setup(struct octeon_i2c *i2c,
+				   struct device_node *node)
+{
+	/* TODO: ACPI support */
+	if (!acpi_disabled)
+		return -EOPNOTSUPP;
+
+	return thunder_i2c_smbus_setup_of(i2c, node);
+}
+
+static void thunder_i2c_smbus_remove(struct octeon_i2c *i2c)
+{
+	if (i2c->ara)
+		i2c_unregister_device(i2c->ara);
+}
+
+static int thunder_i2c_probe_pci(struct pci_dev *pdev,
+				 const struct pci_device_id *ent)
+{
+	struct device *dev = &pdev->dev;
+	struct octeon_i2c *i2c;
+	int ret;
+
+	i2c = devm_kzalloc(dev, sizeof(*i2c), GFP_KERNEL);
+	if (!i2c)
+		return -ENOMEM;
+
+	i2c->roff.sw_twsi = 0x1000;
+	i2c->roff.twsi_int = 0x1010;
+	i2c->roff.sw_twsi_ext = 0x1018;
+
+	i2c->dev = dev;
+	pci_set_drvdata(pdev, i2c);
+	ret = pcim_enable_device(pdev);
+	if (ret)
+		return ret;
+
+	ret = pci_request_regions(pdev, DRV_NAME);
+	if (ret)
+		return ret;
+
+	i2c->twsi_base = pcim_iomap(pdev, 0, pci_resource_len(pdev, 0));
+	if (!i2c->twsi_base)
+		return -EINVAL;
+
+	thunder_i2c_clock_enable(dev, i2c);
+	ret = device_property_read_u32(dev, "clock-frequency", &i2c->twsi_freq);
+	if (ret)
+		i2c->twsi_freq = 100000;
+
+	init_waitqueue_head(&i2c->queue);
+
+	i2c->int_enable = thunder_i2c_int_enable;
+	i2c->int_disable = thunder_i2c_int_disable;
+	i2c->hlc_int_enable = thunder_i2c_hlc_int_enable;
+	i2c->hlc_int_disable = thunder_i2c_hlc_int_disable;
+
+	ret = pci_enable_msix(pdev, &i2c->i2c_msix, 1);
+	if (ret)
+		goto error;
+
+	ret = devm_request_irq(dev, i2c->i2c_msix.vector, octeon_i2c_isr, 0,
+			       DRV_NAME, i2c);
+	if (ret)
+		goto error;
+
+	ret = octeon_i2c_init_lowlevel(i2c);
+	if (ret)
+		goto error;
+
+	octeon_i2c_set_clock(i2c);
+
+	i2c->adap = thunderx_i2c_ops;
+	i2c->adap.retries = 5;
+	i2c->adap.bus_recovery_info = &octeon_i2c_recovery_info;
+	i2c->adap.dev.parent = dev;
+	i2c->adap.dev.of_node = pdev->dev.of_node;
+	snprintf(i2c->adap.name, sizeof(i2c->adap.name),
+		 "Cavium ThunderX i2c adapter at %s", dev_name(dev));
+	i2c_set_adapdata(&i2c->adap, i2c);
+
+	ret = i2c_add_adapter(&i2c->adap);
+	if (ret)
+		goto error;
+
+	dev_info(i2c->dev, "Probed. Set system clock to %u\n", i2c->sys_freq);
+
+	ret = thunder_i2c_smbus_setup(i2c, pdev->dev.of_node);
+	if (ret)
+		dev_info(dev, "SMBUS alert not active on this bus\n");
+
+	return 0;
+
+error:
+	thunder_i2c_clock_disable(dev, i2c->clk);
+	return ret;
+}
+
+static void thunder_i2c_remove_pci(struct pci_dev *pdev)
+{
+	struct octeon_i2c *i2c = pci_get_drvdata(pdev);
+
+	thunder_i2c_smbus_remove(i2c);
+	thunder_i2c_clock_disable(&pdev->dev, i2c->clk);
+	i2c_del_adapter(&i2c->adap);
+}
+
+static const struct pci_device_id thunder_i2c_pci_id_table[] = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_CAVIUM, PCI_DEVICE_ID_THUNDER_TWSI) },
+	{ 0, }
+};
+
+MODULE_DEVICE_TABLE(pci, thunder_i2c_pci_id_table);
+
+static struct pci_driver thunder_i2c_pci_driver = {
+	.name		= DRV_NAME,
+	.id_table	= thunder_i2c_pci_id_table,
+	.probe		= thunder_i2c_probe_pci,
+	.remove		= thunder_i2c_remove_pci,
+};
+
+module_pci_driver(thunder_i2c_pci_driver);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Fred Martin <fmartin@caviumnetworks.com>");
+MODULE_DESCRIPTION("I2C-Bus adapter for Cavium ThunderX SOC");
diff --git a/drivers/i2c/busses/i2c-uniphier-f.c b/drivers/i2c/busses/i2c-uniphier-f.c
index aeead0d27d10..db9105e52c79 100644
--- a/drivers/i2c/busses/i2c-uniphier-f.c
+++ b/drivers/i2c/busses/i2c-uniphier-f.c
@@ -14,6 +14,7 @@
 
 #include <linux/clk.h>
 #include <linux/i2c.h>
+#include <linux/iopoll.h>
 #include <linux/interrupt.h>
 #include <linux/io.h>
 #include <linux/module.h>
@@ -348,14 +349,19 @@ static int uniphier_fi2c_master_xfer_one(struct i2c_adapter *adap,
 	dev_dbg(&adap->dev, "complete\n");
 
 	if (unlikely(priv->flags & UNIPHIER_FI2C_DEFER_STOP_COMP)) {
-		u32 status = readl(priv->membase + UNIPHIER_FI2C_SR);
-
-		if (!(status & UNIPHIER_FI2C_SR_STS) ||
-		    status & UNIPHIER_FI2C_SR_BB) {
+		u32 status;
+		int ret;
+
+		ret = readl_poll_timeout(priv->membase + UNIPHIER_FI2C_SR,
+					 status,
+					 (status & UNIPHIER_FI2C_SR_STS) &&
+					 !(status & UNIPHIER_FI2C_SR_BB),
+					 1, 20);
+		if (ret) {
 			dev_err(&adap->dev,
 				"stop condition was not completed.\n");
 			uniphier_fi2c_recover(priv);
-			return -EBUSY;
+			return ret;
 		}
 	}
 
@@ -455,54 +461,25 @@ static struct i2c_bus_recovery_info uniphier_fi2c_bus_recovery_info = {
 	.unprepare_recovery = uniphier_fi2c_unprepare_recovery,
 };
 
-static int uniphier_fi2c_clk_init(struct device *dev,
-				  struct uniphier_fi2c_priv *priv)
+static void uniphier_fi2c_hw_init(struct uniphier_fi2c_priv *priv,
+				  u32 bus_speed, unsigned long clk_rate)
 {
-	struct device_node *np = dev->of_node;
-	unsigned long clk_rate;
-	u32 bus_speed, clk_count;
-	int ret;
-
-	if (of_property_read_u32(np, "clock-frequency", &bus_speed))
-		bus_speed = UNIPHIER_FI2C_DEFAULT_SPEED;
-
-	if (!bus_speed) {
-		dev_err(dev, "clock-frequency should not be zero\n");
-		return -EINVAL;
-	}
-
-	if (bus_speed > UNIPHIER_FI2C_MAX_SPEED)
-		bus_speed = UNIPHIER_FI2C_MAX_SPEED;
-
-	/* Get input 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);
-	}
+	u32 tmp;
 
-	ret = clk_prepare_enable(priv->clk);
-	if (ret)
-		return ret;
-
-	clk_rate = clk_get_rate(priv->clk);
-	if (!clk_rate) {
-		dev_err(dev, "input clock rate should not be zero\n");
-		return -EINVAL;
-	}
+	tmp = readl(priv->membase + UNIPHIER_FI2C_CR);
+	tmp |= UNIPHIER_FI2C_CR_MST;
+	writel(tmp, priv->membase + UNIPHIER_FI2C_CR);
 
 	uniphier_fi2c_reset(priv);
 
-	clk_count = clk_rate / bus_speed;
+	tmp = clk_rate / bus_speed;
 
-	writel(clk_count, priv->membase + UNIPHIER_FI2C_CYC);
-	writel(clk_count / 2, priv->membase + UNIPHIER_FI2C_LCTL);
-	writel(clk_count / 2, priv->membase + UNIPHIER_FI2C_SSUT);
-	writel(clk_count / 16, priv->membase + UNIPHIER_FI2C_DSUT);
+	writel(tmp, priv->membase + UNIPHIER_FI2C_CYC);
+	writel(tmp / 2, priv->membase + UNIPHIER_FI2C_LCTL);
+	writel(tmp / 2, priv->membase + UNIPHIER_FI2C_SSUT);
+	writel(tmp / 16, priv->membase + UNIPHIER_FI2C_DSUT);
 
 	uniphier_fi2c_prepare_operation(priv);
-
-	return 0;
 }
 
 static int uniphier_fi2c_probe(struct platform_device *pdev)
@@ -510,8 +487,9 @@ static int uniphier_fi2c_probe(struct platform_device *pdev)
 	struct device *dev = &pdev->dev;
 	struct uniphier_fi2c_priv *priv;
 	struct resource *regs;
-	int irq;
-	int ret;
+	u32 bus_speed;
+	unsigned long clk_rate;
+	int irq, ret;
 
 	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
 	if (!priv)
@@ -528,6 +506,31 @@ static int uniphier_fi2c_probe(struct platform_device *pdev)
 		return irq;
 	}
 
+	if (of_property_read_u32(dev->of_node, "clock-frequency", &bus_speed))
+		bus_speed = UNIPHIER_FI2C_DEFAULT_SPEED;
+
+	if (!bus_speed || bus_speed > UNIPHIER_FI2C_MAX_SPEED) {
+		dev_err(dev, "invalid clock-frequency %d\n", bus_speed);
+		return -EINVAL;
+	}
+
+	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)
+		return ret;
+
+	clk_rate = clk_get_rate(priv->clk);
+	if (!clk_rate) {
+		dev_err(dev, "input clock rate should not be zero\n");
+		ret = -EINVAL;
+		goto err;
+	}
+
 	init_completion(&priv->comp);
 	priv->adap.owner = THIS_MODULE;
 	priv->adap.algo = &uniphier_fi2c_algo;
@@ -538,9 +541,7 @@ static int uniphier_fi2c_probe(struct platform_device *pdev)
 	i2c_set_adapdata(&priv->adap, priv);
 	platform_set_drvdata(pdev, priv);
 
-	ret = uniphier_fi2c_clk_init(dev, priv);
-	if (ret)
-		goto err;
+	uniphier_fi2c_hw_init(priv, bus_speed, clk_rate);
 
 	ret = devm_request_irq(dev, irq, uniphier_fi2c_interrupt, 0,
 			       pdev->name, priv);
@@ -550,11 +551,6 @@ static int uniphier_fi2c_probe(struct platform_device *pdev)
 	}
 
 	ret = i2c_add_adapter(&priv->adap);
-	if (ret) {
-		dev_err(dev, "failed to add I2C adapter\n");
-		goto err;
-	}
-
 err:
 	if (ret)
 		clk_disable_unprepare(priv->clk);
diff --git a/drivers/i2c/busses/i2c-uniphier.c b/drivers/i2c/busses/i2c-uniphier.c
index 475a5eb514e2..56e92af46ddc 100644
--- a/drivers/i2c/busses/i2c-uniphier.c
+++ b/drivers/i2c/busses/i2c-uniphier.c
@@ -316,50 +316,15 @@ static struct i2c_bus_recovery_info uniphier_i2c_bus_recovery_info = {
 	.unprepare_recovery = uniphier_i2c_unprepare_recovery,
 };
 
-static int uniphier_i2c_clk_init(struct device *dev,
-				 struct uniphier_i2c_priv *priv)
+static void uniphier_i2c_hw_init(struct uniphier_i2c_priv *priv,
+				 u32 bus_speed, unsigned long clk_rate)
 {
-	struct device_node *np = dev->of_node;
-	unsigned long clk_rate;
-	u32 bus_speed;
-	int ret;
-
-	if (of_property_read_u32(np, "clock-frequency", &bus_speed))
-		bus_speed = UNIPHIER_I2C_DEFAULT_SPEED;
-
-	if (!bus_speed) {
-		dev_err(dev, "clock-frequency should not be zero\n");
-		return -EINVAL;
-	}
-
-	if (bus_speed > UNIPHIER_I2C_MAX_SPEED)
-		bus_speed = UNIPHIER_I2C_MAX_SPEED;
-
-	/* Get input 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)
-		return ret;
-
-	clk_rate = clk_get_rate(priv->clk);
-	if (!clk_rate) {
-		dev_err(dev, "input clock rate should not be zero\n");
-		return -EINVAL;
-	}
-
 	uniphier_i2c_reset(priv, true);
 
 	writel((clk_rate / bus_speed / 2 << 16) | (clk_rate / bus_speed),
 	       priv->membase + UNIPHIER_I2C_CLK);
 
 	uniphier_i2c_reset(priv, false);
-
-	return 0;
 }
 
 static int uniphier_i2c_probe(struct platform_device *pdev)
@@ -367,8 +332,9 @@ static int uniphier_i2c_probe(struct platform_device *pdev)
 	struct device *dev = &pdev->dev;
 	struct uniphier_i2c_priv *priv;
 	struct resource *regs;
-	int irq;
-	int ret;
+	u32 bus_speed;
+	unsigned long clk_rate;
+	int irq, ret;
 
 	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
 	if (!priv)
@@ -385,6 +351,31 @@ static int uniphier_i2c_probe(struct platform_device *pdev)
 		return irq;
 	}
 
+	if (of_property_read_u32(dev->of_node, "clock-frequency", &bus_speed))
+		bus_speed = UNIPHIER_I2C_DEFAULT_SPEED;
+
+	if (!bus_speed || bus_speed > UNIPHIER_I2C_MAX_SPEED) {
+		dev_err(dev, "invalid clock-frequency %d\n", bus_speed);
+		return -EINVAL;
+	}
+
+	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)
+		return ret;
+
+	clk_rate = clk_get_rate(priv->clk);
+	if (!clk_rate) {
+		dev_err(dev, "input clock rate should not be zero\n");
+		ret = -EINVAL;
+		goto err;
+	}
+
 	init_completion(&priv->comp);
 	priv->adap.owner = THIS_MODULE;
 	priv->adap.algo = &uniphier_i2c_algo;
@@ -395,9 +386,7 @@ static int uniphier_i2c_probe(struct platform_device *pdev)
 	i2c_set_adapdata(&priv->adap, priv);
 	platform_set_drvdata(pdev, priv);
 
-	ret = uniphier_i2c_clk_init(dev, priv);
-	if (ret)
-		goto err;
+	uniphier_i2c_hw_init(priv, bus_speed, clk_rate);
 
 	ret = devm_request_irq(dev, irq, uniphier_i2c_interrupt, 0, pdev->name,
 			       priv);
@@ -407,11 +396,6 @@ static int uniphier_i2c_probe(struct platform_device *pdev)
 	}
 
 	ret = i2c_add_adapter(&priv->adap);
-	if (ret) {
-		dev_err(dev, "failed to add I2C adapter\n");
-		goto err;
-	}
-
 err:
 	if (ret)
 		clk_disable_unprepare(priv->clk);
diff --git a/drivers/i2c/busses/i2c-wmt.c b/drivers/i2c/busses/i2c-wmt.c
index e1e3a85596c5..fbd0fd59f312 100644
--- a/drivers/i2c/busses/i2c-wmt.c
+++ b/drivers/i2c/busses/i2c-wmt.c
@@ -432,10 +432,8 @@ static int wmt_i2c_probe(struct platform_device *pdev)
 	}
 
 	err = i2c_add_adapter(adap);
-	if (err) {
-		dev_err(&pdev->dev, "failed to add adapter\n");
+	if (err)
 		return err;
-	}
 
 	platform_set_drvdata(pdev, i2c_dev);
 
diff --git a/drivers/i2c/busses/i2c-xgene-slimpro.c b/drivers/i2c/busses/i2c-xgene-slimpro.c
index 4233f5695352..263685c7a512 100644
--- a/drivers/i2c/busses/i2c-xgene-slimpro.c
+++ b/drivers/i2c/busses/i2c-xgene-slimpro.c
@@ -418,7 +418,6 @@ static int xgene_slimpro_i2c_probe(struct platform_device *pdev)
 	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;
 	}
diff --git a/drivers/i2c/busses/i2c-xiic.c b/drivers/i2c/busses/i2c-xiic.c
index 74f54f2f471f..66bce3b311a1 100644
--- a/drivers/i2c/busses/i2c-xiic.c
+++ b/drivers/i2c/busses/i2c-xiic.c
@@ -804,7 +804,6 @@ static int xiic_i2c_probe(struct platform_device *pdev)
 	/* add i2c adapter to i2c tree */
 	ret = i2c_add_adapter(&i2c->adap);
 	if (ret) {
-		dev_err(&pdev->dev, "Failed to add adapter\n");
 		xiic_deinit(i2c);
 		goto err_clk_dis;
 	}
diff --git a/drivers/i2c/busses/i2c-xlp9xx.c b/drivers/i2c/busses/i2c-xlp9xx.c
index 55a7bef1b2e1..2a972ed7aa0d 100644
--- a/drivers/i2c/busses/i2c-xlp9xx.c
+++ b/drivers/i2c/busses/i2c-xlp9xx.c
@@ -400,10 +400,8 @@ static int xlp9xx_i2c_probe(struct platform_device *pdev)
 	i2c_set_adapdata(&priv->adapter, priv);
 
 	err = i2c_add_adapter(&priv->adapter);
-	if (err) {
-		dev_err(&pdev->dev, "failed to add I2C adapter!\n");
+	if (err)
 		return err;
-	}
 
 	platform_set_drvdata(pdev, priv);
 	dev_dbg(&pdev->dev, "I2C bus:%d added\n", priv->adapter.nr);
diff --git a/drivers/i2c/busses/i2c-xlr.c b/drivers/i2c/busses/i2c-xlr.c
index 613c3a4f2c51..0968f59b6df5 100644
--- a/drivers/i2c/busses/i2c-xlr.c
+++ b/drivers/i2c/busses/i2c-xlr.c
@@ -432,10 +432,8 @@ static int xlr_i2c_probe(struct platform_device *pdev)
 
 	i2c_set_adapdata(&priv->adap, priv);
 	ret = i2c_add_numbered_adapter(&priv->adap);
-	if (ret < 0) {
-		dev_err(&priv->adap.dev, "Failed to add i2c bus.\n");
+	if (ret < 0)
 		return ret;
-	}
 
 	platform_set_drvdata(pdev, priv);
 	dev_info(&priv->adap.dev, "Added I2C Bus.\n");
diff --git a/drivers/i2c/i2c-core.c b/drivers/i2c/i2c-core.c
index da3a02ef4a31..5ab67219f71e 100644
--- a/drivers/i2c/i2c-core.c
+++ b/drivers/i2c/i2c-core.c
@@ -88,7 +88,7 @@ void i2c_transfer_trace_unreg(void)
 }
 
 #if defined(CONFIG_ACPI)
-struct acpi_i2c_handler_data {
+struct i2c_acpi_handler_data {
 	struct acpi_connection_info info;
 	struct i2c_adapter *adapter;
 };
@@ -103,15 +103,18 @@ struct gsb_buffer {
 	};
 } __packed;
 
-struct acpi_i2c_lookup {
+struct i2c_acpi_lookup {
 	struct i2c_board_info *info;
 	acpi_handle adapter_handle;
 	acpi_handle device_handle;
+	acpi_handle search_handle;
+	u32 speed;
+	u32 min_speed;
 };
 
-static int acpi_i2c_fill_info(struct acpi_resource *ares, void *data)
+static int i2c_acpi_fill_info(struct acpi_resource *ares, void *data)
 {
-	struct acpi_i2c_lookup *lookup = data;
+	struct i2c_acpi_lookup *lookup = data;
 	struct i2c_board_info *info = lookup->info;
 	struct acpi_resource_i2c_serialbus *sb;
 	acpi_status status;
@@ -130,19 +133,18 @@ static int acpi_i2c_fill_info(struct acpi_resource *ares, void *data)
 		return 1;
 
 	info->addr = sb->slave_address;
+	lookup->speed = sb->connection_speed;
 	if (sb->access_mode == ACPI_I2C_10BIT_MODE)
 		info->flags |= I2C_CLIENT_TEN;
 
 	return 1;
 }
 
-static int acpi_i2c_get_info(struct acpi_device *adev,
-			     struct i2c_board_info *info,
-			     acpi_handle *adapter_handle)
+static int i2c_acpi_do_lookup(struct acpi_device *adev,
+			      struct i2c_acpi_lookup *lookup)
 {
+	struct i2c_board_info *info = lookup->info;
 	struct list_head resource_list;
-	struct resource_entry *entry;
-	struct acpi_i2c_lookup lookup;
 	int ret;
 
 	if (acpi_bus_get_status(adev) || !adev->status.present ||
@@ -150,24 +152,58 @@ static int acpi_i2c_get_info(struct acpi_device *adev,
 		return -EINVAL;
 
 	memset(info, 0, sizeof(*info));
-	info->fwnode = acpi_fwnode_handle(adev);
-
-	memset(&lookup, 0, sizeof(lookup));
-	lookup.device_handle = acpi_device_handle(adev);
-	lookup.info = info;
+	lookup->device_handle = acpi_device_handle(adev);
 
 	/* Look up for I2cSerialBus resource */
 	INIT_LIST_HEAD(&resource_list);
 	ret = acpi_dev_get_resources(adev, &resource_list,
-				     acpi_i2c_fill_info, &lookup);
+				     i2c_acpi_fill_info, lookup);
 	acpi_dev_free_resource_list(&resource_list);
 
 	if (ret < 0 || !info->addr)
 		return -EINVAL;
 
-	*adapter_handle = lookup.adapter_handle;
+	return 0;
+}
+
+static int i2c_acpi_get_info(struct acpi_device *adev,
+			     struct i2c_board_info *info,
+			     struct i2c_adapter *adapter,
+			     acpi_handle *adapter_handle)
+{
+	struct list_head resource_list;
+	struct resource_entry *entry;
+	struct i2c_acpi_lookup lookup;
+	int ret;
+
+	memset(&lookup, 0, sizeof(lookup));
+	lookup.info = info;
+
+	ret = i2c_acpi_do_lookup(adev, &lookup);
+	if (ret)
+		return ret;
+
+	if (adapter) {
+		/* The adapter must match the one in I2cSerialBus() connector */
+		if (ACPI_HANDLE(&adapter->dev) != lookup.adapter_handle)
+			return -ENODEV;
+	} else {
+		struct acpi_device *adapter_adev;
+
+		/* The adapter must be present */
+		if (acpi_bus_get_device(lookup.adapter_handle, &adapter_adev))
+			return -ENODEV;
+		if (acpi_bus_get_status(adapter_adev) ||
+		    !adapter_adev->status.present)
+			return -ENODEV;
+	}
+
+	info->fwnode = acpi_fwnode_handle(adev);
+	if (adapter_handle)
+		*adapter_handle = lookup.adapter_handle;
 
 	/* Then fill IRQ number if any */
+	INIT_LIST_HEAD(&resource_list);
 	ret = acpi_dev_get_resources(adev, &resource_list, NULL, NULL);
 	if (ret < 0)
 		return -EINVAL;
@@ -186,7 +222,7 @@ static int acpi_i2c_get_info(struct acpi_device *adev,
 	return 0;
 }
 
-static void acpi_i2c_register_device(struct i2c_adapter *adapter,
+static void i2c_acpi_register_device(struct i2c_adapter *adapter,
 				     struct acpi_device *adev,
 				     struct i2c_board_info *info)
 {
@@ -201,39 +237,35 @@ static void acpi_i2c_register_device(struct i2c_adapter *adapter,
 	}
 }
 
-static acpi_status acpi_i2c_add_device(acpi_handle handle, u32 level,
+static acpi_status i2c_acpi_add_device(acpi_handle handle, u32 level,
 				       void *data, void **return_value)
 {
 	struct i2c_adapter *adapter = data;
 	struct acpi_device *adev;
-	acpi_handle adapter_handle;
 	struct i2c_board_info info;
 
 	if (acpi_bus_get_device(handle, &adev))
 		return AE_OK;
 
-	if (acpi_i2c_get_info(adev, &info, &adapter_handle))
-		return AE_OK;
-
-	if (adapter_handle != ACPI_HANDLE(&adapter->dev))
+	if (i2c_acpi_get_info(adev, &info, adapter, NULL))
 		return AE_OK;
 
-	acpi_i2c_register_device(adapter, adev, &info);
+	i2c_acpi_register_device(adapter, adev, &info);
 
 	return AE_OK;
 }
 
-#define ACPI_I2C_MAX_SCAN_DEPTH 32
+#define I2C_ACPI_MAX_SCAN_DEPTH 32
 
 /**
- * acpi_i2c_register_devices - enumerate I2C slave devices behind adapter
+ * i2c_acpi_register_devices - enumerate I2C slave devices behind adapter
  * @adap: pointer to adapter
  *
  * Enumerate all I2C slave devices behind this adapter by walking the ACPI
  * namespace. When a device is found it will be added to the Linux device
  * model and bound to the corresponding ACPI handle.
  */
-static void acpi_i2c_register_devices(struct i2c_adapter *adap)
+static void i2c_acpi_register_devices(struct i2c_adapter *adap)
 {
 	acpi_status status;
 
@@ -241,14 +273,72 @@ static void acpi_i2c_register_devices(struct i2c_adapter *adap)
 		return;
 
 	status = acpi_walk_namespace(ACPI_TYPE_DEVICE, ACPI_ROOT_OBJECT,
-				     ACPI_I2C_MAX_SCAN_DEPTH,
-				     acpi_i2c_add_device, NULL,
+				     I2C_ACPI_MAX_SCAN_DEPTH,
+				     i2c_acpi_add_device, NULL,
 				     adap, NULL);
 	if (ACPI_FAILURE(status))
 		dev_warn(&adap->dev, "failed to enumerate I2C slaves\n");
 }
 
-static int acpi_i2c_match_adapter(struct device *dev, void *data)
+static acpi_status i2c_acpi_lookup_speed(acpi_handle handle, u32 level,
+					   void *data, void **return_value)
+{
+	struct i2c_acpi_lookup *lookup = data;
+	struct acpi_device *adev;
+
+	if (acpi_bus_get_device(handle, &adev))
+		return AE_OK;
+
+	if (i2c_acpi_do_lookup(adev, lookup))
+		return AE_OK;
+
+	if (lookup->search_handle != lookup->adapter_handle)
+		return AE_OK;
+
+	if (lookup->speed <= lookup->min_speed)
+		lookup->min_speed = lookup->speed;
+
+	return AE_OK;
+}
+
+/**
+ * i2c_acpi_find_bus_speed - find I2C bus speed from ACPI
+ * @dev: The device owning the bus
+ *
+ * Find the I2C bus speed by walking the ACPI namespace for all I2C slaves
+ * devices connected to this bus and use the speed of slowest device.
+ *
+ * Returns the speed in Hz or zero
+ */
+u32 i2c_acpi_find_bus_speed(struct device *dev)
+{
+	struct i2c_acpi_lookup lookup;
+	struct i2c_board_info dummy;
+	acpi_status status;
+
+	if (!has_acpi_companion(dev))
+		return 0;
+
+	memset(&lookup, 0, sizeof(lookup));
+	lookup.search_handle = ACPI_HANDLE(dev);
+	lookup.min_speed = UINT_MAX;
+	lookup.info = &dummy;
+
+	status = acpi_walk_namespace(ACPI_TYPE_DEVICE, ACPI_ROOT_OBJECT,
+				     I2C_ACPI_MAX_SCAN_DEPTH,
+				     i2c_acpi_lookup_speed, NULL,
+				     &lookup, NULL);
+
+	if (ACPI_FAILURE(status)) {
+		dev_warn(dev, "unable to find I2C bus speed from ACPI\n");
+		return 0;
+	}
+
+	return lookup.min_speed != UINT_MAX ? lookup.min_speed : 0;
+}
+EXPORT_SYMBOL_GPL(i2c_acpi_find_bus_speed);
+
+static int i2c_acpi_match_adapter(struct device *dev, void *data)
 {
 	struct i2c_adapter *adapter = i2c_verify_adapter(dev);
 
@@ -258,29 +348,29 @@ static int acpi_i2c_match_adapter(struct device *dev, void *data)
 	return ACPI_HANDLE(dev) == (acpi_handle)data;
 }
 
-static int acpi_i2c_match_device(struct device *dev, void *data)
+static int i2c_acpi_match_device(struct device *dev, void *data)
 {
 	return ACPI_COMPANION(dev) == data;
 }
 
-static struct i2c_adapter *acpi_i2c_find_adapter_by_handle(acpi_handle handle)
+static struct i2c_adapter *i2c_acpi_find_adapter_by_handle(acpi_handle handle)
 {
 	struct device *dev;
 
 	dev = bus_find_device(&i2c_bus_type, NULL, handle,
-			      acpi_i2c_match_adapter);
+			      i2c_acpi_match_adapter);
 	return dev ? i2c_verify_adapter(dev) : NULL;
 }
 
-static struct i2c_client *acpi_i2c_find_client_by_adev(struct acpi_device *adev)
+static struct i2c_client *i2c_acpi_find_client_by_adev(struct acpi_device *adev)
 {
 	struct device *dev;
 
-	dev = bus_find_device(&i2c_bus_type, NULL, adev, acpi_i2c_match_device);
+	dev = bus_find_device(&i2c_bus_type, NULL, adev, i2c_acpi_match_device);
 	return dev ? i2c_verify_client(dev) : NULL;
 }
 
-static int acpi_i2c_notify(struct notifier_block *nb, unsigned long value,
+static int i2c_acpi_notify(struct notifier_block *nb, unsigned long value,
 			   void *arg)
 {
 	struct acpi_device *adev = arg;
@@ -291,20 +381,20 @@ static int acpi_i2c_notify(struct notifier_block *nb, unsigned long value,
 
 	switch (value) {
 	case ACPI_RECONFIG_DEVICE_ADD:
-		if (acpi_i2c_get_info(adev, &info, &adapter_handle))
+		if (i2c_acpi_get_info(adev, &info, NULL, &adapter_handle))
 			break;
 
-		adapter = acpi_i2c_find_adapter_by_handle(adapter_handle);
+		adapter = i2c_acpi_find_adapter_by_handle(adapter_handle);
 		if (!adapter)
 			break;
 
-		acpi_i2c_register_device(adapter, adev, &info);
+		i2c_acpi_register_device(adapter, adev, &info);
 		break;
 	case ACPI_RECONFIG_DEVICE_REMOVE:
 		if (!acpi_device_enumerated(adev))
 			break;
 
-		client = acpi_i2c_find_client_by_adev(adev);
+		client = i2c_acpi_find_client_by_adev(adev);
 		if (!client)
 			break;
 
@@ -317,10 +407,10 @@ static int acpi_i2c_notify(struct notifier_block *nb, unsigned long value,
 }
 
 static struct notifier_block i2c_acpi_notifier = {
-	.notifier_call = acpi_i2c_notify,
+	.notifier_call = i2c_acpi_notify,
 };
 #else /* CONFIG_ACPI */
-static inline void acpi_i2c_register_devices(struct i2c_adapter *adap) { }
+static inline void i2c_acpi_register_devices(struct i2c_adapter *adap) { }
 extern struct notifier_block i2c_acpi_notifier;
 #endif /* CONFIG_ACPI */
 
@@ -386,12 +476,12 @@ static int acpi_gsb_i2c_write_bytes(struct i2c_client *client,
 }
 
 static acpi_status
-acpi_i2c_space_handler(u32 function, acpi_physical_address command,
+i2c_acpi_space_handler(u32 function, acpi_physical_address command,
 			u32 bits, u64 *value64,
 			void *handler_context, void *region_context)
 {
 	struct gsb_buffer *gsb = (struct gsb_buffer *)value64;
-	struct acpi_i2c_handler_data *data = handler_context;
+	struct i2c_acpi_handler_data *data = handler_context;
 	struct acpi_connection_info *info = &data->info;
 	struct acpi_resource_i2c_serialbus *sb;
 	struct i2c_adapter *adapter = data->adapter;
@@ -510,10 +600,10 @@ acpi_i2c_space_handler(u32 function, acpi_physical_address command,
 }
 
 
-static int acpi_i2c_install_space_handler(struct i2c_adapter *adapter)
+static int i2c_acpi_install_space_handler(struct i2c_adapter *adapter)
 {
 	acpi_handle handle;
-	struct acpi_i2c_handler_data *data;
+	struct i2c_acpi_handler_data *data;
 	acpi_status status;
 
 	if (!adapter->dev.parent)
@@ -524,7 +614,7 @@ static int acpi_i2c_install_space_handler(struct i2c_adapter *adapter)
 	if (!handle)
 		return -ENODEV;
 
-	data = kzalloc(sizeof(struct acpi_i2c_handler_data),
+	data = kzalloc(sizeof(struct i2c_acpi_handler_data),
 			    GFP_KERNEL);
 	if (!data)
 		return -ENOMEM;
@@ -538,7 +628,7 @@ static int acpi_i2c_install_space_handler(struct i2c_adapter *adapter)
 
 	status = acpi_install_address_space_handler(handle,
 				ACPI_ADR_SPACE_GSBUS,
-				&acpi_i2c_space_handler,
+				&i2c_acpi_space_handler,
 				NULL,
 				data);
 	if (ACPI_FAILURE(status)) {
@@ -552,10 +642,10 @@ static int acpi_i2c_install_space_handler(struct i2c_adapter *adapter)
 	return 0;
 }
 
-static void acpi_i2c_remove_space_handler(struct i2c_adapter *adapter)
+static void i2c_acpi_remove_space_handler(struct i2c_adapter *adapter)
 {
 	acpi_handle handle;
-	struct acpi_i2c_handler_data *data;
+	struct i2c_acpi_handler_data *data;
 	acpi_status status;
 
 	if (!adapter->dev.parent)
@@ -568,7 +658,7 @@ static void acpi_i2c_remove_space_handler(struct i2c_adapter *adapter)
 
 	acpi_remove_address_space_handler(handle,
 				ACPI_ADR_SPACE_GSBUS,
-				&acpi_i2c_space_handler);
+				&i2c_acpi_space_handler);
 
 	status = acpi_bus_get_private_data(handle, (void **)&data);
 	if (ACPI_SUCCESS(status))
@@ -577,10 +667,10 @@ static void acpi_i2c_remove_space_handler(struct i2c_adapter *adapter)
 	acpi_bus_detach_private_data(handle);
 }
 #else /* CONFIG_ACPI_I2C_OPREGION */
-static inline void acpi_i2c_remove_space_handler(struct i2c_adapter *adapter)
+static inline void i2c_acpi_remove_space_handler(struct i2c_adapter *adapter)
 { }
 
-static inline int acpi_i2c_install_space_handler(struct i2c_adapter *adapter)
+static inline int i2c_acpi_install_space_handler(struct i2c_adapter *adapter)
 { return 0; }
 #endif /* CONFIG_ACPI_I2C_OPREGION */
 
@@ -853,7 +943,7 @@ static int i2c_device_probe(struct device *dev)
 			status = 0;
 
 		if (status)
-			dev_warn(&client->dev, "failed to set up wakeup irq");
+			dev_warn(&client->dev, "failed to set up wakeup irq\n");
 	}
 
 	dev_dbg(dev, "probe\n");
@@ -1208,8 +1298,9 @@ i2c_new_device(struct i2c_adapter *adap, struct i2c_board_info const *info)
 	return client;
 
 out_err:
-	dev_err(&adap->dev, "Failed to register i2c client %s at 0x%02x "
-		"(%d)\n", client->name, client->addr, status);
+	dev_err(&adap->dev,
+		"Failed to register i2c client %s at 0x%02x (%d)\n",
+		client->name, client->addr, status);
 out_err_silent:
 	kfree(client);
 	return NULL;
@@ -1335,21 +1426,19 @@ static void i2c_adapter_dev_release(struct device *dev)
 	complete(&adap->dev_released);
 }
 
-/*
- * This function is only needed for mutex_lock_nested, so it is never
- * called unless locking correctness checking is enabled. Thus we
- * make it inline to avoid a compiler warning. That's what gcc ends up
- * doing anyway.
- */
-static inline unsigned int i2c_adapter_depth(struct i2c_adapter *adapter)
+unsigned int i2c_adapter_depth(struct i2c_adapter *adapter)
 {
 	unsigned int depth = 0;
 
 	while ((adapter = i2c_parent_is_i2c_adapter(adapter)))
 		depth++;
 
+	WARN_ONCE(depth >= MAX_LOCKDEP_SUBCLASSES,
+		  "adapter depth exceeds lockdep subclass limit\n");
+
 	return depth;
 }
+EXPORT_SYMBOL_GPL(i2c_adapter_depth);
 
 /*
  * Let users instantiate I2C devices through sysfs. This can be used when
@@ -1591,7 +1680,7 @@ static struct i2c_client *of_i2c_register_device(struct i2c_adapter *adap,
 
 static void of_i2c_register_devices(struct i2c_adapter *adap)
 {
-	struct device_node *node;
+	struct device_node *bus, *node;
 
 	/* Only register child devices if the adapter has a node pointer set */
 	if (!adap->dev.of_node)
@@ -1599,11 +1688,17 @@ 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) {
+	bus = of_get_child_by_name(adap->dev.of_node, "i2c-bus");
+	if (!bus)
+		bus = of_node_get(adap->dev.of_node);
+
+	for_each_available_child_of_node(bus, node) {
 		if (of_node_test_and_set_flag(node, OF_POPULATED))
 			continue;
 		of_i2c_register_device(adap, node);
 	}
+
+	of_node_put(bus);
 }
 
 static int of_dev_node_match(struct device *dev, void *data)
@@ -1678,8 +1773,8 @@ static int i2c_do_add_adapter(struct i2c_driver *driver,
 	if (driver->attach_adapter) {
 		dev_warn(&adap->dev, "%s: attach_adapter method is deprecated\n",
 			 driver->driver.name);
-		dev_warn(&adap->dev, "Please use another way to instantiate "
-			 "your i2c_client\n");
+		dev_warn(&adap->dev,
+			 "Please use another way to instantiate your i2c_client\n");
 		/* We ignore the return code; if it fails, too bad */
 		driver->attach_adapter(adap);
 	}
@@ -1691,6 +1786,12 @@ static int __process_new_adapter(struct device_driver *d, void *data)
 	return i2c_do_add_adapter(to_i2c_driver(d), data);
 }
 
+static const struct i2c_lock_operations i2c_adapter_lock_ops = {
+	.lock_bus =    i2c_adapter_lock_bus,
+	.trylock_bus = i2c_adapter_trylock_bus,
+	.unlock_bus =  i2c_adapter_unlock_bus,
+};
+
 static int i2c_register_adapter(struct i2c_adapter *adap)
 {
 	int res = -EINVAL;
@@ -1710,11 +1811,8 @@ static int i2c_register_adapter(struct i2c_adapter *adap)
 		goto out_list;
 	}
 
-	if (!adap->lock_bus) {
-		adap->lock_bus = i2c_adapter_lock_bus;
-		adap->trylock_bus = i2c_adapter_trylock_bus;
-		adap->unlock_bus = i2c_adapter_unlock_bus;
-	}
+	if (!adap->lock_ops)
+		adap->lock_ops = &i2c_adapter_lock_ops;
 
 	rt_mutex_init(&adap->bus_lock);
 	rt_mutex_init(&adap->mux_lock);
@@ -1752,8 +1850,8 @@ static int i2c_register_adapter(struct i2c_adapter *adap)
 
 	/* create pre-declared device nodes */
 	of_i2c_register_devices(adap);
-	acpi_i2c_register_devices(adap);
-	acpi_i2c_install_space_handler(adap);
+	i2c_acpi_register_devices(adap);
+	i2c_acpi_install_space_handler(adap);
 
 	if (adap->nr < __i2c_first_dynamic_bus_num)
 		i2c_scan_static_board_info(adap);
@@ -1925,7 +2023,7 @@ void i2c_del_adapter(struct i2c_adapter *adap)
 		return;
 	}
 
-	acpi_i2c_remove_space_handler(adap);
+	i2c_acpi_remove_space_handler(adap);
 	/* Tell drivers about this removal */
 	mutex_lock(&core_lock);
 	bus_for_each_drv(&i2c_bus_type, NULL, adap,
@@ -2451,15 +2549,16 @@ int i2c_transfer(struct i2c_adapter *adap, struct i2c_msg *msgs, int num)
 	if (adap->algo->master_xfer) {
 #ifdef DEBUG
 		for (ret = 0; ret < num; ret++) {
-			dev_dbg(&adap->dev, "master_xfer[%d] %c, addr=0x%02x, "
-				"len=%d%s\n", ret, (msgs[ret].flags & I2C_M_RD)
-				? 'R' : 'W', msgs[ret].addr, msgs[ret].len,
+			dev_dbg(&adap->dev,
+				"master_xfer[%d] %c, addr=0x%02x, len=%d%s\n",
+				ret, (msgs[ret].flags & I2C_M_RD) ? 'R' : 'W',
+				msgs[ret].addr, msgs[ret].len,
 				(msgs[ret].flags & I2C_M_RECV_LEN) ? "+" : "");
 		}
 #endif
 
 		if (in_atomic() || irqs_disabled()) {
-			ret = adap->trylock_bus(adap, I2C_LOCK_SEGMENT);
+			ret = i2c_trylock_bus(adap, I2C_LOCK_SEGMENT);
 			if (!ret)
 				/* I2C activity is ongoing. */
 				return -EAGAIN;
@@ -2619,9 +2718,9 @@ static int i2c_detect_address(struct i2c_client *temp_client,
 
 	/* Consistency check */
 	if (info.type[0] == '\0') {
-		dev_err(&adapter->dev, "%s detection function provided "
-			"no name for 0x%x\n", driver->driver.name,
-			addr);
+		dev_err(&adapter->dev,
+			"%s detection function provided no name for 0x%x\n",
+			driver->driver.name, addr);
 	} else {
 		struct i2c_client *client;
 
@@ -2659,9 +2758,8 @@ static int i2c_detect(struct i2c_adapter *adapter, struct i2c_driver *driver)
 	/* Warn that the adapter lost class based instantiation */
 	if (adapter->class == I2C_CLASS_DEPRECATED) {
 		dev_dbg(&adapter->dev,
-			"This adapter dropped support for I2C classes and "
-			"won't auto-detect %s devices anymore. If you need it, check "
-			"'Documentation/i2c/instantiating-devices' for alternatives.\n",
+			"This adapter dropped support for I2C classes and won't auto-detect %s devices anymore. "
+			"If you need it, check 'Documentation/i2c/instantiating-devices' for alternatives.\n",
 			driver->driver.name);
 		return 0;
 	}
@@ -2677,8 +2775,9 @@ static int i2c_detect(struct i2c_adapter *adapter, struct i2c_driver *driver)
 	temp_client->adapter = adapter;
 
 	for (i = 0; address_list[i] != I2C_CLIENT_END; i += 1) {
-		dev_dbg(&adapter->dev, "found normal entry for adapter %d, "
-			"addr 0x%02x\n", adap_id, address_list[i]);
+		dev_dbg(&adapter->dev,
+			"found normal entry for adapter %d, addr 0x%02x\n",
+			adap_id, address_list[i]);
 		temp_client->addr = address_list[i];
 		err = i2c_detect_address(temp_client, driver);
 		if (unlikely(err))
@@ -2710,15 +2809,16 @@ i2c_new_probed_device(struct i2c_adapter *adap,
 	for (i = 0; addr_list[i] != I2C_CLIENT_END; i++) {
 		/* Check address validity */
 		if (i2c_check_7bit_addr_validity_strict(addr_list[i]) < 0) {
-			dev_warn(&adap->dev, "Invalid 7-bit address "
-				 "0x%02x\n", addr_list[i]);
+			dev_warn(&adap->dev, "Invalid 7-bit address 0x%02x\n",
+				 addr_list[i]);
 			continue;
 		}
 
 		/* Check address availability (7 bit, no need to encode flags) */
 		if (i2c_check_addr_busy(adap, addr_list[i])) {
-			dev_dbg(&adap->dev, "Address 0x%02x already in "
-				"use, not probing\n", addr_list[i]);
+			dev_dbg(&adap->dev,
+				"Address 0x%02x already in use, not probing\n",
+				addr_list[i]);
 			continue;
 		}
 
diff --git a/drivers/i2c/i2c-mux.c b/drivers/i2c/i2c-mux.c
index 8eee98634cda..83768e85a919 100644
--- a/drivers/i2c/i2c-mux.c
+++ b/drivers/i2c/i2c-mux.c
@@ -159,7 +159,7 @@ static int i2c_mux_trylock_bus(struct i2c_adapter *adapter, unsigned int flags)
 		return 0;	/* mux_lock not locked, failure */
 	if (!(flags & I2C_LOCK_ROOT_ADAPTER))
 		return 1;	/* we only want mux_lock, success */
-	if (parent->trylock_bus(parent, flags))
+	if (i2c_trylock_bus(parent, flags))
 		return 1;	/* parent locked too, success */
 	rt_mutex_unlock(&parent->mux_lock);
 	return 0;		/* parent not locked, failure */
@@ -193,7 +193,7 @@ static int i2c_parent_trylock_bus(struct i2c_adapter *adapter,
 
 	if (!rt_mutex_trylock(&parent->mux_lock))
 		return 0;	/* mux_lock not locked, failure */
-	if (parent->trylock_bus(parent, flags))
+	if (i2c_trylock_bus(parent, flags))
 		return 1;	/* parent locked too, success */
 	rt_mutex_unlock(&parent->mux_lock);
 	return 0;		/* parent not locked, failure */
@@ -255,6 +255,10 @@ struct i2c_mux_core *i2c_mux_alloc(struct i2c_adapter *parent,
 	muxc->dev = dev;
 	if (flags & I2C_MUX_LOCKED)
 		muxc->mux_locked = true;
+	if (flags & I2C_MUX_ARBITRATOR)
+		muxc->arbitrator = true;
+	if (flags & I2C_MUX_GATE)
+		muxc->gate = true;
 	muxc->select = select;
 	muxc->deselect = deselect;
 	muxc->max_adapters = max_adapters;
@@ -263,6 +267,18 @@ struct i2c_mux_core *i2c_mux_alloc(struct i2c_adapter *parent,
 }
 EXPORT_SYMBOL_GPL(i2c_mux_alloc);
 
+static const struct i2c_lock_operations i2c_mux_lock_ops = {
+	.lock_bus =    i2c_mux_lock_bus,
+	.trylock_bus = i2c_mux_trylock_bus,
+	.unlock_bus =  i2c_mux_unlock_bus,
+};
+
+static const struct i2c_lock_operations i2c_parent_lock_ops = {
+	.lock_bus =    i2c_parent_lock_bus,
+	.trylock_bus = i2c_parent_trylock_bus,
+	.unlock_bus =  i2c_parent_unlock_bus,
+};
+
 int i2c_mux_add_adapter(struct i2c_mux_core *muxc,
 			u32 force_nr, u32 chan_id,
 			unsigned int class)
@@ -312,15 +328,10 @@ int i2c_mux_add_adapter(struct i2c_mux_core *muxc,
 	priv->adap.retries = parent->retries;
 	priv->adap.timeout = parent->timeout;
 	priv->adap.quirks = parent->quirks;
-	if (muxc->mux_locked) {
-		priv->adap.lock_bus = i2c_mux_lock_bus;
-		priv->adap.trylock_bus = i2c_mux_trylock_bus;
-		priv->adap.unlock_bus = i2c_mux_unlock_bus;
-	} else {
-		priv->adap.lock_bus = i2c_parent_lock_bus;
-		priv->adap.trylock_bus = i2c_parent_trylock_bus;
-		priv->adap.unlock_bus = i2c_parent_unlock_bus;
-	}
+	if (muxc->mux_locked)
+		priv->adap.lock_ops = &i2c_mux_lock_ops;
+	else
+		priv->adap.lock_ops = &i2c_parent_lock_ops;
 
 	/* Sanity check on class */
 	if (i2c_mux_parent_classes(parent) & class)
@@ -335,18 +346,42 @@ int i2c_mux_add_adapter(struct i2c_mux_core *muxc,
 	 * nothing if !CONFIG_OF.
 	 */
 	if (muxc->dev->of_node) {
-		struct device_node *child;
+		struct device_node *dev_node = muxc->dev->of_node;
+		struct device_node *mux_node, *child = NULL;
 		u32 reg;
 
-		for_each_child_of_node(muxc->dev->of_node, child) {
-			ret = of_property_read_u32(child, "reg", &reg);
-			if (ret)
-				continue;
-			if (chan_id == reg) {
-				priv->adap.dev.of_node = child;
-				break;
+		if (muxc->arbitrator)
+			mux_node = of_get_child_by_name(dev_node, "i2c-arb");
+		else if (muxc->gate)
+			mux_node = of_get_child_by_name(dev_node, "i2c-gate");
+		else
+			mux_node = of_get_child_by_name(dev_node, "i2c-mux");
+
+		if (mux_node) {
+			/* A "reg" property indicates an old-style DT entry */
+			if (!of_property_read_u32(mux_node, "reg", &reg)) {
+				of_node_put(mux_node);
+				mux_node = NULL;
+			}
+		}
+
+		if (!mux_node)
+			mux_node = of_node_get(dev_node);
+		else if (muxc->arbitrator || muxc->gate)
+			child = of_node_get(mux_node);
+
+		if (!child) {
+			for_each_child_of_node(mux_node, child) {
+				ret = of_property_read_u32(child, "reg", &reg);
+				if (ret)
+					continue;
+				if (chan_id == reg)
+					break;
 			}
 		}
+
+		priv->adap.dev.of_node = child;
+		of_node_put(mux_node);
 	}
 
 	/*
diff --git a/drivers/i2c/muxes/i2c-arb-gpio-challenge.c b/drivers/i2c/muxes/i2c-arb-gpio-challenge.c
index a90bbc4037dd..86fc2d4c081b 100644
--- a/drivers/i2c/muxes/i2c-arb-gpio-challenge.c
+++ b/drivers/i2c/muxes/i2c-arb-gpio-challenge.c
@@ -130,7 +130,7 @@ static int i2c_arbitrator_probe(struct platform_device *pdev)
 		return -EINVAL;
 	}
 
-	muxc = i2c_mux_alloc(NULL, dev, 1, sizeof(*arb), 0,
+	muxc = i2c_mux_alloc(NULL, dev, 1, sizeof(*arb), I2C_MUX_ARBITRATOR,
 			     i2c_arbitrator_select, i2c_arbitrator_deselect);
 	if (!muxc)
 		return -ENOMEM;
diff --git a/drivers/i2c/muxes/i2c-mux-pca9541.c b/drivers/i2c/muxes/i2c-mux-pca9541.c
index 3cb8af635db5..4ea7e691afc7 100644
--- a/drivers/i2c/muxes/i2c-mux-pca9541.c
+++ b/drivers/i2c/muxes/i2c-mux-pca9541.c
@@ -85,6 +85,13 @@ static const struct i2c_device_id pca9541_id[] = {
 
 MODULE_DEVICE_TABLE(i2c, pca9541_id);
 
+#ifdef CONFIG_OF
+static const struct of_device_id pca9541_of_match[] = {
+	{ .compatible = "nxp,pca9541" },
+	{}
+};
+#endif
+
 /*
  * Write to chip register. Don't use i2c_transfer()/i2c_smbus_xfer()
  * as they will try to lock the adapter a second time.
@@ -349,7 +356,8 @@ static int pca9541_probe(struct i2c_client *client,
 	force = 0;
 	if (pdata)
 		force = pdata->modes[0].adap_id;
-	muxc = i2c_mux_alloc(adap, &client->dev, 1, sizeof(*data), 0,
+	muxc = i2c_mux_alloc(adap, &client->dev, 1, sizeof(*data),
+			     I2C_MUX_ARBITRATOR,
 			     pca9541_select_chan, pca9541_release_chan);
 	if (!muxc)
 		return -ENOMEM;
@@ -382,6 +390,7 @@ static int pca9541_remove(struct i2c_client *client)
 static struct i2c_driver pca9541_driver = {
 	.driver = {
 		   .name = "pca9541",
+		   .of_match_table = of_match_ptr(pca9541_of_match),
 		   },
 	.probe = pca9541_probe,
 	.remove = pca9541_remove,
diff --git a/drivers/i2c/muxes/i2c-mux-pca954x.c b/drivers/i2c/muxes/i2c-mux-pca954x.c
index 3278ebf1cc5c..1091346f2480 100644
--- a/drivers/i2c/muxes/i2c-mux-pca954x.c
+++ b/drivers/i2c/muxes/i2c-mux-pca954x.c
@@ -42,6 +42,7 @@
 #include <linux/i2c/pca954x.h>
 #include <linux/module.h>
 #include <linux/of.h>
+#include <linux/of_device.h>
 #include <linux/pm.h>
 #include <linux/slab.h>
 
@@ -58,14 +59,6 @@ enum pca_type {
 	pca_9548,
 };
 
-struct pca954x {
-	enum pca_type type;
-
-	u8 last_chan;		/* last register value */
-	u8 deselect;
-	struct i2c_client *client;
-};
-
 struct chip_desc {
 	u8 nchans;
 	u8 enable;	/* used for muxes only */
@@ -75,6 +68,14 @@ struct chip_desc {
 	} muxtype;
 };
 
+struct pca954x {
+	const struct chip_desc *chip;
+
+	u8 last_chan;		/* last register value */
+	u8 deselect;
+	struct i2c_client *client;
+};
+
 /* Provide specs for the PCA954x types we know about */
 static const struct chip_desc chips[] = {
 	[pca_9540] = {
@@ -119,6 +120,20 @@ static const struct i2c_device_id pca954x_id[] = {
 };
 MODULE_DEVICE_TABLE(i2c, pca954x_id);
 
+#ifdef CONFIG_OF
+static const struct of_device_id pca954x_of_match[] = {
+	{ .compatible = "nxp,pca9540", .data = &chips[pca_9540] },
+	{ .compatible = "nxp,pca9542", .data = &chips[pca_9542] },
+	{ .compatible = "nxp,pca9543", .data = &chips[pca_9543] },
+	{ .compatible = "nxp,pca9544", .data = &chips[pca_9544] },
+	{ .compatible = "nxp,pca9545", .data = &chips[pca_9545] },
+	{ .compatible = "nxp,pca9546", .data = &chips[pca_9546] },
+	{ .compatible = "nxp,pca9547", .data = &chips[pca_9547] },
+	{ .compatible = "nxp,pca9548", .data = &chips[pca_9548] },
+	{}
+};
+#endif
+
 /* Write to mux register. Don't use i2c_transfer()/i2c_smbus_xfer()
    for this as they will try to lock adapter a second time */
 static int pca954x_reg_write(struct i2c_adapter *adap,
@@ -151,7 +166,7 @@ static int pca954x_select_chan(struct i2c_mux_core *muxc, u32 chan)
 {
 	struct pca954x *data = i2c_mux_priv(muxc);
 	struct i2c_client *client = data->client;
-	const struct chip_desc *chip = &chips[data->type];
+	const struct chip_desc *chip = data->chip;
 	u8 regval;
 	int ret = 0;
 
@@ -197,6 +212,7 @@ static int pca954x_probe(struct i2c_client *client,
 	int num, force, class;
 	struct i2c_mux_core *muxc;
 	struct pca954x *data;
+	const struct of_device_id *match;
 	int ret;
 
 	if (!i2c_check_functionality(adap, I2C_FUNC_SMBUS_BYTE))
@@ -226,14 +242,19 @@ static int pca954x_probe(struct i2c_client *client,
 		return -ENODEV;
 	}
 
-	data->type = id->driver_data;
+	match = of_match_device(of_match_ptr(pca954x_of_match), &client->dev);
+	if (match)
+		data->chip = of_device_get_match_data(&client->dev);
+	else
+		data->chip = &chips[id->driver_data];
+
 	data->last_chan = 0;		   /* force the first selection */
 
 	idle_disconnect_dt = of_node &&
 		of_property_read_bool(of_node, "i2c-mux-idle-disconnect");
 
 	/* Now create an adapter for each channel */
-	for (num = 0; num < chips[data->type].nchans; num++) {
+	for (num = 0; num < data->chip->nchans; num++) {
 		bool idle_disconnect_pd = false;
 
 		force = 0;			  /* dynamic adap number */
@@ -263,7 +284,7 @@ static int pca954x_probe(struct i2c_client *client,
 
 	dev_info(&client->dev,
 		 "registered %d multiplexed busses for I2C %s %s\n",
-		 num, chips[data->type].muxtype == pca954x_ismux
+		 num, data->chip->muxtype == pca954x_ismux
 				? "mux" : "switch", client->name);
 
 	return 0;
@@ -299,6 +320,7 @@ static struct i2c_driver pca954x_driver = {
 	.driver		= {
 		.name	= "pca954x",
 		.pm	= &pca954x_pm,
+		.of_match_table = of_match_ptr(pca954x_of_match),
 	},
 	.probe		= pca954x_probe,
 	.remove		= pca954x_remove,
diff --git a/drivers/idle/intel_idle.c b/drivers/idle/intel_idle.c
index 67ec58f9ef99..4466a2f969d7 100644
--- a/drivers/idle/intel_idle.c
+++ b/drivers/idle/intel_idle.c
@@ -863,8 +863,8 @@ static struct cpuidle_state dnv_cstates[] = {
  *
  * Must be called under local_irq_disable().
  */
-static int intel_idle(struct cpuidle_device *dev,
-		struct cpuidle_driver *drv, int index)
+static __cpuidle int intel_idle(struct cpuidle_device *dev,
+				struct cpuidle_driver *drv, int index)
 {
 	unsigned long ecx = 1; /* break on interrupt flag */
 	struct cpuidle_state *state = &drv->states[index];
diff --git a/drivers/iio/accel/Kconfig b/drivers/iio/accel/Kconfig
index 78f148ea9d9f..2b791fe1e2bc 100644
--- a/drivers/iio/accel/Kconfig
+++ b/drivers/iio/accel/Kconfig
@@ -52,6 +52,27 @@ config BMC150_ACCEL_SPI
 	tristate
 	select REGMAP_SPI
 
+config DMARD06
+	tristate "Domintech DMARD06 Digital Accelerometer Driver"
+	depends on OF || COMPILE_TEST
+	depends on I2C
+	help
+	  Say yes here to build support for the Domintech low-g tri-axial
+	  digital accelerometers: DMARD05, DMARD06, DMARD07.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called dmard06.
+
+config DMARD09
+	tristate "Domintech DMARD09 3-axis Accelerometer Driver"
+	depends on I2C
+	help
+	  Say yes here to get support for the Domintech DMARD09 3-axis
+	  accelerometer.
+
+	  Choosing M will build the driver as a module. If so, the module
+	  will be called dmard09.
+
 config HID_SENSOR_ACCEL_3D
 	depends on HID_SENSOR_HUB
 	select IIO_BUFFER
@@ -98,14 +119,35 @@ config IIO_ST_ACCEL_SPI_3AXIS
 
 config KXSD9
 	tristate "Kionix KXSD9 Accelerometer Driver"
-	depends on SPI
+	select IIO_BUFFER
+	select IIO_TRIGGERED_BUFFER
 	help
 	  Say yes here to build support for the Kionix KXSD9 accelerometer.
-	  Currently this only supports the device via an SPI interface.
+	  It can be accessed using an (optional) SPI or I2C interface.
 
 	  To compile this driver as a module, choose M here: the module
 	  will be called kxsd9.
 
+config KXSD9_SPI
+	tristate "Kionix KXSD9 SPI transport"
+	depends on KXSD9
+	depends on SPI
+	default KXSD9
+	select REGMAP_SPI
+	help
+	  Say yes here to enable the Kionix KXSD9 accelerometer
+	  SPI transport channel.
+
+config KXSD9_I2C
+	tristate "Kionix KXSD9 I2C transport"
+	depends on KXSD9
+	depends on I2C
+	default KXSD9
+	select REGMAP_I2C
+	help
+	  Say yes here to enable the Kionix KXSD9 accelerometer
+	  I2C transport channel.
+
 config KXCJK1013
 	tristate "Kionix 3-Axis Accelerometer Driver"
 	depends on I2C
@@ -119,6 +161,16 @@ config KXCJK1013
 	  To compile this driver as a module, choose M here: the module will
 	  be called kxcjk-1013.
 
+config MC3230
+	tristate "mCube MC3230 Digital Accelerometer Driver"
+	depends on I2C
+	help
+	  Say yes here to build support for the mCube MC3230 low-g tri-axial
+	  digital accelerometer.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called mc3230.
+
 config MMA7455
 	tristate
 	select IIO_BUFFER
diff --git a/drivers/iio/accel/Makefile b/drivers/iio/accel/Makefile
index 6cedbecca2ee..f5d3ddee619e 100644
--- a/drivers/iio/accel/Makefile
+++ b/drivers/iio/accel/Makefile
@@ -8,9 +8,14 @@ obj-$(CONFIG_BMA220) += bma220_spi.o
 obj-$(CONFIG_BMC150_ACCEL) += bmc150-accel-core.o
 obj-$(CONFIG_BMC150_ACCEL_I2C) += bmc150-accel-i2c.o
 obj-$(CONFIG_BMC150_ACCEL_SPI) += bmc150-accel-spi.o
+obj-$(CONFIG_DMARD06)	+= dmard06.o
+obj-$(CONFIG_DMARD09)	+= dmard09.o
 obj-$(CONFIG_HID_SENSOR_ACCEL_3D) += hid-sensor-accel-3d.o
 obj-$(CONFIG_KXCJK1013) += kxcjk-1013.o
 obj-$(CONFIG_KXSD9)	+= kxsd9.o
+obj-$(CONFIG_KXSD9_SPI)	+= kxsd9-spi.o
+obj-$(CONFIG_KXSD9_I2C)	+= kxsd9-i2c.o
+obj-$(CONFIG_MC3230)	+= mc3230.o
 
 obj-$(CONFIG_MMA7455)		+= mma7455_core.o
 obj-$(CONFIG_MMA7455_I2C)	+= mma7455_i2c.o
diff --git a/drivers/iio/accel/bma180.c b/drivers/iio/accel/bma180.c
index e3f88ba5faf3..0890934ef66f 100644
--- a/drivers/iio/accel/bma180.c
+++ b/drivers/iio/accel/bma180.c
@@ -469,13 +469,14 @@ static int bma180_read_raw(struct iio_dev *indio_dev,
 
 	switch (mask) {
 	case IIO_CHAN_INFO_RAW:
+		ret = iio_device_claim_direct_mode(indio_dev);
+		if (ret)
+			return ret;
+
 		mutex_lock(&data->mutex);
-		if (iio_buffer_enabled(indio_dev)) {
-			mutex_unlock(&data->mutex);
-			return -EBUSY;
-		}
 		ret = bma180_get_data_reg(data, chan->scan_index);
 		mutex_unlock(&data->mutex);
+		iio_device_release_direct_mode(indio_dev);
 		if (ret < 0)
 			return ret;
 		*val = sign_extend32(ret >> chan->scan_type.shift,
diff --git a/drivers/iio/accel/dmard06.c b/drivers/iio/accel/dmard06.c
new file mode 100644
index 000000000000..656ca8e1927f
--- /dev/null
+++ b/drivers/iio/accel/dmard06.c
@@ -0,0 +1,241 @@
+/*
+ * IIO driver for Domintech DMARD06 accelerometer
+ *
+ * Copyright (C) 2016 Aleksei Mamlin <mamlinav@gmail.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.
+ */
+
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/iio/iio.h>
+
+#define DMARD06_DRV_NAME		"dmard06"
+
+/* Device data registers */
+#define DMARD06_CHIP_ID_REG		0x0f
+#define DMARD06_TOUT_REG		0x40
+#define DMARD06_XOUT_REG		0x41
+#define DMARD06_YOUT_REG		0x42
+#define DMARD06_ZOUT_REG		0x43
+#define DMARD06_CTRL1_REG		0x44
+
+/* Device ID value */
+#define DMARD05_CHIP_ID			0x05
+#define DMARD06_CHIP_ID			0x06
+#define DMARD07_CHIP_ID			0x07
+
+/* Device values */
+#define DMARD05_AXIS_SCALE_VAL		15625
+#define DMARD06_AXIS_SCALE_VAL		31250
+#define DMARD06_TEMP_CENTER_VAL		25
+#define DMARD06_SIGN_BIT		7
+
+/* Device power modes */
+#define DMARD06_MODE_NORMAL		0x27
+#define DMARD06_MODE_POWERDOWN		0x00
+
+/* Device channels */
+#define DMARD06_ACCEL_CHANNEL(_axis, _reg) {			\
+	.type = IIO_ACCEL,					\
+	.address = _reg,					\
+	.channel2 = IIO_MOD_##_axis,				\
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),		\
+	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),	\
+	.modified = 1,						\
+}
+
+#define DMARD06_TEMP_CHANNEL(_reg) {				\
+	.type = IIO_TEMP,					\
+	.address = _reg,					\
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |		\
+			      BIT(IIO_CHAN_INFO_OFFSET),	\
+}
+
+struct dmard06_data {
+	struct i2c_client *client;
+	u8 chip_id;
+};
+
+static const struct iio_chan_spec dmard06_channels[] = {
+	DMARD06_ACCEL_CHANNEL(X, DMARD06_XOUT_REG),
+	DMARD06_ACCEL_CHANNEL(Y, DMARD06_YOUT_REG),
+	DMARD06_ACCEL_CHANNEL(Z, DMARD06_ZOUT_REG),
+	DMARD06_TEMP_CHANNEL(DMARD06_TOUT_REG),
+};
+
+static int dmard06_read_raw(struct iio_dev *indio_dev,
+			    struct iio_chan_spec const *chan,
+			    int *val, int *val2, long mask)
+{
+	struct dmard06_data *dmard06 = iio_priv(indio_dev);
+	int ret;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		ret = i2c_smbus_read_byte_data(dmard06->client,
+					       chan->address);
+		if (ret < 0) {
+			dev_err(&dmard06->client->dev,
+				"Error reading data: %d\n", ret);
+			return ret;
+		}
+
+		*val = sign_extend32(ret, DMARD06_SIGN_BIT);
+
+		if (dmard06->chip_id == DMARD06_CHIP_ID)
+			*val = *val >> 1;
+
+		switch (chan->type) {
+		case IIO_ACCEL:
+			return IIO_VAL_INT;
+		case IIO_TEMP:
+			if (dmard06->chip_id != DMARD06_CHIP_ID)
+				*val = *val / 2;
+			return IIO_VAL_INT;
+		default:
+			return -EINVAL;
+		}
+	case IIO_CHAN_INFO_OFFSET:
+		switch (chan->type) {
+		case IIO_TEMP:
+			*val = DMARD06_TEMP_CENTER_VAL;
+			return IIO_VAL_INT;
+		default:
+			return -EINVAL;
+		}
+	case IIO_CHAN_INFO_SCALE:
+		switch (chan->type) {
+		case IIO_ACCEL:
+			*val = 0;
+			if (dmard06->chip_id == DMARD06_CHIP_ID)
+				*val2 = DMARD06_AXIS_SCALE_VAL;
+			else
+				*val2 = DMARD05_AXIS_SCALE_VAL;
+			return IIO_VAL_INT_PLUS_MICRO;
+		default:
+			return -EINVAL;
+		}
+	default:
+		return -EINVAL;
+	}
+}
+
+static const struct iio_info dmard06_info = {
+	.driver_module	= THIS_MODULE,
+	.read_raw	= dmard06_read_raw,
+};
+
+static int dmard06_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	int ret;
+	struct iio_dev *indio_dev;
+	struct dmard06_data *dmard06;
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		dev_err(&client->dev, "I2C check functionality failed\n");
+		return -ENXIO;
+	}
+
+	indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*dmard06));
+	if (!indio_dev) {
+		dev_err(&client->dev, "Failed to allocate iio device\n");
+		return -ENOMEM;
+	}
+
+	dmard06 = iio_priv(indio_dev);
+	dmard06->client = client;
+
+	ret = i2c_smbus_read_byte_data(dmard06->client, DMARD06_CHIP_ID_REG);
+	if (ret < 0) {
+		dev_err(&client->dev, "Error reading chip id: %d\n", ret);
+		return ret;
+	}
+
+	if (ret != DMARD05_CHIP_ID && ret != DMARD06_CHIP_ID &&
+	    ret != DMARD07_CHIP_ID) {
+		dev_err(&client->dev, "Invalid chip id: %02d\n", ret);
+		return -ENODEV;
+	}
+
+	dmard06->chip_id = ret;
+
+	i2c_set_clientdata(client, indio_dev);
+	indio_dev->dev.parent = &client->dev;
+	indio_dev->name = DMARD06_DRV_NAME;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->channels = dmard06_channels;
+	indio_dev->num_channels = ARRAY_SIZE(dmard06_channels);
+	indio_dev->info = &dmard06_info;
+
+	return devm_iio_device_register(&client->dev, indio_dev);
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int dmard06_suspend(struct device *dev)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(to_i2c_client(dev));
+	struct dmard06_data *dmard06 = iio_priv(indio_dev);
+	int ret;
+
+	ret = i2c_smbus_write_byte_data(dmard06->client, DMARD06_CTRL1_REG,
+					DMARD06_MODE_POWERDOWN);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+
+static int dmard06_resume(struct device *dev)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(to_i2c_client(dev));
+	struct dmard06_data *dmard06 = iio_priv(indio_dev);
+	int ret;
+
+	ret = i2c_smbus_write_byte_data(dmard06->client, DMARD06_CTRL1_REG,
+					DMARD06_MODE_NORMAL);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+
+static SIMPLE_DEV_PM_OPS(dmard06_pm_ops, dmard06_suspend, dmard06_resume);
+#define DMARD06_PM_OPS (&dmard06_pm_ops)
+#else
+#define DMARD06_PM_OPS NULL
+#endif
+
+static const struct i2c_device_id dmard06_id[] = {
+	{ "dmard05", 0 },
+	{ "dmard06", 0 },
+	{ "dmard07", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, dmard06_id);
+
+static const struct of_device_id dmard06_of_match[] = {
+	{ .compatible = "domintech,dmard05" },
+	{ .compatible = "domintech,dmard06" },
+	{ .compatible = "domintech,dmard07" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, dmard06_of_match);
+
+static struct i2c_driver dmard06_driver = {
+	.probe = dmard06_probe,
+	.id_table = dmard06_id,
+	.driver = {
+		.name = DMARD06_DRV_NAME,
+		.of_match_table = of_match_ptr(dmard06_of_match),
+		.pm = DMARD06_PM_OPS,
+	},
+};
+module_i2c_driver(dmard06_driver);
+
+MODULE_AUTHOR("Aleksei Mamlin <mamlinav@gmail.com>");
+MODULE_DESCRIPTION("Domintech DMARD06 accelerometer driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/accel/dmard09.c b/drivers/iio/accel/dmard09.c
new file mode 100644
index 000000000000..d3a28f96565c
--- /dev/null
+++ b/drivers/iio/accel/dmard09.c
@@ -0,0 +1,157 @@
+/*
+ * IIO driver for the 3-axis accelerometer Domintech DMARD09.
+ *
+ * Copyright (c) 2016, Jelle van der Waa <jelle@vdwaa.nl>
+ *
+ * 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 <asm/unaligned.h>
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/iio/iio.h>
+
+#define DMARD09_DRV_NAME	"dmard09"
+
+#define DMARD09_REG_CHIPID      0x18
+#define DMARD09_REG_STAT	0x0A
+#define DMARD09_REG_X		0x0C
+#define DMARD09_REG_Y		0x0E
+#define DMARD09_REG_Z		0x10
+#define DMARD09_CHIPID		0x95
+
+#define DMARD09_BUF_LEN 8
+#define DMARD09_AXIS_X 0
+#define DMARD09_AXIS_Y 1
+#define DMARD09_AXIS_Z 2
+#define DMARD09_AXIS_X_OFFSET ((DMARD09_AXIS_X + 1) * 2)
+#define DMARD09_AXIS_Y_OFFSET ((DMARD09_AXIS_Y + 1 )* 2)
+#define DMARD09_AXIS_Z_OFFSET ((DMARD09_AXIS_Z + 1) * 2)
+
+struct dmard09_data {
+	struct i2c_client *client;
+};
+
+#define DMARD09_CHANNEL(_axis, offset) {			\
+	.type = IIO_ACCEL,					\
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),		\
+	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),	\
+	.modified = 1,						\
+	.address = offset,					\
+	.channel2 = IIO_MOD_##_axis,				\
+}
+
+static const struct iio_chan_spec dmard09_channels[] = {
+	DMARD09_CHANNEL(X, DMARD09_AXIS_X_OFFSET),
+	DMARD09_CHANNEL(Y, DMARD09_AXIS_Y_OFFSET),
+	DMARD09_CHANNEL(Z, DMARD09_AXIS_Z_OFFSET),
+};
+
+static int dmard09_read_raw(struct iio_dev *indio_dev,
+			    struct iio_chan_spec const *chan,
+			    int *val, int *val2, long mask)
+{
+	struct dmard09_data *data = iio_priv(indio_dev);
+	u8 buf[DMARD09_BUF_LEN];
+	int ret;
+	s16 accel;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		/*
+		 * Read from the DMAR09_REG_STAT register, since the chip
+		 * caches reads from the individual X, Y, Z registers.
+		 */
+		ret = i2c_smbus_read_i2c_block_data(data->client,
+						    DMARD09_REG_STAT,
+						    DMARD09_BUF_LEN, buf);
+		if (ret < 0) {
+			dev_err(&data->client->dev, "Error reading reg %d\n",
+				DMARD09_REG_STAT);
+			return ret;
+		}
+
+		accel = get_unaligned_le16(&buf[chan->address]);
+
+		/* Remove lower 3 bits and sign extend */
+		accel <<= 4;
+		accel >>= 7;
+
+		*val = accel;
+
+		return IIO_VAL_INT;
+	default:
+		return -EINVAL;
+	}
+}
+
+static const struct iio_info dmard09_info = {
+	.driver_module	= THIS_MODULE,
+	.read_raw	= dmard09_read_raw,
+};
+
+static int dmard09_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	int ret;
+	struct iio_dev *indio_dev;
+	struct dmard09_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;
+
+	ret = i2c_smbus_read_byte_data(data->client, DMARD09_REG_CHIPID);
+	if (ret < 0) {
+		dev_err(&client->dev, "Error reading chip id %d\n", ret);
+		return ret;
+	}
+
+	if (ret != DMARD09_CHIPID) {
+		dev_err(&client->dev, "Invalid chip id %d\n", ret);
+		return -ENODEV;
+	}
+
+	i2c_set_clientdata(client, indio_dev);
+	indio_dev->dev.parent = &client->dev;
+	indio_dev->name = DMARD09_DRV_NAME;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->channels = dmard09_channels;
+	indio_dev->num_channels = ARRAY_SIZE(dmard09_channels);
+	indio_dev->info = &dmard09_info;
+
+	return devm_iio_device_register(&client->dev, indio_dev);
+}
+
+static const struct i2c_device_id dmard09_id[] = {
+	{ "dmard09", 0},
+	{ },
+};
+
+MODULE_DEVICE_TABLE(i2c, dmard09_id);
+
+static struct i2c_driver dmard09_driver = {
+	.driver = {
+		.name = DMARD09_DRV_NAME
+	},
+	.probe = dmard09_probe,
+	.id_table = dmard09_id,
+};
+
+module_i2c_driver(dmard09_driver);
+
+MODULE_AUTHOR("Jelle van der Waa <jelle@vdwaa.nl>");
+MODULE_DESCRIPTION("DMARD09 3-axis accelerometer driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/iio/accel/kxcjk-1013.c b/drivers/iio/accel/kxcjk-1013.c
index 765a72362dc6..3f968c46e667 100644
--- a/drivers/iio/accel/kxcjk-1013.c
+++ b/drivers/iio/accel/kxcjk-1013.c
@@ -1392,6 +1392,7 @@ static const struct acpi_device_id kx_acpi_match[] = {
 	{"KXCJ1013", KXCJK1013},
 	{"KXCJ1008", KXCJ91008},
 	{"KXCJ9000", KXCJ91008},
+	{"KIOX000A", KXCJ91008},
 	{"KXTJ1009", KXTJ21009},
 	{"SMO8500",  KXCJ91008},
 	{ },
diff --git a/drivers/iio/accel/kxsd9-i2c.c b/drivers/iio/accel/kxsd9-i2c.c
new file mode 100644
index 000000000000..95e20855d2ef
--- /dev/null
+++ b/drivers/iio/accel/kxsd9-i2c.c
@@ -0,0 +1,64 @@
+#include <linux/device.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/delay.h>
+#include <linux/regmap.h>
+
+#include "kxsd9.h"
+
+static int kxsd9_i2c_probe(struct i2c_client *i2c,
+			   const struct i2c_device_id *id)
+{
+	static const struct regmap_config config = {
+		.reg_bits = 8,
+		.val_bits = 8,
+		.max_register = 0x0e,
+	};
+	struct regmap *regmap;
+
+	regmap = devm_regmap_init_i2c(i2c, &config);
+	if (IS_ERR(regmap)) {
+		dev_err(&i2c->dev, "Failed to register i2c regmap %d\n",
+			(int)PTR_ERR(regmap));
+		return PTR_ERR(regmap);
+	}
+
+	return kxsd9_common_probe(&i2c->dev,
+				  regmap,
+				  i2c->name);
+}
+
+static int kxsd9_i2c_remove(struct i2c_client *client)
+{
+	return kxsd9_common_remove(&client->dev);
+}
+
+#ifdef CONFIG_OF
+static const struct of_device_id kxsd9_of_match[] = {
+	{ .compatible = "kionix,kxsd9", },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, kxsd9_of_match);
+#else
+#define kxsd9_of_match NULL
+#endif
+
+static const struct i2c_device_id kxsd9_i2c_id[] = {
+	{"kxsd9", 0},
+	{ },
+};
+MODULE_DEVICE_TABLE(i2c, kxsd9_i2c_id);
+
+static struct i2c_driver kxsd9_i2c_driver = {
+	.driver = {
+		.name	= "kxsd9",
+		.of_match_table = of_match_ptr(kxsd9_of_match),
+		.pm = &kxsd9_dev_pm_ops,
+	},
+	.probe		= kxsd9_i2c_probe,
+	.remove		= kxsd9_i2c_remove,
+	.id_table	= kxsd9_i2c_id,
+};
+module_i2c_driver(kxsd9_i2c_driver);
diff --git a/drivers/iio/accel/kxsd9-spi.c b/drivers/iio/accel/kxsd9-spi.c
new file mode 100644
index 000000000000..b7d0078fd00e
--- /dev/null
+++ b/drivers/iio/accel/kxsd9-spi.c
@@ -0,0 +1,56 @@
+#include <linux/device.h>
+#include <linux/kernel.h>
+#include <linux/spi/spi.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/regmap.h>
+
+#include "kxsd9.h"
+
+static int kxsd9_spi_probe(struct spi_device *spi)
+{
+	static const struct regmap_config config = {
+		.reg_bits = 8,
+		.val_bits = 8,
+		.max_register = 0x0e,
+	};
+	struct regmap *regmap;
+
+	spi->mode = SPI_MODE_0;
+	regmap = devm_regmap_init_spi(spi, &config);
+	if (IS_ERR(regmap)) {
+		dev_err(&spi->dev, "%s: regmap allocation failed: %ld\n",
+			__func__, PTR_ERR(regmap));
+		return PTR_ERR(regmap);
+	}
+
+	return kxsd9_common_probe(&spi->dev,
+				  regmap,
+				  spi_get_device_id(spi)->name);
+}
+
+static int kxsd9_spi_remove(struct spi_device *spi)
+{
+	return kxsd9_common_remove(&spi->dev);
+}
+
+static const struct spi_device_id kxsd9_spi_id[] = {
+	{"kxsd9", 0},
+	{ },
+};
+MODULE_DEVICE_TABLE(spi, kxsd9_spi_id);
+
+static struct spi_driver kxsd9_spi_driver = {
+	.driver = {
+		.name = "kxsd9",
+		.pm = &kxsd9_dev_pm_ops,
+	},
+	.probe = kxsd9_spi_probe,
+	.remove = kxsd9_spi_remove,
+	.id_table = kxsd9_spi_id,
+};
+module_spi_driver(kxsd9_spi_driver);
+
+MODULE_AUTHOR("Jonathan Cameron <jic23@kernel.org>");
+MODULE_DESCRIPTION("Kionix KXSD9 SPI driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/accel/kxsd9.c b/drivers/iio/accel/kxsd9.c
index 9d72d4bcf5e9..9af60ac70738 100644
--- a/drivers/iio/accel/kxsd9.c
+++ b/drivers/iio/accel/kxsd9.c
@@ -12,19 +12,25 @@
  * I have a suitable wire made up.
  *
  * TODO:	Support the motion detector
- *		Uses register address incrementing so could have a
- *		heavily optimized ring buffer access function.
  */
 
 #include <linux/device.h>
 #include <linux/kernel.h>
-#include <linux/spi/spi.h>
 #include <linux/sysfs.h>
 #include <linux/slab.h>
 #include <linux/module.h>
-
+#include <linux/regmap.h>
+#include <linux/bitops.h>
+#include <linux/delay.h>
+#include <linux/regulator/consumer.h>
+#include <linux/pm_runtime.h>
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/triggered_buffer.h>
+#include <linux/iio/trigger_consumer.h>
+
+#include "kxsd9.h"
 
 #define KXSD9_REG_X		0x00
 #define KXSD9_REG_Y		0x02
@@ -33,28 +39,45 @@
 #define KXSD9_REG_RESET		0x0a
 #define KXSD9_REG_CTRL_C	0x0c
 
-#define KXSD9_FS_MASK		0x03
+#define KXSD9_CTRL_C_FS_MASK	0x03
+#define KXSD9_CTRL_C_FS_8G	0x00
+#define KXSD9_CTRL_C_FS_6G	0x01
+#define KXSD9_CTRL_C_FS_4G	0x02
+#define KXSD9_CTRL_C_FS_2G	0x03
+#define KXSD9_CTRL_C_MOT_LAT	BIT(3)
+#define KXSD9_CTRL_C_MOT_LEV	BIT(4)
+#define KXSD9_CTRL_C_LP_MASK	0xe0
+#define KXSD9_CTRL_C_LP_NONE	0x00
+#define KXSD9_CTRL_C_LP_2000HZC	BIT(5)
+#define KXSD9_CTRL_C_LP_2000HZB	BIT(6)
+#define KXSD9_CTRL_C_LP_2000HZA	(BIT(5)|BIT(6))
+#define KXSD9_CTRL_C_LP_1000HZ	BIT(7)
+#define KXSD9_CTRL_C_LP_500HZ	(BIT(7)|BIT(5))
+#define KXSD9_CTRL_C_LP_100HZ	(BIT(7)|BIT(6))
+#define KXSD9_CTRL_C_LP_50HZ	(BIT(7)|BIT(6)|BIT(5))
 
 #define KXSD9_REG_CTRL_B	0x0d
-#define KXSD9_REG_CTRL_A	0x0e
 
-#define KXSD9_READ(a) (0x80 | (a))
-#define KXSD9_WRITE(a) (a)
+#define KXSD9_CTRL_B_CLK_HLD	BIT(7)
+#define KXSD9_CTRL_B_ENABLE	BIT(6)
+#define KXSD9_CTRL_B_ST		BIT(5) /* Self-test */
+
+#define KXSD9_REG_CTRL_A	0x0e
 
-#define KXSD9_STATE_RX_SIZE 2
-#define KXSD9_STATE_TX_SIZE 2
 /**
  * struct kxsd9_state - device related storage
- * @buf_lock:	protect the rx and tx buffers.
- * @us:		spi device
- * @rx:		single rx buffer storage
- * @tx:		single tx buffer storage
- **/
+ * @dev: pointer to the parent device
+ * @map: regmap to the device
+ * @orientation: mounting matrix, flipped axis etc
+ * @regs: regulators for this device, VDD and IOVDD
+ * @scale: the current scaling setting
+ */
 struct kxsd9_state {
-	struct mutex buf_lock;
-	struct spi_device *us;
-	u8 rx[KXSD9_STATE_RX_SIZE] ____cacheline_aligned;
-	u8 tx[KXSD9_STATE_TX_SIZE];
+	struct device *dev;
+	struct regmap *map;
+	struct iio_mount_matrix orientation;
+	struct regulator_bulk_data regs[2];
+	u8 scale;
 };
 
 #define KXSD9_SCALE_2G "0.011978"
@@ -65,6 +88,14 @@ struct kxsd9_state {
 /* reverse order */
 static const int kxsd9_micro_scales[4] = { 47853, 35934, 23927, 11978 };
 
+#define KXSD9_ZERO_G_OFFSET -2048
+
+/*
+ * Regulator names
+ */
+static const char kxsd9_reg_vdd[] = "vdd";
+static const char kxsd9_reg_iovdd[] = "iovdd";
+
 static int kxsd9_write_scale(struct iio_dev *indio_dev, int micro)
 {
 	int ret, i;
@@ -79,42 +110,17 @@ static int kxsd9_write_scale(struct iio_dev *indio_dev, int micro)
 	if (!foundit)
 		return -EINVAL;
 
-	mutex_lock(&st->buf_lock);
-	ret = spi_w8r8(st->us, KXSD9_READ(KXSD9_REG_CTRL_C));
+	ret = regmap_update_bits(st->map,
+				 KXSD9_REG_CTRL_C,
+				 KXSD9_CTRL_C_FS_MASK,
+				 i);
 	if (ret < 0)
 		goto error_ret;
-	st->tx[0] = KXSD9_WRITE(KXSD9_REG_CTRL_C);
-	st->tx[1] = (ret & ~KXSD9_FS_MASK) | i;
 
-	ret = spi_write(st->us, st->tx, 2);
-error_ret:
-	mutex_unlock(&st->buf_lock);
-	return ret;
-}
+	/* Cached scale when the sensor is powered down */
+	st->scale = i;
 
-static int kxsd9_read(struct iio_dev *indio_dev, u8 address)
-{
-	int ret;
-	struct kxsd9_state *st = iio_priv(indio_dev);
-	struct spi_transfer xfers[] = {
-		{
-			.bits_per_word = 8,
-			.len = 1,
-			.delay_usecs = 200,
-			.tx_buf = st->tx,
-		}, {
-			.bits_per_word = 8,
-			.len = 2,
-			.rx_buf = st->rx,
-		},
-	};
-
-	mutex_lock(&st->buf_lock);
-	st->tx[0] = KXSD9_READ(address);
-	ret = spi_sync_transfer(st->us, xfers, ARRAY_SIZE(xfers));
-	if (!ret)
-		ret = (((u16)(st->rx[0])) << 8) | (st->rx[1] & 0xF0);
-	mutex_unlock(&st->buf_lock);
+error_ret:
 	return ret;
 }
 
@@ -136,6 +142,9 @@ static int kxsd9_write_raw(struct iio_dev *indio_dev,
 			   long mask)
 {
 	int ret = -EINVAL;
+	struct kxsd9_state *st = iio_priv(indio_dev);
+
+	pm_runtime_get_sync(st->dev);
 
 	if (mask == IIO_CHAN_INFO_SCALE) {
 		/* Check no integer component */
@@ -144,6 +153,9 @@ static int kxsd9_write_raw(struct iio_dev *indio_dev,
 		ret = kxsd9_write_scale(indio_dev, val2);
 	}
 
+	pm_runtime_mark_last_busy(st->dev);
+	pm_runtime_put_autosuspend(st->dev);
+
 	return ret;
 }
 
@@ -153,46 +165,154 @@ static int kxsd9_read_raw(struct iio_dev *indio_dev,
 {
 	int ret = -EINVAL;
 	struct kxsd9_state *st = iio_priv(indio_dev);
+	unsigned int regval;
+	__be16 raw_val;
+	u16 nval;
+
+	pm_runtime_get_sync(st->dev);
 
 	switch (mask) {
 	case IIO_CHAN_INFO_RAW:
-		ret = kxsd9_read(indio_dev, chan->address);
-		if (ret < 0)
+		ret = regmap_bulk_read(st->map, chan->address, &raw_val,
+				       sizeof(raw_val));
+		if (ret)
 			goto error_ret;
-		*val = ret;
+		nval = be16_to_cpu(raw_val);
+		/* Only 12 bits are valid */
+		nval >>= 4;
+		*val = nval;
+		ret = IIO_VAL_INT;
+		break;
+	case IIO_CHAN_INFO_OFFSET:
+		/* This has a bias of -2048 */
+		*val = KXSD9_ZERO_G_OFFSET;
 		ret = IIO_VAL_INT;
 		break;
 	case IIO_CHAN_INFO_SCALE:
-		ret = spi_w8r8(st->us, KXSD9_READ(KXSD9_REG_CTRL_C));
+		ret = regmap_read(st->map,
+				  KXSD9_REG_CTRL_C,
+				  &regval);
 		if (ret < 0)
 			goto error_ret;
 		*val = 0;
-		*val2 = kxsd9_micro_scales[ret & KXSD9_FS_MASK];
+		*val2 = kxsd9_micro_scales[regval & KXSD9_CTRL_C_FS_MASK];
 		ret = IIO_VAL_INT_PLUS_MICRO;
 		break;
 	}
 
 error_ret:
+	pm_runtime_mark_last_busy(st->dev);
+	pm_runtime_put_autosuspend(st->dev);
+
 	return ret;
 };
-#define KXSD9_ACCEL_CHAN(axis)						\
+
+static irqreturn_t kxsd9_trigger_handler(int irq, void *p)
+{
+	const struct iio_poll_func *pf = p;
+	struct iio_dev *indio_dev = pf->indio_dev;
+	struct kxsd9_state *st = iio_priv(indio_dev);
+	int ret;
+	/* 4 * 16bit values AND timestamp */
+	__be16 hw_values[8];
+
+	ret = regmap_bulk_read(st->map,
+			       KXSD9_REG_X,
+			       &hw_values,
+			       8);
+	if (ret) {
+		dev_err(st->dev,
+			"error reading data\n");
+		return ret;
+	}
+
+	iio_push_to_buffers_with_timestamp(indio_dev,
+					   hw_values,
+					   iio_get_time_ns(indio_dev));
+	iio_trigger_notify_done(indio_dev->trig);
+
+	return IRQ_HANDLED;
+}
+
+static int kxsd9_buffer_preenable(struct iio_dev *indio_dev)
+{
+	struct kxsd9_state *st = iio_priv(indio_dev);
+
+	pm_runtime_get_sync(st->dev);
+
+	return 0;
+}
+
+static int kxsd9_buffer_postdisable(struct iio_dev *indio_dev)
+{
+	struct kxsd9_state *st = iio_priv(indio_dev);
+
+	pm_runtime_mark_last_busy(st->dev);
+	pm_runtime_put_autosuspend(st->dev);
+
+	return 0;
+}
+
+static const struct iio_buffer_setup_ops kxsd9_buffer_setup_ops = {
+	.preenable = kxsd9_buffer_preenable,
+	.postenable = iio_triggered_buffer_postenable,
+	.predisable = iio_triggered_buffer_predisable,
+	.postdisable = kxsd9_buffer_postdisable,
+};
+
+static const struct iio_mount_matrix *
+kxsd9_get_mount_matrix(const struct iio_dev *indio_dev,
+		       const struct iio_chan_spec *chan)
+{
+	struct kxsd9_state *st = iio_priv(indio_dev);
+
+	return &st->orientation;
+}
+
+static const struct iio_chan_spec_ext_info kxsd9_ext_info[] = {
+	IIO_MOUNT_MATRIX(IIO_SHARED_BY_TYPE, kxsd9_get_mount_matrix),
+	{ },
+};
+
+#define KXSD9_ACCEL_CHAN(axis, index)						\
 	{								\
 		.type = IIO_ACCEL,					\
 		.modified = 1,						\
 		.channel2 = IIO_MOD_##axis,				\
 		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),		\
-		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),	\
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE) |	\
+					BIT(IIO_CHAN_INFO_OFFSET),	\
+		.ext_info = kxsd9_ext_info,				\
 		.address = KXSD9_REG_##axis,				\
+		.scan_index = index,					\
+		.scan_type = {                                          \
+			.sign = 'u',					\
+			.realbits = 12,					\
+			.storagebits = 16,				\
+			.shift = 4,					\
+			.endianness = IIO_BE,				\
+		},							\
 	}
 
 static const struct iio_chan_spec kxsd9_channels[] = {
-	KXSD9_ACCEL_CHAN(X), KXSD9_ACCEL_CHAN(Y), KXSD9_ACCEL_CHAN(Z),
+	KXSD9_ACCEL_CHAN(X, 0),
+	KXSD9_ACCEL_CHAN(Y, 1),
+	KXSD9_ACCEL_CHAN(Z, 2),
 	{
 		.type = IIO_VOLTAGE,
 		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
 		.indexed = 1,
 		.address = KXSD9_REG_AUX,
-	}
+		.scan_index = 3,
+		.scan_type = {
+			.sign = 'u',
+			.realbits = 12,
+			.storagebits = 16,
+			.shift = 4,
+			.endianness = IIO_BE,
+		},
+	},
+	IIO_CHAN_SOFT_TIMESTAMP(4),
 };
 
 static const struct attribute_group kxsd9_attribute_group = {
@@ -203,17 +323,69 @@ static int kxsd9_power_up(struct kxsd9_state *st)
 {
 	int ret;
 
-	st->tx[0] = 0x0d;
-	st->tx[1] = 0x40;
-	ret = spi_write(st->us, st->tx, 2);
+	/* Enable the regulators */
+	ret = regulator_bulk_enable(ARRAY_SIZE(st->regs), st->regs);
+	if (ret) {
+		dev_err(st->dev, "Cannot enable regulators\n");
+		return ret;
+	}
+
+	/* Power up */
+	ret = regmap_write(st->map,
+			   KXSD9_REG_CTRL_B,
+			   KXSD9_CTRL_B_ENABLE);
 	if (ret)
 		return ret;
 
-	st->tx[0] = 0x0c;
-	st->tx[1] = 0x9b;
-	return spi_write(st->us, st->tx, 2);
+	/*
+	 * Set 1000Hz LPF, 2g fullscale, motion wakeup threshold 1g,
+	 * latched wakeup
+	 */
+	ret = regmap_write(st->map,
+			   KXSD9_REG_CTRL_C,
+			   KXSD9_CTRL_C_LP_1000HZ |
+			   KXSD9_CTRL_C_MOT_LEV	|
+			   KXSD9_CTRL_C_MOT_LAT |
+			   st->scale);
+	if (ret)
+		return ret;
+
+	/*
+	 * Power-up time depends on the LPF setting, but typ 15.9 ms, let's
+	 * set 20 ms to allow for some slack.
+	 */
+	msleep(20);
+
+	return 0;
 };
 
+static int kxsd9_power_down(struct kxsd9_state *st)
+{
+	int ret;
+
+	/*
+	 * Set into low power mode - since there may be more users of the
+	 * regulators this is the first step of the power saving: it will
+	 * make sure we conserve power even if there are others users on the
+	 * regulators.
+	 */
+	ret = regmap_update_bits(st->map,
+				 KXSD9_REG_CTRL_B,
+				 KXSD9_CTRL_B_ENABLE,
+				 0);
+	if (ret)
+		return ret;
+
+	/* Disable the regulators */
+	ret = regulator_bulk_disable(ARRAY_SIZE(st->regs), st->regs);
+	if (ret) {
+		dev_err(st->dev, "Cannot disable regulators\n");
+		return ret;
+	}
+
+	return 0;
+}
+
 static const struct iio_info kxsd9_info = {
 	.read_raw = &kxsd9_read_raw,
 	.write_raw = &kxsd9_write_raw,
@@ -221,57 +393,136 @@ static const struct iio_info kxsd9_info = {
 	.driver_module = THIS_MODULE,
 };
 
-static int kxsd9_probe(struct spi_device *spi)
+/* Four channels apart from timestamp, scan mask = 0x0f */
+static const unsigned long kxsd9_scan_masks[] = { 0xf, 0 };
+
+int kxsd9_common_probe(struct device *dev,
+		       struct regmap *map,
+		       const char *name)
 {
 	struct iio_dev *indio_dev;
 	struct kxsd9_state *st;
+	int ret;
 
-	indio_dev = devm_iio_device_alloc(&spi->dev, sizeof(*st));
+	indio_dev = devm_iio_device_alloc(dev, sizeof(*st));
 	if (!indio_dev)
 		return -ENOMEM;
 
 	st = iio_priv(indio_dev);
-	spi_set_drvdata(spi, indio_dev);
+	st->dev = dev;
+	st->map = map;
 
-	st->us = spi;
-	mutex_init(&st->buf_lock);
 	indio_dev->channels = kxsd9_channels;
 	indio_dev->num_channels = ARRAY_SIZE(kxsd9_channels);
-	indio_dev->name = spi_get_device_id(spi)->name;
-	indio_dev->dev.parent = &spi->dev;
+	indio_dev->name = name;
+	indio_dev->dev.parent = dev;
 	indio_dev->info = &kxsd9_info;
 	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->available_scan_masks = kxsd9_scan_masks;
+
+	/* Read the mounting matrix, if present */
+	ret = of_iio_read_mount_matrix(dev,
+				       "mount-matrix",
+				       &st->orientation);
+	if (ret)
+		return ret;
+
+	/* Fetch and turn on regulators */
+	st->regs[0].supply = kxsd9_reg_vdd;
+	st->regs[1].supply = kxsd9_reg_iovdd;
+	ret = devm_regulator_bulk_get(dev,
+				      ARRAY_SIZE(st->regs),
+				      st->regs);
+	if (ret) {
+		dev_err(dev, "Cannot get regulators\n");
+		return ret;
+	}
+	/* Default scaling */
+	st->scale = KXSD9_CTRL_C_FS_2G;
 
-	spi->mode = SPI_MODE_0;
-	spi_setup(spi);
 	kxsd9_power_up(st);
 
-	return iio_device_register(indio_dev);
+	ret = iio_triggered_buffer_setup(indio_dev,
+					 iio_pollfunc_store_time,
+					 kxsd9_trigger_handler,
+					 &kxsd9_buffer_setup_ops);
+	if (ret) {
+		dev_err(dev, "triggered buffer setup failed\n");
+		goto err_power_down;
+	}
+
+	ret = iio_device_register(indio_dev);
+	if (ret)
+		goto err_cleanup_buffer;
+
+	dev_set_drvdata(dev, indio_dev);
+
+	/* Enable runtime PM */
+	pm_runtime_get_noresume(dev);
+	pm_runtime_set_active(dev);
+	pm_runtime_enable(dev);
+	/*
+	 * Set autosuspend to two orders of magnitude larger than the
+	 * start-up time. 20ms start-up time means 2000ms autosuspend,
+	 * i.e. 2 seconds.
+	 */
+	pm_runtime_set_autosuspend_delay(dev, 2000);
+	pm_runtime_use_autosuspend(dev);
+	pm_runtime_put(dev);
+
+	return 0;
+
+err_cleanup_buffer:
+	iio_triggered_buffer_cleanup(indio_dev);
+err_power_down:
+	kxsd9_power_down(st);
+
+	return ret;
 }
+EXPORT_SYMBOL(kxsd9_common_probe);
 
-static int kxsd9_remove(struct spi_device *spi)
+int kxsd9_common_remove(struct device *dev)
 {
-	iio_device_unregister(spi_get_drvdata(spi));
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct kxsd9_state *st = iio_priv(indio_dev);
+
+	iio_triggered_buffer_cleanup(indio_dev);
+	iio_device_unregister(indio_dev);
+	pm_runtime_get_sync(dev);
+	pm_runtime_put_noidle(dev);
+	pm_runtime_disable(dev);
+	kxsd9_power_down(st);
 
 	return 0;
 }
+EXPORT_SYMBOL(kxsd9_common_remove);
 
-static const struct spi_device_id kxsd9_id[] = {
-	{"kxsd9", 0},
-	{ },
-};
-MODULE_DEVICE_TABLE(spi, kxsd9_id);
+#ifdef CONFIG_PM
+static int kxsd9_runtime_suspend(struct device *dev)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct kxsd9_state *st = iio_priv(indio_dev);
 
-static struct spi_driver kxsd9_driver = {
-	.driver = {
-		.name = "kxsd9",
-	},
-	.probe = kxsd9_probe,
-	.remove = kxsd9_remove,
-	.id_table = kxsd9_id,
+	return kxsd9_power_down(st);
+}
+
+static int kxsd9_runtime_resume(struct device *dev)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct kxsd9_state *st = iio_priv(indio_dev);
+
+	return kxsd9_power_up(st);
+}
+#endif /* CONFIG_PM */
+
+const struct dev_pm_ops kxsd9_dev_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(pm_runtime_force_suspend,
+				pm_runtime_force_resume)
+	SET_RUNTIME_PM_OPS(kxsd9_runtime_suspend,
+			   kxsd9_runtime_resume, NULL)
 };
-module_spi_driver(kxsd9_driver);
+EXPORT_SYMBOL(kxsd9_dev_pm_ops);
 
 MODULE_AUTHOR("Jonathan Cameron <jic23@kernel.org>");
-MODULE_DESCRIPTION("Kionix KXSD9 SPI driver");
+MODULE_DESCRIPTION("Kionix KXSD9 driver");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/accel/kxsd9.h b/drivers/iio/accel/kxsd9.h
new file mode 100644
index 000000000000..7e8a28168310
--- /dev/null
+++ b/drivers/iio/accel/kxsd9.h
@@ -0,0 +1,12 @@
+#include <linux/device.h>
+#include <linux/kernel.h>
+
+#define KXSD9_STATE_RX_SIZE 2
+#define KXSD9_STATE_TX_SIZE 2
+
+int kxsd9_common_probe(struct device *dev,
+		       struct regmap *map,
+		       const char *name);
+int kxsd9_common_remove(struct device *dev);
+
+extern const struct dev_pm_ops kxsd9_dev_pm_ops;
diff --git a/drivers/iio/accel/mc3230.c b/drivers/iio/accel/mc3230.c
new file mode 100644
index 000000000000..4ea2ff623a6d
--- /dev/null
+++ b/drivers/iio/accel/mc3230.c
@@ -0,0 +1,211 @@
+/**
+ * mCube MC3230 3-Axis Accelerometer
+ *
+ * Copyright (c) 2016 Hans de Goede <hdegoede@redhat.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * IIO driver for mCube MC3230; 7-bit I2C address: 0x4c.
+ */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+
+#define MC3230_REG_XOUT			0x00
+#define MC3230_REG_YOUT			0x01
+#define MC3230_REG_ZOUT			0x02
+
+#define MC3230_REG_MODE			0x07
+#define MC3230_MODE_OPCON_MASK		0x03
+#define MC3230_MODE_OPCON_WAKE		0x01
+#define MC3230_MODE_OPCON_STANDBY	0x03
+
+#define MC3230_REG_CHIP_ID		0x18
+#define MC3230_CHIP_ID			0x01
+
+#define MC3230_REG_PRODUCT_CODE		0x3b
+#define MC3230_PRODUCT_CODE		0x19
+
+/*
+ * The accelerometer has one measurement range:
+ *
+ * -1.5g - +1.5g (8-bit, signed)
+ *
+ * scale = (1.5 + 1.5) * 9.81 / (2^8 - 1)	= 0.115411765
+ */
+
+static const int mc3230_nscale = 115411765;
+
+#define MC3230_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 mc3230_channels[] = {
+	MC3230_CHANNEL(MC3230_REG_XOUT, X),
+	MC3230_CHANNEL(MC3230_REG_YOUT, Y),
+	MC3230_CHANNEL(MC3230_REG_ZOUT, Z),
+};
+
+struct mc3230_data {
+	struct i2c_client *client;
+};
+
+static int mc3230_set_opcon(struct mc3230_data *data, int opcon)
+{
+	int ret;
+	struct i2c_client *client = data->client;
+
+	ret = i2c_smbus_read_byte_data(client, MC3230_REG_MODE);
+	if (ret < 0) {
+		dev_err(&client->dev, "failed to read mode reg: %d\n", ret);
+		return ret;
+	}
+
+	ret &= ~MC3230_MODE_OPCON_MASK;
+	ret |= opcon;
+
+	ret = i2c_smbus_write_byte_data(client, MC3230_REG_MODE, ret);
+	if (ret < 0) {
+		dev_err(&client->dev, "failed to write mode reg: %d\n", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int mc3230_read_raw(struct iio_dev *indio_dev,
+				struct iio_chan_spec const *chan,
+				int *val, int *val2, long mask)
+{
+	struct mc3230_data *data = iio_priv(indio_dev);
+	int ret;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		ret = i2c_smbus_read_byte_data(data->client, chan->address);
+		if (ret < 0)
+			return ret;
+		*val = sign_extend32(ret, 7);
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_SCALE:
+		*val = 0;
+		*val2 = mc3230_nscale;
+		return IIO_VAL_INT_PLUS_NANO;
+	default:
+		return -EINVAL;
+	}
+}
+
+static const struct iio_info mc3230_info = {
+	.driver_module	= THIS_MODULE,
+	.read_raw	= mc3230_read_raw,
+};
+
+static int mc3230_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	int ret;
+	struct iio_dev *indio_dev;
+	struct mc3230_data *data;
+
+	/* First check chip-id and product-id */
+	ret = i2c_smbus_read_byte_data(client, MC3230_REG_CHIP_ID);
+	if (ret != MC3230_CHIP_ID)
+		return (ret < 0) ? ret : -ENODEV;
+
+	ret = i2c_smbus_read_byte_data(client, MC3230_REG_PRODUCT_CODE);
+	if (ret != MC3230_PRODUCT_CODE)
+		return (ret < 0) ? ret : -ENODEV;
+
+	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);
+
+	indio_dev->dev.parent = &client->dev;
+	indio_dev->info = &mc3230_info;
+	indio_dev->name = "mc3230";
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->channels = mc3230_channels;
+	indio_dev->num_channels = ARRAY_SIZE(mc3230_channels);
+
+	ret = mc3230_set_opcon(data, MC3230_MODE_OPCON_WAKE);
+	if (ret < 0)
+		return ret;
+
+	ret = iio_device_register(indio_dev);
+	if (ret < 0) {
+		dev_err(&client->dev, "device_register failed\n");
+		mc3230_set_opcon(data, MC3230_MODE_OPCON_STANDBY);
+	}
+
+	return ret;
+}
+
+static int mc3230_remove(struct i2c_client *client)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(client);
+
+	iio_device_unregister(indio_dev);
+
+	return mc3230_set_opcon(iio_priv(indio_dev), MC3230_MODE_OPCON_STANDBY);
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int mc3230_suspend(struct device *dev)
+{
+	struct mc3230_data *data;
+
+	data = iio_priv(i2c_get_clientdata(to_i2c_client(dev)));
+
+	return mc3230_set_opcon(data, MC3230_MODE_OPCON_STANDBY);
+}
+
+static int mc3230_resume(struct device *dev)
+{
+	struct mc3230_data *data;
+
+	data = iio_priv(i2c_get_clientdata(to_i2c_client(dev)));
+
+	return mc3230_set_opcon(data, MC3230_MODE_OPCON_WAKE);
+}
+#endif
+
+static SIMPLE_DEV_PM_OPS(mc3230_pm_ops, mc3230_suspend, mc3230_resume);
+
+static const struct i2c_device_id mc3230_i2c_id[] = {
+	{"mc3230", 0},
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, mc3230_i2c_id);
+
+static struct i2c_driver mc3230_driver = {
+	.driver = {
+		.name = "mc3230",
+		.pm = &mc3230_pm_ops,
+	},
+	.probe		= mc3230_probe,
+	.remove		= mc3230_remove,
+	.id_table	= mc3230_i2c_id,
+};
+
+module_i2c_driver(mc3230_driver);
+
+MODULE_AUTHOR("Hans de Goede <hdegoede@redhat.com>");
+MODULE_DESCRIPTION("mCube MC3230 3-Axis Accelerometer driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/accel/mma7660.c b/drivers/iio/accel/mma7660.c
index 0acdee516973..03beadf14ad3 100644
--- a/drivers/iio/accel/mma7660.c
+++ b/drivers/iio/accel/mma7660.c
@@ -251,6 +251,7 @@ static const struct i2c_device_id mma7660_i2c_id[] = {
 	{"mma7660", 0},
 	{}
 };
+MODULE_DEVICE_TABLE(i2c, mma7660_i2c_id);
 
 static const struct acpi_device_id mma7660_acpi_id[] = {
 	{"MMA7660", 0},
diff --git a/drivers/iio/accel/mxc6255.c b/drivers/iio/accel/mxc6255.c
index 97ccde722e7b..0abad6948201 100644
--- a/drivers/iio/accel/mxc6255.c
+++ b/drivers/iio/accel/mxc6255.c
@@ -154,7 +154,7 @@ static int mxc6255_probe(struct i2c_client *client,
 		return ret;
 	}
 
-	if (chip_id != MXC6255_CHIP_ID) {
+	if ((chip_id & 0x1f) != MXC6255_CHIP_ID) {
 		dev_err(&client->dev, "Invalid chip id %x\n", chip_id);
 		return -ENODEV;
 	}
@@ -171,12 +171,14 @@ static int mxc6255_probe(struct i2c_client *client,
 }
 
 static const struct acpi_device_id mxc6255_acpi_match[] = {
+	{"MXC6225",	0},
 	{"MXC6255",	0},
 	{ }
 };
 MODULE_DEVICE_TABLE(acpi, mxc6255_acpi_match);
 
 static const struct i2c_device_id mxc6255_id[] = {
+	{"mxc6225",	0},
 	{"mxc6255",	0},
 	{ }
 };
diff --git a/drivers/iio/accel/ssp_accel_sensor.c b/drivers/iio/accel/ssp_accel_sensor.c
index 4ae05fce9f24..31db00970fa0 100644
--- a/drivers/iio/accel/ssp_accel_sensor.c
+++ b/drivers/iio/accel/ssp_accel_sensor.c
@@ -74,7 +74,7 @@ static int ssp_accel_write_raw(struct iio_dev *indio_dev,
 	return -EINVAL;
 }
 
-static struct iio_info ssp_accel_iio_info = {
+static const struct iio_info ssp_accel_iio_info = {
 	.read_raw = &ssp_accel_read_raw,
 	.write_raw = &ssp_accel_write_raw,
 };
diff --git a/drivers/iio/adc/Kconfig b/drivers/iio/adc/Kconfig
index 767577298ee3..7edcf3238620 100644
--- a/drivers/iio/adc/Kconfig
+++ b/drivers/iio/adc/Kconfig
@@ -264,6 +264,15 @@ config LPC18XX_ADC
 	  To compile this driver as a module, choose M here: the module will be
 	  called lpc18xx_adc.
 
+config LTC2485
+	tristate "Linear Technology LTC2485 ADC driver"
+	depends on I2C
+	help
+	  Say yes here to build support for Linear Technology LTC2485 ADC.
+
+	  To compile this driver as a module, choose M here: the module will be
+	  called ltc2485.
+
 config MAX1027
 	tristate "Maxim max1027 ADC driver"
 	depends on SPI
@@ -317,6 +326,19 @@ config MCP3422
 	  This driver can also be built as a module. If so, the module will be
 	  called mcp3422.
 
+config MEDIATEK_MT6577_AUXADC
+        tristate "MediaTek AUXADC driver"
+        depends on ARCH_MEDIATEK || COMPILE_TEST
+        depends on HAS_IOMEM
+        help
+          Say yes here to enable support for MediaTek mt65xx AUXADC.
+
+          The driver supports immediate mode operation to read from one of sixteen
+          channels (external or internal).
+
+          This driver can also be built as a module. If so, the module will be
+          called mt6577_auxadc.
+
 config MEN_Z188_ADC
 	tristate "MEN 16z188 ADC IP Core support"
 	depends on MCB
@@ -397,6 +419,21 @@ config ROCKCHIP_SARADC
 	  To compile this driver as a module, choose M here: the
 	  module will be called rockchip_saradc.
 
+config STX104
+	tristate "Apex Embedded Systems STX104 driver"
+	depends on X86 && ISA_BUS_API
+	select GPIOLIB
+	help
+	  Say yes here to build support for the Apex Embedded Systems STX104
+	  integrated analog PC/104 card.
+
+	  This driver supports the 16 channels of single-ended (8 channels of
+	  differential) analog inputs, 2 channels of analog output, 4 digital
+	  inputs, and 4 digital outputs provided by the STX104.
+
+	  The base port addresses for the devices may be configured via the base
+	  array module parameter.
+
 config TI_ADC081C
 	tristate "Texas Instruments ADC081C/ADC101C/ADC121C family"
 	depends on I2C
@@ -417,6 +454,18 @@ config TI_ADC0832
 	  This driver can also be built as a module. If so, the module will be
 	  called ti-adc0832.
 
+config TI_ADC12138
+	tristate "Texas Instruments ADC12130/ADC12132/ADC12138"
+	depends on SPI
+	select IIO_BUFFER
+	select IIO_TRIGGERED_BUFFER
+	help
+	  If you say yes here you get support for Texas Instruments ADC12130,
+	  ADC12132 and ADC12138 chips.
+
+	  This driver can also be built as a module. If so, the module will be
+	  called ti-adc12138.
+
 config TI_ADC128S052
 	tristate "Texas Instruments ADC128S052/ADC122S021/ADC124S021"
 	depends on SPI
@@ -427,6 +476,18 @@ config TI_ADC128S052
 	  This driver can also be built as a module. If so, the module will be
 	  called ti-adc128s052.
 
+config TI_ADC161S626
+	tristate "Texas Instruments ADC161S626 1-channel differential ADC"
+	depends on SPI
+	select IIO_BUFFER
+	select IIO_TRIGGERED_BUFFER
+	help
+	  If you say yes here you get support for Texas Instruments ADC141S626,
+	  and ADC161S626 chips.
+
+	  This driver can also be built as a module. If so, the module will be
+	  called ti-adc161s626.
+
 config TI_ADS1015
 	tristate "Texas Instruments ADS1015 ADC"
 	depends on I2C && !SENSORS_ADS1015
diff --git a/drivers/iio/adc/Makefile b/drivers/iio/adc/Makefile
index 0ba0d500eedb..7a40c04c311f 100644
--- a/drivers/iio/adc/Makefile
+++ b/drivers/iio/adc/Makefile
@@ -27,10 +27,12 @@ obj-$(CONFIG_IMX7D_ADC) += imx7d_adc.o
 obj-$(CONFIG_INA2XX_ADC) += ina2xx-adc.o
 obj-$(CONFIG_LP8788_ADC) += lp8788_adc.o
 obj-$(CONFIG_LPC18XX_ADC) += lpc18xx_adc.o
+obj-$(CONFIG_LTC2485) += ltc2485.o
 obj-$(CONFIG_MAX1027) += max1027.o
 obj-$(CONFIG_MAX1363) += max1363.o
 obj-$(CONFIG_MCP320X) += mcp320x.o
 obj-$(CONFIG_MCP3422) += mcp3422.o
+obj-$(CONFIG_MEDIATEK_MT6577_AUXADC) += mt6577_auxadc.o
 obj-$(CONFIG_MEN_Z188_ADC) += men_z188_adc.o
 obj-$(CONFIG_MXS_LRADC) += mxs-lradc.o
 obj-$(CONFIG_NAU7802) += nau7802.o
@@ -38,9 +40,12 @@ obj-$(CONFIG_PALMAS_GPADC) += palmas_gpadc.o
 obj-$(CONFIG_QCOM_SPMI_IADC) += qcom-spmi-iadc.o
 obj-$(CONFIG_QCOM_SPMI_VADC) += qcom-spmi-vadc.o
 obj-$(CONFIG_ROCKCHIP_SARADC) += rockchip_saradc.o
+obj-$(CONFIG_STX104) += stx104.o
 obj-$(CONFIG_TI_ADC081C) += ti-adc081c.o
 obj-$(CONFIG_TI_ADC0832) += ti-adc0832.o
+obj-$(CONFIG_TI_ADC12138) += ti-adc12138.o
 obj-$(CONFIG_TI_ADC128S052) += ti-adc128s052.o
+obj-$(CONFIG_TI_ADC161S626) += ti-adc161s626.o
 obj-$(CONFIG_TI_ADS1015) += ti-ads1015.o
 obj-$(CONFIG_TI_ADS8688) += ti-ads8688.o
 obj-$(CONFIG_TI_AM335X_ADC) += ti_am335x_adc.o
diff --git a/drivers/iio/adc/ad7266.c b/drivers/iio/adc/ad7266.c
index c0f6a98fd9bd..b8d5cfd57ec4 100644
--- a/drivers/iio/adc/ad7266.c
+++ b/drivers/iio/adc/ad7266.c
@@ -481,7 +481,7 @@ error_free_gpios:
 	if (!st->fixed_addr)
 		gpio_free_array(st->gpios, ARRAY_SIZE(st->gpios));
 error_disable_reg:
-	if (!IS_ERR_OR_NULL(st->reg))
+	if (!IS_ERR(st->reg))
 		regulator_disable(st->reg);
 
 	return ret;
@@ -496,7 +496,7 @@ static int ad7266_remove(struct spi_device *spi)
 	iio_triggered_buffer_cleanup(indio_dev);
 	if (!st->fixed_addr)
 		gpio_free_array(st->gpios, ARRAY_SIZE(st->gpios));
-	if (!IS_ERR_OR_NULL(st->reg))
+	if (!IS_ERR(st->reg))
 		regulator_disable(st->reg);
 
 	return 0;
diff --git a/drivers/iio/adc/ad7298.c b/drivers/iio/adc/ad7298.c
index 10ec8fce395f..e399bf04c73a 100644
--- a/drivers/iio/adc/ad7298.c
+++ b/drivers/iio/adc/ad7298.c
@@ -239,16 +239,16 @@ static int ad7298_read_raw(struct iio_dev *indio_dev,
 
 	switch (m) {
 	case IIO_CHAN_INFO_RAW:
-		mutex_lock(&indio_dev->mlock);
-		if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) {
-			ret = -EBUSY;
-		} else {
-			if (chan->address == AD7298_CH_TEMP)
-				ret = ad7298_scan_temp(st, val);
-			else
-				ret = ad7298_scan_direct(st, chan->address);
-		}
-		mutex_unlock(&indio_dev->mlock);
+		ret = iio_device_claim_direct_mode(indio_dev);
+		if (ret)
+			return ret;
+
+		if (chan->address == AD7298_CH_TEMP)
+			ret = ad7298_scan_temp(st, val);
+		else
+			ret = ad7298_scan_direct(st, chan->address);
+
+		iio_device_release_direct_mode(indio_dev);
 
 		if (ret < 0)
 			return ret;
diff --git a/drivers/iio/adc/ad7793.c b/drivers/iio/adc/ad7793.c
index 847789bae821..e6706a09e100 100644
--- a/drivers/iio/adc/ad7793.c
+++ b/drivers/iio/adc/ad7793.c
@@ -519,11 +519,9 @@ static int ad7793_write_raw(struct iio_dev *indio_dev,
 	int ret, i;
 	unsigned int tmp;
 
-	mutex_lock(&indio_dev->mlock);
-	if (iio_buffer_enabled(indio_dev)) {
-		mutex_unlock(&indio_dev->mlock);
-		return -EBUSY;
-	}
+	ret = iio_device_claim_direct_mode(indio_dev);
+	if (ret)
+		return ret;
 
 	switch (mask) {
 	case IIO_CHAN_INFO_SCALE:
@@ -548,7 +546,7 @@ static int ad7793_write_raw(struct iio_dev *indio_dev,
 		ret = -EINVAL;
 	}
 
-	mutex_unlock(&indio_dev->mlock);
+	iio_device_release_direct_mode(indio_dev);
 	return ret;
 }
 
diff --git a/drivers/iio/adc/at91_adc.c b/drivers/iio/adc/at91_adc.c
index 0438c68015e8..bbdac07f4aaa 100644
--- a/drivers/iio/adc/at91_adc.c
+++ b/drivers/iio/adc/at91_adc.c
@@ -113,6 +113,7 @@
 #define		AT91_ADC_TSMR_TSAV	(3 << 4)	/* Averages samples */
 #define			AT91_ADC_TSMR_TSAV_(x)		((x) << 4)
 #define		AT91_ADC_TSMR_SCTIM	(0x0f << 16)	/* Switch closure time */
+#define			AT91_ADC_TSMR_SCTIM_(x)		((x) << 16)
 #define		AT91_ADC_TSMR_PENDBC	(0x0f << 28)	/* Pen Debounce time */
 #define			AT91_ADC_TSMR_PENDBC_(x)	((x) << 28)
 #define		AT91_ADC_TSMR_NOTSDMA	(1 << 22)	/* No Touchscreen DMA */
@@ -150,6 +151,7 @@
 #define MAX_RLPOS_BITS         10
 #define TOUCH_SAMPLE_PERIOD_US_RL      10000   /* 10ms, the SoC can't keep up with 2ms */
 #define TOUCH_SHTIM                    0xa
+#define TOUCH_SCTIM_US		10		/* 10us for the Touchscreen Switches Closure Time */
 
 /**
  * struct at91_adc_reg_desc - Various informations relative to registers
@@ -1001,7 +1003,9 @@ static void atmel_ts_close(struct input_dev *dev)
 
 static int at91_ts_hw_init(struct at91_adc_state *st, u32 adc_clk_khz)
 {
+	struct iio_dev *idev = iio_priv_to_dev(st);
 	u32 reg = 0;
+	u32 tssctim = 0;
 	int i = 0;
 
 	/* a Pen Detect Debounce Time is necessary for the ADC Touch to avoid
@@ -1034,11 +1038,20 @@ static int at91_ts_hw_init(struct at91_adc_state *st, u32 adc_clk_khz)
 		return 0;
 	}
 
+	/* Touchscreen Switches Closure time needed for allowing the value to
+	 * stabilize.
+	 * Switch Closure Time = (TSSCTIM * 4) ADCClock periods
+	 */
+	tssctim = DIV_ROUND_UP(TOUCH_SCTIM_US * adc_clk_khz / 1000, 4);
+	dev_dbg(&idev->dev, "adc_clk at: %d KHz, tssctim at: %d\n",
+		adc_clk_khz, tssctim);
+
 	if (st->touchscreen_type == ATMEL_ADC_TOUCHSCREEN_4WIRE)
 		reg = AT91_ADC_TSMR_TSMODE_4WIRE_PRESS;
 	else
 		reg = AT91_ADC_TSMR_TSMODE_5WIRE;
 
+	reg |= AT91_ADC_TSMR_SCTIM_(tssctim) & AT91_ADC_TSMR_SCTIM;
 	reg |= AT91_ADC_TSMR_TSAV_(st->caps->ts_filter_average)
 	       & AT91_ADC_TSMR_TSAV;
 	reg |= AT91_ADC_TSMR_PENDBC_(st->ts_pendbc) & AT91_ADC_TSMR_PENDBC;
diff --git a/drivers/iio/adc/ina2xx-adc.c b/drivers/iio/adc/ina2xx-adc.c
index 955f3fdaf519..59b7d76e1ad2 100644
--- a/drivers/iio/adc/ina2xx-adc.c
+++ b/drivers/iio/adc/ina2xx-adc.c
@@ -114,7 +114,6 @@ struct ina2xx_chip_info {
 	struct mutex state_lock;
 	unsigned int shunt_resistor;
 	int avg;
-	s64 prev_ns; /* track buffer capture time, check for underruns */
 	int int_time_vbus; /* Bus voltage integration time uS */
 	int int_time_vshunt; /* Shunt voltage integration time uS */
 	bool allow_async_readout;
@@ -509,8 +508,6 @@ static int ina2xx_work_buffer(struct iio_dev *indio_dev)
 	iio_push_to_buffers_with_timestamp(indio_dev,
 					   (unsigned int *)data, time_a);
 
-	chip->prev_ns = time_a;
-
 	return (unsigned long)(time_b - time_a) / 1000;
 };
 
@@ -554,8 +551,6 @@ static int ina2xx_buffer_enable(struct iio_dev *indio_dev)
 	dev_dbg(&indio_dev->dev, "Async readout mode: %d\n",
 		chip->allow_async_readout);
 
-	chip->prev_ns = iio_get_time_ns(indio_dev);
-
 	chip->task = kthread_run(ina2xx_capture_thread, (void *)indio_dev,
 				 "%s:%d-%uus", indio_dev->name, indio_dev->id,
 				 sampling_us);
diff --git a/drivers/iio/adc/ltc2485.c b/drivers/iio/adc/ltc2485.c
new file mode 100644
index 000000000000..eab91f12454a
--- /dev/null
+++ b/drivers/iio/adc/ltc2485.c
@@ -0,0 +1,148 @@
+/*
+ * ltc2485.c - Driver for Linear Technology LTC2485 ADC
+ *
+ * Copyright (C) 2016 Alison Schofield <amsfield22@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.
+ *
+ * Datasheet: http://cds.linear.com/docs/en/datasheet/2485fd.pdf
+ */
+
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+
+/* Power-on configuration: rejects both 50/60Hz, operates at 1x speed */
+#define LTC2485_CONFIG_DEFAULT		0
+
+struct ltc2485_data {
+	struct i2c_client	*client;
+	ktime_t			time_prev;	/* last conversion */
+};
+
+static void ltc2485_wait_conv(struct ltc2485_data *data)
+{
+	const unsigned int conv_time = 147;	/* conversion time ms */
+	unsigned int time_elapsed;
+
+	/* delay if conversion time not passed since last read or write */
+	time_elapsed = ktime_ms_delta(ktime_get(), data->time_prev);
+
+	if (time_elapsed < conv_time)
+		msleep(conv_time - time_elapsed);
+}
+
+static int ltc2485_read(struct ltc2485_data *data, int *val)
+{
+	struct i2c_client *client = data->client;
+	__be32 buf = 0;
+	int ret;
+
+	ltc2485_wait_conv(data);
+
+	ret = i2c_master_recv(client, (char *)&buf, 4);
+	if (ret < 0)  {
+		dev_err(&client->dev, "i2c_master_recv failed\n");
+		return ret;
+	}
+	data->time_prev = ktime_get();
+	*val = sign_extend32(be32_to_cpu(buf) >> 6, 24);
+
+	return ret;
+}
+
+static int ltc2485_read_raw(struct iio_dev *indio_dev,
+			    struct iio_chan_spec const *chan,
+			    int *val, int *val2, long mask)
+{
+	struct ltc2485_data *data = iio_priv(indio_dev);
+	int ret;
+
+	if (mask == IIO_CHAN_INFO_RAW) {
+		ret = ltc2485_read(data, val);
+		if (ret < 0)
+			return ret;
+
+		return IIO_VAL_INT;
+
+	} else if (mask == IIO_CHAN_INFO_SCALE) {
+		*val = 5000;			/* on board vref millivolts */
+		*val2 = 25;			/* 25 (24 + sign) data bits */
+		return IIO_VAL_FRACTIONAL_LOG2;
+
+	} else {
+		return -EINVAL;
+	}
+}
+
+static const struct iio_chan_spec ltc2485_channel[] = {
+	{
+		.type = IIO_VOLTAGE,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE)
+	},
+};
+
+static const struct iio_info ltc2485_info = {
+	.read_raw = ltc2485_read_raw,
+	.driver_module = THIS_MODULE,
+};
+
+static int ltc2485_probe(struct i2c_client *client,
+			 const struct i2c_device_id *id)
+{
+	struct iio_dev *indio_dev;
+	struct ltc2485_data *data;
+	int ret;
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C |
+				     I2C_FUNC_SMBUS_WRITE_BYTE))
+		return -EOPNOTSUPP;
+
+	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;
+
+	indio_dev->dev.parent = &client->dev;
+	indio_dev->name = id->name;
+	indio_dev->info = &ltc2485_info;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->channels = ltc2485_channel;
+	indio_dev->num_channels = ARRAY_SIZE(ltc2485_channel);
+
+	ret = i2c_smbus_write_byte(data->client, LTC2485_CONFIG_DEFAULT);
+	if (ret < 0)
+		return ret;
+
+	data->time_prev = ktime_get();
+
+	return devm_iio_device_register(&client->dev, indio_dev);
+}
+
+static const struct i2c_device_id ltc2485_id[] = {
+	{ "ltc2485", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, ltc2485_id);
+
+static struct i2c_driver ltc2485_driver = {
+	.driver = {
+		.name = "ltc2485",
+	},
+	.probe = ltc2485_probe,
+	.id_table = ltc2485_id,
+};
+module_i2c_driver(ltc2485_driver);
+
+MODULE_AUTHOR("Alison Schofield <amsfield22@gmail.com>");
+MODULE_DESCRIPTION("Linear Technology LTC2485 ADC driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/adc/men_z188_adc.c b/drivers/iio/adc/men_z188_adc.c
index d095efe1ba14..8f3606de4eaf 100644
--- a/drivers/iio/adc/men_z188_adc.c
+++ b/drivers/iio/adc/men_z188_adc.c
@@ -78,7 +78,7 @@ static int z188_iio_read_raw(struct iio_dev *iio_dev,
 	return ret;
 }
 
-static struct iio_info z188_adc_info = {
+static const struct iio_info z188_adc_info = {
 	.read_raw = &z188_iio_read_raw,
 	.driver_module = THIS_MODULE,
 };
diff --git a/drivers/iio/adc/mt6577_auxadc.c b/drivers/iio/adc/mt6577_auxadc.c
new file mode 100644
index 000000000000..2d104c828041
--- /dev/null
+++ b/drivers/iio/adc/mt6577_auxadc.c
@@ -0,0 +1,291 @@
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ * Author: Zhiyong Tao <zhiyong.tao@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/delay.h>
+#include <linux/err.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+#include <linux/iopoll.h>
+#include <linux/io.h>
+#include <linux/iio/iio.h>
+
+/* Register definitions */
+#define MT6577_AUXADC_CON0                    0x00
+#define MT6577_AUXADC_CON1                    0x04
+#define MT6577_AUXADC_CON2                    0x10
+#define MT6577_AUXADC_STA                     BIT(0)
+
+#define MT6577_AUXADC_DAT0                    0x14
+#define MT6577_AUXADC_RDY0                    BIT(12)
+
+#define MT6577_AUXADC_MISC                    0x94
+#define MT6577_AUXADC_PDN_EN                  BIT(14)
+
+#define MT6577_AUXADC_DAT_MASK                0xfff
+#define MT6577_AUXADC_SLEEP_US                1000
+#define MT6577_AUXADC_TIMEOUT_US              10000
+#define MT6577_AUXADC_POWER_READY_MS          1
+#define MT6577_AUXADC_SAMPLE_READY_US         25
+
+struct mt6577_auxadc_device {
+	void __iomem *reg_base;
+	struct clk *adc_clk;
+	struct mutex lock;
+};
+
+#define MT6577_AUXADC_CHANNEL(idx) {				    \
+		.type = IIO_VOLTAGE,				    \
+		.indexed = 1,					    \
+		.channel = (idx),				    \
+		.info_mask_separate = BIT(IIO_CHAN_INFO_PROCESSED), \
+}
+
+static const struct iio_chan_spec mt6577_auxadc_iio_channels[] = {
+	MT6577_AUXADC_CHANNEL(0),
+	MT6577_AUXADC_CHANNEL(1),
+	MT6577_AUXADC_CHANNEL(2),
+	MT6577_AUXADC_CHANNEL(3),
+	MT6577_AUXADC_CHANNEL(4),
+	MT6577_AUXADC_CHANNEL(5),
+	MT6577_AUXADC_CHANNEL(6),
+	MT6577_AUXADC_CHANNEL(7),
+	MT6577_AUXADC_CHANNEL(8),
+	MT6577_AUXADC_CHANNEL(9),
+	MT6577_AUXADC_CHANNEL(10),
+	MT6577_AUXADC_CHANNEL(11),
+	MT6577_AUXADC_CHANNEL(12),
+	MT6577_AUXADC_CHANNEL(13),
+	MT6577_AUXADC_CHANNEL(14),
+	MT6577_AUXADC_CHANNEL(15),
+};
+
+static inline void mt6577_auxadc_mod_reg(void __iomem *reg,
+					 u32 or_mask, u32 and_mask)
+{
+	u32 val;
+
+	val = readl(reg);
+	val |= or_mask;
+	val &= ~and_mask;
+	writel(val, reg);
+}
+
+static int mt6577_auxadc_read(struct iio_dev *indio_dev,
+			      struct iio_chan_spec const *chan)
+{
+	u32 val;
+	void __iomem *reg_channel;
+	int ret;
+	struct mt6577_auxadc_device *adc_dev = iio_priv(indio_dev);
+
+	reg_channel = adc_dev->reg_base + MT6577_AUXADC_DAT0 +
+		      chan->channel * 0x04;
+
+	mutex_lock(&adc_dev->lock);
+
+	mt6577_auxadc_mod_reg(adc_dev->reg_base + MT6577_AUXADC_CON1,
+			      0, 1 << chan->channel);
+
+	/* read channel and make sure old ready bit == 0 */
+	ret = readl_poll_timeout(reg_channel, val,
+				 ((val & MT6577_AUXADC_RDY0) == 0),
+				 MT6577_AUXADC_SLEEP_US,
+				 MT6577_AUXADC_TIMEOUT_US);
+	if (ret < 0) {
+		dev_err(indio_dev->dev.parent,
+			"wait for channel[%d] ready bit clear time out\n",
+			chan->channel);
+		goto err_timeout;
+	}
+
+	/* set bit to trigger sample */
+	mt6577_auxadc_mod_reg(adc_dev->reg_base + MT6577_AUXADC_CON1,
+			      1 << chan->channel, 0);
+
+	/* we must delay here for hardware sample channel data */
+	udelay(MT6577_AUXADC_SAMPLE_READY_US);
+
+	/* check MTK_AUXADC_CON2 if auxadc is idle */
+	ret = readl_poll_timeout(adc_dev->reg_base + MT6577_AUXADC_CON2, val,
+				 ((val & MT6577_AUXADC_STA) == 0),
+				 MT6577_AUXADC_SLEEP_US,
+				 MT6577_AUXADC_TIMEOUT_US);
+	if (ret < 0) {
+		dev_err(indio_dev->dev.parent,
+			"wait for auxadc idle time out\n");
+		goto err_timeout;
+	}
+
+	/* read channel and make sure ready bit == 1 */
+	ret = readl_poll_timeout(reg_channel, val,
+				 ((val & MT6577_AUXADC_RDY0) != 0),
+				 MT6577_AUXADC_SLEEP_US,
+				 MT6577_AUXADC_TIMEOUT_US);
+	if (ret < 0) {
+		dev_err(indio_dev->dev.parent,
+			"wait for channel[%d] data ready time out\n",
+			chan->channel);
+		goto err_timeout;
+	}
+
+	/* read data */
+	val = readl(reg_channel) & MT6577_AUXADC_DAT_MASK;
+
+	mutex_unlock(&adc_dev->lock);
+
+	return val;
+
+err_timeout:
+
+	mutex_unlock(&adc_dev->lock);
+
+	return -ETIMEDOUT;
+}
+
+static int mt6577_auxadc_read_raw(struct iio_dev *indio_dev,
+				  struct iio_chan_spec const *chan,
+				  int *val,
+				  int *val2,
+				  long info)
+{
+	switch (info) {
+	case IIO_CHAN_INFO_PROCESSED:
+		*val = mt6577_auxadc_read(indio_dev, chan);
+		if (*val < 0) {
+			dev_err(indio_dev->dev.parent,
+				"failed to sample data on channel[%d]\n",
+				chan->channel);
+			return *val;
+		}
+		return IIO_VAL_INT;
+
+	default:
+		return -EINVAL;
+	}
+}
+
+static const struct iio_info mt6577_auxadc_info = {
+	.driver_module = THIS_MODULE,
+	.read_raw = &mt6577_auxadc_read_raw,
+};
+
+static int mt6577_auxadc_probe(struct platform_device *pdev)
+{
+	struct mt6577_auxadc_device *adc_dev;
+	unsigned long adc_clk_rate;
+	struct resource *res;
+	struct iio_dev *indio_dev;
+	int ret;
+
+	indio_dev = devm_iio_device_alloc(&pdev->dev, sizeof(*adc_dev));
+	if (!indio_dev)
+		return -ENOMEM;
+
+	adc_dev = iio_priv(indio_dev);
+	indio_dev->dev.parent = &pdev->dev;
+	indio_dev->name = dev_name(&pdev->dev);
+	indio_dev->info = &mt6577_auxadc_info;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->channels = mt6577_auxadc_iio_channels;
+	indio_dev->num_channels = ARRAY_SIZE(mt6577_auxadc_iio_channels);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	adc_dev->reg_base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(adc_dev->reg_base)) {
+		dev_err(&pdev->dev, "failed to get auxadc base address\n");
+		return PTR_ERR(adc_dev->reg_base);
+	}
+
+	adc_dev->adc_clk = devm_clk_get(&pdev->dev, "main");
+	if (IS_ERR(adc_dev->adc_clk)) {
+		dev_err(&pdev->dev, "failed to get auxadc clock\n");
+		return PTR_ERR(adc_dev->adc_clk);
+	}
+
+	ret = clk_prepare_enable(adc_dev->adc_clk);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to enable auxadc clock\n");
+		return ret;
+	}
+
+	adc_clk_rate = clk_get_rate(adc_dev->adc_clk);
+	if (!adc_clk_rate) {
+		ret = -EINVAL;
+		dev_err(&pdev->dev, "null clock rate\n");
+		goto err_disable_clk;
+	}
+
+	mutex_init(&adc_dev->lock);
+
+	mt6577_auxadc_mod_reg(adc_dev->reg_base + MT6577_AUXADC_MISC,
+			      MT6577_AUXADC_PDN_EN, 0);
+	mdelay(MT6577_AUXADC_POWER_READY_MS);
+
+	platform_set_drvdata(pdev, indio_dev);
+
+	ret = iio_device_register(indio_dev);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "failed to register iio device\n");
+		goto err_power_off;
+	}
+
+	return 0;
+
+err_power_off:
+	mt6577_auxadc_mod_reg(adc_dev->reg_base + MT6577_AUXADC_MISC,
+			      0, MT6577_AUXADC_PDN_EN);
+err_disable_clk:
+	clk_disable_unprepare(adc_dev->adc_clk);
+	return ret;
+}
+
+static int mt6577_auxadc_remove(struct platform_device *pdev)
+{
+	struct iio_dev *indio_dev = platform_get_drvdata(pdev);
+	struct mt6577_auxadc_device *adc_dev = iio_priv(indio_dev);
+
+	iio_device_unregister(indio_dev);
+
+	mt6577_auxadc_mod_reg(adc_dev->reg_base + MT6577_AUXADC_MISC,
+			      0, MT6577_AUXADC_PDN_EN);
+
+	clk_disable_unprepare(adc_dev->adc_clk);
+
+	return 0;
+}
+
+static const struct of_device_id mt6577_auxadc_of_match[] = {
+	{ .compatible = "mediatek,mt2701-auxadc", },
+	{ .compatible = "mediatek,mt8173-auxadc", },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, mt6577_auxadc_of_match);
+
+static struct platform_driver mt6577_auxadc_driver = {
+	.driver = {
+		.name   = "mt6577-auxadc",
+		.of_match_table = mt6577_auxadc_of_match,
+	},
+	.probe	= mt6577_auxadc_probe,
+	.remove	= mt6577_auxadc_remove,
+};
+module_platform_driver(mt6577_auxadc_driver);
+
+MODULE_AUTHOR("Zhiyong Tao <zhiyong.tao@mediatek.com>");
+MODULE_DESCRIPTION("MTK AUXADC Device Driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/adc/nau7802.c b/drivers/iio/adc/nau7802.c
index db9b829ccf0d..08f446695f97 100644
--- a/drivers/iio/adc/nau7802.c
+++ b/drivers/iio/adc/nau7802.c
@@ -197,7 +197,7 @@ static irqreturn_t nau7802_eoc_trigger(int irq, void *private)
 	if (st->conversion_count < NAU7802_MIN_CONVERSIONS)
 		st->conversion_count++;
 	if (st->conversion_count >= NAU7802_MIN_CONVERSIONS)
-		complete_all(&st->value_ok);
+		complete(&st->value_ok);
 
 	return IRQ_HANDLED;
 }
diff --git a/drivers/iio/dac/stx104.c b/drivers/iio/adc/stx104.c
index bebbd00304ce..7e3645749eaf 100644
--- a/drivers/iio/dac/stx104.c
+++ b/drivers/iio/adc/stx104.c
@@ -1,5 +1,5 @@
 /*
- * DAC driver for the Apex Embedded Systems STX104
+ * IIO driver for the Apex Embedded Systems STX104
  * Copyright (C) 2016 William Breathitt Gray
  *
  * This program is free software; you can redistribute it and/or modify
@@ -20,19 +20,30 @@
 #include <linux/io.h>
 #include <linux/ioport.h>
 #include <linux/isa.h>
+#include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/moduleparam.h>
 #include <linux/spinlock.h>
 
-#define STX104_NUM_CHAN 2
-
-#define STX104_CHAN(chan) {				\
+#define STX104_OUT_CHAN(chan) {				\
 	.type = IIO_VOLTAGE,				\
 	.channel = chan,				\
 	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),	\
 	.indexed = 1,					\
 	.output = 1					\
 }
+#define STX104_IN_CHAN(chan, diff) {					\
+	.type = IIO_VOLTAGE,						\
+	.channel = chan,						\
+	.channel2 = chan,						\
+	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_HARDWAREGAIN) |	\
+		BIT(IIO_CHAN_INFO_OFFSET) | BIT(IIO_CHAN_INFO_SCALE),	\
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),			\
+	.indexed = 1,							\
+	.differential = diff						\
+}
+
+#define STX104_NUM_OUT_CHAN 2
 
 #define STX104_EXTENT 16
 
@@ -47,8 +58,8 @@ MODULE_PARM_DESC(base, "Apex Embedded Systems STX104 base addresses");
  * @base:		base port address of the IIO device
  */
 struct stx104_iio {
-	unsigned chan_out_states[STX104_NUM_CHAN];
-	unsigned base;
+	unsigned int chan_out_states[STX104_NUM_OUT_CHAN];
+	unsigned int base;
 };
 
 /**
@@ -79,28 +90,95 @@ static int stx104_read_raw(struct iio_dev *indio_dev,
 	struct iio_chan_spec const *chan, int *val, int *val2, long mask)
 {
 	struct stx104_iio *const priv = iio_priv(indio_dev);
+	unsigned int adc_config;
+	int adbu;
+	int gain;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_HARDWAREGAIN:
+		/* get gain configuration */
+		adc_config = inb(priv->base + 11);
+		gain = adc_config & 0x3;
+
+		*val = 1 << gain;
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_RAW:
+		if (chan->output) {
+			*val = priv->chan_out_states[chan->channel];
+			return IIO_VAL_INT;
+		}
+
+		/* select ADC channel */
+		outb(chan->channel | (chan->channel << 4), priv->base + 2);
+
+		/* trigger ADC sample capture and wait for completion */
+		outb(0, priv->base);
+		while (inb(priv->base + 8) & BIT(7));
+
+		*val = inw(priv->base);
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_OFFSET:
+		/* get ADC bipolar/unipolar configuration */
+		adc_config = inb(priv->base + 11);
+		adbu = !(adc_config & BIT(2));
+
+		*val = -32768 * adbu;
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_SCALE:
+		/* get ADC bipolar/unipolar and gain configuration */
+		adc_config = inb(priv->base + 11);
+		adbu = !(adc_config & BIT(2));
+		gain = adc_config & 0x3;
+
+		*val = 5;
+		*val2 = 15 - adbu + gain;
+		return IIO_VAL_FRACTIONAL_LOG2;
+	}
 
-	if (mask != IIO_CHAN_INFO_RAW)
-		return -EINVAL;
-
-	*val = priv->chan_out_states[chan->channel];
-
-	return IIO_VAL_INT;
+	return -EINVAL;
 }
 
 static int stx104_write_raw(struct iio_dev *indio_dev,
 	struct iio_chan_spec const *chan, int val, int val2, long mask)
 {
 	struct stx104_iio *const priv = iio_priv(indio_dev);
-	const unsigned chan_addr_offset = 2 * chan->channel;
 
-	if (mask != IIO_CHAN_INFO_RAW)
+	switch (mask) {
+	case IIO_CHAN_INFO_HARDWAREGAIN:
+		/* Only four gain states (x1, x2, x4, x8) */
+		switch (val) {
+		case 1:
+			outb(0, priv->base + 11);
+			break;
+		case 2:
+			outb(1, priv->base + 11);
+			break;
+		case 4:
+			outb(2, priv->base + 11);
+			break;
+		case 8:
+			outb(3, priv->base + 11);
+			break;
+		default:
+			return -EINVAL;
+		}
+
+		return 0;
+	case IIO_CHAN_INFO_RAW:
+		if (chan->output) {
+			/* DAC can only accept up to a 16-bit value */
+			if ((unsigned int)val > 65535)
+				return -EINVAL;
+
+			priv->chan_out_states[chan->channel] = val;
+			outw(val, priv->base + 4 + 2 * chan->channel);
+
+			return 0;
+		}
 		return -EINVAL;
+	}
 
-	priv->chan_out_states[chan->channel] = val;
-	outw(val, priv->base + 4 + chan_addr_offset);
-
-	return 0;
+	return -EINVAL;
 }
 
 static const struct iio_info stx104_info = {
@@ -109,9 +187,22 @@ static const struct iio_info stx104_info = {
 	.write_raw = stx104_write_raw
 };
 
-static const struct iio_chan_spec stx104_channels[STX104_NUM_CHAN] = {
-	STX104_CHAN(0),
-	STX104_CHAN(1)
+/* single-ended input channels configuration */
+static const struct iio_chan_spec stx104_channels_sing[] = {
+	STX104_OUT_CHAN(0), STX104_OUT_CHAN(1),
+	STX104_IN_CHAN(0, 0), STX104_IN_CHAN(1, 0), STX104_IN_CHAN(2, 0),
+	STX104_IN_CHAN(3, 0), STX104_IN_CHAN(4, 0), STX104_IN_CHAN(5, 0),
+	STX104_IN_CHAN(6, 0), STX104_IN_CHAN(7, 0), STX104_IN_CHAN(8, 0),
+	STX104_IN_CHAN(9, 0), STX104_IN_CHAN(10, 0), STX104_IN_CHAN(11, 0),
+	STX104_IN_CHAN(12, 0), STX104_IN_CHAN(13, 0), STX104_IN_CHAN(14, 0),
+	STX104_IN_CHAN(15, 0)
+};
+/* differential input channels configuration */
+static const struct iio_chan_spec stx104_channels_diff[] = {
+	STX104_OUT_CHAN(0), STX104_OUT_CHAN(1),
+	STX104_IN_CHAN(0, 1), STX104_IN_CHAN(1, 1), STX104_IN_CHAN(2, 1),
+	STX104_IN_CHAN(3, 1), STX104_IN_CHAN(4, 1), STX104_IN_CHAN(5, 1),
+	STX104_IN_CHAN(6, 1), STX104_IN_CHAN(7, 1)
 };
 
 static int stx104_gpio_get_direction(struct gpio_chip *chip,
@@ -204,13 +295,27 @@ static int stx104_probe(struct device *dev, unsigned int id)
 
 	indio_dev->info = &stx104_info;
 	indio_dev->modes = INDIO_DIRECT_MODE;
-	indio_dev->channels = stx104_channels;
-	indio_dev->num_channels = STX104_NUM_CHAN;
+
+	/* determine if differential inputs */
+	if (inb(base[id] + 8) & BIT(5)) {
+		indio_dev->num_channels = ARRAY_SIZE(stx104_channels_diff);
+		indio_dev->channels = stx104_channels_diff;
+	} else {
+		indio_dev->num_channels = ARRAY_SIZE(stx104_channels_sing);
+		indio_dev->channels = stx104_channels_sing;
+	}
+
 	indio_dev->name = dev_name(dev);
 
 	priv = iio_priv(indio_dev);
 	priv->base = base[id];
 
+	/* configure device for software trigger operation */
+	outb(0, base[id] + 9);
+
+	/* initialize gain setting to x1 */
+	outb(0, base[id] + 11);
+
 	/* initialize DAC output to 0V */
 	outw(0, base[id] + 4);
 	outw(0, base[id] + 6);
@@ -271,5 +376,5 @@ static struct isa_driver stx104_driver = {
 module_isa_driver(stx104_driver, num_stx104);
 
 MODULE_AUTHOR("William Breathitt Gray <vilhelm.gray@gmail.com>");
-MODULE_DESCRIPTION("Apex Embedded Systems STX104 DAC driver");
+MODULE_DESCRIPTION("Apex Embedded Systems STX104 IIO driver");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/adc/ti-adc12138.c b/drivers/iio/adc/ti-adc12138.c
new file mode 100644
index 000000000000..072f03bfe6a0
--- /dev/null
+++ b/drivers/iio/adc/ti-adc12138.c
@@ -0,0 +1,552 @@
+/*
+ * ADC12130/ADC12132/ADC12138 12-bit plus sign ADC driver
+ *
+ * Copyright (c) 2016 Akinobu Mita <akinobu.mita@gmail.com>
+ *
+ * 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.
+ *
+ * Datasheet: http://www.ti.com/lit/ds/symlink/adc12138.pdf
+ */
+
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/completion.h>
+#include <linux/clk.h>
+#include <linux/spi/spi.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/trigger.h>
+#include <linux/iio/triggered_buffer.h>
+#include <linux/iio/trigger_consumer.h>
+#include <linux/regulator/consumer.h>
+
+#define ADC12138_MODE_AUTO_CAL			0x08
+#define ADC12138_MODE_READ_STATUS		0x0c
+#define ADC12138_MODE_ACQUISITION_TIME_6	0x0e
+#define ADC12138_MODE_ACQUISITION_TIME_10	0x4e
+#define ADC12138_MODE_ACQUISITION_TIME_18	0x8e
+#define ADC12138_MODE_ACQUISITION_TIME_34	0xce
+
+#define ADC12138_STATUS_CAL			BIT(6)
+
+enum {
+	adc12130,
+	adc12132,
+	adc12138,
+};
+
+struct adc12138 {
+	struct spi_device *spi;
+	unsigned int id;
+	/* conversion clock */
+	struct clk *cclk;
+	/* positive analog voltage reference */
+	struct regulator *vref_p;
+	/* negative analog voltage reference */
+	struct regulator *vref_n;
+	struct mutex lock;
+	struct completion complete;
+	/* The number of cclk periods for the S/H's acquisition time */
+	unsigned int acquisition_time;
+
+	u8 tx_buf[2] ____cacheline_aligned;
+	u8 rx_buf[2];
+};
+
+#define ADC12138_VOLTAGE_CHANNEL(chan)					\
+	{								\
+		.type = IIO_VOLTAGE,					\
+		.indexed = 1,						\
+		.channel = chan,					\
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),		\
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE)	\
+					| BIT(IIO_CHAN_INFO_OFFSET),	\
+		.scan_index = chan,					\
+		.scan_type = {						\
+			.sign = 's',					\
+			.realbits = 13,					\
+			.storagebits = 16,				\
+			.shift = 3,					\
+			.endianness = IIO_BE,				\
+		},							\
+	}
+
+#define ADC12138_VOLTAGE_CHANNEL_DIFF(chan1, chan2, si)			\
+	{								\
+		.type = IIO_VOLTAGE,					\
+		.indexed = 1,						\
+		.channel = (chan1),					\
+		.channel2 = (chan2),					\
+		.differential = 1,					\
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),		\
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE)	\
+					| BIT(IIO_CHAN_INFO_OFFSET),	\
+		.scan_index = si,					\
+		.scan_type = {						\
+			.sign = 's',					\
+			.realbits = 13,					\
+			.storagebits = 16,				\
+			.shift = 3,					\
+			.endianness = IIO_BE,				\
+		},							\
+	}
+
+static const struct iio_chan_spec adc12132_channels[] = {
+	ADC12138_VOLTAGE_CHANNEL(0),
+	ADC12138_VOLTAGE_CHANNEL(1),
+	ADC12138_VOLTAGE_CHANNEL_DIFF(0, 1, 2),
+	ADC12138_VOLTAGE_CHANNEL_DIFF(1, 0, 3),
+	IIO_CHAN_SOFT_TIMESTAMP(4),
+};
+
+static const struct iio_chan_spec adc12138_channels[] = {
+	ADC12138_VOLTAGE_CHANNEL(0),
+	ADC12138_VOLTAGE_CHANNEL(1),
+	ADC12138_VOLTAGE_CHANNEL(2),
+	ADC12138_VOLTAGE_CHANNEL(3),
+	ADC12138_VOLTAGE_CHANNEL(4),
+	ADC12138_VOLTAGE_CHANNEL(5),
+	ADC12138_VOLTAGE_CHANNEL(6),
+	ADC12138_VOLTAGE_CHANNEL(7),
+	ADC12138_VOLTAGE_CHANNEL_DIFF(0, 1, 8),
+	ADC12138_VOLTAGE_CHANNEL_DIFF(1, 0, 9),
+	ADC12138_VOLTAGE_CHANNEL_DIFF(2, 3, 10),
+	ADC12138_VOLTAGE_CHANNEL_DIFF(3, 2, 11),
+	ADC12138_VOLTAGE_CHANNEL_DIFF(4, 5, 12),
+	ADC12138_VOLTAGE_CHANNEL_DIFF(5, 4, 13),
+	ADC12138_VOLTAGE_CHANNEL_DIFF(6, 7, 14),
+	ADC12138_VOLTAGE_CHANNEL_DIFF(7, 6, 15),
+	IIO_CHAN_SOFT_TIMESTAMP(16),
+};
+
+static int adc12138_mode_programming(struct adc12138 *adc, u8 mode,
+				     void *rx_buf, int len)
+{
+	struct spi_transfer xfer = {
+		.tx_buf = adc->tx_buf,
+		.rx_buf = adc->rx_buf,
+		.len = len,
+	};
+	int ret;
+
+	/* Skip unused bits for ADC12130 and ADC12132 */
+	if (adc->id != adc12138)
+		mode = (mode & 0xc0) | ((mode & 0x0f) << 2);
+
+	adc->tx_buf[0] = mode;
+
+	ret = spi_sync_transfer(adc->spi, &xfer, 1);
+	if (ret)
+		return ret;
+
+	memcpy(rx_buf, adc->rx_buf, len);
+
+	return 0;
+}
+
+static int adc12138_read_status(struct adc12138 *adc)
+{
+	u8 rx_buf[2];
+	int ret;
+
+	ret = adc12138_mode_programming(adc, ADC12138_MODE_READ_STATUS,
+					rx_buf, 2);
+	if (ret)
+		return ret;
+
+	return (rx_buf[0] << 1) | (rx_buf[1] >> 7);
+}
+
+static int __adc12138_start_conv(struct adc12138 *adc,
+				 struct iio_chan_spec const *channel,
+				 void *data, int len)
+
+{
+	const u8 ch_to_mux[] = { 0, 4, 1, 5, 2, 6, 3, 7 };
+	u8 mode = (ch_to_mux[channel->channel] << 4) |
+		  (channel->differential ? 0 : 0x80);
+
+	return adc12138_mode_programming(adc, mode, data, len);
+}
+
+static int adc12138_start_conv(struct adc12138 *adc,
+			       struct iio_chan_spec const *channel)
+{
+	u8 trash;
+
+	return __adc12138_start_conv(adc, channel, &trash, 1);
+}
+
+static int adc12138_start_and_read_conv(struct adc12138 *adc,
+					struct iio_chan_spec const *channel,
+					__be16 *data)
+{
+	return __adc12138_start_conv(adc, channel, data, 2);
+}
+
+static int adc12138_read_conv_data(struct adc12138 *adc, __be16 *value)
+{
+	/* Issue a read status instruction and read previous conversion data */
+	return adc12138_mode_programming(adc, ADC12138_MODE_READ_STATUS,
+					 value, sizeof(*value));
+}
+
+static int adc12138_wait_eoc(struct adc12138 *adc, unsigned long timeout)
+{
+	if (!wait_for_completion_timeout(&adc->complete, timeout))
+		return -ETIMEDOUT;
+
+	return 0;
+}
+
+static int adc12138_adc_conversion(struct adc12138 *adc,
+				   struct iio_chan_spec const *channel,
+				   __be16 *value)
+{
+	int ret;
+
+	reinit_completion(&adc->complete);
+
+	ret = adc12138_start_conv(adc, channel);
+	if (ret)
+		return ret;
+
+	ret = adc12138_wait_eoc(adc, msecs_to_jiffies(100));
+	if (ret)
+		return ret;
+
+	return adc12138_read_conv_data(adc, value);
+}
+
+static int adc12138_read_raw(struct iio_dev *iio,
+			     struct iio_chan_spec const *channel, int *value,
+			     int *shift, long mask)
+{
+	struct adc12138 *adc = iio_priv(iio);
+	int ret;
+	__be16 data;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		mutex_lock(&adc->lock);
+		ret = adc12138_adc_conversion(adc, channel, &data);
+		mutex_unlock(&adc->lock);
+		if (ret)
+			return ret;
+
+		*value = sign_extend32(be16_to_cpu(data) >> 3, 12);
+
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_SCALE:
+		ret = regulator_get_voltage(adc->vref_p);
+		if (ret < 0)
+			return ret;
+		*value = ret;
+
+		if (!IS_ERR(adc->vref_n)) {
+			ret = regulator_get_voltage(adc->vref_n);
+			if (ret < 0)
+				return ret;
+			*value -= ret;
+		}
+
+		/* convert regulator output voltage to mV */
+		*value /= 1000;
+		*shift = channel->scan_type.realbits - 1;
+
+		return IIO_VAL_FRACTIONAL_LOG2;
+	case IIO_CHAN_INFO_OFFSET:
+		if (!IS_ERR(adc->vref_n)) {
+			*value = regulator_get_voltage(adc->vref_n);
+			if (*value < 0)
+				return *value;
+		} else {
+			*value = 0;
+		}
+
+		/* convert regulator output voltage to mV */
+		*value /= 1000;
+
+		return IIO_VAL_INT;
+	}
+
+	return -EINVAL;
+}
+
+static const struct iio_info adc12138_info = {
+	.read_raw = adc12138_read_raw,
+	.driver_module = THIS_MODULE,
+};
+
+static int adc12138_init(struct adc12138 *adc)
+{
+	int ret;
+	int status;
+	u8 mode;
+	u8 trash;
+
+	reinit_completion(&adc->complete);
+
+	ret = adc12138_mode_programming(adc, ADC12138_MODE_AUTO_CAL, &trash, 1);
+	if (ret)
+		return ret;
+
+	/* data output at this time has no significance */
+	status = adc12138_read_status(adc);
+	if (status < 0)
+		return status;
+
+	adc12138_wait_eoc(adc, msecs_to_jiffies(100));
+
+	status = adc12138_read_status(adc);
+	if (status & ADC12138_STATUS_CAL) {
+		dev_warn(&adc->spi->dev,
+			"Auto Cal sequence is still in progress: %#x\n",
+			status);
+		return -EIO;
+	}
+
+	switch (adc->acquisition_time) {
+	case 6:
+		mode = ADC12138_MODE_ACQUISITION_TIME_6;
+		break;
+	case 10:
+		mode = ADC12138_MODE_ACQUISITION_TIME_10;
+		break;
+	case 18:
+		mode = ADC12138_MODE_ACQUISITION_TIME_18;
+		break;
+	case 34:
+		mode = ADC12138_MODE_ACQUISITION_TIME_34;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return adc12138_mode_programming(adc, mode, &trash, 1);
+}
+
+static irqreturn_t adc12138_trigger_handler(int irq, void *p)
+{
+	struct iio_poll_func *pf = p;
+	struct iio_dev *indio_dev = pf->indio_dev;
+	struct adc12138 *adc = iio_priv(indio_dev);
+	__be16 data[20] = { }; /* 16x 2 bytes ADC data + 8 bytes timestamp */
+	__be16 trash;
+	int ret;
+	int scan_index;
+	int i = 0;
+
+	mutex_lock(&adc->lock);
+
+	for_each_set_bit(scan_index, indio_dev->active_scan_mask,
+			 indio_dev->masklength) {
+		const struct iio_chan_spec *scan_chan =
+				&indio_dev->channels[scan_index];
+
+		reinit_completion(&adc->complete);
+
+		ret = adc12138_start_and_read_conv(adc, scan_chan,
+						   i ? &data[i - 1] : &trash);
+		if (ret) {
+			dev_warn(&adc->spi->dev,
+				 "failed to start conversion\n");
+			goto out;
+		}
+
+		ret = adc12138_wait_eoc(adc, msecs_to_jiffies(100));
+		if (ret) {
+			dev_warn(&adc->spi->dev, "wait eoc timeout\n");
+			goto out;
+		}
+
+		i++;
+	}
+
+	if (i) {
+		ret = adc12138_read_conv_data(adc, &data[i - 1]);
+		if (ret) {
+			dev_warn(&adc->spi->dev,
+				 "failed to get conversion data\n");
+			goto out;
+		}
+	}
+
+	iio_push_to_buffers_with_timestamp(indio_dev, data,
+					   iio_get_time_ns(indio_dev));
+out:
+	mutex_unlock(&adc->lock);
+
+	iio_trigger_notify_done(indio_dev->trig);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t adc12138_eoc_handler(int irq, void *p)
+{
+	struct iio_dev *indio_dev = p;
+	struct adc12138 *adc = iio_priv(indio_dev);
+
+	complete(&adc->complete);
+
+	return IRQ_HANDLED;
+}
+
+static int adc12138_probe(struct spi_device *spi)
+{
+	struct iio_dev *indio_dev;
+	struct adc12138 *adc;
+	int ret;
+
+	indio_dev = devm_iio_device_alloc(&spi->dev, sizeof(*adc));
+	if (!indio_dev)
+		return -ENOMEM;
+
+	adc = iio_priv(indio_dev);
+	adc->spi = spi;
+	adc->id = spi_get_device_id(spi)->driver_data;
+	mutex_init(&adc->lock);
+	init_completion(&adc->complete);
+
+	indio_dev->name = spi_get_device_id(spi)->name;
+	indio_dev->dev.parent = &spi->dev;
+	indio_dev->info = &adc12138_info;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+
+	switch (adc->id) {
+	case adc12130:
+	case adc12132:
+		indio_dev->channels = adc12132_channels;
+		indio_dev->num_channels = ARRAY_SIZE(adc12132_channels);
+		break;
+	case adc12138:
+		indio_dev->channels = adc12138_channels;
+		indio_dev->num_channels = ARRAY_SIZE(adc12138_channels);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	ret = of_property_read_u32(spi->dev.of_node, "ti,acquisition-time",
+				   &adc->acquisition_time);
+	if (ret)
+		adc->acquisition_time = 10;
+
+	adc->cclk = devm_clk_get(&spi->dev, NULL);
+	if (IS_ERR(adc->cclk))
+		return PTR_ERR(adc->cclk);
+
+	adc->vref_p = devm_regulator_get(&spi->dev, "vref-p");
+	if (IS_ERR(adc->vref_p))
+		return PTR_ERR(adc->vref_p);
+
+	adc->vref_n = devm_regulator_get_optional(&spi->dev, "vref-n");
+	if (IS_ERR(adc->vref_n)) {
+		/*
+		 * Assume vref_n is 0V if an optional regulator is not
+		 * specified, otherwise return the error code.
+		 */
+		ret = PTR_ERR(adc->vref_n);
+		if (ret != -ENODEV)
+			return ret;
+	}
+
+	ret = devm_request_irq(&spi->dev, spi->irq, adc12138_eoc_handler,
+			       IRQF_TRIGGER_RISING, indio_dev->name, indio_dev);
+	if (ret)
+		return ret;
+
+	ret = clk_prepare_enable(adc->cclk);
+	if (ret)
+		return ret;
+
+	ret = regulator_enable(adc->vref_p);
+	if (ret)
+		goto err_clk_disable;
+
+	if (!IS_ERR(adc->vref_n)) {
+		ret = regulator_enable(adc->vref_n);
+		if (ret)
+			goto err_vref_p_disable;
+	}
+
+	ret = adc12138_init(adc);
+	if (ret)
+		goto err_vref_n_disable;
+
+	spi_set_drvdata(spi, indio_dev);
+
+	ret = iio_triggered_buffer_setup(indio_dev, NULL,
+					 adc12138_trigger_handler, NULL);
+	if (ret)
+		goto err_vref_n_disable;
+
+	ret = iio_device_register(indio_dev);
+	if (ret)
+		goto err_buffer_cleanup;
+
+	return 0;
+err_buffer_cleanup:
+	iio_triggered_buffer_cleanup(indio_dev);
+err_vref_n_disable:
+	if (!IS_ERR(adc->vref_n))
+		regulator_disable(adc->vref_n);
+err_vref_p_disable:
+	regulator_disable(adc->vref_p);
+err_clk_disable:
+	clk_disable_unprepare(adc->cclk);
+
+	return ret;
+}
+
+static int adc12138_remove(struct spi_device *spi)
+{
+	struct iio_dev *indio_dev = spi_get_drvdata(spi);
+	struct adc12138 *adc = iio_priv(indio_dev);
+
+	iio_device_unregister(indio_dev);
+	iio_triggered_buffer_cleanup(indio_dev);
+	if (!IS_ERR(adc->vref_n))
+		regulator_disable(adc->vref_n);
+	regulator_disable(adc->vref_p);
+	clk_disable_unprepare(adc->cclk);
+
+	return 0;
+}
+
+#ifdef CONFIG_OF
+
+static const struct of_device_id adc12138_dt_ids[] = {
+	{ .compatible = "ti,adc12130", },
+	{ .compatible = "ti,adc12132", },
+	{ .compatible = "ti,adc12138", },
+	{}
+};
+MODULE_DEVICE_TABLE(of, adc12138_dt_ids);
+
+#endif
+
+static const struct spi_device_id adc12138_id[] = {
+	{ "adc12130", adc12130 },
+	{ "adc12132", adc12132 },
+	{ "adc12138", adc12138 },
+	{}
+};
+MODULE_DEVICE_TABLE(spi, adc12138_id);
+
+static struct spi_driver adc12138_driver = {
+	.driver = {
+		.name = "adc12138",
+		.of_match_table = of_match_ptr(adc12138_dt_ids),
+	},
+	.probe = adc12138_probe,
+	.remove = adc12138_remove,
+	.id_table = adc12138_id,
+};
+module_spi_driver(adc12138_driver);
+
+MODULE_AUTHOR("Akinobu Mita <akinobu.mita@gmail.com>");
+MODULE_DESCRIPTION("ADC12130/ADC12132/ADC12138 driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/adc/ti-adc161s626.c b/drivers/iio/adc/ti-adc161s626.c
new file mode 100644
index 000000000000..f94b69f9c288
--- /dev/null
+++ b/drivers/iio/adc/ti-adc161s626.c
@@ -0,0 +1,248 @@
+/*
+ * ti-adc161s626.c - Texas Instruments ADC161S626 1-channel differential ADC
+ *
+ * ADC Devices Supported:
+ *  adc141s626 - 14-bit ADC
+ *  adc161s626 - 16-bit ADC
+ *
+ * Copyright (C) 2016 Matt Ranostay <mranostay@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/init.h>
+#include <linux/err.h>
+#include <linux/spi/spi.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/trigger.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/trigger_consumer.h>
+#include <linux/iio/triggered_buffer.h>
+
+#define TI_ADC_DRV_NAME	"ti-adc161s626"
+
+enum {
+	TI_ADC141S626,
+	TI_ADC161S626,
+};
+
+static const struct iio_chan_spec ti_adc141s626_channels[] = {
+	{
+		.type = IIO_VOLTAGE,
+		.channel = 0,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
+		.scan_index = 0,
+		.scan_type = {
+			.sign = 's',
+			.realbits = 14,
+			.storagebits = 16,
+		},
+	},
+	IIO_CHAN_SOFT_TIMESTAMP(1),
+};
+
+static const struct iio_chan_spec ti_adc161s626_channels[] = {
+	{
+		.type = IIO_VOLTAGE,
+		.channel = 0,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
+		.scan_index = 0,
+		.scan_type = {
+			.sign = 's',
+			.realbits = 16,
+			.storagebits = 16,
+		},
+	},
+	IIO_CHAN_SOFT_TIMESTAMP(1),
+};
+
+struct ti_adc_data {
+	struct iio_dev *indio_dev;
+	struct spi_device *spi;
+	u8 read_size;
+	u8 shift;
+
+	u8 buffer[16] ____cacheline_aligned;
+};
+
+static int ti_adc_read_measurement(struct ti_adc_data *data,
+				   struct iio_chan_spec const *chan, int *val)
+{
+	int ret;
+
+	switch (data->read_size) {
+	case 2: {
+		__be16 buf;
+
+		ret = spi_read(data->spi, (void *) &buf, 2);
+		if (ret)
+			return ret;
+
+		*val = be16_to_cpu(buf);
+		break;
+	}
+	case 3: {
+		__be32 buf;
+
+		ret = spi_read(data->spi, (void *) &buf, 3);
+		if (ret)
+			return ret;
+
+		*val = be32_to_cpu(buf) >> 8;
+		break;
+	}
+	default:
+		return -EINVAL;
+	}
+
+	*val = sign_extend32(*val >> data->shift, chan->scan_type.realbits - 1);
+
+	return 0;
+}
+
+static irqreturn_t ti_adc_trigger_handler(int irq, void *private)
+{
+	struct iio_poll_func *pf = private;
+	struct iio_dev *indio_dev = pf->indio_dev;
+	struct ti_adc_data *data = iio_priv(indio_dev);
+	int ret;
+
+	ret = ti_adc_read_measurement(data, &indio_dev->channels[0],
+				     (int *) &data->buffer);
+	if (!ret)
+		iio_push_to_buffers_with_timestamp(indio_dev,
+					data->buffer,
+					iio_get_time_ns(indio_dev));
+
+	iio_trigger_notify_done(indio_dev->trig);
+
+	return IRQ_HANDLED;
+}
+
+static int ti_adc_read_raw(struct iio_dev *indio_dev,
+			   struct iio_chan_spec const *chan,
+			   int *val, int *val2, long mask)
+{
+	struct ti_adc_data *data = iio_priv(indio_dev);
+	int ret;
+
+	if (mask != IIO_CHAN_INFO_RAW)
+		return -EINVAL;
+
+	ret = iio_device_claim_direct_mode(indio_dev);
+	if (ret)
+		return ret;
+
+	ret = ti_adc_read_measurement(data, chan, val);
+	iio_device_release_direct_mode(indio_dev);
+
+	if (!ret)
+		return IIO_VAL_INT;
+
+	return 0;
+}
+
+static const struct iio_info ti_adc_info = {
+	.driver_module = THIS_MODULE,
+	.read_raw = ti_adc_read_raw,
+};
+
+static int ti_adc_probe(struct spi_device *spi)
+{
+	struct iio_dev *indio_dev;
+	struct ti_adc_data *data;
+	int ret;
+
+	indio_dev = devm_iio_device_alloc(&spi->dev, sizeof(*data));
+	if (!indio_dev)
+		return -ENOMEM;
+
+	indio_dev->info = &ti_adc_info;
+	indio_dev->dev.parent = &spi->dev;
+	indio_dev->dev.of_node = spi->dev.of_node;
+	indio_dev->name = TI_ADC_DRV_NAME;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	spi_set_drvdata(spi, indio_dev);
+
+	data = iio_priv(indio_dev);
+	data->spi = spi;
+
+	switch (spi_get_device_id(spi)->driver_data) {
+	case TI_ADC141S626:
+		indio_dev->channels = ti_adc141s626_channels;
+		indio_dev->num_channels = ARRAY_SIZE(ti_adc141s626_channels);
+		data->shift = 0;
+		data->read_size = 2;
+		break;
+	case TI_ADC161S626:
+		indio_dev->channels = ti_adc161s626_channels;
+		indio_dev->num_channels = ARRAY_SIZE(ti_adc161s626_channels);
+		data->shift = 6;
+		data->read_size = 3;
+		break;
+	}
+
+	ret = iio_triggered_buffer_setup(indio_dev, NULL,
+					 ti_adc_trigger_handler, NULL);
+	if (ret)
+		return ret;
+
+	ret = iio_device_register(indio_dev);
+	if (ret)
+		goto error_unreg_buffer;
+
+	return 0;
+
+error_unreg_buffer:
+	iio_triggered_buffer_cleanup(indio_dev);
+
+	return ret;
+}
+
+static int ti_adc_remove(struct spi_device *spi)
+{
+	struct iio_dev *indio_dev = spi_get_drvdata(spi);
+
+	iio_device_unregister(indio_dev);
+	iio_triggered_buffer_cleanup(indio_dev);
+
+	return 0;
+}
+
+static const struct of_device_id ti_adc_dt_ids[] = {
+	{ .compatible = "ti,adc141s626", },
+	{ .compatible = "ti,adc161s626", },
+	{}
+};
+MODULE_DEVICE_TABLE(of, ti_adc_dt_ids);
+
+static const struct spi_device_id ti_adc_id[] = {
+	{"adc141s626", TI_ADC141S626},
+	{"adc161s626", TI_ADC161S626},
+	{},
+};
+MODULE_DEVICE_TABLE(spi, ti_adc_id);
+
+static struct spi_driver ti_adc_driver = {
+	.driver = {
+		.name	= TI_ADC_DRV_NAME,
+		.of_match_table = of_match_ptr(ti_adc_dt_ids),
+	},
+	.probe		= ti_adc_probe,
+	.remove		= ti_adc_remove,
+	.id_table	= ti_adc_id,
+};
+module_spi_driver(ti_adc_driver);
+
+MODULE_AUTHOR("Matt Ranostay <mranostay@gmail.com>");
+MODULE_DESCRIPTION("Texas Instruments ADC1x1S 1-channel differential ADC");
+MODULE_LICENSE("GPL");
diff --git a/drivers/iio/adc/ti-ads1015.c b/drivers/iio/adc/ti-ads1015.c
index 066abaf80201..cde6f130a99a 100644
--- a/drivers/iio/adc/ti-ads1015.c
+++ b/drivers/iio/adc/ti-ads1015.c
@@ -522,6 +522,7 @@ static int ads1015_get_channels_config_of(struct i2c_client *client)
 			if (pga > 6) {
 				dev_err(&client->dev, "invalid gain on %s\n",
 					node->full_name);
+				of_node_put(node);
 				return -EINVAL;
 			}
 		}
@@ -532,6 +533,7 @@ static int ads1015_get_channels_config_of(struct i2c_client *client)
 				dev_err(&client->dev,
 					"invalid data_rate on %s\n",
 					node->full_name);
+				of_node_put(node);
 				return -EINVAL;
 			}
 		}
diff --git a/drivers/iio/adc/ti-ads8688.c b/drivers/iio/adc/ti-ads8688.c
index c400439900af..4a163496d9e4 100644
--- a/drivers/iio/adc/ti-ads8688.c
+++ b/drivers/iio/adc/ti-ads8688.c
@@ -438,7 +438,7 @@ static int ads8688_probe(struct spi_device *spi)
 	return 0;
 
 error_out:
-	if (!IS_ERR_OR_NULL(st->reg))
+	if (!IS_ERR(st->reg))
 		regulator_disable(st->reg);
 
 	return ret;
@@ -451,7 +451,7 @@ static int ads8688_remove(struct spi_device *spi)
 
 	iio_device_unregister(indio_dev);
 
-	if (!IS_ERR_OR_NULL(st->reg))
+	if (!IS_ERR(st->reg))
 		regulator_disable(st->reg);
 
 	return 0;
diff --git a/drivers/iio/buffer/industrialio-buffer-cb.c b/drivers/iio/buffer/industrialio-buffer-cb.c
index 323079c3ccce..b8f550e47d3d 100644
--- a/drivers/iio/buffer/industrialio-buffer-cb.c
+++ b/drivers/iio/buffer/industrialio-buffer-cb.c
@@ -18,6 +18,7 @@ struct iio_cb_buffer {
 	int (*cb)(const void *data, void *private);
 	void *private;
 	struct iio_channel *channels;
+	struct iio_dev *indio_dev;
 };
 
 static struct iio_cb_buffer *buffer_to_cb_buffer(struct iio_buffer *buffer)
@@ -52,7 +53,6 @@ struct iio_cb_buffer *iio_channel_get_all_cb(struct device *dev,
 {
 	int ret;
 	struct iio_cb_buffer *cb_buff;
-	struct iio_dev *indio_dev;
 	struct iio_channel *chan;
 
 	cb_buff = kzalloc(sizeof(*cb_buff), GFP_KERNEL);
@@ -72,17 +72,17 @@ struct iio_cb_buffer *iio_channel_get_all_cb(struct device *dev,
 		goto error_free_cb_buff;
 	}
 
-	indio_dev = cb_buff->channels[0].indio_dev;
+	cb_buff->indio_dev = cb_buff->channels[0].indio_dev;
 	cb_buff->buffer.scan_mask
-		= kcalloc(BITS_TO_LONGS(indio_dev->masklength), sizeof(long),
-			  GFP_KERNEL);
+		= kcalloc(BITS_TO_LONGS(cb_buff->indio_dev->masklength),
+			  sizeof(long), GFP_KERNEL);
 	if (cb_buff->buffer.scan_mask == NULL) {
 		ret = -ENOMEM;
 		goto error_release_channels;
 	}
 	chan = &cb_buff->channels[0];
 	while (chan->indio_dev) {
-		if (chan->indio_dev != indio_dev) {
+		if (chan->indio_dev != cb_buff->indio_dev) {
 			ret = -EINVAL;
 			goto error_free_scan_mask;
 		}
@@ -105,17 +105,14 @@ EXPORT_SYMBOL_GPL(iio_channel_get_all_cb);
 
 int iio_channel_start_all_cb(struct iio_cb_buffer *cb_buff)
 {
-	return iio_update_buffers(cb_buff->channels[0].indio_dev,
-				  &cb_buff->buffer,
+	return iio_update_buffers(cb_buff->indio_dev, &cb_buff->buffer,
 				  NULL);
 }
 EXPORT_SYMBOL_GPL(iio_channel_start_all_cb);
 
 void iio_channel_stop_all_cb(struct iio_cb_buffer *cb_buff)
 {
-	iio_update_buffers(cb_buff->channels[0].indio_dev,
-			   NULL,
-			   &cb_buff->buffer);
+	iio_update_buffers(cb_buff->indio_dev, NULL, &cb_buff->buffer);
 }
 EXPORT_SYMBOL_GPL(iio_channel_stop_all_cb);
 
@@ -133,6 +130,13 @@ struct iio_channel
 }
 EXPORT_SYMBOL_GPL(iio_channel_cb_get_channels);
 
+struct iio_dev
+*iio_channel_cb_get_iio_dev(const struct iio_cb_buffer *cb_buffer)
+{
+	return cb_buffer->indio_dev;
+}
+EXPORT_SYMBOL_GPL(iio_channel_cb_get_iio_dev);
+
 MODULE_AUTHOR("Jonathan Cameron <jic23@kernel.org>");
 MODULE_DESCRIPTION("Industrial I/O callback buffer");
 MODULE_LICENSE("GPL");
diff --git a/drivers/iio/buffer/industrialio-triggered-buffer.c b/drivers/iio/buffer/industrialio-triggered-buffer.c
index 4b2858ba1fd6..d3db1fce54d2 100644
--- a/drivers/iio/buffer/industrialio-triggered-buffer.c
+++ b/drivers/iio/buffer/industrialio-triggered-buffer.c
@@ -98,6 +98,48 @@ void iio_triggered_buffer_cleanup(struct iio_dev *indio_dev)
 }
 EXPORT_SYMBOL(iio_triggered_buffer_cleanup);
 
+static void devm_iio_triggered_buffer_clean(struct device *dev, void *res)
+{
+	iio_triggered_buffer_cleanup(*(struct iio_dev **)res);
+}
+
+int devm_iio_triggered_buffer_setup(struct device *dev,
+				    struct iio_dev *indio_dev,
+				    irqreturn_t (*h)(int irq, void *p),
+				    irqreturn_t (*thread)(int irq, void *p),
+				    const struct iio_buffer_setup_ops *ops)
+{
+	struct iio_dev **ptr;
+	int ret;
+
+	ptr = devres_alloc(devm_iio_triggered_buffer_clean, sizeof(*ptr),
+			   GFP_KERNEL);
+	if (!ptr)
+		return -ENOMEM;
+
+	*ptr = indio_dev;
+
+	ret = iio_triggered_buffer_setup(indio_dev, h, thread, ops);
+	if (!ret)
+		devres_add(dev, ptr);
+	else
+		devres_free(ptr);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(devm_iio_triggered_buffer_setup);
+
+void devm_iio_triggered_buffer_cleanup(struct device *dev,
+				       struct iio_dev *indio_dev)
+{
+	int rc;
+
+	rc = devres_release(dev, devm_iio_triggered_buffer_clean,
+			    devm_iio_device_match, indio_dev);
+	WARN_ON(rc);
+}
+EXPORT_SYMBOL_GPL(devm_iio_triggered_buffer_cleanup);
+
 MODULE_AUTHOR("Lars-Peter Clausen <lars@metafoo.de>");
 MODULE_DESCRIPTION("IIO helper functions for setting up triggered buffers");
 MODULE_LICENSE("GPL");
diff --git a/drivers/iio/chemical/Kconfig b/drivers/iio/chemical/Kconfig
index 4bcc025e8c8a..cea7f9857a1f 100644
--- a/drivers/iio/chemical/Kconfig
+++ b/drivers/iio/chemical/Kconfig
@@ -16,6 +16,7 @@ config ATLAS_PH_SENSOR
 	 Atlas Scientific OEM SM sensors:
 	    * pH SM sensor
 	    * EC SM sensor
+	    * ORP SM sensor
 
 	 To compile this driver as module, choose M here: the
 	 module will be called atlas-ph-sensor.
diff --git a/drivers/iio/chemical/atlas-ph-sensor.c b/drivers/iio/chemical/atlas-ph-sensor.c
index 407f141a1eee..bd321b305a0a 100644
--- a/drivers/iio/chemical/atlas-ph-sensor.c
+++ b/drivers/iio/chemical/atlas-ph-sensor.c
@@ -66,12 +66,17 @@
 #define ATLAS_REG_TDS_DATA		0x1c
 #define ATLAS_REG_PSS_DATA		0x20
 
+#define ATLAS_REG_ORP_CALIB_STATUS	0x0d
+#define ATLAS_REG_ORP_DATA		0x0e
+
 #define ATLAS_PH_INT_TIME_IN_US		450000
 #define ATLAS_EC_INT_TIME_IN_US		650000
+#define ATLAS_ORP_INT_TIME_IN_US	450000
 
 enum {
 	ATLAS_PH_SM,
 	ATLAS_EC_SM,
+	ATLAS_ORP_SM,
 };
 
 struct atlas_data {
@@ -84,26 +89,10 @@ struct atlas_data {
 	__be32 buffer[6]; /* 96-bit data + 32-bit pad + 64-bit timestamp */
 };
 
-static const struct regmap_range atlas_volatile_ranges[] = {
-	regmap_reg_range(ATLAS_REG_INT_CONTROL, ATLAS_REG_INT_CONTROL),
-	regmap_reg_range(ATLAS_REG_PH_DATA, ATLAS_REG_PH_DATA + 4),
-	regmap_reg_range(ATLAS_REG_EC_DATA, ATLAS_REG_PSS_DATA + 4),
-};
-
-static const struct regmap_access_table atlas_volatile_table = {
-	.yes_ranges	= atlas_volatile_ranges,
-	.n_yes_ranges	= ARRAY_SIZE(atlas_volatile_ranges),
-};
-
 static const struct regmap_config atlas_regmap_config = {
 	.name = ATLAS_REGMAP_NAME,
-
 	.reg_bits = 8,
 	.val_bits = 8,
-
-	.volatile_table = &atlas_volatile_table,
-	.max_register = ATLAS_REG_PSS_DATA + 4,
-	.cache_type = REGCACHE_RBTREE,
 };
 
 static const struct iio_chan_spec atlas_ph_channels[] = {
@@ -175,6 +164,23 @@ static const struct iio_chan_spec atlas_ec_channels[] = {
 	},
 };
 
+static const struct iio_chan_spec atlas_orp_channels[] = {
+	{
+		.type = IIO_VOLTAGE,
+		.address = ATLAS_REG_ORP_DATA,
+		.info_mask_separate =
+			BIT(IIO_CHAN_INFO_RAW) | BIT(IIO_CHAN_INFO_SCALE),
+		.scan_index = 0,
+		.scan_type = {
+			.sign = 's',
+			.realbits = 32,
+			.storagebits = 32,
+			.endianness = IIO_BE,
+		},
+	},
+	IIO_CHAN_SOFT_TIMESTAMP(1),
+};
+
 static int atlas_check_ph_calibration(struct atlas_data *data)
 {
 	struct device *dev = &data->client->dev;
@@ -240,6 +246,22 @@ static int atlas_check_ec_calibration(struct atlas_data *data)
 	return 0;
 }
 
+static int atlas_check_orp_calibration(struct atlas_data *data)
+{
+	struct device *dev = &data->client->dev;
+	int ret;
+	unsigned int val;
+
+	ret = regmap_read(data->regmap, ATLAS_REG_ORP_CALIB_STATUS, &val);
+	if (ret)
+		return ret;
+
+	if (!val)
+		dev_warn(dev, "device has not been calibrated\n");
+
+	return 0;
+};
+
 struct atlas_device {
 	const struct iio_chan_spec *channels;
 	int num_channels;
@@ -264,7 +286,13 @@ static struct atlas_device atlas_devices[] = {
 				.calibration = &atlas_check_ec_calibration,
 				.delay = ATLAS_EC_INT_TIME_IN_US,
 	},
-
+	[ATLAS_ORP_SM] = {
+				.channels = atlas_orp_channels,
+				.num_channels = 2,
+				.data_reg = ATLAS_REG_ORP_DATA,
+				.calibration = &atlas_check_orp_calibration,
+				.delay = ATLAS_ORP_INT_TIME_IN_US,
+	},
 };
 
 static int atlas_set_powermode(struct atlas_data *data, int on)
@@ -402,15 +430,14 @@ static int atlas_read_raw(struct iio_dev *indio_dev,
 		case IIO_PH:
 		case IIO_CONCENTRATION:
 		case IIO_ELECTRICALCONDUCTIVITY:
-			mutex_lock(&indio_dev->mlock);
+		case IIO_VOLTAGE:
+			ret = iio_device_claim_direct_mode(indio_dev);
+			if (ret)
+				return ret;
 
-			if (iio_buffer_enabled(indio_dev))
-				ret = -EBUSY;
-			else
-				ret = atlas_read_measurement(data,
-							chan->address, &reg);
+			ret = atlas_read_measurement(data, chan->address, &reg);
 
-			mutex_unlock(&indio_dev->mlock);
+			iio_device_release_direct_mode(indio_dev);
 			break;
 		default:
 			ret = -EINVAL;
@@ -440,6 +467,10 @@ static int atlas_read_raw(struct iio_dev *indio_dev,
 			*val = 0; /* 0.000000001 */
 			*val2 = 1000;
 			return IIO_VAL_INT_PLUS_NANO;
+		case IIO_VOLTAGE:
+			*val = 1; /* 0.1 */
+			*val2 = 10;
+			break;
 		default:
 			return -EINVAL;
 		}
@@ -475,6 +506,7 @@ static const struct iio_info atlas_info = {
 static const struct i2c_device_id atlas_id[] = {
 	{ "atlas-ph-sm", ATLAS_PH_SM},
 	{ "atlas-ec-sm", ATLAS_EC_SM},
+	{ "atlas-orp-sm", ATLAS_ORP_SM},
 	{}
 };
 MODULE_DEVICE_TABLE(i2c, atlas_id);
@@ -482,6 +514,7 @@ MODULE_DEVICE_TABLE(i2c, atlas_id);
 static const struct of_device_id atlas_dt_ids[] = {
 	{ .compatible = "atlas,ph-sm", .data = (void *)ATLAS_PH_SM, },
 	{ .compatible = "atlas,ec-sm", .data = (void *)ATLAS_EC_SM, },
+	{ .compatible = "atlas,orp-sm", .data = (void *)ATLAS_ORP_SM, },
 	{ }
 };
 MODULE_DEVICE_TABLE(of, atlas_dt_ids);
diff --git a/drivers/iio/chemical/vz89x.c b/drivers/iio/chemical/vz89x.c
index 652649da500f..8e0e4415c161 100644
--- a/drivers/iio/chemical/vz89x.c
+++ b/drivers/iio/chemical/vz89x.c
@@ -19,25 +19,55 @@
 #include <linux/mutex.h>
 #include <linux/init.h>
 #include <linux/i2c.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
 
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
 
 #define VZ89X_REG_MEASUREMENT		0x09
-#define VZ89X_REG_MEASUREMENT_SIZE	6
+#define VZ89X_REG_MEASUREMENT_RD_SIZE	6
+#define VZ89X_REG_MEASUREMENT_WR_SIZE	3
 
 #define VZ89X_VOC_CO2_IDX		0
 #define VZ89X_VOC_SHORT_IDX		1
 #define VZ89X_VOC_TVOC_IDX		2
 #define VZ89X_VOC_RESISTANCE_IDX	3
 
+#define VZ89TE_REG_MEASUREMENT		0x0c
+#define VZ89TE_REG_MEASUREMENT_RD_SIZE	7
+#define VZ89TE_REG_MEASUREMENT_WR_SIZE	6
+
+#define VZ89TE_VOC_TVOC_IDX		0
+#define VZ89TE_VOC_CO2_IDX		1
+#define VZ89TE_VOC_RESISTANCE_IDX	2
+
+enum {
+	VZ89X,
+	VZ89TE,
+};
+
+struct vz89x_chip_data;
+
 struct vz89x_data {
 	struct i2c_client *client;
+	const struct vz89x_chip_data *chip;
 	struct mutex lock;
 	int (*xfer)(struct vz89x_data *data, u8 cmd);
 
+	bool is_valid;
 	unsigned long last_update;
-	u8 buffer[VZ89X_REG_MEASUREMENT_SIZE];
+	u8 buffer[VZ89TE_REG_MEASUREMENT_RD_SIZE];
+};
+
+struct vz89x_chip_data {
+	bool (*valid)(struct vz89x_data *data);
+	const struct iio_chan_spec *channels;
+	u8 num_channels;
+
+	u8 cmd;
+	u8 read_size;
+	u8 write_size;
 };
 
 static const struct iio_chan_spec vz89x_channels[] = {
@@ -70,6 +100,40 @@ static const struct iio_chan_spec vz89x_channels[] = {
 		.info_mask_separate =
 			BIT(IIO_CHAN_INFO_RAW) | BIT(IIO_CHAN_INFO_SCALE),
 		.address = VZ89X_VOC_RESISTANCE_IDX,
+		.scan_index = -1,
+		.scan_type = {
+			.endianness = IIO_LE,
+		},
+	},
+};
+
+static const struct iio_chan_spec vz89te_channels[] = {
+	{
+		.type = IIO_CONCENTRATION,
+		.channel2 = IIO_MOD_VOC,
+		.modified = 1,
+		.info_mask_separate =
+			BIT(IIO_CHAN_INFO_OFFSET) | BIT(IIO_CHAN_INFO_RAW),
+		.address = VZ89TE_VOC_TVOC_IDX,
+	},
+
+	{
+		.type = IIO_CONCENTRATION,
+		.channel2 = IIO_MOD_CO2,
+		.modified = 1,
+		.info_mask_separate =
+			BIT(IIO_CHAN_INFO_OFFSET) | BIT(IIO_CHAN_INFO_RAW),
+		.address = VZ89TE_VOC_CO2_IDX,
+	},
+	{
+		.type = IIO_RESISTANCE,
+		.info_mask_separate =
+			BIT(IIO_CHAN_INFO_RAW) | BIT(IIO_CHAN_INFO_SCALE),
+		.address = VZ89TE_VOC_RESISTANCE_IDX,
+		.scan_index = -1,
+		.scan_type = {
+			.endianness = IIO_BE,
+		},
 	},
 };
 
@@ -93,29 +157,45 @@ static const struct attribute_group vz89x_attrs_group = {
  * always zero, and by also confirming the VOC_short isn't zero.
  */
 
-static int vz89x_measurement_is_valid(struct vz89x_data *data)
+static bool vz89x_measurement_is_valid(struct vz89x_data *data)
 {
 	if (data->buffer[VZ89X_VOC_SHORT_IDX] == 0)
-		return 1;
+		return true;
 
-	return !!(data->buffer[VZ89X_REG_MEASUREMENT_SIZE - 1] > 0);
+	return !!(data->buffer[data->chip->read_size - 1] > 0);
+}
+
+/* VZ89TE device has a modified CRC-8 two complement check */
+static bool vz89te_measurement_is_valid(struct vz89x_data *data)
+{
+	u8 crc = 0;
+	int i, sum = 0;
+
+	for (i = 0; i < (data->chip->read_size - 1); i++) {
+		sum = crc + data->buffer[i];
+		crc = sum;
+		crc += sum / 256;
+	}
+
+	return !((0xff - crc) == data->buffer[data->chip->read_size - 1]);
 }
 
 static int vz89x_i2c_xfer(struct vz89x_data *data, u8 cmd)
 {
+	const struct vz89x_chip_data *chip = data->chip;
 	struct i2c_client *client = data->client;
 	struct i2c_msg msg[2];
 	int ret;
-	u8 buf[3] = { cmd, 0, 0};
+	u8 buf[6] = { cmd, 0, 0, 0, 0, 0xf3 };
 
 	msg[0].addr = client->addr;
 	msg[0].flags = client->flags;
-	msg[0].len = 3;
+	msg[0].len = chip->write_size;
 	msg[0].buf  = (char *) &buf;
 
 	msg[1].addr = client->addr;
 	msg[1].flags = client->flags | I2C_M_RD;
-	msg[1].len = VZ89X_REG_MEASUREMENT_SIZE;
+	msg[1].len = chip->read_size;
 	msg[1].buf = (char *) &data->buffer;
 
 	ret = i2c_transfer(client->adapter, msg, 2);
@@ -133,7 +213,7 @@ static int vz89x_smbus_xfer(struct vz89x_data *data, u8 cmd)
 	if (ret < 0)
 		return ret;
 
-	for (i = 0; i < VZ89X_REG_MEASUREMENT_SIZE; i++) {
+	for (i = 0; i < data->chip->read_size; i++) {
 		ret = i2c_smbus_read_byte(client);
 		if (ret < 0)
 			return ret;
@@ -145,30 +225,47 @@ static int vz89x_smbus_xfer(struct vz89x_data *data, u8 cmd)
 
 static int vz89x_get_measurement(struct vz89x_data *data)
 {
+	const struct vz89x_chip_data *chip = data->chip;
 	int ret;
 
 	/* sensor can only be polled once a second max per datasheet */
 	if (!time_after(jiffies, data->last_update + HZ))
-		return 0;
+		return data->is_valid ? 0 : -EAGAIN;
+
+	data->is_valid = false;
+	data->last_update = jiffies;
 
-	ret = data->xfer(data, VZ89X_REG_MEASUREMENT);
+	ret = data->xfer(data, chip->cmd);
 	if (ret < 0)
 		return ret;
 
-	ret = vz89x_measurement_is_valid(data);
+	ret = chip->valid(data);
 	if (ret)
 		return -EAGAIN;
 
-	data->last_update = jiffies;
+	data->is_valid = true;
 
 	return 0;
 }
 
-static int vz89x_get_resistance_reading(struct vz89x_data *data)
+static int vz89x_get_resistance_reading(struct vz89x_data *data,
+					struct iio_chan_spec const *chan,
+					int *val)
 {
-	u8 *buf = &data->buffer[VZ89X_VOC_RESISTANCE_IDX];
+	u8 *tmp = (u8 *) &data->buffer[chan->address];
 
-	return buf[0] | (buf[1] << 8);
+	switch (chan->scan_type.endianness) {
+	case IIO_LE:
+		*val = le32_to_cpup((__le32 *) tmp) & GENMASK(23, 0);
+		break;
+	case IIO_BE:
+		*val = be32_to_cpup((__be32 *) tmp) >> 8;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
 }
 
 static int vz89x_read_raw(struct iio_dev *indio_dev,
@@ -187,15 +284,15 @@ static int vz89x_read_raw(struct iio_dev *indio_dev,
 		if (ret)
 			return ret;
 
-		switch (chan->address) {
-		case VZ89X_VOC_CO2_IDX:
-		case VZ89X_VOC_SHORT_IDX:
-		case VZ89X_VOC_TVOC_IDX:
+		switch (chan->type) {
+		case IIO_CONCENTRATION:
 			*val = data->buffer[chan->address];
 			return IIO_VAL_INT;
-		case VZ89X_VOC_RESISTANCE_IDX:
-			*val = vz89x_get_resistance_reading(data);
-			return IIO_VAL_INT;
+		case IIO_RESISTANCE:
+			ret = vz89x_get_resistance_reading(data, chan, val);
+			if (!ret)
+				return IIO_VAL_INT;
+			break;
 		default:
 			return -EINVAL;
 		}
@@ -210,12 +307,12 @@ static int vz89x_read_raw(struct iio_dev *indio_dev,
 		}
 		break;
 	case IIO_CHAN_INFO_OFFSET:
-		switch (chan->address) {
-		case VZ89X_VOC_CO2_IDX:
+		switch (chan->channel2) {
+		case IIO_MOD_CO2:
 			*val = 44;
 			*val2 = 250000;
 			return IIO_VAL_INT_PLUS_MICRO;
-		case VZ89X_VOC_TVOC_IDX:
+		case IIO_MOD_VOC:
 			*val = -13;
 			return IIO_VAL_INT;
 		default:
@@ -232,11 +329,43 @@ static const struct iio_info vz89x_info = {
 	.driver_module	= THIS_MODULE,
 };
 
+static const struct vz89x_chip_data vz89x_chips[] = {
+	{
+		.valid = vz89x_measurement_is_valid,
+
+		.cmd = VZ89X_REG_MEASUREMENT,
+		.read_size = VZ89X_REG_MEASUREMENT_RD_SIZE,
+		.write_size = VZ89X_REG_MEASUREMENT_WR_SIZE,
+
+		.channels = vz89x_channels,
+		.num_channels = ARRAY_SIZE(vz89x_channels),
+	},
+	{
+		.valid = vz89te_measurement_is_valid,
+
+		.cmd = VZ89TE_REG_MEASUREMENT,
+		.read_size = VZ89TE_REG_MEASUREMENT_RD_SIZE,
+		.write_size = VZ89TE_REG_MEASUREMENT_WR_SIZE,
+
+		.channels = vz89te_channels,
+		.num_channels = ARRAY_SIZE(vz89te_channels),
+	},
+};
+
+static const struct of_device_id vz89x_dt_ids[] = {
+	{ .compatible = "sgx,vz89x", .data = (void *) VZ89X },
+	{ .compatible = "sgx,vz89te", .data = (void *) VZ89TE },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, vz89x_dt_ids);
+
 static int vz89x_probe(struct i2c_client *client,
 		       const struct i2c_device_id *id)
 {
 	struct iio_dev *indio_dev;
 	struct vz89x_data *data;
+	const struct of_device_id *of_id;
+	int chip_id;
 
 	indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*data));
 	if (!indio_dev)
@@ -251,8 +380,15 @@ static int vz89x_probe(struct i2c_client *client,
 	else
 		return -EOPNOTSUPP;
 
+	of_id = of_match_device(vz89x_dt_ids, &client->dev);
+	if (!of_id)
+		chip_id = id->driver_data;
+	else
+		chip_id = (unsigned long)of_id->data;
+
 	i2c_set_clientdata(client, indio_dev);
 	data->client = client;
+	data->chip = &vz89x_chips[chip_id];
 	data->last_update = jiffies - HZ;
 	mutex_init(&data->lock);
 
@@ -261,24 +397,19 @@ static int vz89x_probe(struct i2c_client *client,
 	indio_dev->name = dev_name(&client->dev);
 	indio_dev->modes = INDIO_DIRECT_MODE;
 
-	indio_dev->channels = vz89x_channels;
-	indio_dev->num_channels = ARRAY_SIZE(vz89x_channels);
+	indio_dev->channels = data->chip->channels;
+	indio_dev->num_channels = data->chip->num_channels;
 
 	return devm_iio_device_register(&client->dev, indio_dev);
 }
 
 static const struct i2c_device_id vz89x_id[] = {
-	{ "vz89x", 0 },
+	{ "vz89x", VZ89X },
+	{ "vz89te", VZ89TE },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, vz89x_id);
 
-static const struct of_device_id vz89x_dt_ids[] = {
-	{ .compatible = "sgx,vz89x" },
-	{ }
-};
-MODULE_DEVICE_TABLE(of, vz89x_dt_ids);
-
 static struct i2c_driver vz89x_driver = {
 	.driver = {
 		.name	= "vz89x",
diff --git a/drivers/iio/common/hid-sensors/hid-sensor-trigger.c b/drivers/iio/common/hid-sensors/hid-sensor-trigger.c
index 5b41f9d0d4f3..a3cce3a38300 100644
--- a/drivers/iio/common/hid-sensors/hid-sensor-trigger.c
+++ b/drivers/iio/common/hid-sensors/hid-sensor-trigger.c
@@ -122,6 +122,14 @@ int hid_sensor_power_state(struct hid_sensor_common *st, bool state)
 #endif
 }
 
+static void hid_sensor_set_power_work(struct work_struct *work)
+{
+	struct hid_sensor_common *attrb = container_of(work,
+						       struct hid_sensor_common,
+						       work);
+	_hid_sensor_power_state(attrb, true);
+}
+
 static int hid_sensor_data_rdy_trigger_set_state(struct iio_trigger *trig,
 						bool state)
 {
@@ -130,6 +138,7 @@ static int hid_sensor_data_rdy_trigger_set_state(struct iio_trigger *trig,
 
 void hid_sensor_remove_trigger(struct hid_sensor_common *attrb)
 {
+	cancel_work_sync(&attrb->work);
 	iio_trigger_unregister(attrb->trigger);
 	iio_trigger_free(attrb->trigger);
 }
@@ -170,6 +179,9 @@ int hid_sensor_setup_trigger(struct iio_dev *indio_dev, const char *name,
 		goto error_unreg_trigger;
 
 	iio_device_set_drvdata(indio_dev, attrb);
+
+	INIT_WORK(&attrb->work, hid_sensor_set_power_work);
+
 	pm_suspend_ignore_children(&attrb->pdev->dev, true);
 	pm_runtime_enable(&attrb->pdev->dev);
 	/* Default to 3 seconds, but can be changed from sysfs */
@@ -187,8 +199,7 @@ error_ret:
 }
 EXPORT_SYMBOL(hid_sensor_setup_trigger);
 
-#ifdef CONFIG_PM
-static int hid_sensor_suspend(struct device *dev)
+static int __maybe_unused hid_sensor_suspend(struct device *dev)
 {
 	struct platform_device *pdev = to_platform_device(dev);
 	struct iio_dev *indio_dev = platform_get_drvdata(pdev);
@@ -197,21 +208,27 @@ static int hid_sensor_suspend(struct device *dev)
 	return _hid_sensor_power_state(attrb, false);
 }
 
-static int hid_sensor_resume(struct device *dev)
+static int __maybe_unused hid_sensor_resume(struct device *dev)
 {
 	struct platform_device *pdev = to_platform_device(dev);
 	struct iio_dev *indio_dev = platform_get_drvdata(pdev);
 	struct hid_sensor_common *attrb = iio_device_get_drvdata(indio_dev);
+	schedule_work(&attrb->work);
+	return 0;
+}
 
+static int __maybe_unused hid_sensor_runtime_resume(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct iio_dev *indio_dev = platform_get_drvdata(pdev);
+	struct hid_sensor_common *attrb = iio_device_get_drvdata(indio_dev);
 	return _hid_sensor_power_state(attrb, true);
 }
 
-#endif
-
 const struct dev_pm_ops hid_sensor_pm_ops = {
 	SET_SYSTEM_SLEEP_PM_OPS(hid_sensor_suspend, hid_sensor_resume)
 	SET_RUNTIME_PM_OPS(hid_sensor_suspend,
-			   hid_sensor_resume, NULL)
+			   hid_sensor_runtime_resume, NULL)
 };
 EXPORT_SYMBOL(hid_sensor_pm_ops);
 
diff --git a/drivers/iio/common/st_sensors/st_sensors_buffer.c b/drivers/iio/common/st_sensors/st_sensors_buffer.c
index d06e728cea37..fe7775bb3740 100644
--- a/drivers/iio/common/st_sensors/st_sensors_buffer.c
+++ b/drivers/iio/common/st_sensors/st_sensors_buffer.c
@@ -63,7 +63,7 @@ irqreturn_t st_sensors_trigger_handler(int irq, void *p)
 	 * the hardware trigger) and the hw_timestamp may get updated.
 	 * By storing it in a local variable first, we are safe.
 	 */
-	if (sdata->hw_irq_trigger)
+	if (iio_trigger_using_own(indio_dev))
 		timestamp = sdata->hw_timestamp;
 	else
 		timestamp = iio_get_time_ns(indio_dev);
diff --git a/drivers/iio/common/st_sensors/st_sensors_core.c b/drivers/iio/common/st_sensors/st_sensors_core.c
index 2d5282e05482..285a64a589d7 100644
--- a/drivers/iio/common/st_sensors/st_sensors_core.c
+++ b/drivers/iio/common/st_sensors/st_sensors_core.c
@@ -234,39 +234,35 @@ int st_sensors_power_enable(struct iio_dev *indio_dev)
 	int err;
 
 	/* Regulators not mandatory, but if requested we should enable them. */
-	pdata->vdd = devm_regulator_get_optional(indio_dev->dev.parent, "vdd");
-	if (!IS_ERR(pdata->vdd)) {
-		err = regulator_enable(pdata->vdd);
-		if (err != 0) {
-			dev_warn(&indio_dev->dev,
-				 "Failed to enable specified Vdd supply\n");
-			return err;
-		}
-	} else {
-		err = PTR_ERR(pdata->vdd);
-		if (err != -ENODEV)
-			return err;
+	pdata->vdd = devm_regulator_get(indio_dev->dev.parent, "vdd");
+	if (IS_ERR(pdata->vdd)) {
+		dev_err(&indio_dev->dev, "unable to get Vdd supply\n");
+		return PTR_ERR(pdata->vdd);
+	}
+	err = regulator_enable(pdata->vdd);
+	if (err != 0) {
+		dev_warn(&indio_dev->dev,
+			 "Failed to enable specified Vdd supply\n");
+		return err;
 	}
 
-	pdata->vdd_io = devm_regulator_get_optional(indio_dev->dev.parent, "vddio");
-	if (!IS_ERR(pdata->vdd_io)) {
-		err = regulator_enable(pdata->vdd_io);
-		if (err != 0) {
-			dev_warn(&indio_dev->dev,
-				 "Failed to enable specified Vdd_IO supply\n");
-			goto st_sensors_disable_vdd;
-		}
-	} else {
+	pdata->vdd_io = devm_regulator_get(indio_dev->dev.parent, "vddio");
+	if (IS_ERR(pdata->vdd_io)) {
+		dev_err(&indio_dev->dev, "unable to get Vdd_IO supply\n");
 		err = PTR_ERR(pdata->vdd_io);
-		if (err != -ENODEV)
-			goto st_sensors_disable_vdd;
+		goto st_sensors_disable_vdd;
+	}
+	err = regulator_enable(pdata->vdd_io);
+	if (err != 0) {
+		dev_warn(&indio_dev->dev,
+			 "Failed to enable specified Vdd_IO supply\n");
+		goto st_sensors_disable_vdd;
 	}
 
 	return 0;
 
 st_sensors_disable_vdd:
-	if (!IS_ERR_OR_NULL(pdata->vdd))
-		regulator_disable(pdata->vdd);
+	regulator_disable(pdata->vdd);
 	return err;
 }
 EXPORT_SYMBOL(st_sensors_power_enable);
@@ -275,11 +271,8 @@ void st_sensors_power_disable(struct iio_dev *indio_dev)
 {
 	struct st_sensor_data *pdata = iio_priv(indio_dev);
 
-	if (!IS_ERR_OR_NULL(pdata->vdd))
-		regulator_disable(pdata->vdd);
-
-	if (!IS_ERR_OR_NULL(pdata->vdd_io))
-		regulator_disable(pdata->vdd_io);
+	regulator_disable(pdata->vdd);
+	regulator_disable(pdata->vdd_io);
 }
 EXPORT_SYMBOL(st_sensors_power_disable);
 
diff --git a/drivers/iio/common/st_sensors/st_sensors_trigger.c b/drivers/iio/common/st_sensors/st_sensors_trigger.c
index e66f12ee8a55..fa73e6795359 100644
--- a/drivers/iio/common/st_sensors/st_sensors_trigger.c
+++ b/drivers/iio/common/st_sensors/st_sensors_trigger.c
@@ -66,7 +66,7 @@ static int st_sensors_new_samples_available(struct iio_dev *indio_dev,
  * @irq: irq number
  * @p: private handler data
  */
-irqreturn_t st_sensors_irq_handler(int irq, void *p)
+static irqreturn_t st_sensors_irq_handler(int irq, void *p)
 {
 	struct iio_trigger *trig = p;
 	struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig);
@@ -82,7 +82,7 @@ irqreturn_t st_sensors_irq_handler(int irq, void *p)
  * @irq: irq number
  * @p: private handler data
  */
-irqreturn_t st_sensors_irq_thread(int irq, void *p)
+static irqreturn_t st_sensors_irq_thread(int irq, void *p)
 {
 	struct iio_trigger *trig = p;
 	struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig);
diff --git a/drivers/iio/dac/Kconfig b/drivers/iio/dac/Kconfig
index ca814479fadf..120b24478469 100644
--- a/drivers/iio/dac/Kconfig
+++ b/drivers/iio/dac/Kconfig
@@ -181,6 +181,25 @@ config AD7303
 	  To compile this driver as module choose M here: the module will be called
 	  ad7303.
 
+config CIO_DAC
+	tristate "Measurement Computing CIO-DAC IIO driver"
+	depends on X86 && ISA_BUS_API
+	help
+	  Say yes here to build support for the Measurement Computing CIO-DAC
+	  analog output device family (CIO-DAC16, CIO-DAC08, PC104-DAC06). The
+	  base port addresses for the devices may be configured via the base
+	  array module parameter.
+
+config AD8801
+	tristate "Analog Devices AD8801/AD8803 DAC driver"
+	depends on SPI_MASTER
+	help
+	  Say yes here to build support for Analog Devices AD8801, AD8803 Digital to
+	  Analog Converters (DAC).
+
+	  To compile this driver as a module choose M here: the module will be called
+	  ad8801.
+
 config LPC18XX_DAC
 	tristate "NXP LPC18xx DAC driver"
 	depends on ARCH_LPC18XX || COMPILE_TEST
@@ -245,16 +264,6 @@ config MCP4922
 	  To compile this driver as a module, choose M here: the module
 	  will be called mcp4922.
 
-config STX104
-	tristate "Apex Embedded Systems STX104 DAC driver"
-	depends on X86 && ISA_BUS_API
-	select GPIOLIB
-	help
-	  Say yes here to build support for the 2-channel DAC and GPIO on the
-	  Apex Embedded Systems STX104 integrated analog PC/104 card. The base
-	  port addresses for the devices may be configured via the base array
-	  module parameter.
-
 config VF610_DAC
 	tristate "Vybrid vf610 DAC driver"
 	depends on OF
diff --git a/drivers/iio/dac/Makefile b/drivers/iio/dac/Makefile
index 8b78d5ca9b11..27642bbf75f2 100644
--- a/drivers/iio/dac/Makefile
+++ b/drivers/iio/dac/Makefile
@@ -20,11 +20,12 @@ obj-$(CONFIG_AD5764) += ad5764.o
 obj-$(CONFIG_AD5791) += ad5791.o
 obj-$(CONFIG_AD5686) += ad5686.o
 obj-$(CONFIG_AD7303) += ad7303.o
+obj-$(CONFIG_AD8801) += ad8801.o
+obj-$(CONFIG_CIO_DAC) += cio-dac.o
 obj-$(CONFIG_LPC18XX_DAC) += lpc18xx_dac.o
 obj-$(CONFIG_M62332) += m62332.o
 obj-$(CONFIG_MAX517) += max517.o
 obj-$(CONFIG_MAX5821) += max5821.o
 obj-$(CONFIG_MCP4725) += mcp4725.o
 obj-$(CONFIG_MCP4922) += mcp4922.o
-obj-$(CONFIG_STX104) += stx104.o
 obj-$(CONFIG_VF610_DAC) += vf610_dac.o
diff --git a/drivers/iio/dac/ad5755.c b/drivers/iio/dac/ad5755.c
index 0fde593ec0d9..5f7968232564 100644
--- a/drivers/iio/dac/ad5755.c
+++ b/drivers/iio/dac/ad5755.c
@@ -655,7 +655,7 @@ static struct ad5755_platform_data *ad5755_parse_dt(struct device *dev)
 
 	devnr = 0;
 	for_each_child_of_node(np, pp) {
-		if (devnr > AD5755_NUM_CHANNELS) {
+		if (devnr >= AD5755_NUM_CHANNELS) {
 			dev_err(dev,
 				"There is to many channels defined in DT\n");
 			goto error_out;
diff --git a/drivers/iio/dac/ad8801.c b/drivers/iio/dac/ad8801.c
new file mode 100644
index 000000000000..f06faa1aec09
--- /dev/null
+++ b/drivers/iio/dac/ad8801.c
@@ -0,0 +1,239 @@
+/*
+ * IIO DAC driver for Analog Devices AD8801 DAC
+ *
+ * Copyright (C) 2016 Gwenhael Goavec-Merou
+ * This program is free software; you can 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/iio/iio.h>
+#include <linux/module.h>
+#include <linux/regulator/consumer.h>
+#include <linux/spi/spi.h>
+#include <linux/sysfs.h>
+
+#define AD8801_CFG_ADDR_OFFSET 8
+
+enum ad8801_device_ids {
+	ID_AD8801,
+	ID_AD8803,
+};
+
+struct ad8801_state {
+	struct spi_device *spi;
+	unsigned char dac_cache[8]; /* Value write on each channel */
+	unsigned int vrefh_mv;
+	unsigned int vrefl_mv;
+	struct regulator *vrefh_reg;
+	struct regulator *vrefl_reg;
+
+	__be16 data ____cacheline_aligned;
+};
+
+static int ad8801_spi_write(struct ad8801_state *state,
+	u8 channel, unsigned char value)
+{
+	state->data = cpu_to_be16((channel << AD8801_CFG_ADDR_OFFSET) | value);
+	return spi_write(state->spi, &state->data, sizeof(state->data));
+}
+
+static int ad8801_write_raw(struct iio_dev *indio_dev,
+	struct iio_chan_spec const *chan, int val, int val2, long mask)
+{
+	struct ad8801_state *state = iio_priv(indio_dev);
+	int ret;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		if (val >= 256 || val < 0)
+			return -EINVAL;
+
+		ret = ad8801_spi_write(state, chan->channel, val);
+		if (ret == 0)
+			state->dac_cache[chan->channel] = val;
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+static int ad8801_read_raw(struct iio_dev *indio_dev,
+	struct iio_chan_spec const *chan, int *val, int *val2, long info)
+{
+	struct ad8801_state *state = iio_priv(indio_dev);
+
+	switch (info) {
+	case IIO_CHAN_INFO_RAW:
+		*val = state->dac_cache[chan->channel];
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_SCALE:
+		*val = state->vrefh_mv - state->vrefl_mv;
+		*val2 = 8;
+		return IIO_VAL_FRACTIONAL_LOG2;
+	case IIO_CHAN_INFO_OFFSET:
+		*val = state->vrefl_mv;
+		return IIO_VAL_INT;
+	default:
+		return -EINVAL;
+	}
+
+	return -EINVAL;
+}
+
+static const struct iio_info ad8801_info = {
+	.read_raw = ad8801_read_raw,
+	.write_raw = ad8801_write_raw,
+	.driver_module = THIS_MODULE,
+};
+
+#define AD8801_CHANNEL(chan) {		\
+	.type = IIO_VOLTAGE,			\
+	.indexed = 1,				\
+	.output = 1,				\
+	.channel = chan,			\
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW), \
+	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE) |	\
+		BIT(IIO_CHAN_INFO_OFFSET), \
+}
+
+static const struct iio_chan_spec ad8801_channels[] = {
+	AD8801_CHANNEL(0),
+	AD8801_CHANNEL(1),
+	AD8801_CHANNEL(2),
+	AD8801_CHANNEL(3),
+	AD8801_CHANNEL(4),
+	AD8801_CHANNEL(5),
+	AD8801_CHANNEL(6),
+	AD8801_CHANNEL(7),
+};
+
+static int ad8801_probe(struct spi_device *spi)
+{
+	struct iio_dev *indio_dev;
+	struct ad8801_state *state;
+	const struct spi_device_id *id;
+	int ret;
+
+	indio_dev = devm_iio_device_alloc(&spi->dev, sizeof(*state));
+	if (indio_dev == NULL)
+		return -ENOMEM;
+
+	state = iio_priv(indio_dev);
+	state->spi = spi;
+	id = spi_get_device_id(spi);
+
+	state->vrefh_reg = devm_regulator_get(&spi->dev, "vrefh");
+	if (IS_ERR(state->vrefh_reg)) {
+		dev_err(&spi->dev, "Vrefh regulator not specified\n");
+		return PTR_ERR(state->vrefh_reg);
+	}
+
+	ret = regulator_enable(state->vrefh_reg);
+	if (ret) {
+		dev_err(&spi->dev, "Failed to enable vrefh regulator: %d\n",
+				ret);
+		return ret;
+	}
+
+	ret = regulator_get_voltage(state->vrefh_reg);
+	if (ret < 0) {
+		dev_err(&spi->dev, "Failed to read vrefh regulator: %d\n",
+				ret);
+		goto error_disable_vrefh_reg;
+	}
+	state->vrefh_mv = ret / 1000;
+
+	if (id->driver_data == ID_AD8803) {
+		state->vrefl_reg = devm_regulator_get(&spi->dev, "vrefl");
+		if (IS_ERR(state->vrefl_reg)) {
+			dev_err(&spi->dev, "Vrefl regulator not specified\n");
+			ret = PTR_ERR(state->vrefl_reg);
+			goto error_disable_vrefh_reg;
+		}
+
+		ret = regulator_enable(state->vrefl_reg);
+		if (ret) {
+			dev_err(&spi->dev, "Failed to enable vrefl regulator: %d\n",
+					ret);
+			goto error_disable_vrefh_reg;
+		}
+
+		ret = regulator_get_voltage(state->vrefl_reg);
+		if (ret < 0) {
+			dev_err(&spi->dev, "Failed to read vrefl regulator: %d\n",
+					ret);
+			goto error_disable_vrefl_reg;
+		}
+		state->vrefl_mv = ret / 1000;
+	} else {
+		state->vrefl_mv = 0;
+		state->vrefl_reg = NULL;
+	}
+
+	spi_set_drvdata(spi, indio_dev);
+	indio_dev->dev.parent = &spi->dev;
+	indio_dev->info = &ad8801_info;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->channels = ad8801_channels;
+	indio_dev->num_channels = ARRAY_SIZE(ad8801_channels);
+	indio_dev->name = id->name;
+
+	ret = iio_device_register(indio_dev);
+	if (ret) {
+		dev_err(&spi->dev, "Failed to register iio device: %d\n",
+				ret);
+		goto error_disable_vrefl_reg;
+	}
+
+	return 0;
+
+error_disable_vrefl_reg:
+	if (state->vrefl_reg)
+		regulator_disable(state->vrefl_reg);
+error_disable_vrefh_reg:
+	regulator_disable(state->vrefh_reg);
+	return ret;
+}
+
+static int ad8801_remove(struct spi_device *spi)
+{
+	struct iio_dev *indio_dev = spi_get_drvdata(spi);
+	struct ad8801_state *state = iio_priv(indio_dev);
+
+	iio_device_unregister(indio_dev);
+	if (state->vrefl_reg)
+		regulator_disable(state->vrefl_reg);
+	regulator_disable(state->vrefh_reg);
+
+	return 0;
+}
+
+static const struct spi_device_id ad8801_ids[] = {
+	{"ad8801", ID_AD8801},
+	{"ad8803", ID_AD8803},
+	{}
+};
+MODULE_DEVICE_TABLE(spi, ad8801_ids);
+
+static struct spi_driver ad8801_driver = {
+	.driver = {
+		.name	= "ad8801",
+	},
+	.probe		= ad8801_probe,
+	.remove		= ad8801_remove,
+	.id_table	= ad8801_ids,
+};
+module_spi_driver(ad8801_driver);
+
+MODULE_AUTHOR("Gwenhael Goavec-Merou <gwenhael.goavec-merou@trabucayre.com>");
+MODULE_DESCRIPTION("Analog Devices AD8801/AD8803 DAC");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/dac/cio-dac.c b/drivers/iio/dac/cio-dac.c
new file mode 100644
index 000000000000..5a743e2a779d
--- /dev/null
+++ b/drivers/iio/dac/cio-dac.c
@@ -0,0 +1,144 @@
+/*
+ * IIO driver for the Measurement Computing CIO-DAC
+ * Copyright (C) 2016 William Breathitt Gray
+ *
+ * This program is free software; you can 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.
+ *
+ * This driver supports the following Measurement Computing devices: CIO-DAC16,
+ * CIO-DAC06, and PC104-DAC06.
+ */
+#include <linux/bitops.h>
+#include <linux/device.h>
+#include <linux/errno.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/types.h>
+#include <linux/io.h>
+#include <linux/ioport.h>
+#include <linux/isa.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+
+#define CIO_DAC_NUM_CHAN 16
+
+#define CIO_DAC_CHAN(chan) {				\
+	.type = IIO_VOLTAGE,				\
+	.channel = chan,				\
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),	\
+	.indexed = 1,					\
+	.output = 1					\
+}
+
+#define CIO_DAC_EXTENT 32
+
+static unsigned int base[max_num_isa_dev(CIO_DAC_EXTENT)];
+static unsigned int num_cio_dac;
+module_param_array(base, uint, &num_cio_dac, 0);
+MODULE_PARM_DESC(base, "Measurement Computing CIO-DAC base addresses");
+
+/**
+ * struct cio_dac_iio - IIO device private data structure
+ * @chan_out_states:	channels' output states
+ * @base:		base port address of the IIO device
+ */
+struct cio_dac_iio {
+	int chan_out_states[CIO_DAC_NUM_CHAN];
+	unsigned int base;
+};
+
+static int cio_dac_read_raw(struct iio_dev *indio_dev,
+	struct iio_chan_spec const *chan, int *val, int *val2, long mask)
+{
+	struct cio_dac_iio *const priv = iio_priv(indio_dev);
+
+	if (mask != IIO_CHAN_INFO_RAW)
+		return -EINVAL;
+
+	*val = priv->chan_out_states[chan->channel];
+
+	return IIO_VAL_INT;
+}
+
+static int cio_dac_write_raw(struct iio_dev *indio_dev,
+	struct iio_chan_spec const *chan, int val, int val2, long mask)
+{
+	struct cio_dac_iio *const priv = iio_priv(indio_dev);
+	const unsigned int chan_addr_offset = 2 * chan->channel;
+
+	if (mask != IIO_CHAN_INFO_RAW)
+		return -EINVAL;
+
+	/* DAC can only accept up to a 16-bit value */
+	if ((unsigned int)val > 65535)
+		return -EINVAL;
+
+	priv->chan_out_states[chan->channel] = val;
+	outw(val, priv->base + chan_addr_offset);
+
+	return 0;
+}
+
+static const struct iio_info cio_dac_info = {
+	.driver_module = THIS_MODULE,
+	.read_raw = cio_dac_read_raw,
+	.write_raw = cio_dac_write_raw
+};
+
+static const struct iio_chan_spec cio_dac_channels[CIO_DAC_NUM_CHAN] = {
+	CIO_DAC_CHAN(0), CIO_DAC_CHAN(1), CIO_DAC_CHAN(2), CIO_DAC_CHAN(3),
+	CIO_DAC_CHAN(4), CIO_DAC_CHAN(5), CIO_DAC_CHAN(6), CIO_DAC_CHAN(7),
+	CIO_DAC_CHAN(8), CIO_DAC_CHAN(9), CIO_DAC_CHAN(10), CIO_DAC_CHAN(11),
+	CIO_DAC_CHAN(12), CIO_DAC_CHAN(13), CIO_DAC_CHAN(14), CIO_DAC_CHAN(15)
+};
+
+static int cio_dac_probe(struct device *dev, unsigned int id)
+{
+	struct iio_dev *indio_dev;
+	struct cio_dac_iio *priv;
+	unsigned int i;
+
+	indio_dev = devm_iio_device_alloc(dev, sizeof(*priv));
+	if (!indio_dev)
+		return -ENOMEM;
+
+	if (!devm_request_region(dev, base[id], CIO_DAC_EXTENT,
+		dev_name(dev))) {
+		dev_err(dev, "Unable to request port addresses (0x%X-0x%X)\n",
+			base[id], base[id] + CIO_DAC_EXTENT);
+		return -EBUSY;
+	}
+
+	indio_dev->info = &cio_dac_info;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->channels = cio_dac_channels;
+	indio_dev->num_channels = CIO_DAC_NUM_CHAN;
+	indio_dev->name = dev_name(dev);
+
+	priv = iio_priv(indio_dev);
+	priv->base = base[id];
+
+	/* initialize DAC outputs to 0V */
+	for (i = 0; i < 32; i += 2)
+		outw(0, base[id] + i);
+
+	return devm_iio_device_register(dev, indio_dev);
+}
+
+static struct isa_driver cio_dac_driver = {
+	.probe = cio_dac_probe,
+	.driver = {
+		.name = "cio-dac"
+	}
+};
+
+module_isa_driver(cio_dac_driver, num_cio_dac);
+
+MODULE_AUTHOR("William Breathitt Gray <vilhelm.gray@gmail.com>");
+MODULE_DESCRIPTION("Measurement Computing CIO-DAC IIO driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/gyro/ssp_gyro_sensor.c b/drivers/iio/gyro/ssp_gyro_sensor.c
index 0a8afdd21728..1f25f406c545 100644
--- a/drivers/iio/gyro/ssp_gyro_sensor.c
+++ b/drivers/iio/gyro/ssp_gyro_sensor.c
@@ -74,7 +74,7 @@ static int ssp_gyro_write_raw(struct iio_dev *indio_dev,
 	return -EINVAL;
 }
 
-static struct iio_info ssp_gyro_iio_info = {
+static const struct iio_info ssp_gyro_iio_info = {
 	.read_raw = &ssp_gyro_read_raw,
 	.write_raw = &ssp_gyro_write_raw,
 };
diff --git a/drivers/iio/humidity/Kconfig b/drivers/iio/humidity/Kconfig
index d04124345992..b17e2e2bd4f5 100644
--- a/drivers/iio/humidity/Kconfig
+++ b/drivers/iio/humidity/Kconfig
@@ -28,11 +28,11 @@ config HDC100X
 	tristate "TI HDC100x relative humidity and temperature sensor"
 	depends on I2C
 	help
-	 Say yes here to build support for the TI HDC100x series of
-	 relative humidity and temperature sensors.
+	  Say yes here to build support for the Texas Instruments
+	  HDC1000 and HDC1008 relative humidity and temperature sensors.
 
-	 To compile this driver as a module, choose M here: the module
-	 will be called hdc100x.
+	  To compile this driver as a module, choose M here: the module
+	  will be called hdc100x.
 
 config HTU21
 	tristate "Measurement Specialties HTU21 humidity & temperature sensor"
diff --git a/drivers/iio/industrialio-core.c b/drivers/iio/industrialio-core.c
index d2b889918c3e..fc340ed3dca1 100644
--- a/drivers/iio/industrialio-core.c
+++ b/drivers/iio/industrialio-core.c
@@ -1308,7 +1308,7 @@ static void devm_iio_device_release(struct device *dev, void *res)
 	iio_device_free(*(struct iio_dev **)res);
 }
 
-static int devm_iio_device_match(struct device *dev, void *res, void *data)
+int devm_iio_device_match(struct device *dev, void *res, void *data)
 {
 	struct iio_dev **r = res;
 	if (!r || !*r) {
@@ -1317,6 +1317,7 @@ static int devm_iio_device_match(struct device *dev, void *res, void *data)
 	}
 	return *r == data;
 }
+EXPORT_SYMBOL_GPL(devm_iio_device_match);
 
 /**
  * devm_iio_device_alloc - Resource-managed iio_device_alloc()
diff --git a/drivers/iio/industrialio-event.c b/drivers/iio/industrialio-event.c
index 0ebfc923a997..90fac8ec63c9 100644
--- a/drivers/iio/industrialio-event.c
+++ b/drivers/iio/industrialio-event.c
@@ -57,6 +57,11 @@ bool iio_event_enabled(const struct iio_event_interface *ev_int)
  *
  * Note: The caller must make sure that this function is not running
  * concurrently for the same indio_dev more than once.
+ *
+ * This function may be safely used as soon as a valid reference to iio_dev has
+ * been obtained via iio_device_alloc(), but any events that are submitted
+ * before iio_device_register() has successfully completed will be silently
+ * discarded.
  **/
 int iio_push_event(struct iio_dev *indio_dev, u64 ev_code, s64 timestamp)
 {
@@ -64,6 +69,9 @@ int iio_push_event(struct iio_dev *indio_dev, u64 ev_code, s64 timestamp)
 	struct iio_event_data ev;
 	int copied;
 
+	if (!ev_int)
+		return 0;
+
 	/* Does anyone care? */
 	if (iio_event_enabled(ev_int)) {
 
diff --git a/drivers/iio/industrialio-trigger.c b/drivers/iio/industrialio-trigger.c
index 7ad82fdd3e5b..e1e104845e38 100644
--- a/drivers/iio/industrialio-trigger.c
+++ b/drivers/iio/industrialio-trigger.c
@@ -119,6 +119,22 @@ void iio_trigger_unregister(struct iio_trigger *trig_info)
 }
 EXPORT_SYMBOL(iio_trigger_unregister);
 
+int iio_trigger_set_immutable(struct iio_dev *indio_dev, struct iio_trigger *trig)
+{
+	if (!indio_dev || !trig)
+		return -EINVAL;
+
+	mutex_lock(&indio_dev->mlock);
+	WARN_ON(indio_dev->trig_readonly);
+
+	indio_dev->trig = iio_trigger_get(trig);
+	indio_dev->trig_readonly = true;
+	mutex_unlock(&indio_dev->mlock);
+
+	return 0;
+}
+EXPORT_SYMBOL(iio_trigger_set_immutable);
+
 /* Search for trigger by name, assuming iio_trigger_list_lock held */
 static struct iio_trigger *__iio_trigger_find_by_name(const char *name)
 {
@@ -255,6 +271,14 @@ static int iio_trigger_attach_poll_func(struct iio_trigger *trig,
 			goto out_free_irq;
 	}
 
+	/*
+	 * Check if we just registered to our own trigger: we determine that
+	 * this is the case if the IIO device and the trigger device share the
+	 * same parent device.
+	 */
+	if (pf->indio_dev->dev.parent == trig->dev.parent)
+		trig->attached_own_device = true;
+
 	return ret;
 
 out_free_irq:
@@ -279,6 +303,8 @@ static int iio_trigger_detach_poll_func(struct iio_trigger *trig,
 		if (ret)
 			return ret;
 	}
+	if (pf->indio_dev->dev.parent == trig->dev.parent)
+		trig->attached_own_device = false;
 	iio_trigger_put_irq(trig, pf->irq);
 	free_irq(pf->irq, pf);
 	module_put(pf->indio_dev->info->driver_module);
@@ -384,6 +410,10 @@ static ssize_t iio_trigger_write_current(struct device *dev,
 		mutex_unlock(&indio_dev->mlock);
 		return -EBUSY;
 	}
+	if (indio_dev->trig_readonly) {
+		mutex_unlock(&indio_dev->mlock);
+		return -EPERM;
+	}
 	mutex_unlock(&indio_dev->mlock);
 
 	trig = iio_trigger_find_by_name(buf, len);
@@ -622,6 +652,71 @@ void devm_iio_trigger_free(struct device *dev, struct iio_trigger *iio_trig)
 }
 EXPORT_SYMBOL_GPL(devm_iio_trigger_free);
 
+static void devm_iio_trigger_unreg(struct device *dev, void *res)
+{
+	iio_trigger_unregister(*(struct iio_trigger **)res);
+}
+
+/**
+ * devm_iio_trigger_register - Resource-managed iio_trigger_register()
+ * @dev:	device this trigger was allocated for
+ * @trig_info:	trigger to register
+ *
+ * Managed iio_trigger_register().  The IIO trigger registered with this
+ * function is automatically unregistered on driver detach. This function
+ * calls iio_trigger_register() internally. Refer to that function for more
+ * information.
+ *
+ * If an iio_trigger registered with this function needs to be unregistered
+ * separately, devm_iio_trigger_unregister() must be used.
+ *
+ * RETURNS:
+ * 0 on success, negative error number on failure.
+ */
+int devm_iio_trigger_register(struct device *dev, struct iio_trigger *trig_info)
+{
+	struct iio_trigger **ptr;
+	int ret;
+
+	ptr = devres_alloc(devm_iio_trigger_unreg, sizeof(*ptr), GFP_KERNEL);
+	if (!ptr)
+		return -ENOMEM;
+
+	*ptr = trig_info;
+	ret = iio_trigger_register(trig_info);
+	if (!ret)
+		devres_add(dev, ptr);
+	else
+		devres_free(ptr);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(devm_iio_trigger_register);
+
+/**
+ * devm_iio_trigger_unregister - Resource-managed iio_trigger_unregister()
+ * @dev:	device this iio_trigger belongs to
+ * @trig_info:	the trigger associated with the device
+ *
+ * Unregister trigger registered with devm_iio_trigger_register().
+ */
+void devm_iio_trigger_unregister(struct device *dev,
+				 struct iio_trigger *trig_info)
+{
+	int rc;
+
+	rc = devres_release(dev, devm_iio_trigger_unreg, devm_iio_trigger_match,
+			    trig_info);
+	WARN_ON(rc);
+}
+EXPORT_SYMBOL_GPL(devm_iio_trigger_unregister);
+
+bool iio_trigger_using_own(struct iio_dev *indio_dev)
+{
+	return indio_dev->trig->attached_own_device;
+}
+EXPORT_SYMBOL(iio_trigger_using_own);
+
 void iio_device_register_trigger_consumer(struct iio_dev *indio_dev)
 {
 	indio_dev->groups[indio_dev->groupcounter++] =
diff --git a/drivers/iio/light/Kconfig b/drivers/iio/light/Kconfig
index 3574945183fe..ba2e64d7ee58 100644
--- a/drivers/iio/light/Kconfig
+++ b/drivers/iio/light/Kconfig
@@ -267,6 +267,19 @@ config PA12203001
          This driver can also be built as a module.  If so, the module
          will be called pa12203001.
 
+config SI1145
+	tristate "SI1132 and SI1141/2/3/5/6/7 combined ALS, UV index and proximity sensor"
+	depends on I2C
+	select IIO_BUFFER
+	select IIO_TRIGGERED_BUFFER
+	help
+	  Say Y here if you want to build a driver for the Silicon Labs SI1132 or
+	  SI1141/2/3/5/6/7 combined ambient light, UV index and proximity sensor
+	  chips.
+
+	  To compile this driver as a module, choose M here: the module will be
+	  called si1145.
+
 config STK3310
 	tristate "STK3310 ALS and proximity sensor"
 	depends on I2C
@@ -334,11 +347,11 @@ config US5182D
 	 will be called us5182d.
 
 config VCNL4000
-	tristate "VCNL4000 combined ALS and proximity sensor"
+	tristate "VCNL4000/4010/4020 combined ALS and proximity sensor"
 	depends on I2C
 	help
-	 Say Y here if you want to build a driver for the Vishay VCNL4000
-	 combined ambient light and proximity sensor.
+	 Say Y here if you want to build a driver for the Vishay VCNL4000,
+	 VCNL4010, VCNL4020 combined ambient light and proximity sensor.
 
 	 To compile this driver as a module, choose M here: the
 	 module will be called vcnl4000.
diff --git a/drivers/iio/light/Makefile b/drivers/iio/light/Makefile
index 6f2a3c62de27..c5768df87a17 100644
--- a/drivers/iio/light/Makefile
+++ b/drivers/iio/light/Makefile
@@ -26,6 +26,7 @@ obj-$(CONFIG_OPT3001)		+= opt3001.o
 obj-$(CONFIG_PA12203001)	+= pa12203001.o
 obj-$(CONFIG_RPR0521)		+= rpr0521.o
 obj-$(CONFIG_SENSORS_TSL2563)	+= tsl2563.o
+obj-$(CONFIG_SI1145)		+= si1145.o
 obj-$(CONFIG_STK3310)          += stk3310.o
 obj-$(CONFIG_TCS3414)		+= tcs3414.o
 obj-$(CONFIG_TCS3472)		+= tcs3472.o
diff --git a/drivers/iio/light/si1145.c b/drivers/iio/light/si1145.c
new file mode 100644
index 000000000000..096034c126a4
--- /dev/null
+++ b/drivers/iio/light/si1145.c
@@ -0,0 +1,1404 @@
+/*
+ * si1145.c - Support for Silabs SI1132 and SI1141/2/3/5/6/7 combined ambient
+ * light, UV index and proximity sensors
+ *
+ * Copyright 2014-16 Peter Meerwald-Stadler <pmeerw@pmeerw.net>
+ * Copyright 2016 Crestez Dan Leonard <leonard.crestez@intel.com>
+ *
+ * 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.
+ *
+ * SI1132 (7-bit I2C slave address 0x60)
+ * SI1141/2/3 (7-bit I2C slave address 0x5a)
+ * SI1145/6/6 (7-bit I2C slave address 0x60)
+ */
+
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/irq.h>
+#include <linux/gpio.h>
+
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/trigger.h>
+#include <linux/iio/trigger_consumer.h>
+#include <linux/iio/triggered_buffer.h>
+#include <linux/iio/buffer.h>
+#include <linux/util_macros.h>
+
+#define SI1145_REG_PART_ID		0x00
+#define SI1145_REG_REV_ID		0x01
+#define SI1145_REG_SEQ_ID		0x02
+#define SI1145_REG_INT_CFG		0x03
+#define SI1145_REG_IRQ_ENABLE		0x04
+#define SI1145_REG_IRQ_MODE		0x05
+#define SI1145_REG_HW_KEY		0x07
+#define SI1145_REG_MEAS_RATE		0x08
+#define SI1145_REG_PS_LED21		0x0f
+#define SI1145_REG_PS_LED3		0x10
+#define SI1145_REG_UCOEF1		0x13
+#define SI1145_REG_UCOEF2		0x14
+#define SI1145_REG_UCOEF3		0x15
+#define SI1145_REG_UCOEF4		0x16
+#define SI1145_REG_PARAM_WR		0x17
+#define SI1145_REG_COMMAND		0x18
+#define SI1145_REG_RESPONSE		0x20
+#define SI1145_REG_IRQ_STATUS		0x21
+#define SI1145_REG_ALSVIS_DATA		0x22
+#define SI1145_REG_ALSIR_DATA		0x24
+#define SI1145_REG_PS1_DATA		0x26
+#define SI1145_REG_PS2_DATA		0x28
+#define SI1145_REG_PS3_DATA		0x2a
+#define SI1145_REG_AUX_DATA		0x2c
+#define SI1145_REG_PARAM_RD		0x2e
+#define SI1145_REG_CHIP_STAT		0x30
+
+#define SI1145_UCOEF1_DEFAULT		0x7b
+#define SI1145_UCOEF2_DEFAULT		0x6b
+#define SI1145_UCOEF3_DEFAULT		0x01
+#define SI1145_UCOEF4_DEFAULT		0x00
+
+/* Helper to figure out PS_LED register / shift per channel */
+#define SI1145_PS_LED_REG(ch) \
+	(((ch) == 2) ? SI1145_REG_PS_LED3 : SI1145_REG_PS_LED21)
+#define SI1145_PS_LED_SHIFT(ch) \
+	(((ch) == 1) ? 4 : 0)
+
+/* Parameter offsets */
+#define SI1145_PARAM_CHLIST		0x01
+#define SI1145_PARAM_PSLED12_SELECT	0x02
+#define SI1145_PARAM_PSLED3_SELECT	0x03
+#define SI1145_PARAM_PS_ENCODING	0x05
+#define SI1145_PARAM_ALS_ENCODING	0x06
+#define SI1145_PARAM_PS1_ADC_MUX	0x07
+#define SI1145_PARAM_PS2_ADC_MUX	0x08
+#define SI1145_PARAM_PS3_ADC_MUX	0x09
+#define SI1145_PARAM_PS_ADC_COUNTER	0x0a
+#define SI1145_PARAM_PS_ADC_GAIN	0x0b
+#define SI1145_PARAM_PS_ADC_MISC	0x0c
+#define SI1145_PARAM_ALS_ADC_MUX	0x0d
+#define SI1145_PARAM_ALSIR_ADC_MUX	0x0e
+#define SI1145_PARAM_AUX_ADC_MUX	0x0f
+#define SI1145_PARAM_ALSVIS_ADC_COUNTER	0x10
+#define SI1145_PARAM_ALSVIS_ADC_GAIN	0x11
+#define SI1145_PARAM_ALSVIS_ADC_MISC	0x12
+#define SI1145_PARAM_LED_RECOVERY	0x1c
+#define SI1145_PARAM_ALSIR_ADC_COUNTER	0x1d
+#define SI1145_PARAM_ALSIR_ADC_GAIN	0x1e
+#define SI1145_PARAM_ALSIR_ADC_MISC	0x1f
+#define SI1145_PARAM_ADC_OFFSET		0x1a
+
+/* Channel enable masks for CHLIST parameter */
+#define SI1145_CHLIST_EN_PS1		BIT(0)
+#define SI1145_CHLIST_EN_PS2		BIT(1)
+#define SI1145_CHLIST_EN_PS3		BIT(2)
+#define SI1145_CHLIST_EN_ALSVIS		BIT(4)
+#define SI1145_CHLIST_EN_ALSIR		BIT(5)
+#define SI1145_CHLIST_EN_AUX		BIT(6)
+#define SI1145_CHLIST_EN_UV		BIT(7)
+
+/* Proximity measurement mode for ADC_MISC parameter */
+#define SI1145_PS_ADC_MODE_NORMAL	BIT(2)
+/* Signal range mask for ADC_MISC parameter */
+#define SI1145_ADC_MISC_RANGE		BIT(5)
+
+/* Commands for REG_COMMAND */
+#define SI1145_CMD_NOP			0x00
+#define SI1145_CMD_RESET		0x01
+#define SI1145_CMD_PS_FORCE		0x05
+#define SI1145_CMD_ALS_FORCE		0x06
+#define SI1145_CMD_PSALS_FORCE		0x07
+#define SI1145_CMD_PS_PAUSE		0x09
+#define SI1145_CMD_ALS_PAUSE		0x0a
+#define SI1145_CMD_PSALS_PAUSE		0x0b
+#define SI1145_CMD_PS_AUTO		0x0d
+#define SI1145_CMD_ALS_AUTO		0x0e
+#define SI1145_CMD_PSALS_AUTO		0x0f
+#define SI1145_CMD_PARAM_QUERY		0x80
+#define SI1145_CMD_PARAM_SET		0xa0
+
+#define SI1145_RSP_INVALID_SETTING	0x80
+#define SI1145_RSP_COUNTER_MASK		0x0F
+
+/* Minimum sleep after each command to ensure it's received */
+#define SI1145_COMMAND_MINSLEEP_MS	5
+/* Return -ETIMEDOUT after this long */
+#define SI1145_COMMAND_TIMEOUT_MS	25
+
+/* Interrupt configuration masks for INT_CFG register */
+#define SI1145_INT_CFG_OE		BIT(0) /* enable interrupt */
+#define SI1145_INT_CFG_MODE		BIT(1) /* auto reset interrupt pin */
+
+/* Interrupt enable masks for IRQ_ENABLE register */
+#define SI1145_MASK_ALL_IE		(BIT(4) | BIT(3) | BIT(2) | BIT(0))
+
+#define SI1145_MUX_TEMP			0x65
+#define SI1145_MUX_VDD			0x75
+
+/* Proximity LED current; see Table 2 in datasheet */
+#define SI1145_LED_CURRENT_45mA		0x04
+
+enum {
+	SI1132,
+	SI1141,
+	SI1142,
+	SI1143,
+	SI1145,
+	SI1146,
+	SI1147,
+};
+
+struct si1145_part_info {
+	u8 part;
+	const struct iio_info *iio_info;
+	const struct iio_chan_spec *channels;
+	unsigned int num_channels;
+	unsigned int num_leds;
+	bool uncompressed_meas_rate;
+};
+
+/**
+ * struct si1145_data - si1145 chip state data
+ * @client:	I2C client
+ * @lock:	mutex to protect shared state.
+ * @cmdlock:	Low-level mutex to protect command execution only
+ * @rsp_seq:	Next expected response number or -1 if counter reset required
+ * @scan_mask:	Saved scan mask to avoid duplicate set_chlist
+ * @autonomous: If automatic measurements are active (for buffer support)
+ * @part_info:	Part information
+ * @trig:	Pointer to iio trigger
+ * @meas_rate:	Value of MEAS_RATE register. Only set in HW in auto mode
+ */
+struct si1145_data {
+	struct i2c_client *client;
+	struct mutex lock;
+	struct mutex cmdlock;
+	int rsp_seq;
+	const struct si1145_part_info *part_info;
+	unsigned long scan_mask;
+	bool autonomous;
+	struct iio_trigger *trig;
+	int meas_rate;
+};
+
+/**
+ * __si1145_command_reset() - Send CMD_NOP and wait for response 0
+ *
+ * Does not modify data->rsp_seq
+ *
+ * Return: 0 on success and -errno on error.
+ */
+static int __si1145_command_reset(struct si1145_data *data)
+{
+	struct device *dev = &data->client->dev;
+	unsigned long stop_jiffies;
+	int ret;
+
+	ret = i2c_smbus_write_byte_data(data->client, SI1145_REG_COMMAND,
+						      SI1145_CMD_NOP);
+	if (ret < 0)
+		return ret;
+	msleep(SI1145_COMMAND_MINSLEEP_MS);
+
+	stop_jiffies = jiffies + SI1145_COMMAND_TIMEOUT_MS * HZ / 1000;
+	while (true) {
+		ret = i2c_smbus_read_byte_data(data->client,
+					       SI1145_REG_RESPONSE);
+		if (ret <= 0)
+			return ret;
+		if (time_after(jiffies, stop_jiffies)) {
+			dev_warn(dev, "timeout on reset\n");
+			return -ETIMEDOUT;
+		}
+		msleep(SI1145_COMMAND_MINSLEEP_MS);
+		continue;
+	}
+}
+
+/**
+ * si1145_command() - Execute a command and poll the response register
+ *
+ * All conversion overflows are reported as -EOVERFLOW
+ * INVALID_SETTING is reported as -EINVAL
+ * Timeouts are reported as -ETIMEDOUT
+ *
+ * Return: 0 on success or -errno on failure
+ */
+static int si1145_command(struct si1145_data *data, u8 cmd)
+{
+	struct device *dev = &data->client->dev;
+	unsigned long stop_jiffies;
+	int ret;
+
+	mutex_lock(&data->cmdlock);
+
+	if (data->rsp_seq < 0) {
+		ret = __si1145_command_reset(data);
+		if (ret < 0) {
+			dev_err(dev, "failed to reset command counter, ret=%d\n",
+				ret);
+			goto out;
+		}
+		data->rsp_seq = 0;
+	}
+
+	ret = i2c_smbus_write_byte_data(data->client, SI1145_REG_COMMAND, cmd);
+	if (ret) {
+		dev_warn(dev, "failed to write command, ret=%d\n", ret);
+		goto out;
+	}
+	/* Sleep a little to ensure the command is received */
+	msleep(SI1145_COMMAND_MINSLEEP_MS);
+
+	stop_jiffies = jiffies + SI1145_COMMAND_TIMEOUT_MS * HZ / 1000;
+	while (true) {
+		ret = i2c_smbus_read_byte_data(data->client,
+					       SI1145_REG_RESPONSE);
+		if (ret < 0) {
+			dev_warn(dev, "failed to read response, ret=%d\n", ret);
+			break;
+		}
+
+		if ((ret & ~SI1145_RSP_COUNTER_MASK) == 0) {
+			if (ret == data->rsp_seq) {
+				if (time_after(jiffies, stop_jiffies)) {
+					dev_warn(dev, "timeout on command %#02hhx\n",
+						 cmd);
+					ret = -ETIMEDOUT;
+					break;
+				}
+				msleep(SI1145_COMMAND_MINSLEEP_MS);
+				continue;
+			}
+			if (ret == ((data->rsp_seq + 1) &
+				SI1145_RSP_COUNTER_MASK)) {
+				data->rsp_seq = ret;
+				ret = 0;
+				break;
+			}
+			dev_warn(dev, "unexpected response counter %d instead of %d\n",
+				 ret, (data->rsp_seq + 1) &
+					SI1145_RSP_COUNTER_MASK);
+			ret = -EIO;
+		} else {
+			if (ret == SI1145_RSP_INVALID_SETTING) {
+				dev_warn(dev, "INVALID_SETTING error on command %#02hhx\n",
+					 cmd);
+				ret = -EINVAL;
+			} else {
+				/* All overflows are treated identically */
+				dev_dbg(dev, "overflow, ret=%d, cmd=%#02hhx\n",
+					ret, cmd);
+				ret = -EOVERFLOW;
+			}
+		}
+
+		/* Force a counter reset next time */
+		data->rsp_seq = -1;
+		break;
+	}
+
+out:
+	mutex_unlock(&data->cmdlock);
+
+	return ret;
+}
+
+static int si1145_param_update(struct si1145_data *data, u8 op, u8 param,
+			       u8 value)
+{
+	int ret;
+
+	ret = i2c_smbus_write_byte_data(data->client,
+		SI1145_REG_PARAM_WR, value);
+	if (ret < 0)
+		return ret;
+
+	return si1145_command(data, op | (param & 0x1F));
+}
+
+static int si1145_param_set(struct si1145_data *data, u8 param, u8 value)
+{
+	return si1145_param_update(data, SI1145_CMD_PARAM_SET, param, value);
+}
+
+/* Set param. Returns negative errno or current value */
+static int si1145_param_query(struct si1145_data *data, u8 param)
+{
+	int ret;
+
+	ret = si1145_command(data, SI1145_CMD_PARAM_QUERY | (param & 0x1F));
+	if (ret < 0)
+		return ret;
+
+	return i2c_smbus_read_byte_data(data->client, SI1145_REG_PARAM_RD);
+}
+
+/* Expand 8 bit compressed value to 16 bit, see Silabs AN498 */
+static u16 si1145_uncompress(u8 x)
+{
+	u16 result = 0;
+	u8 exponent = 0;
+
+	if (x < 8)
+		return 0;
+
+	exponent = (x & 0xf0) >> 4;
+	result = 0x10 | (x & 0x0f);
+
+	if (exponent >= 4)
+		return result << (exponent - 4);
+	return result >> (4 - exponent);
+}
+
+/* Compress 16 bit value to 8 bit, see Silabs AN498 */
+static u8 si1145_compress(u16 x)
+{
+	u32 exponent = 0;
+	u32 significand = 0;
+	u32 tmp = x;
+
+	if (x == 0x0000)
+		return 0x00;
+	if (x == 0x0001)
+		return 0x08;
+
+	while (1) {
+		tmp >>= 1;
+		exponent += 1;
+		if (tmp == 1)
+			break;
+	}
+
+	if (exponent < 5) {
+		significand = x << (4 - exponent);
+		return (exponent << 4) | (significand & 0xF);
+	}
+
+	significand = x >> (exponent - 5);
+	if (significand & 1) {
+		significand += 2;
+		if (significand & 0x0040) {
+			exponent += 1;
+			significand >>= 1;
+		}
+	}
+
+	return (exponent << 4) | ((significand >> 1) & 0xF);
+}
+
+/* Write meas_rate in hardware */
+static int si1145_set_meas_rate(struct si1145_data *data, int interval)
+{
+	if (data->part_info->uncompressed_meas_rate)
+		return i2c_smbus_write_word_data(data->client,
+			SI1145_REG_MEAS_RATE, interval);
+	else
+		return i2c_smbus_write_byte_data(data->client,
+			SI1145_REG_MEAS_RATE, interval);
+}
+
+static int si1145_read_samp_freq(struct si1145_data *data, int *val, int *val2)
+{
+	*val = 32000;
+	if (data->part_info->uncompressed_meas_rate)
+		*val2 = data->meas_rate;
+	else
+		*val2 = si1145_uncompress(data->meas_rate);
+	return IIO_VAL_FRACTIONAL;
+}
+
+/* Set the samp freq in driver private data */
+static int si1145_store_samp_freq(struct si1145_data *data, int val)
+{
+	int ret = 0;
+	int meas_rate;
+
+	if (val <= 0 || val > 32000)
+		return -ERANGE;
+	meas_rate = 32000 / val;
+
+	mutex_lock(&data->lock);
+	if (data->autonomous) {
+		ret = si1145_set_meas_rate(data, meas_rate);
+		if (ret)
+			goto out;
+	}
+	if (data->part_info->uncompressed_meas_rate)
+		data->meas_rate = meas_rate;
+	else
+		data->meas_rate = si1145_compress(meas_rate);
+
+out:
+	mutex_unlock(&data->lock);
+
+	return ret;
+}
+
+static irqreturn_t si1145_trigger_handler(int irq, void *private)
+{
+	struct iio_poll_func *pf = private;
+	struct iio_dev *indio_dev = pf->indio_dev;
+	struct si1145_data *data = iio_priv(indio_dev);
+	/*
+	 * Maximum buffer size:
+	 *   6*2 bytes channels data + 4 bytes alignment +
+	 *   8 bytes timestamp
+	 */
+	u8 buffer[24];
+	int i, j = 0;
+	int ret;
+	u8 irq_status = 0;
+
+	if (!data->autonomous) {
+		ret = si1145_command(data, SI1145_CMD_PSALS_FORCE);
+		if (ret < 0 && ret != -EOVERFLOW)
+			goto done;
+	} else {
+		irq_status = ret = i2c_smbus_read_byte_data(data->client,
+				SI1145_REG_IRQ_STATUS);
+		if (ret < 0)
+			goto done;
+		if (!(irq_status & SI1145_MASK_ALL_IE))
+			goto done;
+	}
+
+	for_each_set_bit(i, indio_dev->active_scan_mask,
+		indio_dev->masklength) {
+		int run = 1;
+
+		while (i + run < indio_dev->masklength) {
+			if (!test_bit(i + run, indio_dev->active_scan_mask))
+				break;
+			if (indio_dev->channels[i + run].address !=
+				indio_dev->channels[i].address + 2 * run)
+				break;
+			run++;
+		}
+
+		ret = i2c_smbus_read_i2c_block_data_or_emulated(
+				data->client, indio_dev->channels[i].address,
+				sizeof(u16) * run, &buffer[j]);
+		if (ret < 0)
+			goto done;
+		j += run * sizeof(u16);
+		i += run - 1;
+	}
+
+	if (data->autonomous) {
+		ret = i2c_smbus_write_byte_data(data->client,
+				SI1145_REG_IRQ_STATUS,
+				irq_status & SI1145_MASK_ALL_IE);
+		if (ret < 0)
+			goto done;
+	}
+
+	iio_push_to_buffers_with_timestamp(indio_dev, buffer,
+		iio_get_time_ns(indio_dev));
+
+done:
+	iio_trigger_notify_done(indio_dev->trig);
+	return IRQ_HANDLED;
+}
+
+static int si1145_set_chlist(struct iio_dev *indio_dev, unsigned long scan_mask)
+{
+	struct si1145_data *data = iio_priv(indio_dev);
+	u8 reg = 0, mux;
+	int ret;
+	int i;
+
+	/* channel list already set, no need to reprogram */
+	if (data->scan_mask == scan_mask)
+		return 0;
+
+	for_each_set_bit(i, &scan_mask, indio_dev->masklength) {
+		switch (indio_dev->channels[i].address) {
+		case SI1145_REG_ALSVIS_DATA:
+			reg |= SI1145_CHLIST_EN_ALSVIS;
+			break;
+		case SI1145_REG_ALSIR_DATA:
+			reg |= SI1145_CHLIST_EN_ALSIR;
+			break;
+		case SI1145_REG_PS1_DATA:
+			reg |= SI1145_CHLIST_EN_PS1;
+			break;
+		case SI1145_REG_PS2_DATA:
+			reg |= SI1145_CHLIST_EN_PS2;
+			break;
+		case SI1145_REG_PS3_DATA:
+			reg |= SI1145_CHLIST_EN_PS3;
+			break;
+		case SI1145_REG_AUX_DATA:
+			switch (indio_dev->channels[i].type) {
+			case IIO_UVINDEX:
+				reg |= SI1145_CHLIST_EN_UV;
+				break;
+			default:
+				reg |= SI1145_CHLIST_EN_AUX;
+				if (indio_dev->channels[i].type == IIO_TEMP)
+					mux = SI1145_MUX_TEMP;
+				else
+					mux = SI1145_MUX_VDD;
+				ret = si1145_param_set(data,
+					SI1145_PARAM_AUX_ADC_MUX, mux);
+				if (ret < 0)
+					return ret;
+
+				break;
+			}
+		}
+	}
+
+	data->scan_mask = scan_mask;
+	ret = si1145_param_set(data, SI1145_PARAM_CHLIST, reg);
+
+	return ret < 0 ? ret : 0;
+}
+
+static int si1145_measure(struct iio_dev *indio_dev,
+			  struct iio_chan_spec const *chan)
+{
+	struct si1145_data *data = iio_priv(indio_dev);
+	u8 cmd;
+	int ret;
+
+	ret = si1145_set_chlist(indio_dev, BIT(chan->scan_index));
+	if (ret < 0)
+		return ret;
+
+	cmd = (chan->type == IIO_PROXIMITY) ? SI1145_CMD_PS_FORCE :
+		SI1145_CMD_ALS_FORCE;
+	ret = si1145_command(data, cmd);
+	if (ret < 0 && ret != -EOVERFLOW)
+		return ret;
+
+	return i2c_smbus_read_word_data(data->client, chan->address);
+}
+
+/*
+ * Conversion between iio scale and ADC_GAIN values
+ * These could be further adjusted but proximity/intensity are dimensionless
+ */
+static const int si1145_proximity_scale_available[] = {
+	128, 64, 32, 16, 8, 4};
+static const int si1145_intensity_scale_available[] = {
+	128, 64, 32, 16, 8, 4, 2, 1};
+static IIO_CONST_ATTR(in_proximity_scale_available,
+	"128 64 32 16 8 4");
+static IIO_CONST_ATTR(in_intensity_scale_available,
+	"128 64 32 16 8 4 2 1");
+static IIO_CONST_ATTR(in_intensity_ir_scale_available,
+	"128 64 32 16 8 4 2 1");
+
+static int si1145_scale_from_adcgain(int regval)
+{
+	return 128 >> regval;
+}
+
+static int si1145_proximity_adcgain_from_scale(int val, int val2)
+{
+	val = find_closest_descending(val, si1145_proximity_scale_available,
+				ARRAY_SIZE(si1145_proximity_scale_available));
+	if (val < 0 || val > 5 || val2 != 0)
+		return -EINVAL;
+
+	return val;
+}
+
+static int si1145_intensity_adcgain_from_scale(int val, int val2)
+{
+	val = find_closest_descending(val, si1145_intensity_scale_available,
+				ARRAY_SIZE(si1145_intensity_scale_available));
+	if (val < 0 || val > 7 || val2 != 0)
+		return -EINVAL;
+
+	return val;
+}
+
+static int si1145_read_raw(struct iio_dev *indio_dev,
+				struct iio_chan_spec const *chan,
+				int *val, int *val2, long mask)
+{
+	struct si1145_data *data = iio_priv(indio_dev);
+	int ret;
+	u8 reg;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		switch (chan->type) {
+		case IIO_INTENSITY:
+		case IIO_PROXIMITY:
+		case IIO_VOLTAGE:
+		case IIO_TEMP:
+		case IIO_UVINDEX:
+			ret = iio_device_claim_direct_mode(indio_dev);
+			if (ret)
+				return ret;
+			ret = si1145_measure(indio_dev, chan);
+			iio_device_release_direct_mode(indio_dev);
+
+			if (ret < 0)
+				return ret;
+
+			*val = ret;
+
+			return IIO_VAL_INT;
+		case IIO_CURRENT:
+			ret = i2c_smbus_read_byte_data(data->client,
+				SI1145_PS_LED_REG(chan->channel));
+			if (ret < 0)
+				return ret;
+
+			*val = (ret >> SI1145_PS_LED_SHIFT(chan->channel))
+				& 0x0f;
+
+			return IIO_VAL_INT;
+		default:
+			return -EINVAL;
+		}
+	case IIO_CHAN_INFO_SCALE:
+		switch (chan->type) {
+		case IIO_PROXIMITY:
+			reg = SI1145_PARAM_PS_ADC_GAIN;
+			break;
+		case IIO_INTENSITY:
+			if (chan->channel2 == IIO_MOD_LIGHT_IR)
+				reg = SI1145_PARAM_ALSIR_ADC_GAIN;
+			else
+				reg = SI1145_PARAM_ALSVIS_ADC_GAIN;
+			break;
+		case IIO_TEMP:
+			*val = 28;
+			*val2 = 571429;
+			return IIO_VAL_INT_PLUS_MICRO;
+		case IIO_UVINDEX:
+			*val = 0;
+			*val2 = 10000;
+			return IIO_VAL_INT_PLUS_MICRO;
+		default:
+			return -EINVAL;
+		}
+
+		ret = si1145_param_query(data, reg);
+		if (ret < 0)
+			return ret;
+
+		*val = si1145_scale_from_adcgain(ret & 0x07);
+
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_OFFSET:
+		switch (chan->type) {
+		case IIO_TEMP:
+			/*
+			 * -ADC offset - ADC counts @ 25°C -
+			 *   35 * ADC counts / °C
+			 */
+			*val = -256 - 11136 + 25 * 35;
+			return IIO_VAL_INT;
+		default:
+			/*
+			 * All ADC measurements have are by default offset
+			 * by -256
+			 * See AN498 5.6.3
+			 */
+			ret = si1145_param_query(data, SI1145_PARAM_ADC_OFFSET);
+			if (ret < 0)
+				return ret;
+			*val = -si1145_uncompress(ret);
+			return IIO_VAL_INT;
+		}
+	case IIO_CHAN_INFO_SAMP_FREQ:
+		return si1145_read_samp_freq(data, val, val2);
+	default:
+		return -EINVAL;
+	}
+}
+
+static int si1145_write_raw(struct iio_dev *indio_dev,
+			       struct iio_chan_spec const *chan,
+			       int val, int val2, long mask)
+{
+	struct si1145_data *data = iio_priv(indio_dev);
+	u8 reg1, reg2, shift;
+	int ret;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_SCALE:
+		switch (chan->type) {
+		case IIO_PROXIMITY:
+			val = si1145_proximity_adcgain_from_scale(val, val2);
+			if (val < 0)
+				return val;
+			reg1 = SI1145_PARAM_PS_ADC_GAIN;
+			reg2 = SI1145_PARAM_PS_ADC_COUNTER;
+			break;
+		case IIO_INTENSITY:
+			val = si1145_intensity_adcgain_from_scale(val, val2);
+			if (val < 0)
+				return val;
+			if (chan->channel2 == IIO_MOD_LIGHT_IR) {
+				reg1 = SI1145_PARAM_ALSIR_ADC_GAIN;
+				reg2 = SI1145_PARAM_ALSIR_ADC_COUNTER;
+			} else {
+				reg1 = SI1145_PARAM_ALSVIS_ADC_GAIN;
+				reg2 = SI1145_PARAM_ALSVIS_ADC_COUNTER;
+			}
+			break;
+		default:
+			return -EINVAL;
+		}
+
+		ret = iio_device_claim_direct_mode(indio_dev);
+		if (ret)
+			return ret;
+
+		ret = si1145_param_set(data, reg1, val);
+		if (ret < 0) {
+			iio_device_release_direct_mode(indio_dev);
+			return ret;
+		}
+		/* Set recovery period to one's complement of gain */
+		ret = si1145_param_set(data, reg2, (~val & 0x07) << 4);
+		iio_device_release_direct_mode(indio_dev);
+		return ret;
+	case IIO_CHAN_INFO_RAW:
+		if (chan->type != IIO_CURRENT)
+			return -EINVAL;
+
+		if (val < 0 || val > 15 || val2 != 0)
+			return -EINVAL;
+
+		reg1 = SI1145_PS_LED_REG(chan->channel);
+		shift = SI1145_PS_LED_SHIFT(chan->channel);
+
+		ret = iio_device_claim_direct_mode(indio_dev);
+		if (ret)
+			return ret;
+
+		ret = i2c_smbus_read_byte_data(data->client, reg1);
+		if (ret < 0) {
+			iio_device_release_direct_mode(indio_dev);
+			return ret;
+		}
+		ret = i2c_smbus_write_byte_data(data->client, reg1,
+			(ret & ~(0x0f << shift)) |
+			((val & 0x0f) << shift));
+		iio_device_release_direct_mode(indio_dev);
+		return ret;
+	case IIO_CHAN_INFO_SAMP_FREQ:
+		return si1145_store_samp_freq(data, val);
+	default:
+		return -EINVAL;
+	}
+}
+
+#define SI1145_ST { \
+	.sign = 'u', \
+	.realbits = 16, \
+	.storagebits = 16, \
+	.endianness = IIO_LE, \
+}
+
+#define SI1145_INTENSITY_CHANNEL(_si) { \
+	.type = IIO_INTENSITY, \
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \
+			      BIT(IIO_CHAN_INFO_OFFSET) | \
+			      BIT(IIO_CHAN_INFO_SCALE), \
+	.info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ), \
+	.scan_type = SI1145_ST, \
+	.scan_index = _si, \
+	.address = SI1145_REG_ALSVIS_DATA, \
+}
+
+#define SI1145_INTENSITY_IR_CHANNEL(_si) { \
+	.type = IIO_INTENSITY, \
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \
+			      BIT(IIO_CHAN_INFO_OFFSET) | \
+			      BIT(IIO_CHAN_INFO_SCALE), \
+	.info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ), \
+	.modified = 1, \
+	.channel2 = IIO_MOD_LIGHT_IR, \
+	.scan_type = SI1145_ST, \
+	.scan_index = _si, \
+	.address = SI1145_REG_ALSIR_DATA, \
+}
+
+#define SI1145_TEMP_CHANNEL(_si) { \
+	.type = IIO_TEMP, \
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \
+			      BIT(IIO_CHAN_INFO_OFFSET) | \
+			      BIT(IIO_CHAN_INFO_SCALE), \
+	.info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ), \
+	.scan_type = SI1145_ST, \
+	.scan_index = _si, \
+	.address = SI1145_REG_AUX_DATA, \
+}
+
+#define SI1145_UV_CHANNEL(_si) { \
+	.type = IIO_UVINDEX, \
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \
+			      BIT(IIO_CHAN_INFO_SCALE), \
+	.info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ), \
+	.scan_type = SI1145_ST, \
+	.scan_index = _si, \
+	.address = SI1145_REG_AUX_DATA, \
+}
+
+#define SI1145_PROXIMITY_CHANNEL(_si, _ch) { \
+	.type = IIO_PROXIMITY, \
+	.indexed = 1, \
+	.channel = _ch, \
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW), \
+	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE) | \
+				    BIT(IIO_CHAN_INFO_OFFSET), \
+	.info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ), \
+	.scan_type = SI1145_ST, \
+	.scan_index = _si, \
+	.address = SI1145_REG_PS1_DATA + _ch * 2, \
+}
+
+#define SI1145_VOLTAGE_CHANNEL(_si) { \
+	.type = IIO_VOLTAGE, \
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW), \
+	.info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ), \
+	.scan_type = SI1145_ST, \
+	.scan_index = _si, \
+	.address = SI1145_REG_AUX_DATA, \
+}
+
+#define SI1145_CURRENT_CHANNEL(_ch) { \
+	.type = IIO_CURRENT, \
+	.indexed = 1, \
+	.channel = _ch, \
+	.output = 1, \
+	.scan_index = -1, \
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW), \
+}
+
+static const struct iio_chan_spec si1132_channels[] = {
+	SI1145_INTENSITY_CHANNEL(0),
+	SI1145_INTENSITY_IR_CHANNEL(1),
+	SI1145_TEMP_CHANNEL(2),
+	SI1145_VOLTAGE_CHANNEL(3),
+	SI1145_UV_CHANNEL(4),
+	IIO_CHAN_SOFT_TIMESTAMP(6),
+};
+
+static const struct iio_chan_spec si1141_channels[] = {
+	SI1145_INTENSITY_CHANNEL(0),
+	SI1145_INTENSITY_IR_CHANNEL(1),
+	SI1145_PROXIMITY_CHANNEL(2, 0),
+	SI1145_TEMP_CHANNEL(3),
+	SI1145_VOLTAGE_CHANNEL(4),
+	IIO_CHAN_SOFT_TIMESTAMP(5),
+	SI1145_CURRENT_CHANNEL(0),
+};
+
+static const struct iio_chan_spec si1142_channels[] = {
+	SI1145_INTENSITY_CHANNEL(0),
+	SI1145_INTENSITY_IR_CHANNEL(1),
+	SI1145_PROXIMITY_CHANNEL(2, 0),
+	SI1145_PROXIMITY_CHANNEL(3, 1),
+	SI1145_TEMP_CHANNEL(4),
+	SI1145_VOLTAGE_CHANNEL(5),
+	IIO_CHAN_SOFT_TIMESTAMP(6),
+	SI1145_CURRENT_CHANNEL(0),
+	SI1145_CURRENT_CHANNEL(1),
+};
+
+static const struct iio_chan_spec si1143_channels[] = {
+	SI1145_INTENSITY_CHANNEL(0),
+	SI1145_INTENSITY_IR_CHANNEL(1),
+	SI1145_PROXIMITY_CHANNEL(2, 0),
+	SI1145_PROXIMITY_CHANNEL(3, 1),
+	SI1145_PROXIMITY_CHANNEL(4, 2),
+	SI1145_TEMP_CHANNEL(5),
+	SI1145_VOLTAGE_CHANNEL(6),
+	IIO_CHAN_SOFT_TIMESTAMP(7),
+	SI1145_CURRENT_CHANNEL(0),
+	SI1145_CURRENT_CHANNEL(1),
+	SI1145_CURRENT_CHANNEL(2),
+};
+
+static const struct iio_chan_spec si1145_channels[] = {
+	SI1145_INTENSITY_CHANNEL(0),
+	SI1145_INTENSITY_IR_CHANNEL(1),
+	SI1145_PROXIMITY_CHANNEL(2, 0),
+	SI1145_TEMP_CHANNEL(3),
+	SI1145_VOLTAGE_CHANNEL(4),
+	SI1145_UV_CHANNEL(5),
+	IIO_CHAN_SOFT_TIMESTAMP(6),
+	SI1145_CURRENT_CHANNEL(0),
+};
+
+static const struct iio_chan_spec si1146_channels[] = {
+	SI1145_INTENSITY_CHANNEL(0),
+	SI1145_INTENSITY_IR_CHANNEL(1),
+	SI1145_TEMP_CHANNEL(2),
+	SI1145_VOLTAGE_CHANNEL(3),
+	SI1145_UV_CHANNEL(4),
+	SI1145_PROXIMITY_CHANNEL(5, 0),
+	SI1145_PROXIMITY_CHANNEL(6, 1),
+	IIO_CHAN_SOFT_TIMESTAMP(7),
+	SI1145_CURRENT_CHANNEL(0),
+	SI1145_CURRENT_CHANNEL(1),
+};
+
+static const struct iio_chan_spec si1147_channels[] = {
+	SI1145_INTENSITY_CHANNEL(0),
+	SI1145_INTENSITY_IR_CHANNEL(1),
+	SI1145_PROXIMITY_CHANNEL(2, 0),
+	SI1145_PROXIMITY_CHANNEL(3, 1),
+	SI1145_PROXIMITY_CHANNEL(4, 2),
+	SI1145_TEMP_CHANNEL(5),
+	SI1145_VOLTAGE_CHANNEL(6),
+	SI1145_UV_CHANNEL(7),
+	IIO_CHAN_SOFT_TIMESTAMP(8),
+	SI1145_CURRENT_CHANNEL(0),
+	SI1145_CURRENT_CHANNEL(1),
+	SI1145_CURRENT_CHANNEL(2),
+};
+
+static struct attribute *si1132_attributes[] = {
+	&iio_const_attr_in_intensity_scale_available.dev_attr.attr,
+	&iio_const_attr_in_intensity_ir_scale_available.dev_attr.attr,
+	NULL,
+};
+
+static struct attribute *si114x_attributes[] = {
+	&iio_const_attr_in_intensity_scale_available.dev_attr.attr,
+	&iio_const_attr_in_intensity_ir_scale_available.dev_attr.attr,
+	&iio_const_attr_in_proximity_scale_available.dev_attr.attr,
+	NULL,
+};
+
+static const struct attribute_group si1132_attribute_group = {
+	.attrs = si1132_attributes,
+};
+
+static const struct attribute_group si114x_attribute_group = {
+	.attrs = si114x_attributes,
+};
+
+
+static const struct iio_info si1132_info = {
+	.read_raw = si1145_read_raw,
+	.write_raw = si1145_write_raw,
+	.driver_module = THIS_MODULE,
+	.attrs = &si1132_attribute_group,
+};
+
+static const struct iio_info si114x_info = {
+	.read_raw = si1145_read_raw,
+	.write_raw = si1145_write_raw,
+	.driver_module = THIS_MODULE,
+	.attrs = &si114x_attribute_group,
+};
+
+#define SI1145_PART(id, iio_info, chans, leds, uncompressed_meas_rate) \
+	{id, iio_info, chans, ARRAY_SIZE(chans), leds, uncompressed_meas_rate}
+
+static const struct si1145_part_info si1145_part_info[] = {
+	[SI1132] = SI1145_PART(0x32, &si1132_info, si1132_channels, 0, true),
+	[SI1141] = SI1145_PART(0x41, &si114x_info, si1141_channels, 1, false),
+	[SI1142] = SI1145_PART(0x42, &si114x_info, si1142_channels, 2, false),
+	[SI1143] = SI1145_PART(0x43, &si114x_info, si1143_channels, 3, false),
+	[SI1145] = SI1145_PART(0x45, &si114x_info, si1145_channels, 1, true),
+	[SI1146] = SI1145_PART(0x46, &si114x_info, si1146_channels, 2, true),
+	[SI1147] = SI1145_PART(0x47, &si114x_info, si1147_channels, 3, true),
+};
+
+static int si1145_initialize(struct si1145_data *data)
+{
+	struct i2c_client *client = data->client;
+	int ret;
+
+	ret = i2c_smbus_write_byte_data(client, SI1145_REG_COMMAND,
+					SI1145_CMD_RESET);
+	if (ret < 0)
+		return ret;
+	msleep(SI1145_COMMAND_TIMEOUT_MS);
+
+	/* Hardware key, magic value */
+	ret = i2c_smbus_write_byte_data(client, SI1145_REG_HW_KEY, 0x17);
+	if (ret < 0)
+		return ret;
+	msleep(SI1145_COMMAND_TIMEOUT_MS);
+
+	/* Turn off autonomous mode */
+	ret = si1145_set_meas_rate(data, 0);
+	if (ret < 0)
+		return ret;
+
+	/* Initialize sampling freq to 10 Hz */
+	ret = si1145_store_samp_freq(data, 10);
+	if (ret < 0)
+		return ret;
+
+	/* Set LED currents to 45 mA; have 4 bits, see Table 2 in datasheet */
+	switch (data->part_info->num_leds) {
+	case 3:
+		ret = i2c_smbus_write_byte_data(client,
+						SI1145_REG_PS_LED3,
+						SI1145_LED_CURRENT_45mA);
+		if (ret < 0)
+			return ret;
+		/* fallthrough */
+	case 2:
+		ret = i2c_smbus_write_byte_data(client,
+						SI1145_REG_PS_LED21,
+						(SI1145_LED_CURRENT_45mA << 4) |
+						SI1145_LED_CURRENT_45mA);
+		break;
+	case 1:
+		ret = i2c_smbus_write_byte_data(client,
+						SI1145_REG_PS_LED21,
+						SI1145_LED_CURRENT_45mA);
+		break;
+	default:
+		ret = 0;
+		break;
+	}
+	if (ret < 0)
+		return ret;
+
+	/* Set normal proximity measurement mode */
+	ret = si1145_param_set(data, SI1145_PARAM_PS_ADC_MISC,
+			       SI1145_PS_ADC_MODE_NORMAL);
+	if (ret < 0)
+		return ret;
+
+	ret = si1145_param_set(data, SI1145_PARAM_PS_ADC_GAIN, 0x01);
+	if (ret < 0)
+		return ret;
+
+	/* ADC_COUNTER should be one complement of ADC_GAIN */
+	ret = si1145_param_set(data, SI1145_PARAM_PS_ADC_COUNTER, 0x06 << 4);
+	if (ret < 0)
+		return ret;
+
+	/* Set ALS visible measurement mode */
+	ret = si1145_param_set(data, SI1145_PARAM_ALSVIS_ADC_MISC,
+			       SI1145_ADC_MISC_RANGE);
+	if (ret < 0)
+		return ret;
+
+	ret = si1145_param_set(data, SI1145_PARAM_ALSVIS_ADC_GAIN, 0x03);
+	if (ret < 0)
+		return ret;
+
+	ret = si1145_param_set(data, SI1145_PARAM_ALSVIS_ADC_COUNTER,
+			       0x04 << 4);
+	if (ret < 0)
+		return ret;
+
+	/* Set ALS IR measurement mode */
+	ret = si1145_param_set(data, SI1145_PARAM_ALSIR_ADC_MISC,
+			       SI1145_ADC_MISC_RANGE);
+	if (ret < 0)
+		return ret;
+
+	ret = si1145_param_set(data, SI1145_PARAM_ALSIR_ADC_GAIN, 0x01);
+	if (ret < 0)
+		return ret;
+
+	ret = si1145_param_set(data, SI1145_PARAM_ALSIR_ADC_COUNTER,
+			       0x06 << 4);
+	if (ret < 0)
+		return ret;
+
+	/*
+	 * Initialize UCOEF to default values in datasheet
+	 * These registers are normally zero on reset
+	 */
+	if (data->part_info == &si1145_part_info[SI1132] ||
+		data->part_info == &si1145_part_info[SI1145] ||
+		data->part_info == &si1145_part_info[SI1146] ||
+		data->part_info == &si1145_part_info[SI1147]) {
+		ret = i2c_smbus_write_byte_data(data->client,
+						SI1145_REG_UCOEF1,
+						SI1145_UCOEF1_DEFAULT);
+		if (ret < 0)
+			return ret;
+		ret = i2c_smbus_write_byte_data(data->client,
+				SI1145_REG_UCOEF2, SI1145_UCOEF2_DEFAULT);
+		if (ret < 0)
+			return ret;
+		ret = i2c_smbus_write_byte_data(data->client,
+				SI1145_REG_UCOEF3, SI1145_UCOEF3_DEFAULT);
+		if (ret < 0)
+			return ret;
+		ret = i2c_smbus_write_byte_data(data->client,
+				SI1145_REG_UCOEF4, SI1145_UCOEF4_DEFAULT);
+		if (ret < 0)
+			return ret;
+	}
+
+	return 0;
+}
+
+/*
+ * Program the channels we want to measure with CMD_PSALS_AUTO. No need for
+ * _postdisable as we stop with CMD_PSALS_PAUSE; single measurement (direct)
+ * mode reprograms the channels list anyway...
+ */
+static int si1145_buffer_preenable(struct iio_dev *indio_dev)
+{
+	struct si1145_data *data = iio_priv(indio_dev);
+	int ret;
+
+	mutex_lock(&data->lock);
+	ret = si1145_set_chlist(indio_dev, *indio_dev->active_scan_mask);
+	mutex_unlock(&data->lock);
+
+	return ret;
+}
+
+static bool si1145_validate_scan_mask(struct iio_dev *indio_dev,
+			       const unsigned long *scan_mask)
+{
+	struct si1145_data *data = iio_priv(indio_dev);
+	unsigned int count = 0;
+	int i;
+
+	/* Check that at most one AUX channel is enabled */
+	for_each_set_bit(i, scan_mask, data->part_info->num_channels) {
+		if (indio_dev->channels[i].address == SI1145_REG_AUX_DATA)
+			count++;
+	}
+
+	return count <= 1;
+}
+
+static const struct iio_buffer_setup_ops si1145_buffer_setup_ops = {
+	.preenable = si1145_buffer_preenable,
+	.postenable = iio_triggered_buffer_postenable,
+	.predisable = iio_triggered_buffer_predisable,
+	.validate_scan_mask = si1145_validate_scan_mask,
+};
+
+/**
+ * si1145_trigger_set_state() - Set trigger state
+ *
+ * When not using triggers interrupts are disabled and measurement rate is
+ * set to zero in order to minimize power consumption.
+ */
+static int si1145_trigger_set_state(struct iio_trigger *trig, bool state)
+{
+	struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig);
+	struct si1145_data *data = iio_priv(indio_dev);
+	int err = 0, ret;
+
+	mutex_lock(&data->lock);
+
+	if (state) {
+		data->autonomous = true;
+		err = i2c_smbus_write_byte_data(data->client,
+				SI1145_REG_INT_CFG, SI1145_INT_CFG_OE);
+		if (err < 0)
+			goto disable;
+		err = i2c_smbus_write_byte_data(data->client,
+				SI1145_REG_IRQ_ENABLE, SI1145_MASK_ALL_IE);
+		if (err < 0)
+			goto disable;
+		err = si1145_set_meas_rate(data, data->meas_rate);
+		if (err < 0)
+			goto disable;
+		err = si1145_command(data, SI1145_CMD_PSALS_AUTO);
+		if (err < 0)
+			goto disable;
+	} else {
+disable:
+		/* Disable as much as possible skipping errors */
+		ret = si1145_command(data, SI1145_CMD_PSALS_PAUSE);
+		if (ret < 0 && !err)
+			err = ret;
+		ret = si1145_set_meas_rate(data, 0);
+		if (ret < 0 && !err)
+			err = ret;
+		ret = i2c_smbus_write_byte_data(data->client,
+						SI1145_REG_IRQ_ENABLE, 0);
+		if (ret < 0 && !err)
+			err = ret;
+		ret = i2c_smbus_write_byte_data(data->client,
+						SI1145_REG_INT_CFG, 0);
+		if (ret < 0 && !err)
+			err = ret;
+		data->autonomous = false;
+	}
+
+	mutex_unlock(&data->lock);
+	return err;
+}
+
+static const struct iio_trigger_ops si1145_trigger_ops = {
+	.owner = THIS_MODULE,
+	.set_trigger_state = si1145_trigger_set_state,
+};
+
+static int si1145_probe_trigger(struct iio_dev *indio_dev)
+{
+	struct si1145_data *data = iio_priv(indio_dev);
+	struct i2c_client *client = data->client;
+	struct iio_trigger *trig;
+	int ret;
+
+	trig = devm_iio_trigger_alloc(&client->dev,
+			"%s-dev%d", indio_dev->name, indio_dev->id);
+	if (!trig)
+		return -ENOMEM;
+
+	trig->dev.parent = &client->dev;
+	trig->ops = &si1145_trigger_ops;
+	iio_trigger_set_drvdata(trig, indio_dev);
+
+	ret = devm_request_irq(&client->dev, client->irq,
+			  iio_trigger_generic_data_rdy_poll,
+			  IRQF_TRIGGER_FALLING,
+			  "si1145_irq",
+			  trig);
+	if (ret < 0) {
+		dev_err(&client->dev, "irq request failed\n");
+		return ret;
+	}
+
+	ret = iio_trigger_register(trig);
+	if (ret)
+		return ret;
+
+	data->trig = trig;
+	indio_dev->trig = iio_trigger_get(data->trig);
+
+	return 0;
+}
+
+static void si1145_remove_trigger(struct iio_dev *indio_dev)
+{
+	struct si1145_data *data = iio_priv(indio_dev);
+
+	if (data->trig) {
+		iio_trigger_unregister(data->trig);
+		data->trig = NULL;
+	}
+}
+
+static int si1145_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	struct si1145_data *data;
+	struct iio_dev *indio_dev;
+	u8 part_id, rev_id, seq_id;
+	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;
+	data->part_info = &si1145_part_info[id->driver_data];
+
+	part_id = ret = i2c_smbus_read_byte_data(data->client,
+						 SI1145_REG_PART_ID);
+	if (ret < 0)
+		return ret;
+	rev_id = ret = i2c_smbus_read_byte_data(data->client,
+						SI1145_REG_REV_ID);
+	if (ret < 0)
+		return ret;
+	seq_id = ret = i2c_smbus_read_byte_data(data->client,
+						SI1145_REG_SEQ_ID);
+	if (ret < 0)
+		return ret;
+	dev_info(&client->dev, "device ID part %#02hhx rev %#02hhx seq %#02hhx\n",
+			part_id, rev_id, seq_id);
+	if (part_id != data->part_info->part) {
+		dev_err(&client->dev, "part ID mismatch got %#02hhx, expected %#02x\n",
+				part_id, data->part_info->part);
+		return -ENODEV;
+	}
+
+	indio_dev->dev.parent = &client->dev;
+	indio_dev->name = id->name;
+	indio_dev->channels = data->part_info->channels;
+	indio_dev->num_channels = data->part_info->num_channels;
+	indio_dev->info = data->part_info->iio_info;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+
+	mutex_init(&data->lock);
+	mutex_init(&data->cmdlock);
+
+	ret = si1145_initialize(data);
+	if (ret < 0)
+		return ret;
+
+	ret = iio_triggered_buffer_setup(indio_dev, NULL,
+		si1145_trigger_handler, &si1145_buffer_setup_ops);
+	if (ret < 0)
+		return ret;
+
+	if (client->irq) {
+		ret = si1145_probe_trigger(indio_dev);
+		if (ret < 0)
+			goto error_free_buffer;
+	} else {
+		dev_info(&client->dev, "no irq, using polling\n");
+	}
+
+	ret = iio_device_register(indio_dev);
+	if (ret < 0)
+		goto error_free_trigger;
+
+	return 0;
+
+error_free_trigger:
+	si1145_remove_trigger(indio_dev);
+error_free_buffer:
+	iio_triggered_buffer_cleanup(indio_dev);
+
+	return ret;
+}
+
+static const struct i2c_device_id si1145_ids[] = {
+	{ "si1132", SI1132 },
+	{ "si1141", SI1141 },
+	{ "si1142", SI1142 },
+	{ "si1143", SI1143 },
+	{ "si1145", SI1145 },
+	{ "si1146", SI1146 },
+	{ "si1147", SI1147 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, si1145_ids);
+
+static int si1145_remove(struct i2c_client *client)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(client);
+
+	iio_device_unregister(indio_dev);
+	si1145_remove_trigger(indio_dev);
+	iio_triggered_buffer_cleanup(indio_dev);
+
+	return 0;
+}
+
+static struct i2c_driver si1145_driver = {
+	.driver = {
+		.name   = "si1145",
+	},
+	.probe  = si1145_probe,
+	.remove = si1145_remove,
+	.id_table = si1145_ids,
+};
+
+module_i2c_driver(si1145_driver);
+
+MODULE_AUTHOR("Peter Meerwald-Stadler <pmeerw@pmeerw.net>");
+MODULE_DESCRIPTION("Silabs SI1132 and SI1141/2/3/5/6/7 proximity, ambient light and UV index sensor driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/iio/light/us5182d.c b/drivers/iio/light/us5182d.c
index 20c40f780964..18cf2e29e4d5 100644
--- a/drivers/iio/light/us5182d.c
+++ b/drivers/iio/light/us5182d.c
@@ -894,7 +894,7 @@ static int us5182d_probe(struct i2c_client *client,
 		goto out_err;
 
 	if (data->default_continuous) {
-		pm_runtime_set_active(&client->dev);
+		ret = pm_runtime_set_active(&client->dev);
 		if (ret < 0)
 			goto out_err;
 	}
diff --git a/drivers/iio/light/vcnl4000.c b/drivers/iio/light/vcnl4000.c
index c9d85bbc9230..360b6e98137a 100644
--- a/drivers/iio/light/vcnl4000.c
+++ b/drivers/iio/light/vcnl4000.c
@@ -1,6 +1,6 @@
 /*
- * vcnl4000.c - Support for Vishay VCNL4000 combined ambient light and
- * proximity sensor
+ * vcnl4000.c - Support for Vishay VCNL4000/4010/4020 combined ambient
+ * light and proximity sensor
  *
  * Copyright 2012 Peter Meerwald <pmeerw@pmeerw.net>
  *
@@ -13,6 +13,8 @@
  * TODO:
  *   allow to adjust IR current
  *   proximity threshold and event handling
+ *   periodic ALS/proximity measurement (VCNL4010/20)
+ *   interrupts (VCNL4010/20)
  */
 
 #include <linux/module.h>
@@ -24,6 +26,8 @@
 #include <linux/iio/sysfs.h>
 
 #define VCNL4000_DRV_NAME "vcnl4000"
+#define VCNL4000_ID		0x01
+#define VCNL4010_ID		0x02 /* for VCNL4020, VCNL4010 */
 
 #define VCNL4000_COMMAND	0x80 /* Command register */
 #define VCNL4000_PROD_REV	0x81 /* Product ID and Revision ID */
@@ -37,13 +41,14 @@
 #define VCNL4000_PS_MOD_ADJ	0x8a /* Proximity modulator timing adjustment */
 
 /* Bit masks for COMMAND register */
-#define VCNL4000_AL_RDY		0x40 /* ALS data ready? */
-#define VCNL4000_PS_RDY		0x20 /* proximity data ready? */
-#define VCNL4000_AL_OD		0x10 /* start on-demand ALS measurement */
-#define VCNL4000_PS_OD		0x08 /* start on-demand proximity measurement */
+#define VCNL4000_AL_RDY		BIT(6) /* ALS data ready? */
+#define VCNL4000_PS_RDY		BIT(5) /* proximity data ready? */
+#define VCNL4000_AL_OD		BIT(4) /* start on-demand ALS measurement */
+#define VCNL4000_PS_OD		BIT(3) /* start on-demand proximity measurement */
 
 struct vcnl4000_data {
 	struct i2c_client *client;
+	struct mutex lock;
 };
 
 static const struct i2c_device_id vcnl4000_id[] = {
@@ -59,16 +64,18 @@ static int vcnl4000_measure(struct vcnl4000_data *data, u8 req_mask,
 	__be16 buf;
 	int ret;
 
+	mutex_lock(&data->lock);
+
 	ret = i2c_smbus_write_byte_data(data->client, VCNL4000_COMMAND,
 					req_mask);
 	if (ret < 0)
-		return ret;
+		goto fail;
 
 	/* wait for data to become ready */
 	while (tries--) {
 		ret = i2c_smbus_read_byte_data(data->client, VCNL4000_COMMAND);
 		if (ret < 0)
-			return ret;
+			goto fail;
 		if (ret & rdy_mask)
 			break;
 		msleep(20); /* measurement takes up to 100 ms */
@@ -77,17 +84,23 @@ static int vcnl4000_measure(struct vcnl4000_data *data, u8 req_mask,
 	if (tries < 0) {
 		dev_err(&data->client->dev,
 			"vcnl4000_measure() failed, data not ready\n");
-		return -EIO;
+		ret = -EIO;
+		goto fail;
 	}
 
 	ret = i2c_smbus_read_i2c_block_data(data->client,
 		data_reg, sizeof(buf), (u8 *) &buf);
 	if (ret < 0)
-		return ret;
+		goto fail;
 
+	mutex_unlock(&data->lock);
 	*val = be16_to_cpu(buf);
 
 	return 0;
+
+fail:
+	mutex_unlock(&data->lock);
+	return ret;
 }
 
 static const struct iio_chan_spec vcnl4000_channels[] = {
@@ -105,7 +118,7 @@ static int vcnl4000_read_raw(struct iio_dev *indio_dev,
 				struct iio_chan_spec const *chan,
 				int *val, int *val2, long mask)
 {
-	int ret = -EINVAL;
+	int ret;
 	struct vcnl4000_data *data = iio_priv(indio_dev);
 
 	switch (mask) {
@@ -117,32 +130,27 @@ static int vcnl4000_read_raw(struct iio_dev *indio_dev,
 				VCNL4000_AL_RESULT_HI, val);
 			if (ret < 0)
 				return ret;
-			ret = IIO_VAL_INT;
-			break;
+			return IIO_VAL_INT;
 		case IIO_PROXIMITY:
 			ret = vcnl4000_measure(data,
 				VCNL4000_PS_OD, VCNL4000_PS_RDY,
 				VCNL4000_PS_RESULT_HI, val);
 			if (ret < 0)
 				return ret;
-			ret = IIO_VAL_INT;
-			break;
+			return IIO_VAL_INT;
 		default:
-			break;
+			return -EINVAL;
 		}
-		break;
 	case IIO_CHAN_INFO_SCALE:
-		if (chan->type == IIO_LIGHT) {
-			*val = 0;
-			*val2 = 250000;
-			ret = IIO_VAL_INT_PLUS_MICRO;
-		}
-		break;
+		if (chan->type != IIO_LIGHT)
+			return -EINVAL;
+
+		*val = 0;
+		*val2 = 250000;
+		return IIO_VAL_INT_PLUS_MICRO;
 	default:
-		break;
+		return -EINVAL;
 	}
-
-	return ret;
 }
 
 static const struct iio_info vcnl4000_info = {
@@ -155,7 +163,7 @@ static int vcnl4000_probe(struct i2c_client *client,
 {
 	struct vcnl4000_data *data;
 	struct iio_dev *indio_dev;
-	int ret;
+	int ret, prod_id;
 
 	indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*data));
 	if (!indio_dev)
@@ -164,13 +172,19 @@ static int vcnl4000_probe(struct i2c_client *client,
 	data = iio_priv(indio_dev);
 	i2c_set_clientdata(client, indio_dev);
 	data->client = client;
+	mutex_init(&data->lock);
 
 	ret = i2c_smbus_read_byte_data(data->client, VCNL4000_PROD_REV);
 	if (ret < 0)
 		return ret;
 
-	dev_info(&client->dev, "VCNL4000 Ambient light/proximity sensor, Prod %02x, Rev: %02x\n",
-		ret >> 4, ret & 0xf);
+	prod_id = ret >> 4;
+	if (prod_id != VCNL4010_ID && prod_id != VCNL4000_ID)
+		return -ENODEV;
+
+	dev_dbg(&client->dev, "%s Ambient light/proximity sensor, Rev: %02x\n",
+		(prod_id == VCNL4010_ID) ? "VCNL4010/4020" : "VCNL4000",
+		ret & 0xf);
 
 	indio_dev->dev.parent = &client->dev;
 	indio_dev->info = &vcnl4000_info;
diff --git a/drivers/iio/magnetometer/Kconfig b/drivers/iio/magnetometer/Kconfig
index 1f842abcb4a4..421ad90a5fbe 100644
--- a/drivers/iio/magnetometer/Kconfig
+++ b/drivers/iio/magnetometer/Kconfig
@@ -5,8 +5,22 @@
 
 menu "Magnetometer sensors"
 
+config AK8974
+	tristate "Asahi Kasei AK8974 3-Axis Magnetometer"
+	depends on I2C
+	depends on OF
+	select REGMAP_I2C
+	select IIO_BUFFER
+	select IIO_TRIGGERED_BUFFER
+	help
+	  Say yes here to build support for Asahi Kasei AK8974 or
+	  AMI305 I2C-based 3-axis magnetometer chips.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called ak8974.
+
 config AK8975
-	tristate "Asahi Kasei AK 3-Axis Magnetometer"
+	tristate "Asahi Kasei AK8975 3-Axis Magnetometer"
 	depends on I2C
 	depends on GPIOLIB || COMPILE_TEST
 	select IIO_BUFFER
diff --git a/drivers/iio/magnetometer/Makefile b/drivers/iio/magnetometer/Makefile
index 92a745c9a6e8..b86d6cb7f285 100644
--- a/drivers/iio/magnetometer/Makefile
+++ b/drivers/iio/magnetometer/Makefile
@@ -3,6 +3,7 @@
 #
 
 # When adding new entries keep the list in alphabetical order
+obj-$(CONFIG_AK8974)	+= ak8974.o
 obj-$(CONFIG_AK8975)	+= ak8975.o
 obj-$(CONFIG_BMC150_MAGN) += bmc150_magn.o
 obj-$(CONFIG_BMC150_MAGN_I2C) += bmc150_magn_i2c.o
diff --git a/drivers/iio/magnetometer/ak8974.c b/drivers/iio/magnetometer/ak8974.c
new file mode 100644
index 000000000000..217353145676
--- /dev/null
+++ b/drivers/iio/magnetometer/ak8974.c
@@ -0,0 +1,860 @@
+/*
+ * Driver for the Asahi Kasei EMD Corporation AK8974
+ * and Aichi Steel AMI305 magnetometer chips.
+ * Based on a patch from Samu Onkalo and the AK8975 IIO driver.
+ *
+ * Copyright (C) 2010 Nokia Corporation and/or its subsidiary(-ies).
+ * Copyright (c) 2010 NVIDIA Corporation.
+ * Copyright (C) 2016 Linaro Ltd.
+ *
+ * Author: Samu Onkalo <samu.p.onkalo@nokia.com>
+ * Author: Linus Walleij <linus.walleij@linaro.org>
+ */
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/i2c.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h> /* For irq_get_irq_data() */
+#include <linux/completion.h>
+#include <linux/err.h>
+#include <linux/mutex.h>
+#include <linux/delay.h>
+#include <linux/bitops.h>
+#include <linux/regmap.h>
+#include <linux/regulator/consumer.h>
+#include <linux/pm_runtime.h>
+
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/trigger.h>
+#include <linux/iio/trigger_consumer.h>
+#include <linux/iio/triggered_buffer.h>
+
+/*
+ * 16-bit registers are little-endian. LSB is at the address defined below
+ * and MSB is at the next higher address.
+ */
+
+/* These registers are common for AK8974 and AMI305 */
+#define AK8974_SELFTEST		0x0C
+#define AK8974_SELFTEST_IDLE	0x55
+#define AK8974_SELFTEST_OK	0xAA
+
+#define AK8974_INFO		0x0D
+
+#define AK8974_WHOAMI		0x0F
+#define AK8974_WHOAMI_VALUE_AMI305 0x47
+#define AK8974_WHOAMI_VALUE_AK8974 0x48
+
+#define AK8974_DATA_X		0x10
+#define AK8974_DATA_Y		0x12
+#define AK8974_DATA_Z		0x14
+#define AK8974_INT_SRC		0x16
+#define AK8974_STATUS		0x18
+#define AK8974_INT_CLEAR	0x1A
+#define AK8974_CTRL1		0x1B
+#define AK8974_CTRL2		0x1C
+#define AK8974_CTRL3		0x1D
+#define AK8974_INT_CTRL		0x1E
+#define AK8974_INT_THRES	0x26  /* Absolute any axis value threshold */
+#define AK8974_PRESET		0x30
+
+/* AK8974-specific offsets */
+#define AK8974_OFFSET_X		0x20
+#define AK8974_OFFSET_Y		0x22
+#define AK8974_OFFSET_Z		0x24
+/* AMI305-specific offsets */
+#define AMI305_OFFSET_X		0x6C
+#define AMI305_OFFSET_Y		0x72
+#define AMI305_OFFSET_Z		0x78
+
+/* Different temperature registers */
+#define AK8974_TEMP		0x31
+#define AMI305_TEMP		0x60
+
+#define AK8974_INT_X_HIGH	BIT(7) /* Axis over +threshold  */
+#define AK8974_INT_Y_HIGH	BIT(6)
+#define AK8974_INT_Z_HIGH	BIT(5)
+#define AK8974_INT_X_LOW	BIT(4) /* Axis below -threshold	*/
+#define AK8974_INT_Y_LOW	BIT(3)
+#define AK8974_INT_Z_LOW	BIT(2)
+#define AK8974_INT_RANGE	BIT(1) /* Range overflow (any axis) */
+
+#define AK8974_STATUS_DRDY	BIT(6) /* Data ready */
+#define AK8974_STATUS_OVERRUN	BIT(5) /* Data overrun */
+#define AK8974_STATUS_INT	BIT(4) /* Interrupt occurred */
+
+#define AK8974_CTRL1_POWER	BIT(7) /* 0 = standby; 1 = active */
+#define AK8974_CTRL1_RATE	BIT(4) /* 0 = 10 Hz; 1 = 20 Hz	 */
+#define AK8974_CTRL1_FORCE_EN	BIT(1) /* 0 = normal; 1 = force	 */
+#define AK8974_CTRL1_MODE2	BIT(0) /* 0 */
+
+#define AK8974_CTRL2_INT_EN	BIT(4)  /* 1 = enable interrupts	      */
+#define AK8974_CTRL2_DRDY_EN	BIT(3)  /* 1 = enable data ready signal */
+#define AK8974_CTRL2_DRDY_POL	BIT(2)  /* 1 = data ready active high   */
+#define AK8974_CTRL2_RESDEF	(AK8974_CTRL2_DRDY_POL)
+
+#define AK8974_CTRL3_RESET	BIT(7) /* Software reset		  */
+#define AK8974_CTRL3_FORCE	BIT(6) /* Start forced measurement */
+#define AK8974_CTRL3_SELFTEST	BIT(4) /* Set selftest register	  */
+#define AK8974_CTRL3_RESDEF	0x00
+
+#define AK8974_INT_CTRL_XEN	BIT(7) /* Enable interrupt for this axis */
+#define AK8974_INT_CTRL_YEN	BIT(6)
+#define AK8974_INT_CTRL_ZEN	BIT(5)
+#define AK8974_INT_CTRL_XYZEN	(BIT(7)|BIT(6)|BIT(5))
+#define AK8974_INT_CTRL_POL	BIT(3) /* 0 = active low; 1 = active high */
+#define AK8974_INT_CTRL_PULSE	BIT(1) /* 0 = latched; 1 = pulse (50 usec) */
+#define AK8974_INT_CTRL_RESDEF	(AK8974_INT_CTRL_XYZEN | AK8974_INT_CTRL_POL)
+
+/* The AMI305 has elaborate FW version and serial number registers */
+#define AMI305_VER		0xE8
+#define AMI305_SN		0xEA
+
+#define AK8974_MAX_RANGE	2048
+
+#define AK8974_POWERON_DELAY	50
+#define AK8974_ACTIVATE_DELAY	1
+#define AK8974_SELFTEST_DELAY	1
+/*
+ * Set the autosuspend to two orders of magnitude larger than the poweron
+ * delay to make sane reasonable power tradeoff savings (5 seconds in
+ * this case).
+ */
+#define AK8974_AUTOSUSPEND_DELAY 5000
+
+#define AK8974_MEASTIME		3
+
+#define AK8974_PWR_ON		1
+#define AK8974_PWR_OFF		0
+
+/**
+ * struct ak8974 - state container for the AK8974 driver
+ * @i2c: parent I2C client
+ * @orientation: mounting matrix, flipped axis etc
+ * @map: regmap to access the AK8974 registers over I2C
+ * @regs: the avdd and dvdd power regulators
+ * @name: the name of the part
+ * @variant: the whoami ID value (for selecting code paths)
+ * @lock: locks the magnetometer for exclusive use during a measurement
+ * @drdy_irq: uses the DRDY IRQ line
+ * @drdy_complete: completion for DRDY
+ * @drdy_active_low: the DRDY IRQ is active low
+ */
+struct ak8974 {
+	struct i2c_client *i2c;
+	struct iio_mount_matrix orientation;
+	struct regmap *map;
+	struct regulator_bulk_data regs[2];
+	const char *name;
+	u8 variant;
+	struct mutex lock;
+	bool drdy_irq;
+	struct completion drdy_complete;
+	bool drdy_active_low;
+};
+
+static const char ak8974_reg_avdd[] = "avdd";
+static const char ak8974_reg_dvdd[] = "dvdd";
+
+static int ak8974_set_power(struct ak8974 *ak8974, bool mode)
+{
+	int ret;
+	u8 val;
+
+	val = mode ? AK8974_CTRL1_POWER : 0;
+	val |= AK8974_CTRL1_FORCE_EN;
+	ret = regmap_write(ak8974->map, AK8974_CTRL1, val);
+	if (ret < 0)
+		return ret;
+
+	if (mode)
+		msleep(AK8974_ACTIVATE_DELAY);
+
+	return 0;
+}
+
+static int ak8974_reset(struct ak8974 *ak8974)
+{
+	int ret;
+
+	/* Power on to get register access. Sets CTRL1 reg to reset state */
+	ret = ak8974_set_power(ak8974, AK8974_PWR_ON);
+	if (ret)
+		return ret;
+	ret = regmap_write(ak8974->map, AK8974_CTRL2, AK8974_CTRL2_RESDEF);
+	if (ret)
+		return ret;
+	ret = regmap_write(ak8974->map, AK8974_CTRL3, AK8974_CTRL3_RESDEF);
+	if (ret)
+		return ret;
+	ret = regmap_write(ak8974->map, AK8974_INT_CTRL,
+			   AK8974_INT_CTRL_RESDEF);
+	if (ret)
+		return ret;
+
+	/* After reset, power off is default state */
+	return ak8974_set_power(ak8974, AK8974_PWR_OFF);
+}
+
+static int ak8974_configure(struct ak8974 *ak8974)
+{
+	int ret;
+
+	ret = regmap_write(ak8974->map, AK8974_CTRL2, AK8974_CTRL2_DRDY_EN |
+			   AK8974_CTRL2_INT_EN);
+	if (ret)
+		return ret;
+	ret = regmap_write(ak8974->map, AK8974_CTRL3, 0);
+	if (ret)
+		return ret;
+	ret = regmap_write(ak8974->map, AK8974_INT_CTRL, AK8974_INT_CTRL_POL);
+	if (ret)
+		return ret;
+
+	return regmap_write(ak8974->map, AK8974_PRESET, 0);
+}
+
+static int ak8974_trigmeas(struct ak8974 *ak8974)
+{
+	unsigned int clear;
+	u8 mask;
+	u8 val;
+	int ret;
+
+	/* Clear any previous measurement overflow status */
+	ret = regmap_read(ak8974->map, AK8974_INT_CLEAR, &clear);
+	if (ret)
+		return ret;
+
+	/* If we have a DRDY IRQ line, use it */
+	if (ak8974->drdy_irq) {
+		mask = AK8974_CTRL2_INT_EN |
+			AK8974_CTRL2_DRDY_EN |
+			AK8974_CTRL2_DRDY_POL;
+		val = AK8974_CTRL2_DRDY_EN;
+
+		if (!ak8974->drdy_active_low)
+			val |= AK8974_CTRL2_DRDY_POL;
+
+		init_completion(&ak8974->drdy_complete);
+		ret = regmap_update_bits(ak8974->map, AK8974_CTRL2,
+					 mask, val);
+		if (ret)
+			return ret;
+	}
+
+	/* Force a measurement */
+	return regmap_update_bits(ak8974->map,
+				  AK8974_CTRL3,
+				  AK8974_CTRL3_FORCE,
+				  AK8974_CTRL3_FORCE);
+}
+
+static int ak8974_await_drdy(struct ak8974 *ak8974)
+{
+	int timeout = 2;
+	unsigned int val;
+	int ret;
+
+	if (ak8974->drdy_irq) {
+		ret = wait_for_completion_timeout(&ak8974->drdy_complete,
+					1 + msecs_to_jiffies(1000));
+		if (!ret) {
+			dev_err(&ak8974->i2c->dev,
+				"timeout waiting for DRDY IRQ\n");
+			return -ETIMEDOUT;
+		}
+		return 0;
+	}
+
+	/* Default delay-based poll loop */
+	do {
+		msleep(AK8974_MEASTIME);
+		ret = regmap_read(ak8974->map, AK8974_STATUS, &val);
+		if (ret < 0)
+			return ret;
+		if (val & AK8974_STATUS_DRDY)
+			return 0;
+	} while (--timeout);
+	if (!timeout) {
+		dev_err(&ak8974->i2c->dev,
+			"timeout waiting for DRDY\n");
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+static int ak8974_getresult(struct ak8974 *ak8974, s16 *result)
+{
+	unsigned int src;
+	int ret;
+
+	ret = ak8974_await_drdy(ak8974);
+	if (ret)
+		return ret;
+	ret = regmap_read(ak8974->map, AK8974_INT_SRC, &src);
+	if (ret < 0)
+		return ret;
+
+	/* Out of range overflow! Strong magnet close? */
+	if (src & AK8974_INT_RANGE) {
+		dev_err(&ak8974->i2c->dev,
+			"range overflow in sensor\n");
+		return -ERANGE;
+	}
+
+	ret = regmap_bulk_read(ak8974->map, AK8974_DATA_X, result, 6);
+	if (ret)
+		return ret;
+
+	return ret;
+}
+
+static irqreturn_t ak8974_drdy_irq(int irq, void *d)
+{
+	struct ak8974 *ak8974 = d;
+
+	if (!ak8974->drdy_irq)
+		return IRQ_NONE;
+
+	/* TODO: timestamp here to get good measurement stamps */
+	return IRQ_WAKE_THREAD;
+}
+
+static irqreturn_t ak8974_drdy_irq_thread(int irq, void *d)
+{
+	struct ak8974 *ak8974 = d;
+	unsigned int val;
+	int ret;
+
+	/* Check if this was a DRDY from us */
+	ret = regmap_read(ak8974->map, AK8974_STATUS, &val);
+	if (ret < 0) {
+		dev_err(&ak8974->i2c->dev, "error reading DRDY status\n");
+		return IRQ_HANDLED;
+	}
+	if (val & AK8974_STATUS_DRDY) {
+		/* Yes this was our IRQ */
+		complete(&ak8974->drdy_complete);
+		return IRQ_HANDLED;
+	}
+
+	/* We may be on a shared IRQ, let the next client check */
+	return IRQ_NONE;
+}
+
+static int ak8974_selftest(struct ak8974 *ak8974)
+{
+	struct device *dev = &ak8974->i2c->dev;
+	unsigned int val;
+	int ret;
+
+	ret = regmap_read(ak8974->map, AK8974_SELFTEST, &val);
+	if (ret)
+		return ret;
+	if (val != AK8974_SELFTEST_IDLE) {
+		dev_err(dev, "selftest not idle before test\n");
+		return -EIO;
+	}
+
+	/* Trigger self-test */
+	ret = regmap_update_bits(ak8974->map,
+			AK8974_CTRL3,
+			AK8974_CTRL3_SELFTEST,
+			AK8974_CTRL3_SELFTEST);
+	if (ret) {
+		dev_err(dev, "could not write CTRL3\n");
+		return ret;
+	}
+
+	msleep(AK8974_SELFTEST_DELAY);
+
+	ret = regmap_read(ak8974->map, AK8974_SELFTEST, &val);
+	if (ret)
+		return ret;
+	if (val != AK8974_SELFTEST_OK) {
+		dev_err(dev, "selftest result NOT OK (%02x)\n", val);
+		return -EIO;
+	}
+
+	ret = regmap_read(ak8974->map, AK8974_SELFTEST, &val);
+	if (ret)
+		return ret;
+	if (val != AK8974_SELFTEST_IDLE) {
+		dev_err(dev, "selftest not idle after test (%02x)\n", val);
+		return -EIO;
+	}
+	dev_dbg(dev, "passed self-test\n");
+
+	return 0;
+}
+
+static int ak8974_get_u16_val(struct ak8974 *ak8974, u8 reg, u16 *val)
+{
+	int ret;
+	u16 bulk;
+
+	ret = regmap_bulk_read(ak8974->map, reg, &bulk, 2);
+	if (ret)
+		return ret;
+	*val = le16_to_cpu(bulk);
+
+	return 0;
+}
+
+static int ak8974_detect(struct ak8974 *ak8974)
+{
+	unsigned int whoami;
+	const char *name;
+	int ret;
+	unsigned int fw;
+	u16 sn;
+
+	ret = regmap_read(ak8974->map, AK8974_WHOAMI, &whoami);
+	if (ret)
+		return ret;
+
+	switch (whoami) {
+	case AK8974_WHOAMI_VALUE_AMI305:
+		name = "ami305";
+		ret = regmap_read(ak8974->map, AMI305_VER, &fw);
+		if (ret)
+			return ret;
+		fw &= 0x7f; /* only bits 0 thru 6 valid */
+		ret = ak8974_get_u16_val(ak8974, AMI305_SN, &sn);
+		if (ret)
+			return ret;
+		dev_info(&ak8974->i2c->dev,
+			 "detected %s, FW ver %02x, S/N: %04x\n",
+			 name, fw, sn);
+		break;
+	case AK8974_WHOAMI_VALUE_AK8974:
+		name = "ak8974";
+		dev_info(&ak8974->i2c->dev, "detected AK8974\n");
+		break;
+	default:
+		dev_err(&ak8974->i2c->dev, "unsupported device (%02x) ",
+			whoami);
+		return -ENODEV;
+	}
+
+	ak8974->name = name;
+	ak8974->variant = whoami;
+
+	return 0;
+}
+
+static int ak8974_read_raw(struct iio_dev *indio_dev,
+			   struct iio_chan_spec const *chan,
+			   int *val, int *val2,
+			   long mask)
+{
+	struct ak8974 *ak8974 = iio_priv(indio_dev);
+	s16 hw_values[3];
+	int ret = -EINVAL;
+
+	pm_runtime_get_sync(&ak8974->i2c->dev);
+	mutex_lock(&ak8974->lock);
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		if (chan->address > 2) {
+			dev_err(&ak8974->i2c->dev, "faulty channel address\n");
+			ret = -EIO;
+			goto out_unlock;
+		}
+		ret = ak8974_trigmeas(ak8974);
+		if (ret)
+			goto out_unlock;
+		ret = ak8974_getresult(ak8974, hw_values);
+		if (ret)
+			goto out_unlock;
+
+		/*
+		 * We read all axes and discard all but one, for optimized
+		 * reading, use the triggered buffer.
+		 */
+		*val = le16_to_cpu(hw_values[chan->address]);
+
+		ret = IIO_VAL_INT;
+	}
+
+ out_unlock:
+	mutex_unlock(&ak8974->lock);
+	pm_runtime_mark_last_busy(&ak8974->i2c->dev);
+	pm_runtime_put_autosuspend(&ak8974->i2c->dev);
+
+	return ret;
+}
+
+static void ak8974_fill_buffer(struct iio_dev *indio_dev)
+{
+	struct ak8974 *ak8974 = iio_priv(indio_dev);
+	int ret;
+	s16 hw_values[8]; /* Three axes + 64bit padding */
+
+	pm_runtime_get_sync(&ak8974->i2c->dev);
+	mutex_lock(&ak8974->lock);
+
+	ret = ak8974_trigmeas(ak8974);
+	if (ret) {
+		dev_err(&ak8974->i2c->dev, "error triggering measure\n");
+		goto out_unlock;
+	}
+	ret = ak8974_getresult(ak8974, hw_values);
+	if (ret) {
+		dev_err(&ak8974->i2c->dev, "error getting measures\n");
+		goto out_unlock;
+	}
+
+	iio_push_to_buffers_with_timestamp(indio_dev, hw_values,
+					   iio_get_time_ns(indio_dev));
+
+ out_unlock:
+	mutex_unlock(&ak8974->lock);
+	pm_runtime_mark_last_busy(&ak8974->i2c->dev);
+	pm_runtime_put_autosuspend(&ak8974->i2c->dev);
+}
+
+static irqreturn_t ak8974_handle_trigger(int irq, void *p)
+{
+	const struct iio_poll_func *pf = p;
+	struct iio_dev *indio_dev = pf->indio_dev;
+
+	ak8974_fill_buffer(indio_dev);
+	iio_trigger_notify_done(indio_dev->trig);
+
+	return IRQ_HANDLED;
+}
+
+static const struct iio_mount_matrix *
+ak8974_get_mount_matrix(const struct iio_dev *indio_dev,
+			const struct iio_chan_spec *chan)
+{
+	struct ak8974 *ak8974 = iio_priv(indio_dev);
+
+	return &ak8974->orientation;
+}
+
+static const struct iio_chan_spec_ext_info ak8974_ext_info[] = {
+	IIO_MOUNT_MATRIX(IIO_SHARED_BY_DIR, ak8974_get_mount_matrix),
+	{ },
+};
+
+#define AK8974_AXIS_CHANNEL(axis, index)				\
+	{								\
+		.type = IIO_MAGN,					\
+		.modified = 1,						\
+		.channel2 = IIO_MOD_##axis,				\
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),		\
+		.ext_info = ak8974_ext_info,				\
+		.address = index,					\
+		.scan_index = index,					\
+		.scan_type = {						\
+			.sign = 's',					\
+			.realbits = 16,					\
+			.storagebits = 16,				\
+			.endianness = IIO_LE				\
+		},							\
+	}
+
+static const struct iio_chan_spec ak8974_channels[] = {
+	AK8974_AXIS_CHANNEL(X, 0),
+	AK8974_AXIS_CHANNEL(Y, 1),
+	AK8974_AXIS_CHANNEL(Z, 2),
+	IIO_CHAN_SOFT_TIMESTAMP(3),
+};
+
+static const unsigned long ak8974_scan_masks[] = { 0x7, 0 };
+
+static const struct iio_info ak8974_info = {
+	.read_raw = &ak8974_read_raw,
+	.driver_module = THIS_MODULE,
+};
+
+static bool ak8974_writeable_reg(struct device *dev, unsigned int reg)
+{
+	struct i2c_client *i2c = to_i2c_client(dev);
+	struct iio_dev *indio_dev = i2c_get_clientdata(i2c);
+	struct ak8974 *ak8974 = iio_priv(indio_dev);
+
+	switch (reg) {
+	case AK8974_CTRL1:
+	case AK8974_CTRL2:
+	case AK8974_CTRL3:
+	case AK8974_INT_CTRL:
+	case AK8974_INT_THRES:
+	case AK8974_INT_THRES + 1:
+	case AK8974_PRESET:
+	case AK8974_PRESET + 1:
+		return true;
+	case AK8974_OFFSET_X:
+	case AK8974_OFFSET_X + 1:
+	case AK8974_OFFSET_Y:
+	case AK8974_OFFSET_Y + 1:
+	case AK8974_OFFSET_Z:
+	case AK8974_OFFSET_Z + 1:
+		if (ak8974->variant == AK8974_WHOAMI_VALUE_AK8974)
+			return true;
+		return false;
+	case AMI305_OFFSET_X:
+	case AMI305_OFFSET_X + 1:
+	case AMI305_OFFSET_Y:
+	case AMI305_OFFSET_Y + 1:
+	case AMI305_OFFSET_Z:
+	case AMI305_OFFSET_Z + 1:
+		if (ak8974->variant == AK8974_WHOAMI_VALUE_AMI305)
+			return true;
+		return false;
+	default:
+		return false;
+	}
+}
+
+static const struct regmap_config ak8974_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
+	.max_register = 0xff,
+	.writeable_reg = ak8974_writeable_reg,
+};
+
+static int ak8974_probe(struct i2c_client *i2c,
+			const struct i2c_device_id *id)
+{
+	struct iio_dev *indio_dev;
+	struct ak8974 *ak8974;
+	unsigned long irq_trig;
+	int irq = i2c->irq;
+	int ret;
+
+	/* Register with IIO */
+	indio_dev = devm_iio_device_alloc(&i2c->dev, sizeof(*ak8974));
+	if (indio_dev == NULL)
+		return -ENOMEM;
+
+	ak8974 = iio_priv(indio_dev);
+	i2c_set_clientdata(i2c, indio_dev);
+	ak8974->i2c = i2c;
+	mutex_init(&ak8974->lock);
+
+	ret = of_iio_read_mount_matrix(&i2c->dev,
+				       "mount-matrix",
+				       &ak8974->orientation);
+	if (ret)
+		return ret;
+
+	ak8974->regs[0].supply = ak8974_reg_avdd;
+	ak8974->regs[1].supply = ak8974_reg_dvdd;
+
+	ret = devm_regulator_bulk_get(&i2c->dev,
+				      ARRAY_SIZE(ak8974->regs),
+				      ak8974->regs);
+	if (ret < 0) {
+		dev_err(&i2c->dev, "cannot get regulators\n");
+		return ret;
+	}
+
+	ret = regulator_bulk_enable(ARRAY_SIZE(ak8974->regs), ak8974->regs);
+	if (ret < 0) {
+		dev_err(&i2c->dev, "cannot enable regulators\n");
+		return ret;
+	}
+
+	/* Take runtime PM online */
+	pm_runtime_get_noresume(&i2c->dev);
+	pm_runtime_set_active(&i2c->dev);
+	pm_runtime_enable(&i2c->dev);
+
+	ak8974->map = devm_regmap_init_i2c(i2c, &ak8974_regmap_config);
+	if (IS_ERR(ak8974->map)) {
+		dev_err(&i2c->dev, "failed to allocate register map\n");
+		return PTR_ERR(ak8974->map);
+	}
+
+	ret = ak8974_set_power(ak8974, AK8974_PWR_ON);
+	if (ret) {
+		dev_err(&i2c->dev, "could not power on\n");
+		goto power_off;
+	}
+
+	ret = ak8974_detect(ak8974);
+	if (ret) {
+		dev_err(&i2c->dev, "neither AK8974 nor AMI305 found\n");
+		goto power_off;
+	}
+
+	ret = ak8974_selftest(ak8974);
+	if (ret)
+		dev_err(&i2c->dev, "selftest failed (continuing anyway)\n");
+
+	ret = ak8974_reset(ak8974);
+	if (ret) {
+		dev_err(&i2c->dev, "AK8974 reset failed\n");
+		goto power_off;
+	}
+
+	pm_runtime_set_autosuspend_delay(&i2c->dev,
+					 AK8974_AUTOSUSPEND_DELAY);
+	pm_runtime_use_autosuspend(&i2c->dev);
+	pm_runtime_put(&i2c->dev);
+
+	indio_dev->dev.parent = &i2c->dev;
+	indio_dev->channels = ak8974_channels;
+	indio_dev->num_channels = ARRAY_SIZE(ak8974_channels);
+	indio_dev->info = &ak8974_info;
+	indio_dev->available_scan_masks = ak8974_scan_masks;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->name = ak8974->name;
+
+	ret = iio_triggered_buffer_setup(indio_dev, NULL,
+					 ak8974_handle_trigger,
+					 NULL);
+	if (ret) {
+		dev_err(&i2c->dev, "triggered buffer setup failed\n");
+		goto disable_pm;
+	}
+
+	/* If we have a valid DRDY IRQ, make use of it */
+	if (irq > 0) {
+		irq_trig = irqd_get_trigger_type(irq_get_irq_data(irq));
+		if (irq_trig == IRQF_TRIGGER_RISING) {
+			dev_info(&i2c->dev, "enable rising edge DRDY IRQ\n");
+		} else if (irq_trig == IRQF_TRIGGER_FALLING) {
+			ak8974->drdy_active_low = true;
+			dev_info(&i2c->dev, "enable falling edge DRDY IRQ\n");
+		} else {
+			irq_trig = IRQF_TRIGGER_RISING;
+		}
+		irq_trig |= IRQF_ONESHOT;
+		irq_trig |= IRQF_SHARED;
+
+		ret = devm_request_threaded_irq(&i2c->dev,
+						irq,
+						ak8974_drdy_irq,
+						ak8974_drdy_irq_thread,
+						irq_trig,
+						ak8974->name,
+						ak8974);
+		if (ret) {
+			dev_err(&i2c->dev, "unable to request DRDY IRQ "
+				"- proceeding without IRQ\n");
+			goto no_irq;
+		}
+		ak8974->drdy_irq = true;
+	}
+
+no_irq:
+	ret = iio_device_register(indio_dev);
+	if (ret) {
+		dev_err(&i2c->dev, "device register failed\n");
+		goto cleanup_buffer;
+	}
+
+	return 0;
+
+cleanup_buffer:
+	iio_triggered_buffer_cleanup(indio_dev);
+disable_pm:
+	pm_runtime_put_noidle(&i2c->dev);
+	pm_runtime_disable(&i2c->dev);
+	ak8974_set_power(ak8974, AK8974_PWR_OFF);
+power_off:
+	regulator_bulk_disable(ARRAY_SIZE(ak8974->regs), ak8974->regs);
+
+	return ret;
+}
+
+static int __exit ak8974_remove(struct i2c_client *i2c)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(i2c);
+	struct ak8974 *ak8974 = iio_priv(indio_dev);
+
+	iio_device_unregister(indio_dev);
+	iio_triggered_buffer_cleanup(indio_dev);
+	pm_runtime_get_sync(&i2c->dev);
+	pm_runtime_put_noidle(&i2c->dev);
+	pm_runtime_disable(&i2c->dev);
+	ak8974_set_power(ak8974, AK8974_PWR_OFF);
+	regulator_bulk_disable(ARRAY_SIZE(ak8974->regs), ak8974->regs);
+
+	return 0;
+}
+
+static int __maybe_unused ak8974_runtime_suspend(struct device *dev)
+{
+	struct ak8974 *ak8974 =
+		iio_priv(i2c_get_clientdata(to_i2c_client(dev)));
+
+	ak8974_set_power(ak8974, AK8974_PWR_OFF);
+	regulator_bulk_disable(ARRAY_SIZE(ak8974->regs), ak8974->regs);
+
+	return 0;
+}
+
+static int __maybe_unused ak8974_runtime_resume(struct device *dev)
+{
+	struct ak8974 *ak8974 =
+		iio_priv(i2c_get_clientdata(to_i2c_client(dev)));
+	int ret;
+
+	ret = regulator_bulk_enable(ARRAY_SIZE(ak8974->regs), ak8974->regs);
+	if (ret)
+		return ret;
+	msleep(AK8974_POWERON_DELAY);
+	ret = ak8974_set_power(ak8974, AK8974_PWR_ON);
+	if (ret)
+		goto out_regulator_disable;
+
+	ret = ak8974_configure(ak8974);
+	if (ret)
+		goto out_disable_power;
+
+	return 0;
+
+out_disable_power:
+	ak8974_set_power(ak8974, AK8974_PWR_OFF);
+out_regulator_disable:
+	regulator_bulk_disable(ARRAY_SIZE(ak8974->regs), ak8974->regs);
+
+	return ret;
+}
+
+static const struct dev_pm_ops ak8974_dev_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(pm_runtime_force_suspend,
+				pm_runtime_force_resume)
+	SET_RUNTIME_PM_OPS(ak8974_runtime_suspend,
+			   ak8974_runtime_resume, NULL)
+};
+
+static const struct i2c_device_id ak8974_id[] = {
+	{"ami305", 0 },
+	{"ak8974", 0 },
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, ak8974_id);
+
+static const struct of_device_id ak8974_of_match[] = {
+	{ .compatible = "asahi-kasei,ak8974", },
+	{}
+};
+MODULE_DEVICE_TABLE(of, ak8974_of_match);
+
+static struct i2c_driver ak8974_driver = {
+	.driver	 = {
+		.name	= "ak8974",
+		.pm = &ak8974_dev_pm_ops,
+		.of_match_table = of_match_ptr(ak8974_of_match),
+	},
+	.probe	  = ak8974_probe,
+	.remove	  = __exit_p(ak8974_remove),
+	.id_table = ak8974_id,
+};
+module_i2c_driver(ak8974_driver);
+
+MODULE_DESCRIPTION("AK8974 and AMI305 3-axis magnetometer driver");
+MODULE_AUTHOR("Samu Onkalo");
+MODULE_AUTHOR("Linus Walleij");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/magnetometer/mag3110.c b/drivers/iio/magnetometer/mag3110.c
index f2be4a049056..f2b3bd7bf862 100644
--- a/drivers/iio/magnetometer/mag3110.c
+++ b/drivers/iio/magnetometer/mag3110.c
@@ -154,34 +154,41 @@ static int mag3110_read_raw(struct iio_dev *indio_dev,
 
 	switch (mask) {
 	case IIO_CHAN_INFO_RAW:
-		if (iio_buffer_enabled(indio_dev))
-			return -EBUSY;
+		ret = iio_device_claim_direct_mode(indio_dev);
+		if (ret)
+			return ret;
 
 		switch (chan->type) {
 		case IIO_MAGN: /* in 0.1 uT / LSB */
 			ret = mag3110_read(data, buffer);
 			if (ret < 0)
-				return ret;
+				goto release;
 			*val = sign_extend32(
 				be16_to_cpu(buffer[chan->scan_index]), 15);
-			return IIO_VAL_INT;
+			ret = IIO_VAL_INT;
+			break;
 		case IIO_TEMP: /* in 1 C / LSB */
 			mutex_lock(&data->lock);
 			ret = mag3110_request(data);
 			if (ret < 0) {
 				mutex_unlock(&data->lock);
-				return ret;
+				goto release;
 			}
 			ret = i2c_smbus_read_byte_data(data->client,
 				MAG3110_DIE_TEMP);
 			mutex_unlock(&data->lock);
 			if (ret < 0)
-				return ret;
+				goto release;
 			*val = sign_extend32(ret, 7);
-			return IIO_VAL_INT;
+			ret = IIO_VAL_INT;
+			break;
 		default:
-			return -EINVAL;
+			ret = -EINVAL;
 		}
+release:
+		iio_device_release_direct_mode(indio_dev);
+		return ret;
+
 	case IIO_CHAN_INFO_SCALE:
 		switch (chan->type) {
 		case IIO_MAGN:
diff --git a/drivers/iio/pressure/Kconfig b/drivers/iio/pressure/Kconfig
index d130cdc78f43..15cd416365c1 100644
--- a/drivers/iio/pressure/Kconfig
+++ b/drivers/iio/pressure/Kconfig
@@ -8,8 +8,6 @@ menu "Pressure sensors"
 config BMP280
 	tristate "Bosch Sensortec BMP180/BMP280 pressure sensor I2C driver"
 	depends on (I2C || SPI_MASTER)
-	depends on !(BMP085_I2C=y || BMP085_I2C=m)
-	depends on !(BMP085_SPI=y || BMP085_SPI=m)
 	select REGMAP
 	select BMP280_I2C if (I2C)
 	select BMP280_SPI if (SPI_MASTER)
@@ -187,4 +185,26 @@ config HP206C
 	  This driver can also be built as a module. If so, the module will
 	  be called hp206c.
 
+config ZPA2326
+	tristate "Murata ZPA2326 pressure sensor driver"
+	select IIO_BUFFER
+	select IIO_TRIGGERED_BUFFER
+	select REGMAP
+	select ZPA2326_I2C if I2C
+	select ZPA2326_SPI if SPI_MASTER
+	help
+	  Say Y here to build support for the Murata ZPA2326 pressure and
+	  temperature sensor.
+
+	  To compile this driver as a module, choose M here: the module will
+	  be called zpa2326.
+
+config ZPA2326_I2C
+	tristate
+	select REGMAP_I2C
+
+config ZPA2326_SPI
+	tristate
+	select REGMAP_SPI
+
 endmenu
diff --git a/drivers/iio/pressure/Makefile b/drivers/iio/pressure/Makefile
index 7f395bed5e88..fff77185a5cc 100644
--- a/drivers/iio/pressure/Makefile
+++ b/drivers/iio/pressure/Makefile
@@ -22,6 +22,9 @@ st_pressure-y := st_pressure_core.o
 st_pressure-$(CONFIG_IIO_BUFFER) += st_pressure_buffer.o
 obj-$(CONFIG_T5403) += t5403.o
 obj-$(CONFIG_HP206C) += hp206c.o
+obj-$(CONFIG_ZPA2326) += zpa2326.o
+obj-$(CONFIG_ZPA2326_I2C) += zpa2326_i2c.o
+obj-$(CONFIG_ZPA2326_SPI) += zpa2326_spi.o
 
 obj-$(CONFIG_IIO_ST_PRESS_I2C) += st_pressure_i2c.o
 obj-$(CONFIG_IIO_ST_PRESS_SPI) += st_pressure_spi.o
diff --git a/drivers/iio/pressure/ms5611_core.c b/drivers/iio/pressure/ms5611_core.c
index feb41f82c64a..a74ed1f0c880 100644
--- a/drivers/iio/pressure/ms5611_core.c
+++ b/drivers/iio/pressure/ms5611_core.c
@@ -416,8 +416,7 @@ static int ms5611_init(struct iio_dev *indio_dev)
 	return 0;
 
 err_regulator_disable:
-	if (!IS_ERR_OR_NULL(st->vdd))
-		regulator_disable(st->vdd);
+	regulator_disable(st->vdd);
 	return ret;
 }
 
@@ -425,8 +424,7 @@ static void ms5611_fini(const struct iio_dev *indio_dev)
 {
 	const struct ms5611_state *st = iio_priv(indio_dev);
 
-	if (!IS_ERR_OR_NULL(st->vdd))
-		regulator_disable(st->vdd);
+	regulator_disable(st->vdd);
 }
 
 int ms5611_probe(struct iio_dev *indio_dev, struct device *dev,
diff --git a/drivers/iio/pressure/zpa2326.c b/drivers/iio/pressure/zpa2326.c
new file mode 100644
index 000000000000..19d2eb46fda6
--- /dev/null
+++ b/drivers/iio/pressure/zpa2326.c
@@ -0,0 +1,1735 @@
+/*
+ * Murata ZPA2326 pressure and temperature sensor IIO driver
+ *
+ * Copyright (c) 2016 Parrot S.A.
+ *
+ * Author: Gregor Boirie <gregor.boirie@parrot.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.
+ */
+
+/**
+ * DOC: ZPA2326 theory of operations
+ *
+ * This driver supports %INDIO_DIRECT_MODE and %INDIO_BUFFER_TRIGGERED IIO
+ * modes.
+ * A internal hardware trigger is also implemented to dispatch registered IIO
+ * trigger consumers upon "sample ready" interrupts.
+ *
+ * ZPA2326 hardware supports 2 sampling mode: one shot and continuous.
+ *
+ * A complete one shot sampling cycle gets device out of low power mode,
+ * performs pressure and temperature measurements, then automatically switches
+ * back to low power mode. It is meant for on demand sampling with optimal power
+ * saving at the cost of lower sampling rate and higher software overhead.
+ * This is a natural candidate for IIO read_raw hook implementation
+ * (%INDIO_DIRECT_MODE). It is also used for triggered buffering support to
+ * ensure explicit synchronization with external trigger events
+ * (%INDIO_BUFFER_TRIGGERED).
+ *
+ * The continuous mode works according to a periodic hardware measurement
+ * process continuously pushing samples into an internal hardware FIFO (for
+ * pressure samples only). Measurement cycle completion may be signaled by a
+ * "sample ready" interrupt.
+ * Typical software sequence of operations :
+ * - get device out of low power mode,
+ * - setup hardware sampling period,
+ * - at end of period, upon data ready interrupt: pop pressure samples out of
+ *   hardware FIFO and fetch temperature sample
+ * - when no longer needed, stop sampling process by putting device into
+ *   low power mode.
+ * This mode is used to implement %INDIO_BUFFER_TRIGGERED mode if device tree
+ * declares a valid interrupt line. In this case, the internal hardware trigger
+ * drives acquisition.
+ *
+ * Note that hardware sampling frequency is taken into account only when
+ * internal hardware trigger is attached as the highest sampling rate seems to
+ * be the most energy efficient.
+ *
+ * TODO:
+ *   preset pressure threshold crossing / IIO events ;
+ *   differential pressure sampling ;
+ *   hardware samples averaging.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/regulator/consumer.h>
+#include <linux/pm_runtime.h>
+#include <linux/regmap.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/trigger.h>
+#include <linux/iio/trigger_consumer.h>
+#include <linux/iio/triggered_buffer.h>
+#include "zpa2326.h"
+
+/* 200 ms should be enough for the longest conversion time in one-shot mode. */
+#define ZPA2326_CONVERSION_JIFFIES (HZ / 5)
+
+/* There should be a 1 ms delay (Tpup) after getting out of reset. */
+#define ZPA2326_TPUP_USEC_MIN      (1000)
+#define ZPA2326_TPUP_USEC_MAX      (2000)
+
+/**
+ * struct zpa2326_frequency - Hardware sampling frequency descriptor
+ * @hz : Frequency in Hertz.
+ * @odr: Output Data Rate word as expected by %ZPA2326_CTRL_REG3_REG.
+ */
+struct zpa2326_frequency {
+	int hz;
+	u16 odr;
+};
+
+/*
+ * Keep these in strict ascending order: last array entry is expected to
+ * correspond to the highest sampling frequency.
+ */
+static const struct zpa2326_frequency zpa2326_sampling_frequencies[] = {
+	{ .hz = 1,  .odr = 1 << ZPA2326_CTRL_REG3_ODR_SHIFT },
+	{ .hz = 5,  .odr = 5 << ZPA2326_CTRL_REG3_ODR_SHIFT },
+	{ .hz = 11, .odr = 6 << ZPA2326_CTRL_REG3_ODR_SHIFT },
+	{ .hz = 23, .odr = 7 << ZPA2326_CTRL_REG3_ODR_SHIFT },
+};
+
+/* Return the highest hardware sampling frequency available. */
+static const struct zpa2326_frequency *zpa2326_highest_frequency(void)
+{
+	return &zpa2326_sampling_frequencies[
+		ARRAY_SIZE(zpa2326_sampling_frequencies) - 1];
+}
+
+/**
+ * struct zpa_private - Per-device internal private state
+ * @timestamp:  Buffered samples ready datum.
+ * @regmap:     Underlying I2C / SPI bus adapter used to abstract slave register
+ *              accesses.
+ * @result:     Allows sampling logic to get completion status of operations
+ *              that interrupt handlers perform asynchronously.
+ * @data_ready: Interrupt handler uses this to wake user context up at sampling
+ *              operation completion.
+ * @trigger:    Optional hardware / interrupt driven trigger used to notify
+ *              external devices a new sample is ready.
+ * @waken:      Flag indicating whether or not device has just been powered on.
+ * @irq:        Optional interrupt line: negative or zero if not declared into
+ *              DT, in which case sampling logic keeps polling status register
+ *              to detect completion.
+ * @frequency:  Current hardware sampling frequency.
+ * @vref:       Power / voltage reference.
+ * @vdd:        Power supply.
+ */
+struct zpa2326_private {
+	s64                             timestamp;
+	struct regmap                  *regmap;
+	int                             result;
+	struct completion               data_ready;
+	struct iio_trigger             *trigger;
+	bool                            waken;
+	int                             irq;
+	const struct zpa2326_frequency *frequency;
+	struct regulator               *vref;
+	struct regulator               *vdd;
+};
+
+#define zpa2326_err(_idev, _format, _arg...) \
+	dev_err(_idev->dev.parent, _format, ##_arg)
+
+#define zpa2326_warn(_idev, _format, _arg...) \
+	dev_warn(_idev->dev.parent, _format, ##_arg)
+
+#ifdef DEBUG
+#define zpa2326_dbg(_idev, _format, _arg...) \
+	dev_dbg(_idev->dev.parent, _format, ##_arg)
+#else
+#define zpa2326_dbg(_idev, _format, _arg...)
+#endif
+
+bool zpa2326_isreg_writeable(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case ZPA2326_REF_P_XL_REG:
+	case ZPA2326_REF_P_L_REG:
+	case ZPA2326_REF_P_H_REG:
+	case ZPA2326_RES_CONF_REG:
+	case ZPA2326_CTRL_REG0_REG:
+	case ZPA2326_CTRL_REG1_REG:
+	case ZPA2326_CTRL_REG2_REG:
+	case ZPA2326_CTRL_REG3_REG:
+	case ZPA2326_THS_P_LOW_REG:
+	case ZPA2326_THS_P_HIGH_REG:
+		return true;
+
+	default:
+		return false;
+	}
+}
+EXPORT_SYMBOL_GPL(zpa2326_isreg_writeable);
+
+bool zpa2326_isreg_readable(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case ZPA2326_REF_P_XL_REG:
+	case ZPA2326_REF_P_L_REG:
+	case ZPA2326_REF_P_H_REG:
+	case ZPA2326_DEVICE_ID_REG:
+	case ZPA2326_RES_CONF_REG:
+	case ZPA2326_CTRL_REG0_REG:
+	case ZPA2326_CTRL_REG1_REG:
+	case ZPA2326_CTRL_REG2_REG:
+	case ZPA2326_CTRL_REG3_REG:
+	case ZPA2326_INT_SOURCE_REG:
+	case ZPA2326_THS_P_LOW_REG:
+	case ZPA2326_THS_P_HIGH_REG:
+	case ZPA2326_STATUS_REG:
+	case ZPA2326_PRESS_OUT_XL_REG:
+	case ZPA2326_PRESS_OUT_L_REG:
+	case ZPA2326_PRESS_OUT_H_REG:
+	case ZPA2326_TEMP_OUT_L_REG:
+	case ZPA2326_TEMP_OUT_H_REG:
+		return true;
+
+	default:
+		return false;
+	}
+}
+EXPORT_SYMBOL_GPL(zpa2326_isreg_readable);
+
+bool zpa2326_isreg_precious(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case ZPA2326_INT_SOURCE_REG:
+	case ZPA2326_PRESS_OUT_H_REG:
+		return true;
+
+	default:
+		return false;
+	}
+}
+EXPORT_SYMBOL_GPL(zpa2326_isreg_precious);
+
+/**
+ * zpa2326_enable_device() - Enable device, i.e. get out of low power mode.
+ * @indio_dev: The IIO device associated with the hardware to enable.
+ *
+ * Required to access complete register space and to perform any sampling
+ * or control operations.
+ *
+ * Return: Zero when successful, a negative error code otherwise.
+ */
+static int zpa2326_enable_device(const struct iio_dev *indio_dev)
+{
+	int err;
+
+	err = regmap_write(((struct zpa2326_private *)
+			    iio_priv(indio_dev))->regmap,
+			    ZPA2326_CTRL_REG0_REG, ZPA2326_CTRL_REG0_ENABLE);
+	if (err) {
+		zpa2326_err(indio_dev, "failed to enable device (%d)", err);
+		return err;
+	}
+
+	zpa2326_dbg(indio_dev, "enabled");
+
+	return 0;
+}
+
+/**
+ * zpa2326_sleep() - Disable device, i.e. switch to low power mode.
+ * @indio_dev: The IIO device associated with the hardware to disable.
+ *
+ * Only %ZPA2326_DEVICE_ID_REG and %ZPA2326_CTRL_REG0_REG registers may be
+ * accessed once device is in the disabled state.
+ *
+ * Return: Zero when successful, a negative error code otherwise.
+ */
+static int zpa2326_sleep(const struct iio_dev *indio_dev)
+{
+	int err;
+
+	err = regmap_write(((struct zpa2326_private *)
+			    iio_priv(indio_dev))->regmap,
+			    ZPA2326_CTRL_REG0_REG, 0);
+	if (err) {
+		zpa2326_err(indio_dev, "failed to sleep (%d)", err);
+		return err;
+	}
+
+	zpa2326_dbg(indio_dev, "sleeping");
+
+	return 0;
+}
+
+/**
+ * zpa2326_reset_device() - Reset device to default hardware state.
+ * @indio_dev: The IIO device associated with the hardware to reset.
+ *
+ * Disable sampling and empty hardware FIFO.
+ * Device must be enabled before reset, i.e. not in low power mode.
+ *
+ * Return: Zero when successful, a negative error code otherwise.
+ */
+static int zpa2326_reset_device(const struct iio_dev *indio_dev)
+{
+	int err;
+
+	err = regmap_write(((struct zpa2326_private *)
+			    iio_priv(indio_dev))->regmap,
+			    ZPA2326_CTRL_REG2_REG, ZPA2326_CTRL_REG2_SWRESET);
+	if (err) {
+		zpa2326_err(indio_dev, "failed to reset device (%d)", err);
+		return err;
+	}
+
+	usleep_range(ZPA2326_TPUP_USEC_MIN, ZPA2326_TPUP_USEC_MAX);
+
+	zpa2326_dbg(indio_dev, "reset");
+
+	return 0;
+}
+
+/**
+ * zpa2326_start_oneshot() - Start a single sampling cycle, i.e. in one shot
+ *                           mode.
+ * @indio_dev: The IIO device associated with the sampling hardware.
+ *
+ * Device must have been previously enabled and configured for one shot mode.
+ * Device will be switched back to low power mode at end of cycle.
+ *
+ * Return: Zero when successful, a negative error code otherwise.
+ */
+static int zpa2326_start_oneshot(const struct iio_dev *indio_dev)
+{
+	int err;
+
+	err = regmap_write(((struct zpa2326_private *)
+			    iio_priv(indio_dev))->regmap,
+			    ZPA2326_CTRL_REG0_REG,
+			    ZPA2326_CTRL_REG0_ENABLE |
+			    ZPA2326_CTRL_REG0_ONE_SHOT);
+	if (err) {
+		zpa2326_err(indio_dev, "failed to start one shot cycle (%d)",
+			    err);
+		return err;
+	}
+
+	zpa2326_dbg(indio_dev, "one shot cycle started");
+
+	return 0;
+}
+
+/**
+ * zpa2326_power_on() - Power on device to allow subsequent configuration.
+ * @indio_dev: The IIO device associated with the sampling hardware.
+ * @private:   Internal private state related to @indio_dev.
+ *
+ * Sampling will be disabled, preventing strange things from happening in our
+ * back. Hardware FIFO content will be cleared.
+ * When successful, device will be left in the enabled state to allow further
+ * configuration.
+ *
+ * Return: Zero when successful, a negative error code otherwise.
+ */
+static int zpa2326_power_on(const struct iio_dev         *indio_dev,
+			    const struct zpa2326_private *private)
+{
+	int err;
+
+	err = regulator_enable(private->vref);
+	if (err)
+		return err;
+
+	err = regulator_enable(private->vdd);
+	if (err)
+		goto vref;
+
+	zpa2326_dbg(indio_dev, "powered on");
+
+	err = zpa2326_enable_device(indio_dev);
+	if (err)
+		goto vdd;
+
+	err = zpa2326_reset_device(indio_dev);
+	if (err)
+		goto sleep;
+
+	return 0;
+
+sleep:
+	zpa2326_sleep(indio_dev);
+vdd:
+	regulator_disable(private->vdd);
+vref:
+	regulator_disable(private->vref);
+
+	zpa2326_dbg(indio_dev, "powered off");
+
+	return err;
+}
+
+/**
+ * zpa2326_power_off() - Power off device, i.e. disable attached power
+ *                       regulators.
+ * @indio_dev: The IIO device associated with the sampling hardware.
+ * @private:   Internal private state related to @indio_dev.
+ *
+ * Return: Zero when successful, a negative error code otherwise.
+ */
+static void zpa2326_power_off(const struct iio_dev         *indio_dev,
+			      const struct zpa2326_private *private)
+{
+	regulator_disable(private->vdd);
+	regulator_disable(private->vref);
+
+	zpa2326_dbg(indio_dev, "powered off");
+}
+
+/**
+ * zpa2326_config_oneshot() - Setup device for one shot / on demand mode.
+ * @indio_dev: The IIO device associated with the sampling hardware.
+ * @irq:       Optional interrupt line the hardware uses to notify new data
+ *             samples are ready. Negative or zero values indicate no interrupts
+ *             are available, meaning polling is required.
+ *
+ * Output Data Rate is configured for the highest possible rate so that
+ * conversion time and power consumption are reduced to a minimum.
+ * Note that hardware internal averaging machinery (not implemented in this
+ * driver) is not applicable in this mode.
+ *
+ * Device must have been previously enabled before calling
+ * zpa2326_config_oneshot().
+ *
+ * Return: Zero when successful, a negative error code otherwise.
+ */
+static int zpa2326_config_oneshot(const struct iio_dev *indio_dev,
+				  int                   irq)
+{
+	struct regmap                  *regs = ((struct zpa2326_private *)
+						iio_priv(indio_dev))->regmap;
+	const struct zpa2326_frequency *freq = zpa2326_highest_frequency();
+	int                             err;
+
+	/* Setup highest available Output Data Rate for one shot mode. */
+	err = regmap_write(regs, ZPA2326_CTRL_REG3_REG, freq->odr);
+	if (err)
+		return err;
+
+	if (irq > 0) {
+		/* Request interrupt when new sample is available. */
+		err = regmap_write(regs, ZPA2326_CTRL_REG1_REG,
+				   (u8)~ZPA2326_CTRL_REG1_MASK_DATA_READY);
+
+		if (err) {
+			dev_err(indio_dev->dev.parent,
+				"failed to setup one shot mode (%d)", err);
+			return err;
+		}
+	}
+
+	zpa2326_dbg(indio_dev, "one shot mode setup @%dHz", freq->hz);
+
+	return 0;
+}
+
+/**
+ * zpa2326_clear_fifo() - Clear remaining entries in hardware FIFO.
+ * @indio_dev: The IIO device associated with the sampling hardware.
+ * @min_count: Number of samples present within hardware FIFO.
+ *
+ * @min_count argument is a hint corresponding to the known minimum number of
+ * samples currently living in the FIFO. This allows to reduce the number of bus
+ * accesses by skipping status register read operation as long as we know for
+ * sure there are still entries left.
+ *
+ * Return: Zero when successful, a negative error code otherwise.
+ */
+static int zpa2326_clear_fifo(const struct iio_dev *indio_dev,
+			      unsigned int          min_count)
+{
+	struct regmap *regs = ((struct zpa2326_private *)
+			       iio_priv(indio_dev))->regmap;
+	int            err;
+	unsigned int   val;
+
+	if (!min_count) {
+		/*
+		 * No hint: read status register to determine whether FIFO is
+		 * empty or not.
+		 */
+		err = regmap_read(regs, ZPA2326_STATUS_REG, &val);
+
+		if (err < 0)
+			goto err;
+
+		if (val & ZPA2326_STATUS_FIFO_E)
+			/* Fifo is empty: nothing to trash. */
+			return 0;
+	}
+
+	/* Clear FIFO. */
+	do {
+		/*
+		 * A single fetch from pressure MSB register is enough to pop
+		 * values out of FIFO.
+		 */
+		err = regmap_read(regs, ZPA2326_PRESS_OUT_H_REG, &val);
+		if (err < 0)
+			goto err;
+
+		if (min_count) {
+			/*
+			 * We know for sure there are at least min_count entries
+			 * left in FIFO. Skip status register read.
+			 */
+			min_count--;
+			continue;
+		}
+
+		err = regmap_read(regs, ZPA2326_STATUS_REG, &val);
+		if (err < 0)
+			goto err;
+
+	} while (!(val & ZPA2326_STATUS_FIFO_E));
+
+	zpa2326_dbg(indio_dev, "FIFO cleared");
+
+	return 0;
+
+err:
+	zpa2326_err(indio_dev, "failed to clear FIFO (%d)", err);
+
+	return err;
+}
+
+/**
+ * zpa2326_dequeue_pressure() - Retrieve the most recent pressure sample from
+ *                              hardware FIFO.
+ * @indio_dev: The IIO device associated with the sampling hardware.
+ * @pressure:  Sampled pressure output.
+ *
+ * Note that ZPA2326 hardware FIFO stores pressure samples only.
+ *
+ * Return: Zero when successful, a negative error code otherwise.
+ */
+static int zpa2326_dequeue_pressure(const struct iio_dev *indio_dev,
+				    u32                  *pressure)
+{
+	struct regmap *regs = ((struct zpa2326_private *)
+			       iio_priv(indio_dev))->regmap;
+	unsigned int   val;
+	int            err;
+	int            cleared = -1;
+
+	err = regmap_read(regs, ZPA2326_STATUS_REG, &val);
+	if (err < 0)
+		return err;
+
+	*pressure = 0;
+
+	if (val & ZPA2326_STATUS_P_OR) {
+		/*
+		 * Fifo overrun : first sample dequeued from FIFO is the
+		 * newest.
+		 */
+		zpa2326_warn(indio_dev, "FIFO overflow");
+
+		err = regmap_bulk_read(regs, ZPA2326_PRESS_OUT_XL_REG, pressure,
+				       3);
+		if (err)
+			return err;
+
+#define ZPA2326_FIFO_DEPTH (16U)
+		/* Hardware FIFO may hold no more than 16 pressure samples. */
+		return zpa2326_clear_fifo(indio_dev, ZPA2326_FIFO_DEPTH - 1);
+	}
+
+	/*
+	 * Fifo has not overflown : retrieve newest sample. We need to pop
+	 * values out until FIFO is empty : last fetched pressure is the newest.
+	 * In nominal cases, we should find a single queued sample only.
+	 */
+	do {
+		err = regmap_bulk_read(regs, ZPA2326_PRESS_OUT_XL_REG, pressure,
+				       3);
+		if (err)
+			return err;
+
+		err = regmap_read(regs, ZPA2326_STATUS_REG, &val);
+		if (err < 0)
+			return err;
+
+		cleared++;
+	} while (!(val & ZPA2326_STATUS_FIFO_E));
+
+	if (cleared)
+		/*
+		 * Samples were pushed by hardware during previous rounds but we
+		 * didn't consume them fast enough: inform user.
+		 */
+		zpa2326_dbg(indio_dev, "cleared %d FIFO entries", cleared);
+
+	return 0;
+}
+
+/**
+ * zpa2326_fill_sample_buffer() - Enqueue new channel samples to IIO buffer.
+ * @indio_dev: The IIO device associated with the sampling hardware.
+ * @private:   Internal private state related to @indio_dev.
+ *
+ * Return: Zero when successful, a negative error code otherwise.
+ */
+static int zpa2326_fill_sample_buffer(struct iio_dev               *indio_dev,
+				      const struct zpa2326_private *private)
+{
+	struct {
+		u32 pressure;
+		u16 temperature;
+		u64 timestamp;
+	}   sample;
+	int err;
+
+	if (test_bit(0, indio_dev->active_scan_mask)) {
+		/* Get current pressure from hardware FIFO. */
+		err = zpa2326_dequeue_pressure(indio_dev, &sample.pressure);
+		if (err) {
+			zpa2326_warn(indio_dev, "failed to fetch pressure (%d)",
+				     err);
+			return err;
+		}
+	}
+
+	if (test_bit(1, indio_dev->active_scan_mask)) {
+		/* Get current temperature. */
+		err = regmap_bulk_read(private->regmap, ZPA2326_TEMP_OUT_L_REG,
+				       &sample.temperature, 2);
+		if (err) {
+			zpa2326_warn(indio_dev,
+				     "failed to fetch temperature (%d)", err);
+			return err;
+		}
+	}
+
+	/*
+	 * Now push samples using timestamp stored either :
+	 *   - by hardware interrupt handler if interrupt is available: see
+	 *     zpa2326_handle_irq(),
+	 *   - or oneshot completion polling machinery : see
+	 *     zpa2326_trigger_handler().
+	 */
+	zpa2326_dbg(indio_dev, "filling raw samples buffer");
+
+	iio_push_to_buffers_with_timestamp(indio_dev, &sample,
+					   private->timestamp);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int zpa2326_runtime_suspend(struct device *parent)
+{
+	const struct iio_dev *indio_dev = dev_get_drvdata(parent);
+
+	if (pm_runtime_autosuspend_expiration(parent))
+		/* Userspace changed autosuspend delay. */
+		return -EAGAIN;
+
+	zpa2326_power_off(indio_dev, iio_priv(indio_dev));
+
+	return 0;
+}
+
+static int zpa2326_runtime_resume(struct device *parent)
+{
+	const struct iio_dev *indio_dev = dev_get_drvdata(parent);
+
+	return zpa2326_power_on(indio_dev, iio_priv(indio_dev));
+}
+
+const struct dev_pm_ops zpa2326_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(pm_runtime_force_suspend,
+				pm_runtime_force_resume)
+	SET_RUNTIME_PM_OPS(zpa2326_runtime_suspend, zpa2326_runtime_resume,
+			   NULL)
+};
+EXPORT_SYMBOL_GPL(zpa2326_pm_ops);
+
+/**
+ * zpa2326_resume() - Request the PM layer to power supply the device.
+ * @indio_dev: The IIO device associated with the sampling hardware.
+ *
+ * Return:
+ *  < 0 - a negative error code meaning failure ;
+ *    0 - success, device has just been powered up ;
+ *    1 - success, device was already powered.
+ */
+static int zpa2326_resume(const struct iio_dev *indio_dev)
+{
+	int err;
+
+	err = pm_runtime_get_sync(indio_dev->dev.parent);
+	if (err < 0)
+		return err;
+
+	if (err > 0) {
+		/*
+		 * Device was already power supplied: get it out of low power
+		 * mode and inform caller.
+		 */
+		zpa2326_enable_device(indio_dev);
+		return 1;
+	}
+
+	/* Inform caller device has just been brought back to life. */
+	return 0;
+}
+
+/**
+ * zpa2326_suspend() - Schedule a power down using autosuspend feature of PM
+ *                     layer.
+ * @indio_dev: The IIO device associated with the sampling hardware.
+ *
+ * Device is switched to low power mode at first to save power even when
+ * attached regulator is a "dummy" one.
+ */
+static void zpa2326_suspend(struct iio_dev *indio_dev)
+{
+	struct device *parent = indio_dev->dev.parent;
+
+	zpa2326_sleep(indio_dev);
+
+	pm_runtime_mark_last_busy(parent);
+	pm_runtime_put_autosuspend(parent);
+}
+
+static void zpa2326_init_runtime(struct device *parent)
+{
+	pm_runtime_get_noresume(parent);
+	pm_runtime_set_active(parent);
+	pm_runtime_enable(parent);
+	pm_runtime_set_autosuspend_delay(parent, 1000);
+	pm_runtime_use_autosuspend(parent);
+	pm_runtime_mark_last_busy(parent);
+	pm_runtime_put_autosuspend(parent);
+}
+
+static void zpa2326_fini_runtime(struct device *parent)
+{
+	pm_runtime_disable(parent);
+	pm_runtime_set_suspended(parent);
+}
+#else /* !CONFIG_PM */
+static int zpa2326_resume(const struct iio_dev *indio_dev)
+{
+	zpa2326_enable_device(indio_dev);
+
+	return 0;
+}
+
+static void zpa2326_suspend(struct iio_dev *indio_dev)
+{
+	zpa2326_sleep(indio_dev);
+}
+
+#define zpa2326_init_runtime(_parent)
+#define zpa2326_fini_runtime(_parent)
+#endif /* !CONFIG_PM */
+
+/**
+ * zpa2326_handle_irq() - Process hardware interrupts.
+ * @irq:  Interrupt line the hardware uses to notify new data has arrived.
+ * @data: The IIO device associated with the sampling hardware.
+ *
+ * Timestamp buffered samples as soon as possible then schedule threaded bottom
+ * half.
+ *
+ * Return: Always successful.
+ */
+static irqreturn_t zpa2326_handle_irq(int irq, void *data)
+{
+	struct iio_dev *indio_dev = (struct iio_dev *)data;
+
+	if (iio_buffer_enabled(indio_dev)) {
+		/* Timestamping needed for buffered sampling only. */
+		((struct zpa2326_private *)
+		 iio_priv(indio_dev))->timestamp = iio_get_time_ns(indio_dev);
+	}
+
+	return IRQ_WAKE_THREAD;
+}
+
+/**
+ * zpa2326_handle_threaded_irq() - Interrupt bottom-half handler.
+ * @irq:  Interrupt line the hardware uses to notify new data has arrived.
+ * @data: The IIO device associated with the sampling hardware.
+ *
+ * Mainly ensures interrupt is caused by a real "new sample available"
+ * condition. This relies upon the ability to perform blocking / sleeping bus
+ * accesses to slave's registers. This is why zpa2326_handle_threaded_irq() is
+ * called from within a thread, i.e. not called from hard interrupt context.
+ *
+ * When device is using its own internal hardware trigger in continuous sampling
+ * mode, data are available into hardware FIFO once interrupt has occurred. All
+ * we have to do is to dispatch the trigger, which in turn will fetch data and
+ * fill IIO buffer.
+ *
+ * When not using its own internal hardware trigger, the device has been
+ * configured in one-shot mode either by an external trigger or the IIO read_raw
+ * hook. This means one of the latter is currently waiting for sampling
+ * completion, in which case we must simply wake it up.
+ *
+ * See zpa2326_trigger_handler().
+ *
+ * Return:
+ *   %IRQ_NONE - no consistent interrupt happened ;
+ *   %IRQ_HANDLED - there was new samples available.
+ */
+static irqreturn_t zpa2326_handle_threaded_irq(int irq, void *data)
+{
+	struct iio_dev         *indio_dev = (struct iio_dev *)data;
+	struct zpa2326_private *priv = iio_priv(indio_dev);
+	unsigned int            val;
+	bool                    cont;
+	irqreturn_t             ret = IRQ_NONE;
+
+	/*
+	 * Are we using our own internal trigger in triggered buffer mode, i.e.,
+	 * currently working in continuous sampling mode ?
+	 */
+	cont = (iio_buffer_enabled(indio_dev) &&
+		iio_trigger_using_own(indio_dev));
+
+	/*
+	 * Device works according to a level interrupt scheme: reading interrupt
+	 * status de-asserts interrupt line.
+	 */
+	priv->result = regmap_read(priv->regmap, ZPA2326_INT_SOURCE_REG, &val);
+	if (priv->result < 0) {
+		if (cont)
+			return IRQ_NONE;
+
+		goto complete;
+	}
+
+	/* Data ready is the only interrupt source we requested. */
+	if (!(val & ZPA2326_INT_SOURCE_DATA_READY)) {
+		/*
+		 * Interrupt happened but no new sample available: likely caused
+		 * by spurious interrupts, in which case, returning IRQ_NONE
+		 * allows to benefit from the generic spurious interrupts
+		 * handling.
+		 */
+		zpa2326_warn(indio_dev, "unexpected interrupt status %02x",
+			     val);
+
+		if (cont)
+			return IRQ_NONE;
+
+		priv->result = -ENODATA;
+		goto complete;
+	}
+
+	/* New sample available: dispatch internal trigger consumers. */
+	iio_trigger_poll_chained(priv->trigger);
+
+	if (cont)
+		/*
+		 * Internal hardware trigger has been scheduled above : it will
+		 * fetch data on its own.
+		 */
+		return IRQ_HANDLED;
+
+	ret = IRQ_HANDLED;
+
+complete:
+	/*
+	 * Wake up direct or externaly triggered buffer mode waiters: see
+	 * zpa2326_sample_oneshot() and zpa2326_trigger_handler().
+	 */
+	complete(&priv->data_ready);
+
+	return ret;
+}
+
+/**
+ * zpa2326_wait_oneshot_completion() - Wait for oneshot data ready interrupt.
+ * @indio_dev: The IIO device associated with the sampling hardware.
+ * @private:   Internal private state related to @indio_dev.
+ *
+ * Return: Zero when successful, a negative error code otherwise.
+ */
+static int zpa2326_wait_oneshot_completion(const struct iio_dev   *indio_dev,
+					   struct zpa2326_private *private)
+{
+	int          ret;
+	unsigned int val;
+
+	zpa2326_dbg(indio_dev, "waiting for one shot completion interrupt");
+
+	ret = wait_for_completion_interruptible_timeout(
+		&private->data_ready, ZPA2326_CONVERSION_JIFFIES);
+	if (ret > 0)
+		/*
+		 * Interrupt handler completed before timeout: return operation
+		 * status.
+		 */
+		return private->result;
+
+	/* Clear all interrupts just to be sure. */
+	regmap_read(private->regmap, ZPA2326_INT_SOURCE_REG, &val);
+
+	if (!ret)
+		/* Timed out. */
+		ret = -ETIME;
+
+	if (ret != -ERESTARTSYS)
+		zpa2326_warn(indio_dev, "no one shot interrupt occurred (%d)",
+			     ret);
+
+	return ret;
+}
+
+static int zpa2326_init_managed_irq(struct device          *parent,
+				    struct iio_dev         *indio_dev,
+				    struct zpa2326_private *private,
+				    int                     irq)
+{
+	int err;
+
+	private->irq = irq;
+
+	if (irq <= 0) {
+		/*
+		 * Platform declared no interrupt line: device will be polled
+		 * for data availability.
+		 */
+		dev_info(parent, "no interrupt found, running in polling mode");
+		return 0;
+	}
+
+	init_completion(&private->data_ready);
+
+	/* Request handler to be scheduled into threaded interrupt context. */
+	err = devm_request_threaded_irq(parent, irq, zpa2326_handle_irq,
+					zpa2326_handle_threaded_irq,
+					IRQF_TRIGGER_RISING | IRQF_ONESHOT,
+					dev_name(parent), indio_dev);
+	if (err) {
+		dev_err(parent, "failed to request interrupt %d (%d)", irq,
+			err);
+		return err;
+	}
+
+	dev_info(parent, "using interrupt %d", irq);
+
+	return 0;
+}
+
+/**
+ * zpa2326_poll_oneshot_completion() - Actively poll for one shot data ready.
+ * @indio_dev: The IIO device associated with the sampling hardware.
+ *
+ * Loop over registers content to detect end of sampling cycle. Used when DT
+ * declared no valid interrupt lines.
+ *
+ * Return: Zero when successful, a negative error code otherwise.
+ */
+static int zpa2326_poll_oneshot_completion(const struct iio_dev *indio_dev)
+{
+	unsigned long  tmout = jiffies + ZPA2326_CONVERSION_JIFFIES;
+	struct regmap *regs = ((struct zpa2326_private *)
+			       iio_priv(indio_dev))->regmap;
+	unsigned int   val;
+	int            err;
+
+	zpa2326_dbg(indio_dev, "polling for one shot completion");
+
+	/*
+	 * At least, 100 ms is needed for the device to complete its one-shot
+	 * cycle.
+	 */
+	if (msleep_interruptible(100))
+		return -ERESTARTSYS;
+
+	/* Poll for conversion completion in hardware. */
+	while (true) {
+		err = regmap_read(regs, ZPA2326_CTRL_REG0_REG, &val);
+		if (err < 0)
+			goto err;
+
+		if (!(val & ZPA2326_CTRL_REG0_ONE_SHOT))
+			/* One-shot bit self clears at conversion end. */
+			break;
+
+		if (time_after(jiffies, tmout)) {
+			/* Prevent from waiting forever : let's time out. */
+			err = -ETIME;
+			goto err;
+		}
+
+		usleep_range(10000, 20000);
+	}
+
+	/*
+	 * In oneshot mode, pressure sample availability guarantees that
+	 * temperature conversion has also completed : just check pressure
+	 * status bit to keep things simple.
+	 */
+	err = regmap_read(regs, ZPA2326_STATUS_REG, &val);
+	if (err < 0)
+		goto err;
+
+	if (!(val & ZPA2326_STATUS_P_DA)) {
+		/* No sample available. */
+		err = -ENODATA;
+		goto err;
+	}
+
+	return 0;
+
+err:
+	zpa2326_warn(indio_dev, "failed to poll one shot completion (%d)", err);
+
+	return err;
+}
+
+/**
+ * zpa2326_fetch_raw_sample() - Retrieve a raw sample and convert it to CPU
+ *                              endianness.
+ * @indio_dev: The IIO device associated with the sampling hardware.
+ * @type:      Type of measurement / channel to fetch from.
+ * @value:     Sample output.
+ *
+ * Return: Zero when successful, a negative error code otherwise.
+ */
+static int zpa2326_fetch_raw_sample(const struct iio_dev *indio_dev,
+				    enum iio_chan_type    type,
+				    int                  *value)
+{
+	struct regmap *regs = ((struct zpa2326_private *)
+			       iio_priv(indio_dev))->regmap;
+	int            err;
+
+	switch (type) {
+	case IIO_PRESSURE:
+		zpa2326_dbg(indio_dev, "fetching raw pressure sample");
+
+		err = regmap_bulk_read(regs, ZPA2326_PRESS_OUT_XL_REG, value,
+				       3);
+		if (err) {
+			zpa2326_warn(indio_dev, "failed to fetch pressure (%d)",
+				     err);
+			return err;
+		}
+
+		/* Pressure is a 24 bits wide little-endian unsigned int. */
+		*value = (((u8 *)value)[2] << 16) | (((u8 *)value)[1] << 8) |
+			 ((u8 *)value)[0];
+
+		return IIO_VAL_INT;
+
+	case IIO_TEMP:
+		zpa2326_dbg(indio_dev, "fetching raw temperature sample");
+
+		err = regmap_bulk_read(regs, ZPA2326_TEMP_OUT_L_REG, value, 2);
+		if (err) {
+			zpa2326_warn(indio_dev,
+				     "failed to fetch temperature (%d)", err);
+			return err;
+		}
+
+		/* Temperature is a 16 bits wide little-endian signed int. */
+		*value = (int)le16_to_cpup((__le16 *)value);
+
+		return IIO_VAL_INT;
+
+	default:
+		return -EINVAL;
+	}
+}
+
+/**
+ * zpa2326_sample_oneshot() - Perform a complete one shot sampling cycle.
+ * @indio_dev: The IIO device associated with the sampling hardware.
+ * @type:      Type of measurement / channel to fetch from.
+ * @value:     Sample output.
+ *
+ * Return: Zero when successful, a negative error code otherwise.
+ */
+static int zpa2326_sample_oneshot(struct iio_dev     *indio_dev,
+				  enum iio_chan_type  type,
+				  int                *value)
+{
+	int                     ret;
+	struct zpa2326_private *priv;
+
+	ret = iio_device_claim_direct_mode(indio_dev);
+	if (ret)
+		return ret;
+
+	ret = zpa2326_resume(indio_dev);
+	if (ret < 0)
+		goto release;
+
+	priv = iio_priv(indio_dev);
+
+	if (ret > 0) {
+		/*
+		 * We were already power supplied. Just clear hardware FIFO to
+		 * get rid of samples acquired during previous rounds (if any).
+		 * Sampling operation always generates both temperature and
+		 * pressure samples. The latter are always enqueued into
+		 * hardware FIFO. This may lead to situations were pressure
+		 * samples still sit into FIFO when previous cycle(s) fetched
+		 * temperature data only.
+		 * Hence, we need to clear hardware FIFO content to prevent from
+		 * getting outdated values at the end of current cycle.
+		 */
+		if (type == IIO_PRESSURE) {
+			ret = zpa2326_clear_fifo(indio_dev, 0);
+			if (ret)
+				goto suspend;
+		}
+	} else {
+		/*
+		 * We have just been power supplied, i.e. device is in default
+		 * "out of reset" state, meaning we need to reconfigure it
+		 * entirely.
+		 */
+		ret = zpa2326_config_oneshot(indio_dev, priv->irq);
+		if (ret)
+			goto suspend;
+	}
+
+	/* Start a sampling cycle in oneshot mode. */
+	ret = zpa2326_start_oneshot(indio_dev);
+	if (ret)
+		goto suspend;
+
+	/* Wait for sampling cycle to complete. */
+	if (priv->irq > 0)
+		ret = zpa2326_wait_oneshot_completion(indio_dev, priv);
+	else
+		ret = zpa2326_poll_oneshot_completion(indio_dev);
+
+	if (ret)
+		goto suspend;
+
+	/* Retrieve raw sample value and convert it to CPU endianness. */
+	ret = zpa2326_fetch_raw_sample(indio_dev, type, value);
+
+suspend:
+	zpa2326_suspend(indio_dev);
+release:
+	iio_device_release_direct_mode(indio_dev);
+
+	return ret;
+}
+
+/**
+ * zpa2326_trigger_handler() - Perform an IIO buffered sampling round in one
+ *                             shot mode.
+ * @irq:  The software interrupt assigned to @data
+ * @data: The IIO poll function dispatched by external trigger our device is
+ *        attached to.
+ *
+ * Bottom-half handler called by the IIO trigger to which our device is
+ * currently attached. Allows us to synchronize this device buffered sampling
+ * either with external events (such as timer expiration, external device sample
+ * ready, etc...) or with its own interrupt (internal hardware trigger).
+ *
+ * When using an external trigger, basically run the same sequence of operations
+ * as for zpa2326_sample_oneshot() with the following hereafter. Hardware FIFO
+ * is not cleared since already done at buffering enable time and samples
+ * dequeueing always retrieves the most recent value.
+ *
+ * Otherwise, when internal hardware trigger has dispatched us, just fetch data
+ * from hardware FIFO.
+ *
+ * Fetched data will pushed unprocessed to IIO buffer since samples conversion
+ * is delegated to userspace in buffered mode (endianness, etc...).
+ *
+ * Return:
+ *   %IRQ_NONE - no consistent interrupt happened ;
+ *   %IRQ_HANDLED - there was new samples available.
+ */
+static irqreturn_t zpa2326_trigger_handler(int irq, void *data)
+{
+	struct iio_dev         *indio_dev = ((struct iio_poll_func *)
+					     data)->indio_dev;
+	struct zpa2326_private *priv = iio_priv(indio_dev);
+	bool                    cont;
+
+	/*
+	 * We have been dispatched, meaning we are in triggered buffer mode.
+	 * Using our own internal trigger implies we are currently in continuous
+	 * hardware sampling mode.
+	 */
+	cont = iio_trigger_using_own(indio_dev);
+
+	if (!cont) {
+		/* On demand sampling : start a one shot cycle. */
+		if (zpa2326_start_oneshot(indio_dev))
+			goto out;
+
+		/* Wait for sampling cycle to complete. */
+		if (priv->irq <= 0) {
+			/* No interrupt available: poll for completion. */
+			if (zpa2326_poll_oneshot_completion(indio_dev))
+				goto out;
+
+			/* Only timestamp sample once it is ready. */
+			priv->timestamp = iio_get_time_ns(indio_dev);
+		} else {
+			/* Interrupt handlers will timestamp for us. */
+			if (zpa2326_wait_oneshot_completion(indio_dev, priv))
+				goto out;
+		}
+	}
+
+	/* Enqueue to IIO buffer / userspace. */
+	zpa2326_fill_sample_buffer(indio_dev, priv);
+
+out:
+	if (!cont)
+		/* Don't switch to low power if sampling continuously. */
+		zpa2326_sleep(indio_dev);
+
+	/* Inform attached trigger we are done. */
+	iio_trigger_notify_done(indio_dev->trig);
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * zpa2326_preenable_buffer() - Prepare device for configuring triggered
+ *                              sampling
+ * modes.
+ * @indio_dev: The IIO device associated with the sampling hardware.
+ *
+ * Basically power up device.
+ * Called with IIO device's lock held.
+ *
+ * Return: Zero when successful, a negative error code otherwise.
+ */
+static int zpa2326_preenable_buffer(struct iio_dev *indio_dev)
+{
+	int ret = zpa2326_resume(indio_dev);
+
+	if (ret < 0)
+		return ret;
+
+	/* Tell zpa2326_postenable_buffer() if we have just been powered on. */
+	((struct zpa2326_private *)
+	 iio_priv(indio_dev))->waken = iio_priv(indio_dev);
+
+	return 0;
+}
+
+/**
+ * zpa2326_postenable_buffer() - Configure device for triggered sampling.
+ * @indio_dev: The IIO device associated with the sampling hardware.
+ *
+ * Basically setup one-shot mode if plugging external trigger.
+ * Otherwise, let internal trigger configure continuous sampling :
+ * see zpa2326_set_trigger_state().
+ *
+ * If an error is returned, IIO layer will call our postdisable hook for us,
+ * i.e. no need to explicitly power device off here.
+ * Called with IIO device's lock held.
+ *
+ * Called with IIO device's lock held.
+ *
+ * Return: Zero when successful, a negative error code otherwise.
+ */
+static int zpa2326_postenable_buffer(struct iio_dev *indio_dev)
+{
+	const struct zpa2326_private *priv = iio_priv(indio_dev);
+	int                           err;
+
+	if (!priv->waken) {
+		/*
+		 * We were already power supplied. Just clear hardware FIFO to
+		 * get rid of samples acquired during previous rounds (if any).
+		 */
+		err = zpa2326_clear_fifo(indio_dev, 0);
+		if (err)
+			goto err;
+	}
+
+	if (!iio_trigger_using_own(indio_dev) && priv->waken) {
+		/*
+		 * We are using an external trigger and we have just been
+		 * powered up: reconfigure one-shot mode.
+		 */
+		err = zpa2326_config_oneshot(indio_dev, priv->irq);
+		if (err)
+			goto err;
+	}
+
+	/* Plug our own trigger event handler. */
+	err = iio_triggered_buffer_postenable(indio_dev);
+	if (err)
+		goto err;
+
+	return 0;
+
+err:
+	zpa2326_err(indio_dev, "failed to enable buffering (%d)", err);
+
+	return err;
+}
+
+static int zpa2326_postdisable_buffer(struct iio_dev *indio_dev)
+{
+	zpa2326_suspend(indio_dev);
+
+	return 0;
+}
+
+static const struct iio_buffer_setup_ops zpa2326_buffer_setup_ops = {
+	.preenable   = zpa2326_preenable_buffer,
+	.postenable  = zpa2326_postenable_buffer,
+	.predisable  = iio_triggered_buffer_predisable,
+	.postdisable = zpa2326_postdisable_buffer
+};
+
+/**
+ * zpa2326_set_trigger_state() - Start / stop continuous sampling.
+ * @trig:  The trigger being attached to IIO device associated with the sampling
+ *         hardware.
+ * @state: Tell whether to start (true) or stop (false)
+ *
+ * Basically enable / disable hardware continuous sampling mode.
+ *
+ * Called with IIO device's lock held at postenable() or predisable() time.
+ *
+ * Return: Zero when successful, a negative error code otherwise.
+ */
+static int zpa2326_set_trigger_state(struct iio_trigger *trig, bool state)
+{
+	const struct iio_dev         *indio_dev = dev_get_drvdata(
+							trig->dev.parent);
+	const struct zpa2326_private *priv = iio_priv(indio_dev);
+	int                           err;
+
+	if (!state) {
+		/*
+		 * Switch trigger off : in case of failure, interrupt is left
+		 * disabled in order to prevent handler from accessing released
+		 * resources.
+		 */
+		unsigned int val;
+
+		/*
+		 * As device is working in continuous mode, handlers may be
+		 * accessing resources we are currently freeing...
+		 * Prevent this by disabling interrupt handlers and ensure
+		 * the device will generate no more interrupts unless explicitly
+		 * required to, i.e. by restoring back to default one shot mode.
+		 */
+		disable_irq(priv->irq);
+
+		/*
+		 * Disable continuous sampling mode to restore settings for
+		 * one shot / direct sampling operations.
+		 */
+		err = regmap_write(priv->regmap, ZPA2326_CTRL_REG3_REG,
+				   zpa2326_highest_frequency()->odr);
+		if (err)
+			return err;
+
+		/*
+		 * Now that device won't generate interrupts on its own,
+		 * acknowledge any currently active interrupts (may happen on
+		 * rare occasions while stopping continuous mode).
+		 */
+		err = regmap_read(priv->regmap, ZPA2326_INT_SOURCE_REG, &val);
+		if (err < 0)
+			return err;
+
+		/*
+		 * Re-enable interrupts only if we can guarantee the device will
+		 * generate no more interrupts to prevent handlers from
+		 * accessing released resources.
+		 */
+		enable_irq(priv->irq);
+
+		zpa2326_dbg(indio_dev, "continuous mode stopped");
+	} else {
+		/*
+		 * Switch trigger on : start continuous sampling at required
+		 * frequency.
+		 */
+
+		if (priv->waken) {
+			/* Enable interrupt if getting out of reset. */
+			err = regmap_write(priv->regmap, ZPA2326_CTRL_REG1_REG,
+					   (u8)
+					   ~ZPA2326_CTRL_REG1_MASK_DATA_READY);
+			if (err)
+				return err;
+		}
+
+		/* Enable continuous sampling at specified frequency. */
+		err = regmap_write(priv->regmap, ZPA2326_CTRL_REG3_REG,
+				   ZPA2326_CTRL_REG3_ENABLE_MEAS |
+				   priv->frequency->odr);
+		if (err)
+			return err;
+
+		zpa2326_dbg(indio_dev, "continuous mode setup @%dHz",
+			    priv->frequency->hz);
+	}
+
+	return 0;
+}
+
+static const struct iio_trigger_ops zpa2326_trigger_ops = {
+	.owner             = THIS_MODULE,
+	.set_trigger_state = zpa2326_set_trigger_state,
+};
+
+/**
+ * zpa2326_init_trigger() - Create an interrupt driven / hardware trigger
+ *                          allowing to notify external devices a new sample is
+ *                          ready.
+ * @parent:    Hardware sampling device @indio_dev is a child of.
+ * @indio_dev: The IIO device associated with the sampling hardware.
+ * @private:   Internal private state related to @indio_dev.
+ * @irq:       Optional interrupt line the hardware uses to notify new data
+ *             samples are ready. Negative or zero values indicate no interrupts
+ *             are available, meaning polling is required.
+ *
+ * Only relevant when DT declares a valid interrupt line.
+ *
+ * Return: Zero when successful, a negative error code otherwise.
+ */
+static int zpa2326_init_managed_trigger(struct device          *parent,
+					struct iio_dev         *indio_dev,
+					struct zpa2326_private *private,
+					int                     irq)
+{
+	struct iio_trigger *trigger;
+	int                 ret;
+
+	if (irq <= 0)
+		return 0;
+
+	trigger = devm_iio_trigger_alloc(parent, "%s-dev%d",
+					 indio_dev->name, indio_dev->id);
+	if (!trigger)
+		return -ENOMEM;
+
+	/* Basic setup. */
+	trigger->dev.parent = parent;
+	trigger->ops = &zpa2326_trigger_ops;
+
+	private->trigger = trigger;
+
+	/* Register to triggers space. */
+	ret = devm_iio_trigger_register(parent, trigger);
+	if (ret)
+		dev_err(parent, "failed to register hardware trigger (%d)",
+			ret);
+
+	return ret;
+}
+
+static int zpa2326_get_frequency(const struct iio_dev *indio_dev)
+{
+	return ((struct zpa2326_private *)iio_priv(indio_dev))->frequency->hz;
+}
+
+static int zpa2326_set_frequency(struct iio_dev *indio_dev, int hz)
+{
+	struct zpa2326_private *priv = iio_priv(indio_dev);
+	int                     freq;
+	int                     err;
+
+	/* Check if requested frequency is supported. */
+	for (freq = 0; freq < ARRAY_SIZE(zpa2326_sampling_frequencies); freq++)
+		if (zpa2326_sampling_frequencies[freq].hz == hz)
+			break;
+	if (freq == ARRAY_SIZE(zpa2326_sampling_frequencies))
+		return -EINVAL;
+
+	/* Don't allow changing frequency if buffered sampling is ongoing. */
+	err = iio_device_claim_direct_mode(indio_dev);
+	if (err)
+		return err;
+
+	priv->frequency = &zpa2326_sampling_frequencies[freq];
+
+	iio_device_release_direct_mode(indio_dev);
+
+	return 0;
+}
+
+/* Expose supported hardware sampling frequencies (Hz) through sysfs. */
+static IIO_CONST_ATTR_SAMP_FREQ_AVAIL("1 5 11 23");
+
+static struct attribute *zpa2326_attributes[] = {
+	&iio_const_attr_sampling_frequency_available.dev_attr.attr,
+	NULL
+};
+
+static const struct attribute_group zpa2326_attribute_group = {
+	.attrs = zpa2326_attributes,
+};
+
+static int zpa2326_read_raw(struct iio_dev             *indio_dev,
+			    struct iio_chan_spec const *chan,
+			    int                        *val,
+			    int                        *val2,
+			    long                        mask)
+{
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		return zpa2326_sample_oneshot(indio_dev, chan->type, val);
+
+	case IIO_CHAN_INFO_SCALE:
+		switch (chan->type) {
+		case IIO_PRESSURE:
+			/*
+			 * Pressure resolution is 1/64 Pascal. Scale to kPascal
+			 * as required by IIO ABI.
+			 */
+			*val = 1;
+			*val2 = 64000;
+			return IIO_VAL_FRACTIONAL;
+
+		case IIO_TEMP:
+			/*
+			 * Temperature follows the equation:
+			 *     Temp[degC] = Tempcode * 0.00649 - 176.83
+			 * where:
+			 *     Tempcode is composed the raw sampled 16 bits.
+			 *
+			 * Hence, to produce a temperature in milli-degrees
+			 * Celsius according to IIO ABI, we need to apply the
+			 * following equation to raw samples:
+			 *     Temp[milli degC] = (Tempcode + Offset) * Scale
+			 * where:
+			 *     Offset = -176.83 / 0.00649
+			 *     Scale = 0.00649 * 1000
+			 */
+			*val = 6;
+			*val2 = 490000;
+			return IIO_VAL_INT_PLUS_MICRO;
+
+		default:
+			return -EINVAL;
+		}
+
+	case IIO_CHAN_INFO_OFFSET:
+		switch (chan->type) {
+		case IIO_TEMP:
+			*val = -17683000;
+			*val2 = 649;
+			return IIO_VAL_FRACTIONAL;
+
+		default:
+			return -EINVAL;
+		}
+
+	case IIO_CHAN_INFO_SAMP_FREQ:
+		*val = zpa2326_get_frequency(indio_dev);
+		return IIO_VAL_INT;
+
+	default:
+		return -EINVAL;
+	}
+}
+
+static int zpa2326_write_raw(struct iio_dev             *indio_dev,
+			     const struct iio_chan_spec *chan,
+			     int                         val,
+			     int                         val2,
+			     long                        mask)
+{
+	if ((mask != IIO_CHAN_INFO_SAMP_FREQ) || val2)
+		return -EINVAL;
+
+	return zpa2326_set_frequency(indio_dev, val);
+}
+
+static const struct iio_chan_spec zpa2326_channels[] = {
+	[0] = {
+		.type                    = IIO_PRESSURE,
+		.scan_index              = 0,
+		.scan_type               = {
+			.sign                   = 'u',
+			.realbits               = 24,
+			.storagebits            = 32,
+			.endianness             = IIO_LE,
+		},
+		.info_mask_separate      = BIT(IIO_CHAN_INFO_RAW) |
+					   BIT(IIO_CHAN_INFO_SCALE),
+		.info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ),
+	},
+	[1] = {
+		.type                    = IIO_TEMP,
+		.scan_index              = 1,
+		.scan_type               = {
+			.sign                   = 's',
+			.realbits               = 16,
+			.storagebits            = 16,
+			.endianness             = IIO_LE,
+		},
+		.info_mask_separate      = BIT(IIO_CHAN_INFO_RAW) |
+					   BIT(IIO_CHAN_INFO_SCALE) |
+					   BIT(IIO_CHAN_INFO_OFFSET),
+		.info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ),
+	},
+	[2] = IIO_CHAN_SOFT_TIMESTAMP(2),
+};
+
+static const struct iio_info zpa2326_info = {
+	.driver_module = THIS_MODULE,
+	.attrs         = &zpa2326_attribute_group,
+	.read_raw      = zpa2326_read_raw,
+	.write_raw     = zpa2326_write_raw,
+};
+
+static struct iio_dev *zpa2326_create_managed_iiodev(struct device *device,
+						     const char    *name,
+						     struct regmap *regmap)
+{
+	struct iio_dev *indio_dev;
+
+	/* Allocate space to hold IIO device internal state. */
+	indio_dev = devm_iio_device_alloc(device,
+					  sizeof(struct zpa2326_private));
+	if (!indio_dev)
+		return NULL;
+
+	/* Setup for userspace synchronous on demand sampling. */
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->dev.parent = device;
+	indio_dev->channels = zpa2326_channels;
+	indio_dev->num_channels = ARRAY_SIZE(zpa2326_channels);
+	indio_dev->name = name;
+	indio_dev->info = &zpa2326_info;
+
+	return indio_dev;
+}
+
+int zpa2326_probe(struct device *parent,
+		  const char    *name,
+		  int            irq,
+		  unsigned int   hwid,
+		  struct regmap *regmap)
+{
+	struct iio_dev         *indio_dev;
+	struct zpa2326_private *priv;
+	int                     err;
+	unsigned int            id;
+
+	indio_dev = zpa2326_create_managed_iiodev(parent, name, regmap);
+	if (!indio_dev)
+		return -ENOMEM;
+
+	priv = iio_priv(indio_dev);
+
+	priv->vref = devm_regulator_get(parent, "vref");
+	if (IS_ERR(priv->vref))
+		return PTR_ERR(priv->vref);
+
+	priv->vdd = devm_regulator_get(parent, "vdd");
+	if (IS_ERR(priv->vdd))
+		return PTR_ERR(priv->vdd);
+
+	/* Set default hardware sampling frequency to highest rate supported. */
+	priv->frequency = zpa2326_highest_frequency();
+
+	/*
+	 * Plug device's underlying bus abstraction : this MUST be set before
+	 * registering interrupt handlers since an interrupt might happen if
+	 * power up sequence is not properly applied.
+	 */
+	priv->regmap = regmap;
+
+	err = devm_iio_triggered_buffer_setup(parent, indio_dev, NULL,
+					      zpa2326_trigger_handler,
+					      &zpa2326_buffer_setup_ops);
+	if (err)
+		return err;
+
+	err = zpa2326_init_managed_trigger(parent, indio_dev, priv, irq);
+	if (err)
+		return err;
+
+	err = zpa2326_init_managed_irq(parent, indio_dev, priv, irq);
+	if (err)
+		return err;
+
+	/* Power up to check device ID and perform initial hardware setup. */
+	err = zpa2326_power_on(indio_dev, priv);
+	if (err)
+		return err;
+
+	/* Read id register to check we are talking to the right slave. */
+	err = regmap_read(regmap, ZPA2326_DEVICE_ID_REG, &id);
+	if (err)
+		goto sleep;
+
+	if (id != hwid) {
+		dev_err(parent, "found device with unexpected id %02x", id);
+		err = -ENODEV;
+		goto sleep;
+	}
+
+	err = zpa2326_config_oneshot(indio_dev, irq);
+	if (err)
+		goto sleep;
+
+	/* Setup done : go sleeping. Device will be awaken upon user request. */
+	err = zpa2326_sleep(indio_dev);
+	if (err)
+		goto poweroff;
+
+	dev_set_drvdata(parent, indio_dev);
+
+	zpa2326_init_runtime(parent);
+
+	err = iio_device_register(indio_dev);
+	if (err) {
+		zpa2326_fini_runtime(parent);
+		goto poweroff;
+	}
+
+	return 0;
+
+sleep:
+	/* Put to sleep just in case power regulators are "dummy" ones. */
+	zpa2326_sleep(indio_dev);
+poweroff:
+	zpa2326_power_off(indio_dev, priv);
+
+	return err;
+}
+EXPORT_SYMBOL_GPL(zpa2326_probe);
+
+void zpa2326_remove(const struct device *parent)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(parent);
+
+	iio_device_unregister(indio_dev);
+	zpa2326_fini_runtime(indio_dev->dev.parent);
+	zpa2326_sleep(indio_dev);
+	zpa2326_power_off(indio_dev, iio_priv(indio_dev));
+}
+EXPORT_SYMBOL_GPL(zpa2326_remove);
+
+MODULE_AUTHOR("Gregor Boirie <gregor.boirie@parrot.com>");
+MODULE_DESCRIPTION("Core driver for Murata ZPA2326 pressure sensor");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/pressure/zpa2326.h b/drivers/iio/pressure/zpa2326.h
new file mode 100644
index 000000000000..05d3e1e3a449
--- /dev/null
+++ b/drivers/iio/pressure/zpa2326.h
@@ -0,0 +1,89 @@
+/*
+ * Murata ZPA2326 pressure and temperature sensor IIO driver
+ *
+ * Copyright (c) 2016 Parrot S.A.
+ *
+ * Author: Gregor Boirie <gregor.boirie@parrot.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 _ZPA2326_H
+#define _ZPA2326_H
+
+/* Register map. */
+#define ZPA2326_REF_P_XL_REG              (0x8)
+#define ZPA2326_REF_P_L_REG               (0x9)
+#define ZPA2326_REF_P_H_REG               (0xa)
+#define ZPA2326_DEVICE_ID_REG             (0xf)
+#define ZPA2326_DEVICE_ID                 (0xb9)
+#define ZPA2326_RES_CONF_REG              (0x10)
+#define ZPA2326_CTRL_REG0_REG             (0x20)
+#define ZPA2326_CTRL_REG0_ONE_SHOT        BIT(0)
+#define ZPA2326_CTRL_REG0_ENABLE          BIT(1)
+#define ZPA2326_CTRL_REG1_REG             (0x21)
+#define ZPA2326_CTRL_REG1_MASK_DATA_READY BIT(2)
+#define ZPA2326_CTRL_REG2_REG             (0x22)
+#define ZPA2326_CTRL_REG2_SWRESET         BIT(2)
+#define ZPA2326_CTRL_REG3_REG             (0x23)
+#define ZPA2326_CTRL_REG3_ODR_SHIFT       (4)
+#define ZPA2326_CTRL_REG3_ENABLE_MEAS     BIT(7)
+#define ZPA2326_INT_SOURCE_REG            (0x24)
+#define ZPA2326_INT_SOURCE_DATA_READY     BIT(2)
+#define ZPA2326_THS_P_LOW_REG             (0x25)
+#define ZPA2326_THS_P_HIGH_REG            (0x26)
+#define ZPA2326_STATUS_REG                (0x27)
+#define ZPA2326_STATUS_P_DA               BIT(1)
+#define ZPA2326_STATUS_FIFO_E             BIT(2)
+#define ZPA2326_STATUS_P_OR               BIT(5)
+#define ZPA2326_PRESS_OUT_XL_REG          (0x28)
+#define ZPA2326_PRESS_OUT_L_REG           (0x29)
+#define ZPA2326_PRESS_OUT_H_REG           (0x2a)
+#define ZPA2326_TEMP_OUT_L_REG            (0x2b)
+#define ZPA2326_TEMP_OUT_H_REG            (0x2c)
+
+struct device;
+struct regmap;
+
+bool zpa2326_isreg_writeable(struct device *dev, unsigned int reg);
+bool zpa2326_isreg_readable(struct device *dev, unsigned int reg);
+bool zpa2326_isreg_precious(struct device *dev, unsigned int reg);
+
+/**
+ * zpa2326_probe() - Instantiate and register core ZPA2326 IIO device
+ * @parent: Hardware sampling device the created IIO device will be a child of.
+ * @name:   Arbitrary name to identify the device.
+ * @irq:    Interrupt line, negative if none.
+ * @hwid:   Expected device hardware id.
+ * @regmap: Registers map used to abstract underlying bus accesses.
+ *
+ * Return: Zero when successful, a negative error code otherwise.
+ */
+int zpa2326_probe(struct device        *parent,
+		  const char           *name,
+		  int                   irq,
+		  unsigned int          hwid,
+		  struct regmap        *regmap);
+
+/**
+ * zpa2326_remove() - Unregister and destroy core ZPA2326 IIO device.
+ * @parent: Hardware sampling device the IIO device to remove is a child of.
+ */
+void zpa2326_remove(const struct device *parent);
+
+#ifdef CONFIG_PM
+#include <linux/pm.h>
+extern const struct dev_pm_ops zpa2326_pm_ops;
+#define ZPA2326_PM_OPS (&zpa2326_pm_ops)
+#else
+#define ZPA2326_PM_OPS (NULL)
+#endif
+
+#endif
diff --git a/drivers/iio/pressure/zpa2326_i2c.c b/drivers/iio/pressure/zpa2326_i2c.c
new file mode 100644
index 000000000000..e4d27dd4493a
--- /dev/null
+++ b/drivers/iio/pressure/zpa2326_i2c.c
@@ -0,0 +1,99 @@
+/*
+ * Murata ZPA2326 I2C pressure and temperature sensor driver
+ *
+ * Copyright (c) 2016 Parrot S.A.
+ *
+ * Author: Gregor Boirie <gregor.boirie@parrot.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/regmap.h>
+#include <linux/i2c.h>
+#include <linux/of_device.h>
+#include "zpa2326.h"
+
+/*
+ * read_flag_mask:
+ *   - address bit 7 must be set to request a register read operation
+ */
+static const struct regmap_config zpa2326_regmap_i2c_config = {
+	.reg_bits       = 8,
+	.val_bits       = 8,
+	.writeable_reg  = zpa2326_isreg_writeable,
+	.readable_reg   = zpa2326_isreg_readable,
+	.precious_reg   = zpa2326_isreg_precious,
+	.max_register   = ZPA2326_TEMP_OUT_H_REG,
+	.read_flag_mask = BIT(7),
+	.cache_type     = REGCACHE_NONE,
+};
+
+static unsigned int zpa2326_i2c_hwid(const struct i2c_client *client)
+{
+#define ZPA2326_SA0(_addr)          (_addr & BIT(0))
+#define ZPA2326_DEVICE_ID_SA0_SHIFT (1)
+
+	/* Identification register bit 1 mirrors device address bit 0. */
+	return (ZPA2326_DEVICE_ID |
+		(ZPA2326_SA0(client->addr) << ZPA2326_DEVICE_ID_SA0_SHIFT));
+}
+
+static int zpa2326_probe_i2c(struct i2c_client          *client,
+			     const struct i2c_device_id *i2c_id)
+{
+	struct regmap *regmap;
+
+	regmap = devm_regmap_init_i2c(client, &zpa2326_regmap_i2c_config);
+	if (IS_ERR(regmap)) {
+		dev_err(&client->dev, "failed to init registers map");
+		return PTR_ERR(regmap);
+	}
+
+	return zpa2326_probe(&client->dev, i2c_id->name, client->irq,
+			     zpa2326_i2c_hwid(client), regmap);
+}
+
+static int zpa2326_remove_i2c(struct i2c_client *client)
+{
+	zpa2326_remove(&client->dev);
+
+	return 0;
+}
+
+static const struct i2c_device_id zpa2326_i2c_ids[] = {
+	{ "zpa2326", 0 },
+	{ },
+};
+MODULE_DEVICE_TABLE(i2c, zpa2326_i2c_ids);
+
+#if defined(CONFIG_OF)
+static const struct of_device_id zpa2326_i2c_matches[] = {
+	{ .compatible = "murata,zpa2326" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, zpa2326_i2c_matches);
+#endif
+
+static struct i2c_driver zpa2326_i2c_driver = {
+	.driver = {
+		.name           = "zpa2326-i2c",
+		.of_match_table = of_match_ptr(zpa2326_i2c_matches),
+		.pm             = ZPA2326_PM_OPS,
+	},
+	.probe    = zpa2326_probe_i2c,
+	.remove   = zpa2326_remove_i2c,
+	.id_table = zpa2326_i2c_ids,
+};
+module_i2c_driver(zpa2326_i2c_driver);
+
+MODULE_AUTHOR("Gregor Boirie <gregor.boirie@parrot.com>");
+MODULE_DESCRIPTION("I2C driver for Murata ZPA2326 pressure sensor");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/pressure/zpa2326_spi.c b/drivers/iio/pressure/zpa2326_spi.c
new file mode 100644
index 000000000000..bd2c1c319fca
--- /dev/null
+++ b/drivers/iio/pressure/zpa2326_spi.c
@@ -0,0 +1,103 @@
+/*
+ * Murata ZPA2326 SPI pressure and temperature sensor driver
+ *
+ * Copyright (c) 2016 Parrot S.A.
+ *
+ * Author: Gregor Boirie <gregor.boirie@parrot.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/regmap.h>
+#include <linux/spi/spi.h>
+#include <linux/of_device.h>
+#include "zpa2326.h"
+
+/*
+ * read_flag_mask:
+ *   - address bit 7 must be set to request a register read operation
+ *   - address bit 6 must be set to request register address auto increment
+ */
+static const struct regmap_config zpa2326_regmap_spi_config = {
+	.reg_bits       = 8,
+	.val_bits       = 8,
+	.writeable_reg  = zpa2326_isreg_writeable,
+	.readable_reg   = zpa2326_isreg_readable,
+	.precious_reg   = zpa2326_isreg_precious,
+	.max_register   = ZPA2326_TEMP_OUT_H_REG,
+	.read_flag_mask = BIT(7) | BIT(6),
+	.cache_type     = REGCACHE_NONE,
+};
+
+static int zpa2326_probe_spi(struct spi_device *spi)
+{
+	struct regmap *regmap;
+	int            err;
+
+	regmap = devm_regmap_init_spi(spi, &zpa2326_regmap_spi_config);
+	if (IS_ERR(regmap)) {
+		dev_err(&spi->dev, "failed to init registers map");
+		return PTR_ERR(regmap);
+	}
+
+	/*
+	 * Enforce SPI slave settings to prevent from DT misconfiguration.
+	 *
+	 * Clock is idle high. Sampling happens on trailing edge, i.e., rising
+	 * edge. Maximum bus frequency is 1 MHz. Registers are 8 bits wide.
+	 */
+	spi->mode = SPI_MODE_3;
+	spi->max_speed_hz = min(spi->max_speed_hz, 1000000U);
+	spi->bits_per_word = 8;
+	err = spi_setup(spi);
+	if (err < 0)
+		return err;
+
+	return zpa2326_probe(&spi->dev, spi_get_device_id(spi)->name,
+			     spi->irq, ZPA2326_DEVICE_ID, regmap);
+}
+
+static int zpa2326_remove_spi(struct spi_device *spi)
+{
+	zpa2326_remove(&spi->dev);
+
+	return 0;
+}
+
+static const struct spi_device_id zpa2326_spi_ids[] = {
+	{ "zpa2326", 0 },
+	{ },
+};
+MODULE_DEVICE_TABLE(spi, zpa2326_spi_ids);
+
+#if defined(CONFIG_OF)
+static const struct of_device_id zpa2326_spi_matches[] = {
+	{ .compatible = "murata,zpa2326" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, zpa2326_spi_matches);
+#endif
+
+static struct spi_driver zpa2326_spi_driver = {
+	.driver = {
+		.name           = "zpa2326-spi",
+		.of_match_table = of_match_ptr(zpa2326_spi_matches),
+		.pm             = ZPA2326_PM_OPS,
+	},
+	.probe    = zpa2326_probe_spi,
+	.remove   = zpa2326_remove_spi,
+	.id_table = zpa2326_spi_ids,
+};
+module_spi_driver(zpa2326_spi_driver);
+
+MODULE_AUTHOR("Gregor Boirie <gregor.boirie@parrot.com>");
+MODULE_DESCRIPTION("SPI driver for Murata ZPA2326 pressure sensor");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/proximity/sx9500.c b/drivers/iio/proximity/sx9500.c
index 1d74b3aafeed..1f06282ec793 100644
--- a/drivers/iio/proximity/sx9500.c
+++ b/drivers/iio/proximity/sx9500.c
@@ -516,7 +516,7 @@ static irqreturn_t sx9500_irq_thread_handler(int irq, void *private)
 		sx9500_push_events(indio_dev);
 
 	if (val & SX9500_CONVDONE_IRQ)
-		complete_all(&data->completion);
+		complete(&data->completion);
 
 out:
 	mutex_unlock(&data->mutex);
@@ -1025,6 +1025,12 @@ static const struct acpi_device_id sx9500_acpi_match[] = {
 };
 MODULE_DEVICE_TABLE(acpi, sx9500_acpi_match);
 
+static const struct of_device_id sx9500_of_match[] = {
+	{ .compatible = "semtech,sx9500", },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, sx9500_of_match);
+
 static const struct i2c_device_id sx9500_id[] = {
 	{"sx9500", 0},
 	{ },
@@ -1035,6 +1041,7 @@ static struct i2c_driver sx9500_driver = {
 	.driver = {
 		.name	= SX9500_DRIVER_NAME,
 		.acpi_match_table = ACPI_PTR(sx9500_acpi_match),
+		.of_match_table = of_match_ptr(sx9500_of_match),
 		.pm = &sx9500_pm_ops,
 	},
 	.probe		= sx9500_probe,
diff --git a/drivers/iio/temperature/Kconfig b/drivers/iio/temperature/Kconfig
index c4664e5de791..5ea77a7e261d 100644
--- a/drivers/iio/temperature/Kconfig
+++ b/drivers/iio/temperature/Kconfig
@@ -3,6 +3,22 @@
 #
 menu "Temperature sensors"
 
+config MAXIM_THERMOCOUPLE
+	tristate "Maxim thermocouple sensors"
+	depends on SPI
+	select IIO_BUFFER
+	select IIO_TRIGGERED_BUFFER
+	help
+	  If you say yes here you get support for the Maxim series of
+	  thermocouple sensors connected via SPI.
+
+	  Supported sensors:
+	   * MAX6675
+	   * MAX31855
+
+	  This driver can also be built as a module. If so, the module will
+	  be called maxim_thermocouple.
+
 config MLX90614
 	tristate "MLX90614 contact-less infrared sensor"
 	depends on I2C
diff --git a/drivers/iio/temperature/Makefile b/drivers/iio/temperature/Makefile
index 02bc79d49b24..78c3de0dc3f0 100644
--- a/drivers/iio/temperature/Makefile
+++ b/drivers/iio/temperature/Makefile
@@ -2,6 +2,7 @@
 # Makefile for industrial I/O temperature drivers
 #
 
+obj-$(CONFIG_MAXIM_THERMOCOUPLE) += maxim_thermocouple.o
 obj-$(CONFIG_MLX90614) += mlx90614.o
 obj-$(CONFIG_TMP006) += tmp006.o
 obj-$(CONFIG_TSYS01) += tsys01.o
diff --git a/drivers/iio/temperature/maxim_thermocouple.c b/drivers/iio/temperature/maxim_thermocouple.c
new file mode 100644
index 000000000000..39dd2026ccc9
--- /dev/null
+++ b/drivers/iio/temperature/maxim_thermocouple.c
@@ -0,0 +1,281 @@
+/*
+ * maxim_thermocouple.c  - Support for Maxim thermocouple chips
+ *
+ * Copyright (C) 2016 Matt Ranostay <mranostay@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/init.h>
+#include <linux/mutex.h>
+#include <linux/err.h>
+#include <linux/spi/spi.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/trigger.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/triggered_buffer.h>
+#include <linux/iio/trigger_consumer.h>
+
+#define MAXIM_THERMOCOUPLE_DRV_NAME	"maxim_thermocouple"
+
+enum {
+	MAX6675,
+	MAX31855,
+};
+
+static const struct iio_chan_spec max6675_channels[] = {
+	{	/* thermocouple temperature */
+		.type = IIO_TEMP,
+		.info_mask_separate =
+			BIT(IIO_CHAN_INFO_RAW) | BIT(IIO_CHAN_INFO_SCALE),
+		.scan_index = 0,
+		.scan_type = {
+			.sign = 's',
+			.realbits = 13,
+			.storagebits = 16,
+			.shift = 3,
+			.endianness = IIO_BE,
+		},
+	},
+	IIO_CHAN_SOFT_TIMESTAMP(1),
+};
+
+static const struct iio_chan_spec max31855_channels[] = {
+	{	/* thermocouple temperature */
+		.type = IIO_TEMP,
+		.address = 2,
+		.info_mask_separate =
+			BIT(IIO_CHAN_INFO_RAW) | BIT(IIO_CHAN_INFO_SCALE),
+		.scan_index = 0,
+		.scan_type = {
+			.sign = 's',
+			.realbits = 14,
+			.storagebits = 16,
+			.shift = 2,
+			.endianness = IIO_BE,
+		},
+	},
+	{	/* cold junction temperature */
+		.type = IIO_TEMP,
+		.address = 0,
+		.channel2 = IIO_MOD_TEMP_AMBIENT,
+		.modified = 1,
+		.info_mask_separate =
+			BIT(IIO_CHAN_INFO_RAW) | BIT(IIO_CHAN_INFO_SCALE),
+		.scan_index = 1,
+		.scan_type = {
+			.sign = 's',
+			.realbits = 12,
+			.storagebits = 16,
+			.shift = 4,
+			.endianness = IIO_BE,
+		},
+	},
+	IIO_CHAN_SOFT_TIMESTAMP(2),
+};
+
+static const unsigned long max31855_scan_masks[] = {0x3, 0};
+
+struct maxim_thermocouple_chip {
+	const struct iio_chan_spec *channels;
+	const unsigned long *scan_masks;
+	u8 num_channels;
+	u8 read_size;
+
+	/* bit-check for valid input */
+	u32 status_bit;
+};
+
+static const struct maxim_thermocouple_chip maxim_thermocouple_chips[] = {
+	[MAX6675] = {
+			.channels = max6675_channels,
+			.num_channels = ARRAY_SIZE(max6675_channels),
+			.read_size = 2,
+			.status_bit = BIT(2),
+		},
+	[MAX31855] = {
+			.channels = max31855_channels,
+			.num_channels = ARRAY_SIZE(max31855_channels),
+			.read_size = 4,
+			.scan_masks = max31855_scan_masks,
+			.status_bit = BIT(16),
+		},
+};
+
+struct maxim_thermocouple_data {
+	struct spi_device *spi;
+	const struct maxim_thermocouple_chip *chip;
+
+	u8 buffer[16] ____cacheline_aligned;
+};
+
+static int maxim_thermocouple_read(struct maxim_thermocouple_data *data,
+				   struct iio_chan_spec const *chan, int *val)
+{
+	unsigned int storage_bytes = data->chip->read_size;
+	unsigned int shift = chan->scan_type.shift + (chan->address * 8);
+	unsigned int buf;
+	int ret;
+
+	ret = spi_read(data->spi, (void *) &buf, storage_bytes);
+	if (ret)
+		return ret;
+
+	switch (storage_bytes) {
+	case 2:
+		*val = be16_to_cpu(buf);
+		break;
+	case 4:
+		*val = be32_to_cpu(buf);
+		break;
+	}
+
+	/* check to be sure this is a valid reading */
+	if (*val & data->chip->status_bit)
+		return -EINVAL;
+
+	*val = sign_extend32(*val >> shift, chan->scan_type.realbits - 1);
+
+	return 0;
+}
+
+static irqreturn_t maxim_thermocouple_trigger_handler(int irq, void *private)
+{
+	struct iio_poll_func *pf = private;
+	struct iio_dev *indio_dev = pf->indio_dev;
+	struct maxim_thermocouple_data *data = iio_priv(indio_dev);
+	int ret;
+
+	ret = spi_read(data->spi, data->buffer, data->chip->read_size);
+	if (!ret) {
+		iio_push_to_buffers_with_timestamp(indio_dev, data->buffer,
+						   iio_get_time_ns(indio_dev));
+	}
+
+	iio_trigger_notify_done(indio_dev->trig);
+
+	return IRQ_HANDLED;
+}
+
+static int maxim_thermocouple_read_raw(struct iio_dev *indio_dev,
+				       struct iio_chan_spec const *chan,
+				       int *val, int *val2, long mask)
+{
+	struct maxim_thermocouple_data *data = iio_priv(indio_dev);
+	int ret = -EINVAL;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		ret = iio_device_claim_direct_mode(indio_dev);
+		if (ret)
+			return ret;
+
+		ret = maxim_thermocouple_read(data, chan, val);
+		iio_device_release_direct_mode(indio_dev);
+
+		if (!ret)
+			return IIO_VAL_INT;
+
+		break;
+	case IIO_CHAN_INFO_SCALE:
+		switch (chan->channel2) {
+		case IIO_MOD_TEMP_AMBIENT:
+			*val = 62;
+			*val2 = 500000; /* 1000 * 0.0625 */
+			ret = IIO_VAL_INT_PLUS_MICRO;
+			break;
+		default:
+			*val = 250; /* 1000 * 0.25 */
+			ret = IIO_VAL_INT;
+		};
+		break;
+	}
+
+	return ret;
+}
+
+static const struct iio_info maxim_thermocouple_info = {
+	.driver_module = THIS_MODULE,
+	.read_raw = maxim_thermocouple_read_raw,
+};
+
+static int maxim_thermocouple_probe(struct spi_device *spi)
+{
+	const struct spi_device_id *id = spi_get_device_id(spi);
+	struct iio_dev *indio_dev;
+	struct maxim_thermocouple_data *data;
+	const struct maxim_thermocouple_chip *chip =
+			&maxim_thermocouple_chips[id->driver_data];
+	int ret;
+
+	indio_dev = devm_iio_device_alloc(&spi->dev, sizeof(*data));
+	if (!indio_dev)
+		return -ENOMEM;
+
+	indio_dev->info = &maxim_thermocouple_info;
+	indio_dev->name = MAXIM_THERMOCOUPLE_DRV_NAME;
+	indio_dev->channels = chip->channels;
+	indio_dev->available_scan_masks = chip->scan_masks;
+	indio_dev->num_channels = chip->num_channels;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+
+	data = iio_priv(indio_dev);
+	data->spi = spi;
+	data->chip = chip;
+
+	ret = iio_triggered_buffer_setup(indio_dev, NULL,
+				maxim_thermocouple_trigger_handler, NULL);
+	if (ret)
+		return ret;
+
+	ret = iio_device_register(indio_dev);
+	if (ret)
+		goto error_unreg_buffer;
+
+	return 0;
+
+error_unreg_buffer:
+	iio_triggered_buffer_cleanup(indio_dev);
+
+	return ret;
+}
+
+static int maxim_thermocouple_remove(struct spi_device *spi)
+{
+	struct iio_dev *indio_dev = spi_get_drvdata(spi);
+
+	iio_device_unregister(indio_dev);
+	iio_triggered_buffer_cleanup(indio_dev);
+
+	return 0;
+}
+
+static const struct spi_device_id maxim_thermocouple_id[] = {
+	{"max6675", MAX6675},
+	{"max31855", MAX31855},
+	{},
+};
+MODULE_DEVICE_TABLE(spi, maxim_thermocouple_id);
+
+static struct spi_driver maxim_thermocouple_driver = {
+	.driver = {
+		.name	= MAXIM_THERMOCOUPLE_DRV_NAME,
+	},
+	.probe		= maxim_thermocouple_probe,
+	.remove		= maxim_thermocouple_remove,
+	.id_table	= maxim_thermocouple_id,
+};
+module_spi_driver(maxim_thermocouple_driver);
+
+MODULE_AUTHOR("Matt Ranostay <mranostay@gmail.com>");
+MODULE_DESCRIPTION("Maxim thermocouple sensors");
+MODULE_LICENSE("GPL");
diff --git a/drivers/infiniband/Kconfig b/drivers/infiniband/Kconfig
index e9b7dc037ff8..fb3fb89640e5 100644
--- a/drivers/infiniband/Kconfig
+++ b/drivers/infiniband/Kconfig
@@ -74,6 +74,7 @@ source "drivers/infiniband/hw/mlx5/Kconfig"
 source "drivers/infiniband/hw/nes/Kconfig"
 source "drivers/infiniband/hw/ocrdma/Kconfig"
 source "drivers/infiniband/hw/usnic/Kconfig"
+source "drivers/infiniband/hw/hns/Kconfig"
 
 source "drivers/infiniband/ulp/ipoib/Kconfig"
 
@@ -88,4 +89,6 @@ source "drivers/infiniband/sw/rxe/Kconfig"
 
 source "drivers/infiniband/hw/hfi1/Kconfig"
 
+source "drivers/infiniband/hw/qedr/Kconfig"
+
 endif # INFINIBAND
diff --git a/drivers/infiniband/core/addr.c b/drivers/infiniband/core/addr.c
index 1374541a4528..b136d3acc5bd 100644
--- a/drivers/infiniband/core/addr.c
+++ b/drivers/infiniband/core/addr.c
@@ -800,7 +800,7 @@ static struct notifier_block nb = {
 
 int addr_init(void)
 {
-	addr_wq = create_singlethread_workqueue("ib_addr");
+	addr_wq = alloc_workqueue("ib_addr", WQ_MEM_RECLAIM, 0);
 	if (!addr_wq)
 		return -ENOMEM;
 
diff --git a/drivers/infiniband/core/cma.c b/drivers/infiniband/core/cma.c
index 5f65a78b27c9..36bf50ebb187 100644
--- a/drivers/infiniband/core/cma.c
+++ b/drivers/infiniband/core/cma.c
@@ -4369,7 +4369,7 @@ static int __init cma_init(void)
 {
 	int ret;
 
-	cma_wq = create_singlethread_workqueue("rdma_cm");
+	cma_wq = alloc_ordered_workqueue("rdma_cm", WQ_MEM_RECLAIM);
 	if (!cma_wq)
 		return -ENOMEM;
 
diff --git a/drivers/infiniband/core/iwcm.c b/drivers/infiniband/core/iwcm.c
index 357624f8b9d3..5495e22839a7 100644
--- a/drivers/infiniband/core/iwcm.c
+++ b/drivers/infiniband/core/iwcm.c
@@ -1160,7 +1160,7 @@ static int __init iw_cm_init(void)
 	if (ret)
 		pr_err("iw_cm: couldn't register netlink callbacks\n");
 
-	iwcm_wq = create_singlethread_workqueue("iw_cm_wq");
+	iwcm_wq = alloc_ordered_workqueue("iw_cm_wq", WQ_MEM_RECLAIM);
 	if (!iwcm_wq)
 		return -ENOMEM;
 
diff --git a/drivers/infiniband/core/mad.c b/drivers/infiniband/core/mad.c
index 2d49228f28b2..40cbd6bdb73b 100644
--- a/drivers/infiniband/core/mad.c
+++ b/drivers/infiniband/core/mad.c
@@ -3160,7 +3160,7 @@ static int ib_mad_port_open(struct ib_device *device,
 		goto error3;
 	}
 
-	port_priv->pd = ib_alloc_pd(device);
+	port_priv->pd = ib_alloc_pd(device, 0);
 	if (IS_ERR(port_priv->pd)) {
 		dev_err(&device->dev, "Couldn't create ib_mad PD\n");
 		ret = PTR_ERR(port_priv->pd);
@@ -3177,7 +3177,7 @@ static int ib_mad_port_open(struct ib_device *device,
 		goto error7;
 
 	snprintf(name, sizeof name, "ib_mad%d", port_num);
-	port_priv->wq = create_singlethread_workqueue(name);
+	port_priv->wq = alloc_ordered_workqueue(name, WQ_MEM_RECLAIM);
 	if (!port_priv->wq) {
 		ret = -ENOMEM;
 		goto error8;
diff --git a/drivers/infiniband/core/multicast.c b/drivers/infiniband/core/multicast.c
index 51c79b2fb0b8..e51b739f6ea3 100644
--- a/drivers/infiniband/core/multicast.c
+++ b/drivers/infiniband/core/multicast.c
@@ -873,7 +873,7 @@ int mcast_init(void)
 {
 	int ret;
 
-	mcast_wq = create_singlethread_workqueue("ib_mcast");
+	mcast_wq = alloc_ordered_workqueue("ib_mcast", WQ_MEM_RECLAIM);
 	if (!mcast_wq)
 		return -ENOMEM;
 
diff --git a/drivers/infiniband/core/sa_query.c b/drivers/infiniband/core/sa_query.c
index b9bf7aa055e7..81b742ca1639 100644
--- a/drivers/infiniband/core/sa_query.c
+++ b/drivers/infiniband/core/sa_query.c
@@ -2015,7 +2015,7 @@ int ib_sa_init(void)
 		goto err2;
 	}
 
-	ib_nl_wq = create_singlethread_workqueue("ib_nl_sa_wq");
+	ib_nl_wq = alloc_ordered_workqueue("ib_nl_sa_wq", WQ_MEM_RECLAIM);
 	if (!ib_nl_wq) {
 		ret = -ENOMEM;
 		goto err3;
diff --git a/drivers/infiniband/core/sysfs.c b/drivers/infiniband/core/sysfs.c
index 15defefecb4f..c1fb545e8d78 100644
--- a/drivers/infiniband/core/sysfs.c
+++ b/drivers/infiniband/core/sysfs.c
@@ -1193,7 +1193,7 @@ static ssize_t set_node_desc(struct device *device,
 	if (!dev->modify_device)
 		return -EIO;
 
-	memcpy(desc.node_desc, buf, min_t(int, count, 64));
+	memcpy(desc.node_desc, buf, min_t(int, count, IB_DEVICE_NODE_DESC_MAX));
 	ret = ib_modify_device(dev, IB_DEVICE_MODIFY_NODE_DESC, &desc);
 	if (ret)
 		return ret;
diff --git a/drivers/infiniband/core/ucma.c b/drivers/infiniband/core/ucma.c
index 2825ece91d3c..9520154f1d7c 100644
--- a/drivers/infiniband/core/ucma.c
+++ b/drivers/infiniband/core/ucma.c
@@ -1638,7 +1638,8 @@ static int ucma_open(struct inode *inode, struct file *filp)
 	if (!file)
 		return -ENOMEM;
 
-	file->close_wq = create_singlethread_workqueue("ucma_close_id");
+	file->close_wq = alloc_ordered_workqueue("ucma_close_id",
+						 WQ_MEM_RECLAIM);
 	if (!file->close_wq) {
 		kfree(file);
 		return -ENOMEM;
diff --git a/drivers/infiniband/core/umem.c b/drivers/infiniband/core/umem.c
index c68746ce6624..224ad274ea0b 100644
--- a/drivers/infiniband/core/umem.c
+++ b/drivers/infiniband/core/umem.c
@@ -94,6 +94,7 @@ struct ib_umem *ib_umem_get(struct ib_ucontext *context, unsigned long addr,
 	unsigned long dma_attrs = 0;
 	struct scatterlist *sg, *sg_list_start;
 	int need_release = 0;
+	unsigned int gup_flags = FOLL_WRITE;
 
 	if (dmasync)
 		dma_attrs |= DMA_ATTR_WRITE_BARRIER;
@@ -183,6 +184,9 @@ struct ib_umem *ib_umem_get(struct ib_ucontext *context, unsigned long addr,
 	if (ret)
 		goto out;
 
+	if (!umem->writable)
+		gup_flags |= FOLL_FORCE;
+
 	need_release = 1;
 	sg_list_start = umem->sg_head.sgl;
 
@@ -190,7 +194,7 @@ struct ib_umem *ib_umem_get(struct ib_ucontext *context, unsigned long addr,
 		ret = get_user_pages(cur_base,
 				     min_t(unsigned long, npages,
 					   PAGE_SIZE / sizeof (struct page *)),
-				     1, !umem->writable, page_list, vma_list);
+				     gup_flags, page_list, vma_list);
 
 		if (ret < 0)
 			goto out;
diff --git a/drivers/infiniband/core/umem_odp.c b/drivers/infiniband/core/umem_odp.c
index 75077a018675..1f0fe3217f23 100644
--- a/drivers/infiniband/core/umem_odp.c
+++ b/drivers/infiniband/core/umem_odp.c
@@ -527,6 +527,7 @@ int ib_umem_odp_map_dma_pages(struct ib_umem *umem, u64 user_virt, u64 bcnt,
 	u64 off;
 	int j, k, ret = 0, start_idx, npages = 0;
 	u64 base_virt_addr;
+	unsigned int flags = 0;
 
 	if (access_mask == 0)
 		return -EINVAL;
@@ -556,6 +557,9 @@ int ib_umem_odp_map_dma_pages(struct ib_umem *umem, u64 user_virt, u64 bcnt,
 		goto out_put_task;
 	}
 
+	if (access_mask & ODP_WRITE_ALLOWED_BIT)
+		flags |= FOLL_WRITE;
+
 	start_idx = (user_virt - ib_umem_start(umem)) >> PAGE_SHIFT;
 	k = start_idx;
 
@@ -574,8 +578,7 @@ int ib_umem_odp_map_dma_pages(struct ib_umem *umem, u64 user_virt, u64 bcnt,
 		 */
 		npages = get_user_pages_remote(owning_process, owning_mm,
 				user_virt, gup_num_pages,
-				access_mask & ODP_WRITE_ALLOWED_BIT,
-				0, local_page_list, NULL);
+				flags, local_page_list, NULL);
 		up_read(&owning_mm->mmap_sem);
 
 		if (npages < 0)
diff --git a/drivers/infiniband/core/uverbs_cmd.c b/drivers/infiniband/core/uverbs_cmd.c
index f6647318138d..cb3f515a2285 100644
--- a/drivers/infiniband/core/uverbs_cmd.c
+++ b/drivers/infiniband/core/uverbs_cmd.c
@@ -571,7 +571,7 @@ ssize_t ib_uverbs_alloc_pd(struct ib_uverbs_file *file,
 
 	pd->device  = ib_dev;
 	pd->uobject = uobj;
-	pd->local_mr = NULL;
+	pd->__internal_mr = NULL;
 	atomic_set(&pd->usecnt, 0);
 
 	uobj->object = pd;
@@ -3078,51 +3078,102 @@ out_put:
 	return ret ? ret : in_len;
 }
 
+static size_t kern_spec_filter_sz(struct ib_uverbs_flow_spec_hdr *spec)
+{
+	/* Returns user space filter size, includes padding */
+	return (spec->size - sizeof(struct ib_uverbs_flow_spec_hdr)) / 2;
+}
+
+static ssize_t spec_filter_size(void *kern_spec_filter, u16 kern_filter_size,
+				u16 ib_real_filter_sz)
+{
+	/*
+	 * User space filter structures must be 64 bit aligned, otherwise this
+	 * may pass, but we won't handle additional new attributes.
+	 */
+
+	if (kern_filter_size > ib_real_filter_sz) {
+		if (memchr_inv(kern_spec_filter +
+			       ib_real_filter_sz, 0,
+			       kern_filter_size - ib_real_filter_sz))
+			return -EINVAL;
+		return ib_real_filter_sz;
+	}
+	return kern_filter_size;
+}
+
 static int kern_spec_to_ib_spec(struct ib_uverbs_flow_spec *kern_spec,
 				union ib_flow_spec *ib_spec)
 {
+	ssize_t actual_filter_sz;
+	ssize_t kern_filter_sz;
+	ssize_t ib_filter_sz;
+	void *kern_spec_mask;
+	void *kern_spec_val;
+
 	if (kern_spec->reserved)
 		return -EINVAL;
 
 	ib_spec->type = kern_spec->type;
 
+	kern_filter_sz = kern_spec_filter_sz(&kern_spec->hdr);
+	/* User flow spec size must be aligned to 4 bytes */
+	if (kern_filter_sz != ALIGN(kern_filter_sz, 4))
+		return -EINVAL;
+
+	kern_spec_val = (void *)kern_spec +
+		sizeof(struct ib_uverbs_flow_spec_hdr);
+	kern_spec_mask = kern_spec_val + kern_filter_sz;
+
 	switch (ib_spec->type) {
 	case IB_FLOW_SPEC_ETH:
-		ib_spec->eth.size = sizeof(struct ib_flow_spec_eth);
-		if (ib_spec->eth.size != kern_spec->eth.size)
+		ib_filter_sz = offsetof(struct ib_flow_eth_filter, real_sz);
+		actual_filter_sz = spec_filter_size(kern_spec_mask,
+						    kern_filter_sz,
+						    ib_filter_sz);
+		if (actual_filter_sz <= 0)
 			return -EINVAL;
-		memcpy(&ib_spec->eth.val, &kern_spec->eth.val,
-		       sizeof(struct ib_flow_eth_filter));
-		memcpy(&ib_spec->eth.mask, &kern_spec->eth.mask,
-		       sizeof(struct ib_flow_eth_filter));
+		ib_spec->size = sizeof(struct ib_flow_spec_eth);
+		memcpy(&ib_spec->eth.val, kern_spec_val, actual_filter_sz);
+		memcpy(&ib_spec->eth.mask, kern_spec_mask, actual_filter_sz);
 		break;
 	case IB_FLOW_SPEC_IPV4:
-		ib_spec->ipv4.size = sizeof(struct ib_flow_spec_ipv4);
-		if (ib_spec->ipv4.size != kern_spec->ipv4.size)
+		ib_filter_sz = offsetof(struct ib_flow_ipv4_filter, real_sz);
+		actual_filter_sz = spec_filter_size(kern_spec_mask,
+						    kern_filter_sz,
+						    ib_filter_sz);
+		if (actual_filter_sz <= 0)
 			return -EINVAL;
-		memcpy(&ib_spec->ipv4.val, &kern_spec->ipv4.val,
-		       sizeof(struct ib_flow_ipv4_filter));
-		memcpy(&ib_spec->ipv4.mask, &kern_spec->ipv4.mask,
-		       sizeof(struct ib_flow_ipv4_filter));
+		ib_spec->size = sizeof(struct ib_flow_spec_ipv4);
+		memcpy(&ib_spec->ipv4.val, kern_spec_val, actual_filter_sz);
+		memcpy(&ib_spec->ipv4.mask, kern_spec_mask, actual_filter_sz);
 		break;
 	case IB_FLOW_SPEC_IPV6:
-		ib_spec->ipv6.size = sizeof(struct ib_flow_spec_ipv6);
-		if (ib_spec->ipv6.size != kern_spec->ipv6.size)
+		ib_filter_sz = offsetof(struct ib_flow_ipv6_filter, real_sz);
+		actual_filter_sz = spec_filter_size(kern_spec_mask,
+						    kern_filter_sz,
+						    ib_filter_sz);
+		if (actual_filter_sz <= 0)
+			return -EINVAL;
+		ib_spec->size = sizeof(struct ib_flow_spec_ipv6);
+		memcpy(&ib_spec->ipv6.val, kern_spec_val, actual_filter_sz);
+		memcpy(&ib_spec->ipv6.mask, kern_spec_mask, actual_filter_sz);
+
+		if ((ntohl(ib_spec->ipv6.mask.flow_label)) >= BIT(20) ||
+		    (ntohl(ib_spec->ipv6.val.flow_label)) >= BIT(20))
 			return -EINVAL;
-		memcpy(&ib_spec->ipv6.val, &kern_spec->ipv6.val,
-		       sizeof(struct ib_flow_ipv6_filter));
-		memcpy(&ib_spec->ipv6.mask, &kern_spec->ipv6.mask,
-		       sizeof(struct ib_flow_ipv6_filter));
 		break;
 	case IB_FLOW_SPEC_TCP:
 	case IB_FLOW_SPEC_UDP:
-		ib_spec->tcp_udp.size = sizeof(struct ib_flow_spec_tcp_udp);
-		if (ib_spec->tcp_udp.size != kern_spec->tcp_udp.size)
+		ib_filter_sz = offsetof(struct ib_flow_tcp_udp_filter, real_sz);
+		actual_filter_sz = spec_filter_size(kern_spec_mask,
+						    kern_filter_sz,
+						    ib_filter_sz);
+		if (actual_filter_sz <= 0)
 			return -EINVAL;
-		memcpy(&ib_spec->tcp_udp.val, &kern_spec->tcp_udp.val,
-		       sizeof(struct ib_flow_tcp_udp_filter));
-		memcpy(&ib_spec->tcp_udp.mask, &kern_spec->tcp_udp.mask,
-		       sizeof(struct ib_flow_tcp_udp_filter));
+		ib_spec->size = sizeof(struct ib_flow_spec_tcp_udp);
+		memcpy(&ib_spec->tcp_udp.val, kern_spec_val, actual_filter_sz);
+		memcpy(&ib_spec->tcp_udp.mask, kern_spec_mask, actual_filter_sz);
 		break;
 	default:
 		return -EINVAL;
@@ -3654,7 +3705,8 @@ int ib_uverbs_ex_create_flow(struct ib_uverbs_file *file,
 		goto err_uobj;
 	}
 
-	flow_attr = kmalloc(sizeof(*flow_attr) + cmd.flow_attr.size, GFP_KERNEL);
+	flow_attr = kzalloc(sizeof(*flow_attr) + cmd.flow_attr.num_of_specs *
+			    sizeof(union ib_flow_spec), GFP_KERNEL);
 	if (!flow_attr) {
 		err = -ENOMEM;
 		goto err_put;
@@ -4173,6 +4225,23 @@ int ib_uverbs_ex_query_device(struct ib_uverbs_file *file,
 
 	resp.device_cap_flags_ex = attr.device_cap_flags;
 	resp.response_length += sizeof(resp.device_cap_flags_ex);
+
+	if (ucore->outlen < resp.response_length + sizeof(resp.rss_caps))
+		goto end;
+
+	resp.rss_caps.supported_qpts = attr.rss_caps.supported_qpts;
+	resp.rss_caps.max_rwq_indirection_tables =
+		attr.rss_caps.max_rwq_indirection_tables;
+	resp.rss_caps.max_rwq_indirection_table_size =
+		attr.rss_caps.max_rwq_indirection_table_size;
+
+	resp.response_length += sizeof(resp.rss_caps);
+
+	if (ucore->outlen < resp.response_length + sizeof(resp.max_wq_type_rq))
+		goto end;
+
+	resp.max_wq_type_rq = attr.max_wq_type_rq;
+	resp.response_length += sizeof(resp.max_wq_type_rq);
 end:
 	err = ib_copy_to_udata(ucore, &resp, resp.response_length);
 	return err;
diff --git a/drivers/infiniband/core/verbs.c b/drivers/infiniband/core/verbs.c
index f2b776efab3a..83687646da68 100644
--- a/drivers/infiniband/core/verbs.c
+++ b/drivers/infiniband/core/verbs.c
@@ -227,9 +227,11 @@ EXPORT_SYMBOL(rdma_port_get_link_layer);
  * Every PD has a local_dma_lkey which can be used as the lkey value for local
  * memory operations.
  */
-struct ib_pd *ib_alloc_pd(struct ib_device *device)
+struct ib_pd *__ib_alloc_pd(struct ib_device *device, unsigned int flags,
+		const char *caller)
 {
 	struct ib_pd *pd;
+	int mr_access_flags = 0;
 
 	pd = device->alloc_pd(device, NULL, NULL);
 	if (IS_ERR(pd))
@@ -237,26 +239,46 @@ struct ib_pd *ib_alloc_pd(struct ib_device *device)
 
 	pd->device = device;
 	pd->uobject = NULL;
-	pd->local_mr = NULL;
+	pd->__internal_mr = NULL;
 	atomic_set(&pd->usecnt, 0);
+	pd->flags = flags;
 
 	if (device->attrs.device_cap_flags & IB_DEVICE_LOCAL_DMA_LKEY)
 		pd->local_dma_lkey = device->local_dma_lkey;
-	else {
+	else
+		mr_access_flags |= IB_ACCESS_LOCAL_WRITE;
+
+	if (flags & IB_PD_UNSAFE_GLOBAL_RKEY) {
+		pr_warn("%s: enabling unsafe global rkey\n", caller);
+		mr_access_flags |= IB_ACCESS_REMOTE_READ | IB_ACCESS_REMOTE_WRITE;
+	}
+
+	if (mr_access_flags) {
 		struct ib_mr *mr;
 
-		mr = ib_get_dma_mr(pd, IB_ACCESS_LOCAL_WRITE);
+		mr = pd->device->get_dma_mr(pd, mr_access_flags);
 		if (IS_ERR(mr)) {
 			ib_dealloc_pd(pd);
-			return (struct ib_pd *)mr;
+			return ERR_CAST(mr);
 		}
 
-		pd->local_mr = mr;
-		pd->local_dma_lkey = pd->local_mr->lkey;
+		mr->device	= pd->device;
+		mr->pd		= pd;
+		mr->uobject	= NULL;
+		mr->need_inval	= false;
+
+		pd->__internal_mr = mr;
+
+		if (!(device->attrs.device_cap_flags & IB_DEVICE_LOCAL_DMA_LKEY))
+			pd->local_dma_lkey = pd->__internal_mr->lkey;
+
+		if (flags & IB_PD_UNSAFE_GLOBAL_RKEY)
+			pd->unsafe_global_rkey = pd->__internal_mr->rkey;
 	}
+
 	return pd;
 }
-EXPORT_SYMBOL(ib_alloc_pd);
+EXPORT_SYMBOL(__ib_alloc_pd);
 
 /**
  * ib_dealloc_pd - Deallocates a protection domain.
@@ -270,10 +292,10 @@ void ib_dealloc_pd(struct ib_pd *pd)
 {
 	int ret;
 
-	if (pd->local_mr) {
-		ret = ib_dereg_mr(pd->local_mr);
+	if (pd->__internal_mr) {
+		ret = pd->device->dereg_mr(pd->__internal_mr);
 		WARN_ON(ret);
-		pd->local_mr = NULL;
+		pd->__internal_mr = NULL;
 	}
 
 	/* uverbs manipulates usecnt with proper locking, while the kabi
@@ -821,7 +843,7 @@ struct ib_qp *ib_create_qp(struct ib_pd *pd,
 		if (ret) {
 			pr_err("failed to init MR pool ret= %d\n", ret);
 			ib_destroy_qp(qp);
-			qp = ERR_PTR(ret);
+			return ERR_PTR(ret);
 		}
 	}
 
@@ -1391,29 +1413,6 @@ EXPORT_SYMBOL(ib_resize_cq);
 
 /* Memory regions */
 
-struct ib_mr *ib_get_dma_mr(struct ib_pd *pd, int mr_access_flags)
-{
-	struct ib_mr *mr;
-	int err;
-
-	err = ib_check_mr_access(mr_access_flags);
-	if (err)
-		return ERR_PTR(err);
-
-	mr = pd->device->get_dma_mr(pd, mr_access_flags);
-
-	if (!IS_ERR(mr)) {
-		mr->device  = pd->device;
-		mr->pd      = pd;
-		mr->uobject = NULL;
-		atomic_inc(&pd->usecnt);
-		mr->need_inval = false;
-	}
-
-	return mr;
-}
-EXPORT_SYMBOL(ib_get_dma_mr);
-
 int ib_dereg_mr(struct ib_mr *mr)
 {
 	struct ib_pd *pd = mr->pd;
@@ -1812,13 +1811,13 @@ EXPORT_SYMBOL(ib_set_vf_guid);
  *
  * Constraints:
  * - The first sg element is allowed to have an offset.
- * - Each sg element must be aligned to page_size (or physically
- *   contiguous to the previous element). In case an sg element has a
- *   non contiguous offset, the mapping prefix will not include it.
+ * - Each sg element must either be aligned to page_size or virtually
+ *   contiguous to the previous element. In case an sg element has a
+ *   non-contiguous offset, the mapping prefix will not include it.
  * - The last sg element is allowed to have length less than page_size.
  * - If sg_nents total byte length exceeds the mr max_num_sge * page_size
  *   then only max_num_sg entries will be mapped.
- * - If the MR was allocated with type IB_MR_TYPE_SG_GAPS_REG, non of these
+ * - If the MR was allocated with type IB_MR_TYPE_SG_GAPS, none of these
  *   constraints holds and the page_size argument is ignored.
  *
  * Returns the number of sg elements that were mapped to the memory region.
diff --git a/drivers/infiniband/hw/Makefile b/drivers/infiniband/hw/Makefile
index c0c7cf8af3f4..e7a5ed9f6f3f 100644
--- a/drivers/infiniband/hw/Makefile
+++ b/drivers/infiniband/hw/Makefile
@@ -9,3 +9,5 @@ obj-$(CONFIG_INFINIBAND_NES)		+= nes/
 obj-$(CONFIG_INFINIBAND_OCRDMA)		+= ocrdma/
 obj-$(CONFIG_INFINIBAND_USNIC)		+= usnic/
 obj-$(CONFIG_INFINIBAND_HFI1)		+= hfi1/
+obj-$(CONFIG_INFINIBAND_HNS)		+= hns/
+obj-$(CONFIG_INFINIBAND_QEDR)		+= qedr/
diff --git a/drivers/infiniband/hw/cxgb3/iwch.c b/drivers/infiniband/hw/cxgb3/iwch.c
index 8e77dc543dd1..b3e11329801d 100644
--- a/drivers/infiniband/hw/cxgb3/iwch.c
+++ b/drivers/infiniband/hw/cxgb3/iwch.c
@@ -36,7 +36,7 @@
 
 #include "cxgb3_offload.h"
 #include "iwch_provider.h"
-#include "iwch_user.h"
+#include <rdma/cxgb3-abi.h>
 #include "iwch.h"
 #include "iwch_cm.h"
 
diff --git a/drivers/infiniband/hw/cxgb3/iwch_cm.c b/drivers/infiniband/hw/cxgb3/iwch_cm.c
index 04bbf172abde..65ee64400deb 100644
--- a/drivers/infiniband/hw/cxgb3/iwch_cm.c
+++ b/drivers/infiniband/hw/cxgb3/iwch_cm.c
@@ -2258,7 +2258,7 @@ int __init iwch_cm_init(void)
 {
 	skb_queue_head_init(&rxq);
 
-	workq = create_singlethread_workqueue("iw_cxgb3");
+	workq = alloc_ordered_workqueue("iw_cxgb3", WQ_MEM_RECLAIM);
 	if (!workq)
 		return -ENOMEM;
 
diff --git a/drivers/infiniband/hw/cxgb3/iwch_provider.c b/drivers/infiniband/hw/cxgb3/iwch_provider.c
index 3edb80644b53..cba57bb53dba 100644
--- a/drivers/infiniband/hw/cxgb3/iwch_provider.c
+++ b/drivers/infiniband/hw/cxgb3/iwch_provider.c
@@ -58,7 +58,7 @@
 #include "iwch.h"
 #include "iwch_provider.h"
 #include "iwch_cm.h"
-#include "iwch_user.h"
+#include <rdma/cxgb3-abi.h>
 #include "common.h"
 
 static struct ib_ah *iwch_ah_create(struct ib_pd *pd,
@@ -1396,6 +1396,7 @@ int iwch_register_device(struct iwch_dev *dev)
 	    (1ull << IB_USER_VERBS_CMD_POST_SEND) |
 	    (1ull << IB_USER_VERBS_CMD_POST_RECV);
 	dev->ibdev.node_type = RDMA_NODE_RNIC;
+	BUILD_BUG_ON(sizeof(IWCH_NODE_DESC) > IB_DEVICE_NODE_DESC_MAX);
 	memcpy(dev->ibdev.node_desc, IWCH_NODE_DESC, sizeof(IWCH_NODE_DESC));
 	dev->ibdev.phys_port_cnt = dev->rdev.port_info.nports;
 	dev->ibdev.num_comp_vectors = 1;
diff --git a/drivers/infiniband/hw/cxgb4/Kconfig b/drivers/infiniband/hw/cxgb4/Kconfig
index 23f38cf2c5cd..afe8b28e0878 100644
--- a/drivers/infiniband/hw/cxgb4/Kconfig
+++ b/drivers/infiniband/hw/cxgb4/Kconfig
@@ -1,6 +1,7 @@
 config INFINIBAND_CXGB4
 	tristate "Chelsio T4/T5 RDMA Driver"
 	depends on CHELSIO_T4 && INET && (IPV6 || IPV6=n)
+	select CHELSIO_LIB
 	select GENERIC_ALLOCATOR
 	---help---
 	  This is an iWARP/RDMA driver for the Chelsio T4 and T5
diff --git a/drivers/infiniband/hw/cxgb4/Makefile b/drivers/infiniband/hw/cxgb4/Makefile
index e11cf7299945..fa40b685831b 100644
--- a/drivers/infiniband/hw/cxgb4/Makefile
+++ b/drivers/infiniband/hw/cxgb4/Makefile
@@ -1,4 +1,5 @@
 ccflags-y := -Idrivers/net/ethernet/chelsio/cxgb4
+ccflags-y += -Idrivers/net/ethernet/chelsio/libcxgb
 
 obj-$(CONFIG_INFINIBAND_CXGB4) += iw_cxgb4.o
 
diff --git a/drivers/infiniband/hw/cxgb4/cm.c b/drivers/infiniband/hw/cxgb4/cm.c
index 80f988984f44..f1510cc76d2d 100644
--- a/drivers/infiniband/hw/cxgb4/cm.c
+++ b/drivers/infiniband/hw/cxgb4/cm.c
@@ -49,6 +49,7 @@
 
 #include <rdma/ib_addr.h>
 
+#include <libcxgb_cm.h>
 #include "iw_cxgb4.h"
 #include "clip_tbl.h"
 
@@ -239,15 +240,13 @@ int c4iw_ofld_send(struct c4iw_rdev *rdev, struct sk_buff *skb)
 
 static void release_tid(struct c4iw_rdev *rdev, u32 hwtid, struct sk_buff *skb)
 {
-	struct cpl_tid_release *req;
+	u32 len = roundup(sizeof(struct cpl_tid_release), 16);
 
-	skb = get_skb(skb, sizeof *req, GFP_KERNEL);
+	skb = get_skb(skb, len, GFP_KERNEL);
 	if (!skb)
 		return;
-	req = (struct cpl_tid_release *) skb_put(skb, sizeof(*req));
-	INIT_TP_WR(req, hwtid);
-	OPCODE_TID(req) = cpu_to_be32(MK_OPCODE_TID(CPL_TID_RELEASE, hwtid));
-	set_wr_txq(skb, CPL_PRIORITY_SETUP, 0);
+
+	cxgb_mk_tid_release(skb, len, hwtid, 0);
 	c4iw_ofld_send(rdev, skb);
 	return;
 }
@@ -466,72 +465,6 @@ static struct net_device *get_real_dev(struct net_device *egress_dev)
 	return rdma_vlan_dev_real_dev(egress_dev) ? : egress_dev;
 }
 
-static int our_interface(struct c4iw_dev *dev, struct net_device *egress_dev)
-{
-	int i;
-
-	egress_dev = get_real_dev(egress_dev);
-	for (i = 0; i < dev->rdev.lldi.nports; i++)
-		if (dev->rdev.lldi.ports[i] == egress_dev)
-			return 1;
-	return 0;
-}
-
-static struct dst_entry *find_route6(struct c4iw_dev *dev, __u8 *local_ip,
-				     __u8 *peer_ip, __be16 local_port,
-				     __be16 peer_port, u8 tos,
-				     __u32 sin6_scope_id)
-{
-	struct dst_entry *dst = NULL;
-
-	if (IS_ENABLED(CONFIG_IPV6)) {
-		struct flowi6 fl6;
-
-		memset(&fl6, 0, sizeof(fl6));
-		memcpy(&fl6.daddr, peer_ip, 16);
-		memcpy(&fl6.saddr, local_ip, 16);
-		if (ipv6_addr_type(&fl6.daddr) & IPV6_ADDR_LINKLOCAL)
-			fl6.flowi6_oif = sin6_scope_id;
-		dst = ip6_route_output(&init_net, NULL, &fl6);
-		if (!dst)
-			goto out;
-		if (!our_interface(dev, ip6_dst_idev(dst)->dev) &&
-		    !(ip6_dst_idev(dst)->dev->flags & IFF_LOOPBACK)) {
-			dst_release(dst);
-			dst = NULL;
-		}
-	}
-
-out:
-	return dst;
-}
-
-static struct dst_entry *find_route(struct c4iw_dev *dev, __be32 local_ip,
-				 __be32 peer_ip, __be16 local_port,
-				 __be16 peer_port, u8 tos)
-{
-	struct rtable *rt;
-	struct flowi4 fl4;
-	struct neighbour *n;
-
-	rt = ip_route_output_ports(&init_net, &fl4, NULL, peer_ip, local_ip,
-				   peer_port, local_port, IPPROTO_TCP,
-				   tos, 0);
-	if (IS_ERR(rt))
-		return NULL;
-	n = dst_neigh_lookup(&rt->dst, &peer_ip);
-	if (!n)
-		return NULL;
-	if (!our_interface(dev, n->dev) &&
-	    !(n->dev->flags & IFF_LOOPBACK)) {
-		neigh_release(n);
-		dst_release(&rt->dst);
-		return NULL;
-	}
-	neigh_release(n);
-	return &rt->dst;
-}
-
 static void arp_failure_discard(void *handle, struct sk_buff *skb)
 {
 	pr_err(MOD "ARP failure\n");
@@ -706,56 +639,32 @@ static int send_flowc(struct c4iw_ep *ep)
 
 static int send_halfclose(struct c4iw_ep *ep)
 {
-	struct cpl_close_con_req *req;
 	struct sk_buff *skb = skb_dequeue(&ep->com.ep_skb_list);
-	int wrlen = roundup(sizeof *req, 16);
+	u32 wrlen = roundup(sizeof(struct cpl_close_con_req), 16);
 
 	PDBG("%s ep %p tid %u\n", __func__, ep, ep->hwtid);
 	if (WARN_ON(!skb))
 		return -ENOMEM;
 
-	set_wr_txq(skb, CPL_PRIORITY_DATA, ep->txq_idx);
-	t4_set_arp_err_handler(skb, NULL, arp_failure_discard);
-	req = (struct cpl_close_con_req *) skb_put(skb, wrlen);
-	memset(req, 0, wrlen);
-	INIT_TP_WR(req, ep->hwtid);
-	OPCODE_TID(req) = cpu_to_be32(MK_OPCODE_TID(CPL_CLOSE_CON_REQ,
-						    ep->hwtid));
+	cxgb_mk_close_con_req(skb, wrlen, ep->hwtid, ep->txq_idx,
+			      NULL, arp_failure_discard);
+
 	return c4iw_l2t_send(&ep->com.dev->rdev, skb, ep->l2t);
 }
 
 static int send_abort(struct c4iw_ep *ep)
 {
-	struct cpl_abort_req *req;
-	int wrlen = roundup(sizeof *req, 16);
+	u32 wrlen = roundup(sizeof(struct cpl_abort_req), 16);
 	struct sk_buff *req_skb = skb_dequeue(&ep->com.ep_skb_list);
 
 	PDBG("%s ep %p tid %u\n", __func__, ep, ep->hwtid);
 	if (WARN_ON(!req_skb))
 		return -ENOMEM;
 
-	set_wr_txq(req_skb, CPL_PRIORITY_DATA, ep->txq_idx);
-	t4_set_arp_err_handler(req_skb, ep, abort_arp_failure);
-	req = (struct cpl_abort_req *)skb_put(req_skb, wrlen);
-	memset(req, 0, wrlen);
-	INIT_TP_WR(req, ep->hwtid);
-	OPCODE_TID(req) = cpu_to_be32(MK_OPCODE_TID(CPL_ABORT_REQ, ep->hwtid));
-	req->cmd = CPL_ABORT_SEND_RST;
-	return c4iw_l2t_send(&ep->com.dev->rdev, req_skb, ep->l2t);
-}
+	cxgb_mk_abort_req(req_skb, wrlen, ep->hwtid, ep->txq_idx,
+			  ep, abort_arp_failure);
 
-static void best_mtu(const unsigned short *mtus, unsigned short mtu,
-		     unsigned int *idx, int use_ts, int ipv6)
-{
-	unsigned short hdr_size = (ipv6 ?
-				   sizeof(struct ipv6hdr) :
-				   sizeof(struct iphdr)) +
-				  sizeof(struct tcphdr) +
-				  (use_ts ?
-				   round_up(TCPOLEN_TIMESTAMP, 4) : 0);
-	unsigned short data_size = mtu - hdr_size;
-
-	cxgb4_best_aligned_mtu(mtus, hdr_size, data_size, 8, idx);
+	return c4iw_l2t_send(&ep->com.dev->rdev, req_skb, ep->l2t);
 }
 
 static int send_connect(struct c4iw_ep *ep)
@@ -770,7 +679,7 @@ static int send_connect(struct c4iw_ep *ep)
 	u64 opt0;
 	u32 opt2;
 	unsigned int mtu_idx;
-	int wscale;
+	u32 wscale;
 	int win, sizev4, sizev6, wrlen;
 	struct sockaddr_in *la = (struct sockaddr_in *)
 				 &ep->com.local_addr;
@@ -817,10 +726,10 @@ static int send_connect(struct c4iw_ep *ep)
 	}
 	set_wr_txq(skb, CPL_PRIORITY_SETUP, ep->ctrlq_idx);
 
-	best_mtu(ep->com.dev->rdev.lldi.mtus, ep->mtu, &mtu_idx,
-		 enable_tcp_timestamps,
-		 (AF_INET == ep->com.remote_addr.ss_family) ? 0 : 1);
-	wscale = compute_wscale(rcv_win);
+	cxgb_best_mtu(ep->com.dev->rdev.lldi.mtus, ep->mtu, &mtu_idx,
+		      enable_tcp_timestamps,
+		      (ep->com.remote_addr.ss_family == AF_INET) ? 0 : 1);
+	wscale = cxgb_compute_wscale(rcv_win);
 
 	/*
 	 * Specify the largest window that will fit in opt0. The
@@ -1447,9 +1356,9 @@ static void established_upcall(struct c4iw_ep *ep)
 
 static int update_rx_credits(struct c4iw_ep *ep, u32 credits)
 {
-	struct cpl_rx_data_ack *req;
 	struct sk_buff *skb;
-	int wrlen = roundup(sizeof *req, 16);
+	u32 wrlen = roundup(sizeof(struct cpl_rx_data_ack), 16);
+	u32 credit_dack;
 
 	PDBG("%s ep %p tid %u credits %u\n", __func__, ep, ep->hwtid, credits);
 	skb = get_skb(NULL, wrlen, GFP_KERNEL);
@@ -1466,15 +1375,12 @@ static int update_rx_credits(struct c4iw_ep *ep, u32 credits)
 	if (ep->rcv_win > RCV_BUFSIZ_M * 1024)
 		credits += ep->rcv_win - RCV_BUFSIZ_M * 1024;
 
-	req = (struct cpl_rx_data_ack *) skb_put(skb, wrlen);
-	memset(req, 0, wrlen);
-	INIT_TP_WR(req, ep->hwtid);
-	OPCODE_TID(req) = cpu_to_be32(MK_OPCODE_TID(CPL_RX_DATA_ACK,
-						    ep->hwtid));
-	req->credit_dack = cpu_to_be32(credits | RX_FORCE_ACK_F |
-				       RX_DACK_CHANGE_F |
-				       RX_DACK_MODE_V(dack_mode));
-	set_wr_txq(skb, CPL_PRIORITY_ACK, ep->ctrlq_idx);
+	credit_dack = credits | RX_FORCE_ACK_F | RX_DACK_CHANGE_F |
+		      RX_DACK_MODE_V(dack_mode);
+
+	cxgb_mk_rx_data_ack(skb, wrlen, ep->hwtid, ep->ctrlq_idx,
+			    credit_dack);
+
 	c4iw_ofld_send(&ep->com.dev->rdev, skb);
 	return credits;
 }
@@ -1972,7 +1878,7 @@ static int send_fw_act_open_req(struct c4iw_ep *ep, unsigned int atid)
 	struct sk_buff *skb;
 	struct fw_ofld_connection_wr *req;
 	unsigned int mtu_idx;
-	int wscale;
+	u32 wscale;
 	struct sockaddr_in *sin;
 	int win;
 
@@ -1997,10 +1903,10 @@ static int send_fw_act_open_req(struct c4iw_ep *ep, unsigned int atid)
 			htons(FW_OFLD_CONNECTION_WR_CPLRXDATAACK_F);
 	req->tcb.tx_max = (__force __be32) jiffies;
 	req->tcb.rcv_adv = htons(1);
-	best_mtu(ep->com.dev->rdev.lldi.mtus, ep->mtu, &mtu_idx,
-		 enable_tcp_timestamps,
-		 (AF_INET == ep->com.remote_addr.ss_family) ? 0 : 1);
-	wscale = compute_wscale(rcv_win);
+	cxgb_best_mtu(ep->com.dev->rdev.lldi.mtus, ep->mtu, &mtu_idx,
+		      enable_tcp_timestamps,
+		      (ep->com.remote_addr.ss_family == AF_INET) ? 0 : 1);
+	wscale = cxgb_compute_wscale(rcv_win);
 
 	/*
 	 * Specify the largest window that will fit in opt0. The
@@ -2054,15 +1960,6 @@ static inline int act_open_has_tid(int status)
 		status != CPL_ERR_CONN_EXIST);
 }
 
-/* Returns whether a CPL status conveys negative advice.
- */
-static int is_neg_adv(unsigned int status)
-{
-	return status == CPL_ERR_RTX_NEG_ADVICE ||
-	       status == CPL_ERR_PERSIST_NEG_ADVICE ||
-	       status == CPL_ERR_KEEPALV_NEG_ADVICE;
-}
-
 static char *neg_adv_str(unsigned int status)
 {
 	switch (status) {
@@ -2218,16 +2115,21 @@ static int c4iw_reconnect(struct c4iw_ep *ep)
 
 	/* find a route */
 	if (ep->com.cm_id->m_local_addr.ss_family == AF_INET) {
-		ep->dst = find_route(ep->com.dev, laddr->sin_addr.s_addr,
-				     raddr->sin_addr.s_addr, laddr->sin_port,
-				     raddr->sin_port, ep->com.cm_id->tos);
+		ep->dst = cxgb_find_route(&ep->com.dev->rdev.lldi, get_real_dev,
+					  laddr->sin_addr.s_addr,
+					  raddr->sin_addr.s_addr,
+					  laddr->sin_port,
+					  raddr->sin_port, ep->com.cm_id->tos);
 		iptype = 4;
 		ra = (__u8 *)&raddr->sin_addr;
 	} else {
-		ep->dst = find_route6(ep->com.dev, laddr6->sin6_addr.s6_addr,
-				      raddr6->sin6_addr.s6_addr,
-				      laddr6->sin6_port, raddr6->sin6_port, 0,
-				      raddr6->sin6_scope_id);
+		ep->dst = cxgb_find_route6(&ep->com.dev->rdev.lldi,
+					   get_real_dev,
+					   laddr6->sin6_addr.s6_addr,
+					   raddr6->sin6_addr.s6_addr,
+					   laddr6->sin6_port,
+					   raddr6->sin6_port, 0,
+					   raddr6->sin6_scope_id);
 		iptype = 6;
 		ra = (__u8 *)&raddr6->sin6_addr;
 	}
@@ -2299,7 +2201,7 @@ static int act_open_rpl(struct c4iw_dev *dev, struct sk_buff *skb)
 	PDBG("%s ep %p atid %u status %u errno %d\n", __func__, ep, atid,
 	     status, status2errno(status));
 
-	if (is_neg_adv(status)) {
+	if (cxgb_is_neg_adv(status)) {
 		PDBG("%s Connection problems for atid %u status %u (%s)\n",
 		     __func__, atid, status, neg_adv_str(status));
 		ep->stats.connect_neg_adv++;
@@ -2426,7 +2328,7 @@ static int accept_cr(struct c4iw_ep *ep, struct sk_buff *skb,
 	unsigned int mtu_idx;
 	u64 opt0;
 	u32 opt2;
-	int wscale;
+	u32 wscale;
 	struct cpl_t5_pass_accept_rpl *rpl5 = NULL;
 	int win;
 	enum chip_type adapter_type = ep->com.dev->rdev.lldi.adapter_type;
@@ -2447,10 +2349,10 @@ static int accept_cr(struct c4iw_ep *ep, struct sk_buff *skb,
 	OPCODE_TID(rpl) = cpu_to_be32(MK_OPCODE_TID(CPL_PASS_ACCEPT_RPL,
 						    ep->hwtid));
 
-	best_mtu(ep->com.dev->rdev.lldi.mtus, ep->mtu, &mtu_idx,
-		 enable_tcp_timestamps && req->tcpopt.tstamp,
-		 (AF_INET == ep->com.remote_addr.ss_family) ? 0 : 1);
-	wscale = compute_wscale(rcv_win);
+	cxgb_best_mtu(ep->com.dev->rdev.lldi.mtus, ep->mtu, &mtu_idx,
+		      enable_tcp_timestamps && req->tcpopt.tstamp,
+		      (ep->com.remote_addr.ss_family == AF_INET) ? 0 : 1);
+	wscale = cxgb_compute_wscale(rcv_win);
 
 	/*
 	 * Specify the largest window that will fit in opt0. The
@@ -2522,42 +2424,6 @@ static void reject_cr(struct c4iw_dev *dev, u32 hwtid, struct sk_buff *skb)
 	return;
 }
 
-static void get_4tuple(struct cpl_pass_accept_req *req, enum chip_type type,
-		       int *iptype, __u8 *local_ip, __u8 *peer_ip,
-		       __be16 *local_port, __be16 *peer_port)
-{
-	int eth_len = (CHELSIO_CHIP_VERSION(type) <= CHELSIO_T5) ?
-		      ETH_HDR_LEN_G(be32_to_cpu(req->hdr_len)) :
-		      T6_ETH_HDR_LEN_G(be32_to_cpu(req->hdr_len));
-	int ip_len = (CHELSIO_CHIP_VERSION(type) <= CHELSIO_T5) ?
-		     IP_HDR_LEN_G(be32_to_cpu(req->hdr_len)) :
-		     T6_IP_HDR_LEN_G(be32_to_cpu(req->hdr_len));
-	struct iphdr *ip = (struct iphdr *)((u8 *)(req + 1) + eth_len);
-	struct ipv6hdr *ip6 = (struct ipv6hdr *)((u8 *)(req + 1) + eth_len);
-	struct tcphdr *tcp = (struct tcphdr *)
-			     ((u8 *)(req + 1) + eth_len + ip_len);
-
-	if (ip->version == 4) {
-		PDBG("%s saddr 0x%x daddr 0x%x sport %u dport %u\n", __func__,
-		     ntohl(ip->saddr), ntohl(ip->daddr), ntohs(tcp->source),
-		     ntohs(tcp->dest));
-		*iptype = 4;
-		memcpy(peer_ip, &ip->saddr, 4);
-		memcpy(local_ip, &ip->daddr, 4);
-	} else {
-		PDBG("%s saddr %pI6 daddr %pI6 sport %u dport %u\n", __func__,
-		     ip6->saddr.s6_addr, ip6->daddr.s6_addr, ntohs(tcp->source),
-		     ntohs(tcp->dest));
-		*iptype = 6;
-		memcpy(peer_ip, ip6->saddr.s6_addr, 16);
-		memcpy(local_ip, ip6->daddr.s6_addr, 16);
-	}
-	*peer_port = tcp->source;
-	*local_port = tcp->dest;
-
-	return;
-}
-
 static int pass_accept_req(struct c4iw_dev *dev, struct sk_buff *skb)
 {
 	struct c4iw_ep *child_ep = NULL, *parent_ep;
@@ -2586,8 +2452,8 @@ static int pass_accept_req(struct c4iw_dev *dev, struct sk_buff *skb)
 		goto reject;
 	}
 
-	get_4tuple(req, parent_ep->com.dev->rdev.lldi.adapter_type, &iptype,
-		   local_ip, peer_ip, &local_port, &peer_port);
+	cxgb_get_4tuple(req, parent_ep->com.dev->rdev.lldi.adapter_type,
+			&iptype, local_ip, peer_ip, &local_port, &peer_port);
 
 	/* Find output route */
 	if (iptype == 4)  {
@@ -2595,18 +2461,19 @@ static int pass_accept_req(struct c4iw_dev *dev, struct sk_buff *skb)
 		     , __func__, parent_ep, hwtid,
 		     local_ip, peer_ip, ntohs(local_port),
 		     ntohs(peer_port), peer_mss);
-		dst = find_route(dev, *(__be32 *)local_ip, *(__be32 *)peer_ip,
-				 local_port, peer_port,
-				 tos);
+		dst = cxgb_find_route(&dev->rdev.lldi, get_real_dev,
+				      *(__be32 *)local_ip, *(__be32 *)peer_ip,
+				      local_port, peer_port, tos);
 	} else {
 		PDBG("%s parent ep %p hwtid %u laddr %pI6 raddr %pI6 lport %d rport %d peer_mss %d\n"
 		     , __func__, parent_ep, hwtid,
 		     local_ip, peer_ip, ntohs(local_port),
 		     ntohs(peer_port), peer_mss);
-		dst = find_route6(dev, local_ip, peer_ip, local_port, peer_port,
-				  PASS_OPEN_TOS_G(ntohl(req->tos_stid)),
-				  ((struct sockaddr_in6 *)
-				  &parent_ep->com.local_addr)->sin6_scope_id);
+		dst = cxgb_find_route6(&dev->rdev.lldi, get_real_dev,
+				local_ip, peer_ip, local_port, peer_port,
+				PASS_OPEN_TOS_G(ntohl(req->tos_stid)),
+				((struct sockaddr_in6 *)
+				 &parent_ep->com.local_addr)->sin6_scope_id);
 	}
 	if (!dst) {
 		printk(KERN_ERR MOD "%s - failed to find dst entry!\n",
@@ -2839,18 +2706,18 @@ static int peer_abort(struct c4iw_dev *dev, struct sk_buff *skb)
 {
 	struct cpl_abort_req_rss *req = cplhdr(skb);
 	struct c4iw_ep *ep;
-	struct cpl_abort_rpl *rpl;
 	struct sk_buff *rpl_skb;
 	struct c4iw_qp_attributes attrs;
 	int ret;
 	int release = 0;
 	unsigned int tid = GET_TID(req);
+	u32 len = roundup(sizeof(struct cpl_abort_rpl), 16);
 
 	ep = get_ep_from_tid(dev, tid);
 	if (!ep)
 		return 0;
 
-	if (is_neg_adv(req->status)) {
+	if (cxgb_is_neg_adv(req->status)) {
 		PDBG("%s Negative advice on abort- tid %u status %d (%s)\n",
 		     __func__, ep->hwtid, req->status,
 		     neg_adv_str(req->status));
@@ -2943,11 +2810,9 @@ static int peer_abort(struct c4iw_dev *dev, struct sk_buff *skb)
 		release = 1;
 		goto out;
 	}
-	set_wr_txq(skb, CPL_PRIORITY_DATA, ep->txq_idx);
-	rpl = (struct cpl_abort_rpl *) skb_put(rpl_skb, sizeof(*rpl));
-	INIT_TP_WR(rpl, ep->hwtid);
-	OPCODE_TID(rpl) = cpu_to_be32(MK_OPCODE_TID(CPL_ABORT_RPL, ep->hwtid));
-	rpl->cmd = CPL_ABORT_NO_RST;
+
+	cxgb_mk_abort_rpl(rpl_skb, len, ep->hwtid, ep->txq_idx);
+
 	c4iw_ofld_send(&ep->com.dev->rdev, rpl_skb);
 out:
 	if (release)
@@ -3379,9 +3244,11 @@ int c4iw_connect(struct iw_cm_id *cm_id, struct iw_cm_conn_param *conn_param)
 		PDBG("%s saddr %pI4 sport 0x%x raddr %pI4 rport 0x%x\n",
 		     __func__, &laddr->sin_addr, ntohs(laddr->sin_port),
 		     ra, ntohs(raddr->sin_port));
-		ep->dst = find_route(dev, laddr->sin_addr.s_addr,
-				     raddr->sin_addr.s_addr, laddr->sin_port,
-				     raddr->sin_port, cm_id->tos);
+		ep->dst = cxgb_find_route(&dev->rdev.lldi, get_real_dev,
+					  laddr->sin_addr.s_addr,
+					  raddr->sin_addr.s_addr,
+					  laddr->sin_port,
+					  raddr->sin_port, cm_id->tos);
 	} else {
 		iptype = 6;
 		ra = (__u8 *)&raddr6->sin6_addr;
@@ -3400,10 +3267,12 @@ int c4iw_connect(struct iw_cm_id *cm_id, struct iw_cm_conn_param *conn_param)
 		     __func__, laddr6->sin6_addr.s6_addr,
 		     ntohs(laddr6->sin6_port),
 		     raddr6->sin6_addr.s6_addr, ntohs(raddr6->sin6_port));
-		ep->dst = find_route6(dev, laddr6->sin6_addr.s6_addr,
-				      raddr6->sin6_addr.s6_addr,
-				      laddr6->sin6_port, raddr6->sin6_port, 0,
-				      raddr6->sin6_scope_id);
+		ep->dst = cxgb_find_route6(&dev->rdev.lldi, get_real_dev,
+					   laddr6->sin6_addr.s6_addr,
+					   raddr6->sin6_addr.s6_addr,
+					   laddr6->sin6_port,
+					   raddr6->sin6_port, 0,
+					   raddr6->sin6_scope_id);
 	}
 	if (!ep->dst) {
 		printk(KERN_ERR MOD "%s - cannot find route.\n", __func__);
@@ -4045,8 +3914,9 @@ static int rx_pkt(struct c4iw_dev *dev, struct sk_buff *skb)
 	     ntohl(iph->daddr), ntohs(tcph->dest), ntohl(iph->saddr),
 	     ntohs(tcph->source), iph->tos);
 
-	dst = find_route(dev, iph->daddr, iph->saddr, tcph->dest, tcph->source,
-			 iph->tos);
+	dst = cxgb_find_route(&dev->rdev.lldi, get_real_dev,
+			      iph->daddr, iph->saddr, tcph->dest,
+			      tcph->source, iph->tos);
 	if (!dst) {
 		pr_err("%s - failed to find dst entry!\n",
 		       __func__);
@@ -4321,7 +4191,7 @@ static int peer_abort_intr(struct c4iw_dev *dev, struct sk_buff *skb)
 		kfree_skb(skb);
 		return 0;
 	}
-	if (is_neg_adv(req->status)) {
+	if (cxgb_is_neg_adv(req->status)) {
 		PDBG("%s Negative advice on abort- tid %u status %d (%s)\n",
 		     __func__, ep->hwtid, req->status,
 		     neg_adv_str(req->status));
@@ -4365,7 +4235,7 @@ int __init c4iw_cm_init(void)
 	spin_lock_init(&timeout_lock);
 	skb_queue_head_init(&rxq);
 
-	workq = create_singlethread_workqueue("iw_cxgb4");
+	workq = alloc_ordered_workqueue("iw_cxgb4", WQ_MEM_RECLAIM);
 	if (!workq)
 		return -ENOMEM;
 
diff --git a/drivers/infiniband/hw/cxgb4/cq.c b/drivers/infiniband/hw/cxgb4/cq.c
index ac926c942fee..867b8cf82be8 100644
--- a/drivers/infiniband/hw/cxgb4/cq.c
+++ b/drivers/infiniband/hw/cxgb4/cq.c
@@ -666,6 +666,18 @@ skip_cqe:
 	return ret;
 }
 
+static void invalidate_mr(struct c4iw_dev *rhp, u32 rkey)
+{
+	struct c4iw_mr *mhp;
+	unsigned long flags;
+
+	spin_lock_irqsave(&rhp->lock, flags);
+	mhp = get_mhp(rhp, rkey >> 8);
+	if (mhp)
+		mhp->attr.state = 0;
+	spin_unlock_irqrestore(&rhp->lock, flags);
+}
+
 /*
  * Get one cq entry from c4iw and map it to openib.
  *
@@ -721,6 +733,7 @@ static int c4iw_poll_cq_one(struct c4iw_cq *chp, struct ib_wc *wc)
 		    CQE_OPCODE(&cqe) == FW_RI_SEND_WITH_SE_INV) {
 			wc->ex.invalidate_rkey = CQE_WRID_STAG(&cqe);
 			wc->wc_flags |= IB_WC_WITH_INVALIDATE;
+			invalidate_mr(qhp->rhp, wc->ex.invalidate_rkey);
 		}
 	} else {
 		switch (CQE_OPCODE(&cqe)) {
@@ -746,6 +759,10 @@ static int c4iw_poll_cq_one(struct c4iw_cq *chp, struct ib_wc *wc)
 			break;
 		case FW_RI_FAST_REGISTER:
 			wc->opcode = IB_WC_REG_MR;
+
+			/* Invalidate the MR if the fastreg failed */
+			if (CQE_STATUS(&cqe) != T4_ERR_SUCCESS)
+				invalidate_mr(qhp->rhp, CQE_WRID_FR_STAG(&cqe));
 			break;
 		default:
 			printk(KERN_ERR MOD "Unexpected opcode %d "
diff --git a/drivers/infiniband/hw/cxgb4/device.c b/drivers/infiniband/hw/cxgb4/device.c
index 3c4b2126e0d1..93e3d270a98a 100644
--- a/drivers/infiniband/hw/cxgb4/device.c
+++ b/drivers/infiniband/hw/cxgb4/device.c
@@ -1480,6 +1480,10 @@ static int c4iw_uld_control(void *handle, enum cxgb4_control control, ...)
 
 static struct cxgb4_uld_info c4iw_uld_info = {
 	.name = DRV_NAME,
+	.nrxq = MAX_ULD_QSETS,
+	.rxq_size = 511,
+	.ciq = true,
+	.lro = false,
 	.add = c4iw_uld_add,
 	.rx_handler = c4iw_uld_rx_handler,
 	.state_change = c4iw_uld_state_change,
diff --git a/drivers/infiniband/hw/cxgb4/iw_cxgb4.h b/drivers/infiniband/hw/cxgb4/iw_cxgb4.h
index 4b83b84f7ddf..7e7f79e55006 100644
--- a/drivers/infiniband/hw/cxgb4/iw_cxgb4.h
+++ b/drivers/infiniband/hw/cxgb4/iw_cxgb4.h
@@ -58,7 +58,7 @@
 #include "cxgb4.h"
 #include "cxgb4_uld.h"
 #include "l2t.h"
-#include "user.h"
+#include <rdma/cxgb4-abi.h>
 
 #define DRV_NAME "iw_cxgb4"
 #define MOD DRV_NAME ":"
@@ -882,15 +882,6 @@ static inline struct c4iw_listen_ep *to_listen_ep(struct iw_cm_id *cm_id)
 	return cm_id->provider_data;
 }
 
-static inline int compute_wscale(int win)
-{
-	int wscale = 0;
-
-	while (wscale < 14 && (65535<<wscale) < win)
-		wscale++;
-	return wscale;
-}
-
 static inline int ocqp_supported(const struct cxgb4_lld_info *infop)
 {
 #if defined(__i386__) || defined(__x86_64__) || defined(CONFIG_PPC64)
diff --git a/drivers/infiniband/hw/cxgb4/mem.c b/drivers/infiniband/hw/cxgb4/mem.c
index 0b91b0f4df71..80e27749420a 100644
--- a/drivers/infiniband/hw/cxgb4/mem.c
+++ b/drivers/infiniband/hw/cxgb4/mem.c
@@ -695,7 +695,7 @@ struct ib_mr *c4iw_alloc_mr(struct ib_pd *pd,
 	mhp->attr.pdid = php->pdid;
 	mhp->attr.type = FW_RI_STAG_NSMR;
 	mhp->attr.stag = stag;
-	mhp->attr.state = 1;
+	mhp->attr.state = 0;
 	mmid = (stag) >> 8;
 	mhp->ibmr.rkey = mhp->ibmr.lkey = stag;
 	if (insert_handle(rhp, &rhp->mmidr, mhp, mmid)) {
diff --git a/drivers/infiniband/hw/cxgb4/provider.c b/drivers/infiniband/hw/cxgb4/provider.c
index df127ce6b6ec..645e606a17c5 100644
--- a/drivers/infiniband/hw/cxgb4/provider.c
+++ b/drivers/infiniband/hw/cxgb4/provider.c
@@ -563,6 +563,7 @@ int c4iw_register_device(struct c4iw_dev *dev)
 	    (1ull << IB_USER_VERBS_CMD_POST_SEND) |
 	    (1ull << IB_USER_VERBS_CMD_POST_RECV);
 	dev->ibdev.node_type = RDMA_NODE_RNIC;
+	BUILD_BUG_ON(sizeof(C4IW_NODE_DESC) > IB_DEVICE_NODE_DESC_MAX);
 	memcpy(dev->ibdev.node_desc, C4IW_NODE_DESC, sizeof(C4IW_NODE_DESC));
 	dev->ibdev.phys_port_cnt = dev->rdev.lldi.nports;
 	dev->ibdev.num_comp_vectors =  dev->rdev.lldi.nciq;
diff --git a/drivers/infiniband/hw/cxgb4/qp.c b/drivers/infiniband/hw/cxgb4/qp.c
index 690435229be7..f57deba6717c 100644
--- a/drivers/infiniband/hw/cxgb4/qp.c
+++ b/drivers/infiniband/hw/cxgb4/qp.c
@@ -609,10 +609,42 @@ static int build_rdma_recv(struct c4iw_qp *qhp, union t4_recv_wr *wqe,
 	return 0;
 }
 
+static void build_tpte_memreg(struct fw_ri_fr_nsmr_tpte_wr *fr,
+			      struct ib_reg_wr *wr, struct c4iw_mr *mhp,
+			      u8 *len16)
+{
+	__be64 *p = (__be64 *)fr->pbl;
+
+	fr->r2 = cpu_to_be32(0);
+	fr->stag = cpu_to_be32(mhp->ibmr.rkey);
+
+	fr->tpte.valid_to_pdid = cpu_to_be32(FW_RI_TPTE_VALID_F |
+		FW_RI_TPTE_STAGKEY_V((mhp->ibmr.rkey & FW_RI_TPTE_STAGKEY_M)) |
+		FW_RI_TPTE_STAGSTATE_V(1) |
+		FW_RI_TPTE_STAGTYPE_V(FW_RI_STAG_NSMR) |
+		FW_RI_TPTE_PDID_V(mhp->attr.pdid));
+	fr->tpte.locread_to_qpid = cpu_to_be32(
+		FW_RI_TPTE_PERM_V(c4iw_ib_to_tpt_access(wr->access)) |
+		FW_RI_TPTE_ADDRTYPE_V(FW_RI_VA_BASED_TO) |
+		FW_RI_TPTE_PS_V(ilog2(wr->mr->page_size) - 12));
+	fr->tpte.nosnoop_pbladdr = cpu_to_be32(FW_RI_TPTE_PBLADDR_V(
+		PBL_OFF(&mhp->rhp->rdev, mhp->attr.pbl_addr)>>3));
+	fr->tpte.dca_mwbcnt_pstag = cpu_to_be32(0);
+	fr->tpte.len_hi = cpu_to_be32(0);
+	fr->tpte.len_lo = cpu_to_be32(mhp->ibmr.length);
+	fr->tpte.va_hi = cpu_to_be32(mhp->ibmr.iova >> 32);
+	fr->tpte.va_lo_fbo = cpu_to_be32(mhp->ibmr.iova & 0xffffffff);
+
+	p[0] = cpu_to_be64((u64)mhp->mpl[0]);
+	p[1] = cpu_to_be64((u64)mhp->mpl[1]);
+
+	*len16 = DIV_ROUND_UP(sizeof(*fr), 16);
+}
+
 static int build_memreg(struct t4_sq *sq, union t4_wr *wqe,
-			struct ib_reg_wr *wr, u8 *len16, bool dsgl_supported)
+			struct ib_reg_wr *wr, struct c4iw_mr *mhp, u8 *len16,
+			bool dsgl_supported)
 {
-	struct c4iw_mr *mhp = to_c4iw_mr(wr->mr);
 	struct fw_ri_immd *imdp;
 	__be64 *p;
 	int i;
@@ -674,9 +706,12 @@ static int build_memreg(struct t4_sq *sq, union t4_wr *wqe,
 	return 0;
 }
 
-static int build_inv_stag(union t4_wr *wqe, struct ib_send_wr *wr,
-			  u8 *len16)
+static int build_inv_stag(struct c4iw_dev *dev, union t4_wr *wqe,
+			  struct ib_send_wr *wr, u8 *len16)
 {
+	struct c4iw_mr *mhp = get_mhp(dev, wr->ex.invalidate_rkey >> 8);
+
+	mhp->attr.state = 0;
 	wqe->inv.stag_inv = cpu_to_be32(wr->ex.invalidate_rkey);
 	wqe->inv.r2 = 0;
 	*len16 = DIV_ROUND_UP(sizeof wqe->inv, 16);
@@ -816,18 +851,32 @@ int c4iw_post_send(struct ib_qp *ibqp, struct ib_send_wr *wr,
 			if (!qhp->wq.sq.oldest_read)
 				qhp->wq.sq.oldest_read = swsqe;
 			break;
-		case IB_WR_REG_MR:
-			fw_opcode = FW_RI_FR_NSMR_WR;
+		case IB_WR_REG_MR: {
+			struct c4iw_mr *mhp = to_c4iw_mr(reg_wr(wr)->mr);
+
 			swsqe->opcode = FW_RI_FAST_REGISTER;
-			err = build_memreg(&qhp->wq.sq, wqe, reg_wr(wr), &len16,
-				qhp->rhp->rdev.lldi.ulptx_memwrite_dsgl);
+			if (qhp->rhp->rdev.lldi.fr_nsmr_tpte_wr_support &&
+			    !mhp->attr.state && mhp->mpl_len <= 2) {
+				fw_opcode = FW_RI_FR_NSMR_TPTE_WR;
+				build_tpte_memreg(&wqe->fr_tpte, reg_wr(wr),
+						  mhp, &len16);
+			} else {
+				fw_opcode = FW_RI_FR_NSMR_WR;
+				err = build_memreg(&qhp->wq.sq, wqe, reg_wr(wr),
+				       mhp, &len16,
+				       qhp->rhp->rdev.lldi.ulptx_memwrite_dsgl);
+				if (err)
+					break;
+			}
+			mhp->attr.state = 1;
 			break;
+		}
 		case IB_WR_LOCAL_INV:
 			if (wr->send_flags & IB_SEND_FENCE)
 				fw_flags |= FW_RI_LOCAL_FENCE_FLAG;
 			fw_opcode = FW_RI_INV_LSTAG_WR;
 			swsqe->opcode = FW_RI_LOCAL_INV;
-			err = build_inv_stag(wqe, wr, &len16);
+			err = build_inv_stag(qhp->rhp, wqe, wr, &len16);
 			break;
 		default:
 			PDBG("%s post of type=%d TBD!\n", __func__,
diff --git a/drivers/infiniband/hw/cxgb4/t4.h b/drivers/infiniband/hw/cxgb4/t4.h
index 02173f4315fa..862381aa83c8 100644
--- a/drivers/infiniband/hw/cxgb4/t4.h
+++ b/drivers/infiniband/hw/cxgb4/t4.h
@@ -95,6 +95,7 @@ union t4_wr {
 	struct fw_ri_rdma_read_wr read;
 	struct fw_ri_bind_mw_wr bind;
 	struct fw_ri_fr_nsmr_wr fr;
+	struct fw_ri_fr_nsmr_tpte_wr fr_tpte;
 	struct fw_ri_inv_lstag_wr inv;
 	struct t4_status_page status;
 	__be64 flits[T4_EQ_ENTRY_SIZE / sizeof(__be64) * T4_SQ_NUM_SLOTS];
@@ -170,7 +171,7 @@ struct t4_cqe {
 			__be32 msn;
 		} rcqe;
 		struct {
-			u32 nada1;
+			u32 stag;
 			u16 nada2;
 			u16 cidx;
 		} scqe;
@@ -232,6 +233,7 @@ struct t4_cqe {
 
 /* used for SQ completion processing */
 #define CQE_WRID_SQ_IDX(x)	((x)->u.scqe.cidx)
+#define CQE_WRID_FR_STAG(x)     (be32_to_cpu((x)->u.scqe.stag))
 
 /* generic accessor macros */
 #define CQE_WRID_HI(x)		(be32_to_cpu((x)->u.gen.wrid_hi))
diff --git a/drivers/infiniband/hw/cxgb4/t4fw_ri_api.h b/drivers/infiniband/hw/cxgb4/t4fw_ri_api.h
index 1e26669793c3..010c709ba3bb 100644
--- a/drivers/infiniband/hw/cxgb4/t4fw_ri_api.h
+++ b/drivers/infiniband/hw/cxgb4/t4fw_ri_api.h
@@ -669,6 +669,18 @@ struct fw_ri_fr_nsmr_wr {
 #define FW_RI_FR_NSMR_WR_DCACPU_G(x)	\
 	(((x) >> FW_RI_FR_NSMR_WR_DCACPU_S) & FW_RI_FR_NSMR_WR_DCACPU_M)
 
+struct fw_ri_fr_nsmr_tpte_wr {
+	__u8	opcode;
+	__u8   flags;
+	__u16  wrid;
+	__u8   r1[3];
+	__u8   len16;
+	__u32  r2;
+	__u32  stag;
+	struct fw_ri_tpte tpte;
+	__u64  pbl[2];
+};
+
 struct fw_ri_inv_lstag_wr {
 	__u8   opcode;
 	__u8   flags;
diff --git a/drivers/infiniband/hw/hfi1/affinity.c b/drivers/infiniband/hw/hfi1/affinity.c
index 0566393e5aba..a26a9a0bfc41 100644
--- a/drivers/infiniband/hw/hfi1/affinity.c
+++ b/drivers/infiniband/hw/hfi1/affinity.c
@@ -47,6 +47,7 @@
 #include <linux/topology.h>
 #include <linux/cpumask.h>
 #include <linux/module.h>
+#include <linux/interrupt.h>
 
 #include "hfi.h"
 #include "affinity.h"
@@ -55,7 +56,7 @@
 
 struct hfi1_affinity_node_list node_affinity = {
 	.list = LIST_HEAD_INIT(node_affinity.list),
-	.lock = __SPIN_LOCK_UNLOCKED(&node_affinity.lock),
+	.lock = __MUTEX_INITIALIZER(node_affinity.lock)
 };
 
 /* Name of IRQ types, indexed by enum irq_type */
@@ -159,14 +160,14 @@ void node_affinity_destroy(void)
 	struct list_head *pos, *q;
 	struct hfi1_affinity_node *entry;
 
-	spin_lock(&node_affinity.lock);
+	mutex_lock(&node_affinity.lock);
 	list_for_each_safe(pos, q, &node_affinity.list) {
 		entry = list_entry(pos, struct hfi1_affinity_node,
 				   list);
 		list_del(pos);
 		kfree(entry);
 	}
-	spin_unlock(&node_affinity.lock);
+	mutex_unlock(&node_affinity.lock);
 	kfree(hfi1_per_node_cntr);
 }
 
@@ -233,9 +234,8 @@ int hfi1_dev_affinity_init(struct hfi1_devdata *dd)
 	if (cpumask_first(local_mask) >= nr_cpu_ids)
 		local_mask = topology_core_cpumask(0);
 
-	spin_lock(&node_affinity.lock);
+	mutex_lock(&node_affinity.lock);
 	entry = node_affinity_lookup(dd->node);
-	spin_unlock(&node_affinity.lock);
 
 	/*
 	 * If this is the first time this NUMA node's affinity is used,
@@ -246,6 +246,7 @@ int hfi1_dev_affinity_init(struct hfi1_devdata *dd)
 		if (!entry) {
 			dd_dev_err(dd,
 				   "Unable to allocate global affinity node\n");
+			mutex_unlock(&node_affinity.lock);
 			return -ENOMEM;
 		}
 		init_cpu_mask_set(&entry->def_intr);
@@ -302,15 +303,113 @@ int hfi1_dev_affinity_init(struct hfi1_devdata *dd)
 					     &entry->general_intr_mask);
 		}
 
-		spin_lock(&node_affinity.lock);
 		node_affinity_add_tail(entry);
-		spin_unlock(&node_affinity.lock);
 	}
-
+	mutex_unlock(&node_affinity.lock);
 	return 0;
 }
 
-int hfi1_get_irq_affinity(struct hfi1_devdata *dd, struct hfi1_msix_entry *msix)
+/*
+ * Function updates the irq affinity hint for msix after it has been changed
+ * by the user using the /proc/irq interface. This function only accepts
+ * one cpu in the mask.
+ */
+static void hfi1_update_sdma_affinity(struct hfi1_msix_entry *msix, int cpu)
+{
+	struct sdma_engine *sde = msix->arg;
+	struct hfi1_devdata *dd = sde->dd;
+	struct hfi1_affinity_node *entry;
+	struct cpu_mask_set *set;
+	int i, old_cpu;
+
+	if (cpu > num_online_cpus() || cpu == sde->cpu)
+		return;
+
+	mutex_lock(&node_affinity.lock);
+	entry = node_affinity_lookup(dd->node);
+	if (!entry)
+		goto unlock;
+
+	old_cpu = sde->cpu;
+	sde->cpu = cpu;
+	cpumask_clear(&msix->mask);
+	cpumask_set_cpu(cpu, &msix->mask);
+	dd_dev_dbg(dd, "IRQ vector: %u, type %s engine %u -> cpu: %d\n",
+		   msix->msix.vector, irq_type_names[msix->type],
+		   sde->this_idx, cpu);
+	irq_set_affinity_hint(msix->msix.vector, &msix->mask);
+
+	/*
+	 * Set the new cpu in the hfi1_affinity_node and clean
+	 * the old cpu if it is not used by any other IRQ
+	 */
+	set = &entry->def_intr;
+	cpumask_set_cpu(cpu, &set->mask);
+	cpumask_set_cpu(cpu, &set->used);
+	for (i = 0; i < dd->num_msix_entries; i++) {
+		struct hfi1_msix_entry *other_msix;
+
+		other_msix = &dd->msix_entries[i];
+		if (other_msix->type != IRQ_SDMA || other_msix == msix)
+			continue;
+
+		if (cpumask_test_cpu(old_cpu, &other_msix->mask))
+			goto unlock;
+	}
+	cpumask_clear_cpu(old_cpu, &set->mask);
+	cpumask_clear_cpu(old_cpu, &set->used);
+unlock:
+	mutex_unlock(&node_affinity.lock);
+}
+
+static void hfi1_irq_notifier_notify(struct irq_affinity_notify *notify,
+				     const cpumask_t *mask)
+{
+	int cpu = cpumask_first(mask);
+	struct hfi1_msix_entry *msix = container_of(notify,
+						    struct hfi1_msix_entry,
+						    notify);
+
+	/* Only one CPU configuration supported currently */
+	hfi1_update_sdma_affinity(msix, cpu);
+}
+
+static void hfi1_irq_notifier_release(struct kref *ref)
+{
+	/*
+	 * This is required by affinity notifier. We don't have anything to
+	 * free here.
+	 */
+}
+
+static void hfi1_setup_sdma_notifier(struct hfi1_msix_entry *msix)
+{
+	struct irq_affinity_notify *notify = &msix->notify;
+
+	notify->irq = msix->msix.vector;
+	notify->notify = hfi1_irq_notifier_notify;
+	notify->release = hfi1_irq_notifier_release;
+
+	if (irq_set_affinity_notifier(notify->irq, notify))
+		pr_err("Failed to register sdma irq affinity notifier for irq %d\n",
+		       notify->irq);
+}
+
+static void hfi1_cleanup_sdma_notifier(struct hfi1_msix_entry *msix)
+{
+	struct irq_affinity_notify *notify = &msix->notify;
+
+	if (irq_set_affinity_notifier(notify->irq, NULL))
+		pr_err("Failed to cleanup sdma irq affinity notifier for irq %d\n",
+		       notify->irq);
+}
+
+/*
+ * Function sets the irq affinity for msix.
+ * It *must* be called with node_affinity.lock held.
+ */
+static int get_irq_affinity(struct hfi1_devdata *dd,
+			    struct hfi1_msix_entry *msix)
 {
 	int ret;
 	cpumask_var_t diff;
@@ -328,9 +427,7 @@ int hfi1_get_irq_affinity(struct hfi1_devdata *dd, struct hfi1_msix_entry *msix)
 	if (!ret)
 		return -ENOMEM;
 
-	spin_lock(&node_affinity.lock);
 	entry = node_affinity_lookup(dd->node);
-	spin_unlock(&node_affinity.lock);
 
 	switch (msix->type) {
 	case IRQ_SDMA:
@@ -360,7 +457,6 @@ int hfi1_get_irq_affinity(struct hfi1_devdata *dd, struct hfi1_msix_entry *msix)
 	 * finds its CPU here.
 	 */
 	if (cpu == -1 && set) {
-		spin_lock(&node_affinity.lock);
 		if (cpumask_equal(&set->mask, &set->used)) {
 			/*
 			 * We've used up all the CPUs, bump up the generation
@@ -372,17 +468,6 @@ int hfi1_get_irq_affinity(struct hfi1_devdata *dd, struct hfi1_msix_entry *msix)
 		cpumask_andnot(diff, &set->mask, &set->used);
 		cpu = cpumask_first(diff);
 		cpumask_set_cpu(cpu, &set->used);
-		spin_unlock(&node_affinity.lock);
-	}
-
-	switch (msix->type) {
-	case IRQ_SDMA:
-		sde->cpu = cpu;
-		break;
-	case IRQ_GENERAL:
-	case IRQ_RCVCTXT:
-	case IRQ_OTHER:
-		break;
 	}
 
 	cpumask_set_cpu(cpu, &msix->mask);
@@ -391,10 +476,25 @@ int hfi1_get_irq_affinity(struct hfi1_devdata *dd, struct hfi1_msix_entry *msix)
 		    extra, cpu);
 	irq_set_affinity_hint(msix->msix.vector, &msix->mask);
 
+	if (msix->type == IRQ_SDMA) {
+		sde->cpu = cpu;
+		hfi1_setup_sdma_notifier(msix);
+	}
+
 	free_cpumask_var(diff);
 	return 0;
 }
 
+int hfi1_get_irq_affinity(struct hfi1_devdata *dd, struct hfi1_msix_entry *msix)
+{
+	int ret;
+
+	mutex_lock(&node_affinity.lock);
+	ret = get_irq_affinity(dd, msix);
+	mutex_unlock(&node_affinity.lock);
+	return ret;
+}
+
 void hfi1_put_irq_affinity(struct hfi1_devdata *dd,
 			   struct hfi1_msix_entry *msix)
 {
@@ -402,13 +502,13 @@ void hfi1_put_irq_affinity(struct hfi1_devdata *dd,
 	struct hfi1_ctxtdata *rcd;
 	struct hfi1_affinity_node *entry;
 
-	spin_lock(&node_affinity.lock);
+	mutex_lock(&node_affinity.lock);
 	entry = node_affinity_lookup(dd->node);
-	spin_unlock(&node_affinity.lock);
 
 	switch (msix->type) {
 	case IRQ_SDMA:
 		set = &entry->def_intr;
+		hfi1_cleanup_sdma_notifier(msix);
 		break;
 	case IRQ_GENERAL:
 		/* Don't do accounting for general contexts */
@@ -420,21 +520,21 @@ void hfi1_put_irq_affinity(struct hfi1_devdata *dd,
 			set = &entry->rcv_intr;
 		break;
 	default:
+		mutex_unlock(&node_affinity.lock);
 		return;
 	}
 
 	if (set) {
-		spin_lock(&node_affinity.lock);
 		cpumask_andnot(&set->used, &set->used, &msix->mask);
 		if (cpumask_empty(&set->used) && set->gen) {
 			set->gen--;
 			cpumask_copy(&set->used, &set->mask);
 		}
-		spin_unlock(&node_affinity.lock);
 	}
 
 	irq_set_affinity_hint(msix->msix.vector, NULL);
 	cpumask_clear(&msix->mask);
+	mutex_unlock(&node_affinity.lock);
 }
 
 /* This should be called with node_affinity.lock held */
@@ -535,7 +635,7 @@ int hfi1_get_proc_affinity(int node)
 	if (!ret)
 		goto free_available_mask;
 
-	spin_lock(&affinity->lock);
+	mutex_lock(&affinity->lock);
 	/*
 	 * If we've used all available HW threads, clear the mask and start
 	 * overloading.
@@ -643,7 +743,8 @@ int hfi1_get_proc_affinity(int node)
 		cpu = -1;
 	else
 		cpumask_set_cpu(cpu, &set->used);
-	spin_unlock(&affinity->lock);
+
+	mutex_unlock(&affinity->lock);
 	hfi1_cdbg(PROC, "Process assigned to CPU %d", cpu);
 
 	free_cpumask_var(intrs_mask);
@@ -664,19 +765,17 @@ void hfi1_put_proc_affinity(int cpu)
 
 	if (cpu < 0)
 		return;
-	spin_lock(&affinity->lock);
+
+	mutex_lock(&affinity->lock);
 	cpumask_clear_cpu(cpu, &set->used);
 	hfi1_cdbg(PROC, "Returning CPU %d for future process assignment", cpu);
 	if (cpumask_empty(&set->used) && set->gen) {
 		set->gen--;
 		cpumask_copy(&set->used, &set->mask);
 	}
-	spin_unlock(&affinity->lock);
+	mutex_unlock(&affinity->lock);
 }
 
-/* Prevents concurrent reads and writes of the sdma_affinity attrib */
-static DEFINE_MUTEX(sdma_affinity_mutex);
-
 int hfi1_set_sdma_affinity(struct hfi1_devdata *dd, const char *buf,
 			   size_t count)
 {
@@ -684,16 +783,19 @@ int hfi1_set_sdma_affinity(struct hfi1_devdata *dd, const char *buf,
 	cpumask_var_t mask;
 	int ret, i;
 
-	spin_lock(&node_affinity.lock);
+	mutex_lock(&node_affinity.lock);
 	entry = node_affinity_lookup(dd->node);
-	spin_unlock(&node_affinity.lock);
 
-	if (!entry)
-		return -EINVAL;
+	if (!entry) {
+		ret = -EINVAL;
+		goto unlock;
+	}
 
 	ret = zalloc_cpumask_var(&mask, GFP_KERNEL);
-	if (!ret)
-		return -ENOMEM;
+	if (!ret) {
+		ret = -ENOMEM;
+		goto unlock;
+	}
 
 	ret = cpulist_parse(buf, mask);
 	if (ret)
@@ -705,13 +807,11 @@ int hfi1_set_sdma_affinity(struct hfi1_devdata *dd, const char *buf,
 		goto out;
 	}
 
-	mutex_lock(&sdma_affinity_mutex);
 	/* reset the SDMA interrupt affinity details */
 	init_cpu_mask_set(&entry->def_intr);
 	cpumask_copy(&entry->def_intr.mask, mask);
-	/*
-	 * Reassign the affinity for each SDMA interrupt.
-	 */
+
+	/* Reassign the affinity for each SDMA interrupt. */
 	for (i = 0; i < dd->num_msix_entries; i++) {
 		struct hfi1_msix_entry *msix;
 
@@ -719,14 +819,15 @@ int hfi1_set_sdma_affinity(struct hfi1_devdata *dd, const char *buf,
 		if (msix->type != IRQ_SDMA)
 			continue;
 
-		ret = hfi1_get_irq_affinity(dd, msix);
+		ret = get_irq_affinity(dd, msix);
 
 		if (ret)
 			break;
 	}
-	mutex_unlock(&sdma_affinity_mutex);
 out:
 	free_cpumask_var(mask);
+unlock:
+	mutex_unlock(&node_affinity.lock);
 	return ret ? ret : strnlen(buf, PAGE_SIZE);
 }
 
@@ -734,15 +835,15 @@ int hfi1_get_sdma_affinity(struct hfi1_devdata *dd, char *buf)
 {
 	struct hfi1_affinity_node *entry;
 
-	spin_lock(&node_affinity.lock);
+	mutex_lock(&node_affinity.lock);
 	entry = node_affinity_lookup(dd->node);
-	spin_unlock(&node_affinity.lock);
 
-	if (!entry)
+	if (!entry) {
+		mutex_unlock(&node_affinity.lock);
 		return -EINVAL;
+	}
 
-	mutex_lock(&sdma_affinity_mutex);
 	cpumap_print_to_pagebuf(true, buf, &entry->def_intr.mask);
-	mutex_unlock(&sdma_affinity_mutex);
+	mutex_unlock(&node_affinity.lock);
 	return strnlen(buf, PAGE_SIZE);
 }
diff --git a/drivers/infiniband/hw/hfi1/affinity.h b/drivers/infiniband/hw/hfi1/affinity.h
index 8879cf7a8cac..b89ea3c0ee1a 100644
--- a/drivers/infiniband/hw/hfi1/affinity.h
+++ b/drivers/infiniband/hw/hfi1/affinity.h
@@ -121,8 +121,7 @@ struct hfi1_affinity_node_list {
 	int num_core_siblings;
 	int num_online_nodes;
 	int num_online_cpus;
-	/* protect affinity node list */
-	spinlock_t lock;
+	struct mutex lock; /* protects affinity nodes */
 };
 
 int node_affinity_init(void);
diff --git a/drivers/infiniband/hw/hfi1/chip.c b/drivers/infiniband/hw/hfi1/chip.c
index cc38004cea42..9bf5f23544d4 100644
--- a/drivers/infiniband/hw/hfi1/chip.c
+++ b/drivers/infiniband/hw/hfi1/chip.c
@@ -971,7 +971,9 @@ static struct flag_table dc8051_info_err_flags[] = {
 	FLAG_ENTRY0("Failed LNI(VerifyCap_1)", FAILED_LNI_VERIFY_CAP1),
 	FLAG_ENTRY0("Failed LNI(VerifyCap_2)", FAILED_LNI_VERIFY_CAP2),
 	FLAG_ENTRY0("Failed LNI(ConfigLT)",    FAILED_LNI_CONFIGLT),
-	FLAG_ENTRY0("Host Handshake Timeout",  HOST_HANDSHAKE_TIMEOUT)
+	FLAG_ENTRY0("Host Handshake Timeout",  HOST_HANDSHAKE_TIMEOUT),
+	FLAG_ENTRY0("External Device Request Timeout",
+		    EXTERNAL_DEVICE_REQ_TIMEOUT),
 };
 
 /*
@@ -6825,7 +6827,6 @@ void handle_link_up(struct work_struct *work)
 		set_link_down_reason(ppd, OPA_LINKDOWN_REASON_SPEED_POLICY, 0,
 				     OPA_LINKDOWN_REASON_SPEED_POLICY);
 		set_link_state(ppd, HLS_DN_OFFLINE);
-		tune_serdes(ppd);
 		start_link(ppd);
 	}
 }
@@ -6998,12 +6999,10 @@ void handle_link_down(struct work_struct *work)
 	 * If there is no cable attached, turn the DC off. Otherwise,
 	 * start the link bring up.
 	 */
-	if (ppd->port_type == PORT_TYPE_QSFP && !qsfp_mod_present(ppd)) {
+	if (ppd->port_type == PORT_TYPE_QSFP && !qsfp_mod_present(ppd))
 		dc_shutdown(ppd->dd);
-	} else {
-		tune_serdes(ppd);
+	else
 		start_link(ppd);
-	}
 }
 
 void handle_link_bounce(struct work_struct *work)
@@ -7016,7 +7015,6 @@ void handle_link_bounce(struct work_struct *work)
 	 */
 	if (ppd->host_link_state & HLS_UP) {
 		set_link_state(ppd, HLS_DN_OFFLINE);
-		tune_serdes(ppd);
 		start_link(ppd);
 	} else {
 		dd_dev_info(ppd->dd, "%s: link not up (%s), nothing to do\n",
@@ -7531,7 +7529,6 @@ done:
 		set_link_down_reason(ppd, OPA_LINKDOWN_REASON_WIDTH_POLICY, 0,
 				     OPA_LINKDOWN_REASON_WIDTH_POLICY);
 		set_link_state(ppd, HLS_DN_OFFLINE);
-		tune_serdes(ppd);
 		start_link(ppd);
 	}
 }
@@ -9161,6 +9158,12 @@ set_local_link_attributes_fail:
  */
 int start_link(struct hfi1_pportdata *ppd)
 {
+	/*
+	 * Tune the SerDes to a ballpark setting for optimal signal and bit
+	 * error rate.  Needs to be done before starting the link.
+	 */
+	tune_serdes(ppd);
+
 	if (!ppd->link_enabled) {
 		dd_dev_info(ppd->dd,
 			    "%s: stopping link start because link is disabled\n",
@@ -9401,8 +9404,6 @@ void qsfp_event(struct work_struct *work)
 		 */
 		set_qsfp_int_n(ppd, 1);
 
-		tune_serdes(ppd);
-
 		start_link(ppd);
 	}
 
@@ -9544,11 +9545,6 @@ static void try_start_link(struct hfi1_pportdata *ppd)
 	}
 	ppd->qsfp_retry_count = 0;
 
-	/*
-	 * Tune the SerDes to a ballpark setting for optimal signal and bit
-	 * error rate.  Needs to be done before starting the link.
-	 */
-	tune_serdes(ppd);
 	start_link(ppd);
 }
 
@@ -9718,12 +9714,12 @@ void hfi1_clear_tids(struct hfi1_ctxtdata *rcd)
 		hfi1_put_tid(dd, i, PT_INVALID, 0, 0);
 }
 
-struct hfi1_message_header *hfi1_get_msgheader(
-				struct hfi1_devdata *dd, __le32 *rhf_addr)
+struct ib_header *hfi1_get_msgheader(
+	struct hfi1_devdata *dd, __le32 *rhf_addr)
 {
 	u32 offset = rhf_hdrq_offset(rhf_to_cpu(rhf_addr));
 
-	return (struct hfi1_message_header *)
+	return (struct ib_header *)
 		(rhf_addr - dd->rhf_offset + offset);
 }
 
@@ -11559,10 +11555,10 @@ void hfi1_rcvctrl(struct hfi1_devdata *dd, unsigned int op, int ctxt)
 	    !(rcvctrl & RCV_CTXT_CTRL_ENABLE_SMASK)) {
 		/* reset the tail and hdr addresses, and sequence count */
 		write_kctxt_csr(dd, ctxt, RCV_HDR_ADDR,
-				rcd->rcvhdrq_phys);
+				rcd->rcvhdrq_dma);
 		if (HFI1_CAP_KGET_MASK(rcd->flags, DMA_RTAIL))
 			write_kctxt_csr(dd, ctxt, RCV_HDR_TAIL_ADDR,
-					rcd->rcvhdrqtailaddr_phys);
+					rcd->rcvhdrqtailaddr_dma);
 		rcd->seq_cnt = 1;
 
 		/* reset the cached receive header queue head value */
@@ -11627,9 +11623,9 @@ void hfi1_rcvctrl(struct hfi1_devdata *dd, unsigned int op, int ctxt)
 		 * update with a dummy tail address and then disable
 		 * receive context.
 		 */
-		if (dd->rcvhdrtail_dummy_physaddr) {
+		if (dd->rcvhdrtail_dummy_dma) {
 			write_kctxt_csr(dd, ctxt, RCV_HDR_TAIL_ADDR,
-					dd->rcvhdrtail_dummy_physaddr);
+					dd->rcvhdrtail_dummy_dma);
 			/* Enabling RcvCtxtCtrl.TailUpd is intentional. */
 			rcvctrl |= RCV_CTXT_CTRL_TAIL_UPD_SMASK;
 		}
@@ -11640,7 +11636,7 @@ void hfi1_rcvctrl(struct hfi1_devdata *dd, unsigned int op, int ctxt)
 		rcvctrl |= RCV_CTXT_CTRL_INTR_AVAIL_SMASK;
 	if (op & HFI1_RCVCTRL_INTRAVAIL_DIS)
 		rcvctrl &= ~RCV_CTXT_CTRL_INTR_AVAIL_SMASK;
-	if (op & HFI1_RCVCTRL_TAILUPD_ENB && rcd->rcvhdrqtailaddr_phys)
+	if (op & HFI1_RCVCTRL_TAILUPD_ENB && rcd->rcvhdrqtailaddr_dma)
 		rcvctrl |= RCV_CTXT_CTRL_TAIL_UPD_SMASK;
 	if (op & HFI1_RCVCTRL_TAILUPD_DIS) {
 		/* See comment on RcvCtxtCtrl.TailUpd above */
@@ -11712,7 +11708,7 @@ void hfi1_rcvctrl(struct hfi1_devdata *dd, unsigned int op, int ctxt)
 		 * so it doesn't contain an address that is invalid.
 		 */
 		write_kctxt_csr(dd, ctxt, RCV_HDR_TAIL_ADDR,
-				dd->rcvhdrtail_dummy_physaddr);
+				dd->rcvhdrtail_dummy_dma);
 }
 
 u32 hfi1_read_cntrs(struct hfi1_devdata *dd, char **namep, u64 **cntrp)
@@ -13389,9 +13385,9 @@ static void init_rbufs(struct hfi1_devdata *dd)
 		/*
 		 * Give up after 1ms - maximum wait time.
 		 *
-		 * RBuf size is 148KiB.  Slowest possible is PCIe Gen1 x1 at
+		 * RBuf size is 136KiB.  Slowest possible is PCIe Gen1 x1 at
 		 * 250MB/s bandwidth.  Lower rate to 66% for overhead to get:
-		 *	148 KB / (66% * 250MB/s) = 920us
+		 *	136 KB / (66% * 250MB/s) = 844us
 		 */
 		if (count++ > 500) {
 			dd_dev_err(dd,
@@ -14570,6 +14566,11 @@ struct hfi1_devdata *hfi1_init_dd(struct pci_dev *pdev,
 	if (ret)
 		goto bail_cleanup;
 
+	/* call before get_platform_config(), after init_chip_resources() */
+	ret = eprom_init(dd);
+	if (ret)
+		goto bail_free_rcverr;
+
 	/* Needs to be called before hfi1_firmware_init */
 	get_platform_config(dd);
 
@@ -14690,10 +14691,6 @@ struct hfi1_devdata *hfi1_init_dd(struct pci_dev *pdev,
 	if (ret)
 		goto bail_free_cntrs;
 
-	ret = eprom_init(dd);
-	if (ret)
-		goto bail_free_rcverr;
-
 	goto bail;
 
 bail_free_rcverr:
diff --git a/drivers/infiniband/hw/hfi1/chip.h b/drivers/infiniband/hw/hfi1/chip.h
index e29573769efc..92345259a8f4 100644
--- a/drivers/infiniband/hw/hfi1/chip.h
+++ b/drivers/infiniband/hw/hfi1/chip.h
@@ -82,7 +82,7 @@
  */
 #define CM_VAU 3
 /* HFI link credit count, AKA receive buffer depth (RBUF_DEPTH) */
-#define CM_GLOBAL_CREDITS 0x940
+#define CM_GLOBAL_CREDITS 0x880
 /* Number of PKey entries in the HW */
 #define MAX_PKEY_VALUES 16
 
@@ -254,12 +254,14 @@
 #define FAILED_LNI_VERIFY_CAP2		BIT(10)
 #define FAILED_LNI_CONFIGLT		BIT(11)
 #define HOST_HANDSHAKE_TIMEOUT		BIT(12)
+#define EXTERNAL_DEVICE_REQ_TIMEOUT	BIT(13)
 
 #define FAILED_LNI (FAILED_LNI_POLLING | FAILED_LNI_DEBOUNCE \
 			| FAILED_LNI_ESTBCOMM | FAILED_LNI_OPTEQ \
 			| FAILED_LNI_VERIFY_CAP1 \
 			| FAILED_LNI_VERIFY_CAP2 \
-			| FAILED_LNI_CONFIGLT | HOST_HANDSHAKE_TIMEOUT)
+			| FAILED_LNI_CONFIGLT | HOST_HANDSHAKE_TIMEOUT \
+			| EXTERNAL_DEVICE_REQ_TIMEOUT)
 
 /* DC_DC8051_DBG_ERR_INFO_SET_BY_8051.HOST_MSG - host message flags */
 #define HOST_REQ_DONE		BIT(0)
@@ -1336,7 +1338,7 @@ enum {
 u64 get_all_cpu_total(u64 __percpu *cntr);
 void hfi1_start_cleanup(struct hfi1_devdata *dd);
 void hfi1_clear_tids(struct hfi1_ctxtdata *rcd);
-struct hfi1_message_header *hfi1_get_msgheader(
+struct ib_header *hfi1_get_msgheader(
 				struct hfi1_devdata *dd, __le32 *rhf_addr);
 int hfi1_init_ctxt(struct send_context *sc);
 void hfi1_put_tid(struct hfi1_devdata *dd, u32 index,
diff --git a/drivers/infiniband/hw/hfi1/common.h b/drivers/infiniband/hw/hfi1/common.h
index fcc9c217a97a..da7be21bedb4 100644
--- a/drivers/infiniband/hw/hfi1/common.h
+++ b/drivers/infiniband/hw/hfi1/common.h
@@ -320,14 +320,6 @@ struct diag_pkt {
 /* RHF receive type error - bypass packet errors */
 #define RHF_RTE_BYPASS_NO_ERR		0x0
 
-/*
- * This structure contains the first field common to all protocols
- * that employ this chip.
- */
-struct hfi1_message_header {
-	__be16 lrh[4];
-};
-
 /* IB - LRH header constants */
 #define HFI1_LRH_GRH 0x0003      /* 1. word of IB LRH - next header: GRH */
 #define HFI1_LRH_BTH 0x0002      /* 1. word of IB LRH - next header: BTH */
diff --git a/drivers/infiniband/hw/hfi1/debugfs.c b/drivers/infiniband/hw/hfi1/debugfs.c
index 5e9be16f6cd3..632ba21759ab 100644
--- a/drivers/infiniband/hw/hfi1/debugfs.c
+++ b/drivers/infiniband/hw/hfi1/debugfs.c
@@ -933,6 +933,43 @@ static const struct counter_info port_cntr_ops[] = {
 	DEBUGFS_OPS("asic_flags", asic_flags_read, asic_flags_write),
 };
 
+static void *_sdma_cpu_list_seq_start(struct seq_file *s, loff_t *pos)
+{
+	if (*pos >= num_online_cpus())
+		return NULL;
+
+	return pos;
+}
+
+static void *_sdma_cpu_list_seq_next(struct seq_file *s, void *v, loff_t *pos)
+{
+	++*pos;
+	if (*pos >= num_online_cpus())
+		return NULL;
+
+	return pos;
+}
+
+static void _sdma_cpu_list_seq_stop(struct seq_file *s, void *v)
+{
+	/* nothing allocated */
+}
+
+static int _sdma_cpu_list_seq_show(struct seq_file *s, void *v)
+{
+	struct hfi1_ibdev *ibd = (struct hfi1_ibdev *)s->private;
+	struct hfi1_devdata *dd = dd_from_dev(ibd);
+	loff_t *spos = v;
+	loff_t i = *spos;
+
+	sdma_seqfile_dump_cpu_list(s, dd, (unsigned long)i);
+	return 0;
+}
+
+DEBUGFS_SEQ_FILE_OPS(sdma_cpu_list);
+DEBUGFS_SEQ_FILE_OPEN(sdma_cpu_list)
+DEBUGFS_FILE_OPS(sdma_cpu_list);
+
 void hfi1_dbg_ibdev_init(struct hfi1_ibdev *ibd)
 {
 	char name[sizeof("port0counters") + 1];
@@ -961,6 +998,7 @@ void hfi1_dbg_ibdev_init(struct hfi1_ibdev *ibd)
 	DEBUGFS_SEQ_FILE_CREATE(ctx_stats, ibd->hfi1_ibdev_dbg, ibd);
 	DEBUGFS_SEQ_FILE_CREATE(qp_stats, ibd->hfi1_ibdev_dbg, ibd);
 	DEBUGFS_SEQ_FILE_CREATE(sdes, ibd->hfi1_ibdev_dbg, ibd);
+	DEBUGFS_SEQ_FILE_CREATE(sdma_cpu_list, ibd->hfi1_ibdev_dbg, ibd);
 	/* dev counter files */
 	for (i = 0; i < ARRAY_SIZE(cntr_ops); i++)
 		DEBUGFS_FILE_CREATE(cntr_ops[i].name,
diff --git a/drivers/infiniband/hw/hfi1/driver.c b/drivers/infiniband/hw/hfi1/driver.c
index 303f10555729..6563e4d38b80 100644
--- a/drivers/infiniband/hw/hfi1/driver.c
+++ b/drivers/infiniband/hw/hfi1/driver.c
@@ -276,7 +276,7 @@ inline int hfi1_rcvbuf_validate(u32 size, u8 type, u16 *encoded)
 static void rcv_hdrerr(struct hfi1_ctxtdata *rcd, struct hfi1_pportdata *ppd,
 		       struct hfi1_packet *packet)
 {
-	struct hfi1_message_header *rhdr = packet->hdr;
+	struct ib_header *rhdr = packet->hdr;
 	u32 rte = rhf_rcv_type_err(packet->rhf);
 	int lnh = be16_to_cpu(rhdr->lrh[0]) & 3;
 	struct hfi1_ibport *ibp = &ppd->ibport_data;
@@ -288,10 +288,9 @@ static void rcv_hdrerr(struct hfi1_ctxtdata *rcd, struct hfi1_pportdata *ppd,
 
 	if (packet->rhf & RHF_TID_ERR) {
 		/* For TIDERR and RC QPs preemptively schedule a NAK */
-		struct hfi1_ib_header *hdr = (struct hfi1_ib_header *)rhdr;
-		struct hfi1_other_headers *ohdr = NULL;
+		struct ib_other_headers *ohdr = NULL;
 		u32 tlen = rhf_pkt_len(packet->rhf); /* in bytes */
-		u16 lid  = be16_to_cpu(hdr->lrh[1]);
+		u16 lid  = be16_to_cpu(rhdr->lrh[1]);
 		u32 qp_num;
 		u32 rcv_flags = 0;
 
@@ -301,14 +300,14 @@ static void rcv_hdrerr(struct hfi1_ctxtdata *rcd, struct hfi1_pportdata *ppd,
 
 		/* Check for GRH */
 		if (lnh == HFI1_LRH_BTH) {
-			ohdr = &hdr->u.oth;
+			ohdr = &rhdr->u.oth;
 		} else if (lnh == HFI1_LRH_GRH) {
 			u32 vtf;
 
-			ohdr = &hdr->u.l.oth;
-			if (hdr->u.l.grh.next_hdr != IB_GRH_NEXT_HDR)
+			ohdr = &rhdr->u.l.oth;
+			if (rhdr->u.l.grh.next_hdr != IB_GRH_NEXT_HDR)
 				goto drop;
-			vtf = be32_to_cpu(hdr->u.l.grh.version_tclass_flow);
+			vtf = be32_to_cpu(rhdr->u.l.grh.version_tclass_flow);
 			if ((vtf >> IB_GRH_VERSION_SHIFT) != IB_GRH_VERSION)
 				goto drop;
 			rcv_flags |= HFI1_HAS_GRH;
@@ -344,7 +343,7 @@ static void rcv_hdrerr(struct hfi1_ctxtdata *rcd, struct hfi1_pportdata *ppd,
 			case IB_QPT_RC:
 				hfi1_rc_hdrerr(
 					rcd,
-					hdr,
+					rhdr,
 					rcv_flags,
 					qp);
 				break;
@@ -452,8 +451,8 @@ void hfi1_process_ecn_slowpath(struct rvt_qp *qp, struct hfi1_packet *pkt,
 			       bool do_cnp)
 {
 	struct hfi1_ibport *ibp = to_iport(qp->ibqp.device, qp->port_num);
-	struct hfi1_ib_header *hdr = pkt->hdr;
-	struct hfi1_other_headers *ohdr = pkt->ohdr;
+	struct ib_header *hdr = pkt->hdr;
+	struct ib_other_headers *ohdr = pkt->ohdr;
 	struct ib_grh *grh = NULL;
 	u32 rqpn = 0, bth1;
 	u16 rlid, dlid = be16_to_cpu(hdr->lrh[1]);
@@ -487,7 +486,7 @@ void hfi1_process_ecn_slowpath(struct rvt_qp *qp, struct hfi1_packet *pkt,
 		return;
 	}
 
-	sc = hdr2sc((struct hfi1_message_header *)hdr, pkt->rhf);
+	sc = hdr2sc(hdr, pkt->rhf);
 
 	bth1 = be32_to_cpu(ohdr->bth[1]);
 	if (do_cnp && (bth1 & HFI1_FECN_SMASK)) {
@@ -599,8 +598,8 @@ static void __prescan_rxq(struct hfi1_packet *packet)
 		__le32 *rhf_addr = (__le32 *)rcd->rcvhdrq + mdata.ps_head +
 					 dd->rhf_offset;
 		struct rvt_qp *qp;
-		struct hfi1_ib_header *hdr;
-		struct hfi1_other_headers *ohdr;
+		struct ib_header *hdr;
+		struct ib_other_headers *ohdr;
 		struct rvt_dev_info *rdi = &dd->verbs_dev.rdi;
 		u64 rhf = rhf_to_cpu(rhf_addr);
 		u32 etype = rhf_rcv_type(rhf), qpn, bth1;
@@ -616,8 +615,8 @@ static void __prescan_rxq(struct hfi1_packet *packet)
 		if (etype != RHF_RCV_TYPE_IB)
 			goto next;
 
-		hdr = (struct hfi1_ib_header *)
-			hfi1_get_msgheader(dd, rhf_addr);
+		hdr = hfi1_get_msgheader(dd, rhf_addr);
+
 		lnh = be16_to_cpu(hdr->lrh[0]) & 3;
 
 		if (lnh == HFI1_LRH_BTH)
@@ -892,8 +891,8 @@ static inline int set_armed_to_active(struct hfi1_ctxtdata *rcd,
 				      struct hfi1_devdata *dd)
 {
 	struct work_struct *lsaw = &rcd->ppd->linkstate_active_work;
-	struct hfi1_message_header *hdr = hfi1_get_msgheader(packet->rcd->dd,
-							     packet->rhf_addr);
+	struct ib_header *hdr = hfi1_get_msgheader(packet->rcd->dd,
+						   packet->rhf_addr);
 	u8 etype = rhf_rcv_type(packet->rhf);
 
 	if (etype == RHF_RCV_TYPE_IB && hdr2sc(hdr, packet->rhf) != 0xf) {
diff --git a/drivers/infiniband/hw/hfi1/eprom.c b/drivers/infiniband/hw/hfi1/eprom.c
index 36b77943cbfd..e70c223801b4 100644
--- a/drivers/infiniband/hw/hfi1/eprom.c
+++ b/drivers/infiniband/hw/hfi1/eprom.c
@@ -49,7 +49,26 @@
 #include "common.h"
 #include "eprom.h"
 
+/*
+ * The EPROM is logically divided into three partitions:
+ *	partition 0: the first 128K, visible from PCI ROM BAR
+ *	partition 1: 4K config file (sector size)
+ *	partition 2: the rest
+ */
+#define P0_SIZE (128 * 1024)
+#define P1_SIZE   (4 * 1024)
+#define P1_START P0_SIZE
+#define P2_START (P0_SIZE + P1_SIZE)
+
+/* controller page size, in bytes */
+#define EP_PAGE_SIZE 256
+#define EP_PAGE_MASK (EP_PAGE_SIZE - 1)
+#define EP_PAGE_DWORDS (EP_PAGE_SIZE / sizeof(u32))
+
+/* controller commands */
 #define CMD_SHIFT 24
+#define CMD_NOP			    (0)
+#define CMD_READ_DATA(addr)	    ((0x03 << CMD_SHIFT) | addr)
 #define CMD_RELEASE_POWERDOWN_NOID  ((0xab << CMD_SHIFT))
 
 /* controller interface speeds */
@@ -61,6 +80,90 @@
  * Double it for safety.
  */
 #define EPROM_TIMEOUT 80000 /* ms */
+
+/*
+ * Read a 256 byte (64 dword) EPROM page.
+ * All callers have verified the offset is at a page boundary.
+ */
+static void read_page(struct hfi1_devdata *dd, u32 offset, u32 *result)
+{
+	int i;
+
+	write_csr(dd, ASIC_EEP_ADDR_CMD, CMD_READ_DATA(offset));
+	for (i = 0; i < EP_PAGE_DWORDS; i++)
+		result[i] = (u32)read_csr(dd, ASIC_EEP_DATA);
+	write_csr(dd, ASIC_EEP_ADDR_CMD, CMD_NOP); /* close open page */
+}
+
+/*
+ * Read length bytes starting at offset from the start of the EPROM.
+ */
+static int read_length(struct hfi1_devdata *dd, u32 start, u32 len, void *dest)
+{
+	u32 buffer[EP_PAGE_DWORDS];
+	u32 end;
+	u32 start_offset;
+	u32 read_start;
+	u32 bytes;
+
+	if (len == 0)
+		return 0;
+
+	end = start + len;
+
+	/*
+	 * Make sure the read range is not outside of the controller read
+	 * command address range.  Note that '>' is correct below - the end
+	 * of the range is OK if it stops at the limit, but no higher.
+	 */
+	if (end > (1 << CMD_SHIFT))
+		return -EINVAL;
+
+	/* read the first partial page */
+	start_offset = start & EP_PAGE_MASK;
+	if (start_offset) {
+		/* partial starting page */
+
+		/* align and read the page that contains the start */
+		read_start = start & ~EP_PAGE_MASK;
+		read_page(dd, read_start, buffer);
+
+		/* the rest of the page is available data */
+		bytes = EP_PAGE_SIZE - start_offset;
+
+		if (len <= bytes) {
+			/* end is within this page */
+			memcpy(dest, (u8 *)buffer + start_offset, len);
+			return 0;
+		}
+
+		memcpy(dest, (u8 *)buffer + start_offset, bytes);
+
+		start += bytes;
+		len -= bytes;
+		dest += bytes;
+	}
+	/* start is now page aligned */
+
+	/* read whole pages */
+	while (len >= EP_PAGE_SIZE) {
+		read_page(dd, start, buffer);
+		memcpy(dest, buffer, EP_PAGE_SIZE);
+
+		start += EP_PAGE_SIZE;
+		len -= EP_PAGE_SIZE;
+		dest += EP_PAGE_SIZE;
+	}
+
+	/* read the last partial page */
+	if (len) {
+		read_page(dd, start, buffer);
+		memcpy(dest, buffer, len);
+	}
+
+	return 0;
+}
+
 /*
  * Initialize the EPROM handler.
  */
@@ -100,3 +203,85 @@ int eprom_init(struct hfi1_devdata *dd)
 done_asic:
 	return ret;
 }
+
+/* magic character sequence that trails an image */
+#define IMAGE_TRAIL_MAGIC "egamiAPO"
+
+/*
+ * Read all of partition 1.  The actual file is at the front.  Adjust
+ * the returned size if a trailing image magic is found.
+ */
+static int read_partition_platform_config(struct hfi1_devdata *dd, void **data,
+					  u32 *size)
+{
+	void *buffer;
+	void *p;
+	u32 length;
+	int ret;
+
+	buffer = kmalloc(P1_SIZE, GFP_KERNEL);
+	if (!buffer)
+		return -ENOMEM;
+
+	ret = read_length(dd, P1_START, P1_SIZE, buffer);
+	if (ret) {
+		kfree(buffer);
+		return ret;
+	}
+
+	/* scan for image magic that may trail the actual data */
+	p = strnstr(buffer, IMAGE_TRAIL_MAGIC, P1_SIZE);
+	if (p)
+		length = p - buffer;
+	else
+		length = P1_SIZE;
+
+	*data = buffer;
+	*size = length;
+	return 0;
+}
+
+/*
+ * Read the platform configuration file from the EPROM.
+ *
+ * On success, an allocated buffer containing the data and its size are
+ * returned.  It is up to the caller to free this buffer.
+ *
+ * Return value:
+ *   0	      - success
+ *   -ENXIO   - no EPROM is available
+ *   -EBUSY   - not able to acquire access to the EPROM
+ *   -ENOENT  - no recognizable file written
+ *   -ENOMEM  - buffer could not be allocated
+ */
+int eprom_read_platform_config(struct hfi1_devdata *dd, void **data, u32 *size)
+{
+	u32 directory[EP_PAGE_DWORDS]; /* aligned buffer */
+	int ret;
+
+	if (!dd->eprom_available)
+		return -ENXIO;
+
+	ret = acquire_chip_resource(dd, CR_EPROM, EPROM_TIMEOUT);
+	if (ret)
+		return -EBUSY;
+
+	/* read the last page of P0 for the EPROM format magic */
+	ret = read_length(dd, P1_START - EP_PAGE_SIZE, EP_PAGE_SIZE, directory);
+	if (ret)
+		goto done;
+
+	/* last dword of P0 contains a magic indicator */
+	if (directory[EP_PAGE_DWORDS - 1] == 0) {
+		/* partition format */
+		ret = read_partition_platform_config(dd, data, size);
+		goto done;
+	}
+
+	/* nothing recognized */
+	ret = -ENOENT;
+
+done:
+	release_chip_resource(dd, CR_EPROM);
+	return ret;
+}
diff --git a/drivers/infiniband/hw/hfi1/eprom.h b/drivers/infiniband/hw/hfi1/eprom.h
index d41f0b1afb15..e774184f1643 100644
--- a/drivers/infiniband/hw/hfi1/eprom.h
+++ b/drivers/infiniband/hw/hfi1/eprom.h
@@ -45,8 +45,8 @@
  *
  */
 
-struct hfi1_cmd;
 struct hfi1_devdata;
 
 int eprom_init(struct hfi1_devdata *dd);
-int handle_eprom_command(struct file *fp, const struct hfi1_cmd *cmd);
+int eprom_read_platform_config(struct hfi1_devdata *dd, void **buf_ret,
+			       u32 *size_ret);
diff --git a/drivers/infiniband/hw/hfi1/file_ops.c b/drivers/infiniband/hw/hfi1/file_ops.c
index 7e03ccd2554d..677efa0e8cd6 100644
--- a/drivers/infiniband/hw/hfi1/file_ops.c
+++ b/drivers/infiniband/hw/hfi1/file_ops.c
@@ -58,7 +58,6 @@
 #include "trace.h"
 #include "user_sdma.h"
 #include "user_exp_rcv.h"
-#include "eprom.h"
 #include "aspm.h"
 #include "mmu_rb.h"
 
@@ -440,9 +439,10 @@ static int hfi1_file_mmap(struct file *fp, struct vm_area_struct *vma)
 	struct hfi1_filedata *fd = fp->private_data;
 	struct hfi1_ctxtdata *uctxt = fd->uctxt;
 	struct hfi1_devdata *dd;
-	unsigned long flags, pfn;
+	unsigned long flags;
 	u64 token = vma->vm_pgoff << PAGE_SHIFT,
 		memaddr = 0;
+	void *memvirt = NULL;
 	u8 subctxt, mapio = 0, vmf = 0, type;
 	ssize_t memlen = 0;
 	int ret = 0;
@@ -493,7 +493,8 @@ static int hfi1_file_mmap(struct file *fp, struct vm_area_struct *vma)
 		 * second or third page allocated for credit returns (if number
 		 * of enabled contexts > 64 and 128 respectively).
 		 */
-		memaddr = dd->cr_base[uctxt->numa_id].pa +
+		memvirt = dd->cr_base[uctxt->numa_id].va;
+		memaddr = virt_to_phys(memvirt) +
 			(((u64)uctxt->sc->hw_free -
 			  (u64)dd->cr_base[uctxt->numa_id].va) & PAGE_MASK);
 		memlen = PAGE_SIZE;
@@ -508,8 +509,8 @@ static int hfi1_file_mmap(struct file *fp, struct vm_area_struct *vma)
 		mapio = 1;
 		break;
 	case RCV_HDRQ:
-		memaddr = uctxt->rcvhdrq_phys;
 		memlen = uctxt->rcvhdrq_size;
+		memvirt = uctxt->rcvhdrq;
 		break;
 	case RCV_EGRBUF: {
 		unsigned long addr;
@@ -533,14 +534,21 @@ static int hfi1_file_mmap(struct file *fp, struct vm_area_struct *vma)
 		vma->vm_flags &= ~VM_MAYWRITE;
 		addr = vma->vm_start;
 		for (i = 0 ; i < uctxt->egrbufs.numbufs; i++) {
+			memlen = uctxt->egrbufs.buffers[i].len;
+			memvirt = uctxt->egrbufs.buffers[i].addr;
 			ret = remap_pfn_range(
 				vma, addr,
-				uctxt->egrbufs.buffers[i].phys >> PAGE_SHIFT,
-				uctxt->egrbufs.buffers[i].len,
+				/*
+				 * virt_to_pfn() does the same, but
+				 * it's not available on x86_64
+				 * when CONFIG_MMU is enabled.
+				 */
+				PFN_DOWN(__pa(memvirt)),
+				memlen,
 				vma->vm_page_prot);
 			if (ret < 0)
 				goto done;
-			addr += uctxt->egrbufs.buffers[i].len;
+			addr += memlen;
 		}
 		ret = 0;
 		goto done;
@@ -596,8 +604,8 @@ static int hfi1_file_mmap(struct file *fp, struct vm_area_struct *vma)
 			ret = -EPERM;
 			goto done;
 		}
-		memaddr = uctxt->rcvhdrqtailaddr_phys;
 		memlen = PAGE_SIZE;
+		memvirt = (void *)uctxt->rcvhdrtail_kvaddr;
 		flags &= ~VM_MAYWRITE;
 		break;
 	case SUBCTXT_UREGS:
@@ -650,16 +658,24 @@ static int hfi1_file_mmap(struct file *fp, struct vm_area_struct *vma)
 		  "%u:%u type:%u io/vf:%d/%d, addr:0x%llx, len:%lu(%lu), flags:0x%lx\n",
 		    ctxt, subctxt, type, mapio, vmf, memaddr, memlen,
 		    vma->vm_end - vma->vm_start, vma->vm_flags);
-	pfn = (unsigned long)(memaddr >> PAGE_SHIFT);
 	if (vmf) {
-		vma->vm_pgoff = pfn;
+		vma->vm_pgoff = PFN_DOWN(memaddr);
 		vma->vm_ops = &vm_ops;
 		ret = 0;
 	} else if (mapio) {
-		ret = io_remap_pfn_range(vma, vma->vm_start, pfn, memlen,
+		ret = io_remap_pfn_range(vma, vma->vm_start,
+					 PFN_DOWN(memaddr),
+					 memlen,
 					 vma->vm_page_prot);
+	} else if (memvirt) {
+		ret = remap_pfn_range(vma, vma->vm_start,
+				      PFN_DOWN(__pa(memvirt)),
+				      memlen,
+				      vma->vm_page_prot);
 	} else {
-		ret = remap_pfn_range(vma, vma->vm_start, pfn, memlen,
+		ret = remap_pfn_range(vma, vma->vm_start,
+				      PFN_DOWN(memaddr),
+				      memlen,
 				      vma->vm_page_prot);
 	}
 done:
@@ -961,14 +977,16 @@ static int allocate_ctxt(struct file *fp, struct hfi1_devdata *dd,
 	 */
 	uctxt->sc = sc_alloc(dd, SC_USER, uctxt->rcvhdrqentsize,
 			     uctxt->dd->node);
-	if (!uctxt->sc)
-		return -ENOMEM;
-
+	if (!uctxt->sc) {
+		ret = -ENOMEM;
+		goto ctxdata_free;
+	}
 	hfi1_cdbg(PROC, "allocated send context %u(%u)\n", uctxt->sc->sw_index,
 		  uctxt->sc->hw_context);
 	ret = sc_enable(uctxt->sc);
 	if (ret)
-		return ret;
+		goto ctxdata_free;
+
 	/*
 	 * Setup shared context resources if the user-level has requested
 	 * shared contexts and this is the 'master' process.
@@ -982,7 +1000,7 @@ static int allocate_ctxt(struct file *fp, struct hfi1_devdata *dd,
 		 * send context because it will be done during file close
 		 */
 		if (ret)
-			return ret;
+			goto ctxdata_free;
 	}
 	uctxt->userversion = uinfo->userversion;
 	uctxt->flags = hfi1_cap_mask; /* save current flag state */
@@ -1002,6 +1020,11 @@ static int allocate_ctxt(struct file *fp, struct hfi1_devdata *dd,
 	fd->uctxt = uctxt;
 
 	return 0;
+
+ctxdata_free:
+	dd->rcd[ctxt] = NULL;
+	hfi1_free_ctxtdata(dd, uctxt);
+	return ret;
 }
 
 static int init_subctxts(struct hfi1_ctxtdata *uctxt,
@@ -1260,7 +1283,7 @@ static int get_base_info(struct file *fp, void __user *ubase, __u32 len)
 					       uctxt->rcvhdrq);
 	binfo.rcvegr_bufbase = HFI1_MMAP_TOKEN(RCV_EGRBUF, uctxt->ctxt,
 					       fd->subctxt,
-					       uctxt->egrbufs.rcvtids[0].phys);
+					       uctxt->egrbufs.rcvtids[0].dma);
 	binfo.sdma_comp_bufbase = HFI1_MMAP_TOKEN(SDMA_COMP, uctxt->ctxt,
 						 fd->subctxt, 0);
 	/*
diff --git a/drivers/infiniband/hw/hfi1/hfi.h b/drivers/infiniband/hw/hfi1/hfi.h
index 325ec211370f..7eef11b316ff 100644
--- a/drivers/infiniband/hw/hfi1/hfi.h
+++ b/drivers/infiniband/hw/hfi1/hfi.h
@@ -64,6 +64,8 @@
 #include <linux/kthread.h>
 #include <linux/i2c.h>
 #include <linux/i2c-algo-bit.h>
+#include <rdma/ib_hdrs.h>
+#include <linux/rhashtable.h>
 #include <rdma/rdma_vt.h>
 
 #include "chip_registers.h"
@@ -171,12 +173,12 @@ struct ctxt_eager_bufs {
 	u32 threshold;           /* head update threshold */
 	struct eager_buffer {
 		void *addr;
-		dma_addr_t phys;
+		dma_addr_t dma;
 		ssize_t len;
 	} *buffers;
 	struct {
 		void *addr;
-		dma_addr_t phys;
+		dma_addr_t dma;
 	} *rcvtids;
 };
 
@@ -207,8 +209,8 @@ struct hfi1_ctxtdata {
 	/* size of each of the rcvhdrq entries */
 	u16 rcvhdrqentsize;
 	/* mmap of hdrq, must fit in 44 bits */
-	dma_addr_t rcvhdrq_phys;
-	dma_addr_t rcvhdrqtailaddr_phys;
+	dma_addr_t rcvhdrq_dma;
+	dma_addr_t rcvhdrqtailaddr_dma;
 	struct ctxt_eager_bufs egrbufs;
 	/* this receive context's assigned PIO ACK send context */
 	struct send_context *sc;
@@ -350,7 +352,7 @@ struct hfi1_packet {
 	struct hfi1_ctxtdata *rcd;
 	__le32 *rhf_addr;
 	struct rvt_qp *qp;
-	struct hfi1_other_headers *ohdr;
+	struct ib_other_headers *ohdr;
 	u64 rhf;
 	u32 maxcnt;
 	u32 rhqoff;
@@ -529,6 +531,7 @@ struct hfi1_msix_entry {
 	void *arg;
 	char name[MAX_NAME_SIZE];
 	cpumask_t mask;
+	struct irq_affinity_notify notify;
 };
 
 /* per-SL CCA information */
@@ -1060,8 +1063,6 @@ struct hfi1_devdata {
 	u8 psxmitwait_supported;
 	/* cycle length of PS* counters in HW (in picoseconds) */
 	u16 psxmitwait_check_rate;
-	/* high volume overflow errors deferred to tasklet */
-	struct tasklet_struct error_tasklet;
 
 	/* MSI-X information */
 	struct hfi1_msix_entry *msix_entries;
@@ -1164,7 +1165,7 @@ struct hfi1_devdata {
 
 	/* receive context tail dummy address */
 	__le64 *rcvhdrtail_dummy_kvaddr;
-	dma_addr_t rcvhdrtail_dummy_physaddr;
+	dma_addr_t rcvhdrtail_dummy_dma;
 
 	bool eprom_available;	/* true if EPROM is available for this device */
 	bool aspm_supported;	/* Does HW support ASPM */
@@ -1175,6 +1176,7 @@ struct hfi1_devdata {
 	atomic_t aspm_disabled_cnt;
 
 	struct hfi1_affinity *affinity;
+	struct rhashtable sdma_rht;
 	struct kobject kobj;
 };
 
@@ -1268,7 +1270,7 @@ static inline u32 driver_lstate(struct hfi1_pportdata *ppd)
 void receive_interrupt_work(struct work_struct *work);
 
 /* extract service channel from header and rhf */
-static inline int hdr2sc(struct hfi1_message_header *hdr, u64 rhf)
+static inline int hdr2sc(struct ib_header *hdr, u64 rhf)
 {
 	return ((be16_to_cpu(hdr->lrh[0]) >> 12) & 0xf) |
 	       ((!!(rhf_dc_info(rhf))) << 4);
@@ -1603,7 +1605,7 @@ void hfi1_process_ecn_slowpath(struct rvt_qp *qp, struct hfi1_packet *pkt,
 static inline bool process_ecn(struct rvt_qp *qp, struct hfi1_packet *pkt,
 			       bool do_cnp)
 {
-	struct hfi1_other_headers *ohdr = pkt->ohdr;
+	struct ib_other_headers *ohdr = pkt->ohdr;
 	u32 bth1;
 
 	bth1 = be32_to_cpu(ohdr->bth[1]);
diff --git a/drivers/infiniband/hw/hfi1/init.c b/drivers/infiniband/hw/hfi1/init.c
index 384b43d2fd49..60db61536fed 100644
--- a/drivers/infiniband/hw/hfi1/init.c
+++ b/drivers/infiniband/hw/hfi1/init.c
@@ -336,6 +336,7 @@ struct hfi1_ctxtdata *hfi1_create_ctxtdata(struct hfi1_pportdata *ppd, u32 ctxt,
 	}
 	return rcd;
 bail:
+	dd->rcd[ctxt] = NULL;
 	kfree(rcd->egrbufs.rcvtids);
 	kfree(rcd->egrbufs.buffers);
 	kfree(rcd);
@@ -709,7 +710,7 @@ int hfi1_init(struct hfi1_devdata *dd, int reinit)
 	/* allocate dummy tail memory for all receive contexts */
 	dd->rcvhdrtail_dummy_kvaddr = dma_zalloc_coherent(
 		&dd->pcidev->dev, sizeof(u64),
-		&dd->rcvhdrtail_dummy_physaddr,
+		&dd->rcvhdrtail_dummy_dma,
 		GFP_KERNEL);
 
 	if (!dd->rcvhdrtail_dummy_kvaddr) {
@@ -942,12 +943,12 @@ void hfi1_free_ctxtdata(struct hfi1_devdata *dd, struct hfi1_ctxtdata *rcd)
 
 	if (rcd->rcvhdrq) {
 		dma_free_coherent(&dd->pcidev->dev, rcd->rcvhdrq_size,
-				  rcd->rcvhdrq, rcd->rcvhdrq_phys);
+				  rcd->rcvhdrq, rcd->rcvhdrq_dma);
 		rcd->rcvhdrq = NULL;
 		if (rcd->rcvhdrtail_kvaddr) {
 			dma_free_coherent(&dd->pcidev->dev, PAGE_SIZE,
 					  (void *)rcd->rcvhdrtail_kvaddr,
-					  rcd->rcvhdrqtailaddr_phys);
+					  rcd->rcvhdrqtailaddr_dma);
 			rcd->rcvhdrtail_kvaddr = NULL;
 		}
 	}
@@ -956,11 +957,11 @@ void hfi1_free_ctxtdata(struct hfi1_devdata *dd, struct hfi1_ctxtdata *rcd)
 	kfree(rcd->egrbufs.rcvtids);
 
 	for (e = 0; e < rcd->egrbufs.alloced; e++) {
-		if (rcd->egrbufs.buffers[e].phys)
+		if (rcd->egrbufs.buffers[e].dma)
 			dma_free_coherent(&dd->pcidev->dev,
 					  rcd->egrbufs.buffers[e].len,
 					  rcd->egrbufs.buffers[e].addr,
-					  rcd->egrbufs.buffers[e].phys);
+					  rcd->egrbufs.buffers[e].dma);
 	}
 	kfree(rcd->egrbufs.buffers);
 
@@ -1354,7 +1355,7 @@ static void cleanup_device_data(struct hfi1_devdata *dd)
 	if (dd->rcvhdrtail_dummy_kvaddr) {
 		dma_free_coherent(&dd->pcidev->dev, sizeof(u64),
 				  (void *)dd->rcvhdrtail_dummy_kvaddr,
-				  dd->rcvhdrtail_dummy_physaddr);
+				  dd->rcvhdrtail_dummy_dma);
 		dd->rcvhdrtail_dummy_kvaddr = NULL;
 	}
 
@@ -1577,7 +1578,7 @@ int hfi1_create_rcvhdrq(struct hfi1_devdata *dd, struct hfi1_ctxtdata *rcd)
 	u64 reg;
 
 	if (!rcd->rcvhdrq) {
-		dma_addr_t phys_hdrqtail;
+		dma_addr_t dma_hdrqtail;
 		gfp_t gfp_flags;
 
 		/*
@@ -1590,7 +1591,7 @@ int hfi1_create_rcvhdrq(struct hfi1_devdata *dd, struct hfi1_ctxtdata *rcd)
 		gfp_flags = (rcd->ctxt >= dd->first_user_ctxt) ?
 			GFP_USER : GFP_KERNEL;
 		rcd->rcvhdrq = dma_zalloc_coherent(
-			&dd->pcidev->dev, amt, &rcd->rcvhdrq_phys,
+			&dd->pcidev->dev, amt, &rcd->rcvhdrq_dma,
 			gfp_flags | __GFP_COMP);
 
 		if (!rcd->rcvhdrq) {
@@ -1602,11 +1603,11 @@ int hfi1_create_rcvhdrq(struct hfi1_devdata *dd, struct hfi1_ctxtdata *rcd)
 
 		if (HFI1_CAP_KGET_MASK(rcd->flags, DMA_RTAIL)) {
 			rcd->rcvhdrtail_kvaddr = dma_zalloc_coherent(
-				&dd->pcidev->dev, PAGE_SIZE, &phys_hdrqtail,
+				&dd->pcidev->dev, PAGE_SIZE, &dma_hdrqtail,
 				gfp_flags);
 			if (!rcd->rcvhdrtail_kvaddr)
 				goto bail_free;
-			rcd->rcvhdrqtailaddr_phys = phys_hdrqtail;
+			rcd->rcvhdrqtailaddr_dma = dma_hdrqtail;
 		}
 
 		rcd->rcvhdrq_size = amt;
@@ -1634,7 +1635,7 @@ int hfi1_create_rcvhdrq(struct hfi1_devdata *dd, struct hfi1_ctxtdata *rcd)
 	 * before enabling any receive context
 	 */
 	write_kctxt_csr(dd, rcd->ctxt, RCV_HDR_TAIL_ADDR,
-			dd->rcvhdrtail_dummy_physaddr);
+			dd->rcvhdrtail_dummy_dma);
 
 	return 0;
 
@@ -1645,7 +1646,7 @@ bail_free:
 	vfree(rcd->user_event_mask);
 	rcd->user_event_mask = NULL;
 	dma_free_coherent(&dd->pcidev->dev, amt, rcd->rcvhdrq,
-			  rcd->rcvhdrq_phys);
+			  rcd->rcvhdrq_dma);
 	rcd->rcvhdrq = NULL;
 bail:
 	return -ENOMEM;
@@ -1706,15 +1707,15 @@ int hfi1_setup_eagerbufs(struct hfi1_ctxtdata *rcd)
 		rcd->egrbufs.buffers[idx].addr =
 			dma_zalloc_coherent(&dd->pcidev->dev,
 					    rcd->egrbufs.rcvtid_size,
-					    &rcd->egrbufs.buffers[idx].phys,
+					    &rcd->egrbufs.buffers[idx].dma,
 					    gfp_flags);
 		if (rcd->egrbufs.buffers[idx].addr) {
 			rcd->egrbufs.buffers[idx].len =
 				rcd->egrbufs.rcvtid_size;
 			rcd->egrbufs.rcvtids[rcd->egrbufs.alloced].addr =
 				rcd->egrbufs.buffers[idx].addr;
-			rcd->egrbufs.rcvtids[rcd->egrbufs.alloced].phys =
-				rcd->egrbufs.buffers[idx].phys;
+			rcd->egrbufs.rcvtids[rcd->egrbufs.alloced].dma =
+				rcd->egrbufs.buffers[idx].dma;
 			rcd->egrbufs.alloced++;
 			alloced_bytes += rcd->egrbufs.rcvtid_size;
 			idx++;
@@ -1755,14 +1756,14 @@ int hfi1_setup_eagerbufs(struct hfi1_ctxtdata *rcd)
 			for (i = 0, j = 0, offset = 0; j < idx; i++) {
 				if (i >= rcd->egrbufs.count)
 					break;
-				rcd->egrbufs.rcvtids[i].phys =
-					rcd->egrbufs.buffers[j].phys + offset;
+				rcd->egrbufs.rcvtids[i].dma =
+					rcd->egrbufs.buffers[j].dma + offset;
 				rcd->egrbufs.rcvtids[i].addr =
 					rcd->egrbufs.buffers[j].addr + offset;
 				rcd->egrbufs.alloced++;
-				if ((rcd->egrbufs.buffers[j].phys + offset +
+				if ((rcd->egrbufs.buffers[j].dma + offset +
 				     new_size) ==
-				    (rcd->egrbufs.buffers[j].phys +
+				    (rcd->egrbufs.buffers[j].dma +
 				     rcd->egrbufs.buffers[j].len)) {
 					j++;
 					offset = 0;
@@ -1814,7 +1815,7 @@ int hfi1_setup_eagerbufs(struct hfi1_ctxtdata *rcd)
 
 	for (idx = 0; idx < rcd->egrbufs.alloced; idx++) {
 		hfi1_put_tid(dd, rcd->eager_base + idx, PT_EAGER,
-			     rcd->egrbufs.rcvtids[idx].phys, order);
+			     rcd->egrbufs.rcvtids[idx].dma, order);
 		cond_resched();
 	}
 	goto bail;
@@ -1826,9 +1827,9 @@ bail_rcvegrbuf_phys:
 		dma_free_coherent(&dd->pcidev->dev,
 				  rcd->egrbufs.buffers[idx].len,
 				  rcd->egrbufs.buffers[idx].addr,
-				  rcd->egrbufs.buffers[idx].phys);
+				  rcd->egrbufs.buffers[idx].dma);
 		rcd->egrbufs.buffers[idx].addr = NULL;
-		rcd->egrbufs.buffers[idx].phys = 0;
+		rcd->egrbufs.buffers[idx].dma = 0;
 		rcd->egrbufs.buffers[idx].len = 0;
 	}
 bail:
diff --git a/drivers/infiniband/hw/hfi1/mad.c b/drivers/infiniband/hw/hfi1/mad.c
index 7ffc14f21523..9487c9bb8920 100644
--- a/drivers/infiniband/hw/hfi1/mad.c
+++ b/drivers/infiniband/hw/hfi1/mad.c
@@ -1013,7 +1013,6 @@ static int set_port_states(struct hfi1_pportdata *ppd, struct opa_smp *smp,
 			 * offline.
 			 */
 			set_link_state(ppd, HLS_DN_OFFLINE);
-			tune_serdes(ppd);
 			start_link(ppd);
 		} else {
 			set_link_state(ppd, link_state);
@@ -1407,12 +1406,6 @@ static int set_pkeys(struct hfi1_devdata *dd, u8 port, u16 *pkeys)
 		if (key == okey)
 			continue;
 		/*
-		 * Don't update pkeys[2], if an HFI port without MgmtAllowed
-		 * by neighbor is a switch.
-		 */
-		if (i == 2 && !ppd->mgmt_allowed && ppd->neighbor_type == 1)
-			continue;
-		/*
 		 * The SM gives us the complete PKey table. We have
 		 * to ensure that we put the PKeys in the matching
 		 * slots.
diff --git a/drivers/infiniband/hw/hfi1/pio.c b/drivers/infiniband/hw/hfi1/pio.c
index ac1bf4a73571..50a3a36d9363 100644
--- a/drivers/infiniband/hw/hfi1/pio.c
+++ b/drivers/infiniband/hw/hfi1/pio.c
@@ -551,11 +551,11 @@ static inline u32 group_size(u32 group)
 }
 
 /*
- * Obtain the credit return addresses, kernel virtual and physical, for the
+ * Obtain the credit return addresses, kernel virtual and bus, for the
  * given sc.
  *
  * To understand this routine:
- * o va and pa are arrays of struct credit_return.  One for each physical
+ * o va and dma are arrays of struct credit_return.  One for each physical
  *   send context, per NUMA.
  * o Each send context always looks in its relative location in a struct
  *   credit_return for its credit return.
@@ -563,14 +563,14 @@ static inline u32 group_size(u32 group)
  *   with the same value.  Use the address of the first send context in the
  *   group.
  */
-static void cr_group_addresses(struct send_context *sc, dma_addr_t *pa)
+static void cr_group_addresses(struct send_context *sc, dma_addr_t *dma)
 {
 	u32 gc = group_context(sc->hw_context, sc->group);
 	u32 index = sc->hw_context & 0x7;
 
 	sc->hw_free = &sc->dd->cr_base[sc->node].va[gc].cr[index];
-	*pa = (unsigned long)
-	       &((struct credit_return *)sc->dd->cr_base[sc->node].pa)[gc];
+	*dma = (unsigned long)
+	       &((struct credit_return *)sc->dd->cr_base[sc->node].dma)[gc];
 }
 
 /*
@@ -710,7 +710,7 @@ struct send_context *sc_alloc(struct hfi1_devdata *dd, int type,
 {
 	struct send_context_info *sci;
 	struct send_context *sc = NULL;
-	dma_addr_t pa;
+	dma_addr_t dma;
 	unsigned long flags;
 	u64 reg;
 	u32 thresh;
@@ -763,7 +763,7 @@ struct send_context *sc_alloc(struct hfi1_devdata *dd, int type,
 
 	sc->sw_index = sw_index;
 	sc->hw_context = hw_context;
-	cr_group_addresses(sc, &pa);
+	cr_group_addresses(sc, &dma);
 	sc->credits = sci->credits;
 
 /* PIO Send Memory Address details */
@@ -805,7 +805,7 @@ struct send_context *sc_alloc(struct hfi1_devdata *dd, int type,
 			((u64)opval << SC(CHECK_OPCODE_VALUE_SHIFT)));
 
 	/* set up credit return */
-	reg = pa & SC(CREDIT_RETURN_ADDR_ADDRESS_SMASK);
+	reg = dma & SC(CREDIT_RETURN_ADDR_ADDRESS_SMASK);
 	write_kctxt_csr(dd, hw_context, SC(CREDIT_RETURN_ADDR), reg);
 
 	/*
@@ -2064,7 +2064,7 @@ int init_credit_return(struct hfi1_devdata *dd)
 		dd->cr_base[i].va = dma_zalloc_coherent(
 					&dd->pcidev->dev,
 					bytes,
-					&dd->cr_base[i].pa,
+					&dd->cr_base[i].dma,
 					GFP_KERNEL);
 		if (!dd->cr_base[i].va) {
 			set_dev_node(&dd->pcidev->dev, dd->node);
@@ -2097,7 +2097,7 @@ void free_credit_return(struct hfi1_devdata *dd)
 					  TXE_NUM_CONTEXTS *
 					  sizeof(struct credit_return),
 					  dd->cr_base[i].va,
-					  dd->cr_base[i].pa);
+					  dd->cr_base[i].dma);
 		}
 	}
 	kfree(dd->cr_base);
diff --git a/drivers/infiniband/hw/hfi1/pio.h b/drivers/infiniband/hw/hfi1/pio.h
index 464cbd27b975..e709eaf743b5 100644
--- a/drivers/infiniband/hw/hfi1/pio.h
+++ b/drivers/infiniband/hw/hfi1/pio.h
@@ -154,7 +154,7 @@ struct credit_return {
 /* NUMA indexed credit return array */
 struct credit_return_base {
 	struct credit_return *va;
-	dma_addr_t pa;
+	dma_addr_t dma;
 };
 
 /* send context configuration sizes (one per type) */
diff --git a/drivers/infiniband/hw/hfi1/pio_copy.c b/drivers/infiniband/hw/hfi1/pio_copy.c
index 3a1ef3056282..aa7773643107 100644
--- a/drivers/infiniband/hw/hfi1/pio_copy.c
+++ b/drivers/infiniband/hw/hfi1/pio_copy.c
@@ -165,9 +165,6 @@ void pio_copy(struct hfi1_devdata *dd, struct pio_buf *pbuf, u64 pbc,
 	preempt_enable();
 }
 
-/* USE_SHIFTS is faster in user-space tests on a Xeon X5570 @ 2.93GHz */
-#define USE_SHIFTS 1
-#ifdef USE_SHIFTS
 /*
  * Handle carry bytes using shifts and masks.
  *
@@ -187,150 +184,6 @@ void pio_copy(struct hfi1_devdata *dd, struct pio_buf *pbuf, u64 pbc,
 #define mshift(x) (8 * (x))
 
 /*
- * Read nbytes bytes from "from" and return them in the LSB bytes
- * of pbuf->carry.  Other bytes are zeroed.  Any previous value
- * pbuf->carry is lost.
- *
- * NOTES:
- * o do not read from from if nbytes is zero
- * o from may _not_ be u64 aligned
- * o nbytes must not span a QW boundary
- */
-static inline void read_low_bytes(struct pio_buf *pbuf, const void *from,
-				  unsigned int nbytes)
-{
-	unsigned long off;
-
-	if (nbytes == 0) {
-		pbuf->carry.val64 = 0;
-	} else {
-		/* align our pointer */
-		off = (unsigned long)from & 0x7;
-		from = (void *)((unsigned long)from & ~0x7l);
-		pbuf->carry.val64 = ((*(u64 *)from)
-				<< zshift(nbytes + off))/* zero upper bytes */
-				>> zshift(nbytes);	/* place at bottom */
-	}
-	pbuf->carry_bytes = nbytes;
-}
-
-/*
- * Read nbytes bytes from "from" and put them at the next significant bytes
- * of pbuf->carry.  Unused bytes are zeroed.  It is expected that the extra
- * read does not overfill carry.
- *
- * NOTES:
- * o from may _not_ be u64 aligned
- * o nbytes may span a QW boundary
- */
-static inline void read_extra_bytes(struct pio_buf *pbuf,
-				    const void *from, unsigned int nbytes)
-{
-	unsigned long off = (unsigned long)from & 0x7;
-	unsigned int room, xbytes;
-
-	/* align our pointer */
-	from = (void *)((unsigned long)from & ~0x7l);
-
-	/* check count first - don't read anything if count is zero */
-	while (nbytes) {
-		/* find the number of bytes in this u64 */
-		room = 8 - off;	/* this u64 has room for this many bytes */
-		xbytes = min(room, nbytes);
-
-		/*
-		 * shift down to zero lower bytes, shift up to zero upper
-		 * bytes, shift back down to move into place
-		 */
-		pbuf->carry.val64 |= (((*(u64 *)from)
-					>> mshift(off))
-					<< zshift(xbytes))
-					>> zshift(xbytes + pbuf->carry_bytes);
-		off = 0;
-		pbuf->carry_bytes += xbytes;
-		nbytes -= xbytes;
-		from += sizeof(u64);
-	}
-}
-
-/*
- * Zero extra bytes from the end of pbuf->carry.
- *
- * NOTES:
- * o zbytes <= old_bytes
- */
-static inline void zero_extra_bytes(struct pio_buf *pbuf, unsigned int zbytes)
-{
-	unsigned int remaining;
-
-	if (zbytes == 0)	/* nothing to do */
-		return;
-
-	remaining = pbuf->carry_bytes - zbytes;	/* remaining bytes */
-
-	/* NOTE: zshift only guaranteed to work if remaining != 0 */
-	if (remaining)
-		pbuf->carry.val64 = (pbuf->carry.val64 << zshift(remaining))
-					>> zshift(remaining);
-	else
-		pbuf->carry.val64 = 0;
-	pbuf->carry_bytes = remaining;
-}
-
-/*
- * Write a quad word using parts of pbuf->carry and the next 8 bytes of src.
- * Put the unused part of the next 8 bytes of src into the LSB bytes of
- * pbuf->carry with the upper bytes zeroed..
- *
- * NOTES:
- * o result must keep unused bytes zeroed
- * o src must be u64 aligned
- */
-static inline void merge_write8(
-	struct pio_buf *pbuf,
-	void __iomem *dest,
-	const void *src)
-{
-	u64 new, temp;
-
-	new = *(u64 *)src;
-	temp = pbuf->carry.val64 | (new << mshift(pbuf->carry_bytes));
-	writeq(temp, dest);
-	pbuf->carry.val64 = new >> zshift(pbuf->carry_bytes);
-}
-
-/*
- * Write a quad word using all bytes of carry.
- */
-static inline void carry8_write8(union mix carry, void __iomem *dest)
-{
-	writeq(carry.val64, dest);
-}
-
-/*
- * Write a quad word using all the valid bytes of carry.  If carry
- * has zero valid bytes, nothing is written.
- * Returns 0 on nothing written, non-zero on quad word written.
- */
-static inline int carry_write8(struct pio_buf *pbuf, void __iomem *dest)
-{
-	if (pbuf->carry_bytes) {
-		/* unused bytes are always kept zeroed, so just write */
-		writeq(pbuf->carry.val64, dest);
-		return 1;
-	}
-
-	return 0;
-}
-
-#else /* USE_SHIFTS */
-/*
- * Handle carry bytes using byte copies.
- *
- * NOTE: the value the unused portion of carry is left uninitialized.
- */
-
-/*
  * Jump copy - no-loop copy for < 8 bytes.
  */
 static inline void jcopy(u8 *dest, const u8 *src, u32 n)
@@ -338,18 +191,25 @@ static inline void jcopy(u8 *dest, const u8 *src, u32 n)
 	switch (n) {
 	case 7:
 		*dest++ = *src++;
+		/* fall through */
 	case 6:
 		*dest++ = *src++;
+		/* fall through */
 	case 5:
 		*dest++ = *src++;
+		/* fall through */
 	case 4:
 		*dest++ = *src++;
+		/* fall through */
 	case 3:
 		*dest++ = *src++;
+		/* fall through */
 	case 2:
 		*dest++ = *src++;
+		/* fall through */
 	case 1:
 		*dest++ = *src++;
+		/* fall through */
 	}
 }
 
@@ -365,6 +225,7 @@ static inline void jcopy(u8 *dest, const u8 *src, u32 n)
 static inline void read_low_bytes(struct pio_buf *pbuf, const void *from,
 				  unsigned int nbytes)
 {
+	pbuf->carry.val64 = 0;
 	jcopy(&pbuf->carry.val8[0], from, nbytes);
 	pbuf->carry_bytes = nbytes;
 }
@@ -385,40 +246,31 @@ static inline void read_extra_bytes(struct pio_buf *pbuf,
 }
 
 /*
- * Zero extra bytes from the end of pbuf->carry.
- *
- * We do not care about the value of unused bytes in carry, so just
- * reduce the byte count.
+ * Write a quad word using parts of pbuf->carry and the next 8 bytes of src.
+ * Put the unused part of the next 8 bytes of src into the LSB bytes of
+ * pbuf->carry with the upper bytes zeroed..
  *
  * NOTES:
- * o zbytes <= old_bytes
- */
-static inline void zero_extra_bytes(struct pio_buf *pbuf, unsigned int zbytes)
-{
-	pbuf->carry_bytes -= zbytes;
-}
-
-/*
- * Write a quad word using parts of pbuf->carry and the next 8 bytes of src.
- * Put the unused part of the next 8 bytes of src into the low bytes of
- * pbuf->carry.
+ * o result must keep unused bytes zeroed
+ * o src must be u64 aligned
  */
 static inline void merge_write8(
 	struct pio_buf *pbuf,
-	void *dest,
+	void __iomem *dest,
 	const void *src)
 {
-	u32 remainder = 8 - pbuf->carry_bytes;
+	u64 new, temp;
 
-	jcopy(&pbuf->carry.val8[pbuf->carry_bytes], src, remainder);
-	writeq(pbuf->carry.val64, dest);
-	jcopy(&pbuf->carry.val8[0], src + remainder, pbuf->carry_bytes);
+	new = *(u64 *)src;
+	temp = pbuf->carry.val64 | (new << mshift(pbuf->carry_bytes));
+	writeq(temp, dest);
+	pbuf->carry.val64 = new >> zshift(pbuf->carry_bytes);
 }
 
 /*
  * Write a quad word using all bytes of carry.
  */
-static inline void carry8_write8(union mix carry, void *dest)
+static inline void carry8_write8(union mix carry, void __iomem *dest)
 {
 	writeq(carry.val64, dest);
 }
@@ -428,20 +280,16 @@ static inline void carry8_write8(union mix carry, void *dest)
  * has zero valid bytes, nothing is written.
  * Returns 0 on nothing written, non-zero on quad word written.
  */
-static inline int carry_write8(struct pio_buf *pbuf, void *dest)
+static inline int carry_write8(struct pio_buf *pbuf, void __iomem *dest)
 {
 	if (pbuf->carry_bytes) {
-		u64 zero = 0;
-
-		jcopy(&pbuf->carry.val8[pbuf->carry_bytes], (u8 *)&zero,
-		      8 - pbuf->carry_bytes);
+		/* unused bytes are always kept zeroed, so just write */
 		writeq(pbuf->carry.val64, dest);
 		return 1;
 	}
 
 	return 0;
 }
-#endif /* USE_SHIFTS */
 
 /*
  * Segmented PIO Copy - start
@@ -550,8 +398,8 @@ static void mid_copy_mix(struct pio_buf *pbuf, const void *from, size_t nbytes)
 {
 	void __iomem *dest = pbuf->start + (pbuf->qw_written * sizeof(u64));
 	void __iomem *dend;			/* 8-byte data end */
-	unsigned long qw_to_write = (pbuf->carry_bytes + nbytes) >> 3;
-	unsigned long bytes_left = (pbuf->carry_bytes + nbytes) & 0x7;
+	unsigned long qw_to_write = nbytes >> 3;
+	unsigned long bytes_left = nbytes & 0x7;
 
 	/* calculate 8-byte data end */
 	dend = dest + (qw_to_write * sizeof(u64));
@@ -621,16 +469,46 @@ static void mid_copy_mix(struct pio_buf *pbuf, const void *from, size_t nbytes)
 		dest += sizeof(u64);
 	}
 
-	/* adjust carry */
-	if (pbuf->carry_bytes < bytes_left) {
-		/* need to read more */
-		read_extra_bytes(pbuf, from, bytes_left - pbuf->carry_bytes);
+	pbuf->qw_written += qw_to_write;
+
+	/* handle carry and left-over bytes */
+	if (pbuf->carry_bytes + bytes_left >= 8) {
+		unsigned long nread;
+
+		/* there is enough to fill another qw - fill carry */
+		nread = 8 - pbuf->carry_bytes;
+		read_extra_bytes(pbuf, from, nread);
+
+		/*
+		 * One more write - but need to make sure dest is correct.
+		 * Check for wrap and the possibility the write
+		 * should be in SOP space.
+		 *
+		 * The two checks immediately below cannot both be true, hence
+		 * the else. If we have wrapped, we cannot still be within the
+		 * first block. Conversely, if we are still in the first block,
+		 * we cannot have wrapped. We do the wrap check first as that
+		 * is more likely.
+		 */
+		/* adjust if we have wrapped */
+		if (dest >= pbuf->end)
+			dest -= pbuf->size;
+		/* jump to the SOP range if within the first block */
+		else if (pbuf->qw_written < PIO_BLOCK_QWS)
+			dest += SOP_DISTANCE;
+
+		/* flush out full carry */
+		carry8_write8(pbuf->carry, dest);
+		pbuf->qw_written++;
+
+		/* now adjust and read the rest of the bytes into carry */
+		bytes_left -= nread;
+		from += nread; /* from is now not aligned */
+		read_low_bytes(pbuf, from, bytes_left);
 	} else {
-		/* remove invalid bytes */
-		zero_extra_bytes(pbuf, pbuf->carry_bytes - bytes_left);
+		/* not enough to fill another qw, append the rest to carry */
+		read_extra_bytes(pbuf, from, bytes_left);
 	}
-
-	pbuf->qw_written += qw_to_write;
 }
 
 /*
diff --git a/drivers/infiniband/hw/hfi1/platform.c b/drivers/infiniband/hw/hfi1/platform.c
index 965c8aef0c60..202433178864 100644
--- a/drivers/infiniband/hw/hfi1/platform.c
+++ b/drivers/infiniband/hw/hfi1/platform.c
@@ -47,29 +47,39 @@
 
 #include "hfi.h"
 #include "efivar.h"
+#include "eprom.h"
 
 void get_platform_config(struct hfi1_devdata *dd)
 {
 	int ret = 0;
 	unsigned long size = 0;
 	u8 *temp_platform_config = NULL;
+	u32 esize;
+
+	ret = eprom_read_platform_config(dd, (void **)&temp_platform_config,
+					 &esize);
+	if (!ret) {
+		/* success */
+		size = esize;
+		goto success;
+	}
+	/* fail, try EFI variable */
 
 	ret = read_hfi1_efi_var(dd, "configuration", &size,
 				(void **)&temp_platform_config);
-	if (ret) {
-		dd_dev_info(dd,
-			    "%s: Failed to get platform config from UEFI, falling back to request firmware\n",
-			    __func__);
-		/* fall back to request firmware */
-		platform_config_load = 1;
-		goto bail;
-	}
+	if (!ret)
+		goto success;
+
+	dd_dev_info(dd,
+		    "%s: Failed to get platform config from UEFI, falling back to request firmware\n",
+		    __func__);
+	/* fall back to request firmware */
+	platform_config_load = 1;
+	return;
 
+success:
 	dd->platform_config.data = temp_platform_config;
 	dd->platform_config.size = size;
-
-bail:
-	/* exit */;
 }
 
 void free_platform_config(struct hfi1_devdata *dd)
diff --git a/drivers/infiniband/hw/hfi1/qp.c b/drivers/infiniband/hw/hfi1/qp.c
index 4e4d8317c281..9fc75e7e8781 100644
--- a/drivers/infiniband/hw/hfi1/qp.c
+++ b/drivers/infiniband/hw/hfi1/qp.c
@@ -202,8 +202,7 @@ static void flush_iowait(struct rvt_qp *qp)
 	write_seqlock_irqsave(&dev->iowait_lock, flags);
 	if (!list_empty(&priv->s_iowait.list)) {
 		list_del_init(&priv->s_iowait.list);
-		if (atomic_dec_and_test(&qp->refcount))
-			wake_up(&qp->wait);
+		rvt_put_qp(qp);
 	}
 	write_sequnlock_irqrestore(&dev->iowait_lock, flags);
 }
@@ -450,13 +449,14 @@ static void qp_pio_drain(struct rvt_qp *qp)
  */
 void hfi1_schedule_send(struct rvt_qp *qp)
 {
+	lockdep_assert_held(&qp->s_lock);
 	if (hfi1_send_ok(qp))
 		_hfi1_schedule_send(qp);
 }
 
 /**
- * hfi1_get_credit - flush the send work queue of a QP
- * @qp: the qp who's send work queue to flush
+ * hfi1_get_credit - handle credit in aeth
+ * @qp: the qp
  * @aeth: the Acknowledge Extended Transport Header
  *
  * The QP s_lock should be held.
@@ -465,6 +465,7 @@ void hfi1_get_credit(struct rvt_qp *qp, u32 aeth)
 {
 	u32 credit = (aeth >> HFI1_AETH_CREDIT_SHIFT) & HFI1_AETH_CREDIT_MASK;
 
+	lockdep_assert_held(&qp->s_lock);
 	/*
 	 * If the credit is invalid, we can send
 	 * as many packets as we like.  Otherwise, we have to
@@ -503,8 +504,7 @@ void hfi1_qp_wakeup(struct rvt_qp *qp, u32 flag)
 	}
 	spin_unlock_irqrestore(&qp->s_lock, flags);
 	/* Notify hfi1_destroy_qp() if it is waiting. */
-	if (atomic_dec_and_test(&qp->refcount))
-		wake_up(&qp->wait);
+	rvt_put_qp(qp);
 }
 
 static int iowait_sleep(
@@ -544,7 +544,7 @@ static int iowait_sleep(
 			qp->s_flags |= RVT_S_WAIT_DMA_DESC;
 			list_add_tail(&priv->s_iowait.list, &sde->dmawait);
 			trace_hfi1_qpsleep(qp, RVT_S_WAIT_DMA_DESC);
-			atomic_inc(&qp->refcount);
+			rvt_get_qp(qp);
 		}
 		write_sequnlock(&dev->iowait_lock);
 		qp->s_flags &= ~RVT_S_BUSY;
@@ -808,6 +808,13 @@ void *qp_priv_alloc(struct rvt_dev_info *rdi, struct rvt_qp *qp,
 		kfree(priv);
 		return ERR_PTR(-ENOMEM);
 	}
+	iowait_init(
+		&priv->s_iowait,
+		1,
+		_hfi1_do_send,
+		iowait_sleep,
+		iowait_wakeup,
+		iowait_sdma_drained);
 	setup_timer(&priv->s_rnr_timer, hfi1_rc_rnr_retry, (unsigned long)qp);
 	qp->s_timer.function = hfi1_rc_timeout;
 	return priv;
@@ -848,6 +855,7 @@ unsigned free_all_qps(struct rvt_dev_info *rdi)
 
 void flush_qp_waiters(struct rvt_qp *qp)
 {
+	lockdep_assert_held(&qp->s_lock);
 	flush_iowait(qp);
 	hfi1_stop_rc_timers(qp);
 }
@@ -873,13 +881,6 @@ void notify_qp_reset(struct rvt_qp *qp)
 {
 	struct hfi1_qp_priv *priv = qp->priv;
 
-	iowait_init(
-		&priv->s_iowait,
-		1,
-		_hfi1_do_send,
-		iowait_sleep,
-		iowait_wakeup,
-		iowait_sdma_drained);
 	priv->r_adefered = 0;
 	clear_ahg(qp);
 }
@@ -963,8 +964,7 @@ void notify_error_qp(struct rvt_qp *qp)
 	if (!list_empty(&priv->s_iowait.list) && !(qp->s_flags & RVT_S_BUSY)) {
 		qp->s_flags &= ~RVT_S_ANY_WAIT_IO;
 		list_del_init(&priv->s_iowait.list);
-		if (atomic_dec_and_test(&qp->refcount))
-			wake_up(&qp->wait);
+		rvt_put_qp(qp);
 	}
 	write_sequnlock(&dev->iowait_lock);
 
diff --git a/drivers/infiniband/hw/hfi1/qsfp.c b/drivers/infiniband/hw/hfi1/qsfp.c
index 4e95ad810847..1869f639c3ae 100644
--- a/drivers/infiniband/hw/hfi1/qsfp.c
+++ b/drivers/infiniband/hw/hfi1/qsfp.c
@@ -161,7 +161,7 @@ static struct hfi1_i2c_bus *init_i2c_bus(struct hfi1_devdata *dd,
 	bus->algo.getsda = hfi1_getsda;
 	bus->algo.getscl = hfi1_getscl;
 	bus->algo.udelay = 5;
-	bus->algo.timeout = usecs_to_jiffies(50);
+	bus->algo.timeout = usecs_to_jiffies(100000);
 	bus->algo.data = bus;
 
 	bus->adapter.owner = THIS_MODULE;
diff --git a/drivers/infiniband/hw/hfi1/rc.c b/drivers/infiniband/hw/hfi1/rc.c
index 5da190e6011b..8bc5013f39a1 100644
--- a/drivers/infiniband/hw/hfi1/rc.c
+++ b/drivers/infiniband/hw/hfi1/rc.c
@@ -55,7 +55,7 @@
 #include "trace.h"
 
 /* cut down ridiculously long IB macro names */
-#define OP(x) IB_OPCODE_RC_##x
+#define OP(x) RC_OP(x)
 
 /**
  * hfi1_add_retry_timer - add/start a retry timer
@@ -68,6 +68,7 @@ static inline void hfi1_add_retry_timer(struct rvt_qp *qp)
 	struct ib_qp *ibqp = &qp->ibqp;
 	struct rvt_dev_info *rdi = ib_to_rvt(ibqp->device);
 
+	lockdep_assert_held(&qp->s_lock);
 	qp->s_flags |= RVT_S_TIMER;
 	/* 4.096 usec. * (1 << qp->timeout) */
 	qp->s_timer.expires = jiffies + qp->timeout_jiffies +
@@ -86,6 +87,7 @@ void hfi1_add_rnr_timer(struct rvt_qp *qp, u32 to)
 {
 	struct hfi1_qp_priv *priv = qp->priv;
 
+	lockdep_assert_held(&qp->s_lock);
 	qp->s_flags |= RVT_S_WAIT_RNR;
 	qp->s_timer.expires = jiffies + usecs_to_jiffies(to);
 	add_timer(&priv->s_rnr_timer);
@@ -103,6 +105,7 @@ static inline void hfi1_mod_retry_timer(struct rvt_qp *qp)
 	struct ib_qp *ibqp = &qp->ibqp;
 	struct rvt_dev_info *rdi = ib_to_rvt(ibqp->device);
 
+	lockdep_assert_held(&qp->s_lock);
 	qp->s_flags |= RVT_S_TIMER;
 	/* 4.096 usec. * (1 << qp->timeout) */
 	mod_timer(&qp->s_timer, jiffies + qp->timeout_jiffies +
@@ -120,6 +123,7 @@ static inline int hfi1_stop_retry_timer(struct rvt_qp *qp)
 {
 	int rval = 0;
 
+	lockdep_assert_held(&qp->s_lock);
 	/* Remove QP from retry */
 	if (qp->s_flags & RVT_S_TIMER) {
 		qp->s_flags &= ~RVT_S_TIMER;
@@ -138,6 +142,7 @@ void hfi1_stop_rc_timers(struct rvt_qp *qp)
 {
 	struct hfi1_qp_priv *priv = qp->priv;
 
+	lockdep_assert_held(&qp->s_lock);
 	/* Remove QP from all timers */
 	if (qp->s_flags & (RVT_S_TIMER | RVT_S_WAIT_RNR)) {
 		qp->s_flags &= ~(RVT_S_TIMER | RVT_S_WAIT_RNR);
@@ -158,6 +163,7 @@ static inline int hfi1_stop_rnr_timer(struct rvt_qp *qp)
 	int rval = 0;
 	struct hfi1_qp_priv *priv = qp->priv;
 
+	lockdep_assert_held(&qp->s_lock);
 	/* Remove QP from rnr timer */
 	if (qp->s_flags & RVT_S_WAIT_RNR) {
 		qp->s_flags &= ~RVT_S_WAIT_RNR;
@@ -178,18 +184,6 @@ void hfi1_del_timers_sync(struct rvt_qp *qp)
 	del_timer_sync(&priv->s_rnr_timer);
 }
 
-/* only opcode mask for adaptive pio */
-const u32 rc_only_opcode =
-	BIT(OP(SEND_ONLY) & 0x1f) |
-	BIT(OP(SEND_ONLY_WITH_IMMEDIATE & 0x1f)) |
-	BIT(OP(RDMA_WRITE_ONLY & 0x1f)) |
-	BIT(OP(RDMA_WRITE_ONLY_WITH_IMMEDIATE & 0x1f)) |
-	BIT(OP(RDMA_READ_REQUEST & 0x1f)) |
-	BIT(OP(ACKNOWLEDGE & 0x1f)) |
-	BIT(OP(ATOMIC_ACKNOWLEDGE & 0x1f)) |
-	BIT(OP(COMPARE_SWAP & 0x1f)) |
-	BIT(OP(FETCH_ADD & 0x1f));
-
 static u32 restart_sge(struct rvt_sge_state *ss, struct rvt_swqe *wqe,
 		       u32 psn, u32 pmtu)
 {
@@ -216,7 +210,7 @@ static u32 restart_sge(struct rvt_sge_state *ss, struct rvt_swqe *wqe,
  * Note the QP s_lock must be held.
  */
 static int make_rc_ack(struct hfi1_ibdev *dev, struct rvt_qp *qp,
-		       struct hfi1_other_headers *ohdr,
+		       struct ib_other_headers *ohdr,
 		       struct hfi1_pkt_state *ps)
 {
 	struct rvt_ack_entry *e;
@@ -228,6 +222,7 @@ static int make_rc_ack(struct hfi1_ibdev *dev, struct rvt_qp *qp,
 	u32 pmtu = qp->pmtu;
 	struct hfi1_qp_priv *priv = qp->priv;
 
+	lockdep_assert_held(&qp->s_lock);
 	/* Don't send an ACK if we aren't supposed to. */
 	if (!(ib_rvt_state_ops[qp->state] & RVT_PROCESS_RECV_OK))
 		goto bail;
@@ -299,10 +294,7 @@ static int make_rc_ack(struct hfi1_ibdev *dev, struct rvt_qp *qp,
 			len = 0;
 			qp->s_ack_state = OP(ATOMIC_ACKNOWLEDGE);
 			ohdr->u.at.aeth = hfi1_compute_aeth(qp);
-			ohdr->u.at.atomic_ack_eth[0] =
-				cpu_to_be32(e->atomic_data >> 32);
-			ohdr->u.at.atomic_ack_eth[1] =
-				cpu_to_be32(e->atomic_data);
+			ib_u64_put(e->atomic_data, &ohdr->u.at.atomic_ack_eth);
 			hwords += sizeof(ohdr->u.at) / sizeof(u32);
 			bth2 = mask_psn(e->psn);
 			e->sent = 1;
@@ -390,7 +382,7 @@ int hfi1_make_rc_req(struct rvt_qp *qp, struct hfi1_pkt_state *ps)
 {
 	struct hfi1_qp_priv *priv = qp->priv;
 	struct hfi1_ibdev *dev = to_idev(qp->ibqp.device);
-	struct hfi1_other_headers *ohdr;
+	struct ib_other_headers *ohdr;
 	struct rvt_sge_state *ss;
 	struct rvt_swqe *wqe;
 	/* header size in 32-bit words LRH+BTH = (8+12)/4. */
@@ -403,6 +395,7 @@ int hfi1_make_rc_req(struct rvt_qp *qp, struct hfi1_pkt_state *ps)
 	int middle = 0;
 	int delta;
 
+	lockdep_assert_held(&qp->s_lock);
 	ps->s_txreq = get_txreq(ps->dev, qp);
 	if (IS_ERR(ps->s_txreq))
 		goto bail_no_tx;
@@ -566,8 +559,9 @@ int hfi1_make_rc_req(struct rvt_qp *qp, struct hfi1_pkt_state *ps)
 				qp->s_flags |= RVT_S_WAIT_SSN_CREDIT;
 				goto bail;
 			}
-			ohdr->u.rc.reth.vaddr =
-				cpu_to_be64(wqe->rdma_wr.remote_addr);
+			put_ib_reth_vaddr(
+				wqe->rdma_wr.remote_addr,
+				&ohdr->u.rc.reth);
 			ohdr->u.rc.reth.rkey =
 				cpu_to_be32(wqe->rdma_wr.rkey);
 			ohdr->u.rc.reth.length = cpu_to_be32(len);
@@ -608,8 +602,9 @@ int hfi1_make_rc_req(struct rvt_qp *qp, struct hfi1_pkt_state *ps)
 				if (!(qp->s_flags & RVT_S_UNLIMITED_CREDIT))
 					qp->s_lsn++;
 			}
-			ohdr->u.rc.reth.vaddr =
-				cpu_to_be64(wqe->rdma_wr.remote_addr);
+			put_ib_reth_vaddr(
+				wqe->rdma_wr.remote_addr,
+				&ohdr->u.rc.reth);
 			ohdr->u.rc.reth.rkey =
 				cpu_to_be32(wqe->rdma_wr.rkey);
 			ohdr->u.rc.reth.length = cpu_to_be32(len);
@@ -640,20 +635,18 @@ int hfi1_make_rc_req(struct rvt_qp *qp, struct hfi1_pkt_state *ps)
 			}
 			if (wqe->wr.opcode == IB_WR_ATOMIC_CMP_AND_SWP) {
 				qp->s_state = OP(COMPARE_SWAP);
-				ohdr->u.atomic_eth.swap_data = cpu_to_be64(
-					wqe->atomic_wr.swap);
-				ohdr->u.atomic_eth.compare_data = cpu_to_be64(
-					wqe->atomic_wr.compare_add);
+				put_ib_ateth_swap(wqe->atomic_wr.swap,
+						  &ohdr->u.atomic_eth);
+				put_ib_ateth_compare(wqe->atomic_wr.compare_add,
+						     &ohdr->u.atomic_eth);
 			} else {
 				qp->s_state = OP(FETCH_ADD);
-				ohdr->u.atomic_eth.swap_data = cpu_to_be64(
-					wqe->atomic_wr.compare_add);
-				ohdr->u.atomic_eth.compare_data = 0;
+				put_ib_ateth_swap(wqe->atomic_wr.compare_add,
+						  &ohdr->u.atomic_eth);
+				put_ib_ateth_compare(0, &ohdr->u.atomic_eth);
 			}
-			ohdr->u.atomic_eth.vaddr[0] = cpu_to_be32(
-				wqe->atomic_wr.remote_addr >> 32);
-			ohdr->u.atomic_eth.vaddr[1] = cpu_to_be32(
-				wqe->atomic_wr.remote_addr);
+			put_ib_ateth_vaddr(wqe->atomic_wr.remote_addr,
+					   &ohdr->u.atomic_eth);
 			ohdr->u.atomic_eth.rkey = cpu_to_be32(
 				wqe->atomic_wr.rkey);
 			hwords += sizeof(struct ib_atomic_eth) / sizeof(u32);
@@ -779,8 +772,9 @@ int hfi1_make_rc_req(struct rvt_qp *qp, struct hfi1_pkt_state *ps)
 		 * See restart_rc().
 		 */
 		len = (delta_psn(qp->s_psn, wqe->psn)) * pmtu;
-		ohdr->u.rc.reth.vaddr =
-			cpu_to_be64(wqe->rdma_wr.remote_addr + len);
+		put_ib_reth_vaddr(
+			wqe->rdma_wr.remote_addr + len,
+			&ohdr->u.rc.reth);
 		ohdr->u.rc.reth.rkey =
 			cpu_to_be32(wqe->rdma_wr.rkey);
 		ohdr->u.rc.reth.length = cpu_to_be32(wqe->length - len);
@@ -841,7 +835,7 @@ bail_no_tx:
  *
  * This is called from hfi1_rc_rcv() and handle_receive_interrupt().
  * Note that RDMA reads and atomics are handled in the
- * send side QP state and tasklet.
+ * send side QP state and send engine.
  */
 void hfi1_send_rc_ack(struct hfi1_ctxtdata *rcd, struct rvt_qp *qp,
 		      int is_fecn)
@@ -856,8 +850,8 @@ void hfi1_send_rc_ack(struct hfi1_ctxtdata *rcd, struct rvt_qp *qp,
 	u32 vl, plen;
 	struct send_context *sc;
 	struct pio_buf *pbuf;
-	struct hfi1_ib_header hdr;
-	struct hfi1_other_headers *ohdr;
+	struct ib_header hdr;
+	struct ib_other_headers *ohdr;
 	unsigned long flags;
 
 	/* Don't send ACK or NAK if a RDMA read or atomic is pending. */
@@ -917,7 +911,7 @@ void hfi1_send_rc_ack(struct hfi1_ctxtdata *rcd, struct rvt_qp *qp,
 	if (!pbuf) {
 		/*
 		 * We have no room to send at the moment.  Pass
-		 * responsibility for sending the ACK to the send tasklet
+		 * responsibility for sending the ACK to the send engine
 		 * so that when enough buffer space becomes available,
 		 * the ACK is sent ahead of other outgoing packets.
 		 */
@@ -932,16 +926,19 @@ void hfi1_send_rc_ack(struct hfi1_ctxtdata *rcd, struct rvt_qp *qp,
 	return;
 
 queue_ack:
-	this_cpu_inc(*ibp->rvp.rc_qacks);
 	spin_lock_irqsave(&qp->s_lock, flags);
+	if (!(ib_rvt_state_ops[qp->state] & RVT_PROCESS_RECV_OK))
+		goto unlock;
+	this_cpu_inc(*ibp->rvp.rc_qacks);
 	qp->s_flags |= RVT_S_ACK_PENDING | RVT_S_RESP_PENDING;
 	qp->s_nak_state = qp->r_nak_state;
 	qp->s_ack_psn = qp->r_ack_psn;
 	if (is_fecn)
 		qp->s_flags |= RVT_S_ECN;
 
-	/* Schedule the send tasklet. */
+	/* Schedule the send engine. */
 	hfi1_schedule_send(qp);
+unlock:
 	spin_unlock_irqrestore(&qp->s_lock, flags);
 }
 
@@ -960,6 +957,7 @@ static void reset_psn(struct rvt_qp *qp, u32 psn)
 	struct rvt_swqe *wqe = rvt_get_swqe_ptr(qp, n);
 	u32 opcode;
 
+	lockdep_assert_held(&qp->s_lock);
 	qp->s_cur = n;
 
 	/*
@@ -1027,7 +1025,7 @@ done:
 	qp->s_psn = psn;
 	/*
 	 * Set RVT_S_WAIT_PSN as rc_complete() may start the timer
-	 * asynchronously before the send tasklet can get scheduled.
+	 * asynchronously before the send engine can get scheduled.
 	 * Doing it in hfi1_make_rc_req() is too late.
 	 */
 	if ((cmp_psn(qp->s_psn, qp->s_sending_hpsn) <= 0) &&
@@ -1045,6 +1043,8 @@ static void restart_rc(struct rvt_qp *qp, u32 psn, int wait)
 	struct rvt_swqe *wqe = rvt_get_swqe_ptr(qp, qp->s_acked);
 	struct hfi1_ibport *ibp;
 
+	lockdep_assert_held(&qp->r_lock);
+	lockdep_assert_held(&qp->s_lock);
 	if (qp->s_retry == 0) {
 		if (qp->s_mig_state == IB_MIG_ARMED) {
 			hfi1_migrate_qp(qp);
@@ -1121,6 +1121,7 @@ static void reset_sending_psn(struct rvt_qp *qp, u32 psn)
 	struct rvt_swqe *wqe;
 	u32 n = qp->s_last;
 
+	lockdep_assert_held(&qp->s_lock);
 	/* Find the work request corresponding to the given PSN. */
 	for (;;) {
 		wqe = rvt_get_swqe_ptr(qp, n);
@@ -1141,15 +1142,16 @@ static void reset_sending_psn(struct rvt_qp *qp, u32 psn)
 /*
  * This should be called with the QP s_lock held and interrupts disabled.
  */
-void hfi1_rc_send_complete(struct rvt_qp *qp, struct hfi1_ib_header *hdr)
+void hfi1_rc_send_complete(struct rvt_qp *qp, struct ib_header *hdr)
 {
-	struct hfi1_other_headers *ohdr;
+	struct ib_other_headers *ohdr;
 	struct rvt_swqe *wqe;
 	struct ib_wc wc;
 	unsigned i;
 	u32 opcode;
 	u32 psn;
 
+	lockdep_assert_held(&qp->s_lock);
 	if (!(ib_rvt_state_ops[qp->state] & RVT_PROCESS_OR_FLUSH_SEND))
 		return;
 
@@ -1241,6 +1243,7 @@ static struct rvt_swqe *do_rc_completion(struct rvt_qp *qp,
 	struct ib_wc wc;
 	unsigned i;
 
+	lockdep_assert_held(&qp->s_lock);
 	/*
 	 * Don't decrement refcount and don't generate a
 	 * completion if the SWQE is being resent until the send
@@ -1340,6 +1343,7 @@ static int do_rc_ack(struct rvt_qp *qp, u32 aeth, u32 psn, int opcode,
 	int diff;
 	unsigned long to;
 
+	lockdep_assert_held(&qp->s_lock);
 	/*
 	 * Note that NAKs implicitly ACK outstanding SEND and RDMA write
 	 * requests and implicitly NAK RDMA read and atomic requests issued
@@ -1389,7 +1393,7 @@ static int do_rc_ack(struct rvt_qp *qp, u32 aeth, u32 psn, int opcode,
 				restart_rc(qp, qp->s_last_psn + 1, 0);
 				if (list_empty(&qp->rspwait)) {
 					qp->r_flags |= RVT_R_RSP_SEND;
-					atomic_inc(&qp->refcount);
+					rvt_get_qp(qp);
 					list_add_tail(&qp->rspwait,
 						      &rcd->qp_wait_list);
 				}
@@ -1555,6 +1559,7 @@ static void rdma_seq_err(struct rvt_qp *qp, struct hfi1_ibport *ibp, u32 psn,
 {
 	struct rvt_swqe *wqe;
 
+	lockdep_assert_held(&qp->s_lock);
 	/* Remove QP from retry timer */
 	hfi1_stop_rc_timers(qp);
 
@@ -1573,7 +1578,7 @@ static void rdma_seq_err(struct rvt_qp *qp, struct hfi1_ibport *ibp, u32 psn,
 	restart_rc(qp, qp->s_last_psn + 1, 0);
 	if (list_empty(&qp->rspwait)) {
 		qp->r_flags |= RVT_R_RSP_SEND;
-		atomic_inc(&qp->refcount);
+		rvt_get_qp(qp);
 		list_add_tail(&qp->rspwait, &rcd->qp_wait_list);
 	}
 }
@@ -1595,7 +1600,7 @@ static void rdma_seq_err(struct rvt_qp *qp, struct hfi1_ibport *ibp, u32 psn,
  * Called at interrupt level.
  */
 static void rc_rcv_resp(struct hfi1_ibport *ibp,
-			struct hfi1_other_headers *ohdr,
+			struct ib_other_headers *ohdr,
 			void *data, u32 tlen, struct rvt_qp *qp,
 			u32 opcode, u32 psn, u32 hdrsize, u32 pmtu,
 			struct hfi1_ctxtdata *rcd)
@@ -1649,14 +1654,10 @@ static void rc_rcv_resp(struct hfi1_ibport *ibp,
 	case OP(ATOMIC_ACKNOWLEDGE):
 	case OP(RDMA_READ_RESPONSE_FIRST):
 		aeth = be32_to_cpu(ohdr->u.aeth);
-		if (opcode == OP(ATOMIC_ACKNOWLEDGE)) {
-			__be32 *p = ohdr->u.at.atomic_ack_eth;
-
-			val = ((u64)be32_to_cpu(p[0]) << 32) |
-				be32_to_cpu(p[1]);
-		} else {
+		if (opcode == OP(ATOMIC_ACKNOWLEDGE))
+			val = ib_u64_get(&ohdr->u.at.atomic_ack_eth);
+		else
 			val = 0;
-		}
 		if (!do_rc_ack(qp, aeth, psn, opcode, val, rcd) ||
 		    opcode != OP(RDMA_READ_RESPONSE_FIRST))
 			goto ack_done;
@@ -1782,7 +1783,7 @@ static inline void rc_defered_ack(struct hfi1_ctxtdata *rcd,
 {
 	if (list_empty(&qp->rspwait)) {
 		qp->r_flags |= RVT_R_RSP_NAK;
-		atomic_inc(&qp->refcount);
+		rvt_get_qp(qp);
 		list_add_tail(&qp->rspwait, &rcd->qp_wait_list);
 	}
 }
@@ -1796,8 +1797,7 @@ static inline void rc_cancel_ack(struct rvt_qp *qp)
 		return;
 	list_del_init(&qp->rspwait);
 	qp->r_flags &= ~RVT_R_RSP_NAK;
-	if (atomic_dec_and_test(&qp->refcount))
-		wake_up(&qp->wait);
+	rvt_put_qp(qp);
 }
 
 /**
@@ -1815,7 +1815,7 @@ static inline void rc_cancel_ack(struct rvt_qp *qp)
  * Return 1 if no more processing is needed; otherwise return 0 to
  * schedule a response to be sent.
  */
-static noinline int rc_rcv_error(struct hfi1_other_headers *ohdr, void *data,
+static noinline int rc_rcv_error(struct ib_other_headers *ohdr, void *data,
 				 struct rvt_qp *qp, u32 opcode, u32 psn,
 				 int diff, struct hfi1_ctxtdata *rcd)
 {
@@ -1923,7 +1923,7 @@ static noinline int rc_rcv_error(struct hfi1_other_headers *ohdr, void *data,
 		}
 		if (len != 0) {
 			u32 rkey = be32_to_cpu(reth->rkey);
-			u64 vaddr = be64_to_cpu(reth->vaddr);
+			u64 vaddr = get_ib_reth_vaddr(reth);
 			int ok;
 
 			ok = rvt_rkey_ok(qp, &e->rdma_sge, len, vaddr, rkey,
@@ -1946,7 +1946,7 @@ static noinline int rc_rcv_error(struct hfi1_other_headers *ohdr, void *data,
 	case OP(FETCH_ADD): {
 		/*
 		 * If we didn't find the atomic request in the ack queue
-		 * or the send tasklet is already backed up to send an
+		 * or the send engine is already backed up to send an
 		 * earlier entry, we can ignore this request.
 		 */
 		if (!e || e->opcode != (u8)opcode || old_req)
@@ -2123,13 +2123,13 @@ void process_becn(struct hfi1_pportdata *ppd, u8 sl, u16 rlid, u32 lqpn,
 void hfi1_rc_rcv(struct hfi1_packet *packet)
 {
 	struct hfi1_ctxtdata *rcd = packet->rcd;
-	struct hfi1_ib_header *hdr = packet->hdr;
+	struct ib_header *hdr = packet->hdr;
 	u32 rcv_flags = packet->rcv_flags;
 	void *data = packet->ebuf;
 	u32 tlen = packet->tlen;
 	struct rvt_qp *qp = packet->qp;
 	struct hfi1_ibport *ibp = to_iport(qp->ibqp.device, qp->port_num);
-	struct hfi1_other_headers *ohdr = packet->ohdr;
+	struct ib_other_headers *ohdr = packet->ohdr;
 	u32 bth0, opcode;
 	u32 hdrsize = packet->hlen;
 	u32 psn;
@@ -2143,6 +2143,7 @@ void hfi1_rc_rcv(struct hfi1_packet *packet)
 	int copy_last = 0;
 	u32 rkey;
 
+	lockdep_assert_held(&qp->r_lock);
 	bth0 = be32_to_cpu(ohdr->bth[0]);
 	if (hfi1_ruc_check_hdr(ibp, hdr, rcv_flags & HFI1_HAS_GRH, qp, bth0))
 		return;
@@ -2342,7 +2343,7 @@ send_last:
 		qp->r_sge.sg_list = NULL;
 		if (qp->r_len != 0) {
 			u32 rkey = be32_to_cpu(reth->rkey);
-			u64 vaddr = be64_to_cpu(reth->vaddr);
+			u64 vaddr = get_ib_reth_vaddr(reth);
 			int ok;
 
 			/* Check rkey & NAK */
@@ -2397,7 +2398,7 @@ send_last:
 		len = be32_to_cpu(reth->length);
 		if (len) {
 			u32 rkey = be32_to_cpu(reth->rkey);
-			u64 vaddr = be64_to_cpu(reth->vaddr);
+			u64 vaddr = get_ib_reth_vaddr(reth);
 			int ok;
 
 			/* Check rkey & NAK */
@@ -2432,7 +2433,7 @@ send_last:
 		qp->r_nak_state = 0;
 		qp->r_head_ack_queue = next;
 
-		/* Schedule the send tasklet. */
+		/* Schedule the send engine. */
 		qp->s_flags |= RVT_S_RESP_PENDING;
 		hfi1_schedule_send(qp);
 
@@ -2469,8 +2470,7 @@ send_last:
 			e->rdma_sge.mr = NULL;
 		}
 		ateth = &ohdr->u.atomic_eth;
-		vaddr = ((u64)be32_to_cpu(ateth->vaddr[0]) << 32) |
-			be32_to_cpu(ateth->vaddr[1]);
+		vaddr = get_ib_ateth_vaddr(ateth);
 		if (unlikely(vaddr & (sizeof(u64) - 1)))
 			goto nack_inv_unlck;
 		rkey = be32_to_cpu(ateth->rkey);
@@ -2481,11 +2481,11 @@ send_last:
 			goto nack_acc_unlck;
 		/* Perform atomic OP and save result. */
 		maddr = (atomic64_t *)qp->r_sge.sge.vaddr;
-		sdata = be64_to_cpu(ateth->swap_data);
+		sdata = get_ib_ateth_swap(ateth);
 		e->atomic_data = (opcode == OP(FETCH_ADD)) ?
 			(u64)atomic64_add_return(sdata, maddr) - sdata :
 			(u64)cmpxchg((u64 *)qp->r_sge.sge.vaddr,
-				      be64_to_cpu(ateth->compare_data),
+				      get_ib_ateth_compare(ateth),
 				      sdata);
 		rvt_put_mr(qp->r_sge.sge.mr);
 		qp->r_sge.num_sge = 0;
@@ -2499,7 +2499,7 @@ send_last:
 		qp->r_nak_state = 0;
 		qp->r_head_ack_queue = next;
 
-		/* Schedule the send tasklet. */
+		/* Schedule the send engine. */
 		qp->s_flags |= RVT_S_RESP_PENDING;
 		hfi1_schedule_send(qp);
 
@@ -2575,12 +2575,12 @@ send_ack:
 
 void hfi1_rc_hdrerr(
 	struct hfi1_ctxtdata *rcd,
-	struct hfi1_ib_header *hdr,
+	struct ib_header *hdr,
 	u32 rcv_flags,
 	struct rvt_qp *qp)
 {
 	int has_grh = rcv_flags & HFI1_HAS_GRH;
-	struct hfi1_other_headers *ohdr;
+	struct ib_other_headers *ohdr;
 	struct hfi1_ibport *ibp = to_iport(qp->ibqp.device, qp->port_num);
 	int diff;
 	u32 opcode;
diff --git a/drivers/infiniband/hw/hfi1/ruc.c b/drivers/infiniband/hw/hfi1/ruc.c
index 48d5094f98e2..a1576aea4756 100644
--- a/drivers/infiniband/hw/hfi1/ruc.c
+++ b/drivers/infiniband/hw/hfi1/ruc.c
@@ -262,7 +262,7 @@ static int gid_ok(union ib_gid *gid, __be64 gid_prefix, __be64 id)
  *
  * The s_lock will be acquired around the hfi1_migrate_qp() call.
  */
-int hfi1_ruc_check_hdr(struct hfi1_ibport *ibp, struct hfi1_ib_header *hdr,
+int hfi1_ruc_check_hdr(struct hfi1_ibport *ibp, struct ib_header *hdr,
 		       int has_grh, struct rvt_qp *qp, u32 bth0)
 {
 	__be64 guid;
@@ -352,7 +352,7 @@ err:
  *
  * This is called from hfi1_do_send() to
  * forward a WQE addressed to the same HFI.
- * Note that although we are single threaded due to the tasklet, we still
+ * Note that although we are single threaded due to the send engine, we still
  * have to protect against post_send().  We don't have to worry about
  * receive interrupts since this is a connected protocol and all packets
  * will pass through here.
@@ -765,7 +765,7 @@ static inline void build_ahg(struct rvt_qp *qp, u32 npsn)
 	}
 }
 
-void hfi1_make_ruc_header(struct rvt_qp *qp, struct hfi1_other_headers *ohdr,
+void hfi1_make_ruc_header(struct rvt_qp *qp, struct ib_other_headers *ohdr,
 			  u32 bth0, u32 bth2, int middle,
 			  struct hfi1_pkt_state *ps)
 {
@@ -846,7 +846,7 @@ void _hfi1_do_send(struct work_struct *work)
  * @work: contains a pointer to the QP
  *
  * Process entries in the send work queue until credit or queue is
- * exhausted.  Only allow one CPU to send a packet per QP (tasklet).
+ * exhausted.  Only allow one CPU to send a packet per QP.
  * Otherwise, two threads could send packets out of order.
  */
 void hfi1_do_send(struct rvt_qp *qp)
@@ -909,7 +909,7 @@ void hfi1_do_send(struct rvt_qp *qp)
 			spin_unlock_irqrestore(&qp->s_lock, ps.flags);
 			/*
 			 * If the packet cannot be sent now, return and
-			 * the send tasklet will be woken up later.
+			 * the send engine will be woken up later.
 			 */
 			if (hfi1_verbs_send(qp, &ps))
 				return;
diff --git a/drivers/infiniband/hw/hfi1/sdma.c b/drivers/infiniband/hw/hfi1/sdma.c
index f9befc05b349..fd39bcaa062d 100644
--- a/drivers/infiniband/hw/hfi1/sdma.c
+++ b/drivers/infiniband/hw/hfi1/sdma.c
@@ -726,6 +726,34 @@ u16 sdma_get_descq_cnt(void)
 }
 
 /**
+ * sdma_engine_get_vl() - return vl for a given sdma engine
+ * @sde: sdma engine
+ *
+ * This function returns the vl mapped to a given engine, or an error if
+ * the mapping can't be found. The mapping fields are protected by RCU.
+ */
+int sdma_engine_get_vl(struct sdma_engine *sde)
+{
+	struct hfi1_devdata *dd = sde->dd;
+	struct sdma_vl_map *m;
+	u8 vl;
+
+	if (sde->this_idx >= TXE_NUM_SDMA_ENGINES)
+		return -EINVAL;
+
+	rcu_read_lock();
+	m = rcu_dereference(dd->sdma_map);
+	if (unlikely(!m)) {
+		rcu_read_unlock();
+		return -EINVAL;
+	}
+	vl = m->engine_to_vl[sde->this_idx];
+	rcu_read_unlock();
+
+	return vl;
+}
+
+/**
  * sdma_select_engine_vl() - select sdma engine
  * @dd: devdata
  * @selector: a spreading factor
@@ -788,6 +816,326 @@ struct sdma_engine *sdma_select_engine_sc(
 	return sdma_select_engine_vl(dd, selector, vl);
 }
 
+struct sdma_rht_map_elem {
+	u32 mask;
+	u8 ctr;
+	struct sdma_engine *sde[0];
+};
+
+struct sdma_rht_node {
+	unsigned long cpu_id;
+	struct sdma_rht_map_elem *map[HFI1_MAX_VLS_SUPPORTED];
+	struct rhash_head node;
+};
+
+#define NR_CPUS_HINT 192
+
+static const struct rhashtable_params sdma_rht_params = {
+	.nelem_hint = NR_CPUS_HINT,
+	.head_offset = offsetof(struct sdma_rht_node, node),
+	.key_offset = offsetof(struct sdma_rht_node, cpu_id),
+	.key_len = FIELD_SIZEOF(struct sdma_rht_node, cpu_id),
+	.max_size = NR_CPUS,
+	.min_size = 8,
+	.automatic_shrinking = true,
+};
+
+/*
+ * sdma_select_user_engine() - select sdma engine based on user setup
+ * @dd: devdata
+ * @selector: a spreading factor
+ * @vl: this vl
+ *
+ * This function returns an sdma engine for a user sdma request.
+ * User defined sdma engine affinity setting is honored when applicable,
+ * otherwise system default sdma engine mapping is used. To ensure correct
+ * ordering, the mapping from <selector, vl> to sde must remain unchanged.
+ */
+struct sdma_engine *sdma_select_user_engine(struct hfi1_devdata *dd,
+					    u32 selector, u8 vl)
+{
+	struct sdma_rht_node *rht_node;
+	struct sdma_engine *sde = NULL;
+	const struct cpumask *current_mask = tsk_cpus_allowed(current);
+	unsigned long cpu_id;
+
+	/*
+	 * To ensure that always the same sdma engine(s) will be
+	 * selected make sure the process is pinned to this CPU only.
+	 */
+	if (cpumask_weight(current_mask) != 1)
+		goto out;
+
+	cpu_id = smp_processor_id();
+	rcu_read_lock();
+	rht_node = rhashtable_lookup_fast(&dd->sdma_rht, &cpu_id,
+					  sdma_rht_params);
+
+	if (rht_node && rht_node->map[vl]) {
+		struct sdma_rht_map_elem *map = rht_node->map[vl];
+
+		sde = map->sde[selector & map->mask];
+	}
+	rcu_read_unlock();
+
+	if (sde)
+		return sde;
+
+out:
+	return sdma_select_engine_vl(dd, selector, vl);
+}
+
+static void sdma_populate_sde_map(struct sdma_rht_map_elem *map)
+{
+	int i;
+
+	for (i = 0; i < roundup_pow_of_two(map->ctr ? : 1) - map->ctr; i++)
+		map->sde[map->ctr + i] = map->sde[i];
+}
+
+static void sdma_cleanup_sde_map(struct sdma_rht_map_elem *map,
+				 struct sdma_engine *sde)
+{
+	unsigned int i, pow;
+
+	/* only need to check the first ctr entries for a match */
+	for (i = 0; i < map->ctr; i++) {
+		if (map->sde[i] == sde) {
+			memmove(&map->sde[i], &map->sde[i + 1],
+				(map->ctr - i - 1) * sizeof(map->sde[0]));
+			map->ctr--;
+			pow = roundup_pow_of_two(map->ctr ? : 1);
+			map->mask = pow - 1;
+			sdma_populate_sde_map(map);
+			break;
+		}
+	}
+}
+
+/*
+ * Prevents concurrent reads and writes of the sdma engine cpu_mask
+ */
+static DEFINE_MUTEX(process_to_sde_mutex);
+
+ssize_t sdma_set_cpu_to_sde_map(struct sdma_engine *sde, const char *buf,
+				size_t count)
+{
+	struct hfi1_devdata *dd = sde->dd;
+	cpumask_var_t mask, new_mask;
+	unsigned long cpu;
+	int ret, vl, sz;
+
+	vl = sdma_engine_get_vl(sde);
+	if (unlikely(vl < 0))
+		return -EINVAL;
+
+	ret = zalloc_cpumask_var(&mask, GFP_KERNEL);
+	if (!ret)
+		return -ENOMEM;
+
+	ret = zalloc_cpumask_var(&new_mask, GFP_KERNEL);
+	if (!ret) {
+		free_cpumask_var(mask);
+		return -ENOMEM;
+	}
+	ret = cpulist_parse(buf, mask);
+	if (ret)
+		goto out_free;
+
+	if (!cpumask_subset(mask, cpu_online_mask)) {
+		dd_dev_warn(sde->dd, "Invalid CPU mask\n");
+		ret = -EINVAL;
+		goto out_free;
+	}
+
+	sz = sizeof(struct sdma_rht_map_elem) +
+			(TXE_NUM_SDMA_ENGINES * sizeof(struct sdma_engine *));
+
+	mutex_lock(&process_to_sde_mutex);
+
+	for_each_cpu(cpu, mask) {
+		struct sdma_rht_node *rht_node;
+
+		/* Check if we have this already mapped */
+		if (cpumask_test_cpu(cpu, &sde->cpu_mask)) {
+			cpumask_set_cpu(cpu, new_mask);
+			continue;
+		}
+
+		rht_node = rhashtable_lookup_fast(&dd->sdma_rht, &cpu,
+						  sdma_rht_params);
+		if (!rht_node) {
+			rht_node = kzalloc(sizeof(*rht_node), GFP_KERNEL);
+			if (!rht_node) {
+				ret = -ENOMEM;
+				goto out;
+			}
+
+			rht_node->map[vl] = kzalloc(sz, GFP_KERNEL);
+			if (!rht_node->map[vl]) {
+				kfree(rht_node);
+				ret = -ENOMEM;
+				goto out;
+			}
+			rht_node->cpu_id = cpu;
+			rht_node->map[vl]->mask = 0;
+			rht_node->map[vl]->ctr = 1;
+			rht_node->map[vl]->sde[0] = sde;
+
+			ret = rhashtable_insert_fast(&dd->sdma_rht,
+						     &rht_node->node,
+						     sdma_rht_params);
+			if (ret) {
+				kfree(rht_node->map[vl]);
+				kfree(rht_node);
+				dd_dev_err(sde->dd, "Failed to set process to sde affinity for cpu %lu\n",
+					   cpu);
+				goto out;
+			}
+
+		} else {
+			int ctr, pow;
+
+			/* Add new user mappings */
+			if (!rht_node->map[vl])
+				rht_node->map[vl] = kzalloc(sz, GFP_KERNEL);
+
+			if (!rht_node->map[vl]) {
+				ret = -ENOMEM;
+				goto out;
+			}
+
+			rht_node->map[vl]->ctr++;
+			ctr = rht_node->map[vl]->ctr;
+			rht_node->map[vl]->sde[ctr - 1] = sde;
+			pow = roundup_pow_of_two(ctr);
+			rht_node->map[vl]->mask = pow - 1;
+
+			/* Populate the sde map table */
+			sdma_populate_sde_map(rht_node->map[vl]);
+		}
+		cpumask_set_cpu(cpu, new_mask);
+	}
+
+	/* Clean up old mappings */
+	for_each_cpu(cpu, cpu_online_mask) {
+		struct sdma_rht_node *rht_node;
+
+		/* Don't cleanup sdes that are set in the new mask */
+		if (cpumask_test_cpu(cpu, mask))
+			continue;
+
+		rht_node = rhashtable_lookup_fast(&dd->sdma_rht, &cpu,
+						  sdma_rht_params);
+		if (rht_node) {
+			bool empty = true;
+			int i;
+
+			/* Remove mappings for old sde */
+			for (i = 0; i < HFI1_MAX_VLS_SUPPORTED; i++)
+				if (rht_node->map[i])
+					sdma_cleanup_sde_map(rht_node->map[i],
+							     sde);
+
+			/* Free empty hash table entries */
+			for (i = 0; i < HFI1_MAX_VLS_SUPPORTED; i++) {
+				if (!rht_node->map[i])
+					continue;
+
+				if (rht_node->map[i]->ctr) {
+					empty = false;
+					break;
+				}
+			}
+
+			if (empty) {
+				ret = rhashtable_remove_fast(&dd->sdma_rht,
+							     &rht_node->node,
+							     sdma_rht_params);
+				WARN_ON(ret);
+
+				for (i = 0; i < HFI1_MAX_VLS_SUPPORTED; i++)
+					kfree(rht_node->map[i]);
+
+				kfree(rht_node);
+			}
+		}
+	}
+
+	cpumask_copy(&sde->cpu_mask, new_mask);
+out:
+	mutex_unlock(&process_to_sde_mutex);
+out_free:
+	free_cpumask_var(mask);
+	free_cpumask_var(new_mask);
+	return ret ? : strnlen(buf, PAGE_SIZE);
+}
+
+ssize_t sdma_get_cpu_to_sde_map(struct sdma_engine *sde, char *buf)
+{
+	mutex_lock(&process_to_sde_mutex);
+	if (cpumask_empty(&sde->cpu_mask))
+		snprintf(buf, PAGE_SIZE, "%s\n", "empty");
+	else
+		cpumap_print_to_pagebuf(true, buf, &sde->cpu_mask);
+	mutex_unlock(&process_to_sde_mutex);
+	return strnlen(buf, PAGE_SIZE);
+}
+
+static void sdma_rht_free(void *ptr, void *arg)
+{
+	struct sdma_rht_node *rht_node = ptr;
+	int i;
+
+	for (i = 0; i < HFI1_MAX_VLS_SUPPORTED; i++)
+		kfree(rht_node->map[i]);
+
+	kfree(rht_node);
+}
+
+/**
+ * sdma_seqfile_dump_cpu_list() - debugfs dump the cpu to sdma mappings
+ * @s: seq file
+ * @dd: hfi1_devdata
+ * @cpuid: cpu id
+ *
+ * This routine dumps the process to sde mappings per cpu
+ */
+void sdma_seqfile_dump_cpu_list(struct seq_file *s,
+				struct hfi1_devdata *dd,
+				unsigned long cpuid)
+{
+	struct sdma_rht_node *rht_node;
+	int i, j;
+
+	rht_node = rhashtable_lookup_fast(&dd->sdma_rht, &cpuid,
+					  sdma_rht_params);
+	if (!rht_node)
+		return;
+
+	seq_printf(s, "cpu%3lu: ", cpuid);
+	for (i = 0; i < HFI1_MAX_VLS_SUPPORTED; i++) {
+		if (!rht_node->map[i] || !rht_node->map[i]->ctr)
+			continue;
+
+		seq_printf(s, " vl%d: [", i);
+
+		for (j = 0; j < rht_node->map[i]->ctr; j++) {
+			if (!rht_node->map[i]->sde[j])
+				continue;
+
+			if (j > 0)
+				seq_puts(s, ",");
+
+			seq_printf(s, " sdma%2d",
+				   rht_node->map[i]->sde[j]->this_idx);
+		}
+		seq_puts(s, " ]");
+	}
+
+	seq_puts(s, "\n");
+}
+
 /*
  * Free the indicated map struct
  */
@@ -1161,6 +1509,10 @@ int sdma_init(struct hfi1_devdata *dd, u8 port)
 	dd->num_sdma = num_engines;
 	if (sdma_map_init(dd, port, ppd->vls_operational, NULL))
 		goto bail;
+
+	if (rhashtable_init(&dd->sdma_rht, &sdma_rht_params))
+		goto bail;
+
 	dd_dev_info(dd, "SDMA num_sdma: %u\n", dd->num_sdma);
 	return 0;
 
@@ -1252,6 +1604,7 @@ void sdma_exit(struct hfi1_devdata *dd)
 		sdma_finalput(&sde->state);
 	}
 	sdma_clean(dd, dd->num_sdma);
+	rhashtable_free_and_destroy(&dd->sdma_rht, sdma_rht_free, NULL);
 }
 
 /*
@@ -2086,6 +2439,11 @@ nodesc:
  * @sde: sdma engine to use
  * @wait: wait structure to use when full (may be NULL)
  * @tx_list: list of sdma_txreqs to submit
+ * @count: pointer to a u32 which, after return will contain the total number of
+ *         sdma_txreqs removed from the tx_list. This will include sdma_txreqs
+ *         whose SDMA descriptors are submitted to the ring and the sdma_txreqs
+ *         which are added to SDMA engine flush list if the SDMA engine state is
+ *         not running.
  *
  * The call submits the list into the ring.
  *
@@ -2100,18 +2458,18 @@ nodesc:
  * side locking.
  *
  * Return:
- * > 0 - Success (value is number of sdma_txreq's submitted),
+ * 0 - Success,
  * -EINVAL - sdma_txreq incomplete, -EBUSY - no space in ring (wait == NULL)
  * -EIOCBQUEUED - tx queued to iowait, -ECOMM bad sdma state
  */
 int sdma_send_txlist(struct sdma_engine *sde, struct iowait *wait,
-		     struct list_head *tx_list)
+		     struct list_head *tx_list, u32 *count_out)
 {
 	struct sdma_txreq *tx, *tx_next;
 	int ret = 0;
 	unsigned long flags;
 	u16 tail = INVALID_TAIL;
-	int count = 0;
+	u32 submit_count = 0, flush_count = 0, total_count;
 
 	spin_lock_irqsave(&sde->tail_lock, flags);
 retry:
@@ -2127,33 +2485,34 @@ retry:
 		}
 		list_del_init(&tx->list);
 		tail = submit_tx(sde, tx);
-		count++;
+		submit_count++;
 		if (tail != INVALID_TAIL &&
-		    (count & SDMA_TAIL_UPDATE_THRESH) == 0) {
+		    (submit_count & SDMA_TAIL_UPDATE_THRESH) == 0) {
 			sdma_update_tail(sde, tail);
 			tail = INVALID_TAIL;
 		}
 	}
 update_tail:
+	total_count = submit_count + flush_count;
 	if (wait)
-		iowait_sdma_add(wait, count);
+		iowait_sdma_add(wait, total_count);
 	if (tail != INVALID_TAIL)
 		sdma_update_tail(sde, tail);
 	spin_unlock_irqrestore(&sde->tail_lock, flags);
-	return ret == 0 ? count : ret;
+	*count_out = total_count;
+	return ret;
 unlock_noconn:
 	spin_lock(&sde->flushlist_lock);
 	list_for_each_entry_safe(tx, tx_next, tx_list, list) {
 		tx->wait = wait;
 		list_del_init(&tx->list);
-		if (wait)
-			iowait_sdma_inc(wait);
 		tx->next_descq_idx = 0;
 #ifdef CONFIG_HFI1_DEBUG_SDMA_ORDER
 		tx->sn = sde->tail_sn++;
 		trace_hfi1_sdma_in_sn(sde, tx->sn);
 #endif
 		list_add_tail(&tx->list, &sde->flushlist);
+		flush_count++;
 		if (wait) {
 			wait->tx_count++;
 			wait->count += tx->num_desc;
diff --git a/drivers/infiniband/hw/hfi1/sdma.h b/drivers/infiniband/hw/hfi1/sdma.h
index 8f50c99fe711..56257ea3598f 100644
--- a/drivers/infiniband/hw/hfi1/sdma.h
+++ b/drivers/infiniband/hw/hfi1/sdma.h
@@ -413,6 +413,8 @@ struct sdma_engine {
 	spinlock_t flushlist_lock;
 	/* private: */
 	struct list_head flushlist;
+	struct cpumask cpu_mask;
+	struct kobject kobj;
 };
 
 int sdma_init(struct hfi1_devdata *dd, u8 port);
@@ -847,7 +849,8 @@ int sdma_send_txreq(struct sdma_engine *sde,
 		    struct sdma_txreq *tx);
 int sdma_send_txlist(struct sdma_engine *sde,
 		     struct iowait *wait,
-		     struct list_head *tx_list);
+		     struct list_head *tx_list,
+		     u32 *count);
 
 int sdma_ahg_alloc(struct sdma_engine *sde);
 void sdma_ahg_free(struct sdma_engine *sde, int ahg_index);
@@ -1058,7 +1061,15 @@ struct sdma_engine *sdma_select_engine_vl(
 	u32 selector,
 	u8 vl);
 
+struct sdma_engine *sdma_select_user_engine(struct hfi1_devdata *dd,
+					    u32 selector, u8 vl);
+ssize_t sdma_get_cpu_to_sde_map(struct sdma_engine *sde, char *buf);
+ssize_t sdma_set_cpu_to_sde_map(struct sdma_engine *sde, const char *buf,
+				size_t count);
+int sdma_engine_get_vl(struct sdma_engine *sde);
 void sdma_seqfile_dump_sde(struct seq_file *s, struct sdma_engine *);
+void sdma_seqfile_dump_cpu_list(struct seq_file *s, struct hfi1_devdata *dd,
+				unsigned long cpuid);
 
 #ifdef CONFIG_SDMA_VERBOSITY
 void sdma_dumpstate(struct sdma_engine *);
diff --git a/drivers/infiniband/hw/hfi1/sysfs.c b/drivers/infiniband/hw/hfi1/sysfs.c
index 74c84c655f7e..edba22461a9c 100644
--- a/drivers/infiniband/hw/hfi1/sysfs.c
+++ b/drivers/infiniband/hw/hfi1/sysfs.c
@@ -766,13 +766,95 @@ bail:
 	return ret;
 }
 
+struct sde_attribute {
+	struct attribute attr;
+	ssize_t (*show)(struct sdma_engine *sde, char *buf);
+	ssize_t (*store)(struct sdma_engine *sde, const char *buf, size_t cnt);
+};
+
+static ssize_t sde_show(struct kobject *kobj, struct attribute *attr, char *buf)
+{
+	struct sde_attribute *sde_attr =
+		container_of(attr, struct sde_attribute, attr);
+	struct sdma_engine *sde =
+		container_of(kobj, struct sdma_engine, kobj);
+
+	if (!sde_attr->show)
+		return -EINVAL;
+
+	return sde_attr->show(sde, buf);
+}
+
+static ssize_t sde_store(struct kobject *kobj, struct attribute *attr,
+			 const char *buf, size_t count)
+{
+	struct sde_attribute *sde_attr =
+		container_of(attr, struct sde_attribute, attr);
+	struct sdma_engine *sde =
+		container_of(kobj, struct sdma_engine, kobj);
+
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+
+	if (!sde_attr->store)
+		return -EINVAL;
+
+	return sde_attr->store(sde, buf, count);
+}
+
+static const struct sysfs_ops sde_sysfs_ops = {
+	.show = sde_show,
+	.store = sde_store,
+};
+
+static struct kobj_type sde_ktype = {
+	.sysfs_ops = &sde_sysfs_ops,
+};
+
+#define SDE_ATTR(_name, _mode, _show, _store) \
+	struct sde_attribute sde_attr_##_name = \
+		__ATTR(_name, _mode, _show, _store)
+
+static ssize_t sde_show_cpu_to_sde_map(struct sdma_engine *sde, char *buf)
+{
+	return sdma_get_cpu_to_sde_map(sde, buf);
+}
+
+static ssize_t sde_store_cpu_to_sde_map(struct sdma_engine *sde,
+					const char *buf, size_t count)
+{
+	return sdma_set_cpu_to_sde_map(sde, buf, count);
+}
+
+static ssize_t sde_show_vl(struct sdma_engine *sde, char *buf)
+{
+	int vl;
+
+	vl = sdma_engine_get_vl(sde);
+	if (vl < 0)
+		return vl;
+
+	return snprintf(buf, PAGE_SIZE, "%d\n", vl);
+}
+
+static SDE_ATTR(cpu_list, S_IWUSR | S_IRUGO,
+		sde_show_cpu_to_sde_map,
+		sde_store_cpu_to_sde_map);
+static SDE_ATTR(vl, S_IRUGO, sde_show_vl, NULL);
+
+static struct sde_attribute *sde_attribs[] = {
+	&sde_attr_cpu_list,
+	&sde_attr_vl
+};
+
 /*
  * Register and create our files in /sys/class/infiniband.
  */
 int hfi1_verbs_register_sysfs(struct hfi1_devdata *dd)
 {
 	struct ib_device *dev = &dd->verbs_dev.rdi.ibdev;
-	int i, ret;
+	struct device *class_dev = &dev->dev;
+	int i, j, ret;
 
 	for (i = 0; i < ARRAY_SIZE(hfi1_attributes); ++i) {
 		ret = device_create_file(&dev->dev, hfi1_attributes[i]);
@@ -780,10 +862,29 @@ int hfi1_verbs_register_sysfs(struct hfi1_devdata *dd)
 			goto bail;
 	}
 
+	for (i = 0; i < dd->num_sdma; i++) {
+		ret = kobject_init_and_add(&dd->per_sdma[i].kobj,
+					   &sde_ktype, &class_dev->kobj,
+					   "sdma%d", i);
+		if (ret)
+			goto bail;
+
+		for (j = 0; j < ARRAY_SIZE(sde_attribs); j++) {
+			ret = sysfs_create_file(&dd->per_sdma[i].kobj,
+						&sde_attribs[j]->attr);
+			if (ret)
+				goto bail;
+		}
+	}
+
 	return 0;
 bail:
 	for (i = 0; i < ARRAY_SIZE(hfi1_attributes); ++i)
 		device_remove_file(&dev->dev, hfi1_attributes[i]);
+
+	for (i = 0; i < dd->num_sdma; i++)
+		kobject_del(&dd->per_sdma[i].kobj);
+
 	return ret;
 }
 
diff --git a/drivers/infiniband/hw/hfi1/trace.c b/drivers/infiniband/hw/hfi1/trace.c
index 4cfb13771897..01f525cd985a 100644
--- a/drivers/infiniband/hw/hfi1/trace.c
+++ b/drivers/infiniband/hw/hfi1/trace.c
@@ -47,9 +47,9 @@
 #define CREATE_TRACE_POINTS
 #include "trace.h"
 
-u8 ibhdr_exhdr_len(struct hfi1_ib_header *hdr)
+u8 ibhdr_exhdr_len(struct ib_header *hdr)
 {
-	struct hfi1_other_headers *ohdr;
+	struct ib_other_headers *ohdr;
 	u8 opcode;
 	u8 lnh = (u8)(be16_to_cpu(hdr->lrh[0]) & 3);
 
@@ -67,16 +67,11 @@ u8 ibhdr_exhdr_len(struct hfi1_ib_header *hdr)
 #define AETH_PRN "aeth syn 0x%.2x %s msn 0x%.8x"
 #define DETH_PRN "deth qkey 0x%.8x sqpn 0x%.6x"
 #define IETH_PRN "ieth rkey 0x%.8x"
-#define ATOMICACKETH_PRN "origdata %lld"
-#define ATOMICETH_PRN "vaddr 0x%llx rkey 0x%.8x sdata %lld cdata %lld"
+#define ATOMICACKETH_PRN "origdata %llx"
+#define ATOMICETH_PRN "vaddr 0x%llx rkey 0x%.8x sdata %llx cdata %llx"
 
 #define OP(transport, op) IB_OPCODE_## transport ## _ ## op
 
-static u64 ib_u64_get(__be32 *p)
-{
-	return ((u64)be32_to_cpu(p[0]) << 32) | be32_to_cpu(p[1]);
-}
-
 static const char *parse_syndrome(u8 syndrome)
 {
 	switch (syndrome >> 5) {
@@ -113,8 +108,7 @@ const char *parse_everbs_hdrs(
 	case OP(RC, RDMA_WRITE_ONLY_WITH_IMMEDIATE):
 	case OP(UC, RDMA_WRITE_ONLY_WITH_IMMEDIATE):
 		trace_seq_printf(p, RETH_PRN " " IMM_PRN,
-				 (unsigned long long)ib_u64_get(
-				 (__be32 *)&eh->rc.reth.vaddr),
+				 get_ib_reth_vaddr(&eh->rc.reth),
 				 be32_to_cpu(eh->rc.reth.rkey),
 				 be32_to_cpu(eh->rc.reth.length),
 				 be32_to_cpu(eh->rc.imm_data));
@@ -126,8 +120,7 @@ const char *parse_everbs_hdrs(
 	case OP(RC, RDMA_WRITE_ONLY):
 	case OP(UC, RDMA_WRITE_ONLY):
 		trace_seq_printf(p, RETH_PRN,
-				 (unsigned long long)ib_u64_get(
-				 (__be32 *)&eh->rc.reth.vaddr),
+				 get_ib_reth_vaddr(&eh->rc.reth),
 				 be32_to_cpu(eh->rc.reth.rkey),
 				 be32_to_cpu(eh->rc.reth.length));
 		break;
@@ -145,20 +138,16 @@ const char *parse_everbs_hdrs(
 				 be32_to_cpu(eh->at.aeth) >> 24,
 				 parse_syndrome(be32_to_cpu(eh->at.aeth) >> 24),
 				 be32_to_cpu(eh->at.aeth) & HFI1_MSN_MASK,
-				 (unsigned long long)
-				 ib_u64_get(eh->at.atomic_ack_eth));
+				 ib_u64_get(&eh->at.atomic_ack_eth));
 		break;
 	/* atomiceth */
 	case OP(RC, COMPARE_SWAP):
 	case OP(RC, FETCH_ADD):
 		trace_seq_printf(p, ATOMICETH_PRN,
-				 (unsigned long long)ib_u64_get(
-				 eh->atomic_eth.vaddr),
+				 get_ib_ateth_vaddr(&eh->atomic_eth),
 				 eh->atomic_eth.rkey,
-				 (unsigned long long)ib_u64_get(
-				 (__be32 *)&eh->atomic_eth.swap_data),
-				 (unsigned long long)ib_u64_get(
-				 (__be32 *)&eh->atomic_eth.compare_data));
+				 get_ib_ateth_swap(&eh->atomic_eth),
+				 get_ib_ateth_compare(&eh->atomic_eth));
 		break;
 	/* deth */
 	case OP(UD, SEND_ONLY):
diff --git a/drivers/infiniband/hw/hfi1/trace_ctxts.h b/drivers/infiniband/hw/hfi1/trace_ctxts.h
index 31654bbac1cf..26ae789e47cf 100644
--- a/drivers/infiniband/hw/hfi1/trace_ctxts.h
+++ b/drivers/infiniband/hw/hfi1/trace_ctxts.h
@@ -67,9 +67,9 @@ TRACE_EVENT(hfi1_uctxtdata,
 			     __field(u64, hw_free)
 			     __field(void __iomem *, piobase)
 			     __field(u16, rcvhdrq_cnt)
-			     __field(u64, rcvhdrq_phys)
+			     __field(u64, rcvhdrq_dma)
 			     __field(u32, eager_cnt)
-			     __field(u64, rcvegr_phys)
+			     __field(u64, rcvegr_dma)
 			     ),
 	    TP_fast_assign(DD_DEV_ASSIGN(dd);
 			   __entry->ctxt = uctxt->ctxt;
@@ -77,10 +77,9 @@ TRACE_EVENT(hfi1_uctxtdata,
 			   __entry->hw_free = le64_to_cpu(*uctxt->sc->hw_free);
 			   __entry->piobase = uctxt->sc->base_addr;
 			   __entry->rcvhdrq_cnt = uctxt->rcvhdrq_cnt;
-			   __entry->rcvhdrq_phys = uctxt->rcvhdrq_phys;
+			   __entry->rcvhdrq_dma = uctxt->rcvhdrq_dma;
 			   __entry->eager_cnt = uctxt->egrbufs.alloced;
-			   __entry->rcvegr_phys =
-			   uctxt->egrbufs.rcvtids[0].phys;
+			   __entry->rcvegr_dma = uctxt->egrbufs.rcvtids[0].dma;
 			   ),
 	    TP_printk("[%s] ctxt %u " UCTXT_FMT,
 		      __get_str(dev),
@@ -89,9 +88,9 @@ TRACE_EVENT(hfi1_uctxtdata,
 		      __entry->hw_free,
 		      __entry->piobase,
 		      __entry->rcvhdrq_cnt,
-		      __entry->rcvhdrq_phys,
+		      __entry->rcvhdrq_dma,
 		      __entry->eager_cnt,
-		      __entry->rcvegr_phys
+		      __entry->rcvegr_dma
 		      )
 );
 
diff --git a/drivers/infiniband/hw/hfi1/trace_ibhdrs.h b/drivers/infiniband/hw/hfi1/trace_ibhdrs.h
index c3e41aed0034..382fcda3a5f6 100644
--- a/drivers/infiniband/hw/hfi1/trace_ibhdrs.h
+++ b/drivers/infiniband/hw/hfi1/trace_ibhdrs.h
@@ -55,7 +55,7 @@
 #undef TRACE_SYSTEM
 #define TRACE_SYSTEM hfi1_ibhdrs
 
-u8 ibhdr_exhdr_len(struct hfi1_ib_header *hdr);
+u8 ibhdr_exhdr_len(struct ib_header *hdr);
 const char *parse_everbs_hdrs(struct trace_seq *p, u8 opcode, void *ehdrs);
 
 #define __parse_ib_ehdrs(op, ehdrs) parse_everbs_hdrs(p, op, ehdrs)
@@ -74,7 +74,7 @@ __print_symbolic(lrh,                    \
 
 DECLARE_EVENT_CLASS(hfi1_ibhdr_template,
 		    TP_PROTO(struct hfi1_devdata *dd,
-			     struct hfi1_ib_header *hdr),
+			     struct ib_header *hdr),
 		    TP_ARGS(dd, hdr),
 		    TP_STRUCT__entry(
 			DD_DEV_ENTRY(dd)
@@ -102,7 +102,7 @@ DECLARE_EVENT_CLASS(hfi1_ibhdr_template,
 			__dynamic_array(u8, ehdrs, ibhdr_exhdr_len(hdr))
 			),
 		      TP_fast_assign(
-			struct hfi1_other_headers *ohdr;
+			struct ib_other_headers *ohdr;
 
 			DD_DEV_ASSIGN(dd);
 			/* LRH */
@@ -185,19 +185,19 @@ DECLARE_EVENT_CLASS(hfi1_ibhdr_template,
 );
 
 DEFINE_EVENT(hfi1_ibhdr_template, input_ibhdr,
-	     TP_PROTO(struct hfi1_devdata *dd, struct hfi1_ib_header *hdr),
+	     TP_PROTO(struct hfi1_devdata *dd, struct ib_header *hdr),
 	     TP_ARGS(dd, hdr));
 
 DEFINE_EVENT(hfi1_ibhdr_template, pio_output_ibhdr,
-	     TP_PROTO(struct hfi1_devdata *dd, struct hfi1_ib_header *hdr),
+	     TP_PROTO(struct hfi1_devdata *dd, struct ib_header *hdr),
 	     TP_ARGS(dd, hdr));
 
 DEFINE_EVENT(hfi1_ibhdr_template, ack_output_ibhdr,
-	     TP_PROTO(struct hfi1_devdata *dd, struct hfi1_ib_header *hdr),
+	     TP_PROTO(struct hfi1_devdata *dd, struct ib_header *hdr),
 	     TP_ARGS(dd, hdr));
 
 DEFINE_EVENT(hfi1_ibhdr_template, sdma_output_ibhdr,
-	     TP_PROTO(struct hfi1_devdata *dd, struct hfi1_ib_header *hdr),
+	     TP_PROTO(struct hfi1_devdata *dd, struct ib_header *hdr),
 	     TP_ARGS(dd, hdr));
 
 #endif /* __HFI1_TRACE_IBHDRS_H */
diff --git a/drivers/infiniband/hw/hfi1/trace_rx.h b/drivers/infiniband/hw/hfi1/trace_rx.h
index 9ba1f615ec95..11e02b228922 100644
--- a/drivers/infiniband/hw/hfi1/trace_rx.h
+++ b/drivers/infiniband/hw/hfi1/trace_rx.h
@@ -260,7 +260,7 @@ TRACE_EVENT(hfi1_mmu_invalidate,
 TRACE_EVENT(snoop_capture,
 	    TP_PROTO(struct hfi1_devdata *dd,
 		     int hdr_len,
-		     struct hfi1_ib_header *hdr,
+		     struct ib_header *hdr,
 		     int data_len,
 		     void *data),
 	    TP_ARGS(dd, hdr_len, hdr, data_len, data),
@@ -279,7 +279,7 @@ TRACE_EVENT(snoop_capture,
 			     __dynamic_array(u8, raw_pkt, data_len)
 			     ),
 	    TP_fast_assign(
-		struct hfi1_other_headers *ohdr;
+		struct ib_other_headers *ohdr;
 
 		__entry->lnh = (u8)(be16_to_cpu(hdr->lrh[0]) & 3);
 		if (__entry->lnh == HFI1_LRH_BTH)
diff --git a/drivers/infiniband/hw/hfi1/uc.c b/drivers/infiniband/hw/hfi1/uc.c
index a726d96d185f..5e6d1bac4914 100644
--- a/drivers/infiniband/hw/hfi1/uc.c
+++ b/drivers/infiniband/hw/hfi1/uc.c
@@ -50,14 +50,7 @@
 #include "qp.h"
 
 /* cut down ridiculously long IB macro names */
-#define OP(x) IB_OPCODE_UC_##x
-
-/* only opcode mask for adaptive pio */
-const u32 uc_only_opcode =
-	BIT(OP(SEND_ONLY) & 0x1f) |
-	BIT(OP(SEND_ONLY_WITH_IMMEDIATE & 0x1f)) |
-	BIT(OP(RDMA_WRITE_ONLY & 0x1f)) |
-	BIT(OP(RDMA_WRITE_ONLY_WITH_IMMEDIATE & 0x1f));
+#define OP(x) UC_OP(x)
 
 /**
  * hfi1_make_uc_req - construct a request packet (SEND, RDMA write)
@@ -70,7 +63,7 @@ const u32 uc_only_opcode =
 int hfi1_make_uc_req(struct rvt_qp *qp, struct hfi1_pkt_state *ps)
 {
 	struct hfi1_qp_priv *priv = qp->priv;
-	struct hfi1_other_headers *ohdr;
+	struct ib_other_headers *ohdr;
 	struct rvt_swqe *wqe;
 	u32 hwords = 5;
 	u32 bth0 = 0;
@@ -304,12 +297,12 @@ bail_no_tx:
 void hfi1_uc_rcv(struct hfi1_packet *packet)
 {
 	struct hfi1_ibport *ibp = &packet->rcd->ppd->ibport_data;
-	struct hfi1_ib_header *hdr = packet->hdr;
+	struct ib_header *hdr = packet->hdr;
 	u32 rcv_flags = packet->rcv_flags;
 	void *data = packet->ebuf;
 	u32 tlen = packet->tlen;
 	struct rvt_qp *qp = packet->qp;
-	struct hfi1_other_headers *ohdr = packet->ohdr;
+	struct ib_other_headers *ohdr = packet->ohdr;
 	u32 bth0, opcode;
 	u32 hdrsize = packet->hlen;
 	u32 psn;
diff --git a/drivers/infiniband/hw/hfi1/ud.c b/drivers/infiniband/hw/hfi1/ud.c
index f01e8e1d62d3..97ae24b6314c 100644
--- a/drivers/infiniband/hw/hfi1/ud.c
+++ b/drivers/infiniband/hw/hfi1/ud.c
@@ -271,7 +271,7 @@ drop:
 int hfi1_make_ud_req(struct rvt_qp *qp, struct hfi1_pkt_state *ps)
 {
 	struct hfi1_qp_priv *priv = qp->priv;
-	struct hfi1_other_headers *ohdr;
+	struct ib_other_headers *ohdr;
 	struct ib_ah_attr *ah_attr;
 	struct hfi1_pportdata *ppd;
 	struct hfi1_ibport *ibp;
@@ -510,8 +510,8 @@ void return_cnp(struct hfi1_ibport *ibp, struct rvt_qp *qp, u32 remote_qpn,
 	u32 bth0, plen, vl, hwords = 5;
 	u16 lrh0;
 	u8 sl = ibp->sc_to_sl[sc5];
-	struct hfi1_ib_header hdr;
-	struct hfi1_other_headers *ohdr;
+	struct ib_header hdr;
+	struct ib_other_headers *ohdr;
 	struct pio_buf *pbuf;
 	struct send_context *ctxt = qp_to_send_context(qp, sc5);
 	struct hfi1_pportdata *ppd = ppd_from_ibp(ibp);
@@ -559,8 +559,8 @@ void return_cnp(struct hfi1_ibport *ibp, struct rvt_qp *qp, u32 remote_qpn,
 
 /*
  * opa_smp_check() - Do the regular pkey checking, and the additional
- * checks for SMPs specified in OPAv1 rev 0.90, section 9.10.26
- * ("SMA Packet Checks").
+ * checks for SMPs specified in OPAv1 rev 1.0, 9/19/2016 update, section
+ * 9.10.25 ("SMA Packet Checks").
  *
  * Note that:
  *   - Checks are done using the pkey directly from the packet's BTH,
@@ -603,23 +603,28 @@ static int opa_smp_check(struct hfi1_ibport *ibp, u16 pkey, u8 sc5,
 
 	/*
 	 * SMPs fall into one of four (disjoint) categories:
-	 * SMA request, SMA response, trap, or trap repress.
-	 * Our response depends, in part, on which type of
-	 * SMP we're processing.
+	 * SMA request, SMA response, SMA trap, or SMA trap repress.
+	 * Our response depends, in part, on which type of SMP we're
+	 * processing.
 	 *
-	 * If this is not an SMA request, or trap repress:
-	 *   - accept MAD if the port is running an SM
-	 *   - pkey == FULL_MGMT_P_KEY =>
-	 *       reply with unsupported method (i.e., just mark
-	 *       the smp's status field here, and let it be
-	 *       processed normally)
-	 *   - pkey != LIM_MGMT_P_KEY =>
-	 *       increment port recv constraint errors, drop MAD
-	 * If this is an SMA request or trap repress:
+	 * If this is an SMA response, skip the check here.
+	 *
+	 * If this is an SMA request or SMA trap repress:
 	 *   - pkey != FULL_MGMT_P_KEY =>
 	 *       increment port recv constraint errors, drop MAD
+	 *
+	 * Otherwise:
+	 *    - accept if the port is running an SM
+	 *    - drop MAD if it's an SMA trap
+	 *    - pkey == FULL_MGMT_P_KEY =>
+	 *        reply with unsupported method
+	 *    - pkey != FULL_MGMT_P_KEY =>
+	 *	  increment port recv constraint errors, drop MAD
 	 */
 	switch (smp->method) {
+	case IB_MGMT_METHOD_GET_RESP:
+	case IB_MGMT_METHOD_REPORT_RESP:
+		break;
 	case IB_MGMT_METHOD_GET:
 	case IB_MGMT_METHOD_SET:
 	case IB_MGMT_METHOD_REPORT:
@@ -629,23 +634,17 @@ static int opa_smp_check(struct hfi1_ibport *ibp, u16 pkey, u8 sc5,
 			return 1;
 		}
 		break;
-	case IB_MGMT_METHOD_SEND:
-	case IB_MGMT_METHOD_TRAP:
-	case IB_MGMT_METHOD_GET_RESP:
-	case IB_MGMT_METHOD_REPORT_RESP:
+	default:
 		if (ibp->rvp.port_cap_flags & IB_PORT_SM)
 			return 0;
+		if (smp->method == IB_MGMT_METHOD_TRAP)
+			return 1;
 		if (pkey == FULL_MGMT_P_KEY) {
 			smp->status |= IB_SMP_UNSUP_METHOD;
 			return 0;
 		}
-		if (pkey != LIM_MGMT_P_KEY) {
-			ingress_pkey_table_fail(ppd, pkey, slid);
-			return 1;
-		}
-		break;
-	default:
-		break;
+		ingress_pkey_table_fail(ppd, pkey, slid);
+		return 1;
 	}
 	return 0;
 }
@@ -665,7 +664,7 @@ static int opa_smp_check(struct hfi1_ibport *ibp, u16 pkey, u8 sc5,
  */
 void hfi1_ud_rcv(struct hfi1_packet *packet)
 {
-	struct hfi1_other_headers *ohdr = packet->ohdr;
+	struct ib_other_headers *ohdr = packet->ohdr;
 	int opcode;
 	u32 hdrsize = packet->hlen;
 	struct ib_wc wc;
@@ -675,13 +674,13 @@ void hfi1_ud_rcv(struct hfi1_packet *packet)
 	int mgmt_pkey_idx = -1;
 	struct hfi1_ibport *ibp = &packet->rcd->ppd->ibport_data;
 	struct hfi1_pportdata *ppd = ppd_from_ibp(ibp);
-	struct hfi1_ib_header *hdr = packet->hdr;
+	struct ib_header *hdr = packet->hdr;
 	u32 rcv_flags = packet->rcv_flags;
 	void *data = packet->ebuf;
 	u32 tlen = packet->tlen;
 	struct rvt_qp *qp = packet->qp;
 	bool has_grh = rcv_flags & HFI1_HAS_GRH;
-	u8 sc5 = hdr2sc((struct hfi1_message_header *)hdr, packet->rhf);
+	u8 sc5 = hdr2sc(hdr, packet->rhf);
 	u32 bth1;
 	u8 sl_from_sc, sl;
 	u16 slid;
diff --git a/drivers/infiniband/hw/hfi1/user_sdma.c b/drivers/infiniband/hw/hfi1/user_sdma.c
index 1694037d1eee..a761f804111e 100644
--- a/drivers/infiniband/hw/hfi1/user_sdma.c
+++ b/drivers/infiniband/hw/hfi1/user_sdma.c
@@ -548,7 +548,7 @@ int hfi1_user_sdma_process_request(struct file *fp, struct iovec *iovec,
 	u8 opcode, sc, vl;
 	int req_queued = 0;
 	u16 dlid;
-	u8 selector;
+	u32 selector;
 
 	if (iovec[idx].iov_len < sizeof(info) + sizeof(req->hdr)) {
 		hfi1_cdbg(
@@ -753,12 +753,9 @@ int hfi1_user_sdma_process_request(struct file *fp, struct iovec *iovec,
 
 	dlid = be16_to_cpu(req->hdr.lrh[1]);
 	selector = dlid_to_selector(dlid);
+	selector += uctxt->ctxt + fd->subctxt;
+	req->sde = sdma_select_user_engine(dd, selector, vl);
 
-	/* Have to select the engine */
-	req->sde = sdma_select_engine_vl(dd,
-					 (u32)(uctxt->ctxt + fd->subctxt +
-					       selector),
-					 vl);
 	if (!req->sde || !sdma_running(req->sde)) {
 		ret = -ECOMM;
 		goto free_req;
@@ -894,7 +891,7 @@ static inline u32 get_lrh_len(struct hfi1_pkt_header hdr, u32 len)
 
 static int user_sdma_send_pkts(struct user_sdma_request *req, unsigned maxpkts)
 {
-	int ret = 0;
+	int ret = 0, count;
 	unsigned npkts = 0;
 	struct user_sdma_txreq *tx = NULL;
 	struct hfi1_user_sdma_pkt_q *pq = NULL;
@@ -1090,23 +1087,18 @@ static int user_sdma_send_pkts(struct user_sdma_request *req, unsigned maxpkts)
 		npkts++;
 	}
 dosend:
-	ret = sdma_send_txlist(req->sde, &pq->busy, &req->txps);
-	if (list_empty(&req->txps)) {
-		req->seqsubmitted = req->seqnum;
-		if (req->seqnum == req->info.npkts) {
-			set_bit(SDMA_REQ_SEND_DONE, &req->flags);
-			/*
-			 * The txreq has already been submitted to the HW queue
-			 * so we can free the AHG entry now. Corruption will not
-			 * happen due to the sequential manner in which
-			 * descriptors are processed.
-			 */
-			if (test_bit(SDMA_REQ_HAVE_AHG, &req->flags))
-				sdma_ahg_free(req->sde, req->ahg_idx);
-		}
-	} else if (ret > 0) {
-		req->seqsubmitted += ret;
-		ret = 0;
+	ret = sdma_send_txlist(req->sde, &pq->busy, &req->txps, &count);
+	req->seqsubmitted += count;
+	if (req->seqsubmitted == req->info.npkts) {
+		set_bit(SDMA_REQ_SEND_DONE, &req->flags);
+		/*
+		 * The txreq has already been submitted to the HW queue
+		 * so we can free the AHG entry now. Corruption will not
+		 * happen due to the sequential manner in which
+		 * descriptors are processed.
+		 */
+		if (test_bit(SDMA_REQ_HAVE_AHG, &req->flags))
+			sdma_ahg_free(req->sde, req->ahg_idx);
 	}
 	return ret;
 
diff --git a/drivers/infiniband/hw/hfi1/verbs.c b/drivers/infiniband/hw/hfi1/verbs.c
index 2b359540901d..4b7a16ceb362 100644
--- a/drivers/infiniband/hw/hfi1/verbs.c
+++ b/drivers/infiniband/hw/hfi1/verbs.c
@@ -76,7 +76,7 @@ static unsigned int hfi1_max_ahs = 0xFFFF;
 module_param_named(max_ahs, hfi1_max_ahs, uint, S_IRUGO);
 MODULE_PARM_DESC(max_ahs, "Maximum number of address handles to support");
 
-unsigned int hfi1_max_cqes = 0x2FFFF;
+unsigned int hfi1_max_cqes = 0x2FFFFF;
 module_param_named(max_cqes, hfi1_max_cqes, uint, S_IRUGO);
 MODULE_PARM_DESC(max_cqes,
 		 "Maximum number of completion queue entries to support");
@@ -89,7 +89,7 @@ unsigned int hfi1_max_qp_wrs = 0x3FFF;
 module_param_named(max_qp_wrs, hfi1_max_qp_wrs, uint, S_IRUGO);
 MODULE_PARM_DESC(max_qp_wrs, "Maximum number of QP WRs to support");
 
-unsigned int hfi1_max_qps = 16384;
+unsigned int hfi1_max_qps = 32768;
 module_param_named(max_qps, hfi1_max_qps, uint, S_IRUGO);
 MODULE_PARM_DESC(max_qps, "Maximum number of QPs to support");
 
@@ -335,7 +335,7 @@ const u8 hdr_len_by_opcode[256] = {
 	[IB_OPCODE_RC_RDMA_READ_RESPONSE_LAST]        = 12 + 8 + 4,
 	[IB_OPCODE_RC_RDMA_READ_RESPONSE_ONLY]        = 12 + 8 + 4,
 	[IB_OPCODE_RC_ACKNOWLEDGE]                    = 12 + 8 + 4,
-	[IB_OPCODE_RC_ATOMIC_ACKNOWLEDGE]             = 12 + 8 + 4,
+	[IB_OPCODE_RC_ATOMIC_ACKNOWLEDGE]             = 12 + 8 + 4 + 8,
 	[IB_OPCODE_RC_COMPARE_SWAP]                   = 12 + 8 + 28,
 	[IB_OPCODE_RC_FETCH_ADD]                      = 12 + 8 + 28,
 	[IB_OPCODE_RC_SEND_LAST_WITH_INVALIDATE]      = 12 + 8 + 4,
@@ -403,6 +403,28 @@ static const opcode_handler opcode_handler_tbl[256] = {
 	[IB_OPCODE_CNP]				      = &hfi1_cnp_rcv
 };
 
+#define OPMASK 0x1f
+
+static const u32 pio_opmask[BIT(3)] = {
+	/* RC */
+	[IB_OPCODE_RC >> 5] =
+		BIT(RC_OP(SEND_ONLY) & OPMASK) |
+		BIT(RC_OP(SEND_ONLY_WITH_IMMEDIATE) & OPMASK) |
+		BIT(RC_OP(RDMA_WRITE_ONLY) & OPMASK) |
+		BIT(RC_OP(RDMA_WRITE_ONLY_WITH_IMMEDIATE) & OPMASK) |
+		BIT(RC_OP(RDMA_READ_REQUEST) & OPMASK) |
+		BIT(RC_OP(ACKNOWLEDGE) & OPMASK) |
+		BIT(RC_OP(ATOMIC_ACKNOWLEDGE) & OPMASK) |
+		BIT(RC_OP(COMPARE_SWAP) & OPMASK) |
+		BIT(RC_OP(FETCH_ADD) & OPMASK),
+	/* UC */
+	[IB_OPCODE_UC >> 5] =
+		BIT(UC_OP(SEND_ONLY) & OPMASK) |
+		BIT(UC_OP(SEND_ONLY_WITH_IMMEDIATE) & OPMASK) |
+		BIT(UC_OP(RDMA_WRITE_ONLY) & OPMASK) |
+		BIT(UC_OP(RDMA_WRITE_ONLY_WITH_IMMEDIATE) & OPMASK),
+};
+
 /*
  * System image GUID.
  */
@@ -567,7 +589,7 @@ static inline opcode_handler qp_ok(int opcode, struct hfi1_packet *packet)
 void hfi1_ib_rcv(struct hfi1_packet *packet)
 {
 	struct hfi1_ctxtdata *rcd = packet->rcd;
-	struct hfi1_ib_header *hdr = packet->hdr;
+	struct ib_header *hdr = packet->hdr;
 	u32 tlen = packet->tlen;
 	struct hfi1_pportdata *ppd = rcd->ppd;
 	struct hfi1_ibport *ibp = &ppd->ibport_data;
@@ -719,7 +741,7 @@ static void verbs_sdma_complete(
 	if (tx->wqe) {
 		hfi1_send_complete(qp, tx->wqe, IB_WC_SUCCESS);
 	} else if (qp->ibqp.qp_type == IB_QPT_RC) {
-		struct hfi1_ib_header *hdr;
+		struct ib_header *hdr;
 
 		hdr = &tx->phdr.hdr;
 		hfi1_rc_send_complete(qp, hdr);
@@ -748,7 +770,7 @@ static int wait_kmem(struct hfi1_ibdev *dev,
 			qp->s_flags |= RVT_S_WAIT_KMEM;
 			list_add_tail(&priv->s_iowait.list, &dev->memwait);
 			trace_hfi1_qpsleep(qp, RVT_S_WAIT_KMEM);
-			atomic_inc(&qp->refcount);
+			rvt_get_qp(qp);
 		}
 		write_sequnlock(&dev->iowait_lock);
 		qp->s_flags &= ~RVT_S_BUSY;
@@ -959,7 +981,7 @@ static int pio_wait(struct rvt_qp *qp,
 			was_empty = list_empty(&sc->piowait);
 			list_add_tail(&priv->s_iowait.list, &sc->piowait);
 			trace_hfi1_qpsleep(qp, RVT_S_WAIT_PIO);
-			atomic_inc(&qp->refcount);
+			rvt_get_qp(qp);
 			/* counting: only call wantpiobuf_intr if first user */
 			if (was_empty)
 				hfi1_sc_wantpiobuf_intr(sc, 1);
@@ -1200,7 +1222,7 @@ static inline send_routine get_send_routine(struct rvt_qp *qp,
 {
 	struct hfi1_devdata *dd = dd_from_ibdev(qp->ibqp.device);
 	struct hfi1_qp_priv *priv = qp->priv;
-	struct hfi1_ib_header *h = &tx->phdr.hdr;
+	struct ib_header *h = &tx->phdr.hdr;
 
 	if (unlikely(!(dd->flags & HFI1_HAS_SEND_DMA)))
 		return dd->process_pio_send;
@@ -1210,22 +1232,18 @@ static inline send_routine get_send_routine(struct rvt_qp *qp,
 	case IB_QPT_GSI:
 	case IB_QPT_UD:
 		break;
-	case IB_QPT_RC:
-		if (piothreshold &&
-		    qp->s_cur_size <= min(piothreshold, qp->pmtu) &&
-		    (BIT(get_opcode(h) & 0x1f) & rc_only_opcode) &&
-		    iowait_sdma_pending(&priv->s_iowait) == 0 &&
-		    !sdma_txreq_built(&tx->txreq))
-			return dd->process_pio_send;
-		break;
 	case IB_QPT_UC:
+	case IB_QPT_RC: {
+		u8 op = get_opcode(h);
+
 		if (piothreshold &&
 		    qp->s_cur_size <= min(piothreshold, qp->pmtu) &&
-		    (BIT(get_opcode(h) & 0x1f) & uc_only_opcode) &&
+		    (BIT(op & OPMASK) & pio_opmask[op >> 5]) &&
 		    iowait_sdma_pending(&priv->s_iowait) == 0 &&
 		    !sdma_txreq_built(&tx->txreq))
 			return dd->process_pio_send;
 		break;
+	}
 	default:
 		break;
 	}
@@ -1244,8 +1262,8 @@ int hfi1_verbs_send(struct rvt_qp *qp, struct hfi1_pkt_state *ps)
 {
 	struct hfi1_devdata *dd = dd_from_ibdev(qp->ibqp.device);
 	struct hfi1_qp_priv *priv = qp->priv;
-	struct hfi1_other_headers *ohdr;
-	struct hfi1_ib_header *hdr;
+	struct ib_other_headers *ohdr;
+	struct ib_header *hdr;
 	send_routine sr;
 	int ret;
 	u8 lnh;
@@ -1423,7 +1441,8 @@ static int modify_device(struct ib_device *device,
 	}
 
 	if (device_modify_mask & IB_DEVICE_MODIFY_NODE_DESC) {
-		memcpy(device->node_desc, device_modify->node_desc, 64);
+		memcpy(device->node_desc, device_modify->node_desc,
+		       IB_DEVICE_NODE_DESC_MAX);
 		for (i = 0; i < dd->num_pports; i++) {
 			struct hfi1_ibport *ibp = &dd->pport[i].ibport_data;
 
@@ -1754,7 +1773,7 @@ void hfi1_cnp_rcv(struct hfi1_packet *packet)
 {
 	struct hfi1_ibport *ibp = &packet->rcd->ppd->ibport_data;
 	struct hfi1_pportdata *ppd = ppd_from_ibp(ibp);
-	struct hfi1_ib_header *hdr = packet->hdr;
+	struct ib_header *hdr = packet->hdr;
 	struct rvt_qp *qp = packet->qp;
 	u32 lqpn, rqpn = 0;
 	u16 rlid = 0;
@@ -1781,7 +1800,7 @@ void hfi1_cnp_rcv(struct hfi1_packet *packet)
 		return;
 	}
 
-	sc5 = hdr2sc((struct hfi1_message_header *)hdr, packet->rhf);
+	sc5 = hdr2sc(hdr, packet->rhf);
 	sl = ibp->sc_to_sl[sc5];
 	lqpn = qp->ibqp.qp_num;
 
diff --git a/drivers/infiniband/hw/hfi1/verbs.h b/drivers/infiniband/hw/hfi1/verbs.h
index d1b101c54828..1c3815d89eb7 100644
--- a/drivers/infiniband/hw/hfi1/verbs.h
+++ b/drivers/infiniband/hw/hfi1/verbs.h
@@ -60,6 +60,7 @@
 #include <rdma/ib_pack.h>
 #include <rdma/ib_user_verbs.h>
 #include <rdma/ib_mad.h>
+#include <rdma/ib_hdrs.h>
 #include <rdma/rdma_vt.h>
 #include <rdma/rdmavt_qp.h>
 #include <rdma/rdmavt_cq.h>
@@ -80,16 +81,6 @@ struct hfi1_packet;
  */
 #define HFI1_UVERBS_ABI_VERSION       2
 
-#define IB_SEQ_NAK	(3 << 29)
-
-/* AETH NAK opcode values */
-#define IB_RNR_NAK                      0x20
-#define IB_NAK_PSN_ERROR                0x60
-#define IB_NAK_INVALID_REQUEST          0x61
-#define IB_NAK_REMOTE_ACCESS_ERROR      0x62
-#define IB_NAK_REMOTE_OPERATIONAL_ERROR 0x63
-#define IB_NAK_INVALID_RD_REQUEST       0x64
-
 /* IB Performance Manager status values */
 #define IB_PMA_SAMPLE_STATUS_DONE       0x00
 #define IB_PMA_SAMPLE_STATUS_STARTED    0x01
@@ -104,80 +95,16 @@ struct hfi1_packet;
 
 #define HFI1_VENDOR_IPG		cpu_to_be16(0xFFA0)
 
-#define IB_BTH_REQ_ACK		BIT(31)
-#define IB_BTH_SOLICITED	BIT(23)
-#define IB_BTH_MIG_REQ		BIT(22)
-
-#define IB_GRH_VERSION		6
-#define IB_GRH_VERSION_MASK	0xF
-#define IB_GRH_VERSION_SHIFT	28
-#define IB_GRH_TCLASS_MASK	0xFF
-#define IB_GRH_TCLASS_SHIFT	20
-#define IB_GRH_FLOW_MASK	0xFFFFF
-#define IB_GRH_FLOW_SHIFT	0
-#define IB_GRH_NEXT_HDR		0x1B
-
 #define IB_DEFAULT_GID_PREFIX	cpu_to_be64(0xfe80000000000000ULL)
 
+#define RC_OP(x) IB_OPCODE_RC_##x
+#define UC_OP(x) IB_OPCODE_UC_##x
+
 /* flags passed by hfi1_ib_rcv() */
 enum {
 	HFI1_HAS_GRH = (1 << 0),
 };
 
-struct ib_reth {
-	__be64 vaddr;
-	__be32 rkey;
-	__be32 length;
-} __packed;
-
-struct ib_atomic_eth {
-	__be32 vaddr[2];        /* unaligned so access as 2 32-bit words */
-	__be32 rkey;
-	__be64 swap_data;
-	__be64 compare_data;
-} __packed;
-
-union ib_ehdrs {
-	struct {
-		__be32 deth[2];
-		__be32 imm_data;
-	} ud;
-	struct {
-		struct ib_reth reth;
-		__be32 imm_data;
-	} rc;
-	struct {
-		__be32 aeth;
-		__be32 atomic_ack_eth[2];
-	} at;
-	__be32 imm_data;
-	__be32 aeth;
-	__be32 ieth;
-	struct ib_atomic_eth atomic_eth;
-}  __packed;
-
-struct hfi1_other_headers {
-	__be32 bth[3];
-	union ib_ehdrs u;
-} __packed;
-
-/*
- * Note that UD packets with a GRH header are 8+40+12+8 = 68 bytes
- * long (72 w/ imm_data).  Only the first 56 bytes of the IB header
- * will be in the eager header buffer.  The remaining 12 or 16 bytes
- * are in the data buffer.
- */
-struct hfi1_ib_header {
-	__be16 lrh[4];
-	union {
-		struct {
-			struct ib_grh grh;
-			struct hfi1_other_headers oth;
-		} l;
-		struct hfi1_other_headers oth;
-	} u;
-} __packed;
-
 struct hfi1_ahg_info {
 	u32 ahgdesc[2];
 	u16 tx_flags;
@@ -187,7 +114,7 @@ struct hfi1_ahg_info {
 
 struct hfi1_sdma_header {
 	__le64 pbc;
-	struct hfi1_ib_header hdr;
+	struct ib_header hdr;
 } __packed;
 
 /*
@@ -386,7 +313,7 @@ void hfi1_rc_rcv(struct hfi1_packet *packet);
 
 void hfi1_rc_hdrerr(
 	struct hfi1_ctxtdata *rcd,
-	struct hfi1_ib_header *hdr,
+	struct ib_header *hdr,
 	u32 rcv_flags,
 	struct rvt_qp *qp);
 
@@ -400,7 +327,7 @@ void hfi1_rc_timeout(unsigned long arg);
 void hfi1_del_timers_sync(struct rvt_qp *qp);
 void hfi1_stop_rc_timers(struct rvt_qp *qp);
 
-void hfi1_rc_send_complete(struct rvt_qp *qp, struct hfi1_ib_header *hdr);
+void hfi1_rc_send_complete(struct rvt_qp *qp, struct ib_header *hdr);
 
 void hfi1_rc_error(struct rvt_qp *qp, enum ib_wc_status err);
 
@@ -423,7 +350,7 @@ int hfi1_check_send_wqe(struct rvt_qp *qp, struct rvt_swqe *wqe);
 extern const u32 rc_only_opcode;
 extern const u32 uc_only_opcode;
 
-static inline u8 get_opcode(struct hfi1_ib_header *h)
+static inline u8 get_opcode(struct ib_header *h)
 {
 	u16 lnh = be16_to_cpu(h->lrh[0]) & 3;
 
@@ -433,13 +360,13 @@ static inline u8 get_opcode(struct hfi1_ib_header *h)
 		return be32_to_cpu(h->u.l.oth.bth[0]) >> 24;
 }
 
-int hfi1_ruc_check_hdr(struct hfi1_ibport *ibp, struct hfi1_ib_header *hdr,
+int hfi1_ruc_check_hdr(struct hfi1_ibport *ibp, struct ib_header *hdr,
 		       int has_grh, struct rvt_qp *qp, u32 bth0);
 
 u32 hfi1_make_grh(struct hfi1_ibport *ibp, struct ib_grh *hdr,
 		  struct ib_global_route *grh, u32 hwords, u32 nwords);
 
-void hfi1_make_ruc_header(struct rvt_qp *qp, struct hfi1_other_headers *ohdr,
+void hfi1_make_ruc_header(struct rvt_qp *qp, struct ib_other_headers *ohdr,
 			  u32 bth0, u32 bth2, int middle,
 			  struct hfi1_pkt_state *ps);
 
diff --git a/drivers/infiniband/hw/hfi1/verbs_txreq.c b/drivers/infiniband/hw/hfi1/verbs_txreq.c
index d8fb056526f8..094ab829ec42 100644
--- a/drivers/infiniband/hw/hfi1/verbs_txreq.c
+++ b/drivers/infiniband/hw/hfi1/verbs_txreq.c
@@ -109,7 +109,7 @@ struct verbs_txreq *__get_txreq(struct hfi1_ibdev *dev,
 			qp->s_flags |= RVT_S_WAIT_TX;
 			list_add_tail(&priv->s_iowait.list, &dev->txwait);
 			trace_hfi1_qpsleep(qp, RVT_S_WAIT_TX);
-			atomic_inc(&qp->refcount);
+			rvt_get_qp(qp);
 		}
 		qp->s_flags &= ~RVT_S_BUSY;
 	}
diff --git a/drivers/infiniband/hw/hns/Kconfig b/drivers/infiniband/hw/hns/Kconfig
new file mode 100644
index 000000000000..e1a6e055cd60
--- /dev/null
+++ b/drivers/infiniband/hw/hns/Kconfig
@@ -0,0 +1,10 @@
+config INFINIBAND_HNS
+	tristate "HNS RoCE Driver"
+	depends on NET_VENDOR_HISILICON
+	depends on ARM64 && HNS && HNS_DSAF && HNS_ENET
+	---help---
+	  This is a RoCE/RDMA driver for the Hisilicon RoCE engine. The engine
+	  is used in Hisilicon Hi1610 and more further ICT SoC.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called hns-roce.
diff --git a/drivers/infiniband/hw/hns/Makefile b/drivers/infiniband/hw/hns/Makefile
new file mode 100644
index 000000000000..7e8ebd24dcae
--- /dev/null
+++ b/drivers/infiniband/hw/hns/Makefile
@@ -0,0 +1,8 @@
+#
+# Makefile for the Hisilicon RoCE drivers.
+#
+
+obj-$(CONFIG_INFINIBAND_HNS) += hns-roce.o
+hns-roce-objs := hns_roce_main.o hns_roce_cmd.o hns_roce_eq.o hns_roce_pd.o \
+	hns_roce_ah.o hns_roce_hem.o hns_roce_mr.o hns_roce_qp.o \
+	hns_roce_cq.o hns_roce_alloc.o hns_roce_hw_v1.o
diff --git a/drivers/infiniband/hw/hns/hns_roce_ah.c b/drivers/infiniband/hw/hns/hns_roce_ah.c
new file mode 100644
index 000000000000..24f79ee39fdf
--- /dev/null
+++ b/drivers/infiniband/hw/hns/hns_roce_ah.c
@@ -0,0 +1,128 @@
+/*
+ * Copyright (c) 2016 Hisilicon Limited.
+ *
+ * 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/platform_device.h>
+#include <rdma/ib_addr.h>
+#include <rdma/ib_cache.h>
+#include "hns_roce_device.h"
+
+#define HNS_ROCE_PORT_NUM_SHIFT		24
+#define HNS_ROCE_VLAN_SL_BIT_MASK	7
+#define HNS_ROCE_VLAN_SL_SHIFT		13
+
+struct ib_ah *hns_roce_create_ah(struct ib_pd *ibpd, struct ib_ah_attr *ah_attr)
+{
+	struct hns_roce_dev *hr_dev = to_hr_dev(ibpd->device);
+	struct device *dev = &hr_dev->pdev->dev;
+	struct ib_gid_attr gid_attr;
+	struct hns_roce_ah *ah;
+	u16 vlan_tag = 0xffff;
+	struct in6_addr in6;
+	union ib_gid sgid;
+	int ret;
+
+	ah = kzalloc(sizeof(*ah), GFP_ATOMIC);
+	if (!ah)
+		return ERR_PTR(-ENOMEM);
+
+	/* Get mac address */
+	memcpy(&in6, ah_attr->grh.dgid.raw, sizeof(ah_attr->grh.dgid.raw));
+	if (rdma_is_multicast_addr(&in6))
+		rdma_get_mcast_mac(&in6, ah->av.mac);
+	else
+		memcpy(ah->av.mac, ah_attr->dmac, sizeof(ah_attr->dmac));
+
+	/* Get source gid */
+	ret = ib_get_cached_gid(ibpd->device, ah_attr->port_num,
+				ah_attr->grh.sgid_index, &sgid, &gid_attr);
+	if (ret) {
+		dev_err(dev, "get sgid failed! ret = %d\n", ret);
+		kfree(ah);
+		return ERR_PTR(ret);
+	}
+
+	if (gid_attr.ndev) {
+		if (is_vlan_dev(gid_attr.ndev))
+			vlan_tag = vlan_dev_vlan_id(gid_attr.ndev);
+		dev_put(gid_attr.ndev);
+	}
+
+	if (vlan_tag < 0x1000)
+		vlan_tag |= (ah_attr->sl & HNS_ROCE_VLAN_SL_BIT_MASK) <<
+			     HNS_ROCE_VLAN_SL_SHIFT;
+
+	ah->av.port_pd = cpu_to_be32(to_hr_pd(ibpd)->pdn | (ah_attr->port_num <<
+				     HNS_ROCE_PORT_NUM_SHIFT));
+	ah->av.gid_index = ah_attr->grh.sgid_index;
+	ah->av.vlan = cpu_to_le16(vlan_tag);
+	dev_dbg(dev, "gid_index = 0x%x,vlan = 0x%x\n", ah->av.gid_index,
+		ah->av.vlan);
+
+	if (ah_attr->static_rate)
+		ah->av.stat_rate = IB_RATE_10_GBPS;
+
+	memcpy(ah->av.dgid, ah_attr->grh.dgid.raw, HNS_ROCE_GID_SIZE);
+	ah->av.sl_tclass_flowlabel = cpu_to_le32(ah_attr->sl <<
+						 HNS_ROCE_SL_SHIFT);
+
+	return &ah->ibah;
+}
+
+int hns_roce_query_ah(struct ib_ah *ibah, struct ib_ah_attr *ah_attr)
+{
+	struct hns_roce_ah *ah = to_hr_ah(ibah);
+
+	memset(ah_attr, 0, sizeof(*ah_attr));
+
+	ah_attr->sl = le32_to_cpu(ah->av.sl_tclass_flowlabel) >>
+				  HNS_ROCE_SL_SHIFT;
+	ah_attr->port_num = le32_to_cpu(ah->av.port_pd) >>
+					HNS_ROCE_PORT_NUM_SHIFT;
+	ah_attr->static_rate = ah->av.stat_rate;
+	ah_attr->ah_flags = IB_AH_GRH;
+	ah_attr->grh.traffic_class = le32_to_cpu(ah->av.sl_tclass_flowlabel) >>
+						 HNS_ROCE_TCLASS_SHIFT;
+	ah_attr->grh.flow_label = le32_to_cpu(ah->av.sl_tclass_flowlabel) &
+					      HNS_ROCE_FLOW_LABLE_MASK;
+	ah_attr->grh.hop_limit = ah->av.hop_limit;
+	ah_attr->grh.sgid_index = ah->av.gid_index;
+	memcpy(ah_attr->grh.dgid.raw, ah->av.dgid, HNS_ROCE_GID_SIZE);
+
+	return 0;
+}
+
+int hns_roce_destroy_ah(struct ib_ah *ah)
+{
+	kfree(to_hr_ah(ah));
+
+	return 0;
+}
diff --git a/drivers/infiniband/hw/hns/hns_roce_alloc.c b/drivers/infiniband/hw/hns/hns_roce_alloc.c
new file mode 100644
index 000000000000..863a17a2de40
--- /dev/null
+++ b/drivers/infiniband/hw/hns/hns_roce_alloc.c
@@ -0,0 +1,257 @@
+/*
+ * Copyright (c) 2016 Hisilicon Limited.
+ * Copyright (c) 2007, 2008 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/platform_device.h>
+#include "hns_roce_device.h"
+
+int hns_roce_bitmap_alloc(struct hns_roce_bitmap *bitmap, unsigned long *obj)
+{
+	int ret = 0;
+
+	spin_lock(&bitmap->lock);
+	*obj = find_next_zero_bit(bitmap->table, bitmap->max, bitmap->last);
+	if (*obj >= bitmap->max) {
+		bitmap->top = (bitmap->top + bitmap->max + bitmap->reserved_top)
+			       & bitmap->mask;
+		*obj = find_first_zero_bit(bitmap->table, bitmap->max);
+	}
+
+	if (*obj < bitmap->max) {
+		set_bit(*obj, bitmap->table);
+		bitmap->last = (*obj + 1);
+		if (bitmap->last == bitmap->max)
+			bitmap->last = 0;
+		*obj |= bitmap->top;
+	} else {
+		ret = -1;
+	}
+
+	spin_unlock(&bitmap->lock);
+
+	return ret;
+}
+
+void hns_roce_bitmap_free(struct hns_roce_bitmap *bitmap, unsigned long obj)
+{
+	hns_roce_bitmap_free_range(bitmap, obj, 1);
+}
+
+int hns_roce_bitmap_alloc_range(struct hns_roce_bitmap *bitmap, int cnt,
+				int align, unsigned long *obj)
+{
+	int ret = 0;
+	int i;
+
+	if (likely(cnt == 1 && align == 1))
+		return hns_roce_bitmap_alloc(bitmap, obj);
+
+	spin_lock(&bitmap->lock);
+
+	*obj = bitmap_find_next_zero_area(bitmap->table, bitmap->max,
+					  bitmap->last, cnt, align - 1);
+	if (*obj >= bitmap->max) {
+		bitmap->top = (bitmap->top + bitmap->max + bitmap->reserved_top)
+			       & bitmap->mask;
+		*obj = bitmap_find_next_zero_area(bitmap->table, bitmap->max, 0,
+						  cnt, align - 1);
+	}
+
+	if (*obj < bitmap->max) {
+		for (i = 0; i < cnt; i++)
+			set_bit(*obj + i, bitmap->table);
+
+		if (*obj == bitmap->last) {
+			bitmap->last = (*obj + cnt);
+			if (bitmap->last >= bitmap->max)
+				bitmap->last = 0;
+		}
+		*obj |= bitmap->top;
+	} else {
+		ret = -1;
+	}
+
+	spin_unlock(&bitmap->lock);
+
+	return ret;
+}
+
+void hns_roce_bitmap_free_range(struct hns_roce_bitmap *bitmap,
+				unsigned long obj, int cnt)
+{
+	int i;
+
+	obj &= bitmap->max + bitmap->reserved_top - 1;
+
+	spin_lock(&bitmap->lock);
+	for (i = 0; i < cnt; i++)
+		clear_bit(obj + i, bitmap->table);
+
+	bitmap->last = min(bitmap->last, obj);
+	bitmap->top = (bitmap->top + bitmap->max + bitmap->reserved_top)
+		       & bitmap->mask;
+	spin_unlock(&bitmap->lock);
+}
+
+int hns_roce_bitmap_init(struct hns_roce_bitmap *bitmap, u32 num, u32 mask,
+			 u32 reserved_bot, u32 reserved_top)
+{
+	u32 i;
+
+	if (num != roundup_pow_of_two(num))
+		return -EINVAL;
+
+	bitmap->last = 0;
+	bitmap->top = 0;
+	bitmap->max = num - reserved_top;
+	bitmap->mask = mask;
+	bitmap->reserved_top = reserved_top;
+	spin_lock_init(&bitmap->lock);
+	bitmap->table = kcalloc(BITS_TO_LONGS(bitmap->max), sizeof(long),
+				GFP_KERNEL);
+	if (!bitmap->table)
+		return -ENOMEM;
+
+	for (i = 0; i < reserved_bot; ++i)
+		set_bit(i, bitmap->table);
+
+	return 0;
+}
+
+void hns_roce_bitmap_cleanup(struct hns_roce_bitmap *bitmap)
+{
+	kfree(bitmap->table);
+}
+
+void hns_roce_buf_free(struct hns_roce_dev *hr_dev, u32 size,
+		       struct hns_roce_buf *buf)
+{
+	int i;
+	struct device *dev = &hr_dev->pdev->dev;
+	u32 bits_per_long = BITS_PER_LONG;
+
+	if (buf->nbufs == 1) {
+		dma_free_coherent(dev, 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(&hr_dev->pdev->dev, PAGE_SIZE,
+						  buf->page_list[i].buf,
+						  buf->page_list[i].map);
+		kfree(buf->page_list);
+	}
+}
+
+int hns_roce_buf_alloc(struct hns_roce_dev *hr_dev, u32 size, u32 max_direct,
+		       struct hns_roce_buf *buf)
+{
+	int i = 0;
+	dma_addr_t t;
+	struct page **pages;
+	struct device *dev = &hr_dev->pdev->dev;
+	u32 bits_per_long = BITS_PER_LONG;
+
+	/* SQ/RQ buf lease than one page, SQ + RQ = 8K */
+	if (size <= max_direct) {
+		buf->nbufs = 1;
+		/* Npages calculated by page_size */
+		buf->npages = 1 << get_order(size);
+		buf->page_shift = PAGE_SHIFT;
+		/* MTT PA must be recorded in 4k alignment, t is 4k aligned */
+		buf->direct.buf = dma_alloc_coherent(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;
+		}
+
+		memset(buf->direct.buf, 0, size);
+	} else {
+		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_alloc_coherent(dev,
+								  PAGE_SIZE, &t,
+								  GFP_KERNEL);
+
+			if (!buf->page_list[i].buf)
+				goto err_free;
+
+			buf->page_list[i].map = t;
+			memset(buf->page_list[i].buf, 0, PAGE_SIZE);
+		}
+		if (bits_per_long == 64) {
+			pages = kmalloc_array(buf->nbufs, sizeof(*pages),
+					      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;
+		}
+	}
+
+	return 0;
+
+err_free:
+	hns_roce_buf_free(hr_dev, size, buf);
+	return -ENOMEM;
+}
+
+void hns_roce_cleanup_bitmap(struct hns_roce_dev *hr_dev)
+{
+	hns_roce_cleanup_qp_table(hr_dev);
+	hns_roce_cleanup_cq_table(hr_dev);
+	hns_roce_cleanup_mr_table(hr_dev);
+	hns_roce_cleanup_pd_table(hr_dev);
+	hns_roce_cleanup_uar_table(hr_dev);
+}
diff --git a/drivers/infiniband/hw/hns/hns_roce_cmd.c b/drivers/infiniband/hw/hns/hns_roce_cmd.c
new file mode 100644
index 000000000000..2a0b6c05da5f
--- /dev/null
+++ b/drivers/infiniband/hw/hns/hns_roce_cmd.c
@@ -0,0 +1,368 @@
+/*
+ * Copyright (c) 2016 Hisilicon Limited.
+ *
+ * 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/dmapool.h>
+#include <linux/platform_device.h>
+#include "hns_roce_common.h"
+#include "hns_roce_device.h"
+#include "hns_roce_cmd.h"
+
+#define CMD_POLL_TOKEN		0xffff
+#define CMD_MAX_NUM		32
+#define STATUS_MASK		0xff
+#define CMD_TOKEN_MASK		0x1f
+#define GO_BIT_TIMEOUT_MSECS	10000
+
+enum {
+	HCR_TOKEN_OFFSET	= 0x14,
+	HCR_STATUS_OFFSET	= 0x18,
+	HCR_GO_BIT		= 15,
+};
+
+static int cmd_pending(struct hns_roce_dev *hr_dev)
+{
+	u32 status = readl(hr_dev->cmd.hcr + HCR_TOKEN_OFFSET);
+
+	return (!!(status & (1 << HCR_GO_BIT)));
+}
+
+/* this function should be serialized with "hcr_mutex" */
+static int __hns_roce_cmd_mbox_post_hw(struct hns_roce_dev *hr_dev,
+				       u64 in_param, u64 out_param,
+				       u32 in_modifier, u8 op_modifier, u16 op,
+				       u16 token, int event)
+{
+	struct hns_roce_cmdq *cmd = &hr_dev->cmd;
+	struct device *dev = &hr_dev->pdev->dev;
+	u32 __iomem *hcr = (u32 *)cmd->hcr;
+	int ret = -EAGAIN;
+	unsigned long end;
+	u32 val = 0;
+
+	end = msecs_to_jiffies(GO_BIT_TIMEOUT_MSECS) + jiffies;
+	while (cmd_pending(hr_dev)) {
+		if (time_after(jiffies, end)) {
+			dev_dbg(dev, "jiffies=%d end=%d\n", (int)jiffies,
+				(int)end);
+			goto out;
+		}
+		cond_resched();
+	}
+
+	roce_set_field(val, ROCEE_MB6_ROCEE_MB_CMD_M, ROCEE_MB6_ROCEE_MB_CMD_S,
+		       op);
+	roce_set_field(val, ROCEE_MB6_ROCEE_MB_CMD_MDF_M,
+		       ROCEE_MB6_ROCEE_MB_CMD_MDF_S, op_modifier);
+	roce_set_bit(val, ROCEE_MB6_ROCEE_MB_EVENT_S, event);
+	roce_set_bit(val, ROCEE_MB6_ROCEE_MB_HW_RUN_S, 1);
+	roce_set_field(val, ROCEE_MB6_ROCEE_MB_TOKEN_M,
+		       ROCEE_MB6_ROCEE_MB_TOKEN_S, token);
+
+	__raw_writeq(cpu_to_le64(in_param), hcr + 0);
+	__raw_writeq(cpu_to_le64(out_param), hcr + 2);
+	__raw_writel(cpu_to_le32(in_modifier), hcr + 4);
+	/* Memory barrier */
+	wmb();
+
+	__raw_writel(cpu_to_le32(val), hcr + 5);
+
+	mmiowb();
+	ret = 0;
+
+out:
+	return ret;
+}
+
+static int hns_roce_cmd_mbox_post_hw(struct hns_roce_dev *hr_dev, u64 in_param,
+				     u64 out_param, u32 in_modifier,
+				     u8 op_modifier, u16 op, u16 token,
+				     int event)
+{
+	struct hns_roce_cmdq *cmd = &hr_dev->cmd;
+	int ret = -EAGAIN;
+
+	mutex_lock(&cmd->hcr_mutex);
+	ret = __hns_roce_cmd_mbox_post_hw(hr_dev, in_param, out_param,
+					  in_modifier, op_modifier, op, token,
+					  event);
+	mutex_unlock(&cmd->hcr_mutex);
+
+	return ret;
+}
+
+/* this should be called with "poll_sem" */
+static int __hns_roce_cmd_mbox_poll(struct hns_roce_dev *hr_dev, u64 in_param,
+				    u64 out_param, unsigned long in_modifier,
+				    u8 op_modifier, u16 op,
+				    unsigned long timeout)
+{
+	struct device *dev = &hr_dev->pdev->dev;
+	u8 __iomem *hcr = hr_dev->cmd.hcr;
+	unsigned long end = 0;
+	u32 status = 0;
+	int ret;
+
+	ret = hns_roce_cmd_mbox_post_hw(hr_dev, in_param, out_param,
+					in_modifier, op_modifier, op,
+					CMD_POLL_TOKEN, 0);
+	if (ret) {
+		dev_err(dev, "[cmd_poll]hns_roce_cmd_mbox_post_hw failed\n");
+		goto out;
+	}
+
+	end = msecs_to_jiffies(timeout) + jiffies;
+	while (cmd_pending(hr_dev) && time_before(jiffies, end))
+		cond_resched();
+
+	if (cmd_pending(hr_dev)) {
+		dev_err(dev, "[cmd_poll]hw run cmd TIMEDOUT!\n");
+		ret = -ETIMEDOUT;
+		goto out;
+	}
+
+	status = le32_to_cpu((__force __be32)
+			      __raw_readl(hcr + HCR_STATUS_OFFSET));
+	if ((status & STATUS_MASK) != 0x1) {
+		dev_err(dev, "mailbox status 0x%x!\n", status);
+		ret = -EBUSY;
+		goto out;
+	}
+
+out:
+	return ret;
+}
+
+static int hns_roce_cmd_mbox_poll(struct hns_roce_dev *hr_dev, u64 in_param,
+				  u64 out_param, unsigned long in_modifier,
+				  u8 op_modifier, u16 op, unsigned long timeout)
+{
+	int ret;
+
+	down(&hr_dev->cmd.poll_sem);
+	ret = __hns_roce_cmd_mbox_poll(hr_dev, in_param, out_param, in_modifier,
+				       op_modifier, op, timeout);
+	up(&hr_dev->cmd.poll_sem);
+
+	return ret;
+}
+
+void hns_roce_cmd_event(struct hns_roce_dev *hr_dev, u16 token, u8 status,
+			u64 out_param)
+{
+	struct hns_roce_cmd_context
+		*context = &hr_dev->cmd.context[token & hr_dev->cmd.token_mask];
+
+	if (token != context->token)
+		return;
+
+	context->result = (status == HNS_ROCE_CMD_SUCCESS) ? 0 : (-EIO);
+	context->out_param = out_param;
+	complete(&context->done);
+}
+
+/* this should be called with "use_events" */
+static int __hns_roce_cmd_mbox_wait(struct hns_roce_dev *hr_dev, u64 in_param,
+				    u64 out_param, unsigned long in_modifier,
+				    u8 op_modifier, u16 op,
+				    unsigned long timeout)
+{
+	struct hns_roce_cmdq *cmd = &hr_dev->cmd;
+	struct device *dev = &hr_dev->pdev->dev;
+	struct hns_roce_cmd_context *context;
+	int ret = 0;
+
+	spin_lock(&cmd->context_lock);
+	WARN_ON(cmd->free_head < 0);
+	context = &cmd->context[cmd->free_head];
+	context->token += cmd->token_mask + 1;
+	cmd->free_head = context->next;
+	spin_unlock(&cmd->context_lock);
+
+	init_completion(&context->done);
+
+	ret = hns_roce_cmd_mbox_post_hw(hr_dev, in_param, out_param,
+					in_modifier, op_modifier, op,
+					context->token, 1);
+	if (ret)
+		goto out;
+
+	/*
+	* It is timeout when wait_for_completion_timeout return 0
+	* The return value is the time limit set in advance
+	* how many seconds showing
+	*/
+	if (!wait_for_completion_timeout(&context->done,
+					 msecs_to_jiffies(timeout))) {
+		dev_err(dev, "[cmd]wait_for_completion_timeout timeout\n");
+		ret = -EBUSY;
+		goto out;
+	}
+
+	ret = context->result;
+	if (ret) {
+		dev_err(dev, "[cmd]event mod cmd process error!err=%d\n", ret);
+		goto out;
+	}
+
+out:
+	spin_lock(&cmd->context_lock);
+	context->next = cmd->free_head;
+	cmd->free_head = context - cmd->context;
+	spin_unlock(&cmd->context_lock);
+
+	return ret;
+}
+
+static int hns_roce_cmd_mbox_wait(struct hns_roce_dev *hr_dev, u64 in_param,
+				  u64 out_param, unsigned long in_modifier,
+				  u8 op_modifier, u16 op, unsigned long timeout)
+{
+	int ret = 0;
+
+	down(&hr_dev->cmd.event_sem);
+	ret = __hns_roce_cmd_mbox_wait(hr_dev, in_param, out_param,
+				       in_modifier, op_modifier, op, timeout);
+	up(&hr_dev->cmd.event_sem);
+
+	return ret;
+}
+
+int hns_roce_cmd_mbox(struct hns_roce_dev *hr_dev, u64 in_param, u64 out_param,
+		      unsigned long in_modifier, u8 op_modifier, u16 op,
+		      unsigned long timeout)
+{
+	if (hr_dev->cmd.use_events)
+		return hns_roce_cmd_mbox_wait(hr_dev, in_param, out_param,
+					      in_modifier, op_modifier, op,
+					      timeout);
+	else
+		return hns_roce_cmd_mbox_poll(hr_dev, in_param, out_param,
+					      in_modifier, op_modifier, op,
+					      timeout);
+}
+
+int hns_roce_cmd_init(struct hns_roce_dev *hr_dev)
+{
+	struct device *dev = &hr_dev->pdev->dev;
+
+	mutex_init(&hr_dev->cmd.hcr_mutex);
+	sema_init(&hr_dev->cmd.poll_sem, 1);
+	hr_dev->cmd.use_events = 0;
+	hr_dev->cmd.toggle = 1;
+	hr_dev->cmd.max_cmds = CMD_MAX_NUM;
+	hr_dev->cmd.hcr = hr_dev->reg_base + ROCEE_MB1_REG;
+	hr_dev->cmd.pool = dma_pool_create("hns_roce_cmd", dev,
+					   HNS_ROCE_MAILBOX_SIZE,
+					   HNS_ROCE_MAILBOX_SIZE, 0);
+	if (!hr_dev->cmd.pool)
+		return -ENOMEM;
+
+	return 0;
+}
+
+void hns_roce_cmd_cleanup(struct hns_roce_dev *hr_dev)
+{
+	dma_pool_destroy(hr_dev->cmd.pool);
+}
+
+int hns_roce_cmd_use_events(struct hns_roce_dev *hr_dev)
+{
+	struct hns_roce_cmdq *hr_cmd = &hr_dev->cmd;
+	int i;
+
+	hr_cmd->context = kmalloc(hr_cmd->max_cmds *
+				  sizeof(struct hns_roce_cmd_context),
+				  GFP_KERNEL);
+	if (!hr_cmd->context)
+		return -ENOMEM;
+
+	for (i = 0; i < hr_cmd->max_cmds; ++i) {
+		hr_cmd->context[i].token = i;
+		hr_cmd->context[i].next = i + 1;
+	}
+
+	hr_cmd->context[hr_cmd->max_cmds - 1].next = -1;
+	hr_cmd->free_head = 0;
+
+	sema_init(&hr_cmd->event_sem, hr_cmd->max_cmds);
+	spin_lock_init(&hr_cmd->context_lock);
+
+	hr_cmd->token_mask = CMD_TOKEN_MASK;
+	hr_cmd->use_events = 1;
+
+	down(&hr_cmd->poll_sem);
+
+	return 0;
+}
+
+void hns_roce_cmd_use_polling(struct hns_roce_dev *hr_dev)
+{
+	struct hns_roce_cmdq *hr_cmd = &hr_dev->cmd;
+	int i;
+
+	hr_cmd->use_events = 0;
+
+	for (i = 0; i < hr_cmd->max_cmds; ++i)
+		down(&hr_cmd->event_sem);
+
+	kfree(hr_cmd->context);
+	up(&hr_cmd->poll_sem);
+}
+
+struct hns_roce_cmd_mailbox
+	*hns_roce_alloc_cmd_mailbox(struct hns_roce_dev *hr_dev)
+{
+	struct hns_roce_cmd_mailbox *mailbox;
+
+	mailbox = kmalloc(sizeof(*mailbox), GFP_KERNEL);
+	if (!mailbox)
+		return ERR_PTR(-ENOMEM);
+
+	mailbox->buf = dma_pool_alloc(hr_dev->cmd.pool, GFP_KERNEL,
+				      &mailbox->dma);
+	if (!mailbox->buf) {
+		kfree(mailbox);
+		return ERR_PTR(-ENOMEM);
+	}
+
+	return mailbox;
+}
+
+void hns_roce_free_cmd_mailbox(struct hns_roce_dev *hr_dev,
+			       struct hns_roce_cmd_mailbox *mailbox)
+{
+	if (!mailbox)
+		return;
+
+	dma_pool_free(hr_dev->cmd.pool, mailbox->buf, mailbox->dma);
+	kfree(mailbox);
+}
diff --git a/drivers/infiniband/hw/hns/hns_roce_cmd.h b/drivers/infiniband/hw/hns/hns_roce_cmd.h
new file mode 100644
index 000000000000..e3997d312c55
--- /dev/null
+++ b/drivers/infiniband/hw/hns/hns_roce_cmd.h
@@ -0,0 +1,80 @@
+/*
+ * Copyright (c) 2016 Hisilicon Limited.
+ *
+ * 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 _HNS_ROCE_CMD_H
+#define _HNS_ROCE_CMD_H
+
+#define HNS_ROCE_MAILBOX_SIZE		4096
+
+enum {
+	/* TPT commands */
+	HNS_ROCE_CMD_SW2HW_MPT		= 0xd,
+	HNS_ROCE_CMD_HW2SW_MPT		= 0xf,
+
+	/* CQ commands */
+	HNS_ROCE_CMD_SW2HW_CQ		= 0x16,
+	HNS_ROCE_CMD_HW2SW_CQ		= 0x17,
+
+	/* QP/EE commands */
+	HNS_ROCE_CMD_RST2INIT_QP	= 0x19,
+	HNS_ROCE_CMD_INIT2RTR_QP	= 0x1a,
+	HNS_ROCE_CMD_RTR2RTS_QP		= 0x1b,
+	HNS_ROCE_CMD_RTS2RTS_QP		= 0x1c,
+	HNS_ROCE_CMD_2ERR_QP		= 0x1e,
+	HNS_ROCE_CMD_RTS2SQD_QP		= 0x1f,
+	HNS_ROCE_CMD_SQD2SQD_QP		= 0x38,
+	HNS_ROCE_CMD_SQD2RTS_QP		= 0x20,
+	HNS_ROCE_CMD_2RST_QP		= 0x21,
+	HNS_ROCE_CMD_QUERY_QP		= 0x22,
+};
+
+enum {
+	HNS_ROCE_CMD_TIME_CLASS_A	= 10000,
+	HNS_ROCE_CMD_TIME_CLASS_B	= 10000,
+	HNS_ROCE_CMD_TIME_CLASS_C	= 10000,
+};
+
+struct hns_roce_cmd_mailbox {
+	void		       *buf;
+	dma_addr_t		dma;
+};
+
+int hns_roce_cmd_mbox(struct hns_roce_dev *hr_dev, u64 in_param, u64 out_param,
+		      unsigned long in_modifier, u8 op_modifier, u16 op,
+		      unsigned long timeout);
+
+struct hns_roce_cmd_mailbox
+	*hns_roce_alloc_cmd_mailbox(struct hns_roce_dev *hr_dev);
+void hns_roce_free_cmd_mailbox(struct hns_roce_dev *hr_dev,
+			       struct hns_roce_cmd_mailbox *mailbox);
+
+#endif /* _HNS_ROCE_CMD_H */
diff --git a/drivers/infiniband/hw/hns/hns_roce_common.h b/drivers/infiniband/hw/hns/hns_roce_common.h
new file mode 100644
index 000000000000..297016103aa7
--- /dev/null
+++ b/drivers/infiniband/hw/hns/hns_roce_common.h
@@ -0,0 +1,325 @@
+/*
+ * Copyright (c) 2016 Hisilicon Limited.
+ *
+ * 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 _HNS_ROCE_COMMON_H
+#define _HNS_ROCE_COMMON_H
+
+#ifndef assert
+#define assert(cond)
+#endif
+
+#define roce_write(dev, reg, val)	writel((val), (dev)->reg_base + (reg))
+#define roce_read(dev, reg)		readl((dev)->reg_base + (reg))
+#define roce_raw_write(value, addr) \
+	__raw_writel((__force u32)cpu_to_le32(value), (addr))
+
+#define roce_get_field(origin, mask, shift) \
+	(((origin) & (mask)) >> (shift))
+
+#define roce_get_bit(origin, shift) \
+	roce_get_field((origin), (1ul << (shift)), (shift))
+
+#define roce_set_field(origin, mask, shift, val) \
+	do { \
+		(origin) &= (~(mask)); \
+		(origin) |= (((u32)(val) << (shift)) & (mask)); \
+	} while (0)
+
+#define roce_set_bit(origin, shift, val) \
+	roce_set_field((origin), (1ul << (shift)), (shift), (val))
+
+#define ROCEE_GLB_CFG_ROCEE_DB_SQ_MODE_S 3
+#define ROCEE_GLB_CFG_ROCEE_DB_OTH_MODE_S 4
+
+#define ROCEE_GLB_CFG_SQ_EXT_DB_MODE_S 5
+
+#define ROCEE_GLB_CFG_OTH_EXT_DB_MODE_S 6
+
+#define ROCEE_GLB_CFG_ROCEE_PORT_ST_S 10
+#define ROCEE_GLB_CFG_ROCEE_PORT_ST_M  \
+	(((1UL << 6) - 1) << ROCEE_GLB_CFG_ROCEE_PORT_ST_S)
+
+#define ROCEE_GLB_CFG_TRP_RAQ_DROP_EN_S 16
+
+#define ROCEE_DMAE_USER_CFG1_ROCEE_STREAM_ID_TB_CFG_S 0
+#define ROCEE_DMAE_USER_CFG1_ROCEE_STREAM_ID_TB_CFG_M  \
+	(((1UL << 24) - 1) << ROCEE_DMAE_USER_CFG1_ROCEE_STREAM_ID_TB_CFG_S)
+
+#define ROCEE_DMAE_USER_CFG1_ROCEE_CACHE_TB_CFG_S 24
+#define ROCEE_DMAE_USER_CFG1_ROCEE_CACHE_TB_CFG_M  \
+	(((1UL << 4) - 1) << ROCEE_DMAE_USER_CFG1_ROCEE_CACHE_TB_CFG_S)
+
+#define ROCEE_DMAE_USER_CFG2_ROCEE_STREAM_ID_PKT_CFG_S 0
+#define ROCEE_DMAE_USER_CFG2_ROCEE_STREAM_ID_PKT_CFG_M   \
+	(((1UL << 24) - 1) << ROCEE_DMAE_USER_CFG2_ROCEE_STREAM_ID_PKT_CFG_S)
+
+#define ROCEE_DMAE_USER_CFG2_ROCEE_CACHE_PKT_CFG_S 24
+#define ROCEE_DMAE_USER_CFG2_ROCEE_CACHE_PKT_CFG_M   \
+	(((1UL << 4) - 1) << ROCEE_DMAE_USER_CFG2_ROCEE_CACHE_PKT_CFG_S)
+
+#define ROCEE_DB_SQ_WL_ROCEE_DB_SQ_WL_S 0
+#define ROCEE_DB_SQ_WL_ROCEE_DB_SQ_WL_M   \
+	(((1UL << 16) - 1) << ROCEE_DB_SQ_WL_ROCEE_DB_SQ_WL_S)
+
+#define ROCEE_DB_SQ_WL_ROCEE_DB_SQ_WL_EMPTY_S 16
+#define ROCEE_DB_SQ_WL_ROCEE_DB_SQ_WL_EMPTY_M   \
+	(((1UL << 16) - 1) << ROCEE_DB_SQ_WL_ROCEE_DB_SQ_WL_EMPTY_S)
+
+#define ROCEE_DB_OTHERS_WL_ROCEE_DB_OTH_WL_S 0
+#define ROCEE_DB_OTHERS_WL_ROCEE_DB_OTH_WL_M   \
+	(((1UL << 16) - 1) << ROCEE_DB_OTHERS_WL_ROCEE_DB_OTH_WL_S)
+
+#define ROCEE_DB_OTHERS_WL_ROCEE_DB_OTH_WL_EMPTY_S 16
+#define ROCEE_DB_OTHERS_WL_ROCEE_DB_OTH_WL_EMPTY_M   \
+	(((1UL << 16) - 1) << ROCEE_DB_OTHERS_WL_ROCEE_DB_OTH_WL_EMPTY_S)
+
+#define ROCEE_RAQ_WL_ROCEE_RAQ_WL_S 0
+#define ROCEE_RAQ_WL_ROCEE_RAQ_WL_M   \
+	(((1UL << 8) - 1) << ROCEE_RAQ_WL_ROCEE_RAQ_WL_S)
+
+#define ROCEE_WRMS_POL_TIME_INTERVAL_WRMS_POL_TIME_INTERVAL_S 0
+#define ROCEE_WRMS_POL_TIME_INTERVAL_WRMS_POL_TIME_INTERVAL_M   \
+	(((1UL << 15) - 1) << \
+	ROCEE_WRMS_POL_TIME_INTERVAL_WRMS_POL_TIME_INTERVAL_S)
+
+#define ROCEE_WRMS_POL_TIME_INTERVAL_WRMS_RAQ_TIMEOUT_CHK_CFG_S 16
+#define ROCEE_WRMS_POL_TIME_INTERVAL_WRMS_RAQ_TIMEOUT_CHK_CFG_M   \
+	(((1UL << 4) - 1) << \
+	ROCEE_WRMS_POL_TIME_INTERVAL_WRMS_RAQ_TIMEOUT_CHK_CFG_S)
+
+#define ROCEE_WRMS_POL_TIME_INTERVAL_WRMS_RAQ_TIMEOUT_CHK_EN_S 20
+
+#define ROCEE_WRMS_POL_TIME_INTERVAL_WRMS_EXT_RAQ_MODE 21
+
+#define ROCEE_EXT_DB_SQ_H_EXT_DB_SQ_SHIFT_S 0
+#define ROCEE_EXT_DB_SQ_H_EXT_DB_SQ_SHIFT_M   \
+	(((1UL << 5) - 1) << ROCEE_EXT_DB_SQ_H_EXT_DB_SQ_SHIFT_S)
+
+#define ROCEE_EXT_DB_SQ_H_EXT_DB_SQ_BA_H_S 5
+#define ROCEE_EXT_DB_SQ_H_EXT_DB_SQ_BA_H_M   \
+	(((1UL << 5) - 1) << ROCEE_EXT_DB_SQ_H_EXT_DB_SQ_BA_H_S)
+
+#define ROCEE_EXT_DB_OTH_H_EXT_DB_OTH_SHIFT_S 0
+#define ROCEE_EXT_DB_OTH_H_EXT_DB_OTH_SHIFT_M   \
+	(((1UL << 5) - 1) << ROCEE_EXT_DB_OTH_H_EXT_DB_OTH_SHIFT_S)
+
+#define ROCEE_EXT_DB_SQ_H_EXT_DB_OTH_BA_H_S 5
+#define ROCEE_EXT_DB_SQ_H_EXT_DB_OTH_BA_H_M   \
+	(((1UL << 5) - 1) << ROCEE_EXT_DB_SQ_H_EXT_DB_OTH_BA_H_S)
+
+#define ROCEE_EXT_RAQ_H_EXT_RAQ_SHIFT_S 0
+#define ROCEE_EXT_RAQ_H_EXT_RAQ_SHIFT_M   \
+	(((1UL << 5) - 1) << ROCEE_EXT_RAQ_H_EXT_RAQ_SHIFT_S)
+
+#define ROCEE_EXT_RAQ_H_EXT_RAQ_BA_H_S 8
+#define ROCEE_EXT_RAQ_H_EXT_RAQ_BA_H_M   \
+	(((1UL << 5) - 1) << ROCEE_EXT_RAQ_H_EXT_RAQ_BA_H_S)
+
+#define ROCEE_BT_CMD_H_ROCEE_BT_CMD_IN_MDF_S 0
+#define ROCEE_BT_CMD_H_ROCEE_BT_CMD_IN_MDF_M   \
+	(((1UL << 19) - 1) << ROCEE_BT_CMD_H_ROCEE_BT_CMD_IN_MDF_S)
+
+#define ROCEE_BT_CMD_H_ROCEE_BT_CMD_S 19
+
+#define ROCEE_BT_CMD_H_ROCEE_BT_CMD_MDF_S 20
+#define ROCEE_BT_CMD_H_ROCEE_BT_CMD_MDF_M   \
+	(((1UL << 2) - 1) << ROCEE_BT_CMD_H_ROCEE_BT_CMD_MDF_S)
+
+#define ROCEE_BT_CMD_H_ROCEE_BT_CMD_BA_H_S 22
+#define ROCEE_BT_CMD_H_ROCEE_BT_CMD_BA_H_M   \
+	(((1UL << 5) - 1) << ROCEE_BT_CMD_H_ROCEE_BT_CMD_BA_H_S)
+
+#define ROCEE_BT_CMD_H_ROCEE_BT_CMD_HW_SYNS_S 31
+
+#define ROCEE_QP1C_CFG0_0_ROCEE_QP1C_QP_ST_S 0
+#define ROCEE_QP1C_CFG0_0_ROCEE_QP1C_QP_ST_M   \
+	(((1UL << 3) - 1) << ROCEE_QP1C_CFG0_0_ROCEE_QP1C_QP_ST_S)
+
+#define ROCEE_QP1C_CFG3_0_ROCEE_QP1C_RQ_HEAD_S 0
+#define ROCEE_QP1C_CFG3_0_ROCEE_QP1C_RQ_HEAD_M   \
+	(((1UL << 15) - 1) << ROCEE_QP1C_CFG3_0_ROCEE_QP1C_RQ_HEAD_S)
+
+#define ROCEE_MB6_ROCEE_MB_CMD_S 0
+#define ROCEE_MB6_ROCEE_MB_CMD_M   \
+	(((1UL << 8) - 1) << ROCEE_MB6_ROCEE_MB_CMD_S)
+
+#define ROCEE_MB6_ROCEE_MB_CMD_MDF_S 8
+#define ROCEE_MB6_ROCEE_MB_CMD_MDF_M   \
+	(((1UL << 4) - 1) << ROCEE_MB6_ROCEE_MB_CMD_MDF_S)
+
+#define ROCEE_MB6_ROCEE_MB_EVENT_S 14
+
+#define ROCEE_MB6_ROCEE_MB_HW_RUN_S 15
+
+#define ROCEE_MB6_ROCEE_MB_TOKEN_S 16
+#define ROCEE_MB6_ROCEE_MB_TOKEN_M   \
+	(((1UL << 16) - 1) << ROCEE_MB6_ROCEE_MB_TOKEN_S)
+
+#define ROCEE_DB_OTHERS_H_ROCEE_DB_OTH_INP_H_S 0
+#define ROCEE_DB_OTHERS_H_ROCEE_DB_OTH_INP_H_M   \
+	(((1UL << 24) - 1) << ROCEE_DB_OTHERS_H_ROCEE_DB_OTH_INP_H_S)
+
+#define ROCEE_DB_OTHERS_H_ROCEE_DB_OTH_CMD_MDF_S 24
+#define ROCEE_DB_OTHERS_H_ROCEE_DB_OTH_CMD_MDF_M   \
+	(((1UL << 4) - 1) << ROCEE_DB_OTHERS_H_ROCEE_DB_OTH_CMD_MDF_S)
+
+#define ROCEE_DB_OTHERS_H_ROCEE_DB_OTH_CMD_S 28
+#define ROCEE_DB_OTHERS_H_ROCEE_DB_OTH_CMD_M   \
+	(((1UL << 3) - 1) << ROCEE_DB_OTHERS_H_ROCEE_DB_OTH_CMD_S)
+
+#define ROCEE_DB_OTHERS_H_ROCEE_DB_OTH_HW_SYNS_S 31
+
+#define ROCEE_SMAC_H_ROCEE_SMAC_H_S 0
+#define ROCEE_SMAC_H_ROCEE_SMAC_H_M   \
+	(((1UL << 16) - 1) << ROCEE_SMAC_H_ROCEE_SMAC_H_S)
+
+#define ROCEE_SMAC_H_ROCEE_PORT_MTU_S 16
+#define ROCEE_SMAC_H_ROCEE_PORT_MTU_M   \
+	(((1UL << 4) - 1) << ROCEE_SMAC_H_ROCEE_PORT_MTU_S)
+
+#define ROCEE_CAEP_AEQC_AEQE_SHIFT_CAEP_AEQC_STATE_S 0
+#define ROCEE_CAEP_AEQC_AEQE_SHIFT_CAEP_AEQC_STATE_M   \
+	(((1UL << 2) - 1) << ROCEE_CAEP_AEQC_AEQE_SHIFT_CAEP_AEQC_STATE_S)
+
+#define ROCEE_CAEP_AEQC_AEQE_SHIFT_CAEP_AEQC_AEQE_SHIFT_S 8
+#define ROCEE_CAEP_AEQC_AEQE_SHIFT_CAEP_AEQC_AEQE_SHIFT_M   \
+	(((1UL << 4) - 1) << ROCEE_CAEP_AEQC_AEQE_SHIFT_CAEP_AEQC_AEQE_SHIFT_S)
+
+#define ROCEE_CAEP_AEQC_AEQE_SHIFT_CAEP_AEQ_ALM_OVF_INT_ST_S 17
+
+#define ROCEE_CAEP_AEQE_CUR_IDX_CAEP_AEQ_BT_H_S 0
+#define ROCEE_CAEP_AEQE_CUR_IDX_CAEP_AEQ_BT_H_M   \
+	(((1UL << 5) - 1) << ROCEE_CAEP_AEQE_CUR_IDX_CAEP_AEQ_BT_H_S)
+
+#define ROCEE_CAEP_AEQE_CUR_IDX_CAEP_AEQE_CUR_IDX_S 16
+#define ROCEE_CAEP_AEQE_CUR_IDX_CAEP_AEQE_CUR_IDX_M   \
+	(((1UL << 16) - 1) << ROCEE_CAEP_AEQE_CUR_IDX_CAEP_AEQE_CUR_IDX_S)
+
+#define ROCEE_CAEP_AEQE_CONS_IDX_CAEP_AEQE_CONS_IDX_S 0
+#define ROCEE_CAEP_AEQE_CONS_IDX_CAEP_AEQE_CONS_IDX_M   \
+	(((1UL << 16) - 1) << ROCEE_CAEP_AEQE_CONS_IDX_CAEP_AEQE_CONS_IDX_S)
+
+#define ROCEE_CAEP_CEQC_SHIFT_CAEP_CEQ_ALM_OVF_INT_ST_S 16
+#define ROCEE_CAEP_CE_IRQ_MASK_CAEP_CEQ_ALM_OVF_MASK_S 1
+#define ROCEE_CAEP_CEQ_ALM_OVF_CAEP_CEQ_ALM_OVF_S 0
+
+#define ROCEE_CAEP_AE_MASK_CAEP_AEQ_ALM_OVF_MASK_S 0
+#define ROCEE_CAEP_AE_MASK_CAEP_AE_IRQ_MASK_S 1
+
+#define ROCEE_CAEP_AE_ST_CAEP_AEQ_ALM_OVF_S 0
+
+#define ROCEE_SDB_ISSUE_PTR_SDB_ISSUE_PTR_S 0
+#define ROCEE_SDB_ISSUE_PTR_SDB_ISSUE_PTR_M   \
+	(((1UL << 28) - 1) << ROCEE_SDB_ISSUE_PTR_SDB_ISSUE_PTR_S)
+
+#define ROCEE_SDB_SEND_PTR_SDB_SEND_PTR_S 0
+#define ROCEE_SDB_SEND_PTR_SDB_SEND_PTR_M   \
+	(((1UL << 28) - 1) << ROCEE_SDB_SEND_PTR_SDB_SEND_PTR_S)
+
+#define ROCEE_SDB_INV_CNT_SDB_INV_CNT_S 0
+#define ROCEE_SDB_INV_CNT_SDB_INV_CNT_M   \
+	(((1UL << 16) - 1) << ROCEE_SDB_INV_CNT_SDB_INV_CNT_S)
+
+/*************ROCEE_REG DEFINITION****************/
+#define ROCEE_VENDOR_ID_REG			0x0
+#define ROCEE_VENDOR_PART_ID_REG		0x4
+
+#define ROCEE_HW_VERSION_REG			0x8
+
+#define ROCEE_SYS_IMAGE_GUID_L_REG		0xC
+#define ROCEE_SYS_IMAGE_GUID_H_REG		0x10
+
+#define ROCEE_PORT_GID_L_0_REG			0x50
+#define ROCEE_PORT_GID_ML_0_REG			0x54
+#define ROCEE_PORT_GID_MH_0_REG			0x58
+#define ROCEE_PORT_GID_H_0_REG			0x5C
+
+#define ROCEE_BT_CMD_H_REG			0x204
+
+#define ROCEE_SMAC_L_0_REG			0x240
+#define ROCEE_SMAC_H_0_REG			0x244
+
+#define ROCEE_QP1C_CFG3_0_REG			0x27C
+
+#define ROCEE_CAEP_AEQE_CONS_IDX_REG		0x3AC
+#define ROCEE_CAEP_CEQC_CONS_IDX_0_REG		0x3BC
+
+#define ROCEE_ECC_UCERR_ALM1_REG		0xB38
+#define ROCEE_ECC_UCERR_ALM2_REG		0xB3C
+#define ROCEE_ECC_CERR_ALM1_REG			0xB44
+#define ROCEE_ECC_CERR_ALM2_REG			0xB48
+
+#define ROCEE_ACK_DELAY_REG			0x14
+#define ROCEE_GLB_CFG_REG			0x18
+
+#define ROCEE_DMAE_USER_CFG1_REG		0x40
+#define ROCEE_DMAE_USER_CFG2_REG		0x44
+
+#define ROCEE_DB_SQ_WL_REG			0x154
+#define ROCEE_DB_OTHERS_WL_REG			0x158
+#define ROCEE_RAQ_WL_REG			0x15C
+#define ROCEE_WRMS_POL_TIME_INTERVAL_REG	0x160
+#define ROCEE_EXT_DB_SQ_REG			0x164
+#define ROCEE_EXT_DB_SQ_H_REG			0x168
+#define ROCEE_EXT_DB_OTH_REG			0x16C
+
+#define ROCEE_EXT_DB_OTH_H_REG			0x170
+#define ROCEE_EXT_DB_SQ_WL_EMPTY_REG		0x174
+#define ROCEE_EXT_DB_SQ_WL_REG			0x178
+#define ROCEE_EXT_DB_OTHERS_WL_EMPTY_REG	0x17C
+#define ROCEE_EXT_DB_OTHERS_WL_REG		0x180
+#define ROCEE_EXT_RAQ_REG			0x184
+#define ROCEE_EXT_RAQ_H_REG			0x188
+
+#define ROCEE_CAEP_CE_INTERVAL_CFG_REG		0x190
+#define ROCEE_CAEP_CE_BURST_NUM_CFG_REG		0x194
+#define ROCEE_BT_CMD_L_REG			0x200
+
+#define ROCEE_MB1_REG				0x210
+#define ROCEE_DB_SQ_L_0_REG			0x230
+#define ROCEE_DB_OTHERS_L_0_REG			0x238
+#define ROCEE_QP1C_CFG0_0_REG			0x270
+
+#define ROCEE_CAEP_AEQC_AEQE_SHIFT_REG		0x3A0
+#define ROCEE_CAEP_CEQC_SHIFT_0_REG		0x3B0
+#define ROCEE_CAEP_CE_IRQ_MASK_0_REG		0x3C0
+#define ROCEE_CAEP_CEQ_ALM_OVF_0_REG		0x3C4
+#define ROCEE_CAEP_AE_MASK_REG			0x6C8
+#define ROCEE_CAEP_AE_ST_REG			0x6CC
+
+#define ROCEE_SDB_ISSUE_PTR_REG			0x758
+#define ROCEE_SDB_SEND_PTR_REG			0x75C
+#define ROCEE_SDB_INV_CNT_REG			0x9A4
+#define ROCEE_ECC_UCERR_ALM0_REG		0xB34
+#define ROCEE_ECC_CERR_ALM0_REG			0xB40
+
+#endif /* _HNS_ROCE_COMMON_H */
diff --git a/drivers/infiniband/hw/hns/hns_roce_cq.c b/drivers/infiniband/hw/hns/hns_roce_cq.c
new file mode 100644
index 000000000000..097365932b09
--- /dev/null
+++ b/drivers/infiniband/hw/hns/hns_roce_cq.c
@@ -0,0 +1,453 @@
+/*
+ * Copyright (c) 2016 Hisilicon Limited.
+ *
+ * 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/platform_device.h>
+#include <rdma/ib_umem.h>
+#include "hns_roce_device.h"
+#include "hns_roce_cmd.h"
+#include "hns_roce_hem.h"
+#include "hns_roce_user.h"
+#include "hns_roce_common.h"
+
+static void hns_roce_ib_cq_comp(struct hns_roce_cq *hr_cq)
+{
+	struct ib_cq *ibcq = &hr_cq->ib_cq;
+
+	ibcq->comp_handler(ibcq, ibcq->cq_context);
+}
+
+static void hns_roce_ib_cq_event(struct hns_roce_cq *hr_cq,
+				 enum hns_roce_event event_type)
+{
+	struct hns_roce_dev *hr_dev;
+	struct ib_event event;
+	struct ib_cq *ibcq;
+
+	ibcq = &hr_cq->ib_cq;
+	hr_dev = to_hr_dev(ibcq->device);
+
+	if (event_type != HNS_ROCE_EVENT_TYPE_CQ_ID_INVALID &&
+	    event_type != HNS_ROCE_EVENT_TYPE_CQ_ACCESS_ERROR &&
+	    event_type != HNS_ROCE_EVENT_TYPE_CQ_OVERFLOW) {
+		dev_err(&hr_dev->pdev->dev,
+			"hns_roce_ib: Unexpected event type 0x%x on CQ %06lx\n",
+			event_type, hr_cq->cqn);
+		return;
+	}
+
+	if (ibcq->event_handler) {
+		event.device = ibcq->device;
+		event.event = IB_EVENT_CQ_ERR;
+		event.element.cq = ibcq;
+		ibcq->event_handler(&event, ibcq->cq_context);
+	}
+}
+
+static int hns_roce_sw2hw_cq(struct hns_roce_dev *dev,
+			     struct hns_roce_cmd_mailbox *mailbox,
+			     unsigned long cq_num)
+{
+	return hns_roce_cmd_mbox(dev, mailbox->dma, 0, cq_num, 0,
+			    HNS_ROCE_CMD_SW2HW_CQ, HNS_ROCE_CMD_TIME_CLASS_A);
+}
+
+static int hns_roce_cq_alloc(struct hns_roce_dev *hr_dev, int nent,
+			     struct hns_roce_mtt *hr_mtt,
+			     struct hns_roce_uar *hr_uar,
+			     struct hns_roce_cq *hr_cq, int vector)
+{
+	struct hns_roce_cmd_mailbox *mailbox = NULL;
+	struct hns_roce_cq_table *cq_table = NULL;
+	struct device *dev = &hr_dev->pdev->dev;
+	dma_addr_t dma_handle;
+	u64 *mtts = NULL;
+	int ret = 0;
+
+	cq_table = &hr_dev->cq_table;
+
+	/* Get the physical address of cq buf */
+	mtts = hns_roce_table_find(&hr_dev->mr_table.mtt_table,
+				   hr_mtt->first_seg, &dma_handle);
+	if (!mtts) {
+		dev_err(dev, "CQ alloc.Failed to find cq buf addr.\n");
+		return -EINVAL;
+	}
+
+	if (vector >= hr_dev->caps.num_comp_vectors) {
+		dev_err(dev, "CQ alloc.Invalid vector.\n");
+		return -EINVAL;
+	}
+	hr_cq->vector = vector;
+
+	ret = hns_roce_bitmap_alloc(&cq_table->bitmap, &hr_cq->cqn);
+	if (ret == -1) {
+		dev_err(dev, "CQ alloc.Failed to alloc index.\n");
+		return -ENOMEM;
+	}
+
+	/* Get CQC memory HEM(Hardware Entry Memory) table */
+	ret = hns_roce_table_get(hr_dev, &cq_table->table, hr_cq->cqn);
+	if (ret) {
+		dev_err(dev, "CQ alloc.Failed to get context mem.\n");
+		goto err_out;
+	}
+
+	/* The cq insert radix tree */
+	spin_lock_irq(&cq_table->lock);
+	/* Radix_tree: The associated pointer and long integer key value like */
+	ret = radix_tree_insert(&cq_table->tree, hr_cq->cqn, hr_cq);
+	spin_unlock_irq(&cq_table->lock);
+	if (ret) {
+		dev_err(dev, "CQ alloc.Failed to radix_tree_insert.\n");
+		goto err_put;
+	}
+
+	/* Allocate mailbox memory */
+	mailbox = hns_roce_alloc_cmd_mailbox(hr_dev);
+	if (IS_ERR(mailbox)) {
+		ret = PTR_ERR(mailbox);
+		goto err_radix;
+	}
+
+	hr_dev->hw->write_cqc(hr_dev, hr_cq, mailbox->buf, mtts, dma_handle,
+			      nent, vector);
+
+	/* Send mailbox to hw */
+	ret = hns_roce_sw2hw_cq(hr_dev, mailbox, hr_cq->cqn);
+	hns_roce_free_cmd_mailbox(hr_dev, mailbox);
+	if (ret) {
+		dev_err(dev, "CQ alloc.Failed to cmd mailbox.\n");
+		goto err_radix;
+	}
+
+	hr_cq->cons_index = 0;
+	hr_cq->uar = hr_uar;
+
+	atomic_set(&hr_cq->refcount, 1);
+	init_completion(&hr_cq->free);
+
+	return 0;
+
+err_radix:
+	spin_lock_irq(&cq_table->lock);
+	radix_tree_delete(&cq_table->tree, hr_cq->cqn);
+	spin_unlock_irq(&cq_table->lock);
+
+err_put:
+	hns_roce_table_put(hr_dev, &cq_table->table, hr_cq->cqn);
+
+err_out:
+	hns_roce_bitmap_free(&cq_table->bitmap, hr_cq->cqn);
+	return ret;
+}
+
+static int hns_roce_hw2sw_cq(struct hns_roce_dev *dev,
+			     struct hns_roce_cmd_mailbox *mailbox,
+			     unsigned long cq_num)
+{
+	return hns_roce_cmd_mbox(dev, 0, mailbox ? mailbox->dma : 0, cq_num,
+				 mailbox ? 0 : 1, HNS_ROCE_CMD_HW2SW_CQ,
+				 HNS_ROCE_CMD_TIME_CLASS_A);
+}
+
+static void hns_roce_free_cq(struct hns_roce_dev *hr_dev,
+			     struct hns_roce_cq *hr_cq)
+{
+	struct hns_roce_cq_table *cq_table = &hr_dev->cq_table;
+	struct device *dev = &hr_dev->pdev->dev;
+	int ret;
+
+	ret = hns_roce_hw2sw_cq(hr_dev, NULL, hr_cq->cqn);
+	if (ret)
+		dev_err(dev, "HW2SW_CQ failed (%d) for CQN %06lx\n", ret,
+			hr_cq->cqn);
+
+	/* Waiting interrupt process procedure carried out */
+	synchronize_irq(hr_dev->eq_table.eq[hr_cq->vector].irq);
+
+	/* wait for all interrupt processed */
+	if (atomic_dec_and_test(&hr_cq->refcount))
+		complete(&hr_cq->free);
+	wait_for_completion(&hr_cq->free);
+
+	spin_lock_irq(&cq_table->lock);
+	radix_tree_delete(&cq_table->tree, hr_cq->cqn);
+	spin_unlock_irq(&cq_table->lock);
+
+	hns_roce_table_put(hr_dev, &cq_table->table, hr_cq->cqn);
+	hns_roce_bitmap_free(&cq_table->bitmap, hr_cq->cqn);
+}
+
+static int hns_roce_ib_get_cq_umem(struct hns_roce_dev *hr_dev,
+				   struct ib_ucontext *context,
+				   struct hns_roce_cq_buf *buf,
+				   struct ib_umem **umem, u64 buf_addr, int cqe)
+{
+	int ret;
+
+	*umem = ib_umem_get(context, buf_addr, cqe * hr_dev->caps.cq_entry_sz,
+			    IB_ACCESS_LOCAL_WRITE, 1);
+	if (IS_ERR(*umem))
+		return PTR_ERR(*umem);
+
+	ret = hns_roce_mtt_init(hr_dev, ib_umem_page_count(*umem),
+				ilog2((unsigned int)(*umem)->page_size),
+				&buf->hr_mtt);
+	if (ret)
+		goto err_buf;
+
+	ret = hns_roce_ib_umem_write_mtt(hr_dev, &buf->hr_mtt, *umem);
+	if (ret)
+		goto err_mtt;
+
+	return 0;
+
+err_mtt:
+	hns_roce_mtt_cleanup(hr_dev, &buf->hr_mtt);
+
+err_buf:
+	ib_umem_release(*umem);
+	return ret;
+}
+
+static int hns_roce_ib_alloc_cq_buf(struct hns_roce_dev *hr_dev,
+				    struct hns_roce_cq_buf *buf, u32 nent)
+{
+	int ret;
+
+	ret = hns_roce_buf_alloc(hr_dev, nent * hr_dev->caps.cq_entry_sz,
+				 PAGE_SIZE * 2, &buf->hr_buf);
+	if (ret)
+		goto out;
+
+	ret = hns_roce_mtt_init(hr_dev, buf->hr_buf.npages,
+				buf->hr_buf.page_shift, &buf->hr_mtt);
+	if (ret)
+		goto err_buf;
+
+	ret = hns_roce_buf_write_mtt(hr_dev, &buf->hr_mtt, &buf->hr_buf);
+	if (ret)
+		goto err_mtt;
+
+	return 0;
+
+err_mtt:
+	hns_roce_mtt_cleanup(hr_dev, &buf->hr_mtt);
+
+err_buf:
+	hns_roce_buf_free(hr_dev, nent * hr_dev->caps.cq_entry_sz,
+			  &buf->hr_buf);
+out:
+	return ret;
+}
+
+static void hns_roce_ib_free_cq_buf(struct hns_roce_dev *hr_dev,
+				    struct hns_roce_cq_buf *buf, int cqe)
+{
+	hns_roce_buf_free(hr_dev, (cqe + 1) * hr_dev->caps.cq_entry_sz,
+			  &buf->hr_buf);
+}
+
+struct ib_cq *hns_roce_ib_create_cq(struct ib_device *ib_dev,
+				    const struct ib_cq_init_attr *attr,
+				    struct ib_ucontext *context,
+				    struct ib_udata *udata)
+{
+	struct hns_roce_dev *hr_dev = to_hr_dev(ib_dev);
+	struct device *dev = &hr_dev->pdev->dev;
+	struct hns_roce_ib_create_cq ucmd;
+	struct hns_roce_cq *hr_cq = NULL;
+	struct hns_roce_uar *uar = NULL;
+	int vector = attr->comp_vector;
+	int cq_entries = attr->cqe;
+	int ret = 0;
+
+	if (cq_entries < 1 || cq_entries > hr_dev->caps.max_cqes) {
+		dev_err(dev, "Creat CQ failed. entries=%d, max=%d\n",
+			cq_entries, hr_dev->caps.max_cqes);
+		return ERR_PTR(-EINVAL);
+	}
+
+	hr_cq = kmalloc(sizeof(*hr_cq), GFP_KERNEL);
+	if (!hr_cq)
+		return ERR_PTR(-ENOMEM);
+
+	/* In v1 engine, parameter verification */
+	if (cq_entries < HNS_ROCE_MIN_CQE_NUM)
+		cq_entries = HNS_ROCE_MIN_CQE_NUM;
+
+	cq_entries = roundup_pow_of_two((unsigned int)cq_entries);
+	hr_cq->ib_cq.cqe = cq_entries - 1;
+	spin_lock_init(&hr_cq->lock);
+
+	if (context) {
+		if (ib_copy_from_udata(&ucmd, udata, sizeof(ucmd))) {
+			dev_err(dev, "Failed to copy_from_udata.\n");
+			ret = -EFAULT;
+			goto err_cq;
+		}
+
+		/* Get user space address, write it into mtt table */
+		ret = hns_roce_ib_get_cq_umem(hr_dev, context, &hr_cq->hr_buf,
+					      &hr_cq->umem, ucmd.buf_addr,
+					      cq_entries);
+		if (ret) {
+			dev_err(dev, "Failed to get_cq_umem.\n");
+			goto err_cq;
+		}
+
+		/* Get user space parameters */
+		uar = &to_hr_ucontext(context)->uar;
+	} else {
+		/* Init mmt table and write buff address to mtt table */
+		ret = hns_roce_ib_alloc_cq_buf(hr_dev, &hr_cq->hr_buf,
+					       cq_entries);
+		if (ret) {
+			dev_err(dev, "Failed to alloc_cq_buf.\n");
+			goto err_cq;
+		}
+
+		uar = &hr_dev->priv_uar;
+		hr_cq->cq_db_l = hr_dev->reg_base + ROCEE_DB_OTHERS_L_0_REG +
+				 0x1000 * uar->index;
+	}
+
+	/* Allocate cq index, fill cq_context */
+	ret = hns_roce_cq_alloc(hr_dev, cq_entries, &hr_cq->hr_buf.hr_mtt, uar,
+				hr_cq, vector);
+	if (ret) {
+		dev_err(dev, "Creat CQ .Failed to cq_alloc.\n");
+		goto err_mtt;
+	}
+
+	/* Get created cq handler and carry out event */
+	hr_cq->comp = hns_roce_ib_cq_comp;
+	hr_cq->event = hns_roce_ib_cq_event;
+	hr_cq->cq_depth = cq_entries;
+
+	if (context) {
+		if (ib_copy_to_udata(udata, &hr_cq->cqn, sizeof(u64))) {
+			ret = -EFAULT;
+			goto err_cqc;
+		}
+	}
+
+	return &hr_cq->ib_cq;
+
+err_cqc:
+	hns_roce_free_cq(hr_dev, hr_cq);
+
+err_mtt:
+	hns_roce_mtt_cleanup(hr_dev, &hr_cq->hr_buf.hr_mtt);
+	if (context)
+		ib_umem_release(hr_cq->umem);
+	else
+		hns_roce_ib_free_cq_buf(hr_dev, &hr_cq->hr_buf,
+					hr_cq->ib_cq.cqe);
+
+err_cq:
+	kfree(hr_cq);
+	return ERR_PTR(ret);
+}
+
+int hns_roce_ib_destroy_cq(struct ib_cq *ib_cq)
+{
+	struct hns_roce_dev *hr_dev = to_hr_dev(ib_cq->device);
+	struct hns_roce_cq *hr_cq = to_hr_cq(ib_cq);
+
+	hns_roce_free_cq(hr_dev, hr_cq);
+	hns_roce_mtt_cleanup(hr_dev, &hr_cq->hr_buf.hr_mtt);
+
+	if (ib_cq->uobject)
+		ib_umem_release(hr_cq->umem);
+	else
+		/* Free the buff of stored cq */
+		hns_roce_ib_free_cq_buf(hr_dev, &hr_cq->hr_buf, ib_cq->cqe);
+
+	kfree(hr_cq);
+
+	return 0;
+}
+
+void hns_roce_cq_completion(struct hns_roce_dev *hr_dev, u32 cqn)
+{
+	struct device *dev = &hr_dev->pdev->dev;
+	struct hns_roce_cq *cq;
+
+	cq = radix_tree_lookup(&hr_dev->cq_table.tree,
+			       cqn & (hr_dev->caps.num_cqs - 1));
+	if (!cq) {
+		dev_warn(dev, "Completion event for bogus CQ 0x%08x\n", cqn);
+		return;
+	}
+
+	cq->comp(cq);
+}
+
+void hns_roce_cq_event(struct hns_roce_dev *hr_dev, u32 cqn, int event_type)
+{
+	struct hns_roce_cq_table *cq_table = &hr_dev->cq_table;
+	struct device *dev = &hr_dev->pdev->dev;
+	struct hns_roce_cq *cq;
+
+	cq = radix_tree_lookup(&cq_table->tree,
+			       cqn & (hr_dev->caps.num_cqs - 1));
+	if (cq)
+		atomic_inc(&cq->refcount);
+
+	if (!cq) {
+		dev_warn(dev, "Async event for bogus CQ %08x\n", cqn);
+		return;
+	}
+
+	cq->event(cq, (enum hns_roce_event)event_type);
+
+	if (atomic_dec_and_test(&cq->refcount))
+		complete(&cq->free);
+}
+
+int hns_roce_init_cq_table(struct hns_roce_dev *hr_dev)
+{
+	struct hns_roce_cq_table *cq_table = &hr_dev->cq_table;
+
+	spin_lock_init(&cq_table->lock);
+	INIT_RADIX_TREE(&cq_table->tree, GFP_ATOMIC);
+
+	return hns_roce_bitmap_init(&cq_table->bitmap, hr_dev->caps.num_cqs,
+				    hr_dev->caps.num_cqs - 1,
+				    hr_dev->caps.reserved_cqs, 0);
+}
+
+void hns_roce_cleanup_cq_table(struct hns_roce_dev *hr_dev)
+{
+	hns_roce_bitmap_cleanup(&hr_dev->cq_table.bitmap);
+}
diff --git a/drivers/infiniband/hw/hns/hns_roce_device.h b/drivers/infiniband/hw/hns/hns_roce_device.h
new file mode 100644
index 000000000000..341731553a60
--- /dev/null
+++ b/drivers/infiniband/hw/hns/hns_roce_device.h
@@ -0,0 +1,728 @@
+/*
+ * Copyright (c) 2016 Hisilicon Limited.
+ *
+ * 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 _HNS_ROCE_DEVICE_H
+#define _HNS_ROCE_DEVICE_H
+
+#include <rdma/ib_verbs.h>
+
+#define DRV_NAME "hns_roce"
+
+#define MAC_ADDR_OCTET_NUM			6
+#define HNS_ROCE_MAX_MSG_LEN			0x80000000
+
+#define HNS_ROCE_ALOGN_UP(a, b) ((((a) + (b) - 1) / (b)) * (b))
+
+#define HNS_ROCE_IB_MIN_SQ_STRIDE		6
+
+#define HNS_ROCE_BA_SIZE			(32 * 4096)
+
+/* Hardware specification only for v1 engine */
+#define HNS_ROCE_MIN_CQE_NUM			0x40
+#define HNS_ROCE_MIN_WQE_NUM			0x20
+
+/* Hardware specification only for v1 engine */
+#define HNS_ROCE_MAX_INNER_MTPT_NUM		0x7
+#define HNS_ROCE_MAX_MTPT_PBL_NUM		0x100000
+
+#define HNS_ROCE_MAX_IRQ_NUM			34
+
+#define HNS_ROCE_COMP_VEC_NUM			32
+
+#define HNS_ROCE_AEQE_VEC_NUM			1
+#define HNS_ROCE_AEQE_OF_VEC_NUM		1
+
+/* 4G/4K = 1M */
+#define HNS_ROCE_SL_SHIFT			28
+#define HNS_ROCE_TCLASS_SHIFT			20
+#define HNS_ROCE_FLOW_LABLE_MASK		0xfffff
+
+#define HNS_ROCE_MAX_PORTS			6
+#define HNS_ROCE_MAX_GID_NUM			16
+#define HNS_ROCE_GID_SIZE			16
+
+#define MR_TYPE_MR				0x00
+#define MR_TYPE_DMA				0x03
+
+#define PKEY_ID					0xffff
+#define GUID_LEN				8
+#define NODE_DESC_SIZE				64
+#define DB_REG_OFFSET				0x1000
+
+#define SERV_TYPE_RC				0
+#define SERV_TYPE_RD				1
+#define SERV_TYPE_UC				2
+#define SERV_TYPE_UD				3
+
+#define PAGES_SHIFT_8				8
+#define PAGES_SHIFT_16				16
+#define PAGES_SHIFT_24				24
+#define PAGES_SHIFT_32				32
+
+enum hns_roce_qp_state {
+	HNS_ROCE_QP_STATE_RST,
+	HNS_ROCE_QP_STATE_INIT,
+	HNS_ROCE_QP_STATE_RTR,
+	HNS_ROCE_QP_STATE_RTS,
+	HNS_ROCE_QP_STATE_SQD,
+	HNS_ROCE_QP_STATE_ERR,
+	HNS_ROCE_QP_NUM_STATE,
+};
+
+enum hns_roce_event {
+	HNS_ROCE_EVENT_TYPE_PATH_MIG                  = 0x01,
+	HNS_ROCE_EVENT_TYPE_PATH_MIG_FAILED           = 0x02,
+	HNS_ROCE_EVENT_TYPE_COMM_EST                  = 0x03,
+	HNS_ROCE_EVENT_TYPE_SQ_DRAINED                = 0x04,
+	HNS_ROCE_EVENT_TYPE_WQ_CATAS_ERROR            = 0x05,
+	HNS_ROCE_EVENT_TYPE_INV_REQ_LOCAL_WQ_ERROR    = 0x06,
+	HNS_ROCE_EVENT_TYPE_LOCAL_WQ_ACCESS_ERROR     = 0x07,
+	HNS_ROCE_EVENT_TYPE_SRQ_LIMIT_REACH           = 0x08,
+	HNS_ROCE_EVENT_TYPE_SRQ_LAST_WQE_REACH        = 0x09,
+	HNS_ROCE_EVENT_TYPE_SRQ_CATAS_ERROR           = 0x0a,
+	HNS_ROCE_EVENT_TYPE_CQ_ACCESS_ERROR           = 0x0b,
+	HNS_ROCE_EVENT_TYPE_CQ_OVERFLOW               = 0x0c,
+	HNS_ROCE_EVENT_TYPE_CQ_ID_INVALID             = 0x0d,
+	HNS_ROCE_EVENT_TYPE_PORT_CHANGE               = 0x0f,
+	/* 0x10 and 0x11 is unused in currently application case */
+	HNS_ROCE_EVENT_TYPE_DB_OVERFLOW               = 0x12,
+	HNS_ROCE_EVENT_TYPE_MB                        = 0x13,
+	HNS_ROCE_EVENT_TYPE_CEQ_OVERFLOW              = 0x14,
+};
+
+/* Local Work Queue Catastrophic Error,SUBTYPE 0x5 */
+enum {
+	HNS_ROCE_LWQCE_QPC_ERROR		= 1,
+	HNS_ROCE_LWQCE_MTU_ERROR		= 2,
+	HNS_ROCE_LWQCE_WQE_BA_ADDR_ERROR	= 3,
+	HNS_ROCE_LWQCE_WQE_ADDR_ERROR		= 4,
+	HNS_ROCE_LWQCE_SQ_WQE_SHIFT_ERROR	= 5,
+	HNS_ROCE_LWQCE_SL_ERROR			= 6,
+	HNS_ROCE_LWQCE_PORT_ERROR		= 7,
+};
+
+/* Local Access Violation Work Queue Error,SUBTYPE 0x7 */
+enum {
+	HNS_ROCE_LAVWQE_R_KEY_VIOLATION		= 1,
+	HNS_ROCE_LAVWQE_LENGTH_ERROR		= 2,
+	HNS_ROCE_LAVWQE_VA_ERROR		= 3,
+	HNS_ROCE_LAVWQE_PD_ERROR		= 4,
+	HNS_ROCE_LAVWQE_RW_ACC_ERROR		= 5,
+	HNS_ROCE_LAVWQE_KEY_STATE_ERROR		= 6,
+	HNS_ROCE_LAVWQE_MR_OPERATION_ERROR	= 7,
+};
+
+/* DOORBELL overflow subtype */
+enum {
+	HNS_ROCE_DB_SUBTYPE_SDB_OVF		= 1,
+	HNS_ROCE_DB_SUBTYPE_SDB_ALM_OVF		= 2,
+	HNS_ROCE_DB_SUBTYPE_ODB_OVF		= 3,
+	HNS_ROCE_DB_SUBTYPE_ODB_ALM_OVF		= 4,
+	HNS_ROCE_DB_SUBTYPE_SDB_ALM_EMP		= 5,
+	HNS_ROCE_DB_SUBTYPE_ODB_ALM_EMP		= 6,
+};
+
+enum {
+	/* RQ&SRQ related operations */
+	HNS_ROCE_OPCODE_SEND_DATA_RECEIVE	= 0x06,
+	HNS_ROCE_OPCODE_RDMA_WITH_IMM_RECEIVE	= 0x07,
+};
+
+#define HNS_ROCE_CMD_SUCCESS			1
+
+#define HNS_ROCE_PORT_DOWN			0
+#define HNS_ROCE_PORT_UP			1
+
+#define HNS_ROCE_MTT_ENTRY_PER_SEG		8
+
+#define PAGE_ADDR_SHIFT				12
+
+struct hns_roce_uar {
+	u64		pfn;
+	unsigned long	index;
+};
+
+struct hns_roce_ucontext {
+	struct ib_ucontext	ibucontext;
+	struct hns_roce_uar	uar;
+};
+
+struct hns_roce_pd {
+	struct ib_pd		ibpd;
+	unsigned long		pdn;
+};
+
+struct hns_roce_bitmap {
+	/* Bitmap Traversal last a bit which is 1 */
+	unsigned long		last;
+	unsigned long		top;
+	unsigned long		max;
+	unsigned long		reserved_top;
+	unsigned long		mask;
+	spinlock_t		lock;
+	unsigned long		*table;
+};
+
+/* Order bitmap length -- bit num compute formula: 1 << (max_order - order) */
+/* Order = 0: bitmap is biggest, order = max bitmap is least (only a bit) */
+/* Every bit repesent to a partner free/used status in bitmap */
+/*
+* Initial, bits of other bitmap are all 0 except that a bit of max_order is 1
+* Bit = 1 represent to idle and available; bit = 0: not available
+*/
+struct hns_roce_buddy {
+	/* Members point to every order level bitmap */
+	unsigned long **bits;
+	/* Represent to avail bits of the order level bitmap */
+	u32            *num_free;
+	int             max_order;
+	spinlock_t      lock;
+};
+
+/* For Hardware Entry Memory */
+struct hns_roce_hem_table {
+	/* HEM type: 0 = qpc, 1 = mtt, 2 = cqc, 3 = srq, 4 = other */
+	u32		type;
+	/* HEM array elment num */
+	unsigned long	num_hem;
+	/* HEM entry record obj total num */
+	unsigned long	num_obj;
+	/*Single obj size */
+	unsigned long	obj_size;
+	int		lowmem;
+	struct mutex	mutex;
+	struct hns_roce_hem **hem;
+};
+
+struct hns_roce_mtt {
+	unsigned long	first_seg;
+	int		order;
+	int		page_shift;
+};
+
+/* Only support 4K page size for mr register */
+#define MR_SIZE_4K 0
+
+struct hns_roce_mr {
+	struct ib_mr		ibmr;
+	struct ib_umem		*umem;
+	u64			iova; /* MR's virtual orignal addr */
+	u64			size; /* Address range of MR */
+	u32			key; /* Key of MR */
+	u32			pd;   /* PD num of MR */
+	u32			access;/* Access permission of MR */
+	int			enabled; /* MR's active status */
+	int			type;	/* MR's register type */
+	u64			*pbl_buf;/* MR's PBL space */
+	dma_addr_t		pbl_dma_addr;	/* MR's PBL space PA */
+};
+
+struct hns_roce_mr_table {
+	struct hns_roce_bitmap		mtpt_bitmap;
+	struct hns_roce_buddy		mtt_buddy;
+	struct hns_roce_hem_table	mtt_table;
+	struct hns_roce_hem_table	mtpt_table;
+};
+
+struct hns_roce_wq {
+	u64		*wrid;     /* Work request ID */
+	spinlock_t	lock;
+	int		wqe_cnt;  /* WQE num */
+	u32		max_post;
+	int		max_gs;
+	int		offset;
+	int		wqe_shift;/* WQE size */
+	u32		head;
+	u32		tail;
+	void __iomem	*db_reg_l;
+};
+
+struct hns_roce_buf_list {
+	void		*buf;
+	dma_addr_t	map;
+};
+
+struct hns_roce_buf {
+	struct hns_roce_buf_list	direct;
+	struct hns_roce_buf_list	*page_list;
+	int				nbufs;
+	u32				npages;
+	int				page_shift;
+};
+
+struct hns_roce_cq_buf {
+	struct hns_roce_buf hr_buf;
+	struct hns_roce_mtt hr_mtt;
+};
+
+struct hns_roce_cq {
+	struct ib_cq			ib_cq;
+	struct hns_roce_cq_buf		hr_buf;
+	spinlock_t			lock;
+	struct ib_umem			*umem;
+	void (*comp)(struct hns_roce_cq *);
+	void (*event)(struct hns_roce_cq *, enum hns_roce_event);
+
+	struct hns_roce_uar		*uar;
+	u32				cq_depth;
+	u32				cons_index;
+	void __iomem			*cq_db_l;
+	void __iomem			*tptr_addr;
+	unsigned long			cqn;
+	u32				vector;
+	atomic_t			refcount;
+	struct completion		free;
+};
+
+struct hns_roce_srq {
+	struct ib_srq		ibsrq;
+	int			srqn;
+};
+
+struct hns_roce_uar_table {
+	struct hns_roce_bitmap bitmap;
+};
+
+struct hns_roce_qp_table {
+	struct hns_roce_bitmap		bitmap;
+	spinlock_t			lock;
+	struct hns_roce_hem_table	qp_table;
+	struct hns_roce_hem_table	irrl_table;
+};
+
+struct hns_roce_cq_table {
+	struct hns_roce_bitmap		bitmap;
+	spinlock_t			lock;
+	struct radix_tree_root		tree;
+	struct hns_roce_hem_table	table;
+};
+
+struct hns_roce_raq_table {
+	struct hns_roce_buf_list	*e_raq_buf;
+};
+
+struct hns_roce_av {
+	__le32      port_pd;
+	u8          gid_index;
+	u8          stat_rate;
+	u8          hop_limit;
+	__le32      sl_tclass_flowlabel;
+	u8          dgid[HNS_ROCE_GID_SIZE];
+	u8          mac[6];
+	__le16      vlan;
+};
+
+struct hns_roce_ah {
+	struct ib_ah		ibah;
+	struct hns_roce_av	av;
+};
+
+struct hns_roce_cmd_context {
+	struct completion	done;
+	int			result;
+	int			next;
+	u64			out_param;
+	u16			token;
+};
+
+struct hns_roce_cmdq {
+	struct dma_pool		*pool;
+	u8 __iomem		*hcr;
+	struct mutex		hcr_mutex;
+	struct semaphore	poll_sem;
+	/*
+	* Event mode: cmd register mutex protection,
+	* ensure to not exceed max_cmds and user use limit region
+	*/
+	struct semaphore	event_sem;
+	int			max_cmds;
+	spinlock_t		context_lock;
+	int			free_head;
+	struct hns_roce_cmd_context *context;
+	/*
+	* Result of get integer part
+	* which max_comds compute according a power of 2
+	*/
+	u16			token_mask;
+	/*
+	* Process whether use event mode, init default non-zero
+	* After the event queue of cmd event ready,
+	* can switch into event mode
+	* close device, switch into poll mode(non event mode)
+	*/
+	u8			use_events;
+	u8			toggle;
+};
+
+struct hns_roce_dev;
+
+struct hns_roce_qp {
+	struct ib_qp		ibqp;
+	struct hns_roce_buf	hr_buf;
+	struct hns_roce_wq	rq;
+	__le64			doorbell_qpn;
+	__le32			sq_signal_bits;
+	u32			sq_next_wqe;
+	int			sq_max_wqes_per_wr;
+	int			sq_spare_wqes;
+	struct hns_roce_wq	sq;
+
+	struct ib_umem		*umem;
+	struct hns_roce_mtt	mtt;
+	u32			buff_size;
+	struct mutex		mutex;
+	u8			port;
+	u8			phy_port;
+	u8			sl;
+	u8			resp_depth;
+	u8			state;
+	u32			access_flags;
+	u32			pkey_index;
+	void			(*event)(struct hns_roce_qp *,
+					 enum hns_roce_event);
+	unsigned long		qpn;
+
+	atomic_t		refcount;
+	struct completion	free;
+};
+
+struct hns_roce_sqp {
+	struct hns_roce_qp	hr_qp;
+};
+
+struct hns_roce_ib_iboe {
+	spinlock_t		lock;
+	struct net_device      *netdevs[HNS_ROCE_MAX_PORTS];
+	struct notifier_block	nb;
+	struct notifier_block	nb_inet;
+	/* 16 GID is shared by 6 port in v1 engine. */
+	union ib_gid		gid_table[HNS_ROCE_MAX_GID_NUM];
+	u8			phy_port[HNS_ROCE_MAX_PORTS];
+};
+
+struct hns_roce_eq {
+	struct hns_roce_dev		*hr_dev;
+	void __iomem			*doorbell;
+
+	int				type_flag;/* Aeq:1 ceq:0 */
+	int				eqn;
+	u32				entries;
+	int				log_entries;
+	int				eqe_size;
+	int				irq;
+	int				log_page_size;
+	int				cons_index;
+	struct hns_roce_buf_list	*buf_list;
+};
+
+struct hns_roce_eq_table {
+	struct hns_roce_eq	*eq;
+	void __iomem		**eqc_base;
+};
+
+struct hns_roce_caps {
+	u8		num_ports;
+	int		gid_table_len[HNS_ROCE_MAX_PORTS];
+	int		pkey_table_len[HNS_ROCE_MAX_PORTS];
+	int		local_ca_ack_delay;
+	int		num_uars;
+	u32		phy_num_uars;
+	u32		max_sq_sg;	/* 2 */
+	u32		max_sq_inline;	/* 32 */
+	u32		max_rq_sg;	/* 2 */
+	int		num_qps;	/* 256k */
+	u32		max_wqes;	/* 16k */
+	u32		max_sq_desc_sz;	/* 64 */
+	u32		max_rq_desc_sz;	/* 64 */
+	int		max_qp_init_rdma;
+	int		max_qp_dest_rdma;
+	int		num_cqs;
+	int		max_cqes;
+	int		reserved_cqs;
+	int		num_aeq_vectors;	/* 1 */
+	int		num_comp_vectors;	/* 32 ceq */
+	int		num_other_vectors;
+	int		num_mtpts;
+	u32		num_mtt_segs;
+	int		reserved_mrws;
+	int		reserved_uars;
+	int		num_pds;
+	int		reserved_pds;
+	u32		mtt_entry_sz;
+	u32		cq_entry_sz;
+	u32		page_size_cap;
+	u32		reserved_lkey;
+	int		mtpt_entry_sz;
+	int		qpc_entry_sz;
+	int		irrl_entry_sz;
+	int		cqc_entry_sz;
+	int		aeqe_depth;
+	int		ceqe_depth[HNS_ROCE_COMP_VEC_NUM];
+	enum ib_mtu	max_mtu;
+};
+
+struct hns_roce_hw {
+	int (*reset)(struct hns_roce_dev *hr_dev, bool enable);
+	void (*hw_profile)(struct hns_roce_dev *hr_dev);
+	int (*hw_init)(struct hns_roce_dev *hr_dev);
+	void (*hw_exit)(struct hns_roce_dev *hr_dev);
+	void (*set_gid)(struct hns_roce_dev *hr_dev, u8 port, int gid_index,
+			union ib_gid *gid);
+	void (*set_mac)(struct hns_roce_dev *hr_dev, u8 phy_port, u8 *addr);
+	void (*set_mtu)(struct hns_roce_dev *hr_dev, u8 phy_port,
+			enum ib_mtu mtu);
+	int (*write_mtpt)(void *mb_buf, struct hns_roce_mr *mr,
+			  unsigned long mtpt_idx);
+	void (*write_cqc)(struct hns_roce_dev *hr_dev,
+			  struct hns_roce_cq *hr_cq, void *mb_buf, u64 *mtts,
+			  dma_addr_t dma_handle, int nent, u32 vector);
+	int (*clear_hem)(struct hns_roce_dev *hr_dev,
+			 struct hns_roce_hem_table *table, int obj);
+	int (*query_qp)(struct ib_qp *ibqp, struct ib_qp_attr *qp_attr,
+			int qp_attr_mask, struct ib_qp_init_attr *qp_init_attr);
+	int (*modify_qp)(struct ib_qp *ibqp, const struct ib_qp_attr *attr,
+			 int attr_mask, enum ib_qp_state cur_state,
+			 enum ib_qp_state new_state);
+	int (*destroy_qp)(struct ib_qp *ibqp);
+	int (*post_send)(struct ib_qp *ibqp, struct ib_send_wr *wr,
+			 struct ib_send_wr **bad_wr);
+	int (*post_recv)(struct ib_qp *qp, struct ib_recv_wr *recv_wr,
+			 struct ib_recv_wr **bad_recv_wr);
+	int (*req_notify_cq)(struct ib_cq *ibcq, enum ib_cq_notify_flags flags);
+	int (*poll_cq)(struct ib_cq *ibcq, int num_entries, struct ib_wc *wc);
+	void	*priv;
+};
+
+struct hns_roce_dev {
+	struct ib_device	ib_dev;
+	struct platform_device  *pdev;
+	struct hns_roce_uar     priv_uar;
+	const char		*irq_names[HNS_ROCE_MAX_IRQ_NUM];
+	spinlock_t		sm_lock;
+	spinlock_t		bt_cmd_lock;
+	struct hns_roce_ib_iboe iboe;
+
+	int			irq[HNS_ROCE_MAX_IRQ_NUM];
+	u8 __iomem		*reg_base;
+	struct hns_roce_caps	caps;
+	struct radix_tree_root  qp_table_tree;
+
+	unsigned char	dev_addr[HNS_ROCE_MAX_PORTS][MAC_ADDR_OCTET_NUM];
+	u64			sys_image_guid;
+	u32                     vendor_id;
+	u32                     vendor_part_id;
+	u32                     hw_rev;
+	void __iomem            *priv_addr;
+
+	struct hns_roce_cmdq	cmd;
+	struct hns_roce_bitmap    pd_bitmap;
+	struct hns_roce_uar_table uar_table;
+	struct hns_roce_mr_table  mr_table;
+	struct hns_roce_cq_table  cq_table;
+	struct hns_roce_qp_table  qp_table;
+	struct hns_roce_eq_table  eq_table;
+
+	int			cmd_mod;
+	int			loop_idc;
+	struct hns_roce_hw	*hw;
+};
+
+static inline struct hns_roce_dev *to_hr_dev(struct ib_device *ib_dev)
+{
+	return container_of(ib_dev, struct hns_roce_dev, ib_dev);
+}
+
+static inline struct hns_roce_ucontext
+			*to_hr_ucontext(struct ib_ucontext *ibucontext)
+{
+	return container_of(ibucontext, struct hns_roce_ucontext, ibucontext);
+}
+
+static inline struct hns_roce_pd *to_hr_pd(struct ib_pd *ibpd)
+{
+	return container_of(ibpd, struct hns_roce_pd, ibpd);
+}
+
+static inline struct hns_roce_ah *to_hr_ah(struct ib_ah *ibah)
+{
+	return container_of(ibah, struct hns_roce_ah, ibah);
+}
+
+static inline struct hns_roce_mr *to_hr_mr(struct ib_mr *ibmr)
+{
+	return container_of(ibmr, struct hns_roce_mr, ibmr);
+}
+
+static inline struct hns_roce_qp *to_hr_qp(struct ib_qp *ibqp)
+{
+	return container_of(ibqp, struct hns_roce_qp, ibqp);
+}
+
+static inline struct hns_roce_cq *to_hr_cq(struct ib_cq *ib_cq)
+{
+	return container_of(ib_cq, struct hns_roce_cq, ib_cq);
+}
+
+static inline struct hns_roce_srq *to_hr_srq(struct ib_srq *ibsrq)
+{
+	return container_of(ibsrq, struct hns_roce_srq, ibsrq);
+}
+
+static inline struct hns_roce_sqp *hr_to_hr_sqp(struct hns_roce_qp *hr_qp)
+{
+	return container_of(hr_qp, struct hns_roce_sqp, hr_qp);
+}
+
+static inline void hns_roce_write64_k(__be32 val[2], void __iomem *dest)
+{
+	__raw_writeq(*(u64 *) val, dest);
+}
+
+static inline struct hns_roce_qp
+	*__hns_roce_qp_lookup(struct hns_roce_dev *hr_dev, u32 qpn)
+{
+	return radix_tree_lookup(&hr_dev->qp_table_tree,
+				 qpn & (hr_dev->caps.num_qps - 1));
+}
+
+static inline void *hns_roce_buf_offset(struct hns_roce_buf *buf, int offset)
+{
+	u32 bits_per_long_val = BITS_PER_LONG;
+
+	if (bits_per_long_val == 64 || buf->nbufs == 1)
+		return (char *)(buf->direct.buf) + offset;
+	else
+		return (char *)(buf->page_list[offset >> PAGE_SHIFT].buf) +
+		       (offset & (PAGE_SIZE - 1));
+}
+
+int hns_roce_init_uar_table(struct hns_roce_dev *dev);
+int hns_roce_uar_alloc(struct hns_roce_dev *dev, struct hns_roce_uar *uar);
+void hns_roce_uar_free(struct hns_roce_dev *dev, struct hns_roce_uar *uar);
+void hns_roce_cleanup_uar_table(struct hns_roce_dev *dev);
+
+int hns_roce_cmd_init(struct hns_roce_dev *hr_dev);
+void hns_roce_cmd_cleanup(struct hns_roce_dev *hr_dev);
+void hns_roce_cmd_event(struct hns_roce_dev *hr_dev, u16 token, u8 status,
+			u64 out_param);
+int hns_roce_cmd_use_events(struct hns_roce_dev *hr_dev);
+void hns_roce_cmd_use_polling(struct hns_roce_dev *hr_dev);
+
+int hns_roce_mtt_init(struct hns_roce_dev *hr_dev, int npages, int page_shift,
+		      struct hns_roce_mtt *mtt);
+void hns_roce_mtt_cleanup(struct hns_roce_dev *hr_dev,
+			  struct hns_roce_mtt *mtt);
+int hns_roce_buf_write_mtt(struct hns_roce_dev *hr_dev,
+			   struct hns_roce_mtt *mtt, struct hns_roce_buf *buf);
+
+int hns_roce_init_pd_table(struct hns_roce_dev *hr_dev);
+int hns_roce_init_mr_table(struct hns_roce_dev *hr_dev);
+int hns_roce_init_eq_table(struct hns_roce_dev *hr_dev);
+int hns_roce_init_cq_table(struct hns_roce_dev *hr_dev);
+int hns_roce_init_qp_table(struct hns_roce_dev *hr_dev);
+
+void hns_roce_cleanup_pd_table(struct hns_roce_dev *hr_dev);
+void hns_roce_cleanup_mr_table(struct hns_roce_dev *hr_dev);
+void hns_roce_cleanup_eq_table(struct hns_roce_dev *hr_dev);
+void hns_roce_cleanup_cq_table(struct hns_roce_dev *hr_dev);
+void hns_roce_cleanup_qp_table(struct hns_roce_dev *hr_dev);
+
+int hns_roce_bitmap_alloc(struct hns_roce_bitmap *bitmap, unsigned long *obj);
+void hns_roce_bitmap_free(struct hns_roce_bitmap *bitmap, unsigned long obj);
+int hns_roce_bitmap_init(struct hns_roce_bitmap *bitmap, u32 num, u32 mask,
+			 u32 reserved_bot, u32 resetrved_top);
+void hns_roce_bitmap_cleanup(struct hns_roce_bitmap *bitmap);
+void hns_roce_cleanup_bitmap(struct hns_roce_dev *hr_dev);
+int hns_roce_bitmap_alloc_range(struct hns_roce_bitmap *bitmap, int cnt,
+				int align, unsigned long *obj);
+void hns_roce_bitmap_free_range(struct hns_roce_bitmap *bitmap,
+				unsigned long obj, int cnt);
+
+struct ib_ah *hns_roce_create_ah(struct ib_pd *pd, struct ib_ah_attr *ah_attr);
+int hns_roce_query_ah(struct ib_ah *ibah, struct ib_ah_attr *ah_attr);
+int hns_roce_destroy_ah(struct ib_ah *ah);
+
+struct ib_pd *hns_roce_alloc_pd(struct ib_device *ib_dev,
+				struct ib_ucontext *context,
+				struct ib_udata *udata);
+int hns_roce_dealloc_pd(struct ib_pd *pd);
+
+struct ib_mr *hns_roce_get_dma_mr(struct ib_pd *pd, int acc);
+struct ib_mr *hns_roce_reg_user_mr(struct ib_pd *pd, u64 start, u64 length,
+				   u64 virt_addr, int access_flags,
+				   struct ib_udata *udata);
+int hns_roce_dereg_mr(struct ib_mr *ibmr);
+
+void hns_roce_buf_free(struct hns_roce_dev *hr_dev, u32 size,
+		       struct hns_roce_buf *buf);
+int hns_roce_buf_alloc(struct hns_roce_dev *hr_dev, u32 size, u32 max_direct,
+		       struct hns_roce_buf *buf);
+
+int hns_roce_ib_umem_write_mtt(struct hns_roce_dev *hr_dev,
+			       struct hns_roce_mtt *mtt, struct ib_umem *umem);
+
+struct ib_qp *hns_roce_create_qp(struct ib_pd *ib_pd,
+				 struct ib_qp_init_attr *init_attr,
+				 struct ib_udata *udata);
+int hns_roce_modify_qp(struct ib_qp *ibqp, struct ib_qp_attr *attr,
+		       int attr_mask, struct ib_udata *udata);
+void *get_recv_wqe(struct hns_roce_qp *hr_qp, int n);
+void *get_send_wqe(struct hns_roce_qp *hr_qp, int n);
+bool hns_roce_wq_overflow(struct hns_roce_wq *hr_wq, int nreq,
+			  struct ib_cq *ib_cq);
+enum hns_roce_qp_state to_hns_roce_state(enum ib_qp_state state);
+void hns_roce_lock_cqs(struct hns_roce_cq *send_cq,
+		       struct hns_roce_cq *recv_cq);
+void hns_roce_unlock_cqs(struct hns_roce_cq *send_cq,
+			 struct hns_roce_cq *recv_cq);
+void hns_roce_qp_remove(struct hns_roce_dev *hr_dev, struct hns_roce_qp *hr_qp);
+void hns_roce_qp_free(struct hns_roce_dev *hr_dev, struct hns_roce_qp *hr_qp);
+void hns_roce_release_range_qp(struct hns_roce_dev *hr_dev, int base_qpn,
+			       int cnt);
+__be32 send_ieth(struct ib_send_wr *wr);
+int to_hr_qp_type(int qp_type);
+
+struct ib_cq *hns_roce_ib_create_cq(struct ib_device *ib_dev,
+				    const struct ib_cq_init_attr *attr,
+				    struct ib_ucontext *context,
+				    struct ib_udata *udata);
+
+int hns_roce_ib_destroy_cq(struct ib_cq *ib_cq);
+
+void hns_roce_cq_completion(struct hns_roce_dev *hr_dev, u32 cqn);
+void hns_roce_cq_event(struct hns_roce_dev *hr_dev, u32 cqn, int event_type);
+void hns_roce_qp_event(struct hns_roce_dev *hr_dev, u32 qpn, int event_type);
+int hns_get_gid_index(struct hns_roce_dev *hr_dev, u8 port, int gid_index);
+
+extern struct hns_roce_hw hns_roce_hw_v1;
+
+#endif /* _HNS_ROCE_DEVICE_H */
diff --git a/drivers/infiniband/hw/hns/hns_roce_eq.c b/drivers/infiniband/hw/hns/hns_roce_eq.c
new file mode 100644
index 000000000000..21e21b03cfb5
--- /dev/null
+++ b/drivers/infiniband/hw/hns/hns_roce_eq.c
@@ -0,0 +1,758 @@
+/*
+ * Copyright (c) 2016 Hisilicon Limited.
+ *
+ * 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/platform_device.h>
+#include "hns_roce_common.h"
+#include "hns_roce_device.h"
+#include "hns_roce_eq.h"
+
+static void eq_set_cons_index(struct hns_roce_eq *eq, int req_not)
+{
+	roce_raw_write((eq->cons_index & CONS_INDEX_MASK) |
+		      (req_not << eq->log_entries), eq->doorbell);
+	/* Memory barrier */
+	mb();
+}
+
+static struct hns_roce_aeqe *get_aeqe(struct hns_roce_eq *eq, u32 entry)
+{
+	unsigned long off = (entry & (eq->entries - 1)) *
+			     HNS_ROCE_AEQ_ENTRY_SIZE;
+
+	return (struct hns_roce_aeqe *)((u8 *)
+		(eq->buf_list[off / HNS_ROCE_BA_SIZE].buf) +
+		off % HNS_ROCE_BA_SIZE);
+}
+
+static struct hns_roce_aeqe *next_aeqe_sw(struct hns_roce_eq *eq)
+{
+	struct hns_roce_aeqe *aeqe = get_aeqe(eq, eq->cons_index);
+
+	return (roce_get_bit(aeqe->asyn, HNS_ROCE_AEQE_U32_4_OWNER_S) ^
+		!!(eq->cons_index & eq->entries)) ? aeqe : NULL;
+}
+
+static void hns_roce_wq_catas_err_handle(struct hns_roce_dev *hr_dev,
+					 struct hns_roce_aeqe *aeqe, int qpn)
+{
+	struct device *dev = &hr_dev->pdev->dev;
+
+	dev_warn(dev, "Local Work Queue Catastrophic Error.\n");
+	switch (roce_get_field(aeqe->asyn, HNS_ROCE_AEQE_U32_4_EVENT_SUB_TYPE_M,
+			       HNS_ROCE_AEQE_U32_4_EVENT_SUB_TYPE_S)) {
+	case HNS_ROCE_LWQCE_QPC_ERROR:
+		dev_warn(dev, "QP %d, QPC error.\n", qpn);
+		break;
+	case HNS_ROCE_LWQCE_MTU_ERROR:
+		dev_warn(dev, "QP %d, MTU error.\n", qpn);
+		break;
+	case HNS_ROCE_LWQCE_WQE_BA_ADDR_ERROR:
+		dev_warn(dev, "QP %d, WQE BA addr error.\n", qpn);
+		break;
+	case HNS_ROCE_LWQCE_WQE_ADDR_ERROR:
+		dev_warn(dev, "QP %d, WQE addr error.\n", qpn);
+		break;
+	case HNS_ROCE_LWQCE_SQ_WQE_SHIFT_ERROR:
+		dev_warn(dev, "QP %d, WQE shift error\n", qpn);
+		break;
+	case HNS_ROCE_LWQCE_SL_ERROR:
+		dev_warn(dev, "QP %d, SL error.\n", qpn);
+		break;
+	case HNS_ROCE_LWQCE_PORT_ERROR:
+		dev_warn(dev, "QP %d, port error.\n", qpn);
+		break;
+	default:
+		break;
+	}
+}
+
+static void hns_roce_local_wq_access_err_handle(struct hns_roce_dev *hr_dev,
+						struct hns_roce_aeqe *aeqe,
+						int qpn)
+{
+	struct device *dev = &hr_dev->pdev->dev;
+
+	dev_warn(dev, "Local Access Violation Work Queue Error.\n");
+	switch (roce_get_field(aeqe->asyn, HNS_ROCE_AEQE_U32_4_EVENT_SUB_TYPE_M,
+			       HNS_ROCE_AEQE_U32_4_EVENT_SUB_TYPE_S)) {
+	case HNS_ROCE_LAVWQE_R_KEY_VIOLATION:
+		dev_warn(dev, "QP %d, R_key violation.\n", qpn);
+		break;
+	case HNS_ROCE_LAVWQE_LENGTH_ERROR:
+		dev_warn(dev, "QP %d, length error.\n", qpn);
+		break;
+	case HNS_ROCE_LAVWQE_VA_ERROR:
+		dev_warn(dev, "QP %d, VA error.\n", qpn);
+		break;
+	case HNS_ROCE_LAVWQE_PD_ERROR:
+		dev_err(dev, "QP %d, PD error.\n", qpn);
+		break;
+	case HNS_ROCE_LAVWQE_RW_ACC_ERROR:
+		dev_warn(dev, "QP %d, rw acc error.\n", qpn);
+		break;
+	case HNS_ROCE_LAVWQE_KEY_STATE_ERROR:
+		dev_warn(dev, "QP %d, key state error.\n", qpn);
+		break;
+	case HNS_ROCE_LAVWQE_MR_OPERATION_ERROR:
+		dev_warn(dev, "QP %d, MR operation error.\n", qpn);
+		break;
+	default:
+		break;
+	}
+}
+
+static void hns_roce_qp_err_handle(struct hns_roce_dev *hr_dev,
+				   struct hns_roce_aeqe *aeqe,
+				   int event_type)
+{
+	struct device *dev = &hr_dev->pdev->dev;
+	int phy_port;
+	int qpn;
+
+	qpn = roce_get_field(aeqe->event.qp_event.qp,
+			     HNS_ROCE_AEQE_EVENT_QP_EVENT_QP_QPN_M,
+			     HNS_ROCE_AEQE_EVENT_QP_EVENT_QP_QPN_S);
+	phy_port = roce_get_field(aeqe->event.qp_event.qp,
+			HNS_ROCE_AEQE_EVENT_QP_EVENT_PORT_NUM_M,
+			HNS_ROCE_AEQE_EVENT_QP_EVENT_PORT_NUM_S);
+	if (qpn <= 1)
+		qpn = HNS_ROCE_MAX_PORTS * qpn + phy_port;
+
+	switch (event_type) {
+	case HNS_ROCE_EVENT_TYPE_INV_REQ_LOCAL_WQ_ERROR:
+		dev_warn(dev, "Invalid Req Local Work Queue Error.\n"
+			      "QP %d, phy_port %d.\n", qpn, phy_port);
+		break;
+	case HNS_ROCE_EVENT_TYPE_WQ_CATAS_ERROR:
+		hns_roce_wq_catas_err_handle(hr_dev, aeqe, qpn);
+		break;
+	case HNS_ROCE_EVENT_TYPE_LOCAL_WQ_ACCESS_ERROR:
+		hns_roce_local_wq_access_err_handle(hr_dev, aeqe, qpn);
+		break;
+	default:
+		break;
+	}
+
+	hns_roce_qp_event(hr_dev, qpn, event_type);
+}
+
+static void hns_roce_cq_err_handle(struct hns_roce_dev *hr_dev,
+				   struct hns_roce_aeqe *aeqe,
+				   int event_type)
+{
+	struct device *dev = &hr_dev->pdev->dev;
+	u32 cqn;
+
+	cqn = le32_to_cpu(roce_get_field(aeqe->event.cq_event.cq,
+		    HNS_ROCE_AEQE_EVENT_CQ_EVENT_CQ_CQN_M,
+		    HNS_ROCE_AEQE_EVENT_CQ_EVENT_CQ_CQN_S));
+
+	switch (event_type) {
+	case HNS_ROCE_EVENT_TYPE_CQ_ACCESS_ERROR:
+		dev_warn(dev, "CQ 0x%x access err.\n", cqn);
+		break;
+	case HNS_ROCE_EVENT_TYPE_CQ_OVERFLOW:
+		dev_warn(dev, "CQ 0x%x overflow\n", cqn);
+		break;
+	case HNS_ROCE_EVENT_TYPE_CQ_ID_INVALID:
+		dev_warn(dev, "CQ 0x%x ID invalid.\n", cqn);
+		break;
+	default:
+		break;
+	}
+
+	hns_roce_cq_event(hr_dev, cqn, event_type);
+}
+
+static void hns_roce_db_overflow_handle(struct hns_roce_dev *hr_dev,
+					struct hns_roce_aeqe *aeqe)
+{
+	struct device *dev = &hr_dev->pdev->dev;
+
+	switch (roce_get_field(aeqe->asyn, HNS_ROCE_AEQE_U32_4_EVENT_SUB_TYPE_M,
+			       HNS_ROCE_AEQE_U32_4_EVENT_SUB_TYPE_S)) {
+	case HNS_ROCE_DB_SUBTYPE_SDB_OVF:
+		dev_warn(dev, "SDB overflow.\n");
+		break;
+	case HNS_ROCE_DB_SUBTYPE_SDB_ALM_OVF:
+		dev_warn(dev, "SDB almost overflow.\n");
+		break;
+	case HNS_ROCE_DB_SUBTYPE_SDB_ALM_EMP:
+		dev_warn(dev, "SDB almost empty.\n");
+		break;
+	case HNS_ROCE_DB_SUBTYPE_ODB_OVF:
+		dev_warn(dev, "ODB overflow.\n");
+		break;
+	case HNS_ROCE_DB_SUBTYPE_ODB_ALM_OVF:
+		dev_warn(dev, "ODB almost overflow.\n");
+		break;
+	case HNS_ROCE_DB_SUBTYPE_ODB_ALM_EMP:
+		dev_warn(dev, "SDB almost empty.\n");
+		break;
+	default:
+		break;
+	}
+}
+
+static int hns_roce_aeq_int(struct hns_roce_dev *hr_dev, struct hns_roce_eq *eq)
+{
+	struct device *dev = &hr_dev->pdev->dev;
+	struct hns_roce_aeqe *aeqe;
+	int aeqes_found = 0;
+	int event_type;
+
+	while ((aeqe = next_aeqe_sw(eq))) {
+		dev_dbg(dev, "aeqe = %p, aeqe->asyn.event_type = 0x%lx\n", aeqe,
+			roce_get_field(aeqe->asyn,
+				       HNS_ROCE_AEQE_U32_4_EVENT_TYPE_M,
+				       HNS_ROCE_AEQE_U32_4_EVENT_TYPE_S));
+		/* Memory barrier */
+		rmb();
+
+		event_type = roce_get_field(aeqe->asyn,
+				HNS_ROCE_AEQE_U32_4_EVENT_TYPE_M,
+				HNS_ROCE_AEQE_U32_4_EVENT_TYPE_S);
+		switch (event_type) {
+		case HNS_ROCE_EVENT_TYPE_PATH_MIG:
+			dev_warn(dev, "PATH MIG not supported\n");
+			break;
+		case HNS_ROCE_EVENT_TYPE_COMM_EST:
+			dev_warn(dev, "COMMUNICATION established\n");
+			break;
+		case HNS_ROCE_EVENT_TYPE_SQ_DRAINED:
+			dev_warn(dev, "SQ DRAINED not supported\n");
+			break;
+		case HNS_ROCE_EVENT_TYPE_PATH_MIG_FAILED:
+			dev_warn(dev, "PATH MIG failed\n");
+			break;
+		case HNS_ROCE_EVENT_TYPE_INV_REQ_LOCAL_WQ_ERROR:
+		case HNS_ROCE_EVENT_TYPE_WQ_CATAS_ERROR:
+		case HNS_ROCE_EVENT_TYPE_LOCAL_WQ_ACCESS_ERROR:
+			hns_roce_qp_err_handle(hr_dev, aeqe, event_type);
+			break;
+		case HNS_ROCE_EVENT_TYPE_SRQ_LIMIT_REACH:
+		case HNS_ROCE_EVENT_TYPE_SRQ_CATAS_ERROR:
+		case HNS_ROCE_EVENT_TYPE_SRQ_LAST_WQE_REACH:
+			dev_warn(dev, "SRQ not support!\n");
+			break;
+		case HNS_ROCE_EVENT_TYPE_CQ_ACCESS_ERROR:
+		case HNS_ROCE_EVENT_TYPE_CQ_OVERFLOW:
+		case HNS_ROCE_EVENT_TYPE_CQ_ID_INVALID:
+			hns_roce_cq_err_handle(hr_dev, aeqe, event_type);
+			break;
+		case HNS_ROCE_EVENT_TYPE_PORT_CHANGE:
+			dev_warn(dev, "port change.\n");
+			break;
+		case HNS_ROCE_EVENT_TYPE_MB:
+			hns_roce_cmd_event(hr_dev,
+					   le16_to_cpu(aeqe->event.cmd.token),
+					   aeqe->event.cmd.status,
+					   le64_to_cpu(aeqe->event.cmd.out_param
+					   ));
+			break;
+		case HNS_ROCE_EVENT_TYPE_DB_OVERFLOW:
+			hns_roce_db_overflow_handle(hr_dev, aeqe);
+			break;
+		case HNS_ROCE_EVENT_TYPE_CEQ_OVERFLOW:
+			dev_warn(dev, "CEQ 0x%lx overflow.\n",
+			roce_get_field(aeqe->event.ce_event.ceqe,
+				     HNS_ROCE_AEQE_EVENT_CE_EVENT_CEQE_CEQN_M,
+				     HNS_ROCE_AEQE_EVENT_CE_EVENT_CEQE_CEQN_S));
+			break;
+		default:
+			dev_warn(dev, "Unhandled event %d on EQ %d at index %u\n",
+				 event_type, eq->eqn, eq->cons_index);
+			break;
+		};
+
+		eq->cons_index++;
+		aeqes_found = 1;
+
+		if (eq->cons_index > 2 * hr_dev->caps.aeqe_depth - 1) {
+			dev_warn(dev, "cons_index overflow, set back to zero\n"
+				);
+			eq->cons_index = 0;
+		}
+	}
+
+	eq_set_cons_index(eq, 0);
+
+	return aeqes_found;
+}
+
+static struct hns_roce_ceqe *get_ceqe(struct hns_roce_eq *eq, u32 entry)
+{
+	unsigned long off = (entry & (eq->entries - 1)) *
+			     HNS_ROCE_CEQ_ENTRY_SIZE;
+
+	return (struct hns_roce_ceqe *)((u8 *)
+			(eq->buf_list[off / HNS_ROCE_BA_SIZE].buf) +
+			off % HNS_ROCE_BA_SIZE);
+}
+
+static struct hns_roce_ceqe *next_ceqe_sw(struct hns_roce_eq *eq)
+{
+	struct hns_roce_ceqe *ceqe = get_ceqe(eq, eq->cons_index);
+
+	return (!!(roce_get_bit(ceqe->ceqe.comp,
+		 HNS_ROCE_CEQE_CEQE_COMP_OWNER_S))) ^
+		 (!!(eq->cons_index & eq->entries)) ? ceqe : NULL;
+}
+
+static int hns_roce_ceq_int(struct hns_roce_dev *hr_dev, struct hns_roce_eq *eq)
+{
+	struct hns_roce_ceqe *ceqe;
+	int ceqes_found = 0;
+	u32 cqn;
+
+	while ((ceqe = next_ceqe_sw(eq))) {
+		/* Memory barrier */
+		rmb();
+		cqn = roce_get_field(ceqe->ceqe.comp,
+				     HNS_ROCE_CEQE_CEQE_COMP_CQN_M,
+				     HNS_ROCE_CEQE_CEQE_COMP_CQN_S);
+		hns_roce_cq_completion(hr_dev, cqn);
+
+		++eq->cons_index;
+		ceqes_found = 1;
+
+		if (eq->cons_index > 2 * hr_dev->caps.ceqe_depth[eq->eqn] - 1) {
+			dev_warn(&eq->hr_dev->pdev->dev,
+				"cons_index overflow, set back to zero\n");
+			eq->cons_index = 0;
+		}
+	}
+
+	eq_set_cons_index(eq, 0);
+
+	return ceqes_found;
+}
+
+static int hns_roce_aeq_ovf_int(struct hns_roce_dev *hr_dev,
+				struct hns_roce_eq *eq)
+{
+	struct device *dev = &eq->hr_dev->pdev->dev;
+	int eqovf_found = 0;
+	u32 caepaemask_val;
+	u32 cealmovf_val;
+	u32 caepaest_val;
+	u32 aeshift_val;
+	u32 ceshift_val;
+	u32 cemask_val;
+	int i = 0;
+
+	/**
+	* AEQ overflow ECC mult bit err CEQ overflow alarm
+	* must clear interrupt, mask irq, clear irq, cancel mask operation
+	*/
+	aeshift_val = roce_read(hr_dev, ROCEE_CAEP_AEQC_AEQE_SHIFT_REG);
+
+	if (roce_get_bit(aeshift_val,
+		ROCEE_CAEP_AEQC_AEQE_SHIFT_CAEP_AEQ_ALM_OVF_INT_ST_S) == 1) {
+		dev_warn(dev, "AEQ overflow!\n");
+
+		/* Set mask */
+		caepaemask_val = roce_read(hr_dev, ROCEE_CAEP_AE_MASK_REG);
+		roce_set_bit(caepaemask_val,
+			     ROCEE_CAEP_AE_MASK_CAEP_AEQ_ALM_OVF_MASK_S,
+			     HNS_ROCE_INT_MASK_ENABLE);
+		roce_write(hr_dev, ROCEE_CAEP_AE_MASK_REG, caepaemask_val);
+
+		/* Clear int state(INT_WC : write 1 clear) */
+		caepaest_val = roce_read(hr_dev, ROCEE_CAEP_AE_ST_REG);
+		roce_set_bit(caepaest_val,
+			     ROCEE_CAEP_AE_ST_CAEP_AEQ_ALM_OVF_S, 1);
+		roce_write(hr_dev, ROCEE_CAEP_AE_ST_REG, caepaest_val);
+
+		/* Clear mask */
+		caepaemask_val = roce_read(hr_dev, ROCEE_CAEP_AE_MASK_REG);
+		roce_set_bit(caepaemask_val,
+			     ROCEE_CAEP_AE_MASK_CAEP_AEQ_ALM_OVF_MASK_S,
+			     HNS_ROCE_INT_MASK_DISABLE);
+		roce_write(hr_dev, ROCEE_CAEP_AE_MASK_REG, caepaemask_val);
+	}
+
+	/* CEQ almost overflow */
+	for (i = 0; i < hr_dev->caps.num_comp_vectors; i++) {
+		ceshift_val = roce_read(hr_dev, ROCEE_CAEP_CEQC_SHIFT_0_REG +
+					i * CEQ_REG_OFFSET);
+
+		if (roce_get_bit(ceshift_val,
+		ROCEE_CAEP_CEQC_SHIFT_CAEP_CEQ_ALM_OVF_INT_ST_S) == 1) {
+			dev_warn(dev, "CEQ[%d] almost overflow!\n", i);
+			eqovf_found++;
+
+			/* Set mask */
+			cemask_val = roce_read(hr_dev,
+					       ROCEE_CAEP_CE_IRQ_MASK_0_REG +
+					       i * CEQ_REG_OFFSET);
+			roce_set_bit(cemask_val,
+				ROCEE_CAEP_CE_IRQ_MASK_CAEP_CEQ_ALM_OVF_MASK_S,
+				HNS_ROCE_INT_MASK_ENABLE);
+			roce_write(hr_dev, ROCEE_CAEP_CE_IRQ_MASK_0_REG +
+				   i * CEQ_REG_OFFSET, cemask_val);
+
+			/* Clear int state(INT_WC : write 1 clear) */
+			cealmovf_val = roce_read(hr_dev,
+				       ROCEE_CAEP_CEQ_ALM_OVF_0_REG +
+				       i * CEQ_REG_OFFSET);
+			roce_set_bit(cealmovf_val,
+				     ROCEE_CAEP_CEQ_ALM_OVF_CAEP_CEQ_ALM_OVF_S,
+				     1);
+			roce_write(hr_dev, ROCEE_CAEP_CEQ_ALM_OVF_0_REG +
+				    i * CEQ_REG_OFFSET, cealmovf_val);
+
+			/* Clear mask */
+			cemask_val = roce_read(hr_dev,
+				     ROCEE_CAEP_CE_IRQ_MASK_0_REG +
+				     i * CEQ_REG_OFFSET);
+			roce_set_bit(cemask_val,
+			       ROCEE_CAEP_CE_IRQ_MASK_CAEP_CEQ_ALM_OVF_MASK_S,
+			       HNS_ROCE_INT_MASK_DISABLE);
+			roce_write(hr_dev, ROCEE_CAEP_CE_IRQ_MASK_0_REG +
+				   i * CEQ_REG_OFFSET, cemask_val);
+		}
+	}
+
+	/* ECC multi-bit error alarm */
+	dev_warn(dev, "ECC UCERR ALARM: 0x%x, 0x%x, 0x%x\n",
+		 roce_read(hr_dev, ROCEE_ECC_UCERR_ALM0_REG),
+		 roce_read(hr_dev, ROCEE_ECC_UCERR_ALM1_REG),
+		 roce_read(hr_dev, ROCEE_ECC_UCERR_ALM2_REG));
+
+	dev_warn(dev, "ECC CERR ALARM: 0x%x, 0x%x, 0x%x\n",
+		 roce_read(hr_dev, ROCEE_ECC_CERR_ALM0_REG),
+		 roce_read(hr_dev, ROCEE_ECC_CERR_ALM1_REG),
+		 roce_read(hr_dev, ROCEE_ECC_CERR_ALM2_REG));
+
+	return eqovf_found;
+}
+
+static int hns_roce_eq_int(struct hns_roce_dev *hr_dev, struct hns_roce_eq *eq)
+{
+	int eqes_found = 0;
+
+	if (likely(eq->type_flag == HNS_ROCE_CEQ))
+		/* CEQ irq routine, CEQ is pulse irq, not clear */
+		eqes_found = hns_roce_ceq_int(hr_dev, eq);
+	else if (likely(eq->type_flag == HNS_ROCE_AEQ))
+		/* AEQ irq routine, AEQ is pulse irq, not clear */
+		eqes_found = hns_roce_aeq_int(hr_dev, eq);
+	else
+		/* AEQ queue overflow irq */
+		eqes_found = hns_roce_aeq_ovf_int(hr_dev, eq);
+
+	return eqes_found;
+}
+
+static irqreturn_t hns_roce_msi_x_interrupt(int irq, void *eq_ptr)
+{
+	int int_work = 0;
+	struct hns_roce_eq  *eq  = eq_ptr;
+	struct hns_roce_dev *hr_dev = eq->hr_dev;
+
+	int_work = hns_roce_eq_int(hr_dev, eq);
+
+	return IRQ_RETVAL(int_work);
+}
+
+static void hns_roce_enable_eq(struct hns_roce_dev *hr_dev, int eq_num,
+			       int enable_flag)
+{
+	void __iomem *eqc = hr_dev->eq_table.eqc_base[eq_num];
+	u32 val;
+
+	val = readl(eqc);
+
+	if (enable_flag)
+		roce_set_field(val,
+			       ROCEE_CAEP_AEQC_AEQE_SHIFT_CAEP_AEQC_STATE_M,
+			       ROCEE_CAEP_AEQC_AEQE_SHIFT_CAEP_AEQC_STATE_S,
+			       HNS_ROCE_EQ_STAT_VALID);
+	else
+		roce_set_field(val,
+			       ROCEE_CAEP_AEQC_AEQE_SHIFT_CAEP_AEQC_STATE_M,
+			       ROCEE_CAEP_AEQC_AEQE_SHIFT_CAEP_AEQC_STATE_S,
+			       HNS_ROCE_EQ_STAT_INVALID);
+	writel(val, eqc);
+}
+
+static int hns_roce_create_eq(struct hns_roce_dev *hr_dev,
+			      struct hns_roce_eq *eq)
+{
+	void __iomem *eqc = hr_dev->eq_table.eqc_base[eq->eqn];
+	struct device *dev = &hr_dev->pdev->dev;
+	dma_addr_t tmp_dma_addr;
+	u32 eqconsindx_val = 0;
+	u32 eqcuridx_val = 0;
+	u32 eqshift_val = 0;
+	int num_bas = 0;
+	int ret;
+	int i;
+
+	num_bas = (PAGE_ALIGN(eq->entries * eq->eqe_size) +
+		   HNS_ROCE_BA_SIZE - 1) / HNS_ROCE_BA_SIZE;
+
+	if ((eq->entries * eq->eqe_size) > HNS_ROCE_BA_SIZE) {
+		dev_err(dev, "[error]eq buf %d gt ba size(%d) need bas=%d\n",
+			(eq->entries * eq->eqe_size), HNS_ROCE_BA_SIZE,
+			num_bas);
+		return -EINVAL;
+	}
+
+	eq->buf_list = kcalloc(num_bas, sizeof(*eq->buf_list), GFP_KERNEL);
+	if (!eq->buf_list)
+		return -ENOMEM;
+
+	for (i = 0; i < num_bas; ++i) {
+		eq->buf_list[i].buf = dma_alloc_coherent(dev, HNS_ROCE_BA_SIZE,
+							 &tmp_dma_addr,
+							 GFP_KERNEL);
+		if (!eq->buf_list[i].buf) {
+			ret = -ENOMEM;
+			goto err_out_free_pages;
+		}
+
+		eq->buf_list[i].map = tmp_dma_addr;
+		memset(eq->buf_list[i].buf, 0, HNS_ROCE_BA_SIZE);
+	}
+	eq->cons_index = 0;
+	roce_set_field(eqshift_val,
+		       ROCEE_CAEP_AEQC_AEQE_SHIFT_CAEP_AEQC_STATE_M,
+		       ROCEE_CAEP_AEQC_AEQE_SHIFT_CAEP_AEQC_STATE_S,
+		       HNS_ROCE_EQ_STAT_INVALID);
+	roce_set_field(eqshift_val,
+		       ROCEE_CAEP_AEQC_AEQE_SHIFT_CAEP_AEQC_AEQE_SHIFT_M,
+		       ROCEE_CAEP_AEQC_AEQE_SHIFT_CAEP_AEQC_AEQE_SHIFT_S,
+		       eq->log_entries);
+	writel(eqshift_val, eqc);
+
+	/* Configure eq extended address 12~44bit */
+	writel((u32)(eq->buf_list[0].map >> 12), (u8 *)eqc + 4);
+
+	/*
+	 * Configure eq extended address 45~49 bit.
+	 * 44 = 32 + 12, When evaluating addr to hardware, shift 12 because of
+	 * using 4K page, and shift more 32 because of
+	 * caculating the high 32 bit value evaluated to hardware.
+	 */
+	roce_set_field(eqcuridx_val, ROCEE_CAEP_AEQE_CUR_IDX_CAEP_AEQ_BT_H_M,
+		       ROCEE_CAEP_AEQE_CUR_IDX_CAEP_AEQ_BT_H_S,
+		       eq->buf_list[0].map >> 44);
+	roce_set_field(eqcuridx_val,
+		       ROCEE_CAEP_AEQE_CUR_IDX_CAEP_AEQE_CUR_IDX_M,
+		       ROCEE_CAEP_AEQE_CUR_IDX_CAEP_AEQE_CUR_IDX_S, 0);
+	writel(eqcuridx_val, (u8 *)eqc + 8);
+
+	/* Configure eq consumer index */
+	roce_set_field(eqconsindx_val,
+		       ROCEE_CAEP_AEQE_CONS_IDX_CAEP_AEQE_CONS_IDX_M,
+		       ROCEE_CAEP_AEQE_CONS_IDX_CAEP_AEQE_CONS_IDX_S, 0);
+	writel(eqconsindx_val, (u8 *)eqc + 0xc);
+
+	return 0;
+
+err_out_free_pages:
+	for (i = i - 1; i >= 0; i--)
+		dma_free_coherent(dev, HNS_ROCE_BA_SIZE, eq->buf_list[i].buf,
+				  eq->buf_list[i].map);
+
+	kfree(eq->buf_list);
+	return ret;
+}
+
+static void hns_roce_free_eq(struct hns_roce_dev *hr_dev,
+			     struct hns_roce_eq *eq)
+{
+	int i = 0;
+	int npages = (PAGE_ALIGN(eq->eqe_size * eq->entries) +
+		      HNS_ROCE_BA_SIZE - 1) / HNS_ROCE_BA_SIZE;
+
+	if (!eq->buf_list)
+		return;
+
+	for (i = 0; i < npages; ++i)
+		dma_free_coherent(&hr_dev->pdev->dev, HNS_ROCE_BA_SIZE,
+				  eq->buf_list[i].buf, eq->buf_list[i].map);
+
+	kfree(eq->buf_list);
+}
+
+static void hns_roce_int_mask_en(struct hns_roce_dev *hr_dev)
+{
+	int i = 0;
+	u32 aemask_val;
+	int masken = 0;
+
+	/* AEQ INT */
+	aemask_val = roce_read(hr_dev, ROCEE_CAEP_AE_MASK_REG);
+	roce_set_bit(aemask_val, ROCEE_CAEP_AE_MASK_CAEP_AEQ_ALM_OVF_MASK_S,
+		     masken);
+	roce_set_bit(aemask_val, ROCEE_CAEP_AE_MASK_CAEP_AE_IRQ_MASK_S, masken);
+	roce_write(hr_dev, ROCEE_CAEP_AE_MASK_REG, aemask_val);
+
+	/* CEQ INT */
+	for (i = 0; i < hr_dev->caps.num_comp_vectors; i++) {
+		/* IRQ mask */
+		roce_write(hr_dev, ROCEE_CAEP_CE_IRQ_MASK_0_REG +
+			   i * CEQ_REG_OFFSET, masken);
+	}
+}
+
+static void hns_roce_ce_int_default_cfg(struct hns_roce_dev *hr_dev)
+{
+	/* Configure ce int interval */
+	roce_write(hr_dev, ROCEE_CAEP_CE_INTERVAL_CFG_REG,
+		   HNS_ROCE_CEQ_DEFAULT_INTERVAL);
+
+	/* Configure ce int burst num */
+	roce_write(hr_dev, ROCEE_CAEP_CE_BURST_NUM_CFG_REG,
+		   HNS_ROCE_CEQ_DEFAULT_BURST_NUM);
+}
+
+int hns_roce_init_eq_table(struct hns_roce_dev *hr_dev)
+{
+	struct hns_roce_eq_table *eq_table = &hr_dev->eq_table;
+	struct device *dev = &hr_dev->pdev->dev;
+	struct hns_roce_eq *eq = NULL;
+	int eq_num = 0;
+	int ret = 0;
+	int i = 0;
+	int j = 0;
+
+	eq_num = hr_dev->caps.num_comp_vectors + hr_dev->caps.num_aeq_vectors;
+	eq_table->eq = kcalloc(eq_num, sizeof(*eq_table->eq), GFP_KERNEL);
+	if (!eq_table->eq)
+		return -ENOMEM;
+
+	eq_table->eqc_base = kcalloc(eq_num, sizeof(*eq_table->eqc_base),
+				     GFP_KERNEL);
+	if (!eq_table->eqc_base) {
+		ret = -ENOMEM;
+		goto err_eqc_base_alloc_fail;
+	}
+
+	for (i = 0; i < eq_num; i++) {
+		eq = &eq_table->eq[i];
+		eq->hr_dev = hr_dev;
+		eq->eqn = i;
+		eq->irq = hr_dev->irq[i];
+		eq->log_page_size = PAGE_SHIFT;
+
+		if (i < hr_dev->caps.num_comp_vectors) {
+			/* CEQ */
+			eq_table->eqc_base[i] = hr_dev->reg_base +
+						ROCEE_CAEP_CEQC_SHIFT_0_REG +
+						HNS_ROCE_CEQC_REG_OFFSET * i;
+			eq->type_flag = HNS_ROCE_CEQ;
+			eq->doorbell = hr_dev->reg_base +
+				       ROCEE_CAEP_CEQC_CONS_IDX_0_REG +
+				       HNS_ROCE_CEQC_REG_OFFSET * i;
+			eq->entries = hr_dev->caps.ceqe_depth[i];
+			eq->log_entries = ilog2(eq->entries);
+			eq->eqe_size = sizeof(struct hns_roce_ceqe);
+		} else {
+			/* AEQ */
+			eq_table->eqc_base[i] = hr_dev->reg_base +
+						ROCEE_CAEP_AEQC_AEQE_SHIFT_REG;
+			eq->type_flag = HNS_ROCE_AEQ;
+			eq->doorbell = hr_dev->reg_base +
+				       ROCEE_CAEP_AEQE_CONS_IDX_REG;
+			eq->entries = hr_dev->caps.aeqe_depth;
+			eq->log_entries = ilog2(eq->entries);
+			eq->eqe_size = sizeof(struct hns_roce_aeqe);
+		}
+	}
+
+	/* Disable irq */
+	hns_roce_int_mask_en(hr_dev);
+
+	/* Configure CE irq interval and burst num */
+	hns_roce_ce_int_default_cfg(hr_dev);
+
+	for (i = 0; i < eq_num; i++) {
+		ret = hns_roce_create_eq(hr_dev, &eq_table->eq[i]);
+		if (ret) {
+			dev_err(dev, "eq create failed\n");
+			goto err_create_eq_fail;
+		}
+	}
+
+	for (j = 0; j < eq_num; j++) {
+		ret = request_irq(eq_table->eq[j].irq, hns_roce_msi_x_interrupt,
+				  0, hr_dev->irq_names[j], eq_table->eq + j);
+		if (ret) {
+			dev_err(dev, "request irq error!\n");
+			goto err_request_irq_fail;
+		}
+	}
+
+	for (i = 0; i < eq_num; i++)
+		hns_roce_enable_eq(hr_dev, i, EQ_ENABLE);
+
+	return 0;
+
+err_request_irq_fail:
+	for (j = j - 1; j >= 0; j--)
+		free_irq(eq_table->eq[j].irq, eq_table->eq + j);
+
+err_create_eq_fail:
+	for (i = i - 1; i >= 0; i--)
+		hns_roce_free_eq(hr_dev, &eq_table->eq[i]);
+
+	kfree(eq_table->eqc_base);
+
+err_eqc_base_alloc_fail:
+	kfree(eq_table->eq);
+
+	return ret;
+}
+
+void hns_roce_cleanup_eq_table(struct hns_roce_dev *hr_dev)
+{
+	int i;
+	int eq_num;
+	struct hns_roce_eq_table *eq_table = &hr_dev->eq_table;
+
+	eq_num = hr_dev->caps.num_comp_vectors + hr_dev->caps.num_aeq_vectors;
+	for (i = 0; i < eq_num; i++) {
+		/* Disable EQ */
+		hns_roce_enable_eq(hr_dev, i, EQ_DISABLE);
+
+		free_irq(eq_table->eq[i].irq, eq_table->eq + i);
+
+		hns_roce_free_eq(hr_dev, &eq_table->eq[i]);
+	}
+
+	kfree(eq_table->eqc_base);
+	kfree(eq_table->eq);
+}
diff --git a/drivers/infiniband/hw/hns/hns_roce_eq.h b/drivers/infiniband/hw/hns/hns_roce_eq.h
new file mode 100644
index 000000000000..c6d212d12e03
--- /dev/null
+++ b/drivers/infiniband/hw/hns/hns_roce_eq.h
@@ -0,0 +1,134 @@
+/*
+ * Copyright (c) 2016 Hisilicon Limited.
+ *
+ * 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 _HNS_ROCE_EQ_H
+#define _HNS_ROCE_EQ_H
+
+#define HNS_ROCE_CEQ			1
+#define HNS_ROCE_AEQ			2
+
+#define HNS_ROCE_CEQ_ENTRY_SIZE		0x4
+#define HNS_ROCE_AEQ_ENTRY_SIZE		0x10
+#define HNS_ROCE_CEQC_REG_OFFSET	0x18
+
+#define HNS_ROCE_CEQ_DEFAULT_INTERVAL	0x10
+#define HNS_ROCE_CEQ_DEFAULT_BURST_NUM	0x10
+
+#define HNS_ROCE_INT_MASK_DISABLE	0
+#define HNS_ROCE_INT_MASK_ENABLE	1
+
+#define EQ_ENABLE			1
+#define EQ_DISABLE			0
+#define CONS_INDEX_MASK			0xffff
+
+#define CEQ_REG_OFFSET			0x18
+
+enum {
+	HNS_ROCE_EQ_STAT_INVALID  = 0,
+	HNS_ROCE_EQ_STAT_VALID    = 2,
+};
+
+struct hns_roce_aeqe {
+	u32 asyn;
+	union {
+		struct {
+			u32 qp;
+			u32 rsv0;
+			u32 rsv1;
+		} qp_event;
+
+		struct {
+			u32 cq;
+			u32 rsv0;
+			u32 rsv1;
+		} cq_event;
+
+		struct {
+			u32 port;
+			u32 rsv0;
+			u32 rsv1;
+		} port_event;
+
+		struct {
+			u32 ceqe;
+			u32 rsv0;
+			u32 rsv1;
+		} ce_event;
+
+		struct {
+			__le64  out_param;
+			__le16  token;
+			u8	status;
+			u8	rsv0;
+		} __packed cmd;
+	 } event;
+};
+
+#define HNS_ROCE_AEQE_U32_4_EVENT_TYPE_S 16
+#define HNS_ROCE_AEQE_U32_4_EVENT_TYPE_M   \
+	(((1UL << 8) - 1) << HNS_ROCE_AEQE_U32_4_EVENT_TYPE_S)
+
+#define HNS_ROCE_AEQE_U32_4_EVENT_SUB_TYPE_S 24
+#define HNS_ROCE_AEQE_U32_4_EVENT_SUB_TYPE_M   \
+	(((1UL << 7) - 1) << HNS_ROCE_AEQE_U32_4_EVENT_SUB_TYPE_S)
+
+#define HNS_ROCE_AEQE_U32_4_OWNER_S 31
+
+#define HNS_ROCE_AEQE_EVENT_QP_EVENT_QP_QPN_S 0
+#define HNS_ROCE_AEQE_EVENT_QP_EVENT_QP_QPN_M   \
+	(((1UL << 24) - 1) << HNS_ROCE_AEQE_EVENT_QP_EVENT_QP_QPN_S)
+
+#define HNS_ROCE_AEQE_EVENT_QP_EVENT_PORT_NUM_S 25
+#define HNS_ROCE_AEQE_EVENT_QP_EVENT_PORT_NUM_M   \
+	(((1UL << 3) - 1) << HNS_ROCE_AEQE_EVENT_QP_EVENT_PORT_NUM_S)
+
+#define HNS_ROCE_AEQE_EVENT_CQ_EVENT_CQ_CQN_S 0
+#define HNS_ROCE_AEQE_EVENT_CQ_EVENT_CQ_CQN_M   \
+	(((1UL << 16) - 1) << HNS_ROCE_AEQE_EVENT_CQ_EVENT_CQ_CQN_S)
+
+#define HNS_ROCE_AEQE_EVENT_CE_EVENT_CEQE_CEQN_S 0
+#define HNS_ROCE_AEQE_EVENT_CE_EVENT_CEQE_CEQN_M   \
+	(((1UL << 5) - 1) << HNS_ROCE_AEQE_EVENT_CE_EVENT_CEQE_CEQN_S)
+
+struct hns_roce_ceqe {
+	union {
+		int		comp;
+	} ceqe;
+};
+
+#define HNS_ROCE_CEQE_CEQE_COMP_OWNER_S	0
+
+#define HNS_ROCE_CEQE_CEQE_COMP_CQN_S 16
+#define HNS_ROCE_CEQE_CEQE_COMP_CQN_M   \
+	(((1UL << 16) - 1) << HNS_ROCE_CEQE_CEQE_COMP_CQN_S)
+
+#endif /* _HNS_ROCE_EQ_H */
diff --git a/drivers/infiniband/hw/hns/hns_roce_hem.c b/drivers/infiniband/hw/hns/hns_roce_hem.c
new file mode 100644
index 000000000000..250d8f280390
--- /dev/null
+++ b/drivers/infiniband/hw/hns/hns_roce_hem.c
@@ -0,0 +1,404 @@
+/*
+ * Copyright (c) 2016 Hisilicon Limited.
+ * Copyright (c) 2007, 2008 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/platform_device.h>
+#include "hns_roce_device.h"
+#include "hns_roce_hem.h"
+#include "hns_roce_common.h"
+
+#define HNS_ROCE_HEM_ALLOC_SIZE		(1 << 17)
+#define HNS_ROCE_TABLE_CHUNK_SIZE	(1 << 17)
+
+#define DMA_ADDR_T_SHIFT		12
+#define BT_BA_SHIFT			32
+
+struct hns_roce_hem *hns_roce_alloc_hem(struct hns_roce_dev *hr_dev, int npages,
+					gfp_t gfp_mask)
+{
+	struct hns_roce_hem_chunk *chunk = NULL;
+	struct hns_roce_hem *hem;
+	struct scatterlist *mem;
+	int order;
+	void *buf;
+
+	WARN_ON(gfp_mask & __GFP_HIGHMEM);
+
+	hem = kmalloc(sizeof(*hem),
+		      gfp_mask & ~(__GFP_HIGHMEM | __GFP_NOWARN));
+	if (!hem)
+		return NULL;
+
+	hem->refcount = 0;
+	INIT_LIST_HEAD(&hem->chunk_list);
+
+	order = get_order(HNS_ROCE_HEM_ALLOC_SIZE);
+
+	while (npages > 0) {
+		if (!chunk) {
+			chunk = kmalloc(sizeof(*chunk),
+				gfp_mask & ~(__GFP_HIGHMEM | __GFP_NOWARN));
+			if (!chunk)
+				goto fail;
+
+			sg_init_table(chunk->mem, HNS_ROCE_HEM_CHUNK_LEN);
+			chunk->npages = 0;
+			chunk->nsg = 0;
+			list_add_tail(&chunk->list, &hem->chunk_list);
+		}
+
+		while (1 << order > npages)
+			--order;
+
+		/*
+		* Alloc memory one time. If failed, don't alloc small block
+		* memory, directly return fail.
+		*/
+		mem = &chunk->mem[chunk->npages];
+		buf = dma_alloc_coherent(&hr_dev->pdev->dev, PAGE_SIZE << order,
+				&sg_dma_address(mem), gfp_mask);
+		if (!buf)
+			goto fail;
+
+		sg_set_buf(mem, buf, PAGE_SIZE << order);
+		WARN_ON(mem->offset);
+		sg_dma_len(mem) = PAGE_SIZE << order;
+
+		++chunk->npages;
+		++chunk->nsg;
+		npages -= 1 << order;
+	}
+
+	return hem;
+
+fail:
+	hns_roce_free_hem(hr_dev, hem);
+	return NULL;
+}
+
+void hns_roce_free_hem(struct hns_roce_dev *hr_dev, struct hns_roce_hem *hem)
+{
+	struct hns_roce_hem_chunk *chunk, *tmp;
+	int i;
+
+	if (!hem)
+		return;
+
+	list_for_each_entry_safe(chunk, tmp, &hem->chunk_list, list) {
+		for (i = 0; i < chunk->npages; ++i)
+			dma_free_coherent(&hr_dev->pdev->dev,
+				   chunk->mem[i].length,
+				   lowmem_page_address(sg_page(&chunk->mem[i])),
+				   sg_dma_address(&chunk->mem[i]));
+		kfree(chunk);
+	}
+
+	kfree(hem);
+}
+
+static int hns_roce_set_hem(struct hns_roce_dev *hr_dev,
+			    struct hns_roce_hem_table *table, unsigned long obj)
+{
+	struct device *dev = &hr_dev->pdev->dev;
+	spinlock_t *lock = &hr_dev->bt_cmd_lock;
+	unsigned long end = 0;
+	unsigned long flags;
+	struct hns_roce_hem_iter iter;
+	void __iomem *bt_cmd;
+	u32 bt_cmd_h_val = 0;
+	u32 bt_cmd_val[2];
+	u32 bt_cmd_l = 0;
+	u64 bt_ba = 0;
+	int ret = 0;
+
+	/* Find the HEM(Hardware Entry Memory) entry */
+	unsigned long i = (obj & (table->num_obj - 1)) /
+			  (HNS_ROCE_TABLE_CHUNK_SIZE / table->obj_size);
+
+	switch (table->type) {
+	case HEM_TYPE_QPC:
+		roce_set_field(bt_cmd_h_val, ROCEE_BT_CMD_H_ROCEE_BT_CMD_MDF_M,
+			       ROCEE_BT_CMD_H_ROCEE_BT_CMD_MDF_S, HEM_TYPE_QPC);
+		break;
+	case HEM_TYPE_MTPT:
+		roce_set_field(bt_cmd_h_val, ROCEE_BT_CMD_H_ROCEE_BT_CMD_MDF_M,
+			       ROCEE_BT_CMD_H_ROCEE_BT_CMD_MDF_S,
+			       HEM_TYPE_MTPT);
+		break;
+	case HEM_TYPE_CQC:
+		roce_set_field(bt_cmd_h_val, ROCEE_BT_CMD_H_ROCEE_BT_CMD_MDF_M,
+			       ROCEE_BT_CMD_H_ROCEE_BT_CMD_MDF_S, HEM_TYPE_CQC);
+		break;
+	case HEM_TYPE_SRQC:
+		roce_set_field(bt_cmd_h_val, ROCEE_BT_CMD_H_ROCEE_BT_CMD_MDF_M,
+			       ROCEE_BT_CMD_H_ROCEE_BT_CMD_MDF_S,
+			       HEM_TYPE_SRQC);
+		break;
+	default:
+		return ret;
+	}
+	roce_set_field(bt_cmd_h_val, ROCEE_BT_CMD_H_ROCEE_BT_CMD_IN_MDF_M,
+		       ROCEE_BT_CMD_H_ROCEE_BT_CMD_IN_MDF_S, obj);
+	roce_set_bit(bt_cmd_h_val, ROCEE_BT_CMD_H_ROCEE_BT_CMD_S, 0);
+	roce_set_bit(bt_cmd_h_val, ROCEE_BT_CMD_H_ROCEE_BT_CMD_HW_SYNS_S, 1);
+
+	/* Currently iter only a chunk */
+	for (hns_roce_hem_first(table->hem[i], &iter);
+	     !hns_roce_hem_last(&iter); hns_roce_hem_next(&iter)) {
+		bt_ba = hns_roce_hem_addr(&iter) >> DMA_ADDR_T_SHIFT;
+
+		spin_lock_irqsave(lock, flags);
+
+		bt_cmd = hr_dev->reg_base + ROCEE_BT_CMD_H_REG;
+
+		end = msecs_to_jiffies(HW_SYNC_TIMEOUT_MSECS) + jiffies;
+		while (1) {
+			if (readl(bt_cmd) >> BT_CMD_SYNC_SHIFT) {
+				if (!(time_before(jiffies, end))) {
+					dev_err(dev, "Write bt_cmd err,hw_sync is not zero.\n");
+					spin_unlock_irqrestore(lock, flags);
+					return -EBUSY;
+				}
+			} else {
+				break;
+			}
+			msleep(HW_SYNC_SLEEP_TIME_INTERVAL);
+		}
+
+		bt_cmd_l = (u32)bt_ba;
+		roce_set_field(bt_cmd_h_val, ROCEE_BT_CMD_H_ROCEE_BT_CMD_BA_H_M,
+			       ROCEE_BT_CMD_H_ROCEE_BT_CMD_BA_H_S,
+			       bt_ba >> BT_BA_SHIFT);
+
+		bt_cmd_val[0] = bt_cmd_l;
+		bt_cmd_val[1] = bt_cmd_h_val;
+		hns_roce_write64_k(bt_cmd_val,
+				   hr_dev->reg_base + ROCEE_BT_CMD_L_REG);
+		spin_unlock_irqrestore(lock, flags);
+	}
+
+	return ret;
+}
+
+int hns_roce_table_get(struct hns_roce_dev *hr_dev,
+		       struct hns_roce_hem_table *table, unsigned long obj)
+{
+	struct device *dev = &hr_dev->pdev->dev;
+	int ret = 0;
+	unsigned long i;
+
+	i = (obj & (table->num_obj - 1)) / (HNS_ROCE_TABLE_CHUNK_SIZE /
+	     table->obj_size);
+
+	mutex_lock(&table->mutex);
+
+	if (table->hem[i]) {
+		++table->hem[i]->refcount;
+		goto out;
+	}
+
+	table->hem[i] = hns_roce_alloc_hem(hr_dev,
+				       HNS_ROCE_TABLE_CHUNK_SIZE >> PAGE_SHIFT,
+				       (table->lowmem ? GFP_KERNEL :
+					GFP_HIGHUSER) | __GFP_NOWARN);
+	if (!table->hem[i]) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	/* Set HEM base address(128K/page, pa) to Hardware */
+	if (hns_roce_set_hem(hr_dev, table, obj)) {
+		ret = -ENODEV;
+		dev_err(dev, "set HEM base address to HW failed.\n");
+		goto out;
+	}
+
+	++table->hem[i]->refcount;
+out:
+	mutex_unlock(&table->mutex);
+	return ret;
+}
+
+void hns_roce_table_put(struct hns_roce_dev *hr_dev,
+			struct hns_roce_hem_table *table, unsigned long obj)
+{
+	struct device *dev = &hr_dev->pdev->dev;
+	unsigned long i;
+
+	i = (obj & (table->num_obj - 1)) /
+	    (HNS_ROCE_TABLE_CHUNK_SIZE / table->obj_size);
+
+	mutex_lock(&table->mutex);
+
+	if (--table->hem[i]->refcount == 0) {
+		/* Clear HEM base address */
+		if (hr_dev->hw->clear_hem(hr_dev, table, obj))
+			dev_warn(dev, "Clear HEM base address failed.\n");
+
+		hns_roce_free_hem(hr_dev, table->hem[i]);
+		table->hem[i] = NULL;
+	}
+
+	mutex_unlock(&table->mutex);
+}
+
+void *hns_roce_table_find(struct hns_roce_hem_table *table, unsigned long obj,
+			  dma_addr_t *dma_handle)
+{
+	struct hns_roce_hem_chunk *chunk;
+	unsigned long idx;
+	int i;
+	int offset, dma_offset;
+	struct hns_roce_hem *hem;
+	struct page *page = NULL;
+
+	if (!table->lowmem)
+		return NULL;
+
+	mutex_lock(&table->mutex);
+	idx = (obj & (table->num_obj - 1)) * table->obj_size;
+	hem = table->hem[idx / HNS_ROCE_TABLE_CHUNK_SIZE];
+	dma_offset = offset = idx % HNS_ROCE_TABLE_CHUNK_SIZE;
+
+	if (!hem)
+		goto out;
+
+	list_for_each_entry(chunk, &hem->chunk_list, list) {
+		for (i = 0; i < chunk->npages; ++i) {
+			if (dma_handle && dma_offset >= 0) {
+				if (sg_dma_len(&chunk->mem[i]) >
+				    (u32)dma_offset)
+					*dma_handle = sg_dma_address(
+						&chunk->mem[i]) + dma_offset;
+				dma_offset -= sg_dma_len(&chunk->mem[i]);
+			}
+
+			if (chunk->mem[i].length > (u32)offset) {
+				page = sg_page(&chunk->mem[i]);
+				goto out;
+			}
+			offset -= chunk->mem[i].length;
+		}
+	}
+
+out:
+	mutex_unlock(&table->mutex);
+	return page ? lowmem_page_address(page) + offset : NULL;
+}
+
+int hns_roce_table_get_range(struct hns_roce_dev *hr_dev,
+			     struct hns_roce_hem_table *table,
+			     unsigned long start, unsigned long end)
+{
+	unsigned long inc = HNS_ROCE_TABLE_CHUNK_SIZE / table->obj_size;
+	unsigned long i = 0;
+	int ret = 0;
+
+	/* Allocate MTT entry memory according to chunk(128K) */
+	for (i = start; i <= end; i += inc) {
+		ret = hns_roce_table_get(hr_dev, table, i);
+		if (ret)
+			goto fail;
+	}
+
+	return 0;
+
+fail:
+	while (i > start) {
+		i -= inc;
+		hns_roce_table_put(hr_dev, table, i);
+	}
+	return ret;
+}
+
+void hns_roce_table_put_range(struct hns_roce_dev *hr_dev,
+			      struct hns_roce_hem_table *table,
+			      unsigned long start, unsigned long end)
+{
+	unsigned long i;
+
+	for (i = start; i <= end;
+		i += HNS_ROCE_TABLE_CHUNK_SIZE / table->obj_size)
+		hns_roce_table_put(hr_dev, table, i);
+}
+
+int hns_roce_init_hem_table(struct hns_roce_dev *hr_dev,
+			    struct hns_roce_hem_table *table, u32 type,
+			    unsigned long obj_size, unsigned long nobj,
+			    int use_lowmem)
+{
+	unsigned long obj_per_chunk;
+	unsigned long num_hem;
+
+	obj_per_chunk = HNS_ROCE_TABLE_CHUNK_SIZE / obj_size;
+	num_hem = (nobj + obj_per_chunk - 1) / obj_per_chunk;
+
+	table->hem = kcalloc(num_hem, sizeof(*table->hem), GFP_KERNEL);
+	if (!table->hem)
+		return -ENOMEM;
+
+	table->type = type;
+	table->num_hem = num_hem;
+	table->num_obj = nobj;
+	table->obj_size = obj_size;
+	table->lowmem = use_lowmem;
+	mutex_init(&table->mutex);
+
+	return 0;
+}
+
+void hns_roce_cleanup_hem_table(struct hns_roce_dev *hr_dev,
+				struct hns_roce_hem_table *table)
+{
+	struct device *dev = &hr_dev->pdev->dev;
+	unsigned long i;
+
+	for (i = 0; i < table->num_hem; ++i)
+		if (table->hem[i]) {
+			if (hr_dev->hw->clear_hem(hr_dev, table,
+			    i * HNS_ROCE_TABLE_CHUNK_SIZE / table->obj_size))
+				dev_err(dev, "Clear HEM base address failed.\n");
+
+			hns_roce_free_hem(hr_dev, table->hem[i]);
+		}
+
+	kfree(table->hem);
+}
+
+void hns_roce_cleanup_hem(struct hns_roce_dev *hr_dev)
+{
+	hns_roce_cleanup_hem_table(hr_dev, &hr_dev->cq_table.table);
+	hns_roce_cleanup_hem_table(hr_dev, &hr_dev->qp_table.irrl_table);
+	hns_roce_cleanup_hem_table(hr_dev, &hr_dev->qp_table.qp_table);
+	hns_roce_cleanup_hem_table(hr_dev, &hr_dev->mr_table.mtpt_table);
+	hns_roce_cleanup_hem_table(hr_dev, &hr_dev->mr_table.mtt_table);
+}
diff --git a/drivers/infiniband/hw/hns/hns_roce_hem.h b/drivers/infiniband/hw/hns/hns_roce_hem.h
new file mode 100644
index 000000000000..435748858252
--- /dev/null
+++ b/drivers/infiniband/hw/hns/hns_roce_hem.h
@@ -0,0 +1,135 @@
+/*
+ * Copyright (c) 2016 Hisilicon Limited.
+ * Copyright (c) 2007, 2008 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.
+ */
+
+#ifndef _HNS_ROCE_HEM_H
+#define _HNS_ROCE_HEM_H
+
+#define HW_SYNC_TIMEOUT_MSECS		500
+#define HW_SYNC_SLEEP_TIME_INTERVAL	20
+#define BT_CMD_SYNC_SHIFT		31
+
+enum {
+	/* MAP HEM(Hardware Entry Memory) */
+	HEM_TYPE_QPC = 0,
+	HEM_TYPE_MTPT,
+	HEM_TYPE_CQC,
+	HEM_TYPE_SRQC,
+
+	 /* UNMAP HEM */
+	HEM_TYPE_MTT,
+	HEM_TYPE_IRRL,
+};
+
+#define HNS_ROCE_HEM_CHUNK_LEN	\
+	 ((256 - sizeof(struct list_head) - 2 * sizeof(int)) /	 \
+	 (sizeof(struct scatterlist)))
+
+enum {
+	 HNS_ROCE_HEM_PAGE_SHIFT = 12,
+	 HNS_ROCE_HEM_PAGE_SIZE  = 1 << HNS_ROCE_HEM_PAGE_SHIFT,
+};
+
+struct hns_roce_hem_chunk {
+	struct list_head	 list;
+	int			 npages;
+	int			 nsg;
+	struct scatterlist	 mem[HNS_ROCE_HEM_CHUNK_LEN];
+};
+
+struct hns_roce_hem {
+	struct list_head	 chunk_list;
+	int			 refcount;
+};
+
+struct hns_roce_hem_iter {
+	struct hns_roce_hem		 *hem;
+	struct hns_roce_hem_chunk	 *chunk;
+	int				 page_idx;
+};
+
+void hns_roce_free_hem(struct hns_roce_dev *hr_dev, struct hns_roce_hem *hem);
+int hns_roce_table_get(struct hns_roce_dev *hr_dev,
+		       struct hns_roce_hem_table *table, unsigned long obj);
+void hns_roce_table_put(struct hns_roce_dev *hr_dev,
+			struct hns_roce_hem_table *table, unsigned long obj);
+void *hns_roce_table_find(struct hns_roce_hem_table *table, unsigned long obj,
+			  dma_addr_t *dma_handle);
+int hns_roce_table_get_range(struct hns_roce_dev *hr_dev,
+			     struct hns_roce_hem_table *table,
+			     unsigned long start, unsigned long end);
+void hns_roce_table_put_range(struct hns_roce_dev *hr_dev,
+			      struct hns_roce_hem_table *table,
+			      unsigned long start, unsigned long end);
+int hns_roce_init_hem_table(struct hns_roce_dev *hr_dev,
+			    struct hns_roce_hem_table *table, u32 type,
+			    unsigned long obj_size, unsigned long nobj,
+			    int use_lowmem);
+void hns_roce_cleanup_hem_table(struct hns_roce_dev *hr_dev,
+				struct hns_roce_hem_table *table);
+void hns_roce_cleanup_hem(struct hns_roce_dev *hr_dev);
+
+static inline void hns_roce_hem_first(struct hns_roce_hem *hem,
+				      struct hns_roce_hem_iter *iter)
+{
+	iter->hem = hem;
+	iter->chunk = list_empty(&hem->chunk_list) ? NULL :
+				 list_entry(hem->chunk_list.next,
+					    struct hns_roce_hem_chunk, list);
+	 iter->page_idx = 0;
+}
+
+static inline int hns_roce_hem_last(struct hns_roce_hem_iter *iter)
+{
+	return !iter->chunk;
+}
+
+static inline void hns_roce_hem_next(struct hns_roce_hem_iter *iter)
+{
+	if (++iter->page_idx >= iter->chunk->nsg) {
+		if (iter->chunk->list.next == &iter->hem->chunk_list) {
+			iter->chunk = NULL;
+			return;
+		}
+
+		iter->chunk = list_entry(iter->chunk->list.next,
+					 struct hns_roce_hem_chunk, list);
+		iter->page_idx = 0;
+	}
+}
+
+static inline dma_addr_t hns_roce_hem_addr(struct hns_roce_hem_iter *iter)
+{
+	return sg_dma_address(&iter->chunk->mem[iter->page_idx]);
+}
+
+#endif /*_HNS_ROCE_HEM_H*/
diff --git a/drivers/infiniband/hw/hns/hns_roce_hw_v1.c b/drivers/infiniband/hw/hns/hns_roce_hw_v1.c
new file mode 100644
index 000000000000..71232e5fabf6
--- /dev/null
+++ b/drivers/infiniband/hw/hns/hns_roce_hw_v1.c
@@ -0,0 +1,2921 @@
+/*
+ * Copyright (c) 2016 Hisilicon Limited.
+ *
+ * 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/platform_device.h>
+#include <linux/acpi.h>
+#include <rdma/ib_umem.h>
+#include "hns_roce_common.h"
+#include "hns_roce_device.h"
+#include "hns_roce_cmd.h"
+#include "hns_roce_hem.h"
+#include "hns_roce_hw_v1.h"
+
+static void set_data_seg(struct hns_roce_wqe_data_seg *dseg, struct ib_sge *sg)
+{
+	dseg->lkey = cpu_to_le32(sg->lkey);
+	dseg->addr = cpu_to_le64(sg->addr);
+	dseg->len  = cpu_to_le32(sg->length);
+}
+
+static void set_raddr_seg(struct hns_roce_wqe_raddr_seg *rseg, u64 remote_addr,
+			  u32 rkey)
+{
+	rseg->raddr = cpu_to_le64(remote_addr);
+	rseg->rkey  = cpu_to_le32(rkey);
+	rseg->len   = 0;
+}
+
+int hns_roce_v1_post_send(struct ib_qp *ibqp, struct ib_send_wr *wr,
+			  struct ib_send_wr **bad_wr)
+{
+	struct hns_roce_dev *hr_dev = to_hr_dev(ibqp->device);
+	struct hns_roce_ah *ah = to_hr_ah(ud_wr(wr)->ah);
+	struct hns_roce_ud_send_wqe *ud_sq_wqe = NULL;
+	struct hns_roce_wqe_ctrl_seg *ctrl = NULL;
+	struct hns_roce_wqe_data_seg *dseg = NULL;
+	struct hns_roce_qp *qp = to_hr_qp(ibqp);
+	struct device *dev = &hr_dev->pdev->dev;
+	struct hns_roce_sq_db sq_db;
+	int ps_opcode = 0, i = 0;
+	unsigned long flags = 0;
+	void *wqe = NULL;
+	u32 doorbell[2];
+	int nreq = 0;
+	u32 ind = 0;
+	int ret = 0;
+
+	if (unlikely(ibqp->qp_type != IB_QPT_GSI &&
+		ibqp->qp_type != IB_QPT_RC)) {
+		dev_err(dev, "un-supported QP type\n");
+		*bad_wr = NULL;
+		return -EOPNOTSUPP;
+	}
+
+	spin_lock_irqsave(&qp->sq.lock, flags);
+	ind = qp->sq_next_wqe;
+	for (nreq = 0; wr; ++nreq, wr = wr->next) {
+		if (hns_roce_wq_overflow(&qp->sq, nreq, qp->ibqp.send_cq)) {
+			ret = -ENOMEM;
+			*bad_wr = wr;
+			goto out;
+		}
+
+		if (unlikely(wr->num_sge > qp->sq.max_gs)) {
+			dev_err(dev, "num_sge=%d > qp->sq.max_gs=%d\n",
+				wr->num_sge, qp->sq.max_gs);
+			ret = -EINVAL;
+			*bad_wr = wr;
+			goto out;
+		}
+
+		wqe = get_send_wqe(qp, ind & (qp->sq.wqe_cnt - 1));
+		qp->sq.wrid[(qp->sq.head + nreq) & (qp->sq.wqe_cnt - 1)] =
+								      wr->wr_id;
+
+		/* Corresponding to the RC and RD type wqe process separately */
+		if (ibqp->qp_type == IB_QPT_GSI) {
+			ud_sq_wqe = wqe;
+			roce_set_field(ud_sq_wqe->dmac_h,
+				       UD_SEND_WQE_U32_4_DMAC_0_M,
+				       UD_SEND_WQE_U32_4_DMAC_0_S,
+				       ah->av.mac[0]);
+			roce_set_field(ud_sq_wqe->dmac_h,
+				       UD_SEND_WQE_U32_4_DMAC_1_M,
+				       UD_SEND_WQE_U32_4_DMAC_1_S,
+				       ah->av.mac[1]);
+			roce_set_field(ud_sq_wqe->dmac_h,
+				       UD_SEND_WQE_U32_4_DMAC_2_M,
+				       UD_SEND_WQE_U32_4_DMAC_2_S,
+				       ah->av.mac[2]);
+			roce_set_field(ud_sq_wqe->dmac_h,
+				       UD_SEND_WQE_U32_4_DMAC_3_M,
+				       UD_SEND_WQE_U32_4_DMAC_3_S,
+				       ah->av.mac[3]);
+
+			roce_set_field(ud_sq_wqe->u32_8,
+				       UD_SEND_WQE_U32_8_DMAC_4_M,
+				       UD_SEND_WQE_U32_8_DMAC_4_S,
+				       ah->av.mac[4]);
+			roce_set_field(ud_sq_wqe->u32_8,
+				       UD_SEND_WQE_U32_8_DMAC_5_M,
+				       UD_SEND_WQE_U32_8_DMAC_5_S,
+				       ah->av.mac[5]);
+			roce_set_field(ud_sq_wqe->u32_8,
+				       UD_SEND_WQE_U32_8_OPERATION_TYPE_M,
+				       UD_SEND_WQE_U32_8_OPERATION_TYPE_S,
+				       HNS_ROCE_WQE_OPCODE_SEND);
+			roce_set_field(ud_sq_wqe->u32_8,
+				       UD_SEND_WQE_U32_8_NUMBER_OF_DATA_SEG_M,
+				       UD_SEND_WQE_U32_8_NUMBER_OF_DATA_SEG_S,
+				       2);
+			roce_set_bit(ud_sq_wqe->u32_8,
+				UD_SEND_WQE_U32_8_SEND_GL_ROUTING_HDR_FLAG_S,
+				1);
+
+			ud_sq_wqe->u32_8 |= (wr->send_flags & IB_SEND_SIGNALED ?
+				cpu_to_le32(HNS_ROCE_WQE_CQ_NOTIFY) : 0) |
+				(wr->send_flags & IB_SEND_SOLICITED ?
+				cpu_to_le32(HNS_ROCE_WQE_SE) : 0) |
+				((wr->opcode == IB_WR_SEND_WITH_IMM) ?
+				cpu_to_le32(HNS_ROCE_WQE_IMM) : 0);
+
+			roce_set_field(ud_sq_wqe->u32_16,
+				       UD_SEND_WQE_U32_16_DEST_QP_M,
+				       UD_SEND_WQE_U32_16_DEST_QP_S,
+				       ud_wr(wr)->remote_qpn);
+			roce_set_field(ud_sq_wqe->u32_16,
+				       UD_SEND_WQE_U32_16_MAX_STATIC_RATE_M,
+				       UD_SEND_WQE_U32_16_MAX_STATIC_RATE_S,
+				       ah->av.stat_rate);
+
+			roce_set_field(ud_sq_wqe->u32_36,
+				       UD_SEND_WQE_U32_36_FLOW_LABEL_M,
+				       UD_SEND_WQE_U32_36_FLOW_LABEL_S, 0);
+			roce_set_field(ud_sq_wqe->u32_36,
+				       UD_SEND_WQE_U32_36_PRIORITY_M,
+				       UD_SEND_WQE_U32_36_PRIORITY_S,
+				       ah->av.sl_tclass_flowlabel >>
+				       HNS_ROCE_SL_SHIFT);
+			roce_set_field(ud_sq_wqe->u32_36,
+				       UD_SEND_WQE_U32_36_SGID_INDEX_M,
+				       UD_SEND_WQE_U32_36_SGID_INDEX_S,
+				       hns_get_gid_index(hr_dev, qp->phy_port,
+							 ah->av.gid_index));
+
+			roce_set_field(ud_sq_wqe->u32_40,
+				       UD_SEND_WQE_U32_40_HOP_LIMIT_M,
+				       UD_SEND_WQE_U32_40_HOP_LIMIT_S,
+				       ah->av.hop_limit);
+			roce_set_field(ud_sq_wqe->u32_40,
+				       UD_SEND_WQE_U32_40_TRAFFIC_CLASS_M,
+				       UD_SEND_WQE_U32_40_TRAFFIC_CLASS_S, 0);
+
+			memcpy(&ud_sq_wqe->dgid[0], &ah->av.dgid[0], GID_LEN);
+
+			ud_sq_wqe->va0_l = (u32)wr->sg_list[0].addr;
+			ud_sq_wqe->va0_h = (wr->sg_list[0].addr) >> 32;
+			ud_sq_wqe->l_key0 = wr->sg_list[0].lkey;
+
+			ud_sq_wqe->va1_l = (u32)wr->sg_list[1].addr;
+			ud_sq_wqe->va1_h = (wr->sg_list[1].addr) >> 32;
+			ud_sq_wqe->l_key1 = wr->sg_list[1].lkey;
+			ind++;
+		} else if (ibqp->qp_type == IB_QPT_RC) {
+			ctrl = wqe;
+			memset(ctrl, 0, sizeof(struct hns_roce_wqe_ctrl_seg));
+			for (i = 0; i < wr->num_sge; i++)
+				ctrl->msg_length += wr->sg_list[i].length;
+
+			ctrl->sgl_pa_h = 0;
+			ctrl->flag = 0;
+			ctrl->imm_data = send_ieth(wr);
+
+			/*Ctrl field, ctrl set type: sig, solic, imm, fence */
+			/* SO wait for conforming application scenarios */
+			ctrl->flag |= (wr->send_flags & IB_SEND_SIGNALED ?
+				      cpu_to_le32(HNS_ROCE_WQE_CQ_NOTIFY) : 0) |
+				      (wr->send_flags & IB_SEND_SOLICITED ?
+				      cpu_to_le32(HNS_ROCE_WQE_SE) : 0) |
+				      ((wr->opcode == IB_WR_SEND_WITH_IMM ||
+				      wr->opcode == IB_WR_RDMA_WRITE_WITH_IMM) ?
+				      cpu_to_le32(HNS_ROCE_WQE_IMM) : 0) |
+				      (wr->send_flags & IB_SEND_FENCE ?
+				      (cpu_to_le32(HNS_ROCE_WQE_FENCE)) : 0);
+
+			wqe += sizeof(struct hns_roce_wqe_ctrl_seg);
+
+			switch (wr->opcode) {
+			case IB_WR_RDMA_READ:
+				ps_opcode = HNS_ROCE_WQE_OPCODE_RDMA_READ;
+				set_raddr_seg(wqe, atomic_wr(wr)->remote_addr,
+					      atomic_wr(wr)->rkey);
+				break;
+			case IB_WR_RDMA_WRITE:
+			case IB_WR_RDMA_WRITE_WITH_IMM:
+				ps_opcode = HNS_ROCE_WQE_OPCODE_RDMA_WRITE;
+				set_raddr_seg(wqe, atomic_wr(wr)->remote_addr,
+					      atomic_wr(wr)->rkey);
+				break;
+			case IB_WR_SEND:
+			case IB_WR_SEND_WITH_INV:
+			case IB_WR_SEND_WITH_IMM:
+				ps_opcode = HNS_ROCE_WQE_OPCODE_SEND;
+				break;
+			case IB_WR_LOCAL_INV:
+				break;
+			case IB_WR_ATOMIC_CMP_AND_SWP:
+			case IB_WR_ATOMIC_FETCH_AND_ADD:
+			case IB_WR_LSO:
+			default:
+				ps_opcode = HNS_ROCE_WQE_OPCODE_MASK;
+				break;
+			}
+			ctrl->flag |= cpu_to_le32(ps_opcode);
+			wqe += sizeof(struct hns_roce_wqe_raddr_seg);
+
+			dseg = wqe;
+			if (wr->send_flags & IB_SEND_INLINE && wr->num_sge) {
+				if (ctrl->msg_length >
+					hr_dev->caps.max_sq_inline) {
+					ret = -EINVAL;
+					*bad_wr = wr;
+					dev_err(dev, "inline len(1-%d)=%d, illegal",
+						ctrl->msg_length,
+						hr_dev->caps.max_sq_inline);
+					goto out;
+				}
+				for (i = 0; i < wr->num_sge; i++) {
+					memcpy(wqe, ((void *) (uintptr_t)
+					       wr->sg_list[i].addr),
+					       wr->sg_list[i].length);
+					wqe += wr->sg_list[i].length;
+				}
+				ctrl->flag |= HNS_ROCE_WQE_INLINE;
+			} else {
+				/*sqe num is two */
+				for (i = 0; i < wr->num_sge; i++)
+					set_data_seg(dseg + i, wr->sg_list + i);
+
+				ctrl->flag |= cpu_to_le32(wr->num_sge <<
+					      HNS_ROCE_WQE_SGE_NUM_BIT);
+			}
+			ind++;
+		}
+	}
+
+out:
+	/* Set DB return */
+	if (likely(nreq)) {
+		qp->sq.head += nreq;
+		/* Memory barrier */
+		wmb();
+
+		sq_db.u32_4 = 0;
+		sq_db.u32_8 = 0;
+		roce_set_field(sq_db.u32_4, SQ_DOORBELL_U32_4_SQ_HEAD_M,
+			       SQ_DOORBELL_U32_4_SQ_HEAD_S,
+			      (qp->sq.head & ((qp->sq.wqe_cnt << 1) - 1)));
+		roce_set_field(sq_db.u32_4, SQ_DOORBELL_U32_4_PORT_M,
+			       SQ_DOORBELL_U32_4_PORT_S, qp->phy_port);
+		roce_set_field(sq_db.u32_8, SQ_DOORBELL_U32_8_QPN_M,
+			       SQ_DOORBELL_U32_8_QPN_S, qp->doorbell_qpn);
+		roce_set_bit(sq_db.u32_8, SQ_DOORBELL_HW_SYNC_S, 1);
+
+		doorbell[0] = sq_db.u32_4;
+		doorbell[1] = sq_db.u32_8;
+
+		hns_roce_write64_k(doorbell, qp->sq.db_reg_l);
+		qp->sq_next_wqe = ind;
+	}
+
+	spin_unlock_irqrestore(&qp->sq.lock, flags);
+
+	return ret;
+}
+
+int hns_roce_v1_post_recv(struct ib_qp *ibqp, struct ib_recv_wr *wr,
+			  struct ib_recv_wr **bad_wr)
+{
+	int ret = 0;
+	int nreq = 0;
+	int ind = 0;
+	int i = 0;
+	u32 reg_val = 0;
+	unsigned long flags = 0;
+	struct hns_roce_rq_wqe_ctrl *ctrl = NULL;
+	struct hns_roce_wqe_data_seg *scat = NULL;
+	struct hns_roce_qp *hr_qp = to_hr_qp(ibqp);
+	struct hns_roce_dev *hr_dev = to_hr_dev(ibqp->device);
+	struct device *dev = &hr_dev->pdev->dev;
+	struct hns_roce_rq_db rq_db;
+	uint32_t doorbell[2] = {0};
+
+	spin_lock_irqsave(&hr_qp->rq.lock, flags);
+	ind = hr_qp->rq.head & (hr_qp->rq.wqe_cnt - 1);
+
+	for (nreq = 0; wr; ++nreq, wr = wr->next) {
+		if (hns_roce_wq_overflow(&hr_qp->rq, nreq,
+			hr_qp->ibqp.recv_cq)) {
+			ret = -ENOMEM;
+			*bad_wr = wr;
+			goto out;
+		}
+
+		if (unlikely(wr->num_sge > hr_qp->rq.max_gs)) {
+			dev_err(dev, "rq:num_sge=%d > qp->sq.max_gs=%d\n",
+				wr->num_sge, hr_qp->rq.max_gs);
+			ret = -EINVAL;
+			*bad_wr = wr;
+			goto out;
+		}
+
+		ctrl = get_recv_wqe(hr_qp, ind);
+
+		roce_set_field(ctrl->rwqe_byte_12,
+			       RQ_WQE_CTRL_RWQE_BYTE_12_RWQE_SGE_NUM_M,
+			       RQ_WQE_CTRL_RWQE_BYTE_12_RWQE_SGE_NUM_S,
+			       wr->num_sge);
+
+		scat = (struct hns_roce_wqe_data_seg *)(ctrl + 1);
+
+		for (i = 0; i < wr->num_sge; i++)
+			set_data_seg(scat + i, wr->sg_list + i);
+
+		hr_qp->rq.wrid[ind] = wr->wr_id;
+
+		ind = (ind + 1) & (hr_qp->rq.wqe_cnt - 1);
+	}
+
+out:
+	if (likely(nreq)) {
+		hr_qp->rq.head += nreq;
+		/* Memory barrier */
+		wmb();
+
+		if (ibqp->qp_type == IB_QPT_GSI) {
+			/* SW update GSI rq header */
+			reg_val = roce_read(to_hr_dev(ibqp->device),
+					    ROCEE_QP1C_CFG3_0_REG +
+					    QP1C_CFGN_OFFSET * hr_qp->phy_port);
+			roce_set_field(reg_val,
+				       ROCEE_QP1C_CFG3_0_ROCEE_QP1C_RQ_HEAD_M,
+				       ROCEE_QP1C_CFG3_0_ROCEE_QP1C_RQ_HEAD_S,
+				       hr_qp->rq.head);
+			roce_write(to_hr_dev(ibqp->device),
+				   ROCEE_QP1C_CFG3_0_REG +
+				   QP1C_CFGN_OFFSET * hr_qp->phy_port, reg_val);
+		} else {
+			rq_db.u32_4 = 0;
+			rq_db.u32_8 = 0;
+
+			roce_set_field(rq_db.u32_4, RQ_DOORBELL_U32_4_RQ_HEAD_M,
+				       RQ_DOORBELL_U32_4_RQ_HEAD_S,
+				       hr_qp->rq.head);
+			roce_set_field(rq_db.u32_8, RQ_DOORBELL_U32_8_QPN_M,
+				       RQ_DOORBELL_U32_8_QPN_S, hr_qp->qpn);
+			roce_set_field(rq_db.u32_8, RQ_DOORBELL_U32_8_CMD_M,
+				       RQ_DOORBELL_U32_8_CMD_S, 1);
+			roce_set_bit(rq_db.u32_8, RQ_DOORBELL_U32_8_HW_SYNC_S,
+				     1);
+
+			doorbell[0] = rq_db.u32_4;
+			doorbell[1] = rq_db.u32_8;
+
+			hns_roce_write64_k(doorbell, hr_qp->rq.db_reg_l);
+		}
+	}
+	spin_unlock_irqrestore(&hr_qp->rq.lock, flags);
+
+	return ret;
+}
+
+static void hns_roce_set_db_event_mode(struct hns_roce_dev *hr_dev,
+				       int sdb_mode, int odb_mode)
+{
+	u32 val;
+
+	val = roce_read(hr_dev, ROCEE_GLB_CFG_REG);
+	roce_set_bit(val, ROCEE_GLB_CFG_ROCEE_DB_SQ_MODE_S, sdb_mode);
+	roce_set_bit(val, ROCEE_GLB_CFG_ROCEE_DB_OTH_MODE_S, odb_mode);
+	roce_write(hr_dev, ROCEE_GLB_CFG_REG, val);
+}
+
+static void hns_roce_set_db_ext_mode(struct hns_roce_dev *hr_dev, u32 sdb_mode,
+				     u32 odb_mode)
+{
+	u32 val;
+
+	/* Configure SDB/ODB extend mode */
+	val = roce_read(hr_dev, ROCEE_GLB_CFG_REG);
+	roce_set_bit(val, ROCEE_GLB_CFG_SQ_EXT_DB_MODE_S, sdb_mode);
+	roce_set_bit(val, ROCEE_GLB_CFG_OTH_EXT_DB_MODE_S, odb_mode);
+	roce_write(hr_dev, ROCEE_GLB_CFG_REG, val);
+}
+
+static void hns_roce_set_sdb(struct hns_roce_dev *hr_dev, u32 sdb_alept,
+			     u32 sdb_alful)
+{
+	u32 val;
+
+	/* Configure SDB */
+	val = roce_read(hr_dev, ROCEE_DB_SQ_WL_REG);
+	roce_set_field(val, ROCEE_DB_SQ_WL_ROCEE_DB_SQ_WL_M,
+		       ROCEE_DB_SQ_WL_ROCEE_DB_SQ_WL_S, sdb_alful);
+	roce_set_field(val, ROCEE_DB_SQ_WL_ROCEE_DB_SQ_WL_EMPTY_M,
+		       ROCEE_DB_SQ_WL_ROCEE_DB_SQ_WL_EMPTY_S, sdb_alept);
+	roce_write(hr_dev, ROCEE_DB_SQ_WL_REG, val);
+}
+
+static void hns_roce_set_odb(struct hns_roce_dev *hr_dev, u32 odb_alept,
+			     u32 odb_alful)
+{
+	u32 val;
+
+	/* Configure ODB */
+	val = roce_read(hr_dev, ROCEE_DB_OTHERS_WL_REG);
+	roce_set_field(val, ROCEE_DB_OTHERS_WL_ROCEE_DB_OTH_WL_M,
+		       ROCEE_DB_OTHERS_WL_ROCEE_DB_OTH_WL_S, odb_alful);
+	roce_set_field(val, ROCEE_DB_OTHERS_WL_ROCEE_DB_OTH_WL_EMPTY_M,
+		       ROCEE_DB_OTHERS_WL_ROCEE_DB_OTH_WL_EMPTY_S, odb_alept);
+	roce_write(hr_dev, ROCEE_DB_OTHERS_WL_REG, val);
+}
+
+static void hns_roce_set_sdb_ext(struct hns_roce_dev *hr_dev, u32 ext_sdb_alept,
+				 u32 ext_sdb_alful)
+{
+	struct device *dev = &hr_dev->pdev->dev;
+	struct hns_roce_v1_priv *priv;
+	struct hns_roce_db_table *db;
+	dma_addr_t sdb_dma_addr;
+	u32 val;
+
+	priv = (struct hns_roce_v1_priv *)hr_dev->hw->priv;
+	db = &priv->db_table;
+
+	/* Configure extend SDB threshold */
+	roce_write(hr_dev, ROCEE_EXT_DB_SQ_WL_EMPTY_REG, ext_sdb_alept);
+	roce_write(hr_dev, ROCEE_EXT_DB_SQ_WL_REG, ext_sdb_alful);
+
+	/* Configure extend SDB base addr */
+	sdb_dma_addr = db->ext_db->sdb_buf_list->map;
+	roce_write(hr_dev, ROCEE_EXT_DB_SQ_REG, (u32)(sdb_dma_addr >> 12));
+
+	/* Configure extend SDB depth */
+	val = roce_read(hr_dev, ROCEE_EXT_DB_SQ_H_REG);
+	roce_set_field(val, ROCEE_EXT_DB_SQ_H_EXT_DB_SQ_SHIFT_M,
+		       ROCEE_EXT_DB_SQ_H_EXT_DB_SQ_SHIFT_S,
+		       db->ext_db->esdb_dep);
+	/*
+	 * 44 = 32 + 12, When evaluating addr to hardware, shift 12 because of
+	 * using 4K page, and shift more 32 because of
+	 * caculating the high 32 bit value evaluated to hardware.
+	 */
+	roce_set_field(val, ROCEE_EXT_DB_SQ_H_EXT_DB_SQ_BA_H_M,
+		       ROCEE_EXT_DB_SQ_H_EXT_DB_SQ_BA_H_S, sdb_dma_addr >> 44);
+	roce_write(hr_dev, ROCEE_EXT_DB_SQ_H_REG, val);
+
+	dev_dbg(dev, "ext SDB depth: 0x%x\n", db->ext_db->esdb_dep);
+	dev_dbg(dev, "ext SDB threshold: epmty: 0x%x, ful: 0x%x\n",
+		ext_sdb_alept, ext_sdb_alful);
+}
+
+static void hns_roce_set_odb_ext(struct hns_roce_dev *hr_dev, u32 ext_odb_alept,
+				 u32 ext_odb_alful)
+{
+	struct device *dev = &hr_dev->pdev->dev;
+	struct hns_roce_v1_priv *priv;
+	struct hns_roce_db_table *db;
+	dma_addr_t odb_dma_addr;
+	u32 val;
+
+	priv = (struct hns_roce_v1_priv *)hr_dev->hw->priv;
+	db = &priv->db_table;
+
+	/* Configure extend ODB threshold */
+	roce_write(hr_dev, ROCEE_EXT_DB_OTHERS_WL_EMPTY_REG, ext_odb_alept);
+	roce_write(hr_dev, ROCEE_EXT_DB_OTHERS_WL_REG, ext_odb_alful);
+
+	/* Configure extend ODB base addr */
+	odb_dma_addr = db->ext_db->odb_buf_list->map;
+	roce_write(hr_dev, ROCEE_EXT_DB_OTH_REG, (u32)(odb_dma_addr >> 12));
+
+	/* Configure extend ODB depth */
+	val = roce_read(hr_dev, ROCEE_EXT_DB_OTH_H_REG);
+	roce_set_field(val, ROCEE_EXT_DB_OTH_H_EXT_DB_OTH_SHIFT_M,
+		       ROCEE_EXT_DB_OTH_H_EXT_DB_OTH_SHIFT_S,
+		       db->ext_db->eodb_dep);
+	roce_set_field(val, ROCEE_EXT_DB_SQ_H_EXT_DB_OTH_BA_H_M,
+		       ROCEE_EXT_DB_SQ_H_EXT_DB_OTH_BA_H_S,
+		       db->ext_db->eodb_dep);
+	roce_write(hr_dev, ROCEE_EXT_DB_OTH_H_REG, val);
+
+	dev_dbg(dev, "ext ODB depth: 0x%x\n", db->ext_db->eodb_dep);
+	dev_dbg(dev, "ext ODB threshold: empty: 0x%x, ful: 0x%x\n",
+		ext_odb_alept, ext_odb_alful);
+}
+
+static int hns_roce_db_ext_init(struct hns_roce_dev *hr_dev, u32 sdb_ext_mod,
+				u32 odb_ext_mod)
+{
+	struct device *dev = &hr_dev->pdev->dev;
+	struct hns_roce_v1_priv *priv;
+	struct hns_roce_db_table *db;
+	dma_addr_t sdb_dma_addr;
+	dma_addr_t odb_dma_addr;
+	int ret = 0;
+
+	priv = (struct hns_roce_v1_priv *)hr_dev->hw->priv;
+	db = &priv->db_table;
+
+	db->ext_db = kmalloc(sizeof(*db->ext_db), GFP_KERNEL);
+	if (!db->ext_db)
+		return -ENOMEM;
+
+	if (sdb_ext_mod) {
+		db->ext_db->sdb_buf_list = kmalloc(
+				sizeof(*db->ext_db->sdb_buf_list), GFP_KERNEL);
+		if (!db->ext_db->sdb_buf_list) {
+			ret = -ENOMEM;
+			goto ext_sdb_buf_fail_out;
+		}
+
+		db->ext_db->sdb_buf_list->buf = dma_alloc_coherent(dev,
+						     HNS_ROCE_V1_EXT_SDB_SIZE,
+						     &sdb_dma_addr, GFP_KERNEL);
+		if (!db->ext_db->sdb_buf_list->buf) {
+			ret = -ENOMEM;
+			goto alloc_sq_db_buf_fail;
+		}
+		db->ext_db->sdb_buf_list->map = sdb_dma_addr;
+
+		db->ext_db->esdb_dep = ilog2(HNS_ROCE_V1_EXT_SDB_DEPTH);
+		hns_roce_set_sdb_ext(hr_dev, HNS_ROCE_V1_EXT_SDB_ALEPT,
+				     HNS_ROCE_V1_EXT_SDB_ALFUL);
+	} else
+		hns_roce_set_sdb(hr_dev, HNS_ROCE_V1_SDB_ALEPT,
+				 HNS_ROCE_V1_SDB_ALFUL);
+
+	if (odb_ext_mod) {
+		db->ext_db->odb_buf_list = kmalloc(
+				sizeof(*db->ext_db->odb_buf_list), GFP_KERNEL);
+		if (!db->ext_db->odb_buf_list) {
+			ret = -ENOMEM;
+			goto ext_odb_buf_fail_out;
+		}
+
+		db->ext_db->odb_buf_list->buf = dma_alloc_coherent(dev,
+						     HNS_ROCE_V1_EXT_ODB_SIZE,
+						     &odb_dma_addr, GFP_KERNEL);
+		if (!db->ext_db->odb_buf_list->buf) {
+			ret = -ENOMEM;
+			goto alloc_otr_db_buf_fail;
+		}
+		db->ext_db->odb_buf_list->map = odb_dma_addr;
+
+		db->ext_db->eodb_dep = ilog2(HNS_ROCE_V1_EXT_ODB_DEPTH);
+		hns_roce_set_odb_ext(hr_dev, HNS_ROCE_V1_EXT_ODB_ALEPT,
+				     HNS_ROCE_V1_EXT_ODB_ALFUL);
+	} else
+		hns_roce_set_odb(hr_dev, HNS_ROCE_V1_ODB_ALEPT,
+				 HNS_ROCE_V1_ODB_ALFUL);
+
+	hns_roce_set_db_ext_mode(hr_dev, sdb_ext_mod, odb_ext_mod);
+
+	return 0;
+
+alloc_otr_db_buf_fail:
+	kfree(db->ext_db->odb_buf_list);
+
+ext_odb_buf_fail_out:
+	if (sdb_ext_mod) {
+		dma_free_coherent(dev, HNS_ROCE_V1_EXT_SDB_SIZE,
+				  db->ext_db->sdb_buf_list->buf,
+				  db->ext_db->sdb_buf_list->map);
+	}
+
+alloc_sq_db_buf_fail:
+	if (sdb_ext_mod)
+		kfree(db->ext_db->sdb_buf_list);
+
+ext_sdb_buf_fail_out:
+	kfree(db->ext_db);
+	return ret;
+}
+
+static int hns_roce_db_init(struct hns_roce_dev *hr_dev)
+{
+	struct device *dev = &hr_dev->pdev->dev;
+	struct hns_roce_v1_priv *priv;
+	struct hns_roce_db_table *db;
+	u32 sdb_ext_mod;
+	u32 odb_ext_mod;
+	u32 sdb_evt_mod;
+	u32 odb_evt_mod;
+	int ret = 0;
+
+	priv = (struct hns_roce_v1_priv *)hr_dev->hw->priv;
+	db = &priv->db_table;
+
+	memset(db, 0, sizeof(*db));
+
+	/* Default DB mode */
+	sdb_ext_mod = HNS_ROCE_SDB_EXTEND_MODE;
+	odb_ext_mod = HNS_ROCE_ODB_EXTEND_MODE;
+	sdb_evt_mod = HNS_ROCE_SDB_NORMAL_MODE;
+	odb_evt_mod = HNS_ROCE_ODB_POLL_MODE;
+
+	db->sdb_ext_mod = sdb_ext_mod;
+	db->odb_ext_mod = odb_ext_mod;
+
+	/* Init extend DB */
+	ret = hns_roce_db_ext_init(hr_dev, sdb_ext_mod, odb_ext_mod);
+	if (ret) {
+		dev_err(dev, "Failed in extend DB configuration.\n");
+		return ret;
+	}
+
+	hns_roce_set_db_event_mode(hr_dev, sdb_evt_mod, odb_evt_mod);
+
+	return 0;
+}
+
+static void hns_roce_db_free(struct hns_roce_dev *hr_dev)
+{
+	struct device *dev = &hr_dev->pdev->dev;
+	struct hns_roce_v1_priv *priv;
+	struct hns_roce_db_table *db;
+
+	priv = (struct hns_roce_v1_priv *)hr_dev->hw->priv;
+	db = &priv->db_table;
+
+	if (db->sdb_ext_mod) {
+		dma_free_coherent(dev, HNS_ROCE_V1_EXT_SDB_SIZE,
+				  db->ext_db->sdb_buf_list->buf,
+				  db->ext_db->sdb_buf_list->map);
+		kfree(db->ext_db->sdb_buf_list);
+	}
+
+	if (db->odb_ext_mod) {
+		dma_free_coherent(dev, HNS_ROCE_V1_EXT_ODB_SIZE,
+				  db->ext_db->odb_buf_list->buf,
+				  db->ext_db->odb_buf_list->map);
+		kfree(db->ext_db->odb_buf_list);
+	}
+
+	kfree(db->ext_db);
+}
+
+static int hns_roce_raq_init(struct hns_roce_dev *hr_dev)
+{
+	int ret;
+	int raq_shift = 0;
+	dma_addr_t addr;
+	u32 val;
+	struct hns_roce_v1_priv *priv;
+	struct hns_roce_raq_table *raq;
+	struct device *dev = &hr_dev->pdev->dev;
+
+	priv = (struct hns_roce_v1_priv *)hr_dev->hw->priv;
+	raq = &priv->raq_table;
+
+	raq->e_raq_buf = kzalloc(sizeof(*(raq->e_raq_buf)), GFP_KERNEL);
+	if (!raq->e_raq_buf)
+		return -ENOMEM;
+
+	raq->e_raq_buf->buf = dma_alloc_coherent(dev, HNS_ROCE_V1_RAQ_SIZE,
+						 &addr, GFP_KERNEL);
+	if (!raq->e_raq_buf->buf) {
+		ret = -ENOMEM;
+		goto err_dma_alloc_raq;
+	}
+	raq->e_raq_buf->map = addr;
+
+	/* Configure raq extended address. 48bit 4K align*/
+	roce_write(hr_dev, ROCEE_EXT_RAQ_REG, raq->e_raq_buf->map >> 12);
+
+	/* Configure raq_shift */
+	raq_shift = ilog2(HNS_ROCE_V1_RAQ_SIZE / HNS_ROCE_V1_RAQ_ENTRY);
+	val = roce_read(hr_dev, ROCEE_EXT_RAQ_H_REG);
+	roce_set_field(val, ROCEE_EXT_RAQ_H_EXT_RAQ_SHIFT_M,
+		       ROCEE_EXT_RAQ_H_EXT_RAQ_SHIFT_S, raq_shift);
+	/*
+	 * 44 = 32 + 12, When evaluating addr to hardware, shift 12 because of
+	 * using 4K page, and shift more 32 because of
+	 * caculating the high 32 bit value evaluated to hardware.
+	 */
+	roce_set_field(val, ROCEE_EXT_RAQ_H_EXT_RAQ_BA_H_M,
+		       ROCEE_EXT_RAQ_H_EXT_RAQ_BA_H_S,
+		       raq->e_raq_buf->map >> 44);
+	roce_write(hr_dev, ROCEE_EXT_RAQ_H_REG, val);
+	dev_dbg(dev, "Configure raq_shift 0x%x.\n", val);
+
+	/* Configure raq threshold */
+	val = roce_read(hr_dev, ROCEE_RAQ_WL_REG);
+	roce_set_field(val, ROCEE_RAQ_WL_ROCEE_RAQ_WL_M,
+		       ROCEE_RAQ_WL_ROCEE_RAQ_WL_S,
+		       HNS_ROCE_V1_EXT_RAQ_WF);
+	roce_write(hr_dev, ROCEE_RAQ_WL_REG, val);
+	dev_dbg(dev, "Configure raq_wl 0x%x.\n", val);
+
+	/* Enable extend raq */
+	val = roce_read(hr_dev, ROCEE_WRMS_POL_TIME_INTERVAL_REG);
+	roce_set_field(val,
+		       ROCEE_WRMS_POL_TIME_INTERVAL_WRMS_POL_TIME_INTERVAL_M,
+		       ROCEE_WRMS_POL_TIME_INTERVAL_WRMS_POL_TIME_INTERVAL_S,
+		       POL_TIME_INTERVAL_VAL);
+	roce_set_bit(val, ROCEE_WRMS_POL_TIME_INTERVAL_WRMS_EXT_RAQ_MODE, 1);
+	roce_set_field(val,
+		       ROCEE_WRMS_POL_TIME_INTERVAL_WRMS_RAQ_TIMEOUT_CHK_CFG_M,
+		       ROCEE_WRMS_POL_TIME_INTERVAL_WRMS_RAQ_TIMEOUT_CHK_CFG_S,
+		       2);
+	roce_set_bit(val,
+		     ROCEE_WRMS_POL_TIME_INTERVAL_WRMS_RAQ_TIMEOUT_CHK_EN_S, 1);
+	roce_write(hr_dev, ROCEE_WRMS_POL_TIME_INTERVAL_REG, val);
+	dev_dbg(dev, "Configure WrmsPolTimeInterval 0x%x.\n", val);
+
+	/* Enable raq drop */
+	val = roce_read(hr_dev, ROCEE_GLB_CFG_REG);
+	roce_set_bit(val, ROCEE_GLB_CFG_TRP_RAQ_DROP_EN_S, 1);
+	roce_write(hr_dev, ROCEE_GLB_CFG_REG, val);
+	dev_dbg(dev, "Configure GlbCfg = 0x%x.\n", val);
+
+	return 0;
+
+err_dma_alloc_raq:
+	kfree(raq->e_raq_buf);
+	return ret;
+}
+
+static void hns_roce_raq_free(struct hns_roce_dev *hr_dev)
+{
+	struct device *dev = &hr_dev->pdev->dev;
+	struct hns_roce_v1_priv *priv;
+	struct hns_roce_raq_table *raq;
+
+	priv = (struct hns_roce_v1_priv *)hr_dev->hw->priv;
+	raq = &priv->raq_table;
+
+	dma_free_coherent(dev, HNS_ROCE_V1_RAQ_SIZE, raq->e_raq_buf->buf,
+			  raq->e_raq_buf->map);
+	kfree(raq->e_raq_buf);
+}
+
+static void hns_roce_port_enable(struct hns_roce_dev *hr_dev, int enable_flag)
+{
+	u32 val;
+
+	if (enable_flag) {
+		val = roce_read(hr_dev, ROCEE_GLB_CFG_REG);
+		 /* Open all ports */
+		roce_set_field(val, ROCEE_GLB_CFG_ROCEE_PORT_ST_M,
+			       ROCEE_GLB_CFG_ROCEE_PORT_ST_S,
+			       ALL_PORT_VAL_OPEN);
+		roce_write(hr_dev, ROCEE_GLB_CFG_REG, val);
+	} else {
+		val = roce_read(hr_dev, ROCEE_GLB_CFG_REG);
+		/* Close all ports */
+		roce_set_field(val, ROCEE_GLB_CFG_ROCEE_PORT_ST_M,
+			       ROCEE_GLB_CFG_ROCEE_PORT_ST_S, 0x0);
+		roce_write(hr_dev, ROCEE_GLB_CFG_REG, val);
+	}
+}
+
+static int hns_roce_bt_init(struct hns_roce_dev *hr_dev)
+{
+	struct device *dev = &hr_dev->pdev->dev;
+	struct hns_roce_v1_priv *priv;
+	int ret;
+
+	priv = (struct hns_roce_v1_priv *)hr_dev->hw->priv;
+
+	priv->bt_table.qpc_buf.buf = dma_alloc_coherent(dev,
+		HNS_ROCE_BT_RSV_BUF_SIZE, &priv->bt_table.qpc_buf.map,
+		GFP_KERNEL);
+	if (!priv->bt_table.qpc_buf.buf)
+		return -ENOMEM;
+
+	priv->bt_table.mtpt_buf.buf = dma_alloc_coherent(dev,
+		HNS_ROCE_BT_RSV_BUF_SIZE, &priv->bt_table.mtpt_buf.map,
+		GFP_KERNEL);
+	if (!priv->bt_table.mtpt_buf.buf) {
+		ret = -ENOMEM;
+		goto err_failed_alloc_mtpt_buf;
+	}
+
+	priv->bt_table.cqc_buf.buf = dma_alloc_coherent(dev,
+		HNS_ROCE_BT_RSV_BUF_SIZE, &priv->bt_table.cqc_buf.map,
+		GFP_KERNEL);
+	if (!priv->bt_table.cqc_buf.buf) {
+		ret = -ENOMEM;
+		goto err_failed_alloc_cqc_buf;
+	}
+
+	return 0;
+
+err_failed_alloc_cqc_buf:
+	dma_free_coherent(dev, HNS_ROCE_BT_RSV_BUF_SIZE,
+		priv->bt_table.mtpt_buf.buf, priv->bt_table.mtpt_buf.map);
+
+err_failed_alloc_mtpt_buf:
+	dma_free_coherent(dev, HNS_ROCE_BT_RSV_BUF_SIZE,
+		priv->bt_table.qpc_buf.buf, priv->bt_table.qpc_buf.map);
+
+	return ret;
+}
+
+static void hns_roce_bt_free(struct hns_roce_dev *hr_dev)
+{
+	struct device *dev = &hr_dev->pdev->dev;
+	struct hns_roce_v1_priv *priv;
+
+	priv = (struct hns_roce_v1_priv *)hr_dev->hw->priv;
+
+	dma_free_coherent(dev, HNS_ROCE_BT_RSV_BUF_SIZE,
+		priv->bt_table.cqc_buf.buf, priv->bt_table.cqc_buf.map);
+
+	dma_free_coherent(dev, HNS_ROCE_BT_RSV_BUF_SIZE,
+		priv->bt_table.mtpt_buf.buf, priv->bt_table.mtpt_buf.map);
+
+	dma_free_coherent(dev, HNS_ROCE_BT_RSV_BUF_SIZE,
+		priv->bt_table.qpc_buf.buf, priv->bt_table.qpc_buf.map);
+}
+
+/**
+ * hns_roce_v1_reset - reset RoCE
+ * @hr_dev: RoCE device struct pointer
+ * @enable: true -- drop reset, false -- reset
+ * return 0 - success , negative --fail
+ */
+int hns_roce_v1_reset(struct hns_roce_dev *hr_dev, bool dereset)
+{
+	struct device_node *dsaf_node;
+	struct device *dev = &hr_dev->pdev->dev;
+	struct device_node *np = dev->of_node;
+	struct fwnode_handle *fwnode;
+	int ret;
+
+	/* check if this is DT/ACPI case */
+	if (dev_of_node(dev)) {
+		dsaf_node = of_parse_phandle(np, "dsaf-handle", 0);
+		if (!dsaf_node) {
+			dev_err(dev, "could not find dsaf-handle\n");
+			return -EINVAL;
+		}
+		fwnode = &dsaf_node->fwnode;
+	} else if (is_acpi_device_node(dev->fwnode)) {
+		struct acpi_reference_args args;
+
+		ret = acpi_node_get_property_reference(dev->fwnode,
+						       "dsaf-handle", 0, &args);
+		if (ret) {
+			dev_err(dev, "could not find dsaf-handle\n");
+			return ret;
+		}
+		fwnode = acpi_fwnode_handle(args.adev);
+	} else {
+		dev_err(dev, "cannot read data from DT or ACPI\n");
+		return -ENXIO;
+	}
+
+	ret = hns_dsaf_roce_reset(fwnode, false);
+	if (ret)
+		return ret;
+
+	if (dereset) {
+		msleep(SLEEP_TIME_INTERVAL);
+		ret = hns_dsaf_roce_reset(fwnode, true);
+	}
+
+	return ret;
+}
+
+void hns_roce_v1_profile(struct hns_roce_dev *hr_dev)
+{
+	int i = 0;
+	struct hns_roce_caps *caps = &hr_dev->caps;
+
+	hr_dev->vendor_id = le32_to_cpu(roce_read(hr_dev, ROCEE_VENDOR_ID_REG));
+	hr_dev->vendor_part_id = le32_to_cpu(roce_read(hr_dev,
+					     ROCEE_VENDOR_PART_ID_REG));
+	hr_dev->hw_rev = le32_to_cpu(roce_read(hr_dev, ROCEE_HW_VERSION_REG));
+
+	hr_dev->sys_image_guid = le32_to_cpu(roce_read(hr_dev,
+					     ROCEE_SYS_IMAGE_GUID_L_REG)) |
+				((u64)le32_to_cpu(roce_read(hr_dev,
+					    ROCEE_SYS_IMAGE_GUID_H_REG)) << 32);
+
+	caps->num_qps		= HNS_ROCE_V1_MAX_QP_NUM;
+	caps->max_wqes		= HNS_ROCE_V1_MAX_WQE_NUM;
+	caps->num_cqs		= HNS_ROCE_V1_MAX_CQ_NUM;
+	caps->max_cqes		= HNS_ROCE_V1_MAX_CQE_NUM;
+	caps->max_sq_sg		= HNS_ROCE_V1_SG_NUM;
+	caps->max_rq_sg		= HNS_ROCE_V1_SG_NUM;
+	caps->max_sq_inline	= HNS_ROCE_V1_INLINE_SIZE;
+	caps->num_uars		= HNS_ROCE_V1_UAR_NUM;
+	caps->phy_num_uars	= HNS_ROCE_V1_PHY_UAR_NUM;
+	caps->num_aeq_vectors	= HNS_ROCE_AEQE_VEC_NUM;
+	caps->num_comp_vectors	= HNS_ROCE_COMP_VEC_NUM;
+	caps->num_other_vectors	= HNS_ROCE_AEQE_OF_VEC_NUM;
+	caps->num_mtpts		= HNS_ROCE_V1_MAX_MTPT_NUM;
+	caps->num_mtt_segs	= HNS_ROCE_V1_MAX_MTT_SEGS;
+	caps->num_pds		= HNS_ROCE_V1_MAX_PD_NUM;
+	caps->max_qp_init_rdma	= HNS_ROCE_V1_MAX_QP_INIT_RDMA;
+	caps->max_qp_dest_rdma	= HNS_ROCE_V1_MAX_QP_DEST_RDMA;
+	caps->max_sq_desc_sz	= HNS_ROCE_V1_MAX_SQ_DESC_SZ;
+	caps->max_rq_desc_sz	= HNS_ROCE_V1_MAX_RQ_DESC_SZ;
+	caps->qpc_entry_sz	= HNS_ROCE_V1_QPC_ENTRY_SIZE;
+	caps->irrl_entry_sz	= HNS_ROCE_V1_IRRL_ENTRY_SIZE;
+	caps->cqc_entry_sz	= HNS_ROCE_V1_CQC_ENTRY_SIZE;
+	caps->mtpt_entry_sz	= HNS_ROCE_V1_MTPT_ENTRY_SIZE;
+	caps->mtt_entry_sz	= HNS_ROCE_V1_MTT_ENTRY_SIZE;
+	caps->cq_entry_sz	= HNS_ROCE_V1_CQE_ENTRY_SIZE;
+	caps->page_size_cap	= HNS_ROCE_V1_PAGE_SIZE_SUPPORT;
+	caps->reserved_lkey	= 0;
+	caps->reserved_pds	= 0;
+	caps->reserved_mrws	= 1;
+	caps->reserved_uars	= 0;
+	caps->reserved_cqs	= 0;
+
+	for (i = 0; i < caps->num_ports; i++)
+		caps->pkey_table_len[i] = 1;
+
+	for (i = 0; i < caps->num_ports; i++) {
+		/* Six ports shared 16 GID in v1 engine */
+		if (i >= (HNS_ROCE_V1_GID_NUM % caps->num_ports))
+			caps->gid_table_len[i] = HNS_ROCE_V1_GID_NUM /
+						 caps->num_ports;
+		else
+			caps->gid_table_len[i] = HNS_ROCE_V1_GID_NUM /
+						 caps->num_ports + 1;
+	}
+
+	for (i = 0; i < caps->num_comp_vectors; i++)
+		caps->ceqe_depth[i] = HNS_ROCE_V1_NUM_COMP_EQE;
+
+	caps->aeqe_depth = HNS_ROCE_V1_NUM_ASYNC_EQE;
+	caps->local_ca_ack_delay = le32_to_cpu(roce_read(hr_dev,
+							 ROCEE_ACK_DELAY_REG));
+	caps->max_mtu = IB_MTU_2048;
+}
+
+int hns_roce_v1_init(struct hns_roce_dev *hr_dev)
+{
+	int ret;
+	u32 val;
+	struct device *dev = &hr_dev->pdev->dev;
+
+	/* DMAE user config */
+	val = roce_read(hr_dev, ROCEE_DMAE_USER_CFG1_REG);
+	roce_set_field(val, ROCEE_DMAE_USER_CFG1_ROCEE_CACHE_TB_CFG_M,
+		       ROCEE_DMAE_USER_CFG1_ROCEE_CACHE_TB_CFG_S, 0xf);
+	roce_set_field(val, ROCEE_DMAE_USER_CFG1_ROCEE_STREAM_ID_TB_CFG_M,
+		       ROCEE_DMAE_USER_CFG1_ROCEE_STREAM_ID_TB_CFG_S,
+		       1 << PAGES_SHIFT_16);
+	roce_write(hr_dev, ROCEE_DMAE_USER_CFG1_REG, val);
+
+	val = roce_read(hr_dev, ROCEE_DMAE_USER_CFG2_REG);
+	roce_set_field(val, ROCEE_DMAE_USER_CFG2_ROCEE_CACHE_PKT_CFG_M,
+		       ROCEE_DMAE_USER_CFG2_ROCEE_CACHE_PKT_CFG_S, 0xf);
+	roce_set_field(val, ROCEE_DMAE_USER_CFG2_ROCEE_STREAM_ID_PKT_CFG_M,
+		       ROCEE_DMAE_USER_CFG2_ROCEE_STREAM_ID_PKT_CFG_S,
+		       1 << PAGES_SHIFT_16);
+
+	ret = hns_roce_db_init(hr_dev);
+	if (ret) {
+		dev_err(dev, "doorbell init failed!\n");
+		return ret;
+	}
+
+	ret = hns_roce_raq_init(hr_dev);
+	if (ret) {
+		dev_err(dev, "raq init failed!\n");
+		goto error_failed_raq_init;
+	}
+
+	hns_roce_port_enable(hr_dev, HNS_ROCE_PORT_UP);
+
+	ret = hns_roce_bt_init(hr_dev);
+	if (ret) {
+		dev_err(dev, "bt init failed!\n");
+		goto error_failed_bt_init;
+	}
+
+	return 0;
+
+error_failed_bt_init:
+	hns_roce_port_enable(hr_dev, HNS_ROCE_PORT_DOWN);
+	hns_roce_raq_free(hr_dev);
+
+error_failed_raq_init:
+	hns_roce_db_free(hr_dev);
+	return ret;
+}
+
+void hns_roce_v1_exit(struct hns_roce_dev *hr_dev)
+{
+	hns_roce_bt_free(hr_dev);
+	hns_roce_port_enable(hr_dev, HNS_ROCE_PORT_DOWN);
+	hns_roce_raq_free(hr_dev);
+	hns_roce_db_free(hr_dev);
+}
+
+void hns_roce_v1_set_gid(struct hns_roce_dev *hr_dev, u8 port, int gid_index,
+			 union ib_gid *gid)
+{
+	u32 *p = NULL;
+	u8 gid_idx = 0;
+
+	gid_idx = hns_get_gid_index(hr_dev, port, gid_index);
+
+	p = (u32 *)&gid->raw[0];
+	roce_raw_write(*p, hr_dev->reg_base + ROCEE_PORT_GID_L_0_REG +
+		       (HNS_ROCE_V1_GID_NUM * gid_idx));
+
+	p = (u32 *)&gid->raw[4];
+	roce_raw_write(*p, hr_dev->reg_base + ROCEE_PORT_GID_ML_0_REG +
+		       (HNS_ROCE_V1_GID_NUM * gid_idx));
+
+	p = (u32 *)&gid->raw[8];
+	roce_raw_write(*p, hr_dev->reg_base + ROCEE_PORT_GID_MH_0_REG +
+		       (HNS_ROCE_V1_GID_NUM * gid_idx));
+
+	p = (u32 *)&gid->raw[0xc];
+	roce_raw_write(*p, hr_dev->reg_base + ROCEE_PORT_GID_H_0_REG +
+		       (HNS_ROCE_V1_GID_NUM * gid_idx));
+}
+
+void hns_roce_v1_set_mac(struct hns_roce_dev *hr_dev, u8 phy_port, u8 *addr)
+{
+	u32 reg_smac_l;
+	u16 reg_smac_h;
+	u16 *p_h;
+	u32 *p;
+	u32 val;
+
+	p = (u32 *)(&addr[0]);
+	reg_smac_l = *p;
+	roce_raw_write(reg_smac_l, hr_dev->reg_base + ROCEE_SMAC_L_0_REG +
+		       PHY_PORT_OFFSET * phy_port);
+
+	val = roce_read(hr_dev,
+			ROCEE_SMAC_H_0_REG + phy_port * PHY_PORT_OFFSET);
+	p_h = (u16 *)(&addr[4]);
+	reg_smac_h  = *p_h;
+	roce_set_field(val, ROCEE_SMAC_H_ROCEE_SMAC_H_M,
+		       ROCEE_SMAC_H_ROCEE_SMAC_H_S, reg_smac_h);
+	roce_write(hr_dev, ROCEE_SMAC_H_0_REG + phy_port * PHY_PORT_OFFSET,
+		   val);
+}
+
+void hns_roce_v1_set_mtu(struct hns_roce_dev *hr_dev, u8 phy_port,
+			 enum ib_mtu mtu)
+{
+	u32 val;
+
+	val = roce_read(hr_dev,
+			ROCEE_SMAC_H_0_REG + phy_port * PHY_PORT_OFFSET);
+	roce_set_field(val, ROCEE_SMAC_H_ROCEE_PORT_MTU_M,
+		       ROCEE_SMAC_H_ROCEE_PORT_MTU_S, mtu);
+	roce_write(hr_dev, ROCEE_SMAC_H_0_REG + phy_port * PHY_PORT_OFFSET,
+		   val);
+}
+
+int hns_roce_v1_write_mtpt(void *mb_buf, struct hns_roce_mr *mr,
+			   unsigned long mtpt_idx)
+{
+	struct hns_roce_v1_mpt_entry *mpt_entry;
+	struct scatterlist *sg;
+	u64 *pages;
+	int entry;
+	int i;
+
+	/* MPT filled into mailbox buf */
+	mpt_entry = (struct hns_roce_v1_mpt_entry *)mb_buf;
+	memset(mpt_entry, 0, sizeof(*mpt_entry));
+
+	roce_set_field(mpt_entry->mpt_byte_4, MPT_BYTE_4_KEY_STATE_M,
+		       MPT_BYTE_4_KEY_STATE_S, KEY_VALID);
+	roce_set_field(mpt_entry->mpt_byte_4, MPT_BYTE_4_KEY_M,
+		       MPT_BYTE_4_KEY_S, mr->key);
+	roce_set_field(mpt_entry->mpt_byte_4, MPT_BYTE_4_PAGE_SIZE_M,
+		       MPT_BYTE_4_PAGE_SIZE_S, MR_SIZE_4K);
+	roce_set_bit(mpt_entry->mpt_byte_4, MPT_BYTE_4_MW_TYPE_S, 0);
+	roce_set_bit(mpt_entry->mpt_byte_4, MPT_BYTE_4_MW_BIND_ENABLE_S,
+		     (mr->access & IB_ACCESS_MW_BIND ? 1 : 0));
+	roce_set_bit(mpt_entry->mpt_byte_4, MPT_BYTE_4_OWN_S, 0);
+	roce_set_field(mpt_entry->mpt_byte_4, MPT_BYTE_4_MEMORY_LOCATION_TYPE_M,
+		       MPT_BYTE_4_MEMORY_LOCATION_TYPE_S, mr->type);
+	roce_set_bit(mpt_entry->mpt_byte_4, MPT_BYTE_4_REMOTE_ATOMIC_S, 0);
+	roce_set_bit(mpt_entry->mpt_byte_4, MPT_BYTE_4_LOCAL_WRITE_S,
+		     (mr->access & IB_ACCESS_LOCAL_WRITE ? 1 : 0));
+	roce_set_bit(mpt_entry->mpt_byte_4, MPT_BYTE_4_REMOTE_WRITE_S,
+		     (mr->access & IB_ACCESS_REMOTE_WRITE ? 1 : 0));
+	roce_set_bit(mpt_entry->mpt_byte_4, MPT_BYTE_4_REMOTE_READ_S,
+		     (mr->access & IB_ACCESS_REMOTE_READ ? 1 : 0));
+	roce_set_bit(mpt_entry->mpt_byte_4, MPT_BYTE_4_REMOTE_INVAL_ENABLE_S,
+		     0);
+	roce_set_bit(mpt_entry->mpt_byte_4, MPT_BYTE_4_ADDRESS_TYPE_S, 0);
+
+	roce_set_field(mpt_entry->mpt_byte_12, MPT_BYTE_12_PBL_ADDR_H_M,
+		       MPT_BYTE_12_PBL_ADDR_H_S, 0);
+	roce_set_field(mpt_entry->mpt_byte_12, MPT_BYTE_12_MW_BIND_COUNTER_M,
+		       MPT_BYTE_12_MW_BIND_COUNTER_S, 0);
+
+	mpt_entry->virt_addr_l = (u32)mr->iova;
+	mpt_entry->virt_addr_h = (u32)(mr->iova >> 32);
+	mpt_entry->length = (u32)mr->size;
+
+	roce_set_field(mpt_entry->mpt_byte_28, MPT_BYTE_28_PD_M,
+		       MPT_BYTE_28_PD_S, mr->pd);
+	roce_set_field(mpt_entry->mpt_byte_28, MPT_BYTE_28_L_KEY_IDX_L_M,
+		       MPT_BYTE_28_L_KEY_IDX_L_S, mtpt_idx);
+	roce_set_field(mpt_entry->mpt_byte_64, MPT_BYTE_64_L_KEY_IDX_H_M,
+		       MPT_BYTE_64_L_KEY_IDX_H_S, mtpt_idx >> MTPT_IDX_SHIFT);
+
+	/* DMA momery regsiter */
+	if (mr->type == MR_TYPE_DMA)
+		return 0;
+
+	pages = (u64 *) __get_free_page(GFP_KERNEL);
+	if (!pages)
+		return -ENOMEM;
+
+	i = 0;
+	for_each_sg(mr->umem->sg_head.sgl, sg, mr->umem->nmap, entry) {
+		pages[i] = ((u64)sg_dma_address(sg)) >> 12;
+
+		/* Directly record to MTPT table firstly 7 entry */
+		if (i >= HNS_ROCE_MAX_INNER_MTPT_NUM)
+			break;
+		i++;
+	}
+
+	/* Register user mr */
+	for (i = 0; i < HNS_ROCE_MAX_INNER_MTPT_NUM; i++) {
+		switch (i) {
+		case 0:
+			mpt_entry->pa0_l = cpu_to_le32((u32)(pages[i]));
+			roce_set_field(mpt_entry->mpt_byte_36,
+				MPT_BYTE_36_PA0_H_M,
+				MPT_BYTE_36_PA0_H_S,
+				cpu_to_le32((u32)(pages[i] >> PAGES_SHIFT_32)));
+			break;
+		case 1:
+			roce_set_field(mpt_entry->mpt_byte_36,
+				       MPT_BYTE_36_PA1_L_M,
+				       MPT_BYTE_36_PA1_L_S,
+				       cpu_to_le32((u32)(pages[i])));
+			roce_set_field(mpt_entry->mpt_byte_40,
+				MPT_BYTE_40_PA1_H_M,
+				MPT_BYTE_40_PA1_H_S,
+				cpu_to_le32((u32)(pages[i] >> PAGES_SHIFT_24)));
+			break;
+		case 2:
+			roce_set_field(mpt_entry->mpt_byte_40,
+				       MPT_BYTE_40_PA2_L_M,
+				       MPT_BYTE_40_PA2_L_S,
+				       cpu_to_le32((u32)(pages[i])));
+			roce_set_field(mpt_entry->mpt_byte_44,
+				MPT_BYTE_44_PA2_H_M,
+				MPT_BYTE_44_PA2_H_S,
+				cpu_to_le32((u32)(pages[i] >> PAGES_SHIFT_16)));
+			break;
+		case 3:
+			roce_set_field(mpt_entry->mpt_byte_44,
+				       MPT_BYTE_44_PA3_L_M,
+				       MPT_BYTE_44_PA3_L_S,
+				       cpu_to_le32((u32)(pages[i])));
+			roce_set_field(mpt_entry->mpt_byte_48,
+				MPT_BYTE_48_PA3_H_M,
+				MPT_BYTE_48_PA3_H_S,
+				cpu_to_le32((u32)(pages[i] >> PAGES_SHIFT_8)));
+			break;
+		case 4:
+			mpt_entry->pa4_l = cpu_to_le32((u32)(pages[i]));
+			roce_set_field(mpt_entry->mpt_byte_56,
+				MPT_BYTE_56_PA4_H_M,
+				MPT_BYTE_56_PA4_H_S,
+				cpu_to_le32((u32)(pages[i] >> PAGES_SHIFT_32)));
+			break;
+		case 5:
+			roce_set_field(mpt_entry->mpt_byte_56,
+				       MPT_BYTE_56_PA5_L_M,
+				       MPT_BYTE_56_PA5_L_S,
+				       cpu_to_le32((u32)(pages[i])));
+			roce_set_field(mpt_entry->mpt_byte_60,
+				MPT_BYTE_60_PA5_H_M,
+				MPT_BYTE_60_PA5_H_S,
+				cpu_to_le32((u32)(pages[i] >> PAGES_SHIFT_24)));
+			break;
+		case 6:
+			roce_set_field(mpt_entry->mpt_byte_60,
+				       MPT_BYTE_60_PA6_L_M,
+				       MPT_BYTE_60_PA6_L_S,
+				       cpu_to_le32((u32)(pages[i])));
+			roce_set_field(mpt_entry->mpt_byte_64,
+				MPT_BYTE_64_PA6_H_M,
+				MPT_BYTE_64_PA6_H_S,
+				cpu_to_le32((u32)(pages[i] >> PAGES_SHIFT_16)));
+			break;
+		default:
+			break;
+		}
+	}
+
+	free_page((unsigned long) pages);
+
+	mpt_entry->pbl_addr_l = (u32)(mr->pbl_dma_addr);
+
+	roce_set_field(mpt_entry->mpt_byte_12, MPT_BYTE_12_PBL_ADDR_H_M,
+		       MPT_BYTE_12_PBL_ADDR_H_S,
+		       ((u32)(mr->pbl_dma_addr >> 32)));
+
+	return 0;
+}
+
+static void *get_cqe(struct hns_roce_cq *hr_cq, int n)
+{
+	return hns_roce_buf_offset(&hr_cq->hr_buf.hr_buf,
+				   n * HNS_ROCE_V1_CQE_ENTRY_SIZE);
+}
+
+static void *get_sw_cqe(struct hns_roce_cq *hr_cq, int n)
+{
+	struct hns_roce_cqe *hr_cqe = get_cqe(hr_cq, n & hr_cq->ib_cq.cqe);
+
+	/* Get cqe when Owner bit is Conversely with the MSB of cons_idx */
+	return (roce_get_bit(hr_cqe->cqe_byte_4, CQE_BYTE_4_OWNER_S) ^
+		!!(n & (hr_cq->ib_cq.cqe + 1))) ? hr_cqe : NULL;
+}
+
+static struct hns_roce_cqe *next_cqe_sw(struct hns_roce_cq *hr_cq)
+{
+	return get_sw_cqe(hr_cq, hr_cq->cons_index);
+}
+
+void hns_roce_v1_cq_set_ci(struct hns_roce_cq *hr_cq, u32 cons_index)
+{
+	u32 doorbell[2];
+
+	doorbell[0] = cons_index & ((hr_cq->cq_depth << 1) - 1);
+	roce_set_bit(doorbell[1], ROCEE_DB_OTHERS_H_ROCEE_DB_OTH_HW_SYNS_S, 1);
+	roce_set_field(doorbell[1], ROCEE_DB_OTHERS_H_ROCEE_DB_OTH_CMD_M,
+		       ROCEE_DB_OTHERS_H_ROCEE_DB_OTH_CMD_S, 3);
+	roce_set_field(doorbell[1], ROCEE_DB_OTHERS_H_ROCEE_DB_OTH_CMD_MDF_M,
+		       ROCEE_DB_OTHERS_H_ROCEE_DB_OTH_CMD_MDF_S, 0);
+	roce_set_field(doorbell[1], ROCEE_DB_OTHERS_H_ROCEE_DB_OTH_INP_H_M,
+		       ROCEE_DB_OTHERS_H_ROCEE_DB_OTH_INP_H_S, hr_cq->cqn);
+
+	hns_roce_write64_k(doorbell, hr_cq->cq_db_l);
+}
+
+static void __hns_roce_v1_cq_clean(struct hns_roce_cq *hr_cq, u32 qpn,
+				   struct hns_roce_srq *srq)
+{
+	struct hns_roce_cqe *cqe, *dest;
+	u32 prod_index;
+	int nfreed = 0;
+	u8 owner_bit;
+
+	for (prod_index = hr_cq->cons_index; get_sw_cqe(hr_cq, prod_index);
+	     ++prod_index) {
+		if (prod_index == hr_cq->cons_index + hr_cq->ib_cq.cqe)
+			break;
+	}
+
+	/*
+	* Now backwards through the CQ, removing CQ entries
+	* that match our QP by overwriting them with next entries.
+	*/
+	while ((int) --prod_index - (int) hr_cq->cons_index >= 0) {
+		cqe = get_cqe(hr_cq, prod_index & hr_cq->ib_cq.cqe);
+		if ((roce_get_field(cqe->cqe_byte_16, CQE_BYTE_16_LOCAL_QPN_M,
+				     CQE_BYTE_16_LOCAL_QPN_S) &
+				     HNS_ROCE_CQE_QPN_MASK) == qpn) {
+			/* In v1 engine, not support SRQ */
+			++nfreed;
+		} else if (nfreed) {
+			dest = get_cqe(hr_cq, (prod_index + nfreed) &
+				       hr_cq->ib_cq.cqe);
+			owner_bit = roce_get_bit(dest->cqe_byte_4,
+						 CQE_BYTE_4_OWNER_S);
+			memcpy(dest, cqe, sizeof(*cqe));
+			roce_set_bit(dest->cqe_byte_4, CQE_BYTE_4_OWNER_S,
+				     owner_bit);
+		}
+	}
+
+	if (nfreed) {
+		hr_cq->cons_index += nfreed;
+		/*
+		* Make sure update of buffer contents is done before
+		* updating consumer index.
+		*/
+		wmb();
+
+		hns_roce_v1_cq_set_ci(hr_cq, hr_cq->cons_index);
+	}
+}
+
+static void hns_roce_v1_cq_clean(struct hns_roce_cq *hr_cq, u32 qpn,
+				 struct hns_roce_srq *srq)
+{
+	spin_lock_irq(&hr_cq->lock);
+	__hns_roce_v1_cq_clean(hr_cq, qpn, srq);
+	spin_unlock_irq(&hr_cq->lock);
+}
+
+void hns_roce_v1_write_cqc(struct hns_roce_dev *hr_dev,
+			   struct hns_roce_cq *hr_cq, void *mb_buf, u64 *mtts,
+			   dma_addr_t dma_handle, int nent, u32 vector)
+{
+	struct hns_roce_cq_context *cq_context = NULL;
+	void __iomem *tptr_addr;
+
+	cq_context = mb_buf;
+	memset(cq_context, 0, sizeof(*cq_context));
+
+	tptr_addr = 0;
+	hr_dev->priv_addr = tptr_addr;
+	hr_cq->tptr_addr = tptr_addr;
+
+	/* Register cq_context members */
+	roce_set_field(cq_context->cqc_byte_4,
+		       CQ_CONTEXT_CQC_BYTE_4_CQC_STATE_M,
+		       CQ_CONTEXT_CQC_BYTE_4_CQC_STATE_S, CQ_STATE_VALID);
+	roce_set_field(cq_context->cqc_byte_4, CQ_CONTEXT_CQC_BYTE_4_CQN_M,
+		       CQ_CONTEXT_CQC_BYTE_4_CQN_S, hr_cq->cqn);
+	cq_context->cqc_byte_4 = cpu_to_le32(cq_context->cqc_byte_4);
+
+	cq_context->cq_bt_l = (u32)dma_handle;
+	cq_context->cq_bt_l = cpu_to_le32(cq_context->cq_bt_l);
+
+	roce_set_field(cq_context->cqc_byte_12,
+		       CQ_CONTEXT_CQC_BYTE_12_CQ_BT_H_M,
+		       CQ_CONTEXT_CQC_BYTE_12_CQ_BT_H_S,
+		       ((u64)dma_handle >> 32));
+	roce_set_field(cq_context->cqc_byte_12,
+		       CQ_CONTEXT_CQC_BYTE_12_CQ_CQE_SHIFT_M,
+		       CQ_CONTEXT_CQC_BYTE_12_CQ_CQE_SHIFT_S,
+		       ilog2((unsigned int)nent));
+	roce_set_field(cq_context->cqc_byte_12, CQ_CONTEXT_CQC_BYTE_12_CEQN_M,
+		       CQ_CONTEXT_CQC_BYTE_12_CEQN_S, vector);
+	cq_context->cqc_byte_12 = cpu_to_le32(cq_context->cqc_byte_12);
+
+	cq_context->cur_cqe_ba0_l = (u32)(mtts[0]);
+	cq_context->cur_cqe_ba0_l = cpu_to_le32(cq_context->cur_cqe_ba0_l);
+
+	roce_set_field(cq_context->cqc_byte_20,
+		       CQ_CONTEXT_CQC_BYTE_20_CUR_CQE_BA0_H_M,
+		       CQ_CONTEXT_CQC_BYTE_20_CUR_CQE_BA0_H_S,
+		       cpu_to_le32((mtts[0]) >> 32));
+	/* Dedicated hardware, directly set 0 */
+	roce_set_field(cq_context->cqc_byte_20,
+		       CQ_CONTEXT_CQC_BYTE_20_CQ_CUR_INDEX_M,
+		       CQ_CONTEXT_CQC_BYTE_20_CQ_CUR_INDEX_S, 0);
+	/**
+	 * 44 = 32 + 12, When evaluating addr to hardware, shift 12 because of
+	 * using 4K page, and shift more 32 because of
+	 * caculating the high 32 bit value evaluated to hardware.
+	 */
+	roce_set_field(cq_context->cqc_byte_20,
+		       CQ_CONTEXT_CQC_BYTE_20_CQE_TPTR_ADDR_H_M,
+		       CQ_CONTEXT_CQC_BYTE_20_CQE_TPTR_ADDR_H_S,
+		       (u64)tptr_addr >> 44);
+	cq_context->cqc_byte_20 = cpu_to_le32(cq_context->cqc_byte_20);
+
+	cq_context->cqe_tptr_addr_l = (u32)((u64)tptr_addr >> 12);
+
+	roce_set_field(cq_context->cqc_byte_32,
+		       CQ_CONTEXT_CQC_BYTE_32_CUR_CQE_BA1_H_M,
+		       CQ_CONTEXT_CQC_BYTE_32_CUR_CQE_BA1_H_S, 0);
+	roce_set_bit(cq_context->cqc_byte_32,
+		     CQ_CONTEXT_CQC_BYTE_32_SE_FLAG_S, 0);
+	roce_set_bit(cq_context->cqc_byte_32,
+		     CQ_CONTEXT_CQC_BYTE_32_CE_FLAG_S, 0);
+	roce_set_bit(cq_context->cqc_byte_32,
+		     CQ_CONTEXT_CQC_BYTE_32_NOTIFICATION_FLAG_S, 0);
+	roce_set_bit(cq_context->cqc_byte_32,
+		     CQ_CQNTEXT_CQC_BYTE_32_TYPE_OF_COMPLETION_NOTIFICATION_S,
+		     0);
+	/*The initial value of cq's ci is 0 */
+	roce_set_field(cq_context->cqc_byte_32,
+		       CQ_CONTEXT_CQC_BYTE_32_CQ_CONS_IDX_M,
+		       CQ_CONTEXT_CQC_BYTE_32_CQ_CONS_IDX_S, 0);
+	cq_context->cqc_byte_32 = cpu_to_le32(cq_context->cqc_byte_32);
+}
+
+int hns_roce_v1_req_notify_cq(struct ib_cq *ibcq, enum ib_cq_notify_flags flags)
+{
+	struct hns_roce_cq *hr_cq = to_hr_cq(ibcq);
+	u32 notification_flag;
+	u32 doorbell[2];
+	int ret = 0;
+
+	notification_flag = (flags & IB_CQ_SOLICITED_MASK) ==
+			    IB_CQ_SOLICITED ? CQ_DB_REQ_NOT : CQ_DB_REQ_NOT_SOL;
+	/*
+	* flags = 0; Notification Flag = 1, next
+	* flags = 1; Notification Flag = 0, solocited
+	*/
+	doorbell[0] = hr_cq->cons_index & ((hr_cq->cq_depth << 1) - 1);
+	roce_set_bit(doorbell[1], ROCEE_DB_OTHERS_H_ROCEE_DB_OTH_HW_SYNS_S, 1);
+	roce_set_field(doorbell[1], ROCEE_DB_OTHERS_H_ROCEE_DB_OTH_CMD_M,
+		       ROCEE_DB_OTHERS_H_ROCEE_DB_OTH_CMD_S, 3);
+	roce_set_field(doorbell[1], ROCEE_DB_OTHERS_H_ROCEE_DB_OTH_CMD_MDF_M,
+		       ROCEE_DB_OTHERS_H_ROCEE_DB_OTH_CMD_MDF_S, 1);
+	roce_set_field(doorbell[1], ROCEE_DB_OTHERS_H_ROCEE_DB_OTH_INP_H_M,
+		       ROCEE_DB_OTHERS_H_ROCEE_DB_OTH_INP_H_S,
+		       hr_cq->cqn | notification_flag);
+
+	hns_roce_write64_k(doorbell, hr_cq->cq_db_l);
+
+	return ret;
+}
+
+static int hns_roce_v1_poll_one(struct hns_roce_cq *hr_cq,
+				struct hns_roce_qp **cur_qp, struct ib_wc *wc)
+{
+	int qpn;
+	int is_send;
+	u16 wqe_ctr;
+	u32 status;
+	u32 opcode;
+	struct hns_roce_cqe *cqe;
+	struct hns_roce_qp *hr_qp;
+	struct hns_roce_wq *wq;
+	struct hns_roce_wqe_ctrl_seg *sq_wqe;
+	struct hns_roce_dev *hr_dev = to_hr_dev(hr_cq->ib_cq.device);
+	struct device *dev = &hr_dev->pdev->dev;
+
+	/* Find cqe according consumer index */
+	cqe = next_cqe_sw(hr_cq);
+	if (!cqe)
+		return -EAGAIN;
+
+	++hr_cq->cons_index;
+	/* Memory barrier */
+	rmb();
+	/* 0->SQ, 1->RQ */
+	is_send  = !(roce_get_bit(cqe->cqe_byte_4, CQE_BYTE_4_SQ_RQ_FLAG_S));
+
+	/* Local_qpn in UD cqe is always 1, so it needs to compute new qpn */
+	if (roce_get_field(cqe->cqe_byte_16, CQE_BYTE_16_LOCAL_QPN_M,
+			   CQE_BYTE_16_LOCAL_QPN_S) <= 1) {
+		qpn = roce_get_field(cqe->cqe_byte_20, CQE_BYTE_20_PORT_NUM_M,
+				     CQE_BYTE_20_PORT_NUM_S) +
+		      roce_get_field(cqe->cqe_byte_16, CQE_BYTE_16_LOCAL_QPN_M,
+				     CQE_BYTE_16_LOCAL_QPN_S) *
+				     HNS_ROCE_MAX_PORTS;
+	} else {
+		qpn = roce_get_field(cqe->cqe_byte_16, CQE_BYTE_16_LOCAL_QPN_M,
+				     CQE_BYTE_16_LOCAL_QPN_S);
+	}
+
+	if (!*cur_qp || (qpn & HNS_ROCE_CQE_QPN_MASK) != (*cur_qp)->qpn) {
+		hr_qp = __hns_roce_qp_lookup(hr_dev, qpn);
+		if (unlikely(!hr_qp)) {
+			dev_err(dev, "CQ %06lx with entry for unknown QPN %06x\n",
+				hr_cq->cqn, (qpn & HNS_ROCE_CQE_QPN_MASK));
+			return -EINVAL;
+		}
+
+		*cur_qp = hr_qp;
+	}
+
+	wc->qp = &(*cur_qp)->ibqp;
+	wc->vendor_err = 0;
+
+	status = roce_get_field(cqe->cqe_byte_4,
+				CQE_BYTE_4_STATUS_OF_THE_OPERATION_M,
+				CQE_BYTE_4_STATUS_OF_THE_OPERATION_S) &
+				HNS_ROCE_CQE_STATUS_MASK;
+	switch (status) {
+	case HNS_ROCE_CQE_SUCCESS:
+		wc->status = IB_WC_SUCCESS;
+		break;
+	case HNS_ROCE_CQE_SYNDROME_LOCAL_LENGTH_ERR:
+		wc->status = IB_WC_LOC_LEN_ERR;
+		break;
+	case HNS_ROCE_CQE_SYNDROME_LOCAL_QP_OP_ERR:
+		wc->status = IB_WC_LOC_QP_OP_ERR;
+		break;
+	case HNS_ROCE_CQE_SYNDROME_LOCAL_PROT_ERR:
+		wc->status = IB_WC_LOC_PROT_ERR;
+		break;
+	case HNS_ROCE_CQE_SYNDROME_WR_FLUSH_ERR:
+		wc->status = IB_WC_WR_FLUSH_ERR;
+		break;
+	case HNS_ROCE_CQE_SYNDROME_MEM_MANAGE_OPERATE_ERR:
+		wc->status = IB_WC_MW_BIND_ERR;
+		break;
+	case HNS_ROCE_CQE_SYNDROME_BAD_RESP_ERR:
+		wc->status = IB_WC_BAD_RESP_ERR;
+		break;
+	case HNS_ROCE_CQE_SYNDROME_LOCAL_ACCESS_ERR:
+		wc->status = IB_WC_LOC_ACCESS_ERR;
+		break;
+	case HNS_ROCE_CQE_SYNDROME_REMOTE_INVAL_REQ_ERR:
+		wc->status = IB_WC_REM_INV_REQ_ERR;
+		break;
+	case HNS_ROCE_CQE_SYNDROME_REMOTE_ACCESS_ERR:
+		wc->status = IB_WC_REM_ACCESS_ERR;
+		break;
+	case HNS_ROCE_CQE_SYNDROME_REMOTE_OP_ERR:
+		wc->status = IB_WC_REM_OP_ERR;
+		break;
+	case HNS_ROCE_CQE_SYNDROME_TRANSPORT_RETRY_EXC_ERR:
+		wc->status = IB_WC_RETRY_EXC_ERR;
+		break;
+	case HNS_ROCE_CQE_SYNDROME_RNR_RETRY_EXC_ERR:
+		wc->status = IB_WC_RNR_RETRY_EXC_ERR;
+		break;
+	default:
+		wc->status = IB_WC_GENERAL_ERR;
+		break;
+	}
+
+	/* CQE status error, directly return */
+	if (wc->status != IB_WC_SUCCESS)
+		return 0;
+
+	if (is_send) {
+		/* SQ conrespond to CQE */
+		sq_wqe = get_send_wqe(*cur_qp, roce_get_field(cqe->cqe_byte_4,
+						CQE_BYTE_4_WQE_INDEX_M,
+						CQE_BYTE_4_WQE_INDEX_S)&
+						((*cur_qp)->sq.wqe_cnt-1));
+		switch (sq_wqe->flag & HNS_ROCE_WQE_OPCODE_MASK) {
+		case HNS_ROCE_WQE_OPCODE_SEND:
+			wc->opcode = IB_WC_SEND;
+			break;
+		case HNS_ROCE_WQE_OPCODE_RDMA_READ:
+			wc->opcode = IB_WC_RDMA_READ;
+			wc->byte_len = le32_to_cpu(cqe->byte_cnt);
+			break;
+		case HNS_ROCE_WQE_OPCODE_RDMA_WRITE:
+			wc->opcode = IB_WC_RDMA_WRITE;
+			break;
+		case HNS_ROCE_WQE_OPCODE_LOCAL_INV:
+			wc->opcode = IB_WC_LOCAL_INV;
+			break;
+		case HNS_ROCE_WQE_OPCODE_UD_SEND:
+			wc->opcode = IB_WC_SEND;
+			break;
+		default:
+			wc->status = IB_WC_GENERAL_ERR;
+			break;
+		}
+		wc->wc_flags = (sq_wqe->flag & HNS_ROCE_WQE_IMM ?
+				IB_WC_WITH_IMM : 0);
+
+		wq = &(*cur_qp)->sq;
+		if ((*cur_qp)->sq_signal_bits) {
+			/*
+			* If sg_signal_bit is 1,
+			* firstly tail pointer updated to wqe
+			* which current cqe correspond to
+			*/
+			wqe_ctr = (u16)roce_get_field(cqe->cqe_byte_4,
+						      CQE_BYTE_4_WQE_INDEX_M,
+						      CQE_BYTE_4_WQE_INDEX_S);
+			wq->tail += (wqe_ctr - (u16)wq->tail) &
+				    (wq->wqe_cnt - 1);
+		}
+		wc->wr_id = wq->wrid[wq->tail & (wq->wqe_cnt - 1)];
+		++wq->tail;
+		} else {
+		/* RQ conrespond to CQE */
+		wc->byte_len = le32_to_cpu(cqe->byte_cnt);
+		opcode = roce_get_field(cqe->cqe_byte_4,
+					CQE_BYTE_4_OPERATION_TYPE_M,
+					CQE_BYTE_4_OPERATION_TYPE_S) &
+					HNS_ROCE_CQE_OPCODE_MASK;
+		switch (opcode) {
+		case HNS_ROCE_OPCODE_RDMA_WITH_IMM_RECEIVE:
+			wc->opcode = IB_WC_RECV_RDMA_WITH_IMM;
+			wc->wc_flags = IB_WC_WITH_IMM;
+			wc->ex.imm_data = le32_to_cpu(cqe->immediate_data);
+			break;
+		case HNS_ROCE_OPCODE_SEND_DATA_RECEIVE:
+			if (roce_get_bit(cqe->cqe_byte_4,
+					 CQE_BYTE_4_IMM_INDICATOR_S)) {
+				wc->opcode = IB_WC_RECV;
+				wc->wc_flags = IB_WC_WITH_IMM;
+				wc->ex.imm_data = le32_to_cpu(
+						  cqe->immediate_data);
+			} else {
+				wc->opcode = IB_WC_RECV;
+				wc->wc_flags = 0;
+			}
+			break;
+		default:
+			wc->status = IB_WC_GENERAL_ERR;
+			break;
+		}
+
+		/* Update tail pointer, record wr_id */
+		wq = &(*cur_qp)->rq;
+		wc->wr_id = wq->wrid[wq->tail & (wq->wqe_cnt - 1)];
+		++wq->tail;
+		wc->sl = (u8)roce_get_field(cqe->cqe_byte_20, CQE_BYTE_20_SL_M,
+					    CQE_BYTE_20_SL_S);
+		wc->src_qp = (u8)roce_get_field(cqe->cqe_byte_20,
+						CQE_BYTE_20_REMOTE_QPN_M,
+						CQE_BYTE_20_REMOTE_QPN_S);
+		wc->wc_flags |= (roce_get_bit(cqe->cqe_byte_20,
+					      CQE_BYTE_20_GRH_PRESENT_S) ?
+					      IB_WC_GRH : 0);
+		wc->pkey_index = (u16)roce_get_field(cqe->cqe_byte_28,
+						     CQE_BYTE_28_P_KEY_IDX_M,
+						     CQE_BYTE_28_P_KEY_IDX_S);
+	}
+
+	return 0;
+}
+
+int hns_roce_v1_poll_cq(struct ib_cq *ibcq, int num_entries, struct ib_wc *wc)
+{
+	struct hns_roce_cq *hr_cq = to_hr_cq(ibcq);
+	struct hns_roce_qp *cur_qp = NULL;
+	unsigned long flags;
+	int npolled;
+	int ret = 0;
+
+	spin_lock_irqsave(&hr_cq->lock, flags);
+
+	for (npolled = 0; npolled < num_entries; ++npolled) {
+		ret = hns_roce_v1_poll_one(hr_cq, &cur_qp, wc + npolled);
+		if (ret)
+			break;
+	}
+
+	if (npolled)
+		hns_roce_v1_cq_set_ci(hr_cq, hr_cq->cons_index);
+
+	spin_unlock_irqrestore(&hr_cq->lock, flags);
+
+	if (ret == 0 || ret == -EAGAIN)
+		return npolled;
+	else
+		return ret;
+}
+
+int hns_roce_v1_clear_hem(struct hns_roce_dev *hr_dev,
+		struct hns_roce_hem_table *table, int obj)
+{
+	struct device *dev = &hr_dev->pdev->dev;
+	struct hns_roce_v1_priv *priv;
+	unsigned long end = 0, flags = 0;
+	uint32_t bt_cmd_val[2] = {0};
+	void __iomem *bt_cmd;
+	u64 bt_ba = 0;
+
+	priv = (struct hns_roce_v1_priv *)hr_dev->hw->priv;
+
+	switch (table->type) {
+	case HEM_TYPE_QPC:
+		roce_set_field(bt_cmd_val[1], ROCEE_BT_CMD_H_ROCEE_BT_CMD_MDF_M,
+			ROCEE_BT_CMD_H_ROCEE_BT_CMD_MDF_S, HEM_TYPE_QPC);
+		bt_ba = priv->bt_table.qpc_buf.map >> 12;
+		break;
+	case HEM_TYPE_MTPT:
+		roce_set_field(bt_cmd_val[1], ROCEE_BT_CMD_H_ROCEE_BT_CMD_MDF_M,
+			ROCEE_BT_CMD_H_ROCEE_BT_CMD_MDF_S, HEM_TYPE_MTPT);
+		bt_ba = priv->bt_table.mtpt_buf.map >> 12;
+		break;
+	case HEM_TYPE_CQC:
+		roce_set_field(bt_cmd_val[1], ROCEE_BT_CMD_H_ROCEE_BT_CMD_MDF_M,
+			ROCEE_BT_CMD_H_ROCEE_BT_CMD_MDF_S, HEM_TYPE_CQC);
+		bt_ba = priv->bt_table.cqc_buf.map >> 12;
+		break;
+	case HEM_TYPE_SRQC:
+		dev_dbg(dev, "HEM_TYPE_SRQC not support.\n");
+		return -EINVAL;
+	default:
+		return 0;
+	}
+	roce_set_field(bt_cmd_val[1], ROCEE_BT_CMD_H_ROCEE_BT_CMD_IN_MDF_M,
+		ROCEE_BT_CMD_H_ROCEE_BT_CMD_IN_MDF_S, obj);
+	roce_set_bit(bt_cmd_val[1], ROCEE_BT_CMD_H_ROCEE_BT_CMD_S, 0);
+	roce_set_bit(bt_cmd_val[1], ROCEE_BT_CMD_H_ROCEE_BT_CMD_HW_SYNS_S, 1);
+
+	spin_lock_irqsave(&hr_dev->bt_cmd_lock, flags);
+
+	bt_cmd = hr_dev->reg_base + ROCEE_BT_CMD_H_REG;
+
+	end = msecs_to_jiffies(HW_SYNC_TIMEOUT_MSECS) + jiffies;
+	while (1) {
+		if (readl(bt_cmd) >> BT_CMD_SYNC_SHIFT) {
+			if (!(time_before(jiffies, end))) {
+				dev_err(dev, "Write bt_cmd err,hw_sync is not zero.\n");
+				spin_unlock_irqrestore(&hr_dev->bt_cmd_lock,
+					flags);
+				return -EBUSY;
+			}
+		} else {
+			break;
+		}
+		msleep(HW_SYNC_SLEEP_TIME_INTERVAL);
+	}
+
+	bt_cmd_val[0] = (uint32_t)bt_ba;
+	roce_set_field(bt_cmd_val[1], ROCEE_BT_CMD_H_ROCEE_BT_CMD_BA_H_M,
+		ROCEE_BT_CMD_H_ROCEE_BT_CMD_BA_H_S, bt_ba >> 32);
+	hns_roce_write64_k(bt_cmd_val, hr_dev->reg_base + ROCEE_BT_CMD_L_REG);
+
+	spin_unlock_irqrestore(&hr_dev->bt_cmd_lock, flags);
+
+	return 0;
+}
+
+static int hns_roce_v1_qp_modify(struct hns_roce_dev *hr_dev,
+				 struct hns_roce_mtt *mtt,
+				 enum hns_roce_qp_state cur_state,
+				 enum hns_roce_qp_state new_state,
+				 struct hns_roce_qp_context *context,
+				 struct hns_roce_qp *hr_qp)
+{
+	static const u16
+	op[HNS_ROCE_QP_NUM_STATE][HNS_ROCE_QP_NUM_STATE] = {
+		[HNS_ROCE_QP_STATE_RST] = {
+		[HNS_ROCE_QP_STATE_RST] = HNS_ROCE_CMD_2RST_QP,
+		[HNS_ROCE_QP_STATE_ERR] = HNS_ROCE_CMD_2ERR_QP,
+		[HNS_ROCE_QP_STATE_INIT] = HNS_ROCE_CMD_RST2INIT_QP,
+		},
+		[HNS_ROCE_QP_STATE_INIT] = {
+		[HNS_ROCE_QP_STATE_RST] = HNS_ROCE_CMD_2RST_QP,
+		[HNS_ROCE_QP_STATE_ERR] = HNS_ROCE_CMD_2ERR_QP,
+		/* Note: In v1 engine, HW doesn't support RST2INIT.
+		 * We use RST2INIT cmd instead of INIT2INIT.
+		 */
+		[HNS_ROCE_QP_STATE_INIT] = HNS_ROCE_CMD_RST2INIT_QP,
+		[HNS_ROCE_QP_STATE_RTR] = HNS_ROCE_CMD_INIT2RTR_QP,
+		},
+		[HNS_ROCE_QP_STATE_RTR] = {
+		[HNS_ROCE_QP_STATE_RST] = HNS_ROCE_CMD_2RST_QP,
+		[HNS_ROCE_QP_STATE_ERR] = HNS_ROCE_CMD_2ERR_QP,
+		[HNS_ROCE_QP_STATE_RTS] = HNS_ROCE_CMD_RTR2RTS_QP,
+		},
+		[HNS_ROCE_QP_STATE_RTS] = {
+		[HNS_ROCE_QP_STATE_RST] = HNS_ROCE_CMD_2RST_QP,
+		[HNS_ROCE_QP_STATE_ERR] = HNS_ROCE_CMD_2ERR_QP,
+		[HNS_ROCE_QP_STATE_RTS] = HNS_ROCE_CMD_RTS2RTS_QP,
+		[HNS_ROCE_QP_STATE_SQD] = HNS_ROCE_CMD_RTS2SQD_QP,
+		},
+		[HNS_ROCE_QP_STATE_SQD] = {
+		[HNS_ROCE_QP_STATE_RST] = HNS_ROCE_CMD_2RST_QP,
+		[HNS_ROCE_QP_STATE_ERR] = HNS_ROCE_CMD_2ERR_QP,
+		[HNS_ROCE_QP_STATE_RTS] = HNS_ROCE_CMD_SQD2RTS_QP,
+		[HNS_ROCE_QP_STATE_SQD] = HNS_ROCE_CMD_SQD2SQD_QP,
+		},
+		[HNS_ROCE_QP_STATE_ERR] = {
+		[HNS_ROCE_QP_STATE_RST] = HNS_ROCE_CMD_2RST_QP,
+		[HNS_ROCE_QP_STATE_ERR] = HNS_ROCE_CMD_2ERR_QP,
+		}
+	};
+
+	struct hns_roce_cmd_mailbox *mailbox;
+	struct device *dev = &hr_dev->pdev->dev;
+	int ret = 0;
+
+	if (cur_state >= HNS_ROCE_QP_NUM_STATE ||
+	    new_state >= HNS_ROCE_QP_NUM_STATE ||
+	    !op[cur_state][new_state]) {
+		dev_err(dev, "[modify_qp]not support state %d to %d\n",
+			cur_state, new_state);
+		return -EINVAL;
+	}
+
+	if (op[cur_state][new_state] == HNS_ROCE_CMD_2RST_QP)
+		return hns_roce_cmd_mbox(hr_dev, 0, 0, hr_qp->qpn, 2,
+					 HNS_ROCE_CMD_2RST_QP,
+					 HNS_ROCE_CMD_TIME_CLASS_A);
+
+	if (op[cur_state][new_state] == HNS_ROCE_CMD_2ERR_QP)
+		return hns_roce_cmd_mbox(hr_dev, 0, 0, hr_qp->qpn, 2,
+					 HNS_ROCE_CMD_2ERR_QP,
+					 HNS_ROCE_CMD_TIME_CLASS_A);
+
+	mailbox = hns_roce_alloc_cmd_mailbox(hr_dev);
+	if (IS_ERR(mailbox))
+		return PTR_ERR(mailbox);
+
+	memcpy(mailbox->buf, context, sizeof(*context));
+
+	ret = hns_roce_cmd_mbox(hr_dev, mailbox->dma, 0, hr_qp->qpn, 0,
+				op[cur_state][new_state],
+				HNS_ROCE_CMD_TIME_CLASS_C);
+
+	hns_roce_free_cmd_mailbox(hr_dev, mailbox);
+	return ret;
+}
+
+static int hns_roce_v1_m_sqp(struct ib_qp *ibqp, const struct ib_qp_attr *attr,
+			     int attr_mask, enum ib_qp_state cur_state,
+			     enum ib_qp_state new_state)
+{
+	struct hns_roce_dev *hr_dev = to_hr_dev(ibqp->device);
+	struct hns_roce_qp *hr_qp = to_hr_qp(ibqp);
+	struct hns_roce_sqp_context *context;
+	struct device *dev = &hr_dev->pdev->dev;
+	dma_addr_t dma_handle = 0;
+	int rq_pa_start;
+	u32 reg_val;
+	u64 *mtts;
+	u32 *addr;
+
+	context = kzalloc(sizeof(*context), GFP_KERNEL);
+	if (!context)
+		return -ENOMEM;
+
+	/* Search QP buf's MTTs */
+	mtts = hns_roce_table_find(&hr_dev->mr_table.mtt_table,
+				   hr_qp->mtt.first_seg, &dma_handle);
+	if (!mtts) {
+		dev_err(dev, "qp buf pa find failed\n");
+		goto out;
+	}
+
+	if (cur_state == IB_QPS_RESET && new_state == IB_QPS_INIT) {
+		roce_set_field(context->qp1c_bytes_4,
+			       QP1C_BYTES_4_SQ_WQE_SHIFT_M,
+			       QP1C_BYTES_4_SQ_WQE_SHIFT_S,
+			       ilog2((unsigned int)hr_qp->sq.wqe_cnt));
+		roce_set_field(context->qp1c_bytes_4,
+			       QP1C_BYTES_4_RQ_WQE_SHIFT_M,
+			       QP1C_BYTES_4_RQ_WQE_SHIFT_S,
+			       ilog2((unsigned int)hr_qp->rq.wqe_cnt));
+		roce_set_field(context->qp1c_bytes_4, QP1C_BYTES_4_PD_M,
+			       QP1C_BYTES_4_PD_S, to_hr_pd(ibqp->pd)->pdn);
+
+		context->sq_rq_bt_l = (u32)(dma_handle);
+		roce_set_field(context->qp1c_bytes_12,
+			       QP1C_BYTES_12_SQ_RQ_BT_H_M,
+			       QP1C_BYTES_12_SQ_RQ_BT_H_S,
+			       ((u32)(dma_handle >> 32)));
+
+		roce_set_field(context->qp1c_bytes_16, QP1C_BYTES_16_RQ_HEAD_M,
+			       QP1C_BYTES_16_RQ_HEAD_S, hr_qp->rq.head);
+		roce_set_field(context->qp1c_bytes_16, QP1C_BYTES_16_PORT_NUM_M,
+			       QP1C_BYTES_16_PORT_NUM_S, hr_qp->phy_port);
+		roce_set_bit(context->qp1c_bytes_16,
+			     QP1C_BYTES_16_SIGNALING_TYPE_S,
+			     hr_qp->sq_signal_bits);
+		roce_set_bit(context->qp1c_bytes_16, QP1C_BYTES_16_RQ_BA_FLG_S,
+			     1);
+		roce_set_bit(context->qp1c_bytes_16, QP1C_BYTES_16_SQ_BA_FLG_S,
+			     1);
+		roce_set_bit(context->qp1c_bytes_16, QP1C_BYTES_16_QP1_ERR_S,
+			     0);
+
+		roce_set_field(context->qp1c_bytes_20, QP1C_BYTES_20_SQ_HEAD_M,
+			       QP1C_BYTES_20_SQ_HEAD_S, hr_qp->sq.head);
+		roce_set_field(context->qp1c_bytes_20, QP1C_BYTES_20_PKEY_IDX_M,
+			       QP1C_BYTES_20_PKEY_IDX_S, attr->pkey_index);
+
+		rq_pa_start = (u32)hr_qp->rq.offset / PAGE_SIZE;
+		context->cur_rq_wqe_ba_l = (u32)(mtts[rq_pa_start]);
+
+		roce_set_field(context->qp1c_bytes_28,
+			       QP1C_BYTES_28_CUR_RQ_WQE_BA_H_M,
+			       QP1C_BYTES_28_CUR_RQ_WQE_BA_H_S,
+			       (mtts[rq_pa_start]) >> 32);
+		roce_set_field(context->qp1c_bytes_28,
+			       QP1C_BYTES_28_RQ_CUR_IDX_M,
+			       QP1C_BYTES_28_RQ_CUR_IDX_S, 0);
+
+		roce_set_field(context->qp1c_bytes_32,
+			       QP1C_BYTES_32_RX_CQ_NUM_M,
+			       QP1C_BYTES_32_RX_CQ_NUM_S,
+			       to_hr_cq(ibqp->recv_cq)->cqn);
+		roce_set_field(context->qp1c_bytes_32,
+			       QP1C_BYTES_32_TX_CQ_NUM_M,
+			       QP1C_BYTES_32_TX_CQ_NUM_S,
+			       to_hr_cq(ibqp->send_cq)->cqn);
+
+		context->cur_sq_wqe_ba_l  = (u32)mtts[0];
+
+		roce_set_field(context->qp1c_bytes_40,
+			       QP1C_BYTES_40_CUR_SQ_WQE_BA_H_M,
+			       QP1C_BYTES_40_CUR_SQ_WQE_BA_H_S,
+			       (mtts[0]) >> 32);
+		roce_set_field(context->qp1c_bytes_40,
+			       QP1C_BYTES_40_SQ_CUR_IDX_M,
+			       QP1C_BYTES_40_SQ_CUR_IDX_S, 0);
+
+		/* Copy context to QP1C register */
+		addr = (u32 *)(hr_dev->reg_base + ROCEE_QP1C_CFG0_0_REG +
+			hr_qp->phy_port * sizeof(*context));
+
+		writel(context->qp1c_bytes_4, addr);
+		writel(context->sq_rq_bt_l, addr + 1);
+		writel(context->qp1c_bytes_12, addr + 2);
+		writel(context->qp1c_bytes_16, addr + 3);
+		writel(context->qp1c_bytes_20, addr + 4);
+		writel(context->cur_rq_wqe_ba_l, addr + 5);
+		writel(context->qp1c_bytes_28, addr + 6);
+		writel(context->qp1c_bytes_32, addr + 7);
+		writel(context->cur_sq_wqe_ba_l, addr + 8);
+		writel(context->qp1c_bytes_40, addr + 9);
+	}
+
+	/* Modify QP1C status */
+	reg_val = roce_read(hr_dev, ROCEE_QP1C_CFG0_0_REG +
+			    hr_qp->phy_port * sizeof(*context));
+	roce_set_field(reg_val, ROCEE_QP1C_CFG0_0_ROCEE_QP1C_QP_ST_M,
+		       ROCEE_QP1C_CFG0_0_ROCEE_QP1C_QP_ST_S, new_state);
+	roce_write(hr_dev, ROCEE_QP1C_CFG0_0_REG +
+		    hr_qp->phy_port * sizeof(*context), reg_val);
+
+	hr_qp->state = new_state;
+	if (new_state == IB_QPS_RESET) {
+		hns_roce_v1_cq_clean(to_hr_cq(ibqp->recv_cq), hr_qp->qpn,
+				     ibqp->srq ? to_hr_srq(ibqp->srq) : NULL);
+		if (ibqp->send_cq != ibqp->recv_cq)
+			hns_roce_v1_cq_clean(to_hr_cq(ibqp->send_cq),
+					     hr_qp->qpn, NULL);
+
+		hr_qp->rq.head = 0;
+		hr_qp->rq.tail = 0;
+		hr_qp->sq.head = 0;
+		hr_qp->sq.tail = 0;
+		hr_qp->sq_next_wqe = 0;
+	}
+
+	kfree(context);
+	return 0;
+
+out:
+	kfree(context);
+	return -EINVAL;
+}
+
+static int hns_roce_v1_m_qp(struct ib_qp *ibqp, const struct ib_qp_attr *attr,
+			    int attr_mask, enum ib_qp_state cur_state,
+			    enum ib_qp_state new_state)
+{
+	struct hns_roce_dev *hr_dev = to_hr_dev(ibqp->device);
+	struct hns_roce_qp *hr_qp = to_hr_qp(ibqp);
+	struct device *dev = &hr_dev->pdev->dev;
+	struct hns_roce_qp_context *context;
+	dma_addr_t dma_handle_2 = 0;
+	dma_addr_t dma_handle = 0;
+	uint32_t doorbell[2] = {0};
+	int rq_pa_start = 0;
+	u64 *mtts_2 = NULL;
+	int ret = -EINVAL;
+	u64 *mtts = NULL;
+	int port;
+	u8 *dmac;
+	u8 *smac;
+
+	context = kzalloc(sizeof(*context), GFP_KERNEL);
+	if (!context)
+		return -ENOMEM;
+
+	/* Search qp buf's mtts */
+	mtts = hns_roce_table_find(&hr_dev->mr_table.mtt_table,
+				   hr_qp->mtt.first_seg, &dma_handle);
+	if (mtts == NULL) {
+		dev_err(dev, "qp buf pa find failed\n");
+		goto out;
+	}
+
+	/* Search IRRL's mtts */
+	mtts_2 = hns_roce_table_find(&hr_dev->qp_table.irrl_table, hr_qp->qpn,
+				     &dma_handle_2);
+	if (mtts_2 == NULL) {
+		dev_err(dev, "qp irrl_table find failed\n");
+		goto out;
+	}
+
+	/*
+	*Reset to init
+	*	Mandatory param:
+	*	IB_QP_STATE | IB_QP_PKEY_INDEX | IB_QP_PORT | IB_QP_ACCESS_FLAGS
+	*	Optional param: NA
+	*/
+	if (cur_state == IB_QPS_RESET && new_state == IB_QPS_INIT) {
+		roce_set_field(context->qpc_bytes_4,
+			       QP_CONTEXT_QPC_BYTES_4_TRANSPORT_SERVICE_TYPE_M,
+			       QP_CONTEXT_QPC_BYTES_4_TRANSPORT_SERVICE_TYPE_S,
+			       to_hr_qp_type(hr_qp->ibqp.qp_type));
+
+		roce_set_bit(context->qpc_bytes_4,
+			     QP_CONTEXT_QPC_BYTE_4_ENABLE_FPMR_S, 0);
+		roce_set_bit(context->qpc_bytes_4,
+			     QP_CONTEXT_QPC_BYTE_4_RDMA_READ_ENABLE_S,
+			     !!(attr->qp_access_flags & IB_ACCESS_REMOTE_READ));
+		roce_set_bit(context->qpc_bytes_4,
+			     QP_CONTEXT_QPC_BYTE_4_RDMA_WRITE_ENABLE_S,
+			     !!(attr->qp_access_flags & IB_ACCESS_REMOTE_WRITE)
+			     );
+		roce_set_bit(context->qpc_bytes_4,
+			     QP_CONTEXT_QPC_BYTE_4_ATOMIC_OPERATION_ENABLE_S,
+			     !!(attr->qp_access_flags & IB_ACCESS_REMOTE_ATOMIC)
+			     );
+		roce_set_bit(context->qpc_bytes_4,
+			     QP_CONTEXT_QPC_BYTE_4_RDMAR_USE_S, 1);
+		roce_set_field(context->qpc_bytes_4,
+			       QP_CONTEXT_QPC_BYTES_4_SQ_WQE_SHIFT_M,
+			       QP_CONTEXT_QPC_BYTES_4_SQ_WQE_SHIFT_S,
+			       ilog2((unsigned int)hr_qp->sq.wqe_cnt));
+		roce_set_field(context->qpc_bytes_4,
+			       QP_CONTEXT_QPC_BYTES_4_RQ_WQE_SHIFT_M,
+			       QP_CONTEXT_QPC_BYTES_4_RQ_WQE_SHIFT_S,
+			       ilog2((unsigned int)hr_qp->rq.wqe_cnt));
+		roce_set_field(context->qpc_bytes_4,
+			       QP_CONTEXT_QPC_BYTES_4_PD_M,
+			       QP_CONTEXT_QPC_BYTES_4_PD_S,
+			       to_hr_pd(ibqp->pd)->pdn);
+		hr_qp->access_flags = attr->qp_access_flags;
+		roce_set_field(context->qpc_bytes_8,
+			       QP_CONTEXT_QPC_BYTES_8_TX_COMPLETION_M,
+			       QP_CONTEXT_QPC_BYTES_8_TX_COMPLETION_S,
+			       to_hr_cq(ibqp->send_cq)->cqn);
+		roce_set_field(context->qpc_bytes_8,
+			       QP_CONTEXT_QPC_BYTES_8_RX_COMPLETION_M,
+			       QP_CONTEXT_QPC_BYTES_8_RX_COMPLETION_S,
+			       to_hr_cq(ibqp->recv_cq)->cqn);
+
+		if (ibqp->srq)
+			roce_set_field(context->qpc_bytes_12,
+				       QP_CONTEXT_QPC_BYTES_12_SRQ_NUMBER_M,
+				       QP_CONTEXT_QPC_BYTES_12_SRQ_NUMBER_S,
+				       to_hr_srq(ibqp->srq)->srqn);
+
+		roce_set_field(context->qpc_bytes_12,
+			       QP_CONTEXT_QPC_BYTES_12_P_KEY_INDEX_M,
+			       QP_CONTEXT_QPC_BYTES_12_P_KEY_INDEX_S,
+			       attr->pkey_index);
+		hr_qp->pkey_index = attr->pkey_index;
+		roce_set_field(context->qpc_bytes_16,
+			       QP_CONTEXT_QPC_BYTES_16_QP_NUM_M,
+			       QP_CONTEXT_QPC_BYTES_16_QP_NUM_S, hr_qp->qpn);
+
+	} else if (cur_state == IB_QPS_INIT && new_state == IB_QPS_INIT) {
+		roce_set_field(context->qpc_bytes_4,
+			       QP_CONTEXT_QPC_BYTES_4_TRANSPORT_SERVICE_TYPE_M,
+			       QP_CONTEXT_QPC_BYTES_4_TRANSPORT_SERVICE_TYPE_S,
+			       to_hr_qp_type(hr_qp->ibqp.qp_type));
+		roce_set_bit(context->qpc_bytes_4,
+			     QP_CONTEXT_QPC_BYTE_4_ENABLE_FPMR_S, 0);
+		if (attr_mask & IB_QP_ACCESS_FLAGS) {
+			roce_set_bit(context->qpc_bytes_4,
+				     QP_CONTEXT_QPC_BYTE_4_RDMA_READ_ENABLE_S,
+				     !!(attr->qp_access_flags &
+				     IB_ACCESS_REMOTE_READ));
+			roce_set_bit(context->qpc_bytes_4,
+				     QP_CONTEXT_QPC_BYTE_4_RDMA_WRITE_ENABLE_S,
+				     !!(attr->qp_access_flags &
+				     IB_ACCESS_REMOTE_WRITE));
+		} else {
+			roce_set_bit(context->qpc_bytes_4,
+				     QP_CONTEXT_QPC_BYTE_4_RDMA_READ_ENABLE_S,
+				     !!(hr_qp->access_flags &
+				     IB_ACCESS_REMOTE_READ));
+			roce_set_bit(context->qpc_bytes_4,
+				     QP_CONTEXT_QPC_BYTE_4_RDMA_WRITE_ENABLE_S,
+				     !!(hr_qp->access_flags &
+				     IB_ACCESS_REMOTE_WRITE));
+		}
+
+		roce_set_bit(context->qpc_bytes_4,
+			     QP_CONTEXT_QPC_BYTE_4_RDMAR_USE_S, 1);
+		roce_set_field(context->qpc_bytes_4,
+			       QP_CONTEXT_QPC_BYTES_4_SQ_WQE_SHIFT_M,
+			       QP_CONTEXT_QPC_BYTES_4_SQ_WQE_SHIFT_S,
+			       ilog2((unsigned int)hr_qp->sq.wqe_cnt));
+		roce_set_field(context->qpc_bytes_4,
+			       QP_CONTEXT_QPC_BYTES_4_RQ_WQE_SHIFT_M,
+			       QP_CONTEXT_QPC_BYTES_4_RQ_WQE_SHIFT_S,
+			       ilog2((unsigned int)hr_qp->rq.wqe_cnt));
+		roce_set_field(context->qpc_bytes_4,
+			       QP_CONTEXT_QPC_BYTES_4_PD_M,
+			       QP_CONTEXT_QPC_BYTES_4_PD_S,
+			       to_hr_pd(ibqp->pd)->pdn);
+
+		roce_set_field(context->qpc_bytes_8,
+			       QP_CONTEXT_QPC_BYTES_8_TX_COMPLETION_M,
+			       QP_CONTEXT_QPC_BYTES_8_TX_COMPLETION_S,
+			       to_hr_cq(ibqp->send_cq)->cqn);
+		roce_set_field(context->qpc_bytes_8,
+			       QP_CONTEXT_QPC_BYTES_8_RX_COMPLETION_M,
+			       QP_CONTEXT_QPC_BYTES_8_RX_COMPLETION_S,
+			       to_hr_cq(ibqp->recv_cq)->cqn);
+
+		if (ibqp->srq)
+			roce_set_field(context->qpc_bytes_12,
+				       QP_CONTEXT_QPC_BYTES_12_SRQ_NUMBER_M,
+				       QP_CONTEXT_QPC_BYTES_12_SRQ_NUMBER_S,
+				       to_hr_srq(ibqp->srq)->srqn);
+		if (attr_mask & IB_QP_PKEY_INDEX)
+			roce_set_field(context->qpc_bytes_12,
+				       QP_CONTEXT_QPC_BYTES_12_P_KEY_INDEX_M,
+				       QP_CONTEXT_QPC_BYTES_12_P_KEY_INDEX_S,
+				       attr->pkey_index);
+		else
+			roce_set_field(context->qpc_bytes_12,
+				       QP_CONTEXT_QPC_BYTES_12_P_KEY_INDEX_M,
+				       QP_CONTEXT_QPC_BYTES_12_P_KEY_INDEX_S,
+				       hr_qp->pkey_index);
+
+		roce_set_field(context->qpc_bytes_16,
+			       QP_CONTEXT_QPC_BYTES_16_QP_NUM_M,
+			       QP_CONTEXT_QPC_BYTES_16_QP_NUM_S, hr_qp->qpn);
+	} else if (cur_state == IB_QPS_INIT && new_state == IB_QPS_RTR) {
+		if ((attr_mask & IB_QP_ALT_PATH) ||
+		    (attr_mask & IB_QP_ACCESS_FLAGS) ||
+		    (attr_mask & IB_QP_PKEY_INDEX) ||
+		    (attr_mask & IB_QP_QKEY)) {
+			dev_err(dev, "INIT2RTR attr_mask error\n");
+			goto out;
+		}
+
+		dmac = (u8 *)attr->ah_attr.dmac;
+
+		context->sq_rq_bt_l = (u32)(dma_handle);
+		roce_set_field(context->qpc_bytes_24,
+			       QP_CONTEXT_QPC_BYTES_24_SQ_RQ_BT_H_M,
+			       QP_CONTEXT_QPC_BYTES_24_SQ_RQ_BT_H_S,
+			       ((u32)(dma_handle >> 32)));
+		roce_set_bit(context->qpc_bytes_24,
+			     QP_CONTEXT_QPC_BYTE_24_REMOTE_ENABLE_E2E_CREDITS_S,
+			     1);
+		roce_set_field(context->qpc_bytes_24,
+			       QP_CONTEXT_QPC_BYTES_24_MINIMUM_RNR_NAK_TIMER_M,
+			       QP_CONTEXT_QPC_BYTES_24_MINIMUM_RNR_NAK_TIMER_S,
+			       attr->min_rnr_timer);
+		context->irrl_ba_l = (u32)(dma_handle_2);
+		roce_set_field(context->qpc_bytes_32,
+			       QP_CONTEXT_QPC_BYTES_32_IRRL_BA_H_M,
+			       QP_CONTEXT_QPC_BYTES_32_IRRL_BA_H_S,
+			       ((u32)(dma_handle_2 >> 32)) &
+				QP_CONTEXT_QPC_BYTES_32_IRRL_BA_H_M);
+		roce_set_field(context->qpc_bytes_32,
+			       QP_CONTEXT_QPC_BYTES_32_MIG_STATE_M,
+			       QP_CONTEXT_QPC_BYTES_32_MIG_STATE_S, 0);
+		roce_set_bit(context->qpc_bytes_32,
+			     QP_CONTEXT_QPC_BYTE_32_LOCAL_ENABLE_E2E_CREDITS_S,
+			     1);
+		roce_set_bit(context->qpc_bytes_32,
+			     QP_CONTEXT_QPC_BYTE_32_SIGNALING_TYPE_S,
+			     hr_qp->sq_signal_bits);
+
+		for (port = 0; port < hr_dev->caps.num_ports; port++) {
+			smac = (u8 *)hr_dev->dev_addr[port];
+			dev_dbg(dev, "smac: %2x: %2x: %2x: %2x: %2x: %2x\n",
+				smac[0], smac[1], smac[2], smac[3], smac[4],
+				smac[5]);
+			if ((dmac[0] == smac[0]) && (dmac[1] == smac[1]) &&
+			    (dmac[2] == smac[2]) && (dmac[3] == smac[3]) &&
+			    (dmac[4] == smac[4]) && (dmac[5] == smac[5])) {
+				roce_set_bit(context->qpc_bytes_32,
+				    QP_CONTEXT_QPC_BYTE_32_LOOPBACK_INDICATOR_S,
+				    1);
+				break;
+			}
+		}
+
+		if (hr_dev->loop_idc == 0x1)
+			roce_set_bit(context->qpc_bytes_32,
+				QP_CONTEXT_QPC_BYTE_32_LOOPBACK_INDICATOR_S, 1);
+
+		roce_set_bit(context->qpc_bytes_32,
+			     QP_CONTEXT_QPC_BYTE_32_GLOBAL_HEADER_S,
+			     attr->ah_attr.ah_flags);
+		roce_set_field(context->qpc_bytes_32,
+			       QP_CONTEXT_QPC_BYTES_32_RESPONDER_RESOURCES_M,
+			       QP_CONTEXT_QPC_BYTES_32_RESPONDER_RESOURCES_S,
+			       ilog2((unsigned int)attr->max_dest_rd_atomic));
+
+		roce_set_field(context->qpc_bytes_36,
+			       QP_CONTEXT_QPC_BYTES_36_DEST_QP_M,
+			       QP_CONTEXT_QPC_BYTES_36_DEST_QP_S,
+			       attr->dest_qp_num);
+
+		/* Configure GID index */
+		roce_set_field(context->qpc_bytes_36,
+			       QP_CONTEXT_QPC_BYTES_36_SGID_INDEX_M,
+			       QP_CONTEXT_QPC_BYTES_36_SGID_INDEX_S,
+			       hns_get_gid_index(hr_dev,
+						 attr->ah_attr.port_num - 1,
+						 attr->ah_attr.grh.sgid_index));
+
+		memcpy(&(context->dmac_l), dmac, 4);
+
+		roce_set_field(context->qpc_bytes_44,
+			       QP_CONTEXT_QPC_BYTES_44_DMAC_H_M,
+			       QP_CONTEXT_QPC_BYTES_44_DMAC_H_S,
+			       *((u16 *)(&dmac[4])));
+		roce_set_field(context->qpc_bytes_44,
+			       QP_CONTEXT_QPC_BYTES_44_MAXIMUM_STATIC_RATE_M,
+			       QP_CONTEXT_QPC_BYTES_44_MAXIMUM_STATIC_RATE_S,
+			       attr->ah_attr.static_rate);
+		roce_set_field(context->qpc_bytes_44,
+			       QP_CONTEXT_QPC_BYTES_44_HOPLMT_M,
+			       QP_CONTEXT_QPC_BYTES_44_HOPLMT_S,
+			       attr->ah_attr.grh.hop_limit);
+
+		roce_set_field(context->qpc_bytes_48,
+			       QP_CONTEXT_QPC_BYTES_48_FLOWLABEL_M,
+			       QP_CONTEXT_QPC_BYTES_48_FLOWLABEL_S,
+			       attr->ah_attr.grh.flow_label);
+		roce_set_field(context->qpc_bytes_48,
+			       QP_CONTEXT_QPC_BYTES_48_TCLASS_M,
+			       QP_CONTEXT_QPC_BYTES_48_TCLASS_S,
+			       attr->ah_attr.grh.traffic_class);
+		roce_set_field(context->qpc_bytes_48,
+			       QP_CONTEXT_QPC_BYTES_48_MTU_M,
+			       QP_CONTEXT_QPC_BYTES_48_MTU_S, attr->path_mtu);
+
+		memcpy(context->dgid, attr->ah_attr.grh.dgid.raw,
+		       sizeof(attr->ah_attr.grh.dgid.raw));
+
+		dev_dbg(dev, "dmac:%x :%lx\n", context->dmac_l,
+			roce_get_field(context->qpc_bytes_44,
+				       QP_CONTEXT_QPC_BYTES_44_DMAC_H_M,
+				       QP_CONTEXT_QPC_BYTES_44_DMAC_H_S));
+
+		roce_set_field(context->qpc_bytes_68,
+			       QP_CONTEXT_QPC_BYTES_68_RQ_HEAD_M,
+			       QP_CONTEXT_QPC_BYTES_68_RQ_HEAD_S,
+			       hr_qp->rq.head);
+		roce_set_field(context->qpc_bytes_68,
+			       QP_CONTEXT_QPC_BYTES_68_RQ_CUR_INDEX_M,
+			       QP_CONTEXT_QPC_BYTES_68_RQ_CUR_INDEX_S, 0);
+
+		rq_pa_start = (u32)hr_qp->rq.offset / PAGE_SIZE;
+		context->cur_rq_wqe_ba_l = (u32)(mtts[rq_pa_start]);
+
+		roce_set_field(context->qpc_bytes_76,
+			QP_CONTEXT_QPC_BYTES_76_CUR_RQ_WQE_BA_H_M,
+			QP_CONTEXT_QPC_BYTES_76_CUR_RQ_WQE_BA_H_S,
+			mtts[rq_pa_start] >> 32);
+		roce_set_field(context->qpc_bytes_76,
+			       QP_CONTEXT_QPC_BYTES_76_RX_REQ_MSN_M,
+			       QP_CONTEXT_QPC_BYTES_76_RX_REQ_MSN_S, 0);
+
+		context->rx_rnr_time = 0;
+
+		roce_set_field(context->qpc_bytes_84,
+			       QP_CONTEXT_QPC_BYTES_84_LAST_ACK_PSN_M,
+			       QP_CONTEXT_QPC_BYTES_84_LAST_ACK_PSN_S,
+			       attr->rq_psn - 1);
+		roce_set_field(context->qpc_bytes_84,
+			       QP_CONTEXT_QPC_BYTES_84_TRRL_HEAD_M,
+			       QP_CONTEXT_QPC_BYTES_84_TRRL_HEAD_S, 0);
+
+		roce_set_field(context->qpc_bytes_88,
+			       QP_CONTEXT_QPC_BYTES_88_RX_REQ_EPSN_M,
+			       QP_CONTEXT_QPC_BYTES_88_RX_REQ_EPSN_S,
+			       attr->rq_psn);
+		roce_set_bit(context->qpc_bytes_88,
+			     QP_CONTEXT_QPC_BYTES_88_RX_REQ_PSN_ERR_FLAG_S, 0);
+		roce_set_bit(context->qpc_bytes_88,
+			     QP_CONTEXT_QPC_BYTES_88_RX_LAST_OPCODE_FLG_S, 0);
+		roce_set_field(context->qpc_bytes_88,
+			QP_CONTEXT_QPC_BYTES_88_RQ_REQ_LAST_OPERATION_TYPE_M,
+			QP_CONTEXT_QPC_BYTES_88_RQ_REQ_LAST_OPERATION_TYPE_S,
+			0);
+		roce_set_field(context->qpc_bytes_88,
+			       QP_CONTEXT_QPC_BYTES_88_RQ_REQ_RDMA_WR_FLAG_M,
+			       QP_CONTEXT_QPC_BYTES_88_RQ_REQ_RDMA_WR_FLAG_S,
+			       0);
+
+		context->dma_length = 0;
+		context->r_key = 0;
+		context->va_l = 0;
+		context->va_h = 0;
+
+		roce_set_field(context->qpc_bytes_108,
+			       QP_CONTEXT_QPC_BYTES_108_TRRL_SDB_PSN_M,
+			       QP_CONTEXT_QPC_BYTES_108_TRRL_SDB_PSN_S, 0);
+		roce_set_bit(context->qpc_bytes_108,
+			     QP_CONTEXT_QPC_BYTES_108_TRRL_SDB_PSN_FLG_S, 0);
+		roce_set_bit(context->qpc_bytes_108,
+			     QP_CONTEXT_QPC_BYTES_108_TRRL_TDB_PSN_FLG_S, 0);
+
+		roce_set_field(context->qpc_bytes_112,
+			       QP_CONTEXT_QPC_BYTES_112_TRRL_TDB_PSN_M,
+			       QP_CONTEXT_QPC_BYTES_112_TRRL_TDB_PSN_S, 0);
+		roce_set_field(context->qpc_bytes_112,
+			       QP_CONTEXT_QPC_BYTES_112_TRRL_TAIL_M,
+			       QP_CONTEXT_QPC_BYTES_112_TRRL_TAIL_S, 0);
+
+		/* For chip resp ack */
+		roce_set_field(context->qpc_bytes_156,
+			       QP_CONTEXT_QPC_BYTES_156_PORT_NUM_M,
+			       QP_CONTEXT_QPC_BYTES_156_PORT_NUM_S,
+			       hr_qp->phy_port);
+		roce_set_field(context->qpc_bytes_156,
+			       QP_CONTEXT_QPC_BYTES_156_SL_M,
+			       QP_CONTEXT_QPC_BYTES_156_SL_S, attr->ah_attr.sl);
+		hr_qp->sl = attr->ah_attr.sl;
+	} else if (cur_state == IB_QPS_RTR &&
+		new_state == IB_QPS_RTS) {
+		/* If exist optional param, return error */
+		if ((attr_mask & IB_QP_ALT_PATH) ||
+		    (attr_mask & IB_QP_ACCESS_FLAGS) ||
+		    (attr_mask & IB_QP_QKEY) ||
+		    (attr_mask & IB_QP_PATH_MIG_STATE) ||
+		    (attr_mask & IB_QP_CUR_STATE) ||
+		    (attr_mask & IB_QP_MIN_RNR_TIMER)) {
+			dev_err(dev, "RTR2RTS attr_mask error\n");
+			goto out;
+		}
+
+		context->rx_cur_sq_wqe_ba_l = (u32)(mtts[0]);
+
+		roce_set_field(context->qpc_bytes_120,
+			       QP_CONTEXT_QPC_BYTES_120_RX_CUR_SQ_WQE_BA_H_M,
+			       QP_CONTEXT_QPC_BYTES_120_RX_CUR_SQ_WQE_BA_H_S,
+			       (mtts[0]) >> 32);
+
+		roce_set_field(context->qpc_bytes_124,
+			       QP_CONTEXT_QPC_BYTES_124_RX_ACK_MSN_M,
+			       QP_CONTEXT_QPC_BYTES_124_RX_ACK_MSN_S, 0);
+		roce_set_field(context->qpc_bytes_124,
+			       QP_CONTEXT_QPC_BYTES_124_IRRL_MSG_IDX_M,
+			       QP_CONTEXT_QPC_BYTES_124_IRRL_MSG_IDX_S, 0);
+
+		roce_set_field(context->qpc_bytes_128,
+			       QP_CONTEXT_QPC_BYTES_128_RX_ACK_EPSN_M,
+			       QP_CONTEXT_QPC_BYTES_128_RX_ACK_EPSN_S,
+			       attr->sq_psn);
+		roce_set_bit(context->qpc_bytes_128,
+			     QP_CONTEXT_QPC_BYTES_128_RX_ACK_PSN_ERR_FLG_S, 0);
+		roce_set_field(context->qpc_bytes_128,
+			     QP_CONTEXT_QPC_BYTES_128_ACK_LAST_OPERATION_TYPE_M,
+			     QP_CONTEXT_QPC_BYTES_128_ACK_LAST_OPERATION_TYPE_S,
+			     0);
+		roce_set_bit(context->qpc_bytes_128,
+			     QP_CONTEXT_QPC_BYTES_128_IRRL_PSN_VLD_FLG_S, 0);
+
+		roce_set_field(context->qpc_bytes_132,
+			       QP_CONTEXT_QPC_BYTES_132_IRRL_PSN_M,
+			       QP_CONTEXT_QPC_BYTES_132_IRRL_PSN_S, 0);
+		roce_set_field(context->qpc_bytes_132,
+			       QP_CONTEXT_QPC_BYTES_132_IRRL_TAIL_M,
+			       QP_CONTEXT_QPC_BYTES_132_IRRL_TAIL_S, 0);
+
+		roce_set_field(context->qpc_bytes_136,
+			       QP_CONTEXT_QPC_BYTES_136_RETRY_MSG_PSN_M,
+			       QP_CONTEXT_QPC_BYTES_136_RETRY_MSG_PSN_S,
+			       attr->sq_psn);
+		roce_set_field(context->qpc_bytes_136,
+			       QP_CONTEXT_QPC_BYTES_136_RETRY_MSG_FPKT_PSN_L_M,
+			       QP_CONTEXT_QPC_BYTES_136_RETRY_MSG_FPKT_PSN_L_S,
+			       attr->sq_psn);
+
+		roce_set_field(context->qpc_bytes_140,
+			       QP_CONTEXT_QPC_BYTES_140_RETRY_MSG_FPKT_PSN_H_M,
+			       QP_CONTEXT_QPC_BYTES_140_RETRY_MSG_FPKT_PSN_H_S,
+			       (attr->sq_psn >> SQ_PSN_SHIFT));
+		roce_set_field(context->qpc_bytes_140,
+			       QP_CONTEXT_QPC_BYTES_140_RETRY_MSG_MSN_M,
+			       QP_CONTEXT_QPC_BYTES_140_RETRY_MSG_MSN_S, 0);
+		roce_set_bit(context->qpc_bytes_140,
+			     QP_CONTEXT_QPC_BYTES_140_RNR_RETRY_FLG_S, 0);
+
+		roce_set_field(context->qpc_bytes_148,
+			       QP_CONTEXT_QPC_BYTES_148_CHECK_FLAG_M,
+			       QP_CONTEXT_QPC_BYTES_148_CHECK_FLAG_S, 0);
+		roce_set_field(context->qpc_bytes_148,
+			       QP_CONTEXT_QPC_BYTES_148_RETRY_COUNT_M,
+			       QP_CONTEXT_QPC_BYTES_148_RETRY_COUNT_S,
+			       attr->retry_cnt);
+		roce_set_field(context->qpc_bytes_148,
+			       QP_CONTEXT_QPC_BYTES_148_RNR_RETRY_COUNT_M,
+			       QP_CONTEXT_QPC_BYTES_148_RNR_RETRY_COUNT_S,
+			       attr->rnr_retry);
+		roce_set_field(context->qpc_bytes_148,
+			       QP_CONTEXT_QPC_BYTES_148_LSN_M,
+			       QP_CONTEXT_QPC_BYTES_148_LSN_S, 0x100);
+
+		context->rnr_retry = 0;
+
+		roce_set_field(context->qpc_bytes_156,
+			       QP_CONTEXT_QPC_BYTES_156_RETRY_COUNT_INIT_M,
+			       QP_CONTEXT_QPC_BYTES_156_RETRY_COUNT_INIT_S,
+			       attr->retry_cnt);
+		if (attr->timeout < 0x12) {
+			dev_info(dev, "ack timeout value(0x%x) must bigger than 0x12.\n",
+				 attr->timeout);
+			roce_set_field(context->qpc_bytes_156,
+				       QP_CONTEXT_QPC_BYTES_156_ACK_TIMEOUT_M,
+				       QP_CONTEXT_QPC_BYTES_156_ACK_TIMEOUT_S,
+				       0x12);
+		} else {
+			roce_set_field(context->qpc_bytes_156,
+				       QP_CONTEXT_QPC_BYTES_156_ACK_TIMEOUT_M,
+				       QP_CONTEXT_QPC_BYTES_156_ACK_TIMEOUT_S,
+				       attr->timeout);
+		}
+		roce_set_field(context->qpc_bytes_156,
+			       QP_CONTEXT_QPC_BYTES_156_RNR_RETRY_COUNT_INIT_M,
+			       QP_CONTEXT_QPC_BYTES_156_RNR_RETRY_COUNT_INIT_S,
+			       attr->rnr_retry);
+		roce_set_field(context->qpc_bytes_156,
+			       QP_CONTEXT_QPC_BYTES_156_PORT_NUM_M,
+			       QP_CONTEXT_QPC_BYTES_156_PORT_NUM_S,
+			       hr_qp->phy_port);
+		roce_set_field(context->qpc_bytes_156,
+			       QP_CONTEXT_QPC_BYTES_156_SL_M,
+			       QP_CONTEXT_QPC_BYTES_156_SL_S, attr->ah_attr.sl);
+		hr_qp->sl = attr->ah_attr.sl;
+		roce_set_field(context->qpc_bytes_156,
+			       QP_CONTEXT_QPC_BYTES_156_INITIATOR_DEPTH_M,
+			       QP_CONTEXT_QPC_BYTES_156_INITIATOR_DEPTH_S,
+			       ilog2((unsigned int)attr->max_rd_atomic));
+		roce_set_field(context->qpc_bytes_156,
+			       QP_CONTEXT_QPC_BYTES_156_ACK_REQ_IND_M,
+			       QP_CONTEXT_QPC_BYTES_156_ACK_REQ_IND_S, 0);
+		context->pkt_use_len = 0;
+
+		roce_set_field(context->qpc_bytes_164,
+			       QP_CONTEXT_QPC_BYTES_164_SQ_PSN_M,
+			       QP_CONTEXT_QPC_BYTES_164_SQ_PSN_S, attr->sq_psn);
+		roce_set_field(context->qpc_bytes_164,
+			       QP_CONTEXT_QPC_BYTES_164_IRRL_HEAD_M,
+			       QP_CONTEXT_QPC_BYTES_164_IRRL_HEAD_S, 0);
+
+		roce_set_field(context->qpc_bytes_168,
+			       QP_CONTEXT_QPC_BYTES_168_RETRY_SQ_PSN_M,
+			       QP_CONTEXT_QPC_BYTES_168_RETRY_SQ_PSN_S,
+			       attr->sq_psn);
+		roce_set_field(context->qpc_bytes_168,
+			       QP_CONTEXT_QPC_BYTES_168_SGE_USE_FLA_M,
+			       QP_CONTEXT_QPC_BYTES_168_SGE_USE_FLA_S, 0);
+		roce_set_field(context->qpc_bytes_168,
+			       QP_CONTEXT_QPC_BYTES_168_DB_TYPE_M,
+			       QP_CONTEXT_QPC_BYTES_168_DB_TYPE_S, 0);
+		roce_set_bit(context->qpc_bytes_168,
+			     QP_CONTEXT_QPC_BYTES_168_MSG_LP_IND_S, 0);
+		roce_set_bit(context->qpc_bytes_168,
+			     QP_CONTEXT_QPC_BYTES_168_CSDB_LP_IND_S, 0);
+		roce_set_bit(context->qpc_bytes_168,
+			     QP_CONTEXT_QPC_BYTES_168_QP_ERR_FLG_S, 0);
+		context->sge_use_len = 0;
+
+		roce_set_field(context->qpc_bytes_176,
+			       QP_CONTEXT_QPC_BYTES_176_DB_CUR_INDEX_M,
+			       QP_CONTEXT_QPC_BYTES_176_DB_CUR_INDEX_S, 0);
+		roce_set_field(context->qpc_bytes_176,
+			       QP_CONTEXT_QPC_BYTES_176_RETRY_DB_CUR_INDEX_M,
+			       QP_CONTEXT_QPC_BYTES_176_RETRY_DB_CUR_INDEX_S,
+			       0);
+		roce_set_field(context->qpc_bytes_180,
+			       QP_CONTEXT_QPC_BYTES_180_SQ_CUR_INDEX_M,
+			       QP_CONTEXT_QPC_BYTES_180_SQ_CUR_INDEX_S, 0);
+		roce_set_field(context->qpc_bytes_180,
+			       QP_CONTEXT_QPC_BYTES_180_SQ_HEAD_M,
+			       QP_CONTEXT_QPC_BYTES_180_SQ_HEAD_S, 0);
+
+		context->tx_cur_sq_wqe_ba_l = (u32)(mtts[0]);
+
+		roce_set_field(context->qpc_bytes_188,
+			       QP_CONTEXT_QPC_BYTES_188_TX_CUR_SQ_WQE_BA_H_M,
+			       QP_CONTEXT_QPC_BYTES_188_TX_CUR_SQ_WQE_BA_H_S,
+			       (mtts[0]) >> 32);
+		roce_set_bit(context->qpc_bytes_188,
+			     QP_CONTEXT_QPC_BYTES_188_PKT_RETRY_FLG_S, 0);
+		roce_set_field(context->qpc_bytes_188,
+			       QP_CONTEXT_QPC_BYTES_188_TX_RETRY_CUR_INDEX_M,
+			       QP_CONTEXT_QPC_BYTES_188_TX_RETRY_CUR_INDEX_S,
+			       0);
+	} else if (!((cur_state == IB_QPS_INIT && new_state == IB_QPS_RESET) ||
+		   (cur_state == IB_QPS_INIT && new_state == IB_QPS_ERR) ||
+		   (cur_state == IB_QPS_RTR && new_state == IB_QPS_RESET) ||
+		   (cur_state == IB_QPS_RTR && new_state == IB_QPS_ERR) ||
+		   (cur_state == IB_QPS_RTS && new_state == IB_QPS_RESET) ||
+		   (cur_state == IB_QPS_RTS && new_state == IB_QPS_ERR) ||
+		   (cur_state == IB_QPS_ERR && new_state == IB_QPS_RESET) ||
+		   (cur_state == IB_QPS_ERR && new_state == IB_QPS_ERR))) {
+		dev_err(dev, "not support this status migration\n");
+		goto out;
+	}
+
+	/* Every status migrate must change state */
+	roce_set_field(context->qpc_bytes_144,
+		       QP_CONTEXT_QPC_BYTES_144_QP_STATE_M,
+		       QP_CONTEXT_QPC_BYTES_144_QP_STATE_S, attr->qp_state);
+
+	/* SW pass context to HW */
+	ret = hns_roce_v1_qp_modify(hr_dev, &hr_qp->mtt,
+				    to_hns_roce_state(cur_state),
+				    to_hns_roce_state(new_state), context,
+				    hr_qp);
+	if (ret) {
+		dev_err(dev, "hns_roce_qp_modify failed\n");
+		goto out;
+	}
+
+	/*
+	* Use rst2init to instead of init2init with drv,
+	* need to hw to flash RQ HEAD by DB again
+	*/
+	if (cur_state == IB_QPS_INIT && new_state == IB_QPS_INIT) {
+		/* Memory barrier */
+		wmb();
+
+		roce_set_field(doorbell[0], RQ_DOORBELL_U32_4_RQ_HEAD_M,
+			       RQ_DOORBELL_U32_4_RQ_HEAD_S, hr_qp->rq.head);
+		roce_set_field(doorbell[1], RQ_DOORBELL_U32_8_QPN_M,
+			       RQ_DOORBELL_U32_8_QPN_S, hr_qp->qpn);
+		roce_set_field(doorbell[1], RQ_DOORBELL_U32_8_CMD_M,
+			       RQ_DOORBELL_U32_8_CMD_S, 1);
+		roce_set_bit(doorbell[1], RQ_DOORBELL_U32_8_HW_SYNC_S, 1);
+
+		if (ibqp->uobject) {
+			hr_qp->rq.db_reg_l = hr_dev->reg_base +
+				     ROCEE_DB_OTHERS_L_0_REG +
+				     DB_REG_OFFSET * hr_dev->priv_uar.index;
+		}
+
+		hns_roce_write64_k(doorbell, hr_qp->rq.db_reg_l);
+	}
+
+	hr_qp->state = new_state;
+
+	if (attr_mask & IB_QP_MAX_DEST_RD_ATOMIC)
+		hr_qp->resp_depth = attr->max_dest_rd_atomic;
+	if (attr_mask & IB_QP_PORT) {
+		hr_qp->port = attr->port_num - 1;
+		hr_qp->phy_port = hr_dev->iboe.phy_port[hr_qp->port];
+	}
+
+	if (new_state == IB_QPS_RESET && !ibqp->uobject) {
+		hns_roce_v1_cq_clean(to_hr_cq(ibqp->recv_cq), hr_qp->qpn,
+				     ibqp->srq ? to_hr_srq(ibqp->srq) : NULL);
+		if (ibqp->send_cq != ibqp->recv_cq)
+			hns_roce_v1_cq_clean(to_hr_cq(ibqp->send_cq),
+					     hr_qp->qpn, NULL);
+
+		hr_qp->rq.head = 0;
+		hr_qp->rq.tail = 0;
+		hr_qp->sq.head = 0;
+		hr_qp->sq.tail = 0;
+		hr_qp->sq_next_wqe = 0;
+	}
+out:
+	kfree(context);
+	return ret;
+}
+
+int hns_roce_v1_modify_qp(struct ib_qp *ibqp, const struct ib_qp_attr *attr,
+			  int attr_mask, enum ib_qp_state cur_state,
+			  enum ib_qp_state new_state)
+{
+
+	if (ibqp->qp_type == IB_QPT_GSI || ibqp->qp_type == IB_QPT_SMI)
+		return hns_roce_v1_m_sqp(ibqp, attr, attr_mask, cur_state,
+					 new_state);
+	else
+		return hns_roce_v1_m_qp(ibqp, attr, attr_mask, cur_state,
+					new_state);
+}
+
+static enum ib_qp_state to_ib_qp_state(enum hns_roce_qp_state state)
+{
+	switch (state) {
+	case HNS_ROCE_QP_STATE_RST:
+		return IB_QPS_RESET;
+	case HNS_ROCE_QP_STATE_INIT:
+		return IB_QPS_INIT;
+	case HNS_ROCE_QP_STATE_RTR:
+		return IB_QPS_RTR;
+	case HNS_ROCE_QP_STATE_RTS:
+		return IB_QPS_RTS;
+	case HNS_ROCE_QP_STATE_SQD:
+		return IB_QPS_SQD;
+	case HNS_ROCE_QP_STATE_ERR:
+		return IB_QPS_ERR;
+	default:
+		return IB_QPS_ERR;
+	}
+}
+
+static int hns_roce_v1_query_qpc(struct hns_roce_dev *hr_dev,
+				 struct hns_roce_qp *hr_qp,
+				 struct hns_roce_qp_context *hr_context)
+{
+	struct hns_roce_cmd_mailbox *mailbox;
+	int ret;
+
+	mailbox = hns_roce_alloc_cmd_mailbox(hr_dev);
+	if (IS_ERR(mailbox))
+		return PTR_ERR(mailbox);
+
+	ret = hns_roce_cmd_mbox(hr_dev, 0, mailbox->dma, hr_qp->qpn, 0,
+				HNS_ROCE_CMD_QUERY_QP,
+				HNS_ROCE_CMD_TIME_CLASS_A);
+	if (!ret)
+		memcpy(hr_context, mailbox->buf, sizeof(*hr_context));
+	else
+		dev_err(&hr_dev->pdev->dev, "QUERY QP cmd process error\n");
+
+	hns_roce_free_cmd_mailbox(hr_dev, mailbox);
+
+	return ret;
+}
+
+int hns_roce_v1_query_qp(struct ib_qp *ibqp, struct ib_qp_attr *qp_attr,
+			 int qp_attr_mask, struct ib_qp_init_attr *qp_init_attr)
+{
+	struct hns_roce_dev *hr_dev = to_hr_dev(ibqp->device);
+	struct hns_roce_qp *hr_qp = to_hr_qp(ibqp);
+	struct device *dev = &hr_dev->pdev->dev;
+	struct hns_roce_qp_context *context;
+	int tmp_qp_state = 0;
+	int ret = 0;
+	int state;
+
+	context = kzalloc(sizeof(*context), GFP_KERNEL);
+	if (!context)
+		return -ENOMEM;
+
+	memset(qp_attr, 0, sizeof(*qp_attr));
+	memset(qp_init_attr, 0, sizeof(*qp_init_attr));
+
+	mutex_lock(&hr_qp->mutex);
+
+	if (hr_qp->state == IB_QPS_RESET) {
+		qp_attr->qp_state = IB_QPS_RESET;
+		goto done;
+	}
+
+	ret = hns_roce_v1_query_qpc(hr_dev, hr_qp, context);
+	if (ret) {
+		dev_err(dev, "query qpc error\n");
+		ret = -EINVAL;
+		goto out;
+	}
+
+	state = roce_get_field(context->qpc_bytes_144,
+			       QP_CONTEXT_QPC_BYTES_144_QP_STATE_M,
+			       QP_CONTEXT_QPC_BYTES_144_QP_STATE_S);
+	tmp_qp_state = (int)to_ib_qp_state((enum hns_roce_qp_state)state);
+	if (tmp_qp_state == -1) {
+		dev_err(dev, "to_ib_qp_state error\n");
+		ret = -EINVAL;
+		goto out;
+	}
+	hr_qp->state = (u8)tmp_qp_state;
+	qp_attr->qp_state = (enum ib_qp_state)hr_qp->state;
+	qp_attr->path_mtu = (enum ib_mtu)roce_get_field(context->qpc_bytes_48,
+					       QP_CONTEXT_QPC_BYTES_48_MTU_M,
+					       QP_CONTEXT_QPC_BYTES_48_MTU_S);
+	qp_attr->path_mig_state = IB_MIG_ARMED;
+	if (hr_qp->ibqp.qp_type == IB_QPT_UD)
+		qp_attr->qkey = QKEY_VAL;
+
+	qp_attr->rq_psn = roce_get_field(context->qpc_bytes_88,
+					 QP_CONTEXT_QPC_BYTES_88_RX_REQ_EPSN_M,
+					 QP_CONTEXT_QPC_BYTES_88_RX_REQ_EPSN_S);
+	qp_attr->sq_psn = (u32)roce_get_field(context->qpc_bytes_164,
+					     QP_CONTEXT_QPC_BYTES_164_SQ_PSN_M,
+					     QP_CONTEXT_QPC_BYTES_164_SQ_PSN_S);
+	qp_attr->dest_qp_num = (u8)roce_get_field(context->qpc_bytes_36,
+					QP_CONTEXT_QPC_BYTES_36_DEST_QP_M,
+					QP_CONTEXT_QPC_BYTES_36_DEST_QP_S);
+	qp_attr->qp_access_flags = ((roce_get_bit(context->qpc_bytes_4,
+			QP_CONTEXT_QPC_BYTE_4_RDMA_READ_ENABLE_S)) << 2) |
+				   ((roce_get_bit(context->qpc_bytes_4,
+			QP_CONTEXT_QPC_BYTE_4_RDMA_WRITE_ENABLE_S)) << 1) |
+				   ((roce_get_bit(context->qpc_bytes_4,
+			QP_CONTEXT_QPC_BYTE_4_ATOMIC_OPERATION_ENABLE_S)) << 3);
+
+	if (hr_qp->ibqp.qp_type == IB_QPT_RC ||
+	    hr_qp->ibqp.qp_type == IB_QPT_UC) {
+		qp_attr->ah_attr.sl = roce_get_field(context->qpc_bytes_156,
+						QP_CONTEXT_QPC_BYTES_156_SL_M,
+						QP_CONTEXT_QPC_BYTES_156_SL_S);
+		qp_attr->ah_attr.grh.flow_label = roce_get_field(
+					context->qpc_bytes_48,
+					QP_CONTEXT_QPC_BYTES_48_FLOWLABEL_M,
+					QP_CONTEXT_QPC_BYTES_48_FLOWLABEL_S);
+		qp_attr->ah_attr.grh.sgid_index = roce_get_field(
+					context->qpc_bytes_36,
+					QP_CONTEXT_QPC_BYTES_36_SGID_INDEX_M,
+					QP_CONTEXT_QPC_BYTES_36_SGID_INDEX_S);
+		qp_attr->ah_attr.grh.hop_limit = roce_get_field(
+					context->qpc_bytes_44,
+					QP_CONTEXT_QPC_BYTES_44_HOPLMT_M,
+					QP_CONTEXT_QPC_BYTES_44_HOPLMT_S);
+		qp_attr->ah_attr.grh.traffic_class = roce_get_field(
+					context->qpc_bytes_48,
+					QP_CONTEXT_QPC_BYTES_48_TCLASS_M,
+					QP_CONTEXT_QPC_BYTES_48_TCLASS_S);
+
+		memcpy(qp_attr->ah_attr.grh.dgid.raw, context->dgid,
+		       sizeof(qp_attr->ah_attr.grh.dgid.raw));
+	}
+
+	qp_attr->pkey_index = roce_get_field(context->qpc_bytes_12,
+			      QP_CONTEXT_QPC_BYTES_12_P_KEY_INDEX_M,
+			      QP_CONTEXT_QPC_BYTES_12_P_KEY_INDEX_S);
+	qp_attr->port_num = (u8)roce_get_field(context->qpc_bytes_156,
+			     QP_CONTEXT_QPC_BYTES_156_PORT_NUM_M,
+			     QP_CONTEXT_QPC_BYTES_156_PORT_NUM_S) + 1;
+	qp_attr->sq_draining = 0;
+	qp_attr->max_rd_atomic = roce_get_field(context->qpc_bytes_156,
+				 QP_CONTEXT_QPC_BYTES_156_INITIATOR_DEPTH_M,
+				 QP_CONTEXT_QPC_BYTES_156_INITIATOR_DEPTH_S);
+	qp_attr->max_dest_rd_atomic = roce_get_field(context->qpc_bytes_32,
+				 QP_CONTEXT_QPC_BYTES_32_RESPONDER_RESOURCES_M,
+				 QP_CONTEXT_QPC_BYTES_32_RESPONDER_RESOURCES_S);
+	qp_attr->min_rnr_timer = (u8)(roce_get_field(context->qpc_bytes_24,
+			QP_CONTEXT_QPC_BYTES_24_MINIMUM_RNR_NAK_TIMER_M,
+			QP_CONTEXT_QPC_BYTES_24_MINIMUM_RNR_NAK_TIMER_S));
+	qp_attr->timeout = (u8)(roce_get_field(context->qpc_bytes_156,
+			    QP_CONTEXT_QPC_BYTES_156_ACK_TIMEOUT_M,
+			    QP_CONTEXT_QPC_BYTES_156_ACK_TIMEOUT_S));
+	qp_attr->retry_cnt = roce_get_field(context->qpc_bytes_148,
+			     QP_CONTEXT_QPC_BYTES_148_RETRY_COUNT_M,
+			     QP_CONTEXT_QPC_BYTES_148_RETRY_COUNT_S);
+	qp_attr->rnr_retry = context->rnr_retry;
+
+done:
+	qp_attr->cur_qp_state = qp_attr->qp_state;
+	qp_attr->cap.max_recv_wr = hr_qp->rq.wqe_cnt;
+	qp_attr->cap.max_recv_sge = hr_qp->rq.max_gs;
+
+	if (!ibqp->uobject) {
+		qp_attr->cap.max_send_wr = hr_qp->sq.wqe_cnt;
+		qp_attr->cap.max_send_sge = hr_qp->sq.max_gs;
+	} else {
+		qp_attr->cap.max_send_wr = 0;
+		qp_attr->cap.max_send_sge = 0;
+	}
+
+	qp_init_attr->cap = qp_attr->cap;
+
+out:
+	mutex_unlock(&hr_qp->mutex);
+	kfree(context);
+	return ret;
+}
+
+static void hns_roce_v1_destroy_qp_common(struct hns_roce_dev *hr_dev,
+					  struct hns_roce_qp *hr_qp,
+					  int is_user)
+{
+	u32 sdbinvcnt;
+	unsigned long end = 0;
+	u32 sdbinvcnt_val;
+	u32 sdbsendptr_val;
+	u32 sdbisusepr_val;
+	struct hns_roce_cq *send_cq, *recv_cq;
+	struct device *dev = &hr_dev->pdev->dev;
+
+	if (hr_qp->ibqp.qp_type == IB_QPT_RC) {
+		if (hr_qp->state != IB_QPS_RESET) {
+			/*
+			* Set qp to ERR,
+			* waiting for hw complete processing all dbs
+			*/
+			if (hns_roce_v1_qp_modify(hr_dev, NULL,
+					to_hns_roce_state(
+						(enum ib_qp_state)hr_qp->state),
+						HNS_ROCE_QP_STATE_ERR, NULL,
+						hr_qp))
+				dev_err(dev, "modify QP %06lx to ERR failed.\n",
+					hr_qp->qpn);
+
+			/* Record issued doorbell */
+			sdbisusepr_val = roce_read(hr_dev,
+					 ROCEE_SDB_ISSUE_PTR_REG);
+			/*
+			* Query db process status,
+			* until hw process completely
+			*/
+			end = msecs_to_jiffies(
+			      HNS_ROCE_QP_DESTROY_TIMEOUT_MSECS) + jiffies;
+			do {
+				sdbsendptr_val = roce_read(hr_dev,
+						 ROCEE_SDB_SEND_PTR_REG);
+				if (!time_before(jiffies, end)) {
+					dev_err(dev, "destroy qp(0x%lx) timeout!!!",
+						hr_qp->qpn);
+					break;
+				}
+			} while ((short)(roce_get_field(sdbsendptr_val,
+					ROCEE_SDB_SEND_PTR_SDB_SEND_PTR_M,
+					ROCEE_SDB_SEND_PTR_SDB_SEND_PTR_S) -
+				roce_get_field(sdbisusepr_val,
+					ROCEE_SDB_ISSUE_PTR_SDB_ISSUE_PTR_M,
+					ROCEE_SDB_ISSUE_PTR_SDB_ISSUE_PTR_S)
+				) < 0);
+
+			/* Get list pointer */
+			sdbinvcnt = roce_read(hr_dev, ROCEE_SDB_INV_CNT_REG);
+
+			/* Query db's list status, until hw reversal */
+			do {
+				sdbinvcnt_val = roce_read(hr_dev,
+						ROCEE_SDB_INV_CNT_REG);
+				if (!time_before(jiffies, end)) {
+					dev_err(dev, "destroy qp(0x%lx) timeout!!!",
+						hr_qp->qpn);
+					dev_err(dev, "SdbInvCnt = 0x%x\n",
+						sdbinvcnt_val);
+					break;
+				}
+			} while ((short)(roce_get_field(sdbinvcnt_val,
+				  ROCEE_SDB_INV_CNT_SDB_INV_CNT_M,
+				  ROCEE_SDB_INV_CNT_SDB_INV_CNT_S) -
+				  (sdbinvcnt + SDB_INV_CNT_OFFSET)) < 0);
+
+			/* Modify qp to reset before destroying qp */
+			if (hns_roce_v1_qp_modify(hr_dev, NULL,
+					to_hns_roce_state(
+					(enum ib_qp_state)hr_qp->state),
+					HNS_ROCE_QP_STATE_RST, NULL, hr_qp))
+				dev_err(dev, "modify QP %06lx to RESET failed.\n",
+					hr_qp->qpn);
+		}
+	}
+
+	send_cq = to_hr_cq(hr_qp->ibqp.send_cq);
+	recv_cq = to_hr_cq(hr_qp->ibqp.recv_cq);
+
+	hns_roce_lock_cqs(send_cq, recv_cq);
+
+	if (!is_user) {
+		__hns_roce_v1_cq_clean(recv_cq, hr_qp->qpn, hr_qp->ibqp.srq ?
+				       to_hr_srq(hr_qp->ibqp.srq) : NULL);
+		if (send_cq != recv_cq)
+			__hns_roce_v1_cq_clean(send_cq, hr_qp->qpn, NULL);
+	}
+
+	hns_roce_qp_remove(hr_dev, hr_qp);
+
+	hns_roce_unlock_cqs(send_cq, recv_cq);
+
+	hns_roce_qp_free(hr_dev, hr_qp);
+
+	/* Not special_QP, free their QPN */
+	if ((hr_qp->ibqp.qp_type == IB_QPT_RC) ||
+	    (hr_qp->ibqp.qp_type == IB_QPT_UC) ||
+	    (hr_qp->ibqp.qp_type == IB_QPT_UD))
+		hns_roce_release_range_qp(hr_dev, hr_qp->qpn, 1);
+
+	hns_roce_mtt_cleanup(hr_dev, &hr_qp->mtt);
+
+	if (is_user) {
+		ib_umem_release(hr_qp->umem);
+	} else {
+		kfree(hr_qp->sq.wrid);
+		kfree(hr_qp->rq.wrid);
+		hns_roce_buf_free(hr_dev, hr_qp->buff_size, &hr_qp->hr_buf);
+	}
+}
+
+int hns_roce_v1_destroy_qp(struct ib_qp *ibqp)
+{
+	struct hns_roce_dev *hr_dev = to_hr_dev(ibqp->device);
+	struct hns_roce_qp *hr_qp = to_hr_qp(ibqp);
+
+	hns_roce_v1_destroy_qp_common(hr_dev, hr_qp, !!ibqp->pd->uobject);
+
+	if (hr_qp->ibqp.qp_type == IB_QPT_GSI)
+		kfree(hr_to_hr_sqp(hr_qp));
+	else
+		kfree(hr_qp);
+
+	return 0;
+}
+
+struct hns_roce_v1_priv hr_v1_priv;
+
+struct hns_roce_hw hns_roce_hw_v1 = {
+	.reset = hns_roce_v1_reset,
+	.hw_profile = hns_roce_v1_profile,
+	.hw_init = hns_roce_v1_init,
+	.hw_exit = hns_roce_v1_exit,
+	.set_gid = hns_roce_v1_set_gid,
+	.set_mac = hns_roce_v1_set_mac,
+	.set_mtu = hns_roce_v1_set_mtu,
+	.write_mtpt = hns_roce_v1_write_mtpt,
+	.write_cqc = hns_roce_v1_write_cqc,
+	.clear_hem = hns_roce_v1_clear_hem,
+	.modify_qp = hns_roce_v1_modify_qp,
+	.query_qp = hns_roce_v1_query_qp,
+	.destroy_qp = hns_roce_v1_destroy_qp,
+	.post_send = hns_roce_v1_post_send,
+	.post_recv = hns_roce_v1_post_recv,
+	.req_notify_cq = hns_roce_v1_req_notify_cq,
+	.poll_cq = hns_roce_v1_poll_cq,
+	.priv = &hr_v1_priv,
+};
diff --git a/drivers/infiniband/hw/hns/hns_roce_hw_v1.h b/drivers/infiniband/hw/hns/hns_roce_hw_v1.h
new file mode 100644
index 000000000000..539b0a3b92b0
--- /dev/null
+++ b/drivers/infiniband/hw/hns/hns_roce_hw_v1.h
@@ -0,0 +1,990 @@
+/*
+ * Copyright (c) 2016 Hisilicon Limited.
+ *
+ * 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 _HNS_ROCE_HW_V1_H
+#define _HNS_ROCE_HW_V1_H
+
+#define CQ_STATE_VALID					2
+
+#define HNS_ROCE_V1_MAX_PD_NUM				0x8000
+#define HNS_ROCE_V1_MAX_CQ_NUM				0x10000
+#define HNS_ROCE_V1_MAX_CQE_NUM				0x8000
+
+#define HNS_ROCE_V1_MAX_QP_NUM				0x40000
+#define HNS_ROCE_V1_MAX_WQE_NUM				0x4000
+
+#define HNS_ROCE_V1_MAX_MTPT_NUM			0x80000
+
+#define HNS_ROCE_V1_MAX_MTT_SEGS			0x100000
+
+#define HNS_ROCE_V1_MAX_QP_INIT_RDMA			128
+#define HNS_ROCE_V1_MAX_QP_DEST_RDMA			128
+
+#define HNS_ROCE_V1_MAX_SQ_DESC_SZ			64
+#define HNS_ROCE_V1_MAX_RQ_DESC_SZ			64
+#define HNS_ROCE_V1_SG_NUM				2
+#define HNS_ROCE_V1_INLINE_SIZE				32
+
+#define HNS_ROCE_V1_UAR_NUM				256
+#define HNS_ROCE_V1_PHY_UAR_NUM				8
+
+#define HNS_ROCE_V1_GID_NUM				16
+
+#define HNS_ROCE_V1_NUM_COMP_EQE			0x8000
+#define HNS_ROCE_V1_NUM_ASYNC_EQE			0x400
+
+#define HNS_ROCE_V1_QPC_ENTRY_SIZE			256
+#define HNS_ROCE_V1_IRRL_ENTRY_SIZE			8
+#define HNS_ROCE_V1_CQC_ENTRY_SIZE			64
+#define HNS_ROCE_V1_MTPT_ENTRY_SIZE			64
+#define HNS_ROCE_V1_MTT_ENTRY_SIZE			64
+
+#define HNS_ROCE_V1_CQE_ENTRY_SIZE			32
+#define HNS_ROCE_V1_PAGE_SIZE_SUPPORT			0xFFFFF000
+
+#define HNS_ROCE_V1_EXT_RAQ_WF				8
+#define HNS_ROCE_V1_RAQ_ENTRY				64
+#define HNS_ROCE_V1_RAQ_DEPTH				32768
+#define HNS_ROCE_V1_RAQ_SIZE	(HNS_ROCE_V1_RAQ_ENTRY * HNS_ROCE_V1_RAQ_DEPTH)
+
+#define HNS_ROCE_V1_SDB_DEPTH				0x400
+#define HNS_ROCE_V1_ODB_DEPTH				0x400
+
+#define HNS_ROCE_V1_DB_RSVD				0x80
+
+#define HNS_ROCE_V1_SDB_ALEPT				HNS_ROCE_V1_DB_RSVD
+#define HNS_ROCE_V1_SDB_ALFUL	(HNS_ROCE_V1_SDB_DEPTH - HNS_ROCE_V1_DB_RSVD)
+#define HNS_ROCE_V1_ODB_ALEPT				HNS_ROCE_V1_DB_RSVD
+#define HNS_ROCE_V1_ODB_ALFUL	(HNS_ROCE_V1_ODB_DEPTH - HNS_ROCE_V1_DB_RSVD)
+
+#define HNS_ROCE_V1_EXT_SDB_DEPTH			0x4000
+#define HNS_ROCE_V1_EXT_ODB_DEPTH			0x4000
+#define HNS_ROCE_V1_EXT_SDB_ENTRY			16
+#define HNS_ROCE_V1_EXT_ODB_ENTRY			16
+#define HNS_ROCE_V1_EXT_SDB_SIZE  \
+	(HNS_ROCE_V1_EXT_SDB_DEPTH * HNS_ROCE_V1_EXT_SDB_ENTRY)
+#define HNS_ROCE_V1_EXT_ODB_SIZE  \
+	(HNS_ROCE_V1_EXT_ODB_DEPTH * HNS_ROCE_V1_EXT_ODB_ENTRY)
+
+#define HNS_ROCE_V1_EXT_SDB_ALEPT			HNS_ROCE_V1_DB_RSVD
+#define HNS_ROCE_V1_EXT_SDB_ALFUL  \
+	(HNS_ROCE_V1_EXT_SDB_DEPTH - HNS_ROCE_V1_DB_RSVD)
+#define HNS_ROCE_V1_EXT_ODB_ALEPT			HNS_ROCE_V1_DB_RSVD
+#define HNS_ROCE_V1_EXT_ODB_ALFUL	\
+	(HNS_ROCE_V1_EXT_ODB_DEPTH - HNS_ROCE_V1_DB_RSVD)
+
+#define HNS_ROCE_BT_RSV_BUF_SIZE			(1 << 17)
+
+#define HNS_ROCE_ODB_POLL_MODE				0
+
+#define HNS_ROCE_SDB_NORMAL_MODE			0
+#define HNS_ROCE_SDB_EXTEND_MODE			1
+
+#define HNS_ROCE_ODB_EXTEND_MODE			1
+
+#define KEY_VALID					0x02
+
+#define HNS_ROCE_CQE_QPN_MASK				0x3ffff
+#define HNS_ROCE_CQE_STATUS_MASK			0x1f
+#define HNS_ROCE_CQE_OPCODE_MASK			0xf
+
+#define HNS_ROCE_CQE_SUCCESS				0x00
+#define HNS_ROCE_CQE_SYNDROME_LOCAL_LENGTH_ERR		0x01
+#define HNS_ROCE_CQE_SYNDROME_LOCAL_QP_OP_ERR		0x02
+#define HNS_ROCE_CQE_SYNDROME_LOCAL_PROT_ERR		0x03
+#define HNS_ROCE_CQE_SYNDROME_WR_FLUSH_ERR		0x04
+#define HNS_ROCE_CQE_SYNDROME_MEM_MANAGE_OPERATE_ERR	0x05
+#define HNS_ROCE_CQE_SYNDROME_BAD_RESP_ERR		0x06
+#define HNS_ROCE_CQE_SYNDROME_LOCAL_ACCESS_ERR		0x07
+#define HNS_ROCE_CQE_SYNDROME_REMOTE_INVAL_REQ_ERR	0x08
+#define HNS_ROCE_CQE_SYNDROME_REMOTE_ACCESS_ERR		0x09
+#define HNS_ROCE_CQE_SYNDROME_REMOTE_OP_ERR		0x0a
+#define HNS_ROCE_CQE_SYNDROME_TRANSPORT_RETRY_EXC_ERR	0x0b
+#define HNS_ROCE_CQE_SYNDROME_RNR_RETRY_EXC_ERR		0x0c
+
+#define QP1C_CFGN_OFFSET				0x28
+#define PHY_PORT_OFFSET					0x8
+#define MTPT_IDX_SHIFT					16
+#define ALL_PORT_VAL_OPEN				0x3f
+#define POL_TIME_INTERVAL_VAL				0x80
+#define SLEEP_TIME_INTERVAL				20
+#define SQ_PSN_SHIFT					8
+#define QKEY_VAL					0x80010000
+#define SDB_INV_CNT_OFFSET				8
+
+struct hns_roce_cq_context {
+	u32 cqc_byte_4;
+	u32 cq_bt_l;
+	u32 cqc_byte_12;
+	u32 cur_cqe_ba0_l;
+	u32 cqc_byte_20;
+	u32 cqe_tptr_addr_l;
+	u32 cur_cqe_ba1_l;
+	u32 cqc_byte_32;
+};
+
+#define CQ_CONTEXT_CQC_BYTE_4_CQC_STATE_S 0
+#define CQ_CONTEXT_CQC_BYTE_4_CQC_STATE_M   \
+	(((1UL << 2) - 1) << CQ_CONTEXT_CQC_BYTE_4_CQC_STATE_S)
+
+#define CQ_CONTEXT_CQC_BYTE_4_CQN_S 16
+#define CQ_CONTEXT_CQC_BYTE_4_CQN_M   \
+	(((1UL << 16) - 1) << CQ_CONTEXT_CQC_BYTE_4_CQN_S)
+
+#define CQ_CONTEXT_CQC_BYTE_12_CQ_BT_H_S 0
+#define CQ_CONTEXT_CQC_BYTE_12_CQ_BT_H_M   \
+	(((1UL << 17) - 1) << CQ_CONTEXT_CQC_BYTE_12_CQ_BT_H_S)
+
+#define CQ_CONTEXT_CQC_BYTE_12_CQ_CQE_SHIFT_S 20
+#define CQ_CONTEXT_CQC_BYTE_12_CQ_CQE_SHIFT_M   \
+	(((1UL << 4) - 1) << CQ_CONTEXT_CQC_BYTE_12_CQ_CQE_SHIFT_S)
+
+#define CQ_CONTEXT_CQC_BYTE_12_CEQN_S 24
+#define CQ_CONTEXT_CQC_BYTE_12_CEQN_M   \
+	(((1UL << 5) - 1) << CQ_CONTEXT_CQC_BYTE_12_CEQN_S)
+
+#define CQ_CONTEXT_CQC_BYTE_20_CUR_CQE_BA0_H_S 0
+#define CQ_CONTEXT_CQC_BYTE_20_CUR_CQE_BA0_H_M   \
+	(((1UL << 5) - 1) << CQ_CONTEXT_CQC_BYTE_20_CUR_CQE_BA0_H_S)
+
+#define CQ_CONTEXT_CQC_BYTE_20_CQ_CUR_INDEX_S 16
+#define CQ_CONTEXT_CQC_BYTE_20_CQ_CUR_INDEX_M   \
+	(((1UL << 16) - 1) << CQ_CONTEXT_CQC_BYTE_20_CQ_CUR_INDEX_S)
+
+#define CQ_CONTEXT_CQC_BYTE_20_CQE_TPTR_ADDR_H_S 8
+#define CQ_CONTEXT_CQC_BYTE_20_CQE_TPTR_ADDR_H_M   \
+	(((1UL << 5) - 1) << CQ_CONTEXT_CQC_BYTE_20_CQE_TPTR_ADDR_H_S)
+
+#define CQ_CONTEXT_CQC_BYTE_32_CUR_CQE_BA1_H_S 0
+#define CQ_CONTEXT_CQC_BYTE_32_CUR_CQE_BA1_H_M   \
+	(((1UL << 5) - 1) << CQ_CONTEXT_CQC_BYTE_32_CUR_CQE_BA1_H_S)
+
+#define CQ_CONTEXT_CQC_BYTE_32_SE_FLAG_S 9
+
+#define CQ_CONTEXT_CQC_BYTE_32_CE_FLAG_S 8
+#define CQ_CONTEXT_CQC_BYTE_32_NOTIFICATION_FLAG_S 14
+#define CQ_CQNTEXT_CQC_BYTE_32_TYPE_OF_COMPLETION_NOTIFICATION_S 15
+
+#define CQ_CONTEXT_CQC_BYTE_32_CQ_CONS_IDX_S 16
+#define CQ_CONTEXT_CQC_BYTE_32_CQ_CONS_IDX_M   \
+	(((1UL << 16) - 1) << CQ_CONTEXT_CQC_BYTE_32_CQ_CONS_IDX_S)
+
+struct hns_roce_cqe {
+	u32 cqe_byte_4;
+	union {
+		u32 r_key;
+		u32 immediate_data;
+	};
+	u32 byte_cnt;
+	u32 cqe_byte_16;
+	u32 cqe_byte_20;
+	u32 s_mac_l;
+	u32 cqe_byte_28;
+	u32 reserved;
+};
+
+#define CQE_BYTE_4_OWNER_S 7
+#define CQE_BYTE_4_SQ_RQ_FLAG_S 14
+
+#define CQE_BYTE_4_STATUS_OF_THE_OPERATION_S 8
+#define CQE_BYTE_4_STATUS_OF_THE_OPERATION_M   \
+	(((1UL << 5) - 1) << CQE_BYTE_4_STATUS_OF_THE_OPERATION_S)
+
+#define CQE_BYTE_4_WQE_INDEX_S 16
+#define CQE_BYTE_4_WQE_INDEX_M	(((1UL << 14) - 1) << CQE_BYTE_4_WQE_INDEX_S)
+
+#define CQE_BYTE_4_OPERATION_TYPE_S 0
+#define CQE_BYTE_4_OPERATION_TYPE_M   \
+	(((1UL << 4) - 1) << CQE_BYTE_4_OPERATION_TYPE_S)
+
+#define CQE_BYTE_4_IMM_INDICATOR_S 15
+
+#define CQE_BYTE_16_LOCAL_QPN_S 0
+#define CQE_BYTE_16_LOCAL_QPN_M	(((1UL << 24) - 1) << CQE_BYTE_16_LOCAL_QPN_S)
+
+#define CQE_BYTE_20_PORT_NUM_S 26
+#define CQE_BYTE_20_PORT_NUM_M	(((1UL << 3) - 1) << CQE_BYTE_20_PORT_NUM_S)
+
+#define CQE_BYTE_20_SL_S 24
+#define CQE_BYTE_20_SL_M	(((1UL << 2) - 1) << CQE_BYTE_20_SL_S)
+
+#define CQE_BYTE_20_REMOTE_QPN_S 0
+#define CQE_BYTE_20_REMOTE_QPN_M   \
+	(((1UL << 24) - 1) << CQE_BYTE_20_REMOTE_QPN_S)
+
+#define CQE_BYTE_20_GRH_PRESENT_S 29
+
+#define CQE_BYTE_28_P_KEY_IDX_S 16
+#define CQE_BYTE_28_P_KEY_IDX_M	(((1UL << 16) - 1) << CQE_BYTE_28_P_KEY_IDX_S)
+
+#define CQ_DB_REQ_NOT_SOL	0
+#define CQ_DB_REQ_NOT		(1 << 16)
+
+struct hns_roce_v1_mpt_entry {
+	u32  mpt_byte_4;
+	u32  pbl_addr_l;
+	u32  mpt_byte_12;
+	u32  virt_addr_l;
+	u32  virt_addr_h;
+	u32  length;
+	u32  mpt_byte_28;
+	u32  pa0_l;
+	u32  mpt_byte_36;
+	u32  mpt_byte_40;
+	u32  mpt_byte_44;
+	u32  mpt_byte_48;
+	u32  pa4_l;
+	u32  mpt_byte_56;
+	u32  mpt_byte_60;
+	u32  mpt_byte_64;
+};
+
+#define MPT_BYTE_4_KEY_STATE_S 0
+#define MPT_BYTE_4_KEY_STATE_M	(((1UL << 2) - 1) << MPT_BYTE_4_KEY_STATE_S)
+
+#define MPT_BYTE_4_KEY_S 8
+#define MPT_BYTE_4_KEY_M	(((1UL << 8) - 1) << MPT_BYTE_4_KEY_S)
+
+#define MPT_BYTE_4_PAGE_SIZE_S 16
+#define MPT_BYTE_4_PAGE_SIZE_M	(((1UL << 2) - 1) << MPT_BYTE_4_PAGE_SIZE_S)
+
+#define MPT_BYTE_4_MW_TYPE_S 20
+
+#define MPT_BYTE_4_MW_BIND_ENABLE_S 21
+
+#define MPT_BYTE_4_OWN_S 22
+
+#define MPT_BYTE_4_MEMORY_LOCATION_TYPE_S 24
+#define MPT_BYTE_4_MEMORY_LOCATION_TYPE_M   \
+	(((1UL << 2) - 1) << MPT_BYTE_4_MEMORY_LOCATION_TYPE_S)
+
+#define MPT_BYTE_4_REMOTE_ATOMIC_S 26
+#define MPT_BYTE_4_LOCAL_WRITE_S 27
+#define MPT_BYTE_4_REMOTE_WRITE_S 28
+#define MPT_BYTE_4_REMOTE_READ_S 29
+#define MPT_BYTE_4_REMOTE_INVAL_ENABLE_S 30
+#define MPT_BYTE_4_ADDRESS_TYPE_S 31
+
+#define MPT_BYTE_12_PBL_ADDR_H_S 0
+#define MPT_BYTE_12_PBL_ADDR_H_M   \
+	(((1UL << 17) - 1) << MPT_BYTE_12_PBL_ADDR_H_S)
+
+#define MPT_BYTE_12_MW_BIND_COUNTER_S 17
+#define MPT_BYTE_12_MW_BIND_COUNTER_M   \
+	(((1UL << 15) - 1) << MPT_BYTE_12_MW_BIND_COUNTER_S)
+
+#define MPT_BYTE_28_PD_S 0
+#define MPT_BYTE_28_PD_M	(((1UL << 16) - 1) << MPT_BYTE_28_PD_S)
+
+#define MPT_BYTE_28_L_KEY_IDX_L_S 16
+#define MPT_BYTE_28_L_KEY_IDX_L_M   \
+	(((1UL << 16) - 1) << MPT_BYTE_28_L_KEY_IDX_L_S)
+
+#define MPT_BYTE_36_PA0_H_S 0
+#define MPT_BYTE_36_PA0_H_M	(((1UL << 5) - 1) << MPT_BYTE_36_PA0_H_S)
+
+#define MPT_BYTE_36_PA1_L_S 8
+#define MPT_BYTE_36_PA1_L_M	(((1UL << 24) - 1) << MPT_BYTE_36_PA1_L_S)
+
+#define MPT_BYTE_40_PA1_H_S 0
+#define MPT_BYTE_40_PA1_H_M	(((1UL << 13) - 1) << MPT_BYTE_40_PA1_H_S)
+
+#define MPT_BYTE_40_PA2_L_S 16
+#define MPT_BYTE_40_PA2_L_M	(((1UL << 16) - 1) << MPT_BYTE_40_PA2_L_S)
+
+#define MPT_BYTE_44_PA2_H_S 0
+#define MPT_BYTE_44_PA2_H_M	(((1UL << 21) - 1) << MPT_BYTE_44_PA2_H_S)
+
+#define MPT_BYTE_44_PA3_L_S 24
+#define MPT_BYTE_44_PA3_L_M	(((1UL << 8) - 1) << MPT_BYTE_44_PA3_L_S)
+
+#define MPT_BYTE_48_PA3_H_S 0
+#define MPT_BYTE_48_PA3_H_M	(((1UL << 29) - 1) << MPT_BYTE_48_PA3_H_S)
+
+#define MPT_BYTE_56_PA4_H_S 0
+#define MPT_BYTE_56_PA4_H_M	(((1UL << 5) - 1) << MPT_BYTE_56_PA4_H_S)
+
+#define MPT_BYTE_56_PA5_L_S 8
+#define MPT_BYTE_56_PA5_L_M	(((1UL << 24) - 1) << MPT_BYTE_56_PA5_L_S)
+
+#define MPT_BYTE_60_PA5_H_S 0
+#define MPT_BYTE_60_PA5_H_M	(((1UL << 13) - 1) << MPT_BYTE_60_PA5_H_S)
+
+#define MPT_BYTE_60_PA6_L_S 16
+#define MPT_BYTE_60_PA6_L_M	(((1UL << 16) - 1) << MPT_BYTE_60_PA6_L_S)
+
+#define MPT_BYTE_64_PA6_H_S 0
+#define MPT_BYTE_64_PA6_H_M	(((1UL << 21) - 1) << MPT_BYTE_64_PA6_H_S)
+
+#define MPT_BYTE_64_L_KEY_IDX_H_S 24
+#define MPT_BYTE_64_L_KEY_IDX_H_M   \
+	(((1UL << 8) - 1) << MPT_BYTE_64_L_KEY_IDX_H_S)
+
+struct hns_roce_wqe_ctrl_seg {
+	__be32 sgl_pa_h;
+	__be32 flag;
+	__be32 imm_data;
+	__be32 msg_length;
+};
+
+struct hns_roce_wqe_data_seg {
+	__be64    addr;
+	__be32    lkey;
+	__be32    len;
+};
+
+struct hns_roce_wqe_raddr_seg {
+	__be32 rkey;
+	__be32 len;/* reserved */
+	__be64 raddr;
+};
+
+struct hns_roce_rq_wqe_ctrl {
+
+	u32 rwqe_byte_4;
+	u32 rocee_sgl_ba_l;
+	u32 rwqe_byte_12;
+	u32 reserved[5];
+};
+
+#define RQ_WQE_CTRL_RWQE_BYTE_12_RWQE_SGE_NUM_S 16
+#define RQ_WQE_CTRL_RWQE_BYTE_12_RWQE_SGE_NUM_M   \
+	(((1UL << 6) - 1) << RQ_WQE_CTRL_RWQE_BYTE_12_RWQE_SGE_NUM_S)
+
+#define HNS_ROCE_QP_DESTROY_TIMEOUT_MSECS	10000
+
+#define GID_LEN					16
+
+struct hns_roce_ud_send_wqe {
+	u32 dmac_h;
+	u32 u32_8;
+	u32 immediate_data;
+
+	u32 u32_16;
+	union {
+		unsigned char dgid[GID_LEN];
+		struct {
+			u32 u32_20;
+			u32 u32_24;
+			u32 u32_28;
+			u32 u32_32;
+		};
+	};
+
+	u32 u32_36;
+	u32 u32_40;
+
+	u32 va0_l;
+	u32 va0_h;
+	u32 l_key0;
+
+	u32 va1_l;
+	u32 va1_h;
+	u32 l_key1;
+};
+
+#define UD_SEND_WQE_U32_4_DMAC_0_S 0
+#define UD_SEND_WQE_U32_4_DMAC_0_M   \
+	(((1UL << 8) - 1) << UD_SEND_WQE_U32_4_DMAC_0_S)
+
+#define UD_SEND_WQE_U32_4_DMAC_1_S 8
+#define UD_SEND_WQE_U32_4_DMAC_1_M   \
+	(((1UL << 8) - 1) << UD_SEND_WQE_U32_4_DMAC_1_S)
+
+#define UD_SEND_WQE_U32_4_DMAC_2_S 16
+#define UD_SEND_WQE_U32_4_DMAC_2_M   \
+	(((1UL << 8) - 1) << UD_SEND_WQE_U32_4_DMAC_2_S)
+
+#define UD_SEND_WQE_U32_4_DMAC_3_S 24
+#define UD_SEND_WQE_U32_4_DMAC_3_M   \
+	(((1UL << 8) - 1) << UD_SEND_WQE_U32_4_DMAC_3_S)
+
+#define UD_SEND_WQE_U32_8_DMAC_4_S 0
+#define UD_SEND_WQE_U32_8_DMAC_4_M   \
+	(((1UL << 8) - 1) << UD_SEND_WQE_U32_8_DMAC_4_S)
+
+#define UD_SEND_WQE_U32_8_DMAC_5_S 8
+#define UD_SEND_WQE_U32_8_DMAC_5_M   \
+	(((1UL << 8) - 1) << UD_SEND_WQE_U32_8_DMAC_5_S)
+
+#define UD_SEND_WQE_U32_8_OPERATION_TYPE_S 16
+#define UD_SEND_WQE_U32_8_OPERATION_TYPE_M   \
+	(((1UL << 4) - 1) << UD_SEND_WQE_U32_8_OPERATION_TYPE_S)
+
+#define UD_SEND_WQE_U32_8_NUMBER_OF_DATA_SEG_S 24
+#define UD_SEND_WQE_U32_8_NUMBER_OF_DATA_SEG_M   \
+	(((1UL << 6) - 1) << UD_SEND_WQE_U32_8_NUMBER_OF_DATA_SEG_S)
+
+#define UD_SEND_WQE_U32_8_SEND_GL_ROUTING_HDR_FLAG_S 31
+
+#define UD_SEND_WQE_U32_16_DEST_QP_S 0
+#define UD_SEND_WQE_U32_16_DEST_QP_M   \
+	(((1UL << 24) - 1) << UD_SEND_WQE_U32_16_DEST_QP_S)
+
+#define UD_SEND_WQE_U32_16_MAX_STATIC_RATE_S 24
+#define UD_SEND_WQE_U32_16_MAX_STATIC_RATE_M   \
+	(((1UL << 8) - 1) << UD_SEND_WQE_U32_16_MAX_STATIC_RATE_S)
+
+#define UD_SEND_WQE_U32_36_FLOW_LABEL_S 0
+#define UD_SEND_WQE_U32_36_FLOW_LABEL_M   \
+	(((1UL << 20) - 1) << UD_SEND_WQE_U32_36_FLOW_LABEL_S)
+
+#define UD_SEND_WQE_U32_36_PRIORITY_S 20
+#define UD_SEND_WQE_U32_36_PRIORITY_M   \
+	(((1UL << 4) - 1) << UD_SEND_WQE_U32_36_PRIORITY_S)
+
+#define UD_SEND_WQE_U32_36_SGID_INDEX_S 24
+#define UD_SEND_WQE_U32_36_SGID_INDEX_M   \
+	(((1UL << 8) - 1) << UD_SEND_WQE_U32_36_SGID_INDEX_S)
+
+#define UD_SEND_WQE_U32_40_HOP_LIMIT_S 0
+#define UD_SEND_WQE_U32_40_HOP_LIMIT_M   \
+	(((1UL << 8) - 1) << UD_SEND_WQE_U32_40_HOP_LIMIT_S)
+
+#define UD_SEND_WQE_U32_40_TRAFFIC_CLASS_S 8
+#define UD_SEND_WQE_U32_40_TRAFFIC_CLASS_M   \
+	(((1UL << 8) - 1) << UD_SEND_WQE_U32_40_TRAFFIC_CLASS_S)
+
+struct hns_roce_sqp_context {
+	u32 qp1c_bytes_4;
+	u32 sq_rq_bt_l;
+	u32 qp1c_bytes_12;
+	u32 qp1c_bytes_16;
+	u32 qp1c_bytes_20;
+	u32 qp1c_bytes_28;
+	u32 cur_rq_wqe_ba_l;
+	u32 qp1c_bytes_32;
+	u32 cur_sq_wqe_ba_l;
+	u32 qp1c_bytes_40;
+};
+
+#define QP1C_BYTES_4_SQ_WQE_SHIFT_S 8
+#define QP1C_BYTES_4_SQ_WQE_SHIFT_M   \
+	(((1UL << 4) - 1) << QP1C_BYTES_4_SQ_WQE_SHIFT_S)
+
+#define QP1C_BYTES_4_RQ_WQE_SHIFT_S 12
+#define QP1C_BYTES_4_RQ_WQE_SHIFT_M   \
+	(((1UL << 4) - 1) << QP1C_BYTES_4_RQ_WQE_SHIFT_S)
+
+#define QP1C_BYTES_4_PD_S 16
+#define QP1C_BYTES_4_PD_M	(((1UL << 16) - 1) << QP1C_BYTES_4_PD_S)
+
+#define QP1C_BYTES_12_SQ_RQ_BT_H_S 0
+#define QP1C_BYTES_12_SQ_RQ_BT_H_M   \
+	(((1UL << 17) - 1) << QP1C_BYTES_12_SQ_RQ_BT_H_S)
+
+#define QP1C_BYTES_16_RQ_HEAD_S 0
+#define QP1C_BYTES_16_RQ_HEAD_M	(((1UL << 15) - 1) << QP1C_BYTES_16_RQ_HEAD_S)
+
+#define QP1C_BYTES_16_PORT_NUM_S 16
+#define QP1C_BYTES_16_PORT_NUM_M   \
+	(((1UL << 3) - 1) << QP1C_BYTES_16_PORT_NUM_S)
+
+#define QP1C_BYTES_16_SIGNALING_TYPE_S 27
+#define QP1C_BYTES_16_LOCAL_ENABLE_E2E_CREDIT_S 28
+#define QP1C_BYTES_16_RQ_BA_FLG_S 29
+#define QP1C_BYTES_16_SQ_BA_FLG_S 30
+#define QP1C_BYTES_16_QP1_ERR_S 31
+
+#define QP1C_BYTES_20_SQ_HEAD_S 0
+#define QP1C_BYTES_20_SQ_HEAD_M	(((1UL << 15) - 1) << QP1C_BYTES_20_SQ_HEAD_S)
+
+#define QP1C_BYTES_20_PKEY_IDX_S 16
+#define QP1C_BYTES_20_PKEY_IDX_M   \
+	(((1UL << 16) - 1) << QP1C_BYTES_20_PKEY_IDX_S)
+
+#define QP1C_BYTES_28_CUR_RQ_WQE_BA_H_S 0
+#define QP1C_BYTES_28_CUR_RQ_WQE_BA_H_M   \
+	(((1UL << 5) - 1) << QP1C_BYTES_28_CUR_RQ_WQE_BA_H_S)
+
+#define QP1C_BYTES_28_RQ_CUR_IDX_S 16
+#define QP1C_BYTES_28_RQ_CUR_IDX_M   \
+	(((1UL << 15) - 1) << QP1C_BYTES_28_RQ_CUR_IDX_S)
+
+#define QP1C_BYTES_32_TX_CQ_NUM_S 0
+#define QP1C_BYTES_32_TX_CQ_NUM_M   \
+	(((1UL << 16) - 1) << QP1C_BYTES_32_TX_CQ_NUM_S)
+
+#define QP1C_BYTES_32_RX_CQ_NUM_S 16
+#define QP1C_BYTES_32_RX_CQ_NUM_M   \
+	(((1UL << 16) - 1) << QP1C_BYTES_32_RX_CQ_NUM_S)
+
+#define QP1C_BYTES_40_CUR_SQ_WQE_BA_H_S 0
+#define QP1C_BYTES_40_CUR_SQ_WQE_BA_H_M   \
+	(((1UL << 5) - 1) << QP1C_BYTES_40_CUR_SQ_WQE_BA_H_S)
+
+#define QP1C_BYTES_40_SQ_CUR_IDX_S 16
+#define QP1C_BYTES_40_SQ_CUR_IDX_M   \
+	(((1UL << 15) - 1) << QP1C_BYTES_40_SQ_CUR_IDX_S)
+
+#define HNS_ROCE_WQE_INLINE		(1UL<<31)
+#define HNS_ROCE_WQE_SE			(1UL<<30)
+
+#define HNS_ROCE_WQE_SGE_NUM_BIT	24
+#define HNS_ROCE_WQE_IMM		(1UL<<23)
+#define HNS_ROCE_WQE_FENCE		(1UL<<21)
+#define HNS_ROCE_WQE_CQ_NOTIFY		(1UL<<20)
+
+#define HNS_ROCE_WQE_OPCODE_SEND	(0<<16)
+#define HNS_ROCE_WQE_OPCODE_RDMA_READ	(1<<16)
+#define HNS_ROCE_WQE_OPCODE_RDMA_WRITE	(2<<16)
+#define HNS_ROCE_WQE_OPCODE_LOCAL_INV	(4<<16)
+#define HNS_ROCE_WQE_OPCODE_UD_SEND	(7<<16)
+#define HNS_ROCE_WQE_OPCODE_MASK	(15<<16)
+
+struct hns_roce_qp_context {
+	u32 qpc_bytes_4;
+	u32 qpc_bytes_8;
+	u32 qpc_bytes_12;
+	u32 qpc_bytes_16;
+	u32 sq_rq_bt_l;
+	u32 qpc_bytes_24;
+	u32 irrl_ba_l;
+	u32 qpc_bytes_32;
+	u32 qpc_bytes_36;
+	u32 dmac_l;
+	u32 qpc_bytes_44;
+	u32 qpc_bytes_48;
+	u8 dgid[16];
+	u32 qpc_bytes_68;
+	u32 cur_rq_wqe_ba_l;
+	u32 qpc_bytes_76;
+	u32 rx_rnr_time;
+	u32 qpc_bytes_84;
+	u32 qpc_bytes_88;
+	union {
+		u32 rx_sge_len;
+		u32 dma_length;
+	};
+	union {
+		u32 rx_sge_num;
+		u32 rx_send_pktn;
+		u32 r_key;
+	};
+	u32 va_l;
+	u32 va_h;
+	u32 qpc_bytes_108;
+	u32 qpc_bytes_112;
+	u32 rx_cur_sq_wqe_ba_l;
+	u32 qpc_bytes_120;
+	u32 qpc_bytes_124;
+	u32 qpc_bytes_128;
+	u32 qpc_bytes_132;
+	u32 qpc_bytes_136;
+	u32 qpc_bytes_140;
+	u32 qpc_bytes_144;
+	u32 qpc_bytes_148;
+	union {
+		u32 rnr_retry;
+		u32 ack_time;
+	};
+	u32 qpc_bytes_156;
+	u32 pkt_use_len;
+	u32 qpc_bytes_164;
+	u32 qpc_bytes_168;
+	union {
+		u32 sge_use_len;
+		u32 pa_use_len;
+	};
+	u32 qpc_bytes_176;
+	u32 qpc_bytes_180;
+	u32 tx_cur_sq_wqe_ba_l;
+	u32 qpc_bytes_188;
+	u32 rvd21;
+};
+
+#define QP_CONTEXT_QPC_BYTES_4_TRANSPORT_SERVICE_TYPE_S 0
+#define QP_CONTEXT_QPC_BYTES_4_TRANSPORT_SERVICE_TYPE_M   \
+	(((1UL << 3) - 1) << QP_CONTEXT_QPC_BYTES_4_TRANSPORT_SERVICE_TYPE_S)
+
+#define QP_CONTEXT_QPC_BYTE_4_ENABLE_FPMR_S 3
+#define QP_CONTEXT_QPC_BYTE_4_RDMA_READ_ENABLE_S 4
+#define QP_CONTEXT_QPC_BYTE_4_RDMA_WRITE_ENABLE_S 5
+#define QP_CONTEXT_QPC_BYTE_4_ATOMIC_OPERATION_ENABLE_S 6
+#define QP_CONTEXT_QPC_BYTE_4_RDMAR_USE_S 7
+
+#define QP_CONTEXT_QPC_BYTES_4_SQ_WQE_SHIFT_S 8
+#define QP_CONTEXT_QPC_BYTES_4_SQ_WQE_SHIFT_M   \
+	(((1UL << 4) - 1) << QP_CONTEXT_QPC_BYTES_4_SQ_WQE_SHIFT_S)
+
+#define QP_CONTEXT_QPC_BYTES_4_RQ_WQE_SHIFT_S 12
+#define QP_CONTEXT_QPC_BYTES_4_RQ_WQE_SHIFT_M   \
+	(((1UL << 4) - 1) << QP_CONTEXT_QPC_BYTES_4_RQ_WQE_SHIFT_S)
+
+#define QP_CONTEXT_QPC_BYTES_4_PD_S 16
+#define QP_CONTEXT_QPC_BYTES_4_PD_M   \
+	(((1UL << 16) - 1) << QP_CONTEXT_QPC_BYTES_4_PD_S)
+
+#define QP_CONTEXT_QPC_BYTES_8_TX_COMPLETION_S 0
+#define QP_CONTEXT_QPC_BYTES_8_TX_COMPLETION_M   \
+	(((1UL << 16) - 1) << QP_CONTEXT_QPC_BYTES_8_TX_COMPLETION_S)
+
+#define QP_CONTEXT_QPC_BYTES_8_RX_COMPLETION_S 16
+#define QP_CONTEXT_QPC_BYTES_8_RX_COMPLETION_M   \
+	(((1UL << 16) - 1) << QP_CONTEXT_QPC_BYTES_8_RX_COMPLETION_S)
+
+#define QP_CONTEXT_QPC_BYTES_12_SRQ_NUMBER_S 0
+#define QP_CONTEXT_QPC_BYTES_12_SRQ_NUMBER_M   \
+	(((1UL << 16) - 1) << QP_CONTEXT_QPC_BYTES_12_SRQ_NUMBER_S)
+
+#define QP_CONTEXT_QPC_BYTES_12_P_KEY_INDEX_S 16
+#define QP_CONTEXT_QPC_BYTES_12_P_KEY_INDEX_M   \
+	(((1UL << 16) - 1) << QP_CONTEXT_QPC_BYTES_12_P_KEY_INDEX_S)
+
+#define QP_CONTEXT_QPC_BYTES_16_QP_NUM_S 0
+#define QP_CONTEXT_QPC_BYTES_16_QP_NUM_M   \
+	(((1UL << 24) - 1) << QP_CONTEXT_QPC_BYTES_16_QP_NUM_S)
+
+#define QP_CONTEXT_QPC_BYTES_24_SQ_RQ_BT_H_S 0
+#define QP_CONTEXT_QPC_BYTES_24_SQ_RQ_BT_H_M   \
+	(((1UL << 17) - 1) << QP_CONTEXT_QPC_BYTES_24_SQ_RQ_BT_H_S)
+
+#define QP_CONTEXT_QPC_BYTES_24_MINIMUM_RNR_NAK_TIMER_S 18
+#define QP_CONTEXT_QPC_BYTES_24_MINIMUM_RNR_NAK_TIMER_M   \
+	(((1UL << 5) - 1) << QP_CONTEXT_QPC_BYTES_24_MINIMUM_RNR_NAK_TIMER_S)
+
+#define QP_CONTEXT_QPC_BYTE_24_REMOTE_ENABLE_E2E_CREDITS_S 23
+
+#define QP_CONTEXT_QPC_BYTES_32_IRRL_BA_H_S 0
+#define QP_CONTEXT_QPC_BYTES_32_IRRL_BA_H_M   \
+	(((1UL << 17) - 1) << QP_CONTEXT_QPC_BYTES_32_IRRL_BA_H_S)
+
+#define QP_CONTEXT_QPC_BYTES_32_MIG_STATE_S 18
+#define QP_CONTEXT_QPC_BYTES_32_MIG_STATE_M   \
+	(((1UL << 2) - 1) << QP_CONTEXT_QPC_BYTES_32_MIG_STATE_S)
+
+#define QP_CONTEXT_QPC_BYTE_32_LOCAL_ENABLE_E2E_CREDITS_S 20
+#define QP_CONTEXT_QPC_BYTE_32_SIGNALING_TYPE_S 21
+#define QP_CONTEXT_QPC_BYTE_32_LOOPBACK_INDICATOR_S 22
+#define QP_CONTEXT_QPC_BYTE_32_GLOBAL_HEADER_S 23
+
+#define QP_CONTEXT_QPC_BYTES_32_RESPONDER_RESOURCES_S 24
+#define QP_CONTEXT_QPC_BYTES_32_RESPONDER_RESOURCES_M   \
+	(((1UL << 8) - 1) << QP_CONTEXT_QPC_BYTES_32_RESPONDER_RESOURCES_S)
+
+#define QP_CONTEXT_QPC_BYTES_36_DEST_QP_S 0
+#define QP_CONTEXT_QPC_BYTES_36_DEST_QP_M   \
+	(((1UL << 24) - 1) << QP_CONTEXT_QPC_BYTES_36_DEST_QP_S)
+
+#define QP_CONTEXT_QPC_BYTES_36_SGID_INDEX_S 24
+#define QP_CONTEXT_QPC_BYTES_36_SGID_INDEX_M   \
+	(((1UL << 8) - 1) << QP_CONTEXT_QPC_BYTES_36_SGID_INDEX_S)
+
+#define QP_CONTEXT_QPC_BYTES_44_DMAC_H_S 0
+#define QP_CONTEXT_QPC_BYTES_44_DMAC_H_M   \
+	(((1UL << 16) - 1) << QP_CONTEXT_QPC_BYTES_44_DMAC_H_S)
+
+#define QP_CONTEXT_QPC_BYTES_44_MAXIMUM_STATIC_RATE_S 16
+#define QP_CONTEXT_QPC_BYTES_44_MAXIMUM_STATIC_RATE_M   \
+	(((1UL << 8) - 1) << QP_CONTEXT_QPC_BYTES_44_MAXIMUM_STATIC_RATE_S)
+
+#define QP_CONTEXT_QPC_BYTES_44_HOPLMT_S 24
+#define QP_CONTEXT_QPC_BYTES_44_HOPLMT_M   \
+	(((1UL << 8) - 1) << QP_CONTEXT_QPC_BYTES_44_HOPLMT_S)
+
+#define QP_CONTEXT_QPC_BYTES_48_FLOWLABEL_S 0
+#define QP_CONTEXT_QPC_BYTES_48_FLOWLABEL_M   \
+	(((1UL << 20) - 1) << QP_CONTEXT_QPC_BYTES_48_FLOWLABEL_S)
+
+#define QP_CONTEXT_QPC_BYTES_48_TCLASS_S 20
+#define QP_CONTEXT_QPC_BYTES_48_TCLASS_M   \
+	(((1UL << 8) - 1) << QP_CONTEXT_QPC_BYTES_48_TCLASS_S)
+
+#define QP_CONTEXT_QPC_BYTES_48_MTU_S 28
+#define QP_CONTEXT_QPC_BYTES_48_MTU_M   \
+	(((1UL << 4) - 1) << QP_CONTEXT_QPC_BYTES_48_MTU_S)
+
+#define QP_CONTEXT_QPC_BYTES_68_RQ_HEAD_S 0
+#define QP_CONTEXT_QPC_BYTES_68_RQ_HEAD_M   \
+	(((1UL << 15) - 1) << QP_CONTEXT_QPC_BYTES_68_RQ_HEAD_S)
+
+#define QP_CONTEXT_QPC_BYTES_68_RQ_CUR_INDEX_S 16
+#define QP_CONTEXT_QPC_BYTES_68_RQ_CUR_INDEX_M   \
+	(((1UL << 15) - 1) << QP_CONTEXT_QPC_BYTES_68_RQ_CUR_INDEX_S)
+
+#define QP_CONTEXT_QPC_BYTES_76_CUR_RQ_WQE_BA_H_S 0
+#define QP_CONTEXT_QPC_BYTES_76_CUR_RQ_WQE_BA_H_M   \
+	(((1UL << 5) - 1) << QP_CONTEXT_QPC_BYTES_76_CUR_RQ_WQE_BA_H_S)
+
+#define QP_CONTEXT_QPC_BYTES_76_RX_REQ_MSN_S 8
+#define QP_CONTEXT_QPC_BYTES_76_RX_REQ_MSN_M   \
+	(((1UL << 24) - 1) << QP_CONTEXT_QPC_BYTES_76_RX_REQ_MSN_S)
+
+#define QP_CONTEXT_QPC_BYTES_84_LAST_ACK_PSN_S 0
+#define QP_CONTEXT_QPC_BYTES_84_LAST_ACK_PSN_M   \
+	(((1UL << 24) - 1) << QP_CONTEXT_QPC_BYTES_84_LAST_ACK_PSN_S)
+
+#define QP_CONTEXT_QPC_BYTES_84_TRRL_HEAD_S 24
+#define QP_CONTEXT_QPC_BYTES_84_TRRL_HEAD_M   \
+	(((1UL << 8) - 1) << QP_CONTEXT_QPC_BYTES_84_TRRL_HEAD_S)
+
+#define QP_CONTEXT_QPC_BYTES_88_RX_REQ_EPSN_S 0
+#define QP_CONTEXT_QPC_BYTES_88_RX_REQ_EPSN_M   \
+	(((1UL << 24) - 1) << QP_CONTEXT_QPC_BYTES_88_RX_REQ_EPSN_S)
+
+#define QP_CONTEXT_QPC_BYTES_88_RX_REQ_PSN_ERR_FLAG_S 24
+#define QP_CONTEXT_QPC_BYTES_88_RX_LAST_OPCODE_FLG_S 25
+
+#define QP_CONTEXT_QPC_BYTES_88_RQ_REQ_LAST_OPERATION_TYPE_S 26
+#define QP_CONTEXT_QPC_BYTES_88_RQ_REQ_LAST_OPERATION_TYPE_M   \
+	(((1UL << 2) - 1) << \
+	QP_CONTEXT_QPC_BYTES_88_RQ_REQ_LAST_OPERATION_TYPE_S)
+
+#define QP_CONTEXT_QPC_BYTES_88_RQ_REQ_RDMA_WR_FLAG_S 29
+#define QP_CONTEXT_QPC_BYTES_88_RQ_REQ_RDMA_WR_FLAG_M   \
+	(((1UL << 2) - 1) << QP_CONTEXT_QPC_BYTES_88_RQ_REQ_RDMA_WR_FLAG_S)
+
+#define QP_CONTEXT_QPC_BYTES_108_TRRL_SDB_PSN_S 0
+#define QP_CONTEXT_QPC_BYTES_108_TRRL_SDB_PSN_M   \
+	(((1UL << 24) - 1) << QP_CONTEXT_QPC_BYTES_108_TRRL_SDB_PSN_S)
+
+#define QP_CONTEXT_QPC_BYTES_108_TRRL_SDB_PSN_FLG_S 24
+#define QP_CONTEXT_QPC_BYTES_108_TRRL_TDB_PSN_FLG_S 25
+
+#define QP_CONTEXT_QPC_BYTES_112_TRRL_TDB_PSN_S 0
+#define QP_CONTEXT_QPC_BYTES_112_TRRL_TDB_PSN_M   \
+	(((1UL << 24) - 1) << QP_CONTEXT_QPC_BYTES_112_TRRL_TDB_PSN_S)
+
+#define QP_CONTEXT_QPC_BYTES_112_TRRL_TAIL_S 24
+#define QP_CONTEXT_QPC_BYTES_112_TRRL_TAIL_M   \
+	(((1UL << 8) - 1) << QP_CONTEXT_QPC_BYTES_112_TRRL_TAIL_S)
+
+#define QP_CONTEXT_QPC_BYTES_120_RX_CUR_SQ_WQE_BA_H_S 0
+#define QP_CONTEXT_QPC_BYTES_120_RX_CUR_SQ_WQE_BA_H_M   \
+	(((1UL << 5) - 1) << QP_CONTEXT_QPC_BYTES_120_RX_CUR_SQ_WQE_BA_H_S)
+
+#define QP_CONTEXT_QPC_BYTES_124_RX_ACK_MSN_S 0
+#define QP_CONTEXT_QPC_BYTES_124_RX_ACK_MSN_M   \
+	(((1UL << 15) - 1) << QP_CONTEXT_QPC_BYTES_124_RX_ACK_MSN_S)
+
+#define QP_CONTEXT_QPC_BYTES_124_IRRL_MSG_IDX_S 16
+#define QP_CONTEXT_QPC_BYTES_124_IRRL_MSG_IDX_M   \
+	(((1UL << 15) - 1) << QP_CONTEXT_QPC_BYTES_124_IRRL_MSG_IDX_S)
+
+#define QP_CONTEXT_QPC_BYTES_128_RX_ACK_EPSN_S 0
+#define QP_CONTEXT_QPC_BYTES_128_RX_ACK_EPSN_M   \
+	(((1UL << 24) - 1) << QP_CONTEXT_QPC_BYTES_128_RX_ACK_EPSN_S)
+
+#define QP_CONTEXT_QPC_BYTES_128_RX_ACK_PSN_ERR_FLG_S 24
+
+#define QP_CONTEXT_QPC_BYTES_128_ACK_LAST_OPERATION_TYPE_S 25
+#define QP_CONTEXT_QPC_BYTES_128_ACK_LAST_OPERATION_TYPE_M   \
+	(((1UL << 2) - 1) << QP_CONTEXT_QPC_BYTES_128_ACK_LAST_OPERATION_TYPE_S)
+
+#define QP_CONTEXT_QPC_BYTES_128_IRRL_PSN_VLD_FLG_S 27
+
+#define QP_CONTEXT_QPC_BYTES_132_IRRL_PSN_S 0
+#define QP_CONTEXT_QPC_BYTES_132_IRRL_PSN_M   \
+	(((1UL << 24) - 1) << QP_CONTEXT_QPC_BYTES_132_IRRL_PSN_S)
+
+#define QP_CONTEXT_QPC_BYTES_132_IRRL_TAIL_S 24
+#define QP_CONTEXT_QPC_BYTES_132_IRRL_TAIL_M   \
+	(((1UL << 8) - 1) << QP_CONTEXT_QPC_BYTES_132_IRRL_TAIL_S)
+
+#define QP_CONTEXT_QPC_BYTES_136_RETRY_MSG_PSN_S 0
+#define QP_CONTEXT_QPC_BYTES_136_RETRY_MSG_PSN_M   \
+	(((1UL << 24) - 1) << QP_CONTEXT_QPC_BYTES_136_RETRY_MSG_PSN_S)
+
+#define QP_CONTEXT_QPC_BYTES_136_RETRY_MSG_FPKT_PSN_L_S 24
+#define QP_CONTEXT_QPC_BYTES_136_RETRY_MSG_FPKT_PSN_L_M   \
+	(((1UL << 8) - 1) << QP_CONTEXT_QPC_BYTES_136_RETRY_MSG_FPKT_PSN_L_S)
+
+#define QP_CONTEXT_QPC_BYTES_140_RETRY_MSG_FPKT_PSN_H_S 0
+#define QP_CONTEXT_QPC_BYTES_140_RETRY_MSG_FPKT_PSN_H_M   \
+	(((1UL << 16) - 1) << QP_CONTEXT_QPC_BYTES_140_RETRY_MSG_FPKT_PSN_H_S)
+
+#define QP_CONTEXT_QPC_BYTES_140_RETRY_MSG_MSN_S 16
+#define QP_CONTEXT_QPC_BYTES_140_RETRY_MSG_MSN_M   \
+	(((1UL << 15) - 1) << QP_CONTEXT_QPC_BYTES_140_RETRY_MSG_MSN_S)
+
+#define QP_CONTEXT_QPC_BYTES_140_RNR_RETRY_FLG_S 31
+
+#define QP_CONTEXT_QPC_BYTES_144_QP_STATE_S 0
+#define QP_CONTEXT_QPC_BYTES_144_QP_STATE_M   \
+	(((1UL << 3) - 1) << QP_CONTEXT_QPC_BYTES_144_QP_STATE_S)
+
+#define QP_CONTEXT_QPC_BYTES_148_CHECK_FLAG_S 0
+#define QP_CONTEXT_QPC_BYTES_148_CHECK_FLAG_M   \
+	(((1UL << 2) - 1) << QP_CONTEXT_QPC_BYTES_148_CHECK_FLAG_S)
+
+#define QP_CONTEXT_QPC_BYTES_148_RETRY_COUNT_S 2
+#define QP_CONTEXT_QPC_BYTES_148_RETRY_COUNT_M   \
+	(((1UL << 3) - 1) << QP_CONTEXT_QPC_BYTES_148_RETRY_COUNT_S)
+
+#define QP_CONTEXT_QPC_BYTES_148_RNR_RETRY_COUNT_S 5
+#define QP_CONTEXT_QPC_BYTES_148_RNR_RETRY_COUNT_M   \
+	(((1UL << 3) - 1) << QP_CONTEXT_QPC_BYTES_148_RNR_RETRY_COUNT_S)
+
+#define QP_CONTEXT_QPC_BYTES_148_LSN_S 8
+#define QP_CONTEXT_QPC_BYTES_148_LSN_M   \
+	(((1UL << 16) - 1) << QP_CONTEXT_QPC_BYTES_148_LSN_S)
+
+#define QP_CONTEXT_QPC_BYTES_156_RETRY_COUNT_INIT_S 0
+#define QP_CONTEXT_QPC_BYTES_156_RETRY_COUNT_INIT_M   \
+	(((1UL << 3) - 1) << QP_CONTEXT_QPC_BYTES_156_RETRY_COUNT_INIT_S)
+
+#define QP_CONTEXT_QPC_BYTES_156_ACK_TIMEOUT_S 3
+#define QP_CONTEXT_QPC_BYTES_156_ACK_TIMEOUT_M   \
+	(((1UL << 5) - 1) << QP_CONTEXT_QPC_BYTES_156_ACK_TIMEOUT_S)
+
+#define QP_CONTEXT_QPC_BYTES_156_RNR_RETRY_COUNT_INIT_S 8
+#define QP_CONTEXT_QPC_BYTES_156_RNR_RETRY_COUNT_INIT_M   \
+	(((1UL << 3) - 1) << QP_CONTEXT_QPC_BYTES_156_RNR_RETRY_COUNT_INIT_S)
+
+#define QP_CONTEXT_QPC_BYTES_156_PORT_NUM_S 11
+#define QP_CONTEXT_QPC_BYTES_156_PORT_NUM_M   \
+	(((1UL << 3) - 1) << QP_CONTEXT_QPC_BYTES_156_PORT_NUM_S)
+
+#define QP_CONTEXT_QPC_BYTES_156_SL_S 14
+#define QP_CONTEXT_QPC_BYTES_156_SL_M   \
+	(((1UL << 2) - 1) << QP_CONTEXT_QPC_BYTES_156_SL_S)
+
+#define QP_CONTEXT_QPC_BYTES_156_INITIATOR_DEPTH_S 16
+#define QP_CONTEXT_QPC_BYTES_156_INITIATOR_DEPTH_M   \
+	(((1UL << 8) - 1) << QP_CONTEXT_QPC_BYTES_156_INITIATOR_DEPTH_S)
+
+#define QP_CONTEXT_QPC_BYTES_156_ACK_REQ_IND_S 24
+#define QP_CONTEXT_QPC_BYTES_156_ACK_REQ_IND_M   \
+	(((1UL << 2) - 1) << QP_CONTEXT_QPC_BYTES_156_ACK_REQ_IND_S)
+
+#define QP_CONTEXT_QPC_BYTES_164_SQ_PSN_S 0
+#define QP_CONTEXT_QPC_BYTES_164_SQ_PSN_M   \
+	(((1UL << 24) - 1) << QP_CONTEXT_QPC_BYTES_164_SQ_PSN_S)
+
+#define QP_CONTEXT_QPC_BYTES_164_IRRL_HEAD_S 24
+#define QP_CONTEXT_QPC_BYTES_164_IRRL_HEAD_M   \
+	(((1UL << 8) - 1) << QP_CONTEXT_QPC_BYTES_164_IRRL_HEAD_S)
+
+#define QP_CONTEXT_QPC_BYTES_168_RETRY_SQ_PSN_S 0
+#define QP_CONTEXT_QPC_BYTES_168_RETRY_SQ_PSN_M   \
+	(((1UL << 24) - 1) << QP_CONTEXT_QPC_BYTES_168_RETRY_SQ_PSN_S)
+
+#define QP_CONTEXT_QPC_BYTES_168_SGE_USE_FLA_S 24
+#define QP_CONTEXT_QPC_BYTES_168_SGE_USE_FLA_M   \
+	(((1UL << 2) - 1) << QP_CONTEXT_QPC_BYTES_168_SGE_USE_FLA_S)
+
+#define QP_CONTEXT_QPC_BYTES_168_DB_TYPE_S 26
+#define QP_CONTEXT_QPC_BYTES_168_DB_TYPE_M   \
+	(((1UL << 2) - 1) << QP_CONTEXT_QPC_BYTES_168_DB_TYPE_S)
+
+#define QP_CONTEXT_QPC_BYTES_168_MSG_LP_IND_S 28
+#define QP_CONTEXT_QPC_BYTES_168_CSDB_LP_IND_S 29
+#define QP_CONTEXT_QPC_BYTES_168_QP_ERR_FLG_S 30
+
+#define QP_CONTEXT_QPC_BYTES_176_DB_CUR_INDEX_S 0
+#define QP_CONTEXT_QPC_BYTES_176_DB_CUR_INDEX_M   \
+	(((1UL << 15) - 1) << QP_CONTEXT_QPC_BYTES_176_DB_CUR_INDEX_S)
+
+#define QP_CONTEXT_QPC_BYTES_176_RETRY_DB_CUR_INDEX_S 16
+#define QP_CONTEXT_QPC_BYTES_176_RETRY_DB_CUR_INDEX_M   \
+	(((1UL << 15) - 1) << QP_CONTEXT_QPC_BYTES_176_RETRY_DB_CUR_INDEX_S)
+
+#define QP_CONTEXT_QPC_BYTES_180_SQ_HEAD_S 0
+#define QP_CONTEXT_QPC_BYTES_180_SQ_HEAD_M   \
+	(((1UL << 15) - 1) << QP_CONTEXT_QPC_BYTES_180_SQ_HEAD_S)
+
+#define QP_CONTEXT_QPC_BYTES_180_SQ_CUR_INDEX_S 16
+#define QP_CONTEXT_QPC_BYTES_180_SQ_CUR_INDEX_M   \
+	(((1UL << 15) - 1) << QP_CONTEXT_QPC_BYTES_180_SQ_CUR_INDEX_S)
+
+#define QP_CONTEXT_QPC_BYTES_188_TX_CUR_SQ_WQE_BA_H_S 0
+#define QP_CONTEXT_QPC_BYTES_188_TX_CUR_SQ_WQE_BA_H_M   \
+	(((1UL << 5) - 1) << QP_CONTEXT_QPC_BYTES_188_TX_CUR_SQ_WQE_BA_H_S)
+
+#define QP_CONTEXT_QPC_BYTES_188_PKT_RETRY_FLG_S 8
+
+#define QP_CONTEXT_QPC_BYTES_188_TX_RETRY_CUR_INDEX_S 16
+#define QP_CONTEXT_QPC_BYTES_188_TX_RETRY_CUR_INDEX_M   \
+	(((1UL << 15) - 1) << QP_CONTEXT_QPC_BYTES_188_TX_RETRY_CUR_INDEX_S)
+
+struct hns_roce_rq_db {
+	u32    u32_4;
+	u32    u32_8;
+};
+
+#define RQ_DOORBELL_U32_4_RQ_HEAD_S 0
+#define RQ_DOORBELL_U32_4_RQ_HEAD_M   \
+	(((1UL << 15) - 1) << RQ_DOORBELL_U32_4_RQ_HEAD_S)
+
+#define RQ_DOORBELL_U32_8_QPN_S 0
+#define RQ_DOORBELL_U32_8_QPN_M   (((1UL << 24) - 1) << RQ_DOORBELL_U32_8_QPN_S)
+
+#define RQ_DOORBELL_U32_8_CMD_S 28
+#define RQ_DOORBELL_U32_8_CMD_M   (((1UL << 3) - 1) << RQ_DOORBELL_U32_8_CMD_S)
+
+#define RQ_DOORBELL_U32_8_HW_SYNC_S 31
+
+struct hns_roce_sq_db {
+	u32    u32_4;
+	u32    u32_8;
+};
+
+#define SQ_DOORBELL_U32_4_SQ_HEAD_S 0
+#define SQ_DOORBELL_U32_4_SQ_HEAD_M   \
+	(((1UL << 15) - 1) << SQ_DOORBELL_U32_4_SQ_HEAD_S)
+
+#define SQ_DOORBELL_U32_4_PORT_S 18
+#define SQ_DOORBELL_U32_4_PORT_M  (((1UL << 3) - 1) << SQ_DOORBELL_U32_4_PORT_S)
+
+#define SQ_DOORBELL_U32_8_QPN_S 0
+#define SQ_DOORBELL_U32_8_QPN_M   (((1UL << 24) - 1) << SQ_DOORBELL_U32_8_QPN_S)
+
+#define SQ_DOORBELL_HW_SYNC_S 31
+
+struct hns_roce_ext_db {
+	int esdb_dep;
+	int eodb_dep;
+	struct hns_roce_buf_list *sdb_buf_list;
+	struct hns_roce_buf_list *odb_buf_list;
+};
+
+struct hns_roce_db_table {
+	int  sdb_ext_mod;
+	int  odb_ext_mod;
+	struct hns_roce_ext_db *ext_db;
+};
+
+struct hns_roce_bt_table {
+	struct hns_roce_buf_list qpc_buf;
+	struct hns_roce_buf_list mtpt_buf;
+	struct hns_roce_buf_list cqc_buf;
+};
+
+struct hns_roce_v1_priv {
+	struct hns_roce_db_table  db_table;
+	struct hns_roce_raq_table raq_table;
+	struct hns_roce_bt_table  bt_table;
+};
+
+int hns_dsaf_roce_reset(struct fwnode_handle *dsaf_fwnode, bool dereset);
+
+#endif
diff --git a/drivers/infiniband/hw/hns/hns_roce_main.c b/drivers/infiniband/hw/hns/hns_roce_main.c
new file mode 100644
index 000000000000..764e35a54457
--- /dev/null
+++ b/drivers/infiniband/hw/hns/hns_roce_main.c
@@ -0,0 +1,1168 @@
+/*
+ * Copyright (c) 2016 Hisilicon Limited.
+ * Copyright (c) 2007, 2008 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/acpi.h>
+#include <linux/of_platform.h>
+#include <rdma/ib_addr.h>
+#include <rdma/ib_smi.h>
+#include <rdma/ib_user_verbs.h>
+#include "hns_roce_common.h"
+#include "hns_roce_device.h"
+#include "hns_roce_user.h"
+#include "hns_roce_hem.h"
+
+/**
+ * hns_roce_addrconf_ifid_eui48 - Get default gid.
+ * @eui: eui.
+ * @vlan_id:  gid
+ * @dev:  net device
+ * Description:
+ *    MAC convert to GID
+ *        gid[0..7] = fe80 0000 0000 0000
+ *        gid[8] = mac[0] ^ 2
+ *        gid[9] = mac[1]
+ *        gid[10] = mac[2]
+ *        gid[11] = ff        (VLAN ID high byte (4 MS bits))
+ *        gid[12] = fe        (VLAN ID low byte)
+ *        gid[13] = mac[3]
+ *        gid[14] = mac[4]
+ *        gid[15] = mac[5]
+ */
+static void hns_roce_addrconf_ifid_eui48(u8 *eui, u16 vlan_id,
+					 struct net_device *dev)
+{
+	memcpy(eui, dev->dev_addr, 3);
+	memcpy(eui + 5, dev->dev_addr + 3, 3);
+	if (vlan_id < 0x1000) {
+		eui[3] = vlan_id >> 8;
+		eui[4] = vlan_id & 0xff;
+	} else {
+		eui[3] = 0xff;
+		eui[4] = 0xfe;
+	}
+	eui[0] ^= 2;
+}
+
+static void hns_roce_make_default_gid(struct net_device *dev, union ib_gid *gid)
+{
+	memset(gid, 0, sizeof(*gid));
+	gid->raw[0] = 0xFE;
+	gid->raw[1] = 0x80;
+	hns_roce_addrconf_ifid_eui48(&gid->raw[8], 0xffff, dev);
+}
+
+/**
+ * hns_get_gid_index - Get gid index.
+ * @hr_dev: pointer to structure hns_roce_dev.
+ * @port:  port, value range: 0 ~ MAX
+ * @gid_index:  gid_index, value range: 0 ~ MAX
+ * Description:
+ *    N ports shared gids, allocation method as follow:
+ *		GID[0][0], GID[1][0],.....GID[N - 1][0],
+ *		GID[0][0], GID[1][0],.....GID[N - 1][0],
+ *		And so on
+ */
+int hns_get_gid_index(struct hns_roce_dev *hr_dev, u8 port, int gid_index)
+{
+	return gid_index * hr_dev->caps.num_ports + port;
+}
+
+static int hns_roce_set_gid(struct hns_roce_dev *hr_dev, u8 port, int gid_index,
+		     union ib_gid *gid)
+{
+	struct device *dev = &hr_dev->pdev->dev;
+	u8 gid_idx = 0;
+
+	if (gid_index >= hr_dev->caps.gid_table_len[port]) {
+		dev_err(dev, "gid_index %d illegal, port %d gid range: 0~%d\n",
+			gid_index, port, hr_dev->caps.gid_table_len[port] - 1);
+		return -EINVAL;
+	}
+
+	gid_idx = hns_get_gid_index(hr_dev, port, gid_index);
+
+	if (!memcmp(gid, &hr_dev->iboe.gid_table[gid_idx], sizeof(*gid)))
+		return -EINVAL;
+
+	memcpy(&hr_dev->iboe.gid_table[gid_idx], gid, sizeof(*gid));
+
+	hr_dev->hw->set_gid(hr_dev, port, gid_index, gid);
+
+	return 0;
+}
+
+static void hns_roce_set_mac(struct hns_roce_dev *hr_dev, u8 port, u8 *addr)
+{
+	u8 phy_port;
+	u32 i = 0;
+
+	if (!memcmp(hr_dev->dev_addr[port], addr, MAC_ADDR_OCTET_NUM))
+		return;
+
+	for (i = 0; i < MAC_ADDR_OCTET_NUM; i++)
+		hr_dev->dev_addr[port][i] = addr[i];
+
+	phy_port = hr_dev->iboe.phy_port[port];
+	hr_dev->hw->set_mac(hr_dev, phy_port, addr);
+}
+
+static void hns_roce_set_mtu(struct hns_roce_dev *hr_dev, u8 port, int mtu)
+{
+	u8 phy_port = hr_dev->iboe.phy_port[port];
+	enum ib_mtu tmp;
+
+	tmp = iboe_get_mtu(mtu);
+	if (!tmp)
+		tmp = IB_MTU_256;
+
+	hr_dev->hw->set_mtu(hr_dev, phy_port, tmp);
+}
+
+static void hns_roce_update_gids(struct hns_roce_dev *hr_dev, int port)
+{
+	struct ib_event event;
+
+	/* Refresh gid in ib_cache */
+	event.device = &hr_dev->ib_dev;
+	event.element.port_num = port + 1;
+	event.event = IB_EVENT_GID_CHANGE;
+	ib_dispatch_event(&event);
+}
+
+static int handle_en_event(struct hns_roce_dev *hr_dev, u8 port,
+			   unsigned long event)
+{
+	struct device *dev = &hr_dev->pdev->dev;
+	struct net_device *netdev;
+	unsigned long flags;
+	union ib_gid gid;
+	int ret = 0;
+
+	netdev = hr_dev->iboe.netdevs[port];
+	if (!netdev) {
+		dev_err(dev, "port(%d) can't find netdev\n", port);
+		return -ENODEV;
+	}
+
+	spin_lock_irqsave(&hr_dev->iboe.lock, flags);
+
+	switch (event) {
+	case NETDEV_UP:
+	case NETDEV_CHANGE:
+	case NETDEV_REGISTER:
+	case NETDEV_CHANGEADDR:
+		hns_roce_set_mac(hr_dev, port, netdev->dev_addr);
+		hns_roce_make_default_gid(netdev, &gid);
+		ret = hns_roce_set_gid(hr_dev, port, 0, &gid);
+		if (!ret)
+			hns_roce_update_gids(hr_dev, port);
+		break;
+	case NETDEV_DOWN:
+		/*
+		* In v1 engine, only support all ports closed together.
+		*/
+		break;
+	default:
+		dev_dbg(dev, "NETDEV event = 0x%x!\n", (u32)(event));
+		break;
+	}
+
+	spin_unlock_irqrestore(&hr_dev->iboe.lock, flags);
+	return ret;
+}
+
+static int hns_roce_netdev_event(struct notifier_block *self,
+				 unsigned long event, void *ptr)
+{
+	struct net_device *dev = netdev_notifier_info_to_dev(ptr);
+	struct hns_roce_ib_iboe *iboe = NULL;
+	struct hns_roce_dev *hr_dev = NULL;
+	u8 port = 0;
+	int ret = 0;
+
+	hr_dev = container_of(self, struct hns_roce_dev, iboe.nb);
+	iboe = &hr_dev->iboe;
+
+	for (port = 0; port < hr_dev->caps.num_ports; port++) {
+		if (dev == iboe->netdevs[port]) {
+			ret = handle_en_event(hr_dev, port, event);
+			if (ret)
+				return NOTIFY_DONE;
+			break;
+		}
+	}
+
+	return NOTIFY_DONE;
+}
+
+static void hns_roce_addr_event(int event, struct net_device *event_netdev,
+				struct hns_roce_dev *hr_dev, union ib_gid *gid)
+{
+	struct hns_roce_ib_iboe *iboe = NULL;
+	int gid_table_len = 0;
+	unsigned long flags;
+	union ib_gid zgid;
+	u8 gid_idx = 0;
+	u8 port = 0;
+	int i = 0;
+	int free;
+	struct net_device *real_dev = rdma_vlan_dev_real_dev(event_netdev) ?
+				      rdma_vlan_dev_real_dev(event_netdev) :
+				      event_netdev;
+
+	if (event != NETDEV_UP && event != NETDEV_DOWN)
+		return;
+
+	iboe = &hr_dev->iboe;
+	while (port < hr_dev->caps.num_ports) {
+		if (real_dev == iboe->netdevs[port])
+			break;
+		port++;
+	}
+
+	if (port >= hr_dev->caps.num_ports) {
+		dev_dbg(&hr_dev->pdev->dev, "can't find netdev\n");
+		return;
+	}
+
+	memset(zgid.raw, 0, sizeof(zgid.raw));
+	free = -1;
+	gid_table_len = hr_dev->caps.gid_table_len[port];
+
+	spin_lock_irqsave(&hr_dev->iboe.lock, flags);
+
+	for (i = 0; i < gid_table_len; i++) {
+		gid_idx = hns_get_gid_index(hr_dev, port, i);
+		if (!memcmp(gid->raw, iboe->gid_table[gid_idx].raw,
+			    sizeof(gid->raw)))
+			break;
+		if (free < 0 && !memcmp(zgid.raw,
+			iboe->gid_table[gid_idx].raw, sizeof(zgid.raw)))
+			free = i;
+	}
+
+	if (i >= gid_table_len) {
+		if (free < 0) {
+			spin_unlock_irqrestore(&hr_dev->iboe.lock, flags);
+			dev_dbg(&hr_dev->pdev->dev,
+				"gid_index overflow, port(%d)\n", port);
+			return;
+		}
+		if (!hns_roce_set_gid(hr_dev, port, free, gid))
+			hns_roce_update_gids(hr_dev, port);
+	} else if (event == NETDEV_DOWN) {
+		if (!hns_roce_set_gid(hr_dev, port, i, &zgid))
+			hns_roce_update_gids(hr_dev, port);
+	}
+
+	spin_unlock_irqrestore(&hr_dev->iboe.lock, flags);
+}
+
+static int hns_roce_inet_event(struct notifier_block *self, unsigned long event,
+			       void *ptr)
+{
+	struct in_ifaddr *ifa = ptr;
+	struct hns_roce_dev *hr_dev;
+	struct net_device *dev = ifa->ifa_dev->dev;
+	union ib_gid gid;
+
+	ipv6_addr_set_v4mapped(ifa->ifa_address, (struct in6_addr *)&gid);
+
+	hr_dev = container_of(self, struct hns_roce_dev, iboe.nb_inet);
+
+	hns_roce_addr_event(event, dev, hr_dev, &gid);
+
+	return NOTIFY_DONE;
+}
+
+static int hns_roce_setup_mtu_gids(struct hns_roce_dev *hr_dev)
+{
+	struct in_ifaddr *ifa_list = NULL;
+	union ib_gid gid = {{0} };
+	u32 ipaddr = 0;
+	int index = 0;
+	int ret = 0;
+	u8 i = 0;
+
+	for (i = 0; i < hr_dev->caps.num_ports; i++) {
+		hns_roce_set_mtu(hr_dev, i,
+				 ib_mtu_enum_to_int(hr_dev->caps.max_mtu));
+		hns_roce_set_mac(hr_dev, i, hr_dev->iboe.netdevs[i]->dev_addr);
+
+		if (hr_dev->iboe.netdevs[i]->ip_ptr) {
+			ifa_list = hr_dev->iboe.netdevs[i]->ip_ptr->ifa_list;
+			index = 1;
+			while (ifa_list) {
+				ipaddr = ifa_list->ifa_address;
+				ipv6_addr_set_v4mapped(ipaddr,
+						       (struct in6_addr *)&gid);
+				ret = hns_roce_set_gid(hr_dev, i, index, &gid);
+				if (ret)
+					break;
+				index++;
+				ifa_list = ifa_list->ifa_next;
+			}
+			hns_roce_update_gids(hr_dev, i);
+		}
+	}
+
+	return ret;
+}
+
+static int hns_roce_query_device(struct ib_device *ib_dev,
+				 struct ib_device_attr *props,
+				 struct ib_udata *uhw)
+{
+	struct hns_roce_dev *hr_dev = to_hr_dev(ib_dev);
+
+	memset(props, 0, sizeof(*props));
+
+	props->sys_image_guid = hr_dev->sys_image_guid;
+	props->max_mr_size = (u64)(~(0ULL));
+	props->page_size_cap = hr_dev->caps.page_size_cap;
+	props->vendor_id = hr_dev->vendor_id;
+	props->vendor_part_id = hr_dev->vendor_part_id;
+	props->hw_ver = hr_dev->hw_rev;
+	props->max_qp = hr_dev->caps.num_qps;
+	props->max_qp_wr = hr_dev->caps.max_wqes;
+	props->device_cap_flags = IB_DEVICE_PORT_ACTIVE_EVENT |
+				  IB_DEVICE_RC_RNR_NAK_GEN;
+	props->max_sge = hr_dev->caps.max_sq_sg;
+	props->max_sge_rd = 1;
+	props->max_cq = hr_dev->caps.num_cqs;
+	props->max_cqe = hr_dev->caps.max_cqes;
+	props->max_mr = hr_dev->caps.num_mtpts;
+	props->max_pd = hr_dev->caps.num_pds;
+	props->max_qp_rd_atom = hr_dev->caps.max_qp_dest_rdma;
+	props->max_qp_init_rd_atom = hr_dev->caps.max_qp_init_rdma;
+	props->atomic_cap = IB_ATOMIC_NONE;
+	props->max_pkeys = 1;
+	props->local_ca_ack_delay = hr_dev->caps.local_ca_ack_delay;
+
+	return 0;
+}
+
+static struct net_device *hns_roce_get_netdev(struct ib_device *ib_dev,
+					      u8 port_num)
+{
+	struct hns_roce_dev *hr_dev = to_hr_dev(ib_dev);
+	struct net_device *ndev;
+
+	if (port_num < 1 || port_num > hr_dev->caps.num_ports)
+		return NULL;
+
+	rcu_read_lock();
+
+	ndev = hr_dev->iboe.netdevs[port_num - 1];
+	if (ndev)
+		dev_hold(ndev);
+
+	rcu_read_unlock();
+	return ndev;
+}
+
+static int hns_roce_query_port(struct ib_device *ib_dev, u8 port_num,
+			       struct ib_port_attr *props)
+{
+	struct hns_roce_dev *hr_dev = to_hr_dev(ib_dev);
+	struct device *dev = &hr_dev->pdev->dev;
+	struct net_device *net_dev;
+	unsigned long flags;
+	enum ib_mtu mtu;
+	u8 port;
+
+	assert(port_num > 0);
+	port = port_num - 1;
+
+	memset(props, 0, sizeof(*props));
+
+	props->max_mtu = hr_dev->caps.max_mtu;
+	props->gid_tbl_len = hr_dev->caps.gid_table_len[port];
+	props->port_cap_flags = IB_PORT_CM_SUP | IB_PORT_REINIT_SUP |
+				IB_PORT_VENDOR_CLASS_SUP |
+				IB_PORT_BOOT_MGMT_SUP;
+	props->max_msg_sz = HNS_ROCE_MAX_MSG_LEN;
+	props->pkey_tbl_len = 1;
+	props->active_width = IB_WIDTH_4X;
+	props->active_speed = 1;
+
+	spin_lock_irqsave(&hr_dev->iboe.lock, flags);
+
+	net_dev = hr_dev->iboe.netdevs[port];
+	if (!net_dev) {
+		spin_unlock_irqrestore(&hr_dev->iboe.lock, flags);
+		dev_err(dev, "find netdev %d failed!\r\n", port);
+		return -EINVAL;
+	}
+
+	mtu = iboe_get_mtu(net_dev->mtu);
+	props->active_mtu = mtu ? min(props->max_mtu, mtu) : IB_MTU_256;
+	props->state = (netif_running(net_dev) && netif_carrier_ok(net_dev)) ?
+			IB_PORT_ACTIVE : IB_PORT_DOWN;
+	props->phys_state = (props->state == IB_PORT_ACTIVE) ? 5 : 3;
+
+	spin_unlock_irqrestore(&hr_dev->iboe.lock, flags);
+
+	return 0;
+}
+
+static enum rdma_link_layer hns_roce_get_link_layer(struct ib_device *device,
+						    u8 port_num)
+{
+	return IB_LINK_LAYER_ETHERNET;
+}
+
+static int hns_roce_query_gid(struct ib_device *ib_dev, u8 port_num, int index,
+			      union ib_gid *gid)
+{
+	struct hns_roce_dev *hr_dev = to_hr_dev(ib_dev);
+	struct device *dev = &hr_dev->pdev->dev;
+	u8 gid_idx = 0;
+	u8 port;
+
+	if (port_num < 1 || port_num > hr_dev->caps.num_ports ||
+	    index >= hr_dev->caps.gid_table_len[port_num - 1]) {
+		dev_err(dev,
+			"port_num %d index %d illegal! correct range: port_num 1~%d index 0~%d!\n",
+			port_num, index, hr_dev->caps.num_ports,
+			hr_dev->caps.gid_table_len[port_num - 1] - 1);
+		return -EINVAL;
+	}
+
+	port = port_num - 1;
+	gid_idx = hns_get_gid_index(hr_dev, port, index);
+	if (gid_idx >= HNS_ROCE_MAX_GID_NUM) {
+		dev_err(dev, "port_num %d index %d illegal! total gid num %d!\n",
+			port_num, index, HNS_ROCE_MAX_GID_NUM);
+		return -EINVAL;
+	}
+
+	memcpy(gid->raw, hr_dev->iboe.gid_table[gid_idx].raw,
+	       HNS_ROCE_GID_SIZE);
+
+	return 0;
+}
+
+static int hns_roce_query_pkey(struct ib_device *ib_dev, u8 port, u16 index,
+			       u16 *pkey)
+{
+	*pkey = PKEY_ID;
+
+	return 0;
+}
+
+static int hns_roce_modify_device(struct ib_device *ib_dev, int mask,
+				  struct ib_device_modify *props)
+{
+	unsigned long flags;
+
+	if (mask & ~IB_DEVICE_MODIFY_NODE_DESC)
+		return -EOPNOTSUPP;
+
+	if (mask & IB_DEVICE_MODIFY_NODE_DESC) {
+		spin_lock_irqsave(&to_hr_dev(ib_dev)->sm_lock, flags);
+		memcpy(ib_dev->node_desc, props->node_desc, NODE_DESC_SIZE);
+		spin_unlock_irqrestore(&to_hr_dev(ib_dev)->sm_lock, flags);
+	}
+
+	return 0;
+}
+
+static int hns_roce_modify_port(struct ib_device *ib_dev, u8 port_num, int mask,
+				struct ib_port_modify *props)
+{
+	return 0;
+}
+
+static struct ib_ucontext *hns_roce_alloc_ucontext(struct ib_device *ib_dev,
+						   struct ib_udata *udata)
+{
+	int ret = 0;
+	struct hns_roce_ucontext *context;
+	struct hns_roce_ib_alloc_ucontext_resp resp;
+	struct hns_roce_dev *hr_dev = to_hr_dev(ib_dev);
+
+	resp.qp_tab_size = hr_dev->caps.num_qps;
+
+	context = kmalloc(sizeof(*context), GFP_KERNEL);
+	if (!context)
+		return ERR_PTR(-ENOMEM);
+
+	ret = hns_roce_uar_alloc(hr_dev, &context->uar);
+	if (ret)
+		goto error_fail_uar_alloc;
+
+	ret = ib_copy_to_udata(udata, &resp, sizeof(resp));
+	if (ret)
+		goto error_fail_copy_to_udata;
+
+	return &context->ibucontext;
+
+error_fail_copy_to_udata:
+	hns_roce_uar_free(hr_dev, &context->uar);
+
+error_fail_uar_alloc:
+	kfree(context);
+
+	return ERR_PTR(ret);
+}
+
+static int hns_roce_dealloc_ucontext(struct ib_ucontext *ibcontext)
+{
+	struct hns_roce_ucontext *context = to_hr_ucontext(ibcontext);
+
+	hns_roce_uar_free(to_hr_dev(ibcontext->device), &context->uar);
+	kfree(context);
+
+	return 0;
+}
+
+static int hns_roce_mmap(struct ib_ucontext *context,
+			 struct vm_area_struct *vma)
+{
+	if (((vma->vm_end - vma->vm_start) % PAGE_SIZE) != 0)
+		return -EINVAL;
+
+	if (vma->vm_pgoff == 0) {
+		vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+		if (io_remap_pfn_range(vma, vma->vm_start,
+				       to_hr_ucontext(context)->uar.pfn,
+				       PAGE_SIZE, vma->vm_page_prot))
+			return -EAGAIN;
+
+	} else {
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int hns_roce_port_immutable(struct ib_device *ib_dev, u8 port_num,
+				   struct ib_port_immutable *immutable)
+{
+	struct ib_port_attr attr;
+	int ret;
+
+	ret = hns_roce_query_port(ib_dev, port_num, &attr);
+	if (ret)
+		return ret;
+
+	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 void hns_roce_unregister_device(struct hns_roce_dev *hr_dev)
+{
+	struct hns_roce_ib_iboe *iboe = &hr_dev->iboe;
+
+	unregister_inetaddr_notifier(&iboe->nb_inet);
+	unregister_netdevice_notifier(&iboe->nb);
+	ib_unregister_device(&hr_dev->ib_dev);
+}
+
+static int hns_roce_register_device(struct hns_roce_dev *hr_dev)
+{
+	int ret;
+	struct hns_roce_ib_iboe *iboe = NULL;
+	struct ib_device *ib_dev = NULL;
+	struct device *dev = &hr_dev->pdev->dev;
+
+	iboe = &hr_dev->iboe;
+	spin_lock_init(&iboe->lock);
+
+	ib_dev = &hr_dev->ib_dev;
+	strlcpy(ib_dev->name, "hisi_%d", IB_DEVICE_NAME_MAX);
+
+	ib_dev->owner			= THIS_MODULE;
+	ib_dev->node_type		= RDMA_NODE_IB_CA;
+	ib_dev->dma_device		= dev;
+
+	ib_dev->phys_port_cnt		= hr_dev->caps.num_ports;
+	ib_dev->local_dma_lkey		= hr_dev->caps.reserved_lkey;
+	ib_dev->num_comp_vectors	= hr_dev->caps.num_comp_vectors;
+	ib_dev->uverbs_abi_ver		= 1;
+	ib_dev->uverbs_cmd_mask		=
+		(1ULL << IB_USER_VERBS_CMD_GET_CONTEXT) |
+		(1ULL << IB_USER_VERBS_CMD_QUERY_DEVICE) |
+		(1ULL << IB_USER_VERBS_CMD_QUERY_PORT) |
+		(1ULL << IB_USER_VERBS_CMD_ALLOC_PD) |
+		(1ULL << IB_USER_VERBS_CMD_DEALLOC_PD) |
+		(1ULL << IB_USER_VERBS_CMD_REG_MR) |
+		(1ULL << IB_USER_VERBS_CMD_DEREG_MR) |
+		(1ULL << IB_USER_VERBS_CMD_CREATE_COMP_CHANNEL) |
+		(1ULL << IB_USER_VERBS_CMD_CREATE_CQ) |
+		(1ULL << IB_USER_VERBS_CMD_DESTROY_CQ) |
+		(1ULL << IB_USER_VERBS_CMD_CREATE_QP) |
+		(1ULL << IB_USER_VERBS_CMD_MODIFY_QP) |
+		(1ULL << IB_USER_VERBS_CMD_QUERY_QP) |
+		(1ULL << IB_USER_VERBS_CMD_DESTROY_QP);
+
+	/* HCA||device||port */
+	ib_dev->modify_device		= hns_roce_modify_device;
+	ib_dev->query_device		= hns_roce_query_device;
+	ib_dev->query_port		= hns_roce_query_port;
+	ib_dev->modify_port		= hns_roce_modify_port;
+	ib_dev->get_link_layer		= hns_roce_get_link_layer;
+	ib_dev->get_netdev		= hns_roce_get_netdev;
+	ib_dev->query_gid		= hns_roce_query_gid;
+	ib_dev->query_pkey		= hns_roce_query_pkey;
+	ib_dev->alloc_ucontext		= hns_roce_alloc_ucontext;
+	ib_dev->dealloc_ucontext	= hns_roce_dealloc_ucontext;
+	ib_dev->mmap			= hns_roce_mmap;
+
+	/* PD */
+	ib_dev->alloc_pd		= hns_roce_alloc_pd;
+	ib_dev->dealloc_pd		= hns_roce_dealloc_pd;
+
+	/* AH */
+	ib_dev->create_ah		= hns_roce_create_ah;
+	ib_dev->query_ah		= hns_roce_query_ah;
+	ib_dev->destroy_ah		= hns_roce_destroy_ah;
+
+	/* QP */
+	ib_dev->create_qp		= hns_roce_create_qp;
+	ib_dev->modify_qp		= hns_roce_modify_qp;
+	ib_dev->query_qp		= hr_dev->hw->query_qp;
+	ib_dev->destroy_qp		= hr_dev->hw->destroy_qp;
+	ib_dev->post_send		= hr_dev->hw->post_send;
+	ib_dev->post_recv		= hr_dev->hw->post_recv;
+
+	/* CQ */
+	ib_dev->create_cq		= hns_roce_ib_create_cq;
+	ib_dev->destroy_cq		= hns_roce_ib_destroy_cq;
+	ib_dev->req_notify_cq		= hr_dev->hw->req_notify_cq;
+	ib_dev->poll_cq			= hr_dev->hw->poll_cq;
+
+	/* MR */
+	ib_dev->get_dma_mr		= hns_roce_get_dma_mr;
+	ib_dev->reg_user_mr		= hns_roce_reg_user_mr;
+	ib_dev->dereg_mr		= hns_roce_dereg_mr;
+
+	/* OTHERS */
+	ib_dev->get_port_immutable	= hns_roce_port_immutable;
+
+	ret = ib_register_device(ib_dev, NULL);
+	if (ret) {
+		dev_err(dev, "ib_register_device failed!\n");
+		return ret;
+	}
+
+	ret = hns_roce_setup_mtu_gids(hr_dev);
+	if (ret) {
+		dev_err(dev, "roce_setup_mtu_gids failed!\n");
+		goto error_failed_setup_mtu_gids;
+	}
+
+	iboe->nb.notifier_call = hns_roce_netdev_event;
+	ret = register_netdevice_notifier(&iboe->nb);
+	if (ret) {
+		dev_err(dev, "register_netdevice_notifier failed!\n");
+		goto error_failed_setup_mtu_gids;
+	}
+
+	iboe->nb_inet.notifier_call = hns_roce_inet_event;
+	ret = register_inetaddr_notifier(&iboe->nb_inet);
+	if (ret) {
+		dev_err(dev, "register inet addr notifier failed!\n");
+		goto error_failed_register_inetaddr_notifier;
+	}
+
+	return 0;
+
+error_failed_register_inetaddr_notifier:
+	unregister_netdevice_notifier(&iboe->nb);
+
+error_failed_setup_mtu_gids:
+	ib_unregister_device(ib_dev);
+
+	return ret;
+}
+
+static const struct of_device_id hns_roce_of_match[] = {
+	{ .compatible = "hisilicon,hns-roce-v1", .data = &hns_roce_hw_v1, },
+	{},
+};
+MODULE_DEVICE_TABLE(of, hns_roce_of_match);
+
+static const struct acpi_device_id hns_roce_acpi_match[] = {
+	{ "HISI00D1", (kernel_ulong_t)&hns_roce_hw_v1 },
+	{},
+};
+MODULE_DEVICE_TABLE(acpi, hns_roce_acpi_match);
+
+static int hns_roce_node_match(struct device *dev, void *fwnode)
+{
+	return dev->fwnode == fwnode;
+}
+
+static struct
+platform_device *hns_roce_find_pdev(struct fwnode_handle *fwnode)
+{
+	struct device *dev;
+
+	/* get the 'device'corresponding to matching 'fwnode' */
+	dev = bus_find_device(&platform_bus_type, NULL,
+			      fwnode, hns_roce_node_match);
+	/* get the platform device */
+	return dev ? to_platform_device(dev) : NULL;
+}
+
+static int hns_roce_get_cfg(struct hns_roce_dev *hr_dev)
+{
+	int i;
+	int ret;
+	u8 phy_port;
+	int port_cnt = 0;
+	struct device *dev = &hr_dev->pdev->dev;
+	struct device_node *net_node;
+	struct net_device *netdev = NULL;
+	struct platform_device *pdev = NULL;
+	struct resource *res;
+
+	/* check if we are compatible with the underlying SoC */
+	if (dev_of_node(dev)) {
+		const struct of_device_id *of_id;
+
+		of_id = of_match_node(hns_roce_of_match, dev->of_node);
+		if (!of_id) {
+			dev_err(dev, "device is not compatible!\n");
+			return -ENXIO;
+		}
+		hr_dev->hw = (struct hns_roce_hw *)of_id->data;
+		if (!hr_dev->hw) {
+			dev_err(dev, "couldn't get H/W specific DT data!\n");
+			return -ENXIO;
+		}
+	} else if (is_acpi_device_node(dev->fwnode)) {
+		const struct acpi_device_id *acpi_id;
+
+		acpi_id = acpi_match_device(hns_roce_acpi_match, dev);
+		if (!acpi_id) {
+			dev_err(dev, "device is not compatible!\n");
+			return -ENXIO;
+		}
+		hr_dev->hw = (struct hns_roce_hw *) acpi_id->driver_data;
+		if (!hr_dev->hw) {
+			dev_err(dev, "couldn't get H/W specific ACPI data!\n");
+			return -ENXIO;
+		}
+	} else {
+		dev_err(dev, "can't read compatibility data from DT or ACPI\n");
+		return -ENXIO;
+	}
+
+	/* get the mapped register base address */
+	res = platform_get_resource(hr_dev->pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(dev, "memory resource not found!\n");
+		return -EINVAL;
+	}
+	hr_dev->reg_base = devm_ioremap_resource(dev, res);
+	if (IS_ERR(hr_dev->reg_base))
+		return PTR_ERR(hr_dev->reg_base);
+
+	/* read the node_guid of IB device from the DT or ACPI */
+	ret = device_property_read_u8_array(dev, "node-guid",
+					    (u8 *)&hr_dev->ib_dev.node_guid,
+					    GUID_LEN);
+	if (ret) {
+		dev_err(dev, "couldn't get node_guid from DT or ACPI!\n");
+		return ret;
+	}
+
+	/* get the RoCE associated ethernet ports or netdevices */
+	for (i = 0; i < HNS_ROCE_MAX_PORTS; i++) {
+		if (dev_of_node(dev)) {
+			net_node = of_parse_phandle(dev->of_node, "eth-handle",
+						    i);
+			if (!net_node)
+				continue;
+			pdev = of_find_device_by_node(net_node);
+		} else if (is_acpi_device_node(dev->fwnode)) {
+			struct acpi_reference_args args;
+			struct fwnode_handle *fwnode;
+
+			ret = acpi_node_get_property_reference(dev->fwnode,
+							       "eth-handle",
+							       i, &args);
+			if (ret)
+				continue;
+			fwnode = acpi_fwnode_handle(args.adev);
+			pdev = hns_roce_find_pdev(fwnode);
+		} else {
+			dev_err(dev, "cannot read data from DT or ACPI\n");
+			return -ENXIO;
+		}
+
+		if (pdev) {
+			netdev = platform_get_drvdata(pdev);
+			phy_port = (u8)i;
+			if (netdev) {
+				hr_dev->iboe.netdevs[port_cnt] = netdev;
+				hr_dev->iboe.phy_port[port_cnt] = phy_port;
+			} else {
+				dev_err(dev, "no netdev found with pdev %s\n",
+					pdev->name);
+				return -ENODEV;
+			}
+			port_cnt++;
+		}
+	}
+
+	if (port_cnt == 0) {
+		dev_err(dev, "unable to get eth-handle for available ports!\n");
+		return -EINVAL;
+	}
+
+	hr_dev->caps.num_ports = port_cnt;
+
+	/* cmd issue mode: 0 is poll, 1 is event */
+	hr_dev->cmd_mod = 1;
+	hr_dev->loop_idc = 0;
+
+	/* read the interrupt names from the DT or ACPI */
+	ret = device_property_read_string_array(dev, "interrupt-names",
+						hr_dev->irq_names,
+						HNS_ROCE_MAX_IRQ_NUM);
+	if (ret < 0) {
+		dev_err(dev, "couldn't get interrupt names from DT or ACPI!\n");
+		return ret;
+	}
+
+	/* fetch the interrupt numbers */
+	for (i = 0; i < HNS_ROCE_MAX_IRQ_NUM; i++) {
+		hr_dev->irq[i] = platform_get_irq(hr_dev->pdev, i);
+		if (hr_dev->irq[i] <= 0) {
+			dev_err(dev, "platform get of irq[=%d] failed!\n", i);
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
+static int hns_roce_init_hem(struct hns_roce_dev *hr_dev)
+{
+	int ret;
+	struct device *dev = &hr_dev->pdev->dev;
+
+	ret = hns_roce_init_hem_table(hr_dev, &hr_dev->mr_table.mtt_table,
+				      HEM_TYPE_MTT, hr_dev->caps.mtt_entry_sz,
+				      hr_dev->caps.num_mtt_segs, 1);
+	if (ret) {
+		dev_err(dev, "Failed to init MTT context memory, aborting.\n");
+		return ret;
+	}
+
+	ret = hns_roce_init_hem_table(hr_dev, &hr_dev->mr_table.mtpt_table,
+				      HEM_TYPE_MTPT, hr_dev->caps.mtpt_entry_sz,
+				      hr_dev->caps.num_mtpts, 1);
+	if (ret) {
+		dev_err(dev, "Failed to init MTPT context memory, aborting.\n");
+		goto err_unmap_mtt;
+	}
+
+	ret = hns_roce_init_hem_table(hr_dev, &hr_dev->qp_table.qp_table,
+				      HEM_TYPE_QPC, hr_dev->caps.qpc_entry_sz,
+				      hr_dev->caps.num_qps, 1);
+	if (ret) {
+		dev_err(dev, "Failed to init QP context memory, aborting.\n");
+		goto err_unmap_dmpt;
+	}
+
+	ret = hns_roce_init_hem_table(hr_dev, &hr_dev->qp_table.irrl_table,
+				      HEM_TYPE_IRRL,
+				      hr_dev->caps.irrl_entry_sz *
+				      hr_dev->caps.max_qp_init_rdma,
+				      hr_dev->caps.num_qps, 1);
+	if (ret) {
+		dev_err(dev, "Failed to init irrl_table memory, aborting.\n");
+		goto err_unmap_qp;
+	}
+
+	ret = hns_roce_init_hem_table(hr_dev, &hr_dev->cq_table.table,
+				      HEM_TYPE_CQC, hr_dev->caps.cqc_entry_sz,
+				      hr_dev->caps.num_cqs, 1);
+	if (ret) {
+		dev_err(dev, "Failed to init CQ context memory, aborting.\n");
+		goto err_unmap_irrl;
+	}
+
+	return 0;
+
+err_unmap_irrl:
+	hns_roce_cleanup_hem_table(hr_dev, &hr_dev->qp_table.irrl_table);
+
+err_unmap_qp:
+	hns_roce_cleanup_hem_table(hr_dev, &hr_dev->qp_table.qp_table);
+
+err_unmap_dmpt:
+	hns_roce_cleanup_hem_table(hr_dev, &hr_dev->mr_table.mtpt_table);
+
+err_unmap_mtt:
+	hns_roce_cleanup_hem_table(hr_dev, &hr_dev->mr_table.mtt_table);
+
+	return ret;
+}
+
+/**
+* hns_roce_setup_hca - setup host channel adapter
+* @hr_dev: pointer to hns roce device
+* Return : int
+*/
+static int hns_roce_setup_hca(struct hns_roce_dev *hr_dev)
+{
+	int ret;
+	struct device *dev = &hr_dev->pdev->dev;
+
+	spin_lock_init(&hr_dev->sm_lock);
+	spin_lock_init(&hr_dev->bt_cmd_lock);
+
+	ret = hns_roce_init_uar_table(hr_dev);
+	if (ret) {
+		dev_err(dev, "Failed to initialize uar table. aborting\n");
+		return ret;
+	}
+
+	ret = hns_roce_uar_alloc(hr_dev, &hr_dev->priv_uar);
+	if (ret) {
+		dev_err(dev, "Failed to allocate priv_uar.\n");
+		goto err_uar_table_free;
+	}
+
+	ret = hns_roce_init_pd_table(hr_dev);
+	if (ret) {
+		dev_err(dev, "Failed to init protected domain table.\n");
+		goto err_uar_alloc_free;
+	}
+
+	ret = hns_roce_init_mr_table(hr_dev);
+	if (ret) {
+		dev_err(dev, "Failed to init memory region table.\n");
+		goto err_pd_table_free;
+	}
+
+	ret = hns_roce_init_cq_table(hr_dev);
+	if (ret) {
+		dev_err(dev, "Failed to init completion queue table.\n");
+		goto err_mr_table_free;
+	}
+
+	ret = hns_roce_init_qp_table(hr_dev);
+	if (ret) {
+		dev_err(dev, "Failed to init queue pair table.\n");
+		goto err_cq_table_free;
+	}
+
+	return 0;
+
+err_cq_table_free:
+	hns_roce_cleanup_cq_table(hr_dev);
+
+err_mr_table_free:
+	hns_roce_cleanup_mr_table(hr_dev);
+
+err_pd_table_free:
+	hns_roce_cleanup_pd_table(hr_dev);
+
+err_uar_alloc_free:
+	hns_roce_uar_free(hr_dev, &hr_dev->priv_uar);
+
+err_uar_table_free:
+	hns_roce_cleanup_uar_table(hr_dev);
+	return ret;
+}
+
+/**
+* hns_roce_probe - RoCE driver entrance
+* @pdev: pointer to platform device
+* Return : int
+*
+*/
+static int hns_roce_probe(struct platform_device *pdev)
+{
+	int ret;
+	struct hns_roce_dev *hr_dev;
+	struct device *dev = &pdev->dev;
+
+	hr_dev = (struct hns_roce_dev *)ib_alloc_device(sizeof(*hr_dev));
+	if (!hr_dev)
+		return -ENOMEM;
+
+	memset((u8 *)hr_dev + sizeof(struct ib_device), 0,
+		sizeof(struct hns_roce_dev) - sizeof(struct ib_device));
+
+	hr_dev->pdev = pdev;
+	platform_set_drvdata(pdev, hr_dev);
+
+	if (dma_set_mask_and_coherent(dev, DMA_BIT_MASK(64ULL)) &&
+	    dma_set_mask_and_coherent(dev, DMA_BIT_MASK(32ULL))) {
+		dev_err(dev, "Not usable DMA addressing mode\n");
+		ret = -EIO;
+		goto error_failed_get_cfg;
+	}
+
+	ret = hns_roce_get_cfg(hr_dev);
+	if (ret) {
+		dev_err(dev, "Get Configuration failed!\n");
+		goto error_failed_get_cfg;
+	}
+
+	ret = hr_dev->hw->reset(hr_dev, true);
+	if (ret) {
+		dev_err(dev, "Reset RoCE engine failed!\n");
+		goto error_failed_get_cfg;
+	}
+
+	hr_dev->hw->hw_profile(hr_dev);
+
+	ret = hns_roce_cmd_init(hr_dev);
+	if (ret) {
+		dev_err(dev, "cmd init failed!\n");
+		goto error_failed_cmd_init;
+	}
+
+	ret = hns_roce_init_eq_table(hr_dev);
+	if (ret) {
+		dev_err(dev, "eq init failed!\n");
+		goto error_failed_eq_table;
+	}
+
+	if (hr_dev->cmd_mod) {
+		ret = hns_roce_cmd_use_events(hr_dev);
+		if (ret) {
+			dev_err(dev, "Switch to event-driven cmd failed!\n");
+			goto error_failed_use_event;
+		}
+	}
+
+	ret = hns_roce_init_hem(hr_dev);
+	if (ret) {
+		dev_err(dev, "init HEM(Hardware Entry Memory) failed!\n");
+		goto error_failed_init_hem;
+	}
+
+	ret = hns_roce_setup_hca(hr_dev);
+	if (ret) {
+		dev_err(dev, "setup hca failed!\n");
+		goto error_failed_setup_hca;
+	}
+
+	ret = hr_dev->hw->hw_init(hr_dev);
+	if (ret) {
+		dev_err(dev, "hw_init failed!\n");
+		goto error_failed_engine_init;
+	}
+
+	ret = hns_roce_register_device(hr_dev);
+	if (ret)
+		goto error_failed_register_device;
+
+	return 0;
+
+error_failed_register_device:
+	hr_dev->hw->hw_exit(hr_dev);
+
+error_failed_engine_init:
+	hns_roce_cleanup_bitmap(hr_dev);
+
+error_failed_setup_hca:
+	hns_roce_cleanup_hem(hr_dev);
+
+error_failed_init_hem:
+	if (hr_dev->cmd_mod)
+		hns_roce_cmd_use_polling(hr_dev);
+
+error_failed_use_event:
+	hns_roce_cleanup_eq_table(hr_dev);
+
+error_failed_eq_table:
+	hns_roce_cmd_cleanup(hr_dev);
+
+error_failed_cmd_init:
+	ret = hr_dev->hw->reset(hr_dev, false);
+	if (ret)
+		dev_err(&hr_dev->pdev->dev, "roce_engine reset fail\n");
+
+error_failed_get_cfg:
+	ib_dealloc_device(&hr_dev->ib_dev);
+
+	return ret;
+}
+
+/**
+* hns_roce_remove - remove RoCE device
+* @pdev: pointer to platform device
+*/
+static int hns_roce_remove(struct platform_device *pdev)
+{
+	struct hns_roce_dev *hr_dev = platform_get_drvdata(pdev);
+
+	hns_roce_unregister_device(hr_dev);
+	hr_dev->hw->hw_exit(hr_dev);
+	hns_roce_cleanup_bitmap(hr_dev);
+	hns_roce_cleanup_hem(hr_dev);
+
+	if (hr_dev->cmd_mod)
+		hns_roce_cmd_use_polling(hr_dev);
+
+	hns_roce_cleanup_eq_table(hr_dev);
+	hns_roce_cmd_cleanup(hr_dev);
+	hr_dev->hw->reset(hr_dev, false);
+
+	ib_dealloc_device(&hr_dev->ib_dev);
+
+	return 0;
+}
+
+static struct platform_driver hns_roce_driver = {
+	.probe = hns_roce_probe,
+	.remove = hns_roce_remove,
+	.driver = {
+		.name = DRV_NAME,
+		.of_match_table = hns_roce_of_match,
+		.acpi_match_table = ACPI_PTR(hns_roce_acpi_match),
+	},
+};
+
+module_platform_driver(hns_roce_driver);
+
+MODULE_LICENSE("Dual BSD/GPL");
+MODULE_AUTHOR("Wei Hu <xavier.huwei@huawei.com>");
+MODULE_AUTHOR("Nenglong Zhao <zhaonenglong@hisilicon.com>");
+MODULE_AUTHOR("Lijun Ou <oulijun@huawei.com>");
+MODULE_DESCRIPTION("HNS RoCE Driver");
diff --git a/drivers/infiniband/hw/hns/hns_roce_mr.c b/drivers/infiniband/hw/hns/hns_roce_mr.c
new file mode 100644
index 000000000000..fb87883ead34
--- /dev/null
+++ b/drivers/infiniband/hw/hns/hns_roce_mr.c
@@ -0,0 +1,617 @@
+/*
+ * Copyright (c) 2016 Hisilicon Limited.
+ * Copyright (c) 2007, 2008 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/platform_device.h>
+#include <rdma/ib_umem.h>
+#include "hns_roce_device.h"
+#include "hns_roce_cmd.h"
+#include "hns_roce_hem.h"
+
+static u32 hw_index_to_key(unsigned long ind)
+{
+	return (u32)(ind >> 24) | (ind << 8);
+}
+
+static unsigned long key_to_hw_index(u32 key)
+{
+	return (key << 24) | (key >> 8);
+}
+
+static int hns_roce_sw2hw_mpt(struct hns_roce_dev *hr_dev,
+			      struct hns_roce_cmd_mailbox *mailbox,
+			      unsigned long mpt_index)
+{
+	return hns_roce_cmd_mbox(hr_dev, mailbox->dma, 0, mpt_index, 0,
+				 HNS_ROCE_CMD_SW2HW_MPT,
+				 HNS_ROCE_CMD_TIME_CLASS_B);
+}
+
+static int hns_roce_hw2sw_mpt(struct hns_roce_dev *hr_dev,
+			      struct hns_roce_cmd_mailbox *mailbox,
+			      unsigned long mpt_index)
+{
+	return hns_roce_cmd_mbox(hr_dev, 0, mailbox ? mailbox->dma : 0,
+				 mpt_index, !mailbox, HNS_ROCE_CMD_HW2SW_MPT,
+				 HNS_ROCE_CMD_TIME_CLASS_B);
+}
+
+static int hns_roce_buddy_alloc(struct hns_roce_buddy *buddy, int order,
+				unsigned long *seg)
+{
+	int o;
+	u32 m;
+
+	spin_lock(&buddy->lock);
+
+	for (o = order; o <= buddy->max_order; ++o) {
+		if (buddy->num_free[o]) {
+			m = 1 << (buddy->max_order - o);
+			*seg = find_first_bit(buddy->bits[o], m);
+			if (*seg < m)
+				goto found;
+		}
+	}
+	spin_unlock(&buddy->lock);
+	return -1;
+
+ found:
+	clear_bit(*seg, buddy->bits[o]);
+	--buddy->num_free[o];
+
+	while (o > order) {
+		--o;
+		*seg <<= 1;
+		set_bit(*seg ^ 1, buddy->bits[o]);
+		++buddy->num_free[o];
+	}
+
+	spin_unlock(&buddy->lock);
+
+	*seg <<= order;
+	return 0;
+}
+
+static void hns_roce_buddy_free(struct hns_roce_buddy *buddy, unsigned long seg,
+				int order)
+{
+	seg >>= order;
+
+	spin_lock(&buddy->lock);
+
+	while (test_bit(seg ^ 1, buddy->bits[order])) {
+		clear_bit(seg ^ 1, buddy->bits[order]);
+		--buddy->num_free[order];
+		seg >>= 1;
+		++order;
+	}
+
+	set_bit(seg, buddy->bits[order]);
+	++buddy->num_free[order];
+
+	spin_unlock(&buddy->lock);
+}
+
+static int hns_roce_buddy_init(struct hns_roce_buddy *buddy, int max_order)
+{
+	int i, s;
+
+	buddy->max_order = max_order;
+	spin_lock_init(&buddy->lock);
+
+	buddy->bits = kzalloc((buddy->max_order + 1) * sizeof(long *),
+			       GFP_KERNEL);
+	buddy->num_free = kzalloc((buddy->max_order + 1) * sizeof(int *),
+				   GFP_KERNEL);
+	if (!buddy->bits || !buddy->num_free)
+		goto err_out;
+
+	for (i = 0; i <= buddy->max_order; ++i) {
+		s = BITS_TO_LONGS(1 << (buddy->max_order - i));
+		buddy->bits[i] = kmalloc_array(s, sizeof(long), GFP_KERNEL);
+		if (!buddy->bits[i])
+			goto err_out_free;
+
+		bitmap_zero(buddy->bits[i], 1 << (buddy->max_order - i));
+	}
+
+	set_bit(0, buddy->bits[buddy->max_order]);
+	buddy->num_free[buddy->max_order] = 1;
+
+	return 0;
+
+err_out_free:
+	for (i = 0; i <= buddy->max_order; ++i)
+		kfree(buddy->bits[i]);
+
+err_out:
+	kfree(buddy->bits);
+	kfree(buddy->num_free);
+	return -ENOMEM;
+}
+
+static void hns_roce_buddy_cleanup(struct hns_roce_buddy *buddy)
+{
+	int i;
+
+	for (i = 0; i <= buddy->max_order; ++i)
+		kfree(buddy->bits[i]);
+
+	kfree(buddy->bits);
+	kfree(buddy->num_free);
+}
+
+static int hns_roce_alloc_mtt_range(struct hns_roce_dev *hr_dev, int order,
+				    unsigned long *seg)
+{
+	struct hns_roce_mr_table *mr_table = &hr_dev->mr_table;
+	int ret = 0;
+
+	ret = hns_roce_buddy_alloc(&mr_table->mtt_buddy, order, seg);
+	if (ret == -1)
+		return -1;
+
+	if (hns_roce_table_get_range(hr_dev, &mr_table->mtt_table, *seg,
+				     *seg + (1 << order) - 1)) {
+		hns_roce_buddy_free(&mr_table->mtt_buddy, *seg, order);
+		return -1;
+	}
+
+	return 0;
+}
+
+int hns_roce_mtt_init(struct hns_roce_dev *hr_dev, int npages, int page_shift,
+		      struct hns_roce_mtt *mtt)
+{
+	int ret = 0;
+	int i;
+
+	/* Page num is zero, correspond to DMA memory register */
+	if (!npages) {
+		mtt->order = -1;
+		mtt->page_shift = HNS_ROCE_HEM_PAGE_SHIFT;
+		return 0;
+	}
+
+	/* Note: if page_shift is zero, FAST memory regsiter */
+	mtt->page_shift = page_shift;
+
+	/* Compute MTT entry necessary */
+	for (mtt->order = 0, i = HNS_ROCE_MTT_ENTRY_PER_SEG; i < npages;
+	     i <<= 1)
+		++mtt->order;
+
+	/* Allocate MTT entry */
+	ret = hns_roce_alloc_mtt_range(hr_dev, mtt->order, &mtt->first_seg);
+	if (ret == -1)
+		return -ENOMEM;
+
+	return 0;
+}
+
+void hns_roce_mtt_cleanup(struct hns_roce_dev *hr_dev, struct hns_roce_mtt *mtt)
+{
+	struct hns_roce_mr_table *mr_table = &hr_dev->mr_table;
+
+	if (mtt->order < 0)
+		return;
+
+	hns_roce_buddy_free(&mr_table->mtt_buddy, mtt->first_seg, mtt->order);
+	hns_roce_table_put_range(hr_dev, &mr_table->mtt_table, mtt->first_seg,
+				 mtt->first_seg + (1 << mtt->order) - 1);
+}
+
+static int hns_roce_mr_alloc(struct hns_roce_dev *hr_dev, u32 pd, u64 iova,
+			     u64 size, u32 access, int npages,
+			     struct hns_roce_mr *mr)
+{
+	unsigned long index = 0;
+	int ret = 0;
+	struct device *dev = &hr_dev->pdev->dev;
+
+	/* Allocate a key for mr from mr_table */
+	ret = hns_roce_bitmap_alloc(&hr_dev->mr_table.mtpt_bitmap, &index);
+	if (ret == -1)
+		return -ENOMEM;
+
+	mr->iova = iova;			/* MR va starting addr */
+	mr->size = size;			/* MR addr range */
+	mr->pd = pd;				/* MR num */
+	mr->access = access;			/* MR access permit */
+	mr->enabled = 0;			/* MR active status */
+	mr->key = hw_index_to_key(index);	/* MR key */
+
+	if (size == ~0ull) {
+		mr->type = MR_TYPE_DMA;
+		mr->pbl_buf = NULL;
+		mr->pbl_dma_addr = 0;
+	} else {
+		mr->type = MR_TYPE_MR;
+		mr->pbl_buf = dma_alloc_coherent(dev, npages * 8,
+						 &(mr->pbl_dma_addr),
+						 GFP_KERNEL);
+		if (!mr->pbl_buf)
+			return -ENOMEM;
+	}
+
+	return 0;
+}
+
+static void hns_roce_mr_free(struct hns_roce_dev *hr_dev,
+			     struct hns_roce_mr *mr)
+{
+	struct device *dev = &hr_dev->pdev->dev;
+	int npages = 0;
+	int ret;
+
+	if (mr->enabled) {
+		ret = hns_roce_hw2sw_mpt(hr_dev, NULL, key_to_hw_index(mr->key)
+					 & (hr_dev->caps.num_mtpts - 1));
+		if (ret)
+			dev_warn(dev, "HW2SW_MPT failed (%d)\n", ret);
+	}
+
+	if (mr->size != ~0ULL) {
+		npages = ib_umem_page_count(mr->umem);
+		dma_free_coherent(dev, (unsigned int)(npages * 8), mr->pbl_buf,
+				  mr->pbl_dma_addr);
+	}
+
+	hns_roce_bitmap_free(&hr_dev->mr_table.mtpt_bitmap,
+			     key_to_hw_index(mr->key));
+}
+
+static int hns_roce_mr_enable(struct hns_roce_dev *hr_dev,
+			      struct hns_roce_mr *mr)
+{
+	int ret;
+	unsigned long mtpt_idx = key_to_hw_index(mr->key);
+	struct device *dev = &hr_dev->pdev->dev;
+	struct hns_roce_cmd_mailbox *mailbox;
+	struct hns_roce_mr_table *mr_table = &hr_dev->mr_table;
+
+	/* Prepare HEM entry memory */
+	ret = hns_roce_table_get(hr_dev, &mr_table->mtpt_table, mtpt_idx);
+	if (ret)
+		return ret;
+
+	/* Allocate mailbox memory */
+	mailbox = hns_roce_alloc_cmd_mailbox(hr_dev);
+	if (IS_ERR(mailbox)) {
+		ret = PTR_ERR(mailbox);
+		goto err_table;
+	}
+
+	ret = hr_dev->hw->write_mtpt(mailbox->buf, mr, mtpt_idx);
+	if (ret) {
+		dev_err(dev, "Write mtpt fail!\n");
+		goto err_page;
+	}
+
+	ret = hns_roce_sw2hw_mpt(hr_dev, mailbox,
+				 mtpt_idx & (hr_dev->caps.num_mtpts - 1));
+	if (ret) {
+		dev_err(dev, "SW2HW_MPT failed (%d)\n", ret);
+		goto err_page;
+	}
+
+	mr->enabled = 1;
+	hns_roce_free_cmd_mailbox(hr_dev, mailbox);
+
+	return 0;
+
+err_page:
+	hns_roce_free_cmd_mailbox(hr_dev, mailbox);
+
+err_table:
+	hns_roce_table_put(hr_dev, &mr_table->mtpt_table, mtpt_idx);
+	return ret;
+}
+
+static int hns_roce_write_mtt_chunk(struct hns_roce_dev *hr_dev,
+				    struct hns_roce_mtt *mtt, u32 start_index,
+				    u32 npages, u64 *page_list)
+{
+	u32 i = 0;
+	__le64 *mtts = NULL;
+	dma_addr_t dma_handle;
+	u32 s = start_index * sizeof(u64);
+
+	/* All MTTs must fit in the same page */
+	if (start_index / (PAGE_SIZE / sizeof(u64)) !=
+		(start_index + npages - 1) / (PAGE_SIZE / sizeof(u64)))
+		return -EINVAL;
+
+	if (start_index & (HNS_ROCE_MTT_ENTRY_PER_SEG - 1))
+		return -EINVAL;
+
+	mtts = hns_roce_table_find(&hr_dev->mr_table.mtt_table,
+				mtt->first_seg + s / hr_dev->caps.mtt_entry_sz,
+				&dma_handle);
+	if (!mtts)
+		return -ENOMEM;
+
+	/* Save page addr, low 12 bits : 0 */
+	for (i = 0; i < npages; ++i)
+		mtts[i] = (cpu_to_le64(page_list[i])) >> PAGE_ADDR_SHIFT;
+
+	return 0;
+}
+
+static int hns_roce_write_mtt(struct hns_roce_dev *hr_dev,
+			      struct hns_roce_mtt *mtt, u32 start_index,
+			      u32 npages, u64 *page_list)
+{
+	int chunk;
+	int ret;
+
+	if (mtt->order < 0)
+		return -EINVAL;
+
+	while (npages > 0) {
+		chunk = min_t(int, PAGE_SIZE / sizeof(u64), npages);
+
+		ret = hns_roce_write_mtt_chunk(hr_dev, mtt, start_index, chunk,
+					       page_list);
+		if (ret)
+			return ret;
+
+		npages -= chunk;
+		start_index += chunk;
+		page_list += chunk;
+	}
+
+	return 0;
+}
+
+int hns_roce_buf_write_mtt(struct hns_roce_dev *hr_dev,
+			   struct hns_roce_mtt *mtt, struct hns_roce_buf *buf)
+{
+	u32 i = 0;
+	int ret = 0;
+	u64 *page_list = NULL;
+
+	page_list = kmalloc_array(buf->npages, sizeof(*page_list), GFP_KERNEL);
+	if (!page_list)
+		return -ENOMEM;
+
+	for (i = 0; i < buf->npages; ++i) {
+		if (buf->nbufs == 1)
+			page_list[i] = buf->direct.map + (i << buf->page_shift);
+		else
+			page_list[i] = buf->page_list[i].map;
+
+	}
+	ret = hns_roce_write_mtt(hr_dev, mtt, 0, buf->npages, page_list);
+
+	kfree(page_list);
+
+	return ret;
+}
+
+int hns_roce_init_mr_table(struct hns_roce_dev *hr_dev)
+{
+	struct hns_roce_mr_table *mr_table = &hr_dev->mr_table;
+	int ret = 0;
+
+	ret = hns_roce_bitmap_init(&mr_table->mtpt_bitmap,
+				   hr_dev->caps.num_mtpts,
+				   hr_dev->caps.num_mtpts - 1,
+				   hr_dev->caps.reserved_mrws, 0);
+	if (ret)
+		return ret;
+
+	ret = hns_roce_buddy_init(&mr_table->mtt_buddy,
+				  ilog2(hr_dev->caps.num_mtt_segs));
+	if (ret)
+		goto err_buddy;
+
+	return 0;
+
+err_buddy:
+	hns_roce_bitmap_cleanup(&mr_table->mtpt_bitmap);
+	return ret;
+}
+
+void hns_roce_cleanup_mr_table(struct hns_roce_dev *hr_dev)
+{
+	struct hns_roce_mr_table *mr_table = &hr_dev->mr_table;
+
+	hns_roce_buddy_cleanup(&mr_table->mtt_buddy);
+	hns_roce_bitmap_cleanup(&mr_table->mtpt_bitmap);
+}
+
+struct ib_mr *hns_roce_get_dma_mr(struct ib_pd *pd, int acc)
+{
+	int ret = 0;
+	struct hns_roce_mr *mr = NULL;
+
+	mr = kmalloc(sizeof(*mr), GFP_KERNEL);
+	if (mr == NULL)
+		return  ERR_PTR(-ENOMEM);
+
+	/* Allocate memory region key */
+	ret = hns_roce_mr_alloc(to_hr_dev(pd->device), to_hr_pd(pd)->pdn, 0,
+				~0ULL, acc, 0, mr);
+	if (ret)
+		goto err_free;
+
+	ret = hns_roce_mr_enable(to_hr_dev(pd->device), mr);
+	if (ret)
+		goto err_mr;
+
+	mr->ibmr.rkey = mr->ibmr.lkey = mr->key;
+	mr->umem = NULL;
+
+	return &mr->ibmr;
+
+err_mr:
+	hns_roce_mr_free(to_hr_dev(pd->device), mr);
+
+err_free:
+	kfree(mr);
+	return ERR_PTR(ret);
+}
+
+int hns_roce_ib_umem_write_mtt(struct hns_roce_dev *hr_dev,
+			       struct hns_roce_mtt *mtt, struct ib_umem *umem)
+{
+	struct scatterlist *sg;
+	int i, k, entry;
+	int ret = 0;
+	u64 *pages;
+	u32 n;
+	int len;
+
+	pages = (u64 *) __get_free_page(GFP_KERNEL);
+	if (!pages)
+		return -ENOMEM;
+
+	i = n = 0;
+
+	for_each_sg(umem->sg_head.sgl, sg, umem->nmap, entry) {
+		len = sg_dma_len(sg) >> mtt->page_shift;
+		for (k = 0; k < len; ++k) {
+			pages[i++] = sg_dma_address(sg) + umem->page_size * k;
+			if (i == PAGE_SIZE / sizeof(u64)) {
+				ret = hns_roce_write_mtt(hr_dev, mtt, n, i,
+							 pages);
+				if (ret)
+					goto out;
+				n += i;
+				i = 0;
+			}
+		}
+	}
+
+	if (i)
+		ret = hns_roce_write_mtt(hr_dev, mtt, n, i, pages);
+
+out:
+	free_page((unsigned long) pages);
+	return ret;
+}
+
+static int hns_roce_ib_umem_write_mr(struct hns_roce_mr *mr,
+				     struct ib_umem *umem)
+{
+	int i = 0;
+	int entry;
+	struct scatterlist *sg;
+
+	for_each_sg(umem->sg_head.sgl, sg, umem->nmap, entry) {
+		mr->pbl_buf[i] = ((u64)sg_dma_address(sg)) >> 12;
+		i++;
+	}
+
+	/* Memory barrier */
+	mb();
+
+	return 0;
+}
+
+struct ib_mr *hns_roce_reg_user_mr(struct ib_pd *pd, u64 start, u64 length,
+				   u64 virt_addr, int access_flags,
+				   struct ib_udata *udata)
+{
+	struct hns_roce_dev *hr_dev = to_hr_dev(pd->device);
+	struct device *dev = &hr_dev->pdev->dev;
+	struct hns_roce_mr *mr = NULL;
+	int ret = 0;
+	int n = 0;
+
+	mr = kmalloc(sizeof(*mr), GFP_KERNEL);
+	if (!mr)
+		return ERR_PTR(-ENOMEM);
+
+	mr->umem = ib_umem_get(pd->uobject->context, start, length,
+			       access_flags, 0);
+	if (IS_ERR(mr->umem)) {
+		ret = PTR_ERR(mr->umem);
+		goto err_free;
+	}
+
+	n = ib_umem_page_count(mr->umem);
+	if (mr->umem->page_size != HNS_ROCE_HEM_PAGE_SIZE) {
+		dev_err(dev, "Just support 4K page size but is 0x%x now!\n",
+			mr->umem->page_size);
+		ret = -EINVAL;
+		goto err_umem;
+	}
+
+	if (n > HNS_ROCE_MAX_MTPT_PBL_NUM) {
+		dev_err(dev, " MR len %lld err. MR is limited to 4G at most!\n",
+			length);
+		ret = -EINVAL;
+		goto err_umem;
+	}
+
+	ret = hns_roce_mr_alloc(hr_dev, to_hr_pd(pd)->pdn, virt_addr, length,
+				access_flags, n, mr);
+	if (ret)
+		goto err_umem;
+
+	ret = hns_roce_ib_umem_write_mr(mr, mr->umem);
+	if (ret)
+		goto err_mr;
+
+	ret = hns_roce_mr_enable(hr_dev, mr);
+	if (ret)
+		goto err_mr;
+
+	mr->ibmr.rkey = mr->ibmr.lkey = mr->key;
+
+	return &mr->ibmr;
+
+err_mr:
+	hns_roce_mr_free(hr_dev, mr);
+
+err_umem:
+	ib_umem_release(mr->umem);
+
+err_free:
+	kfree(mr);
+	return ERR_PTR(ret);
+}
+
+int hns_roce_dereg_mr(struct ib_mr *ibmr)
+{
+	struct hns_roce_mr *mr = to_hr_mr(ibmr);
+
+	hns_roce_mr_free(to_hr_dev(ibmr->device), mr);
+	if (mr->umem)
+		ib_umem_release(mr->umem);
+
+	kfree(mr);
+
+	return 0;
+}
diff --git a/drivers/infiniband/hw/hns/hns_roce_pd.c b/drivers/infiniband/hw/hns/hns_roce_pd.c
new file mode 100644
index 000000000000..05db7d59812a
--- /dev/null
+++ b/drivers/infiniband/hw/hns/hns_roce_pd.c
@@ -0,0 +1,138 @@
+/*
+ * Copyright (c) 2016 Hisilicon Limited.
+ *
+ * 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/platform_device.h>
+#include "hns_roce_device.h"
+
+static int hns_roce_pd_alloc(struct hns_roce_dev *hr_dev, unsigned long *pdn)
+{
+	return hns_roce_bitmap_alloc(&hr_dev->pd_bitmap, pdn);
+}
+
+static void hns_roce_pd_free(struct hns_roce_dev *hr_dev, unsigned long pdn)
+{
+	hns_roce_bitmap_free(&hr_dev->pd_bitmap, pdn);
+}
+
+int hns_roce_init_pd_table(struct hns_roce_dev *hr_dev)
+{
+	return hns_roce_bitmap_init(&hr_dev->pd_bitmap, hr_dev->caps.num_pds,
+				    hr_dev->caps.num_pds - 1,
+				    hr_dev->caps.reserved_pds, 0);
+}
+
+void hns_roce_cleanup_pd_table(struct hns_roce_dev *hr_dev)
+{
+	hns_roce_bitmap_cleanup(&hr_dev->pd_bitmap);
+}
+
+struct ib_pd *hns_roce_alloc_pd(struct ib_device *ib_dev,
+				struct ib_ucontext *context,
+				struct ib_udata *udata)
+{
+	struct hns_roce_dev *hr_dev = to_hr_dev(ib_dev);
+	struct device *dev = &hr_dev->pdev->dev;
+	struct hns_roce_pd *pd;
+	int ret;
+
+	pd = kmalloc(sizeof(*pd), GFP_KERNEL);
+	if (!pd)
+		return ERR_PTR(-ENOMEM);
+
+	ret = hns_roce_pd_alloc(to_hr_dev(ib_dev), &pd->pdn);
+	if (ret) {
+		kfree(pd);
+		dev_err(dev, "[alloc_pd]hns_roce_pd_alloc failed!\n");
+		return ERR_PTR(ret);
+	}
+
+	if (context) {
+		if (ib_copy_to_udata(udata, &pd->pdn, sizeof(u64))) {
+			hns_roce_pd_free(to_hr_dev(ib_dev), pd->pdn);
+			dev_err(dev, "[alloc_pd]ib_copy_to_udata failed!\n");
+			kfree(pd);
+			return ERR_PTR(-EFAULT);
+		}
+	}
+
+	return &pd->ibpd;
+}
+
+int hns_roce_dealloc_pd(struct ib_pd *pd)
+{
+	hns_roce_pd_free(to_hr_dev(pd->device), to_hr_pd(pd)->pdn);
+	kfree(to_hr_pd(pd));
+
+	return 0;
+}
+
+int hns_roce_uar_alloc(struct hns_roce_dev *hr_dev, struct hns_roce_uar *uar)
+{
+	struct resource *res;
+	int ret = 0;
+
+	/* Using bitmap to manager UAR index */
+	ret = hns_roce_bitmap_alloc(&hr_dev->uar_table.bitmap, &uar->index);
+	if (ret == -1)
+		return -ENOMEM;
+
+	if (uar->index > 0)
+		uar->index = (uar->index - 1) %
+			     (hr_dev->caps.phy_num_uars - 1) + 1;
+
+	res = platform_get_resource(hr_dev->pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(&hr_dev->pdev->dev, "memory resource not found!\n");
+		return -EINVAL;
+	}
+	uar->pfn = ((res->start) >> PAGE_SHIFT) + uar->index;
+
+	return 0;
+}
+
+void hns_roce_uar_free(struct hns_roce_dev *hr_dev, struct hns_roce_uar *uar)
+{
+	hns_roce_bitmap_free(&hr_dev->uar_table.bitmap, uar->index);
+}
+
+int hns_roce_init_uar_table(struct hns_roce_dev *hr_dev)
+{
+	return hns_roce_bitmap_init(&hr_dev->uar_table.bitmap,
+				    hr_dev->caps.num_uars,
+				    hr_dev->caps.num_uars - 1,
+				    hr_dev->caps.reserved_uars, 0);
+}
+
+void hns_roce_cleanup_uar_table(struct hns_roce_dev *hr_dev)
+{
+	hns_roce_bitmap_cleanup(&hr_dev->uar_table.bitmap);
+}
diff --git a/drivers/infiniband/hw/hns/hns_roce_qp.c b/drivers/infiniband/hw/hns/hns_roce_qp.c
new file mode 100644
index 000000000000..e86dd8d06777
--- /dev/null
+++ b/drivers/infiniband/hw/hns/hns_roce_qp.c
@@ -0,0 +1,838 @@
+/*
+ * Copyright (c) 2016 Hisilicon Limited.
+ * Copyright (c) 2007, 2008 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/platform_device.h>
+#include <rdma/ib_addr.h>
+#include <rdma/ib_umem.h>
+#include "hns_roce_common.h"
+#include "hns_roce_device.h"
+#include "hns_roce_hem.h"
+#include "hns_roce_user.h"
+
+#define SQP_NUM				(2 * HNS_ROCE_MAX_PORTS)
+
+void hns_roce_qp_event(struct hns_roce_dev *hr_dev, u32 qpn, int event_type)
+{
+	struct hns_roce_qp_table *qp_table = &hr_dev->qp_table;
+	struct device *dev = &hr_dev->pdev->dev;
+	struct hns_roce_qp *qp;
+
+	spin_lock(&qp_table->lock);
+
+	qp = __hns_roce_qp_lookup(hr_dev, qpn);
+	if (qp)
+		atomic_inc(&qp->refcount);
+
+	spin_unlock(&qp_table->lock);
+
+	if (!qp) {
+		dev_warn(dev, "Async event for bogus QP %08x\n", qpn);
+		return;
+	}
+
+	qp->event(qp, (enum hns_roce_event)event_type);
+
+	if (atomic_dec_and_test(&qp->refcount))
+		complete(&qp->free);
+}
+
+static void hns_roce_ib_qp_event(struct hns_roce_qp *hr_qp,
+				 enum hns_roce_event type)
+{
+	struct ib_event event;
+	struct ib_qp *ibqp = &hr_qp->ibqp;
+
+	if (ibqp->event_handler) {
+		event.device = ibqp->device;
+		event.element.qp = ibqp;
+		switch (type) {
+		case HNS_ROCE_EVENT_TYPE_PATH_MIG:
+			event.event = IB_EVENT_PATH_MIG;
+			break;
+		case HNS_ROCE_EVENT_TYPE_COMM_EST:
+			event.event = IB_EVENT_COMM_EST;
+			break;
+		case HNS_ROCE_EVENT_TYPE_SQ_DRAINED:
+			event.event = IB_EVENT_SQ_DRAINED;
+			break;
+		case HNS_ROCE_EVENT_TYPE_SRQ_LAST_WQE_REACH:
+			event.event = IB_EVENT_QP_LAST_WQE_REACHED;
+			break;
+		case HNS_ROCE_EVENT_TYPE_WQ_CATAS_ERROR:
+			event.event = IB_EVENT_QP_FATAL;
+			break;
+		case HNS_ROCE_EVENT_TYPE_PATH_MIG_FAILED:
+			event.event = IB_EVENT_PATH_MIG_ERR;
+			break;
+		case HNS_ROCE_EVENT_TYPE_INV_REQ_LOCAL_WQ_ERROR:
+			event.event = IB_EVENT_QP_REQ_ERR;
+			break;
+		case HNS_ROCE_EVENT_TYPE_LOCAL_WQ_ACCESS_ERROR:
+			event.event = IB_EVENT_QP_ACCESS_ERR;
+			break;
+		default:
+			dev_dbg(ibqp->device->dma_device, "roce_ib: Unexpected event type %d on QP %06lx\n",
+				type, hr_qp->qpn);
+			return;
+		}
+		ibqp->event_handler(&event, ibqp->qp_context);
+	}
+}
+
+static int hns_roce_reserve_range_qp(struct hns_roce_dev *hr_dev, int cnt,
+				     int align, unsigned long *base)
+{
+	struct hns_roce_qp_table *qp_table = &hr_dev->qp_table;
+
+	return hns_roce_bitmap_alloc_range(&qp_table->bitmap, cnt, align, base);
+}
+
+enum hns_roce_qp_state to_hns_roce_state(enum ib_qp_state state)
+{
+	switch (state) {
+	case IB_QPS_RESET:
+		return HNS_ROCE_QP_STATE_RST;
+	case IB_QPS_INIT:
+		return HNS_ROCE_QP_STATE_INIT;
+	case IB_QPS_RTR:
+		return HNS_ROCE_QP_STATE_RTR;
+	case IB_QPS_RTS:
+		return HNS_ROCE_QP_STATE_RTS;
+	case IB_QPS_SQD:
+		return HNS_ROCE_QP_STATE_SQD;
+	case IB_QPS_ERR:
+		return HNS_ROCE_QP_STATE_ERR;
+	default:
+		return HNS_ROCE_QP_NUM_STATE;
+	}
+}
+
+static int hns_roce_gsi_qp_alloc(struct hns_roce_dev *hr_dev, unsigned long qpn,
+				 struct hns_roce_qp *hr_qp)
+{
+	struct hns_roce_qp_table *qp_table = &hr_dev->qp_table;
+	int ret;
+
+	if (!qpn)
+		return -EINVAL;
+
+	hr_qp->qpn = qpn;
+
+	spin_lock_irq(&qp_table->lock);
+	ret = radix_tree_insert(&hr_dev->qp_table_tree,
+				hr_qp->qpn & (hr_dev->caps.num_qps - 1), hr_qp);
+	spin_unlock_irq(&qp_table->lock);
+	if (ret) {
+		dev_err(&hr_dev->pdev->dev, "QPC radix_tree_insert failed\n");
+		goto err_put_irrl;
+	}
+
+	atomic_set(&hr_qp->refcount, 1);
+	init_completion(&hr_qp->free);
+
+	return 0;
+
+err_put_irrl:
+
+	return ret;
+}
+
+static int hns_roce_qp_alloc(struct hns_roce_dev *hr_dev, unsigned long qpn,
+			     struct hns_roce_qp *hr_qp)
+{
+	struct hns_roce_qp_table *qp_table = &hr_dev->qp_table;
+	struct device *dev = &hr_dev->pdev->dev;
+	int ret;
+
+	if (!qpn)
+		return -EINVAL;
+
+	hr_qp->qpn = qpn;
+
+	/* Alloc memory for QPC */
+	ret = hns_roce_table_get(hr_dev, &qp_table->qp_table, hr_qp->qpn);
+	if (ret) {
+		dev_err(dev, "QPC table get failed\n");
+		goto err_out;
+	}
+
+	/* Alloc memory for IRRL */
+	ret = hns_roce_table_get(hr_dev, &qp_table->irrl_table, hr_qp->qpn);
+	if (ret) {
+		dev_err(dev, "IRRL table get failed\n");
+		goto err_put_qp;
+	}
+
+	spin_lock_irq(&qp_table->lock);
+	ret = radix_tree_insert(&hr_dev->qp_table_tree,
+				hr_qp->qpn & (hr_dev->caps.num_qps - 1), hr_qp);
+	spin_unlock_irq(&qp_table->lock);
+	if (ret) {
+		dev_err(dev, "QPC radix_tree_insert failed\n");
+		goto err_put_irrl;
+	}
+
+	atomic_set(&hr_qp->refcount, 1);
+	init_completion(&hr_qp->free);
+
+	return 0;
+
+err_put_irrl:
+	hns_roce_table_put(hr_dev, &qp_table->irrl_table, hr_qp->qpn);
+
+err_put_qp:
+	hns_roce_table_put(hr_dev, &qp_table->qp_table, hr_qp->qpn);
+
+err_out:
+	return ret;
+}
+
+void hns_roce_qp_remove(struct hns_roce_dev *hr_dev, struct hns_roce_qp *hr_qp)
+{
+	struct hns_roce_qp_table *qp_table = &hr_dev->qp_table;
+	unsigned long flags;
+
+	spin_lock_irqsave(&qp_table->lock, flags);
+	radix_tree_delete(&hr_dev->qp_table_tree,
+			  hr_qp->qpn & (hr_dev->caps.num_qps - 1));
+	spin_unlock_irqrestore(&qp_table->lock, flags);
+}
+
+void hns_roce_qp_free(struct hns_roce_dev *hr_dev, struct hns_roce_qp *hr_qp)
+{
+	struct hns_roce_qp_table *qp_table = &hr_dev->qp_table;
+
+	if (atomic_dec_and_test(&hr_qp->refcount))
+		complete(&hr_qp->free);
+	wait_for_completion(&hr_qp->free);
+
+	if ((hr_qp->ibqp.qp_type) != IB_QPT_GSI) {
+		hns_roce_table_put(hr_dev, &qp_table->irrl_table, hr_qp->qpn);
+		hns_roce_table_put(hr_dev, &qp_table->qp_table, hr_qp->qpn);
+	}
+}
+
+void hns_roce_release_range_qp(struct hns_roce_dev *hr_dev, int base_qpn,
+			       int cnt)
+{
+	struct hns_roce_qp_table *qp_table = &hr_dev->qp_table;
+
+	if (base_qpn < SQP_NUM)
+		return;
+
+	hns_roce_bitmap_free_range(&qp_table->bitmap, base_qpn, cnt);
+}
+
+static int hns_roce_set_rq_size(struct hns_roce_dev *hr_dev,
+				struct ib_qp_cap *cap, int is_user, int has_srq,
+				struct hns_roce_qp *hr_qp)
+{
+	u32 max_cnt;
+	struct device *dev = &hr_dev->pdev->dev;
+
+	/* Check the validity of QP support capacity */
+	if (cap->max_recv_wr > hr_dev->caps.max_wqes ||
+	    cap->max_recv_sge > hr_dev->caps.max_rq_sg) {
+		dev_err(dev, "RQ WR or sge error!max_recv_wr=%d max_recv_sge=%d\n",
+			cap->max_recv_wr, cap->max_recv_sge);
+		return -EINVAL;
+	}
+
+	/* If srq exit, set zero for relative number of rq */
+	if (has_srq) {
+		if (cap->max_recv_wr) {
+			dev_dbg(dev, "srq no need config max_recv_wr\n");
+			return -EINVAL;
+		}
+
+		hr_qp->rq.wqe_cnt = hr_qp->rq.max_gs = 0;
+	} else {
+		if (is_user && (!cap->max_recv_wr || !cap->max_recv_sge)) {
+			dev_err(dev, "user space no need config max_recv_wr max_recv_sge\n");
+			return -EINVAL;
+		}
+
+		/* In v1 engine, parameter verification procession */
+		max_cnt = cap->max_recv_wr > HNS_ROCE_MIN_WQE_NUM ?
+			  cap->max_recv_wr : HNS_ROCE_MIN_WQE_NUM;
+		hr_qp->rq.wqe_cnt = roundup_pow_of_two(max_cnt);
+
+		if ((u32)hr_qp->rq.wqe_cnt > hr_dev->caps.max_wqes) {
+			dev_err(dev, "hns_roce_set_rq_size rq.wqe_cnt too large\n");
+			return -EINVAL;
+		}
+
+		max_cnt = max(1U, cap->max_recv_sge);
+		hr_qp->rq.max_gs = roundup_pow_of_two(max_cnt);
+		/* WQE is fixed for 64B */
+		hr_qp->rq.wqe_shift = ilog2(hr_dev->caps.max_rq_desc_sz);
+	}
+
+	cap->max_recv_wr = hr_qp->rq.max_post = hr_qp->rq.wqe_cnt;
+	cap->max_recv_sge = hr_qp->rq.max_gs;
+
+	return 0;
+}
+
+static int hns_roce_set_user_sq_size(struct hns_roce_dev *hr_dev,
+				     struct hns_roce_qp *hr_qp,
+				     struct hns_roce_ib_create_qp *ucmd)
+{
+	u32 roundup_sq_stride = roundup_pow_of_two(hr_dev->caps.max_sq_desc_sz);
+	u8 max_sq_stride = ilog2(roundup_sq_stride);
+
+	/* Sanity check SQ size before proceeding */
+	if ((u32)(1 << ucmd->log_sq_bb_count) > hr_dev->caps.max_wqes ||
+	     ucmd->log_sq_stride > max_sq_stride ||
+	     ucmd->log_sq_stride < HNS_ROCE_IB_MIN_SQ_STRIDE) {
+		dev_err(&hr_dev->pdev->dev, "check SQ size error!\n");
+		return -EINVAL;
+	}
+
+	hr_qp->sq.wqe_cnt = 1 << ucmd->log_sq_bb_count;
+	hr_qp->sq.wqe_shift = ucmd->log_sq_stride;
+
+	/* Get buf size, SQ and RQ  are aligned to page_szie */
+	hr_qp->buff_size = HNS_ROCE_ALOGN_UP((hr_qp->rq.wqe_cnt <<
+					     hr_qp->rq.wqe_shift), PAGE_SIZE) +
+			   HNS_ROCE_ALOGN_UP((hr_qp->sq.wqe_cnt <<
+					     hr_qp->sq.wqe_shift), PAGE_SIZE);
+
+	hr_qp->sq.offset = 0;
+	hr_qp->rq.offset = HNS_ROCE_ALOGN_UP((hr_qp->sq.wqe_cnt <<
+					     hr_qp->sq.wqe_shift), PAGE_SIZE);
+
+	return 0;
+}
+
+static int hns_roce_set_kernel_sq_size(struct hns_roce_dev *hr_dev,
+				       struct ib_qp_cap *cap,
+				       struct hns_roce_qp *hr_qp)
+{
+	struct device *dev = &hr_dev->pdev->dev;
+	u32 max_cnt;
+
+	if (cap->max_send_wr  > hr_dev->caps.max_wqes  ||
+	    cap->max_send_sge > hr_dev->caps.max_sq_sg ||
+	    cap->max_inline_data > hr_dev->caps.max_sq_inline) {
+		dev_err(dev, "hns_roce_set_kernel_sq_size error1\n");
+		return -EINVAL;
+	}
+
+	hr_qp->sq.wqe_shift = ilog2(hr_dev->caps.max_sq_desc_sz);
+	hr_qp->sq_max_wqes_per_wr = 1;
+	hr_qp->sq_spare_wqes = 0;
+
+	/* In v1 engine, parameter verification procession */
+	max_cnt = cap->max_send_wr > HNS_ROCE_MIN_WQE_NUM ?
+		  cap->max_send_wr : HNS_ROCE_MIN_WQE_NUM;
+	hr_qp->sq.wqe_cnt = roundup_pow_of_two(max_cnt);
+	if ((u32)hr_qp->sq.wqe_cnt > hr_dev->caps.max_wqes) {
+		dev_err(dev, "hns_roce_set_kernel_sq_size sq.wqe_cnt too large\n");
+		return -EINVAL;
+	}
+
+	/* Get data_seg numbers */
+	max_cnt = max(1U, cap->max_send_sge);
+	hr_qp->sq.max_gs = roundup_pow_of_two(max_cnt);
+
+	/* Get buf size, SQ and RQ  are aligned to page_szie */
+	hr_qp->buff_size = HNS_ROCE_ALOGN_UP((hr_qp->rq.wqe_cnt <<
+					     hr_qp->rq.wqe_shift), PAGE_SIZE) +
+			   HNS_ROCE_ALOGN_UP((hr_qp->sq.wqe_cnt <<
+					     hr_qp->sq.wqe_shift), PAGE_SIZE);
+	hr_qp->sq.offset = 0;
+	hr_qp->rq.offset = HNS_ROCE_ALOGN_UP((hr_qp->sq.wqe_cnt <<
+					      hr_qp->sq.wqe_shift), PAGE_SIZE);
+
+	/* Get wr and sge number which send */
+	cap->max_send_wr = hr_qp->sq.max_post = hr_qp->sq.wqe_cnt;
+	cap->max_send_sge = hr_qp->sq.max_gs;
+
+	/* We don't support inline sends for kernel QPs (yet) */
+	cap->max_inline_data = 0;
+
+	return 0;
+}
+
+static int hns_roce_create_qp_common(struct hns_roce_dev *hr_dev,
+				     struct ib_pd *ib_pd,
+				     struct ib_qp_init_attr *init_attr,
+				     struct ib_udata *udata, unsigned long sqpn,
+				     struct hns_roce_qp *hr_qp)
+{
+	struct device *dev = &hr_dev->pdev->dev;
+	struct hns_roce_ib_create_qp ucmd;
+	unsigned long qpn = 0;
+	int ret = 0;
+
+	mutex_init(&hr_qp->mutex);
+	spin_lock_init(&hr_qp->sq.lock);
+	spin_lock_init(&hr_qp->rq.lock);
+
+	hr_qp->state = IB_QPS_RESET;
+
+	if (init_attr->sq_sig_type == IB_SIGNAL_ALL_WR)
+		hr_qp->sq_signal_bits = IB_SIGNAL_ALL_WR;
+	else
+		hr_qp->sq_signal_bits = IB_SIGNAL_REQ_WR;
+
+	ret = hns_roce_set_rq_size(hr_dev, &init_attr->cap, !!ib_pd->uobject,
+				   !!init_attr->srq, hr_qp);
+	if (ret) {
+		dev_err(dev, "hns_roce_set_rq_size failed\n");
+		goto err_out;
+	}
+
+	if (ib_pd->uobject) {
+		if (ib_copy_from_udata(&ucmd, udata, sizeof(ucmd))) {
+			dev_err(dev, "ib_copy_from_udata error for create qp\n");
+			ret = -EFAULT;
+			goto err_out;
+		}
+
+		ret = hns_roce_set_user_sq_size(hr_dev, hr_qp, &ucmd);
+		if (ret) {
+			dev_err(dev, "hns_roce_set_user_sq_size error for create qp\n");
+			goto err_out;
+		}
+
+		hr_qp->umem = ib_umem_get(ib_pd->uobject->context,
+					  ucmd.buf_addr, hr_qp->buff_size, 0,
+					  0);
+		if (IS_ERR(hr_qp->umem)) {
+			dev_err(dev, "ib_umem_get error for create qp\n");
+			ret = PTR_ERR(hr_qp->umem);
+			goto err_out;
+		}
+
+		ret = hns_roce_mtt_init(hr_dev, ib_umem_page_count(hr_qp->umem),
+				    ilog2((unsigned int)hr_qp->umem->page_size),
+				    &hr_qp->mtt);
+		if (ret) {
+			dev_err(dev, "hns_roce_mtt_init error for create qp\n");
+			goto err_buf;
+		}
+
+		ret = hns_roce_ib_umem_write_mtt(hr_dev, &hr_qp->mtt,
+						 hr_qp->umem);
+		if (ret) {
+			dev_err(dev, "hns_roce_ib_umem_write_mtt error for create qp\n");
+			goto err_mtt;
+		}
+	} else {
+		if (init_attr->create_flags &
+		    IB_QP_CREATE_BLOCK_MULTICAST_LOOPBACK) {
+			dev_err(dev, "init_attr->create_flags error!\n");
+			ret = -EINVAL;
+			goto err_out;
+		}
+
+		if (init_attr->create_flags & IB_QP_CREATE_IPOIB_UD_LSO) {
+			dev_err(dev, "init_attr->create_flags error!\n");
+			ret = -EINVAL;
+			goto err_out;
+		}
+
+		/* Set SQ size */
+		ret = hns_roce_set_kernel_sq_size(hr_dev, &init_attr->cap,
+						  hr_qp);
+		if (ret) {
+			dev_err(dev, "hns_roce_set_kernel_sq_size error!\n");
+			goto err_out;
+		}
+
+		/* QP doorbell register address */
+		hr_qp->sq.db_reg_l = hr_dev->reg_base + ROCEE_DB_SQ_L_0_REG +
+				     DB_REG_OFFSET * hr_dev->priv_uar.index;
+		hr_qp->rq.db_reg_l = hr_dev->reg_base +
+				     ROCEE_DB_OTHERS_L_0_REG +
+				     DB_REG_OFFSET * hr_dev->priv_uar.index;
+
+		/* Allocate QP buf */
+		if (hns_roce_buf_alloc(hr_dev, hr_qp->buff_size, PAGE_SIZE * 2,
+				       &hr_qp->hr_buf)) {
+			dev_err(dev, "hns_roce_buf_alloc error!\n");
+			ret = -ENOMEM;
+			goto err_out;
+		}
+
+		/* Write MTT */
+		ret = hns_roce_mtt_init(hr_dev, hr_qp->hr_buf.npages,
+					hr_qp->hr_buf.page_shift, &hr_qp->mtt);
+		if (ret) {
+			dev_err(dev, "hns_roce_mtt_init error for kernel create qp\n");
+			goto err_buf;
+		}
+
+		ret = hns_roce_buf_write_mtt(hr_dev, &hr_qp->mtt,
+					     &hr_qp->hr_buf);
+		if (ret) {
+			dev_err(dev, "hns_roce_buf_write_mtt error for kernel create qp\n");
+			goto err_mtt;
+		}
+
+		hr_qp->sq.wrid = kmalloc_array(hr_qp->sq.wqe_cnt, sizeof(u64),
+					       GFP_KERNEL);
+		hr_qp->rq.wrid = kmalloc_array(hr_qp->rq.wqe_cnt, sizeof(u64),
+					       GFP_KERNEL);
+		if (!hr_qp->sq.wrid || !hr_qp->rq.wrid) {
+			ret = -ENOMEM;
+			goto err_wrid;
+		}
+	}
+
+	if (sqpn) {
+		qpn = sqpn;
+	} else {
+		/* Get QPN */
+		ret = hns_roce_reserve_range_qp(hr_dev, 1, 1, &qpn);
+		if (ret) {
+			dev_err(dev, "hns_roce_reserve_range_qp alloc qpn error\n");
+			goto err_wrid;
+		}
+	}
+
+	if ((init_attr->qp_type) == IB_QPT_GSI) {
+		ret = hns_roce_gsi_qp_alloc(hr_dev, qpn, hr_qp);
+		if (ret) {
+			dev_err(dev, "hns_roce_qp_alloc failed!\n");
+			goto err_qpn;
+		}
+	} else {
+		ret = hns_roce_qp_alloc(hr_dev, qpn, hr_qp);
+		if (ret) {
+			dev_err(dev, "hns_roce_qp_alloc failed!\n");
+			goto err_qpn;
+		}
+	}
+
+	if (sqpn)
+		hr_qp->doorbell_qpn = 1;
+	else
+		hr_qp->doorbell_qpn = cpu_to_le64(hr_qp->qpn);
+
+	hr_qp->event = hns_roce_ib_qp_event;
+
+	return 0;
+
+err_qpn:
+	if (!sqpn)
+		hns_roce_release_range_qp(hr_dev, qpn, 1);
+
+err_wrid:
+	kfree(hr_qp->sq.wrid);
+	kfree(hr_qp->rq.wrid);
+
+err_mtt:
+	hns_roce_mtt_cleanup(hr_dev, &hr_qp->mtt);
+
+err_buf:
+	if (ib_pd->uobject)
+		ib_umem_release(hr_qp->umem);
+	else
+		hns_roce_buf_free(hr_dev, hr_qp->buff_size, &hr_qp->hr_buf);
+
+err_out:
+	return ret;
+}
+
+struct ib_qp *hns_roce_create_qp(struct ib_pd *pd,
+				 struct ib_qp_init_attr *init_attr,
+				 struct ib_udata *udata)
+{
+	struct hns_roce_dev *hr_dev = to_hr_dev(pd->device);
+	struct device *dev = &hr_dev->pdev->dev;
+	struct hns_roce_sqp *hr_sqp;
+	struct hns_roce_qp *hr_qp;
+	int ret;
+
+	switch (init_attr->qp_type) {
+	case IB_QPT_RC: {
+		hr_qp = kzalloc(sizeof(*hr_qp), GFP_KERNEL);
+		if (!hr_qp)
+			return ERR_PTR(-ENOMEM);
+
+		ret = hns_roce_create_qp_common(hr_dev, pd, init_attr, udata, 0,
+						hr_qp);
+		if (ret) {
+			dev_err(dev, "Create RC QP failed\n");
+			kfree(hr_qp);
+			return ERR_PTR(ret);
+		}
+
+		hr_qp->ibqp.qp_num = hr_qp->qpn;
+
+		break;
+	}
+	case IB_QPT_GSI: {
+		/* Userspace is not allowed to create special QPs: */
+		if (pd->uobject) {
+			dev_err(dev, "not support usr space GSI\n");
+			return ERR_PTR(-EINVAL);
+		}
+
+		hr_sqp = kzalloc(sizeof(*hr_sqp), GFP_KERNEL);
+		if (!hr_sqp)
+			return ERR_PTR(-ENOMEM);
+
+		hr_qp = &hr_sqp->hr_qp;
+		hr_qp->port = init_attr->port_num - 1;
+		hr_qp->phy_port = hr_dev->iboe.phy_port[hr_qp->port];
+		hr_qp->ibqp.qp_num = HNS_ROCE_MAX_PORTS +
+				     hr_dev->iboe.phy_port[hr_qp->port];
+
+		ret = hns_roce_create_qp_common(hr_dev, pd, init_attr, udata,
+						hr_qp->ibqp.qp_num, hr_qp);
+		if (ret) {
+			dev_err(dev, "Create GSI QP failed!\n");
+			kfree(hr_sqp);
+			return ERR_PTR(ret);
+		}
+
+		break;
+	}
+	default:{
+		dev_err(dev, "not support QP type %d\n", init_attr->qp_type);
+		return ERR_PTR(-EINVAL);
+	}
+	}
+
+	return &hr_qp->ibqp;
+}
+
+int to_hr_qp_type(int qp_type)
+{
+	int transport_type;
+
+	if (qp_type == IB_QPT_RC)
+		transport_type = SERV_TYPE_RC;
+	else if (qp_type == IB_QPT_UC)
+		transport_type = SERV_TYPE_UC;
+	else if (qp_type == IB_QPT_UD)
+		transport_type = SERV_TYPE_UD;
+	else if (qp_type == IB_QPT_GSI)
+		transport_type = SERV_TYPE_UD;
+	else
+		transport_type = -1;
+
+	return transport_type;
+}
+
+int hns_roce_modify_qp(struct ib_qp *ibqp, struct ib_qp_attr *attr,
+		       int attr_mask, struct ib_udata *udata)
+{
+	struct hns_roce_dev *hr_dev = to_hr_dev(ibqp->device);
+	struct hns_roce_qp *hr_qp = to_hr_qp(ibqp);
+	enum ib_qp_state cur_state, new_state;
+	struct device *dev = &hr_dev->pdev->dev;
+	int ret = -EINVAL;
+	int p;
+	enum ib_mtu active_mtu;
+
+	mutex_lock(&hr_qp->mutex);
+
+	cur_state = attr_mask & IB_QP_CUR_STATE ?
+		    attr->cur_qp_state : (enum ib_qp_state)hr_qp->state;
+	new_state = attr_mask & IB_QP_STATE ?
+		    attr->qp_state : cur_state;
+
+	if (!ib_modify_qp_is_ok(cur_state, new_state, ibqp->qp_type, attr_mask,
+				IB_LINK_LAYER_ETHERNET)) {
+		dev_err(dev, "ib_modify_qp_is_ok failed\n");
+		goto out;
+	}
+
+	if ((attr_mask & IB_QP_PORT) &&
+	    (attr->port_num == 0 || attr->port_num > hr_dev->caps.num_ports)) {
+		dev_err(dev, "attr port_num invalid.attr->port_num=%d\n",
+			attr->port_num);
+		goto out;
+	}
+
+	if (attr_mask & IB_QP_PKEY_INDEX) {
+		p = attr_mask & IB_QP_PORT ? (attr->port_num - 1) : hr_qp->port;
+		if (attr->pkey_index >= hr_dev->caps.pkey_table_len[p]) {
+			dev_err(dev, "attr pkey_index invalid.attr->pkey_index=%d\n",
+				attr->pkey_index);
+			goto out;
+		}
+	}
+
+	if (attr_mask & IB_QP_PATH_MTU) {
+		p = attr_mask & IB_QP_PORT ? (attr->port_num - 1) : hr_qp->port;
+		active_mtu = iboe_get_mtu(hr_dev->iboe.netdevs[p]->mtu);
+
+		if (attr->path_mtu > IB_MTU_2048 ||
+		    attr->path_mtu < IB_MTU_256 ||
+		    attr->path_mtu > active_mtu) {
+			dev_err(dev, "attr path_mtu(%d)invalid while modify qp",
+				attr->path_mtu);
+			goto out;
+		}
+	}
+
+	if (attr_mask & IB_QP_MAX_QP_RD_ATOMIC &&
+	    attr->max_rd_atomic > hr_dev->caps.max_qp_init_rdma) {
+		dev_err(dev, "attr max_rd_atomic invalid.attr->max_rd_atomic=%d\n",
+			attr->max_rd_atomic);
+		goto out;
+	}
+
+	if (attr_mask & IB_QP_MAX_DEST_RD_ATOMIC &&
+	    attr->max_dest_rd_atomic > hr_dev->caps.max_qp_dest_rdma) {
+		dev_err(dev, "attr max_dest_rd_atomic invalid.attr->max_dest_rd_atomic=%d\n",
+			attr->max_dest_rd_atomic);
+		goto out;
+	}
+
+	if (cur_state == new_state && cur_state == IB_QPS_RESET) {
+		ret = -EPERM;
+		dev_err(dev, "cur_state=%d new_state=%d\n", cur_state,
+			new_state);
+		goto out;
+	}
+
+	ret = hr_dev->hw->modify_qp(ibqp, attr, attr_mask, cur_state,
+				    new_state);
+
+out:
+	mutex_unlock(&hr_qp->mutex);
+
+	return ret;
+}
+
+void hns_roce_lock_cqs(struct hns_roce_cq *send_cq, struct hns_roce_cq *recv_cq)
+		       __acquires(&send_cq->lock) __acquires(&recv_cq->lock)
+{
+	if (send_cq == recv_cq) {
+		spin_lock_irq(&send_cq->lock);
+		__acquire(&recv_cq->lock);
+	} else if (send_cq->cqn < recv_cq->cqn) {
+		spin_lock_irq(&send_cq->lock);
+		spin_lock_nested(&recv_cq->lock, SINGLE_DEPTH_NESTING);
+	} else {
+		spin_lock_irq(&recv_cq->lock);
+		spin_lock_nested(&send_cq->lock, SINGLE_DEPTH_NESTING);
+	}
+}
+
+void hns_roce_unlock_cqs(struct hns_roce_cq *send_cq,
+			 struct hns_roce_cq *recv_cq) __releases(&send_cq->lock)
+			 __releases(&recv_cq->lock)
+{
+	if (send_cq == recv_cq) {
+		__release(&recv_cq->lock);
+		spin_unlock_irq(&send_cq->lock);
+	} else if (send_cq->cqn < recv_cq->cqn) {
+		spin_unlock(&recv_cq->lock);
+		spin_unlock_irq(&send_cq->lock);
+	} else {
+		spin_unlock(&send_cq->lock);
+		spin_unlock_irq(&recv_cq->lock);
+	}
+}
+
+__be32 send_ieth(struct ib_send_wr *wr)
+{
+	switch (wr->opcode) {
+	case IB_WR_SEND_WITH_IMM:
+	case IB_WR_RDMA_WRITE_WITH_IMM:
+		return cpu_to_le32(wr->ex.imm_data);
+	case IB_WR_SEND_WITH_INV:
+		return cpu_to_le32(wr->ex.invalidate_rkey);
+	default:
+		return 0;
+	}
+}
+
+static void *get_wqe(struct hns_roce_qp *hr_qp, int offset)
+{
+
+	return hns_roce_buf_offset(&hr_qp->hr_buf, offset);
+}
+
+void *get_recv_wqe(struct hns_roce_qp *hr_qp, int n)
+{
+	return get_wqe(hr_qp, hr_qp->rq.offset + (n << hr_qp->rq.wqe_shift));
+}
+
+void *get_send_wqe(struct hns_roce_qp *hr_qp, int n)
+{
+	return get_wqe(hr_qp, hr_qp->sq.offset + (n << hr_qp->sq.wqe_shift));
+}
+
+bool hns_roce_wq_overflow(struct hns_roce_wq *hr_wq, int nreq,
+			  struct ib_cq *ib_cq)
+{
+	struct hns_roce_cq *hr_cq;
+	u32 cur;
+
+	cur = hr_wq->head - hr_wq->tail;
+	if (likely(cur + nreq < hr_wq->max_post))
+		return 0;
+
+	hr_cq = to_hr_cq(ib_cq);
+	spin_lock(&hr_cq->lock);
+	cur = hr_wq->head - hr_wq->tail;
+	spin_unlock(&hr_cq->lock);
+
+	return cur + nreq >= hr_wq->max_post;
+}
+
+int hns_roce_init_qp_table(struct hns_roce_dev *hr_dev)
+{
+	struct hns_roce_qp_table *qp_table = &hr_dev->qp_table;
+	int reserved_from_top = 0;
+	int ret;
+
+	spin_lock_init(&qp_table->lock);
+	INIT_RADIX_TREE(&hr_dev->qp_table_tree, GFP_ATOMIC);
+
+	/* A port include two SQP, six port total 12 */
+	ret = hns_roce_bitmap_init(&qp_table->bitmap, hr_dev->caps.num_qps,
+				   hr_dev->caps.num_qps - 1, SQP_NUM,
+				   reserved_from_top);
+	if (ret) {
+		dev_err(&hr_dev->pdev->dev, "qp bitmap init failed!error=%d\n",
+			ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+void hns_roce_cleanup_qp_table(struct hns_roce_dev *hr_dev)
+{
+	hns_roce_bitmap_cleanup(&hr_dev->qp_table.bitmap);
+}
diff --git a/drivers/infiniband/hw/cxgb3/iwch_user.h b/drivers/infiniband/hw/hns/hns_roce_user.h
index a277c31fcaf7..a28f761a9f65 100644
--- a/drivers/infiniband/hw/cxgb3/iwch_user.h
+++ b/drivers/infiniband/hw/hns/hns_roce_user.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2006 Chelsio, Inc. All rights reserved.
+ * Copyright (c) 2016 Hisilicon Limited.
  *
  * 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
@@ -29,46 +29,25 @@
  * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
  * SOFTWARE.
  */
-#ifndef __IWCH_USER_H__
-#define __IWCH_USER_H__
 
-#define IWCH_UVERBS_ABI_VERSION	1
+#ifndef _HNS_ROCE_USER_H
+#define _HNS_ROCE_USER_H
 
-/*
- * Make sure that all structs defined in this file remain laid out so
- * that they pack the same way on 32-bit and 64-bit architectures (to
- * avoid incompatibility between 32-bit userspace and 64-bit kernels).
- * In particular do not use pointer types -- pass pointers in __u64
- * instead.
- */
-struct iwch_create_cq_req {
-	__u64 user_rptr_addr;
+struct hns_roce_ib_create_cq {
+	__u64   buf_addr;
 };
 
-struct iwch_create_cq_resp_v0 {
-	__u64 key;
-	__u32 cqid;
-	__u32 size_log2;
+struct hns_roce_ib_create_qp {
+	__u64	buf_addr;
+	__u64   db_addr;
+	__u8    log_sq_bb_count;
+	__u8    log_sq_stride;
+	__u8    sq_no_prefetch;
+	__u8    reserved[5];
 };
 
-struct iwch_create_cq_resp {
-	__u64 key;
-	__u32 cqid;
-	__u32 size_log2;
-	__u32 memsize;
-	__u32 reserved;
+struct hns_roce_ib_alloc_ucontext_resp {
+	__u32	qp_tab_size;
 };
 
-struct iwch_create_qp_resp {
-	__u64 key;
-	__u64 db_key;
-	__u32 qpid;
-	__u32 size_log2;
-	__u32 sq_size_log2;
-	__u32 rq_size_log2;
-};
-
-struct iwch_reg_user_mr_resp {
-	__u32 pbl_addr;
-};
-#endif
+#endif /*_HNS_ROCE_USER_H */
diff --git a/drivers/infiniband/hw/i40iw/i40iw_cm.c b/drivers/infiniband/hw/i40iw/i40iw_cm.c
index 7ca0638579c0..85637696f6e9 100644
--- a/drivers/infiniband/hw/i40iw/i40iw_cm.c
+++ b/drivers/infiniband/hw/i40iw/i40iw_cm.c
@@ -3166,8 +3166,11 @@ void i40iw_setup_cm_core(struct i40iw_device *iwdev)
 	spin_lock_init(&cm_core->ht_lock);
 	spin_lock_init(&cm_core->listen_list_lock);
 
-	cm_core->event_wq = create_singlethread_workqueue("iwewq");
-	cm_core->disconn_wq = create_singlethread_workqueue("iwdwq");
+	cm_core->event_wq = alloc_ordered_workqueue("iwewq",
+						    WQ_MEM_RECLAIM);
+
+	cm_core->disconn_wq = alloc_ordered_workqueue("iwdwq",
+						      WQ_MEM_RECLAIM);
 }
 
 /**
diff --git a/drivers/infiniband/hw/i40iw/i40iw_main.c b/drivers/infiniband/hw/i40iw/i40iw_main.c
index 445e230d5ff8..ac2f3cd9478c 100644
--- a/drivers/infiniband/hw/i40iw/i40iw_main.c
+++ b/drivers/infiniband/hw/i40iw/i40iw_main.c
@@ -1615,7 +1615,7 @@ static int i40iw_open(struct i40e_info *ldev, struct i40e_client *client)
 		status = i40iw_hmc_init_pble(&iwdev->sc_dev, iwdev->pble_rsrc);
 		if (status)
 			break;
-		iwdev->virtchnl_wq = create_singlethread_workqueue("iwvch");
+		iwdev->virtchnl_wq = alloc_ordered_workqueue("iwvch", WQ_MEM_RECLAIM);
 		i40iw_register_notifiers();
 		iwdev->init_state = INET_NOTIFIER;
 		status = i40iw_add_mac_ip(iwdev);
diff --git a/drivers/infiniband/hw/mlx4/alias_GUID.c b/drivers/infiniband/hw/mlx4/alias_GUID.c
index c74ef2620b85..5e9939045852 100644
--- a/drivers/infiniband/hw/mlx4/alias_GUID.c
+++ b/drivers/infiniband/hw/mlx4/alias_GUID.c
@@ -881,7 +881,7 @@ int mlx4_ib_init_alias_guid_service(struct mlx4_ib_dev *dev)
 
 		snprintf(alias_wq_name, sizeof alias_wq_name, "alias_guid%d", i);
 		dev->sriov.alias_guid.ports_guid[i].wq =
-			create_singlethread_workqueue(alias_wq_name);
+			alloc_ordered_workqueue(alias_wq_name, WQ_MEM_RECLAIM);
 		if (!dev->sriov.alias_guid.ports_guid[i].wq) {
 			ret = -ENOMEM;
 			goto err_thread;
diff --git a/drivers/infiniband/hw/mlx4/cq.c b/drivers/infiniband/hw/mlx4/cq.c
index 5df63dacaaa3..1ea686b9e0f9 100644
--- a/drivers/infiniband/hw/mlx4/cq.c
+++ b/drivers/infiniband/hw/mlx4/cq.c
@@ -37,7 +37,7 @@
 #include <linux/slab.h>
 
 #include "mlx4_ib.h"
-#include "user.h"
+#include <rdma/mlx4-abi.h>
 
 static void mlx4_ib_cq_comp(struct mlx4_cq *cq)
 {
diff --git a/drivers/infiniband/hw/mlx4/mad.c b/drivers/infiniband/hw/mlx4/mad.c
index 0f21c3a25552..1672907ff219 100644
--- a/drivers/infiniband/hw/mlx4/mad.c
+++ b/drivers/infiniband/hw/mlx4/mad.c
@@ -230,6 +230,8 @@ static void smp_snoop(struct ib_device *ibdev, u8 port_num, const struct ib_mad
 	    mad->mad_hdr.method == IB_MGMT_METHOD_SET)
 		switch (mad->mad_hdr.attr_id) {
 		case IB_SMP_ATTR_PORT_INFO:
+			if (dev->dev->caps.flags & MLX4_DEV_CAP_FLAG_PORT_MNG_CHG_EV)
+				return;
 			pinfo = (struct ib_port_info *) ((struct ib_smp *) mad)->data;
 			lid = be16_to_cpu(pinfo->lid);
 
@@ -245,6 +247,8 @@ static void smp_snoop(struct ib_device *ibdev, u8 port_num, const struct ib_mad
 			break;
 
 		case IB_SMP_ATTR_PKEY_TABLE:
+			if (dev->dev->caps.flags & MLX4_DEV_CAP_FLAG_PORT_MNG_CHG_EV)
+				return;
 			if (!mlx4_is_mfunc(dev->dev)) {
 				mlx4_ib_dispatch_event(dev, port_num,
 						       IB_EVENT_PKEY_CHANGE);
@@ -281,6 +285,8 @@ static void smp_snoop(struct ib_device *ibdev, u8 port_num, const struct ib_mad
 			break;
 
 		case IB_SMP_ATTR_GUID_INFO:
+			if (dev->dev->caps.flags & MLX4_DEV_CAP_FLAG_PORT_MNG_CHG_EV)
+				return;
 			/* paravirtualized master's guid is guid 0 -- does not change */
 			if (!mlx4_is_master(dev->dev))
 				mlx4_ib_dispatch_event(dev, port_num,
@@ -296,6 +302,26 @@ static void smp_snoop(struct ib_device *ibdev, u8 port_num, const struct ib_mad
 			}
 			break;
 
+		case IB_SMP_ATTR_SL_TO_VL_TABLE:
+			/* cache sl to vl mapping changes for use in
+			 * filling QP1 LRH VL field when sending packets
+			 */
+			if (dev->dev->caps.flags & MLX4_DEV_CAP_FLAG_PORT_MNG_CHG_EV &&
+			    dev->dev->caps.flags2 & MLX4_DEV_CAP_FLAG2_SL_TO_VL_CHANGE_EVENT)
+				return;
+			if (!mlx4_is_slave(dev->dev)) {
+				union sl2vl_tbl_to_u64 sl2vl64;
+				int jj;
+
+				for (jj = 0; jj < 8; jj++) {
+					sl2vl64.sl8[jj] = ((struct ib_smp *)mad)->data[jj];
+					pr_debug("port %u, sl2vl[%d] = %02x\n",
+						 port_num, jj, sl2vl64.sl8[jj]);
+				}
+				atomic64_set(&dev->sl2vl[port_num - 1], sl2vl64.sl64);
+			}
+			break;
+
 		default:
 			break;
 		}
@@ -345,7 +371,8 @@ static void node_desc_override(struct ib_device *dev,
 	    mad->mad_hdr.method == IB_MGMT_METHOD_GET_RESP &&
 	    mad->mad_hdr.attr_id == IB_SMP_ATTR_NODE_DESC) {
 		spin_lock_irqsave(&to_mdev(dev)->sm_lock, flags);
-		memcpy(((struct ib_smp *) mad)->data, dev->node_desc, 64);
+		memcpy(((struct ib_smp *) mad)->data, dev->node_desc,
+		       IB_DEVICE_NODE_DESC_MAX);
 		spin_unlock_irqrestore(&to_mdev(dev)->sm_lock, flags);
 	}
 }
@@ -805,8 +832,7 @@ static int ib_process_mad(struct ib_device *ibdev, int mad_flags, u8 port_num,
 		return IB_MAD_RESULT_FAILURE;
 
 	if (!out_mad->mad_hdr.status) {
-		if (!(to_mdev(ibdev)->dev->caps.flags & MLX4_DEV_CAP_FLAG_PORT_MNG_CHG_EV))
-			smp_snoop(ibdev, port_num, in_mad, prev_lid);
+		smp_snoop(ibdev, port_num, in_mad, prev_lid);
 		/* slaves get node desc from FW */
 		if (!mlx4_is_slave(to_mdev(ibdev)->dev))
 			node_desc_override(ibdev, out_mad);
@@ -1037,6 +1063,23 @@ static void handle_client_rereg_event(struct mlx4_ib_dev *dev, u8 port_num)
 						    MLX4_EQ_PORT_INFO_CLIENT_REREG_MASK);
 		}
 	}
+
+	/* Update the sl to vl table from inside client rereg
+	 * only if in secure-host mode (snooping is not possible)
+	 * and the sl-to-vl change event is not generated by FW.
+	 */
+	if (!mlx4_is_slave(dev->dev) &&
+	    dev->dev->flags & MLX4_FLAG_SECURE_HOST &&
+	    !(dev->dev->caps.flags2 & MLX4_DEV_CAP_FLAG2_SL_TO_VL_CHANGE_EVENT)) {
+		if (mlx4_is_master(dev->dev))
+			/* already in work queue from mlx4_ib_event queueing
+			 * mlx4_handle_port_mgmt_change_event, which calls
+			 * this procedure. Therefore, call sl2vl_update directly.
+			 */
+			mlx4_ib_sl2vl_update(dev, port_num);
+		else
+			mlx4_sched_ib_sl2vl_update_work(dev, port_num);
+	}
 	mlx4_ib_dispatch_event(dev, port_num, IB_EVENT_CLIENT_REREGISTER);
 }
 
@@ -1176,6 +1219,24 @@ void handle_port_mgmt_change_event(struct work_struct *work)
 			handle_slaves_guid_change(dev, port, tbl_block, change_bitmap);
 		}
 		break;
+
+	case MLX4_DEV_PMC_SUBTYPE_SL_TO_VL_MAP:
+		/* cache sl to vl mapping changes for use in
+		 * filling QP1 LRH VL field when sending packets
+		 */
+		if (!mlx4_is_slave(dev->dev)) {
+			union sl2vl_tbl_to_u64 sl2vl64;
+			int jj;
+
+			for (jj = 0; jj < 8; jj++) {
+				sl2vl64.sl8[jj] =
+					eqe->event.port_mgmt_change.params.sl2vl_tbl_change_info.sl2vl_table[jj];
+				pr_debug("port %u, sl2vl[%d] = %02x\n",
+					 port, jj, sl2vl64.sl8[jj]);
+			}
+			atomic64_set(&dev->sl2vl[port - 1], sl2vl64.sl64);
+		}
+		break;
 	default:
 		pr_warn("Unsupported subtype 0x%x for "
 			"Port Management Change event\n", eqe->subtype);
@@ -1918,7 +1979,7 @@ static int create_pv_resources(struct ib_device *ibdev, int slave, int port,
 		goto err_buf;
 	}
 
-	ctx->pd = ib_alloc_pd(ctx->ib_dev);
+	ctx->pd = ib_alloc_pd(ctx->ib_dev, 0);
 	if (IS_ERR(ctx->pd)) {
 		ret = PTR_ERR(ctx->pd);
 		pr_err("Couldn't create tunnel PD (%d)\n", ret);
@@ -2091,7 +2152,7 @@ static int mlx4_ib_alloc_demux_ctx(struct mlx4_ib_dev *dev,
 	}
 
 	snprintf(name, sizeof name, "mlx4_ibt%d", port);
-	ctx->wq = create_singlethread_workqueue(name);
+	ctx->wq = alloc_ordered_workqueue(name, WQ_MEM_RECLAIM);
 	if (!ctx->wq) {
 		pr_err("Failed to create tunnelling WQ for port %d\n", port);
 		ret = -ENOMEM;
@@ -2099,7 +2160,7 @@ static int mlx4_ib_alloc_demux_ctx(struct mlx4_ib_dev *dev,
 	}
 
 	snprintf(name, sizeof name, "mlx4_ibud%d", port);
-	ctx->ud_wq = create_singlethread_workqueue(name);
+	ctx->ud_wq = alloc_ordered_workqueue(name, WQ_MEM_RECLAIM);
 	if (!ctx->ud_wq) {
 		pr_err("Failed to create up/down WQ for port %d\n", port);
 		ret = -ENOMEM;
diff --git a/drivers/infiniband/hw/mlx4/main.c b/drivers/infiniband/hw/mlx4/main.c
index 87ba9bca4181..b597e8227591 100644
--- a/drivers/infiniband/hw/mlx4/main.c
+++ b/drivers/infiniband/hw/mlx4/main.c
@@ -55,7 +55,7 @@
 #include <linux/mlx4/qp.h>
 
 #include "mlx4_ib.h"
-#include "user.h"
+#include <rdma/mlx4-abi.h>
 
 #define DRV_NAME	MLX4_IB_DRV_NAME
 #define DRV_VERSION	"2.2-1"
@@ -832,6 +832,66 @@ static int mlx4_ib_query_gid(struct ib_device *ibdev, u8 port, int index,
 	return ret;
 }
 
+static int mlx4_ib_query_sl2vl(struct ib_device *ibdev, u8 port, u64 *sl2vl_tbl)
+{
+	union sl2vl_tbl_to_u64 sl2vl64;
+	struct ib_smp *in_mad  = NULL;
+	struct ib_smp *out_mad = NULL;
+	int mad_ifc_flags = MLX4_MAD_IFC_IGNORE_KEYS;
+	int err = -ENOMEM;
+	int jj;
+
+	if (mlx4_is_slave(to_mdev(ibdev)->dev)) {
+		*sl2vl_tbl = 0;
+		return 0;
+	}
+
+	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_SL_TO_VL_TABLE;
+	in_mad->attr_mod = 0;
+
+	if (mlx4_is_mfunc(to_mdev(ibdev)->dev))
+		mad_ifc_flags |= MLX4_MAD_IFC_NET_VIEW;
+
+	err = mlx4_MAD_IFC(to_mdev(ibdev), mad_ifc_flags, port, NULL, NULL,
+			   in_mad, out_mad);
+	if (err)
+		goto out;
+
+	for (jj = 0; jj < 8; jj++)
+		sl2vl64.sl8[jj] = ((struct ib_smp *)out_mad)->data[jj];
+	*sl2vl_tbl = sl2vl64.sl64;
+
+out:
+	kfree(in_mad);
+	kfree(out_mad);
+	return err;
+}
+
+static void mlx4_init_sl2vl_tbl(struct mlx4_ib_dev *mdev)
+{
+	u64 sl2vl;
+	int i;
+	int err;
+
+	for (i = 1; i <= mdev->dev->caps.num_ports; i++) {
+		if (mdev->dev->caps.port_type[i] == MLX4_PORT_TYPE_ETH)
+			continue;
+		err = mlx4_ib_query_sl2vl(&mdev->ib_dev, i, &sl2vl);
+		if (err) {
+			pr_err("Unable to get default sl to vl mapping for port %d.  Using all zeroes (%d)\n",
+			       i, err);
+			sl2vl = 0;
+		}
+		atomic64_set(&mdev->sl2vl[i - 1], sl2vl);
+	}
+}
+
 int __mlx4_ib_query_pkey(struct ib_device *ibdev, u8 port, u16 index,
 			 u16 *pkey, int netw_view)
 {
@@ -886,7 +946,7 @@ static int mlx4_ib_modify_device(struct ib_device *ibdev, int mask,
 		return -EOPNOTSUPP;
 
 	spin_lock_irqsave(&to_mdev(ibdev)->sm_lock, flags);
-	memcpy(ibdev->node_desc, props->node_desc, 64);
+	memcpy(ibdev->node_desc, props->node_desc, IB_DEVICE_NODE_DESC_MAX);
 	spin_unlock_irqrestore(&to_mdev(ibdev)->sm_lock, flags);
 
 	/*
@@ -897,7 +957,7 @@ static int mlx4_ib_modify_device(struct ib_device *ibdev, int mask,
 	if (IS_ERR(mailbox))
 		return 0;
 
-	memcpy(mailbox->buf, props->node_desc, 64);
+	memcpy(mailbox->buf, props->node_desc, IB_DEVICE_NODE_DESC_MAX);
 	mlx4_cmd(to_mdev(ibdev)->dev, mailbox->dma, 1, 0,
 		 MLX4_CMD_SET_NODE, MLX4_CMD_TIME_CLASS_A, MLX4_CMD_NATIVE);
 
@@ -1259,7 +1319,7 @@ static struct ib_xrcd *mlx4_ib_alloc_xrcd(struct ib_device *ibdev,
 	if (err)
 		goto err1;
 
-	xrcd->pd = ib_alloc_pd(ibdev);
+	xrcd->pd = ib_alloc_pd(ibdev, 0);
 	if (IS_ERR(xrcd->pd)) {
 		err = PTR_ERR(xrcd->pd);
 		goto err2;
@@ -1361,6 +1421,19 @@ struct mlx4_ib_steering {
 	union ib_gid gid;
 };
 
+#define LAST_ETH_FIELD vlan_tag
+#define LAST_IB_FIELD sl
+#define LAST_IPV4_FIELD dst_ip
+#define LAST_TCP_UDP_FIELD src_port
+
+/* Field is the last supported field */
+#define FIELDS_NOT_SUPPORTED(filter, field)\
+	memchr_inv((void *)&filter.field  +\
+		   sizeof(filter.field), 0,\
+		   sizeof(filter) -\
+		   offsetof(typeof(filter), field) -\
+		   sizeof(filter.field))
+
 static int parse_flow_attr(struct mlx4_dev *dev,
 			   u32 qp_num,
 			   union ib_flow_spec *ib_spec,
@@ -1370,6 +1443,9 @@ static int parse_flow_attr(struct mlx4_dev *dev,
 
 	switch (ib_spec->type) {
 	case IB_FLOW_SPEC_ETH:
+		if (FIELDS_NOT_SUPPORTED(ib_spec->eth.mask, LAST_ETH_FIELD))
+			return -ENOTSUPP;
+
 		type = MLX4_NET_TRANS_RULE_ID_ETH;
 		memcpy(mlx4_spec->eth.dst_mac, ib_spec->eth.val.dst_mac,
 		       ETH_ALEN);
@@ -1379,6 +1455,9 @@ static int parse_flow_attr(struct mlx4_dev *dev,
 		mlx4_spec->eth.vlan_tag_msk = ib_spec->eth.mask.vlan_tag;
 		break;
 	case IB_FLOW_SPEC_IB:
+		if (FIELDS_NOT_SUPPORTED(ib_spec->ib.mask, LAST_IB_FIELD))
+			return -ENOTSUPP;
+
 		type = MLX4_NET_TRANS_RULE_ID_IB;
 		mlx4_spec->ib.l3_qpn =
 			cpu_to_be32(qp_num);
@@ -1388,6 +1467,9 @@ static int parse_flow_attr(struct mlx4_dev *dev,
 
 
 	case IB_FLOW_SPEC_IPV4:
+		if (FIELDS_NOT_SUPPORTED(ib_spec->ipv4.mask, LAST_IPV4_FIELD))
+			return -ENOTSUPP;
+
 		type = MLX4_NET_TRANS_RULE_ID_IPV4;
 		mlx4_spec->ipv4.src_ip = ib_spec->ipv4.val.src_ip;
 		mlx4_spec->ipv4.src_ip_msk = ib_spec->ipv4.mask.src_ip;
@@ -1397,6 +1479,9 @@ static int parse_flow_attr(struct mlx4_dev *dev,
 
 	case IB_FLOW_SPEC_TCP:
 	case IB_FLOW_SPEC_UDP:
+		if (FIELDS_NOT_SUPPORTED(ib_spec->tcp_udp.mask, LAST_TCP_UDP_FIELD))
+			return -ENOTSUPP;
+
 		type = ib_spec->type == IB_FLOW_SPEC_TCP ?
 					MLX4_NET_TRANS_RULE_ID_TCP :
 					MLX4_NET_TRANS_RULE_ID_UDP;
@@ -2000,7 +2085,7 @@ static int init_node_data(struct mlx4_ib_dev *dev)
 	if (err)
 		goto out;
 
-	memcpy(dev->ib_dev.node_desc, out_mad->data, 64);
+	memcpy(dev->ib_dev.node_desc, out_mad->data, IB_DEVICE_NODE_DESC_MAX);
 
 	in_mad->attr_id = IB_SMP_ATTR_NODE_INFO;
 
@@ -2653,6 +2738,7 @@ static void *mlx4_ib_add(struct mlx4_dev *dev)
 
 	if (init_node_data(ibdev))
 		goto err_map;
+	mlx4_init_sl2vl_tbl(ibdev);
 
 	for (i = 0; i < ibdev->num_ports; ++i) {
 		mutex_init(&ibdev->counters_table[i].mutex);
@@ -3101,6 +3187,47 @@ static void handle_bonded_port_state_event(struct work_struct *work)
 	ib_dispatch_event(&ibev);
 }
 
+void mlx4_ib_sl2vl_update(struct mlx4_ib_dev *mdev, int port)
+{
+	u64 sl2vl;
+	int err;
+
+	err = mlx4_ib_query_sl2vl(&mdev->ib_dev, port, &sl2vl);
+	if (err) {
+		pr_err("Unable to get current sl to vl mapping for port %d.  Using all zeroes (%d)\n",
+		       port, err);
+		sl2vl = 0;
+	}
+	atomic64_set(&mdev->sl2vl[port - 1], sl2vl);
+}
+
+static void ib_sl2vl_update_work(struct work_struct *work)
+{
+	struct ib_event_work *ew = container_of(work, struct ib_event_work, work);
+	struct mlx4_ib_dev *mdev = ew->ib_dev;
+	int port = ew->port;
+
+	mlx4_ib_sl2vl_update(mdev, port);
+
+	kfree(ew);
+}
+
+void mlx4_sched_ib_sl2vl_update_work(struct mlx4_ib_dev *ibdev,
+				     int port)
+{
+	struct ib_event_work *ew;
+
+	ew = kmalloc(sizeof(*ew), GFP_ATOMIC);
+	if (ew) {
+		INIT_WORK(&ew->work, ib_sl2vl_update_work);
+		ew->port = port;
+		ew->ib_dev = ibdev;
+		queue_work(wq, &ew->work);
+	} else {
+		pr_err("failed to allocate memory for sl2vl update work\n");
+	}
+}
+
 static void mlx4_ib_event(struct mlx4_dev *dev, void *ibdev_ptr,
 			  enum mlx4_dev_event event, unsigned long param)
 {
@@ -3131,10 +3258,14 @@ static void mlx4_ib_event(struct mlx4_dev *dev, void *ibdev_ptr,
 	case MLX4_DEV_EVENT_PORT_UP:
 		if (p > ibdev->num_ports)
 			return;
-		if (mlx4_is_master(dev) &&
+		if (!mlx4_is_slave(dev) &&
 		    rdma_port_get_link_layer(&ibdev->ib_dev, p) ==
 			IB_LINK_LAYER_INFINIBAND) {
-			mlx4_ib_invalidate_all_guid_record(ibdev, p);
+			if (mlx4_is_master(dev))
+				mlx4_ib_invalidate_all_guid_record(ibdev, p);
+			if (ibdev->dev->flags & MLX4_FLAG_SECURE_HOST &&
+			    !(ibdev->dev->caps.flags2 & MLX4_DEV_CAP_FLAG2_SL_TO_VL_CHANGE_EVENT))
+				mlx4_sched_ib_sl2vl_update_work(ibdev, p);
 		}
 		ibev.event = IB_EVENT_PORT_ACTIVE;
 		break;
@@ -3222,7 +3353,7 @@ static int __init mlx4_ib_init(void)
 {
 	int err;
 
-	wq = create_singlethread_workqueue("mlx4_ib");
+	wq = alloc_ordered_workqueue("mlx4_ib", WQ_MEM_RECLAIM);
 	if (!wq)
 		return -ENOMEM;
 
diff --git a/drivers/infiniband/hw/mlx4/mcg.c b/drivers/infiniband/hw/mlx4/mcg.c
index 097bfcc4ee99..a21d37f02f35 100644
--- a/drivers/infiniband/hw/mlx4/mcg.c
+++ b/drivers/infiniband/hw/mlx4/mcg.c
@@ -1045,7 +1045,7 @@ int mlx4_ib_mcg_port_init(struct mlx4_ib_demux_ctx *ctx)
 
 	atomic_set(&ctx->tid, 0);
 	sprintf(name, "mlx4_ib_mcg%d", ctx->port);
-	ctx->mcg_wq = create_singlethread_workqueue(name);
+	ctx->mcg_wq = alloc_ordered_workqueue(name, WQ_MEM_RECLAIM);
 	if (!ctx->mcg_wq)
 		return -ENOMEM;
 
@@ -1246,7 +1246,7 @@ void clean_vf_mcast(struct mlx4_ib_demux_ctx *ctx, int slave)
 
 int mlx4_ib_mcg_init(void)
 {
-	clean_wq = create_singlethread_workqueue("mlx4_ib_mcg");
+	clean_wq = alloc_ordered_workqueue("mlx4_ib_mcg", WQ_MEM_RECLAIM);
 	if (!clean_wq)
 		return -ENOMEM;
 
diff --git a/drivers/infiniband/hw/mlx4/mlx4_ib.h b/drivers/infiniband/hw/mlx4/mlx4_ib.h
index 686ab48ff644..35141f451e5c 100644
--- a/drivers/infiniband/hw/mlx4/mlx4_ib.h
+++ b/drivers/infiniband/hw/mlx4/mlx4_ib.h
@@ -570,6 +570,7 @@ struct mlx4_ib_dev {
 	struct ib_mad_agent    *send_agent[MLX4_MAX_PORTS][2];
 	struct ib_ah	       *sm_ah[MLX4_MAX_PORTS];
 	spinlock_t		sm_lock;
+	atomic64_t		sl2vl[MLX4_MAX_PORTS];
 	struct mlx4_ib_sriov	sriov;
 
 	struct mutex		cap_mask_mutex;
@@ -600,6 +601,7 @@ struct ib_event_work {
 	struct work_struct	work;
 	struct mlx4_ib_dev	*ib_dev;
 	struct mlx4_eqe		ib_eqe;
+	int			port;
 };
 
 struct mlx4_ib_qp_tunnel_init_attr {
@@ -883,4 +885,9 @@ int mlx4_ib_rereg_user_mr(struct ib_mr *mr, int flags,
 int mlx4_ib_gid_index_to_real_index(struct mlx4_ib_dev *ibdev,
 				    u8 port_num, int index);
 
+void mlx4_sched_ib_sl2vl_update_work(struct mlx4_ib_dev *ibdev,
+				     int port);
+
+void mlx4_ib_sl2vl_update(struct mlx4_ib_dev *mdev, int port);
+
 #endif /* MLX4_IB_H */
diff --git a/drivers/infiniband/hw/mlx4/qp.c b/drivers/infiniband/hw/mlx4/qp.c
index 7fb9629bd12b..570bc866b1d6 100644
--- a/drivers/infiniband/hw/mlx4/qp.c
+++ b/drivers/infiniband/hw/mlx4/qp.c
@@ -47,7 +47,7 @@
 #include <linux/mlx4/qp.h>
 
 #include "mlx4_ib.h"
-#include "user.h"
+#include <rdma/mlx4-abi.h>
 
 static void mlx4_ib_lock_cqs(struct mlx4_ib_cq *send_cq,
 			     struct mlx4_ib_cq *recv_cq);
@@ -2405,6 +2405,22 @@ static int build_sriov_qp0_header(struct mlx4_ib_sqp *sqp,
 	return 0;
 }
 
+static u8 sl_to_vl(struct mlx4_ib_dev *dev, u8 sl, int port_num)
+{
+	union sl2vl_tbl_to_u64 tmp_vltab;
+	u8 vl;
+
+	if (sl > 15)
+		return 0xf;
+	tmp_vltab.sl64 = atomic64_read(&dev->sl2vl[port_num - 1]);
+	vl = tmp_vltab.sl8[sl >> 1];
+	if (sl & 1)
+		vl &= 0x0f;
+	else
+		vl >>= 4;
+	return vl;
+}
+
 #define MLX4_ROCEV2_QP1_SPORT 0xC000
 static int build_mlx_header(struct mlx4_ib_sqp *sqp, struct ib_ud_wr *wr,
 			    void *wqe, unsigned *mlx_seg_len)
@@ -2590,7 +2606,12 @@ static int build_mlx_header(struct mlx4_ib_sqp *sqp, struct ib_ud_wr *wr,
 			sqp->ud_header.vlan.tag = cpu_to_be16(vlan | pcp);
 		}
 	} else {
-		sqp->ud_header.lrh.virtual_lane    = !sqp->qp.ibqp.qp_num ? 15 : 0;
+		sqp->ud_header.lrh.virtual_lane    = !sqp->qp.ibqp.qp_num ? 15 :
+							sl_to_vl(to_mdev(ib_dev),
+								 sqp->ud_header.lrh.service_level,
+								 sqp->qp.port);
+		if (sqp->qp.ibqp.qp_num && sqp->ud_header.lrh.virtual_lane == 15)
+			return -EINVAL;
 		if (sqp->ud_header.lrh.destination_lid == IB_LID_PERMISSIVE)
 			sqp->ud_header.lrh.source_lid = IB_LID_PERMISSIVE;
 	}
diff --git a/drivers/infiniband/hw/mlx4/srq.c b/drivers/infiniband/hw/mlx4/srq.c
index 0597f3eef5d0..7dd3f267f06b 100644
--- a/drivers/infiniband/hw/mlx4/srq.c
+++ b/drivers/infiniband/hw/mlx4/srq.c
@@ -37,7 +37,7 @@
 #include <linux/vmalloc.h>
 
 #include "mlx4_ib.h"
-#include "user.h"
+#include <rdma/mlx4-abi.h>
 
 static void *get_wqe(struct mlx4_ib_srq *srq, int n)
 {
diff --git a/drivers/infiniband/hw/mlx4/user.h b/drivers/infiniband/hw/mlx4/user.h
deleted file mode 100644
index 07e6769ef43b..000000000000
--- a/drivers/infiniband/hw/mlx4/user.h
+++ /dev/null
@@ -1,107 +0,0 @@
-/*
- * Copyright (c) 2007 Cisco Systems, Inc. All rights reserved.
- * Copyright (c) 2007, 2008 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.
- */
-
-#ifndef MLX4_IB_USER_H
-#define MLX4_IB_USER_H
-
-#include <linux/types.h>
-
-/*
- * Increment this value if any changes that break userspace ABI
- * compatibility are made.
- */
-
-#define MLX4_IB_UVERBS_NO_DEV_CAPS_ABI_VERSION	3
-#define MLX4_IB_UVERBS_ABI_VERSION		4
-
-/*
- * Make sure that all structs defined in this file remain laid out so
- * that they pack the same way on 32-bit and 64-bit architectures (to
- * avoid incompatibility between 32-bit userspace and 64-bit kernels).
- * In particular do not use pointer types -- pass pointers in __u64
- * instead.
- */
-
-struct mlx4_ib_alloc_ucontext_resp_v3 {
-	__u32	qp_tab_size;
-	__u16	bf_reg_size;
-	__u16	bf_regs_per_page;
-};
-
-struct mlx4_ib_alloc_ucontext_resp {
-	__u32	dev_caps;
-	__u32	qp_tab_size;
-	__u16	bf_reg_size;
-	__u16	bf_regs_per_page;
-	__u32	cqe_size;
-};
-
-struct mlx4_ib_alloc_pd_resp {
-	__u32	pdn;
-	__u32	reserved;
-};
-
-struct mlx4_ib_create_cq {
-	__u64	buf_addr;
-	__u64	db_addr;
-};
-
-struct mlx4_ib_create_cq_resp {
-	__u32	cqn;
-	__u32	reserved;
-};
-
-struct mlx4_ib_resize_cq {
-	__u64	buf_addr;
-};
-
-struct mlx4_ib_create_srq {
-	__u64	buf_addr;
-	__u64	db_addr;
-};
-
-struct mlx4_ib_create_srq_resp {
-	__u32	srqn;
-	__u32	reserved;
-};
-
-struct mlx4_ib_create_qp {
-	__u64	buf_addr;
-	__u64	db_addr;
-	__u8	log_sq_bb_count;
-	__u8	log_sq_stride;
-	__u8	sq_no_prefetch;
-	__u8	reserved[5];
-};
-
-#endif /* MLX4_IB_USER_H */
diff --git a/drivers/infiniband/hw/mlx5/cq.c b/drivers/infiniband/hw/mlx5/cq.c
index e4fac9292e4a..79d017baf6f4 100644
--- a/drivers/infiniband/hw/mlx5/cq.c
+++ b/drivers/infiniband/hw/mlx5/cq.c
@@ -35,7 +35,6 @@
 #include <rdma/ib_user_verbs.h>
 #include <rdma/ib_cache.h>
 #include "mlx5_ib.h"
-#include "user.h"
 
 static void mlx5_ib_cq_comp(struct mlx5_core_cq *cq)
 {
@@ -729,14 +728,16 @@ static int alloc_cq_buf(struct mlx5_ib_dev *dev, struct mlx5_ib_cq_buf *buf,
 
 static int create_cq_user(struct mlx5_ib_dev *dev, struct ib_udata *udata,
 			  struct ib_ucontext *context, struct mlx5_ib_cq *cq,
-			  int entries, struct mlx5_create_cq_mbox_in **cqb,
+			  int entries, u32 **cqb,
 			  int *cqe_size, int *index, int *inlen)
 {
 	struct mlx5_ib_create_cq ucmd;
 	size_t ucmdlen;
 	int page_shift;
+	__be64 *pas;
 	int npages;
 	int ncont;
+	void *cqc;
 	int err;
 
 	ucmdlen =
@@ -774,14 +775,20 @@ static int create_cq_user(struct mlx5_ib_dev *dev, struct ib_udata *udata,
 	mlx5_ib_dbg(dev, "addr 0x%llx, size %u, npages %d, page_shift %d, ncont %d\n",
 		    ucmd.buf_addr, entries * ucmd.cqe_size, npages, page_shift, ncont);
 
-	*inlen = sizeof(**cqb) + sizeof(*(*cqb)->pas) * ncont;
+	*inlen = MLX5_ST_SZ_BYTES(create_cq_in) +
+		 MLX5_FLD_SZ_BYTES(create_cq_in, pas[0]) * ncont;
 	*cqb = mlx5_vzalloc(*inlen);
 	if (!*cqb) {
 		err = -ENOMEM;
 		goto err_db;
 	}
-	mlx5_ib_populate_pas(dev, cq->buf.umem, page_shift, (*cqb)->pas, 0);
-	(*cqb)->ctx.log_pg_sz = page_shift - MLX5_ADAPTER_PAGE_SHIFT;
+
+	pas = (__be64 *)MLX5_ADDR_OF(create_cq_in, *cqb, pas);
+	mlx5_ib_populate_pas(dev, cq->buf.umem, page_shift, pas, 0);
+
+	cqc = MLX5_ADDR_OF(create_cq_in, *cqb, cq_context);
+	MLX5_SET(cqc, cqc, log_page_size,
+		 page_shift - MLX5_ADAPTER_PAGE_SHIFT);
 
 	*index = to_mucontext(context)->uuari.uars[0].index;
 
@@ -816,9 +823,10 @@ static void init_cq_buf(struct mlx5_ib_cq *cq, struct mlx5_ib_cq_buf *buf)
 
 static int create_cq_kernel(struct mlx5_ib_dev *dev, struct mlx5_ib_cq *cq,
 			    int entries, int cqe_size,
-			    struct mlx5_create_cq_mbox_in **cqb,
-			    int *index, int *inlen)
+			    u32 **cqb, int *index, int *inlen)
 {
+	__be64 *pas;
+	void *cqc;
 	int err;
 
 	err = mlx5_db_alloc(dev->mdev, &cq->db);
@@ -835,15 +843,21 @@ static int create_cq_kernel(struct mlx5_ib_dev *dev, struct mlx5_ib_cq *cq,
 
 	init_cq_buf(cq, &cq->buf);
 
-	*inlen = sizeof(**cqb) + sizeof(*(*cqb)->pas) * cq->buf.buf.npages;
+	*inlen = MLX5_ST_SZ_BYTES(create_cq_in) +
+		 MLX5_FLD_SZ_BYTES(create_cq_in, pas[0]) * cq->buf.buf.npages;
 	*cqb = mlx5_vzalloc(*inlen);
 	if (!*cqb) {
 		err = -ENOMEM;
 		goto err_buf;
 	}
-	mlx5_fill_page_array(&cq->buf.buf, (*cqb)->pas);
 
-	(*cqb)->ctx.log_pg_sz = cq->buf.buf.page_shift - MLX5_ADAPTER_PAGE_SHIFT;
+	pas = (__be64 *)MLX5_ADDR_OF(create_cq_in, *cqb, pas);
+	mlx5_fill_page_array(&cq->buf.buf, pas);
+
+	cqc = MLX5_ADDR_OF(create_cq_in, *cqb, cq_context);
+	MLX5_SET(cqc, cqc, log_page_size,
+		 cq->buf.buf.page_shift - MLX5_ADAPTER_PAGE_SHIFT);
+
 	*index = dev->mdev->priv.uuari.uars[0].index;
 
 	return 0;
@@ -877,11 +891,12 @@ struct ib_cq *mlx5_ib_create_cq(struct ib_device *ibdev,
 {
 	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;
 	int uninitialized_var(index);
 	int uninitialized_var(inlen);
+	u32 *cqb = NULL;
+	void *cqc;
 	int cqe_size;
 	unsigned int irqn;
 	int eqn;
@@ -927,19 +942,20 @@ struct ib_cq *mlx5_ib_create_cq(struct ib_device *ibdev,
 		INIT_WORK(&cq->notify_work, notify_soft_wc_handler);
 	}
 
-	cq->cqe_size = cqe_size;
-	cqb->ctx.cqe_sz_flags = cqe_sz_to_mlx_sz(cqe_size) << 5;
-
-	if (cq->create_flags & IB_CQ_FLAGS_IGNORE_OVERRUN)
-		cqb->ctx.cqe_sz_flags |= (1 << 1);
-
-	cqb->ctx.log_sz_usr_page = cpu_to_be32((ilog2(entries) << 24) | index);
 	err = mlx5_vector2eqn(dev->mdev, vector, &eqn, &irqn);
 	if (err)
 		goto err_cqb;
 
-	cqb->ctx.c_eqn = cpu_to_be16(eqn);
-	cqb->ctx.db_record_addr = cpu_to_be64(cq->db.dma);
+	cq->cqe_size = cqe_size;
+
+	cqc = MLX5_ADDR_OF(create_cq_in, cqb, cq_context);
+	MLX5_SET(cqc, cqc, cqe_sz, cqe_sz_to_mlx_sz(cqe_size));
+	MLX5_SET(cqc, cqc, log_cq_size, ilog2(entries));
+	MLX5_SET(cqc, cqc, uar_page, index);
+	MLX5_SET(cqc, cqc, c_eqn, eqn);
+	MLX5_SET64(cqc, cqc, dbr_addr, cq->db.dma);
+	if (cq->create_flags & IB_CQ_FLAGS_IGNORE_OVERRUN)
+		MLX5_SET(cqc, cqc, oi, 1);
 
 	err = mlx5_core_create_cq(dev->mdev, &cq->mcq, cqb, inlen);
 	if (err)
@@ -1070,27 +1086,15 @@ void mlx5_ib_cq_clean(struct mlx5_ib_cq *cq, u32 qpn, struct mlx5_ib_srq *srq)
 
 int mlx5_ib_modify_cq(struct ib_cq *cq, u16 cq_count, u16 cq_period)
 {
-	struct mlx5_modify_cq_mbox_in *in;
 	struct mlx5_ib_dev *dev = to_mdev(cq->device);
 	struct mlx5_ib_cq *mcq = to_mcq(cq);
 	int err;
-	u32 fsel;
 
 	if (!MLX5_CAP_GEN(dev->mdev, cq_moderation))
 		return -ENOSYS;
 
-	in = kzalloc(sizeof(*in), GFP_KERNEL);
-	if (!in)
-		return -ENOMEM;
-
-	in->cqn = cpu_to_be32(mcq->mcq.cqn);
-	fsel = (MLX5_CQ_MODIFY_PERIOD | MLX5_CQ_MODIFY_COUNT);
-	in->ctx.cq_period = cpu_to_be16(cq_period);
-	in->ctx.cq_max_count = cpu_to_be16(cq_count);
-	in->field_select = cpu_to_be32(fsel);
-	err = mlx5_core_modify_cq(dev->mdev, &mcq->mcq, in, sizeof(*in));
-	kfree(in);
-
+	err = mlx5_core_modify_cq_moderation(dev->mdev, &mcq->mcq,
+					     cq_period, cq_count);
 	if (err)
 		mlx5_ib_warn(dev, "modify cq 0x%x failed\n", mcq->mcq.cqn);
 
@@ -1223,9 +1227,11 @@ int mlx5_ib_resize_cq(struct ib_cq *ibcq, int entries, struct ib_udata *udata)
 {
 	struct mlx5_ib_dev *dev = to_mdev(ibcq->device);
 	struct mlx5_ib_cq *cq = to_mcq(ibcq);
-	struct mlx5_modify_cq_mbox_in *in;
+	void *cqc;
+	u32 *in;
 	int err;
 	int npas;
+	__be64 *pas;
 	int page_shift;
 	int inlen;
 	int uninitialized_var(cqe_size);
@@ -1267,28 +1273,37 @@ int mlx5_ib_resize_cq(struct ib_cq *ibcq, int entries, struct ib_udata *udata)
 	if (err)
 		goto ex;
 
-	inlen = sizeof(*in) + npas * sizeof(in->pas[0]);
+	inlen = MLX5_ST_SZ_BYTES(modify_cq_in) +
+		MLX5_FLD_SZ_BYTES(modify_cq_in, pas[0]) * npas;
+
 	in = mlx5_vzalloc(inlen);
 	if (!in) {
 		err = -ENOMEM;
 		goto ex_resize;
 	}
 
+	pas = (__be64 *)MLX5_ADDR_OF(modify_cq_in, in, pas);
 	if (udata)
 		mlx5_ib_populate_pas(dev, cq->resize_umem, page_shift,
-				     in->pas, 0);
+				     pas, 0);
 	else
-		mlx5_fill_page_array(&cq->resize_buf->buf, in->pas);
-
-	in->field_select = cpu_to_be32(MLX5_MODIFY_CQ_MASK_LOG_SIZE  |
-				       MLX5_MODIFY_CQ_MASK_PG_OFFSET |
-				       MLX5_MODIFY_CQ_MASK_PG_SIZE);
-	in->ctx.log_pg_sz = page_shift - MLX5_ADAPTER_PAGE_SHIFT;
-	in->ctx.cqe_sz_flags = cqe_sz_to_mlx_sz(cqe_size) << 5;
-	in->ctx.page_offset = 0;
-	in->ctx.log_sz_usr_page = cpu_to_be32(ilog2(entries) << 24);
-	in->hdr.opmod = cpu_to_be16(MLX5_CQ_OPMOD_RESIZE);
-	in->cqn = cpu_to_be32(cq->mcq.cqn);
+		mlx5_fill_page_array(&cq->resize_buf->buf, pas);
+
+	MLX5_SET(modify_cq_in, in,
+		 modify_field_select_resize_field_select.resize_field_select.resize_field_select,
+		 MLX5_MODIFY_CQ_MASK_LOG_SIZE  |
+		 MLX5_MODIFY_CQ_MASK_PG_OFFSET |
+		 MLX5_MODIFY_CQ_MASK_PG_SIZE);
+
+	cqc = MLX5_ADDR_OF(modify_cq_in, in, cq_context);
+
+	MLX5_SET(cqc, cqc, log_page_size,
+		 page_shift - MLX5_ADAPTER_PAGE_SHIFT);
+	MLX5_SET(cqc, cqc, cqe_sz, cqe_sz_to_mlx_sz(cqe_size));
+	MLX5_SET(cqc, cqc, log_cq_size, ilog2(entries));
+
+	MLX5_SET(modify_cq_in, in, op_mod, MLX5_CQ_OPMOD_RESIZE);
+	MLX5_SET(modify_cq_in, in, cqn, cq->mcq.cqn);
 
 	err = mlx5_core_modify_cq(dev->mdev, &cq->mcq, in, inlen);
 	if (err)
diff --git a/drivers/infiniband/hw/mlx5/mad.c b/drivers/infiniband/hw/mlx5/mad.c
index 364aab9f3c9e..39e58489dcc2 100644
--- a/drivers/infiniband/hw/mlx5/mad.c
+++ b/drivers/infiniband/hw/mlx5/mad.c
@@ -394,7 +394,7 @@ int mlx5_query_mad_ifc_node_desc(struct mlx5_ib_dev *dev, char *node_desc)
 	if (err)
 		goto out;
 
-	memcpy(node_desc, out_mad->data, 64);
+	memcpy(node_desc, out_mad->data, IB_DEVICE_NODE_DESC_MAX);
 out:
 	kfree(in_mad);
 	kfree(out_mad);
diff --git a/drivers/infiniband/hw/mlx5/main.c b/drivers/infiniband/hw/mlx5/main.c
index e19537cf44ab..22174774dbb8 100644
--- a/drivers/infiniband/hw/mlx5/main.c
+++ b/drivers/infiniband/hw/mlx5/main.c
@@ -53,7 +53,6 @@
 #include <linux/in.h>
 #include <linux/etherdevice.h>
 #include <linux/mlx5/fs.h>
-#include "user.h"
 #include "mlx5_ib.h"
 
 #define DRIVER_NAME "mlx5_ib"
@@ -106,13 +105,42 @@ static int mlx5_netdev_event(struct notifier_block *this,
 	struct mlx5_ib_dev *ibdev = container_of(this, struct mlx5_ib_dev,
 						 roce.nb);
 
-	if ((event != NETDEV_UNREGISTER) && (event != NETDEV_REGISTER))
-		return NOTIFY_DONE;
+	switch (event) {
+	case NETDEV_REGISTER:
+	case NETDEV_UNREGISTER:
+		write_lock(&ibdev->roce.netdev_lock);
+		if (ndev->dev.parent == &ibdev->mdev->pdev->dev)
+			ibdev->roce.netdev = (event == NETDEV_UNREGISTER) ?
+					     NULL : ndev;
+		write_unlock(&ibdev->roce.netdev_lock);
+		break;
+
+	case NETDEV_UP:
+	case NETDEV_DOWN: {
+		struct net_device *lag_ndev = mlx5_lag_get_roce_netdev(ibdev->mdev);
+		struct net_device *upper = NULL;
+
+		if (lag_ndev) {
+			upper = netdev_master_upper_dev_get(lag_ndev);
+			dev_put(lag_ndev);
+		}
+
+		if ((upper == ndev || (!upper && ndev == ibdev->roce.netdev))
+		    && ibdev->ib_active) {
+			struct ib_event ibev = {0};
+
+			ibev.device = &ibdev->ib_dev;
+			ibev.event = (event == NETDEV_UP) ?
+				     IB_EVENT_PORT_ACTIVE : IB_EVENT_PORT_ERR;
+			ibev.element.port_num = 1;
+			ib_dispatch_event(&ibev);
+		}
+		break;
+	}
 
-	write_lock(&ibdev->roce.netdev_lock);
-	if (ndev->dev.parent == &ibdev->mdev->pdev->dev)
-		ibdev->roce.netdev = (event == NETDEV_UNREGISTER) ? NULL : ndev;
-	write_unlock(&ibdev->roce.netdev_lock);
+	default:
+		break;
+	}
 
 	return NOTIFY_DONE;
 }
@@ -123,6 +151,10 @@ static struct net_device *mlx5_ib_get_netdev(struct ib_device *device,
 	struct mlx5_ib_dev *ibdev = to_mdev(device);
 	struct net_device *ndev;
 
+	ndev = mlx5_lag_get_roce_netdev(ibdev->mdev);
+	if (ndev)
+		return ndev;
+
 	/* Ensure ndev does not disappear before we invoke dev_hold()
 	 */
 	read_lock(&ibdev->roce.netdev_lock);
@@ -138,7 +170,7 @@ static int mlx5_query_port_roce(struct ib_device *device, u8 port_num,
 				struct ib_port_attr *props)
 {
 	struct mlx5_ib_dev *dev = to_mdev(device);
-	struct net_device *ndev;
+	struct net_device *ndev, *upper;
 	enum ib_mtu ndev_ib_mtu;
 	u16 qkey_viol_cntr;
 
@@ -162,6 +194,17 @@ static int mlx5_query_port_roce(struct ib_device *device, u8 port_num,
 	if (!ndev)
 		return 0;
 
+	if (mlx5_lag_is_active(dev->mdev)) {
+		rcu_read_lock();
+		upper = netdev_master_upper_dev_get_rcu(ndev);
+		if (upper) {
+			dev_put(ndev);
+			ndev = upper;
+			dev_hold(ndev);
+		}
+		rcu_read_unlock();
+	}
+
 	if (netif_running(ndev) && netif_carrier_ok(ndev)) {
 		props->state      = IB_PORT_ACTIVE;
 		props->phys_state = 5;
@@ -232,23 +275,19 @@ static int set_roce_addr(struct ib_device *device, u8 port_num,
 			 const union ib_gid *gid,
 			 const struct ib_gid_attr *attr)
 {
-	struct mlx5_ib_dev *dev	= to_mdev(device);
-	u32  in[MLX5_ST_SZ_DW(set_roce_address_in)];
-	u32 out[MLX5_ST_SZ_DW(set_roce_address_out)];
+	struct mlx5_ib_dev *dev = to_mdev(device);
+	u32  in[MLX5_ST_SZ_DW(set_roce_address_in)]  = {0};
+	u32 out[MLX5_ST_SZ_DW(set_roce_address_out)] = {0};
 	void *in_addr = MLX5_ADDR_OF(set_roce_address_in, in, roce_address);
 	enum rdma_link_layer ll = mlx5_ib_port_link_layer(device, port_num);
 
 	if (ll != IB_LINK_LAYER_ETHERNET)
 		return -EINVAL;
 
-	memset(in, 0, sizeof(in));
-
 	ib_gid_to_mlx5_roce_addr(gid, attr, in_addr);
 
 	MLX5_SET(set_roce_address_in, in, roce_address_index, index);
 	MLX5_SET(set_roce_address_in, in, opcode, MLX5_CMD_OP_SET_ROCE_ADDRESS);
-
-	memset(out, 0, sizeof(out));
 	return mlx5_cmd_exec(dev->mdev, in, sizeof(in), out, sizeof(out));
 }
 
@@ -433,7 +472,7 @@ static int mlx5_query_node_guid(struct mlx5_ib_dev *dev,
 }
 
 struct mlx5_reg_node_desc {
-	u8	desc[64];
+	u8	desc[IB_DEVICE_NODE_DESC_MAX];
 };
 
 static int mlx5_query_node_desc(struct mlx5_ib_dev *dev, char *node_desc)
@@ -536,6 +575,26 @@ static int mlx5_ib_query_device(struct ib_device *ibdev,
 				resp.response_length += sizeof(resp.tso_caps);
 			}
 		}
+
+		if (field_avail(typeof(resp), rss_caps, uhw->outlen)) {
+			resp.rss_caps.rx_hash_function =
+						MLX5_RX_HASH_FUNC_TOEPLITZ;
+			resp.rss_caps.rx_hash_fields_mask =
+						MLX5_RX_HASH_SRC_IPV4 |
+						MLX5_RX_HASH_DST_IPV4 |
+						MLX5_RX_HASH_SRC_IPV6 |
+						MLX5_RX_HASH_DST_IPV6 |
+						MLX5_RX_HASH_SRC_PORT_TCP |
+						MLX5_RX_HASH_DST_PORT_TCP |
+						MLX5_RX_HASH_SRC_PORT_UDP |
+						MLX5_RX_HASH_DST_PORT_UDP;
+			resp.response_length += sizeof(resp.rss_caps);
+		}
+	} else {
+		if (field_avail(typeof(resp), tso_caps, uhw->outlen))
+			resp.response_length += sizeof(resp.tso_caps);
+		if (field_avail(typeof(resp), rss_caps, uhw->outlen))
+			resp.response_length += sizeof(resp.rss_caps);
 	}
 
 	if (MLX5_CAP_GEN(mdev, ipoib_basic_offloads)) {
@@ -599,6 +658,17 @@ static int mlx5_ib_query_device(struct ib_device *ibdev,
 	if (!mlx5_core_is_pf(mdev))
 		props->device_cap_flags |= IB_DEVICE_VIRTUAL_FUNCTION;
 
+	if (mlx5_ib_port_link_layer(ibdev, 1) ==
+	    IB_LINK_LAYER_ETHERNET) {
+		props->rss_caps.max_rwq_indirection_tables =
+			1 << MLX5_CAP_GEN(dev->mdev, log_max_rqt);
+		props->rss_caps.max_rwq_indirection_table_size =
+			1 << MLX5_CAP_GEN(dev->mdev, log_max_rqt_size);
+		props->rss_caps.supported_qpts = 1 << IB_QPT_RAW_PACKET;
+		props->max_wq_type_rq =
+			1 << MLX5_CAP_GEN(dev->mdev, log_max_rq);
+	}
+
 	if (uhw->outlen) {
 		err = ib_copy_to_udata(uhw, &resp, resp.response_length);
 
@@ -753,8 +823,7 @@ static int mlx5_query_hca_port(struct ib_device *ibdev, u8 port,
 				     &props->active_width);
 	if (err)
 		goto out;
-	err = mlx5_query_port_proto_oper(mdev, &props->active_speed, MLX5_PTYS_IB,
-					 port);
+	err = mlx5_query_port_ib_proto_oper(mdev, &props->active_speed, port);
 	if (err)
 		goto out;
 
@@ -851,13 +920,13 @@ static int mlx5_ib_modify_device(struct ib_device *ibdev, int mask,
 	 * If possible, pass node desc to FW, so it can generate
 	 * a 144 trap.  If cmd fails, just ignore.
 	 */
-	memcpy(&in, props->node_desc, 64);
+	memcpy(&in, props->node_desc, IB_DEVICE_NODE_DESC_MAX);
 	err = mlx5_core_access_reg(dev->mdev, &in, sizeof(in), &out,
 				   sizeof(out), MLX5_REG_NODE_DESC, 0, 1);
 	if (err)
 		return err;
 
-	memcpy(ibdev->node_desc, props->node_desc, 64);
+	memcpy(ibdev->node_desc, props->node_desc, IB_DEVICE_NODE_DESC_MAX);
 
 	return err;
 }
@@ -1400,28 +1469,77 @@ static int mlx5_ib_dealloc_pd(struct ib_pd *pd)
 	return 0;
 }
 
-static bool outer_header_zero(u32 *match_criteria)
+enum {
+	MATCH_CRITERIA_ENABLE_OUTER_BIT,
+	MATCH_CRITERIA_ENABLE_MISC_BIT,
+	MATCH_CRITERIA_ENABLE_INNER_BIT
+};
+
+#define HEADER_IS_ZERO(match_criteria, headers)			           \
+	!(memchr_inv(MLX5_ADDR_OF(fte_match_param, match_criteria, headers), \
+		    0, MLX5_FLD_SZ_BYTES(fte_match_param, headers)))       \
+
+static u8 get_match_criteria_enable(u32 *match_criteria)
+{
+	u8 match_criteria_enable;
+
+	match_criteria_enable =
+		(!HEADER_IS_ZERO(match_criteria, outer_headers)) <<
+		MATCH_CRITERIA_ENABLE_OUTER_BIT;
+	match_criteria_enable |=
+		(!HEADER_IS_ZERO(match_criteria, misc_parameters)) <<
+		MATCH_CRITERIA_ENABLE_MISC_BIT;
+	match_criteria_enable |=
+		(!HEADER_IS_ZERO(match_criteria, inner_headers)) <<
+		MATCH_CRITERIA_ENABLE_INNER_BIT;
+
+	return match_criteria_enable;
+}
+
+static void set_proto(void *outer_c, void *outer_v, u8 mask, u8 val)
 {
-	int size = MLX5_ST_SZ_BYTES(fte_match_param);
-	char *outer_headers_c = MLX5_ADDR_OF(fte_match_param, match_criteria,
-					     outer_headers);
+	MLX5_SET(fte_match_set_lyr_2_4, outer_c, ip_protocol, mask);
+	MLX5_SET(fte_match_set_lyr_2_4, outer_v, ip_protocol, val);
+}
 
-	return outer_headers_c[0] == 0 && !memcmp(outer_headers_c,
-						  outer_headers_c + 1,
-						  size - 1);
+static void set_tos(void *outer_c, void *outer_v, u8 mask, u8 val)
+{
+	MLX5_SET(fte_match_set_lyr_2_4, outer_c, ip_ecn, mask);
+	MLX5_SET(fte_match_set_lyr_2_4, outer_v, ip_ecn, val);
+	MLX5_SET(fte_match_set_lyr_2_4, outer_c, ip_dscp, mask >> 2);
+	MLX5_SET(fte_match_set_lyr_2_4, outer_v, ip_dscp, val >> 2);
 }
 
+#define LAST_ETH_FIELD vlan_tag
+#define LAST_IB_FIELD sl
+#define LAST_IPV4_FIELD tos
+#define LAST_IPV6_FIELD traffic_class
+#define LAST_TCP_UDP_FIELD src_port
+
+/* Field is the last supported field */
+#define FIELDS_NOT_SUPPORTED(filter, field)\
+	memchr_inv((void *)&filter.field  +\
+		   sizeof(filter.field), 0,\
+		   sizeof(filter) -\
+		   offsetof(typeof(filter), field) -\
+		   sizeof(filter.field))
+
 static int parse_flow_attr(u32 *match_c, u32 *match_v,
-			   union ib_flow_spec *ib_spec)
+			   const union ib_flow_spec *ib_spec)
 {
 	void *outer_headers_c = MLX5_ADDR_OF(fte_match_param, match_c,
 					     outer_headers);
 	void *outer_headers_v = MLX5_ADDR_OF(fte_match_param, match_v,
 					     outer_headers);
+	void *misc_params_c = MLX5_ADDR_OF(fte_match_param, match_c,
+					   misc_parameters);
+	void *misc_params_v = MLX5_ADDR_OF(fte_match_param, match_v,
+					   misc_parameters);
+
 	switch (ib_spec->type) {
 	case IB_FLOW_SPEC_ETH:
-		if (ib_spec->size != sizeof(ib_spec->eth))
-			return -EINVAL;
+		if (FIELDS_NOT_SUPPORTED(ib_spec->eth.mask, LAST_ETH_FIELD))
+			return -ENOTSUPP;
 
 		ether_addr_copy(MLX5_ADDR_OF(fte_match_set_lyr_2_4, outer_headers_c,
 					     dmac_47_16),
@@ -1468,8 +1586,8 @@ static int parse_flow_attr(u32 *match_c, u32 *match_v,
 			 ethertype, ntohs(ib_spec->eth.val.ether_type));
 		break;
 	case IB_FLOW_SPEC_IPV4:
-		if (ib_spec->size != sizeof(ib_spec->ipv4))
-			return -EINVAL;
+		if (FIELDS_NOT_SUPPORTED(ib_spec->ipv4.mask, LAST_IPV4_FIELD))
+			return -ENOTSUPP;
 
 		MLX5_SET(fte_match_set_lyr_2_4, outer_headers_c,
 			 ethertype, 0xffff);
@@ -1492,10 +1610,16 @@ static int parse_flow_attr(u32 *match_c, u32 *match_v,
 				    dst_ipv4_dst_ipv6.ipv4_layout.ipv4),
 		       &ib_spec->ipv4.val.dst_ip,
 		       sizeof(ib_spec->ipv4.val.dst_ip));
+
+		set_tos(outer_headers_c, outer_headers_v,
+			ib_spec->ipv4.mask.tos, ib_spec->ipv4.val.tos);
+
+		set_proto(outer_headers_c, outer_headers_v,
+			  ib_spec->ipv4.mask.proto, ib_spec->ipv4.val.proto);
 		break;
 	case IB_FLOW_SPEC_IPV6:
-		if (ib_spec->size != sizeof(ib_spec->ipv6))
-			return -EINVAL;
+		if (FIELDS_NOT_SUPPORTED(ib_spec->ipv6.mask, LAST_IPV6_FIELD))
+			return -ENOTSUPP;
 
 		MLX5_SET(fte_match_set_lyr_2_4, outer_headers_c,
 			 ethertype, 0xffff);
@@ -1518,10 +1642,26 @@ static int parse_flow_attr(u32 *match_c, u32 *match_v,
 				    dst_ipv4_dst_ipv6.ipv6_layout.ipv6),
 		       &ib_spec->ipv6.val.dst_ip,
 		       sizeof(ib_spec->ipv6.val.dst_ip));
+
+		set_tos(outer_headers_c, outer_headers_v,
+			ib_spec->ipv6.mask.traffic_class,
+			ib_spec->ipv6.val.traffic_class);
+
+		set_proto(outer_headers_c, outer_headers_v,
+			  ib_spec->ipv6.mask.next_hdr,
+			  ib_spec->ipv6.val.next_hdr);
+
+		MLX5_SET(fte_match_set_misc, misc_params_c,
+			 outer_ipv6_flow_label,
+			 ntohl(ib_spec->ipv6.mask.flow_label));
+		MLX5_SET(fte_match_set_misc, misc_params_v,
+			 outer_ipv6_flow_label,
+			 ntohl(ib_spec->ipv6.val.flow_label));
 		break;
 	case IB_FLOW_SPEC_TCP:
-		if (ib_spec->size != sizeof(ib_spec->tcp_udp))
-			return -EINVAL;
+		if (FIELDS_NOT_SUPPORTED(ib_spec->tcp_udp.mask,
+					 LAST_TCP_UDP_FIELD))
+			return -ENOTSUPP;
 
 		MLX5_SET(fte_match_set_lyr_2_4, outer_headers_c, ip_protocol,
 			 0xff);
@@ -1539,8 +1679,9 @@ static int parse_flow_attr(u32 *match_c, u32 *match_v,
 			 ntohs(ib_spec->tcp_udp.val.dst_port));
 		break;
 	case IB_FLOW_SPEC_UDP:
-		if (ib_spec->size != sizeof(ib_spec->tcp_udp))
-			return -EINVAL;
+		if (FIELDS_NOT_SUPPORTED(ib_spec->tcp_udp.mask,
+					 LAST_TCP_UDP_FIELD))
+			return -ENOTSUPP;
 
 		MLX5_SET(fte_match_set_lyr_2_4, outer_headers_c, ip_protocol,
 			 0xff);
@@ -1587,7 +1728,7 @@ static bool flow_is_multicast_only(struct ib_flow_attr *ib_attr)
 	       is_multicast_ether_addr(eth_spec->val.dst_mac);
 }
 
-static bool is_valid_attr(struct ib_flow_attr *flow_attr)
+static bool is_valid_attr(const struct ib_flow_attr *flow_attr)
 {
 	union ib_flow_spec *ib_spec = (union ib_flow_spec *)(flow_attr + 1);
 	bool has_ipv4_spec = false;
@@ -1631,12 +1772,13 @@ static int mlx5_ib_destroy_flow(struct ib_flow *flow_id)
 
 	list_for_each_entry_safe(iter, tmp, &handler->list, list) {
 		mlx5_del_flow_rule(iter->rule);
+		put_flow_table(dev, iter->prio, true);
 		list_del(&iter->list);
 		kfree(iter);
 	}
 
 	mlx5_del_flow_rule(handler->rule);
-	put_flow_table(dev, &dev->flow_db.prios[handler->prio], true);
+	put_flow_table(dev, handler->prio, true);
 	mutex_unlock(&dev->flow_db.lock);
 
 	kfree(handler);
@@ -1652,10 +1794,16 @@ static int ib_prio_to_core_prio(unsigned int priority, bool dont_trap)
 	return priority;
 }
 
+enum flow_table_type {
+	MLX5_IB_FT_RX,
+	MLX5_IB_FT_TX
+};
+
 #define MLX5_FS_MAX_TYPES	 10
 #define MLX5_FS_MAX_ENTRIES	 32000UL
 static struct mlx5_ib_flow_prio *get_flow_table(struct mlx5_ib_dev *dev,
-						struct ib_flow_attr *flow_attr)
+						struct ib_flow_attr *flow_attr,
+						enum flow_table_type ft_type)
 {
 	bool dont_trap = flow_attr->flags & IB_FLOW_ATTR_FLAGS_DONT_TRAP;
 	struct mlx5_flow_namespace *ns = NULL;
@@ -1686,6 +1834,19 @@ static struct mlx5_ib_flow_prio *get_flow_table(struct mlx5_ib_dev *dev,
 					 &num_entries,
 					 &num_groups);
 		prio = &dev->flow_db.prios[MLX5_IB_FLOW_LEFTOVERS_PRIO];
+	} else if (flow_attr->type == IB_FLOW_ATTR_SNIFFER) {
+		if (!MLX5_CAP_FLOWTABLE(dev->mdev,
+					allow_sniffer_and_nic_rx_shared_tir))
+			return ERR_PTR(-ENOTSUPP);
+
+		ns = mlx5_get_flow_namespace(dev->mdev, ft_type == MLX5_IB_FT_RX ?
+					     MLX5_FLOW_NAMESPACE_SNIFFER_RX :
+					     MLX5_FLOW_NAMESPACE_SNIFFER_TX);
+
+		prio = &dev->flow_db.sniffer[ft_type];
+		priority = 0;
+		num_entries = 1;
+		num_groups = 1;
 	}
 
 	if (!ns)
@@ -1711,13 +1872,13 @@ static struct mlx5_ib_flow_prio *get_flow_table(struct mlx5_ib_dev *dev,
 
 static struct mlx5_ib_flow_handler *create_flow_rule(struct mlx5_ib_dev *dev,
 						     struct mlx5_ib_flow_prio *ft_prio,
-						     struct ib_flow_attr *flow_attr,
+						     const struct ib_flow_attr *flow_attr,
 						     struct mlx5_flow_destination *dst)
 {
 	struct mlx5_flow_table	*ft = ft_prio->flow_table;
 	struct mlx5_ib_flow_handler *handler;
 	struct mlx5_flow_spec *spec;
-	void *ib_flow = flow_attr + 1;
+	const void *ib_flow = (const void *)flow_attr + sizeof(*flow_attr);
 	unsigned int spec_index;
 	u32 action;
 	int err = 0;
@@ -1743,9 +1904,7 @@ static struct mlx5_ib_flow_handler *create_flow_rule(struct mlx5_ib_dev *dev,
 		ib_flow += ((union ib_flow_spec *)ib_flow)->size;
 	}
 
-	/* Outer header support only */
-	spec->match_criteria_enable = (!outer_header_zero(spec->match_criteria))
-		<< 0;
+	spec->match_criteria_enable = get_match_criteria_enable(spec->match_criteria);
 	action = dst ? MLX5_FLOW_CONTEXT_ACTION_FWD_DEST :
 		MLX5_FLOW_CONTEXT_ACTION_FWD_NEXT_PRIO;
 	handler->rule = mlx5_add_flow_rule(ft, spec,
@@ -1758,7 +1917,8 @@ static struct mlx5_ib_flow_handler *create_flow_rule(struct mlx5_ib_dev *dev,
 		goto free;
 	}
 
-	handler->prio = ft_prio - dev->flow_db.prios;
+	ft_prio->refcount++;
+	handler->prio = ft_prio;
 
 	ft_prio->flow_table = ft;
 free:
@@ -1782,6 +1942,7 @@ static struct mlx5_ib_flow_handler *create_dont_trap_rule(struct mlx5_ib_dev *de
 					       flow_attr, dst);
 		if (IS_ERR(handler_dst)) {
 			mlx5_del_flow_rule(handler->rule);
+			ft_prio->refcount--;
 			kfree(handler);
 			handler = handler_dst;
 		} else {
@@ -1843,6 +2004,8 @@ static struct mlx5_ib_flow_handler *create_leftovers_rule(struct mlx5_ib_dev *de
 						 &leftovers_specs[LEFTOVERS_UC].flow_attr,
 						 dst);
 		if (IS_ERR(handler_ucast)) {
+			mlx5_del_flow_rule(handler->rule);
+			ft_prio->refcount--;
 			kfree(handler);
 			handler = handler_ucast;
 		} else {
@@ -1853,6 +2016,43 @@ static struct mlx5_ib_flow_handler *create_leftovers_rule(struct mlx5_ib_dev *de
 	return handler;
 }
 
+static struct mlx5_ib_flow_handler *create_sniffer_rule(struct mlx5_ib_dev *dev,
+							struct mlx5_ib_flow_prio *ft_rx,
+							struct mlx5_ib_flow_prio *ft_tx,
+							struct mlx5_flow_destination *dst)
+{
+	struct mlx5_ib_flow_handler *handler_rx;
+	struct mlx5_ib_flow_handler *handler_tx;
+	int err;
+	static const struct ib_flow_attr flow_attr  = {
+		.num_of_specs = 0,
+		.size = sizeof(flow_attr)
+	};
+
+	handler_rx = create_flow_rule(dev, ft_rx, &flow_attr, dst);
+	if (IS_ERR(handler_rx)) {
+		err = PTR_ERR(handler_rx);
+		goto err;
+	}
+
+	handler_tx = create_flow_rule(dev, ft_tx, &flow_attr, dst);
+	if (IS_ERR(handler_tx)) {
+		err = PTR_ERR(handler_tx);
+		goto err_tx;
+	}
+
+	list_add(&handler_tx->list, &handler_rx->list);
+
+	return handler_rx;
+
+err_tx:
+	mlx5_del_flow_rule(handler_rx->rule);
+	ft_rx->refcount--;
+	kfree(handler_rx);
+err:
+	return ERR_PTR(err);
+}
+
 static struct ib_flow *mlx5_ib_create_flow(struct ib_qp *qp,
 					   struct ib_flow_attr *flow_attr,
 					   int domain)
@@ -1861,6 +2061,7 @@ static struct ib_flow *mlx5_ib_create_flow(struct ib_qp *qp,
 	struct mlx5_ib_qp *mqp = to_mqp(qp);
 	struct mlx5_ib_flow_handler *handler = NULL;
 	struct mlx5_flow_destination *dst = NULL;
+	struct mlx5_ib_flow_prio *ft_prio_tx = NULL;
 	struct mlx5_ib_flow_prio *ft_prio;
 	int err;
 
@@ -1878,11 +2079,19 @@ static struct ib_flow *mlx5_ib_create_flow(struct ib_qp *qp,
 
 	mutex_lock(&dev->flow_db.lock);
 
-	ft_prio = get_flow_table(dev, flow_attr);
+	ft_prio = get_flow_table(dev, flow_attr, MLX5_IB_FT_RX);
 	if (IS_ERR(ft_prio)) {
 		err = PTR_ERR(ft_prio);
 		goto unlock;
 	}
+	if (flow_attr->type == IB_FLOW_ATTR_SNIFFER) {
+		ft_prio_tx = get_flow_table(dev, flow_attr, MLX5_IB_FT_TX);
+		if (IS_ERR(ft_prio_tx)) {
+			err = PTR_ERR(ft_prio_tx);
+			ft_prio_tx = NULL;
+			goto destroy_ft;
+		}
+	}
 
 	dst->type = MLX5_FLOW_DESTINATION_TYPE_TIR;
 	if (mqp->flags & MLX5_IB_QP_RSS)
@@ -1902,6 +2111,8 @@ static struct ib_flow *mlx5_ib_create_flow(struct ib_qp *qp,
 		   flow_attr->type == IB_FLOW_ATTR_MC_DEFAULT) {
 		handler = create_leftovers_rule(dev, ft_prio, flow_attr,
 						dst);
+	} else if (flow_attr->type == IB_FLOW_ATTR_SNIFFER) {
+		handler = create_sniffer_rule(dev, ft_prio, ft_prio_tx, dst);
 	} else {
 		err = -EINVAL;
 		goto destroy_ft;
@@ -1913,7 +2124,6 @@ static struct ib_flow *mlx5_ib_create_flow(struct ib_qp *qp,
 		goto destroy_ft;
 	}
 
-	ft_prio->refcount++;
 	mutex_unlock(&dev->flow_db.lock);
 	kfree(dst);
 
@@ -1921,6 +2131,8 @@ static struct ib_flow *mlx5_ib_create_flow(struct ib_qp *qp,
 
 destroy_ft:
 	put_flow_table(dev, ft_prio, false);
+	if (ft_prio_tx)
+		put_flow_table(dev, ft_prio_tx, false);
 unlock:
 	mutex_unlock(&dev->flow_db.lock);
 	kfree(dst);
@@ -2110,14 +2322,19 @@ static void mlx5_ib_event(struct mlx5_core_dev *dev, void *context,
 		break;
 
 	case MLX5_DEV_EVENT_PORT_UP:
-		ibev.event = IB_EVENT_PORT_ACTIVE;
-		port = (u8)param;
-		break;
-
 	case MLX5_DEV_EVENT_PORT_DOWN:
 	case MLX5_DEV_EVENT_PORT_INITIALIZED:
-		ibev.event = IB_EVENT_PORT_ERR;
 		port = (u8)param;
+
+		/* In RoCE, port up/down events are handled in
+		 * mlx5_netdev_event().
+		 */
+		if (mlx5_ib_port_link_layer(&ibdev->ib_dev, port) ==
+			IB_LINK_LAYER_ETHERNET)
+			return;
+
+		ibev.event = (event == MLX5_DEV_EVENT_PORT_UP) ?
+			     IB_EVENT_PORT_ACTIVE : IB_EVENT_PORT_ERR;
 		break;
 
 	case MLX5_DEV_EVENT_LID_CHANGE:
@@ -2240,7 +2457,7 @@ static int create_umr_res(struct mlx5_ib_dev *dev)
 		goto error_0;
 	}
 
-	pd = ib_alloc_pd(&dev->ib_dev);
+	pd = ib_alloc_pd(&dev->ib_dev, 0);
 	if (IS_ERR(pd)) {
 		mlx5_ib_dbg(dev, "Couldn't create PD for sync UMR QP\n");
 		ret = PTR_ERR(pd);
@@ -2522,30 +2739,88 @@ static void get_dev_fw_str(struct ib_device *ibdev, char *str,
 		       fw_rev_min(dev->mdev), fw_rev_sub(dev->mdev));
 }
 
+static int mlx5_roce_lag_init(struct mlx5_ib_dev *dev)
+{
+	struct mlx5_core_dev *mdev = dev->mdev;
+	struct mlx5_flow_namespace *ns = mlx5_get_flow_namespace(mdev,
+								 MLX5_FLOW_NAMESPACE_LAG);
+	struct mlx5_flow_table *ft;
+	int err;
+
+	if (!ns || !mlx5_lag_is_active(mdev))
+		return 0;
+
+	err = mlx5_cmd_create_vport_lag(mdev);
+	if (err)
+		return err;
+
+	ft = mlx5_create_lag_demux_flow_table(ns, 0, 0);
+	if (IS_ERR(ft)) {
+		err = PTR_ERR(ft);
+		goto err_destroy_vport_lag;
+	}
+
+	dev->flow_db.lag_demux_ft = ft;
+	return 0;
+
+err_destroy_vport_lag:
+	mlx5_cmd_destroy_vport_lag(mdev);
+	return err;
+}
+
+static void mlx5_roce_lag_cleanup(struct mlx5_ib_dev *dev)
+{
+	struct mlx5_core_dev *mdev = dev->mdev;
+
+	if (dev->flow_db.lag_demux_ft) {
+		mlx5_destroy_flow_table(dev->flow_db.lag_demux_ft);
+		dev->flow_db.lag_demux_ft = NULL;
+
+		mlx5_cmd_destroy_vport_lag(mdev);
+	}
+}
+
+static void mlx5_remove_roce_notifier(struct mlx5_ib_dev *dev)
+{
+	if (dev->roce.nb.notifier_call) {
+		unregister_netdevice_notifier(&dev->roce.nb);
+		dev->roce.nb.notifier_call = NULL;
+	}
+}
+
 static int mlx5_enable_roce(struct mlx5_ib_dev *dev)
 {
 	int err;
 
 	dev->roce.nb.notifier_call = mlx5_netdev_event;
 	err = register_netdevice_notifier(&dev->roce.nb);
-	if (err)
+	if (err) {
+		dev->roce.nb.notifier_call = NULL;
 		return err;
+	}
 
 	err = mlx5_nic_vport_enable_roce(dev->mdev);
 	if (err)
 		goto err_unregister_netdevice_notifier;
 
+	err = mlx5_roce_lag_init(dev);
+	if (err)
+		goto err_disable_roce;
+
 	return 0;
 
+err_disable_roce:
+	mlx5_nic_vport_disable_roce(dev->mdev);
+
 err_unregister_netdevice_notifier:
-	unregister_netdevice_notifier(&dev->roce.nb);
+	mlx5_remove_roce_notifier(dev);
 	return err;
 }
 
 static void mlx5_disable_roce(struct mlx5_ib_dev *dev)
 {
+	mlx5_roce_lag_cleanup(dev);
 	mlx5_nic_vport_disable_roce(dev->mdev);
-	unregister_netdevice_notifier(&dev->roce.nb);
 }
 
 static void mlx5_ib_dealloc_q_counters(struct mlx5_ib_dev *dev)
@@ -2660,6 +2935,7 @@ static void *mlx5_ib_add(struct mlx5_core_dev *mdev)
 	struct mlx5_ib_dev *dev;
 	enum rdma_link_layer ll;
 	int port_type_cap;
+	const char *name;
 	int err;
 	int i;
 
@@ -2692,7 +2968,12 @@ static void *mlx5_ib_add(struct mlx5_core_dev *mdev)
 
 	MLX5_INIT_DOORBELL_LOCK(&dev->uar_lock);
 
-	strlcpy(dev->ib_dev.name, "mlx5_%d", IB_DEVICE_NAME_MAX);
+	if (!mlx5_lag_is_active(mdev))
+		name = "mlx5_%d";
+	else
+		name = "mlx5_bond_%d";
+
+	strlcpy(dev->ib_dev.name, name, 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	= 0 /* not supported for now */;
@@ -2894,8 +3175,10 @@ err_rsrc:
 	destroy_dev_resources(&dev->devr);
 
 err_disable_roce:
-	if (ll == IB_LINK_LAYER_ETHERNET)
+	if (ll == IB_LINK_LAYER_ETHERNET) {
 		mlx5_disable_roce(dev);
+		mlx5_remove_roce_notifier(dev);
+	}
 
 err_free_port:
 	kfree(dev->port);
@@ -2911,6 +3194,7 @@ static void mlx5_ib_remove(struct mlx5_core_dev *mdev, void *context)
 	struct mlx5_ib_dev *dev = context;
 	enum rdma_link_layer ll = mlx5_ib_port_link_layer(&dev->ib_dev, 1);
 
+	mlx5_remove_roce_notifier(dev);
 	ib_unregister_device(&dev->ib_dev);
 	mlx5_ib_dealloc_q_counters(dev);
 	destroy_umrc_res(dev);
diff --git a/drivers/infiniband/hw/mlx5/mlx5_ib.h b/drivers/infiniband/hw/mlx5/mlx5_ib.h
index 95146f4aa3e3..dcdcd195fe53 100644
--- a/drivers/infiniband/hw/mlx5/mlx5_ib.h
+++ b/drivers/infiniband/hw/mlx5/mlx5_ib.h
@@ -44,6 +44,7 @@
 #include <linux/types.h>
 #include <linux/mlx5/transobj.h>
 #include <rdma/ib_user_verbs.h>
+#include <rdma/mlx5-abi.h>
 
 #define mlx5_ib_dbg(dev, format, arg...)				\
 pr_debug("%s:%s:%d:(pid %d): " format, (dev)->ib_dev.name, __func__,	\
@@ -142,6 +143,7 @@ struct mlx5_ib_pd {
 #define MLX5_IB_FLOW_LEFTOVERS_PRIO	(MLX5_IB_FLOW_MCAST_PRIO + 1)
 
 #define MLX5_IB_NUM_FLOW_FT		(MLX5_IB_FLOW_LEFTOVERS_PRIO + 1)
+#define MLX5_IB_NUM_SNIFFER_FTS		2
 struct mlx5_ib_flow_prio {
 	struct mlx5_flow_table		*flow_table;
 	unsigned int			refcount;
@@ -150,12 +152,14 @@ struct mlx5_ib_flow_prio {
 struct mlx5_ib_flow_handler {
 	struct list_head		list;
 	struct ib_flow			ibflow;
-	unsigned int			prio;
+	struct mlx5_ib_flow_prio	*prio;
 	struct mlx5_flow_rule	*rule;
 };
 
 struct mlx5_ib_flow_db {
 	struct mlx5_ib_flow_prio	prios[MLX5_IB_NUM_FLOW_FT];
+	struct mlx5_ib_flow_prio	sniffer[MLX5_IB_NUM_SNIFFER_FTS];
+	struct mlx5_flow_table		*lag_demux_ft;
 	/* Protect flow steering bypass flow tables
 	 * when add/del flow rules.
 	 * only single add/removal of flow steering rule could be done
@@ -225,7 +229,7 @@ struct mlx5_ib_wq {
 
 struct mlx5_ib_rwq {
 	struct ib_wq		ibwq;
-	u32			rqn;
+	struct mlx5_core_qp	core_qp;
 	u32			rq_num_pas;
 	u32			log_rq_stride;
 	u32			log_rq_size;
@@ -505,7 +509,7 @@ struct mlx5_ib_mr {
 	int			umred;
 	int			npages;
 	struct mlx5_ib_dev     *dev;
-	struct mlx5_create_mkey_mbox_out out;
+	u32 out[MLX5_ST_SZ_DW(create_mkey_out)];
 	struct mlx5_core_sig_ctx    *sig;
 	int			live;
 	void			*descs_alloc;
@@ -603,6 +607,7 @@ struct mlx5_roce {
 	rwlock_t		netdev_lock;
 	struct net_device	*netdev;
 	struct notifier_block	nb;
+	atomic_t		next_port;
 };
 
 struct mlx5_ib_dev {
@@ -663,6 +668,11 @@ static inline struct mlx5_ib_qp *to_mibqp(struct mlx5_core_qp *mqp)
 	return container_of(mqp, struct mlx5_ib_qp_base, mqp)->container_mibqp;
 }
 
+static inline struct mlx5_ib_rwq *to_mibrwq(struct mlx5_core_qp *core_qp)
+{
+	return container_of(core_qp, struct mlx5_ib_rwq, core_qp);
+}
+
 static inline struct mlx5_ib_mr *to_mibmr(struct mlx5_core_mkey *mmkey)
 {
 	return container_of(mmkey, struct mlx5_ib_mr, mmkey);
@@ -947,4 +957,40 @@ static inline int verify_assign_uidx(u8 cqe_version, u32 cmd_uidx,
 
 	return 0;
 }
+
+static inline int get_qp_user_index(struct mlx5_ib_ucontext *ucontext,
+				    struct mlx5_ib_create_qp *ucmd,
+				    int inlen,
+				    u32 *user_index)
+{
+	u8 cqe_version = ucontext->cqe_version;
+
+	if (field_avail(struct mlx5_ib_create_qp, uidx, inlen) &&
+	    !cqe_version && (ucmd->uidx == MLX5_IB_DEFAULT_UIDX))
+		return 0;
+
+	if (!!(field_avail(struct mlx5_ib_create_qp, uidx, inlen) !=
+	       !!cqe_version))
+		return -EINVAL;
+
+	return verify_assign_uidx(cqe_version, ucmd->uidx, user_index);
+}
+
+static inline int get_srq_user_index(struct mlx5_ib_ucontext *ucontext,
+				     struct mlx5_ib_create_srq *ucmd,
+				     int inlen,
+				     u32 *user_index)
+{
+	u8 cqe_version = ucontext->cqe_version;
+
+	if (field_avail(struct mlx5_ib_create_srq, uidx, inlen) &&
+	    !cqe_version && (ucmd->uidx == MLX5_IB_DEFAULT_UIDX))
+		return 0;
+
+	if (!!(field_avail(struct mlx5_ib_create_srq, uidx, inlen) !=
+	       !!cqe_version))
+		return -EINVAL;
+
+	return verify_assign_uidx(cqe_version, ucmd->uidx, user_index);
+}
 #endif /* MLX5_IB_H */
diff --git a/drivers/infiniband/hw/mlx5/mr.c b/drivers/infiniband/hw/mlx5/mr.c
index 4b021305c321..d4ad672b905b 100644
--- a/drivers/infiniband/hw/mlx5/mr.c
+++ b/drivers/infiniband/hw/mlx5/mr.c
@@ -40,7 +40,6 @@
 #include <rdma/ib_umem_odp.h>
 #include <rdma/ib_verbs.h>
 #include "mlx5_ib.h"
-#include "user.h"
 
 enum {
 	MAX_PENDING_REG_MR = 8,
@@ -135,20 +134,10 @@ static void reg_mr_callback(int status, void *context)
 		return;
 	}
 
-	if (mr->out.hdr.status) {
-		mlx5_ib_warn(dev, "failed - status %d, syndorme 0x%x\n",
-			     mr->out.hdr.status,
-			     be32_to_cpu(mr->out.hdr.syndrome));
-		kfree(mr);
-		dev->fill_delay = 1;
-		mod_timer(&dev->delay_timer, jiffies + HZ);
-		return;
-	}
-
 	spin_lock_irqsave(&dev->mdev->priv.mkey_lock, flags);
 	key = dev->mdev->priv.mkey_key++;
 	spin_unlock_irqrestore(&dev->mdev->priv.mkey_lock, flags);
-	mr->mmkey.key = mlx5_idx_to_mkey(be32_to_cpu(mr->out.mkey) & 0xffffff) | key;
+	mr->mmkey.key = mlx5_idx_to_mkey(MLX5_GET(create_mkey_out, mr->out, mkey_index)) | key;
 
 	cache->last_add = jiffies;
 
@@ -170,16 +159,19 @@ static int add_keys(struct mlx5_ib_dev *dev, int c, int num)
 {
 	struct mlx5_mr_cache *cache = &dev->cache;
 	struct mlx5_cache_ent *ent = &cache->ent[c];
-	struct mlx5_create_mkey_mbox_in *in;
+	int inlen = MLX5_ST_SZ_BYTES(create_mkey_in);
 	struct mlx5_ib_mr *mr;
 	int npages = 1 << ent->order;
+	void *mkc;
+	u32 *in;
 	int err = 0;
 	int i;
 
-	in = kzalloc(sizeof(*in), GFP_KERNEL);
+	in = kzalloc(inlen, GFP_KERNEL);
 	if (!in)
 		return -ENOMEM;
 
+	mkc = MLX5_ADDR_OF(create_mkey_in, in, memory_key_mkey_entry);
 	for (i = 0; i < num; i++) {
 		if (ent->pending >= MAX_PENDING_REG_MR) {
 			err = -EAGAIN;
@@ -194,18 +186,22 @@ static int add_keys(struct mlx5_ib_dev *dev, int c, int num)
 		mr->order = ent->order;
 		mr->umred = 1;
 		mr->dev = dev;
-		in->seg.status = MLX5_MKEY_STATUS_FREE;
-		in->seg.xlt_oct_size = cpu_to_be32((npages + 1) / 2);
-		in->seg.qpn_mkey7_0 = cpu_to_be32(0xffffff << 8);
-		in->seg.flags = MLX5_ACCESS_MODE_MTT | MLX5_PERM_UMR_EN;
-		in->seg.log2_page_size = 12;
+
+		MLX5_SET(mkc, mkc, free, 1);
+		MLX5_SET(mkc, mkc, umr_en, 1);
+		MLX5_SET(mkc, mkc, access_mode, MLX5_MKC_ACCESS_MODE_MTT);
+
+		MLX5_SET(mkc, mkc, qpn, 0xffffff);
+		MLX5_SET(mkc, mkc, translations_octword_size, (npages + 1) / 2);
+		MLX5_SET(mkc, mkc, log_page_size, 12);
 
 		spin_lock_irq(&ent->lock);
 		ent->pending++;
 		spin_unlock_irq(&ent->lock);
-		err = mlx5_core_create_mkey(dev->mdev, &mr->mmkey, in,
-					    sizeof(*in), reg_mr_callback,
-					    mr, &mr->out);
+		err = mlx5_core_create_mkey_cb(dev->mdev, &mr->mmkey,
+					       in, inlen,
+					       mr->out, sizeof(mr->out),
+					       reg_mr_callback, mr);
 		if (err) {
 			spin_lock_irq(&ent->lock);
 			ent->pending--;
@@ -614,7 +610,7 @@ int mlx5_mr_cache_init(struct mlx5_ib_dev *dev)
 	int err;
 	int i;
 
-	cache->wq = create_singlethread_workqueue("mkey_cache");
+	cache->wq = alloc_ordered_workqueue("mkey_cache", WQ_MEM_RECLAIM);
 	if (!cache->wq) {
 		mlx5_ib_warn(dev, "failed to create work queue\n");
 		return -ENOMEM;
@@ -670,30 +666,38 @@ int mlx5_mr_cache_cleanup(struct mlx5_ib_dev *dev)
 struct ib_mr *mlx5_ib_get_dma_mr(struct ib_pd *pd, int acc)
 {
 	struct mlx5_ib_dev *dev = to_mdev(pd->device);
+	int inlen = MLX5_ST_SZ_BYTES(create_mkey_in);
 	struct mlx5_core_dev *mdev = dev->mdev;
-	struct mlx5_create_mkey_mbox_in *in;
-	struct mlx5_mkey_seg *seg;
 	struct mlx5_ib_mr *mr;
+	void *mkc;
+	u32 *in;
 	int err;
 
 	mr = kzalloc(sizeof(*mr), GFP_KERNEL);
 	if (!mr)
 		return ERR_PTR(-ENOMEM);
 
-	in = kzalloc(sizeof(*in), GFP_KERNEL);
+	in = kzalloc(inlen, GFP_KERNEL);
 	if (!in) {
 		err = -ENOMEM;
 		goto err_free;
 	}
 
-	seg = &in->seg;
-	seg->flags = convert_access(acc) | MLX5_ACCESS_MODE_PA;
-	seg->flags_pd = cpu_to_be32(to_mpd(pd)->pdn | MLX5_MKEY_LEN64);
-	seg->qpn_mkey7_0 = cpu_to_be32(0xffffff << 8);
-	seg->start_addr = 0;
+	mkc = MLX5_ADDR_OF(create_mkey_in, in, memory_key_mkey_entry);
+
+	MLX5_SET(mkc, mkc, access_mode, MLX5_MKC_ACCESS_MODE_PA);
+	MLX5_SET(mkc, mkc, a, !!(acc & IB_ACCESS_REMOTE_ATOMIC));
+	MLX5_SET(mkc, mkc, rw, !!(acc & IB_ACCESS_REMOTE_WRITE));
+	MLX5_SET(mkc, mkc, rr, !!(acc & IB_ACCESS_REMOTE_READ));
+	MLX5_SET(mkc, mkc, lw, !!(acc & IB_ACCESS_LOCAL_WRITE));
+	MLX5_SET(mkc, mkc, lr, 1);
 
-	err = mlx5_core_create_mkey(mdev, &mr->mmkey, in, sizeof(*in), NULL, NULL,
-				    NULL);
+	MLX5_SET(mkc, mkc, length64, 1);
+	MLX5_SET(mkc, mkc, pd, to_mpd(pd)->pdn);
+	MLX5_SET(mkc, mkc, qpn, 0xffffff);
+	MLX5_SET64(mkc, mkc, start_addr, 0);
+
+	err = mlx5_core_create_mkey(mdev, &mr->mmkey, in, inlen);
 	if (err)
 		goto err_in;
 
@@ -1063,9 +1067,11 @@ static struct mlx5_ib_mr *reg_create(struct ib_mr *ibmr, struct ib_pd *pd,
 				     int page_shift, int access_flags)
 {
 	struct mlx5_ib_dev *dev = to_mdev(pd->device);
-	struct mlx5_create_mkey_mbox_in *in;
 	struct mlx5_ib_mr *mr;
+	__be64 *pas;
+	void *mkc;
 	int inlen;
+	u32 *in;
 	int err;
 	bool pg_cap = !!(MLX5_CAP_GEN(dev->mdev, pg));
 
@@ -1073,31 +1079,41 @@ static struct mlx5_ib_mr *reg_create(struct ib_mr *ibmr, struct ib_pd *pd,
 	if (!mr)
 		return ERR_PTR(-ENOMEM);
 
-	inlen = sizeof(*in) + sizeof(*in->pas) * ((npages + 1) / 2) * 2;
+	inlen = MLX5_ST_SZ_BYTES(create_mkey_in) +
+		sizeof(*pas) * ((npages + 1) / 2) * 2;
 	in = mlx5_vzalloc(inlen);
 	if (!in) {
 		err = -ENOMEM;
 		goto err_1;
 	}
-	mlx5_ib_populate_pas(dev, umem, page_shift, in->pas,
+	pas = (__be64 *)MLX5_ADDR_OF(create_mkey_in, in, klm_pas_mtt);
+	mlx5_ib_populate_pas(dev, umem, page_shift, pas,
 			     pg_cap ? MLX5_IB_MTT_PRESENT : 0);
 
-	/* The MLX5_MKEY_INBOX_PG_ACCESS bit allows setting the access flags
+	/* The pg_access bit allows setting the access flags
 	 * in the page list submitted with the command. */
-	in->flags = pg_cap ? cpu_to_be32(MLX5_MKEY_INBOX_PG_ACCESS) : 0;
-	in->seg.flags = convert_access(access_flags) |
-		MLX5_ACCESS_MODE_MTT;
-	in->seg.flags_pd = cpu_to_be32(to_mpd(pd)->pdn);
-	in->seg.start_addr = cpu_to_be64(virt_addr);
-	in->seg.len = cpu_to_be64(length);
-	in->seg.bsfs_octo_size = 0;
-	in->seg.xlt_oct_size = cpu_to_be32(get_octo_len(virt_addr, length, 1 << page_shift));
-	in->seg.log2_page_size = page_shift;
-	in->seg.qpn_mkey7_0 = cpu_to_be32(0xffffff << 8);
-	in->xlat_oct_act_size = cpu_to_be32(get_octo_len(virt_addr, length,
-							 1 << page_shift));
-	err = mlx5_core_create_mkey(dev->mdev, &mr->mmkey, in, inlen, NULL,
-				    NULL, NULL);
+	MLX5_SET(create_mkey_in, in, pg_access, !!(pg_cap));
+
+	mkc = MLX5_ADDR_OF(create_mkey_in, in, memory_key_mkey_entry);
+	MLX5_SET(mkc, mkc, access_mode, MLX5_MKC_ACCESS_MODE_MTT);
+	MLX5_SET(mkc, mkc, a, !!(access_flags & IB_ACCESS_REMOTE_ATOMIC));
+	MLX5_SET(mkc, mkc, rw, !!(access_flags & IB_ACCESS_REMOTE_WRITE));
+	MLX5_SET(mkc, mkc, rr, !!(access_flags & IB_ACCESS_REMOTE_READ));
+	MLX5_SET(mkc, mkc, lw, !!(access_flags & IB_ACCESS_LOCAL_WRITE));
+	MLX5_SET(mkc, mkc, lr, 1);
+
+	MLX5_SET64(mkc, mkc, start_addr, virt_addr);
+	MLX5_SET64(mkc, mkc, len, length);
+	MLX5_SET(mkc, mkc, pd, to_mpd(pd)->pdn);
+	MLX5_SET(mkc, mkc, bsf_octword_size, 0);
+	MLX5_SET(mkc, mkc, translations_octword_size,
+		 get_octo_len(virt_addr, length, 1 << page_shift));
+	MLX5_SET(mkc, mkc, log_page_size, page_shift);
+	MLX5_SET(mkc, mkc, qpn, 0xffffff);
+	MLX5_SET(create_mkey_in, in, translations_octword_actual_size,
+		 get_octo_len(virt_addr, length, 1 << page_shift));
+
+	err = mlx5_core_create_mkey(dev->mdev, &mr->mmkey, in, inlen);
 	if (err) {
 		mlx5_ib_warn(dev, "create mkey failed\n");
 		goto err_2;
@@ -1523,30 +1539,32 @@ struct ib_mr *mlx5_ib_alloc_mr(struct ib_pd *pd,
 			       u32 max_num_sg)
 {
 	struct mlx5_ib_dev *dev = to_mdev(pd->device);
-	struct mlx5_create_mkey_mbox_in *in;
-	struct mlx5_ib_mr *mr;
+	int inlen = MLX5_ST_SZ_BYTES(create_mkey_in);
 	int ndescs = ALIGN(max_num_sg, 4);
+	struct mlx5_ib_mr *mr;
+	void *mkc;
+	u32 *in;
 	int err;
 
 	mr = kzalloc(sizeof(*mr), GFP_KERNEL);
 	if (!mr)
 		return ERR_PTR(-ENOMEM);
 
-	in = kzalloc(sizeof(*in), GFP_KERNEL);
+	in = kzalloc(inlen, GFP_KERNEL);
 	if (!in) {
 		err = -ENOMEM;
 		goto err_free;
 	}
 
-	in->seg.status = MLX5_MKEY_STATUS_FREE;
-	in->seg.xlt_oct_size = cpu_to_be32(ndescs);
-	in->seg.qpn_mkey7_0 = cpu_to_be32(0xffffff << 8);
-	in->seg.flags_pd = cpu_to_be32(to_mpd(pd)->pdn);
+	mkc = MLX5_ADDR_OF(create_mkey_in, in, memory_key_mkey_entry);
+	MLX5_SET(mkc, mkc, free, 1);
+	MLX5_SET(mkc, mkc, translations_octword_size, ndescs);
+	MLX5_SET(mkc, mkc, qpn, 0xffffff);
+	MLX5_SET(mkc, mkc, pd, to_mpd(pd)->pdn);
 
 	if (mr_type == IB_MR_TYPE_MEM_REG) {
-		mr->access_mode = MLX5_ACCESS_MODE_MTT;
-		in->seg.log2_page_size = PAGE_SHIFT;
-
+		mr->access_mode = MLX5_MKC_ACCESS_MODE_MTT;
+		MLX5_SET(mkc, mkc, log_page_size, PAGE_SHIFT);
 		err = mlx5_alloc_priv_descs(pd->device, mr,
 					    ndescs, sizeof(u64));
 		if (err)
@@ -1555,7 +1573,7 @@ struct ib_mr *mlx5_ib_alloc_mr(struct ib_pd *pd,
 		mr->desc_size = sizeof(u64);
 		mr->max_descs = ndescs;
 	} else if (mr_type == IB_MR_TYPE_SG_GAPS) {
-		mr->access_mode = MLX5_ACCESS_MODE_KLM;
+		mr->access_mode = MLX5_MKC_ACCESS_MODE_KLMS;
 
 		err = mlx5_alloc_priv_descs(pd->device, mr,
 					    ndescs, sizeof(struct mlx5_klm));
@@ -1566,9 +1584,8 @@ struct ib_mr *mlx5_ib_alloc_mr(struct ib_pd *pd,
 	} else if (mr_type == IB_MR_TYPE_SIGNATURE) {
 		u32 psv_index[2];
 
-		in->seg.flags_pd = cpu_to_be32(be32_to_cpu(in->seg.flags_pd) |
-							   MLX5_MKEY_BSF_EN);
-		in->seg.bsfs_octo_size = cpu_to_be32(MLX5_MKEY_BSF_OCTO_SIZE);
+		MLX5_SET(mkc, mkc, bsf_en, 1);
+		MLX5_SET(mkc, mkc, bsf_octword_size, MLX5_MKEY_BSF_OCTO_SIZE);
 		mr->sig = kzalloc(sizeof(*mr->sig), GFP_KERNEL);
 		if (!mr->sig) {
 			err = -ENOMEM;
@@ -1581,7 +1598,7 @@ struct ib_mr *mlx5_ib_alloc_mr(struct ib_pd *pd,
 		if (err)
 			goto err_free_sig;
 
-		mr->access_mode = MLX5_ACCESS_MODE_KLM;
+		mr->access_mode = MLX5_MKC_ACCESS_MODE_KLMS;
 		mr->sig->psv_memory.psv_idx = psv_index[0];
 		mr->sig->psv_wire.psv_idx = psv_index[1];
 
@@ -1595,9 +1612,10 @@ struct ib_mr *mlx5_ib_alloc_mr(struct ib_pd *pd,
 		goto err_free_in;
 	}
 
-	in->seg.flags = MLX5_PERM_UMR_EN | mr->access_mode;
-	err = mlx5_core_create_mkey(dev->mdev, &mr->mmkey, in, sizeof(*in),
-				    NULL, NULL, NULL);
+	MLX5_SET(mkc, mkc, access_mode, mr->access_mode);
+	MLX5_SET(mkc, mkc, umr_en, 1);
+
+	err = mlx5_core_create_mkey(dev->mdev, &mr->mmkey, in, inlen);
 	if (err)
 		goto err_destroy_psv;
 
@@ -1633,8 +1651,10 @@ struct ib_mw *mlx5_ib_alloc_mw(struct ib_pd *pd, enum ib_mw_type type,
 			       struct ib_udata *udata)
 {
 	struct mlx5_ib_dev *dev = to_mdev(pd->device);
-	struct mlx5_create_mkey_mbox_in *in = NULL;
+	int inlen = MLX5_ST_SZ_BYTES(create_mkey_in);
 	struct mlx5_ib_mw *mw = NULL;
+	u32 *in = NULL;
+	void *mkc;
 	int ndescs;
 	int err;
 	struct mlx5_ib_alloc_mw req = {};
@@ -1658,23 +1678,24 @@ struct ib_mw *mlx5_ib_alloc_mw(struct ib_pd *pd, enum ib_mw_type type,
 	ndescs = req.num_klms ? roundup(req.num_klms, 4) : roundup(1, 4);
 
 	mw = kzalloc(sizeof(*mw), GFP_KERNEL);
-	in = kzalloc(sizeof(*in), GFP_KERNEL);
+	in = kzalloc(inlen, GFP_KERNEL);
 	if (!mw || !in) {
 		err = -ENOMEM;
 		goto free;
 	}
 
-	in->seg.status = MLX5_MKEY_STATUS_FREE;
-	in->seg.xlt_oct_size = cpu_to_be32(ndescs);
-	in->seg.flags_pd = cpu_to_be32(to_mpd(pd)->pdn);
-	in->seg.flags = MLX5_PERM_UMR_EN | MLX5_ACCESS_MODE_KLM |
-		MLX5_PERM_LOCAL_READ;
-	if (type == IB_MW_TYPE_2)
-		in->seg.flags_pd |= cpu_to_be32(MLX5_MKEY_REMOTE_INVAL);
-	in->seg.qpn_mkey7_0 = cpu_to_be32(0xffffff << 8);
-
-	err = mlx5_core_create_mkey(dev->mdev, &mw->mmkey, in, sizeof(*in),
-				    NULL, NULL, NULL);
+	mkc = MLX5_ADDR_OF(create_mkey_in, in, memory_key_mkey_entry);
+
+	MLX5_SET(mkc, mkc, free, 1);
+	MLX5_SET(mkc, mkc, translations_octword_size, ndescs);
+	MLX5_SET(mkc, mkc, pd, to_mpd(pd)->pdn);
+	MLX5_SET(mkc, mkc, umr_en, 1);
+	MLX5_SET(mkc, mkc, lr, 1);
+	MLX5_SET(mkc, mkc, access_mode, MLX5_MKC_ACCESS_MODE_KLMS);
+	MLX5_SET(mkc, mkc, en_rinval, !!((type == IB_MW_TYPE_2)));
+	MLX5_SET(mkc, mkc, qpn, 0xffffff);
+
+	err = mlx5_core_create_mkey(dev->mdev, &mw->mmkey, in, inlen);
 	if (err)
 		goto free;
 
@@ -1811,7 +1832,7 @@ int mlx5_ib_map_mr_sg(struct ib_mr *ibmr, struct scatterlist *sg, int sg_nents,
 				   mr->desc_size * mr->max_descs,
 				   DMA_TO_DEVICE);
 
-	if (mr->access_mode == MLX5_ACCESS_MODE_KLM)
+	if (mr->access_mode == MLX5_MKC_ACCESS_MODE_KLMS)
 		n = mlx5_ib_sg_to_klms(mr, sg, sg_nents, sg_offset);
 	else
 		n = ib_sg_to_pages(ibmr, sg, sg_nents, sg_offset,
diff --git a/drivers/infiniband/hw/mlx5/odp.c b/drivers/infiniband/hw/mlx5/odp.c
index 34e79e709c67..cacb631a7b0a 100644
--- a/drivers/infiniband/hw/mlx5/odp.c
+++ b/drivers/infiniband/hw/mlx5/odp.c
@@ -782,8 +782,8 @@ void mlx5_ib_odp_remove_one(struct mlx5_ib_dev *ibdev)
 
 int __init mlx5_ib_odp_init(void)
 {
-	mlx5_ib_page_fault_wq =
-		create_singlethread_workqueue("mlx5_ib_page_faults");
+	mlx5_ib_page_fault_wq = alloc_ordered_workqueue("mlx5_ib_page_faults",
+							WQ_MEM_RECLAIM);
 	if (!mlx5_ib_page_fault_wq)
 		return -ENOMEM;
 
diff --git a/drivers/infiniband/hw/mlx5/qp.c b/drivers/infiniband/hw/mlx5/qp.c
index affc3f6598ca..41f4c2afbcdd 100644
--- a/drivers/infiniband/hw/mlx5/qp.c
+++ b/drivers/infiniband/hw/mlx5/qp.c
@@ -35,7 +35,6 @@
 #include <rdma/ib_cache.h>
 #include <rdma/ib_user_verbs.h>
 #include "mlx5_ib.h"
-#include "user.h"
 
 /* not supported currently */
 static int wq_signature;
@@ -77,6 +76,17 @@ struct mlx5_wqe_eth_pad {
 	u8 rsvd0[16];
 };
 
+enum raw_qp_set_mask_map {
+	MLX5_RAW_QP_MOD_SET_RQ_Q_CTR_ID		= 1UL << 0,
+};
+
+struct mlx5_modify_raw_qp_param {
+	u16 operation;
+
+	u32 set_mask; /* raw_qp_set_mask_map */
+	u8 rq_q_ctr_id;
+};
+
 static void get_cqs(enum ib_qp_type qp_type,
 		    struct ib_cq *ib_send_cq, struct ib_cq *ib_recv_cq,
 		    struct mlx5_ib_cq **send_cq, struct mlx5_ib_cq **recv_cq);
@@ -726,7 +736,7 @@ err_umem:
 static int create_user_qp(struct mlx5_ib_dev *dev, struct ib_pd *pd,
 			  struct mlx5_ib_qp *qp, struct ib_udata *udata,
 			  struct ib_qp_init_attr *attr,
-			  struct mlx5_create_qp_mbox_in **in,
+			  u32 **in,
 			  struct mlx5_ib_create_qp_resp *resp, int *inlen,
 			  struct mlx5_ib_qp_base *base)
 {
@@ -739,6 +749,8 @@ static int create_user_qp(struct mlx5_ib_dev *dev, struct ib_pd *pd,
 	u32 offset = 0;
 	int uuarn;
 	int ncont = 0;
+	__be64 *pas;
+	void *qpc;
 	int err;
 
 	err = ib_copy_from_udata(&ucmd, udata, sizeof(ucmd));
@@ -795,20 +807,24 @@ static int create_user_qp(struct mlx5_ib_dev *dev, struct ib_pd *pd,
 		ubuffer->umem = NULL;
 	}
 
-	*inlen = sizeof(**in) + sizeof(*(*in)->pas) * ncont;
+	*inlen = MLX5_ST_SZ_BYTES(create_qp_in) +
+		 MLX5_FLD_SZ_BYTES(create_qp_in, pas[0]) * ncont;
 	*in = mlx5_vzalloc(*inlen);
 	if (!*in) {
 		err = -ENOMEM;
 		goto err_umem;
 	}
+
+	pas = (__be64 *)MLX5_ADDR_OF(create_qp_in, *in, pas);
 	if (ubuffer->umem)
-		mlx5_ib_populate_pas(dev, ubuffer->umem, page_shift,
-				     (*in)->pas, 0);
-	(*in)->ctx.log_pg_sz_remote_qpn =
-		cpu_to_be32((page_shift - MLX5_ADAPTER_PAGE_SHIFT) << 24);
-	(*in)->ctx.params2 = cpu_to_be32(offset << 6);
+		mlx5_ib_populate_pas(dev, ubuffer->umem, page_shift, pas, 0);
+
+	qpc = MLX5_ADDR_OF(create_qp_in, *in, qpc);
 
-	(*in)->ctx.qp_counter_set_usr_page = cpu_to_be32(uar_index);
+	MLX5_SET(qpc, qpc, log_page_size, page_shift - MLX5_ADAPTER_PAGE_SHIFT);
+	MLX5_SET(qpc, qpc, page_offset, offset);
+
+	MLX5_SET(qpc, qpc, uar_page, uar_index);
 	resp->uuar_index = uuarn;
 	qp->uuarn = uuarn;
 
@@ -857,12 +873,13 @@ static void destroy_qp_user(struct ib_pd *pd, struct mlx5_ib_qp *qp,
 static int create_kernel_qp(struct mlx5_ib_dev *dev,
 			    struct ib_qp_init_attr *init_attr,
 			    struct mlx5_ib_qp *qp,
-			    struct mlx5_create_qp_mbox_in **in, int *inlen,
+			    u32 **in, int *inlen,
 			    struct mlx5_ib_qp_base *base)
 {
 	enum mlx5_ib_latency_class lc = MLX5_IB_LATENCY_CLASS_LOW;
 	struct mlx5_uuar_info *uuari;
 	int uar_index;
+	void *qpc;
 	int uuarn;
 	int err;
 
@@ -902,25 +919,29 @@ static int create_kernel_qp(struct mlx5_ib_dev *dev,
 	}
 
 	qp->sq.qend = mlx5_get_send_wqe(qp, qp->sq.wqe_cnt);
-	*inlen = sizeof(**in) + sizeof(*(*in)->pas) * qp->buf.npages;
+	*inlen = MLX5_ST_SZ_BYTES(create_qp_in) +
+		 MLX5_FLD_SZ_BYTES(create_qp_in, pas[0]) * qp->buf.npages;
 	*in = mlx5_vzalloc(*inlen);
 	if (!*in) {
 		err = -ENOMEM;
 		goto err_buf;
 	}
-	(*in)->ctx.qp_counter_set_usr_page = cpu_to_be32(uar_index);
-	(*in)->ctx.log_pg_sz_remote_qpn =
-		cpu_to_be32((qp->buf.page_shift - MLX5_ADAPTER_PAGE_SHIFT) << 24);
+
+	qpc = MLX5_ADDR_OF(create_qp_in, *in, qpc);
+	MLX5_SET(qpc, qpc, uar_page, uar_index);
+	MLX5_SET(qpc, qpc, log_page_size, qp->buf.page_shift - MLX5_ADAPTER_PAGE_SHIFT);
+
 	/* Set "fast registration enabled" for all kernel QPs */
-	(*in)->ctx.params1 |= cpu_to_be32(1 << 11);
-	(*in)->ctx.sq_crq_size |= cpu_to_be16(1 << 4);
+	MLX5_SET(qpc, qpc, fre, 1);
+	MLX5_SET(qpc, qpc, rlky, 1);
 
 	if (init_attr->create_flags & mlx5_ib_create_qp_sqpn_qp1()) {
-		(*in)->ctx.deth_sqpn = cpu_to_be32(1);
+		MLX5_SET(qpc, qpc, deth_sqpn, 1);
 		qp->flags |= MLX5_IB_QP_SQPN_QP1;
 	}
 
-	mlx5_fill_page_array(&qp->buf, (*in)->pas);
+	mlx5_fill_page_array(&qp->buf,
+			     (__be64 *)MLX5_ADDR_OF(create_qp_in, *in, pas));
 
 	err = mlx5_db_alloc(dev->mdev, &qp->db);
 	if (err) {
@@ -974,15 +995,15 @@ static void destroy_qp_kernel(struct mlx5_ib_dev *dev, struct mlx5_ib_qp *qp)
 	free_uuar(&dev->mdev->priv.uuari, qp->bf->uuarn);
 }
 
-static __be32 get_rx_type(struct mlx5_ib_qp *qp, struct ib_qp_init_attr *attr)
+static u32 get_rx_type(struct mlx5_ib_qp *qp, struct ib_qp_init_attr *attr)
 {
 	if (attr->srq || (attr->qp_type == IB_QPT_XRC_TGT) ||
 	    (attr->qp_type == IB_QPT_XRC_INI))
-		return cpu_to_be32(MLX5_SRQ_RQ);
+		return MLX5_SRQ_RQ;
 	else if (!qp->has_rq)
-		return cpu_to_be32(MLX5_ZERO_LEN_RQ);
+		return MLX5_ZERO_LEN_RQ;
 	else
-		return cpu_to_be32(MLX5_NON_ZERO_RQ);
+		return MLX5_NON_ZERO_RQ;
 }
 
 static int is_connected(enum ib_qp_type qp_type)
@@ -996,13 +1017,10 @@ static int is_connected(enum ib_qp_type qp_type)
 static int create_raw_packet_qp_tis(struct mlx5_ib_dev *dev,
 				    struct mlx5_ib_sq *sq, u32 tdn)
 {
-	u32 in[MLX5_ST_SZ_DW(create_tis_in)];
+	u32 in[MLX5_ST_SZ_DW(create_tis_in)] = {0};
 	void *tisc = MLX5_ADDR_OF(create_tis_in, in, ctx);
 
-	memset(in, 0, sizeof(in));
-
 	MLX5_SET(tisc, tisc, transport_domain, tdn);
-
 	return mlx5_core_create_tis(dev->mdev, in, sizeof(in), &sq->tisn);
 }
 
@@ -1191,7 +1209,7 @@ static void destroy_raw_packet_qp_tir(struct mlx5_ib_dev *dev,
 }
 
 static int create_raw_packet_qp(struct mlx5_ib_dev *dev, struct mlx5_ib_qp *qp,
-				struct mlx5_create_qp_mbox_in *in,
+				u32 *in,
 				struct ib_pd *pd)
 {
 	struct mlx5_ib_raw_packet_qp *raw_packet_qp = &qp->raw_packet_qp;
@@ -1462,18 +1480,18 @@ 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;
+	int inlen = MLX5_ST_SZ_BYTES(create_qp_in);
 	struct mlx5_core_dev *mdev = dev->mdev;
-	struct mlx5_ib_qp_base *base;
 	struct mlx5_ib_create_qp_resp resp;
-	struct mlx5_create_qp_mbox_in *in;
-	struct mlx5_ib_create_qp ucmd;
 	struct mlx5_ib_cq *send_cq;
 	struct mlx5_ib_cq *recv_cq;
 	unsigned long flags;
-	int inlen = sizeof(*in);
-	int err;
 	u32 uidx = MLX5_IB_DEFAULT_UIDX;
+	struct mlx5_ib_create_qp ucmd;
+	struct mlx5_ib_qp_base *base;
 	void *qpc;
+	u32 *in;
+	int err;
 
 	base = init_attr->qp_type == IB_QPT_RAW_PACKET ?
 	       &qp->raw_packet_qp.rq.base :
@@ -1601,7 +1619,7 @@ static int create_qp_common(struct mlx5_ib_dev *dev, struct ib_pd *pd,
 		if (err)
 			return err;
 	} else {
-		in = mlx5_vzalloc(sizeof(*in));
+		in = mlx5_vzalloc(inlen);
 		if (!in)
 			return -ENOMEM;
 
@@ -1611,26 +1629,29 @@ static int create_qp_common(struct mlx5_ib_dev *dev, struct ib_pd *pd,
 	if (is_sqp(init_attr->qp_type))
 		qp->port = init_attr->port_num;
 
-	in->ctx.flags = cpu_to_be32(to_mlx5_st(init_attr->qp_type) << 16 |
-				    MLX5_QP_PM_MIGRATED << 11);
+	qpc = MLX5_ADDR_OF(create_qp_in, in, qpc);
+
+	MLX5_SET(qpc, qpc, st, to_mlx5_st(init_attr->qp_type));
+	MLX5_SET(qpc, qpc, pm_state, MLX5_QP_PM_MIGRATED);
 
 	if (init_attr->qp_type != MLX5_IB_QPT_REG_UMR)
-		in->ctx.flags_pd = cpu_to_be32(to_mpd(pd ? pd : devr->p0)->pdn);
+		MLX5_SET(qpc, qpc, pd, to_mpd(pd ? pd : devr->p0)->pdn);
 	else
-		in->ctx.flags_pd = cpu_to_be32(MLX5_QP_LAT_SENSITIVE);
+		MLX5_SET(qpc, qpc, latency_sensitive, 1);
+
 
 	if (qp->wq_sig)
-		in->ctx.flags_pd |= cpu_to_be32(MLX5_QP_ENABLE_SIG);
+		MLX5_SET(qpc, qpc, wq_signature, 1);
 
 	if (qp->flags & MLX5_IB_QP_BLOCK_MULTICAST_LOOPBACK)
-		in->ctx.flags_pd |= cpu_to_be32(MLX5_QP_BLOCK_MCAST);
+		MLX5_SET(qpc, qpc, block_lb_mc, 1);
 
 	if (qp->flags & MLX5_IB_QP_CROSS_CHANNEL)
-		in->ctx.params2 |= cpu_to_be32(MLX5_QP_BIT_CC_MASTER);
+		MLX5_SET(qpc, qpc, cd_master, 1);
 	if (qp->flags & MLX5_IB_QP_MANAGED_SEND)
-		in->ctx.params2 |= cpu_to_be32(MLX5_QP_BIT_CC_SLAVE_SEND);
+		MLX5_SET(qpc, qpc, cd_slave_send, 1);
 	if (qp->flags & MLX5_IB_QP_MANAGED_RECV)
-		in->ctx.params2 |= cpu_to_be32(MLX5_QP_BIT_CC_SLAVE_RECV);
+		MLX5_SET(qpc, qpc, cd_slave_receive, 1);
 
 	if (qp->scat_cqe && is_connected(init_attr->qp_type)) {
 		int rcqe_sz;
@@ -1640,71 +1661,68 @@ static int create_qp_common(struct mlx5_ib_dev *dev, struct ib_pd *pd,
 		scqe_sz = mlx5_ib_get_cqe_size(dev, init_attr->send_cq);
 
 		if (rcqe_sz == 128)
-			in->ctx.cs_res = MLX5_RES_SCAT_DATA64_CQE;
+			MLX5_SET(qpc, qpc, cs_res, MLX5_RES_SCAT_DATA64_CQE);
 		else
-			in->ctx.cs_res = MLX5_RES_SCAT_DATA32_CQE;
+			MLX5_SET(qpc, qpc, cs_res, MLX5_RES_SCAT_DATA32_CQE);
 
 		if (init_attr->sq_sig_type == IB_SIGNAL_ALL_WR) {
 			if (scqe_sz == 128)
-				in->ctx.cs_req = MLX5_REQ_SCAT_DATA64_CQE;
+				MLX5_SET(qpc, qpc, cs_req, MLX5_REQ_SCAT_DATA64_CQE);
 			else
-				in->ctx.cs_req = MLX5_REQ_SCAT_DATA32_CQE;
+				MLX5_SET(qpc, qpc, cs_req, MLX5_REQ_SCAT_DATA32_CQE);
 		}
 	}
 
 	if (qp->rq.wqe_cnt) {
-		in->ctx.rq_size_stride = (qp->rq.wqe_shift - 4);
-		in->ctx.rq_size_stride |= ilog2(qp->rq.wqe_cnt) << 3;
+		MLX5_SET(qpc, qpc, log_rq_stride, qp->rq.wqe_shift - 4);
+		MLX5_SET(qpc, qpc, log_rq_size, ilog2(qp->rq.wqe_cnt));
 	}
 
-	in->ctx.rq_type_srqn = get_rx_type(qp, init_attr);
+	MLX5_SET(qpc, qpc, rq_type, get_rx_type(qp, init_attr));
 
 	if (qp->sq.wqe_cnt)
-		in->ctx.sq_crq_size |= cpu_to_be16(ilog2(qp->sq.wqe_cnt) << 11);
+		MLX5_SET(qpc, qpc, log_sq_size, ilog2(qp->sq.wqe_cnt));
 	else
-		in->ctx.sq_crq_size |= cpu_to_be16(0x8000);
+		MLX5_SET(qpc, qpc, no_sq, 1);
 
 	/* Set default resources */
 	switch (init_attr->qp_type) {
 	case IB_QPT_XRC_TGT:
-		in->ctx.cqn_recv = cpu_to_be32(to_mcq(devr->c0)->mcq.cqn);
-		in->ctx.cqn_send = cpu_to_be32(to_mcq(devr->c0)->mcq.cqn);
-		in->ctx.rq_type_srqn |= cpu_to_be32(to_msrq(devr->s0)->msrq.srqn);
-		in->ctx.xrcd = cpu_to_be32(to_mxrcd(init_attr->xrcd)->xrcdn);
+		MLX5_SET(qpc, qpc, cqn_rcv, to_mcq(devr->c0)->mcq.cqn);
+		MLX5_SET(qpc, qpc, cqn_snd, to_mcq(devr->c0)->mcq.cqn);
+		MLX5_SET(qpc, qpc, srqn_rmpn_xrqn, to_msrq(devr->s0)->msrq.srqn);
+		MLX5_SET(qpc, qpc, xrcd, to_mxrcd(init_attr->xrcd)->xrcdn);
 		break;
 	case IB_QPT_XRC_INI:
-		in->ctx.cqn_recv = cpu_to_be32(to_mcq(devr->c0)->mcq.cqn);
-		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);
+		MLX5_SET(qpc, qpc, cqn_rcv, to_mcq(devr->c0)->mcq.cqn);
+		MLX5_SET(qpc, qpc, xrcd, to_mxrcd(devr->x1)->xrcdn);
+		MLX5_SET(qpc, qpc, srqn_rmpn_xrqn, to_msrq(devr->s0)->msrq.srqn);
 		break;
 	default:
 		if (init_attr->srq) {
-			in->ctx.xrcd = cpu_to_be32(to_mxrcd(devr->x0)->xrcdn);
-			in->ctx.rq_type_srqn |= cpu_to_be32(to_msrq(init_attr->srq)->msrq.srqn);
+			MLX5_SET(qpc, qpc, xrcd, to_mxrcd(devr->x0)->xrcdn);
+			MLX5_SET(qpc, qpc, srqn_rmpn_xrqn, 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->s1)->msrq.srqn);
+			MLX5_SET(qpc, qpc, xrcd, to_mxrcd(devr->x1)->xrcdn);
+			MLX5_SET(qpc, qpc, srqn_rmpn_xrqn, to_msrq(devr->s1)->msrq.srqn);
 		}
 	}
 
 	if (init_attr->send_cq)
-		in->ctx.cqn_send = cpu_to_be32(to_mcq(init_attr->send_cq)->mcq.cqn);
+		MLX5_SET(qpc, qpc, cqn_snd, to_mcq(init_attr->send_cq)->mcq.cqn);
 
 	if (init_attr->recv_cq)
-		in->ctx.cqn_recv = cpu_to_be32(to_mcq(init_attr->recv_cq)->mcq.cqn);
+		MLX5_SET(qpc, qpc, cqn_rcv, to_mcq(init_attr->recv_cq)->mcq.cqn);
 
-	in->ctx.db_rec_addr = cpu_to_be64(qp->db.dma);
+	MLX5_SET64(qpc, qpc, dbr_addr, qp->db.dma);
 
-	if (MLX5_CAP_GEN(mdev, cqe_version) == MLX5_CQE_VERSION_V1) {
-		qpc = MLX5_ADDR_OF(create_qp_in, in, qpc);
-		/* 0xffffff means we ask to work with cqe version 0 */
+	/* 0xffffff means we ask to work with cqe version 0 */
+	if (MLX5_CAP_GEN(mdev, cqe_version) == MLX5_CQE_VERSION_V1)
 		MLX5_SET(qpc, qpc, user_index, uidx);
-	}
+
 	/* we use IB_QP_CREATE_IPOIB_UD_LSO to indicates ipoib qp */
 	if (init_attr->qp_type == IB_QPT_UD &&
 	    (init_attr->create_flags & IB_QP_CREATE_IPOIB_UD_LSO)) {
-		qpc = MLX5_ADDR_OF(create_qp_in, in, qpc);
 		MLX5_SET(qpc, qpc, ulp_stateless_offload_mode, 1);
 		qp->flags |= MLX5_IB_QP_LSO;
 	}
@@ -1855,13 +1873,13 @@ static void get_cqs(enum ib_qp_type qp_type,
 }
 
 static int modify_raw_packet_qp(struct mlx5_ib_dev *dev, struct mlx5_ib_qp *qp,
-				u16 operation);
+				const struct mlx5_modify_raw_qp_param *raw_qp_param,
+				u8 lag_tx_affinity);
 
 static void destroy_qp_common(struct mlx5_ib_dev *dev, struct mlx5_ib_qp *qp)
 {
 	struct mlx5_ib_cq *send_cq, *recv_cq;
 	struct mlx5_ib_qp_base *base = &qp->trans_qp.base;
-	struct mlx5_modify_qp_mbox_in *in;
 	unsigned long flags;
 	int err;
 
@@ -1874,19 +1892,18 @@ static void destroy_qp_common(struct mlx5_ib_dev *dev, struct mlx5_ib_qp *qp)
 	       &qp->raw_packet_qp.rq.base :
 	       &qp->trans_qp.base;
 
-	in = kzalloc(sizeof(*in), GFP_KERNEL);
-	if (!in)
-		return;
-
 	if (qp->state != IB_QPS_RESET) {
 		if (qp->ibqp.qp_type != IB_QPT_RAW_PACKET) {
 			mlx5_ib_qp_disable_pagefaults(qp);
 			err = mlx5_core_qp_modify(dev->mdev,
-						  MLX5_CMD_OP_2RST_QP, in, 0,
-						  &base->mqp);
+						  MLX5_CMD_OP_2RST_QP, 0,
+						  NULL, &base->mqp);
 		} else {
-			err = modify_raw_packet_qp(dev, qp,
-						   MLX5_CMD_OP_2RST_QP);
+			struct mlx5_modify_raw_qp_param raw_qp_param = {
+				.operation = MLX5_CMD_OP_2RST_QP
+			};
+
+			err = modify_raw_packet_qp(dev, qp, &raw_qp_param, 0);
 		}
 		if (err)
 			mlx5_ib_warn(dev, "mlx5_ib: modify QP 0x%06x to RESET failed\n",
@@ -1925,8 +1942,6 @@ static void destroy_qp_common(struct mlx5_ib_dev *dev, struct mlx5_ib_qp *qp)
 				     base->mqp.qpn);
 	}
 
-	kfree(in);
-
 	if (qp->create_type == MLX5_QP_KERNEL)
 		destroy_qp_kernel(dev, qp);
 	else if (qp->create_type == MLX5_QP_USER)
@@ -2152,6 +2167,31 @@ static int modify_raw_packet_eth_prio(struct mlx5_core_dev *dev,
 	return err;
 }
 
+static int modify_raw_packet_tx_affinity(struct mlx5_core_dev *dev,
+					 struct mlx5_ib_sq *sq, u8 tx_affinity)
+{
+	void *in;
+	void *tisc;
+	int inlen;
+	int err;
+
+	inlen = MLX5_ST_SZ_BYTES(modify_tis_in);
+	in = mlx5_vzalloc(inlen);
+	if (!in)
+		return -ENOMEM;
+
+	MLX5_SET(modify_tis_in, in, bitmask.lag_tx_port_affinity, 1);
+
+	tisc = MLX5_ADDR_OF(modify_tis_in, in, ctx);
+	MLX5_SET(tisc, tisc, lag_tx_port_affinity, tx_affinity);
+
+	err = mlx5_core_modify_tis(dev, sq->tisn, in, inlen);
+
+	kvfree(in);
+
+	return err;
+}
+
 static int mlx5_set_path(struct mlx5_ib_dev *dev, struct mlx5_ib_qp *qp,
 			 const struct ib_ah_attr *ah,
 			 struct mlx5_qp_path *path, u8 port, int attr_mask,
@@ -2362,8 +2402,9 @@ static int ib_mask_to_mlx5_opt(int ib_mask)
 	return result;
 }
 
-static int modify_raw_packet_qp_rq(struct mlx5_core_dev *dev,
-				   struct mlx5_ib_rq *rq, int new_state)
+static int modify_raw_packet_qp_rq(struct mlx5_ib_dev *dev,
+				   struct mlx5_ib_rq *rq, int new_state,
+				   const struct mlx5_modify_raw_qp_param *raw_qp_param)
 {
 	void *in;
 	void *rqc;
@@ -2380,7 +2421,17 @@ static int modify_raw_packet_qp_rq(struct mlx5_core_dev *dev,
 	rqc = MLX5_ADDR_OF(modify_rq_in, in, ctx);
 	MLX5_SET(rqc, rqc, state, new_state);
 
-	err = mlx5_core_modify_rq(dev, rq->base.mqp.qpn, in, inlen);
+	if (raw_qp_param->set_mask & MLX5_RAW_QP_MOD_SET_RQ_Q_CTR_ID) {
+		if (MLX5_CAP_GEN(dev->mdev, modify_rq_counter_set_id)) {
+			MLX5_SET64(modify_rq_in, in, modify_bitmask,
+				   MLX5_MODIFY_RQ_IN_MODIFY_BITMASK_MODIFY_RQ_COUNTER_SET_ID);
+			MLX5_SET(rqc, rqc, counter_set_id, raw_qp_param->rq_q_ctr_id);
+		} else
+			pr_info_once("%s: RAW PACKET QP counters are not supported on current FW\n",
+				     dev->ib_dev.name);
+	}
+
+	err = mlx5_core_modify_rq(dev->mdev, rq->base.mqp.qpn, in, inlen);
 	if (err)
 		goto out;
 
@@ -2421,7 +2472,8 @@ out:
 }
 
 static int modify_raw_packet_qp(struct mlx5_ib_dev *dev, struct mlx5_ib_qp *qp,
-				u16 operation)
+				const struct mlx5_modify_raw_qp_param *raw_qp_param,
+				u8 tx_affinity)
 {
 	struct mlx5_ib_raw_packet_qp *raw_packet_qp = &qp->raw_packet_qp;
 	struct mlx5_ib_rq *rq = &raw_packet_qp->rq;
@@ -2430,7 +2482,7 @@ static int modify_raw_packet_qp(struct mlx5_ib_dev *dev, struct mlx5_ib_qp *qp,
 	int sq_state;
 	int err;
 
-	switch (operation) {
+	switch (raw_qp_param->operation) {
 	case MLX5_CMD_OP_RST2INIT_QP:
 		rq_state = MLX5_RQC_STATE_RDY;
 		sq_state = MLX5_SQC_STATE_RDY;
@@ -2447,21 +2499,31 @@ static int modify_raw_packet_qp(struct mlx5_ib_dev *dev, struct mlx5_ib_qp *qp,
 	case MLX5_CMD_OP_INIT2RTR_QP:
 	case MLX5_CMD_OP_RTR2RTS_QP:
 	case MLX5_CMD_OP_RTS2RTS_QP:
-		/* Nothing to do here... */
-		return 0;
+		if (raw_qp_param->set_mask)
+			return -EINVAL;
+		else
+			return 0;
 	default:
 		WARN_ON(1);
 		return -EINVAL;
 	}
 
 	if (qp->rq.wqe_cnt) {
-		err =  modify_raw_packet_qp_rq(dev->mdev, rq, rq_state);
+		err = modify_raw_packet_qp_rq(dev, rq, rq_state, raw_qp_param);
 		if (err)
 			return err;
 	}
 
-	if (qp->sq.wqe_cnt)
+	if (qp->sq.wqe_cnt) {
+		if (tx_affinity) {
+			err = modify_raw_packet_tx_affinity(dev->mdev, sq,
+							    tx_affinity);
+			if (err)
+				return err;
+		}
+
 		return modify_raw_packet_qp_sq(dev->mdev, sq, sq_state);
+	}
 
 	return 0;
 }
@@ -2512,20 +2574,20 @@ static int __mlx5_ib_modify_qp(struct ib_qp *ibqp,
 	struct mlx5_ib_qp_base *base = &qp->trans_qp.base;
 	struct mlx5_ib_cq *send_cq, *recv_cq;
 	struct mlx5_qp_context *context;
-	struct mlx5_modify_qp_mbox_in *in;
 	struct mlx5_ib_pd *pd;
+	struct mlx5_ib_port *mibport = NULL;
 	enum mlx5_qp_state mlx5_cur, mlx5_new;
 	enum mlx5_qp_optpar optpar;
 	int sqd_event;
 	int mlx5_st;
 	int err;
 	u16 op;
+	u8 tx_affinity = 0;
 
-	in = kzalloc(sizeof(*in), GFP_KERNEL);
-	if (!in)
+	context = kzalloc(sizeof(*context), GFP_KERNEL);
+	if (!context)
 		return -ENOMEM;
 
-	context = &in->ctx;
 	err = to_mlx5_st(ibqp->qp_type);
 	if (err < 0) {
 		mlx5_ib_dbg(dev, "unsupported qp type %d\n", ibqp->qp_type);
@@ -2550,6 +2612,23 @@ static int __mlx5_ib_modify_qp(struct ib_qp *ibqp,
 		}
 	}
 
+	if ((cur_state == IB_QPS_RESET) && (new_state == IB_QPS_INIT)) {
+		if ((ibqp->qp_type == IB_QPT_RC) ||
+		    (ibqp->qp_type == IB_QPT_UD &&
+		     !(qp->flags & MLX5_IB_QP_SQPN_QP1)) ||
+		    (ibqp->qp_type == IB_QPT_UC) ||
+		    (ibqp->qp_type == IB_QPT_RAW_PACKET) ||
+		    (ibqp->qp_type == IB_QPT_XRC_INI) ||
+		    (ibqp->qp_type == IB_QPT_XRC_TGT)) {
+			if (mlx5_lag_is_active(dev->mdev)) {
+				tx_affinity = (unsigned int)atomic_add_return(1,
+						&dev->roce.next_port) %
+						MLX5_MAX_PORTS + 1;
+				context->flags |= cpu_to_be32(tx_affinity << 24);
+			}
+		}
+	}
+
 	if (is_sqp(ibqp->qp_type)) {
 		context->mtu_msgmax = (IB_MTU_256 << 5) | 8;
 	} else if (ibqp->qp_type == IB_QPT_UD ||
@@ -2655,8 +2734,7 @@ static int __mlx5_ib_modify_qp(struct ib_qp *ibqp,
 	if (cur_state == IB_QPS_RESET && new_state == IB_QPS_INIT) {
 		u8 port_num = (attr_mask & IB_QP_PORT ? attr->port_num :
 			       qp->port) - 1;
-		struct mlx5_ib_port *mibport = &dev->port[port_num];
-
+		mibport = &dev->port[port_num];
 		context->qp_counter_set_usr_page |=
 			cpu_to_be32((u32)(mibport->q_cnt_id) << 24);
 	}
@@ -2690,13 +2768,21 @@ static int __mlx5_ib_modify_qp(struct ib_qp *ibqp,
 	op = optab[mlx5_cur][mlx5_new];
 	optpar = ib_mask_to_mlx5_opt(attr_mask);
 	optpar &= opt_mask[mlx5_cur][mlx5_new][mlx5_st];
-	in->optparam = cpu_to_be32(optpar);
 
-	if (qp->ibqp.qp_type == IB_QPT_RAW_PACKET)
-		err = modify_raw_packet_qp(dev, qp, op);
-	else
-		err = mlx5_core_qp_modify(dev->mdev, op, in, sqd_event,
+	if (qp->ibqp.qp_type == IB_QPT_RAW_PACKET) {
+		struct mlx5_modify_raw_qp_param raw_qp_param = {};
+
+		raw_qp_param.operation = op;
+		if (cur_state == IB_QPS_RESET && new_state == IB_QPS_INIT) {
+			raw_qp_param.rq_q_ctr_id = mibport->q_cnt_id;
+			raw_qp_param.set_mask |= MLX5_RAW_QP_MOD_SET_RQ_Q_CTR_ID;
+		}
+		err = modify_raw_packet_qp(dev, qp, &raw_qp_param, tx_affinity);
+	} else {
+		err = mlx5_core_qp_modify(dev->mdev, op, optpar, context,
 					  &base->mqp);
+	}
+
 	if (err)
 		goto out;
 
@@ -2736,7 +2822,7 @@ static int __mlx5_ib_modify_qp(struct ib_qp *ibqp,
 	}
 
 out:
-	kfree(in);
+	kfree(context);
 	return err;
 }
 
@@ -2969,7 +3055,7 @@ static void set_reg_umr_seg(struct mlx5_wqe_umr_ctrl_seg *umr,
 
 	memset(umr, 0, sizeof(*umr));
 
-	if (mr->access_mode == MLX5_ACCESS_MODE_KLM)
+	if (mr->access_mode == MLX5_MKC_ACCESS_MODE_KLMS)
 		/* KLMs take twice the size of MTTs */
 		ndescs *= 2;
 
@@ -3112,9 +3198,9 @@ static void set_reg_mkey_seg(struct mlx5_mkey_seg *seg,
 
 	memset(seg, 0, sizeof(*seg));
 
-	if (mr->access_mode == MLX5_ACCESS_MODE_MTT)
+	if (mr->access_mode == MLX5_MKC_ACCESS_MODE_MTT)
 		seg->log2_page_size = ilog2(mr->ibmr.page_size);
-	else if (mr->access_mode == MLX5_ACCESS_MODE_KLM)
+	else if (mr->access_mode == MLX5_MKC_ACCESS_MODE_KLMS)
 		/* KLMs take twice the size of MTTs */
 		ndescs *= 2;
 
@@ -3455,7 +3541,7 @@ static void set_sig_mkey_segment(struct mlx5_mkey_seg *seg,
 	memset(seg, 0, sizeof(*seg));
 
 	seg->flags = get_umr_flags(wr->access_flags) |
-				   MLX5_ACCESS_MODE_KLM;
+				   MLX5_MKC_ACCESS_MODE_KLMS;
 	seg->qpn_mkey7_0 = cpu_to_be32((sig_key & 0xff) | 0xffffff00);
 	seg->flags_pd = cpu_to_be32(MLX5_MKEY_REMOTE_INVAL | sigerr << 26 |
 				    MLX5_MKEY_BSF_EN | pdn);
@@ -4317,21 +4403,24 @@ static int query_raw_packet_qp_state(struct mlx5_ib_dev *dev,
 static int query_qp_attr(struct mlx5_ib_dev *dev, struct mlx5_ib_qp *qp,
 			 struct ib_qp_attr *qp_attr)
 {
-	struct mlx5_query_qp_mbox_out *outb;
+	int outlen = MLX5_ST_SZ_BYTES(query_qp_out);
 	struct mlx5_qp_context *context;
 	int mlx5_state;
+	u32 *outb;
 	int err = 0;
 
-	outb = kzalloc(sizeof(*outb), GFP_KERNEL);
+	outb = kzalloc(outlen, GFP_KERNEL);
 	if (!outb)
 		return -ENOMEM;
 
-	context = &outb->ctx;
 	err = mlx5_core_qp_query(dev->mdev, &qp->trans_qp.base.mqp, outb,
-				 sizeof(*outb));
+				 outlen);
 	if (err)
 		goto out;
 
+	/* FIXME: use MLX5_GET rather than mlx5_qp_context manual struct */
+	context = (struct mlx5_qp_context *)MLX5_ADDR_OF(query_qp_out, outb, qpc);
+
 	mlx5_state = be32_to_cpu(context->flags) >> 28;
 
 	qp->state		     = to_ib_qp_state(mlx5_state);
@@ -4496,6 +4585,28 @@ int mlx5_ib_dealloc_xrcd(struct ib_xrcd *xrcd)
 	return 0;
 }
 
+static void mlx5_ib_wq_event(struct mlx5_core_qp *core_qp, int type)
+{
+	struct mlx5_ib_rwq *rwq = to_mibrwq(core_qp);
+	struct mlx5_ib_dev *dev = to_mdev(rwq->ibwq.device);
+	struct ib_event event;
+
+	if (rwq->ibwq.event_handler) {
+		event.device     = rwq->ibwq.device;
+		event.element.wq = &rwq->ibwq;
+		switch (type) {
+		case MLX5_EVENT_TYPE_WQ_CATAS_ERROR:
+			event.event = IB_EVENT_WQ_FATAL;
+			break;
+		default:
+			mlx5_ib_warn(dev, "Unexpected event type %d on WQ %06x\n", type, core_qp->qpn);
+			return;
+		}
+
+		rwq->ibwq.event_handler(&event, rwq->ibwq.wq_context);
+	}
+}
+
 static int  create_rq(struct mlx5_ib_rwq *rwq, struct ib_pd *pd,
 		      struct ib_wq_init_attr *init_attr)
 {
@@ -4533,7 +4644,7 @@ static int  create_rq(struct mlx5_ib_rwq *rwq, struct ib_pd *pd,
 	MLX5_SET64(wq, wq, dbr_addr, rwq->db.dma);
 	rq_pas0 = (__be64 *)MLX5_ADDR_OF(wq, wq, pas);
 	mlx5_ib_populate_pas(dev, rwq->umem, rwq->page_shift, rq_pas0, 0);
-	err = mlx5_core_create_rq(dev->mdev, in, inlen, &rwq->rqn);
+	err = mlx5_core_create_rq_tracked(dev->mdev, in, inlen, &rwq->core_qp);
 	kvfree(in);
 	return err;
 }
@@ -4649,7 +4760,7 @@ struct ib_wq *mlx5_ib_create_wq(struct ib_pd *pd,
 		return ERR_PTR(-EINVAL);
 	}
 
-	rwq->ibwq.wq_num = rwq->rqn;
+	rwq->ibwq.wq_num = rwq->core_qp.qpn;
 	rwq->ibwq.state = IB_WQS_RESET;
 	if (udata->outlen) {
 		resp.response_length = offsetof(typeof(resp), response_length) +
@@ -4659,10 +4770,12 @@ struct ib_wq *mlx5_ib_create_wq(struct ib_pd *pd,
 			goto err_copy;
 	}
 
+	rwq->core_qp.event = mlx5_ib_wq_event;
+	rwq->ibwq.event_handler = init_attr->event_handler;
 	return &rwq->ibwq;
 
 err_copy:
-	mlx5_core_destroy_rq(dev->mdev, rwq->rqn);
+	mlx5_core_destroy_rq_tracked(dev->mdev, &rwq->core_qp);
 err_user_rq:
 	destroy_user_rq(pd, rwq);
 err:
@@ -4675,7 +4788,7 @@ int mlx5_ib_destroy_wq(struct ib_wq *wq)
 	struct mlx5_ib_dev *dev = to_mdev(wq->device);
 	struct mlx5_ib_rwq *rwq = to_mrwq(wq);
 
-	mlx5_core_destroy_rq(dev->mdev, rwq->rqn);
+	mlx5_core_destroy_rq_tracked(dev->mdev, &rwq->core_qp);
 	destroy_user_rq(wq->pd, rwq);
 	kfree(rwq);
 
@@ -4807,7 +4920,7 @@ int mlx5_ib_modify_wq(struct ib_wq *wq, struct ib_wq_attr *wq_attr,
 	MLX5_SET(modify_rq_in, in, rq_state, curr_wq_state);
 	MLX5_SET(rqc, rqc, state, wq_state);
 
-	err = mlx5_core_modify_rq(dev->mdev, rwq->rqn, in, inlen);
+	err = mlx5_core_modify_rq(dev->mdev, rwq->core_qp.qpn, in, inlen);
 	kvfree(in);
 	if (!err)
 		rwq->ibwq.state = (wq_state == MLX5_RQC_STATE_ERR) ? IB_WQS_ERR : wq_state;
diff --git a/drivers/infiniband/hw/mlx5/srq.c b/drivers/infiniband/hw/mlx5/srq.c
index ed6ac52355f1..3857dbd9c956 100644
--- a/drivers/infiniband/hw/mlx5/srq.c
+++ b/drivers/infiniband/hw/mlx5/srq.c
@@ -38,7 +38,6 @@
 #include <rdma/ib_user_verbs.h>
 
 #include "mlx5_ib.h"
-#include "user.h"
 
 /* not supported currently */
 static int srq_signature;
diff --git a/drivers/infiniband/hw/mlx5/user.h b/drivers/infiniband/hw/mlx5/user.h
deleted file mode 100644
index 188dac4301b5..000000000000
--- a/drivers/infiniband/hw/mlx5/user.h
+++ /dev/null
@@ -1,281 +0,0 @@
-/*
- * Copyright (c) 2013-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.
- */
-
-#ifndef MLX5_IB_USER_H
-#define MLX5_IB_USER_H
-
-#include <linux/types.h>
-
-#include "mlx5_ib.h"
-
-enum {
-	MLX5_QP_FLAG_SIGNATURE		= 1 << 0,
-	MLX5_QP_FLAG_SCATTER_CQE	= 1 << 1,
-};
-
-enum {
-	MLX5_SRQ_FLAG_SIGNATURE		= 1 << 0,
-};
-
-enum {
-	MLX5_WQ_FLAG_SIGNATURE		= 1 << 0,
-};
-
-
-/* Increment this value if any changes that break userspace ABI
- * compatibility are made.
- */
-#define MLX5_IB_UVERBS_ABI_VERSION	1
-
-/* Make sure that all structs defined in this file remain laid out so
- * that they pack the same way on 32-bit and 64-bit architectures (to
- * avoid incompatibility between 32-bit userspace and 64-bit kernels).
- * In particular do not use pointer types -- pass pointers in __u64
- * instead.
- */
-
-struct mlx5_ib_alloc_ucontext_req {
-	__u32	total_num_uuars;
-	__u32	num_low_latency_uuars;
-};
-
-struct mlx5_ib_alloc_ucontext_req_v2 {
-	__u32	total_num_uuars;
-	__u32	num_low_latency_uuars;
-	__u32	flags;
-	__u32	comp_mask;
-	__u8	max_cqe_version;
-	__u8	reserved0;
-	__u16	reserved1;
-	__u32	reserved2;
-};
-
-enum mlx5_ib_alloc_ucontext_resp_mask {
-	MLX5_IB_ALLOC_UCONTEXT_RESP_MASK_CORE_CLOCK_OFFSET = 1UL << 0,
-};
-
-enum mlx5_user_cmds_supp_uhw {
-	MLX5_USER_CMDS_SUPP_UHW_QUERY_DEVICE = 1 << 0,
-};
-
-struct mlx5_ib_alloc_ucontext_resp {
-	__u32	qp_tab_size;
-	__u32	bf_reg_size;
-	__u32	tot_uuars;
-	__u32	cache_line_size;
-	__u16	max_sq_desc_sz;
-	__u16	max_rq_desc_sz;
-	__u32	max_send_wqebb;
-	__u32	max_recv_wr;
-	__u32	max_srq_recv_wr;
-	__u16	num_ports;
-	__u16	reserved1;
-	__u32	comp_mask;
-	__u32	response_length;
-	__u8	cqe_version;
-	__u8	cmds_supp_uhw;
-	__u16	reserved2;
-	__u64	hca_core_clock_offset;
-};
-
-struct mlx5_ib_alloc_pd_resp {
-	__u32	pdn;
-};
-
-struct mlx5_ib_tso_caps {
-	__u32 max_tso; /* Maximum tso payload size in bytes */
-
-	/* Corresponding bit will be set if qp type from
-	 * 'enum ib_qp_type' is supported, e.g.
-	 * supported_qpts |= 1 << IB_QPT_UD
-	 */
-	__u32 supported_qpts;
-};
-
-struct mlx5_ib_query_device_resp {
-	__u32	comp_mask;
-	__u32	response_length;
-	struct	mlx5_ib_tso_caps tso_caps;
-};
-
-struct mlx5_ib_create_cq {
-	__u64	buf_addr;
-	__u64	db_addr;
-	__u32	cqe_size;
-	__u32	reserved; /* explicit padding (optional on i386) */
-};
-
-struct mlx5_ib_create_cq_resp {
-	__u32	cqn;
-	__u32	reserved;
-};
-
-struct mlx5_ib_resize_cq {
-	__u64	buf_addr;
-	__u16	cqe_size;
-	__u16	reserved0;
-	__u32	reserved1;
-};
-
-struct mlx5_ib_create_srq {
-	__u64	buf_addr;
-	__u64	db_addr;
-	__u32	flags;
-	__u32	reserved0; /* explicit padding (optional on i386) */
-	__u32	uidx;
-	__u32	reserved1;
-};
-
-struct mlx5_ib_create_srq_resp {
-	__u32	srqn;
-	__u32	reserved;
-};
-
-struct mlx5_ib_create_qp {
-	__u64	buf_addr;
-	__u64	db_addr;
-	__u32	sq_wqe_count;
-	__u32	rq_wqe_count;
-	__u32	rq_wqe_shift;
-	__u32	flags;
-	__u32	uidx;
-	__u32	reserved0;
-	__u64	sq_buf_addr;
-};
-
-/* RX Hash function flags */
-enum mlx5_rx_hash_function_flags {
-	MLX5_RX_HASH_FUNC_TOEPLITZ	= 1 << 0,
-};
-
-/*
- * RX Hash flags, these flags allows to set which incoming packet's field should
- * participates in RX Hash. Each flag represent certain packet's field,
- * when the flag is set the field that is represented by the flag will
- * participate in RX Hash calculation.
- * Note: *IPV4 and *IPV6 flags can't be enabled together on the same QP
- * and *TCP and *UDP flags can't be enabled together on the same QP.
-*/
-enum mlx5_rx_hash_fields {
-	MLX5_RX_HASH_SRC_IPV4	= 1 << 0,
-	MLX5_RX_HASH_DST_IPV4	= 1 << 1,
-	MLX5_RX_HASH_SRC_IPV6	= 1 << 2,
-	MLX5_RX_HASH_DST_IPV6	= 1 << 3,
-	MLX5_RX_HASH_SRC_PORT_TCP	= 1 << 4,
-	MLX5_RX_HASH_DST_PORT_TCP	= 1 << 5,
-	MLX5_RX_HASH_SRC_PORT_UDP	= 1 << 6,
-	MLX5_RX_HASH_DST_PORT_UDP	= 1 << 7
-};
-
-struct mlx5_ib_create_qp_rss {
-	__u64 rx_hash_fields_mask; /* enum mlx5_rx_hash_fields */
-	__u8 rx_hash_function; /* enum mlx5_rx_hash_function_flags */
-	__u8 rx_key_len; /* valid only for Toeplitz */
-	__u8 reserved[6];
-	__u8 rx_hash_key[128]; /* valid only for Toeplitz */
-	__u32   comp_mask;
-	__u32   reserved1;
-};
-
-struct mlx5_ib_create_qp_resp {
-	__u32	uuar_index;
-};
-
-struct mlx5_ib_alloc_mw {
-	__u32	comp_mask;
-	__u8	num_klms;
-	__u8	reserved1;
-	__u16	reserved2;
-};
-
-struct mlx5_ib_create_wq {
-	__u64   buf_addr;
-	__u64   db_addr;
-	__u32   rq_wqe_count;
-	__u32   rq_wqe_shift;
-	__u32   user_index;
-	__u32   flags;
-	__u32   comp_mask;
-	__u32   reserved;
-};
-
-struct mlx5_ib_create_wq_resp {
-	__u32	response_length;
-	__u32	reserved;
-};
-
-struct mlx5_ib_create_rwq_ind_tbl_resp {
-	__u32	response_length;
-	__u32	reserved;
-};
-
-struct mlx5_ib_modify_wq {
-	__u32	comp_mask;
-	__u32	reserved;
-};
-
-static inline int get_qp_user_index(struct mlx5_ib_ucontext *ucontext,
-				    struct mlx5_ib_create_qp *ucmd,
-				    int inlen,
-				    u32 *user_index)
-{
-	u8 cqe_version = ucontext->cqe_version;
-
-	if (field_avail(struct mlx5_ib_create_qp, uidx, inlen) &&
-	    !cqe_version && (ucmd->uidx == MLX5_IB_DEFAULT_UIDX))
-		return 0;
-
-	if (!!(field_avail(struct mlx5_ib_create_qp, uidx, inlen) !=
-	       !!cqe_version))
-		return -EINVAL;
-
-	return verify_assign_uidx(cqe_version, ucmd->uidx, user_index);
-}
-
-static inline int get_srq_user_index(struct mlx5_ib_ucontext *ucontext,
-				     struct mlx5_ib_create_srq *ucmd,
-				     int inlen,
-				     u32 *user_index)
-{
-	u8 cqe_version = ucontext->cqe_version;
-
-	if (field_avail(struct mlx5_ib_create_srq, uidx, inlen) &&
-	    !cqe_version && (ucmd->uidx == MLX5_IB_DEFAULT_UIDX))
-		return 0;
-
-	if (!!(field_avail(struct mlx5_ib_create_srq, uidx, inlen) !=
-	       !!cqe_version))
-		return -EINVAL;
-
-	return verify_assign_uidx(cqe_version, ucmd->uidx, user_index);
-}
-#endif /* MLX5_IB_USER_H */
diff --git a/drivers/infiniband/hw/mthca/mthca_catas.c b/drivers/infiniband/hw/mthca/mthca_catas.c
index 712d2a30fbe5..f6474c24f193 100644
--- a/drivers/infiniband/hw/mthca/mthca_catas.c
+++ b/drivers/infiniband/hw/mthca/mthca_catas.c
@@ -187,7 +187,7 @@ int __init mthca_catas_init(void)
 {
 	INIT_WORK(&catas_work, catas_reset);
 
-	catas_wq = create_singlethread_workqueue("mthca_catas");
+	catas_wq = alloc_ordered_workqueue("mthca_catas", WQ_MEM_RECLAIM);
 	if (!catas_wq)
 		return -ENOMEM;
 
diff --git a/drivers/infiniband/hw/mthca/mthca_mad.c b/drivers/infiniband/hw/mthca/mthca_mad.c
index 7c3f2fb44ba5..9139405c4810 100644
--- a/drivers/infiniband/hw/mthca/mthca_mad.c
+++ b/drivers/infiniband/hw/mthca/mthca_mad.c
@@ -153,7 +153,8 @@ static void node_desc_override(struct ib_device *dev,
 	    mad->mad_hdr.method == IB_MGMT_METHOD_GET_RESP &&
 	    mad->mad_hdr.attr_id == IB_SMP_ATTR_NODE_DESC) {
 		mutex_lock(&to_mdev(dev)->cap_mask_mutex);
-		memcpy(((struct ib_smp *) mad)->data, dev->node_desc, 64);
+		memcpy(((struct ib_smp *) mad)->data, dev->node_desc,
+		       IB_DEVICE_NODE_DESC_MAX);
 		mutex_unlock(&to_mdev(dev)->cap_mask_mutex);
 	}
 }
diff --git a/drivers/infiniband/hw/mthca/mthca_memfree.c b/drivers/infiniband/hw/mthca/mthca_memfree.c
index 6c00d04b8b28..c6fe89d79248 100644
--- a/drivers/infiniband/hw/mthca/mthca_memfree.c
+++ b/drivers/infiniband/hw/mthca/mthca_memfree.c
@@ -472,7 +472,7 @@ int mthca_map_user_db(struct mthca_dev *dev, struct mthca_uar *uar,
 		goto out;
 	}
 
-	ret = get_user_pages(uaddr & PAGE_MASK, 1, 1, 0, pages, NULL);
+	ret = get_user_pages(uaddr & PAGE_MASK, 1, FOLL_WRITE, pages, NULL);
 	if (ret < 0)
 		goto out;
 
diff --git a/drivers/infiniband/hw/mthca/mthca_provider.c b/drivers/infiniband/hw/mthca/mthca_provider.c
index da2335f7f7c3..358930a41e36 100644
--- a/drivers/infiniband/hw/mthca/mthca_provider.c
+++ b/drivers/infiniband/hw/mthca/mthca_provider.c
@@ -46,7 +46,7 @@
 
 #include "mthca_dev.h"
 #include "mthca_cmd.h"
-#include "mthca_user.h"
+#include <rdma/mthca-abi.h>
 #include "mthca_memfree.h"
 
 static void init_query_mad(struct ib_smp *mad)
@@ -193,7 +193,8 @@ static int mthca_modify_device(struct ib_device *ibdev,
 	if (mask & IB_DEVICE_MODIFY_NODE_DESC) {
 		if (mutex_lock_interruptible(&to_mdev(ibdev)->cap_mask_mutex))
 			return -ERESTARTSYS;
-		memcpy(ibdev->node_desc, props->node_desc, 64);
+		memcpy(ibdev->node_desc, props->node_desc,
+		       IB_DEVICE_NODE_DESC_MAX);
 		mutex_unlock(&to_mdev(ibdev)->cap_mask_mutex);
 	}
 
@@ -1138,7 +1139,7 @@ static int mthca_init_node_data(struct mthca_dev *dev)
 	if (err)
 		goto out;
 
-	memcpy(dev->ib_dev.node_desc, out_mad->data, 64);
+	memcpy(dev->ib_dev.node_desc, out_mad->data, IB_DEVICE_NODE_DESC_MAX);
 
 	in_mad->attr_id = IB_SMP_ATTR_NODE_INFO;
 
diff --git a/drivers/infiniband/hw/nes/nes.h b/drivers/infiniband/hw/nes/nes.h
index bd9d132f11c7..e7430c9254d3 100644
--- a/drivers/infiniband/hw/nes/nes.h
+++ b/drivers/infiniband/hw/nes/nes.h
@@ -165,7 +165,7 @@ do { \
 #include "nes_hw.h"
 #include "nes_verbs.h"
 #include "nes_context.h"
-#include "nes_user.h"
+#include <rdma/nes-abi.h>
 #include "nes_cm.h"
 #include "nes_mgt.h"
 
diff --git a/drivers/infiniband/hw/nes/nes_cm.c b/drivers/infiniband/hw/nes/nes_cm.c
index 7f0aa23aef9d..57db9b332f44 100644
--- a/drivers/infiniband/hw/nes/nes_cm.c
+++ b/drivers/infiniband/hw/nes/nes_cm.c
@@ -2692,12 +2692,12 @@ static struct nes_cm_core *nes_cm_alloc_core(void)
 	nes_debug(NES_DBG_CM, "Init CM Core completed -- cm_core=%p\n", cm_core);
 
 	nes_debug(NES_DBG_CM, "Enable QUEUE EVENTS\n");
-	cm_core->event_wq = create_singlethread_workqueue("nesewq");
+	cm_core->event_wq = alloc_ordered_workqueue("nesewq", 0);
 	if (!cm_core->event_wq)
 		goto out_free_cmcore;
 	cm_core->post_event = nes_cm_post_event;
 	nes_debug(NES_DBG_CM, "Enable QUEUE DISCONNECTS\n");
-	cm_core->disconn_wq = create_singlethread_workqueue("nesdwq");
+	cm_core->disconn_wq = alloc_ordered_workqueue("nesdwq", 0);
 	if (!cm_core->disconn_wq)
 		goto out_free_wq;
 
diff --git a/drivers/infiniband/hw/nes/nes_user.h b/drivers/infiniband/hw/nes/nes_user.h
deleted file mode 100644
index 529c421bb15c..000000000000
--- a/drivers/infiniband/hw/nes/nes_user.h
+++ /dev/null
@@ -1,114 +0,0 @@
-/*
- * Copyright (c) 2006 - 2011 Intel Corporation.  All rights reserved.
- * Copyright (c) 2005 Topspin Communications.  All rights reserved.
- * Copyright (c) 2005 Cisco Systems.  All rights reserved.
- * Copyright (c) 2005 Open Grid Computing, Inc. All rights reserved.
- *
- * This software is available to you under a choice of one of two
- * licenses.  You may choose to be licensed under the terms of the GNU
- * General Public License (GPL) Version 2, available from the file
- * 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 NES_USER_H
-#define NES_USER_H
-
-#include <linux/types.h>
-
-#define NES_ABI_USERSPACE_VER 2
-#define NES_ABI_KERNEL_VER    2
-
-/*
- * Make sure that all structs defined in this file remain laid out so
- * that they pack the same way on 32-bit and 64-bit architectures (to
- * avoid incompatibility between 32-bit userspace and 64-bit kernels).
- * In particular do not use pointer types -- pass pointers in __u64
- * instead.
- */
-
-struct nes_alloc_ucontext_req {
-	__u32 reserved32;
-	__u8  userspace_ver;
-	__u8  reserved8[3];
-};
-
-struct nes_alloc_ucontext_resp {
-	__u32 max_pds; /* maximum pds allowed for this user process */
-	__u32 max_qps; /* maximum qps allowed for this user process */
-	__u32 wq_size; /* size of the WQs (sq+rq) allocated to the mmaped area */
-	__u8  virtwq;  /* flag to indicate if virtual WQ are to be used or not */
-	__u8  kernel_ver;
-	__u8  reserved[2];
-};
-
-struct nes_alloc_pd_resp {
-	__u32 pd_id;
-	__u32 mmap_db_index;
-};
-
-struct nes_create_cq_req {
-	__u64 user_cq_buffer;
-	__u32 mcrqf;
-	__u8 reserved[4];
-};
-
-struct nes_create_qp_req {
-	__u64 user_wqe_buffers;
-	__u64 user_qp_buffer;
-};
-
-enum iwnes_memreg_type {
-	IWNES_MEMREG_TYPE_MEM = 0x0000,
-	IWNES_MEMREG_TYPE_QP = 0x0001,
-	IWNES_MEMREG_TYPE_CQ = 0x0002,
-	IWNES_MEMREG_TYPE_MW = 0x0003,
-	IWNES_MEMREG_TYPE_FMR = 0x0004,
-	IWNES_MEMREG_TYPE_FMEM = 0x0005,
-};
-
-struct nes_mem_reg_req {
-	__u32 reg_type;	/* indicates if id is memory, QP or CQ */
-	__u32 reserved;
-};
-
-struct nes_create_cq_resp {
-	__u32 cq_id;
-	__u32 cq_size;
-	__u32 mmap_db_index;
-	__u32 reserved;
-};
-
-struct nes_create_qp_resp {
-	__u32 qp_id;
-	__u32 actual_sq_size;
-	__u32 actual_rq_size;
-	__u32 mmap_sq_db_index;
-	__u32 mmap_rq_db_index;
-	__u32 nes_drv_opt;
-};
-
-#endif				/* NES_USER_H */
diff --git a/drivers/infiniband/hw/ocrdma/ocrdma_abi.h b/drivers/infiniband/hw/ocrdma/ocrdma_abi.h
deleted file mode 100644
index 430b1350fe96..000000000000
--- a/drivers/infiniband/hw/ocrdma/ocrdma_abi.h
+++ /dev/null
@@ -1,149 +0,0 @@
-/* This file is part of the Emulex RoCE Device Driver for
- * RoCE (RDMA over Converged Ethernet) adapters.
- * Copyright (C) 2012-2015 Emulex. All rights reserved.
- * EMULEX and SLI are trademarks of Emulex.
- * www.emulex.com
- *
- * 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 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.
- *
- * 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 HOLDER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
- * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
- * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
- * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
- * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- *
- * Contact Information:
- * linux-drivers@emulex.com
- *
- * Emulex
- * 3333 Susan Street
- * Costa Mesa, CA 92626
- */
-
-#ifndef __OCRDMA_ABI_H__
-#define __OCRDMA_ABI_H__
-
-#define OCRDMA_ABI_VERSION 2
-#define OCRDMA_BE_ROCE_ABI_VERSION 1
-/* user kernel communication data structures. */
-
-struct ocrdma_alloc_ucontext_resp {
-	u32 dev_id;
-	u32 wqe_size;
-	u32 max_inline_data;
-	u32 dpp_wqe_size;
-	u64 ah_tbl_page;
-	u32 ah_tbl_len;
-	u32 rqe_size;
-	u8 fw_ver[32];
-	/* for future use/new features in progress */
-	u64 rsvd1;
-	u64 rsvd2;
-};
-
-struct ocrdma_alloc_pd_ureq {
-	u64 rsvd1;
-};
-
-struct ocrdma_alloc_pd_uresp {
-	u32 id;
-	u32 dpp_enabled;
-	u32 dpp_page_addr_hi;
-	u32 dpp_page_addr_lo;
-	u64 rsvd1;
-};
-
-struct ocrdma_create_cq_ureq {
-	u32 dpp_cq;
-	u32 rsvd; /* pad */
-};
-
-#define MAX_CQ_PAGES 8
-struct ocrdma_create_cq_uresp {
-	u32 cq_id;
-	u32 page_size;
-	u32 num_pages;
-	u32 max_hw_cqe;
-	u64 page_addr[MAX_CQ_PAGES];
-	u64 db_page_addr;
-	u32 db_page_size;
-	u32 phase_change;
-	/* for future use/new features in progress */
-	u64 rsvd1;
-	u64 rsvd2;
-};
-
-#define MAX_QP_PAGES 8
-#define MAX_UD_AV_PAGES 8
-
-struct ocrdma_create_qp_ureq {
-	u8 enable_dpp_cq;
-	u8 rsvd;
-	u16 dpp_cq_id;
-	u32 rsvd1;	/* pad */
-};
-
-struct ocrdma_create_qp_uresp {
-	u16 qp_id;
-	u16 sq_dbid;
-	u16 rq_dbid;
-	u16 resv0;	/* pad */
-	u32 sq_page_size;
-	u32 rq_page_size;
-	u32 num_sq_pages;
-	u32 num_rq_pages;
-	u64 sq_page_addr[MAX_QP_PAGES];
-	u64 rq_page_addr[MAX_QP_PAGES];
-	u64 db_page_addr;
-	u32 db_page_size;
-	u32 dpp_credit;
-	u32 dpp_offset;
-	u32 num_wqe_allocated;
-	u32 num_rqe_allocated;
-	u32 db_sq_offset;
-	u32 db_rq_offset;
-	u32 db_shift;
-	u64 rsvd[11];
-} __packed;
-
-struct ocrdma_create_srq_uresp {
-	u16 rq_dbid;
-	u16 resv0;	/* pad */
-	u32 resv1;
-
-	u32 rq_page_size;
-	u32 num_rq_pages;
-
-	u64 rq_page_addr[MAX_QP_PAGES];
-	u64 db_page_addr;
-
-	u32 db_page_size;
-	u32 num_rqe_allocated;
-	u32 db_rq_offset;
-	u32 db_shift;
-
-	u64 rsvd2;
-	u64 rsvd3;
-};
-
-#endif				/* __OCRDMA_ABI_H__ */
diff --git a/drivers/infiniband/hw/ocrdma/ocrdma_main.c b/drivers/infiniband/hw/ocrdma/ocrdma_main.c
index 07d0c6c5b046..896071502739 100644
--- a/drivers/infiniband/hw/ocrdma/ocrdma_main.c
+++ b/drivers/infiniband/hw/ocrdma/ocrdma_main.c
@@ -56,7 +56,7 @@
 #include "be_roce.h"
 #include "ocrdma_hw.h"
 #include "ocrdma_stats.h"
-#include "ocrdma_abi.h"
+#include <rdma/ocrdma-abi.h>
 
 MODULE_VERSION(OCRDMA_ROCE_DRV_VERSION);
 MODULE_DESCRIPTION(OCRDMA_ROCE_DRV_DESC " " OCRDMA_ROCE_DRV_VERSION);
@@ -119,6 +119,7 @@ static int ocrdma_register_device(struct ocrdma_dev *dev)
 {
 	strlcpy(dev->ibdev.name, "ocrdma%d", IB_DEVICE_NAME_MAX);
 	ocrdma_get_guid(dev, (u8 *)&dev->ibdev.node_guid);
+	BUILD_BUG_ON(sizeof(OCRDMA_NODE_DESC) > IB_DEVICE_NODE_DESC_MAX);
 	memcpy(dev->ibdev.node_desc, OCRDMA_NODE_DESC,
 	       sizeof(OCRDMA_NODE_DESC));
 	dev->ibdev.owner = THIS_MODULE;
diff --git a/drivers/infiniband/hw/ocrdma/ocrdma_verbs.c b/drivers/infiniband/hw/ocrdma/ocrdma_verbs.c
index 0aa854737e74..6af44f8db3d5 100644
--- a/drivers/infiniband/hw/ocrdma/ocrdma_verbs.c
+++ b/drivers/infiniband/hw/ocrdma/ocrdma_verbs.c
@@ -51,7 +51,7 @@
 #include "ocrdma.h"
 #include "ocrdma_hw.h"
 #include "ocrdma_verbs.h"
-#include "ocrdma_abi.h"
+#include <rdma/ocrdma-abi.h>
 
 int ocrdma_query_pkey(struct ib_device *ibdev, u8 port, u16 index, u16 *pkey)
 {
diff --git a/drivers/infiniband/hw/qedr/Kconfig b/drivers/infiniband/hw/qedr/Kconfig
new file mode 100644
index 000000000000..7c06d85568d4
--- /dev/null
+++ b/drivers/infiniband/hw/qedr/Kconfig
@@ -0,0 +1,7 @@
+config INFINIBAND_QEDR
+	tristate "QLogic RoCE driver"
+	depends on 64BIT && QEDE
+	select QED_LL2
+	---help---
+	  This driver provides low-level InfiniBand over Ethernet
+	  support for QLogic QED host channel adapters (HCAs).
diff --git a/drivers/infiniband/hw/qedr/Makefile b/drivers/infiniband/hw/qedr/Makefile
new file mode 100644
index 000000000000..ba7067c77f2f
--- /dev/null
+++ b/drivers/infiniband/hw/qedr/Makefile
@@ -0,0 +1,3 @@
+obj-$(CONFIG_INFINIBAND_QEDR) := qedr.o
+
+qedr-y := main.o verbs.o qedr_cm.o
diff --git a/drivers/infiniband/hw/qedr/main.c b/drivers/infiniband/hw/qedr/main.c
new file mode 100644
index 000000000000..7b74d09a8217
--- /dev/null
+++ b/drivers/infiniband/hw/qedr/main.c
@@ -0,0 +1,914 @@
+/* QLogic qedr NIC Driver
+ * Copyright (c) 2015-2016  QLogic Corporation
+ *
+ * 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/module.h>
+#include <rdma/ib_verbs.h>
+#include <rdma/ib_addr.h>
+#include <rdma/ib_user_verbs.h>
+#include <linux/netdevice.h>
+#include <linux/iommu.h>
+#include <net/addrconf.h>
+#include <linux/qed/qede_roce.h>
+#include <linux/qed/qed_chain.h>
+#include <linux/qed/qed_if.h>
+#include "qedr.h"
+#include "verbs.h"
+#include <rdma/qedr-abi.h>
+
+MODULE_DESCRIPTION("QLogic 40G/100G ROCE Driver");
+MODULE_AUTHOR("QLogic Corporation");
+MODULE_LICENSE("Dual BSD/GPL");
+MODULE_VERSION(QEDR_MODULE_VERSION);
+
+#define QEDR_WQ_MULTIPLIER_DFT	(3)
+
+void qedr_ib_dispatch_event(struct qedr_dev *dev, u8 port_num,
+			    enum ib_event_type type)
+{
+	struct ib_event ibev;
+
+	ibev.device = &dev->ibdev;
+	ibev.element.port_num = port_num;
+	ibev.event = type;
+
+	ib_dispatch_event(&ibev);
+}
+
+static enum rdma_link_layer qedr_link_layer(struct ib_device *device,
+					    u8 port_num)
+{
+	return IB_LINK_LAYER_ETHERNET;
+}
+
+static void qedr_get_dev_fw_str(struct ib_device *ibdev, char *str,
+				size_t str_len)
+{
+	struct qedr_dev *qedr = get_qedr_dev(ibdev);
+	u32 fw_ver = (u32)qedr->attr.fw_ver;
+
+	snprintf(str, str_len, "%d. %d. %d. %d",
+		 (fw_ver >> 24) & 0xFF, (fw_ver >> 16) & 0xFF,
+		 (fw_ver >> 8) & 0xFF, fw_ver & 0xFF);
+}
+
+static struct net_device *qedr_get_netdev(struct ib_device *dev, u8 port_num)
+{
+	struct qedr_dev *qdev;
+
+	qdev = get_qedr_dev(dev);
+	dev_hold(qdev->ndev);
+
+	/* The HW vendor's device driver must guarantee
+	 * that this function returns NULL before the net device reaches
+	 * NETDEV_UNREGISTER_FINAL state.
+	 */
+	return qdev->ndev;
+}
+
+static int qedr_register_device(struct qedr_dev *dev)
+{
+	strlcpy(dev->ibdev.name, "qedr%d", IB_DEVICE_NAME_MAX);
+
+	dev->ibdev.node_guid = dev->attr.node_guid;
+	memcpy(dev->ibdev.node_desc, QEDR_NODE_DESC, sizeof(QEDR_NODE_DESC));
+	dev->ibdev.owner = THIS_MODULE;
+	dev->ibdev.uverbs_abi_ver = QEDR_ABI_VERSION;
+
+	dev->ibdev.uverbs_cmd_mask = QEDR_UVERBS(GET_CONTEXT) |
+				     QEDR_UVERBS(QUERY_DEVICE) |
+				     QEDR_UVERBS(QUERY_PORT) |
+				     QEDR_UVERBS(ALLOC_PD) |
+				     QEDR_UVERBS(DEALLOC_PD) |
+				     QEDR_UVERBS(CREATE_COMP_CHANNEL) |
+				     QEDR_UVERBS(CREATE_CQ) |
+				     QEDR_UVERBS(RESIZE_CQ) |
+				     QEDR_UVERBS(DESTROY_CQ) |
+				     QEDR_UVERBS(REQ_NOTIFY_CQ) |
+				     QEDR_UVERBS(CREATE_QP) |
+				     QEDR_UVERBS(MODIFY_QP) |
+				     QEDR_UVERBS(QUERY_QP) |
+				     QEDR_UVERBS(DESTROY_QP) |
+				     QEDR_UVERBS(REG_MR) |
+				     QEDR_UVERBS(DEREG_MR) |
+				     QEDR_UVERBS(POLL_CQ) |
+				     QEDR_UVERBS(POST_SEND) |
+				     QEDR_UVERBS(POST_RECV);
+
+	dev->ibdev.phys_port_cnt = 1;
+	dev->ibdev.num_comp_vectors = dev->num_cnq;
+	dev->ibdev.node_type = RDMA_NODE_IB_CA;
+
+	dev->ibdev.query_device = qedr_query_device;
+	dev->ibdev.query_port = qedr_query_port;
+	dev->ibdev.modify_port = qedr_modify_port;
+
+	dev->ibdev.query_gid = qedr_query_gid;
+	dev->ibdev.add_gid = qedr_add_gid;
+	dev->ibdev.del_gid = qedr_del_gid;
+
+	dev->ibdev.alloc_ucontext = qedr_alloc_ucontext;
+	dev->ibdev.dealloc_ucontext = qedr_dealloc_ucontext;
+	dev->ibdev.mmap = qedr_mmap;
+
+	dev->ibdev.alloc_pd = qedr_alloc_pd;
+	dev->ibdev.dealloc_pd = qedr_dealloc_pd;
+
+	dev->ibdev.create_cq = qedr_create_cq;
+	dev->ibdev.destroy_cq = qedr_destroy_cq;
+	dev->ibdev.resize_cq = qedr_resize_cq;
+	dev->ibdev.req_notify_cq = qedr_arm_cq;
+
+	dev->ibdev.create_qp = qedr_create_qp;
+	dev->ibdev.modify_qp = qedr_modify_qp;
+	dev->ibdev.query_qp = qedr_query_qp;
+	dev->ibdev.destroy_qp = qedr_destroy_qp;
+
+	dev->ibdev.query_pkey = qedr_query_pkey;
+
+	dev->ibdev.create_ah = qedr_create_ah;
+	dev->ibdev.destroy_ah = qedr_destroy_ah;
+
+	dev->ibdev.get_dma_mr = qedr_get_dma_mr;
+	dev->ibdev.dereg_mr = qedr_dereg_mr;
+	dev->ibdev.reg_user_mr = qedr_reg_user_mr;
+	dev->ibdev.alloc_mr = qedr_alloc_mr;
+	dev->ibdev.map_mr_sg = qedr_map_mr_sg;
+
+	dev->ibdev.poll_cq = qedr_poll_cq;
+	dev->ibdev.post_send = qedr_post_send;
+	dev->ibdev.post_recv = qedr_post_recv;
+
+	dev->ibdev.process_mad = qedr_process_mad;
+	dev->ibdev.get_port_immutable = qedr_port_immutable;
+	dev->ibdev.get_netdev = qedr_get_netdev;
+
+	dev->ibdev.dma_device = &dev->pdev->dev;
+
+	dev->ibdev.get_link_layer = qedr_link_layer;
+	dev->ibdev.get_dev_fw_str = qedr_get_dev_fw_str;
+
+	return ib_register_device(&dev->ibdev, NULL);
+}
+
+/* This function allocates fast-path status block memory */
+static int qedr_alloc_mem_sb(struct qedr_dev *dev,
+			     struct qed_sb_info *sb_info, u16 sb_id)
+{
+	struct status_block *sb_virt;
+	dma_addr_t sb_phys;
+	int rc;
+
+	sb_virt = dma_alloc_coherent(&dev->pdev->dev,
+				     sizeof(*sb_virt), &sb_phys, GFP_KERNEL);
+	if (!sb_virt)
+		return -ENOMEM;
+
+	rc = dev->ops->common->sb_init(dev->cdev, sb_info,
+				       sb_virt, sb_phys, sb_id,
+				       QED_SB_TYPE_CNQ);
+	if (rc) {
+		pr_err("Status block initialization failed\n");
+		dma_free_coherent(&dev->pdev->dev, sizeof(*sb_virt),
+				  sb_virt, sb_phys);
+		return rc;
+	}
+
+	return 0;
+}
+
+static void qedr_free_mem_sb(struct qedr_dev *dev,
+			     struct qed_sb_info *sb_info, int sb_id)
+{
+	if (sb_info->sb_virt) {
+		dev->ops->common->sb_release(dev->cdev, sb_info, sb_id);
+		dma_free_coherent(&dev->pdev->dev, sizeof(*sb_info->sb_virt),
+				  (void *)sb_info->sb_virt, sb_info->sb_phys);
+	}
+}
+
+static void qedr_free_resources(struct qedr_dev *dev)
+{
+	int i;
+
+	for (i = 0; i < dev->num_cnq; i++) {
+		qedr_free_mem_sb(dev, &dev->sb_array[i], dev->sb_start + i);
+		dev->ops->common->chain_free(dev->cdev, &dev->cnq_array[i].pbl);
+	}
+
+	kfree(dev->cnq_array);
+	kfree(dev->sb_array);
+	kfree(dev->sgid_tbl);
+}
+
+static int qedr_alloc_resources(struct qedr_dev *dev)
+{
+	struct qedr_cnq *cnq;
+	__le16 *cons_pi;
+	u16 n_entries;
+	int i, rc;
+
+	dev->sgid_tbl = kzalloc(sizeof(union ib_gid) *
+				QEDR_MAX_SGID, GFP_KERNEL);
+	if (!dev->sgid_tbl)
+		return -ENOMEM;
+
+	spin_lock_init(&dev->sgid_lock);
+
+	/* Allocate Status blocks for CNQ */
+	dev->sb_array = kcalloc(dev->num_cnq, sizeof(*dev->sb_array),
+				GFP_KERNEL);
+	if (!dev->sb_array) {
+		rc = -ENOMEM;
+		goto err1;
+	}
+
+	dev->cnq_array = kcalloc(dev->num_cnq,
+				 sizeof(*dev->cnq_array), GFP_KERNEL);
+	if (!dev->cnq_array) {
+		rc = -ENOMEM;
+		goto err2;
+	}
+
+	dev->sb_start = dev->ops->rdma_get_start_sb(dev->cdev);
+
+	/* Allocate CNQ PBLs */
+	n_entries = min_t(u32, QED_RDMA_MAX_CNQ_SIZE, QEDR_ROCE_MAX_CNQ_SIZE);
+	for (i = 0; i < dev->num_cnq; i++) {
+		cnq = &dev->cnq_array[i];
+
+		rc = qedr_alloc_mem_sb(dev, &dev->sb_array[i],
+				       dev->sb_start + i);
+		if (rc)
+			goto err3;
+
+		rc = dev->ops->common->chain_alloc(dev->cdev,
+						   QED_CHAIN_USE_TO_CONSUME,
+						   QED_CHAIN_MODE_PBL,
+						   QED_CHAIN_CNT_TYPE_U16,
+						   n_entries,
+						   sizeof(struct regpair *),
+						   &cnq->pbl);
+		if (rc)
+			goto err4;
+
+		cnq->dev = dev;
+		cnq->sb = &dev->sb_array[i];
+		cons_pi = dev->sb_array[i].sb_virt->pi_array;
+		cnq->hw_cons_ptr = &cons_pi[QED_ROCE_PROTOCOL_INDEX];
+		cnq->index = i;
+		sprintf(cnq->name, "qedr%d@pci:%s", i, pci_name(dev->pdev));
+
+		DP_DEBUG(dev, QEDR_MSG_INIT, "cnq[%d].cons=%d\n",
+			 i, qed_chain_get_cons_idx(&cnq->pbl));
+	}
+
+	return 0;
+err4:
+	qedr_free_mem_sb(dev, &dev->sb_array[i], dev->sb_start + i);
+err3:
+	for (--i; i >= 0; i--) {
+		dev->ops->common->chain_free(dev->cdev, &dev->cnq_array[i].pbl);
+		qedr_free_mem_sb(dev, &dev->sb_array[i], dev->sb_start + i);
+	}
+	kfree(dev->cnq_array);
+err2:
+	kfree(dev->sb_array);
+err1:
+	kfree(dev->sgid_tbl);
+	return rc;
+}
+
+/* QEDR sysfs interface */
+static ssize_t show_rev(struct device *device, struct device_attribute *attr,
+			char *buf)
+{
+	struct qedr_dev *dev = dev_get_drvdata(device);
+
+	return scnprintf(buf, PAGE_SIZE, "0x%x\n", dev->pdev->vendor);
+}
+
+static ssize_t show_hca_type(struct device *device,
+			     struct device_attribute *attr, char *buf)
+{
+	return scnprintf(buf, PAGE_SIZE, "%s\n", "HCA_TYPE_TO_SET");
+}
+
+static DEVICE_ATTR(hw_rev, S_IRUGO, show_rev, NULL);
+static DEVICE_ATTR(hca_type, S_IRUGO, show_hca_type, NULL);
+
+static struct device_attribute *qedr_attributes[] = {
+	&dev_attr_hw_rev,
+	&dev_attr_hca_type
+};
+
+static void qedr_remove_sysfiles(struct qedr_dev *dev)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(qedr_attributes); i++)
+		device_remove_file(&dev->ibdev.dev, qedr_attributes[i]);
+}
+
+static void qedr_pci_set_atomic(struct qedr_dev *dev, struct pci_dev *pdev)
+{
+	struct pci_dev *bridge;
+	u32 val;
+
+	dev->atomic_cap = IB_ATOMIC_NONE;
+
+	bridge = pdev->bus->self;
+	if (!bridge)
+		return;
+
+	/* Check whether we are connected directly or via a switch */
+	while (bridge && bridge->bus->parent) {
+		DP_DEBUG(dev, QEDR_MSG_INIT,
+			 "Device is not connected directly to root. bridge->bus->number=%d primary=%d\n",
+			 bridge->bus->number, bridge->bus->primary);
+		/* Need to check Atomic Op Routing Supported all the way to
+		 * root complex.
+		 */
+		pcie_capability_read_dword(bridge, PCI_EXP_DEVCAP2, &val);
+		if (!(val & PCI_EXP_DEVCAP2_ATOMIC_ROUTE)) {
+			pcie_capability_clear_word(pdev,
+						   PCI_EXP_DEVCTL2,
+						   PCI_EXP_DEVCTL2_ATOMIC_REQ);
+			return;
+		}
+		bridge = bridge->bus->parent->self;
+	}
+	bridge = pdev->bus->self;
+
+	/* according to bridge capability */
+	pcie_capability_read_dword(bridge, PCI_EXP_DEVCAP2, &val);
+	if (val & PCI_EXP_DEVCAP2_ATOMIC_COMP64) {
+		pcie_capability_set_word(pdev, PCI_EXP_DEVCTL2,
+					 PCI_EXP_DEVCTL2_ATOMIC_REQ);
+		dev->atomic_cap = IB_ATOMIC_GLOB;
+	} else {
+		pcie_capability_clear_word(pdev, PCI_EXP_DEVCTL2,
+					   PCI_EXP_DEVCTL2_ATOMIC_REQ);
+	}
+}
+
+static const struct qed_rdma_ops *qed_ops;
+
+#define HILO_U64(hi, lo)		((((u64)(hi)) << 32) + (lo))
+
+static irqreturn_t qedr_irq_handler(int irq, void *handle)
+{
+	u16 hw_comp_cons, sw_comp_cons;
+	struct qedr_cnq *cnq = handle;
+	struct regpair *cq_handle;
+	struct qedr_cq *cq;
+
+	qed_sb_ack(cnq->sb, IGU_INT_DISABLE, 0);
+
+	qed_sb_update_sb_idx(cnq->sb);
+
+	hw_comp_cons = le16_to_cpu(*cnq->hw_cons_ptr);
+	sw_comp_cons = qed_chain_get_cons_idx(&cnq->pbl);
+
+	/* Align protocol-index and chain reads */
+	rmb();
+
+	while (sw_comp_cons != hw_comp_cons) {
+		cq_handle = (struct regpair *)qed_chain_consume(&cnq->pbl);
+		cq = (struct qedr_cq *)(uintptr_t)HILO_U64(cq_handle->hi,
+				cq_handle->lo);
+
+		if (cq == NULL) {
+			DP_ERR(cnq->dev,
+			       "Received NULL CQ cq_handle->hi=%d cq_handle->lo=%d sw_comp_cons=%d hw_comp_cons=%d\n",
+			       cq_handle->hi, cq_handle->lo, sw_comp_cons,
+			       hw_comp_cons);
+
+			break;
+		}
+
+		if (cq->sig != QEDR_CQ_MAGIC_NUMBER) {
+			DP_ERR(cnq->dev,
+			       "Problem with cq signature, cq_handle->hi=%d ch_handle->lo=%d cq=%p\n",
+			       cq_handle->hi, cq_handle->lo, cq);
+			break;
+		}
+
+		cq->arm_flags = 0;
+
+		if (cq->ibcq.comp_handler)
+			(*cq->ibcq.comp_handler)
+				(&cq->ibcq, cq->ibcq.cq_context);
+
+		sw_comp_cons = qed_chain_get_cons_idx(&cnq->pbl);
+
+		cnq->n_comp++;
+
+	}
+
+	qed_ops->rdma_cnq_prod_update(cnq->dev->rdma_ctx, cnq->index,
+				      sw_comp_cons);
+
+	qed_sb_ack(cnq->sb, IGU_INT_ENABLE, 1);
+
+	return IRQ_HANDLED;
+}
+
+static void qedr_sync_free_irqs(struct qedr_dev *dev)
+{
+	u32 vector;
+	int i;
+
+	for (i = 0; i < dev->int_info.used_cnt; i++) {
+		if (dev->int_info.msix_cnt) {
+			vector = dev->int_info.msix[i * dev->num_hwfns].vector;
+			synchronize_irq(vector);
+			free_irq(vector, &dev->cnq_array[i]);
+		}
+	}
+
+	dev->int_info.used_cnt = 0;
+}
+
+static int qedr_req_msix_irqs(struct qedr_dev *dev)
+{
+	int i, rc = 0;
+
+	if (dev->num_cnq > dev->int_info.msix_cnt) {
+		DP_ERR(dev,
+		       "Interrupt mismatch: %d CNQ queues > %d MSI-x vectors\n",
+		       dev->num_cnq, dev->int_info.msix_cnt);
+		return -EINVAL;
+	}
+
+	for (i = 0; i < dev->num_cnq; i++) {
+		rc = request_irq(dev->int_info.msix[i * dev->num_hwfns].vector,
+				 qedr_irq_handler, 0, dev->cnq_array[i].name,
+				 &dev->cnq_array[i]);
+		if (rc) {
+			DP_ERR(dev, "Request cnq %d irq failed\n", i);
+			qedr_sync_free_irqs(dev);
+		} else {
+			DP_DEBUG(dev, QEDR_MSG_INIT,
+				 "Requested cnq irq for %s [entry %d]. Cookie is at %p\n",
+				 dev->cnq_array[i].name, i,
+				 &dev->cnq_array[i]);
+			dev->int_info.used_cnt++;
+		}
+	}
+
+	return rc;
+}
+
+static int qedr_setup_irqs(struct qedr_dev *dev)
+{
+	int rc;
+
+	DP_DEBUG(dev, QEDR_MSG_INIT, "qedr_setup_irqs\n");
+
+	/* Learn Interrupt configuration */
+	rc = dev->ops->rdma_set_rdma_int(dev->cdev, dev->num_cnq);
+	if (rc < 0)
+		return rc;
+
+	rc = dev->ops->rdma_get_rdma_int(dev->cdev, &dev->int_info);
+	if (rc) {
+		DP_DEBUG(dev, QEDR_MSG_INIT, "get_rdma_int failed\n");
+		return rc;
+	}
+
+	if (dev->int_info.msix_cnt) {
+		DP_DEBUG(dev, QEDR_MSG_INIT, "rdma msix_cnt = %d\n",
+			 dev->int_info.msix_cnt);
+		rc = qedr_req_msix_irqs(dev);
+		if (rc)
+			return rc;
+	}
+
+	DP_DEBUG(dev, QEDR_MSG_INIT, "qedr_setup_irqs succeeded\n");
+
+	return 0;
+}
+
+static int qedr_set_device_attr(struct qedr_dev *dev)
+{
+	struct qed_rdma_device *qed_attr;
+	struct qedr_device_attr *attr;
+	u32 page_size;
+
+	/* Part 1 - query core capabilities */
+	qed_attr = dev->ops->rdma_query_device(dev->rdma_ctx);
+
+	/* Part 2 - check capabilities */
+	page_size = ~dev->attr.page_size_caps + 1;
+	if (page_size > PAGE_SIZE) {
+		DP_ERR(dev,
+		       "Kernel PAGE_SIZE is %ld which is smaller than minimum page size (%d) required by qedr\n",
+		       PAGE_SIZE, page_size);
+		return -ENODEV;
+	}
+
+	/* Part 3 - copy and update capabilities */
+	attr = &dev->attr;
+	attr->vendor_id = qed_attr->vendor_id;
+	attr->vendor_part_id = qed_attr->vendor_part_id;
+	attr->hw_ver = qed_attr->hw_ver;
+	attr->fw_ver = qed_attr->fw_ver;
+	attr->node_guid = qed_attr->node_guid;
+	attr->sys_image_guid = qed_attr->sys_image_guid;
+	attr->max_cnq = qed_attr->max_cnq;
+	attr->max_sge = qed_attr->max_sge;
+	attr->max_inline = qed_attr->max_inline;
+	attr->max_sqe = min_t(u32, qed_attr->max_wqe, QEDR_MAX_SQE);
+	attr->max_rqe = min_t(u32, qed_attr->max_wqe, QEDR_MAX_RQE);
+	attr->max_qp_resp_rd_atomic_resc = qed_attr->max_qp_resp_rd_atomic_resc;
+	attr->max_qp_req_rd_atomic_resc = qed_attr->max_qp_req_rd_atomic_resc;
+	attr->max_dev_resp_rd_atomic_resc =
+	    qed_attr->max_dev_resp_rd_atomic_resc;
+	attr->max_cq = qed_attr->max_cq;
+	attr->max_qp = qed_attr->max_qp;
+	attr->max_mr = qed_attr->max_mr;
+	attr->max_mr_size = qed_attr->max_mr_size;
+	attr->max_cqe = min_t(u64, qed_attr->max_cqe, QEDR_MAX_CQES);
+	attr->max_mw = qed_attr->max_mw;
+	attr->max_fmr = qed_attr->max_fmr;
+	attr->max_mr_mw_fmr_pbl = qed_attr->max_mr_mw_fmr_pbl;
+	attr->max_mr_mw_fmr_size = qed_attr->max_mr_mw_fmr_size;
+	attr->max_pd = qed_attr->max_pd;
+	attr->max_ah = qed_attr->max_ah;
+	attr->max_pkey = qed_attr->max_pkey;
+	attr->max_srq = qed_attr->max_srq;
+	attr->max_srq_wr = qed_attr->max_srq_wr;
+	attr->dev_caps = qed_attr->dev_caps;
+	attr->page_size_caps = qed_attr->page_size_caps;
+	attr->dev_ack_delay = qed_attr->dev_ack_delay;
+	attr->reserved_lkey = qed_attr->reserved_lkey;
+	attr->bad_pkey_counter = qed_attr->bad_pkey_counter;
+	attr->max_stats_queues = qed_attr->max_stats_queues;
+
+	return 0;
+}
+
+void qedr_unaffiliated_event(void *context,
+			     u8 event_code)
+{
+	pr_err("unaffiliated event not implemented yet\n");
+}
+
+void qedr_affiliated_event(void *context, u8 e_code, void *fw_handle)
+{
+#define EVENT_TYPE_NOT_DEFINED	0
+#define EVENT_TYPE_CQ		1
+#define EVENT_TYPE_QP		2
+	struct qedr_dev *dev = (struct qedr_dev *)context;
+	union event_ring_data *data = fw_handle;
+	u64 roce_handle64 = ((u64)data->roce_handle.hi << 32) +
+			    data->roce_handle.lo;
+	u8 event_type = EVENT_TYPE_NOT_DEFINED;
+	struct ib_event event;
+	struct ib_cq *ibcq;
+	struct ib_qp *ibqp;
+	struct qedr_cq *cq;
+	struct qedr_qp *qp;
+
+	switch (e_code) {
+	case ROCE_ASYNC_EVENT_CQ_OVERFLOW_ERR:
+		event.event = IB_EVENT_CQ_ERR;
+		event_type = EVENT_TYPE_CQ;
+		break;
+	case ROCE_ASYNC_EVENT_SQ_DRAINED:
+		event.event = IB_EVENT_SQ_DRAINED;
+		event_type = EVENT_TYPE_QP;
+		break;
+	case ROCE_ASYNC_EVENT_QP_CATASTROPHIC_ERR:
+		event.event = IB_EVENT_QP_FATAL;
+		event_type = EVENT_TYPE_QP;
+		break;
+	case ROCE_ASYNC_EVENT_LOCAL_INVALID_REQUEST_ERR:
+		event.event = IB_EVENT_QP_REQ_ERR;
+		event_type = EVENT_TYPE_QP;
+		break;
+	case ROCE_ASYNC_EVENT_LOCAL_ACCESS_ERR:
+		event.event = IB_EVENT_QP_ACCESS_ERR;
+		event_type = EVENT_TYPE_QP;
+		break;
+	default:
+		DP_ERR(dev, "unsupported event %d on handle=%llx\n", e_code,
+		       roce_handle64);
+	}
+
+	switch (event_type) {
+	case EVENT_TYPE_CQ:
+		cq = (struct qedr_cq *)(uintptr_t)roce_handle64;
+		if (cq) {
+			ibcq = &cq->ibcq;
+			if (ibcq->event_handler) {
+				event.device = ibcq->device;
+				event.element.cq = ibcq;
+				ibcq->event_handler(&event, ibcq->cq_context);
+			}
+		} else {
+			WARN(1,
+			     "Error: CQ event with NULL pointer ibcq. Handle=%llx\n",
+			     roce_handle64);
+		}
+		DP_ERR(dev, "CQ event %d on hanlde %p\n", e_code, cq);
+		break;
+	case EVENT_TYPE_QP:
+		qp = (struct qedr_qp *)(uintptr_t)roce_handle64;
+		if (qp) {
+			ibqp = &qp->ibqp;
+			if (ibqp->event_handler) {
+				event.device = ibqp->device;
+				event.element.qp = ibqp;
+				ibqp->event_handler(&event, ibqp->qp_context);
+			}
+		} else {
+			WARN(1,
+			     "Error: QP event with NULL pointer ibqp. Handle=%llx\n",
+			     roce_handle64);
+		}
+		DP_ERR(dev, "QP event %d on hanlde %p\n", e_code, qp);
+		break;
+	default:
+		break;
+	}
+}
+
+static int qedr_init_hw(struct qedr_dev *dev)
+{
+	struct qed_rdma_add_user_out_params out_params;
+	struct qed_rdma_start_in_params *in_params;
+	struct qed_rdma_cnq_params *cur_pbl;
+	struct qed_rdma_events events;
+	dma_addr_t p_phys_table;
+	u32 page_cnt;
+	int rc = 0;
+	int i;
+
+	in_params =  kzalloc(sizeof(*in_params), GFP_KERNEL);
+	if (!in_params) {
+		rc = -ENOMEM;
+		goto out;
+	}
+
+	in_params->desired_cnq = dev->num_cnq;
+	for (i = 0; i < dev->num_cnq; i++) {
+		cur_pbl = &in_params->cnq_pbl_list[i];
+
+		page_cnt = qed_chain_get_page_cnt(&dev->cnq_array[i].pbl);
+		cur_pbl->num_pbl_pages = page_cnt;
+
+		p_phys_table = qed_chain_get_pbl_phys(&dev->cnq_array[i].pbl);
+		cur_pbl->pbl_ptr = (u64)p_phys_table;
+	}
+
+	events.affiliated_event = qedr_affiliated_event;
+	events.unaffiliated_event = qedr_unaffiliated_event;
+	events.context = dev;
+
+	in_params->events = &events;
+	in_params->cq_mode = QED_RDMA_CQ_MODE_32_BITS;
+	in_params->max_mtu = dev->ndev->mtu;
+	ether_addr_copy(&in_params->mac_addr[0], dev->ndev->dev_addr);
+
+	rc = dev->ops->rdma_init(dev->cdev, in_params);
+	if (rc)
+		goto out;
+
+	rc = dev->ops->rdma_add_user(dev->rdma_ctx, &out_params);
+	if (rc)
+		goto out;
+
+	dev->db_addr = (void *)(uintptr_t)out_params.dpi_addr;
+	dev->db_phys_addr = out_params.dpi_phys_addr;
+	dev->db_size = out_params.dpi_size;
+	dev->dpi = out_params.dpi;
+
+	rc = qedr_set_device_attr(dev);
+out:
+	kfree(in_params);
+	if (rc)
+		DP_ERR(dev, "Init HW Failed rc = %d\n", rc);
+
+	return rc;
+}
+
+void qedr_stop_hw(struct qedr_dev *dev)
+{
+	dev->ops->rdma_remove_user(dev->rdma_ctx, dev->dpi);
+	dev->ops->rdma_stop(dev->rdma_ctx);
+}
+
+static struct qedr_dev *qedr_add(struct qed_dev *cdev, struct pci_dev *pdev,
+				 struct net_device *ndev)
+{
+	struct qed_dev_rdma_info dev_info;
+	struct qedr_dev *dev;
+	int rc = 0, i;
+
+	dev = (struct qedr_dev *)ib_alloc_device(sizeof(*dev));
+	if (!dev) {
+		pr_err("Unable to allocate ib device\n");
+		return NULL;
+	}
+
+	DP_DEBUG(dev, QEDR_MSG_INIT, "qedr add device called\n");
+
+	dev->pdev = pdev;
+	dev->ndev = ndev;
+	dev->cdev = cdev;
+
+	qed_ops = qed_get_rdma_ops();
+	if (!qed_ops) {
+		DP_ERR(dev, "Failed to get qed roce operations\n");
+		goto init_err;
+	}
+
+	dev->ops = qed_ops;
+	rc = qed_ops->fill_dev_info(cdev, &dev_info);
+	if (rc)
+		goto init_err;
+
+	dev->num_hwfns = dev_info.common.num_hwfns;
+	dev->rdma_ctx = dev->ops->rdma_get_rdma_ctx(cdev);
+
+	dev->num_cnq = dev->ops->rdma_get_min_cnq_msix(cdev);
+	if (!dev->num_cnq) {
+		DP_ERR(dev, "not enough CNQ resources.\n");
+		goto init_err;
+	}
+
+	dev->wq_multiplier = QEDR_WQ_MULTIPLIER_DFT;
+
+	qedr_pci_set_atomic(dev, pdev);
+
+	rc = qedr_alloc_resources(dev);
+	if (rc)
+		goto init_err;
+
+	rc = qedr_init_hw(dev);
+	if (rc)
+		goto alloc_err;
+
+	rc = qedr_setup_irqs(dev);
+	if (rc)
+		goto irq_err;
+
+	rc = qedr_register_device(dev);
+	if (rc) {
+		DP_ERR(dev, "Unable to allocate register device\n");
+		goto reg_err;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(qedr_attributes); i++)
+		if (device_create_file(&dev->ibdev.dev, qedr_attributes[i]))
+			goto sysfs_err;
+
+	DP_DEBUG(dev, QEDR_MSG_INIT, "qedr driver loaded successfully\n");
+	return dev;
+
+sysfs_err:
+	ib_unregister_device(&dev->ibdev);
+reg_err:
+	qedr_sync_free_irqs(dev);
+irq_err:
+	qedr_stop_hw(dev);
+alloc_err:
+	qedr_free_resources(dev);
+init_err:
+	ib_dealloc_device(&dev->ibdev);
+	DP_ERR(dev, "qedr driver load failed rc=%d\n", rc);
+
+	return NULL;
+}
+
+static void qedr_remove(struct qedr_dev *dev)
+{
+	/* First unregister with stack to stop all the active traffic
+	 * of the registered clients.
+	 */
+	qedr_remove_sysfiles(dev);
+	ib_unregister_device(&dev->ibdev);
+
+	qedr_stop_hw(dev);
+	qedr_sync_free_irqs(dev);
+	qedr_free_resources(dev);
+	ib_dealloc_device(&dev->ibdev);
+}
+
+static int qedr_close(struct qedr_dev *dev)
+{
+	qedr_ib_dispatch_event(dev, 1, IB_EVENT_PORT_ERR);
+
+	return 0;
+}
+
+static void qedr_shutdown(struct qedr_dev *dev)
+{
+	qedr_close(dev);
+	qedr_remove(dev);
+}
+
+static void qedr_mac_address_change(struct qedr_dev *dev)
+{
+	union ib_gid *sgid = &dev->sgid_tbl[0];
+	u8 guid[8], mac_addr[6];
+	int rc;
+
+	/* Update SGID */
+	ether_addr_copy(&mac_addr[0], dev->ndev->dev_addr);
+	guid[0] = mac_addr[0] ^ 2;
+	guid[1] = mac_addr[1];
+	guid[2] = mac_addr[2];
+	guid[3] = 0xff;
+	guid[4] = 0xfe;
+	guid[5] = mac_addr[3];
+	guid[6] = mac_addr[4];
+	guid[7] = mac_addr[5];
+	sgid->global.subnet_prefix = cpu_to_be64(0xfe80000000000000LL);
+	memcpy(&sgid->raw[8], guid, sizeof(guid));
+
+	/* Update LL2 */
+	rc = dev->ops->roce_ll2_set_mac_filter(dev->cdev,
+					       dev->gsi_ll2_mac_address,
+					       dev->ndev->dev_addr);
+
+	ether_addr_copy(dev->gsi_ll2_mac_address, dev->ndev->dev_addr);
+
+	qedr_ib_dispatch_event(dev, 1, IB_EVENT_GID_CHANGE);
+
+	if (rc)
+		DP_ERR(dev, "Error updating mac filter\n");
+}
+
+/* event handling via NIC driver ensures that all the NIC specific
+ * initialization done before RoCE driver notifies
+ * event to stack.
+ */
+static void qedr_notify(struct qedr_dev *dev, enum qede_roce_event event)
+{
+	switch (event) {
+	case QEDE_UP:
+		qedr_ib_dispatch_event(dev, 1, IB_EVENT_PORT_ACTIVE);
+		break;
+	case QEDE_DOWN:
+		qedr_close(dev);
+		break;
+	case QEDE_CLOSE:
+		qedr_shutdown(dev);
+		break;
+	case QEDE_CHANGE_ADDR:
+		qedr_mac_address_change(dev);
+		break;
+	default:
+		pr_err("Event not supported\n");
+	}
+}
+
+static struct qedr_driver qedr_drv = {
+	.name = "qedr_driver",
+	.add = qedr_add,
+	.remove = qedr_remove,
+	.notify = qedr_notify,
+};
+
+static int __init qedr_init_module(void)
+{
+	return qede_roce_register_driver(&qedr_drv);
+}
+
+static void __exit qedr_exit_module(void)
+{
+	qede_roce_unregister_driver(&qedr_drv);
+}
+
+module_init(qedr_init_module);
+module_exit(qedr_exit_module);
diff --git a/drivers/infiniband/hw/qedr/qedr.h b/drivers/infiniband/hw/qedr/qedr.h
new file mode 100644
index 000000000000..620badd7d4fb
--- /dev/null
+++ b/drivers/infiniband/hw/qedr/qedr.h
@@ -0,0 +1,495 @@
+/* QLogic qedr NIC Driver
+ * Copyright (c) 2015-2016  QLogic Corporation
+ *
+ * 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 __QEDR_H__
+#define __QEDR_H__
+
+#include <linux/pci.h>
+#include <rdma/ib_addr.h>
+#include <linux/qed/qed_if.h>
+#include <linux/qed/qed_chain.h>
+#include <linux/qed/qed_roce_if.h>
+#include <linux/qed/qede_roce.h>
+#include "qedr_hsi.h"
+
+#define QEDR_MODULE_VERSION	"8.10.10.0"
+#define QEDR_NODE_DESC "QLogic 579xx RoCE HCA"
+#define DP_NAME(dev) ((dev)->ibdev.name)
+
+#define DP_DEBUG(dev, module, fmt, ...)					\
+	pr_debug("(%s) " module ": " fmt,				\
+		 DP_NAME(dev) ? DP_NAME(dev) : "", ## __VA_ARGS__)
+
+#define QEDR_MSG_INIT "INIT"
+#define QEDR_MSG_MISC "MISC"
+#define QEDR_MSG_CQ   "  CQ"
+#define QEDR_MSG_MR   "  MR"
+#define QEDR_MSG_RQ   "  RQ"
+#define QEDR_MSG_SQ   "  SQ"
+#define QEDR_MSG_QP   "  QP"
+#define QEDR_MSG_GSI  " GSI"
+
+#define QEDR_CQ_MAGIC_NUMBER   (0x11223344)
+
+struct qedr_dev;
+
+struct qedr_cnq {
+	struct qedr_dev		*dev;
+	struct qed_chain	pbl;
+	struct qed_sb_info	*sb;
+	char			name[32];
+	u64			n_comp;
+	__le16			*hw_cons_ptr;
+	u8			index;
+};
+
+#define QEDR_MAX_SGID 128
+
+struct qedr_device_attr {
+	u32	vendor_id;
+	u32	vendor_part_id;
+	u32	hw_ver;
+	u64	fw_ver;
+	u64	node_guid;
+	u64	sys_image_guid;
+	u8	max_cnq;
+	u8	max_sge;
+	u16	max_inline;
+	u32	max_sqe;
+	u32	max_rqe;
+	u8	max_qp_resp_rd_atomic_resc;
+	u8	max_qp_req_rd_atomic_resc;
+	u64	max_dev_resp_rd_atomic_resc;
+	u32	max_cq;
+	u32	max_qp;
+	u32	max_mr;
+	u64	max_mr_size;
+	u32	max_cqe;
+	u32	max_mw;
+	u32	max_fmr;
+	u32	max_mr_mw_fmr_pbl;
+	u64	max_mr_mw_fmr_size;
+	u32	max_pd;
+	u32	max_ah;
+	u8	max_pkey;
+	u32	max_srq;
+	u32	max_srq_wr;
+	u8	max_srq_sge;
+	u8	max_stats_queues;
+	u32	dev_caps;
+
+	u64	page_size_caps;
+	u8	dev_ack_delay;
+	u32	reserved_lkey;
+	u32	bad_pkey_counter;
+	struct qed_rdma_events events;
+};
+
+struct qedr_dev {
+	struct ib_device	ibdev;
+	struct qed_dev		*cdev;
+	struct pci_dev		*pdev;
+	struct net_device	*ndev;
+
+	enum ib_atomic_cap	atomic_cap;
+
+	void *rdma_ctx;
+	struct qedr_device_attr attr;
+
+	const struct qed_rdma_ops *ops;
+	struct qed_int_info	int_info;
+
+	struct qed_sb_info	*sb_array;
+	struct qedr_cnq		*cnq_array;
+	int			num_cnq;
+	int			sb_start;
+
+	void __iomem		*db_addr;
+	u64			db_phys_addr;
+	u32			db_size;
+	u16			dpi;
+
+	union ib_gid *sgid_tbl;
+
+	/* Lock for sgid table */
+	spinlock_t sgid_lock;
+
+	u64			guid;
+
+	u32			dp_module;
+	u8			dp_level;
+	u8			num_hwfns;
+	uint			wq_multiplier;
+	u8			gsi_ll2_mac_address[ETH_ALEN];
+	int			gsi_qp_created;
+	struct qedr_cq		*gsi_sqcq;
+	struct qedr_cq		*gsi_rqcq;
+	struct qedr_qp		*gsi_qp;
+};
+
+#define QEDR_MAX_SQ_PBL			(0x8000)
+#define QEDR_MAX_SQ_PBL_ENTRIES		(0x10000 / sizeof(void *))
+#define QEDR_SQE_ELEMENT_SIZE		(sizeof(struct rdma_sq_sge))
+#define QEDR_MAX_SQE_ELEMENTS_PER_SQE	(ROCE_REQ_MAX_SINGLE_SQ_WQE_SIZE / \
+					 QEDR_SQE_ELEMENT_SIZE)
+#define QEDR_MAX_SQE_ELEMENTS_PER_PAGE	((RDMA_RING_PAGE_SIZE) / \
+					 QEDR_SQE_ELEMENT_SIZE)
+#define QEDR_MAX_SQE			((QEDR_MAX_SQ_PBL_ENTRIES) *\
+					 (RDMA_RING_PAGE_SIZE) / \
+					 (QEDR_SQE_ELEMENT_SIZE) /\
+					 (QEDR_MAX_SQE_ELEMENTS_PER_SQE))
+/* RQ */
+#define QEDR_MAX_RQ_PBL			(0x2000)
+#define QEDR_MAX_RQ_PBL_ENTRIES		(0x10000 / sizeof(void *))
+#define QEDR_RQE_ELEMENT_SIZE		(sizeof(struct rdma_rq_sge))
+#define QEDR_MAX_RQE_ELEMENTS_PER_RQE	(RDMA_MAX_SGE_PER_RQ_WQE)
+#define QEDR_MAX_RQE_ELEMENTS_PER_PAGE	((RDMA_RING_PAGE_SIZE) / \
+					 QEDR_RQE_ELEMENT_SIZE)
+#define QEDR_MAX_RQE			((QEDR_MAX_RQ_PBL_ENTRIES) *\
+					 (RDMA_RING_PAGE_SIZE) / \
+					 (QEDR_RQE_ELEMENT_SIZE) /\
+					 (QEDR_MAX_RQE_ELEMENTS_PER_RQE))
+
+#define QEDR_CQE_SIZE	(sizeof(union rdma_cqe))
+#define QEDR_MAX_CQE_PBL_SIZE (512 * 1024)
+#define QEDR_MAX_CQE_PBL_ENTRIES (((QEDR_MAX_CQE_PBL_SIZE) / \
+				  sizeof(u64)) - 1)
+#define QEDR_MAX_CQES ((u32)((QEDR_MAX_CQE_PBL_ENTRIES) * \
+			     (QED_CHAIN_PAGE_SIZE) / QEDR_CQE_SIZE))
+
+#define QEDR_ROCE_MAX_CNQ_SIZE		(0x4000)
+
+#define QEDR_MAX_PORT			(1)
+
+#define QEDR_UVERBS(CMD_NAME) (1ull << IB_USER_VERBS_CMD_##CMD_NAME)
+
+#define QEDR_ROCE_PKEY_MAX 1
+#define QEDR_ROCE_PKEY_TABLE_LEN 1
+#define QEDR_ROCE_PKEY_DEFAULT 0xffff
+
+struct qedr_pbl {
+	struct list_head list_entry;
+	void *va;
+	dma_addr_t pa;
+};
+
+struct qedr_ucontext {
+	struct ib_ucontext ibucontext;
+	struct qedr_dev *dev;
+	struct qedr_pd *pd;
+	u64 dpi_addr;
+	u64 dpi_phys_addr;
+	u32 dpi_size;
+	u16 dpi;
+
+	struct list_head mm_head;
+
+	/* Lock to protect mm list */
+	struct mutex mm_list_lock;
+};
+
+union db_prod64 {
+	struct rdma_pwm_val32_data data;
+	u64 raw;
+};
+
+enum qedr_cq_type {
+	QEDR_CQ_TYPE_GSI,
+	QEDR_CQ_TYPE_KERNEL,
+	QEDR_CQ_TYPE_USER,
+};
+
+struct qedr_pbl_info {
+	u32 num_pbls;
+	u32 num_pbes;
+	u32 pbl_size;
+	u32 pbe_size;
+	bool two_layered;
+};
+
+struct qedr_userq {
+	struct ib_umem *umem;
+	struct qedr_pbl_info pbl_info;
+	struct qedr_pbl *pbl_tbl;
+	u64 buf_addr;
+	size_t buf_len;
+};
+
+struct qedr_cq {
+	struct ib_cq ibcq;
+
+	enum qedr_cq_type cq_type;
+	u32 sig;
+
+	u16 icid;
+
+	/* Lock to protect completion handler */
+	spinlock_t comp_handler_lock;
+
+	/* Lock to protect multiplem CQ's */
+	spinlock_t cq_lock;
+	u8 arm_flags;
+	struct qed_chain pbl;
+
+	void __iomem *db_addr;
+	union db_prod64 db;
+
+	u8 pbl_toggle;
+	union rdma_cqe *latest_cqe;
+	union rdma_cqe *toggle_cqe;
+
+	u32 cq_cons;
+
+	struct qedr_userq q;
+};
+
+struct qedr_pd {
+	struct ib_pd ibpd;
+	u32 pd_id;
+	struct qedr_ucontext *uctx;
+};
+
+struct qedr_mm {
+	struct {
+		u64 phy_addr;
+		unsigned long len;
+	} key;
+	struct list_head entry;
+};
+
+union db_prod32 {
+	struct rdma_pwm_val16_data data;
+	u32 raw;
+};
+
+struct qedr_qp_hwq_info {
+	/* WQE Elements */
+	struct qed_chain pbl;
+	u64 p_phys_addr_tbl;
+	u32 max_sges;
+
+	/* WQE */
+	u16 prod;
+	u16 cons;
+	u16 wqe_cons;
+	u16 gsi_cons;
+	u16 max_wr;
+
+	/* DB */
+	void __iomem *db;
+	union db_prod32 db_data;
+};
+
+#define QEDR_INC_SW_IDX(p_info, index)					\
+	do {								\
+		p_info->index = (p_info->index + 1) &			\
+				qed_chain_get_capacity(p_info->pbl)	\
+	} while (0)
+
+enum qedr_qp_err_bitmap {
+	QEDR_QP_ERR_SQ_FULL = 1,
+	QEDR_QP_ERR_RQ_FULL = 2,
+	QEDR_QP_ERR_BAD_SR = 4,
+	QEDR_QP_ERR_BAD_RR = 8,
+	QEDR_QP_ERR_SQ_PBL_FULL = 16,
+	QEDR_QP_ERR_RQ_PBL_FULL = 32,
+};
+
+struct qedr_qp {
+	struct ib_qp ibqp;	/* must be first */
+	struct qedr_dev *dev;
+
+	struct qedr_qp_hwq_info sq;
+	struct qedr_qp_hwq_info rq;
+
+	u32 max_inline_data;
+
+	/* Lock for QP's */
+	spinlock_t q_lock;
+	struct qedr_cq *sq_cq;
+	struct qedr_cq *rq_cq;
+	struct qedr_srq *srq;
+	enum qed_roce_qp_state state;
+	u32 id;
+	struct qedr_pd *pd;
+	enum ib_qp_type qp_type;
+	struct qed_rdma_qp *qed_qp;
+	u32 qp_id;
+	u16 icid;
+	u16 mtu;
+	int sgid_idx;
+	u32 rq_psn;
+	u32 sq_psn;
+	u32 qkey;
+	u32 dest_qp_num;
+
+	/* Relevant to qps created from kernel space only (ULPs) */
+	u8 prev_wqe_size;
+	u16 wqe_cons;
+	u32 err_bitmap;
+	bool signaled;
+
+	/* SQ shadow */
+	struct {
+		u64 wr_id;
+		enum ib_wc_opcode opcode;
+		u32 bytes_len;
+		u8 wqe_size;
+		bool signaled;
+		dma_addr_t icrc_mapping;
+		u32 *icrc;
+		struct qedr_mr *mr;
+	} *wqe_wr_id;
+
+	/* RQ shadow */
+	struct {
+		u64 wr_id;
+		struct ib_sge sg_list[RDMA_MAX_SGE_PER_RQ_WQE];
+		u8 wqe_size;
+
+		u8 smac[ETH_ALEN];
+		u16 vlan_id;
+		int rc;
+	} *rqe_wr_id;
+
+	/* Relevant to qps created from user space only (applications) */
+	struct qedr_userq usq;
+	struct qedr_userq urq;
+};
+
+struct qedr_ah {
+	struct ib_ah ibah;
+	struct ib_ah_attr attr;
+};
+
+enum qedr_mr_type {
+	QEDR_MR_USER,
+	QEDR_MR_KERNEL,
+	QEDR_MR_DMA,
+	QEDR_MR_FRMR,
+};
+
+struct mr_info {
+	struct qedr_pbl *pbl_table;
+	struct qedr_pbl_info pbl_info;
+	struct list_head free_pbl_list;
+	struct list_head inuse_pbl_list;
+	u32 completed;
+	u32 completed_handled;
+};
+
+struct qedr_mr {
+	struct ib_mr ibmr;
+	struct ib_umem *umem;
+
+	struct qed_rdma_register_tid_in_params hw_mr;
+	enum qedr_mr_type type;
+
+	struct qedr_dev *dev;
+	struct mr_info info;
+
+	u64 *pages;
+	u32 npages;
+};
+
+#define SET_FIELD2(value, name, flag) ((value) |= ((flag) << (name ## _SHIFT)))
+
+#define QEDR_RESP_IMM	(RDMA_CQE_RESPONDER_IMM_FLG_MASK << \
+			 RDMA_CQE_RESPONDER_IMM_FLG_SHIFT)
+#define QEDR_RESP_RDMA	(RDMA_CQE_RESPONDER_RDMA_FLG_MASK << \
+			 RDMA_CQE_RESPONDER_RDMA_FLG_SHIFT)
+#define QEDR_RESP_RDMA_IMM (QEDR_RESP_IMM | QEDR_RESP_RDMA)
+
+static inline void qedr_inc_sw_cons(struct qedr_qp_hwq_info *info)
+{
+	info->cons = (info->cons + 1) % info->max_wr;
+	info->wqe_cons++;
+}
+
+static inline void qedr_inc_sw_prod(struct qedr_qp_hwq_info *info)
+{
+	info->prod = (info->prod + 1) % info->max_wr;
+}
+
+static inline int qedr_get_dmac(struct qedr_dev *dev,
+				struct ib_ah_attr *ah_attr, u8 *mac_addr)
+{
+	union ib_gid zero_sgid = { { 0 } };
+	struct in6_addr in6;
+
+	if (!memcmp(&ah_attr->grh.dgid, &zero_sgid, sizeof(union ib_gid))) {
+		DP_ERR(dev, "Local port GID not supported\n");
+		eth_zero_addr(mac_addr);
+		return -EINVAL;
+	}
+
+	memcpy(&in6, ah_attr->grh.dgid.raw, sizeof(in6));
+	ether_addr_copy(mac_addr, ah_attr->dmac);
+
+	return 0;
+}
+
+static inline
+struct qedr_ucontext *get_qedr_ucontext(struct ib_ucontext *ibucontext)
+{
+	return container_of(ibucontext, struct qedr_ucontext, ibucontext);
+}
+
+static inline struct qedr_dev *get_qedr_dev(struct ib_device *ibdev)
+{
+	return container_of(ibdev, struct qedr_dev, ibdev);
+}
+
+static inline struct qedr_pd *get_qedr_pd(struct ib_pd *ibpd)
+{
+	return container_of(ibpd, struct qedr_pd, ibpd);
+}
+
+static inline struct qedr_cq *get_qedr_cq(struct ib_cq *ibcq)
+{
+	return container_of(ibcq, struct qedr_cq, ibcq);
+}
+
+static inline struct qedr_qp *get_qedr_qp(struct ib_qp *ibqp)
+{
+	return container_of(ibqp, struct qedr_qp, ibqp);
+}
+
+static inline struct qedr_ah *get_qedr_ah(struct ib_ah *ibah)
+{
+	return container_of(ibah, struct qedr_ah, ibah);
+}
+
+static inline struct qedr_mr *get_qedr_mr(struct ib_mr *ibmr)
+{
+	return container_of(ibmr, struct qedr_mr, ibmr);
+}
+#endif
diff --git a/drivers/infiniband/hw/qedr/qedr_cm.c b/drivers/infiniband/hw/qedr/qedr_cm.c
new file mode 100644
index 000000000000..63890ebb72bd
--- /dev/null
+++ b/drivers/infiniband/hw/qedr/qedr_cm.c
@@ -0,0 +1,622 @@
+/* QLogic qedr NIC Driver
+ * Copyright (c) 2015-2016  QLogic Corporation
+ *
+ * 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/dma-mapping.h>
+#include <linux/crc32.h>
+#include <linux/iommu.h>
+#include <net/ip.h>
+#include <net/ipv6.h>
+#include <net/udp.h>
+
+#include <rdma/ib_verbs.h>
+#include <rdma/ib_user_verbs.h>
+#include <rdma/iw_cm.h>
+#include <rdma/ib_umem.h>
+#include <rdma/ib_addr.h>
+#include <rdma/ib_cache.h>
+
+#include "qedr_hsi.h"
+#include <linux/qed/qed_if.h>
+#include <linux/qed/qed_roce_if.h>
+#include "qedr.h"
+#include "qedr_hsi.h"
+#include "verbs.h"
+#include <rdma/qedr-abi.h>
+#include "qedr_hsi.h"
+#include "qedr_cm.h"
+
+void qedr_inc_sw_gsi_cons(struct qedr_qp_hwq_info *info)
+{
+	info->gsi_cons = (info->gsi_cons + 1) % info->max_wr;
+}
+
+void qedr_store_gsi_qp_cq(struct qedr_dev *dev, struct qedr_qp *qp,
+			  struct ib_qp_init_attr *attrs)
+{
+	dev->gsi_qp_created = 1;
+	dev->gsi_sqcq = get_qedr_cq(attrs->send_cq);
+	dev->gsi_rqcq = get_qedr_cq(attrs->recv_cq);
+	dev->gsi_qp = qp;
+}
+
+void qedr_ll2_tx_cb(void *_qdev, struct qed_roce_ll2_packet *pkt)
+{
+	struct qedr_dev *dev = (struct qedr_dev *)_qdev;
+	struct qedr_cq *cq = dev->gsi_sqcq;
+	struct qedr_qp *qp = dev->gsi_qp;
+	unsigned long flags;
+
+	DP_DEBUG(dev, QEDR_MSG_GSI,
+		 "LL2 TX CB: gsi_sqcq=%p, gsi_rqcq=%p, gsi_cons=%d, ibcq_comp=%s\n",
+		 dev->gsi_sqcq, dev->gsi_rqcq, qp->sq.gsi_cons,
+		 cq->ibcq.comp_handler ? "Yes" : "No");
+
+	dma_free_coherent(&dev->pdev->dev, pkt->header.len, pkt->header.vaddr,
+			  pkt->header.baddr);
+	kfree(pkt);
+
+	spin_lock_irqsave(&qp->q_lock, flags);
+	qedr_inc_sw_gsi_cons(&qp->sq);
+	spin_unlock_irqrestore(&qp->q_lock, flags);
+
+	if (cq->ibcq.comp_handler) {
+		spin_lock_irqsave(&cq->comp_handler_lock, flags);
+		(*cq->ibcq.comp_handler) (&cq->ibcq, cq->ibcq.cq_context);
+		spin_unlock_irqrestore(&cq->comp_handler_lock, flags);
+	}
+}
+
+void qedr_ll2_rx_cb(void *_dev, struct qed_roce_ll2_packet *pkt,
+		    struct qed_roce_ll2_rx_params *params)
+{
+	struct qedr_dev *dev = (struct qedr_dev *)_dev;
+	struct qedr_cq *cq = dev->gsi_rqcq;
+	struct qedr_qp *qp = dev->gsi_qp;
+	unsigned long flags;
+
+	spin_lock_irqsave(&qp->q_lock, flags);
+
+	qp->rqe_wr_id[qp->rq.gsi_cons].rc = params->rc;
+	qp->rqe_wr_id[qp->rq.gsi_cons].vlan_id = params->vlan_id;
+	qp->rqe_wr_id[qp->rq.gsi_cons].sg_list[0].length = pkt->payload[0].len;
+	ether_addr_copy(qp->rqe_wr_id[qp->rq.gsi_cons].smac, params->smac);
+
+	qedr_inc_sw_gsi_cons(&qp->rq);
+
+	spin_unlock_irqrestore(&qp->q_lock, flags);
+
+	if (cq->ibcq.comp_handler) {
+		spin_lock_irqsave(&cq->comp_handler_lock, flags);
+		(*cq->ibcq.comp_handler) (&cq->ibcq, cq->ibcq.cq_context);
+		spin_unlock_irqrestore(&cq->comp_handler_lock, flags);
+	}
+}
+
+static void qedr_destroy_gsi_cq(struct qedr_dev *dev,
+				struct ib_qp_init_attr *attrs)
+{
+	struct qed_rdma_destroy_cq_in_params iparams;
+	struct qed_rdma_destroy_cq_out_params oparams;
+	struct qedr_cq *cq;
+
+	cq = get_qedr_cq(attrs->send_cq);
+	iparams.icid = cq->icid;
+	dev->ops->rdma_destroy_cq(dev->rdma_ctx, &iparams, &oparams);
+	dev->ops->common->chain_free(dev->cdev, &cq->pbl);
+
+	cq = get_qedr_cq(attrs->recv_cq);
+	/* if a dedicated recv_cq was used, delete it too */
+	if (iparams.icid != cq->icid) {
+		iparams.icid = cq->icid;
+		dev->ops->rdma_destroy_cq(dev->rdma_ctx, &iparams, &oparams);
+		dev->ops->common->chain_free(dev->cdev, &cq->pbl);
+	}
+}
+
+static inline int qedr_check_gsi_qp_attrs(struct qedr_dev *dev,
+					  struct ib_qp_init_attr *attrs)
+{
+	if (attrs->cap.max_recv_sge > QEDR_GSI_MAX_RECV_SGE) {
+		DP_ERR(dev,
+		       " create gsi qp: failed. max_recv_sge is larger the max %d>%d\n",
+		       attrs->cap.max_recv_sge, QEDR_GSI_MAX_RECV_SGE);
+		return -EINVAL;
+	}
+
+	if (attrs->cap.max_recv_wr > QEDR_GSI_MAX_RECV_WR) {
+		DP_ERR(dev,
+		       " create gsi qp: failed. max_recv_wr is too large %d>%d\n",
+		       attrs->cap.max_recv_wr, QEDR_GSI_MAX_RECV_WR);
+		return -EINVAL;
+	}
+
+	if (attrs->cap.max_send_wr > QEDR_GSI_MAX_SEND_WR) {
+		DP_ERR(dev,
+		       " create gsi qp: failed. max_send_wr is too large %d>%d\n",
+		       attrs->cap.max_send_wr, QEDR_GSI_MAX_SEND_WR);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+struct ib_qp *qedr_create_gsi_qp(struct qedr_dev *dev,
+				 struct ib_qp_init_attr *attrs,
+				 struct qedr_qp *qp)
+{
+	struct qed_roce_ll2_params ll2_params;
+	int rc;
+
+	rc = qedr_check_gsi_qp_attrs(dev, attrs);
+	if (rc)
+		return ERR_PTR(rc);
+
+	/* configure and start LL2 */
+	memset(&ll2_params, 0, sizeof(ll2_params));
+	ll2_params.max_tx_buffers = attrs->cap.max_send_wr;
+	ll2_params.max_rx_buffers = attrs->cap.max_recv_wr;
+	ll2_params.cbs.tx_cb = qedr_ll2_tx_cb;
+	ll2_params.cbs.rx_cb = qedr_ll2_rx_cb;
+	ll2_params.cb_cookie = (void *)dev;
+	ll2_params.mtu = dev->ndev->mtu;
+	ether_addr_copy(ll2_params.mac_address, dev->ndev->dev_addr);
+	rc = dev->ops->roce_ll2_start(dev->cdev, &ll2_params);
+	if (rc) {
+		DP_ERR(dev, "create gsi qp: failed on ll2 start. rc=%d\n", rc);
+		return ERR_PTR(rc);
+	}
+
+	/* create QP */
+	qp->ibqp.qp_num = 1;
+	qp->rq.max_wr = attrs->cap.max_recv_wr;
+	qp->sq.max_wr = attrs->cap.max_send_wr;
+
+	qp->rqe_wr_id = kcalloc(qp->rq.max_wr, sizeof(*qp->rqe_wr_id),
+				GFP_KERNEL);
+	if (!qp->rqe_wr_id)
+		goto err;
+	qp->wqe_wr_id = kcalloc(qp->sq.max_wr, sizeof(*qp->wqe_wr_id),
+				GFP_KERNEL);
+	if (!qp->wqe_wr_id)
+		goto err;
+
+	qedr_store_gsi_qp_cq(dev, qp, attrs);
+	ether_addr_copy(dev->gsi_ll2_mac_address, dev->ndev->dev_addr);
+
+	/* the GSI CQ is handled by the driver so remove it from the FW */
+	qedr_destroy_gsi_cq(dev, attrs);
+	dev->gsi_rqcq->cq_type = QEDR_CQ_TYPE_GSI;
+	dev->gsi_rqcq->cq_type = QEDR_CQ_TYPE_GSI;
+
+	DP_DEBUG(dev, QEDR_MSG_GSI, "created GSI QP %p\n", qp);
+
+	return &qp->ibqp;
+
+err:
+	kfree(qp->rqe_wr_id);
+
+	rc = dev->ops->roce_ll2_stop(dev->cdev);
+	if (rc)
+		DP_ERR(dev, "create gsi qp: failed destroy on create\n");
+
+	return ERR_PTR(-ENOMEM);
+}
+
+int qedr_destroy_gsi_qp(struct qedr_dev *dev)
+{
+	int rc;
+
+	rc = dev->ops->roce_ll2_stop(dev->cdev);
+	if (rc)
+		DP_ERR(dev, "destroy gsi qp: failed (rc=%d)\n", rc);
+	else
+		DP_DEBUG(dev, QEDR_MSG_GSI, "destroy gsi qp: success\n");
+
+	return rc;
+}
+
+#define QEDR_MAX_UD_HEADER_SIZE	(100)
+#define QEDR_GSI_QPN		(1)
+static inline int qedr_gsi_build_header(struct qedr_dev *dev,
+					struct qedr_qp *qp,
+					struct ib_send_wr *swr,
+					struct ib_ud_header *udh,
+					int *roce_mode)
+{
+	bool has_vlan = false, has_grh_ipv6 = true;
+	struct ib_ah_attr *ah_attr = &get_qedr_ah(ud_wr(swr)->ah)->attr;
+	struct ib_global_route *grh = &ah_attr->grh;
+	union ib_gid sgid;
+	int send_size = 0;
+	u16 vlan_id = 0;
+	u16 ether_type;
+	struct ib_gid_attr sgid_attr;
+	int rc;
+	int ip_ver = 0;
+
+	bool has_udp = false;
+	int i;
+
+	send_size = 0;
+	for (i = 0; i < swr->num_sge; ++i)
+		send_size += swr->sg_list[i].length;
+
+	rc = ib_get_cached_gid(qp->ibqp.device, ah_attr->port_num,
+			       grh->sgid_index, &sgid, &sgid_attr);
+	if (rc) {
+		DP_ERR(dev,
+		       "gsi post send: failed to get cached GID (port=%d, ix=%d)\n",
+		       ah_attr->port_num, grh->sgid_index);
+		return rc;
+	}
+
+	vlan_id = rdma_vlan_dev_vlan_id(sgid_attr.ndev);
+	if (vlan_id < VLAN_CFI_MASK)
+		has_vlan = true;
+	if (sgid_attr.ndev)
+		dev_put(sgid_attr.ndev);
+
+	if (!memcmp(&sgid, &zgid, sizeof(sgid))) {
+		DP_ERR(dev, "gsi post send: GID not found GID index %d\n",
+		       ah_attr->grh.sgid_index);
+		return -ENOENT;
+	}
+
+	has_udp = (sgid_attr.gid_type == IB_GID_TYPE_ROCE_UDP_ENCAP);
+	if (!has_udp) {
+		/* RoCE v1 */
+		ether_type = ETH_P_ROCE;
+		*roce_mode = ROCE_V1;
+	} else if (ipv6_addr_v4mapped((struct in6_addr *)&sgid)) {
+		/* RoCE v2 IPv4 */
+		ip_ver = 4;
+		ether_type = ETH_P_IP;
+		has_grh_ipv6 = false;
+		*roce_mode = ROCE_V2_IPV4;
+	} else {
+		/* RoCE v2 IPv6 */
+		ip_ver = 6;
+		ether_type = ETH_P_IPV6;
+		*roce_mode = ROCE_V2_IPV6;
+	}
+
+	rc = ib_ud_header_init(send_size, false, true, has_vlan,
+			       has_grh_ipv6, ip_ver, has_udp, 0, udh);
+	if (rc) {
+		DP_ERR(dev, "gsi post send: failed to init header\n");
+		return rc;
+	}
+
+	/* ENET + VLAN headers */
+	ether_addr_copy(udh->eth.dmac_h, ah_attr->dmac);
+	ether_addr_copy(udh->eth.smac_h, dev->ndev->dev_addr);
+	if (has_vlan) {
+		udh->eth.type = htons(ETH_P_8021Q);
+		udh->vlan.tag = htons(vlan_id);
+		udh->vlan.type = htons(ether_type);
+	} else {
+		udh->eth.type = htons(ether_type);
+	}
+
+	/* BTH */
+	udh->bth.solicited_event = !!(swr->send_flags & IB_SEND_SOLICITED);
+	udh->bth.pkey = QEDR_ROCE_PKEY_DEFAULT;
+	udh->bth.destination_qpn = htonl(ud_wr(swr)->remote_qpn);
+	udh->bth.psn = htonl((qp->sq_psn++) & ((1 << 24) - 1));
+	udh->bth.opcode = IB_OPCODE_UD_SEND_ONLY;
+
+	/* DETH */
+	udh->deth.qkey = htonl(0x80010000);
+	udh->deth.source_qpn = htonl(QEDR_GSI_QPN);
+
+	if (has_grh_ipv6) {
+		/* GRH / IPv6 header */
+		udh->grh.traffic_class = grh->traffic_class;
+		udh->grh.flow_label = grh->flow_label;
+		udh->grh.hop_limit = grh->hop_limit;
+		udh->grh.destination_gid = grh->dgid;
+		memcpy(&udh->grh.source_gid.raw, &sgid.raw,
+		       sizeof(udh->grh.source_gid.raw));
+	} else {
+		/* IPv4 header */
+		u32 ipv4_addr;
+
+		udh->ip4.protocol = IPPROTO_UDP;
+		udh->ip4.tos = htonl(ah_attr->grh.flow_label);
+		udh->ip4.frag_off = htons(IP_DF);
+		udh->ip4.ttl = ah_attr->grh.hop_limit;
+
+		ipv4_addr = qedr_get_ipv4_from_gid(sgid.raw);
+		udh->ip4.saddr = ipv4_addr;
+		ipv4_addr = qedr_get_ipv4_from_gid(ah_attr->grh.dgid.raw);
+		udh->ip4.daddr = ipv4_addr;
+		/* note: checksum is calculated by the device */
+	}
+
+	/* UDP */
+	if (has_udp) {
+		udh->udp.sport = htons(QEDR_ROCE_V2_UDP_SPORT);
+		udh->udp.dport = htons(ROCE_V2_UDP_DPORT);
+		udh->udp.csum = 0;
+		/* UDP length is untouched hence is zero */
+	}
+	return 0;
+}
+
+static inline int qedr_gsi_build_packet(struct qedr_dev *dev,
+					struct qedr_qp *qp,
+					struct ib_send_wr *swr,
+					struct qed_roce_ll2_packet **p_packet)
+{
+	u8 ud_header_buffer[QEDR_MAX_UD_HEADER_SIZE];
+	struct qed_roce_ll2_packet *packet;
+	struct pci_dev *pdev = dev->pdev;
+	int roce_mode, header_size;
+	struct ib_ud_header udh;
+	int i, rc;
+
+	*p_packet = NULL;
+
+	rc = qedr_gsi_build_header(dev, qp, swr, &udh, &roce_mode);
+	if (rc)
+		return rc;
+
+	header_size = ib_ud_header_pack(&udh, &ud_header_buffer);
+
+	packet = kzalloc(sizeof(*packet), GFP_ATOMIC);
+	if (!packet)
+		return -ENOMEM;
+
+	packet->header.vaddr = dma_alloc_coherent(&pdev->dev, header_size,
+						  &packet->header.baddr,
+						  GFP_ATOMIC);
+	if (!packet->header.vaddr) {
+		kfree(packet);
+		return -ENOMEM;
+	}
+
+	if (ether_addr_equal(udh.eth.smac_h, udh.eth.dmac_h))
+		packet->tx_dest = QED_ROCE_LL2_TX_DEST_NW;
+	else
+		packet->tx_dest = QED_ROCE_LL2_TX_DEST_LB;
+
+	packet->roce_mode = roce_mode;
+	memcpy(packet->header.vaddr, ud_header_buffer, header_size);
+	packet->header.len = header_size;
+	packet->n_seg = swr->num_sge;
+	for (i = 0; i < packet->n_seg; i++) {
+		packet->payload[i].baddr = swr->sg_list[i].addr;
+		packet->payload[i].len = swr->sg_list[i].length;
+	}
+
+	*p_packet = packet;
+
+	return 0;
+}
+
+int qedr_gsi_post_send(struct ib_qp *ibqp, struct ib_send_wr *wr,
+		       struct ib_send_wr **bad_wr)
+{
+	struct qed_roce_ll2_packet *pkt = NULL;
+	struct qedr_qp *qp = get_qedr_qp(ibqp);
+	struct qed_roce_ll2_tx_params params;
+	struct qedr_dev *dev = qp->dev;
+	unsigned long flags;
+	int rc;
+
+	if (qp->state != QED_ROCE_QP_STATE_RTS) {
+		*bad_wr = wr;
+		DP_ERR(dev,
+		       "gsi post recv: failed to post rx buffer. state is %d and not QED_ROCE_QP_STATE_RTS\n",
+		       qp->state);
+		return -EINVAL;
+	}
+
+	if (wr->num_sge > RDMA_MAX_SGE_PER_SQ_WQE) {
+		DP_ERR(dev, "gsi post send: num_sge is too large (%d>%d)\n",
+		       wr->num_sge, RDMA_MAX_SGE_PER_SQ_WQE);
+		rc = -EINVAL;
+		goto err;
+	}
+
+	if (wr->opcode != IB_WR_SEND) {
+		DP_ERR(dev,
+		       "gsi post send: failed due to unsupported opcode %d\n",
+		       wr->opcode);
+		rc = -EINVAL;
+		goto err;
+	}
+
+	memset(&params, 0, sizeof(params));
+
+	spin_lock_irqsave(&qp->q_lock, flags);
+
+	rc = qedr_gsi_build_packet(dev, qp, wr, &pkt);
+	if (rc) {
+		spin_unlock_irqrestore(&qp->q_lock, flags);
+		goto err;
+	}
+
+	rc = dev->ops->roce_ll2_tx(dev->cdev, pkt, &params);
+	if (!rc) {
+		qp->wqe_wr_id[qp->sq.prod].wr_id = wr->wr_id;
+		qedr_inc_sw_prod(&qp->sq);
+		DP_DEBUG(qp->dev, QEDR_MSG_GSI,
+			 "gsi post send: opcode=%d, in_irq=%ld, irqs_disabled=%d, wr_id=%llx\n",
+			 wr->opcode, in_irq(), irqs_disabled(), wr->wr_id);
+	} else {
+		if (rc == QED_ROCE_TX_HEAD_FAILURE) {
+			/* TX failed while posting header - release resources */
+			dma_free_coherent(&dev->pdev->dev, pkt->header.len,
+					  pkt->header.vaddr, pkt->header.baddr);
+			kfree(pkt);
+		} else if (rc == QED_ROCE_TX_FRAG_FAILURE) {
+			/* NTD since TX failed while posting a fragment. We will
+			 * release the resources on TX callback
+			 */
+		}
+
+		DP_ERR(dev, "gsi post send: failed to transmit (rc=%d)\n", rc);
+		rc = -EAGAIN;
+		*bad_wr = wr;
+	}
+
+	spin_unlock_irqrestore(&qp->q_lock, flags);
+
+	if (wr->next) {
+		DP_ERR(dev,
+		       "gsi post send: failed second WR. Only one WR may be passed at a time\n");
+		*bad_wr = wr->next;
+		rc = -EINVAL;
+	}
+
+	return rc;
+
+err:
+	*bad_wr = wr;
+	return rc;
+}
+
+int qedr_gsi_post_recv(struct ib_qp *ibqp, struct ib_recv_wr *wr,
+		       struct ib_recv_wr **bad_wr)
+{
+	struct qedr_dev *dev = get_qedr_dev(ibqp->device);
+	struct qedr_qp *qp = get_qedr_qp(ibqp);
+	struct qed_roce_ll2_buffer buf;
+	unsigned long flags;
+	int status = 0;
+	int rc;
+
+	if ((qp->state != QED_ROCE_QP_STATE_RTR) &&
+	    (qp->state != QED_ROCE_QP_STATE_RTS)) {
+		*bad_wr = wr;
+		DP_ERR(dev,
+		       "gsi post recv: failed to post rx buffer. state is %d and not QED_ROCE_QP_STATE_RTR/S\n",
+		       qp->state);
+		return -EINVAL;
+	}
+
+	memset(&buf, 0, sizeof(buf));
+
+	spin_lock_irqsave(&qp->q_lock, flags);
+
+	while (wr) {
+		if (wr->num_sge > QEDR_GSI_MAX_RECV_SGE) {
+			DP_ERR(dev,
+			       "gsi post recv: failed to post rx buffer. too many sges %d>%d\n",
+			       wr->num_sge, QEDR_GSI_MAX_RECV_SGE);
+			goto err;
+		}
+
+		buf.baddr = wr->sg_list[0].addr;
+		buf.len = wr->sg_list[0].length;
+
+		rc = dev->ops->roce_ll2_post_rx_buffer(dev->cdev, &buf, 0, 1);
+		if (rc) {
+			DP_ERR(dev,
+			       "gsi post recv: failed to post rx buffer (rc=%d)\n",
+			       rc);
+			goto err;
+		}
+
+		memset(&qp->rqe_wr_id[qp->rq.prod], 0,
+		       sizeof(qp->rqe_wr_id[qp->rq.prod]));
+		qp->rqe_wr_id[qp->rq.prod].sg_list[0] = wr->sg_list[0];
+		qp->rqe_wr_id[qp->rq.prod].wr_id = wr->wr_id;
+
+		qedr_inc_sw_prod(&qp->rq);
+
+		wr = wr->next;
+	}
+
+	spin_unlock_irqrestore(&qp->q_lock, flags);
+
+	return status;
+err:
+	spin_unlock_irqrestore(&qp->q_lock, flags);
+	*bad_wr = wr;
+	return -ENOMEM;
+}
+
+int qedr_gsi_poll_cq(struct ib_cq *ibcq, int num_entries, struct ib_wc *wc)
+{
+	struct qedr_dev *dev = get_qedr_dev(ibcq->device);
+	struct qedr_cq *cq = get_qedr_cq(ibcq);
+	struct qedr_qp *qp = dev->gsi_qp;
+	unsigned long flags;
+	int i = 0;
+
+	spin_lock_irqsave(&cq->cq_lock, flags);
+
+	while (i < num_entries && qp->rq.cons != qp->rq.gsi_cons) {
+		memset(&wc[i], 0, sizeof(*wc));
+
+		wc[i].qp = &qp->ibqp;
+		wc[i].wr_id = qp->rqe_wr_id[qp->rq.cons].wr_id;
+		wc[i].opcode = IB_WC_RECV;
+		wc[i].pkey_index = 0;
+		wc[i].status = (qp->rqe_wr_id[qp->rq.cons].rc) ?
+		    IB_WC_GENERAL_ERR : IB_WC_SUCCESS;
+		/* 0 - currently only one recv sg is supported */
+		wc[i].byte_len = qp->rqe_wr_id[qp->rq.cons].sg_list[0].length;
+		wc[i].wc_flags |= IB_WC_GRH | IB_WC_IP_CSUM_OK;
+		ether_addr_copy(wc[i].smac, qp->rqe_wr_id[qp->rq.cons].smac);
+		wc[i].wc_flags |= IB_WC_WITH_SMAC;
+		if (qp->rqe_wr_id[qp->rq.cons].vlan_id) {
+			wc[i].wc_flags |= IB_WC_WITH_VLAN;
+			wc[i].vlan_id = qp->rqe_wr_id[qp->rq.cons].vlan_id;
+		}
+
+		qedr_inc_sw_cons(&qp->rq);
+		i++;
+	}
+
+	while (i < num_entries && qp->sq.cons != qp->sq.gsi_cons) {
+		memset(&wc[i], 0, sizeof(*wc));
+
+		wc[i].qp = &qp->ibqp;
+		wc[i].wr_id = qp->wqe_wr_id[qp->sq.cons].wr_id;
+		wc[i].opcode = IB_WC_SEND;
+		wc[i].status = IB_WC_SUCCESS;
+
+		qedr_inc_sw_cons(&qp->sq);
+		i++;
+	}
+
+	spin_unlock_irqrestore(&cq->cq_lock, flags);
+
+	DP_DEBUG(dev, QEDR_MSG_GSI,
+		 "gsi poll_cq: requested entries=%d, actual=%d, qp->rq.cons=%d, qp->rq.gsi_cons=%x, qp->sq.cons=%d, qp->sq.gsi_cons=%d, qp_num=%d\n",
+		 num_entries, i, qp->rq.cons, qp->rq.gsi_cons, qp->sq.cons,
+		 qp->sq.gsi_cons, qp->ibqp.qp_num);
+
+	return i;
+}
diff --git a/drivers/infiniband/hw/qedr/qedr_cm.h b/drivers/infiniband/hw/qedr/qedr_cm.h
new file mode 100644
index 000000000000..9ba6e15cd93f
--- /dev/null
+++ b/drivers/infiniband/hw/qedr/qedr_cm.h
@@ -0,0 +1,61 @@
+/* QLogic qedr NIC Driver
+ * Copyright (c) 2015-2016  QLogic Corporation
+ *
+ * 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 LINUX_QEDR_CM_H_
+#define LINUX_QEDR_CM_H_
+
+#define QEDR_GSI_MAX_RECV_WR	(4096)
+#define QEDR_GSI_MAX_SEND_WR	(4096)
+
+#define QEDR_GSI_MAX_RECV_SGE	(1)	/* LL2 FW limitation */
+
+#define ETH_P_ROCE		(0x8915)
+#define QEDR_ROCE_V2_UDP_SPORT	(0000)
+
+static inline u32 qedr_get_ipv4_from_gid(u8 *gid)
+{
+	return *(u32 *)(void *)&gid[12];
+}
+
+/* RDMA CM */
+int qedr_gsi_poll_cq(struct ib_cq *ibcq, int num_entries, struct ib_wc *wc);
+int qedr_gsi_post_recv(struct ib_qp *ibqp, struct ib_recv_wr *wr,
+		       struct ib_recv_wr **bad_wr);
+int qedr_gsi_post_send(struct ib_qp *ibqp, struct ib_send_wr *wr,
+		       struct ib_send_wr **bad_wr);
+struct ib_qp *qedr_create_gsi_qp(struct qedr_dev *dev,
+				 struct ib_qp_init_attr *attrs,
+				 struct qedr_qp *qp);
+void qedr_store_gsi_qp_cq(struct qedr_dev *dev,
+			  struct qedr_qp *qp, struct ib_qp_init_attr *attrs);
+int qedr_destroy_gsi_qp(struct qedr_dev *dev);
+void qedr_inc_sw_gsi_cons(struct qedr_qp_hwq_info *info);
+#endif
diff --git a/drivers/infiniband/hw/qedr/qedr_hsi.h b/drivers/infiniband/hw/qedr/qedr_hsi.h
new file mode 100644
index 000000000000..66d27521373f
--- /dev/null
+++ b/drivers/infiniband/hw/qedr/qedr_hsi.h
@@ -0,0 +1,56 @@
+/* QLogic qedr NIC Driver
+ * Copyright (c) 2015-2016  QLogic Corporation
+ *
+ * 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 __QED_HSI_ROCE__
+#define __QED_HSI_ROCE__
+
+#include <linux/qed/common_hsi.h>
+#include <linux/qed/roce_common.h>
+#include "qedr_hsi_rdma.h"
+
+/* Affiliated asynchronous events / errors enumeration */
+enum roce_async_events_type {
+	ROCE_ASYNC_EVENT_NONE = 0,
+	ROCE_ASYNC_EVENT_COMM_EST = 1,
+	ROCE_ASYNC_EVENT_SQ_DRAINED,
+	ROCE_ASYNC_EVENT_SRQ_LIMIT,
+	ROCE_ASYNC_EVENT_LAST_WQE_REACHED,
+	ROCE_ASYNC_EVENT_CQ_ERR,
+	ROCE_ASYNC_EVENT_LOCAL_INVALID_REQUEST_ERR,
+	ROCE_ASYNC_EVENT_LOCAL_CATASTROPHIC_ERR,
+	ROCE_ASYNC_EVENT_LOCAL_ACCESS_ERR,
+	ROCE_ASYNC_EVENT_QP_CATASTROPHIC_ERR,
+	ROCE_ASYNC_EVENT_CQ_OVERFLOW_ERR,
+	ROCE_ASYNC_EVENT_SRQ_EMPTY,
+	MAX_ROCE_ASYNC_EVENTS_TYPE
+};
+
+#endif /* __QED_HSI_ROCE__ */
diff --git a/drivers/infiniband/hw/qedr/qedr_hsi_rdma.h b/drivers/infiniband/hw/qedr/qedr_hsi_rdma.h
new file mode 100644
index 000000000000..5c98d2055cad
--- /dev/null
+++ b/drivers/infiniband/hw/qedr/qedr_hsi_rdma.h
@@ -0,0 +1,748 @@
+/* QLogic qedr NIC Driver
+ * Copyright (c) 2015-2016  QLogic Corporation
+ *
+ * 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 __QED_HSI_RDMA__
+#define __QED_HSI_RDMA__
+
+#include <linux/qed/rdma_common.h>
+
+/* rdma completion notification queue element */
+struct rdma_cnqe {
+	struct regpair	cq_handle;
+};
+
+struct rdma_cqe_responder {
+	struct regpair srq_wr_id;
+	struct regpair qp_handle;
+	__le32 imm_data_or_inv_r_Key;
+	__le32 length;
+	__le32 imm_data_hi;
+	__le16 rq_cons;
+	u8 flags;
+#define RDMA_CQE_RESPONDER_TOGGLE_BIT_MASK  0x1
+#define RDMA_CQE_RESPONDER_TOGGLE_BIT_SHIFT 0
+#define RDMA_CQE_RESPONDER_TYPE_MASK        0x3
+#define RDMA_CQE_RESPONDER_TYPE_SHIFT       1
+#define RDMA_CQE_RESPONDER_INV_FLG_MASK     0x1
+#define RDMA_CQE_RESPONDER_INV_FLG_SHIFT    3
+#define RDMA_CQE_RESPONDER_IMM_FLG_MASK     0x1
+#define RDMA_CQE_RESPONDER_IMM_FLG_SHIFT    4
+#define RDMA_CQE_RESPONDER_RDMA_FLG_MASK    0x1
+#define RDMA_CQE_RESPONDER_RDMA_FLG_SHIFT   5
+#define RDMA_CQE_RESPONDER_RESERVED2_MASK   0x3
+#define RDMA_CQE_RESPONDER_RESERVED2_SHIFT  6
+	u8 status;
+};
+
+struct rdma_cqe_requester {
+	__le16 sq_cons;
+	__le16 reserved0;
+	__le32 reserved1;
+	struct regpair qp_handle;
+	struct regpair reserved2;
+	__le32 reserved3;
+	__le16 reserved4;
+	u8 flags;
+#define RDMA_CQE_REQUESTER_TOGGLE_BIT_MASK  0x1
+#define RDMA_CQE_REQUESTER_TOGGLE_BIT_SHIFT 0
+#define RDMA_CQE_REQUESTER_TYPE_MASK        0x3
+#define RDMA_CQE_REQUESTER_TYPE_SHIFT       1
+#define RDMA_CQE_REQUESTER_RESERVED5_MASK   0x1F
+#define RDMA_CQE_REQUESTER_RESERVED5_SHIFT  3
+	u8 status;
+};
+
+struct rdma_cqe_common {
+	struct regpair reserved0;
+	struct regpair qp_handle;
+	__le16 reserved1[7];
+	u8 flags;
+#define RDMA_CQE_COMMON_TOGGLE_BIT_MASK  0x1
+#define RDMA_CQE_COMMON_TOGGLE_BIT_SHIFT 0
+#define RDMA_CQE_COMMON_TYPE_MASK        0x3
+#define RDMA_CQE_COMMON_TYPE_SHIFT       1
+#define RDMA_CQE_COMMON_RESERVED2_MASK   0x1F
+#define RDMA_CQE_COMMON_RESERVED2_SHIFT  3
+	u8 status;
+};
+
+/* rdma completion queue element */
+union rdma_cqe {
+	struct rdma_cqe_responder resp;
+	struct rdma_cqe_requester req;
+	struct rdma_cqe_common cmn;
+};
+
+/* * CQE requester status enumeration */
+enum rdma_cqe_requester_status_enum {
+	RDMA_CQE_REQ_STS_OK,
+	RDMA_CQE_REQ_STS_BAD_RESPONSE_ERR,
+	RDMA_CQE_REQ_STS_LOCAL_LENGTH_ERR,
+	RDMA_CQE_REQ_STS_LOCAL_QP_OPERATION_ERR,
+	RDMA_CQE_REQ_STS_LOCAL_PROTECTION_ERR,
+	RDMA_CQE_REQ_STS_MEMORY_MGT_OPERATION_ERR,
+	RDMA_CQE_REQ_STS_REMOTE_INVALID_REQUEST_ERR,
+	RDMA_CQE_REQ_STS_REMOTE_ACCESS_ERR,
+	RDMA_CQE_REQ_STS_REMOTE_OPERATION_ERR,
+	RDMA_CQE_REQ_STS_RNR_NAK_RETRY_CNT_ERR,
+	RDMA_CQE_REQ_STS_TRANSPORT_RETRY_CNT_ERR,
+	RDMA_CQE_REQ_STS_WORK_REQUEST_FLUSHED_ERR,
+	MAX_RDMA_CQE_REQUESTER_STATUS_ENUM
+};
+
+/* CQE responder status enumeration */
+enum rdma_cqe_responder_status_enum {
+	RDMA_CQE_RESP_STS_OK,
+	RDMA_CQE_RESP_STS_LOCAL_ACCESS_ERR,
+	RDMA_CQE_RESP_STS_LOCAL_LENGTH_ERR,
+	RDMA_CQE_RESP_STS_LOCAL_QP_OPERATION_ERR,
+	RDMA_CQE_RESP_STS_LOCAL_PROTECTION_ERR,
+	RDMA_CQE_RESP_STS_MEMORY_MGT_OPERATION_ERR,
+	RDMA_CQE_RESP_STS_REMOTE_INVALID_REQUEST_ERR,
+	RDMA_CQE_RESP_STS_WORK_REQUEST_FLUSHED_ERR,
+	MAX_RDMA_CQE_RESPONDER_STATUS_ENUM
+};
+
+/* CQE type enumeration */
+enum rdma_cqe_type {
+	RDMA_CQE_TYPE_REQUESTER,
+	RDMA_CQE_TYPE_RESPONDER_RQ,
+	RDMA_CQE_TYPE_RESPONDER_SRQ,
+	RDMA_CQE_TYPE_INVALID,
+	MAX_RDMA_CQE_TYPE
+};
+
+struct rdma_sq_sge {
+	__le32 length;
+	struct regpair	addr;
+	__le32 l_key;
+};
+
+struct rdma_rq_sge {
+	struct regpair addr;
+	__le32 length;
+	__le32 flags;
+#define RDMA_RQ_SGE_L_KEY_MASK      0x3FFFFFF
+#define RDMA_RQ_SGE_L_KEY_SHIFT     0
+#define RDMA_RQ_SGE_NUM_SGES_MASK   0x7
+#define RDMA_RQ_SGE_NUM_SGES_SHIFT  26
+#define RDMA_RQ_SGE_RESERVED0_MASK  0x7
+#define RDMA_RQ_SGE_RESERVED0_SHIFT 29
+};
+
+struct rdma_srq_sge {
+	struct regpair addr;
+	__le32 length;
+	__le32 l_key;
+};
+
+/* Rdma doorbell data for SQ and RQ */
+struct rdma_pwm_val16_data {
+	__le16 icid;
+	__le16 value;
+};
+
+union rdma_pwm_val16_data_union {
+	struct rdma_pwm_val16_data as_struct;
+	__le32 as_dword;
+};
+
+/* Rdma doorbell data for CQ */
+struct rdma_pwm_val32_data {
+	__le16 icid;
+	u8 agg_flags;
+	u8 params;
+#define RDMA_PWM_VAL32_DATA_AGG_CMD_MASK    0x3
+#define RDMA_PWM_VAL32_DATA_AGG_CMD_SHIFT   0
+#define RDMA_PWM_VAL32_DATA_BYPASS_EN_MASK  0x1
+#define RDMA_PWM_VAL32_DATA_BYPASS_EN_SHIFT 2
+#define RDMA_PWM_VAL32_DATA_RESERVED_MASK   0x1F
+#define RDMA_PWM_VAL32_DATA_RESERVED_SHIFT  3
+	__le32 value;
+};
+
+/* DIF Block size options */
+enum rdma_dif_block_size {
+	RDMA_DIF_BLOCK_512 = 0,
+	RDMA_DIF_BLOCK_4096 = 1,
+	MAX_RDMA_DIF_BLOCK_SIZE
+};
+
+/* DIF CRC initial value */
+enum rdma_dif_crc_seed {
+	RDMA_DIF_CRC_SEED_0000 = 0,
+	RDMA_DIF_CRC_SEED_FFFF = 1,
+	MAX_RDMA_DIF_CRC_SEED
+};
+
+/* RDMA DIF Error Result Structure */
+struct rdma_dif_error_result {
+	__le32 error_intervals;
+	__le32 dif_error_1st_interval;
+	u8 flags;
+#define RDMA_DIF_ERROR_RESULT_DIF_ERROR_TYPE_CRC_MASK      0x1
+#define RDMA_DIF_ERROR_RESULT_DIF_ERROR_TYPE_CRC_SHIFT     0
+#define RDMA_DIF_ERROR_RESULT_DIF_ERROR_TYPE_APP_TAG_MASK  0x1
+#define RDMA_DIF_ERROR_RESULT_DIF_ERROR_TYPE_APP_TAG_SHIFT 1
+#define RDMA_DIF_ERROR_RESULT_DIF_ERROR_TYPE_REF_TAG_MASK  0x1
+#define RDMA_DIF_ERROR_RESULT_DIF_ERROR_TYPE_REF_TAG_SHIFT 2
+#define RDMA_DIF_ERROR_RESULT_RESERVED0_MASK               0xF
+#define RDMA_DIF_ERROR_RESULT_RESERVED0_SHIFT              3
+#define RDMA_DIF_ERROR_RESULT_TOGGLE_BIT_MASK              0x1
+#define RDMA_DIF_ERROR_RESULT_TOGGLE_BIT_SHIFT             7
+	u8 reserved1[55];
+};
+
+/* DIF IO direction */
+enum rdma_dif_io_direction_flg {
+	RDMA_DIF_DIR_RX = 0,
+	RDMA_DIF_DIR_TX = 1,
+	MAX_RDMA_DIF_IO_DIRECTION_FLG
+};
+
+/* RDMA DIF Runt Result Structure */
+struct rdma_dif_runt_result {
+	__le16 guard_tag;
+	__le16 reserved[3];
+};
+
+/* Memory window type enumeration */
+enum rdma_mw_type {
+	RDMA_MW_TYPE_1,
+	RDMA_MW_TYPE_2A,
+	MAX_RDMA_MW_TYPE
+};
+
+struct rdma_sq_atomic_wqe {
+	__le32 reserved1;
+	__le32 length;
+	__le32 xrc_srq;
+	u8 req_type;
+	u8 flags;
+#define RDMA_SQ_ATOMIC_WQE_COMP_FLG_MASK         0x1
+#define RDMA_SQ_ATOMIC_WQE_COMP_FLG_SHIFT        0
+#define RDMA_SQ_ATOMIC_WQE_RD_FENCE_FLG_MASK     0x1
+#define RDMA_SQ_ATOMIC_WQE_RD_FENCE_FLG_SHIFT    1
+#define RDMA_SQ_ATOMIC_WQE_INV_FENCE_FLG_MASK    0x1
+#define RDMA_SQ_ATOMIC_WQE_INV_FENCE_FLG_SHIFT   2
+#define RDMA_SQ_ATOMIC_WQE_SE_FLG_MASK           0x1
+#define RDMA_SQ_ATOMIC_WQE_SE_FLG_SHIFT          3
+#define RDMA_SQ_ATOMIC_WQE_INLINE_FLG_MASK       0x1
+#define RDMA_SQ_ATOMIC_WQE_INLINE_FLG_SHIFT      4
+#define RDMA_SQ_ATOMIC_WQE_DIF_ON_HOST_FLG_MASK  0x1
+#define RDMA_SQ_ATOMIC_WQE_DIF_ON_HOST_FLG_SHIFT 5
+#define RDMA_SQ_ATOMIC_WQE_RESERVED0_MASK        0x3
+#define RDMA_SQ_ATOMIC_WQE_RESERVED0_SHIFT       6
+	u8 wqe_size;
+	u8 prev_wqe_size;
+	struct regpair remote_va;
+	__le32 r_key;
+	__le32 reserved2;
+	struct regpair cmp_data;
+	struct regpair swap_data;
+};
+
+/* First element (16 bytes) of atomic wqe */
+struct rdma_sq_atomic_wqe_1st {
+	__le32 reserved1;
+	__le32 length;
+	__le32 xrc_srq;
+	u8 req_type;
+	u8 flags;
+#define RDMA_SQ_ATOMIC_WQE_1ST_COMP_FLG_MASK       0x1
+#define RDMA_SQ_ATOMIC_WQE_1ST_COMP_FLG_SHIFT      0
+#define RDMA_SQ_ATOMIC_WQE_1ST_RD_FENCE_FLG_MASK   0x1
+#define RDMA_SQ_ATOMIC_WQE_1ST_RD_FENCE_FLG_SHIFT  1
+#define RDMA_SQ_ATOMIC_WQE_1ST_INV_FENCE_FLG_MASK  0x1
+#define RDMA_SQ_ATOMIC_WQE_1ST_INV_FENCE_FLG_SHIFT 2
+#define RDMA_SQ_ATOMIC_WQE_1ST_SE_FLG_MASK         0x1
+#define RDMA_SQ_ATOMIC_WQE_1ST_SE_FLG_SHIFT        3
+#define RDMA_SQ_ATOMIC_WQE_1ST_INLINE_FLG_MASK     0x1
+#define RDMA_SQ_ATOMIC_WQE_1ST_INLINE_FLG_SHIFT    4
+#define RDMA_SQ_ATOMIC_WQE_1ST_RESERVED0_MASK      0x7
+#define RDMA_SQ_ATOMIC_WQE_1ST_RESERVED0_SHIFT     5
+	u8 wqe_size;
+	u8 prev_wqe_size;
+};
+
+/* Second element (16 bytes) of atomic wqe */
+struct rdma_sq_atomic_wqe_2nd {
+	struct regpair remote_va;
+	__le32 r_key;
+	__le32 reserved2;
+};
+
+/* Third element (16 bytes) of atomic wqe */
+struct rdma_sq_atomic_wqe_3rd {
+	struct regpair cmp_data;
+	struct regpair swap_data;
+};
+
+struct rdma_sq_bind_wqe {
+	struct regpair addr;
+	__le32 l_key;
+	u8 req_type;
+	u8 flags;
+#define RDMA_SQ_BIND_WQE_COMP_FLG_MASK       0x1
+#define RDMA_SQ_BIND_WQE_COMP_FLG_SHIFT      0
+#define RDMA_SQ_BIND_WQE_RD_FENCE_FLG_MASK   0x1
+#define RDMA_SQ_BIND_WQE_RD_FENCE_FLG_SHIFT  1
+#define RDMA_SQ_BIND_WQE_INV_FENCE_FLG_MASK  0x1
+#define RDMA_SQ_BIND_WQE_INV_FENCE_FLG_SHIFT 2
+#define RDMA_SQ_BIND_WQE_SE_FLG_MASK         0x1
+#define RDMA_SQ_BIND_WQE_SE_FLG_SHIFT        3
+#define RDMA_SQ_BIND_WQE_INLINE_FLG_MASK     0x1
+#define RDMA_SQ_BIND_WQE_INLINE_FLG_SHIFT    4
+#define RDMA_SQ_BIND_WQE_RESERVED0_MASK      0x7
+#define RDMA_SQ_BIND_WQE_RESERVED0_SHIFT     5
+	u8 wqe_size;
+	u8 prev_wqe_size;
+	u8 bind_ctrl;
+#define RDMA_SQ_BIND_WQE_ZERO_BASED_MASK     0x1
+#define RDMA_SQ_BIND_WQE_ZERO_BASED_SHIFT    0
+#define RDMA_SQ_BIND_WQE_MW_TYPE_MASK        0x1
+#define RDMA_SQ_BIND_WQE_MW_TYPE_SHIFT       1
+#define RDMA_SQ_BIND_WQE_RESERVED1_MASK      0x3F
+#define RDMA_SQ_BIND_WQE_RESERVED1_SHIFT     2
+	u8 access_ctrl;
+#define RDMA_SQ_BIND_WQE_REMOTE_READ_MASK    0x1
+#define RDMA_SQ_BIND_WQE_REMOTE_READ_SHIFT   0
+#define RDMA_SQ_BIND_WQE_REMOTE_WRITE_MASK   0x1
+#define RDMA_SQ_BIND_WQE_REMOTE_WRITE_SHIFT  1
+#define RDMA_SQ_BIND_WQE_ENABLE_ATOMIC_MASK  0x1
+#define RDMA_SQ_BIND_WQE_ENABLE_ATOMIC_SHIFT 2
+#define RDMA_SQ_BIND_WQE_LOCAL_READ_MASK     0x1
+#define RDMA_SQ_BIND_WQE_LOCAL_READ_SHIFT    3
+#define RDMA_SQ_BIND_WQE_LOCAL_WRITE_MASK    0x1
+#define RDMA_SQ_BIND_WQE_LOCAL_WRITE_SHIFT   4
+#define RDMA_SQ_BIND_WQE_RESERVED2_MASK      0x7
+#define RDMA_SQ_BIND_WQE_RESERVED2_SHIFT     5
+	u8 reserved3;
+	u8 length_hi;
+	__le32 length_lo;
+	__le32 parent_l_key;
+	__le32 reserved4;
+};
+
+/* First element (16 bytes) of bind wqe */
+struct rdma_sq_bind_wqe_1st {
+	struct regpair addr;
+	__le32 l_key;
+	u8 req_type;
+	u8 flags;
+#define RDMA_SQ_BIND_WQE_1ST_COMP_FLG_MASK       0x1
+#define RDMA_SQ_BIND_WQE_1ST_COMP_FLG_SHIFT      0
+#define RDMA_SQ_BIND_WQE_1ST_RD_FENCE_FLG_MASK   0x1
+#define RDMA_SQ_BIND_WQE_1ST_RD_FENCE_FLG_SHIFT  1
+#define RDMA_SQ_BIND_WQE_1ST_INV_FENCE_FLG_MASK  0x1
+#define RDMA_SQ_BIND_WQE_1ST_INV_FENCE_FLG_SHIFT 2
+#define RDMA_SQ_BIND_WQE_1ST_SE_FLG_MASK         0x1
+#define RDMA_SQ_BIND_WQE_1ST_SE_FLG_SHIFT        3
+#define RDMA_SQ_BIND_WQE_1ST_INLINE_FLG_MASK     0x1
+#define RDMA_SQ_BIND_WQE_1ST_INLINE_FLG_SHIFT    4
+#define RDMA_SQ_BIND_WQE_1ST_RESERVED0_MASK      0x7
+#define RDMA_SQ_BIND_WQE_1ST_RESERVED0_SHIFT     5
+	u8 wqe_size;
+	u8 prev_wqe_size;
+};
+
+/* Second element (16 bytes) of bind wqe */
+struct rdma_sq_bind_wqe_2nd {
+	u8 bind_ctrl;
+#define RDMA_SQ_BIND_WQE_2ND_ZERO_BASED_MASK     0x1
+#define RDMA_SQ_BIND_WQE_2ND_ZERO_BASED_SHIFT    0
+#define RDMA_SQ_BIND_WQE_2ND_MW_TYPE_MASK        0x1
+#define RDMA_SQ_BIND_WQE_2ND_MW_TYPE_SHIFT       1
+#define RDMA_SQ_BIND_WQE_2ND_RESERVED1_MASK      0x3F
+#define RDMA_SQ_BIND_WQE_2ND_RESERVED1_SHIFT     2
+	u8 access_ctrl;
+#define RDMA_SQ_BIND_WQE_2ND_REMOTE_READ_MASK    0x1
+#define RDMA_SQ_BIND_WQE_2ND_REMOTE_READ_SHIFT   0
+#define RDMA_SQ_BIND_WQE_2ND_REMOTE_WRITE_MASK   0x1
+#define RDMA_SQ_BIND_WQE_2ND_REMOTE_WRITE_SHIFT  1
+#define RDMA_SQ_BIND_WQE_2ND_ENABLE_ATOMIC_MASK  0x1
+#define RDMA_SQ_BIND_WQE_2ND_ENABLE_ATOMIC_SHIFT 2
+#define RDMA_SQ_BIND_WQE_2ND_LOCAL_READ_MASK     0x1
+#define RDMA_SQ_BIND_WQE_2ND_LOCAL_READ_SHIFT    3
+#define RDMA_SQ_BIND_WQE_2ND_LOCAL_WRITE_MASK    0x1
+#define RDMA_SQ_BIND_WQE_2ND_LOCAL_WRITE_SHIFT   4
+#define RDMA_SQ_BIND_WQE_2ND_RESERVED2_MASK      0x7
+#define RDMA_SQ_BIND_WQE_2ND_RESERVED2_SHIFT     5
+	u8 reserved3;
+	u8 length_hi;
+	__le32 length_lo;
+	__le32 parent_l_key;
+	__le32 reserved4;
+};
+
+/* Structure with only the SQ WQE common
+ * fields. Size is of one SQ element (16B)
+ */
+struct rdma_sq_common_wqe {
+	__le32 reserved1[3];
+	u8 req_type;
+	u8 flags;
+#define RDMA_SQ_COMMON_WQE_COMP_FLG_MASK       0x1
+#define RDMA_SQ_COMMON_WQE_COMP_FLG_SHIFT      0
+#define RDMA_SQ_COMMON_WQE_RD_FENCE_FLG_MASK   0x1
+#define RDMA_SQ_COMMON_WQE_RD_FENCE_FLG_SHIFT  1
+#define RDMA_SQ_COMMON_WQE_INV_FENCE_FLG_MASK  0x1
+#define RDMA_SQ_COMMON_WQE_INV_FENCE_FLG_SHIFT 2
+#define RDMA_SQ_COMMON_WQE_SE_FLG_MASK         0x1
+#define RDMA_SQ_COMMON_WQE_SE_FLG_SHIFT        3
+#define RDMA_SQ_COMMON_WQE_INLINE_FLG_MASK     0x1
+#define RDMA_SQ_COMMON_WQE_INLINE_FLG_SHIFT    4
+#define RDMA_SQ_COMMON_WQE_RESERVED0_MASK      0x7
+#define RDMA_SQ_COMMON_WQE_RESERVED0_SHIFT     5
+	u8 wqe_size;
+	u8 prev_wqe_size;
+};
+
+struct rdma_sq_fmr_wqe {
+	struct regpair addr;
+	__le32 l_key;
+	u8 req_type;
+	u8 flags;
+#define RDMA_SQ_FMR_WQE_COMP_FLG_MASK                0x1
+#define RDMA_SQ_FMR_WQE_COMP_FLG_SHIFT               0
+#define RDMA_SQ_FMR_WQE_RD_FENCE_FLG_MASK            0x1
+#define RDMA_SQ_FMR_WQE_RD_FENCE_FLG_SHIFT           1
+#define RDMA_SQ_FMR_WQE_INV_FENCE_FLG_MASK           0x1
+#define RDMA_SQ_FMR_WQE_INV_FENCE_FLG_SHIFT          2
+#define RDMA_SQ_FMR_WQE_SE_FLG_MASK                  0x1
+#define RDMA_SQ_FMR_WQE_SE_FLG_SHIFT                 3
+#define RDMA_SQ_FMR_WQE_INLINE_FLG_MASK              0x1
+#define RDMA_SQ_FMR_WQE_INLINE_FLG_SHIFT             4
+#define RDMA_SQ_FMR_WQE_DIF_ON_HOST_FLG_MASK         0x1
+#define RDMA_SQ_FMR_WQE_DIF_ON_HOST_FLG_SHIFT        5
+#define RDMA_SQ_FMR_WQE_RESERVED0_MASK               0x3
+#define RDMA_SQ_FMR_WQE_RESERVED0_SHIFT              6
+	u8 wqe_size;
+	u8 prev_wqe_size;
+	u8 fmr_ctrl;
+#define RDMA_SQ_FMR_WQE_PAGE_SIZE_LOG_MASK           0x1F
+#define RDMA_SQ_FMR_WQE_PAGE_SIZE_LOG_SHIFT          0
+#define RDMA_SQ_FMR_WQE_ZERO_BASED_MASK              0x1
+#define RDMA_SQ_FMR_WQE_ZERO_BASED_SHIFT             5
+#define RDMA_SQ_FMR_WQE_BIND_EN_MASK                 0x1
+#define RDMA_SQ_FMR_WQE_BIND_EN_SHIFT                6
+#define RDMA_SQ_FMR_WQE_RESERVED1_MASK               0x1
+#define RDMA_SQ_FMR_WQE_RESERVED1_SHIFT              7
+	u8 access_ctrl;
+#define RDMA_SQ_FMR_WQE_REMOTE_READ_MASK             0x1
+#define RDMA_SQ_FMR_WQE_REMOTE_READ_SHIFT            0
+#define RDMA_SQ_FMR_WQE_REMOTE_WRITE_MASK            0x1
+#define RDMA_SQ_FMR_WQE_REMOTE_WRITE_SHIFT           1
+#define RDMA_SQ_FMR_WQE_ENABLE_ATOMIC_MASK           0x1
+#define RDMA_SQ_FMR_WQE_ENABLE_ATOMIC_SHIFT          2
+#define RDMA_SQ_FMR_WQE_LOCAL_READ_MASK              0x1
+#define RDMA_SQ_FMR_WQE_LOCAL_READ_SHIFT             3
+#define RDMA_SQ_FMR_WQE_LOCAL_WRITE_MASK             0x1
+#define RDMA_SQ_FMR_WQE_LOCAL_WRITE_SHIFT            4
+#define RDMA_SQ_FMR_WQE_RESERVED2_MASK               0x7
+#define RDMA_SQ_FMR_WQE_RESERVED2_SHIFT              5
+	u8 reserved3;
+	u8 length_hi;
+	__le32 length_lo;
+	struct regpair pbl_addr;
+	__le32 dif_base_ref_tag;
+	__le16 dif_app_tag;
+	__le16 dif_app_tag_mask;
+	__le16 dif_runt_crc_value;
+	__le16 dif_flags;
+#define RDMA_SQ_FMR_WQE_DIF_IO_DIRECTION_FLG_MASK    0x1
+#define RDMA_SQ_FMR_WQE_DIF_IO_DIRECTION_FLG_SHIFT   0
+#define RDMA_SQ_FMR_WQE_DIF_BLOCK_SIZE_MASK          0x1
+#define RDMA_SQ_FMR_WQE_DIF_BLOCK_SIZE_SHIFT         1
+#define RDMA_SQ_FMR_WQE_DIF_RUNT_VALID_FLG_MASK      0x1
+#define RDMA_SQ_FMR_WQE_DIF_RUNT_VALID_FLG_SHIFT     2
+#define RDMA_SQ_FMR_WQE_DIF_VALIDATE_CRC_GUARD_MASK  0x1
+#define RDMA_SQ_FMR_WQE_DIF_VALIDATE_CRC_GUARD_SHIFT 3
+#define RDMA_SQ_FMR_WQE_DIF_VALIDATE_REF_TAG_MASK    0x1
+#define RDMA_SQ_FMR_WQE_DIF_VALIDATE_REF_TAG_SHIFT   4
+#define RDMA_SQ_FMR_WQE_DIF_VALIDATE_APP_TAG_MASK    0x1
+#define RDMA_SQ_FMR_WQE_DIF_VALIDATE_APP_TAG_SHIFT   5
+#define RDMA_SQ_FMR_WQE_DIF_CRC_SEED_MASK            0x1
+#define RDMA_SQ_FMR_WQE_DIF_CRC_SEED_SHIFT           6
+#define RDMA_SQ_FMR_WQE_RESERVED4_MASK               0x1FF
+#define RDMA_SQ_FMR_WQE_RESERVED4_SHIFT              7
+	__le32 Reserved5;
+};
+
+/* First element (16 bytes) of fmr wqe */
+struct rdma_sq_fmr_wqe_1st {
+	struct regpair addr;
+	__le32 l_key;
+	u8 req_type;
+	u8 flags;
+#define RDMA_SQ_FMR_WQE_1ST_COMP_FLG_MASK         0x1
+#define RDMA_SQ_FMR_WQE_1ST_COMP_FLG_SHIFT        0
+#define RDMA_SQ_FMR_WQE_1ST_RD_FENCE_FLG_MASK     0x1
+#define RDMA_SQ_FMR_WQE_1ST_RD_FENCE_FLG_SHIFT    1
+#define RDMA_SQ_FMR_WQE_1ST_INV_FENCE_FLG_MASK    0x1
+#define RDMA_SQ_FMR_WQE_1ST_INV_FENCE_FLG_SHIFT   2
+#define RDMA_SQ_FMR_WQE_1ST_SE_FLG_MASK           0x1
+#define RDMA_SQ_FMR_WQE_1ST_SE_FLG_SHIFT          3
+#define RDMA_SQ_FMR_WQE_1ST_INLINE_FLG_MASK       0x1
+#define RDMA_SQ_FMR_WQE_1ST_INLINE_FLG_SHIFT      4
+#define RDMA_SQ_FMR_WQE_1ST_DIF_ON_HOST_FLG_MASK  0x1
+#define RDMA_SQ_FMR_WQE_1ST_DIF_ON_HOST_FLG_SHIFT 5
+#define RDMA_SQ_FMR_WQE_1ST_RESERVED0_MASK        0x3
+#define RDMA_SQ_FMR_WQE_1ST_RESERVED0_SHIFT       6
+	u8 wqe_size;
+	u8 prev_wqe_size;
+};
+
+/* Second element (16 bytes) of fmr wqe */
+struct rdma_sq_fmr_wqe_2nd {
+	u8 fmr_ctrl;
+#define RDMA_SQ_FMR_WQE_2ND_PAGE_SIZE_LOG_MASK  0x1F
+#define RDMA_SQ_FMR_WQE_2ND_PAGE_SIZE_LOG_SHIFT 0
+#define RDMA_SQ_FMR_WQE_2ND_ZERO_BASED_MASK     0x1
+#define RDMA_SQ_FMR_WQE_2ND_ZERO_BASED_SHIFT    5
+#define RDMA_SQ_FMR_WQE_2ND_BIND_EN_MASK        0x1
+#define RDMA_SQ_FMR_WQE_2ND_BIND_EN_SHIFT       6
+#define RDMA_SQ_FMR_WQE_2ND_RESERVED1_MASK      0x1
+#define RDMA_SQ_FMR_WQE_2ND_RESERVED1_SHIFT     7
+	u8 access_ctrl;
+#define RDMA_SQ_FMR_WQE_2ND_REMOTE_READ_MASK    0x1
+#define RDMA_SQ_FMR_WQE_2ND_REMOTE_READ_SHIFT   0
+#define RDMA_SQ_FMR_WQE_2ND_REMOTE_WRITE_MASK   0x1
+#define RDMA_SQ_FMR_WQE_2ND_REMOTE_WRITE_SHIFT  1
+#define RDMA_SQ_FMR_WQE_2ND_ENABLE_ATOMIC_MASK  0x1
+#define RDMA_SQ_FMR_WQE_2ND_ENABLE_ATOMIC_SHIFT 2
+#define RDMA_SQ_FMR_WQE_2ND_LOCAL_READ_MASK     0x1
+#define RDMA_SQ_FMR_WQE_2ND_LOCAL_READ_SHIFT    3
+#define RDMA_SQ_FMR_WQE_2ND_LOCAL_WRITE_MASK    0x1
+#define RDMA_SQ_FMR_WQE_2ND_LOCAL_WRITE_SHIFT   4
+#define RDMA_SQ_FMR_WQE_2ND_RESERVED2_MASK      0x7
+#define RDMA_SQ_FMR_WQE_2ND_RESERVED2_SHIFT     5
+	u8 reserved3;
+	u8 length_hi;
+	__le32 length_lo;
+	struct regpair pbl_addr;
+};
+
+/* Third element (16 bytes) of fmr wqe */
+struct rdma_sq_fmr_wqe_3rd {
+	__le32 dif_base_ref_tag;
+	__le16 dif_app_tag;
+	__le16 dif_app_tag_mask;
+	__le16 dif_runt_crc_value;
+	__le16 dif_flags;
+#define RDMA_SQ_FMR_WQE_3RD_DIF_IO_DIRECTION_FLG_MASK    0x1
+#define RDMA_SQ_FMR_WQE_3RD_DIF_IO_DIRECTION_FLG_SHIFT   0
+#define RDMA_SQ_FMR_WQE_3RD_DIF_BLOCK_SIZE_MASK          0x1
+#define RDMA_SQ_FMR_WQE_3RD_DIF_BLOCK_SIZE_SHIFT         1
+#define RDMA_SQ_FMR_WQE_3RD_DIF_RUNT_VALID_FLG_MASK      0x1
+#define RDMA_SQ_FMR_WQE_3RD_DIF_RUNT_VALID_FLG_SHIFT     2
+#define RDMA_SQ_FMR_WQE_3RD_DIF_VALIDATE_CRC_GUARD_MASK  0x1
+#define RDMA_SQ_FMR_WQE_3RD_DIF_VALIDATE_CRC_GUARD_SHIFT 3
+#define RDMA_SQ_FMR_WQE_3RD_DIF_VALIDATE_REF_TAG_MASK    0x1
+#define RDMA_SQ_FMR_WQE_3RD_DIF_VALIDATE_REF_TAG_SHIFT   4
+#define RDMA_SQ_FMR_WQE_3RD_DIF_VALIDATE_APP_TAG_MASK    0x1
+#define RDMA_SQ_FMR_WQE_3RD_DIF_VALIDATE_APP_TAG_SHIFT   5
+#define RDMA_SQ_FMR_WQE_3RD_DIF_CRC_SEED_MASK            0x1
+#define RDMA_SQ_FMR_WQE_3RD_DIF_CRC_SEED_SHIFT           6
+#define RDMA_SQ_FMR_WQE_3RD_RESERVED4_MASK               0x1FF
+#define RDMA_SQ_FMR_WQE_3RD_RESERVED4_SHIFT              7
+	__le32 Reserved5;
+};
+
+struct rdma_sq_local_inv_wqe {
+	struct regpair reserved;
+	__le32 inv_l_key;
+	u8 req_type;
+	u8 flags;
+#define RDMA_SQ_LOCAL_INV_WQE_COMP_FLG_MASK         0x1
+#define RDMA_SQ_LOCAL_INV_WQE_COMP_FLG_SHIFT        0
+#define RDMA_SQ_LOCAL_INV_WQE_RD_FENCE_FLG_MASK     0x1
+#define RDMA_SQ_LOCAL_INV_WQE_RD_FENCE_FLG_SHIFT    1
+#define RDMA_SQ_LOCAL_INV_WQE_INV_FENCE_FLG_MASK    0x1
+#define RDMA_SQ_LOCAL_INV_WQE_INV_FENCE_FLG_SHIFT   2
+#define RDMA_SQ_LOCAL_INV_WQE_SE_FLG_MASK           0x1
+#define RDMA_SQ_LOCAL_INV_WQE_SE_FLG_SHIFT          3
+#define RDMA_SQ_LOCAL_INV_WQE_INLINE_FLG_MASK       0x1
+#define RDMA_SQ_LOCAL_INV_WQE_INLINE_FLG_SHIFT      4
+#define RDMA_SQ_LOCAL_INV_WQE_DIF_ON_HOST_FLG_MASK  0x1
+#define RDMA_SQ_LOCAL_INV_WQE_DIF_ON_HOST_FLG_SHIFT 5
+#define RDMA_SQ_LOCAL_INV_WQE_RESERVED0_MASK        0x3
+#define RDMA_SQ_LOCAL_INV_WQE_RESERVED0_SHIFT       6
+	u8 wqe_size;
+	u8 prev_wqe_size;
+};
+
+struct rdma_sq_rdma_wqe {
+	__le32 imm_data;
+	__le32 length;
+	__le32 xrc_srq;
+	u8 req_type;
+	u8 flags;
+#define RDMA_SQ_RDMA_WQE_COMP_FLG_MASK                  0x1
+#define RDMA_SQ_RDMA_WQE_COMP_FLG_SHIFT                 0
+#define RDMA_SQ_RDMA_WQE_RD_FENCE_FLG_MASK              0x1
+#define RDMA_SQ_RDMA_WQE_RD_FENCE_FLG_SHIFT             1
+#define RDMA_SQ_RDMA_WQE_INV_FENCE_FLG_MASK             0x1
+#define RDMA_SQ_RDMA_WQE_INV_FENCE_FLG_SHIFT            2
+#define RDMA_SQ_RDMA_WQE_SE_FLG_MASK                    0x1
+#define RDMA_SQ_RDMA_WQE_SE_FLG_SHIFT                   3
+#define RDMA_SQ_RDMA_WQE_INLINE_FLG_MASK                0x1
+#define RDMA_SQ_RDMA_WQE_INLINE_FLG_SHIFT               4
+#define RDMA_SQ_RDMA_WQE_DIF_ON_HOST_FLG_MASK           0x1
+#define RDMA_SQ_RDMA_WQE_DIF_ON_HOST_FLG_SHIFT          5
+#define RDMA_SQ_RDMA_WQE_RESERVED0_MASK                 0x3
+#define RDMA_SQ_RDMA_WQE_RESERVED0_SHIFT                6
+	u8 wqe_size;
+	u8 prev_wqe_size;
+	struct regpair remote_va;
+	__le32 r_key;
+	u8 dif_flags;
+#define RDMA_SQ_RDMA_WQE_DIF_BLOCK_SIZE_MASK            0x1
+#define RDMA_SQ_RDMA_WQE_DIF_BLOCK_SIZE_SHIFT           0
+#define RDMA_SQ_RDMA_WQE_DIF_FIRST_RDMA_IN_IO_FLG_MASK  0x1
+#define RDMA_SQ_RDMA_WQE_DIF_FIRST_RDMA_IN_IO_FLG_SHIFT 1
+#define RDMA_SQ_RDMA_WQE_DIF_LAST_RDMA_IN_IO_FLG_MASK   0x1
+#define RDMA_SQ_RDMA_WQE_DIF_LAST_RDMA_IN_IO_FLG_SHIFT  2
+#define RDMA_SQ_RDMA_WQE_RESERVED1_MASK                 0x1F
+#define RDMA_SQ_RDMA_WQE_RESERVED1_SHIFT                3
+	u8 reserved2[3];
+};
+
+/* First element (16 bytes) of rdma wqe */
+struct rdma_sq_rdma_wqe_1st {
+	__le32 imm_data;
+	__le32 length;
+	__le32 xrc_srq;
+	u8 req_type;
+	u8 flags;
+#define RDMA_SQ_RDMA_WQE_1ST_COMP_FLG_MASK         0x1
+#define RDMA_SQ_RDMA_WQE_1ST_COMP_FLG_SHIFT        0
+#define RDMA_SQ_RDMA_WQE_1ST_RD_FENCE_FLG_MASK     0x1
+#define RDMA_SQ_RDMA_WQE_1ST_RD_FENCE_FLG_SHIFT    1
+#define RDMA_SQ_RDMA_WQE_1ST_INV_FENCE_FLG_MASK    0x1
+#define RDMA_SQ_RDMA_WQE_1ST_INV_FENCE_FLG_SHIFT   2
+#define RDMA_SQ_RDMA_WQE_1ST_SE_FLG_MASK           0x1
+#define RDMA_SQ_RDMA_WQE_1ST_SE_FLG_SHIFT          3
+#define RDMA_SQ_RDMA_WQE_1ST_INLINE_FLG_MASK       0x1
+#define RDMA_SQ_RDMA_WQE_1ST_INLINE_FLG_SHIFT      4
+#define RDMA_SQ_RDMA_WQE_1ST_DIF_ON_HOST_FLG_MASK  0x1
+#define RDMA_SQ_RDMA_WQE_1ST_DIF_ON_HOST_FLG_SHIFT 5
+#define RDMA_SQ_RDMA_WQE_1ST_RESERVED0_MASK        0x3
+#define RDMA_SQ_RDMA_WQE_1ST_RESERVED0_SHIFT       6
+	u8 wqe_size;
+	u8 prev_wqe_size;
+};
+
+/* Second element (16 bytes) of rdma wqe */
+struct rdma_sq_rdma_wqe_2nd {
+	struct regpair remote_va;
+	__le32 r_key;
+	u8 dif_flags;
+#define RDMA_SQ_RDMA_WQE_2ND_DIF_BLOCK_SIZE_MASK         0x1
+#define RDMA_SQ_RDMA_WQE_2ND_DIF_BLOCK_SIZE_SHIFT        0
+#define RDMA_SQ_RDMA_WQE_2ND_DIF_FIRST_SEGMENT_FLG_MASK  0x1
+#define RDMA_SQ_RDMA_WQE_2ND_DIF_FIRST_SEGMENT_FLG_SHIFT 1
+#define RDMA_SQ_RDMA_WQE_2ND_DIF_LAST_SEGMENT_FLG_MASK   0x1
+#define RDMA_SQ_RDMA_WQE_2ND_DIF_LAST_SEGMENT_FLG_SHIFT  2
+#define RDMA_SQ_RDMA_WQE_2ND_RESERVED1_MASK              0x1F
+#define RDMA_SQ_RDMA_WQE_2ND_RESERVED1_SHIFT             3
+	u8 reserved2[3];
+};
+
+/* SQ WQE req type enumeration */
+enum rdma_sq_req_type {
+	RDMA_SQ_REQ_TYPE_SEND,
+	RDMA_SQ_REQ_TYPE_SEND_WITH_IMM,
+	RDMA_SQ_REQ_TYPE_SEND_WITH_INVALIDATE,
+	RDMA_SQ_REQ_TYPE_RDMA_WR,
+	RDMA_SQ_REQ_TYPE_RDMA_WR_WITH_IMM,
+	RDMA_SQ_REQ_TYPE_RDMA_RD,
+	RDMA_SQ_REQ_TYPE_ATOMIC_CMP_AND_SWAP,
+	RDMA_SQ_REQ_TYPE_ATOMIC_ADD,
+	RDMA_SQ_REQ_TYPE_LOCAL_INVALIDATE,
+	RDMA_SQ_REQ_TYPE_FAST_MR,
+	RDMA_SQ_REQ_TYPE_BIND,
+	RDMA_SQ_REQ_TYPE_INVALID,
+	MAX_RDMA_SQ_REQ_TYPE
+};
+
+struct rdma_sq_send_wqe {
+	__le32 inv_key_or_imm_data;
+	__le32 length;
+	__le32 xrc_srq;
+	u8 req_type;
+	u8 flags;
+#define RDMA_SQ_SEND_WQE_COMP_FLG_MASK         0x1
+#define RDMA_SQ_SEND_WQE_COMP_FLG_SHIFT        0
+#define RDMA_SQ_SEND_WQE_RD_FENCE_FLG_MASK     0x1
+#define RDMA_SQ_SEND_WQE_RD_FENCE_FLG_SHIFT    1
+#define RDMA_SQ_SEND_WQE_INV_FENCE_FLG_MASK    0x1
+#define RDMA_SQ_SEND_WQE_INV_FENCE_FLG_SHIFT   2
+#define RDMA_SQ_SEND_WQE_SE_FLG_MASK           0x1
+#define RDMA_SQ_SEND_WQE_SE_FLG_SHIFT          3
+#define RDMA_SQ_SEND_WQE_INLINE_FLG_MASK       0x1
+#define RDMA_SQ_SEND_WQE_INLINE_FLG_SHIFT      4
+#define RDMA_SQ_SEND_WQE_DIF_ON_HOST_FLG_MASK  0x1
+#define RDMA_SQ_SEND_WQE_DIF_ON_HOST_FLG_SHIFT 5
+#define RDMA_SQ_SEND_WQE_RESERVED0_MASK        0x3
+#define RDMA_SQ_SEND_WQE_RESERVED0_SHIFT       6
+	u8 wqe_size;
+	u8 prev_wqe_size;
+	__le32 reserved1[4];
+};
+
+struct rdma_sq_send_wqe_1st {
+	__le32 inv_key_or_imm_data;
+	__le32 length;
+	__le32 xrc_srq;
+	u8 req_type;
+	u8 flags;
+#define RDMA_SQ_SEND_WQE_1ST_COMP_FLG_MASK       0x1
+#define RDMA_SQ_SEND_WQE_1ST_COMP_FLG_SHIFT      0
+#define RDMA_SQ_SEND_WQE_1ST_RD_FENCE_FLG_MASK   0x1
+#define RDMA_SQ_SEND_WQE_1ST_RD_FENCE_FLG_SHIFT  1
+#define RDMA_SQ_SEND_WQE_1ST_INV_FENCE_FLG_MASK  0x1
+#define RDMA_SQ_SEND_WQE_1ST_INV_FENCE_FLG_SHIFT 2
+#define RDMA_SQ_SEND_WQE_1ST_SE_FLG_MASK         0x1
+#define RDMA_SQ_SEND_WQE_1ST_SE_FLG_SHIFT        3
+#define RDMA_SQ_SEND_WQE_1ST_INLINE_FLG_MASK     0x1
+#define RDMA_SQ_SEND_WQE_1ST_INLINE_FLG_SHIFT    4
+#define RDMA_SQ_SEND_WQE_1ST_RESERVED0_MASK      0x7
+#define RDMA_SQ_SEND_WQE_1ST_RESERVED0_SHIFT     5
+	u8 wqe_size;
+	u8 prev_wqe_size;
+};
+
+struct rdma_sq_send_wqe_2st {
+	__le32 reserved1[4];
+};
+
+#endif /* __QED_HSI_RDMA__ */
diff --git a/drivers/infiniband/hw/qedr/verbs.c b/drivers/infiniband/hw/qedr/verbs.c
new file mode 100644
index 000000000000..a61514296767
--- /dev/null
+++ b/drivers/infiniband/hw/qedr/verbs.c
@@ -0,0 +1,3547 @@
+/* QLogic qedr NIC Driver
+ * Copyright (c) 2015-2016  QLogic Corporation
+ *
+ * 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/dma-mapping.h>
+#include <linux/crc32.h>
+#include <net/ip.h>
+#include <net/ipv6.h>
+#include <net/udp.h>
+#include <linux/iommu.h>
+
+#include <rdma/ib_verbs.h>
+#include <rdma/ib_user_verbs.h>
+#include <rdma/iw_cm.h>
+#include <rdma/ib_umem.h>
+#include <rdma/ib_addr.h>
+#include <rdma/ib_cache.h>
+
+#include "qedr_hsi.h"
+#include <linux/qed/qed_if.h>
+#include "qedr.h"
+#include "verbs.h"
+#include <rdma/qedr-abi.h>
+#include "qedr_cm.h"
+
+#define DB_ADDR_SHIFT(addr)		((addr) << DB_PWM_ADDR_OFFSET_SHIFT)
+
+int qedr_query_pkey(struct ib_device *ibdev, u8 port, u16 index, u16 *pkey)
+{
+	if (index > QEDR_ROCE_PKEY_TABLE_LEN)
+		return -EINVAL;
+
+	*pkey = QEDR_ROCE_PKEY_DEFAULT;
+	return 0;
+}
+
+int qedr_query_gid(struct ib_device *ibdev, u8 port, int index,
+		   union ib_gid *sgid)
+{
+	struct qedr_dev *dev = get_qedr_dev(ibdev);
+	int rc = 0;
+
+	if (!rdma_cap_roce_gid_table(ibdev, port))
+		return -ENODEV;
+
+	rc = ib_get_cached_gid(ibdev, port, index, sgid, NULL);
+	if (rc == -EAGAIN) {
+		memcpy(sgid, &zgid, sizeof(*sgid));
+		return 0;
+	}
+
+	DP_DEBUG(dev, QEDR_MSG_INIT, "query gid: index=%d %llx:%llx\n", index,
+		 sgid->global.interface_id, sgid->global.subnet_prefix);
+
+	return rc;
+}
+
+int qedr_add_gid(struct ib_device *device, u8 port_num,
+		 unsigned int index, const union ib_gid *gid,
+		 const struct ib_gid_attr *attr, void **context)
+{
+	if (!rdma_cap_roce_gid_table(device, port_num))
+		return -EINVAL;
+
+	if (port_num > QEDR_MAX_PORT)
+		return -EINVAL;
+
+	if (!context)
+		return -EINVAL;
+
+	return 0;
+}
+
+int qedr_del_gid(struct ib_device *device, u8 port_num,
+		 unsigned int index, void **context)
+{
+	if (!rdma_cap_roce_gid_table(device, port_num))
+		return -EINVAL;
+
+	if (port_num > QEDR_MAX_PORT)
+		return -EINVAL;
+
+	if (!context)
+		return -EINVAL;
+
+	return 0;
+}
+
+int qedr_query_device(struct ib_device *ibdev,
+		      struct ib_device_attr *attr, struct ib_udata *udata)
+{
+	struct qedr_dev *dev = get_qedr_dev(ibdev);
+	struct qedr_device_attr *qattr = &dev->attr;
+
+	if (!dev->rdma_ctx) {
+		DP_ERR(dev,
+		       "qedr_query_device called with invalid params rdma_ctx=%p\n",
+		       dev->rdma_ctx);
+		return -EINVAL;
+	}
+
+	memset(attr, 0, sizeof(*attr));
+
+	attr->fw_ver = qattr->fw_ver;
+	attr->sys_image_guid = qattr->sys_image_guid;
+	attr->max_mr_size = qattr->max_mr_size;
+	attr->page_size_cap = qattr->page_size_caps;
+	attr->vendor_id = qattr->vendor_id;
+	attr->vendor_part_id = qattr->vendor_part_id;
+	attr->hw_ver = qattr->hw_ver;
+	attr->max_qp = qattr->max_qp;
+	attr->max_qp_wr = max_t(u32, qattr->max_sqe, qattr->max_rqe);
+	attr->device_cap_flags = IB_DEVICE_CURR_QP_STATE_MOD |
+	    IB_DEVICE_RC_RNR_NAK_GEN |
+	    IB_DEVICE_LOCAL_DMA_LKEY | IB_DEVICE_MEM_MGT_EXTENSIONS;
+
+	attr->max_sge = qattr->max_sge;
+	attr->max_sge_rd = qattr->max_sge;
+	attr->max_cq = qattr->max_cq;
+	attr->max_cqe = qattr->max_cqe;
+	attr->max_mr = qattr->max_mr;
+	attr->max_mw = qattr->max_mw;
+	attr->max_pd = qattr->max_pd;
+	attr->atomic_cap = dev->atomic_cap;
+	attr->max_fmr = qattr->max_fmr;
+	attr->max_map_per_fmr = 16;
+	attr->max_qp_init_rd_atom =
+	    1 << (fls(qattr->max_qp_req_rd_atomic_resc) - 1);
+	attr->max_qp_rd_atom =
+	    min(1 << (fls(qattr->max_qp_resp_rd_atomic_resc) - 1),
+		attr->max_qp_init_rd_atom);
+
+	attr->max_srq = qattr->max_srq;
+	attr->max_srq_sge = qattr->max_srq_sge;
+	attr->max_srq_wr = qattr->max_srq_wr;
+
+	attr->local_ca_ack_delay = qattr->dev_ack_delay;
+	attr->max_fast_reg_page_list_len = qattr->max_mr / 8;
+	attr->max_pkeys = QEDR_ROCE_PKEY_MAX;
+	attr->max_ah = qattr->max_ah;
+
+	return 0;
+}
+
+#define QEDR_SPEED_SDR		(1)
+#define QEDR_SPEED_DDR		(2)
+#define QEDR_SPEED_QDR		(4)
+#define QEDR_SPEED_FDR10	(8)
+#define QEDR_SPEED_FDR		(16)
+#define QEDR_SPEED_EDR		(32)
+
+static inline void get_link_speed_and_width(int speed, u8 *ib_speed,
+					    u8 *ib_width)
+{
+	switch (speed) {
+	case 1000:
+		*ib_speed = QEDR_SPEED_SDR;
+		*ib_width = IB_WIDTH_1X;
+		break;
+	case 10000:
+		*ib_speed = QEDR_SPEED_QDR;
+		*ib_width = IB_WIDTH_1X;
+		break;
+
+	case 20000:
+		*ib_speed = QEDR_SPEED_DDR;
+		*ib_width = IB_WIDTH_4X;
+		break;
+
+	case 25000:
+		*ib_speed = QEDR_SPEED_EDR;
+		*ib_width = IB_WIDTH_1X;
+		break;
+
+	case 40000:
+		*ib_speed = QEDR_SPEED_QDR;
+		*ib_width = IB_WIDTH_4X;
+		break;
+
+	case 50000:
+		*ib_speed = QEDR_SPEED_QDR;
+		*ib_width = IB_WIDTH_4X;
+		break;
+
+	case 100000:
+		*ib_speed = QEDR_SPEED_EDR;
+		*ib_width = IB_WIDTH_4X;
+		break;
+
+	default:
+		/* Unsupported */
+		*ib_speed = QEDR_SPEED_SDR;
+		*ib_width = IB_WIDTH_1X;
+	}
+}
+
+int qedr_query_port(struct ib_device *ibdev, u8 port, struct ib_port_attr *attr)
+{
+	struct qedr_dev *dev;
+	struct qed_rdma_port *rdma_port;
+
+	dev = get_qedr_dev(ibdev);
+	if (port > 1) {
+		DP_ERR(dev, "invalid_port=0x%x\n", port);
+		return -EINVAL;
+	}
+
+	if (!dev->rdma_ctx) {
+		DP_ERR(dev, "rdma_ctx is NULL\n");
+		return -EINVAL;
+	}
+
+	rdma_port = dev->ops->rdma_query_port(dev->rdma_ctx);
+	memset(attr, 0, sizeof(*attr));
+
+	if (rdma_port->port_state == QED_RDMA_PORT_UP) {
+		attr->state = IB_PORT_ACTIVE;
+		attr->phys_state = 5;
+	} else {
+		attr->state = IB_PORT_DOWN;
+		attr->phys_state = 3;
+	}
+	attr->max_mtu = IB_MTU_4096;
+	attr->active_mtu = iboe_get_mtu(dev->ndev->mtu);
+	attr->lid = 0;
+	attr->lmc = 0;
+	attr->sm_lid = 0;
+	attr->sm_sl = 0;
+	attr->port_cap_flags = IB_PORT_IP_BASED_GIDS;
+	attr->gid_tbl_len = QEDR_MAX_SGID;
+	attr->pkey_tbl_len = QEDR_ROCE_PKEY_TABLE_LEN;
+	attr->bad_pkey_cntr = rdma_port->pkey_bad_counter;
+	attr->qkey_viol_cntr = 0;
+	get_link_speed_and_width(rdma_port->link_speed,
+				 &attr->active_speed, &attr->active_width);
+	attr->max_msg_sz = rdma_port->max_msg_size;
+	attr->max_vl_num = 4;
+
+	return 0;
+}
+
+int qedr_modify_port(struct ib_device *ibdev, u8 port, int mask,
+		     struct ib_port_modify *props)
+{
+	struct qedr_dev *dev;
+
+	dev = get_qedr_dev(ibdev);
+	if (port > 1) {
+		DP_ERR(dev, "invalid_port=0x%x\n", port);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int qedr_add_mmap(struct qedr_ucontext *uctx, u64 phy_addr,
+			 unsigned long len)
+{
+	struct qedr_mm *mm;
+
+	mm = kzalloc(sizeof(*mm), GFP_KERNEL);
+	if (!mm)
+		return -ENOMEM;
+
+	mm->key.phy_addr = phy_addr;
+	/* This function might be called with a length which is not a multiple
+	 * of PAGE_SIZE, while the mapping is PAGE_SIZE grained and the kernel
+	 * forces this granularity by increasing the requested size if needed.
+	 * When qedr_mmap is called, it will search the list with the updated
+	 * length as a key. To prevent search failures, the length is rounded up
+	 * in advance to PAGE_SIZE.
+	 */
+	mm->key.len = roundup(len, PAGE_SIZE);
+	INIT_LIST_HEAD(&mm->entry);
+
+	mutex_lock(&uctx->mm_list_lock);
+	list_add(&mm->entry, &uctx->mm_head);
+	mutex_unlock(&uctx->mm_list_lock);
+
+	DP_DEBUG(uctx->dev, QEDR_MSG_MISC,
+		 "added (addr=0x%llx,len=0x%lx) for ctx=%p\n",
+		 (unsigned long long)mm->key.phy_addr,
+		 (unsigned long)mm->key.len, uctx);
+
+	return 0;
+}
+
+static bool qedr_search_mmap(struct qedr_ucontext *uctx, u64 phy_addr,
+			     unsigned long len)
+{
+	bool found = false;
+	struct qedr_mm *mm;
+
+	mutex_lock(&uctx->mm_list_lock);
+	list_for_each_entry(mm, &uctx->mm_head, entry) {
+		if (len != mm->key.len || phy_addr != mm->key.phy_addr)
+			continue;
+
+		found = true;
+		break;
+	}
+	mutex_unlock(&uctx->mm_list_lock);
+	DP_DEBUG(uctx->dev, QEDR_MSG_MISC,
+		 "searched for (addr=0x%llx,len=0x%lx) for ctx=%p, result=%d\n",
+		 mm->key.phy_addr, mm->key.len, uctx, found);
+
+	return found;
+}
+
+struct ib_ucontext *qedr_alloc_ucontext(struct ib_device *ibdev,
+					struct ib_udata *udata)
+{
+	int rc;
+	struct qedr_ucontext *ctx;
+	struct qedr_alloc_ucontext_resp uresp;
+	struct qedr_dev *dev = get_qedr_dev(ibdev);
+	struct qed_rdma_add_user_out_params oparams;
+
+	if (!udata)
+		return ERR_PTR(-EFAULT);
+
+	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
+	if (!ctx)
+		return ERR_PTR(-ENOMEM);
+
+	rc = dev->ops->rdma_add_user(dev->rdma_ctx, &oparams);
+	if (rc) {
+		DP_ERR(dev,
+		       "failed to allocate a DPI for a new RoCE application, rc=%d. To overcome this consider to increase the number of DPIs, increase the doorbell BAR size or just close unnecessary RoCE applications. In order to increase the number of DPIs consult the qedr readme\n",
+		       rc);
+		goto err;
+	}
+
+	ctx->dpi = oparams.dpi;
+	ctx->dpi_addr = oparams.dpi_addr;
+	ctx->dpi_phys_addr = oparams.dpi_phys_addr;
+	ctx->dpi_size = oparams.dpi_size;
+	INIT_LIST_HEAD(&ctx->mm_head);
+	mutex_init(&ctx->mm_list_lock);
+
+	memset(&uresp, 0, sizeof(uresp));
+
+	uresp.db_pa = ctx->dpi_phys_addr;
+	uresp.db_size = ctx->dpi_size;
+	uresp.max_send_wr = dev->attr.max_sqe;
+	uresp.max_recv_wr = dev->attr.max_rqe;
+	uresp.max_srq_wr = dev->attr.max_srq_wr;
+	uresp.sges_per_send_wr = QEDR_MAX_SQE_ELEMENTS_PER_SQE;
+	uresp.sges_per_recv_wr = QEDR_MAX_RQE_ELEMENTS_PER_RQE;
+	uresp.sges_per_srq_wr = dev->attr.max_srq_sge;
+	uresp.max_cqes = QEDR_MAX_CQES;
+
+	rc = ib_copy_to_udata(udata, &uresp, sizeof(uresp));
+	if (rc)
+		goto err;
+
+	ctx->dev = dev;
+
+	rc = qedr_add_mmap(ctx, ctx->dpi_phys_addr, ctx->dpi_size);
+	if (rc)
+		goto err;
+
+	DP_DEBUG(dev, QEDR_MSG_INIT, "Allocating user context %p\n",
+		 &ctx->ibucontext);
+	return &ctx->ibucontext;
+
+err:
+	kfree(ctx);
+	return ERR_PTR(rc);
+}
+
+int qedr_dealloc_ucontext(struct ib_ucontext *ibctx)
+{
+	struct qedr_ucontext *uctx = get_qedr_ucontext(ibctx);
+	struct qedr_mm *mm, *tmp;
+	int status = 0;
+
+	DP_DEBUG(uctx->dev, QEDR_MSG_INIT, "Deallocating user context %p\n",
+		 uctx);
+	uctx->dev->ops->rdma_remove_user(uctx->dev->rdma_ctx, uctx->dpi);
+
+	list_for_each_entry_safe(mm, tmp, &uctx->mm_head, entry) {
+		DP_DEBUG(uctx->dev, QEDR_MSG_MISC,
+			 "deleted (addr=0x%llx,len=0x%lx) for ctx=%p\n",
+			 mm->key.phy_addr, mm->key.len, uctx);
+		list_del(&mm->entry);
+		kfree(mm);
+	}
+
+	kfree(uctx);
+	return status;
+}
+
+int qedr_mmap(struct ib_ucontext *context, struct vm_area_struct *vma)
+{
+	struct qedr_ucontext *ucontext = get_qedr_ucontext(context);
+	struct qedr_dev *dev = get_qedr_dev(context->device);
+	unsigned long vm_page = vma->vm_pgoff << PAGE_SHIFT;
+	u64 unmapped_db = dev->db_phys_addr;
+	unsigned long len = (vma->vm_end - vma->vm_start);
+	int rc = 0;
+	bool found;
+
+	DP_DEBUG(dev, QEDR_MSG_INIT,
+		 "qedr_mmap called vm_page=0x%lx vm_pgoff=0x%lx unmapped_db=0x%llx db_size=%x, len=%lx\n",
+		 vm_page, vma->vm_pgoff, unmapped_db, dev->db_size, len);
+	if (vma->vm_start & (PAGE_SIZE - 1)) {
+		DP_ERR(dev, "Vma_start not page aligned = %ld\n",
+		       vma->vm_start);
+		return -EINVAL;
+	}
+
+	found = qedr_search_mmap(ucontext, vm_page, len);
+	if (!found) {
+		DP_ERR(dev, "Vma_pgoff not found in mapped array = %ld\n",
+		       vma->vm_pgoff);
+		return -EINVAL;
+	}
+
+	DP_DEBUG(dev, QEDR_MSG_INIT, "Mapping doorbell bar\n");
+
+	if ((vm_page >= unmapped_db) && (vm_page <= (unmapped_db +
+						     dev->db_size))) {
+		DP_DEBUG(dev, QEDR_MSG_INIT, "Mapping doorbell bar\n");
+		if (vma->vm_flags & VM_READ) {
+			DP_ERR(dev, "Trying to map doorbell bar for read\n");
+			return -EPERM;
+		}
+
+		vma->vm_page_prot = pgprot_writecombine(vma->vm_page_prot);
+
+		rc = io_remap_pfn_range(vma, vma->vm_start, vma->vm_pgoff,
+					PAGE_SIZE, vma->vm_page_prot);
+	} else {
+		DP_DEBUG(dev, QEDR_MSG_INIT, "Mapping chains\n");
+		rc = remap_pfn_range(vma, vma->vm_start,
+				     vma->vm_pgoff, len, vma->vm_page_prot);
+	}
+	DP_DEBUG(dev, QEDR_MSG_INIT, "qedr_mmap return code: %d\n", rc);
+	return rc;
+}
+
+struct ib_pd *qedr_alloc_pd(struct ib_device *ibdev,
+			    struct ib_ucontext *context, struct ib_udata *udata)
+{
+	struct qedr_dev *dev = get_qedr_dev(ibdev);
+	struct qedr_ucontext *uctx = NULL;
+	struct qedr_alloc_pd_uresp uresp;
+	struct qedr_pd *pd;
+	u16 pd_id;
+	int rc;
+
+	DP_DEBUG(dev, QEDR_MSG_INIT, "Function called from: %s\n",
+		 (udata && context) ? "User Lib" : "Kernel");
+
+	if (!dev->rdma_ctx) {
+		DP_ERR(dev, "invlaid RDMA context\n");
+		return ERR_PTR(-EINVAL);
+	}
+
+	pd = kzalloc(sizeof(*pd), GFP_KERNEL);
+	if (!pd)
+		return ERR_PTR(-ENOMEM);
+
+	dev->ops->rdma_alloc_pd(dev->rdma_ctx, &pd_id);
+
+	uresp.pd_id = pd_id;
+	pd->pd_id = pd_id;
+
+	if (udata && context) {
+		rc = ib_copy_to_udata(udata, &uresp, sizeof(uresp));
+		if (rc)
+			DP_ERR(dev, "copy error pd_id=0x%x.\n", pd_id);
+		uctx = get_qedr_ucontext(context);
+		uctx->pd = pd;
+		pd->uctx = uctx;
+	}
+
+	return &pd->ibpd;
+}
+
+int qedr_dealloc_pd(struct ib_pd *ibpd)
+{
+	struct qedr_dev *dev = get_qedr_dev(ibpd->device);
+	struct qedr_pd *pd = get_qedr_pd(ibpd);
+
+	if (!pd)
+		pr_err("Invalid PD received in dealloc_pd\n");
+
+	DP_DEBUG(dev, QEDR_MSG_INIT, "Deallocating PD %d\n", pd->pd_id);
+	dev->ops->rdma_dealloc_pd(dev->rdma_ctx, pd->pd_id);
+
+	kfree(pd);
+
+	return 0;
+}
+
+static void qedr_free_pbl(struct qedr_dev *dev,
+			  struct qedr_pbl_info *pbl_info, struct qedr_pbl *pbl)
+{
+	struct pci_dev *pdev = dev->pdev;
+	int i;
+
+	for (i = 0; i < pbl_info->num_pbls; i++) {
+		if (!pbl[i].va)
+			continue;
+		dma_free_coherent(&pdev->dev, pbl_info->pbl_size,
+				  pbl[i].va, pbl[i].pa);
+	}
+
+	kfree(pbl);
+}
+
+#define MIN_FW_PBL_PAGE_SIZE (4 * 1024)
+#define MAX_FW_PBL_PAGE_SIZE (64 * 1024)
+
+#define NUM_PBES_ON_PAGE(_page_size) (_page_size / sizeof(u64))
+#define MAX_PBES_ON_PAGE NUM_PBES_ON_PAGE(MAX_FW_PBL_PAGE_SIZE)
+#define MAX_PBES_TWO_LAYER (MAX_PBES_ON_PAGE * MAX_PBES_ON_PAGE)
+
+static struct qedr_pbl *qedr_alloc_pbl_tbl(struct qedr_dev *dev,
+					   struct qedr_pbl_info *pbl_info,
+					   gfp_t flags)
+{
+	struct pci_dev *pdev = dev->pdev;
+	struct qedr_pbl *pbl_table;
+	dma_addr_t *pbl_main_tbl;
+	dma_addr_t pa;
+	void *va;
+	int i;
+
+	pbl_table = kcalloc(pbl_info->num_pbls, sizeof(*pbl_table), flags);
+	if (!pbl_table)
+		return ERR_PTR(-ENOMEM);
+
+	for (i = 0; i < pbl_info->num_pbls; i++) {
+		va = dma_alloc_coherent(&pdev->dev, pbl_info->pbl_size,
+					&pa, flags);
+		if (!va)
+			goto err;
+
+		memset(va, 0, pbl_info->pbl_size);
+		pbl_table[i].va = va;
+		pbl_table[i].pa = pa;
+	}
+
+	/* Two-Layer PBLs, if we have more than one pbl we need to initialize
+	 * the first one with physical pointers to all of the rest
+	 */
+	pbl_main_tbl = (dma_addr_t *)pbl_table[0].va;
+	for (i = 0; i < pbl_info->num_pbls - 1; i++)
+		pbl_main_tbl[i] = pbl_table[i + 1].pa;
+
+	return pbl_table;
+
+err:
+	for (i--; i >= 0; i--)
+		dma_free_coherent(&pdev->dev, pbl_info->pbl_size,
+				  pbl_table[i].va, pbl_table[i].pa);
+
+	qedr_free_pbl(dev, pbl_info, pbl_table);
+
+	return ERR_PTR(-ENOMEM);
+}
+
+static int qedr_prepare_pbl_tbl(struct qedr_dev *dev,
+				struct qedr_pbl_info *pbl_info,
+				u32 num_pbes, int two_layer_capable)
+{
+	u32 pbl_capacity;
+	u32 pbl_size;
+	u32 num_pbls;
+
+	if ((num_pbes > MAX_PBES_ON_PAGE) && two_layer_capable) {
+		if (num_pbes > MAX_PBES_TWO_LAYER) {
+			DP_ERR(dev, "prepare pbl table: too many pages %d\n",
+			       num_pbes);
+			return -EINVAL;
+		}
+
+		/* calculate required pbl page size */
+		pbl_size = MIN_FW_PBL_PAGE_SIZE;
+		pbl_capacity = NUM_PBES_ON_PAGE(pbl_size) *
+			       NUM_PBES_ON_PAGE(pbl_size);
+
+		while (pbl_capacity < num_pbes) {
+			pbl_size *= 2;
+			pbl_capacity = pbl_size / sizeof(u64);
+			pbl_capacity = pbl_capacity * pbl_capacity;
+		}
+
+		num_pbls = DIV_ROUND_UP(num_pbes, NUM_PBES_ON_PAGE(pbl_size));
+		num_pbls++;	/* One for the layer0 ( points to the pbls) */
+		pbl_info->two_layered = true;
+	} else {
+		/* One layered PBL */
+		num_pbls = 1;
+		pbl_size = max_t(u32, MIN_FW_PBL_PAGE_SIZE,
+				 roundup_pow_of_two((num_pbes * sizeof(u64))));
+		pbl_info->two_layered = false;
+	}
+
+	pbl_info->num_pbls = num_pbls;
+	pbl_info->pbl_size = pbl_size;
+	pbl_info->num_pbes = num_pbes;
+
+	DP_DEBUG(dev, QEDR_MSG_MR,
+		 "prepare pbl table: num_pbes=%d, num_pbls=%d, pbl_size=%d\n",
+		 pbl_info->num_pbes, pbl_info->num_pbls, pbl_info->pbl_size);
+
+	return 0;
+}
+
+static void qedr_populate_pbls(struct qedr_dev *dev, struct ib_umem *umem,
+			       struct qedr_pbl *pbl,
+			       struct qedr_pbl_info *pbl_info)
+{
+	int shift, pg_cnt, pages, pbe_cnt, total_num_pbes = 0;
+	struct qedr_pbl *pbl_tbl;
+	struct scatterlist *sg;
+	struct regpair *pbe;
+	int entry;
+	u32 addr;
+
+	if (!pbl_info->num_pbes)
+		return;
+
+	/* If we have a two layered pbl, the first pbl points to the rest
+	 * of the pbls and the first entry lays on the second pbl in the table
+	 */
+	if (pbl_info->two_layered)
+		pbl_tbl = &pbl[1];
+	else
+		pbl_tbl = pbl;
+
+	pbe = (struct regpair *)pbl_tbl->va;
+	if (!pbe) {
+		DP_ERR(dev, "cannot populate PBL due to a NULL PBE\n");
+		return;
+	}
+
+	pbe_cnt = 0;
+
+	shift = ilog2(umem->page_size);
+
+	for_each_sg(umem->sg_head.sgl, sg, umem->nmap, entry) {
+		pages = sg_dma_len(sg) >> shift;
+		for (pg_cnt = 0; pg_cnt < pages; pg_cnt++) {
+			/* store the page address in pbe */
+			pbe->lo = cpu_to_le32(sg_dma_address(sg) +
+					      umem->page_size * pg_cnt);
+			addr = upper_32_bits(sg_dma_address(sg) +
+					     umem->page_size * pg_cnt);
+			pbe->hi = cpu_to_le32(addr);
+			pbe_cnt++;
+			total_num_pbes++;
+			pbe++;
+
+			if (total_num_pbes == pbl_info->num_pbes)
+				return;
+
+			/* If the given pbl is full storing the pbes,
+			 * move to next pbl.
+			 */
+			if (pbe_cnt == (pbl_info->pbl_size / sizeof(u64))) {
+				pbl_tbl++;
+				pbe = (struct regpair *)pbl_tbl->va;
+				pbe_cnt = 0;
+			}
+		}
+	}
+}
+
+static int qedr_copy_cq_uresp(struct qedr_dev *dev,
+			      struct qedr_cq *cq, struct ib_udata *udata)
+{
+	struct qedr_create_cq_uresp uresp;
+	int rc;
+
+	memset(&uresp, 0, sizeof(uresp));
+
+	uresp.db_offset = DB_ADDR_SHIFT(DQ_PWM_OFFSET_UCM_RDMA_CQ_CONS_32BIT);
+	uresp.icid = cq->icid;
+
+	rc = ib_copy_to_udata(udata, &uresp, sizeof(uresp));
+	if (rc)
+		DP_ERR(dev, "copy error cqid=0x%x.\n", cq->icid);
+
+	return rc;
+}
+
+static void consume_cqe(struct qedr_cq *cq)
+{
+	if (cq->latest_cqe == cq->toggle_cqe)
+		cq->pbl_toggle ^= RDMA_CQE_REQUESTER_TOGGLE_BIT_MASK;
+
+	cq->latest_cqe = qed_chain_consume(&cq->pbl);
+}
+
+static inline int qedr_align_cq_entries(int entries)
+{
+	u64 size, aligned_size;
+
+	/* We allocate an extra entry that we don't report to the FW. */
+	size = (entries + 1) * QEDR_CQE_SIZE;
+	aligned_size = ALIGN(size, PAGE_SIZE);
+
+	return aligned_size / QEDR_CQE_SIZE;
+}
+
+static inline int qedr_init_user_queue(struct ib_ucontext *ib_ctx,
+				       struct qedr_dev *dev,
+				       struct qedr_userq *q,
+				       u64 buf_addr, size_t buf_len,
+				       int access, int dmasync)
+{
+	int page_cnt;
+	int rc;
+
+	q->buf_addr = buf_addr;
+	q->buf_len = buf_len;
+	q->umem = ib_umem_get(ib_ctx, q->buf_addr, q->buf_len, access, dmasync);
+	if (IS_ERR(q->umem)) {
+		DP_ERR(dev, "create user queue: failed ib_umem_get, got %ld\n",
+		       PTR_ERR(q->umem));
+		return PTR_ERR(q->umem);
+	}
+
+	page_cnt = ib_umem_page_count(q->umem);
+	rc = qedr_prepare_pbl_tbl(dev, &q->pbl_info, page_cnt, 0);
+	if (rc)
+		goto err0;
+
+	q->pbl_tbl = qedr_alloc_pbl_tbl(dev, &q->pbl_info, GFP_KERNEL);
+	if (IS_ERR_OR_NULL(q->pbl_tbl))
+		goto err0;
+
+	qedr_populate_pbls(dev, q->umem, q->pbl_tbl, &q->pbl_info);
+
+	return 0;
+
+err0:
+	ib_umem_release(q->umem);
+
+	return rc;
+}
+
+static inline void qedr_init_cq_params(struct qedr_cq *cq,
+				       struct qedr_ucontext *ctx,
+				       struct qedr_dev *dev, int vector,
+				       int chain_entries, int page_cnt,
+				       u64 pbl_ptr,
+				       struct qed_rdma_create_cq_in_params
+				       *params)
+{
+	memset(params, 0, sizeof(*params));
+	params->cq_handle_hi = upper_32_bits((uintptr_t)cq);
+	params->cq_handle_lo = lower_32_bits((uintptr_t)cq);
+	params->cnq_id = vector;
+	params->cq_size = chain_entries - 1;
+	params->dpi = (ctx) ? ctx->dpi : dev->dpi;
+	params->pbl_num_pages = page_cnt;
+	params->pbl_ptr = pbl_ptr;
+	params->pbl_two_level = 0;
+}
+
+static void doorbell_cq(struct qedr_cq *cq, u32 cons, u8 flags)
+{
+	/* Flush data before signalling doorbell */
+	wmb();
+	cq->db.data.agg_flags = flags;
+	cq->db.data.value = cpu_to_le32(cons);
+	writeq(cq->db.raw, cq->db_addr);
+
+	/* Make sure write would stick */
+	mmiowb();
+}
+
+int qedr_arm_cq(struct ib_cq *ibcq, enum ib_cq_notify_flags flags)
+{
+	struct qedr_cq *cq = get_qedr_cq(ibcq);
+	unsigned long sflags;
+
+	if (cq->cq_type == QEDR_CQ_TYPE_GSI)
+		return 0;
+
+	spin_lock_irqsave(&cq->cq_lock, sflags);
+
+	cq->arm_flags = 0;
+
+	if (flags & IB_CQ_SOLICITED)
+		cq->arm_flags |= DQ_UCM_ROCE_CQ_ARM_SE_CF_CMD;
+
+	if (flags & IB_CQ_NEXT_COMP)
+		cq->arm_flags |= DQ_UCM_ROCE_CQ_ARM_CF_CMD;
+
+	doorbell_cq(cq, cq->cq_cons - 1, cq->arm_flags);
+
+	spin_unlock_irqrestore(&cq->cq_lock, sflags);
+
+	return 0;
+}
+
+struct ib_cq *qedr_create_cq(struct ib_device *ibdev,
+			     const struct ib_cq_init_attr *attr,
+			     struct ib_ucontext *ib_ctx, struct ib_udata *udata)
+{
+	struct qedr_ucontext *ctx = get_qedr_ucontext(ib_ctx);
+	struct qed_rdma_destroy_cq_out_params destroy_oparams;
+	struct qed_rdma_destroy_cq_in_params destroy_iparams;
+	struct qedr_dev *dev = get_qedr_dev(ibdev);
+	struct qed_rdma_create_cq_in_params params;
+	struct qedr_create_cq_ureq ureq;
+	int vector = attr->comp_vector;
+	int entries = attr->cqe;
+	struct qedr_cq *cq;
+	int chain_entries;
+	int page_cnt;
+	u64 pbl_ptr;
+	u16 icid;
+	int rc;
+
+	DP_DEBUG(dev, QEDR_MSG_INIT,
+		 "create_cq: called from %s. entries=%d, vector=%d\n",
+		 udata ? "User Lib" : "Kernel", entries, vector);
+
+	if (entries > QEDR_MAX_CQES) {
+		DP_ERR(dev,
+		       "create cq: the number of entries %d is too high. Must be equal or below %d.\n",
+		       entries, QEDR_MAX_CQES);
+		return ERR_PTR(-EINVAL);
+	}
+
+	chain_entries = qedr_align_cq_entries(entries);
+	chain_entries = min_t(int, chain_entries, QEDR_MAX_CQES);
+
+	cq = kzalloc(sizeof(*cq), GFP_KERNEL);
+	if (!cq)
+		return ERR_PTR(-ENOMEM);
+
+	if (udata) {
+		memset(&ureq, 0, sizeof(ureq));
+		if (ib_copy_from_udata(&ureq, udata, sizeof(ureq))) {
+			DP_ERR(dev,
+			       "create cq: problem copying data from user space\n");
+			goto err0;
+		}
+
+		if (!ureq.len) {
+			DP_ERR(dev,
+			       "create cq: cannot create a cq with 0 entries\n");
+			goto err0;
+		}
+
+		cq->cq_type = QEDR_CQ_TYPE_USER;
+
+		rc = qedr_init_user_queue(ib_ctx, dev, &cq->q, ureq.addr,
+					  ureq.len, IB_ACCESS_LOCAL_WRITE, 1);
+		if (rc)
+			goto err0;
+
+		pbl_ptr = cq->q.pbl_tbl->pa;
+		page_cnt = cq->q.pbl_info.num_pbes;
+	} else {
+		cq->cq_type = QEDR_CQ_TYPE_KERNEL;
+
+		rc = dev->ops->common->chain_alloc(dev->cdev,
+						   QED_CHAIN_USE_TO_CONSUME,
+						   QED_CHAIN_MODE_PBL,
+						   QED_CHAIN_CNT_TYPE_U32,
+						   chain_entries,
+						   sizeof(union rdma_cqe),
+						   &cq->pbl);
+		if (rc)
+			goto err1;
+
+		page_cnt = qed_chain_get_page_cnt(&cq->pbl);
+		pbl_ptr = qed_chain_get_pbl_phys(&cq->pbl);
+	}
+
+	qedr_init_cq_params(cq, ctx, dev, vector, chain_entries, page_cnt,
+			    pbl_ptr, &params);
+
+	rc = dev->ops->rdma_create_cq(dev->rdma_ctx, &params, &icid);
+	if (rc)
+		goto err2;
+
+	cq->icid = icid;
+	cq->sig = QEDR_CQ_MAGIC_NUMBER;
+	spin_lock_init(&cq->cq_lock);
+
+	if (ib_ctx) {
+		rc = qedr_copy_cq_uresp(dev, cq, udata);
+		if (rc)
+			goto err3;
+	} else {
+		/* Generate doorbell address. */
+		cq->db_addr = dev->db_addr +
+		    DB_ADDR_SHIFT(DQ_PWM_OFFSET_UCM_RDMA_CQ_CONS_32BIT);
+		cq->db.data.icid = cq->icid;
+		cq->db.data.params = DB_AGG_CMD_SET <<
+		    RDMA_PWM_VAL32_DATA_AGG_CMD_SHIFT;
+
+		/* point to the very last element, passing it we will toggle */
+		cq->toggle_cqe = qed_chain_get_last_elem(&cq->pbl);
+		cq->pbl_toggle = RDMA_CQE_REQUESTER_TOGGLE_BIT_MASK;
+		cq->latest_cqe = NULL;
+		consume_cqe(cq);
+		cq->cq_cons = qed_chain_get_cons_idx_u32(&cq->pbl);
+	}
+
+	DP_DEBUG(dev, QEDR_MSG_CQ,
+		 "create cq: icid=0x%0x, addr=%p, size(entries)=0x%0x\n",
+		 cq->icid, cq, params.cq_size);
+
+	return &cq->ibcq;
+
+err3:
+	destroy_iparams.icid = cq->icid;
+	dev->ops->rdma_destroy_cq(dev->rdma_ctx, &destroy_iparams,
+				  &destroy_oparams);
+err2:
+	if (udata)
+		qedr_free_pbl(dev, &cq->q.pbl_info, cq->q.pbl_tbl);
+	else
+		dev->ops->common->chain_free(dev->cdev, &cq->pbl);
+err1:
+	if (udata)
+		ib_umem_release(cq->q.umem);
+err0:
+	kfree(cq);
+	return ERR_PTR(-EINVAL);
+}
+
+int qedr_resize_cq(struct ib_cq *ibcq, int new_cnt, struct ib_udata *udata)
+{
+	struct qedr_dev *dev = get_qedr_dev(ibcq->device);
+	struct qedr_cq *cq = get_qedr_cq(ibcq);
+
+	DP_ERR(dev, "cq %p RESIZE NOT SUPPORTED\n", cq);
+
+	return 0;
+}
+
+int qedr_destroy_cq(struct ib_cq *ibcq)
+{
+	struct qedr_dev *dev = get_qedr_dev(ibcq->device);
+	struct qed_rdma_destroy_cq_out_params oparams;
+	struct qed_rdma_destroy_cq_in_params iparams;
+	struct qedr_cq *cq = get_qedr_cq(ibcq);
+
+	DP_DEBUG(dev, QEDR_MSG_CQ, "destroy cq: cq_id %d", cq->icid);
+
+	/* GSIs CQs are handled by driver, so they don't exist in the FW */
+	if (cq->cq_type != QEDR_CQ_TYPE_GSI) {
+		iparams.icid = cq->icid;
+		dev->ops->rdma_destroy_cq(dev->rdma_ctx, &iparams, &oparams);
+		dev->ops->common->chain_free(dev->cdev, &cq->pbl);
+	}
+
+	if (ibcq->uobject && ibcq->uobject->context) {
+		qedr_free_pbl(dev, &cq->q.pbl_info, cq->q.pbl_tbl);
+		ib_umem_release(cq->q.umem);
+	}
+
+	kfree(cq);
+
+	return 0;
+}
+
+static inline int get_gid_info_from_table(struct ib_qp *ibqp,
+					  struct ib_qp_attr *attr,
+					  int attr_mask,
+					  struct qed_rdma_modify_qp_in_params
+					  *qp_params)
+{
+	enum rdma_network_type nw_type;
+	struct ib_gid_attr gid_attr;
+	union ib_gid gid;
+	u32 ipv4_addr;
+	int rc = 0;
+	int i;
+
+	rc = ib_get_cached_gid(ibqp->device, attr->ah_attr.port_num,
+			       attr->ah_attr.grh.sgid_index, &gid, &gid_attr);
+	if (rc)
+		return rc;
+
+	if (!memcmp(&gid, &zgid, sizeof(gid)))
+		return -ENOENT;
+
+	if (gid_attr.ndev) {
+		qp_params->vlan_id = rdma_vlan_dev_vlan_id(gid_attr.ndev);
+
+		dev_put(gid_attr.ndev);
+		nw_type = ib_gid_to_network_type(gid_attr.gid_type, &gid);
+		switch (nw_type) {
+		case RDMA_NETWORK_IPV6:
+			memcpy(&qp_params->sgid.bytes[0], &gid.raw[0],
+			       sizeof(qp_params->sgid));
+			memcpy(&qp_params->dgid.bytes[0],
+			       &attr->ah_attr.grh.dgid,
+			       sizeof(qp_params->dgid));
+			qp_params->roce_mode = ROCE_V2_IPV6;
+			SET_FIELD(qp_params->modify_flags,
+				  QED_ROCE_MODIFY_QP_VALID_ROCE_MODE, 1);
+			break;
+		case RDMA_NETWORK_IB:
+			memcpy(&qp_params->sgid.bytes[0], &gid.raw[0],
+			       sizeof(qp_params->sgid));
+			memcpy(&qp_params->dgid.bytes[0],
+			       &attr->ah_attr.grh.dgid,
+			       sizeof(qp_params->dgid));
+			qp_params->roce_mode = ROCE_V1;
+			break;
+		case RDMA_NETWORK_IPV4:
+			memset(&qp_params->sgid, 0, sizeof(qp_params->sgid));
+			memset(&qp_params->dgid, 0, sizeof(qp_params->dgid));
+			ipv4_addr = qedr_get_ipv4_from_gid(gid.raw);
+			qp_params->sgid.ipv4_addr = ipv4_addr;
+			ipv4_addr =
+			    qedr_get_ipv4_from_gid(attr->ah_attr.grh.dgid.raw);
+			qp_params->dgid.ipv4_addr = ipv4_addr;
+			SET_FIELD(qp_params->modify_flags,
+				  QED_ROCE_MODIFY_QP_VALID_ROCE_MODE, 1);
+			qp_params->roce_mode = ROCE_V2_IPV4;
+			break;
+		}
+	}
+
+	for (i = 0; i < 4; i++) {
+		qp_params->sgid.dwords[i] = ntohl(qp_params->sgid.dwords[i]);
+		qp_params->dgid.dwords[i] = ntohl(qp_params->dgid.dwords[i]);
+	}
+
+	if (qp_params->vlan_id >= VLAN_CFI_MASK)
+		qp_params->vlan_id = 0;
+
+	return 0;
+}
+
+static void qedr_cleanup_user_sq(struct qedr_dev *dev, struct qedr_qp *qp)
+{
+	qedr_free_pbl(dev, &qp->usq.pbl_info, qp->usq.pbl_tbl);
+	ib_umem_release(qp->usq.umem);
+}
+
+static void qedr_cleanup_user_rq(struct qedr_dev *dev, struct qedr_qp *qp)
+{
+	qedr_free_pbl(dev, &qp->urq.pbl_info, qp->urq.pbl_tbl);
+	ib_umem_release(qp->urq.umem);
+}
+
+static void qedr_cleanup_kernel_sq(struct qedr_dev *dev, struct qedr_qp *qp)
+{
+	dev->ops->common->chain_free(dev->cdev, &qp->sq.pbl);
+	kfree(qp->wqe_wr_id);
+}
+
+static void qedr_cleanup_kernel_rq(struct qedr_dev *dev, struct qedr_qp *qp)
+{
+	dev->ops->common->chain_free(dev->cdev, &qp->rq.pbl);
+	kfree(qp->rqe_wr_id);
+}
+
+static int qedr_check_qp_attrs(struct ib_pd *ibpd, struct qedr_dev *dev,
+			       struct ib_qp_init_attr *attrs)
+{
+	struct qedr_device_attr *qattr = &dev->attr;
+
+	/* QP0... attrs->qp_type == IB_QPT_GSI */
+	if (attrs->qp_type != IB_QPT_RC && attrs->qp_type != IB_QPT_GSI) {
+		DP_DEBUG(dev, QEDR_MSG_QP,
+			 "create qp: unsupported qp type=0x%x requested\n",
+			 attrs->qp_type);
+		return -EINVAL;
+	}
+
+	if (attrs->cap.max_send_wr > qattr->max_sqe) {
+		DP_ERR(dev,
+		       "create qp: cannot create a SQ with %d elements (max_send_wr=0x%x)\n",
+		       attrs->cap.max_send_wr, qattr->max_sqe);
+		return -EINVAL;
+	}
+
+	if (attrs->cap.max_inline_data > qattr->max_inline) {
+		DP_ERR(dev,
+		       "create qp: unsupported inline data size=0x%x requested (max_inline=0x%x)\n",
+		       attrs->cap.max_inline_data, qattr->max_inline);
+		return -EINVAL;
+	}
+
+	if (attrs->cap.max_send_sge > qattr->max_sge) {
+		DP_ERR(dev,
+		       "create qp: unsupported send_sge=0x%x requested (max_send_sge=0x%x)\n",
+		       attrs->cap.max_send_sge, qattr->max_sge);
+		return -EINVAL;
+	}
+
+	if (attrs->cap.max_recv_sge > qattr->max_sge) {
+		DP_ERR(dev,
+		       "create qp: unsupported recv_sge=0x%x requested (max_recv_sge=0x%x)\n",
+		       attrs->cap.max_recv_sge, qattr->max_sge);
+		return -EINVAL;
+	}
+
+	/* Unprivileged user space cannot create special QP */
+	if (ibpd->uobject && attrs->qp_type == IB_QPT_GSI) {
+		DP_ERR(dev,
+		       "create qp: userspace can't create special QPs of type=0x%x\n",
+		       attrs->qp_type);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static void qedr_copy_rq_uresp(struct qedr_create_qp_uresp *uresp,
+			       struct qedr_qp *qp)
+{
+	uresp->rq_db_offset = DB_ADDR_SHIFT(DQ_PWM_OFFSET_TCM_ROCE_RQ_PROD);
+	uresp->rq_icid = qp->icid;
+}
+
+static void qedr_copy_sq_uresp(struct qedr_create_qp_uresp *uresp,
+			       struct qedr_qp *qp)
+{
+	uresp->sq_db_offset = DB_ADDR_SHIFT(DQ_PWM_OFFSET_XCM_RDMA_SQ_PROD);
+	uresp->sq_icid = qp->icid + 1;
+}
+
+static int qedr_copy_qp_uresp(struct qedr_dev *dev,
+			      struct qedr_qp *qp, struct ib_udata *udata)
+{
+	struct qedr_create_qp_uresp uresp;
+	int rc;
+
+	memset(&uresp, 0, sizeof(uresp));
+	qedr_copy_sq_uresp(&uresp, qp);
+	qedr_copy_rq_uresp(&uresp, qp);
+
+	uresp.atomic_supported = dev->atomic_cap != IB_ATOMIC_NONE;
+	uresp.qp_id = qp->qp_id;
+
+	rc = ib_copy_to_udata(udata, &uresp, sizeof(uresp));
+	if (rc)
+		DP_ERR(dev,
+		       "create qp: failed a copy to user space with qp icid=0x%x.\n",
+		       qp->icid);
+
+	return rc;
+}
+
+static void qedr_set_qp_init_params(struct qedr_dev *dev,
+				    struct qedr_qp *qp,
+				    struct qedr_pd *pd,
+				    struct ib_qp_init_attr *attrs)
+{
+	qp->pd = pd;
+
+	spin_lock_init(&qp->q_lock);
+
+	qp->qp_type = attrs->qp_type;
+	qp->max_inline_data = attrs->cap.max_inline_data;
+	qp->sq.max_sges = attrs->cap.max_send_sge;
+	qp->state = QED_ROCE_QP_STATE_RESET;
+	qp->signaled = (attrs->sq_sig_type == IB_SIGNAL_ALL_WR) ? true : false;
+	qp->sq_cq = get_qedr_cq(attrs->send_cq);
+	qp->rq_cq = get_qedr_cq(attrs->recv_cq);
+	qp->dev = dev;
+
+	DP_DEBUG(dev, QEDR_MSG_QP,
+		 "QP params:\tpd = %d, qp_type = %d, max_inline_data = %d, state = %d, signaled = %d, use_srq=%d\n",
+		 pd->pd_id, qp->qp_type, qp->max_inline_data,
+		 qp->state, qp->signaled, (attrs->srq) ? 1 : 0);
+	DP_DEBUG(dev, QEDR_MSG_QP,
+		 "SQ params:\tsq_max_sges = %d, sq_cq_id = %d\n",
+		 qp->sq.max_sges, qp->sq_cq->icid);
+	qp->rq.max_sges = attrs->cap.max_recv_sge;
+	DP_DEBUG(dev, QEDR_MSG_QP,
+		 "RQ params:\trq_max_sges = %d, rq_cq_id = %d\n",
+		 qp->rq.max_sges, qp->rq_cq->icid);
+}
+
+static inline void
+qedr_init_qp_user_params(struct qed_rdma_create_qp_in_params *params,
+			 struct qedr_create_qp_ureq *ureq)
+{
+	/* QP handle to be written in CQE */
+	params->qp_handle_lo = ureq->qp_handle_lo;
+	params->qp_handle_hi = ureq->qp_handle_hi;
+}
+
+static inline void
+qedr_init_qp_kernel_doorbell_sq(struct qedr_dev *dev, struct qedr_qp *qp)
+{
+	qp->sq.db = dev->db_addr +
+		    DB_ADDR_SHIFT(DQ_PWM_OFFSET_XCM_RDMA_SQ_PROD);
+	qp->sq.db_data.data.icid = qp->icid + 1;
+}
+
+static inline void
+qedr_init_qp_kernel_doorbell_rq(struct qedr_dev *dev, struct qedr_qp *qp)
+{
+	qp->rq.db = dev->db_addr +
+		    DB_ADDR_SHIFT(DQ_PWM_OFFSET_TCM_ROCE_RQ_PROD);
+	qp->rq.db_data.data.icid = qp->icid;
+}
+
+static inline int
+qedr_init_qp_kernel_params_rq(struct qedr_dev *dev,
+			      struct qedr_qp *qp, struct ib_qp_init_attr *attrs)
+{
+	/* Allocate driver internal RQ array */
+	qp->rqe_wr_id = kcalloc(qp->rq.max_wr, sizeof(*qp->rqe_wr_id),
+				GFP_KERNEL);
+	if (!qp->rqe_wr_id)
+		return -ENOMEM;
+
+	DP_DEBUG(dev, QEDR_MSG_QP, "RQ max_wr set to %d.\n", qp->rq.max_wr);
+
+	return 0;
+}
+
+static inline int
+qedr_init_qp_kernel_params_sq(struct qedr_dev *dev,
+			      struct qedr_qp *qp,
+			      struct ib_qp_init_attr *attrs,
+			      struct qed_rdma_create_qp_in_params *params)
+{
+	u32 temp_max_wr;
+
+	/* Allocate driver internal SQ array */
+	temp_max_wr = attrs->cap.max_send_wr * dev->wq_multiplier;
+	temp_max_wr = min_t(u32, temp_max_wr, dev->attr.max_sqe);
+
+	/* temp_max_wr < attr->max_sqe < u16 so the casting is safe */
+	qp->sq.max_wr = (u16)temp_max_wr;
+	qp->wqe_wr_id = kcalloc(qp->sq.max_wr, sizeof(*qp->wqe_wr_id),
+				GFP_KERNEL);
+	if (!qp->wqe_wr_id)
+		return -ENOMEM;
+
+	DP_DEBUG(dev, QEDR_MSG_QP, "SQ max_wr set to %d.\n", qp->sq.max_wr);
+
+	/* QP handle to be written in CQE */
+	params->qp_handle_lo = lower_32_bits((uintptr_t)qp);
+	params->qp_handle_hi = upper_32_bits((uintptr_t)qp);
+
+	return 0;
+}
+
+static inline int qedr_init_qp_kernel_sq(struct qedr_dev *dev,
+					 struct qedr_qp *qp,
+					 struct ib_qp_init_attr *attrs)
+{
+	u32 n_sq_elems, n_sq_entries;
+	int rc;
+
+	/* A single work request may take up to QEDR_MAX_SQ_WQE_SIZE elements in
+	 * the ring. The ring should allow at least a single WR, even if the
+	 * user requested none, due to allocation issues.
+	 */
+	n_sq_entries = attrs->cap.max_send_wr;
+	n_sq_entries = min_t(u32, n_sq_entries, dev->attr.max_sqe);
+	n_sq_entries = max_t(u32, n_sq_entries, 1);
+	n_sq_elems = n_sq_entries * QEDR_MAX_SQE_ELEMENTS_PER_SQE;
+	rc = dev->ops->common->chain_alloc(dev->cdev,
+					   QED_CHAIN_USE_TO_PRODUCE,
+					   QED_CHAIN_MODE_PBL,
+					   QED_CHAIN_CNT_TYPE_U32,
+					   n_sq_elems,
+					   QEDR_SQE_ELEMENT_SIZE,
+					   &qp->sq.pbl);
+	if (rc) {
+		DP_ERR(dev, "failed to allocate QP %p SQ\n", qp);
+		return rc;
+	}
+
+	DP_DEBUG(dev, QEDR_MSG_SQ,
+		 "SQ Pbl base addr = %llx max_send_wr=%d max_wr=%d capacity=%d, rc=%d\n",
+		 qed_chain_get_pbl_phys(&qp->sq.pbl), attrs->cap.max_send_wr,
+		 n_sq_entries, qed_chain_get_capacity(&qp->sq.pbl), rc);
+	return 0;
+}
+
+static inline int qedr_init_qp_kernel_rq(struct qedr_dev *dev,
+					 struct qedr_qp *qp,
+					 struct ib_qp_init_attr *attrs)
+{
+	u32 n_rq_elems, n_rq_entries;
+	int rc;
+
+	/* A single work request may take up to QEDR_MAX_RQ_WQE_SIZE elements in
+	 * the ring. There ring should allow at least a single WR, even if the
+	 * user requested none, due to allocation issues.
+	 */
+	n_rq_entries = max_t(u32, attrs->cap.max_recv_wr, 1);
+	n_rq_elems = n_rq_entries * QEDR_MAX_RQE_ELEMENTS_PER_RQE;
+	rc = dev->ops->common->chain_alloc(dev->cdev,
+					   QED_CHAIN_USE_TO_CONSUME_PRODUCE,
+					   QED_CHAIN_MODE_PBL,
+					   QED_CHAIN_CNT_TYPE_U32,
+					   n_rq_elems,
+					   QEDR_RQE_ELEMENT_SIZE,
+					   &qp->rq.pbl);
+
+	if (rc) {
+		DP_ERR(dev, "failed to allocate memory for QP %p RQ\n", qp);
+		return -ENOMEM;
+	}
+
+	DP_DEBUG(dev, QEDR_MSG_RQ,
+		 "RQ Pbl base addr = %llx max_recv_wr=%d max_wr=%d capacity=%d, rc=%d\n",
+		 qed_chain_get_pbl_phys(&qp->rq.pbl), attrs->cap.max_recv_wr,
+		 n_rq_entries, qed_chain_get_capacity(&qp->rq.pbl), rc);
+
+	/* n_rq_entries < u16 so the casting is safe */
+	qp->rq.max_wr = (u16)n_rq_entries;
+
+	return 0;
+}
+
+static inline void
+qedr_init_qp_in_params_sq(struct qedr_dev *dev,
+			  struct qedr_pd *pd,
+			  struct qedr_qp *qp,
+			  struct ib_qp_init_attr *attrs,
+			  struct ib_udata *udata,
+			  struct qed_rdma_create_qp_in_params *params)
+{
+	/* QP handle to be written in an async event */
+	params->qp_handle_async_lo = lower_32_bits((uintptr_t)qp);
+	params->qp_handle_async_hi = upper_32_bits((uintptr_t)qp);
+
+	params->signal_all = (attrs->sq_sig_type == IB_SIGNAL_ALL_WR);
+	params->fmr_and_reserved_lkey = !udata;
+	params->pd = pd->pd_id;
+	params->dpi = pd->uctx ? pd->uctx->dpi : dev->dpi;
+	params->sq_cq_id = get_qedr_cq(attrs->send_cq)->icid;
+	params->max_sq_sges = 0;
+	params->stats_queue = 0;
+
+	if (udata) {
+		params->sq_num_pages = qp->usq.pbl_info.num_pbes;
+		params->sq_pbl_ptr = qp->usq.pbl_tbl->pa;
+	} else {
+		params->sq_num_pages = qed_chain_get_page_cnt(&qp->sq.pbl);
+		params->sq_pbl_ptr = qed_chain_get_pbl_phys(&qp->sq.pbl);
+	}
+}
+
+static inline void
+qedr_init_qp_in_params_rq(struct qedr_qp *qp,
+			  struct ib_qp_init_attr *attrs,
+			  struct ib_udata *udata,
+			  struct qed_rdma_create_qp_in_params *params)
+{
+	params->rq_cq_id = get_qedr_cq(attrs->recv_cq)->icid;
+	params->srq_id = 0;
+	params->use_srq = false;
+
+	if (udata) {
+		params->rq_num_pages = qp->urq.pbl_info.num_pbes;
+		params->rq_pbl_ptr = qp->urq.pbl_tbl->pa;
+	} else {
+		params->rq_num_pages = qed_chain_get_page_cnt(&qp->rq.pbl);
+		params->rq_pbl_ptr = qed_chain_get_pbl_phys(&qp->rq.pbl);
+	}
+}
+
+static inline void qedr_qp_user_print(struct qedr_dev *dev, struct qedr_qp *qp)
+{
+	DP_DEBUG(dev, QEDR_MSG_QP,
+		 "create qp: successfully created user QP. qp=%p, sq_addr=0x%llx, sq_len=%zd, rq_addr=0x%llx, rq_len=%zd\n",
+		 qp, qp->usq.buf_addr, qp->usq.buf_len, qp->urq.buf_addr,
+		 qp->urq.buf_len);
+}
+
+static inline int qedr_init_user_qp(struct ib_ucontext *ib_ctx,
+				    struct qedr_dev *dev,
+				    struct qedr_qp *qp,
+				    struct qedr_create_qp_ureq *ureq)
+{
+	int rc;
+
+	/* SQ - read access only (0), dma sync not required (0) */
+	rc = qedr_init_user_queue(ib_ctx, dev, &qp->usq, ureq->sq_addr,
+				  ureq->sq_len, 0, 0);
+	if (rc)
+		return rc;
+
+	/* RQ - read access only (0), dma sync not required (0) */
+	rc = qedr_init_user_queue(ib_ctx, dev, &qp->urq, ureq->rq_addr,
+				  ureq->rq_len, 0, 0);
+
+	if (rc)
+		qedr_cleanup_user_sq(dev, qp);
+	return rc;
+}
+
+static inline int
+qedr_init_kernel_qp(struct qedr_dev *dev,
+		    struct qedr_qp *qp,
+		    struct ib_qp_init_attr *attrs,
+		    struct qed_rdma_create_qp_in_params *params)
+{
+	int rc;
+
+	rc = qedr_init_qp_kernel_sq(dev, qp, attrs);
+	if (rc) {
+		DP_ERR(dev, "failed to init kernel QP %p SQ\n", qp);
+		return rc;
+	}
+
+	rc = qedr_init_qp_kernel_params_sq(dev, qp, attrs, params);
+	if (rc) {
+		dev->ops->common->chain_free(dev->cdev, &qp->sq.pbl);
+		DP_ERR(dev, "failed to init kernel QP %p SQ params\n", qp);
+		return rc;
+	}
+
+	rc = qedr_init_qp_kernel_rq(dev, qp, attrs);
+	if (rc) {
+		qedr_cleanup_kernel_sq(dev, qp);
+		DP_ERR(dev, "failed to init kernel QP %p RQ\n", qp);
+		return rc;
+	}
+
+	rc = qedr_init_qp_kernel_params_rq(dev, qp, attrs);
+	if (rc) {
+		DP_ERR(dev, "failed to init kernel QP %p RQ params\n", qp);
+		qedr_cleanup_kernel_sq(dev, qp);
+		dev->ops->common->chain_free(dev->cdev, &qp->rq.pbl);
+		return rc;
+	}
+
+	return rc;
+}
+
+struct ib_qp *qedr_create_qp(struct ib_pd *ibpd,
+			     struct ib_qp_init_attr *attrs,
+			     struct ib_udata *udata)
+{
+	struct qedr_dev *dev = get_qedr_dev(ibpd->device);
+	struct qed_rdma_create_qp_out_params out_params;
+	struct qed_rdma_create_qp_in_params in_params;
+	struct qedr_pd *pd = get_qedr_pd(ibpd);
+	struct ib_ucontext *ib_ctx = NULL;
+	struct qedr_ucontext *ctx = NULL;
+	struct qedr_create_qp_ureq ureq;
+	struct qedr_qp *qp;
+	int rc = 0;
+
+	DP_DEBUG(dev, QEDR_MSG_QP, "create qp: called from %s, pd=%p\n",
+		 udata ? "user library" : "kernel", pd);
+
+	rc = qedr_check_qp_attrs(ibpd, dev, attrs);
+	if (rc)
+		return ERR_PTR(rc);
+
+	qp = kzalloc(sizeof(*qp), GFP_KERNEL);
+	if (!qp)
+		return ERR_PTR(-ENOMEM);
+
+	if (attrs->srq)
+		return ERR_PTR(-EINVAL);
+
+	DP_DEBUG(dev, QEDR_MSG_QP,
+		 "create qp: sq_cq=%p, sq_icid=%d, rq_cq=%p, rq_icid=%d\n",
+		 get_qedr_cq(attrs->send_cq),
+		 get_qedr_cq(attrs->send_cq)->icid,
+		 get_qedr_cq(attrs->recv_cq),
+		 get_qedr_cq(attrs->recv_cq)->icid);
+
+	qedr_set_qp_init_params(dev, qp, pd, attrs);
+
+	if (attrs->qp_type == IB_QPT_GSI) {
+		if (udata) {
+			DP_ERR(dev,
+			       "create qp: unexpected udata when creating GSI QP\n");
+			goto err0;
+		}
+		return qedr_create_gsi_qp(dev, attrs, qp);
+	}
+
+	memset(&in_params, 0, sizeof(in_params));
+
+	if (udata) {
+		if (!(udata && ibpd->uobject && ibpd->uobject->context))
+			goto err0;
+
+		ib_ctx = ibpd->uobject->context;
+		ctx = get_qedr_ucontext(ib_ctx);
+
+		memset(&ureq, 0, sizeof(ureq));
+		if (ib_copy_from_udata(&ureq, udata, sizeof(ureq))) {
+			DP_ERR(dev,
+			       "create qp: problem copying data from user space\n");
+			goto err0;
+		}
+
+		rc = qedr_init_user_qp(ib_ctx, dev, qp, &ureq);
+		if (rc)
+			goto err0;
+
+		qedr_init_qp_user_params(&in_params, &ureq);
+	} else {
+		rc = qedr_init_kernel_qp(dev, qp, attrs, &in_params);
+		if (rc)
+			goto err0;
+	}
+
+	qedr_init_qp_in_params_sq(dev, pd, qp, attrs, udata, &in_params);
+	qedr_init_qp_in_params_rq(qp, attrs, udata, &in_params);
+
+	qp->qed_qp = dev->ops->rdma_create_qp(dev->rdma_ctx,
+					      &in_params, &out_params);
+
+	if (!qp->qed_qp)
+		goto err1;
+
+	qp->qp_id = out_params.qp_id;
+	qp->icid = out_params.icid;
+	qp->ibqp.qp_num = qp->qp_id;
+
+	if (udata) {
+		rc = qedr_copy_qp_uresp(dev, qp, udata);
+		if (rc)
+			goto err2;
+
+		qedr_qp_user_print(dev, qp);
+	} else {
+		qedr_init_qp_kernel_doorbell_sq(dev, qp);
+		qedr_init_qp_kernel_doorbell_rq(dev, qp);
+	}
+
+	DP_DEBUG(dev, QEDR_MSG_QP, "created %s space QP %p\n",
+		 udata ? "user" : "kernel", qp);
+
+	return &qp->ibqp;
+
+err2:
+	rc = dev->ops->rdma_destroy_qp(dev->rdma_ctx, qp->qed_qp);
+	if (rc)
+		DP_ERR(dev, "create qp: fatal fault. rc=%d", rc);
+err1:
+	if (udata) {
+		qedr_cleanup_user_sq(dev, qp);
+		qedr_cleanup_user_rq(dev, qp);
+	} else {
+		qedr_cleanup_kernel_sq(dev, qp);
+		qedr_cleanup_kernel_rq(dev, qp);
+	}
+
+err0:
+	kfree(qp);
+
+	return ERR_PTR(-EFAULT);
+}
+
+enum ib_qp_state qedr_get_ibqp_state(enum qed_roce_qp_state qp_state)
+{
+	switch (qp_state) {
+	case QED_ROCE_QP_STATE_RESET:
+		return IB_QPS_RESET;
+	case QED_ROCE_QP_STATE_INIT:
+		return IB_QPS_INIT;
+	case QED_ROCE_QP_STATE_RTR:
+		return IB_QPS_RTR;
+	case QED_ROCE_QP_STATE_RTS:
+		return IB_QPS_RTS;
+	case QED_ROCE_QP_STATE_SQD:
+		return IB_QPS_SQD;
+	case QED_ROCE_QP_STATE_ERR:
+		return IB_QPS_ERR;
+	case QED_ROCE_QP_STATE_SQE:
+		return IB_QPS_SQE;
+	}
+	return IB_QPS_ERR;
+}
+
+enum qed_roce_qp_state qedr_get_state_from_ibqp(enum ib_qp_state qp_state)
+{
+	switch (qp_state) {
+	case IB_QPS_RESET:
+		return QED_ROCE_QP_STATE_RESET;
+	case IB_QPS_INIT:
+		return QED_ROCE_QP_STATE_INIT;
+	case IB_QPS_RTR:
+		return QED_ROCE_QP_STATE_RTR;
+	case IB_QPS_RTS:
+		return QED_ROCE_QP_STATE_RTS;
+	case IB_QPS_SQD:
+		return QED_ROCE_QP_STATE_SQD;
+	case IB_QPS_ERR:
+		return QED_ROCE_QP_STATE_ERR;
+	default:
+		return QED_ROCE_QP_STATE_ERR;
+	}
+}
+
+static void qedr_reset_qp_hwq_info(struct qedr_qp_hwq_info *qph)
+{
+	qed_chain_reset(&qph->pbl);
+	qph->prod = 0;
+	qph->cons = 0;
+	qph->wqe_cons = 0;
+	qph->db_data.data.value = cpu_to_le16(0);
+}
+
+static int qedr_update_qp_state(struct qedr_dev *dev,
+				struct qedr_qp *qp,
+				enum qed_roce_qp_state new_state)
+{
+	int status = 0;
+
+	if (new_state == qp->state)
+		return 1;
+
+	switch (qp->state) {
+	case QED_ROCE_QP_STATE_RESET:
+		switch (new_state) {
+		case QED_ROCE_QP_STATE_INIT:
+			qp->prev_wqe_size = 0;
+			qedr_reset_qp_hwq_info(&qp->sq);
+			qedr_reset_qp_hwq_info(&qp->rq);
+			break;
+		default:
+			status = -EINVAL;
+			break;
+		};
+		break;
+	case QED_ROCE_QP_STATE_INIT:
+		switch (new_state) {
+		case QED_ROCE_QP_STATE_RTR:
+			/* Update doorbell (in case post_recv was
+			 * done before move to RTR)
+			 */
+			wmb();
+			writel(qp->rq.db_data.raw, qp->rq.db);
+			/* Make sure write takes effect */
+			mmiowb();
+			break;
+		case QED_ROCE_QP_STATE_ERR:
+			break;
+		default:
+			/* Invalid state change. */
+			status = -EINVAL;
+			break;
+		};
+		break;
+	case QED_ROCE_QP_STATE_RTR:
+		/* RTR->XXX */
+		switch (new_state) {
+		case QED_ROCE_QP_STATE_RTS:
+			break;
+		case QED_ROCE_QP_STATE_ERR:
+			break;
+		default:
+			/* Invalid state change. */
+			status = -EINVAL;
+			break;
+		};
+		break;
+	case QED_ROCE_QP_STATE_RTS:
+		/* RTS->XXX */
+		switch (new_state) {
+		case QED_ROCE_QP_STATE_SQD:
+			break;
+		case QED_ROCE_QP_STATE_ERR:
+			break;
+		default:
+			/* Invalid state change. */
+			status = -EINVAL;
+			break;
+		};
+		break;
+	case QED_ROCE_QP_STATE_SQD:
+		/* SQD->XXX */
+		switch (new_state) {
+		case QED_ROCE_QP_STATE_RTS:
+		case QED_ROCE_QP_STATE_ERR:
+			break;
+		default:
+			/* Invalid state change. */
+			status = -EINVAL;
+			break;
+		};
+		break;
+	case QED_ROCE_QP_STATE_ERR:
+		/* ERR->XXX */
+		switch (new_state) {
+		case QED_ROCE_QP_STATE_RESET:
+			break;
+		default:
+			status = -EINVAL;
+			break;
+		};
+		break;
+	default:
+		status = -EINVAL;
+		break;
+	};
+
+	return status;
+}
+
+int qedr_modify_qp(struct ib_qp *ibqp, struct ib_qp_attr *attr,
+		   int attr_mask, struct ib_udata *udata)
+{
+	struct qedr_qp *qp = get_qedr_qp(ibqp);
+	struct qed_rdma_modify_qp_in_params qp_params = { 0 };
+	struct qedr_dev *dev = get_qedr_dev(&qp->dev->ibdev);
+	enum ib_qp_state old_qp_state, new_qp_state;
+	int rc = 0;
+
+	DP_DEBUG(dev, QEDR_MSG_QP,
+		 "modify qp: qp %p attr_mask=0x%x, state=%d", qp, attr_mask,
+		 attr->qp_state);
+
+	old_qp_state = qedr_get_ibqp_state(qp->state);
+	if (attr_mask & IB_QP_STATE)
+		new_qp_state = attr->qp_state;
+	else
+		new_qp_state = old_qp_state;
+
+	if (!ib_modify_qp_is_ok
+	    (old_qp_state, new_qp_state, ibqp->qp_type, attr_mask,
+	     IB_LINK_LAYER_ETHERNET)) {
+		DP_ERR(dev,
+		       "modify qp: invalid attribute mask=0x%x specified for\n"
+		       "qpn=0x%x of type=0x%x old_qp_state=0x%x, new_qp_state=0x%x\n",
+		       attr_mask, qp->qp_id, ibqp->qp_type, old_qp_state,
+		       new_qp_state);
+		rc = -EINVAL;
+		goto err;
+	}
+
+	/* Translate the masks... */
+	if (attr_mask & IB_QP_STATE) {
+		SET_FIELD(qp_params.modify_flags,
+			  QED_RDMA_MODIFY_QP_VALID_NEW_STATE, 1);
+		qp_params.new_state = qedr_get_state_from_ibqp(attr->qp_state);
+	}
+
+	if (attr_mask & IB_QP_EN_SQD_ASYNC_NOTIFY)
+		qp_params.sqd_async = true;
+
+	if (attr_mask & IB_QP_PKEY_INDEX) {
+		SET_FIELD(qp_params.modify_flags,
+			  QED_ROCE_MODIFY_QP_VALID_PKEY, 1);
+		if (attr->pkey_index >= QEDR_ROCE_PKEY_TABLE_LEN) {
+			rc = -EINVAL;
+			goto err;
+		}
+
+		qp_params.pkey = QEDR_ROCE_PKEY_DEFAULT;
+	}
+
+	if (attr_mask & IB_QP_QKEY)
+		qp->qkey = attr->qkey;
+
+	if (attr_mask & IB_QP_ACCESS_FLAGS) {
+		SET_FIELD(qp_params.modify_flags,
+			  QED_RDMA_MODIFY_QP_VALID_RDMA_OPS_EN, 1);
+		qp_params.incoming_rdma_read_en = attr->qp_access_flags &
+						  IB_ACCESS_REMOTE_READ;
+		qp_params.incoming_rdma_write_en = attr->qp_access_flags &
+						   IB_ACCESS_REMOTE_WRITE;
+		qp_params.incoming_atomic_en = attr->qp_access_flags &
+					       IB_ACCESS_REMOTE_ATOMIC;
+	}
+
+	if (attr_mask & (IB_QP_AV | IB_QP_PATH_MTU)) {
+		if (attr_mask & IB_QP_PATH_MTU) {
+			if (attr->path_mtu < IB_MTU_256 ||
+			    attr->path_mtu > IB_MTU_4096) {
+				pr_err("error: Only MTU sizes of 256, 512, 1024, 2048 and 4096 are supported by RoCE\n");
+				rc = -EINVAL;
+				goto err;
+			}
+			qp->mtu = min(ib_mtu_enum_to_int(attr->path_mtu),
+				      ib_mtu_enum_to_int(iboe_get_mtu
+							 (dev->ndev->mtu)));
+		}
+
+		if (!qp->mtu) {
+			qp->mtu =
+			ib_mtu_enum_to_int(iboe_get_mtu(dev->ndev->mtu));
+			pr_err("Fixing zeroed MTU to qp->mtu = %d\n", qp->mtu);
+		}
+
+		SET_FIELD(qp_params.modify_flags,
+			  QED_ROCE_MODIFY_QP_VALID_ADDRESS_VECTOR, 1);
+
+		qp_params.traffic_class_tos = attr->ah_attr.grh.traffic_class;
+		qp_params.flow_label = attr->ah_attr.grh.flow_label;
+		qp_params.hop_limit_ttl = attr->ah_attr.grh.hop_limit;
+
+		qp->sgid_idx = attr->ah_attr.grh.sgid_index;
+
+		rc = get_gid_info_from_table(ibqp, attr, attr_mask, &qp_params);
+		if (rc) {
+			DP_ERR(dev,
+			       "modify qp: problems with GID index %d (rc=%d)\n",
+			       attr->ah_attr.grh.sgid_index, rc);
+			return rc;
+		}
+
+		rc = qedr_get_dmac(dev, &attr->ah_attr,
+				   qp_params.remote_mac_addr);
+		if (rc)
+			return rc;
+
+		qp_params.use_local_mac = true;
+		ether_addr_copy(qp_params.local_mac_addr, dev->ndev->dev_addr);
+
+		DP_DEBUG(dev, QEDR_MSG_QP, "dgid=%x:%x:%x:%x\n",
+			 qp_params.dgid.dwords[0], qp_params.dgid.dwords[1],
+			 qp_params.dgid.dwords[2], qp_params.dgid.dwords[3]);
+		DP_DEBUG(dev, QEDR_MSG_QP, "sgid=%x:%x:%x:%x\n",
+			 qp_params.sgid.dwords[0], qp_params.sgid.dwords[1],
+			 qp_params.sgid.dwords[2], qp_params.sgid.dwords[3]);
+		DP_DEBUG(dev, QEDR_MSG_QP, "remote_mac=[%pM]\n",
+			 qp_params.remote_mac_addr);
+;
+
+		qp_params.mtu = qp->mtu;
+		qp_params.lb_indication = false;
+	}
+
+	if (!qp_params.mtu) {
+		/* Stay with current MTU */
+		if (qp->mtu)
+			qp_params.mtu = qp->mtu;
+		else
+			qp_params.mtu =
+			    ib_mtu_enum_to_int(iboe_get_mtu(dev->ndev->mtu));
+	}
+
+	if (attr_mask & IB_QP_TIMEOUT) {
+		SET_FIELD(qp_params.modify_flags,
+			  QED_ROCE_MODIFY_QP_VALID_ACK_TIMEOUT, 1);
+
+		qp_params.ack_timeout = attr->timeout;
+		if (attr->timeout) {
+			u32 temp;
+
+			temp = 4096 * (1UL << attr->timeout) / 1000 / 1000;
+			/* FW requires [msec] */
+			qp_params.ack_timeout = temp;
+		} else {
+			/* Infinite */
+			qp_params.ack_timeout = 0;
+		}
+	}
+	if (attr_mask & IB_QP_RETRY_CNT) {
+		SET_FIELD(qp_params.modify_flags,
+			  QED_ROCE_MODIFY_QP_VALID_RETRY_CNT, 1);
+		qp_params.retry_cnt = attr->retry_cnt;
+	}
+
+	if (attr_mask & IB_QP_RNR_RETRY) {
+		SET_FIELD(qp_params.modify_flags,
+			  QED_ROCE_MODIFY_QP_VALID_RNR_RETRY_CNT, 1);
+		qp_params.rnr_retry_cnt = attr->rnr_retry;
+	}
+
+	if (attr_mask & IB_QP_RQ_PSN) {
+		SET_FIELD(qp_params.modify_flags,
+			  QED_ROCE_MODIFY_QP_VALID_RQ_PSN, 1);
+		qp_params.rq_psn = attr->rq_psn;
+		qp->rq_psn = attr->rq_psn;
+	}
+
+	if (attr_mask & IB_QP_MAX_QP_RD_ATOMIC) {
+		if (attr->max_rd_atomic > dev->attr.max_qp_req_rd_atomic_resc) {
+			rc = -EINVAL;
+			DP_ERR(dev,
+			       "unsupported max_rd_atomic=%d, supported=%d\n",
+			       attr->max_rd_atomic,
+			       dev->attr.max_qp_req_rd_atomic_resc);
+			goto err;
+		}
+
+		SET_FIELD(qp_params.modify_flags,
+			  QED_RDMA_MODIFY_QP_VALID_MAX_RD_ATOMIC_REQ, 1);
+		qp_params.max_rd_atomic_req = attr->max_rd_atomic;
+	}
+
+	if (attr_mask & IB_QP_MIN_RNR_TIMER) {
+		SET_FIELD(qp_params.modify_flags,
+			  QED_ROCE_MODIFY_QP_VALID_MIN_RNR_NAK_TIMER, 1);
+		qp_params.min_rnr_nak_timer = attr->min_rnr_timer;
+	}
+
+	if (attr_mask & IB_QP_SQ_PSN) {
+		SET_FIELD(qp_params.modify_flags,
+			  QED_ROCE_MODIFY_QP_VALID_SQ_PSN, 1);
+		qp_params.sq_psn = attr->sq_psn;
+		qp->sq_psn = attr->sq_psn;
+	}
+
+	if (attr_mask & IB_QP_MAX_DEST_RD_ATOMIC) {
+		if (attr->max_dest_rd_atomic >
+		    dev->attr.max_qp_resp_rd_atomic_resc) {
+			DP_ERR(dev,
+			       "unsupported max_dest_rd_atomic=%d, supported=%d\n",
+			       attr->max_dest_rd_atomic,
+			       dev->attr.max_qp_resp_rd_atomic_resc);
+
+			rc = -EINVAL;
+			goto err;
+		}
+
+		SET_FIELD(qp_params.modify_flags,
+			  QED_RDMA_MODIFY_QP_VALID_MAX_RD_ATOMIC_RESP, 1);
+		qp_params.max_rd_atomic_resp = attr->max_dest_rd_atomic;
+	}
+
+	if (attr_mask & IB_QP_DEST_QPN) {
+		SET_FIELD(qp_params.modify_flags,
+			  QED_ROCE_MODIFY_QP_VALID_DEST_QP, 1);
+
+		qp_params.dest_qp = attr->dest_qp_num;
+		qp->dest_qp_num = attr->dest_qp_num;
+	}
+
+	if (qp->qp_type != IB_QPT_GSI)
+		rc = dev->ops->rdma_modify_qp(dev->rdma_ctx,
+					      qp->qed_qp, &qp_params);
+
+	if (attr_mask & IB_QP_STATE) {
+		if ((qp->qp_type != IB_QPT_GSI) && (!udata))
+			qedr_update_qp_state(dev, qp, qp_params.new_state);
+		qp->state = qp_params.new_state;
+	}
+
+err:
+	return rc;
+}
+
+static int qedr_to_ib_qp_acc_flags(struct qed_rdma_query_qp_out_params *params)
+{
+	int ib_qp_acc_flags = 0;
+
+	if (params->incoming_rdma_write_en)
+		ib_qp_acc_flags |= IB_ACCESS_REMOTE_WRITE;
+	if (params->incoming_rdma_read_en)
+		ib_qp_acc_flags |= IB_ACCESS_REMOTE_READ;
+	if (params->incoming_atomic_en)
+		ib_qp_acc_flags |= IB_ACCESS_REMOTE_ATOMIC;
+	ib_qp_acc_flags |= IB_ACCESS_LOCAL_WRITE;
+	return ib_qp_acc_flags;
+}
+
+int qedr_query_qp(struct ib_qp *ibqp,
+		  struct ib_qp_attr *qp_attr,
+		  int attr_mask, struct ib_qp_init_attr *qp_init_attr)
+{
+	struct qed_rdma_query_qp_out_params params;
+	struct qedr_qp *qp = get_qedr_qp(ibqp);
+	struct qedr_dev *dev = qp->dev;
+	int rc = 0;
+
+	memset(&params, 0, sizeof(params));
+
+	rc = dev->ops->rdma_query_qp(dev->rdma_ctx, qp->qed_qp, &params);
+	if (rc)
+		goto err;
+
+	memset(qp_attr, 0, sizeof(*qp_attr));
+	memset(qp_init_attr, 0, sizeof(*qp_init_attr));
+
+	qp_attr->qp_state = qedr_get_ibqp_state(params.state);
+	qp_attr->cur_qp_state = qedr_get_ibqp_state(params.state);
+	qp_attr->path_mtu = iboe_get_mtu(params.mtu);
+	qp_attr->path_mig_state = IB_MIG_MIGRATED;
+	qp_attr->rq_psn = params.rq_psn;
+	qp_attr->sq_psn = params.sq_psn;
+	qp_attr->dest_qp_num = params.dest_qp;
+
+	qp_attr->qp_access_flags = qedr_to_ib_qp_acc_flags(&params);
+
+	qp_attr->cap.max_send_wr = qp->sq.max_wr;
+	qp_attr->cap.max_recv_wr = qp->rq.max_wr;
+	qp_attr->cap.max_send_sge = qp->sq.max_sges;
+	qp_attr->cap.max_recv_sge = qp->rq.max_sges;
+	qp_attr->cap.max_inline_data = qp->max_inline_data;
+	qp_init_attr->cap = qp_attr->cap;
+
+	memcpy(&qp_attr->ah_attr.grh.dgid.raw[0], &params.dgid.bytes[0],
+	       sizeof(qp_attr->ah_attr.grh.dgid.raw));
+
+	qp_attr->ah_attr.grh.flow_label = params.flow_label;
+	qp_attr->ah_attr.grh.sgid_index = qp->sgid_idx;
+	qp_attr->ah_attr.grh.hop_limit = params.hop_limit_ttl;
+	qp_attr->ah_attr.grh.traffic_class = params.traffic_class_tos;
+
+	qp_attr->ah_attr.ah_flags = IB_AH_GRH;
+	qp_attr->ah_attr.port_num = 1;
+	qp_attr->ah_attr.sl = 0;
+	qp_attr->timeout = params.timeout;
+	qp_attr->rnr_retry = params.rnr_retry;
+	qp_attr->retry_cnt = params.retry_cnt;
+	qp_attr->min_rnr_timer = params.min_rnr_nak_timer;
+	qp_attr->pkey_index = params.pkey_index;
+	qp_attr->port_num = 1;
+	qp_attr->ah_attr.src_path_bits = 0;
+	qp_attr->ah_attr.static_rate = 0;
+	qp_attr->alt_pkey_index = 0;
+	qp_attr->alt_port_num = 0;
+	qp_attr->alt_timeout = 0;
+	memset(&qp_attr->alt_ah_attr, 0, sizeof(qp_attr->alt_ah_attr));
+
+	qp_attr->sq_draining = (params.state == QED_ROCE_QP_STATE_SQD) ? 1 : 0;
+	qp_attr->max_dest_rd_atomic = params.max_dest_rd_atomic;
+	qp_attr->max_rd_atomic = params.max_rd_atomic;
+	qp_attr->en_sqd_async_notify = (params.sqd_async) ? 1 : 0;
+
+	DP_DEBUG(dev, QEDR_MSG_QP, "QEDR_QUERY_QP: max_inline_data=%d\n",
+		 qp_attr->cap.max_inline_data);
+
+err:
+	return rc;
+}
+
+int qedr_destroy_qp(struct ib_qp *ibqp)
+{
+	struct qedr_qp *qp = get_qedr_qp(ibqp);
+	struct qedr_dev *dev = qp->dev;
+	struct ib_qp_attr attr;
+	int attr_mask = 0;
+	int rc = 0;
+
+	DP_DEBUG(dev, QEDR_MSG_QP, "destroy qp: destroying %p, qp type=%d\n",
+		 qp, qp->qp_type);
+
+	if (qp->state != (QED_ROCE_QP_STATE_RESET | QED_ROCE_QP_STATE_ERR |
+			  QED_ROCE_QP_STATE_INIT)) {
+		attr.qp_state = IB_QPS_ERR;
+		attr_mask |= IB_QP_STATE;
+
+		/* Change the QP state to ERROR */
+		qedr_modify_qp(ibqp, &attr, attr_mask, NULL);
+	}
+
+	if (qp->qp_type != IB_QPT_GSI) {
+		rc = dev->ops->rdma_destroy_qp(dev->rdma_ctx, qp->qed_qp);
+		if (rc)
+			return rc;
+	} else {
+		qedr_destroy_gsi_qp(dev);
+	}
+
+	if (ibqp->uobject && ibqp->uobject->context) {
+		qedr_cleanup_user_sq(dev, qp);
+		qedr_cleanup_user_rq(dev, qp);
+	} else {
+		qedr_cleanup_kernel_sq(dev, qp);
+		qedr_cleanup_kernel_rq(dev, qp);
+	}
+
+	kfree(qp);
+
+	return rc;
+}
+
+struct ib_ah *qedr_create_ah(struct ib_pd *ibpd, struct ib_ah_attr *attr)
+{
+	struct qedr_ah *ah;
+
+	ah = kzalloc(sizeof(*ah), GFP_ATOMIC);
+	if (!ah)
+		return ERR_PTR(-ENOMEM);
+
+	ah->attr = *attr;
+
+	return &ah->ibah;
+}
+
+int qedr_destroy_ah(struct ib_ah *ibah)
+{
+	struct qedr_ah *ah = get_qedr_ah(ibah);
+
+	kfree(ah);
+	return 0;
+}
+
+static void free_mr_info(struct qedr_dev *dev, struct mr_info *info)
+{
+	struct qedr_pbl *pbl, *tmp;
+
+	if (info->pbl_table)
+		list_add_tail(&info->pbl_table->list_entry,
+			      &info->free_pbl_list);
+
+	if (!list_empty(&info->inuse_pbl_list))
+		list_splice(&info->inuse_pbl_list, &info->free_pbl_list);
+
+	list_for_each_entry_safe(pbl, tmp, &info->free_pbl_list, list_entry) {
+		list_del(&pbl->list_entry);
+		qedr_free_pbl(dev, &info->pbl_info, pbl);
+	}
+}
+
+static int init_mr_info(struct qedr_dev *dev, struct mr_info *info,
+			size_t page_list_len, bool two_layered)
+{
+	struct qedr_pbl *tmp;
+	int rc;
+
+	INIT_LIST_HEAD(&info->free_pbl_list);
+	INIT_LIST_HEAD(&info->inuse_pbl_list);
+
+	rc = qedr_prepare_pbl_tbl(dev, &info->pbl_info,
+				  page_list_len, two_layered);
+	if (rc)
+		goto done;
+
+	info->pbl_table = qedr_alloc_pbl_tbl(dev, &info->pbl_info, GFP_KERNEL);
+	if (!info->pbl_table) {
+		rc = -ENOMEM;
+		goto done;
+	}
+
+	DP_DEBUG(dev, QEDR_MSG_MR, "pbl_table_pa = %pa\n",
+		 &info->pbl_table->pa);
+
+	/* in usual case we use 2 PBLs, so we add one to free
+	 * list and allocating another one
+	 */
+	tmp = qedr_alloc_pbl_tbl(dev, &info->pbl_info, GFP_KERNEL);
+	if (!tmp) {
+		DP_DEBUG(dev, QEDR_MSG_MR, "Extra PBL is not allocated\n");
+		goto done;
+	}
+
+	list_add_tail(&tmp->list_entry, &info->free_pbl_list);
+
+	DP_DEBUG(dev, QEDR_MSG_MR, "extra pbl_table_pa = %pa\n", &tmp->pa);
+
+done:
+	if (rc)
+		free_mr_info(dev, info);
+
+	return rc;
+}
+
+struct ib_mr *qedr_reg_user_mr(struct ib_pd *ibpd, u64 start, u64 len,
+			       u64 usr_addr, int acc, struct ib_udata *udata)
+{
+	struct qedr_dev *dev = get_qedr_dev(ibpd->device);
+	struct qedr_mr *mr;
+	struct qedr_pd *pd;
+	int rc = -ENOMEM;
+
+	pd = get_qedr_pd(ibpd);
+	DP_DEBUG(dev, QEDR_MSG_MR,
+		 "qedr_register user mr pd = %d start = %lld, len = %lld, usr_addr = %lld, acc = %d\n",
+		 pd->pd_id, start, len, usr_addr, acc);
+
+	if (acc & IB_ACCESS_REMOTE_WRITE && !(acc & IB_ACCESS_LOCAL_WRITE))
+		return ERR_PTR(-EINVAL);
+
+	mr = kzalloc(sizeof(*mr), GFP_KERNEL);
+	if (!mr)
+		return ERR_PTR(rc);
+
+	mr->type = QEDR_MR_USER;
+
+	mr->umem = ib_umem_get(ibpd->uobject->context, start, len, acc, 0);
+	if (IS_ERR(mr->umem)) {
+		rc = -EFAULT;
+		goto err0;
+	}
+
+	rc = init_mr_info(dev, &mr->info, ib_umem_page_count(mr->umem), 1);
+	if (rc)
+		goto err1;
+
+	qedr_populate_pbls(dev, mr->umem, mr->info.pbl_table,
+			   &mr->info.pbl_info);
+
+	rc = dev->ops->rdma_alloc_tid(dev->rdma_ctx, &mr->hw_mr.itid);
+	if (rc) {
+		DP_ERR(dev, "roce alloc tid returned an error %d\n", rc);
+		goto err1;
+	}
+
+	/* Index only, 18 bit long, lkey = itid << 8 | key */
+	mr->hw_mr.tid_type = QED_RDMA_TID_REGISTERED_MR;
+	mr->hw_mr.key = 0;
+	mr->hw_mr.pd = pd->pd_id;
+	mr->hw_mr.local_read = 1;
+	mr->hw_mr.local_write = (acc & IB_ACCESS_LOCAL_WRITE) ? 1 : 0;
+	mr->hw_mr.remote_read = (acc & IB_ACCESS_REMOTE_READ) ? 1 : 0;
+	mr->hw_mr.remote_write = (acc & IB_ACCESS_REMOTE_WRITE) ? 1 : 0;
+	mr->hw_mr.remote_atomic = (acc & IB_ACCESS_REMOTE_ATOMIC) ? 1 : 0;
+	mr->hw_mr.mw_bind = false;
+	mr->hw_mr.pbl_ptr = mr->info.pbl_table[0].pa;
+	mr->hw_mr.pbl_two_level = mr->info.pbl_info.two_layered;
+	mr->hw_mr.pbl_page_size_log = ilog2(mr->info.pbl_info.pbl_size);
+	mr->hw_mr.page_size_log = ilog2(mr->umem->page_size);
+	mr->hw_mr.fbo = ib_umem_offset(mr->umem);
+	mr->hw_mr.length = len;
+	mr->hw_mr.vaddr = usr_addr;
+	mr->hw_mr.zbva = false;
+	mr->hw_mr.phy_mr = false;
+	mr->hw_mr.dma_mr = false;
+
+	rc = dev->ops->rdma_register_tid(dev->rdma_ctx, &mr->hw_mr);
+	if (rc) {
+		DP_ERR(dev, "roce register tid returned an error %d\n", rc);
+		goto err2;
+	}
+
+	mr->ibmr.lkey = mr->hw_mr.itid << 8 | mr->hw_mr.key;
+	if (mr->hw_mr.remote_write || mr->hw_mr.remote_read ||
+	    mr->hw_mr.remote_atomic)
+		mr->ibmr.rkey = mr->hw_mr.itid << 8 | mr->hw_mr.key;
+
+	DP_DEBUG(dev, QEDR_MSG_MR, "register user mr lkey: %x\n",
+		 mr->ibmr.lkey);
+	return &mr->ibmr;
+
+err2:
+	dev->ops->rdma_free_tid(dev->rdma_ctx, mr->hw_mr.itid);
+err1:
+	qedr_free_pbl(dev, &mr->info.pbl_info, mr->info.pbl_table);
+err0:
+	kfree(mr);
+	return ERR_PTR(rc);
+}
+
+int qedr_dereg_mr(struct ib_mr *ib_mr)
+{
+	struct qedr_mr *mr = get_qedr_mr(ib_mr);
+	struct qedr_dev *dev = get_qedr_dev(ib_mr->device);
+	int rc = 0;
+
+	rc = dev->ops->rdma_deregister_tid(dev->rdma_ctx, mr->hw_mr.itid);
+	if (rc)
+		return rc;
+
+	dev->ops->rdma_free_tid(dev->rdma_ctx, mr->hw_mr.itid);
+
+	if ((mr->type != QEDR_MR_DMA) && (mr->type != QEDR_MR_FRMR))
+		qedr_free_pbl(dev, &mr->info.pbl_info, mr->info.pbl_table);
+
+	/* it could be user registered memory. */
+	if (mr->umem)
+		ib_umem_release(mr->umem);
+
+	kfree(mr);
+
+	return rc;
+}
+
+struct qedr_mr *__qedr_alloc_mr(struct ib_pd *ibpd, int max_page_list_len)
+{
+	struct qedr_pd *pd = get_qedr_pd(ibpd);
+	struct qedr_dev *dev = get_qedr_dev(ibpd->device);
+	struct qedr_mr *mr;
+	int rc = -ENOMEM;
+
+	DP_DEBUG(dev, QEDR_MSG_MR,
+		 "qedr_alloc_frmr pd = %d max_page_list_len= %d\n", pd->pd_id,
+		 max_page_list_len);
+
+	mr = kzalloc(sizeof(*mr), GFP_KERNEL);
+	if (!mr)
+		return ERR_PTR(rc);
+
+	mr->dev = dev;
+	mr->type = QEDR_MR_FRMR;
+
+	rc = init_mr_info(dev, &mr->info, max_page_list_len, 1);
+	if (rc)
+		goto err0;
+
+	rc = dev->ops->rdma_alloc_tid(dev->rdma_ctx, &mr->hw_mr.itid);
+	if (rc) {
+		DP_ERR(dev, "roce alloc tid returned an error %d\n", rc);
+		goto err0;
+	}
+
+	/* Index only, 18 bit long, lkey = itid << 8 | key */
+	mr->hw_mr.tid_type = QED_RDMA_TID_FMR;
+	mr->hw_mr.key = 0;
+	mr->hw_mr.pd = pd->pd_id;
+	mr->hw_mr.local_read = 1;
+	mr->hw_mr.local_write = 0;
+	mr->hw_mr.remote_read = 0;
+	mr->hw_mr.remote_write = 0;
+	mr->hw_mr.remote_atomic = 0;
+	mr->hw_mr.mw_bind = false;
+	mr->hw_mr.pbl_ptr = 0;
+	mr->hw_mr.pbl_two_level = mr->info.pbl_info.two_layered;
+	mr->hw_mr.pbl_page_size_log = ilog2(mr->info.pbl_info.pbl_size);
+	mr->hw_mr.fbo = 0;
+	mr->hw_mr.length = 0;
+	mr->hw_mr.vaddr = 0;
+	mr->hw_mr.zbva = false;
+	mr->hw_mr.phy_mr = true;
+	mr->hw_mr.dma_mr = false;
+
+	rc = dev->ops->rdma_register_tid(dev->rdma_ctx, &mr->hw_mr);
+	if (rc) {
+		DP_ERR(dev, "roce register tid returned an error %d\n", rc);
+		goto err1;
+	}
+
+	mr->ibmr.lkey = mr->hw_mr.itid << 8 | mr->hw_mr.key;
+	mr->ibmr.rkey = mr->ibmr.lkey;
+
+	DP_DEBUG(dev, QEDR_MSG_MR, "alloc frmr: %x\n", mr->ibmr.lkey);
+	return mr;
+
+err1:
+	dev->ops->rdma_free_tid(dev->rdma_ctx, mr->hw_mr.itid);
+err0:
+	kfree(mr);
+	return ERR_PTR(rc);
+}
+
+struct ib_mr *qedr_alloc_mr(struct ib_pd *ibpd,
+			    enum ib_mr_type mr_type, u32 max_num_sg)
+{
+	struct qedr_dev *dev;
+	struct qedr_mr *mr;
+
+	if (mr_type != IB_MR_TYPE_MEM_REG)
+		return ERR_PTR(-EINVAL);
+
+	mr = __qedr_alloc_mr(ibpd, max_num_sg);
+
+	if (IS_ERR(mr))
+		return ERR_PTR(-EINVAL);
+
+	dev = mr->dev;
+
+	return &mr->ibmr;
+}
+
+static int qedr_set_page(struct ib_mr *ibmr, u64 addr)
+{
+	struct qedr_mr *mr = get_qedr_mr(ibmr);
+	struct qedr_pbl *pbl_table;
+	struct regpair *pbe;
+	u32 pbes_in_page;
+
+	if (unlikely(mr->npages == mr->info.pbl_info.num_pbes)) {
+		DP_ERR(mr->dev, "qedr_set_page failes when %d\n", mr->npages);
+		return -ENOMEM;
+	}
+
+	DP_DEBUG(mr->dev, QEDR_MSG_MR, "qedr_set_page pages[%d] = 0x%llx\n",
+		 mr->npages, addr);
+
+	pbes_in_page = mr->info.pbl_info.pbl_size / sizeof(u64);
+	pbl_table = mr->info.pbl_table + (mr->npages / pbes_in_page);
+	pbe = (struct regpair *)pbl_table->va;
+	pbe +=  mr->npages % pbes_in_page;
+	pbe->lo = cpu_to_le32((u32)addr);
+	pbe->hi = cpu_to_le32((u32)upper_32_bits(addr));
+
+	mr->npages++;
+
+	return 0;
+}
+
+static void handle_completed_mrs(struct qedr_dev *dev, struct mr_info *info)
+{
+	int work = info->completed - info->completed_handled - 1;
+
+	DP_DEBUG(dev, QEDR_MSG_MR, "Special FMR work = %d\n", work);
+	while (work-- > 0 && !list_empty(&info->inuse_pbl_list)) {
+		struct qedr_pbl *pbl;
+
+		/* Free all the page list that are possible to be freed
+		 * (all the ones that were invalidated), under the assumption
+		 * that if an FMR was completed successfully that means that
+		 * if there was an invalidate operation before it also ended
+		 */
+		pbl = list_first_entry(&info->inuse_pbl_list,
+				       struct qedr_pbl, list_entry);
+		list_del(&pbl->list_entry);
+		list_add_tail(&pbl->list_entry, &info->free_pbl_list);
+		info->completed_handled++;
+	}
+}
+
+int qedr_map_mr_sg(struct ib_mr *ibmr, struct scatterlist *sg,
+		   int sg_nents, unsigned int *sg_offset)
+{
+	struct qedr_mr *mr = get_qedr_mr(ibmr);
+
+	mr->npages = 0;
+
+	handle_completed_mrs(mr->dev, &mr->info);
+	return ib_sg_to_pages(ibmr, sg, sg_nents, NULL, qedr_set_page);
+}
+
+struct ib_mr *qedr_get_dma_mr(struct ib_pd *ibpd, int acc)
+{
+	struct qedr_dev *dev = get_qedr_dev(ibpd->device);
+	struct qedr_pd *pd = get_qedr_pd(ibpd);
+	struct qedr_mr *mr;
+	int rc;
+
+	mr = kzalloc(sizeof(*mr), GFP_KERNEL);
+	if (!mr)
+		return ERR_PTR(-ENOMEM);
+
+	mr->type = QEDR_MR_DMA;
+
+	rc = dev->ops->rdma_alloc_tid(dev->rdma_ctx, &mr->hw_mr.itid);
+	if (rc) {
+		DP_ERR(dev, "roce alloc tid returned an error %d\n", rc);
+		goto err1;
+	}
+
+	/* index only, 18 bit long, lkey = itid << 8 | key */
+	mr->hw_mr.tid_type = QED_RDMA_TID_REGISTERED_MR;
+	mr->hw_mr.pd = pd->pd_id;
+	mr->hw_mr.local_read = 1;
+	mr->hw_mr.local_write = (acc & IB_ACCESS_LOCAL_WRITE) ? 1 : 0;
+	mr->hw_mr.remote_read = (acc & IB_ACCESS_REMOTE_READ) ? 1 : 0;
+	mr->hw_mr.remote_write = (acc & IB_ACCESS_REMOTE_WRITE) ? 1 : 0;
+	mr->hw_mr.remote_atomic = (acc & IB_ACCESS_REMOTE_ATOMIC) ? 1 : 0;
+	mr->hw_mr.dma_mr = true;
+
+	rc = dev->ops->rdma_register_tid(dev->rdma_ctx, &mr->hw_mr);
+	if (rc) {
+		DP_ERR(dev, "roce register tid returned an error %d\n", rc);
+		goto err2;
+	}
+
+	mr->ibmr.lkey = mr->hw_mr.itid << 8 | mr->hw_mr.key;
+	if (mr->hw_mr.remote_write || mr->hw_mr.remote_read ||
+	    mr->hw_mr.remote_atomic)
+		mr->ibmr.rkey = mr->hw_mr.itid << 8 | mr->hw_mr.key;
+
+	DP_DEBUG(dev, QEDR_MSG_MR, "get dma mr: lkey = %x\n", mr->ibmr.lkey);
+	return &mr->ibmr;
+
+err2:
+	dev->ops->rdma_free_tid(dev->rdma_ctx, mr->hw_mr.itid);
+err1:
+	kfree(mr);
+	return ERR_PTR(rc);
+}
+
+static inline int qedr_wq_is_full(struct qedr_qp_hwq_info *wq)
+{
+	return (((wq->prod + 1) % wq->max_wr) == wq->cons);
+}
+
+static int sge_data_len(struct ib_sge *sg_list, int num_sge)
+{
+	int i, len = 0;
+
+	for (i = 0; i < num_sge; i++)
+		len += sg_list[i].length;
+
+	return len;
+}
+
+static void swap_wqe_data64(u64 *p)
+{
+	int i;
+
+	for (i = 0; i < QEDR_SQE_ELEMENT_SIZE / sizeof(u64); i++, p++)
+		*p = cpu_to_be64(cpu_to_le64(*p));
+}
+
+static u32 qedr_prepare_sq_inline_data(struct qedr_dev *dev,
+				       struct qedr_qp *qp, u8 *wqe_size,
+				       struct ib_send_wr *wr,
+				       struct ib_send_wr **bad_wr, u8 *bits,
+				       u8 bit)
+{
+	u32 data_size = sge_data_len(wr->sg_list, wr->num_sge);
+	char *seg_prt, *wqe;
+	int i, seg_siz;
+
+	if (data_size > ROCE_REQ_MAX_INLINE_DATA_SIZE) {
+		DP_ERR(dev, "Too much inline data in WR: %d\n", data_size);
+		*bad_wr = wr;
+		return 0;
+	}
+
+	if (!data_size)
+		return data_size;
+
+	*bits |= bit;
+
+	seg_prt = NULL;
+	wqe = NULL;
+	seg_siz = 0;
+
+	/* Copy data inline */
+	for (i = 0; i < wr->num_sge; i++) {
+		u32 len = wr->sg_list[i].length;
+		void *src = (void *)(uintptr_t)wr->sg_list[i].addr;
+
+		while (len > 0) {
+			u32 cur;
+
+			/* New segment required */
+			if (!seg_siz) {
+				wqe = (char *)qed_chain_produce(&qp->sq.pbl);
+				seg_prt = wqe;
+				seg_siz = sizeof(struct rdma_sq_common_wqe);
+				(*wqe_size)++;
+			}
+
+			/* Calculate currently allowed length */
+			cur = min_t(u32, len, seg_siz);
+			memcpy(seg_prt, src, cur);
+
+			/* Update segment variables */
+			seg_prt += cur;
+			seg_siz -= cur;
+
+			/* Update sge variables */
+			src += cur;
+			len -= cur;
+
+			/* Swap fully-completed segments */
+			if (!seg_siz)
+				swap_wqe_data64((u64 *)wqe);
+		}
+	}
+
+	/* swap last not completed segment */
+	if (seg_siz)
+		swap_wqe_data64((u64 *)wqe);
+
+	return data_size;
+}
+
+#define RQ_SGE_SET(sge, vaddr, vlength, vflags)			\
+	do {							\
+		DMA_REGPAIR_LE(sge->addr, vaddr);		\
+		(sge)->length = cpu_to_le32(vlength);		\
+		(sge)->flags = cpu_to_le32(vflags);		\
+	} while (0)
+
+#define SRQ_HDR_SET(hdr, vwr_id, num_sge)			\
+	do {							\
+		DMA_REGPAIR_LE(hdr->wr_id, vwr_id);		\
+		(hdr)->num_sges = num_sge;			\
+	} while (0)
+
+#define SRQ_SGE_SET(sge, vaddr, vlength, vlkey)			\
+	do {							\
+		DMA_REGPAIR_LE(sge->addr, vaddr);		\
+		(sge)->length = cpu_to_le32(vlength);		\
+		(sge)->l_key = cpu_to_le32(vlkey);		\
+	} while (0)
+
+static u32 qedr_prepare_sq_sges(struct qedr_qp *qp, u8 *wqe_size,
+				struct ib_send_wr *wr)
+{
+	u32 data_size = 0;
+	int i;
+
+	for (i = 0; i < wr->num_sge; i++) {
+		struct rdma_sq_sge *sge = qed_chain_produce(&qp->sq.pbl);
+
+		DMA_REGPAIR_LE(sge->addr, wr->sg_list[i].addr);
+		sge->l_key = cpu_to_le32(wr->sg_list[i].lkey);
+		sge->length = cpu_to_le32(wr->sg_list[i].length);
+		data_size += wr->sg_list[i].length;
+	}
+
+	if (wqe_size)
+		*wqe_size += wr->num_sge;
+
+	return data_size;
+}
+
+static u32 qedr_prepare_sq_rdma_data(struct qedr_dev *dev,
+				     struct qedr_qp *qp,
+				     struct rdma_sq_rdma_wqe_1st *rwqe,
+				     struct rdma_sq_rdma_wqe_2nd *rwqe2,
+				     struct ib_send_wr *wr,
+				     struct ib_send_wr **bad_wr)
+{
+	rwqe2->r_key = cpu_to_le32(rdma_wr(wr)->rkey);
+	DMA_REGPAIR_LE(rwqe2->remote_va, rdma_wr(wr)->remote_addr);
+
+	if (wr->send_flags & IB_SEND_INLINE) {
+		u8 flags = 0;
+
+		SET_FIELD2(flags, RDMA_SQ_RDMA_WQE_1ST_INLINE_FLG, 1);
+		return qedr_prepare_sq_inline_data(dev, qp, &rwqe->wqe_size, wr,
+						   bad_wr, &rwqe->flags, flags);
+	}
+
+	return qedr_prepare_sq_sges(qp, &rwqe->wqe_size, wr);
+}
+
+static u32 qedr_prepare_sq_send_data(struct qedr_dev *dev,
+				     struct qedr_qp *qp,
+				     struct rdma_sq_send_wqe_1st *swqe,
+				     struct rdma_sq_send_wqe_2st *swqe2,
+				     struct ib_send_wr *wr,
+				     struct ib_send_wr **bad_wr)
+{
+	memset(swqe2, 0, sizeof(*swqe2));
+	if (wr->send_flags & IB_SEND_INLINE) {
+		u8 flags = 0;
+
+		SET_FIELD2(flags, RDMA_SQ_SEND_WQE_INLINE_FLG, 1);
+		return qedr_prepare_sq_inline_data(dev, qp, &swqe->wqe_size, wr,
+						   bad_wr, &swqe->flags, flags);
+	}
+
+	return qedr_prepare_sq_sges(qp, &swqe->wqe_size, wr);
+}
+
+static int qedr_prepare_reg(struct qedr_qp *qp,
+			    struct rdma_sq_fmr_wqe_1st *fwqe1,
+			    struct ib_reg_wr *wr)
+{
+	struct qedr_mr *mr = get_qedr_mr(wr->mr);
+	struct rdma_sq_fmr_wqe_2nd *fwqe2;
+
+	fwqe2 = (struct rdma_sq_fmr_wqe_2nd *)qed_chain_produce(&qp->sq.pbl);
+	fwqe1->addr.hi = upper_32_bits(mr->ibmr.iova);
+	fwqe1->addr.lo = lower_32_bits(mr->ibmr.iova);
+	fwqe1->l_key = wr->key;
+
+	SET_FIELD2(fwqe2->access_ctrl, RDMA_SQ_FMR_WQE_2ND_REMOTE_READ,
+		   !!(wr->access & IB_ACCESS_REMOTE_READ));
+	SET_FIELD2(fwqe2->access_ctrl, RDMA_SQ_FMR_WQE_2ND_REMOTE_WRITE,
+		   !!(wr->access & IB_ACCESS_REMOTE_WRITE));
+	SET_FIELD2(fwqe2->access_ctrl, RDMA_SQ_FMR_WQE_2ND_ENABLE_ATOMIC,
+		   !!(wr->access & IB_ACCESS_REMOTE_ATOMIC));
+	SET_FIELD2(fwqe2->access_ctrl, RDMA_SQ_FMR_WQE_2ND_LOCAL_READ, 1);
+	SET_FIELD2(fwqe2->access_ctrl, RDMA_SQ_FMR_WQE_2ND_LOCAL_WRITE,
+		   !!(wr->access & IB_ACCESS_LOCAL_WRITE));
+	fwqe2->fmr_ctrl = 0;
+
+	SET_FIELD2(fwqe2->fmr_ctrl, RDMA_SQ_FMR_WQE_2ND_PAGE_SIZE_LOG,
+		   ilog2(mr->ibmr.page_size) - 12);
+
+	fwqe2->length_hi = 0;
+	fwqe2->length_lo = mr->ibmr.length;
+	fwqe2->pbl_addr.hi = upper_32_bits(mr->info.pbl_table->pa);
+	fwqe2->pbl_addr.lo = lower_32_bits(mr->info.pbl_table->pa);
+
+	qp->wqe_wr_id[qp->sq.prod].mr = mr;
+
+	return 0;
+}
+
+enum ib_wc_opcode qedr_ib_to_wc_opcode(enum ib_wr_opcode opcode)
+{
+	switch (opcode) {
+	case IB_WR_RDMA_WRITE:
+	case IB_WR_RDMA_WRITE_WITH_IMM:
+		return IB_WC_RDMA_WRITE;
+	case IB_WR_SEND_WITH_IMM:
+	case IB_WR_SEND:
+	case IB_WR_SEND_WITH_INV:
+		return IB_WC_SEND;
+	case IB_WR_RDMA_READ:
+		return IB_WC_RDMA_READ;
+	case IB_WR_ATOMIC_CMP_AND_SWP:
+		return IB_WC_COMP_SWAP;
+	case IB_WR_ATOMIC_FETCH_AND_ADD:
+		return IB_WC_FETCH_ADD;
+	case IB_WR_REG_MR:
+		return IB_WC_REG_MR;
+	case IB_WR_LOCAL_INV:
+		return IB_WC_LOCAL_INV;
+	default:
+		return IB_WC_SEND;
+	}
+}
+
+inline bool qedr_can_post_send(struct qedr_qp *qp, struct ib_send_wr *wr)
+{
+	int wq_is_full, err_wr, pbl_is_full;
+	struct qedr_dev *dev = qp->dev;
+
+	/* prevent SQ overflow and/or processing of a bad WR */
+	err_wr = wr->num_sge > qp->sq.max_sges;
+	wq_is_full = qedr_wq_is_full(&qp->sq);
+	pbl_is_full = qed_chain_get_elem_left_u32(&qp->sq.pbl) <
+		      QEDR_MAX_SQE_ELEMENTS_PER_SQE;
+	if (wq_is_full || err_wr || pbl_is_full) {
+		if (wq_is_full && !(qp->err_bitmap & QEDR_QP_ERR_SQ_FULL)) {
+			DP_ERR(dev,
+			       "error: WQ is full. Post send on QP %p failed (this error appears only once)\n",
+			       qp);
+			qp->err_bitmap |= QEDR_QP_ERR_SQ_FULL;
+		}
+
+		if (err_wr && !(qp->err_bitmap & QEDR_QP_ERR_BAD_SR)) {
+			DP_ERR(dev,
+			       "error: WR is bad. Post send on QP %p failed (this error appears only once)\n",
+			       qp);
+			qp->err_bitmap |= QEDR_QP_ERR_BAD_SR;
+		}
+
+		if (pbl_is_full &&
+		    !(qp->err_bitmap & QEDR_QP_ERR_SQ_PBL_FULL)) {
+			DP_ERR(dev,
+			       "error: WQ PBL is full. Post send on QP %p failed (this error appears only once)\n",
+			       qp);
+			qp->err_bitmap |= QEDR_QP_ERR_SQ_PBL_FULL;
+		}
+		return false;
+	}
+	return true;
+}
+
+int __qedr_post_send(struct ib_qp *ibqp, struct ib_send_wr *wr,
+		     struct ib_send_wr **bad_wr)
+{
+	struct qedr_dev *dev = get_qedr_dev(ibqp->device);
+	struct qedr_qp *qp = get_qedr_qp(ibqp);
+	struct rdma_sq_atomic_wqe_1st *awqe1;
+	struct rdma_sq_atomic_wqe_2nd *awqe2;
+	struct rdma_sq_atomic_wqe_3rd *awqe3;
+	struct rdma_sq_send_wqe_2st *swqe2;
+	struct rdma_sq_local_inv_wqe *iwqe;
+	struct rdma_sq_rdma_wqe_2nd *rwqe2;
+	struct rdma_sq_send_wqe_1st *swqe;
+	struct rdma_sq_rdma_wqe_1st *rwqe;
+	struct rdma_sq_fmr_wqe_1st *fwqe1;
+	struct rdma_sq_common_wqe *wqe;
+	u32 length;
+	int rc = 0;
+	bool comp;
+
+	if (!qedr_can_post_send(qp, wr)) {
+		*bad_wr = wr;
+		return -ENOMEM;
+	}
+
+	wqe = qed_chain_produce(&qp->sq.pbl);
+	qp->wqe_wr_id[qp->sq.prod].signaled =
+		!!(wr->send_flags & IB_SEND_SIGNALED) || qp->signaled;
+
+	wqe->flags = 0;
+	SET_FIELD2(wqe->flags, RDMA_SQ_SEND_WQE_SE_FLG,
+		   !!(wr->send_flags & IB_SEND_SOLICITED));
+	comp = (!!(wr->send_flags & IB_SEND_SIGNALED)) || qp->signaled;
+	SET_FIELD2(wqe->flags, RDMA_SQ_SEND_WQE_COMP_FLG, comp);
+	SET_FIELD2(wqe->flags, RDMA_SQ_SEND_WQE_RD_FENCE_FLG,
+		   !!(wr->send_flags & IB_SEND_FENCE));
+	wqe->prev_wqe_size = qp->prev_wqe_size;
+
+	qp->wqe_wr_id[qp->sq.prod].opcode = qedr_ib_to_wc_opcode(wr->opcode);
+
+	switch (wr->opcode) {
+	case IB_WR_SEND_WITH_IMM:
+		wqe->req_type = RDMA_SQ_REQ_TYPE_SEND_WITH_IMM;
+		swqe = (struct rdma_sq_send_wqe_1st *)wqe;
+		swqe->wqe_size = 2;
+		swqe2 = qed_chain_produce(&qp->sq.pbl);
+
+		swqe->inv_key_or_imm_data = cpu_to_le32(wr->ex.imm_data);
+		length = qedr_prepare_sq_send_data(dev, qp, swqe, swqe2,
+						   wr, bad_wr);
+		swqe->length = cpu_to_le32(length);
+		qp->wqe_wr_id[qp->sq.prod].wqe_size = swqe->wqe_size;
+		qp->prev_wqe_size = swqe->wqe_size;
+		qp->wqe_wr_id[qp->sq.prod].bytes_len = swqe->length;
+		break;
+	case IB_WR_SEND:
+		wqe->req_type = RDMA_SQ_REQ_TYPE_SEND;
+		swqe = (struct rdma_sq_send_wqe_1st *)wqe;
+
+		swqe->wqe_size = 2;
+		swqe2 = qed_chain_produce(&qp->sq.pbl);
+		length = qedr_prepare_sq_send_data(dev, qp, swqe, swqe2,
+						   wr, bad_wr);
+		swqe->length = cpu_to_le32(length);
+		qp->wqe_wr_id[qp->sq.prod].wqe_size = swqe->wqe_size;
+		qp->prev_wqe_size = swqe->wqe_size;
+		qp->wqe_wr_id[qp->sq.prod].bytes_len = swqe->length;
+		break;
+	case IB_WR_SEND_WITH_INV:
+		wqe->req_type = RDMA_SQ_REQ_TYPE_SEND_WITH_INVALIDATE;
+		swqe = (struct rdma_sq_send_wqe_1st *)wqe;
+		swqe2 = qed_chain_produce(&qp->sq.pbl);
+		swqe->wqe_size = 2;
+		swqe->inv_key_or_imm_data = cpu_to_le32(wr->ex.invalidate_rkey);
+		length = qedr_prepare_sq_send_data(dev, qp, swqe, swqe2,
+						   wr, bad_wr);
+		swqe->length = cpu_to_le32(length);
+		qp->wqe_wr_id[qp->sq.prod].wqe_size = swqe->wqe_size;
+		qp->prev_wqe_size = swqe->wqe_size;
+		qp->wqe_wr_id[qp->sq.prod].bytes_len = swqe->length;
+		break;
+
+	case IB_WR_RDMA_WRITE_WITH_IMM:
+		wqe->req_type = RDMA_SQ_REQ_TYPE_RDMA_WR_WITH_IMM;
+		rwqe = (struct rdma_sq_rdma_wqe_1st *)wqe;
+
+		rwqe->wqe_size = 2;
+		rwqe->imm_data = htonl(cpu_to_le32(wr->ex.imm_data));
+		rwqe2 = qed_chain_produce(&qp->sq.pbl);
+		length = qedr_prepare_sq_rdma_data(dev, qp, rwqe, rwqe2,
+						   wr, bad_wr);
+		rwqe->length = cpu_to_le32(length);
+		qp->wqe_wr_id[qp->sq.prod].wqe_size = rwqe->wqe_size;
+		qp->prev_wqe_size = rwqe->wqe_size;
+		qp->wqe_wr_id[qp->sq.prod].bytes_len = rwqe->length;
+		break;
+	case IB_WR_RDMA_WRITE:
+		wqe->req_type = RDMA_SQ_REQ_TYPE_RDMA_WR;
+		rwqe = (struct rdma_sq_rdma_wqe_1st *)wqe;
+
+		rwqe->wqe_size = 2;
+		rwqe2 = qed_chain_produce(&qp->sq.pbl);
+		length = qedr_prepare_sq_rdma_data(dev, qp, rwqe, rwqe2,
+						   wr, bad_wr);
+		rwqe->length = cpu_to_le32(length);
+		qp->wqe_wr_id[qp->sq.prod].wqe_size = rwqe->wqe_size;
+		qp->prev_wqe_size = rwqe->wqe_size;
+		qp->wqe_wr_id[qp->sq.prod].bytes_len = rwqe->length;
+		break;
+	case IB_WR_RDMA_READ_WITH_INV:
+		DP_ERR(dev,
+		       "RDMA READ WITH INVALIDATE not supported\n");
+		*bad_wr = wr;
+		rc = -EINVAL;
+		break;
+
+	case IB_WR_RDMA_READ:
+		wqe->req_type = RDMA_SQ_REQ_TYPE_RDMA_RD;
+		rwqe = (struct rdma_sq_rdma_wqe_1st *)wqe;
+
+		rwqe->wqe_size = 2;
+		rwqe2 = qed_chain_produce(&qp->sq.pbl);
+		length = qedr_prepare_sq_rdma_data(dev, qp, rwqe, rwqe2,
+						   wr, bad_wr);
+		rwqe->length = cpu_to_le32(length);
+		qp->wqe_wr_id[qp->sq.prod].wqe_size = rwqe->wqe_size;
+		qp->prev_wqe_size = rwqe->wqe_size;
+		qp->wqe_wr_id[qp->sq.prod].bytes_len = rwqe->length;
+		break;
+
+	case IB_WR_ATOMIC_CMP_AND_SWP:
+	case IB_WR_ATOMIC_FETCH_AND_ADD:
+		awqe1 = (struct rdma_sq_atomic_wqe_1st *)wqe;
+		awqe1->wqe_size = 4;
+
+		awqe2 = qed_chain_produce(&qp->sq.pbl);
+		DMA_REGPAIR_LE(awqe2->remote_va, atomic_wr(wr)->remote_addr);
+		awqe2->r_key = cpu_to_le32(atomic_wr(wr)->rkey);
+
+		awqe3 = qed_chain_produce(&qp->sq.pbl);
+
+		if (wr->opcode == IB_WR_ATOMIC_FETCH_AND_ADD) {
+			wqe->req_type = RDMA_SQ_REQ_TYPE_ATOMIC_ADD;
+			DMA_REGPAIR_LE(awqe3->swap_data,
+				       atomic_wr(wr)->compare_add);
+		} else {
+			wqe->req_type = RDMA_SQ_REQ_TYPE_ATOMIC_CMP_AND_SWAP;
+			DMA_REGPAIR_LE(awqe3->swap_data,
+				       atomic_wr(wr)->swap);
+			DMA_REGPAIR_LE(awqe3->cmp_data,
+				       atomic_wr(wr)->compare_add);
+		}
+
+		qedr_prepare_sq_sges(qp, NULL, wr);
+
+		qp->wqe_wr_id[qp->sq.prod].wqe_size = awqe1->wqe_size;
+		qp->prev_wqe_size = awqe1->wqe_size;
+		break;
+
+	case IB_WR_LOCAL_INV:
+		iwqe = (struct rdma_sq_local_inv_wqe *)wqe;
+		iwqe->wqe_size = 1;
+
+		iwqe->req_type = RDMA_SQ_REQ_TYPE_LOCAL_INVALIDATE;
+		iwqe->inv_l_key = wr->ex.invalidate_rkey;
+		qp->wqe_wr_id[qp->sq.prod].wqe_size = iwqe->wqe_size;
+		qp->prev_wqe_size = iwqe->wqe_size;
+		break;
+	case IB_WR_REG_MR:
+		DP_DEBUG(dev, QEDR_MSG_CQ, "REG_MR\n");
+		wqe->req_type = RDMA_SQ_REQ_TYPE_FAST_MR;
+		fwqe1 = (struct rdma_sq_fmr_wqe_1st *)wqe;
+		fwqe1->wqe_size = 2;
+
+		rc = qedr_prepare_reg(qp, fwqe1, reg_wr(wr));
+		if (rc) {
+			DP_ERR(dev, "IB_REG_MR failed rc=%d\n", rc);
+			*bad_wr = wr;
+			break;
+		}
+
+		qp->wqe_wr_id[qp->sq.prod].wqe_size = fwqe1->wqe_size;
+		qp->prev_wqe_size = fwqe1->wqe_size;
+		break;
+	default:
+		DP_ERR(dev, "invalid opcode 0x%x!\n", wr->opcode);
+		rc = -EINVAL;
+		*bad_wr = wr;
+		break;
+	}
+
+	if (*bad_wr) {
+		u16 value;
+
+		/* Restore prod to its position before
+		 * this WR was processed
+		 */
+		value = le16_to_cpu(qp->sq.db_data.data.value);
+		qed_chain_set_prod(&qp->sq.pbl, value, wqe);
+
+		/* Restore prev_wqe_size */
+		qp->prev_wqe_size = wqe->prev_wqe_size;
+		rc = -EINVAL;
+		DP_ERR(dev, "POST SEND FAILED\n");
+	}
+
+	return rc;
+}
+
+int qedr_post_send(struct ib_qp *ibqp, struct ib_send_wr *wr,
+		   struct ib_send_wr **bad_wr)
+{
+	struct qedr_dev *dev = get_qedr_dev(ibqp->device);
+	struct qedr_qp *qp = get_qedr_qp(ibqp);
+	unsigned long flags;
+	int rc = 0;
+
+	*bad_wr = NULL;
+
+	if (qp->qp_type == IB_QPT_GSI)
+		return qedr_gsi_post_send(ibqp, wr, bad_wr);
+
+	spin_lock_irqsave(&qp->q_lock, flags);
+
+	if ((qp->state == QED_ROCE_QP_STATE_RESET) ||
+	    (qp->state == QED_ROCE_QP_STATE_ERR)) {
+		spin_unlock_irqrestore(&qp->q_lock, flags);
+		*bad_wr = wr;
+		DP_DEBUG(dev, QEDR_MSG_CQ,
+			 "QP in wrong state! QP icid=0x%x state %d\n",
+			 qp->icid, qp->state);
+		return -EINVAL;
+	}
+
+	if (!wr) {
+		DP_ERR(dev, "Got an empty post send.\n");
+		return -EINVAL;
+	}
+
+	while (wr) {
+		rc = __qedr_post_send(ibqp, wr, bad_wr);
+		if (rc)
+			break;
+
+		qp->wqe_wr_id[qp->sq.prod].wr_id = wr->wr_id;
+
+		qedr_inc_sw_prod(&qp->sq);
+
+		qp->sq.db_data.data.value++;
+
+		wr = wr->next;
+	}
+
+	/* Trigger doorbell
+	 * If there was a failure in the first WR then it will be triggered in
+	 * vane. However this is not harmful (as long as the producer value is
+	 * unchanged). For performance reasons we avoid checking for this
+	 * redundant doorbell.
+	 */
+	wmb();
+	writel(qp->sq.db_data.raw, qp->sq.db);
+
+	/* Make sure write sticks */
+	mmiowb();
+
+	spin_unlock_irqrestore(&qp->q_lock, flags);
+
+	return rc;
+}
+
+int qedr_post_recv(struct ib_qp *ibqp, struct ib_recv_wr *wr,
+		   struct ib_recv_wr **bad_wr)
+{
+	struct qedr_qp *qp = get_qedr_qp(ibqp);
+	struct qedr_dev *dev = qp->dev;
+	unsigned long flags;
+	int status = 0;
+
+	if (qp->qp_type == IB_QPT_GSI)
+		return qedr_gsi_post_recv(ibqp, wr, bad_wr);
+
+	spin_lock_irqsave(&qp->q_lock, flags);
+
+	if ((qp->state == QED_ROCE_QP_STATE_RESET) ||
+	    (qp->state == QED_ROCE_QP_STATE_ERR)) {
+		spin_unlock_irqrestore(&qp->q_lock, flags);
+		*bad_wr = wr;
+		return -EINVAL;
+	}
+
+	while (wr) {
+		int i;
+
+		if (qed_chain_get_elem_left_u32(&qp->rq.pbl) <
+		    QEDR_MAX_RQE_ELEMENTS_PER_RQE ||
+		    wr->num_sge > qp->rq.max_sges) {
+			DP_ERR(dev, "Can't post WR  (%d < %d) || (%d > %d)\n",
+			       qed_chain_get_elem_left_u32(&qp->rq.pbl),
+			       QEDR_MAX_RQE_ELEMENTS_PER_RQE, wr->num_sge,
+			       qp->rq.max_sges);
+			status = -ENOMEM;
+			*bad_wr = wr;
+			break;
+		}
+		for (i = 0; i < wr->num_sge; i++) {
+			u32 flags = 0;
+			struct rdma_rq_sge *rqe =
+			    qed_chain_produce(&qp->rq.pbl);
+
+			/* First one must include the number
+			 * of SGE in the list
+			 */
+			if (!i)
+				SET_FIELD(flags, RDMA_RQ_SGE_NUM_SGES,
+					  wr->num_sge);
+
+			SET_FIELD(flags, RDMA_RQ_SGE_L_KEY,
+				  wr->sg_list[i].lkey);
+
+			RQ_SGE_SET(rqe, wr->sg_list[i].addr,
+				   wr->sg_list[i].length, flags);
+		}
+
+		/* Special case of no sges. FW requires between 1-4 sges...
+		 * in this case we need to post 1 sge with length zero. this is
+		 * because rdma write with immediate consumes an RQ.
+		 */
+		if (!wr->num_sge) {
+			u32 flags = 0;
+			struct rdma_rq_sge *rqe =
+			    qed_chain_produce(&qp->rq.pbl);
+
+			/* First one must include the number
+			 * of SGE in the list
+			 */
+			SET_FIELD(flags, RDMA_RQ_SGE_L_KEY, 0);
+			SET_FIELD(flags, RDMA_RQ_SGE_NUM_SGES, 1);
+
+			RQ_SGE_SET(rqe, 0, 0, flags);
+			i = 1;
+		}
+
+		qp->rqe_wr_id[qp->rq.prod].wr_id = wr->wr_id;
+		qp->rqe_wr_id[qp->rq.prod].wqe_size = i;
+
+		qedr_inc_sw_prod(&qp->rq);
+
+		/* Flush all the writes before signalling doorbell */
+		wmb();
+
+		qp->rq.db_data.data.value++;
+
+		writel(qp->rq.db_data.raw, qp->rq.db);
+
+		/* Make sure write sticks */
+		mmiowb();
+
+		wr = wr->next;
+	}
+
+	spin_unlock_irqrestore(&qp->q_lock, flags);
+
+	return status;
+}
+
+static int is_valid_cqe(struct qedr_cq *cq, union rdma_cqe *cqe)
+{
+	struct rdma_cqe_requester *resp_cqe = &cqe->req;
+
+	return (resp_cqe->flags & RDMA_CQE_REQUESTER_TOGGLE_BIT_MASK) ==
+		cq->pbl_toggle;
+}
+
+static struct qedr_qp *cqe_get_qp(union rdma_cqe *cqe)
+{
+	struct rdma_cqe_requester *resp_cqe = &cqe->req;
+	struct qedr_qp *qp;
+
+	qp = (struct qedr_qp *)(uintptr_t)HILO_GEN(resp_cqe->qp_handle.hi,
+						   resp_cqe->qp_handle.lo,
+						   u64);
+	return qp;
+}
+
+static enum rdma_cqe_type cqe_get_type(union rdma_cqe *cqe)
+{
+	struct rdma_cqe_requester *resp_cqe = &cqe->req;
+
+	return GET_FIELD(resp_cqe->flags, RDMA_CQE_REQUESTER_TYPE);
+}
+
+/* Return latest CQE (needs processing) */
+static union rdma_cqe *get_cqe(struct qedr_cq *cq)
+{
+	return cq->latest_cqe;
+}
+
+/* In fmr we need to increase the number of fmr completed counter for the fmr
+ * algorithm determining whether we can free a pbl or not.
+ * we need to perform this whether the work request was signaled or not. for
+ * this purpose we call this function from the condition that checks if a wr
+ * should be skipped, to make sure we don't miss it ( possibly this fmr
+ * operation was not signalted)
+ */
+static inline void qedr_chk_if_fmr(struct qedr_qp *qp)
+{
+	if (qp->wqe_wr_id[qp->sq.cons].opcode == IB_WC_REG_MR)
+		qp->wqe_wr_id[qp->sq.cons].mr->info.completed++;
+}
+
+static int process_req(struct qedr_dev *dev, struct qedr_qp *qp,
+		       struct qedr_cq *cq, int num_entries,
+		       struct ib_wc *wc, u16 hw_cons, enum ib_wc_status status,
+		       int force)
+{
+	u16 cnt = 0;
+
+	while (num_entries && qp->sq.wqe_cons != hw_cons) {
+		if (!qp->wqe_wr_id[qp->sq.cons].signaled && !force) {
+			qedr_chk_if_fmr(qp);
+			/* skip WC */
+			goto next_cqe;
+		}
+
+		/* fill WC */
+		wc->status = status;
+		wc->wc_flags = 0;
+		wc->src_qp = qp->id;
+		wc->qp = &qp->ibqp;
+
+		wc->wr_id = qp->wqe_wr_id[qp->sq.cons].wr_id;
+		wc->opcode = qp->wqe_wr_id[qp->sq.cons].opcode;
+
+		switch (wc->opcode) {
+		case IB_WC_RDMA_WRITE:
+			wc->byte_len = qp->wqe_wr_id[qp->sq.cons].bytes_len;
+			break;
+		case IB_WC_COMP_SWAP:
+		case IB_WC_FETCH_ADD:
+			wc->byte_len = 8;
+			break;
+		case IB_WC_REG_MR:
+			qp->wqe_wr_id[qp->sq.cons].mr->info.completed++;
+			break;
+		default:
+			break;
+		}
+
+		num_entries--;
+		wc++;
+		cnt++;
+next_cqe:
+		while (qp->wqe_wr_id[qp->sq.cons].wqe_size--)
+			qed_chain_consume(&qp->sq.pbl);
+		qedr_inc_sw_cons(&qp->sq);
+	}
+
+	return cnt;
+}
+
+static int qedr_poll_cq_req(struct qedr_dev *dev,
+			    struct qedr_qp *qp, struct qedr_cq *cq,
+			    int num_entries, struct ib_wc *wc,
+			    struct rdma_cqe_requester *req)
+{
+	int cnt = 0;
+
+	switch (req->status) {
+	case RDMA_CQE_REQ_STS_OK:
+		cnt = process_req(dev, qp, cq, num_entries, wc, req->sq_cons,
+				  IB_WC_SUCCESS, 0);
+		break;
+	case RDMA_CQE_REQ_STS_WORK_REQUEST_FLUSHED_ERR:
+		DP_ERR(dev,
+		       "Error: POLL CQ with RDMA_CQE_REQ_STS_WORK_REQUEST_FLUSHED_ERR. CQ icid=0x%x, QP icid=0x%x\n",
+		       cq->icid, qp->icid);
+		cnt = process_req(dev, qp, cq, num_entries, wc, req->sq_cons,
+				  IB_WC_WR_FLUSH_ERR, 0);
+		break;
+	default:
+		/* process all WQE before the cosumer */
+		qp->state = QED_ROCE_QP_STATE_ERR;
+		cnt = process_req(dev, qp, cq, num_entries, wc,
+				  req->sq_cons - 1, IB_WC_SUCCESS, 0);
+		wc += cnt;
+		/* if we have extra WC fill it with actual error info */
+		if (cnt < num_entries) {
+			enum ib_wc_status wc_status;
+
+			switch (req->status) {
+			case RDMA_CQE_REQ_STS_BAD_RESPONSE_ERR:
+				DP_ERR(dev,
+				       "Error: POLL CQ with RDMA_CQE_REQ_STS_BAD_RESPONSE_ERR. CQ icid=0x%x, QP icid=0x%x\n",
+				       cq->icid, qp->icid);
+				wc_status = IB_WC_BAD_RESP_ERR;
+				break;
+			case RDMA_CQE_REQ_STS_LOCAL_LENGTH_ERR:
+				DP_ERR(dev,
+				       "Error: POLL CQ with RDMA_CQE_REQ_STS_LOCAL_LENGTH_ERR. CQ icid=0x%x, QP icid=0x%x\n",
+				       cq->icid, qp->icid);
+				wc_status = IB_WC_LOC_LEN_ERR;
+				break;
+			case RDMA_CQE_REQ_STS_LOCAL_QP_OPERATION_ERR:
+				DP_ERR(dev,
+				       "Error: POLL CQ with RDMA_CQE_REQ_STS_LOCAL_QP_OPERATION_ERR. CQ icid=0x%x, QP icid=0x%x\n",
+				       cq->icid, qp->icid);
+				wc_status = IB_WC_LOC_QP_OP_ERR;
+				break;
+			case RDMA_CQE_REQ_STS_LOCAL_PROTECTION_ERR:
+				DP_ERR(dev,
+				       "Error: POLL CQ with RDMA_CQE_REQ_STS_LOCAL_PROTECTION_ERR. CQ icid=0x%x, QP icid=0x%x\n",
+				       cq->icid, qp->icid);
+				wc_status = IB_WC_LOC_PROT_ERR;
+				break;
+			case RDMA_CQE_REQ_STS_MEMORY_MGT_OPERATION_ERR:
+				DP_ERR(dev,
+				       "Error: POLL CQ with RDMA_CQE_REQ_STS_MEMORY_MGT_OPERATION_ERR. CQ icid=0x%x, QP icid=0x%x\n",
+				       cq->icid, qp->icid);
+				wc_status = IB_WC_MW_BIND_ERR;
+				break;
+			case RDMA_CQE_REQ_STS_REMOTE_INVALID_REQUEST_ERR:
+				DP_ERR(dev,
+				       "Error: POLL CQ with RDMA_CQE_REQ_STS_REMOTE_INVALID_REQUEST_ERR. CQ icid=0x%x, QP icid=0x%x\n",
+				       cq->icid, qp->icid);
+				wc_status = IB_WC_REM_INV_REQ_ERR;
+				break;
+			case RDMA_CQE_REQ_STS_REMOTE_ACCESS_ERR:
+				DP_ERR(dev,
+				       "Error: POLL CQ with RDMA_CQE_REQ_STS_REMOTE_ACCESS_ERR. CQ icid=0x%x, QP icid=0x%x\n",
+				       cq->icid, qp->icid);
+				wc_status = IB_WC_REM_ACCESS_ERR;
+				break;
+			case RDMA_CQE_REQ_STS_REMOTE_OPERATION_ERR:
+				DP_ERR(dev,
+				       "Error: POLL CQ with RDMA_CQE_REQ_STS_REMOTE_OPERATION_ERR. CQ icid=0x%x, QP icid=0x%x\n",
+				       cq->icid, qp->icid);
+				wc_status = IB_WC_REM_OP_ERR;
+				break;
+			case RDMA_CQE_REQ_STS_RNR_NAK_RETRY_CNT_ERR:
+				DP_ERR(dev,
+				       "Error: POLL CQ with RDMA_CQE_REQ_STS_RNR_NAK_RETRY_CNT_ERR. CQ icid=0x%x, QP icid=0x%x\n",
+				       cq->icid, qp->icid);
+				wc_status = IB_WC_RNR_RETRY_EXC_ERR;
+				break;
+			case RDMA_CQE_REQ_STS_TRANSPORT_RETRY_CNT_ERR:
+				DP_ERR(dev,
+				       "Error: POLL CQ with ROCE_CQE_REQ_STS_TRANSPORT_RETRY_CNT_ERR. CQ icid=0x%x, QP icid=0x%x\n",
+				       cq->icid, qp->icid);
+				wc_status = IB_WC_RETRY_EXC_ERR;
+				break;
+			default:
+				DP_ERR(dev,
+				       "Error: POLL CQ with IB_WC_GENERAL_ERR. CQ icid=0x%x, QP icid=0x%x\n",
+				       cq->icid, qp->icid);
+				wc_status = IB_WC_GENERAL_ERR;
+			}
+			cnt += process_req(dev, qp, cq, 1, wc, req->sq_cons,
+					   wc_status, 1);
+		}
+	}
+
+	return cnt;
+}
+
+static void __process_resp_one(struct qedr_dev *dev, struct qedr_qp *qp,
+			       struct qedr_cq *cq, struct ib_wc *wc,
+			       struct rdma_cqe_responder *resp, u64 wr_id)
+{
+	enum ib_wc_status wc_status = IB_WC_SUCCESS;
+	u8 flags;
+
+	wc->opcode = IB_WC_RECV;
+	wc->wc_flags = 0;
+
+	switch (resp->status) {
+	case RDMA_CQE_RESP_STS_LOCAL_ACCESS_ERR:
+		wc_status = IB_WC_LOC_ACCESS_ERR;
+		break;
+	case RDMA_CQE_RESP_STS_LOCAL_LENGTH_ERR:
+		wc_status = IB_WC_LOC_LEN_ERR;
+		break;
+	case RDMA_CQE_RESP_STS_LOCAL_QP_OPERATION_ERR:
+		wc_status = IB_WC_LOC_QP_OP_ERR;
+		break;
+	case RDMA_CQE_RESP_STS_LOCAL_PROTECTION_ERR:
+		wc_status = IB_WC_LOC_PROT_ERR;
+		break;
+	case RDMA_CQE_RESP_STS_MEMORY_MGT_OPERATION_ERR:
+		wc_status = IB_WC_MW_BIND_ERR;
+		break;
+	case RDMA_CQE_RESP_STS_REMOTE_INVALID_REQUEST_ERR:
+		wc_status = IB_WC_REM_INV_RD_REQ_ERR;
+		break;
+	case RDMA_CQE_RESP_STS_OK:
+		wc_status = IB_WC_SUCCESS;
+		wc->byte_len = le32_to_cpu(resp->length);
+
+		flags = resp->flags & QEDR_RESP_RDMA_IMM;
+
+		if (flags == QEDR_RESP_RDMA_IMM)
+			wc->opcode = IB_WC_RECV_RDMA_WITH_IMM;
+
+		if (flags == QEDR_RESP_RDMA_IMM || flags == QEDR_RESP_IMM) {
+			wc->ex.imm_data =
+				le32_to_cpu(resp->imm_data_or_inv_r_Key);
+			wc->wc_flags |= IB_WC_WITH_IMM;
+		}
+		break;
+	default:
+		wc->status = IB_WC_GENERAL_ERR;
+		DP_ERR(dev, "Invalid CQE status detected\n");
+	}
+
+	/* fill WC */
+	wc->status = wc_status;
+	wc->src_qp = qp->id;
+	wc->qp = &qp->ibqp;
+	wc->wr_id = wr_id;
+}
+
+static int process_resp_one(struct qedr_dev *dev, struct qedr_qp *qp,
+			    struct qedr_cq *cq, struct ib_wc *wc,
+			    struct rdma_cqe_responder *resp)
+{
+	u64 wr_id = qp->rqe_wr_id[qp->rq.cons].wr_id;
+
+	__process_resp_one(dev, qp, cq, wc, resp, wr_id);
+
+	while (qp->rqe_wr_id[qp->rq.cons].wqe_size--)
+		qed_chain_consume(&qp->rq.pbl);
+	qedr_inc_sw_cons(&qp->rq);
+
+	return 1;
+}
+
+static int process_resp_flush(struct qedr_qp *qp, struct qedr_cq *cq,
+			      int num_entries, struct ib_wc *wc, u16 hw_cons)
+{
+	u16 cnt = 0;
+
+	while (num_entries && qp->rq.wqe_cons != hw_cons) {
+		/* fill WC */
+		wc->status = IB_WC_WR_FLUSH_ERR;
+		wc->wc_flags = 0;
+		wc->src_qp = qp->id;
+		wc->byte_len = 0;
+		wc->wr_id = qp->rqe_wr_id[qp->rq.cons].wr_id;
+		wc->qp = &qp->ibqp;
+		num_entries--;
+		wc++;
+		cnt++;
+		while (qp->rqe_wr_id[qp->rq.cons].wqe_size--)
+			qed_chain_consume(&qp->rq.pbl);
+		qedr_inc_sw_cons(&qp->rq);
+	}
+
+	return cnt;
+}
+
+static void try_consume_resp_cqe(struct qedr_cq *cq, struct qedr_qp *qp,
+				 struct rdma_cqe_responder *resp, int *update)
+{
+	if (le16_to_cpu(resp->rq_cons) == qp->rq.wqe_cons) {
+		consume_cqe(cq);
+		*update |= 1;
+	}
+}
+
+static int qedr_poll_cq_resp(struct qedr_dev *dev, struct qedr_qp *qp,
+			     struct qedr_cq *cq, int num_entries,
+			     struct ib_wc *wc, struct rdma_cqe_responder *resp,
+			     int *update)
+{
+	int cnt;
+
+	if (resp->status == RDMA_CQE_RESP_STS_WORK_REQUEST_FLUSHED_ERR) {
+		cnt = process_resp_flush(qp, cq, num_entries, wc,
+					 resp->rq_cons);
+		try_consume_resp_cqe(cq, qp, resp, update);
+	} else {
+		cnt = process_resp_one(dev, qp, cq, wc, resp);
+		consume_cqe(cq);
+		*update |= 1;
+	}
+
+	return cnt;
+}
+
+static void try_consume_req_cqe(struct qedr_cq *cq, struct qedr_qp *qp,
+				struct rdma_cqe_requester *req, int *update)
+{
+	if (le16_to_cpu(req->sq_cons) == qp->sq.wqe_cons) {
+		consume_cqe(cq);
+		*update |= 1;
+	}
+}
+
+int qedr_poll_cq(struct ib_cq *ibcq, int num_entries, struct ib_wc *wc)
+{
+	struct qedr_dev *dev = get_qedr_dev(ibcq->device);
+	struct qedr_cq *cq = get_qedr_cq(ibcq);
+	union rdma_cqe *cqe = cq->latest_cqe;
+	u32 old_cons, new_cons;
+	unsigned long flags;
+	int update = 0;
+	int done = 0;
+
+	if (cq->cq_type == QEDR_CQ_TYPE_GSI)
+		return qedr_gsi_poll_cq(ibcq, num_entries, wc);
+
+	spin_lock_irqsave(&cq->cq_lock, flags);
+	old_cons = qed_chain_get_cons_idx_u32(&cq->pbl);
+	while (num_entries && is_valid_cqe(cq, cqe)) {
+		struct qedr_qp *qp;
+		int cnt = 0;
+
+		/* prevent speculative reads of any field of CQE */
+		rmb();
+
+		qp = cqe_get_qp(cqe);
+		if (!qp) {
+			WARN(1, "Error: CQE QP pointer is NULL. CQE=%p\n", cqe);
+			break;
+		}
+
+		wc->qp = &qp->ibqp;
+
+		switch (cqe_get_type(cqe)) {
+		case RDMA_CQE_TYPE_REQUESTER:
+			cnt = qedr_poll_cq_req(dev, qp, cq, num_entries, wc,
+					       &cqe->req);
+			try_consume_req_cqe(cq, qp, &cqe->req, &update);
+			break;
+		case RDMA_CQE_TYPE_RESPONDER_RQ:
+			cnt = qedr_poll_cq_resp(dev, qp, cq, num_entries, wc,
+						&cqe->resp, &update);
+			break;
+		case RDMA_CQE_TYPE_INVALID:
+		default:
+			DP_ERR(dev, "Error: invalid CQE type = %d\n",
+			       cqe_get_type(cqe));
+		}
+		num_entries -= cnt;
+		wc += cnt;
+		done += cnt;
+
+		cqe = get_cqe(cq);
+	}
+	new_cons = qed_chain_get_cons_idx_u32(&cq->pbl);
+
+	cq->cq_cons += new_cons - old_cons;
+
+	if (update)
+		/* doorbell notifies abount latest VALID entry,
+		 * but chain already point to the next INVALID one
+		 */
+		doorbell_cq(cq, cq->cq_cons - 1, cq->arm_flags);
+
+	spin_unlock_irqrestore(&cq->cq_lock, flags);
+	return done;
+}
+
+int qedr_process_mad(struct ib_device *ibdev, int process_mad_flags,
+		     u8 port_num,
+		     const struct ib_wc *in_wc,
+		     const struct ib_grh *in_grh,
+		     const struct ib_mad_hdr *mad_hdr,
+		     size_t in_mad_size, struct ib_mad_hdr *out_mad,
+		     size_t *out_mad_size, u16 *out_mad_pkey_index)
+{
+	struct qedr_dev *dev = get_qedr_dev(ibdev);
+
+	DP_DEBUG(dev, QEDR_MSG_GSI,
+		 "QEDR_PROCESS_MAD in_mad %x %x %x %x %x %x %x %x\n",
+		 mad_hdr->attr_id, mad_hdr->base_version, mad_hdr->attr_mod,
+		 mad_hdr->class_specific, mad_hdr->class_version,
+		 mad_hdr->method, mad_hdr->mgmt_class, mad_hdr->status);
+	return IB_MAD_RESULT_SUCCESS;
+}
+
+int qedr_port_immutable(struct ib_device *ibdev, u8 port_num,
+			struct ib_port_immutable *immutable)
+{
+	struct ib_port_attr attr;
+	int err;
+
+	err = qedr_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 |
+				    RDMA_CORE_PORT_IBA_ROCE_UDP_ENCAP;
+	immutable->max_mad_size = IB_MGMT_MAD_SIZE;
+
+	return 0;
+}
diff --git a/drivers/infiniband/hw/qedr/verbs.h b/drivers/infiniband/hw/qedr/verbs.h
new file mode 100644
index 000000000000..a9b5e67bb81e
--- /dev/null
+++ b/drivers/infiniband/hw/qedr/verbs.h
@@ -0,0 +1,101 @@
+/* QLogic qedr NIC Driver
+ * Copyright (c) 2015-2016  QLogic Corporation
+ *
+ * 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 __QEDR_VERBS_H__
+#define __QEDR_VERBS_H__
+
+int qedr_query_device(struct ib_device *ibdev,
+		      struct ib_device_attr *attr, struct ib_udata *udata);
+int qedr_query_port(struct ib_device *, u8 port, struct ib_port_attr *props);
+int qedr_modify_port(struct ib_device *, u8 port, int mask,
+		     struct ib_port_modify *props);
+
+int qedr_query_gid(struct ib_device *, u8 port, int index, union ib_gid *gid);
+
+int qedr_query_pkey(struct ib_device *, u8 port, u16 index, u16 *pkey);
+
+struct ib_ucontext *qedr_alloc_ucontext(struct ib_device *, struct ib_udata *);
+int qedr_dealloc_ucontext(struct ib_ucontext *);
+
+int qedr_mmap(struct ib_ucontext *, struct vm_area_struct *vma);
+int qedr_del_gid(struct ib_device *device, u8 port_num,
+		 unsigned int index, void **context);
+int qedr_add_gid(struct ib_device *device, u8 port_num,
+		 unsigned int index, const union ib_gid *gid,
+		 const struct ib_gid_attr *attr, void **context);
+struct ib_pd *qedr_alloc_pd(struct ib_device *,
+			    struct ib_ucontext *, struct ib_udata *);
+int qedr_dealloc_pd(struct ib_pd *pd);
+
+struct ib_cq *qedr_create_cq(struct ib_device *ibdev,
+			     const struct ib_cq_init_attr *attr,
+			     struct ib_ucontext *ib_ctx,
+			     struct ib_udata *udata);
+int qedr_resize_cq(struct ib_cq *, int cqe, struct ib_udata *);
+int qedr_destroy_cq(struct ib_cq *);
+int qedr_arm_cq(struct ib_cq *ibcq, enum ib_cq_notify_flags flags);
+struct ib_qp *qedr_create_qp(struct ib_pd *, struct ib_qp_init_attr *attrs,
+			     struct ib_udata *);
+int qedr_modify_qp(struct ib_qp *, struct ib_qp_attr *attr,
+		   int attr_mask, struct ib_udata *udata);
+int qedr_query_qp(struct ib_qp *, struct ib_qp_attr *qp_attr,
+		  int qp_attr_mask, struct ib_qp_init_attr *);
+int qedr_destroy_qp(struct ib_qp *ibqp);
+
+struct ib_ah *qedr_create_ah(struct ib_pd *ibpd, struct ib_ah_attr *attr);
+int qedr_destroy_ah(struct ib_ah *ibah);
+
+int qedr_dereg_mr(struct ib_mr *);
+struct ib_mr *qedr_get_dma_mr(struct ib_pd *, int acc);
+
+struct ib_mr *qedr_reg_user_mr(struct ib_pd *, u64 start, u64 length,
+			       u64 virt, int acc, struct ib_udata *);
+
+int qedr_map_mr_sg(struct ib_mr *ibmr, struct scatterlist *sg,
+		   int sg_nents, unsigned int *sg_offset);
+
+struct ib_mr *qedr_alloc_mr(struct ib_pd *pd, enum ib_mr_type mr_type,
+			    u32 max_num_sg);
+int qedr_poll_cq(struct ib_cq *, int num_entries, struct ib_wc *wc);
+int qedr_post_send(struct ib_qp *, struct ib_send_wr *,
+		   struct ib_send_wr **bad_wr);
+int qedr_post_recv(struct ib_qp *, struct ib_recv_wr *,
+		   struct ib_recv_wr **bad_wr);
+int qedr_process_mad(struct ib_device *ibdev, int process_mad_flags,
+		     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);
+
+int qedr_port_immutable(struct ib_device *ibdev, u8 port_num,
+			struct ib_port_immutable *immutable);
+#endif
diff --git a/drivers/infiniband/hw/qib/qib.h b/drivers/infiniband/hw/qib/qib.h
index bbf0a163aeab..a3e21a25cea5 100644
--- a/drivers/infiniband/hw/qib/qib.h
+++ b/drivers/infiniband/hw/qib/qib.h
@@ -52,6 +52,7 @@
 #include <linux/kref.h>
 #include <linux/sched.h>
 #include <linux/kthread.h>
+#include <rdma/ib_hdrs.h>
 #include <rdma/rdma_vt.h>
 
 #include "qib_common.h"
@@ -1131,7 +1132,6 @@ extern spinlock_t qib_devs_lock;
 extern struct qib_devdata *qib_lookup(int unit);
 extern u32 qib_cpulist_count;
 extern unsigned long *qib_cpulist;
-extern u16 qpt_mask;
 extern unsigned qib_cc_table_size;
 
 int qib_init(struct qib_devdata *, int);
diff --git a/drivers/infiniband/hw/qib/qib_driver.c b/drivers/infiniband/hw/qib/qib_driver.c
index 67ee6438cf59..728e0a030d2e 100644
--- a/drivers/infiniband/hw/qib/qib_driver.c
+++ b/drivers/infiniband/hw/qib/qib_driver.c
@@ -319,8 +319,8 @@ static u32 qib_rcv_hdrerr(struct qib_ctxtdata *rcd, struct qib_pportdata *ppd,
 		ret = 1;
 	else if (eflags == QLOGIC_IB_RHF_H_TIDERR) {
 		/* For TIDERR and RC QPs premptively schedule a NAK */
-		struct qib_ib_header *hdr = (struct qib_ib_header *) rhdr;
-		struct qib_other_headers *ohdr = NULL;
+		struct ib_header *hdr = (struct ib_header *)rhdr;
+		struct ib_other_headers *ohdr = NULL;
 		struct qib_ibport *ibp = &ppd->ibport_data;
 		struct qib_devdata *dd = ppd->dd;
 		struct rvt_dev_info *rdi = &dd->verbs_dev.rdi;
@@ -588,8 +588,7 @@ move_along:
 				qib_schedule_send(qp);
 			spin_unlock_irqrestore(&qp->s_lock, flags);
 		}
-		if (atomic_dec_and_test(&qp->refcount))
-			wake_up(&qp->wait);
+		rvt_put_qp(qp);
 	}
 
 bail:
diff --git a/drivers/infiniband/hw/qib/qib_fs.c b/drivers/infiniband/hw/qib/qib_fs.c
index c3edc033f7c4..f1e66efea98a 100644
--- a/drivers/infiniband/hw/qib/qib_fs.c
+++ b/drivers/infiniband/hw/qib/qib_fs.c
@@ -64,7 +64,7 @@ static int qibfs_mknod(struct inode *dir, struct dentry *dentry,
 	inode->i_uid = GLOBAL_ROOT_UID;
 	inode->i_gid = GLOBAL_ROOT_GID;
 	inode->i_blocks = 0;
-	inode->i_atime = CURRENT_TIME;
+	inode->i_atime = current_time(inode);
 	inode->i_mtime = inode->i_atime;
 	inode->i_ctime = inode->i_atime;
 	inode->i_private = data;
diff --git a/drivers/infiniband/hw/qib/qib_iba7322.c b/drivers/infiniband/hw/qib/qib_iba7322.c
index ce4034071f9c..ded27172320e 100644
--- a/drivers/infiniband/hw/qib/qib_iba7322.c
+++ b/drivers/infiniband/hw/qib/qib_iba7322.c
@@ -1415,7 +1415,7 @@ static void flush_fifo(struct qib_pportdata *ppd)
 	u32 *hdr;
 	u64 pbc;
 	const unsigned hdrwords = 7;
-	static struct qib_ib_header ibhdr = {
+	static struct ib_header ibhdr = {
 		.lrh[0] = cpu_to_be16(0xF000 | QIB_LRH_BTH),
 		.lrh[1] = IB_LID_PERMISSIVE,
 		.lrh[2] = cpu_to_be16(hdrwords + SIZE_OF_CRC),
diff --git a/drivers/infiniband/hw/qib/qib_init.c b/drivers/infiniband/hw/qib/qib_init.c
index f253111e682e..1730aa839a47 100644
--- a/drivers/infiniband/hw/qib/qib_init.c
+++ b/drivers/infiniband/hw/qib/qib_init.c
@@ -614,8 +614,8 @@ static int qib_create_workqueues(struct qib_devdata *dd)
 
 			snprintf(wq_name, sizeof(wq_name), "qib%d_%d",
 				dd->unit, pidx);
-			ppd->qib_wq =
-				create_singlethread_workqueue(wq_name);
+			ppd->qib_wq = alloc_ordered_workqueue(wq_name,
+							      WQ_MEM_RECLAIM);
 			if (!ppd->qib_wq)
 				goto wq_error;
 		}
diff --git a/drivers/infiniband/hw/qib/qib_qp.c b/drivers/infiniband/hw/qib/qib_qp.c
index f9b8cd2354d1..99d31efe4c2f 100644
--- a/drivers/infiniband/hw/qib/qib_qp.c
+++ b/drivers/infiniband/hw/qib/qib_qp.c
@@ -41,14 +41,6 @@
 
 #include "qib.h"
 
-/*
- * mask field which was present in now deleted qib_qpn_table
- * is not present in rvt_qpn_table. Defining the same field
- * as qpt_mask here instead of adding the mask field to
- * rvt_qpn_table.
- */
-u16 qpt_mask;
-
 static inline unsigned mk_qpn(struct rvt_qpn_table *qpt,
 			      struct rvt_qpn_map *map, unsigned off)
 {
@@ -57,7 +49,7 @@ static inline unsigned mk_qpn(struct rvt_qpn_table *qpt,
 
 static inline unsigned find_next_offset(struct rvt_qpn_table *qpt,
 					struct rvt_qpn_map *map, unsigned off,
-					unsigned n)
+					unsigned n, u16 qpt_mask)
 {
 	if (qpt_mask) {
 		off++;
@@ -179,6 +171,7 @@ int qib_alloc_qpn(struct rvt_dev_info *rdi, struct rvt_qpn_table *qpt,
 	struct qib_ibdev *verbs_dev = container_of(rdi, struct qib_ibdev, rdi);
 	struct qib_devdata *dd = container_of(verbs_dev, struct qib_devdata,
 					      verbs_dev);
+	u16 qpt_mask = dd->qpn_mask;
 
 	if (type == IB_QPT_SMI || type == IB_QPT_GSI) {
 		unsigned n;
@@ -215,7 +208,7 @@ int qib_alloc_qpn(struct rvt_dev_info *rdi, struct rvt_qpn_table *qpt,
 				goto bail;
 			}
 			offset = find_next_offset(qpt, map, offset,
-				dd->n_krcv_queues);
+				dd->n_krcv_queues, qpt_mask);
 			qpn = mk_qpn(qpt, map, offset);
 			/*
 			 * This test differs from alloc_pidmap().
diff --git a/drivers/infiniband/hw/qib/qib_rc.c b/drivers/infiniband/hw/qib/qib_rc.c
index 444028a3582a..2097512e75aa 100644
--- a/drivers/infiniband/hw/qib/qib_rc.c
+++ b/drivers/infiniband/hw/qib/qib_rc.c
@@ -75,7 +75,7 @@ static void start_timer(struct rvt_qp *qp)
  * Note the QP s_lock must be held.
  */
 static int qib_make_rc_ack(struct qib_ibdev *dev, struct rvt_qp *qp,
-			   struct qib_other_headers *ohdr, u32 pmtu)
+			   struct ib_other_headers *ohdr, u32 pmtu)
 {
 	struct rvt_ack_entry *e;
 	u32 hwords;
@@ -154,10 +154,7 @@ static int qib_make_rc_ack(struct qib_ibdev *dev, struct rvt_qp *qp,
 			len = 0;
 			qp->s_ack_state = OP(ATOMIC_ACKNOWLEDGE);
 			ohdr->u.at.aeth = qib_compute_aeth(qp);
-			ohdr->u.at.atomic_ack_eth[0] =
-				cpu_to_be32(e->atomic_data >> 32);
-			ohdr->u.at.atomic_ack_eth[1] =
-				cpu_to_be32(e->atomic_data);
+			ib_u64_put(e->atomic_data, &ohdr->u.at.atomic_ack_eth);
 			hwords += sizeof(ohdr->u.at) / sizeof(u32);
 			bth2 = e->psn & QIB_PSN_MASK;
 			e->sent = 1;
@@ -234,7 +231,7 @@ int qib_make_rc_req(struct rvt_qp *qp, unsigned long *flags)
 {
 	struct qib_qp_priv *priv = qp->priv;
 	struct qib_ibdev *dev = to_idev(qp->ibqp.device);
-	struct qib_other_headers *ohdr;
+	struct ib_other_headers *ohdr;
 	struct rvt_sge_state *ss;
 	struct rvt_swqe *wqe;
 	u32 hwords;
@@ -444,20 +441,18 @@ int qib_make_rc_req(struct rvt_qp *qp, unsigned long *flags)
 			}
 			if (wqe->atomic_wr.wr.opcode == IB_WR_ATOMIC_CMP_AND_SWP) {
 				qp->s_state = OP(COMPARE_SWAP);
-				ohdr->u.atomic_eth.swap_data = cpu_to_be64(
-					wqe->atomic_wr.swap);
-				ohdr->u.atomic_eth.compare_data = cpu_to_be64(
-					wqe->atomic_wr.compare_add);
+				put_ib_ateth_swap(wqe->atomic_wr.swap,
+						  &ohdr->u.atomic_eth);
+				put_ib_ateth_swap(wqe->atomic_wr.compare_add,
+						  &ohdr->u.atomic_eth);
 			} else {
 				qp->s_state = OP(FETCH_ADD);
-				ohdr->u.atomic_eth.swap_data = cpu_to_be64(
-					wqe->atomic_wr.compare_add);
-				ohdr->u.atomic_eth.compare_data = 0;
+				put_ib_ateth_swap(wqe->atomic_wr.compare_add,
+						  &ohdr->u.atomic_eth);
+				put_ib_ateth_swap(0, &ohdr->u.atomic_eth);
 			}
-			ohdr->u.atomic_eth.vaddr[0] = cpu_to_be32(
-				wqe->atomic_wr.remote_addr >> 32);
-			ohdr->u.atomic_eth.vaddr[1] = cpu_to_be32(
-				wqe->atomic_wr.remote_addr);
+			put_ib_ateth_vaddr(wqe->atomic_wr.remote_addr,
+					   &ohdr->u.atomic_eth);
 			ohdr->u.atomic_eth.rkey = cpu_to_be32(
 				wqe->atomic_wr.rkey);
 			hwords += sizeof(struct ib_atomic_eth) / sizeof(u32);
@@ -632,8 +627,8 @@ void qib_send_rc_ack(struct rvt_qp *qp)
 	u32 hwords;
 	u32 pbufn;
 	u32 __iomem *piobuf;
-	struct qib_ib_header hdr;
-	struct qib_other_headers *ohdr;
+	struct ib_header hdr;
+	struct ib_other_headers *ohdr;
 	u32 control;
 	unsigned long flags;
 
@@ -942,9 +937,9 @@ static void reset_sending_psn(struct rvt_qp *qp, u32 psn)
 /*
  * This should be called with the QP s_lock held and interrupts disabled.
  */
-void qib_rc_send_complete(struct rvt_qp *qp, struct qib_ib_header *hdr)
+void qib_rc_send_complete(struct rvt_qp *qp, struct ib_header *hdr)
 {
-	struct qib_other_headers *ohdr;
+	struct ib_other_headers *ohdr;
 	struct rvt_swqe *wqe;
 	struct ib_wc wc;
 	unsigned i;
@@ -1177,7 +1172,7 @@ static int do_rc_ack(struct rvt_qp *qp, u32 aeth, u32 psn, int opcode,
 				qib_restart_rc(qp, qp->s_last_psn + 1, 0);
 				if (list_empty(&qp->rspwait)) {
 					qp->r_flags |= RVT_R_RSP_SEND;
-					atomic_inc(&qp->refcount);
+					rvt_get_qp(qp);
 					list_add_tail(&qp->rspwait,
 						      &rcd->qp_wait_list);
 				}
@@ -1361,7 +1356,7 @@ static void rdma_seq_err(struct rvt_qp *qp, struct qib_ibport *ibp, u32 psn,
 	qib_restart_rc(qp, qp->s_last_psn + 1, 0);
 	if (list_empty(&qp->rspwait)) {
 		qp->r_flags |= RVT_R_RSP_SEND;
-		atomic_inc(&qp->refcount);
+		rvt_get_qp(qp);
 		list_add_tail(&qp->rspwait, &rcd->qp_wait_list);
 	}
 }
@@ -1383,7 +1378,7 @@ static void rdma_seq_err(struct rvt_qp *qp, struct qib_ibport *ibp, u32 psn,
  * Called at interrupt level.
  */
 static void qib_rc_rcv_resp(struct qib_ibport *ibp,
-			    struct qib_other_headers *ohdr,
+			    struct ib_other_headers *ohdr,
 			    void *data, u32 tlen,
 			    struct rvt_qp *qp,
 			    u32 opcode,
@@ -1463,12 +1458,9 @@ static void qib_rc_rcv_resp(struct qib_ibport *ibp,
 	case OP(ATOMIC_ACKNOWLEDGE):
 	case OP(RDMA_READ_RESPONSE_FIRST):
 		aeth = be32_to_cpu(ohdr->u.aeth);
-		if (opcode == OP(ATOMIC_ACKNOWLEDGE)) {
-			__be32 *p = ohdr->u.at.atomic_ack_eth;
-
-			val = ((u64) be32_to_cpu(p[0]) << 32) |
-				be32_to_cpu(p[1]);
-		} else
+		if (opcode == OP(ATOMIC_ACKNOWLEDGE))
+			val = ib_u64_get(&ohdr->u.at.atomic_ack_eth);
+		else
 			val = 0;
 		if (!do_rc_ack(qp, aeth, psn, opcode, val, rcd) ||
 		    opcode != OP(RDMA_READ_RESPONSE_FIRST))
@@ -1608,7 +1600,7 @@ bail:
  * Return 1 if no more processing is needed; otherwise return 0 to
  * schedule a response to be sent.
  */
-static int qib_rc_rcv_error(struct qib_other_headers *ohdr,
+static int qib_rc_rcv_error(struct ib_other_headers *ohdr,
 			    void *data,
 			    struct rvt_qp *qp,
 			    u32 opcode,
@@ -1640,7 +1632,7 @@ static int qib_rc_rcv_error(struct qib_other_headers *ohdr,
 			 */
 			if (list_empty(&qp->rspwait)) {
 				qp->r_flags |= RVT_R_RSP_NAK;
-				atomic_inc(&qp->refcount);
+				rvt_get_qp(qp);
 				list_add_tail(&qp->rspwait, &rcd->qp_wait_list);
 			}
 		}
@@ -1848,11 +1840,11 @@ static inline void qib_update_ack_queue(struct rvt_qp *qp, unsigned n)
  * for the given QP.
  * Called at interrupt level.
  */
-void qib_rc_rcv(struct qib_ctxtdata *rcd, struct qib_ib_header *hdr,
+void qib_rc_rcv(struct qib_ctxtdata *rcd, struct ib_header *hdr,
 		int has_grh, void *data, u32 tlen, struct rvt_qp *qp)
 {
 	struct qib_ibport *ibp = &rcd->ppd->ibport_data;
-	struct qib_other_headers *ohdr;
+	struct ib_other_headers *ohdr;
 	u32 opcode;
 	u32 hdrsize;
 	u32 psn;
@@ -2177,8 +2169,7 @@ send_last:
 			e->rdma_sge.mr = NULL;
 		}
 		ateth = &ohdr->u.atomic_eth;
-		vaddr = ((u64) be32_to_cpu(ateth->vaddr[0]) << 32) |
-			be32_to_cpu(ateth->vaddr[1]);
+		vaddr = get_ib_ateth_vaddr(ateth);
 		if (unlikely(vaddr & (sizeof(u64) - 1)))
 			goto nack_inv_unlck;
 		rkey = be32_to_cpu(ateth->rkey);
@@ -2189,11 +2180,11 @@ send_last:
 			goto nack_acc_unlck;
 		/* Perform atomic OP and save result. */
 		maddr = (atomic64_t *) qp->r_sge.sge.vaddr;
-		sdata = be64_to_cpu(ateth->swap_data);
+		sdata = get_ib_ateth_swap(ateth);
 		e->atomic_data = (opcode == OP(FETCH_ADD)) ?
 			(u64) atomic64_add_return(sdata, maddr) - sdata :
 			(u64) cmpxchg((u64 *) qp->r_sge.sge.vaddr,
-				      be64_to_cpu(ateth->compare_data),
+				      get_ib_ateth_compare(ateth),
 				      sdata);
 		rvt_put_mr(qp->r_sge.sge.mr);
 		qp->r_sge.num_sge = 0;
@@ -2233,7 +2224,7 @@ rnr_nak:
 	/* Queue RNR NAK for later */
 	if (list_empty(&qp->rspwait)) {
 		qp->r_flags |= RVT_R_RSP_NAK;
-		atomic_inc(&qp->refcount);
+		rvt_get_qp(qp);
 		list_add_tail(&qp->rspwait, &rcd->qp_wait_list);
 	}
 	return;
@@ -2245,7 +2236,7 @@ nack_op_err:
 	/* Queue NAK for later */
 	if (list_empty(&qp->rspwait)) {
 		qp->r_flags |= RVT_R_RSP_NAK;
-		atomic_inc(&qp->refcount);
+		rvt_get_qp(qp);
 		list_add_tail(&qp->rspwait, &rcd->qp_wait_list);
 	}
 	return;
@@ -2259,7 +2250,7 @@ nack_inv:
 	/* Queue NAK for later */
 	if (list_empty(&qp->rspwait)) {
 		qp->r_flags |= RVT_R_RSP_NAK;
-		atomic_inc(&qp->refcount);
+		rvt_get_qp(qp);
 		list_add_tail(&qp->rspwait, &rcd->qp_wait_list);
 	}
 	return;
diff --git a/drivers/infiniband/hw/qib/qib_ruc.c b/drivers/infiniband/hw/qib/qib_ruc.c
index b67779256297..de1bde5950f5 100644
--- a/drivers/infiniband/hw/qib/qib_ruc.c
+++ b/drivers/infiniband/hw/qib/qib_ruc.c
@@ -265,7 +265,7 @@ static int gid_ok(union ib_gid *gid, __be64 gid_prefix, __be64 id)
  *
  * The s_lock will be acquired around the qib_migrate_qp() call.
  */
-int qib_ruc_check_hdr(struct qib_ibport *ibp, struct qib_ib_header *hdr,
+int qib_ruc_check_hdr(struct qib_ibport *ibp, struct ib_header *hdr,
 		      int has_grh, struct rvt_qp *qp, u32 bth0)
 {
 	__be64 guid;
@@ -680,7 +680,7 @@ u32 qib_make_grh(struct qib_ibport *ibp, struct ib_grh *hdr,
 	return sizeof(struct ib_grh) / sizeof(u32);
 }
 
-void qib_make_ruc_header(struct rvt_qp *qp, struct qib_other_headers *ohdr,
+void qib_make_ruc_header(struct rvt_qp *qp, struct ib_other_headers *ohdr,
 			 u32 bth0, u32 bth2)
 {
 	struct qib_qp_priv *priv = qp->priv;
diff --git a/drivers/infiniband/hw/qib/qib_uc.c b/drivers/infiniband/hw/qib/qib_uc.c
index 1d61bd04f449..5b2d483451ad 100644
--- a/drivers/infiniband/hw/qib/qib_uc.c
+++ b/drivers/infiniband/hw/qib/qib_uc.c
@@ -48,7 +48,7 @@
 int qib_make_uc_req(struct rvt_qp *qp, unsigned long *flags)
 {
 	struct qib_qp_priv *priv = qp->priv;
-	struct qib_other_headers *ohdr;
+	struct ib_other_headers *ohdr;
 	struct rvt_swqe *wqe;
 	u32 hwords;
 	u32 bth0;
@@ -236,10 +236,10 @@ bail:
  * for the given QP.
  * Called at interrupt level.
  */
-void qib_uc_rcv(struct qib_ibport *ibp, struct qib_ib_header *hdr,
+void qib_uc_rcv(struct qib_ibport *ibp, struct ib_header *hdr,
 		int has_grh, void *data, u32 tlen, struct rvt_qp *qp)
 {
-	struct qib_other_headers *ohdr;
+	struct ib_other_headers *ohdr;
 	u32 opcode;
 	u32 hdrsize;
 	u32 psn;
diff --git a/drivers/infiniband/hw/qib/qib_ud.c b/drivers/infiniband/hw/qib/qib_ud.c
index 10d062561bd9..f45cad1198b0 100644
--- a/drivers/infiniband/hw/qib/qib_ud.c
+++ b/drivers/infiniband/hw/qib/qib_ud.c
@@ -245,7 +245,7 @@ drop:
 int qib_make_ud_req(struct rvt_qp *qp, unsigned long *flags)
 {
 	struct qib_qp_priv *priv = qp->priv;
-	struct qib_other_headers *ohdr;
+	struct ib_other_headers *ohdr;
 	struct ib_ah_attr *ah_attr;
 	struct qib_pportdata *ppd;
 	struct qib_ibport *ibp;
@@ -435,10 +435,10 @@ static unsigned qib_lookup_pkey(struct qib_ibport *ibp, u16 pkey)
  * for the given QP.
  * Called at interrupt level.
  */
-void qib_ud_rcv(struct qib_ibport *ibp, struct qib_ib_header *hdr,
+void qib_ud_rcv(struct qib_ibport *ibp, struct ib_header *hdr,
 		int has_grh, void *data, u32 tlen, struct rvt_qp *qp)
 {
-	struct qib_other_headers *ohdr;
+	struct ib_other_headers *ohdr;
 	int opcode;
 	u32 hdrsize;
 	u32 pad;
diff --git a/drivers/infiniband/hw/qib/qib_user_pages.c b/drivers/infiniband/hw/qib/qib_user_pages.c
index 2d2b94fd3633..75f08624ac05 100644
--- a/drivers/infiniband/hw/qib/qib_user_pages.c
+++ b/drivers/infiniband/hw/qib/qib_user_pages.c
@@ -67,7 +67,8 @@ static int __qib_get_user_pages(unsigned long start_page, size_t num_pages,
 
 	for (got = 0; got < num_pages; got += ret) {
 		ret = get_user_pages(start_page + got * PAGE_SIZE,
-				     num_pages - got, 1, 1,
+				     num_pages - got,
+				     FOLL_WRITE | FOLL_FORCE,
 				     p + got, NULL);
 		if (ret < 0)
 			goto bail_release;
diff --git a/drivers/infiniband/hw/qib/qib_verbs.c b/drivers/infiniband/hw/qib/qib_verbs.c
index fd1dfbce5539..954f15064514 100644
--- a/drivers/infiniband/hw/qib/qib_verbs.c
+++ b/drivers/infiniband/hw/qib/qib_verbs.c
@@ -313,7 +313,7 @@ static void qib_copy_from_sge(void *data, struct rvt_sge_state *ss, u32 length)
  * for the given QP.
  * Called at interrupt level.
  */
-static void qib_qp_rcv(struct qib_ctxtdata *rcd, struct qib_ib_header *hdr,
+static void qib_qp_rcv(struct qib_ctxtdata *rcd, struct ib_header *hdr,
 		       int has_grh, void *data, u32 tlen, struct rvt_qp *qp)
 {
 	struct qib_ibport *ibp = &rcd->ppd->ibport_data;
@@ -366,10 +366,10 @@ void qib_ib_rcv(struct qib_ctxtdata *rcd, void *rhdr, void *data, u32 tlen)
 {
 	struct qib_pportdata *ppd = rcd->ppd;
 	struct qib_ibport *ibp = &ppd->ibport_data;
-	struct qib_ib_header *hdr = rhdr;
+	struct ib_header *hdr = rhdr;
 	struct qib_devdata *dd = ppd->dd;
 	struct rvt_dev_info *rdi = &dd->verbs_dev.rdi;
-	struct qib_other_headers *ohdr;
+	struct ib_other_headers *ohdr;
 	struct rvt_qp *qp;
 	u32 qp_num;
 	int lnh;
@@ -841,7 +841,7 @@ static void sdma_complete(struct qib_sdma_txreq *cookie, int status)
 	if (tx->wqe)
 		qib_send_complete(qp, tx->wqe, IB_WC_SUCCESS);
 	else if (qp->ibqp.qp_type == IB_QPT_RC) {
-		struct qib_ib_header *hdr;
+		struct ib_header *hdr;
 
 		if (tx->txreq.flags & QIB_SDMA_TXREQ_F_FREEBUF)
 			hdr = &tx->align_buf->hdr;
@@ -889,7 +889,7 @@ static int wait_kmem(struct qib_ibdev *dev, struct rvt_qp *qp)
 	return ret;
 }
 
-static int qib_verbs_send_dma(struct rvt_qp *qp, struct qib_ib_header *hdr,
+static int qib_verbs_send_dma(struct rvt_qp *qp, struct ib_header *hdr,
 			      u32 hdrwords, struct rvt_sge_state *ss, u32 len,
 			      u32 plen, u32 dwords)
 {
@@ -1025,7 +1025,7 @@ static int no_bufs_available(struct rvt_qp *qp)
 	return ret;
 }
 
-static int qib_verbs_send_pio(struct rvt_qp *qp, struct qib_ib_header *ibhdr,
+static int qib_verbs_send_pio(struct rvt_qp *qp, struct ib_header *ibhdr,
 			      u32 hdrwords, struct rvt_sge_state *ss, u32 len,
 			      u32 plen, u32 dwords)
 {
@@ -1133,7 +1133,7 @@ done:
  * Return zero if packet is sent or queued OK.
  * Return non-zero and clear qp->s_flags RVT_S_BUSY otherwise.
  */
-int qib_verbs_send(struct rvt_qp *qp, struct qib_ib_header *hdr,
+int qib_verbs_send(struct rvt_qp *qp, struct ib_header *hdr,
 		   u32 hdrwords, struct rvt_sge_state *ss, u32 len)
 {
 	struct qib_devdata *dd = dd_from_ibdev(qp->ibqp.device);
@@ -1370,7 +1370,8 @@ static int qib_modify_device(struct ib_device *device,
 	}
 
 	if (device_modify_mask & IB_DEVICE_MODIFY_NODE_DESC) {
-		memcpy(device->node_desc, device_modify->node_desc, 64);
+		memcpy(device->node_desc, device_modify->node_desc,
+		       IB_DEVICE_NODE_DESC_MAX);
 		for (i = 0; i < dd->num_pports; i++) {
 			struct qib_ibport *ibp = &dd->pport[i].ibport_data;
 
@@ -1606,8 +1607,6 @@ int qib_register_ib_device(struct qib_devdata *dd)
 	/* Only need to initialize non-zero fields. */
 	setup_timer(&dev->mem_timer, mem_timer, (unsigned long)dev);
 
-	qpt_mask = dd->qpn_mask;
-
 	INIT_LIST_HEAD(&dev->piowait);
 	INIT_LIST_HEAD(&dev->dmawait);
 	INIT_LIST_HEAD(&dev->txwait);
diff --git a/drivers/infiniband/hw/qib/qib_verbs.h b/drivers/infiniband/hw/qib/qib_verbs.h
index 736ced684842..94fd30fdedac 100644
--- a/drivers/infiniband/hw/qib/qib_verbs.h
+++ b/drivers/infiniband/hw/qib/qib_verbs.h
@@ -45,6 +45,7 @@
 #include <linux/completion.h>
 #include <rdma/ib_pack.h>
 #include <rdma/ib_user_verbs.h>
+#include <rdma/ib_hdrs.h>
 #include <rdma/rdma_vt.h>
 #include <rdma/rdmavt_cq.h>
 
@@ -63,16 +64,6 @@ struct qib_verbs_txreq;
  */
 #define QIB_UVERBS_ABI_VERSION       2
 
-#define IB_SEQ_NAK	(3 << 29)
-
-/* AETH NAK opcode values */
-#define IB_RNR_NAK                      0x20
-#define IB_NAK_PSN_ERROR                0x60
-#define IB_NAK_INVALID_REQUEST          0x61
-#define IB_NAK_REMOTE_ACCESS_ERROR      0x62
-#define IB_NAK_REMOTE_OPERATIONAL_ERROR 0x63
-#define IB_NAK_INVALID_RD_REQUEST       0x64
-
 /* IB Performance Manager status values */
 #define IB_PMA_SAMPLE_STATUS_DONE       0x00
 #define IB_PMA_SAMPLE_STATUS_STARTED    0x01
@@ -87,22 +78,9 @@ struct qib_verbs_txreq;
 
 #define QIB_VENDOR_IPG		cpu_to_be16(0xFFA0)
 
-#define IB_BTH_REQ_ACK		(1 << 31)
-#define IB_BTH_SOLICITED	(1 << 23)
-#define IB_BTH_MIG_REQ		(1 << 22)
-
 /* XXX Should be defined in ib_verbs.h enum ib_port_cap_flags */
 #define IB_PORT_OTHER_LOCAL_CHANGES_SUP (1 << 26)
 
-#define IB_GRH_VERSION		6
-#define IB_GRH_VERSION_MASK	0xF
-#define IB_GRH_VERSION_SHIFT	28
-#define IB_GRH_TCLASS_MASK	0xFF
-#define IB_GRH_TCLASS_SHIFT	20
-#define IB_GRH_FLOW_MASK	0xFFFFF
-#define IB_GRH_FLOW_SHIFT	0
-#define IB_GRH_NEXT_HDR		0x1B
-
 #define IB_DEFAULT_GID_PREFIX	cpu_to_be64(0xfe80000000000000ULL)
 
 /* Values for set/get portinfo VLCap OperationalVLs */
@@ -129,61 +107,9 @@ static inline int qib_num_vls(int vls)
 	}
 }
 
-struct ib_reth {
-	__be64 vaddr;
-	__be32 rkey;
-	__be32 length;
-} __packed;
-
-struct ib_atomic_eth {
-	__be32 vaddr[2];        /* unaligned so access as 2 32-bit words */
-	__be32 rkey;
-	__be64 swap_data;
-	__be64 compare_data;
-} __packed;
-
-struct qib_other_headers {
-	__be32 bth[3];
-	union {
-		struct {
-			__be32 deth[2];
-			__be32 imm_data;
-		} ud;
-		struct {
-			struct ib_reth reth;
-			__be32 imm_data;
-		} rc;
-		struct {
-			__be32 aeth;
-			__be32 atomic_ack_eth[2];
-		} at;
-		__be32 imm_data;
-		__be32 aeth;
-		__be32 ieth;
-		struct ib_atomic_eth atomic_eth;
-	} u;
-} __packed;
-
-/*
- * Note that UD packets with a GRH header are 8+40+12+8 = 68 bytes
- * long (72 w/ imm_data).  Only the first 56 bytes of the IB header
- * will be in the eager header buffer.  The remaining 12 or 16 bytes
- * are in the data buffer.
- */
-struct qib_ib_header {
-	__be16 lrh[4];
-	union {
-		struct {
-			struct ib_grh grh;
-			struct qib_other_headers oth;
-		} l;
-		struct qib_other_headers oth;
-	} u;
-} __packed;
-
 struct qib_pio_header {
 	__le32 pbc[2];
-	struct qib_ib_header hdr;
+	struct ib_header hdr;
 } __packed;
 
 /*
@@ -191,7 +117,7 @@ struct qib_pio_header {
  * is made common.
  */
 struct qib_qp_priv {
-	struct qib_ib_header *s_hdr;    /* next packet header to send */
+	struct ib_header *s_hdr;        /* next packet header to send */
 	struct list_head iowait;        /* link for wait PIO buf */
 	atomic_t s_dma_busy;
 	struct qib_verbs_txreq *s_tx;
@@ -376,7 +302,7 @@ void qib_verbs_sdma_desc_avail(struct qib_pportdata *ppd, unsigned avail);
 
 void qib_put_txreq(struct qib_verbs_txreq *tx);
 
-int qib_verbs_send(struct rvt_qp *qp, struct qib_ib_header *hdr,
+int qib_verbs_send(struct rvt_qp *qp, struct ib_header *hdr,
 		   u32 hdrwords, struct rvt_sge_state *ss, u32 len);
 
 void qib_copy_sge(struct rvt_sge_state *ss, void *data, u32 length,
@@ -384,10 +310,10 @@ void qib_copy_sge(struct rvt_sge_state *ss, void *data, u32 length,
 
 void qib_skip_sge(struct rvt_sge_state *ss, u32 length, int release);
 
-void qib_uc_rcv(struct qib_ibport *ibp, struct qib_ib_header *hdr,
+void qib_uc_rcv(struct qib_ibport *ibp, struct ib_header *hdr,
 		int has_grh, void *data, u32 tlen, struct rvt_qp *qp);
 
-void qib_rc_rcv(struct qib_ctxtdata *rcd, struct qib_ib_header *hdr,
+void qib_rc_rcv(struct qib_ctxtdata *rcd, struct ib_header *hdr,
 		int has_grh, void *data, u32 tlen, struct rvt_qp *qp);
 
 int qib_check_ah(struct ib_device *ibdev, struct ib_ah_attr *ah_attr);
@@ -398,13 +324,13 @@ struct ib_ah *qib_create_qp0_ah(struct qib_ibport *ibp, u16 dlid);
 
 void qib_rc_rnr_retry(unsigned long arg);
 
-void qib_rc_send_complete(struct rvt_qp *qp, struct qib_ib_header *hdr);
+void qib_rc_send_complete(struct rvt_qp *qp, struct ib_header *hdr);
 
 void qib_rc_error(struct rvt_qp *qp, enum ib_wc_status err);
 
 int qib_post_ud_send(struct rvt_qp *qp, struct ib_send_wr *wr);
 
-void qib_ud_rcv(struct qib_ibport *ibp, struct qib_ib_header *hdr,
+void qib_ud_rcv(struct qib_ibport *ibp, struct ib_header *hdr,
 		int has_grh, void *data, u32 tlen, struct rvt_qp *qp);
 
 void mr_rcu_callback(struct rcu_head *list);
@@ -413,13 +339,13 @@ int qib_get_rwqe(struct rvt_qp *qp, int wr_id_only);
 
 void qib_migrate_qp(struct rvt_qp *qp);
 
-int qib_ruc_check_hdr(struct qib_ibport *ibp, struct qib_ib_header *hdr,
+int qib_ruc_check_hdr(struct qib_ibport *ibp, struct ib_header *hdr,
 		      int has_grh, struct rvt_qp *qp, u32 bth0);
 
 u32 qib_make_grh(struct qib_ibport *ibp, struct ib_grh *hdr,
 		 struct ib_global_route *grh, u32 hwords, u32 nwords);
 
-void qib_make_ruc_header(struct rvt_qp *qp, struct qib_other_headers *ohdr,
+void qib_make_ruc_header(struct rvt_qp *qp, struct ib_other_headers *ohdr,
 			 u32 bth0, u32 bth2);
 
 void _qib_do_send(struct work_struct *work);
diff --git a/drivers/infiniband/hw/usnic/usnic_uiom.c b/drivers/infiniband/hw/usnic/usnic_uiom.c
index a0b6ebee4d8a..1ccee6ea5bc3 100644
--- a/drivers/infiniband/hw/usnic/usnic_uiom.c
+++ b/drivers/infiniband/hw/usnic/usnic_uiom.c
@@ -111,6 +111,7 @@ static int usnic_uiom_get_pages(unsigned long addr, size_t size, int writable,
 	int i;
 	int flags;
 	dma_addr_t pa;
+	unsigned int gup_flags;
 
 	if (!can_do_mlock())
 		return -EPERM;
@@ -135,6 +136,8 @@ static int usnic_uiom_get_pages(unsigned long addr, size_t size, int writable,
 
 	flags = IOMMU_READ | IOMMU_CACHE;
 	flags |= (writable) ? IOMMU_WRITE : 0;
+	gup_flags = FOLL_WRITE;
+	gup_flags |= (writable) ? 0 : FOLL_FORCE;
 	cur_base = addr & PAGE_MASK;
 	ret = 0;
 
@@ -142,7 +145,7 @@ static int usnic_uiom_get_pages(unsigned long addr, size_t size, int writable,
 		ret = get_user_pages(cur_base,
 					min_t(unsigned long, npages,
 					PAGE_SIZE / sizeof(struct page *)),
-					1, !writable, page_list, NULL);
+					gup_flags, page_list, NULL);
 
 		if (ret < 0)
 			goto out;
diff --git a/drivers/infiniband/sw/rdmavt/cq.c b/drivers/infiniband/sw/rdmavt/cq.c
index f2f229efbe64..6d9904a4a0ab 100644
--- a/drivers/infiniband/sw/rdmavt/cq.c
+++ b/drivers/infiniband/sw/rdmavt/cq.c
@@ -129,7 +129,7 @@ void rvt_cq_enter(struct rvt_cq *cq, struct ib_wc *entry, bool solicited)
 		if (likely(worker)) {
 			cq->notify = RVT_CQ_NONE;
 			cq->triggered++;
-			queue_kthread_work(worker, &cq->comptask);
+			kthread_queue_work(worker, &cq->comptask);
 		}
 	}
 
@@ -265,7 +265,7 @@ struct ib_cq *rvt_create_cq(struct ib_device *ibdev,
 	cq->ibcq.cqe = entries;
 	cq->notify = RVT_CQ_NONE;
 	spin_lock_init(&cq->lock);
-	init_kthread_work(&cq->comptask, send_complete);
+	kthread_init_work(&cq->comptask, send_complete);
 	cq->queue = wc;
 
 	ret = &cq->ibcq;
@@ -295,7 +295,7 @@ int rvt_destroy_cq(struct ib_cq *ibcq)
 	struct rvt_cq *cq = ibcq_to_rvtcq(ibcq);
 	struct rvt_dev_info *rdi = cq->rdi;
 
-	flush_kthread_work(&cq->comptask);
+	kthread_flush_work(&cq->comptask);
 	spin_lock(&rdi->n_cqs_lock);
 	rdi->n_cqs_allocated--;
 	spin_unlock(&rdi->n_cqs_lock);
@@ -514,7 +514,7 @@ int rvt_driver_cq_init(struct rvt_dev_info *rdi)
 	rdi->worker = kzalloc(sizeof(*rdi->worker), GFP_KERNEL);
 	if (!rdi->worker)
 		return -ENOMEM;
-	init_kthread_worker(rdi->worker);
+	kthread_init_worker(rdi->worker);
 	task = kthread_create_on_node(
 		kthread_worker_fn,
 		rdi->worker,
@@ -547,7 +547,7 @@ void rvt_cq_exit(struct rvt_dev_info *rdi)
 	/* blocks future queuing from send_complete() */
 	rdi->worker = NULL;
 	smp_wmb(); /* See rdi_cq_enter */
-	flush_kthread_worker(worker);
+	kthread_flush_worker(worker);
 	kthread_stop(worker->task);
 	kfree(worker);
 }
diff --git a/drivers/infiniband/sw/rdmavt/dma.c b/drivers/infiniband/sw/rdmavt/dma.c
index 33076a5eee2f..01f71caa3ac4 100644
--- a/drivers/infiniband/sw/rdmavt/dma.c
+++ b/drivers/infiniband/sw/rdmavt/dma.c
@@ -138,6 +138,21 @@ static void rvt_unmap_sg(struct ib_device *dev,
 	/* This is a stub, nothing to be done here */
 }
 
+static int rvt_map_sg_attrs(struct ib_device *dev, struct scatterlist *sgl,
+			    int nents, enum dma_data_direction direction,
+			    unsigned long attrs)
+{
+	return rvt_map_sg(dev, sgl, nents, direction);
+}
+
+static void rvt_unmap_sg_attrs(struct ib_device *dev,
+			       struct scatterlist *sg, int nents,
+			       enum dma_data_direction direction,
+			       unsigned long attrs)
+{
+	return rvt_unmap_sg(dev, sg, nents, direction);
+}
+
 static void rvt_sync_single_for_cpu(struct ib_device *dev, u64 addr,
 				    size_t size, enum dma_data_direction dir)
 {
@@ -177,6 +192,8 @@ struct ib_dma_mapping_ops rvt_default_dma_mapping_ops = {
 	.unmap_page = rvt_dma_unmap_page,
 	.map_sg = rvt_map_sg,
 	.unmap_sg = rvt_unmap_sg,
+	.map_sg_attrs = rvt_map_sg_attrs,
+	.unmap_sg_attrs = rvt_unmap_sg_attrs,
 	.sync_single_for_cpu = rvt_sync_single_for_cpu,
 	.sync_single_for_device = rvt_sync_single_for_device,
 	.alloc_coherent = rvt_dma_alloc_coherent,
diff --git a/drivers/infiniband/sw/rdmavt/qp.c b/drivers/infiniband/sw/rdmavt/qp.c
index 870b4f212fbc..6500c3b5a89c 100644
--- a/drivers/infiniband/sw/rdmavt/qp.c
+++ b/drivers/infiniband/sw/rdmavt/qp.c
@@ -488,60 +488,23 @@ static void rvt_remove_qp(struct rvt_dev_info *rdi, struct rvt_qp *qp)
 	spin_unlock_irqrestore(&rdi->qp_dev->qpt_lock, flags);
 	if (removed) {
 		synchronize_rcu();
-		if (atomic_dec_and_test(&qp->refcount))
-			wake_up(&qp->wait);
+		rvt_put_qp(qp);
 	}
 }
 
 /**
- * reset_qp - initialize the QP state to the reset state
- * @qp: the QP to reset
+ * rvt_init_qp - initialize the QP state to the reset state
+ * @qp: the QP to init or reinit
  * @type: the QP type
- * r and s lock are required to be held by the caller
+ *
+ * This function is called from both rvt_create_qp() and
+ * rvt_reset_qp().   The difference is that the reset
+ * patch the necessary locks to protect against concurent
+ * access.
  */
-static void rvt_reset_qp(struct rvt_dev_info *rdi, struct rvt_qp *qp,
-		  enum ib_qp_type type)
-	__releases(&qp->s_lock)
-	__releases(&qp->s_hlock)
-	__releases(&qp->r_lock)
-	__acquires(&qp->r_lock)
-	__acquires(&qp->s_hlock)
-	__acquires(&qp->s_lock)
+static void rvt_init_qp(struct rvt_dev_info *rdi, struct rvt_qp *qp,
+			enum ib_qp_type type)
 {
-	if (qp->state != IB_QPS_RESET) {
-		qp->state = IB_QPS_RESET;
-
-		/* Let drivers flush their waitlist */
-		rdi->driver_f.flush_qp_waiters(qp);
-		qp->s_flags &= ~(RVT_S_TIMER | RVT_S_ANY_WAIT);
-		spin_unlock(&qp->s_lock);
-		spin_unlock(&qp->s_hlock);
-		spin_unlock_irq(&qp->r_lock);
-
-		/* Stop the send queue and the retry timer */
-		rdi->driver_f.stop_send_queue(qp);
-
-		/* Wait for things to stop */
-		rdi->driver_f.quiesce_qp(qp);
-
-		/* take qp out the hash and wait for it to be unused */
-		rvt_remove_qp(rdi, qp);
-		wait_event(qp->wait, !atomic_read(&qp->refcount));
-
-		/* grab the lock b/c it was locked at call time */
-		spin_lock_irq(&qp->r_lock);
-		spin_lock(&qp->s_hlock);
-		spin_lock(&qp->s_lock);
-
-		rvt_clear_mr_refs(qp, 1);
-	}
-
-	/*
-	 * Let the driver do any tear down it needs to for a qp
-	 * that has been reset
-	 */
-	rdi->driver_f.notify_qp_reset(qp);
-
 	qp->remote_qpn = 0;
 	qp->qkey = 0;
 	qp->qp_access_flags = 0;
@@ -587,6 +550,60 @@ static void rvt_reset_qp(struct rvt_dev_info *rdi, struct rvt_qp *qp,
 }
 
 /**
+ * rvt_reset_qp - initialize the QP state to the reset state
+ * @qp: the QP to reset
+ * @type: the QP type
+ *
+ * r_lock, s_hlock, and s_lock are required to be held by the caller
+ */
+static void rvt_reset_qp(struct rvt_dev_info *rdi, struct rvt_qp *qp,
+			 enum ib_qp_type type)
+	__must_hold(&qp->s_lock)
+	__must_hold(&qp->s_hlock)
+	__must_hold(&qp->r_lock)
+{
+	lockdep_assert_held(&qp->r_lock);
+	lockdep_assert_held(&qp->s_hlock);
+	lockdep_assert_held(&qp->s_lock);
+	if (qp->state != IB_QPS_RESET) {
+		qp->state = IB_QPS_RESET;
+
+		/* Let drivers flush their waitlist */
+		rdi->driver_f.flush_qp_waiters(qp);
+		qp->s_flags &= ~(RVT_S_TIMER | RVT_S_ANY_WAIT);
+		spin_unlock(&qp->s_lock);
+		spin_unlock(&qp->s_hlock);
+		spin_unlock_irq(&qp->r_lock);
+
+		/* Stop the send queue and the retry timer */
+		rdi->driver_f.stop_send_queue(qp);
+
+		/* Wait for things to stop */
+		rdi->driver_f.quiesce_qp(qp);
+
+		/* take qp out the hash and wait for it to be unused */
+		rvt_remove_qp(rdi, qp);
+		wait_event(qp->wait, !atomic_read(&qp->refcount));
+
+		/* grab the lock b/c it was locked at call time */
+		spin_lock_irq(&qp->r_lock);
+		spin_lock(&qp->s_hlock);
+		spin_lock(&qp->s_lock);
+
+		rvt_clear_mr_refs(qp, 1);
+		/*
+		 * Let the driver do any tear down or re-init it needs to for
+		 * a qp that has been reset
+		 */
+		rdi->driver_f.notify_qp_reset(qp);
+	}
+	rvt_init_qp(rdi, qp, type);
+	lockdep_assert_held(&qp->r_lock);
+	lockdep_assert_held(&qp->s_hlock);
+	lockdep_assert_held(&qp->s_lock);
+}
+
+/**
  * rvt_create_qp - create a queue pair for a device
  * @ibpd: the protection domain who's device we create the queue pair for
  * @init_attr: the attributes of the queue pair
@@ -766,7 +783,7 @@ struct ib_qp *rvt_create_qp(struct ib_pd *ibpd,
 		}
 		qp->ibqp.qp_num = err;
 		qp->port_num = init_attr->port_num;
-		rvt_reset_qp(rdi, qp, init_attr->qp_type);
+		rvt_init_qp(rdi, qp, init_attr->qp_type);
 		break;
 
 	default:
@@ -906,6 +923,8 @@ int rvt_error_qp(struct rvt_qp *qp, enum ib_wc_status err)
 	int ret = 0;
 	struct rvt_dev_info *rdi = ib_to_rvt(qp->ibqp.device);
 
+	lockdep_assert_held(&qp->r_lock);
+	lockdep_assert_held(&qp->s_lock);
 	if (qp->state == IB_QPS_ERR || qp->state == IB_QPS_RESET)
 		goto bail;
 
@@ -980,7 +999,7 @@ static void rvt_insert_qp(struct rvt_dev_info *rdi, struct rvt_qp *qp)
 	struct rvt_ibport *rvp = rdi->ports[qp->port_num - 1];
 	unsigned long flags;
 
-	atomic_inc(&qp->refcount);
+	rvt_get_qp(qp);
 	spin_lock_irqsave(&rdi->qp_dev->qpt_lock, flags);
 
 	if (qp->ibqp.qp_num <= 1) {
@@ -997,7 +1016,7 @@ static void rvt_insert_qp(struct rvt_dev_info *rdi, struct rvt_qp *qp)
 }
 
 /**
- * qib_modify_qp - modify the attributes of a queue pair
+ * rvt_modify_qp - modify the attributes of a queue pair
  * @ibqp: the queue pair who's attributes we're modifying
  * @attr: the new attributes
  * @attr_mask: the mask of attributes to modify
diff --git a/drivers/infiniband/sw/rxe/rxe.c b/drivers/infiniband/sw/rxe/rxe.c
index ddd59270ff6d..ab6c3c25d7ff 100644
--- a/drivers/infiniband/sw/rxe/rxe.c
+++ b/drivers/infiniband/sw/rxe/rxe.c
@@ -358,38 +358,16 @@ static int __init rxe_module_init(void)
 	/* initialize slab caches for managed objects */
 	err = rxe_cache_init();
 	if (err) {
-		pr_err("rxe: unable to init object pools\n");
+		pr_err("unable to init object pools\n");
 		return err;
 	}
 
-	err = rxe_net_ipv4_init();
-	if (err) {
-		pr_err("rxe: unable to init ipv4 tunnel\n");
-		rxe_cache_exit();
-		goto exit;
-	}
-
-	err = rxe_net_ipv6_init();
-	if (err) {
-		pr_err("rxe: unable to init ipv6 tunnel\n");
-		rxe_cache_exit();
-		goto exit;
-	}
-
-	err = register_netdevice_notifier(&rxe_net_notifier);
-	if (err) {
-		pr_err("rxe: Failed to rigister netdev notifier\n");
-		goto exit;
-	}
-
-	pr_info("rxe: loaded\n");
+	err = rxe_net_init();
+	if (err)
+		return err;
 
+	pr_info("loaded\n");
 	return 0;
-
-exit:
-	rxe_release_udp_tunnel(recv_sockets.sk4);
-	rxe_release_udp_tunnel(recv_sockets.sk6);
-	return err;
 }
 
 static void __exit rxe_module_exit(void)
@@ -398,8 +376,8 @@ static void __exit rxe_module_exit(void)
 	rxe_net_exit();
 	rxe_cache_exit();
 
-	pr_info("rxe: unloaded\n");
+	pr_info("unloaded\n");
 }
 
-module_init(rxe_module_init);
+late_initcall(rxe_module_init);
 module_exit(rxe_module_exit);
diff --git a/drivers/infiniband/sw/rxe/rxe.h b/drivers/infiniband/sw/rxe/rxe.h
index 12c71c549f97..a696af81e4a5 100644
--- a/drivers/infiniband/sw/rxe/rxe.h
+++ b/drivers/infiniband/sw/rxe/rxe.h
@@ -34,6 +34,11 @@
 #ifndef RXE_H
 #define RXE_H
 
+#ifdef pr_fmt
+#undef pr_fmt
+#endif
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/module.h>
 #include <linux/skbuff.h>
 #include <linux/crc32.h>
diff --git a/drivers/infiniband/sw/rxe/rxe_av.c b/drivers/infiniband/sw/rxe/rxe_av.c
index 5c9474212d4e..604f6fee96bd 100644
--- a/drivers/infiniband/sw/rxe/rxe_av.c
+++ b/drivers/infiniband/sw/rxe/rxe_av.c
@@ -39,7 +39,7 @@ int rxe_av_chk_attr(struct rxe_dev *rxe, struct ib_ah_attr *attr)
 	struct rxe_port *port;
 
 	if (attr->port_num != 1) {
-		pr_info("rxe: invalid port_num = %d\n", attr->port_num);
+		pr_info("invalid port_num = %d\n", attr->port_num);
 		return -EINVAL;
 	}
 
@@ -47,7 +47,7 @@ int rxe_av_chk_attr(struct rxe_dev *rxe, struct ib_ah_attr *attr)
 
 	if (attr->ah_flags & IB_AH_GRH) {
 		if (attr->grh.sgid_index > port->attr.gid_tbl_len) {
-			pr_info("rxe: invalid sgid index = %d\n",
+			pr_info("invalid sgid index = %d\n",
 				attr->grh.sgid_index);
 			return -EINVAL;
 		}
diff --git a/drivers/infiniband/sw/rxe/rxe_comp.c b/drivers/infiniband/sw/rxe/rxe_comp.c
index 1c59ef2c67aa..6c5e29db88e3 100644
--- a/drivers/infiniband/sw/rxe/rxe_comp.c
+++ b/drivers/infiniband/sw/rxe/rxe_comp.c
@@ -567,7 +567,8 @@ int rxe_completer(void *arg)
 	state = COMPST_GET_ACK;
 
 	while (1) {
-		pr_debug("state = %s\n", comp_state_name[state]);
+		pr_debug("qp#%d state = %s\n", qp_num(qp),
+			 comp_state_name[state]);
 		switch (state) {
 		case COMPST_GET_ACK:
 			skb = skb_dequeue(&qp->resp_pkts);
@@ -709,7 +710,8 @@ int rxe_completer(void *arg)
 					qp->comp.rnr_retry--;
 
 				qp->req.need_retry = 1;
-				pr_debug("set rnr nak timer\n");
+				pr_debug("qp#%d set rnr nak timer\n",
+					 qp_num(qp));
 				mod_timer(&qp->rnr_nak_timer,
 					  jiffies + rnrnak_jiffies(aeth_syn(pkt)
 						& ~AETH_TYPE_MASK));
diff --git a/drivers/infiniband/sw/rxe/rxe_dma.c b/drivers/infiniband/sw/rxe/rxe_dma.c
index 7634c1a81b2b..a0f8af5851ae 100644
--- a/drivers/infiniband/sw/rxe/rxe_dma.c
+++ b/drivers/infiniband/sw/rxe/rxe_dma.c
@@ -117,6 +117,21 @@ static void rxe_unmap_sg(struct ib_device *dev,
 	WARN_ON(!valid_dma_direction(direction));
 }
 
+static int rxe_map_sg_attrs(struct ib_device *dev, struct scatterlist *sgl,
+			    int nents, enum dma_data_direction direction,
+			    unsigned long attrs)
+{
+	return rxe_map_sg(dev, sgl, nents, direction);
+}
+
+static void rxe_unmap_sg_attrs(struct ib_device *dev,
+			       struct scatterlist *sg, int nents,
+			       enum dma_data_direction direction,
+			       unsigned long attrs)
+{
+	rxe_unmap_sg(dev, sg, nents, direction);
+}
+
 static void rxe_sync_single_for_cpu(struct ib_device *dev,
 				    u64 addr,
 				    size_t size, enum dma_data_direction dir)
@@ -159,6 +174,8 @@ struct ib_dma_mapping_ops rxe_dma_mapping_ops = {
 	.unmap_page		= rxe_dma_unmap_page,
 	.map_sg			= rxe_map_sg,
 	.unmap_sg		= rxe_unmap_sg,
+	.map_sg_attrs		= rxe_map_sg_attrs,
+	.unmap_sg_attrs		= rxe_unmap_sg_attrs,
 	.sync_single_for_cpu	= rxe_sync_single_for_cpu,
 	.sync_single_for_device	= rxe_sync_single_for_device,
 	.alloc_coherent		= rxe_dma_alloc_coherent,
diff --git a/drivers/infiniband/sw/rxe/rxe_loc.h b/drivers/infiniband/sw/rxe/rxe_loc.h
index 4a5484ef604f..73849a5a91b3 100644
--- a/drivers/infiniband/sw/rxe/rxe_loc.h
+++ b/drivers/infiniband/sw/rxe/rxe_loc.h
@@ -198,7 +198,7 @@ void free_rd_atomic_resource(struct rxe_qp *qp, struct resp_res *res);
 static inline void rxe_advance_resp_resource(struct rxe_qp *qp)
 {
 	qp->resp.res_head++;
-	if (unlikely(qp->resp.res_head == qp->attr.max_rd_atomic))
+	if (unlikely(qp->resp.res_head == qp->attr.max_dest_rd_atomic))
 		qp->resp.res_head = 0;
 }
 
diff --git a/drivers/infiniband/sw/rxe/rxe_mmap.c b/drivers/infiniband/sw/rxe/rxe_mmap.c
index 54b3c7c99eff..c572a4c09359 100644
--- a/drivers/infiniband/sw/rxe/rxe_mmap.c
+++ b/drivers/infiniband/sw/rxe/rxe_mmap.c
@@ -126,7 +126,7 @@ found_it:
 
 	ret = remap_vmalloc_range(vma, ip->obj, 0);
 	if (ret) {
-		pr_err("rxe: err %d from remap_vmalloc_range\n", ret);
+		pr_err("err %d from remap_vmalloc_range\n", ret);
 		goto done;
 	}
 
diff --git a/drivers/infiniband/sw/rxe/rxe_mr.c b/drivers/infiniband/sw/rxe/rxe_mr.c
index f3dab6574504..1869152f1d23 100644
--- a/drivers/infiniband/sw/rxe/rxe_mr.c
+++ b/drivers/infiniband/sw/rxe/rxe_mr.c
@@ -39,7 +39,7 @@
  */
 static u8 rxe_get_key(void)
 {
-	static unsigned key = 1;
+	static u32 key = 1;
 
 	key = key << 1;
 
diff --git a/drivers/infiniband/sw/rxe/rxe_net.c b/drivers/infiniband/sw/rxe/rxe_net.c
index eedf2f1cafdf..b8258e4f0aea 100644
--- a/drivers/infiniband/sw/rxe/rxe_net.c
+++ b/drivers/infiniband/sw/rxe/rxe_net.c
@@ -65,7 +65,7 @@ struct rxe_dev *net_to_rxe(struct net_device *ndev)
 	return found;
 }
 
-struct rxe_dev *get_rxe_by_name(const char* name)
+struct rxe_dev *get_rxe_by_name(const char *name)
 {
 	struct rxe_dev *rxe;
 	struct rxe_dev *found = NULL;
@@ -350,14 +350,14 @@ static void prepare_ipv6_hdr(struct dst_entry *dst, struct sk_buff *skb,
 	ip6h->payload_len = htons(skb->len - sizeof(*ip6h));
 }
 
-static int prepare4(struct rxe_dev *rxe, struct sk_buff *skb, struct rxe_av *av)
+static int prepare4(struct rxe_dev *rxe, struct rxe_pkt_info *pkt,
+		    struct sk_buff *skb, struct rxe_av *av)
 {
 	struct dst_entry *dst;
 	bool xnet = false;
 	__be16 df = htons(IP_DF);
 	struct in_addr *saddr = &av->sgid_addr._sockaddr_in.sin_addr;
 	struct in_addr *daddr = &av->dgid_addr._sockaddr_in.sin_addr;
-	struct rxe_pkt_info *pkt = SKB_TO_PKT(skb);
 
 	dst = rxe_find_route4(rxe->ndev, saddr, daddr);
 	if (!dst) {
@@ -376,12 +376,12 @@ static int prepare4(struct rxe_dev *rxe, struct sk_buff *skb, struct rxe_av *av)
 	return 0;
 }
 
-static int prepare6(struct rxe_dev *rxe, struct sk_buff *skb, struct rxe_av *av)
+static int prepare6(struct rxe_dev *rxe, struct rxe_pkt_info *pkt,
+		    struct sk_buff *skb, struct rxe_av *av)
 {
 	struct dst_entry *dst;
 	struct in6_addr *saddr = &av->sgid_addr._sockaddr_in6.sin6_addr;
 	struct in6_addr *daddr = &av->dgid_addr._sockaddr_in6.sin6_addr;
-	struct rxe_pkt_info *pkt = SKB_TO_PKT(skb);
 
 	dst = rxe_find_route6(rxe->ndev, saddr, daddr);
 	if (!dst) {
@@ -408,9 +408,9 @@ static int prepare(struct rxe_dev *rxe, struct rxe_pkt_info *pkt,
 	struct rxe_av *av = rxe_get_av(pkt);
 
 	if (av->network_type == RDMA_NETWORK_IPV4)
-		err = prepare4(rxe, skb, av);
+		err = prepare4(rxe, pkt, skb, av);
 	else if (av->network_type == RDMA_NETWORK_IPV6)
-		err = prepare6(rxe, skb, av);
+		err = prepare6(rxe, pkt, skb, av);
 
 	*crc = rxe_icrc_hdr(pkt, skb);
 
@@ -601,8 +601,7 @@ void rxe_port_up(struct rxe_dev *rxe)
 	port->attr.phys_state = IB_PHYS_STATE_LINK_UP;
 
 	rxe_port_event(rxe, IB_EVENT_PORT_ACTIVE);
-	pr_info("rxe: set %s active\n", rxe->ib_dev.name);
-	return;
+	pr_info("set %s active\n", rxe->ib_dev.name);
 }
 
 /* Caller must hold net_info_lock */
@@ -615,8 +614,7 @@ void rxe_port_down(struct rxe_dev *rxe)
 	port->attr.phys_state = IB_PHYS_STATE_LINK_DOWN;
 
 	rxe_port_event(rxe, IB_EVENT_PORT_ERR);
-	pr_info("rxe: set %s down\n", rxe->ib_dev.name);
-	return;
+	pr_info("set %s down\n", rxe->ib_dev.name);
 }
 
 static int rxe_notify(struct notifier_block *not_blk,
@@ -641,7 +639,7 @@ static int rxe_notify(struct notifier_block *not_blk,
 		rxe_port_down(rxe);
 		break;
 	case NETDEV_CHANGEMTU:
-		pr_info("rxe: %s changed mtu to %d\n", ndev->name, ndev->mtu);
+		pr_info("%s changed mtu to %d\n", ndev->name, ndev->mtu);
 		rxe_set_mtu(rxe, ndev->mtu);
 		break;
 	case NETDEV_REBOOT:
@@ -651,7 +649,7 @@ static int rxe_notify(struct notifier_block *not_blk,
 	case NETDEV_CHANGENAME:
 	case NETDEV_FEAT_CHANGE:
 	default:
-		pr_info("rxe: ignoring netdev event = %ld for %s\n",
+		pr_info("ignoring netdev event = %ld for %s\n",
 			event, ndev->name);
 		break;
 	}
@@ -671,7 +669,7 @@ int rxe_net_ipv4_init(void)
 				htons(ROCE_V2_UDP_DPORT), false);
 	if (IS_ERR(recv_sockets.sk4)) {
 		recv_sockets.sk4 = NULL;
-		pr_err("rxe: Failed to create IPv4 UDP tunnel\n");
+		pr_err("Failed to create IPv4 UDP tunnel\n");
 		return -1;
 	}
 
@@ -688,7 +686,7 @@ int rxe_net_ipv6_init(void)
 						htons(ROCE_V2_UDP_DPORT), true);
 	if (IS_ERR(recv_sockets.sk6)) {
 		recv_sockets.sk6 = NULL;
-		pr_err("rxe: Failed to create IPv6 UDP tunnel\n");
+		pr_err("Failed to create IPv6 UDP tunnel\n");
 		return -1;
 	}
 #endif
@@ -701,3 +699,26 @@ void rxe_net_exit(void)
 	rxe_release_udp_tunnel(recv_sockets.sk4);
 	unregister_netdevice_notifier(&rxe_net_notifier);
 }
+
+int rxe_net_init(void)
+{
+	int err;
+
+	recv_sockets.sk6 = NULL;
+
+	err = rxe_net_ipv4_init();
+	if (err)
+		return err;
+	err = rxe_net_ipv6_init();
+	if (err)
+		goto err_out;
+	err = register_netdevice_notifier(&rxe_net_notifier);
+	if (err) {
+		pr_err("Failed to register netdev notifier\n");
+		goto err_out;
+	}
+	return 0;
+err_out:
+	rxe_net_exit();
+	return err;
+}
diff --git a/drivers/infiniband/sw/rxe/rxe_net.h b/drivers/infiniband/sw/rxe/rxe_net.h
index 0daf7f09e5b5..1c06b3bfe1b6 100644
--- a/drivers/infiniband/sw/rxe/rxe_net.h
+++ b/drivers/infiniband/sw/rxe/rxe_net.h
@@ -49,8 +49,7 @@ void rxe_release_udp_tunnel(struct socket *sk);
 
 struct rxe_dev *rxe_net_add(struct net_device *ndev);
 
-int rxe_net_ipv4_init(void);
-int rxe_net_ipv6_init(void);
+int rxe_net_init(void);
 void rxe_net_exit(void);
 
 #endif /* RXE_NET_H */
diff --git a/drivers/infiniband/sw/rxe/rxe_qp.c b/drivers/infiniband/sw/rxe/rxe_qp.c
index 22ba24f2a2c1..b8036cfbce04 100644
--- a/drivers/infiniband/sw/rxe/rxe_qp.c
+++ b/drivers/infiniband/sw/rxe/rxe_qp.c
@@ -146,7 +146,7 @@ static void free_rd_atomic_resources(struct rxe_qp *qp)
 	if (qp->resp.resources) {
 		int i;
 
-		for (i = 0; i < qp->attr.max_rd_atomic; i++) {
+		for (i = 0; i < qp->attr.max_dest_rd_atomic; i++) {
 			struct resp_res *res = &qp->resp.resources[i];
 
 			free_rd_atomic_resource(qp, res);
@@ -174,7 +174,7 @@ static void cleanup_rd_atomic_resources(struct rxe_qp *qp)
 	struct resp_res *res;
 
 	if (qp->resp.resources) {
-		for (i = 0; i < qp->attr.max_rd_atomic; i++) {
+		for (i = 0; i < qp->attr.max_dest_rd_atomic; i++) {
 			res = &qp->resp.resources[i];
 			free_rd_atomic_resource(qp, res);
 		}
@@ -298,8 +298,8 @@ static int rxe_qp_init_resp(struct rxe_dev *rxe, struct rxe_qp *qp,
 
 		wqe_size = rcv_wqe_size(qp->rq.max_sge);
 
-		pr_debug("max_wr = %d, max_sge = %d, wqe_size = %d\n",
-			 qp->rq.max_wr, qp->rq.max_sge, wqe_size);
+		pr_debug("qp#%d max_wr = %d, max_sge = %d, wqe_size = %d\n",
+			 qp_num(qp), qp->rq.max_wr, qp->rq.max_sge, wqe_size);
 
 		qp->rq.queue = rxe_queue_init(rxe,
 					      &qp->rq.max_wr,
@@ -596,14 +596,21 @@ int rxe_qp_from_attr(struct rxe_qp *qp, struct ib_qp_attr *attr, int mask,
 	if (mask & IB_QP_MAX_QP_RD_ATOMIC) {
 		int max_rd_atomic = __roundup_pow_of_two(attr->max_rd_atomic);
 
+		qp->attr.max_rd_atomic = max_rd_atomic;
+		atomic_set(&qp->req.rd_atomic, max_rd_atomic);
+	}
+
+	if (mask & IB_QP_MAX_DEST_RD_ATOMIC) {
+		int max_dest_rd_atomic =
+			__roundup_pow_of_two(attr->max_dest_rd_atomic);
+
+		qp->attr.max_dest_rd_atomic = max_dest_rd_atomic;
+
 		free_rd_atomic_resources(qp);
 
-		err = alloc_rd_atomic_resources(qp, max_rd_atomic);
+		err = alloc_rd_atomic_resources(qp, max_dest_rd_atomic);
 		if (err)
 			return err;
-
-		qp->attr.max_rd_atomic = max_rd_atomic;
-		atomic_set(&qp->req.rd_atomic, max_rd_atomic);
 	}
 
 	if (mask & IB_QP_CUR_STATE)
@@ -673,24 +680,27 @@ int rxe_qp_from_attr(struct rxe_qp *qp, struct ib_qp_attr *attr, int mask,
 	if (mask & IB_QP_RETRY_CNT) {
 		qp->attr.retry_cnt = attr->retry_cnt;
 		qp->comp.retry_cnt = attr->retry_cnt;
-		pr_debug("set retry count = %d\n", attr->retry_cnt);
+		pr_debug("qp#%d set retry count = %d\n", qp_num(qp),
+			 attr->retry_cnt);
 	}
 
 	if (mask & IB_QP_RNR_RETRY) {
 		qp->attr.rnr_retry = attr->rnr_retry;
 		qp->comp.rnr_retry = attr->rnr_retry;
-		pr_debug("set rnr retry count = %d\n", attr->rnr_retry);
+		pr_debug("qp#%d set rnr retry count = %d\n", qp_num(qp),
+			 attr->rnr_retry);
 	}
 
 	if (mask & IB_QP_RQ_PSN) {
 		qp->attr.rq_psn = (attr->rq_psn & BTH_PSN_MASK);
 		qp->resp.psn = qp->attr.rq_psn;
-		pr_debug("set resp psn = 0x%x\n", qp->resp.psn);
+		pr_debug("qp#%d set resp psn = 0x%x\n", qp_num(qp),
+			 qp->resp.psn);
 	}
 
 	if (mask & IB_QP_MIN_RNR_TIMER) {
 		qp->attr.min_rnr_timer = attr->min_rnr_timer;
-		pr_debug("set min rnr timer = 0x%x\n",
+		pr_debug("qp#%d set min rnr timer = 0x%x\n", qp_num(qp),
 			 attr->min_rnr_timer);
 	}
 
@@ -698,12 +708,7 @@ int rxe_qp_from_attr(struct rxe_qp *qp, struct ib_qp_attr *attr, int mask,
 		qp->attr.sq_psn = (attr->sq_psn & BTH_PSN_MASK);
 		qp->req.psn = qp->attr.sq_psn;
 		qp->comp.psn = qp->attr.sq_psn;
-		pr_debug("set req psn = 0x%x\n", qp->req.psn);
-	}
-
-	if (mask & IB_QP_MAX_DEST_RD_ATOMIC) {
-		qp->attr.max_dest_rd_atomic =
-			__roundup_pow_of_two(attr->max_dest_rd_atomic);
+		pr_debug("qp#%d set req psn = 0x%x\n", qp_num(qp), qp->req.psn);
 	}
 
 	if (mask & IB_QP_PATH_MIG_STATE)
@@ -717,38 +722,38 @@ int rxe_qp_from_attr(struct rxe_qp *qp, struct ib_qp_attr *attr, int mask,
 
 		switch (attr->qp_state) {
 		case IB_QPS_RESET:
-			pr_debug("qp state -> RESET\n");
+			pr_debug("qp#%d state -> RESET\n", qp_num(qp));
 			rxe_qp_reset(qp);
 			break;
 
 		case IB_QPS_INIT:
-			pr_debug("qp state -> INIT\n");
+			pr_debug("qp#%d state -> INIT\n", qp_num(qp));
 			qp->req.state = QP_STATE_INIT;
 			qp->resp.state = QP_STATE_INIT;
 			break;
 
 		case IB_QPS_RTR:
-			pr_debug("qp state -> RTR\n");
+			pr_debug("qp#%d state -> RTR\n", qp_num(qp));
 			qp->resp.state = QP_STATE_READY;
 			break;
 
 		case IB_QPS_RTS:
-			pr_debug("qp state -> RTS\n");
+			pr_debug("qp#%d state -> RTS\n", qp_num(qp));
 			qp->req.state = QP_STATE_READY;
 			break;
 
 		case IB_QPS_SQD:
-			pr_debug("qp state -> SQD\n");
+			pr_debug("qp#%d state -> SQD\n", qp_num(qp));
 			rxe_qp_drain(qp);
 			break;
 
 		case IB_QPS_SQE:
-			pr_warn("qp state -> SQE !!?\n");
+			pr_warn("qp#%d state -> SQE !!?\n", qp_num(qp));
 			/* Not possible from modify_qp. */
 			break;
 
 		case IB_QPS_ERR:
-			pr_debug("qp state -> ERR\n");
+			pr_debug("qp#%d state -> ERR\n", qp_num(qp));
 			rxe_qp_error(qp);
 			break;
 		}
diff --git a/drivers/infiniband/sw/rxe/rxe_recv.c b/drivers/infiniband/sw/rxe/rxe_recv.c
index 144d2f129fcd..46f062842a9a 100644
--- a/drivers/infiniband/sw/rxe/rxe_recv.c
+++ b/drivers/infiniband/sw/rxe/rxe_recv.c
@@ -387,7 +387,8 @@ int rxe_rcv(struct sk_buff *skb)
 	pack_icrc = be32_to_cpu(*icrcp);
 
 	calc_icrc = rxe_icrc_hdr(pkt, skb);
-	calc_icrc = crc32_le(calc_icrc, (u8 *)payload_addr(pkt), payload_size(pkt));
+	calc_icrc = crc32_le(calc_icrc, (u8 *)payload_addr(pkt),
+			     payload_size(pkt));
 	calc_icrc = cpu_to_be32(~calc_icrc);
 	if (unlikely(calc_icrc != pack_icrc)) {
 		char saddr[sizeof(struct in6_addr)];
diff --git a/drivers/infiniband/sw/rxe/rxe_req.c b/drivers/infiniband/sw/rxe/rxe_req.c
index 13a848a518e8..832846b73ea0 100644
--- a/drivers/infiniband/sw/rxe/rxe_req.c
+++ b/drivers/infiniband/sw/rxe/rxe_req.c
@@ -38,7 +38,7 @@
 #include "rxe_queue.h"
 
 static int next_opcode(struct rxe_qp *qp, struct rxe_send_wqe *wqe,
-		       unsigned opcode);
+		       u32 opcode);
 
 static inline void retry_first_write_send(struct rxe_qp *qp,
 					  struct rxe_send_wqe *wqe,
@@ -121,7 +121,7 @@ void rnr_nak_timer(unsigned long data)
 {
 	struct rxe_qp *qp = (struct rxe_qp *)data;
 
-	pr_debug("rnr nak timer fired\n");
+	pr_debug("qp#%d rnr nak timer fired\n", qp_num(qp));
 	rxe_run_task(&qp->req.task, 1);
 }
 
@@ -187,7 +187,7 @@ static struct rxe_send_wqe *req_next_wqe(struct rxe_qp *qp)
 	return wqe;
 }
 
-static int next_opcode_rc(struct rxe_qp *qp, unsigned opcode, int fits)
+static int next_opcode_rc(struct rxe_qp *qp, u32 opcode, int fits)
 {
 	switch (opcode) {
 	case IB_WR_RDMA_WRITE:
@@ -259,7 +259,7 @@ static int next_opcode_rc(struct rxe_qp *qp, unsigned opcode, int fits)
 	return -EINVAL;
 }
 
-static int next_opcode_uc(struct rxe_qp *qp, unsigned opcode, int fits)
+static int next_opcode_uc(struct rxe_qp *qp, u32 opcode, int fits)
 {
 	switch (opcode) {
 	case IB_WR_RDMA_WRITE:
@@ -311,7 +311,7 @@ static int next_opcode_uc(struct rxe_qp *qp, unsigned opcode, int fits)
 }
 
 static int next_opcode(struct rxe_qp *qp, struct rxe_send_wqe *wqe,
-		       unsigned opcode)
+		       u32 opcode)
 {
 	int fits = (wqe->dma.resid <= qp->mtu);
 
@@ -588,7 +588,7 @@ int rxe_requester(void *arg)
 	struct rxe_pkt_info pkt;
 	struct sk_buff *skb;
 	struct rxe_send_wqe *wqe;
-	unsigned mask;
+	enum rxe_hdr_mask mask;
 	int payload;
 	int mtu;
 	int opcode;
@@ -626,7 +626,8 @@ next_wqe:
 			rmr = rxe_pool_get_index(&rxe->mr_pool,
 						 wqe->wr.ex.invalidate_rkey >> 8);
 			if (!rmr) {
-				pr_err("No mr for key %#x\n", wqe->wr.ex.invalidate_rkey);
+				pr_err("No mr for key %#x\n",
+				       wqe->wr.ex.invalidate_rkey);
 				wqe->state = wqe_state_error;
 				wqe->status = IB_WC_MW_BIND_ERR;
 				goto exit;
@@ -702,12 +703,12 @@ next_wqe:
 
 	skb = init_req_packet(qp, wqe, opcode, payload, &pkt);
 	if (unlikely(!skb)) {
-		pr_err("Failed allocating skb\n");
+		pr_err("qp#%d Failed allocating skb\n", qp_num(qp));
 		goto err;
 	}
 
 	if (fill_packet(qp, wqe, &pkt, skb, payload)) {
-		pr_debug("Error during fill packet\n");
+		pr_debug("qp#%d Error during fill packet\n", qp_num(qp));
 		goto err;
 	}
 
diff --git a/drivers/infiniband/sw/rxe/rxe_resp.c b/drivers/infiniband/sw/rxe/rxe_resp.c
index 3e0f0f2baace..dd3d88adc003 100644
--- a/drivers/infiniband/sw/rxe/rxe_resp.c
+++ b/drivers/infiniband/sw/rxe/rxe_resp.c
@@ -383,7 +383,7 @@ static enum resp_states check_resource(struct rxe_qp *qp,
 		 * too many read/atomic ops, we just
 		 * recycle the responder resource queue
 		 */
-		if (likely(qp->attr.max_rd_atomic > 0))
+		if (likely(qp->attr.max_dest_rd_atomic > 0))
 			return RESPST_CHK_LENGTH;
 		else
 			return RESPST_ERR_TOO_MANY_RDMA_ATM_REQ;
@@ -749,6 +749,18 @@ static enum resp_states read_reply(struct rxe_qp *qp,
 	return state;
 }
 
+static void build_rdma_network_hdr(union rdma_network_hdr *hdr,
+				   struct rxe_pkt_info *pkt)
+{
+	struct sk_buff *skb = PKT_TO_SKB(pkt);
+
+	memset(hdr, 0, sizeof(*hdr));
+	if (skb->protocol == htons(ETH_P_IP))
+		memcpy(&hdr->roce4grh, ip_hdr(skb), sizeof(hdr->roce4grh));
+	else if (skb->protocol == htons(ETH_P_IPV6))
+		memcpy(&hdr->ibgrh, ipv6_hdr(skb), sizeof(hdr->ibgrh));
+}
+
 /* Executes a new request. A retried request never reach that function (send
  * and writes are discarded, and reads and atomics are retried elsewhere.
  */
@@ -761,13 +773,8 @@ static enum resp_states execute(struct rxe_qp *qp, struct rxe_pkt_info *pkt)
 		    qp_type(qp) == IB_QPT_SMI ||
 		    qp_type(qp) == IB_QPT_GSI) {
 			union rdma_network_hdr hdr;
-			struct sk_buff *skb = PKT_TO_SKB(pkt);
 
-			memset(&hdr, 0, sizeof(hdr));
-			if (skb->protocol == htons(ETH_P_IP))
-				memcpy(&hdr.roce4grh, ip_hdr(skb), sizeof(hdr.roce4grh));
-			else if (skb->protocol == htons(ETH_P_IPV6))
-				memcpy(&hdr.ibgrh, ipv6_hdr(skb), sizeof(hdr.ibgrh));
+			build_rdma_network_hdr(&hdr, pkt);
 
 			err = send_data_in(qp, &hdr, sizeof(hdr));
 			if (err)
@@ -881,7 +888,8 @@ static enum resp_states do_complete(struct rxe_qp *qp,
 				rmr = rxe_pool_get_index(&rxe->mr_pool,
 							 wc->ex.invalidate_rkey >> 8);
 				if (unlikely(!rmr)) {
-					pr_err("Bad rkey %#x invalidation\n", wc->ex.invalidate_rkey);
+					pr_err("Bad rkey %#x invalidation\n",
+					       wc->ex.invalidate_rkey);
 					return RESPST_ERROR;
 				}
 				rmr->state = RXE_MEM_STATE_FREE;
@@ -1208,7 +1216,8 @@ int rxe_responder(void *arg)
 	}
 
 	while (1) {
-		pr_debug("state = %s\n", resp_state_name[state]);
+		pr_debug("qp#%d state = %s\n", qp_num(qp),
+			 resp_state_name[state]);
 		switch (state) {
 		case RESPST_GET_REQ:
 			state = get_req(qp, &pkt);
diff --git a/drivers/infiniband/sw/rxe/rxe_sysfs.c b/drivers/infiniband/sw/rxe/rxe_sysfs.c
index cf8e77800046..d5ed7571128f 100644
--- a/drivers/infiniband/sw/rxe/rxe_sysfs.c
+++ b/drivers/infiniband/sw/rxe/rxe_sysfs.c
@@ -79,7 +79,7 @@ static int rxe_param_set_add(const char *val, const struct kernel_param *kp)
 
 	len = sanitize_arg(val, intf, sizeof(intf));
 	if (!len) {
-		pr_err("rxe: add: invalid interface name\n");
+		pr_err("add: invalid interface name\n");
 		err = -EINVAL;
 		goto err;
 	}
@@ -92,20 +92,20 @@ static int rxe_param_set_add(const char *val, const struct kernel_param *kp)
 	}
 
 	if (net_to_rxe(ndev)) {
-		pr_err("rxe: already configured on %s\n", intf);
+		pr_err("already configured on %s\n", intf);
 		err = -EINVAL;
 		goto err;
 	}
 
 	rxe = rxe_net_add(ndev);
 	if (!rxe) {
-		pr_err("rxe: failed to add %s\n", intf);
+		pr_err("failed to add %s\n", intf);
 		err = -EINVAL;
 		goto err;
 	}
 
 	rxe_set_port_state(ndev);
-	pr_info("rxe: added %s to %s\n", rxe->ib_dev.name, intf);
+	pr_info("added %s to %s\n", rxe->ib_dev.name, intf);
 err:
 	if (ndev)
 		dev_put(ndev);
@@ -120,7 +120,7 @@ static int rxe_param_set_remove(const char *val, const struct kernel_param *kp)
 
 	len = sanitize_arg(val, intf, sizeof(intf));
 	if (!len) {
-		pr_err("rxe: add: invalid interface name\n");
+		pr_err("add: invalid interface name\n");
 		return -EINVAL;
 	}
 
@@ -133,7 +133,7 @@ static int rxe_param_set_remove(const char *val, const struct kernel_param *kp)
 	rxe = get_rxe_by_name(intf);
 
 	if (!rxe) {
-		pr_err("rxe: not configured on %s\n", intf);
+		pr_err("not configured on %s\n", intf);
 		return -EINVAL;
 	}
 
diff --git a/drivers/infiniband/sw/rxe/rxe_verbs.c b/drivers/infiniband/sw/rxe/rxe_verbs.c
index 4552be960c6a..19841c863daf 100644
--- a/drivers/infiniband/sw/rxe/rxe_verbs.c
+++ b/drivers/infiniband/sw/rxe/rxe_verbs.c
@@ -100,10 +100,12 @@ static int rxe_query_port(struct ib_device *dev,
 		rxe->ndev->ethtool_ops->get_settings(rxe->ndev, &cmd);
 		speed = cmd.speed;
 	} else {
-		pr_warn("%s speed is unknown, defaulting to 1000\n", rxe->ndev->name);
+		pr_warn("%s speed is unknown, defaulting to 1000\n",
+			rxe->ndev->name);
 		speed = 1000;
 	}
-	rxe_eth_speed_to_ib_speed(speed, &attr->active_speed, &attr->active_width);
+	rxe_eth_speed_to_ib_speed(speed, &attr->active_speed,
+				  &attr->active_width);
 	mutex_unlock(&rxe->usdev_lock);
 
 	return 0;
@@ -761,7 +763,7 @@ static int init_send_wqe(struct rxe_qp *qp, struct ib_send_wr *ibwr,
 }
 
 static int post_one_send(struct rxe_qp *qp, struct ib_send_wr *ibwr,
-			 unsigned mask, u32 length)
+			 unsigned int mask, u32 length)
 {
 	int err;
 	struct rxe_sq *sq = &qp->sq;
@@ -801,26 +803,15 @@ err1:
 	return err;
 }
 
-static int rxe_post_send(struct ib_qp *ibqp, struct ib_send_wr *wr,
-			 struct ib_send_wr **bad_wr)
+static int rxe_post_send_kernel(struct rxe_qp *qp, struct ib_send_wr *wr,
+				struct ib_send_wr **bad_wr)
 {
 	int err = 0;
-	struct rxe_qp *qp = to_rqp(ibqp);
 	unsigned int mask;
 	unsigned int length = 0;
 	int i;
 	int must_sched;
 
-	if (unlikely(!qp->valid)) {
-		*bad_wr = wr;
-		return -EINVAL;
-	}
-
-	if (unlikely(qp->req.state < QP_STATE_READY)) {
-		*bad_wr = wr;
-		return -EINVAL;
-	}
-
 	while (wr) {
 		mask = wr_opcode_mask(wr->opcode, qp);
 		if (unlikely(!mask)) {
@@ -861,6 +852,29 @@ static int rxe_post_send(struct ib_qp *ibqp, struct ib_send_wr *wr,
 	return err;
 }
 
+static int rxe_post_send(struct ib_qp *ibqp, struct ib_send_wr *wr,
+			 struct ib_send_wr **bad_wr)
+{
+	struct rxe_qp *qp = to_rqp(ibqp);
+
+	if (unlikely(!qp->valid)) {
+		*bad_wr = wr;
+		return -EINVAL;
+	}
+
+	if (unlikely(qp->req.state < QP_STATE_READY)) {
+		*bad_wr = wr;
+		return -EINVAL;
+	}
+
+	if (qp->is_user) {
+		/* Utilize process context to do protocol processing */
+		rxe_run_task(&qp->req.task, 0);
+		return 0;
+	} else
+		return rxe_post_send_kernel(qp, wr, bad_wr);
+}
+
 static int rxe_post_recv(struct ib_qp *ibqp, struct ib_recv_wr *wr,
 			 struct ib_recv_wr **bad_wr)
 {
@@ -1133,8 +1147,8 @@ static int rxe_set_page(struct ib_mr *ibmr, u64 addr)
 	return 0;
 }
 
-static int rxe_map_mr_sg(struct ib_mr *ibmr, struct scatterlist *sg, int sg_nents,
-			 unsigned int *sg_offset)
+static int rxe_map_mr_sg(struct ib_mr *ibmr, struct scatterlist *sg,
+			 int sg_nents, unsigned int *sg_offset)
 {
 	struct rxe_mem *mr = to_rmr(ibmr);
 	int n;
diff --git a/drivers/infiniband/ulp/ipoib/ipoib.h b/drivers/infiniband/ulp/ipoib/ipoib.h
index 9dbfcc0ab577..7b8d2d9e2263 100644
--- a/drivers/infiniband/ulp/ipoib/ipoib.h
+++ b/drivers/infiniband/ulp/ipoib/ipoib.h
@@ -772,7 +772,13 @@ static inline void ipoib_unregister_debugfs(void) { }
 #define ipoib_printk(level, priv, format, arg...)	\
 	printk(level "%s: " format, ((struct ipoib_dev_priv *) priv)->dev->name , ## arg)
 #define ipoib_warn(priv, format, arg...)		\
-	ipoib_printk(KERN_WARNING, priv, format , ## arg)
+do {							\
+	static DEFINE_RATELIMIT_STATE(_rs,		\
+		10 * HZ /*10 seconds */,		\
+		100);		\
+	if (__ratelimit(&_rs))				\
+		ipoib_printk(KERN_WARNING, priv, format , ## arg);\
+} while (0)
 
 extern int ipoib_sendq_size;
 extern int ipoib_recvq_size;
diff --git a/drivers/infiniband/ulp/ipoib/ipoib_main.c b/drivers/infiniband/ulp/ipoib/ipoib_main.c
index cc1c1b062ea5..5636fc3da6b8 100644
--- a/drivers/infiniband/ulp/ipoib/ipoib_main.c
+++ b/drivers/infiniband/ulp/ipoib/ipoib_main.c
@@ -2196,7 +2196,8 @@ static int __init ipoib_init_module(void)
 	 * its private workqueue, and we only queue up flush events
 	 * on our global flush workqueue.  This avoids the deadlocks.
 	 */
-	ipoib_workqueue = create_singlethread_workqueue("ipoib_flush");
+	ipoib_workqueue = alloc_ordered_workqueue("ipoib_flush",
+						  WQ_MEM_RECLAIM);
 	if (!ipoib_workqueue) {
 		ret = -ENOMEM;
 		goto err_fs;
diff --git a/drivers/infiniband/ulp/ipoib/ipoib_verbs.c b/drivers/infiniband/ulp/ipoib/ipoib_verbs.c
index c55ecb2c3736..189dcd1709d2 100644
--- a/drivers/infiniband/ulp/ipoib/ipoib_verbs.c
+++ b/drivers/infiniband/ulp/ipoib/ipoib_verbs.c
@@ -147,7 +147,7 @@ int ipoib_transport_dev_init(struct net_device *dev, struct ib_device *ca)
 	int ret, size;
 	int i;
 
-	priv->pd = ib_alloc_pd(priv->ca);
+	priv->pd = ib_alloc_pd(priv->ca, 0);
 	if (IS_ERR(priv->pd)) {
 		printk(KERN_WARNING "%s: failed to allocate PD\n", ca->name);
 		return -ENODEV;
@@ -157,7 +157,7 @@ int ipoib_transport_dev_init(struct net_device *dev, struct ib_device *ca)
 	 * the various IPoIB tasks assume they will never race against
 	 * themselves, so always use a single thread workqueue
 	 */
-	priv->wq = create_singlethread_workqueue("ipoib_wq");
+	priv->wq = alloc_ordered_workqueue("ipoib_wq", WQ_MEM_RECLAIM);
 	if (!priv->wq) {
 		printk(KERN_WARNING "ipoib: failed to allocate device WQ\n");
 		goto out_free_pd;
diff --git a/drivers/infiniband/ulp/iser/iscsi_iser.h b/drivers/infiniband/ulp/iser/iscsi_iser.h
index 0351059783b1..0be6a7c5ddb5 100644
--- a/drivers/infiniband/ulp/iser/iscsi_iser.h
+++ b/drivers/infiniband/ulp/iser/iscsi_iser.h
@@ -374,7 +374,6 @@ struct iser_reg_ops {
 struct iser_device {
 	struct ib_device             *ib_device;
 	struct ib_pd	             *pd;
-	struct ib_mr	             *mr;
 	struct ib_event_handler      event_handler;
 	struct list_head             ig_list;
 	int                          refcount;
diff --git a/drivers/infiniband/ulp/iser/iser_memory.c b/drivers/infiniband/ulp/iser/iser_memory.c
index 90be56893414..9c3e9ab53a41 100644
--- a/drivers/infiniband/ulp/iser/iser_memory.c
+++ b/drivers/infiniband/ulp/iser/iser_memory.c
@@ -199,7 +199,11 @@ iser_reg_dma(struct iser_device *device, struct iser_data_buf *mem,
 	 * FIXME: rework the registration code path to differentiate
 	 * rkey/lkey use cases
 	 */
-	reg->rkey = device->mr ? device->mr->rkey : 0;
+
+	if (device->pd->flags & IB_PD_UNSAFE_GLOBAL_RKEY)
+		reg->rkey = device->pd->unsafe_global_rkey;
+	else
+		reg->rkey = 0;
 	reg->sge.addr = ib_sg_dma_address(device->ib_device, &sg[0]);
 	reg->sge.length = ib_sg_dma_len(device->ib_device, &sg[0]);
 
diff --git a/drivers/infiniband/ulp/iser/iser_verbs.c b/drivers/infiniband/ulp/iser/iser_verbs.c
index 1b4945367e4f..a4b791dfaa1d 100644
--- a/drivers/infiniband/ulp/iser/iser_verbs.c
+++ b/drivers/infiniband/ulp/iser/iser_verbs.c
@@ -88,7 +88,8 @@ static int iser_create_device_ib_res(struct iser_device *device)
 		  device->comps_used, ib_dev->name,
 		  ib_dev->num_comp_vectors, max_cqe);
 
-	device->pd = ib_alloc_pd(ib_dev);
+	device->pd = ib_alloc_pd(ib_dev,
+		iser_always_reg ? 0 : IB_PD_UNSAFE_GLOBAL_RKEY);
 	if (IS_ERR(device->pd))
 		goto pd_err;
 
@@ -103,26 +104,13 @@ static int iser_create_device_ib_res(struct iser_device *device)
 		}
 	}
 
-	if (!iser_always_reg) {
-		int access = IB_ACCESS_LOCAL_WRITE |
-			     IB_ACCESS_REMOTE_WRITE |
-			     IB_ACCESS_REMOTE_READ;
-
-		device->mr = ib_get_dma_mr(device->pd, access);
-		if (IS_ERR(device->mr))
-			goto cq_err;
-	}
-
 	INIT_IB_EVENT_HANDLER(&device->event_handler, ib_dev,
 			      iser_event_handler);
 	if (ib_register_event_handler(&device->event_handler))
-		goto handler_err;
+		goto cq_err;
 
 	return 0;
 
-handler_err:
-	if (device->mr)
-		ib_dereg_mr(device->mr);
 cq_err:
 	for (i = 0; i < device->comps_used; i++) {
 		struct iser_comp *comp = &device->comps[i];
@@ -154,14 +142,10 @@ static void iser_free_device_ib_res(struct iser_device *device)
 	}
 
 	(void)ib_unregister_event_handler(&device->event_handler);
-	if (device->mr)
-		(void)ib_dereg_mr(device->mr);
 	ib_dealloc_pd(device->pd);
 
 	kfree(device->comps);
 	device->comps = NULL;
-
-	device->mr = NULL;
 	device->pd = NULL;
 }
 
diff --git a/drivers/infiniband/ulp/isert/ib_isert.c b/drivers/infiniband/ulp/isert/ib_isert.c
index cae9bbcc27e7..6dd43f63238e 100644
--- a/drivers/infiniband/ulp/isert/ib_isert.c
+++ b/drivers/infiniband/ulp/isert/ib_isert.c
@@ -309,7 +309,7 @@ isert_create_device_ib_res(struct isert_device *device)
 	if (ret)
 		goto out;
 
-	device->pd = ib_alloc_pd(ib_dev);
+	device->pd = ib_alloc_pd(ib_dev, 0);
 	if (IS_ERR(device->pd)) {
 		ret = PTR_ERR(device->pd);
 		isert_err("failed to allocate pd, device %p, ret=%d\n",
diff --git a/drivers/infiniband/ulp/srp/ib_srp.c b/drivers/infiniband/ulp/srp/ib_srp.c
index 3322ed750172..d980fb458ad4 100644
--- a/drivers/infiniband/ulp/srp/ib_srp.c
+++ b/drivers/infiniband/ulp/srp/ib_srp.c
@@ -1262,6 +1262,7 @@ static int srp_map_finish_fmr(struct srp_map_state *state,
 {
 	struct srp_target_port *target = ch->target;
 	struct srp_device *dev = target->srp_host->srp_dev;
+	struct ib_pd *pd = target->pd;
 	struct ib_pool_fmr *fmr;
 	u64 io_addr = 0;
 
@@ -1273,9 +1274,9 @@ static int srp_map_finish_fmr(struct srp_map_state *state,
 	if (state->npages == 0)
 		return 0;
 
-	if (state->npages == 1 && target->global_mr) {
+	if (state->npages == 1 && (pd->flags & IB_PD_UNSAFE_GLOBAL_RKEY)) {
 		srp_map_desc(state, state->base_dma_addr, state->dma_len,
-			     target->global_mr->rkey);
+			     pd->unsafe_global_rkey);
 		goto reset_state;
 	}
 
@@ -1315,6 +1316,7 @@ static int srp_map_finish_fr(struct srp_map_state *state,
 {
 	struct srp_target_port *target = ch->target;
 	struct srp_device *dev = target->srp_host->srp_dev;
+	struct ib_pd *pd = target->pd;
 	struct ib_send_wr *bad_wr;
 	struct ib_reg_wr wr;
 	struct srp_fr_desc *desc;
@@ -1326,12 +1328,12 @@ static int srp_map_finish_fr(struct srp_map_state *state,
 
 	WARN_ON_ONCE(!dev->use_fast_reg);
 
-	if (sg_nents == 1 && target->global_mr) {
+	if (sg_nents == 1 && (pd->flags & IB_PD_UNSAFE_GLOBAL_RKEY)) {
 		unsigned int sg_offset = sg_offset_p ? *sg_offset_p : 0;
 
 		srp_map_desc(state, sg_dma_address(state->sg) + sg_offset,
 			     sg_dma_len(state->sg) - sg_offset,
-			     target->global_mr->rkey);
+			     pd->unsafe_global_rkey);
 		if (sg_offset_p)
 			*sg_offset_p = 0;
 		return 1;
@@ -1386,7 +1388,7 @@ static int srp_map_finish_fr(struct srp_map_state *state,
 
 static int srp_map_sg_entry(struct srp_map_state *state,
 			    struct srp_rdma_ch *ch,
-			    struct scatterlist *sg, int sg_index)
+			    struct scatterlist *sg)
 {
 	struct srp_target_port *target = ch->target;
 	struct srp_device *dev = target->srp_host->srp_dev;
@@ -1400,7 +1402,9 @@ static int srp_map_sg_entry(struct srp_map_state *state,
 
 	while (dma_len) {
 		unsigned offset = dma_addr & ~dev->mr_page_mask;
-		if (state->npages == dev->max_pages_per_mr || offset != 0) {
+
+		if (state->npages == dev->max_pages_per_mr ||
+		    (state->npages > 0 && offset != 0)) {
 			ret = srp_map_finish_fmr(state, ch);
 			if (ret)
 				return ret;
@@ -1417,12 +1421,12 @@ static int srp_map_sg_entry(struct srp_map_state *state,
 	}
 
 	/*
-	 * If the last entry of the MR wasn't a full page, then we need to
+	 * If the end of the MR is not on a page boundary then we need to
 	 * close it out and start a new one -- we can only merge at page
 	 * boundaries.
 	 */
 	ret = 0;
-	if (len != dev->mr_page_size)
+	if ((dma_addr & ~dev->mr_page_mask) != 0)
 		ret = srp_map_finish_fmr(state, ch);
 	return ret;
 }
@@ -1439,7 +1443,7 @@ static int srp_map_sg_fmr(struct srp_map_state *state, struct srp_rdma_ch *ch,
 	state->fmr.end = req->fmr_list + ch->target->mr_per_cmd;
 
 	for_each_sg(scat, sg, count, i) {
-		ret = srp_map_sg_entry(state, ch, sg, i);
+		ret = srp_map_sg_entry(state, ch, sg);
 		if (ret)
 			return ret;
 	}
@@ -1491,7 +1495,7 @@ static int srp_map_sg_dma(struct srp_map_state *state, struct srp_rdma_ch *ch,
 	for_each_sg(scat, sg, count, i) {
 		srp_map_desc(state, ib_sg_dma_address(dev->dev, sg),
 			     ib_sg_dma_len(dev->dev, sg),
-			     target->global_mr->rkey);
+			     target->pd->unsafe_global_rkey);
 	}
 
 	return 0;
@@ -1591,6 +1595,7 @@ static int srp_map_data(struct scsi_cmnd *scmnd, struct srp_rdma_ch *ch,
 			struct srp_request *req)
 {
 	struct srp_target_port *target = ch->target;
+	struct ib_pd *pd = target->pd;
 	struct scatterlist *scat;
 	struct srp_cmd *cmd = req->cmd->buf;
 	int len, nents, count, ret;
@@ -1626,7 +1631,7 @@ static int srp_map_data(struct scsi_cmnd *scmnd, struct srp_rdma_ch *ch,
 	fmt = SRP_DATA_DESC_DIRECT;
 	len = sizeof (struct srp_cmd) +	sizeof (struct srp_direct_buf);
 
-	if (count == 1 && target->global_mr) {
+	if (count == 1 && (pd->flags & IB_PD_UNSAFE_GLOBAL_RKEY)) {
 		/*
 		 * The midlayer only generated a single gather/scatter
 		 * entry, or DMA mapping coalesced everything to a
@@ -1636,7 +1641,7 @@ static int srp_map_data(struct scsi_cmnd *scmnd, struct srp_rdma_ch *ch,
 		struct srp_direct_buf *buf = (void *) cmd->add_data;
 
 		buf->va  = cpu_to_be64(ib_sg_dma_address(ibdev, scat));
-		buf->key = cpu_to_be32(target->global_mr->rkey);
+		buf->key = cpu_to_be32(pd->unsafe_global_rkey);
 		buf->len = cpu_to_be32(ib_sg_dma_len(ibdev, scat));
 
 		req->nmdesc = 0;
@@ -1709,14 +1714,14 @@ static int srp_map_data(struct scsi_cmnd *scmnd, struct srp_rdma_ch *ch,
 	memcpy(indirect_hdr->desc_list, req->indirect_desc,
 	       count * sizeof (struct srp_direct_buf));
 
-	if (!target->global_mr) {
+	if (!(pd->flags & IB_PD_UNSAFE_GLOBAL_RKEY)) {
 		ret = srp_map_idb(ch, req, state.gen.next, state.gen.end,
 				  idb_len, &idb_rkey);
 		if (ret < 0)
 			goto unmap;
 		req->nmdesc++;
 	} else {
-		idb_rkey = cpu_to_be32(target->global_mr->rkey);
+		idb_rkey = cpu_to_be32(pd->unsafe_global_rkey);
 	}
 
 	indirect_hdr->table_desc.va = cpu_to_be64(req->indirect_dma_addr);
@@ -3268,8 +3273,8 @@ static ssize_t srp_create_target(struct device *dev,
 	target->io_class	= SRP_REV16A_IB_IO_CLASS;
 	target->scsi_host	= target_host;
 	target->srp_host	= host;
+	target->pd		= host->srp_dev->pd;
 	target->lkey		= host->srp_dev->pd->local_dma_lkey;
-	target->global_mr	= host->srp_dev->global_mr;
 	target->cmd_sg_cnt	= cmd_sg_entries;
 	target->sg_tablesize	= indirect_sg_entries ? : cmd_sg_entries;
 	target->allow_ext_sg	= allow_ext_sg;
@@ -3524,6 +3529,7 @@ static void srp_add_one(struct ib_device *device)
 	struct srp_host *host;
 	int mr_page_shift, p;
 	u64 max_pages_per_mr;
+	unsigned int flags = 0;
 
 	srp_dev = kzalloc(sizeof(*srp_dev), GFP_KERNEL);
 	if (!srp_dev)
@@ -3558,6 +3564,10 @@ static void srp_add_one(struct ib_device *device)
 		srp_dev->use_fmr = !srp_dev->use_fast_reg && srp_dev->has_fmr;
 	}
 
+	if (never_register || !register_always ||
+	    (!srp_dev->has_fmr && !srp_dev->has_fr))
+		flags |= IB_PD_UNSAFE_GLOBAL_RKEY;
+
 	if (srp_dev->use_fast_reg) {
 		srp_dev->max_pages_per_mr =
 			min_t(u32, srp_dev->max_pages_per_mr,
@@ -3573,19 +3583,10 @@ static void srp_add_one(struct ib_device *device)
 	INIT_LIST_HEAD(&srp_dev->dev_list);
 
 	srp_dev->dev = device;
-	srp_dev->pd  = ib_alloc_pd(device);
+	srp_dev->pd  = ib_alloc_pd(device, flags);
 	if (IS_ERR(srp_dev->pd))
 		goto free_dev;
 
-	if (never_register || !register_always ||
-	    (!srp_dev->has_fmr && !srp_dev->has_fr)) {
-		srp_dev->global_mr = ib_get_dma_mr(srp_dev->pd,
-						   IB_ACCESS_LOCAL_WRITE |
-						   IB_ACCESS_REMOTE_READ |
-						   IB_ACCESS_REMOTE_WRITE);
-		if (IS_ERR(srp_dev->global_mr))
-			goto err_pd;
-	}
 
 	for (p = rdma_start_port(device); p <= rdma_end_port(device); ++p) {
 		host = srp_add_port(srp_dev, p);
@@ -3596,9 +3597,6 @@ static void srp_add_one(struct ib_device *device)
 	ib_set_client_data(device, &srp_client, srp_dev);
 	return;
 
-err_pd:
-	ib_dealloc_pd(srp_dev->pd);
-
 free_dev:
 	kfree(srp_dev);
 }
@@ -3638,8 +3636,6 @@ static void srp_remove_one(struct ib_device *device, void *client_data)
 		kfree(host);
 	}
 
-	if (srp_dev->global_mr)
-		ib_dereg_mr(srp_dev->global_mr);
 	ib_dealloc_pd(srp_dev->pd);
 
 	kfree(srp_dev);
diff --git a/drivers/infiniband/ulp/srp/ib_srp.h b/drivers/infiniband/ulp/srp/ib_srp.h
index 26bb9b0a7a63..21c69695f9d4 100644
--- a/drivers/infiniband/ulp/srp/ib_srp.h
+++ b/drivers/infiniband/ulp/srp/ib_srp.h
@@ -90,7 +90,6 @@ struct srp_device {
 	struct list_head	dev_list;
 	struct ib_device       *dev;
 	struct ib_pd	       *pd;
-	struct ib_mr	       *global_mr;
 	u64			mr_page_mask;
 	int			mr_page_size;
 	int			mr_max_size;
@@ -179,7 +178,7 @@ struct srp_target_port {
 	spinlock_t		lock;
 
 	/* read only in the hot path */
-	struct ib_mr		*global_mr;
+	struct ib_pd		*pd;
 	struct srp_rdma_ch	*ch;
 	u32			ch_count;
 	u32			lkey;
diff --git a/drivers/infiniband/ulp/srpt/ib_srpt.c b/drivers/infiniband/ulp/srpt/ib_srpt.c
index 883bbfe08e0e..0b1f69ed2e92 100644
--- a/drivers/infiniband/ulp/srpt/ib_srpt.c
+++ b/drivers/infiniband/ulp/srpt/ib_srpt.c
@@ -2480,7 +2480,7 @@ static void srpt_add_one(struct ib_device *device)
 	init_waitqueue_head(&sdev->ch_releaseQ);
 	mutex_init(&sdev->mutex);
 
-	sdev->pd = ib_alloc_pd(device);
+	sdev->pd = ib_alloc_pd(device, 0);
 	if (IS_ERR(sdev->pd))
 		goto free_dev;
 
diff --git a/drivers/input/joydev.c b/drivers/input/joydev.c
index 5d11fea3c8ec..f3135ae22df4 100644
--- a/drivers/input/joydev.c
+++ b/drivers/input/joydev.c
@@ -950,6 +950,12 @@ static const struct input_device_id joydev_ids[] = {
 		.flags = INPUT_DEVICE_ID_MATCH_EVBIT |
 				INPUT_DEVICE_ID_MATCH_ABSBIT,
 		.evbit = { BIT_MASK(EV_ABS) },
+		.absbit = { BIT_MASK(ABS_Z) },
+	},
+	{
+		.flags = INPUT_DEVICE_ID_MATCH_EVBIT |
+				INPUT_DEVICE_ID_MATCH_ABSBIT,
+		.evbit = { BIT_MASK(EV_ABS) },
 		.absbit = { BIT_MASK(ABS_WHEEL) },
 	},
 	{
diff --git a/drivers/input/keyboard/Kconfig b/drivers/input/keyboard/Kconfig
index 509608c95994..cbd75cf44739 100644
--- a/drivers/input/keyboard/Kconfig
+++ b/drivers/input/keyboard/Kconfig
@@ -12,6 +12,21 @@ menuconfig INPUT_KEYBOARD
 
 if INPUT_KEYBOARD
 
+config KEYBOARD_ADC
+	tristate "ADC Ladder Buttons"
+	depends on IIO
+	select INPUT_POLLDEV
+	help
+	  This driver implements support for buttons connected
+	  to an ADC using a resistor ladder.
+
+	  Say Y here if your device has such buttons connected to an ADC.  Your
+	  board-specific setup logic must also provide a configuration data
+	  for mapping voltages to buttons.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called adc_keys.
+
 config KEYBOARD_ADP5520
 	tristate "Keypad Support for ADP5520 PMIC"
 	depends on PMIC_ADP5520
diff --git a/drivers/input/keyboard/Makefile b/drivers/input/keyboard/Makefile
index 1d416ddf84e4..d9f4cfcf3410 100644
--- a/drivers/input/keyboard/Makefile
+++ b/drivers/input/keyboard/Makefile
@@ -4,6 +4,7 @@
 
 # Each configuration option enables a list of files.
 
+obj-$(CONFIG_KEYBOARD_ADC)		+= adc-keys.o
 obj-$(CONFIG_KEYBOARD_ADP5520)		+= adp5520-keys.o
 obj-$(CONFIG_KEYBOARD_ADP5588)		+= adp5588-keys.o
 obj-$(CONFIG_KEYBOARD_ADP5589)		+= adp5589-keys.o
diff --git a/drivers/input/keyboard/adc-keys.c b/drivers/input/keyboard/adc-keys.c
new file mode 100644
index 000000000000..f8cf2ccacefd
--- /dev/null
+++ b/drivers/input/keyboard/adc-keys.c
@@ -0,0 +1,210 @@
+/*
+ * Input driver for resistor ladder connected on ADC
+ *
+ * Copyright (c) 2016 Alexandre Belloni
+ *
+ * This program is free software; you can 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/iio/consumer.h>
+#include <linux/iio/types.h>
+#include <linux/input.h>
+#include <linux/input-polldev.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/property.h>
+#include <linux/slab.h>
+
+struct adc_keys_button {
+	u32 voltage;
+	u32 keycode;
+};
+
+struct adc_keys_state {
+	struct iio_channel *channel;
+	u32 num_keys;
+	u32 last_key;
+	u32 keyup_voltage;
+	const struct adc_keys_button *map;
+};
+
+static void adc_keys_poll(struct input_polled_dev *dev)
+{
+	struct adc_keys_state *st = dev->private;
+	int i, value, ret;
+	u32 diff, closest = 0xffffffff;
+	int keycode = 0;
+
+	ret = iio_read_channel_processed(st->channel, &value);
+	if (unlikely(ret < 0)) {
+		/* Forcibly release key if any was pressed */
+		value = st->keyup_voltage;
+	} else {
+		for (i = 0; i < st->num_keys; i++) {
+			diff = abs(st->map[i].voltage - value);
+			if (diff < closest) {
+				closest = diff;
+				keycode = st->map[i].keycode;
+			}
+		}
+	}
+
+	if (abs(st->keyup_voltage - value) < closest)
+		keycode = 0;
+
+	if (st->last_key && st->last_key != keycode)
+		input_report_key(dev->input, st->last_key, 0);
+
+	if (keycode)
+		input_report_key(dev->input, keycode, 1);
+
+	input_sync(dev->input);
+	st->last_key = keycode;
+}
+
+static int adc_keys_load_keymap(struct device *dev, struct adc_keys_state *st)
+{
+	struct adc_keys_button *map;
+	struct fwnode_handle *child;
+	int i;
+
+	st->num_keys = device_get_child_node_count(dev);
+	if (st->num_keys == 0) {
+		dev_err(dev, "keymap is missing\n");
+		return -EINVAL;
+	}
+
+	map = devm_kmalloc_array(dev, st->num_keys, sizeof(*map), GFP_KERNEL);
+	if (!map)
+		return -ENOMEM;
+
+	i = 0;
+	device_for_each_child_node(dev, child) {
+		if (fwnode_property_read_u32(child, "press-threshold-microvolt",
+					     &map[i].voltage)) {
+			dev_err(dev, "Key with invalid or missing voltage\n");
+			fwnode_handle_put(child);
+			return -EINVAL;
+		}
+		map[i].voltage /= 1000;
+
+		if (fwnode_property_read_u32(child, "linux,code",
+					     &map[i].keycode)) {
+			dev_err(dev, "Key with invalid or missing linux,code\n");
+			fwnode_handle_put(child);
+			return -EINVAL;
+		}
+
+		i++;
+	}
+
+	st->map = map;
+	return 0;
+}
+
+static int adc_keys_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct adc_keys_state *st;
+	struct input_polled_dev *poll_dev;
+	struct input_dev *input;
+	enum iio_chan_type type;
+	int i, value;
+	int error;
+
+	st = devm_kzalloc(dev, sizeof(*st), GFP_KERNEL);
+	if (!st)
+		return -ENOMEM;
+
+	st->channel = devm_iio_channel_get(dev, "buttons");
+	if (IS_ERR(st->channel))
+		return PTR_ERR(st->channel);
+
+	if (!st->channel->indio_dev)
+		return -ENXIO;
+
+	error = iio_get_channel_type(st->channel, &type);
+	if (error < 0)
+		return error;
+
+	if (type != IIO_VOLTAGE) {
+		dev_err(dev, "Incompatible channel type %d\n", type);
+		return -EINVAL;
+	}
+
+	if (device_property_read_u32(dev, "keyup-threshold-microvolt",
+				     &st->keyup_voltage)) {
+		dev_err(dev, "Invalid or missing keyup voltage\n");
+		return -EINVAL;
+	}
+	st->keyup_voltage /= 1000;
+
+	error = adc_keys_load_keymap(dev, st);
+	if (error)
+		return error;
+
+	platform_set_drvdata(pdev, st);
+
+	poll_dev = devm_input_allocate_polled_device(dev);
+	if (!poll_dev) {
+		dev_err(dev, "failed to allocate input device\n");
+		return -ENOMEM;
+	}
+
+	if (!device_property_read_u32(dev, "poll-interval", &value))
+		poll_dev->poll_interval = value;
+
+	poll_dev->poll = adc_keys_poll;
+	poll_dev->private = st;
+
+	input = poll_dev->input;
+
+	input->name = pdev->name;
+	input->phys = "adc-keys/input0";
+
+	input->id.bustype = BUS_HOST;
+	input->id.vendor = 0x0001;
+	input->id.product = 0x0001;
+	input->id.version = 0x0100;
+
+	__set_bit(EV_KEY, input->evbit);
+	for (i = 0; i < st->num_keys; i++)
+		__set_bit(st->map[i].keycode, input->keybit);
+
+	if (device_property_read_bool(dev, "autorepeat"))
+		__set_bit(EV_REP, input->evbit);
+
+	error = input_register_polled_device(poll_dev);
+	if (error) {
+		dev_err(dev, "Unable to register input device: %d\n", error);
+		return error;
+	}
+
+	return 0;
+}
+
+#ifdef CONFIG_OF
+static const struct of_device_id adc_keys_of_match[] = {
+	{ .compatible = "adc-keys", },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, adc_keys_of_match);
+#endif
+
+static struct platform_driver __refdata adc_keys_driver = {
+	.driver = {
+		.name = "adc_keys",
+		.of_match_table = of_match_ptr(adc_keys_of_match),
+	},
+	.probe = adc_keys_probe,
+};
+module_platform_driver(adc_keys_driver);
+
+MODULE_AUTHOR("Alexandre Belloni <alexandre.belloni@free-electrons.com>");
+MODULE_DESCRIPTION("Input driver for resistor ladder connected on ADC");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/input/keyboard/cros_ec_keyb.c b/drivers/input/keyboard/cros_ec_keyb.c
index 4b0878f35471..25943e9bc8bf 100644
--- a/drivers/input/keyboard/cros_ec_keyb.c
+++ b/drivers/input/keyboard/cros_ec_keyb.c
@@ -27,6 +27,7 @@
 #include <linux/input.h>
 #include <linux/interrupt.h>
 #include <linux/kernel.h>
+#include <linux/notifier.h>
 #include <linux/platform_device.h>
 #include <linux/slab.h>
 #include <linux/input/matrix_keypad.h>
@@ -44,6 +45,7 @@
  * @dev: Device pointer
  * @idev: Input device
  * @ec: Top level ChromeOS device to use to talk to EC
+ * @notifier: interrupt event notifier for transport devices
  */
 struct cros_ec_keyb {
 	unsigned int rows;
@@ -57,6 +59,7 @@ struct cros_ec_keyb {
 	struct device *dev;
 	struct input_dev *idev;
 	struct cros_ec_device *ec;
+	struct notifier_block notifier;
 };
 
 
@@ -146,67 +149,44 @@ static void cros_ec_keyb_process(struct cros_ec_keyb *ckdev,
 	input_sync(ckdev->idev);
 }
 
-static int cros_ec_keyb_get_state(struct cros_ec_keyb *ckdev, uint8_t *kb_state)
-{
-	int ret = 0;
-	struct cros_ec_command *msg;
-
-	msg = kmalloc(sizeof(*msg) + ckdev->cols, GFP_KERNEL);
-	if (!msg)
-		return -ENOMEM;
-
-	msg->version = 0;
-	msg->command = EC_CMD_MKBP_STATE;
-	msg->insize = ckdev->cols;
-	msg->outsize = 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)
+static int cros_ec_keyb_open(struct input_dev *dev)
 {
-	struct cros_ec_keyb *ckdev = data;
-	struct cros_ec_device *ec = ckdev->ec;
-	int ret;
-	uint8_t kb_state[ckdev->cols];
-
-	if (device_may_wakeup(ec->dev))
-		pm_wakeup_event(ec->dev, 0);
-
-	ret = cros_ec_keyb_get_state(ckdev, kb_state);
-	if (ret >= 0)
-		cros_ec_keyb_process(ckdev, kb_state, ret);
-	else
-		dev_err(ckdev->dev, "failed to get keyboard state: %d\n", ret);
+	struct cros_ec_keyb *ckdev = input_get_drvdata(dev);
 
-	return IRQ_HANDLED;
+	return blocking_notifier_chain_register(&ckdev->ec->event_notifier,
+						&ckdev->notifier);
 }
 
-static int cros_ec_keyb_open(struct input_dev *dev)
+static void cros_ec_keyb_close(struct input_dev *dev)
 {
 	struct cros_ec_keyb *ckdev = input_get_drvdata(dev);
-	struct cros_ec_device *ec = ckdev->ec;
 
-	return request_threaded_irq(ec->irq, NULL, cros_ec_keyb_irq,
-					IRQF_TRIGGER_LOW | IRQF_ONESHOT,
-					"cros_ec_keyb", ckdev);
+	blocking_notifier_chain_unregister(&ckdev->ec->event_notifier,
+					   &ckdev->notifier);
 }
 
-static void cros_ec_keyb_close(struct input_dev *dev)
+static int cros_ec_keyb_work(struct notifier_block *nb,
+			     unsigned long queued_during_suspend, void *_notify)
 {
-	struct cros_ec_keyb *ckdev = input_get_drvdata(dev);
-	struct cros_ec_device *ec = ckdev->ec;
+	struct cros_ec_keyb *ckdev = container_of(nb, struct cros_ec_keyb,
+						  notifier);
 
-	free_irq(ec->irq, ckdev);
+	if (ckdev->ec->event_data.event_type != EC_MKBP_EVENT_KEY_MATRIX)
+		return NOTIFY_DONE;
+	/*
+	 * If EC is not the wake source, discard key state changes during
+	 * suspend.
+	 */
+	if (queued_during_suspend)
+		return NOTIFY_OK;
+	if (ckdev->ec->event_size != ckdev->cols) {
+		dev_err(ckdev->dev,
+			"Discarded incomplete key matrix event.\n");
+		return NOTIFY_OK;
+	}
+	cros_ec_keyb_process(ckdev, ckdev->ec->event_data.data.key_matrix,
+			     ckdev->ec->event_size);
+	return NOTIFY_OK;
 }
 
 /*
@@ -265,12 +245,8 @@ static int cros_ec_keyb_probe(struct platform_device *pdev)
 	if (!idev)
 		return -ENOMEM;
 
-	if (!ec->irq) {
-		dev_err(dev, "no EC IRQ specified\n");
-		return -EINVAL;
-	}
-
 	ckdev->ec = ec;
+	ckdev->notifier.notifier_call = cros_ec_keyb_work;
 	ckdev->dev = dev;
 	dev_set_drvdata(dev, ckdev);
 
@@ -311,54 +287,6 @@ static int cros_ec_keyb_probe(struct platform_device *pdev)
 	return 0;
 }
 
-#ifdef CONFIG_PM_SLEEP
-/* Clear any keys in the buffer */
-static void cros_ec_keyb_clear_keyboard(struct cros_ec_keyb *ckdev)
-{
-	uint8_t old_state[ckdev->cols];
-	uint8_t new_state[ckdev->cols];
-	unsigned long duration;
-	int i, ret;
-
-	/*
-	 * Keep reading until we see that the scan state does not change.
-	 * That indicates that we are done.
-	 *
-	 * Assume that the EC keyscan buffer is at most 32 deep.
-	 */
-	duration = jiffies;
-	ret = cros_ec_keyb_get_state(ckdev, new_state);
-	for (i = 1; !ret && i < 32; i++) {
-		memcpy(old_state, new_state, sizeof(old_state));
-		ret = cros_ec_keyb_get_state(ckdev, new_state);
-		if (0 == memcmp(old_state, new_state, sizeof(old_state)))
-			break;
-	}
-	duration = jiffies - duration;
-	dev_info(ckdev->dev, "Discarded %d keyscan(s) in %dus\n", i,
-		jiffies_to_usecs(duration));
-}
-
-static int cros_ec_keyb_resume(struct device *dev)
-{
-	struct cros_ec_keyb *ckdev = dev_get_drvdata(dev);
-
-	/*
-	 * When the EC is not a wake source, then it could not have caused the
-	 * resume, so we clear the EC's key scan buffer. If the EC was a
-	 * wake source (e.g. the lid is open and the user might press a key to
-	 * wake) then the key scan buffer should be preserved.
-	 */
-	if (!ckdev->ec->was_wake_device)
-		cros_ec_keyb_clear_keyboard(ckdev);
-
-	return 0;
-}
-
-#endif
-
-static SIMPLE_DEV_PM_OPS(cros_ec_keyb_pm_ops, NULL, cros_ec_keyb_resume);
-
 #ifdef CONFIG_OF
 static const struct of_device_id cros_ec_keyb_of_match[] = {
 	{ .compatible = "google,cros-ec-keyb" },
@@ -372,7 +300,6 @@ static struct platform_driver cros_ec_keyb_driver = {
 	.driver = {
 		.name = "cros-ec-keyb",
 		.of_match_table = of_match_ptr(cros_ec_keyb_of_match),
-		.pm	= &cros_ec_keyb_pm_ops,
 	},
 };
 
diff --git a/drivers/input/keyboard/jornada720_kbd.c b/drivers/input/keyboard/jornada720_kbd.c
index 421d9c55b0e8..1277c39f9482 100644
--- a/drivers/input/keyboard/jornada720_kbd.c
+++ b/drivers/input/keyboard/jornada720_kbd.c
@@ -25,8 +25,6 @@
 #include <linux/slab.h>
 
 #include <mach/jornada720.h>
-#include <mach/hardware.h>
-#include <mach/irqs.h>
 
 MODULE_AUTHOR("Kristoffer Ericson <Kristoffer.Ericson@gmail.com>");
 MODULE_DESCRIPTION("HP Jornada 710/720/728 keyboard driver");
@@ -66,10 +64,8 @@ static irqreturn_t jornada720_kbd_interrupt(int irq, void *dev_id)
 	jornada_ssp_start();
 
 	if (jornada_ssp_inout(GETSCANKEYCODE) != TXDUMMY) {
-		printk(KERN_DEBUG
-			"jornada720_kbd: "
-			"GetKeycode command failed with ETIMEDOUT, "
-			"flushed bus\n");
+		dev_dbg(&pdev->dev,
+			"GetKeycode command failed with ETIMEDOUT, flushed bus\n");
 	} else {
 		/* How many keycodes are waiting for us? */
 		count = jornada_ssp_byte(TXDUMMY);
@@ -97,14 +93,16 @@ static int jornada720_kbd_probe(struct platform_device *pdev)
 {
 	struct jornadakbd *jornadakbd;
 	struct input_dev *input_dev;
-	int i, err;
+	int i, err, irq;
 
-	jornadakbd = kzalloc(sizeof(struct jornadakbd), GFP_KERNEL);
-	input_dev = input_allocate_device();
-	if (!jornadakbd || !input_dev) {
-		err = -ENOMEM;
-		goto fail1;
-	}
+	irq = platform_get_irq(pdev, 0);
+	if (irq <= 0)
+		return irq < 0 ? irq : -EINVAL;
+
+	jornadakbd = devm_kzalloc(&pdev->dev, sizeof(*jornadakbd), GFP_KERNEL);
+	input_dev = devm_input_allocate_device(&pdev->dev);
+	if (!jornadakbd || !input_dev)
+		return -ENOMEM;
 
 	platform_set_drvdata(pdev, jornadakbd);
 
@@ -127,40 +125,16 @@ static int jornada720_kbd_probe(struct platform_device *pdev)
 
 	input_set_capability(input_dev, EV_MSC, MSC_SCAN);
 
-	err = request_irq(IRQ_GPIO0,
-			  jornada720_kbd_interrupt,
-			  IRQF_TRIGGER_FALLING,
-			  "jornadakbd", pdev);
+	err = devm_request_irq(&pdev->dev, irq, jornada720_kbd_interrupt,
+			       IRQF_TRIGGER_FALLING, "jornadakbd", pdev);
 	if (err) {
-		printk(KERN_INFO "jornadakbd720_kbd: Unable to grab IRQ\n");
-		goto fail1;
+		dev_err(&pdev->dev, "unable to grab IRQ%d: %d\n", irq, err);
+		return err;
 	}
 
-	err = input_register_device(jornadakbd->input);
-	if (err)
-		goto fail2;
-
-	return 0;
-
- fail2:	/* IRQ, DEVICE, MEMORY */
-	free_irq(IRQ_GPIO0, pdev);
- fail1:	/* DEVICE, MEMORY */
-	input_free_device(input_dev);
-	kfree(jornadakbd);
-	return err;
+	return input_register_device(jornadakbd->input);
 };
 
-static int jornada720_kbd_remove(struct platform_device *pdev)
-{
-	struct jornadakbd *jornadakbd = platform_get_drvdata(pdev);
-
-	free_irq(IRQ_GPIO0, pdev);
-	input_unregister_device(jornadakbd->input);
-	kfree(jornadakbd);
-
-	return 0;
-}
-
 /* work with hotplug and coldplug */
 MODULE_ALIAS("platform:jornada720_kbd");
 
@@ -169,6 +143,5 @@ static struct platform_driver jornada720_kbd_driver = {
 		.name    = "jornada720_kbd",
 	 },
 	.probe   = jornada720_kbd_probe,
-	.remove  = jornada720_kbd_remove,
 };
 module_platform_driver(jornada720_kbd_driver);
diff --git a/drivers/input/keyboard/snvs_pwrkey.c b/drivers/input/keyboard/snvs_pwrkey.c
index 24a9f599082f..7544888c4749 100644
--- a/drivers/input/keyboard/snvs_pwrkey.c
+++ b/drivers/input/keyboard/snvs_pwrkey.c
@@ -168,7 +168,6 @@ static int imx_snvs_pwrkey_probe(struct platform_device *pdev)
 	error = input_register_device(input);
 	if (error < 0) {
 		dev_err(&pdev->dev, "failed to register input device\n");
-		input_free_device(input);
 		return error;
 	}
 
diff --git a/drivers/input/misc/Kconfig b/drivers/input/misc/Kconfig
index efb0ca871327..7ffb614ce566 100644
--- a/drivers/input/misc/Kconfig
+++ b/drivers/input/misc/Kconfig
@@ -292,6 +292,18 @@ config INPUT_GPIO_TILT_POLLED
 	  To compile this driver as a module, choose M here: the
 	  module will be called gpio_tilt_polled.
 
+config INPUT_GPIO_DECODER
+	tristate "Polled GPIO Decoder Input driver"
+	depends on GPIOLIB || COMPILE_TEST
+	select INPUT_POLLDEV
+	help
+	 Say Y here if you want driver to read status of multiple GPIO
+	 lines and report the encoded value as an absolute integer to
+	 input subsystem.
+
+	 To compile this driver as a module, choose M here: the module
+	 will be called gpio_decoder.
+
 config INPUT_IXP4XX_BEEPER
 	tristate "IXP4XX Beeper support"
 	depends on ARCH_IXP4XX
@@ -454,10 +466,10 @@ config INPUT_RETU_PWRBUTTON
 
 config INPUT_TPS65218_PWRBUTTON
 	tristate "TPS65218 Power button driver"
-	depends on MFD_TPS65218
+	depends on (MFD_TPS65217 || MFD_TPS65218)
 	help
 	  Say Y here if you want to enable power buttong reporting for
-	  the TPS65218 Power Management IC device.
+	  TPS65217 and TPS65218 Power Management IC devices.
 
 	  To compile this driver as a module, choose M here. The module will
 	  be called tps65218-pwrbutton.
diff --git a/drivers/input/misc/Makefile b/drivers/input/misc/Makefile
index 6a1e5e20fc1c..0b6d025f0487 100644
--- a/drivers/input/misc/Makefile
+++ b/drivers/input/misc/Makefile
@@ -35,6 +35,7 @@ obj-$(CONFIG_INPUT_DRV2667_HAPTICS)	+= drv2667.o
 obj-$(CONFIG_INPUT_GP2A)		+= gp2ap002a00f.o
 obj-$(CONFIG_INPUT_GPIO_BEEPER)		+= gpio-beeper.o
 obj-$(CONFIG_INPUT_GPIO_TILT_POLLED)	+= gpio_tilt_polled.o
+obj-$(CONFIG_INPUT_GPIO_DECODER)	+= gpio_decoder.o
 obj-$(CONFIG_INPUT_HISI_POWERKEY)	+= hisi_powerkey.o
 obj-$(CONFIG_HP_SDC_RTC)		+= hp_sdc_rtc.o
 obj-$(CONFIG_INPUT_IMS_PCU)		+= ims-pcu.o
diff --git a/drivers/input/misc/gpio_decoder.c b/drivers/input/misc/gpio_decoder.c
new file mode 100644
index 000000000000..ca7e0bacb2d8
--- /dev/null
+++ b/drivers/input/misc/gpio_decoder.c
@@ -0,0 +1,137 @@
+/*
+ * Copyright (C) 2016 Texas Instruments Incorporated - http://www.ti.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.
+ *
+ * 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.
+ *
+ * A generic driver to read multiple gpio lines and translate the
+ * encoded numeric value into an input event.
+ */
+
+#include <linux/device.h>
+#include <linux/gpio/consumer.h>
+#include <linux/input.h>
+#include <linux/input-polldev.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+
+struct gpio_decoder {
+	struct input_polled_dev *poll_dev;
+	struct gpio_descs *input_gpios;
+	struct device *dev;
+	u32 axis;
+	u32 last_stable;
+};
+
+static int gpio_decoder_get_gpios_state(struct gpio_decoder *decoder)
+{
+	struct gpio_descs *gpios = decoder->input_gpios;
+	unsigned int ret = 0;
+	int i, val;
+
+	for (i = 0; i < gpios->ndescs; i++) {
+		val = gpiod_get_value_cansleep(gpios->desc[i]);
+		if (val < 0) {
+			dev_err(decoder->dev,
+				"Error reading gpio %d: %d\n",
+				desc_to_gpio(gpios->desc[i]), val);
+			return val;
+		}
+
+		val = !!val;
+		ret = (ret << 1) | val;
+	}
+
+	return ret;
+}
+
+static void gpio_decoder_poll_gpios(struct input_polled_dev *poll_dev)
+{
+	struct gpio_decoder *decoder = poll_dev->private;
+	int state;
+
+	state = gpio_decoder_get_gpios_state(decoder);
+	if (state >= 0 && state != decoder->last_stable) {
+		input_report_abs(poll_dev->input, decoder->axis, state);
+		input_sync(poll_dev->input);
+		decoder->last_stable = state;
+	}
+}
+
+static int gpio_decoder_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct gpio_decoder *decoder;
+	struct input_polled_dev *poll_dev;
+	u32  max;
+	int err;
+
+	decoder = devm_kzalloc(dev, sizeof(struct gpio_decoder), GFP_KERNEL);
+	if (!decoder)
+		return -ENOMEM;
+
+	device_property_read_u32(dev, "linux,axis", &decoder->axis);
+	decoder->input_gpios = devm_gpiod_get_array(dev, NULL, GPIOD_IN);
+	if (IS_ERR(decoder->input_gpios)) {
+		dev_err(dev, "unable to acquire input gpios\n");
+		return PTR_ERR(decoder->input_gpios);
+	}
+	if (decoder->input_gpios->ndescs < 2) {
+		dev_err(dev, "not enough gpios found\n");
+		return -EINVAL;
+	}
+
+	if (device_property_read_u32(dev, "decoder-max-value", &max))
+		max = (1U << decoder->input_gpios->ndescs) - 1;
+
+	decoder->dev = dev;
+	poll_dev = devm_input_allocate_polled_device(decoder->dev);
+	if (!poll_dev)
+		return -ENOMEM;
+
+	poll_dev->private = decoder;
+	poll_dev->poll = gpio_decoder_poll_gpios;
+	decoder->poll_dev = poll_dev;
+
+	poll_dev->input->name = pdev->name;
+	poll_dev->input->id.bustype = BUS_HOST;
+	input_set_abs_params(poll_dev->input, decoder->axis, 0, max, 0, 0);
+
+	err = input_register_polled_device(poll_dev);
+	if (err) {
+		dev_err(dev, "failed to register polled  device\n");
+		return err;
+	}
+	platform_set_drvdata(pdev, decoder);
+
+	return 0;
+}
+
+#ifdef CONFIG_OF
+static const struct of_device_id gpio_decoder_of_match[] = {
+	{ .compatible = "gpio-decoder", },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, gpio_decoder_of_match);
+#endif
+
+static struct platform_driver gpio_decoder_driver = {
+	.probe		= gpio_decoder_probe,
+	.driver		= {
+		.name	= "gpio-decoder",
+		.of_match_table = of_match_ptr(gpio_decoder_of_match),
+	}
+};
+module_platform_driver(gpio_decoder_driver);
+
+MODULE_DESCRIPTION("GPIO decoder input driver");
+MODULE_AUTHOR("Vignesh R <vigneshr@ti.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/input/misc/max77693-haptic.c b/drivers/input/misc/max77693-haptic.c
index 29ddeb7be84b..46b0f48fbf49 100644
--- a/drivers/input/misc/max77693-haptic.c
+++ b/drivers/input/misc/max77693-haptic.c
@@ -3,7 +3,7 @@
  *
  * Copyright (C) 2014,2015 Samsung Electronics
  * Jaewon Kim <jaewon02.kim@samsung.com>
- * Krzysztof Kozlowski <k.kozlowski@samsung.com>
+ * Krzysztof Kozlowski <krzk@kernel.org>
  *
  * This program is not provided / owned by Maxim Integrated Products.
  *
@@ -426,7 +426,7 @@ static struct platform_driver max77693_haptic_driver = {
 module_platform_driver(max77693_haptic_driver);
 
 MODULE_AUTHOR("Jaewon Kim <jaewon02.kim@samsung.com>");
-MODULE_AUTHOR("Krzysztof Kozlowski <k.kozlowski@samsung.com>");
+MODULE_AUTHOR("Krzysztof Kozlowski <krzk@kernel.org>");
 MODULE_DESCRIPTION("MAXIM 77693/77843 Haptic driver");
 MODULE_ALIAS("platform:max77693-haptic");
 MODULE_LICENSE("GPL");
diff --git a/drivers/input/misc/tps65218-pwrbutton.c b/drivers/input/misc/tps65218-pwrbutton.c
index a39b62651a4b..3273217ce80c 100644
--- a/drivers/input/misc/tps65218-pwrbutton.c
+++ b/drivers/input/misc/tps65218-pwrbutton.c
@@ -1,8 +1,9 @@
 /*
- * Texas Instruments' TPS65218 Power Button Input Driver
+ * Texas Instruments' TPS65217 and TPS65218 Power Button Input Driver
  *
  * Copyright (C) 2014 Texas Instruments Incorporated - http://www.ti.com/
  * Author: Felipe Balbi <balbi@ti.com>
+ * Author: Marcin Niestroj <m.niestroj@grinn-global.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
@@ -18,31 +19,61 @@
 #include <linux/input.h>
 #include <linux/interrupt.h>
 #include <linux/kernel.h>
+#include <linux/mfd/tps65217.h>
 #include <linux/mfd/tps65218.h>
 #include <linux/module.h>
 #include <linux/of.h>
 #include <linux/platform_device.h>
+#include <linux/regmap.h>
 #include <linux/slab.h>
 
-struct tps65218_pwrbutton {
+struct tps6521x_data {
+	unsigned int reg_status;
+	unsigned int pb_mask;
+	const char *name;
+};
+
+static const struct tps6521x_data tps65217_data = {
+	.reg_status = TPS65217_REG_STATUS,
+	.pb_mask = TPS65217_STATUS_PB,
+	.name = "tps65217_pwrbutton",
+};
+
+static const struct tps6521x_data tps65218_data = {
+	.reg_status = TPS65218_REG_STATUS,
+	.pb_mask = TPS65218_STATUS_PB_STATE,
+	.name = "tps65218_pwrbutton",
+};
+
+struct tps6521x_pwrbutton {
 	struct device *dev;
-	struct tps65218 *tps;
+	struct regmap *regmap;
 	struct input_dev *idev;
+	const struct tps6521x_data *data;
+	char phys[32];
+};
+
+static const struct of_device_id of_tps6521x_pb_match[] = {
+	{ .compatible = "ti,tps65217-pwrbutton", .data = &tps65217_data },
+	{ .compatible = "ti,tps65218-pwrbutton", .data = &tps65218_data },
+	{ },
 };
+MODULE_DEVICE_TABLE(of, of_tps6521x_pb_match);
 
-static irqreturn_t tps65218_pwr_irq(int irq, void *_pwr)
+static irqreturn_t tps6521x_pb_irq(int irq, void *_pwr)
 {
-	struct tps65218_pwrbutton *pwr = _pwr;
+	struct tps6521x_pwrbutton *pwr = _pwr;
+	const struct tps6521x_data *tps_data = pwr->data;
 	unsigned int reg;
 	int error;
 
-	error = tps65218_reg_read(pwr->tps, TPS65218_REG_STATUS, &reg);
+	error = regmap_read(pwr->regmap, tps_data->reg_status, &reg);
 	if (error) {
 		dev_err(pwr->dev, "can't read register: %d\n", error);
 		goto out;
 	}
 
-	if (reg & TPS65218_STATUS_PB_STATE) {
+	if (reg & tps_data->pb_mask) {
 		input_report_key(pwr->idev, KEY_POWER, 1);
 		pm_wakeup_event(pwr->dev, 0);
 	} else {
@@ -55,42 +86,55 @@ out:
 	return IRQ_HANDLED;
 }
 
-static int tps65218_pwron_probe(struct platform_device *pdev)
+static int tps6521x_pb_probe(struct platform_device *pdev)
 {
-	struct tps65218 *tps = dev_get_drvdata(pdev->dev.parent);
 	struct device *dev = &pdev->dev;
-	struct tps65218_pwrbutton *pwr;
+	struct tps6521x_pwrbutton *pwr;
 	struct input_dev *idev;
+	const struct of_device_id *match;
 	int error;
 	int irq;
 
+	match = of_match_node(of_tps6521x_pb_match, pdev->dev.of_node);
+	if (!match)
+		return -ENXIO;
+
 	pwr = devm_kzalloc(dev, sizeof(*pwr), GFP_KERNEL);
 	if (!pwr)
 		return -ENOMEM;
 
+	pwr->data = match->data;
+
 	idev = devm_input_allocate_device(dev);
 	if (!idev)
 		return -ENOMEM;
 
-	idev->name = "tps65218_pwrbutton";
-	idev->phys = "tps65218_pwrbutton/input0";
+	idev->name = pwr->data->name;
+	snprintf(pwr->phys, sizeof(pwr->phys), "%s/input0",
+		pwr->data->name);
+	idev->phys = pwr->phys;
 	idev->dev.parent = dev;
 	idev->id.bustype = BUS_I2C;
 
 	input_set_capability(idev, EV_KEY, KEY_POWER);
 
-	pwr->tps = tps;
+	pwr->regmap = dev_get_regmap(pdev->dev.parent, NULL);
 	pwr->dev = dev;
 	pwr->idev = idev;
 	platform_set_drvdata(pdev, pwr);
 	device_init_wakeup(dev, true);
 
 	irq = platform_get_irq(pdev, 0);
-	error = devm_request_threaded_irq(dev, irq, NULL, tps65218_pwr_irq,
+	if (irq < 0) {
+		dev_err(dev, "No IRQ resource!\n");
+		return -EINVAL;
+	}
+
+	error = devm_request_threaded_irq(dev, irq, NULL, tps6521x_pb_irq,
 					  IRQF_TRIGGER_RISING |
 						IRQF_TRIGGER_FALLING |
 						IRQF_ONESHOT,
-					  "tps65218-pwrbutton", pwr);
+					  pwr->data->name, pwr);
 	if (error) {
 		dev_err(dev, "failed to request IRQ #%d: %d\n",
 			irq, error);
@@ -106,21 +150,15 @@ static int tps65218_pwron_probe(struct platform_device *pdev)
 	return 0;
 }
 
-static const struct of_device_id of_tps65218_pwr_match[] = {
-	{ .compatible = "ti,tps65218-pwrbutton" },
-	{ },
-};
-MODULE_DEVICE_TABLE(of, of_tps65218_pwr_match);
-
-static struct platform_driver tps65218_pwron_driver = {
-	.probe	= tps65218_pwron_probe,
+static struct platform_driver tps6521x_pb_driver = {
+	.probe	= tps6521x_pb_probe,
 	.driver	= {
-		.name	= "tps65218_pwrbutton",
-		.of_match_table = of_tps65218_pwr_match,
+		.name	= "tps6521x_pwrbutton",
+		.of_match_table = of_tps6521x_pb_match,
 	},
 };
-module_platform_driver(tps65218_pwron_driver);
+module_platform_driver(tps6521x_pb_driver);
 
-MODULE_DESCRIPTION("TPS65218 Power Button");
+MODULE_DESCRIPTION("TPS6521X Power Button");
 MODULE_LICENSE("GPL v2");
 MODULE_AUTHOR("Felipe Balbi <balbi@ti.com>");
diff --git a/drivers/input/misc/uinput.c b/drivers/input/misc/uinput.c
index 65ebbd111702..92595b98e7ed 100644
--- a/drivers/input/misc/uinput.c
+++ b/drivers/input/misc/uinput.c
@@ -1013,23 +1013,12 @@ static struct miscdevice uinput_misc = {
 	.minor		= UINPUT_MINOR,
 	.name		= UINPUT_NAME,
 };
+module_misc_device(uinput_misc);
+
 MODULE_ALIAS_MISCDEV(UINPUT_MINOR);
 MODULE_ALIAS("devname:" UINPUT_NAME);
 
-static int __init uinput_init(void)
-{
-	return misc_register(&uinput_misc);
-}
-
-static void __exit uinput_exit(void)
-{
-	misc_deregister(&uinput_misc);
-}
-
 MODULE_AUTHOR("Aristeu Sergio Rozanski Filho");
 MODULE_DESCRIPTION("User level driver support for input subsystem");
 MODULE_LICENSE("GPL");
 MODULE_VERSION("0.3");
-
-module_init(uinput_init);
-module_exit(uinput_exit);
diff --git a/drivers/input/mouse/alps.c b/drivers/input/mouse/alps.c
index 936f07a4e35f..6d7de9bfed9a 100644
--- a/drivers/input/mouse/alps.c
+++ b/drivers/input/mouse/alps.c
@@ -103,6 +103,7 @@ static const struct alps_nibble_commands alps_v6_nibble_commands[] = {
 					   6-byte ALPS packet */
 #define ALPS_STICK_BITS		0x100	/* separate stick button bits */
 #define ALPS_BUTTONPAD		0x200	/* device is a clickpad */
+#define ALPS_DUALPOINT_WITH_PRESSURE	0x400	/* device can report trackpoint pressure */
 
 static const struct alps_model_info alps_model_data[] = {
 	{ { 0x32, 0x02, 0x14 }, 0x00, { ALPS_PROTO_V2, 0xf8, 0xf8, ALPS_PASS | ALPS_DUALPOINT } },	/* Toshiba Salellite Pro M10 */
@@ -1156,15 +1157,28 @@ static unsigned char alps_get_pkt_id_ss4_v2(unsigned char *byte)
 {
 	unsigned char pkt_id = SS4_PACKET_ID_IDLE;
 
-	if (byte[0] == 0x18 && byte[1] == 0x10 && byte[2] == 0x00 &&
-	    (byte[3] & 0x88) == 0x08 && byte[4] == 0x10 && byte[5] == 0x00) {
-		pkt_id = SS4_PACKET_ID_IDLE;
-	} else if (!(byte[3] & 0x10)) {
-		pkt_id = SS4_PACKET_ID_ONE;
-	} else if (!(byte[3] & 0x20)) {
+	switch (byte[3] & 0x30) {
+	case 0x00:
+		if (byte[0] == 0x18 && byte[1] == 0x10 && byte[2] == 0x00 &&
+		    (byte[3] & 0x88) == 0x08 && byte[4] == 0x10 &&
+		    byte[5] == 0x00) {
+			pkt_id = SS4_PACKET_ID_IDLE;
+		} else {
+			pkt_id = SS4_PACKET_ID_ONE;
+		}
+		break;
+	case 0x10:
+		/* two-finger finger positions */
 		pkt_id = SS4_PACKET_ID_TWO;
-	} else {
+		break;
+	case 0x20:
+		/* stick pointer */
+		pkt_id = SS4_PACKET_ID_STICK;
+		break;
+	case 0x30:
+		/* third and fourth finger positions */
 		pkt_id = SS4_PACKET_ID_MULTI;
+		break;
 	}
 
 	return pkt_id;
@@ -1185,7 +1199,13 @@ static int alps_decode_ss4_v2(struct alps_fields *f,
 		f->mt[0].x = SS4_1F_X_V2(p);
 		f->mt[0].y = SS4_1F_Y_V2(p);
 		f->pressure = ((SS4_1F_Z_V2(p)) * 2) & 0x7f;
-		f->fingers = 1;
+		/*
+		 * When a button is held the device will give us events
+		 * with x, y, and pressure of 0. This causes annoying jumps
+		 * if a touch is released while the button is held.
+		 * Handle this by claiming zero contacts.
+		 */
+		f->fingers = f->pressure > 0 ? 1 : 0;
 		f->first_mp = 0;
 		f->is_mp = 0;
 		break;
@@ -1246,16 +1266,40 @@ static int alps_decode_ss4_v2(struct alps_fields *f,
 		}
 		break;
 
+	case SS4_PACKET_ID_STICK:
+		if (!(priv->flags & ALPS_DUALPOINT)) {
+			psmouse_warn(psmouse,
+				     "Rejected trackstick packet from non DualPoint device");
+		} else {
+			int x = (s8)(((p[0] & 1) << 7) | (p[1] & 0x7f));
+			int y = (s8)(((p[3] & 1) << 7) | (p[2] & 0x7f));
+			int pressure = (s8)(p[4] & 0x7f);
+
+			input_report_rel(priv->dev2, REL_X, x);
+			input_report_rel(priv->dev2, REL_Y, -y);
+			input_report_abs(priv->dev2, ABS_PRESSURE, pressure);
+		}
+		break;
+
 	case SS4_PACKET_ID_IDLE:
 	default:
 		memset(f, 0, sizeof(struct alps_fields));
 		break;
 	}
 
-	f->left = !!(SS4_BTN_V2(p) & 0x01);
-	if (!(priv->flags & ALPS_BUTTONPAD)) {
-		f->right = !!(SS4_BTN_V2(p) & 0x02);
-		f->middle = !!(SS4_BTN_V2(p) & 0x04);
+	/* handle buttons */
+	if (pkt_id == SS4_PACKET_ID_STICK) {
+		f->ts_left = !!(SS4_BTN_V2(p) & 0x01);
+		if (!(priv->flags & ALPS_BUTTONPAD)) {
+			f->ts_right = !!(SS4_BTN_V2(p) & 0x02);
+			f->ts_middle = !!(SS4_BTN_V2(p) & 0x04);
+		}
+	} else {
+		f->left = !!(SS4_BTN_V2(p) & 0x01);
+		if (!(priv->flags & ALPS_BUTTONPAD)) {
+			f->right = !!(SS4_BTN_V2(p) & 0x02);
+			f->middle = !!(SS4_BTN_V2(p) & 0x04);
+		}
 	}
 
 	return 0;
@@ -1266,6 +1310,7 @@ static void alps_process_packet_ss4_v2(struct psmouse *psmouse)
 	struct alps_data *priv = psmouse->private;
 	unsigned char *packet = psmouse->packet;
 	struct input_dev *dev = psmouse->dev;
+	struct input_dev *dev2 = priv->dev2;
 	struct alps_fields *f = &priv->f;
 
 	memset(f, 0, sizeof(struct alps_fields));
@@ -1311,6 +1356,13 @@ static void alps_process_packet_ss4_v2(struct psmouse *psmouse)
 
 	input_report_abs(dev, ABS_PRESSURE, f->pressure);
 	input_sync(dev);
+
+	if (priv->flags & ALPS_DUALPOINT) {
+		input_report_key(dev2, BTN_LEFT, f->ts_left);
+		input_report_key(dev2, BTN_RIGHT, f->ts_right);
+		input_report_key(dev2, BTN_MIDDLE, f->ts_middle);
+		input_sync(dev2);
+	}
 }
 
 static bool alps_is_valid_package_ss4_v2(struct psmouse *psmouse)
@@ -2695,6 +2747,10 @@ static int alps_set_protocol(struct psmouse *psmouse,
 		if (alps_set_defaults_ss4_v2(psmouse, priv))
 			return -EIO;
 
+		if (priv->fw_ver[1] == 0x1)
+			priv->flags |= ALPS_DUALPOINT |
+					ALPS_DUALPOINT_WITH_PRESSURE;
+
 		break;
 	}
 
@@ -2767,6 +2823,9 @@ static int alps_identify(struct psmouse *psmouse, struct alps_data *priv)
 		} else if (e7[0] == 0x73 && e7[1] == 0x03 &&
 			   e7[2] == 0x14 && ec[1] == 0x02) {
 			protocol = &alps_v8_protocol_data;
+		} else if (e7[0] == 0x73 && e7[1] == 0x03 &&
+			   e7[2] == 0x28 && ec[1] == 0x01) {
+			protocol = &alps_v8_protocol_data;
 		} else {
 			psmouse_dbg(psmouse,
 				    "Likely not an ALPS touchpad: E7=%3ph, EC=%3ph\n", e7, ec);
@@ -2949,6 +3008,10 @@ int alps_init(struct psmouse *psmouse)
 
 		input_set_capability(dev2, EV_REL, REL_X);
 		input_set_capability(dev2, EV_REL, REL_Y);
+		if (priv->flags & ALPS_DUALPOINT_WITH_PRESSURE) {
+			input_set_capability(dev2, EV_ABS, ABS_PRESSURE);
+			input_set_abs_params(dev2, ABS_PRESSURE, 0, 127, 0, 0);
+		}
 		input_set_capability(dev2, EV_KEY, BTN_LEFT);
 		input_set_capability(dev2, EV_KEY, BTN_RIGHT);
 		input_set_capability(dev2, EV_KEY, BTN_MIDDLE);
diff --git a/drivers/input/mouse/alps.h b/drivers/input/mouse/alps.h
index d37f814dc447..b9417e2d7ad3 100644
--- a/drivers/input/mouse/alps.h
+++ b/drivers/input/mouse/alps.h
@@ -37,12 +37,14 @@
  *  or there's button activities.
  * SS4_PACKET_ID_TWO: There's two or more fingers on touchpad
  * SS4_PACKET_ID_MULTI: There's three or more fingers on touchpad
+ * SS4_PACKET_ID_STICK: A stick pointer packet
 */
 enum SS4_PACKET_ID {
 	SS4_PACKET_ID_IDLE = 0,
 	SS4_PACKET_ID_ONE,
 	SS4_PACKET_ID_TWO,
 	SS4_PACKET_ID_MULTI,
+	SS4_PACKET_ID_STICK,
 };
 
 #define SS4_COUNT_PER_ELECTRODE		256
diff --git a/drivers/input/mouse/elan_i2c_smbus.c b/drivers/input/mouse/elan_i2c_smbus.c
index cb6aecbc1dc2..e23b2495d52e 100644
--- a/drivers/input/mouse/elan_i2c_smbus.c
+++ b/drivers/input/mouse/elan_i2c_smbus.c
@@ -222,11 +222,13 @@ static int elan_smbus_get_checksum(struct i2c_client *client,
 static int elan_smbus_get_max(struct i2c_client *client,
 			      unsigned int *max_x, unsigned int *max_y)
 {
+	int ret;
 	int error;
 	u8 val[3];
 
-	error = i2c_smbus_read_block_data(client, ETP_SMBUS_RANGE_CMD, val);
-	if (error) {
+	ret = i2c_smbus_read_block_data(client, ETP_SMBUS_RANGE_CMD, val);
+	if (ret != 3) {
+		error = ret < 0 ? ret : -EIO;
 		dev_err(&client->dev, "failed to get dimensions: %d\n", error);
 		return error;
 	}
@@ -240,12 +242,13 @@ static int elan_smbus_get_max(struct i2c_client *client,
 static int elan_smbus_get_resolution(struct i2c_client *client,
 				     u8 *hw_res_x, u8 *hw_res_y)
 {
+	int ret;
 	int error;
 	u8 val[3];
 
-	error = i2c_smbus_read_block_data(client,
-					  ETP_SMBUS_RESOLUTION_CMD, val);
-	if (error) {
+	ret = i2c_smbus_read_block_data(client, ETP_SMBUS_RESOLUTION_CMD, val);
+	if (ret != 3) {
+		error = ret < 0 ? ret : -EIO;
 		dev_err(&client->dev, "failed to get resolution: %d\n", error);
 		return error;
 	}
@@ -260,12 +263,13 @@ static int elan_smbus_get_num_traces(struct i2c_client *client,
 				     unsigned int *x_traces,
 				     unsigned int *y_traces)
 {
+	int ret;
 	int error;
 	u8 val[3];
 
-	error = i2c_smbus_read_block_data(client,
-					  ETP_SMBUS_XY_TRACENUM_CMD, val);
-	if (error) {
+	ret = i2c_smbus_read_block_data(client, ETP_SMBUS_XY_TRACENUM_CMD, val);
+	if (ret != 3) {
+		error = ret < 0 ? ret : -EIO;
 		dev_err(&client->dev, "failed to get trace info: %d\n", error);
 		return error;
 	}
diff --git a/drivers/input/mouse/elantech.c b/drivers/input/mouse/elantech.c
index 08e252a42480..db7d1d666ac1 100644
--- a/drivers/input/mouse/elantech.c
+++ b/drivers/input/mouse/elantech.c
@@ -1134,7 +1134,7 @@ static int elantech_get_resolution_v4(struct psmouse *psmouse,
  * System76 Pangolin       0x250f01        ?               2 hw buttons
  * (*) + 3 trackpoint buttons
  * (**) + 0 trackpoint buttons
- * Note: Lenovo L430 and Lenovo L430 have the same fw_version/caps
+ * Note: Lenovo L430 and Lenovo L530 have the same fw_version/caps
  */
 static void elantech_set_buttonpad_prop(struct psmouse *psmouse)
 {
@@ -1159,6 +1159,13 @@ static const struct dmi_system_id elantech_dmi_has_middle_button[] = {
 			DMI_MATCH(DMI_PRODUCT_NAME, "CELSIUS H730"),
 		},
 	},
+	{
+		/* Fujitsu H760 also has a middle button */
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "FUJITSU"),
+			DMI_MATCH(DMI_PRODUCT_NAME, "CELSIUS H760"),
+		},
+	},
 #endif
 	{ }
 };
@@ -1503,10 +1510,10 @@ static const struct dmi_system_id elantech_dmi_force_crc_enabled[] = {
 		},
 	},
 	{
-		/* Fujitsu LIFEBOOK E554  does not work with crc_enabled == 0 */
+		/* Fujitsu H760 does not work with crc_enabled == 0 */
 		.matches = {
 			DMI_MATCH(DMI_SYS_VENDOR, "FUJITSU"),
-			DMI_MATCH(DMI_PRODUCT_NAME, "LIFEBOOK E554"),
+			DMI_MATCH(DMI_PRODUCT_NAME, "CELSIUS H760"),
 		},
 	},
 	{
@@ -1517,6 +1524,20 @@ static const struct dmi_system_id elantech_dmi_force_crc_enabled[] = {
 		},
 	},
 	{
+		/* Fujitsu LIFEBOOK E554  does not work with crc_enabled == 0 */
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "FUJITSU"),
+			DMI_MATCH(DMI_PRODUCT_NAME, "LIFEBOOK E554"),
+		},
+	},
+	{
+		/* Fujitsu LIFEBOOK E556 does not work with crc_enabled == 0 */
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "FUJITSU"),
+			DMI_MATCH(DMI_PRODUCT_NAME, "LIFEBOOK E556"),
+		},
+	},
+	{
 		/* Fujitsu LIFEBOOK U745 does not work with crc_enabled == 0 */
 		.matches = {
 			DMI_MATCH(DMI_SYS_VENDOR, "FUJITSU"),
diff --git a/drivers/input/mouse/focaltech.c b/drivers/input/mouse/focaltech.c
index c8c6a8cc329d..54eceb30ede5 100644
--- a/drivers/input/mouse/focaltech.c
+++ b/drivers/input/mouse/focaltech.c
@@ -390,7 +390,8 @@ static int focaltech_read_size(struct psmouse *psmouse)
 	return 0;
 }
 
-void focaltech_set_resolution(struct psmouse *psmouse, unsigned int resolution)
+static void focaltech_set_resolution(struct psmouse *psmouse,
+				     unsigned int resolution)
 {
 	/* not supported yet */
 }
diff --git a/drivers/input/mouse/psmouse-base.c b/drivers/input/mouse/psmouse-base.c
index 5784e20542a4..fb4b185dea96 100644
--- a/drivers/input/mouse/psmouse-base.c
+++ b/drivers/input/mouse/psmouse-base.c
@@ -1916,7 +1916,7 @@ static int __init psmouse_init(void)
 	synaptics_module_init();
 	hgpk_module_init();
 
-	kpsmoused_wq = create_singlethread_workqueue("kpsmoused");
+	kpsmoused_wq = alloc_ordered_workqueue("kpsmoused", 0);
 	if (!kpsmoused_wq) {
 		pr_err("failed to create kpsmoused workqueue\n");
 		return -ENOMEM;
diff --git a/drivers/input/rmi4/Kconfig b/drivers/input/rmi4/Kconfig
index f73df2495fed..4c8a55857e00 100644
--- a/drivers/input/rmi4/Kconfig
+++ b/drivers/input/rmi4/Kconfig
@@ -61,3 +61,14 @@ config RMI4_F30
 
 	  Function 30 provides GPIO and LED support for RMI4 devices. This
 	  includes support for buttons on TouchPads and ClickPads.
+
+config RMI4_F54
+	bool "RMI4 Function 54 (Analog diagnostics)"
+	depends on RMI4_CORE
+	depends on VIDEO_V4L2=y || (RMI4_CORE=m && VIDEO_V4L2=m)
+	select VIDEOBUF2_VMALLOC
+	help
+	  Say Y here if you want to add support for RMI4 function 54
+
+	  Function 54 provides access to various diagnostic features in certain
+	  RMI4 touch sensors.
diff --git a/drivers/input/rmi4/Makefile b/drivers/input/rmi4/Makefile
index 95c00a783992..0bafc8502c4b 100644
--- a/drivers/input/rmi4/Makefile
+++ b/drivers/input/rmi4/Makefile
@@ -7,6 +7,7 @@ rmi_core-$(CONFIG_RMI4_2D_SENSOR) += rmi_2d_sensor.o
 rmi_core-$(CONFIG_RMI4_F11) += rmi_f11.o
 rmi_core-$(CONFIG_RMI4_F12) += rmi_f12.o
 rmi_core-$(CONFIG_RMI4_F30) += rmi_f30.o
+rmi_core-$(CONFIG_RMI4_F54) += rmi_f54.o
 
 # Transports
 obj-$(CONFIG_RMI4_I2C) += rmi_i2c.o
diff --git a/drivers/input/rmi4/rmi_bus.c b/drivers/input/rmi4/rmi_bus.c
index a73580654c6b..ef8c747c35e7 100644
--- a/drivers/input/rmi4/rmi_bus.c
+++ b/drivers/input/rmi4/rmi_bus.c
@@ -9,7 +9,6 @@
 
 #include <linux/kernel.h>
 #include <linux/device.h>
-#include <linux/kconfig.h>
 #include <linux/list.h>
 #include <linux/pm.h>
 #include <linux/rmi.h>
@@ -312,6 +311,9 @@ static struct rmi_function_handler *fn_handlers[] = {
 #ifdef CONFIG_RMI4_F30
 	&rmi_f30_handler,
 #endif
+#ifdef CONFIG_RMI4_F54
+	&rmi_f54_handler,
+#endif
 };
 
 static void __rmi_unregister_function_handlers(int start_idx)
diff --git a/drivers/input/rmi4/rmi_driver.c b/drivers/input/rmi4/rmi_driver.c
index c83bce89028b..4a88312fbd25 100644
--- a/drivers/input/rmi4/rmi_driver.c
+++ b/drivers/input/rmi4/rmi_driver.c
@@ -17,7 +17,6 @@
 #include <linux/bitmap.h>
 #include <linux/delay.h>
 #include <linux/fs.h>
-#include <linux/kconfig.h>
 #include <linux/pm.h>
 #include <linux/slab.h>
 #include <linux/of.h>
diff --git a/drivers/input/rmi4/rmi_driver.h b/drivers/input/rmi4/rmi_driver.h
index 6e140fa3cce1..8dfbebe9bf86 100644
--- a/drivers/input/rmi4/rmi_driver.h
+++ b/drivers/input/rmi4/rmi_driver.h
@@ -102,4 +102,5 @@ extern struct rmi_function_handler rmi_f01_handler;
 extern struct rmi_function_handler rmi_f11_handler;
 extern struct rmi_function_handler rmi_f12_handler;
 extern struct rmi_function_handler rmi_f30_handler;
+extern struct rmi_function_handler rmi_f54_handler;
 #endif
diff --git a/drivers/input/rmi4/rmi_f01.c b/drivers/input/rmi4/rmi_f01.c
index fac81fc9bcf6..b5d2dfc23bad 100644
--- a/drivers/input/rmi4/rmi_f01.c
+++ b/drivers/input/rmi4/rmi_f01.c
@@ -8,7 +8,6 @@
  */
 
 #include <linux/kernel.h>
-#include <linux/kconfig.h>
 #include <linux/rmi.h>
 #include <linux/slab.h>
 #include <linux/uaccess.h>
diff --git a/drivers/input/rmi4/rmi_f11.c b/drivers/input/rmi4/rmi_f11.c
index 20c7134b3d3b..f798f427a46f 100644
--- a/drivers/input/rmi4/rmi_f11.c
+++ b/drivers/input/rmi4/rmi_f11.c
@@ -12,7 +12,6 @@
 #include <linux/device.h>
 #include <linux/input.h>
 #include <linux/input/mt.h>
-#include <linux/kconfig.h>
 #include <linux/rmi.h>
 #include <linux/slab.h>
 #include <linux/of.h>
diff --git a/drivers/input/rmi4/rmi_f54.c b/drivers/input/rmi4/rmi_f54.c
new file mode 100644
index 000000000000..cf805b960866
--- /dev/null
+++ b/drivers/input/rmi4/rmi_f54.c
@@ -0,0 +1,757 @@
+/*
+ * Copyright (c) 2012-2015 Synaptics Incorporated
+ * Copyright (C) 2016 Zodiac Inflight Innovations
+ *
+ * This program is free software; you can 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/rmi.h>
+#include <linux/input.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/videobuf2-v4l2.h>
+#include <media/videobuf2-vmalloc.h>
+#include "rmi_driver.h"
+
+#define F54_NAME		"rmi4_f54"
+
+/* F54 data offsets */
+#define F54_REPORT_DATA_OFFSET  3
+#define F54_FIFO_OFFSET         1
+#define F54_NUM_TX_OFFSET       1
+#define F54_NUM_RX_OFFSET       0
+
+/* F54 commands */
+#define F54_GET_REPORT          1
+#define F54_FORCE_CAL           2
+
+/* Fixed sizes of reports */
+#define F54_QUERY_LEN			27
+
+/* F54 capabilities */
+#define F54_CAP_BASELINE	(1 << 2)
+#define F54_CAP_IMAGE8		(1 << 3)
+#define F54_CAP_IMAGE16		(1 << 6)
+
+/**
+ * enum rmi_f54_report_type - RMI4 F54 report types
+ *
+ * @F54_8BIT_IMAGE:	Normalized 8-Bit Image Report. The capacitance variance
+ *			from baseline for each pixel.
+ *
+ * @F54_16BIT_IMAGE:	Normalized 16-Bit Image Report. The capacitance variance
+ *			from baseline for each pixel.
+ *
+ * @F54_RAW_16BIT_IMAGE:
+ *			Raw 16-Bit Image Report. The raw capacitance for each
+ *			pixel.
+ *
+ * @F54_TRUE_BASELINE:	True Baseline Report. The baseline capacitance for each
+ *			pixel.
+ *
+ * @F54_FULL_RAW_CAP:   Full Raw Capacitance Report. The raw capacitance with
+ *			low reference set to its minimum value and high
+ *			reference set to its maximum value.
+ *
+ * @F54_FULL_RAW_CAP_RX_OFFSET_REMOVED:
+ *			Full Raw Capacitance with Receiver Offset Removed
+ *			Report. Set Low reference to its minimum value and high
+ *			references to its maximum value, then report the raw
+ *			capacitance for each pixel.
+ */
+enum rmi_f54_report_type {
+	F54_REPORT_NONE = 0,
+	F54_8BIT_IMAGE = 1,
+	F54_16BIT_IMAGE = 2,
+	F54_RAW_16BIT_IMAGE = 3,
+	F54_TRUE_BASELINE = 9,
+	F54_FULL_RAW_CAP = 19,
+	F54_FULL_RAW_CAP_RX_OFFSET_REMOVED = 20,
+	F54_MAX_REPORT_TYPE,
+};
+
+const char *rmi_f54_report_type_names[] = {
+	[F54_REPORT_NONE]		= "Unknown",
+	[F54_8BIT_IMAGE]		= "Normalized 8-Bit Image",
+	[F54_16BIT_IMAGE]		= "Normalized 16-Bit Image",
+	[F54_RAW_16BIT_IMAGE]		= "Raw 16-Bit Image",
+	[F54_TRUE_BASELINE]		= "True Baseline",
+	[F54_FULL_RAW_CAP]		= "Full Raw Capacitance",
+	[F54_FULL_RAW_CAP_RX_OFFSET_REMOVED]
+					= "Full Raw Capacitance RX Offset Removed",
+};
+
+struct rmi_f54_reports {
+	int start;
+	int size;
+};
+
+struct f54_data {
+	struct rmi_function *fn;
+
+	u8 qry[F54_QUERY_LEN];
+	u8 num_rx_electrodes;
+	u8 num_tx_electrodes;
+	u8 capabilities;
+	u16 clock_rate;
+	u8 family;
+
+	enum rmi_f54_report_type report_type;
+	u8 *report_data;
+	int report_size;
+	struct rmi_f54_reports standard_report[2];
+
+	bool is_busy;
+	struct mutex status_mutex;
+	struct mutex data_mutex;
+
+	struct workqueue_struct *workqueue;
+	struct delayed_work work;
+	unsigned long timeout;
+
+	struct completion cmd_done;
+
+	/* V4L2 support */
+	struct v4l2_device v4l2;
+	struct v4l2_pix_format format;
+	struct video_device vdev;
+	struct vb2_queue queue;
+	struct mutex lock;
+	int input;
+	enum rmi_f54_report_type inputs[F54_MAX_REPORT_TYPE];
+};
+
+/*
+ * Basic checks on report_type to ensure we write a valid type
+ * to the sensor.
+ */
+static bool is_f54_report_type_valid(struct f54_data *f54,
+				     enum rmi_f54_report_type reptype)
+{
+	switch (reptype) {
+	case F54_8BIT_IMAGE:
+		return f54->capabilities & F54_CAP_IMAGE8;
+	case F54_16BIT_IMAGE:
+	case F54_RAW_16BIT_IMAGE:
+		return f54->capabilities & F54_CAP_IMAGE16;
+	case F54_TRUE_BASELINE:
+		return f54->capabilities & F54_CAP_IMAGE16;
+	case F54_FULL_RAW_CAP:
+	case F54_FULL_RAW_CAP_RX_OFFSET_REMOVED:
+		return true;
+	default:
+		return false;
+	}
+}
+
+static enum rmi_f54_report_type rmi_f54_get_reptype(struct f54_data *f54,
+						unsigned int i)
+{
+	if (i >= F54_MAX_REPORT_TYPE)
+		return F54_REPORT_NONE;
+
+	return f54->inputs[i];
+}
+
+static void rmi_f54_create_input_map(struct f54_data *f54)
+{
+	int i = 0;
+	enum rmi_f54_report_type reptype;
+
+	for (reptype = 1; reptype < F54_MAX_REPORT_TYPE; reptype++) {
+		if (!is_f54_report_type_valid(f54, reptype))
+			continue;
+
+		f54->inputs[i++] = reptype;
+	}
+
+	/* Remaining values are zero via kzalloc */
+}
+
+static int rmi_f54_request_report(struct rmi_function *fn, u8 report_type)
+{
+	struct f54_data *f54 = dev_get_drvdata(&fn->dev);
+	struct rmi_device *rmi_dev = fn->rmi_dev;
+	int error;
+
+	/* Write Report Type into F54_AD_Data0 */
+	if (f54->report_type != report_type) {
+		error = rmi_write(rmi_dev, f54->fn->fd.data_base_addr,
+				  report_type);
+		if (error)
+			return error;
+		f54->report_type = report_type;
+	}
+
+	/*
+	 * Small delay after disabling interrupts to avoid race condition
+	 * in firmare. This value is a bit higher than absolutely necessary.
+	 * Should be removed once issue is resolved in firmware.
+	 */
+	usleep_range(2000, 3000);
+
+	mutex_lock(&f54->data_mutex);
+
+	error = rmi_write(rmi_dev, fn->fd.command_base_addr, F54_GET_REPORT);
+	if (error < 0)
+		return error;
+
+	init_completion(&f54->cmd_done);
+
+	f54->is_busy = 1;
+	f54->timeout = jiffies + msecs_to_jiffies(100);
+
+	queue_delayed_work(f54->workqueue, &f54->work, 0);
+
+	mutex_unlock(&f54->data_mutex);
+
+	return 0;
+}
+
+static size_t rmi_f54_get_report_size(struct f54_data *f54)
+{
+	u8 rx = f54->num_rx_electrodes ? : f54->num_rx_electrodes;
+	u8 tx = f54->num_tx_electrodes ? : f54->num_tx_electrodes;
+	size_t size;
+
+	switch (rmi_f54_get_reptype(f54, f54->input)) {
+	case F54_8BIT_IMAGE:
+		size = rx * tx;
+		break;
+	case F54_16BIT_IMAGE:
+	case F54_RAW_16BIT_IMAGE:
+	case F54_TRUE_BASELINE:
+	case F54_FULL_RAW_CAP:
+	case F54_FULL_RAW_CAP_RX_OFFSET_REMOVED:
+		size = sizeof(u16) * rx * tx;
+		break;
+	default:
+		size = 0;
+	}
+
+	return size;
+}
+
+static int rmi_f54_get_pixel_fmt(enum rmi_f54_report_type reptype, u32 *pixfmt)
+{
+	int ret = 0;
+
+	switch (reptype) {
+	case F54_8BIT_IMAGE:
+		*pixfmt = V4L2_TCH_FMT_DELTA_TD08;
+		break;
+
+	case F54_16BIT_IMAGE:
+		*pixfmt = V4L2_TCH_FMT_DELTA_TD16;
+		break;
+
+	case F54_RAW_16BIT_IMAGE:
+	case F54_TRUE_BASELINE:
+	case F54_FULL_RAW_CAP:
+	case F54_FULL_RAW_CAP_RX_OFFSET_REMOVED:
+		*pixfmt = V4L2_TCH_FMT_TU16;
+		break;
+
+	case F54_REPORT_NONE:
+	case F54_MAX_REPORT_TYPE:
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
+static const struct v4l2_file_operations rmi_f54_video_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 rmi_f54_queue_setup(struct vb2_queue *q, unsigned int *nbuffers,
+			       unsigned int *nplanes, unsigned int sizes[],
+			       struct device *alloc_devs[])
+{
+	struct f54_data *f54 = q->drv_priv;
+
+	if (*nplanes)
+		return sizes[0] < rmi_f54_get_report_size(f54) ? -EINVAL : 0;
+
+	*nplanes = 1;
+	sizes[0] = rmi_f54_get_report_size(f54);
+
+	return 0;
+}
+
+static void rmi_f54_buffer_queue(struct vb2_buffer *vb)
+{
+	struct f54_data *f54 = vb2_get_drv_priv(vb->vb2_queue);
+	u16 *ptr;
+	enum vb2_buffer_state state;
+	enum rmi_f54_report_type reptype;
+	int ret;
+
+	mutex_lock(&f54->status_mutex);
+
+	reptype = rmi_f54_get_reptype(f54, f54->input);
+	if (reptype == F54_REPORT_NONE) {
+		state = VB2_BUF_STATE_ERROR;
+		goto done;
+	}
+
+	if (f54->is_busy) {
+		state = VB2_BUF_STATE_ERROR;
+		goto done;
+	}
+
+	ret = rmi_f54_request_report(f54->fn, reptype);
+	if (ret) {
+		dev_err(&f54->fn->dev, "Error requesting F54 report\n");
+		state = VB2_BUF_STATE_ERROR;
+		goto done;
+	}
+
+	/* get frame data */
+	mutex_lock(&f54->data_mutex);
+
+	while (f54->is_busy) {
+		mutex_unlock(&f54->data_mutex);
+		if (!wait_for_completion_timeout(&f54->cmd_done,
+						 msecs_to_jiffies(1000))) {
+			dev_err(&f54->fn->dev, "Timed out\n");
+			state = VB2_BUF_STATE_ERROR;
+			goto done;
+		}
+		mutex_lock(&f54->data_mutex);
+	}
+
+	ptr = vb2_plane_vaddr(vb, 0);
+	if (!ptr) {
+		dev_err(&f54->fn->dev, "Error acquiring frame ptr\n");
+		state = VB2_BUF_STATE_ERROR;
+		goto data_done;
+	}
+
+	memcpy(ptr, f54->report_data, f54->report_size);
+	vb2_set_plane_payload(vb, 0, rmi_f54_get_report_size(f54));
+	state = VB2_BUF_STATE_DONE;
+
+data_done:
+	mutex_unlock(&f54->data_mutex);
+done:
+	vb2_buffer_done(vb, state);
+	mutex_unlock(&f54->status_mutex);
+}
+
+/* V4L2 structures */
+static const struct vb2_ops rmi_f54_queue_ops = {
+	.queue_setup            = rmi_f54_queue_setup,
+	.buf_queue              = rmi_f54_buffer_queue,
+	.wait_prepare           = vb2_ops_wait_prepare,
+	.wait_finish            = vb2_ops_wait_finish,
+};
+
+static const struct vb2_queue rmi_f54_queue = {
+	.type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
+	.io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF | VB2_READ,
+	.buf_struct_size = sizeof(struct vb2_buffer),
+	.ops = &rmi_f54_queue_ops,
+	.mem_ops = &vb2_vmalloc_memops,
+	.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC,
+	.min_buffers_needed = 1,
+};
+
+static int rmi_f54_vidioc_querycap(struct file *file, void *priv,
+				   struct v4l2_capability *cap)
+{
+	struct f54_data *f54 = video_drvdata(file);
+
+	strlcpy(cap->driver, F54_NAME, sizeof(cap->driver));
+	strlcpy(cap->card, SYNAPTICS_INPUT_DEVICE_NAME, sizeof(cap->card));
+	snprintf(cap->bus_info, sizeof(cap->bus_info),
+		"rmi4:%s", dev_name(&f54->fn->dev));
+
+	return 0;
+}
+
+static int rmi_f54_vidioc_enum_input(struct file *file, void *priv,
+				     struct v4l2_input *i)
+{
+	struct f54_data *f54 = video_drvdata(file);
+	enum rmi_f54_report_type reptype;
+
+	reptype = rmi_f54_get_reptype(f54, i->index);
+	if (reptype == F54_REPORT_NONE)
+		return -EINVAL;
+
+	i->type = V4L2_INPUT_TYPE_TOUCH;
+
+	strlcpy(i->name, rmi_f54_report_type_names[reptype], sizeof(i->name));
+	return 0;
+}
+
+static int rmi_f54_set_input(struct f54_data *f54, unsigned int i)
+{
+	struct v4l2_pix_format *f = &f54->format;
+	enum rmi_f54_report_type reptype;
+	int ret;
+
+	reptype = rmi_f54_get_reptype(f54, i);
+	if (reptype == F54_REPORT_NONE)
+		return -EINVAL;
+
+	ret = rmi_f54_get_pixel_fmt(reptype, &f->pixelformat);
+	if (ret)
+		return ret;
+
+	f54->input = i;
+
+	f->width = f54->num_rx_electrodes;
+	f->height = f54->num_tx_electrodes;
+	f->field = V4L2_FIELD_NONE;
+	f->colorspace = V4L2_COLORSPACE_RAW;
+	f->bytesperline = f->width * sizeof(u16);
+	f->sizeimage = f->width * f->height * sizeof(u16);
+
+	return 0;
+}
+
+static int rmi_f54_vidioc_s_input(struct file *file, void *priv, unsigned int i)
+{
+	return rmi_f54_set_input(video_drvdata(file), i);
+}
+
+static int rmi_f54_vidioc_g_input(struct file *file, void *priv,
+				  unsigned int *i)
+{
+	struct f54_data *f54 = video_drvdata(file);
+
+	*i = f54->input;
+
+	return 0;
+}
+
+static int rmi_f54_vidioc_fmt(struct file *file, void *priv,
+			      struct v4l2_format *f)
+{
+	struct f54_data *f54 = video_drvdata(file);
+
+	f->fmt.pix = f54->format;
+
+	return 0;
+}
+
+static int rmi_f54_vidioc_enum_fmt(struct file *file, void *priv,
+				   struct v4l2_fmtdesc *fmt)
+{
+	if (fmt->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	switch (fmt->index) {
+	case 0:
+		fmt->pixelformat = V4L2_TCH_FMT_DELTA_TD16;
+		break;
+
+	case 1:
+		fmt->pixelformat = V4L2_TCH_FMT_DELTA_TD08;
+		break;
+
+	case 2:
+		fmt->pixelformat = V4L2_TCH_FMT_TU16;
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int rmi_f54_vidioc_g_parm(struct file *file, void *fh,
+				 struct v4l2_streamparm *a)
+{
+	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	a->parm.capture.readbuffers = 1;
+	a->parm.capture.timeperframe.numerator = 1;
+	a->parm.capture.timeperframe.denominator = 10;
+	return 0;
+}
+
+static const struct v4l2_ioctl_ops rmi_f54_video_ioctl_ops = {
+	.vidioc_querycap	= rmi_f54_vidioc_querycap,
+
+	.vidioc_enum_fmt_vid_cap = rmi_f54_vidioc_enum_fmt,
+	.vidioc_s_fmt_vid_cap	= rmi_f54_vidioc_fmt,
+	.vidioc_g_fmt_vid_cap	= rmi_f54_vidioc_fmt,
+	.vidioc_try_fmt_vid_cap	= rmi_f54_vidioc_fmt,
+	.vidioc_g_parm		= rmi_f54_vidioc_g_parm,
+
+	.vidioc_enum_input	= rmi_f54_vidioc_enum_input,
+	.vidioc_g_input		= rmi_f54_vidioc_g_input,
+	.vidioc_s_input		= rmi_f54_vidioc_s_input,
+
+	.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,
+};
+
+static const struct video_device rmi_f54_video_device = {
+	.name = "Synaptics RMI4",
+	.fops = &rmi_f54_video_fops,
+	.ioctl_ops = &rmi_f54_video_ioctl_ops,
+	.release = video_device_release_empty,
+	.device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_TOUCH |
+		       V4L2_CAP_READWRITE | V4L2_CAP_STREAMING,
+};
+
+static void rmi_f54_work(struct work_struct *work)
+{
+	struct f54_data *f54 = container_of(work, struct f54_data, work.work);
+	struct rmi_function *fn = f54->fn;
+	u8 fifo[2];
+	struct rmi_f54_reports *report;
+	int report_size;
+	u8 command;
+	u8 *data;
+	int error;
+
+	data = f54->report_data;
+	report_size = rmi_f54_get_report_size(f54);
+	if (report_size == 0) {
+		dev_err(&fn->dev, "Bad report size, report type=%d\n",
+				f54->report_type);
+		error = -EINVAL;
+		goto error;     /* retry won't help */
+	}
+	f54->standard_report[0].size = report_size;
+	report = f54->standard_report;
+
+	mutex_lock(&f54->data_mutex);
+
+	/*
+	 * Need to check if command has completed.
+	 * If not try again later.
+	 */
+	error = rmi_read(fn->rmi_dev, f54->fn->fd.command_base_addr,
+			 &command);
+	if (error) {
+		dev_err(&fn->dev, "Failed to read back command\n");
+		goto error;
+	}
+	if (command & F54_GET_REPORT) {
+		if (time_after(jiffies, f54->timeout)) {
+			dev_err(&fn->dev, "Get report command timed out\n");
+			error = -ETIMEDOUT;
+		}
+		report_size = 0;
+		goto error;
+	}
+
+	rmi_dbg(RMI_DEBUG_FN, &fn->dev, "Get report command completed, reading data\n");
+
+	report_size = 0;
+	for (; report->size; report++) {
+		fifo[0] = report->start & 0xff;
+		fifo[1] = (report->start >> 8) & 0xff;
+		error = rmi_write_block(fn->rmi_dev,
+					fn->fd.data_base_addr + F54_FIFO_OFFSET,
+					fifo, sizeof(fifo));
+		if (error) {
+			dev_err(&fn->dev, "Failed to set fifo start offset\n");
+			goto abort;
+		}
+
+		error = rmi_read_block(fn->rmi_dev, fn->fd.data_base_addr +
+				       F54_REPORT_DATA_OFFSET, data,
+				       report->size);
+		if (error) {
+			dev_err(&fn->dev, "%s: read [%d bytes] returned %d\n",
+				__func__, report->size, error);
+			goto abort;
+		}
+		data += report->size;
+		report_size += report->size;
+	}
+
+abort:
+	f54->report_size = error ? 0 : report_size;
+error:
+	if (error)
+		report_size = 0;
+
+	if (report_size == 0 && !error) {
+		queue_delayed_work(f54->workqueue, &f54->work,
+				   msecs_to_jiffies(1));
+	} else {
+		f54->is_busy = false;
+		complete(&f54->cmd_done);
+	}
+
+	mutex_unlock(&f54->data_mutex);
+}
+
+static int rmi_f54_attention(struct rmi_function *fn, unsigned long *irqbits)
+{
+	return 0;
+}
+
+static int rmi_f54_config(struct rmi_function *fn)
+{
+	struct rmi_driver *drv = fn->rmi_dev->driver;
+
+	drv->set_irq_bits(fn->rmi_dev, fn->irq_mask);
+
+	return 0;
+}
+
+static int rmi_f54_detect(struct rmi_function *fn)
+{
+	int error;
+	struct f54_data *f54;
+
+	f54 = dev_get_drvdata(&fn->dev);
+
+	error = rmi_read_block(fn->rmi_dev, fn->fd.query_base_addr,
+			       &f54->qry, sizeof(f54->qry));
+	if (error) {
+		dev_err(&fn->dev, "%s: Failed to query F54 properties\n",
+			__func__);
+		return error;
+	}
+
+	f54->num_rx_electrodes = f54->qry[0];
+	f54->num_tx_electrodes = f54->qry[1];
+	f54->capabilities = f54->qry[2];
+	f54->clock_rate = f54->qry[3] | (f54->qry[4] << 8);
+	f54->family = f54->qry[5];
+
+	rmi_dbg(RMI_DEBUG_FN, &fn->dev, "F54 num_rx_electrodes: %d\n",
+		f54->num_rx_electrodes);
+	rmi_dbg(RMI_DEBUG_FN, &fn->dev, "F54 num_tx_electrodes: %d\n",
+		f54->num_tx_electrodes);
+	rmi_dbg(RMI_DEBUG_FN, &fn->dev, "F54 capabilities: 0x%x\n",
+		f54->capabilities);
+	rmi_dbg(RMI_DEBUG_FN, &fn->dev, "F54 clock rate: 0x%x\n",
+		f54->clock_rate);
+	rmi_dbg(RMI_DEBUG_FN, &fn->dev, "F54 family: 0x%x\n",
+		f54->family);
+
+	f54->is_busy = false;
+
+	return 0;
+}
+
+static int rmi_f54_probe(struct rmi_function *fn)
+{
+	struct f54_data *f54;
+	int ret;
+	u8 rx, tx;
+
+	f54 = devm_kzalloc(&fn->dev, sizeof(struct f54_data), GFP_KERNEL);
+	if (!f54)
+		return -ENOMEM;
+
+	f54->fn = fn;
+	dev_set_drvdata(&fn->dev, f54);
+
+	ret = rmi_f54_detect(fn);
+	if (ret)
+		return ret;
+
+	mutex_init(&f54->data_mutex);
+	mutex_init(&f54->status_mutex);
+
+	rx = f54->num_rx_electrodes;
+	tx = f54->num_tx_electrodes;
+	f54->report_data = devm_kzalloc(&fn->dev,
+					sizeof(u16) * tx * rx,
+					GFP_KERNEL);
+	if (f54->report_data == NULL)
+		return -ENOMEM;
+
+	INIT_DELAYED_WORK(&f54->work, rmi_f54_work);
+
+	f54->workqueue = create_singlethread_workqueue("rmi4-poller");
+	if (!f54->workqueue)
+		return -ENOMEM;
+
+	rmi_f54_create_input_map(f54);
+
+	/* register video device */
+	strlcpy(f54->v4l2.name, F54_NAME, sizeof(f54->v4l2.name));
+	ret = v4l2_device_register(&fn->dev, &f54->v4l2);
+	if (ret) {
+		dev_err(&fn->dev, "Unable to register video dev.\n");
+		goto remove_wq;
+	}
+
+	/* initialize the queue */
+	mutex_init(&f54->lock);
+	f54->queue = rmi_f54_queue;
+	f54->queue.drv_priv = f54;
+	f54->queue.lock = &f54->lock;
+	f54->queue.dev = &fn->dev;
+
+	ret = vb2_queue_init(&f54->queue);
+	if (ret)
+		goto remove_v4l2;
+
+	f54->vdev = rmi_f54_video_device;
+	f54->vdev.v4l2_dev = &f54->v4l2;
+	f54->vdev.lock = &f54->lock;
+	f54->vdev.vfl_dir = VFL_DIR_RX;
+	f54->vdev.queue = &f54->queue;
+	video_set_drvdata(&f54->vdev, f54);
+
+	ret = video_register_device(&f54->vdev, VFL_TYPE_TOUCH, -1);
+	if (ret) {
+		dev_err(&fn->dev, "Unable to register video subdevice.");
+		goto remove_v4l2;
+	}
+
+	return 0;
+
+remove_v4l2:
+	v4l2_device_unregister(&f54->v4l2);
+remove_wq:
+	cancel_delayed_work_sync(&f54->work);
+	flush_workqueue(f54->workqueue);
+	destroy_workqueue(f54->workqueue);
+	return ret;
+}
+
+static void rmi_f54_remove(struct rmi_function *fn)
+{
+	struct f54_data *f54 = dev_get_drvdata(&fn->dev);
+
+	video_unregister_device(&f54->vdev);
+	v4l2_device_unregister(&f54->v4l2);
+}
+
+struct rmi_function_handler rmi_f54_handler = {
+	.driver = {
+		.name = F54_NAME,
+	},
+	.func = 0x54,
+	.probe = rmi_f54_probe,
+	.config = rmi_f54_config,
+	.attention = rmi_f54_attention,
+	.remove = rmi_f54_remove,
+};
diff --git a/drivers/input/rmi4/rmi_i2c.c b/drivers/input/rmi4/rmi_i2c.c
index 6f2e0e4f0296..1ebc2c1debae 100644
--- a/drivers/input/rmi4/rmi_i2c.c
+++ b/drivers/input/rmi4/rmi_i2c.c
@@ -221,6 +221,21 @@ static const struct of_device_id rmi_i2c_of_match[] = {
 MODULE_DEVICE_TABLE(of, rmi_i2c_of_match);
 #endif
 
+static void rmi_i2c_regulator_bulk_disable(void *data)
+{
+	struct rmi_i2c_xport *rmi_i2c = data;
+
+	regulator_bulk_disable(ARRAY_SIZE(rmi_i2c->supplies),
+			       rmi_i2c->supplies);
+}
+
+static void rmi_i2c_unregister_transport(void *data)
+{
+	struct rmi_i2c_xport *rmi_i2c = data;
+
+	rmi_unregister_transport_device(&rmi_i2c->xport);
+}
+
 static int rmi_i2c_probe(struct i2c_client *client,
 			 const struct i2c_device_id *id)
 {
@@ -264,6 +279,12 @@ static int rmi_i2c_probe(struct i2c_client *client,
 	if (retval < 0)
 		return retval;
 
+	retval = devm_add_action_or_reset(&client->dev,
+					  rmi_i2c_regulator_bulk_disable,
+					  rmi_i2c);
+	if (retval)
+		return retval;
+
 	of_property_read_u32(client->dev.of_node, "syna,startup-delay-ms",
 			     &rmi_i2c->startup_delay);
 
@@ -294,6 +315,11 @@ static int rmi_i2c_probe(struct i2c_client *client,
 			client->addr);
 		return retval;
 	}
+	retval = devm_add_action_or_reset(&client->dev,
+					  rmi_i2c_unregister_transport,
+					  rmi_i2c);
+	if (retval)
+		return retval;
 
 	retval = rmi_i2c_init_irq(client);
 	if (retval < 0)
@@ -304,17 +330,6 @@ static int rmi_i2c_probe(struct i2c_client *client,
 	return 0;
 }
 
-static int rmi_i2c_remove(struct i2c_client *client)
-{
-	struct rmi_i2c_xport *rmi_i2c = i2c_get_clientdata(client);
-
-	rmi_unregister_transport_device(&rmi_i2c->xport);
-	regulator_bulk_disable(ARRAY_SIZE(rmi_i2c->supplies),
-			       rmi_i2c->supplies);
-
-	return 0;
-}
-
 #ifdef CONFIG_PM_SLEEP
 static int rmi_i2c_suspend(struct device *dev)
 {
@@ -431,7 +446,6 @@ static struct i2c_driver rmi_i2c_driver = {
 	},
 	.id_table	= rmi_id,
 	.probe		= rmi_i2c_probe,
-	.remove		= rmi_i2c_remove,
 };
 
 module_i2c_driver(rmi_i2c_driver);
diff --git a/drivers/input/rmi4/rmi_spi.c b/drivers/input/rmi4/rmi_spi.c
index 55bd1b34970c..4ebef607e214 100644
--- a/drivers/input/rmi4/rmi_spi.c
+++ b/drivers/input/rmi4/rmi_spi.c
@@ -396,6 +396,13 @@ static inline int rmi_spi_of_probe(struct spi_device *spi,
 }
 #endif
 
+static void rmi_spi_unregister_transport(void *data)
+{
+	struct rmi_spi_xport *rmi_spi = data;
+
+	rmi_unregister_transport_device(&rmi_spi->xport);
+}
+
 static int rmi_spi_probe(struct spi_device *spi)
 {
 	struct rmi_spi_xport *rmi_spi;
@@ -464,6 +471,11 @@ static int rmi_spi_probe(struct spi_device *spi)
 		dev_err(&spi->dev, "failed to register transport.\n");
 		return retval;
 	}
+	retval = devm_add_action_or_reset(&spi->dev,
+					  rmi_spi_unregister_transport,
+					  rmi_spi);
+	if (retval)
+		return retval;
 
 	retval = rmi_spi_init_irq(spi);
 	if (retval < 0)
@@ -473,15 +485,6 @@ static int rmi_spi_probe(struct spi_device *spi)
 	return 0;
 }
 
-static int rmi_spi_remove(struct spi_device *spi)
-{
-	struct rmi_spi_xport *rmi_spi = spi_get_drvdata(spi);
-
-	rmi_unregister_transport_device(&rmi_spi->xport);
-
-	return 0;
-}
-
 #ifdef CONFIG_PM_SLEEP
 static int rmi_spi_suspend(struct device *dev)
 {
@@ -577,7 +580,6 @@ static struct spi_driver rmi_spi_driver = {
 	},
 	.id_table	= rmi_id,
 	.probe		= rmi_spi_probe,
-	.remove		= rmi_spi_remove,
 };
 
 module_spi_driver(rmi_spi_driver);
diff --git a/drivers/input/serio/i8042-io.h b/drivers/input/serio/i8042-io.h
index a5eed2ade53d..34da81c006b6 100644
--- a/drivers/input/serio/i8042-io.h
+++ b/drivers/input/serio/i8042-io.h
@@ -81,7 +81,7 @@ static inline int i8042_platform_init(void)
 		return -EBUSY;
 #endif
 
-	i8042_reset = 1;
+	i8042_reset = I8042_RESET_ALWAYS;
 	return 0;
 }
 
diff --git a/drivers/input/serio/i8042-ip22io.h b/drivers/input/serio/i8042-ip22io.h
index ee1ad27d6ed0..08a1c10a1448 100644
--- a/drivers/input/serio/i8042-ip22io.h
+++ b/drivers/input/serio/i8042-ip22io.h
@@ -61,7 +61,7 @@ static inline int i8042_platform_init(void)
 		return -EBUSY;
 #endif
 
-	i8042_reset = 1;
+	i8042_reset = I8042_RESET_ALWAYS;
 
 	return 0;
 }
diff --git a/drivers/input/serio/i8042-ppcio.h b/drivers/input/serio/i8042-ppcio.h
index f708c75d16f1..1aabea43329e 100644
--- a/drivers/input/serio/i8042-ppcio.h
+++ b/drivers/input/serio/i8042-ppcio.h
@@ -44,7 +44,7 @@ static inline void i8042_write_command(int val)
 
 static inline int i8042_platform_init(void)
 {
-	i8042_reset = 1;
+	i8042_reset = I8042_RESET_ALWAYS;
 	return 0;
 }
 
diff --git a/drivers/input/serio/i8042-sparcio.h b/drivers/input/serio/i8042-sparcio.h
index afcd1c1a05b2..6231d63860ee 100644
--- a/drivers/input/serio/i8042-sparcio.h
+++ b/drivers/input/serio/i8042-sparcio.h
@@ -130,7 +130,7 @@ static int __init i8042_platform_init(void)
 		}
 	}
 
-	i8042_reset = 1;
+	i8042_reset = I8042_RESET_ALWAYS;
 
 	return 0;
 }
diff --git a/drivers/input/serio/i8042-unicore32io.h b/drivers/input/serio/i8042-unicore32io.h
index 73f5cc124a36..455747552f85 100644
--- a/drivers/input/serio/i8042-unicore32io.h
+++ b/drivers/input/serio/i8042-unicore32io.h
@@ -61,7 +61,7 @@ static inline int i8042_platform_init(void)
 	if (!request_mem_region(I8042_REGION_START, I8042_REGION_SIZE, "i8042"))
 		return -EBUSY;
 
-	i8042_reset = 1;
+	i8042_reset = I8042_RESET_ALWAYS;
 	return 0;
 }
 
diff --git a/drivers/input/serio/i8042-x86ia64io.h b/drivers/input/serio/i8042-x86ia64io.h
index 68f5f4a0f1e7..f4bfb4b2d50a 100644
--- a/drivers/input/serio/i8042-x86ia64io.h
+++ b/drivers/input/serio/i8042-x86ia64io.h
@@ -510,6 +510,90 @@ static const struct dmi_system_id __initconst i8042_dmi_nomux_table[] = {
 	{ }
 };
 
+/*
+ * On some Asus laptops, just running self tests cause problems.
+ */
+static const struct dmi_system_id i8042_dmi_noselftest_table[] = {
+	{
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "ASUSTeK COMPUTER INC."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "A455LD"),
+		},
+	},
+	{
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "ASUSTeK COMPUTER INC."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "K401LB"),
+		},
+	},
+	{
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "ASUSTeK COMPUTER INC."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "K501LB"),
+		},
+	},
+	{
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "ASUSTeK COMPUTER INC."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "K501LX"),
+		},
+	},
+	{
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "ASUSTeK COMPUTER INC."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "R409L"),
+		},
+	},
+	{
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "ASUSTeK COMPUTER INC."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "V502LX"),
+		},
+	},
+	{
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "ASUSTeK COMPUTER INC."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "X302LA"),
+		},
+	},
+	{
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "ASUSTeK COMPUTER INC."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "X450LCP"),
+		},
+	},
+	{
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "ASUSTeK COMPUTER INC."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "X450LD"),
+		},
+	},
+	{
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "ASUSTeK COMPUTER INC."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "X455LAB"),
+		},
+	},
+	{
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "ASUSTeK COMPUTER INC."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "X455LDB"),
+		},
+	},
+	{
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "ASUSTeK COMPUTER INC."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "X455LF"),
+		},
+	},
+	{
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "ASUSTeK COMPUTER INC."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "Z450LA"),
+		},
+	},
+	{ }
+};
 static const struct dmi_system_id __initconst i8042_dmi_reset_table[] = {
 	{
 		/* MSI Wind U-100 */
@@ -1072,12 +1156,18 @@ static int __init i8042_platform_init(void)
 		return retval;
 
 #if defined(__ia64__)
-        i8042_reset = true;
+        i8042_reset = I8042_RESET_ALWAYS;
 #endif
 
 #ifdef CONFIG_X86
-	if (dmi_check_system(i8042_dmi_reset_table))
-		i8042_reset = true;
+	/* Honor module parameter when value is not default */
+	if (i8042_reset == I8042_RESET_DEFAULT) {
+		if (dmi_check_system(i8042_dmi_reset_table))
+			i8042_reset = I8042_RESET_ALWAYS;
+
+		if (dmi_check_system(i8042_dmi_noselftest_table))
+			i8042_reset = I8042_RESET_NEVER;
+	}
 
 	if (dmi_check_system(i8042_dmi_noloop_table))
 		i8042_noloop = true;
diff --git a/drivers/input/serio/i8042.c b/drivers/input/serio/i8042.c
index 405252a884dd..89abfdb539ac 100644
--- a/drivers/input/serio/i8042.c
+++ b/drivers/input/serio/i8042.c
@@ -48,9 +48,39 @@ static bool i8042_unlock;
 module_param_named(unlock, i8042_unlock, bool, 0);
 MODULE_PARM_DESC(unlock, "Ignore keyboard lock.");
 
-static bool i8042_reset;
-module_param_named(reset, i8042_reset, bool, 0);
-MODULE_PARM_DESC(reset, "Reset controller during init and cleanup.");
+enum i8042_controller_reset_mode {
+	I8042_RESET_NEVER,
+	I8042_RESET_ALWAYS,
+	I8042_RESET_ON_S2RAM,
+#define I8042_RESET_DEFAULT	I8042_RESET_ON_S2RAM
+};
+static enum i8042_controller_reset_mode i8042_reset = I8042_RESET_DEFAULT;
+static int i8042_set_reset(const char *val, const struct kernel_param *kp)
+{
+	enum i8042_controller_reset_mode *arg = kp->arg;
+	int error;
+	bool reset;
+
+	if (val) {
+		error = kstrtobool(val, &reset);
+		if (error)
+			return error;
+	} else {
+		reset = true;
+	}
+
+	*arg = reset ? I8042_RESET_ALWAYS : I8042_RESET_NEVER;
+	return 0;
+}
+
+static const struct kernel_param_ops param_ops_reset_param = {
+	.flags = KERNEL_PARAM_OPS_FL_NOARG,
+	.set = i8042_set_reset,
+};
+#define param_check_reset_param(name, p)	\
+	__param_check(name, p, enum i8042_controller_reset_mode)
+module_param_named(reset, i8042_reset, reset_param, 0);
+MODULE_PARM_DESC(reset, "Reset controller on resume, cleanup or both");
 
 static bool i8042_direct;
 module_param_named(direct, i8042_direct, bool, 0);
@@ -1019,7 +1049,7 @@ static int i8042_controller_init(void)
  * Reset the controller and reset CRT to the original value set by BIOS.
  */
 
-static void i8042_controller_reset(bool force_reset)
+static void i8042_controller_reset(bool s2r_wants_reset)
 {
 	i8042_flush();
 
@@ -1044,8 +1074,10 @@ static void i8042_controller_reset(bool force_reset)
  * Reset the controller if requested.
  */
 
-	if (i8042_reset || force_reset)
+	if (i8042_reset == I8042_RESET_ALWAYS ||
+	    (i8042_reset == I8042_RESET_ON_S2RAM && s2r_wants_reset)) {
 		i8042_controller_selftest();
+	}
 
 /*
  * Restore the original control register setting.
@@ -1110,7 +1142,7 @@ static void i8042_dritek_enable(void)
  * before suspending.
  */
 
-static int i8042_controller_resume(bool force_reset)
+static int i8042_controller_resume(bool s2r_wants_reset)
 {
 	int error;
 
@@ -1118,7 +1150,8 @@ static int i8042_controller_resume(bool force_reset)
 	if (error)
 		return error;
 
-	if (i8042_reset || force_reset) {
+	if (i8042_reset == I8042_RESET_ALWAYS ||
+	    (i8042_reset == I8042_RESET_ON_S2RAM && s2r_wants_reset)) {
 		error = i8042_controller_selftest();
 		if (error)
 			return error;
@@ -1195,7 +1228,7 @@ static int i8042_pm_resume_noirq(struct device *dev)
 
 static int i8042_pm_resume(struct device *dev)
 {
-	bool force_reset;
+	bool want_reset;
 	int i;
 
 	for (i = 0; i < I8042_NUM_PORTS; i++) {
@@ -1218,9 +1251,9 @@ static int i8042_pm_resume(struct device *dev)
 	 * off control to the platform firmware, otherwise we can simply restore
 	 * the mode.
 	 */
-	force_reset = pm_resume_via_firmware();
+	want_reset = pm_resume_via_firmware();
 
-	return i8042_controller_resume(force_reset);
+	return i8042_controller_resume(want_reset);
 }
 
 static int i8042_pm_thaw(struct device *dev)
@@ -1482,7 +1515,7 @@ static int __init i8042_probe(struct platform_device *dev)
 
 	i8042_platform_device = dev;
 
-	if (i8042_reset) {
+	if (i8042_reset == I8042_RESET_ALWAYS) {
 		error = i8042_controller_selftest();
 		if (error)
 			return error;
diff --git a/drivers/input/serio/serport.c b/drivers/input/serio/serport.c
index 9c927d35c1f5..d189843f3727 100644
--- a/drivers/input/serio/serport.c
+++ b/drivers/input/serio/serport.c
@@ -71,10 +71,7 @@ static void serport_serio_close(struct serio *serio)
 
 	spin_lock_irqsave(&serport->lock, flags);
 	clear_bit(SERPORT_ACTIVE, &serport->flags);
-	set_bit(SERPORT_DEAD, &serport->flags);
 	spin_unlock_irqrestore(&serport->lock, flags);
-
-	wake_up_interruptible(&serport->wait);
 }
 
 /*
@@ -248,6 +245,19 @@ static long serport_ldisc_compat_ioctl(struct tty_struct *tty,
 }
 #endif
 
+static int serport_ldisc_hangup(struct tty_struct *tty)
+{
+	struct serport *serport = (struct serport *) tty->disc_data;
+	unsigned long flags;
+
+	spin_lock_irqsave(&serport->lock, flags);
+	set_bit(SERPORT_DEAD, &serport->flags);
+	spin_unlock_irqrestore(&serport->lock, flags);
+
+	wake_up_interruptible(&serport->wait);
+	return 0;
+}
+
 static void serport_ldisc_write_wakeup(struct tty_struct * tty)
 {
 	struct serport *serport = (struct serport *) tty->disc_data;
@@ -274,6 +284,7 @@ static struct tty_ldisc_ops serport_ldisc = {
 	.compat_ioctl =	serport_ldisc_compat_ioctl,
 #endif
 	.receive_buf =	serport_ldisc_receive,
+	.hangup =	serport_ldisc_hangup,
 	.write_wakeup =	serport_ldisc_write_wakeup
 };
 
diff --git a/drivers/input/tablet/pegasus_notetaker.c b/drivers/input/tablet/pegasus_notetaker.c
index 949dacc78664..47de5a81172f 100644
--- a/drivers/input/tablet/pegasus_notetaker.c
+++ b/drivers/input/tablet/pegasus_notetaker.c
@@ -40,6 +40,7 @@
 #include <linux/input.h>
 #include <linux/usb/input.h>
 #include <linux/slab.h>
+#include <linux/workqueue.h>
 
 /* USB HID defines */
 #define USB_REQ_GET_REPORT		0x01
diff --git a/drivers/input/touchscreen/Kconfig b/drivers/input/touchscreen/Kconfig
index 2fb1f430a431..efca0133e266 100644
--- a/drivers/input/touchscreen/Kconfig
+++ b/drivers/input/touchscreen/Kconfig
@@ -115,6 +115,15 @@ config TOUCHSCREEN_ATMEL_MXT
 	  To compile this driver as a module, choose M here: the
 	  module will be called atmel_mxt_ts.
 
+config TOUCHSCREEN_ATMEL_MXT_T37
+	bool "Support T37 Diagnostic Data"
+	depends on TOUCHSCREEN_ATMEL_MXT
+	depends on VIDEO_V4L2=y || (TOUCHSCREEN_ATMEL_MXT=m && VIDEO_V4L2=m)
+	select VIDEOBUF2_VMALLOC
+	help
+	  Say Y here if you want support to output data from the T37
+	  Diagnostic Data object using a V4L device.
+
 config TOUCHSCREEN_AUO_PIXCIR
 	tristate "AUO in-cell touchscreen using Pixcir ICs"
 	depends on I2C
@@ -305,19 +314,6 @@ config TOUCHSCREEN_EGALAX_SERIAL
 	  To compile this driver as a module, choose M here: the
 	  module will be called egalax_ts_serial.
 
-config TOUCHSCREEN_FT6236
-	tristate "FT6236 I2C touchscreen"
-	depends on I2C
-	depends on GPIOLIB || COMPILE_TEST
-	help
-	  Say Y here to enable support for the I2C connected FT6x06 and
-	  FT6x36 family of capacitive touchscreen drivers.
-
-	  If unsure, say N.
-
-	  To compile this driver as a module, choose M here: the
-	  module will be called ft6236.
-
 config TOUCHSCREEN_FUJITSU
 	tristate "Fujitsu serial touchscreen"
 	select SERIO
@@ -397,6 +393,18 @@ config TOUCHSCREEN_GUNZE
 	  To compile this driver as a module, choose M here: the
 	  module will be called gunze.
 
+config TOUCHSCREEN_EKTF2127
+	tristate "Elan eKTF2127 I2C touchscreen"
+	depends on I2C
+	help
+	  Say Y here if you have an Elan eKTF2127 touchscreen
+	  connected to your system.
+
+	  If unsure, say N.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ektf2127.
+
 config TOUCHSCREEN_ELAN
 	tristate "Elan eKTH I2C touchscreen"
 	depends on I2C
diff --git a/drivers/input/touchscreen/Makefile b/drivers/input/touchscreen/Makefile
index b4373d6be402..81b86451782d 100644
--- a/drivers/input/touchscreen/Makefile
+++ b/drivers/input/touchscreen/Makefile
@@ -32,11 +32,11 @@ obj-$(CONFIG_TOUCHSCREEN_EDT_FT5X06)	+= edt-ft5x06.o
 obj-$(CONFIG_TOUCHSCREEN_HAMPSHIRE)	+= hampshire.o
 obj-$(CONFIG_TOUCHSCREEN_GUNZE)		+= gunze.o
 obj-$(CONFIG_TOUCHSCREEN_EETI)		+= eeti_ts.o
+obj-$(CONFIG_TOUCHSCREEN_EKTF2127)	+= ektf2127.o
 obj-$(CONFIG_TOUCHSCREEN_ELAN)		+= elants_i2c.o
 obj-$(CONFIG_TOUCHSCREEN_ELO)		+= elo.o
 obj-$(CONFIG_TOUCHSCREEN_EGALAX)	+= egalax_ts.o
 obj-$(CONFIG_TOUCHSCREEN_EGALAX_SERIAL)	+= egalax_ts_serial.o
-obj-$(CONFIG_TOUCHSCREEN_FT6236)	+= ft6236.o
 obj-$(CONFIG_TOUCHSCREEN_FUJITSU)	+= fujitsu_ts.o
 obj-$(CONFIG_TOUCHSCREEN_GOODIX)	+= goodix.o
 obj-$(CONFIG_TOUCHSCREEN_ILI210X)	+= ili210x.o
diff --git a/drivers/input/touchscreen/atmel_mxt_ts.c b/drivers/input/touchscreen/atmel_mxt_ts.c
index 5af7907d0af4..e5d185fe69b9 100644
--- a/drivers/input/touchscreen/atmel_mxt_ts.c
+++ b/drivers/input/touchscreen/atmel_mxt_ts.c
@@ -4,6 +4,7 @@
  * Copyright (C) 2010 Samsung Electronics Co.Ltd
  * Copyright (C) 2011-2014 Atmel Corporation
  * Copyright (C) 2012 Google, Inc.
+ * Copyright (C) 2016 Zodiac Inflight Innovations
  *
  * Author: Joonyoung Shim <jy0922.shim@samsung.com>
  *
@@ -28,6 +29,10 @@
 #include <linux/of.h>
 #include <linux/slab.h>
 #include <asm/unaligned.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/videobuf2-v4l2.h>
+#include <media/videobuf2-vmalloc.h>
 
 /* Firmware files */
 #define MXT_FW_NAME		"maxtouch.fw"
@@ -99,6 +104,8 @@ struct t7_config {
 
 /* MXT_TOUCH_MULTI_T9 field */
 #define MXT_T9_CTRL		0
+#define MXT_T9_XSIZE		3
+#define MXT_T9_YSIZE		4
 #define MXT_T9_ORIENT		9
 #define MXT_T9_RANGE		18
 
@@ -119,11 +126,31 @@ struct t9_range {
 
 /* MXT_TOUCH_MULTI_T9 orient */
 #define MXT_T9_ORIENT_SWITCH	(1 << 0)
+#define MXT_T9_ORIENT_INVERTX	(1 << 1)
+#define MXT_T9_ORIENT_INVERTY	(1 << 2)
 
 /* MXT_SPT_COMMSCONFIG_T18 */
 #define MXT_COMMS_CTRL		0
 #define MXT_COMMS_CMD		1
 
+/* MXT_DEBUG_DIAGNOSTIC_T37 */
+#define MXT_DIAGNOSTIC_PAGEUP	0x01
+#define MXT_DIAGNOSTIC_DELTAS	0x10
+#define MXT_DIAGNOSTIC_REFS	0x11
+#define MXT_DIAGNOSTIC_SIZE	128
+
+#define MXT_FAMILY_1386			160
+#define MXT1386_COLUMNS			3
+#define MXT1386_PAGES_PER_COLUMN	8
+
+struct t37_debug {
+#ifdef CONFIG_TOUCHSCREEN_ATMEL_MXT_T37
+	u8 mode;
+	u8 page;
+	u8 data[MXT_DIAGNOSTIC_SIZE];
+#endif
+};
+
 /* Define for MXT_GEN_COMMAND_T6 */
 #define MXT_BOOT_VALUE		0xa5
 #define MXT_RESET_VALUE		0x01
@@ -133,10 +160,14 @@ struct t9_range {
 #define MXT_T100_CTRL		0
 #define MXT_T100_CFG1		1
 #define MXT_T100_TCHAUX		3
+#define MXT_T100_XSIZE		9
 #define MXT_T100_XRANGE		13
+#define MXT_T100_YSIZE		20
 #define MXT_T100_YRANGE		24
 
 #define MXT_T100_CFG_SWITCHXY	BIT(5)
+#define MXT_T100_CFG_INVERTY	BIT(6)
+#define MXT_T100_CFG_INVERTX	BIT(7)
 
 #define MXT_T100_TCHAUX_VECT	BIT(0)
 #define MXT_T100_TCHAUX_AMPL	BIT(1)
@@ -205,6 +236,37 @@ struct mxt_object {
 	u8 num_report_ids;
 } __packed;
 
+struct mxt_dbg {
+	u16 t37_address;
+	u16 diag_cmd_address;
+	struct t37_debug *t37_buf;
+	unsigned int t37_pages;
+	unsigned int t37_nodes;
+
+	struct v4l2_device v4l2;
+	struct v4l2_pix_format format;
+	struct video_device vdev;
+	struct vb2_queue queue;
+	struct mutex lock;
+	int input;
+};
+
+enum v4l_dbg_inputs {
+	MXT_V4L_INPUT_DELTAS,
+	MXT_V4L_INPUT_REFS,
+	MXT_V4L_INPUT_MAX,
+};
+
+static const struct v4l2_file_operations mxt_video_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,
+};
+
 /* Each client has this additional data */
 struct mxt_data {
 	struct i2c_client *client;
@@ -216,7 +278,11 @@ struct mxt_data {
 	unsigned int irq;
 	unsigned int max_x;
 	unsigned int max_y;
+	bool invertx;
+	bool inverty;
 	bool xy_switch;
+	u8 xsize;
+	u8 ysize;
 	bool in_bootloader;
 	u16 mem_size;
 	u8 t100_aux_ampl;
@@ -233,6 +299,7 @@ struct mxt_data {
 	u8 num_touchids;
 	u8 multitouch;
 	struct t7_config t7_cfg;
+	struct mxt_dbg dbg;
 
 	/* Cached parameters from object table */
 	u16 T5_address;
@@ -257,6 +324,11 @@ struct mxt_data {
 	struct completion crc_completion;
 };
 
+struct mxt_vb2_buffer {
+	struct vb2_buffer	vb;
+	struct list_head	list;
+};
+
 static size_t mxt_obj_size(const struct mxt_object *obj)
 {
 	return obj->size_minus_one + 1;
@@ -1503,6 +1575,11 @@ static void mxt_free_input_device(struct mxt_data *data)
 
 static void mxt_free_object_table(struct mxt_data *data)
 {
+#ifdef CONFIG_TOUCHSCREEN_ATMEL_MXT_T37
+	video_unregister_device(&data->dbg.vdev);
+	v4l2_device_unregister(&data->dbg.v4l2);
+#endif
+
 	kfree(data->object_table);
 	data->object_table = NULL;
 	kfree(data->msg_buf);
@@ -1661,6 +1738,18 @@ static int mxt_read_t9_resolution(struct mxt_data *data)
 		return -EINVAL;
 
 	error = __mxt_read_reg(client,
+			       object->start_address + MXT_T9_XSIZE,
+			       sizeof(data->xsize), &data->xsize);
+	if (error)
+		return error;
+
+	error = __mxt_read_reg(client,
+			       object->start_address + MXT_T9_YSIZE,
+			       sizeof(data->ysize), &data->ysize);
+	if (error)
+		return error;
+
+	error = __mxt_read_reg(client,
 			       object->start_address + MXT_T9_RANGE,
 			       sizeof(range), &range);
 	if (error)
@@ -1676,6 +1765,8 @@ static int mxt_read_t9_resolution(struct mxt_data *data)
 		return error;
 
 	data->xy_switch = orient & MXT_T9_ORIENT_SWITCH;
+	data->invertx = orient & MXT_T9_ORIENT_INVERTX;
+	data->inverty = orient & MXT_T9_ORIENT_INVERTY;
 
 	return 0;
 }
@@ -1710,6 +1801,18 @@ static int mxt_read_t100_config(struct mxt_data *data)
 
 	data->max_y = get_unaligned_le16(&range_y);
 
+	error = __mxt_read_reg(client,
+			       object->start_address + MXT_T100_XSIZE,
+			       sizeof(data->xsize), &data->xsize);
+	if (error)
+		return error;
+
+	error = __mxt_read_reg(client,
+			       object->start_address + MXT_T100_YSIZE,
+			       sizeof(data->ysize), &data->ysize);
+	if (error)
+		return error;
+
 	/* read orientation config */
 	error =  __mxt_read_reg(client,
 				object->start_address + MXT_T100_CFG1,
@@ -1718,6 +1821,8 @@ static int mxt_read_t100_config(struct mxt_data *data)
 		return error;
 
 	data->xy_switch = cfg & MXT_T100_CFG_SWITCHXY;
+	data->invertx = cfg & MXT_T100_CFG_INVERTX;
+	data->inverty = cfg & MXT_T100_CFG_INVERTY;
 
 	/* allocate aux bytes */
 	error =  __mxt_read_reg(client,
@@ -2043,6 +2148,420 @@ recheck:
 	return 0;
 }
 
+#ifdef CONFIG_TOUCHSCREEN_ATMEL_MXT_T37
+static u16 mxt_get_debug_value(struct mxt_data *data, unsigned int x,
+			       unsigned int y)
+{
+	struct mxt_info *info = &data->info;
+	struct mxt_dbg *dbg = &data->dbg;
+	unsigned int ofs, page;
+	unsigned int col = 0;
+	unsigned int col_width;
+
+	if (info->family_id == MXT_FAMILY_1386) {
+		col_width = info->matrix_ysize / MXT1386_COLUMNS;
+		col = y / col_width;
+		y = y % col_width;
+	} else {
+		col_width = info->matrix_ysize;
+	}
+
+	ofs = (y + (x * col_width)) * sizeof(u16);
+	page = ofs / MXT_DIAGNOSTIC_SIZE;
+	ofs %= MXT_DIAGNOSTIC_SIZE;
+
+	if (info->family_id == MXT_FAMILY_1386)
+		page += col * MXT1386_PAGES_PER_COLUMN;
+
+	return get_unaligned_le16(&dbg->t37_buf[page].data[ofs]);
+}
+
+static int mxt_convert_debug_pages(struct mxt_data *data, u16 *outbuf)
+{
+	struct mxt_dbg *dbg = &data->dbg;
+	unsigned int x = 0;
+	unsigned int y = 0;
+	unsigned int i, rx, ry;
+
+	for (i = 0; i < dbg->t37_nodes; i++) {
+		/* Handle orientation */
+		rx = data->xy_switch ? y : x;
+		ry = data->xy_switch ? x : y;
+		rx = data->invertx ? (data->xsize - 1 - rx) : rx;
+		ry = data->inverty ? (data->ysize - 1 - ry) : ry;
+
+		outbuf[i] = mxt_get_debug_value(data, rx, ry);
+
+		/* Next value */
+		if (++x >= (data->xy_switch ? data->ysize : data->xsize)) {
+			x = 0;
+			y++;
+		}
+	}
+
+	return 0;
+}
+
+static int mxt_read_diagnostic_debug(struct mxt_data *data, u8 mode,
+				     u16 *outbuf)
+{
+	struct mxt_dbg *dbg = &data->dbg;
+	int retries = 0;
+	int page;
+	int ret;
+	u8 cmd = mode;
+	struct t37_debug *p;
+	u8 cmd_poll;
+
+	for (page = 0; page < dbg->t37_pages; page++) {
+		p = dbg->t37_buf + page;
+
+		ret = mxt_write_reg(data->client, dbg->diag_cmd_address,
+				    cmd);
+		if (ret)
+			return ret;
+
+		retries = 0;
+		msleep(20);
+wait_cmd:
+		/* Read back command byte */
+		ret = __mxt_read_reg(data->client, dbg->diag_cmd_address,
+				     sizeof(cmd_poll), &cmd_poll);
+		if (ret)
+			return ret;
+
+		/* Field is cleared once the command has been processed */
+		if (cmd_poll) {
+			if (retries++ > 100)
+				return -EINVAL;
+
+			msleep(20);
+			goto wait_cmd;
+		}
+
+		/* Read T37 page */
+		ret = __mxt_read_reg(data->client, dbg->t37_address,
+				     sizeof(struct t37_debug), p);
+		if (ret)
+			return ret;
+
+		if (p->mode != mode || p->page != page) {
+			dev_err(&data->client->dev, "T37 page mismatch\n");
+			return -EINVAL;
+		}
+
+		dev_dbg(&data->client->dev, "%s page:%d retries:%d\n",
+			__func__, page, retries);
+
+		/* For remaining pages, write PAGEUP rather than mode */
+		cmd = MXT_DIAGNOSTIC_PAGEUP;
+	}
+
+	return mxt_convert_debug_pages(data, outbuf);
+}
+
+static int mxt_queue_setup(struct vb2_queue *q,
+		       unsigned int *nbuffers, unsigned int *nplanes,
+		       unsigned int sizes[], struct device *alloc_devs[])
+{
+	struct mxt_data *data = q->drv_priv;
+	size_t size = data->dbg.t37_nodes * sizeof(u16);
+
+	if (*nplanes)
+		return sizes[0] < size ? -EINVAL : 0;
+
+	*nplanes = 1;
+	sizes[0] = size;
+
+	return 0;
+}
+
+static void mxt_buffer_queue(struct vb2_buffer *vb)
+{
+	struct mxt_data *data = vb2_get_drv_priv(vb->vb2_queue);
+	u16 *ptr;
+	int ret;
+	u8 mode;
+
+	ptr = vb2_plane_vaddr(vb, 0);
+	if (!ptr) {
+		dev_err(&data->client->dev, "Error acquiring frame ptr\n");
+		goto fault;
+	}
+
+	switch (data->dbg.input) {
+	case MXT_V4L_INPUT_DELTAS:
+	default:
+		mode = MXT_DIAGNOSTIC_DELTAS;
+		break;
+
+	case MXT_V4L_INPUT_REFS:
+		mode = MXT_DIAGNOSTIC_REFS;
+		break;
+	}
+
+	ret = mxt_read_diagnostic_debug(data, mode, ptr);
+	if (ret)
+		goto fault;
+
+	vb2_set_plane_payload(vb, 0, data->dbg.t37_nodes * sizeof(u16));
+	vb2_buffer_done(vb, VB2_BUF_STATE_DONE);
+	return;
+
+fault:
+	vb2_buffer_done(vb, VB2_BUF_STATE_ERROR);
+}
+
+/* V4L2 structures */
+static const struct vb2_ops mxt_queue_ops = {
+	.queue_setup		= mxt_queue_setup,
+	.buf_queue		= mxt_buffer_queue,
+	.wait_prepare		= vb2_ops_wait_prepare,
+	.wait_finish		= vb2_ops_wait_finish,
+};
+
+static const struct vb2_queue mxt_queue = {
+	.type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
+	.io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF | VB2_READ,
+	.buf_struct_size = sizeof(struct mxt_vb2_buffer),
+	.ops = &mxt_queue_ops,
+	.mem_ops = &vb2_vmalloc_memops,
+	.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC,
+	.min_buffers_needed = 1,
+};
+
+static int mxt_vidioc_querycap(struct file *file, void *priv,
+				 struct v4l2_capability *cap)
+{
+	struct mxt_data *data = video_drvdata(file);
+
+	strlcpy(cap->driver, "atmel_mxt_ts", sizeof(cap->driver));
+	strlcpy(cap->card, "atmel_mxt_ts touch", sizeof(cap->card));
+	snprintf(cap->bus_info, sizeof(cap->bus_info),
+		 "I2C:%s", dev_name(&data->client->dev));
+	return 0;
+}
+
+static int mxt_vidioc_enum_input(struct file *file, void *priv,
+				   struct v4l2_input *i)
+{
+	if (i->index >= MXT_V4L_INPUT_MAX)
+		return -EINVAL;
+
+	i->type = V4L2_INPUT_TYPE_TOUCH;
+
+	switch (i->index) {
+	case MXT_V4L_INPUT_REFS:
+		strlcpy(i->name, "Mutual Capacitance References",
+			sizeof(i->name));
+		break;
+	case MXT_V4L_INPUT_DELTAS:
+		strlcpy(i->name, "Mutual Capacitance Deltas", sizeof(i->name));
+		break;
+	}
+
+	return 0;
+}
+
+static int mxt_set_input(struct mxt_data *data, unsigned int i)
+{
+	struct v4l2_pix_format *f = &data->dbg.format;
+
+	if (i >= MXT_V4L_INPUT_MAX)
+		return -EINVAL;
+
+	if (i == MXT_V4L_INPUT_DELTAS)
+		f->pixelformat = V4L2_TCH_FMT_DELTA_TD16;
+	else
+		f->pixelformat = V4L2_TCH_FMT_TU16;
+
+	f->width = data->xy_switch ? data->ysize : data->xsize;
+	f->height = data->xy_switch ? data->xsize : data->ysize;
+	f->field = V4L2_FIELD_NONE;
+	f->colorspace = V4L2_COLORSPACE_RAW;
+	f->bytesperline = f->width * sizeof(u16);
+	f->sizeimage = f->width * f->height * sizeof(u16);
+
+	data->dbg.input = i;
+
+	return 0;
+}
+
+static int mxt_vidioc_s_input(struct file *file, void *priv, unsigned int i)
+{
+	return mxt_set_input(video_drvdata(file), i);
+}
+
+static int mxt_vidioc_g_input(struct file *file, void *priv, unsigned int *i)
+{
+	struct mxt_data *data = video_drvdata(file);
+
+	*i = data->dbg.input;
+
+	return 0;
+}
+
+static int mxt_vidioc_fmt(struct file *file, void *priv, struct v4l2_format *f)
+{
+	struct mxt_data *data = video_drvdata(file);
+
+	f->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	f->fmt.pix = data->dbg.format;
+
+	return 0;
+}
+
+static int mxt_vidioc_enum_fmt(struct file *file, void *priv,
+				 struct v4l2_fmtdesc *fmt)
+{
+	if (fmt->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	switch (fmt->index) {
+	case 0:
+		fmt->pixelformat = V4L2_TCH_FMT_TU16;
+		break;
+
+	case 1:
+		fmt->pixelformat = V4L2_TCH_FMT_DELTA_TD16;
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int mxt_vidioc_g_parm(struct file *file, void *fh,
+			     struct v4l2_streamparm *a)
+{
+	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	a->parm.capture.readbuffers = 1;
+	a->parm.capture.timeperframe.numerator = 1;
+	a->parm.capture.timeperframe.denominator = 10;
+	return 0;
+}
+
+static const struct v4l2_ioctl_ops mxt_video_ioctl_ops = {
+	.vidioc_querycap        = mxt_vidioc_querycap,
+
+	.vidioc_enum_fmt_vid_cap = mxt_vidioc_enum_fmt,
+	.vidioc_s_fmt_vid_cap   = mxt_vidioc_fmt,
+	.vidioc_g_fmt_vid_cap   = mxt_vidioc_fmt,
+	.vidioc_try_fmt_vid_cap	= mxt_vidioc_fmt,
+	.vidioc_g_parm		= mxt_vidioc_g_parm,
+
+	.vidioc_enum_input      = mxt_vidioc_enum_input,
+	.vidioc_g_input         = mxt_vidioc_g_input,
+	.vidioc_s_input         = mxt_vidioc_s_input,
+
+	.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,
+};
+
+static const struct video_device mxt_video_device = {
+	.name = "Atmel maxTouch",
+	.fops = &mxt_video_fops,
+	.ioctl_ops = &mxt_video_ioctl_ops,
+	.release = video_device_release_empty,
+	.device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_TOUCH |
+		       V4L2_CAP_READWRITE | V4L2_CAP_STREAMING,
+};
+
+static void mxt_debug_init(struct mxt_data *data)
+{
+	struct mxt_info *info = &data->info;
+	struct mxt_dbg *dbg = &data->dbg;
+	struct mxt_object *object;
+	int error;
+
+	object = mxt_get_object(data, MXT_GEN_COMMAND_T6);
+	if (!object)
+		goto error;
+
+	dbg->diag_cmd_address = object->start_address + MXT_COMMAND_DIAGNOSTIC;
+
+	object = mxt_get_object(data, MXT_DEBUG_DIAGNOSTIC_T37);
+	if (!object)
+		goto error;
+
+	if (mxt_obj_size(object) != sizeof(struct t37_debug)) {
+		dev_warn(&data->client->dev, "Bad T37 size");
+		goto error;
+	}
+
+	dbg->t37_address = object->start_address;
+
+	/* Calculate size of data and allocate buffer */
+	dbg->t37_nodes = data->xsize * data->ysize;
+
+	if (info->family_id == MXT_FAMILY_1386)
+		dbg->t37_pages = MXT1386_COLUMNS * MXT1386_PAGES_PER_COLUMN;
+	else
+		dbg->t37_pages = DIV_ROUND_UP(data->xsize *
+					      data->info.matrix_ysize *
+					      sizeof(u16),
+					      sizeof(dbg->t37_buf->data));
+
+	dbg->t37_buf = devm_kmalloc_array(&data->client->dev, dbg->t37_pages,
+					  sizeof(struct t37_debug), GFP_KERNEL);
+	if (!dbg->t37_buf)
+		goto error;
+
+	/* init channel to zero */
+	mxt_set_input(data, 0);
+
+	/* register video device */
+	snprintf(dbg->v4l2.name, sizeof(dbg->v4l2.name), "%s", "atmel_mxt_ts");
+	error = v4l2_device_register(&data->client->dev, &dbg->v4l2);
+	if (error)
+		goto error;
+
+	/* initialize the queue */
+	mutex_init(&dbg->lock);
+	dbg->queue = mxt_queue;
+	dbg->queue.drv_priv = data;
+	dbg->queue.lock = &dbg->lock;
+	dbg->queue.dev = &data->client->dev;
+
+	error = vb2_queue_init(&dbg->queue);
+	if (error)
+		goto error_unreg_v4l2;
+
+	dbg->vdev = mxt_video_device;
+	dbg->vdev.v4l2_dev = &dbg->v4l2;
+	dbg->vdev.lock = &dbg->lock;
+	dbg->vdev.vfl_dir = VFL_DIR_RX;
+	dbg->vdev.queue = &dbg->queue;
+	video_set_drvdata(&dbg->vdev, data);
+
+	error = video_register_device(&dbg->vdev, VFL_TYPE_TOUCH, -1);
+	if (error)
+		goto error_unreg_v4l2;
+
+	return;
+
+error_unreg_v4l2:
+	v4l2_device_unregister(&dbg->v4l2);
+error:
+	dev_warn(&data->client->dev, "Error initializing T37\n");
+}
+#else
+static void mxt_debug_init(struct mxt_data *data)
+{
+}
+#endif
+
 static int mxt_configure_objects(struct mxt_data *data,
 				 const struct firmware *cfg)
 {
@@ -2070,6 +2589,8 @@ static int mxt_configure_objects(struct mxt_data *data,
 		dev_warn(dev, "No touch object detected\n");
 	}
 
+	mxt_debug_init(data);
+
 	dev_info(dev,
 		 "Family: %u Variant: %u Firmware V%u.%u.%02X Objects: %u\n",
 		 info->family_id, info->variant_id, info->version >> 4,
diff --git a/drivers/input/touchscreen/edt-ft5x06.c b/drivers/input/touchscreen/edt-ft5x06.c
index 703e295a37ed..28466e358fee 100644
--- a/drivers/input/touchscreen/edt-ft5x06.c
+++ b/drivers/input/touchscreen/edt-ft5x06.c
@@ -1063,9 +1063,15 @@ static const struct edt_i2c_chip_data edt_ft5506_data = {
 	.max_support_points = 10,
 };
 
+static const struct edt_i2c_chip_data edt_ft6236_data = {
+	.max_support_points = 2,
+};
+
 static const struct i2c_device_id edt_ft5x06_ts_id[] = {
 	{ .name = "edt-ft5x06", .driver_data = (long)&edt_ft5x06_data },
 	{ .name = "edt-ft5506", .driver_data = (long)&edt_ft5506_data },
+	/* Note no edt- prefix for compatibility with the ft6236.c driver */
+	{ .name = "ft6236", .driver_data = (long)&edt_ft6236_data },
 	{ /* sentinel */ }
 };
 MODULE_DEVICE_TABLE(i2c, edt_ft5x06_ts_id);
@@ -1076,6 +1082,8 @@ static const struct of_device_id edt_ft5x06_of_match[] = {
 	{ .compatible = "edt,edt-ft5306", .data = &edt_ft5x06_data },
 	{ .compatible = "edt,edt-ft5406", .data = &edt_ft5x06_data },
 	{ .compatible = "edt,edt-ft5506", .data = &edt_ft5506_data },
+	/* Note focaltech vendor prefix for compatibility with ft6236.c */
+	{ .compatible = "focaltech,ft6236", .data = &edt_ft6236_data },
 	{ /* sentinel */ }
 };
 MODULE_DEVICE_TABLE(of, edt_ft5x06_of_match);
diff --git a/drivers/input/touchscreen/ektf2127.c b/drivers/input/touchscreen/ektf2127.c
new file mode 100644
index 000000000000..0ed34ff787ce
--- /dev/null
+++ b/drivers/input/touchscreen/ektf2127.c
@@ -0,0 +1,336 @@
+/*
+ * Driver for ELAN eKTF2127 i2c touchscreen controller
+ *
+ * For this driver the layout of the Chipone icn8318 i2c
+ * touchscreencontroller is used.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * Author:
+ * Michel Verlaan <michel.verl@gmail.com>
+ * Siebren Vroegindeweij <siebren.vroegindeweij@hotmail.com>
+ *
+ * Original chipone_icn8318 driver:
+ * Hans de Goede <hdegoede@redhat.com>
+ */
+
+#include <linux/gpio/consumer.h>
+#include <linux/interrupt.h>
+#include <linux/i2c.h>
+#include <linux/input.h>
+#include <linux/input/mt.h>
+#include <linux/input/touchscreen.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/delay.h>
+
+/* Packet header defines (first byte of data send / received) */
+#define EKTF2127_NOISE			0x40
+#define EKTF2127_RESPONSE		0x52
+#define EKTF2127_REQUEST		0x53
+#define EKTF2127_HELLO			0x55
+#define EKTF2127_REPORT			0x5d
+#define EKTF2127_CALIB_DONE		0x66
+
+/* Register defines (second byte of data send / received) */
+#define EKTF2127_ENV_NOISY		0x41
+#define EKTF2127_HEIGHT			0x60
+#define EKTF2127_WIDTH			0x63
+
+/* 2 bytes header + 5 * 3 bytes coordinates + 3 bytes pressure info + footer */
+#define EKTF2127_TOUCH_REPORT_SIZE	21
+#define EKTF2127_MAX_TOUCHES		5
+
+struct ektf2127_ts {
+	struct i2c_client *client;
+	struct input_dev *input;
+	struct gpio_desc *power_gpios;
+	struct touchscreen_properties prop;
+};
+
+static void ektf2127_parse_coordinates(const u8* buf, unsigned int touch_count,
+				       struct input_mt_pos *touches)
+{
+	int index = 0;
+	int i;
+
+	for (i = 0; i < touch_count; i++) {
+		index = 2 + i * 3;
+
+		touches[i].x = (buf[index] & 0x0f);
+		touches[i].x <<= 8;
+		touches[i].x |= buf[index + 2];
+
+		touches[i].y = (buf[index] & 0xf0);
+		touches[i].y <<= 4;
+		touches[i].y |= buf[index + 1];
+	}
+}
+
+static void ektf2127_report_event(struct ektf2127_ts *ts, const u8 *buf)
+{
+	struct input_mt_pos touches[EKTF2127_MAX_TOUCHES];
+	int slots[EKTF2127_MAX_TOUCHES];
+	unsigned int touch_count, i;
+
+	touch_count = buf[1] & 0x07;
+	if (touch_count > EKTF2127_MAX_TOUCHES) {
+		dev_err(&ts->client->dev,
+			"Too many touches %d > %d\n",
+			touch_count, EKTF2127_MAX_TOUCHES);
+		touch_count = EKTF2127_MAX_TOUCHES;
+	}
+
+	ektf2127_parse_coordinates(buf, touch_count, touches);
+	input_mt_assign_slots(ts->input, slots, touches,
+			      touch_count, 0);
+
+	for (i = 0; i < touch_count; i++) {
+		input_mt_slot(ts->input, slots[i]);
+		input_mt_report_slot_state(ts->input, MT_TOOL_FINGER, true);
+		touchscreen_report_pos(ts->input, &ts->prop,
+				       touches[i].x, touches[i].y, true);
+	}
+
+	input_mt_sync_frame(ts->input);
+	input_sync(ts->input);
+}
+
+static irqreturn_t ektf2127_irq(int irq, void *dev_id)
+{
+	struct ektf2127_ts *ts = dev_id;
+	struct device *dev = &ts->client->dev;
+	char buf[EKTF2127_TOUCH_REPORT_SIZE];
+	int ret;
+
+	ret = i2c_master_recv(ts->client, buf, EKTF2127_TOUCH_REPORT_SIZE);
+	if (ret != EKTF2127_TOUCH_REPORT_SIZE) {
+		dev_err(dev, "Error reading touch data: %d\n", ret);
+		goto out;
+	}
+
+	switch (buf[0]) {
+	case EKTF2127_REPORT:
+		ektf2127_report_event(ts, buf);
+		break;
+
+	case EKTF2127_NOISE:
+		if (buf[1] == EKTF2127_ENV_NOISY)
+			dev_dbg(dev, "Environment is electrically noisy\n");
+		break;
+
+	case EKTF2127_HELLO:
+	case EKTF2127_CALIB_DONE:
+		break;
+
+	default:
+		dev_err(dev, "Unexpected packet header byte %#02x\n", buf[0]);
+		break;
+	}
+
+out:
+	return IRQ_HANDLED;
+}
+
+static int ektf2127_start(struct input_dev *dev)
+{
+	struct ektf2127_ts *ts = input_get_drvdata(dev);
+
+	enable_irq(ts->client->irq);
+	gpiod_set_value_cansleep(ts->power_gpios, 1);
+
+	return 0;
+}
+
+static void ektf2127_stop(struct input_dev *dev)
+{
+	struct ektf2127_ts *ts = input_get_drvdata(dev);
+
+	disable_irq(ts->client->irq);
+	gpiod_set_value_cansleep(ts->power_gpios, 0);
+}
+
+static int __maybe_unused ektf2127_suspend(struct device *dev)
+{
+	struct ektf2127_ts *ts = i2c_get_clientdata(to_i2c_client(dev));
+
+	mutex_lock(&ts->input->mutex);
+	if (ts->input->users)
+		ektf2127_stop(ts->input);
+	mutex_unlock(&ts->input->mutex);
+
+	return 0;
+}
+
+static int __maybe_unused ektf2127_resume(struct device *dev)
+{
+	struct ektf2127_ts *ts = i2c_get_clientdata(to_i2c_client(dev));
+
+	mutex_lock(&ts->input->mutex);
+	if (ts->input->users)
+		ektf2127_start(ts->input);
+	mutex_unlock(&ts->input->mutex);
+
+	return 0;
+}
+
+static SIMPLE_DEV_PM_OPS(ektf2127_pm_ops, ektf2127_suspend,
+			 ektf2127_resume);
+
+static int ektf2127_query_dimension(struct i2c_client *client, bool width)
+{
+	struct device *dev = &client->dev;
+	const char *what = width ? "width" : "height";
+	u8 what_code = width ? EKTF2127_WIDTH : EKTF2127_HEIGHT;
+	u8 buf[4];
+	int ret;
+	int error;
+
+	/* Request dimension */
+	buf[0] = EKTF2127_REQUEST;
+	buf[1] = width ? EKTF2127_WIDTH : EKTF2127_HEIGHT;
+	buf[2] = 0x00;
+	buf[3] = 0x00;
+	ret = i2c_master_send(client, buf, sizeof(buf));
+	if (ret != sizeof(buf)) {
+		error = ret < 0 ? ret : -EIO;
+		dev_err(dev, "Failed to request %s: %d\n", what, error);
+		return error;
+	}
+
+	msleep(20);
+
+	/* Read response */
+	ret = i2c_master_recv(client, buf, sizeof(buf));
+	if (ret != sizeof(buf)) {
+		error = ret < 0 ? ret : -EIO;
+		dev_err(dev, "Failed to receive %s data: %d\n", what, error);
+		return error;
+	}
+
+	if (buf[0] != EKTF2127_RESPONSE || buf[1] != what_code) {
+		dev_err(dev, "Unexpected %s data: %#02x %#02x\n",
+			what, buf[0], buf[1]);
+		return -EIO;
+	}
+
+	return (((buf[3] & 0xf0) << 4) | buf[2]) - 1;
+}
+
+static int ektf2127_probe(struct i2c_client *client,
+			  const struct i2c_device_id *id)
+{
+	struct device *dev = &client->dev;
+	struct ektf2127_ts *ts;
+	struct input_dev *input;
+	u8 buf[4];
+	int max_x, max_y;
+	int error;
+
+	if (!client->irq) {
+		dev_err(dev, "Error no irq specified\n");
+		return -EINVAL;
+	}
+
+	ts = devm_kzalloc(dev, sizeof(*ts), GFP_KERNEL);
+	if (!ts)
+		return -ENOMEM;
+
+	/* This requests the gpio *and* turns on the touchscreen controller */
+	ts->power_gpios = devm_gpiod_get(dev, "power", GPIOD_OUT_HIGH);
+	if (IS_ERR(ts->power_gpios)) {
+		error = PTR_ERR(ts->power_gpios);
+		if (error != -EPROBE_DEFER)
+			dev_err(dev, "Error getting power gpio: %d\n", error);
+		return error;
+	}
+
+	input = devm_input_allocate_device(dev);
+	if (!input)
+		return -ENOMEM;
+
+	input->name = client->name;
+	input->id.bustype = BUS_I2C;
+	input->open = ektf2127_start;
+	input->close = ektf2127_stop;
+
+	ts->client = client;
+
+	/* Read hello (ignore result, depends on initial power state) */
+	msleep(20);
+	i2c_master_recv(ts->client, buf, sizeof(buf));
+
+	/* Read resolution from chip */
+	max_x = ektf2127_query_dimension(client, true);
+	if (max_x < 0)
+		return max_x;
+
+	max_y = ektf2127_query_dimension(client, false);
+	if (max_y < 0)
+		return max_y;
+
+	input_set_abs_params(input, ABS_MT_POSITION_X, 0, max_x, 0, 0);
+	input_set_abs_params(input, ABS_MT_POSITION_Y, 0, max_y, 0, 0);
+	touchscreen_parse_properties(input, true, &ts->prop);
+
+	error = input_mt_init_slots(input, EKTF2127_MAX_TOUCHES,
+				    INPUT_MT_DIRECT |
+					INPUT_MT_DROP_UNUSED |
+					INPUT_MT_TRACK);
+	if (error)
+		return error;
+
+	ts->input = input;
+	input_set_drvdata(input, ts);
+
+	error = devm_request_threaded_irq(dev, client->irq,
+					  NULL, ektf2127_irq,
+					  IRQF_ONESHOT, client->name, ts);
+	if (error) {
+		dev_err(dev, "Error requesting irq: %d\n", error);
+		return error;
+	}
+
+	/* Stop device till opened */
+	ektf2127_stop(ts->input);
+
+	error = input_register_device(input);
+	if (error)
+		return error;
+
+	i2c_set_clientdata(client, ts);
+
+	return 0;
+}
+
+#ifdef CONFIG_OF
+static const struct of_device_id ektf2127_of_match[] = {
+	{ .compatible = "elan,ektf2127" },
+	{}
+};
+MODULE_DEVICE_TABLE(of, ektf2127_of_match);
+#endif
+
+static const struct i2c_device_id ektf2127_i2c_id[] = {
+	{ "ektf2127", 0 },
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, ektf2127_i2c_id);
+
+static struct i2c_driver ektf2127_driver = {
+	.driver = {
+		.name	= "elan_ektf2127",
+		.pm	= &ektf2127_pm_ops,
+		.of_match_table = of_match_ptr(ektf2127_of_match),
+	},
+	.probe = ektf2127_probe,
+	.id_table = ektf2127_i2c_id,
+};
+module_i2c_driver(ektf2127_driver);
+
+MODULE_DESCRIPTION("ELAN eKTF2127 I2C Touchscreen Driver");
+MODULE_AUTHOR("Michel Verlaan, Siebren Vroegindeweij");
+MODULE_LICENSE("GPL");
diff --git a/drivers/input/touchscreen/elants_i2c.c b/drivers/input/touchscreen/elants_i2c.c
index ac09855fa435..02aec284deca 100644
--- a/drivers/input/touchscreen/elants_i2c.c
+++ b/drivers/input/touchscreen/elants_i2c.c
@@ -298,7 +298,7 @@ static u16 elants_i2c_parse_version(u8 *buf)
 	return get_unaligned_be32(buf) >> 4;
 }
 
-static int elants_i2c_query_fw_id(struct elants_data *ts)
+static int elants_i2c_query_hw_version(struct elants_data *ts)
 {
 	struct i2c_client *client = ts->client;
 	int error, retry_cnt;
@@ -318,8 +318,13 @@ static int elants_i2c_query_fw_id(struct elants_data *ts)
 			error, (int)sizeof(resp), resp);
 	}
 
-	dev_err(&client->dev,
-		"Failed to read fw id or fw id is invalid\n");
+	if (error) {
+		dev_err(&client->dev,
+			"Failed to read fw id: %d\n", error);
+		return error;
+	}
+
+	dev_err(&client->dev, "Invalid fw id: %#04x\n", ts->hw_version);
 
 	return -EINVAL;
 }
@@ -508,7 +513,7 @@ static int elants_i2c_fastboot(struct i2c_client *client)
 static int elants_i2c_initialize(struct elants_data *ts)
 {
 	struct i2c_client *client = ts->client;
-	int error, retry_cnt;
+	int error, error2, retry_cnt;
 	const u8 hello_packet[] = { 0x55, 0x55, 0x55, 0x55 };
 	const u8 recov_packet[] = { 0x55, 0x55, 0x80, 0x80 };
 	u8 buf[HEADER_SIZE];
@@ -553,18 +558,22 @@ static int elants_i2c_initialize(struct elants_data *ts)
 		}
 	}
 
+	/* hw version is available even if device in recovery state */
+	error2 = elants_i2c_query_hw_version(ts);
 	if (!error)
-		error = elants_i2c_query_fw_id(ts);
+		error = error2;
+
 	if (!error)
 		error = elants_i2c_query_fw_version(ts);
+	if (!error)
+		error = elants_i2c_query_test_version(ts);
+	if (!error)
+		error = elants_i2c_query_bc_version(ts);
+	if (!error)
+		error = elants_i2c_query_ts_info(ts);
 
-	if (error) {
+	if (error)
 		ts->iap_mode = ELAN_IAP_RECOVERY;
-	} else {
-		elants_i2c_query_test_version(ts);
-		elants_i2c_query_bc_version(ts);
-		elants_i2c_query_ts_info(ts);
-	}
 
 	return 0;
 }
diff --git a/drivers/input/touchscreen/ft6236.c b/drivers/input/touchscreen/ft6236.c
deleted file mode 100644
index d240d2e212bd..000000000000
--- a/drivers/input/touchscreen/ft6236.c
+++ /dev/null
@@ -1,326 +0,0 @@
-/*
- * FocalTech FT6236 TouchScreen driver.
- *
- * Copyright (c) 2010  Focal tech Ltd.
- *
- * This software is licensed under the terms of the GNU General Public
- * License version 2, as published by the Free Software Foundation, and
- * may be copied, distributed, and modified under those terms.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- */
-
-#include <linux/delay.h>
-#include <linux/gpio/consumer.h>
-#include <linux/i2c.h>
-#include <linux/input.h>
-#include <linux/input/mt.h>
-#include <linux/interrupt.h>
-#include <linux/module.h>
-#include <linux/property.h>
-
-#define FT6236_MAX_TOUCH_POINTS		2
-
-#define FT6236_REG_TH_GROUP		0x80
-#define FT6236_REG_PERIODACTIVE		0x88
-#define FT6236_REG_LIB_VER_H		0xa1
-#define FT6236_REG_LIB_VER_L		0xa2
-#define FT6236_REG_CIPHER		0xa3
-#define FT6236_REG_FIRMID		0xa6
-#define FT6236_REG_FOCALTECH_ID		0xa8
-#define FT6236_REG_RELEASE_CODE_ID	0xaf
-
-#define FT6236_EVENT_PRESS_DOWN		0
-#define FT6236_EVENT_LIFT_UP		1
-#define FT6236_EVENT_CONTACT		2
-#define FT6236_EVENT_NO_EVENT		3
-
-struct ft6236_data {
-	struct i2c_client *client;
-	struct input_dev *input;
-	struct gpio_desc *reset_gpio;
-	u32 max_x;
-	u32 max_y;
-	bool invert_x;
-	bool invert_y;
-	bool swap_xy;
-};
-
-/*
- * This struct is a touchpoint as stored in hardware.  Note that the id,
- * as well as the event, are stored in the upper nybble of the hi byte.
- */
-struct ft6236_touchpoint {
-	union {
-		u8 xhi;
-		u8 event;
-	};
-	u8 xlo;
-	union {
-		u8 yhi;
-		u8 id;
-	};
-	u8 ylo;
-	u8 weight;
-	u8 misc;
-} __packed;
-
-/* This packet represents the register map as read from offset 0 */
-struct ft6236_packet {
-	u8 dev_mode;
-	u8 gest_id;
-	u8 touches;
-	struct ft6236_touchpoint points[FT6236_MAX_TOUCH_POINTS];
-} __packed;
-
-static int ft6236_read(struct i2c_client *client, u8 reg, u8 len, void *data)
-{
-	int error;
-
-	error = i2c_smbus_read_i2c_block_data(client, reg, len, data);
-	if (error < 0)
-		return error;
-
-	if (error != len)
-		return -EIO;
-
-	return 0;
-}
-
-static irqreturn_t ft6236_interrupt(int irq, void *dev_id)
-{
-	struct ft6236_data *ft6236 = dev_id;
-	struct device *dev = &ft6236->client->dev;
-	struct input_dev *input = ft6236->input;
-	struct ft6236_packet buf;
-	u8 touches;
-	int i, error;
-
-	error = ft6236_read(ft6236->client, 0, sizeof(buf), &buf);
-	if (error) {
-		dev_err(dev, "read touchdata failed %d\n", error);
-		return IRQ_HANDLED;
-	}
-
-	touches = buf.touches & 0xf;
-	if (touches > FT6236_MAX_TOUCH_POINTS) {
-		dev_dbg(dev,
-			"%d touch points reported, only %d are supported\n",
-			touches, FT6236_MAX_TOUCH_POINTS);
-		touches = FT6236_MAX_TOUCH_POINTS;
-	}
-
-	for (i = 0; i < touches; i++) {
-		struct ft6236_touchpoint *point = &buf.points[i];
-		u16 x = ((point->xhi & 0xf) << 8) | buf.points[i].xlo;
-		u16 y = ((point->yhi & 0xf) << 8) | buf.points[i].ylo;
-		u8 event = point->event >> 6;
-		u8 id = point->id >> 4;
-		bool act = (event == FT6236_EVENT_PRESS_DOWN ||
-			    event == FT6236_EVENT_CONTACT);
-
-		input_mt_slot(input, id);
-		input_mt_report_slot_state(input, MT_TOOL_FINGER, act);
-		if (!act)
-			continue;
-
-		if (ft6236->invert_x)
-			x = ft6236->max_x - x;
-
-		if (ft6236->invert_y)
-			y = ft6236->max_y - y;
-
-		if (ft6236->swap_xy) {
-			input_report_abs(input, ABS_MT_POSITION_X, y);
-			input_report_abs(input, ABS_MT_POSITION_Y, x);
-		} else {
-			input_report_abs(input, ABS_MT_POSITION_X, x);
-			input_report_abs(input, ABS_MT_POSITION_Y, y);
-		}
-	}
-
-	input_mt_sync_frame(input);
-	input_sync(input);
-
-	return IRQ_HANDLED;
-}
-
-static u8 ft6236_debug_read_byte(struct ft6236_data *ft6236, u8 reg)
-{
-	struct i2c_client *client = ft6236->client;
-	u8 val = 0;
-	int error;
-
-	error = ft6236_read(client, reg, 1, &val);
-	if (error)
-		dev_dbg(&client->dev,
-			"error reading register 0x%02x: %d\n", reg, error);
-
-	return val;
-}
-
-static void ft6236_debug_info(struct ft6236_data *ft6236)
-{
-	struct device *dev = &ft6236->client->dev;
-
-	dev_dbg(dev, "Touch threshold is %d\n",
-		ft6236_debug_read_byte(ft6236, FT6236_REG_TH_GROUP) * 4);
-	dev_dbg(dev, "Report rate is %dHz\n",
-		ft6236_debug_read_byte(ft6236, FT6236_REG_PERIODACTIVE) * 10);
-	dev_dbg(dev, "Firmware library version 0x%02x%02x\n",
-		ft6236_debug_read_byte(ft6236, FT6236_REG_LIB_VER_H),
-		ft6236_debug_read_byte(ft6236, FT6236_REG_LIB_VER_L));
-	dev_dbg(dev, "Firmware version 0x%02x\n",
-		ft6236_debug_read_byte(ft6236, FT6236_REG_FIRMID));
-	dev_dbg(dev, "Chip vendor ID 0x%02x\n",
-		ft6236_debug_read_byte(ft6236, FT6236_REG_CIPHER));
-	dev_dbg(dev, "CTPM vendor ID 0x%02x\n",
-		ft6236_debug_read_byte(ft6236, FT6236_REG_FOCALTECH_ID));
-	dev_dbg(dev, "Release code version 0x%02x\n",
-		ft6236_debug_read_byte(ft6236, FT6236_REG_RELEASE_CODE_ID));
-}
-
-static void ft6236_reset(struct ft6236_data *ft6236)
-{
-	if (!ft6236->reset_gpio)
-		return;
-
-	gpiod_set_value_cansleep(ft6236->reset_gpio, 1);
-	usleep_range(5000, 20000);
-	gpiod_set_value_cansleep(ft6236->reset_gpio, 0);
-	msleep(300);
-}
-
-static int ft6236_probe(struct i2c_client *client,
-			const struct i2c_device_id *id)
-{
-	struct device *dev = &client->dev;
-	struct ft6236_data *ft6236;
-	struct input_dev *input;
-	u32 fuzz_x = 0, fuzz_y = 0;
-	u8 val;
-	int error;
-
-	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C))
-		return -ENXIO;
-
-	if (!client->irq) {
-		dev_err(dev, "irq is missing\n");
-		return -EINVAL;
-	}
-
-	ft6236 = devm_kzalloc(dev, sizeof(*ft6236), GFP_KERNEL);
-	if (!ft6236)
-		return -ENOMEM;
-
-	ft6236->client = client;
-	ft6236->reset_gpio = devm_gpiod_get_optional(dev, "reset",
-						     GPIOD_OUT_LOW);
-	if (IS_ERR(ft6236->reset_gpio)) {
-		error = PTR_ERR(ft6236->reset_gpio);
-		if (error != -EPROBE_DEFER)
-			dev_err(dev, "error getting reset gpio: %d\n", error);
-		return error;
-	}
-
-	ft6236_reset(ft6236);
-
-	/* verify that the controller is present */
-	error = ft6236_read(client, 0x00, 1, &val);
-	if (error) {
-		dev_err(dev, "failed to read from controller: %d\n", error);
-		return error;
-	}
-
-	ft6236_debug_info(ft6236);
-
-	input = devm_input_allocate_device(dev);
-	if (!input)
-		return -ENOMEM;
-
-	ft6236->input = input;
-	input->name = client->name;
-	input->id.bustype = BUS_I2C;
-
-	if (device_property_read_u32(dev, "touchscreen-size-x",
-				     &ft6236->max_x) ||
-	    device_property_read_u32(dev, "touchscreen-size-y",
-				     &ft6236->max_y)) {
-		dev_err(dev, "touchscreen-size-x and/or -y missing\n");
-		return -EINVAL;
-	}
-
-	device_property_read_u32(dev, "touchscreen-fuzz-x", &fuzz_x);
-	device_property_read_u32(dev, "touchscreen-fuzz-y", &fuzz_y);
-	ft6236->invert_x = device_property_read_bool(dev,
-						     "touchscreen-inverted-x");
-	ft6236->invert_y = device_property_read_bool(dev,
-						     "touchscreen-inverted-y");
-	ft6236->swap_xy = device_property_read_bool(dev,
-						    "touchscreen-swapped-x-y");
-
-	if (ft6236->swap_xy) {
-		input_set_abs_params(input, ABS_MT_POSITION_X, 0,
-				     ft6236->max_y, fuzz_y, 0);
-		input_set_abs_params(input, ABS_MT_POSITION_Y, 0,
-				     ft6236->max_x, fuzz_x, 0);
-	} else {
-		input_set_abs_params(input, ABS_MT_POSITION_X, 0,
-				     ft6236->max_x, fuzz_x, 0);
-		input_set_abs_params(input, ABS_MT_POSITION_Y, 0,
-				     ft6236->max_y, fuzz_y, 0);
-	}
-
-	error = input_mt_init_slots(input, FT6236_MAX_TOUCH_POINTS,
-				    INPUT_MT_DIRECT | INPUT_MT_DROP_UNUSED);
-	if (error)
-		return error;
-
-	error = devm_request_threaded_irq(dev, client->irq, NULL,
-					  ft6236_interrupt, IRQF_ONESHOT,
-					  client->name, ft6236);
-	if (error) {
-		dev_err(dev, "request irq %d failed: %d\n", client->irq, error);
-		return error;
-	}
-
-	error = input_register_device(input);
-	if (error) {
-		dev_err(dev, "failed to register input device: %d\n", error);
-		return error;
-	}
-
-	return 0;
-}
-
-#ifdef CONFIG_OF
-static const struct of_device_id ft6236_of_match[] = {
-	{ .compatible = "focaltech,ft6236", },
-	{ }
-};
-MODULE_DEVICE_TABLE(of, ft6236_of_match);
-#endif
-
-static const struct i2c_device_id ft6236_id[] = {
-	{ "ft6236", },
-	{ }
-};
-MODULE_DEVICE_TABLE(i2c, ft6236_id);
-
-static struct i2c_driver ft6236_driver = {
-	.driver = {
-		.name = "ft6236",
-		.of_match_table = of_match_ptr(ft6236_of_match),
-	},
-	.probe = ft6236_probe,
-	.id_table = ft6236_id,
-};
-module_i2c_driver(ft6236_driver);
-
-MODULE_AUTHOR("Sean Cross <xobs@kosagi.com>");
-MODULE_AUTHOR("Noralf Trønnes <noralf@tronnes.org>");
-MODULE_DESCRIPTION("FocalTech FT6236 TouchScreen driver");
-MODULE_LICENSE("GPL v2");
diff --git a/drivers/input/touchscreen/jornada720_ts.c b/drivers/input/touchscreen/jornada720_ts.c
index ea3b6a5b83e6..729b3c89324c 100644
--- a/drivers/input/touchscreen/jornada720_ts.c
+++ b/drivers/input/touchscreen/jornada720_ts.c
@@ -13,6 +13,7 @@
  * HP Jornada 710/720/729 Touchscreen Driver
  */
 
+#include <linux/gpio/consumer.h>
 #include <linux/platform_device.h>
 #include <linux/input.h>
 #include <linux/interrupt.h>
@@ -20,9 +21,7 @@
 #include <linux/slab.h>
 #include <linux/io.h>
 
-#include <mach/hardware.h>
 #include <mach/jornada720.h>
-#include <mach/irqs.h>
 
 MODULE_AUTHOR("Kristoffer Ericson <kristoffer.ericson@gmail.com>");
 MODULE_DESCRIPTION("HP Jornada 710/720/728 touchscreen driver");
@@ -30,6 +29,7 @@ MODULE_LICENSE("GPL v2");
 
 struct jornada_ts {
 	struct input_dev *dev;
+	struct gpio_desc *gpio;
 	int x_data[4];		/* X sample values */
 	int y_data[4];		/* Y sample values */
 };
@@ -71,8 +71,8 @@ static irqreturn_t jornada720_ts_interrupt(int irq, void *dev_id)
 	struct input_dev *input = jornada_ts->dev;
 	int x, y;
 
-	/* If GPIO_GPIO9 is set to high then report pen up */
-	if (GPLR & GPIO_GPIO(9)) {
+	/* If gpio is high then report pen up */
+	if (gpiod_get_value(jornada_ts->gpio)) {
 		input_report_key(input, BTN_TOUCH, 0);
 		input_sync(input);
 	} else {
@@ -101,7 +101,7 @@ static int jornada720_ts_probe(struct platform_device *pdev)
 {
 	struct jornada_ts *jornada_ts;
 	struct input_dev *input_dev;
-	int error;
+	int error, irq;
 
 	jornada_ts = devm_kzalloc(&pdev->dev, sizeof(*jornada_ts), GFP_KERNEL);
 	if (!jornada_ts)
@@ -113,6 +113,14 @@ static int jornada720_ts_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, jornada_ts);
 
+	jornada_ts->gpio = devm_gpiod_get(&pdev->dev, "penup", GPIOD_IN);
+	if (IS_ERR(jornada_ts->gpio))
+		return PTR_ERR(jornada_ts->gpio);
+
+	irq = gpiod_to_irq(jornada_ts->gpio);
+	if (irq <= 0)
+		return irq < 0 ? irq : -EINVAL;
+
 	jornada_ts->dev = input_dev;
 
 	input_dev->name = "HP Jornada 7xx Touchscreen";
@@ -125,8 +133,7 @@ static int jornada720_ts_probe(struct platform_device *pdev)
 	input_set_abs_params(input_dev, ABS_X, 270, 3900, 0, 0);
 	input_set_abs_params(input_dev, ABS_Y, 180, 3700, 0, 0);
 
-	error = devm_request_irq(&pdev->dev, IRQ_GPIO9,
-				 jornada720_ts_interrupt,
+	error = devm_request_irq(&pdev->dev, irq, jornada720_ts_interrupt,
 				 IRQF_TRIGGER_RISING,
 				 "HP7XX Touchscreen driver", pdev);
 	if (error) {
diff --git a/drivers/input/touchscreen/mc13783_ts.c b/drivers/input/touchscreen/mc13783_ts.c
index 913e25a994b4..ef64f36c5ffc 100644
--- a/drivers/input/touchscreen/mc13783_ts.c
+++ b/drivers/input/touchscreen/mc13783_ts.c
@@ -37,7 +37,6 @@ struct mc13783_ts_priv {
 	struct input_dev *idev;
 	struct mc13xxx *mc13xxx;
 	struct delayed_work work;
-	struct workqueue_struct *workq;
 	unsigned int sample[4];
 	struct mc13xxx_ts_platform_data *touch;
 };
@@ -54,7 +53,7 @@ static irqreturn_t mc13783_ts_handler(int irq, void *data)
 	 * be rescheduled for immediate execution here. However the rearm
 	 * delay is HZ / 50 which is acceptable.
 	 */
-	queue_delayed_work(priv->workq, &priv->work, 0);
+	schedule_delayed_work(&priv->work, 0);
 
 	return IRQ_HANDLED;
 }
@@ -106,16 +105,18 @@ static void mc13783_ts_report_sample(struct mc13783_ts_priv *priv)
 
 			dev_dbg(&idev->dev, "report (%d, %d, %d)\n",
 					x1, y1, 0x1000 - cr0);
-			queue_delayed_work(priv->workq, &priv->work, HZ / 50);
-		} else
+			schedule_delayed_work(&priv->work, HZ / 50);
+		} else {
 			dev_dbg(&idev->dev, "report release\n");
+		}
 
 		input_report_abs(idev, ABS_PRESSURE,
 				cr0 ? 0x1000 - cr0 : cr0);
 		input_report_key(idev, BTN_TOUCH, cr0);
 		input_sync(idev);
-	} else
+	} else {
 		dev_dbg(&idev->dev, "discard event\n");
+	}
 }
 
 static void mc13783_ts_work(struct work_struct *work)
@@ -189,14 +190,6 @@ static int __init mc13783_ts_probe(struct platform_device *pdev)
 		goto err_free_mem;
 	}
 
-	/*
-	 * We need separate workqueue because mc13783_adc_do_conversion
-	 * uses keventd and thus would deadlock.
-	 */
-	priv->workq = create_singlethread_workqueue("mc13783_ts");
-	if (!priv->workq)
-		goto err_free_mem;
-
 	idev->name = MC13783_TS_NAME;
 	idev->dev.parent = &pdev->dev;
 
@@ -215,14 +208,12 @@ static int __init mc13783_ts_probe(struct platform_device *pdev)
 	if (ret) {
 		dev_err(&pdev->dev,
 			"register input device failed with %d\n", ret);
-		goto err_destroy_wq;
+		goto err_free_mem;
 	}
 
 	platform_set_drvdata(pdev, priv);
 	return 0;
 
-err_destroy_wq:
-	destroy_workqueue(priv->workq);
 err_free_mem:
 	input_free_device(idev);
 	kfree(priv);
@@ -233,7 +224,6 @@ static int mc13783_ts_remove(struct platform_device *pdev)
 {
 	struct mc13783_ts_priv *priv = platform_get_drvdata(pdev);
 
-	destroy_workqueue(priv->workq);
 	input_unregister_device(priv->idev);
 	kfree(priv);
 
diff --git a/drivers/input/touchscreen/melfas_mip4.c b/drivers/input/touchscreen/melfas_mip4.c
index fb5fb9140ca9..552a3773f79d 100644
--- a/drivers/input/touchscreen/melfas_mip4.c
+++ b/drivers/input/touchscreen/melfas_mip4.c
@@ -157,6 +157,7 @@ struct mip4_ts {
 
 	char phys[32];
 	char product_name[16];
+	char ic_name[4];
 
 	unsigned int max_x;
 	unsigned int max_y;
@@ -263,6 +264,18 @@ static int mip4_query_device(struct mip4_ts *ts)
 		dev_dbg(&ts->client->dev, "product name: %.*s\n",
 			(int)sizeof(ts->product_name), ts->product_name);
 
+	/* IC name */
+	cmd[0] = MIP4_R0_INFO;
+	cmd[1] = MIP4_R1_INFO_IC_NAME;
+	error = mip4_i2c_xfer(ts, cmd, sizeof(cmd),
+			      ts->ic_name, sizeof(ts->ic_name));
+	if (error)
+		dev_warn(&ts->client->dev,
+			 "Failed to retrieve IC name: %d\n", error);
+	else
+		dev_dbg(&ts->client->dev, "IC name: %.*s\n",
+			(int)sizeof(ts->ic_name), ts->ic_name);
+
 	/* Firmware version */
 	error = mip4_get_fw_version(ts);
 	if (error)
@@ -1326,7 +1339,7 @@ static ssize_t mip4_sysfs_read_hw_version(struct device *dev,
 	 * paired with current firmware in the chip.
 	 */
 	count = snprintf(buf, PAGE_SIZE, "%.*s\n",
-		(int)sizeof(ts->product_name), ts->product_name);
+			 (int)sizeof(ts->product_name), ts->product_name);
 
 	mutex_unlock(&ts->input->mutex);
 
@@ -1335,9 +1348,30 @@ static ssize_t mip4_sysfs_read_hw_version(struct device *dev,
 
 static DEVICE_ATTR(hw_version, S_IRUGO, mip4_sysfs_read_hw_version, NULL);
 
+static ssize_t mip4_sysfs_read_ic_name(struct device *dev,
+					  struct device_attribute *attr,
+					  char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct mip4_ts *ts = i2c_get_clientdata(client);
+	size_t count;
+
+	mutex_lock(&ts->input->mutex);
+
+	count = snprintf(buf, PAGE_SIZE, "%.*s\n",
+			 (int)sizeof(ts->ic_name), ts->ic_name);
+
+	mutex_unlock(&ts->input->mutex);
+
+	return count;
+}
+
+static DEVICE_ATTR(ic_name, S_IRUGO, mip4_sysfs_read_ic_name, NULL);
+
 static struct attribute *mip4_attrs[] = {
 	&dev_attr_fw_version.attr,
 	&dev_attr_hw_version.attr,
+	&dev_attr_ic_name.attr,
 	&dev_attr_update_fw.attr,
 	NULL,
 };
@@ -1538,6 +1572,6 @@ static struct i2c_driver mip4_driver = {
 module_i2c_driver(mip4_driver);
 
 MODULE_DESCRIPTION("MELFAS MIP4 Touchscreen");
-MODULE_VERSION("2016.03.12");
+MODULE_VERSION("2016.09.28");
 MODULE_AUTHOR("Sangwon Jee <jeesw@melfas.com>");
 MODULE_LICENSE("GPL");
diff --git a/drivers/input/touchscreen/pixcir_i2c_ts.c b/drivers/input/touchscreen/pixcir_i2c_ts.c
index d159e14f4d20..3bb0637d832e 100644
--- a/drivers/input/touchscreen/pixcir_i2c_ts.c
+++ b/drivers/input/touchscreen/pixcir_i2c_ts.c
@@ -11,10 +11,6 @@
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public
- * License along with this library; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
  */
 
 #include <linux/delay.h>
@@ -404,7 +400,6 @@ static int __maybe_unused pixcir_i2c_ts_resume(struct device *dev)
 	mutex_lock(&input->mutex);
 
 	if (device_may_wakeup(&client->dev)) {
-
 		if (!input->users) {
 			ret = pixcir_stop(ts);
 			if (ret) {
@@ -431,13 +426,7 @@ static const struct of_device_id pixcir_of_match[];
 static int pixcir_parse_dt(struct device *dev,
 			   struct pixcir_i2c_ts_data *tsdata)
 {
-	const struct of_device_id *match;
-
-	match = of_match_device(of_match_ptr(pixcir_of_match), dev);
-	if (!match)
-		return -EINVAL;
-
-	tsdata->chip = (const struct pixcir_i2c_chip_data *)match->data;
+	tsdata->chip = of_device_get_match_data(dev);
 	if (!tsdata->chip)
 		return -EINVAL;
 
diff --git a/drivers/input/touchscreen/sur40.c b/drivers/input/touchscreen/sur40.c
index 4ea475775d58..aefb6e11f88a 100644
--- a/drivers/input/touchscreen/sur40.c
+++ b/drivers/input/touchscreen/sur40.c
@@ -139,6 +139,27 @@ struct sur40_image_header {
 #define SUR40_GET_STATE   0xc5 /*  4 bytes state (?) */
 #define SUR40_GET_SENSORS 0xb1 /*  8 bytes sensors   */
 
+static const struct v4l2_pix_format sur40_pix_format[] = {
+	{
+		.pixelformat = V4L2_TCH_FMT_TU08,
+		.width  = SENSOR_RES_X / 2,
+		.height = SENSOR_RES_Y / 2,
+		.field = V4L2_FIELD_NONE,
+		.colorspace = V4L2_COLORSPACE_SRGB,
+		.bytesperline = SENSOR_RES_X / 2,
+		.sizeimage = (SENSOR_RES_X/2) * (SENSOR_RES_Y/2),
+	},
+	{
+		.pixelformat = V4L2_PIX_FMT_GREY,
+		.width  = SENSOR_RES_X / 2,
+		.height = SENSOR_RES_Y / 2,
+		.field = V4L2_FIELD_NONE,
+		.colorspace = V4L2_COLORSPACE_SRGB,
+		.bytesperline = SENSOR_RES_X / 2,
+		.sizeimage = (SENSOR_RES_X/2) * (SENSOR_RES_Y/2),
+	}
+};
+
 /* master device state */
 struct sur40_state {
 
@@ -149,6 +170,7 @@ struct sur40_state {
 	struct v4l2_device v4l2;
 	struct video_device vdev;
 	struct mutex lock;
+	struct v4l2_pix_format pix_fmt;
 
 	struct vb2_queue queue;
 	struct list_head buf_list;
@@ -169,7 +191,6 @@ struct sur40_buffer {
 
 /* forward declarations */
 static const struct video_device sur40_video_device;
-static const struct v4l2_pix_format sur40_video_format;
 static const struct vb2_queue sur40_queue;
 static void sur40_process_video(struct sur40_state *sur40);
 
@@ -420,7 +441,7 @@ static void sur40_process_video(struct sur40_state *sur40)
 		goto err_poll;
 	}
 
-	if (le32_to_cpu(img->size) != sur40_video_format.sizeimage) {
+	if (le32_to_cpu(img->size) != sur40->pix_fmt.sizeimage) {
 		dev_err(sur40->dev, "image size mismatch\n");
 		goto err_poll;
 	}
@@ -431,7 +452,7 @@ static void sur40_process_video(struct sur40_state *sur40)
 
 	result = usb_sg_init(&sgr, sur40->usbdev,
 		usb_rcvbulkpipe(sur40->usbdev, VIDEO_ENDPOINT), 0,
-		sgt->sgl, sgt->nents, sur40_video_format.sizeimage, 0);
+		sgt->sgl, sgt->nents, sur40->pix_fmt.sizeimage, 0);
 	if (result < 0) {
 		dev_err(sur40->dev, "error %d in usb_sg_init\n", result);
 		goto err_poll;
@@ -586,13 +607,14 @@ static int sur40_probe(struct usb_interface *interface,
 	if (error)
 		goto err_unreg_v4l2;
 
+	sur40->pix_fmt = sur40_pix_format[0];
 	sur40->vdev = sur40_video_device;
 	sur40->vdev.v4l2_dev = &sur40->v4l2;
 	sur40->vdev.lock = &sur40->lock;
 	sur40->vdev.queue = &sur40->queue;
 	video_set_drvdata(&sur40->vdev, sur40);
 
-	error = video_register_device(&sur40->vdev, VFL_TYPE_GRABBER, -1);
+	error = video_register_device(&sur40->vdev, VFL_TYPE_TOUCH, -1);
 	if (error) {
 		dev_err(&interface->dev,
 			"Unable to register video subdevice.");
@@ -647,14 +669,16 @@ static int sur40_queue_setup(struct vb2_queue *q,
 		       unsigned int *nbuffers, unsigned int *nplanes,
 		       unsigned int sizes[], struct device *alloc_devs[])
 {
+	struct sur40_state *sur40 = vb2_get_drv_priv(q);
+
 	if (q->num_buffers + *nbuffers < 3)
 		*nbuffers = 3 - q->num_buffers;
 
 	if (*nplanes)
-		return sizes[0] < sur40_video_format.sizeimage ? -EINVAL : 0;
+		return sizes[0] < sur40->pix_fmt.sizeimage ? -EINVAL : 0;
 
 	*nplanes = 1;
-	sizes[0] = sur40_video_format.sizeimage;
+	sizes[0] = sur40->pix_fmt.sizeimage;
 
 	return 0;
 }
@@ -666,7 +690,7 @@ static int sur40_queue_setup(struct vb2_queue *q,
 static int sur40_buffer_prepare(struct vb2_buffer *vb)
 {
 	struct sur40_state *sur40 = vb2_get_drv_priv(vb->vb2_queue);
-	unsigned long size = sur40_video_format.sizeimage;
+	unsigned long size = sur40->pix_fmt.sizeimage;
 
 	if (vb2_plane_size(vb, 0) < size) {
 		dev_err(&sur40->usbdev->dev, "buffer too small (%lu < %lu)\n",
@@ -741,7 +765,7 @@ static int sur40_vidioc_querycap(struct file *file, void *priv,
 	strlcpy(cap->driver, DRIVER_SHORT, sizeof(cap->driver));
 	strlcpy(cap->card, DRIVER_LONG, sizeof(cap->card));
 	usb_make_path(sur40->usbdev, cap->bus_info, sizeof(cap->bus_info));
-	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE |
+	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_TOUCH |
 		V4L2_CAP_READWRITE |
 		V4L2_CAP_STREAMING;
 	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
@@ -753,7 +777,7 @@ static int sur40_vidioc_enum_input(struct file *file, void *priv,
 {
 	if (i->index != 0)
 		return -EINVAL;
-	i->type = V4L2_INPUT_TYPE_CAMERA;
+	i->type = V4L2_INPUT_TYPE_TOUCH;
 	i->std = V4L2_STD_UNKNOWN;
 	strlcpy(i->name, "In-Cell Sensor", sizeof(i->name));
 	i->capabilities = 0;
@@ -771,19 +795,70 @@ static int sur40_vidioc_g_input(struct file *file, void *priv, unsigned int *i)
 	return 0;
 }
 
-static int sur40_vidioc_fmt(struct file *file, void *priv,
+static int sur40_vidioc_try_fmt(struct file *file, void *priv,
+			    struct v4l2_format *f)
+{
+	switch (f->fmt.pix.pixelformat) {
+	case V4L2_PIX_FMT_GREY:
+		f->fmt.pix = sur40_pix_format[1];
+		break;
+
+	default:
+		f->fmt.pix = sur40_pix_format[0];
+		break;
+	}
+
+	return 0;
+}
+
+static int sur40_vidioc_s_fmt(struct file *file, void *priv,
+			    struct v4l2_format *f)
+{
+	struct sur40_state *sur40 = video_drvdata(file);
+
+	switch (f->fmt.pix.pixelformat) {
+	case V4L2_PIX_FMT_GREY:
+		sur40->pix_fmt = sur40_pix_format[1];
+		break;
+
+	default:
+		sur40->pix_fmt = sur40_pix_format[0];
+		break;
+	}
+
+	f->fmt.pix = sur40->pix_fmt;
+	return 0;
+}
+
+static int sur40_vidioc_g_fmt(struct file *file, void *priv,
 			    struct v4l2_format *f)
 {
-	f->fmt.pix = sur40_video_format;
+	struct sur40_state *sur40 = video_drvdata(file);
+
+	f->fmt.pix = sur40->pix_fmt;
+	return 0;
+}
+
+static int sur40_ioctl_parm(struct file *file, void *priv,
+			    struct v4l2_streamparm *p)
+{
+	if (p->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	p->parm.capture.capability = V4L2_CAP_TIMEPERFRAME;
+	p->parm.capture.timeperframe.numerator = 1;
+	p->parm.capture.timeperframe.denominator = 60;
+	p->parm.capture.readbuffers = 3;
 	return 0;
 }
 
 static int sur40_vidioc_enum_fmt(struct file *file, void *priv,
 				 struct v4l2_fmtdesc *f)
 {
-	if (f->index != 0)
+	if (f->index >= ARRAY_SIZE(sur40_pix_format))
 		return -EINVAL;
-	f->pixelformat = V4L2_PIX_FMT_GREY;
+
+	f->pixelformat = sur40_pix_format[f->index].pixelformat;
 	f->flags = 0;
 	return 0;
 }
@@ -791,25 +866,31 @@ static int sur40_vidioc_enum_fmt(struct file *file, void *priv,
 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))
+	struct sur40_state *sur40 = video_drvdata(file);
+
+	if ((f->index != 0) || ((f->pixel_format != V4L2_TCH_FMT_TU08)
+		&& (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;
+	f->discrete.width  = sur40->pix_fmt.width;
+	f->discrete.height = sur40->pix_fmt.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;
+	struct sur40_state *sur40 = video_drvdata(file);
+
+	if ((f->index > 0) || ((f->pixel_format != V4L2_TCH_FMT_TU08)
+		&& (f->pixel_format != V4L2_PIX_FMT_GREY))
+		|| (f->width  != sur40->pix_fmt.width)
+		|| (f->height != sur40->pix_fmt.height))
+		return -EINVAL;
 
 	f->type = V4L2_FRMIVAL_TYPE_DISCRETE;
-	f->discrete.denominator  = 60/(f->index+1);
+	f->discrete.denominator  = 60;
 	f->discrete.numerator = 1;
 	return 0;
 }
@@ -862,13 +943,16 @@ static const struct v4l2_ioctl_ops sur40_video_ioctl_ops = {
 	.vidioc_querycap	= sur40_vidioc_querycap,
 
 	.vidioc_enum_fmt_vid_cap = sur40_vidioc_enum_fmt,
-	.vidioc_try_fmt_vid_cap	= sur40_vidioc_fmt,
-	.vidioc_s_fmt_vid_cap	= sur40_vidioc_fmt,
-	.vidioc_g_fmt_vid_cap	= sur40_vidioc_fmt,
+	.vidioc_try_fmt_vid_cap	= sur40_vidioc_try_fmt,
+	.vidioc_s_fmt_vid_cap	= sur40_vidioc_s_fmt,
+	.vidioc_g_fmt_vid_cap	= sur40_vidioc_g_fmt,
 
 	.vidioc_enum_framesizes = sur40_vidioc_enum_framesizes,
 	.vidioc_enum_frameintervals = sur40_vidioc_enum_frameintervals,
 
+	.vidioc_g_parm = sur40_ioctl_parm,
+	.vidioc_s_parm = sur40_ioctl_parm,
+
 	.vidioc_enum_input	= sur40_vidioc_enum_input,
 	.vidioc_g_input		= sur40_vidioc_g_input,
 	.vidioc_s_input		= sur40_vidioc_s_input,
@@ -891,16 +975,6 @@ static const struct video_device sur40_video_device = {
 	.release = video_device_release_empty,
 };
 
-static const struct v4l2_pix_format sur40_video_format = {
-	.pixelformat = V4L2_PIX_FMT_GREY,
-	.width  = SENSOR_RES_X / 2,
-	.height = SENSOR_RES_Y / 2,
-	.field = V4L2_FIELD_NONE,
-	.colorspace = V4L2_COLORSPACE_SRGB,
-	.bytesperline = SENSOR_RES_X / 2,
-	.sizeimage = (SENSOR_RES_X/2) * (SENSOR_RES_Y/2),
-};
-
 /* USB-specific object needed to register this driver with the USB subsystem. */
 static struct usb_driver sur40_driver = {
 	.name = DRIVER_SHORT,
diff --git a/drivers/input/touchscreen/wdt87xx_i2c.c b/drivers/input/touchscreen/wdt87xx_i2c.c
index 73861ad22df4..a9132603ab34 100644
--- a/drivers/input/touchscreen/wdt87xx_i2c.c
+++ b/drivers/input/touchscreen/wdt87xx_i2c.c
@@ -23,7 +23,7 @@
 #include <asm/unaligned.h>
 
 #define WDT87XX_NAME		"wdt87xx_i2c"
-#define WDT87XX_DRV_VER		"0.9.7"
+#define WDT87XX_DRV_VER		"0.9.8"
 #define WDT87XX_FW_NAME		"wdt87xx_fw.bin"
 #define WDT87XX_CFG_NAME	"wdt87xx_cfg.bin"
 
@@ -157,6 +157,7 @@
 /* Controller requires minimum 300us between commands */
 #define WDT_COMMAND_DELAY_MS		2
 #define WDT_FLASH_WRITE_DELAY_MS	4
+#define WDT_FLASH_ERASE_DELAY_MS	200
 #define WDT_FW_RESET_TIME		2500
 
 struct wdt87xx_sys_param {
@@ -726,7 +727,7 @@ static int wdt87xx_write_firmware(struct i2c_client *client, const void *chunk)
 				break;
 			}
 
-			msleep(50);
+			msleep(WDT_FLASH_ERASE_DELAY_MS);
 
 			error = wdt87xx_write_data(client, data, start_addr,
 						   page_size);
diff --git a/drivers/input/touchscreen/wm97xx-core.c b/drivers/input/touchscreen/wm97xx-core.c
index 1534e9b0788c..90d6be3c26cc 100644
--- a/drivers/input/touchscreen/wm97xx-core.c
+++ b/drivers/input/touchscreen/wm97xx-core.c
@@ -500,7 +500,7 @@ static int wm97xx_ts_input_open(struct input_dev *idev)
 {
 	struct wm97xx *wm = input_get_drvdata(idev);
 
-	wm->ts_workq = create_singlethread_workqueue("kwm97xx");
+	wm->ts_workq = alloc_ordered_workqueue("kwm97xx", 0);
 	if (wm->ts_workq == NULL) {
 		dev_err(wm->dev,
 			"Failed to create workqueue\n");
diff --git a/drivers/iommu/Kconfig b/drivers/iommu/Kconfig
index d432ca828472..8ee54d71c7eb 100644
--- a/drivers/iommu/Kconfig
+++ b/drivers/iommu/Kconfig
@@ -309,7 +309,7 @@ config ARM_SMMU
 
 config ARM_SMMU_V3
 	bool "ARM Ltd. System MMU Version 3 (SMMUv3) Support"
-	depends on ARM64 && PCI
+	depends on ARM64
 	select IOMMU_API
 	select IOMMU_IO_PGTABLE_LPAE
 	select GENERIC_MSI_IRQ_DOMAIN
diff --git a/drivers/iommu/amd_iommu.c b/drivers/iommu/amd_iommu.c
index 96de97a46079..754595ee11b6 100644
--- a/drivers/iommu/amd_iommu.c
+++ b/drivers/iommu/amd_iommu.c
@@ -103,7 +103,7 @@ struct flush_queue {
 	struct flush_queue_entry *entries;
 };
 
-DEFINE_PER_CPU(struct flush_queue, flush_queue);
+static DEFINE_PER_CPU(struct flush_queue, flush_queue);
 
 static atomic_t queue_timer_on;
 static struct timer_list queue_timer;
@@ -137,6 +137,7 @@ struct iommu_dev_data {
 	bool pri_tlp;			  /* PASID TLB required for
 					     PPR completions */
 	u32 errata;			  /* Bitmap for errata to apply */
+	bool use_vapic;			  /* Enable device to use vapic mode */
 };
 
 /*
@@ -707,14 +708,74 @@ static void iommu_poll_ppr_log(struct amd_iommu *iommu)
 	}
 }
 
+#ifdef CONFIG_IRQ_REMAP
+static int (*iommu_ga_log_notifier)(u32);
+
+int amd_iommu_register_ga_log_notifier(int (*notifier)(u32))
+{
+	iommu_ga_log_notifier = notifier;
+
+	return 0;
+}
+EXPORT_SYMBOL(amd_iommu_register_ga_log_notifier);
+
+static void iommu_poll_ga_log(struct amd_iommu *iommu)
+{
+	u32 head, tail, cnt = 0;
+
+	if (iommu->ga_log == NULL)
+		return;
+
+	head = readl(iommu->mmio_base + MMIO_GA_HEAD_OFFSET);
+	tail = readl(iommu->mmio_base + MMIO_GA_TAIL_OFFSET);
+
+	while (head != tail) {
+		volatile u64 *raw;
+		u64 log_entry;
+
+		raw = (u64 *)(iommu->ga_log + head);
+		cnt++;
+
+		/* Avoid memcpy function-call overhead */
+		log_entry = *raw;
+
+		/* Update head pointer of hardware ring-buffer */
+		head = (head + GA_ENTRY_SIZE) % GA_LOG_SIZE;
+		writel(head, iommu->mmio_base + MMIO_GA_HEAD_OFFSET);
+
+		/* Handle GA entry */
+		switch (GA_REQ_TYPE(log_entry)) {
+		case GA_GUEST_NR:
+			if (!iommu_ga_log_notifier)
+				break;
+
+			pr_debug("AMD-Vi: %s: devid=%#x, ga_tag=%#x\n",
+				 __func__, GA_DEVID(log_entry),
+				 GA_TAG(log_entry));
+
+			if (iommu_ga_log_notifier(GA_TAG(log_entry)) != 0)
+				pr_err("AMD-Vi: GA log notifier failed.\n");
+			break;
+		default:
+			break;
+		}
+	}
+}
+#endif /* CONFIG_IRQ_REMAP */
+
+#define AMD_IOMMU_INT_MASK	\
+	(MMIO_STATUS_EVT_INT_MASK | \
+	 MMIO_STATUS_PPR_INT_MASK | \
+	 MMIO_STATUS_GALOG_INT_MASK)
+
 irqreturn_t amd_iommu_int_thread(int irq, void *data)
 {
 	struct amd_iommu *iommu = (struct amd_iommu *) data;
 	u32 status = readl(iommu->mmio_base + MMIO_STATUS_OFFSET);
 
-	while (status & (MMIO_STATUS_EVT_INT_MASK | MMIO_STATUS_PPR_INT_MASK)) {
-		/* Enable EVT and PPR interrupts again */
-		writel((MMIO_STATUS_EVT_INT_MASK | MMIO_STATUS_PPR_INT_MASK),
+	while (status & AMD_IOMMU_INT_MASK) {
+		/* Enable EVT and PPR and GA interrupts again */
+		writel(AMD_IOMMU_INT_MASK,
 			iommu->mmio_base + MMIO_STATUS_OFFSET);
 
 		if (status & MMIO_STATUS_EVT_INT_MASK) {
@@ -727,6 +788,13 @@ irqreturn_t amd_iommu_int_thread(int irq, void *data)
 			iommu_poll_ppr_log(iommu);
 		}
 
+#ifdef CONFIG_IRQ_REMAP
+		if (status & MMIO_STATUS_GALOG_INT_MASK) {
+			pr_devel("AMD-Vi: Processing IOMMU GA Log\n");
+			iommu_poll_ga_log(iommu);
+		}
+#endif
+
 		/*
 		 * Hardware bug: ERBT1312
 		 * When re-enabling interrupt (by writing 1
@@ -940,15 +1008,13 @@ static void build_inv_irt(struct iommu_cmd *cmd, u16 devid)
  * Writes the command to the IOMMUs command buffer and informs the
  * hardware about the new command.
  */
-static int iommu_queue_command_sync(struct amd_iommu *iommu,
-				    struct iommu_cmd *cmd,
-				    bool sync)
+static int __iommu_queue_command_sync(struct amd_iommu *iommu,
+				      struct iommu_cmd *cmd,
+				      bool sync)
 {
 	u32 left, tail, head, next_tail;
-	unsigned long flags;
 
 again:
-	spin_lock_irqsave(&iommu->lock, flags);
 
 	head      = readl(iommu->mmio_base + MMIO_CMD_HEAD_OFFSET);
 	tail      = readl(iommu->mmio_base + MMIO_CMD_TAIL_OFFSET);
@@ -957,15 +1023,14 @@ again:
 
 	if (left <= 2) {
 		struct iommu_cmd sync_cmd;
-		volatile u64 sem = 0;
 		int ret;
 
-		build_completion_wait(&sync_cmd, (u64)&sem);
-		copy_cmd_to_buffer(iommu, &sync_cmd, tail);
+		iommu->cmd_sem = 0;
 
-		spin_unlock_irqrestore(&iommu->lock, flags);
+		build_completion_wait(&sync_cmd, (u64)&iommu->cmd_sem);
+		copy_cmd_to_buffer(iommu, &sync_cmd, tail);
 
-		if ((ret = wait_on_sem(&sem)) != 0)
+		if ((ret = wait_on_sem(&iommu->cmd_sem)) != 0)
 			return ret;
 
 		goto again;
@@ -976,9 +1041,21 @@ again:
 	/* We need to sync now to make sure all commands are processed */
 	iommu->need_sync = sync;
 
+	return 0;
+}
+
+static int iommu_queue_command_sync(struct amd_iommu *iommu,
+				    struct iommu_cmd *cmd,
+				    bool sync)
+{
+	unsigned long flags;
+	int ret;
+
+	spin_lock_irqsave(&iommu->lock, flags);
+	ret = __iommu_queue_command_sync(iommu, cmd, sync);
 	spin_unlock_irqrestore(&iommu->lock, flags);
 
-	return 0;
+	return ret;
 }
 
 static int iommu_queue_command(struct amd_iommu *iommu, struct iommu_cmd *cmd)
@@ -993,19 +1070,29 @@ static int iommu_queue_command(struct amd_iommu *iommu, struct iommu_cmd *cmd)
 static int iommu_completion_wait(struct amd_iommu *iommu)
 {
 	struct iommu_cmd cmd;
-	volatile u64 sem = 0;
+	unsigned long flags;
 	int ret;
 
 	if (!iommu->need_sync)
 		return 0;
 
-	build_completion_wait(&cmd, (u64)&sem);
 
-	ret = iommu_queue_command_sync(iommu, &cmd, false);
+	build_completion_wait(&cmd, (u64)&iommu->cmd_sem);
+
+	spin_lock_irqsave(&iommu->lock, flags);
+
+	iommu->cmd_sem = 0;
+
+	ret = __iommu_queue_command_sync(iommu, &cmd, false);
 	if (ret)
-		return ret;
+		goto out_unlock;
 
-	return wait_on_sem(&sem);
+	ret = wait_on_sem(&iommu->cmd_sem);
+
+out_unlock:
+	spin_unlock_irqrestore(&iommu->lock, flags);
+
+	return ret;
 }
 
 static int iommu_flush_dte(struct amd_iommu *iommu, u16 devid)
@@ -1274,7 +1361,8 @@ static u64 *alloc_pte(struct protection_domain *domain,
 
 			__npte = PM_LEVEL_PDE(level, virt_to_phys(page));
 
-			if (cmpxchg64(pte, __pte, __npte)) {
+			/* pte could have been changed somewhere. */
+			if (cmpxchg64(pte, __pte, __npte) != __pte) {
 				free_page((unsigned long)page);
 				continue;
 			}
@@ -1654,6 +1742,9 @@ static void dma_ops_domain_free(struct dma_ops_domain *dom)
 
 	free_pagetable(&dom->domain);
 
+	if (dom->domain.id)
+		domain_id_free(dom->domain.id);
+
 	kfree(dom);
 }
 
@@ -2948,6 +3039,12 @@ static void amd_iommu_detach_device(struct iommu_domain *dom,
 	if (!iommu)
 		return;
 
+#ifdef CONFIG_IRQ_REMAP
+	if (AMD_IOMMU_GUEST_IR_VAPIC(amd_iommu_guest_ir) &&
+	    (dom->type == IOMMU_DOMAIN_UNMANAGED))
+		dev_data->use_vapic = 0;
+#endif
+
 	iommu_completion_wait(iommu);
 }
 
@@ -2973,6 +3070,15 @@ static int amd_iommu_attach_device(struct iommu_domain *dom,
 
 	ret = attach_device(dev, domain);
 
+#ifdef CONFIG_IRQ_REMAP
+	if (AMD_IOMMU_GUEST_IR_VAPIC(amd_iommu_guest_ir)) {
+		if (dom->type == IOMMU_DOMAIN_UNMANAGED)
+			dev_data->use_vapic = 1;
+		else
+			dev_data->use_vapic = 0;
+	}
+#endif
+
 	iommu_completion_wait(iommu);
 
 	return ret;
@@ -3511,34 +3617,6 @@ EXPORT_SYMBOL(amd_iommu_device_info);
  *
  *****************************************************************************/
 
-union irte {
-	u32 val;
-	struct {
-		u32 valid	: 1,
-		    no_fault	: 1,
-		    int_type	: 3,
-		    rq_eoi	: 1,
-		    dm		: 1,
-		    rsvd_1	: 1,
-		    destination	: 8,
-		    vector	: 8,
-		    rsvd_2	: 8;
-	} 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)
@@ -3560,8 +3638,6 @@ static void set_dte_irq_entry(u16 devid, struct irq_remap_table *table)
 	amd_iommu_dev_table[devid].data[2] = dte;
 }
 
-#define IRTE_ALLOCATED (~1U)
-
 static struct irq_remap_table *get_irq_table(u16 devid, bool ioapic)
 {
 	struct irq_remap_table *table = NULL;
@@ -3577,7 +3653,7 @@ static struct irq_remap_table *get_irq_table(u16 devid, bool ioapic)
 
 	table = irq_lookup_table[devid];
 	if (table)
-		goto out;
+		goto out_unlock;
 
 	alias = amd_iommu_alias_table[devid];
 	table = irq_lookup_table[alias];
@@ -3591,7 +3667,7 @@ static struct irq_remap_table *get_irq_table(u16 devid, bool ioapic)
 	/* Nothing there yet, allocate new irq remapping table */
 	table = kzalloc(sizeof(*table), GFP_ATOMIC);
 	if (!table)
-		goto out;
+		goto out_unlock;
 
 	/* Initialize table spin-lock */
 	spin_lock_init(&table->lock);
@@ -3604,16 +3680,21 @@ static struct irq_remap_table *get_irq_table(u16 devid, bool ioapic)
 	if (!table->table) {
 		kfree(table);
 		table = NULL;
-		goto out;
+		goto out_unlock;
 	}
 
-	memset(table->table, 0, MAX_IRQS_PER_TABLE * sizeof(u32));
+	if (!AMD_IOMMU_GUEST_IR_GA(amd_iommu_guest_ir))
+		memset(table->table, 0,
+		       MAX_IRQS_PER_TABLE * sizeof(u32));
+	else
+		memset(table->table, 0,
+		       (MAX_IRQS_PER_TABLE * (sizeof(u64) * 2)));
 
 	if (ioapic) {
 		int i;
 
 		for (i = 0; i < 32; ++i)
-			table->table[i] = IRTE_ALLOCATED;
+			iommu->irte_ops->set_allocated(table, i);
 	}
 
 	irq_lookup_table[devid] = table;
@@ -3639,6 +3720,10 @@ static int alloc_irq_index(u16 devid, int count)
 	struct irq_remap_table *table;
 	unsigned long flags;
 	int index, c;
+	struct amd_iommu *iommu = amd_iommu_rlookup_table[devid];
+
+	if (!iommu)
+		return -ENODEV;
 
 	table = get_irq_table(devid, false);
 	if (!table)
@@ -3650,14 +3735,14 @@ static int alloc_irq_index(u16 devid, int count)
 	for (c = 0, index = table->min_index;
 	     index < MAX_IRQS_PER_TABLE;
 	     ++index) {
-		if (table->table[index] == 0)
+		if (!iommu->irte_ops->is_allocated(table, index))
 			c += 1;
 		else
 			c = 0;
 
 		if (c == count)	{
 			for (; c != 0; --c)
-				table->table[index - c + 1] = IRTE_ALLOCATED;
+				iommu->irte_ops->set_allocated(table, index - c + 1);
 
 			index -= count - 1;
 			goto out;
@@ -3672,7 +3757,42 @@ out:
 	return index;
 }
 
-static int modify_irte(u16 devid, int index, union irte irte)
+static int modify_irte_ga(u16 devid, int index, struct irte_ga *irte,
+			  struct amd_ir_data *data)
+{
+	struct irq_remap_table *table;
+	struct amd_iommu *iommu;
+	unsigned long flags;
+	struct irte_ga *entry;
+
+	iommu = amd_iommu_rlookup_table[devid];
+	if (iommu == NULL)
+		return -EINVAL;
+
+	table = get_irq_table(devid, false);
+	if (!table)
+		return -ENOMEM;
+
+	spin_lock_irqsave(&table->lock, flags);
+
+	entry = (struct irte_ga *)table->table;
+	entry = &entry[index];
+	entry->lo.fields_remap.valid = 0;
+	entry->hi.val = irte->hi.val;
+	entry->lo.val = irte->lo.val;
+	entry->lo.fields_remap.valid = 1;
+	if (data)
+		data->ref = entry;
+
+	spin_unlock_irqrestore(&table->lock, flags);
+
+	iommu_flush_irt(iommu, devid);
+	iommu_completion_wait(iommu);
+
+	return 0;
+}
+
+static int modify_irte(u16 devid, int index, union irte *irte)
 {
 	struct irq_remap_table *table;
 	struct amd_iommu *iommu;
@@ -3687,7 +3807,7 @@ static int modify_irte(u16 devid, int index, union irte irte)
 		return -ENOMEM;
 
 	spin_lock_irqsave(&table->lock, flags);
-	table->table[index] = irte.val;
+	table->table[index] = irte->val;
 	spin_unlock_irqrestore(&table->lock, flags);
 
 	iommu_flush_irt(iommu, devid);
@@ -3711,13 +3831,146 @@ static void free_irte(u16 devid, int index)
 		return;
 
 	spin_lock_irqsave(&table->lock, flags);
-	table->table[index] = 0;
+	iommu->irte_ops->clear_allocated(table, index);
 	spin_unlock_irqrestore(&table->lock, flags);
 
 	iommu_flush_irt(iommu, devid);
 	iommu_completion_wait(iommu);
 }
 
+static void irte_prepare(void *entry,
+			 u32 delivery_mode, u32 dest_mode,
+			 u8 vector, u32 dest_apicid, int devid)
+{
+	union irte *irte = (union irte *) entry;
+
+	irte->val                = 0;
+	irte->fields.vector      = vector;
+	irte->fields.int_type    = delivery_mode;
+	irte->fields.destination = dest_apicid;
+	irte->fields.dm          = dest_mode;
+	irte->fields.valid       = 1;
+}
+
+static void irte_ga_prepare(void *entry,
+			    u32 delivery_mode, u32 dest_mode,
+			    u8 vector, u32 dest_apicid, int devid)
+{
+	struct irte_ga *irte = (struct irte_ga *) entry;
+	struct iommu_dev_data *dev_data = search_dev_data(devid);
+
+	irte->lo.val                      = 0;
+	irte->hi.val                      = 0;
+	irte->lo.fields_remap.guest_mode  = dev_data ? dev_data->use_vapic : 0;
+	irte->lo.fields_remap.int_type    = delivery_mode;
+	irte->lo.fields_remap.dm          = dest_mode;
+	irte->hi.fields.vector            = vector;
+	irte->lo.fields_remap.destination = dest_apicid;
+	irte->lo.fields_remap.valid       = 1;
+}
+
+static void irte_activate(void *entry, u16 devid, u16 index)
+{
+	union irte *irte = (union irte *) entry;
+
+	irte->fields.valid = 1;
+	modify_irte(devid, index, irte);
+}
+
+static void irte_ga_activate(void *entry, u16 devid, u16 index)
+{
+	struct irte_ga *irte = (struct irte_ga *) entry;
+
+	irte->lo.fields_remap.valid = 1;
+	modify_irte_ga(devid, index, irte, NULL);
+}
+
+static void irte_deactivate(void *entry, u16 devid, u16 index)
+{
+	union irte *irte = (union irte *) entry;
+
+	irte->fields.valid = 0;
+	modify_irte(devid, index, irte);
+}
+
+static void irte_ga_deactivate(void *entry, u16 devid, u16 index)
+{
+	struct irte_ga *irte = (struct irte_ga *) entry;
+
+	irte->lo.fields_remap.valid = 0;
+	modify_irte_ga(devid, index, irte, NULL);
+}
+
+static void irte_set_affinity(void *entry, u16 devid, u16 index,
+			      u8 vector, u32 dest_apicid)
+{
+	union irte *irte = (union irte *) entry;
+
+	irte->fields.vector = vector;
+	irte->fields.destination = dest_apicid;
+	modify_irte(devid, index, irte);
+}
+
+static void irte_ga_set_affinity(void *entry, u16 devid, u16 index,
+				 u8 vector, u32 dest_apicid)
+{
+	struct irte_ga *irte = (struct irte_ga *) entry;
+	struct iommu_dev_data *dev_data = search_dev_data(devid);
+
+	if (!dev_data || !dev_data->use_vapic) {
+		irte->hi.fields.vector = vector;
+		irte->lo.fields_remap.destination = dest_apicid;
+		irte->lo.fields_remap.guest_mode = 0;
+		modify_irte_ga(devid, index, irte, NULL);
+	}
+}
+
+#define IRTE_ALLOCATED (~1U)
+static void irte_set_allocated(struct irq_remap_table *table, int index)
+{
+	table->table[index] = IRTE_ALLOCATED;
+}
+
+static void irte_ga_set_allocated(struct irq_remap_table *table, int index)
+{
+	struct irte_ga *ptr = (struct irte_ga *)table->table;
+	struct irte_ga *irte = &ptr[index];
+
+	memset(&irte->lo.val, 0, sizeof(u64));
+	memset(&irte->hi.val, 0, sizeof(u64));
+	irte->hi.fields.vector = 0xff;
+}
+
+static bool irte_is_allocated(struct irq_remap_table *table, int index)
+{
+	union irte *ptr = (union irte *)table->table;
+	union irte *irte = &ptr[index];
+
+	return irte->val != 0;
+}
+
+static bool irte_ga_is_allocated(struct irq_remap_table *table, int index)
+{
+	struct irte_ga *ptr = (struct irte_ga *)table->table;
+	struct irte_ga *irte = &ptr[index];
+
+	return irte->hi.fields.vector != 0;
+}
+
+static void irte_clear_allocated(struct irq_remap_table *table, int index)
+{
+	table->table[index] = 0;
+}
+
+static void irte_ga_clear_allocated(struct irq_remap_table *table, int index)
+{
+	struct irte_ga *ptr = (struct irte_ga *)table->table;
+	struct irte_ga *irte = &ptr[index];
+
+	memset(&irte->lo.val, 0, sizeof(u64));
+	memset(&irte->hi.val, 0, sizeof(u64));
+}
+
 static int get_devid(struct irq_alloc_info *info)
 {
 	int devid = -1;
@@ -3802,19 +4055,17 @@ static void irq_remapping_prepare_irte(struct amd_ir_data *data,
 {
 	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;
+	struct amd_iommu *iommu = amd_iommu_rlookup_table[devid];
+
+	if (!iommu)
+		return;
 
 	data->irq_2_irte.devid = devid;
 	data->irq_2_irte.index = index + sub_handle;
-
-	/* 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;
+	iommu->irte_ops->prepare(data->entry, apic->irq_delivery_mode,
+				 apic->irq_dest_mode, irq_cfg->vector,
+				 irq_cfg->dest_apicid, devid);
 
 	switch (info->type) {
 	case X86_IRQ_ALLOC_TYPE_IOAPIC:
@@ -3845,12 +4096,32 @@ static void irq_remapping_prepare_irte(struct amd_ir_data *data,
 	}
 }
 
+struct amd_irte_ops irte_32_ops = {
+	.prepare = irte_prepare,
+	.activate = irte_activate,
+	.deactivate = irte_deactivate,
+	.set_affinity = irte_set_affinity,
+	.set_allocated = irte_set_allocated,
+	.is_allocated = irte_is_allocated,
+	.clear_allocated = irte_clear_allocated,
+};
+
+struct amd_irte_ops irte_128_ops = {
+	.prepare = irte_ga_prepare,
+	.activate = irte_ga_activate,
+	.deactivate = irte_ga_deactivate,
+	.set_affinity = irte_ga_set_affinity,
+	.set_allocated = irte_ga_set_allocated,
+	.is_allocated = irte_ga_is_allocated,
+	.clear_allocated = irte_ga_clear_allocated,
+};
+
 static int irq_remapping_alloc(struct irq_domain *domain, unsigned int virq,
 			       unsigned int nr_irqs, void *arg)
 {
 	struct irq_alloc_info *info = arg;
 	struct irq_data *irq_data;
-	struct amd_ir_data *data;
+	struct amd_ir_data *data = NULL;
 	struct irq_cfg *cfg;
 	int i, ret, devid;
 	int index = -1;
@@ -3886,6 +4157,7 @@ static int irq_remapping_alloc(struct irq_domain *domain, unsigned int virq,
 	}
 	if (index < 0) {
 		pr_warn("Failed to allocate IRTE\n");
+		ret = index;
 		goto out_free_parent;
 	}
 
@@ -3902,6 +4174,16 @@ static int irq_remapping_alloc(struct irq_domain *domain, unsigned int virq,
 		if (!data)
 			goto out_free_data;
 
+		if (!AMD_IOMMU_GUEST_IR_GA(amd_iommu_guest_ir))
+			data->entry = kzalloc(sizeof(union irte), GFP_KERNEL);
+		else
+			data->entry = kzalloc(sizeof(struct irte_ga),
+						     GFP_KERNEL);
+		if (!data->entry) {
+			kfree(data);
+			goto out_free_data;
+		}
+
 		irq_data->hwirq = (devid << 16) + i;
 		irq_data->chip_data = data;
 		irq_data->chip = &amd_ir_chip;
@@ -3938,6 +4220,7 @@ static void irq_remapping_free(struct irq_domain *domain, unsigned int virq,
 			data = irq_data->chip_data;
 			irte_info = &data->irq_2_irte;
 			free_irte(irte_info->devid, irte_info->index);
+			kfree(data->entry);
 			kfree(data);
 		}
 	}
@@ -3949,8 +4232,11 @@ static void irq_remapping_activate(struct irq_domain *domain,
 {
 	struct amd_ir_data *data = irq_data->chip_data;
 	struct irq_2_irte *irte_info = &data->irq_2_irte;
+	struct amd_iommu *iommu = amd_iommu_rlookup_table[irte_info->devid];
 
-	modify_irte(irte_info->devid, irte_info->index, data->irte_entry);
+	if (iommu)
+		iommu->irte_ops->activate(data->entry, irte_info->devid,
+					  irte_info->index);
 }
 
 static void irq_remapping_deactivate(struct irq_domain *domain,
@@ -3958,10 +4244,11 @@ static void irq_remapping_deactivate(struct irq_domain *domain,
 {
 	struct amd_ir_data *data = irq_data->chip_data;
 	struct irq_2_irte *irte_info = &data->irq_2_irte;
-	union irte entry;
+	struct amd_iommu *iommu = amd_iommu_rlookup_table[irte_info->devid];
 
-	entry.val = 0;
-	modify_irte(irte_info->devid, irte_info->index, data->irte_entry);
+	if (iommu)
+		iommu->irte_ops->deactivate(data->entry, irte_info->devid,
+					    irte_info->index);
 }
 
 static struct irq_domain_ops amd_ir_domain_ops = {
@@ -3971,6 +4258,70 @@ static struct irq_domain_ops amd_ir_domain_ops = {
 	.deactivate = irq_remapping_deactivate,
 };
 
+static int amd_ir_set_vcpu_affinity(struct irq_data *data, void *vcpu_info)
+{
+	struct amd_iommu *iommu;
+	struct amd_iommu_pi_data *pi_data = vcpu_info;
+	struct vcpu_data *vcpu_pi_info = pi_data->vcpu_data;
+	struct amd_ir_data *ir_data = data->chip_data;
+	struct irte_ga *irte = (struct irte_ga *) ir_data->entry;
+	struct irq_2_irte *irte_info = &ir_data->irq_2_irte;
+	struct iommu_dev_data *dev_data = search_dev_data(irte_info->devid);
+
+	/* Note:
+	 * This device has never been set up for guest mode.
+	 * we should not modify the IRTE
+	 */
+	if (!dev_data || !dev_data->use_vapic)
+		return 0;
+
+	pi_data->ir_data = ir_data;
+
+	/* Note:
+	 * SVM tries to set up for VAPIC mode, but we are in
+	 * legacy mode. So, we force legacy mode instead.
+	 */
+	if (!AMD_IOMMU_GUEST_IR_VAPIC(amd_iommu_guest_ir)) {
+		pr_debug("AMD-Vi: %s: Fall back to using intr legacy remap\n",
+			 __func__);
+		pi_data->is_guest_mode = false;
+	}
+
+	iommu = amd_iommu_rlookup_table[irte_info->devid];
+	if (iommu == NULL)
+		return -EINVAL;
+
+	pi_data->prev_ga_tag = ir_data->cached_ga_tag;
+	if (pi_data->is_guest_mode) {
+		/* Setting */
+		irte->hi.fields.ga_root_ptr = (pi_data->base >> 12);
+		irte->hi.fields.vector = vcpu_pi_info->vector;
+		irte->lo.fields_vapic.guest_mode = 1;
+		irte->lo.fields_vapic.ga_tag = pi_data->ga_tag;
+
+		ir_data->cached_ga_tag = pi_data->ga_tag;
+	} else {
+		/* Un-Setting */
+		struct irq_cfg *cfg = irqd_cfg(data);
+
+		irte->hi.val = 0;
+		irte->lo.val = 0;
+		irte->hi.fields.vector = cfg->vector;
+		irte->lo.fields_remap.guest_mode = 0;
+		irte->lo.fields_remap.destination = cfg->dest_apicid;
+		irte->lo.fields_remap.int_type = apic->irq_delivery_mode;
+		irte->lo.fields_remap.dm = apic->irq_dest_mode;
+
+		/*
+		 * This communicates the ga_tag back to the caller
+		 * so that it can do all the necessary clean up.
+		 */
+		ir_data->cached_ga_tag = 0;
+	}
+
+	return modify_irte_ga(irte_info->devid, irte_info->index, irte, ir_data);
+}
+
 static int amd_ir_set_affinity(struct irq_data *data,
 			       const struct cpumask *mask, bool force)
 {
@@ -3978,8 +4329,12 @@ static int amd_ir_set_affinity(struct irq_data *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;
+	struct amd_iommu *iommu = amd_iommu_rlookup_table[irte_info->devid];
 	int ret;
 
+	if (!iommu)
+		return -ENODEV;
+
 	ret = parent->chip->irq_set_affinity(parent, mask, force);
 	if (ret < 0 || ret == IRQ_SET_MASK_OK_DONE)
 		return ret;
@@ -3988,9 +4343,8 @@ static int amd_ir_set_affinity(struct irq_data *data,
 	 * 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);
+	iommu->irte_ops->set_affinity(ir_data->entry, irte_info->devid,
+			    irte_info->index, cfg->vector, cfg->dest_apicid);
 
 	/*
 	 * After this point, all the interrupts will start arriving
@@ -4012,6 +4366,7 @@ static void ir_compose_msi_msg(struct irq_data *irq_data, struct msi_msg *msg)
 static struct irq_chip amd_ir_chip = {
 	.irq_ack = ir_ack_apic_edge,
 	.irq_set_affinity = amd_ir_set_affinity,
+	.irq_set_vcpu_affinity = amd_ir_set_vcpu_affinity,
 	.irq_compose_msi_msg = ir_compose_msi_msg,
 };
 
@@ -4026,4 +4381,43 @@ int amd_iommu_create_irq_domain(struct amd_iommu *iommu)
 
 	return 0;
 }
+
+int amd_iommu_update_ga(int cpu, bool is_run, void *data)
+{
+	unsigned long flags;
+	struct amd_iommu *iommu;
+	struct irq_remap_table *irt;
+	struct amd_ir_data *ir_data = (struct amd_ir_data *)data;
+	int devid = ir_data->irq_2_irte.devid;
+	struct irte_ga *entry = (struct irte_ga *) ir_data->entry;
+	struct irte_ga *ref = (struct irte_ga *) ir_data->ref;
+
+	if (!AMD_IOMMU_GUEST_IR_VAPIC(amd_iommu_guest_ir) ||
+	    !ref || !entry || !entry->lo.fields_vapic.guest_mode)
+		return 0;
+
+	iommu = amd_iommu_rlookup_table[devid];
+	if (!iommu)
+		return -ENODEV;
+
+	irt = get_irq_table(devid, false);
+	if (!irt)
+		return -ENODEV;
+
+	spin_lock_irqsave(&irt->lock, flags);
+
+	if (ref->lo.fields_vapic.guest_mode) {
+		if (cpu >= 0)
+			ref->lo.fields_vapic.destination = cpu;
+		ref->lo.fields_vapic.is_run = is_run;
+		barrier();
+	}
+
+	spin_unlock_irqrestore(&irt->lock, flags);
+
+	iommu_flush_irt(iommu, devid);
+	iommu_completion_wait(iommu);
+	return 0;
+}
+EXPORT_SYMBOL(amd_iommu_update_ga);
 #endif
diff --git a/drivers/iommu/amd_iommu_init.c b/drivers/iommu/amd_iommu_init.c
index 59741ead7e15..157e93421fb8 100644
--- a/drivers/iommu/amd_iommu_init.c
+++ b/drivers/iommu/amd_iommu_init.c
@@ -20,6 +20,7 @@
 #include <linux/pci.h>
 #include <linux/acpi.h>
 #include <linux/list.h>
+#include <linux/bitmap.h>
 #include <linux/slab.h>
 #include <linux/syscore_ops.h>
 #include <linux/interrupt.h>
@@ -84,6 +85,7 @@
 #define ACPI_DEVFLAG_LINT1              0x80
 #define ACPI_DEVFLAG_ATSDIS             0x10000000
 
+#define LOOP_TIMEOUT	100000
 /*
  * ACPI table definitions
  *
@@ -145,6 +147,8 @@ struct ivmd_header {
 bool amd_iommu_dump;
 bool amd_iommu_irq_remap __read_mostly;
 
+int amd_iommu_guest_ir = AMD_IOMMU_GUEST_IR_VAPIC;
+
 static bool amd_iommu_detected;
 static bool __initdata amd_iommu_disabled;
 static int amd_iommu_target_ivhd_type;
@@ -386,6 +390,10 @@ static void iommu_disable(struct amd_iommu *iommu)
 	iommu_feature_disable(iommu, CONTROL_EVT_INT_EN);
 	iommu_feature_disable(iommu, CONTROL_EVT_LOG_EN);
 
+	/* Disable IOMMU GA_LOG */
+	iommu_feature_disable(iommu, CONTROL_GALOG_EN);
+	iommu_feature_disable(iommu, CONTROL_GAINT_EN);
+
 	/* Disable IOMMU hardware itself */
 	iommu_feature_disable(iommu, CONTROL_IOMMU_EN);
 }
@@ -671,6 +679,99 @@ static void __init free_ppr_log(struct amd_iommu *iommu)
 	free_pages((unsigned long)iommu->ppr_log, get_order(PPR_LOG_SIZE));
 }
 
+static void free_ga_log(struct amd_iommu *iommu)
+{
+#ifdef CONFIG_IRQ_REMAP
+	if (iommu->ga_log)
+		free_pages((unsigned long)iommu->ga_log,
+			    get_order(GA_LOG_SIZE));
+	if (iommu->ga_log_tail)
+		free_pages((unsigned long)iommu->ga_log_tail,
+			    get_order(8));
+#endif
+}
+
+static int iommu_ga_log_enable(struct amd_iommu *iommu)
+{
+#ifdef CONFIG_IRQ_REMAP
+	u32 status, i;
+
+	if (!iommu->ga_log)
+		return -EINVAL;
+
+	status = readl(iommu->mmio_base + MMIO_STATUS_OFFSET);
+
+	/* Check if already running */
+	if (status & (MMIO_STATUS_GALOG_RUN_MASK))
+		return 0;
+
+	iommu_feature_enable(iommu, CONTROL_GAINT_EN);
+	iommu_feature_enable(iommu, CONTROL_GALOG_EN);
+
+	for (i = 0; i < LOOP_TIMEOUT; ++i) {
+		status = readl(iommu->mmio_base + MMIO_STATUS_OFFSET);
+		if (status & (MMIO_STATUS_GALOG_RUN_MASK))
+			break;
+	}
+
+	if (i >= LOOP_TIMEOUT)
+		return -EINVAL;
+#endif /* CONFIG_IRQ_REMAP */
+	return 0;
+}
+
+#ifdef CONFIG_IRQ_REMAP
+static int iommu_init_ga_log(struct amd_iommu *iommu)
+{
+	u64 entry;
+
+	if (!AMD_IOMMU_GUEST_IR_VAPIC(amd_iommu_guest_ir))
+		return 0;
+
+	iommu->ga_log = (u8 *)__get_free_pages(GFP_KERNEL | __GFP_ZERO,
+					get_order(GA_LOG_SIZE));
+	if (!iommu->ga_log)
+		goto err_out;
+
+	iommu->ga_log_tail = (u8 *)__get_free_pages(GFP_KERNEL | __GFP_ZERO,
+					get_order(8));
+	if (!iommu->ga_log_tail)
+		goto err_out;
+
+	entry = (u64)virt_to_phys(iommu->ga_log) | GA_LOG_SIZE_512;
+	memcpy_toio(iommu->mmio_base + MMIO_GA_LOG_BASE_OFFSET,
+		    &entry, sizeof(entry));
+	entry = ((u64)virt_to_phys(iommu->ga_log) & 0xFFFFFFFFFFFFFULL) & ~7ULL;
+	memcpy_toio(iommu->mmio_base + MMIO_GA_LOG_TAIL_OFFSET,
+		    &entry, sizeof(entry));
+	writel(0x00, iommu->mmio_base + MMIO_GA_HEAD_OFFSET);
+	writel(0x00, iommu->mmio_base + MMIO_GA_TAIL_OFFSET);
+
+	return 0;
+err_out:
+	free_ga_log(iommu);
+	return -EINVAL;
+}
+#endif /* CONFIG_IRQ_REMAP */
+
+static int iommu_init_ga(struct amd_iommu *iommu)
+{
+	int ret = 0;
+
+#ifdef CONFIG_IRQ_REMAP
+	/* Note: We have already checked GASup from IVRS table.
+	 *       Now, we need to make sure that GAMSup is set.
+	 */
+	if (AMD_IOMMU_GUEST_IR_VAPIC(amd_iommu_guest_ir) &&
+	    !iommu_feature(iommu, FEATURE_GAM_VAPIC))
+		amd_iommu_guest_ir = AMD_IOMMU_GUEST_IR_LEGACY_GA;
+
+	ret = iommu_init_ga_log(iommu);
+#endif /* CONFIG_IRQ_REMAP */
+
+	return ret;
+}
+
 static void iommu_enable_gt(struct amd_iommu *iommu)
 {
 	if (!iommu_feature(iommu, FEATURE_GT))
@@ -1144,6 +1245,7 @@ static void __init free_iommu_one(struct amd_iommu *iommu)
 	free_command_buffer(iommu);
 	free_event_buffer(iommu);
 	free_ppr_log(iommu);
+	free_ga_log(iommu);
 	iommu_unmap_mmio_space(iommu);
 }
 
@@ -1258,6 +1360,8 @@ static int __init init_iommu_one(struct amd_iommu *iommu, struct ivhd_header *h)
 			iommu->mmio_phys_end = MMIO_REG_END_OFFSET;
 		else
 			iommu->mmio_phys_end = MMIO_CNTR_CONF_OFFSET;
+		if (((h->efr_attr & (0x1 << IOMMU_FEAT_GASUP_SHIFT)) == 0))
+			amd_iommu_guest_ir = AMD_IOMMU_GUEST_IR_LEGACY;
 		break;
 	case 0x11:
 	case 0x40:
@@ -1265,6 +1369,8 @@ static int __init init_iommu_one(struct amd_iommu *iommu, struct ivhd_header *h)
 			iommu->mmio_phys_end = MMIO_REG_END_OFFSET;
 		else
 			iommu->mmio_phys_end = MMIO_CNTR_CONF_OFFSET;
+		if (((h->efr_reg & (0x1 << IOMMU_EFR_GASUP_SHIFT)) == 0))
+			amd_iommu_guest_ir = AMD_IOMMU_GUEST_IR_LEGACY;
 		break;
 	default:
 		return -EINVAL;
@@ -1432,6 +1538,7 @@ static int iommu_init_pci(struct amd_iommu *iommu)
 {
 	int cap_ptr = iommu->cap_ptr;
 	u32 range, misc, low, high;
+	int ret;
 
 	iommu->dev = pci_get_bus_and_slot(PCI_BUS_NUM(iommu->devid),
 					  iommu->devid & 0xff);
@@ -1488,6 +1595,10 @@ static int iommu_init_pci(struct amd_iommu *iommu)
 	if (iommu_feature(iommu, FEATURE_PPR) && alloc_ppr_log(iommu))
 		return -ENOMEM;
 
+	ret = iommu_init_ga(iommu);
+	if (ret)
+		return ret;
+
 	if (iommu->cap & (1UL << IOMMU_CAP_NPCACHE))
 		amd_iommu_np_cache = true;
 
@@ -1545,16 +1656,24 @@ static void print_iommu_info(void)
 			dev_name(&iommu->dev->dev), iommu->cap_ptr);
 
 		if (iommu->cap & (1 << IOMMU_CAP_EFR)) {
-			pr_info("AMD-Vi:  Extended features: ");
+			pr_info("AMD-Vi: Extended features (%#llx):\n",
+				iommu->features);
 			for (i = 0; i < ARRAY_SIZE(feat_str); ++i) {
 				if (iommu_feature(iommu, (1ULL << i)))
 					pr_cont(" %s", feat_str[i]);
 			}
+
+			if (iommu->features & FEATURE_GAM_VAPIC)
+				pr_cont(" GA_vAPIC");
+
 			pr_cont("\n");
 		}
 	}
-	if (irq_remapping_enabled)
+	if (irq_remapping_enabled) {
 		pr_info("AMD-Vi: Interrupt remapping enabled\n");
+		if (AMD_IOMMU_GUEST_IR_VAPIC(amd_iommu_guest_ir))
+			pr_info("AMD-Vi: virtual APIC enabled\n");
+	}
 }
 
 static int __init amd_iommu_init_pci(void)
@@ -1645,6 +1764,8 @@ enable_faults:
 	if (iommu->ppr_log != NULL)
 		iommu_feature_enable(iommu, CONTROL_PPFINT_EN);
 
+	iommu_ga_log_enable(iommu);
+
 	return 0;
 }
 
@@ -1862,6 +1983,24 @@ static void iommu_apply_resume_quirks(struct amd_iommu *iommu)
 			       iommu->stored_addr_lo | 1);
 }
 
+static void iommu_enable_ga(struct amd_iommu *iommu)
+{
+#ifdef CONFIG_IRQ_REMAP
+	switch (amd_iommu_guest_ir) {
+	case AMD_IOMMU_GUEST_IR_VAPIC:
+		iommu_feature_enable(iommu, CONTROL_GAM_EN);
+		/* Fall through */
+	case AMD_IOMMU_GUEST_IR_LEGACY_GA:
+		iommu_feature_enable(iommu, CONTROL_GA_EN);
+		iommu->irte_ops = &irte_128_ops;
+		break;
+	default:
+		iommu->irte_ops = &irte_32_ops;
+		break;
+	}
+#endif
+}
+
 /*
  * This function finally enables all IOMMUs found in the system after
  * they have been initialized
@@ -1877,9 +2016,15 @@ static void early_enable_iommus(void)
 		iommu_enable_command_buffer(iommu);
 		iommu_enable_event_buffer(iommu);
 		iommu_set_exclusion_range(iommu);
+		iommu_enable_ga(iommu);
 		iommu_enable(iommu);
 		iommu_flush_all_caches(iommu);
 	}
+
+#ifdef CONFIG_IRQ_REMAP
+	if (AMD_IOMMU_GUEST_IR_VAPIC(amd_iommu_guest_ir))
+		amd_iommu_irq_ops.capability |= (1 << IRQ_POSTING_CAP);
+#endif
 }
 
 static void enable_iommus_v2(void)
@@ -1905,6 +2050,11 @@ static void disable_iommus(void)
 
 	for_each_iommu(iommu)
 		iommu_disable(iommu);
+
+#ifdef CONFIG_IRQ_REMAP
+	if (AMD_IOMMU_GUEST_IR_VAPIC(amd_iommu_guest_ir))
+		amd_iommu_irq_ops.capability &= ~(1 << IRQ_POSTING_CAP);
+#endif
 }
 
 /*
@@ -2059,7 +2209,7 @@ static int __init early_amd_iommu_init(void)
 	struct acpi_table_header *ivrs_base;
 	acpi_size ivrs_size;
 	acpi_status status;
-	int i, ret = 0;
+	int i, remap_cache_sz, ret = 0;
 
 	if (!amd_iommu_detected)
 		return -ENODEV;
@@ -2136,7 +2286,7 @@ static int __init early_amd_iommu_init(void)
 	 * never allocate domain 0 because its used as the non-allocated and
 	 * error value placeholder
 	 */
-	amd_iommu_pd_alloc_bitmap[0] = 1;
+	__set_bit(0, amd_iommu_pd_alloc_bitmap);
 
 	spin_lock_init(&amd_iommu_pd_lock);
 
@@ -2157,10 +2307,14 @@ static int __init early_amd_iommu_init(void)
 		 * remapping tables.
 		 */
 		ret = -ENOMEM;
+		if (!AMD_IOMMU_GUEST_IR_GA(amd_iommu_guest_ir))
+			remap_cache_sz = MAX_IRQS_PER_TABLE * sizeof(u32);
+		else
+			remap_cache_sz = MAX_IRQS_PER_TABLE * (sizeof(u64) * 2);
 		amd_iommu_irq_cache = kmem_cache_create("irq_remap_cache",
-				MAX_IRQS_PER_TABLE * sizeof(u32),
-				IRQ_TABLE_ALIGNMENT,
-				0, NULL);
+							remap_cache_sz,
+							IRQ_TABLE_ALIGNMENT,
+							0, NULL);
 		if (!amd_iommu_irq_cache)
 			goto out;
 
@@ -2413,6 +2567,21 @@ static int __init parse_amd_iommu_dump(char *str)
 	return 1;
 }
 
+static int __init parse_amd_iommu_intr(char *str)
+{
+	for (; *str; ++str) {
+		if (strncmp(str, "legacy", 6) == 0) {
+			amd_iommu_guest_ir = AMD_IOMMU_GUEST_IR_LEGACY;
+			break;
+		}
+		if (strncmp(str, "vapic", 5) == 0) {
+			amd_iommu_guest_ir = AMD_IOMMU_GUEST_IR_VAPIC;
+			break;
+		}
+	}
+	return 1;
+}
+
 static int __init parse_amd_iommu_options(char *str)
 {
 	for (; *str; ++str) {
@@ -2521,6 +2690,7 @@ static int __init parse_ivrs_acpihid(char *str)
 
 __setup("amd_iommu_dump",	parse_amd_iommu_dump);
 __setup("amd_iommu=",		parse_amd_iommu_options);
+__setup("amd_iommu_intr=",	parse_amd_iommu_intr);
 __setup("ivrs_ioapic",		parse_ivrs_ioapic);
 __setup("ivrs_hpet",		parse_ivrs_hpet);
 __setup("ivrs_acpihid",		parse_ivrs_acpihid);
diff --git a/drivers/iommu/amd_iommu_proto.h b/drivers/iommu/amd_iommu_proto.h
index 0bd9eb374462..7eb60c15c582 100644
--- a/drivers/iommu/amd_iommu_proto.h
+++ b/drivers/iommu/amd_iommu_proto.h
@@ -38,6 +38,7 @@ extern int amd_iommu_enable(void);
 extern void amd_iommu_disable(void);
 extern int amd_iommu_reenable(int);
 extern int amd_iommu_enable_faulting(void);
+extern int amd_iommu_guest_ir;
 
 /* IOMMUv2 specific functions */
 struct iommu_domain;
@@ -78,12 +79,6 @@ static inline int amd_iommu_create_irq_domain(struct amd_iommu *iommu)
 extern int amd_iommu_complete_ppr(struct pci_dev *pdev, int pasid,
 				  int status, int tag);
 
-#ifndef CONFIG_AMD_IOMMU_STATS
-
-static inline void amd_iommu_stats_init(void) { }
-
-#endif /* !CONFIG_AMD_IOMMU_STATS */
-
 static inline bool is_rd890_iommu(struct pci_dev *pdev)
 {
 	return (pdev->vendor == PCI_VENDOR_ID_ATI) &&
diff --git a/drivers/iommu/amd_iommu_types.h b/drivers/iommu/amd_iommu_types.h
index caf5e3822715..0d91785ebdc3 100644
--- a/drivers/iommu/amd_iommu_types.h
+++ b/drivers/iommu/amd_iommu_types.h
@@ -22,6 +22,7 @@
 
 #include <linux/types.h>
 #include <linux/mutex.h>
+#include <linux/msi.h>
 #include <linux/list.h>
 #include <linux/spinlock.h>
 #include <linux/pci.h>
@@ -69,6 +70,8 @@
 #define MMIO_EXCL_LIMIT_OFFSET  0x0028
 #define MMIO_EXT_FEATURES	0x0030
 #define MMIO_PPR_LOG_OFFSET	0x0038
+#define MMIO_GA_LOG_BASE_OFFSET	0x00e0
+#define MMIO_GA_LOG_TAIL_OFFSET	0x00e8
 #define MMIO_CMD_HEAD_OFFSET	0x2000
 #define MMIO_CMD_TAIL_OFFSET	0x2008
 #define MMIO_EVT_HEAD_OFFSET	0x2010
@@ -76,6 +79,8 @@
 #define MMIO_STATUS_OFFSET	0x2020
 #define MMIO_PPR_HEAD_OFFSET	0x2030
 #define MMIO_PPR_TAIL_OFFSET	0x2038
+#define MMIO_GA_HEAD_OFFSET	0x2040
+#define MMIO_GA_TAIL_OFFSET	0x2048
 #define MMIO_CNTR_CONF_OFFSET	0x4000
 #define MMIO_CNTR_REG_OFFSET	0x40000
 #define MMIO_REG_END_OFFSET	0x80000
@@ -92,6 +97,7 @@
 #define FEATURE_GA		(1ULL<<7)
 #define FEATURE_HE		(1ULL<<8)
 #define FEATURE_PC		(1ULL<<9)
+#define FEATURE_GAM_VAPIC	(1ULL<<21)
 
 #define FEATURE_PASID_SHIFT	32
 #define FEATURE_PASID_MASK	(0x1fULL << FEATURE_PASID_SHIFT)
@@ -110,6 +116,9 @@
 #define MMIO_STATUS_EVT_INT_MASK	(1 << 1)
 #define MMIO_STATUS_COM_WAIT_INT_MASK	(1 << 2)
 #define MMIO_STATUS_PPR_INT_MASK	(1 << 6)
+#define MMIO_STATUS_GALOG_RUN_MASK	(1 << 8)
+#define MMIO_STATUS_GALOG_OVERFLOW_MASK	(1 << 9)
+#define MMIO_STATUS_GALOG_INT_MASK	(1 << 10)
 
 /* event logging constants */
 #define EVENT_ENTRY_SIZE	0x10
@@ -146,6 +155,10 @@
 #define CONTROL_PPFINT_EN       0x0eULL
 #define CONTROL_PPR_EN          0x0fULL
 #define CONTROL_GT_EN           0x10ULL
+#define CONTROL_GA_EN           0x11ULL
+#define CONTROL_GAM_EN          0x19ULL
+#define CONTROL_GALOG_EN        0x1CULL
+#define CONTROL_GAINT_EN        0x1DULL
 
 #define CTRL_INV_TO_MASK	(7 << CONTROL_INV_TIMEOUT)
 #define CTRL_INV_TO_NONE	0
@@ -224,6 +237,19 @@
 
 #define PPR_REQ_FAULT		0x01
 
+/* Constants for GA Log handling */
+#define GA_LOG_ENTRIES		512
+#define GA_LOG_SIZE_SHIFT	56
+#define GA_LOG_SIZE_512		(0x8ULL << GA_LOG_SIZE_SHIFT)
+#define GA_ENTRY_SIZE		8
+#define GA_LOG_SIZE		(GA_ENTRY_SIZE * GA_LOG_ENTRIES)
+
+#define GA_TAG(x)		(u32)(x & 0xffffffffULL)
+#define GA_DEVID(x)		(u16)(((x) >> 32) & 0xffffULL)
+#define GA_REQ_TYPE(x)		(((x) >> 60) & 0xfULL)
+
+#define GA_GUEST_NR		0x1
+
 #define PAGE_MODE_NONE    0x00
 #define PAGE_MODE_1_LEVEL 0x01
 #define PAGE_MODE_2_LEVEL 0x02
@@ -329,6 +355,12 @@
 #define IOMMU_CAP_NPCACHE 26
 #define IOMMU_CAP_EFR     27
 
+/* IOMMU Feature Reporting Field (for IVHD type 10h */
+#define IOMMU_FEAT_GASUP_SHIFT	6
+
+/* IOMMU Extended Feature Register (EFR) */
+#define IOMMU_EFR_GASUP_SHIFT	7
+
 #define MAX_DOMAIN_ID 65536
 
 /* Protection domain flags */
@@ -400,6 +432,7 @@ struct amd_iommu_fault {
 
 struct iommu_domain;
 struct irq_domain;
+struct amd_irte_ops;
 
 /*
  * This structure contains generic data for  IOMMU protection domains
@@ -490,6 +523,12 @@ struct amd_iommu {
 	/* Base of the PPR log, if present */
 	u8 *ppr_log;
 
+	/* Base of the GA log, if present */
+	u8 *ga_log;
+
+	/* Tail of the GA log, if present */
+	u8 *ga_log_tail;
+
 	/* true if interrupts for this IOMMU are already enabled */
 	bool int_enabled;
 
@@ -523,7 +562,11 @@ struct amd_iommu {
 #ifdef CONFIG_IRQ_REMAP
 	struct irq_domain *ir_domain;
 	struct irq_domain *msi_domain;
+
+	struct amd_irte_ops *irte_ops;
 #endif
+
+	volatile u64 __aligned(8) cmd_sem;
 };
 
 #define ACPIHID_UID_LEN 256
@@ -681,4 +724,112 @@ static inline int get_hpet_devid(int id)
 	return -EINVAL;
 }
 
+enum amd_iommu_intr_mode_type {
+	AMD_IOMMU_GUEST_IR_LEGACY,
+
+	/* This mode is not visible to users. It is used when
+	 * we cannot fully enable vAPIC and fallback to only support
+	 * legacy interrupt remapping via 128-bit IRTE.
+	 */
+	AMD_IOMMU_GUEST_IR_LEGACY_GA,
+	AMD_IOMMU_GUEST_IR_VAPIC,
+};
+
+#define AMD_IOMMU_GUEST_IR_GA(x)	(x == AMD_IOMMU_GUEST_IR_VAPIC || \
+					 x == AMD_IOMMU_GUEST_IR_LEGACY_GA)
+
+#define AMD_IOMMU_GUEST_IR_VAPIC(x)	(x == AMD_IOMMU_GUEST_IR_VAPIC)
+
+union irte {
+	u32 val;
+	struct {
+		u32 valid	: 1,
+		    no_fault	: 1,
+		    int_type	: 3,
+		    rq_eoi	: 1,
+		    dm		: 1,
+		    rsvd_1	: 1,
+		    destination	: 8,
+		    vector	: 8,
+		    rsvd_2	: 8;
+	} fields;
+};
+
+union irte_ga_lo {
+	u64 val;
+
+	/* For int remapping */
+	struct {
+		u64 valid	: 1,
+		    no_fault	: 1,
+		    /* ------ */
+		    int_type	: 3,
+		    rq_eoi	: 1,
+		    dm		: 1,
+		    /* ------ */
+		    guest_mode	: 1,
+		    destination	: 8,
+		    rsvd	: 48;
+	} fields_remap;
+
+	/* For guest vAPIC */
+	struct {
+		u64 valid	: 1,
+		    no_fault	: 1,
+		    /* ------ */
+		    ga_log_intr	: 1,
+		    rsvd1	: 3,
+		    is_run	: 1,
+		    /* ------ */
+		    guest_mode	: 1,
+		    destination	: 8,
+		    rsvd2	: 16,
+		    ga_tag	: 32;
+	} fields_vapic;
+};
+
+union irte_ga_hi {
+	u64 val;
+	struct {
+		u64 vector	: 8,
+		    rsvd_1	: 4,
+		    ga_root_ptr	: 40,
+		    rsvd_2	: 12;
+	} fields;
+};
+
+struct irte_ga {
+	union irte_ga_lo lo;
+	union irte_ga_hi hi;
+};
+
+struct irq_2_irte {
+	u16 devid; /* Device ID for IRTE table */
+	u16 index; /* Index into IRTE table*/
+};
+
+struct amd_ir_data {
+	u32 cached_ga_tag;
+	struct irq_2_irte irq_2_irte;
+	struct msi_msg msi_entry;
+	void *entry;    /* Pointer to union irte or struct irte_ga */
+	void *ref;      /* Pointer to the actual irte */
+};
+
+struct amd_irte_ops {
+	void (*prepare)(void *, u32, u32, u8, u32, int);
+	void (*activate)(void *, u16, u16);
+	void (*deactivate)(void *, u16, u16);
+	void (*set_affinity)(void *, u16, u16, u8, u32);
+	void *(*get)(struct irq_remap_table *, int);
+	void (*set_allocated)(struct irq_remap_table *, int);
+	bool (*is_allocated)(struct irq_remap_table *, int);
+	void (*clear_allocated)(struct irq_remap_table *, int);
+};
+
+#ifdef CONFIG_IRQ_REMAP
+extern struct amd_irte_ops irte_32_ops;
+extern struct amd_irte_ops irte_128_ops;
+#endif
+
 #endif /* _ASM_X86_AMD_IOMMU_TYPES_H */
diff --git a/drivers/iommu/arm-smmu-v3.c b/drivers/iommu/arm-smmu-v3.c
index 641e88761319..15c01c3cd540 100644
--- a/drivers/iommu/arm-smmu-v3.c
+++ b/drivers/iommu/arm-smmu-v3.c
@@ -30,10 +30,13 @@
 #include <linux/msi.h>
 #include <linux/of.h>
 #include <linux/of_address.h>
+#include <linux/of_iommu.h>
 #include <linux/of_platform.h>
 #include <linux/pci.h>
 #include <linux/platform_device.h>
 
+#include <linux/amba/bus.h>
+
 #include "io-pgtable.h"
 
 /* MMIO registers */
@@ -123,6 +126,10 @@
 #define CR2_RECINVSID			(1 << 1)
 #define CR2_E2H				(1 << 0)
 
+#define ARM_SMMU_GBPA			0x44
+#define GBPA_ABORT			(1 << 20)
+#define GBPA_UPDATE			(1 << 31)
+
 #define ARM_SMMU_IRQ_CTRL		0x50
 #define IRQ_CTRL_EVTQ_IRQEN		(1 << 2)
 #define IRQ_CTRL_PRIQ_IRQEN		(1 << 1)
@@ -260,6 +267,9 @@
 #define STRTAB_STE_1_SHCFG_INCOMING	1UL
 #define STRTAB_STE_1_SHCFG_SHIFT	44
 
+#define STRTAB_STE_1_PRIVCFG_UNPRIV	2UL
+#define STRTAB_STE_1_PRIVCFG_SHIFT	48
+
 #define STRTAB_STE_2_S2VMID_SHIFT	0
 #define STRTAB_STE_2_S2VMID_MASK	0xffffUL
 #define STRTAB_STE_2_VTCR_SHIFT		32
@@ -606,12 +616,9 @@ struct arm_smmu_device {
 	struct arm_smmu_strtab_cfg	strtab_cfg;
 };
 
-/* SMMU private data for an IOMMU group */
-struct arm_smmu_group {
+/* SMMU private data for each master */
+struct arm_smmu_master_data {
 	struct arm_smmu_device		*smmu;
-	struct arm_smmu_domain		*domain;
-	int				num_sids;
-	u32				*sids;
 	struct arm_smmu_strtab_ent	ste;
 };
 
@@ -713,19 +720,15 @@ static void queue_inc_prod(struct arm_smmu_queue *q)
 	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)
+/*
+ * Wait for the SMMU to consume items. If drain is true, wait until the queue
+ * is empty. Otherwise, wait until there is at least one free slot.
+ */
+static int queue_poll_cons(struct arm_smmu_queue *q, bool drain, 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)) {
+	while (queue_sync_cons(q), (drain ? !queue_empty(q) : queue_full(q))) {
 		if (ktime_compare(ktime_get(), timeout) > 0)
 			return -ETIMEDOUT;
 
@@ -896,8 +899,8 @@ static void arm_smmu_cmdq_skip_err(struct arm_smmu_device *smmu)
 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];
+	unsigned long flags;
 	bool wfe = !!(smmu->features & ARM_SMMU_FEAT_SEV);
 	struct arm_smmu_queue *q = &smmu->cmdq.q;
 
@@ -907,20 +910,15 @@ static void arm_smmu_cmdq_issue_cmd(struct arm_smmu_device *smmu,
 		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))
+	spin_lock_irqsave(&smmu->cmdq.lock, flags);
+	while (queue_insert_raw(q, cmd) == -ENOSPC) {
+		if (queue_poll_cons(q, false, wfe))
 			dev_err_ratelimited(smmu->dev, "CMDQ timeout\n");
 	}
 
-	if (ent->opcode == CMDQ_OP_CMD_SYNC && queue_poll_cons(q, until, wfe))
+	if (ent->opcode == CMDQ_OP_CMD_SYNC && queue_poll_cons(q, true, wfe))
 		dev_err_ratelimited(smmu->dev, "CMD_SYNC timeout\n");
-	spin_unlock(&smmu->cmdq.lock);
+	spin_unlock_irqrestore(&smmu->cmdq.lock, flags);
 }
 
 /* Context descriptor manipulation functions */
@@ -1073,7 +1071,9 @@ static void arm_smmu_write_strtab_ent(struct arm_smmu_device *smmu, u32 sid,
 #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);
+			 STRTAB_STE_1_STRW_NSEL1 << STRTAB_STE_1_STRW_SHIFT |
+			 STRTAB_STE_1_PRIVCFG_UNPRIV <<
+			 STRTAB_STE_1_PRIVCFG_SHIFT);
 
 		if (smmu->features & ARM_SMMU_FEAT_STALLS)
 			dst[1] |= cpu_to_le64(STRTAB_STE_1_S1STALLD);
@@ -1161,36 +1161,66 @@ static irqreturn_t arm_smmu_evtq_thread(int irq, void *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;
+	do {
+		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]);
-	}
+			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]);
+
+		}
+
+		/*
+		 * 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");
+	} while (!queue_empty(q));
 
 	/* 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;
+static void arm_smmu_handle_ppr(struct arm_smmu_device *smmu, u64 *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,
+			},
+		};
 
-	/*
-	 * 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;
+		arm_smmu_cmdq_issue_cmd(smmu, &cmd);
+	}
 }
 
 static irqreturn_t arm_smmu_priq_thread(int irq, void *dev)
@@ -1199,63 +1229,19 @@ static irqreturn_t arm_smmu_priq_thread(int irq, void *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;
+	do {
+		while (!queue_remove_raw(q, evt))
+			arm_smmu_handle_ppr(smmu, evt);
 
-		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);
-		}
-	}
+		if (queue_sync_prod(q) == -EOVERFLOW)
+			dev_err(smmu->dev, "PRIQ overflow detected -- requests lost\n");
+	} while (!queue_empty(q));
 
 	/* 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 */
@@ -1288,15 +1274,11 @@ static irqreturn_t arm_smmu_gerror_handler(int irq, void *dev)
 	if (active & GERROR_MSI_GERROR_ABT_ERR)
 		dev_warn(smmu->dev, "GERROR MSI write aborted\n");
 
-	if (active & GERROR_MSI_PRIQ_ABT_ERR) {
+	if (active & GERROR_MSI_PRIQ_ABT_ERR)
 		dev_warn(smmu->dev, "PRIQ MSI write aborted\n");
-		arm_smmu_priq_handler(irq, smmu->dev);
-	}
 
-	if (active & GERROR_MSI_EVTQ_ABT_ERR) {
+	if (active & GERROR_MSI_EVTQ_ABT_ERR)
 		dev_warn(smmu->dev, "EVTQ MSI write aborted\n");
-		arm_smmu_evtq_handler(irq, smmu->dev);
-	}
 
 	if (active & GERROR_MSI_CMDQ_ABT_ERR) {
 		dev_warn(smmu->dev, "CMDQ MSI write aborted\n");
@@ -1569,6 +1551,8 @@ static int arm_smmu_domain_finalise(struct iommu_domain *domain)
 		return -ENOMEM;
 
 	domain->pgsize_bitmap = pgtbl_cfg.pgsize_bitmap;
+	domain->geometry.aperture_end = (1UL << ias) - 1;
+	domain->geometry.force_aperture = true;
 	smmu_domain->pgtbl_ops = pgtbl_ops;
 
 	ret = finalise_stage_fn(smmu_domain, &pgtbl_cfg);
@@ -1578,20 +1562,6 @@ static int arm_smmu_domain_finalise(struct iommu_domain *domain)
 	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;
@@ -1614,27 +1584,17 @@ static __le64 *arm_smmu_get_step_for_sid(struct arm_smmu_device *smmu, u32 sid)
 	return step;
 }
 
-static int arm_smmu_install_ste_for_group(struct arm_smmu_group *smmu_group)
+static int arm_smmu_install_ste_for_dev(struct iommu_fwspec *fwspec)
 {
 	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;
+	struct arm_smmu_master_data *master = fwspec->iommu_priv;
+	struct arm_smmu_device *smmu = master->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];
+	for (i = 0; i < fwspec->num_ids; ++i) {
+		u32 sid = fwspec->ids[i];
 		__le64 *step = arm_smmu_get_step_for_sid(smmu, sid);
 
-		arm_smmu_write_strtab_ent(smmu, sid, step, ste);
+		arm_smmu_write_strtab_ent(smmu, sid, step, &master->ste);
 	}
 
 	return 0;
@@ -1642,13 +1602,11 @@ static int arm_smmu_install_ste_for_group(struct arm_smmu_group *smmu_group)
 
 static void arm_smmu_detach_dev(struct device *dev)
 {
-	struct arm_smmu_group *smmu_group = arm_smmu_group_get(dev);
+	struct arm_smmu_master_data *master = dev->iommu_fwspec->iommu_priv;
 
-	smmu_group->ste.bypass = true;
-	if (arm_smmu_install_ste_for_group(smmu_group) < 0)
+	master->ste.bypass = true;
+	if (arm_smmu_install_ste_for_dev(dev->iommu_fwspec) < 0)
 		dev_warn(dev, "failed to install bypass STE\n");
-
-	smmu_group->domain = NULL;
 }
 
 static int arm_smmu_attach_dev(struct iommu_domain *domain, struct device *dev)
@@ -1656,16 +1614,20 @@ 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);
+	struct arm_smmu_master_data *master;
+	struct arm_smmu_strtab_ent *ste;
 
-	if (!smmu_group)
+	if (!dev->iommu_fwspec)
 		return -ENOENT;
 
+	master = dev->iommu_fwspec->iommu_priv;
+	smmu = master->smmu;
+	ste = &master->ste;
+
 	/* Already attached to a different domain? */
-	if (smmu_group->domain && smmu_group->domain != smmu_domain)
+	if (!ste->bypass)
 		arm_smmu_detach_dev(dev);
 
-	smmu = smmu_group->smmu;
 	mutex_lock(&smmu_domain->init_mutex);
 
 	if (!smmu_domain->smmu) {
@@ -1684,21 +1646,21 @@ static int arm_smmu_attach_dev(struct iommu_domain *domain, struct device *dev)
 		goto out_unlock;
 	}
 
-	/* Group already attached to this domain? */
-	if (smmu_group->domain)
-		goto out_unlock;
-
-	smmu_group->domain	= smmu_domain;
+	ste->bypass = false;
+	ste->valid = true;
 
-	/*
-	 * FIXME: This should always be "false" once we have IOMMU-backed
-	 * DMA ops for all devices behind the SMMU.
-	 */
-	smmu_group->ste.bypass	= domain->type == IOMMU_DOMAIN_DMA;
+	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;
+	}
 
-	ret = arm_smmu_install_ste_for_group(smmu_group);
+	ret = arm_smmu_install_ste_for_dev(dev->iommu_fwspec);
 	if (ret < 0)
-		smmu_group->domain = NULL;
+		ste->valid = false;
 
 out_unlock:
 	mutex_unlock(&smmu_domain->init_mutex);
@@ -1757,40 +1719,19 @@ arm_smmu_iova_to_phys(struct iommu_domain *domain, dma_addr_t iova)
 	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 struct platform_driver arm_smmu_driver;
 
-static void __arm_smmu_release_pci_iommudata(void *data)
+static int arm_smmu_match_node(struct device *dev, void *data)
 {
-	kfree(data);
+	return dev->of_node == data;
 }
 
-static struct arm_smmu_device *arm_smmu_get_for_pci_dev(struct pci_dev *pdev)
+static struct arm_smmu_device *arm_smmu_get_by_node(struct device_node *np)
 {
-	struct device_node *of_node;
-	struct platform_device *smmu_pdev;
-	struct arm_smmu_device *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 */
-	smmu_pdev = of_find_device_by_node(of_node);
-	if (smmu_pdev)
-		smmu = platform_get_drvdata(smmu_pdev);
-
-	of_node_put(of_node);
-	return smmu;
+	struct device *dev = driver_find_device(&arm_smmu_driver.driver, NULL,
+						np, arm_smmu_match_node);
+	put_device(dev);
+	return dev ? dev_get_drvdata(dev) : NULL;
 }
 
 static bool arm_smmu_sid_in_range(struct arm_smmu_device *smmu, u32 sid)
@@ -1803,94 +1744,91 @@ static bool arm_smmu_sid_in_range(struct arm_smmu_device *smmu, u32 sid)
 	return sid < limit;
 }
 
+static struct iommu_ops arm_smmu_ops;
+
 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;
+	struct arm_smmu_master_data *master;
+	struct iommu_fwspec *fwspec = dev->iommu_fwspec;
+	struct iommu_group *group;
 
-	/* We only support PCI, for now */
-	if (!dev_is_pci(dev))
+	if (!fwspec || fwspec->ops != &arm_smmu_ops)
 		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_remove_dev;
-		}
-
-		smmu_group = kzalloc(sizeof(*smmu_group), GFP_KERNEL);
-		if (!smmu_group) {
-			ret = -ENOMEM;
-			goto out_remove_dev;
-		}
-
-		smmu_group->ste.valid	= true;
-		smmu_group->smmu	= smmu;
-		iommu_group_set_iommudata(group, smmu_group,
-					  __arm_smmu_release_pci_iommudata);
+	/*
+	 * We _can_ actually withstand dodgy bus code re-calling add_device()
+	 * without an intervening remove_device()/of_xlate() sequence, but
+	 * we're not going to do so quietly...
+	 */
+	if (WARN_ON_ONCE(fwspec->iommu_priv)) {
+		master = fwspec->iommu_priv;
+		smmu = master->smmu;
 	} else {
-		smmu = smmu_group->smmu;
-	}
+		smmu = arm_smmu_get_by_node(to_of_node(fwspec->iommu_fwnode));
+		if (!smmu)
+			return -ENODEV;
+		master = kzalloc(sizeof(*master), GFP_KERNEL);
+		if (!master)
+			return -ENOMEM;
 
-	/* 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)
-			goto out_put_group;
+		master->smmu = smmu;
+		fwspec->iommu_priv = master;
 	}
 
-	/* 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_remove_dev;
-	}
+	/* Check the SIDs are in range of the SMMU and our stream table */
+	for (i = 0; i < fwspec->num_ids; i++) {
+		u32 sid = fwspec->ids[i];
 
-	/* 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_remove_dev;
-	}
+		if (!arm_smmu_sid_in_range(smmu, sid))
+			return -ERANGE;
 
-	/* 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_remove_dev;
+		/* Ensure l2 strtab is initialised */
+		if (smmu->features & ARM_SMMU_FEAT_2_LVL_STRTAB) {
+			ret = arm_smmu_init_l2_strtab(smmu, sid);
+			if (ret)
+				return ret;
+		}
 	}
 
-	/* Add the new SID */
-	sids[smmu_group->num_sids - 1] = sid;
-	smmu_group->sids = sids;
-
-out_put_group:
-	iommu_group_put(group);
-	return 0;
+	group = iommu_group_get_for_dev(dev);
+	if (!IS_ERR(group))
+		iommu_group_put(group);
 
-out_remove_dev:
-	iommu_group_remove_device(dev);
-	iommu_group_put(group);
-	return ret;
+	return PTR_ERR_OR_ZERO(group);
 }
 
 static void arm_smmu_remove_device(struct device *dev)
 {
+	struct iommu_fwspec *fwspec = dev->iommu_fwspec;
+	struct arm_smmu_master_data *master;
+
+	if (!fwspec || fwspec->ops != &arm_smmu_ops)
+		return;
+
+	master = fwspec->iommu_priv;
+	if (master && master->ste.valid)
+		arm_smmu_detach_dev(dev);
 	iommu_group_remove_device(dev);
+	kfree(master);
+	iommu_fwspec_free(dev);
+}
+
+static struct iommu_group *arm_smmu_device_group(struct device *dev)
+{
+	struct iommu_group *group;
+
+	/*
+	 * We don't support devices sharing stream IDs other than PCI RID
+	 * aliases, since the necessary ID-to-device lookup becomes rather
+	 * impractical given a potential sparse 32-bit stream ID space.
+	 */
+	if (dev_is_pci(dev))
+		group = pci_device_group(dev);
+	else
+		group = generic_device_group(dev);
+
+	return group;
 }
 
 static int arm_smmu_domain_get_attr(struct iommu_domain *domain,
@@ -1937,6 +1875,11 @@ out_unlock:
 	return ret;
 }
 
+static int arm_smmu_of_xlate(struct device *dev, struct of_phandle_args *args)
+{
+	return iommu_fwspec_add_ids(dev, args->args, 1);
+}
+
 static struct iommu_ops arm_smmu_ops = {
 	.capable		= arm_smmu_capable,
 	.domain_alloc		= arm_smmu_domain_alloc,
@@ -1948,9 +1891,10 @@ static struct iommu_ops arm_smmu_ops = {
 	.iova_to_phys		= arm_smmu_iova_to_phys,
 	.add_device		= arm_smmu_add_device,
 	.remove_device		= arm_smmu_remove_device,
-	.device_group		= pci_device_group,
+	.device_group		= arm_smmu_device_group,
 	.domain_get_attr	= arm_smmu_domain_get_attr,
 	.domain_set_attr	= arm_smmu_domain_set_attr,
+	.of_xlate		= arm_smmu_of_xlate,
 	.pgsize_bitmap		= -1UL, /* Restricted during device attach */
 };
 
@@ -2151,6 +2095,24 @@ static int arm_smmu_write_reg_sync(struct arm_smmu_device *smmu, u32 val,
 					  1, ARM_SMMU_POLL_TIMEOUT_US);
 }
 
+/* GBPA is "special" */
+static int arm_smmu_update_gbpa(struct arm_smmu_device *smmu, u32 set, u32 clr)
+{
+	int ret;
+	u32 reg, __iomem *gbpa = smmu->base + ARM_SMMU_GBPA;
+
+	ret = readl_relaxed_poll_timeout(gbpa, reg, !(reg & GBPA_UPDATE),
+					 1, ARM_SMMU_POLL_TIMEOUT_US);
+	if (ret)
+		return ret;
+
+	reg &= ~clr;
+	reg |= set;
+	writel_relaxed(reg | GBPA_UPDATE, gbpa);
+	return readl_relaxed_poll_timeout(gbpa, reg, !(reg & GBPA_UPDATE),
+					  1, ARM_SMMU_POLL_TIMEOUT_US);
+}
+
 static void arm_smmu_free_msis(void *data)
 {
 	struct device *dev = data;
@@ -2235,10 +2197,10 @@ static int arm_smmu_setup_irqs(struct arm_smmu_device *smmu)
 	/* Request interrupt lines */
 	irq = smmu->evtq.q.irq;
 	if (irq) {
-		ret = devm_request_threaded_irq(smmu->dev, irq,
-						arm_smmu_evtq_handler,
+		ret = devm_request_threaded_irq(smmu->dev, irq, NULL,
 						arm_smmu_evtq_thread,
-						0, "arm-smmu-v3-evtq", smmu);
+						IRQF_ONESHOT,
+						"arm-smmu-v3-evtq", smmu);
 		if (ret < 0)
 			dev_warn(smmu->dev, "failed to enable evtq irq\n");
 	}
@@ -2263,10 +2225,10 @@ static int arm_smmu_setup_irqs(struct arm_smmu_device *smmu)
 	if (smmu->features & ARM_SMMU_FEAT_PRI) {
 		irq = smmu->priq.q.irq;
 		if (irq) {
-			ret = devm_request_threaded_irq(smmu->dev, irq,
-							arm_smmu_priq_handler,
+			ret = devm_request_threaded_irq(smmu->dev, irq, NULL,
 							arm_smmu_priq_thread,
-							0, "arm-smmu-v3-priq",
+							IRQF_ONESHOT,
+							"arm-smmu-v3-priq",
 							smmu);
 			if (ret < 0)
 				dev_warn(smmu->dev,
@@ -2296,7 +2258,7 @@ static int arm_smmu_device_disable(struct arm_smmu_device *smmu)
 	return ret;
 }
 
-static int arm_smmu_device_reset(struct arm_smmu_device *smmu)
+static int arm_smmu_device_reset(struct arm_smmu_device *smmu, bool bypass)
 {
 	int ret;
 	u32 reg, enables;
@@ -2397,8 +2359,17 @@ static int arm_smmu_device_reset(struct arm_smmu_device *smmu)
 		return ret;
 	}
 
-	/* Enable the SMMU interface */
-	enables |= CR0_SMMUEN;
+
+	/* Enable the SMMU interface, or ensure bypass */
+	if (!bypass || disable_bypass) {
+		enables |= CR0_SMMUEN;
+	} else {
+		ret = arm_smmu_update_gbpa(smmu, 0, GBPA_ABORT);
+		if (ret) {
+			dev_err(smmu->dev, "GBPA not responding to update\n");
+			return ret;
+		}
+	}
 	ret = arm_smmu_write_reg_sync(smmu, enables, ARM_SMMU_CR0,
 				      ARM_SMMU_CR0ACK);
 	if (ret) {
@@ -2597,6 +2568,15 @@ static int arm_smmu_device_dt_probe(struct platform_device *pdev)
 	struct resource *res;
 	struct arm_smmu_device *smmu;
 	struct device *dev = &pdev->dev;
+	bool bypass = true;
+	u32 cells;
+
+	if (of_property_read_u32(dev->of_node, "#iommu-cells", &cells))
+		dev_err(dev, "missing #iommu-cells property\n");
+	else if (cells != 1)
+		dev_err(dev, "invalid #iommu-cells value (%d)\n", cells);
+	else
+		bypass = false;
 
 	smmu = devm_kzalloc(dev, sizeof(*smmu), GFP_KERNEL);
 	if (!smmu) {
@@ -2649,7 +2629,24 @@ static int arm_smmu_device_dt_probe(struct platform_device *pdev)
 	platform_set_drvdata(pdev, smmu);
 
 	/* Reset the device */
-	return arm_smmu_device_reset(smmu);
+	ret = arm_smmu_device_reset(smmu, bypass);
+	if (ret)
+		return ret;
+
+	/* And we're up. Go go go! */
+	of_iommu_set_ops(dev->of_node, &arm_smmu_ops);
+#ifdef CONFIG_PCI
+	pci_request_acs();
+	ret = bus_set_iommu(&pci_bus_type, &arm_smmu_ops);
+	if (ret)
+		return ret;
+#endif
+#ifdef CONFIG_ARM_AMBA
+	ret = bus_set_iommu(&amba_bustype, &arm_smmu_ops);
+	if (ret)
+		return ret;
+#endif
+	return bus_set_iommu(&platform_bus_type, &arm_smmu_ops);
 }
 
 static int arm_smmu_device_remove(struct platform_device *pdev)
@@ -2677,22 +2674,14 @@ static struct platform_driver arm_smmu_driver = {
 
 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;
-
-	pci_request_acs();
+	static bool registered;
+	int ret = 0;
 
-	return bus_set_iommu(&pci_bus_type, &arm_smmu_ops);
+	if (!registered) {
+		ret = platform_driver_register(&arm_smmu_driver);
+		registered = !ret;
+	}
+	return ret;
 }
 
 static void __exit arm_smmu_exit(void)
@@ -2703,6 +2692,20 @@ static void __exit arm_smmu_exit(void)
 subsys_initcall(arm_smmu_init);
 module_exit(arm_smmu_exit);
 
+static int __init arm_smmu_of_init(struct device_node *np)
+{
+	int ret = arm_smmu_init();
+
+	if (ret)
+		return ret;
+
+	if (!of_platform_device_create(np, NULL, platform_bus_type.dev_root))
+		return -ENODEV;
+
+	return 0;
+}
+IOMMU_OF_DECLARE(arm_smmuv3, "arm,smmu-v3", arm_smmu_of_init);
+
 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 2db74ebc3240..c841eb7a1a74 100644
--- a/drivers/iommu/arm-smmu.c
+++ b/drivers/iommu/arm-smmu.c
@@ -28,6 +28,7 @@
 
 #define pr_fmt(fmt) "arm-smmu: " fmt
 
+#include <linux/atomic.h>
 #include <linux/delay.h>
 #include <linux/dma-iommu.h>
 #include <linux/dma-mapping.h>
@@ -40,6 +41,8 @@
 #include <linux/module.h>
 #include <linux/of.h>
 #include <linux/of_address.h>
+#include <linux/of_device.h>
+#include <linux/of_iommu.h>
 #include <linux/pci.h>
 #include <linux/platform_device.h>
 #include <linux/slab.h>
@@ -49,15 +52,9 @@
 
 #include "io-pgtable.h"
 
-/* Maximum number of stream IDs assigned to a single device */
-#define MAX_MASTER_STREAMIDS		128
-
 /* Maximum number of context banks per SMMU */
 #define ARM_SMMU_MAX_CBS		128
 
-/* Maximum number of mapping groups per SMMU */
-#define ARM_SMMU_MAX_SMRS		128
-
 /* SMMU global address space */
 #define ARM_SMMU_GR0(smmu)		((smmu)->base)
 #define ARM_SMMU_GR1(smmu)		((smmu)->base + (1 << (smmu)->pgshift))
@@ -165,21 +162,27 @@
 #define ARM_SMMU_GR0_SMR(n)		(0x800 + ((n) << 2))
 #define SMR_VALID			(1 << 31)
 #define SMR_MASK_SHIFT			16
-#define SMR_MASK_MASK			0x7fff
 #define SMR_ID_SHIFT			0
-#define SMR_ID_MASK			0x7fff
 
 #define ARM_SMMU_GR0_S2CR(n)		(0xc00 + ((n) << 2))
 #define S2CR_CBNDX_SHIFT		0
 #define S2CR_CBNDX_MASK			0xff
 #define S2CR_TYPE_SHIFT			16
 #define S2CR_TYPE_MASK			0x3
-#define S2CR_TYPE_TRANS			(0 << S2CR_TYPE_SHIFT)
-#define S2CR_TYPE_BYPASS		(1 << S2CR_TYPE_SHIFT)
-#define S2CR_TYPE_FAULT			(2 << S2CR_TYPE_SHIFT)
+enum arm_smmu_s2cr_type {
+	S2CR_TYPE_TRANS,
+	S2CR_TYPE_BYPASS,
+	S2CR_TYPE_FAULT,
+};
 
 #define S2CR_PRIVCFG_SHIFT		24
-#define S2CR_PRIVCFG_UNPRIV		(2 << S2CR_PRIVCFG_SHIFT)
+#define S2CR_PRIVCFG_MASK		0x3
+enum arm_smmu_s2cr_privcfg {
+	S2CR_PRIVCFG_DEFAULT,
+	S2CR_PRIVCFG_DIPAN,
+	S2CR_PRIVCFG_UNPRIV,
+	S2CR_PRIVCFG_PRIV,
+};
 
 /* Context bank attribute registers */
 #define ARM_SMMU_GR1_CBAR(n)		(0x0 + ((n) << 2))
@@ -217,6 +220,7 @@
 #define ARM_SMMU_CB_TTBR0		0x20
 #define ARM_SMMU_CB_TTBR1		0x28
 #define ARM_SMMU_CB_TTBCR		0x30
+#define ARM_SMMU_CB_CONTEXTIDR		0x34
 #define ARM_SMMU_CB_S1_MAIR0		0x38
 #define ARM_SMMU_CB_S1_MAIR1		0x3c
 #define ARM_SMMU_CB_PAR			0x50
@@ -239,7 +243,6 @@
 #define SCTLR_AFE			(1 << 2)
 #define SCTLR_TRE			(1 << 1)
 #define SCTLR_M				(1 << 0)
-#define SCTLR_EAE_SBOP			(SCTLR_AFE | SCTLR_TRE)
 
 #define ARM_MMU500_ACTLR_CPRE		(1 << 1)
 
@@ -296,23 +299,33 @@ enum arm_smmu_implementation {
 	CAVIUM_SMMUV2,
 };
 
+struct arm_smmu_s2cr {
+	struct iommu_group		*group;
+	int				count;
+	enum arm_smmu_s2cr_type		type;
+	enum arm_smmu_s2cr_privcfg	privcfg;
+	u8				cbndx;
+};
+
+#define s2cr_init_val (struct arm_smmu_s2cr){				\
+	.type = disable_bypass ? S2CR_TYPE_FAULT : S2CR_TYPE_BYPASS,	\
+}
+
 struct arm_smmu_smr {
-	u8				idx;
 	u16				mask;
 	u16				id;
+	bool				valid;
 };
 
 struct arm_smmu_master_cfg {
-	int				num_streamids;
-	u16				streamids[MAX_MASTER_STREAMIDS];
-	struct arm_smmu_smr		*smrs;
-};
-
-struct arm_smmu_master {
-	struct device_node		*of_node;
-	struct rb_node			node;
-	struct arm_smmu_master_cfg	cfg;
+	struct arm_smmu_device		*smmu;
+	s16				smendx[];
 };
+#define INVALID_SMENDX			-1
+#define __fwspec_cfg(fw) ((struct arm_smmu_master_cfg *)fw->iommu_priv)
+#define fwspec_smmu(fw)  (__fwspec_cfg(fw)->smmu)
+#define for_each_cfg_sme(fw, i, idx) \
+	for (i = 0; idx = __fwspec_cfg(fw)->smendx[i], i < fw->num_ids; ++i)
 
 struct arm_smmu_device {
 	struct device			*dev;
@@ -346,7 +359,11 @@ struct arm_smmu_device {
 	atomic_t			irptndx;
 
 	u32				num_mapping_groups;
-	DECLARE_BITMAP(smr_map, ARM_SMMU_MAX_SMRS);
+	u16				streamid_mask;
+	u16				smr_mask_mask;
+	struct arm_smmu_smr		*smrs;
+	struct arm_smmu_s2cr		*s2crs;
+	struct mutex			stream_map_mutex;
 
 	unsigned long			va_size;
 	unsigned long			ipa_size;
@@ -357,9 +374,6 @@ struct arm_smmu_device {
 	u32				num_context_irqs;
 	unsigned int			*irqs;
 
-	struct list_head		list;
-	struct rb_root			masters;
-
 	u32				cavium_id_base; /* Specific to Cavium */
 };
 
@@ -397,15 +411,6 @@ struct arm_smmu_domain {
 	struct iommu_domain		domain;
 };
 
-struct arm_smmu_phandle_args {
-	struct device_node *np;
-	int args_count;
-	uint32_t args[MAX_MASTER_STREAMIDS];
-};
-
-static DEFINE_SPINLOCK(arm_smmu_devices_lock);
-static LIST_HEAD(arm_smmu_devices);
-
 struct arm_smmu_option_prop {
 	u32 opt;
 	const char *prop;
@@ -413,6 +418,8 @@ struct arm_smmu_option_prop {
 
 static atomic_t cavium_smmu_context_count = ATOMIC_INIT(0);
 
+static bool using_legacy_binding, using_generic_binding;
+
 static struct arm_smmu_option_prop arm_smmu_options[] = {
 	{ ARM_SMMU_OPT_SECURE_CFG_ACCESS, "calxeda,smmu-secure-config-access" },
 	{ 0, NULL},
@@ -444,131 +451,86 @@ static struct device_node *dev_get_dev_node(struct device *dev)
 
 		while (!pci_is_root_bus(bus))
 			bus = bus->parent;
-		return bus->bridge->parent->of_node;
+		return of_node_get(bus->bridge->parent->of_node);
 	}
 
-	return dev->of_node;
+	return of_node_get(dev->of_node);
 }
 
-static struct arm_smmu_master *find_smmu_master(struct arm_smmu_device *smmu,
-						struct device_node *dev_node)
+static int __arm_smmu_get_pci_sid(struct pci_dev *pdev, u16 alias, void *data)
 {
-	struct rb_node *node = smmu->masters.rb_node;
-
-	while (node) {
-		struct arm_smmu_master *master;
-
-		master = container_of(node, struct arm_smmu_master, node);
-
-		if (dev_node < master->of_node)
-			node = node->rb_left;
-		else if (dev_node > master->of_node)
-			node = node->rb_right;
-		else
-			return master;
-	}
-
-	return NULL;
+	*((__be32 *)data) = cpu_to_be32(alias);
+	return 0; /* Continue walking */
 }
 
-static struct arm_smmu_master_cfg *
-find_smmu_master_cfg(struct device *dev)
+static int __find_legacy_master_phandle(struct device *dev, void *data)
 {
-	struct arm_smmu_master_cfg *cfg = NULL;
-	struct iommu_group *group = iommu_group_get(dev);
-
-	if (group) {
-		cfg = iommu_group_get_iommudata(group);
-		iommu_group_put(group);
-	}
-
-	return cfg;
+	struct of_phandle_iterator *it = *(void **)data;
+	struct device_node *np = it->node;
+	int err;
+
+	of_for_each_phandle(it, err, dev->of_node, "mmu-masters",
+			    "#stream-id-cells", 0)
+		if (it->node == np) {
+			*(void **)data = dev;
+			return 1;
+		}
+	it->node = np;
+	return err == -ENOENT ? 0 : err;
 }
 
-static int insert_smmu_master(struct arm_smmu_device *smmu,
-			      struct arm_smmu_master *master)
+static struct platform_driver arm_smmu_driver;
+static struct iommu_ops arm_smmu_ops;
+
+static int arm_smmu_register_legacy_master(struct device *dev,
+					   struct arm_smmu_device **smmu)
 {
-	struct rb_node **new, *parent;
-
-	new = &smmu->masters.rb_node;
-	parent = NULL;
-	while (*new) {
-		struct arm_smmu_master *this
-			= container_of(*new, struct arm_smmu_master, node);
-
-		parent = *new;
-		if (master->of_node < this->of_node)
-			new = &((*new)->rb_left);
-		else if (master->of_node > this->of_node)
-			new = &((*new)->rb_right);
-		else
-			return -EEXIST;
+	struct device *smmu_dev;
+	struct device_node *np;
+	struct of_phandle_iterator it;
+	void *data = &it;
+	u32 *sids;
+	__be32 pci_sid;
+	int err;
+
+	np = dev_get_dev_node(dev);
+	if (!np || !of_find_property(np, "#stream-id-cells", NULL)) {
+		of_node_put(np);
+		return -ENODEV;
 	}
 
-	rb_link_node(&master->node, parent, new);
-	rb_insert_color(&master->node, &smmu->masters);
-	return 0;
-}
-
-static int register_smmu_master(struct arm_smmu_device *smmu,
-				struct device *dev,
-				struct arm_smmu_phandle_args *masterspec)
-{
-	int i;
-	struct arm_smmu_master *master;
+	it.node = np;
+	err = driver_for_each_device(&arm_smmu_driver.driver, NULL, &data,
+				     __find_legacy_master_phandle);
+	smmu_dev = data;
+	of_node_put(np);
+	if (err == 0)
+		return -ENODEV;
+	if (err < 0)
+		return err;
 
-	master = find_smmu_master(smmu, masterspec->np);
-	if (master) {
-		dev_err(dev,
-			"rejecting multiple registrations for master device %s\n",
-			masterspec->np->name);
-		return -EBUSY;
+	if (dev_is_pci(dev)) {
+		/* "mmu-masters" assumes Stream ID == Requester ID */
+		pci_for_each_dma_alias(to_pci_dev(dev), __arm_smmu_get_pci_sid,
+				       &pci_sid);
+		it.cur = &pci_sid;
+		it.cur_count = 1;
 	}
 
-	if (masterspec->args_count > MAX_MASTER_STREAMIDS) {
-		dev_err(dev,
-			"reached maximum number (%d) of stream IDs for master device %s\n",
-			MAX_MASTER_STREAMIDS, masterspec->np->name);
-		return -ENOSPC;
-	}
+	err = iommu_fwspec_init(dev, &smmu_dev->of_node->fwnode,
+				&arm_smmu_ops);
+	if (err)
+		return err;
 
-	master = devm_kzalloc(dev, sizeof(*master), GFP_KERNEL);
-	if (!master)
+	sids = kcalloc(it.cur_count, sizeof(*sids), GFP_KERNEL);
+	if (!sids)
 		return -ENOMEM;
 
-	master->of_node			= masterspec->np;
-	master->cfg.num_streamids	= masterspec->args_count;
-
-	for (i = 0; i < master->cfg.num_streamids; ++i) {
-		u16 streamid = masterspec->args[i];
-
-		if (!(smmu->features & ARM_SMMU_FEAT_STREAM_MATCH) &&
-		     (streamid >= smmu->num_mapping_groups)) {
-			dev_err(dev,
-				"stream ID for master device %s greater than maximum allowed (%d)\n",
-				masterspec->np->name, smmu->num_mapping_groups);
-			return -ERANGE;
-		}
-		master->cfg.streamids[i] = streamid;
-	}
-	return insert_smmu_master(smmu, master);
-}
-
-static struct arm_smmu_device *find_smmu_for_device(struct device *dev)
-{
-	struct arm_smmu_device *smmu;
-	struct arm_smmu_master *master = NULL;
-	struct device_node *dev_node = dev_get_dev_node(dev);
-
-	spin_lock(&arm_smmu_devices_lock);
-	list_for_each_entry(smmu, &arm_smmu_devices, list) {
-		master = find_smmu_master(smmu, dev_node);
-		if (master)
-			break;
-	}
-	spin_unlock(&arm_smmu_devices_lock);
-
-	return master ? smmu : NULL;
+	*smmu = dev_get_drvdata(smmu_dev);
+	of_phandle_iterator_args(&it, sids, it.cur_count);
+	err = iommu_fwspec_add_ids(dev, sids, it.cur_count);
+	kfree(sids);
+	return err;
 }
 
 static int __arm_smmu_alloc_bitmap(unsigned long *map, int start, int end)
@@ -738,7 +700,7 @@ static irqreturn_t arm_smmu_global_fault(int irq, void *dev)
 static void arm_smmu_init_context_bank(struct arm_smmu_domain *smmu_domain,
 				       struct io_pgtable_cfg *pgtbl_cfg)
 {
-	u32 reg;
+	u32 reg, reg2;
 	u64 reg64;
 	bool stage1;
 	struct arm_smmu_cfg *cfg = &smmu_domain->cfg;
@@ -781,14 +743,22 @@ static void arm_smmu_init_context_bank(struct arm_smmu_domain *smmu_domain,
 
 	/* TTBRs */
 	if (stage1) {
-		reg64 = pgtbl_cfg->arm_lpae_s1_cfg.ttbr[0];
-
-		reg64 |= ((u64)ARM_SMMU_CB_ASID(smmu, cfg)) << TTBRn_ASID_SHIFT;
-		writeq_relaxed(reg64, cb_base + ARM_SMMU_CB_TTBR0);
-
-		reg64 = pgtbl_cfg->arm_lpae_s1_cfg.ttbr[1];
-		reg64 |= ((u64)ARM_SMMU_CB_ASID(smmu, cfg)) << TTBRn_ASID_SHIFT;
-		writeq_relaxed(reg64, cb_base + ARM_SMMU_CB_TTBR1);
+		u16 asid = ARM_SMMU_CB_ASID(smmu, cfg);
+
+		if (cfg->fmt == ARM_SMMU_CTX_FMT_AARCH32_S) {
+			reg = pgtbl_cfg->arm_v7s_cfg.ttbr[0];
+			writel_relaxed(reg, cb_base + ARM_SMMU_CB_TTBR0);
+			reg = pgtbl_cfg->arm_v7s_cfg.ttbr[1];
+			writel_relaxed(reg, cb_base + ARM_SMMU_CB_TTBR1);
+			writel_relaxed(asid, cb_base + ARM_SMMU_CB_CONTEXTIDR);
+		} else {
+			reg64 = pgtbl_cfg->arm_lpae_s1_cfg.ttbr[0];
+			reg64 |= (u64)asid << TTBRn_ASID_SHIFT;
+			writeq_relaxed(reg64, cb_base + ARM_SMMU_CB_TTBR0);
+			reg64 = pgtbl_cfg->arm_lpae_s1_cfg.ttbr[1];
+			reg64 |= (u64)asid << TTBRn_ASID_SHIFT;
+			writeq_relaxed(reg64, cb_base + ARM_SMMU_CB_TTBR1);
+		}
 	} else {
 		reg64 = pgtbl_cfg->arm_lpae_s2_cfg.vttbr;
 		writeq_relaxed(reg64, cb_base + ARM_SMMU_CB_TTBR0);
@@ -796,28 +766,36 @@ static void arm_smmu_init_context_bank(struct arm_smmu_domain *smmu_domain,
 
 	/* TTBCR */
 	if (stage1) {
-		reg = pgtbl_cfg->arm_lpae_s1_cfg.tcr;
-		writel_relaxed(reg, cb_base + ARM_SMMU_CB_TTBCR);
-		if (smmu->version > ARM_SMMU_V1) {
-			reg = pgtbl_cfg->arm_lpae_s1_cfg.tcr >> 32;
-			reg |= TTBCR2_SEP_UPSTREAM;
-			writel_relaxed(reg, cb_base + ARM_SMMU_CB_TTBCR2);
+		if (cfg->fmt == ARM_SMMU_CTX_FMT_AARCH32_S) {
+			reg = pgtbl_cfg->arm_v7s_cfg.tcr;
+			reg2 = 0;
+		} else {
+			reg = pgtbl_cfg->arm_lpae_s1_cfg.tcr;
+			reg2 = pgtbl_cfg->arm_lpae_s1_cfg.tcr >> 32;
+			reg2 |= TTBCR2_SEP_UPSTREAM;
 		}
+		if (smmu->version > ARM_SMMU_V1)
+			writel_relaxed(reg2, cb_base + ARM_SMMU_CB_TTBCR2);
 	} else {
 		reg = pgtbl_cfg->arm_lpae_s2_cfg.vtcr;
-		writel_relaxed(reg, cb_base + ARM_SMMU_CB_TTBCR);
 	}
+	writel_relaxed(reg, cb_base + ARM_SMMU_CB_TTBCR);
 
 	/* MAIRs (stage-1 only) */
 	if (stage1) {
-		reg = pgtbl_cfg->arm_lpae_s1_cfg.mair[0];
+		if (cfg->fmt == ARM_SMMU_CTX_FMT_AARCH32_S) {
+			reg = pgtbl_cfg->arm_v7s_cfg.prrr;
+			reg2 = pgtbl_cfg->arm_v7s_cfg.nmrr;
+		} else {
+			reg = pgtbl_cfg->arm_lpae_s1_cfg.mair[0];
+			reg2 = pgtbl_cfg->arm_lpae_s1_cfg.mair[1];
+		}
 		writel_relaxed(reg, cb_base + ARM_SMMU_CB_S1_MAIR0);
-		reg = pgtbl_cfg->arm_lpae_s1_cfg.mair[1];
-		writel_relaxed(reg, cb_base + ARM_SMMU_CB_S1_MAIR1);
+		writel_relaxed(reg2, cb_base + ARM_SMMU_CB_S1_MAIR1);
 	}
 
 	/* SCTLR */
-	reg = SCTLR_CFIE | SCTLR_CFRE | SCTLR_M | SCTLR_EAE_SBOP;
+	reg = SCTLR_CFIE | SCTLR_CFRE | SCTLR_AFE | SCTLR_TRE | SCTLR_M;
 	if (stage1)
 		reg |= SCTLR_S1_ASIDPNE;
 #ifdef __BIG_ENDIAN
@@ -841,12 +819,6 @@ static int arm_smmu_init_domain_context(struct iommu_domain *domain,
 	if (smmu_domain->smmu)
 		goto out_unlock;
 
-	/* We're bypassing these SIDs, so don't allocate an actual context */
-	if (domain->type == IOMMU_DOMAIN_DMA) {
-		smmu_domain->smmu = smmu;
-		goto out_unlock;
-	}
-
 	/*
 	 * Mapping the requested stage onto what we support is surprisingly
 	 * complicated, mainly because the spec allows S1+S2 SMMUs without
@@ -880,6 +852,11 @@ static int arm_smmu_init_domain_context(struct iommu_domain *domain,
 	 */
 	if (smmu->features & ARM_SMMU_FEAT_FMT_AARCH32_L)
 		cfg->fmt = ARM_SMMU_CTX_FMT_AARCH32_L;
+	if (IS_ENABLED(CONFIG_IOMMU_IO_PGTABLE_ARMV7S) &&
+	    !IS_ENABLED(CONFIG_64BIT) && !IS_ENABLED(CONFIG_ARM_LPAE) &&
+	    (smmu->features & ARM_SMMU_FEAT_FMT_AARCH32_S) &&
+	    (smmu_domain->stage == ARM_SMMU_DOMAIN_S1))
+		cfg->fmt = ARM_SMMU_CTX_FMT_AARCH32_S;
 	if ((IS_ENABLED(CONFIG_64BIT) || cfg->fmt == ARM_SMMU_CTX_FMT_NONE) &&
 	    (smmu->features & (ARM_SMMU_FEAT_FMT_AARCH64_64K |
 			       ARM_SMMU_FEAT_FMT_AARCH64_16K |
@@ -899,10 +876,14 @@ static int arm_smmu_init_domain_context(struct iommu_domain *domain,
 		oas = smmu->ipa_size;
 		if (cfg->fmt == ARM_SMMU_CTX_FMT_AARCH64) {
 			fmt = ARM_64_LPAE_S1;
-		} else {
+		} else if (cfg->fmt == ARM_SMMU_CTX_FMT_AARCH32_L) {
 			fmt = ARM_32_LPAE_S1;
 			ias = min(ias, 32UL);
 			oas = min(oas, 40UL);
+		} else {
+			fmt = ARM_V7S;
+			ias = min(ias, 32UL);
+			oas = min(oas, 32UL);
 		}
 		break;
 	case ARM_SMMU_DOMAIN_NESTED:
@@ -958,6 +939,8 @@ static int arm_smmu_init_domain_context(struct iommu_domain *domain,
 
 	/* Update the domain's page sizes to reflect the page table format */
 	domain->pgsize_bitmap = pgtbl_cfg.pgsize_bitmap;
+	domain->geometry.aperture_end = (1UL << ias) - 1;
+	domain->geometry.force_aperture = true;
 
 	/* Initialise the context bank with our page table cfg */
 	arm_smmu_init_context_bank(smmu_domain, &pgtbl_cfg);
@@ -996,7 +979,7 @@ static void arm_smmu_destroy_domain_context(struct iommu_domain *domain)
 	void __iomem *cb_base;
 	int irq;
 
-	if (!smmu || domain->type == IOMMU_DOMAIN_DMA)
+	if (!smmu)
 		return;
 
 	/*
@@ -1030,8 +1013,8 @@ static struct iommu_domain *arm_smmu_domain_alloc(unsigned type)
 	if (!smmu_domain)
 		return NULL;
 
-	if (type == IOMMU_DOMAIN_DMA &&
-	    iommu_get_dma_cookie(&smmu_domain->domain)) {
+	if (type == IOMMU_DOMAIN_DMA && (using_legacy_binding ||
+	    iommu_get_dma_cookie(&smmu_domain->domain))) {
 		kfree(smmu_domain);
 		return NULL;
 	}
@@ -1055,162 +1038,197 @@ static void arm_smmu_domain_free(struct iommu_domain *domain)
 	kfree(smmu_domain);
 }
 
-static int arm_smmu_master_configure_smrs(struct arm_smmu_device *smmu,
-					  struct arm_smmu_master_cfg *cfg)
+static void arm_smmu_write_smr(struct arm_smmu_device *smmu, int idx)
 {
-	int i;
-	struct arm_smmu_smr *smrs;
-	void __iomem *gr0_base = ARM_SMMU_GR0(smmu);
+	struct arm_smmu_smr *smr = smmu->smrs + idx;
+	u32 reg = smr->id << SMR_ID_SHIFT | smr->mask << SMR_MASK_SHIFT;
 
-	if (!(smmu->features & ARM_SMMU_FEAT_STREAM_MATCH))
-		return 0;
+	if (smr->valid)
+		reg |= SMR_VALID;
+	writel_relaxed(reg, ARM_SMMU_GR0(smmu) + ARM_SMMU_GR0_SMR(idx));
+}
 
-	if (cfg->smrs)
-		return -EEXIST;
+static void arm_smmu_write_s2cr(struct arm_smmu_device *smmu, int idx)
+{
+	struct arm_smmu_s2cr *s2cr = smmu->s2crs + idx;
+	u32 reg = (s2cr->type & S2CR_TYPE_MASK) << S2CR_TYPE_SHIFT |
+		  (s2cr->cbndx & S2CR_CBNDX_MASK) << S2CR_CBNDX_SHIFT |
+		  (s2cr->privcfg & S2CR_PRIVCFG_MASK) << S2CR_PRIVCFG_SHIFT;
 
-	smrs = kmalloc_array(cfg->num_streamids, sizeof(*smrs), GFP_KERNEL);
-	if (!smrs) {
-		dev_err(smmu->dev, "failed to allocate %d SMRs\n",
-			cfg->num_streamids);
-		return -ENOMEM;
-	}
+	writel_relaxed(reg, ARM_SMMU_GR0(smmu) + ARM_SMMU_GR0_S2CR(idx));
+}
 
-	/* Allocate the SMRs on the SMMU */
-	for (i = 0; i < cfg->num_streamids; ++i) {
-		int idx = __arm_smmu_alloc_bitmap(smmu->smr_map, 0,
-						  smmu->num_mapping_groups);
-		if (idx < 0) {
-			dev_err(smmu->dev, "failed to allocate free SMR\n");
-			goto err_free_smrs;
-		}
+static void arm_smmu_write_sme(struct arm_smmu_device *smmu, int idx)
+{
+	arm_smmu_write_s2cr(smmu, idx);
+	if (smmu->smrs)
+		arm_smmu_write_smr(smmu, idx);
+}
 
-		smrs[i] = (struct arm_smmu_smr) {
-			.idx	= idx,
-			.mask	= 0, /* We don't currently share SMRs */
-			.id	= cfg->streamids[i],
-		};
-	}
+static int arm_smmu_find_sme(struct arm_smmu_device *smmu, u16 id, u16 mask)
+{
+	struct arm_smmu_smr *smrs = smmu->smrs;
+	int i, free_idx = -ENOSPC;
 
-	/* It worked! Now, poke the actual hardware */
-	for (i = 0; i < cfg->num_streamids; ++i) {
-		u32 reg = SMR_VALID | smrs[i].id << SMR_ID_SHIFT |
-			  smrs[i].mask << SMR_MASK_SHIFT;
-		writel_relaxed(reg, gr0_base + ARM_SMMU_GR0_SMR(smrs[i].idx));
-	}
+	/* Stream indexing is blissfully easy */
+	if (!smrs)
+		return id;
 
-	cfg->smrs = smrs;
-	return 0;
+	/* Validating SMRs is... less so */
+	for (i = 0; i < smmu->num_mapping_groups; ++i) {
+		if (!smrs[i].valid) {
+			/*
+			 * Note the first free entry we come across, which
+			 * we'll claim in the end if nothing else matches.
+			 */
+			if (free_idx < 0)
+				free_idx = i;
+			continue;
+		}
+		/*
+		 * If the new entry is _entirely_ matched by an existing entry,
+		 * then reuse that, with the guarantee that there also cannot
+		 * be any subsequent conflicting entries. In normal use we'd
+		 * expect simply identical entries for this case, but there's
+		 * no harm in accommodating the generalisation.
+		 */
+		if ((mask & smrs[i].mask) == mask &&
+		    !((id ^ smrs[i].id) & ~smrs[i].mask))
+			return i;
+		/*
+		 * If the new entry has any other overlap with an existing one,
+		 * though, then there always exists at least one stream ID
+		 * which would cause a conflict, and we can't allow that risk.
+		 */
+		if (!((id ^ smrs[i].id) & ~(smrs[i].mask | mask)))
+			return -EINVAL;
+	}
 
-err_free_smrs:
-	while (--i >= 0)
-		__arm_smmu_free_bitmap(smmu->smr_map, smrs[i].idx);
-	kfree(smrs);
-	return -ENOSPC;
+	return free_idx;
 }
 
-static void arm_smmu_master_free_smrs(struct arm_smmu_device *smmu,
-				      struct arm_smmu_master_cfg *cfg)
+static bool arm_smmu_free_sme(struct arm_smmu_device *smmu, int idx)
 {
-	int i;
-	void __iomem *gr0_base = ARM_SMMU_GR0(smmu);
-	struct arm_smmu_smr *smrs = cfg->smrs;
-
-	if (!smrs)
-		return;
-
-	/* Invalidate the SMRs before freeing back to the allocator */
-	for (i = 0; i < cfg->num_streamids; ++i) {
-		u8 idx = smrs[i].idx;
+	if (--smmu->s2crs[idx].count)
+		return false;
 
-		writel_relaxed(~SMR_VALID, gr0_base + ARM_SMMU_GR0_SMR(idx));
-		__arm_smmu_free_bitmap(smmu->smr_map, idx);
-	}
+	smmu->s2crs[idx] = s2cr_init_val;
+	if (smmu->smrs)
+		smmu->smrs[idx].valid = false;
 
-	cfg->smrs = NULL;
-	kfree(smrs);
+	return true;
 }
 
-static int arm_smmu_domain_add_master(struct arm_smmu_domain *smmu_domain,
-				      struct arm_smmu_master_cfg *cfg)
+static int arm_smmu_master_alloc_smes(struct device *dev)
 {
-	int i, ret;
-	struct arm_smmu_device *smmu = smmu_domain->smmu;
-	void __iomem *gr0_base = ARM_SMMU_GR0(smmu);
+	struct iommu_fwspec *fwspec = dev->iommu_fwspec;
+	struct arm_smmu_master_cfg *cfg = fwspec->iommu_priv;
+	struct arm_smmu_device *smmu = cfg->smmu;
+	struct arm_smmu_smr *smrs = smmu->smrs;
+	struct iommu_group *group;
+	int i, idx, ret;
 
-	/*
-	 * FIXME: This won't be needed once we have IOMMU-backed DMA ops
-	 * for all devices behind the SMMU. Note that we need to take
-	 * care configuring SMRs for devices both a platform_device and
-	 * and a PCI device (i.e. a PCI host controller)
-	 */
-	if (smmu_domain->domain.type == IOMMU_DOMAIN_DMA)
-		return 0;
+	mutex_lock(&smmu->stream_map_mutex);
+	/* Figure out a viable stream map entry allocation */
+	for_each_cfg_sme(fwspec, i, idx) {
+		u16 sid = fwspec->ids[i];
+		u16 mask = fwspec->ids[i] >> SMR_MASK_SHIFT;
 
-	/* Devices in an IOMMU group may already be configured */
-	ret = arm_smmu_master_configure_smrs(smmu, cfg);
-	if (ret)
-		return ret == -EEXIST ? 0 : ret;
+		if (idx != INVALID_SMENDX) {
+			ret = -EEXIST;
+			goto out_err;
+		}
 
-	for (i = 0; i < cfg->num_streamids; ++i) {
-		u32 idx, s2cr;
+		ret = arm_smmu_find_sme(smmu, sid, mask);
+		if (ret < 0)
+			goto out_err;
+
+		idx = ret;
+		if (smrs && smmu->s2crs[idx].count == 0) {
+			smrs[idx].id = sid;
+			smrs[idx].mask = mask;
+			smrs[idx].valid = true;
+		}
+		smmu->s2crs[idx].count++;
+		cfg->smendx[i] = (s16)idx;
+	}
 
-		idx = cfg->smrs ? cfg->smrs[i].idx : cfg->streamids[i];
-		s2cr = S2CR_TYPE_TRANS | S2CR_PRIVCFG_UNPRIV |
-		       (smmu_domain->cfg.cbndx << S2CR_CBNDX_SHIFT);
-		writel_relaxed(s2cr, gr0_base + ARM_SMMU_GR0_S2CR(idx));
+	group = iommu_group_get_for_dev(dev);
+	if (!group)
+		group = ERR_PTR(-ENOMEM);
+	if (IS_ERR(group)) {
+		ret = PTR_ERR(group);
+		goto out_err;
 	}
+	iommu_group_put(group);
 
+	/* It worked! Now, poke the actual hardware */
+	for_each_cfg_sme(fwspec, i, idx) {
+		arm_smmu_write_sme(smmu, idx);
+		smmu->s2crs[idx].group = group;
+	}
+
+	mutex_unlock(&smmu->stream_map_mutex);
 	return 0;
+
+out_err:
+	while (i--) {
+		arm_smmu_free_sme(smmu, cfg->smendx[i]);
+		cfg->smendx[i] = INVALID_SMENDX;
+	}
+	mutex_unlock(&smmu->stream_map_mutex);
+	return ret;
 }
 
-static void arm_smmu_domain_remove_master(struct arm_smmu_domain *smmu_domain,
-					  struct arm_smmu_master_cfg *cfg)
+static void arm_smmu_master_free_smes(struct iommu_fwspec *fwspec)
 {
-	int i;
-	struct arm_smmu_device *smmu = smmu_domain->smmu;
-	void __iomem *gr0_base = ARM_SMMU_GR0(smmu);
-
-	/* An IOMMU group is torn down by the first device to be removed */
-	if ((smmu->features & ARM_SMMU_FEAT_STREAM_MATCH) && !cfg->smrs)
-		return;
+	struct arm_smmu_device *smmu = fwspec_smmu(fwspec);
+	struct arm_smmu_master_cfg *cfg = fwspec->iommu_priv;
+	int i, idx;
 
-	/*
-	 * We *must* clear the S2CR first, because freeing the SMR means
-	 * that it can be re-allocated immediately.
-	 */
-	for (i = 0; i < cfg->num_streamids; ++i) {
-		u32 idx = cfg->smrs ? cfg->smrs[i].idx : cfg->streamids[i];
-		u32 reg = disable_bypass ? S2CR_TYPE_FAULT : S2CR_TYPE_BYPASS;
-
-		writel_relaxed(reg, gr0_base + ARM_SMMU_GR0_S2CR(idx));
+	mutex_lock(&smmu->stream_map_mutex);
+	for_each_cfg_sme(fwspec, i, idx) {
+		if (arm_smmu_free_sme(smmu, idx))
+			arm_smmu_write_sme(smmu, idx);
+		cfg->smendx[i] = INVALID_SMENDX;
 	}
-
-	arm_smmu_master_free_smrs(smmu, cfg);
+	mutex_unlock(&smmu->stream_map_mutex);
 }
 
-static void arm_smmu_detach_dev(struct device *dev,
-				struct arm_smmu_master_cfg *cfg)
+static int arm_smmu_domain_add_master(struct arm_smmu_domain *smmu_domain,
+				      struct iommu_fwspec *fwspec)
 {
-	struct iommu_domain *domain = dev->archdata.iommu;
-	struct arm_smmu_domain *smmu_domain = to_smmu_domain(domain);
+	struct arm_smmu_device *smmu = smmu_domain->smmu;
+	struct arm_smmu_s2cr *s2cr = smmu->s2crs;
+	enum arm_smmu_s2cr_type type = S2CR_TYPE_TRANS;
+	u8 cbndx = smmu_domain->cfg.cbndx;
+	int i, idx;
+
+	for_each_cfg_sme(fwspec, i, idx) {
+		if (type == s2cr[idx].type && cbndx == s2cr[idx].cbndx)
+			continue;
 
-	dev->archdata.iommu = NULL;
-	arm_smmu_domain_remove_master(smmu_domain, cfg);
+		s2cr[idx].type = type;
+		s2cr[idx].privcfg = S2CR_PRIVCFG_UNPRIV;
+		s2cr[idx].cbndx = cbndx;
+		arm_smmu_write_s2cr(smmu, idx);
+	}
+	return 0;
 }
 
 static int arm_smmu_attach_dev(struct iommu_domain *domain, struct device *dev)
 {
 	int ret;
-	struct arm_smmu_domain *smmu_domain = to_smmu_domain(domain);
+	struct iommu_fwspec *fwspec = dev->iommu_fwspec;
 	struct arm_smmu_device *smmu;
-	struct arm_smmu_master_cfg *cfg;
+	struct arm_smmu_domain *smmu_domain = to_smmu_domain(domain);
 
-	smmu = find_smmu_for_device(dev);
-	if (!smmu) {
+	if (!fwspec || fwspec->ops != &arm_smmu_ops) {
 		dev_err(dev, "cannot attach to SMMU, is it on the same bus?\n");
 		return -ENXIO;
 	}
 
+	smmu = fwspec_smmu(fwspec);
 	/* Ensure that the domain is finalised */
 	ret = arm_smmu_init_domain_context(domain, smmu);
 	if (ret < 0)
@@ -1228,18 +1246,7 @@ static int arm_smmu_attach_dev(struct iommu_domain *domain, struct device *dev)
 	}
 
 	/* Looks ok, so add the device to the domain */
-	cfg = find_smmu_master_cfg(dev);
-	if (!cfg)
-		return -ENODEV;
-
-	/* Detach the dev from its current domain */
-	if (dev->archdata.iommu)
-		arm_smmu_detach_dev(dev, cfg);
-
-	ret = arm_smmu_domain_add_master(smmu_domain, cfg);
-	if (!ret)
-		dev->archdata.iommu = domain;
-	return ret;
+	return arm_smmu_domain_add_master(smmu_domain, fwspec);
 }
 
 static int arm_smmu_map(struct iommu_domain *domain, unsigned long iova,
@@ -1358,110 +1365,113 @@ static bool arm_smmu_capable(enum iommu_cap cap)
 	}
 }
 
-static int __arm_smmu_get_pci_sid(struct pci_dev *pdev, u16 alias, void *data)
+static int arm_smmu_match_node(struct device *dev, void *data)
 {
-	*((u16 *)data) = alias;
-	return 0; /* Continue walking */
+	return dev->of_node == data;
 }
 
-static void __arm_smmu_release_pci_iommudata(void *data)
+static struct arm_smmu_device *arm_smmu_get_by_node(struct device_node *np)
 {
-	kfree(data);
+	struct device *dev = driver_find_device(&arm_smmu_driver.driver, NULL,
+						np, arm_smmu_match_node);
+	put_device(dev);
+	return dev ? dev_get_drvdata(dev) : NULL;
 }
 
-static int arm_smmu_init_pci_device(struct pci_dev *pdev,
-				    struct iommu_group *group)
+static int arm_smmu_add_device(struct device *dev)
 {
+	struct arm_smmu_device *smmu;
 	struct arm_smmu_master_cfg *cfg;
-	u16 sid;
-	int i;
-
-	cfg = iommu_group_get_iommudata(group);
-	if (!cfg) {
-		cfg = kzalloc(sizeof(*cfg), GFP_KERNEL);
-		if (!cfg)
-			return -ENOMEM;
+	struct iommu_fwspec *fwspec = dev->iommu_fwspec;
+	int i, ret;
 
-		iommu_group_set_iommudata(group, cfg,
-					  __arm_smmu_release_pci_iommudata);
+	if (using_legacy_binding) {
+		ret = arm_smmu_register_legacy_master(dev, &smmu);
+		fwspec = dev->iommu_fwspec;
+		if (ret)
+			goto out_free;
+	} else if (fwspec) {
+		smmu = arm_smmu_get_by_node(to_of_node(fwspec->iommu_fwnode));
+	} else {
+		return -ENODEV;
 	}
 
-	if (cfg->num_streamids >= MAX_MASTER_STREAMIDS)
-		return -ENOSPC;
+	ret = -EINVAL;
+	for (i = 0; i < fwspec->num_ids; i++) {
+		u16 sid = fwspec->ids[i];
+		u16 mask = fwspec->ids[i] >> SMR_MASK_SHIFT;
 
-	/*
-	 * Assume Stream ID == Requester ID for now.
-	 * We need a way to describe the ID mappings in FDT.
-	 */
-	pci_for_each_dma_alias(pdev, __arm_smmu_get_pci_sid, &sid);
-	for (i = 0; i < cfg->num_streamids; ++i)
-		if (cfg->streamids[i] == sid)
-			break;
-
-	/* Avoid duplicate SIDs, as this can lead to SMR conflicts */
-	if (i == cfg->num_streamids)
-		cfg->streamids[cfg->num_streamids++] = sid;
-
-	return 0;
-}
-
-static int arm_smmu_init_platform_device(struct device *dev,
-					 struct iommu_group *group)
-{
-	struct arm_smmu_device *smmu = find_smmu_for_device(dev);
-	struct arm_smmu_master *master;
+		if (sid & ~smmu->streamid_mask) {
+			dev_err(dev, "stream ID 0x%x out of range for SMMU (0x%x)\n",
+				sid, smmu->streamid_mask);
+			goto out_free;
+		}
+		if (mask & ~smmu->smr_mask_mask) {
+			dev_err(dev, "SMR mask 0x%x out of range for SMMU (0x%x)\n",
+				sid, smmu->smr_mask_mask);
+			goto out_free;
+		}
+	}
 
-	if (!smmu)
-		return -ENODEV;
+	ret = -ENOMEM;
+	cfg = kzalloc(offsetof(struct arm_smmu_master_cfg, smendx[i]),
+		      GFP_KERNEL);
+	if (!cfg)
+		goto out_free;
 
-	master = find_smmu_master(smmu, dev->of_node);
-	if (!master)
-		return -ENODEV;
+	cfg->smmu = smmu;
+	fwspec->iommu_priv = cfg;
+	while (i--)
+		cfg->smendx[i] = INVALID_SMENDX;
 
-	iommu_group_set_iommudata(group, &master->cfg, NULL);
+	ret = arm_smmu_master_alloc_smes(dev);
+	if (ret)
+		goto out_free;
 
 	return 0;
-}
 
-static int arm_smmu_add_device(struct device *dev)
-{
-	struct iommu_group *group;
-
-	group = iommu_group_get_for_dev(dev);
-	if (IS_ERR(group))
-		return PTR_ERR(group);
-
-	iommu_group_put(group);
-	return 0;
+out_free:
+	if (fwspec)
+		kfree(fwspec->iommu_priv);
+	iommu_fwspec_free(dev);
+	return ret;
 }
 
 static void arm_smmu_remove_device(struct device *dev)
 {
+	struct iommu_fwspec *fwspec = dev->iommu_fwspec;
+
+	if (!fwspec || fwspec->ops != &arm_smmu_ops)
+		return;
+
+	arm_smmu_master_free_smes(fwspec);
 	iommu_group_remove_device(dev);
+	kfree(fwspec->iommu_priv);
+	iommu_fwspec_free(dev);
 }
 
 static struct iommu_group *arm_smmu_device_group(struct device *dev)
 {
-	struct iommu_group *group;
-	int ret;
+	struct iommu_fwspec *fwspec = dev->iommu_fwspec;
+	struct arm_smmu_device *smmu = fwspec_smmu(fwspec);
+	struct iommu_group *group = NULL;
+	int i, idx;
 
-	if (dev_is_pci(dev))
-		group = pci_device_group(dev);
-	else
-		group = generic_device_group(dev);
+	for_each_cfg_sme(fwspec, i, idx) {
+		if (group && smmu->s2crs[idx].group &&
+		    group != smmu->s2crs[idx].group)
+			return ERR_PTR(-EINVAL);
+
+		group = smmu->s2crs[idx].group;
+	}
 
-	if (IS_ERR(group))
+	if (group)
 		return group;
 
 	if (dev_is_pci(dev))
-		ret = arm_smmu_init_pci_device(to_pci_dev(dev), group);
+		group = pci_device_group(dev);
 	else
-		ret = arm_smmu_init_platform_device(dev, group);
-
-	if (ret) {
-		iommu_group_put(group);
-		group = ERR_PTR(ret);
-	}
+		group = generic_device_group(dev);
 
 	return group;
 }
@@ -1510,6 +1520,19 @@ out_unlock:
 	return ret;
 }
 
+static int arm_smmu_of_xlate(struct device *dev, struct of_phandle_args *args)
+{
+	u32 fwid = 0;
+
+	if (args->args_count > 0)
+		fwid |= (u16)args->args[0];
+
+	if (args->args_count > 1)
+		fwid |= (u16)args->args[1] << SMR_MASK_SHIFT;
+
+	return iommu_fwspec_add_ids(dev, &fwid, 1);
+}
+
 static struct iommu_ops arm_smmu_ops = {
 	.capable		= arm_smmu_capable,
 	.domain_alloc		= arm_smmu_domain_alloc,
@@ -1524,6 +1547,7 @@ static struct iommu_ops arm_smmu_ops = {
 	.device_group		= arm_smmu_device_group,
 	.domain_get_attr	= arm_smmu_domain_get_attr,
 	.domain_set_attr	= arm_smmu_domain_set_attr,
+	.of_xlate		= arm_smmu_of_xlate,
 	.pgsize_bitmap		= -1UL, /* Restricted during device attach */
 };
 
@@ -1531,19 +1555,19 @@ static void arm_smmu_device_reset(struct arm_smmu_device *smmu)
 {
 	void __iomem *gr0_base = ARM_SMMU_GR0(smmu);
 	void __iomem *cb_base;
-	int i = 0;
+	int i;
 	u32 reg, major;
 
 	/* clear global FSR */
 	reg = readl_relaxed(ARM_SMMU_GR0_NS(smmu) + ARM_SMMU_GR0_sGFSR);
 	writel(reg, ARM_SMMU_GR0_NS(smmu) + ARM_SMMU_GR0_sGFSR);
 
-	/* Mark all SMRn as invalid and all S2CRn as bypass unless overridden */
-	reg = disable_bypass ? S2CR_TYPE_FAULT : S2CR_TYPE_BYPASS;
-	for (i = 0; i < smmu->num_mapping_groups; ++i) {
-		writel_relaxed(0, gr0_base + ARM_SMMU_GR0_SMR(i));
-		writel_relaxed(reg, gr0_base + ARM_SMMU_GR0_S2CR(i));
-	}
+	/*
+	 * Reset stream mapping groups: Initial values mark all SMRn as
+	 * invalid and all S2CRn as bypass unless overridden.
+	 */
+	for (i = 0; i < smmu->num_mapping_groups; ++i)
+		arm_smmu_write_sme(smmu, i);
 
 	/*
 	 * Before clearing ARM_MMU500_ACTLR_CPRE, need to
@@ -1632,6 +1656,7 @@ static int arm_smmu_device_cfg_probe(struct arm_smmu_device *smmu)
 	void __iomem *gr0_base = ARM_SMMU_GR0(smmu);
 	u32 id;
 	bool cttw_dt, cttw_reg;
+	int i;
 
 	dev_notice(smmu->dev, "probing hardware configuration...\n");
 	dev_notice(smmu->dev, "SMMUv%d with:\n",
@@ -1690,39 +1715,55 @@ static int arm_smmu_device_cfg_probe(struct arm_smmu_device *smmu)
 		dev_notice(smmu->dev,
 			   "\t(IDR0.CTTW overridden by dma-coherent property)\n");
 
+	/* Max. number of entries we have for stream matching/indexing */
+	size = 1 << ((id >> ID0_NUMSIDB_SHIFT) & ID0_NUMSIDB_MASK);
+	smmu->streamid_mask = size - 1;
 	if (id & ID0_SMS) {
-		u32 smr, sid, mask;
+		u32 smr;
 
 		smmu->features |= ARM_SMMU_FEAT_STREAM_MATCH;
-		smmu->num_mapping_groups = (id >> ID0_NUMSMRG_SHIFT) &
-					   ID0_NUMSMRG_MASK;
-		if (smmu->num_mapping_groups == 0) {
+		size = (id >> ID0_NUMSMRG_SHIFT) & ID0_NUMSMRG_MASK;
+		if (size == 0) {
 			dev_err(smmu->dev,
 				"stream-matching supported, but no SMRs present!\n");
 			return -ENODEV;
 		}
 
-		smr = SMR_MASK_MASK << SMR_MASK_SHIFT;
-		smr |= (SMR_ID_MASK << SMR_ID_SHIFT);
+		/*
+		 * SMR.ID bits may not be preserved if the corresponding MASK
+		 * bits are set, so check each one separately. We can reject
+		 * masters later if they try to claim IDs outside these masks.
+		 */
+		smr = smmu->streamid_mask << SMR_ID_SHIFT;
 		writel_relaxed(smr, gr0_base + ARM_SMMU_GR0_SMR(0));
 		smr = readl_relaxed(gr0_base + ARM_SMMU_GR0_SMR(0));
+		smmu->streamid_mask = smr >> SMR_ID_SHIFT;
 
-		mask = (smr >> SMR_MASK_SHIFT) & SMR_MASK_MASK;
-		sid = (smr >> SMR_ID_SHIFT) & SMR_ID_MASK;
-		if ((mask & sid) != sid) {
-			dev_err(smmu->dev,
-				"SMR mask bits (0x%x) insufficient for ID field (0x%x)\n",
-				mask, sid);
-			return -ENODEV;
-		}
+		smr = smmu->streamid_mask << SMR_MASK_SHIFT;
+		writel_relaxed(smr, gr0_base + ARM_SMMU_GR0_SMR(0));
+		smr = readl_relaxed(gr0_base + ARM_SMMU_GR0_SMR(0));
+		smmu->smr_mask_mask = smr >> SMR_MASK_SHIFT;
+
+		/* Zero-initialised to mark as invalid */
+		smmu->smrs = devm_kcalloc(smmu->dev, size, sizeof(*smmu->smrs),
+					  GFP_KERNEL);
+		if (!smmu->smrs)
+			return -ENOMEM;
 
 		dev_notice(smmu->dev,
-			   "\tstream matching with %u register groups, mask 0x%x",
-			   smmu->num_mapping_groups, mask);
-	} else {
-		smmu->num_mapping_groups = (id >> ID0_NUMSIDB_SHIFT) &
-					   ID0_NUMSIDB_MASK;
+			   "\tstream matching with %lu register groups, mask 0x%x",
+			   size, smmu->smr_mask_mask);
 	}
+	/* s2cr->type == 0 means translation, so initialise explicitly */
+	smmu->s2crs = devm_kmalloc_array(smmu->dev, size, sizeof(*smmu->s2crs),
+					 GFP_KERNEL);
+	if (!smmu->s2crs)
+		return -ENOMEM;
+	for (i = 0; i < size; i++)
+		smmu->s2crs[i] = s2cr_init_val;
+
+	smmu->num_mapping_groups = size;
+	mutex_init(&smmu->stream_map_mutex);
 
 	if (smmu->version < ARM_SMMU_V2 || !(id & ID0_PTFS_NO_AARCH32)) {
 		smmu->features |= ARM_SMMU_FEAT_FMT_AARCH32_L;
@@ -1855,15 +1896,24 @@ MODULE_DEVICE_TABLE(of, arm_smmu_of_match);
 
 static int arm_smmu_device_dt_probe(struct platform_device *pdev)
 {
-	const struct of_device_id *of_id;
 	const struct arm_smmu_match_data *data;
 	struct resource *res;
 	struct arm_smmu_device *smmu;
 	struct device *dev = &pdev->dev;
-	struct rb_node *node;
-	struct of_phandle_iterator it;
-	struct arm_smmu_phandle_args *masterspec;
 	int num_irqs, i, err;
+	bool legacy_binding;
+
+	legacy_binding = of_find_property(dev->of_node, "mmu-masters", NULL);
+	if (legacy_binding && !using_generic_binding) {
+		if (!using_legacy_binding)
+			pr_notice("deprecated \"mmu-masters\" DT property in use; DMA API support unavailable\n");
+		using_legacy_binding = true;
+	} else if (!legacy_binding && !using_legacy_binding) {
+		using_generic_binding = true;
+	} else {
+		dev_err(dev, "not probing due to mismatched DT properties\n");
+		return -ENODEV;
+	}
 
 	smmu = devm_kzalloc(dev, sizeof(*smmu), GFP_KERNEL);
 	if (!smmu) {
@@ -1872,8 +1922,7 @@ static int arm_smmu_device_dt_probe(struct platform_device *pdev)
 	}
 	smmu->dev = dev;
 
-	of_id = of_match_node(arm_smmu_of_match, dev->of_node);
-	data = of_id->data;
+	data = of_device_get_match_data(dev);
 	smmu->version = data->version;
 	smmu->model = data->model;
 
@@ -1923,37 +1972,6 @@ static int arm_smmu_device_dt_probe(struct platform_device *pdev)
 	if (err)
 		return err;
 
-	i = 0;
-	smmu->masters = RB_ROOT;
-
-	err = -ENOMEM;
-	/* No need to zero the memory for masterspec */
-	masterspec = kmalloc(sizeof(*masterspec), GFP_KERNEL);
-	if (!masterspec)
-		goto out_put_masters;
-
-	of_for_each_phandle(&it, err, dev->of_node,
-			    "mmu-masters", "#stream-id-cells", 0) {
-		int count = of_phandle_iterator_args(&it, masterspec->args,
-						     MAX_MASTER_STREAMIDS);
-		masterspec->np		= of_node_get(it.node);
-		masterspec->args_count	= count;
-
-		err = register_smmu_master(smmu, dev, masterspec);
-		if (err) {
-			dev_err(dev, "failed to add master %s\n",
-				masterspec->np->name);
-			kfree(masterspec);
-			goto out_put_masters;
-		}
-
-		i++;
-	}
-
-	dev_notice(dev, "registered %d master devices\n", i);
-
-	kfree(masterspec);
-
 	parse_driver_options(smmu);
 
 	if (smmu->version == ARM_SMMU_V2 &&
@@ -1961,8 +1979,7 @@ static int arm_smmu_device_dt_probe(struct platform_device *pdev)
 		dev_err(dev,
 			"found only %d context interrupt(s) but %d required\n",
 			smmu->num_context_irqs, smmu->num_context_banks);
-		err = -ENODEV;
-		goto out_put_masters;
+		return -ENODEV;
 	}
 
 	for (i = 0; i < smmu->num_global_irqs; ++i) {
@@ -1974,59 +1991,39 @@ static int arm_smmu_device_dt_probe(struct platform_device *pdev)
 		if (err) {
 			dev_err(dev, "failed to request global IRQ %d (%u)\n",
 				i, smmu->irqs[i]);
-			goto out_put_masters;
+			return err;
 		}
 	}
 
-	INIT_LIST_HEAD(&smmu->list);
-	spin_lock(&arm_smmu_devices_lock);
-	list_add(&smmu->list, &arm_smmu_devices);
-	spin_unlock(&arm_smmu_devices_lock);
-
+	of_iommu_set_ops(dev->of_node, &arm_smmu_ops);
+	platform_set_drvdata(pdev, smmu);
 	arm_smmu_device_reset(smmu);
-	return 0;
 
-out_put_masters:
-	for (node = rb_first(&smmu->masters); node; node = rb_next(node)) {
-		struct arm_smmu_master *master
-			= container_of(node, struct arm_smmu_master, node);
-		of_node_put(master->of_node);
+	/* Oh, for a proper bus abstraction */
+	if (!iommu_present(&platform_bus_type))
+		bus_set_iommu(&platform_bus_type, &arm_smmu_ops);
+#ifdef CONFIG_ARM_AMBA
+	if (!iommu_present(&amba_bustype))
+		bus_set_iommu(&amba_bustype, &arm_smmu_ops);
+#endif
+#ifdef CONFIG_PCI
+	if (!iommu_present(&pci_bus_type)) {
+		pci_request_acs();
+		bus_set_iommu(&pci_bus_type, &arm_smmu_ops);
 	}
-
-	return err;
+#endif
+	return 0;
 }
 
 static int arm_smmu_device_remove(struct platform_device *pdev)
 {
-	int i;
-	struct device *dev = &pdev->dev;
-	struct arm_smmu_device *curr, *smmu = NULL;
-	struct rb_node *node;
-
-	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);
+	struct arm_smmu_device *smmu = platform_get_drvdata(pdev);
 
 	if (!smmu)
 		return -ENODEV;
 
-	for (node = rb_first(&smmu->masters); node; node = rb_next(node)) {
-		struct arm_smmu_master *master
-			= container_of(node, struct arm_smmu_master, node);
-		of_node_put(master->of_node);
-	}
-
 	if (!bitmap_empty(smmu->context_map, ARM_SMMU_MAX_CBS))
-		dev_err(dev, "removing device with active domains!\n");
-
-	for (i = 0; i < smmu->num_global_irqs; ++i)
-		devm_free_irq(smmu->dev, smmu->irqs[i], smmu);
+		dev_err(&pdev->dev, "removing device with active domains!\n");
 
 	/* Turn the thing off */
 	writel(sCR0_CLIENTPD, ARM_SMMU_GR0_NS(smmu) + ARM_SMMU_GR0_sCR0);
@@ -2044,41 +2041,14 @@ static struct platform_driver arm_smmu_driver = {
 
 static int __init arm_smmu_init(void)
 {
-	struct device_node *np;
-	int ret;
-
-	/*
-	 * Play nice with systems that don't have an ARM SMMU by checking that
-	 * an ARM SMMU exists in the system before proceeding with the driver
-	 * and IOMMU bus operation registration.
-	 */
-	np = of_find_matching_node(NULL, arm_smmu_of_match);
-	if (!np)
-		return 0;
-
-	of_node_put(np);
-
-	ret = platform_driver_register(&arm_smmu_driver);
-	if (ret)
-		return ret;
-
-	/* Oh, for a proper bus abstraction */
-	if (!iommu_present(&platform_bus_type))
-		bus_set_iommu(&platform_bus_type, &arm_smmu_ops);
-
-#ifdef CONFIG_ARM_AMBA
-	if (!iommu_present(&amba_bustype))
-		bus_set_iommu(&amba_bustype, &arm_smmu_ops);
-#endif
+	static bool registered;
+	int ret = 0;
 
-#ifdef CONFIG_PCI
-	if (!iommu_present(&pci_bus_type)) {
-		pci_request_acs();
-		bus_set_iommu(&pci_bus_type, &arm_smmu_ops);
+	if (!registered) {
+		ret = platform_driver_register(&arm_smmu_driver);
+		registered = !ret;
 	}
-#endif
-
-	return 0;
+	return ret;
 }
 
 static void __exit arm_smmu_exit(void)
@@ -2089,6 +2059,25 @@ static void __exit arm_smmu_exit(void)
 subsys_initcall(arm_smmu_init);
 module_exit(arm_smmu_exit);
 
+static int __init arm_smmu_of_init(struct device_node *np)
+{
+	int ret = arm_smmu_init();
+
+	if (ret)
+		return ret;
+
+	if (!of_platform_device_create(np, NULL, platform_bus_type.dev_root))
+		return -ENODEV;
+
+	return 0;
+}
+IOMMU_OF_DECLARE(arm_smmuv1, "arm,smmu-v1", arm_smmu_of_init);
+IOMMU_OF_DECLARE(arm_smmuv2, "arm,smmu-v2", arm_smmu_of_init);
+IOMMU_OF_DECLARE(arm_mmu400, "arm,mmu-400", arm_smmu_of_init);
+IOMMU_OF_DECLARE(arm_mmu401, "arm,mmu-401", arm_smmu_of_init);
+IOMMU_OF_DECLARE(arm_mmu500, "arm,mmu-500", arm_smmu_of_init);
+IOMMU_OF_DECLARE(cavium_smmuv2, "cavium,smmu-v2", arm_smmu_of_init);
+
 MODULE_DESCRIPTION("IOMMU API for ARM architected SMMU implementations");
 MODULE_AUTHOR("Will Deacon <will.deacon@arm.com>");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/iommu/dma-iommu.c b/drivers/iommu/dma-iommu.c
index 00c8a08d56e7..c5ab8667e6f2 100644
--- a/drivers/iommu/dma-iommu.c
+++ b/drivers/iommu/dma-iommu.c
@@ -25,10 +25,29 @@
 #include <linux/huge_mm.h>
 #include <linux/iommu.h>
 #include <linux/iova.h>
+#include <linux/irq.h>
 #include <linux/mm.h>
+#include <linux/pci.h>
 #include <linux/scatterlist.h>
 #include <linux/vmalloc.h>
 
+struct iommu_dma_msi_page {
+	struct list_head	list;
+	dma_addr_t		iova;
+	phys_addr_t		phys;
+};
+
+struct iommu_dma_cookie {
+	struct iova_domain	iovad;
+	struct list_head	msi_page_list;
+	spinlock_t		msi_lock;
+};
+
+static inline struct iova_domain *cookie_iovad(struct iommu_domain *domain)
+{
+	return &((struct iommu_dma_cookie *)domain->iova_cookie)->iovad;
+}
+
 int iommu_dma_init(void)
 {
 	return iova_cache_get();
@@ -43,15 +62,19 @@ int iommu_dma_init(void)
  */
 int iommu_get_dma_cookie(struct iommu_domain *domain)
 {
-	struct iova_domain *iovad;
+	struct iommu_dma_cookie *cookie;
 
 	if (domain->iova_cookie)
 		return -EEXIST;
 
-	iovad = kzalloc(sizeof(*iovad), GFP_KERNEL);
-	domain->iova_cookie = iovad;
+	cookie = kzalloc(sizeof(*cookie), GFP_KERNEL);
+	if (!cookie)
+		return -ENOMEM;
 
-	return iovad ? 0 : -ENOMEM;
+	spin_lock_init(&cookie->msi_lock);
+	INIT_LIST_HEAD(&cookie->msi_page_list);
+	domain->iova_cookie = cookie;
+	return 0;
 }
 EXPORT_SYMBOL(iommu_get_dma_cookie);
 
@@ -63,32 +86,58 @@ EXPORT_SYMBOL(iommu_get_dma_cookie);
  */
 void iommu_put_dma_cookie(struct iommu_domain *domain)
 {
-	struct iova_domain *iovad = domain->iova_cookie;
+	struct iommu_dma_cookie *cookie = domain->iova_cookie;
+	struct iommu_dma_msi_page *msi, *tmp;
 
-	if (!iovad)
+	if (!cookie)
 		return;
 
-	if (iovad->granule)
-		put_iova_domain(iovad);
-	kfree(iovad);
+	if (cookie->iovad.granule)
+		put_iova_domain(&cookie->iovad);
+
+	list_for_each_entry_safe(msi, tmp, &cookie->msi_page_list, list) {
+		list_del(&msi->list);
+		kfree(msi);
+	}
+	kfree(cookie);
 	domain->iova_cookie = NULL;
 }
 EXPORT_SYMBOL(iommu_put_dma_cookie);
 
+static void iova_reserve_pci_windows(struct pci_dev *dev,
+		struct iova_domain *iovad)
+{
+	struct pci_host_bridge *bridge = pci_find_host_bridge(dev->bus);
+	struct resource_entry *window;
+	unsigned long lo, hi;
+
+	resource_list_for_each_entry(window, &bridge->windows) {
+		if (resource_type(window->res) != IORESOURCE_MEM &&
+		    resource_type(window->res) != IORESOURCE_IO)
+			continue;
+
+		lo = iova_pfn(iovad, window->res->start - window->offset);
+		hi = iova_pfn(iovad, window->res->end - window->offset);
+		reserve_iova(iovad, lo, hi);
+	}
+}
+
 /**
  * iommu_dma_init_domain - Initialise a DMA mapping domain
  * @domain: IOMMU domain previously prepared by iommu_get_dma_cookie()
  * @base: IOVA at which the mappable address space starts
  * @size: Size of IOVA space
+ * @dev: Device the domain is being initialised for
  *
  * @base and @size should be exact multiples of IOMMU page granularity to
  * avoid rounding surprises. If necessary, we reserve the page at address 0
  * to ensure it is an invalid IOVA. It is safe to reinitialise a domain, but
  * any change which could make prior IOVAs invalid will fail.
  */
-int iommu_dma_init_domain(struct iommu_domain *domain, dma_addr_t base, u64 size)
+int iommu_dma_init_domain(struct iommu_domain *domain, dma_addr_t base,
+		u64 size, struct device *dev)
 {
-	struct iova_domain *iovad = domain->iova_cookie;
+	struct iova_domain *iovad = cookie_iovad(domain);
 	unsigned long order, base_pfn, end_pfn;
 
 	if (!iovad)
@@ -124,6 +173,8 @@ int iommu_dma_init_domain(struct iommu_domain *domain, dma_addr_t base, u64 size
 		iovad->dma_32bit_pfn = end_pfn;
 	} else {
 		init_iova_domain(iovad, 1UL << order, base_pfn, end_pfn);
+		if (dev && dev_is_pci(dev))
+			iova_reserve_pci_windows(to_pci_dev(dev), iovad);
 	}
 	return 0;
 }
@@ -155,7 +206,7 @@ int dma_direction_to_prot(enum dma_data_direction dir, bool coherent)
 static struct iova *__alloc_iova(struct iommu_domain *domain, size_t size,
 		dma_addr_t dma_limit)
 {
-	struct iova_domain *iovad = domain->iova_cookie;
+	struct iova_domain *iovad = cookie_iovad(domain);
 	unsigned long shift = iova_shift(iovad);
 	unsigned long length = iova_align(iovad, size) >> shift;
 
@@ -171,7 +222,7 @@ static struct iova *__alloc_iova(struct iommu_domain *domain, size_t size,
 /* The IOVA allocator knows what we mapped, so just unmap whatever that was */
 static void __iommu_dma_unmap(struct iommu_domain *domain, dma_addr_t dma_addr)
 {
-	struct iova_domain *iovad = domain->iova_cookie;
+	struct iova_domain *iovad = cookie_iovad(domain);
 	unsigned long shift = iova_shift(iovad);
 	unsigned long pfn = dma_addr >> shift;
 	struct iova *iova = find_iova(iovad, pfn);
@@ -294,7 +345,7 @@ struct page **iommu_dma_alloc(struct device *dev, size_t size, gfp_t gfp,
 		void (*flush_page)(struct device *, const void *, phys_addr_t))
 {
 	struct iommu_domain *domain = iommu_get_domain_for_dev(dev);
-	struct iova_domain *iovad = domain->iova_cookie;
+	struct iova_domain *iovad = cookie_iovad(domain);
 	struct iova *iova;
 	struct page **pages;
 	struct sg_table sgt;
@@ -386,7 +437,7 @@ dma_addr_t iommu_dma_map_page(struct device *dev, struct page *page,
 {
 	dma_addr_t dma_addr;
 	struct iommu_domain *domain = iommu_get_domain_for_dev(dev);
-	struct iova_domain *iovad = domain->iova_cookie;
+	struct iova_domain *iovad = cookie_iovad(domain);
 	phys_addr_t phys = page_to_phys(page) + offset;
 	size_t iova_off = iova_offset(iovad, phys);
 	size_t len = iova_align(iovad, size + iova_off);
@@ -495,7 +546,7 @@ int iommu_dma_map_sg(struct device *dev, struct scatterlist *sg,
 		int nents, int prot)
 {
 	struct iommu_domain *domain = iommu_get_domain_for_dev(dev);
-	struct iova_domain *iovad = domain->iova_cookie;
+	struct iova_domain *iovad = cookie_iovad(domain);
 	struct iova *iova;
 	struct scatterlist *s, *prev = NULL;
 	dma_addr_t dma_addr;
@@ -587,3 +638,81 @@ int iommu_dma_mapping_error(struct device *dev, dma_addr_t dma_addr)
 {
 	return dma_addr == DMA_ERROR_CODE;
 }
+
+static struct iommu_dma_msi_page *iommu_dma_get_msi_page(struct device *dev,
+		phys_addr_t msi_addr, struct iommu_domain *domain)
+{
+	struct iommu_dma_cookie *cookie = domain->iova_cookie;
+	struct iommu_dma_msi_page *msi_page;
+	struct iova_domain *iovad = &cookie->iovad;
+	struct iova *iova;
+	int prot = IOMMU_WRITE | IOMMU_NOEXEC | IOMMU_MMIO;
+
+	msi_addr &= ~(phys_addr_t)iova_mask(iovad);
+	list_for_each_entry(msi_page, &cookie->msi_page_list, list)
+		if (msi_page->phys == msi_addr)
+			return msi_page;
+
+	msi_page = kzalloc(sizeof(*msi_page), GFP_ATOMIC);
+	if (!msi_page)
+		return NULL;
+
+	iova = __alloc_iova(domain, iovad->granule, dma_get_mask(dev));
+	if (!iova)
+		goto out_free_page;
+
+	msi_page->phys = msi_addr;
+	msi_page->iova = iova_dma_addr(iovad, iova);
+	if (iommu_map(domain, msi_page->iova, msi_addr, iovad->granule, prot))
+		goto out_free_iova;
+
+	INIT_LIST_HEAD(&msi_page->list);
+	list_add(&msi_page->list, &cookie->msi_page_list);
+	return msi_page;
+
+out_free_iova:
+	__free_iova(iovad, iova);
+out_free_page:
+	kfree(msi_page);
+	return NULL;
+}
+
+void iommu_dma_map_msi_msg(int irq, struct msi_msg *msg)
+{
+	struct device *dev = msi_desc_to_dev(irq_get_msi_desc(irq));
+	struct iommu_domain *domain = iommu_get_domain_for_dev(dev);
+	struct iommu_dma_cookie *cookie;
+	struct iommu_dma_msi_page *msi_page;
+	phys_addr_t msi_addr = (u64)msg->address_hi << 32 | msg->address_lo;
+	unsigned long flags;
+
+	if (!domain || !domain->iova_cookie)
+		return;
+
+	cookie = domain->iova_cookie;
+
+	/*
+	 * We disable IRQs to rule out a possible inversion against
+	 * irq_desc_lock if, say, someone tries to retarget the affinity
+	 * of an MSI from within an IPI handler.
+	 */
+	spin_lock_irqsave(&cookie->msi_lock, flags);
+	msi_page = iommu_dma_get_msi_page(dev, msi_addr, domain);
+	spin_unlock_irqrestore(&cookie->msi_lock, flags);
+
+	if (WARN_ON(!msi_page)) {
+		/*
+		 * We're called from a void callback, so the best we can do is
+		 * 'fail' by filling the message with obviously bogus values.
+		 * Since we got this far due to an IOMMU being present, it's
+		 * not like the existing address would have worked anyway...
+		 */
+		msg->address_hi = ~0U;
+		msg->address_lo = ~0U;
+		msg->data = ~0U;
+	} else {
+		msg->address_hi = upper_32_bits(msi_page->iova);
+		msg->address_lo &= iova_mask(&cookie->iovad);
+		msg->address_lo += lower_32_bits(msi_page->iova);
+	}
+}
diff --git a/drivers/iommu/exynos-iommu.c b/drivers/iommu/exynos-iommu.c
index 33dcc29ec200..30808e91b775 100644
--- a/drivers/iommu/exynos-iommu.c
+++ b/drivers/iommu/exynos-iommu.c
@@ -1345,8 +1345,8 @@ static int __init exynos_iommu_of_setup(struct device_node *np)
 		exynos_iommu_init();
 
 	pdev = of_platform_device_create(np, NULL, platform_bus_type.dev_root);
-	if (IS_ERR(pdev))
-		return PTR_ERR(pdev);
+	if (!pdev)
+		return -ENODEV;
 
 	/*
 	 * use the first registered sysmmu device for performing
diff --git a/drivers/iommu/intel-iommu.c b/drivers/iommu/intel-iommu.c
index ebb5bf3ddbd9..a4407eabf0e6 100644
--- a/drivers/iommu/intel-iommu.c
+++ b/drivers/iommu/intel-iommu.c
@@ -2452,20 +2452,15 @@ static int get_last_alias(struct pci_dev *pdev, u16 alias, void *opaque)
 	return 0;
 }
 
-/* domain is initialized */
-static struct dmar_domain *get_domain_for_dev(struct device *dev, int gaw)
+static struct dmar_domain *find_or_alloc_domain(struct device *dev, int gaw)
 {
 	struct device_domain_info *info = NULL;
-	struct dmar_domain *domain, *tmp;
+	struct dmar_domain *domain = NULL;
 	struct intel_iommu *iommu;
 	u16 req_id, dma_alias;
 	unsigned long flags;
 	u8 bus, devfn;
 
-	domain = find_domain(dev);
-	if (domain)
-		return domain;
-
 	iommu = device_to_iommu(dev, &bus, &devfn);
 	if (!iommu)
 		return NULL;
@@ -2487,9 +2482,9 @@ static struct dmar_domain *get_domain_for_dev(struct device *dev, int gaw)
 		}
 		spin_unlock_irqrestore(&device_domain_lock, flags);
 
-		/* DMA alias already has a domain, uses it */
+		/* DMA alias already has a domain, use it */
 		if (info)
-			goto found_domain;
+			goto out;
 	}
 
 	/* Allocate and initialize new domain for the device */
@@ -2501,28 +2496,67 @@ static struct dmar_domain *get_domain_for_dev(struct device *dev, int gaw)
 		return NULL;
 	}
 
-	/* register PCI DMA alias device */
-	if (dev_is_pci(dev) && req_id != dma_alias) {
-		tmp = dmar_insert_one_dev_info(iommu, PCI_BUS_NUM(dma_alias),
-					       dma_alias & 0xff, NULL, domain);
+out:
 
-		if (!tmp || tmp != domain) {
-			domain_exit(domain);
-			domain = tmp;
-		}
+	return domain;
+}
 
-		if (!domain)
-			return NULL;
+static struct dmar_domain *set_domain_for_dev(struct device *dev,
+					      struct dmar_domain *domain)
+{
+	struct intel_iommu *iommu;
+	struct dmar_domain *tmp;
+	u16 req_id, dma_alias;
+	u8 bus, devfn;
+
+	iommu = device_to_iommu(dev, &bus, &devfn);
+	if (!iommu)
+		return NULL;
+
+	req_id = ((u16)bus << 8) | devfn;
+
+	if (dev_is_pci(dev)) {
+		struct pci_dev *pdev = to_pci_dev(dev);
+
+		pci_for_each_dma_alias(pdev, get_last_alias, &dma_alias);
+
+		/* register PCI DMA alias device */
+		if (req_id != dma_alias) {
+			tmp = dmar_insert_one_dev_info(iommu, PCI_BUS_NUM(dma_alias),
+					dma_alias & 0xff, NULL, domain);
+
+			if (!tmp || tmp != domain)
+				return tmp;
+		}
 	}
 
-found_domain:
 	tmp = dmar_insert_one_dev_info(iommu, bus, devfn, dev, domain);
+	if (!tmp || tmp != domain)
+		return tmp;
+
+	return domain;
+}
 
-	if (!tmp || tmp != domain) {
+static struct dmar_domain *get_domain_for_dev(struct device *dev, int gaw)
+{
+	struct dmar_domain *domain, *tmp;
+
+	domain = find_domain(dev);
+	if (domain)
+		goto out;
+
+	domain = find_or_alloc_domain(dev, gaw);
+	if (!domain)
+		goto out;
+
+	tmp = set_domain_for_dev(dev, domain);
+	if (!tmp || domain != tmp) {
 		domain_exit(domain);
 		domain = tmp;
 	}
 
+out:
+
 	return domain;
 }
 
@@ -3394,17 +3428,18 @@ static unsigned long intel_alloc_iova(struct device *dev,
 
 static struct dmar_domain *__get_valid_domain_for_dev(struct device *dev)
 {
+	struct dmar_domain *domain, *tmp;
 	struct dmar_rmrr_unit *rmrr;
-	struct dmar_domain *domain;
 	struct device *i_dev;
 	int i, ret;
 
-	domain = get_domain_for_dev(dev, DEFAULT_DOMAIN_ADDRESS_WIDTH);
-	if (!domain) {
-		pr_err("Allocating domain for %s failed\n",
-		       dev_name(dev));
-		return NULL;
-	}
+	domain = find_domain(dev);
+	if (domain)
+		goto out;
+
+	domain = find_or_alloc_domain(dev, DEFAULT_DOMAIN_ADDRESS_WIDTH);
+	if (!domain)
+		goto out;
 
 	/* We have a new domain - setup possible RMRRs for the device */
 	rcu_read_lock();
@@ -3423,6 +3458,18 @@ static struct dmar_domain *__get_valid_domain_for_dev(struct device *dev)
 	}
 	rcu_read_unlock();
 
+	tmp = set_domain_for_dev(dev, domain);
+	if (!tmp || domain != tmp) {
+		domain_exit(domain);
+		domain = tmp;
+	}
+
+out:
+
+	if (!domain)
+		pr_err("Allocating domain for %s failed\n", dev_name(dev));
+
+
 	return domain;
 }
 
diff --git a/drivers/iommu/io-pgtable-arm-v7s.c b/drivers/iommu/io-pgtable-arm-v7s.c
index def8ca1c982d..f50e51c1a9c8 100644
--- a/drivers/iommu/io-pgtable-arm-v7s.c
+++ b/drivers/iommu/io-pgtable-arm-v7s.c
@@ -633,6 +633,10 @@ static struct io_pgtable *arm_v7s_alloc_pgtable(struct io_pgtable_cfg *cfg,
 {
 	struct arm_v7s_io_pgtable *data;
 
+#ifdef PHYS_OFFSET
+	if (upper_32_bits(PHYS_OFFSET))
+		return NULL;
+#endif
 	if (cfg->ias > ARM_V7S_ADDR_BITS || cfg->oas > ARM_V7S_ADDR_BITS)
 		return NULL;
 
diff --git a/drivers/iommu/iommu.c b/drivers/iommu/iommu.c
index b06d93594436..9a2f1960873b 100644
--- a/drivers/iommu/iommu.c
+++ b/drivers/iommu/iommu.c
@@ -31,6 +31,7 @@
 #include <linux/err.h>
 #include <linux/pci.h>
 #include <linux/bitops.h>
+#include <linux/property.h>
 #include <trace/events/iommu.h>
 
 static struct kset *iommu_group_kset;
@@ -1613,3 +1614,60 @@ out:
 
 	return ret;
 }
+
+int iommu_fwspec_init(struct device *dev, struct fwnode_handle *iommu_fwnode,
+		      const struct iommu_ops *ops)
+{
+	struct iommu_fwspec *fwspec = dev->iommu_fwspec;
+
+	if (fwspec)
+		return ops == fwspec->ops ? 0 : -EINVAL;
+
+	fwspec = kzalloc(sizeof(*fwspec), GFP_KERNEL);
+	if (!fwspec)
+		return -ENOMEM;
+
+	of_node_get(to_of_node(iommu_fwnode));
+	fwspec->iommu_fwnode = iommu_fwnode;
+	fwspec->ops = ops;
+	dev->iommu_fwspec = fwspec;
+	return 0;
+}
+EXPORT_SYMBOL_GPL(iommu_fwspec_init);
+
+void iommu_fwspec_free(struct device *dev)
+{
+	struct iommu_fwspec *fwspec = dev->iommu_fwspec;
+
+	if (fwspec) {
+		fwnode_handle_put(fwspec->iommu_fwnode);
+		kfree(fwspec);
+		dev->iommu_fwspec = NULL;
+	}
+}
+EXPORT_SYMBOL_GPL(iommu_fwspec_free);
+
+int iommu_fwspec_add_ids(struct device *dev, u32 *ids, int num_ids)
+{
+	struct iommu_fwspec *fwspec = dev->iommu_fwspec;
+	size_t size;
+	int i;
+
+	if (!fwspec)
+		return -EINVAL;
+
+	size = offsetof(struct iommu_fwspec, ids[fwspec->num_ids + num_ids]);
+	if (size > sizeof(*fwspec)) {
+		fwspec = krealloc(dev->iommu_fwspec, size, GFP_KERNEL);
+		if (!fwspec)
+			return -ENOMEM;
+	}
+
+	for (i = 0; i < num_ids; i++)
+		fwspec->ids[fwspec->num_ids + i] = ids[i];
+
+	fwspec->num_ids += num_ids;
+	dev->iommu_fwspec = fwspec;
+	return 0;
+}
+EXPORT_SYMBOL_GPL(iommu_fwspec_add_ids);
diff --git a/drivers/iommu/ipmmu-vmsa.c b/drivers/iommu/ipmmu-vmsa.c
index 2fdbac67a77f..ace331da6459 100644
--- a/drivers/iommu/ipmmu-vmsa.c
+++ b/drivers/iommu/ipmmu-vmsa.c
@@ -636,7 +636,7 @@ static int ipmmu_add_device(struct device *dev)
 	spin_unlock(&ipmmu_devices_lock);
 
 	if (ret < 0)
-		return -ENODEV;
+		goto error;
 
 	for (i = 0; i < num_utlbs; ++i) {
 		if (utlbs[i] >= mmu->num_utlbs) {
diff --git a/drivers/iommu/of_iommu.c b/drivers/iommu/of_iommu.c
index 57f23eaaa2f9..5b82862f571f 100644
--- a/drivers/iommu/of_iommu.c
+++ b/drivers/iommu/of_iommu.c
@@ -22,6 +22,7 @@
 #include <linux/limits.h>
 #include <linux/of.h>
 #include <linux/of_iommu.h>
+#include <linux/of_pci.h>
 #include <linux/slab.h>
 
 static const struct of_device_id __iommu_of_table_sentinel
@@ -134,6 +135,47 @@ const struct iommu_ops *of_iommu_get_ops(struct device_node *np)
 	return ops;
 }
 
+static int __get_pci_rid(struct pci_dev *pdev, u16 alias, void *data)
+{
+	struct of_phandle_args *iommu_spec = data;
+
+	iommu_spec->args[0] = alias;
+	return iommu_spec->np == pdev->bus->dev.of_node;
+}
+
+static const struct iommu_ops
+*of_pci_iommu_configure(struct pci_dev *pdev, struct device_node *bridge_np)
+{
+	const struct iommu_ops *ops;
+	struct of_phandle_args iommu_spec;
+
+	/*
+	 * Start by tracing the RID alias down the PCI topology as
+	 * far as the host bridge whose OF node we have...
+	 * (we're not even attempting to handle multi-alias devices yet)
+	 */
+	iommu_spec.args_count = 1;
+	iommu_spec.np = bridge_np;
+	pci_for_each_dma_alias(pdev, __get_pci_rid, &iommu_spec);
+	/*
+	 * ...then find out what that becomes once it escapes the PCI
+	 * bus into the system beyond, and which IOMMU it ends up at.
+	 */
+	iommu_spec.np = NULL;
+	if (of_pci_map_rid(bridge_np, iommu_spec.args[0], "iommu-map",
+			   "iommu-map-mask", &iommu_spec.np, iommu_spec.args))
+		return NULL;
+
+	ops = of_iommu_get_ops(iommu_spec.np);
+	if (!ops || !ops->of_xlate ||
+	    iommu_fwspec_init(&pdev->dev, &iommu_spec.np->fwnode, ops) ||
+	    ops->of_xlate(&pdev->dev, &iommu_spec))
+		ops = NULL;
+
+	of_node_put(iommu_spec.np);
+	return ops;
+}
+
 const struct iommu_ops *of_iommu_configure(struct device *dev,
 					   struct device_node *master_np)
 {
@@ -142,12 +184,8 @@ const struct iommu_ops *of_iommu_configure(struct device *dev,
 	const struct iommu_ops *ops = NULL;
 	int idx = 0;
 
-	/*
-	 * We can't do much for PCI devices without knowing how
-	 * device IDs are wired up from the PCI bus to the IOMMU.
-	 */
 	if (dev_is_pci(dev))
-		return NULL;
+		return of_pci_iommu_configure(to_pci_dev(dev), master_np);
 
 	/*
 	 * We don't currently walk up the tree looking for a parent IOMMU.
@@ -160,7 +198,9 @@ const struct iommu_ops *of_iommu_configure(struct device *dev,
 		np = iommu_spec.np;
 		ops = of_iommu_get_ops(np);
 
-		if (!ops || !ops->of_xlate || ops->of_xlate(dev, &iommu_spec))
+		if (!ops || !ops->of_xlate ||
+		    iommu_fwspec_init(dev, &np->fwnode, ops) ||
+		    ops->of_xlate(dev, &iommu_spec))
 			goto err_put_node;
 
 		of_node_put(np);
diff --git a/drivers/iommu/s390-iommu.c b/drivers/iommu/s390-iommu.c
index a04d491cf431..3b44b1d82f3b 100644
--- a/drivers/iommu/s390-iommu.c
+++ b/drivers/iommu/s390-iommu.c
@@ -101,8 +101,7 @@ static int s390_iommu_attach_device(struct iommu_domain *domain,
 		zpci_dma_exit_device(zdev);
 
 	zdev->dma_table = s390_domain->dma_table;
-	rc = zpci_register_ioat(zdev, 0, zdev->start_dma + PAGE_OFFSET,
-				zdev->start_dma + zdev->iommu_size - 1,
+	rc = zpci_register_ioat(zdev, 0, zdev->start_dma, zdev->end_dma,
 				(u64) zdev->dma_table);
 	if (rc)
 		goto out_restore;
diff --git a/drivers/irqchip/Kconfig b/drivers/irqchip/Kconfig
index 7f8728984f44..bc0af3307bbf 100644
--- a/drivers/irqchip/Kconfig
+++ b/drivers/irqchip/Kconfig
@@ -39,6 +39,7 @@ config ARM_GIC_V3_ITS
 	bool
 	depends on PCI
 	depends on PCI_MSI
+	select ACPI_IORT if ACPI
 
 config ARM_NVIC
 	bool
@@ -156,6 +157,13 @@ config PIC32_EVIC
 	select GENERIC_IRQ_CHIP
 	select IRQ_DOMAIN
 
+config JCORE_AIC
+	bool "J-Core integrated AIC" if COMPILE_TEST
+	depends on OF
+	select IRQ_DOMAIN
+	help
+	  Support for the J-Core integrated AIC.
+
 config RENESAS_INTC_IRQPIN
 	bool
 	select IRQ_DOMAIN
@@ -251,6 +259,9 @@ config IRQ_MXS
 config MVEBU_ODMI
 	bool
 
+config MVEBU_PIC
+	bool
+
 config LS_SCFG_MSI
 	def_bool y if SOC_LS1021A || ARCH_LAYERSCAPE
 	depends on PCI && PCI_MSI
@@ -264,3 +275,7 @@ config EZNPS_GIC
 	select IRQ_DOMAIN
 	help
 	  Support the EZchip NPS400 global interrupt controller
+
+config STM32_EXTI
+	bool
+	select IRQ_DOMAIN
diff --git a/drivers/irqchip/Makefile b/drivers/irqchip/Makefile
index 4c203b6b8163..e4dbfc85abdb 100644
--- a/drivers/irqchip/Makefile
+++ b/drivers/irqchip/Makefile
@@ -25,7 +25,7 @@ obj-$(CONFIG_ARCH_SUNXI)		+= irq-sunxi-nmi.o
 obj-$(CONFIG_ARCH_SPEAR3XX)		+= spear-shirq.o
 obj-$(CONFIG_ARM_GIC)			+= irq-gic.o irq-gic-common.o
 obj-$(CONFIG_ARM_GIC_PM)		+= irq-gic-pm.o
-obj-$(CONFIG_REALVIEW_DT)		+= irq-gic-realview.o
+obj-$(CONFIG_ARCH_REALVIEW)		+= irq-gic-realview.o
 obj-$(CONFIG_ARM_GIC_V2M)		+= irq-gic-v2m.o
 obj-$(CONFIG_ARM_GIC_V3)		+= irq-gic-v3.o irq-gic-common.o
 obj-$(CONFIG_ARM_GIC_V3_ITS)		+= irq-gic-v3-its.o irq-gic-v3-its-pci-msi.o irq-gic-v3-its-platform-msi.o
@@ -40,6 +40,7 @@ obj-$(CONFIG_I8259)			+= irq-i8259.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_JCORE_AIC)			+= irq-jcore-aic.o
 obj-$(CONFIG_RENESAS_INTC_IRQPIN)	+= irq-renesas-intc-irqpin.o
 obj-$(CONFIG_RENESAS_IRQC)		+= irq-renesas-irqc.o
 obj-$(CONFIG_VERSATILE_FPGA_IRQ)	+= irq-versatile-fpga.o
@@ -68,6 +69,8 @@ obj-$(CONFIG_INGENIC_IRQ)		+= irq-ingenic.o
 obj-$(CONFIG_IMX_GPCV2)			+= irq-imx-gpcv2.o
 obj-$(CONFIG_PIC32_EVIC)		+= irq-pic32-evic.o
 obj-$(CONFIG_MVEBU_ODMI)		+= irq-mvebu-odmi.o
+obj-$(CONFIG_MVEBU_PIC)			+= irq-mvebu-pic.o
 obj-$(CONFIG_LS_SCFG_MSI)		+= irq-ls-scfg-msi.o
 obj-$(CONFIG_EZNPS_GIC)			+= irq-eznps.o
 obj-$(CONFIG_ARCH_ASPEED)		+= irq-aspeed-vic.o
+obj-$(CONFIG_STM32_EXTI) 		+= irq-stm32-exti.o
diff --git a/drivers/irqchip/irq-bcm6345-l1.c b/drivers/irqchip/irq-bcm6345-l1.c
index b844c89a9506..daa4ae89e466 100644
--- a/drivers/irqchip/irq-bcm6345-l1.c
+++ b/drivers/irqchip/irq-bcm6345-l1.c
@@ -52,7 +52,6 @@
 
 #include <linux/bitops.h>
 #include <linux/cpumask.h>
-#include <linux/kconfig.h>
 #include <linux/kernel.h>
 #include <linux/init.h>
 #include <linux/interrupt.h>
diff --git a/drivers/irqchip/irq-bcm7038-l1.c b/drivers/irqchip/irq-bcm7038-l1.c
index 0fea985ef1dc..353c54986211 100644
--- a/drivers/irqchip/irq-bcm7038-l1.c
+++ b/drivers/irqchip/irq-bcm7038-l1.c
@@ -12,7 +12,6 @@
 #define pr_fmt(fmt)	KBUILD_MODNAME	": " fmt
 
 #include <linux/bitops.h>
-#include <linux/kconfig.h>
 #include <linux/kernel.h>
 #include <linux/init.h>
 #include <linux/interrupt.h>
diff --git a/drivers/irqchip/irq-bcm7120-l2.c b/drivers/irqchip/irq-bcm7120-l2.c
index 0ec92631e23c..64c2692070ef 100644
--- a/drivers/irqchip/irq-bcm7120-l2.c
+++ b/drivers/irqchip/irq-bcm7120-l2.c
@@ -13,7 +13,6 @@
 #include <linux/init.h>
 #include <linux/slab.h>
 #include <linux/module.h>
-#include <linux/kconfig.h>
 #include <linux/kernel.h>
 #include <linux/platform_device.h>
 #include <linux/of.h>
diff --git a/drivers/irqchip/irq-brcmstb-l2.c b/drivers/irqchip/irq-brcmstb-l2.c
index 1d4a5b46d9ae..bddf169c4b37 100644
--- a/drivers/irqchip/irq-brcmstb-l2.c
+++ b/drivers/irqchip/irq-brcmstb-l2.c
@@ -18,7 +18,6 @@
 #include <linux/init.h>
 #include <linux/slab.h>
 #include <linux/module.h>
-#include <linux/kconfig.h>
 #include <linux/platform_device.h>
 #include <linux/spinlock.h>
 #include <linux/of.h>
diff --git a/drivers/irqchip/irq-eznps.c b/drivers/irqchip/irq-eznps.c
index efbf0e4304b7..2a7a38830a8d 100644
--- a/drivers/irqchip/irq-eznps.c
+++ b/drivers/irqchip/irq-eznps.c
@@ -85,7 +85,7 @@ static void nps400_irq_eoi_global(struct irq_data *irqd)
 	nps_ack_gic();
 }
 
-static void nps400_irq_eoi(struct irq_data *irqd)
+static void nps400_irq_ack(struct irq_data *irqd)
 {
 	unsigned int __maybe_unused irq = irqd_to_hwirq(irqd);
 
@@ -103,7 +103,7 @@ static struct irq_chip nps400_irq_chip_percpu = {
 	.name		= "NPS400 IC",
 	.irq_mask	= nps400_irq_mask,
 	.irq_unmask	= nps400_irq_unmask,
-	.irq_eoi	= nps400_irq_eoi,
+	.irq_ack	= nps400_irq_ack,
 };
 
 static int nps400_irq_map(struct irq_domain *d, unsigned int virq,
@@ -135,7 +135,7 @@ static const struct irq_domain_ops nps400_irq_ops = {
 static int __init nps400_of_init(struct device_node *node,
 				 struct device_node *parent)
 {
-	static struct irq_domain *nps400_root_domain;
+	struct irq_domain *nps400_root_domain;
 
 	if (parent) {
 		pr_err("DeviceTree incore ic not a root irq controller\n");
diff --git a/drivers/irqchip/irq-gic-pm.c b/drivers/irqchip/irq-gic-pm.c
index 4cbffba3ff13..ecafd295c31c 100644
--- a/drivers/irqchip/irq-gic-pm.c
+++ b/drivers/irqchip/irq-gic-pm.c
@@ -64,7 +64,6 @@ static int gic_runtime_suspend(struct device *dev)
 
 static int gic_get_clocks(struct device *dev, const struct gic_clk_data *data)
 {
-	struct clk *clk;
 	unsigned int i;
 	int ret;
 
@@ -76,28 +75,16 @@ static int gic_get_clocks(struct device *dev, const struct gic_clk_data *data)
 		return ret;
 
 	for (i = 0; i < data->num_clocks; i++) {
-		clk = of_clk_get_by_name(dev->of_node, data->clocks[i]);
-		if (IS_ERR(clk)) {
-			dev_err(dev, "failed to get clock %s\n",
-				data->clocks[i]);
-			ret = PTR_ERR(clk);
-			goto error;
-		}
-
-		ret = pm_clk_add_clk(dev, clk);
+		ret = of_pm_clk_add_clk(dev, data->clocks[i]);
 		if (ret) {
-			dev_err(dev, "failed to add clock at index %d\n", i);
-			clk_put(clk);
-			goto error;
+			dev_err(dev, "failed to add clock %s\n",
+				data->clocks[i]);
+			pm_clk_destroy(dev);
+			return ret;
 		}
 	}
 
 	return 0;
-
-error:
-	pm_clk_destroy(dev);
-
-	return ret;
 }
 
 static int gic_probe(struct platform_device *pdev)
diff --git a/drivers/irqchip/irq-gic-v2m.c b/drivers/irqchip/irq-gic-v2m.c
index 35eb7ac5d21f..863e073c6f7f 100644
--- a/drivers/irqchip/irq-gic-v2m.c
+++ b/drivers/irqchip/irq-gic-v2m.c
@@ -16,6 +16,7 @@
 #define pr_fmt(fmt) "GICv2m: " fmt
 
 #include <linux/acpi.h>
+#include <linux/dma-iommu.h>
 #include <linux/irq.h>
 #include <linux/irqdomain.h>
 #include <linux/kernel.h>
@@ -108,6 +109,8 @@ static void gicv2m_compose_msi_msg(struct irq_data *data, struct msi_msg *msg)
 
 	if (v2m->flags & GICV2M_NEEDS_SPI_OFFSET)
 		msg->data -= v2m->spi_offset;
+
+	iommu_dma_map_msi_msg(data->irq, msg);
 }
 
 static struct irq_chip gicv2m_irq_chip = {
diff --git a/drivers/irqchip/irq-gic-v3-its-pci-msi.c b/drivers/irqchip/irq-gic-v3-its-pci-msi.c
index aee60ed025dc..aee1c60d7ab5 100644
--- a/drivers/irqchip/irq-gic-v3-its-pci-msi.c
+++ b/drivers/irqchip/irq-gic-v3-its-pci-msi.c
@@ -15,6 +15,7 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#include <linux/acpi_iort.h>
 #include <linux/msi.h>
 #include <linux/of.h>
 #include <linux/of_irq.h>
@@ -106,34 +107,91 @@ static struct of_device_id its_device_id[] = {
 	{},
 };
 
-static int __init its_pci_msi_init(void)
+static int __init its_pci_msi_init_one(struct fwnode_handle *handle,
+				       const char *name)
 {
-	struct device_node *np;
 	struct irq_domain *parent;
 
+	parent = irq_find_matching_fwnode(handle, DOMAIN_BUS_NEXUS);
+	if (!parent || !msi_get_domain_info(parent)) {
+		pr_err("%s: Unable to locate ITS domain\n", name);
+		return -ENXIO;
+	}
+
+	if (!pci_msi_create_irq_domain(handle, &its_pci_msi_domain_info,
+				       parent)) {
+		pr_err("%s: Unable to create PCI domain\n", name);
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+
+static int __init its_pci_of_msi_init(void)
+{
+	struct device_node *np;
+
 	for (np = of_find_matching_node(NULL, its_device_id); np;
 	     np = of_find_matching_node(np, its_device_id)) {
 		if (!of_property_read_bool(np, "msi-controller"))
 			continue;
 
-		parent = irq_find_matching_host(np, DOMAIN_BUS_NEXUS);
-		if (!parent || !msi_get_domain_info(parent)) {
-			pr_err("%s: unable to locate ITS domain\n",
-			       np->full_name);
-			continue;
-		}
-
-		if (!pci_msi_create_irq_domain(of_node_to_fwnode(np),
-					       &its_pci_msi_domain_info,
-					       parent)) {
-			pr_err("%s: unable to create PCI domain\n",
-			       np->full_name);
+		if (its_pci_msi_init_one(of_node_to_fwnode(np), np->full_name))
 			continue;
-		}
 
 		pr_info("PCI/MSI: %s domain created\n", np->full_name);
 	}
 
 	return 0;
 }
+
+#ifdef CONFIG_ACPI
+
+static int __init
+its_pci_msi_parse_madt(struct acpi_subtable_header *header,
+		       const unsigned long end)
+{
+	struct acpi_madt_generic_translator *its_entry;
+	struct fwnode_handle *dom_handle;
+	const char *node_name;
+	int err = -ENXIO;
+
+	its_entry = (struct acpi_madt_generic_translator *)header;
+	node_name = kasprintf(GFP_KERNEL, "ITS@0x%lx",
+			      (long)its_entry->base_address);
+	dom_handle = iort_find_domain_token(its_entry->translation_id);
+	if (!dom_handle) {
+		pr_err("%s: Unable to locate ITS domain handle\n", node_name);
+		goto out;
+	}
+
+	err = its_pci_msi_init_one(dom_handle, node_name);
+	if (!err)
+		pr_info("PCI/MSI: %s domain created\n", node_name);
+
+out:
+	kfree(node_name);
+	return err;
+}
+
+static int __init its_pci_acpi_msi_init(void)
+{
+	acpi_table_parse_madt(ACPI_MADT_TYPE_GENERIC_TRANSLATOR,
+			      its_pci_msi_parse_madt, 0);
+	return 0;
+}
+#else
+static int __init its_pci_acpi_msi_init(void)
+{
+	return 0;
+}
+#endif
+
+static int __init its_pci_msi_init(void)
+{
+	its_pci_of_msi_init();
+	its_pci_acpi_msi_init();
+
+	return 0;
+}
 early_initcall(its_pci_msi_init);
diff --git a/drivers/irqchip/irq-gic-v3-its.c b/drivers/irqchip/irq-gic-v3-its.c
index 36b9c28a5c91..c5dee300e8a3 100644
--- a/drivers/irqchip/irq-gic-v3-its.c
+++ b/drivers/irqchip/irq-gic-v3-its.c
@@ -15,10 +15,14 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#include <linux/acpi.h>
 #include <linux/bitmap.h>
 #include <linux/cpu.h>
 #include <linux/delay.h>
+#include <linux/dma-iommu.h>
 #include <linux/interrupt.h>
+#include <linux/irqdomain.h>
+#include <linux/acpi_iort.h>
 #include <linux/log2.h>
 #include <linux/mm.h>
 #include <linux/msi.h>
@@ -75,7 +79,7 @@ struct its_node {
 	raw_spinlock_t		lock;
 	struct list_head	entry;
 	void __iomem		*base;
-	unsigned long		phys_base;
+	phys_addr_t		phys_base;
 	struct its_cmd_block	*cmd_base;
 	struct its_cmd_block	*cmd_write;
 	struct its_baser	tables[GITS_BASER_NR_REGS];
@@ -115,6 +119,7 @@ struct its_device {
 static LIST_HEAD(its_nodes);
 static DEFINE_SPINLOCK(its_lock);
 static struct rdists *gic_rdists;
+static struct irq_domain *its_parent;
 
 #define gic_data_rdist()		(raw_cpu_ptr(gic_rdists->rdist))
 #define gic_data_rdist_rd_base()	(gic_data_rdist()->rd_base)
@@ -655,6 +660,8 @@ static void its_irq_compose_msi_msg(struct irq_data *d, struct msi_msg *msg)
 	msg->address_lo		= addr & ((1UL << 32) - 1);
 	msg->address_hi		= addr >> 32;
 	msg->data		= its_get_event_id(d);
+
+	iommu_dma_map_msi_msg(d->irq, msg);
 }
 
 static struct irq_chip its_irq_chip = {
@@ -1016,7 +1023,7 @@ static void its_free_tables(struct its_node *its)
 
 static int its_alloc_tables(struct its_node *its)
 {
-	u64 typer = readq_relaxed(its->base + GITS_TYPER);
+	u64 typer = gic_read_typer(its->base + GITS_TYPER);
 	u32 ids = GITS_TYPER_DEVBITS(typer);
 	u64 shr = GITS_BASER_InnerShareable;
 	u64 cache = GITS_BASER_WaWb;
@@ -1191,7 +1198,7 @@ static void its_cpu_init_collection(void)
 		 * We now have to bind each collection to its target
 		 * redistributor.
 		 */
-		if (readq_relaxed(its->base + GITS_TYPER) & GITS_TYPER_PTA) {
+		if (gic_read_typer(its->base + GITS_TYPER) & GITS_TYPER_PTA) {
 			/*
 			 * This ITS wants the physical address of the
 			 * redistributor.
@@ -1201,7 +1208,7 @@ static void its_cpu_init_collection(void)
 			/*
 			 * This ITS wants a linear CPU number.
 			 */
-			target = readq_relaxed(gic_data_rdist_rd_base() + GICR_TYPER);
+			target = gic_read_typer(gic_data_rdist_rd_base() + GICR_TYPER);
 			target = GICR_TYPER_CPU_NUMBER(target) << 16;
 		}
 
@@ -1437,6 +1444,11 @@ static int its_irq_gic_domain_alloc(struct irq_domain *domain,
 		fwspec.param[0] = GIC_IRQ_TYPE_LPI;
 		fwspec.param[1] = hwirq;
 		fwspec.param[2] = IRQ_TYPE_EDGE_RISING;
+	} else if (is_fwnode_irqchip(domain->parent->fwnode)) {
+		fwspec.fwnode = domain->parent->fwnode;
+		fwspec.param_count = 2;
+		fwspec.param[0] = hwirq;
+		fwspec.param[1] = IRQ_TYPE_EDGE_RISING;
 	} else {
 		return -EINVAL;
 	}
@@ -1614,44 +1626,59 @@ static void its_enable_quirks(struct its_node *its)
 	gic_enable_quirks(iidr, its_quirks, its);
 }
 
-static int __init its_probe(struct device_node *node,
-			    struct irq_domain *parent)
+static int its_init_domain(struct fwnode_handle *handle, struct its_node *its)
+{
+	struct irq_domain *inner_domain;
+	struct msi_domain_info *info;
+
+	info = kzalloc(sizeof(*info), GFP_KERNEL);
+	if (!info)
+		return -ENOMEM;
+
+	inner_domain = irq_domain_create_tree(handle, &its_domain_ops, its);
+	if (!inner_domain) {
+		kfree(info);
+		return -ENOMEM;
+	}
+
+	inner_domain->parent = its_parent;
+	inner_domain->bus_token = DOMAIN_BUS_NEXUS;
+	info->ops = &its_msi_domain_ops;
+	info->data = its;
+	inner_domain->host_data = info;
+
+	return 0;
+}
+
+static int __init its_probe_one(struct resource *res,
+				struct fwnode_handle *handle, int numa_node)
 {
-	struct resource res;
 	struct its_node *its;
 	void __iomem *its_base;
-	struct irq_domain *inner_domain;
 	u32 val;
 	u64 baser, tmp;
 	int err;
 
-	err = of_address_to_resource(node, 0, &res);
-	if (err) {
-		pr_warn("%s: no regs?\n", node->full_name);
-		return -ENXIO;
-	}
-
-	its_base = ioremap(res.start, resource_size(&res));
+	its_base = ioremap(res->start, resource_size(res));
 	if (!its_base) {
-		pr_warn("%s: unable to map registers\n", node->full_name);
+		pr_warn("ITS@%pa: Unable to map ITS registers\n", &res->start);
 		return -ENOMEM;
 	}
 
 	val = readl_relaxed(its_base + GITS_PIDR2) & GIC_PIDR2_ARCH_MASK;
 	if (val != 0x30 && val != 0x40) {
-		pr_warn("%s: no ITS detected, giving up\n", node->full_name);
+		pr_warn("ITS@%pa: No ITS detected, giving up\n", &res->start);
 		err = -ENODEV;
 		goto out_unmap;
 	}
 
 	err = its_force_quiescent(its_base);
 	if (err) {
-		pr_warn("%s: failed to quiesce, giving up\n",
-			node->full_name);
+		pr_warn("ITS@%pa: Failed to quiesce, giving up\n", &res->start);
 		goto out_unmap;
 	}
 
-	pr_info("ITS: %s\n", node->full_name);
+	pr_info("ITS %pR\n", res);
 
 	its = kzalloc(sizeof(*its), GFP_KERNEL);
 	if (!its) {
@@ -1663,9 +1690,9 @@ static int __init its_probe(struct device_node *node,
 	INIT_LIST_HEAD(&its->entry);
 	INIT_LIST_HEAD(&its->its_device_list);
 	its->base = its_base;
-	its->phys_base = res.start;
-	its->ite_size = ((readl_relaxed(its_base + GITS_TYPER) >> 4) & 0xf) + 1;
-	its->numa_node = of_node_to_nid(node);
+	its->phys_base = res->start;
+	its->ite_size = ((gic_read_typer(its_base + GITS_TYPER) >> 4) & 0xf) + 1;
+	its->numa_node = numa_node;
 
 	its->cmd_base = kzalloc(ITS_CMD_QUEUE_SZ, GFP_KERNEL);
 	if (!its->cmd_base) {
@@ -1712,28 +1739,9 @@ static int __init its_probe(struct device_node *node,
 	writeq_relaxed(0, its->base + GITS_CWRITER);
 	writel_relaxed(GITS_CTLR_ENABLE, its->base + GITS_CTLR);
 
-	if (of_property_read_bool(node, "msi-controller")) {
-		struct msi_domain_info *info;
-
-		info = kzalloc(sizeof(*info), GFP_KERNEL);
-		if (!info) {
-			err = -ENOMEM;
-			goto out_free_tables;
-		}
-
-		inner_domain = irq_domain_add_tree(node, &its_domain_ops, its);
-		if (!inner_domain) {
-			err = -ENOMEM;
-			kfree(info);
-			goto out_free_tables;
-		}
-
-		inner_domain->parent = parent;
-		inner_domain->bus_token = DOMAIN_BUS_NEXUS;
-		info->ops = &its_msi_domain_ops;
-		info->data = its;
-		inner_domain->host_data = info;
-	}
+	err = its_init_domain(handle, its);
+	if (err)
+		goto out_free_tables;
 
 	spin_lock(&its_lock);
 	list_add(&its->entry, &its_nodes);
@@ -1749,13 +1757,13 @@ out_free_its:
 	kfree(its);
 out_unmap:
 	iounmap(its_base);
-	pr_err("ITS: failed probing %s (%d)\n", node->full_name, err);
+	pr_err("ITS@%pa: failed probing (%d)\n", &res->start, err);
 	return err;
 }
 
 static bool gic_rdists_supports_plpis(void)
 {
-	return !!(readl_relaxed(gic_data_rdist_rd_base() + GICR_TYPER) & GICR_TYPER_PLPIS);
+	return !!(gic_read_typer(gic_data_rdist_rd_base() + GICR_TYPER) & GICR_TYPER_PLPIS);
 }
 
 int its_cpu_init(void)
@@ -1777,16 +1785,92 @@ static struct of_device_id its_device_id[] = {
 	{},
 };
 
-int __init its_init(struct device_node *node, struct rdists *rdists,
-	     struct irq_domain *parent_domain)
+static int __init its_of_probe(struct device_node *node)
 {
 	struct device_node *np;
+	struct resource res;
 
 	for (np = of_find_matching_node(node, its_device_id); np;
 	     np = of_find_matching_node(np, its_device_id)) {
-		its_probe(np, parent_domain);
+		if (!of_property_read_bool(np, "msi-controller")) {
+			pr_warn("%s: no msi-controller property, ITS ignored\n",
+				np->full_name);
+			continue;
+		}
+
+		if (of_address_to_resource(np, 0, &res)) {
+			pr_warn("%s: no regs?\n", np->full_name);
+			continue;
+		}
+
+		its_probe_one(&res, &np->fwnode, of_node_to_nid(np));
+	}
+	return 0;
+}
+
+#ifdef CONFIG_ACPI
+
+#define ACPI_GICV3_ITS_MEM_SIZE (SZ_128K)
+
+static int __init gic_acpi_parse_madt_its(struct acpi_subtable_header *header,
+					  const unsigned long end)
+{
+	struct acpi_madt_generic_translator *its_entry;
+	struct fwnode_handle *dom_handle;
+	struct resource res;
+	int err;
+
+	its_entry = (struct acpi_madt_generic_translator *)header;
+	memset(&res, 0, sizeof(res));
+	res.start = its_entry->base_address;
+	res.end = its_entry->base_address + ACPI_GICV3_ITS_MEM_SIZE - 1;
+	res.flags = IORESOURCE_MEM;
+
+	dom_handle = irq_domain_alloc_fwnode((void *)its_entry->base_address);
+	if (!dom_handle) {
+		pr_err("ITS@%pa: Unable to allocate GICv3 ITS domain token\n",
+		       &res.start);
+		return -ENOMEM;
 	}
 
+	err = iort_register_domain_token(its_entry->translation_id, dom_handle);
+	if (err) {
+		pr_err("ITS@%pa: Unable to register GICv3 ITS domain token (ITS ID %d) to IORT\n",
+		       &res.start, its_entry->translation_id);
+		goto dom_err;
+	}
+
+	err = its_probe_one(&res, dom_handle, NUMA_NO_NODE);
+	if (!err)
+		return 0;
+
+	iort_deregister_domain_token(its_entry->translation_id);
+dom_err:
+	irq_domain_free_fwnode(dom_handle);
+	return err;
+}
+
+static void __init its_acpi_probe(void)
+{
+	acpi_table_parse_madt(ACPI_MADT_TYPE_GENERIC_TRANSLATOR,
+			      gic_acpi_parse_madt_its, 0);
+}
+#else
+static void __init its_acpi_probe(void) { }
+#endif
+
+int __init its_init(struct fwnode_handle *handle, struct rdists *rdists,
+		    struct irq_domain *parent_domain)
+{
+	struct device_node *of_node;
+
+	its_parent = parent_domain;
+	of_node = to_of_node(handle);
+	if (of_node)
+		its_of_probe(of_node);
+	else
+		its_acpi_probe();
+
 	if (list_empty(&its_nodes)) {
 		pr_warn("ITS: No ITS available, not enabling LPIs\n");
 		return -ENXIO;
diff --git a/drivers/irqchip/irq-gic-v3.c b/drivers/irqchip/irq-gic-v3.c
index da6c0ba61d4f..19d642eae096 100644
--- a/drivers/irqchip/irq-gic-v3.c
+++ b/drivers/irqchip/irq-gic-v3.c
@@ -153,7 +153,7 @@ static void gic_enable_redist(bool enable)
 			return;	/* No PM support in this redistributor */
 	}
 
-	while (count--) {
+	while (--count) {
 		val = readl_relaxed(rbase + GICR_WAKER);
 		if (enable ^ (bool)(val & GICR_WAKER_ChildrenAsleep))
 			break;
@@ -495,6 +495,14 @@ static void gic_cpu_sys_reg_init(void)
 	/* Set priority mask register */
 	gic_write_pmr(DEFAULT_PMR_VALUE);
 
+	/*
+	 * Some firmwares hand over to the kernel with the BPR changed from
+	 * its reset value (and with a value large enough to prevent
+	 * any pre-emptive interrupts from working at all). Writing a zero
+	 * to BPR restores is reset value.
+	 */
+	gic_write_bpr1(0);
+
 	if (static_key_true(&supports_deactivate)) {
 		/* EOI drops priority only (mode 1) */
 		gic_write_ctlr(ICC_CTLR_EL1_EOImode_drop);
@@ -911,7 +919,6 @@ static int __init gic_init_bases(void __iomem *dist_base,
 				 u64 redist_stride,
 				 struct fwnode_handle *handle)
 {
-	struct device_node *node;
 	u32 typer;
 	int gic_irqs;
 	int err;
@@ -952,10 +959,8 @@ static int __init gic_init_bases(void __iomem *dist_base,
 
 	set_handle_irq(gic_handle_irq);
 
-	node = to_of_node(handle);
-	if (IS_ENABLED(CONFIG_ARM_GIC_V3_ITS) && gic_dist_supports_lpis() &&
-	    node) /* Temp hack to prevent ITS init for ACPI */
-		its_init(node, &gic_data.rdists, gic_data.domain);
+	if (IS_ENABLED(CONFIG_ARM_GIC_V3_ITS) && gic_dist_supports_lpis())
+		its_init(handle, &gic_data.rdists, gic_data.domain);
 
 	gic_smp_init();
 	gic_dist_init();
diff --git a/drivers/irqchip/irq-gic.c b/drivers/irqchip/irq-gic.c
index 390fac59c6bc..d6c404b3584d 100644
--- a/drivers/irqchip/irq-gic.c
+++ b/drivers/irqchip/irq-gic.c
@@ -91,7 +91,27 @@ struct gic_chip_data {
 #endif
 };
 
-static DEFINE_RAW_SPINLOCK(irq_controller_lock);
+#ifdef CONFIG_BL_SWITCHER
+
+static DEFINE_RAW_SPINLOCK(cpu_map_lock);
+
+#define gic_lock_irqsave(f)		\
+	raw_spin_lock_irqsave(&cpu_map_lock, (f))
+#define gic_unlock_irqrestore(f)	\
+	raw_spin_unlock_irqrestore(&cpu_map_lock, (f))
+
+#define gic_lock()			raw_spin_lock(&cpu_map_lock)
+#define gic_unlock()			raw_spin_unlock(&cpu_map_lock)
+
+#else
+
+#define gic_lock_irqsave(f)		do { (void)(f); } while(0)
+#define gic_unlock_irqrestore(f)	do { (void)(f); } while(0)
+
+#define gic_lock()			do { } while(0)
+#define gic_unlock()			do { } while(0)
+
+#endif
 
 /*
  * The GIC mapping of CPU interfaces does not necessarily match
@@ -317,12 +337,12 @@ static int gic_set_affinity(struct irq_data *d, const struct cpumask *mask_val,
 	if (cpu >= NR_GIC_CPU_IF || cpu >= nr_cpu_ids)
 		return -EINVAL;
 
-	raw_spin_lock_irqsave(&irq_controller_lock, flags);
+	gic_lock_irqsave(flags);
 	mask = 0xff << shift;
 	bit = gic_cpu_map[cpu] << shift;
 	val = readl_relaxed(reg) & ~mask;
 	writel_relaxed(val | bit, reg);
-	raw_spin_unlock_irqrestore(&irq_controller_lock, flags);
+	gic_unlock_irqrestore(flags);
 
 	return IRQ_SET_MASK_OK_DONE;
 }
@@ -374,9 +394,7 @@ static void gic_handle_cascade_irq(struct irq_desc *desc)
 
 	chained_irq_enter(chip, desc);
 
-	raw_spin_lock(&irq_controller_lock);
 	status = readl_relaxed(gic_data_cpu_base(chip_data) + GIC_CPU_INTACK);
-	raw_spin_unlock(&irq_controller_lock);
 
 	gic_irq = (status & GICC_IAR_INT_ID_MASK);
 	if (gic_irq == GICC_INT_SPURIOUS)
@@ -776,7 +794,7 @@ static void gic_raise_softirq(const struct cpumask *mask, unsigned int irq)
 		return;
 	}
 
-	raw_spin_lock_irqsave(&irq_controller_lock, flags);
+	gic_lock_irqsave(flags);
 
 	/* Convert our logical CPU mask into a physical one. */
 	for_each_cpu(cpu, mask)
@@ -791,7 +809,7 @@ static void gic_raise_softirq(const struct cpumask *mask, unsigned int irq)
 	/* this always happens on GIC0 */
 	writel_relaxed(map << 16 | irq, gic_data_dist_base(&gic_data[0]) + GIC_DIST_SOFTINT);
 
-	raw_spin_unlock_irqrestore(&irq_controller_lock, flags);
+	gic_unlock_irqrestore(flags);
 }
 #endif
 
@@ -859,7 +877,7 @@ void gic_migrate_target(unsigned int new_cpu_id)
 	cur_target_mask = 0x01010101 << cur_cpu_id;
 	ror_val = (cur_cpu_id - new_cpu_id) & 31;
 
-	raw_spin_lock(&irq_controller_lock);
+	gic_lock();
 
 	/* Update the target interface for this logical CPU */
 	gic_cpu_map[cpu] = 1 << new_cpu_id;
@@ -879,7 +897,7 @@ void gic_migrate_target(unsigned int new_cpu_id)
 		}
 	}
 
-	raw_spin_unlock(&irq_controller_lock);
+	gic_unlock();
 
 	/*
 	 * Now let's migrate and clear any potential SGIs that might be
@@ -921,7 +939,7 @@ unsigned long gic_get_sgir_physaddr(void)
 	return gic_dist_physaddr + GIC_DIST_SOFTINT;
 }
 
-void __init gic_init_physaddr(struct device_node *node)
+static void __init gic_init_physaddr(struct device_node *node)
 {
 	struct resource res;
 	if (of_address_to_resource(node, 0, &res) == 0) {
@@ -1261,7 +1279,7 @@ static bool gic_check_eoimode(struct device_node *node, void __iomem **base)
 		 */
 		*base += 0xf000;
 		cpuif_res.start += 0xf000;
-		pr_warn("GIC: Adjusting CPU interface base to %pa",
+		pr_warn("GIC: Adjusting CPU interface base to %pa\n",
 			&cpuif_res.start);
 	}
 
diff --git a/drivers/irqchip/irq-i8259.c b/drivers/irqchip/irq-i8259.c
index 6b304eb39bd2..1aec12c6d9ac 100644
--- a/drivers/irqchip/irq-i8259.c
+++ b/drivers/irqchip/irq-i8259.c
@@ -38,6 +38,7 @@ static void disable_8259A_irq(struct irq_data *d);
 static void enable_8259A_irq(struct irq_data *d);
 static void mask_and_ack_8259A(struct irq_data *d);
 static void init_8259A(int auto_eoi);
+static int (*i8259_poll)(void) = i8259_irq;
 
 static struct irq_chip i8259A_chip = {
 	.name			= "XT-PIC",
@@ -51,6 +52,11 @@ static struct irq_chip i8259A_chip = {
  * 8259A PIC functions to handle ISA devices:
  */
 
+void i8259_set_poll(int (*poll)(void))
+{
+	i8259_poll = poll;
+}
+
 /*
  * This contains the irq mask for both 8259A irq controllers,
  */
@@ -89,24 +95,6 @@ static void enable_8259A_irq(struct irq_data *d)
 	raw_spin_unlock_irqrestore(&i8259A_lock, flags);
 }
 
-int i8259A_irq_pending(unsigned int irq)
-{
-	unsigned int mask;
-	unsigned long flags;
-	int ret;
-
-	irq -= I8259A_IRQ_BASE;
-	mask = 1 << irq;
-	raw_spin_lock_irqsave(&i8259A_lock, flags);
-	if (irq < 8)
-		ret = inb(PIC_MASTER_CMD) & mask;
-	else
-		ret = inb(PIC_SLAVE_CMD) & (mask >> 8);
-	raw_spin_unlock_irqrestore(&i8259A_lock, flags);
-
-	return ret;
-}
-
 void make_8259A_irq(unsigned int irq)
 {
 	disable_irq_nosync(irq);
@@ -355,7 +343,7 @@ void __init init_i8259_irqs(void)
 static void i8259_irq_dispatch(struct irq_desc *desc)
 {
 	struct irq_domain *domain = irq_desc_get_handler_data(desc);
-	int hwirq = i8259_irq();
+	int hwirq = i8259_poll();
 	unsigned int irq;
 
 	if (hwirq < 0)
@@ -370,13 +358,15 @@ int __init i8259_of_init(struct device_node *node, struct device_node *parent)
 	struct irq_domain *domain;
 	unsigned int parent_irq;
 
+	domain = __init_i8259_irqs(node);
+
 	parent_irq = irq_of_parse_and_map(node, 0);
 	if (!parent_irq) {
 		pr_err("Failed to map i8259 parent IRQ\n");
+		irq_domain_remove(domain);
 		return -ENODEV;
 	}
 
-	domain = __init_i8259_irqs(node);
 	irq_set_chained_handler_and_data(parent_irq, i8259_irq_dispatch,
 					 domain);
 	return 0;
diff --git a/drivers/irqchip/irq-jcore-aic.c b/drivers/irqchip/irq-jcore-aic.c
new file mode 100644
index 000000000000..033bccb41455
--- /dev/null
+++ b/drivers/irqchip/irq-jcore-aic.c
@@ -0,0 +1,113 @@
+/*
+ * J-Core SoC AIC driver
+ *
+ * Copyright (C) 2015-2016 Smart Energy Instruments, Inc.
+ *
+ * This file is subject to the terms and conditions of the GNU General Public
+ * License.  See the file "COPYING" in the main directory of this archive
+ * for more details.
+ */
+
+#include <linux/irq.h>
+#include <linux/io.h>
+#include <linux/irqchip.h>
+#include <linux/irqdomain.h>
+#include <linux/cpu.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+
+#define JCORE_AIC_MAX_HWIRQ	127
+#define JCORE_AIC1_MIN_HWIRQ	16
+#define JCORE_AIC2_MIN_HWIRQ	64
+
+#define JCORE_AIC1_INTPRI_REG	8
+
+static struct irq_chip jcore_aic;
+
+/*
+ * The J-Core AIC1 and AIC2 are cpu-local interrupt controllers and do
+ * not distinguish or use distinct irq number ranges for per-cpu event
+ * interrupts (timer, IPI). Since information to determine whether a
+ * particular irq number should be treated as per-cpu is not available
+ * at mapping time, we use a wrapper handler function which chooses
+ * the right handler at runtime based on whether IRQF_PERCPU was used
+ * when requesting the irq.
+ */
+
+static void handle_jcore_irq(struct irq_desc *desc)
+{
+	if (irqd_is_per_cpu(irq_desc_get_irq_data(desc)))
+		handle_percpu_irq(desc);
+	else
+		handle_simple_irq(desc);
+}
+
+static int jcore_aic_irqdomain_map(struct irq_domain *d, unsigned int irq,
+				   irq_hw_number_t hwirq)
+{
+	struct irq_chip *aic = d->host_data;
+
+	irq_set_chip_and_handler(irq, aic, handle_jcore_irq);
+
+	return 0;
+}
+
+static const struct irq_domain_ops jcore_aic_irqdomain_ops = {
+	.map = jcore_aic_irqdomain_map,
+	.xlate = irq_domain_xlate_onecell,
+};
+
+static void noop(struct irq_data *data)
+{
+}
+
+static int __init aic_irq_of_init(struct device_node *node,
+				  struct device_node *parent)
+{
+	unsigned min_irq = JCORE_AIC2_MIN_HWIRQ;
+	unsigned dom_sz = JCORE_AIC_MAX_HWIRQ+1;
+	struct irq_domain *domain;
+
+	pr_info("Initializing J-Core AIC\n");
+
+	/* AIC1 needs priority initialization to receive interrupts. */
+	if (of_device_is_compatible(node, "jcore,aic1")) {
+		unsigned cpu;
+
+		for_each_present_cpu(cpu) {
+			void __iomem *base = of_iomap(node, cpu);
+
+			if (!base) {
+				pr_err("Unable to map AIC for cpu %u\n", cpu);
+				return -ENOMEM;
+			}
+			__raw_writel(0xffffffff, base + JCORE_AIC1_INTPRI_REG);
+			iounmap(base);
+		}
+		min_irq = JCORE_AIC1_MIN_HWIRQ;
+	}
+
+	/*
+	 * The irq chip framework requires either mask/unmask or enable/disable
+	 * function pointers to be provided, but the hardware does not have any
+	 * such mechanism; the only interrupt masking is at the cpu level and
+	 * it affects all interrupts. We provide dummy mask/unmask. The hardware
+	 * handles all interrupt control and clears pending status when the cpu
+	 * accepts the interrupt.
+	 */
+	jcore_aic.irq_mask = noop;
+	jcore_aic.irq_unmask = noop;
+	jcore_aic.name = "AIC";
+
+	domain = irq_domain_add_linear(node, dom_sz, &jcore_aic_irqdomain_ops,
+				       &jcore_aic);
+	if (!domain)
+		return -ENOMEM;
+	irq_create_strict_mappings(domain, min_irq, min_irq, dom_sz - min_irq);
+
+	return 0;
+}
+
+IRQCHIP_DECLARE(jcore_aic2, "jcore,aic2", aic_irq_of_init);
+IRQCHIP_DECLARE(jcore_aic1, "jcore,aic1", aic_irq_of_init);
diff --git a/drivers/irqchip/irq-keystone.c b/drivers/irqchip/irq-keystone.c
index deb89d63a728..54a5e870a8f5 100644
--- a/drivers/irqchip/irq-keystone.c
+++ b/drivers/irqchip/irq-keystone.c
@@ -109,7 +109,7 @@ static void keystone_irq_handler(struct irq_desc *desc)
 			dev_dbg(kirq->dev, "dispatch bit %d, virq %d\n",
 				src, virq);
 			if (!virq)
-				dev_warn(kirq->dev, "sporious irq detected hwirq %d, virq %d\n",
+				dev_warn(kirq->dev, "spurious irq detected hwirq %d, virq %d\n",
 					 src, virq);
 			generic_handle_irq(virq);
 		}
diff --git a/drivers/irqchip/irq-metag-ext.c b/drivers/irqchip/irq-metag-ext.c
index 8c38b3d92e1c..0cdd923d1535 100644
--- a/drivers/irqchip/irq-metag-ext.c
+++ b/drivers/irqchip/irq-metag-ext.c
@@ -436,7 +436,6 @@ static int meta_intc_irq_set_type(struct irq_data *data, unsigned int flow_type)
 
 /**
  * meta_intc_irq_demux() - external irq de-multiplexer
- * @irq:	the virtual interrupt number
  * @desc:	the interrupt description structure for this irq
  *
  * The cpu receives an interrupt on TR2 when a SoC interrupt has occurred. It is
diff --git a/drivers/irqchip/irq-mips-gic.c b/drivers/irqchip/irq-mips-gic.c
index 6185696405d5..c0178a122940 100644
--- a/drivers/irqchip/irq-mips-gic.c
+++ b/drivers/irqchip/irq-mips-gic.c
@@ -371,18 +371,13 @@ static void gic_handle_shared_int(bool chained)
 	bitmap_and(pending, pending, intrmask, gic_shared_intrs);
 	bitmap_and(pending, pending, pcpu_mask, gic_shared_intrs);
 
-	intr = find_first_bit(pending, gic_shared_intrs);
-	while (intr != gic_shared_intrs) {
+	for_each_set_bit(intr, pending, gic_shared_intrs) {
 		virq = irq_linear_revmap(gic_irq_domain,
 					 GIC_SHARED_TO_HWIRQ(intr));
 		if (chained)
 			generic_handle_irq(virq);
 		else
 			do_IRQ(virq);
-
-		/* go to next pending bit */
-		bitmap_clear(pending, intr, 1);
-		intr = find_first_bit(pending, gic_shared_intrs);
 	}
 }
 
@@ -518,18 +513,13 @@ static void gic_handle_local_int(bool chained)
 
 	bitmap_and(&pending, &pending, &masked, GIC_NUM_LOCAL_INTRS);
 
-	intr = find_first_bit(&pending, GIC_NUM_LOCAL_INTRS);
-	while (intr != GIC_NUM_LOCAL_INTRS) {
+	for_each_set_bit(intr, &pending, GIC_NUM_LOCAL_INTRS) {
 		virq = irq_linear_revmap(gic_irq_domain,
 					 GIC_LOCAL_TO_HWIRQ(intr));
 		if (chained)
 			generic_handle_irq(virq);
 		else
 			do_IRQ(virq);
-
-		/* go to next pending bit */
-		bitmap_clear(&pending, intr, 1);
-		intr = find_first_bit(&pending, GIC_NUM_LOCAL_INTRS);
 	}
 }
 
diff --git a/drivers/irqchip/irq-mvebu-pic.c b/drivers/irqchip/irq-mvebu-pic.c
new file mode 100644
index 000000000000..eec63951129a
--- /dev/null
+++ b/drivers/irqchip/irq-mvebu-pic.c
@@ -0,0 +1,197 @@
+/*
+ * Copyright (C) 2016 Marvell
+ *
+ * Yehuda Yitschak <yehuday@marvell.com>
+ * Thomas Petazzoni <thomas.petazzoni@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/interrupt.h>
+#include <linux/io.h>
+#include <linux/irq.h>
+#include <linux/irqchip.h>
+#include <linux/irqchip/chained_irq.h>
+#include <linux/irqdomain.h>
+#include <linux/module.h>
+#include <linux/of_irq.h>
+#include <linux/platform_device.h>
+
+#define PIC_CAUSE	       0x0
+#define PIC_MASK	       0x4
+
+#define PIC_MAX_IRQS		32
+#define PIC_MAX_IRQ_MASK	((1UL << PIC_MAX_IRQS) - 1)
+
+struct mvebu_pic {
+	void __iomem *base;
+	u32 parent_irq;
+	struct irq_domain *domain;
+	struct irq_chip irq_chip;
+};
+
+static void mvebu_pic_reset(struct mvebu_pic *pic)
+{
+	/* ACK and mask all interrupts */
+	writel(0, pic->base + PIC_MASK);
+	writel(PIC_MAX_IRQ_MASK, pic->base + PIC_CAUSE);
+}
+
+static void mvebu_pic_eoi_irq(struct irq_data *d)
+{
+	struct mvebu_pic *pic = irq_data_get_irq_chip_data(d);
+
+	writel(1 << d->hwirq, pic->base + PIC_CAUSE);
+}
+
+static void mvebu_pic_mask_irq(struct irq_data *d)
+{
+	struct mvebu_pic *pic = irq_data_get_irq_chip_data(d);
+	u32 reg;
+
+	reg =  readl(pic->base + PIC_MASK);
+	reg |= (1 << d->hwirq);
+	writel(reg, pic->base + PIC_MASK);
+}
+
+static void mvebu_pic_unmask_irq(struct irq_data *d)
+{
+	struct mvebu_pic *pic = irq_data_get_irq_chip_data(d);
+	u32 reg;
+
+	reg = readl(pic->base + PIC_MASK);
+	reg &= ~(1 << d->hwirq);
+	writel(reg, pic->base + PIC_MASK);
+}
+
+static int mvebu_pic_irq_map(struct irq_domain *domain, unsigned int virq,
+			     irq_hw_number_t hwirq)
+{
+	struct mvebu_pic *pic = domain->host_data;
+
+	irq_set_percpu_devid(virq);
+	irq_set_chip_data(virq, pic);
+	irq_set_chip_and_handler(virq, &pic->irq_chip,
+				 handle_percpu_devid_irq);
+	irq_set_status_flags(virq, IRQ_LEVEL);
+	irq_set_probe(virq);
+
+	return 0;
+}
+
+static const struct irq_domain_ops mvebu_pic_domain_ops = {
+	.map = mvebu_pic_irq_map,
+	.xlate = irq_domain_xlate_onecell,
+};
+
+static void mvebu_pic_handle_cascade_irq(struct irq_desc *desc)
+{
+	struct mvebu_pic *pic = irq_desc_get_handler_data(desc);
+	struct irq_chip *chip = irq_desc_get_chip(desc);
+	unsigned long irqmap, irqn;
+	unsigned int cascade_irq;
+
+	irqmap = readl_relaxed(pic->base + PIC_CAUSE);
+	chained_irq_enter(chip, desc);
+
+	for_each_set_bit(irqn, &irqmap, BITS_PER_LONG) {
+		cascade_irq = irq_find_mapping(pic->domain, irqn);
+		generic_handle_irq(cascade_irq);
+	}
+
+	chained_irq_exit(chip, desc);
+}
+
+static void mvebu_pic_enable_percpu_irq(void *data)
+{
+	struct mvebu_pic *pic = data;
+
+	mvebu_pic_reset(pic);
+	enable_percpu_irq(pic->parent_irq, IRQ_TYPE_NONE);
+}
+
+static void mvebu_pic_disable_percpu_irq(void *data)
+{
+	struct mvebu_pic *pic = data;
+
+	disable_percpu_irq(pic->parent_irq);
+}
+
+static int mvebu_pic_probe(struct platform_device *pdev)
+{
+	struct device_node *node = pdev->dev.of_node;
+	struct mvebu_pic *pic;
+	struct irq_chip *irq_chip;
+	struct resource *res;
+
+	pic = devm_kzalloc(&pdev->dev, sizeof(struct mvebu_pic), GFP_KERNEL);
+	if (!pic)
+		return -ENOMEM;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	pic->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(pic->base))
+		return PTR_ERR(pic->base);
+
+	irq_chip = &pic->irq_chip;
+	irq_chip->name = dev_name(&pdev->dev);
+	irq_chip->irq_mask = mvebu_pic_mask_irq;
+	irq_chip->irq_unmask = mvebu_pic_unmask_irq;
+	irq_chip->irq_eoi = mvebu_pic_eoi_irq;
+
+	pic->parent_irq = irq_of_parse_and_map(node, 0);
+	if (pic->parent_irq <= 0) {
+		dev_err(&pdev->dev, "Failed to parse parent interrupt\n");
+		return -EINVAL;
+	}
+
+	pic->domain = irq_domain_add_linear(node, PIC_MAX_IRQS,
+					    &mvebu_pic_domain_ops, pic);
+	if (!pic->domain) {
+		dev_err(&pdev->dev, "Failed to allocate irq domain\n");
+		return -ENOMEM;
+	}
+
+	irq_set_chained_handler(pic->parent_irq, mvebu_pic_handle_cascade_irq);
+	irq_set_handler_data(pic->parent_irq, pic);
+
+	on_each_cpu(mvebu_pic_enable_percpu_irq, pic, 1);
+
+	platform_set_drvdata(pdev, pic);
+
+	return 0;
+}
+
+static int mvebu_pic_remove(struct platform_device *pdev)
+{
+	struct mvebu_pic *pic = platform_get_drvdata(pdev);
+
+	on_each_cpu(mvebu_pic_disable_percpu_irq, pic, 1);
+	irq_domain_remove(pic->domain);
+
+	return 0;
+}
+
+static const struct of_device_id mvebu_pic_of_match[] = {
+	{ .compatible = "marvell,armada-8k-pic", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, mvebu_pic_of_match);
+
+static struct platform_driver mvebu_pic_driver = {
+	.probe  = mvebu_pic_probe,
+	.remove = mvebu_pic_remove,
+	.driver = {
+		.name = "mvebu-pic",
+		.of_match_table = mvebu_pic_of_match,
+	},
+};
+module_platform_driver(mvebu_pic_driver);
+
+MODULE_AUTHOR("Yehuda Yitschak <yehuday@marvell.com>");
+MODULE_AUTHOR("Thomas Petazzoni <thomas.petazzoni@free-electrons.com>");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:mvebu_pic");
+
diff --git a/drivers/irqchip/irq-stm32-exti.c b/drivers/irqchip/irq-stm32-exti.c
new file mode 100644
index 000000000000..491568c95aa5
--- /dev/null
+++ b/drivers/irqchip/irq-stm32-exti.c
@@ -0,0 +1,201 @@
+/*
+ * Copyright (C) Maxime Coquelin 2015
+ * Author:  Maxime Coquelin <mcoquelin.stm32@gmail.com>
+ * License terms:  GNU General Public License (GPL), version 2
+ */
+
+#include <linux/bitops.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/irq.h>
+#include <linux/irqchip.h>
+#include <linux/irqchip/chained_irq.h>
+#include <linux/irqdomain.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+
+#define EXTI_IMR	0x0
+#define EXTI_EMR	0x4
+#define EXTI_RTSR	0x8
+#define EXTI_FTSR	0xc
+#define EXTI_SWIER	0x10
+#define EXTI_PR		0x14
+
+static void stm32_irq_handler(struct irq_desc *desc)
+{
+	struct irq_domain *domain = irq_desc_get_handler_data(desc);
+	struct irq_chip_generic *gc = domain->gc->gc[0];
+	struct irq_chip *chip = irq_desc_get_chip(desc);
+	unsigned long pending;
+	int n;
+
+	chained_irq_enter(chip, desc);
+
+	while ((pending = irq_reg_readl(gc, EXTI_PR))) {
+		for_each_set_bit(n, &pending, BITS_PER_LONG) {
+			generic_handle_irq(irq_find_mapping(domain, n));
+			irq_reg_writel(gc, BIT(n), EXTI_PR);
+		}
+	}
+
+	chained_irq_exit(chip, desc);
+}
+
+static int stm32_irq_set_type(struct irq_data *data, unsigned int type)
+{
+	struct irq_chip_generic *gc = irq_data_get_irq_chip_data(data);
+	int pin = data->hwirq;
+	u32 rtsr, ftsr;
+
+	irq_gc_lock(gc);
+
+	rtsr = irq_reg_readl(gc, EXTI_RTSR);
+	ftsr = irq_reg_readl(gc, EXTI_FTSR);
+
+	switch (type) {
+	case IRQ_TYPE_EDGE_RISING:
+		rtsr |= BIT(pin);
+		ftsr &= ~BIT(pin);
+		break;
+	case IRQ_TYPE_EDGE_FALLING:
+		rtsr &= ~BIT(pin);
+		ftsr |= BIT(pin);
+		break;
+	case IRQ_TYPE_EDGE_BOTH:
+		rtsr |= BIT(pin);
+		ftsr |= BIT(pin);
+		break;
+	default:
+		irq_gc_unlock(gc);
+		return -EINVAL;
+	}
+
+	irq_reg_writel(gc, rtsr, EXTI_RTSR);
+	irq_reg_writel(gc, ftsr, EXTI_FTSR);
+
+	irq_gc_unlock(gc);
+
+	return 0;
+}
+
+static int stm32_irq_set_wake(struct irq_data *data, unsigned int on)
+{
+	struct irq_chip_generic *gc = irq_data_get_irq_chip_data(data);
+	int pin = data->hwirq;
+	u32 emr;
+
+	irq_gc_lock(gc);
+
+	emr = irq_reg_readl(gc, EXTI_EMR);
+	if (on)
+		emr |= BIT(pin);
+	else
+		emr &= ~BIT(pin);
+	irq_reg_writel(gc, emr, EXTI_EMR);
+
+	irq_gc_unlock(gc);
+
+	return 0;
+}
+
+static int stm32_exti_alloc(struct irq_domain *d, unsigned int virq,
+			    unsigned int nr_irqs, void *data)
+{
+	struct irq_chip_generic *gc = d->gc->gc[0];
+	struct irq_fwspec *fwspec = data;
+	irq_hw_number_t hwirq;
+
+	hwirq = fwspec->param[0];
+
+	irq_map_generic_chip(d, virq, hwirq);
+	irq_domain_set_info(d, virq, hwirq, &gc->chip_types->chip, gc,
+			    handle_simple_irq, NULL, NULL);
+
+	return 0;
+}
+
+static void stm32_exti_free(struct irq_domain *d, unsigned int virq,
+			    unsigned int nr_irqs)
+{
+	struct irq_data *data = irq_domain_get_irq_data(d, virq);
+
+	irq_domain_reset_irq_data(data);
+}
+
+struct irq_domain_ops irq_exti_domain_ops = {
+	.map	= irq_map_generic_chip,
+	.xlate	= irq_domain_xlate_onetwocell,
+	.alloc  = stm32_exti_alloc,
+	.free	= stm32_exti_free,
+};
+
+static int __init stm32_exti_init(struct device_node *node,
+				  struct device_node *parent)
+{
+	unsigned int clr = IRQ_NOREQUEST | IRQ_NOPROBE | IRQ_NOAUTOEN;
+	int nr_irqs, nr_exti, ret, i;
+	struct irq_chip_generic *gc;
+	struct irq_domain *domain;
+	void *base;
+
+	base = of_iomap(node, 0);
+	if (!base) {
+		pr_err("%s: Unable to map registers\n", node->full_name);
+		return -ENOMEM;
+	}
+
+	/* Determine number of irqs supported */
+	writel_relaxed(~0UL, base + EXTI_RTSR);
+	nr_exti = fls(readl_relaxed(base + EXTI_RTSR));
+	writel_relaxed(0, base + EXTI_RTSR);
+
+	pr_info("%s: %d External IRQs detected\n", node->full_name, nr_exti);
+
+	domain = irq_domain_add_linear(node, nr_exti,
+				       &irq_exti_domain_ops, NULL);
+	if (!domain) {
+		pr_err("%s: Could not register interrupt domain.\n",
+				node->name);
+		ret = -ENOMEM;
+		goto out_unmap;
+	}
+
+	ret = irq_alloc_domain_generic_chips(domain, nr_exti, 1, "exti",
+					     handle_edge_irq, clr, 0, 0);
+	if (ret) {
+		pr_err("%s: Could not allocate generic interrupt chip.\n",
+			node->full_name);
+		goto out_free_domain;
+	}
+
+	gc = domain->gc->gc[0];
+	gc->reg_base                         = base;
+	gc->chip_types->type               = IRQ_TYPE_EDGE_BOTH;
+	gc->chip_types->chip.name          = gc->chip_types[0].chip.name;
+	gc->chip_types->chip.irq_ack       = irq_gc_ack_set_bit;
+	gc->chip_types->chip.irq_mask      = irq_gc_mask_clr_bit;
+	gc->chip_types->chip.irq_unmask    = irq_gc_mask_set_bit;
+	gc->chip_types->chip.irq_set_type  = stm32_irq_set_type;
+	gc->chip_types->chip.irq_set_wake  = stm32_irq_set_wake;
+	gc->chip_types->regs.ack           = EXTI_PR;
+	gc->chip_types->regs.mask          = EXTI_IMR;
+	gc->chip_types->handler            = handle_edge_irq;
+
+	nr_irqs = of_irq_count(node);
+	for (i = 0; i < nr_irqs; i++) {
+		unsigned int irq = irq_of_parse_and_map(node, i);
+
+		irq_set_handler_data(irq, domain);
+		irq_set_chained_handler(irq, stm32_irq_handler);
+	}
+
+	return 0;
+
+out_free_domain:
+	irq_domain_remove(domain);
+out_unmap:
+	iounmap(base);
+	return ret;
+}
+
+IRQCHIP_DECLARE(stm32_exti, "st,stm32-exti", stm32_exti_init);
diff --git a/drivers/irqchip/irq-vic.c b/drivers/irqchip/irq-vic.c
index f811a7de5857..74192f62dd67 100644
--- a/drivers/irqchip/irq-vic.c
+++ b/drivers/irqchip/irq-vic.c
@@ -498,7 +498,6 @@ void __init vic_init(void __iomem *base, unsigned int irq_start,
  * vic_init_cascaded() - initialise a cascaded vectored interrupt controller
  * @base: iomem base address
  * @parent_irq: the parent IRQ we're cascaded off
- * @irq_start: starting interrupt number, must be muliple of 32
  * @vic_sources: bitmask of interrupt sources to allow
  * @resume_sources: bitmask of interrupt sources to allow for resume
  *
diff --git a/drivers/isdn/hardware/mISDN/avmfritz.c b/drivers/isdn/hardware/mISDN/avmfritz.c
index 292991c90c02..e3fa1cd64470 100644
--- a/drivers/isdn/hardware/mISDN/avmfritz.c
+++ b/drivers/isdn/hardware/mISDN/avmfritz.c
@@ -284,7 +284,7 @@ __write_ctrl_pciv2(struct fritzcard *fc, struct hdlc_hw *hdlc, u32 channel) {
 					  AVM_HDLC_STATUS_1));
 }
 
-void
+static void
 write_ctrl(struct bchannel *bch, int which) {
 	struct fritzcard *fc = bch->hw;
 	struct hdlc_hw *hdlc;
@@ -741,7 +741,7 @@ inithdlc(struct fritzcard *fc)
 	modehdlc(&fc->bch[1], -1);
 }
 
-void
+static void
 clear_pending_hdlc_ints(struct fritzcard *fc)
 {
 	u32 val;
@@ -962,7 +962,7 @@ avm_dctrl(struct mISDNchannel *ch, u32 cmd, void *arg)
 	return err;
 }
 
-int
+static int
 setup_fritz(struct fritzcard *fc)
 {
 	u32 val, ver;
diff --git a/drivers/isdn/hardware/mISDN/hfcmulti.c b/drivers/isdn/hardware/mISDN/hfcmulti.c
index 28543d795188..480c2d7794eb 100644
--- a/drivers/isdn/hardware/mISDN/hfcmulti.c
+++ b/drivers/isdn/hardware/mISDN/hfcmulti.c
@@ -564,19 +564,19 @@ disable_hwirq(struct hfc_multi *hc)
 #define	MAX_TDM_CHAN 32
 
 
-inline void
+static inline void
 enablepcibridge(struct hfc_multi *c)
 {
 	HFC_outb(c, R_BRG_PCM_CFG, (0x0 << 6) | 0x3); /* was _io before */
 }
 
-inline void
+static inline void
 disablepcibridge(struct hfc_multi *c)
 {
 	HFC_outb(c, R_BRG_PCM_CFG, (0x0 << 6) | 0x2); /* was _io before */
 }
 
-inline unsigned char
+static inline unsigned char
 readpcibridge(struct hfc_multi *hc, unsigned char address)
 {
 	unsigned short cipv;
@@ -604,7 +604,7 @@ readpcibridge(struct hfc_multi *hc, unsigned char address)
 	return data;
 }
 
-inline void
+static inline void
 writepcibridge(struct hfc_multi *hc, unsigned char address, unsigned char data)
 {
 	unsigned short cipv;
@@ -634,14 +634,14 @@ writepcibridge(struct hfc_multi *hc, unsigned char address, unsigned char data)
 	outl(datav, hc->pci_iobase);
 }
 
-inline void
+static inline void
 cpld_set_reg(struct hfc_multi *hc, unsigned char reg)
 {
 	/* Do data pin read low byte */
 	HFC_outb(hc, R_GPIO_OUT1, reg);
 }
 
-inline void
+static inline void
 cpld_write_reg(struct hfc_multi *hc, unsigned char reg, unsigned char val)
 {
 	cpld_set_reg(hc, reg);
@@ -653,7 +653,7 @@ cpld_write_reg(struct hfc_multi *hc, unsigned char reg, unsigned char val)
 	return;
 }
 
-inline unsigned char
+static inline unsigned char
 cpld_read_reg(struct hfc_multi *hc, unsigned char reg)
 {
 	unsigned char bytein;
@@ -670,14 +670,14 @@ cpld_read_reg(struct hfc_multi *hc, unsigned char reg)
 	return bytein;
 }
 
-inline void
+static inline void
 vpm_write_address(struct hfc_multi *hc, unsigned short addr)
 {
 	cpld_write_reg(hc, 0, 0xff & addr);
 	cpld_write_reg(hc, 1, 0x01 & (addr >> 8));
 }
 
-inline unsigned short
+static inline unsigned short
 vpm_read_address(struct hfc_multi *c)
 {
 	unsigned short addr;
@@ -691,7 +691,7 @@ vpm_read_address(struct hfc_multi *c)
 	return addr & 0x1ff;
 }
 
-inline unsigned char
+static inline unsigned char
 vpm_in(struct hfc_multi *c, int which, unsigned short addr)
 {
 	unsigned char res;
@@ -712,7 +712,7 @@ vpm_in(struct hfc_multi *c, int which, unsigned short addr)
 	return res;
 }
 
-inline void
+static inline void
 vpm_out(struct hfc_multi *c, int which, unsigned short addr,
 	unsigned char data)
 {
@@ -1024,7 +1024,7 @@ hfcmulti_resync(struct hfc_multi *locked, struct hfc_multi *newmaster, int rm)
 }
 
 /* This must be called AND hc must be locked irqsave!!! */
-inline void
+static inline void
 plxsd_checksync(struct hfc_multi *hc, int rm)
 {
 	if (hc->syncronized) {
diff --git a/drivers/isdn/hardware/mISDN/mISDNipac.c b/drivers/isdn/hardware/mISDN/mISDNipac.c
index aa9b6c3cadc1..8d338ba366d0 100644
--- a/drivers/isdn/hardware/mISDN/mISDNipac.c
+++ b/drivers/isdn/hardware/mISDN/mISDNipac.c
@@ -113,7 +113,7 @@ isac_ph_state_bh(struct dchannel *dch)
 	pr_debug("%s: TE newstate %x\n", isac->name, dch->state);
 }
 
-void
+static void
 isac_empty_fifo(struct isac_hw *isac, int count)
 {
 	u8 *ptr;
diff --git a/drivers/isdn/hardware/mISDN/w6692.c b/drivers/isdn/hardware/mISDN/w6692.c
index 741675525b53..3b067ea656bd 100644
--- a/drivers/isdn/hardware/mISDN/w6692.c
+++ b/drivers/isdn/hardware/mISDN/w6692.c
@@ -848,7 +848,7 @@ dbusy_timer_handler(struct dchannel *dch)
 	}
 }
 
-void initW6692(struct w6692_hw *card)
+static void initW6692(struct w6692_hw *card)
 {
 	u8	val;
 
diff --git a/drivers/leds/Kconfig b/drivers/leds/Kconfig
index 9dcc9b13d495..7a628c6516f6 100644
--- a/drivers/leds/Kconfig
+++ b/drivers/leds/Kconfig
@@ -584,6 +584,18 @@ config LEDS_SEAD3
 	  This driver can also be built as a module. If so the module
 	  will be called leds-sead3.
 
+config LEDS_IS31FL319X
+	tristate "LED Support for ISSI IS31FL319x I2C LED controller family"
+	depends on LEDS_CLASS && I2C && OF
+	select REGMAP_I2C
+	help
+	  This option enables support for LEDs connected to ISSI IS31FL319x
+	  fancy LED driver chips accessed via the I2C bus.
+	  Driver supports individual PWM brightness control for each channel.
+
+	  This driver can also be built as a module. If so the module will be
+	  called leds-is31fl319x.
+
 config LEDS_IS31FL32XX
 	tristate "LED support for ISSI IS31FL32XX I2C LED controller family"
 	depends on LEDS_CLASS && I2C && OF
@@ -631,6 +643,22 @@ config LEDS_VERSATILE
 	  This option enabled support for the LEDs on the ARM Versatile
 	  and RealView boards. Say Y to enabled these.
 
+config LEDS_PM8058
+	tristate "LED Support for the Qualcomm PM8058 PMIC"
+	depends on MFD_PM8921_CORE
+	depends on LEDS_CLASS
+	help
+	  Choose this option if you want to use the LED drivers in
+	  the Qualcomm PM8058 PMIC.
+
+config LEDS_MLXCPLD
+	tristate "LED support for the Mellanox boards"
+	depends on X86_64 && DMI
+	depends on LEDS_CLASS
+	help
+	  This option enabled support for the LEDs on the Mellanox
+	  boards. Say Y to enabled these.
+
 comment "LED Triggers"
 source "drivers/leds/trigger/Kconfig"
 
diff --git a/drivers/leds/Makefile b/drivers/leds/Makefile
index 0684c865a1c0..3965070190f5 100644
--- a/drivers/leds/Makefile
+++ b/drivers/leds/Makefile
@@ -67,7 +67,10 @@ obj-$(CONFIG_LEDS_MENF21BMC)		+= leds-menf21bmc.o
 obj-$(CONFIG_LEDS_KTD2692)		+= leds-ktd2692.o
 obj-$(CONFIG_LEDS_POWERNV)		+= leds-powernv.o
 obj-$(CONFIG_LEDS_SEAD3)		+= leds-sead3.o
+obj-$(CONFIG_LEDS_IS31FL319X)		+= leds-is31fl319x.o
 obj-$(CONFIG_LEDS_IS31FL32XX)		+= leds-is31fl32xx.o
+obj-$(CONFIG_LEDS_PM8058)		+= leds-pm8058.o
+obj-$(CONFIG_LEDS_MLXCPLD)		+= leds-mlxcpld.o
 
 # LED SPI Drivers
 obj-$(CONFIG_LEDS_DAC124S085)		+= leds-dac124s085.o
diff --git a/drivers/leds/led-triggers.c b/drivers/leds/led-triggers.c
index c92702a684ce..431123b048a2 100644
--- a/drivers/leds/led-triggers.c
+++ b/drivers/leds/led-triggers.c
@@ -11,7 +11,7 @@
  *
  */
 
-#include <linux/module.h>
+#include <linux/export.h>
 #include <linux/kernel.h>
 #include <linux/list.h>
 #include <linux/spinlock.h>
@@ -81,21 +81,23 @@ ssize_t led_trigger_show(struct device *dev, struct device_attribute *attr,
 	down_read(&led_cdev->trigger_lock);
 
 	if (!led_cdev->trigger)
-		len += sprintf(buf+len, "[none] ");
+		len += scnprintf(buf+len, PAGE_SIZE - len, "[none] ");
 	else
-		len += sprintf(buf+len, "none ");
+		len += scnprintf(buf+len, PAGE_SIZE - len, "none ");
 
 	list_for_each_entry(trig, &trigger_list, next_trig) {
 		if (led_cdev->trigger && !strcmp(led_cdev->trigger->name,
 							trig->name))
-			len += sprintf(buf+len, "[%s] ", trig->name);
+			len += scnprintf(buf+len, PAGE_SIZE - len, "[%s] ",
+					 trig->name);
 		else
-			len += sprintf(buf+len, "%s ", trig->name);
+			len += scnprintf(buf+len, PAGE_SIZE - len, "%s ",
+					 trig->name);
 	}
 	up_read(&led_cdev->trigger_lock);
 	up_read(&triggers_list_lock);
 
-	len += sprintf(len+buf, "\n");
+	len += scnprintf(len+buf, PAGE_SIZE - len, "\n");
 	return len;
 }
 EXPORT_SYMBOL_GPL(led_trigger_show);
@@ -108,6 +110,9 @@ void led_trigger_set(struct led_classdev *led_cdev, struct led_trigger *trig)
 	char *envp[2];
 	const char *name;
 
+	if (!led_cdev->trigger && !trig)
+		return;
+
 	name = trig ? trig->name : "none";
 	event = kasprintf(GFP_KERNEL, "TRIGGER=%s", name);
 
@@ -136,7 +141,9 @@ void led_trigger_set(struct led_classdev *led_cdev, struct led_trigger *trig)
 	if (event) {
 		envp[0] = event;
 		envp[1] = NULL;
-		kobject_uevent_env(&led_cdev->dev->kobj, KOBJ_CHANGE, envp);
+		if (kobject_uevent_env(&led_cdev->dev->kobj, KOBJ_CHANGE, envp))
+			dev_err(led_cdev->dev,
+				"%s: Error sending uevent\n", __func__);
 		kfree(event);
 	}
 }
@@ -357,7 +364,3 @@ void led_trigger_unregister_simple(struct led_trigger *trig)
 	kfree(trig);
 }
 EXPORT_SYMBOL_GPL(led_trigger_unregister_simple);
-
-MODULE_AUTHOR("Richard Purdie");
-MODULE_LICENSE("GPL");
-MODULE_DESCRIPTION("LED Triggers Core");
diff --git a/drivers/leds/leds-gpio.c b/drivers/leds/leds-gpio.c
index 9b991d46ed84..d400dcaf4d29 100644
--- a/drivers/leds/leds-gpio.c
+++ b/drivers/leds/leds-gpio.c
@@ -26,15 +26,19 @@ struct gpio_led_data {
 	struct gpio_desc *gpiod;
 	u8 can_sleep;
 	u8 blinking;
-	int (*platform_gpio_blink_set)(struct gpio_desc *desc, int state,
-			unsigned long *delay_on, unsigned long *delay_off);
+	gpio_blink_set_t platform_gpio_blink_set;
 };
 
+static inline struct gpio_led_data *
+			cdev_to_gpio_led_data(struct led_classdev *led_cdev)
+{
+	return container_of(led_cdev, struct gpio_led_data, cdev);
+}
+
 static void gpio_led_set(struct led_classdev *led_cdev,
 	enum led_brightness value)
 {
-	struct gpio_led_data *led_dat =
-		container_of(led_cdev, struct gpio_led_data, cdev);
+	struct gpio_led_data *led_dat = cdev_to_gpio_led_data(led_cdev);
 	int level;
 
 	if (value == LED_OFF)
@@ -64,8 +68,7 @@ static int gpio_led_set_blocking(struct led_classdev *led_cdev,
 static int gpio_blink_set(struct led_classdev *led_cdev,
 	unsigned long *delay_on, unsigned long *delay_off)
 {
-	struct gpio_led_data *led_dat =
-		container_of(led_cdev, struct gpio_led_data, cdev);
+	struct gpio_led_data *led_dat = cdev_to_gpio_led_data(led_cdev);
 
 	led_dat->blinking = 1;
 	return led_dat->platform_gpio_blink_set(led_dat->gpiod, GPIO_LED_BLINK,
@@ -74,8 +77,7 @@ static int gpio_blink_set(struct led_classdev *led_cdev,
 
 static int create_gpio_led(const struct gpio_led *template,
 	struct gpio_led_data *led_dat, struct device *parent,
-	int (*blink_set)(struct gpio_desc *, int, unsigned long *,
-			 unsigned long *))
+	gpio_blink_set_t blink_set)
 {
 	int ret, state;
 
@@ -120,10 +122,13 @@ static int create_gpio_led(const struct gpio_led *template,
 		led_dat->platform_gpio_blink_set = blink_set;
 		led_dat->cdev.blink_set = gpio_blink_set;
 	}
-	if (template->default_state == LEDS_GPIO_DEFSTATE_KEEP)
-		state = !!gpiod_get_value_cansleep(led_dat->gpiod);
-	else
+	if (template->default_state == LEDS_GPIO_DEFSTATE_KEEP) {
+		state = gpiod_get_value_cansleep(led_dat->gpiod);
+		if (state < 0)
+			return state;
+	} else {
 		state = (template->default_state == LEDS_GPIO_DEFSTATE_ON);
+	}
 	led_dat->cdev.brightness = state ? LED_FULL : LED_OFF;
 	if (!template->retain_state_suspended)
 		led_dat->cdev.flags |= LED_CORE_SUSPENDRESUME;
@@ -134,7 +139,7 @@ static int create_gpio_led(const struct gpio_led *template,
 	if (ret < 0)
 		return ret;
 
-	return led_classdev_register(parent, &led_dat->cdev);
+	return devm_led_classdev_register(parent, &led_dat->cdev);
 }
 
 struct gpio_leds_priv {
@@ -154,7 +159,6 @@ static struct gpio_leds_priv *gpio_leds_create(struct platform_device *pdev)
 	struct fwnode_handle *child;
 	struct gpio_leds_priv *priv;
 	int count, ret;
-	struct device_node *np;
 
 	count = device_get_child_node_count(dev);
 	if (!count)
@@ -168,26 +172,22 @@ static struct gpio_leds_priv *gpio_leds_create(struct platform_device *pdev)
 		struct gpio_led_data *led_dat = &priv->leds[priv->num_leds];
 		struct gpio_led led = {};
 		const char *state = NULL;
+		struct device_node *np = to_of_node(child);
 
 		led.gpiod = devm_get_gpiod_from_child(dev, NULL, child);
 		if (IS_ERR(led.gpiod)) {
 			fwnode_handle_put(child);
-			ret = PTR_ERR(led.gpiod);
-			goto err;
+			return ERR_CAST(led.gpiod);
 		}
 
-		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) {
-				ret = -EINVAL;
-				goto err;
-			}
+		ret = fwnode_property_read_string(child, "label", &led.name);
+		if (ret && IS_ENABLED(CONFIG_OF) && np)
+			led.name = np->name;
+		if (!led.name) {
+			fwnode_handle_put(child);
+			return ERR_PTR(-EINVAL);
 		}
+
 		fwnode_property_read_string(child, "linux,default-trigger",
 					    &led.default_trigger);
 
@@ -209,18 +209,13 @@ static struct gpio_leds_priv *gpio_leds_create(struct platform_device *pdev)
 		ret = create_gpio_led(&led, led_dat, dev, NULL);
 		if (ret < 0) {
 			fwnode_handle_put(child);
-			goto err;
+			return ERR_PTR(ret);
 		}
 		led_dat->cdev.dev->of_node = np;
 		priv->num_leds++;
 	}
 
 	return priv;
-
-err:
-	for (count = priv->num_leds - 1; count >= 0; count--)
-		led_classdev_unregister(&priv->leds[count].cdev);
-	return ERR_PTR(ret);
 }
 
 static const struct of_device_id of_gpio_leds_match[] = {
@@ -248,13 +243,8 @@ static int gpio_led_probe(struct platform_device *pdev)
 			ret = create_gpio_led(&pdata->leds[i],
 					      &priv->leds[i],
 					      &pdev->dev, pdata->gpio_blink_set);
-			if (ret < 0) {
-				/* On failure: unwind the led creations */
-				for (i = i - 1; i >= 0; i--)
-					led_classdev_unregister(
-							&priv->leds[i].cdev);
+			if (ret < 0)
 				return ret;
-			}
 		}
 	} else {
 		priv = gpio_leds_create(pdev);
@@ -267,17 +257,6 @@ static int gpio_led_probe(struct platform_device *pdev)
 	return 0;
 }
 
-static int gpio_led_remove(struct platform_device *pdev)
-{
-	struct gpio_leds_priv *priv = platform_get_drvdata(pdev);
-	int i;
-
-	for (i = 0; i < priv->num_leds; i++)
-		led_classdev_unregister(&priv->leds[i].cdev);
-
-	return 0;
-}
-
 static void gpio_led_shutdown(struct platform_device *pdev)
 {
 	struct gpio_leds_priv *priv = platform_get_drvdata(pdev);
@@ -292,7 +271,6 @@ static void gpio_led_shutdown(struct platform_device *pdev)
 
 static struct platform_driver gpio_led_driver = {
 	.probe		= gpio_led_probe,
-	.remove		= gpio_led_remove,
 	.shutdown	= gpio_led_shutdown,
 	.driver		= {
 		.name	= "leds-gpio",
diff --git a/drivers/leds/leds-is31fl319x.c b/drivers/leds/leds-is31fl319x.c
new file mode 100644
index 000000000000..f123309597e4
--- /dev/null
+++ b/drivers/leds/leds-is31fl319x.c
@@ -0,0 +1,450 @@
+/*
+ * Copyright 2015-16 Golden Delicious Computers
+ *
+ * Author: Nikolaus Schaller <hns@goldelico.com>
+ *
+ * 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.
+ *
+ * LED driver for the IS31FL319{0,1,3,6,9} to drive 1, 3, 6 or 9 light
+ * effect LEDs.
+ *
+ */
+
+#include <linux/err.h>
+#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>
+
+/* register numbers */
+#define IS31FL319X_SHUTDOWN		0x00
+#define IS31FL319X_CTRL1		0x01
+#define IS31FL319X_CTRL2		0x02
+#define IS31FL319X_CONFIG1		0x03
+#define IS31FL319X_CONFIG2		0x04
+#define IS31FL319X_RAMP_MODE		0x05
+#define IS31FL319X_BREATH_MASK		0x06
+#define IS31FL319X_PWM(channel)		(0x07 + channel)
+#define IS31FL319X_DATA_UPDATE		0x10
+#define IS31FL319X_T0(channel)		(0x11 + channel)
+#define IS31FL319X_T123_1		0x1a
+#define IS31FL319X_T123_2		0x1b
+#define IS31FL319X_T123_3		0x1c
+#define IS31FL319X_T4(channel)		(0x1d + channel)
+#define IS31FL319X_TIME_UPDATE		0x26
+#define IS31FL319X_RESET		0xff
+
+#define IS31FL319X_REG_CNT		(IS31FL319X_RESET + 1)
+
+#define IS31FL319X_MAX_LEDS		9
+
+/* CS (Current Setting) in CONFIG2 register */
+#define IS31FL319X_CONFIG2_CS_SHIFT	4
+#define IS31FL319X_CONFIG2_CS_MASK	0x7
+#define IS31FL319X_CONFIG2_CS_STEP_REF	12
+
+#define IS31FL319X_CURRENT_MIN		((u32)5000)
+#define IS31FL319X_CURRENT_MAX		((u32)40000)
+#define IS31FL319X_CURRENT_STEP		((u32)5000)
+#define IS31FL319X_CURRENT_DEFAULT	((u32)20000)
+
+/* Audio gain in CONFIG2 register */
+#define IS31FL319X_AUDIO_GAIN_DB_MAX	((u32)21)
+#define IS31FL319X_AUDIO_GAIN_DB_STEP	((u32)3)
+
+/*
+ * regmap is used as a cache of chip's register space,
+ * to avoid reading back brightness values from chip,
+ * which is known to hang.
+ */
+struct is31fl319x_chip {
+	const struct is31fl319x_chipdef *cdef;
+	struct i2c_client               *client;
+	struct regmap                   *regmap;
+	struct mutex                    lock;
+	u32                             audio_gain_db;
+
+	struct is31fl319x_led {
+		struct is31fl319x_chip  *chip;
+		struct led_classdev     cdev;
+		u32                     max_microamp;
+		bool                    configured;
+	} leds[IS31FL319X_MAX_LEDS];
+};
+
+struct is31fl319x_chipdef {
+	int num_leds;
+};
+
+static const struct is31fl319x_chipdef is31fl3190_cdef = {
+	.num_leds = 1,
+};
+
+static const struct is31fl319x_chipdef is31fl3193_cdef = {
+	.num_leds = 3,
+};
+
+static const struct is31fl319x_chipdef is31fl3196_cdef = {
+	.num_leds = 6,
+};
+
+static const struct is31fl319x_chipdef is31fl3199_cdef = {
+	.num_leds = 9,
+};
+
+static const struct of_device_id of_is31fl319x_match[] = {
+	{ .compatible = "issi,is31fl3190", .data = &is31fl3190_cdef, },
+	{ .compatible = "issi,is31fl3191", .data = &is31fl3190_cdef, },
+	{ .compatible = "issi,is31fl3193", .data = &is31fl3193_cdef, },
+	{ .compatible = "issi,is31fl3196", .data = &is31fl3196_cdef, },
+	{ .compatible = "issi,is31fl3199", .data = &is31fl3199_cdef, },
+	{ .compatible = "si-en,sn3199",    .data = &is31fl3199_cdef, },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, of_is31fl319x_match);
+
+static int is31fl319x_brightness_set(struct led_classdev *cdev,
+				     enum led_brightness brightness)
+{
+	struct is31fl319x_led *led = container_of(cdev, struct is31fl319x_led,
+						  cdev);
+	struct is31fl319x_chip *is31 = led->chip;
+	int chan = led - is31->leds;
+	int ret;
+	int i;
+	u8 ctrl1 = 0, ctrl2 = 0;
+
+	dev_dbg(&is31->client->dev, "%s %d: %d\n", __func__, chan, brightness);
+
+	mutex_lock(&is31->lock);
+
+	/* update PWM register */
+	ret = regmap_write(is31->regmap, IS31FL319X_PWM(chan), brightness);
+	if (ret < 0)
+		goto out;
+
+	/* read current brightness of all PWM channels */
+	for (i = 0; i < is31->cdef->num_leds; i++) {
+		unsigned int pwm_value;
+		bool on;
+
+		/*
+		 * since neither cdev nor the chip can provide
+		 * the current setting, we read from the regmap cache
+		 */
+
+		ret = regmap_read(is31->regmap, IS31FL319X_PWM(i), &pwm_value);
+		dev_dbg(&is31->client->dev, "%s read %d: ret=%d: %d\n",
+			__func__, i, ret, pwm_value);
+		on = ret >= 0 && pwm_value > LED_OFF;
+
+		if (i < 3)
+			ctrl1 |= on << i;       /* 0..2 => bit 0..2 */
+		else if (i < 6)
+			ctrl1 |= on << (i + 1); /* 3..5 => bit 4..6 */
+		else
+			ctrl2 |= on << (i - 6); /* 6..8 => bit 0..2 */
+	}
+
+	if (ctrl1 > 0 || ctrl2 > 0) {
+		dev_dbg(&is31->client->dev, "power up %02x %02x\n",
+			ctrl1, ctrl2);
+		regmap_write(is31->regmap, IS31FL319X_CTRL1, ctrl1);
+		regmap_write(is31->regmap, IS31FL319X_CTRL2, ctrl2);
+		/* update PWMs */
+		regmap_write(is31->regmap, IS31FL319X_DATA_UPDATE, 0x00);
+		/* enable chip from shut down */
+		ret = regmap_write(is31->regmap, IS31FL319X_SHUTDOWN, 0x01);
+	} else {
+		dev_dbg(&is31->client->dev, "power down\n");
+		/* shut down (no need to clear CTRL1/2) */
+		ret = regmap_write(is31->regmap, IS31FL319X_SHUTDOWN, 0x00);
+	}
+
+out:
+	mutex_unlock(&is31->lock);
+
+	return ret;
+}
+
+static int is31fl319x_parse_child_dt(const struct device *dev,
+				     const struct device_node *child,
+				     struct is31fl319x_led *led)
+{
+	struct led_classdev *cdev = &led->cdev;
+	int ret;
+
+	if (of_property_read_string(child, "label", &cdev->name))
+		cdev->name = child->name;
+
+	ret = of_property_read_string(child, "linux,default-trigger",
+				      &cdev->default_trigger);
+	if (ret < 0 && ret != -EINVAL) /* is optional */
+		return ret;
+
+	led->max_microamp = IS31FL319X_CURRENT_DEFAULT;
+	ret = of_property_read_u32(child, "led-max-microamp",
+				   &led->max_microamp);
+	if (!ret) {
+		if (led->max_microamp < IS31FL319X_CURRENT_MIN)
+			return -EINVAL;	/* not supported */
+		led->max_microamp = min(led->max_microamp,
+					  IS31FL319X_CURRENT_MAX);
+	}
+
+	return 0;
+}
+
+static int is31fl319x_parse_dt(struct device *dev,
+			       struct is31fl319x_chip *is31)
+{
+	struct device_node *np = dev->of_node, *child;
+	const struct of_device_id *of_dev_id;
+	int count;
+	int ret;
+
+	if (!np)
+		return -ENODEV;
+
+	of_dev_id = of_match_device(of_is31fl319x_match, dev);
+	if (!of_dev_id) {
+		dev_err(dev, "Failed to match device with supported chips\n");
+		return -EINVAL;
+	}
+
+	is31->cdef = of_dev_id->data;
+
+	count = of_get_child_count(np);
+
+	dev_dbg(dev, "probe %s with %d leds defined in DT\n",
+		of_dev_id->compatible, count);
+
+	if (!count || count > is31->cdef->num_leds) {
+		dev_err(dev, "Number of leds defined must be between 1 and %u\n",
+			is31->cdef->num_leds);
+		return -ENODEV;
+	}
+
+	for_each_child_of_node(np, child) {
+		struct is31fl319x_led *led;
+		u32 reg;
+
+		ret = of_property_read_u32(child, "reg", &reg);
+		if (ret) {
+			dev_err(dev, "Failed to read led 'reg' property\n");
+			goto put_child_node;
+		}
+
+		if (reg < 1 || reg > is31->cdef->num_leds) {
+			dev_err(dev, "invalid led reg %u\n", reg);
+			ret = -EINVAL;
+			goto put_child_node;
+		}
+
+		led = &is31->leds[reg - 1];
+
+		if (led->configured) {
+			dev_err(dev, "led %u is already configured\n", reg);
+			ret = -EINVAL;
+			goto put_child_node;
+		}
+
+		ret = is31fl319x_parse_child_dt(dev, child, led);
+		if (ret) {
+			dev_err(dev, "led %u DT parsing failed\n", reg);
+			goto put_child_node;
+		}
+
+		led->configured = true;
+	}
+
+	is31->audio_gain_db = 0;
+	ret = of_property_read_u32(np, "audio-gain-db", &is31->audio_gain_db);
+	if (!ret)
+		is31->audio_gain_db = min(is31->audio_gain_db,
+					  IS31FL319X_AUDIO_GAIN_DB_MAX);
+
+	return 0;
+
+put_child_node:
+	of_node_put(child);
+	return ret;
+}
+
+static bool is31fl319x_readable_reg(struct device *dev, unsigned int reg)
+{ /* we have no readable registers */
+	return false;
+}
+
+static bool is31fl319x_volatile_reg(struct device *dev, unsigned int reg)
+{ /* volatile registers are not cached */
+	switch (reg) {
+	case IS31FL319X_DATA_UPDATE:
+	case IS31FL319X_TIME_UPDATE:
+	case IS31FL319X_RESET:
+		return true; /* always write-through */
+	default:
+		return false;
+	}
+}
+
+static const struct reg_default is31fl319x_reg_defaults[] = {
+	{ IS31FL319X_CONFIG1, 0x00},
+	{ IS31FL319X_CONFIG2, 0x00},
+	{ IS31FL319X_PWM(0), 0x00},
+	{ IS31FL319X_PWM(1), 0x00},
+	{ IS31FL319X_PWM(2), 0x00},
+	{ IS31FL319X_PWM(3), 0x00},
+	{ IS31FL319X_PWM(4), 0x00},
+	{ IS31FL319X_PWM(5), 0x00},
+	{ IS31FL319X_PWM(6), 0x00},
+	{ IS31FL319X_PWM(7), 0x00},
+	{ IS31FL319X_PWM(8), 0x00},
+};
+
+static struct regmap_config regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
+	.max_register = IS31FL319X_REG_CNT,
+	.cache_type = REGCACHE_FLAT,
+	.readable_reg = is31fl319x_readable_reg,
+	.volatile_reg = is31fl319x_volatile_reg,
+	.reg_defaults = is31fl319x_reg_defaults,
+	.num_reg_defaults = ARRAY_SIZE(is31fl319x_reg_defaults),
+};
+
+static inline int is31fl319x_microamp_to_cs(struct device *dev, u32 microamp)
+{ /* round down to nearest supported value (range check done by caller) */
+	u32 step = microamp / IS31FL319X_CURRENT_STEP;
+
+	return ((IS31FL319X_CONFIG2_CS_STEP_REF - step) &
+		IS31FL319X_CONFIG2_CS_MASK) <<
+		IS31FL319X_CONFIG2_CS_SHIFT; /* CS encoding */
+}
+
+static inline int is31fl319x_db_to_gain(u32 dezibel)
+{ /* round down to nearest supported value (range check done by caller) */
+	return dezibel / IS31FL319X_AUDIO_GAIN_DB_STEP;
+}
+
+static int is31fl319x_probe(struct i2c_client *client,
+			    const struct i2c_device_id *id)
+{
+	struct is31fl319x_chip *is31;
+	struct device *dev = &client->dev;
+	struct i2c_adapter *adapter = to_i2c_adapter(dev->parent);
+	int err;
+	int i = 0;
+	u32 aggregated_led_microamp = IS31FL319X_CURRENT_MAX;
+
+	if (!i2c_check_functionality(adapter, I2C_FUNC_I2C))
+		return -EIO;
+
+	is31 = devm_kzalloc(&client->dev, sizeof(*is31), GFP_KERNEL);
+	if (!is31)
+		return -ENOMEM;
+
+	mutex_init(&is31->lock);
+
+	err = is31fl319x_parse_dt(&client->dev, is31);
+	if (err)
+		goto free_mutex;
+
+	is31->client = client;
+	is31->regmap = devm_regmap_init_i2c(client, &regmap_config);
+	if (IS_ERR(is31->regmap)) {
+		dev_err(&client->dev, "failed to allocate register map\n");
+		err = PTR_ERR(is31->regmap);
+		goto free_mutex;
+	}
+
+	i2c_set_clientdata(client, is31);
+
+	/* check for write-reply from chip (we can't read any registers) */
+	err = regmap_write(is31->regmap, IS31FL319X_RESET, 0x00);
+	if (err < 0) {
+		dev_err(&client->dev, "no response from chip write: err = %d\n",
+			err);
+		err = -EIO; /* does not answer */
+		goto free_mutex;
+	}
+
+	/*
+	 * Kernel conventions require per-LED led-max-microamp property.
+	 * But the chip does not allow to limit individual LEDs.
+	 * So we take minimum from all subnodes for safety of hardware.
+	 */
+	for (i = 0; i < is31->cdef->num_leds; i++)
+		if (is31->leds[i].configured &&
+		    is31->leds[i].max_microamp < aggregated_led_microamp)
+			aggregated_led_microamp = is31->leds[i].max_microamp;
+
+	regmap_write(is31->regmap, IS31FL319X_CONFIG2,
+		     is31fl319x_microamp_to_cs(dev, aggregated_led_microamp) |
+		     is31fl319x_db_to_gain(is31->audio_gain_db));
+
+	for (i = 0; i < is31->cdef->num_leds; i++) {
+		struct is31fl319x_led *led = &is31->leds[i];
+
+		if (!led->configured)
+			continue;
+
+		led->chip = is31;
+		led->cdev.brightness_set_blocking = is31fl319x_brightness_set;
+
+		err = devm_led_classdev_register(&client->dev, &led->cdev);
+		if (err < 0)
+			goto free_mutex;
+	}
+
+	return 0;
+
+free_mutex:
+	mutex_destroy(&is31->lock);
+	return err;
+}
+
+static int is31fl319x_remove(struct i2c_client *client)
+{
+	struct is31fl319x_chip *is31 = i2c_get_clientdata(client);
+
+	mutex_destroy(&is31->lock);
+	return 0;
+}
+
+/*
+ * i2c-core (and modalias) requires that id_table be properly filled,
+ * even though it is not used for DeviceTree based instantiation.
+ */
+static const struct i2c_device_id is31fl319x_id[] = {
+	{ "is31fl3190" },
+	{ "is31fl3191" },
+	{ "is31fl3193" },
+	{ "is31fl3196" },
+	{ "is31fl3199" },
+	{ "sn3199" },
+	{},
+};
+MODULE_DEVICE_TABLE(i2c, is31fl319x_id);
+
+static struct i2c_driver is31fl319x_driver = {
+	.driver   = {
+		.name           = "leds-is31fl319x",
+		.of_match_table = of_match_ptr(of_is31fl319x_match),
+	},
+	.probe    = is31fl319x_probe,
+	.remove   = is31fl319x_remove,
+	.id_table = is31fl319x_id,
+};
+
+module_i2c_driver(is31fl319x_driver);
+
+MODULE_AUTHOR("H. Nikolaus Schaller <hns@goldelico.com>");
+MODULE_AUTHOR("Andrey Utkin <andrey_utkin@fastmail.com>");
+MODULE_DESCRIPTION("IS31FL319X LED driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/leds/leds-mlxcpld.c b/drivers/leds/leds-mlxcpld.c
new file mode 100644
index 000000000000..197ab9b29a9c
--- /dev/null
+++ b/drivers/leds/leds-mlxcpld.c
@@ -0,0 +1,430 @@
+/*
+ * drivers/leds/leds-mlxcpld.c
+ * Copyright (c) 2016 Mellanox Technologies. All rights reserved.
+ * Copyright (c) 2016 Vadim Pasternak <vadimp@mellanox.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. 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.
+ * 3. Neither the names of the copyright holders nor the names of its
+ *    contributors may be used to endorse or promote products derived from
+ *    this software without specific prior written permission.
+ *
+ * Alternatively, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") version 2 as published by the Free
+ * Software Foundation.
+ *
+ * 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/acpi.h>
+#include <linux/device.h>
+#include <linux/dmi.h>
+#include <linux/hwmon.h>
+#include <linux/hwmon-sysfs.h>
+#include <linux/io.h>
+#include <linux/leds.h>
+#include <linux/module.h>
+#include <linux/mod_devicetable.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+
+#define MLXPLAT_CPLD_LPC_REG_BASE_ADRR     0x2500 /* LPC bus access */
+
+/* Color codes for LEDs */
+#define MLXCPLD_LED_OFFSET_HALF		0x01 /* Offset from solid: 3Hz blink */
+#define MLXCPLD_LED_OFFSET_FULL		0x02 /* Offset from solid: 6Hz blink */
+#define MLXCPLD_LED_IS_OFF		0x00 /* Off */
+#define MLXCPLD_LED_RED_STATIC_ON	0x05 /* Solid red */
+#define MLXCPLD_LED_RED_BLINK_HALF	(MLXCPLD_LED_RED_STATIC_ON + \
+					 MLXCPLD_LED_OFFSET_HALF)
+#define MLXCPLD_LED_RED_BLINK_FULL	(MLXCPLD_LED_RED_STATIC_ON + \
+					 MLXCPLD_LED_OFFSET_FULL)
+#define MLXCPLD_LED_GREEN_STATIC_ON	0x0D /* Solid green */
+#define MLXCPLD_LED_GREEN_BLINK_HALF	(MLXCPLD_LED_GREEN_STATIC_ON + \
+					 MLXCPLD_LED_OFFSET_HALF)
+#define MLXCPLD_LED_GREEN_BLINK_FULL	(MLXCPLD_LED_GREEN_STATIC_ON + \
+					 MLXCPLD_LED_OFFSET_FULL)
+#define MLXCPLD_LED_BLINK_3HZ		167 /* ~167 msec off/on */
+#define MLXCPLD_LED_BLINK_6HZ		83 /* ~83 msec off/on */
+
+/**
+ * mlxcpld_param - LED access parameters:
+ * @offset - offset for LED access in CPLD device
+ * @mask - mask for LED access in CPLD device
+ * @base_color - base color code for LED
+**/
+struct mlxcpld_param {
+	u8 offset;
+	u8 mask;
+	u8 base_color;
+};
+
+/**
+ * mlxcpld_led_priv - LED private data:
+ * @cled - LED class device instance
+ * @param - LED CPLD access parameters
+**/
+struct mlxcpld_led_priv {
+	struct led_classdev cdev;
+	struct mlxcpld_param param;
+};
+
+#define cdev_to_priv(c)		container_of(c, struct mlxcpld_led_priv, cdev)
+
+/**
+ * mlxcpld_led_profile - system LED profile (defined per system class):
+ * @offset - offset for LED access in CPLD device
+ * @mask - mask for LED access in CPLD device
+ * @base_color - base color code
+ * @brightness - default brightness setting (on/off)
+ * @name - LED name
+**/
+struct mlxcpld_led_profile {
+	u8 offset;
+	u8 mask;
+	u8 base_color;
+	enum led_brightness brightness;
+	const char *name;
+};
+
+/**
+ * mlxcpld_led_pdata - system LED private data
+ * @pdev - platform device pointer
+ * @pled - LED class device instance
+ * @profile - system configuration profile
+ * @num_led_instances - number of LED instances
+ * @lock - device access lock
+**/
+struct mlxcpld_led_pdata {
+	struct platform_device *pdev;
+	struct mlxcpld_led_priv *pled;
+	struct mlxcpld_led_profile *profile;
+	int num_led_instances;
+	spinlock_t lock;
+};
+
+static struct mlxcpld_led_pdata *mlxcpld_led;
+
+/* Default profile fit the next Mellanox systems:
+ * "msx6710", "msx6720", "msb7700", "msn2700", "msx1410",
+ * "msn2410", "msb7800", "msn2740"
+ */
+static struct mlxcpld_led_profile mlxcpld_led_default_profile[] = {
+	{
+		0x21, 0xf0, MLXCPLD_LED_GREEN_STATIC_ON, 1,
+		"mlxcpld:fan1:green",
+	},
+	{
+		0x21, 0xf0, MLXCPLD_LED_RED_STATIC_ON, LED_OFF,
+		"mlxcpld:fan1:red",
+	},
+	{
+		0x21, 0x0f, MLXCPLD_LED_GREEN_STATIC_ON, 1,
+		"mlxcpld:fan2:green",
+	},
+	{
+		0x21, 0x0f, MLXCPLD_LED_RED_STATIC_ON, LED_OFF,
+		"mlxcpld:fan2:red",
+	},
+	{
+		0x22, 0xf0, MLXCPLD_LED_GREEN_STATIC_ON, 1,
+		"mlxcpld:fan3:green",
+	},
+	{
+		0x22, 0xf0, MLXCPLD_LED_RED_STATIC_ON, LED_OFF,
+		"mlxcpld:fan3:red",
+	},
+	{
+		0x22, 0x0f, MLXCPLD_LED_GREEN_STATIC_ON, 1,
+		"mlxcpld:fan4:green",
+	},
+	{
+		0x22, 0x0f, MLXCPLD_LED_RED_STATIC_ON, LED_OFF,
+		"mlxcpld:fan4:red",
+	},
+	{
+		0x20, 0x0f, MLXCPLD_LED_GREEN_STATIC_ON, 1,
+		"mlxcpld:psu:green",
+	},
+	{
+		0x20, 0x0f, MLXCPLD_LED_RED_STATIC_ON, LED_OFF,
+		"mlxcpld:psu:red",
+	},
+	{
+		0x20, 0xf0, MLXCPLD_LED_GREEN_STATIC_ON, 1,
+		"mlxcpld:status:green",
+	},
+	{
+		0x20, 0xf0, MLXCPLD_LED_RED_STATIC_ON, LED_OFF,
+		"mlxcpld:status:red",
+	},
+};
+
+/* Profile fit the Mellanox systems based on "msn2100" */
+static struct mlxcpld_led_profile mlxcpld_led_msn2100_profile[] = {
+	{
+		0x21, 0xf0, MLXCPLD_LED_GREEN_STATIC_ON, 1,
+		"mlxcpld:fan:green",
+	},
+	{
+		0x21, 0xf0, MLXCPLD_LED_RED_STATIC_ON, LED_OFF,
+		"mlxcpld:fan:red",
+	},
+	{
+		0x23, 0xf0, MLXCPLD_LED_GREEN_STATIC_ON, 1,
+		"mlxcpld:psu1:green",
+	},
+	{
+		0x23, 0xf0, MLXCPLD_LED_RED_STATIC_ON, LED_OFF,
+		"mlxcpld:psu1:red",
+	},
+	{
+		0x23, 0x0f, MLXCPLD_LED_GREEN_STATIC_ON, 1,
+		"mlxcpld:psu2:green",
+	},
+	{
+		0x23, 0x0f, MLXCPLD_LED_RED_STATIC_ON, LED_OFF,
+		"mlxcpld:psu2:red",
+	},
+	{
+		0x20, 0xf0, MLXCPLD_LED_GREEN_STATIC_ON, 1,
+		"mlxcpld:status:green",
+	},
+	{
+		0x20, 0xf0, MLXCPLD_LED_RED_STATIC_ON, LED_OFF,
+		"mlxcpld:status:red",
+	},
+	{
+		0x24, 0xf0, MLXCPLD_LED_GREEN_STATIC_ON, LED_OFF,
+		"mlxcpld:uid:blue",
+	},
+};
+
+enum mlxcpld_led_platform_types {
+	MLXCPLD_LED_PLATFORM_DEFAULT,
+	MLXCPLD_LED_PLATFORM_MSN2100,
+};
+
+static const char *mlx_product_names[] = {
+	"DEFAULT",
+	"MSN2100",
+};
+
+static enum
+mlxcpld_led_platform_types mlxcpld_led_platform_check_sys_type(void)
+{
+	const char *mlx_product_name;
+	int i;
+
+	mlx_product_name = dmi_get_system_info(DMI_PRODUCT_NAME);
+	if (!mlx_product_name)
+		return MLXCPLD_LED_PLATFORM_DEFAULT;
+
+	for (i = 1;  i < ARRAY_SIZE(mlx_product_names); i++) {
+		if (strstr(mlx_product_name, mlx_product_names[i]))
+			return i;
+	}
+
+	return MLXCPLD_LED_PLATFORM_DEFAULT;
+}
+
+static void mlxcpld_led_bus_access_func(u16 base, u8 offset, u8 rw_flag,
+					u8 *data)
+{
+	u32 addr = base + offset;
+
+	if (rw_flag == 0)
+		outb(*data, addr);
+	else
+		*data = inb(addr);
+}
+
+static void mlxcpld_led_store_hw(u8 mask, u8 off, u8 vset)
+{
+	u8 nib, val;
+
+	/*
+	 * Each LED is controlled through low or high nibble of the relevant
+	 * CPLD register. Register offset is specified by off parameter.
+	 * Parameter vset provides color code: 0x0 for off, 0x5 for solid red,
+	 * 0x6 for 3Hz blink red, 0xd for solid green, 0xe for 3Hz blink
+	 * green.
+	 * Parameter mask specifies which nibble is used for specific LED: mask
+	 * 0xf0 - lower nibble is to be used (bits from 0 to 3), mask 0x0f -
+	 * higher nibble (bits from 4 to 7).
+	 */
+	spin_lock(&mlxcpld_led->lock);
+	mlxcpld_led_bus_access_func(MLXPLAT_CPLD_LPC_REG_BASE_ADRR, off, 1,
+				    &val);
+	nib = (mask == 0xf0) ? vset : (vset << 4);
+	val = (val & mask) | nib;
+	mlxcpld_led_bus_access_func(MLXPLAT_CPLD_LPC_REG_BASE_ADRR, off, 0,
+				    &val);
+	spin_unlock(&mlxcpld_led->lock);
+}
+
+static void mlxcpld_led_brightness_set(struct led_classdev *led,
+				       enum led_brightness value)
+{
+	struct mlxcpld_led_priv *pled = cdev_to_priv(led);
+
+	if (value) {
+		mlxcpld_led_store_hw(pled->param.mask, pled->param.offset,
+				     pled->param.base_color);
+		return;
+	}
+
+	mlxcpld_led_store_hw(pled->param.mask, pled->param.offset,
+			     MLXCPLD_LED_IS_OFF);
+}
+
+static int mlxcpld_led_blink_set(struct led_classdev *led,
+				 unsigned long *delay_on,
+				 unsigned long *delay_off)
+{
+	struct mlxcpld_led_priv *pled = cdev_to_priv(led);
+
+	/*
+	 * HW supports two types of blinking: full (6Hz) and half (3Hz).
+	 * For delay on/off zero default setting 3Hz is used.
+	 */
+	if (!(*delay_on == 0 && *delay_off == 0) &&
+	    !(*delay_on == MLXCPLD_LED_BLINK_3HZ &&
+	      *delay_off == MLXCPLD_LED_BLINK_3HZ) &&
+	    !(*delay_on == MLXCPLD_LED_BLINK_6HZ &&
+	      *delay_off == MLXCPLD_LED_BLINK_6HZ))
+		return -EINVAL;
+
+	if (*delay_on == MLXCPLD_LED_BLINK_6HZ)
+		mlxcpld_led_store_hw(pled->param.mask, pled->param.offset,
+				     pled->param.base_color +
+				     MLXCPLD_LED_OFFSET_FULL);
+	else
+		mlxcpld_led_store_hw(pled->param.mask, pled->param.offset,
+				     pled->param.base_color +
+				     MLXCPLD_LED_OFFSET_HALF);
+
+	return 0;
+}
+
+static int mlxcpld_led_config(struct device *dev,
+			      struct mlxcpld_led_pdata *cpld)
+{
+	int i;
+	int err;
+
+	cpld->pled = devm_kzalloc(dev, sizeof(struct mlxcpld_led_priv) *
+				  cpld->num_led_instances, GFP_KERNEL);
+	if (!cpld->pled)
+		return -ENOMEM;
+
+	for (i = 0; i < cpld->num_led_instances; i++) {
+		cpld->pled[i].cdev.name = cpld->profile[i].name;
+		cpld->pled[i].cdev.brightness = cpld->profile[i].brightness;
+		cpld->pled[i].cdev.max_brightness = 1;
+		cpld->pled[i].cdev.brightness_set = mlxcpld_led_brightness_set;
+		cpld->pled[i].cdev.blink_set = mlxcpld_led_blink_set;
+		cpld->pled[i].cdev.flags = LED_CORE_SUSPENDRESUME;
+		err = devm_led_classdev_register(dev, &cpld->pled[i].cdev);
+		if (err)
+			return err;
+
+		cpld->pled[i].param.offset = mlxcpld_led->profile[i].offset;
+		cpld->pled[i].param.mask = mlxcpld_led->profile[i].mask;
+		cpld->pled[i].param.base_color =
+					mlxcpld_led->profile[i].base_color;
+
+		if (mlxcpld_led->profile[i].brightness)
+			mlxcpld_led_brightness_set(&cpld->pled[i].cdev,
+					mlxcpld_led->profile[i].brightness);
+	}
+
+	return 0;
+}
+
+static int __init mlxcpld_led_probe(struct platform_device *pdev)
+{
+	enum mlxcpld_led_platform_types mlxcpld_led_plat =
+					mlxcpld_led_platform_check_sys_type();
+
+	mlxcpld_led = devm_kzalloc(&pdev->dev, sizeof(*mlxcpld_led),
+				   GFP_KERNEL);
+	if (!mlxcpld_led)
+		return -ENOMEM;
+
+	mlxcpld_led->pdev = pdev;
+
+	switch (mlxcpld_led_plat) {
+	case MLXCPLD_LED_PLATFORM_MSN2100:
+		mlxcpld_led->profile = mlxcpld_led_msn2100_profile;
+		mlxcpld_led->num_led_instances =
+				ARRAY_SIZE(mlxcpld_led_msn2100_profile);
+		break;
+
+	default:
+		mlxcpld_led->profile = mlxcpld_led_default_profile;
+		mlxcpld_led->num_led_instances =
+				ARRAY_SIZE(mlxcpld_led_default_profile);
+		break;
+	}
+
+	spin_lock_init(&mlxcpld_led->lock);
+
+	return mlxcpld_led_config(&pdev->dev, mlxcpld_led);
+}
+
+static struct platform_driver mlxcpld_led_driver = {
+	.driver = {
+		.name	= KBUILD_MODNAME,
+	},
+};
+
+static int __init mlxcpld_led_init(void)
+{
+	struct platform_device *pdev;
+	int err;
+
+	pdev = platform_device_register_simple(KBUILD_MODNAME, -1, NULL, 0);
+	if (IS_ERR(pdev)) {
+		pr_err("Device allocation failed\n");
+		return PTR_ERR(pdev);
+	}
+
+	err = platform_driver_probe(&mlxcpld_led_driver, mlxcpld_led_probe);
+	if (err) {
+		pr_err("Probe platform driver failed\n");
+		platform_device_unregister(pdev);
+	}
+
+	return err;
+}
+
+static void __exit mlxcpld_led_exit(void)
+{
+	platform_device_unregister(mlxcpld_led->pdev);
+	platform_driver_unregister(&mlxcpld_led_driver);
+}
+
+module_init(mlxcpld_led_init);
+module_exit(mlxcpld_led_exit);
+
+MODULE_AUTHOR("Vadim Pasternak <vadimp@mellanox.com>");
+MODULE_DESCRIPTION("Mellanox board LED driver");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:leds_mlxcpld");
diff --git a/drivers/leds/leds-pm8058.c b/drivers/leds/leds-pm8058.c
new file mode 100644
index 000000000000..a52674327857
--- /dev/null
+++ b/drivers/leds/leds-pm8058.c
@@ -0,0 +1,191 @@
+/* Copyright (c) 2010, 2011, 2016 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/leds.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+#include <linux/pm.h>
+#include <linux/regmap.h>
+
+#define PM8058_LED_TYPE_COMMON	0x00
+#define PM8058_LED_TYPE_KEYPAD	0x01
+#define PM8058_LED_TYPE_FLASH	0x02
+
+#define PM8058_LED_TYPE_COMMON_MASK	0xf8
+#define PM8058_LED_TYPE_KEYPAD_MASK	0xf0
+#define PM8058_LED_TYPE_COMMON_SHIFT	3
+#define PM8058_LED_TYPE_KEYPAD_SHIFT	4
+
+struct pm8058_led {
+	struct regmap *map;
+	u32 reg;
+	u32 ledtype;
+	struct led_classdev cdev;
+};
+
+static void pm8058_led_set(struct led_classdev *cled,
+	enum led_brightness value)
+{
+	struct pm8058_led *led;
+	int ret = 0;
+	unsigned int mask = 0;
+	unsigned int val = 0;
+
+	led = container_of(cled, struct pm8058_led, cdev);
+	switch (led->ledtype) {
+	case PM8058_LED_TYPE_COMMON:
+		mask = PM8058_LED_TYPE_COMMON_MASK;
+		val = value << PM8058_LED_TYPE_COMMON_SHIFT;
+		break;
+	case PM8058_LED_TYPE_KEYPAD:
+	case PM8058_LED_TYPE_FLASH:
+		mask = PM8058_LED_TYPE_KEYPAD_MASK;
+		val = value << PM8058_LED_TYPE_KEYPAD_SHIFT;
+		break;
+	default:
+		break;
+	}
+
+	ret = regmap_update_bits(led->map, led->reg, mask, val);
+	if (ret)
+		pr_err("Failed to set LED brightness\n");
+}
+
+static enum led_brightness pm8058_led_get(struct led_classdev *cled)
+{
+	struct pm8058_led *led;
+	int ret;
+	unsigned int val;
+
+	led = container_of(cled, struct pm8058_led, cdev);
+
+	ret = regmap_read(led->map, led->reg, &val);
+	if (ret) {
+		pr_err("Failed to get LED brightness\n");
+		return LED_OFF;
+	}
+
+	switch (led->ledtype) {
+	case PM8058_LED_TYPE_COMMON:
+		val &= PM8058_LED_TYPE_COMMON_MASK;
+		val >>= PM8058_LED_TYPE_COMMON_SHIFT;
+		break;
+	case PM8058_LED_TYPE_KEYPAD:
+	case PM8058_LED_TYPE_FLASH:
+		val &= PM8058_LED_TYPE_KEYPAD_MASK;
+		val >>= PM8058_LED_TYPE_KEYPAD_SHIFT;
+		break;
+	default:
+		val = LED_OFF;
+		break;
+	}
+
+	return val;
+}
+
+static int pm8058_led_probe(struct platform_device *pdev)
+{
+	struct pm8058_led *led;
+	struct device_node *np = pdev->dev.of_node;
+	int ret;
+	struct regmap *map;
+	const char *state;
+	enum led_brightness maxbright;
+
+	led = devm_kzalloc(&pdev->dev, sizeof(*led), GFP_KERNEL);
+	if (!led)
+		return -ENOMEM;
+
+	led->ledtype = (u32)of_device_get_match_data(&pdev->dev);
+
+	map = dev_get_regmap(pdev->dev.parent, NULL);
+	if (!map) {
+		dev_err(&pdev->dev, "Parent regmap unavailable.\n");
+		return -ENXIO;
+	}
+	led->map = map;
+
+	ret = of_property_read_u32(np, "reg", &led->reg);
+	if (ret) {
+		dev_err(&pdev->dev, "no register offset specified\n");
+		return -EINVAL;
+	}
+
+	/* Use label else node name */
+	led->cdev.name = of_get_property(np, "label", NULL) ? : np->name;
+	led->cdev.default_trigger =
+		of_get_property(np, "linux,default-trigger", NULL);
+	led->cdev.brightness_set = pm8058_led_set;
+	led->cdev.brightness_get = pm8058_led_get;
+	if (led->ledtype == PM8058_LED_TYPE_COMMON)
+		maxbright = 31; /* 5 bits */
+	else
+		maxbright = 15; /* 4 bits */
+	led->cdev.max_brightness = maxbright;
+
+	state = of_get_property(np, "default-state", NULL);
+	if (state) {
+		if (!strcmp(state, "keep")) {
+			led->cdev.brightness = pm8058_led_get(&led->cdev);
+		} else if (!strcmp(state, "on")) {
+			led->cdev.brightness = maxbright;
+			pm8058_led_set(&led->cdev, maxbright);
+		} else {
+			led->cdev.brightness = LED_OFF;
+			pm8058_led_set(&led->cdev, LED_OFF);
+		}
+	}
+
+	if (led->ledtype == PM8058_LED_TYPE_KEYPAD ||
+	    led->ledtype == PM8058_LED_TYPE_FLASH)
+		led->cdev.flags	= LED_CORE_SUSPENDRESUME;
+
+	ret = devm_led_classdev_register(&pdev->dev, &led->cdev);
+	if (ret) {
+		dev_err(&pdev->dev, "unable to register led \"%s\"\n",
+			led->cdev.name);
+		return ret;
+	}
+
+	return 0;
+}
+
+static const struct of_device_id pm8058_leds_id_table[] = {
+	{
+		.compatible = "qcom,pm8058-led",
+		.data = (void *)PM8058_LED_TYPE_COMMON
+	},
+	{
+		.compatible = "qcom,pm8058-keypad-led",
+		.data = (void *)PM8058_LED_TYPE_KEYPAD
+	},
+	{
+		.compatible = "qcom,pm8058-flash-led",
+		.data = (void *)PM8058_LED_TYPE_FLASH
+	},
+	{ },
+};
+MODULE_DEVICE_TABLE(of, pm8058_leds_id_table);
+
+static struct platform_driver pm8058_led_driver = {
+	.probe		= pm8058_led_probe,
+	.driver		= {
+		.name	= "pm8058-leds",
+		.of_match_table = pm8058_leds_id_table,
+	},
+};
+module_platform_driver(pm8058_led_driver);
+
+MODULE_DESCRIPTION("PM8058 LEDs driver");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:pm8058-leds");
diff --git a/drivers/lightnvm/Kconfig b/drivers/lightnvm/Kconfig
index 61c68a1f054a..2f5d5f4a4c75 100644
--- a/drivers/lightnvm/Kconfig
+++ b/drivers/lightnvm/Kconfig
@@ -4,7 +4,7 @@
 
 menuconfig NVM
 	bool "Open-Channel SSD target support"
-	depends on BLOCK
+	depends on BLOCK && HAS_DMA
 	help
 	  Say Y here to get to enable Open-channel SSDs.
 
diff --git a/drivers/lightnvm/Makefile b/drivers/lightnvm/Makefile
index a7a0a22cf1a5..1f6b6521016a 100644
--- a/drivers/lightnvm/Makefile
+++ b/drivers/lightnvm/Makefile
@@ -2,6 +2,6 @@
 # Makefile for Open-Channel SSDs.
 #
 
-obj-$(CONFIG_NVM)		:= core.o sysblk.o
+obj-$(CONFIG_NVM)		:= core.o sysblk.o sysfs.o
 obj-$(CONFIG_NVM_GENNVM) 	+= gennvm.o
 obj-$(CONFIG_NVM_RRPC)		+= rrpc.o
diff --git a/drivers/lightnvm/core.c b/drivers/lightnvm/core.c
index 9ebd2cfbd849..1cac0f8bc0dc 100644
--- a/drivers/lightnvm/core.c
+++ b/drivers/lightnvm/core.c
@@ -27,6 +27,8 @@
 #include <linux/lightnvm.h>
 #include <linux/sched/sysctl.h>
 
+#include "lightnvm.h"
+
 static LIST_HEAD(nvm_tgt_types);
 static DECLARE_RWSEM(nvm_tgtt_lock);
 static LIST_HEAD(nvm_mgrs);
@@ -581,6 +583,8 @@ static int nvm_core_init(struct nvm_dev *dev)
 	mutex_init(&dev->mlock);
 	spin_lock_init(&dev->lock);
 
+	blk_queue_logical_block_size(dev->q, dev->sec_size);
+
 	return 0;
 err_fmtype:
 	kfree(dev->lun_map);
@@ -596,15 +600,19 @@ static void nvm_free_mgr(struct nvm_dev *dev)
 	dev->mt = NULL;
 }
 
-static void nvm_free(struct nvm_dev *dev)
+void nvm_free(struct nvm_dev *dev)
 {
 	if (!dev)
 		return;
 
 	nvm_free_mgr(dev);
 
+	if (dev->dma_pool)
+		dev->ops->destroy_dma_pool(dev->dma_pool);
+
 	kfree(dev->lptbl);
 	kfree(dev->lun_map);
+	kfree(dev);
 }
 
 static int nvm_init(struct nvm_dev *dev)
@@ -651,30 +659,19 @@ err:
 
 static void nvm_exit(struct nvm_dev *dev)
 {
-	if (dev->dma_pool)
-		dev->ops->destroy_dma_pool(dev->dma_pool);
-	nvm_free(dev);
+	nvm_sysfs_unregister_dev(dev);
+}
 
-	pr_info("nvm: successfully unloaded\n");
+struct nvm_dev *nvm_alloc_dev(int node)
+{
+	return kzalloc_node(sizeof(struct nvm_dev), GFP_KERNEL, node);
 }
+EXPORT_SYMBOL(nvm_alloc_dev);
 
-int nvm_register(struct request_queue *q, char *disk_name,
-							struct nvm_dev_ops *ops)
+int nvm_register(struct nvm_dev *dev)
 {
-	struct nvm_dev *dev;
 	int ret;
 
-	if (!ops->identity)
-		return -EINVAL;
-
-	dev = kzalloc(sizeof(struct nvm_dev), GFP_KERNEL);
-	if (!dev)
-		return -ENOMEM;
-
-	dev->q = q;
-	dev->ops = ops;
-	strncpy(dev->name, disk_name, DISK_NAME_LEN);
-
 	ret = nvm_init(dev);
 	if (ret)
 		goto err_init;
@@ -694,6 +691,10 @@ int nvm_register(struct request_queue *q, char *disk_name,
 		}
 	}
 
+	ret = nvm_sysfs_register_dev(dev);
+	if (ret)
+		goto err_ppalist;
+
 	if (dev->identity.cap & NVM_ID_DCAP_BBLKMGMT) {
 		ret = nvm_get_sysblock(dev, &dev->sb);
 		if (!ret)
@@ -710,31 +711,21 @@ int nvm_register(struct request_queue *q, char *disk_name,
 	up_write(&nvm_lock);
 
 	return 0;
+err_ppalist:
+	dev->ops->destroy_dma_pool(dev->dma_pool);
 err_init:
 	kfree(dev->lun_map);
-	kfree(dev);
 	return ret;
 }
 EXPORT_SYMBOL(nvm_register);
 
-void nvm_unregister(char *disk_name)
+void nvm_unregister(struct nvm_dev *dev)
 {
-	struct nvm_dev *dev;
-
 	down_write(&nvm_lock);
-	dev = nvm_find_nvm_dev(disk_name);
-	if (!dev) {
-		pr_err("nvm: could not find device %s to unregister\n",
-								disk_name);
-		up_write(&nvm_lock);
-		return;
-	}
-
 	list_del(&dev->devices);
 	up_write(&nvm_lock);
 
 	nvm_exit(dev);
-	kfree(dev);
 }
 EXPORT_SYMBOL(nvm_unregister);
 
@@ -1171,27 +1162,10 @@ static struct miscdevice _nvm_misc = {
 	.nodename	= "lightnvm/control",
 	.fops		= &_ctl_fops,
 };
+module_misc_device(_nvm_misc);
 
 MODULE_ALIAS_MISCDEV(MISC_DYNAMIC_MINOR);
 
-static int __init nvm_mod_init(void)
-{
-	int ret;
-
-	ret = misc_register(&_nvm_misc);
-	if (ret)
-		pr_err("nvm: misc_register failed for control device");
-
-	return ret;
-}
-
-static void __exit nvm_mod_exit(void)
-{
-	misc_deregister(&_nvm_misc);
-}
-
 MODULE_AUTHOR("Matias Bjorling <m@bjorling.me>");
 MODULE_LICENSE("GPL v2");
 MODULE_VERSION("0.1");
-module_init(nvm_mod_init);
-module_exit(nvm_mod_exit);
diff --git a/drivers/lightnvm/lightnvm.h b/drivers/lightnvm/lightnvm.h
new file mode 100644
index 000000000000..305c181509a6
--- /dev/null
+++ b/drivers/lightnvm/lightnvm.h
@@ -0,0 +1,35 @@
+/*
+ * Copyright (C) 2016 CNEX Labs. All rights reserved.
+ * Initial release: Matias Bjorling <matias@cnexlabs.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 675 Mass Ave, Cambridge, MA 02139,
+ * USA.
+ *
+ */
+
+#ifndef LIGHTNVM_H
+#define LIGHTNVM_H
+
+#include <linux/lightnvm.h>
+
+/* core -> sysfs.c */
+int __must_check nvm_sysfs_register_dev(struct nvm_dev *);
+void nvm_sysfs_unregister_dev(struct nvm_dev *);
+int nvm_sysfs_register(void);
+void nvm_sysfs_unregister(void);
+
+/* sysfs > core */
+void nvm_free(struct nvm_dev *);
+
+#endif
diff --git a/drivers/lightnvm/sysfs.c b/drivers/lightnvm/sysfs.c
new file mode 100644
index 000000000000..0338c27ab95a
--- /dev/null
+++ b/drivers/lightnvm/sysfs.c
@@ -0,0 +1,198 @@
+#include <linux/kernel.h>
+#include <linux/lightnvm.h>
+#include <linux/miscdevice.h>
+#include <linux/kobject.h>
+#include <linux/blk-mq.h>
+
+#include "lightnvm.h"
+
+static ssize_t nvm_dev_attr_show(struct device *dev,
+				 struct device_attribute *dattr, char *page)
+{
+	struct nvm_dev *ndev = container_of(dev, struct nvm_dev, dev);
+	struct nvm_id *id = &ndev->identity;
+	struct nvm_id_group *grp = &id->groups[0];
+	struct attribute *attr = &dattr->attr;
+
+	if (strcmp(attr->name, "version") == 0) {
+		return scnprintf(page, PAGE_SIZE, "%u\n", id->ver_id);
+	} else if (strcmp(attr->name, "vendor_opcode") == 0) {
+		return scnprintf(page, PAGE_SIZE, "%u\n", id->vmnt);
+	} else if (strcmp(attr->name, "capabilities") == 0) {
+		return scnprintf(page, PAGE_SIZE, "%u\n", id->cap);
+	} else if (strcmp(attr->name, "device_mode") == 0) {
+		return scnprintf(page, PAGE_SIZE, "%u\n", id->dom);
+	} else if (strcmp(attr->name, "media_manager") == 0) {
+		if (!ndev->mt)
+			return scnprintf(page, PAGE_SIZE, "%s\n", "none");
+		return scnprintf(page, PAGE_SIZE, "%s\n", ndev->mt->name);
+	} else if (strcmp(attr->name, "ppa_format") == 0) {
+		return scnprintf(page, PAGE_SIZE,
+			"0x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x\n",
+			id->ppaf.ch_offset, id->ppaf.ch_len,
+			id->ppaf.lun_offset, id->ppaf.lun_len,
+			id->ppaf.pln_offset, id->ppaf.pln_len,
+			id->ppaf.blk_offset, id->ppaf.blk_len,
+			id->ppaf.pg_offset, id->ppaf.pg_len,
+			id->ppaf.sect_offset, id->ppaf.sect_len);
+	} else if (strcmp(attr->name, "media_type") == 0) {	/* u8 */
+		return scnprintf(page, PAGE_SIZE, "%u\n", grp->mtype);
+	} else if (strcmp(attr->name, "flash_media_type") == 0) {
+		return scnprintf(page, PAGE_SIZE, "%u\n", grp->fmtype);
+	} else if (strcmp(attr->name, "num_channels") == 0) {
+		return scnprintf(page, PAGE_SIZE, "%u\n", grp->num_ch);
+	} else if (strcmp(attr->name, "num_luns") == 0) {
+		return scnprintf(page, PAGE_SIZE, "%u\n", grp->num_lun);
+	} else if (strcmp(attr->name, "num_planes") == 0) {
+		return scnprintf(page, PAGE_SIZE, "%u\n", grp->num_pln);
+	} else if (strcmp(attr->name, "num_blocks") == 0) {	/* u16 */
+		return scnprintf(page, PAGE_SIZE, "%u\n", grp->num_blk);
+	} else if (strcmp(attr->name, "num_pages") == 0) {
+		return scnprintf(page, PAGE_SIZE, "%u\n", grp->num_pg);
+	} else if (strcmp(attr->name, "page_size") == 0) {
+		return scnprintf(page, PAGE_SIZE, "%u\n", grp->fpg_sz);
+	} else if (strcmp(attr->name, "hw_sector_size") == 0) {
+		return scnprintf(page, PAGE_SIZE, "%u\n", grp->csecs);
+	} else if (strcmp(attr->name, "oob_sector_size") == 0) {/* u32 */
+		return scnprintf(page, PAGE_SIZE, "%u\n", grp->sos);
+	} else if (strcmp(attr->name, "read_typ") == 0) {
+		return scnprintf(page, PAGE_SIZE, "%u\n", grp->trdt);
+	} else if (strcmp(attr->name, "read_max") == 0) {
+		return scnprintf(page, PAGE_SIZE, "%u\n", grp->trdm);
+	} else if (strcmp(attr->name, "prog_typ") == 0) {
+		return scnprintf(page, PAGE_SIZE, "%u\n", grp->tprt);
+	} else if (strcmp(attr->name, "prog_max") == 0) {
+		return scnprintf(page, PAGE_SIZE, "%u\n", grp->tprm);
+	} else if (strcmp(attr->name, "erase_typ") == 0) {
+		return scnprintf(page, PAGE_SIZE, "%u\n", grp->tbet);
+	} else if (strcmp(attr->name, "erase_max") == 0) {
+		return scnprintf(page, PAGE_SIZE, "%u\n", grp->tbem);
+	} else if (strcmp(attr->name, "multiplane_modes") == 0) {
+		return scnprintf(page, PAGE_SIZE, "0x%08x\n", grp->mpos);
+	} else if (strcmp(attr->name, "media_capabilities") == 0) {
+		return scnprintf(page, PAGE_SIZE, "0x%08x\n", grp->mccap);
+	} else if (strcmp(attr->name, "max_phys_secs") == 0) {
+		return scnprintf(page, PAGE_SIZE, "%u\n",
+				ndev->ops->max_phys_sect);
+	} else {
+		return scnprintf(page,
+				 PAGE_SIZE,
+				 "Unhandled attr(%s) in `nvm_dev_attr_show`\n",
+				 attr->name);
+	}
+}
+
+#define NVM_DEV_ATTR_RO(_name)						\
+	DEVICE_ATTR(_name, S_IRUGO, nvm_dev_attr_show, NULL)
+
+static NVM_DEV_ATTR_RO(version);
+static NVM_DEV_ATTR_RO(vendor_opcode);
+static NVM_DEV_ATTR_RO(capabilities);
+static NVM_DEV_ATTR_RO(device_mode);
+static NVM_DEV_ATTR_RO(ppa_format);
+static NVM_DEV_ATTR_RO(media_manager);
+
+static NVM_DEV_ATTR_RO(media_type);
+static NVM_DEV_ATTR_RO(flash_media_type);
+static NVM_DEV_ATTR_RO(num_channels);
+static NVM_DEV_ATTR_RO(num_luns);
+static NVM_DEV_ATTR_RO(num_planes);
+static NVM_DEV_ATTR_RO(num_blocks);
+static NVM_DEV_ATTR_RO(num_pages);
+static NVM_DEV_ATTR_RO(page_size);
+static NVM_DEV_ATTR_RO(hw_sector_size);
+static NVM_DEV_ATTR_RO(oob_sector_size);
+static NVM_DEV_ATTR_RO(read_typ);
+static NVM_DEV_ATTR_RO(read_max);
+static NVM_DEV_ATTR_RO(prog_typ);
+static NVM_DEV_ATTR_RO(prog_max);
+static NVM_DEV_ATTR_RO(erase_typ);
+static NVM_DEV_ATTR_RO(erase_max);
+static NVM_DEV_ATTR_RO(multiplane_modes);
+static NVM_DEV_ATTR_RO(media_capabilities);
+static NVM_DEV_ATTR_RO(max_phys_secs);
+
+#define NVM_DEV_ATTR(_name) (dev_attr_##_name##)
+
+static struct attribute *nvm_dev_attrs[] = {
+	&dev_attr_version.attr,
+	&dev_attr_vendor_opcode.attr,
+	&dev_attr_capabilities.attr,
+	&dev_attr_device_mode.attr,
+	&dev_attr_media_manager.attr,
+
+	&dev_attr_ppa_format.attr,
+	&dev_attr_media_type.attr,
+	&dev_attr_flash_media_type.attr,
+	&dev_attr_num_channels.attr,
+	&dev_attr_num_luns.attr,
+	&dev_attr_num_planes.attr,
+	&dev_attr_num_blocks.attr,
+	&dev_attr_num_pages.attr,
+	&dev_attr_page_size.attr,
+	&dev_attr_hw_sector_size.attr,
+	&dev_attr_oob_sector_size.attr,
+	&dev_attr_read_typ.attr,
+	&dev_attr_read_max.attr,
+	&dev_attr_prog_typ.attr,
+	&dev_attr_prog_max.attr,
+	&dev_attr_erase_typ.attr,
+	&dev_attr_erase_max.attr,
+	&dev_attr_multiplane_modes.attr,
+	&dev_attr_media_capabilities.attr,
+	&dev_attr_max_phys_secs.attr,
+	NULL,
+};
+
+static struct attribute_group nvm_dev_attr_group = {
+	.name = "lightnvm",
+	.attrs = nvm_dev_attrs,
+};
+
+static const struct attribute_group *nvm_dev_attr_groups[] = {
+	&nvm_dev_attr_group,
+	NULL,
+};
+
+static void nvm_dev_release(struct device *device)
+{
+	struct nvm_dev *dev = container_of(device, struct nvm_dev, dev);
+	struct request_queue *q = dev->q;
+
+	pr_debug("nvm/sysfs: `nvm_dev_release`\n");
+
+	blk_mq_unregister_dev(device, q);
+
+	nvm_free(dev);
+}
+
+static struct device_type nvm_type = {
+	.name		= "lightnvm",
+	.groups		= nvm_dev_attr_groups,
+	.release	= nvm_dev_release,
+};
+
+int nvm_sysfs_register_dev(struct nvm_dev *dev)
+{
+	int ret;
+
+	if (!dev->parent_dev)
+		return 0;
+
+	dev->dev.parent = dev->parent_dev;
+	dev_set_name(&dev->dev, "%s", dev->name);
+	dev->dev.type = &nvm_type;
+	device_initialize(&dev->dev);
+	ret = device_add(&dev->dev);
+
+	if (!ret)
+		blk_mq_register_dev(&dev->dev, dev->q);
+
+	return ret;
+}
+
+void nvm_sysfs_unregister_dev(struct nvm_dev *dev)
+{
+	if (dev && dev->parent_dev)
+		kobject_put(&dev->dev.kobj);
+}
diff --git a/drivers/macintosh/macio_asic.c b/drivers/macintosh/macio_asic.c
index b6819f0fc608..3f041b187033 100644
--- a/drivers/macintosh/macio_asic.c
+++ b/drivers/macintosh/macio_asic.c
@@ -236,7 +236,7 @@ static void macio_create_fixup_irq(struct macio_dev *dev, int index,
 	unsigned int irq;
 
 	irq = irq_create_mapping(NULL, line);
-	if (irq != NO_IRQ) {
+	if (!irq) {
 		dev->interrupt[index].start = irq;
 		dev->interrupt[index].flags = IORESOURCE_IRQ;
 		dev->interrupt[index].name = dev_name(&dev->ofdev.dev);
@@ -299,7 +299,7 @@ static void macio_setup_interrupts(struct macio_dev *dev)
 			break;
 		res = &dev->interrupt[j];
 		irq = irq_of_parse_and_map(np, i++);
-		if (irq == NO_IRQ)
+		if (!irq)
 			break;
 		res->start = irq;
 		res->flags = IORESOURCE_IRQ;
diff --git a/drivers/macintosh/rack-meter.c b/drivers/macintosh/rack-meter.c
index 465c52219639..775527135b93 100644
--- a/drivers/macintosh/rack-meter.c
+++ b/drivers/macintosh/rack-meter.c
@@ -427,7 +427,7 @@ static int rackmeter_probe(struct macio_dev* mdev,
 	rm->irq = macio_irq(mdev, 1);
 #else
 	rm->irq = irq_of_parse_and_map(i2s, 1);
-	if (rm->irq == NO_IRQ ||
+	if (!rm->irq ||
 	    of_address_to_resource(i2s, 0, &ri2s) ||
 	    of_address_to_resource(i2s, 1, &rdma)) {
 		printk(KERN_ERR
diff --git a/drivers/macintosh/smu.c b/drivers/macintosh/smu.c
index d6f72c826c1c..08edb2c25b60 100644
--- a/drivers/macintosh/smu.c
+++ b/drivers/macintosh/smu.c
@@ -279,7 +279,7 @@ int smu_queue_cmd(struct smu_cmd *cmd)
 	spin_unlock_irqrestore(&smu->lock, flags);
 
 	/* Workaround for early calls when irq isn't available */
-	if (!smu_irq_inited || smu->db_irq == NO_IRQ)
+	if (!smu_irq_inited || !smu->db_irq)
 		smu_spinwait_cmd(cmd);
 
 	return 0;
@@ -498,8 +498,8 @@ int __init smu_init (void)
 	INIT_LIST_HEAD(&smu->cmd_list);
 	INIT_LIST_HEAD(&smu->cmd_i2c_list);
 	smu->of_node = np;
-	smu->db_irq = NO_IRQ;
-	smu->msg_irq = NO_IRQ;
+	smu->db_irq = 0;
+	smu->msg_irq = 0;
 
 	/* smu_cmdbuf_abs is in the low 2G of RAM, can be converted to a
 	 * 32 bits value safely
@@ -587,13 +587,13 @@ static int smu_late_init(void)
 
 	if (smu->db_node) {
 		smu->db_irq = irq_of_parse_and_map(smu->db_node, 0);
-		if (smu->db_irq == NO_IRQ)
+		if (!smu->db_irq)
 			printk(KERN_ERR "smu: failed to map irq for node %s\n",
 			       smu->db_node->full_name);
 	}
 	if (smu->msg_node) {
 		smu->msg_irq = irq_of_parse_and_map(smu->msg_node, 0);
-		if (smu->msg_irq == NO_IRQ)
+		if (!smu->msg_irq)
 			printk(KERN_ERR "smu: failed to map irq for node %s\n",
 			       smu->msg_node->full_name);
 	}
@@ -602,23 +602,23 @@ static int smu_late_init(void)
 	 * Try to request the interrupts
 	 */
 
-	if (smu->db_irq != NO_IRQ) {
+	if (smu->db_irq) {
 		if (request_irq(smu->db_irq, smu_db_intr,
 				IRQF_SHARED, "SMU doorbell", smu) < 0) {
 			printk(KERN_WARNING "SMU: can't "
 			       "request interrupt %d\n",
 			       smu->db_irq);
-			smu->db_irq = NO_IRQ;
+			smu->db_irq = 0;
 		}
 	}
 
-	if (smu->msg_irq != NO_IRQ) {
+	if (smu->msg_irq) {
 		if (request_irq(smu->msg_irq, smu_msg_intr,
 				IRQF_SHARED, "SMU message", smu) < 0) {
 			printk(KERN_WARNING "SMU: can't "
 			       "request interrupt %d\n",
 			       smu->msg_irq);
-			smu->msg_irq = NO_IRQ;
+			smu->msg_irq = 0;
 		}
 	}
 
diff --git a/drivers/macintosh/via-cuda.c b/drivers/macintosh/via-cuda.c
index bad18130f125..2088e23a8002 100644
--- a/drivers/macintosh/via-cuda.c
+++ b/drivers/macintosh/via-cuda.c
@@ -209,7 +209,7 @@ static int __init via_cuda_start(void)
     cuda_irq = IRQ_MAC_ADB;
 #else
     cuda_irq = irq_of_parse_and_map(vias, 0);
-    if (cuda_irq == NO_IRQ) {
+    if (!cuda_irq) {
 	printk(KERN_ERR "via-cuda: can't map interrupts for %s\n",
 	       vias->full_name);
 	return -ENODEV;
diff --git a/drivers/macintosh/via-pmu.c b/drivers/macintosh/via-pmu.c
index f8b6d1403c16..91081dcdc272 100644
--- a/drivers/macintosh/via-pmu.c
+++ b/drivers/macintosh/via-pmu.c
@@ -145,7 +145,7 @@ static int pmu_fully_inited;
 static int pmu_has_adb;
 static struct device_node *gpio_node;
 static unsigned char __iomem *gpio_reg;
-static int gpio_irq = NO_IRQ;
+static int gpio_irq = 0;
 static int gpio_irq_enabled = -1;
 static volatile int pmu_suspended;
 static spinlock_t pmu_lock;
@@ -402,7 +402,7 @@ static int __init via_pmu_start(void)
 	batt_req.complete = 1;
 
 	irq = irq_of_parse_and_map(vias, 0);
-	if (irq == NO_IRQ) {
+	if (!irq) {
 		printk(KERN_ERR "via-pmu: can't map interrupt\n");
 		return -ENODEV;
 	}
@@ -424,7 +424,7 @@ static int __init via_pmu_start(void)
 		if (gpio_node)
 			gpio_irq = irq_of_parse_and_map(gpio_node, 0);
 
-		if (gpio_irq != NO_IRQ) {
+		if (gpio_irq) {
 			if (request_irq(gpio_irq, gpio1_interrupt,
 					IRQF_NO_SUSPEND, "GPIO1 ADB",
 					(void *)0))
diff --git a/drivers/mailbox/Kconfig b/drivers/mailbox/Kconfig
index 7817d40d81e7..11eebfe8a4cb 100644
--- a/drivers/mailbox/Kconfig
+++ b/drivers/mailbox/Kconfig
@@ -15,6 +15,16 @@ config ARM_MHU
 	  The controller has 3 mailbox channels, the last of which can be
 	  used in Secure mode only.
 
+config PLATFORM_MHU
+	tristate "Platform MHU Mailbox"
+	depends on OF
+	depends on HAS_IOMEM
+	help
+	  Say Y here if you want to build a platform specific variant MHU
+	  controller driver.
+	  The controller has a maximum of 3 mailbox channels, the last of
+	  which can be used in Secure mode only.
+
 config PL320_MBOX
 	bool "ARM PL320 Mailbox"
 	depends on ARM_AMBA
diff --git a/drivers/mailbox/Makefile b/drivers/mailbox/Makefile
index 66c38e300dfc..ace6fed8fea9 100644
--- a/drivers/mailbox/Makefile
+++ b/drivers/mailbox/Makefile
@@ -6,6 +6,8 @@ obj-$(CONFIG_MAILBOX_TEST)	+= mailbox-test.o
 
 obj-$(CONFIG_ARM_MHU)	+= arm_mhu.o
 
+obj-$(CONFIG_PLATFORM_MHU)	+= platform_mhu.o
+
 obj-$(CONFIG_PL320_MBOX)	+= pl320-ipc.o
 
 obj-$(CONFIG_OMAP2PLUS_MBOX)	+= omap-mailbox.o
diff --git a/drivers/mailbox/pcc.c b/drivers/mailbox/pcc.c
index 043828d541f7..08c87fadca8c 100644
--- a/drivers/mailbox/pcc.c
+++ b/drivers/mailbox/pcc.c
@@ -59,6 +59,7 @@
 #include <linux/delay.h>
 #include <linux/io.h>
 #include <linux/init.h>
+#include <linux/interrupt.h>
 #include <linux/list.h>
 #include <linux/platform_device.h>
 #include <linux/mailbox_controller.h>
@@ -68,11 +69,16 @@
 #include "mailbox.h"
 
 #define MAX_PCC_SUBSPACES	256
+#define MBOX_IRQ_NAME		"pcc-mbox"
 
 static struct mbox_chan *pcc_mbox_channels;
 
 /* Array of cached virtual address for doorbell registers */
 static void __iomem **pcc_doorbell_vaddr;
+/* Array of cached virtual address for doorbell ack registers */
+static void __iomem **pcc_doorbell_ack_vaddr;
+/* Array of doorbell interrupts */
+static int *pcc_doorbell_irq;
 
 static struct mbox_controller pcc_mbox_ctrl = {};
 /**
@@ -91,6 +97,132 @@ static struct mbox_chan *get_pcc_channel(int id)
 	return &pcc_mbox_channels[id];
 }
 
+/*
+ * PCC can be used with perf critical drivers such as CPPC
+ * So it makes sense to locally cache the virtual address and
+ * use it to read/write to PCC registers such as doorbell register
+ *
+ * The below read_register and write_registers are used to read and
+ * write from perf critical registers such as PCC doorbell register
+ */
+static int read_register(void __iomem *vaddr, u64 *val, unsigned int bit_width)
+{
+	int ret_val = 0;
+
+	switch (bit_width) {
+	case 8:
+		*val = readb(vaddr);
+		break;
+	case 16:
+		*val = readw(vaddr);
+		break;
+	case 32:
+		*val = readl(vaddr);
+		break;
+	case 64:
+		*val = readq(vaddr);
+		break;
+	default:
+		pr_debug("Error: Cannot read register of %u bit width",
+			bit_width);
+		ret_val = -EFAULT;
+		break;
+	}
+	return ret_val;
+}
+
+static int write_register(void __iomem *vaddr, u64 val, unsigned int bit_width)
+{
+	int ret_val = 0;
+
+	switch (bit_width) {
+	case 8:
+		writeb(val, vaddr);
+		break;
+	case 16:
+		writew(val, vaddr);
+		break;
+	case 32:
+		writel(val, vaddr);
+		break;
+	case 64:
+		writeq(val, vaddr);
+		break;
+	default:
+		pr_debug("Error: Cannot write register of %u bit width",
+			bit_width);
+		ret_val = -EFAULT;
+		break;
+	}
+	return ret_val;
+}
+
+/**
+ * pcc_map_interrupt - Map a PCC subspace GSI to a linux IRQ number
+ * @interrupt: GSI number.
+ * @flags: interrupt flags
+ *
+ * Returns: a valid linux IRQ number on success
+ *		0 or -EINVAL on failure
+ */
+static int pcc_map_interrupt(u32 interrupt, u32 flags)
+{
+	int trigger, polarity;
+
+	if (!interrupt)
+		return 0;
+
+	trigger = (flags & ACPI_PCCT_INTERRUPT_MODE) ? ACPI_EDGE_SENSITIVE
+			: ACPI_LEVEL_SENSITIVE;
+
+	polarity = (flags & ACPI_PCCT_INTERRUPT_POLARITY) ? ACPI_ACTIVE_LOW
+			: ACPI_ACTIVE_HIGH;
+
+	return acpi_register_gsi(NULL, interrupt, trigger, polarity);
+}
+
+/**
+ * pcc_mbox_irq - PCC mailbox interrupt handler
+ */
+static irqreturn_t pcc_mbox_irq(int irq, void *p)
+{
+	struct acpi_generic_address *doorbell_ack;
+	struct acpi_pcct_hw_reduced *pcct_ss;
+	struct mbox_chan *chan = p;
+	u64 doorbell_ack_preserve;
+	u64 doorbell_ack_write;
+	u64 doorbell_ack_val;
+	int ret;
+
+	pcct_ss = chan->con_priv;
+
+	mbox_chan_received_data(chan, NULL);
+
+	if (pcct_ss->header.type == ACPI_PCCT_TYPE_HW_REDUCED_SUBSPACE_TYPE2) {
+		struct acpi_pcct_hw_reduced_type2 *pcct2_ss = chan->con_priv;
+		u32 id = chan - pcc_mbox_channels;
+
+		doorbell_ack = &pcct2_ss->doorbell_ack_register;
+		doorbell_ack_preserve = pcct2_ss->ack_preserve_mask;
+		doorbell_ack_write = pcct2_ss->ack_write_mask;
+
+		ret = read_register(pcc_doorbell_ack_vaddr[id],
+				    &doorbell_ack_val,
+				    doorbell_ack->bit_width);
+		if (ret)
+			return IRQ_NONE;
+
+		ret = write_register(pcc_doorbell_ack_vaddr[id],
+				     (doorbell_ack_val & doorbell_ack_preserve)
+					| doorbell_ack_write,
+				     doorbell_ack->bit_width);
+		if (ret)
+			return IRQ_NONE;
+	}
+
+	return IRQ_HANDLED;
+}
+
 /**
  * pcc_mbox_request_channel - PCC clients call this function to
  *		request a pointer to their PCC subspace, from which they
@@ -135,6 +267,18 @@ struct mbox_chan *pcc_mbox_request_channel(struct mbox_client *cl,
 	if (chan->txdone_method == TXDONE_BY_POLL && cl->knows_txdone)
 		chan->txdone_method |= TXDONE_BY_ACK;
 
+	if (pcc_doorbell_irq[subspace_id] > 0) {
+		int rc;
+
+		rc = devm_request_irq(dev, pcc_doorbell_irq[subspace_id],
+				      pcc_mbox_irq, 0, MBOX_IRQ_NAME, chan);
+		if (unlikely(rc)) {
+			dev_err(dev, "failed to register PCC interrupt %d\n",
+				pcc_doorbell_irq[subspace_id]);
+			chan = ERR_PTR(rc);
+		}
+	}
+
 	spin_unlock_irqrestore(&chan->lock, flags);
 
 	return chan;
@@ -149,80 +293,30 @@ EXPORT_SYMBOL_GPL(pcc_mbox_request_channel);
  */
 void pcc_mbox_free_channel(struct mbox_chan *chan)
 {
+	u32 id = chan - pcc_mbox_channels;
 	unsigned long flags;
 
 	if (!chan || !chan->cl)
 		return;
 
+	if (id >= pcc_mbox_ctrl.num_chans) {
+		pr_debug("pcc_mbox_free_channel: Invalid mbox_chan passed\n");
+		return;
+	}
+
 	spin_lock_irqsave(&chan->lock, flags);
 	chan->cl = NULL;
 	chan->active_req = NULL;
 	if (chan->txdone_method == (TXDONE_BY_POLL | TXDONE_BY_ACK))
 		chan->txdone_method = TXDONE_BY_POLL;
 
+	if (pcc_doorbell_irq[id] > 0)
+		devm_free_irq(chan->mbox->dev, pcc_doorbell_irq[id], chan);
+
 	spin_unlock_irqrestore(&chan->lock, flags);
 }
 EXPORT_SYMBOL_GPL(pcc_mbox_free_channel);
 
-/*
- * PCC can be used with perf critical drivers such as CPPC
- * So it makes sense to locally cache the virtual address and
- * use it to read/write to PCC registers such as doorbell register
- *
- * The below read_register and write_registers are used to read and
- * write from perf critical registers such as PCC doorbell register
- */
-static int read_register(void __iomem *vaddr, u64 *val, unsigned int bit_width)
-{
-	int ret_val = 0;
-
-	switch (bit_width) {
-	case 8:
-		*val = readb(vaddr);
-		break;
-	case 16:
-		*val = readw(vaddr);
-		break;
-	case 32:
-		*val = readl(vaddr);
-		break;
-	case 64:
-		*val = readq(vaddr);
-		break;
-	default:
-		pr_debug("Error: Cannot read register of %u bit width",
-			bit_width);
-		ret_val = -EFAULT;
-		break;
-	}
-	return ret_val;
-}
-
-static int write_register(void __iomem *vaddr, u64 val, unsigned int bit_width)
-{
-	int ret_val = 0;
-
-	switch (bit_width) {
-	case 8:
-		writeb(val, vaddr);
-		break;
-	case 16:
-		writew(val, vaddr);
-		break;
-	case 32:
-		writel(val, vaddr);
-		break;
-	case 64:
-		writeq(val, vaddr);
-		break;
-	default:
-		pr_debug("Error: Cannot write register of %u bit width",
-			bit_width);
-		ret_val = -EFAULT;
-		break;
-	}
-	return ret_val;
-}
 
 /**
  * pcc_send_data - Called from Mailbox Controller code. Used
@@ -296,8 +390,10 @@ static int parse_pcc_subspace(struct acpi_subtable_header *header,
 	if (pcc_mbox_ctrl.num_chans <= MAX_PCC_SUBSPACES) {
 		pcct_ss = (struct acpi_pcct_hw_reduced *) header;
 
-		if (pcct_ss->header.type !=
-				ACPI_PCCT_TYPE_HW_REDUCED_SUBSPACE) {
+		if ((pcct_ss->header.type !=
+				ACPI_PCCT_TYPE_HW_REDUCED_SUBSPACE)
+		    && (pcct_ss->header.type !=
+				ACPI_PCCT_TYPE_HW_REDUCED_SUBSPACE_TYPE2)) {
 			pr_err("Incorrect PCC Subspace type detected\n");
 			return -EINVAL;
 		}
@@ -307,6 +403,43 @@ static int parse_pcc_subspace(struct acpi_subtable_header *header,
 }
 
 /**
+ * pcc_parse_subspace_irq - Parse the PCC IRQ and PCC ACK register
+ *		There should be one entry per PCC client.
+ * @id: PCC subspace index.
+ * @pcct_ss: Pointer to the ACPI subtable header under the PCCT.
+ *
+ * Return: 0 for Success, else errno.
+ *
+ * This gets called for each entry in the PCC table.
+ */
+static int pcc_parse_subspace_irq(int id,
+				  struct acpi_pcct_hw_reduced *pcct_ss)
+{
+	pcc_doorbell_irq[id] = pcc_map_interrupt(pcct_ss->doorbell_interrupt,
+						 (u32)pcct_ss->flags);
+	if (pcc_doorbell_irq[id] <= 0) {
+		pr_err("PCC GSI %d not registered\n",
+		       pcct_ss->doorbell_interrupt);
+		return -EINVAL;
+	}
+
+	if (pcct_ss->header.type
+		== ACPI_PCCT_TYPE_HW_REDUCED_SUBSPACE_TYPE2) {
+		struct acpi_pcct_hw_reduced_type2 *pcct2_ss = (void *)pcct_ss;
+
+		pcc_doorbell_ack_vaddr[id] = acpi_os_ioremap(
+				pcct2_ss->doorbell_ack_register.address,
+				pcct2_ss->doorbell_ack_register.bit_width / 8);
+		if (!pcc_doorbell_ack_vaddr[id]) {
+			pr_err("Failed to ioremap PCC ACK register\n");
+			return -ENOMEM;
+		}
+	}
+
+	return 0;
+}
+
+/**
  * acpi_pcc_probe - Parse the ACPI tree for the PCCT.
  *
  * Return: 0 for Success, else errno.
@@ -316,7 +449,9 @@ static int __init acpi_pcc_probe(void)
 	acpi_size pcct_tbl_header_size;
 	struct acpi_table_header *pcct_tbl;
 	struct acpi_subtable_header *pcct_entry;
-	int count, i;
+	struct acpi_table_pcct *acpi_pcct_tbl;
+	int count, i, rc;
+	int sum = 0;
 	acpi_status status = AE_OK;
 
 	/* Search for PCCT */
@@ -333,37 +468,66 @@ static int __init acpi_pcc_probe(void)
 			sizeof(struct acpi_table_pcct),
 			ACPI_PCCT_TYPE_HW_REDUCED_SUBSPACE,
 			parse_pcc_subspace, MAX_PCC_SUBSPACES);
+	sum += (count > 0) ? count : 0;
+
+	count = acpi_table_parse_entries(ACPI_SIG_PCCT,
+			sizeof(struct acpi_table_pcct),
+			ACPI_PCCT_TYPE_HW_REDUCED_SUBSPACE_TYPE2,
+			parse_pcc_subspace, MAX_PCC_SUBSPACES);
+	sum += (count > 0) ? count : 0;
 
-	if (count <= 0) {
+	if (sum == 0 || sum >= MAX_PCC_SUBSPACES) {
 		pr_err("Error parsing PCC subspaces from PCCT\n");
 		return -EINVAL;
 	}
 
 	pcc_mbox_channels = kzalloc(sizeof(struct mbox_chan) *
-			count, GFP_KERNEL);
-
+			sum, GFP_KERNEL);
 	if (!pcc_mbox_channels) {
 		pr_err("Could not allocate space for PCC mbox channels\n");
 		return -ENOMEM;
 	}
 
-	pcc_doorbell_vaddr = kcalloc(count, sizeof(void *), GFP_KERNEL);
+	pcc_doorbell_vaddr = kcalloc(sum, sizeof(void *), GFP_KERNEL);
 	if (!pcc_doorbell_vaddr) {
-		kfree(pcc_mbox_channels);
-		return -ENOMEM;
+		rc = -ENOMEM;
+		goto err_free_mbox;
+	}
+
+	pcc_doorbell_ack_vaddr = kcalloc(sum, sizeof(void *), GFP_KERNEL);
+	if (!pcc_doorbell_ack_vaddr) {
+		rc = -ENOMEM;
+		goto err_free_db_vaddr;
+	}
+
+	pcc_doorbell_irq = kcalloc(sum, sizeof(int), GFP_KERNEL);
+	if (!pcc_doorbell_irq) {
+		rc = -ENOMEM;
+		goto err_free_db_ack_vaddr;
 	}
 
 	/* Point to the first PCC subspace entry */
 	pcct_entry = (struct acpi_subtable_header *) (
 		(unsigned long) pcct_tbl + sizeof(struct acpi_table_pcct));
 
-	for (i = 0; i < count; i++) {
+	acpi_pcct_tbl = (struct acpi_table_pcct *) pcct_tbl;
+	if (acpi_pcct_tbl->flags & ACPI_PCCT_DOORBELL)
+		pcc_mbox_ctrl.txdone_irq = true;
+
+	for (i = 0; i < sum; i++) {
 		struct acpi_generic_address *db_reg;
 		struct acpi_pcct_hw_reduced *pcct_ss;
 		pcc_mbox_channels[i].con_priv = pcct_entry;
 
+		pcct_ss = (struct acpi_pcct_hw_reduced *) pcct_entry;
+
+		if (pcc_mbox_ctrl.txdone_irq) {
+			rc = pcc_parse_subspace_irq(i, pcct_ss);
+			if (rc < 0)
+				goto err;
+		}
+
 		/* If doorbell is in system memory cache the virt address */
-		pcct_ss = (struct acpi_pcct_hw_reduced *)pcct_entry;
 		db_reg = &pcct_ss->doorbell_register;
 		if (db_reg->space_id == ACPI_ADR_SPACE_SYSTEM_MEMORY)
 			pcc_doorbell_vaddr[i] = acpi_os_ioremap(db_reg->address,
@@ -372,11 +536,21 @@ static int __init acpi_pcc_probe(void)
 			((unsigned long) pcct_entry + pcct_entry->length);
 	}
 
-	pcc_mbox_ctrl.num_chans = count;
+	pcc_mbox_ctrl.num_chans = sum;
 
 	pr_info("Detected %d PCC Subspaces\n", pcc_mbox_ctrl.num_chans);
 
 	return 0;
+
+err:
+	kfree(pcc_doorbell_irq);
+err_free_db_ack_vaddr:
+	kfree(pcc_doorbell_ack_vaddr);
+err_free_db_vaddr:
+	kfree(pcc_doorbell_vaddr);
+err_free_mbox:
+	kfree(pcc_mbox_channels);
+	return rc;
 }
 
 /**
diff --git a/drivers/mailbox/platform_mhu.c b/drivers/mailbox/platform_mhu.c
new file mode 100644
index 000000000000..e13201a5cec6
--- /dev/null
+++ b/drivers/mailbox/platform_mhu.c
@@ -0,0 +1,205 @@
+/*
+ * Copyright (C) 2016 BayLibre SAS.
+ * Author: Neil Armstrong <narmstrong@baylibre.com>
+ * Synchronised with arm_mhu.c from :
+ * Copyright (C) 2013-2015 Fujitsu Semiconductor Ltd.
+ * Copyright (C) 2015 Linaro Ltd.
+ * Author: Jassi Brar <jaswinder.singh@linaro.org>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/interrupt.h>
+#include <linux/spinlock.h>
+#include <linux/mutex.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/mailbox_controller.h>
+
+#define INTR_SET_OFS	0x0
+#define INTR_STAT_OFS	0x4
+#define INTR_CLR_OFS	0x8
+
+#define MHU_SEC_OFFSET	0x0
+#define MHU_LP_OFFSET	0xc
+#define MHU_HP_OFFSET	0x18
+#define TX_REG_OFFSET	0x24
+
+#define MHU_CHANS	3
+
+struct platform_mhu_link {
+	int irq;
+	void __iomem *tx_reg;
+	void __iomem *rx_reg;
+};
+
+struct platform_mhu {
+	void __iomem *base;
+	struct platform_mhu_link mlink[MHU_CHANS];
+	struct mbox_chan chan[MHU_CHANS];
+	struct mbox_controller mbox;
+};
+
+static irqreturn_t platform_mhu_rx_interrupt(int irq, void *p)
+{
+	struct mbox_chan *chan = p;
+	struct platform_mhu_link *mlink = chan->con_priv;
+	u32 val;
+
+	val = readl_relaxed(mlink->rx_reg + INTR_STAT_OFS);
+	if (!val)
+		return IRQ_NONE;
+
+	mbox_chan_received_data(chan, (void *)&val);
+
+	writel_relaxed(val, mlink->rx_reg + INTR_CLR_OFS);
+
+	return IRQ_HANDLED;
+}
+
+static bool platform_mhu_last_tx_done(struct mbox_chan *chan)
+{
+	struct platform_mhu_link *mlink = chan->con_priv;
+	u32 val = readl_relaxed(mlink->tx_reg + INTR_STAT_OFS);
+
+	return (val == 0);
+}
+
+static int platform_mhu_send_data(struct mbox_chan *chan, void *data)
+{
+	struct platform_mhu_link *mlink = chan->con_priv;
+	u32 *arg = data;
+
+	writel_relaxed(*arg, mlink->tx_reg + INTR_SET_OFS);
+
+	return 0;
+}
+
+static int platform_mhu_startup(struct mbox_chan *chan)
+{
+	struct platform_mhu_link *mlink = chan->con_priv;
+	u32 val;
+	int ret;
+
+	val = readl_relaxed(mlink->tx_reg + INTR_STAT_OFS);
+	writel_relaxed(val, mlink->tx_reg + INTR_CLR_OFS);
+
+	ret = request_irq(mlink->irq, platform_mhu_rx_interrupt,
+			  IRQF_SHARED, "platform_mhu_link", chan);
+	if (ret) {
+		dev_err(chan->mbox->dev,
+			"Unable to acquire IRQ %d\n", mlink->irq);
+		return ret;
+	}
+
+	return 0;
+}
+
+static void platform_mhu_shutdown(struct mbox_chan *chan)
+{
+	struct platform_mhu_link *mlink = chan->con_priv;
+
+	free_irq(mlink->irq, chan);
+}
+
+static const struct mbox_chan_ops platform_mhu_ops = {
+	.send_data = platform_mhu_send_data,
+	.startup = platform_mhu_startup,
+	.shutdown = platform_mhu_shutdown,
+	.last_tx_done = platform_mhu_last_tx_done,
+};
+
+static int platform_mhu_probe(struct platform_device *pdev)
+{
+	int i, err;
+	struct platform_mhu *mhu;
+	struct device *dev = &pdev->dev;
+	struct resource *res;
+	int platform_mhu_reg[MHU_CHANS] = {
+		MHU_SEC_OFFSET, MHU_LP_OFFSET, MHU_HP_OFFSET
+	};
+
+	/* Allocate memory for device */
+	mhu = devm_kzalloc(dev, sizeof(*mhu), GFP_KERNEL);
+	if (!mhu)
+		return -ENOMEM;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	mhu->base = devm_ioremap_resource(dev, res);
+	if (IS_ERR(mhu->base)) {
+		dev_err(dev, "ioremap failed\n");
+		return PTR_ERR(mhu->base);
+	}
+
+	for (i = 0; i < MHU_CHANS; i++) {
+		mhu->chan[i].con_priv = &mhu->mlink[i];
+		mhu->mlink[i].irq = platform_get_irq(pdev, i);
+		if (mhu->mlink[i].irq < 0) {
+			dev_err(dev, "failed to get irq%d\n", i);
+			return mhu->mlink[i].irq;
+		}
+		mhu->mlink[i].rx_reg = mhu->base + platform_mhu_reg[i];
+		mhu->mlink[i].tx_reg = mhu->mlink[i].rx_reg + TX_REG_OFFSET;
+	}
+
+	mhu->mbox.dev = dev;
+	mhu->mbox.chans = &mhu->chan[0];
+	mhu->mbox.num_chans = MHU_CHANS;
+	mhu->mbox.ops = &platform_mhu_ops;
+	mhu->mbox.txdone_irq = false;
+	mhu->mbox.txdone_poll = true;
+	mhu->mbox.txpoll_period = 1;
+
+	platform_set_drvdata(pdev, mhu);
+
+	err = mbox_controller_register(&mhu->mbox);
+	if (err) {
+		dev_err(dev, "Failed to register mailboxes %d\n", err);
+		return err;
+	}
+
+	dev_info(dev, "Platform MHU Mailbox registered\n");
+	return 0;
+}
+
+static int platform_mhu_remove(struct platform_device *pdev)
+{
+	struct platform_mhu *mhu = platform_get_drvdata(pdev);
+
+	mbox_controller_unregister(&mhu->mbox);
+
+	return 0;
+}
+
+static const struct of_device_id platform_mhu_dt_ids[] = {
+	{ .compatible = "amlogic,meson-gxbb-mhu", },
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, platform_mhu_dt_ids);
+
+static struct platform_driver platform_mhu_driver = {
+	.probe	= platform_mhu_probe,
+	.remove	= platform_mhu_remove,
+	.driver = {
+		.name = "platform-mhu",
+		.of_match_table	= platform_mhu_dt_ids,
+	},
+};
+
+module_platform_driver(platform_mhu_driver);
+
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:platform-mhu");
+MODULE_DESCRIPTION("Platform MHU Driver");
+MODULE_AUTHOR("Neil Armstrong <narmstrong@baylibre.com>");
diff --git a/drivers/mcb/Kconfig b/drivers/mcb/Kconfig
index e9a6976e1010..76d9c51de6c9 100644
--- a/drivers/mcb/Kconfig
+++ b/drivers/mcb/Kconfig
@@ -28,4 +28,13 @@ config MCB_PCI
 
 	   If build as a module, the module is called mcb-pci.ko
 
+config MCB_LPC
+	   tristate "LPC (non PCI) based MCB carrier"
+	   default n
+	   help
+
+	   This is a MCB carrier on a LPC or non PCI device.
+
+	   If build as a module, the module is called mcb-lpc.ko
+
 endif # MCB
diff --git a/drivers/mcb/Makefile b/drivers/mcb/Makefile
index 1ae141311def..bcc7745774ab 100644
--- a/drivers/mcb/Makefile
+++ b/drivers/mcb/Makefile
@@ -5,3 +5,4 @@ mcb-y += mcb-core.o
 mcb-y += mcb-parse.o
 
 obj-$(CONFIG_MCB_PCI) += mcb-pci.o
+obj-$(CONFIG_MCB_LPC) += mcb-lpc.o
diff --git a/drivers/mcb/mcb-core.c b/drivers/mcb/mcb-core.c
index 6f2c8522e14a..921a5d2a802b 100644
--- a/drivers/mcb/mcb-core.c
+++ b/drivers/mcb/mcb-core.c
@@ -233,6 +233,7 @@ int mcb_device_register(struct mcb_bus *bus, struct mcb_device *dev)
 	dev->dev.bus = &mcb_bus_type;
 	dev->dev.parent = bus->dev.parent;
 	dev->dev.release = mcb_release_dev;
+	dev->dma_dev = bus->carrier;
 
 	device_id = dev->id;
 	dev_set_name(&dev->dev, "mcb%d-16z%03d-%d:%d:%d",
@@ -369,7 +370,6 @@ struct mcb_device *mcb_alloc_dev(struct mcb_bus *bus)
 	if (!dev)
 		return NULL;
 
-	INIT_LIST_HEAD(&dev->bus_list);
 	dev->bus = bus;
 
 	return dev;
@@ -405,20 +405,6 @@ static int __mcb_bus_add_devices(struct device *dev, void *data)
 	return 0;
 }
 
-static int __mcb_bus_add_child(struct device *dev, void *data)
-{
-	struct mcb_device *mdev = to_mcb_device(dev);
-	struct mcb_bus *child;
-
-	BUG_ON(!mdev->is_added);
-	child = mdev->subordinate;
-
-	if (child)
-		mcb_bus_add_devices(child);
-
-	return 0;
-}
-
 /**
  * mcb_bus_add_devices() - Add devices in the bus' internal device list
  * @bus: The @mcb_bus we add the devices
@@ -428,8 +414,6 @@ static int __mcb_bus_add_child(struct device *dev, void *data)
 void mcb_bus_add_devices(const struct mcb_bus *bus)
 {
 	bus_for_each_dev(&mcb_bus_type, NULL, NULL, __mcb_bus_add_devices);
-	bus_for_each_dev(&mcb_bus_type, NULL, NULL, __mcb_bus_add_child);
-
 }
 EXPORT_SYMBOL_GPL(mcb_bus_add_devices);
 
diff --git a/drivers/mcb/mcb-internal.h b/drivers/mcb/mcb-internal.h
index 5254e0285725..d6e6933b19f1 100644
--- a/drivers/mcb/mcb-internal.h
+++ b/drivers/mcb/mcb-internal.h
@@ -112,6 +112,15 @@ struct chameleon_bdd {
 	u32 size;
 } __packed;
 
+struct chameleon_bar {
+	u32 addr;
+	u32 size;
+};
+
+#define BAR_CNT(x) ((x) & 0x07)
+#define CHAMELEON_BAR_MAX	6
+#define BAR_DESC_SIZE(x)	((x) * sizeof(struct chameleon_bar) + sizeof(__le32))
+
 int chameleon_parse_cells(struct mcb_bus *bus, phys_addr_t mapbase,
 			  void __iomem *base);
 
diff --git a/drivers/mcb/mcb-lpc.c b/drivers/mcb/mcb-lpc.c
new file mode 100644
index 000000000000..d072c088ce73
--- /dev/null
+++ b/drivers/mcb/mcb-lpc.c
@@ -0,0 +1,158 @@
+/*
+ * MEN Chameleon Bus.
+ *
+ * Copyright (C) 2014 MEN Mikroelektronik GmbH (www.men.de)
+ * Author: Andreas Werner <andreas.werner@men.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 of the License.
+ */
+
+#include <linux/platform_device.h>
+#include <linux/module.h>
+#include <linux/dmi.h>
+#include <linux/mcb.h>
+#include <linux/io.h>
+#include "mcb-internal.h"
+
+struct priv {
+	struct mcb_bus *bus;
+	struct resource *mem;
+	void __iomem *base;
+};
+
+static int mcb_lpc_probe(struct platform_device *pdev)
+{
+	struct resource *res;
+	struct priv *priv;
+	int ret = 0;
+
+	priv = devm_kzalloc(&pdev->dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!priv->mem) {
+		dev_err(&pdev->dev, "No Memory resource\n");
+		return -ENODEV;
+	}
+
+	res = devm_request_mem_region(&pdev->dev, priv->mem->start,
+				      resource_size(priv->mem),
+				      KBUILD_MODNAME);
+	if (!res) {
+		dev_err(&pdev->dev, "Failed to request IO memory\n");
+		return -EBUSY;
+	}
+
+	priv->base = devm_ioremap(&pdev->dev, priv->mem->start,
+				  resource_size(priv->mem));
+	if (!priv->base) {
+		dev_err(&pdev->dev, "Cannot ioremap\n");
+		return -ENOMEM;
+	}
+
+	platform_set_drvdata(pdev, priv);
+
+	priv->bus = mcb_alloc_bus(&pdev->dev);
+	if (IS_ERR(priv->bus))
+		return PTR_ERR(priv->bus);
+
+	ret = chameleon_parse_cells(priv->bus, priv->mem->start, priv->base);
+	if (ret < 0) {
+		mcb_release_bus(priv->bus);
+		return ret;
+	}
+
+	dev_dbg(&pdev->dev, "Found %d cells\n", ret);
+
+	mcb_bus_add_devices(priv->bus);
+
+	return 0;
+
+}
+
+static int mcb_lpc_remove(struct platform_device *pdev)
+{
+	struct priv *priv = platform_get_drvdata(pdev);
+
+	mcb_release_bus(priv->bus);
+
+	return 0;
+}
+
+static struct platform_device *mcb_lpc_pdev;
+
+static int mcb_lpc_create_platform_device(const struct dmi_system_id *id)
+{
+	struct resource *res = id->driver_data;
+	int ret;
+
+	mcb_lpc_pdev = platform_device_alloc("mcb-lpc", -1);
+	if (!mcb_lpc_pdev)
+		return -ENOMEM;
+
+	ret = platform_device_add_resources(mcb_lpc_pdev, res, 1);
+	if (ret)
+		goto out_put;
+
+	ret = platform_device_add(mcb_lpc_pdev);
+	if (ret)
+		goto out_put;
+
+	return 0;
+
+out_put:
+	platform_device_put(mcb_lpc_pdev);
+	return ret;
+}
+
+static struct resource sc24_fpga_resource = {
+	.start = 0xe000e000,
+	.end = 0xe000e000 + CHAM_HEADER_SIZE,
+	.flags = IORESOURCE_MEM,
+};
+
+static struct platform_driver mcb_lpc_driver = {
+	.driver		= {
+		.name = "mcb-lpc",
+	},
+	.probe		= mcb_lpc_probe,
+	.remove		= mcb_lpc_remove,
+};
+
+static const struct dmi_system_id mcb_lpc_dmi_table[] = {
+	{
+		.ident = "SC24",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "MEN"),
+			DMI_MATCH(DMI_PRODUCT_VERSION, "14SC24"),
+		},
+		.driver_data = (void *)&sc24_fpga_resource,
+		.callback = mcb_lpc_create_platform_device,
+	},
+	{}
+};
+MODULE_DEVICE_TABLE(dmi, mcb_lpc_dmi_table);
+
+static int __init mcb_lpc_init(void)
+{
+	if (!dmi_check_system(mcb_lpc_dmi_table))
+		return -ENODEV;
+
+	return platform_driver_register(&mcb_lpc_driver);
+}
+
+static void __exit mcb_lpc_exit(void)
+{
+	platform_device_unregister(mcb_lpc_pdev);
+	platform_driver_unregister(&mcb_lpc_driver);
+}
+
+module_init(mcb_lpc_init);
+module_exit(mcb_lpc_exit);
+
+MODULE_AUTHOR("Andreas Werner <andreas.werner@men.de>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("MCB over LPC support");
diff --git a/drivers/mcb/mcb-parse.c b/drivers/mcb/mcb-parse.c
index dbecbed0d258..4ca2739b4fad 100644
--- a/drivers/mcb/mcb-parse.c
+++ b/drivers/mcb/mcb-parse.c
@@ -26,19 +26,20 @@ static inline uint32_t get_next_dtype(void __iomem *p)
 }
 
 static int chameleon_parse_bdd(struct mcb_bus *bus,
-			phys_addr_t mapbase,
+			struct chameleon_bar *cb,
 			void __iomem *base)
 {
 	return 0;
 }
 
 static int chameleon_parse_gdd(struct mcb_bus *bus,
-			phys_addr_t mapbase,
-			void __iomem *base)
+			struct chameleon_bar *cb,
+			void __iomem *base, int bar_count)
 {
 	struct chameleon_gdd __iomem *gdd =
 		(struct chameleon_gdd __iomem *) base;
 	struct mcb_device *mdev;
+	u32 dev_mapbase;
 	u32 offset;
 	u32 size;
 	int ret;
@@ -61,13 +62,39 @@ static int chameleon_parse_gdd(struct mcb_bus *bus,
 	mdev->group = GDD_GRP(reg2);
 	mdev->inst = GDD_INS(reg2);
 
+	/*
+	 * If the BAR is missing, dev_mapbase is zero, or if the
+	 * device is IO mapped we just print a warning and go on with the
+	 * next device, instead of completely stop the gdd parser
+	 */
+	if (mdev->bar > bar_count - 1) {
+		pr_info("No BAR for 16z%03d\n", mdev->id);
+		ret = 0;
+		goto err;
+	}
+
+	dev_mapbase = cb[mdev->bar].addr;
+	if (!dev_mapbase) {
+		pr_info("BAR not assigned for 16z%03d\n", mdev->id);
+		ret = 0;
+		goto err;
+	}
+
+	if (dev_mapbase & 0x01) {
+		pr_info("IO mapped Device (16z%03d) not yet supported\n",
+			mdev->id);
+		ret = 0;
+		goto err;
+	}
+
 	pr_debug("Found a 16z%03d\n", mdev->id);
 
 	mdev->irq.start = GDD_IRQ(reg1);
 	mdev->irq.end = GDD_IRQ(reg1);
 	mdev->irq.flags = IORESOURCE_IRQ;
 
-	mdev->mem.start = mapbase + offset;
+	mdev->mem.start = dev_mapbase + offset;
+
 	mdev->mem.end = mdev->mem.start + size - 1;
 	mdev->mem.flags = IORESOURCE_MEM;
 
@@ -85,13 +112,76 @@ err:
 	return ret;
 }
 
+static void chameleon_parse_bar(void __iomem *base,
+				struct chameleon_bar *cb, int bar_count)
+{
+	char __iomem *p = base;
+	int i;
+
+	/* skip reg1 */
+	p += sizeof(__le32);
+
+	for (i = 0; i < bar_count; i++) {
+		cb[i].addr = readl(p);
+		cb[i].size = readl(p + 4);
+
+		p += sizeof(struct chameleon_bar);
+	}
+}
+
+static int chameleon_get_bar(char __iomem **base, phys_addr_t mapbase,
+			     struct chameleon_bar **cb)
+{
+	struct chameleon_bar *c;
+	int bar_count;
+	__le32 reg;
+	u32 dtype;
+
+	/*
+	 * For those devices which are not connected
+	 * to the PCI Bus (e.g. LPC) there is a bar
+	 * descriptor located directly after the
+	 * chameleon header. This header is comparable
+	 * to a PCI header.
+	 */
+	dtype = get_next_dtype(*base);
+	if (dtype == CHAMELEON_DTYPE_BAR) {
+		reg = readl(*base);
+
+		bar_count = BAR_CNT(reg);
+		if (bar_count <= 0 && bar_count > CHAMELEON_BAR_MAX)
+			return -ENODEV;
+
+		c = kcalloc(bar_count, sizeof(struct chameleon_bar),
+			    GFP_KERNEL);
+		if (!c)
+			return -ENOMEM;
+
+		chameleon_parse_bar(*base, c, bar_count);
+		*base += BAR_DESC_SIZE(bar_count);
+	} else {
+		c = kzalloc(sizeof(struct chameleon_bar), GFP_KERNEL);
+		if (!c)
+			return -ENOMEM;
+
+		bar_count = 1;
+		c->addr = mapbase;
+	}
+
+	*cb = c;
+
+	return bar_count;
+}
+
 int chameleon_parse_cells(struct mcb_bus *bus, phys_addr_t mapbase,
 			void __iomem *base)
 {
-	char __iomem *p = base;
 	struct chameleon_fpga_header *header;
-	uint32_t dtype;
+	struct chameleon_bar *cb;
+	char __iomem *p = base;
 	int num_cells = 0;
+	uint32_t dtype;
+	int bar_count;
 	int ret = 0;
 	u32 hsize;
 
@@ -108,8 +198,8 @@ int chameleon_parse_cells(struct mcb_bus *bus, phys_addr_t mapbase,
 	if (header->magic != CHAMELEONV2_MAGIC) {
 		pr_err("Unsupported chameleon version 0x%x\n",
 				header->magic);
-		kfree(header);
-		return -ENODEV;
+		ret = -ENODEV;
+		goto free_header;
 	}
 	p += hsize;
 
@@ -119,16 +209,20 @@ int chameleon_parse_cells(struct mcb_bus *bus, phys_addr_t mapbase,
 	snprintf(bus->name, CHAMELEON_FILENAME_LEN + 1, "%s",
 		 header->filename);
 
+	bar_count = chameleon_get_bar(&p, mapbase, &cb);
+	if (bar_count < 0)
+		goto free_header;
+
 	for_each_chameleon_cell(dtype, p) {
 		switch (dtype) {
 		case CHAMELEON_DTYPE_GENERAL:
-			ret = chameleon_parse_gdd(bus, mapbase, p);
+			ret = chameleon_parse_gdd(bus, cb, p, bar_count);
 			if (ret < 0)
-				goto out;
+				goto free_bar;
 			p += sizeof(struct chameleon_gdd);
 			break;
 		case CHAMELEON_DTYPE_BRIDGE:
-			chameleon_parse_bdd(bus, mapbase, p);
+			chameleon_parse_bdd(bus, cb, p);
 			p += sizeof(struct chameleon_bdd);
 			break;
 		case CHAMELEON_DTYPE_END:
@@ -136,8 +230,8 @@ int chameleon_parse_cells(struct mcb_bus *bus, phys_addr_t mapbase,
 		default:
 			pr_err("Invalid chameleon descriptor type 0x%x\n",
 				dtype);
-			kfree(header);
-			return -EINVAL;
+			ret = -EINVAL;
+			goto free_bar;
 		}
 		num_cells++;
 	}
@@ -145,11 +239,15 @@ int chameleon_parse_cells(struct mcb_bus *bus, phys_addr_t mapbase,
 	if (num_cells == 0)
 		num_cells = -EINVAL;
 
+	kfree(cb);
 	kfree(header);
 	return num_cells;
 
-out:
+free_bar:
+	kfree(cb);
+free_header:
 	kfree(header);
+
 	return ret;
 }
 EXPORT_SYMBOL_GPL(chameleon_parse_cells);
diff --git a/drivers/mcb/mcb-pci.c b/drivers/mcb/mcb-pci.c
index b15a0349cd97..af4d2f26f1c6 100644
--- a/drivers/mcb/mcb-pci.c
+++ b/drivers/mcb/mcb-pci.c
@@ -46,6 +46,7 @@ static int mcb_pci_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 		dev_err(&pdev->dev, "Failed to enable PCI device\n");
 		return -ENODEV;
 	}
+	pci_set_master(pdev);
 
 	priv->mapbase = pci_resource_start(pdev, 0);
 	if (!priv->mapbase) {
diff --git a/drivers/md/bcache/btree.c b/drivers/md/bcache/btree.c
index 76f7534d1dd1..81d3db40cd7b 100644
--- a/drivers/md/bcache/btree.c
+++ b/drivers/md/bcache/btree.c
@@ -361,12 +361,8 @@ static void __btree_node_write_done(struct closure *cl)
 static void btree_node_write_done(struct closure *cl)
 {
 	struct btree *b = container_of(cl, struct btree, io);
-	struct bio_vec *bv;
-	int n;
-
-	bio_for_each_segment_all(bv, b->bio, n)
-		__free_page(bv->bv_page);
 
+	bio_free_pages(b->bio);
 	__btree_node_write_done(cl);
 }
 
diff --git a/drivers/md/bcache/debug.c b/drivers/md/bcache/debug.c
index c28df164701e..333a1e5f6ae6 100644
--- a/drivers/md/bcache/debug.c
+++ b/drivers/md/bcache/debug.c
@@ -107,9 +107,8 @@ void bch_data_verify(struct cached_dev *dc, struct bio *bio)
 {
 	char name[BDEVNAME_SIZE];
 	struct bio *check;
-	struct bio_vec bv, *bv2;
+	struct bio_vec bv;
 	struct bvec_iter iter;
-	int i;
 
 	check = bio_clone(bio, GFP_NOIO);
 	if (!check)
@@ -136,8 +135,7 @@ void bch_data_verify(struct cached_dev *dc, struct bio *bio)
 		kunmap_atomic(p1);
 	}
 
-	bio_for_each_segment_all(bv2, check, i)
-		__free_page(bv2->bv_page);
+	bio_free_pages(check);
 out_put:
 	bio_put(check);
 }
diff --git a/drivers/md/bcache/movinggc.c b/drivers/md/bcache/movinggc.c
index 1881319f2298..5c4bddecfaf0 100644
--- a/drivers/md/bcache/movinggc.c
+++ b/drivers/md/bcache/movinggc.c
@@ -44,11 +44,8 @@ static void write_moving_finish(struct closure *cl)
 {
 	struct moving_io *io = container_of(cl, struct moving_io, cl);
 	struct bio *bio = &io->bio.bio;
-	struct bio_vec *bv;
-	int i;
 
-	bio_for_each_segment_all(bv, bio, i)
-		__free_page(bv->bv_page);
+	bio_free_pages(bio);
 
 	if (io->op.replace_collision)
 		trace_bcache_gc_copy_collision(&io->w->key);
diff --git a/drivers/md/bcache/request.c b/drivers/md/bcache/request.c
index 4b177fe11ebb..40ffe5e424b3 100644
--- a/drivers/md/bcache/request.c
+++ b/drivers/md/bcache/request.c
@@ -694,13 +694,8 @@ static void cached_dev_cache_miss_done(struct closure *cl)
 	if (s->iop.replace_collision)
 		bch_mark_cache_miss_collision(s->iop.c, s->d);
 
-	if (s->iop.bio) {
-		int i;
-		struct bio_vec *bv;
-
-		bio_for_each_segment_all(bv, s->iop.bio, i)
-			__free_page(bv->bv_page);
-	}
+	if (s->iop.bio)
+		bio_free_pages(s->iop.bio);
 
 	cached_dev_bio_complete(cl);
 }
diff --git a/drivers/md/bcache/writeback.c b/drivers/md/bcache/writeback.c
index d9fd2a62e5f6..e51644e503a5 100644
--- a/drivers/md/bcache/writeback.c
+++ b/drivers/md/bcache/writeback.c
@@ -128,11 +128,8 @@ static void write_dirty_finish(struct closure *cl)
 	struct dirty_io *io = container_of(cl, struct dirty_io, cl);
 	struct keybuf_key *w = io->bio.bi_private;
 	struct cached_dev *dc = io->dc;
-	struct bio_vec *bv;
-	int i;
 
-	bio_for_each_segment_all(bv, &io->bio, i)
-		__free_page(bv->bv_page);
+	bio_free_pages(&io->bio);
 
 	/* This is kind of a dumb way of signalling errors. */
 	if (KEY_DIRTY(&w->key)) {
diff --git a/drivers/md/bitmap.c b/drivers/md/bitmap.c
index 13041ee37ad6..2d826927a3bf 100644
--- a/drivers/md/bitmap.c
+++ b/drivers/md/bitmap.c
@@ -1903,10 +1903,8 @@ int bitmap_copy_from_slot(struct mddev *mddev, int slot,
 	struct bitmap_counts *counts;
 	struct bitmap *bitmap = bitmap_create(mddev, slot);
 
-	if (IS_ERR(bitmap)) {
-		bitmap_free(bitmap);
+	if (IS_ERR(bitmap))
 		return PTR_ERR(bitmap);
-	}
 
 	rv = bitmap_init_from_disk(bitmap, 0);
 	if (rv)
diff --git a/drivers/md/dm-bufio.c b/drivers/md/dm-bufio.c
index 8625040bae92..125aedc3875f 100644
--- a/drivers/md/dm-bufio.c
+++ b/drivers/md/dm-bufio.c
@@ -191,19 +191,6 @@ static void dm_bufio_unlock(struct dm_bufio_client *c)
 	mutex_unlock(&c->lock);
 }
 
-/*
- * FIXME Move to sched.h?
- */
-#ifdef CONFIG_PREEMPT_VOLUNTARY
-#  define dm_bufio_cond_resched()		\
-do {						\
-	if (unlikely(need_resched()))		\
-		_cond_resched();		\
-} while (0)
-#else
-#  define dm_bufio_cond_resched()                do { } while (0)
-#endif
-
 /*----------------------------------------------------------------*/
 
 /*
@@ -741,7 +728,7 @@ static void __flush_write_list(struct list_head *write_list)
 			list_entry(write_list->next, struct dm_buffer, write_list);
 		list_del(&b->write_list);
 		submit_io(b, WRITE, b->block, write_endio);
-		dm_bufio_cond_resched();
+		cond_resched();
 	}
 	blk_finish_plug(&plug);
 }
@@ -780,7 +767,7 @@ static struct dm_buffer *__get_unclaimed_buffer(struct dm_bufio_client *c)
 			__unlink_buffer(b);
 			return b;
 		}
-		dm_bufio_cond_resched();
+		cond_resched();
 	}
 
 	list_for_each_entry_reverse(b, &c->lru[LIST_DIRTY], lru_list) {
@@ -791,7 +778,7 @@ static struct dm_buffer *__get_unclaimed_buffer(struct dm_bufio_client *c)
 			__unlink_buffer(b);
 			return b;
 		}
-		dm_bufio_cond_resched();
+		cond_resched();
 	}
 
 	return NULL;
@@ -923,7 +910,7 @@ static void __write_dirty_buffers_async(struct dm_bufio_client *c, int no_wait,
 			return;
 
 		__write_dirty_buffer(b, write_list);
-		dm_bufio_cond_resched();
+		cond_resched();
 	}
 }
 
@@ -973,7 +960,7 @@ static void __check_watermark(struct dm_bufio_client *c,
 			return;
 
 		__free_buffer_wake(b);
-		dm_bufio_cond_resched();
+		cond_resched();
 	}
 
 	if (c->n_buffers[LIST_DIRTY] > threshold_buffers)
@@ -1170,7 +1157,7 @@ void dm_bufio_prefetch(struct dm_bufio_client *c,
 				submit_io(b, READ, b->block, read_endio);
 			dm_bufio_release(b);
 
-			dm_bufio_cond_resched();
+			cond_resched();
 
 			if (!n_blocks)
 				goto flush_plug;
@@ -1291,7 +1278,7 @@ again:
 		    !test_bit(B_WRITING, &b->state))
 			__relink_lru(b, LIST_CLEAN);
 
-		dm_bufio_cond_resched();
+		cond_resched();
 
 		/*
 		 * If we dropped the lock, the list is no longer consistent,
@@ -1574,7 +1561,7 @@ static unsigned long __scan(struct dm_bufio_client *c, unsigned long nr_to_scan,
 				freed++;
 			if (!--nr_to_scan || ((count - freed) <= retain_target))
 				return freed;
-			dm_bufio_cond_resched();
+			cond_resched();
 		}
 	}
 	return freed;
@@ -1808,7 +1795,7 @@ static void __evict_old_buffers(struct dm_bufio_client *c, unsigned long age_hz)
 		if (__try_evict_buffer(b, 0))
 			count--;
 
-		dm_bufio_cond_resched();
+		cond_resched();
 	}
 
 	dm_bufio_unlock(c);
diff --git a/drivers/md/dm-cache-metadata.c b/drivers/md/dm-cache-metadata.c
index 3970cda10080..695577812cf6 100644
--- a/drivers/md/dm-cache-metadata.c
+++ b/drivers/md/dm-cache-metadata.c
@@ -140,6 +140,13 @@ struct dm_cache_metadata {
 	 * the device.
 	 */
 	bool fail_io:1;
+
+	/*
+	 * These structures are used when loading metadata.  They're too
+	 * big to put on the stack.
+	 */
+	struct dm_array_cursor mapping_cursor;
+	struct dm_array_cursor hint_cursor;
 };
 
 /*-------------------------------------------------------------------
@@ -1171,31 +1178,37 @@ static bool hints_array_available(struct dm_cache_metadata *cmd,
 		hints_array_initialized(cmd);
 }
 
-static int __load_mapping(void *context, uint64_t cblock, void *leaf)
+static int __load_mapping(struct dm_cache_metadata *cmd,
+			  uint64_t cb, bool hints_valid,
+			  struct dm_array_cursor *mapping_cursor,
+			  struct dm_array_cursor *hint_cursor,
+			  load_mapping_fn fn, void *context)
 {
 	int r = 0;
-	bool dirty;
-	__le64 value;
-	__le32 hint_value = 0;
+
+	__le64 mapping;
+	__le32 hint = 0;
+
+	__le64 *mapping_value_le;
+	__le32 *hint_value_le;
+
 	dm_oblock_t oblock;
 	unsigned flags;
-	struct thunk *thunk = context;
-	struct dm_cache_metadata *cmd = thunk->cmd;
 
-	memcpy(&value, leaf, sizeof(value));
-	unpack_value(value, &oblock, &flags);
+	dm_array_cursor_get_value(mapping_cursor, (void **) &mapping_value_le);
+	memcpy(&mapping, mapping_value_le, sizeof(mapping));
+	unpack_value(mapping, &oblock, &flags);
 
 	if (flags & M_VALID) {
-		if (thunk->hints_valid) {
-			r = dm_array_get_value(&cmd->hint_info, cmd->hint_root,
-					       cblock, &hint_value);
-			if (r && r != -ENODATA)
-				return r;
+		if (hints_valid) {
+			dm_array_cursor_get_value(hint_cursor, (void **) &hint_value_le);
+			memcpy(&hint, hint_value_le, sizeof(hint));
 		}
 
-		dirty = thunk->respect_dirty_flags ? (flags & M_DIRTY) : true;
-		r = thunk->fn(thunk->context, oblock, to_cblock(cblock),
-			      dirty, le32_to_cpu(hint_value), thunk->hints_valid);
+		r = fn(context, oblock, to_cblock(cb), flags & M_DIRTY,
+		       le32_to_cpu(hint), hints_valid);
+		if (r)
+			DMERR("policy couldn't load cblock");
 	}
 
 	return r;
@@ -1205,16 +1218,60 @@ static int __load_mappings(struct dm_cache_metadata *cmd,
 			   struct dm_cache_policy *policy,
 			   load_mapping_fn fn, void *context)
 {
-	struct thunk thunk;
+	int r;
+	uint64_t cb;
+
+	bool hints_valid = hints_array_available(cmd, policy);
+
+	if (from_cblock(cmd->cache_blocks) == 0)
+		/* Nothing to do */
+		return 0;
+
+	r = dm_array_cursor_begin(&cmd->info, cmd->root, &cmd->mapping_cursor);
+	if (r)
+		return r;
 
-	thunk.fn = fn;
-	thunk.context = context;
+	if (hints_valid) {
+		r = dm_array_cursor_begin(&cmd->hint_info, cmd->hint_root, &cmd->hint_cursor);
+		if (r) {
+			dm_array_cursor_end(&cmd->mapping_cursor);
+			return r;
+		}
+	}
+
+	for (cb = 0; ; cb++) {
+		r = __load_mapping(cmd, cb, hints_valid,
+				   &cmd->mapping_cursor, &cmd->hint_cursor,
+				   fn, context);
+		if (r)
+			goto out;
+
+		/*
+		 * We need to break out before we move the cursors.
+		 */
+		if (cb >= (from_cblock(cmd->cache_blocks) - 1))
+			break;
 
-	thunk.cmd = cmd;
-	thunk.respect_dirty_flags = cmd->clean_when_opened;
-	thunk.hints_valid = hints_array_available(cmd, policy);
+		r = dm_array_cursor_next(&cmd->mapping_cursor);
+		if (r) {
+			DMERR("dm_array_cursor_next for mapping failed");
+			goto out;
+		}
 
-	return dm_array_walk(&cmd->info, cmd->root, __load_mapping, &thunk);
+		if (hints_valid) {
+			r = dm_array_cursor_next(&cmd->hint_cursor);
+			if (r) {
+				DMERR("dm_array_cursor_next for hint failed");
+				goto out;
+			}
+		}
+	}
+out:
+	dm_array_cursor_end(&cmd->mapping_cursor);
+	if (hints_valid)
+		dm_array_cursor_end(&cmd->hint_cursor);
+
+	return r;
 }
 
 int dm_cache_load_mappings(struct dm_cache_metadata *cmd,
@@ -1368,10 +1425,24 @@ int dm_cache_get_metadata_dev_size(struct dm_cache_metadata *cmd,
 
 /*----------------------------------------------------------------*/
 
-static int begin_hints(struct dm_cache_metadata *cmd, struct dm_cache_policy *policy)
+static int get_hint(uint32_t index, void *value_le, void *context)
+{
+	uint32_t value;
+	struct dm_cache_policy *policy = context;
+
+	value = policy_get_hint(policy, to_cblock(index));
+	*((__le32 *) value_le) = cpu_to_le32(value);
+
+	return 0;
+}
+
+/*
+ * It's quicker to always delete the hint array, and recreate with
+ * dm_array_new().
+ */
+static int write_hints(struct dm_cache_metadata *cmd, struct dm_cache_policy *policy)
 {
 	int r;
-	__le32 value;
 	size_t hint_size;
 	const char *policy_name = dm_cache_policy_get_name(policy);
 	const unsigned *policy_version = dm_cache_policy_get_version(policy);
@@ -1380,63 +1451,23 @@ static int begin_hints(struct dm_cache_metadata *cmd, struct dm_cache_policy *po
 	    (strlen(policy_name) > sizeof(cmd->policy_name) - 1))
 		return -EINVAL;
 
-	if (!policy_unchanged(cmd, policy)) {
-		strncpy(cmd->policy_name, policy_name, sizeof(cmd->policy_name));
-		memcpy(cmd->policy_version, policy_version, sizeof(cmd->policy_version));
-
-		hint_size = dm_cache_policy_get_hint_size(policy);
-		if (!hint_size)
-			return 0; /* short-circuit hints initialization */
-		cmd->policy_hint_size = hint_size;
+	strncpy(cmd->policy_name, policy_name, sizeof(cmd->policy_name));
+	memcpy(cmd->policy_version, policy_version, sizeof(cmd->policy_version));
 
-		if (cmd->hint_root) {
-			r = dm_array_del(&cmd->hint_info, cmd->hint_root);
-			if (r)
-				return r;
-		}
+	hint_size = dm_cache_policy_get_hint_size(policy);
+	if (!hint_size)
+		return 0; /* short-circuit hints initialization */
+	cmd->policy_hint_size = hint_size;
 
-		r = dm_array_empty(&cmd->hint_info, &cmd->hint_root);
+	if (cmd->hint_root) {
+		r = dm_array_del(&cmd->hint_info, cmd->hint_root);
 		if (r)
 			return r;
-
-		value = cpu_to_le32(0);
-		__dm_bless_for_disk(&value);
-		r = dm_array_resize(&cmd->hint_info, cmd->hint_root, 0,
-				    from_cblock(cmd->cache_blocks),
-				    &value, &cmd->hint_root);
-		if (r)
-			return r;
-	}
-
-	return 0;
-}
-
-static int save_hint(void *context, dm_cblock_t cblock, dm_oblock_t oblock, uint32_t hint)
-{
-	struct dm_cache_metadata *cmd = context;
-	__le32 value = cpu_to_le32(hint);
-	int r;
-
-	__dm_bless_for_disk(&value);
-
-	r = dm_array_set_value(&cmd->hint_info, cmd->hint_root,
-			       from_cblock(cblock), &value, &cmd->hint_root);
-	cmd->changed = true;
-
-	return r;
-}
-
-static int write_hints(struct dm_cache_metadata *cmd, struct dm_cache_policy *policy)
-{
-	int r;
-
-	r = begin_hints(cmd, policy);
-	if (r) {
-		DMERR("begin_hints failed");
-		return r;
 	}
 
-	return policy_walk_mappings(policy, save_hint, cmd);
+	return dm_array_new(&cmd->hint_info, &cmd->hint_root,
+			    from_cblock(cmd->cache_blocks),
+			    get_hint, policy);
 }
 
 int dm_cache_write_hints(struct dm_cache_metadata *cmd, struct dm_cache_policy *policy)
diff --git a/drivers/md/dm-cache-policy-cleaner.c b/drivers/md/dm-cache-policy-cleaner.c
index 14aaaf059f06..2e8a8f1d8358 100644
--- a/drivers/md/dm-cache-policy-cleaner.c
+++ b/drivers/md/dm-cache-policy-cleaner.c
@@ -395,7 +395,7 @@ static void init_policy_functions(struct policy *p)
 	p->policy.set_dirty = wb_set_dirty;
 	p->policy.clear_dirty = wb_clear_dirty;
 	p->policy.load_mapping = wb_load_mapping;
-	p->policy.walk_mappings = NULL;
+	p->policy.get_hint = NULL;
 	p->policy.remove_mapping = wb_remove_mapping;
 	p->policy.writeback_work = wb_writeback_work;
 	p->policy.force_mapping = wb_force_mapping;
diff --git a/drivers/md/dm-cache-policy-internal.h b/drivers/md/dm-cache-policy-internal.h
index 2816018faa7f..808ee0e2b2c4 100644
--- a/drivers/md/dm-cache-policy-internal.h
+++ b/drivers/md/dm-cache-policy-internal.h
@@ -48,10 +48,10 @@ static inline int policy_load_mapping(struct dm_cache_policy *p,
 	return p->load_mapping(p, oblock, cblock, hint, hint_valid);
 }
 
-static inline int policy_walk_mappings(struct dm_cache_policy *p,
-				      policy_walk_fn fn, void *context)
+static inline uint32_t policy_get_hint(struct dm_cache_policy *p,
+				       dm_cblock_t cblock)
 {
-	return p->walk_mappings ? p->walk_mappings(p, fn, context) : 0;
+	return p->get_hint ? p->get_hint(p, cblock) : 0;
 }
 
 static inline int policy_writeback_work(struct dm_cache_policy *p,
diff --git a/drivers/md/dm-cache-policy-smq.c b/drivers/md/dm-cache-policy-smq.c
index cf48a617a3a4..c33f4a6e1d7d 100644
--- a/drivers/md/dm-cache-policy-smq.c
+++ b/drivers/md/dm-cache-policy-smq.c
@@ -1359,6 +1359,11 @@ static void smq_clear_dirty(struct dm_cache_policy *p, dm_oblock_t oblock)
 	spin_unlock_irqrestore(&mq->lock, flags);
 }
 
+static unsigned random_level(dm_cblock_t cblock)
+{
+	return hash_32_generic(from_cblock(cblock), 9) & (NR_CACHE_LEVELS - 1);
+}
+
 static int smq_load_mapping(struct dm_cache_policy *p,
 			    dm_oblock_t oblock, dm_cblock_t cblock,
 			    uint32_t hint, bool hint_valid)
@@ -1369,47 +1374,21 @@ static int smq_load_mapping(struct dm_cache_policy *p,
 	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;
+	e->level = hint_valid ? min(hint, NR_CACHE_LEVELS - 1) : random_level(cblock);
 	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)
+static uint32_t smq_get_hint(struct dm_cache_policy *p, dm_cblock_t cblock)
 {
 	struct smq_policy *mq = to_smq_policy(p);
-	int r = 0;
+	struct entry *e = get_entry(&mq->cache_alloc, from_cblock(cblock));
 
-	/*
-	 * We don't need to lock here since this method is only called once
-	 * the IO has stopped.
-	 */
-	r = smq_save_hints(mq, &mq->clean, fn, context);
-	if (!r)
-		r = smq_save_hints(mq, &mq->dirty, fn, context);
+	if (!e->allocated)
+		return 0;
 
-	return r;
+	return e->level;
 }
 
 static void __remove_mapping(struct smq_policy *mq, dm_oblock_t oblock)
@@ -1616,7 +1595,7 @@ static void init_policy_functions(struct smq_policy *mq, bool mimic_mq)
 	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.get_hint = smq_get_hint;
 	mq->policy.remove_mapping = smq_remove_mapping;
 	mq->policy.remove_cblock = smq_remove_cblock;
 	mq->policy.writeback_work = smq_writeback_work;
diff --git a/drivers/md/dm-cache-policy.h b/drivers/md/dm-cache-policy.h
index 05db56eedb6a..aa10b1493f34 100644
--- a/drivers/md/dm-cache-policy.h
+++ b/drivers/md/dm-cache-policy.h
@@ -90,9 +90,6 @@ struct policy_result {
 	dm_cblock_t cblock;	/* POLICY_HIT, POLICY_NEW, POLICY_REPLACE */
 };
 
-typedef int (*policy_walk_fn)(void *context, dm_cblock_t cblock,
-			      dm_oblock_t oblock, uint32_t hint);
-
 /*
  * The cache policy object.  Just a bunch of methods.  It is envisaged that
  * this structure will be embedded in a bigger, policy specific structure
@@ -158,8 +155,11 @@ struct dm_cache_policy {
 	int (*load_mapping)(struct dm_cache_policy *p, dm_oblock_t oblock,
 			    dm_cblock_t cblock, uint32_t hint, bool hint_valid);
 
-	int (*walk_mappings)(struct dm_cache_policy *p, policy_walk_fn fn,
-			     void *context);
+	/*
+	 * Gets the hint for a given cblock.  Called in a single threaded
+	 * context.  So no locking required.
+	 */
+	uint32_t (*get_hint)(struct dm_cache_policy *p, dm_cblock_t cblock);
 
 	/*
 	 * Override functions used on the error paths of the core target.
diff --git a/drivers/md/dm-crypt.c b/drivers/md/dm-crypt.c
index 874295757caa..a2768835d394 100644
--- a/drivers/md/dm-crypt.c
+++ b/drivers/md/dm-crypt.c
@@ -113,8 +113,7 @@ struct iv_tcw_private {
  * and encrypts / decrypts at the same time.
  */
 enum flags { DM_CRYPT_SUSPENDED, DM_CRYPT_KEY_VALID,
-	     DM_CRYPT_SAME_CPU, DM_CRYPT_NO_OFFLOAD,
-	     DM_CRYPT_EXIT_THREAD};
+	     DM_CRYPT_SAME_CPU, DM_CRYPT_NO_OFFLOAD };
 
 /*
  * The fields in here must be read only after initialization.
@@ -1136,7 +1135,7 @@ static void clone_init(struct dm_crypt_io *io, struct bio *clone)
 	clone->bi_private = io;
 	clone->bi_end_io  = crypt_endio;
 	clone->bi_bdev    = cc->dev->bdev;
-	bio_set_op_attrs(clone, bio_op(io->base_bio), io->base_bio->bi_opf);
+	bio_set_op_attrs(clone, bio_op(io->base_bio), bio_flags(io->base_bio));
 }
 
 static int kcryptd_io_read(struct dm_crypt_io *io, gfp_t gfp)
@@ -1207,18 +1206,20 @@ continue_locked:
 		if (!RB_EMPTY_ROOT(&cc->write_tree))
 			goto pop_from_list;
 
-		if (unlikely(test_bit(DM_CRYPT_EXIT_THREAD, &cc->flags))) {
-			spin_unlock_irq(&cc->write_thread_wait.lock);
-			break;
-		}
-
-		__set_current_state(TASK_INTERRUPTIBLE);
+		set_current_state(TASK_INTERRUPTIBLE);
 		__add_wait_queue(&cc->write_thread_wait, &wait);
 
 		spin_unlock_irq(&cc->write_thread_wait.lock);
 
+		if (unlikely(kthread_should_stop())) {
+			set_task_state(current, TASK_RUNNING);
+			remove_wait_queue(&cc->write_thread_wait, &wait);
+			break;
+		}
+
 		schedule();
 
+		set_task_state(current, TASK_RUNNING);
 		spin_lock_irq(&cc->write_thread_wait.lock);
 		__remove_wait_queue(&cc->write_thread_wait, &wait);
 		goto continue_locked;
@@ -1533,13 +1534,8 @@ static void crypt_dtr(struct dm_target *ti)
 	if (!cc)
 		return;
 
-	if (cc->write_thread) {
-		spin_lock_irq(&cc->write_thread_wait.lock);
-		set_bit(DM_CRYPT_EXIT_THREAD, &cc->flags);
-		wake_up_locked(&cc->write_thread_wait);
-		spin_unlock_irq(&cc->write_thread_wait.lock);
+	if (cc->write_thread)
 		kthread_stop(cc->write_thread);
-	}
 
 	if (cc->io_queue)
 		destroy_workqueue(cc->io_queue);
diff --git a/drivers/md/dm-log-writes.c b/drivers/md/dm-log-writes.c
index 49e4d8d4558f..4dfe38655a49 100644
--- a/drivers/md/dm-log-writes.c
+++ b/drivers/md/dm-log-writes.c
@@ -149,8 +149,6 @@ static void put_io_block(struct log_writes_c *lc)
 static void log_end_io(struct bio *bio)
 {
 	struct log_writes_c *lc = bio->bi_private;
-	struct bio_vec *bvec;
-	int i;
 
 	if (bio->bi_error) {
 		unsigned long flags;
@@ -161,9 +159,7 @@ static void log_end_io(struct bio *bio)
 		spin_unlock_irqrestore(&lc->blocks_lock, flags);
 	}
 
-	bio_for_each_segment_all(bvec, bio, i)
-		__free_page(bvec->bv_page);
-
+	bio_free_pages(bio);
 	put_io_block(lc);
 	bio_put(bio);
 }
diff --git a/drivers/md/dm-mpath.c b/drivers/md/dm-mpath.c
index ac734e5bbe48..e477af8596e2 100644
--- a/drivers/md/dm-mpath.c
+++ b/drivers/md/dm-mpath.c
@@ -550,9 +550,9 @@ static int __multipath_map(struct dm_target *ti, struct request *clone,
 		pgpath = choose_pgpath(m, nr_bytes);
 
 	if (!pgpath) {
-		if (!must_push_back_rq(m))
-			r = -EIO;	/* Failed */
-		return r;
+		if (must_push_back_rq(m))
+			return DM_MAPIO_DELAY_REQUEUE;
+		return -EIO;	/* Failed */
 	} else if (test_bit(MPATHF_QUEUE_IO, &m->flags) ||
 		   test_bit(MPATHF_PG_INIT_REQUIRED, &m->flags)) {
 		pg_init_all_paths(m);
@@ -680,9 +680,11 @@ static int multipath_map_bio(struct dm_target *ti, struct bio *bio)
 	return __multipath_map_bio(m, bio, mpio);
 }
 
-static void process_queued_bios_list(struct multipath *m)
+static void process_queued_io_list(struct multipath *m)
 {
-	if (m->queue_mode == DM_TYPE_BIO_BASED)
+	if (m->queue_mode == DM_TYPE_MQ_REQUEST_BASED)
+		dm_mq_kick_requeue_list(dm_table_get_md(m->ti->table));
+	else if (m->queue_mode == DM_TYPE_BIO_BASED)
 		queue_work(kmultipathd, &m->process_queued_bios);
 }
 
@@ -752,7 +754,7 @@ static int queue_if_no_path(struct multipath *m, bool queue_if_no_path,
 
 	if (!queue_if_no_path) {
 		dm_table_run_md_queue_async(m->ti->table);
-		process_queued_bios_list(m);
+		process_queued_io_list(m);
 	}
 
 	return 0;
@@ -1193,21 +1195,17 @@ static int multipath_ctr(struct dm_target *ti, unsigned argc, char **argv)
 
 static void multipath_wait_for_pg_init_completion(struct multipath *m)
 {
-	DECLARE_WAITQUEUE(wait, current);
-
-	add_wait_queue(&m->pg_init_wait, &wait);
+	DEFINE_WAIT(wait);
 
 	while (1) {
-		set_current_state(TASK_UNINTERRUPTIBLE);
+		prepare_to_wait(&m->pg_init_wait, &wait, TASK_UNINTERRUPTIBLE);
 
 		if (!atomic_read(&m->pg_init_in_progress))
 			break;
 
 		io_schedule();
 	}
-	set_current_state(TASK_RUNNING);
-
-	remove_wait_queue(&m->pg_init_wait, &wait);
+	finish_wait(&m->pg_init_wait, &wait);
 }
 
 static void flush_multipath_work(struct multipath *m)
@@ -1308,7 +1306,7 @@ out:
 	spin_unlock_irqrestore(&m->lock, flags);
 	if (run_queue) {
 		dm_table_run_md_queue_async(m->ti->table);
-		process_queued_bios_list(m);
+		process_queued_io_list(m);
 	}
 
 	return r;
@@ -1506,7 +1504,7 @@ static void pg_init_done(void *data, int errors)
 	}
 	clear_bit(MPATHF_QUEUE_IO, &m->flags);
 
-	process_queued_bios_list(m);
+	process_queued_io_list(m);
 
 	/*
 	 * Wake up any thread waiting to suspend.
@@ -1521,10 +1519,10 @@ static void activate_path(struct work_struct *work)
 {
 	struct pgpath *pgpath =
 		container_of(work, struct pgpath, activate_path.work);
+	struct request_queue *q = bdev_get_queue(pgpath->path.dev->bdev);
 
-	if (pgpath->is_active)
-		scsi_dh_activate(bdev_get_queue(pgpath->path.dev->bdev),
-				 pg_init_done, pgpath);
+	if (pgpath->is_active && !blk_queue_dying(q))
+		scsi_dh_activate(q, pg_init_done, pgpath);
 	else
 		pg_init_done(pgpath, SCSI_DH_DEV_OFFLINED);
 }
@@ -1532,6 +1530,14 @@ static void activate_path(struct work_struct *work)
 static int noretry_error(int error)
 {
 	switch (error) {
+	case -EBADE:
+		/*
+		 * EBADE signals an reservation conflict.
+		 * We shouldn't fail the path here as we can communicate with
+		 * the target.  We should failover to the next path, but in
+		 * doing so we might be causing a ping-pong between paths.
+		 * So just return the reservation conflict error.
+		 */
 	case -EOPNOTSUPP:
 	case -EREMOTEIO:
 	case -EILSEQ:
@@ -1576,9 +1582,6 @@ static int do_end_io(struct multipath *m, struct request *clone,
 		if (!test_bit(MPATHF_QUEUE_IF_NO_PATH, &m->flags)) {
 			if (!must_push_back_rq(m))
 				r = -EIO;
-		} else {
-			if (error == -EBADE)
-				r = error;
 		}
 	}
 
@@ -1627,9 +1630,6 @@ static int do_end_io_bio(struct multipath *m, struct bio *clone,
 			if (!must_push_back_bio(m))
 				return -EIO;
 			return DM_ENDIO_REQUEUE;
-		} else {
-			if (error == -EBADE)
-				return error;
 		}
 	}
 
@@ -1941,7 +1941,7 @@ static int multipath_prepare_ioctl(struct dm_target *ti,
 		if (test_bit(MPATHF_PG_INIT_REQUIRED, &m->flags))
 			pg_init_all_paths(m);
 		dm_table_run_md_queue_async(m->ti->table);
-		process_queued_bios_list(m);
+		process_queued_io_list(m);
 	}
 
 	/*
@@ -1994,11 +1994,14 @@ static int multipath_busy(struct dm_target *ti)
 	struct priority_group *pg, *next_pg;
 	struct pgpath *pgpath;
 
-	/* pg_init in progress or no paths available */
-	if (atomic_read(&m->pg_init_in_progress) ||
-	    (!atomic_read(&m->nr_valid_paths) && test_bit(MPATHF_QUEUE_IF_NO_PATH, &m->flags)))
+	/* pg_init in progress */
+	if (atomic_read(&m->pg_init_in_progress))
 		return true;
 
+	/* no paths available, for blk-mq: rely on IO mapping to delay requeue */
+	if (!atomic_read(&m->nr_valid_paths) && test_bit(MPATHF_QUEUE_IF_NO_PATH, &m->flags))
+		return (m->queue_mode != DM_TYPE_MQ_REQUEST_BASED);
+
 	/* Guess which priority_group will be used at next mapping time */
 	pg = lockless_dereference(m->current_pg);
 	next_pg = lockless_dereference(m->next_pg);
diff --git a/drivers/md/dm-rq.c b/drivers/md/dm-rq.c
index 1ca7463e8bb2..dc75bea0d541 100644
--- a/drivers/md/dm-rq.c
+++ b/drivers/md/dm-rq.c
@@ -73,15 +73,24 @@ static void dm_old_start_queue(struct request_queue *q)
 	spin_unlock_irqrestore(q->queue_lock, flags);
 }
 
+static void dm_mq_start_queue(struct request_queue *q)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(q->queue_lock, flags);
+	queue_flag_clear(QUEUE_FLAG_STOPPED, q);
+	spin_unlock_irqrestore(q->queue_lock, flags);
+
+	blk_mq_start_stopped_hw_queues(q, true);
+	blk_mq_kick_requeue_list(q);
+}
+
 void dm_start_queue(struct request_queue *q)
 {
 	if (!q->mq_ops)
 		dm_old_start_queue(q);
-	else {
-		queue_flag_clear_unlocked(QUEUE_FLAG_STOPPED, q);
-		blk_mq_start_stopped_hw_queues(q, true);
-		blk_mq_kick_requeue_list(q);
-	}
+	else
+		dm_mq_start_queue(q);
 }
 
 static void dm_old_stop_queue(struct request_queue *q)
@@ -89,27 +98,35 @@ static void dm_old_stop_queue(struct request_queue *q)
 	unsigned long flags;
 
 	spin_lock_irqsave(q->queue_lock, flags);
+	if (!blk_queue_stopped(q))
+		blk_stop_queue(q);
+	spin_unlock_irqrestore(q->queue_lock, flags);
+}
+
+static void dm_mq_stop_queue(struct request_queue *q)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(q->queue_lock, flags);
 	if (blk_queue_stopped(q)) {
 		spin_unlock_irqrestore(q->queue_lock, flags);
 		return;
 	}
 
-	blk_stop_queue(q);
+	queue_flag_set(QUEUE_FLAG_STOPPED, q);
 	spin_unlock_irqrestore(q->queue_lock, flags);
+
+	/* Avoid that requeuing could restart the queue. */
+	blk_mq_cancel_requeue_work(q);
+	blk_mq_stop_hw_queues(q);
 }
 
 void dm_stop_queue(struct request_queue *q)
 {
 	if (!q->mq_ops)
 		dm_old_stop_queue(q);
-	else {
-		spin_lock_irq(q->queue_lock);
-		queue_flag_set(QUEUE_FLAG_STOPPED, q);
-		spin_unlock_irq(q->queue_lock);
-
-		blk_mq_cancel_requeue_work(q);
-		blk_mq_stop_hw_queues(q);
-	}
+	else
+		dm_mq_stop_queue(q);
 }
 
 static struct dm_rq_target_io *alloc_old_rq_tio(struct mapped_device *md,
@@ -319,21 +336,32 @@ static void dm_old_requeue_request(struct request *rq)
 	spin_unlock_irqrestore(q->queue_lock, flags);
 }
 
-static void dm_mq_requeue_request(struct request *rq)
+static void __dm_mq_kick_requeue_list(struct request_queue *q, unsigned long msecs)
 {
-	struct request_queue *q = rq->q;
 	unsigned long flags;
 
-	blk_mq_requeue_request(rq);
 	spin_lock_irqsave(q->queue_lock, flags);
 	if (!blk_queue_stopped(q))
-		blk_mq_kick_requeue_list(q);
+		blk_mq_delay_kick_requeue_list(q, msecs);
 	spin_unlock_irqrestore(q->queue_lock, flags);
 }
 
-static void dm_requeue_original_request(struct mapped_device *md,
-					struct request *rq)
+void dm_mq_kick_requeue_list(struct mapped_device *md)
+{
+	__dm_mq_kick_requeue_list(dm_get_md_queue(md), 0);
+}
+EXPORT_SYMBOL(dm_mq_kick_requeue_list);
+
+static void dm_mq_delay_requeue_request(struct request *rq, unsigned long msecs)
+{
+	blk_mq_requeue_request(rq);
+	__dm_mq_kick_requeue_list(rq->q, msecs);
+}
+
+static void dm_requeue_original_request(struct dm_rq_target_io *tio, bool delay_requeue)
 {
+	struct mapped_device *md = tio->md;
+	struct request *rq = tio->orig;
 	int rw = rq_data_dir(rq);
 
 	rq_end_stats(md, rq);
@@ -342,7 +370,7 @@ static void dm_requeue_original_request(struct mapped_device *md,
 	if (!rq->q->mq_ops)
 		dm_old_requeue_request(rq);
 	else
-		dm_mq_requeue_request(rq);
+		dm_mq_delay_requeue_request(rq, delay_requeue ? 5000 : 0);
 
 	rq_completed(md, rw, false);
 }
@@ -372,7 +400,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_original_request(tio->md, tio->orig);
+		dm_requeue_original_request(tio, false);
 	else {
 		DMWARN("unimplemented target endio return value: %d", r);
 		BUG();
@@ -553,7 +581,7 @@ static void init_tio(struct dm_rq_target_io *tio, struct request *rq,
 	if (!md->init_tio_pdu)
 		memset(&tio->info, 0, sizeof(tio->info));
 	if (md->kworker_task)
-		init_kthread_work(&tio->work, map_tio_request);
+		kthread_init_work(&tio->work, map_tio_request);
 }
 
 static struct dm_rq_target_io *dm_old_prep_tio(struct request *rq,
@@ -612,20 +640,23 @@ static int dm_old_prep_fn(struct request_queue *q, struct request *rq)
 
 /*
  * Returns:
- * 0                : the request has been processed
- * DM_MAPIO_REQUEUE : the original request needs to be requeued
+ * DM_MAPIO_*       : the request has been processed as indicated
+ * DM_MAPIO_REQUEUE : the original request needs to be immediately requeued
  * < 0              : the request was completed due to failure
  */
-static int map_request(struct dm_rq_target_io *tio, struct request *rq,
-		       struct mapped_device *md)
+static int map_request(struct dm_rq_target_io *tio)
 {
 	int r;
 	struct dm_target *ti = tio->ti;
+	struct mapped_device *md = tio->md;
+	struct request *rq = tio->orig;
 	struct request *clone = NULL;
 
 	if (tio->clone) {
 		clone = tio->clone;
 		r = ti->type->map_rq(ti, clone, &tio->info);
+		if (r == DM_MAPIO_DELAY_REQUEUE)
+			return DM_MAPIO_REQUEUE; /* .request_fn requeue is always immediate */
 	} else {
 		r = ti->type->clone_and_map_rq(ti, rq, &tio->info, &clone);
 		if (r < 0) {
@@ -633,9 +664,8 @@ static int map_request(struct dm_rq_target_io *tio, struct request *rq,
 			dm_kill_unmapped_request(rq, r);
 			return r;
 		}
-		if (r != DM_MAPIO_REMAPPED)
-			return r;
-		if (setup_clone(clone, rq, tio, GFP_ATOMIC)) {
+		if (r == DM_MAPIO_REMAPPED &&
+		    setup_clone(clone, rq, tio, GFP_ATOMIC)) {
 			/* -ENOMEM */
 			ti->type->release_clone_rq(clone);
 			return DM_MAPIO_REQUEUE;
@@ -654,7 +684,10 @@ 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_original_request(md, tio->orig);
+		break;
+	case DM_MAPIO_DELAY_REQUEUE:
+		/* The target wants to requeue the I/O after a delay */
+		dm_requeue_original_request(tio, true);
 		break;
 	default:
 		if (r > 0) {
@@ -664,10 +697,9 @@ static int map_request(struct dm_rq_target_io *tio, struct request *rq,
 
 		/* The target wants to complete the I/O */
 		dm_kill_unmapped_request(rq, r);
-		return r;
 	}
 
-	return 0;
+	return r;
 }
 
 static void dm_start_request(struct mapped_device *md, struct request *orig)
@@ -706,11 +738,9 @@ static void dm_start_request(struct mapped_device *md, struct request *orig)
 static void map_tio_request(struct kthread_work *work)
 {
 	struct dm_rq_target_io *tio = container_of(work, struct dm_rq_target_io, work);
-	struct request *rq = tio->orig;
-	struct mapped_device *md = tio->md;
 
-	if (map_request(tio, rq, md) == DM_MAPIO_REQUEUE)
-		dm_requeue_original_request(md, rq);
+	if (map_request(tio) == DM_MAPIO_REQUEUE)
+		dm_requeue_original_request(tio, false);
 }
 
 ssize_t dm_attr_rq_based_seq_io_merge_deadline_show(struct mapped_device *md, char *buf)
@@ -801,7 +831,7 @@ static void dm_old_request_fn(struct request_queue *q)
 		tio = tio_from_request(rq);
 		/* Establish tio->ti before queuing work (map_tio_request) */
 		tio->ti = ti;
-		queue_kthread_work(&md->kworker, &tio->work);
+		kthread_queue_work(&md->kworker, &tio->work);
 		BUG_ON(!irqs_disabled());
 	}
 }
@@ -823,7 +853,7 @@ int dm_old_init_request_queue(struct mapped_device *md)
 	blk_queue_prep_rq(md->queue, dm_old_prep_fn);
 
 	/* Initialize the request-based DM worker thread */
-	init_kthread_worker(&md->kworker);
+	kthread_init_worker(&md->kworker);
 	md->kworker_task = kthread_run(kthread_worker_fn, &md->kworker,
 				       "kdmwork-%s", dm_device_name(md));
 	if (IS_ERR(md->kworker_task))
@@ -896,7 +926,7 @@ static int dm_mq_queue_rq(struct blk_mq_hw_ctx *hctx,
 	tio->ti = ti;
 
 	/* Direct call is fine since .queue_rq allows allocations */
-	if (map_request(tio, rq, md) == DM_MAPIO_REQUEUE) {
+	if (map_request(tio) == DM_MAPIO_REQUEUE) {
 		/* Undo dm_start_request() before requeuing */
 		rq_end_stats(md, rq);
 		rq_completed(md, rq_data_dir(rq), false);
@@ -908,7 +938,6 @@ static int dm_mq_queue_rq(struct blk_mq_hw_ctx *hctx,
 
 static struct blk_mq_ops dm_mq_ops = {
 	.queue_rq = dm_mq_queue_rq,
-	.map_queue = blk_mq_map_queue,
 	.complete = dm_softirq_done,
 	.init_request = dm_mq_init_request,
 };
@@ -955,7 +984,7 @@ int dm_mq_init_request_queue(struct mapped_device *md, struct dm_table *t)
 	dm_init_md_queue(md);
 
 	/* backfill 'mq' sysfs registration normally done in blk_register_queue */
-	blk_mq_register_disk(md->disk);
+	blk_mq_register_dev(disk_to_dev(md->disk), q);
 
 	return 0;
 
diff --git a/drivers/md/dm-rq.h b/drivers/md/dm-rq.h
index 9e6f0a3773d4..4da06cae7bad 100644
--- a/drivers/md/dm-rq.h
+++ b/drivers/md/dm-rq.h
@@ -55,6 +55,8 @@ void dm_mq_cleanup_mapped_device(struct mapped_device *md);
 void dm_start_queue(struct request_queue *q);
 void dm_stop_queue(struct request_queue *q);
 
+void dm_mq_kick_requeue_list(struct mapped_device *md);
+
 unsigned dm_get_reserved_rq_based_ios(void);
 
 ssize_t dm_attr_rq_based_seq_io_merge_deadline_show(struct mapped_device *md, char *buf);
diff --git a/drivers/md/dm.c b/drivers/md/dm.c
index fa9b1cb4438a..147af9536d0c 100644
--- a/drivers/md/dm.c
+++ b/drivers/md/dm.c
@@ -1648,6 +1648,8 @@ static struct dm_table *__bind(struct mapped_device *md, struct dm_table *t,
 	struct request_queue *q = md->queue;
 	sector_t size;
 
+	lockdep_assert_held(&md->suspend_lock);
+
 	size = dm_table_get_size(t);
 
 	/*
@@ -1873,6 +1875,7 @@ EXPORT_SYMBOL_GPL(dm_device_name);
 
 static void __dm_destroy(struct mapped_device *md, bool wait)
 {
+	struct request_queue *q = dm_get_md_queue(md);
 	struct dm_table *map;
 	int srcu_idx;
 
@@ -1883,8 +1886,12 @@ static void __dm_destroy(struct mapped_device *md, bool wait)
 	set_bit(DMF_FREEING, &md->flags);
 	spin_unlock(&_minor_lock);
 
+	spin_lock_irq(q->queue_lock);
+	queue_flag_set(QUEUE_FLAG_DYING, q);
+	spin_unlock_irq(q->queue_lock);
+
 	if (dm_request_based(md) && md->kworker_task)
-		flush_kthread_worker(&md->kworker);
+		kthread_flush_worker(&md->kworker);
 
 	/*
 	 * Take suspend_lock so that presuspend and postsuspend methods
@@ -1934,30 +1941,25 @@ void dm_put(struct mapped_device *md)
 }
 EXPORT_SYMBOL_GPL(dm_put);
 
-static int dm_wait_for_completion(struct mapped_device *md, int interruptible)
+static int dm_wait_for_completion(struct mapped_device *md, long task_state)
 {
 	int r = 0;
-	DECLARE_WAITQUEUE(wait, current);
-
-	add_wait_queue(&md->wait, &wait);
+	DEFINE_WAIT(wait);
 
 	while (1) {
-		set_current_state(interruptible);
+		prepare_to_wait(&md->wait, &wait, task_state);
 
 		if (!md_in_flight(md))
 			break;
 
-		if (interruptible == TASK_INTERRUPTIBLE &&
-		    signal_pending(current)) {
+		if (signal_pending_state(task_state, current)) {
 			r = -EINTR;
 			break;
 		}
 
 		io_schedule();
 	}
-	set_current_state(TASK_RUNNING);
-
-	remove_wait_queue(&md->wait, &wait);
+	finish_wait(&md->wait, &wait);
 
 	return r;
 }
@@ -2075,6 +2077,10 @@ static void unlock_fs(struct mapped_device *md)
 }
 
 /*
+ * @suspend_flags: DM_SUSPEND_LOCKFS_FLAG and/or DM_SUSPEND_NOFLUSH_FLAG
+ * @task_state: e.g. TASK_INTERRUPTIBLE or TASK_UNINTERRUPTIBLE
+ * @dmf_suspended_flag: DMF_SUSPENDED or DMF_SUSPENDED_INTERNALLY
+ *
  * If __dm_suspend returns 0, the device is completely quiescent
  * now. There is no request-processing activity. All new requests
  * are being added to md->deferred list.
@@ -2082,13 +2088,15 @@ static void unlock_fs(struct mapped_device *md)
  * Caller must hold md->suspend_lock
  */
 static int __dm_suspend(struct mapped_device *md, struct dm_table *map,
-			unsigned suspend_flags, int interruptible,
+			unsigned suspend_flags, long task_state,
 			int dmf_suspended_flag)
 {
 	bool do_lockfs = suspend_flags & DM_SUSPEND_LOCKFS_FLAG;
 	bool noflush = suspend_flags & DM_SUSPEND_NOFLUSH_FLAG;
 	int r;
 
+	lockdep_assert_held(&md->suspend_lock);
+
 	/*
 	 * DMF_NOFLUSH_SUSPENDING must be set before presuspend.
 	 * This flag is cleared before dm_suspend returns.
@@ -2139,7 +2147,7 @@ static int __dm_suspend(struct mapped_device *md, struct dm_table *map,
 	if (dm_request_based(md)) {
 		dm_stop_queue(md->queue);
 		if (md->kworker_task)
-			flush_kthread_worker(&md->kworker);
+			kthread_flush_worker(&md->kworker);
 	}
 
 	flush_workqueue(md->wq);
@@ -2149,7 +2157,7 @@ static int __dm_suspend(struct mapped_device *md, struct dm_table *map,
 	 * We call dm_wait_for_completion to wait for all existing requests
 	 * to finish.
 	 */
-	r = dm_wait_for_completion(md, interruptible);
+	r = dm_wait_for_completion(md, task_state);
 	if (!r)
 		set_bit(dmf_suspended_flag, &md->flags);
 
@@ -2249,10 +2257,11 @@ static int __dm_resume(struct mapped_device *md, struct dm_table *map)
 
 int dm_resume(struct mapped_device *md)
 {
-	int r = -EINVAL;
+	int r;
 	struct dm_table *map = NULL;
 
 retry:
+	r = -EINVAL;
 	mutex_lock_nested(&md->suspend_lock, SINGLE_DEPTH_NESTING);
 
 	if (!dm_suspended_md(md))
@@ -2276,8 +2285,6 @@ retry:
 		goto out;
 
 	clear_bit(DMF_SUSPENDED, &md->flags);
-
-	r = 0;
 out:
 	mutex_unlock(&md->suspend_lock);
 
diff --git a/drivers/md/md-cluster.c b/drivers/md/md-cluster.c
index 34a840d9df76..2b13117fb918 100644
--- a/drivers/md/md-cluster.c
+++ b/drivers/md/md-cluster.c
@@ -10,6 +10,7 @@
 
 
 #include <linux/module.h>
+#include <linux/kthread.h>
 #include <linux/dlm.h>
 #include <linux/sched.h>
 #include <linux/raid/md_p.h>
@@ -25,7 +26,8 @@ struct dlm_lock_resource {
 	struct dlm_lksb lksb;
 	char *name; /* lock name. */
 	uint32_t flags; /* flags to pass to dlm_lock() */
-	struct completion completion; /* completion for synchronized locking */
+	wait_queue_head_t sync_locking; /* wait queue for synchronized locking */
+	bool sync_locking_done;
 	void (*bast)(void *arg, int mode); /* blocking AST function pointer*/
 	struct mddev *mddev; /* pointing back to mddev. */
 	int mode;
@@ -118,7 +120,8 @@ static void sync_ast(void *arg)
 	struct dlm_lock_resource *res;
 
 	res = arg;
-	complete(&res->completion);
+	res->sync_locking_done = true;
+	wake_up(&res->sync_locking);
 }
 
 static int dlm_lock_sync(struct dlm_lock_resource *res, int mode)
@@ -130,7 +133,8 @@ static int dlm_lock_sync(struct dlm_lock_resource *res, int mode)
 			0, sync_ast, res, res->bast);
 	if (ret)
 		return ret;
-	wait_for_completion(&res->completion);
+	wait_event(res->sync_locking, res->sync_locking_done);
+	res->sync_locking_done = false;
 	if (res->lksb.sb_status == 0)
 		res->mode = mode;
 	return res->lksb.sb_status;
@@ -141,6 +145,44 @@ static int dlm_unlock_sync(struct dlm_lock_resource *res)
 	return dlm_lock_sync(res, DLM_LOCK_NL);
 }
 
+/*
+ * An variation of dlm_lock_sync, which make lock request could
+ * be interrupted
+ */
+static int dlm_lock_sync_interruptible(struct dlm_lock_resource *res, int mode,
+				       struct mddev *mddev)
+{
+	int ret = 0;
+
+	ret = dlm_lock(res->ls, mode, &res->lksb,
+			res->flags, res->name, strlen(res->name),
+			0, sync_ast, res, res->bast);
+	if (ret)
+		return ret;
+
+	wait_event(res->sync_locking, res->sync_locking_done
+				      || kthread_should_stop()
+				      || test_bit(MD_CLOSING, &mddev->flags));
+	if (!res->sync_locking_done) {
+		/*
+		 * the convert queue contains the lock request when request is
+		 * interrupted, and sync_ast could still be run, so need to
+		 * cancel the request and reset completion
+		 */
+		ret = dlm_unlock(res->ls, res->lksb.sb_lkid, DLM_LKF_CANCEL,
+			&res->lksb, res);
+		res->sync_locking_done = false;
+		if (unlikely(ret != 0))
+			pr_info("failed to cancel previous lock request "
+				 "%s return %d\n", res->name, ret);
+		return -EPERM;
+	} else
+		res->sync_locking_done = false;
+	if (res->lksb.sb_status == 0)
+		res->mode = mode;
+	return res->lksb.sb_status;
+}
+
 static struct dlm_lock_resource *lockres_init(struct mddev *mddev,
 		char *name, void (*bastfn)(void *arg, int mode), int with_lvb)
 {
@@ -151,7 +193,8 @@ static struct dlm_lock_resource *lockres_init(struct mddev *mddev,
 	res = kzalloc(sizeof(struct dlm_lock_resource), GFP_KERNEL);
 	if (!res)
 		return NULL;
-	init_completion(&res->completion);
+	init_waitqueue_head(&res->sync_locking);
+	res->sync_locking_done = false;
 	res->ls = cinfo->lockspace;
 	res->mddev = mddev;
 	res->mode = DLM_LOCK_IV;
@@ -194,25 +237,21 @@ out_err:
 
 static void lockres_free(struct dlm_lock_resource *res)
 {
-	int ret;
+	int ret = 0;
 
 	if (!res)
 		return;
 
-	/* cancel a lock request or a conversion request that is blocked */
-	res->flags |= DLM_LKF_CANCEL;
-retry:
-	ret = dlm_unlock(res->ls, res->lksb.sb_lkid, 0, &res->lksb, res);
-	if (unlikely(ret != 0)) {
-		pr_info("%s: failed to unlock %s return %d\n", __func__, res->name, ret);
-
-		/* if a lock conversion is cancelled, then the lock is put
-		 * back to grant queue, need to ensure it is unlocked */
-		if (ret == -DLM_ECANCEL)
-			goto retry;
-	}
-	res->flags &= ~DLM_LKF_CANCEL;
-	wait_for_completion(&res->completion);
+	/*
+	 * use FORCEUNLOCK flag, so we can unlock even the lock is on the
+	 * waiting or convert queue
+	 */
+	ret = dlm_unlock(res->ls, res->lksb.sb_lkid, DLM_LKF_FORCEUNLOCK,
+		&res->lksb, res);
+	if (unlikely(ret != 0))
+		pr_err("failed to unlock %s return %d\n", res->name, ret);
+	else
+		wait_event(res->sync_locking, res->sync_locking_done);
 
 	kfree(res->name);
 	kfree(res->lksb.sb_lvbptr);
@@ -279,7 +318,7 @@ static void recover_bitmaps(struct md_thread *thread)
 			goto clear_bit;
 		}
 
-		ret = dlm_lock_sync(bm_lockres, DLM_LOCK_PW);
+		ret = dlm_lock_sync_interruptible(bm_lockres, DLM_LOCK_PW, mddev);
 		if (ret) {
 			pr_err("md-cluster: Could not DLM lock %s: %d\n",
 					str, ret);
@@ -288,7 +327,7 @@ static void recover_bitmaps(struct md_thread *thread)
 		ret = bitmap_copy_from_slot(mddev, slot, &lo, &hi, true);
 		if (ret) {
 			pr_err("md-cluster: Could not copy data from bitmap %d\n", slot);
-			goto dlm_unlock;
+			goto clear_bit;
 		}
 		if (hi > 0) {
 			if (lo < mddev->recovery_cp)
@@ -300,8 +339,6 @@ static void recover_bitmaps(struct md_thread *thread)
 			    md_wakeup_thread(mddev->thread);
 			}
 		}
-dlm_unlock:
-		dlm_unlock_sync(bm_lockres);
 clear_bit:
 		lockres_free(bm_lockres);
 		clear_bit(slot, &cinfo->recovery_map);
@@ -495,9 +532,10 @@ static void process_metadata_update(struct mddev *mddev, struct cluster_msg *msg
 
 static void process_remove_disk(struct mddev *mddev, struct cluster_msg *msg)
 {
-	struct md_rdev *rdev = md_find_rdev_nr_rcu(mddev,
-						   le32_to_cpu(msg->raid_slot));
+	struct md_rdev *rdev;
 
+	rcu_read_lock();
+	rdev = md_find_rdev_nr_rcu(mddev, le32_to_cpu(msg->raid_slot));
 	if (rdev) {
 		set_bit(ClusterRemove, &rdev->flags);
 		set_bit(MD_RECOVERY_NEEDED, &mddev->recovery);
@@ -506,18 +544,21 @@ static void process_remove_disk(struct mddev *mddev, struct cluster_msg *msg)
 	else
 		pr_warn("%s: %d Could not find disk(%d) to REMOVE\n",
 			__func__, __LINE__, le32_to_cpu(msg->raid_slot));
+	rcu_read_unlock();
 }
 
 static void process_readd_disk(struct mddev *mddev, struct cluster_msg *msg)
 {
-	struct md_rdev *rdev = md_find_rdev_nr_rcu(mddev,
-						   le32_to_cpu(msg->raid_slot));
+	struct md_rdev *rdev;
 
+	rcu_read_lock();
+	rdev = md_find_rdev_nr_rcu(mddev, le32_to_cpu(msg->raid_slot));
 	if (rdev && test_bit(Faulty, &rdev->flags))
 		clear_bit(Faulty, &rdev->flags);
 	else
 		pr_warn("%s: %d Could not find disk(%d) which is faulty",
 			__func__, __LINE__, le32_to_cpu(msg->raid_slot));
+	rcu_read_unlock();
 }
 
 static int process_recvd_msg(struct mddev *mddev, struct cluster_msg *msg)
@@ -770,7 +811,6 @@ static int gather_all_resync_info(struct mddev *mddev, int total_slots)
 			md_check_recovery(mddev);
 		}
 
-		dlm_unlock_sync(bm_lockres);
 		lockres_free(bm_lockres);
 	}
 out:
@@ -1006,7 +1046,7 @@ static void metadata_update_cancel(struct mddev *mddev)
 static int resync_start(struct mddev *mddev)
 {
 	struct md_cluster_info *cinfo = mddev->cluster_info;
-	return dlm_lock_sync(cinfo->resync_lockres, DLM_LOCK_EX);
+	return dlm_lock_sync_interruptible(cinfo->resync_lockres, DLM_LOCK_EX, mddev);
 }
 
 static int resync_info_update(struct mddev *mddev, sector_t lo, sector_t hi)
@@ -1186,7 +1226,6 @@ static void unlock_all_bitmaps(struct mddev *mddev)
 	if (cinfo->other_bitmap_lockres) {
 		for (i = 0; i < mddev->bitmap_info.nodes - 1; i++) {
 			if (cinfo->other_bitmap_lockres[i]) {
-				dlm_unlock_sync(cinfo->other_bitmap_lockres[i]);
 				lockres_free(cinfo->other_bitmap_lockres[i]);
 			}
 		}
diff --git a/drivers/md/md.c b/drivers/md/md.c
index 915e84d631a2..eac84d8ff724 100644
--- a/drivers/md/md.c
+++ b/drivers/md/md.c
@@ -5297,6 +5297,21 @@ int md_run(struct mddev *mddev)
 		return err;
 	}
 	if (mddev->queue) {
+		bool nonrot = true;
+
+		rdev_for_each(rdev, mddev) {
+			if (rdev->raid_disk >= 0 &&
+			    !blk_queue_nonrot(bdev_get_queue(rdev->bdev))) {
+				nonrot = false;
+				break;
+			}
+		}
+		if (mddev->degraded)
+			nonrot = false;
+		if (nonrot)
+			queue_flag_set_unlocked(QUEUE_FLAG_NONROT, mddev->queue);
+		else
+			queue_flag_clear_unlocked(QUEUE_FLAG_NONROT, mddev->queue);
 		mddev->queue->backing_dev_info.congested_data = mddev;
 		mddev->queue->backing_dev_info.congested_fn = md_congested;
 	}
@@ -5454,12 +5469,14 @@ static void md_clean(struct mddev *mddev)
 	mddev->degraded = 0;
 	mddev->safemode = 0;
 	mddev->private = NULL;
+	mddev->cluster_info = NULL;
 	mddev->bitmap_info.offset = 0;
 	mddev->bitmap_info.default_offset = 0;
 	mddev->bitmap_info.default_space = 0;
 	mddev->bitmap_info.chunksize = 0;
 	mddev->bitmap_info.daemon_sleep = 0;
 	mddev->bitmap_info.max_write_behind = 0;
+	mddev->bitmap_info.nodes = 0;
 }
 
 static void __md_stop_writes(struct mddev *mddev)
@@ -5573,8 +5590,7 @@ static int md_set_readonly(struct mddev *mddev, struct block_device *bdev)
 	mutex_lock(&mddev->open_mutex);
 	if ((mddev->pers && atomic_read(&mddev->openers) > !!bdev) ||
 	    mddev->sync_thread ||
-	    test_bit(MD_RECOVERY_RUNNING, &mddev->recovery) ||
-	    (bdev && !test_bit(MD_STILL_CLOSED, &mddev->flags))) {
+	    test_bit(MD_RECOVERY_RUNNING, &mddev->recovery)) {
 		printk("md: %s still in use.\n",mdname(mddev));
 		if (did_freeze) {
 			clear_bit(MD_RECOVERY_FROZEN, &mddev->recovery);
@@ -5636,8 +5652,7 @@ static int do_md_stop(struct mddev *mddev, int mode,
 	if ((mddev->pers && atomic_read(&mddev->openers) > !!bdev) ||
 	    mddev->sysfs_active ||
 	    mddev->sync_thread ||
-	    test_bit(MD_RECOVERY_RUNNING, &mddev->recovery) ||
-	    (bdev && !test_bit(MD_STILL_CLOSED, &mddev->flags))) {
+	    test_bit(MD_RECOVERY_RUNNING, &mddev->recovery)) {
 		printk("md: %s still in use.\n",mdname(mddev));
 		mutex_unlock(&mddev->open_mutex);
 		if (did_freeze) {
@@ -6101,9 +6116,14 @@ static int add_new_disk(struct mddev *mddev, mdu_disk_info_t *info)
 			export_rdev(rdev);
 
 		if (mddev_is_clustered(mddev)) {
-			if (info->state & (1 << MD_DISK_CANDIDATE))
-				md_cluster_ops->new_disk_ack(mddev, (err == 0));
-			else {
+			if (info->state & (1 << MD_DISK_CANDIDATE)) {
+				if (!err) {
+					err = md_cluster_ops->new_disk_ack(mddev,
+						err == 0);
+					if (err)
+						md_kick_rdev_from_array(rdev);
+				}
+			} else {
 				if (err)
 					md_cluster_ops->add_new_disk_cancel(mddev);
 				else
@@ -6821,7 +6841,7 @@ static int md_ioctl(struct block_device *bdev, fmode_t mode,
 			err = -EBUSY;
 			goto out;
 		}
-		set_bit(MD_STILL_CLOSED, &mddev->flags);
+		set_bit(MD_CLOSING, &mddev->flags);
 		mutex_unlock(&mddev->open_mutex);
 		sync_blockdev(bdev);
 	}
@@ -7070,9 +7090,13 @@ static int md_open(struct block_device *bdev, fmode_t mode)
 	if ((err = mutex_lock_interruptible(&mddev->open_mutex)))
 		goto out;
 
+	if (test_bit(MD_CLOSING, &mddev->flags)) {
+		mutex_unlock(&mddev->open_mutex);
+		return -ENODEV;
+	}
+
 	err = 0;
 	atomic_inc(&mddev->openers);
-	clear_bit(MD_STILL_CLOSED, &mddev->flags);
 	mutex_unlock(&mddev->open_mutex);
 
 	check_disk_change(bdev);
@@ -8873,7 +8897,9 @@ static void autostart_arrays(int part)
 		list_del(&node_detected_dev->list);
 		dev = node_detected_dev->dev;
 		kfree(node_detected_dev);
+		mutex_unlock(&detected_devices_mutex);
 		rdev = md_import_device(dev,0, 90);
+		mutex_lock(&detected_devices_mutex);
 		if (IS_ERR(rdev))
 			continue;
 
diff --git a/drivers/md/md.h b/drivers/md/md.h
index 20c667579ede..2b2041773e79 100644
--- a/drivers/md/md.h
+++ b/drivers/md/md.h
@@ -201,9 +201,8 @@ struct mddev {
 #define MD_CHANGE_PENDING 2	/* switch from 'clean' to 'active' in progress */
 #define MD_UPDATE_SB_FLAGS (1 | 2 | 4)	/* If these are set, md_update_sb needed */
 #define MD_ARRAY_FIRST_USE 3    /* First use of array, needs initialization */
-#define MD_STILL_CLOSED	4	/* If set, then array has not been opened since
-				 * md_ioctl checked on it.
-				 */
+#define MD_CLOSING	4	/* If set, we are closing the array, do not open
+				 * it then */
 #define MD_JOURNAL_CLEAN 5	/* A raid with journal is already clean */
 #define MD_HAS_JOURNAL	6	/* The raid array has journal feature set */
 #define MD_RELOAD_SB	7	/* Reload the superblock because another node
diff --git a/drivers/md/persistent-data/dm-array.c b/drivers/md/persistent-data/dm-array.c
index 431a03067d64..e83047cbb2da 100644
--- a/drivers/md/persistent-data/dm-array.c
+++ b/drivers/md/persistent-data/dm-array.c
@@ -277,6 +277,48 @@ static int insert_ablock(struct dm_array_info *info, uint64_t index,
 	return dm_btree_insert(&info->btree_info, *root, &index, &block_le, root);
 }
 
+/*----------------------------------------------------------------*/
+
+static int __shadow_ablock(struct dm_array_info *info, dm_block_t b,
+			   struct dm_block **block, struct array_block **ab)
+{
+	int inc;
+	int r = dm_tm_shadow_block(info->btree_info.tm, b,
+				   &array_validator, block, &inc);
+	if (r)
+		return r;
+
+	*ab = dm_block_data(*block);
+	if (inc)
+		inc_ablock_entries(info, *ab);
+
+	return 0;
+}
+
+/*
+ * The shadow op will often be a noop.  Only insert if it really
+ * copied data.
+ */
+static int __reinsert_ablock(struct dm_array_info *info, unsigned index,
+			     struct dm_block *block, dm_block_t b,
+			     dm_block_t *root)
+{
+	int r = 0;
+
+	if (dm_block_location(block) != b) {
+		/*
+		 * dm_tm_shadow_block will have already decremented the old
+		 * block, but it is still referenced by the btree.  We
+		 * increment to stop the insert decrementing it below zero
+		 * when overwriting the old value.
+		 */
+		dm_tm_inc(info->btree_info.tm, b);
+		r = insert_ablock(info, index, block, root);
+	}
+
+	return r;
+}
+
 /*
  * Looks up an array block in the btree.  Then shadows it, and updates the
  * btree to point to this new shadow.  'root' is an input/output parameter
@@ -286,49 +328,21 @@ static int shadow_ablock(struct dm_array_info *info, dm_block_t *root,
 			 unsigned index, struct dm_block **block,
 			 struct array_block **ab)
 {
-	int r, inc;
+	int r;
 	uint64_t key = index;
 	dm_block_t b;
 	__le64 block_le;
 
-	/*
-	 * lookup
-	 */
 	r = dm_btree_lookup(&info->btree_info, *root, &key, &block_le);
 	if (r)
 		return r;
 	b = le64_to_cpu(block_le);
 
-	/*
-	 * shadow
-	 */
-	r = dm_tm_shadow_block(info->btree_info.tm, b,
-			       &array_validator, block, &inc);
+	r = __shadow_ablock(info, b, block, ab);
 	if (r)
 		return r;
 
-	*ab = dm_block_data(*block);
-	if (inc)
-		inc_ablock_entries(info, *ab);
-
-	/*
-	 * Reinsert.
-	 *
-	 * The shadow op will often be a noop.  Only insert if it really
-	 * copied data.
-	 */
-	if (dm_block_location(*block) != b) {
-		/*
-		 * dm_tm_shadow_block will have already decremented the old
-		 * block, but it is still referenced by the btree.  We
-		 * increment to stop the insert decrementing it below zero
-		 * when overwriting the old value.
-		 */
-		dm_tm_inc(info->btree_info.tm, b);
-		r = insert_ablock(info, index, *block, root);
-	}
-
-	return r;
+	return __reinsert_ablock(info, index, *block, b, root);
 }
 
 /*
@@ -681,6 +695,72 @@ int dm_array_resize(struct dm_array_info *info, dm_block_t root,
 }
 EXPORT_SYMBOL_GPL(dm_array_resize);
 
+static int populate_ablock_with_values(struct dm_array_info *info, struct array_block *ab,
+				       value_fn fn, void *context, unsigned base, unsigned new_nr)
+{
+	int r;
+	unsigned i;
+	uint32_t nr_entries;
+	struct dm_btree_value_type *vt = &info->value_type;
+
+	BUG_ON(le32_to_cpu(ab->nr_entries));
+	BUG_ON(new_nr > le32_to_cpu(ab->max_entries));
+
+	nr_entries = le32_to_cpu(ab->nr_entries);
+	for (i = 0; i < new_nr; i++) {
+		r = fn(base + i, element_at(info, ab, i), context);
+		if (r)
+			return r;
+
+		if (vt->inc)
+			vt->inc(vt->context, element_at(info, ab, i));
+	}
+
+	ab->nr_entries = cpu_to_le32(new_nr);
+	return 0;
+}
+
+int dm_array_new(struct dm_array_info *info, dm_block_t *root,
+		 uint32_t size, value_fn fn, void *context)
+{
+	int r;
+	struct dm_block *block;
+	struct array_block *ab;
+	unsigned block_index, end_block, size_of_block, max_entries;
+
+	r = dm_array_empty(info, root);
+	if (r)
+		return r;
+
+	size_of_block = dm_bm_block_size(dm_tm_get_bm(info->btree_info.tm));
+	max_entries = calc_max_entries(info->value_type.size, size_of_block);
+	end_block = dm_div_up(size, max_entries);
+
+	for (block_index = 0; block_index != end_block; block_index++) {
+		r = alloc_ablock(info, size_of_block, max_entries, &block, &ab);
+		if (r)
+			break;
+
+		r = populate_ablock_with_values(info, ab, fn, context,
+						block_index * max_entries,
+						min(max_entries, size));
+		if (r) {
+			unlock_ablock(info, block);
+			break;
+		}
+
+		r = insert_ablock(info, block_index, block, root);
+		unlock_ablock(info, block);
+		if (r)
+			break;
+
+		size -= max_entries;
+	}
+
+	return r;
+}
+EXPORT_SYMBOL_GPL(dm_array_new);
+
 int dm_array_del(struct dm_array_info *info, dm_block_t root)
 {
 	return dm_btree_del(&info->btree_info, root);
@@ -819,3 +899,89 @@ int dm_array_walk(struct dm_array_info *info, dm_block_t root,
 EXPORT_SYMBOL_GPL(dm_array_walk);
 
 /*----------------------------------------------------------------*/
+
+static int load_ablock(struct dm_array_cursor *c)
+{
+	int r;
+	__le64 value_le;
+	uint64_t key;
+
+	if (c->block)
+		unlock_ablock(c->info, c->block);
+
+	c->block = NULL;
+	c->ab = NULL;
+	c->index = 0;
+
+	r = dm_btree_cursor_get_value(&c->cursor, &key, &value_le);
+	if (r) {
+		DMERR("dm_btree_cursor_get_value failed");
+		dm_btree_cursor_end(&c->cursor);
+
+	} else {
+		r = get_ablock(c->info, le64_to_cpu(value_le), &c->block, &c->ab);
+		if (r) {
+			DMERR("get_ablock failed");
+			dm_btree_cursor_end(&c->cursor);
+		}
+	}
+
+	return r;
+}
+
+int dm_array_cursor_begin(struct dm_array_info *info, dm_block_t root,
+			  struct dm_array_cursor *c)
+{
+	int r;
+
+	memset(c, 0, sizeof(*c));
+	c->info = info;
+	r = dm_btree_cursor_begin(&info->btree_info, root, true, &c->cursor);
+	if (r) {
+		DMERR("couldn't create btree cursor");
+		return r;
+	}
+
+	return load_ablock(c);
+}
+EXPORT_SYMBOL_GPL(dm_array_cursor_begin);
+
+void dm_array_cursor_end(struct dm_array_cursor *c)
+{
+	if (c->block) {
+		unlock_ablock(c->info, c->block);
+		dm_btree_cursor_end(&c->cursor);
+	}
+}
+EXPORT_SYMBOL_GPL(dm_array_cursor_end);
+
+int dm_array_cursor_next(struct dm_array_cursor *c)
+{
+	int r;
+
+	if (!c->block)
+		return -ENODATA;
+
+	c->index++;
+
+	if (c->index >= le32_to_cpu(c->ab->nr_entries)) {
+		r = dm_btree_cursor_next(&c->cursor);
+		if (r)
+			return r;
+
+		r = load_ablock(c);
+		if (r)
+			return r;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(dm_array_cursor_next);
+
+void dm_array_cursor_get_value(struct dm_array_cursor *c, void **value_le)
+{
+	*value_le = element_at(c->info, c->ab, c->index);
+}
+EXPORT_SYMBOL_GPL(dm_array_cursor_get_value);
+
+/*----------------------------------------------------------------*/
diff --git a/drivers/md/persistent-data/dm-array.h b/drivers/md/persistent-data/dm-array.h
index ea177d6fa58f..27ee49a55473 100644
--- a/drivers/md/persistent-data/dm-array.h
+++ b/drivers/md/persistent-data/dm-array.h
@@ -112,6 +112,25 @@ int dm_array_resize(struct dm_array_info *info, dm_block_t root,
 	__dm_written_to_disk(value);
 
 /*
+ * Creates a new array populated with values provided by a callback
+ * function.  This is more efficient than creating an empty array,
+ * resizing, and then setting values since that process incurs a lot of
+ * copying.
+ *
+ * Assumes 32bit values for now since it's only used by the cache hint
+ * array.
+ *
+ * info - describes the array
+ * root - the root block of the array on disk
+ * size - the number of entries in the array
+ * fn - the callback
+ * context - passed to the callback
+ */
+typedef int (*value_fn)(uint32_t index, void *value_le, void *context);
+int dm_array_new(struct dm_array_info *info, dm_block_t *root,
+		 uint32_t size, value_fn fn, void *context);
+
+/*
  * Frees a whole array.  The value_type's decrement operation will be called
  * for all values in the array
  */
@@ -163,4 +182,37 @@ int dm_array_walk(struct dm_array_info *info, dm_block_t root,
 
 /*----------------------------------------------------------------*/
 
+/*
+ * Cursor api.
+ *
+ * This lets you iterate through all the entries in an array efficiently
+ * (it will preload metadata).
+ *
+ * I'm using a cursor, rather than a walk function with a callback because
+ * the cache target needs to iterate both the mapping and hint arrays in
+ * unison.
+ */
+struct dm_array_cursor {
+	struct dm_array_info *info;
+	struct dm_btree_cursor cursor;
+
+	struct dm_block *block;
+	struct array_block *ab;
+	unsigned index;
+};
+
+int dm_array_cursor_begin(struct dm_array_info *info,
+			  dm_block_t root, struct dm_array_cursor *c);
+void dm_array_cursor_end(struct dm_array_cursor *c);
+
+uint32_t dm_array_cursor_index(struct dm_array_cursor *c);
+int dm_array_cursor_next(struct dm_array_cursor *c);
+
+/*
+ * value_le is only valid while the cursor points at the current value.
+ */
+void dm_array_cursor_get_value(struct dm_array_cursor *c, void **value_le);
+
+/*----------------------------------------------------------------*/
+
 #endif	/* _LINUX_DM_ARRAY_H */
diff --git a/drivers/md/persistent-data/dm-btree.c b/drivers/md/persistent-data/dm-btree.c
index 2cc1877804c2..20a40329d84a 100644
--- a/drivers/md/persistent-data/dm-btree.c
+++ b/drivers/md/persistent-data/dm-btree.c
@@ -994,3 +994,165 @@ int dm_btree_walk(struct dm_btree_info *info, dm_block_t root,
 	return walk_node(info, root, fn, context);
 }
 EXPORT_SYMBOL_GPL(dm_btree_walk);
+
+/*----------------------------------------------------------------*/
+
+static void prefetch_values(struct dm_btree_cursor *c)
+{
+	unsigned i, nr;
+	__le64 value_le;
+	struct cursor_node *n = c->nodes + c->depth - 1;
+	struct btree_node *bn = dm_block_data(n->b);
+	struct dm_block_manager *bm = dm_tm_get_bm(c->info->tm);
+
+	BUG_ON(c->info->value_type.size != sizeof(value_le));
+
+	nr = le32_to_cpu(bn->header.nr_entries);
+	for (i = 0; i < nr; i++) {
+		memcpy(&value_le, value_ptr(bn, i), sizeof(value_le));
+		dm_bm_prefetch(bm, le64_to_cpu(value_le));
+	}
+}
+
+static bool leaf_node(struct dm_btree_cursor *c)
+{
+	struct cursor_node *n = c->nodes + c->depth - 1;
+	struct btree_node *bn = dm_block_data(n->b);
+
+	return le32_to_cpu(bn->header.flags) & LEAF_NODE;
+}
+
+static int push_node(struct dm_btree_cursor *c, dm_block_t b)
+{
+	int r;
+	struct cursor_node *n = c->nodes + c->depth;
+
+	if (c->depth >= DM_BTREE_CURSOR_MAX_DEPTH - 1) {
+		DMERR("couldn't push cursor node, stack depth too high");
+		return -EINVAL;
+	}
+
+	r = bn_read_lock(c->info, b, &n->b);
+	if (r)
+		return r;
+
+	n->index = 0;
+	c->depth++;
+
+	if (c->prefetch_leaves || !leaf_node(c))
+		prefetch_values(c);
+
+	return 0;
+}
+
+static void pop_node(struct dm_btree_cursor *c)
+{
+	c->depth--;
+	unlock_block(c->info, c->nodes[c->depth].b);
+}
+
+static int inc_or_backtrack(struct dm_btree_cursor *c)
+{
+	struct cursor_node *n;
+	struct btree_node *bn;
+
+	for (;;) {
+		if (!c->depth)
+			return -ENODATA;
+
+		n = c->nodes + c->depth - 1;
+		bn = dm_block_data(n->b);
+
+		n->index++;
+		if (n->index < le32_to_cpu(bn->header.nr_entries))
+			break;
+
+		pop_node(c);
+	}
+
+	return 0;
+}
+
+static int find_leaf(struct dm_btree_cursor *c)
+{
+	int r = 0;
+	struct cursor_node *n;
+	struct btree_node *bn;
+	__le64 value_le;
+
+	for (;;) {
+		n = c->nodes + c->depth - 1;
+		bn = dm_block_data(n->b);
+
+		if (le32_to_cpu(bn->header.flags) & LEAF_NODE)
+			break;
+
+		memcpy(&value_le, value_ptr(bn, n->index), sizeof(value_le));
+		r = push_node(c, le64_to_cpu(value_le));
+		if (r) {
+			DMERR("push_node failed");
+			break;
+		}
+	}
+
+	if (!r && (le32_to_cpu(bn->header.nr_entries) == 0))
+		return -ENODATA;
+
+	return r;
+}
+
+int dm_btree_cursor_begin(struct dm_btree_info *info, dm_block_t root,
+			  bool prefetch_leaves, struct dm_btree_cursor *c)
+{
+	int r;
+
+	c->info = info;
+	c->root = root;
+	c->depth = 0;
+	c->prefetch_leaves = prefetch_leaves;
+
+	r = push_node(c, root);
+	if (r)
+		return r;
+
+	return find_leaf(c);
+}
+EXPORT_SYMBOL_GPL(dm_btree_cursor_begin);
+
+void dm_btree_cursor_end(struct dm_btree_cursor *c)
+{
+	while (c->depth)
+		pop_node(c);
+}
+EXPORT_SYMBOL_GPL(dm_btree_cursor_end);
+
+int dm_btree_cursor_next(struct dm_btree_cursor *c)
+{
+	int r = inc_or_backtrack(c);
+	if (!r) {
+		r = find_leaf(c);
+		if (r)
+			DMERR("find_leaf failed");
+	}
+
+	return r;
+}
+EXPORT_SYMBOL_GPL(dm_btree_cursor_next);
+
+int dm_btree_cursor_get_value(struct dm_btree_cursor *c, uint64_t *key, void *value_le)
+{
+	if (c->depth) {
+		struct cursor_node *n = c->nodes + c->depth - 1;
+		struct btree_node *bn = dm_block_data(n->b);
+
+		if (le32_to_cpu(bn->header.flags) & INTERNAL_NODE)
+			return -EINVAL;
+
+		*key = le64_to_cpu(*key_ptr(bn, n->index));
+		memcpy(value_le, value_ptr(bn, n->index), c->info->value_type.size);
+		return 0;
+
+	} else
+		return -ENODATA;
+}
+EXPORT_SYMBOL_GPL(dm_btree_cursor_get_value);
diff --git a/drivers/md/persistent-data/dm-btree.h b/drivers/md/persistent-data/dm-btree.h
index c74301fa5a37..db9bd26adf31 100644
--- a/drivers/md/persistent-data/dm-btree.h
+++ b/drivers/md/persistent-data/dm-btree.h
@@ -176,4 +176,39 @@ int dm_btree_walk(struct dm_btree_info *info, dm_block_t root,
 		  int (*fn)(void *context, uint64_t *keys, void *leaf),
 		  void *context);
 
+
+/*----------------------------------------------------------------*/
+
+/*
+ * Cursor API.  This does not follow the rolling lock convention.  Since we
+ * know the order that values are required we can issue prefetches to speed
+ * up iteration.  Use on a single level btree only.
+ */
+#define DM_BTREE_CURSOR_MAX_DEPTH 16
+
+struct cursor_node {
+	struct dm_block *b;
+	unsigned index;
+};
+
+struct dm_btree_cursor {
+	struct dm_btree_info *info;
+	dm_block_t root;
+
+	bool prefetch_leaves;
+	unsigned depth;
+	struct cursor_node nodes[DM_BTREE_CURSOR_MAX_DEPTH];
+};
+
+/*
+ * Creates a fresh cursor.  If prefetch_leaves is set then it is assumed
+ * the btree contains block indexes that will be prefetched.  The cursor is
+ * quite large, so you probably don't want to put it on the stack.
+ */
+int dm_btree_cursor_begin(struct dm_btree_info *info, dm_block_t root,
+			  bool prefetch_leaves, struct dm_btree_cursor *c);
+void dm_btree_cursor_end(struct dm_btree_cursor *c);
+int dm_btree_cursor_next(struct dm_btree_cursor *c);
+int dm_btree_cursor_get_value(struct dm_btree_cursor *c, uint64_t *key, void *value_le);
+
 #endif	/* _LINUX_DM_BTREE_H */
diff --git a/drivers/md/raid1.c b/drivers/md/raid1.c
index 21dc00eb1989..1961d827dbd1 100644
--- a/drivers/md/raid1.c
+++ b/drivers/md/raid1.c
@@ -145,12 +145,8 @@ static void * r1buf_pool_alloc(gfp_t gfp_flags, void *data)
 	return r1_bio;
 
 out_free_pages:
-	while (--j >= 0) {
-		struct bio_vec *bv;
-
-		bio_for_each_segment_all(bv, r1_bio->bios[j], i)
-			__free_page(bv->bv_page);
-	}
+	while (--j >= 0)
+		bio_free_pages(r1_bio->bios[j]);
 
 out_free_bio:
 	while (++j < pi->raid_disks)
diff --git a/drivers/md/raid5.c b/drivers/md/raid5.c
index ee7fc3701700..92ac251e91e6 100644
--- a/drivers/md/raid5.c
+++ b/drivers/md/raid5.c
@@ -6349,22 +6349,20 @@ static int alloc_scratch_buffer(struct r5conf *conf, struct raid5_percpu *percpu
 	return 0;
 }
 
-static void raid5_free_percpu(struct r5conf *conf)
+static int raid456_cpu_dead(unsigned int cpu, struct hlist_node *node)
 {
-	unsigned long cpu;
+	struct r5conf *conf = hlist_entry_safe(node, struct r5conf, node);
+
+	free_scratch_buffer(conf, per_cpu_ptr(conf->percpu, cpu));
+	return 0;
+}
 
+static void raid5_free_percpu(struct r5conf *conf)
+{
 	if (!conf->percpu)
 		return;
 
-#ifdef CONFIG_HOTPLUG_CPU
-	unregister_cpu_notifier(&conf->cpu_notify);
-#endif
-
-	get_online_cpus();
-	for_each_possible_cpu(cpu)
-		free_scratch_buffer(conf, per_cpu_ptr(conf->percpu, cpu));
-	put_online_cpus();
-
+	cpuhp_state_remove_instance(CPUHP_MD_RAID5_PREPARE, &conf->node);
 	free_percpu(conf->percpu);
 }
 
@@ -6372,7 +6370,7 @@ static void free_conf(struct r5conf *conf)
 {
 	if (conf->log)
 		r5l_exit_log(conf->log);
-	if (conf->shrinker.seeks)
+	if (conf->shrinker.nr_deferred)
 		unregister_shrinker(&conf->shrinker);
 
 	free_thread_groups(conf);
@@ -6383,64 +6381,28 @@ static void free_conf(struct r5conf *conf)
 	kfree(conf);
 }
 
-#ifdef CONFIG_HOTPLUG_CPU
-static int raid456_cpu_notify(struct notifier_block *nfb, unsigned long action,
-			      void *hcpu)
+static int raid456_cpu_up_prepare(unsigned int cpu, struct hlist_node *node)
 {
-	struct r5conf *conf = container_of(nfb, struct r5conf, cpu_notify);
-	long cpu = (long)hcpu;
+	struct r5conf *conf = hlist_entry_safe(node, struct r5conf, node);
 	struct raid5_percpu *percpu = per_cpu_ptr(conf->percpu, cpu);
 
-	switch (action) {
-	case CPU_UP_PREPARE:
-	case CPU_UP_PREPARE_FROZEN:
-		if (alloc_scratch_buffer(conf, percpu)) {
-			pr_err("%s: failed memory allocation for cpu%ld\n",
-			       __func__, cpu);
-			return notifier_from_errno(-ENOMEM);
-		}
-		break;
-	case CPU_DEAD:
-	case CPU_DEAD_FROZEN:
-	case CPU_UP_CANCELED:
-	case CPU_UP_CANCELED_FROZEN:
-		free_scratch_buffer(conf, per_cpu_ptr(conf->percpu, cpu));
-		break;
-	default:
-		break;
+	if (alloc_scratch_buffer(conf, percpu)) {
+		pr_err("%s: failed memory allocation for cpu%u\n",
+		       __func__, cpu);
+		return -ENOMEM;
 	}
-	return NOTIFY_OK;
+	return 0;
 }
-#endif
 
 static int raid5_alloc_percpu(struct r5conf *conf)
 {
-	unsigned long cpu;
 	int err = 0;
 
 	conf->percpu = alloc_percpu(struct raid5_percpu);
 	if (!conf->percpu)
 		return -ENOMEM;
 
-#ifdef CONFIG_HOTPLUG_CPU
-	conf->cpu_notify.notifier_call = raid456_cpu_notify;
-	conf->cpu_notify.priority = 0;
-	err = register_cpu_notifier(&conf->cpu_notify);
-	if (err)
-		return err;
-#endif
-
-	get_online_cpus();
-	for_each_present_cpu(cpu) {
-		err = alloc_scratch_buffer(conf, per_cpu_ptr(conf->percpu, cpu));
-		if (err) {
-			pr_err("%s: failed memory allocation for cpu%ld\n",
-			       __func__, cpu);
-			break;
-		}
-	}
-	put_online_cpus();
-
+	err = cpuhp_state_add_instance(CPUHP_MD_RAID5_PREPARE, &conf->node);
 	if (!err) {
 		conf->scribble_disks = max(conf->raid_disks,
 			conf->previous_raid_disks);
@@ -6670,7 +6632,12 @@ static struct r5conf *setup_conf(struct mddev *mddev)
 	conf->shrinker.count_objects = raid5_cache_count;
 	conf->shrinker.batch = 128;
 	conf->shrinker.flags = 0;
-	register_shrinker(&conf->shrinker);
+	if (register_shrinker(&conf->shrinker)) {
+		printk(KERN_ERR
+		       "md/raid:%s: couldn't register shrinker.\n",
+		       mdname(mddev));
+		goto abort;
+	}
 
 	sprintf(pers_name, "raid%d", mddev->new_level);
 	conf->thread = md_register_thread(raid5d, mddev, pers_name);
@@ -7066,6 +7033,8 @@ static int raid5_run(struct mddev *mddev)
 		else
 			queue_flag_clear_unlocked(QUEUE_FLAG_DISCARD,
 						mddev->queue);
+
+		blk_queue_max_hw_sectors(mddev->queue, UINT_MAX);
 	}
 
 	if (journal_dev) {
@@ -7985,10 +7954,21 @@ static struct md_personality raid4_personality =
 
 static int __init raid5_init(void)
 {
+	int ret;
+
 	raid5_wq = alloc_workqueue("raid5wq",
 		WQ_UNBOUND|WQ_MEM_RECLAIM|WQ_CPU_INTENSIVE|WQ_SYSFS, 0);
 	if (!raid5_wq)
 		return -ENOMEM;
+
+	ret = cpuhp_setup_state_multi(CPUHP_MD_RAID5_PREPARE,
+				      "md/raid5:prepare",
+				      raid456_cpu_up_prepare,
+				      raid456_cpu_dead);
+	if (ret) {
+		destroy_workqueue(raid5_wq);
+		return ret;
+	}
 	register_md_personality(&raid6_personality);
 	register_md_personality(&raid5_personality);
 	register_md_personality(&raid4_personality);
@@ -8000,6 +7980,7 @@ static void raid5_exit(void)
 	unregister_md_personality(&raid6_personality);
 	unregister_md_personality(&raid5_personality);
 	unregister_md_personality(&raid4_personality);
+	cpuhp_remove_multi_state(CPUHP_MD_RAID5_PREPARE);
 	destroy_workqueue(raid5_wq);
 }
 
diff --git a/drivers/md/raid5.h b/drivers/md/raid5.h
index 517d4b68a1be..57ec49f0839e 100644
--- a/drivers/md/raid5.h
+++ b/drivers/md/raid5.h
@@ -512,9 +512,7 @@ struct r5conf {
 	} __percpu *percpu;
 	int scribble_disks;
 	int scribble_sectors;
-#ifdef CONFIG_HOTPLUG_CPU
-	struct notifier_block	cpu_notify;
-#endif
+	struct hlist_node node;
 
 	/*
 	 * Free stripes pool
diff --git a/drivers/media/Kconfig b/drivers/media/Kconfig
index 962f2a9a6614..7b8540291217 100644
--- a/drivers/media/Kconfig
+++ b/drivers/media/Kconfig
@@ -180,14 +180,14 @@ source "drivers/media/firewire/Kconfig"
 # Common driver options
 source "drivers/media/common/Kconfig"
 
-comment "Media ancillary drivers (tuners, sensors, i2c, frontends)"
+comment "Media ancillary drivers (tuners, sensors, i2c, spi, frontends)"
 
 #
-# Ancillary drivers (tuners, i2c, frontends)
+# Ancillary drivers (tuners, i2c, spi, frontends)
 #
 
 config MEDIA_SUBDRV_AUTOSELECT
-	bool "Autoselect ancillary drivers (tuners, sensors, i2c, frontends)"
+	bool "Autoselect ancillary drivers (tuners, sensors, i2c, spi, frontends)"
 	depends on MEDIA_ANALOG_TV_SUPPORT || MEDIA_DIGITAL_TV_SUPPORT || MEDIA_CAMERA_SUPPORT || MEDIA_SDR_SUPPORT
 	depends on HAS_IOMEM
 	select I2C
@@ -216,6 +216,7 @@ config MEDIA_ATTACH
 	default MODULES
 
 source "drivers/media/i2c/Kconfig"
+source "drivers/media/spi/Kconfig"
 source "drivers/media/tuners/Kconfig"
 source "drivers/media/dvb-frontends/Kconfig"
 
diff --git a/drivers/media/Makefile b/drivers/media/Makefile
index 081a7866fd44..0deaa93efdee 100644
--- a/drivers/media/Makefile
+++ b/drivers/media/Makefile
@@ -32,6 +32,6 @@ obj-y += rc/
 # Finally, merge the drivers that require the core
 #
 
-obj-y += common/ platform/ pci/ usb/ mmc/ firewire/
+obj-y += common/ platform/ pci/ usb/ mmc/ firewire/ spi/
 obj-$(CONFIG_VIDEO_DEV) += radio/
 
diff --git a/drivers/media/common/v4l2-tpg/v4l2-tpg-core.c b/drivers/media/common/v4l2-tpg/v4l2-tpg-core.c
index 3ec3cebe62b9..1684810cab83 100644
--- a/drivers/media/common/v4l2-tpg/v4l2-tpg-core.c
+++ b/drivers/media/common/v4l2-tpg/v4l2-tpg-core.c
@@ -504,14 +504,14 @@ static void color_to_ycbcr(struct tpg_data *tpg, int r, int g, int b,
 #define COEFF(v, r) ((int)(0.5 + (v) * (r) * 256.0))
 
 	static const int bt601[3][3] = {
-		{ COEFF(0.299, 219),  COEFF(0.587, 219),  COEFF(0.114, 219)  },
-		{ COEFF(-0.169, 224), COEFF(-0.331, 224), COEFF(0.5, 224)    },
-		{ COEFF(0.5, 224),    COEFF(-0.419, 224), COEFF(-0.081, 224) },
+		{ COEFF(0.299, 219),   COEFF(0.587, 219),   COEFF(0.114, 219)   },
+		{ COEFF(-0.1687, 224), COEFF(-0.3313, 224), COEFF(0.5, 224)     },
+		{ COEFF(0.5, 224),     COEFF(-0.4187, 224), COEFF(-0.0813, 224) },
 	};
 	static const int bt601_full[3][3] = {
-		{ COEFF(0.299, 255),  COEFF(0.587, 255),  COEFF(0.114, 255)  },
-		{ COEFF(-0.169, 255), COEFF(-0.331, 255), COEFF(0.5, 255)    },
-		{ COEFF(0.5, 255),    COEFF(-0.419, 255), COEFF(-0.081, 255) },
+		{ COEFF(0.299, 255),   COEFF(0.587, 255),   COEFF(0.114, 255)   },
+		{ COEFF(-0.1687, 255), COEFF(-0.3313, 255), COEFF(0.5, 255)     },
+		{ COEFF(0.5, 255),     COEFF(-0.4187, 255), COEFF(-0.0813, 255) },
 	};
 	static const int rec709[3][3] = {
 		{ COEFF(0.2126, 219),  COEFF(0.7152, 219),  COEFF(0.0722, 219)  },
@@ -558,7 +558,6 @@ static void color_to_ycbcr(struct tpg_data *tpg, int r, int g, int b,
 
 	switch (tpg->real_ycbcr_enc) {
 	case V4L2_YCBCR_ENC_601:
-	case V4L2_YCBCR_ENC_SYCC:
 		rgb2ycbcr(full ? bt601_full : bt601, r, g, b, y_offset, y, cb, cr);
 		break;
 	case V4L2_YCBCR_ENC_XV601:
@@ -674,7 +673,6 @@ static void ycbcr_to_color(struct tpg_data *tpg, int y, int cb, int cr,
 
 	switch (tpg->real_ycbcr_enc) {
 	case V4L2_YCBCR_ENC_601:
-	case V4L2_YCBCR_ENC_SYCC:
 		ycbcr2rgb(full ? bt601_full : bt601, y, cb, cr, y_offset, r, g, b);
 		break;
 	case V4L2_YCBCR_ENC_XV601:
diff --git a/drivers/media/dvb-core/demux.h b/drivers/media/dvb-core/demux.h
index 4b4c1da20f4b..aeda2b64931c 100644
--- a/drivers/media/dvb-core/demux.h
+++ b/drivers/media/dvb-core/demux.h
@@ -202,7 +202,7 @@ struct dmx_section_feed {
  *
  * This function callback prototype, provided by the client of the demux API,
  * is called from the demux code. The function is only called when filtering
- * on ae TS feed has been enabled using the start_filtering() function at
+ * on a TS feed has been enabled using the start_filtering\(\) function at
  * the &dmx_demux.
  * Any TS packets that match the filter settings are copied to a circular
  * buffer. The filtered TS packets are delivered to the client using this
@@ -243,8 +243,10 @@ struct dmx_section_feed {
  * will also be sent to the hardware MPEG decoder.
  *
  * Return:
- * 	0, on success;
- * 	-EOVERFLOW, on buffer overflow.
+ *
+ * - 0, on success;
+ *
+ * - -EOVERFLOW, on buffer overflow.
  */
 typedef int (*dmx_ts_cb)(const u8 *buffer1,
 			 size_t buffer1_length,
@@ -293,9 +295,9 @@ typedef int (*dmx_section_cb)(const u8 *buffer1,
 			      size_t buffer2_len,
 			      struct dmx_section_filter *source);
 
-/*--------------------------------------------------------------------------*/
-/* DVB Front-End */
-/*--------------------------------------------------------------------------*/
+/*
+ * DVB Front-End
+ */
 
 /**
  * enum dmx_frontend_source - Used to identify the type of frontend
@@ -349,15 +351,15 @@ enum dmx_demux_caps {
 
 /*
  * Demux resource type identifier.
-*/
-
-/*
- * DMX_FE_ENTRY(): Casts elements in the list of registered
- * front-ends from the generic type struct list_head
- * to the type * struct dmx_frontend
- *.
-*/
+ */
 
+/**
+ * DMX_FE_ENTRY - Casts elements in the list of registered
+ *		  front-ends from the generic type struct list_head
+ *		  to the type * struct dmx_frontend
+ *
+ * @list: list of struct dmx_frontend
+ */
 #define DMX_FE_ENTRY(list) \
 	list_entry(list, struct dmx_frontend, connectivity_list)
 
@@ -551,7 +553,6 @@ enum dmx_demux_caps {
  *	0 on success;
  *	-EINVAL on bad parameter.
  */
-
 struct dmx_demux {
 	enum dmx_demux_caps capabilities;
 	struct dmx_frontend *frontend;
@@ -581,15 +582,12 @@ struct dmx_demux {
 
 	int (*get_pes_pids)(struct dmx_demux *demux, u16 *pids);
 
-	/* private: Not used upstream and never documented */
-#if 0
-	int (*get_caps)(struct dmx_demux *demux, struct dmx_caps *caps);
-	int (*set_source)(struct dmx_demux *demux, const dmx_source_t *src);
-#endif
+	/* private: */
+
 	/*
-	 * private: Only used at av7110, to read some data from firmware.
-	 *	As this was never documented, we have no clue about what's
-	 *	there, and its usage on other drivers aren't encouraged.
+	 * Only used at av7110, to read some data from firmware.
+	 * As this was never documented, we have no clue about what's
+	 * there, and its usage on other drivers aren't encouraged.
 	 */
 	int (*get_stc)(struct dmx_demux *demux, unsigned int num,
 		       u64 *stc, unsigned int *base);
diff --git a/drivers/media/dvb-core/dvb_frontend.c b/drivers/media/dvb-core/dvb_frontend.c
index be99c8dbc5f8..01511e5a5566 100644
--- a/drivers/media/dvb-core/dvb_frontend.c
+++ b/drivers/media/dvb-core/dvb_frontend.c
@@ -1969,17 +1969,9 @@ static int dvb_frontend_ioctl_properties(struct file *file,
 		if ((tvps->num == 0) || (tvps->num > DTV_IOCTL_MAX_MSGS))
 			return -EINVAL;
 
-		tvp = kmalloc(tvps->num * sizeof(struct dtv_property), GFP_KERNEL);
-		if (!tvp) {
-			err = -ENOMEM;
-			goto out;
-		}
-
-		if (copy_from_user(tvp, (void __user *)tvps->props,
-				   tvps->num * sizeof(struct dtv_property))) {
-			err = -EFAULT;
-			goto out;
-		}
+		tvp = memdup_user(tvps->props, tvps->num * sizeof(*tvp));
+		if (IS_ERR(tvp))
+			return PTR_ERR(tvp);
 
 		for (i = 0; i < tvps->num; i++) {
 			err = dtv_property_process_set(fe, tvp + i, file);
@@ -2002,17 +1994,9 @@ static int dvb_frontend_ioctl_properties(struct file *file,
 		if ((tvps->num == 0) || (tvps->num > DTV_IOCTL_MAX_MSGS))
 			return -EINVAL;
 
-		tvp = kmalloc(tvps->num * sizeof(struct dtv_property), GFP_KERNEL);
-		if (!tvp) {
-			err = -ENOMEM;
-			goto out;
-		}
-
-		if (copy_from_user(tvp, (void __user *)tvps->props,
-				   tvps->num * sizeof(struct dtv_property))) {
-			err = -EFAULT;
-			goto out;
-		}
+		tvp = memdup_user(tvps->props, tvps->num * sizeof(*tvp));
+		if (IS_ERR(tvp))
+			return PTR_ERR(tvp);
 
 		/*
 		 * Let's use our own copy of property cache, in order to
diff --git a/drivers/media/dvb-core/dvb_math.h b/drivers/media/dvb-core/dvb_math.h
index 2f0326674ca6..4d11d3529c14 100644
--- a/drivers/media/dvb-core/dvb_math.h
+++ b/drivers/media/dvb-core/dvb_math.h
@@ -25,7 +25,7 @@
 #include <linux/types.h>
 
 /**
- * cintlog2 - computes log2 of a value; the result is shifted left by 24 bits
+ * intlog2 - computes log2 of a value; the result is shifted left by 24 bits
  *
  * @value: The value (must be != 0)
  *
diff --git a/drivers/media/dvb-core/dvb_ringbuffer.h b/drivers/media/dvb-core/dvb_ringbuffer.h
index 8af642399f1e..bbe94873d44d 100644
--- a/drivers/media/dvb-core/dvb_ringbuffer.h
+++ b/drivers/media/dvb-core/dvb_ringbuffer.h
@@ -18,10 +18,6 @@
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU Lesser General Public License for more details.
- *
- * You should have received a copy of the GNU Lesser General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
  */
 
 #ifndef _DVB_RINGBUFFER_H_
@@ -30,6 +26,18 @@
 #include <linux/spinlock.h>
 #include <linux/wait.h>
 
+/**
+ * struct dvb_ringbuffer - Describes a ring buffer used at DVB framework
+ *
+ * @data: Area were the ringbuffer data is written
+ * @size: size of the ringbuffer
+ * @pread: next position to read
+ * @pwrite: next position to write
+ * @error: used by ringbuffer clients to indicate that an error happened.
+ * @queue: Wait queue used by ringbuffer clients to indicate when buffer
+ *         was filled
+ * @lock: Spinlock used to protect the ringbuffer
+ */
 struct dvb_ringbuffer {
 	u8               *data;
 	ssize_t           size;
@@ -43,99 +51,161 @@ struct dvb_ringbuffer {
 
 #define DVB_RINGBUFFER_PKTHDRSIZE 3
 
+/**
+ * dvb_ringbuffer_init - initialize ring buffer, lock and queue
+ *
+ * @rbuf: pointer to struct dvb_ringbuffer
+ * @data: pointer to the buffer where the data will be stored
+ * @len: bytes from ring buffer into @buf
+ */
+extern void dvb_ringbuffer_init(struct dvb_ringbuffer *rbuf, void *data,
+				size_t len);
 
-/*
- * Notes:
- * ------
- * (1) For performance reasons read and write routines don't check buffer sizes
- *     and/or number of bytes free/available. This has to be done before these
- *     routines are called. For example:
- *
- *     *** write @buflen: bytes ***
- *     free = dvb_ringbuffer_free(rbuf);
- *     if (free >= buflen)
- *         count = dvb_ringbuffer_write(rbuf, buffer, buflen);
- *     else
- *         ...
- *
- *     *** read min. 1000, max. @bufsize: bytes ***
- *     avail = dvb_ringbuffer_avail(rbuf);
- *     if (avail >= 1000)
- *         count = dvb_ringbuffer_read(rbuf, buffer, min(avail, bufsize));
- *     else
- *         ...
- *
- * (2) If there is exactly one reader and one writer, there is no need
- *     to lock read or write operations.
- *     Two or more readers must be locked against each other.
- *     Flushing the buffer counts as a read operation.
- *     Resetting the buffer counts as a read and write operation.
- *     Two or more writers must be locked against each other.
- */
-
-/* initialize ring buffer, lock and queue */
-extern void dvb_ringbuffer_init(struct dvb_ringbuffer *rbuf, void *data, size_t len);
-
-/* test whether buffer is empty */
+/**
+ * dvb_ringbuffer_empty - test whether buffer is empty
+ *
+ * @rbuf: pointer to struct dvb_ringbuffer
+ */
 extern int dvb_ringbuffer_empty(struct dvb_ringbuffer *rbuf);
 
-/* return the number of free bytes in the buffer */
+/**
+ * dvb_ringbuffer_free - returns the number of free bytes in the buffer
+ *
+ * @rbuf: pointer to struct dvb_ringbuffer
+ *
+ * Return: number of free bytes in the buffer
+ */
 extern ssize_t dvb_ringbuffer_free(struct dvb_ringbuffer *rbuf);
 
-/* return the number of bytes waiting in the buffer */
+/**
+ * dvb_ringbuffer_avail - returns the number of bytes waiting in the buffer
+ *
+ * @rbuf: pointer to struct dvb_ringbuffer
+ *
+ * Return: number of bytes waiting in the buffer
+ */
 extern ssize_t dvb_ringbuffer_avail(struct dvb_ringbuffer *rbuf);
 
-
-/*
- * Reset the read and write pointers to zero and flush the buffer
+/**
+ * dvb_ringbuffer_reset - resets the ringbuffer to initial state
+ *
+ * @rbuf: pointer to struct dvb_ringbuffer
+ *
+ * Resets the read and write pointers to zero and flush the buffer.
+ *
  * This counts as a read and write operation
  */
 extern void dvb_ringbuffer_reset(struct dvb_ringbuffer *rbuf);
 
+/*
+ * read routines & macros
+ */
 
-/* read routines & macros */
-/* ---------------------- */
-/* flush buffer */
+/**
+ * dvb_ringbuffer_flush - flush buffer
+ *
+ * @rbuf: pointer to struct dvb_ringbuffer
+ */
 extern void dvb_ringbuffer_flush(struct dvb_ringbuffer *rbuf);
 
-/* flush buffer protected by spinlock and wake-up waiting task(s) */
+/**
+ * dvb_ringbuffer_flush_spinlock_wakeup- flush buffer protected by spinlock
+ *      and wake-up waiting task(s)
+ *
+ * @rbuf: pointer to struct dvb_ringbuffer
+ */
 extern void dvb_ringbuffer_flush_spinlock_wakeup(struct dvb_ringbuffer *rbuf);
 
-/* peek at byte @offs: in the buffer */
-#define DVB_RINGBUFFER_PEEK(rbuf,offs)	\
-			(rbuf)->data[((rbuf)->pread+(offs))%(rbuf)->size]
+/**
+ * DVB_RINGBUFFER_PEEK - peek at byte @offs in the buffer
+ *
+ * @rbuf: pointer to struct dvb_ringbuffer
+ * @offs: offset inside the ringbuffer
+ */
+#define DVB_RINGBUFFER_PEEK(rbuf, offs)	\
+			((rbuf)->data[((rbuf)->pread + (offs)) % (rbuf)->size])
 
-/* advance read ptr by @num: bytes */
-#define DVB_RINGBUFFER_SKIP(rbuf,num)	\
-			(rbuf)->pread=((rbuf)->pread+(num))%(rbuf)->size
+/**
+ * DVB_RINGBUFFER_SKIP - advance read ptr by @num bytes
+ *
+ * @rbuf: pointer to struct dvb_ringbuffer
+ * @num: number of bytes to advance
+ */
+#define DVB_RINGBUFFER_SKIP(rbuf, num)	{\
+			(rbuf)->pread = ((rbuf)->pread + (num)) % (rbuf)->size;\
+}
 
-/*
- * read @len: bytes from ring buffer into @buf:
- * @usermem: specifies whether @buf: resides in user space
- * returns number of bytes transferred or -EFAULT
+/**
+ * dvb_ringbuffer_read_user - Reads a buffer into an user pointer
+ *
+ * @rbuf: pointer to struct dvb_ringbuffer
+ * @buf: pointer to the buffer where the data will be stored
+ * @len: bytes from ring buffer into @buf
+ *
+ * This variant assumes that the buffer is a memory at the userspace. So,
+ * it will internally call copy_to_user().
+ *
+ * Return: number of bytes transferred or -EFAULT
  */
 extern ssize_t dvb_ringbuffer_read_user(struct dvb_ringbuffer *rbuf,
 				   u8 __user *buf, size_t len);
+
+/**
+ * dvb_ringbuffer_read - Reads a buffer into a pointer
+ *
+ * @rbuf: pointer to struct dvb_ringbuffer
+ * @buf: pointer to the buffer where the data will be stored
+ * @len: bytes from ring buffer into @buf
+ *
+ * This variant assumes that the buffer is a memory at the Kernel space
+ *
+ * Return: number of bytes transferred or -EFAULT
+ */
 extern void dvb_ringbuffer_read(struct dvb_ringbuffer *rbuf,
 				   u8 *buf, size_t len);
 
-
-/* write routines & macros */
-/* ----------------------- */
-/* write single byte to ring buffer */
-#define DVB_RINGBUFFER_WRITE_BYTE(rbuf,byte)	\
-			{ (rbuf)->data[(rbuf)->pwrite]=(byte); \
-			(rbuf)->pwrite=((rbuf)->pwrite+1)%(rbuf)->size; }
 /*
- * write @len: bytes to ring buffer
- * @usermem: specifies whether @buf: resides in user space
- * returns number of bytes transferred or -EFAULT
-*/
+ * write routines & macros
+ */
+
+/**
+ * DVB_RINGBUFFER_WRITE_BYTE - write single byte to ring buffer
+ *
+ * @rbuf: pointer to struct dvb_ringbuffer
+ * @byte: byte to write
+ */
+#define DVB_RINGBUFFER_WRITE_BYTE(rbuf, byte)	\
+			{ (rbuf)->data[(rbuf)->pwrite] = (byte); \
+			(rbuf)->pwrite = ((rbuf)->pwrite + 1) % (rbuf)->size; }
+
+/**
+ * dvb_ringbuffer_write - Writes a buffer into the ringbuffer
+ *
+ * @rbuf: pointer to struct dvb_ringbuffer
+ * @buf: pointer to the buffer where the data will be read
+ * @len: bytes from ring buffer into @buf
+ *
+ * This variant assumes that the buffer is a memory at the Kernel space
+ *
+ * return: number of bytes transferred or -EFAULT
+ */
 extern ssize_t dvb_ringbuffer_write(struct dvb_ringbuffer *rbuf, const u8 *buf,
 				    size_t len);
-extern ssize_t dvb_ringbuffer_write_user(struct dvb_ringbuffer *rbuf,
-				         const u8 __user *buf, size_t len);
 
+/**
+ * dvb_ringbuffer_write_user - Writes a buffer received via an user pointer
+ *
+ * @rbuf: pointer to struct dvb_ringbuffer
+ * @buf: pointer to the buffer where the data will be read
+ * @len: bytes from ring buffer into @buf
+ *
+ * This variant assumes that the buffer is a memory at the userspace. So,
+ * it will internally call copy_from_user().
+ *
+ * Return: number of bytes transferred or -EFAULT
+ */
+extern ssize_t dvb_ringbuffer_write_user(struct dvb_ringbuffer *rbuf,
+					 const u8 __user *buf, size_t len);
 
 /**
  * dvb_ringbuffer_pkt_write - Write a packet into the ringbuffer.
@@ -143,9 +213,10 @@ extern ssize_t dvb_ringbuffer_write_user(struct dvb_ringbuffer *rbuf,
  * @rbuf: Ringbuffer to write to.
  * @buf: Buffer to write.
  * @len: Length of buffer (currently limited to 65535 bytes max).
- * returns Number of bytes written, or -EFAULT, -ENOMEM, -EVINAL.
+ *
+ * Return: Number of bytes written, or -EFAULT, -ENOMEM, -EVINAL.
  */
-extern ssize_t dvb_ringbuffer_pkt_write(struct dvb_ringbuffer *rbuf, u8* buf,
+extern ssize_t dvb_ringbuffer_pkt_write(struct dvb_ringbuffer *rbuf, u8 *buf,
 					size_t len);
 
 /**
@@ -157,7 +228,7 @@ extern ssize_t dvb_ringbuffer_pkt_write(struct dvb_ringbuffer *rbuf, u8* buf,
  * @buf: Destination buffer for data.
  * @len: Size of destination buffer.
  *
- * returns Number of bytes read, or -EFAULT.
+ * Return: Number of bytes read, or -EFAULT.
  *
  * .. note::
  *
@@ -167,7 +238,7 @@ extern ssize_t dvb_ringbuffer_pkt_write(struct dvb_ringbuffer *rbuf, u8* buf,
  */
 extern ssize_t dvb_ringbuffer_pkt_read_user(struct dvb_ringbuffer *rbuf,
 					    size_t idx,
-				            int offset, u8 __user *buf,
+					    int offset, u8 __user *buf,
 					    size_t len);
 
 /**
@@ -181,7 +252,7 @@ extern ssize_t dvb_ringbuffer_pkt_read_user(struct dvb_ringbuffer *rbuf,
  * @buf: Destination buffer for data.
  * @len: Size of destination buffer.
  *
- * returns Number of bytes read, or -EFAULT.
+ * Return: Number of bytes read, or -EFAULT.
  */
 extern ssize_t dvb_ringbuffer_pkt_read(struct dvb_ringbuffer *rbuf, size_t idx,
 				       int offset, u8 *buf, size_t len);
@@ -199,10 +270,11 @@ extern void dvb_ringbuffer_pkt_dispose(struct dvb_ringbuffer *rbuf, size_t idx);
  *
  * @rbuf: Ringbuffer concerned.
  * @idx: Previous packet index, or -1 to return the first packet index.
- * @pktlen: On success, will be updated to contain the length of the packet in bytes.
+ * @pktlen: On success, will be updated to contain the length of the packet
+ *          in bytes.
  * returns Packet index (if >=0), or -1 if no packets available.
  */
-extern ssize_t dvb_ringbuffer_pkt_next(struct dvb_ringbuffer *rbuf, size_t idx, size_t* pktlen);
-
+extern ssize_t dvb_ringbuffer_pkt_next(struct dvb_ringbuffer *rbuf,
+				       size_t idx, size_t *pktlen);
 
 #endif /* _DVB_RINGBUFFER_H_ */
diff --git a/drivers/media/dvb-frontends/Kconfig b/drivers/media/dvb-frontends/Kconfig
index c645aa81f423..012225587c25 100644
--- a/drivers/media/dvb-frontends/Kconfig
+++ b/drivers/media/dvb-frontends/Kconfig
@@ -67,6 +67,7 @@ config DVB_TDA18271C2DD
 config DVB_SI2165
 	tristate "Silicon Labs si2165 based"
 	depends on DVB_CORE && I2C
+	select REGMAP_I2C
 	default m if !MEDIA_SUBDRV_AUTOSELECT
 	help
 	  A DVB-C/T demodulator.
@@ -463,6 +464,7 @@ config DVB_STV0367
 config DVB_CXD2820R
 	tristate "Sony CXD2820R"
 	depends on DVB_CORE && I2C
+	select REGMAP_I2C
 	default m if !MEDIA_SUBDRV_AUTOSELECT
 	help
 	  Say Y when you want to support this frontend.
diff --git a/drivers/media/dvb-frontends/af9013.h b/drivers/media/dvb-frontends/af9013.h
index 1dcc936e1661..dcdd163ace85 100644
--- a/drivers/media/dvb-frontends/af9013.h
+++ b/drivers/media/dvb-frontends/af9013.h
@@ -25,7 +25,6 @@
 #ifndef AF9013_H
 #define AF9013_H
 
-#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 /* AF9013/5 GPIOs (mostly guessed)
diff --git a/drivers/media/dvb-frontends/af9033.h b/drivers/media/dvb-frontends/af9033.h
index 6ad22b69a636..5b83e4f96297 100644
--- a/drivers/media/dvb-frontends/af9033.h
+++ b/drivers/media/dvb-frontends/af9033.h
@@ -22,8 +22,6 @@
 #ifndef AF9033_H
 #define AF9033_H
 
-#include <linux/kconfig.h>
-
 /*
  * I2C address (TODO: are these in 8-bit format?)
  * 0x38, 0x3a, 0x3c, 0x3e
diff --git a/drivers/media/dvb-frontends/ascot2e.c b/drivers/media/dvb-frontends/ascot2e.c
index 8cc8c4597b6a..ad304eed656d 100644
--- a/drivers/media/dvb-frontends/ascot2e.c
+++ b/drivers/media/dvb-frontends/ascot2e.c
@@ -464,7 +464,7 @@ static int ascot2e_get_frequency(struct dvb_frontend *fe, u32 *frequency)
 	return 0;
 }
 
-static struct dvb_tuner_ops ascot2e_tuner_ops = {
+static const struct dvb_tuner_ops ascot2e_tuner_ops = {
 	.info = {
 		.name = "Sony ASCOT2E",
 		.frequency_min = 1000000,
diff --git a/drivers/media/dvb-frontends/ascot2e.h b/drivers/media/dvb-frontends/ascot2e.h
index 6da4ae6d6cc3..dc61bf7d1b09 100644
--- a/drivers/media/dvb-frontends/ascot2e.h
+++ b/drivers/media/dvb-frontends/ascot2e.h
@@ -22,7 +22,6 @@
 #ifndef __DVB_ASCOT2E_H__
 #define __DVB_ASCOT2E_H__
 
-#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 #include <linux/i2c.h>
 
diff --git a/drivers/media/dvb-frontends/atbm8830.h b/drivers/media/dvb-frontends/atbm8830.h
index 5446d13fdfe8..bb862387080f 100644
--- a/drivers/media/dvb-frontends/atbm8830.h
+++ b/drivers/media/dvb-frontends/atbm8830.h
@@ -22,7 +22,6 @@
 #ifndef __ATBM8830_H__
 #define __ATBM8830_H__
 
-#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 #include <linux/i2c.h>
 
diff --git a/drivers/media/dvb-frontends/au8522.h b/drivers/media/dvb-frontends/au8522.h
index 78bf3f73e58d..21c51a4c519a 100644
--- a/drivers/media/dvb-frontends/au8522.h
+++ b/drivers/media/dvb-frontends/au8522.h
@@ -22,7 +22,6 @@
 #ifndef __AU8522_H__
 #define __AU8522_H__
 
-#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 enum au8522_if_freq {
diff --git a/drivers/media/dvb-frontends/cx22702.h b/drivers/media/dvb-frontends/cx22702.h
index 68b69a7660d2..a1956a9ba406 100644
--- a/drivers/media/dvb-frontends/cx22702.h
+++ b/drivers/media/dvb-frontends/cx22702.h
@@ -28,7 +28,6 @@
 #ifndef CX22702_H
 #define CX22702_H
 
-#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 struct cx22702_config {
diff --git a/drivers/media/dvb-frontends/cx24113.h b/drivers/media/dvb-frontends/cx24113.h
index 962919b9b6e6..194c703611b4 100644
--- a/drivers/media/dvb-frontends/cx24113.h
+++ b/drivers/media/dvb-frontends/cx24113.h
@@ -22,8 +22,6 @@
 #ifndef CX24113_H
 #define CX24113_H
 
-#include <linux/kconfig.h>
-
 struct dvb_frontend;
 
 struct cx24113_config {
diff --git a/drivers/media/dvb-frontends/cx24116.h b/drivers/media/dvb-frontends/cx24116.h
index f6dbabc1d62b..9ff8df8d44b8 100644
--- a/drivers/media/dvb-frontends/cx24116.h
+++ b/drivers/media/dvb-frontends/cx24116.h
@@ -21,7 +21,6 @@
 #ifndef CX24116_H
 #define CX24116_H
 
-#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 struct cx24116_config {
diff --git a/drivers/media/dvb-frontends/cx24117.h b/drivers/media/dvb-frontends/cx24117.h
index 1648ab432168..445f13faf63a 100644
--- a/drivers/media/dvb-frontends/cx24117.h
+++ b/drivers/media/dvb-frontends/cx24117.h
@@ -22,7 +22,6 @@
 #ifndef CX24117_H
 #define CX24117_H
 
-#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 struct cx24117_config {
diff --git a/drivers/media/dvb-frontends/cx24120.h b/drivers/media/dvb-frontends/cx24120.h
index f0970423e16f..de4ca9aa0923 100644
--- a/drivers/media/dvb-frontends/cx24120.h
+++ b/drivers/media/dvb-frontends/cx24120.h
@@ -20,7 +20,6 @@
 #ifndef CX24120_H
 #define CX24120_H
 
-#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 #include <linux/firmware.h>
 
diff --git a/drivers/media/dvb-frontends/cx24123.h b/drivers/media/dvb-frontends/cx24123.h
index 975f3c926fe8..aac23444aa9a 100644
--- a/drivers/media/dvb-frontends/cx24123.h
+++ b/drivers/media/dvb-frontends/cx24123.h
@@ -21,7 +21,6 @@
 #ifndef CX24123_H
 #define CX24123_H
 
-#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 struct cx24123_config {
diff --git a/drivers/media/dvb-frontends/cxd2820r.h b/drivers/media/dvb-frontends/cxd2820r.h
index 56d42760263d..f3ff8f6eb3bb 100644
--- a/drivers/media/dvb-frontends/cxd2820r.h
+++ b/drivers/media/dvb-frontends/cxd2820r.h
@@ -22,7 +22,6 @@
 #ifndef CXD2820R_H
 #define CXD2820R_H
 
-#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 #define CXD2820R_GPIO_D (0 << 0) /* disable */
@@ -37,6 +36,32 @@
 #define CXD2820R_TS_PARALLEL      0x30
 #define CXD2820R_TS_PARALLEL_MSB  0x70
 
+/*
+ * I2C address: 0x6c, 0x6d
+ */
+
+/**
+ * struct cxd2820r_platform_data - Platform data for the cxd2820r driver
+ * @ts_mode: TS mode.
+ * @ts_clk_inv: TS clock inverted.
+ * @if_agc_polarity: IF AGC polarity.
+ * @spec_inv: Input spectrum inverted.
+ * @gpio_chip_base: GPIO.
+ * @get_dvb_frontend: Get DVB frontend.
+ */
+
+struct cxd2820r_platform_data {
+	u8 ts_mode;
+	bool ts_clk_inv;
+	bool if_agc_polarity;
+	bool spec_inv;
+	int **gpio_chip_base;
+
+	struct dvb_frontend* (*get_dvb_frontend)(struct i2c_client *);
+/* private: For legacy media attach wrapper. Do not set value. */
+	bool attach_in_use;
+};
+
 struct cxd2820r_config {
 	/* Demodulator I2C address.
 	 * Driver determines DVB-C slave I2C address automatically from master
diff --git a/drivers/media/dvb-frontends/cxd2820r_c.c b/drivers/media/dvb-frontends/cxd2820r_c.c
index a674a6312c38..d75b0776d5b5 100644
--- a/drivers/media/dvb-frontends/cxd2820r_c.c
+++ b/drivers/media/dvb-frontends/cxd2820r_c.c
@@ -24,12 +24,12 @@
 int cxd2820r_set_frontend_c(struct dvb_frontend *fe)
 {
 	struct cxd2820r_priv *priv = fe->demodulator_priv;
+	struct i2c_client *client = priv->client[0];
 	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
-	int ret, i;
+	int ret;
+	unsigned int utmp;
 	u8 buf[2];
-	u32 if_freq;
-	u16 if_ctl;
-	u64 num;
+	u32 if_frequency;
 	struct reg_val_mask tab[] = {
 		{ 0x00080, 0x01, 0xff },
 		{ 0x00081, 0x05, 0xff },
@@ -43,25 +43,24 @@ int cxd2820r_set_frontend_c(struct dvb_frontend *fe)
 		{ 0x10059, 0x50, 0xff },
 		{ 0x10087, 0x0c, 0x3c },
 		{ 0x1008b, 0x07, 0xff },
-		{ 0x1001f, priv->cfg.if_agc_polarity << 7, 0x80 },
-		{ 0x10070, priv->cfg.ts_mode, 0xff },
-		{ 0x10071, !priv->cfg.ts_clock_inv << 4, 0x10 },
+		{ 0x1001f, priv->if_agc_polarity << 7, 0x80 },
+		{ 0x10070, priv->ts_mode, 0xff },
+		{ 0x10071, !priv->ts_clk_inv << 4, 0x10 },
 	};
 
-	dev_dbg(&priv->i2c->dev, "%s: frequency=%d symbol_rate=%d\n", __func__,
-			c->frequency, c->symbol_rate);
+	dev_dbg(&client->dev,
+		"delivery_system=%d modulation=%d frequency=%u symbol_rate=%u inversion=%d\n",
+		c->delivery_system, c->modulation, c->frequency,
+		c->symbol_rate, c->inversion);
 
 	/* program tuner */
 	if (fe->ops.tuner_ops.set_params)
 		fe->ops.tuner_ops.set_params(fe);
 
 	if (priv->delivery_system !=  SYS_DVBC_ANNEX_A) {
-		for (i = 0; i < ARRAY_SIZE(tab); i++) {
-			ret = cxd2820r_wr_reg_mask(priv, tab[i].reg,
-				tab[i].val, tab[i].mask);
-			if (ret)
-				goto error;
-		}
+		ret = cxd2820r_wr_reg_val_mask_tab(priv, tab, ARRAY_SIZE(tab));
+		if (ret)
+			goto error;
 	}
 
 	priv->delivery_system = SYS_DVBC_ANNEX_A;
@@ -69,35 +68,33 @@ int cxd2820r_set_frontend_c(struct dvb_frontend *fe)
 
 	/* program IF frequency */
 	if (fe->ops.tuner_ops.get_if_frequency) {
-		ret = fe->ops.tuner_ops.get_if_frequency(fe, &if_freq);
+		ret = fe->ops.tuner_ops.get_if_frequency(fe, &if_frequency);
 		if (ret)
 			goto error;
-	} else
-		if_freq = 0;
-
-	dev_dbg(&priv->i2c->dev, "%s: if_freq=%d\n", __func__, if_freq);
-
-	num = if_freq / 1000; /* Hz => kHz */
-	num *= 0x4000;
-	if_ctl = 0x4000 - DIV_ROUND_CLOSEST_ULL(num, 41000);
-	buf[0] = (if_ctl >> 8) & 0x3f;
-	buf[1] = (if_ctl >> 0) & 0xff;
+		dev_dbg(&client->dev, "if_frequency=%u\n", if_frequency);
+	} else {
+		ret = -EINVAL;
+		goto error;
+	}
 
-	ret = cxd2820r_wr_regs(priv, 0x10042, buf, 2);
+	utmp = 0x4000 - DIV_ROUND_CLOSEST_ULL((u64)if_frequency * 0x4000, CXD2820R_CLK);
+	buf[0] = (utmp >> 8) & 0xff;
+	buf[1] = (utmp >> 0) & 0xff;
+	ret = regmap_bulk_write(priv->regmap[1], 0x0042, buf, 2);
 	if (ret)
 		goto error;
 
-	ret = cxd2820r_wr_reg(priv, 0x000ff, 0x08);
+	ret = regmap_write(priv->regmap[0], 0x00ff, 0x08);
 	if (ret)
 		goto error;
 
-	ret = cxd2820r_wr_reg(priv, 0x000fe, 0x01);
+	ret = regmap_write(priv->regmap[0], 0x00fe, 0x01);
 	if (ret)
 		goto error;
 
 	return ret;
 error:
-	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&client->dev, "failed=%d\n", ret);
 	return ret;
 }
 
@@ -105,20 +102,24 @@ int cxd2820r_get_frontend_c(struct dvb_frontend *fe,
 			    struct dtv_frontend_properties *c)
 {
 	struct cxd2820r_priv *priv = fe->demodulator_priv;
+	struct i2c_client *client = priv->client[0];
 	int ret;
+	unsigned int utmp;
 	u8 buf[2];
 
-	ret = cxd2820r_rd_regs(priv, 0x1001a, buf, 2);
+	dev_dbg(&client->dev, "\n");
+
+	ret = regmap_bulk_read(priv->regmap[1], 0x001a, buf, 2);
 	if (ret)
 		goto error;
 
 	c->symbol_rate = 2500 * ((buf[0] & 0x0f) << 8 | buf[1]);
 
-	ret = cxd2820r_rd_reg(priv, 0x10019, &buf[0]);
+	ret = regmap_read(priv->regmap[1], 0x0019, &utmp);
 	if (ret)
 		goto error;
 
-	switch ((buf[0] >> 0) & 0x07) {
+	switch ((utmp >> 0) & 0x07) {
 	case 0:
 		c->modulation = QAM_16;
 		break;
@@ -136,7 +137,7 @@ int cxd2820r_get_frontend_c(struct dvb_frontend *fe,
 		break;
 	}
 
-	switch ((buf[0] >> 7) & 0x01) {
+	switch ((utmp >> 7) & 0x01) {
 	case 0:
 		c->inversion = INVERSION_OFF;
 		break;
@@ -147,167 +148,169 @@ int cxd2820r_get_frontend_c(struct dvb_frontend *fe,
 
 	return ret;
 error:
-	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&client->dev, "failed=%d\n", ret);
 	return ret;
 }
 
-int cxd2820r_read_ber_c(struct dvb_frontend *fe, u32 *ber)
+int cxd2820r_read_status_c(struct dvb_frontend *fe, enum fe_status *status)
 {
 	struct cxd2820r_priv *priv = fe->demodulator_priv;
+	struct i2c_client *client = priv->client[0];
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 	int ret;
-	u8 buf[3], start_ber = 0;
-	*ber = 0;
+	unsigned int utmp, utmp1, utmp2;
+	u8 buf[3];
 
-	if (priv->ber_running) {
-		ret = cxd2820r_rd_regs(priv, 0x10076, buf, sizeof(buf));
-		if (ret)
-			goto error;
+	/* Lock detection */
+	ret = regmap_bulk_read(priv->regmap[1], 0x0088, &buf[0], 1);
+	if (ret)
+		goto error;
+	ret = regmap_bulk_read(priv->regmap[1], 0x0073, &buf[1], 1);
+	if (ret)
+		goto error;
 
-		if ((buf[2] >> 7) & 0x01 || (buf[2] >> 4) & 0x01) {
-			*ber = (buf[2] & 0x0f) << 16 | buf[1] << 8 | buf[0];
-			start_ber = 1;
-		}
+	utmp1 = (buf[0] >> 0) & 0x01;
+	utmp2 = (buf[1] >> 3) & 0x01;
+
+	if (utmp1 == 1 && utmp2 == 1) {
+		*status = FE_HAS_SIGNAL | FE_HAS_CARRIER |
+			  FE_HAS_VITERBI | FE_HAS_SYNC | FE_HAS_LOCK;
+	} else if (utmp1 == 1 || utmp2 == 1) {
+		*status = FE_HAS_SIGNAL | FE_HAS_CARRIER |
+			  FE_HAS_VITERBI | FE_HAS_SYNC;
 	} else {
-		priv->ber_running = true;
-		start_ber = 1;
+		*status = 0;
 	}
 
-	if (start_ber) {
-		/* (re)start BER */
-		ret = cxd2820r_wr_reg(priv, 0x10079, 0x01);
-		if (ret)
-			goto error;
-	}
+	dev_dbg(&client->dev, "status=%02x raw=%*ph sync=%u ts=%u\n",
+		*status, 2, buf, utmp1, utmp2);
 
-	return ret;
-error:
-	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
-	return ret;
-}
+	/* Signal strength */
+	if (*status & FE_HAS_SIGNAL) {
+		unsigned int strength;
 
-int cxd2820r_read_signal_strength_c(struct dvb_frontend *fe,
-	u16 *strength)
-{
-	struct cxd2820r_priv *priv = fe->demodulator_priv;
-	int ret;
-	u8 buf[2];
-	u16 tmp;
+		ret = regmap_bulk_read(priv->regmap[1], 0x0049, buf, 2);
+		if (ret)
+			goto error;
 
-	ret = cxd2820r_rd_regs(priv, 0x10049, buf, sizeof(buf));
-	if (ret)
-		goto error;
+		utmp = buf[0] << 8 | buf[1] << 0;
+		utmp = 511 - sign_extend32(utmp, 9);
+		/* Scale value to 0x0000-0xffff */
+		strength = utmp << 6 | utmp >> 4;
 
-	tmp = (buf[0] & 0x03) << 8 | buf[1];
-	tmp = (~tmp & 0x03ff);
+		c->strength.len = 1;
+		c->strength.stat[0].scale = FE_SCALE_RELATIVE;
+		c->strength.stat[0].uvalue = strength;
+	} else {
+		c->strength.len = 1;
+		c->strength.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	}
 
-	if (tmp == 512)
-		/* ~no signal */
-		tmp = 0;
-	else if (tmp > 350)
-		tmp = 350;
+	/* CNR */
+	if (*status & FE_HAS_VITERBI) {
+		unsigned int cnr, const_a, const_b;
 
-	/* scale value to 0x0000-0xffff */
-	*strength = tmp * 0xffff / (350-0);
+		ret = regmap_read(priv->regmap[1], 0x0019, &utmp);
+		if (ret)
+			goto error;
 
-	return ret;
-error:
-	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
-	return ret;
-}
+		if (((utmp >> 0) & 0x03) % 2) {
+			const_a = 8750;
+			const_b = 650;
+		} else {
+			const_a = 9500;
+			const_b = 760;
+		}
 
-int cxd2820r_read_snr_c(struct dvb_frontend *fe, u16 *snr)
-{
-	struct cxd2820r_priv *priv = fe->demodulator_priv;
-	int ret;
-	u8 tmp;
-	unsigned int A, B;
-	/* report SNR in dB * 10 */
+		ret = regmap_read(priv->regmap[1], 0x004d, &utmp);
+		if (ret)
+			goto error;
 
-	ret = cxd2820r_rd_reg(priv, 0x10019, &tmp);
-	if (ret)
-		goto error;
+		#define CXD2820R_LOG2_E_24 24204406 /* log2(e) << 24 */
+		if (utmp)
+			cnr = div_u64((u64)(intlog2(const_b) - intlog2(utmp))
+				      * const_a, CXD2820R_LOG2_E_24);
+		else
+			cnr = 0;
 
-	if (((tmp >> 0) & 0x03) % 2) {
-		A = 875;
-		B = 650;
+		c->cnr.len = 1;
+		c->cnr.stat[0].scale = FE_SCALE_DECIBEL;
+		c->cnr.stat[0].svalue = cnr;
 	} else {
-		A = 950;
-		B = 760;
+		c->cnr.len = 1;
+		c->cnr.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
 	}
 
-	ret = cxd2820r_rd_reg(priv, 0x1004d, &tmp);
-	if (ret)
-		goto error;
-
-	#define CXD2820R_LOG2_E_24 24204406 /* log2(e) << 24 */
-	if (tmp)
-		*snr = A * (intlog2(B / tmp) >> 5) / (CXD2820R_LOG2_E_24 >> 5)
-			/ 10;
-	else
-		*snr = 0;
-
-	return ret;
-error:
-	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
-	return ret;
-}
+	/* BER */
+	if (*status & FE_HAS_SYNC) {
+		unsigned int post_bit_error;
+		bool start_ber;
 
-int cxd2820r_read_ucblocks_c(struct dvb_frontend *fe, u32 *ucblocks)
-{
-	*ucblocks = 0;
-	/* no way to read ? */
-	return 0;
-}
+		if (priv->ber_running) {
+			ret = regmap_bulk_read(priv->regmap[1], 0x0076, buf, 3);
+			if (ret)
+				goto error;
 
-int cxd2820r_read_status_c(struct dvb_frontend *fe, enum fe_status *status)
-{
-	struct cxd2820r_priv *priv = fe->demodulator_priv;
-	int ret;
-	u8 buf[2];
-	*status = 0;
+			if ((buf[2] >> 7) & 0x01) {
+				post_bit_error = buf[2] << 16 | buf[1] << 8 |
+						 buf[0] << 0;
+				post_bit_error &= 0x0fffff;
+				start_ber = true;
+			} else {
+				post_bit_error = 0;
+				start_ber = false;
+			}
+		} else {
+			post_bit_error = 0;
+			start_ber = true;
+		}
 
-	ret = cxd2820r_rd_regs(priv, 0x10088, buf, sizeof(buf));
-	if (ret)
-		goto error;
+		if (start_ber) {
+			ret = regmap_write(priv->regmap[1], 0x0079, 0x01);
+			if (ret)
+				goto error;
+			priv->ber_running = true;
+		}
 
-	if (((buf[0] >> 0) & 0x01) == 1) {
-		*status |= FE_HAS_SIGNAL | FE_HAS_CARRIER |
-			FE_HAS_VITERBI | FE_HAS_SYNC;
+		priv->post_bit_error += post_bit_error;
 
-		if (((buf[1] >> 3) & 0x01) == 1) {
-			*status |= FE_HAS_SIGNAL | FE_HAS_CARRIER |
-				FE_HAS_VITERBI | FE_HAS_SYNC | FE_HAS_LOCK;
-		}
+		c->post_bit_error.len = 1;
+		c->post_bit_error.stat[0].scale = FE_SCALE_COUNTER;
+		c->post_bit_error.stat[0].uvalue = priv->post_bit_error;
+	} else {
+		c->post_bit_error.len = 1;
+		c->post_bit_error.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
 	}
 
-	dev_dbg(&priv->i2c->dev, "%s: lock=%02x %02x\n", __func__, buf[0],
-			buf[1]);
-
 	return ret;
 error:
-	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&client->dev, "failed=%d\n", ret);
 	return ret;
 }
 
 int cxd2820r_init_c(struct dvb_frontend *fe)
 {
 	struct cxd2820r_priv *priv = fe->demodulator_priv;
+	struct i2c_client *client = priv->client[0];
 	int ret;
 
-	ret = cxd2820r_wr_reg(priv, 0x00085, 0x07);
+	dev_dbg(&client->dev, "\n");
+
+	ret = regmap_write(priv->regmap[0], 0x0085, 0x07);
 	if (ret)
 		goto error;
 
 	return ret;
 error:
-	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&client->dev, "failed=%d\n", ret);
 	return ret;
 }
 
 int cxd2820r_sleep_c(struct dvb_frontend *fe)
 {
 	struct cxd2820r_priv *priv = fe->demodulator_priv;
-	int ret, i;
+	struct i2c_client *client = priv->client[0];
+	int ret;
 	struct reg_val_mask tab[] = {
 		{ 0x000ff, 0x1f, 0xff },
 		{ 0x00085, 0x00, 0xff },
@@ -316,20 +319,17 @@ int cxd2820r_sleep_c(struct dvb_frontend *fe)
 		{ 0x00080, 0x00, 0xff },
 	};
 
-	dev_dbg(&priv->i2c->dev, "%s\n", __func__);
+	dev_dbg(&client->dev, "\n");
 
 	priv->delivery_system = SYS_UNDEFINED;
 
-	for (i = 0; i < ARRAY_SIZE(tab); i++) {
-		ret = cxd2820r_wr_reg_mask(priv, tab[i].reg, tab[i].val,
-			tab[i].mask);
-		if (ret)
-			goto error;
-	}
+	ret = cxd2820r_wr_reg_val_mask_tab(priv, tab, ARRAY_SIZE(tab));
+	if (ret)
+		goto error;
 
 	return ret;
 error:
-	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&client->dev, "failed=%d\n", ret);
 	return ret;
 }
 
diff --git a/drivers/media/dvb-frontends/cxd2820r_core.c b/drivers/media/dvb-frontends/cxd2820r_core.c
index 314d3b8c1080..95267c6edb3a 100644
--- a/drivers/media/dvb-frontends/cxd2820r_core.c
+++ b/drivers/media/dvb-frontends/cxd2820r_core.c
@@ -21,178 +21,50 @@
 
 #include "cxd2820r_priv.h"
 
-/* Max transfer size done by I2C transfer functions */
-#define MAX_XFER_SIZE  64
-
-/* write multiple registers */
-static int cxd2820r_wr_regs_i2c(struct cxd2820r_priv *priv, u8 i2c, u8 reg,
-	u8 *val, int len)
-{
-	int ret;
-	u8 buf[MAX_XFER_SIZE];
-	struct i2c_msg msg[1] = {
-		{
-			.addr = i2c,
-			.flags = 0,
-			.len = len + 1,
-			.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 cxd2820r_rd_regs_i2c(struct cxd2820r_priv *priv, u8 i2c, u8 reg,
-	u8 *val, int len)
-{
-	int ret;
-	u8 buf[MAX_XFER_SIZE];
-	struct i2c_msg msg[2] = {
-		{
-			.addr = i2c,
-			.flags = 0,
-			.len = 1,
-			.buf = &reg,
-		}, {
-			.addr = i2c,
-			.flags = I2C_M_RD,
-			.len = len,
-			.buf = buf,
-		}
-	};
-
-	if (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;
-	}
-
-	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 multiple registers */
-int cxd2820r_wr_regs(struct cxd2820r_priv *priv, u32 reginfo, u8 *val,
-	int len)
+/* Write register table */
+int cxd2820r_wr_reg_val_mask_tab(struct cxd2820r_priv *priv,
+				 const struct reg_val_mask *tab, int tab_len)
 {
+	struct i2c_client *client = priv->client[0];
 	int ret;
-	u8 i2c_addr;
-	u8 reg = (reginfo >> 0) & 0xff;
-	u8 bank = (reginfo >> 8) & 0xff;
-	u8 i2c = (reginfo >> 16) & 0x01;
-
-	/* select I2C */
-	if (i2c)
-		i2c_addr = priv->cfg.i2c_address | (1 << 1); /* DVB-C */
-	else
-		i2c_addr = priv->cfg.i2c_address; /* DVB-T/T2 */
+	unsigned int i, reg, mask, val;
+	struct regmap *regmap;
 
-	/* switch bank if needed */
-	if (bank != priv->bank[i2c]) {
-		ret = cxd2820r_wr_regs_i2c(priv, i2c_addr, 0x00, &bank, 1);
-		if (ret)
-			return ret;
-		priv->bank[i2c] = bank;
-	}
-	return cxd2820r_wr_regs_i2c(priv, i2c_addr, reg, val, len);
-}
-
-/* read multiple registers */
-int cxd2820r_rd_regs(struct cxd2820r_priv *priv, u32 reginfo, u8 *val,
-	int len)
-{
-	int ret;
-	u8 i2c_addr;
-	u8 reg = (reginfo >> 0) & 0xff;
-	u8 bank = (reginfo >> 8) & 0xff;
-	u8 i2c = (reginfo >> 16) & 0x01;
-
-	/* select I2C */
-	if (i2c)
-		i2c_addr = priv->cfg.i2c_address | (1 << 1); /* DVB-C */
-	else
-		i2c_addr = priv->cfg.i2c_address; /* DVB-T/T2 */
+	dev_dbg(&client->dev, "tab_len=%d\n", tab_len);
 
-	/* switch bank if needed */
-	if (bank != priv->bank[i2c]) {
-		ret = cxd2820r_wr_regs_i2c(priv, i2c_addr, 0x00, &bank, 1);
-		if (ret)
-			return ret;
-		priv->bank[i2c] = bank;
-	}
-	return cxd2820r_rd_regs_i2c(priv, i2c_addr, reg, val, len);
-}
-
-/* write single register */
-int cxd2820r_wr_reg(struct cxd2820r_priv *priv, u32 reg, u8 val)
-{
-	return cxd2820r_wr_regs(priv, reg, &val, 1);
-}
-
-/* read single register */
-int cxd2820r_rd_reg(struct cxd2820r_priv *priv, u32 reg, u8 *val)
-{
-	return cxd2820r_rd_regs(priv, reg, val, 1);
-}
+	for (i = 0; i < tab_len; i++) {
+		if ((tab[i].reg >> 16) & 0x1)
+			regmap = priv->regmap[1];
+		else
+			regmap = priv->regmap[0];
 
-/* write single register with mask */
-int cxd2820r_wr_reg_mask(struct cxd2820r_priv *priv, u32 reg, u8 val,
-	u8 mask)
-{
-	int ret;
-	u8 tmp;
+		reg = (tab[i].reg >> 0) & 0xffff;
+		val = tab[i].val;
+		mask = tab[i].mask;
 
-	/* no need for read if whole reg is written */
-	if (mask != 0xff) {
-		ret = cxd2820r_rd_reg(priv, reg, &tmp);
+		if (mask == 0xff)
+			ret = regmap_write(regmap, reg, val);
+		else
+			ret = regmap_write_bits(regmap, reg, mask, val);
 		if (ret)
-			return ret;
-
-		val &= mask;
-		tmp &= ~mask;
-		val |= tmp;
+			goto error;
 	}
 
-	return cxd2820r_wr_reg(priv, reg, val);
+	return 0;
+error:
+	dev_dbg(&client->dev, "failed=%d\n", ret);
+	return ret;
 }
 
 int cxd2820r_gpio(struct dvb_frontend *fe, u8 *gpio)
 {
 	struct cxd2820r_priv *priv = fe->demodulator_priv;
+	struct i2c_client *client = priv->client[0];
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 	int ret, i;
 	u8 tmp0, tmp1;
 
-	dev_dbg(&priv->i2c->dev, "%s: delsys=%d\n", __func__,
-			fe->dtv_property_cache.delivery_system);
+	dev_dbg(&client->dev, "delivery_system=%d\n", c->delivery_system);
 
 	/* update GPIOs only when needed */
 	if (!memcmp(gpio, priv->gpio, sizeof(priv->gpio)))
@@ -219,20 +91,18 @@ int cxd2820r_gpio(struct dvb_frontend *fe, u8 *gpio)
 		else
 			tmp1 |= (0 << (0 + i));
 
-		dev_dbg(&priv->i2c->dev, "%s: gpio i=%d %02x %02x\n", __func__,
-				i, tmp0, tmp1);
+		dev_dbg(&client->dev, "gpio i=%d %02x %02x\n", i, tmp0, tmp1);
 	}
 
-	dev_dbg(&priv->i2c->dev, "%s: wr gpio=%02x %02x\n", __func__, tmp0,
-			tmp1);
+	dev_dbg(&client->dev, "wr gpio=%02x %02x\n", tmp0, tmp1);
 
 	/* write bits [7:2] */
-	ret = cxd2820r_wr_reg_mask(priv, 0x00089, tmp0, 0xfc);
+	ret = regmap_update_bits(priv->regmap[0], 0x0089, 0xfc, tmp0);
 	if (ret)
 		goto error;
 
 	/* write bits [5:0] */
-	ret = cxd2820r_wr_reg_mask(priv, 0x0008e, tmp1, 0x3f);
+	ret = regmap_update_bits(priv->regmap[0], 0x008e, 0x3f, tmp1);
 	if (ret)
 		goto error;
 
@@ -240,18 +110,18 @@ int cxd2820r_gpio(struct dvb_frontend *fe, u8 *gpio)
 
 	return ret;
 error:
-	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&client->dev, "failed=%d\n", ret);
 	return ret;
 }
 
 static int cxd2820r_set_frontend(struct dvb_frontend *fe)
 {
 	struct cxd2820r_priv *priv = fe->demodulator_priv;
+	struct i2c_client *client = priv->client[0];
 	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 	int ret;
 
-	dev_dbg(&priv->i2c->dev, "%s: delsys=%d\n", __func__,
-			fe->dtv_property_cache.delivery_system);
+	dev_dbg(&client->dev, "delivery_system=%d\n", c->delivery_system);
 
 	switch (c->delivery_system) {
 	case SYS_DVBT:
@@ -279,8 +149,7 @@ static int cxd2820r_set_frontend(struct dvb_frontend *fe)
 			goto err;
 		break;
 	default:
-		dev_dbg(&priv->i2c->dev, "%s: error state=%d\n", __func__,
-				fe->dtv_property_cache.delivery_system);
+		dev_dbg(&client->dev, "invalid delivery_system\n");
 		ret = -EINVAL;
 		break;
 	}
@@ -291,12 +160,13 @@ err:
 static int cxd2820r_read_status(struct dvb_frontend *fe, enum fe_status *status)
 {
 	struct cxd2820r_priv *priv = fe->demodulator_priv;
+	struct i2c_client *client = priv->client[0];
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 	int ret;
 
-	dev_dbg(&priv->i2c->dev, "%s: delsys=%d\n", __func__,
-			fe->dtv_property_cache.delivery_system);
+	dev_dbg(&client->dev, "delivery_system=%d\n", c->delivery_system);
 
-	switch (fe->dtv_property_cache.delivery_system) {
+	switch (c->delivery_system) {
 	case SYS_DVBT:
 		ret = cxd2820r_read_status_t(fe, status);
 		break;
@@ -317,15 +187,16 @@ static int cxd2820r_get_frontend(struct dvb_frontend *fe,
 				 struct dtv_frontend_properties *p)
 {
 	struct cxd2820r_priv *priv = fe->demodulator_priv;
+	struct i2c_client *client = priv->client[0];
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 	int ret;
 
-	dev_dbg(&priv->i2c->dev, "%s: delsys=%d\n", __func__,
-			fe->dtv_property_cache.delivery_system);
+	dev_dbg(&client->dev, "delivery_system=%d\n", c->delivery_system);
 
 	if (priv->delivery_system == SYS_UNDEFINED)
 		return 0;
 
-	switch (fe->dtv_property_cache.delivery_system) {
+	switch (c->delivery_system) {
 	case SYS_DVBT:
 		ret = cxd2820r_get_frontend_t(fe, p);
 		break;
@@ -345,101 +216,60 @@ static int cxd2820r_get_frontend(struct dvb_frontend *fe,
 static int cxd2820r_read_ber(struct dvb_frontend *fe, u32 *ber)
 {
 	struct cxd2820r_priv *priv = fe->demodulator_priv;
-	int ret;
+	struct i2c_client *client = priv->client[0];
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 
-	dev_dbg(&priv->i2c->dev, "%s: delsys=%d\n", __func__,
-			fe->dtv_property_cache.delivery_system);
+	dev_dbg(&client->dev, "delivery_system=%d\n", c->delivery_system);
 
-	switch (fe->dtv_property_cache.delivery_system) {
-	case SYS_DVBT:
-		ret = cxd2820r_read_ber_t(fe, ber);
-		break;
-	case SYS_DVBT2:
-		ret = cxd2820r_read_ber_t2(fe, ber);
-		break;
-	case SYS_DVBC_ANNEX_A:
-		ret = cxd2820r_read_ber_c(fe, ber);
-		break;
-	default:
-		ret = -EINVAL;
-		break;
-	}
-	return ret;
+	*ber = (priv->post_bit_error - priv->post_bit_error_prev_dvbv3);
+	priv->post_bit_error_prev_dvbv3 = priv->post_bit_error;
+
+	return 0;
 }
 
 static int cxd2820r_read_signal_strength(struct dvb_frontend *fe, u16 *strength)
 {
 	struct cxd2820r_priv *priv = fe->demodulator_priv;
-	int ret;
+	struct i2c_client *client = priv->client[0];
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 
-	dev_dbg(&priv->i2c->dev, "%s: delsys=%d\n", __func__,
-			fe->dtv_property_cache.delivery_system);
+	dev_dbg(&client->dev, "delivery_system=%d\n", c->delivery_system);
 
-	switch (fe->dtv_property_cache.delivery_system) {
-	case SYS_DVBT:
-		ret = cxd2820r_read_signal_strength_t(fe, strength);
-		break;
-	case SYS_DVBT2:
-		ret = cxd2820r_read_signal_strength_t2(fe, strength);
-		break;
-	case SYS_DVBC_ANNEX_A:
-		ret = cxd2820r_read_signal_strength_c(fe, strength);
-		break;
-	default:
-		ret = -EINVAL;
-		break;
-	}
-	return ret;
+	if (c->strength.stat[0].scale == FE_SCALE_RELATIVE)
+		*strength = c->strength.stat[0].uvalue;
+	else
+		*strength = 0;
+
+	return 0;
 }
 
 static int cxd2820r_read_snr(struct dvb_frontend *fe, u16 *snr)
 {
 	struct cxd2820r_priv *priv = fe->demodulator_priv;
-	int ret;
+	struct i2c_client *client = priv->client[0];
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 
-	dev_dbg(&priv->i2c->dev, "%s: delsys=%d\n", __func__,
-			fe->dtv_property_cache.delivery_system);
+	dev_dbg(&client->dev, "delivery_system=%d\n", c->delivery_system);
 
-	switch (fe->dtv_property_cache.delivery_system) {
-	case SYS_DVBT:
-		ret = cxd2820r_read_snr_t(fe, snr);
-		break;
-	case SYS_DVBT2:
-		ret = cxd2820r_read_snr_t2(fe, snr);
-		break;
-	case SYS_DVBC_ANNEX_A:
-		ret = cxd2820r_read_snr_c(fe, snr);
-		break;
-	default:
-		ret = -EINVAL;
-		break;
-	}
-	return ret;
+	if (c->cnr.stat[0].scale == FE_SCALE_DECIBEL)
+		*snr = div_s64(c->cnr.stat[0].svalue, 100);
+	else
+		*snr = 0;
+
+	return 0;
 }
 
 static int cxd2820r_read_ucblocks(struct dvb_frontend *fe, u32 *ucblocks)
 {
 	struct cxd2820r_priv *priv = fe->demodulator_priv;
-	int ret;
+	struct i2c_client *client = priv->client[0];
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 
-	dev_dbg(&priv->i2c->dev, "%s: delsys=%d\n", __func__,
-			fe->dtv_property_cache.delivery_system);
+	dev_dbg(&client->dev, "delivery_system=%d\n", c->delivery_system);
 
-	switch (fe->dtv_property_cache.delivery_system) {
-	case SYS_DVBT:
-		ret = cxd2820r_read_ucblocks_t(fe, ucblocks);
-		break;
-	case SYS_DVBT2:
-		ret = cxd2820r_read_ucblocks_t2(fe, ucblocks);
-		break;
-	case SYS_DVBC_ANNEX_A:
-		ret = cxd2820r_read_ucblocks_c(fe, ucblocks);
-		break;
-	default:
-		ret = -EINVAL;
-		break;
-	}
-	return ret;
+	*ucblocks = 0;
+
+	return 0;
 }
 
 static int cxd2820r_init(struct dvb_frontend *fe)
@@ -450,12 +280,13 @@ static int cxd2820r_init(struct dvb_frontend *fe)
 static int cxd2820r_sleep(struct dvb_frontend *fe)
 {
 	struct cxd2820r_priv *priv = fe->demodulator_priv;
+	struct i2c_client *client = priv->client[0];
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 	int ret;
 
-	dev_dbg(&priv->i2c->dev, "%s: delsys=%d\n", __func__,
-			fe->dtv_property_cache.delivery_system);
+	dev_dbg(&client->dev, "delivery_system=%d\n", c->delivery_system);
 
-	switch (fe->dtv_property_cache.delivery_system) {
+	switch (c->delivery_system) {
 	case SYS_DVBT:
 		ret = cxd2820r_sleep_t(fe);
 		break;
@@ -476,12 +307,13 @@ static int cxd2820r_get_tune_settings(struct dvb_frontend *fe,
 				      struct dvb_frontend_tune_settings *s)
 {
 	struct cxd2820r_priv *priv = fe->demodulator_priv;
+	struct i2c_client *client = priv->client[0];
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 	int ret;
 
-	dev_dbg(&priv->i2c->dev, "%s: delsys=%d\n", __func__,
-			fe->dtv_property_cache.delivery_system);
+	dev_dbg(&client->dev, "delivery_system=%d\n", c->delivery_system);
 
-	switch (fe->dtv_property_cache.delivery_system) {
+	switch (c->delivery_system) {
 	case SYS_DVBT:
 		ret = cxd2820r_get_tune_settings_t(fe, s);
 		break;
@@ -501,12 +333,12 @@ static int cxd2820r_get_tune_settings(struct dvb_frontend *fe,
 static enum dvbfe_search cxd2820r_search(struct dvb_frontend *fe)
 {
 	struct cxd2820r_priv *priv = fe->demodulator_priv;
+	struct i2c_client *client = priv->client[0];
 	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 	int ret, i;
 	enum fe_status status = 0;
 
-	dev_dbg(&priv->i2c->dev, "%s: delsys=%d\n", __func__,
-			fe->dtv_property_cache.delivery_system);
+	dev_dbg(&client->dev, "delivery_system=%d\n", c->delivery_system);
 
 	/* switch between DVB-T and DVB-T2 when tune fails */
 	if (priv->last_tune_failed) {
@@ -530,7 +362,6 @@ static enum dvbfe_search cxd2820r_search(struct dvb_frontend *fe)
 	if (ret)
 		goto error;
 
-
 	/* frontend lock wait loop count */
 	switch (priv->delivery_system) {
 	case SYS_DVBT:
@@ -548,7 +379,7 @@ static enum dvbfe_search cxd2820r_search(struct dvb_frontend *fe)
 
 	/* wait frontend lock */
 	for (; i > 0; i--) {
-		dev_dbg(&priv->i2c->dev, "%s: loop=%d\n", __func__, i);
+		dev_dbg(&client->dev, "loop=%d\n", i);
 		msleep(50);
 		ret = cxd2820r_read_status(fe, &status);
 		if (ret)
@@ -568,7 +399,7 @@ static enum dvbfe_search cxd2820r_search(struct dvb_frontend *fe)
 	}
 
 error:
-	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&client->dev, "failed=%d\n", ret);
 	return DVBFE_ALGO_SEARCH_ERROR;
 }
 
@@ -580,27 +411,23 @@ static int cxd2820r_get_frontend_algo(struct dvb_frontend *fe)
 static void cxd2820r_release(struct dvb_frontend *fe)
 {
 	struct cxd2820r_priv *priv = fe->demodulator_priv;
+	struct i2c_client *client = priv->client[0];
 
-	dev_dbg(&priv->i2c->dev, "%s\n", __func__);
+	dev_dbg(&client->dev, "\n");
 
-#ifdef CONFIG_GPIOLIB
-	/* remove GPIOs */
-	if (priv->gpio_chip.label)
-		gpiochip_remove(&priv->gpio_chip);
+	i2c_unregister_device(client);
 
-#endif
-	kfree(priv);
 	return;
 }
 
 static int cxd2820r_i2c_gate_ctrl(struct dvb_frontend *fe, int enable)
 {
 	struct cxd2820r_priv *priv = fe->demodulator_priv;
+	struct i2c_client *client = priv->client[0];
 
-	dev_dbg(&priv->i2c->dev, "%s: %d\n", __func__, enable);
+	dev_dbg_ratelimited(&client->dev, "enable=%d\n", enable);
 
-	/* Bit 0 of reg 0xdb in bank 0x00 controls I2C repeater */
-	return cxd2820r_wr_reg_mask(priv, 0xdb, enable ? 1 : 0, 0x1);
+	return regmap_update_bits(priv->regmap[0], 0x00db, 0x01, enable ? 1 : 0);
 }
 
 #ifdef CONFIG_GPIOLIB
@@ -608,9 +435,10 @@ static int cxd2820r_gpio_direction_output(struct gpio_chip *chip, unsigned nr,
 		int val)
 {
 	struct cxd2820r_priv *priv = gpiochip_get_data(chip);
+	struct i2c_client *client = priv->client[0];
 	u8 gpio[GPIO_COUNT];
 
-	dev_dbg(&priv->i2c->dev, "%s: nr=%d val=%d\n", __func__, nr, val);
+	dev_dbg(&client->dev, "nr=%u val=%d\n", nr, val);
 
 	memcpy(gpio, priv->gpio, sizeof(gpio));
 	gpio[nr] = CXD2820R_GPIO_E | CXD2820R_GPIO_O | (val << 2);
@@ -621,9 +449,10 @@ static int cxd2820r_gpio_direction_output(struct gpio_chip *chip, unsigned nr,
 static void cxd2820r_gpio_set(struct gpio_chip *chip, unsigned nr, int val)
 {
 	struct cxd2820r_priv *priv = gpiochip_get_data(chip);
+	struct i2c_client *client = priv->client[0];
 	u8 gpio[GPIO_COUNT];
 
-	dev_dbg(&priv->i2c->dev, "%s: nr=%d val=%d\n", __func__, nr, val);
+	dev_dbg(&client->dev, "nr=%u val=%d\n", nr, val);
 
 	memcpy(gpio, priv->gpio, sizeof(gpio));
 	gpio[nr] = CXD2820R_GPIO_E | CXD2820R_GPIO_O | (val << 2);
@@ -636,8 +465,9 @@ static void cxd2820r_gpio_set(struct gpio_chip *chip, unsigned nr, int val)
 static int cxd2820r_gpio_get(struct gpio_chip *chip, unsigned nr)
 {
 	struct cxd2820r_priv *priv = gpiochip_get_data(chip);
+	struct i2c_client *client = priv->client[0];
 
-	dev_dbg(&priv->i2c->dev, "%s: nr=%d\n", __func__, nr);
+	dev_dbg(&client->dev, "nr=%u\n", nr);
 
 	return (priv->gpio[nr] >> 2) & 0x01;
 }
@@ -689,52 +519,163 @@ static const struct dvb_frontend_ops cxd2820r_ops = {
 	.read_signal_strength	= cxd2820r_read_signal_strength,
 };
 
-struct dvb_frontend *cxd2820r_attach(const struct cxd2820r_config *cfg,
-		struct i2c_adapter *i2c, int *gpio_chip_base
-)
+/*
+ * XXX: That is wrapper to cxd2820r_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 *cxd2820r_attach(const struct cxd2820r_config *config,
+				     struct i2c_adapter *adapter,
+				     int *gpio_chip_base)
+{
+	struct i2c_client *client;
+	struct i2c_board_info board_info;
+	struct cxd2820r_platform_data pdata;
+
+	pdata.ts_mode = config->ts_mode;
+	pdata.ts_clk_inv = config->ts_clock_inv;
+	pdata.if_agc_polarity = config->if_agc_polarity;
+	pdata.spec_inv = config->spec_inv;
+	pdata.gpio_chip_base = &gpio_chip_base;
+	pdata.attach_in_use = true;
+
+	memset(&board_info, 0, sizeof(board_info));
+	strlcpy(board_info.type, "cxd2820r", I2C_NAME_SIZE);
+	board_info.addr = config->i2c_address;
+	board_info.platform_data = &pdata;
+	client = i2c_new_device(adapter, &board_info);
+	if (!client || !client->dev.driver)
+		return NULL;
+
+	return pdata.get_dvb_frontend(client);
+}
+EXPORT_SYMBOL(cxd2820r_attach);
+
+static struct dvb_frontend *cxd2820r_get_dvb_frontend(struct i2c_client *client)
+{
+	struct cxd2820r_priv *priv = i2c_get_clientdata(client);
+
+	dev_dbg(&client->dev, "\n");
+
+	return &priv->fe;
+}
+
+static int cxd2820r_probe(struct i2c_client *client,
+			  const struct i2c_device_id *id)
 {
+	struct cxd2820r_platform_data *pdata = client->dev.platform_data;
 	struct cxd2820r_priv *priv;
-	int ret;
-	u8 tmp;
+	int ret, *gpio_chip_base;
+	unsigned int utmp;
+	static const struct regmap_range_cfg regmap_range_cfg0[] = {
+		{
+			.range_min        = 0x0000,
+			.range_max        = 0x3fff,
+			.selector_reg     = 0x00,
+			.selector_mask    = 0xff,
+			.selector_shift   = 0,
+			.window_start     = 0x00,
+			.window_len       = 0x100,
+		},
+	};
+	static const struct regmap_range_cfg regmap_range_cfg1[] = {
+		{
+			.range_min        = 0x0000,
+			.range_max        = 0x01ff,
+			.selector_reg     = 0x00,
+			.selector_mask    = 0xff,
+			.selector_shift   = 0,
+			.window_start     = 0x00,
+			.window_len       = 0x100,
+		},
+	};
+	static const struct regmap_config regmap_config0 = {
+		.reg_bits = 8,
+		.val_bits = 8,
+		.max_register = 0x3fff,
+		.ranges = regmap_range_cfg0,
+		.num_ranges = ARRAY_SIZE(regmap_range_cfg0),
+		.cache_type = REGCACHE_NONE,
+	};
+	static const struct regmap_config regmap_config1 = {
+		.reg_bits = 8,
+		.val_bits = 8,
+		.max_register = 0x01ff,
+		.ranges = regmap_range_cfg1,
+		.num_ranges = ARRAY_SIZE(regmap_range_cfg1),
+		.cache_type = REGCACHE_NONE,
+	};
+
+	dev_dbg(&client->dev, "\n");
 
-	priv = kzalloc(sizeof(struct cxd2820r_priv), GFP_KERNEL);
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
 	if (!priv) {
 		ret = -ENOMEM;
-		dev_err(&i2c->dev, "%s: kzalloc() failed\n",
-				KBUILD_MODNAME);
-		goto error;
+		goto err;
 	}
 
-	priv->i2c = i2c;
-	memcpy(&priv->cfg, cfg, sizeof(struct cxd2820r_config));
-	memcpy(&priv->fe.ops, &cxd2820r_ops, sizeof(struct dvb_frontend_ops));
-	priv->fe.demodulator_priv = priv;
+	priv->client[0] = client;
+	priv->i2c = client->adapter;
+	priv->ts_mode = pdata->ts_mode;
+	priv->ts_clk_inv = pdata->ts_clk_inv;
+	priv->if_agc_polarity = pdata->if_agc_polarity;
+	priv->spec_inv = pdata->spec_inv;
+	gpio_chip_base = *pdata->gpio_chip_base;
+	priv->regmap[0] = regmap_init_i2c(priv->client[0], &regmap_config0);
+	if (IS_ERR(priv->regmap[0])) {
+		ret = PTR_ERR(priv->regmap[0]);
+		goto err_kfree;
+	}
 
-	priv->bank[0] = priv->bank[1] = 0xff;
-	ret = cxd2820r_rd_reg(priv, 0x000fd, &tmp);
-	dev_dbg(&priv->i2c->dev, "%s: chip id=%02x\n", __func__, tmp);
-	if (ret || tmp != 0xe1)
-		goto error;
+	/* Check demod answers with correct chip id */
+	ret = regmap_read(priv->regmap[0], 0x00fd, &utmp);
+	if (ret)
+		goto err_regmap_0_regmap_exit;
+
+	dev_dbg(&client->dev, "chip_id=%02x\n", utmp);
+
+	if (utmp != 0xe1) {
+		ret = -ENODEV;
+		goto err_regmap_0_regmap_exit;
+	}
+
+	/*
+	 * Chip has two I2C addresses for different register banks. We register
+	 * one dummy I2C client in in order to get own I2C client for each
+	 * register bank.
+	 */
+	priv->client[1] = i2c_new_dummy(client->adapter, client->addr | (1 << 1));
+	if (!priv->client[1]) {
+		ret = -ENODEV;
+		dev_err(&client->dev, "I2C registration failed\n");
+		if (ret)
+			goto err_regmap_0_regmap_exit;
+	}
+
+	priv->regmap[1] = regmap_init_i2c(priv->client[1], &regmap_config1);
+	if (IS_ERR(priv->regmap[1])) {
+		ret = PTR_ERR(priv->regmap[1]);
+		goto err_client_1_i2c_unregister_device;
+	}
 
 	if (gpio_chip_base) {
 #ifdef CONFIG_GPIOLIB
-		/* add GPIOs */
+		/* Add GPIOs */
 		priv->gpio_chip.label = KBUILD_MODNAME;
-		priv->gpio_chip.parent = &priv->i2c->dev;
+		priv->gpio_chip.parent = &client->dev;
 		priv->gpio_chip.owner = THIS_MODULE;
-		priv->gpio_chip.direction_output =
-				cxd2820r_gpio_direction_output;
+		priv->gpio_chip.direction_output = cxd2820r_gpio_direction_output;
 		priv->gpio_chip.set = cxd2820r_gpio_set;
 		priv->gpio_chip.get = cxd2820r_gpio_get;
-		priv->gpio_chip.base = -1; /* dynamic allocation */
+		priv->gpio_chip.base = -1; /* Dynamic allocation */
 		priv->gpio_chip.ngpio = GPIO_COUNT;
 		priv->gpio_chip.can_sleep = 1;
 		ret = gpiochip_add_data(&priv->gpio_chip, priv);
 		if (ret)
-			goto error;
+			goto err_regmap_1_regmap_exit;
 
-		dev_dbg(&priv->i2c->dev, "%s: gpio_chip.base=%d\n", __func__,
-				priv->gpio_chip.base);
+		dev_dbg(&client->dev, "gpio_chip.base=%d\n",
+			priv->gpio_chip.base);
 
 		*gpio_chip_base = priv->gpio_chip.base;
 #else
@@ -748,17 +689,73 @@ struct dvb_frontend *cxd2820r_attach(const struct cxd2820r_config *cfg,
 		gpio[2] = 0;
 		ret = cxd2820r_gpio(&priv->fe, gpio);
 		if (ret)
-			goto error;
+			goto err_regmap_1_regmap_exit;
 #endif
 	}
 
-	return &priv->fe;
-error:
-	dev_dbg(&i2c->dev, "%s: failed=%d\n", __func__, ret);
+	/* Create dvb frontend */
+	memcpy(&priv->fe.ops, &cxd2820r_ops, sizeof(priv->fe.ops));
+	if (!pdata->attach_in_use)
+		priv->fe.ops.release = NULL;
+	priv->fe.demodulator_priv = priv;
+	i2c_set_clientdata(client, priv);
+
+	/* Setup callbacks */
+	pdata->get_dvb_frontend = cxd2820r_get_dvb_frontend;
+
+	dev_info(&client->dev, "Sony CXD2820R successfully identified\n");
+
+	return 0;
+err_regmap_1_regmap_exit:
+	regmap_exit(priv->regmap[1]);
+err_client_1_i2c_unregister_device:
+	i2c_unregister_device(priv->client[1]);
+err_regmap_0_regmap_exit:
+	regmap_exit(priv->regmap[0]);
+err_kfree:
 	kfree(priv);
-	return NULL;
+err:
+	dev_dbg(&client->dev, "failed=%d\n", ret);
+	return ret;
 }
-EXPORT_SYMBOL(cxd2820r_attach);
+
+static int cxd2820r_remove(struct i2c_client *client)
+{
+	struct cxd2820r_priv *priv = i2c_get_clientdata(client);
+
+	dev_dbg(&client->dev, "\n");
+
+#ifdef CONFIG_GPIOLIB
+	if (priv->gpio_chip.label)
+		gpiochip_remove(&priv->gpio_chip);
+#endif
+	regmap_exit(priv->regmap[1]);
+	i2c_unregister_device(priv->client[1]);
+
+	regmap_exit(priv->regmap[0]);
+
+	kfree(priv);
+
+	return 0;
+}
+
+static const struct i2c_device_id cxd2820r_id_table[] = {
+	{"cxd2820r", 0},
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, cxd2820r_id_table);
+
+static struct i2c_driver cxd2820r_driver = {
+	.driver = {
+		.name                = "cxd2820r",
+		.suppress_bind_attrs = true,
+	},
+	.probe    = cxd2820r_probe,
+	.remove   = cxd2820r_remove,
+	.id_table = cxd2820r_id_table,
+};
+
+module_i2c_driver(cxd2820r_driver);
 
 MODULE_AUTHOR("Antti Palosaari <crope@iki.fi>");
 MODULE_DESCRIPTION("Sony CXD2820R demodulator driver");
diff --git a/drivers/media/dvb-frontends/cxd2820r_priv.h b/drivers/media/dvb-frontends/cxd2820r_priv.h
index e31c48e53097..0d096206ac66 100644
--- a/drivers/media/dvb-frontends/cxd2820r_priv.h
+++ b/drivers/media/dvb-frontends/cxd2820r_priv.h
@@ -27,6 +27,8 @@
 #include "dvb_math.h"
 #include "cxd2820r.h"
 #include <linux/gpio.h>
+#include <linux/math64.h>
+#include <linux/regmap.h>
 
 struct reg_val_mask {
 	u32 reg;
@@ -34,14 +36,23 @@ struct reg_val_mask {
 	u8  mask;
 };
 
+#define CXD2820R_CLK 41000000
+
 struct cxd2820r_priv {
+	struct i2c_client *client[2];
+	struct regmap *regmap[2];
 	struct i2c_adapter *i2c;
 	struct dvb_frontend fe;
-	struct cxd2820r_config cfg;
+	u8 ts_mode;
+	bool ts_clk_inv;
+	bool if_agc_polarity;
+	bool spec_inv;
+
+	u64 post_bit_error_prev_dvbv3;
+	u64 post_bit_error;
 
 	bool ber_running;
 
-	u8 bank[2];
 #define GPIO_COUNT 3
 	u8 gpio[GPIO_COUNT];
 #ifdef CONFIG_GPIOLIB
@@ -58,6 +69,9 @@ extern int cxd2820r_debug;
 
 int cxd2820r_gpio(struct dvb_frontend *fe, u8 *gpio);
 
+int cxd2820r_wr_reg_val_mask_tab(struct cxd2820r_priv *priv,
+				 const struct reg_val_mask *tab, int tab_len);
+
 int cxd2820r_wr_reg_mask(struct cxd2820r_priv *priv, u32 reg, u8 val,
 	u8 mask);
 
@@ -83,14 +97,6 @@ int cxd2820r_set_frontend_c(struct dvb_frontend *fe);
 
 int cxd2820r_read_status_c(struct dvb_frontend *fe, enum fe_status *status);
 
-int cxd2820r_read_ber_c(struct dvb_frontend *fe, u32 *ber);
-
-int cxd2820r_read_signal_strength_c(struct dvb_frontend *fe, u16 *strength);
-
-int cxd2820r_read_snr_c(struct dvb_frontend *fe, u16 *snr);
-
-int cxd2820r_read_ucblocks_c(struct dvb_frontend *fe, u32 *ucblocks);
-
 int cxd2820r_init_c(struct dvb_frontend *fe);
 
 int cxd2820r_sleep_c(struct dvb_frontend *fe);
@@ -107,14 +113,6 @@ int cxd2820r_set_frontend_t(struct dvb_frontend *fe);
 
 int cxd2820r_read_status_t(struct dvb_frontend *fe, enum fe_status *status);
 
-int cxd2820r_read_ber_t(struct dvb_frontend *fe, u32 *ber);
-
-int cxd2820r_read_signal_strength_t(struct dvb_frontend *fe, u16 *strength);
-
-int cxd2820r_read_snr_t(struct dvb_frontend *fe, u16 *snr);
-
-int cxd2820r_read_ucblocks_t(struct dvb_frontend *fe, u32 *ucblocks);
-
 int cxd2820r_init_t(struct dvb_frontend *fe);
 
 int cxd2820r_sleep_t(struct dvb_frontend *fe);
@@ -131,14 +129,6 @@ int cxd2820r_set_frontend_t2(struct dvb_frontend *fe);
 
 int cxd2820r_read_status_t2(struct dvb_frontend *fe, enum fe_status *status);
 
-int cxd2820r_read_ber_t2(struct dvb_frontend *fe, u32 *ber);
-
-int cxd2820r_read_signal_strength_t2(struct dvb_frontend *fe, u16 *strength);
-
-int cxd2820r_read_snr_t2(struct dvb_frontend *fe, u16 *snr);
-
-int cxd2820r_read_ucblocks_t2(struct dvb_frontend *fe, u32 *ucblocks);
-
 int cxd2820r_init_t2(struct dvb_frontend *fe);
 
 int cxd2820r_sleep_t2(struct dvb_frontend *fe);
diff --git a/drivers/media/dvb-frontends/cxd2820r_t.c b/drivers/media/dvb-frontends/cxd2820r_t.c
index 75ce7d8ded00..c2e7caf9b010 100644
--- a/drivers/media/dvb-frontends/cxd2820r_t.c
+++ b/drivers/media/dvb-frontends/cxd2820r_t.c
@@ -24,10 +24,11 @@
 int cxd2820r_set_frontend_t(struct dvb_frontend *fe)
 {
 	struct cxd2820r_priv *priv = fe->demodulator_priv;
+	struct i2c_client *client = priv->client[0];
 	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
-	int ret, i, bw_i;
-	u32 if_freq, if_ctl;
-	u64 num;
+	int ret, bw_i;
+	unsigned int utmp;
+	u32 if_frequency;
 	u8 buf[3], bw_param;
 	u8 bw_params1[][5] = {
 		{ 0x17, 0xea, 0xaa, 0xaa, 0xaa }, /* 6 MHz */
@@ -45,9 +46,9 @@ int cxd2820r_set_frontend_t(struct dvb_frontend *fe)
 		{ 0x00085, 0x07, 0xff },
 		{ 0x00088, 0x01, 0xff },
 
-		{ 0x00070, priv->cfg.ts_mode, 0xff },
-		{ 0x00071, !priv->cfg.ts_clock_inv << 4, 0x10 },
-		{ 0x000cb, priv->cfg.if_agc_polarity << 6, 0x40 },
+		{ 0x00070, priv->ts_mode, 0xff },
+		{ 0x00071, !priv->ts_clk_inv << 4, 0x10 },
+		{ 0x000cb, priv->if_agc_polarity << 6, 0x40 },
 		{ 0x000a5, 0x00, 0x01 },
 		{ 0x00082, 0x20, 0x60 },
 		{ 0x000c2, 0xc3, 0xff },
@@ -55,8 +56,10 @@ int cxd2820r_set_frontend_t(struct dvb_frontend *fe)
 		{ 0x00427, 0x41, 0xff },
 	};
 
-	dev_dbg(&priv->i2c->dev, "%s: frequency=%d bandwidth_hz=%d\n", __func__,
-			c->frequency, c->bandwidth_hz);
+	dev_dbg(&client->dev,
+		"delivery_system=%d modulation=%d frequency=%u bandwidth_hz=%u inversion=%d\n",
+		c->delivery_system, c->modulation, c->frequency,
+		c->bandwidth_hz, c->inversion);
 
 	switch (c->bandwidth_hz) {
 	case 6000000:
@@ -80,12 +83,9 @@ int cxd2820r_set_frontend_t(struct dvb_frontend *fe)
 		fe->ops.tuner_ops.set_params(fe);
 
 	if (priv->delivery_system != SYS_DVBT) {
-		for (i = 0; i < ARRAY_SIZE(tab); i++) {
-			ret = cxd2820r_wr_reg_mask(priv, tab[i].reg,
-				tab[i].val, tab[i].mask);
-			if (ret)
-				goto error;
-		}
+		ret = cxd2820r_wr_reg_val_mask_tab(priv, tab, ARRAY_SIZE(tab));
+		if (ret)
+			goto error;
 	}
 
 	priv->delivery_system = SYS_DVBT;
@@ -93,48 +93,46 @@ int cxd2820r_set_frontend_t(struct dvb_frontend *fe)
 
 	/* program IF frequency */
 	if (fe->ops.tuner_ops.get_if_frequency) {
-		ret = fe->ops.tuner_ops.get_if_frequency(fe, &if_freq);
+		ret = fe->ops.tuner_ops.get_if_frequency(fe, &if_frequency);
 		if (ret)
 			goto error;
-	} else
-		if_freq = 0;
-
-	dev_dbg(&priv->i2c->dev, "%s: if_freq=%d\n", __func__, if_freq);
-
-	num = if_freq / 1000; /* Hz => kHz */
-	num *= 0x1000000;
-	if_ctl = DIV_ROUND_CLOSEST_ULL(num, 41000);
-	buf[0] = ((if_ctl >> 16) & 0xff);
-	buf[1] = ((if_ctl >>  8) & 0xff);
-	buf[2] = ((if_ctl >>  0) & 0xff);
+		dev_dbg(&client->dev, "if_frequency=%u\n", if_frequency);
+	} else {
+		ret = -EINVAL;
+		goto error;
+	}
 
-	ret = cxd2820r_wr_regs(priv, 0x000b6, buf, 3);
+	utmp = DIV_ROUND_CLOSEST_ULL((u64)if_frequency * 0x1000000, CXD2820R_CLK);
+	buf[0] = (utmp >> 16) & 0xff;
+	buf[1] = (utmp >>  8) & 0xff;
+	buf[2] = (utmp >>  0) & 0xff;
+	ret = regmap_bulk_write(priv->regmap[0], 0x00b6, buf, 3);
 	if (ret)
 		goto error;
 
-	ret = cxd2820r_wr_regs(priv, 0x0009f, bw_params1[bw_i], 5);
+	ret = regmap_bulk_write(priv->regmap[0], 0x009f, bw_params1[bw_i], 5);
 	if (ret)
 		goto error;
 
-	ret = cxd2820r_wr_reg_mask(priv, 0x000d7, bw_param << 6, 0xc0);
+	ret = regmap_update_bits(priv->regmap[0], 0x00d7, 0xc0, bw_param << 6);
 	if (ret)
 		goto error;
 
-	ret = cxd2820r_wr_regs(priv, 0x000d9, bw_params2[bw_i], 2);
+	ret = regmap_bulk_write(priv->regmap[0], 0x00d9, bw_params2[bw_i], 2);
 	if (ret)
 		goto error;
 
-	ret = cxd2820r_wr_reg(priv, 0x000ff, 0x08);
+	ret = regmap_write(priv->regmap[0], 0x00ff, 0x08);
 	if (ret)
 		goto error;
 
-	ret = cxd2820r_wr_reg(priv, 0x000fe, 0x01);
+	ret = regmap_write(priv->regmap[0], 0x00fe, 0x01);
 	if (ret)
 		goto error;
 
 	return ret;
 error:
-	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&client->dev, "failed=%d\n", ret);
 	return ret;
 }
 
@@ -142,10 +140,14 @@ int cxd2820r_get_frontend_t(struct dvb_frontend *fe,
 			    struct dtv_frontend_properties *c)
 {
 	struct cxd2820r_priv *priv = fe->demodulator_priv;
+	struct i2c_client *client = priv->client[0];
 	int ret;
+	unsigned int utmp;
 	u8 buf[2];
 
-	ret = cxd2820r_rd_regs(priv, 0x0002f, buf, sizeof(buf));
+	dev_dbg(&client->dev, "\n");
+
+	ret = regmap_bulk_read(priv->regmap[0], 0x002f, buf, sizeof(buf));
 	if (ret)
 		goto error;
 
@@ -236,11 +238,11 @@ int cxd2820r_get_frontend_t(struct dvb_frontend *fe,
 		break;
 	}
 
-	ret = cxd2820r_rd_reg(priv, 0x007c6, &buf[0]);
+	ret = regmap_read(priv->regmap[0], 0x07c6, &utmp);
 	if (ret)
 		goto error;
 
-	switch ((buf[0] >> 0) & 0x01) {
+	switch ((utmp >> 0) & 0x01) {
 	case 0:
 		c->inversion = INVERSION_OFF;
 		break;
@@ -251,169 +253,158 @@ int cxd2820r_get_frontend_t(struct dvb_frontend *fe,
 
 	return ret;
 error:
-	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
-	return ret;
-}
-
-int cxd2820r_read_ber_t(struct dvb_frontend *fe, u32 *ber)
-{
-	struct cxd2820r_priv *priv = fe->demodulator_priv;
-	int ret;
-	u8 buf[3], start_ber = 0;
-	*ber = 0;
-
-	if (priv->ber_running) {
-		ret = cxd2820r_rd_regs(priv, 0x00076, buf, sizeof(buf));
-		if (ret)
-			goto error;
-
-		if ((buf[2] >> 7) & 0x01 || (buf[2] >> 4) & 0x01) {
-			*ber = (buf[2] & 0x0f) << 16 | buf[1] << 8 | buf[0];
-			start_ber = 1;
-		}
-	} else {
-		priv->ber_running = true;
-		start_ber = 1;
-	}
-
-	if (start_ber) {
-		/* (re)start BER */
-		ret = cxd2820r_wr_reg(priv, 0x00079, 0x01);
-		if (ret)
-			goto error;
-	}
-
-	return ret;
-error:
-	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&client->dev, "failed=%d\n", ret);
 	return ret;
 }
 
-int cxd2820r_read_signal_strength_t(struct dvb_frontend *fe,
-	u16 *strength)
+int cxd2820r_read_status_t(struct dvb_frontend *fe, enum fe_status *status)
 {
 	struct cxd2820r_priv *priv = fe->demodulator_priv;
+	struct i2c_client *client = priv->client[0];
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 	int ret;
-	u8 buf[2];
-	u16 tmp;
+	unsigned int utmp, utmp1, utmp2;
+	u8 buf[3];
 
-	ret = cxd2820r_rd_regs(priv, 0x00026, buf, sizeof(buf));
+	/* Lock detection */
+	ret = regmap_bulk_read(priv->regmap[0], 0x0010, &buf[0], 1);
+	if (ret)
+		goto error;
+	ret = regmap_bulk_read(priv->regmap[0], 0x0073, &buf[1], 1);
 	if (ret)
 		goto error;
 
-	tmp = (buf[0] & 0x0f) << 8 | buf[1];
-	tmp = ~tmp & 0x0fff;
+	utmp1 = (buf[0] >> 0) & 0x07;
+	utmp2 = (buf[1] >> 3) & 0x01;
 
-	/* scale value to 0x0000-0xffff from 0x0000-0x0fff */
-	*strength = tmp * 0xffff / 0x0fff;
+	if (utmp1 == 6 && utmp2 == 1) {
+		*status = FE_HAS_SIGNAL | FE_HAS_CARRIER |
+			  FE_HAS_VITERBI | FE_HAS_SYNC | FE_HAS_LOCK;
+	} else if (utmp1 == 6 || utmp2 == 1) {
+		*status = FE_HAS_SIGNAL | FE_HAS_CARRIER |
+			  FE_HAS_VITERBI | FE_HAS_SYNC;
+	} else {
+		*status = 0;
+	}
 
-	return ret;
-error:
-	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
-	return ret;
-}
+	dev_dbg(&client->dev, "status=%02x raw=%*ph sync=%u ts=%u\n",
+		*status, 2, buf, utmp1, utmp2);
 
-int cxd2820r_read_snr_t(struct dvb_frontend *fe, u16 *snr)
-{
-	struct cxd2820r_priv *priv = fe->demodulator_priv;
-	int ret;
-	u8 buf[2];
-	u16 tmp;
-	/* report SNR in dB * 10 */
+	/* Signal strength */
+	if (*status & FE_HAS_SIGNAL) {
+		unsigned int strength;
 
-	ret = cxd2820r_rd_regs(priv, 0x00028, buf, sizeof(buf));
-	if (ret)
-		goto error;
+		ret = regmap_bulk_read(priv->regmap[0], 0x0026, buf, 2);
+		if (ret)
+			goto error;
 
-	tmp = (buf[0] & 0x1f) << 8 | buf[1];
-	#define CXD2820R_LOG10_8_24 15151336 /* log10(8) << 24 */
-	if (tmp)
-		*snr = (intlog10(tmp) - CXD2820R_LOG10_8_24) / ((1 << 24)
-			/ 100);
-	else
-		*snr = 0;
+		utmp = buf[0] << 8 | buf[1] << 0;
+		utmp = ~utmp & 0x0fff;
+		/* Scale value to 0x0000-0xffff */
+		strength = utmp << 4 | utmp >> 8;
 
-	dev_dbg(&priv->i2c->dev, "%s: dBx10=%d val=%04x\n", __func__, *snr,
-			tmp);
+		c->strength.len = 1;
+		c->strength.stat[0].scale = FE_SCALE_RELATIVE;
+		c->strength.stat[0].uvalue = strength;
+	} else {
+		c->strength.len = 1;
+		c->strength.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	}
 
-	return ret;
-error:
-	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
-	return ret;
-}
+	/* CNR */
+	if (*status & FE_HAS_VITERBI) {
+		unsigned int cnr;
 
-int cxd2820r_read_ucblocks_t(struct dvb_frontend *fe, u32 *ucblocks)
-{
-	*ucblocks = 0;
-	/* no way to read ? */
-	return 0;
-}
+		ret = regmap_bulk_read(priv->regmap[0], 0x002c, buf, 2);
+		if (ret)
+			goto error;
 
-int cxd2820r_read_status_t(struct dvb_frontend *fe, enum fe_status *status)
-{
-	struct cxd2820r_priv *priv = fe->demodulator_priv;
-	int ret;
-	u8 buf[4];
-	*status = 0;
+		utmp = buf[0] << 8 | buf[1] << 0;
+		if (utmp)
+			cnr = div_u64((u64)(intlog10(utmp)
+				      - intlog10(32000 - utmp) + 55532585)
+				      * 10000, (1 << 24));
+		else
+			cnr = 0;
+
+		c->cnr.len = 1;
+		c->cnr.stat[0].scale = FE_SCALE_DECIBEL;
+		c->cnr.stat[0].svalue = cnr;
+	} else {
+		c->cnr.len = 1;
+		c->cnr.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	}
 
-	ret = cxd2820r_rd_reg(priv, 0x00010, &buf[0]);
-	if (ret)
-		goto error;
+	/* BER */
+	if (*status & FE_HAS_SYNC) {
+		unsigned int post_bit_error;
+		bool start_ber;
 
-	if ((buf[0] & 0x07) == 6) {
-		ret = cxd2820r_rd_reg(priv, 0x00073, &buf[1]);
-		if (ret)
-			goto error;
+		if (priv->ber_running) {
+			ret = regmap_bulk_read(priv->regmap[0], 0x0076, buf, 3);
+			if (ret)
+				goto error;
 
-		if (((buf[1] >> 3) & 0x01) == 1) {
-			*status |= FE_HAS_SIGNAL | FE_HAS_CARRIER |
-				FE_HAS_VITERBI | FE_HAS_SYNC | FE_HAS_LOCK;
+			if ((buf[2] >> 7) & 0x01) {
+				post_bit_error = buf[2] << 16 | buf[1] << 8 |
+						 buf[0] << 0;
+				post_bit_error &= 0x0fffff;
+				start_ber = true;
+			} else {
+				post_bit_error = 0;
+				start_ber = false;
+			}
 		} else {
-			*status |= FE_HAS_SIGNAL | FE_HAS_CARRIER |
-				FE_HAS_VITERBI | FE_HAS_SYNC;
+			post_bit_error = 0;
+			start_ber = true;
 		}
-	} else {
-		ret = cxd2820r_rd_reg(priv, 0x00014, &buf[2]);
-		if (ret)
-			goto error;
 
-		if ((buf[2] & 0x0f) >= 4) {
-			ret = cxd2820r_rd_reg(priv, 0x00a14, &buf[3]);
+		if (start_ber) {
+			ret = regmap_write(priv->regmap[0], 0x0079, 0x01);
 			if (ret)
 				goto error;
-
-			if (((buf[3] >> 4) & 0x01) == 1)
-				*status |= FE_HAS_SIGNAL;
+			priv->ber_running = true;
 		}
-	}
 
-	dev_dbg(&priv->i2c->dev, "%s: lock=%*ph\n", __func__, 4, buf);
+		priv->post_bit_error += post_bit_error;
+
+		c->post_bit_error.len = 1;
+		c->post_bit_error.stat[0].scale = FE_SCALE_COUNTER;
+		c->post_bit_error.stat[0].uvalue = priv->post_bit_error;
+	} else {
+		c->post_bit_error.len = 1;
+		c->post_bit_error.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	}
 
 	return ret;
 error:
-	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&client->dev, "failed=%d\n", ret);
 	return ret;
 }
 
 int cxd2820r_init_t(struct dvb_frontend *fe)
 {
 	struct cxd2820r_priv *priv = fe->demodulator_priv;
+	struct i2c_client *client = priv->client[0];
 	int ret;
 
-	ret = cxd2820r_wr_reg(priv, 0x00085, 0x07);
+	dev_dbg(&client->dev, "\n");
+
+	ret = regmap_write(priv->regmap[0], 0x0085, 0x07);
 	if (ret)
 		goto error;
 
 	return ret;
 error:
-	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&client->dev, "failed=%d\n", ret);
 	return ret;
 }
 
 int cxd2820r_sleep_t(struct dvb_frontend *fe)
 {
 	struct cxd2820r_priv *priv = fe->demodulator_priv;
-	int ret, i;
+	struct i2c_client *client = priv->client[0];
+	int ret;
 	struct reg_val_mask tab[] = {
 		{ 0x000ff, 0x1f, 0xff },
 		{ 0x00085, 0x00, 0xff },
@@ -422,20 +413,17 @@ int cxd2820r_sleep_t(struct dvb_frontend *fe)
 		{ 0x00080, 0x00, 0xff },
 	};
 
-	dev_dbg(&priv->i2c->dev, "%s\n", __func__);
+	dev_dbg(&client->dev, "\n");
 
 	priv->delivery_system = SYS_UNDEFINED;
 
-	for (i = 0; i < ARRAY_SIZE(tab); i++) {
-		ret = cxd2820r_wr_reg_mask(priv, tab[i].reg, tab[i].val,
-			tab[i].mask);
-		if (ret)
-			goto error;
-	}
+	ret = cxd2820r_wr_reg_val_mask_tab(priv, tab, ARRAY_SIZE(tab));
+	if (ret)
+		goto error;
 
 	return ret;
 error:
-	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&client->dev, "failed=%d\n", ret);
 	return ret;
 }
 
diff --git a/drivers/media/dvb-frontends/cxd2820r_t2.c b/drivers/media/dvb-frontends/cxd2820r_t2.c
index 704475676234..e641fde75379 100644
--- a/drivers/media/dvb-frontends/cxd2820r_t2.c
+++ b/drivers/media/dvb-frontends/cxd2820r_t2.c
@@ -23,11 +23,12 @@
 
 int cxd2820r_set_frontend_t2(struct dvb_frontend *fe)
 {
-	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 	struct cxd2820r_priv *priv = fe->demodulator_priv;
-	int ret, i, bw_i;
-	u32 if_freq, if_ctl;
-	u64 num;
+	struct i2c_client *client = priv->client[0];
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	int ret, bw_i;
+	unsigned int utmp;
+	u32 if_frequency;
 	u8 buf[3], bw_param;
 	u8 bw_params1[][5] = {
 		{ 0x1c, 0xb3, 0x33, 0x33, 0x33 }, /* 5 MHz */
@@ -45,10 +46,10 @@ int cxd2820r_set_frontend_t2(struct dvb_frontend *fe)
 		{ 0x0207f, 0x2a, 0xff },
 		{ 0x02082, 0x0a, 0xff },
 		{ 0x02083, 0x0a, 0xff },
-		{ 0x020cb, priv->cfg.if_agc_polarity << 6, 0x40 },
-		{ 0x02070, priv->cfg.ts_mode, 0xff },
-		{ 0x02071, !priv->cfg.ts_clock_inv << 6, 0x40 },
-		{ 0x020b5, priv->cfg.spec_inv << 4, 0x10 },
+		{ 0x020cb, priv->if_agc_polarity << 6, 0x40 },
+		{ 0x02070, priv->ts_mode, 0xff },
+		{ 0x02071, !priv->ts_clk_inv << 6, 0x40 },
+		{ 0x020b5, priv->spec_inv << 4, 0x10 },
 		{ 0x02567, 0x07, 0x0f },
 		{ 0x02569, 0x03, 0x03 },
 		{ 0x02595, 0x1a, 0xff },
@@ -69,8 +70,10 @@ int cxd2820r_set_frontend_t2(struct dvb_frontend *fe)
 		{ 0x027ef, 0x10, 0x18 },
 	};
 
-	dev_dbg(&priv->i2c->dev, "%s: frequency=%d bandwidth_hz=%d\n", __func__,
-			c->frequency, c->bandwidth_hz);
+	dev_dbg(&client->dev,
+		"delivery_system=%d modulation=%d frequency=%u bandwidth_hz=%u inversion=%d stream_id=%u\n",
+		c->delivery_system, c->modulation, c->frequency,
+		c->bandwidth_hz, c->inversion, c->stream_id);
 
 	switch (c->bandwidth_hz) {
 	case 5000000:
@@ -98,73 +101,67 @@ int cxd2820r_set_frontend_t2(struct dvb_frontend *fe)
 		fe->ops.tuner_ops.set_params(fe);
 
 	if (priv->delivery_system != SYS_DVBT2) {
-		for (i = 0; i < ARRAY_SIZE(tab); i++) {
-			ret = cxd2820r_wr_reg_mask(priv, tab[i].reg,
-				tab[i].val, tab[i].mask);
-			if (ret)
-				goto error;
-		}
+		ret = cxd2820r_wr_reg_val_mask_tab(priv, tab, ARRAY_SIZE(tab));
+		if (ret)
+			goto error;
 	}
 
 	priv->delivery_system = SYS_DVBT2;
 
 	/* program IF frequency */
 	if (fe->ops.tuner_ops.get_if_frequency) {
-		ret = fe->ops.tuner_ops.get_if_frequency(fe, &if_freq);
+		ret = fe->ops.tuner_ops.get_if_frequency(fe, &if_frequency);
 		if (ret)
 			goto error;
-	} else
-		if_freq = 0;
-
-	dev_dbg(&priv->i2c->dev, "%s: if_freq=%d\n", __func__, if_freq);
+		dev_dbg(&client->dev, "if_frequency=%u\n", if_frequency);
+	} else {
+		ret = -EINVAL;
+		goto error;
+	}
 
-	num = if_freq / 1000; /* Hz => kHz */
-	num *= 0x1000000;
-	if_ctl = DIV_ROUND_CLOSEST_ULL(num, 41000);
-	buf[0] = ((if_ctl >> 16) & 0xff);
-	buf[1] = ((if_ctl >>  8) & 0xff);
-	buf[2] = ((if_ctl >>  0) & 0xff);
+	utmp = DIV_ROUND_CLOSEST_ULL((u64)if_frequency * 0x1000000, CXD2820R_CLK);
+	buf[0] = (utmp >> 16) & 0xff;
+	buf[1] = (utmp >>  8) & 0xff;
+	buf[2] = (utmp >>  0) & 0xff;
+	ret = regmap_bulk_write(priv->regmap[0], 0x20b6, buf, 3);
+	if (ret)
+		goto error;
 
 	/* PLP filtering */
 	if (c->stream_id > 255) {
-		dev_dbg(&priv->i2c->dev, "%s: Disable PLP filtering\n", __func__);
-		ret = cxd2820r_wr_reg(priv, 0x023ad , 0);
+		dev_dbg(&client->dev, "disable PLP filtering\n");
+		ret = regmap_write(priv->regmap[0], 0x23ad, 0x00);
 		if (ret)
 			goto error;
 	} else {
-		dev_dbg(&priv->i2c->dev, "%s: Enable PLP filtering = %d\n", __func__,
-				c->stream_id);
-		ret = cxd2820r_wr_reg(priv, 0x023af , c->stream_id & 0xFF);
+		dev_dbg(&client->dev, "enable PLP filtering\n");
+		ret = regmap_write(priv->regmap[0], 0x23af, c->stream_id & 0xff);
 		if (ret)
 			goto error;
-		ret = cxd2820r_wr_reg(priv, 0x023ad , 1);
+		ret = regmap_write(priv->regmap[0], 0x23ad, 0x01);
 		if (ret)
 			goto error;
 	}
 
-	ret = cxd2820r_wr_regs(priv, 0x020b6, buf, 3);
+	ret = regmap_bulk_write(priv->regmap[0], 0x209f, bw_params1[bw_i], 5);
 	if (ret)
 		goto error;
 
-	ret = cxd2820r_wr_regs(priv, 0x0209f, bw_params1[bw_i], 5);
+	ret = regmap_update_bits(priv->regmap[0], 0x20d7, 0xc0, bw_param << 6);
 	if (ret)
 		goto error;
 
-	ret = cxd2820r_wr_reg_mask(priv, 0x020d7, bw_param << 6, 0xc0);
+	ret = regmap_write(priv->regmap[0], 0x00ff, 0x08);
 	if (ret)
 		goto error;
 
-	ret = cxd2820r_wr_reg(priv, 0x000ff, 0x08);
-	if (ret)
-		goto error;
-
-	ret = cxd2820r_wr_reg(priv, 0x000fe, 0x01);
+	ret = regmap_write(priv->regmap[0], 0x00fe, 0x01);
 	if (ret)
 		goto error;
 
 	return ret;
 error:
-	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&client->dev, "failed=%d\n", ret);
 	return ret;
 
 }
@@ -173,10 +170,14 @@ int cxd2820r_get_frontend_t2(struct dvb_frontend *fe,
 			     struct dtv_frontend_properties *c)
 {
 	struct cxd2820r_priv *priv = fe->demodulator_priv;
+	struct i2c_client *client = priv->client[0];
 	int ret;
+	unsigned int utmp;
 	u8 buf[2];
 
-	ret = cxd2820r_rd_regs(priv, 0x0205c, buf, 2);
+	dev_dbg(&client->dev, "\n");
+
+	ret = regmap_bulk_read(priv->regmap[0], 0x205c, buf, 2);
 	if (ret)
 		goto error;
 
@@ -225,7 +226,7 @@ int cxd2820r_get_frontend_t2(struct dvb_frontend *fe,
 		break;
 	}
 
-	ret = cxd2820r_rd_regs(priv, 0x0225b, buf, 2);
+	ret = regmap_bulk_read(priv->regmap[0], 0x225b, buf, 2);
 	if (ret)
 		goto error;
 
@@ -265,11 +266,11 @@ int cxd2820r_get_frontend_t2(struct dvb_frontend *fe,
 		break;
 	}
 
-	ret = cxd2820r_rd_reg(priv, 0x020b5, &buf[0]);
+	ret = regmap_read(priv->regmap[0], 0x20b5, &utmp);
 	if (ret)
 		goto error;
 
-	switch ((buf[0] >> 4) & 0x01) {
+	switch ((utmp >> 4) & 0x01) {
 	case 0:
 		c->inversion = INVERSION_OFF;
 		break;
@@ -280,130 +281,124 @@ int cxd2820r_get_frontend_t2(struct dvb_frontend *fe,
 
 	return ret;
 error:
-	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&client->dev, "failed=%d\n", ret);
 	return ret;
 }
 
 int cxd2820r_read_status_t2(struct dvb_frontend *fe, enum fe_status *status)
 {
 	struct cxd2820r_priv *priv = fe->demodulator_priv;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	struct i2c_client *client = priv->client[0];
 	int ret;
-	u8 buf[1];
-	*status = 0;
+	unsigned int utmp, utmp1, utmp2;
+	u8 buf[4];
 
-	ret = cxd2820r_rd_reg(priv, 0x02010 , &buf[0]);
+	/* Lock detection */
+	ret = regmap_bulk_read(priv->regmap[0], 0x2010, &buf[0], 1);
 	if (ret)
 		goto error;
 
-	if ((buf[0] & 0x07) == 6) {
-		if (((buf[0] >> 5) & 0x01) == 1) {
-			*status |= FE_HAS_SIGNAL | FE_HAS_CARRIER |
-				FE_HAS_VITERBI | FE_HAS_SYNC | FE_HAS_LOCK;
-		} else {
-			*status |= FE_HAS_SIGNAL | FE_HAS_CARRIER |
-				FE_HAS_VITERBI | FE_HAS_SYNC;
-		}
-	}
+	utmp1 = (buf[0] >> 0) & 0x07;
+	utmp2 = (buf[0] >> 5) & 0x01;
 
-	dev_dbg(&priv->i2c->dev, "%s: lock=%02x\n", __func__, buf[0]);
+	if (utmp1 == 6 && utmp2 == 1) {
+		*status = FE_HAS_SIGNAL | FE_HAS_CARRIER |
+			  FE_HAS_VITERBI | FE_HAS_SYNC | FE_HAS_LOCK;
+	} else if (utmp1 == 6 || utmp2 == 1) {
+		*status = FE_HAS_SIGNAL | FE_HAS_CARRIER |
+			  FE_HAS_VITERBI | FE_HAS_SYNC;
+	} else {
+		*status = 0;
+	}
 
-	return ret;
-error:
-	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
-	return ret;
-}
+	dev_dbg(&client->dev, "status=%02x raw=%*ph sync=%u ts=%u\n",
+		*status, 1, buf, utmp1, utmp2);
 
-int cxd2820r_read_ber_t2(struct dvb_frontend *fe, u32 *ber)
-{
-	struct cxd2820r_priv *priv = fe->demodulator_priv;
-	int ret;
-	u8 buf[4];
-	unsigned int errbits;
-	*ber = 0;
-	/* FIXME: correct calculation */
+	/* Signal strength */
+	if (*status & FE_HAS_SIGNAL) {
+		unsigned int strength;
 
-	ret = cxd2820r_rd_regs(priv, 0x02039, buf, sizeof(buf));
-	if (ret)
-		goto error;
+		ret = regmap_bulk_read(priv->regmap[0], 0x2026, buf, 2);
+		if (ret)
+			goto error;
 
-	if ((buf[0] >> 4) & 0x01) {
-		errbits = (buf[0] & 0x0f) << 24 | buf[1] << 16 |
-			buf[2] << 8 | buf[3];
+		utmp = buf[0] << 8 | buf[1] << 0;
+		utmp = ~utmp & 0x0fff;
+		/* Scale value to 0x0000-0xffff */
+		strength = utmp << 4 | utmp >> 8;
 
-		if (errbits)
-			*ber = errbits * 64 / 16588800;
+		c->strength.len = 1;
+		c->strength.stat[0].scale = FE_SCALE_RELATIVE;
+		c->strength.stat[0].uvalue = strength;
+	} else {
+		c->strength.len = 1;
+		c->strength.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
 	}
 
-	return ret;
-error:
-	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
-	return ret;
-}
+	/* CNR */
+	if (*status & FE_HAS_VITERBI) {
+		unsigned int cnr;
 
-int cxd2820r_read_signal_strength_t2(struct dvb_frontend *fe,
-	u16 *strength)
-{
-	struct cxd2820r_priv *priv = fe->demodulator_priv;
-	int ret;
-	u8 buf[2];
-	u16 tmp;
-
-	ret = cxd2820r_rd_regs(priv, 0x02026, buf, sizeof(buf));
-	if (ret)
-		goto error;
-
-	tmp = (buf[0] & 0x0f) << 8 | buf[1];
-	tmp = ~tmp & 0x0fff;
+		ret = regmap_bulk_read(priv->regmap[0], 0x2028, buf, 2);
+		if (ret)
+			goto error;
 
-	/* scale value to 0x0000-0xffff from 0x0000-0x0fff */
-	*strength = tmp * 0xffff / 0x0fff;
+		utmp = buf[0] << 8 | buf[1] << 0;
+		utmp = utmp & 0x0fff;
+		#define CXD2820R_LOG10_8_24 15151336 /* log10(8) << 24 */
+		if (utmp)
+			cnr = div_u64((u64)(intlog10(utmp)
+				      - CXD2820R_LOG10_8_24) * 10000,
+				      (1 << 24));
+		else
+			cnr = 0;
+
+		c->cnr.len = 1;
+		c->cnr.stat[0].scale = FE_SCALE_DECIBEL;
+		c->cnr.stat[0].svalue = cnr;
+	} else {
+		c->cnr.len = 1;
+		c->cnr.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	}
 
-	return ret;
-error:
-	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
-	return ret;
-}
+	/* BER */
+	if (*status & FE_HAS_SYNC) {
+		unsigned int post_bit_error;
 
-int cxd2820r_read_snr_t2(struct dvb_frontend *fe, u16 *snr)
-{
-	struct cxd2820r_priv *priv = fe->demodulator_priv;
-	int ret;
-	u8 buf[2];
-	u16 tmp;
-	/* report SNR in dB * 10 */
+		ret = regmap_bulk_read(priv->regmap[0], 0x2039, buf, 4);
+		if (ret)
+			goto error;
 
-	ret = cxd2820r_rd_regs(priv, 0x02028, buf, sizeof(buf));
-	if (ret)
-		goto error;
+		if ((buf[0] >> 4) & 0x01) {
+			post_bit_error = buf[0] << 24 | buf[1] << 16 |
+					 buf[2] << 8 | buf[3] << 0;
+			post_bit_error &= 0x0fffffff;
+		} else {
+			post_bit_error = 0;
+		}
 
-	tmp = (buf[0] & 0x0f) << 8 | buf[1];
-	#define CXD2820R_LOG10_8_24 15151336 /* log10(8) << 24 */
-	if (tmp)
-		*snr = (intlog10(tmp) - CXD2820R_LOG10_8_24) / ((1 << 24)
-			/ 100);
-	else
-		*snr = 0;
+		priv->post_bit_error += post_bit_error;
 
-	dev_dbg(&priv->i2c->dev, "%s: dBx10=%d val=%04x\n", __func__, *snr,
-			tmp);
+		c->post_bit_error.len = 1;
+		c->post_bit_error.stat[0].scale = FE_SCALE_COUNTER;
+		c->post_bit_error.stat[0].uvalue = priv->post_bit_error;
+	} else {
+		c->post_bit_error.len = 1;
+		c->post_bit_error.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	}
 
 	return ret;
 error:
-	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&client->dev, "failed=%d\n", ret);
 	return ret;
 }
 
-int cxd2820r_read_ucblocks_t2(struct dvb_frontend *fe, u32 *ucblocks)
-{
-	*ucblocks = 0;
-	/* no way to read ? */
-	return 0;
-}
-
 int cxd2820r_sleep_t2(struct dvb_frontend *fe)
 {
 	struct cxd2820r_priv *priv = fe->demodulator_priv;
-	int ret, i;
+	struct i2c_client *client = priv->client[0];
+	int ret;
 	struct reg_val_mask tab[] = {
 		{ 0x000ff, 0x1f, 0xff },
 		{ 0x00085, 0x00, 0xff },
@@ -413,20 +408,17 @@ int cxd2820r_sleep_t2(struct dvb_frontend *fe)
 		{ 0x00080, 0x00, 0xff },
 	};
 
-	dev_dbg(&priv->i2c->dev, "%s\n", __func__);
+	dev_dbg(&client->dev, "\n");
 
-	for (i = 0; i < ARRAY_SIZE(tab); i++) {
-		ret = cxd2820r_wr_reg_mask(priv, tab[i].reg, tab[i].val,
-			tab[i].mask);
-		if (ret)
-			goto error;
-	}
+	ret = cxd2820r_wr_reg_val_mask_tab(priv, tab, ARRAY_SIZE(tab));
+	if (ret)
+		goto error;
 
 	priv->delivery_system = SYS_UNDEFINED;
 
 	return ret;
 error:
-	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&client->dev, "failed=%d\n", ret);
 	return ret;
 }
 
diff --git a/drivers/media/dvb-frontends/cxd2841er.c b/drivers/media/dvb-frontends/cxd2841er.c
index ffe88bc6b813..5afb9c508f65 100644
--- a/drivers/media/dvb-frontends/cxd2841er.c
+++ b/drivers/media/dvb-frontends/cxd2841er.c
@@ -206,6 +206,9 @@ static const struct cxd2841er_cnr_data s2_cn_data[] = {
 		(u32)(((iffreq)/48.0)*16777216.0 + 0.5) : \
 		(u32)(((iffreq)/41.0)*16777216.0 + 0.5))
 
+static int cxd2841er_freeze_regs(struct cxd2841er_priv *priv);
+static int cxd2841er_unfreeze_regs(struct cxd2841er_priv *priv);
+
 static void cxd2841er_i2c_debug(struct cxd2841er_priv *priv,
 				u8 addr, u8 reg, u8 write,
 				const u8 *data, u32 len)
@@ -1401,6 +1404,41 @@ static int cxd2841er_read_ber_c(struct cxd2841er_priv *priv,
 	return 0;
 }
 
+static int cxd2841er_read_ber_i(struct cxd2841er_priv *priv,
+		u32 *bit_error, u32 *bit_count)
+{
+	u8 data[3];
+	u8 pktnum[2];
+
+	dev_dbg(&priv->i2c->dev, "%s()\n", __func__);
+	if (priv->state != STATE_ACTIVE_TC) {
+		dev_dbg(&priv->i2c->dev, "%s(): invalid state %d\n",
+				__func__, priv->state);
+		return -EINVAL;
+	}
+
+	cxd2841er_freeze_regs(priv);
+	cxd2841er_write_reg(priv, I2C_SLVT, 0x00, 0x60);
+	cxd2841er_read_regs(priv, I2C_SLVT, 0x5B, pktnum, sizeof(pktnum));
+	cxd2841er_read_regs(priv, I2C_SLVT, 0x16, data, sizeof(data));
+
+	if (!pktnum[0] && !pktnum[1]) {
+		dev_dbg(&priv->i2c->dev,
+				"%s(): no valid BER data\n", __func__);
+		cxd2841er_unfreeze_regs(priv);
+		return -EINVAL;
+	}
+
+	*bit_error = ((u32)(data[0] & 0x7F) << 16) |
+		((u32)data[1] << 8) | data[2];
+	*bit_count = ((((u32)pktnum[0] << 8) | pktnum[1]) * 204 * 8);
+	dev_dbg(&priv->i2c->dev, "%s(): bit_error=%u bit_count=%u\n",
+			__func__, *bit_error, *bit_count);
+
+	cxd2841er_unfreeze_regs(priv);
+	return 0;
+}
+
 static int cxd2841er_mon_read_ber_s(struct cxd2841er_priv *priv,
 				    u32 *bit_error, u32 *bit_count)
 {
@@ -1570,6 +1608,25 @@ static int cxd2841er_read_ber_t(struct cxd2841er_priv *priv,
 	return 0;
 }
 
+static int cxd2841er_freeze_regs(struct cxd2841er_priv *priv)
+{
+	/*
+	 * Freeze registers: ensure multiple separate register reads
+	 * are from the same snapshot
+	 */
+	cxd2841er_write_reg(priv, I2C_SLVT, 0x01, 0x01);
+	return 0;
+}
+
+static int cxd2841er_unfreeze_regs(struct cxd2841er_priv *priv)
+{
+	/*
+	 * un-freeze registers
+	 */
+	cxd2841er_write_reg(priv, I2C_SLVT, 0x01, 0x00);
+	return 0;
+}
+
 static u32 cxd2841er_dvbs_read_snr(struct cxd2841er_priv *priv,
 		u8 delsys, u32 *snr)
 {
@@ -1578,6 +1635,7 @@ static u32 cxd2841er_dvbs_read_snr(struct cxd2841er_priv *priv,
 	int min_index, max_index, index;
 	static const struct cxd2841er_cnr_data *cn_data;
 
+	cxd2841er_freeze_regs(priv);
 	/* Set SLV-T Bank : 0xA1 */
 	cxd2841er_write_reg(priv, I2C_SLVT, 0x00, 0xa1);
 	/*
@@ -1629,9 +1687,11 @@ static u32 cxd2841er_dvbs_read_snr(struct cxd2841er_priv *priv,
 	} else {
 		dev_dbg(&priv->i2c->dev,
 			"%s(): no data available\n", __func__);
+		cxd2841er_unfreeze_regs(priv);
 		return -EINVAL;
 	}
 done:
+	cxd2841er_unfreeze_regs(priv);
 	*snr = res;
 	return 0;
 }
@@ -1655,12 +1715,7 @@ static int cxd2841er_read_snr_c(struct cxd2841er_priv *priv, u32 *snr)
 		return -EINVAL;
 	}
 
-	/*
-	 * Freeze registers: ensure multiple separate register reads
-	 * are from the same snapshot
-	 */
-	cxd2841er_write_reg(priv, I2C_SLVT, 0x01, 0x01);
-
+	cxd2841er_freeze_regs(priv);
 	cxd2841er_write_reg(priv, I2C_SLVT, 0x00, 0x40);
 	cxd2841er_read_regs(priv, I2C_SLVT, 0x19, data, 1);
 	qam = (enum sony_dvbc_constellation_t) (data[0] & 0x07);
@@ -1670,6 +1725,7 @@ static int cxd2841er_read_snr_c(struct cxd2841er_priv *priv, u32 *snr)
 	if (reg == 0) {
 		dev_dbg(&priv->i2c->dev,
 				"%s(): reg value out of range\n", __func__);
+		cxd2841er_unfreeze_regs(priv);
 		return 0;
 	}
 
@@ -1690,9 +1746,11 @@ static int cxd2841er_read_snr_c(struct cxd2841er_priv *priv, u32 *snr)
 		*snr = -88 * (int32_t)sony_log(reg) + 86999;
 		break;
 	default:
+		cxd2841er_unfreeze_regs(priv);
 		return -EINVAL;
 	}
 
+	cxd2841er_unfreeze_regs(priv);
 	return 0;
 }
 
@@ -1707,17 +1765,21 @@ static int cxd2841er_read_snr_t(struct cxd2841er_priv *priv, u32 *snr)
 			"%s(): invalid state %d\n", __func__, priv->state);
 		return -EINVAL;
 	}
+
+	cxd2841er_freeze_regs(priv);
 	cxd2841er_write_reg(priv, I2C_SLVT, 0x00, 0x10);
 	cxd2841er_read_regs(priv, I2C_SLVT, 0x28, data, sizeof(data));
 	reg = ((u32)data[0] << 8) | (u32)data[1];
 	if (reg == 0) {
 		dev_dbg(&priv->i2c->dev,
 			"%s(): reg value out of range\n", __func__);
+		cxd2841er_unfreeze_regs(priv);
 		return 0;
 	}
 	if (reg > 4996)
 		reg = 4996;
 	*snr = 10000 * ((intlog10(reg) - intlog10(5350 - reg)) >> 24) + 28500;
+	cxd2841er_unfreeze_regs(priv);
 	return 0;
 }
 
@@ -1732,18 +1794,22 @@ static int cxd2841er_read_snr_t2(struct cxd2841er_priv *priv, u32 *snr)
 			"%s(): invalid state %d\n", __func__, priv->state);
 		return -EINVAL;
 	}
+
+	cxd2841er_freeze_regs(priv);
 	cxd2841er_write_reg(priv, I2C_SLVT, 0x00, 0x20);
 	cxd2841er_read_regs(priv, I2C_SLVT, 0x28, data, sizeof(data));
 	reg = ((u32)data[0] << 8) | (u32)data[1];
 	if (reg == 0) {
 		dev_dbg(&priv->i2c->dev,
 			"%s(): reg value out of range\n", __func__);
+		cxd2841er_unfreeze_regs(priv);
 		return 0;
 	}
 	if (reg > 10876)
 		reg = 10876;
 	*snr = 10000 * ((intlog10(reg) -
 		intlog10(12600 - reg)) >> 24) + 32000;
+	cxd2841er_unfreeze_regs(priv);
 	return 0;
 }
 
@@ -1760,21 +1826,18 @@ static int cxd2841er_read_snr_i(struct cxd2841er_priv *priv, u32 *snr)
 		return -EINVAL;
 	}
 
-	/* Freeze all registers */
-	cxd2841er_write_reg(priv, I2C_SLVT, 0x01, 0x01);
-
-
+	cxd2841er_freeze_regs(priv);
 	cxd2841er_write_reg(priv, I2C_SLVT, 0x00, 0x60);
 	cxd2841er_read_regs(priv, I2C_SLVT, 0x28, data, sizeof(data));
 	reg = ((u32)data[0] << 8) | (u32)data[1];
 	if (reg == 0) {
 		dev_dbg(&priv->i2c->dev,
 				"%s(): reg value out of range\n", __func__);
+		cxd2841er_unfreeze_regs(priv);
 		return 0;
 	}
-	if (reg > 4996)
-		reg = 4996;
-	*snr = 100 * intlog10(reg) - 9031;
+	*snr = 10000 * (intlog10(reg) >> 24) - 9031;
+	cxd2841er_unfreeze_regs(priv);
 	return 0;
 }
 
@@ -1852,6 +1915,9 @@ static void cxd2841er_read_ber(struct dvb_frontend *fe)
 	case SYS_DVBC_ANNEX_C:
 		ret = cxd2841er_read_ber_c(priv, &bit_error, &bit_count);
 		break;
+	case SYS_ISDBT:
+		ret = cxd2841er_read_ber_i(priv, &bit_error, &bit_count);
+		break;
 	case SYS_DVBS:
 		ret = cxd2841er_mon_read_ber_s(priv, &bit_error, &bit_count);
 		break;
@@ -1965,6 +2031,9 @@ static void cxd2841er_read_snr(struct dvb_frontend *fe)
 		return;
 	}
 
+	dev_dbg(&priv->i2c->dev, "%s(): snr=%d\n",
+			__func__, (int32_t)tmp);
+
 	if (!ret) {
 		p->cnr.stat[0].scale = FE_SCALE_DECIBEL;
 		p->cnr.stat[0].svalue = tmp;
@@ -1977,7 +2046,7 @@ static void cxd2841er_read_ucblocks(struct dvb_frontend *fe)
 {
 	struct dtv_frontend_properties *p = &fe->dtv_property_cache;
 	struct cxd2841er_priv *priv = fe->demodulator_priv;
-	u32 ucblocks;
+	u32 ucblocks = 0;
 
 	dev_dbg(&priv->i2c->dev, "%s()\n", __func__);
 	switch (p->delivery_system) {
@@ -1999,7 +2068,7 @@ static void cxd2841er_read_ucblocks(struct dvb_frontend *fe)
 		p->block_error.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
 		return;
 	}
-	dev_dbg(&priv->i2c->dev, "%s()\n", __func__);
+	dev_dbg(&priv->i2c->dev, "%s() ucblocks=%u\n", __func__, ucblocks);
 
 	p->block_error.stat[0].scale = FE_SCALE_COUNTER;
 	p->block_error.stat[0].uvalue = ucblocks;
@@ -2694,6 +2763,14 @@ static int cxd2841er_sleep_tc_to_active_c_band(struct cxd2841er_priv *priv,
 	u8 b10_b6[3];
 	u32 iffreq;
 
+	if (bandwidth != 6000000 &&
+			bandwidth != 7000000 &&
+			bandwidth != 8000000) {
+		dev_info(&priv->i2c->dev, "%s(): unsupported bandwidth %d. Forcing 8Mhz!\n",
+				__func__, bandwidth);
+		bandwidth = 8000000;
+	}
+
 	dev_dbg(&priv->i2c->dev, "%s() bw=%d\n", __func__, bandwidth);
 	cxd2841er_write_reg(priv, I2C_SLVT, 0x00, 0x10);
 	switch (bandwidth) {
@@ -3076,6 +3153,7 @@ static int cxd2841er_sleep_tc_to_active_c(struct cxd2841er_priv *priv,
 	/* Enable demod clock */
 	cxd2841er_write_reg(priv, I2C_SLVT, 0x2c, 0x01);
 	/* Disable RF level monitor */
+	cxd2841er_write_reg(priv, I2C_SLVT, 0x59, 0x00);
 	cxd2841er_write_reg(priv, I2C_SLVT, 0x2f, 0x00);
 	/* Enable ADC clock */
 	cxd2841er_write_reg(priv, I2C_SLVT, 0x30, 0x00);
diff --git a/drivers/media/dvb-frontends/cxd2841er.h b/drivers/media/dvb-frontends/cxd2841er.h
index 62ad5f07390b..7f1acfb8f4f5 100644
--- a/drivers/media/dvb-frontends/cxd2841er.h
+++ b/drivers/media/dvb-frontends/cxd2841er.h
@@ -22,7 +22,6 @@
 #ifndef CXD2841ER_H
 #define CXD2841ER_H
 
-#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 enum cxd2841er_xtal {
diff --git a/drivers/media/dvb-frontends/dib3000mc.h b/drivers/media/dvb-frontends/dib3000mc.h
index b37e69e6a58c..67a6d50865fb 100644
--- a/drivers/media/dvb-frontends/dib3000mc.h
+++ b/drivers/media/dvb-frontends/dib3000mc.h
@@ -13,8 +13,6 @@
 #ifndef DIB3000MC_H
 #define DIB3000MC_H
 
-#include <linux/kconfig.h>
-
 #include "dibx000_common.h"
 
 struct dib3000mc_config {
diff --git a/drivers/media/dvb-frontends/dib7000m.h b/drivers/media/dvb-frontends/dib7000m.h
index 6468c278cc4d..8f84dfa9bb58 100644
--- a/drivers/media/dvb-frontends/dib7000m.h
+++ b/drivers/media/dvb-frontends/dib7000m.h
@@ -1,8 +1,6 @@
 #ifndef DIB7000M_H
 #define DIB7000M_H
 
-#include <linux/kconfig.h>
-
 #include "dibx000_common.h"
 
 struct dib7000m_config {
diff --git a/drivers/media/dvb-frontends/dib7000p.h b/drivers/media/dvb-frontends/dib7000p.h
index baa278928cf3..205fbbff632b 100644
--- a/drivers/media/dvb-frontends/dib7000p.h
+++ b/drivers/media/dvb-frontends/dib7000p.h
@@ -1,8 +1,6 @@
 #ifndef DIB7000P_H
 #define DIB7000P_H
 
-#include <linux/kconfig.h>
-
 #include "dibx000_common.h"
 
 struct dib7000p_config {
diff --git a/drivers/media/dvb-frontends/drxd.h b/drivers/media/dvb-frontends/drxd.h
index a47c22d6667e..f0507cdbb503 100644
--- a/drivers/media/dvb-frontends/drxd.h
+++ b/drivers/media/dvb-frontends/drxd.h
@@ -24,7 +24,6 @@
 #ifndef _DRXD_H_
 #define _DRXD_H_
 
-#include <linux/kconfig.h>
 #include <linux/types.h>
 #include <linux/i2c.h>
 
diff --git a/drivers/media/dvb-frontends/drxk.h b/drivers/media/dvb-frontends/drxk.h
index 8f0b9eec528f..a629897eb905 100644
--- a/drivers/media/dvb-frontends/drxk.h
+++ b/drivers/media/dvb-frontends/drxk.h
@@ -1,7 +1,6 @@
 #ifndef _DRXK_H_
 #define _DRXK_H_
 
-#include <linux/kconfig.h>
 #include <linux/types.h>
 #include <linux/i2c.h>
 
diff --git a/drivers/media/dvb-frontends/drxk_hard.c b/drivers/media/dvb-frontends/drxk_hard.c
index b975da099929..c595adc61c6f 100644
--- a/drivers/media/dvb-frontends/drxk_hard.c
+++ b/drivers/media/dvb-frontends/drxk_hard.c
@@ -6448,7 +6448,7 @@ static int get_strength(struct drxk_state *state, u64 *strength)
 			return status;
 
 		/* SCU c.o.c. */
-		read16(state, SCU_RAM_AGC_RF_IACCU_HI_CO__A, &scu_coc);
+		status = read16(state, SCU_RAM_AGC_RF_IACCU_HI_CO__A, &scu_coc);
 		if (status < 0)
 			return status;
 
diff --git a/drivers/media/dvb-frontends/ds3000.h b/drivers/media/dvb-frontends/ds3000.h
index 153169da9017..82e8c2531f26 100644
--- a/drivers/media/dvb-frontends/ds3000.h
+++ b/drivers/media/dvb-frontends/ds3000.h
@@ -22,7 +22,6 @@
 #ifndef DS3000_H
 #define DS3000_H
 
-#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 struct ds3000_config {
diff --git a/drivers/media/dvb-frontends/dvb-pll.c b/drivers/media/dvb-frontends/dvb-pll.c
index 53089e142715..735a96662022 100644
--- a/drivers/media/dvb-frontends/dvb-pll.c
+++ b/drivers/media/dvb-frontends/dvb-pll.c
@@ -739,7 +739,7 @@ static int dvb_pll_init(struct dvb_frontend *fe)
 	return -EINVAL;
 }
 
-static struct dvb_tuner_ops dvb_pll_tuner_ops = {
+static const struct dvb_tuner_ops dvb_pll_tuner_ops = {
 	.release = dvb_pll_release,
 	.sleep = dvb_pll_sleep,
 	.init = dvb_pll_init,
diff --git a/drivers/media/dvb-frontends/dvb_dummy_fe.h b/drivers/media/dvb-frontends/dvb_dummy_fe.h
index 15e4ceab869a..50f1af512b62 100644
--- a/drivers/media/dvb-frontends/dvb_dummy_fe.h
+++ b/drivers/media/dvb-frontends/dvb_dummy_fe.h
@@ -22,7 +22,6 @@
 #ifndef DVB_DUMMY_FE_H
 #define DVB_DUMMY_FE_H
 
-#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 #include "dvb_frontend.h"
 
diff --git a/drivers/media/dvb-frontends/ec100.h b/drivers/media/dvb-frontends/ec100.h
index 9544bab5cd1d..e894bdcf35a3 100644
--- a/drivers/media/dvb-frontends/ec100.h
+++ b/drivers/media/dvb-frontends/ec100.h
@@ -22,7 +22,6 @@
 #ifndef EC100_H
 #define EC100_H
 
-#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 struct ec100_config {
diff --git a/drivers/media/dvb-frontends/hd29l2.h b/drivers/media/dvb-frontends/hd29l2.h
index 48e9ab74c883..a14d6f36dbf6 100644
--- a/drivers/media/dvb-frontends/hd29l2.h
+++ b/drivers/media/dvb-frontends/hd29l2.h
@@ -23,7 +23,6 @@
 #ifndef HD29L2_H
 #define HD29L2_H
 
-#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 struct hd29l2_config {
diff --git a/drivers/media/dvb-frontends/helene.c b/drivers/media/dvb-frontends/helene.c
index 97a8982740a6..dc43c5f6d0ea 100644
--- a/drivers/media/dvb-frontends/helene.c
+++ b/drivers/media/dvb-frontends/helene.c
@@ -842,7 +842,7 @@ static int helene_get_frequency(struct dvb_frontend *fe, u32 *frequency)
 	return 0;
 }
 
-static struct dvb_tuner_ops helene_tuner_ops = {
+static const struct dvb_tuner_ops helene_tuner_ops = {
 	.info = {
 		.name = "Sony HELENE Ter tuner",
 		.frequency_min = 1000000,
@@ -856,7 +856,7 @@ static struct dvb_tuner_ops helene_tuner_ops = {
 	.get_frequency = helene_get_frequency,
 };
 
-static struct dvb_tuner_ops helene_tuner_ops_s = {
+static const struct dvb_tuner_ops helene_tuner_ops_s = {
 	.info = {
 		.name = "Sony HELENE Sat tuner",
 		.frequency_min = 500000,
@@ -987,8 +987,10 @@ struct dvb_frontend *helene_attach_s(struct dvb_frontend *fe,
 	if (fe->ops.i2c_gate_ctrl)
 		fe->ops.i2c_gate_ctrl(fe, 1);
 
-	if (helene_x_pon(priv) != 0)
+	if (helene_x_pon(priv) != 0) {
+		kfree(priv);
 		return NULL;
+	}
 
 	if (fe->ops.i2c_gate_ctrl)
 		fe->ops.i2c_gate_ctrl(fe, 0);
@@ -1021,8 +1023,10 @@ struct dvb_frontend *helene_attach(struct dvb_frontend *fe,
 	if (fe->ops.i2c_gate_ctrl)
 		fe->ops.i2c_gate_ctrl(fe, 1);
 
-	if (helene_x_pon(priv) != 0)
+	if (helene_x_pon(priv) != 0) {
+		kfree(priv);
 		return NULL;
+	}
 
 	if (fe->ops.i2c_gate_ctrl)
 		fe->ops.i2c_gate_ctrl(fe, 0);
diff --git a/drivers/media/dvb-frontends/helene.h b/drivers/media/dvb-frontends/helene.h
index e1b9224cfc55..333615491d9e 100644
--- a/drivers/media/dvb-frontends/helene.h
+++ b/drivers/media/dvb-frontends/helene.h
@@ -21,7 +21,6 @@
 #ifndef __DVB_HELENE_H__
 #define __DVB_HELENE_H__
 
-#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 #include <linux/i2c.h>
 
diff --git a/drivers/media/dvb-frontends/horus3a.c b/drivers/media/dvb-frontends/horus3a.c
index a98bca5270d9..0c089b5986a1 100644
--- a/drivers/media/dvb-frontends/horus3a.c
+++ b/drivers/media/dvb-frontends/horus3a.c
@@ -326,7 +326,7 @@ static int horus3a_get_frequency(struct dvb_frontend *fe, u32 *frequency)
 	return 0;
 }
 
-static struct dvb_tuner_ops horus3a_tuner_ops = {
+static const struct dvb_tuner_ops horus3a_tuner_ops = {
 	.info = {
 		.name = "Sony Horus3a",
 		.frequency_min = 950000,
diff --git a/drivers/media/dvb-frontends/horus3a.h b/drivers/media/dvb-frontends/horus3a.h
index c1e2d1834b78..672a556df71a 100644
--- a/drivers/media/dvb-frontends/horus3a.h
+++ b/drivers/media/dvb-frontends/horus3a.h
@@ -22,7 +22,6 @@
 #ifndef __DVB_HORUS3A_H__
 #define __DVB_HORUS3A_H__
 
-#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 #include <linux/i2c.h>
 
diff --git a/drivers/media/dvb-frontends/ix2505v.c b/drivers/media/dvb-frontends/ix2505v.c
index 0e3387e00952..2826bbb36b73 100644
--- a/drivers/media/dvb-frontends/ix2505v.c
+++ b/drivers/media/dvb-frontends/ix2505v.c
@@ -258,7 +258,7 @@ static int ix2505v_get_frequency(struct dvb_frontend *fe, u32 *frequency)
 	return 0;
 }
 
-static struct dvb_tuner_ops ix2505v_tuner_ops = {
+static const struct dvb_tuner_ops ix2505v_tuner_ops = {
 	.info = {
 		.name = "Sharp IX2505V (B0017)",
 		.frequency_min = 950000,
diff --git a/drivers/media/dvb-frontends/ix2505v.h b/drivers/media/dvb-frontends/ix2505v.h
index af107a2dd357..5eab39744b23 100644
--- a/drivers/media/dvb-frontends/ix2505v.h
+++ b/drivers/media/dvb-frontends/ix2505v.h
@@ -20,7 +20,6 @@
 #ifndef DVB_IX2505V_H
 #define DVB_IX2505V_H
 
-#include <linux/kconfig.h>
 #include <linux/i2c.h>
 #include "dvb_frontend.h"
 
diff --git a/drivers/media/dvb-frontends/lg2160.h b/drivers/media/dvb-frontends/lg2160.h
index d20bd909de39..8c74ddc6b88a 100644
--- a/drivers/media/dvb-frontends/lg2160.h
+++ b/drivers/media/dvb-frontends/lg2160.h
@@ -22,7 +22,6 @@
 #ifndef _LG2160_H_
 #define _LG2160_H_
 
-#include <linux/kconfig.h>
 #include <linux/i2c.h>
 #include "dvb_frontend.h"
 
diff --git a/drivers/media/dvb-frontends/lgdt3305.h b/drivers/media/dvb-frontends/lgdt3305.h
index f91a1b49ce2f..e7dceb60e572 100644
--- a/drivers/media/dvb-frontends/lgdt3305.h
+++ b/drivers/media/dvb-frontends/lgdt3305.h
@@ -22,7 +22,6 @@
 #ifndef _LGDT3305_H_
 #define _LGDT3305_H_
 
-#include <linux/kconfig.h>
 #include <linux/i2c.h>
 #include "dvb_frontend.h"
 
diff --git a/drivers/media/dvb-frontends/lgdt3306a.c b/drivers/media/dvb-frontends/lgdt3306a.c
index 179c26e5eb4e..0ca4e810e9d8 100644
--- a/drivers/media/dvb-frontends/lgdt3306a.c
+++ b/drivers/media/dvb-frontends/lgdt3306a.c
@@ -731,7 +731,7 @@ static int lgdt3306a_set_if(struct lgdt3306a_state *state,
 
 	switch (if_freq_khz) {
 	default:
-		pr_warn("IF=%d KHz is not supportted, 3250 assumed\n",
+		pr_warn("IF=%d KHz is not supported, 3250 assumed\n",
 			if_freq_khz);
 		/* fallthrough */
 	case 3250: /* 3.25Mhz */
@@ -1737,24 +1737,16 @@ static int lgdt3306a_get_tune_settings(struct dvb_frontend *fe,
 static int lgdt3306a_search(struct dvb_frontend *fe)
 {
 	enum fe_status status = 0;
-	int i, ret;
+	int ret;
 
 	/* set frontend */
 	ret = lgdt3306a_set_parameters(fe);
 	if (ret)
 		goto error;
 
-	/* wait frontend lock */
-	for (i = 20; i > 0; i--) {
-		dbg_info(": loop=%d\n", i);
-		msleep(50);
-		ret = lgdt3306a_read_status(fe, &status);
-		if (ret)
-			goto error;
-
-		if (status & FE_HAS_LOCK)
-			break;
-	}
+	ret = lgdt3306a_read_status(fe, &status);
+	if (ret)
+		goto error;
 
 	/* check if we have a valid signal */
 	if (status & FE_HAS_LOCK)
diff --git a/drivers/media/dvb-frontends/lgs8gl5.h b/drivers/media/dvb-frontends/lgs8gl5.h
index a5b3faf121f0..f36a7fd0b102 100644
--- a/drivers/media/dvb-frontends/lgs8gl5.h
+++ b/drivers/media/dvb-frontends/lgs8gl5.h
@@ -23,7 +23,6 @@
 #ifndef LGS8GL5_H
 #define LGS8GL5_H
 
-#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 struct lgs8gl5_config {
diff --git a/drivers/media/dvb-frontends/lgs8gxx.h b/drivers/media/dvb-frontends/lgs8gxx.h
index 368c9928ef7f..7519c0210399 100644
--- a/drivers/media/dvb-frontends/lgs8gxx.h
+++ b/drivers/media/dvb-frontends/lgs8gxx.h
@@ -26,7 +26,6 @@
 #ifndef __LGS8GXX_H__
 #define __LGS8GXX_H__
 
-#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 #include <linux/i2c.h>
 
diff --git a/drivers/media/dvb-frontends/lnbh24.h b/drivers/media/dvb-frontends/lnbh24.h
index a088b8ec1e53..24431dfdce1f 100644
--- a/drivers/media/dvb-frontends/lnbh24.h
+++ b/drivers/media/dvb-frontends/lnbh24.h
@@ -23,8 +23,6 @@
 #ifndef _LNBH24_H
 #define _LNBH24_H
 
-#include <linux/kconfig.h>
-
 /* system register bits */
 #define LNBH24_OLF	0x01
 #define LNBH24_OTF	0x02
diff --git a/drivers/media/dvb-frontends/lnbh25.h b/drivers/media/dvb-frontends/lnbh25.h
index 1f329ef05acc..f13fd0308b3e 100644
--- a/drivers/media/dvb-frontends/lnbh25.h
+++ b/drivers/media/dvb-frontends/lnbh25.h
@@ -22,7 +22,6 @@
 #define LNBH25_H
 
 #include <linux/i2c.h>
-#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 /* 22 kHz tone enabled. Tone output controlled by DSQIN pin */
diff --git a/drivers/media/dvb-frontends/lnbp21.h b/drivers/media/dvb-frontends/lnbp21.h
index cd9101f6e579..4bb6439068ec 100644
--- a/drivers/media/dvb-frontends/lnbp21.h
+++ b/drivers/media/dvb-frontends/lnbp21.h
@@ -27,8 +27,6 @@
 #ifndef _LNBP21_H
 #define _LNBP21_H
 
-#include <linux/kconfig.h>
-
 /* system register bits */
 /* [RO] 0=OK; 1=over current limit flag */
 #define LNBP21_OLF	0x01
diff --git a/drivers/media/dvb-frontends/lnbp22.h b/drivers/media/dvb-frontends/lnbp22.h
index 5d01d92814c2..0cb72126c498 100644
--- a/drivers/media/dvb-frontends/lnbp22.h
+++ b/drivers/media/dvb-frontends/lnbp22.h
@@ -28,8 +28,6 @@
 #ifndef _LNBP22_H
 #define _LNBP22_H
 
-#include <linux/kconfig.h>
-
 /* Enable */
 #define LNBP22_EN	  0x10
 /* Voltage selection */
diff --git a/drivers/media/dvb-frontends/m88rs2000.h b/drivers/media/dvb-frontends/m88rs2000.h
index de7430178e9e..1a313b0f5875 100644
--- a/drivers/media/dvb-frontends/m88rs2000.h
+++ b/drivers/media/dvb-frontends/m88rs2000.h
@@ -20,7 +20,6 @@
 #ifndef M88RS2000_H
 #define M88RS2000_H
 
-#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 #include "dvb_frontend.h"
 
diff --git a/drivers/media/dvb-frontends/mb86a20s.c b/drivers/media/dvb-frontends/mb86a20s.c
index 41325328a22e..fe79358b035e 100644
--- a/drivers/media/dvb-frontends/mb86a20s.c
+++ b/drivers/media/dvb-frontends/mb86a20s.c
@@ -71,25 +71,27 @@ static struct regdata mb86a20s_init1[] = {
 };
 
 static struct regdata mb86a20s_init2[] = {
-	{ 0x28, 0x22 }, { 0x29, 0x00 }, { 0x2a, 0x1f }, { 0x2b, 0xf0 },
+	{ 0x50, 0xd1 }, { 0x51, 0x22 },
+	{ 0x39, 0x01 },
+	{ 0x71, 0x00 },
 	{ 0x3b, 0x21 },
-	{ 0x3c, 0x38 },
+	{ 0x3c, 0x3a },
 	{ 0x01, 0x0d },
-	{ 0x04, 0x08 }, { 0x05, 0x03 },
+	{ 0x04, 0x08 }, { 0x05, 0x05 },
 	{ 0x04, 0x0e }, { 0x05, 0x00 },
-	{ 0x04, 0x0f }, { 0x05, 0x37 },
-	{ 0x04, 0x0b }, { 0x05, 0x78 },
+	{ 0x04, 0x0f }, { 0x05, 0x14 },
+	{ 0x04, 0x0b }, { 0x05, 0x8c },
 	{ 0x04, 0x00 }, { 0x05, 0x00 },
-	{ 0x04, 0x01 }, { 0x05, 0x1e },
-	{ 0x04, 0x02 }, { 0x05, 0x07 },
-	{ 0x04, 0x03 }, { 0x05, 0xd0 },
+	{ 0x04, 0x01 }, { 0x05, 0x07 },
+	{ 0x04, 0x02 }, { 0x05, 0x0f },
+	{ 0x04, 0x03 }, { 0x05, 0xa0 },
 	{ 0x04, 0x09 }, { 0x05, 0x00 },
 	{ 0x04, 0x0a }, { 0x05, 0xff },
-	{ 0x04, 0x27 }, { 0x05, 0x00 },
+	{ 0x04, 0x27 }, { 0x05, 0x64 },
 	{ 0x04, 0x28 }, { 0x05, 0x00 },
-	{ 0x04, 0x1e }, { 0x05, 0x00 },
-	{ 0x04, 0x29 }, { 0x05, 0x64 },
-	{ 0x04, 0x32 }, { 0x05, 0x02 },
+	{ 0x04, 0x1e }, { 0x05, 0xff },
+	{ 0x04, 0x29 }, { 0x05, 0x0a },
+	{ 0x04, 0x32 }, { 0x05, 0x0a },
 	{ 0x04, 0x14 }, { 0x05, 0x02 },
 	{ 0x04, 0x04 }, { 0x05, 0x00 },
 	{ 0x04, 0x05 }, { 0x05, 0x22 },
@@ -97,8 +99,6 @@ static struct regdata mb86a20s_init2[] = {
 	{ 0x04, 0x07 }, { 0x05, 0xd8 },
 	{ 0x04, 0x12 }, { 0x05, 0x00 },
 	{ 0x04, 0x13 }, { 0x05, 0xff },
-	{ 0x04, 0x15 }, { 0x05, 0x4e },
-	{ 0x04, 0x16 }, { 0x05, 0x20 },
 
 	/*
 	 * On this demod, when the bit count reaches the count below,
@@ -152,42 +152,36 @@ static struct regdata mb86a20s_init2[] = {
 	{ 0x50, 0x51 }, { 0x51, 0x04 },		/* MER symbol 4 */
 	{ 0x45, 0x04 },				/* CN symbol 4 */
 	{ 0x48, 0x04 },				/* CN manual mode */
-
+	{ 0x50, 0xd5 }, { 0x51, 0x01 },
 	{ 0x50, 0xd6 }, { 0x51, 0x1f },
 	{ 0x50, 0xd2 }, { 0x51, 0x03 },
-	{ 0x50, 0xd7 }, { 0x51, 0xbf },
-	{ 0x28, 0x74 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0xff },
-	{ 0x28, 0x46 }, { 0x29, 0x00 }, { 0x2a, 0x1a }, { 0x2b, 0x0c },
-
-	{ 0x04, 0x40 }, { 0x05, 0x00 },
-	{ 0x28, 0x00 }, { 0x2b, 0x08 },
-	{ 0x28, 0x05 }, { 0x2b, 0x00 },
+	{ 0x50, 0xd7 }, { 0x51, 0x3f },
 	{ 0x1c, 0x01 },
-	{ 0x28, 0x06 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x1f },
-	{ 0x28, 0x07 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x18 },
-	{ 0x28, 0x08 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x12 },
-	{ 0x28, 0x09 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x30 },
-	{ 0x28, 0x0a }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x37 },
-	{ 0x28, 0x0b }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x02 },
-	{ 0x28, 0x0c }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x09 },
-	{ 0x28, 0x0d }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x06 },
-	{ 0x28, 0x0e }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x7b },
-	{ 0x28, 0x0f }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x76 },
-	{ 0x28, 0x10 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x7d },
-	{ 0x28, 0x11 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x08 },
-	{ 0x28, 0x12 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x0b },
-	{ 0x28, 0x13 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x00 },
-	{ 0x28, 0x14 }, { 0x29, 0x00 }, { 0x2a, 0x01 }, { 0x2b, 0xf2 },
-	{ 0x28, 0x15 }, { 0x29, 0x00 }, { 0x2a, 0x01 }, { 0x2b, 0xf3 },
-	{ 0x28, 0x16 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x05 },
-	{ 0x28, 0x17 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x16 },
-	{ 0x28, 0x18 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x0f },
-	{ 0x28, 0x19 }, { 0x29, 0x00 }, { 0x2a, 0x07 }, { 0x2b, 0xef },
-	{ 0x28, 0x1a }, { 0x29, 0x00 }, { 0x2a, 0x07 }, { 0x2b, 0xd8 },
-	{ 0x28, 0x1b }, { 0x29, 0x00 }, { 0x2a, 0x07 }, { 0x2b, 0xf1 },
-	{ 0x28, 0x1c }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x3d },
-	{ 0x28, 0x1d }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x94 },
-	{ 0x28, 0x1e }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0xba },
+	{ 0x28, 0x06 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x03 },
+	{ 0x28, 0x07 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x0d },
+	{ 0x28, 0x08 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x02 },
+	{ 0x28, 0x09 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x01 },
+	{ 0x28, 0x0a }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x21 },
+	{ 0x28, 0x0b }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x29 },
+	{ 0x28, 0x0c }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x16 },
+	{ 0x28, 0x0d }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x31 },
+	{ 0x28, 0x0e }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x0e },
+	{ 0x28, 0x0f }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x4e },
+	{ 0x28, 0x10 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x46 },
+	{ 0x28, 0x11 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x0f },
+	{ 0x28, 0x12 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x56 },
+	{ 0x28, 0x13 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x35 },
+	{ 0x28, 0x14 }, { 0x29, 0x00 }, { 0x2a, 0x01 }, { 0x2b, 0xbe },
+	{ 0x28, 0x15 }, { 0x29, 0x00 }, { 0x2a, 0x01 }, { 0x2b, 0x84 },
+	{ 0x28, 0x16 }, { 0x29, 0x00 }, { 0x2a, 0x03 }, { 0x2b, 0xee },
+	{ 0x28, 0x17 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x98 },
+	{ 0x28, 0x18 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x9f },
+	{ 0x28, 0x19 }, { 0x29, 0x00 }, { 0x2a, 0x07 }, { 0x2b, 0xb2 },
+	{ 0x28, 0x1a }, { 0x29, 0x00 }, { 0x2a, 0x06 }, { 0x2b, 0xc2 },
+	{ 0x28, 0x1b }, { 0x29, 0x00 }, { 0x2a, 0x07 }, { 0x2b, 0x4a },
+	{ 0x28, 0x1c }, { 0x29, 0x00 }, { 0x2a, 0x01 }, { 0x2b, 0xbc },
+	{ 0x28, 0x1d }, { 0x29, 0x00 }, { 0x2a, 0x04 }, { 0x2b, 0xba },
+	{ 0x28, 0x1e }, { 0x29, 0x00 }, { 0x2a, 0x06 }, { 0x2b, 0x14 },
 	{ 0x50, 0x1e }, { 0x51, 0x5d },
 	{ 0x50, 0x22 }, { 0x51, 0x00 },
 	{ 0x50, 0x23 }, { 0x51, 0xc8 },
@@ -196,9 +190,7 @@ static struct regdata mb86a20s_init2[] = {
 	{ 0x50, 0x26 }, { 0x51, 0x00 },
 	{ 0x50, 0x27 }, { 0x51, 0xc3 },
 	{ 0x50, 0x39 }, { 0x51, 0x02 },
-	{ 0xec, 0x0f },
-	{ 0xeb, 0x1f },
-	{ 0x28, 0x6a }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x00 },
+	{ 0x50, 0xd5 }, { 0x51, 0x01 },
 	{ 0xd0, 0x00 },
 };
 
@@ -318,7 +310,11 @@ static int mb86a20s_read_status(struct dvb_frontend *fe, enum fe_status *status)
 	if (val >= 7)
 		*status |= FE_HAS_SYNC;
 
-	if (val >= 8)				/* Maybe 9? */
+	/*
+	 * Actually, on state S8, it starts receiving TS, but the TS
+	 * output is only on normal state after the transition to S9.
+	 */
+	if (val >= 9)
 		*status |= FE_HAS_LOCK;
 
 	dev_dbg(&state->i2c->dev, "%s: Status = 0x%02x (state = %d)\n",
@@ -2058,6 +2054,11 @@ static void mb86a20s_release(struct dvb_frontend *fe)
 	kfree(state);
 }
 
+static int mb86a20s_get_frontend_algo(struct dvb_frontend *fe)
+{
+        return DVBFE_ALGO_HW;
+}
+
 static struct dvb_frontend_ops mb86a20s_ops;
 
 struct dvb_frontend *mb86a20s_attach(const struct mb86a20s_config *config,
@@ -2130,6 +2131,7 @@ static struct dvb_frontend_ops mb86a20s_ops = {
 	.read_status = mb86a20s_read_status_and_stats,
 	.read_signal_strength = mb86a20s_read_signal_strength_from_cache,
 	.tune = mb86a20s_tune,
+	.get_frontend_algo = mb86a20s_get_frontend_algo,
 };
 
 MODULE_DESCRIPTION("DVB Frontend module for Fujitsu mb86A20s hardware");
diff --git a/drivers/media/dvb-frontends/mb86a20s.h b/drivers/media/dvb-frontends/mb86a20s.h
index a113282d6956..dfb02db2126c 100644
--- a/drivers/media/dvb-frontends/mb86a20s.h
+++ b/drivers/media/dvb-frontends/mb86a20s.h
@@ -16,7 +16,6 @@
 #ifndef MB86A20S_H
 #define MB86A20S_H
 
-#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 /**
diff --git a/drivers/media/dvb-frontends/rtl2832_sdr.c b/drivers/media/dvb-frontends/rtl2832_sdr.c
index 6e22af36b637..e038e886731b 100644
--- a/drivers/media/dvb-frontends/rtl2832_sdr.c
+++ b/drivers/media/dvb-frontends/rtl2832_sdr.c
@@ -392,7 +392,6 @@ static int rtl2832_sdr_alloc_urbs(struct rtl2832_sdr_dev *dev)
 		dev_dbg(&pdev->dev, "alloc urb=%d\n", i);
 		dev->urb_list[i] = usb_alloc_urb(0, GFP_ATOMIC);
 		if (!dev->urb_list[i]) {
-			dev_dbg(&pdev->dev, "failed\n");
 			for (j = 0; j < i; j++)
 				usb_free_urb(dev->urb_list[j]);
 			return -ENOMEM;
diff --git a/drivers/media/dvb-frontends/s5h1409.h b/drivers/media/dvb-frontends/s5h1409.h
index f58b9ca5557a..b38557c451b9 100644
--- a/drivers/media/dvb-frontends/s5h1409.h
+++ b/drivers/media/dvb-frontends/s5h1409.h
@@ -22,7 +22,6 @@
 #ifndef __S5H1409_H__
 #define __S5H1409_H__
 
-#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 struct s5h1409_config {
diff --git a/drivers/media/dvb-frontends/s5h1411.h b/drivers/media/dvb-frontends/s5h1411.h
index f3a87f7ec360..791bab0e16e9 100644
--- a/drivers/media/dvb-frontends/s5h1411.h
+++ b/drivers/media/dvb-frontends/s5h1411.h
@@ -22,7 +22,6 @@
 #ifndef __S5H1411_H__
 #define __S5H1411_H__
 
-#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 #define S5H1411_I2C_TOP_ADDR (0x32 >> 1)
diff --git a/drivers/media/dvb-frontends/s5h1432.h b/drivers/media/dvb-frontends/s5h1432.h
index f490c5ee5801..b81c9bd4e422 100644
--- a/drivers/media/dvb-frontends/s5h1432.h
+++ b/drivers/media/dvb-frontends/s5h1432.h
@@ -22,7 +22,6 @@
 #ifndef __S5H1432_H__
 #define __S5H1432_H__
 
-#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 #define S5H1432_I2C_TOP_ADDR (0x02 >> 1)
diff --git a/drivers/media/dvb-frontends/s921.h b/drivers/media/dvb-frontends/s921.h
index f5b722d8081b..a47ed894d4ae 100644
--- a/drivers/media/dvb-frontends/s921.h
+++ b/drivers/media/dvb-frontends/s921.h
@@ -17,7 +17,6 @@
 #ifndef S921_H
 #define S921_H
 
-#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 struct s921_config {
diff --git a/drivers/media/dvb-frontends/si2165.c b/drivers/media/dvb-frontends/si2165.c
index 8bf716a8ea58..78669ea68c61 100644
--- a/drivers/media/dvb-frontends/si2165.c
+++ b/drivers/media/dvb-frontends/si2165.c
@@ -25,6 +25,7 @@
 #include <linux/string.h>
 #include <linux/slab.h>
 #include <linux/firmware.h>
+#include <linux/regmap.h>
 
 #include "dvb_frontend.h"
 #include "dvb_math.h"
@@ -40,7 +41,9 @@
  */
 
 struct si2165_state {
-	struct i2c_adapter *i2c;
+	struct i2c_client *client;
+
+	struct regmap *regmap;
 
 	struct dvb_frontend fe;
 
@@ -108,61 +111,27 @@ static int si2165_write(struct si2165_state *state, const u16 reg,
 		       const u8 *src, const int count)
 {
 	int ret;
-	struct i2c_msg msg;
-	u8 buf[2 + 4]; /* write a maximum of 4 bytes of data */
-
-	if (count + 2 > sizeof(buf)) {
-		dev_warn(&state->i2c->dev,
-			  "%s: i2c wr reg=%04x: count=%d is too big!\n",
-			  KBUILD_MODNAME, reg, count);
-		return -EINVAL;
-	}
-	buf[0] = reg >> 8;
-	buf[1] = reg & 0xff;
-	memcpy(buf + 2, src, count);
-
-	msg.addr = state->config.i2c_addr;
-	msg.flags = 0;
-	msg.buf = buf;
-	msg.len = count + 2;
 
 	if (debug & DEBUG_I2C_WRITE)
 		deb_i2c_write("reg: 0x%04x, data: %*ph\n", reg, count, src);
 
-	ret = i2c_transfer(state->i2c, &msg, 1);
+	ret = regmap_bulk_write(state->regmap, reg, src, count);
 
-	if (ret != 1) {
-		dev_err(&state->i2c->dev, "%s: ret == %d\n", __func__, ret);
-		if (ret < 0)
-			return ret;
-		else
-			return -EREMOTEIO;
-	}
+	if (ret)
+		dev_err(&state->client->dev, "%s: ret == %d\n", __func__, ret);
 
-	return 0;
+	return ret;
 }
 
 static int si2165_read(struct si2165_state *state,
 		       const u16 reg, u8 *val, const int count)
 {
-	int ret;
-	u8 reg_buf[] = { reg >> 8, reg & 0xff };
-	struct i2c_msg msg[] = {
-		{ .addr = state->config.i2c_addr,
-		  .flags = 0, .buf = reg_buf, .len = 2 },
-		{ .addr = state->config.i2c_addr,
-		  .flags = I2C_M_RD, .buf = val, .len = count },
-	};
-
-	ret = i2c_transfer(state->i2c, msg, 2);
+	int ret = regmap_bulk_read(state->regmap, reg, val, count);
 
-	if (ret != 2) {
-		dev_err(&state->i2c->dev, "%s: error (addr %02x reg %04x error (ret == %i)\n",
+	if (ret) {
+		dev_err(&state->client->dev, "%s: error (addr %02x reg %04x error (ret == %i)\n",
 			__func__, state->config.i2c_addr, reg, ret);
-		if (ret < 0)
-			return ret;
-		else
-			return -EREMOTEIO;
+		return ret;
 	}
 
 	if (debug & DEBUG_I2C_READ)
@@ -174,9 +143,9 @@ static int si2165_read(struct si2165_state *state,
 static int si2165_readreg8(struct si2165_state *state,
 		       const u16 reg, u8 *val)
 {
-	int ret;
-
-	ret = si2165_read(state, reg, val, 1);
+	unsigned int val_tmp;
+	int ret = regmap_read(state->regmap, reg, &val_tmp);
+	*val = (u8)val_tmp;
 	deb_readreg("R(0x%04x)=0x%02x\n", reg, *val);
 	return ret;
 }
@@ -194,7 +163,7 @@ static int si2165_readreg16(struct si2165_state *state,
 
 static int si2165_writereg8(struct si2165_state *state, const u16 reg, u8 val)
 {
-	return si2165_write(state, reg, &val, 1);
+	return regmap_write(state->regmap, reg, val);
 }
 
 static int si2165_writereg16(struct si2165_state *state, const u16 reg, u16 val)
@@ -345,7 +314,7 @@ static int si2165_wait_init_done(struct si2165_state *state)
 			return 0;
 		usleep_range(1000, 50000);
 	}
-	dev_err(&state->i2c->dev, "%s: init_done was not set\n",
+	dev_err(&state->client->dev, "%s: init_done was not set\n",
 		KBUILD_MODNAME);
 	return ret;
 }
@@ -374,14 +343,14 @@ static int si2165_upload_firmware_block(struct si2165_state *state,
 		wordcount = data[offset];
 		if (wordcount < 1 || data[offset+1] ||
 		    data[offset+2] || data[offset+3]) {
-			dev_warn(&state->i2c->dev,
+			dev_warn(&state->client->dev,
 				 "%s: bad fw data[0..3] = %*ph\n",
 				KBUILD_MODNAME, 4, data);
 			return -EINVAL;
 		}
 
 		if (offset + 8 + wordcount * 4 > len) {
-			dev_warn(&state->i2c->dev,
+			dev_warn(&state->client->dev,
 				 "%s: len is too small for block len=%d, wordcount=%d\n",
 				KBUILD_MODNAME, len, wordcount);
 			return -EINVAL;
@@ -444,15 +413,15 @@ static int si2165_upload_firmware(struct si2165_state *state)
 		fw_file = SI2165_FIRMWARE_REV_D;
 		break;
 	default:
-		dev_info(&state->i2c->dev, "%s: no firmware file for revision=%d\n",
+		dev_info(&state->client->dev, "%s: no firmware file for revision=%d\n",
 			KBUILD_MODNAME, state->chip_revcode);
 		return 0;
 	}
 
 	/* request the firmware, this will block and timeout */
-	ret = request_firmware(&fw, fw_file, state->i2c->dev.parent);
+	ret = request_firmware(&fw, fw_file, &state->client->dev);
 	if (ret) {
-		dev_warn(&state->i2c->dev, "%s: firmware file '%s' not found\n",
+		dev_warn(&state->client->dev, "%s: firmware file '%s' not found\n",
 				KBUILD_MODNAME, fw_file);
 		goto error;
 	}
@@ -460,11 +429,11 @@ static int si2165_upload_firmware(struct si2165_state *state)
 	data = fw->data;
 	len = fw->size;
 
-	dev_info(&state->i2c->dev, "%s: downloading firmware from file '%s' size=%d\n",
+	dev_info(&state->client->dev, "%s: downloading firmware from file '%s' size=%d\n",
 			KBUILD_MODNAME, fw_file, len);
 
 	if (len % 4 != 0) {
-		dev_warn(&state->i2c->dev, "%s: firmware size is not multiple of 4\n",
+		dev_warn(&state->client->dev, "%s: firmware size is not multiple of 4\n",
 				KBUILD_MODNAME);
 		ret = -EINVAL;
 		goto error;
@@ -472,14 +441,14 @@ static int si2165_upload_firmware(struct si2165_state *state)
 
 	/* check header (8 bytes) */
 	if (len < 8) {
-		dev_warn(&state->i2c->dev, "%s: firmware header is missing\n",
+		dev_warn(&state->client->dev, "%s: firmware header is missing\n",
 				KBUILD_MODNAME);
 		ret = -EINVAL;
 		goto error;
 	}
 
 	if (data[0] != 1 || data[1] != 0) {
-		dev_warn(&state->i2c->dev, "%s: firmware file version is wrong\n",
+		dev_warn(&state->client->dev, "%s: firmware file version is wrong\n",
 				KBUILD_MODNAME);
 		ret = -EINVAL;
 		goto error;
@@ -517,7 +486,7 @@ static int si2165_upload_firmware(struct si2165_state *state)
 	/* start right after the header */
 	offset = 8;
 
-	dev_info(&state->i2c->dev, "%s: si2165_upload_firmware extracted patch_version=0x%02x, block_count=0x%02x, crc_expected=0x%04x\n",
+	dev_info(&state->client->dev, "%s: si2165_upload_firmware extracted patch_version=0x%02x, block_count=0x%02x, crc_expected=0x%04x\n",
 		KBUILD_MODNAME, patch_version, block_count, crc_expected);
 
 	ret = si2165_upload_firmware_block(state, data, len, &offset, 1);
@@ -536,7 +505,7 @@ static int si2165_upload_firmware(struct si2165_state *state)
 	ret = si2165_upload_firmware_block(state, data, len,
 					   &offset, block_count);
 	if (ret < 0) {
-		dev_err(&state->i2c->dev,
+		dev_err(&state->client->dev,
 			"%s: firmware could not be uploaded\n",
 			KBUILD_MODNAME);
 		goto error;
@@ -548,7 +517,7 @@ static int si2165_upload_firmware(struct si2165_state *state)
 		goto error;
 
 	if (val16 != crc_expected) {
-		dev_err(&state->i2c->dev,
+		dev_err(&state->client->dev,
 			"%s: firmware crc mismatch %04x != %04x\n",
 			KBUILD_MODNAME, val16, crc_expected);
 		ret = -EINVAL;
@@ -560,7 +529,7 @@ static int si2165_upload_firmware(struct si2165_state *state)
 		goto error;
 
 	if (len != offset) {
-		dev_err(&state->i2c->dev,
+		dev_err(&state->client->dev,
 			"%s: firmware len mismatch %04x != %04x\n",
 			KBUILD_MODNAME, len, offset);
 		ret = -EINVAL;
@@ -577,7 +546,7 @@ static int si2165_upload_firmware(struct si2165_state *state)
 	if (ret < 0)
 		goto error;
 
-	dev_info(&state->i2c->dev, "%s: fw load finished\n", KBUILD_MODNAME);
+	dev_info(&state->client->dev, "%s: fw load finished\n", KBUILD_MODNAME);
 
 	ret = 0;
 	state->firmware_loaded = true;
@@ -611,7 +580,7 @@ static int si2165_init(struct dvb_frontend *fe)
 	if (ret < 0)
 		goto error;
 	if (val != state->config.chip_mode) {
-		dev_err(&state->i2c->dev, "%s: could not set chip_mode\n",
+		dev_err(&state->client->dev, "%s: could not set chip_mode\n",
 			KBUILD_MODNAME);
 		return -EINVAL;
 	}
@@ -751,6 +720,9 @@ static int si2165_set_oversamp(struct si2165_state *state, u32 dvb_rate)
 	u64 oversamp;
 	u32 reg_value;
 
+	if (!dvb_rate)
+		return -EINVAL;
+
 	oversamp = si2165_get_fe_clk(state);
 	oversamp <<= 23;
 	do_div(oversamp, dvb_rate);
@@ -769,12 +741,15 @@ static int si2165_set_if_freq_shift(struct si2165_state *state)
 	u32 IF = 0;
 
 	if (!fe->ops.tuner_ops.get_if_frequency) {
-		dev_err(&state->i2c->dev,
+		dev_err(&state->client->dev,
 			"%s: Error: get_if_frequency() not defined at tuner. Can't work without it!\n",
 			KBUILD_MODNAME);
 		return -EINVAL;
 	}
 
+	if (!fe_clk)
+		return -EINVAL;
+
 	fe->ops.tuner_ops.get_if_frequency(fe, &IF);
 	if_freq_shift = IF;
 	if_freq_shift <<= 29;
@@ -1003,14 +978,6 @@ static int si2165_set_frontend(struct dvb_frontend *fe)
 	return 0;
 }
 
-static void si2165_release(struct dvb_frontend *fe)
-{
-	struct si2165_state *state = fe->demodulator_priv;
-
-	dprintk("%s: called\n", __func__);
-	kfree(state);
-}
-
 static struct dvb_frontend_ops si2165_ops = {
 	.info = {
 		.name = "Silicon Labs ",
@@ -1046,67 +1013,83 @@ static struct dvb_frontend_ops si2165_ops = {
 
 	.set_frontend      = si2165_set_frontend,
 	.read_status       = si2165_read_status,
-
-	.release = si2165_release,
 };
 
-struct dvb_frontend *si2165_attach(const struct si2165_config *config,
-				   struct i2c_adapter *i2c)
+static int si2165_probe(struct i2c_client *client,
+		const struct i2c_device_id *id)
 {
 	struct si2165_state *state = NULL;
+	struct si2165_platform_data *pdata = client->dev.platform_data;
 	int n;
-	int io_ret;
+	int ret = 0;
 	u8 val;
 	char rev_char;
 	const char *chip_name;
-
-	if (config == NULL || i2c == NULL)
-		goto error;
+	static const struct regmap_config regmap_config = {
+		.reg_bits = 16,
+		.val_bits = 8,
+		.max_register = 0x08ff,
+	};
 
 	/* allocate memory for the internal state */
 	state = kzalloc(sizeof(struct si2165_state), GFP_KERNEL);
-	if (state == NULL)
+	if (state == NULL) {
+		ret = -ENOMEM;
 		goto error;
+	}
+
+	/* create regmap */
+	state->regmap = devm_regmap_init_i2c(client, &regmap_config);
+	if (IS_ERR(state->regmap)) {
+		ret = PTR_ERR(state->regmap);
+		goto error;
+	}
 
 	/* setup the state */
-	state->i2c = i2c;
-	state->config = *config;
+	state->client = client;
+	state->config.i2c_addr = client->addr;
+	state->config.chip_mode = pdata->chip_mode;
+	state->config.ref_freq_Hz = pdata->ref_freq_Hz;
+	state->config.inversion = pdata->inversion;
 
 	if (state->config.ref_freq_Hz < 4000000
 	    || state->config.ref_freq_Hz > 27000000) {
-		dev_err(&state->i2c->dev, "%s: ref_freq of %d Hz not supported by this driver\n",
+		dev_err(&state->client->dev, "%s: ref_freq of %d Hz not supported by this driver\n",
 			 KBUILD_MODNAME, state->config.ref_freq_Hz);
+		ret = -EINVAL;
 		goto error;
 	}
 
 	/* create dvb_frontend */
 	memcpy(&state->fe.ops, &si2165_ops,
 		sizeof(struct dvb_frontend_ops));
+	state->fe.ops.release = NULL;
 	state->fe.demodulator_priv = state;
+	i2c_set_clientdata(client, state);
 
 	/* powerup */
-	io_ret = si2165_writereg8(state, 0x0000, state->config.chip_mode);
-	if (io_ret < 0)
-		goto error;
+	ret = si2165_writereg8(state, 0x0000, state->config.chip_mode);
+	if (ret < 0)
+		goto nodev_error;
 
-	io_ret = si2165_readreg8(state, 0x0000, &val);
-	if (io_ret < 0)
-		goto error;
+	ret = si2165_readreg8(state, 0x0000, &val);
+	if (ret < 0)
+		goto nodev_error;
 	if (val != state->config.chip_mode)
-		goto error;
+		goto nodev_error;
 
-	io_ret = si2165_readreg8(state, 0x0023, &state->chip_revcode);
-	if (io_ret < 0)
-		goto error;
+	ret = si2165_readreg8(state, 0x0023, &state->chip_revcode);
+	if (ret < 0)
+		goto nodev_error;
 
-	io_ret = si2165_readreg8(state, 0x0118, &state->chip_type);
-	if (io_ret < 0)
-		goto error;
+	ret = si2165_readreg8(state, 0x0118, &state->chip_type);
+	if (ret < 0)
+		goto nodev_error;
 
 	/* powerdown */
-	io_ret = si2165_writereg8(state, 0x0000, SI2165_MODE_OFF);
-	if (io_ret < 0)
-		goto error;
+	ret = si2165_writereg8(state, 0x0000, SI2165_MODE_OFF);
+	if (ret < 0)
+		goto nodev_error;
 
 	if (state->chip_revcode < 26)
 		rev_char = 'A' + state->chip_revcode;
@@ -1124,12 +1107,12 @@ struct dvb_frontend *si2165_attach(const struct si2165_config *config,
 		state->has_dvbc = true;
 		break;
 	default:
-		dev_err(&state->i2c->dev, "%s: Unsupported Silicon Labs chip (type %d, rev %d)\n",
+		dev_err(&state->client->dev, "%s: Unsupported Silicon Labs chip (type %d, rev %d)\n",
 			KBUILD_MODNAME, state->chip_type, state->chip_revcode);
-		goto error;
+		goto nodev_error;
 	}
 
-	dev_info(&state->i2c->dev,
+	dev_info(&state->client->dev,
 		"%s: Detected Silicon Labs %s-%c (type %d, rev %d)\n",
 		KBUILD_MODNAME, chip_name, rev_char, state->chip_type,
 		state->chip_revcode);
@@ -1149,13 +1132,46 @@ struct dvb_frontend *si2165_attach(const struct si2165_config *config,
 			sizeof(state->fe.ops.info.name));
 	}
 
-	return &state->fe;
+	/* return fe pointer */
+	*pdata->fe = &state->fe;
+
+	return 0;
 
+nodev_error:
+	ret = -ENODEV;
 error:
 	kfree(state);
-	return NULL;
+	dev_dbg(&client->dev, "failed=%d\n", ret);
+	return ret;
 }
-EXPORT_SYMBOL(si2165_attach);
+
+static int si2165_remove(struct i2c_client *client)
+{
+	struct si2165_state *state = i2c_get_clientdata(client);
+
+	dev_dbg(&client->dev, "\n");
+
+	kfree(state);
+	return 0;
+}
+
+static const struct i2c_device_id si2165_id_table[] = {
+	{"si2165", 0},
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, si2165_id_table);
+
+static struct i2c_driver si2165_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.name	= "si2165",
+	},
+	.probe		= si2165_probe,
+	.remove		= si2165_remove,
+	.id_table	= si2165_id_table,
+};
+
+module_i2c_driver(si2165_driver);
 
 module_param(debug, int, 0644);
 MODULE_PARM_DESC(debug, "Turn on/off frontend debugging (default:off).");
diff --git a/drivers/media/dvb-frontends/si2165.h b/drivers/media/dvb-frontends/si2165.h
index 8a15d6a9c552..76c2ca7d7edb 100644
--- a/drivers/media/dvb-frontends/si2165.h
+++ b/drivers/media/dvb-frontends/si2165.h
@@ -28,10 +28,15 @@ enum {
 	SI2165_MODE_PLL_XTAL = 0x21
 };
 
-struct si2165_config {
-	/* i2c addr
-	 * possible values: 0x64,0x65,0x66,0x67 */
-	u8 i2c_addr;
+/* I2C addresses
+ * possible values: 0x64,0x65,0x66,0x67
+ */
+struct si2165_platform_data {
+	/*
+	 * frontend
+	 * returned by driver
+	 */
+	struct dvb_frontend **fe;
 
 	/* external clock or XTAL */
 	u8 chip_mode;
@@ -45,18 +50,4 @@ struct si2165_config {
 	bool inversion;
 };
 
-#if IS_REACHABLE(CONFIG_DVB_SI2165)
-struct dvb_frontend *si2165_attach(
-	const struct si2165_config *config,
-	struct i2c_adapter *i2c);
-#else
-static inline struct dvb_frontend *si2165_attach(
-	const struct si2165_config *config,
-	struct i2c_adapter *i2c)
-{
-	pr_warn("%s: driver disabled by Kconfig\n", __func__);
-	return NULL;
-}
-#endif /* CONFIG_DVB_SI2165 */
-
 #endif /* _DVB_SI2165_H */
diff --git a/drivers/media/dvb-frontends/si2165_priv.h b/drivers/media/dvb-frontends/si2165_priv.h
index 2b70cf12cd79..e5932118834b 100644
--- a/drivers/media/dvb-frontends/si2165_priv.h
+++ b/drivers/media/dvb-frontends/si2165_priv.h
@@ -20,4 +20,21 @@
 
 #define SI2165_FIRMWARE_REV_D "dvb-demod-si2165.fw"
 
+struct si2165_config {
+	/* i2c addr
+	 * possible values: 0x64,0x65,0x66,0x67 */
+	u8 i2c_addr;
+
+	/* external clock or XTAL */
+	u8 chip_mode;
+
+	/* frequency of external clock or xtal in Hz
+	 * possible values: 4000000, 16000000, 20000000, 240000000, 27000000
+	 */
+	u32 ref_freq_Hz;
+
+	/* invert the spectrum */
+	bool inversion;
+};
+
 #endif /* _DVB_SI2165_PRIV */
diff --git a/drivers/media/dvb-frontends/si21xx.h b/drivers/media/dvb-frontends/si21xx.h
index ef5f351ca68e..b1be62f1983a 100644
--- a/drivers/media/dvb-frontends/si21xx.h
+++ b/drivers/media/dvb-frontends/si21xx.h
@@ -1,7 +1,6 @@
 #ifndef SI21XX_H
 #define SI21XX_H
 
-#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 #include "dvb_frontend.h"
 
diff --git a/drivers/media/dvb-frontends/sp2.h b/drivers/media/dvb-frontends/sp2.h
index 6cceea022d49..3901cd74b3f7 100644
--- a/drivers/media/dvb-frontends/sp2.h
+++ b/drivers/media/dvb-frontends/sp2.h
@@ -17,7 +17,6 @@
 #ifndef SP2_H
 #define SP2_H
 
-#include <linux/kconfig.h>
 #include "dvb_ca_en50221.h"
 
 /*
diff --git a/drivers/media/dvb-frontends/stb6000.c b/drivers/media/dvb-frontends/stb6000.c
index a0c3c526b132..73347d51f340 100644
--- a/drivers/media/dvb-frontends/stb6000.c
+++ b/drivers/media/dvb-frontends/stb6000.c
@@ -186,7 +186,7 @@ static int stb6000_get_frequency(struct dvb_frontend *fe, u32 *frequency)
 	return 0;
 }
 
-static struct dvb_tuner_ops stb6000_tuner_ops = {
+static const struct dvb_tuner_ops stb6000_tuner_ops = {
 	.info = {
 		.name = "ST STB6000",
 		.frequency_min = 950000,
diff --git a/drivers/media/dvb-frontends/stb6000.h b/drivers/media/dvb-frontends/stb6000.h
index da581b652cb9..78e75dfc317f 100644
--- a/drivers/media/dvb-frontends/stb6000.h
+++ b/drivers/media/dvb-frontends/stb6000.h
@@ -23,7 +23,6 @@
 #ifndef __DVB_STB6000_H__
 #define __DVB_STB6000_H__
 
-#include <linux/kconfig.h>
 #include <linux/i2c.h>
 #include "dvb_frontend.h"
 
diff --git a/drivers/media/dvb-frontends/stb6100.c b/drivers/media/dvb-frontends/stb6100.c
index b9c2511bf019..5add1182c3ca 100644
--- a/drivers/media/dvb-frontends/stb6100.c
+++ b/drivers/media/dvb-frontends/stb6100.c
@@ -522,7 +522,7 @@ static int stb6100_set_params(struct dvb_frontend *fe)
 	return 0;
 }
 
-static struct dvb_tuner_ops stb6100_ops = {
+static const struct dvb_tuner_ops stb6100_ops = {
 	.info = {
 		.name			= "STB6100 Silicon Tuner",
 		.frequency_min		= 950000,
diff --git a/drivers/media/dvb-frontends/stv0288.h b/drivers/media/dvb-frontends/stv0288.h
index b58603c00c80..803acb917282 100644
--- a/drivers/media/dvb-frontends/stv0288.h
+++ b/drivers/media/dvb-frontends/stv0288.h
@@ -27,7 +27,6 @@
 #ifndef STV0288_H
 #define STV0288_H
 
-#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 #include "dvb_frontend.h"
 
diff --git a/drivers/media/dvb-frontends/stv0367.h b/drivers/media/dvb-frontends/stv0367.h
index 92b3e85fb818..b88166a9716f 100644
--- a/drivers/media/dvb-frontends/stv0367.h
+++ b/drivers/media/dvb-frontends/stv0367.h
@@ -26,7 +26,6 @@
 #ifndef STV0367_H
 #define STV0367_H
 
-#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 #include "dvb_frontend.h"
 
diff --git a/drivers/media/dvb-frontends/stv0900.h b/drivers/media/dvb-frontends/stv0900.h
index c90bf00ea9ce..9ca2da90c7d7 100644
--- a/drivers/media/dvb-frontends/stv0900.h
+++ b/drivers/media/dvb-frontends/stv0900.h
@@ -26,7 +26,6 @@
 #ifndef STV0900_H
 #define STV0900_H
 
-#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 #include "dvb_frontend.h"
 
diff --git a/drivers/media/dvb-frontends/stv6110.c b/drivers/media/dvb-frontends/stv6110.c
index 91c6dcf65d2a..66a5a7f2295c 100644
--- a/drivers/media/dvb-frontends/stv6110.c
+++ b/drivers/media/dvb-frontends/stv6110.c
@@ -382,7 +382,7 @@ static int stv6110_get_bandwidth(struct dvb_frontend *fe, u32 *bandwidth)
 	return 0;
 }
 
-static struct dvb_tuner_ops stv6110_tuner_ops = {
+static const struct dvb_tuner_ops stv6110_tuner_ops = {
 	.info = {
 		.name = "ST STV6110",
 		.frequency_min = 950000,
diff --git a/drivers/media/dvb-frontends/stv6110.h b/drivers/media/dvb-frontends/stv6110.h
index f3c8a5c6b77d..4604f793d954 100644
--- a/drivers/media/dvb-frontends/stv6110.h
+++ b/drivers/media/dvb-frontends/stv6110.h
@@ -25,7 +25,6 @@
 #ifndef __DVB_STV6110_H__
 #define __DVB_STV6110_H__
 
-#include <linux/kconfig.h>
 #include <linux/i2c.h>
 #include "dvb_frontend.h"
 
diff --git a/drivers/media/dvb-frontends/stv6110x.c b/drivers/media/dvb-frontends/stv6110x.c
index a62c01e454f5..c611ad210b5c 100644
--- a/drivers/media/dvb-frontends/stv6110x.c
+++ b/drivers/media/dvb-frontends/stv6110x.c
@@ -345,7 +345,7 @@ static int stv6110x_release(struct dvb_frontend *fe)
 	return 0;
 }
 
-static struct dvb_tuner_ops stv6110x_ops = {
+static const struct dvb_tuner_ops stv6110x_ops = {
 	.info = {
 		.name		= "STV6110(A) Silicon Tuner",
 		.frequency_min	=  950000,
diff --git a/drivers/media/dvb-frontends/tda10048.h b/drivers/media/dvb-frontends/tda10048.h
index bc77a7311de1..a2cebb0cceba 100644
--- a/drivers/media/dvb-frontends/tda10048.h
+++ b/drivers/media/dvb-frontends/tda10048.h
@@ -22,7 +22,6 @@
 #ifndef TDA10048_H
 #define TDA10048_H
 
-#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 #include <linux/firmware.h>
 
diff --git a/drivers/media/dvb-frontends/tda18271c2dd.c b/drivers/media/dvb-frontends/tda18271c2dd.c
index de0a1c110972..bc247f9b553a 100644
--- a/drivers/media/dvb-frontends/tda18271c2dd.c
+++ b/drivers/media/dvb-frontends/tda18271c2dd.c
@@ -1217,7 +1217,7 @@ static int get_bandwidth(struct dvb_frontend *fe, u32 *bandwidth)
 }
 
 
-static struct dvb_tuner_ops tuner_ops = {
+static const struct dvb_tuner_ops tuner_ops = {
 	.info = {
 		.name = "NXP TDA18271C2D",
 		.frequency_min  =  47125000,
diff --git a/drivers/media/dvb-frontends/tda18271c2dd.h b/drivers/media/dvb-frontends/tda18271c2dd.h
index 7ebd8eaff4eb..e6ccf240f54c 100644
--- a/drivers/media/dvb-frontends/tda18271c2dd.h
+++ b/drivers/media/dvb-frontends/tda18271c2dd.h
@@ -1,8 +1,6 @@
 #ifndef _TDA18271C2DD_H_
 #define _TDA18271C2DD_H_
 
-#include <linux/kconfig.h>
-
 #if IS_REACHABLE(CONFIG_DVB_TDA18271C2DD)
 struct dvb_frontend *tda18271c2dd_attach(struct dvb_frontend *fe,
 					 struct i2c_adapter *i2c, u8 adr);
diff --git a/drivers/media/dvb-frontends/tda665x.c b/drivers/media/dvb-frontends/tda665x.c
index 82f8cc534f33..7ca965987f40 100644
--- a/drivers/media/dvb-frontends/tda665x.c
+++ b/drivers/media/dvb-frontends/tda665x.c
@@ -206,7 +206,7 @@ static int tda665x_release(struct dvb_frontend *fe)
 	return 0;
 }
 
-static struct dvb_tuner_ops tda665x_ops = {
+static const struct dvb_tuner_ops tda665x_ops = {
 	.get_status	= tda665x_get_status,
 	.set_params	= tda665x_set_params,
 	.get_frequency	= tda665x_get_frequency,
diff --git a/drivers/media/dvb-frontends/tda8261.c b/drivers/media/dvb-frontends/tda8261.c
index 3285b1bc4642..e0df93191b9e 100644
--- a/drivers/media/dvb-frontends/tda8261.c
+++ b/drivers/media/dvb-frontends/tda8261.c
@@ -161,7 +161,7 @@ static int tda8261_release(struct dvb_frontend *fe)
 	return 0;
 }
 
-static struct dvb_tuner_ops tda8261_ops = {
+static const struct dvb_tuner_ops tda8261_ops = {
 
 	.info = {
 		.name		= "TDA8261",
diff --git a/drivers/media/dvb-frontends/tda826x.c b/drivers/media/dvb-frontends/tda826x.c
index 04bbcc24de0a..2ec671df1441 100644
--- a/drivers/media/dvb-frontends/tda826x.c
+++ b/drivers/media/dvb-frontends/tda826x.c
@@ -129,7 +129,7 @@ static int tda826x_get_frequency(struct dvb_frontend *fe, u32 *frequency)
 	return 0;
 }
 
-static struct dvb_tuner_ops tda826x_tuner_ops = {
+static const struct dvb_tuner_ops tda826x_tuner_ops = {
 	.info = {
 		.name = "Philips TDA826X",
 		.frequency_min = 950000,
diff --git a/drivers/media/dvb-frontends/ts2020.c b/drivers/media/dvb-frontends/ts2020.c
index 14b410ffe612..a9f6bbea6df3 100644
--- a/drivers/media/dvb-frontends/ts2020.c
+++ b/drivers/media/dvb-frontends/ts2020.c
@@ -496,7 +496,7 @@ static int ts2020_read_signal_strength(struct dvb_frontend *fe,
 	return 0;
 }
 
-static struct dvb_tuner_ops ts2020_tuner_ops = {
+static const struct dvb_tuner_ops ts2020_tuner_ops = {
 	.info = {
 		.name = "TS2020",
 		.frequency_min = 950000,
diff --git a/drivers/media/dvb-frontends/ts2020.h b/drivers/media/dvb-frontends/ts2020.h
index 9220e5cf0d21..facc54f0a6af 100644
--- a/drivers/media/dvb-frontends/ts2020.h
+++ b/drivers/media/dvb-frontends/ts2020.h
@@ -22,7 +22,6 @@
 #ifndef TS2020_H
 #define TS2020_H
 
-#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 struct ts2020_config {
diff --git a/drivers/media/dvb-frontends/tua6100.c b/drivers/media/dvb-frontends/tua6100.c
index 029384d1fddd..6da12b9e55eb 100644
--- a/drivers/media/dvb-frontends/tua6100.c
+++ b/drivers/media/dvb-frontends/tua6100.c
@@ -157,7 +157,7 @@ static int tua6100_get_frequency(struct dvb_frontend *fe, u32 *frequency)
 	return 0;
 }
 
-static struct dvb_tuner_ops tua6100_tuner_ops = {
+static const struct dvb_tuner_ops tua6100_tuner_ops = {
 	.info = {
 		.name = "Infineon TUA6100",
 		.frequency_min = 950000,
diff --git a/drivers/media/dvb-frontends/zl10036.c b/drivers/media/dvb-frontends/zl10036.c
index 0903d461b8fa..7ed81315965f 100644
--- a/drivers/media/dvb-frontends/zl10036.c
+++ b/drivers/media/dvb-frontends/zl10036.c
@@ -446,7 +446,7 @@ static int zl10036_init(struct dvb_frontend *fe)
 	return ret;
 }
 
-static struct dvb_tuner_ops zl10036_tuner_ops = {
+static const struct dvb_tuner_ops zl10036_tuner_ops = {
 	.info = {
 		.name = "Zarlink ZL10036",
 		.frequency_min = 950000,
diff --git a/drivers/media/dvb-frontends/zl10036.h b/drivers/media/dvb-frontends/zl10036.h
index 670e76a654ee..c568d8d59de3 100644
--- a/drivers/media/dvb-frontends/zl10036.h
+++ b/drivers/media/dvb-frontends/zl10036.h
@@ -21,7 +21,6 @@
 #ifndef DVB_ZL10036_H
 #define DVB_ZL10036_H
 
-#include <linux/kconfig.h>
 #include <linux/i2c.h>
 #include "dvb_frontend.h"
 
diff --git a/drivers/media/dvb-frontends/zl10039.c b/drivers/media/dvb-frontends/zl10039.c
index ee09ec26c553..f8c271be196c 100644
--- a/drivers/media/dvb-frontends/zl10039.c
+++ b/drivers/media/dvb-frontends/zl10039.c
@@ -255,7 +255,7 @@ static int zl10039_release(struct dvb_frontend *fe)
 	return 0;
 }
 
-static struct dvb_tuner_ops zl10039_ops = {
+static const struct dvb_tuner_ops zl10039_ops = {
 	.release = zl10039_release,
 	.init = zl10039_init,
 	.sleep = zl10039_sleep,
diff --git a/drivers/media/dvb-frontends/zl10039.h b/drivers/media/dvb-frontends/zl10039.h
index 070929444e71..66e708569375 100644
--- a/drivers/media/dvb-frontends/zl10039.h
+++ b/drivers/media/dvb-frontends/zl10039.h
@@ -22,8 +22,6 @@
 #ifndef ZL10039_H
 #define ZL10039_H
 
-#include <linux/kconfig.h>
-
 #if IS_REACHABLE(CONFIG_DVB_ZL10039)
 struct dvb_frontend *zl10039_attach(struct dvb_frontend *fe,
 					u8 i2c_addr,
diff --git a/drivers/media/i2c/Kconfig b/drivers/media/i2c/Kconfig
index ce9006e10a30..2669b4bad910 100644
--- a/drivers/media/i2c/Kconfig
+++ b/drivers/media/i2c/Kconfig
@@ -21,7 +21,7 @@ config VIDEO_IR_I2C
 # Encoder / Decoder module configuration
 #
 
-menu "Encoders, decoders, sensors and other helper chips"
+menu "I2C Encoders, decoders, sensors and other helper chips"
 	visible if !MEDIA_SUBDRV_AUTOSELECT || COMPILE_TEST
 
 comment "Audio decoders, processors and mixers"
@@ -187,7 +187,7 @@ comment "Video decoders"
 
 config VIDEO_ADV7180
 	tristate "Analog Devices ADV7180 decoder"
-	depends on VIDEO_V4L2 && I2C && VIDEO_V4L2_SUBDEV_API
+	depends on GPIOLIB && VIDEO_V4L2 && I2C && VIDEO_V4L2_SUBDEV_API
 	---help---
 	  Support for the Analog Devices ADV7180 video decoder.
 
@@ -295,6 +295,13 @@ config VIDEO_ML86V7667
 	  To compile this driver as a module, choose M here: the
 	  module will be called ml86v7667.
 
+config VIDEO_AD5820
+	tristate "AD5820 lens voice coil support"
+	depends on I2C && VIDEO_V4L2 && MEDIA_CONTROLLER
+	---help---
+	  This is a driver for the AD5820 camera lens voice coil.
+	  It is used for example in Nokia N900 (RX-51).
+
 config VIDEO_SAA7110
 	tristate "Philips SAA7110 video decoder"
 	depends on VIDEO_V4L2 && I2C
@@ -571,6 +578,13 @@ config VIDEO_MT9M032
 	  This driver supports MT9M032 camera sensors from Aptina, monochrome
 	  models only.
 
+config VIDEO_MT9M111
+	tristate "mt9m111, mt9m112 and mt9m131 support"
+	depends on I2C && VIDEO_V4L2
+	help
+	  This driver supports MT9M111, MT9M112 and MT9M131 cameras from
+	  Micron/Aptina
+
 config VIDEO_MT9P031
 	tristate "Aptina MT9P031 support"
 	depends on I2C && VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API
diff --git a/drivers/media/i2c/Makefile b/drivers/media/i2c/Makefile
index 94f2c99e890d..92773b2e6225 100644
--- a/drivers/media/i2c/Makefile
+++ b/drivers/media/i2c/Makefile
@@ -19,6 +19,7 @@ obj-$(CONFIG_VIDEO_SAA717X) += saa717x.o
 obj-$(CONFIG_VIDEO_SAA7127) += saa7127.o
 obj-$(CONFIG_VIDEO_SAA7185) += saa7185.o
 obj-$(CONFIG_VIDEO_SAA6752HS) += saa6752hs.o
+obj-$(CONFIG_VIDEO_AD5820)  += ad5820.o
 obj-$(CONFIG_VIDEO_ADV7170) += adv7170.o
 obj-$(CONFIG_VIDEO_ADV7175) += adv7175.o
 obj-$(CONFIG_VIDEO_ADV7180) += adv7180.o
@@ -59,6 +60,7 @@ obj-$(CONFIG_VIDEO_OV7640) += ov7640.o
 obj-$(CONFIG_VIDEO_OV7670) += ov7670.o
 obj-$(CONFIG_VIDEO_OV9650) += ov9650.o
 obj-$(CONFIG_VIDEO_MT9M032) += mt9m032.o
+obj-$(CONFIG_VIDEO_MT9M111) += mt9m111.o
 obj-$(CONFIG_VIDEO_MT9P031) += mt9p031.o
 obj-$(CONFIG_VIDEO_MT9T001) += mt9t001.o
 obj-$(CONFIG_VIDEO_MT9V011) += mt9v011.o
diff --git a/drivers/media/i2c/ad5820.c b/drivers/media/i2c/ad5820.c
new file mode 100644
index 000000000000..beab2f381b81
--- /dev/null
+++ b/drivers/media/i2c/ad5820.c
@@ -0,0 +1,372 @@
+/*
+ * drivers/media/i2c/ad5820.c
+ *
+ * AD5820 DAC driver for camera voice coil focus.
+ *
+ * Copyright (C) 2008 Nokia Corporation
+ * Copyright (C) 2007 Texas Instruments
+ * Copyright (C) 2016 Pavel Machek <pavel@ucw.cz>
+ *
+ * Contact: Tuukka Toivonen <tuukkat76@gmail.com>
+ *	    Sakari Ailus <sakari.ailus@iki.fi>
+ *
+ * Based on af_d88.c by Texas Instruments.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT 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/errno.h>
+#include <linux/i2c.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/regulator/consumer.h>
+
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-subdev.h>
+
+#define AD5820_NAME		"ad5820"
+
+/* Register definitions */
+#define AD5820_POWER_DOWN		(1 << 15)
+#define AD5820_DAC_SHIFT		4
+#define AD5820_RAMP_MODE_LINEAR		(0 << 3)
+#define AD5820_RAMP_MODE_64_16		(1 << 3)
+
+#define CODE_TO_RAMP_US(s)	((s) == 0 ? 0 : (1 << ((s) - 1)) * 50)
+#define RAMP_US_TO_CODE(c)	fls(((c) + ((c)>>1)) / 50)
+
+#define to_ad5820_device(sd)	container_of(sd, struct ad5820_device, subdev)
+
+struct ad5820_device {
+	struct v4l2_subdev subdev;
+	struct ad5820_platform_data *platform_data;
+	struct regulator *vana;
+
+	struct v4l2_ctrl_handler ctrls;
+	u32 focus_absolute;
+	u32 focus_ramp_time;
+	u32 focus_ramp_mode;
+
+	struct mutex power_lock;
+	int power_count;
+
+	bool standby;
+};
+
+static int ad5820_write(struct ad5820_device *coil, u16 data)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&coil->subdev);
+	struct i2c_msg msg;
+	int r;
+
+	if (!client->adapter)
+		return -ENODEV;
+
+	data = cpu_to_be16(data);
+	msg.addr  = client->addr;
+	msg.flags = 0;
+	msg.len   = 2;
+	msg.buf   = (u8 *)&data;
+
+	r = i2c_transfer(client->adapter, &msg, 1);
+	if (r < 0) {
+		dev_err(&client->dev, "write failed, error %d\n", r);
+		return r;
+	}
+
+	return 0;
+}
+
+/*
+ * Calculate status word and write it to the device based on current
+ * values of V4L2 controls. It is assumed that the stored V4L2 control
+ * values are properly limited and rounded.
+ */
+static int ad5820_update_hw(struct ad5820_device *coil)
+{
+	u16 status;
+
+	status = RAMP_US_TO_CODE(coil->focus_ramp_time);
+	status |= coil->focus_ramp_mode
+		? AD5820_RAMP_MODE_64_16 : AD5820_RAMP_MODE_LINEAR;
+	status |= coil->focus_absolute << AD5820_DAC_SHIFT;
+
+	if (coil->standby)
+		status |= AD5820_POWER_DOWN;
+
+	return ad5820_write(coil, status);
+}
+
+/*
+ * Power handling
+ */
+static int ad5820_power_off(struct ad5820_device *coil, bool standby)
+{
+	int ret = 0, ret2;
+
+	/*
+	 * Go to standby first as real power off my be denied by the hardware
+	 * (single power line control for both coil and sensor).
+	 */
+	if (standby) {
+		coil->standby = true;
+		ret = ad5820_update_hw(coil);
+	}
+
+	ret2 = regulator_disable(coil->vana);
+	if (ret)
+		return ret;
+	return ret2;
+}
+
+static int ad5820_power_on(struct ad5820_device *coil, bool restore)
+{
+	int ret;
+
+	ret = regulator_enable(coil->vana);
+	if (ret < 0)
+		return ret;
+
+	if (restore) {
+		/* Restore the hardware settings. */
+		coil->standby = false;
+		ret = ad5820_update_hw(coil);
+		if (ret)
+			goto fail;
+	}
+	return 0;
+
+fail:
+	coil->standby = true;
+	regulator_disable(coil->vana);
+
+	return ret;
+}
+
+/*
+ * V4L2 controls
+ */
+static int ad5820_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct ad5820_device *coil =
+		container_of(ctrl->handler, struct ad5820_device, ctrls);
+
+	switch (ctrl->id) {
+	case V4L2_CID_FOCUS_ABSOLUTE:
+		coil->focus_absolute = ctrl->val;
+		return ad5820_update_hw(coil);
+	}
+
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops ad5820_ctrl_ops = {
+	.s_ctrl = ad5820_set_ctrl,
+};
+
+
+static int ad5820_init_controls(struct ad5820_device *coil)
+{
+	v4l2_ctrl_handler_init(&coil->ctrls, 1);
+
+	/*
+	 * V4L2_CID_FOCUS_ABSOLUTE
+	 *
+	 * Minimum current is 0 mA, maximum is 100 mA. Thus, 1 code is
+	 * equivalent to 100/1023 = 0.0978 mA. Nevertheless, we do not use [mA]
+	 * for focus position, because it is meaningless for user. Meaningful
+	 * would be to use focus distance or even its inverse, but since the
+	 * driver doesn't have sufficiently knowledge to do the conversion, we
+	 * will just use abstract codes here. In any case, smaller value = focus
+	 * position farther from camera. The default zero value means focus at
+	 * infinity, and also least current consumption.
+	 */
+	v4l2_ctrl_new_std(&coil->ctrls, &ad5820_ctrl_ops,
+			  V4L2_CID_FOCUS_ABSOLUTE, 0, 1023, 1, 0);
+
+	if (coil->ctrls.error)
+		return coil->ctrls.error;
+
+	coil->focus_absolute = 0;
+	coil->focus_ramp_time = 0;
+	coil->focus_ramp_mode = 0;
+
+	coil->subdev.ctrl_handler = &coil->ctrls;
+
+	return 0;
+}
+
+/*
+ * V4L2 subdev operations
+ */
+static int ad5820_registered(struct v4l2_subdev *subdev)
+{
+	struct ad5820_device *coil = to_ad5820_device(subdev);
+
+	return ad5820_init_controls(coil);
+}
+
+static int
+ad5820_set_power(struct v4l2_subdev *subdev, int on)
+{
+	struct ad5820_device *coil = to_ad5820_device(subdev);
+	int ret = 0;
+
+	mutex_lock(&coil->power_lock);
+
+	/*
+	 * If the power count is modified from 0 to != 0 or from != 0 to 0,
+	 * update the power state.
+	 */
+	if (coil->power_count == !on) {
+		ret = on ? ad5820_power_on(coil, true) :
+			ad5820_power_off(coil, true);
+		if (ret < 0)
+			goto done;
+	}
+
+	/* Update the power count. */
+	coil->power_count += on ? 1 : -1;
+	WARN_ON(coil->power_count < 0);
+
+done:
+	mutex_unlock(&coil->power_lock);
+	return ret;
+}
+
+static int ad5820_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	return ad5820_set_power(sd, 1);
+}
+
+static int ad5820_close(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	return ad5820_set_power(sd, 0);
+}
+
+static const struct v4l2_subdev_core_ops ad5820_core_ops = {
+	.s_power = ad5820_set_power,
+};
+
+static const struct v4l2_subdev_ops ad5820_ops = {
+	.core = &ad5820_core_ops,
+};
+
+static const struct v4l2_subdev_internal_ops ad5820_internal_ops = {
+	.registered = ad5820_registered,
+	.open = ad5820_open,
+	.close = ad5820_close,
+};
+
+/*
+ * I2C driver
+ */
+static int __maybe_unused ad5820_suspend(struct device *dev)
+{
+	struct i2c_client *client = container_of(dev, struct i2c_client, dev);
+	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
+	struct ad5820_device *coil = to_ad5820_device(subdev);
+
+	if (!coil->power_count)
+		return 0;
+
+	return ad5820_power_off(coil, false);
+}
+
+static int __maybe_unused ad5820_resume(struct device *dev)
+{
+	struct i2c_client *client = container_of(dev, struct i2c_client, dev);
+	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
+	struct ad5820_device *coil = to_ad5820_device(subdev);
+
+	if (!coil->power_count)
+		return 0;
+
+	return ad5820_power_on(coil, true);
+}
+
+static int ad5820_probe(struct i2c_client *client,
+			const struct i2c_device_id *devid)
+{
+	struct ad5820_device *coil;
+	int ret;
+
+	coil = devm_kzalloc(&client->dev, sizeof(*coil), GFP_KERNEL);
+	if (!coil)
+		return -ENOMEM;
+
+	coil->vana = devm_regulator_get(&client->dev, "VANA");
+	if (IS_ERR(coil->vana)) {
+		ret = PTR_ERR(coil->vana);
+		if (ret != -EPROBE_DEFER)
+			dev_err(&client->dev, "could not get regulator for vana\n");
+		return ret;
+	}
+
+	mutex_init(&coil->power_lock);
+
+	v4l2_i2c_subdev_init(&coil->subdev, client, &ad5820_ops);
+	coil->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	coil->subdev.internal_ops = &ad5820_internal_ops;
+	strcpy(coil->subdev.name, "ad5820 focus");
+
+	ret = media_entity_pads_init(&coil->subdev.entity, 0, NULL);
+	if (ret < 0)
+		goto cleanup2;
+
+	ret = v4l2_async_register_subdev(&coil->subdev);
+	if (ret < 0)
+		goto cleanup;
+
+	return ret;
+
+cleanup2:
+	mutex_destroy(&coil->power_lock);
+cleanup:
+	media_entity_cleanup(&coil->subdev.entity);
+	return ret;
+}
+
+static int __exit ad5820_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
+	struct ad5820_device *coil = to_ad5820_device(subdev);
+
+	v4l2_device_unregister_subdev(&coil->subdev);
+	v4l2_ctrl_handler_free(&coil->ctrls);
+	media_entity_cleanup(&coil->subdev.entity);
+	mutex_destroy(&coil->power_lock);
+	return 0;
+}
+
+static const struct i2c_device_id ad5820_id_table[] = {
+	{ AD5820_NAME, 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, ad5820_id_table);
+
+static SIMPLE_DEV_PM_OPS(ad5820_pm, ad5820_suspend, ad5820_resume);
+
+static struct i2c_driver ad5820_i2c_driver = {
+	.driver		= {
+		.name	= AD5820_NAME,
+		.pm	= &ad5820_pm,
+	},
+	.probe		= ad5820_probe,
+	.remove		= __exit_p(ad5820_remove),
+	.id_table	= ad5820_id_table,
+};
+
+module_i2c_driver(ad5820_i2c_driver);
+
+MODULE_AUTHOR("Tuukka Toivonen");
+MODULE_DESCRIPTION("AD5820 camera lens driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/i2c/ad9389b.c b/drivers/media/i2c/ad9389b.c
index 0462f461e679..50f354144ee7 100644
--- a/drivers/media/i2c/ad9389b.c
+++ b/drivers/media/i2c/ad9389b.c
@@ -98,7 +98,6 @@ struct ad9389b_state {
 	struct ad9389b_state_edid edid;
 	/* Running counter of the number of detected EDIDs (for debugging) */
 	unsigned edid_detect_counter;
-	struct workqueue_struct *work_queue;
 	struct delayed_work edid_handler; /* work entry */
 };
 
@@ -843,8 +842,7 @@ static void ad9389b_edid_handler(struct work_struct *work)
 			v4l2_dbg(1, debug, sd, "%s: edid read failed\n", __func__);
 			ad9389b_s_power(sd, false);
 			ad9389b_s_power(sd, true);
-			queue_delayed_work(state->work_queue,
-					   &state->edid_handler, EDID_DELAY);
+			schedule_delayed_work(&state->edid_handler, EDID_DELAY);
 			return;
 		}
 	}
@@ -933,8 +931,7 @@ static void ad9389b_update_monitor_present_status(struct v4l2_subdev *sd)
 		ad9389b_setup(sd);
 		ad9389b_notify_monitor_detect(sd);
 		state->edid.read_retries = EDID_MAX_RETRIES;
-		queue_delayed_work(state->work_queue,
-				   &state->edid_handler, EDID_DELAY);
+		schedule_delayed_work(&state->edid_handler, EDID_DELAY);
 	} else if (!(status & MASK_AD9389B_HPD_DETECT)) {
 		v4l2_dbg(1, debug, sd, "%s: hotplug not detected\n", __func__);
 		state->have_monitor = false;
@@ -1065,8 +1062,7 @@ static bool ad9389b_check_edid_status(struct v4l2_subdev *sd)
 		ad9389b_wr(sd, 0xc9, 0xf);
 		ad9389b_wr(sd, 0xc4, state->edid.segments);
 		state->edid.read_retries = EDID_MAX_RETRIES;
-		queue_delayed_work(state->work_queue,
-				   &state->edid_handler, EDID_DELAY);
+		schedule_delayed_work(&state->edid_handler, EDID_DELAY);
 		return false;
 	}
 
@@ -1170,13 +1166,6 @@ static int ad9389b_probe(struct i2c_client *client, const struct i2c_device_id *
 		goto err_entity;
 	}
 
-	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;
-	}
-
 	INIT_DELAYED_WORK(&state->edid_handler, ad9389b_edid_handler);
 	state->dv_timings = dv1080p60;
 
@@ -1187,8 +1176,6 @@ static int ad9389b_probe(struct i2c_client *client, const struct i2c_device_id *
 		  client->addr << 1, client->adapter->name);
 	return 0;
 
-err_unreg:
-	i2c_unregister_device(state->edid_i2c_client);
 err_entity:
 	media_entity_cleanup(&sd->entity);
 err_hdl:
@@ -1211,9 +1198,8 @@ static int ad9389b_remove(struct i2c_client *client)
 	ad9389b_s_stream(sd, false);
 	ad9389b_s_audio_stream(sd, false);
 	ad9389b_init_setup(sd);
-	cancel_delayed_work(&state->edid_handler);
+	cancel_delayed_work_sync(&state->edid_handler);
 	i2c_unregister_device(state->edid_i2c_client);
-	destroy_workqueue(state->work_queue);
 	v4l2_device_unregister_subdev(sd);
 	media_entity_cleanup(&sd->entity);
 	v4l2_ctrl_handler_free(sd->ctrl_handler);
@@ -1231,7 +1217,6 @@ MODULE_DEVICE_TABLE(i2c, ad9389b_id);
 
 static struct i2c_driver ad9389b_driver = {
 	.driver = {
-		.owner = THIS_MODULE,
 		.name = "ad9389b",
 	},
 	.probe = ad9389b_probe,
diff --git a/drivers/media/i2c/adv7180.c b/drivers/media/i2c/adv7180.c
index 95cbc857f36e..cbed2bc29325 100644
--- a/drivers/media/i2c/adv7180.c
+++ b/drivers/media/i2c/adv7180.c
@@ -26,6 +26,7 @@
 #include <linux/i2c.h>
 #include <linux/slab.h>
 #include <linux/of.h>
+#include <linux/gpio/consumer.h>
 #include <linux/videodev2.h>
 #include <media/v4l2-ioctl.h>
 #include <media/v4l2-event.h>
@@ -56,10 +57,11 @@
 
 #define ADV7182_REG_INPUT_VIDSEL			0x0002
 
+#define ADV7180_REG_OUTPUT_CONTROL			0x0003
 #define ADV7180_REG_EXTENDED_OUTPUT_CONTROL		0x0004
 #define ADV7180_EXTENDED_OUTPUT_CONTROL_NTSCDIS		0xC5
 
-#define ADV7180_REG_AUTODETECT_ENABLE			0x07
+#define ADV7180_REG_AUTODETECT_ENABLE			0x0007
 #define ADV7180_AUTODETECT_DEFAULT			0x7f
 /* Contrast */
 #define ADV7180_REG_CON		0x0008	/*Unsigned */
@@ -100,6 +102,20 @@
 #define ADV7180_REG_IDENT 0x0011
 #define ADV7180_ID_7180 0x18
 
+#define ADV7180_REG_STATUS3		0x0013
+#define ADV7180_REG_ANALOG_CLAMP_CTL	0x0014
+#define ADV7180_REG_SHAP_FILTER_CTL_1	0x0017
+#define ADV7180_REG_CTRL_2		0x001d
+#define ADV7180_REG_VSYNC_FIELD_CTL_1	0x0031
+#define ADV7180_REG_MANUAL_WIN_CTL_1	0x003d
+#define ADV7180_REG_MANUAL_WIN_CTL_2	0x003e
+#define ADV7180_REG_MANUAL_WIN_CTL_3	0x003f
+#define ADV7180_REG_LOCK_CNT		0x0051
+#define ADV7180_REG_CVBS_TRIM		0x0052
+#define ADV7180_REG_CLAMP_ADJ		0x005a
+#define ADV7180_REG_RES_CIR		0x005f
+#define ADV7180_REG_DIFF_MODE		0x0060
+
 #define ADV7180_REG_ICONF1		0x2040
 #define ADV7180_ICONF1_ACTIVE_LOW	0x01
 #define ADV7180_ICONF1_PSYNC_ONLY	0x10
@@ -129,9 +145,15 @@
 #define ADV7180_REG_VPP_SLAVE_ADDR	0xFD
 #define ADV7180_REG_CSI_SLAVE_ADDR	0xFE
 
-#define ADV7180_REG_FLCONTROL 0x40e0
+#define ADV7180_REG_ACE_CTRL1		0x4080
+#define ADV7180_REG_ACE_CTRL5		0x4084
+#define ADV7180_REG_FLCONTROL		0x40e0
 #define ADV7180_FLCONTROL_FL_ENABLE 0x1
 
+#define ADV7180_REG_RST_CLAMP	0x809c
+#define ADV7180_REG_AGC_ADJ1	0x80b6
+#define ADV7180_REG_AGC_ADJ2	0x80c0
+
 #define ADV7180_CSI_REG_PWRDN	0x00
 #define ADV7180_CSI_PWRDN	0x80
 
@@ -192,6 +214,7 @@ struct adv7180_state {
 	struct media_pad	pad;
 	struct mutex		mutex; /* mutual excl. when accessing chip */
 	int			irq;
+	struct gpio_desc	*pwdn_gpio;
 	v4l2_std_id		curr_norm;
 	bool			powered;
 	bool			streaming;
@@ -442,6 +465,19 @@ static int adv7180_g_std(struct v4l2_subdev *sd, v4l2_std_id *norm)
 	return 0;
 }
 
+static void adv7180_set_power_pin(struct adv7180_state *state, bool on)
+{
+	if (!state->pwdn_gpio)
+		return;
+
+	if (on) {
+		gpiod_set_value_cansleep(state->pwdn_gpio, 0);
+		usleep_range(5000, 10000);
+	} else {
+		gpiod_set_value_cansleep(state->pwdn_gpio, 1);
+	}
+}
+
 static int adv7180_set_power(struct adv7180_state *state, bool on)
 {
 	u8 val;
@@ -597,7 +633,7 @@ static int adv7180_enum_mbus_code(struct v4l2_subdev *sd,
 	if (code->index != 0)
 		return -EINVAL;
 
-	code->code = MEDIA_BUS_FMT_YUYV8_2X8;
+	code->code = MEDIA_BUS_FMT_UYVY8_2X8;
 
 	return 0;
 }
@@ -607,7 +643,7 @@ static int adv7180_mbus_fmt(struct v4l2_subdev *sd,
 {
 	struct adv7180_state *state = to_state(sd);
 
-	fmt->code = MEDIA_BUS_FMT_YUYV8_2X8;
+	fmt->code = MEDIA_BUS_FMT_UYVY8_2X8;
 	fmt->colorspace = V4L2_COLORSPACE_SMPTE170M;
 	fmt->width = 720;
 	fmt->height = state->curr_norm & V4L2_STD_525_60 ? 480 : 576;
@@ -675,6 +711,7 @@ static int adv7180_set_pad_format(struct v4l2_subdev *sd,
 {
 	struct adv7180_state *state = to_state(sd);
 	struct v4l2_mbus_framefmt *framefmt;
+	int ret;
 
 	switch (format->format.field) {
 	case V4L2_FIELD_NONE:
@@ -686,8 +723,9 @@ static int adv7180_set_pad_format(struct v4l2_subdev *sd,
 		break;
 	}
 
+	ret = adv7180_mbus_fmt(sd,  &format->format);
+
 	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
-		framefmt = &format->format;
 		if (state->field != format->format.field) {
 			state->field = format->format.field;
 			adv7180_set_power(state, false);
@@ -699,7 +737,7 @@ static int adv7180_set_pad_format(struct v4l2_subdev *sd,
 		*framefmt = format->format;
 	}
 
-	return adv7180_mbus_fmt(sd, framefmt);
+	return ret;
 }
 
 static int adv7180_g_mbus_config(struct v4l2_subdev *sd,
@@ -725,16 +763,16 @@ static int adv7180_g_mbus_config(struct v4l2_subdev *sd,
 	return 0;
 }
 
-static int adv7180_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *cropcap)
+static int adv7180_g_pixelaspect(struct v4l2_subdev *sd, struct v4l2_fract *aspect)
 {
 	struct adv7180_state *state = to_state(sd);
 
 	if (state->curr_norm & V4L2_STD_525_60) {
-		cropcap->pixelaspect.numerator = 11;
-		cropcap->pixelaspect.denominator = 10;
+		aspect->numerator = 11;
+		aspect->denominator = 10;
 	} else {
-		cropcap->pixelaspect.numerator = 54;
-		cropcap->pixelaspect.denominator = 59;
+		aspect->numerator = 54;
+		aspect->denominator = 59;
 	}
 
 	return 0;
@@ -787,7 +825,7 @@ static const struct v4l2_subdev_video_ops adv7180_video_ops = {
 	.g_input_status = adv7180_g_input_status,
 	.s_routing = adv7180_s_routing,
 	.g_mbus_config = adv7180_g_mbus_config,
-	.cropcap = adv7180_cropcap,
+	.g_pixelaspect = adv7180_g_pixelaspect,
 	.g_tvnorms = adv7180_g_tvnorms,
 	.s_stream = adv7180_s_stream,
 };
@@ -886,16 +924,20 @@ static int adv7182_init(struct adv7180_state *state)
 
 	/* ADI required writes */
 	if (state->chip_info->flags & ADV7180_FLAG_MIPI_CSI2) {
-		adv7180_write(state, 0x0003, 0x4e);
-		adv7180_write(state, 0x0004, 0x57);
-		adv7180_write(state, 0x001d, 0xc0);
+		adv7180_write(state, ADV7180_REG_OUTPUT_CONTROL, 0x4e);
+		adv7180_write(state, ADV7180_REG_EXTENDED_OUTPUT_CONTROL, 0x57);
+		adv7180_write(state, ADV7180_REG_CTRL_2, 0xc0);
 	} else {
 		if (state->chip_info->flags & ADV7180_FLAG_V2)
-			adv7180_write(state, 0x0004, 0x17);
+			adv7180_write(state,
+				      ADV7180_REG_EXTENDED_OUTPUT_CONTROL,
+				      0x17);
 		else
-			adv7180_write(state, 0x0004, 0x07);
-		adv7180_write(state, 0x0003, 0x0c);
-		adv7180_write(state, 0x001d, 0x40);
+			adv7180_write(state,
+				      ADV7180_REG_EXTENDED_OUTPUT_CONTROL,
+				      0x07);
+		adv7180_write(state, ADV7180_REG_OUTPUT_CONTROL, 0x0c);
+		adv7180_write(state, ADV7180_REG_CTRL_2, 0x40);
 	}
 
 	adv7180_write(state, 0x0013, 0x00);
@@ -972,8 +1014,8 @@ static int adv7182_select_input(struct adv7180_state *state, unsigned int input)
 		return ret;
 
 	/* Reset clamp circuitry - ADI recommended writes */
-	adv7180_write(state, 0x809c, 0x00);
-	adv7180_write(state, 0x809c, 0xff);
+	adv7180_write(state, ADV7180_REG_RST_CLAMP, 0x00);
+	adv7180_write(state, ADV7180_REG_RST_CLAMP, 0xff);
 
 	input_type = adv7182_get_input_type(input);
 
@@ -981,10 +1023,10 @@ static int adv7182_select_input(struct adv7180_state *state, unsigned int input)
 	case ADV7182_INPUT_TYPE_CVBS:
 	case ADV7182_INPUT_TYPE_DIFF_CVBS:
 		/* ADI recommends to use the SH1 filter */
-		adv7180_write(state, 0x0017, 0x41);
+		adv7180_write(state, ADV7180_REG_SHAP_FILTER_CTL_1, 0x41);
 		break;
 	default:
-		adv7180_write(state, 0x0017, 0x01);
+		adv7180_write(state, ADV7180_REG_SHAP_FILTER_CTL_1, 0x01);
 		break;
 	}
 
@@ -994,21 +1036,21 @@ static int adv7182_select_input(struct adv7180_state *state, unsigned int input)
 		lbias = adv7182_lbias_settings[input_type];
 
 	for (i = 0; i < ARRAY_SIZE(adv7182_lbias_settings[0]); i++)
-		adv7180_write(state, 0x0052 + i, lbias[i]);
+		adv7180_write(state, ADV7180_REG_CVBS_TRIM + i, lbias[i]);
 
 	if (input_type == ADV7182_INPUT_TYPE_DIFF_CVBS) {
 		/* ADI required writes to make differential CVBS work */
-		adv7180_write(state, 0x005f, 0xa8);
-		adv7180_write(state, 0x005a, 0x90);
-		adv7180_write(state, 0x0060, 0xb0);
-		adv7180_write(state, 0x80b6, 0x08);
-		adv7180_write(state, 0x80c0, 0xa0);
+		adv7180_write(state, ADV7180_REG_RES_CIR, 0xa8);
+		adv7180_write(state, ADV7180_REG_CLAMP_ADJ, 0x90);
+		adv7180_write(state, ADV7180_REG_DIFF_MODE, 0xb0);
+		adv7180_write(state, ADV7180_REG_AGC_ADJ1, 0x08);
+		adv7180_write(state, ADV7180_REG_AGC_ADJ2, 0xa0);
 	} else {
-		adv7180_write(state, 0x005f, 0xf0);
-		adv7180_write(state, 0x005a, 0xd0);
-		adv7180_write(state, 0x0060, 0x10);
-		adv7180_write(state, 0x80b6, 0x9c);
-		adv7180_write(state, 0x80c0, 0x00);
+		adv7180_write(state, ADV7180_REG_RES_CIR, 0xf0);
+		adv7180_write(state, ADV7180_REG_CLAMP_ADJ, 0xd0);
+		adv7180_write(state, ADV7180_REG_DIFF_MODE, 0x10);
+		adv7180_write(state, ADV7180_REG_AGC_ADJ1, 0x9c);
+		adv7180_write(state, ADV7180_REG_AGC_ADJ2, 0x00);
 	}
 
 	return 0;
@@ -1185,6 +1227,8 @@ static int init_device(struct adv7180_state *state)
 
 	mutex_lock(&state->mutex);
 
+	adv7180_set_power_pin(state, true);
+
 	adv7180_write(state, ADV7180_REG_PWR_MAN, ADV7180_PWR_MAN_RES);
 	usleep_range(5000, 10000);
 
@@ -1254,6 +1298,14 @@ static int adv7180_probe(struct i2c_client *client,
 	state->field = V4L2_FIELD_INTERLACED;
 	state->chip_info = (struct adv7180_chip_info *)id->driver_data;
 
+	state->pwdn_gpio = devm_gpiod_get_optional(&client->dev, "powerdown",
+						   GPIOD_OUT_HIGH);
+	if (IS_ERR(state->pwdn_gpio)) {
+		ret = PTR_ERR(state->pwdn_gpio);
+		v4l_err(client, "request for power pin failed: %d\n", ret);
+		return ret;
+	}
+
 	if (state->chip_info->flags & ADV7180_FLAG_MIPI_CSI2) {
 		state->csi_client = i2c_new_dummy(client->adapter,
 				ADV7180_DEFAULT_CSI_I2C_ADDR);
@@ -1345,6 +1397,8 @@ static int adv7180_remove(struct i2c_client *client)
 	if (state->chip_info->flags & ADV7180_FLAG_MIPI_CSI2)
 		i2c_unregister_device(state->csi_client);
 
+	adv7180_set_power_pin(state, false);
+
 	mutex_destroy(&state->mutex);
 
 	return 0;
diff --git a/drivers/media/i2c/adv7183.c b/drivers/media/i2c/adv7183.c
index 2bec737881e9..04eecda74d66 100644
--- a/drivers/media/i2c/adv7183.c
+++ b/drivers/media/i2c/adv7183.c
@@ -644,7 +644,6 @@ MODULE_DEVICE_TABLE(i2c, adv7183_id);
 
 static struct i2c_driver adv7183_driver = {
 	.driver = {
-		.owner  = THIS_MODULE,
 		.name   = "adv7183",
 	},
 	.probe          = adv7183_probe,
diff --git a/drivers/media/i2c/adv7393.c b/drivers/media/i2c/adv7393.c
index 76d987476e35..f19ad4ecd11e 100644
--- a/drivers/media/i2c/adv7393.c
+++ b/drivers/media/i2c/adv7393.c
@@ -456,7 +456,6 @@ MODULE_DEVICE_TABLE(i2c, adv7393_id);
 
 static struct i2c_driver adv7393_driver = {
 	.driver = {
-		.owner	= THIS_MODULE,
 		.name	= "adv7393",
 	},
 	.probe		= adv7393_probe,
diff --git a/drivers/media/i2c/adv7511.c b/drivers/media/i2c/adv7511.c
index 53030d631653..5ba0f21bcfe4 100644
--- a/drivers/media/i2c/adv7511.c
+++ b/drivers/media/i2c/adv7511.c
@@ -1898,6 +1898,7 @@ static int adv7511_probe(struct i2c_client *client, const struct i2c_device_id *
 					       state->i2c_cec_addr >> 1);
 		if (state->i2c_cec == NULL) {
 			v4l2_err(sd, "failed to register cec i2c client\n");
+			err = -ENOMEM;
 			goto err_unreg_edid;
 		}
 		adv7511_wr(sd, 0xe2, 0x00); /* power up cec section */
diff --git a/drivers/media/i2c/ak881x.c b/drivers/media/i2c/ak881x.c
index d9f2b6b76d59..3a795dcb7d8e 100644
--- a/drivers/media/i2c/ak881x.c
+++ b/drivers/media/i2c/ak881x.c
@@ -124,21 +124,27 @@ static int ak881x_enum_mbus_code(struct v4l2_subdev *sd,
 	return 0;
 }
 
-static int ak881x_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
+static int ak881x_get_selection(struct v4l2_subdev *sd,
+				struct v4l2_subdev_pad_config *cfg,
+				struct v4l2_subdev_selection *sel)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct ak881x *ak881x = to_ak881x(client);
 
-	a->bounds.left			= 0;
-	a->bounds.top			= 0;
-	a->bounds.width			= 720;
-	a->bounds.height		= ak881x->lines;
-	a->defrect			= a->bounds;
-	a->type				= V4L2_BUF_TYPE_VIDEO_OUTPUT;
-	a->pixelaspect.numerator	= 1;
-	a->pixelaspect.denominator	= 1;
+	if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE)
+		return -EINVAL;
 
-	return 0;
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+		sel->r.left = 0;
+		sel->r.top = 0;
+		sel->r.width = 720;
+		sel->r.height = ak881x->lines;
+		return 0;
+	default:
+		return -EINVAL;
+	}
 }
 
 static int ak881x_s_std_output(struct v4l2_subdev *sd, v4l2_std_id std)
@@ -207,13 +213,13 @@ static struct v4l2_subdev_core_ops ak881x_subdev_core_ops = {
 };
 
 static struct v4l2_subdev_video_ops ak881x_subdev_video_ops = {
-	.cropcap	= ak881x_cropcap,
 	.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,
+	.get_selection	= ak881x_get_selection,
 	.set_fmt	= ak881x_fill_fmt,
 	.get_fmt	= ak881x_fill_fmt,
 };
diff --git a/drivers/media/i2c/cs3308.c b/drivers/media/i2c/cs3308.c
index d28b4f37fe5f..7da5f69cace6 100644
--- a/drivers/media/i2c/cs3308.c
+++ b/drivers/media/i2c/cs3308.c
@@ -127,7 +127,6 @@ MODULE_DEVICE_TABLE(i2c, cs3308_id);
 
 static struct i2c_driver cs3308_driver = {
 	.driver = {
-		.owner  = THIS_MODULE,
 		.name   = "cs3308",
 	},
 	.probe          = cs3308_probe,
diff --git a/drivers/media/i2c/ir-kbd-i2c.c b/drivers/media/i2c/ir-kbd-i2c.c
index bf82726fd3f4..f95a6bc839d5 100644
--- a/drivers/media/i2c/ir-kbd-i2c.c
+++ b/drivers/media/i2c/ir-kbd-i2c.c
@@ -35,6 +35,7 @@
  *
  */
 
+#include <asm/unaligned.h>
 #include <linux/module.h>
 #include <linux/init.h>
 #include <linux/kernel.h>
@@ -63,51 +64,80 @@ module_param(debug, int, 0644);    /* debug level (0,1,2) */
 /* ----------------------------------------------------------------------- */
 
 static int get_key_haup_common(struct IR_i2c *ir, enum rc_type *protocol,
-			       u32 *scancode, u8 *ptoggle, int size, int offset)
+					u32 *scancode, u8 *ptoggle, int size)
 {
 	unsigned char buf[6];
-	int start, range, toggle, dev, code, ircode;
+	int start, range, toggle, dev, code, ircode, vendor;
 
 	/* poll IR chip */
 	if (size != i2c_master_recv(ir->c, buf, size))
 		return -EIO;
 
-	/* split rc5 data block ... */
-	start  = (buf[offset] >> 7) &    1;
-	range  = (buf[offset] >> 6) &    1;
-	toggle = (buf[offset] >> 5) &    1;
-	dev    =  buf[offset]       & 0x1f;
-	code   = (buf[offset+1] >> 2) & 0x3f;
+	if (buf[0] & 0x80) {
+		int offset = (size == 6) ? 3 : 0;
 
-	/* rc5 has two start bits
-	 * the first bit must be one
-	 * the second bit defines the command range (1 = 0-63, 0 = 64 - 127)
-	 */
-	if (!start)
-		/* no key pressed */
-		return 0;
+		/* split rc5 data block ... */
+		start  = (buf[offset] >> 7) &    1;
+		range  = (buf[offset] >> 6) &    1;
+		toggle = (buf[offset] >> 5) &    1;
+		dev    =  buf[offset]       & 0x1f;
+		code   = (buf[offset+1] >> 2) & 0x3f;
 
-	/* filter out invalid key presses */
-	ircode = (start << 12) | (toggle << 11) | (dev << 6) | code;
-	if ((ircode & 0x1fff) == 0x1fff)
-		return 0;
+		/* rc5 has two start bits
+		 * the first bit must be one
+		 * the second bit defines the command range:
+		 * 1 = 0-63, 0 = 64 - 127
+		 */
+		if (!start)
+			/* no key pressed */
+			return 0;
 
-	if (!range)
-		code += 64;
+		/* filter out invalid key presses */
+		ircode = (start << 12) | (toggle << 11) | (dev << 6) | code;
+		if ((ircode & 0x1fff) == 0x1fff)
+			return 0;
 
-	dprintk(1,"ir hauppauge (rc5): s%d r%d t%d dev=%d code=%d\n",
-		start, range, toggle, dev, code);
+		if (!range)
+			code += 64;
 
-	*protocol = RC_TYPE_RC5;
-	*scancode = RC_SCANCODE_RC5(dev, code);
-	*ptoggle = toggle;
-	return 1;
+		dprintk(1, "ir hauppauge (rc5): s%d r%d t%d dev=%d code=%d\n",
+			start, range, toggle, dev, code);
+
+		*protocol = RC_TYPE_RC5;
+		*scancode = RC_SCANCODE_RC5(dev, code);
+		*ptoggle = toggle;
+
+		return 1;
+	} else if (size == 6 && (buf[0] & 0x40)) {
+		code = buf[4];
+		dev = buf[3];
+		vendor = get_unaligned_be16(buf + 1);
+
+		if (vendor == 0x800f) {
+			*ptoggle = (dev & 0x80) != 0;
+			*protocol = RC_TYPE_RC6_MCE;
+			dev &= 0x7f;
+			dprintk(1, "ir hauppauge (rc6-mce): t%d vendor=%d dev=%d code=%d\n",
+						toggle, vendor, dev, code);
+		} else {
+			*ptoggle = 0;
+			*protocol = RC_TYPE_RC6_6A_32;
+			dprintk(1, "ir hauppauge (rc6-6a-32): vendor=%d dev=%d code=%d\n",
+							vendor, dev, code);
+		}
+
+		*scancode = RC_SCANCODE_RC6_6A(vendor, dev, code);
+
+		return 1;
+	}
+
+	return 0;
 }
 
 static int get_key_haup(struct IR_i2c *ir, enum rc_type *protocol,
 			u32 *scancode, u8 *toggle)
 {
-	return get_key_haup_common (ir, protocol, scancode, toggle, 3, 0);
+	return get_key_haup_common(ir, protocol, scancode, toggle, 3);
 }
 
 static int get_key_haup_xvr(struct IR_i2c *ir, enum rc_type *protocol,
@@ -126,7 +156,7 @@ static int get_key_haup_xvr(struct IR_i2c *ir, enum rc_type *protocol,
 	if (ret != 1)
 		return (ret < 0) ? ret : -EINVAL;
 
-	return get_key_haup_common(ir, protocol, scancode, toggle, 6, 3);
+	return get_key_haup_common(ir, protocol, scancode, toggle, 6);
 }
 
 static int get_key_pixelview(struct IR_i2c *ir, enum rc_type *protocol,
@@ -347,7 +377,7 @@ static int ir_probe(struct i2c_client *client, const struct i2c_device_id *id)
 	case 0x71:
 		name        = "Hauppauge/Zilog Z8";
 		ir->get_key = get_key_haup_xvr;
-		rc_type     = RC_BIT_RC5;
+		rc_type     = RC_BIT_RC5 | RC_BIT_RC6_MCE | RC_BIT_RC6_6A_32;
 		ir_codes    = RC_MAP_HAUPPAUGE;
 		break;
 	}
diff --git a/drivers/media/i2c/soc_camera/mt9m111.c b/drivers/media/i2c/mt9m111.c
index 6dfaead6aaa8..72e71b762827 100644
--- a/drivers/media/i2c/soc_camera/mt9m111.c
+++ b/drivers/media/i2c/mt9m111.c
@@ -16,10 +16,11 @@
 #include <linux/v4l2-mediabus.h>
 #include <linux/module.h>
 
-#include <media/soc_camera.h>
+#include <media/v4l2-async.h>
 #include <media/v4l2-clk.h>
 #include <media/v4l2-common.h>
 #include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
 
 /*
  * MT9M111, MT9M112 and MT9M131:
@@ -187,10 +188,10 @@ struct mt9m111_datafmt {
 };
 
 static const struct mt9m111_datafmt mt9m111_colour_fmts[] = {
-	{MEDIA_BUS_FMT_YUYV8_2X8, V4L2_COLORSPACE_JPEG},
-	{MEDIA_BUS_FMT_YVYU8_2X8, V4L2_COLORSPACE_JPEG},
-	{MEDIA_BUS_FMT_UYVY8_2X8, V4L2_COLORSPACE_JPEG},
-	{MEDIA_BUS_FMT_VYUY8_2X8, V4L2_COLORSPACE_JPEG},
+	{MEDIA_BUS_FMT_YUYV8_2X8, V4L2_COLORSPACE_SRGB},
+	{MEDIA_BUS_FMT_YVYU8_2X8, V4L2_COLORSPACE_SRGB},
+	{MEDIA_BUS_FMT_UYVY8_2X8, V4L2_COLORSPACE_SRGB},
+	{MEDIA_BUS_FMT_VYUY8_2X8, V4L2_COLORSPACE_SRGB},
 	{MEDIA_BUS_FMT_RGB555_2X8_PADHI_LE, V4L2_COLORSPACE_SRGB},
 	{MEDIA_BUS_FMT_RGB555_2X8_PADHI_BE, V4L2_COLORSPACE_SRGB},
 	{MEDIA_BUS_FMT_RGB565_2X8_LE, V4L2_COLORSPACE_SRGB},
@@ -383,30 +384,36 @@ static int mt9m111_reset(struct mt9m111 *mt9m111)
 	return ret;
 }
 
-static int mt9m111_s_crop(struct v4l2_subdev *sd, const struct v4l2_crop *a)
+static int mt9m111_set_selection(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_pad_config *cfg,
+				 struct v4l2_subdev_selection *sel)
 {
-	struct v4l2_rect rect = a->c;
-	struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9m111 *mt9m111 = to_mt9m111(client);
+	struct v4l2_rect rect = sel->r;
 	int width, height;
-	int ret;
+	int ret, align = 0;
 
-	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+	if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE ||
+	    sel->target != V4L2_SEL_TGT_CROP)
 		return -EINVAL;
 
 	if (mt9m111->fmt->code == MEDIA_BUS_FMT_SBGGR8_1X8 ||
 	    mt9m111->fmt->code == MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE) {
 		/* Bayer format - even size lengths */
-		rect.width	= ALIGN(rect.width, 2);
-		rect.height	= ALIGN(rect.height, 2);
+		align = 1;
 		/* Let the user play with the starting pixel */
 	}
 
 	/* FIXME: the datasheet doesn't specify minimum sizes */
-	soc_camera_limit_side(&rect.left, &rect.width,
-		     MT9M111_MIN_DARK_COLS, 2, MT9M111_MAX_WIDTH);
-
-	soc_camera_limit_side(&rect.top, &rect.height,
-		     MT9M111_MIN_DARK_ROWS, 2, MT9M111_MAX_HEIGHT);
+	v4l_bound_align_image(&rect.width, 2, MT9M111_MAX_WIDTH, align,
+			      &rect.height, 2, MT9M111_MAX_HEIGHT, align, 0);
+	rect.left = clamp(rect.left, MT9M111_MIN_DARK_COLS,
+			  MT9M111_MIN_DARK_COLS + MT9M111_MAX_WIDTH -
+			  (__s32)rect.width);
+	rect.top = clamp(rect.top, MT9M111_MIN_DARK_ROWS,
+			 MT9M111_MIN_DARK_ROWS + MT9M111_MAX_HEIGHT -
+			 (__s32)rect.height);
 
 	width = min(mt9m111->width, rect.width);
 	height = min(mt9m111->height, rect.height);
@@ -421,30 +428,30 @@ static int mt9m111_s_crop(struct v4l2_subdev *sd, const struct v4l2_crop *a)
 	return ret;
 }
 
-static int mt9m111_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
+static int mt9m111_get_selection(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_pad_config *cfg,
+				 struct v4l2_subdev_selection *sel)
 {
-	struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
-
-	a->c	= mt9m111->rect;
-	a->type	= V4L2_BUF_TYPE_VIDEO_CAPTURE;
-
-	return 0;
-}
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9m111 *mt9m111 = to_mt9m111(client);
 
-static int mt9m111_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
-{
-	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+	if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE)
 		return -EINVAL;
 
-	a->bounds.left			= MT9M111_MIN_DARK_COLS;
-	a->bounds.top			= MT9M111_MIN_DARK_ROWS;
-	a->bounds.width			= MT9M111_MAX_WIDTH;
-	a->bounds.height		= MT9M111_MAX_HEIGHT;
-	a->defrect			= a->bounds;
-	a->pixelaspect.numerator	= 1;
-	a->pixelaspect.denominator	= 1;
-
-	return 0;
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+		sel->r.left = MT9M111_MIN_DARK_COLS;
+		sel->r.top = MT9M111_MIN_DARK_ROWS;
+		sel->r.width = MT9M111_MAX_WIDTH;
+		sel->r.height = MT9M111_MAX_HEIGHT;
+		return 0;
+	case V4L2_SEL_TGT_CROP:
+		sel->r = mt9m111->rect;
+		return 0;
+	default:
+		return -EINVAL;
+	}
 }
 
 static int mt9m111_get_fmt(struct v4l2_subdev *sd,
@@ -775,17 +782,16 @@ static int mt9m111_init(struct mt9m111 *mt9m111)
 static int mt9m111_power_on(struct mt9m111 *mt9m111)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
-	struct soc_camera_subdev_desc *ssdd = soc_camera_i2c_to_desc(client);
 	int ret;
 
-	ret = soc_camera_power_on(&client->dev, ssdd, mt9m111->clk);
+	ret = v4l2_clk_enable(mt9m111->clk);
 	if (ret < 0)
 		return ret;
 
 	ret = mt9m111_resume(mt9m111);
 	if (ret < 0) {
 		dev_err(&client->dev, "Failed to resume the sensor: %d\n", ret);
-		soc_camera_power_off(&client->dev, ssdd, mt9m111->clk);
+		v4l2_clk_disable(mt9m111->clk);
 	}
 
 	return ret;
@@ -793,11 +799,8 @@ static int mt9m111_power_on(struct mt9m111 *mt9m111)
 
 static void mt9m111_power_off(struct mt9m111 *mt9m111)
 {
-	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
-	struct soc_camera_subdev_desc *ssdd = soc_camera_i2c_to_desc(client);
-
 	mt9m111_suspend(mt9m111);
-	soc_camera_power_off(&client->dev, ssdd, mt9m111->clk);
+	v4l2_clk_disable(mt9m111->clk);
 }
 
 static int mt9m111_s_power(struct v4l2_subdev *sd, int on)
@@ -854,27 +857,22 @@ static int mt9m111_enum_mbus_code(struct v4l2_subdev *sd,
 static int mt9m111_g_mbus_config(struct v4l2_subdev *sd,
 				struct v4l2_mbus_config *cfg)
 {
-	struct i2c_client *client = v4l2_get_subdevdata(sd);
-	struct soc_camera_subdev_desc *ssdd = soc_camera_i2c_to_desc(client);
-
 	cfg->flags = V4L2_MBUS_MASTER | V4L2_MBUS_PCLK_SAMPLE_RISING |
 		V4L2_MBUS_HSYNC_ACTIVE_HIGH | V4L2_MBUS_VSYNC_ACTIVE_HIGH |
 		V4L2_MBUS_DATA_ACTIVE_HIGH;
 	cfg->type = V4L2_MBUS_PARALLEL;
-	cfg->flags = soc_camera_apply_board_flags(ssdd, cfg);
 
 	return 0;
 }
 
 static struct v4l2_subdev_video_ops mt9m111_subdev_video_ops = {
-	.s_crop		= mt9m111_s_crop,
-	.g_crop		= mt9m111_g_crop,
-	.cropcap	= mt9m111_cropcap,
 	.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_selection	= mt9m111_get_selection,
+	.set_selection	= mt9m111_set_selection,
 	.get_fmt	= mt9m111_get_fmt,
 	.set_fmt	= mt9m111_set_fmt,
 };
@@ -933,20 +931,8 @@ static int mt9m111_probe(struct i2c_client *client,
 {
 	struct mt9m111 *mt9m111;
 	struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
-	struct soc_camera_subdev_desc *ssdd = soc_camera_i2c_to_desc(client);
 	int ret;
 
-	if (client->dev.of_node) {
-		ssdd = devm_kzalloc(&client->dev, sizeof(*ssdd), GFP_KERNEL);
-		if (!ssdd)
-			return -ENOMEM;
-		client->dev.platform_data = ssdd;
-	}
-	if (!ssdd) {
-		dev_err(&client->dev, "mt9m111: driver needs platform data\n");
-		return -EINVAL;
-	}
-
 	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_WORD_DATA)) {
 		dev_warn(&adapter->dev,
 			 "I2C-Adapter doesn't support I2C_FUNC_SMBUS_WORD\n");
@@ -992,10 +978,6 @@ static int mt9m111_probe(struct i2c_client *client,
 	mt9m111->lastpage	= -1;
 	mutex_init(&mt9m111->power_lock);
 
-	ret = soc_camera_power_init(&client->dev, ssdd);
-	if (ret < 0)
-		goto out_hdlfree;
-
 	ret = mt9m111_video_probe(client);
 	if (ret < 0)
 		goto out_hdlfree;
diff --git a/drivers/media/i2c/ov9650.c b/drivers/media/i2c/ov9650.c
index be5a7fd4f076..502c72238a4a 100644
--- a/drivers/media/i2c/ov9650.c
+++ b/drivers/media/i2c/ov9650.c
@@ -23,6 +23,7 @@
 #include <linux/videodev2.h>
 
 #include <media/media-entity.h>
+#include <media/v4l2-async.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-event.h>
@@ -1520,6 +1521,10 @@ static int ov965x_probe(struct i2c_client *client,
 	/* Update exposure time min/max to match frame format */
 	ov965x_update_exposure_ctrl(ov965x);
 
+	ret = v4l2_async_register_subdev(sd);
+	if (ret < 0)
+		goto err_ctrls;
+
 	return 0;
 err_ctrls:
 	v4l2_ctrl_handler_free(sd->ctrl_handler);
@@ -1532,7 +1537,7 @@ static int ov965x_remove(struct i2c_client *client)
 {
 	struct v4l2_subdev *sd = i2c_get_clientdata(client);
 
-	v4l2_device_unregister_subdev(sd);
+	v4l2_async_unregister_subdev(sd);
 	v4l2_ctrl_handler_free(sd->ctrl_handler);
 	media_entity_cleanup(&sd->entity);
 
diff --git a/drivers/media/i2c/s5c73m3/s5c73m3-core.c b/drivers/media/i2c/s5c73m3/s5c73m3-core.c
index 08af58fb8e7d..3844853ab0a0 100644
--- a/drivers/media/i2c/s5c73m3/s5c73m3-core.c
+++ b/drivers/media/i2c/s5c73m3/s5c73m3-core.c
@@ -1706,7 +1706,7 @@ static int s5c73m3_probe(struct i2c_client *client,
 	state->oif_pads[OIF_ISP_PAD].flags = MEDIA_PAD_FL_SINK;
 	state->oif_pads[OIF_JPEG_PAD].flags = MEDIA_PAD_FL_SINK;
 	state->oif_pads[OIF_SOURCE_PAD].flags = MEDIA_PAD_FL_SOURCE;
-	oif_sd->entity.function = MEDIA_ENT_F_V4L2_SUBDEV_UNKNOWN;
+	oif_sd->entity.function = MEDIA_ENT_F_PROC_VIDEO_SCALER;
 
 	ret = media_entity_pads_init(&oif_sd->entity, OIF_NUM_PADS,
 							state->oif_pads);
diff --git a/drivers/media/i2c/s5k4ecgx.c b/drivers/media/i2c/s5k4ecgx.c
index 8a0f22da590f..6ebcf254989a 100644
--- a/drivers/media/i2c/s5k4ecgx.c
+++ b/drivers/media/i2c/s5k4ecgx.c
@@ -1019,7 +1019,6 @@ MODULE_DEVICE_TABLE(i2c, s5k4ecgx_id);
 
 static struct i2c_driver v4l2_i2c_driver = {
 	.driver = {
-		.owner	= THIS_MODULE,
 		.name = S5K4ECGX_DRIVER_NAME,
 	},
 	.probe = s5k4ecgx_probe,
diff --git a/drivers/media/i2c/s5k6a3.c b/drivers/media/i2c/s5k6a3.c
index cbe4711e9b31..769964057881 100644
--- a/drivers/media/i2c/s5k6a3.c
+++ b/drivers/media/i2c/s5k6a3.c
@@ -331,6 +331,7 @@ static int s5k6a3_probe(struct i2c_client *client,
 	sensor->format.width = S5K6A3_DEFAULT_WIDTH;
 	sensor->format.height = S5K6A3_DEFAULT_HEIGHT;
 
+	sd->entity.function = MEDIA_ENT_F_CAM_SENSOR;
 	sensor->pad.flags = MEDIA_PAD_FL_SOURCE;
 	ret = media_entity_pads_init(&sd->entity, 1, &sensor->pad);
 	if (ret < 0)
@@ -376,7 +377,6 @@ static struct i2c_driver s5k6a3_driver = {
 	.driver = {
 		.of_match_table	= of_match_ptr(s5k6a3_of_match),
 		.name		= S5K6A3_DRV_NAME,
-		.owner		= THIS_MODULE,
 	},
 	.probe		= s5k6a3_probe,
 	.remove		= s5k6a3_remove,
diff --git a/drivers/media/i2c/smiapp/smiapp-core.c b/drivers/media/i2c/smiapp/smiapp-core.c
index d08ab6c8357c..44f8c7e10a35 100644
--- a/drivers/media/i2c/smiapp/smiapp-core.c
+++ b/drivers/media/i2c/smiapp/smiapp-core.c
@@ -24,8 +24,8 @@
 #include <linux/delay.h>
 #include <linux/device.h>
 #include <linux/gpio.h>
+#include <linux/gpio/consumer.h>
 #include <linux/module.h>
-#include <linux/of_gpio.h>
 #include <linux/regulator/consumer.h>
 #include <linux/slab.h>
 #include <linux/smiapp.h>
@@ -328,6 +328,14 @@ static void __smiapp_update_exposure_limits(struct smiapp_sensor *sensor)
  *    orders must be defined.
  */
 static const struct smiapp_csi_data_format smiapp_csi_data_formats[] = {
+	{ MEDIA_BUS_FMT_SGRBG16_1X16, 16, 16, SMIAPP_PIXEL_ORDER_GRBG, },
+	{ MEDIA_BUS_FMT_SRGGB16_1X16, 16, 16, SMIAPP_PIXEL_ORDER_RGGB, },
+	{ MEDIA_BUS_FMT_SBGGR16_1X16, 16, 16, SMIAPP_PIXEL_ORDER_BGGR, },
+	{ MEDIA_BUS_FMT_SGBRG16_1X16, 16, 16, SMIAPP_PIXEL_ORDER_GBRG, },
+	{ MEDIA_BUS_FMT_SGRBG14_1X14, 14, 14, SMIAPP_PIXEL_ORDER_GRBG, },
+	{ MEDIA_BUS_FMT_SRGGB14_1X14, 14, 14, SMIAPP_PIXEL_ORDER_RGGB, },
+	{ MEDIA_BUS_FMT_SBGGR14_1X14, 14, 14, SMIAPP_PIXEL_ORDER_BGGR, },
+	{ MEDIA_BUS_FMT_SGBRG14_1X14, 14, 14, SMIAPP_PIXEL_ORDER_GBRG, },
 	{ MEDIA_BUS_FMT_SGRBG12_1X12, 12, 12, SMIAPP_PIXEL_ORDER_GRBG, },
 	{ MEDIA_BUS_FMT_SRGGB12_1X12, 12, 12, SMIAPP_PIXEL_ORDER_RGGB, },
 	{ MEDIA_BUS_FMT_SBGGR12_1X12, 12, 12, SMIAPP_PIXEL_ORDER_BGGR, },
@@ -625,12 +633,12 @@ static int smiapp_init_late_controls(struct smiapp_sensor *sensor)
 				0, max_value, 1, max_value);
 	}
 
-	for (max = 0; sensor->platform_data->op_sys_clock[max + 1]; max++);
+	for (max = 0; sensor->hwcfg->op_sys_clock[max + 1]; max++);
 
 	sensor->link_freq = v4l2_ctrl_new_int_menu(
 		&sensor->src->ctrl_handler, &smiapp_ctrl_ops,
 		V4L2_CID_LINK_FREQ, __fls(*valid_link_freqs),
-		__ffs(*valid_link_freqs), sensor->platform_data->op_sys_clock);
+		__ffs(*valid_link_freqs), sensor->hwcfg->op_sys_clock);
 
 	return sensor->src->ctrl_handler.error;
 }
@@ -833,8 +841,8 @@ static int smiapp_get_mbus_formats(struct smiapp_sensor *sensor)
 
 		pll->bits_per_pixel = f->compressed;
 
-		for (j = 0; sensor->platform_data->op_sys_clock[j]; j++) {
-			pll->link_freq = sensor->platform_data->op_sys_clock[j];
+		for (j = 0; sensor->hwcfg->op_sys_clock[j]; j++) {
+			pll->link_freq = sensor->hwcfg->op_sys_clock[j];
 
 			rval = smiapp_pll_try(sensor, pll);
 			dev_dbg(&client->dev, "link freq %u Hz, bpp %u %s\n",
@@ -1032,22 +1040,22 @@ static int smiapp_change_cci_addr(struct smiapp_sensor *sensor)
 	int rval;
 	u32 val;
 
-	client->addr = sensor->platform_data->i2c_addr_dfl;
+	client->addr = sensor->hwcfg->i2c_addr_dfl;
 
 	rval = smiapp_write(sensor,
 			    SMIAPP_REG_U8_CCI_ADDRESS_CONTROL,
-			    sensor->platform_data->i2c_addr_alt << 1);
+			    sensor->hwcfg->i2c_addr_alt << 1);
 	if (rval)
 		return rval;
 
-	client->addr = sensor->platform_data->i2c_addr_alt;
+	client->addr = sensor->hwcfg->i2c_addr_alt;
 
 	/* verify addr change went ok */
 	rval = smiapp_read(sensor, SMIAPP_REG_U8_CCI_ADDRESS_CONTROL, &val);
 	if (rval)
 		return rval;
 
-	if (val != sensor->platform_data->i2c_addr_alt << 1)
+	if (val != sensor->hwcfg->i2c_addr_alt << 1)
 		return -ENODEV;
 
 	return 0;
@@ -1061,13 +1069,13 @@ static int smiapp_change_cci_addr(struct smiapp_sensor *sensor)
 static int smiapp_setup_flash_strobe(struct smiapp_sensor *sensor)
 {
 	struct smiapp_flash_strobe_parms *strobe_setup;
-	unsigned int ext_freq = sensor->platform_data->ext_clk;
+	unsigned int ext_freq = sensor->hwcfg->ext_clk;
 	u32 tmp;
 	u32 strobe_adjustment;
 	u32 strobe_width_high_rs;
 	int rval;
 
-	strobe_setup = sensor->platform_data->strobe_setup;
+	strobe_setup = sensor->hwcfg->strobe_setup;
 
 	/*
 	 * How to calculate registers related to strobe length. Please
@@ -1179,7 +1187,7 @@ static int smiapp_setup_flash_strobe(struct smiapp_sensor *sensor)
 			    strobe_setup->trigger);
 
 out:
-	sensor->platform_data->strobe_setup->trigger = 0;
+	sensor->hwcfg->strobe_setup->trigger = 0;
 
 	return rval;
 }
@@ -1201,21 +1209,16 @@ static int smiapp_power_on(struct smiapp_sensor *sensor)
 	}
 	usleep_range(1000, 1000);
 
-	if (sensor->platform_data->set_xclk)
-		rval = sensor->platform_data->set_xclk(
-			&sensor->src->sd, sensor->platform_data->ext_clk);
-	else
-		rval = clk_prepare_enable(sensor->ext_clk);
+	rval = clk_prepare_enable(sensor->ext_clk);
 	if (rval < 0) {
 		dev_dbg(&client->dev, "failed to enable xclk\n");
 		goto out_xclk_fail;
 	}
 	usleep_range(1000, 1000);
 
-	if (gpio_is_valid(sensor->platform_data->xshutdown))
-		gpio_set_value(sensor->platform_data->xshutdown, 1);
+	gpiod_set_value(sensor->xshutdown, 1);
 
-	sleep = SMIAPP_RESET_DELAY(sensor->platform_data->ext_clk);
+	sleep = SMIAPP_RESET_DELAY(sensor->hwcfg->ext_clk);
 	usleep_range(sleep, sleep);
 
 	/*
@@ -1229,7 +1232,7 @@ static int smiapp_power_on(struct smiapp_sensor *sensor)
 	 * is found.
 	 */
 
-	if (sensor->platform_data->i2c_addr_alt) {
+	if (sensor->hwcfg->i2c_addr_alt) {
 		rval = smiapp_change_cci_addr(sensor);
 		if (rval) {
 			dev_err(&client->dev, "cci address change error\n");
@@ -1244,7 +1247,7 @@ static int smiapp_power_on(struct smiapp_sensor *sensor)
 		goto out_cci_addr_fail;
 	}
 
-	if (sensor->platform_data->i2c_addr_alt) {
+	if (sensor->hwcfg->i2c_addr_alt) {
 		rval = smiapp_change_cci_addr(sensor);
 		if (rval) {
 			dev_err(&client->dev, "cci address change error\n");
@@ -1261,14 +1264,14 @@ static int smiapp_power_on(struct smiapp_sensor *sensor)
 
 	rval = smiapp_write(
 		sensor, SMIAPP_REG_U16_EXTCLK_FREQUENCY_MHZ,
-		sensor->platform_data->ext_clk / (1000000 / (1 << 8)));
+		sensor->hwcfg->ext_clk / (1000000 / (1 << 8)));
 	if (rval) {
 		dev_err(&client->dev, "extclk frequency set failed\n");
 		goto out_cci_addr_fail;
 	}
 
 	rval = smiapp_write(sensor, SMIAPP_REG_U8_CSI_LANE_MODE,
-			    sensor->platform_data->lanes - 1);
+			    sensor->hwcfg->lanes - 1);
 	if (rval) {
 		dev_err(&client->dev, "csi lane mode set failed\n");
 		goto out_cci_addr_fail;
@@ -1282,7 +1285,7 @@ static int smiapp_power_on(struct smiapp_sensor *sensor)
 	}
 
 	rval = smiapp_write(sensor, SMIAPP_REG_U8_CSI_SIGNALLING_MODE,
-			    sensor->platform_data->csi_signalling_mode);
+			    sensor->hwcfg->csi_signalling_mode);
 	if (rval) {
 		dev_err(&client->dev, "csi signalling mode set failed\n");
 		goto out_cci_addr_fail;
@@ -1322,12 +1325,8 @@ static int smiapp_power_on(struct smiapp_sensor *sensor)
 	return 0;
 
 out_cci_addr_fail:
-	if (gpio_is_valid(sensor->platform_data->xshutdown))
-		gpio_set_value(sensor->platform_data->xshutdown, 0);
-	if (sensor->platform_data->set_xclk)
-		sensor->platform_data->set_xclk(&sensor->src->sd, 0);
-	else
-		clk_disable_unprepare(sensor->ext_clk);
+	gpiod_set_value(sensor->xshutdown, 0);
+	clk_disable_unprepare(sensor->ext_clk);
 
 out_xclk_fail:
 	regulator_disable(sensor->vana);
@@ -1343,17 +1342,13 @@ static void smiapp_power_off(struct smiapp_sensor *sensor)
 	 * really see a power off and next time the cci address change
 	 * will fail. So do a soft reset explicitly here.
 	 */
-	if (sensor->platform_data->i2c_addr_alt)
+	if (sensor->hwcfg->i2c_addr_alt)
 		smiapp_write(sensor,
 			     SMIAPP_REG_U8_SOFTWARE_RESET,
 			     SMIAPP_SOFTWARE_RESET);
 
-	if (gpio_is_valid(sensor->platform_data->xshutdown))
-		gpio_set_value(sensor->platform_data->xshutdown, 0);
-	if (sensor->platform_data->set_xclk)
-		sensor->platform_data->set_xclk(&sensor->src->sd, 0);
-	else
-		clk_disable_unprepare(sensor->ext_clk);
+	gpiod_set_value(sensor->xshutdown, 0);
+	clk_disable_unprepare(sensor->ext_clk);
 	usleep_range(5000, 5000);
 	regulator_disable(sensor->vana);
 	sensor->streaming = false;
@@ -1491,8 +1486,8 @@ static int smiapp_start_streaming(struct smiapp_sensor *sensor)
 	if ((sensor->limits[SMIAPP_LIMIT_FLASH_MODE_CAPABILITY] &
 	     (SMIAPP_FLASH_MODE_CAPABILITY_SINGLE_STROBE |
 	      SMIAPP_FLASH_MODE_CAPABILITY_MULTIPLE_STROBE)) &&
-	    sensor->platform_data->strobe_setup != NULL &&
-	    sensor->platform_data->strobe_setup->trigger != 0) {
+	    sensor->hwcfg->strobe_setup != NULL &&
+	    sensor->hwcfg->strobe_setup->trigger != 0) {
 		rval = smiapp_setup_flash_strobe(sensor);
 		if (rval)
 			goto out;
@@ -2309,7 +2304,7 @@ smiapp_sysfs_nvm_read(struct device *dev, struct device_attribute *attr,
 
 	if (!sensor->nvm_size) {
 		/* NVM not read yet - read it now */
-		sensor->nvm_size = sensor->platform_data->nvm_size;
+		sensor->nvm_size = sensor->hwcfg->nvm_size;
 		if (smiapp_set_power(subdev, 1) < 0)
 			return -ENODEV;
 		if (smiapp_read_nvm(sensor, sensor->nvm)) {
@@ -2554,35 +2549,27 @@ static int smiapp_init(struct smiapp_sensor *sensor)
 		return PTR_ERR(sensor->vana);
 	}
 
-	if (!sensor->platform_data->set_xclk) {
-		sensor->ext_clk = devm_clk_get(&client->dev, NULL);
-		if (IS_ERR(sensor->ext_clk)) {
-			dev_err(&client->dev, "could not get clock\n");
-			return PTR_ERR(sensor->ext_clk);
-		}
-
-		rval = clk_set_rate(sensor->ext_clk,
-				    sensor->platform_data->ext_clk);
-		if (rval < 0) {
-			dev_err(&client->dev,
-				"unable to set clock freq to %u\n",
-				sensor->platform_data->ext_clk);
-			return rval;
-		}
+	sensor->ext_clk = devm_clk_get(&client->dev, NULL);
+	if (IS_ERR(sensor->ext_clk)) {
+		dev_err(&client->dev, "could not get clock (%ld)\n",
+			PTR_ERR(sensor->ext_clk));
+		return -EPROBE_DEFER;
 	}
 
-	if (gpio_is_valid(sensor->platform_data->xshutdown)) {
-		rval = devm_gpio_request_one(
-			&client->dev, sensor->platform_data->xshutdown, 0,
-			"SMIA++ xshutdown");
-		if (rval < 0) {
-			dev_err(&client->dev,
-				"unable to acquire reset gpio %d\n",
-				sensor->platform_data->xshutdown);
-			return rval;
-		}
+	rval = clk_set_rate(sensor->ext_clk,
+			    sensor->hwcfg->ext_clk);
+	if (rval < 0) {
+		dev_err(&client->dev,
+			"unable to set clock freq to %u\n",
+			sensor->hwcfg->ext_clk);
+		return rval;
 	}
 
+	sensor->xshutdown = devm_gpiod_get_optional(&client->dev, "xshutdown",
+						    GPIOD_OUT_LOW);
+	if (IS_ERR(sensor->xshutdown))
+		return PTR_ERR(sensor->xshutdown);
+
 	rval = smiapp_power_on(sensor);
 	if (rval)
 		return -ENODEV;
@@ -2612,7 +2599,7 @@ static int smiapp_init(struct smiapp_sensor *sensor)
 	 *
 	 * Rotation also changes the bayer pattern.
 	 */
-	if (sensor->platform_data->module_board_orient ==
+	if (sensor->hwcfg->module_board_orient ==
 	    SMIAPP_MODULE_BOARD_ORIENT_180)
 		sensor->hvflip_inv_mask = SMIAPP_IMAGE_ORIENTATION_HFLIP |
 					  SMIAPP_IMAGE_ORIENTATION_VFLIP;
@@ -2661,9 +2648,9 @@ static int smiapp_init(struct smiapp_sensor *sensor)
 	/* SMIA++ NVM initialization - it will be read from the sensor
 	 * when it is first requested by userspace.
 	 */
-	if (sensor->minfo.smiapp_version && sensor->platform_data->nvm_size) {
+	if (sensor->minfo.smiapp_version && sensor->hwcfg->nvm_size) {
 		sensor->nvm = devm_kzalloc(&client->dev,
-				sensor->platform_data->nvm_size, GFP_KERNEL);
+				sensor->hwcfg->nvm_size, GFP_KERNEL);
 		if (sensor->nvm == NULL) {
 			dev_err(&client->dev, "nvm buf allocation failed\n");
 			rval = -ENOMEM;
@@ -2706,8 +2693,8 @@ static int smiapp_init(struct smiapp_sensor *sensor)
 
 	/* prepare PLL configuration input values */
 	pll->bus_type = SMIAPP_PLL_BUS_TYPE_CSI2;
-	pll->csi2.lanes = sensor->platform_data->lanes;
-	pll->ext_clk_freq_hz = sensor->platform_data->ext_clk;
+	pll->csi2.lanes = sensor->hwcfg->lanes;
+	pll->ext_clk_freq_hz = sensor->hwcfg->ext_clk;
 	pll->scale_n = sensor->limits[SMIAPP_LIMIT_SCALER_N_MIN];
 	/* Profile 0 sensors have no separate OP clock branch. */
 	if (sensor->minfo.smiapp_profile == SMIAPP_PROFILE_0)
@@ -2984,9 +2971,9 @@ static int smiapp_resume(struct device *dev)
 
 #endif /* CONFIG_PM */
 
-static struct smiapp_platform_data *smiapp_get_pdata(struct device *dev)
+static struct smiapp_hwconfig *smiapp_get_hwconfig(struct device *dev)
 {
-	struct smiapp_platform_data *pdata;
+	struct smiapp_hwconfig *hwcfg;
 	struct v4l2_of_endpoint *bus_cfg;
 	struct device_node *ep;
 	int i;
@@ -3003,58 +2990,55 @@ static struct smiapp_platform_data *smiapp_get_pdata(struct device *dev)
 	if (IS_ERR(bus_cfg))
 		goto out_err;
 
-	pdata = devm_kzalloc(dev, sizeof(*pdata), GFP_KERNEL);
-	if (!pdata)
+	hwcfg = devm_kzalloc(dev, sizeof(*hwcfg), GFP_KERNEL);
+	if (!hwcfg)
 		goto out_err;
 
 	switch (bus_cfg->bus_type) {
 	case V4L2_MBUS_CSI2:
-		pdata->csi_signalling_mode = SMIAPP_CSI_SIGNALLING_MODE_CSI2;
+		hwcfg->csi_signalling_mode = SMIAPP_CSI_SIGNALLING_MODE_CSI2;
 		break;
 		/* FIXME: add CCP2 support. */
 	default:
 		goto out_err;
 	}
 
-	pdata->lanes = bus_cfg->bus.mipi_csi2.num_data_lanes;
-	dev_dbg(dev, "lanes %u\n", pdata->lanes);
-
-	/* xshutdown GPIO is optional */
-	pdata->xshutdown = of_get_named_gpio(dev->of_node, "reset-gpios", 0);
+	hwcfg->lanes = bus_cfg->bus.mipi_csi2.num_data_lanes;
+	dev_dbg(dev, "lanes %u\n", hwcfg->lanes);
 
 	/* NVM size is not mandatory */
 	of_property_read_u32(dev->of_node, "nokia,nvm-size",
-				    &pdata->nvm_size);
+				    &hwcfg->nvm_size);
 
 	rval = of_property_read_u32(dev->of_node, "clock-frequency",
-				    &pdata->ext_clk);
+				    &hwcfg->ext_clk);
 	if (rval) {
 		dev_warn(dev, "can't get clock-frequency\n");
 		goto out_err;
 	}
 
-	dev_dbg(dev, "reset %d, nvm %d, clk %d, csi %d\n", pdata->xshutdown,
-		pdata->nvm_size, pdata->ext_clk, pdata->csi_signalling_mode);
+	dev_dbg(dev, "nvm %d, clk %d, csi %d\n", hwcfg->nvm_size,
+		hwcfg->ext_clk, hwcfg->csi_signalling_mode);
 
 	if (!bus_cfg->nr_of_link_frequencies) {
 		dev_warn(dev, "no link frequencies defined\n");
 		goto out_err;
 	}
 
-	pdata->op_sys_clock = devm_kcalloc(
+	hwcfg->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)
+		sizeof(*hwcfg->op_sys_clock), GFP_KERNEL);
+	if (!hwcfg->op_sys_clock)
 		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]);
+		hwcfg->op_sys_clock[i] = bus_cfg->link_frequencies[i];
+		dev_dbg(dev, "freq %d: %lld\n", i, hwcfg->op_sys_clock[i]);
 	}
 
 	v4l2_of_free_endpoint(bus_cfg);
 	of_node_put(ep);
-	return pdata;
+	return hwcfg;
 
 out_err:
 	v4l2_of_free_endpoint(bus_cfg);
@@ -3066,17 +3050,17 @@ static int smiapp_probe(struct i2c_client *client,
 			const struct i2c_device_id *devid)
 {
 	struct smiapp_sensor *sensor;
-	struct smiapp_platform_data *pdata = smiapp_get_pdata(&client->dev);
+	struct smiapp_hwconfig *hwcfg = smiapp_get_hwconfig(&client->dev);
 	int rval;
 
-	if (pdata == NULL)
+	if (hwcfg == NULL)
 		return -ENODEV;
 
 	sensor = devm_kzalloc(&client->dev, sizeof(*sensor), GFP_KERNEL);
 	if (sensor == NULL)
 		return -ENOMEM;
 
-	sensor->platform_data = pdata;
+	sensor->hwcfg = hwcfg;
 	mutex_init(&sensor->mutex);
 	mutex_init(&sensor->power_mutex);
 	sensor->src = &sensor->ssds[sensor->ssds_used];
@@ -3119,12 +3103,8 @@ static int smiapp_remove(struct i2c_client *client)
 	v4l2_async_unregister_subdev(subdev);
 
 	if (sensor->power_count) {
-		if (gpio_is_valid(sensor->platform_data->xshutdown))
-			gpio_set_value(sensor->platform_data->xshutdown, 0);
-		if (sensor->platform_data->set_xclk)
-			sensor->platform_data->set_xclk(&sensor->src->sd, 0);
-		else
-			clk_disable_unprepare(sensor->ext_clk);
+		gpiod_set_value(sensor->xshutdown, 0);
+		clk_disable_unprepare(sensor->ext_clk);
 		sensor->power_count = 0;
 	}
 
diff --git a/drivers/media/i2c/smiapp/smiapp-quirk.c b/drivers/media/i2c/smiapp/smiapp-quirk.c
index abf9ea7a0fb7..cb128eae9c54 100644
--- a/drivers/media/i2c/smiapp/smiapp-quirk.c
+++ b/drivers/media/i2c/smiapp/smiapp-quirk.c
@@ -26,7 +26,7 @@ static int smiapp_write_8(struct smiapp_sensor *sensor, u16 reg, u8 val)
 }
 
 static int smiapp_write_8s(struct smiapp_sensor *sensor,
-			   struct smiapp_reg_8 *regs, int len)
+			   const struct smiapp_reg_8 *regs, int len)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
 	int rval;
@@ -71,7 +71,7 @@ static int jt8ew9_limits(struct smiapp_sensor *sensor)
 
 static int jt8ew9_post_poweron(struct smiapp_sensor *sensor)
 {
-	struct smiapp_reg_8 regs[] = {
+	const struct smiapp_reg_8 regs[] = {
 		{ 0x30a3, 0xd8 }, /* Output port control : LVDS ports only */
 		{ 0x30ae, 0x00 }, /* 0x0307 pll_multiplier maximum value on PLL input 9.6MHz ( 19.2MHz is divided on pre_pll_div) */
 		{ 0x30af, 0xd0 }, /* 0x0307 pll_multiplier maximum value on PLL input 9.6MHz ( 19.2MHz is divided on pre_pll_div) */
@@ -115,7 +115,7 @@ const struct smiapp_quirk smiapp_jt8ew9_quirk = {
 static int imx125es_post_poweron(struct smiapp_sensor *sensor)
 {
 	/* Taken from v02. No idea what the other two are. */
-	struct smiapp_reg_8 regs[] = {
+	const struct smiapp_reg_8 regs[] = {
 		/*
 		 * 0x3302: clk during frame blanking:
 		 * 0x00 - HS mode, 0x01 - LP11
@@ -145,8 +145,7 @@ static int jt8ev1_post_poweron(struct smiapp_sensor *sensor)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
 	int rval;
-
-	struct smiapp_reg_8 regs[] = {
+	const struct smiapp_reg_8 regs[] = {
 		{ 0x3031, 0xcd }, /* For digital binning (EQ_MONI) */
 		{ 0x30a3, 0xd0 }, /* FLASH STROBE enable */
 		{ 0x3237, 0x00 }, /* For control of pulse timing for ADC */
@@ -167,8 +166,7 @@ static int jt8ev1_post_poweron(struct smiapp_sensor *sensor)
 		{ 0x33cf, 0xec }, /* For Black sun */
 		{ 0x3328, 0x80 }, /* Ugh. No idea what's this. */
 	};
-
-	struct smiapp_reg_8 regs_96[] = {
+	const struct smiapp_reg_8 regs_96[] = {
 		{ 0x30ae, 0x00 }, /* For control of ADC clock */
 		{ 0x30af, 0xd0 },
 		{ 0x30b0, 0x01 },
@@ -178,13 +176,13 @@ static int jt8ev1_post_poweron(struct smiapp_sensor *sensor)
 	if (rval < 0)
 		return rval;
 
-	switch (sensor->platform_data->ext_clk) {
+	switch (sensor->hwcfg->ext_clk) {
 	case 9600000:
 		return smiapp_write_8s(sensor, regs_96,
 				       ARRAY_SIZE(regs_96));
 	default:
 		dev_warn(&client->dev, "no MSRs for %d Hz ext_clk\n",
-			 sensor->platform_data->ext_clk);
+			 sensor->hwcfg->ext_clk);
 		return 0;
 	}
 }
diff --git a/drivers/media/i2c/smiapp/smiapp-regs.c b/drivers/media/i2c/smiapp/smiapp-regs.c
index 6b6c20b61397..1e501c06d18c 100644
--- a/drivers/media/i2c/smiapp/smiapp-regs.c
+++ b/drivers/media/i2c/smiapp/smiapp-regs.c
@@ -188,7 +188,8 @@ int smiapp_read_no_quirk(struct smiapp_sensor *sensor, u32 reg, u32 *val)
 				   SMIAPP_QUIRK_FLAG_8BIT_READ_ONLY));
 }
 
-int smiapp_read(struct smiapp_sensor *sensor, u32 reg, u32 *val)
+static int smiapp_read_quirk(struct smiapp_sensor *sensor, u32 reg, u32 *val,
+			     bool force8)
 {
 	int rval;
 
@@ -199,21 +200,20 @@ int smiapp_read(struct smiapp_sensor *sensor, u32 reg, u32 *val)
 	if (rval < 0)
 		return rval;
 
+	if (force8)
+		return __smiapp_read(sensor, reg, val, true);
+
 	return smiapp_read_no_quirk(sensor, reg, val);
 }
 
-int smiapp_read_8only(struct smiapp_sensor *sensor, u32 reg, u32 *val)
+int smiapp_read(struct smiapp_sensor *sensor, u32 reg, u32 *val)
 {
-	int rval;
-
-	*val = 0;
-	rval = smiapp_call_quirk(sensor, reg_access, false, &reg, val);
-	if (rval == -ENOIOCTLCMD)
-		return 0;
-	if (rval < 0)
-		return rval;
+	return smiapp_read_quirk(sensor, reg, val, false);
+}
 
-	return __smiapp_read(sensor, reg, val, true);
+int smiapp_read_8only(struct smiapp_sensor *sensor, u32 reg, u32 *val)
+{
+	return smiapp_read_quirk(sensor, reg, val, true);
 }
 
 int smiapp_write_no_quirk(struct smiapp_sensor *sensor, u32 reg, u32 val)
diff --git a/drivers/media/i2c/smiapp/smiapp.h b/drivers/media/i2c/smiapp/smiapp.h
index 2174f89a00db..aae72bc87bf7 100644
--- a/drivers/media/i2c/smiapp/smiapp.h
+++ b/drivers/media/i2c/smiapp/smiapp.h
@@ -151,7 +151,7 @@ struct smiapp_csi_data_format {
 #define SMIAPP_PADS			2
 
 #define SMIAPP_COMPRESSED_BASE		8
-#define SMIAPP_COMPRESSED_MAX		12
+#define SMIAPP_COMPRESSED_MAX		16
 #define SMIAPP_NR_OF_COMPRESSED		(SMIAPP_COMPRESSED_MAX - \
 					 SMIAPP_COMPRESSED_BASE + 1)
 
@@ -197,9 +197,10 @@ struct smiapp_sensor {
 	struct smiapp_subdev *binner;
 	struct smiapp_subdev *scaler;
 	struct smiapp_subdev *pixel_array;
-	struct smiapp_platform_data *platform_data;
+	struct smiapp_hwconfig *hwcfg;
 	struct regulator *vana;
 	struct clk *ext_clk;
+	struct gpio_desc *xshutdown;
 	u32 limits[SMIAPP_LIMIT_LAST];
 	u8 nbinning_subtypes;
 	struct smiapp_binning_subtype binning_subtypes[SMIAPP_BINNING_SUBTYPES];
diff --git a/drivers/media/i2c/soc_camera/Kconfig b/drivers/media/i2c/soc_camera/Kconfig
index 23d352f0adf0..7704bcf5cc25 100644
--- a/drivers/media/i2c/soc_camera/Kconfig
+++ b/drivers/media/i2c/soc_camera/Kconfig
@@ -14,11 +14,14 @@ config SOC_CAMERA_MT9M001
 	  and colour models.
 
 config SOC_CAMERA_MT9M111
-	tristate "mt9m111, mt9m112 and mt9m131 support"
+	tristate "legacy soc_camera mt9m111, mt9m112 and mt9m131 support"
 	depends on SOC_CAMERA && I2C
+	select VIDEO_MT9M111
 	help
 	  This driver supports MT9M111, MT9M112 and MT9M131 cameras from
-	  Micron/Aptina
+	  Micron/Aptina.
+	  This is the legacy configuration which shouldn't be used anymore,
+	  while VIDEO_MT9M111 should be used instead.
 
 config SOC_CAMERA_MT9T031
 	tristate "mt9t031 support"
diff --git a/drivers/media/i2c/soc_camera/Makefile b/drivers/media/i2c/soc_camera/Makefile
index d0421feaa796..6f994f9353a0 100644
--- a/drivers/media/i2c/soc_camera/Makefile
+++ b/drivers/media/i2c/soc_camera/Makefile
@@ -1,6 +1,5 @@
 obj-$(CONFIG_SOC_CAMERA_IMX074)		+= imx074.o
 obj-$(CONFIG_SOC_CAMERA_MT9M001)	+= mt9m001.o
-obj-$(CONFIG_SOC_CAMERA_MT9M111)	+= mt9m111.o
 obj-$(CONFIG_SOC_CAMERA_MT9T031)	+= mt9t031.o
 obj-$(CONFIG_SOC_CAMERA_MT9T112)	+= mt9t112.o
 obj-$(CONFIG_SOC_CAMERA_MT9V022)	+= mt9v022.o
diff --git a/drivers/media/i2c/soc_camera/imx074.c b/drivers/media/i2c/soc_camera/imx074.c
index f68c2352c63c..05b55cfe8147 100644
--- a/drivers/media/i2c/soc_camera/imx074.c
+++ b/drivers/media/i2c/soc_camera/imx074.c
@@ -209,31 +209,26 @@ static int imx074_get_fmt(struct v4l2_subdev *sd,
 	return 0;
 }
 
-static int imx074_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
+static int imx074_get_selection(struct v4l2_subdev *sd,
+				struct v4l2_subdev_pad_config *cfg,
+				struct v4l2_subdev_selection *sel)
 {
-	struct v4l2_rect *rect = &a->c;
-
-	a->type		= V4L2_BUF_TYPE_VIDEO_CAPTURE;
-	rect->top	= 0;
-	rect->left	= 0;
-	rect->width	= IMX074_WIDTH;
-	rect->height	= IMX074_HEIGHT;
-
-	return 0;
-}
+	if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE)
+		return -EINVAL;
 
-static int imx074_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
-{
-	a->bounds.left			= 0;
-	a->bounds.top			= 0;
-	a->bounds.width			= IMX074_WIDTH;
-	a->bounds.height		= IMX074_HEIGHT;
-	a->defrect			= a->bounds;
-	a->type				= V4L2_BUF_TYPE_VIDEO_CAPTURE;
-	a->pixelaspect.numerator	= 1;
-	a->pixelaspect.denominator	= 1;
+	sel->r.left = 0;
+	sel->r.top = 0;
+	sel->r.width = IMX074_WIDTH;
+	sel->r.height = IMX074_HEIGHT;
 
-	return 0;
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+	case V4L2_SEL_TGT_CROP:
+		return 0;
+	default:
+		return -EINVAL;
+	}
 }
 
 static int imx074_enum_mbus_code(struct v4l2_subdev *sd,
@@ -278,8 +273,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,
-	.g_crop		= imx074_g_crop,
-	.cropcap	= imx074_cropcap,
 	.g_mbus_config	= imx074_g_mbus_config,
 };
 
@@ -289,6 +282,7 @@ static struct v4l2_subdev_core_ops imx074_subdev_core_ops = {
 
 static const struct v4l2_subdev_pad_ops imx074_subdev_pad_ops = {
 	.enum_mbus_code = imx074_enum_mbus_code,
+	.get_selection	= imx074_get_selection,
 	.get_fmt	= imx074_get_fmt,
 	.set_fmt	= imx074_set_fmt,
 };
diff --git a/drivers/media/i2c/soc_camera/mt9m001.c b/drivers/media/i2c/soc_camera/mt9m001.c
index 69becc358659..3d6378d4491c 100644
--- a/drivers/media/i2c/soc_camera/mt9m001.c
+++ b/drivers/media/i2c/soc_camera/mt9m001.c
@@ -171,13 +171,19 @@ static int mt9m001_s_stream(struct v4l2_subdev *sd, int enable)
 	return 0;
 }
 
-static int mt9m001_s_crop(struct v4l2_subdev *sd, const struct v4l2_crop *a)
+static int mt9m001_set_selection(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_selection *sel)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct mt9m001 *mt9m001 = to_mt9m001(client);
-	struct v4l2_rect rect = a->c;
-	int ret;
+	struct v4l2_rect rect = sel->r;
 	const u16 hblank = 9, vblank = 25;
+	int ret;
+
+	if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE ||
+	    sel->target != V4L2_SEL_TGT_CROP)
+		return -EINVAL;
 
 	if (mt9m001->fmts == mt9m001_colour_fmts)
 		/*
@@ -225,29 +231,30 @@ static int mt9m001_s_crop(struct v4l2_subdev *sd, const struct v4l2_crop *a)
 	return ret;
 }
 
-static int mt9m001_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
+static int mt9m001_get_selection(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_selection *sel)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct mt9m001 *mt9m001 = to_mt9m001(client);
 
-	a->c	= mt9m001->rect;
-	a->type	= V4L2_BUF_TYPE_VIDEO_CAPTURE;
-
-	return 0;
-}
-
-static int mt9m001_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
-{
-	a->bounds.left			= MT9M001_COLUMN_SKIP;
-	a->bounds.top			= MT9M001_ROW_SKIP;
-	a->bounds.width			= MT9M001_MAX_WIDTH;
-	a->bounds.height		= MT9M001_MAX_HEIGHT;
-	a->defrect			= a->bounds;
-	a->type				= V4L2_BUF_TYPE_VIDEO_CAPTURE;
-	a->pixelaspect.numerator	= 1;
-	a->pixelaspect.denominator	= 1;
+	if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE)
+		return -EINVAL;
 
-	return 0;
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+		sel->r.left = MT9M001_COLUMN_SKIP;
+		sel->r.top = MT9M001_ROW_SKIP;
+		sel->r.width = MT9M001_MAX_WIDTH;
+		sel->r.height = MT9M001_MAX_HEIGHT;
+		return 0;
+	case V4L2_SEL_TGT_CROP:
+		sel->r = mt9m001->rect;
+		return 0;
+	default:
+		return -EINVAL;
+	}
 }
 
 static int mt9m001_get_fmt(struct v4l2_subdev *sd,
@@ -275,18 +282,18 @@ static int mt9m001_s_fmt(struct v4l2_subdev *sd,
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct mt9m001 *mt9m001 = to_mt9m001(client);
-	struct v4l2_crop a = {
-		.c = {
-			.left	= mt9m001->rect.left,
-			.top	= mt9m001->rect.top,
-			.width	= mf->width,
-			.height	= mf->height,
-		},
+	struct v4l2_subdev_selection sel = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		.target = V4L2_SEL_TGT_CROP,
+		.r.left = mt9m001->rect.left,
+		.r.top = mt9m001->rect.top,
+		.r.width = mf->width,
+		.r.height = mf->height,
 	};
 	int ret;
 
 	/* No support for scaling so far, just crop. TODO: use skipping */
-	ret = mt9m001_s_crop(sd, &a);
+	ret = mt9m001_set_selection(sd, NULL, &sel);
 	if (!ret) {
 		mf->width	= mt9m001->rect.width;
 		mf->height	= mt9m001->rect.height;
@@ -625,9 +632,6 @@ 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_crop		= mt9m001_s_crop,
-	.g_crop		= mt9m001_g_crop,
-	.cropcap	= mt9m001_cropcap,
 	.g_mbus_config	= mt9m001_g_mbus_config,
 	.s_mbus_config	= mt9m001_s_mbus_config,
 };
@@ -638,6 +642,8 @@ static const struct v4l2_subdev_sensor_ops mt9m001_subdev_sensor_ops = {
 
 static const struct v4l2_subdev_pad_ops mt9m001_subdev_pad_ops = {
 	.enum_mbus_code = mt9m001_enum_mbus_code,
+	.get_selection	= mt9m001_get_selection,
+	.set_selection	= mt9m001_set_selection,
 	.get_fmt	= mt9m001_get_fmt,
 	.set_fmt	= mt9m001_set_fmt,
 };
diff --git a/drivers/media/i2c/soc_camera/mt9t031.c b/drivers/media/i2c/soc_camera/mt9t031.c
index 5c8e3ffe3b27..3aa5569065ad 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
-	 * .set_fmt(FORMAT_TRY), soc_camera_s_crop() and soc_camera_cropcap()
+	 * .set_fmt(FORMAT_TRY), soc_camera_s_selection() and soc_camera_cropcap()
 	 */
 	if (ret >= 0)
 		ret = reg_write(client, MT9T031_COLUMN_START, rect->left);
@@ -294,11 +294,17 @@ static int mt9t031_set_params(struct i2c_client *client,
 	return ret < 0 ? ret : 0;
 }
 
-static int mt9t031_s_crop(struct v4l2_subdev *sd, const struct v4l2_crop *a)
+static int mt9t031_set_selection(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_selection *sel)
 {
-	struct v4l2_rect rect = a->c;
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct mt9t031 *mt9t031 = to_mt9t031(client);
+	struct v4l2_rect rect = sel->r;
+
+	if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE ||
+	    sel->target != V4L2_SEL_TGT_CROP)
+		return -EINVAL;
 
 	rect.width = ALIGN(rect.width, 2);
 	rect.height = ALIGN(rect.height, 2);
@@ -312,29 +318,30 @@ static int mt9t031_s_crop(struct v4l2_subdev *sd, const struct v4l2_crop *a)
 	return mt9t031_set_params(client, &rect, mt9t031->xskip, mt9t031->yskip);
 }
 
-static int mt9t031_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
+static int mt9t031_get_selection(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_selection *sel)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct mt9t031 *mt9t031 = to_mt9t031(client);
 
-	a->c	= mt9t031->rect;
-	a->type	= V4L2_BUF_TYPE_VIDEO_CAPTURE;
-
-	return 0;
-}
-
-static int mt9t031_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
-{
-	a->bounds.left			= MT9T031_COLUMN_SKIP;
-	a->bounds.top			= MT9T031_ROW_SKIP;
-	a->bounds.width			= MT9T031_MAX_WIDTH;
-	a->bounds.height		= MT9T031_MAX_HEIGHT;
-	a->defrect			= a->bounds;
-	a->type				= V4L2_BUF_TYPE_VIDEO_CAPTURE;
-	a->pixelaspect.numerator	= 1;
-	a->pixelaspect.denominator	= 1;
+	if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE)
+		return -EINVAL;
 
-	return 0;
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+		sel->r.left = MT9T031_COLUMN_SKIP;
+		sel->r.top = MT9T031_ROW_SKIP;
+		sel->r.width = MT9T031_MAX_WIDTH;
+		sel->r.height = MT9T031_MAX_HEIGHT;
+		return 0;
+	case V4L2_SEL_TGT_CROP:
+		sel->r = mt9t031->rect;
+		return 0;
+	default:
+		return -EINVAL;
+	}
 }
 
 static int mt9t031_get_fmt(struct v4l2_subdev *sd,
@@ -721,9 +728,6 @@ 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_crop		= mt9t031_s_crop,
-	.g_crop		= mt9t031_g_crop,
-	.cropcap	= mt9t031_cropcap,
 	.g_mbus_config	= mt9t031_g_mbus_config,
 	.s_mbus_config	= mt9t031_s_mbus_config,
 };
@@ -734,6 +738,8 @@ static const struct v4l2_subdev_sensor_ops mt9t031_subdev_sensor_ops = {
 
 static const struct v4l2_subdev_pad_ops mt9t031_subdev_pad_ops = {
 	.enum_mbus_code = mt9t031_enum_mbus_code,
+	.get_selection	= mt9t031_get_selection,
+	.set_selection	= mt9t031_set_selection,
 	.get_fmt	= mt9t031_get_fmt,
 	.set_fmt	= mt9t031_set_fmt,
 };
diff --git a/drivers/media/i2c/soc_camera/mt9t112.c b/drivers/media/i2c/soc_camera/mt9t112.c
index 6a1b2a9f9a09..2ef22241ec14 100644
--- a/drivers/media/i2c/soc_camera/mt9t112.c
+++ b/drivers/media/i2c/soc_camera/mt9t112.c
@@ -867,39 +867,48 @@ static int mt9t112_set_params(struct mt9t112_priv *priv,
 	return 0;
 }
 
-static int mt9t112_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
-{
-	a->bounds.left			= 0;
-	a->bounds.top			= 0;
-	a->bounds.width			= MAX_WIDTH;
-	a->bounds.height		= MAX_HEIGHT;
-	a->defrect.left			= 0;
-	a->defrect.top			= 0;
-	a->defrect.width		= VGA_WIDTH;
-	a->defrect.height		= VGA_HEIGHT;
-	a->type				= V4L2_BUF_TYPE_VIDEO_CAPTURE;
-	a->pixelaspect.numerator	= 1;
-	a->pixelaspect.denominator	= 1;
-
-	return 0;
-}
-
-static int mt9t112_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
+static int mt9t112_get_selection(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_selection *sel)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct mt9t112_priv *priv = to_mt9t112(client);
 
-	a->c	= priv->frame;
-	a->type	= V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE)
+		return -EINVAL;
 
-	return 0;
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		sel->r.left = 0;
+		sel->r.top = 0;
+		sel->r.width = MAX_WIDTH;
+		sel->r.height = MAX_HEIGHT;
+		return 0;
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+		sel->r.left = 0;
+		sel->r.top = 0;
+		sel->r.width = VGA_WIDTH;
+		sel->r.height = VGA_HEIGHT;
+		return 0;
+	case V4L2_SEL_TGT_CROP:
+		sel->r = priv->frame;
+		return 0;
+	default:
+		return -EINVAL;
+	}
 }
 
-static int mt9t112_s_crop(struct v4l2_subdev *sd, const struct v4l2_crop *a)
+static int mt9t112_set_selection(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_selection *sel)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct mt9t112_priv *priv = to_mt9t112(client);
-	const struct v4l2_rect *rect = &a->c;
+	const struct v4l2_rect *rect = &sel->r;
+
+	if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE ||
+	    sel->target != V4L2_SEL_TGT_CROP)
+		return -EINVAL;
 
 	return mt9t112_set_params(priv, rect, priv->format->code);
 }
@@ -1024,15 +1033,14 @@ 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,
-	.cropcap	= mt9t112_cropcap,
-	.g_crop		= mt9t112_g_crop,
-	.s_crop		= mt9t112_s_crop,
 	.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_selection	= mt9t112_get_selection,
+	.set_selection	= mt9t112_set_selection,
 	.get_fmt	= mt9t112_get_fmt,
 	.set_fmt	= mt9t112_set_fmt,
 };
diff --git a/drivers/media/i2c/soc_camera/mt9v022.c b/drivers/media/i2c/soc_camera/mt9v022.c
index 2721e583bfa0..6a14ab5e4f2d 100644
--- a/drivers/media/i2c/soc_camera/mt9v022.c
+++ b/drivers/media/i2c/soc_camera/mt9v022.c
@@ -276,14 +276,20 @@ static int mt9v022_s_stream(struct v4l2_subdev *sd, int enable)
 	return 0;
 }
 
-static int mt9v022_s_crop(struct v4l2_subdev *sd, const struct v4l2_crop *a)
+static int mt9v022_set_selection(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_selection *sel)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct mt9v022 *mt9v022 = to_mt9v022(client);
-	struct v4l2_rect rect = a->c;
+	struct v4l2_rect rect = sel->r;
 	int min_row, min_blank;
 	int ret;
 
+	if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE ||
+	    sel->target != V4L2_SEL_TGT_CROP)
+		return -EINVAL;
+
 	/* Bayer format - even size lengths */
 	if (mt9v022->fmts == mt9v022_colour_fmts) {
 		rect.width	= ALIGN(rect.width, 2);
@@ -350,29 +356,30 @@ static int mt9v022_s_crop(struct v4l2_subdev *sd, const struct v4l2_crop *a)
 	return 0;
 }
 
-static int mt9v022_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
+static int mt9v022_get_selection(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_selection *sel)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct mt9v022 *mt9v022 = to_mt9v022(client);
 
-	a->c	= mt9v022->rect;
-	a->type	= V4L2_BUF_TYPE_VIDEO_CAPTURE;
-
-	return 0;
-}
-
-static int mt9v022_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
-{
-	a->bounds.left			= MT9V022_COLUMN_SKIP;
-	a->bounds.top			= MT9V022_ROW_SKIP;
-	a->bounds.width			= MT9V022_MAX_WIDTH;
-	a->bounds.height		= MT9V022_MAX_HEIGHT;
-	a->defrect			= a->bounds;
-	a->type				= V4L2_BUF_TYPE_VIDEO_CAPTURE;
-	a->pixelaspect.numerator	= 1;
-	a->pixelaspect.denominator	= 1;
+	if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE)
+		return -EINVAL;
 
-	return 0;
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+		sel->r.left = MT9V022_COLUMN_SKIP;
+		sel->r.top = MT9V022_ROW_SKIP;
+		sel->r.width = MT9V022_MAX_WIDTH;
+		sel->r.height = MT9V022_MAX_HEIGHT;
+		return 0;
+	case V4L2_SEL_TGT_CROP:
+		sel->r = mt9v022->rect;
+		return 0;
+	default:
+		return -EINVAL;
+	}
 }
 
 static int mt9v022_get_fmt(struct v4l2_subdev *sd,
@@ -400,13 +407,13 @@ static int mt9v022_s_fmt(struct v4l2_subdev *sd,
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct mt9v022 *mt9v022 = to_mt9v022(client);
-	struct v4l2_crop a = {
-		.c = {
-			.left	= mt9v022->rect.left,
-			.top	= mt9v022->rect.top,
-			.width	= mf->width,
-			.height	= mf->height,
-		},
+	struct v4l2_subdev_selection sel = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		.target = V4L2_SEL_TGT_CROP,
+		.r.left = mt9v022->rect.left,
+		.r.top = mt9v022->rect.top,
+		.r.width = mf->width,
+		.r.height = mf->height,
 	};
 	int ret;
 
@@ -430,7 +437,7 @@ static int mt9v022_s_fmt(struct v4l2_subdev *sd,
 	}
 
 	/* No support for scaling on this camera, just crop. */
-	ret = mt9v022_s_crop(sd, &a);
+	ret = mt9v022_set_selection(sd, NULL, &sel);
 	if (!ret) {
 		mf->width	= mt9v022->rect.width;
 		mf->height	= mt9v022->rect.height;
@@ -853,9 +860,6 @@ 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_crop		= mt9v022_s_crop,
-	.g_crop		= mt9v022_g_crop,
-	.cropcap	= mt9v022_cropcap,
 	.g_mbus_config	= mt9v022_g_mbus_config,
 	.s_mbus_config	= mt9v022_s_mbus_config,
 };
@@ -866,6 +870,8 @@ static const struct v4l2_subdev_sensor_ops mt9v022_subdev_sensor_ops = {
 
 static const struct v4l2_subdev_pad_ops mt9v022_subdev_pad_ops = {
 	.enum_mbus_code = mt9v022_enum_mbus_code,
+	.get_selection	= mt9v022_get_selection,
+	.set_selection	= mt9v022_set_selection,
 	.get_fmt	= mt9v022_get_fmt,
 	.set_fmt	= mt9v022_set_fmt,
 };
diff --git a/drivers/media/i2c/soc_camera/ov2640.c b/drivers/media/i2c/soc_camera/ov2640.c
index 9b4f5deec748..56de18263359 100644
--- a/drivers/media/i2c/soc_camera/ov2640.c
+++ b/drivers/media/i2c/soc_camera/ov2640.c
@@ -928,29 +928,25 @@ static int ov2640_enum_mbus_code(struct v4l2_subdev *sd,
 	return 0;
 }
 
-static int ov2640_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
-{
-	a->c.left	= 0;
-	a->c.top	= 0;
-	a->c.width	= UXGA_WIDTH;
-	a->c.height	= UXGA_HEIGHT;
-	a->type		= V4L2_BUF_TYPE_VIDEO_CAPTURE;
-
-	return 0;
-}
-
-static int ov2640_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
+static int ov2640_get_selection(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_selection *sel)
 {
-	a->bounds.left			= 0;
-	a->bounds.top			= 0;
-	a->bounds.width			= UXGA_WIDTH;
-	a->bounds.height		= UXGA_HEIGHT;
-	a->defrect			= a->bounds;
-	a->type				= V4L2_BUF_TYPE_VIDEO_CAPTURE;
-	a->pixelaspect.numerator	= 1;
-	a->pixelaspect.denominator	= 1;
+	if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE)
+		return -EINVAL;
 
-	return 0;
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+	case V4L2_SEL_TGT_CROP:
+		sel->r.left = 0;
+		sel->r.top = 0;
+		sel->r.width = UXGA_WIDTH;
+		sel->r.height = UXGA_HEIGHT;
+		return 0;
+	default:
+		return -EINVAL;
+	}
 }
 
 static int ov2640_video_probe(struct i2c_client *client)
@@ -1024,13 +1020,12 @@ 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,
-	.cropcap	= ov2640_cropcap,
-	.g_crop		= ov2640_g_crop,
 	.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_selection	= ov2640_get_selection,
 	.get_fmt	= ov2640_get_fmt,
 	.set_fmt	= ov2640_set_fmt,
 };
diff --git a/drivers/media/i2c/soc_camera/ov5642.c b/drivers/media/i2c/soc_camera/ov5642.c
index bab9ac0c1764..3d185bd622a3 100644
--- a/drivers/media/i2c/soc_camera/ov5642.c
+++ b/drivers/media/i2c/soc_camera/ov5642.c
@@ -850,13 +850,19 @@ static int ov5642_enum_mbus_code(struct v4l2_subdev *sd,
 	return 0;
 }
 
-static int ov5642_s_crop(struct v4l2_subdev *sd, const struct v4l2_crop *a)
+static int ov5642_set_selection(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_selection *sel)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct ov5642 *priv = to_ov5642(client);
-	struct v4l2_rect rect = a->c;
+	struct v4l2_rect rect = sel->r;
 	int ret;
 
+	if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE ||
+	    sel->target != V4L2_SEL_TGT_CROP)
+		return -EINVAL;
+
 	v4l_bound_align_image(&rect.width, 48, OV5642_MAX_WIDTH, 1,
 			      &rect.height, 32, OV5642_MAX_HEIGHT, 1, 0);
 
@@ -878,32 +884,30 @@ static int ov5642_s_crop(struct v4l2_subdev *sd, const struct v4l2_crop *a)
 	return ret;
 }
 
-static int ov5642_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
+static int ov5642_get_selection(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_selection *sel)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct ov5642 *priv = to_ov5642(client);
-	struct v4l2_rect *rect = &a->c;
 
-	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+	if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE)
 		return -EINVAL;
 
-	*rect = priv->crop_rect;
-
-	return 0;
-}
-
-static int ov5642_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
-{
-	a->bounds.left			= 0;
-	a->bounds.top			= 0;
-	a->bounds.width			= OV5642_MAX_WIDTH;
-	a->bounds.height		= OV5642_MAX_HEIGHT;
-	a->defrect			= a->bounds;
-	a->type				= V4L2_BUF_TYPE_VIDEO_CAPTURE;
-	a->pixelaspect.numerator	= 1;
-	a->pixelaspect.denominator	= 1;
-
-	return 0;
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+		sel->r.left = 0;
+		sel->r.top = 0;
+		sel->r.width = OV5642_MAX_WIDTH;
+		sel->r.height = OV5642_MAX_HEIGHT;
+		return 0;
+	case V4L2_SEL_TGT_CROP:
+		sel->r = priv->crop_rect;
+		return 0;
+	default:
+		return -EINVAL;
+	}
 }
 
 static int ov5642_g_mbus_config(struct v4l2_subdev *sd,
@@ -940,14 +944,13 @@ static int ov5642_s_power(struct v4l2_subdev *sd, int on)
 }
 
 static struct v4l2_subdev_video_ops ov5642_subdev_video_ops = {
-	.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_selection	= ov5642_get_selection,
+	.set_selection	= ov5642_set_selection,
 	.get_fmt	= ov5642_get_fmt,
 	.set_fmt	= ov5642_set_fmt,
 };
diff --git a/drivers/media/i2c/soc_camera/ov6650.c b/drivers/media/i2c/soc_camera/ov6650.c
index 1f8af1ee8352..4bf2995e1cb8 100644
--- a/drivers/media/i2c/soc_camera/ov6650.c
+++ b/drivers/media/i2c/soc_camera/ov6650.c
@@ -432,25 +432,43 @@ static int ov6650_s_power(struct v4l2_subdev *sd, int on)
 	return soc_camera_set_power(&client->dev, ssdd, priv->clk, on);
 }
 
-static int ov6650_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
+static int ov6650_get_selection(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_selection *sel)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct ov6650 *priv = to_ov6650(client);
 
-	a->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-	a->c = priv->rect;
+	if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE)
+		return -EINVAL;
 
-	return 0;
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+		sel->r.left = DEF_HSTRT << 1;
+		sel->r.top = DEF_VSTRT << 1;
+		sel->r.width = W_CIF;
+		sel->r.height = H_CIF;
+		return 0;
+	case V4L2_SEL_TGT_CROP:
+		sel->r = priv->rect;
+		return 0;
+	default:
+		return -EINVAL;
+	}
 }
 
-static int ov6650_s_crop(struct v4l2_subdev *sd, const struct v4l2_crop *a)
+static int ov6650_set_selection(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_selection *sel)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct ov6650 *priv = to_ov6650(client);
-	struct v4l2_rect rect = a->c;
+	struct v4l2_rect rect = sel->r;
 	int ret;
 
-	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+	if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE ||
+	    sel->target != V4L2_SEL_TGT_CROP)
 		return -EINVAL;
 
 	rect.left   = ALIGN(rect.left,   2);
@@ -483,22 +501,6 @@ static int ov6650_s_crop(struct v4l2_subdev *sd, const struct v4l2_crop *a)
 	return ret;
 }
 
-static int ov6650_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
-{
-	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
-
-	a->bounds.left			= DEF_HSTRT << 1;
-	a->bounds.top			= DEF_VSTRT << 1;
-	a->bounds.width			= W_CIF;
-	a->bounds.height		= H_CIF;
-	a->defrect			= a->bounds;
-	a->pixelaspect.numerator	= 1;
-	a->pixelaspect.denominator	= 1;
-
-	return 0;
-}
-
 static int ov6650_get_fmt(struct v4l2_subdev *sd,
 		struct v4l2_subdev_pad_config *cfg,
 		struct v4l2_subdev_format *format)
@@ -549,16 +551,15 @@ static int ov6650_s_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *mf)
 	struct soc_camera_sense *sense = icd->sense;
 	struct ov6650 *priv = to_ov6650(client);
 	bool half_scale = !is_unscaled_ok(mf->width, mf->height, &priv->rect);
-	struct v4l2_crop a = {
-		.type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
-		.c = {
-			.left	= priv->rect.left + (priv->rect.width >> 1) -
-					(mf->width >> (1 - half_scale)),
-			.top	= priv->rect.top + (priv->rect.height >> 1) -
-					(mf->height >> (1 - half_scale)),
-			.width	= mf->width << half_scale,
-			.height	= mf->height << half_scale,
-		},
+	struct v4l2_subdev_selection sel = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		.target = V4L2_SEL_TGT_CROP,
+		.r.left = priv->rect.left + (priv->rect.width >> 1) -
+			(mf->width >> (1 - half_scale)),
+		.r.top = priv->rect.top + (priv->rect.height >> 1) -
+			(mf->height >> (1 - half_scale)),
+		.r.width = mf->width << half_scale,
+		.r.height = mf->height << half_scale,
 	};
 	u32 code = mf->code;
 	unsigned long mclk, pclk;
@@ -672,7 +673,7 @@ static int ov6650_s_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *mf)
 	dev_dbg(&client->dev, "pixel clock divider: %ld.%ld\n",
 			mclk / pclk, 10 * mclk % pclk / pclk);
 
-	ret = ov6650_s_crop(sd, &a);
+	ret = ov6650_set_selection(sd, NULL, &sel);
 	if (!ret)
 		ret = ov6650_reg_rmw(client, REG_COMA, coma_set, coma_mask);
 	if (!ret)
@@ -943,9 +944,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,
-	.cropcap	= ov6650_cropcap,
-	.g_crop		= ov6650_g_crop,
-	.s_crop		= ov6650_s_crop,
 	.g_parm		= ov6650_g_parm,
 	.s_parm		= ov6650_s_parm,
 	.g_mbus_config	= ov6650_g_mbus_config,
@@ -954,6 +952,8 @@ static struct v4l2_subdev_video_ops ov6650_video_ops = {
 
 static const struct v4l2_subdev_pad_ops ov6650_pad_ops = {
 	.enum_mbus_code = ov6650_enum_mbus_code,
+	.get_selection	= ov6650_get_selection,
+	.set_selection	= ov6650_set_selection,
 	.get_fmt	= ov6650_get_fmt,
 	.set_fmt	= ov6650_set_fmt,
 };
diff --git a/drivers/media/i2c/soc_camera/ov772x.c b/drivers/media/i2c/soc_camera/ov772x.c
index a43410c1e254..7e68762b3a4b 100644
--- a/drivers/media/i2c/soc_camera/ov772x.c
+++ b/drivers/media/i2c/soc_camera/ov772x.c
@@ -851,29 +851,28 @@ ov772x_set_fmt_error:
 	return ret;
 }
 
-static int ov772x_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
-{
-	a->c.left	= 0;
-	a->c.top	= 0;
-	a->c.width	= VGA_WIDTH;
-	a->c.height	= VGA_HEIGHT;
-	a->type		= V4L2_BUF_TYPE_VIDEO_CAPTURE;
-
-	return 0;
-}
-
-static int ov772x_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
+static int ov772x_get_selection(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_selection *sel)
 {
-	a->bounds.left			= 0;
-	a->bounds.top			= 0;
-	a->bounds.width			= OV772X_MAX_WIDTH;
-	a->bounds.height		= OV772X_MAX_HEIGHT;
-	a->defrect			= a->bounds;
-	a->type				= V4L2_BUF_TYPE_VIDEO_CAPTURE;
-	a->pixelaspect.numerator	= 1;
-	a->pixelaspect.denominator	= 1;
+	if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE)
+		return -EINVAL;
 
-	return 0;
+	sel->r.left = 0;
+	sel->r.top = 0;
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+		sel->r.width = OV772X_MAX_WIDTH;
+		sel->r.height = OV772X_MAX_HEIGHT;
+		return 0;
+	case V4L2_SEL_TGT_CROP:
+		sel->r.width = VGA_WIDTH;
+		sel->r.height = VGA_HEIGHT;
+		return 0;
+	default:
+		return -EINVAL;
+	}
 }
 
 static int ov772x_get_fmt(struct v4l2_subdev *sd,
@@ -1030,13 +1029,12 @@ 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,
-	.cropcap	= ov772x_cropcap,
-	.g_crop		= ov772x_g_crop,
 	.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_selection	= ov772x_get_selection,
 	.get_fmt	= ov772x_get_fmt,
 	.set_fmt	= ov772x_set_fmt,
 };
diff --git a/drivers/media/i2c/soc_camera/ov9640.c b/drivers/media/i2c/soc_camera/ov9640.c
index 8caae1c07541..8c93c57af71c 100644
--- a/drivers/media/i2c/soc_camera/ov9640.c
+++ b/drivers/media/i2c/soc_camera/ov9640.c
@@ -561,29 +561,25 @@ static int ov9640_enum_mbus_code(struct v4l2_subdev *sd,
 	return 0;
 }
 
-static int ov9640_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
-{
-	a->c.left	= 0;
-	a->c.top	= 0;
-	a->c.width	= W_SXGA;
-	a->c.height	= H_SXGA;
-	a->type		= V4L2_BUF_TYPE_VIDEO_CAPTURE;
-
-	return 0;
-}
-
-static int ov9640_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
+static int ov9640_get_selection(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_selection *sel)
 {
-	a->bounds.left			= 0;
-	a->bounds.top			= 0;
-	a->bounds.width			= W_SXGA;
-	a->bounds.height		= H_SXGA;
-	a->defrect			= a->bounds;
-	a->type				= V4L2_BUF_TYPE_VIDEO_CAPTURE;
-	a->pixelaspect.numerator	= 1;
-	a->pixelaspect.denominator	= 1;
+	if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE)
+		return -EINVAL;
 
-	return 0;
+	sel->r.left = 0;
+	sel->r.top = 0;
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+	case V4L2_SEL_TGT_CROP:
+		sel->r.width = W_SXGA;
+		sel->r.height = H_SXGA;
+		return 0;
+	default:
+		return -EINVAL;
+	}
 }
 
 static int ov9640_video_probe(struct i2c_client *client)
@@ -667,13 +663,12 @@ static int ov9640_g_mbus_config(struct v4l2_subdev *sd,
 
 static struct v4l2_subdev_video_ops ov9640_video_ops = {
 	.s_stream	= ov9640_s_stream,
-	.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,
+	.get_selection	= ov9640_get_selection,
 	.set_fmt	= ov9640_set_fmt,
 };
 
diff --git a/drivers/media/i2c/soc_camera/ov9740.c b/drivers/media/i2c/soc_camera/ov9740.c
index 03a7fc7316ae..0da632d7d33a 100644
--- a/drivers/media/i2c/soc_camera/ov9740.c
+++ b/drivers/media/i2c/soc_camera/ov9740.c
@@ -737,29 +737,25 @@ static int ov9740_enum_mbus_code(struct v4l2_subdev *sd,
 	return 0;
 }
 
-static int ov9740_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
-{
-	a->bounds.left		= 0;
-	a->bounds.top		= 0;
-	a->bounds.width		= OV9740_MAX_WIDTH;
-	a->bounds.height	= OV9740_MAX_HEIGHT;
-	a->defrect		= a->bounds;
-	a->type			= V4L2_BUF_TYPE_VIDEO_CAPTURE;
-	a->pixelaspect.numerator	= 1;
-	a->pixelaspect.denominator	= 1;
-
-	return 0;
-}
-
-static int ov9740_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
+static int ov9740_get_selection(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_selection *sel)
 {
-	a->c.left		= 0;
-	a->c.top		= 0;
-	a->c.width		= OV9740_MAX_WIDTH;
-	a->c.height		= OV9740_MAX_HEIGHT;
-	a->type			= V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE)
+		return -EINVAL;
 
-	return 0;
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+	case V4L2_SEL_TGT_CROP:
+		sel->r.left = 0;
+		sel->r.top = 0;
+		sel->r.width = OV9740_MAX_WIDTH;
+		sel->r.height = OV9740_MAX_HEIGHT;
+		return 0;
+	default:
+		return -EINVAL;
+	}
 }
 
 /* Set status of additional camera capabilities */
@@ -914,8 +910,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,
-	.cropcap	= ov9740_cropcap,
-	.g_crop		= ov9740_g_crop,
 	.g_mbus_config	= ov9740_g_mbus_config,
 };
 
@@ -929,6 +923,7 @@ static struct v4l2_subdev_core_ops ov9740_core_ops = {
 
 static const struct v4l2_subdev_pad_ops ov9740_pad_ops = {
 	.enum_mbus_code = ov9740_enum_mbus_code,
+	.get_selection	= ov9740_get_selection,
 	.set_fmt	= ov9740_set_fmt,
 };
 
diff --git a/drivers/media/i2c/soc_camera/rj54n1cb0c.c b/drivers/media/i2c/soc_camera/rj54n1cb0c.c
index aa7bfbb4ad71..bc8ec59a3fbd 100644
--- a/drivers/media/i2c/soc_camera/rj54n1cb0c.c
+++ b/drivers/media/i2c/soc_camera/rj54n1cb0c.c
@@ -538,15 +538,21 @@ static int rj54n1_commit(struct i2c_client *client)
 static int rj54n1_sensor_scale(struct v4l2_subdev *sd, s32 *in_w, s32 *in_h,
 			       s32 *out_w, s32 *out_h);
 
-static int rj54n1_s_crop(struct v4l2_subdev *sd, const struct v4l2_crop *a)
+static int rj54n1_set_selection(struct v4l2_subdev *sd,
+				struct v4l2_subdev_pad_config *cfg,
+				struct v4l2_subdev_selection *sel)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct rj54n1 *rj54n1 = to_rj54n1(client);
-	const struct v4l2_rect *rect = &a->c;
+	const struct v4l2_rect *rect = &sel->r;
 	int dummy = 0, output_w, output_h,
 		input_w = rect->width, input_h = rect->height;
 	int ret;
 
+	if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE ||
+	    sel->target != V4L2_SEL_TGT_CROP)
+		return -EINVAL;
+
 	/* arbitrary minimum width and height, edges unimportant */
 	soc_camera_limit_side(&dummy, &input_w,
 		     RJ54N1_COLUMN_SKIP, 8, RJ54N1_MAX_WIDTH);
@@ -573,29 +579,30 @@ static int rj54n1_s_crop(struct v4l2_subdev *sd, const struct v4l2_crop *a)
 	return 0;
 }
 
-static int rj54n1_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
+static int rj54n1_get_selection(struct v4l2_subdev *sd,
+				struct v4l2_subdev_pad_config *cfg,
+				struct v4l2_subdev_selection *sel)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct rj54n1 *rj54n1 = to_rj54n1(client);
 
-	a->c	= rj54n1->rect;
-	a->type	= V4L2_BUF_TYPE_VIDEO_CAPTURE;
-
-	return 0;
-}
-
-static int rj54n1_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
-{
-	a->bounds.left			= RJ54N1_COLUMN_SKIP;
-	a->bounds.top			= RJ54N1_ROW_SKIP;
-	a->bounds.width			= RJ54N1_MAX_WIDTH;
-	a->bounds.height		= RJ54N1_MAX_HEIGHT;
-	a->defrect			= a->bounds;
-	a->type				= V4L2_BUF_TYPE_VIDEO_CAPTURE;
-	a->pixelaspect.numerator	= 1;
-	a->pixelaspect.denominator	= 1;
+	if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE)
+		return -EINVAL;
 
-	return 0;
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+		sel->r.left = RJ54N1_COLUMN_SKIP;
+		sel->r.top = RJ54N1_ROW_SKIP;
+		sel->r.width = RJ54N1_MAX_WIDTH;
+		sel->r.height = RJ54N1_MAX_HEIGHT;
+		return 0;
+	case V4L2_SEL_TGT_CROP:
+		sel->r = rj54n1->rect;
+		return 0;
+	default:
+		return -EINVAL;
+	}
 }
 
 static int rj54n1_get_fmt(struct v4l2_subdev *sd,
@@ -1246,15 +1253,14 @@ 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,
-	.g_crop		= rj54n1_g_crop,
-	.s_crop		= rj54n1_s_crop,
-	.cropcap	= rj54n1_cropcap,
 	.g_mbus_config	= rj54n1_g_mbus_config,
 	.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_selection	= rj54n1_get_selection,
+	.set_selection	= rj54n1_set_selection,
 	.get_fmt	= rj54n1_get_fmt,
 	.set_fmt	= rj54n1_set_fmt,
 };
diff --git a/drivers/media/i2c/soc_camera/tw9910.c b/drivers/media/i2c/soc_camera/tw9910.c
index 06aff81787a7..4002c07f3857 100644
--- a/drivers/media/i2c/soc_camera/tw9910.c
+++ b/drivers/media/i2c/soc_camera/tw9910.c
@@ -676,44 +676,28 @@ tw9910_set_fmt_error:
 	return ret;
 }
 
-static int tw9910_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
+static int tw9910_get_selection(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_selection *sel)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct tw9910_priv *priv = to_tw9910(client);
 
-	a->c.left	= 0;
-	a->c.top	= 0;
-	if (priv->norm & V4L2_STD_NTSC) {
-		a->c.width	= 640;
-		a->c.height	= 480;
-	} else {
-		a->c.width	= 768;
-		a->c.height	= 576;
-	}
-	a->type		= V4L2_BUF_TYPE_VIDEO_CAPTURE;
-
-	return 0;
-}
-
-static int tw9910_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
-{
-	struct i2c_client *client = v4l2_get_subdevdata(sd);
-	struct tw9910_priv *priv = to_tw9910(client);
+	if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE)
+		return -EINVAL;
+	/* Only CROP, CROP_DEFAULT and CROP_BOUNDS are supported */
+	if (sel->target > V4L2_SEL_TGT_CROP_BOUNDS)
+		return -EINVAL;
 
-	a->bounds.left			= 0;
-	a->bounds.top			= 0;
+	sel->r.left	= 0;
+	sel->r.top	= 0;
 	if (priv->norm & V4L2_STD_NTSC) {
-		a->bounds.width		= 640;
-		a->bounds.height	= 480;
+		sel->r.width	= 640;
+		sel->r.height	= 480;
 	} else {
-		a->bounds.width		= 768;
-		a->bounds.height	= 576;
+		sel->r.width	= 768;
+		sel->r.height	= 576;
 	}
-	a->defrect			= a->bounds;
-	a->type				= V4L2_BUF_TYPE_VIDEO_CAPTURE;
-	a->pixelaspect.numerator	= 1;
-	a->pixelaspect.denominator	= 1;
-
 	return 0;
 }
 
@@ -921,8 +905,6 @@ 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,
-	.cropcap	= tw9910_cropcap,
-	.g_crop		= tw9910_g_crop,
 	.g_mbus_config	= tw9910_g_mbus_config,
 	.s_mbus_config	= tw9910_s_mbus_config,
 	.g_tvnorms	= tw9910_g_tvnorms,
@@ -930,6 +912,7 @@ static struct v4l2_subdev_video_ops tw9910_subdev_video_ops = {
 
 static const struct v4l2_subdev_pad_ops tw9910_subdev_pad_ops = {
 	.enum_mbus_code = tw9910_enum_mbus_code,
+	.get_selection	= tw9910_get_selection,
 	.get_fmt	= tw9910_get_fmt,
 	.set_fmt	= tw9910_set_fmt,
 };
diff --git a/drivers/media/i2c/ths8200.c b/drivers/media/i2c/ths8200.c
index 73fc42bc2de6..42340e364cea 100644
--- a/drivers/media/i2c/ths8200.c
+++ b/drivers/media/i2c/ths8200.c
@@ -499,7 +499,6 @@ MODULE_DEVICE_TABLE(of, ths8200_of_match);
 
 static struct i2c_driver ths8200_driver = {
 	.driver = {
-		.owner = THIS_MODULE,
 		.name = "ths8200",
 		.of_match_table = of_match_ptr(ths8200_of_match),
 	},
diff --git a/drivers/media/i2c/tlv320aic23b.c b/drivers/media/i2c/tlv320aic23b.c
index 0370dd89f1fc..2e06c06cac9b 100644
--- a/drivers/media/i2c/tlv320aic23b.c
+++ b/drivers/media/i2c/tlv320aic23b.c
@@ -210,7 +210,6 @@ MODULE_DEVICE_TABLE(i2c, tlv320aic23b_id);
 
 static struct i2c_driver tlv320aic23b_driver = {
 	.driver = {
-		.owner	= THIS_MODULE,
 		.name	= "tlv320aic23b",
 	},
 	.probe		= tlv320aic23b_probe,
diff --git a/drivers/media/i2c/tvp514x.c b/drivers/media/i2c/tvp514x.c
index 7cdd94842938..d5c9347f4c6d 100644
--- a/drivers/media/i2c/tvp514x.c
+++ b/drivers/media/i2c/tvp514x.c
@@ -977,7 +977,7 @@ static const struct v4l2_subdev_ops tvp514x_ops = {
 	.pad = &tvp514x_pad_ops,
 };
 
-static struct tvp514x_decoder tvp514x_dev = {
+static const struct tvp514x_decoder tvp514x_dev = {
 	.streaming = 0,
 	.fmt_list = tvp514x_fmt_list,
 	.num_fmts = ARRAY_SIZE(tvp514x_fmt_list),
@@ -1233,7 +1233,6 @@ MODULE_DEVICE_TABLE(of, tvp514x_of_match);
 static struct i2c_driver tvp514x_driver = {
 	.driver = {
 		.of_match_table = of_match_ptr(tvp514x_of_match),
-		.owner = THIS_MODULE,
 		.name = TVP514X_MODULE_NAME,
 	},
 	.probe = tvp514x_probe,
diff --git a/drivers/media/i2c/tvp5150.c b/drivers/media/i2c/tvp5150.c
index 0b6d46c453bf..4740da39d698 100644
--- a/drivers/media/i2c/tvp5150.c
+++ b/drivers/media/i2c/tvp5150.c
@@ -871,19 +871,22 @@ static int tvp5150_fill_fmt(struct v4l2_subdev *sd,
 	return 0;
 }
 
-static int tvp5150_s_crop(struct v4l2_subdev *sd, const struct v4l2_crop *a)
+static int tvp5150_set_selection(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_pad_config *cfg,
+				 struct v4l2_subdev_selection *sel)
 {
-	struct v4l2_rect rect = a->c;
 	struct tvp5150 *decoder = to_tvp5150(sd);
+	struct v4l2_rect rect = sel->r;
 	v4l2_std_id std;
-	unsigned int hmax;
+	int hmax;
+
+	if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE ||
+	    sel->target != V4L2_SEL_TGT_CROP)
+		return -EINVAL;
 
 	v4l2_dbg(1, debug, sd, "%s left=%d, top=%d, width=%d, height=%d\n",
 		__func__, rect.left, rect.top, rect.width, rect.height);
 
-	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
-
 	/* tvp5150 has some special limits */
 	rect.left = clamp(rect.left, 0, TVP5150_MAX_CROP_LEFT);
 	rect.width = clamp_t(unsigned int, rect.width,
@@ -924,44 +927,39 @@ static int tvp5150_s_crop(struct v4l2_subdev *sd, const struct v4l2_crop *a)
 	return 0;
 }
 
-static int tvp5150_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
-{
-	struct tvp5150 *decoder = to_tvp5150(sd);
-
-	a->c	= decoder->rect;
-	a->type	= V4L2_BUF_TYPE_VIDEO_CAPTURE;
-
-	return 0;
-}
-
-static int tvp5150_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
+static int tvp5150_get_selection(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_pad_config *cfg,
+				 struct v4l2_subdev_selection *sel)
 {
-	struct tvp5150 *decoder = to_tvp5150(sd);
+	struct tvp5150 *decoder = container_of(sd, struct tvp5150, sd);
 	v4l2_std_id std;
 
-	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+	if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE)
 		return -EINVAL;
 
-	a->bounds.left			= 0;
-	a->bounds.top			= 0;
-	a->bounds.width			= TVP5150_H_MAX;
-
-	/* Calculate height based on current standard */
-	if (decoder->norm == V4L2_STD_ALL)
-		std = tvp5150_read_std(sd);
-	else
-		std = decoder->norm;
-
-	if (std & V4L2_STD_525_60)
-		a->bounds.height = TVP5150_V_MAX_525_60;
-	else
-		a->bounds.height = TVP5150_V_MAX_OTHERS;
-
-	a->defrect			= a->bounds;
-	a->pixelaspect.numerator	= 1;
-	a->pixelaspect.denominator	= 1;
-
-	return 0;
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+		sel->r.left = 0;
+		sel->r.top = 0;
+		sel->r.width = TVP5150_H_MAX;
+
+		/* Calculate height based on current standard */
+		if (decoder->norm == V4L2_STD_ALL)
+			std = tvp5150_read_std(sd);
+		else
+			std = decoder->norm;
+		if (std & V4L2_STD_525_60)
+			sel->r.height = TVP5150_V_MAX_525_60;
+		else
+			sel->r.height = TVP5150_V_MAX_OTHERS;
+		return 0;
+	case V4L2_SEL_TGT_CROP:
+		sel->r = decoder->rect;
+		return 0;
+	default:
+		return -EINVAL;
+	}
 }
 
 static int tvp5150_g_mbus_config(struct v4l2_subdev *sd,
@@ -1173,7 +1171,7 @@ static int tvp5150_g_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
 	return 0;
 }
 
-static int tvp5150_registered_async(struct v4l2_subdev *sd)
+static int tvp5150_registered(struct v4l2_subdev *sd)
 {
 #ifdef CONFIG_MEDIA_CONTROLLER
 	struct tvp5150 *decoder = to_tvp5150(sd);
@@ -1222,7 +1220,6 @@ static const struct v4l2_subdev_core_ops tvp5150_core_ops = {
 	.g_register = tvp5150_g_register,
 	.s_register = tvp5150_s_register,
 #endif
-	.registered_async = tvp5150_registered_async,
 };
 
 static const struct v4l2_subdev_tuner_ops tvp5150_tuner_ops = {
@@ -1233,9 +1230,6 @@ static const struct v4l2_subdev_video_ops tvp5150_video_ops = {
 	.s_std = tvp5150_s_std,
 	.s_stream = tvp5150_s_stream,
 	.s_routing = tvp5150_s_routing,
-	.s_crop = tvp5150_s_crop,
-	.g_crop = tvp5150_g_crop,
-	.cropcap = tvp5150_cropcap,
 	.g_mbus_config = tvp5150_g_mbus_config,
 };
 
@@ -1251,6 +1245,8 @@ static const struct v4l2_subdev_pad_ops tvp5150_pad_ops = {
 	.enum_frame_size = tvp5150_enum_frame_size,
 	.set_fmt = tvp5150_fill_fmt,
 	.get_fmt = tvp5150_fill_fmt,
+	.get_selection = tvp5150_get_selection,
+	.set_selection = tvp5150_set_selection,
 };
 
 static const struct v4l2_subdev_ops tvp5150_ops = {
@@ -1261,6 +1257,10 @@ static const struct v4l2_subdev_ops tvp5150_ops = {
 	.pad = &tvp5150_pad_ops,
 };
 
+static const struct v4l2_subdev_internal_ops tvp5150_internal_ops = {
+	.registered = tvp5150_registered,
+};
+
 
 /****************************************************************************
 			I2C Client & Driver
@@ -1474,6 +1474,7 @@ static int tvp5150_probe(struct i2c_client *c,
 	}
 
 	v4l2_i2c_subdev_init(sd, c, &tvp5150_ops);
+	sd->internal_ops = &tvp5150_internal_ops;
 	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
 
 #if defined(CONFIG_MEDIA_CONTROLLER)
diff --git a/drivers/media/i2c/tvp7002.c b/drivers/media/i2c/tvp7002.c
index 4df640c3aa40..3dc3341c4896 100644
--- a/drivers/media/i2c/tvp7002.c
+++ b/drivers/media/i2c/tvp7002.c
@@ -1086,7 +1086,6 @@ MODULE_DEVICE_TABLE(of, tvp7002_of_match);
 static struct i2c_driver tvp7002_driver = {
 	.driver = {
 		.of_match_table = of_match_ptr(tvp7002_of_match),
-		.owner = THIS_MODULE,
 		.name = TVP7002_MODULE_NAME,
 	},
 	.probe = tvp7002_probe,
diff --git a/drivers/media/i2c/vs6624.c b/drivers/media/i2c/vs6624.c
index 4c72a18c0b8c..be4cb7a8bdeb 100644
--- a/drivers/media/i2c/vs6624.c
+++ b/drivers/media/i2c/vs6624.c
@@ -863,7 +863,6 @@ MODULE_DEVICE_TABLE(i2c, vs6624_id);
 
 static struct i2c_driver vs6624_driver = {
 	.driver = {
-		.owner  = THIS_MODULE,
 		.name   = "vs6624",
 	},
 	.probe          = vs6624_probe,
diff --git a/drivers/media/media-device.c b/drivers/media/media-device.c
index 1795abeda658..2783531f9fc0 100644
--- a/drivers/media/media-device.c
+++ b/drivers/media/media-device.c
@@ -59,27 +59,24 @@ static int media_device_close(struct file *filp)
 }
 
 static int media_device_get_info(struct media_device *dev,
-				 struct media_device_info __user *__info)
+				 struct media_device_info *info)
 {
-	struct media_device_info info;
-
-	memset(&info, 0, sizeof(info));
+	memset(info, 0, sizeof(*info));
 
 	if (dev->driver_name[0])
-		strlcpy(info.driver, dev->driver_name, sizeof(info.driver));
+		strlcpy(info->driver, dev->driver_name, sizeof(info->driver));
 	else
-		strlcpy(info.driver, dev->dev->driver->name, sizeof(info.driver));
+		strlcpy(info->driver, dev->dev->driver->name,
+			sizeof(info->driver));
 
-	strlcpy(info.model, dev->model, sizeof(info.model));
-	strlcpy(info.serial, dev->serial, sizeof(info.serial));
-	strlcpy(info.bus_info, dev->bus_info, sizeof(info.bus_info));
+	strlcpy(info->model, dev->model, sizeof(info->model));
+	strlcpy(info->serial, dev->serial, sizeof(info->serial));
+	strlcpy(info->bus_info, dev->bus_info, sizeof(info->bus_info));
 
-	info.media_version = MEDIA_API_VERSION;
-	info.hw_revision = dev->hw_revision;
-	info.driver_version = dev->driver_version;
+	info->media_version = MEDIA_API_VERSION;
+	info->hw_revision = dev->hw_revision;
+	info->driver_version = dev->driver_version;
 
-	if (copy_to_user(__info, &info, sizeof(*__info)))
-		return -EFAULT;
 	return 0;
 }
 
@@ -101,29 +98,25 @@ static struct media_entity *find_entity(struct media_device *mdev, u32 id)
 }
 
 static long media_device_enum_entities(struct media_device *mdev,
-				       struct media_entity_desc __user *uent)
+				       struct media_entity_desc *entd)
 {
 	struct media_entity *ent;
-	struct media_entity_desc u_ent;
-
-	memset(&u_ent, 0, sizeof(u_ent));
-	if (copy_from_user(&u_ent.id, &uent->id, sizeof(u_ent.id)))
-		return -EFAULT;
-
-	ent = find_entity(mdev, u_ent.id);
 
+	ent = find_entity(mdev, entd->id);
 	if (ent == NULL)
 		return -EINVAL;
 
-	u_ent.id = media_entity_id(ent);
+	memset(entd, 0, sizeof(*entd));
+
+	entd->id = media_entity_id(ent);
 	if (ent->name)
-		strlcpy(u_ent.name, ent->name, sizeof(u_ent.name));
-	u_ent.type = ent->function;
-	u_ent.revision = 0;		/* Unused */
-	u_ent.flags = ent->flags;
-	u_ent.group_id = 0;		/* Unused */
-	u_ent.pads = ent->num_pads;
-	u_ent.links = ent->num_links - ent->num_backlinks;
+		strlcpy(entd->name, ent->name, sizeof(entd->name));
+	entd->type = ent->function;
+	entd->revision = 0;		/* Unused */
+	entd->flags = ent->flags;
+	entd->group_id = 0;		/* Unused */
+	entd->pads = ent->num_pads;
+	entd->links = ent->num_links - ent->num_backlinks;
 
 	/*
 	 * Workaround for a bug at media-ctl <= v1.10 that makes it to
@@ -139,14 +132,13 @@ static long media_device_enum_entities(struct media_device *mdev,
 	if (ent->function < MEDIA_ENT_F_OLD_BASE ||
 	    ent->function > MEDIA_ENT_T_DEVNODE_UNKNOWN) {
 		if (is_media_entity_v4l2_subdev(ent))
-			u_ent.type = MEDIA_ENT_F_V4L2_SUBDEV_UNKNOWN;
+			entd->type = MEDIA_ENT_F_V4L2_SUBDEV_UNKNOWN;
 		else if (ent->function != MEDIA_ENT_F_IO_V4L)
-			u_ent.type = MEDIA_ENT_T_DEVNODE_UNKNOWN;
+			entd->type = MEDIA_ENT_T_DEVNODE_UNKNOWN;
 	}
 
-	memcpy(&u_ent.raw, &ent->info, sizeof(ent->info));
-	if (copy_to_user(uent, &u_ent, sizeof(u_ent)))
-		return -EFAULT;
+	memcpy(&entd->raw, &ent->info, sizeof(ent->info));
+
 	return 0;
 }
 
@@ -158,8 +150,8 @@ static void media_device_kpad_to_upad(const struct media_pad *kpad,
 	upad->flags = kpad->flags;
 }
 
-static long __media_device_enum_links(struct media_device *mdev,
-				      struct media_links_enum *links)
+static long media_device_enum_links(struct media_device *mdev,
+				    struct media_links_enum *links)
 {
 	struct media_entity *entity;
 
@@ -206,64 +198,35 @@ static long __media_device_enum_links(struct media_device *mdev,
 	return 0;
 }
 
-static long media_device_enum_links(struct media_device *mdev,
-				    struct media_links_enum __user *ulinks)
-{
-	struct media_links_enum links;
-	int rval;
-
-	if (copy_from_user(&links, ulinks, sizeof(links)))
-		return -EFAULT;
-
-	rval = __media_device_enum_links(mdev, &links);
-	if (rval < 0)
-		return rval;
-
-	if (copy_to_user(ulinks, &links, sizeof(*ulinks)))
-		return -EFAULT;
-
-	return 0;
-}
-
 static long media_device_setup_link(struct media_device *mdev,
-				    struct media_link_desc __user *_ulink)
+				    struct media_link_desc *linkd)
 {
 	struct media_link *link = NULL;
-	struct media_link_desc ulink;
 	struct media_entity *source;
 	struct media_entity *sink;
-	int ret;
-
-	if (copy_from_user(&ulink, _ulink, sizeof(ulink)))
-		return -EFAULT;
 
 	/* Find the source and sink entities and link.
 	 */
-	source = find_entity(mdev, ulink.source.entity);
-	sink = find_entity(mdev, ulink.sink.entity);
+	source = find_entity(mdev, linkd->source.entity);
+	sink = find_entity(mdev, linkd->sink.entity);
 
 	if (source == NULL || sink == NULL)
 		return -EINVAL;
 
-	if (ulink.source.index >= source->num_pads ||
-	    ulink.sink.index >= sink->num_pads)
+	if (linkd->source.index >= source->num_pads ||
+	    linkd->sink.index >= sink->num_pads)
 		return -EINVAL;
 
-	link = media_entity_find_link(&source->pads[ulink.source.index],
-				      &sink->pads[ulink.sink.index]);
+	link = media_entity_find_link(&source->pads[linkd->source.index],
+				      &sink->pads[linkd->sink.index]);
 	if (link == NULL)
 		return -EINVAL;
 
 	/* Setup the link on both entities. */
-	ret = __media_entity_setup_link(link, ulink.flags);
-
-	if (copy_to_user(_ulink, &ulink, sizeof(ulink)))
-		return -EFAULT;
-
-	return ret;
+	return __media_entity_setup_link(link, linkd->flags);
 }
 
-static long __media_device_get_topology(struct media_device *mdev,
+static long media_device_get_topology(struct media_device *mdev,
 				      struct media_v2_topology *topo)
 {
 	struct media_entity *entity;
@@ -400,63 +363,98 @@ static long __media_device_get_topology(struct media_device *mdev,
 	return ret;
 }
 
-static long media_device_get_topology(struct media_device *mdev,
-				      struct media_v2_topology __user *utopo)
+static long copy_arg_from_user(void *karg, void __user *uarg, unsigned int cmd)
 {
-	struct media_v2_topology ktopo;
-	int ret;
-
-	if (copy_from_user(&ktopo, utopo, sizeof(ktopo)))
+	/* All media IOCTLs are _IOWR() */
+	if (copy_from_user(karg, uarg, _IOC_SIZE(cmd)))
 		return -EFAULT;
 
-	ret = __media_device_get_topology(mdev, &ktopo);
-	if (ret < 0)
-		return ret;
+	return 0;
+}
 
-	if (copy_to_user(utopo, &ktopo, sizeof(*utopo)))
+static long copy_arg_to_user(void __user *uarg, void *karg, unsigned int cmd)
+{
+	/* All media IOCTLs are _IOWR() */
+	if (copy_to_user(uarg, karg, _IOC_SIZE(cmd)))
 		return -EFAULT;
 
 	return 0;
 }
 
+/* Do acquire the graph mutex */
+#define MEDIA_IOC_FL_GRAPH_MUTEX	BIT(0)
+
+#define MEDIA_IOC_ARG(__cmd, func, fl, from_user, to_user)		\
+	[_IOC_NR(MEDIA_IOC_##__cmd)] = {				\
+		.cmd = MEDIA_IOC_##__cmd,				\
+		.fn = (long (*)(struct media_device *, void *))func,	\
+		.flags = fl,						\
+		.arg_from_user = from_user,				\
+		.arg_to_user = to_user,					\
+	}
+
+#define MEDIA_IOC(__cmd, func, fl)					\
+	MEDIA_IOC_ARG(__cmd, func, fl, copy_arg_from_user, copy_arg_to_user)
+
+/* the table is indexed by _IOC_NR(cmd) */
+struct media_ioctl_info {
+	unsigned int cmd;
+	unsigned short flags;
+	long (*fn)(struct media_device *dev, void *arg);
+	long (*arg_from_user)(void *karg, void __user *uarg, unsigned int cmd);
+	long (*arg_to_user)(void __user *uarg, void *karg, unsigned int cmd);
+};
+
+static const struct media_ioctl_info ioctl_info[] = {
+	MEDIA_IOC(DEVICE_INFO, media_device_get_info, MEDIA_IOC_FL_GRAPH_MUTEX),
+	MEDIA_IOC(ENUM_ENTITIES, media_device_enum_entities, MEDIA_IOC_FL_GRAPH_MUTEX),
+	MEDIA_IOC(ENUM_LINKS, media_device_enum_links, MEDIA_IOC_FL_GRAPH_MUTEX),
+	MEDIA_IOC(SETUP_LINK, media_device_setup_link, MEDIA_IOC_FL_GRAPH_MUTEX),
+	MEDIA_IOC(G_TOPOLOGY, media_device_get_topology, MEDIA_IOC_FL_GRAPH_MUTEX),
+};
+
 static long media_device_ioctl(struct file *filp, unsigned int cmd,
-			       unsigned long arg)
+			       unsigned long __arg)
 {
 	struct media_devnode *devnode = media_devnode_data(filp);
 	struct media_device *dev = devnode->media_dev;
+	const struct media_ioctl_info *info;
+	void __user *arg = (void __user *)__arg;
+	char __karg[256], *karg = __karg;
 	long ret;
 
-	mutex_lock(&dev->graph_mutex);
-	switch (cmd) {
-	case MEDIA_IOC_DEVICE_INFO:
-		ret = media_device_get_info(dev,
-				(struct media_device_info __user *)arg);
-		break;
+	if (_IOC_NR(cmd) >= ARRAY_SIZE(ioctl_info)
+	    || ioctl_info[_IOC_NR(cmd)].cmd != cmd)
+		return -ENOIOCTLCMD;
 
-	case MEDIA_IOC_ENUM_ENTITIES:
-		ret = media_device_enum_entities(dev,
-				(struct media_entity_desc __user *)arg);
-		break;
+	info = &ioctl_info[_IOC_NR(cmd)];
 
-	case MEDIA_IOC_ENUM_LINKS:
-		ret = media_device_enum_links(dev,
-				(struct media_links_enum __user *)arg);
-		break;
+	if (_IOC_SIZE(info->cmd) > sizeof(__karg)) {
+		karg = kmalloc(_IOC_SIZE(info->cmd), GFP_KERNEL);
+		if (!karg)
+			return -ENOMEM;
+	}
 
-	case MEDIA_IOC_SETUP_LINK:
-		ret = media_device_setup_link(dev,
-				(struct media_link_desc __user *)arg);
-		break;
+	if (info->arg_from_user) {
+		ret = info->arg_from_user(karg, arg, cmd);
+		if (ret)
+			goto out_free;
+	}
 
-	case MEDIA_IOC_G_TOPOLOGY:
-		ret = media_device_get_topology(dev,
-				(struct media_v2_topology __user *)arg);
-		break;
+	if (info->flags & MEDIA_IOC_FL_GRAPH_MUTEX)
+		mutex_lock(&dev->graph_mutex);
 
-	default:
-		ret = -ENOIOCTLCMD;
-	}
-	mutex_unlock(&dev->graph_mutex);
+	ret = info->fn(dev, karg);
+
+	if (info->flags & MEDIA_IOC_FL_GRAPH_MUTEX)
+		mutex_unlock(&dev->graph_mutex);
+
+	if (!ret && info->arg_to_user)
+		ret = info->arg_to_user(arg, karg, cmd);
+
+out_free:
+	if (karg != __karg)
+		kfree(karg);
 
 	return ret;
 }
@@ -486,7 +484,7 @@ static long media_device_enum_links32(struct media_device *mdev,
 	links.pads = compat_ptr(pads_ptr);
 	links.links = compat_ptr(links_ptr);
 
-	return __media_device_enum_links(mdev, &links);
+	return media_device_enum_links(mdev, &links);
 }
 
 #define MEDIA_IOC_ENUM_LINKS32		_IOWR('|', 0x02, struct media_links_enum32)
diff --git a/drivers/media/media-entity.c b/drivers/media/media-entity.c
index d8a2299f0c2a..c68239e60487 100644
--- a/drivers/media/media-entity.c
+++ b/drivers/media/media-entity.c
@@ -65,6 +65,8 @@ static inline const char *intf_type(struct media_interface *intf)
 		return "v4l-subdev";
 	case MEDIA_INTF_T_V4L_SWRADIO:
 		return "v4l-swradio";
+	case MEDIA_INTF_T_V4L_TOUCH:
+		return "v4l-touch";
 	case MEDIA_INTF_T_ALSA_PCM_CAPTURE:
 		return "alsa-pcm-capture";
 	case MEDIA_INTF_T_ALSA_PCM_PLAYBACK:
@@ -806,17 +808,18 @@ int __media_entity_setup_link(struct media_link *link, u32 flags)
 
 	mdev = source->graph_obj.mdev;
 
-	if (mdev->link_notify) {
-		ret = mdev->link_notify(link, flags,
-					MEDIA_DEV_NOTIFY_PRE_LINK_CH);
+	if (mdev->ops && mdev->ops->link_notify) {
+		ret = mdev->ops->link_notify(link, flags,
+					     MEDIA_DEV_NOTIFY_PRE_LINK_CH);
 		if (ret < 0)
 			return ret;
 	}
 
 	ret = __media_entity_setup_link_notify(link, flags);
 
-	if (mdev->link_notify)
-		mdev->link_notify(link, flags, MEDIA_DEV_NOTIFY_POST_LINK_CH);
+	if (mdev->ops && mdev->ops->link_notify)
+		mdev->ops->link_notify(link, flags,
+				       MEDIA_DEV_NOTIFY_POST_LINK_CH);
 
 	return ret;
 }
diff --git a/drivers/media/pci/Kconfig b/drivers/media/pci/Kconfig
index 4f6467fbaeb4..da28e68c87d8 100644
--- a/drivers/media/pci/Kconfig
+++ b/drivers/media/pci/Kconfig
@@ -13,6 +13,7 @@ if MEDIA_CAMERA_SUPPORT
 source "drivers/media/pci/meye/Kconfig"
 source "drivers/media/pci/solo6x10/Kconfig"
 source "drivers/media/pci/sta2x11/Kconfig"
+source "drivers/media/pci/tw5864/Kconfig"
 source "drivers/media/pci/tw68/Kconfig"
 source "drivers/media/pci/tw686x/Kconfig"
 source "drivers/media/pci/zoran/Kconfig"
diff --git a/drivers/media/pci/Makefile b/drivers/media/pci/Makefile
index 2e54c36441f7..a7e8af0f64a7 100644
--- a/drivers/media/pci/Makefile
+++ b/drivers/media/pci/Makefile
@@ -31,3 +31,4 @@ obj-$(CONFIG_VIDEO_MEYE) += meye/
 obj-$(CONFIG_STA2X11_VIP) += sta2x11/
 obj-$(CONFIG_VIDEO_SOLO6X10) += solo6x10/
 obj-$(CONFIG_VIDEO_COBALT) += cobalt/
+obj-$(CONFIG_VIDEO_TW5864) += tw5864/
diff --git a/drivers/media/pci/bt8xx/bttv-driver.c b/drivers/media/pci/bt8xx/bttv-driver.c
index df54e17ef864..97b91a9f9fa9 100644
--- a/drivers/media/pci/bt8xx/bttv-driver.c
+++ b/drivers/media/pci/bt8xx/bttv-driver.c
@@ -2804,30 +2804,44 @@ static int bttv_cropcap(struct file *file, void *priv,
 	    cap->type != V4L2_BUF_TYPE_VIDEO_OVERLAY)
 		return -EINVAL;
 
-	*cap = bttv_tvnorms[btv->tvnorm].cropcap;
+	/* defrect and bounds are set via g_selection */
+	cap->pixelaspect = bttv_tvnorms[btv->tvnorm].cropcap.pixelaspect;
 
 	return 0;
 }
 
-static int bttv_g_crop(struct file *file, void *f, struct v4l2_crop *crop)
+static int bttv_g_selection(struct file *file, void *f, struct v4l2_selection *sel)
 {
 	struct bttv_fh *fh = f;
 	struct bttv *btv = fh->btv;
 
-	if (crop->type != V4L2_BUF_TYPE_VIDEO_CAPTURE &&
-	    crop->type != V4L2_BUF_TYPE_VIDEO_OVERLAY)
+	if (sel->type != V4L2_BUF_TYPE_VIDEO_CAPTURE &&
+	    sel->type != V4L2_BUF_TYPE_VIDEO_OVERLAY)
 		return -EINVAL;
 
-	/* No fh->do_crop = 1; because btv->crop[1] may be
-	   inconsistent with fh->width or fh->height and apps
-	   do not expect a change here. */
-
-	crop->c = btv->crop[!!fh->do_crop].rect;
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP:
+		/*
+		 * No fh->do_crop = 1; because btv->crop[1] may be
+		 * inconsistent with fh->width or fh->height and apps
+		 * do not expect a change here.
+		 */
+		sel->r = btv->crop[!!fh->do_crop].rect;
+		break;
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+		sel->r = bttv_tvnorms[btv->tvnorm].cropcap.defrect;
+		break;
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		sel->r = bttv_tvnorms[btv->tvnorm].cropcap.bounds;
+		break;
+	default:
+		return -EINVAL;
+	}
 
 	return 0;
 }
 
-static int bttv_s_crop(struct file *file, void *f, const struct v4l2_crop *crop)
+static int bttv_s_selection(struct file *file, void *f, struct v4l2_selection *sel)
 {
 	struct bttv_fh *fh = f;
 	struct bttv *btv = fh->btv;
@@ -2839,8 +2853,11 @@ static int bttv_s_crop(struct file *file, void *f, const struct v4l2_crop *crop)
 	__s32 b_right;
 	__s32 b_bottom;
 
-	if (crop->type != V4L2_BUF_TYPE_VIDEO_CAPTURE &&
-	    crop->type != V4L2_BUF_TYPE_VIDEO_OVERLAY)
+	if (sel->type != V4L2_BUF_TYPE_VIDEO_CAPTURE &&
+	    sel->type != V4L2_BUF_TYPE_VIDEO_OVERLAY)
+		return -EINVAL;
+
+	if (sel->target != V4L2_SEL_TGT_CROP)
 		return -EINVAL;
 
 	/* Make sure tvnorm, vbi_end and the current cropping
@@ -2864,22 +2881,24 @@ static int bttv_s_crop(struct file *file, void *f, const struct v4l2_crop *crop)
 	}
 
 	/* Min. scaled size 48 x 32. */
-	c.rect.left = clamp_t(s32, crop->c.left, b_left, b_right - 48);
+	c.rect.left = clamp_t(s32, sel->r.left, b_left, b_right - 48);
 	c.rect.left = min(c.rect.left, (__s32) MAX_HDELAY);
 
-	c.rect.width = clamp_t(s32, crop->c.width,
+	c.rect.width = clamp_t(s32, sel->r.width,
 			     48, b_right - c.rect.left);
 
-	c.rect.top = clamp_t(s32, crop->c.top, b_top, b_bottom - 32);
+	c.rect.top = clamp_t(s32, sel->r.top, b_top, b_bottom - 32);
 	/* Top and height must be a multiple of two. */
 	c.rect.top = (c.rect.top + 1) & ~1;
 
-	c.rect.height = clamp_t(s32, crop->c.height,
+	c.rect.height = clamp_t(s32, sel->r.height,
 			      32, b_bottom - c.rect.top);
 	c.rect.height = (c.rect.height + 1) & ~1;
 
 	bttv_crop_calc_limits(&c);
 
+	sel->r = c.rect;
+
 	btv->crop[1] = c;
 
 	fh->do_crop = 1;
@@ -3047,10 +3066,10 @@ static int bttv_open(struct file *file)
 	   which only change on request. These are stored in btv->crop[1].
 	   However for compatibility with V4L apps and cropping unaware
 	   V4L2 apps we now reset the cropping parameters as seen through
-	   this fh, which is to say VIDIOC_G_CROP and scaling limit checks
+	   this fh, which is to say VIDIOC_G_SELECTION and scaling limit checks
 	   will use btv->crop[0], the default cropping parameters for the
 	   current video standard, and VIDIOC_S_FMT will not implicitely
-	   change the cropping parameters until VIDIOC_S_CROP has been
+	   change the cropping parameters until VIDIOC_S_SELECTION has been
 	   called. */
 	fh->do_crop = !reset_crop; /* module parameter */
 
@@ -3159,8 +3178,8 @@ static const struct v4l2_ioctl_ops bttv_ioctl_ops = {
 	.vidioc_streamoff               = bttv_streamoff,
 	.vidioc_g_tuner                 = bttv_g_tuner,
 	.vidioc_s_tuner                 = bttv_s_tuner,
-	.vidioc_g_crop                  = bttv_g_crop,
-	.vidioc_s_crop                  = bttv_s_crop,
+	.vidioc_g_selection             = bttv_g_selection,
+	.vidioc_s_selection             = bttv_s_selection,
 	.vidioc_g_fbuf                  = bttv_g_fbuf,
 	.vidioc_s_fbuf                  = bttv_s_fbuf,
 	.vidioc_overlay                 = bttv_overlay,
diff --git a/drivers/media/pci/bt8xx/bttvp.h b/drivers/media/pci/bt8xx/bttvp.h
index b1e0023f923c..9efc4559fa8e 100644
--- a/drivers/media/pci/bt8xx/bttvp.h
+++ b/drivers/media/pci/bt8xx/bttvp.h
@@ -232,7 +232,7 @@ struct bttv_fh {
 	const struct bttv_format *ovfmt;
 	struct bttv_overlay      ov;
 
-	/* Application called VIDIOC_S_CROP. */
+	/* Application called VIDIOC_S_SELECTION. */
 	int                      do_crop;
 
 	/* vbi capture */
diff --git a/drivers/media/pci/cobalt/cobalt-alsa-pcm.c b/drivers/media/pci/cobalt/cobalt-alsa-pcm.c
index f0bdf10cfd57..49013c6b8646 100644
--- a/drivers/media/pci/cobalt/cobalt-alsa-pcm.c
+++ b/drivers/media/pci/cobalt/cobalt-alsa-pcm.c
@@ -510,7 +510,7 @@ static struct page *snd_pcm_get_vmalloc_page(struct snd_pcm_substream *subs,
 	return vmalloc_to_page(pageptr);
 }
 
-static struct snd_pcm_ops snd_cobalt_pcm_capture_ops = {
+static const 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,
@@ -522,7 +522,7 @@ static struct snd_pcm_ops snd_cobalt_pcm_capture_ops = {
 	.page		= snd_pcm_get_vmalloc_page,
 };
 
-static struct snd_pcm_ops snd_cobalt_pcm_playback_ops = {
+static const 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,
diff --git a/drivers/media/pci/cobalt/cobalt-driver.c b/drivers/media/pci/cobalt/cobalt-driver.c
index 476f7f0dcf81..979634000597 100644
--- a/drivers/media/pci/cobalt/cobalt-driver.c
+++ b/drivers/media/pci/cobalt/cobalt-driver.c
@@ -60,30 +60,31 @@ 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,
+	0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00,
+	0x50, 0x21, 0x32, 0x27, 0x00, 0x00, 0x00, 0x00,
+	0x22, 0x1a, 0x01, 0x03, 0x80, 0x30, 0x1b, 0x78,
+	0x0f, 0xee, 0x91, 0xa3, 0x54, 0x4c, 0x99, 0x26,
+	0x0f, 0x50, 0x54, 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,
+	0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x02, 0x3a,
+	0x80, 0x18, 0x71, 0x38, 0x2d, 0x40, 0x58, 0x2c,
+	0x46, 0x00, 0xe0, 0x0e, 0x11, 0x00, 0x00, 0x1e,
+	0x00, 0x00, 0x00, 0xfd, 0x00, 0x18, 0x55, 0x18,
+	0x5e, 0x11, 0x00, 0x0a, 0x20, 0x20, 0x20, 0x20,
+	0x20, 0x20, 0x00, 0x00, 0x00, 0xfc, 0x00, 0x63,
+	0x6f, 0x62, 0x61, 0x6c, 0x74, 0x0a, 0x20, 0x20,
+	0x20, 0x20, 0x20, 0x20, 0x00, 0x00, 0x00, 0x10,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x9c,
+
+	0x02, 0x03, 0x1f, 0xf0, 0x4a, 0x90, 0x1f, 0x04,
+	0x13, 0x22, 0x21, 0x20, 0x02, 0x11, 0x01, 0x23,
+	0x09, 0x07, 0x07, 0x68, 0x03, 0x0c, 0x00, 0x10,
+	0x00, 0x00, 0x22, 0x0f, 0xe2, 0x00, 0xea, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	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,
@@ -91,7 +92,7 @@ static u8 edid[256] = {
 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 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
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xa7,
 };
 
 static void cobalt_set_interrupt(struct cobalt *cobalt, bool enable)
diff --git a/drivers/media/pci/cobalt/cobalt-v4l2.c b/drivers/media/pci/cobalt/cobalt-v4l2.c
index d05672fe9ff9..5c76637900d0 100644
--- a/drivers/media/pci/cobalt/cobalt-v4l2.c
+++ b/drivers/media/pci/cobalt/cobalt-v4l2.c
@@ -161,8 +161,11 @@ static void cobalt_enable_output(struct cobalt_stream *s)
 	struct v4l2_subdev_format sd_fmt = {
 		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
 	};
+	u64 clk = bt->pixelclock;
 
-	if (!cobalt_cpld_set_freq(cobalt, bt->pixelclock)) {
+	if (bt->flags & V4L2_DV_FL_REDUCED_FPS)
+		clk = div_u64(clk * 1000ULL, 1001);
+	if (!cobalt_cpld_set_freq(cobalt, clk)) {
 		cobalt_err("pixelclock out of range\n");
 		return;
 	}
@@ -644,7 +647,7 @@ static int cobalt_s_dv_timings(struct file *file, void *priv_fh,
 		return 0;
 	}
 
-	if (v4l2_match_dv_timings(timings, &s->timings, 0, false))
+	if (v4l2_match_dv_timings(timings, &s->timings, 0, true))
 		return 0;
 
 	if (vb2_is_busy(&s->q))
diff --git a/drivers/media/pci/cx18/cx18-alsa-pcm.c b/drivers/media/pci/cx18/cx18-alsa-pcm.c
index ffb6acdc575f..5344510fbea3 100644
--- a/drivers/media/pci/cx18/cx18-alsa-pcm.c
+++ b/drivers/media/pci/cx18/cx18-alsa-pcm.c
@@ -311,7 +311,7 @@ static struct page *snd_pcm_get_vmalloc_page(struct snd_pcm_substream *subs,
 	return vmalloc_to_page(pageptr);
 }
 
-static struct snd_pcm_ops snd_cx18_pcm_capture_ops = {
+static const struct snd_pcm_ops snd_cx18_pcm_capture_ops = {
 	.open		= snd_cx18_pcm_capture_open,
 	.close		= snd_cx18_pcm_capture_close,
 	.ioctl		= snd_cx18_pcm_ioctl,
diff --git a/drivers/media/pci/cx18/cx18-i2c.c b/drivers/media/pci/cx18/cx18-i2c.c
index 4af8cd6df95d..c9329371a3f8 100644
--- a/drivers/media/pci/cx18/cx18-i2c.c
+++ b/drivers/media/pci/cx18/cx18-i2c.c
@@ -98,7 +98,8 @@ static int cx18_i2c_new_ir(struct cx18 *cx, struct i2c_adapter *adap, u32 hw,
 	case CX18_HW_Z8F0811_IR_RX_HAUP:
 		init_data->ir_codes = RC_MAP_HAUPPAUGE;
 		init_data->internal_get_key_func = IR_KBD_GET_KEY_HAUP_XVR;
-		init_data->type = RC_BIT_RC5;
+		init_data->type = RC_BIT_RC5 | RC_BIT_RC6_MCE |
+							RC_BIT_RC6_6A_32;
 		init_data->name = cx->card_name;
 		info.platform_data = init_data;
 		break;
diff --git a/drivers/media/pci/cx23885/altera-ci.h b/drivers/media/pci/cx23885/altera-ci.h
index 6c511723fd1b..57a40c84b46e 100644
--- a/drivers/media/pci/cx23885/altera-ci.h
+++ b/drivers/media/pci/cx23885/altera-ci.h
@@ -20,8 +20,6 @@
 #ifndef __ALTERA_CI_H
 #define __ALTERA_CI_H
 
-#include <linux/kconfig.h>
-
 #define ALT_DATA	0x000000ff
 #define ALT_TDI		0x00008000
 #define ALT_TDO		0x00004000
diff --git a/drivers/media/pci/cx23885/cx23885-417.c b/drivers/media/pci/cx23885/cx23885-417.c
index 4d080da7afaf..da892f3e3c29 100644
--- a/drivers/media/pci/cx23885/cx23885-417.c
+++ b/drivers/media/pci/cx23885/cx23885-417.c
@@ -1223,7 +1223,7 @@ static void cx23885_stop_streaming(struct vb2_queue *q)
 	cx23885_cancel_buffers(&dev->ts1);
 }
 
-static struct vb2_ops cx23885_qops = {
+static const struct vb2_ops cx23885_qops = {
 	.queue_setup    = queue_setup,
 	.buf_prepare  = buffer_prepare,
 	.buf_finish = buffer_finish,
diff --git a/drivers/media/pci/cx23885/cx23885-alsa.c b/drivers/media/pci/cx23885/cx23885-alsa.c
index ae7c2e89ad1c..6115d4e148ba 100644
--- a/drivers/media/pci/cx23885/cx23885-alsa.c
+++ b/drivers/media/pci/cx23885/cx23885-alsa.c
@@ -506,7 +506,7 @@ static struct page *snd_cx23885_page(struct snd_pcm_substream *substream,
 /*
  * operators
  */
-static struct snd_pcm_ops snd_cx23885_pcm_ops = {
+static const struct snd_pcm_ops snd_cx23885_pcm_ops = {
 	.open = snd_cx23885_pcm_open,
 	.close = snd_cx23885_close,
 	.ioctl = snd_pcm_lib_ioctl,
diff --git a/drivers/media/pci/cx23885/cx23885-cards.c b/drivers/media/pci/cx23885/cx23885-cards.c
index 4abf50f2694f..99ba8d6328f0 100644
--- a/drivers/media/pci/cx23885/cx23885-cards.c
+++ b/drivers/media/pci/cx23885/cx23885-cards.c
@@ -770,6 +770,11 @@ struct cx23885_board cx23885_boards[] = {
 		.portb        = CX23885_MPEG_DVB,
 		.portc        = CX23885_MPEG_DVB,
 	},
+	[CX23885_BOARD_HAUPPAUGE_QUADHD_ATSC] = {
+		.name        = "Hauppauge WinTV-QuadHD-ATSC",
+		.portb        = CX23885_MPEG_DVB,
+		.portc        = CX23885_MPEG_DVB,
+	},
 };
 const unsigned int cx23885_bcount = ARRAY_SIZE(cx23885_boards);
 
@@ -1073,6 +1078,14 @@ struct cx23885_subid cx23885_subids[] = {
 		.subvendor = 0x0070,
 		.subdevice = 0x6b28,
 		.card      = CX23885_BOARD_HAUPPAUGE_QUADHD_DVB, /* Tuner Pair 2 */
+	}, {
+		.subvendor = 0x0070,
+		.subdevice = 0x6a18,
+		.card      = CX23885_BOARD_HAUPPAUGE_QUADHD_ATSC, /* Tuner Pair 1 */
+	}, {
+		.subvendor = 0x0070,
+		.subdevice = 0x6b18,
+		.card      = CX23885_BOARD_HAUPPAUGE_QUADHD_ATSC, /* Tuner Pair 2 */
 	},
 };
 const unsigned int cx23885_idcount = ARRAY_SIZE(cx23885_subids);
@@ -1278,6 +1291,18 @@ static void hauppauge_eeprom(struct cx23885_dev *dev, u8 *eeprom_data)
 		/* WinTV-QuadHD (DVB) Tuner Pair 2 (PCIe, IR, half height,
 		   DVB-T/T2/C, DVB-T/T2/C */
 		break;
+	case 165100:
+		/*
+		 * WinTV-QuadHD (ATSC) Tuner Pair 1 (PCIe, IR, half height,
+		 * ATSC, ATSC
+		 */
+		break;
+	case 165101:
+		/*
+		 * WinTV-QuadHD (DVB) Tuner Pair 2 (PCIe, IR, half height,
+		 * ATSC, ATSC
+		 */
+		break;
 	default:
 		printk(KERN_WARNING "%s: warning: "
 			"unknown hauppauge model #%d\n",
@@ -1751,6 +1776,7 @@ void cx23885_gpio_setup(struct cx23885_dev *dev)
 		break;
 	case CX23885_BOARD_HAUPPAUGE_HVR5525:
 	case CX23885_BOARD_HAUPPAUGE_QUADHD_DVB:
+	case CX23885_BOARD_HAUPPAUGE_QUADHD_ATSC:
 		/*
 		 * HVR5525 GPIO Details:
 		 *  GPIO-00 IR_WIDE
@@ -1826,6 +1852,7 @@ int cx23885_ir_init(struct cx23885_dev *dev)
 	case CX23885_BOARD_HAUPPAUGE_HVR1255_22111:
 	case CX23885_BOARD_HAUPPAUGE_HVR1210:
 	case CX23885_BOARD_HAUPPAUGE_QUADHD_DVB:
+	case CX23885_BOARD_HAUPPAUGE_QUADHD_ATSC:
 		/* FIXME: Implement me */
 		break;
 	case CX23885_BOARD_HAUPPAUGE_HVR1270:
@@ -2025,6 +2052,7 @@ void cx23885_card_setup(struct cx23885_dev *dev)
 	case CX23885_BOARD_HAUPPAUGE_IMPACTVCBE:
 	case CX23885_BOARD_HAUPPAUGE_HVR5525:
 	case CX23885_BOARD_HAUPPAUGE_QUADHD_DVB:
+	case CX23885_BOARD_HAUPPAUGE_QUADHD_ATSC:
 		if (dev->i2c_bus[0].i2c_rc == 0)
 			hauppauge_eeprom(dev, eeprom+0xc0);
 		break;
@@ -2171,6 +2199,7 @@ void cx23885_card_setup(struct cx23885_dev *dev)
 		ts2->src_sel_val   = CX23885_SRC_SEL_PARALLEL_MPEG_VIDEO;
 		break;
 	case CX23885_BOARD_HAUPPAUGE_QUADHD_DVB:
+	case CX23885_BOARD_HAUPPAUGE_QUADHD_ATSC:
 		ts1->gen_ctrl_val  = 0xc; /* Serial bus + punctured clock */
 		ts1->ts_clk_en_val = 0x1; /* Enable TS_CLK */
 		ts1->src_sel_val   = CX23885_SRC_SEL_PARALLEL_MPEG_VIDEO;
diff --git a/drivers/media/pci/cx23885/cx23885-dvb.c b/drivers/media/pci/cx23885/cx23885-dvb.c
index e5748a93c479..818f3c2fc98d 100644
--- a/drivers/media/pci/cx23885/cx23885-dvb.c
+++ b/drivers/media/pci/cx23885/cx23885-dvb.c
@@ -74,6 +74,7 @@
 #include "sp2.h"
 #include "m88ds3103.h"
 #include "m88rs6000t.h"
+#include "lgdt3306a.h"
 
 static unsigned int debug;
 
@@ -172,7 +173,7 @@ static void cx23885_stop_streaming(struct vb2_queue *q)
 	cx23885_cancel_buffers(port);
 }
 
-static struct vb2_ops dvb_qops = {
+static const struct vb2_ops dvb_qops = {
 	.queue_setup    = queue_setup,
 	.buf_prepare  = buffer_prepare,
 	.buf_finish = buffer_finish,
@@ -574,6 +575,30 @@ static struct stb6100_config prof_8000_stb6100_config = {
 	.refclock = 27000000,
 };
 
+static struct lgdt3306a_config hauppauge_quadHD_ATSC_a_config = {
+	.i2c_addr               = 0x59,
+	.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_quadHD_ATSC_b_config = {
+	.i2c_addr               = 0x0e,
+	.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 int p8000_set_voltage(struct dvb_frontend *fe,
 			     enum fe_sec_voltage voltage)
 {
@@ -867,12 +892,6 @@ static const struct tda10071_platform_data hauppauge_tda10071_pdata = {
 	.tuner_i2c_addr = 0x54,
 };
 
-static const struct si2165_config hauppauge_hvr4400_si2165_config = {
-	.i2c_addr	= 0x64,
-	.chip_mode	= SI2165_MODE_PLL_XTAL,
-	.ref_freq_Hz	= 16000000,
-};
-
 static const struct m88ds3103_config dvbsky_t9580_m88ds3103_config = {
 	.i2c_addr = 0x68,
 	.clock = 27000000,
@@ -1182,6 +1201,7 @@ static int dvb_register(struct cx23885_tsport *port)
 	struct cx23885_i2c *i2c_bus = NULL, *i2c_bus2 = NULL;
 	struct vb2_dvb_frontend *fe0, *fe1 = NULL;
 	struct si2168_config si2168_config;
+	struct si2165_platform_data si2165_pdata;
 	struct si2157_config si2157_config;
 	struct ts2020_config ts2020_config;
 	struct i2c_board_info info;
@@ -1700,6 +1720,9 @@ static int dvb_register(struct cx23885_tsport *port)
 		}
 		break;
 	case CX23885_BOARD_NETUP_DUAL_DVB_T_C_CI_RF:
+		if (port->nr > 2)
+			return 0;
+
 		i2c_bus = &dev->i2c_bus[0];
 		mfe_shared = 1;/* MFE */
 		port->frontends.gate = 0;/* not clear for me yet */
@@ -1839,9 +1862,26 @@ static int dvb_register(struct cx23885_tsport *port)
 			break;
 		/* port c */
 		case 2:
-			fe0->dvb.frontend = dvb_attach(si2165_attach,
-					&hauppauge_hvr4400_si2165_config,
-					&i2c_bus->i2c_adap);
+			/* attach frontend */
+			memset(&si2165_pdata, 0, sizeof(si2165_pdata));
+			si2165_pdata.fe = &fe0->dvb.frontend;
+			si2165_pdata.chip_mode = SI2165_MODE_PLL_XTAL,
+			si2165_pdata.ref_freq_Hz = 16000000,
+			memset(&info, 0, sizeof(struct i2c_board_info));
+			strlcpy(info.type, "si2165", I2C_NAME_SIZE);
+			info.addr = 0x64;
+			info.platform_data = &si2165_pdata;
+			request_module(info.type);
+			client_demod = i2c_new_device(&i2c_bus->i2c_adap, &info);
+			if (client_demod == NULL ||
+					client_demod->dev.driver == NULL)
+				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;
+
 			if (fe0->dvb.frontend == NULL)
 				break;
 			fe0->dvb.frontend->ops.i2c_gate_ctrl = NULL;
@@ -2365,6 +2405,81 @@ static int dvb_register(struct cx23885_tsport *port)
 			break;
 		}
 		break;
+	case CX23885_BOARD_HAUPPAUGE_QUADHD_ATSC:
+		switch (port->nr) {
+		/* port b - Terrestrial/cable */
+		case 1:
+			/* attach frontend */
+			i2c_bus = &dev->i2c_bus[0];
+			fe0->dvb.frontend = dvb_attach(lgdt3306a_attach,
+				&hauppauge_quadHD_ATSC_a_config, &i2c_bus->i2c_adap);
+			if (fe0->dvb.frontend == NULL)
+				break;
+
+			/* attach tuner */
+			memset(&si2157_config, 0, sizeof(si2157_config));
+			si2157_config.fe = fe0->dvb.frontend;
+			si2157_config.if_port = 1;
+			si2157_config.inversion = 1;
+			memset(&info, 0, sizeof(struct i2c_board_info));
+			strlcpy(info.type, "si2157", I2C_NAME_SIZE);
+			info.addr = 0x60;
+			info.platform_data = &si2157_config;
+			request_module("%s", 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);
+				port->i2c_client_demod = NULL;
+				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);
+				port->i2c_client_demod = NULL;
+				goto frontend_detach;
+			}
+			port->i2c_client_tuner = client_tuner;
+			break;
+
+		/* port c - terrestrial/cable */
+		case 2:
+			/* attach frontend */
+			i2c_bus = &dev->i2c_bus[0];
+			fe0->dvb.frontend = dvb_attach(lgdt3306a_attach,
+				&hauppauge_quadHD_ATSC_b_config, &i2c_bus->i2c_adap);
+			if (fe0->dvb.frontend == NULL)
+				break;
+
+			/* attach tuner */
+			memset(&si2157_config, 0, sizeof(si2157_config));
+			si2157_config.fe = fe0->dvb.frontend;
+			si2157_config.if_port = 1;
+			si2157_config.inversion = 1;
+			memset(&info, 0, sizeof(struct i2c_board_info));
+			strlcpy(info.type, "si2157", I2C_NAME_SIZE);
+			info.addr = 0x62;
+			info.platform_data = &si2157_config;
+			request_module("%s", 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);
+				port->i2c_client_demod = NULL;
+				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);
+				port->i2c_client_demod = NULL;
+				goto frontend_detach;
+			}
+			port->i2c_client_tuner = client_tuner;
+			break;
+		}
+		break;
 
 	default:
 		printk(KERN_INFO "%s: The frontend of your DVB/ATSC card "
diff --git a/drivers/media/pci/cx23885/cx23885-i2c.c b/drivers/media/pci/cx23885/cx23885-i2c.c
index ae061b358591..61591225be9a 100644
--- a/drivers/media/pci/cx23885/cx23885-i2c.c
+++ b/drivers/media/pci/cx23885/cx23885-i2c.c
@@ -258,7 +258,7 @@ static u32 cx23885_functionality(struct i2c_adapter *adap)
 	return I2C_FUNC_SMBUS_EMUL | I2C_FUNC_I2C;
 }
 
-static struct i2c_algorithm cx23885_i2c_algo_template = {
+static const struct i2c_algorithm cx23885_i2c_algo_template = {
 	.master_xfer	= i2c_xfer,
 	.functionality	= cx23885_functionality,
 };
diff --git a/drivers/media/pci/cx23885/cx23885-input.c b/drivers/media/pci/cx23885/cx23885-input.c
index 64328d08ac2f..410c3141c163 100644
--- a/drivers/media/pci/cx23885/cx23885-input.c
+++ b/drivers/media/pci/cx23885/cx23885-input.c
@@ -293,7 +293,7 @@ int cx23885_input_init(struct cx23885_dev *dev)
 	case CX23885_BOARD_TERRATEC_CINERGY_T_PCIE_DUAL:
 		/* Integrated CX23885 IR controller */
 		driver_type = RC_DRIVER_IR_RAW;
-		allowed_protos = RC_BIT_NEC;
+		allowed_protos = RC_BIT_ALL;
 		/* The grey Terratec remote with orange buttons */
 		rc_map = RC_MAP_NEC_TERRATEC_CINERGY_XS;
 		break;
diff --git a/drivers/media/pci/cx23885/cx23885-video.c b/drivers/media/pci/cx23885/cx23885-video.c
index 6d735222a958..33d168ef278d 100644
--- a/drivers/media/pci/cx23885/cx23885-video.c
+++ b/drivers/media/pci/cx23885/cx23885-video.c
@@ -517,7 +517,7 @@ static void cx23885_stop_streaming(struct vb2_queue *q)
 	spin_unlock_irqrestore(&dev->slock, flags);
 }
 
-static struct vb2_ops cx23885_video_qops = {
+static const struct vb2_ops cx23885_video_qops = {
 	.queue_setup    = queue_setup,
 	.buf_prepare  = buffer_prepare,
 	.buf_finish = buffer_finish,
diff --git a/drivers/media/pci/cx23885/cx23885.h b/drivers/media/pci/cx23885/cx23885.h
index 24a0a6c5b501..a6735afe2269 100644
--- a/drivers/media/pci/cx23885/cx23885.h
+++ b/drivers/media/pci/cx23885/cx23885.h
@@ -103,7 +103,8 @@
 #define CX23885_BOARD_HAUPPAUGE_STARBURST      53
 #define CX23885_BOARD_VIEWCAST_260E            54
 #define CX23885_BOARD_VIEWCAST_460E            55
-#define CX23885_BOARD_HAUPPAUGE_QUADHD_DVB    56
+#define CX23885_BOARD_HAUPPAUGE_QUADHD_DVB     56
+#define CX23885_BOARD_HAUPPAUGE_QUADHD_ATSC    57
 
 #define GPIO_0 0x00000001
 #define GPIO_1 0x00000002
@@ -256,7 +257,7 @@ struct cx23885_dmaqueue {
 struct cx23885_tsport {
 	struct cx23885_dev *dev;
 
-	int                        nr;
+	unsigned                   nr;
 	int                        sram_chno;
 
 	struct vb2_dvb_frontends   frontends;
diff --git a/drivers/media/pci/cx25821/cx25821-alsa.c b/drivers/media/pci/cx25821/cx25821-alsa.c
index df189b16af12..4711583de8fe 100644
--- a/drivers/media/pci/cx25821/cx25821-alsa.c
+++ b/drivers/media/pci/cx25821/cx25821-alsa.c
@@ -649,7 +649,7 @@ static struct page *snd_cx25821_page(struct snd_pcm_substream *substream,
 /*
  * operators
  */
-static struct snd_pcm_ops snd_cx25821_pcm_ops = {
+static const struct snd_pcm_ops snd_cx25821_pcm_ops = {
 	.open = snd_cx25821_pcm_open,
 	.close = snd_cx25821_close,
 	.ioctl = snd_pcm_lib_ioctl,
diff --git a/drivers/media/pci/cx25821/cx25821-audio-upstream.c b/drivers/media/pci/cx25821/cx25821-audio-upstream.c
index 68dbc2dbc982..7c8edb6181ec 100644
--- a/drivers/media/pci/cx25821/cx25821-audio-upstream.c
+++ b/drivers/media/pci/cx25821/cx25821-audio-upstream.c
@@ -242,8 +242,7 @@ void cx25821_stop_upstream_audio(struct cx25821_dev *dev)
 	dev->_audioframe_count = 0;
 	dev->_audiofile_status = END_OF_FILE;
 
-	kfree(dev->_irq_audio_queues);
-	dev->_irq_audio_queues = NULL;
+	flush_work(&dev->_audio_work_entry);
 
 	kfree(dev->_audiofilename);
 }
@@ -446,8 +445,7 @@ static int cx25821_audio_upstream_irq(struct cx25821_dev *dev, int chan_num,
 
 			dev->_audioframe_index = dev->_last_index_irq;
 
-			queue_work(dev->_irq_audio_queues,
-				   &dev->_audio_work_entry);
+			schedule_work(&dev->_audio_work_entry);
 		}
 
 		if (dev->_is_first_audio_frame) {
@@ -639,14 +637,6 @@ int cx25821_audio_upstream_init(struct cx25821_dev *dev, int channel_select)
 
 	/* Work queue */
 	INIT_WORK(&dev->_audio_work_entry, cx25821_audioups_handler);
-	dev->_irq_audio_queues =
-	    create_singlethread_workqueue("cx25821_audioworkqueue");
-
-	if (!dev->_irq_audio_queues) {
-		printk(KERN_DEBUG
-			pr_fmt("ERROR: create_singlethread_workqueue() for Audio FAILED!\n"));
-		return -ENOMEM;
-	}
 
 	dev->_last_index_irq = 0;
 	dev->_audio_is_running = 0;
diff --git a/drivers/media/pci/cx25821/cx25821-i2c.c b/drivers/media/pci/cx25821/cx25821-i2c.c
index dca37c7dba73..63ba25b82692 100644
--- a/drivers/media/pci/cx25821/cx25821-i2c.c
+++ b/drivers/media/pci/cx25821/cx25821-i2c.c
@@ -281,7 +281,7 @@ static u32 cx25821_functionality(struct i2c_adapter *adap)
 		I2C_FUNC_SMBUS_READ_WORD_DATA | I2C_FUNC_SMBUS_WRITE_WORD_DATA;
 }
 
-static struct i2c_algorithm cx25821_i2c_algo_template = {
+static const struct i2c_algorithm cx25821_i2c_algo_template = {
 	.master_xfer = i2c_xfer,
 	.functionality = cx25821_functionality,
 #ifdef NEED_ALGO_CONTROL
diff --git a/drivers/media/pci/cx25821/cx25821-video.c b/drivers/media/pci/cx25821/cx25821-video.c
index adcd09be347d..7ce352a0f2d3 100644
--- a/drivers/media/pci/cx25821/cx25821-video.c
+++ b/drivers/media/pci/cx25821/cx25821-video.c
@@ -307,7 +307,7 @@ static void cx25821_stop_streaming(struct vb2_queue *q)
 	spin_unlock_irqrestore(&dev->slock, flags);
 }
 
-static struct vb2_ops cx25821_video_qops = {
+static const struct vb2_ops cx25821_video_qops = {
 	.queue_setup    = cx25821_queue_setup,
 	.buf_prepare  = cx25821_buffer_prepare,
 	.buf_finish = cx25821_buffer_finish,
diff --git a/drivers/media/pci/cx25821/cx25821.h b/drivers/media/pci/cx25821/cx25821.h
index 35c7375e4617..ef61dea982e8 100644
--- a/drivers/media/pci/cx25821/cx25821.h
+++ b/drivers/media/pci/cx25821/cx25821.h
@@ -293,7 +293,6 @@ struct cx25821_dev {
 	u32 audio_upstream_riscbuf_size;
 	u32 audio_upstream_databuf_size;
 	int _audioframe_index;
-	struct workqueue_struct *_irq_audio_queues;
 	struct work_struct _audio_work_entry;
 	char *input_audiofilename;
 
diff --git a/drivers/media/pci/cx88/cx88-alsa.c b/drivers/media/pci/cx88/cx88-alsa.c
index f3f13eb0c16e..723f06462104 100644
--- a/drivers/media/pci/cx88/cx88-alsa.c
+++ b/drivers/media/pci/cx88/cx88-alsa.c
@@ -599,7 +599,7 @@ static struct page *snd_cx88_page(struct snd_pcm_substream *substream,
 /*
  * operators
  */
-static struct snd_pcm_ops snd_cx88_pcm_ops = {
+static const struct snd_pcm_ops snd_cx88_pcm_ops = {
 	.open = snd_cx88_pcm_open,
 	.close = snd_cx88_close,
 	.ioctl = snd_pcm_lib_ioctl,
diff --git a/drivers/media/pci/cx88/cx88-blackbird.c b/drivers/media/pci/cx88/cx88-blackbird.c
index 04fe9af2a802..b532e49e8f33 100644
--- a/drivers/media/pci/cx88/cx88-blackbird.c
+++ b/drivers/media/pci/cx88/cx88-blackbird.c
@@ -756,7 +756,7 @@ static void stop_streaming(struct vb2_queue *q)
 	spin_unlock_irqrestore(&dev->slock, flags);
 }
 
-static struct vb2_ops blackbird_qops = {
+static const struct vb2_ops blackbird_qops = {
 	.queue_setup    = queue_setup,
 	.buf_prepare  = buffer_prepare,
 	.buf_finish = buffer_finish,
diff --git a/drivers/media/pci/cx88/cx88-dvb.c b/drivers/media/pci/cx88/cx88-dvb.c
index 5bb63e7a5691..ac2392d8887a 100644
--- a/drivers/media/pci/cx88/cx88-dvb.c
+++ b/drivers/media/pci/cx88/cx88-dvb.c
@@ -156,7 +156,7 @@ static void stop_streaming(struct vb2_queue *q)
 	spin_unlock_irqrestore(&dev->slock, flags);
 }
 
-static struct vb2_ops dvb_qops = {
+static const struct vb2_ops dvb_qops = {
 	.queue_setup    = queue_setup,
 	.buf_prepare  = buffer_prepare,
 	.buf_finish = buffer_finish,
diff --git a/drivers/media/pci/cx88/cx88-input.c b/drivers/media/pci/cx88/cx88-input.c
index 3f1342c98b46..cd7687183381 100644
--- a/drivers/media/pci/cx88/cx88-input.c
+++ b/drivers/media/pci/cx88/cx88-input.c
@@ -144,7 +144,8 @@ static void cx88_ir_handle_key(struct cx88_IR *ir)
 		scancode = RC_SCANCODE_NECX(addr, cmd);
 
 		if (0 == (gpio & ir->mask_keyup))
-			rc_keydown_notimeout(ir->dev, RC_TYPE_NEC, scancode, 0);
+			rc_keydown_notimeout(ir->dev, RC_TYPE_NECX, scancode,
+									0);
 		else
 			rc_keyup(ir->dev);
 
@@ -345,7 +346,7 @@ int cx88_ir_init(struct cx88_core *core, struct pci_dev *pci)
 		 * 002-T mini RC, provided with newer PV hardware
 		 */
 		ir_codes = RC_MAP_PIXELVIEW_MK12;
-		rc_type = RC_BIT_NEC;
+		rc_type = RC_BIT_NECX;
 		ir->gpio_addr = MO_GP1_IO;
 		ir->mask_keyup = 0x80;
 		ir->polling = 10; /* ms */
@@ -631,7 +632,8 @@ void cx88_i2c_init_ir(struct cx88_core *core)
 			/* Hauppauge XVR */
 			core->init_data.name = "cx88 Hauppauge XVR remote";
 			core->init_data.ir_codes = RC_MAP_HAUPPAUGE;
-			core->init_data.type = RC_BIT_RC5;
+			core->init_data.type = RC_BIT_RC5 | RC_BIT_RC6_MCE |
+							RC_BIT_RC6_6A_32;
 			core->init_data.internal_get_key_func = IR_KBD_GET_KEY_HAUP_XVR;
 
 			info.platform_data = &core->init_data;
diff --git a/drivers/media/pci/cx88/cx88-video.c b/drivers/media/pci/cx88/cx88-video.c
index 5dc1e3f08d50..d83eb3b10f54 100644
--- a/drivers/media/pci/cx88/cx88-video.c
+++ b/drivers/media/pci/cx88/cx88-video.c
@@ -567,7 +567,7 @@ static void stop_streaming(struct vb2_queue *q)
 	spin_unlock_irqrestore(&dev->slock, flags);
 }
 
-static struct vb2_ops cx8800_video_qops = {
+static const struct vb2_ops cx8800_video_qops = {
 	.queue_setup    = queue_setup,
 	.buf_prepare  = buffer_prepare,
 	.buf_finish = buffer_finish,
diff --git a/drivers/media/pci/ddbridge/ddbridge-core.c b/drivers/media/pci/ddbridge/ddbridge-core.c
index 47def73b3502..18e3a4deee64 100644
--- a/drivers/media/pci/ddbridge/ddbridge-core.c
+++ b/drivers/media/pci/ddbridge/ddbridge-core.c
@@ -1643,53 +1643,53 @@ fail:
 /******************************************************************************/
 /******************************************************************************/
 
-static struct ddb_info ddb_none = {
+static const struct ddb_info ddb_none = {
 	.type     = DDB_NONE,
 	.name     = "Digital Devices PCIe bridge",
 };
 
-static struct ddb_info ddb_octopus = {
+static const struct ddb_info ddb_octopus = {
 	.type     = DDB_OCTOPUS,
 	.name     = "Digital Devices Octopus DVB adapter",
 	.port_num = 4,
 };
 
-static struct ddb_info ddb_octopus_le = {
+static const struct ddb_info ddb_octopus_le = {
 	.type     = DDB_OCTOPUS,
 	.name     = "Digital Devices Octopus LE DVB adapter",
 	.port_num = 2,
 };
 
-static struct ddb_info ddb_octopus_mini = {
+static const struct ddb_info ddb_octopus_mini = {
 	.type     = DDB_OCTOPUS,
 	.name     = "Digital Devices Octopus Mini",
 	.port_num = 4,
 };
 
-static struct ddb_info ddb_v6 = {
+static const struct ddb_info ddb_v6 = {
 	.type     = DDB_OCTOPUS,
 	.name     = "Digital Devices Cine S2 V6 DVB adapter",
 	.port_num = 3,
 };
-static struct ddb_info ddb_v6_5 = {
+static const struct ddb_info ddb_v6_5 = {
 	.type     = DDB_OCTOPUS,
 	.name     = "Digital Devices Cine S2 V6.5 DVB adapter",
 	.port_num = 4,
 };
 
-static struct ddb_info ddb_dvbct = {
+static const struct ddb_info ddb_dvbct = {
 	.type     = DDB_OCTOPUS,
 	.name     = "Digital Devices DVBCT V6.1 DVB adapter",
 	.port_num = 3,
 };
 
-static struct ddb_info ddb_satixS2v3 = {
+static const struct ddb_info ddb_satixS2v3 = {
 	.type     = DDB_OCTOPUS,
 	.name     = "Mystique SaTiX-S2 V3 DVB adapter",
 	.port_num = 3,
 };
 
-static struct ddb_info ddb_octopusv3 = {
+static const struct ddb_info ddb_octopusv3 = {
 	.type     = DDB_OCTOPUS,
 	.name     = "Digital Devices Octopus V3 DVB adapter",
 	.port_num = 4,
diff --git a/drivers/media/pci/ivtv/ivtv-alsa-pcm.c b/drivers/media/pci/ivtv/ivtv-alsa-pcm.c
index f198b9826ed8..a26f9800eca3 100644
--- a/drivers/media/pci/ivtv/ivtv-alsa-pcm.c
+++ b/drivers/media/pci/ivtv/ivtv-alsa-pcm.c
@@ -318,7 +318,7 @@ static struct page *snd_pcm_get_vmalloc_page(struct snd_pcm_substream *subs,
 	return vmalloc_to_page(pageptr);
 }
 
-static struct snd_pcm_ops snd_ivtv_pcm_capture_ops = {
+static const struct snd_pcm_ops snd_ivtv_pcm_capture_ops = {
 	.open		= snd_ivtv_pcm_capture_open,
 	.close		= snd_ivtv_pcm_capture_close,
 	.ioctl		= snd_ivtv_pcm_ioctl,
diff --git a/drivers/media/pci/ivtv/ivtv-driver.c b/drivers/media/pci/ivtv/ivtv-driver.c
index 374033a5bdaf..ee48c3e09de4 100644
--- a/drivers/media/pci/ivtv/ivtv-driver.c
+++ b/drivers/media/pci/ivtv/ivtv-driver.c
@@ -750,7 +750,7 @@ static int ivtv_init_struct1(struct ivtv *itv)
 	spin_lock_init(&itv->lock);
 	spin_lock_init(&itv->dma_reg_lock);
 
-	init_kthread_worker(&itv->irq_worker);
+	kthread_init_worker(&itv->irq_worker);
 	itv->irq_worker_task = kthread_run(kthread_worker_fn, &itv->irq_worker,
 					   "%s", itv->v4l2_dev.name);
 	if (IS_ERR(itv->irq_worker_task)) {
@@ -760,7 +760,7 @@ static int ivtv_init_struct1(struct ivtv *itv)
 	/* must use the FIFO scheduler as it is realtime sensitive */
 	sched_setscheduler(itv->irq_worker_task, SCHED_FIFO, &param);
 
-	init_kthread_work(&itv->irq_work, ivtv_irq_work_handler);
+	kthread_init_work(&itv->irq_work, ivtv_irq_work_handler);
 
 	/* Initial settings */
 	itv->cxhdl.port = CX2341X_PORT_MEMORY;
@@ -1441,7 +1441,7 @@ static void ivtv_remove(struct pci_dev *pdev)
 	del_timer_sync(&itv->dma_timer);
 
 	/* Kill irq worker */
-	flush_kthread_worker(&itv->irq_worker);
+	kthread_flush_worker(&itv->irq_worker);
 	kthread_stop(itv->irq_worker_task);
 
 	ivtv_streams_cleanup(itv);
diff --git a/drivers/media/pci/ivtv/ivtv-i2c.c b/drivers/media/pci/ivtv/ivtv-i2c.c
index bccbf2d18e30..dea80efd5836 100644
--- a/drivers/media/pci/ivtv/ivtv-i2c.c
+++ b/drivers/media/pci/ivtv/ivtv-i2c.c
@@ -215,7 +215,8 @@ static int ivtv_i2c_new_ir(struct ivtv *itv, u32 hw, const char *type, u8 addr)
 		/* Default to grey remote */
 		init_data->ir_codes = RC_MAP_HAUPPAUGE;
 		init_data->internal_get_key_func = IR_KBD_GET_KEY_HAUP_XVR;
-		init_data->type = RC_BIT_RC5;
+		init_data->type = RC_BIT_RC5 | RC_BIT_RC6_MCE |
+							RC_BIT_RC6_6A_32;
 		init_data->name = itv->card_name;
 		break;
 	case IVTV_HW_I2C_IR_RX_ADAPTEC:
@@ -625,7 +626,7 @@ static u32 ivtv_functionality(struct i2c_adapter *adap)
 	return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL;
 }
 
-static struct i2c_algorithm ivtv_algo = {
+static const struct i2c_algorithm ivtv_algo = {
 	.master_xfer   = ivtv_xfer,
 	.functionality = ivtv_functionality,
 };
diff --git a/drivers/media/pci/ivtv/ivtv-irq.c b/drivers/media/pci/ivtv/ivtv-irq.c
index 36ca2d67c812..6efe1f71262c 100644
--- a/drivers/media/pci/ivtv/ivtv-irq.c
+++ b/drivers/media/pci/ivtv/ivtv-irq.c
@@ -1062,7 +1062,7 @@ irqreturn_t ivtv_irq_handler(int irq, void *dev_id)
 	}
 
 	if (test_and_clear_bit(IVTV_F_I_HAVE_WORK, &itv->i_flags)) {
-		queue_kthread_work(&itv->irq_worker, &itv->irq_work);
+		kthread_queue_work(&itv->irq_worker, &itv->irq_work);
 	}
 
 	spin_unlock(&itv->dma_reg_lock);
diff --git a/drivers/media/pci/ivtv/ivtv-udma.c b/drivers/media/pci/ivtv/ivtv-udma.c
index 4769469fe842..2c9232ef7baa 100644
--- a/drivers/media/pci/ivtv/ivtv-udma.c
+++ b/drivers/media/pci/ivtv/ivtv-udma.c
@@ -124,8 +124,8 @@ int ivtv_udma_setup(struct ivtv *itv, unsigned long ivtv_dest_addr,
 	}
 
 	/* Get user pages for DMA Xfer */
-	err = get_user_pages_unlocked(user_dma.uaddr, user_dma.page_count, 0,
-			1, dma->map);
+	err = get_user_pages_unlocked(user_dma.uaddr, user_dma.page_count,
+			dma->map, FOLL_FORCE);
 
 	if (user_dma.page_count != err) {
 		IVTV_DEBUG_WARN("failed to map user pages, returned %d instead of %d\n",
diff --git a/drivers/media/pci/ivtv/ivtv-yuv.c b/drivers/media/pci/ivtv/ivtv-yuv.c
index b094054cda6e..f7299d3d8244 100644
--- a/drivers/media/pci/ivtv/ivtv-yuv.c
+++ b/drivers/media/pci/ivtv/ivtv-yuv.c
@@ -76,11 +76,12 @@ static int ivtv_yuv_prep_user_dma(struct ivtv *itv, struct ivtv_user_dma *dma,
 
 	/* Get user pages for DMA Xfer */
 	y_pages = get_user_pages_unlocked(y_dma.uaddr,
-			y_dma.page_count, 0, 1, &dma->map[0]);
+			y_dma.page_count, &dma->map[0], FOLL_FORCE);
 	uv_pages = 0; /* silence gcc. value is set and consumed only if: */
 	if (y_pages == y_dma.page_count) {
 		uv_pages = get_user_pages_unlocked(uv_dma.uaddr,
-				uv_dma.page_count, 0, 1, &dma->map[y_pages]);
+				uv_dma.page_count, &dma->map[y_pages],
+				FOLL_FORCE);
 	}
 
 	if (y_pages != y_dma.page_count || uv_pages != uv_dma.page_count) {
diff --git a/drivers/media/pci/netup_unidvb/netup_unidvb_core.c b/drivers/media/pci/netup_unidvb/netup_unidvb_core.c
index ac547cb84de8..b078ac2a682c 100644
--- a/drivers/media/pci/netup_unidvb/netup_unidvb_core.c
+++ b/drivers/media/pci/netup_unidvb/netup_unidvb_core.c
@@ -353,7 +353,7 @@ static void netup_unidvb_stop_streaming(struct vb2_queue *q)
 	netup_unidvb_queue_cleanup(dma);
 }
 
-static struct vb2_ops dvb_qops = {
+static const struct vb2_ops dvb_qops = {
 	.queue_setup		= netup_unidvb_queue_setup,
 	.buf_prepare		= netup_unidvb_buf_prepare,
 	.buf_queue		= netup_unidvb_buf_queue,
diff --git a/drivers/media/pci/netup_unidvb/netup_unidvb_i2c.c b/drivers/media/pci/netup_unidvb/netup_unidvb_i2c.c
index c09c52bc6eab..b49e4f9788e8 100644
--- a/drivers/media/pci/netup_unidvb/netup_unidvb_i2c.c
+++ b/drivers/media/pci/netup_unidvb/netup_unidvb_i2c.c
@@ -327,11 +327,8 @@ static int netup_i2c_init(struct netup_unidvb_dev *ndev, int bus_num)
 	i2c->adap.dev.parent = &ndev->pci_dev->dev;
 	i2c_set_adapdata(&i2c->adap, i2c);
 	ret = i2c_add_adapter(&i2c->adap);
-	if (ret) {
-		dev_err(&ndev->pci_dev->dev,
-			"%s(): failed to add I2C adapter\n", __func__);
+	if (ret)
 		return ret;
-	}
 	dev_info(&ndev->pci_dev->dev,
 		"%s(): registered I2C bus %d at 0x%x\n",
 		__func__,
diff --git a/drivers/media/pci/ngene/ngene-cards.c b/drivers/media/pci/ngene/ngene-cards.c
index 4e783a68bf4a..423e8c889310 100644
--- a/drivers/media/pci/ngene/ngene-cards.c
+++ b/drivers/media/pci/ngene/ngene-cards.c
@@ -613,7 +613,7 @@ static struct stv6110x_config tuner_cineS2_1 = {
 	.clk_div = 1,
 };
 
-static struct ngene_info ngene_info_cineS2 = {
+static const struct ngene_info ngene_info_cineS2 = {
 	.type		= NGENE_SIDEWINDER,
 	.name		= "Linux4Media cineS2 DVB-S2 Twin Tuner",
 	.io_type	= {NGENE_IO_TSIN, NGENE_IO_TSIN},
@@ -627,7 +627,7 @@ static struct ngene_info ngene_info_cineS2 = {
 	.msi_supported	= true,
 };
 
-static struct ngene_info ngene_info_satixS2 = {
+static const struct ngene_info ngene_info_satixS2 = {
 	.type		= NGENE_SIDEWINDER,
 	.name		= "Mystique SaTiX-S2 Dual",
 	.io_type	= {NGENE_IO_TSIN, NGENE_IO_TSIN},
@@ -641,7 +641,7 @@ static struct ngene_info ngene_info_satixS2 = {
 	.msi_supported	= true,
 };
 
-static struct ngene_info ngene_info_satixS2v2 = {
+static const struct ngene_info ngene_info_satixS2v2 = {
 	.type		= NGENE_SIDEWINDER,
 	.name		= "Mystique SaTiX-S2 Dual (v2)",
 	.io_type	= {NGENE_IO_TSIN, NGENE_IO_TSIN, NGENE_IO_TSIN, NGENE_IO_TSIN,
@@ -656,7 +656,7 @@ static struct ngene_info ngene_info_satixS2v2 = {
 	.msi_supported	= true,
 };
 
-static struct ngene_info ngene_info_cineS2v5 = {
+static const struct ngene_info ngene_info_cineS2v5 = {
 	.type		= NGENE_SIDEWINDER,
 	.name		= "Linux4Media cineS2 DVB-S2 Twin Tuner (v5)",
 	.io_type	= {NGENE_IO_TSIN, NGENE_IO_TSIN, NGENE_IO_TSIN, NGENE_IO_TSIN,
@@ -672,7 +672,7 @@ static struct ngene_info ngene_info_cineS2v5 = {
 };
 
 
-static struct ngene_info ngene_info_duoFlex = {
+static const struct ngene_info ngene_info_duoFlex = {
 	.type           = NGENE_SIDEWINDER,
 	.name           = "Digital Devices DuoFlex PCIe or miniPCIe",
 	.io_type        = {NGENE_IO_TSIN, NGENE_IO_TSIN, NGENE_IO_TSIN, NGENE_IO_TSIN,
@@ -687,7 +687,7 @@ static struct ngene_info ngene_info_duoFlex = {
 	.msi_supported	= true,
 };
 
-static struct ngene_info ngene_info_m780 = {
+static const struct ngene_info ngene_info_m780 = {
 	.type           = NGENE_APP,
 	.name           = "Aver M780 ATSC/QAM-B",
 
@@ -727,7 +727,7 @@ static struct drxd_config fe_terratec_dvbt_1 = {
 	.osc_deviation  = osc_deviation,
 };
 
-static struct ngene_info ngene_info_terratec = {
+static const struct ngene_info ngene_info_terratec = {
 	.type           = NGENE_TERRATEC,
 	.name           = "Terratec Integra/Cinergy2400i Dual DVB-T",
 	.io_type        = {NGENE_IO_TSIN, NGENE_IO_TSIN},
diff --git a/drivers/media/pci/pt3/pt3.c b/drivers/media/pci/pt3/pt3.c
index eff5e9f51ace..7fb649e523f4 100644
--- a/drivers/media/pci/pt3/pt3.c
+++ b/drivers/media/pci/pt3/pt3.c
@@ -798,10 +798,8 @@ static int pt3_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	strlcpy(i2c->name, DRV_NAME, sizeof(i2c->name));
 	i2c_set_adapdata(i2c, pt3);
 	ret = i2c_add_adapter(i2c);
-	if (ret < 0) {
-		dev_err(&pdev->dev, "Failed to add i2c adapter\n");
+	if (ret < 0)
 		goto err_i2cbuf;
-	}
 
 	for (i = 0; i < PT3_NUM_FE; i++) {
 		ret = pt3_alloc_adapter(pt3, i);
diff --git a/drivers/media/pci/saa7134/saa7134-alsa.c b/drivers/media/pci/saa7134/saa7134-alsa.c
index 94f816244407..dc0e2fc5f68b 100644
--- a/drivers/media/pci/saa7134/saa7134-alsa.c
+++ b/drivers/media/pci/saa7134/saa7134-alsa.c
@@ -877,7 +877,7 @@ static struct page *snd_card_saa7134_page(struct snd_pcm_substream *substream,
  * ALSA capture callbacks definition
  */
 
-static struct snd_pcm_ops snd_card_saa7134_capture_ops = {
+static const struct snd_pcm_ops snd_card_saa7134_capture_ops = {
 	.open =			snd_card_saa7134_capture_open,
 	.close =		snd_card_saa7134_capture_close,
 	.ioctl =		snd_pcm_lib_ioctl,
diff --git a/drivers/media/pci/saa7134/saa7134-empress.c b/drivers/media/pci/saa7134/saa7134-empress.c
index 791a5161809b..f0fe2524259f 100644
--- a/drivers/media/pci/saa7134/saa7134-empress.c
+++ b/drivers/media/pci/saa7134/saa7134-empress.c
@@ -85,7 +85,7 @@ static void stop_streaming(struct vb2_queue *vq)
 	dev->empress_started = 0;
 }
 
-static struct vb2_ops saa7134_empress_qops = {
+static const struct vb2_ops saa7134_empress_qops = {
 	.queue_setup	= saa7134_ts_queue_setup,
 	.buf_init	= saa7134_ts_buffer_init,
 	.buf_prepare	= saa7134_ts_buffer_prepare,
diff --git a/drivers/media/pci/saa7134/saa7134-i2c.c b/drivers/media/pci/saa7134/saa7134-i2c.c
index 8ef6399d794f..2dac48fa1386 100644
--- a/drivers/media/pci/saa7134/saa7134-i2c.c
+++ b/drivers/media/pci/saa7134/saa7134-i2c.c
@@ -338,7 +338,7 @@ static u32 functionality(struct i2c_adapter *adap)
 	return I2C_FUNC_SMBUS_EMUL;
 }
 
-static struct i2c_algorithm saa7134_algo = {
+static const struct i2c_algorithm saa7134_algo = {
 	.master_xfer   = saa7134_i2c_xfer,
 	.functionality = functionality,
 };
diff --git a/drivers/media/pci/saa7134/saa7134-input.c b/drivers/media/pci/saa7134/saa7134-input.c
index c8042c3888cd..eff52bbbfd66 100644
--- a/drivers/media/pci/saa7134/saa7134-input.c
+++ b/drivers/media/pci/saa7134/saa7134-input.c
@@ -345,7 +345,7 @@ static int get_key_beholdm6xx(struct IR_i2c *ir, enum rc_type *protocol,
 	if (data[9] != (unsigned char)(~data[8]))
 		return 0;
 
-	*protocol = RC_TYPE_NEC;
+	*protocol = RC_TYPE_NECX;
 	*scancode = RC_SCANCODE_NECX(data[11] << 8 | data[10], data[9]);
 	*toggle = 0;
 	return 1;
@@ -1035,7 +1035,7 @@ void saa7134_probe_i2c_ir(struct saa7134_dev *dev)
 		dev->init_data.name = "BeholdTV";
 		dev->init_data.get_key = get_key_beholdm6xx;
 		dev->init_data.ir_codes = RC_MAP_BEHOLD;
-		dev->init_data.type = RC_BIT_NEC;
+		dev->init_data.type = RC_BIT_NECX;
 		info.addr = 0x2d;
 		break;
 	case SAA7134_BOARD_AVERMEDIA_CARDBUS_501:
diff --git a/drivers/media/pci/saa7134/saa7134-video.c b/drivers/media/pci/saa7134/saa7134-video.c
index 8a6ebd087889..cbb173d99085 100644
--- a/drivers/media/pci/saa7134/saa7134-video.c
+++ b/drivers/media/pci/saa7134/saa7134-video.c
@@ -1054,7 +1054,7 @@ void saa7134_vb2_stop_streaming(struct vb2_queue *vq)
 		pm_qos_remove_request(&dev->qos_request);
 }
 
-static struct vb2_ops vb2_qops = {
+static const struct vb2_ops vb2_qops = {
 	.queue_setup	= queue_setup,
 	.buf_init	= buffer_init,
 	.buf_prepare	= buffer_prepare,
@@ -1659,8 +1659,6 @@ static int saa7134_cropcap(struct file *file, void *priv,
 	if (cap->type != V4L2_BUF_TYPE_VIDEO_CAPTURE &&
 	    cap->type != V4L2_BUF_TYPE_VIDEO_OVERLAY)
 		return -EINVAL;
-	cap->bounds  = dev->crop_bounds;
-	cap->defrect = dev->crop_defrect;
 	cap->pixelaspect.numerator   = 1;
 	cap->pixelaspect.denominator = 1;
 	if (dev->tvnorm->id & V4L2_STD_525_60) {
@@ -1674,25 +1672,41 @@ static int saa7134_cropcap(struct file *file, void *priv,
 	return 0;
 }
 
-static int saa7134_g_crop(struct file *file, void *f, struct v4l2_crop *crop)
+static int saa7134_g_selection(struct file *file, void *f, struct v4l2_selection *sel)
 {
 	struct saa7134_dev *dev = video_drvdata(file);
 
-	if (crop->type != V4L2_BUF_TYPE_VIDEO_CAPTURE &&
-	    crop->type != V4L2_BUF_TYPE_VIDEO_OVERLAY)
+	if (sel->type != V4L2_BUF_TYPE_VIDEO_CAPTURE &&
+	    sel->type != V4L2_BUF_TYPE_VIDEO_OVERLAY)
 		return -EINVAL;
-	crop->c = dev->crop_current;
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP:
+		sel->r = dev->crop_current;
+		break;
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+		sel->r = dev->crop_defrect;
+		break;
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		sel->r  = dev->crop_bounds;
+		break;
+	default:
+		return -EINVAL;
+	}
 	return 0;
 }
 
-static int saa7134_s_crop(struct file *file, void *f, const struct v4l2_crop *crop)
+static int saa7134_s_selection(struct file *file, void *f, struct v4l2_selection *sel)
 {
 	struct saa7134_dev *dev = video_drvdata(file);
 	struct v4l2_rect *b = &dev->crop_bounds;
 	struct v4l2_rect *c = &dev->crop_current;
 
-	if (crop->type != V4L2_BUF_TYPE_VIDEO_CAPTURE &&
-	    crop->type != V4L2_BUF_TYPE_VIDEO_OVERLAY)
+	if (sel->type != V4L2_BUF_TYPE_VIDEO_CAPTURE &&
+	    sel->type != V4L2_BUF_TYPE_VIDEO_OVERLAY)
+		return -EINVAL;
+
+	if (sel->target != V4L2_SEL_TGT_CROP)
 		return -EINVAL;
 
 	if (dev->overlay_owner)
@@ -1700,7 +1714,7 @@ static int saa7134_s_crop(struct file *file, void *f, const struct v4l2_crop *cr
 	if (vb2_is_streaming(&dev->video_vbq))
 		return -EBUSY;
 
-	*c = crop->c;
+	*c = sel->r;
 	if (c->top < b->top)
 		c->top = b->top;
 	if (c->top > b->top + b->height)
@@ -1714,6 +1728,7 @@ static int saa7134_s_crop(struct file *file, void *f, const struct v4l2_crop *cr
 		c->left = b->left + b->width;
 	if (c->width > b->left - c->left + b->width)
 		c->width = b->left - c->left + b->width;
+	sel->r = *c;
 	return 0;
 }
 
@@ -1989,8 +2004,8 @@ static const struct v4l2_ioctl_ops video_ioctl_ops = {
 	.vidioc_streamoff		= vb2_ioctl_streamoff,
 	.vidioc_g_tuner			= saa7134_g_tuner,
 	.vidioc_s_tuner			= saa7134_s_tuner,
-	.vidioc_g_crop			= saa7134_g_crop,
-	.vidioc_s_crop			= saa7134_s_crop,
+	.vidioc_g_selection		= saa7134_g_selection,
+	.vidioc_s_selection		= saa7134_s_selection,
 	.vidioc_g_fbuf			= saa7134_g_fbuf,
 	.vidioc_s_fbuf			= saa7134_s_fbuf,
 	.vidioc_overlay			= saa7134_overlay,
diff --git a/drivers/media/pci/saa7164/saa7164-i2c.c b/drivers/media/pci/saa7164/saa7164-i2c.c
index 0342d84913b8..024f4e29e840 100644
--- a/drivers/media/pci/saa7164/saa7164-i2c.c
+++ b/drivers/media/pci/saa7164/saa7164-i2c.c
@@ -75,7 +75,7 @@ static u32 saa7164_functionality(struct i2c_adapter *adap)
 	return I2C_FUNC_I2C;
 }
 
-static struct i2c_algorithm saa7164_i2c_algo_template = {
+static const struct i2c_algorithm saa7164_i2c_algo_template = {
 	.master_xfer	= i2c_xfer,
 	.functionality	= saa7164_functionality,
 };
diff --git a/drivers/media/pci/smipcie/smipcie-main.c b/drivers/media/pci/smipcie/smipcie-main.c
index 83981d611a79..6dbe3b4d09ce 100644
--- a/drivers/media/pci/smipcie/smipcie-main.c
+++ b/drivers/media/pci/smipcie/smipcie-main.c
@@ -1060,7 +1060,7 @@ static void smi_remove(struct pci_dev *pdev)
 }
 
 /* DVBSky cards */
-static struct smi_cfg_info dvbsky_s950_cfg = {
+static const struct smi_cfg_info dvbsky_s950_cfg = {
 	.type = SMI_DVBSKY_S950,
 	.name = "DVBSky S950 V3",
 	.ts_0 = SMI_TS_NULL,
@@ -1070,7 +1070,7 @@ static struct smi_cfg_info dvbsky_s950_cfg = {
 	.rc_map = RC_MAP_DVBSKY,
 };
 
-static struct smi_cfg_info dvbsky_s952_cfg = {
+static const struct smi_cfg_info dvbsky_s952_cfg = {
 	.type = SMI_DVBSKY_S952,
 	.name = "DVBSky S952 V3",
 	.ts_0 = SMI_TS_DMA_BOTH,
@@ -1080,7 +1080,7 @@ static struct smi_cfg_info dvbsky_s952_cfg = {
 	.rc_map = RC_MAP_DVBSKY,
 };
 
-static struct smi_cfg_info dvbsky_t9580_cfg = {
+static const struct smi_cfg_info dvbsky_t9580_cfg = {
 	.type = SMI_DVBSKY_T9580,
 	.name = "DVBSky T9580 V3",
 	.ts_0 = SMI_TS_DMA_BOTH,
@@ -1090,7 +1090,7 @@ static struct smi_cfg_info dvbsky_t9580_cfg = {
 	.rc_map = RC_MAP_DVBSKY,
 };
 
-static struct smi_cfg_info technotrend_s2_4200_cfg = {
+static const struct smi_cfg_info technotrend_s2_4200_cfg = {
 	.type = SMI_TECHNOTREND_S2_4200,
 	.name = "TechnoTrend TT-budget S2-4200 Twin",
 	.ts_0 = SMI_TS_DMA_BOTH,
diff --git a/drivers/media/pci/solo6x10/solo6x10-g723.c b/drivers/media/pci/solo6x10/solo6x10-g723.c
index 4a37a1c51c48..6a35107aca25 100644
--- a/drivers/media/pci/solo6x10/solo6x10-g723.c
+++ b/drivers/media/pci/solo6x10/solo6x10-g723.c
@@ -252,7 +252,7 @@ static int snd_solo_pcm_copy(struct snd_pcm_substream *ss, int channel,
 	return 0;
 }
 
-static struct snd_pcm_ops snd_solo_pcm_ops = {
+static const struct snd_pcm_ops snd_solo_pcm_ops = {
 	.open = snd_solo_pcm_open,
 	.close = snd_solo_pcm_close,
 	.ioctl = snd_pcm_lib_ioctl,
diff --git a/drivers/media/pci/solo6x10/solo6x10-v4l2-enc.c b/drivers/media/pci/solo6x10/solo6x10-v4l2-enc.c
index 399164314c28..25a2137ab799 100644
--- a/drivers/media/pci/solo6x10/solo6x10-v4l2-enc.c
+++ b/drivers/media/pci/solo6x10/solo6x10-v4l2-enc.c
@@ -759,7 +759,7 @@ static void solo_enc_buf_finish(struct vb2_buffer *vb)
 	}
 }
 
-static struct vb2_ops solo_enc_video_qops = {
+static const struct vb2_ops solo_enc_video_qops = {
 	.queue_setup	= solo_enc_queue_setup,
 	.buf_queue	= solo_enc_buf_queue,
 	.buf_finish	= solo_enc_buf_finish,
diff --git a/drivers/media/pci/tw5864/Kconfig b/drivers/media/pci/tw5864/Kconfig
new file mode 100644
index 000000000000..87c8f327e2d4
--- /dev/null
+++ b/drivers/media/pci/tw5864/Kconfig
@@ -0,0 +1,12 @@
+config VIDEO_TW5864
+	tristate "Techwell TW5864 video/audio grabber and encoder"
+	depends on VIDEO_DEV && PCI && VIDEO_V4L2
+	depends on HAS_DMA
+	select VIDEOBUF2_DMA_CONTIG
+	---help---
+	  Support for boards based on Techwell TW5864 chip which provides
+	  multichannel video & audio grabbing and encoding (H.264, MJPEG,
+	  ADPCM G.726).
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called tw5864.
diff --git a/drivers/media/pci/tw5864/Makefile b/drivers/media/pci/tw5864/Makefile
new file mode 100644
index 000000000000..4fc8b3b1a45a
--- /dev/null
+++ b/drivers/media/pci/tw5864/Makefile
@@ -0,0 +1,3 @@
+tw5864-objs := tw5864-core.o tw5864-video.o tw5864-h264.o tw5864-util.o
+
+obj-$(CONFIG_VIDEO_TW5864) += tw5864.o
diff --git a/drivers/media/pci/tw5864/tw5864-core.c b/drivers/media/pci/tw5864/tw5864-core.c
new file mode 100644
index 000000000000..1d43b96958ea
--- /dev/null
+++ b/drivers/media/pci/tw5864/tw5864-core.c
@@ -0,0 +1,359 @@
+/*
+ *  TW5864 driver - core functions
+ *
+ *  Copyright (C) 2016 Bluecherry, LLC <maintainers@bluecherrydvr.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/init.h>
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/kmod.h>
+#include <linux/sound.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/dma-mapping.h>
+#include <linux/pm.h>
+#include <linux/pci_ids.h>
+#include <linux/jiffies.h>
+#include <asm/dma.h>
+#include <media/v4l2-dev.h>
+
+#include "tw5864.h"
+#include "tw5864-reg.h"
+
+MODULE_DESCRIPTION("V4L2 driver module for tw5864-based multimedia capture & encoding devices");
+MODULE_AUTHOR("Bluecherry Maintainers <maintainers@bluecherrydvr.com>");
+MODULE_AUTHOR("Andrey Utkin <andrey.utkin@corp.bluecherry.net>");
+MODULE_LICENSE("GPL");
+
+/*
+ * BEWARE OF KNOWN ISSUES WITH VIDEO QUALITY
+ *
+ * This driver was developed by Bluecherry LLC by deducing behaviour of
+ * original manufacturer's driver, from both source code and execution traces.
+ * It is known that there are some artifacts on output video with this driver:
+ *  - on all known hardware samples: random pixels of wrong color (mostly
+ *    white, red or blue) appearing and disappearing on sequences of P-frames;
+ *  - on some hardware samples (known with H.264 core version e006:2800):
+ *    total madness on P-frames: blocks of wrong luminance; blocks of wrong
+ *    colors "creeping" across the picture.
+ * There is a workaround for both issues: avoid P-frames by setting GOP size
+ * to 1. To do that, run this command on device files created by this driver:
+ *
+ * v4l2-ctl --device /dev/videoX --set-ctrl=video_gop_size=1
+ *
+ * These issues are not decoding errors; all produced H.264 streams are decoded
+ * properly. Streams without P-frames don't have these artifacts so it's not
+ * analog-to-digital conversion issues nor internal memory errors; we conclude
+ * it's internal H.264 encoder issues.
+ * We cannot even check the original driver's behaviour because it has never
+ * worked properly at all in our development environment. So these issues may
+ * be actually related to firmware or hardware. However it may be that there's
+ * just some more register settings missing in the driver which would please
+ * the hardware.
+ * Manufacturer didn't help much on our inquiries, but feel free to disturb
+ * again the support of Intersil (owner of former Techwell).
+ */
+
+/* take first free /dev/videoX indexes by default */
+static unsigned int video_nr[] = {[0 ... (TW5864_INPUTS - 1)] = -1 };
+
+module_param_array(video_nr, int, NULL, 0444);
+MODULE_PARM_DESC(video_nr, "video devices numbers array");
+
+/*
+ * Please add any new PCI IDs to: http://pci-ids.ucw.cz.  This keeps
+ * the PCI ID database up to date.  Note that the entries must be
+ * added under vendor 0x1797 (Techwell Inc.) as subsystem IDs.
+ */
+static const struct pci_device_id tw5864_pci_tbl[] = {
+	{PCI_DEVICE(PCI_VENDOR_ID_TECHWELL, PCI_DEVICE_ID_TECHWELL_5864)},
+	{0,}
+};
+
+void tw5864_irqmask_apply(struct tw5864_dev *dev)
+{
+	tw_writel(TW5864_INTR_ENABLE_L, dev->irqmask & 0xffff);
+	tw_writel(TW5864_INTR_ENABLE_H, (dev->irqmask >> 16));
+}
+
+static void tw5864_interrupts_disable(struct tw5864_dev *dev)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&dev->slock, flags);
+	dev->irqmask = 0;
+	tw5864_irqmask_apply(dev);
+	spin_unlock_irqrestore(&dev->slock, flags);
+}
+
+static void tw5864_timer_isr(struct tw5864_dev *dev);
+static void tw5864_h264_isr(struct tw5864_dev *dev);
+
+static irqreturn_t tw5864_isr(int irq, void *dev_id)
+{
+	struct tw5864_dev *dev = dev_id;
+	u32 status;
+
+	status = tw_readl(TW5864_INTR_STATUS_L) |
+		tw_readl(TW5864_INTR_STATUS_H) << 16;
+	if (!status)
+		return IRQ_NONE;
+
+	tw_writel(TW5864_INTR_CLR_L, 0xffff);
+	tw_writel(TW5864_INTR_CLR_H, 0xffff);
+
+	if (status & TW5864_INTR_VLC_DONE)
+		tw5864_h264_isr(dev);
+
+	if (status & TW5864_INTR_TIMER)
+		tw5864_timer_isr(dev);
+
+	if (!(status & (TW5864_INTR_TIMER | TW5864_INTR_VLC_DONE))) {
+		dev_dbg(&dev->pci->dev, "Unknown interrupt, status 0x%08X\n",
+			status);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static void tw5864_h264_isr(struct tw5864_dev *dev)
+{
+	int channel = tw_readl(TW5864_DSP) & TW5864_DSP_ENC_CHN;
+	struct tw5864_input *input = &dev->inputs[channel];
+	int cur_frame_index, next_frame_index;
+	struct tw5864_h264_frame *cur_frame, *next_frame;
+	unsigned long flags;
+
+	spin_lock_irqsave(&dev->slock, flags);
+
+	cur_frame_index = dev->h264_buf_w_index;
+	next_frame_index = (cur_frame_index + 1) % H264_BUF_CNT;
+	cur_frame = &dev->h264_buf[cur_frame_index];
+	next_frame = &dev->h264_buf[next_frame_index];
+
+	if (next_frame_index != dev->h264_buf_r_index) {
+		cur_frame->vlc_len = tw_readl(TW5864_VLC_LENGTH) << 2;
+		cur_frame->checksum = tw_readl(TW5864_VLC_CRC_REG);
+		cur_frame->input = input;
+		cur_frame->timestamp = ktime_get_ns();
+		cur_frame->seqno = input->frame_seqno;
+		cur_frame->gop_seqno = input->frame_gop_seqno;
+
+		dev->h264_buf_w_index = next_frame_index;
+		tasklet_schedule(&dev->tasklet);
+
+		cur_frame = next_frame;
+
+		spin_lock(&input->slock);
+		input->frame_seqno++;
+		input->frame_gop_seqno++;
+		if (input->frame_gop_seqno >= input->gop)
+			input->frame_gop_seqno = 0;
+		spin_unlock(&input->slock);
+	} else {
+		dev_err(&dev->pci->dev,
+			"Skipped frame on input %d because all buffers busy\n",
+			channel);
+	}
+
+	dev->encoder_busy = 0;
+
+	spin_unlock_irqrestore(&dev->slock, flags);
+
+	tw_writel(TW5864_VLC_STREAM_BASE_ADDR, cur_frame->vlc.dma_addr);
+	tw_writel(TW5864_MV_STREAM_BASE_ADDR, cur_frame->mv.dma_addr);
+
+	/* Additional ack for this interrupt */
+	tw_writel(TW5864_VLC_DSP_INTR, 0x00000001);
+	tw_writel(TW5864_PCI_INTR_STATUS, TW5864_VLC_DONE_INTR);
+}
+
+static void tw5864_input_deadline_update(struct tw5864_input *input)
+{
+	input->new_frame_deadline = jiffies + msecs_to_jiffies(1000);
+}
+
+static void tw5864_timer_isr(struct tw5864_dev *dev)
+{
+	unsigned long flags;
+	int i;
+	int encoder_busy;
+
+	/* Additional ack for this interrupt */
+	tw_writel(TW5864_PCI_INTR_STATUS, TW5864_TIMER_INTR);
+
+	spin_lock_irqsave(&dev->slock, flags);
+	encoder_busy = dev->encoder_busy;
+	spin_unlock_irqrestore(&dev->slock, flags);
+
+	if (encoder_busy)
+		return;
+
+	/*
+	 * Traversing inputs in round-robin fashion, starting from next to the
+	 * last processed one
+	 */
+	for (i = 0; i < TW5864_INPUTS; i++) {
+		int next_input = (i + dev->next_input) % TW5864_INPUTS;
+		struct tw5864_input *input = &dev->inputs[next_input];
+		int raw_buf_id; /* id of internal buf with last raw frame */
+
+		spin_lock_irqsave(&input->slock, flags);
+		if (!input->enabled)
+			goto next;
+
+		/* Check if new raw frame is available */
+		raw_buf_id = tw_mask_shift_readl(TW5864_SENIF_ORG_FRM_PTR1, 0x3,
+						 2 * input->nr);
+
+		if (input->buf_id != raw_buf_id) {
+			input->buf_id = raw_buf_id;
+			tw5864_input_deadline_update(input);
+			spin_unlock_irqrestore(&input->slock, flags);
+
+			spin_lock_irqsave(&dev->slock, flags);
+			dev->encoder_busy = 1;
+			dev->next_input = (next_input + 1) % TW5864_INPUTS;
+			spin_unlock_irqrestore(&dev->slock, flags);
+
+			tw5864_request_encoded_frame(input);
+			break;
+		}
+
+		/* No new raw frame; check if channel is stuck */
+		if (time_is_after_jiffies(input->new_frame_deadline)) {
+			/* If stuck, request new raw frames again */
+			tw_mask_shift_writel(TW5864_ENC_BUF_PTR_REC1, 0x3,
+					     2 * input->nr, input->buf_id + 3);
+			tw5864_input_deadline_update(input);
+		}
+next:
+		spin_unlock_irqrestore(&input->slock, flags);
+	}
+}
+
+static int tw5864_initdev(struct pci_dev *pci_dev,
+			  const struct pci_device_id *pci_id)
+{
+	struct tw5864_dev *dev;
+	int err;
+
+	dev = devm_kzalloc(&pci_dev->dev, sizeof(*dev), GFP_KERNEL);
+	if (!dev)
+		return -ENOMEM;
+
+	snprintf(dev->name, sizeof(dev->name), "tw5864:%s", pci_name(pci_dev));
+
+	err = v4l2_device_register(&pci_dev->dev, &dev->v4l2_dev);
+	if (err)
+		return err;
+
+	/* pci init */
+	dev->pci = pci_dev;
+	err = pci_enable_device(pci_dev);
+	if (err) {
+		dev_err(&dev->pci->dev, "pci_enable_device() failed\n");
+		goto unreg_v4l2;
+	}
+
+	pci_set_master(pci_dev);
+
+	err = pci_set_dma_mask(pci_dev, DMA_BIT_MASK(32));
+	if (err) {
+		dev_err(&dev->pci->dev, "32 bit PCI DMA is not supported\n");
+		goto disable_pci;
+	}
+
+	/* get mmio */
+	err = pci_request_regions(pci_dev, dev->name);
+	if (err) {
+		dev_err(&dev->pci->dev, "Cannot request regions for MMIO\n");
+		goto disable_pci;
+	}
+	dev->mmio = pci_ioremap_bar(pci_dev, 0);
+	if (!dev->mmio) {
+		err = -EIO;
+		dev_err(&dev->pci->dev, "can't ioremap() MMIO memory\n");
+		goto release_mmio;
+	}
+
+	spin_lock_init(&dev->slock);
+
+	dev_info(&pci_dev->dev, "TW5864 hardware version: %04x\n",
+		 tw_readl(TW5864_HW_VERSION));
+	dev_info(&pci_dev->dev, "TW5864 H.264 core version: %04x:%04x\n",
+		 tw_readl(TW5864_H264REV),
+		 tw_readl(TW5864_UNDECLARED_H264REV_PART2));
+
+	err = tw5864_video_init(dev, video_nr);
+	if (err)
+		goto unmap_mmio;
+
+	/* get irq */
+	err = devm_request_irq(&pci_dev->dev, pci_dev->irq, tw5864_isr,
+			       IRQF_SHARED, "tw5864", dev);
+	if (err < 0) {
+		dev_err(&dev->pci->dev, "can't get IRQ %d\n", pci_dev->irq);
+		goto fini_video;
+	}
+
+	dev_info(&pci_dev->dev, "Note: there are known video quality issues. For details\n");
+	dev_info(&pci_dev->dev, "see the comment in drivers/media/pci/tw5864/tw5864-core.c.\n");
+
+	return 0;
+
+fini_video:
+	tw5864_video_fini(dev);
+unmap_mmio:
+	iounmap(dev->mmio);
+release_mmio:
+	pci_release_regions(pci_dev);
+disable_pci:
+	pci_disable_device(pci_dev);
+unreg_v4l2:
+	v4l2_device_unregister(&dev->v4l2_dev);
+	return err;
+}
+
+static void tw5864_finidev(struct pci_dev *pci_dev)
+{
+	struct v4l2_device *v4l2_dev = pci_get_drvdata(pci_dev);
+	struct tw5864_dev *dev =
+		container_of(v4l2_dev, struct tw5864_dev, v4l2_dev);
+
+	/* shutdown subsystems */
+	tw5864_interrupts_disable(dev);
+
+	/* unregister */
+	tw5864_video_fini(dev);
+
+	/* release resources */
+	iounmap(dev->mmio);
+	release_mem_region(pci_resource_start(pci_dev, 0),
+			   pci_resource_len(pci_dev, 0));
+
+	v4l2_device_unregister(&dev->v4l2_dev);
+	devm_kfree(&pci_dev->dev, dev);
+}
+
+static struct pci_driver tw5864_pci_driver = {
+	.name = "tw5864",
+	.id_table = tw5864_pci_tbl,
+	.probe = tw5864_initdev,
+	.remove = tw5864_finidev,
+};
+
+module_pci_driver(tw5864_pci_driver);
diff --git a/drivers/media/pci/tw5864/tw5864-h264.c b/drivers/media/pci/tw5864/tw5864-h264.c
new file mode 100644
index 000000000000..330d200f52cd
--- /dev/null
+++ b/drivers/media/pci/tw5864/tw5864-h264.c
@@ -0,0 +1,259 @@
+/*
+ *  TW5864 driver - H.264 headers generation functions
+ *
+ *  Copyright (C) 2016 Bluecherry, LLC <maintainers@bluecherrydvr.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/log2.h>
+
+#include "tw5864.h"
+
+static u8 marker[] = { 0x00, 0x00, 0x00, 0x01 };
+
+/*
+ * Exponential-Golomb coding functions
+ *
+ * These functions are used for generation of H.264 bitstream headers.
+ *
+ * This code is derived from tw5864 reference driver by manufacturers, which
+ * itself apparently was derived from x264 project.
+ */
+
+/* Bitstream writing context */
+struct bs {
+	u8 *buf; /* pointer to buffer beginning */
+	u8 *buf_end; /* pointer to buffer end */
+	u8 *ptr; /* pointer to current byte in buffer */
+	unsigned int bits_left; /* number of available bits in current byte */
+};
+
+static void bs_init(struct bs *s, void *buf, int size)
+{
+	s->buf = buf;
+	s->ptr = buf;
+	s->buf_end = s->ptr + size;
+	s->bits_left = 8;
+}
+
+static int bs_len(struct bs *s)
+{
+	return s->ptr - s->buf;
+}
+
+static void bs_write(struct bs *s, int count, u32 bits)
+{
+	if (s->ptr >= s->buf_end - 4)
+		return;
+	while (count > 0) {
+		if (count < 32)
+			bits &= (1 << count) - 1;
+		if (count < s->bits_left) {
+			*s->ptr = (*s->ptr << count) | bits;
+			s->bits_left -= count;
+			break;
+		}
+		*s->ptr = (*s->ptr << s->bits_left) |
+			(bits >> (count - s->bits_left));
+		count -= s->bits_left;
+		s->ptr++;
+		s->bits_left = 8;
+	}
+}
+
+static void bs_write1(struct bs *s, u32 bit)
+{
+	if (s->ptr < s->buf_end) {
+		*s->ptr <<= 1;
+		*s->ptr |= bit;
+		s->bits_left--;
+		if (s->bits_left == 0) {
+			s->ptr++;
+			s->bits_left = 8;
+		}
+	}
+}
+
+static void bs_write_ue(struct bs *s, u32 val)
+{
+	if (val == 0) {
+		bs_write1(s, 1);
+	} else {
+		val++;
+		bs_write(s, 2 * fls(val) - 1, val);
+	}
+}
+
+static void bs_write_se(struct bs *s, int val)
+{
+	bs_write_ue(s, val <= 0 ? -val * 2 : val * 2 - 1);
+}
+
+static void bs_rbsp_trailing(struct bs *s)
+{
+	bs_write1(s, 1);
+	if (s->bits_left != 8)
+		bs_write(s, s->bits_left, 0x00);
+}
+
+/* H.264 headers generation functions */
+
+static int tw5864_h264_gen_sps_rbsp(u8 *buf, size_t size, int width, int height)
+{
+	struct bs bs, *s;
+
+	s = &bs;
+	bs_init(s, buf, size);
+	bs_write(s, 8, 0x42); /* profile_idc, baseline */
+	bs_write(s, 1, 1); /* constraint_set0_flag */
+	bs_write(s, 1, 1); /* constraint_set1_flag */
+	bs_write(s, 1, 0); /* constraint_set2_flag */
+	bs_write(s, 5, 0); /* reserved_zero_5bits */
+	bs_write(s, 8, 0x1e); /* level_idc */
+	bs_write_ue(s, 0); /* seq_parameter_set_id */
+	bs_write_ue(s, ilog2(MAX_GOP_SIZE) - 4); /* log2_max_frame_num_minus4 */
+	bs_write_ue(s, 0); /* pic_order_cnt_type */
+	/* log2_max_pic_order_cnt_lsb_minus4 */
+	bs_write_ue(s, ilog2(MAX_GOP_SIZE) - 4);
+	bs_write_ue(s, 1); /* num_ref_frames */
+	bs_write(s, 1, 0); /* gaps_in_frame_num_value_allowed_flag */
+	bs_write_ue(s, width / 16 - 1); /* pic_width_in_mbs_minus1 */
+	bs_write_ue(s, height / 16 - 1); /* pic_height_in_map_units_minus1 */
+	bs_write(s, 1, 1); /* frame_mbs_only_flag */
+	bs_write(s, 1, 0); /* direct_8x8_inference_flag */
+	bs_write(s, 1, 0); /* frame_cropping_flag */
+	bs_write(s, 1, 0); /* vui_parameters_present_flag */
+	bs_rbsp_trailing(s);
+	return bs_len(s);
+}
+
+static int tw5864_h264_gen_pps_rbsp(u8 *buf, size_t size, int qp)
+{
+	struct bs bs, *s;
+
+	s = &bs;
+	bs_init(s, buf, size);
+	bs_write_ue(s, 0); /* pic_parameter_set_id */
+	bs_write_ue(s, 0); /* seq_parameter_set_id */
+	bs_write(s, 1, 0); /* entropy_coding_mode_flag */
+	bs_write(s, 1, 0); /* pic_order_present_flag */
+	bs_write_ue(s, 0); /* num_slice_groups_minus1 */
+	bs_write_ue(s, 0); /* i_num_ref_idx_l0_active_minus1 */
+	bs_write_ue(s, 0); /* i_num_ref_idx_l1_active_minus1 */
+	bs_write(s, 1, 0); /* weighted_pred_flag */
+	bs_write(s, 2, 0); /* weighted_bipred_idc */
+	bs_write_se(s, qp - 26); /* pic_init_qp_minus26 */
+	bs_write_se(s, qp - 26); /* pic_init_qs_minus26 */
+	bs_write_se(s, 0); /* chroma_qp_index_offset */
+	bs_write(s, 1, 0); /* deblocking_filter_control_present_flag */
+	bs_write(s, 1, 0); /* constrained_intra_pred_flag */
+	bs_write(s, 1, 0); /* redundant_pic_cnt_present_flag */
+	bs_rbsp_trailing(s);
+	return bs_len(s);
+}
+
+static int tw5864_h264_gen_slice_head(u8 *buf, size_t size,
+				      unsigned int idr_pic_id,
+				      unsigned int frame_gop_seqno,
+				      int *tail_nb_bits, u8 *tail)
+{
+	struct bs bs, *s;
+	int is_i_frame = frame_gop_seqno == 0;
+
+	s = &bs;
+	bs_init(s, buf, size);
+	bs_write_ue(s, 0); /* first_mb_in_slice */
+	bs_write_ue(s, is_i_frame ? 2 : 5); /* slice_type - I or P */
+	bs_write_ue(s, 0); /* pic_parameter_set_id */
+	bs_write(s, ilog2(MAX_GOP_SIZE), frame_gop_seqno); /* frame_num */
+	if (is_i_frame)
+		bs_write_ue(s, idr_pic_id);
+
+	/* pic_order_cnt_lsb */
+	bs_write(s, ilog2(MAX_GOP_SIZE), frame_gop_seqno);
+
+	if (is_i_frame) {
+		bs_write1(s, 0); /* no_output_of_prior_pics_flag */
+		bs_write1(s, 0); /* long_term_reference_flag */
+	} else {
+		bs_write1(s, 0); /* num_ref_idx_active_override_flag */
+		bs_write1(s, 0); /* ref_pic_list_reordering_flag_l0 */
+		bs_write1(s, 0); /* adaptive_ref_pic_marking_mode_flag */
+	}
+
+	bs_write_se(s, 0); /* slice_qp_delta */
+
+	if (s->bits_left != 8) {
+		*tail = ((s->ptr[0]) << s->bits_left);
+		*tail_nb_bits = 8 - s->bits_left;
+	} else {
+		*tail = 0;
+		*tail_nb_bits = 0;
+	}
+
+	return bs_len(s);
+}
+
+void tw5864_h264_put_stream_header(u8 **buf, size_t *space_left, int qp,
+				   int width, int height)
+{
+	int nal_len;
+
+	/* SPS */
+	memcpy(*buf, marker, sizeof(marker));
+	*buf += 4;
+	*space_left -= 4;
+
+	**buf = 0x67; /* SPS NAL header */
+	*buf += 1;
+	*space_left -= 1;
+
+	nal_len = tw5864_h264_gen_sps_rbsp(*buf, *space_left, width, height);
+	*buf += nal_len;
+	*space_left -= nal_len;
+
+	/* PPS */
+	memcpy(*buf, marker, sizeof(marker));
+	*buf += 4;
+	*space_left -= 4;
+
+	**buf = 0x68; /* PPS NAL header */
+	*buf += 1;
+	*space_left -= 1;
+
+	nal_len = tw5864_h264_gen_pps_rbsp(*buf, *space_left, qp);
+	*buf += nal_len;
+	*space_left -= nal_len;
+}
+
+void tw5864_h264_put_slice_header(u8 **buf, size_t *space_left,
+				  unsigned int idr_pic_id,
+				  unsigned int frame_gop_seqno,
+				  int *tail_nb_bits, u8 *tail)
+{
+	int nal_len;
+
+	memcpy(*buf, marker, sizeof(marker));
+	*buf += 4;
+	*space_left -= 4;
+
+	/* Frame NAL header */
+	**buf = (frame_gop_seqno == 0) ? 0x25 : 0x21;
+	*buf += 1;
+	*space_left -= 1;
+
+	nal_len = tw5864_h264_gen_slice_head(*buf, *space_left, idr_pic_id,
+					     frame_gop_seqno, tail_nb_bits,
+					     tail);
+	*buf += nal_len;
+	*space_left -= nal_len;
+}
diff --git a/drivers/media/pci/tw5864/tw5864-reg.h b/drivers/media/pci/tw5864/tw5864-reg.h
new file mode 100644
index 000000000000..92a1b077ef8a
--- /dev/null
+++ b/drivers/media/pci/tw5864/tw5864-reg.h
@@ -0,0 +1,2133 @@
+/*
+ *  TW5864 driver - registers description
+ *
+ *  Copyright (C) 2016 Bluecherry, LLC <maintainers@bluecherrydvr.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.
+ */
+
+/* According to TW5864_datasheet_0.6d.pdf, tw5864b1-ds.pdf */
+
+/* Register Description - Direct Map Space */
+/* 0x0000 ~ 0x1ffc - H264 Register Map */
+/* [15:0] The Version register for H264 core (Read Only) */
+#define TW5864_H264REV 0x0000
+
+#define TW5864_EMU 0x0004
+/* Define controls in register TW5864_EMU */
+/* DDR controller enabled */
+#define TW5864_EMU_EN_DDR BIT(0)
+/* Enable bit for Inter module */
+#define TW5864_EMU_EN_ME BIT(1)
+/* Enable bit for Sensor Interface module */
+#define TW5864_EMU_EN_SEN BIT(2)
+/* Enable bit for Host Burst Access */
+#define TW5864_EMU_EN_BHOST BIT(3)
+/* Enable bit for Loop Filter module */
+#define TW5864_EMU_EN_LPF BIT(4)
+/* Enable bit for PLBK module */
+#define TW5864_EMU_EN_PLBK BIT(5)
+/*
+ * Video Frame mapping in DDR
+ * 00 CIF
+ * 01 D1
+ * 10 Reserved
+ * 11 Reserved
+ *
+ */
+#define TW5864_DSP_FRAME_TYPE (3 << 6)
+#define TW5864_DSP_FRAME_TYPE_D1 BIT(6)
+
+#define TW5864_UNDECLARED_H264REV_PART2 0x0008
+
+#define TW5864_SLICE 0x000c
+/* Define controls in register TW5864_SLICE */
+/* VLC Slice end flag */
+#define TW5864_VLC_SLICE_END BIT(0)
+/* Master Slice End Flag */
+#define TW5864_MAS_SLICE_END BIT(4)
+/* Host to start a new slice Address */
+#define TW5864_START_NSLICE BIT(15)
+
+/*
+ * [15:0] Two bit for each channel (channel 0 ~ 7). Each two bits are the buffer
+ * pointer for the last encoded frame of the corresponding channel.
+ */
+#define TW5864_ENC_BUF_PTR_REC1 0x0010
+
+/* [5:0] DSP_MB_QP and [15:10] DSP_LPF_OFFSET */
+#define TW5864_DSP_QP 0x0018
+/* Define controls in register TW5864_DSP_QP */
+/* [5:0] H264 QP Value for codec */
+#define TW5864_DSP_MB_QP 0x003f
+/*
+ * [15:10] H264 LPF_OFFSET Address
+ * (Default 0)
+ */
+#define TW5864_DSP_LPF_OFFSET 0xfc00
+
+#define TW5864_DSP_CODEC 0x001c
+/* Define controls in register TW5864_DSP_CODEC */
+/*
+ * 0: Encode (TW5864 Default)
+ * 1: Decode
+ */
+#define TW5864_DSP_CODEC_MODE BIT(0)
+/*
+ * 0->3 4 VLC data buffer in DDR (1M each)
+ * 0->7 8 VLC data buffer in DDR (512k each)
+ */
+#define TW5864_VLC_BUF_ID (7 << 2)
+/*
+ * 0 4CIF in 1 MB
+ * 1 1CIF in 1 MB
+ */
+#define TW5864_CIF_MAP_MD BIT(6)
+/*
+ * 0 2 falf D1 in 1 MB
+ * 1 1 half D1 in 1 MB
+ */
+#define TW5864_HD1_MAP_MD BIT(7)
+/* VLC Stream valid */
+#define TW5864_VLC_VLD BIT(8)
+/* MV Vector Valid */
+#define TW5864_MV_VECT_VLD BIT(9)
+/* MV Flag Valid */
+#define TW5864_MV_FLAG_VLD BIT(10)
+
+#define TW5864_DSP_SEN 0x0020
+/* Define controls in register TW5864_DSP_SEN */
+/* Org Buffer Base for Luma (default 0) */
+#define TW5864_DSP_SEN_PIC_LU 0x000f
+/* Org Buffer Base for Chroma (default 4) */
+#define TW5864_DSP_SEN_PIC_CHM 0x00f0
+/* Maximum Number of Buffers (default 4) */
+#define TW5864_DSP_SEN_PIC_MAX 0x0700
+/*
+ * Original Frame D1 or HD1 switch
+ * (Default 0)
+ */
+#define TW5864_DSP_SEN_HFULL 0x1000
+
+#define TW5864_DSP_REF_PIC 0x0024
+/* Define controls in register TW5864_DSP_REF_PIC */
+/* Ref Buffer Base for Luma (default 0) */
+#define TW5864_DSP_REF_PIC_LU 0x000f
+/* Ref Buffer Base for Chroma (default 4) */
+#define TW5864_DSP_REF_PIC_CHM 0x00f0
+/* Maximum Number of Buffers (default 4) */
+#define TW5864_DSP_REF_PIC_MAX 0x0700
+
+/* [15:0] SEN_EN_CH[n] SENIF original frame capture enable for each channel */
+#define TW5864_SEN_EN_CH 0x0028
+
+#define TW5864_DSP 0x002c
+/* Define controls in register TW5864_DSP */
+/* The ID for channel selected for encoding operation */
+#define TW5864_DSP_ENC_CHN 0x000f
+/* See DSP_MB_DELAY below */
+#define TW5864_DSP_MB_WAIT 0x0010
+/*
+ * DSP Chroma Switch
+ * 0 DDRB
+ * 1 DDRA
+ */
+#define TW5864_DSP_CHROM_SW 0x0020
+/* VLC Flow Control: 1 for enable */
+#define TW5864_DSP_FLW_CNTL 0x0040
+/*
+ * If DSP_MB_WAIT == 0, MB delay is DSP_MB_DELAY * 16
+ * If DSP_MB_DELAY == 1, MB delay is DSP_MB_DELAY * 128
+ */
+#define TW5864_DSP_MB_DELAY 0x0f00
+
+#define TW5864_DDR 0x0030
+/* Define controls in register TW5864_DDR */
+/* DDR Single Access Page Number */
+#define TW5864_DDR_PAGE_CNTL 0x00ff
+/* DDR-DPR Burst Read Enable */
+#define TW5864_DDR_BRST_EN BIT(13)
+/*
+ * DDR A/B Select as HOST access
+ * 0 Select DDRA
+ * 1 Select DDRB
+ */
+#define TW5864_DDR_AB_SEL BIT(14)
+/*
+ * DDR Access Mode Select
+ * 0 Single R/W Access (Host <-> DDR)
+ * 1 Burst R/W Access (Host <-> DPR)
+ */
+#define TW5864_DDR_MODE BIT(15)
+
+/* The original frame capture pointer. Two bits for each channel */
+/* SENIF_ORG_FRM_PTR [15:0] */
+#define TW5864_SENIF_ORG_FRM_PTR1 0x0038
+/* SENIF_ORG_FRM_PTR [31:16] */
+#define TW5864_SENIF_ORG_FRM_PTR2 0x003c
+
+#define TW5864_DSP_SEN_MODE 0x0040
+/* Define controls in register TW5864_DSP_SEN_MODE */
+#define TW5864_DSP_SEN_MODE_CH0 0x000f
+#define TW5864_DSP_SEN_MODE_CH1 0x00f0
+
+/*
+ * [15:0]: ENC_BUF_PTR_REC[31:16] Two bit for each channel (channel 8 ~ 15).
+ * Each two bits are the buffer pointer for the last encoded frame of a channel
+ */
+#define TW5864_ENC_BUF_PTR_REC2 0x004c
+
+/* Current MV Flag Status Pointer for Channel n. (Read only) */
+/*
+ * [1:0] CH0_MV_PTR, ..., [15:14] CH7_MV_PTR
+ */
+#define TW5864_CH_MV_PTR1 0x0060
+/*
+ * [1:0] CH8_MV_PTR, ..., [15:14] CH15_MV_PTR
+ */
+#define TW5864_CH_MV_PTR2 0x0064
+
+/*
+ * [15:0] Reset Current MV Flag Status Pointer for Channel n (one bit each)
+ */
+#define TW5864_RST_MV_PTR 0x0068
+#define TW5864_INTERLACING 0x0200
+/* Define controls in register TW5864_INTERLACING */
+/*
+ * Inter_Mode Start. 2-nd bit? A guess. Missing in datasheet. Without this bit
+ * set, the output video is interlaced (stripy).
+ */
+#define TW5864_DSP_INTER_ST BIT(1)
+/* Deinterlacer Enable */
+#define TW5864_DI_EN BIT(2)
+/*
+ * De-interlacer Mode
+ * 1 Shuffled frame
+ * 0 Normal Un-Shuffled Frame
+ */
+#define TW5864_DI_MD BIT(3)
+/*
+ * Down scale original frame in X direction
+ * 11: Un-used
+ * 10: down-sample to 1/4
+ * 01: down-sample to 1/2
+ * 00: down-sample disabled
+ */
+#define TW5864_DSP_DWN_X (3 << 4)
+/*
+ * Down scale original frame in Y direction
+ * 11: Un-used
+ * 10: down-sample to 1/4
+ * 01: down-sample to 1/2
+ * 00: down-sample disabled
+ */
+#define TW5864_DSP_DWN_Y (3 << 6)
+/*
+ * 1 Dual Stream
+ * 0 Single Stream
+ */
+#define TW5864_DUAL_STR BIT(8)
+
+#define TW5864_DSP_REF 0x0204
+/* Define controls in register TW5864_DSP_REF */
+/* Number of reference frame (Default 1 for TW5864B) */
+#define TW5864_DSP_REF_FRM 0x000f
+/* Window size */
+#define TW5864_DSP_WIN_SIZE 0x02f0
+
+#define TW5864_DSP_SKIP 0x0208
+/* Define controls in register TW5864_DSP_SKIP */
+/*
+ * Skip Offset Enable bit
+ * 0 DSP_SKIP_OFFSET value is not used (default 8)
+ * 1 DSP_SKIP_OFFSET value is used in HW
+ */
+#define TW5864_DSP_SKIP_OFEN 0x0080
+/* Skip mode cost offset (default 8) */
+#define TW5864_DSP_SKIP_OFFSET 0x007f
+
+#define TW5864_MOTION_SEARCH_ETC 0x020c
+/* Define controls in register TW5864_MOTION_SEARCH_ETC */
+/* Enable quarter pel search mode */
+#define TW5864_QPEL_EN BIT(0)
+/* Enable half pel search mode */
+#define TW5864_HPEL_EN BIT(1)
+/* Enable motion search mode */
+#define TW5864_ME_EN BIT(2)
+/* Enable Intra mode */
+#define TW5864_INTRA_EN BIT(3)
+/* Enable Skip Mode */
+#define TW5864_SKIP_EN BIT(4)
+/* Search Option (Default 2"b01) */
+#define TW5864_SRCH_OPT (3 << 5)
+
+#define TW5864_DSP_ENC_REC 0x0210
+/* Define controls in register TW5864_DSP_ENC_REC */
+/* Reference Buffer Pointer for encoding */
+#define TW5864_DSP_ENC_REF_PTR 0x0007
+/* Reconstruct Buffer pointer */
+#define TW5864_DSP_REC_BUF_PTR 0x7000
+
+/* [15:0] Lambda Value for H264 */
+#define TW5864_DSP_REF_MVP_LAMBDA 0x0214
+
+#define TW5864_DSP_PIC_MAX_MB 0x0218
+/* Define controls in register TW5864_DSP_PIC_MAX_MB */
+/* The MB number in Y direction for a frame */
+#define TW5864_DSP_PIC_MAX_MB_Y 0x007f
+/* The MB number in X direction for a frame */
+#define TW5864_DSP_PIC_MAX_MB_X 0x7f00
+
+/* The original frame pointer for encoding */
+#define TW5864_DSP_ENC_ORG_PTR_REG 0x021c
+/* Mask to use with TW5864_DSP_ENC_ORG_PTR */
+#define TW5864_DSP_ENC_ORG_PTR_MASK 0x7000
+/* Number of bits to shift with TW5864_DSP_ENC_ORG_PTR */
+#define TW5864_DSP_ENC_ORG_PTR_SHIFT 12
+
+/* DDR base address of OSD rectangle attribute data */
+#define TW5864_DSP_OSD_ATTRI_BASE 0x0220
+/* OSD enable bit for each channel */
+#define TW5864_DSP_OSD_ENABLE 0x0228
+
+/* 0x0280 ~ 0x029c – Motion Vector for 1st 4x4 Block, e.g., 80 (X), 84 (Y) */
+#define TW5864_ME_MV_VEC1 0x0280
+/* 0x02a0 ~ 0x02bc – Motion Vector for 2nd 4x4 Block, e.g., A0 (X), A4 (Y) */
+#define TW5864_ME_MV_VEC2 0x02a0
+/* 0x02c0 ~ 0x02dc – Motion Vector for 3rd 4x4 Block, e.g., C0 (X), C4 (Y) */
+#define TW5864_ME_MV_VEC3 0x02c0
+/* 0x02e0 ~ 0x02fc – Motion Vector for 4th 4x4 Block, e.g., E0 (X), E4 (Y) */
+#define TW5864_ME_MV_VEC4 0x02e0
+
+/*
+ * [5:0]
+ * if (intra16x16_cost < (intra4x4_cost+dsp_i4x4_offset))
+ * Intra_mode = intra16x16_mode
+ * Else
+ * Intra_mode = intra4x4_mode
+ */
+#define TW5864_DSP_I4x4_OFFSET 0x040c
+
+/*
+ * [6:4]
+ * 0x5 Only 4x4
+ * 0x6 Only 16x16
+ * 0x7 16x16 & 4x4
+ */
+#define TW5864_DSP_INTRA_MODE 0x0410
+#define TW5864_DSP_INTRA_MODE_SHIFT 4
+#define TW5864_DSP_INTRA_MODE_MASK (7 << 4)
+#define TW5864_DSP_INTRA_MODE_4x4 0x5
+#define TW5864_DSP_INTRA_MODE_16x16 0x6
+#define TW5864_DSP_INTRA_MODE_4x4_AND_16x16 0x7
+/*
+ * [5:0] WEIGHT Factor for I4x4 cost calculation (QP dependent)
+ */
+#define TW5864_DSP_I4x4_WEIGHT 0x0414
+
+/*
+ * [7:0] Offset used to affect Intra/ME model decision
+ * If (me_cost < intra_cost + dsp_resid_mode_offset)
+ * Pred_Mode = me_mode
+ * Else
+ * Pred_mode = intra_mode
+ */
+#define TW5864_DSP_RESID_MODE_OFFSET 0x0604
+
+/* 0x0800 ~ 0x09ff - Quantization TABLE Values */
+#define TW5864_QUAN_TAB 0x0800
+
+/* Valid channel value [0; f], frame value [0; 3] */
+#define TW5864_RT_CNTR_CH_FRM(channel, frame) \
+	(0x0c00 | (channel << 4) | (frame << 2))
+
+#define TW5864_FRAME_BUS1 0x0d00
+/*
+ * 1 Progressive in part A in bus n
+ * 0 Interlaced in part A in bus n
+ */
+#define TW5864_PROG_A BIT(0)
+/*
+ * 1 Progressive in part B in bus n
+ * 0 Interlaced in part B in bus n
+ */
+#define TW5864_PROG_B BIT(1)
+/*
+ * 1 Frame Mode in bus n
+ * 0 Field Mode in bus n
+ */
+#define TW5864_FRAME BIT(2)
+/*
+ * 0 4CIF in bus n
+ * 1 1D1 + 4 CIF in bus n
+ * 2 2D1 in bus n
+ */
+#define TW5864_BUS_D1 (3 << 3)
+/* Bus 1 goes in TW5864_FRAME_BUS1 in [4:0] */
+/* Bus 2 goes in TW5864_FRAME_BUS1 in [12:8] */
+#define TW5864_FRAME_BUS2 0x0d04
+/* Bus 3 goes in TW5864_FRAME_BUS2 in [4:0] */
+/* Bus 4 goes in TW5864_FRAME_BUS2 in [12:8] */
+
+/* [15:0] Horizontal Mirror for channel n */
+#define TW5864_SENIF_HOR_MIR 0x0d08
+/* [15:0] Vertical Mirror for channel n */
+#define TW5864_SENIF_VER_MIR 0x0d0c
+
+/*
+ * FRAME_WIDTH_BUSn_A
+ * 0x15f: 4 CIF
+ * 0x2cf: 1 D1 + 3 CIF
+ * 0x2cf: 2 D1
+ * FRAME_WIDTH_BUSn_B
+ * 0x15f: 4 CIF
+ * 0x2cf: 1 D1 + 3 CIF
+ * 0x2cf: 2 D1
+ * FRAME_HEIGHT_BUSn_A
+ * 0x11f: 4CIF (PAL)
+ * 0x23f: 1D1 + 3CIF (PAL)
+ * 0x23f: 2 D1 (PAL)
+ * 0x0ef: 4CIF (NTSC)
+ * 0x1df: 1D1 + 3CIF (NTSC)
+ * 0x1df: 2 D1 (NTSC)
+ * FRAME_HEIGHT_BUSn_B
+ * 0x11f: 4CIF (PAL)
+ * 0x23f: 1D1 + 3CIF (PAL)
+ * 0x23f: 2 D1 (PAL)
+ * 0x0ef: 4CIF (NTSC)
+ * 0x1df: 1D1 + 3CIF (NTSC)
+ * 0x1df: 2 D1 (NTSC)
+ */
+#define TW5864_FRAME_WIDTH_BUS_A(bus) (0x0d10 + 0x0010 * bus)
+#define TW5864_FRAME_WIDTH_BUS_B(bus) (0x0d14 + 0x0010 * bus)
+#define TW5864_FRAME_HEIGHT_BUS_A(bus) (0x0d18 + 0x0010 * bus)
+#define TW5864_FRAME_HEIGHT_BUS_B(bus) (0x0d1c + 0x0010 * bus)
+
+/*
+ * 1: the bus mapped Channel n Full D1
+ * 0: the bus mapped Channel n Half D1
+ */
+#define TW5864_FULL_HALF_FLAG 0x0d50
+
+/*
+ * 0 The bus mapped Channel select partA Mode
+ * 1 The bus mapped Channel select partB Mode
+ */
+#define TW5864_FULL_HALF_MODE_SEL 0x0d54
+
+#define TW5864_VLC 0x1000
+/* Define controls in register TW5864_VLC */
+/* QP Value used by H264 CAVLC */
+#define TW5864_VLC_SLICE_QP 0x003f
+/*
+ * Swap byte order of VLC stream in d-word.
+ * 1 Normal (VLC output= [31:0])
+ * 0 Swap (VLC output={[23:16],[31:24],[7:0], [15:8]})
+ */
+#define TW5864_VLC_BYTE_SWP BIT(6)
+/* Enable Adding 03 circuit for VLC stream */
+#define TW5864_VLC_ADD03_EN BIT(7)
+/* Number of bit for VLC bit Align */
+#define TW5864_VLC_BIT_ALIGN_SHIFT 8
+#define TW5864_VLC_BIT_ALIGN_MASK (0x1f << 8)
+/*
+ * Synchronous Interface select for VLC Stream
+ * 1 CDC_VLCS_MAS read VLC stream
+ * 0 CPU read VLC stream
+ */
+#define TW5864_VLC_INF_SEL BIT(13)
+/* Enable VLC overflow control */
+#define TW5864_VLC_OVFL_CNTL BIT(14)
+/*
+ * 1 PCI Master Mode
+ * 0 Non PCI Master Mode
+ */
+#define TW5864_VLC_PCI_SEL BIT(15)
+/*
+ * 0 Enable Adding 03 to VLC header and stream
+ * 1 Disable Adding 03 to VLC header of "00000001"
+ */
+#define TW5864_VLC_A03_DISAB BIT(16)
+/*
+ * Status of VLC stream in DDR (one bit for each buffer)
+ * 1 VLC is ready in buffer n (HW set)
+ * 0 VLC is not ready in buffer n (SW clear)
+ */
+#define TW5864_VLC_BUF_RDY_SHIFT 24
+#define TW5864_VLC_BUF_RDY_MASK (0xff << 24)
+
+/* Total number of bit in the slice */
+#define TW5864_SLICE_TOTAL_BIT 0x1004
+/* Total number of bit in the residue */
+#define TW5864_RES_TOTAL_BIT 0x1008
+
+#define TW5864_VLC_BUF 0x100c
+/* Define controls in register TW5864_VLC_BUF */
+/* VLC BK0 full status, write ‘1’ to clear */
+#define TW5864_VLC_BK0_FULL BIT(0)
+/* VLC BK1 full status, write ‘1’ to clear */
+#define TW5864_VLC_BK1_FULL BIT(1)
+/* VLC end slice status, write ‘1’ to clear */
+#define TW5864_VLC_END_SLICE BIT(2)
+/* VLC Buffer overflow status, write ‘1’ to clear */
+#define TW5864_DSP_RD_OF BIT(3)
+/* VLC string length in either buffer 0 or 1 at end of frame */
+#define TW5864_VLC_STREAM_LEN_SHIFT 4
+#define TW5864_VLC_STREAM_LEN_MASK (0x1ff << 4)
+
+/* [15:0] Total coefficient number in a frame */
+#define TW5864_TOTAL_COEF_NO 0x1010
+/* [0] VLC Encoder Interrupt. Write ‘1’ to clear */
+#define TW5864_VLC_DSP_INTR 0x1014
+/* [31:0] VLC stream CRC checksum */
+#define TW5864_VLC_STREAM_CRC 0x1018
+
+#define TW5864_VLC_RD 0x101c
+/* Define controls in register TW5864_VLC_RD */
+/*
+ * 1 Read VLC lookup Memory
+ * 0 Read VLC Stream Memory
+ */
+#define TW5864_VLC_RD_MEM BIT(0)
+/*
+ * 1 Read VLC Stream Memory in burst mode
+ * 0 Read VLC Stream Memory in single mode
+ */
+#define TW5864_VLC_RD_BRST BIT(1)
+
+/* 0x2000 ~ 0x2ffc -- H264 Stream Memory Map */
+/*
+ * A word is 4 bytes. I.e.,
+ * VLC_STREAM_MEM[0] address: 0x2000
+ * VLC_STREAM_MEM[1] address: 0x2004
+ * ...
+ * VLC_STREAM_MEM[3FF] address: 0x2ffc
+ */
+#define TW5864_VLC_STREAM_MEM_START 0x2000
+#define TW5864_VLC_STREAM_MEM_MAX_OFFSET 0x3ff
+#define TW5864_VLC_STREAM_MEM(offset) (TW5864_VLC_STREAM_MEM_START + 4 * offset)
+
+/* 0x4000 ~ 0x4ffc -- Audio Register Map */
+/* [31:0] config 1ms cnt = Realtime clk/1000 */
+#define TW5864_CFG_1MS_CNT 0x4000
+
+#define TW5864_ADPCM 0x4004
+/* Define controls in register TW5864_ADPCM */
+/* ADPCM decoder enable */
+#define TW5864_ADPCM_DEC BIT(0)
+/* ADPCM input data enable */
+#define TW5864_ADPCM_IN_DATA BIT(1)
+/* ADPCM encoder enable */
+#define TW5864_ADPCM_ENC BIT(2)
+
+#define TW5864_AUD 0x4008
+/* Define controls in register TW5864_AUD */
+/* Record path PCM Audio enable bit for each channel */
+#define TW5864_AUD_ORG_CH_EN 0x00ff
+/* Speaker path PCM Audio Enable */
+#define TW5864_SPK_ORG_EN BIT(16)
+/*
+ * 0 16bit
+ * 1 8bit
+ */
+#define TW5864_AD_BIT_MODE BIT(17)
+#define TW5864_AUD_TYPE_SHIFT 18
+/*
+ * 0 PCM
+ * 3 ADPCM
+ */
+#define TW5864_AUD_TYPE (0xf << 18)
+#define TW5864_AUD_SAMPLE_RATE_SHIFT 22
+/*
+ * 0 8K
+ * 1 16K
+ */
+#define TW5864_AUD_SAMPLE_RATE (3 << 22)
+/* Channel ID used to select audio channel (0 to 16) for loopback */
+#define TW5864_TESTLOOP_CHID_SHIFT 24
+#define TW5864_TESTLOOP_CHID (0x1f << 24)
+/* Enable AD Loopback Test */
+#define TW5864_TEST_ADLOOP_EN BIT(30)
+/*
+ * 0 Asynchronous Mode or PCI target mode
+ * 1 PCI Initiator Mode
+ */
+#define TW5864_AUD_MODE BIT(31)
+
+#define TW5864_AUD_ADPCM 0x400c
+/* Define controls in register TW5864_AUD_ADPCM */
+/* Record path ADPCM audio channel enable, one bit for each */
+#define TW5864_AUD_ADPCM_CH_EN 0x00ff
+/* Speaker path ADPCM audio channel enable */
+#define TW5864_SPK_ADPCM_EN BIT(16)
+
+#define TW5864_PC_BLOCK_ADPCM_RD_NO 0x4018
+#define TW5864_PC_BLOCK_ADPCM_RD_NO_MASK 0x1f
+
+/*
+ * For ADPCM_ENC_WR_PTR, ADPCM_ENC_RD_PTR (see below):
+ * Bit[2:0] ch0
+ * Bit[5:3] ch1
+ * Bit[8:6] ch2
+ * Bit[11:9] ch3
+ * Bit[14:12] ch4
+ * Bit[17:15] ch5
+ * Bit[20:18] ch6
+ * Bit[23:21] ch7
+ * Bit[26:24] ch8
+ * Bit[29:27] ch9
+ * Bit[32:30] ch10
+ * Bit[35:33] ch11
+ * Bit[38:36] ch12
+ * Bit[41:39] ch13
+ * Bit[44:42] ch14
+ * Bit[47:45] ch15
+ * Bit[50:48] ch16
+ */
+#define TW5864_ADPCM_ENC_XX_MASK 0x3fff
+#define TW5864_ADPCM_ENC_XX_PTR2_SHIFT 30
+/* ADPCM_ENC_WR_PTR[29:0] */
+#define TW5864_ADPCM_ENC_WR_PTR1 0x401c
+/* ADPCM_ENC_WR_PTR[50:30] */
+#define TW5864_ADPCM_ENC_WR_PTR2 0x4020
+
+/* ADPCM_ENC_RD_PTR[29:0] */
+#define TW5864_ADPCM_ENC_RD_PTR1 0x4024
+/* ADPCM_ENC_RD_PTR[50:30] */
+#define TW5864_ADPCM_ENC_RD_PTR2 0x4028
+
+/* [3:0] rd ch0, [7:4] rd ch1, [11:8] wr ch0, [15:12] wr ch1 */
+#define TW5864_ADPCM_DEC_RD_WR_PTR 0x402c
+
+/*
+ * For TW5864_AD_ORIG_WR_PTR, TW5864_AD_ORIG_RD_PTR:
+ * Bit[3:0] ch0
+ * Bit[7:4] ch1
+ * Bit[11:8] ch2
+ * Bit[15:12] ch3
+ * Bit[19:16] ch4
+ * Bit[23:20] ch5
+ * Bit[27:24] ch6
+ * Bit[31:28] ch7
+ * Bit[35:32] ch8
+ * Bit[39:36] ch9
+ * Bit[43:40] ch10
+ * Bit[47:44] ch11
+ * Bit[51:48] ch12
+ * Bit[55:52] ch13
+ * Bit[59:56] ch14
+ * Bit[63:60] ch15
+ * Bit[67:64] ch16
+ */
+/* AD_ORIG_WR_PTR[31:0] */
+#define TW5864_AD_ORIG_WR_PTR1 0x4030
+/* AD_ORIG_WR_PTR[63:32] */
+#define TW5864_AD_ORIG_WR_PTR2 0x4034
+/* AD_ORIG_WR_PTR[67:64] */
+#define TW5864_AD_ORIG_WR_PTR3 0x4038
+
+/* AD_ORIG_RD_PTR[31:0] */
+#define TW5864_AD_ORIG_RD_PTR1 0x403c
+/* AD_ORIG_RD_PTR[63:32] */
+#define TW5864_AD_ORIG_RD_PTR2 0x4040
+/* AD_ORIG_RD_PTR[67:64] */
+#define TW5864_AD_ORIG_RD_PTR3 0x4044
+
+#define TW5864_PC_BLOCK_ORIG_RD_NO 0x4048
+#define TW5864_PC_BLOCK_ORIG_RD_NO_MASK 0x1f
+
+#define TW5864_PCI_AUD 0x404c
+/* Define controls in register TW5864_PCI_AUD */
+/*
+ * The register is applicable to PCI initiator mode only. Used to select PCM(0)
+ * or ADPCM(1) audio data sent to PC. One bit for each channel
+ */
+#define TW5864_PCI_DATA_SEL 0xffff
+/*
+ * Audio flow control mode selection bit.
+ * 0 Flow control disabled. TW5864 continuously sends audio frame to PC
+ * (initiator mode)
+ * 1 Flow control enabled
+ */
+#define TW5864_PCI_FLOW_EN BIT(16)
+/*
+ * When PCI_FLOW_EN is set, PCI need to toggle this bit to send an audio frame
+ * to PC. One toggle to send one frame.
+ */
+#define TW5864_PCI_AUD_FRM_EN BIT(17)
+
+/* [1:0] CS valid to data valid CLK cycles when writing operation */
+#define TW5864_CS2DAT_CNT 0x8000
+/* [2:0] Data valid signal width by system clock cycles */
+#define TW5864_DATA_VLD_WIDTH 0x8004
+
+#define TW5864_SYNC 0x8008
+/* Define controls in register TW5864_SYNC */
+/*
+ * 0 vlc stream to syncrous port
+ * 1 vlc stream to ddr buffers
+ */
+#define TW5864_SYNC_CFG BIT(7)
+/*
+ * 0 SYNC Address sampled on Rising edge
+ * 1 SYNC Address sampled on Falling edge
+ */
+#define TW5864_SYNC_ADR_EDGE BIT(0)
+#define TW5864_VLC_STR_DELAY_SHIFT 1
+/*
+ * 0 No system delay
+ * 1 One system clock delay
+ * 2 Two system clock delay
+ * 3 Three system clock delay
+ */
+#define TW5864_VLC_STR_DELAY (3 << 1)
+/*
+ * 0 Rising edge output
+ * 1 Falling edge output
+ */
+#define TW5864_VLC_OUT_EDGE BIT(3)
+
+/*
+ * [1:0]
+ * 2’b00 phase set to 180 degree
+ * 2’b01 phase set to 270 degree
+ * 2’b10 phase set to 0 degree
+ * 2’b11 phase set to 90 degree
+ */
+#define TW5864_I2C_PHASE_CFG 0x800c
+
+/*
+ * The system / DDR clock (166 MHz) is generated with an on-chip system clock
+ * PLL (SYSPLL) using input crystal clock of 27 MHz. The system clock PLL
+ * frequency is controlled with the following equation.
+ * CLK_OUT = CLK_IN * (M+1) / ((N+1) * P)
+ * SYSPLL_M M parameter
+ * SYSPLL_N N parameter
+ * SYSPLL_P P parameter
+ */
+/* SYSPLL_M[7:0] */
+#define TW5864_SYSPLL1 0x8018
+/* Define controls in register TW5864_SYSPLL1 */
+#define TW5864_SYSPLL_M_LOW 0x00ff
+
+/* [2:0]: SYSPLL_M[10:8], [7:3]: SYSPLL_N[4:0] */
+#define TW5864_SYSPLL2 0x8019
+/* Define controls in register TW5864_SYSPLL2 */
+#define TW5864_SYSPLL_M_HI 0x07
+#define TW5864_SYSPLL_N_LOW_SHIFT 3
+#define TW5864_SYSPLL_N_LOW (0x1f << 3)
+
+/*
+ * [1:0]: SYSPLL_N[6:5], [3:2]: SYSPLL_P, [4]: SYSPLL_IREF, [7:5]: SYSPLL_CP_SEL
+ */
+#define TW5864_SYSPLL3 0x8020
+/* Define controls in register TW5864_SYSPLL3 */
+#define TW5864_SYSPLL_N_HI 0x03
+#define TW5864_SYSPLL_P_SHIFT 2
+#define TW5864_SYSPLL_P (0x03 << 2)
+/*
+ * SYSPLL bias current control
+ * 0 Lower current (default)
+ * 1 30% higher current
+ */
+#define TW5864_SYSPLL_IREF BIT(4)
+/*
+ * SYSPLL charge pump current selection
+ * 0 1,5 uA
+ * 1 4 uA
+ * 2 9 uA
+ * 3 19 uA
+ * 4 39 uA
+ * 5 79 uA
+ * 6 159 uA
+ * 7 319 uA
+ */
+#define TW5864_SYSPLL_CP_SEL_SHIFT 5
+#define TW5864_SYSPLL_CP_SEL (0x07 << 5)
+
+/*
+ * [1:0]: SYSPLL_VCO, [3:2]: SYSPLL_LP_X8, [5:4]: SYSPLL_ICP_SEL,
+ * [6]: SYSPLL_LPF_5PF, [7]: SYSPLL_ED_SEL
+ */
+#define TW5864_SYSPLL4 0x8021
+/* Define controls in register TW5864_SYSPLL4 */
+/*
+ * SYSPLL_VCO VCO Range selection
+ * 00 5 ~ 75 MHz
+ * 01 50 ~ 140 MHz
+ * 10 110 ~ 320 MHz
+ * 11 270 ~ 700 MHz
+ */
+#define TW5864_SYSPLL_VCO 0x03
+#define TW5864_SYSPLL_LP_X8_SHIFT 2
+/*
+ * Loop resister
+ * 0 38.5K ohms
+ * 1 6.6K ohms (default)
+ * 2 2.2K ohms
+ * 3 1.1K ohms
+ */
+#define TW5864_SYSPLL_LP_X8 (0x03 << 2)
+#define TW5864_SYSPLL_ICP_SEL_SHIFT 4
+/*
+ * PLL charge pump fine tune
+ * 00 x1 (default)
+ * 01 x1/2
+ * 10 x1/7
+ * 11 x1/8
+ */
+#define TW5864_SYSPLL_ICP_SEL (0x03 << 4)
+/*
+ * PLL low pass filter phase margin adjustment
+ * 0 no 5pF (default)
+ * 1 5pF added
+ */
+#define TW5864_SYSPLL_LPF_5PF BIT(6)
+/*
+ * PFD select edge for detection
+ * 0 Falling edge (default)
+ * 1 Rising edge
+ */
+#define TW5864_SYSPLL_ED_SEL BIT(7)
+
+/* [0]: SYSPLL_RST, [4]: SYSPLL_PD */
+#define TW5864_SYSPLL5 0x8024
+/* Define controls in register TW5864_SYSPLL5 */
+/* Reset SYSPLL */
+#define TW5864_SYSPLL_RST BIT(0)
+/* Power down SYSPLL */
+#define TW5864_SYSPLL_PD BIT(4)
+
+#define TW5864_PLL_CFG 0x801c
+/* Define controls in register TW5864_PLL_CFG */
+/*
+ * Issue Soft Reset from Async Host Interface / PCI Interface clock domain.
+ * Become valid after sync to the xtal clock domain. This bit is set only if
+ * LOAD register bit is also set to 1.
+ */
+#define TW5864_SRST BIT(0)
+/*
+ * Issue SYSPLL (166 MHz) configuration latch from Async host interface / PCI
+ * Interface clock domain. The configuration setting becomes effective only if
+ * LOAD register bit is also set to 1.
+ */
+#define TW5864_SYSPLL_CFG BIT(2)
+/*
+ * Issue SPLL (108 MHz) configuration load from Async host interface / PCI
+ * Interface clock domain. The configuration setting becomes effective only if
+ * the LOAD register bit is also set to 1.
+ */
+#define TW5864_SPLL_CFG BIT(4)
+/*
+ * Set this bit to latch the SRST, SYSPLL_CFG, SPLL_CFG setting into the xtal
+ * clock domain to restart the PLL. This bit is self cleared.
+ */
+#define TW5864_LOAD BIT(3)
+
+/* SPLL_IREF, SPLL_LPX4, SPLL_CPX4, SPLL_PD, SPLL_DBG */
+#define TW5864_SPLL 0x8028
+
+/* 0x8800 ~ 0x88fc -- Interrupt Register Map */
+/*
+ * Trigger mode of interrupt source 0 ~ 15
+ * 1 Edge trigger mode
+ * 0 Level trigger mode
+ */
+#define TW5864_TRIGGER_MODE_L 0x8800
+/* Trigger mode of interrupt source 16 ~ 31 */
+#define TW5864_TRIGGER_MODE_H 0x8804
+/* Enable of interrupt source 0 ~ 15 */
+#define TW5864_INTR_ENABLE_L 0x8808
+/* Enable of interrupt source 16 ~ 31 */
+#define TW5864_INTR_ENABLE_H 0x880c
+/* Clear interrupt command of interrupt source 0 ~ 15 */
+#define TW5864_INTR_CLR_L 0x8810
+/* Clear interrupt command of interrupt source 16 ~ 31 */
+#define TW5864_INTR_CLR_H 0x8814
+/*
+ * Assertion of interrupt source 0 ~ 15
+ * 1 High level or pos-edge is assertion
+ * 0 Low level or neg-edge is assertion
+ */
+#define TW5864_INTR_ASSERT_L 0x8818
+/* Assertion of interrupt source 16 ~ 31 */
+#define TW5864_INTR_ASSERT_H 0x881c
+/*
+ * Output level of interrupt
+ * 1 Interrupt output is high assertion
+ * 0 Interrupt output is low assertion
+ */
+#define TW5864_INTR_OUT_LEVEL 0x8820
+/*
+ * Status of interrupt source 0 ~ 15
+ * Bit[0]: VLC 4k RAM interrupt
+ * Bit[1]: BURST DDR RAM interrupt
+ * Bit[2]: MV DSP interrupt
+ * Bit[3]: video lost interrupt
+ * Bit[4]: gpio 0 interrupt
+ * Bit[5]: gpio 1 interrupt
+ * Bit[6]: gpio 2 interrupt
+ * Bit[7]: gpio 3 interrupt
+ * Bit[8]: gpio 4 interrupt
+ * Bit[9]: gpio 5 interrupt
+ * Bit[10]: gpio 6 interrupt
+ * Bit[11]: gpio 7 interrupt
+ * Bit[12]: JPEG interrupt
+ * Bit[13:15]: Reserved
+ */
+#define TW5864_INTR_STATUS_L 0x8838
+/*
+ * Status of interrupt source 16 ~ 31
+ * Bit[0]: Reserved
+ * Bit[1]: VLC done interrupt
+ * Bit[2]: Reserved
+ * Bit[3]: AD Vsync interrupt
+ * Bit[4]: Preview eof interrupt
+ * Bit[5]: Preview overflow interrupt
+ * Bit[6]: Timer interrupt
+ * Bit[7]: Reserved
+ * Bit[8]: Audio eof interrupt
+ * Bit[9]: I2C done interrupt
+ * Bit[10]: AD interrupt
+ * Bit[11:15]: Reserved
+ */
+#define TW5864_INTR_STATUS_H 0x883c
+
+/* Defines of interrupt bits, united for both low and high word registers */
+#define TW5864_INTR_VLC_RAM BIT(0)
+#define TW5864_INTR_BURST BIT(1)
+#define TW5864_INTR_MV_DSP BIT(2)
+#define TW5864_INTR_VIN_LOST BIT(3)
+/* n belongs to [0; 7] */
+#define TW5864_INTR_GPIO(n) (1 << (4 + n))
+#define TW5864_INTR_JPEG BIT(12)
+#define TW5864_INTR_VLC_DONE BIT(17)
+#define TW5864_INTR_AD_VSYNC BIT(19)
+#define TW5864_INTR_PV_EOF BIT(20)
+#define TW5864_INTR_PV_OVERFLOW BIT(21)
+#define TW5864_INTR_TIMER BIT(22)
+#define TW5864_INTR_AUD_EOF BIT(24)
+#define TW5864_INTR_I2C_DONE BIT(25)
+#define TW5864_INTR_AD BIT(26)
+
+/* 0x9000 ~ 0x920c -- Video Capture (VIF) Register Map */
+/*
+ * H264EN_CH_STATUS[n] Status of Vsync synchronized H264EN_CH_EN (Read Only)
+ * 1 Channel Enabled
+ * 0 Channel Disabled
+ */
+#define TW5864_H264EN_CH_STATUS 0x9000
+/*
+ * [15:0] H264EN_CH_EN[n] H264 Encoding Path Enable for channel
+ * 1 Channel Enabled
+ * 0 Channel Disabled
+ */
+#define TW5864_H264EN_CH_EN 0x9004
+/*
+ * H264EN_CH_DNS[n] H264 Encoding Path Downscale Video Decoder Input for
+ * channel n
+ * 1 Downscale Y to 1/2
+ * 0 Does not downscale
+ */
+#define TW5864_H264EN_CH_DNS 0x9008
+/*
+ * H264EN_CH_PROG[n] H264 Encoding Path channel n is progressive
+ * 1 Progressive (Not valid for TW5864)
+ * 0 Interlaced (TW5864 default)
+ */
+#define TW5864_H264EN_CH_PROG 0x900c
+/*
+ * [3:0] H264EN_BUS_MAX_CH[n]
+ * H264 Encoding Path maximum number of channel on BUS n
+ * 0 Max 4 channels
+ * 1 Max 2 channels
+ */
+#define TW5864_H264EN_BUS_MAX_CH 0x9010
+
+/*
+ * H264EN_RATE_MAX_LINE_n H264 Encoding path Rate Mapping Maximum Line Number
+ * on Bus n
+ */
+#define TW5864_H264EN_RATE_MAX_LINE_EVEN 0x1f
+#define TW5864_H264EN_RATE_MAX_LINE_ODD_SHIFT 5
+#define TW5864_H264EN_RATE_MAX_LINE_ODD (0x1f << 5)
+/*
+ * [4:0] H264EN_RATE_MAX_LINE_0
+ * [9:5] H264EN_RATE_MAX_LINE_1
+ */
+#define TW5864_H264EN_RATE_MAX_LINE_REG1 0x9014
+/*
+ * [4:0] H264EN_RATE_MAX_LINE_2
+ * [9:5] H264EN_RATE_MAX_LINE_3
+ */
+#define TW5864_H264EN_RATE_MAX_LINE_REG2 0x9018
+
+/*
+ * H264EN_CHn_FMT H264 Encoding Path Format configuration of Channel n
+ * 00 D1 (For D1 and hD1 frame)
+ * 01 (Reserved)
+ * 10 (Reserved)
+ * 11 D1 with 1/2 size in X (for CIF frame)
+ * Note: To be used with 0x9008 register to configure the frame size
+ */
+/*
+ * [1:0]: H264EN_CH0_FMT,
+ * ..., [15:14]: H264EN_CH7_FMT
+ */
+#define TW5864_H264EN_CH_FMT_REG1 0x9020
+/*
+ * [1:0]: H264EN_CH8_FMT (?),
+ * ..., [15:14]: H264EN_CH15_FMT (?)
+ */
+#define TW5864_H264EN_CH_FMT_REG2 0x9024
+
+/*
+ * H264EN_RATE_CNTL_BUSm_CHn H264 Encoding Path BUS m Rate Control for Channel n
+ */
+#define TW5864_H264EN_RATE_CNTL_LO_WORD(bus, channel) \
+	(0x9100 + bus * 0x20 + channel * 0x08)
+#define TW5864_H264EN_RATE_CNTL_HI_WORD(bus, channel) \
+	(0x9104 + bus * 0x20 + channel * 0x08)
+
+/*
+ * H264EN_BUSm_MAP_CHn The 16-to-1 MUX configuration register for each encoding
+ * channel (total of 16 channels). Four bits for each channel.
+ */
+#define TW5864_H264EN_BUS0_MAP 0x9200
+#define TW5864_H264EN_BUS1_MAP 0x9204
+#define TW5864_H264EN_BUS2_MAP 0x9208
+#define TW5864_H264EN_BUS3_MAP 0x920c
+
+/* This register is not defined in datasheet, but used in reference driver */
+#define TW5864_UNDECLARED_ERROR_FLAGS_0x9218 0x9218
+
+#define TW5864_GPIO1 0x9800
+#define TW5864_GPIO2 0x9804
+/* Define controls in registers TW5864_GPIO1, TW5864_GPIO2 */
+/* GPIO DATA of Group n */
+#define TW5864_GPIO_DATA 0x00ff
+#define TW5864_GPIO_OEN_SHIFT 8
+/* GPIO Output Enable of Group n */
+#define TW5864_GPIO_OEN (0xff << 8)
+
+/* 0xa000 ~ 0xa8ff – DDR Controller Register Map */
+/* DDR Controller A */
+/*
+ * [2:0] Data valid counter after read command to DDR. This is the delay value
+ * to show how many cycles the data will be back from DDR after we issue a read
+ * command.
+ */
+#define TW5864_RD_ACK_VLD_MUX 0xa000
+
+#define TW5864_DDR_PERIODS 0xa004
+/* Define controls in register TW5864_DDR_PERIODS */
+/*
+ * Tras value, the minimum cycle of active to precharge command period,
+ * default is 7
+ */
+#define TW5864_TRAS_CNT_MAX 0x000f
+/*
+ * Trfc value, the minimum cycle of refresh to active or refresh command period,
+ * default is 4"hf
+ */
+#define TW5864_RFC_CNT_MAX_SHIFT 8
+#define TW5864_RFC_CNT_MAX (0x0f << 8)
+/*
+ * Trcd value, the minimum cycle of active to internal read/write command
+ * period, default is 4"h2
+ */
+#define TW5864_TCD_CNT_MAX_SHIFT 4
+#define TW5864_TCD_CNT_MAX (0x0f << 4)
+/* Twr value, write recovery time, default is 4"h3 */
+#define TW5864_TWR_CNT_MAX_SHIFT 12
+#define TW5864_TWR_CNT_MAX (0x0f << 12)
+
+/*
+ * [2:0] CAS latency, the delay cycle between internal read command and the
+ * availability of the first bit of output data, default is 3
+ */
+#define TW5864_CAS_LATENCY 0xa008
+/*
+ * [15:0] Maximum average periodic refresh, the value is based on the current
+ * frequency to match 7.8mcs
+ */
+#define TW5864_DDR_REF_CNTR_MAX 0xa00c
+/*
+ * DDR_ON_CHIP_MAP [1:0]
+ * 0 256M DDR on board
+ * 1 512M DDR on board
+ * 2 1G DDR on board
+ * DDR_ON_CHIP_MAP [2]
+ * 0 Only one DDR chip
+ * 1 Two DDR chips
+ */
+#define TW5864_DDR_ON_CHIP_MAP 0xa01c
+#define TW5864_DDR_SELFTEST_MODE 0xa020
+/* Define controls in register TW5864_DDR_SELFTEST_MODE */
+/*
+ * 0 Common read/write mode
+ * 1 DDR self-test mode
+ */
+#define TW5864_MASTER_MODE BIT(0)
+/*
+ * 0 DDR self-test single read/write
+ * 1 DDR self-test burst read/write
+ */
+#define TW5864_SINGLE_PROC BIT(1)
+/*
+ * 0 DDR self-test write command
+ * 1 DDR self-test read command
+ */
+#define TW5864_WRITE_FLAG BIT(2)
+#define TW5864_DATA_MODE_SHIFT 4
+/*
+ * 0 write 32'haaaa5555 to DDR
+ * 1 write 32'hffffffff to DDR
+ * 2 write 32'hha5a55a5a to DDR
+ * 3 write increasing data to DDR
+ */
+#define TW5864_DATA_MODE (0x3 << 4)
+
+/* [7:0] The maximum data of one burst in DDR self-test mode */
+#define TW5864_BURST_CNTR_MAX 0xa024
+/* [15:0] The maximum burst counter (bit 15~0) in DDR self-test mode */
+#define TW5864_DDR_PROC_CNTR_MAX_L 0xa028
+/* The maximum burst counter (bit 31~16) in DDR self-test mode */
+#define TW5864_DDR_PROC_CNTR_MAX_H 0xa02c
+/* [0]: Start one DDR self-test */
+#define TW5864_DDR_SELF_TEST_CMD 0xa030
+/* The maximum error counter (bit 15 ~ 0) in DDR self-test */
+#define TW5864_ERR_CNTR_L 0xa034
+
+#define TW5864_ERR_CNTR_H_AND_FLAG 0xa038
+/* Define controls in register TW5864_ERR_CNTR_H_AND_FLAG */
+/* The maximum error counter (bit 30 ~ 16) in DDR self-test */
+#define TW5864_ERR_CNTR_H_MASK 0x3fff
+/* DDR self-test end flag */
+#define TW5864_END_FLAG 0x8000
+
+/*
+ * DDR Controller B: same as 0xa000 ~ 0xa038, but add TW5864_DDR_B_OFFSET to all
+ * addresses
+ */
+#define TW5864_DDR_B_OFFSET 0x0800
+
+/* 0xb004 ~ 0xb018 – HW version/ARB12 Register Map */
+/* [15:0] Default is C013 */
+#define TW5864_HW_VERSION 0xb004
+
+#define TW5864_REQS_ENABLE 0xb010
+/* Define controls in register TW5864_REQS_ENABLE */
+/* Audio data in to DDR enable (default 1) */
+#define TW5864_AUD_DATA_IN_ENB BIT(0)
+/* Audio encode request to DDR enable (default 1) */
+#define TW5864_AUD_ENC_REQ_ENB BIT(1)
+/* Audio decode request0 to DDR enable (default 1) */
+#define TW5864_AUD_DEC_REQ0_ENB BIT(2)
+/* Audio decode request1 to DDR enable (default 1) */
+#define TW5864_AUD_DEC_REQ1_ENB BIT(3)
+/* VLC stream request to DDR enable (default 1) */
+#define TW5864_VLC_STRM_REQ_ENB BIT(4)
+/* H264 MV request to DDR enable (default 1) */
+#define TW5864_DVM_MV_REQ_ENB BIT(5)
+/* mux_core MVD request to DDR enable (default 1) */
+#define TW5864_MVD_REQ_ENB BIT(6)
+/* mux_core MVD temp data request to DDR enable (default 1) */
+#define TW5864_MVD_TMP_REQ_ENB BIT(7)
+/* JPEG request to DDR enable (default 1) */
+#define TW5864_JPEG_REQ_ENB BIT(8)
+/* mv_flag request to DDR enable (default 1) */
+#define TW5864_MV_FLAG_REQ_ENB BIT(9)
+
+#define TW5864_ARB12 0xb018
+/* Define controls in register TW5864_ARB12 */
+/* ARB12 Enable (default 1) */
+#define TW5864_ARB12_ENB BIT(15)
+/* ARB12 maximum value of time out counter (default 15"h1FF) */
+#define TW5864_ARB12_TIME_OUT_CNT 0x7fff
+
+/* 0xb800 ~ 0xb80c -- Indirect Access Register Map */
+/*
+ * Spec says:
+ * In order to access the indirect register space, the following procedure is
+ * followed.
+ * But reference driver implementation, and current driver, too, does it
+ * differently.
+ *
+ * Write Registers:
+ * (1) Write IND_DATA at 0xb804 ~ 0xb807
+ * (2) Read BUSY flag from 0xb803. Wait until BUSY signal is 0.
+ * (3) Write IND_ADDR at 0xb800 ~ 0xb801. Set R/W to "1", ENABLE to "1"
+ * Read Registers:
+ * (1) Read BUSY flag from 0xb803. Wait until BUSY signal is 0.
+ * (2) Write IND_ADDR at 0xb800 ~ 0xb801. Set R/W to "0", ENABLE to "1"
+ * (3) Read BUSY flag from 0xb803. Wait until BUSY signal is 0.
+ * (4) Read IND_DATA from 0xb804 ~ 0xb807
+ */
+#define TW5864_IND_CTL 0xb800
+/* Define controls in register TW5864_IND_CTL */
+/* Address used to access indirect register space */
+#define TW5864_IND_ADDR 0x0000ffff
+/* Wait until this bit is "0" before using indirect access */
+#define TW5864_BUSY BIT(31)
+/* Activate the indirect access. This bit is self cleared */
+#define TW5864_ENABLE BIT(25)
+/* Read/Write command */
+#define TW5864_RW BIT(24)
+
+/* [31:0] Data used to read/write indirect register space */
+#define TW5864_IND_DATA 0xb804
+
+/* 0xc000 ~ 0xc7fc -- Preview Register Map */
+/* Mostly skipped this section. */
+/*
+ * [15:0] Status of Vsync Synchronized PCI_PV_CH_EN (Read Only)
+ * 1 Channel Enabled
+ * 0 Channel Disabled
+ */
+#define TW5864_PCI_PV_CH_STATUS 0xc000
+/*
+ * [15:0] PCI Preview Path Enable for channel n
+ * 1 Channel Enable
+ * 0 Channel Disable
+ */
+#define TW5864_PCI_PV_CH_EN 0xc004
+
+/* 0xc800 ~ 0xc804 -- JPEG Capture Register Map */
+/* Skipped. */
+/* 0xd000 ~ 0xd0fc -- JPEG Control Register Map */
+/* Skipped. */
+
+/* 0xe000 ~ 0xfc04 – Motion Vector Register Map */
+
+/* ME Motion Vector data (Four Byte Each) 0xe000 ~ 0xe7fc */
+#define TW5864_ME_MV_VEC_START 0xe000
+#define TW5864_ME_MV_VEC_MAX_OFFSET 0x1ff
+#define TW5864_ME_MV_VEC(offset) (TW5864_ME_MV_VEC_START + 4 * offset)
+
+#define TW5864_MV 0xfc00
+/* Define controls in register TW5864_MV */
+/* mv bank0 full status , write "1" to clear */
+#define TW5864_MV_BK0_FULL BIT(0)
+/* mv bank1 full status , write "1" to clear */
+#define TW5864_MV_BK1_FULL BIT(1)
+/* slice end status; write "1" to clear */
+#define TW5864_MV_EOF BIT(2)
+/* mv encode interrupt status; write "1" to clear */
+#define TW5864_MV_DSP_INTR BIT(3)
+/* mv write memory overflow, write "1" to clear */
+#define TW5864_DSP_WR_OF BIT(4)
+#define TW5864_MV_LEN_SHIFT 5
+/* mv stream length */
+#define TW5864_MV_LEN (0xff << 5)
+/* The configured status bit written into bit 15 of 0xfc04 */
+#define TW5864_MPI_DDR_SEL BIT(13)
+
+#define TW5864_MPI_DDR_SEL_REG 0xfc04
+/* Define controls in register TW5864_MPI_DDR_SEL_REG */
+/*
+ * SW configure register
+ * 0 MV is saved in internal DPR
+ * 1 MV is saved in DDR
+ */
+#define TW5864_MPI_DDR_SEL2 BIT(15)
+
+/* 0x18000 ~ 0x181fc – PCI Master/Slave Control Map */
+#define TW5864_PCI_INTR_STATUS 0x18000
+/* Define controls in register TW5864_PCI_INTR_STATUS */
+/* vlc done */
+#define TW5864_VLC_DONE_INTR BIT(1)
+/* ad vsync */
+#define TW5864_AD_VSYNC_INTR BIT(3)
+/* preview eof */
+#define TW5864_PREV_EOF_INTR BIT(4)
+/* preview overflow interrupt */
+#define TW5864_PREV_OVERFLOW_INTR BIT(5)
+/* timer interrupt */
+#define TW5864_TIMER_INTR BIT(6)
+/* audio eof */
+#define TW5864_AUDIO_EOF_INTR BIT(8)
+/* IIC done */
+#define TW5864_IIC_DONE_INTR BIT(24)
+/* ad interrupt (e.g.: video lost, video format changed) */
+#define TW5864_AD_INTR_REG BIT(25)
+
+#define TW5864_PCI_INTR_CTL 0x18004
+/* Define controls in register TW5864_PCI_INTR_CTL */
+/* master enable */
+#define TW5864_PCI_MAST_ENB BIT(0)
+/* mvd&vlc master enable */
+#define TW5864_MVD_VLC_MAST_ENB 0x06
+/* (Need to set 0 in TW5864A) */
+#define TW5864_AD_MAST_ENB BIT(3)
+/* preview master enable */
+#define TW5864_PREV_MAST_ENB BIT(4)
+/* preview overflow enable */
+#define TW5864_PREV_OVERFLOW_ENB BIT(5)
+/* timer interrupt enable */
+#define TW5864_TIMER_INTR_ENB BIT(6)
+/* JPEG master (push mode) enable */
+#define TW5864_JPEG_MAST_ENB BIT(7)
+#define TW5864_AU_MAST_ENB_CHN_SHIFT 8
+/* audio master channel enable */
+#define TW5864_AU_MAST_ENB_CHN (0xffff << 8)
+/* IIC interrupt enable */
+#define TW5864_IIC_INTR_ENB BIT(24)
+/* ad interrupt enable */
+#define TW5864_AD_INTR_ENB BIT(25)
+/* target burst enable */
+#define TW5864_PCI_TAR_BURST_ENB BIT(26)
+/* vlc stream burst enable */
+#define TW5864_PCI_VLC_BURST_ENB BIT(27)
+/* ddr burst enable (1 enable, and must set DDR_BRST_EN) */
+#define TW5864_PCI_DDR_BURST_ENB BIT(28)
+
+/*
+ * Because preview and audio have 16 channels separately, so using this
+ * registers to indicate interrupt status for every channels. This is secondary
+ * interrupt status register. OR operating of the PREV_INTR_REG is
+ * PREV_EOF_INTR, OR operating of the AU_INTR_REG bits is AUDIO_EOF_INTR
+ */
+#define TW5864_PREV_AND_AU_INTR 0x18008
+/* Define controls in register TW5864_PREV_AND_AU_INTR */
+/* preview eof interrupt flag */
+#define TW5864_PREV_INTR_REG 0x0000ffff
+#define TW5864_AU_INTR_REG_SHIFT 16
+/* audio eof interrupt flag */
+#define TW5864_AU_INTR_REG (0xffff << 16)
+
+#define TW5864_MASTER_ENB_REG 0x1800c
+/* Define controls in register TW5864_MASTER_ENB_REG */
+/* master enable */
+#define TW5864_PCI_VLC_INTR_ENB BIT(1)
+/* mvd and vlc master enable */
+#define TW5864_PCI_PREV_INTR_ENB BIT(4)
+/* ad vsync master enable */
+#define TW5864_PCI_PREV_OF_INTR_ENB BIT(5)
+/* jpeg master enable */
+#define TW5864_PCI_JPEG_INTR_ENB BIT(7)
+/* preview master enable */
+#define TW5864_PCI_AUD_INTR_ENB BIT(8)
+
+/*
+ * Every channel of preview and audio have ping-pong buffers in system memory,
+ * this register is the buffer flag to notify software which buffer is been
+ * operated.
+ */
+#define TW5864_PREV_AND_AU_BUF_FLAG 0x18010
+/* Define controls in register TW5864_PREV_AND_AU_BUF_FLAG */
+/* preview buffer A/B flag */
+#define TW5864_PREV_BUF_FLAG 0xffff
+#define TW5864_AUDIO_BUF_FLAG_SHIFT 16
+/* audio buffer A/B flag */
+#define TW5864_AUDIO_BUF_FLAG (0xffff << 16)
+
+#define TW5864_IIC 0x18014
+/* Define controls in register TW5864_IIC */
+/* register data */
+#define TW5864_IIC_DATA 0x00ff
+#define TW5864_IIC_REG_ADDR_SHIFT 8
+/* register addr */
+#define TW5864_IIC_REG_ADDR (0xff << 8)
+/* rd/wr flag rd=1,wr=0 */
+#define TW5864_IIC_RW BIT(16)
+#define TW5864_IIC_DEV_ADDR_SHIFT 17
+/* device addr */
+#define TW5864_IIC_DEV_ADDR (0x7f << 17)
+/*
+ * iic done, software kick off one time iic transaction through setting this
+ * bit to 1. Then poll this bit, value 1 indicate iic transaction have
+ * completed, if read, valid data have been stored in iic_data
+ */
+#define TW5864_IIC_DONE BIT(24)
+
+#define TW5864_RST_AND_IF_INFO 0x18018
+/* Define controls in register TW5864_RST_AND_IF_INFO */
+/* application software soft reset */
+#define TW5864_APP_SOFT_RST BIT(0)
+#define TW5864_PCI_INF_VERSION_SHIFT 16
+/* PCI interface version, read only */
+#define TW5864_PCI_INF_VERSION (0xffff << 16)
+
+/* vlc stream crc value, it is calculated in pci module */
+#define TW5864_VLC_CRC_REG 0x1801c
+/*
+ * vlc max length, it is defined by software based on software assign memory
+ * space for vlc
+ */
+#define TW5864_VLC_MAX_LENGTH 0x18020
+/* vlc length of one frame */
+#define TW5864_VLC_LENGTH 0x18024
+/* vlc original crc value */
+#define TW5864_VLC_INTRA_CRC_I_REG 0x18028
+/* vlc original crc value */
+#define TW5864_VLC_INTRA_CRC_O_REG 0x1802c
+/* mv stream crc value, it is calculated in pci module */
+#define TW5864_VLC_PAR_CRC_REG 0x18030
+/* mv length */
+#define TW5864_VLC_PAR_LENGTH_REG 0x18034
+/* mv original crc value */
+#define TW5864_VLC_PAR_I_REG 0x18038
+/* mv original crc value */
+#define TW5864_VLC_PAR_O_REG 0x1803c
+
+/*
+ * Configuration register for 9[or 10] CIFs or 1D1+15QCIF Preview mode.
+ * PREV_PCI_ENB_CHN[0] Enable 9th preview channel (9CIF prev) or 1D1 channel in
+ * (1D1+15QCIF prev)
+ * PREV_PCI_ENB_CHN[1] Enable 10th preview channel
+ */
+#define TW5864_PREV_PCI_ENB_CHN 0x18040
+/* Description skipped. */
+#define TW5864_PREV_FRAME_FORMAT_IN 0x18044
+/* IIC enable */
+#define TW5864_IIC_ENB 0x18048
+/*
+ * Timer interrupt interval
+ * 0 1ms
+ * 1 2ms
+ * 2 4ms
+ * 3 8ms
+ */
+#define TW5864_PCI_INTTM_SCALE 0x1804c
+
+/*
+ * The above register is pci base address registers. Application software will
+ * initialize them to tell chip where the corresponding stream will be dumped
+ * to. Application software will select appropriate base address interval based
+ * on the stream length.
+ */
+/* VLC stream base address */
+#define TW5864_VLC_STREAM_BASE_ADDR 0x18080
+/* MV stream base address */
+#define TW5864_MV_STREAM_BASE_ADDR 0x18084
+/* 0x180a0 – 0x180bc: audio burst base address. Skipped. */
+/* 0x180c0 ~ 0x180dc – JPEG Push Mode Buffer Base Address. Skipped. */
+/* 0x18100 – 0x1817c: preview burst base address. Skipped. */
+
+/* 0x80000 ~ 0x87fff -- DDR Burst RW Register Map */
+#define TW5864_DDR_CTL 0x80000
+/* Define controls in register TW5864_DDR_CTL */
+#define TW5864_BRST_LENGTH_SHIFT 2
+/* Length of 32-bit data burst */
+#define TW5864_BRST_LENGTH (0x3fff << 2)
+/*
+ * Burst Read/Write
+ * 0 Read Burst from DDR
+ * 1 Write Burst to DDR
+ */
+#define TW5864_BRST_RW BIT(16)
+/* Begin a new DDR Burst. This bit is self cleared */
+#define TW5864_NEW_BRST_CMD BIT(17)
+/* DDR Burst End Flag */
+#define TW5864_BRST_END BIT(24)
+/* Enable Error Interrupt for Single DDR Access */
+#define TW5864_SING_ERR_INTR BIT(25)
+/* Enable Error Interrupt for Burst DDR Access */
+#define TW5864_BRST_ERR_INTR BIT(26)
+/* Enable Interrupt for End of DDR Burst Access */
+#define TW5864_BRST_END_INTR BIT(27)
+/* DDR Single Access Error Flag */
+#define TW5864_SINGLE_ERR BIT(28)
+/* DDR Single Access Busy Flag */
+#define TW5864_SINGLE_BUSY BIT(29)
+/* DDR Burst Access Error Flag */
+#define TW5864_BRST_ERR BIT(30)
+/* DDR Burst Access Busy Flag */
+#define TW5864_BRST_BUSY BIT(31)
+
+/* [27:0] DDR Access Address. Bit [1:0] has to be 0 */
+#define TW5864_DDR_ADDR 0x80004
+/* DDR Access Internal Buffer Address. Bit [1:0] has to be 0 */
+#define TW5864_DPR_BUF_ADDR 0x80008
+/* SRAM Buffer MPI Access Space. Totally 16 KB */
+#define TW5864_DPR_BUF_START 0x84000
+/* 0x84000 - 0x87ffc */
+#define TW5864_DPR_BUF_SIZE 0x4000
+
+/* Indirect Map Space */
+/*
+ * The indirect space is accessed through 0xb800 ~ 0xb807 registers in direct
+ * access space
+ */
+/* Analog Video / Audio Decoder / Encoder */
+/* Allowed channel values: [0; 3] */
+/* Read-only register */
+#define TW5864_INDIR_VIN_0(channel) (0x000 + channel * 0x010)
+/* Define controls in register TW5864_INDIR_VIN_0 */
+/*
+ * 1 Video not present. (sync is not detected in number of consecutive line
+ * periods specified by MISSCNT register)
+ * 0 Video detected.
+ */
+#define TW5864_INDIR_VIN_0_VDLOSS BIT(7)
+/*
+ * 1 Horizontal sync PLL is locked to the incoming video source.
+ * 0 Horizontal sync PLL is not locked.
+ */
+#define TW5864_INDIR_VIN_0_HLOCK BIT(6)
+/*
+ * 1 Sub-carrier PLL is locked to the incoming video source.
+ * 0 Sub-carrier PLL is not locked.
+ */
+#define TW5864_INDIR_VIN_0_SLOCK BIT(5)
+/*
+ * 1 Even field is being decoded.
+ * 0 Odd field is being decoded.
+ */
+#define TW5864_INDIR_VIN_0_FLD BIT(4)
+/*
+ * 1 Vertical logic is locked to the incoming video source.
+ * 0 Vertical logic is not locked.
+ */
+#define TW5864_INDIR_VIN_0_VLOCK BIT(3)
+/*
+ * 1 No color burst signal detected.
+ * 0 Color burst signal detected.
+ */
+#define TW5864_INDIR_VIN_0_MONO BIT(1)
+/*
+ * 0 60Hz source detected
+ * 1 50Hz source detected
+ * The actual vertical scanning frequency depends on the current standard
+ * invoked.
+ */
+#define TW5864_INDIR_VIN_0_DET50 BIT(0)
+
+#define TW5864_INDIR_VIN_1(channel) (0x001 + channel * 0x010)
+/* VCR signal indicator. Read-only. */
+#define TW5864_INDIR_VIN_1_VCR BIT(7)
+/* Weak signal indicator 2. Read-only. */
+#define TW5864_INDIR_VIN_1_WKAIR BIT(6)
+/* Weak signal indicator controlled by WKTH. Read-only. */
+#define TW5864_INDIR_VIN_1_WKAIR1 BIT(5)
+/*
+ * 1 = Standard signal
+ * 0 = Non-standard signal
+ * Read-only
+ */
+#define TW5864_INDIR_VIN_1_VSTD BIT(4)
+/*
+ * 1 = Non-interlaced signal
+ * 0 = interlaced signal
+ * Read-only
+ */
+#define TW5864_INDIR_VIN_1_NINTL BIT(3)
+/*
+ * Vertical Sharpness Control. Writable.
+ * 0 = None (default)
+ * 7 = Highest
+ * **Note: VSHP must be set to ‘0’ if COMB = 0
+ */
+#define TW5864_INDIR_VIN_1_VSHP 0x07
+
+/* HDELAY_XY[7:0] */
+#define TW5864_INDIR_VIN_2_HDELAY_XY_LO(channel) (0x002 + channel * 0x010)
+/* HACTIVE_XY[7:0] */
+#define TW5864_INDIR_VIN_3_HACTIVE_XY_LO(channel) (0x003 + channel * 0x010)
+/* VDELAY_XY[7:0] */
+#define TW5864_INDIR_VIN_4_VDELAY_XY_LO(channel) (0x004 + channel * 0x010)
+/* VACTIVE_XY[7:0] */
+#define TW5864_INDIR_VIN_5_VACTIVE_XY_LO(channel) (0x005 + channel * 0x010)
+
+#define TW5864_INDIR_VIN_6(channel) (0x006 + channel * 0x010)
+/* Define controls in register TW5864_INDIR_VIN_6 */
+#define TW5864_INDIR_VIN_6_HDELAY_XY_HI 0x03
+#define TW5864_INDIR_VIN_6_HACTIVE_XY_HI_SHIFT 2
+#define TW5864_INDIR_VIN_6_HACTIVE_XY_HI (0x03 << 2)
+#define TW5864_INDIR_VIN_6_VDELAY_XY_HI BIT(4)
+#define TW5864_INDIR_VIN_6_VACTIVE_XY_HI BIT(5)
+
+/*
+ * HDELAY_XY This 10bit register defines the starting location of horizontal
+ * active pixel for display / record path. A unit is 1 pixel. The default value
+ * is 0x00f for NTSC and 0x00a for PAL.
+ *
+ * HACTIVE_XY This 10bit register defines the number of horizontal active pixel
+ * for display / record path. A unit is 1 pixel. The default value is decimal
+ * 720.
+ *
+ * VDELAY_XY This 9bit register defines the starting location of vertical
+ * active for display / record path. A unit is 1 line. The default value is
+ * decimal 6.
+ *
+ * VACTIVE_XY This 9bit register defines the number of vertical active lines
+ * for display / record path. A unit is 1 line. The default value is decimal
+ * 240.
+ */
+
+/* HUE These bits control the color hue as 2's complement number. They have
+ * value from +36o (7Fh) to -36o (80h) with an increment of 2.8o. The 2 LSB has
+ * no effect. The positive value gives greenish tone and negative value gives
+ * purplish tone. The default value is 0o (00h). This is effective only on NTSC
+ * system. The default is 00h.
+ */
+#define TW5864_INDIR_VIN_7_HUE(channel) (0x007 + channel * 0x010)
+
+#define TW5864_INDIR_VIN_8(channel) (0x008 + channel * 0x010)
+/* Define controls in register TW5864_INDIR_VIN_8 */
+/*
+ * This bit controls the center frequency of the peaking filter.
+ * The corresponding gain adjustment is HFLT.
+ * 0 Low
+ * 1 center
+ */
+#define TW5864_INDIR_VIN_8_SCURVE BIT(7)
+/* CTI level selection. The default is 1.
+ * 0 None
+ * 3 Highest
+ */
+#define TW5864_INDIR_VIN_8_CTI_SHIFT 4
+#define TW5864_INDIR_VIN_8_CTI (0x03 << 4)
+
+/*
+ * These bits control the amount of sharpness enhancement on the luminance
+ * signals. There are 16 levels of control with "0" having no effect on the
+ * output image. 1 through 15 provides sharpness enhancement with "F" being the
+ * strongest. The default is 1.
+ */
+#define TW5864_INDIR_VIN_8_SHARPNESS 0x0f
+
+/*
+ * These bits control the luminance contrast gain. A value of 100 (64h) has a
+ * gain of 1. The range adjustment is from 0% to 255% at 1% per step. The
+ * default is 64h.
+ */
+#define TW5864_INDIR_VIN_9_CNTRST(channel) (0x009 + channel * 0x010)
+
+/*
+ * These bits control the brightness. They have value of –128 to 127 in 2's
+ * complement form. Positive value increases brightness. A value 0 has no
+ * effect on the data. The default is 00h.
+ */
+#define TW5864_INDIR_VIN_A_BRIGHT(channel) (0x00a + channel * 0x010)
+
+/*
+ * These bits control the digital gain adjustment to the U (or Cb) component of
+ * the digital video signal. The color saturation can be adjusted by adjusting
+ * the U and V color gain components by the same amount in the normal
+ * situation. The U and V can also be adjusted independently to provide greater
+ * flexibility. The range of adjustment is 0 to 200%. A value of 128 (80h) has
+ * gain of 100%. The default is 80h.
+ */
+#define TW5864_INDIR_VIN_B_SAT_U(channel) (0x00b + channel * 0x010)
+
+/*
+ * These bits control the digital gain adjustment to the V (or Cr) component of
+ * the digital video signal. The color saturation can be adjusted by adjusting
+ * the U and V color gain components by the same amount in the normal
+ * situation. The U and V can also be adjusted independently to provide greater
+ * flexibility. The range of adjustment is 0 to 200%. A value of 128 (80h) has
+ * gain of 100%. The default is 80h.
+ */
+#define TW5864_INDIR_VIN_C_SAT_V(channel) (0x00c + channel * 0x010)
+
+/* Read-only */
+#define TW5864_INDIR_VIN_D(channel) (0x00d + channel * 0x010)
+/* Define controls in register TW5864_INDIR_VIN_D */
+/* Macrovision color stripe detection may be un-reliable */
+#define TW5864_INDIR_VIN_D_CSBAD BIT(3)
+/* Macrovision AGC pulse detected */
+#define TW5864_INDIR_VIN_D_MCVSN BIT(2)
+/* Macrovision color stripe protection burst detected */
+#define TW5864_INDIR_VIN_D_CSTRIPE BIT(1)
+/*
+ * This bit is valid only when color stripe protection is detected, i.e. if
+ * CSTRIPE=1,
+ * 1 Type 2 color stripe protection
+ * 0 Type 3 color stripe protection
+ */
+#define TW5864_INDIR_VIN_D_CTYPE2 BIT(0)
+
+/* Read-only */
+#define TW5864_INDIR_VIN_E(channel) (0x00e + channel * 0x010)
+/* Define controls in register TW5864_INDIR_VIN_E */
+/*
+ * Read-only.
+ * 0 Idle
+ * 1 Detection in progress
+ */
+#define TW5864_INDIR_VIN_E_DETSTUS BIT(7)
+/*
+ * STDNOW Current standard invoked
+ * 0 NTSC (M)
+ * 1 PAL (B, D, G, H, I)
+ * 2 SECAM
+ * 3 NTSC4.43
+ * 4 PAL (M)
+ * 5 PAL (CN)
+ * 6 PAL 60
+ * 7 Not valid
+ */
+#define TW5864_INDIR_VIN_E_STDNOW_SHIFT 4
+#define TW5864_INDIR_VIN_E_STDNOW (0x07 << 4)
+
+/*
+ * 1 Disable the shadow registers
+ * 0 Enable VACTIVE and HDELAY shadow registers value depending on STANDARD.
+ * (Default)
+ */
+#define TW5864_INDIR_VIN_E_ATREG BIT(3)
+/*
+ * STANDARD Standard selection
+ * 0 NTSC (M)
+ * 1 PAL (B, D, G, H, I)
+ * 2 SECAM
+ * 3 NTSC4.43
+ * 4 PAL (M)
+ * 5 PAL (CN)
+ * 6 PAL 60
+ * 7 Auto detection (Default)
+ */
+#define TW5864_INDIR_VIN_E_STANDARD 0x07
+
+#define TW5864_INDIR_VIN_F(channel) (0x00f + channel * 0x010)
+/* Define controls in register TW5864_INDIR_VIN_F */
+/*
+ * 1 Writing 1 to this bit will manually initiate the auto format detection
+ * process. This bit is a self-clearing bit
+ * 0 Manual initiation of auto format detection is done. (Default)
+ */
+#define TW5864_INDIR_VIN_F_ATSTART BIT(7)
+/* Enable recognition of PAL60 (Default) */
+#define TW5864_INDIR_VIN_F_PAL60EN BIT(6)
+/* Enable recognition of PAL (CN). (Default) */
+#define TW5864_INDIR_VIN_F_PALCNEN BIT(5)
+/* Enable recognition of PAL (M). (Default) */
+#define TW5864_INDIR_VIN_F_PALMEN BIT(4)
+/* Enable recognition of NTSC 4.43. (Default) */
+#define TW5864_INDIR_VIN_F_NTSC44EN BIT(3)
+/* Enable recognition of SECAM. (Default) */
+#define TW5864_INDIR_VIN_F_SECAMEN BIT(2)
+/* Enable recognition of PAL (B, D, G, H, I). (Default) */
+#define TW5864_INDIR_VIN_F_PALBEN BIT(1)
+/* Enable recognition of NTSC (M). (Default) */
+#define TW5864_INDIR_VIN_F_NTSCEN BIT(0)
+
+/* Some registers skipped. */
+
+/* Use falling edge to sample VD1-VD4 from 54 MHz to 108 MHz */
+#define TW5864_INDIR_VD_108_POL 0x041
+#define TW5864_INDIR_VD_108_POL_VD12 BIT(0)
+#define TW5864_INDIR_VD_108_POL_VD34 BIT(1)
+#define TW5864_INDIR_VD_108_POL_BOTH \
+	(TW5864_INDIR_VD_108_POL_VD12 | TW5864_INDIR_VD_108_POL_VD34)
+
+/* Some registers skipped. */
+
+/*
+ * Audio Input ADC gain control
+ * 0 0.25
+ * 1 0.31
+ * 2 0.38
+ * 3 0.44
+ * 4 0.50
+ * 5 0.63
+ * 6 0.75
+ * 7 0.88
+ * 8 1.00 (default)
+ * 9 1.25
+ * 10 1.50
+ * 11 1.75
+ * 12 2.00
+ * 13 2.25
+ * 14 2.50
+ * 15 2.75
+ */
+/* [3:0] channel 0, [7:4] channel 1 */
+#define TW5864_INDIR_AIGAIN1 0x060
+/* [3:0] channel 2, [7:4] channel 3 */
+#define TW5864_INDIR_AIGAIN2 0x061
+
+/* Some registers skipped */
+
+#define TW5864_INDIR_AIN_0x06D 0x06d
+/* Define controls in register TW5864_INDIR_AIN_0x06D */
+/*
+ * LAWMD Select u-Law/A-Law/PCM/SB data output format on ADATR and ADATM pin.
+ * 0 PCM output (default)
+ * 1 SB (Signed MSB bit in PCM data is inverted) output
+ * 2 u-Law output
+ * 3 A-Law output
+ */
+#define TW5864_INDIR_AIN_LAWMD_SHIFT 6
+#define TW5864_INDIR_AIN_LAWMD (0x03 << 6)
+/*
+ * Disable the mixing ratio value for all audio.
+ * 0 Apply individual mixing ratio value for each audio (default)
+ * 1 Apply nominal value for all audio commonly
+ */
+#define TW5864_INDIR_AIN_MIX_DERATIO BIT(5)
+/*
+ * Enable the mute function for audio channel AINn when n is 0 to 3. It effects
+ * only for mixing. When n = 4, it enable the mute function of the playback
+ * audio input. It effects only for single chip or the last stage chip
+ * 0 Normal
+ * 1 Muted (default)
+ */
+#define TW5864_INDIR_AIN_MIX_MUTE 0x1f
+
+/* Some registers skipped */
+
+#define TW5864_INDIR_AIN_0x0E3 0x0e3
+/* Define controls in register TW5864_INDIR_AIN_0x0E3 */
+/*
+ * ADATP signal is coming from external ADPCM decoder, instead of on-chip ADPCM
+ * decoder
+ */
+#define TW5864_INDIR_AIN_0x0E3_EXT_ADATP BIT(7)
+/* ACLKP output signal polarity inverse */
+#define TW5864_INDIR_AIN_0x0E3_ACLKPPOLO BIT(6)
+/*
+ * ACLKR input signal polarity inverse.
+ * 0 Not inversed (Default)
+ * 1 Inversed
+ */
+#define TW5864_INDIR_AIN_0x0E3_ACLKRPOL BIT(5)
+/*
+ * ACLKP input signal polarity inverse.
+ * 0 Not inversed (Default)
+ * 1 Inversed
+ */
+#define TW5864_INDIR_AIN_0x0E3_ACLKPPOLI BIT(4)
+/*
+ * ACKI [21:0] control automatic set up with AFMD registers
+ * This mode is only effective when ACLKRMASTER=1
+ * 0 ACKI [21:0] registers set up ACKI control
+ * 1 ACKI control is automatically set up by AFMD register values
+ */
+#define TW5864_INDIR_AIN_0x0E3_AFAUTO BIT(3)
+/*
+ * AFAUTO control mode
+ * 0 8kHz setting (Default)
+ * 1 16kHz setting
+ * 2 32kHz setting
+ * 3 44.1kHz setting
+ * 4 48kHz setting
+ */
+#define TW5864_INDIR_AIN_0x0E3_AFMD 0x07
+
+#define TW5864_INDIR_AIN_0x0E4 0x0e4
+/* Define controls in register TW5864_INDIR_AIN_0x0ED */
+/*
+ * 8bit I2S Record output mode.
+ * 0 L/R half length separated output (Default).
+ * 1 One continuous packed output equal to DSP output format.
+ */
+#define TW5864_INDIR_AIN_0x0E4_I2S8MODE BIT(7)
+/*
+ * Audio Clock Master ACLKR output wave format.
+ * 0 High periods is one 27MHz clock period (default).
+ * 1 Almost duty 50-50% clock output on ACLKR pin. If this mode is selected, two
+ * times bigger number value need to be set up on the ACKI register. If
+ * AFAUTO=1, ACKI control is automatically set up even if MASCKMD=1.
+ */
+#define TW5864_INDIR_AIN_0x0E4_MASCKMD BIT(6)
+/* Playback ACLKP/ASYNP/ADATP input data MSB-LSB swapping */
+#define TW5864_INDIR_AIN_0x0E4_PBINSWAP BIT(5)
+/*
+ * ASYNR input signal delay.
+ * 0 No delay
+ * 1 Add one 27MHz period delay in ASYNR signal input
+ */
+#define TW5864_INDIR_AIN_0x0E4_ASYNRDLY BIT(4)
+/*
+ * ASYNP input signal delay.
+ * 0 no delay
+ * 1 add one 27MHz period delay in ASYNP signal input
+ */
+#define TW5864_INDIR_AIN_0x0E4_ASYNPDLY BIT(3)
+/*
+ * ADATP input data delay by one ACLKP clock.
+ * 0 No delay (Default). This is for I2S type 1T delay input interface.
+ * 1 Add 1 ACLKP clock delay in ADATP input data. This is for left-justified
+ * type 0T delay input interface.
+ */
+#define TW5864_INDIR_AIN_0x0E4_ADATPDLY BIT(2)
+/*
+ * Select u-Law/A-Law/PCM/SB data input format on ADATP pin.
+ * 0 PCM input (Default)
+ * 1 SB (Signed MSB bit in PCM data is inverted) input
+ * 2 u-Law input
+ * 3 A-Law input
+ */
+#define TW5864_INDIR_AIN_0x0E4_INLAWMD 0x03
+
+/*
+ * Enable state register updating and interrupt request of audio AIN5 detection
+ * for each input
+ */
+#define TW5864_INDIR_AIN_A5DETENA 0x0e5
+
+/* Some registers skipped */
+
+/*
+ * [7:3]: DEV_ID The TW5864 product ID code is 01000
+ * [2:0]: REV_ID The revision number is 0h
+ */
+#define TW5864_INDIR_ID 0x0fe
+
+#define TW5864_INDIR_IN_PIC_WIDTH(channel) (0x200 + 4 * channel)
+#define TW5864_INDIR_IN_PIC_HEIGHT(channel) (0x201 + 4 * channel)
+#define TW5864_INDIR_OUT_PIC_WIDTH(channel) (0x202 + 4 * channel)
+#define TW5864_INDIR_OUT_PIC_HEIGHT(channel) (0x203 + 4 * channel)
+/*
+ * Interrupt status register from the front-end. Write "1" to each bit to clear
+ * the interrupt
+ * 15:0 Motion detection interrupt for channel 0 ~ 15
+ * 31:16 Night detection interrupt for channel 0 ~ 15
+ * 47:32 Blind detection interrupt for channel 0 ~ 15
+ * 63:48 No video interrupt for channel 0 ~ 15
+ * 79:64 Line mode underflow interrupt for channel 0 ~ 15
+ * 95:80 Line mode overflow interrupt for channel 0 ~ 15
+ */
+/* 0x2d0~0x2d7: [63:0] bits */
+#define TW5864_INDIR_INTERRUPT1 0x2d0
+/* 0x2e0~0x2e3: [95:64] bits */
+#define TW5864_INDIR_INTERRUPT2 0x2e0
+
+/*
+ * Interrupt mask register for interrupts in 0x2d0 ~ 0x2d7
+ * 15:0 Motion detection interrupt for channel 0 ~ 15
+ * 31:16 Night detection interrupt for channel 0 ~ 15
+ * 47:32 Blind detection interrupt for channel 0 ~ 15
+ * 63:48 No video interrupt for channel 0 ~ 15
+ * 79:64 Line mode underflow interrupt for channel 0 ~ 15
+ * 95:80 Line mode overflow interrupt for channel 0 ~ 15
+ */
+/* 0x2d8~0x2df: [63:0] bits */
+#define TW5864_INDIR_INTERRUPT_MASK1 0x2d8
+/* 0x2e8~0x2eb: [95:64] bits */
+#define TW5864_INDIR_INTERRUPT_MASK2 0x2e8
+
+/* [11:0]: Interrupt summary register for interrupts & interrupt mask from in
+ * 0x2d0 ~ 0x2d7 and 0x2d8 ~ 0x2df
+ * bit 0: interrupt occurs in 0x2d0 & 0x2d8
+ * bit 1: interrupt occurs in 0x2d1 & 0x2d9
+ * bit 2: interrupt occurs in 0x2d2 & 0x2da
+ * bit 3: interrupt occurs in 0x2d3 & 0x2db
+ * bit 4: interrupt occurs in 0x2d4 & 0x2dc
+ * bit 5: interrupt occurs in 0x2d5 & 0x2dd
+ * bit 6: interrupt occurs in 0x2d6 & 0x2de
+ * bit 7: interrupt occurs in 0x2d7 & 0x2df
+ * bit 8: interrupt occurs in 0x2e0 & 0x2e8
+ * bit 9: interrupt occurs in 0x2e1 & 0x2e9
+ * bit 10: interrupt occurs in 0x2e2 & 0x2ea
+ * bit 11: interrupt occurs in 0x2e3 & 0x2eb
+ */
+#define TW5864_INDIR_INTERRUPT_SUMMARY 0x2f0
+
+/* Motion / Blind / Night Detection */
+/* valid value for channel is [0:15] */
+#define TW5864_INDIR_DETECTION_CTL0(channel) (0x300 + channel * 0x08)
+/* Define controls in register TW5864_INDIR_DETECTION_CTL0 */
+/*
+ * Disable the motion and blind detection.
+ * 0 Enable motion and blind detection (default)
+ * 1 Disable motion and blind detection
+ */
+#define TW5864_INDIR_DETECTION_CTL0_MD_DIS BIT(5)
+/*
+ * Request to start motion detection on manual trigger mode
+ * 0 None Operation (default)
+ * 1 Request to start motion detection
+ */
+#define TW5864_INDIR_DETECTION_CTL0_MD_STRB BIT(3)
+/*
+ * Select the trigger mode of motion detection
+ * 0 Automatic trigger mode of motion detection (default)
+ * 1 Manual trigger mode for motion detection
+ */
+#define TW5864_INDIR_DETECTION_CTL0_MD_STRB_EN BIT(2)
+/*
+ * Define the threshold of cell for blind detection.
+ * 0 Low threshold (More sensitive) (default)
+ * : :
+ * 3 High threshold (Less sensitive)
+ */
+#define TW5864_INDIR_DETECTION_CTL0_BD_CELSENS 0x03
+
+#define TW5864_INDIR_DETECTION_CTL1(channel) (0x301 + channel * 0x08)
+/* Define controls in register TW5864_INDIR_DETECTION_CTL1 */
+/*
+ * Control the temporal sensitivity of motion detector.
+ * 0 More Sensitive (default)
+ * : :
+ * 15 Less Sensitive
+ */
+#define TW5864_INDIR_DETECTION_CTL1_MD_TMPSENS_SHIFT 4
+#define TW5864_INDIR_DETECTION_CTL1_MD_TMPSENS (0x0f << 4)
+/*
+ * Adjust the horizontal starting position for motion detection
+ * 0 0 pixel (default)
+ * : :
+ * 15 15 pixels
+ */
+#define TW5864_INDIR_DETECTION_CTL1_MD_PIXEL_OS 0x0f
+
+#define TW5864_INDIR_DETECTION_CTL2(channel) (0x302 + channel * 0x08)
+/* Define controls in register TW5864_INDIR_DETECTION_CTL2 */
+/*
+ * Control the updating time of reference field for motion detection.
+ * 0 Update reference field every field (default)
+ * 1 Update reference field according to MD_SPEED
+ */
+#define TW5864_INDIR_DETECTION_CTL2_MD_REFFLD BIT(7)
+/*
+ * Select the field for motion detection.
+ * 0 Detecting motion for only odd field (default)
+ * 1 Detecting motion for only even field
+ * 2 Detecting motion for any field
+ * 3 Detecting motion for both odd and even field
+ */
+#define TW5864_INDIR_DETECTION_CTL2_MD_FIELD_SHIFT 5
+#define TW5864_INDIR_DETECTION_CTL2_MD_FIELD (0x03 << 5)
+/*
+ * Control the level sensitivity of motion detector.
+ * 0 More sensitive (default)
+ * : :
+ * 15 Less sensitive
+ */
+#define TW5864_INDIR_DETECTION_CTL2_MD_LVSENS 0x1f
+
+#define TW5864_INDIR_DETECTION_CTL3(channel) (0x303 + channel * 0x08)
+/* Define controls in register TW5864_INDIR_DETECTION_CTL3 */
+/*
+ * Define the threshold of sub-cell number for motion detection.
+ * 0 Motion is detected if 1 sub-cell has motion (More sensitive) (default)
+ * 1 Motion is detected if 2 sub-cells have motion
+ * 2 Motion is detected if 3 sub-cells have motion
+ * 3 Motion is detected if 4 sub-cells have motion (Less sensitive)
+ */
+#define TW5864_INDIR_DETECTION_CTL3_MD_CELSENS_SHIFT 6
+#define TW5864_INDIR_DETECTION_CTL3_MD_CELSENS (0x03 << 6)
+/*
+ * Control the velocity of motion detector.
+ * Large value is suitable for slow motion detection.
+ * In MD_DUAL_EN = 1, MD_SPEED should be limited to 0 ~ 31.
+ * 0 1 field intervals (default)
+ * 1 2 field intervals
+ * : :
+ * 61 62 field intervals
+ * 62 63 field intervals
+ * 63 Not supported
+ */
+#define TW5864_INDIR_DETECTION_CTL3_MD_SPEED 0x3f
+
+#define TW5864_INDIR_DETECTION_CTL4(channel) (0x304 + channel * 0x08)
+/* Define controls in register TW5864_INDIR_DETECTION_CTL4 */
+/*
+ * Control the spatial sensitivity of motion detector.
+ * 0 More Sensitive (default)
+ * : :
+ * 15 Less Sensitive
+ */
+#define TW5864_INDIR_DETECTION_CTL4_MD_SPSENS_SHIFT 4
+#define TW5864_INDIR_DETECTION_CTL4_MD_SPSENS (0x0f << 4)
+/*
+ * Define the threshold of level for blind detection.
+ * 0 Low threshold (More sensitive) (default)
+ * : :
+ * 15 High threshold (Less sensitive)
+ */
+#define TW5864_INDIR_DETECTION_CTL4_BD_LVSENS 0x0f
+
+#define TW5864_INDIR_DETECTION_CTL5(channel) (0x305 + channel * 0x08)
+/*
+ * Define the threshold of temporal sensitivity for night detection.
+ * 0 Low threshold (More sensitive) (default)
+ * : :
+ * 15 High threshold (Less sensitive)
+ */
+#define TW5864_INDIR_DETECTION_CTL5_ND_TMPSENS_SHIFT 4
+#define TW5864_INDIR_DETECTION_CTL5_ND_TMPSENS (0x0f << 4)
+/*
+ * Define the threshold of level for night detection.
+ * 0 Low threshold (More sensitive) (default)
+ * : :
+ * 3 High threshold (Less sensitive)
+ */
+#define TW5864_INDIR_DETECTION_CTL5_ND_LVSENS 0x0f
+
+/*
+ * [11:0] The base address of the motion detection buffer. This address is in
+ * unit of 64K bytes. The generated DDR address will be {MD_BASE_ADDR,
+ * 16"h0000}. The default value should be 12"h000
+ */
+#define TW5864_INDIR_MD_BASE_ADDR 0x380
+
+/*
+ * This controls the channel of the motion detection result shown in register
+ * 0x3a0 ~ 0x3b7. Before reading back motion result, always set this first.
+ */
+#define TW5864_INDIR_RGR_MOTION_SEL 0x382
+
+/* [15:0] MD strobe has been performed at channel n (read only) */
+#define TW5864_INDIR_MD_STRB 0x386
+/* NO_VIDEO Detected from channel n (read only) */
+#define TW5864_INDIR_NOVID_DET 0x388
+/* Motion Detected from channel n (read only) */
+#define TW5864_INDIR_MD_DET 0x38a
+/* Blind Detected from channel n (read only) */
+#define TW5864_INDIR_BD_DET 0x38c
+/* Night Detected from channel n (read only) */
+#define TW5864_INDIR_ND_DET 0x38e
+
+/* 192 bit motion flag of the channel specified by RGR_MOTION_SEL in 0x382 */
+#define TW5864_INDIR_MOTION_FLAG 0x3a0
+#define TW5864_INDIR_MOTION_FLAG_BYTE_COUNT 24
+
+/*
+ * [9:0] The motion cell count of a specific channel selected by 0x382. This is
+ * for DI purpose
+ */
+#define TW5864_INDIR_MD_DI_CNT 0x3b8
+/* The motion detection cell sensitivity for DI purpose */
+#define TW5864_INDIR_MD_DI_CELLSENS 0x3ba
+/* The motion detection threshold level for DI purpose */
+#define TW5864_INDIR_MD_DI_LVSENS 0x3bb
+
+/* 192 bit motion mask of the channel specified by MASK_CH_SEL in 0x3fe */
+#define TW5864_INDIR_MOTION_MASK 0x3e0
+#define TW5864_INDIR_MOTION_MASK_BYTE_COUNT 24
+
+/* [4:0] The channel selection to access masks in 0x3e0 ~ 0x3f7 */
+#define TW5864_INDIR_MASK_CH_SEL 0x3fe
+
+/* Clock PLL / Analog IP Control */
+/* Some registers skipped */
+
+#define TW5864_INDIR_DDRA_DLL_DQS_SEL0 0xee6
+#define TW5864_INDIR_DDRA_DLL_DQS_SEL1 0xee7
+#define TW5864_INDIR_DDRA_DLL_CLK90_SEL 0xee8
+#define TW5864_INDIR_DDRA_DLL_TEST_SEL_AND_TAP_S 0xee9
+
+#define TW5864_INDIR_DDRB_DLL_DQS_SEL0 0xeeb
+#define TW5864_INDIR_DDRB_DLL_DQS_SEL1 0xeec
+#define TW5864_INDIR_DDRB_DLL_CLK90_SEL 0xeed
+#define TW5864_INDIR_DDRB_DLL_TEST_SEL_AND_TAP_S 0xeee
+
+#define TW5864_INDIR_RESET 0xef0
+#define TW5864_INDIR_RESET_VD BIT(7)
+#define TW5864_INDIR_RESET_DLL BIT(6)
+#define TW5864_INDIR_RESET_MUX_CORE BIT(5)
+
+#define TW5864_INDIR_PV_VD_CK_POL 0xefd
+#define TW5864_INDIR_PV_VD_CK_POL_PV(channel) BIT(channel)
+#define TW5864_INDIR_PV_VD_CK_POL_VD(channel) BIT(channel + 4)
+
+#define TW5864_INDIR_CLK0_SEL 0xefe
+#define TW5864_INDIR_CLK0_SEL_VD_SHIFT 0
+#define TW5864_INDIR_CLK0_SEL_VD_MASK 0x3
+#define TW5864_INDIR_CLK0_SEL_PV_SHIFT 2
+#define TW5864_INDIR_CLK0_SEL_PV_MASK (0x3 << 2)
+#define TW5864_INDIR_CLK0_SEL_PV2_SHIFT 4
+#define TW5864_INDIR_CLK0_SEL_PV2_MASK (0x3 << 4)
diff --git a/drivers/media/pci/tw5864/tw5864-util.c b/drivers/media/pci/tw5864/tw5864-util.c
new file mode 100644
index 000000000000..771eef235755
--- /dev/null
+++ b/drivers/media/pci/tw5864/tw5864-util.c
@@ -0,0 +1,37 @@
+#include "tw5864.h"
+
+void tw5864_indir_writeb(struct tw5864_dev *dev, u16 addr, u8 data)
+{
+	int retries = 30000;
+
+	while (tw_readl(TW5864_IND_CTL) & BIT(31) && --retries)
+		;
+	if (!retries)
+		dev_err(&dev->pci->dev,
+			"tw_indir_writel() retries exhausted before writing\n");
+
+	tw_writel(TW5864_IND_DATA, data);
+	tw_writel(TW5864_IND_CTL, addr << 2 | TW5864_RW | TW5864_ENABLE);
+}
+
+u8 tw5864_indir_readb(struct tw5864_dev *dev, u16 addr)
+{
+	int retries = 30000;
+
+	while (tw_readl(TW5864_IND_CTL) & BIT(31) && --retries)
+		;
+	if (!retries)
+		dev_err(&dev->pci->dev,
+			"tw_indir_readl() retries exhausted before reading\n");
+
+	tw_writel(TW5864_IND_CTL, addr << 2 | TW5864_ENABLE);
+
+	retries = 30000;
+	while (tw_readl(TW5864_IND_CTL) & BIT(31) && --retries)
+		;
+	if (!retries)
+		dev_err(&dev->pci->dev,
+			"tw_indir_readl() retries exhausted at reading\n");
+
+	return tw_readl(TW5864_IND_DATA);
+}
diff --git a/drivers/media/pci/tw5864/tw5864-video.c b/drivers/media/pci/tw5864/tw5864-video.c
new file mode 100644
index 000000000000..652a059b2e0a
--- /dev/null
+++ b/drivers/media/pci/tw5864/tw5864-video.c
@@ -0,0 +1,1510 @@
+/*
+ *  TW5864 driver - video encoding functions
+ *
+ *  Copyright (C) 2016 Bluecherry, LLC <maintainers@bluecherrydvr.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 <media/v4l2-common.h>
+#include <media/v4l2-event.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "tw5864.h"
+#include "tw5864-reg.h"
+
+#define QUANTIZATION_TABLE_LEN 96
+#define VLC_LOOKUP_TABLE_LEN 1024
+
+static const u16 forward_quantization_table[QUANTIZATION_TABLE_LEN] = {
+	0x3333, 0x1f82, 0x3333, 0x1f82, 0x1f82, 0x147b, 0x1f82, 0x147b,
+	0x3333, 0x1f82, 0x3333, 0x1f82, 0x1f82, 0x147b, 0x1f82, 0x147b,
+	0x2e8c, 0x1d42, 0x2e8c, 0x1d42, 0x1d42, 0x1234, 0x1d42, 0x1234,
+	0x2e8c, 0x1d42, 0x2e8c, 0x1d42, 0x1d42, 0x1234, 0x1d42, 0x1234,
+	0x2762, 0x199a, 0x2762, 0x199a, 0x199a, 0x1062, 0x199a, 0x1062,
+	0x2762, 0x199a, 0x2762, 0x199a, 0x199a, 0x1062, 0x199a, 0x1062,
+	0x2492, 0x16c1, 0x2492, 0x16c1, 0x16c1, 0x0e3f, 0x16c1, 0x0e3f,
+	0x2492, 0x16c1, 0x2492, 0x16c1, 0x16c1, 0x0e3f, 0x16c1, 0x0e3f,
+	0x2000, 0x147b, 0x2000, 0x147b, 0x147b, 0x0d1b, 0x147b, 0x0d1b,
+	0x2000, 0x147b, 0x2000, 0x147b, 0x147b, 0x0d1b, 0x147b, 0x0d1b,
+	0x1c72, 0x11cf, 0x1c72, 0x11cf, 0x11cf, 0x0b4d, 0x11cf, 0x0b4d,
+	0x1c72, 0x11cf, 0x1c72, 0x11cf, 0x11cf, 0x0b4d, 0x11cf, 0x0b4d
+};
+
+static const u16 inverse_quantization_table[QUANTIZATION_TABLE_LEN] = {
+	0x800a, 0x800d, 0x800a, 0x800d, 0x800d, 0x8010, 0x800d, 0x8010,
+	0x800a, 0x800d, 0x800a, 0x800d, 0x800d, 0x8010, 0x800d, 0x8010,
+	0x800b, 0x800e, 0x800b, 0x800e, 0x800e, 0x8012, 0x800e, 0x8012,
+	0x800b, 0x800e, 0x800b, 0x800e, 0x800e, 0x8012, 0x800e, 0x8012,
+	0x800d, 0x8010, 0x800d, 0x8010, 0x8010, 0x8014, 0x8010, 0x8014,
+	0x800d, 0x8010, 0x800d, 0x8010, 0x8010, 0x8014, 0x8010, 0x8014,
+	0x800e, 0x8012, 0x800e, 0x8012, 0x8012, 0x8017, 0x8012, 0x8017,
+	0x800e, 0x8012, 0x800e, 0x8012, 0x8012, 0x8017, 0x8012, 0x8017,
+	0x8010, 0x8014, 0x8010, 0x8014, 0x8014, 0x8019, 0x8014, 0x8019,
+	0x8010, 0x8014, 0x8010, 0x8014, 0x8014, 0x8019, 0x8014, 0x8019,
+	0x8012, 0x8017, 0x8012, 0x8017, 0x8017, 0x801d, 0x8017, 0x801d,
+	0x8012, 0x8017, 0x8012, 0x8017, 0x8017, 0x801d, 0x8017, 0x801d
+};
+
+static const u16 encoder_vlc_lookup_table[VLC_LOOKUP_TABLE_LEN] = {
+	0x011, 0x000, 0x000, 0x000, 0x065, 0x021, 0x000, 0x000, 0x087, 0x064,
+	0x031, 0x000, 0x097, 0x086, 0x075, 0x053, 0x0a7, 0x096, 0x085, 0x063,
+	0x0b7, 0x0a6, 0x095, 0x074, 0x0df, 0x0b6, 0x0a5, 0x084, 0x0db, 0x0de,
+	0x0b5, 0x094, 0x0d8, 0x0da, 0x0dd, 0x0a4, 0x0ef, 0x0ee, 0x0d9, 0x0b4,
+	0x0eb, 0x0ea, 0x0ed, 0x0dc, 0x0ff, 0x0fe, 0x0e9, 0x0ec, 0x0fb, 0x0fa,
+	0x0fd, 0x0e8, 0x10f, 0x0f1, 0x0f9, 0x0fc, 0x10b, 0x10e, 0x10d, 0x0f8,
+	0x107, 0x10a, 0x109, 0x10c, 0x104, 0x106, 0x105, 0x108, 0x023, 0x000,
+	0x000, 0x000, 0x06b, 0x022, 0x000, 0x000, 0x067, 0x057, 0x033, 0x000,
+	0x077, 0x06a, 0x069, 0x045, 0x087, 0x066, 0x065, 0x044, 0x084, 0x076,
+	0x075, 0x056, 0x097, 0x086, 0x085, 0x068, 0x0bf, 0x096, 0x095, 0x064,
+	0x0bb, 0x0be, 0x0bd, 0x074, 0x0cf, 0x0ba, 0x0b9, 0x094, 0x0cb, 0x0ce,
+	0x0cd, 0x0bc, 0x0c8, 0x0ca, 0x0c9, 0x0b8, 0x0df, 0x0de, 0x0dd, 0x0cc,
+	0x0db, 0x0da, 0x0d9, 0x0dc, 0x0d7, 0x0eb, 0x0d6, 0x0d8, 0x0e9, 0x0e8,
+	0x0ea, 0x0d1, 0x0e7, 0x0e6, 0x0e5, 0x0e4, 0x04f, 0x000, 0x000, 0x000,
+	0x06f, 0x04e, 0x000, 0x000, 0x06b, 0x05f, 0x04d, 0x000, 0x068, 0x05c,
+	0x05e, 0x04c, 0x07f, 0x05a, 0x05b, 0x04b, 0x07b, 0x058, 0x059, 0x04a,
+	0x079, 0x06e, 0x06d, 0x049, 0x078, 0x06a, 0x069, 0x048, 0x08f, 0x07e,
+	0x07d, 0x05d, 0x08b, 0x08e, 0x07a, 0x06c, 0x09f, 0x08a, 0x08d, 0x07c,
+	0x09b, 0x09e, 0x089, 0x08c, 0x098, 0x09a, 0x09d, 0x088, 0x0ad, 0x097,
+	0x099, 0x09c, 0x0a9, 0x0ac, 0x0ab, 0x0aa, 0x0a5, 0x0a8, 0x0a7, 0x0a6,
+	0x0a1, 0x0a4, 0x0a3, 0x0a2, 0x021, 0x000, 0x000, 0x000, 0x067, 0x011,
+	0x000, 0x000, 0x064, 0x066, 0x031, 0x000, 0x063, 0x073, 0x072, 0x065,
+	0x062, 0x083, 0x082, 0x070, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
+	0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
+	0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
+	0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
+	0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
+	0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
+	0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
+	0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
+	0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
+	0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
+	0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
+	0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
+	0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
+	0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
+	0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
+	0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
+	0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
+	0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
+	0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
+	0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
+	0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
+	0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
+	0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
+	0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
+	0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
+	0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
+	0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
+	0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
+	0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
+	0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
+	0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x011, 0x010,
+	0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
+	0x000, 0x000, 0x000, 0x000, 0x011, 0x021, 0x020, 0x000, 0x000, 0x000,
+	0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
+	0x023, 0x022, 0x021, 0x020, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
+	0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x023, 0x022, 0x021, 0x031,
+	0x030, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
+	0x000, 0x000, 0x023, 0x022, 0x033, 0x032, 0x031, 0x030, 0x000, 0x000,
+	0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x023, 0x030,
+	0x031, 0x033, 0x032, 0x035, 0x034, 0x000, 0x000, 0x000, 0x000, 0x000,
+	0x000, 0x000, 0x000, 0x000, 0x037, 0x036, 0x035, 0x034, 0x033, 0x032,
+	0x031, 0x041, 0x051, 0x061, 0x071, 0x081, 0x091, 0x0a1, 0x0b1, 0x000,
+	0x002, 0x000, 0x0e4, 0x011, 0x0f4, 0x002, 0x024, 0x003, 0x005, 0x012,
+	0x034, 0x013, 0x065, 0x024, 0x013, 0x063, 0x015, 0x022, 0x075, 0x034,
+	0x044, 0x023, 0x023, 0x073, 0x054, 0x033, 0x033, 0x004, 0x043, 0x014,
+	0x011, 0x043, 0x014, 0x001, 0x025, 0x015, 0x035, 0x025, 0x064, 0x055,
+	0x045, 0x035, 0x074, 0x065, 0x085, 0x0d5, 0x012, 0x095, 0x055, 0x045,
+	0x095, 0x0e5, 0x084, 0x075, 0x022, 0x0a5, 0x094, 0x085, 0x032, 0x0b5,
+	0x003, 0x0c5, 0x001, 0x044, 0x0a5, 0x032, 0x0b5, 0x094, 0x0c5, 0x0a4,
+	0x0a4, 0x054, 0x0d5, 0x0b4, 0x0b4, 0x064, 0x0f5, 0x0f5, 0x053, 0x0d4,
+	0x0e5, 0x0c4, 0x105, 0x105, 0x0c4, 0x074, 0x063, 0x0e4, 0x0d4, 0x084,
+	0x073, 0x0f4, 0x004, 0x005, 0x000, 0x053, 0x000, 0x000, 0x000, 0x000,
+	0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
+	0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
+	0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
+	0x000, 0x000, 0x011, 0x021, 0x031, 0x030, 0x011, 0x021, 0x020, 0x000,
+	0x011, 0x010, 0x000, 0x000, 0x011, 0x033, 0x032, 0x043, 0x042, 0x053,
+	0x052, 0x063, 0x062, 0x073, 0x072, 0x083, 0x082, 0x093, 0x092, 0x091,
+	0x037, 0x036, 0x035, 0x034, 0x033, 0x045, 0x044, 0x043, 0x042, 0x053,
+	0x052, 0x063, 0x062, 0x061, 0x060, 0x000, 0x045, 0x037, 0x036, 0x035,
+	0x044, 0x043, 0x034, 0x033, 0x042, 0x053, 0x052, 0x061, 0x051, 0x060,
+	0x000, 0x000, 0x053, 0x037, 0x045, 0x044, 0x036, 0x035, 0x034, 0x043,
+	0x033, 0x042, 0x052, 0x051, 0x050, 0x000, 0x000, 0x000, 0x045, 0x044,
+	0x043, 0x037, 0x036, 0x035, 0x034, 0x033, 0x042, 0x051, 0x041, 0x050,
+	0x000, 0x000, 0x000, 0x000, 0x061, 0x051, 0x037, 0x036, 0x035, 0x034,
+	0x033, 0x032, 0x041, 0x031, 0x060, 0x000, 0x000, 0x000, 0x000, 0x000,
+	0x061, 0x051, 0x035, 0x034, 0x033, 0x023, 0x032, 0x041, 0x031, 0x060,
+	0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x061, 0x041, 0x051, 0x033,
+	0x023, 0x022, 0x032, 0x031, 0x060, 0x000, 0x000, 0x000, 0x000, 0x000,
+	0x000, 0x000, 0x061, 0x060, 0x041, 0x023, 0x022, 0x031, 0x021, 0x051,
+	0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x051, 0x050,
+	0x031, 0x023, 0x022, 0x021, 0x041, 0x000, 0x000, 0x000, 0x000, 0x000,
+	0x000, 0x000, 0x000, 0x000, 0x040, 0x041, 0x031, 0x032, 0x011, 0x033,
+	0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
+	0x040, 0x041, 0x021, 0x011, 0x031, 0x000, 0x000, 0x000, 0x000, 0x000,
+	0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x030, 0x031, 0x011, 0x021,
+	0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
+	0x000, 0x000, 0x020, 0x021, 0x011, 0x000, 0x000, 0x000, 0x000, 0x000,
+	0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x010, 0x011,
+	0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
+	0x000, 0x000, 0x000, 0x000
+};
+
+static const unsigned int lambda_lookup_table[] = {
+	0x0020, 0x0020, 0x0020, 0x0020, 0x0020, 0x0020, 0x0020, 0x0020,
+	0x0020, 0x0020, 0x0020, 0x0020, 0x0020, 0x0020, 0x0020, 0x0020,
+	0x0040, 0x0040, 0x0040, 0x0040, 0x0060, 0x0060, 0x0060, 0x0080,
+	0x0080, 0x0080, 0x00a0, 0x00c0, 0x00c0, 0x00e0, 0x0100, 0x0120,
+	0x0140, 0x0160, 0x01a0, 0x01c0, 0x0200, 0x0240, 0x0280, 0x02e0,
+	0x0320, 0x03a0, 0x0400, 0x0480, 0x0500, 0x05a0, 0x0660, 0x0720,
+	0x0800, 0x0900, 0x0a20, 0x0b60
+};
+
+static const unsigned int intra4x4_lambda3[] = {
+	1, 1, 1, 1, 1, 1, 1, 1,
+	1, 1, 1, 1, 1, 1, 1, 1,
+	2, 2, 2, 2, 3, 3, 3, 4,
+	4, 4, 5, 6, 6, 7, 8, 9,
+	10, 11, 13, 14, 16, 18, 20, 23,
+	25, 29, 32, 36, 40, 45, 51, 57,
+	64, 72, 81, 91
+};
+
+static v4l2_std_id tw5864_get_v4l2_std(enum tw5864_vid_std std);
+static enum tw5864_vid_std tw5864_from_v4l2_std(v4l2_std_id v4l2_std);
+
+static void tw5864_handle_frame_task(unsigned long data);
+static void tw5864_handle_frame(struct tw5864_h264_frame *frame);
+static void tw5864_frame_interval_set(struct tw5864_input *input);
+
+static int tw5864_queue_setup(struct vb2_queue *q, unsigned int *num_buffers,
+			      unsigned int *num_planes, unsigned int sizes[],
+			      struct device *alloc_ctxs[])
+{
+	if (*num_planes)
+		return sizes[0] < H264_VLC_BUF_SIZE ? -EINVAL : 0;
+
+	sizes[0] = H264_VLC_BUF_SIZE;
+	*num_planes = 1;
+
+	return 0;
+}
+
+static void tw5864_buf_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct vb2_queue *vq = vb->vb2_queue;
+	struct tw5864_input *dev = vb2_get_drv_priv(vq);
+	struct tw5864_buf *buf = container_of(vbuf, struct tw5864_buf, vb);
+	unsigned long flags;
+
+	spin_lock_irqsave(&dev->slock, flags);
+	list_add_tail(&buf->list, &dev->active);
+	spin_unlock_irqrestore(&dev->slock, flags);
+}
+
+static int tw5864_input_std_get(struct tw5864_input *input,
+				enum tw5864_vid_std *std)
+{
+	struct tw5864_dev *dev = input->root;
+	u8 std_reg = tw_indir_readb(TW5864_INDIR_VIN_E(input->nr));
+
+	*std = (std_reg & 0x70) >> 4;
+
+	if (std_reg & 0x80) {
+		dev_dbg(&dev->pci->dev,
+			"Video format detection is in progress, please wait\n");
+		return -EAGAIN;
+	}
+
+	return 0;
+}
+
+static int tw5864_enable_input(struct tw5864_input *input)
+{
+	struct tw5864_dev *dev = input->root;
+	int nr = input->nr;
+	unsigned long flags;
+	int d1_width = 720;
+	int d1_height;
+	int frame_width_bus_value = 0;
+	int frame_height_bus_value = 0;
+	int reg_frame_bus = 0x1c;
+	int fmt_reg_value = 0;
+	int downscale_enabled = 0;
+
+	dev_dbg(&dev->pci->dev, "Enabling channel %d\n", nr);
+
+	input->frame_seqno = 0;
+	input->frame_gop_seqno = 0;
+	input->h264_idr_pic_id = 0;
+
+	input->reg_dsp_qp = input->qp;
+	input->reg_dsp_ref_mvp_lambda = lambda_lookup_table[input->qp];
+	input->reg_dsp_i4x4_weight = intra4x4_lambda3[input->qp];
+	input->reg_emu = TW5864_EMU_EN_LPF | TW5864_EMU_EN_BHOST
+		| TW5864_EMU_EN_SEN | TW5864_EMU_EN_ME | TW5864_EMU_EN_DDR;
+	input->reg_dsp = nr /* channel id */
+		| TW5864_DSP_CHROM_SW
+		| ((0xa << 8) & TW5864_DSP_MB_DELAY)
+		;
+
+	input->resolution = D1;
+
+	d1_height = (input->std == STD_NTSC) ? 480 : 576;
+
+	input->width = d1_width;
+	input->height = d1_height;
+
+	input->reg_interlacing = 0x4;
+
+	switch (input->resolution) {
+	case D1:
+		frame_width_bus_value = 0x2cf;
+		frame_height_bus_value = input->height - 1;
+		reg_frame_bus = 0x1c;
+		fmt_reg_value = 0;
+		downscale_enabled = 0;
+		input->reg_dsp_codec |= TW5864_CIF_MAP_MD | TW5864_HD1_MAP_MD;
+		input->reg_emu |= TW5864_DSP_FRAME_TYPE_D1;
+		input->reg_interlacing = TW5864_DI_EN | TW5864_DSP_INTER_ST;
+
+		tw_setl(TW5864_FULL_HALF_FLAG, 1 << nr);
+		break;
+	case HD1:
+		input->height /= 2;
+		input->width /= 2;
+		frame_width_bus_value = 0x2cf;
+		frame_height_bus_value = input->height * 2 - 1;
+		reg_frame_bus = 0x1c;
+		fmt_reg_value = 0;
+		downscale_enabled = 0;
+		input->reg_dsp_codec |= TW5864_HD1_MAP_MD;
+		input->reg_emu |= TW5864_DSP_FRAME_TYPE_D1;
+
+		tw_clearl(TW5864_FULL_HALF_FLAG, 1 << nr);
+
+		break;
+	case CIF:
+		input->height /= 4;
+		input->width /= 2;
+		frame_width_bus_value = 0x15f;
+		frame_height_bus_value = input->height * 2 - 1;
+		reg_frame_bus = 0x07;
+		fmt_reg_value = 1;
+		downscale_enabled = 1;
+		input->reg_dsp_codec |= TW5864_CIF_MAP_MD;
+
+		tw_clearl(TW5864_FULL_HALF_FLAG, 1 << nr);
+		break;
+	case QCIF:
+		input->height /= 4;
+		input->width /= 4;
+		frame_width_bus_value = 0x15f;
+		frame_height_bus_value = input->height * 2 - 1;
+		reg_frame_bus = 0x07;
+		fmt_reg_value = 1;
+		downscale_enabled = 1;
+		input->reg_dsp_codec |= TW5864_CIF_MAP_MD;
+
+		tw_clearl(TW5864_FULL_HALF_FLAG, 1 << nr);
+		break;
+	}
+
+	/* analog input width / 4 */
+	tw_indir_writeb(TW5864_INDIR_IN_PIC_WIDTH(nr), d1_width / 4);
+	tw_indir_writeb(TW5864_INDIR_IN_PIC_HEIGHT(nr), d1_height / 4);
+
+	/* output width / 4 */
+	tw_indir_writeb(TW5864_INDIR_OUT_PIC_WIDTH(nr), input->width / 4);
+	tw_indir_writeb(TW5864_INDIR_OUT_PIC_HEIGHT(nr), input->height / 4);
+
+	tw_writel(TW5864_DSP_PIC_MAX_MB,
+		  ((input->width / 16) << 8) | (input->height / 16));
+
+	tw_writel(TW5864_FRAME_WIDTH_BUS_A(nr),
+		  frame_width_bus_value);
+	tw_writel(TW5864_FRAME_WIDTH_BUS_B(nr),
+		  frame_width_bus_value);
+	tw_writel(TW5864_FRAME_HEIGHT_BUS_A(nr),
+		  frame_height_bus_value);
+	tw_writel(TW5864_FRAME_HEIGHT_BUS_B(nr),
+		  (frame_height_bus_value + 1) / 2 - 1);
+
+	tw5864_frame_interval_set(input);
+
+	if (downscale_enabled)
+		tw_setl(TW5864_H264EN_CH_DNS, 1 << nr);
+
+	tw_mask_shift_writel(TW5864_H264EN_CH_FMT_REG1, 0x3, 2 * nr,
+			     fmt_reg_value);
+
+	tw_mask_shift_writel((nr < 2
+			      ? TW5864_H264EN_RATE_MAX_LINE_REG1
+			      : TW5864_H264EN_RATE_MAX_LINE_REG2),
+			     0x1f, 5 * (nr % 2),
+			     input->std == STD_NTSC ? 29 : 24);
+
+	tw_mask_shift_writel((nr < 2) ? TW5864_FRAME_BUS1 :
+			     TW5864_FRAME_BUS2, 0xff, (nr % 2) * 8,
+			     reg_frame_bus);
+
+	spin_lock_irqsave(&dev->slock, flags);
+	input->enabled = 1;
+	spin_unlock_irqrestore(&dev->slock, flags);
+
+	return 0;
+}
+
+void tw5864_request_encoded_frame(struct tw5864_input *input)
+{
+	struct tw5864_dev *dev = input->root;
+	u32 enc_buf_id_new;
+
+	tw_setl(TW5864_DSP_CODEC, TW5864_CIF_MAP_MD | TW5864_HD1_MAP_MD);
+	tw_writel(TW5864_EMU, input->reg_emu);
+	tw_writel(TW5864_INTERLACING, input->reg_interlacing);
+	tw_writel(TW5864_DSP, input->reg_dsp);
+
+	tw_writel(TW5864_DSP_QP, input->reg_dsp_qp);
+	tw_writel(TW5864_DSP_REF_MVP_LAMBDA, input->reg_dsp_ref_mvp_lambda);
+	tw_writel(TW5864_DSP_I4x4_WEIGHT, input->reg_dsp_i4x4_weight);
+	tw_mask_shift_writel(TW5864_DSP_INTRA_MODE, TW5864_DSP_INTRA_MODE_MASK,
+			     TW5864_DSP_INTRA_MODE_SHIFT,
+			     TW5864_DSP_INTRA_MODE_16x16);
+
+	if (input->frame_gop_seqno == 0) {
+		/* Produce I-frame */
+		tw_writel(TW5864_MOTION_SEARCH_ETC, TW5864_INTRA_EN);
+		input->h264_idr_pic_id++;
+		input->h264_idr_pic_id &= TW5864_DSP_REF_FRM;
+	} else {
+		/* Produce P-frame */
+		tw_writel(TW5864_MOTION_SEARCH_ETC, TW5864_INTRA_EN |
+			  TW5864_ME_EN | BIT(5) /* SRCH_OPT default */);
+	}
+	tw5864_prepare_frame_headers(input);
+	tw_writel(TW5864_VLC,
+		  TW5864_VLC_PCI_SEL |
+		  ((input->tail_nb_bits + 24) << TW5864_VLC_BIT_ALIGN_SHIFT) |
+		  input->reg_dsp_qp);
+
+	enc_buf_id_new = tw_mask_shift_readl(TW5864_ENC_BUF_PTR_REC1, 0x3,
+					     2 * input->nr);
+	tw_writel(TW5864_DSP_ENC_ORG_PTR_REG,
+		  enc_buf_id_new << TW5864_DSP_ENC_ORG_PTR_SHIFT);
+	tw_writel(TW5864_DSP_ENC_REC,
+		  enc_buf_id_new << 12 | ((enc_buf_id_new + 3) & 3));
+
+	tw_writel(TW5864_SLICE, TW5864_START_NSLICE);
+	tw_writel(TW5864_SLICE, 0);
+}
+
+static int tw5864_disable_input(struct tw5864_input *input)
+{
+	struct tw5864_dev *dev = input->root;
+	unsigned long flags;
+
+	dev_dbg(&dev->pci->dev, "Disabling channel %d\n", input->nr);
+
+	spin_lock_irqsave(&dev->slock, flags);
+	input->enabled = 0;
+	spin_unlock_irqrestore(&dev->slock, flags);
+	return 0;
+}
+
+static int tw5864_start_streaming(struct vb2_queue *q, unsigned int count)
+{
+	struct tw5864_input *input = vb2_get_drv_priv(q);
+	int ret;
+
+	ret = tw5864_enable_input(input);
+	if (!ret)
+		return 0;
+
+	while (!list_empty(&input->active)) {
+		struct tw5864_buf *buf = list_entry(input->active.next,
+						    struct tw5864_buf, list);
+
+		list_del(&buf->list);
+		vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_QUEUED);
+	}
+	return ret;
+}
+
+static void tw5864_stop_streaming(struct vb2_queue *q)
+{
+	unsigned long flags;
+	struct tw5864_input *input = vb2_get_drv_priv(q);
+
+	tw5864_disable_input(input);
+
+	spin_lock_irqsave(&input->slock, flags);
+	if (input->vb) {
+		vb2_buffer_done(&input->vb->vb.vb2_buf, VB2_BUF_STATE_ERROR);
+		input->vb = NULL;
+	}
+	while (!list_empty(&input->active)) {
+		struct tw5864_buf *buf = list_entry(input->active.next,
+						    struct tw5864_buf, list);
+
+		list_del(&buf->list);
+		vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
+	}
+	spin_unlock_irqrestore(&input->slock, flags);
+}
+
+static const struct vb2_ops tw5864_video_qops = {
+	.queue_setup = tw5864_queue_setup,
+	.buf_queue = tw5864_buf_queue,
+	.start_streaming = tw5864_start_streaming,
+	.stop_streaming = tw5864_stop_streaming,
+	.wait_prepare = vb2_ops_wait_prepare,
+	.wait_finish = vb2_ops_wait_finish,
+};
+
+static int tw5864_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct tw5864_input *input =
+		container_of(ctrl->handler, struct tw5864_input, hdl);
+	struct tw5864_dev *dev = input->root;
+	unsigned long flags;
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		tw_indir_writeb(TW5864_INDIR_VIN_A_BRIGHT(input->nr),
+				(u8)ctrl->val);
+		break;
+	case V4L2_CID_HUE:
+		tw_indir_writeb(TW5864_INDIR_VIN_7_HUE(input->nr),
+				(u8)ctrl->val);
+		break;
+	case V4L2_CID_CONTRAST:
+		tw_indir_writeb(TW5864_INDIR_VIN_9_CNTRST(input->nr),
+				(u8)ctrl->val);
+		break;
+	case V4L2_CID_SATURATION:
+		tw_indir_writeb(TW5864_INDIR_VIN_B_SAT_U(input->nr),
+				(u8)ctrl->val);
+		tw_indir_writeb(TW5864_INDIR_VIN_C_SAT_V(input->nr),
+				(u8)ctrl->val);
+		break;
+	case V4L2_CID_MPEG_VIDEO_GOP_SIZE:
+		input->gop = ctrl->val;
+		return 0;
+	case V4L2_CID_MPEG_VIDEO_H264_MIN_QP:
+		spin_lock_irqsave(&input->slock, flags);
+		input->qp = ctrl->val;
+		input->reg_dsp_qp = input->qp;
+		input->reg_dsp_ref_mvp_lambda = lambda_lookup_table[input->qp];
+		input->reg_dsp_i4x4_weight = intra4x4_lambda3[input->qp];
+		spin_unlock_irqrestore(&input->slock, flags);
+		return 0;
+	case V4L2_CID_DETECT_MD_GLOBAL_THRESHOLD:
+		memset(input->md_threshold_grid_values, ctrl->val,
+		       sizeof(input->md_threshold_grid_values));
+		return 0;
+	case V4L2_CID_DETECT_MD_MODE:
+		return 0;
+	case V4L2_CID_DETECT_MD_THRESHOLD_GRID:
+		/* input->md_threshold_grid_ctrl->p_new.p_u16 contains data */
+		memcpy(input->md_threshold_grid_values,
+		       input->md_threshold_grid_ctrl->p_new.p_u16,
+		       sizeof(input->md_threshold_grid_values));
+		return 0;
+	}
+	return 0;
+}
+
+static int tw5864_fmt_vid_cap(struct file *file, void *priv,
+			      struct v4l2_format *f)
+{
+	struct tw5864_input *input = video_drvdata(file);
+
+	f->fmt.pix.width = 720;
+	switch (input->std) {
+	default:
+		WARN_ON_ONCE(1);
+	case STD_NTSC:
+		f->fmt.pix.height = 480;
+		break;
+	case STD_PAL:
+	case STD_SECAM:
+		f->fmt.pix.height = 576;
+		break;
+	}
+	f->fmt.pix.field = V4L2_FIELD_INTERLACED;
+	f->fmt.pix.pixelformat = V4L2_PIX_FMT_H264;
+	f->fmt.pix.sizeimage = H264_VLC_BUF_SIZE;
+	f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
+	return 0;
+}
+
+static int tw5864_enum_input(struct file *file, void *priv,
+			     struct v4l2_input *i)
+{
+	struct tw5864_input *input = video_drvdata(file);
+	struct tw5864_dev *dev = input->root;
+
+	u8 indir_0x000 = tw_indir_readb(TW5864_INDIR_VIN_0(input->nr));
+	u8 indir_0x00d = tw_indir_readb(TW5864_INDIR_VIN_D(input->nr));
+	u8 v1 = indir_0x000;
+	u8 v2 = indir_0x00d;
+
+	if (i->index)
+		return -EINVAL;
+
+	i->type = V4L2_INPUT_TYPE_CAMERA;
+	snprintf(i->name, sizeof(i->name), "Encoder %d", input->nr);
+	i->std = TW5864_NORMS;
+	if (v1 & (1 << 7))
+		i->status |= V4L2_IN_ST_NO_SYNC;
+	if (!(v1 & (1 << 6)))
+		i->status |= V4L2_IN_ST_NO_H_LOCK;
+	if (v1 & (1 << 2))
+		i->status |= V4L2_IN_ST_NO_SIGNAL;
+	if (v1 & (1 << 1))
+		i->status |= V4L2_IN_ST_NO_COLOR;
+	if (v2 & (1 << 2))
+		i->status |= V4L2_IN_ST_MACROVISION;
+
+	return 0;
+}
+
+static int tw5864_g_input(struct file *file, void *priv, unsigned int *i)
+{
+	*i = 0;
+	return 0;
+}
+
+static int tw5864_s_input(struct file *file, void *priv, unsigned int i)
+{
+	if (i)
+		return -EINVAL;
+	return 0;
+}
+
+static int tw5864_querycap(struct file *file, void *priv,
+			   struct v4l2_capability *cap)
+{
+	struct tw5864_input *input = video_drvdata(file);
+
+	strcpy(cap->driver, "tw5864");
+	snprintf(cap->card, sizeof(cap->card), "TW5864 Encoder %d",
+		 input->nr);
+	sprintf(cap->bus_info, "PCI:%s", pci_name(input->root->pci));
+	return 0;
+}
+
+static int tw5864_querystd(struct file *file, void *priv, v4l2_std_id *std)
+{
+	struct tw5864_input *input = video_drvdata(file);
+	enum tw5864_vid_std tw_std;
+	int ret;
+
+	ret = tw5864_input_std_get(input, &tw_std);
+	if (ret)
+		return ret;
+	*std = tw5864_get_v4l2_std(tw_std);
+
+	return 0;
+}
+
+static int tw5864_g_std(struct file *file, void *priv, v4l2_std_id *std)
+{
+	struct tw5864_input *input = video_drvdata(file);
+
+	*std = input->v4l2_std;
+	return 0;
+}
+
+static int tw5864_s_std(struct file *file, void *priv, v4l2_std_id std)
+{
+	struct tw5864_input *input = video_drvdata(file);
+	struct tw5864_dev *dev = input->root;
+
+	input->v4l2_std = std;
+	input->std = tw5864_from_v4l2_std(std);
+	tw_indir_writeb(TW5864_INDIR_VIN_E(input->nr), input->std);
+	return 0;
+}
+
+static int tw5864_enum_fmt_vid_cap(struct file *file, void *priv,
+				   struct v4l2_fmtdesc *f)
+{
+	if (f->index)
+		return -EINVAL;
+
+	f->pixelformat = V4L2_PIX_FMT_H264;
+
+	return 0;
+}
+
+static int tw5864_subscribe_event(struct v4l2_fh *fh,
+				  const struct v4l2_event_subscription *sub)
+{
+	switch (sub->type) {
+	case V4L2_EVENT_CTRL:
+		return v4l2_ctrl_subscribe_event(fh, sub);
+	case V4L2_EVENT_MOTION_DET:
+		/*
+		 * Allow for up to 30 events (1 second for NTSC) to be stored.
+		 */
+		return v4l2_event_subscribe(fh, sub, 30, NULL);
+	}
+	return -EINVAL;
+}
+
+static void tw5864_frame_interval_set(struct tw5864_input *input)
+{
+	/*
+	 * This register value seems to follow such approach: In each second
+	 * interval, when processing Nth frame, it checks Nth bit of register
+	 * value and, if the bit is 1, it processes the frame, otherwise the
+	 * frame is discarded.
+	 * So unary representation would work, but more or less equal gaps
+	 * between the frames should be preserved.
+	 *
+	 * For 1 FPS - 0x00000001
+	 * 00000000 00000000 00000000 00000001
+	 *
+	 * For max FPS - set all 25/30 lower bits:
+	 * 00111111 11111111 11111111 11111111 (NTSC)
+	 * 00000001 11111111 11111111 11111111 (PAL)
+	 *
+	 * For half of max FPS - use such pattern:
+	 * 00010101 01010101 01010101 01010101 (NTSC)
+	 * 00000001 01010101 01010101 01010101 (PAL)
+	 *
+	 * Et cetera.
+	 *
+	 * The value supplied to hardware is capped by mask of 25/30 lower bits.
+	 */
+	struct tw5864_dev *dev = input->root;
+	u32 unary_framerate = 0;
+	int shift = 0;
+	int std_max_fps = input->std == STD_NTSC ? 30 : 25;
+
+	for (shift = 0; shift < std_max_fps; shift += input->frame_interval)
+		unary_framerate |= 0x00000001 << shift;
+
+	tw_writel(TW5864_H264EN_RATE_CNTL_LO_WORD(input->nr, 0),
+		  unary_framerate >> 16);
+	tw_writel(TW5864_H264EN_RATE_CNTL_HI_WORD(input->nr, 0),
+		  unary_framerate & 0xffff);
+}
+
+static int tw5864_frameinterval_get(struct tw5864_input *input,
+				    struct v4l2_fract *frameinterval)
+{
+	switch (input->std) {
+	case STD_NTSC:
+		frameinterval->numerator = 1001;
+		frameinterval->denominator = 30000;
+		break;
+	case STD_PAL:
+	case STD_SECAM:
+		frameinterval->numerator = 1;
+		frameinterval->denominator = 25;
+		break;
+	default:
+		WARN(1, "tw5864_frameinterval_get requested for unknown std %d\n",
+		     input->std);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int tw5864_enum_framesizes(struct file *file, void *priv,
+				  struct v4l2_frmsizeenum *fsize)
+{
+	struct tw5864_input *input = video_drvdata(file);
+
+	if (fsize->index > 0)
+		return -EINVAL;
+	if (fsize->pixel_format != V4L2_PIX_FMT_H264)
+		return -EINVAL;
+
+	fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
+	fsize->discrete.width = 720;
+	fsize->discrete.height = input->std == STD_NTSC ? 480 : 576;
+
+	return 0;
+}
+
+static int tw5864_enum_frameintervals(struct file *file, void *priv,
+				      struct v4l2_frmivalenum *fintv)
+{
+	struct tw5864_input *input = video_drvdata(file);
+	struct v4l2_fract frameinterval;
+	int std_max_fps = input->std == STD_NTSC ? 30 : 25;
+	struct v4l2_frmsizeenum fsize = { .index = fintv->index,
+		.pixel_format = fintv->pixel_format };
+	int ret;
+
+	ret = tw5864_enum_framesizes(file, priv, &fsize);
+	if (ret)
+		return ret;
+
+	if (fintv->width != fsize.discrete.width ||
+	    fintv->height != fsize.discrete.height)
+		return -EINVAL;
+
+	fintv->type = V4L2_FRMIVAL_TYPE_STEPWISE;
+
+	ret = tw5864_frameinterval_get(input, &frameinterval);
+	fintv->stepwise.step = frameinterval;
+	fintv->stepwise.min = frameinterval;
+	fintv->stepwise.max = frameinterval;
+	fintv->stepwise.max.numerator *= std_max_fps;
+
+	return ret;
+}
+
+static int tw5864_g_parm(struct file *file, void *priv,
+			 struct v4l2_streamparm *sp)
+{
+	struct tw5864_input *input = video_drvdata(file);
+	struct v4l2_captureparm *cp = &sp->parm.capture;
+	int ret;
+
+	cp->capability = V4L2_CAP_TIMEPERFRAME;
+
+	ret = tw5864_frameinterval_get(input, &cp->timeperframe);
+	cp->timeperframe.numerator *= input->frame_interval;
+	cp->capturemode = 0;
+	cp->readbuffers = 2;
+
+	return ret;
+}
+
+static int tw5864_s_parm(struct file *file, void *priv,
+			 struct v4l2_streamparm *sp)
+{
+	struct tw5864_input *input = video_drvdata(file);
+	struct v4l2_fract *t = &sp->parm.capture.timeperframe;
+	struct v4l2_fract time_base;
+	int ret;
+
+	ret = tw5864_frameinterval_get(input, &time_base);
+	if (ret)
+		return ret;
+
+	if (!t->numerator || !t->denominator) {
+		t->numerator = time_base.numerator * input->frame_interval;
+		t->denominator = time_base.denominator;
+	} else if (t->denominator != time_base.denominator) {
+		t->numerator = t->numerator * time_base.denominator /
+			t->denominator;
+		t->denominator = time_base.denominator;
+	}
+
+	input->frame_interval = t->numerator / time_base.numerator;
+	if (input->frame_interval < 1)
+		input->frame_interval = 1;
+	tw5864_frame_interval_set(input);
+	return tw5864_g_parm(file, priv, sp);
+}
+
+static const struct v4l2_ctrl_ops tw5864_ctrl_ops = {
+	.s_ctrl = tw5864_s_ctrl,
+};
+
+static const struct v4l2_file_operations video_fops = {
+	.owner = THIS_MODULE,
+	.open = v4l2_fh_open,
+	.release = vb2_fop_release,
+	.read = vb2_fop_read,
+	.poll = vb2_fop_poll,
+	.mmap = vb2_fop_mmap,
+	.unlocked_ioctl = video_ioctl2,
+};
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+
+#define INDIR_SPACE_MAP_SHIFT 0x100000
+
+static int tw5864_g_reg(struct file *file, void *fh,
+			struct v4l2_dbg_register *reg)
+{
+	struct tw5864_input *input = video_drvdata(file);
+	struct tw5864_dev *dev = input->root;
+
+	if (reg->reg < INDIR_SPACE_MAP_SHIFT) {
+		if (reg->reg > 0x87fff)
+			return -EINVAL;
+		reg->size = 4;
+		reg->val = tw_readl(reg->reg);
+	} else {
+		__u64 indir_addr = reg->reg - INDIR_SPACE_MAP_SHIFT;
+
+		if (indir_addr > 0xefe)
+			return -EINVAL;
+		reg->size = 1;
+		reg->val = tw_indir_readb(reg->reg);
+	}
+	return 0;
+}
+
+static int tw5864_s_reg(struct file *file, void *fh,
+			const struct v4l2_dbg_register *reg)
+{
+	struct tw5864_input *input = video_drvdata(file);
+	struct tw5864_dev *dev = input->root;
+
+	if (reg->reg < INDIR_SPACE_MAP_SHIFT) {
+		if (reg->reg > 0x87fff)
+			return -EINVAL;
+		tw_writel(reg->reg, reg->val);
+	} else {
+		__u64 indir_addr = reg->reg - INDIR_SPACE_MAP_SHIFT;
+
+		if (indir_addr > 0xefe)
+			return -EINVAL;
+		tw_indir_writeb(reg->reg, reg->val);
+	}
+	return 0;
+}
+#endif
+
+static const struct v4l2_ioctl_ops video_ioctl_ops = {
+	.vidioc_querycap = tw5864_querycap,
+	.vidioc_enum_fmt_vid_cap = tw5864_enum_fmt_vid_cap,
+	.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_querystd = tw5864_querystd,
+	.vidioc_s_std = tw5864_s_std,
+	.vidioc_g_std = tw5864_g_std,
+	.vidioc_enum_input = tw5864_enum_input,
+	.vidioc_g_input = tw5864_g_input,
+	.vidioc_s_input = tw5864_s_input,
+	.vidioc_streamon = vb2_ioctl_streamon,
+	.vidioc_streamoff = vb2_ioctl_streamoff,
+	.vidioc_try_fmt_vid_cap = tw5864_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap = tw5864_fmt_vid_cap,
+	.vidioc_g_fmt_vid_cap = tw5864_fmt_vid_cap,
+	.vidioc_log_status = v4l2_ctrl_log_status,
+	.vidioc_subscribe_event = tw5864_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
+	.vidioc_enum_framesizes = tw5864_enum_framesizes,
+	.vidioc_enum_frameintervals = tw5864_enum_frameintervals,
+	.vidioc_s_parm = tw5864_s_parm,
+	.vidioc_g_parm = tw5864_g_parm,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.vidioc_g_register = tw5864_g_reg,
+	.vidioc_s_register = tw5864_s_reg,
+#endif
+};
+
+static const struct video_device tw5864_video_template = {
+	.name = "tw5864_video",
+	.fops = &video_fops,
+	.ioctl_ops = &video_ioctl_ops,
+	.release = video_device_release_empty,
+	.tvnorms = TW5864_NORMS,
+	.device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE |
+		V4L2_CAP_STREAMING,
+};
+
+/* Motion Detection Threshold matrix */
+static const struct v4l2_ctrl_config tw5864_md_thresholds = {
+	.ops = &tw5864_ctrl_ops,
+	.id = V4L2_CID_DETECT_MD_THRESHOLD_GRID,
+	.dims = {MD_CELLS_HOR, MD_CELLS_VERT},
+	.def = 14,
+	/* See tw5864_md_metric_from_mvd() */
+	.max = 2 * 0x0f,
+	.step = 1,
+};
+
+static int tw5864_video_input_init(struct tw5864_input *dev, int video_nr);
+static void tw5864_video_input_fini(struct tw5864_input *dev);
+static void tw5864_encoder_tables_upload(struct tw5864_dev *dev);
+
+int tw5864_video_init(struct tw5864_dev *dev, int *video_nr)
+{
+	int i;
+	int ret;
+	unsigned long flags;
+	int last_dma_allocated = -1;
+	int last_input_nr_registered = -1;
+
+	for (i = 0; i < H264_BUF_CNT; i++) {
+		struct tw5864_h264_frame *frame = &dev->h264_buf[i];
+
+		frame->vlc.addr = dma_alloc_coherent(&dev->pci->dev,
+						     H264_VLC_BUF_SIZE,
+						     &frame->vlc.dma_addr,
+						     GFP_KERNEL | GFP_DMA32);
+		if (!frame->vlc.addr) {
+			dev_err(&dev->pci->dev, "dma alloc fail\n");
+			ret = -ENOMEM;
+			goto free_dma;
+		}
+		frame->mv.addr = dma_alloc_coherent(&dev->pci->dev,
+						    H264_MV_BUF_SIZE,
+						    &frame->mv.dma_addr,
+						    GFP_KERNEL | GFP_DMA32);
+		if (!frame->mv.addr) {
+			dev_err(&dev->pci->dev, "dma alloc fail\n");
+			ret = -ENOMEM;
+			dma_free_coherent(&dev->pci->dev, H264_VLC_BUF_SIZE,
+					  frame->vlc.addr, frame->vlc.dma_addr);
+			goto free_dma;
+		}
+		last_dma_allocated = i;
+	}
+
+	tw5864_encoder_tables_upload(dev);
+
+	/* Picture is distorted without this block */
+	/* use falling edge to sample 54M to 108M */
+	tw_indir_writeb(TW5864_INDIR_VD_108_POL, TW5864_INDIR_VD_108_POL_BOTH);
+	tw_indir_writeb(TW5864_INDIR_CLK0_SEL, 0x00);
+
+	tw_indir_writeb(TW5864_INDIR_DDRA_DLL_DQS_SEL0, 0x02);
+	tw_indir_writeb(TW5864_INDIR_DDRA_DLL_DQS_SEL1, 0x02);
+	tw_indir_writeb(TW5864_INDIR_DDRA_DLL_CLK90_SEL, 0x02);
+	tw_indir_writeb(TW5864_INDIR_DDRB_DLL_DQS_SEL0, 0x02);
+	tw_indir_writeb(TW5864_INDIR_DDRB_DLL_DQS_SEL1, 0x02);
+	tw_indir_writeb(TW5864_INDIR_DDRB_DLL_CLK90_SEL, 0x02);
+
+	/* video input reset */
+	tw_indir_writeb(TW5864_INDIR_RESET, 0);
+	tw_indir_writeb(TW5864_INDIR_RESET, TW5864_INDIR_RESET_VD |
+			TW5864_INDIR_RESET_DLL | TW5864_INDIR_RESET_MUX_CORE);
+	msleep(20);
+
+	/*
+	 * Select Part A mode for all channels.
+	 * tw_setl instead of tw_clearl for Part B mode.
+	 *
+	 * I guess "Part B" is primarily for downscaled version of same channel
+	 * which goes in Part A of same bus
+	 */
+	tw_writel(TW5864_FULL_HALF_MODE_SEL, 0);
+
+	tw_indir_writeb(TW5864_INDIR_PV_VD_CK_POL,
+			TW5864_INDIR_PV_VD_CK_POL_VD(0) |
+			TW5864_INDIR_PV_VD_CK_POL_VD(1) |
+			TW5864_INDIR_PV_VD_CK_POL_VD(2) |
+			TW5864_INDIR_PV_VD_CK_POL_VD(3));
+
+	spin_lock_irqsave(&dev->slock, flags);
+	dev->encoder_busy = 0;
+	dev->h264_buf_r_index = 0;
+	dev->h264_buf_w_index = 0;
+	tw_writel(TW5864_VLC_STREAM_BASE_ADDR,
+		  dev->h264_buf[dev->h264_buf_w_index].vlc.dma_addr);
+	tw_writel(TW5864_MV_STREAM_BASE_ADDR,
+		  dev->h264_buf[dev->h264_buf_w_index].mv.dma_addr);
+	spin_unlock_irqrestore(&dev->slock, flags);
+
+	tw_writel(TW5864_SEN_EN_CH, 0x000f);
+	tw_writel(TW5864_H264EN_CH_EN, 0x000f);
+
+	tw_writel(TW5864_H264EN_BUS0_MAP, 0x00000000);
+	tw_writel(TW5864_H264EN_BUS1_MAP, 0x00001111);
+	tw_writel(TW5864_H264EN_BUS2_MAP, 0x00002222);
+	tw_writel(TW5864_H264EN_BUS3_MAP, 0x00003333);
+
+	/*
+	 * Quote from Intersil (manufacturer):
+	 * 0x0038 is managed by HW, and by default it won't pass the pointer set
+	 * at 0x0010. So if you don't do encoding, 0x0038 should stay at '3'
+	 * (with 4 frames in buffer). If you encode one frame and then move
+	 * 0x0010 to '1' for example, HW will take one more frame and set it to
+	 * buffer #0, and then you should see 0x0038 is set to '0'.  There is
+	 * only one HW encoder engine, so 4 channels cannot get encoded
+	 * simultaneously. But each channel does have its own buffer (for
+	 * original frames and reconstructed frames). So there is no problem to
+	 * manage encoding for 4 channels at same time and no need to force
+	 * I-frames in switching channels.
+	 * End of quote.
+	 *
+	 * If we set 0x0010 (TW5864_ENC_BUF_PTR_REC1) to 0 (for any channel), we
+	 * have no "rolling" (until we change this value).
+	 * If we set 0x0010 (TW5864_ENC_BUF_PTR_REC1) to 0x3, it starts to roll
+	 * continuously together with 0x0038.
+	 */
+	tw_writel(TW5864_ENC_BUF_PTR_REC1, 0x00ff);
+	tw_writel(TW5864_PCI_INTTM_SCALE, 0);
+
+	tw_writel(TW5864_INTERLACING, TW5864_DI_EN);
+	tw_writel(TW5864_MASTER_ENB_REG, TW5864_PCI_VLC_INTR_ENB);
+	tw_writel(TW5864_PCI_INTR_CTL,
+		  TW5864_TIMER_INTR_ENB | TW5864_PCI_MAST_ENB |
+		  TW5864_MVD_VLC_MAST_ENB);
+
+	dev->irqmask |= TW5864_INTR_VLC_DONE | TW5864_INTR_TIMER;
+	tw5864_irqmask_apply(dev);
+
+	tasklet_init(&dev->tasklet, tw5864_handle_frame_task,
+		     (unsigned long)dev);
+
+	for (i = 0; i < TW5864_INPUTS; i++) {
+		dev->inputs[i].root = dev;
+		dev->inputs[i].nr = i;
+		ret = tw5864_video_input_init(&dev->inputs[i], video_nr[i]);
+		if (ret)
+			goto fini_video_inputs;
+		last_input_nr_registered = i;
+	}
+
+	return 0;
+
+fini_video_inputs:
+	for (i = last_input_nr_registered; i >= 0; i--)
+		tw5864_video_input_fini(&dev->inputs[i]);
+
+	tasklet_kill(&dev->tasklet);
+
+free_dma:
+	for (i = last_dma_allocated; i >= 0; i--) {
+		dma_free_coherent(&dev->pci->dev, H264_VLC_BUF_SIZE,
+				  dev->h264_buf[i].vlc.addr,
+				  dev->h264_buf[i].vlc.dma_addr);
+		dma_free_coherent(&dev->pci->dev, H264_MV_BUF_SIZE,
+				  dev->h264_buf[i].mv.addr,
+				  dev->h264_buf[i].mv.dma_addr);
+	}
+
+	return ret;
+}
+
+static int tw5864_video_input_init(struct tw5864_input *input, int video_nr)
+{
+	struct tw5864_dev *dev = input->root;
+	int ret;
+	struct v4l2_ctrl_handler *hdl = &input->hdl;
+
+	mutex_init(&input->lock);
+	spin_lock_init(&input->slock);
+
+	/* setup video buffers queue */
+	INIT_LIST_HEAD(&input->active);
+	input->vidq.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	input->vidq.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	input->vidq.io_modes = VB2_MMAP | VB2_READ | VB2_DMABUF;
+	input->vidq.ops = &tw5864_video_qops;
+	input->vidq.mem_ops = &vb2_dma_contig_memops;
+	input->vidq.drv_priv = input;
+	input->vidq.gfp_flags = 0;
+	input->vidq.buf_struct_size = sizeof(struct tw5864_buf);
+	input->vidq.lock = &input->lock;
+	input->vidq.min_buffers_needed = 2;
+	input->vidq.dev = &input->root->pci->dev;
+	ret = vb2_queue_init(&input->vidq);
+	if (ret)
+		goto free_mutex;
+
+	input->vdev = tw5864_video_template;
+	input->vdev.v4l2_dev = &input->root->v4l2_dev;
+	input->vdev.lock = &input->lock;
+	input->vdev.queue = &input->vidq;
+	video_set_drvdata(&input->vdev, input);
+
+	/* Initialize the device control structures */
+	v4l2_ctrl_handler_init(hdl, 6);
+	v4l2_ctrl_new_std(hdl, &tw5864_ctrl_ops,
+			  V4L2_CID_BRIGHTNESS, -128, 127, 1, 0);
+	v4l2_ctrl_new_std(hdl, &tw5864_ctrl_ops,
+			  V4L2_CID_CONTRAST, 0, 255, 1, 100);
+	v4l2_ctrl_new_std(hdl, &tw5864_ctrl_ops,
+			  V4L2_CID_SATURATION, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(hdl, &tw5864_ctrl_ops, V4L2_CID_HUE, -128, 127, 1, 0);
+	v4l2_ctrl_new_std(hdl, &tw5864_ctrl_ops, V4L2_CID_MPEG_VIDEO_GOP_SIZE,
+			  1, MAX_GOP_SIZE, 1, GOP_SIZE);
+	v4l2_ctrl_new_std(hdl, &tw5864_ctrl_ops,
+			  V4L2_CID_MPEG_VIDEO_H264_MIN_QP, 28, 51, 1, QP_VALUE);
+	v4l2_ctrl_new_std_menu(hdl, &tw5864_ctrl_ops,
+			       V4L2_CID_DETECT_MD_MODE,
+			       V4L2_DETECT_MD_MODE_THRESHOLD_GRID, 0,
+			       V4L2_DETECT_MD_MODE_DISABLED);
+	v4l2_ctrl_new_std(hdl, &tw5864_ctrl_ops,
+			  V4L2_CID_DETECT_MD_GLOBAL_THRESHOLD,
+			  tw5864_md_thresholds.min, tw5864_md_thresholds.max,
+			  tw5864_md_thresholds.step, tw5864_md_thresholds.def);
+	input->md_threshold_grid_ctrl =
+		v4l2_ctrl_new_custom(hdl, &tw5864_md_thresholds, NULL);
+	if (hdl->error) {
+		ret = hdl->error;
+		goto free_v4l2_hdl;
+	}
+	input->vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_setup(hdl);
+
+	input->qp = QP_VALUE;
+	input->gop = GOP_SIZE;
+	input->frame_interval = 1;
+
+	ret = video_register_device(&input->vdev, VFL_TYPE_GRABBER, video_nr);
+	if (ret)
+		goto free_v4l2_hdl;
+
+	dev_info(&input->root->pci->dev, "Registered video device %s\n",
+		 video_device_node_name(&input->vdev));
+
+	/*
+	 * Set default video standard. Doesn't matter which, the detected value
+	 * will be found out by VIDIOC_QUERYSTD handler.
+	 */
+	input->v4l2_std = V4L2_STD_NTSC_M;
+	input->std = STD_NTSC;
+
+	tw_indir_writeb(TW5864_INDIR_VIN_E(video_nr), 0x07);
+	/* to initiate auto format recognition */
+	tw_indir_writeb(TW5864_INDIR_VIN_F(video_nr), 0xff);
+
+	return 0;
+
+free_v4l2_hdl:
+	v4l2_ctrl_handler_free(hdl);
+	vb2_queue_release(&input->vidq);
+free_mutex:
+	mutex_destroy(&input->lock);
+
+	return ret;
+}
+
+static void tw5864_video_input_fini(struct tw5864_input *dev)
+{
+	video_unregister_device(&dev->vdev);
+	v4l2_ctrl_handler_free(&dev->hdl);
+	vb2_queue_release(&dev->vidq);
+}
+
+void tw5864_video_fini(struct tw5864_dev *dev)
+{
+	int i;
+
+	tasklet_kill(&dev->tasklet);
+
+	for (i = 0; i < TW5864_INPUTS; i++)
+		tw5864_video_input_fini(&dev->inputs[i]);
+
+	for (i = 0; i < H264_BUF_CNT; i++) {
+		dma_free_coherent(&dev->pci->dev, H264_VLC_BUF_SIZE,
+				  dev->h264_buf[i].vlc.addr,
+				  dev->h264_buf[i].vlc.dma_addr);
+		dma_free_coherent(&dev->pci->dev, H264_MV_BUF_SIZE,
+				  dev->h264_buf[i].mv.addr,
+				  dev->h264_buf[i].mv.dma_addr);
+	}
+}
+
+void tw5864_prepare_frame_headers(struct tw5864_input *input)
+{
+	struct tw5864_buf *vb = input->vb;
+	u8 *dst;
+	size_t dst_space;
+	unsigned long flags;
+
+	if (!vb) {
+		spin_lock_irqsave(&input->slock, flags);
+		if (list_empty(&input->active)) {
+			spin_unlock_irqrestore(&input->slock, flags);
+			input->vb = NULL;
+			return;
+		}
+		vb = list_first_entry(&input->active, struct tw5864_buf, list);
+		list_del(&vb->list);
+		spin_unlock_irqrestore(&input->slock, flags);
+	}
+
+	dst = vb2_plane_vaddr(&vb->vb.vb2_buf, 0);
+	dst_space = vb2_plane_size(&vb->vb.vb2_buf, 0);
+
+	/*
+	 * Low-level bitstream writing functions don't have a fine way to say
+	 * correctly that supplied buffer is too small. So we just check there
+	 * and warn, and don't care at lower level.
+	 * Currently all headers take below 32 bytes.
+	 * The buffer is supposed to have plenty of free space at this point,
+	 * anyway.
+	 */
+	if (WARN_ON_ONCE(dst_space < 128))
+		return;
+
+	/*
+	 * Generate H264 headers:
+	 * If this is first frame, put SPS and PPS
+	 */
+	if (input->frame_gop_seqno == 0)
+		tw5864_h264_put_stream_header(&dst, &dst_space, input->qp,
+					      input->width, input->height);
+
+	/* Put slice header */
+	tw5864_h264_put_slice_header(&dst, &dst_space, input->h264_idr_pic_id,
+				     input->frame_gop_seqno,
+				     &input->tail_nb_bits, &input->tail);
+	input->vb = vb;
+	input->buf_cur_ptr = dst;
+	input->buf_cur_space_left = dst_space;
+}
+
+/*
+ * Returns heuristic motion detection metric value from known components of
+ * hardware-provided Motion Vector Data.
+ */
+static unsigned int tw5864_md_metric_from_mvd(u32 mvd)
+{
+	/*
+	 * Format of motion vector data exposed by tw5864, according to
+	 * manufacturer:
+	 * mv_x 10 bits
+	 * mv_y 10 bits
+	 * non_zero_members 8 bits
+	 * mb_type 3 bits
+	 * reserved 1 bit
+	 *
+	 * non_zero_members: number of non-zero residuals in each macro block
+	 * after quantization
+	 *
+	 * unsigned int reserved = mvd >> 31;
+	 * unsigned int mb_type = (mvd >> 28) & 0x7;
+	 * unsigned int non_zero_members = (mvd >> 20) & 0xff;
+	 */
+	unsigned int mv_y = (mvd >> 10) & 0x3ff;
+	unsigned int mv_x = mvd & 0x3ff;
+
+	/* heuristic: */
+	mv_x &= 0x0f;
+	mv_y &= 0x0f;
+
+	return mv_y + mv_x;
+}
+
+static int tw5864_is_motion_triggered(struct tw5864_h264_frame *frame)
+{
+	struct tw5864_input *input = frame->input;
+	u32 *mv = (u32 *)frame->mv.addr;
+	int i;
+	int detected = 0;
+
+	for (i = 0; i < MD_CELLS; i++) {
+		const u16 thresh = input->md_threshold_grid_values[i];
+		const unsigned int metric = tw5864_md_metric_from_mvd(mv[i]);
+
+		if (metric > thresh)
+			detected = 1;
+
+		if (detected)
+			break;
+	}
+	return detected;
+}
+
+static void tw5864_handle_frame_task(unsigned long data)
+{
+	struct tw5864_dev *dev = (struct tw5864_dev *)data;
+	unsigned long flags;
+	int batch_size = H264_BUF_CNT;
+
+	spin_lock_irqsave(&dev->slock, flags);
+	while (dev->h264_buf_r_index != dev->h264_buf_w_index && batch_size--) {
+		struct tw5864_h264_frame *frame =
+			&dev->h264_buf[dev->h264_buf_r_index];
+
+		spin_unlock_irqrestore(&dev->slock, flags);
+		dma_sync_single_for_cpu(&dev->pci->dev, frame->vlc.dma_addr,
+					H264_VLC_BUF_SIZE, DMA_FROM_DEVICE);
+		dma_sync_single_for_cpu(&dev->pci->dev, frame->mv.dma_addr,
+					H264_MV_BUF_SIZE, DMA_FROM_DEVICE);
+		tw5864_handle_frame(frame);
+		dma_sync_single_for_device(&dev->pci->dev, frame->vlc.dma_addr,
+					   H264_VLC_BUF_SIZE, DMA_FROM_DEVICE);
+		dma_sync_single_for_device(&dev->pci->dev, frame->mv.dma_addr,
+					   H264_MV_BUF_SIZE, DMA_FROM_DEVICE);
+		spin_lock_irqsave(&dev->slock, flags);
+
+		dev->h264_buf_r_index++;
+		dev->h264_buf_r_index %= H264_BUF_CNT;
+	}
+	spin_unlock_irqrestore(&dev->slock, flags);
+}
+
+#ifdef DEBUG
+static u32 tw5864_vlc_checksum(u32 *data, int len)
+{
+	u32 val, count_len = len;
+
+	val = *data++;
+	while (((count_len >> 2) - 1) > 0) {
+		val ^= *data++;
+		count_len -= 4;
+	}
+	val ^= htonl((len >> 2));
+	return val;
+}
+#endif
+
+static void tw5864_handle_frame(struct tw5864_h264_frame *frame)
+{
+#define SKIP_VLCBUF_BYTES 3
+	struct tw5864_input *input = frame->input;
+	struct tw5864_dev *dev = input->root;
+	struct tw5864_buf *vb;
+	struct vb2_v4l2_buffer *v4l2_buf;
+	int frame_len = frame->vlc_len - SKIP_VLCBUF_BYTES;
+	u8 *dst = input->buf_cur_ptr;
+	u8 tail_mask, vlc_mask = 0;
+	int i;
+	u8 vlc_first_byte = ((u8 *)(frame->vlc.addr + SKIP_VLCBUF_BYTES))[0];
+	unsigned long flags;
+	int zero_run;
+	u8 *src;
+	u8 *src_end;
+
+#ifdef DEBUG
+	if (frame->checksum !=
+	    tw5864_vlc_checksum((u32 *)frame->vlc.addr, frame_len))
+		dev_err(&dev->pci->dev,
+			"Checksum of encoded frame doesn't match!\n");
+#endif
+
+	spin_lock_irqsave(&input->slock, flags);
+	vb = input->vb;
+	input->vb = NULL;
+	spin_unlock_irqrestore(&input->slock, flags);
+
+	v4l2_buf = to_vb2_v4l2_buffer(&vb->vb.vb2_buf);
+
+	if (!vb) { /* Gone because of disabling */
+		dev_dbg(&dev->pci->dev, "vb is empty, dropping frame\n");
+		return;
+	}
+
+	/*
+	 * Check for space.
+	 * Mind the overhead of startcode emulation prevention.
+	 */
+	if (input->buf_cur_space_left < frame_len * 5 / 4) {
+		dev_err_once(&dev->pci->dev,
+			     "Left space in vb2 buffer, %d bytes, is less than considered safely enough to put frame of length %d. Dropping this frame.\n",
+			     input->buf_cur_space_left, frame_len);
+		return;
+	}
+
+	for (i = 0; i < 8 - input->tail_nb_bits; i++)
+		vlc_mask |= 1 << i;
+	tail_mask = (~vlc_mask) & 0xff;
+
+	dst[0] = (input->tail & tail_mask) | (vlc_first_byte & vlc_mask);
+	frame_len--;
+	dst++;
+
+	/* H.264 startcode emulation prevention */
+	src = frame->vlc.addr + SKIP_VLCBUF_BYTES + 1;
+	src_end = src + frame_len;
+	zero_run = 0;
+	for (; src < src_end; src++) {
+		if (zero_run < 2) {
+			if (*src == 0)
+				++zero_run;
+			else
+				zero_run = 0;
+		} else {
+			if ((*src & ~0x03) == 0)
+				*dst++ = 0x03;
+			zero_run = *src == 0;
+		}
+		*dst++ = *src;
+	}
+
+	vb2_set_plane_payload(&vb->vb.vb2_buf, 0,
+			      dst - (u8 *)vb2_plane_vaddr(&vb->vb.vb2_buf, 0));
+
+	vb->vb.vb2_buf.timestamp = frame->timestamp;
+	v4l2_buf->field = V4L2_FIELD_INTERLACED;
+	v4l2_buf->sequence = frame->seqno;
+
+	/* Check for motion flags */
+	if (frame->gop_seqno /* P-frame */ &&
+	    tw5864_is_motion_triggered(frame)) {
+		struct v4l2_event ev = {
+			.type = V4L2_EVENT_MOTION_DET,
+			.u.motion_det = {
+				.flags = V4L2_EVENT_MD_FL_HAVE_FRAME_SEQ,
+				.frame_sequence = v4l2_buf->sequence,
+			},
+		};
+
+		v4l2_event_queue(&input->vdev, &ev);
+	}
+
+	vb2_buffer_done(&vb->vb.vb2_buf, VB2_BUF_STATE_DONE);
+}
+
+static v4l2_std_id tw5864_get_v4l2_std(enum tw5864_vid_std std)
+{
+	switch (std) {
+	case STD_NTSC:    return V4L2_STD_NTSC_M;
+	case STD_PAL:     return V4L2_STD_PAL_B;
+	case STD_SECAM:   return V4L2_STD_SECAM_B;
+	case STD_NTSC443: return V4L2_STD_NTSC_443;
+	case STD_PAL_M:   return V4L2_STD_PAL_M;
+	case STD_PAL_CN:  return V4L2_STD_PAL_Nc;
+	case STD_PAL_60:  return V4L2_STD_PAL_60;
+	case STD_INVALID: return V4L2_STD_UNKNOWN;
+	}
+	return 0;
+}
+
+static enum tw5864_vid_std tw5864_from_v4l2_std(v4l2_std_id v4l2_std)
+{
+	if (v4l2_std & V4L2_STD_NTSC_M)
+		return STD_NTSC;
+	if (v4l2_std & V4L2_STD_PAL_B)
+		return STD_PAL;
+	if (v4l2_std & V4L2_STD_SECAM_B)
+		return STD_SECAM;
+	if (v4l2_std & V4L2_STD_NTSC_443)
+		return STD_NTSC443;
+	if (v4l2_std & V4L2_STD_PAL_M)
+		return STD_PAL_M;
+	if (v4l2_std & V4L2_STD_PAL_Nc)
+		return STD_PAL_CN;
+	if (v4l2_std & V4L2_STD_PAL_60)
+		return STD_PAL_60;
+
+	return STD_INVALID;
+}
+
+static void tw5864_encoder_tables_upload(struct tw5864_dev *dev)
+{
+	int i;
+
+	tw_writel(TW5864_VLC_RD, 0x1);
+	for (i = 0; i < VLC_LOOKUP_TABLE_LEN; i++) {
+		tw_writel((TW5864_VLC_STREAM_MEM_START + i * 4),
+			  encoder_vlc_lookup_table[i]);
+	}
+	tw_writel(TW5864_VLC_RD, 0x0);
+
+	for (i = 0; i < QUANTIZATION_TABLE_LEN; i++) {
+		tw_writel((TW5864_QUAN_TAB + i * 4),
+			  forward_quantization_table[i]);
+	}
+
+	for (i = 0; i < QUANTIZATION_TABLE_LEN; i++) {
+		tw_writel((TW5864_QUAN_TAB + i * 4),
+			  inverse_quantization_table[i]);
+	}
+}
diff --git a/drivers/media/pci/tw5864/tw5864.h b/drivers/media/pci/tw5864/tw5864.h
new file mode 100644
index 000000000000..f5de9f6ef119
--- /dev/null
+++ b/drivers/media/pci/tw5864/tw5864.h
@@ -0,0 +1,205 @@
+/*
+ *  TW5864 driver  - common header file
+ *
+ *  Copyright (C) 2016 Bluecherry, LLC <maintainers@bluecherrydvr.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/pci.h>
+#include <linux/videodev2.h>
+#include <linux/notifier.h>
+#include <linux/delay.h>
+#include <linux/mutex.h>
+#include <linux/io.h>
+#include <linux/interrupt.h>
+
+#include <media/v4l2-common.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/videobuf2-dma-sg.h>
+
+#include "tw5864-reg.h"
+
+#define PCI_DEVICE_ID_TECHWELL_5864 0x5864
+
+#define TW5864_NORMS V4L2_STD_ALL
+
+/* ----------------------------------------------------------- */
+/* card configuration   */
+
+#define TW5864_INPUTS 4
+
+/* The TW5864 uses 192 (16x12) detection cells in full screen for motion
+ * detection. Each detection cell is composed of 44 pixels and 20 lines for
+ * NTSC and 24 lines for PAL.
+ */
+#define MD_CELLS_HOR 16
+#define MD_CELLS_VERT 12
+#define MD_CELLS (MD_CELLS_HOR * MD_CELLS_VERT)
+
+#define H264_VLC_BUF_SIZE 0x80000
+#define H264_MV_BUF_SIZE 0x2000 /* device writes 5396 bytes */
+#define QP_VALUE 28
+#define MAX_GOP_SIZE 255
+#define GOP_SIZE MAX_GOP_SIZE
+
+enum resolution {
+	D1 = 1,
+	HD1 = 2, /* half d1 - 360x(240|288) */
+	CIF = 3,
+	QCIF = 4,
+};
+
+/* ----------------------------------------------------------- */
+/* device / file handle status                                 */
+
+struct tw5864_dev; /* forward delclaration */
+
+/* buffer for one video/vbi/ts frame */
+struct tw5864_buf {
+	struct vb2_v4l2_buffer vb;
+	struct list_head list;
+
+	unsigned int size;
+};
+
+struct tw5864_dma_buf {
+	void *addr;
+	dma_addr_t dma_addr;
+};
+
+enum tw5864_vid_std {
+	STD_NTSC = 0, /* NTSC (M) */
+	STD_PAL = 1, /* PAL (B, D, G, H, I) */
+	STD_SECAM = 2, /* SECAM */
+	STD_NTSC443 = 3, /* NTSC4.43 */
+	STD_PAL_M = 4, /* PAL (M) */
+	STD_PAL_CN = 5, /* PAL (CN) */
+	STD_PAL_60 = 6, /* PAL 60 */
+	STD_INVALID = 7,
+	STD_AUTO = 7,
+};
+
+struct tw5864_input {
+	int nr; /* input number */
+	struct tw5864_dev *root;
+	struct mutex lock; /* used for vidq and vdev */
+	spinlock_t slock; /* used for sync between ISR, tasklet & V4L2 API */
+	struct video_device vdev;
+	struct v4l2_ctrl_handler hdl;
+	struct vb2_queue vidq;
+	struct list_head active;
+	enum resolution resolution;
+	unsigned int width, height;
+	unsigned int frame_seqno;
+	unsigned int frame_gop_seqno;
+	unsigned int h264_idr_pic_id;
+	int enabled;
+	enum tw5864_vid_std std;
+	v4l2_std_id v4l2_std;
+	int tail_nb_bits;
+	u8 tail;
+	u8 *buf_cur_ptr;
+	int buf_cur_space_left;
+
+	u32 reg_interlacing;
+	u32 reg_vlc;
+	u32 reg_dsp_codec;
+	u32 reg_dsp;
+	u32 reg_emu;
+	u32 reg_dsp_qp;
+	u32 reg_dsp_ref_mvp_lambda;
+	u32 reg_dsp_i4x4_weight;
+	u32 buf_id;
+
+	struct tw5864_buf *vb;
+
+	struct v4l2_ctrl *md_threshold_grid_ctrl;
+	u16 md_threshold_grid_values[12 * 16];
+	int qp;
+	int gop;
+
+	/*
+	 * In (1/MAX_FPS) units.
+	 * For max FPS (default), set to 1.
+	 * For 1 FPS, set to e.g. 32.
+	 */
+	int frame_interval;
+	unsigned long new_frame_deadline;
+};
+
+struct tw5864_h264_frame {
+	struct tw5864_dma_buf vlc;
+	struct tw5864_dma_buf mv;
+	int vlc_len;
+	u32 checksum;
+	struct tw5864_input *input;
+	u64 timestamp;
+	unsigned int seqno;
+	unsigned int gop_seqno;
+};
+
+/* global device status */
+struct tw5864_dev {
+	spinlock_t slock; /* used for sync between ISR, tasklet & V4L2 API */
+	struct v4l2_device v4l2_dev;
+	struct tw5864_input inputs[TW5864_INPUTS];
+#define H264_BUF_CNT 4
+	struct tw5864_h264_frame h264_buf[H264_BUF_CNT];
+	int h264_buf_r_index;
+	int h264_buf_w_index;
+
+	struct tasklet_struct tasklet;
+
+	int encoder_busy;
+	/* Input number to check next for ready raw picture (in RR fashion) */
+	int next_input;
+
+	/* pci i/o */
+	char name[64];
+	struct pci_dev *pci;
+	void __iomem *mmio;
+	u32 irqmask;
+};
+
+#define tw_readl(reg) readl(dev->mmio + reg)
+#define tw_mask_readl(reg, mask) \
+	(tw_readl(reg) & (mask))
+#define tw_mask_shift_readl(reg, mask, shift) \
+	(tw_mask_readl((reg), ((mask) << (shift))) >> (shift))
+
+#define tw_writel(reg, value) writel((value), dev->mmio + reg)
+#define tw_mask_writel(reg, mask, value) \
+	tw_writel(reg, (tw_readl(reg) & ~(mask)) | ((value) & (mask)))
+#define tw_mask_shift_writel(reg, mask, shift, value) \
+	tw_mask_writel((reg), ((mask) << (shift)), ((value) << (shift)))
+
+#define tw_setl(reg, bit) tw_writel((reg), tw_readl(reg) | (bit))
+#define tw_clearl(reg, bit) tw_writel((reg), tw_readl(reg) & ~(bit))
+
+u8 tw5864_indir_readb(struct tw5864_dev *dev, u16 addr);
+#define tw_indir_readb(addr) tw5864_indir_readb(dev, addr)
+void tw5864_indir_writeb(struct tw5864_dev *dev, u16 addr, u8 data);
+#define tw_indir_writeb(addr, data) tw5864_indir_writeb(dev, addr, data)
+
+void tw5864_irqmask_apply(struct tw5864_dev *dev);
+int tw5864_video_init(struct tw5864_dev *dev, int *video_nr);
+void tw5864_video_fini(struct tw5864_dev *dev);
+void tw5864_prepare_frame_headers(struct tw5864_input *input);
+void tw5864_h264_put_stream_header(u8 **buf, size_t *space_left, int qp,
+				   int width, int height);
+void tw5864_h264_put_slice_header(u8 **buf, size_t *space_left,
+				  unsigned int idr_pic_id,
+				  unsigned int frame_gop_seqno,
+				  int *tail_nb_bits, u8 *tail);
+void tw5864_request_encoded_frame(struct tw5864_input *input);
diff --git a/drivers/media/pci/tw68/tw68-video.c b/drivers/media/pci/tw68/tw68-video.c
index 5e8212845c87..a45e02367321 100644
--- a/drivers/media/pci/tw68/tw68-video.c
+++ b/drivers/media/pci/tw68/tw68-video.c
@@ -535,7 +535,7 @@ static void tw68_stop_streaming(struct vb2_queue *q)
 	}
 }
 
-static struct vb2_ops tw68_video_qops = {
+static const struct vb2_ops tw68_video_qops = {
 	.queue_setup	= tw68_queue_setup,
 	.buf_queue	= tw68_buf_queue,
 	.buf_prepare	= tw68_buf_prepare,
diff --git a/drivers/media/pci/tw686x/tw686x-audio.c b/drivers/media/pci/tw686x/tw686x-audio.c
index 96e444c49173..77190768622a 100644
--- a/drivers/media/pci/tw686x/tw686x-audio.c
+++ b/drivers/media/pci/tw686x/tw686x-audio.c
@@ -269,7 +269,7 @@ static snd_pcm_uframes_t tw686x_pcm_pointer(struct snd_pcm_substream *ss)
 	return bytes_to_frames(ss->runtime, ac->ptr);
 }
 
-static struct snd_pcm_ops tw686x_pcm_ops = {
+static const struct snd_pcm_ops tw686x_pcm_ops = {
 	.open = tw686x_pcm_open,
 	.close = tw686x_pcm_close,
 	.ioctl = snd_pcm_lib_ioctl,
diff --git a/drivers/media/pci/tw686x/tw686x-video.c b/drivers/media/pci/tw686x/tw686x-video.c
index cdb16de770fe..c3fafa97b2d0 100644
--- a/drivers/media/pci/tw686x/tw686x-video.c
+++ b/drivers/media/pci/tw686x/tw686x-video.c
@@ -577,7 +577,7 @@ static int tw686x_buf_prepare(struct vb2_buffer *vb)
 	return 0;
 }
 
-static struct vb2_ops tw686x_video_qops = {
+static const struct vb2_ops tw686x_video_qops = {
 	.queue_setup		= tw686x_queue_setup,
 	.buf_queue		= tw686x_buf_queue,
 	.buf_prepare		= tw686x_buf_prepare,
@@ -672,30 +672,20 @@ static int tw686x_try_fmt_vid_cap(struct file *file, void *priv,
 	return 0;
 }
 
-static int tw686x_s_fmt_vid_cap(struct file *file, void *priv,
-				struct v4l2_format *f)
+static int tw686x_set_format(struct tw686x_video_channel *vc,
+			     unsigned int pixelformat, unsigned int width,
+			     unsigned int height, bool realloc)
 {
-	struct tw686x_video_channel *vc = video_drvdata(file);
 	struct tw686x_dev *dev = vc->dev;
-	u32 val, width, line_width, height;
-	unsigned long bitsperframe;
+	u32 val, dma_width, dma_height, dma_line_width;
 	int err, pb;
 
-	if (vb2_is_busy(&vc->vidq))
-		return -EBUSY;
-
-	bitsperframe = vc->width * vc->height * vc->format->depth;
-	err = tw686x_try_fmt_vid_cap(file, priv, f);
-	if (err)
-		return err;
-
-	vc->format = format_by_fourcc(f->fmt.pix.pixelformat);
-	vc->width = f->fmt.pix.width;
-	vc->height = f->fmt.pix.height;
+	vc->format = format_by_fourcc(pixelformat);
+	vc->width = width;
+	vc->height = height;
 
 	/* We need new DMA buffers if the framesize has changed */
-	if (dev->dma_ops->alloc &&
-	    bitsperframe != vc->width * vc->height * vc->format->depth) {
+	if (dev->dma_ops->alloc && realloc) {
 		for (pb = 0; pb < 2; pb++)
 			dev->dma_ops->free(vc, pb);
 
@@ -739,14 +729,36 @@ static int tw686x_s_fmt_vid_cap(struct file *file, void *priv,
 	reg_write(vc->dev, VDMA_CHANNEL_CONFIG[vc->ch], val);
 
 	/* Program the DMA frame size */
-	width = (vc->width * 2) & 0x7ff;
-	height = vc->height / 2;
-	line_width = (vc->width * 2) & 0x7ff;
-	val = (height << 22) | (line_width << 11)  | width;
+	dma_width = (vc->width * 2) & 0x7ff;
+	dma_height = vc->height / 2;
+	dma_line_width = (vc->width * 2) & 0x7ff;
+	val = (dma_height << 22) | (dma_line_width << 11)  | dma_width;
 	reg_write(vc->dev, VDMA_WHP[vc->ch], val);
 	return 0;
 }
 
+static int tw686x_s_fmt_vid_cap(struct file *file, void *priv,
+				struct v4l2_format *f)
+{
+	struct tw686x_video_channel *vc = video_drvdata(file);
+	unsigned long area;
+	bool realloc;
+	int err;
+
+	if (vb2_is_busy(&vc->vidq))
+		return -EBUSY;
+
+	area = vc->width * vc->height;
+	err = tw686x_try_fmt_vid_cap(file, priv, f);
+	if (err)
+		return err;
+
+	realloc = area != (f->fmt.pix.width * f->fmt.pix.height);
+	return tw686x_set_format(vc, f->fmt.pix.pixelformat,
+				 f->fmt.pix.width, f->fmt.pix.height,
+				 realloc);
+}
+
 static int tw686x_querycap(struct file *file, void *priv,
 			   struct v4l2_capability *cap)
 {
@@ -763,17 +775,9 @@ static int tw686x_querycap(struct file *file, void *priv,
 	return 0;
 }
 
-static int tw686x_s_std(struct file *file, void *priv, v4l2_std_id id)
+static int tw686x_set_standard(struct tw686x_video_channel *vc, v4l2_std_id id)
 {
-	struct tw686x_video_channel *vc = video_drvdata(file);
-	struct v4l2_format f;
-	u32 val, ret;
-
-	if (vc->video_standard == id)
-		return 0;
-
-	if (vb2_is_busy(&vc->vidq))
-		return -EBUSY;
+	u32 val;
 
 	if (id & V4L2_STD_NTSC)
 		val = 0;
@@ -802,14 +806,31 @@ static int tw686x_s_std(struct file *file, void *priv, v4l2_std_id id)
 		val |= (1 << (SYS_MODE_DMA_SHIFT + vc->ch));
 	reg_write(vc->dev, VIDEO_CONTROL1, val);
 
+	return 0;
+}
+
+static int tw686x_s_std(struct file *file, void *priv, v4l2_std_id id)
+{
+	struct tw686x_video_channel *vc = video_drvdata(file);
+	struct v4l2_format f;
+	int ret;
+
+	if (vc->video_standard == id)
+		return 0;
+
+	if (vb2_is_busy(&vc->vidq))
+		return -EBUSY;
+
+	ret = tw686x_set_standard(vc, id);
+	if (ret)
+		return ret;
 	/*
 	 * Adjust format after V4L2_STD_525_60/V4L2_STD_625_50 change,
 	 * calling g_fmt and s_fmt will sanitize the height
 	 * according to the standard.
 	 */
-	ret = tw686x_g_fmt_vid_cap(file, priv, &f);
-	if (!ret)
-		tw686x_s_fmt_vid_cap(file, priv, &f);
+	tw686x_g_fmt_vid_cap(file, priv, &f);
+	tw686x_s_fmt_vid_cap(file, priv, &f);
 
 	/*
 	 * Frame decimation depends on the chosen standard,
@@ -885,6 +906,42 @@ static int tw686x_g_std(struct file *file, void *priv, v4l2_std_id *id)
 	return 0;
 }
 
+static int tw686x_enum_framesizes(struct file *file, void *priv,
+				  struct v4l2_frmsizeenum *fsize)
+{
+	struct tw686x_video_channel *vc = video_drvdata(file);
+
+	if (fsize->index)
+		return -EINVAL;
+	fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
+	fsize->stepwise.max_width = TW686X_VIDEO_WIDTH;
+	fsize->stepwise.min_width = fsize->stepwise.max_width / 2;
+	fsize->stepwise.step_width = fsize->stepwise.min_width;
+	fsize->stepwise.max_height = TW686X_VIDEO_HEIGHT(vc->video_standard);
+	fsize->stepwise.min_height = fsize->stepwise.max_height / 2;
+	fsize->stepwise.step_height = fsize->stepwise.min_height;
+	return 0;
+}
+
+static int tw686x_enum_frameintervals(struct file *file, void *priv,
+				      struct v4l2_frmivalenum *ival)
+{
+	struct tw686x_video_channel *vc = video_drvdata(file);
+	int max_fps = TW686X_MAX_FPS(vc->video_standard);
+	int max_rates = DIV_ROUND_UP(max_fps, 2);
+
+	if (ival->index >= max_rates)
+		return -EINVAL;
+
+	ival->type = V4L2_FRMIVAL_TYPE_DISCRETE;
+	ival->discrete.numerator = 1;
+	if (ival->index < (max_rates - 1))
+		ival->discrete.denominator = (ival->index + 1) * 2;
+	else
+		ival->discrete.denominator = max_fps;
+	return 0;
+}
+
 static int tw686x_g_parm(struct file *file, void *priv,
 			 struct v4l2_streamparm *sp)
 {
@@ -928,10 +985,21 @@ static int tw686x_enum_fmt_vid_cap(struct file *file, void *priv,
 	return 0;
 }
 
+static void tw686x_set_input(struct tw686x_video_channel *vc, unsigned int i)
+{
+	u32 val;
+
+	vc->input = i;
+
+	val = reg_read(vc->dev, VDMA_CHANNEL_CONFIG[vc->ch]);
+	val &= ~(0x3 << 30);
+	val |= i << 30;
+	reg_write(vc->dev, VDMA_CHANNEL_CONFIG[vc->ch], val);
+}
+
 static int tw686x_s_input(struct file *file, void *priv, unsigned int i)
 {
 	struct tw686x_video_channel *vc = video_drvdata(file);
-	u32 val;
 
 	if (i >= TW686X_INPUTS_PER_CH)
 		return -EINVAL;
@@ -943,12 +1011,7 @@ static int tw686x_s_input(struct file *file, void *priv, unsigned int i)
 	if (vb2_is_busy(&vc->vidq))
 		return -EBUSY;
 
-	vc->input = i;
-
-	val = reg_read(vc->dev, VDMA_CHANNEL_CONFIG[vc->ch]);
-	val &= ~(0x3 << 30);
-	val |= i << 30;
-	reg_write(vc->dev, VDMA_CHANNEL_CONFIG[vc->ch], val);
+	tw686x_set_input(vc, i);
 	return 0;
 }
 
@@ -1007,6 +1070,8 @@ static const struct v4l2_ioctl_ops tw686x_video_ioctl_ops = {
 
 	.vidioc_g_parm			= tw686x_g_parm,
 	.vidioc_s_parm			= tw686x_s_parm,
+	.vidioc_enum_framesizes		= tw686x_enum_framesizes,
+	.vidioc_enum_frameintervals	= tw686x_enum_frameintervals,
 
 	.vidioc_enum_input		= tw686x_enum_input,
 	.vidioc_g_input			= tw686x_g_input,
@@ -1093,8 +1158,7 @@ void tw686x_video_free(struct tw686x_dev *dev)
 	for (ch = 0; ch < max_channels(dev); ch++) {
 		struct tw686x_video_channel *vc = &dev->video_channels[ch];
 
-		if (vc->device)
-			video_unregister_device(vc->device);
+		video_unregister_device(vc->device);
 
 		if (dev->dma_ops->free)
 			for (pb = 0; pb < 2; pb++)
@@ -1104,7 +1168,7 @@ void tw686x_video_free(struct tw686x_dev *dev)
 
 int tw686x_video_init(struct tw686x_dev *dev)
 {
-	unsigned int ch, val, pb;
+	unsigned int ch, val;
 	int err;
 
 	if (dev->dma_mode == TW686X_DMA_MODE_MEMCPY)
@@ -1138,27 +1202,23 @@ int tw686x_video_init(struct tw686x_dev *dev)
 		vc->ch = ch;
 
 		/* default settings */
-		vc->format = &formats[0];
-		vc->video_standard = V4L2_STD_NTSC;
-		vc->width = TW686X_VIDEO_WIDTH;
-		vc->height = TW686X_VIDEO_HEIGHT(vc->video_standard);
-		vc->input = 0;
+		err = tw686x_set_standard(vc, V4L2_STD_NTSC);
+		if (err)
+			goto error;
 
-		reg_write(vc->dev, SDT[ch], 0);
-		tw686x_set_framerate(vc, 30);
+		err = tw686x_set_format(vc, formats[0].fourcc,
+				TW686X_VIDEO_WIDTH,
+				TW686X_VIDEO_HEIGHT(vc->video_standard),
+				true);
+		if (err)
+			goto error;
 
+		tw686x_set_input(vc, 0);
+		tw686x_set_framerate(vc, 30);
 		reg_write(dev, VDELAY_LO[ch], 0x14);
 		reg_write(dev, HACTIVE_LO[ch], 0xd0);
 		reg_write(dev, VIDEO_SIZE[ch], 0);
 
-		if (dev->dma_ops->alloc) {
-			for (pb = 0; pb < 2; pb++) {
-				err = dev->dma_ops->alloc(vc, pb);
-				if (err)
-					goto error;
-			}
-		}
-
 		vc->vidq.io_modes = VB2_READ | VB2_MMAP | VB2_DMABUF;
 		vc->vidq.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 		vc->vidq.drv_priv = vc;
diff --git a/drivers/media/pci/zoran/zoran_driver.c b/drivers/media/pci/zoran/zoran_driver.c
index 80caa70c6360..d6b631add216 100644
--- a/drivers/media/pci/zoran/zoran_driver.c
+++ b/drivers/media/pci/zoran/zoran_driver.c
@@ -2365,94 +2365,80 @@ static int zoran_s_output(struct file *file, void *__fh, unsigned int output)
 }
 
 /* cropping (sub-frame capture) */
-static int zoran_cropcap(struct file *file, void *__fh,
-					struct v4l2_cropcap *cropcap)
+static int zoran_g_selection(struct file *file, void *__fh, struct v4l2_selection *sel)
 {
 	struct zoran_fh *fh = __fh;
 	struct zoran *zr = fh->zr;
-	int type = cropcap->type, res = 0;
 
-	memset(cropcap, 0, sizeof(*cropcap));
-	cropcap->type = type;
+	if (sel->type != V4L2_BUF_TYPE_VIDEO_OUTPUT &&
+	    sel->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
 
-	if (cropcap->type != V4L2_BUF_TYPE_VIDEO_OUTPUT &&
-	    (cropcap->type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
-	     fh->map_mode == ZORAN_MAP_MODE_RAW)) {
+	if (fh->map_mode == ZORAN_MAP_MODE_RAW) {
 		dprintk(1, KERN_ERR
-			"%s: VIDIOC_CROPCAP - subcapture only supported for compressed capture\n",
+			"%s: VIDIOC_G_SELECTION - subcapture only supported for compressed capture\n",
 			ZR_DEVNAME(zr));
-		res = -EINVAL;
-		return res;
+		return -EINVAL;
 	}
 
-	cropcap->bounds.top = cropcap->bounds.left = 0;
-	cropcap->bounds.width = BUZ_MAX_WIDTH;
-	cropcap->bounds.height = BUZ_MAX_HEIGHT;
-	cropcap->defrect.top = cropcap->defrect.left = 0;
-	cropcap->defrect.width = BUZ_MIN_WIDTH;
-	cropcap->defrect.height = BUZ_MIN_HEIGHT;
-	return res;
-}
-
-static int zoran_g_crop(struct file *file, void *__fh, struct v4l2_crop *crop)
-{
-	struct zoran_fh *fh = __fh;
-	struct zoran *zr = fh->zr;
-	int type = crop->type, res = 0;
-
-	memset(crop, 0, sizeof(*crop));
-	crop->type = type;
-
-	if (crop->type != V4L2_BUF_TYPE_VIDEO_OUTPUT &&
-	    (crop->type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
-	     fh->map_mode == ZORAN_MAP_MODE_RAW)) {
-		dprintk(1,
-			KERN_ERR
-			"%s: VIDIOC_G_CROP - subcapture only supported for compressed capture\n",
-			ZR_DEVNAME(zr));
-		res = -EINVAL;
-		return res;
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP:
+		sel->r.top = fh->jpg_settings.img_y;
+		sel->r.left = fh->jpg_settings.img_x;
+		sel->r.width = fh->jpg_settings.img_width;
+		sel->r.height = fh->jpg_settings.img_height;
+		break;
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+		sel->r.top = sel->r.left = 0;
+		sel->r.width = BUZ_MIN_WIDTH;
+		sel->r.height = BUZ_MIN_HEIGHT;
+		break;
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		sel->r.top = sel->r.left = 0;
+		sel->r.width = BUZ_MAX_WIDTH;
+		sel->r.height = BUZ_MAX_HEIGHT;
+		break;
+	default:
+		return -EINVAL;
 	}
-
-	crop->c.top = fh->jpg_settings.img_y;
-	crop->c.left = fh->jpg_settings.img_x;
-	crop->c.width = fh->jpg_settings.img_width;
-	crop->c.height = fh->jpg_settings.img_height;
-	return res;
+	return 0;
 }
 
-static int zoran_s_crop(struct file *file, void *__fh, const struct v4l2_crop *crop)
+static int zoran_s_selection(struct file *file, void *__fh, struct v4l2_selection *sel)
 {
 	struct zoran_fh *fh = __fh;
 	struct zoran *zr = fh->zr;
-	int res = 0;
 	struct zoran_jpg_settings settings;
+	int res;
 
-	settings = fh->jpg_settings;
+	if (sel->type != V4L2_BUF_TYPE_VIDEO_OUTPUT &&
+	    sel->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
 
-	if (fh->buffers.allocated) {
+	if (sel->target != V4L2_SEL_TGT_CROP)
+		return -EINVAL;
+
+	if (fh->map_mode == ZORAN_MAP_MODE_RAW) {
 		dprintk(1, KERN_ERR
-			"%s: VIDIOC_S_CROP - cannot change settings while active\n",
+			"%s: VIDIOC_S_SELECTION - subcapture only supported for compressed capture\n",
 			ZR_DEVNAME(zr));
-		res = -EBUSY;
-		return res;
+		return -EINVAL;
 	}
 
-	if (crop->type != V4L2_BUF_TYPE_VIDEO_OUTPUT &&
-	    (crop->type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
-	     fh->map_mode == ZORAN_MAP_MODE_RAW)) {
+	settings = fh->jpg_settings;
+
+	if (fh->buffers.allocated) {
 		dprintk(1, KERN_ERR
-			"%s: VIDIOC_G_CROP - subcapture only supported for compressed capture\n",
+			"%s: VIDIOC_S_SELECTION - cannot change settings while active\n",
 			ZR_DEVNAME(zr));
-		res = -EINVAL;
-		return res;
+		return -EBUSY;
 	}
 
 	/* move into a form that we understand */
-	settings.img_x = crop->c.left;
-	settings.img_y = crop->c.top;
-	settings.img_width = crop->c.width;
-	settings.img_height = crop->c.height;
+	settings.img_x = sel->r.left;
+	settings.img_y = sel->r.top;
+	settings.img_width = sel->r.width;
+	settings.img_height = sel->r.height;
 
 	/* check validity */
 	res = zoran_check_jpg_settings(zr, &settings, 0);
@@ -2808,9 +2794,8 @@ zoran_mmap (struct file           *file,
 
 static const struct v4l2_ioctl_ops zoran_ioctl_ops = {
 	.vidioc_querycap    		    = zoran_querycap,
-	.vidioc_cropcap       		    = zoran_cropcap,
-	.vidioc_s_crop       		    = zoran_s_crop,
-	.vidioc_g_crop       		    = zoran_g_crop,
+	.vidioc_s_selection		    = zoran_s_selection,
+	.vidioc_g_selection		    = zoran_g_selection,
 	.vidioc_enum_input     		    = zoran_enum_input,
 	.vidioc_g_input      		    = zoran_g_input,
 	.vidioc_s_input      		    = zoran_s_input,
diff --git a/drivers/media/platform/Kconfig b/drivers/media/platform/Kconfig
index 552b635cfce7..ce4a96fccc43 100644
--- a/drivers/media/platform/Kconfig
+++ b/drivers/media/platform/Kconfig
@@ -91,6 +91,15 @@ config VIDEO_OMAP3_DEBUG
 	---help---
 	  Enable debug messages on OMAP 3 camera controller driver.
 
+config VIDEO_PXA27x
+	tristate "PXA27x Quick Capture Interface driver"
+	depends on VIDEO_DEV && HAS_DMA
+	depends on PXA27x || COMPILE_TEST
+	select VIDEOBUF2_DMA_SG
+	select SG_SPLIT
+	---help---
+	  This is a v4l2 driver for the PXA27x Quick Capture Interface
+
 config VIDEO_S3C_CAMIF
 	tristate "Samsung S3C24XX/S3C64XX SoC Camera Interface driver"
 	depends on VIDEO_V4L2 && I2C && VIDEO_V4L2_SUBDEV_API
@@ -107,10 +116,10 @@ config VIDEO_S3C_CAMIF
 
 source "drivers/media/platform/soc_camera/Kconfig"
 source "drivers/media/platform/exynos4-is/Kconfig"
-source "drivers/media/platform/s5p-tv/Kconfig"
 source "drivers/media/platform/am437x/Kconfig"
 source "drivers/media/platform/xilinx/Kconfig"
 source "drivers/media/platform/rcar-vin/Kconfig"
+source "drivers/media/platform/atmel/Kconfig"
 
 config VIDEO_TI_CAL
 	tristate "TI CAL (Camera Adaptation Layer) driver"
@@ -155,7 +164,7 @@ config VIDEO_CODA
 
 config VIDEO_MEDIATEK_VPU
 	tristate "Mediatek Video Processor Unit"
-	depends on VIDEO_DEV && VIDEO_V4L2
+	depends on VIDEO_DEV && VIDEO_V4L2 && HAS_DMA
 	depends on ARCH_MEDIATEK || COMPILE_TEST
 	---help---
 	    This driver provides downloading VPU firmware and
@@ -257,6 +266,21 @@ config VIDEO_STI_BDISP
 	help
 	  This v4l2 mem2mem driver is a 2D blitter for STMicroelectronics SoC.
 
+config VIDEO_STI_HVA
+	tristate "STMicroelectronics HVA multi-format video encoder V4L2 driver"
+	depends on VIDEO_DEV && VIDEO_V4L2
+	depends on HAS_DMA
+	depends on ARCH_STI || COMPILE_TEST
+	select VIDEOBUF2_DMA_CONTIG
+	select V4L2_MEM2MEM_DEV
+	help
+	  This V4L2 driver enables HVA (Hardware Video Accelerator) multi-format
+	  video encoder of STMicroelectronics SoC, allowing hardware encoding of
+	  raw uncompressed formats in various compressed video bitstreams format.
+
+	  To compile this driver as a module, choose M here:
+	  the module will be called st-hva.
+
 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 21771c1a13fb..40b18d12726e 100644
--- a/drivers/media/platform/Makefile
+++ b/drivers/media/platform/Makefile
@@ -9,6 +9,7 @@ obj-$(CONFIG_VIDEO_CAFE_CCIC) += marvell-ccic/
 obj-$(CONFIG_VIDEO_MMP_CAMERA) += marvell-ccic/
 
 obj-$(CONFIG_VIDEO_OMAP3)	+= omap3isp/
+obj-$(CONFIG_VIDEO_PXA27x)	+= pxa_camera.o
 
 obj-$(CONFIG_VIDEO_VIU) += fsl-viu.o
 
@@ -30,12 +31,12 @@ obj-$(CONFIG_VIDEO_S3C_CAMIF) 		+= s3c-camif/
 obj-$(CONFIG_VIDEO_SAMSUNG_EXYNOS4_IS) 	+= exynos4-is/
 obj-$(CONFIG_VIDEO_SAMSUNG_S5P_JPEG)	+= s5p-jpeg/
 obj-$(CONFIG_VIDEO_SAMSUNG_S5P_MFC)	+= s5p-mfc/
-obj-$(CONFIG_VIDEO_SAMSUNG_S5P_TV)	+= s5p-tv/
 
 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_VIDEO_STI_HVA)		+= sti/hva/
 obj-$(CONFIG_DVB_C8SECTPFE)		+= sti/c8sectpfe/
 
 obj-$(CONFIG_BLACKFIN)                  += blackfin/
@@ -58,6 +59,8 @@ obj-$(CONFIG_VIDEO_XILINX)		+= xilinx/
 
 obj-$(CONFIG_VIDEO_RCAR_VIN)		+= rcar-vin/
 
+obj-$(CONFIG_VIDEO_ATMEL_ISC)		+= atmel/
+
 ccflags-y += -I$(srctree)/drivers/media/i2c
 
 obj-$(CONFIG_VIDEO_MEDIATEK_VPU)	+= mtk-vpu/
diff --git a/drivers/media/platform/atmel/Kconfig b/drivers/media/platform/atmel/Kconfig
new file mode 100644
index 000000000000..867dca22a473
--- /dev/null
+++ b/drivers/media/platform/atmel/Kconfig
@@ -0,0 +1,9 @@
+config VIDEO_ATMEL_ISC
+	tristate "ATMEL Image Sensor Controller (ISC) support"
+	depends on VIDEO_V4L2 && COMMON_CLK && VIDEO_V4L2_SUBDEV_API && HAS_DMA
+	depends on ARCH_AT91 || COMPILE_TEST
+	select VIDEOBUF2_DMA_CONTIG
+	select REGMAP_MMIO
+	help
+	   This module makes the ATMEL Image Sensor Controller available
+	   as a v4l2 device.
\ No newline at end of file
diff --git a/drivers/media/platform/atmel/Makefile b/drivers/media/platform/atmel/Makefile
new file mode 100644
index 000000000000..9d7c999d434d
--- /dev/null
+++ b/drivers/media/platform/atmel/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_VIDEO_ATMEL_ISC) += atmel-isc.o
diff --git a/drivers/media/platform/atmel/atmel-isc-regs.h b/drivers/media/platform/atmel/atmel-isc-regs.h
new file mode 100644
index 000000000000..00c449717cde
--- /dev/null
+++ b/drivers/media/platform/atmel/atmel-isc-regs.h
@@ -0,0 +1,165 @@
+#ifndef __ATMEL_ISC_REGS_H
+#define __ATMEL_ISC_REGS_H
+
+#include <linux/bitops.h>
+
+/* ISC Control Enable Register 0 */
+#define ISC_CTRLEN      0x00000000
+
+/* ISC Control Disable Register 0 */
+#define ISC_CTRLDIS     0x00000004
+
+/* ISC Control Status Register 0 */
+#define ISC_CTRLSR      0x00000008
+
+#define ISC_CTRL_CAPTURE	BIT(0)
+#define ISC_CTRL_UPPRO		BIT(1)
+#define ISC_CTRL_HISREQ		BIT(2)
+#define ISC_CTRL_HISCLR		BIT(3)
+
+/* ISC Parallel Front End Configuration 0 Register */
+#define ISC_PFE_CFG0    0x0000000c
+
+#define ISC_PFE_CFG0_HPOL_LOW   BIT(0)
+#define ISC_PFE_CFG0_VPOL_LOW   BIT(1)
+#define ISC_PFE_CFG0_PPOL_LOW   BIT(2)
+
+#define ISC_PFE_CFG0_MODE_PROGRESSIVE   (0x0 << 4)
+#define ISC_PFE_CFG0_MODE_MASK          GENMASK(6, 4)
+
+#define ISC_PFE_CFG0_BPS_EIGHT  (0x4 << 28)
+#define ISC_PFG_CFG0_BPS_NINE   (0x3 << 28)
+#define ISC_PFG_CFG0_BPS_TEN    (0x2 << 28)
+#define ISC_PFG_CFG0_BPS_ELEVEN (0x1 << 28)
+#define ISC_PFG_CFG0_BPS_TWELVE (0x0 << 28)
+#define ISC_PFE_CFG0_BPS_MASK   GENMASK(30, 28)
+
+/* ISC Clock Enable Register */
+#define ISC_CLKEN               0x00000018
+
+/* ISC Clock Disable Register */
+#define ISC_CLKDIS              0x0000001c
+
+/* ISC Clock Status Register */
+#define ISC_CLKSR               0x00000020
+
+#define ISC_CLK(n)		BIT(n)
+
+/* ISC Clock Configuration Register */
+#define ISC_CLKCFG              0x00000024
+#define ISC_CLKCFG_DIV_SHIFT(n) ((n)*16)
+#define ISC_CLKCFG_DIV_MASK(n)  GENMASK(((n)*16 + 7), (n)*16)
+#define ISC_CLKCFG_SEL_SHIFT(n) ((n)*16 + 8)
+#define ISC_CLKCFG_SEL_MASK(n)  GENMASK(((n)*17 + 8), ((n)*16 + 8))
+
+/* ISC Interrupt Enable Register */
+#define ISC_INTEN       0x00000028
+
+/* ISC Interrupt Disable Register */
+#define ISC_INTDIS      0x0000002c
+
+/* ISC Interrupt Mask Register */
+#define ISC_INTMASK     0x00000030
+
+/* ISC Interrupt Status Register */
+#define ISC_INTSR       0x00000034
+
+#define ISC_INT_DDONE		BIT(8)
+
+/* ISC White Balance Control Register */
+#define ISC_WB_CTRL     0x00000058
+
+/* ISC White Balance Configuration Register */
+#define ISC_WB_CFG      0x0000005c
+
+/* ISC Color Filter Array Control Register */
+#define ISC_CFA_CTRL    0x00000070
+
+/* ISC Color Filter Array Configuration Register */
+#define ISC_CFA_CFG     0x00000074
+
+#define ISC_BAY_CFG_GRGR	0x0
+#define ISC_BAY_CFG_RGRG	0x1
+#define ISC_BAY_CFG_GBGB	0x2
+#define ISC_BAY_CFG_BGBG	0x3
+#define ISC_BAY_CFG_MASK	GENMASK(1, 0)
+
+/* ISC Color Correction Control Register */
+#define ISC_CC_CTRL     0x00000078
+
+/* ISC Gamma Correction Control Register */
+#define ISC_GAM_CTRL    0x00000094
+
+/* Color Space Conversion Control Register */
+#define ISC_CSC_CTRL    0x00000398
+
+/* Contrast And Brightness Control Register */
+#define ISC_CBC_CTRL    0x000003b4
+
+/* Subsampling 4:4:4 to 4:2:2 Control Register */
+#define ISC_SUB422_CTRL 0x000003c4
+
+/* Subsampling 4:2:2 to 4:2:0 Control Register */
+#define ISC_SUB420_CTRL 0x000003cc
+
+/* Rounding, Limiting and Packing Configuration Register */
+#define ISC_RLP_CFG     0x000003d0
+
+#define ISC_RLP_CFG_MODE_DAT8           0x0
+#define ISC_RLP_CFG_MODE_DAT9           0x1
+#define ISC_RLP_CFG_MODE_DAT10          0x2
+#define ISC_RLP_CFG_MODE_DAT11          0x3
+#define ISC_RLP_CFG_MODE_DAT12          0x4
+#define ISC_RLP_CFG_MODE_DATY8          0x5
+#define ISC_RLP_CFG_MODE_DATY10         0x6
+#define ISC_RLP_CFG_MODE_ARGB444        0x7
+#define ISC_RLP_CFG_MODE_ARGB555        0x8
+#define ISC_RLP_CFG_MODE_RGB565         0x9
+#define ISC_RLP_CFG_MODE_ARGB32         0xa
+#define ISC_RLP_CFG_MODE_YYCC           0xb
+#define ISC_RLP_CFG_MODE_YYCC_LIMITED   0xc
+#define ISC_RLP_CFG_MODE_MASK           GENMASK(3, 0)
+
+/* DMA Configuration Register */
+#define ISC_DCFG        0x000003e0
+#define ISC_DCFG_IMODE_PACKED8          0x0
+#define ISC_DCFG_IMODE_PACKED16         0x1
+#define ISC_DCFG_IMODE_PACKED32         0x2
+#define ISC_DCFG_IMODE_YC422SP          0x3
+#define ISC_DCFG_IMODE_YC422P           0x4
+#define ISC_DCFG_IMODE_YC420SP          0x5
+#define ISC_DCFG_IMODE_YC420P           0x6
+#define ISC_DCFG_IMODE_MASK             GENMASK(2, 0)
+
+#define ISC_DCFG_YMBSIZE_SINGLE         (0x0 << 4)
+#define ISC_DCFG_YMBSIZE_BEATS4         (0x1 << 4)
+#define ISC_DCFG_YMBSIZE_BEATS8         (0x2 << 4)
+#define ISC_DCFG_YMBSIZE_BEATS16        (0x3 << 4)
+#define ISC_DCFG_YMBSIZE_MASK           GENMASK(5, 4)
+
+#define ISC_DCFG_CMBSIZE_SINGLE         (0x0 << 8)
+#define ISC_DCFG_CMBSIZE_BEATS4         (0x1 << 8)
+#define ISC_DCFG_CMBSIZE_BEATS8         (0x2 << 8)
+#define ISC_DCFG_CMBSIZE_BEATS16        (0x3 << 8)
+#define ISC_DCFG_CMBSIZE_MASK           GENMASK(9, 8)
+
+/* DMA Control Register */
+#define ISC_DCTRL       0x000003e4
+
+#define ISC_DCTRL_DVIEW_PACKED          (0x0 << 1)
+#define ISC_DCTRL_DVIEW_SEMIPLANAR      (0x1 << 1)
+#define ISC_DCTRL_DVIEW_PLANAR          (0x2 << 1)
+#define ISC_DCTRL_DVIEW_MASK            GENMASK(2, 1)
+
+#define ISC_DCTRL_IE_IS			(0x0 << 4)
+
+/* DMA Descriptor Address Register */
+#define ISC_DNDA        0x000003e8
+
+/* DMA Address 0 Register */
+#define ISC_DAD0        0x000003ec
+
+/* DMA Stride 0 Register */
+#define ISC_DST0        0x000003f0
+
+#endif
diff --git a/drivers/media/platform/atmel/atmel-isc.c b/drivers/media/platform/atmel/atmel-isc.c
new file mode 100644
index 000000000000..ccfe13b7d3f8
--- /dev/null
+++ b/drivers/media/platform/atmel/atmel-isc.c
@@ -0,0 +1,1520 @@
+/*
+ * Atmel Image Sensor Controller (ISC) driver
+ *
+ * Copyright (C) 2016 Atmel
+ *
+ * Author: Songjun Wu <songjun.wu@microchip.com>
+ *
+ * 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.
+ *
+ * Sensor-->PFE-->WB-->CFA-->CC-->GAM-->CSC-->CBC-->SUB-->RLP-->DMA
+ *
+ * ISC video pipeline integrates the following submodules:
+ * PFE: Parallel Front End to sample the camera sensor input stream
+ *  WB: Programmable white balance in the Bayer domain
+ * CFA: Color filter array interpolation module
+ *  CC: Programmable color correction
+ * GAM: Gamma correction
+ * CSC: Programmable color space conversion
+ * CBC: Contrast and Brightness control
+ * SUB: This module performs YCbCr444 to YCbCr420 chrominance subsampling
+ * RLP: This module performs rounding, range limiting
+ *      and packing of the incoming data
+ */
+
+#include <linux/clk.h>
+#include <linux/clkdev.h>
+#include <linux/clk-provider.h>
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/regmap.h>
+#include <linux/videodev2.h>
+
+#include <media/v4l2-device.h>
+#include <media/v4l2-image-sizes.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-of.h>
+#include <media/v4l2-subdev.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "atmel-isc-regs.h"
+
+#define ATMEL_ISC_NAME		"atmel_isc"
+
+#define ISC_MAX_SUPPORT_WIDTH   2592
+#define ISC_MAX_SUPPORT_HEIGHT  1944
+
+#define ISC_CLK_MAX_DIV		255
+
+enum isc_clk_id {
+	ISC_ISPCK = 0,
+	ISC_MCK = 1,
+};
+
+struct isc_clk {
+	struct clk_hw   hw;
+	struct clk      *clk;
+	struct regmap   *regmap;
+	u8		id;
+	u8		parent_id;
+	u32		div;
+	struct device	*dev;
+};
+
+#define to_isc_clk(hw) container_of(hw, struct isc_clk, hw)
+
+struct isc_buffer {
+	struct vb2_v4l2_buffer  vb;
+	struct list_head	list;
+};
+
+struct isc_subdev_entity {
+	struct v4l2_subdev		*sd;
+	struct v4l2_async_subdev	*asd;
+	struct v4l2_async_notifier      notifier;
+	struct v4l2_subdev_pad_config	*config;
+
+	u32 pfe_cfg0;
+
+	struct list_head list;
+};
+
+/*
+ * struct isc_format - ISC media bus format information
+ * @fourcc:		Fourcc code for this format
+ * @mbus_code:		V4L2 media bus format code.
+ * @bpp:		Bytes per pixel (when stored in memory)
+ * @reg_bps:		reg value for bits per sample
+ *			(when transferred over a bus)
+ * @support:		Indicates format supported by subdev
+ */
+struct isc_format {
+	u32	fourcc;
+	u32	mbus_code;
+	u8	bpp;
+
+	u32	reg_bps;
+	u32	reg_rlp_mode;
+	u32	reg_dcfg_imode;
+	u32	reg_dctrl_dview;
+
+	bool	support;
+};
+
+#define ISC_PIPE_LINE_NODE_NUM	11
+
+struct isc_device {
+	struct regmap		*regmap;
+	struct clk		*hclock;
+	struct clk		*ispck;
+	struct isc_clk		isc_clks[2];
+
+	struct device		*dev;
+	struct v4l2_device	v4l2_dev;
+	struct video_device	video_dev;
+
+	struct vb2_queue	vb2_vidq;
+	spinlock_t		dma_queue_lock;
+	struct list_head	dma_queue;
+	struct isc_buffer	*cur_frm;
+	unsigned int		sequence;
+	bool			stop;
+	struct completion	comp;
+
+	struct v4l2_format	fmt;
+	struct isc_format	**user_formats;
+	unsigned int		num_user_formats;
+	const struct isc_format	*current_fmt;
+
+	struct mutex		lock;
+
+	struct regmap_field	*pipeline[ISC_PIPE_LINE_NODE_NUM];
+
+	struct isc_subdev_entity	*current_subdev;
+	struct list_head		subdev_entities;
+};
+
+static struct isc_format isc_formats[] = {
+	{ V4L2_PIX_FMT_SBGGR8, MEDIA_BUS_FMT_SBGGR8_1X8,
+	  1, ISC_PFE_CFG0_BPS_EIGHT, ISC_RLP_CFG_MODE_DAT8,
+	  ISC_DCFG_IMODE_PACKED8, ISC_DCTRL_DVIEW_PACKED, false },
+	{ V4L2_PIX_FMT_SGBRG8, MEDIA_BUS_FMT_SGBRG8_1X8,
+	  1, ISC_PFE_CFG0_BPS_EIGHT, ISC_RLP_CFG_MODE_DAT8,
+	  ISC_DCFG_IMODE_PACKED8, ISC_DCTRL_DVIEW_PACKED, false },
+	{ V4L2_PIX_FMT_SGRBG8, MEDIA_BUS_FMT_SGRBG8_1X8,
+	  1, ISC_PFE_CFG0_BPS_EIGHT, ISC_RLP_CFG_MODE_DAT8,
+	  ISC_DCFG_IMODE_PACKED8, ISC_DCTRL_DVIEW_PACKED, false },
+	{ V4L2_PIX_FMT_SRGGB8, MEDIA_BUS_FMT_SRGGB8_1X8,
+	  1, ISC_PFE_CFG0_BPS_EIGHT, ISC_RLP_CFG_MODE_DAT8,
+	  ISC_DCFG_IMODE_PACKED8, ISC_DCTRL_DVIEW_PACKED, false },
+
+	{ V4L2_PIX_FMT_SBGGR10, MEDIA_BUS_FMT_SBGGR10_1X10,
+	  2, ISC_PFG_CFG0_BPS_TEN, ISC_RLP_CFG_MODE_DAT10,
+	  ISC_DCFG_IMODE_PACKED16, ISC_DCTRL_DVIEW_PACKED, false },
+	{ V4L2_PIX_FMT_SGBRG10, MEDIA_BUS_FMT_SGBRG10_1X10,
+	  2, ISC_PFG_CFG0_BPS_TEN, ISC_RLP_CFG_MODE_DAT10,
+	  ISC_DCFG_IMODE_PACKED16, ISC_DCTRL_DVIEW_PACKED, false },
+	{ V4L2_PIX_FMT_SGRBG10, MEDIA_BUS_FMT_SGRBG10_1X10,
+	  2, ISC_PFG_CFG0_BPS_TEN, ISC_RLP_CFG_MODE_DAT10,
+	  ISC_DCFG_IMODE_PACKED16, ISC_DCTRL_DVIEW_PACKED, false },
+	{ V4L2_PIX_FMT_SRGGB10, MEDIA_BUS_FMT_SRGGB10_1X10,
+	  2, ISC_PFG_CFG0_BPS_TEN, ISC_RLP_CFG_MODE_DAT10,
+	  ISC_DCFG_IMODE_PACKED16, ISC_DCTRL_DVIEW_PACKED, false },
+
+	{ V4L2_PIX_FMT_SBGGR12, MEDIA_BUS_FMT_SBGGR12_1X12,
+	  2, ISC_PFG_CFG0_BPS_TWELVE, ISC_RLP_CFG_MODE_DAT12,
+	  ISC_DCFG_IMODE_PACKED16, ISC_DCTRL_DVIEW_PACKED, false },
+	{ V4L2_PIX_FMT_SGBRG12, MEDIA_BUS_FMT_SGBRG12_1X12,
+	  2, ISC_PFG_CFG0_BPS_TWELVE, ISC_RLP_CFG_MODE_DAT12,
+	  ISC_DCFG_IMODE_PACKED16, ISC_DCTRL_DVIEW_PACKED, false },
+	{ V4L2_PIX_FMT_SGRBG12, MEDIA_BUS_FMT_SGRBG12_1X12,
+	  2, ISC_PFG_CFG0_BPS_TWELVE, ISC_RLP_CFG_MODE_DAT12,
+	  ISC_DCFG_IMODE_PACKED16, ISC_DCTRL_DVIEW_PACKED, false },
+	{ V4L2_PIX_FMT_SRGGB12, MEDIA_BUS_FMT_SRGGB12_1X12,
+	  2, ISC_PFG_CFG0_BPS_TWELVE, ISC_RLP_CFG_MODE_DAT12,
+	  ISC_DCFG_IMODE_PACKED16, ISC_DCTRL_DVIEW_PACKED, false },
+
+	{ V4L2_PIX_FMT_YUYV, MEDIA_BUS_FMT_YUYV8_2X8,
+	  2, ISC_PFE_CFG0_BPS_EIGHT, ISC_RLP_CFG_MODE_DAT8,
+	  ISC_DCFG_IMODE_PACKED8, ISC_DCTRL_DVIEW_PACKED, false },
+};
+
+static int isc_clk_enable(struct clk_hw *hw)
+{
+	struct isc_clk *isc_clk = to_isc_clk(hw);
+	u32 id = isc_clk->id;
+	struct regmap *regmap = isc_clk->regmap;
+
+	dev_dbg(isc_clk->dev, "ISC CLK: %s, div = %d, parent id = %d\n",
+		__func__, isc_clk->div, isc_clk->parent_id);
+
+	regmap_update_bits(regmap, ISC_CLKCFG,
+			   ISC_CLKCFG_DIV_MASK(id) | ISC_CLKCFG_SEL_MASK(id),
+			   (isc_clk->div << ISC_CLKCFG_DIV_SHIFT(id)) |
+			   (isc_clk->parent_id << ISC_CLKCFG_SEL_SHIFT(id)));
+
+	regmap_write(regmap, ISC_CLKEN, ISC_CLK(id));
+
+	return 0;
+}
+
+static void isc_clk_disable(struct clk_hw *hw)
+{
+	struct isc_clk *isc_clk = to_isc_clk(hw);
+	u32 id = isc_clk->id;
+
+	regmap_write(isc_clk->regmap, ISC_CLKDIS, ISC_CLK(id));
+}
+
+static int isc_clk_is_enabled(struct clk_hw *hw)
+{
+	struct isc_clk *isc_clk = to_isc_clk(hw);
+	u32 status;
+
+	regmap_read(isc_clk->regmap, ISC_CLKSR, &status);
+
+	return status & ISC_CLK(isc_clk->id) ? 1 : 0;
+}
+
+static unsigned long
+isc_clk_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
+{
+	struct isc_clk *isc_clk = to_isc_clk(hw);
+
+	return DIV_ROUND_CLOSEST(parent_rate, isc_clk->div + 1);
+}
+
+static int isc_clk_determine_rate(struct clk_hw *hw,
+				   struct clk_rate_request *req)
+{
+	struct isc_clk *isc_clk = to_isc_clk(hw);
+	long best_rate = -EINVAL;
+	int best_diff = -1;
+	unsigned int i, div;
+
+	for (i = 0; i < clk_hw_get_num_parents(hw); i++) {
+		struct clk_hw *parent;
+		unsigned long parent_rate;
+
+		parent = clk_hw_get_parent_by_index(hw, i);
+		if (!parent)
+			continue;
+
+		parent_rate = clk_hw_get_rate(parent);
+		if (!parent_rate)
+			continue;
+
+		for (div = 1; div < ISC_CLK_MAX_DIV + 2; div++) {
+			unsigned long rate;
+			int diff;
+
+			rate = DIV_ROUND_CLOSEST(parent_rate, div);
+			diff = abs(req->rate - rate);
+
+			if (best_diff < 0 || best_diff > diff) {
+				best_rate = rate;
+				best_diff = diff;
+				req->best_parent_rate = parent_rate;
+				req->best_parent_hw = parent;
+			}
+
+			if (!best_diff || rate < req->rate)
+				break;
+		}
+
+		if (!best_diff)
+			break;
+	}
+
+	dev_dbg(isc_clk->dev,
+		"ISC CLK: %s, best_rate = %ld, parent clk: %s @ %ld\n",
+		__func__, best_rate,
+		__clk_get_name((req->best_parent_hw)->clk),
+		req->best_parent_rate);
+
+	if (best_rate < 0)
+		return best_rate;
+
+	req->rate = best_rate;
+
+	return 0;
+}
+
+static int isc_clk_set_parent(struct clk_hw *hw, u8 index)
+{
+	struct isc_clk *isc_clk = to_isc_clk(hw);
+
+	if (index >= clk_hw_get_num_parents(hw))
+		return -EINVAL;
+
+	isc_clk->parent_id = index;
+
+	return 0;
+}
+
+static u8 isc_clk_get_parent(struct clk_hw *hw)
+{
+	struct isc_clk *isc_clk = to_isc_clk(hw);
+
+	return isc_clk->parent_id;
+}
+
+static int isc_clk_set_rate(struct clk_hw *hw,
+			     unsigned long rate,
+			     unsigned long parent_rate)
+{
+	struct isc_clk *isc_clk = to_isc_clk(hw);
+	u32 div;
+
+	if (!rate)
+		return -EINVAL;
+
+	div = DIV_ROUND_CLOSEST(parent_rate, rate);
+	if (div > (ISC_CLK_MAX_DIV + 1) || !div)
+		return -EINVAL;
+
+	isc_clk->div = div - 1;
+
+	return 0;
+}
+
+static const struct clk_ops isc_clk_ops = {
+	.enable		= isc_clk_enable,
+	.disable	= isc_clk_disable,
+	.is_enabled	= isc_clk_is_enabled,
+	.recalc_rate	= isc_clk_recalc_rate,
+	.determine_rate	= isc_clk_determine_rate,
+	.set_parent	= isc_clk_set_parent,
+	.get_parent	= isc_clk_get_parent,
+	.set_rate	= isc_clk_set_rate,
+};
+
+static int isc_clk_register(struct isc_device *isc, unsigned int id)
+{
+	struct regmap *regmap = isc->regmap;
+	struct device_node *np = isc->dev->of_node;
+	struct isc_clk *isc_clk;
+	struct clk_init_data init;
+	const char *clk_name = np->name;
+	const char *parent_names[3];
+	int num_parents;
+
+	num_parents = of_clk_get_parent_count(np);
+	if (num_parents < 1 || num_parents > 3)
+		return -EINVAL;
+
+	if (num_parents > 2 && id == ISC_ISPCK)
+		num_parents = 2;
+
+	of_clk_parent_fill(np, parent_names, num_parents);
+
+	if (id == ISC_MCK)
+		of_property_read_string(np, "clock-output-names", &clk_name);
+	else
+		clk_name = "isc-ispck";
+
+	init.parent_names	= parent_names;
+	init.num_parents	= num_parents;
+	init.name		= clk_name;
+	init.ops		= &isc_clk_ops;
+	init.flags		= CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE;
+
+	isc_clk = &isc->isc_clks[id];
+	isc_clk->hw.init	= &init;
+	isc_clk->regmap		= regmap;
+	isc_clk->id		= id;
+	isc_clk->dev		= isc->dev;
+
+	isc_clk->clk = clk_register(isc->dev, &isc_clk->hw);
+	if (IS_ERR(isc_clk->clk)) {
+		dev_err(isc->dev, "%s: clock register fail\n", clk_name);
+		return PTR_ERR(isc_clk->clk);
+	} else if (id == ISC_MCK)
+		of_clk_add_provider(np, of_clk_src_simple_get, isc_clk->clk);
+
+	return 0;
+}
+
+static int isc_clk_init(struct isc_device *isc)
+{
+	unsigned int i;
+	int ret;
+
+	for (i = 0; i < ARRAY_SIZE(isc->isc_clks); i++)
+		isc->isc_clks[i].clk = ERR_PTR(-EINVAL);
+
+	for (i = 0; i < ARRAY_SIZE(isc->isc_clks); i++) {
+		ret = isc_clk_register(isc, i);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+static void isc_clk_cleanup(struct isc_device *isc)
+{
+	unsigned int i;
+
+	of_clk_del_provider(isc->dev->of_node);
+
+	for (i = 0; i < ARRAY_SIZE(isc->isc_clks); i++) {
+		struct isc_clk *isc_clk = &isc->isc_clks[i];
+
+		if (!IS_ERR(isc_clk->clk))
+			clk_unregister(isc_clk->clk);
+	}
+}
+
+static int isc_queue_setup(struct vb2_queue *vq,
+			    unsigned int *nbuffers, unsigned int *nplanes,
+			    unsigned int sizes[], struct device *alloc_devs[])
+{
+	struct isc_device *isc = vb2_get_drv_priv(vq);
+	unsigned int size = isc->fmt.fmt.pix.sizeimage;
+
+	if (*nplanes)
+		return sizes[0] < size ? -EINVAL : 0;
+
+	*nplanes = 1;
+	sizes[0] = size;
+
+	return 0;
+}
+
+static int isc_buffer_prepare(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct isc_device *isc = vb2_get_drv_priv(vb->vb2_queue);
+	unsigned long size = isc->fmt.fmt.pix.sizeimage;
+
+	if (vb2_plane_size(vb, 0) < size) {
+		v4l2_err(&isc->v4l2_dev, "buffer too small (%lu < %lu)\n",
+			 vb2_plane_size(vb, 0), size);
+		return -EINVAL;
+	}
+
+	vb2_set_plane_payload(vb, 0, size);
+
+	vbuf->field = isc->fmt.fmt.pix.field;
+
+	return 0;
+}
+
+static inline void isc_start_dma(struct regmap *regmap,
+				  struct isc_buffer *frm, u32 dview)
+{
+	dma_addr_t addr;
+
+	addr = vb2_dma_contig_plane_dma_addr(&frm->vb.vb2_buf, 0);
+
+	regmap_write(regmap, ISC_DCTRL, dview | ISC_DCTRL_IE_IS);
+	regmap_write(regmap, ISC_DAD0, addr);
+	regmap_write(regmap, ISC_CTRLEN, ISC_CTRL_CAPTURE);
+}
+
+static void isc_set_pipeline(struct isc_device *isc, u32 pipeline)
+{
+	u32 val;
+	unsigned int i;
+
+	for (i = 0; i < ISC_PIPE_LINE_NODE_NUM; i++) {
+		val = pipeline & BIT(i) ? 1 : 0;
+		regmap_field_write(isc->pipeline[i], val);
+	}
+}
+
+static int isc_configure(struct isc_device *isc)
+{
+	struct regmap *regmap = isc->regmap;
+	const struct isc_format *current_fmt = isc->current_fmt;
+	struct isc_subdev_entity *subdev = isc->current_subdev;
+	u32 val, mask;
+	int counter = 10;
+
+	val = current_fmt->reg_bps | subdev->pfe_cfg0 |
+	      ISC_PFE_CFG0_MODE_PROGRESSIVE;
+	mask = ISC_PFE_CFG0_BPS_MASK | ISC_PFE_CFG0_HPOL_LOW |
+	       ISC_PFE_CFG0_VPOL_LOW | ISC_PFE_CFG0_PPOL_LOW |
+	       ISC_PFE_CFG0_MODE_MASK;
+
+	regmap_update_bits(regmap, ISC_PFE_CFG0, mask, val);
+
+	regmap_update_bits(regmap, ISC_RLP_CFG, ISC_RLP_CFG_MODE_MASK,
+			   current_fmt->reg_rlp_mode);
+
+	regmap_update_bits(regmap, ISC_DCFG, ISC_DCFG_IMODE_MASK,
+			   current_fmt->reg_dcfg_imode);
+
+	/* Disable the pipeline */
+	isc_set_pipeline(isc, 0x0);
+
+	/* Update profile */
+	regmap_write(regmap, ISC_CTRLEN, ISC_CTRL_UPPRO);
+
+	regmap_read(regmap, ISC_CTRLSR, &val);
+	while ((val & ISC_CTRL_UPPRO) && counter--) {
+		usleep_range(1000, 2000);
+		regmap_read(regmap, ISC_CTRLSR, &val);
+	}
+
+	if (counter < 0)
+		return -ETIMEDOUT;
+
+	return 0;
+}
+
+static int isc_start_streaming(struct vb2_queue *vq, unsigned int count)
+{
+	struct isc_device *isc = vb2_get_drv_priv(vq);
+	struct regmap *regmap = isc->regmap;
+	struct isc_buffer *buf;
+	unsigned long flags;
+	int ret;
+	u32 val;
+
+	/* Enable stream on the sub device */
+	ret = v4l2_subdev_call(isc->current_subdev->sd, video, s_stream, 1);
+	if (ret && ret != -ENOIOCTLCMD) {
+		v4l2_err(&isc->v4l2_dev, "stream on failed in subdev\n");
+		goto err_start_stream;
+	}
+
+	pm_runtime_get_sync(isc->dev);
+
+	/* Disable all the interrupts */
+	regmap_write(isc->regmap, ISC_INTDIS, (u32)~0UL);
+
+	/* Clean the interrupt status register */
+	regmap_read(regmap, ISC_INTSR, &val);
+
+	ret = isc_configure(isc);
+	if (unlikely(ret))
+		goto err_configure;
+
+	/* Enable DMA interrupt */
+	regmap_write(regmap, ISC_INTEN, ISC_INT_DDONE);
+
+	spin_lock_irqsave(&isc->dma_queue_lock, flags);
+
+	isc->sequence = 0;
+	isc->stop = false;
+	reinit_completion(&isc->comp);
+
+	isc->cur_frm = list_first_entry(&isc->dma_queue,
+					struct isc_buffer, list);
+	list_del(&isc->cur_frm->list);
+
+	isc_start_dma(regmap, isc->cur_frm, isc->current_fmt->reg_dctrl_dview);
+
+	spin_unlock_irqrestore(&isc->dma_queue_lock, flags);
+
+	return 0;
+
+err_configure:
+	pm_runtime_put_sync(isc->dev);
+
+	v4l2_subdev_call(isc->current_subdev->sd, video, s_stream, 0);
+
+err_start_stream:
+	spin_lock_irqsave(&isc->dma_queue_lock, flags);
+	list_for_each_entry(buf, &isc->dma_queue, list)
+		vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_QUEUED);
+	INIT_LIST_HEAD(&isc->dma_queue);
+	spin_unlock_irqrestore(&isc->dma_queue_lock, flags);
+
+	return ret;
+}
+
+static void isc_stop_streaming(struct vb2_queue *vq)
+{
+	struct isc_device *isc = vb2_get_drv_priv(vq);
+	unsigned long flags;
+	struct isc_buffer *buf;
+	int ret;
+
+	isc->stop = true;
+
+	/* Wait until the end of the current frame */
+	if (isc->cur_frm && !wait_for_completion_timeout(&isc->comp, 5 * HZ))
+		v4l2_err(&isc->v4l2_dev,
+			 "Timeout waiting for end of the capture\n");
+
+	/* Disable DMA interrupt */
+	regmap_write(isc->regmap, ISC_INTDIS, ISC_INT_DDONE);
+
+	pm_runtime_put_sync(isc->dev);
+
+	/* Disable stream on the sub device */
+	ret = v4l2_subdev_call(isc->current_subdev->sd, video, s_stream, 0);
+	if (ret && ret != -ENOIOCTLCMD)
+		v4l2_err(&isc->v4l2_dev, "stream off failed in subdev\n");
+
+	/* Release all active buffers */
+	spin_lock_irqsave(&isc->dma_queue_lock, flags);
+	if (unlikely(isc->cur_frm)) {
+		vb2_buffer_done(&isc->cur_frm->vb.vb2_buf,
+				VB2_BUF_STATE_ERROR);
+		isc->cur_frm = NULL;
+	}
+	list_for_each_entry(buf, &isc->dma_queue, list)
+		vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
+	INIT_LIST_HEAD(&isc->dma_queue);
+	spin_unlock_irqrestore(&isc->dma_queue_lock, flags);
+}
+
+static void isc_buffer_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct isc_buffer *buf = container_of(vbuf, struct isc_buffer, vb);
+	struct isc_device *isc = vb2_get_drv_priv(vb->vb2_queue);
+	unsigned long flags;
+
+	spin_lock_irqsave(&isc->dma_queue_lock, flags);
+	list_add_tail(&buf->list, &isc->dma_queue);
+	spin_unlock_irqrestore(&isc->dma_queue_lock, flags);
+}
+
+static struct vb2_ops isc_vb2_ops = {
+	.queue_setup		= isc_queue_setup,
+	.wait_prepare		= vb2_ops_wait_prepare,
+	.wait_finish		= vb2_ops_wait_finish,
+	.buf_prepare		= isc_buffer_prepare,
+	.start_streaming	= isc_start_streaming,
+	.stop_streaming		= isc_stop_streaming,
+	.buf_queue		= isc_buffer_queue,
+};
+
+static int isc_querycap(struct file *file, void *priv,
+			 struct v4l2_capability *cap)
+{
+	struct isc_device *isc = video_drvdata(file);
+
+	strcpy(cap->driver, ATMEL_ISC_NAME);
+	strcpy(cap->card, "Atmel Image Sensor Controller");
+	snprintf(cap->bus_info, sizeof(cap->bus_info),
+		 "platform:%s", isc->v4l2_dev.name);
+
+	return 0;
+}
+
+static int isc_enum_fmt_vid_cap(struct file *file, void *priv,
+				 struct v4l2_fmtdesc *f)
+{
+	struct isc_device *isc = video_drvdata(file);
+	u32 index = f->index;
+
+	if (index >= isc->num_user_formats)
+		return -EINVAL;
+
+	f->pixelformat = isc->user_formats[index]->fourcc;
+
+	return 0;
+}
+
+static int isc_g_fmt_vid_cap(struct file *file, void *priv,
+			      struct v4l2_format *fmt)
+{
+	struct isc_device *isc = video_drvdata(file);
+
+	*fmt = isc->fmt;
+
+	return 0;
+}
+
+static struct isc_format *find_format_by_fourcc(struct isc_device *isc,
+						 unsigned int fourcc)
+{
+	unsigned int num_formats = isc->num_user_formats;
+	struct isc_format *fmt;
+	unsigned int i;
+
+	for (i = 0; i < num_formats; i++) {
+		fmt = isc->user_formats[i];
+		if (fmt->fourcc == fourcc)
+			return fmt;
+	}
+
+	return NULL;
+}
+
+static int isc_try_fmt(struct isc_device *isc, struct v4l2_format *f,
+			struct isc_format **current_fmt)
+{
+	struct isc_format *isc_fmt;
+	struct v4l2_pix_format *pixfmt = &f->fmt.pix;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_TRY,
+	};
+	int ret;
+
+	if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	isc_fmt = find_format_by_fourcc(isc, pixfmt->pixelformat);
+	if (!isc_fmt) {
+		v4l2_warn(&isc->v4l2_dev, "Format 0x%x not found\n",
+			  pixfmt->pixelformat);
+		isc_fmt = isc->user_formats[isc->num_user_formats - 1];
+		pixfmt->pixelformat = isc_fmt->fourcc;
+	}
+
+	/* Limit to Atmel ISC hardware capabilities */
+	if (pixfmt->width > ISC_MAX_SUPPORT_WIDTH)
+		pixfmt->width = ISC_MAX_SUPPORT_WIDTH;
+	if (pixfmt->height > ISC_MAX_SUPPORT_HEIGHT)
+		pixfmt->height = ISC_MAX_SUPPORT_HEIGHT;
+
+	v4l2_fill_mbus_format(&format.format, pixfmt, isc_fmt->mbus_code);
+	ret = v4l2_subdev_call(isc->current_subdev->sd, pad, set_fmt,
+			       isc->current_subdev->config, &format);
+	if (ret < 0)
+		return ret;
+
+	v4l2_fill_pix_format(pixfmt, &format.format);
+
+	pixfmt->field = V4L2_FIELD_NONE;
+	pixfmt->bytesperline = pixfmt->width * isc_fmt->bpp;
+	pixfmt->sizeimage = pixfmt->bytesperline * pixfmt->height;
+
+	if (current_fmt)
+		*current_fmt = isc_fmt;
+
+	return 0;
+}
+
+static int isc_set_fmt(struct isc_device *isc, struct v4l2_format *f)
+{
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	struct isc_format *current_fmt;
+	int ret;
+
+	ret = isc_try_fmt(isc, f, &current_fmt);
+	if (ret)
+		return ret;
+
+	v4l2_fill_mbus_format(&format.format, &f->fmt.pix,
+			      current_fmt->mbus_code);
+	ret = v4l2_subdev_call(isc->current_subdev->sd, pad,
+			       set_fmt, NULL, &format);
+	if (ret < 0)
+		return ret;
+
+	isc->fmt = *f;
+	isc->current_fmt = current_fmt;
+
+	return 0;
+}
+
+static int isc_s_fmt_vid_cap(struct file *file, void *priv,
+			      struct v4l2_format *f)
+{
+	struct isc_device *isc = video_drvdata(file);
+
+	if (vb2_is_streaming(&isc->vb2_vidq))
+		return -EBUSY;
+
+	return isc_set_fmt(isc, f);
+}
+
+static int isc_try_fmt_vid_cap(struct file *file, void *priv,
+				struct v4l2_format *f)
+{
+	struct isc_device *isc = video_drvdata(file);
+
+	return isc_try_fmt(isc, f, NULL);
+}
+
+static int isc_enum_input(struct file *file, void *priv,
+			   struct v4l2_input *inp)
+{
+	if (inp->index != 0)
+		return -EINVAL;
+
+	inp->type = V4L2_INPUT_TYPE_CAMERA;
+	inp->std = 0;
+	strcpy(inp->name, "Camera");
+
+	return 0;
+}
+
+static int isc_g_input(struct file *file, void *priv, unsigned int *i)
+{
+	*i = 0;
+
+	return 0;
+}
+
+static int isc_s_input(struct file *file, void *priv, unsigned int i)
+{
+	if (i > 0)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int isc_g_parm(struct file *file, void *fh, struct v4l2_streamparm *a)
+{
+	struct isc_device *isc = video_drvdata(file);
+
+	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	return v4l2_subdev_call(isc->current_subdev->sd, video, g_parm, a);
+}
+
+static int isc_s_parm(struct file *file, void *fh, struct v4l2_streamparm *a)
+{
+	struct isc_device *isc = video_drvdata(file);
+
+	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	return v4l2_subdev_call(isc->current_subdev->sd, video, s_parm, a);
+}
+
+static int isc_enum_framesizes(struct file *file, void *fh,
+			       struct v4l2_frmsizeenum *fsize)
+{
+	struct isc_device *isc = video_drvdata(file);
+	const struct isc_format *isc_fmt;
+	struct v4l2_subdev_frame_size_enum fse = {
+		.index = fsize->index,
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	int ret;
+
+	isc_fmt = find_format_by_fourcc(isc, fsize->pixel_format);
+	if (!isc_fmt)
+		return -EINVAL;
+
+	fse.code = isc_fmt->mbus_code;
+
+	ret = v4l2_subdev_call(isc->current_subdev->sd, pad, enum_frame_size,
+			       NULL, &fse);
+	if (ret)
+		return ret;
+
+	fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
+	fsize->discrete.width = fse.max_width;
+	fsize->discrete.height = fse.max_height;
+
+	return 0;
+}
+
+static int isc_enum_frameintervals(struct file *file, void *fh,
+				    struct v4l2_frmivalenum *fival)
+{
+	struct isc_device *isc = video_drvdata(file);
+	const struct isc_format *isc_fmt;
+	struct v4l2_subdev_frame_interval_enum fie = {
+		.index = fival->index,
+		.width = fival->width,
+		.height = fival->height,
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	int ret;
+
+	isc_fmt = find_format_by_fourcc(isc, fival->pixel_format);
+	if (!isc_fmt)
+		return -EINVAL;
+
+	fie.code = isc_fmt->mbus_code;
+
+	ret = v4l2_subdev_call(isc->current_subdev->sd, pad,
+			       enum_frame_interval, NULL, &fie);
+	if (ret)
+		return ret;
+
+	fival->type = V4L2_FRMIVAL_TYPE_DISCRETE;
+	fival->discrete = fie.interval;
+
+	return 0;
+}
+
+static const struct v4l2_ioctl_ops isc_ioctl_ops = {
+	.vidioc_querycap		= isc_querycap,
+	.vidioc_enum_fmt_vid_cap	= isc_enum_fmt_vid_cap,
+	.vidioc_g_fmt_vid_cap		= isc_g_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap		= isc_s_fmt_vid_cap,
+	.vidioc_try_fmt_vid_cap		= isc_try_fmt_vid_cap,
+
+	.vidioc_enum_input		= isc_enum_input,
+	.vidioc_g_input			= isc_g_input,
+	.vidioc_s_input			= isc_s_input,
+
+	.vidioc_reqbufs			= vb2_ioctl_reqbufs,
+	.vidioc_querybuf		= vb2_ioctl_querybuf,
+	.vidioc_qbuf			= vb2_ioctl_qbuf,
+	.vidioc_expbuf			= vb2_ioctl_expbuf,
+	.vidioc_dqbuf			= vb2_ioctl_dqbuf,
+	.vidioc_create_bufs		= vb2_ioctl_create_bufs,
+	.vidioc_prepare_buf		= vb2_ioctl_prepare_buf,
+	.vidioc_streamon		= vb2_ioctl_streamon,
+	.vidioc_streamoff		= vb2_ioctl_streamoff,
+
+	.vidioc_g_parm			= isc_g_parm,
+	.vidioc_s_parm			= isc_s_parm,
+	.vidioc_enum_framesizes		= isc_enum_framesizes,
+	.vidioc_enum_frameintervals	= isc_enum_frameintervals,
+};
+
+static int isc_open(struct file *file)
+{
+	struct isc_device *isc = video_drvdata(file);
+	struct v4l2_subdev *sd = isc->current_subdev->sd;
+	int ret;
+
+	if (mutex_lock_interruptible(&isc->lock))
+		return -ERESTARTSYS;
+
+	ret = v4l2_fh_open(file);
+	if (ret < 0)
+		goto unlock;
+
+	if (!v4l2_fh_is_singular_file(file))
+		goto unlock;
+
+	ret = v4l2_subdev_call(sd, core, s_power, 1);
+	if (ret < 0 && ret != -ENOIOCTLCMD) {
+		v4l2_fh_release(file);
+		goto unlock;
+	}
+
+	ret = isc_set_fmt(isc, &isc->fmt);
+	if (ret) {
+		v4l2_subdev_call(sd, core, s_power, 0);
+		v4l2_fh_release(file);
+	}
+
+unlock:
+	mutex_unlock(&isc->lock);
+	return ret;
+}
+
+static int isc_release(struct file *file)
+{
+	struct isc_device *isc = video_drvdata(file);
+	struct v4l2_subdev *sd = isc->current_subdev->sd;
+	bool fh_singular;
+	int ret;
+
+	mutex_lock(&isc->lock);
+
+	fh_singular = v4l2_fh_is_singular_file(file);
+
+	ret = _vb2_fop_release(file, NULL);
+
+	if (fh_singular)
+		v4l2_subdev_call(sd, core, s_power, 0);
+
+	mutex_unlock(&isc->lock);
+
+	return ret;
+}
+
+static const struct v4l2_file_operations isc_fops = {
+	.owner		= THIS_MODULE,
+	.open		= isc_open,
+	.release	= isc_release,
+	.unlocked_ioctl	= video_ioctl2,
+	.read		= vb2_fop_read,
+	.mmap		= vb2_fop_mmap,
+	.poll		= vb2_fop_poll,
+};
+
+static irqreturn_t isc_interrupt(int irq, void *dev_id)
+{
+	struct isc_device *isc = (struct isc_device *)dev_id;
+	struct regmap *regmap = isc->regmap;
+	u32 isc_intsr, isc_intmask, pending;
+	irqreturn_t ret = IRQ_NONE;
+
+	spin_lock(&isc->dma_queue_lock);
+
+	regmap_read(regmap, ISC_INTSR, &isc_intsr);
+	regmap_read(regmap, ISC_INTMASK, &isc_intmask);
+
+	pending = isc_intsr & isc_intmask;
+
+	if (likely(pending & ISC_INT_DDONE)) {
+		if (isc->cur_frm) {
+			struct vb2_v4l2_buffer *vbuf = &isc->cur_frm->vb;
+			struct vb2_buffer *vb = &vbuf->vb2_buf;
+
+			vb->timestamp = ktime_get_ns();
+			vbuf->sequence = isc->sequence++;
+			vb2_buffer_done(vb, VB2_BUF_STATE_DONE);
+			isc->cur_frm = NULL;
+		}
+
+		if (!list_empty(&isc->dma_queue) && !isc->stop) {
+			isc->cur_frm = list_first_entry(&isc->dma_queue,
+						     struct isc_buffer, list);
+			list_del(&isc->cur_frm->list);
+
+			isc_start_dma(regmap, isc->cur_frm,
+				      isc->current_fmt->reg_dctrl_dview);
+		}
+
+		if (isc->stop)
+			complete(&isc->comp);
+
+		ret = IRQ_HANDLED;
+	}
+
+	spin_unlock(&isc->dma_queue_lock);
+
+	return ret;
+}
+
+static int isc_async_bound(struct v4l2_async_notifier *notifier,
+			    struct v4l2_subdev *subdev,
+			    struct v4l2_async_subdev *asd)
+{
+	struct isc_device *isc = container_of(notifier->v4l2_dev,
+					      struct isc_device, v4l2_dev);
+	struct isc_subdev_entity *subdev_entity =
+		container_of(notifier, struct isc_subdev_entity, notifier);
+
+	if (video_is_registered(&isc->video_dev)) {
+		v4l2_err(&isc->v4l2_dev, "only supports one sub-device.\n");
+		return -EBUSY;
+	}
+
+	subdev_entity->sd = subdev;
+
+	return 0;
+}
+
+static void isc_async_unbind(struct v4l2_async_notifier *notifier,
+			      struct v4l2_subdev *subdev,
+			      struct v4l2_async_subdev *asd)
+{
+	struct isc_device *isc = container_of(notifier->v4l2_dev,
+					      struct isc_device, v4l2_dev);
+
+	video_unregister_device(&isc->video_dev);
+	if (isc->current_subdev->config)
+		v4l2_subdev_free_pad_config(isc->current_subdev->config);
+}
+
+static struct isc_format *find_format_by_code(unsigned int code, int *index)
+{
+	struct isc_format *fmt = &isc_formats[0];
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(isc_formats); i++) {
+		if (fmt->mbus_code == code) {
+			*index = i;
+			return fmt;
+		}
+
+		fmt++;
+	}
+
+	return NULL;
+}
+
+static int isc_formats_init(struct isc_device *isc)
+{
+	struct isc_format *fmt;
+	struct v4l2_subdev *subdev = isc->current_subdev->sd;
+	int num_fmts = 0, i, j;
+	struct v4l2_subdev_mbus_code_enum mbus_code = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+
+	fmt = &isc_formats[0];
+	for (i = 0; i < ARRAY_SIZE(isc_formats); i++) {
+		fmt->support = false;
+		fmt++;
+	}
+
+	while (!v4l2_subdev_call(subdev, pad, enum_mbus_code,
+	       NULL, &mbus_code)) {
+		mbus_code.index++;
+		fmt = find_format_by_code(mbus_code.code, &i);
+		if (!fmt)
+			continue;
+
+		fmt->support = true;
+		num_fmts++;
+	}
+
+	if (!num_fmts)
+		return -ENXIO;
+
+	isc->num_user_formats = num_fmts;
+	isc->user_formats = devm_kcalloc(isc->dev,
+					 num_fmts, sizeof(struct isc_format *),
+					 GFP_KERNEL);
+	if (!isc->user_formats) {
+		v4l2_err(&isc->v4l2_dev, "could not allocate memory\n");
+		return -ENOMEM;
+	}
+
+	fmt = &isc_formats[0];
+	for (i = 0, j = 0; i < ARRAY_SIZE(isc_formats); i++) {
+		if (fmt->support)
+			isc->user_formats[j++] = fmt;
+
+		fmt++;
+	}
+
+	return 0;
+}
+
+static int isc_set_default_fmt(struct isc_device *isc)
+{
+	struct v4l2_format f = {
+		.type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
+		.fmt.pix = {
+			.width		= VGA_WIDTH,
+			.height		= VGA_HEIGHT,
+			.field		= V4L2_FIELD_NONE,
+			.pixelformat	= isc->user_formats[0]->fourcc,
+		},
+	};
+	int ret;
+
+	ret = isc_try_fmt(isc, &f, NULL);
+	if (ret)
+		return ret;
+
+	isc->current_fmt = isc->user_formats[0];
+	isc->fmt = f;
+
+	return 0;
+}
+
+static int isc_async_complete(struct v4l2_async_notifier *notifier)
+{
+	struct isc_device *isc = container_of(notifier->v4l2_dev,
+					      struct isc_device, v4l2_dev);
+	struct isc_subdev_entity *sd_entity;
+	struct video_device *vdev = &isc->video_dev;
+	struct vb2_queue *q = &isc->vb2_vidq;
+	int ret;
+
+	isc->current_subdev = container_of(notifier,
+					   struct isc_subdev_entity, notifier);
+	sd_entity = isc->current_subdev;
+
+	mutex_init(&isc->lock);
+	init_completion(&isc->comp);
+
+	/* Initialize videobuf2 queue */
+	q->type			= V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	q->io_modes		= VB2_MMAP | VB2_DMABUF | VB2_READ;
+	q->drv_priv		= isc;
+	q->buf_struct_size	= sizeof(struct isc_buffer);
+	q->ops			= &isc_vb2_ops;
+	q->mem_ops		= &vb2_dma_contig_memops;
+	q->timestamp_flags	= V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	q->lock			= &isc->lock;
+	q->min_buffers_needed	= 1;
+	q->dev			= isc->dev;
+
+	ret = vb2_queue_init(q);
+	if (ret < 0) {
+		v4l2_err(&isc->v4l2_dev,
+			 "vb2_queue_init() failed: %d\n", ret);
+		return ret;
+	}
+
+	/* Init video dma queues */
+	INIT_LIST_HEAD(&isc->dma_queue);
+	spin_lock_init(&isc->dma_queue_lock);
+
+	sd_entity->config = v4l2_subdev_alloc_pad_config(sd_entity->sd);
+	if (sd_entity->config == NULL)
+		return -ENOMEM;
+
+	ret = isc_formats_init(isc);
+	if (ret < 0) {
+		v4l2_err(&isc->v4l2_dev,
+			 "Init format failed: %d\n", ret);
+		return ret;
+	}
+
+	ret = isc_set_default_fmt(isc);
+	if (ret) {
+		v4l2_err(&isc->v4l2_dev, "Could not set default format\n");
+		return ret;
+	}
+
+	/* Register video device */
+	strlcpy(vdev->name, ATMEL_ISC_NAME, sizeof(vdev->name));
+	vdev->release		= video_device_release_empty;
+	vdev->fops		= &isc_fops;
+	vdev->ioctl_ops		= &isc_ioctl_ops;
+	vdev->v4l2_dev		= &isc->v4l2_dev;
+	vdev->vfl_dir		= VFL_DIR_RX;
+	vdev->queue		= q;
+	vdev->lock		= &isc->lock;
+	vdev->ctrl_handler	= isc->current_subdev->sd->ctrl_handler;
+	vdev->device_caps	= V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_CAPTURE;
+	video_set_drvdata(vdev, isc);
+
+	ret = video_register_device(vdev, VFL_TYPE_GRABBER, -1);
+	if (ret < 0) {
+		v4l2_err(&isc->v4l2_dev,
+			 "video_register_device failed: %d\n", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static void isc_subdev_cleanup(struct isc_device *isc)
+{
+	struct isc_subdev_entity *subdev_entity;
+
+	list_for_each_entry(subdev_entity, &isc->subdev_entities, list)
+		v4l2_async_notifier_unregister(&subdev_entity->notifier);
+
+	INIT_LIST_HEAD(&isc->subdev_entities);
+}
+
+static int isc_pipeline_init(struct isc_device *isc)
+{
+	struct device *dev = isc->dev;
+	struct regmap *regmap = isc->regmap;
+	struct regmap_field *regs;
+	unsigned int i;
+
+	/* WB-->CFA-->CC-->GAM-->CSC-->CBC-->SUB422-->SUB420 */
+	const struct reg_field regfields[ISC_PIPE_LINE_NODE_NUM] = {
+		REG_FIELD(ISC_WB_CTRL, 0, 0),
+		REG_FIELD(ISC_CFA_CTRL, 0, 0),
+		REG_FIELD(ISC_CC_CTRL, 0, 0),
+		REG_FIELD(ISC_GAM_CTRL, 0, 0),
+		REG_FIELD(ISC_GAM_CTRL, 1, 1),
+		REG_FIELD(ISC_GAM_CTRL, 2, 2),
+		REG_FIELD(ISC_GAM_CTRL, 3, 3),
+		REG_FIELD(ISC_CSC_CTRL, 0, 0),
+		REG_FIELD(ISC_CBC_CTRL, 0, 0),
+		REG_FIELD(ISC_SUB422_CTRL, 0, 0),
+		REG_FIELD(ISC_SUB420_CTRL, 0, 0),
+	};
+
+	for (i = 0; i < ISC_PIPE_LINE_NODE_NUM; i++) {
+		regs = devm_regmap_field_alloc(dev, regmap, regfields[i]);
+		if (IS_ERR(regs))
+			return PTR_ERR(regs);
+
+		isc->pipeline[i] =  regs;
+	}
+
+	return 0;
+}
+
+static int isc_parse_dt(struct device *dev, struct isc_device *isc)
+{
+	struct device_node *np = dev->of_node;
+	struct device_node *epn = NULL, *rem;
+	struct v4l2_of_endpoint v4l2_epn;
+	struct isc_subdev_entity *subdev_entity;
+	unsigned int flags;
+	int ret;
+
+	INIT_LIST_HEAD(&isc->subdev_entities);
+
+	for (; ;) {
+		epn = of_graph_get_next_endpoint(np, epn);
+		if (!epn)
+			break;
+
+		rem = of_graph_get_remote_port_parent(epn);
+		if (!rem) {
+			dev_notice(dev, "Remote device at %s not found\n",
+				   of_node_full_name(epn));
+			continue;
+		}
+
+		ret = v4l2_of_parse_endpoint(epn, &v4l2_epn);
+		if (ret) {
+			of_node_put(rem);
+			ret = -EINVAL;
+			dev_err(dev, "Could not parse the endpoint\n");
+			break;
+		}
+
+		subdev_entity = devm_kzalloc(dev,
+					  sizeof(*subdev_entity), GFP_KERNEL);
+		if (subdev_entity == NULL) {
+			of_node_put(rem);
+			ret = -ENOMEM;
+			break;
+		}
+
+		subdev_entity->asd = devm_kzalloc(dev,
+				     sizeof(*subdev_entity->asd), GFP_KERNEL);
+		if (subdev_entity->asd == NULL) {
+			of_node_put(rem);
+			ret = -ENOMEM;
+			break;
+		}
+
+		flags = v4l2_epn.bus.parallel.flags;
+
+		if (flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)
+			subdev_entity->pfe_cfg0 = ISC_PFE_CFG0_HPOL_LOW;
+
+		if (flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)
+			subdev_entity->pfe_cfg0 |= ISC_PFE_CFG0_VPOL_LOW;
+
+		if (flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)
+			subdev_entity->pfe_cfg0 |= ISC_PFE_CFG0_PPOL_LOW;
+
+		subdev_entity->asd->match_type = V4L2_ASYNC_MATCH_OF;
+		subdev_entity->asd->match.of.node = rem;
+		list_add_tail(&subdev_entity->list, &isc->subdev_entities);
+	}
+
+	of_node_put(epn);
+	return ret;
+}
+
+/* regmap configuration */
+#define ATMEL_ISC_REG_MAX    0xbfc
+static const struct regmap_config isc_regmap_config = {
+	.reg_bits       = 32,
+	.reg_stride     = 4,
+	.val_bits       = 32,
+	.max_register	= ATMEL_ISC_REG_MAX,
+};
+
+static int atmel_isc_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct isc_device *isc;
+	struct resource *res;
+	void __iomem *io_base;
+	struct isc_subdev_entity *subdev_entity;
+	int irq;
+	int ret;
+
+	isc = devm_kzalloc(dev, sizeof(*isc), GFP_KERNEL);
+	if (!isc)
+		return -ENOMEM;
+
+	platform_set_drvdata(pdev, isc);
+	isc->dev = dev;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	io_base = devm_ioremap_resource(dev, res);
+	if (IS_ERR(io_base))
+		return PTR_ERR(io_base);
+
+	isc->regmap = devm_regmap_init_mmio(dev, io_base, &isc_regmap_config);
+	if (IS_ERR(isc->regmap)) {
+		ret = PTR_ERR(isc->regmap);
+		dev_err(dev, "failed to init register map: %d\n", ret);
+		return ret;
+	}
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0) {
+		ret = irq;
+		dev_err(dev, "failed to get irq: %d\n", ret);
+		return ret;
+	}
+
+	ret = devm_request_irq(dev, irq, isc_interrupt, 0,
+			       ATMEL_ISC_NAME, isc);
+	if (ret < 0) {
+		dev_err(dev, "can't register ISR for IRQ %u (ret=%i)\n",
+			irq, ret);
+		return ret;
+	}
+
+	ret = isc_pipeline_init(isc);
+	if (ret)
+		return ret;
+
+	isc->hclock = devm_clk_get(dev, "hclock");
+	if (IS_ERR(isc->hclock)) {
+		ret = PTR_ERR(isc->hclock);
+		dev_err(dev, "failed to get hclock: %d\n", ret);
+		return ret;
+	}
+
+	ret = isc_clk_init(isc);
+	if (ret) {
+		dev_err(dev, "failed to init isc clock: %d\n", ret);
+		goto clean_isc_clk;
+	}
+
+	isc->ispck = isc->isc_clks[ISC_ISPCK].clk;
+
+	/* ispck should be greater or equal to hclock */
+	ret = clk_set_rate(isc->ispck, clk_get_rate(isc->hclock));
+	if (ret) {
+		dev_err(dev, "failed to set ispck rate: %d\n", ret);
+		goto clean_isc_clk;
+	}
+
+	ret = v4l2_device_register(dev, &isc->v4l2_dev);
+	if (ret) {
+		dev_err(dev, "unable to register v4l2 device.\n");
+		goto clean_isc_clk;
+	}
+
+	ret = isc_parse_dt(dev, isc);
+	if (ret) {
+		dev_err(dev, "fail to parse device tree\n");
+		goto unregister_v4l2_device;
+	}
+
+	if (list_empty(&isc->subdev_entities)) {
+		dev_err(dev, "no subdev found\n");
+		goto unregister_v4l2_device;
+	}
+
+	list_for_each_entry(subdev_entity, &isc->subdev_entities, list) {
+		subdev_entity->notifier.subdevs = &subdev_entity->asd;
+		subdev_entity->notifier.num_subdevs = 1;
+		subdev_entity->notifier.bound = isc_async_bound;
+		subdev_entity->notifier.unbind = isc_async_unbind;
+		subdev_entity->notifier.complete = isc_async_complete;
+
+		ret = v4l2_async_notifier_register(&isc->v4l2_dev,
+						   &subdev_entity->notifier);
+		if (ret) {
+			dev_err(dev, "fail to register async notifier\n");
+			goto cleanup_subdev;
+		}
+
+		if (video_is_registered(&isc->video_dev))
+			break;
+	}
+
+	pm_runtime_enable(dev);
+
+	return 0;
+
+cleanup_subdev:
+	isc_subdev_cleanup(isc);
+
+unregister_v4l2_device:
+	v4l2_device_unregister(&isc->v4l2_dev);
+
+clean_isc_clk:
+	isc_clk_cleanup(isc);
+
+	return ret;
+}
+
+static int atmel_isc_remove(struct platform_device *pdev)
+{
+	struct isc_device *isc = platform_get_drvdata(pdev);
+
+	pm_runtime_disable(&pdev->dev);
+
+	isc_subdev_cleanup(isc);
+
+	v4l2_device_unregister(&isc->v4l2_dev);
+
+	isc_clk_cleanup(isc);
+
+	return 0;
+}
+
+static int __maybe_unused isc_runtime_suspend(struct device *dev)
+{
+	struct isc_device *isc = dev_get_drvdata(dev);
+
+	clk_disable_unprepare(isc->ispck);
+	clk_disable_unprepare(isc->hclock);
+
+	return 0;
+}
+
+static int __maybe_unused isc_runtime_resume(struct device *dev)
+{
+	struct isc_device *isc = dev_get_drvdata(dev);
+	int ret;
+
+	ret = clk_prepare_enable(isc->hclock);
+	if (ret)
+		return ret;
+
+	return clk_prepare_enable(isc->ispck);
+}
+
+static const struct dev_pm_ops atmel_isc_dev_pm_ops = {
+	SET_RUNTIME_PM_OPS(isc_runtime_suspend, isc_runtime_resume, NULL)
+};
+
+static const struct of_device_id atmel_isc_of_match[] = {
+	{ .compatible = "atmel,sama5d2-isc" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, atmel_isc_of_match);
+
+static struct platform_driver atmel_isc_driver = {
+	.probe	= atmel_isc_probe,
+	.remove	= atmel_isc_remove,
+	.driver	= {
+		.name		= ATMEL_ISC_NAME,
+		.pm		= &atmel_isc_dev_pm_ops,
+		.of_match_table = of_match_ptr(atmel_isc_of_match),
+	},
+};
+
+module_platform_driver(atmel_isc_driver);
+
+MODULE_AUTHOR("Songjun Wu <songjun.wu@microchip.com>");
+MODULE_DESCRIPTION("The V4L2 driver for Atmel-ISC");
+MODULE_LICENSE("GPL v2");
+MODULE_SUPPORTED_DEVICE("video");
diff --git a/drivers/media/platform/davinci/vpbe_display.c b/drivers/media/platform/davinci/vpbe_display.c
index 0b1709e96673..a9bc0175e4d3 100644
--- a/drivers/media/platform/davinci/vpbe_display.c
+++ b/drivers/media/platform/davinci/vpbe_display.c
@@ -440,7 +440,7 @@ vpbe_disp_calculate_scale_factor(struct vpbe_display *disp_dev,
 	/*
 	 * Application initially set the image format. Current display
 	 * size is obtained from the vpbe display controller. expected_xsize
-	 * and expected_ysize are set through S_CROP ioctl. Based on this,
+	 * and expected_ysize are set through S_SELECTION ioctl. Based on this,
 	 * driver will calculate the scale factors for vertical and
 	 * horizontal direction so that the image is displayed scaled
 	 * and expanded. Application uses expansion to display the image
@@ -649,24 +649,23 @@ static int vpbe_display_querycap(struct file *file, void  *priv,
 	return 0;
 }
 
-static int vpbe_display_s_crop(struct file *file, void *priv,
-			     const struct v4l2_crop *crop)
+static int vpbe_display_s_selection(struct file *file, void *priv,
+			     struct v4l2_selection *sel)
 {
 	struct vpbe_layer *layer = video_drvdata(file);
 	struct vpbe_display *disp_dev = layer->disp_dev;
 	struct vpbe_device *vpbe_dev = disp_dev->vpbe_dev;
 	struct osd_layer_config *cfg = &layer->layer_info.config;
 	struct osd_state *osd_device = disp_dev->osd_device;
-	struct v4l2_rect rect = crop->c;
+	struct v4l2_rect rect = sel->r;
 	int ret;
 
 	v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev,
-		"VIDIOC_S_CROP, layer id = %d\n", layer->device_id);
+		"VIDIOC_S_SELECTION, layer id = %d\n", layer->device_id);
 
-	if (crop->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) {
-		v4l2_err(&vpbe_dev->v4l2_dev, "Invalid buf type\n");
+	if (sel->type != V4L2_BUF_TYPE_VIDEO_OUTPUT ||
+	    sel->target != V4L2_SEL_TGT_CROP)
 		return -EINVAL;
-	}
 
 	if (rect.top < 0)
 		rect.top = 0;
@@ -714,32 +713,45 @@ static int vpbe_display_s_crop(struct file *file, void *priv,
 	else
 		osd_device->ops.set_interpolation_filter(osd_device, 0);
 
+	sel->r = rect;
 	return 0;
 }
 
-static int vpbe_display_g_crop(struct file *file, void *priv,
-			     struct v4l2_crop *crop)
+static int vpbe_display_g_selection(struct file *file, void *priv,
+				    struct v4l2_selection *sel)
 {
 	struct vpbe_layer *layer = video_drvdata(file);
 	struct osd_layer_config *cfg = &layer->layer_info.config;
 	struct vpbe_device *vpbe_dev = layer->disp_dev->vpbe_dev;
 	struct osd_state *osd_device = layer->disp_dev->osd_device;
-	struct v4l2_rect *rect = &crop->c;
+	struct v4l2_rect *rect = &sel->r;
 
 	v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev,
-			"VIDIOC_G_CROP, layer id = %d\n",
+			"VIDIOC_G_SELECTION, layer id = %d\n",
 			layer->device_id);
 
-	if (crop->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) {
-		v4l2_err(&vpbe_dev->v4l2_dev, "Invalid buf type\n");
+	if (sel->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
+		return -EINVAL;
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP:
+		osd_device->ops.get_layer_config(osd_device,
+						 layer->layer_info.id, cfg);
+		rect->top = cfg->ypos;
+		rect->left = cfg->xpos;
+		rect->width = cfg->xsize;
+		rect->height = cfg->ysize;
+		break;
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		rect->left = 0;
+		rect->top = 0;
+		rect->width = vpbe_dev->current_timings.xres;
+		rect->height = vpbe_dev->current_timings.yres;
+		break;
+	default:
 		return -EINVAL;
 	}
-	osd_device->ops.get_layer_config(osd_device,
-				layer->layer_info.id, cfg);
-	rect->top = cfg->ypos;
-	rect->left = cfg->xpos;
-	rect->width = cfg->xsize;
-	rect->height = cfg->ysize;
 
 	return 0;
 }
@@ -752,13 +764,10 @@ static int vpbe_display_cropcap(struct file *file, void *priv,
 
 	v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev, "VIDIOC_CROPCAP ioctl\n");
 
-	cropcap->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
-	cropcap->bounds.left = 0;
-	cropcap->bounds.top = 0;
-	cropcap->bounds.width = vpbe_dev->current_timings.xres;
-	cropcap->bounds.height = vpbe_dev->current_timings.yres;
+	if (cropcap->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
+		return -EINVAL;
+
 	cropcap->pixelaspect = vpbe_dev->current_timings.aspect;
-	cropcap->defrect = cropcap->bounds;
 	return 0;
 }
 
@@ -1251,8 +1260,8 @@ static const struct v4l2_ioctl_ops vpbe_ioctl_ops = {
 	.vidioc_expbuf		 = vb2_ioctl_expbuf,
 
 	.vidioc_cropcap		 = vpbe_display_cropcap,
-	.vidioc_g_crop		 = vpbe_display_g_crop,
-	.vidioc_s_crop		 = vpbe_display_s_crop,
+	.vidioc_g_selection	 = vpbe_display_g_selection,
+	.vidioc_s_selection	 = vpbe_display_s_selection,
 
 	.vidioc_s_std		 = vpbe_display_s_std,
 	.vidioc_g_std		 = vpbe_display_g_std,
diff --git a/drivers/media/platform/davinci/vpfe_capture.c b/drivers/media/platform/davinci/vpfe_capture.c
index 7767e072d623..6efb2f1631c4 100644
--- a/drivers/media/platform/davinci/vpfe_capture.c
+++ b/drivers/media/platform/davinci/vpfe_capture.c
@@ -1610,38 +1610,53 @@ static int vpfe_cropcap(struct file *file, void *priv,
 
 	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_cropcap\n");
 
-	if (vpfe_dev->std_index >= ARRAY_SIZE(vpfe_standards))
+	if (crop->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 		return -EINVAL;
+	/* If std_index is invalid, then just return (== 1:1 aspect) */
+	if (vpfe_dev->std_index >= ARRAY_SIZE(vpfe_standards))
+		return 0;
 
-	memset(crop, 0, sizeof(struct v4l2_cropcap));
-	crop->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-	crop->bounds.width = crop->defrect.width =
-		vpfe_standards[vpfe_dev->std_index].width;
-	crop->bounds.height = crop->defrect.height =
-		vpfe_standards[vpfe_dev->std_index].height;
 	crop->pixelaspect = vpfe_standards[vpfe_dev->std_index].pixelaspect;
 	return 0;
 }
 
-static int vpfe_g_crop(struct file *file, void *priv,
-			     struct v4l2_crop *crop)
+static int vpfe_g_selection(struct file *file, void *priv,
+			    struct v4l2_selection *sel)
 {
 	struct vpfe_device *vpfe_dev = video_drvdata(file);
 
-	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_g_crop\n");
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_g_selection\n");
 
-	crop->c = vpfe_dev->crop;
+	if (sel->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP:
+		sel->r = vpfe_dev->crop;
+		break;
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		sel->r.width = vpfe_standards[vpfe_dev->std_index].width;
+		sel->r.height = vpfe_standards[vpfe_dev->std_index].height;
+		break;
+	default:
+		return -EINVAL;
+	}
 	return 0;
 }
 
-static int vpfe_s_crop(struct file *file, void *priv,
-			     const struct v4l2_crop *crop)
+static int vpfe_s_selection(struct file *file, void *priv,
+			    struct v4l2_selection *sel)
 {
 	struct vpfe_device *vpfe_dev = video_drvdata(file);
-	struct v4l2_rect rect = crop->c;
+	struct v4l2_rect rect = sel->r;
 	int ret = 0;
 
-	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_s_crop\n");
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_s_selection\n");
+
+	if (sel->type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
+	    sel->target != V4L2_SEL_TGT_CROP)
+		return -EINVAL;
 
 	if (vpfe_dev->started) {
 		/* make sure streaming is not started */
@@ -1669,7 +1684,7 @@ static int vpfe_s_crop(struct file *file, void *priv,
 		vpfe_dev->std_info.active_pixels) ||
 	    (rect.top + rect.height >
 		vpfe_dev->std_info.active_lines)) {
-		v4l2_err(&vpfe_dev->v4l2_dev, "Error in S_CROP params\n");
+		v4l2_err(&vpfe_dev->v4l2_dev, "Error in S_SELECTION params\n");
 		ret = -EINVAL;
 		goto unlock_out;
 	}
@@ -1682,6 +1697,7 @@ static int vpfe_s_crop(struct file *file, void *priv,
 		vpfe_dev->fmt.fmt.pix.bytesperline *
 		vpfe_dev->fmt.fmt.pix.height;
 	vpfe_dev->crop = rect;
+	sel->r = rect;
 unlock_out:
 	mutex_unlock(&vpfe_dev->lock);
 	return ret;
@@ -1760,8 +1776,8 @@ static const struct v4l2_ioctl_ops vpfe_ioctl_ops = {
 	.vidioc_streamon	 = vpfe_streamon,
 	.vidioc_streamoff	 = vpfe_streamoff,
 	.vidioc_cropcap		 = vpfe_cropcap,
-	.vidioc_g_crop		 = vpfe_g_crop,
-	.vidioc_s_crop		 = vpfe_s_crop,
+	.vidioc_g_selection	 = vpfe_g_selection,
+	.vidioc_s_selection	 = vpfe_s_selection,
 	.vidioc_default		 = vpfe_param_handler,
 };
 
diff --git a/drivers/media/platform/exynos-gsc/gsc-m2m.c b/drivers/media/platform/exynos-gsc/gsc-m2m.c
index ec6494cbdd45..9f03b791b711 100644
--- a/drivers/media/platform/exynos-gsc/gsc-m2m.c
+++ b/drivers/media/platform/exynos-gsc/gsc-m2m.c
@@ -261,7 +261,7 @@ static void gsc_m2m_buf_queue(struct vb2_buffer *vb)
 		v4l2_m2m_buf_queue(ctx->m2m_ctx, vbuf);
 }
 
-static struct vb2_ops gsc_m2m_qops = {
+static const struct vb2_ops gsc_m2m_qops = {
 	.queue_setup	 = gsc_m2m_queue_setup,
 	.buf_prepare	 = gsc_m2m_buf_prepare,
 	.buf_queue	 = gsc_m2m_buf_queue,
@@ -277,9 +277,10 @@ static int gsc_m2m_querycap(struct file *file, void *fh,
 	struct gsc_ctx *ctx = fh_to_ctx(fh);
 	struct gsc_dev *gsc = ctx->gsc_dev;
 
-	strlcpy(cap->driver, gsc->pdev->name, sizeof(cap->driver));
-	strlcpy(cap->card, gsc->pdev->name, sizeof(cap->card));
-	strlcpy(cap->bus_info, "platform", sizeof(cap->bus_info));
+	strlcpy(cap->driver, GSC_MODULE_NAME, sizeof(cap->driver));
+	strlcpy(cap->card, GSC_MODULE_NAME " gscaler", sizeof(cap->card));
+	snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s",
+		 dev_name(&gsc->pdev->dev));
 	cap->device_caps = V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_M2M_MPLANE |
 		V4L2_CAP_VIDEO_CAPTURE_MPLANE |	V4L2_CAP_VIDEO_OUTPUT_MPLANE;
 
diff --git a/drivers/media/platform/exynos4-is/fimc-capture.c b/drivers/media/platform/exynos4-is/fimc-capture.c
index fdec499fbbda..964f4a681934 100644
--- a/drivers/media/platform/exynos4-is/fimc-capture.c
+++ b/drivers/media/platform/exynos4-is/fimc-capture.c
@@ -452,7 +452,7 @@ static void buffer_queue(struct vb2_buffer *vb)
 	spin_unlock_irqrestore(&fimc->slock, flags);
 }
 
-static struct vb2_ops fimc_capture_qops = {
+static const struct vb2_ops fimc_capture_qops = {
 	.queue_setup		= queue_setup,
 	.buf_prepare		= buffer_prepare,
 	.buf_queue		= buffer_queue,
@@ -1796,6 +1796,7 @@ static int fimc_register_capture_device(struct fimc_dev *fimc,
 	vid_cap->wb_fmt.code = fmt->mbus_code;
 
 	vid_cap->vd_pad.flags = MEDIA_PAD_FL_SINK;
+	vfd->entity.function = MEDIA_ENT_F_PROC_VIDEO_SCALER;
 	ret = media_entity_pads_init(&vfd->entity, 1, &vid_cap->vd_pad);
 	if (ret)
 		goto err_free_ctx;
diff --git a/drivers/media/platform/exynos4-is/fimc-is-i2c.c b/drivers/media/platform/exynos4-is/fimc-is-i2c.c
index 7521aa59b064..6bba4ca022be 100644
--- a/drivers/media/platform/exynos4-is/fimc-is-i2c.c
+++ b/drivers/media/platform/exynos4-is/fimc-is-i2c.c
@@ -55,26 +55,33 @@ static int fimc_is_i2c_probe(struct platform_device *pdev)
 	i2c_adap->algo = &fimc_is_i2c_algorithm;
 	i2c_adap->class = I2C_CLASS_SPD;
 
-	ret = i2c_add_adapter(i2c_adap);
-	if (ret < 0) {
-		dev_err(&pdev->dev, "failed to add I2C bus %s\n",
-						node->full_name);
-		return ret;
-	}
-
 	platform_set_drvdata(pdev, isp_i2c);
-
 	pm_runtime_enable(&pdev->dev);
-	pm_runtime_enable(&i2c_adap->dev);
 
+	ret = i2c_add_adapter(i2c_adap);
+	if (ret < 0)
+		goto err_pm_dis;
+	/*
+	 * Client drivers of this adapter don't do any I2C transfers as that
+	 * is handled by the ISP firmware.  But we rely on the runtime PM
+	 * state propagation from the clients up to the adapter driver so
+	 * clear the ignore_children flags here.  PM rutnime calls are not
+	 * used in probe() handler of clients of this adapter so there is
+	 * no issues with clearing the flag right after registering the I2C
+	 * adapter.
+	 */
+	pm_suspend_ignore_children(&i2c_adap->dev, false);
 	return 0;
+
+err_pm_dis:
+	pm_runtime_disable(&pdev->dev);
+	return ret;
 }
 
 static int fimc_is_i2c_remove(struct platform_device *pdev)
 {
 	struct fimc_is_i2c *isp_i2c = platform_get_drvdata(pdev);
 
-	pm_runtime_disable(&isp_i2c->adapter.dev);
 	pm_runtime_disable(&pdev->dev);
 	i2c_del_adapter(&isp_i2c->adapter);
 
diff --git a/drivers/media/platform/exynos4-is/fimc-is.c b/drivers/media/platform/exynos4-is/fimc-is.c
index 32ca55f16677..518ad34f80d7 100644
--- a/drivers/media/platform/exynos4-is/fimc-is.c
+++ b/drivers/media/platform/exynos4-is/fimc-is.c
@@ -52,6 +52,9 @@ static char *fimc_is_clocks[ISS_CLKS_MAX] = {
 	[ISS_CLK_DRC]			= "drc",
 	[ISS_CLK_FD]			= "fd",
 	[ISS_CLK_MCUISP]		= "mcuisp",
+	[ISS_CLK_GICISP]		= "gicisp",
+	[ISS_CLK_PWM_ISP]		= "pwm_isp",
+	[ISS_CLK_MCUCTL_ISP]		= "mcuctl_isp",
 	[ISS_CLK_UART]			= "uart",
 	[ISS_CLK_ISP_DIV0]		= "ispdiv0",
 	[ISS_CLK_ISP_DIV1]		= "ispdiv1",
@@ -165,6 +168,7 @@ static int fimc_is_parse_sensor_config(struct fimc_is *is, unsigned int index,
 						struct device_node *node)
 {
 	struct fimc_is_sensor *sensor = &is->sensor[index];
+	struct device_node *ep, *port;
 	u32 tmp = 0;
 	int ret;
 
@@ -175,22 +179,25 @@ static int fimc_is_parse_sensor_config(struct fimc_is *is, unsigned int index,
 		return -EINVAL;
 	}
 
-	node = of_graph_get_next_endpoint(node, NULL);
-	if (!node)
+	ep = of_graph_get_next_endpoint(node, NULL);
+	if (!ep)
 		return -ENXIO;
 
-	node = of_graph_get_remote_port(node);
-	if (!node)
+	port = of_graph_get_remote_port(ep);
+	of_node_put(ep);
+	if (!port)
 		return -ENXIO;
 
 	/* Use MIPI-CSIS channel id to determine the ISP I2C bus index. */
-	ret = of_property_read_u32(node, "reg", &tmp);
+	ret = of_property_read_u32(port, "reg", &tmp);
 	if (ret < 0) {
 		dev_err(&is->pdev->dev, "reg property not found at: %s\n",
-							 node->full_name);
+							 port->full_name);
+		of_node_put(port);
 		return ret;
 	}
 
+	of_node_put(port);
 	sensor->i2c_bus = tmp - FIMC_INPUT_MIPI_CSI2_0;
 	return 0;
 }
@@ -845,13 +852,18 @@ static int fimc_is_probe(struct platform_device *pdev)
 		goto err_pm;
 
 	vb2_dma_contig_set_max_seg_size(dev, DMA_BIT_MASK(32));
+
+	ret = of_platform_populate(dev->of_node, NULL, NULL, dev);
+	if (ret < 0)
+		goto err_pm;
+
 	/*
 	 * Register FIMC-IS V4L2 subdevs to this driver. The video nodes
 	 * will be created within the subdev's registered() callback.
 	 */
 	ret = fimc_is_register_subdevs(is);
 	if (ret < 0)
-		goto err_pm;
+		goto err_of_dep;
 
 	ret = fimc_is_debugfs_create(is);
 	if (ret < 0)
@@ -870,6 +882,8 @@ err_dfs:
 	fimc_is_debugfs_remove(is);
 err_sd:
 	fimc_is_unregister_subdevs(is);
+err_of_dep:
+	of_platform_depopulate(dev);
 err_pm:
 	if (!pm_runtime_enabled(dev))
 		fimc_is_runtime_suspend(dev);
@@ -929,6 +943,7 @@ static int fimc_is_remove(struct platform_device *pdev)
 	if (!pm_runtime_status_suspended(dev))
 		fimc_is_runtime_suspend(dev);
 	free_irq(is->irq, is);
+	of_platform_depopulate(dev);
 	fimc_is_unregister_subdevs(is);
 	vb2_dma_contig_clear_max_seg_size(dev);
 	fimc_is_put_clocks(is);
diff --git a/drivers/media/platform/exynos4-is/fimc-is.h b/drivers/media/platform/exynos4-is/fimc-is.h
index 3a82c6a214c7..ee05da034aa1 100644
--- a/drivers/media/platform/exynos4-is/fimc-is.h
+++ b/drivers/media/platform/exynos4-is/fimc-is.h
@@ -77,6 +77,9 @@ enum {
 	ISS_CLK_DRC,
 	ISS_CLK_FD,
 	ISS_CLK_MCUISP,
+	ISS_CLK_GICISP,
+	ISS_CLK_PWM_ISP,
+	ISS_CLK_MCUCTL_ISP,
 	ISS_CLK_UART,
 	ISS_GATE_CLKS_MAX,
 	ISS_CLK_ISP_DIV0 = ISS_GATE_CLKS_MAX,
diff --git a/drivers/media/platform/exynos4-is/fimc-isp.c b/drivers/media/platform/exynos4-is/fimc-isp.c
index 293b807020c4..8efe9160ab34 100644
--- a/drivers/media/platform/exynos4-is/fimc-isp.c
+++ b/drivers/media/platform/exynos4-is/fimc-isp.c
@@ -705,6 +705,7 @@ int fimc_isp_subdev_create(struct fimc_isp *isp)
 	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
 	snprintf(sd->name, sizeof(sd->name), "FIMC-IS-ISP");
 
+	sd->entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;
 	isp->subdev_pads[FIMC_ISP_SD_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
 	isp->subdev_pads[FIMC_ISP_SD_PAD_SRC_FIFO].flags = MEDIA_PAD_FL_SOURCE;
 	isp->subdev_pads[FIMC_ISP_SD_PAD_SRC_DMA].flags = MEDIA_PAD_FL_SOURCE;
diff --git a/drivers/media/platform/exynos4-is/fimc-lite.c b/drivers/media/platform/exynos4-is/fimc-lite.c
index a0f149fb88e1..b91abf1c4d43 100644
--- a/drivers/media/platform/exynos4-is/fimc-lite.c
+++ b/drivers/media/platform/exynos4-is/fimc-lite.c
@@ -1432,6 +1432,7 @@ static int fimc_lite_create_capture_subdev(struct fimc_lite *fimc)
 
 	sd->ctrl_handler = handler;
 	sd->internal_ops = &fimc_lite_subdev_internal_ops;
+	sd->entity.function = MEDIA_ENT_F_PROC_VIDEO_SCALER;
 	sd->entity.ops = &fimc_lite_subdev_media_ops;
 	sd->owner = THIS_MODULE;
 	v4l2_set_subdevdata(sd, fimc);
@@ -1454,25 +1455,17 @@ static void fimc_lite_clk_put(struct fimc_lite *fimc)
 	if (IS_ERR(fimc->clock))
 		return;
 
-	clk_unprepare(fimc->clock);
 	clk_put(fimc->clock);
 	fimc->clock = ERR_PTR(-EINVAL);
 }
 
 static int fimc_lite_clk_get(struct fimc_lite *fimc)
 {
-	int ret;
-
 	fimc->clock = clk_get(&fimc->pdev->dev, FLITE_CLK_NAME);
 	if (IS_ERR(fimc->clock))
 		return PTR_ERR(fimc->clock);
 
-	ret = clk_prepare(fimc->clock);
-	if (ret < 0) {
-		clk_put(fimc->clock);
-		fimc->clock = ERR_PTR(-EINVAL);
-	}
-	return ret;
+	return 0;
 }
 
 static const struct of_device_id flite_of_match[];
@@ -1543,7 +1536,7 @@ static int fimc_lite_probe(struct platform_device *pdev)
 	pm_runtime_enable(dev);
 
 	if (!pm_runtime_enabled(dev)) {
-		ret = clk_enable(fimc->clock);
+		ret = clk_prepare_enable(fimc->clock);
 		if (ret < 0)
 			goto err_sd;
 	}
@@ -1568,7 +1561,7 @@ static int fimc_lite_runtime_resume(struct device *dev)
 {
 	struct fimc_lite *fimc = dev_get_drvdata(dev);
 
-	clk_enable(fimc->clock);
+	clk_prepare_enable(fimc->clock);
 	return 0;
 }
 
@@ -1576,7 +1569,7 @@ static int fimc_lite_runtime_suspend(struct device *dev)
 {
 	struct fimc_lite *fimc = dev_get_drvdata(dev);
 
-	clk_disable(fimc->clock);
+	clk_disable_unprepare(fimc->clock);
 	return 0;
 }
 #endif
diff --git a/drivers/media/platform/exynos4-is/fimc-m2m.c b/drivers/media/platform/exynos4-is/fimc-m2m.c
index b1309e114edb..6028e4fbaed3 100644
--- a/drivers/media/platform/exynos4-is/fimc-m2m.c
+++ b/drivers/media/platform/exynos4-is/fimc-m2m.c
@@ -219,7 +219,7 @@ static void fimc_buf_queue(struct vb2_buffer *vb)
 	v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vbuf);
 }
 
-static struct vb2_ops fimc_qops = {
+static const struct vb2_ops fimc_qops = {
 	.queue_setup	 = fimc_queue_setup,
 	.buf_prepare	 = fimc_buf_prepare,
 	.buf_queue	 = fimc_buf_queue,
diff --git a/drivers/media/platform/exynos4-is/media-dev.c b/drivers/media/platform/exynos4-is/media-dev.c
index 891625e77ef5..1a1154a9dfa4 100644
--- a/drivers/media/platform/exynos4-is/media-dev.c
+++ b/drivers/media/platform/exynos4-is/media-dev.c
@@ -1190,6 +1190,10 @@ static int fimc_md_link_notify(struct media_link *link, unsigned int flags,
 	return ret ? -EPIPE : 0;
 }
 
+static const struct media_device_ops fimc_md_ops = {
+	.link_notify = fimc_md_link_notify,
+};
+
 static ssize_t fimc_md_sysfs_show(struct device *dev,
 				  struct device_attribute *attr, char *buf)
 {
@@ -1416,7 +1420,7 @@ static int fimc_md_probe(struct platform_device *pdev)
 
 	strlcpy(fmd->media_dev.model, "SAMSUNG S5P FIMC",
 		sizeof(fmd->media_dev.model));
-	fmd->media_dev.link_notify = fimc_md_link_notify;
+	fmd->media_dev.ops = &fimc_md_ops;
 	fmd->media_dev.dev = dev;
 
 	v4l2_dev = &fmd->v4l2_dev;
diff --git a/drivers/media/platform/exynos4-is/mipi-csis.c b/drivers/media/platform/exynos4-is/mipi-csis.c
index 86e681daa89d..befd9fc0adc4 100644
--- a/drivers/media/platform/exynos4-is/mipi-csis.c
+++ b/drivers/media/platform/exynos4-is/mipi-csis.c
@@ -853,6 +853,7 @@ static int s5pcsis_probe(struct platform_device *pdev)
 	state->format.width = S5PCSIS_DEF_PIX_WIDTH;
 	state->format.height = S5PCSIS_DEF_PIX_HEIGHT;
 
+	state->sd.entity.function = MEDIA_ENT_F_IO_V4L;
 	state->pads[CSIS_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
 	state->pads[CSIS_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
 	ret = media_entity_pads_init(&state->sd.entity,
diff --git a/drivers/media/platform/m2m-deinterlace.c b/drivers/media/platform/m2m-deinterlace.c
index 0fcb5c78031d..bedc7cc4c7d6 100644
--- a/drivers/media/platform/m2m-deinterlace.c
+++ b/drivers/media/platform/m2m-deinterlace.c
@@ -852,7 +852,7 @@ static void deinterlace_buf_queue(struct vb2_buffer *vb)
 	v4l2_m2m_buf_queue(ctx->m2m_ctx, vbuf);
 }
 
-static struct vb2_ops deinterlace_qops = {
+static const struct vb2_ops deinterlace_qops = {
 	.queue_setup	 = deinterlace_queue_setup,
 	.buf_prepare	 = deinterlace_buf_prepare,
 	.buf_queue	 = deinterlace_buf_queue,
@@ -1016,7 +1016,7 @@ static int deinterlace_probe(struct platform_device *pdev)
 		return -ENODEV;
 
 	if (!dma_has_cap(DMA_INTERLEAVE, pcdev->dma_chan->device->cap_mask)) {
-		v4l2_err(&pcdev->v4l2_dev, "DMA does not support INTERLEAVE\n");
+		dev_err(&pdev->dev, "DMA does not support INTERLEAVE\n");
 		goto rel_dma;
 	}
 
diff --git a/drivers/media/platform/mtk-vcodec/mtk_vcodec_drv.h b/drivers/media/platform/mtk-vcodec/mtk_vcodec_drv.h
index 3a8e6958adae..c8eaa41c00e6 100644
--- a/drivers/media/platform/mtk-vcodec/mtk_vcodec_drv.h
+++ b/drivers/media/platform/mtk-vcodec/mtk_vcodec_drv.h
@@ -239,7 +239,7 @@ struct mtk_vcodec_ctx {
 	enum mtk_encode_param param_change;
 	struct mtk_enc_params enc_params;
 
-	struct venc_common_if *enc_if;
+	const struct venc_common_if *enc_if;
 	unsigned long drv_handle;
 
 	int int_cond;
diff --git a/drivers/media/platform/mtk-vcodec/mtk_vcodec_enc.c b/drivers/media/platform/mtk-vcodec/mtk_vcodec_enc.c
index 2c5719ac23b2..1b1a28abbf1f 100644
--- a/drivers/media/platform/mtk-vcodec/mtk_vcodec_enc.c
+++ b/drivers/media/platform/mtk-vcodec/mtk_vcodec_enc.c
@@ -243,6 +243,8 @@ static int vidioc_venc_s_parm(struct file *file, void *priv,
 			a->parm.output.timeperframe.numerator;
 	ctx->param_change |= MTK_ENCODE_PARAM_FRAMERATE;
 
+	a->parm.output.capability = V4L2_CAP_TIMEPERFRAME;
+
 	return 0;
 }
 
@@ -254,6 +256,7 @@ static int vidioc_venc_g_parm(struct file *file, void *priv,
 	if (a->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
 		return -EINVAL;
 
+	a->parm.output.capability = V4L2_CAP_TIMEPERFRAME;
 	a->parm.output.timeperframe.denominator =
 			ctx->enc_params.framerate_num;
 	a->parm.output.timeperframe.numerator =
@@ -621,6 +624,69 @@ static int vidioc_try_fmt_vid_out_mplane(struct file *file, void *priv,
 	return vidioc_try_fmt(f, fmt);
 }
 
+static int vidioc_venc_g_selection(struct file *file, void *priv,
+				     struct v4l2_selection *s)
+{
+	struct mtk_vcodec_ctx *ctx = fh_to_ctx(priv);
+	struct mtk_q_data *q_data;
+
+	if (s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
+		return -EINVAL;
+
+	q_data = mtk_venc_get_q_data(ctx, s->type);
+	if (!q_data)
+		return -EINVAL;
+
+	switch (s->target) {
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		s->r.top = 0;
+		s->r.left = 0;
+		s->r.width = q_data->coded_width;
+		s->r.height = q_data->coded_height;
+		break;
+	case V4L2_SEL_TGT_CROP:
+		s->r.top = 0;
+		s->r.left = 0;
+		s->r.width = q_data->visible_width;
+		s->r.height = q_data->visible_height;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int vidioc_venc_s_selection(struct file *file, void *priv,
+				     struct v4l2_selection *s)
+{
+	struct mtk_vcodec_ctx *ctx = fh_to_ctx(priv);
+	struct mtk_q_data *q_data;
+
+	if (s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
+		return -EINVAL;
+
+	q_data = mtk_venc_get_q_data(ctx, s->type);
+	if (!q_data)
+		return -EINVAL;
+
+	switch (s->target) {
+	case V4L2_SEL_TGT_CROP:
+		/* Only support crop from (0,0) */
+		s->r.top = 0;
+		s->r.left = 0;
+		s->r.width = min(s->r.width, q_data->coded_width);
+		s->r.height = min(s->r.height, q_data->coded_height);
+		q_data->visible_width = s->r.width;
+		q_data->visible_height = s->r.height;
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
 static int vidioc_venc_qbuf(struct file *file, void *priv,
 			    struct v4l2_buffer *buf)
 {
@@ -679,6 +745,9 @@ const struct v4l2_ioctl_ops mtk_venc_ioctl_ops = {
 
 	.vidioc_create_bufs		= v4l2_m2m_ioctl_create_bufs,
 	.vidioc_prepare_buf		= v4l2_m2m_ioctl_prepare_buf,
+
+	.vidioc_g_selection		= vidioc_venc_g_selection,
+	.vidioc_s_selection		= vidioc_venc_s_selection,
 };
 
 static int vb2ops_venc_queue_setup(struct vb2_queue *vq,
@@ -854,7 +923,7 @@ static void vb2ops_venc_stop_streaming(struct vb2_queue *q)
 	ctx->state = MTK_STATE_FREE;
 }
 
-static struct vb2_ops mtk_venc_vb2_ops = {
+static const struct vb2_ops mtk_venc_vb2_ops = {
 	.queue_setup		= vb2ops_venc_queue_setup,
 	.buf_prepare		= vb2ops_venc_buf_prepare,
 	.buf_queue		= vb2ops_venc_buf_queue,
diff --git a/drivers/media/platform/mtk-vcodec/venc/venc_h264_if.c b/drivers/media/platform/mtk-vcodec/venc/venc_h264_if.c
index 63d4be4ff327..b76c80bdf30b 100644
--- a/drivers/media/platform/mtk-vcodec/venc/venc_h264_if.c
+++ b/drivers/media/platform/mtk-vcodec/venc/venc_h264_if.c
@@ -664,16 +664,16 @@ static int h264_enc_deinit(unsigned long handle)
 	return ret;
 }
 
-static struct venc_common_if venc_h264_if = {
+static const struct venc_common_if venc_h264_if = {
 	h264_enc_init,
 	h264_enc_encode,
 	h264_enc_set_param,
 	h264_enc_deinit,
 };
 
-struct venc_common_if *get_h264_enc_comm_if(void);
+const struct venc_common_if *get_h264_enc_comm_if(void);
 
-struct venc_common_if *get_h264_enc_comm_if(void)
+const struct venc_common_if *get_h264_enc_comm_if(void)
 {
 	return &venc_h264_if;
 }
diff --git a/drivers/media/platform/mtk-vcodec/venc/venc_vp8_if.c b/drivers/media/platform/mtk-vcodec/venc/venc_vp8_if.c
index 6d9758479f9a..544f57186243 100644
--- a/drivers/media/platform/mtk-vcodec/venc/venc_vp8_if.c
+++ b/drivers/media/platform/mtk-vcodec/venc/venc_vp8_if.c
@@ -469,16 +469,16 @@ static int vp8_enc_deinit(unsigned long handle)
 	return ret;
 }
 
-static struct venc_common_if venc_vp8_if = {
+static const struct venc_common_if venc_vp8_if = {
 	vp8_enc_init,
 	vp8_enc_encode,
 	vp8_enc_set_param,
 	vp8_enc_deinit,
 };
 
-struct venc_common_if *get_vp8_enc_comm_if(void);
+const struct venc_common_if *get_vp8_enc_comm_if(void);
 
-struct venc_common_if *get_vp8_enc_comm_if(void)
+const struct venc_common_if *get_vp8_enc_comm_if(void)
 {
 	return &venc_vp8_if;
 }
diff --git a/drivers/media/platform/mtk-vcodec/venc_drv_if.c b/drivers/media/platform/mtk-vcodec/venc_drv_if.c
index c4c83e7189c3..d02d5f1df279 100644
--- a/drivers/media/platform/mtk-vcodec/venc_drv_if.c
+++ b/drivers/media/platform/mtk-vcodec/venc_drv_if.c
@@ -26,8 +26,8 @@
 #include "mtk_vcodec_enc_pm.h"
 #include "mtk_vpu.h"
 
-struct venc_common_if *get_h264_enc_comm_if(void);
-struct venc_common_if *get_vp8_enc_comm_if(void);
+const struct venc_common_if *get_h264_enc_comm_if(void);
+const struct venc_common_if *get_vp8_enc_comm_if(void);
 
 int venc_if_init(struct mtk_vcodec_ctx *ctx, unsigned int fourcc)
 {
diff --git a/drivers/media/platform/mx2_emmaprp.c b/drivers/media/platform/mx2_emmaprp.c
index c639406fe72e..e68d271b10af 100644
--- a/drivers/media/platform/mx2_emmaprp.c
+++ b/drivers/media/platform/mx2_emmaprp.c
@@ -743,7 +743,7 @@ static void emmaprp_buf_queue(struct vb2_buffer *vb)
 	v4l2_m2m_buf_queue(ctx->m2m_ctx, vbuf);
 }
 
-static struct vb2_ops emmaprp_qops = {
+static const struct vb2_ops emmaprp_qops = {
 	.queue_setup	 = emmaprp_queue_setup,
 	.buf_prepare	 = emmaprp_buf_prepare,
 	.buf_queue	 = emmaprp_buf_queue,
diff --git a/drivers/media/platform/omap/omap_vout.c b/drivers/media/platform/omap/omap_vout.c
index 6b01e126fe73..a31b95cb3b09 100644
--- a/drivers/media/platform/omap/omap_vout.c
+++ b/drivers/media/platform/omap/omap_vout.c
@@ -214,7 +214,7 @@ static int omap_vout_get_userptr(struct videobuf_buffer *vb, u32 virtp,
 	if (!vec)
 		return -ENOMEM;
 
-	ret = get_vaddr_frames(virtp, 1, true, false, vec);
+	ret = get_vaddr_frames(virtp, 1, FOLL_WRITE, vec);
 	if (ret != 1) {
 		frame_vector_destroy(vec);
 		return -EINVAL;
@@ -1247,36 +1247,33 @@ static int vidioc_g_fmt_vid_overlay(struct file *file, void *fh,
 	return 0;
 }
 
-static int vidioc_cropcap(struct file *file, void *fh,
-		struct v4l2_cropcap *cropcap)
+static int vidioc_g_selection(struct file *file, void *fh, struct v4l2_selection *sel)
 {
 	struct omap_vout_device *vout = fh;
 	struct v4l2_pix_format *pix = &vout->pix;
 
-	if (cropcap->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
+	if (sel->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
 		return -EINVAL;
 
-	/* Width and height are always even */
-	cropcap->bounds.width = pix->width & ~1;
-	cropcap->bounds.height = pix->height & ~1;
-
-	omap_vout_default_crop(&vout->pix, &vout->fbuf, &cropcap->defrect);
-	cropcap->pixelaspect.numerator = 1;
-	cropcap->pixelaspect.denominator = 1;
-	return 0;
-}
-
-static int vidioc_g_crop(struct file *file, void *fh, struct v4l2_crop *crop)
-{
-	struct omap_vout_device *vout = fh;
-
-	if (crop->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP:
+		sel->r = vout->crop;
+		break;
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+		omap_vout_default_crop(&vout->pix, &vout->fbuf, &sel->r);
+		break;
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		/* Width and height are always even */
+		sel->r.width = pix->width & ~1;
+		sel->r.height = pix->height & ~1;
+		break;
+	default:
 		return -EINVAL;
-	crop->c = vout->crop;
+	}
 	return 0;
 }
 
-static int vidioc_s_crop(struct file *file, void *fh, const struct v4l2_crop *crop)
+static int vidioc_s_selection(struct file *file, void *fh, struct v4l2_selection *sel)
 {
 	int ret = -EINVAL;
 	struct omap_vout_device *vout = fh;
@@ -1285,6 +1282,12 @@ static int vidioc_s_crop(struct file *file, void *fh, const struct v4l2_crop *cr
 	struct omap_video_timings *timing;
 	struct omap_dss_device *dssdev;
 
+	if (sel->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
+		return -EINVAL;
+
+	if (sel->target != V4L2_SEL_TGT_CROP)
+		return -EINVAL;
+
 	if (vout->streaming)
 		return -EBUSY;
 
@@ -1309,9 +1312,8 @@ static int vidioc_s_crop(struct file *file, void *fh, const struct v4l2_crop *cr
 		vout->fbuf.fmt.width = timing->x_res;
 	}
 
-	if (crop->type == V4L2_BUF_TYPE_VIDEO_OUTPUT)
-		ret = omap_vout_new_crop(&vout->pix, &vout->crop, &vout->win,
-				&vout->fbuf, &crop->c);
+	ret = omap_vout_new_crop(&vout->pix, &vout->crop, &vout->win,
+				 &vout->fbuf, &sel->r);
 
 s_crop_err:
 	mutex_unlock(&vout->lock);
@@ -1780,9 +1782,8 @@ static const struct v4l2_ioctl_ops vout_ioctl_ops = {
 	.vidioc_try_fmt_vid_out_overlay		= vidioc_try_fmt_vid_overlay,
 	.vidioc_s_fmt_vid_out_overlay		= vidioc_s_fmt_vid_overlay,
 	.vidioc_g_fmt_vid_out_overlay		= vidioc_g_fmt_vid_overlay,
-	.vidioc_cropcap				= vidioc_cropcap,
-	.vidioc_g_crop				= vidioc_g_crop,
-	.vidioc_s_crop				= vidioc_s_crop,
+	.vidioc_g_selection			= vidioc_g_selection,
+	.vidioc_s_selection			= vidioc_s_selection,
 	.vidioc_reqbufs				= vidioc_reqbufs,
 	.vidioc_querybuf			= vidioc_querybuf,
 	.vidioc_qbuf				= vidioc_qbuf,
diff --git a/drivers/media/platform/omap3isp/isp.c b/drivers/media/platform/omap3isp/isp.c
index 5d54e2c6c16b..0321d84addc7 100644
--- a/drivers/media/platform/omap3isp/isp.c
+++ b/drivers/media/platform/omap3isp/isp.c
@@ -657,6 +657,10 @@ static irqreturn_t isp_isr(int irq, void *_isp)
 	return IRQ_HANDLED;
 }
 
+static const struct media_device_ops isp_media_ops = {
+	.link_notify = v4l2_pipeline_link_notify,
+};
+
 /* -----------------------------------------------------------------------------
  * Pipeline stream management
  */
@@ -1680,7 +1684,7 @@ static int isp_register_entities(struct isp_device *isp)
 	strlcpy(isp->media_dev.model, "TI OMAP3 ISP",
 		sizeof(isp->media_dev.model));
 	isp->media_dev.hw_revision = isp->revision;
-	isp->media_dev.link_notify = v4l2_pipeline_link_notify;
+	isp->media_dev.ops = &isp_media_ops;
 	media_device_init(&isp->media_dev);
 
 	isp->v4l2_dev.mdev = &isp->media_dev;
diff --git a/drivers/media/platform/omap3isp/ispvideo.c b/drivers/media/platform/omap3isp/ispvideo.c
index 7d9f35976d18..7354469670b7 100644
--- a/drivers/media/platform/omap3isp/ispvideo.c
+++ b/drivers/media/platform/omap3isp/ispvideo.c
@@ -772,40 +772,45 @@ isp_video_try_format(struct file *file, void *fh, struct v4l2_format *format)
 }
 
 static int
-isp_video_cropcap(struct file *file, void *fh, struct v4l2_cropcap *cropcap)
-{
-	struct isp_video *video = video_drvdata(file);
-	struct v4l2_subdev *subdev;
-	int ret;
-
-	subdev = isp_video_remote_subdev(video, NULL);
-	if (subdev == NULL)
-		return -EINVAL;
-
-	mutex_lock(&video->mutex);
-	ret = v4l2_subdev_call(subdev, video, cropcap, cropcap);
-	mutex_unlock(&video->mutex);
-
-	return ret == -ENOIOCTLCMD ? -ENOTTY : ret;
-}
-
-static int
-isp_video_get_crop(struct file *file, void *fh, struct v4l2_crop *crop)
+isp_video_get_selection(struct file *file, void *fh, struct v4l2_selection *sel)
 {
 	struct isp_video *video = video_drvdata(file);
 	struct v4l2_subdev_format format;
 	struct v4l2_subdev *subdev;
+	struct v4l2_subdev_selection sdsel = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		.target = sel->target,
+	};
 	u32 pad;
 	int ret;
 
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+		if (video->type == V4L2_BUF_TYPE_VIDEO_OUTPUT)
+			return -EINVAL;
+		break;
+	case V4L2_SEL_TGT_COMPOSE:
+	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+	case V4L2_SEL_TGT_COMPOSE_DEFAULT:
+		if (video->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+			return -EINVAL;
+		break;
+	default:
+		return -EINVAL;
+	}
 	subdev = isp_video_remote_subdev(video, &pad);
 	if (subdev == NULL)
 		return -EINVAL;
 
-	/* Try the get crop operation first and fallback to get format if not
+	/* Try the get selection operation first and fallback to get format if not
 	 * implemented.
 	 */
-	ret = v4l2_subdev_call(subdev, video, g_crop, crop);
+	sdsel.pad = pad;
+	ret = v4l2_subdev_call(subdev, pad, get_selection, NULL, &sdsel);
+	if (!ret)
+		sel->r = sdsel.r;
 	if (ret != -ENOIOCTLCMD)
 		return ret;
 
@@ -815,28 +820,50 @@ isp_video_get_crop(struct file *file, void *fh, struct v4l2_crop *crop)
 	if (ret < 0)
 		return ret == -ENOIOCTLCMD ? -ENOTTY : ret;
 
-	crop->c.left = 0;
-	crop->c.top = 0;
-	crop->c.width = format.format.width;
-	crop->c.height = format.format.height;
+	sel->r.left = 0;
+	sel->r.top = 0;
+	sel->r.width = format.format.width;
+	sel->r.height = format.format.height;
 
 	return 0;
 }
 
 static int
-isp_video_set_crop(struct file *file, void *fh, const struct v4l2_crop *crop)
+isp_video_set_selection(struct file *file, void *fh, struct v4l2_selection *sel)
 {
 	struct isp_video *video = video_drvdata(file);
 	struct v4l2_subdev *subdev;
+	struct v4l2_subdev_selection sdsel = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		.target = sel->target,
+		.flags = sel->flags,
+		.r = sel->r,
+	};
+	u32 pad;
 	int ret;
 
-	subdev = isp_video_remote_subdev(video, NULL);
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP:
+		if (video->type == V4L2_BUF_TYPE_VIDEO_OUTPUT)
+			return -EINVAL;
+		break;
+	case V4L2_SEL_TGT_COMPOSE:
+		if (video->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+			return -EINVAL;
+		break;
+	default:
+		return -EINVAL;
+	}
+	subdev = isp_video_remote_subdev(video, &pad);
 	if (subdev == NULL)
 		return -EINVAL;
 
+	sdsel.pad = pad;
 	mutex_lock(&video->mutex);
-	ret = v4l2_subdev_call(subdev, video, s_crop, crop);
+	ret = v4l2_subdev_call(subdev, pad, set_selection, NULL, &sdsel);
 	mutex_unlock(&video->mutex);
+	if (!ret)
+		sel->r = sdsel.r;
 
 	return ret == -ENOIOCTLCMD ? -ENOTTY : ret;
 }
@@ -1252,9 +1279,8 @@ static const struct v4l2_ioctl_ops isp_video_ioctl_ops = {
 	.vidioc_g_fmt_vid_out		= isp_video_get_format,
 	.vidioc_s_fmt_vid_out		= isp_video_set_format,
 	.vidioc_try_fmt_vid_out		= isp_video_try_format,
-	.vidioc_cropcap			= isp_video_cropcap,
-	.vidioc_g_crop			= isp_video_get_crop,
-	.vidioc_s_crop			= isp_video_set_crop,
+	.vidioc_g_selection		= isp_video_get_selection,
+	.vidioc_s_selection		= isp_video_set_selection,
 	.vidioc_g_parm			= isp_video_get_param,
 	.vidioc_s_parm			= isp_video_set_param,
 	.vidioc_reqbufs			= isp_video_reqbufs,
diff --git a/drivers/media/platform/soc_camera/pxa_camera.c b/drivers/media/platform/pxa_camera.c
index 2aaf4a8f71a0..c12209c701d3 100644
--- a/drivers/media/platform/soc_camera/pxa_camera.c
+++ b/drivers/media/platform/pxa_camera.c
@@ -3,6 +3,7 @@
  *
  * Copyright (C) 2006, Sascha Hauer, Pengutronix
  * Copyright (C) 2008, Guennadi Liakhovetski <kernel@pengutronix.de>
+ * Copyright (C) 2016, 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 as published by
@@ -14,6 +15,7 @@
 #include <linux/module.h>
 #include <linux/io.h>
 #include <linux/delay.h>
+#include <linux/device.h>
 #include <linux/dma-mapping.h>
 #include <linux/err.h>
 #include <linux/errno.h>
@@ -22,8 +24,8 @@
 #include <linux/kernel.h>
 #include <linux/mm.h>
 #include <linux/moduleparam.h>
+#include <linux/of.h>
 #include <linux/time.h>
-#include <linux/device.h>
 #include <linux/platform_device.h>
 #include <linux/clk.h>
 #include <linux/sched.h>
@@ -32,13 +34,15 @@
 #include <linux/dma-mapping.h>
 #include <linux/dma/pxa-dma.h>
 
+#include <media/v4l2-async.h>
+#include <media/v4l2-clk.h>
 #include <media/v4l2-common.h>
-#include <media/v4l2-dev.h>
-#include <media/videobuf-dma-sg.h>
-#include <media/soc_camera.h>
-#include <media/drv-intf/soc_mediabus.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
 #include <media/v4l2-of.h>
 
+#include <media/videobuf2-dma-sg.h>
+
 #include <linux/videodev2.h>
 
 #include <linux/platform_data/media/camera-pxa.h>
@@ -46,6 +50,9 @@
 #define PXA_CAM_VERSION "0.0.6"
 #define PXA_CAM_DRV_NAME "pxa27x-camera"
 
+#define DEFAULT_WIDTH	640
+#define DEFAULT_HEIGHT	480
+
 /* Camera Interface */
 #define CICR0		0x0000
 #define CICR1		0x0004
@@ -168,6 +175,462 @@
 			CICR0_PERRM | CICR0_QDM | CICR0_CDM | CICR0_SOFM | \
 			CICR0_EOFM | CICR0_FOM)
 
+#define sensor_call(cam, o, f, args...) \
+	v4l2_subdev_call(cam->sensor, o, f, ##args)
+
+/*
+ * Format handling
+ */
+
+/**
+ * enum pxa_mbus_packing - data packing types on the media-bus
+ * @PXA_MBUS_PACKING_NONE:	no packing, bit-for-bit transfer to RAM, one
+ *				sample represents one pixel
+ * @PXA_MBUS_PACKING_2X8_PADHI:	16 bits transferred in 2 8-bit samples, in the
+ *				possibly incomplete byte high bits are padding
+ * @PXA_MBUS_PACKING_EXTEND16:	sample width (e.g., 10 bits) has to be extended
+ *				to 16 bits
+ */
+enum pxa_mbus_packing {
+	PXA_MBUS_PACKING_NONE,
+	PXA_MBUS_PACKING_2X8_PADHI,
+	PXA_MBUS_PACKING_EXTEND16,
+};
+
+/**
+ * enum pxa_mbus_order - sample order on the media bus
+ * @PXA_MBUS_ORDER_LE:		least significant sample first
+ * @PXA_MBUS_ORDER_BE:		most significant sample first
+ */
+enum pxa_mbus_order {
+	PXA_MBUS_ORDER_LE,
+	PXA_MBUS_ORDER_BE,
+};
+
+/**
+ * enum pxa_mbus_layout - planes layout in memory
+ * @PXA_MBUS_LAYOUT_PACKED:		color components packed
+ * @PXA_MBUS_LAYOUT_PLANAR_2Y_U_V:	YUV components stored in 3 planes (4:2:2)
+ * @PXA_MBUS_LAYOUT_PLANAR_2Y_C:	YUV components stored in a luma and a
+ *					chroma plane (C plane is half the size
+ *					of Y plane)
+ * @PXA_MBUS_LAYOUT_PLANAR_Y_C:		YUV components stored in a luma and a
+ *					chroma plane (C plane is the same size
+ *					as Y plane)
+ */
+enum pxa_mbus_layout {
+	PXA_MBUS_LAYOUT_PACKED = 0,
+	PXA_MBUS_LAYOUT_PLANAR_2Y_U_V,
+	PXA_MBUS_LAYOUT_PLANAR_2Y_C,
+	PXA_MBUS_LAYOUT_PLANAR_Y_C,
+};
+
+/**
+ * struct pxa_mbus_pixelfmt - Data format on the media bus
+ * @name:		Name of the format
+ * @fourcc:		Fourcc code, that will be obtained if the data is
+ *			stored in memory in the following way:
+ * @packing:		Type of sample-packing, that has to be used
+ * @order:		Sample order when storing in memory
+ * @bits_per_sample:	How many bits the bridge has to sample
+ */
+struct pxa_mbus_pixelfmt {
+	const char		*name;
+	u32			fourcc;
+	enum pxa_mbus_packing	packing;
+	enum pxa_mbus_order	order;
+	enum pxa_mbus_layout	layout;
+	u8			bits_per_sample;
+};
+
+/**
+ * struct pxa_mbus_lookup - Lookup FOURCC IDs by mediabus codes for pass-through
+ * @code:	mediabus pixel-code
+ * @fmt:	pixel format description
+ */
+struct pxa_mbus_lookup {
+	u32	code;
+	struct pxa_mbus_pixelfmt	fmt;
+};
+
+static const struct pxa_mbus_lookup mbus_fmt[] = {
+{
+	.code = MEDIA_BUS_FMT_YUYV8_2X8,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_YUYV,
+		.name			= "YUYV",
+		.bits_per_sample	= 8,
+		.packing		= PXA_MBUS_PACKING_2X8_PADHI,
+		.order			= PXA_MBUS_ORDER_LE,
+		.layout			= PXA_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_YVYU8_2X8,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_YVYU,
+		.name			= "YVYU",
+		.bits_per_sample	= 8,
+		.packing		= PXA_MBUS_PACKING_2X8_PADHI,
+		.order			= PXA_MBUS_ORDER_LE,
+		.layout			= PXA_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_UYVY8_2X8,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_UYVY,
+		.name			= "UYVY",
+		.bits_per_sample	= 8,
+		.packing		= PXA_MBUS_PACKING_2X8_PADHI,
+		.order			= PXA_MBUS_ORDER_LE,
+		.layout			= PXA_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_VYUY8_2X8,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_VYUY,
+		.name			= "VYUY",
+		.bits_per_sample	= 8,
+		.packing		= PXA_MBUS_PACKING_2X8_PADHI,
+		.order			= PXA_MBUS_ORDER_LE,
+		.layout			= PXA_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_RGB555_2X8_PADHI_LE,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_RGB555,
+		.name			= "RGB555",
+		.bits_per_sample	= 8,
+		.packing		= PXA_MBUS_PACKING_2X8_PADHI,
+		.order			= PXA_MBUS_ORDER_LE,
+		.layout			= PXA_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_RGB555_2X8_PADHI_BE,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_RGB555X,
+		.name			= "RGB555X",
+		.bits_per_sample	= 8,
+		.packing		= PXA_MBUS_PACKING_2X8_PADHI,
+		.order			= PXA_MBUS_ORDER_BE,
+		.layout			= PXA_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_RGB565_2X8_LE,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_RGB565,
+		.name			= "RGB565",
+		.bits_per_sample	= 8,
+		.packing		= PXA_MBUS_PACKING_2X8_PADHI,
+		.order			= PXA_MBUS_ORDER_LE,
+		.layout			= PXA_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_RGB565_2X8_BE,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_RGB565X,
+		.name			= "RGB565X",
+		.bits_per_sample	= 8,
+		.packing		= PXA_MBUS_PACKING_2X8_PADHI,
+		.order			= PXA_MBUS_ORDER_BE,
+		.layout			= PXA_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_SBGGR8_1X8,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_SBGGR8,
+		.name			= "Bayer 8 BGGR",
+		.bits_per_sample	= 8,
+		.packing		= PXA_MBUS_PACKING_NONE,
+		.order			= PXA_MBUS_ORDER_LE,
+		.layout			= PXA_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_SBGGR10_1X10,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_SBGGR10,
+		.name			= "Bayer 10 BGGR",
+		.bits_per_sample	= 10,
+		.packing		= PXA_MBUS_PACKING_EXTEND16,
+		.order			= PXA_MBUS_ORDER_LE,
+		.layout			= PXA_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_Y8_1X8,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_GREY,
+		.name			= "Grey",
+		.bits_per_sample	= 8,
+		.packing		= PXA_MBUS_PACKING_NONE,
+		.order			= PXA_MBUS_ORDER_LE,
+		.layout			= PXA_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_Y10_1X10,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_Y10,
+		.name			= "Grey 10bit",
+		.bits_per_sample	= 10,
+		.packing		= PXA_MBUS_PACKING_EXTEND16,
+		.order			= PXA_MBUS_ORDER_LE,
+		.layout			= PXA_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_SBGGR10,
+		.name			= "Bayer 10 BGGR",
+		.bits_per_sample	= 8,
+		.packing		= PXA_MBUS_PACKING_2X8_PADHI,
+		.order			= PXA_MBUS_ORDER_LE,
+		.layout			= PXA_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_BE,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_SBGGR10,
+		.name			= "Bayer 10 BGGR",
+		.bits_per_sample	= 8,
+		.packing		= PXA_MBUS_PACKING_2X8_PADHI,
+		.order			= PXA_MBUS_ORDER_BE,
+		.layout			= PXA_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_RGB444_2X8_PADHI_BE,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_RGB444,
+		.name			= "RGB444",
+		.bits_per_sample	= 8,
+		.packing		= PXA_MBUS_PACKING_2X8_PADHI,
+		.order			= PXA_MBUS_ORDER_BE,
+		.layout			= PXA_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_UYVY8_1X16,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_UYVY,
+		.name			= "UYVY 16bit",
+		.bits_per_sample	= 16,
+		.packing		= PXA_MBUS_PACKING_EXTEND16,
+		.order			= PXA_MBUS_ORDER_LE,
+		.layout			= PXA_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_VYUY8_1X16,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_VYUY,
+		.name			= "VYUY 16bit",
+		.bits_per_sample	= 16,
+		.packing		= PXA_MBUS_PACKING_EXTEND16,
+		.order			= PXA_MBUS_ORDER_LE,
+		.layout			= PXA_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_YUYV8_1X16,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_YUYV,
+		.name			= "YUYV 16bit",
+		.bits_per_sample	= 16,
+		.packing		= PXA_MBUS_PACKING_EXTEND16,
+		.order			= PXA_MBUS_ORDER_LE,
+		.layout			= PXA_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_YVYU8_1X16,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_YVYU,
+		.name			= "YVYU 16bit",
+		.bits_per_sample	= 16,
+		.packing		= PXA_MBUS_PACKING_EXTEND16,
+		.order			= PXA_MBUS_ORDER_LE,
+		.layout			= PXA_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_SGRBG8_1X8,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_SGRBG8,
+		.name			= "Bayer 8 GRBG",
+		.bits_per_sample	= 8,
+		.packing		= PXA_MBUS_PACKING_NONE,
+		.order			= PXA_MBUS_ORDER_LE,
+		.layout			= PXA_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_SGRBG10_DPCM8_1X8,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_SGRBG10DPCM8,
+		.name			= "Bayer 10 BGGR DPCM 8",
+		.bits_per_sample	= 8,
+		.packing		= PXA_MBUS_PACKING_NONE,
+		.order			= PXA_MBUS_ORDER_LE,
+		.layout			= PXA_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_SGBRG10_1X10,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_SGBRG10,
+		.name			= "Bayer 10 GBRG",
+		.bits_per_sample	= 10,
+		.packing		= PXA_MBUS_PACKING_EXTEND16,
+		.order			= PXA_MBUS_ORDER_LE,
+		.layout			= PXA_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_SGRBG10_1X10,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_SGRBG10,
+		.name			= "Bayer 10 GRBG",
+		.bits_per_sample	= 10,
+		.packing		= PXA_MBUS_PACKING_EXTEND16,
+		.order			= PXA_MBUS_ORDER_LE,
+		.layout			= PXA_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_SRGGB10_1X10,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_SRGGB10,
+		.name			= "Bayer 10 RGGB",
+		.bits_per_sample	= 10,
+		.packing		= PXA_MBUS_PACKING_EXTEND16,
+		.order			= PXA_MBUS_ORDER_LE,
+		.layout			= PXA_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_SBGGR12_1X12,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_SBGGR12,
+		.name			= "Bayer 12 BGGR",
+		.bits_per_sample	= 12,
+		.packing		= PXA_MBUS_PACKING_EXTEND16,
+		.order			= PXA_MBUS_ORDER_LE,
+		.layout			= PXA_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_SGBRG12_1X12,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_SGBRG12,
+		.name			= "Bayer 12 GBRG",
+		.bits_per_sample	= 12,
+		.packing		= PXA_MBUS_PACKING_EXTEND16,
+		.order			= PXA_MBUS_ORDER_LE,
+		.layout			= PXA_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_SGRBG12_1X12,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_SGRBG12,
+		.name			= "Bayer 12 GRBG",
+		.bits_per_sample	= 12,
+		.packing		= PXA_MBUS_PACKING_EXTEND16,
+		.order			= PXA_MBUS_ORDER_LE,
+		.layout			= PXA_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_SRGGB12_1X12,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_SRGGB12,
+		.name			= "Bayer 12 RGGB",
+		.bits_per_sample	= 12,
+		.packing		= PXA_MBUS_PACKING_EXTEND16,
+		.order			= PXA_MBUS_ORDER_LE,
+		.layout			= PXA_MBUS_LAYOUT_PACKED,
+	},
+},
+};
+
+static s32 pxa_mbus_bytes_per_line(u32 width, const struct pxa_mbus_pixelfmt *mf)
+{
+	if (mf->layout != PXA_MBUS_LAYOUT_PACKED)
+		return width * mf->bits_per_sample / 8;
+
+	switch (mf->packing) {
+	case PXA_MBUS_PACKING_NONE:
+		return width * mf->bits_per_sample / 8;
+	case PXA_MBUS_PACKING_2X8_PADHI:
+	case PXA_MBUS_PACKING_EXTEND16:
+		return width * 2;
+	}
+	return -EINVAL;
+}
+
+static s32 pxa_mbus_image_size(const struct pxa_mbus_pixelfmt *mf,
+			u32 bytes_per_line, u32 height)
+{
+	switch (mf->packing) {
+	case PXA_MBUS_PACKING_2X8_PADHI:
+		return bytes_per_line * height * 2;
+	default:
+		return -EINVAL;
+	}
+}
+
+static const struct pxa_mbus_pixelfmt *pxa_mbus_find_fmtdesc(
+	u32 code,
+	const struct pxa_mbus_lookup *lookup,
+	int n)
+{
+	int i;
+
+	for (i = 0; i < n; i++)
+		if (lookup[i].code == code)
+			return &lookup[i].fmt;
+
+	return NULL;
+}
+
+static const struct pxa_mbus_pixelfmt *pxa_mbus_get_fmtdesc(
+	u32 code)
+{
+	return pxa_mbus_find_fmtdesc(code, mbus_fmt, ARRAY_SIZE(mbus_fmt));
+}
+
+static unsigned int pxa_mbus_config_compatible(const struct v4l2_mbus_config *cfg,
+					unsigned int flags)
+{
+	unsigned long common_flags;
+	bool hsync = true, vsync = true, pclk, data, mode;
+	bool mipi_lanes, mipi_clock;
+
+	common_flags = cfg->flags & flags;
+
+	switch (cfg->type) {
+	case V4L2_MBUS_PARALLEL:
+		hsync = common_flags & (V4L2_MBUS_HSYNC_ACTIVE_HIGH |
+					V4L2_MBUS_HSYNC_ACTIVE_LOW);
+		vsync = common_flags & (V4L2_MBUS_VSYNC_ACTIVE_HIGH |
+					V4L2_MBUS_VSYNC_ACTIVE_LOW);
+		/* fall through */
+	case V4L2_MBUS_BT656:
+		pclk = common_flags & (V4L2_MBUS_PCLK_SAMPLE_RISING |
+				       V4L2_MBUS_PCLK_SAMPLE_FALLING);
+		data = common_flags & (V4L2_MBUS_DATA_ACTIVE_HIGH |
+				       V4L2_MBUS_DATA_ACTIVE_LOW);
+		mode = common_flags & (V4L2_MBUS_MASTER | V4L2_MBUS_SLAVE);
+		return (!hsync || !vsync || !pclk || !data || !mode) ?
+			0 : common_flags;
+	case V4L2_MBUS_CSI2:
+		mipi_lanes = common_flags & V4L2_MBUS_CSI2_LANES;
+		mipi_clock = common_flags & (V4L2_MBUS_CSI2_NONCONTINUOUS_CLOCK |
+					     V4L2_MBUS_CSI2_CONTINUOUS_CLOCK);
+		return (!mipi_lanes || !mipi_clock) ? 0 : common_flags;
+	}
+	return 0;
+}
+
+/**
+ * struct soc_camera_format_xlate - match between host and sensor formats
+ * @code: code of a sensor provided format
+ * @host_fmt: host format after host translation from code
+ *
+ * Host and sensor translation structure. Used in table of host and sensor
+ * formats matchings in soc_camera_device. A host can override the generic list
+ * generation by implementing get_formats(), and use it for format checks and
+ * format setup.
+ */
+struct soc_camera_format_xlate {
+	u32 code;
+	const struct pxa_mbus_pixelfmt *host_fmt;
+};
+
 /*
  * Structures
  */
@@ -180,19 +643,33 @@ enum pxa_camera_active_dma {
 /* buffer for one video frame */
 struct pxa_buffer {
 	/* common v4l buffer stuff -- must be first */
-	struct videobuf_buffer		vb;
+	struct vb2_v4l2_buffer		vbuf;
+	struct list_head		queue;
 	u32	code;
+	int				nb_planes;
 	/* our descriptor lists for Y, U and V channels */
 	struct dma_async_tx_descriptor	*descs[3];
 	dma_cookie_t			cookie[3];
 	struct scatterlist		*sg[3];
 	int				sg_len[3];
+	size_t				plane_sizes[3];
 	int				inwork;
 	enum pxa_camera_active_dma	active_dma;
 };
 
 struct pxa_camera_dev {
-	struct soc_camera_host	soc_host;
+	struct v4l2_device	v4l2_dev;
+	struct video_device	vdev;
+	struct v4l2_async_notifier notifier;
+	struct vb2_queue	vb2_vq;
+	struct v4l2_subdev	*sensor;
+	struct soc_camera_format_xlate *user_formats;
+	const struct soc_camera_format_xlate *current_fmt;
+	struct v4l2_pix_format	current_pix;
+
+	struct v4l2_async_subdev asd;
+	struct v4l2_async_subdev *asds[1];
+
 	/*
 	 * PXA27x is only supposed to handle one camera on its Quick Capture
 	 * interface. If anyone ever builds hardware to enable more than
@@ -212,11 +689,14 @@ struct pxa_camera_dev {
 	unsigned long		ciclk;
 	unsigned long		mclk;
 	u32			mclk_divisor;
+	struct v4l2_clk		*mclk_clk;
 	u16			width_flags;	/* max 10 bits */
 
 	struct list_head	capture;
 
 	spinlock_t		lock;
+	struct mutex		mlock;
+	unsigned int		buf_sequence;
 
 	struct pxa_buffer	*active;
 	struct tasklet_struct	task_eof;
@@ -230,59 +710,90 @@ struct pxa_cam {
 
 static const char *pxa_cam_driver_description = "PXA_Camera";
 
-static unsigned int vid_limit = 16;	/* Video memory limit, in Mb */
-
 /*
- *  Videobuf operations
+ * Format translation functions
  */
-static int pxa_videobuf_setup(struct videobuf_queue *vq, unsigned int *count,
-			      unsigned int *size)
+static const struct soc_camera_format_xlate
+*pxa_mbus_xlate_by_fourcc(struct soc_camera_format_xlate *user_formats,
+			  unsigned int fourcc)
 {
-	struct soc_camera_device *icd = vq->priv_data;
+	unsigned int i;
 
-	dev_dbg(icd->parent, "count=%d, size=%d\n", *count, *size);
-
-	*size = icd->sizeimage;
-
-	if (0 == *count)
-		*count = 32;
-	if (*size * *count > vid_limit * 1024 * 1024)
-		*count = (vid_limit * 1024 * 1024) / *size;
-
-	return 0;
+	for (i = 0; user_formats[i].code; i++)
+		if (user_formats[i].host_fmt->fourcc == fourcc)
+			return user_formats + i;
+	return NULL;
 }
 
-static void free_buffer(struct videobuf_queue *vq, struct pxa_buffer *buf)
+static struct soc_camera_format_xlate *pxa_mbus_build_fmts_xlate(
+	struct v4l2_device *v4l2_dev, struct v4l2_subdev *subdev,
+	int (*get_formats)(struct v4l2_device *, unsigned int,
+			   struct soc_camera_format_xlate *xlate))
 {
-	struct soc_camera_device *icd = vq->priv_data;
-	struct videobuf_dmabuf *dma = videobuf_to_dma(&buf->vb);
-	int i;
-
-	BUG_ON(in_interrupt());
+	unsigned int i, fmts = 0, raw_fmts = 0;
+	int ret;
+	struct v4l2_subdev_mbus_code_enum code = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	struct soc_camera_format_xlate *user_formats;
 
-	dev_dbg(icd->parent, "%s (vb=0x%p) 0x%08lx %d\n", __func__,
-		&buf->vb, buf->vb.baddr, buf->vb.bsize);
+	while (!v4l2_subdev_call(subdev, pad, enum_mbus_code, NULL, &code)) {
+		raw_fmts++;
+		code.index++;
+	}
 
 	/*
-	 * This waits until this buffer is out of danger, i.e., until it is no
-	 * longer in STATE_QUEUED or STATE_ACTIVE
+	 * First pass - only count formats this host-sensor
+	 * configuration can provide
 	 */
-	videobuf_waiton(vq, &buf->vb, 0, 0);
+	for (i = 0; i < raw_fmts; i++) {
+		ret = get_formats(v4l2_dev, i, NULL);
+		if (ret < 0)
+			return ERR_PTR(ret);
+		fmts += ret;
+	}
 
-	for (i = 0; i < 3 && buf->descs[i]; i++) {
-		dmaengine_desc_free(buf->descs[i]);
-		kfree(buf->sg[i]);
-		buf->descs[i] = NULL;
-		buf->sg[i] = NULL;
-		buf->sg_len[i] = 0;
+	if (!fmts)
+		return ERR_PTR(-ENXIO);
+
+	user_formats = kcalloc(fmts + 1, sizeof(*user_formats), GFP_KERNEL);
+	if (!user_formats)
+		return ERR_PTR(-ENOMEM);
+
+	/* Second pass - actually fill data formats */
+	fmts = 0;
+	for (i = 0; i < raw_fmts; i++) {
+		ret = get_formats(v4l2_dev, i, user_formats + fmts);
+		if (ret < 0)
+			goto egfmt;
+		fmts += ret;
 	}
-	videobuf_dma_unmap(vq->dev, dma);
-	videobuf_dma_free(dma);
+	user_formats[fmts].code = 0;
 
-	buf->vb.state = VIDEOBUF_NEEDS_INIT;
+	return user_formats;
+egfmt:
+	kfree(user_formats);
+	return ERR_PTR(ret);
+}
 
-	dev_dbg(icd->parent, "%s end (vb=0x%p) 0x%08lx %d\n", __func__,
-		&buf->vb, buf->vb.baddr, buf->vb.bsize);
+/*
+ *  Videobuf operations
+ */
+static struct pxa_buffer *vb2_to_pxa_buffer(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+
+	return container_of(vbuf, struct pxa_buffer, vbuf);
+}
+
+static struct device *pcdev_to_dev(struct pxa_camera_dev *pcdev)
+{
+	return pcdev->v4l2_dev.dev;
+}
+
+static struct pxa_camera_dev *v4l2_dev_to_pcdev(struct v4l2_device *v4l2_dev)
+{
+	return container_of(v4l2_dev, struct pxa_camera_dev, v4l2_dev);
 }
 
 static void pxa_camera_dma_irq(struct pxa_camera_dev *pcdev,
@@ -312,31 +823,26 @@ static void pxa_camera_dma_irq_v(void *data)
 /**
  * pxa_init_dma_channel - init dma descriptors
  * @pcdev: pxa camera device
- * @buf: pxa buffer to find pxa dma channel
+ * @vb: videobuffer2 buffer
  * @dma: dma video buffer
  * @channel: dma channel (0 => 'Y', 1 => 'U', 2 => 'V')
  * @cibr: camera Receive Buffer Register
- * @size: bytes to transfer
- * @offset: offset in videobuffer of the first byte to transfer
  *
  * Prepares the pxa dma descriptors to transfer one camera channel.
  *
  * Returns 0 if success or -ENOMEM if no memory is available
  */
 static int pxa_init_dma_channel(struct pxa_camera_dev *pcdev,
-				struct pxa_buffer *buf,
-				struct videobuf_dmabuf *dma, int channel,
-				int cibr, int size, int offset)
+				struct pxa_buffer *buf, int channel,
+				struct scatterlist *sg, int sglen)
 {
 	struct dma_chan *dma_chan = pcdev->dma_chans[channel];
-	struct scatterlist *sg = buf->sg[channel];
-	int sglen = buf->sg_len[channel];
 	struct dma_async_tx_descriptor *tx;
 
 	tx = dmaengine_prep_slave_sg(dma_chan, sg, sglen, DMA_DEV_TO_MEM,
 				     DMA_PREP_INTERRUPT | DMA_CTRL_REUSE);
 	if (!tx) {
-		dev_err(pcdev->soc_host.v4l2_dev.dev,
+		dev_err(pcdev_to_dev(pcdev),
 			"dmaengine_prep_slave_sg failed\n");
 		goto fail;
 	}
@@ -357,11 +863,9 @@ static int pxa_init_dma_channel(struct pxa_camera_dev *pcdev,
 	buf->descs[channel] = tx;
 	return 0;
 fail:
-	kfree(sg);
-
-	dev_dbg(pcdev->soc_host.v4l2_dev.dev,
-		"%s (vb=0x%p) dma_tx=%p\n",
-		__func__, &buf->vb, tx);
+	dev_dbg(pcdev_to_dev(pcdev),
+		"%s (vb=%p) dma_tx=%p\n",
+		__func__, buf, tx);
 
 	return -ENOMEM;
 }
@@ -370,133 +874,10 @@ static void pxa_videobuf_set_actdma(struct pxa_camera_dev *pcdev,
 				    struct pxa_buffer *buf)
 {
 	buf->active_dma = DMA_Y;
-	if (pcdev->channels == 3)
+	if (buf->nb_planes == 3)
 		buf->active_dma |= DMA_U | DMA_V;
 }
 
-/*
- * Please check the DMA prepared buffer structure in :
- *   Documentation/video4linux/pxa_camera.txt
- * Please check also in pxa_camera_check_link_miss() to understand why DMA chain
- * modification while DMA chain is running will work anyway.
- */
-static int pxa_videobuf_prepare(struct videobuf_queue *vq,
-		struct videobuf_buffer *vb, enum v4l2_field field)
-{
-	struct soc_camera_device *icd = vq->priv_data;
-	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
-	struct pxa_camera_dev *pcdev = ici->priv;
-	struct device *dev = pcdev->soc_host.v4l2_dev.dev;
-	struct pxa_buffer *buf = container_of(vb, struct pxa_buffer, vb);
-	int ret;
-	int size_y, size_u = 0, size_v = 0;
-	size_t sizes[3];
-
-	dev_dbg(dev, "%s (vb=0x%p) 0x%08lx %d\n", __func__,
-		vb, vb->baddr, vb->bsize);
-
-	/* Added list head initialization on alloc */
-	WARN_ON(!list_empty(&vb->queue));
-
-#ifdef DEBUG
-	/*
-	 * This can be useful if you want to see if we actually fill
-	 * the buffer with something
-	 */
-	memset((void *)vb->baddr, 0xaa, vb->bsize);
-#endif
-
-	BUG_ON(NULL == icd->current_fmt);
-
-	/*
-	 * I think, in buf_prepare you only have to protect global data,
-	 * the actual buffer is yours
-	 */
-	buf->inwork = 1;
-
-	if (buf->code	!= icd->current_fmt->code ||
-	    vb->width	!= icd->user_width ||
-	    vb->height	!= icd->user_height ||
-	    vb->field	!= field) {
-		buf->code	= icd->current_fmt->code;
-		vb->width	= icd->user_width;
-		vb->height	= icd->user_height;
-		vb->field	= field;
-		vb->state	= VIDEOBUF_NEEDS_INIT;
-	}
-
-	vb->size = icd->sizeimage;
-	if (0 != vb->baddr && vb->bsize < vb->size) {
-		ret = -EINVAL;
-		goto out;
-	}
-
-	if (vb->state == VIDEOBUF_NEEDS_INIT) {
-		int size = vb->size;
-		struct videobuf_dmabuf *dma = videobuf_to_dma(vb);
-
-		ret = videobuf_iolock(vq, vb, NULL);
-		if (ret)
-			goto out;
-
-		if (pcdev->channels == 3) {
-			size_y = size / 2;
-			size_u = size_v = size / 4;
-		} else {
-			size_y = size;
-		}
-
-		sizes[0] = size_y;
-		sizes[1] = size_u;
-		sizes[2] = size_v;
-		ret = sg_split(dma->sglist, dma->sglen, 0, pcdev->channels,
-			       sizes, buf->sg, buf->sg_len, GFP_KERNEL);
-		if (ret < 0) {
-			dev_err(dev, "sg_split failed: %d\n", ret);
-			goto fail;
-		}
-
-		/* init DMA for Y channel */
-		ret = pxa_init_dma_channel(pcdev, buf, dma, 0, CIBR0,
-					   size_y, 0);
-		if (ret) {
-			dev_err(dev, "DMA initialization for Y/RGB failed\n");
-			goto fail;
-		}
-
-		/* init DMA for U channel */
-		if (size_u)
-			ret = pxa_init_dma_channel(pcdev, buf, dma, 1, CIBR1,
-						   size_u, size_y);
-		if (ret) {
-			dev_err(dev, "DMA initialization for U failed\n");
-			goto fail;
-		}
-
-		/* init DMA for V channel */
-		if (size_v)
-			ret = pxa_init_dma_channel(pcdev, buf, dma, 2, CIBR2,
-						   size_v, size_y + size_u);
-		if (ret) {
-			dev_err(dev, "DMA initialization for V failed\n");
-			goto fail;
-		}
-
-		vb->state = VIDEOBUF_PREPARED;
-	}
-
-	buf->inwork = 0;
-	pxa_videobuf_set_actdma(pcdev, buf);
-
-	return 0;
-
-fail:
-	free_buffer(vq, buf);
-out:
-	buf->inwork = 0;
-	return ret;
-}
-
 /**
  * pxa_dma_start_channels - start DMA channel for active buffer
  * @pcdev: pxa camera device
@@ -507,12 +888,9 @@ out:
 static void pxa_dma_start_channels(struct pxa_camera_dev *pcdev)
 {
 	int i;
-	struct pxa_buffer *active;
-
-	active = pcdev->active;
 
 	for (i = 0; i < pcdev->channels; i++) {
-		dev_dbg(pcdev->soc_host.v4l2_dev.dev,
+		dev_dbg(pcdev_to_dev(pcdev),
 			"%s (channel=%d)\n", __func__, i);
 		dma_async_issue_pending(pcdev->dma_chans[i]);
 	}
@@ -523,7 +901,7 @@ static void pxa_dma_stop_channels(struct pxa_camera_dev *pcdev)
 	int i;
 
 	for (i = 0; i < pcdev->channels; i++) {
-		dev_dbg(pcdev->soc_host.v4l2_dev.dev,
+		dev_dbg(pcdev_to_dev(pcdev),
 			"%s (channel=%d)\n", __func__, i);
 		dmaengine_terminate_all(pcdev->dma_chans[i]);
 	}
@@ -536,7 +914,7 @@ static void pxa_dma_add_tail_buf(struct pxa_camera_dev *pcdev,
 
 	for (i = 0; i < pcdev->channels; i++) {
 		buf->cookie[i] = dmaengine_submit(buf->descs[i]);
-		dev_dbg(pcdev->soc_host.v4l2_dev.dev,
+		dev_dbg(pcdev_to_dev(pcdev),
 			"%s (channel=%d) : submit vb=%p cookie=%d\n",
 			__func__, i, buf, buf->descs[i]->cookie);
 	}
@@ -554,7 +932,7 @@ static void pxa_camera_start_capture(struct pxa_camera_dev *pcdev)
 {
 	unsigned long cicr0;
 
-	dev_dbg(pcdev->soc_host.v4l2_dev.dev, "%s\n", __func__);
+	dev_dbg(pcdev_to_dev(pcdev), "%s\n", __func__);
 	__raw_writel(__raw_readl(pcdev->base + CISR), pcdev->base + CISR);
 	/* Enable End-Of-Frame Interrupt */
 	cicr0 = __raw_readl(pcdev->base + CICR0) | CICR0_ENB;
@@ -572,72 +950,24 @@ static void pxa_camera_stop_capture(struct pxa_camera_dev *pcdev)
 	__raw_writel(cicr0, pcdev->base + CICR0);
 
 	pcdev->active = NULL;
-	dev_dbg(pcdev->soc_host.v4l2_dev.dev, "%s\n", __func__);
-}
-
-/* Called under spinlock_irqsave(&pcdev->lock, ...) */
-static void pxa_videobuf_queue(struct videobuf_queue *vq,
-			       struct videobuf_buffer *vb)
-{
-	struct soc_camera_device *icd = vq->priv_data;
-	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
-	struct pxa_camera_dev *pcdev = ici->priv;
-	struct pxa_buffer *buf = container_of(vb, struct pxa_buffer, vb);
-
-	dev_dbg(icd->parent, "%s (vb=0x%p) 0x%08lx %d active=%p\n",
-		__func__, vb, vb->baddr, vb->bsize, pcdev->active);
-
-	list_add_tail(&vb->queue, &pcdev->capture);
-
-	vb->state = VIDEOBUF_ACTIVE;
-	pxa_dma_add_tail_buf(pcdev, buf);
-
-	if (!pcdev->active)
-		pxa_camera_start_capture(pcdev);
-}
-
-static void pxa_videobuf_release(struct videobuf_queue *vq,
-				 struct videobuf_buffer *vb)
-{
-	struct pxa_buffer *buf = container_of(vb, struct pxa_buffer, vb);
-#ifdef DEBUG
-	struct soc_camera_device *icd = vq->priv_data;
-	struct device *dev = icd->parent;
-
-	dev_dbg(dev, "%s (vb=0x%p) 0x%08lx %d\n", __func__,
-		vb, vb->baddr, vb->bsize);
-
-	switch (vb->state) {
-	case VIDEOBUF_ACTIVE:
-		dev_dbg(dev, "%s (active)\n", __func__);
-		break;
-	case VIDEOBUF_QUEUED:
-		dev_dbg(dev, "%s (queued)\n", __func__);
-		break;
-	case VIDEOBUF_PREPARED:
-		dev_dbg(dev, "%s (prepared)\n", __func__);
-		break;
-	default:
-		dev_dbg(dev, "%s (unknown)\n", __func__);
-		break;
-	}
-#endif
-
-	free_buffer(vq, buf);
+	dev_dbg(pcdev_to_dev(pcdev), "%s\n", __func__);
 }
 
 static void pxa_camera_wakeup(struct pxa_camera_dev *pcdev,
-			      struct videobuf_buffer *vb,
-			      struct pxa_buffer *buf)
+			      struct pxa_buffer *buf,
+			      enum vb2_buffer_state state)
 {
+	struct vb2_buffer *vb = &buf->vbuf.vb2_buf;
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+
 	/* _init is used to debug races, see comment in pxa_camera_reqbufs() */
-	list_del_init(&vb->queue);
-	vb->state = VIDEOBUF_DONE;
-	v4l2_get_timestamp(&vb->ts);
-	vb->field_count++;
-	wake_up(&vb->done);
-	dev_dbg(pcdev->soc_host.v4l2_dev.dev, "%s dequeud buffer (vb=0x%p)\n",
-		__func__, vb);
+	list_del_init(&buf->queue);
+	vb->timestamp = ktime_get_ns();
+	vbuf->sequence = pcdev->buf_sequence++;
+	vbuf->field = V4L2_FIELD_NONE;
+	vb2_buffer_done(vb, VB2_BUF_STATE_DONE);
+	dev_dbg(pcdev_to_dev(pcdev), "%s dequeued buffer (buf=0x%p)\n",
+		__func__, buf);
 
 	if (list_empty(&pcdev->capture)) {
 		pxa_camera_stop_capture(pcdev);
@@ -645,7 +975,7 @@ static void pxa_camera_wakeup(struct pxa_camera_dev *pcdev,
 	}
 
 	pcdev->active = list_entry(pcdev->capture.next,
-				   struct pxa_buffer, vb.queue);
+				   struct pxa_buffer, queue);
 }
 
 /**
@@ -670,7 +1000,7 @@ static void pxa_camera_check_link_miss(struct pxa_camera_dev *pcdev,
 {
 	bool is_dma_stopped = last_submitted != last_issued;
 
-	dev_dbg(pcdev->soc_host.v4l2_dev.dev,
+	dev_dbg(pcdev_to_dev(pcdev),
 		"%s : top queued buffer=%p, is_dma_stopped=%d\n",
 		__func__, pcdev->active, is_dma_stopped);
 
@@ -681,19 +1011,17 @@ static void pxa_camera_check_link_miss(struct pxa_camera_dev *pcdev,
 static void pxa_camera_dma_irq(struct pxa_camera_dev *pcdev,
 			       enum pxa_camera_active_dma act_dma)
 {
-	struct device *dev = pcdev->soc_host.v4l2_dev.dev;
 	struct pxa_buffer *buf, *last_buf;
 	unsigned long flags;
 	u32 camera_status, overrun;
 	int chan;
-	struct videobuf_buffer *vb;
 	enum dma_status last_status;
 	dma_cookie_t last_issued;
 
 	spin_lock_irqsave(&pcdev->lock, flags);
 
 	camera_status = __raw_readl(pcdev->base + CISR);
-	dev_dbg(dev, "camera dma irq, cisr=0x%x dma=%d\n",
+	dev_dbg(pcdev_to_dev(pcdev), "camera dma irq, cisr=0x%x dma=%d\n",
 		camera_status, act_dma);
 	overrun = CISR_IFO_0;
 	if (pcdev->channels == 3)
@@ -714,9 +1042,8 @@ static void pxa_camera_dma_irq(struct pxa_camera_dev *pcdev,
 	if (!pcdev->active)
 		goto out;
 
-	vb = &pcdev->active->vb;
-	buf = container_of(vb, struct pxa_buffer, vb);
-	WARN_ON(buf->inwork || list_empty(&vb->queue));
+	buf = pcdev->active;
+	WARN_ON(buf->inwork || list_empty(&buf->queue));
 
 	/*
 	 * It's normal if the last frame creates an overrun, as there
@@ -734,23 +1061,23 @@ static void pxa_camera_dma_irq(struct pxa_camera_dev *pcdev,
 		break;
 	}
 	last_buf = list_entry(pcdev->capture.prev,
-			      struct pxa_buffer, vb.queue);
+			      struct pxa_buffer, queue);
 	last_status = dma_async_is_tx_complete(pcdev->dma_chans[chan],
 					       last_buf->cookie[chan],
 					       NULL, &last_issued);
 	if (camera_status & overrun &&
 	    last_status != DMA_COMPLETE) {
-		dev_dbg(dev, "FIFO overrun! CISR: %x\n",
+		dev_dbg(pcdev_to_dev(pcdev), "FIFO overrun! CISR: %x\n",
 			camera_status);
 		pxa_camera_stop_capture(pcdev);
-		list_for_each_entry(buf, &pcdev->capture, vb.queue)
+		list_for_each_entry(buf, &pcdev->capture, queue)
 			pxa_dma_add_tail_buf(pcdev, buf);
 		pxa_camera_start_capture(pcdev);
 		goto out;
 	}
 	buf->active_dma &= ~act_dma;
 	if (!buf->active_dma) {
-		pxa_camera_wakeup(pcdev, vb, buf);
+		pxa_camera_wakeup(pcdev, buf, VB2_BUF_STATE_DONE);
 		pxa_camera_check_link_miss(pcdev, last_buf->cookie[chan],
 					   last_issued);
 	}
@@ -759,33 +1086,10 @@ out:
 	spin_unlock_irqrestore(&pcdev->lock, flags);
 }
 
-static struct videobuf_queue_ops pxa_videobuf_ops = {
-	.buf_setup      = pxa_videobuf_setup,
-	.buf_prepare    = pxa_videobuf_prepare,
-	.buf_queue      = pxa_videobuf_queue,
-	.buf_release    = pxa_videobuf_release,
-};
-
-static void pxa_camera_init_videobuf(struct videobuf_queue *q,
-			      struct soc_camera_device *icd)
-{
-	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
-	struct pxa_camera_dev *pcdev = ici->priv;
-
-	/*
-	 * We must pass NULL as dev pointer, then all pci_* dma operations
-	 * transform to normal dma_* ones.
-	 */
-	videobuf_queue_sg_init(q, &pxa_videobuf_ops, NULL, &pcdev->lock,
-				V4L2_BUF_TYPE_VIDEO_CAPTURE, V4L2_FIELD_NONE,
-				sizeof(struct pxa_buffer), icd, &ici->host_lock);
-}
-
 static u32 mclk_get_divisor(struct platform_device *pdev,
 			    struct pxa_camera_dev *pcdev)
 {
 	unsigned long mclk = pcdev->mclk;
-	struct device *dev = &pdev->dev;
 	u32 div;
 	unsigned long lcdclk;
 
@@ -795,7 +1099,8 @@ static u32 mclk_get_divisor(struct platform_device *pdev,
 	/* mclk <= ciclk / 4 (27.4.2) */
 	if (mclk > lcdclk / 4) {
 		mclk = lcdclk / 4;
-		dev_warn(dev, "Limiting master clock to %lu\n", mclk);
+		dev_warn(pcdev_to_dev(pcdev),
+			 "Limiting master clock to %lu\n", mclk);
 	}
 
 	/* We verify mclk != 0, so if anyone breaks it, here comes their Oops */
@@ -805,7 +1110,7 @@ static u32 mclk_get_divisor(struct platform_device *pdev,
 	if (pcdev->platform_flags & PXA_CAMERA_MCLK_EN)
 		pcdev->mclk = lcdclk / (2 * (div + 1));
 
-	dev_dbg(dev, "LCD clock %luHz, target freq %luHz, divisor %u\n",
+	dev_dbg(pcdev_to_dev(pcdev), "LCD clock %luHz, target freq %luHz, divisor %u\n",
 		lcdclk, mclk, div);
 
 	return div;
@@ -860,9 +1165,8 @@ static void pxa_camera_eof(unsigned long arg)
 	struct pxa_camera_dev *pcdev = (struct pxa_camera_dev *)arg;
 	unsigned long cifr;
 	struct pxa_buffer *buf;
-	struct videobuf_buffer *vb;
 
-	dev_dbg(pcdev->soc_host.v4l2_dev.dev,
+	dev_dbg(pcdev_to_dev(pcdev),
 		"Camera interrupt status 0x%x\n",
 		__raw_readl(pcdev->base + CISR));
 
@@ -871,9 +1175,8 @@ static void pxa_camera_eof(unsigned long arg)
 	__raw_writel(cifr, pcdev->base + CIFR);
 
 	pcdev->active = list_first_entry(&pcdev->capture,
-					 struct pxa_buffer, vb.queue);
-	vb = &pcdev->active->vb;
-	buf = container_of(vb, struct pxa_buffer, vb);
+					 struct pxa_buffer, queue);
+	buf = pcdev->active;
 	pxa_videobuf_set_actdma(pcdev, buf);
 
 	pxa_dma_start_channels(pcdev);
@@ -885,7 +1188,7 @@ static irqreturn_t pxa_camera_irq(int irq, void *data)
 	unsigned long status, cicr0;
 
 	status = __raw_readl(pcdev->base + CISR);
-	dev_dbg(pcdev->soc_host.v4l2_dev.dev,
+	dev_dbg(pcdev_to_dev(pcdev),
 		"Camera interrupt status 0x%lx\n", status);
 
 	if (!status)
@@ -902,47 +1205,6 @@ static irqreturn_t pxa_camera_irq(int irq, void *data)
 	return IRQ_HANDLED;
 }
 
-static int pxa_camera_add_device(struct soc_camera_device *icd)
-{
-	dev_info(icd->parent, "PXA Camera driver attached to camera %d\n",
-		 icd->devnum);
-
-	return 0;
-}
-
-static void pxa_camera_remove_device(struct soc_camera_device *icd)
-{
-	dev_info(icd->parent, "PXA Camera driver detached from camera %d\n",
-		 icd->devnum);
-}
-
-/*
- * The following two functions absolutely depend on the fact, that
- * there can be only one camera on PXA quick capture interface
- * Called with .host_lock held
- */
-static int pxa_camera_clock_start(struct soc_camera_host *ici)
-{
-	struct pxa_camera_dev *pcdev = ici->priv;
-
-	pxa_camera_activate(pcdev);
-
-	return 0;
-}
-
-/* Called with .host_lock held */
-static void pxa_camera_clock_stop(struct soc_camera_host *ici)
-{
-	struct pxa_camera_dev *pcdev = ici->priv;
-
-	/* disable capture, disable interrupts */
-	__raw_writel(0x3ff, pcdev->base + CICR0);
-
-	/* Stop DMA engine */
-	pxa_dma_stop_channels(pcdev);
-	pxa_camera_deactivate(pcdev);
-}
-
 static int test_platform_param(struct pxa_camera_dev *pcdev,
 			       unsigned char buswidth, unsigned long *flags)
 {
@@ -968,15 +1230,12 @@ static int test_platform_param(struct pxa_camera_dev *pcdev,
 	return -EINVAL;
 }
 
-static void pxa_camera_setup_cicr(struct soc_camera_device *icd,
+static void pxa_camera_setup_cicr(struct pxa_camera_dev *pcdev,
 				  unsigned long flags, __u32 pixfmt)
 {
-	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
-	struct pxa_camera_dev *pcdev = ici->priv;
-	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 	unsigned long dw, bpp;
 	u32 cicr0, cicr1, cicr2, cicr3, cicr4 = 0, y_skip_top;
-	int ret = v4l2_subdev_call(sd, sensor, g_skip_top_lines, &y_skip_top);
+	int ret = sensor_call(pcdev, sensor, g_skip_top_lines, &y_skip_top);
 
 	if (ret < 0)
 		y_skip_top = 0;
@@ -985,7 +1244,7 @@ static void pxa_camera_setup_cicr(struct soc_camera_device *icd,
 	 * Datawidth is now guaranteed to be equal to one of the three values.
 	 * We fix bit-per-pixel equal to data-width...
 	 */
-	switch (icd->current_fmt->host_fmt->bits_per_sample) {
+	switch (pcdev->current_fmt->host_fmt->bits_per_sample) {
 	case 10:
 		dw = 4;
 		bpp = 0x40;
@@ -1019,7 +1278,7 @@ static void pxa_camera_setup_cicr(struct soc_camera_device *icd,
 	if (cicr0 & CICR0_ENB)
 		__raw_writel(cicr0 & ~CICR0_ENB, pcdev->base + CICR0);
 
-	cicr1 = CICR1_PPL_VAL(icd->user_width - 1) | bpp | dw;
+	cicr1 = CICR1_PPL_VAL(pcdev->current_pix.width - 1) | bpp | dw;
 
 	switch (pixfmt) {
 	case V4L2_PIX_FMT_YUV422P:
@@ -1048,7 +1307,7 @@ static void pxa_camera_setup_cicr(struct soc_camera_device *icd,
 	}
 
 	cicr2 = 0;
-	cicr3 = CICR3_LPF_VAL(icd->user_height - 1) |
+	cicr3 = CICR3_LPF_VAL(pcdev->current_pix.height - 1) |
 		CICR3_BFW_VAL(min((u32)255, y_skip_top));
 	cicr4 |= pcdev->mclk_divisor;
 
@@ -1064,28 +1323,271 @@ static void pxa_camera_setup_cicr(struct soc_camera_device *icd,
 	__raw_writel(cicr0, pcdev->base + CICR0);
 }
 
-static int pxa_camera_set_bus_param(struct soc_camera_device *icd)
+/*
+ * Videobuf2 section
+ */
+static void pxa_buffer_cleanup(struct pxa_buffer *buf)
+{
+	int i;
+
+	for (i = 0; i < 3 && buf->descs[i]; i++) {
+		dmaengine_desc_free(buf->descs[i]);
+		kfree(buf->sg[i]);
+		buf->descs[i] = NULL;
+		buf->sg[i] = NULL;
+		buf->sg_len[i] = 0;
+		buf->plane_sizes[i] = 0;
+	}
+	buf->nb_planes = 0;
+}
+
+static int pxa_buffer_init(struct pxa_camera_dev *pcdev,
+			   struct pxa_buffer *buf)
+{
+	struct vb2_buffer *vb = &buf->vbuf.vb2_buf;
+	struct sg_table *sgt = vb2_dma_sg_plane_desc(vb, 0);
+	int nb_channels = pcdev->channels;
+	int i, ret = 0;
+	unsigned long size = vb2_plane_size(vb, 0);
+
+	switch (nb_channels) {
+	case 1:
+		buf->plane_sizes[0] = size;
+		break;
+	case 3:
+		buf->plane_sizes[0] = size / 2;
+		buf->plane_sizes[1] = size / 4;
+		buf->plane_sizes[2] = size / 4;
+		break;
+	default:
+		return -EINVAL;
+	};
+	buf->nb_planes = nb_channels;
+
+	ret = sg_split(sgt->sgl, sgt->nents, 0, nb_channels,
+		       buf->plane_sizes, buf->sg, buf->sg_len, GFP_KERNEL);
+	if (ret < 0) {
+		dev_err(pcdev_to_dev(pcdev),
+			"sg_split failed: %d\n", ret);
+		return ret;
+	}
+	for (i = 0; i < nb_channels; i++) {
+		ret = pxa_init_dma_channel(pcdev, buf, i,
+					   buf->sg[i], buf->sg_len[i]);
+		if (ret) {
+			pxa_buffer_cleanup(buf);
+			return ret;
+		}
+	}
+	INIT_LIST_HEAD(&buf->queue);
+
+	return ret;
+}
+
+static void pxac_vb2_cleanup(struct vb2_buffer *vb)
+{
+	struct pxa_buffer *buf = vb2_to_pxa_buffer(vb);
+	struct pxa_camera_dev *pcdev = vb2_get_drv_priv(vb->vb2_queue);
+
+	dev_dbg(pcdev_to_dev(pcdev),
+		 "%s(vb=%p)\n", __func__, vb);
+	pxa_buffer_cleanup(buf);
+}
+
+static void pxac_vb2_queue(struct vb2_buffer *vb)
+{
+	struct pxa_buffer *buf = vb2_to_pxa_buffer(vb);
+	struct pxa_camera_dev *pcdev = vb2_get_drv_priv(vb->vb2_queue);
+
+	dev_dbg(pcdev_to_dev(pcdev),
+		 "%s(vb=%p) nb_channels=%d size=%lu active=%p\n",
+		__func__, vb, pcdev->channels, vb2_get_plane_payload(vb, 0),
+		pcdev->active);
+
+	list_add_tail(&buf->queue, &pcdev->capture);
+
+	pxa_dma_add_tail_buf(pcdev, buf);
+}
+
+/*
+ * Please check the DMA prepared buffer structure in :
+ *   Documentation/video4linux/pxa_camera.txt
+ * Please check also in pxa_camera_check_link_miss() to understand why DMA chain
+ * modification while DMA chain is running will work anyway.
+ */
+static int pxac_vb2_prepare(struct vb2_buffer *vb)
+{
+	struct pxa_camera_dev *pcdev = vb2_get_drv_priv(vb->vb2_queue);
+	struct pxa_buffer *buf = vb2_to_pxa_buffer(vb);
+	int ret = 0;
+
+	switch (pcdev->channels) {
+	case 1:
+	case 3:
+		vb2_set_plane_payload(vb, 0, pcdev->current_pix.sizeimage);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	dev_dbg(pcdev_to_dev(pcdev),
+		 "%s (vb=%p) nb_channels=%d size=%lu\n",
+		__func__, vb, pcdev->channels, vb2_get_plane_payload(vb, 0));
+
+	WARN_ON(!pcdev->current_fmt);
+
+#ifdef DEBUG
+	/*
+	 * This can be useful if you want to see if we actually fill
+	 * the buffer with something
+	 */
+	for (i = 0; i < vb->num_planes; i++)
+		memset((void *)vb2_plane_vaddr(vb, i),
+		       0xaa, vb2_get_plane_payload(vb, i));
+#endif
+
+	/*
+	 * I think, in buf_prepare you only have to protect global data,
+	 * the actual buffer is yours
+	 */
+	buf->inwork = 0;
+	pxa_videobuf_set_actdma(pcdev, buf);
+
+	return ret;
+}
+
+static int pxac_vb2_init(struct vb2_buffer *vb)
+{
+	struct pxa_camera_dev *pcdev = vb2_get_drv_priv(vb->vb2_queue);
+	struct pxa_buffer *buf = vb2_to_pxa_buffer(vb);
+
+	dev_dbg(pcdev_to_dev(pcdev),
+		 "%s(nb_channels=%d)\n",
+		__func__, pcdev->channels);
+
+	return pxa_buffer_init(pcdev, buf);
+}
+
+static int pxac_vb2_queue_setup(struct vb2_queue *vq,
+				unsigned int *nbufs,
+				unsigned int *num_planes, unsigned int sizes[],
+				struct device *alloc_devs[])
+{
+	struct pxa_camera_dev *pcdev = vb2_get_drv_priv(vq);
+	int size = pcdev->current_pix.sizeimage;
+
+	dev_dbg(pcdev_to_dev(pcdev),
+		 "%s(vq=%p nbufs=%d num_planes=%d size=%d)\n",
+		__func__, vq, *nbufs, *num_planes, size);
+	/*
+	 * Called from VIDIOC_REQBUFS or in compatibility mode For YUV422P
+	 * format, even if there are 3 planes Y, U and V, we reply there is only
+	 * one plane, containing Y, U and V data, one after the other.
+	 */
+	if (*num_planes)
+		return sizes[0] < size ? -EINVAL : 0;
+
+	*num_planes = 1;
+	switch (pcdev->channels) {
+	case 1:
+	case 3:
+		sizes[0] = size;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	if (!*nbufs)
+		*nbufs = 1;
+
+	return 0;
+}
+
+static int pxac_vb2_start_streaming(struct vb2_queue *vq, unsigned int count)
+{
+	struct pxa_camera_dev *pcdev = vb2_get_drv_priv(vq);
+
+	dev_dbg(pcdev_to_dev(pcdev), "%s(count=%d) active=%p\n",
+		__func__, count, pcdev->active);
+
+	pcdev->buf_sequence = 0;
+	if (!pcdev->active)
+		pxa_camera_start_capture(pcdev);
+
+	return 0;
+}
+
+static void pxac_vb2_stop_streaming(struct vb2_queue *vq)
+{
+	struct pxa_camera_dev *pcdev = vb2_get_drv_priv(vq);
+	struct pxa_buffer *buf, *tmp;
+
+	dev_dbg(pcdev_to_dev(pcdev), "%s active=%p\n",
+		__func__, pcdev->active);
+	pxa_camera_stop_capture(pcdev);
+
+	list_for_each_entry_safe(buf, tmp, &pcdev->capture, queue)
+		pxa_camera_wakeup(pcdev, buf, VB2_BUF_STATE_ERROR);
+}
+
+static struct vb2_ops pxac_vb2_ops = {
+	.queue_setup		= pxac_vb2_queue_setup,
+	.buf_init		= pxac_vb2_init,
+	.buf_prepare		= pxac_vb2_prepare,
+	.buf_queue		= pxac_vb2_queue,
+	.buf_cleanup		= pxac_vb2_cleanup,
+	.start_streaming	= pxac_vb2_start_streaming,
+	.stop_streaming		= pxac_vb2_stop_streaming,
+	.wait_prepare		= vb2_ops_wait_prepare,
+	.wait_finish		= vb2_ops_wait_finish,
+};
+
+static int pxa_camera_init_videobuf2(struct pxa_camera_dev *pcdev)
+{
+	int ret;
+	struct vb2_queue *vq = &pcdev->vb2_vq;
+
+	memset(vq, 0, sizeof(*vq));
+	vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
+	vq->drv_priv = pcdev;
+	vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	vq->buf_struct_size = sizeof(struct pxa_buffer);
+	vq->dev = pcdev->v4l2_dev.dev;
+
+	vq->ops = &pxac_vb2_ops;
+	vq->mem_ops = &vb2_dma_sg_memops;
+	vq->lock = &pcdev->mlock;
+
+	ret = vb2_queue_init(vq);
+	dev_dbg(pcdev_to_dev(pcdev),
+		 "vb2_queue_init(vq=%p): %d\n", vq, ret);
+
+	return ret;
+}
+
+/*
+ * Video ioctls section
+ */
+static int pxa_camera_set_bus_param(struct pxa_camera_dev *pcdev)
 {
-	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
-	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
-	struct pxa_camera_dev *pcdev = ici->priv;
 	struct v4l2_mbus_config cfg = {.type = V4L2_MBUS_PARALLEL,};
-	u32 pixfmt = icd->current_fmt->host_fmt->fourcc;
+	u32 pixfmt = pcdev->current_fmt->host_fmt->fourcc;
 	unsigned long bus_flags, common_flags;
 	int ret;
-	struct pxa_cam *cam = icd->host_priv;
 
-	ret = test_platform_param(pcdev, icd->current_fmt->host_fmt->bits_per_sample,
+	ret = test_platform_param(pcdev,
+				  pcdev->current_fmt->host_fmt->bits_per_sample,
 				  &bus_flags);
 	if (ret < 0)
 		return ret;
 
-	ret = v4l2_subdev_call(sd, video, g_mbus_config, &cfg);
+	ret = sensor_call(pcdev, video, g_mbus_config, &cfg);
 	if (!ret) {
-		common_flags = soc_mbus_config_compatible(&cfg,
+		common_flags = pxa_mbus_config_compatible(&cfg,
 							  bus_flags);
 		if (!common_flags) {
-			dev_warn(icd->parent,
+			dev_warn(pcdev_to_dev(pcdev),
 				 "Flags incompatible: camera 0x%x, host 0x%lx\n",
 				 cfg.flags, bus_flags);
 			return -EINVAL;
@@ -1124,26 +1626,22 @@ static int pxa_camera_set_bus_param(struct soc_camera_device *icd)
 	}
 
 	cfg.flags = common_flags;
-	ret = v4l2_subdev_call(sd, video, s_mbus_config, &cfg);
+	ret = sensor_call(pcdev, video, s_mbus_config, &cfg);
 	if (ret < 0 && ret != -ENOIOCTLCMD) {
-		dev_dbg(icd->parent, "camera s_mbus_config(0x%lx) returned %d\n",
+		dev_dbg(pcdev_to_dev(pcdev),
+			"camera s_mbus_config(0x%lx) returned %d\n",
 			common_flags, ret);
 		return ret;
 	}
 
-	cam->flags = common_flags;
-
-	pxa_camera_setup_cicr(icd, common_flags, pixfmt);
+	pxa_camera_setup_cicr(pcdev, common_flags, pixfmt);
 
 	return 0;
 }
 
-static int pxa_camera_try_bus_param(struct soc_camera_device *icd,
+static int pxa_camera_try_bus_param(struct pxa_camera_dev *pcdev,
 				    unsigned char buswidth)
 {
-	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
-	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
-	struct pxa_camera_dev *pcdev = ici->priv;
 	struct v4l2_mbus_config cfg = {.type = V4L2_MBUS_PARALLEL,};
 	unsigned long bus_flags, common_flags;
 	int ret = test_platform_param(pcdev, buswidth, &bus_flags);
@@ -1151,12 +1649,12 @@ static int pxa_camera_try_bus_param(struct soc_camera_device *icd,
 	if (ret < 0)
 		return ret;
 
-	ret = v4l2_subdev_call(sd, video, g_mbus_config, &cfg);
+	ret = sensor_call(pcdev, video, g_mbus_config, &cfg);
 	if (!ret) {
-		common_flags = soc_mbus_config_compatible(&cfg,
+		common_flags = pxa_mbus_config_compatible(&cfg,
 							  bus_flags);
 		if (!common_flags) {
-			dev_warn(icd->parent,
+			dev_warn(pcdev_to_dev(pcdev),
 				 "Flags incompatible: camera 0x%x, host 0x%lx\n",
 				 cfg.flags, bus_flags);
 			return -EINVAL;
@@ -1168,66 +1666,56 @@ static int pxa_camera_try_bus_param(struct soc_camera_device *icd,
 	return ret;
 }
 
-static const struct soc_mbus_pixelfmt pxa_camera_formats[] = {
+static const struct pxa_mbus_pixelfmt pxa_camera_formats[] = {
 	{
 		.fourcc			= V4L2_PIX_FMT_YUV422P,
 		.name			= "Planar YUV422 16 bit",
 		.bits_per_sample	= 8,
-		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
-		.order			= SOC_MBUS_ORDER_LE,
-		.layout			= SOC_MBUS_LAYOUT_PLANAR_2Y_U_V,
+		.packing		= PXA_MBUS_PACKING_2X8_PADHI,
+		.order			= PXA_MBUS_ORDER_LE,
+		.layout			= PXA_MBUS_LAYOUT_PLANAR_2Y_U_V,
 	},
 };
 
 /* This will be corrected as we get more formats */
-static bool pxa_camera_packing_supported(const struct soc_mbus_pixelfmt *fmt)
+static bool pxa_camera_packing_supported(const struct pxa_mbus_pixelfmt *fmt)
 {
-	return	fmt->packing == SOC_MBUS_PACKING_NONE ||
+	return	fmt->packing == PXA_MBUS_PACKING_NONE ||
 		(fmt->bits_per_sample == 8 &&
-		 fmt->packing == SOC_MBUS_PACKING_2X8_PADHI) ||
+		 fmt->packing == PXA_MBUS_PACKING_2X8_PADHI) ||
 		(fmt->bits_per_sample > 8 &&
-		 fmt->packing == SOC_MBUS_PACKING_EXTEND16);
+		 fmt->packing == PXA_MBUS_PACKING_EXTEND16);
 }
 
-static int pxa_camera_get_formats(struct soc_camera_device *icd, unsigned int idx,
+static int pxa_camera_get_formats(struct v4l2_device *v4l2_dev,
+				  unsigned int idx,
 				  struct soc_camera_format_xlate *xlate)
 {
-	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
-	struct device *dev = icd->parent;
+	struct pxa_camera_dev *pcdev = v4l2_dev_to_pcdev(v4l2_dev);
 	int formats = 0, ret;
-	struct pxa_cam *cam;
 	struct v4l2_subdev_mbus_code_enum code = {
 		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
 		.index = idx,
 	};
-	const struct soc_mbus_pixelfmt *fmt;
+	const struct pxa_mbus_pixelfmt *fmt;
 
-	ret = v4l2_subdev_call(sd, pad, enum_mbus_code, NULL, &code);
+	ret = sensor_call(pcdev, pad, enum_mbus_code, NULL, &code);
 	if (ret < 0)
 		/* No more formats */
 		return 0;
 
-	fmt = soc_mbus_get_fmtdesc(code.code);
+	fmt = pxa_mbus_get_fmtdesc(code.code);
 	if (!fmt) {
-		dev_err(dev, "Invalid format code #%u: %d\n", idx, code.code);
+		dev_err(pcdev_to_dev(pcdev),
+			"Invalid format code #%u: %d\n", idx, code.code);
 		return 0;
 	}
 
 	/* This also checks support for the requested bits-per-sample */
-	ret = pxa_camera_try_bus_param(icd, fmt->bits_per_sample);
+	ret = pxa_camera_try_bus_param(pcdev, fmt->bits_per_sample);
 	if (ret < 0)
 		return 0;
 
-	if (!icd->host_priv) {
-		cam = kzalloc(sizeof(*cam), GFP_KERNEL);
-		if (!cam)
-			return -ENOMEM;
-
-		icd->host_priv = cam;
-	} else {
-		cam = icd->host_priv;
-	}
-
 	switch (code.code) {
 	case MEDIA_BUS_FMT_UYVY8_2X8:
 		formats++;
@@ -1235,25 +1723,29 @@ static int pxa_camera_get_formats(struct soc_camera_device *icd, unsigned int id
 			xlate->host_fmt	= &pxa_camera_formats[0];
 			xlate->code	= code.code;
 			xlate++;
-			dev_dbg(dev, "Providing format %s using code %d\n",
+			dev_dbg(pcdev_to_dev(pcdev),
+				"Providing format %s using code %d\n",
 				pxa_camera_formats[0].name, code.code);
 		}
+	/* fall through */
 	case MEDIA_BUS_FMT_VYUY8_2X8:
 	case MEDIA_BUS_FMT_YUYV8_2X8:
 	case MEDIA_BUS_FMT_YVYU8_2X8:
 	case MEDIA_BUS_FMT_RGB565_2X8_LE:
 	case MEDIA_BUS_FMT_RGB555_2X8_PADHI_LE:
 		if (xlate)
-			dev_dbg(dev, "Providing format %s packed\n",
+			dev_dbg(pcdev_to_dev(pcdev),
+				"Providing format %s packed\n",
 				fmt->name);
 		break;
 	default:
 		if (!pxa_camera_packing_supported(fmt))
 			return 0;
 		if (xlate)
-			dev_dbg(dev,
+			dev_dbg(pcdev_to_dev(pcdev),
 				"Providing format %s in pass-through mode\n",
 				fmt->name);
+		break;
 	}
 
 	/* Generic pass-through */
@@ -1267,10 +1759,22 @@ static int pxa_camera_get_formats(struct soc_camera_device *icd, unsigned int id
 	return formats;
 }
 
-static void pxa_camera_put_formats(struct soc_camera_device *icd)
+static int pxa_camera_build_formats(struct pxa_camera_dev *pcdev)
 {
-	kfree(icd->host_priv);
-	icd->host_priv = NULL;
+	struct soc_camera_format_xlate *xlate;
+
+	xlate = pxa_mbus_build_fmts_xlate(&pcdev->v4l2_dev, pcdev->sensor,
+					  pxa_camera_get_formats);
+	if (IS_ERR(xlate))
+		return PTR_ERR(xlate);
+
+	pcdev->user_formats = xlate;
+	return 0;
+}
+
+static void pxa_camera_destroy_formats(struct pxa_camera_dev *pcdev)
+{
+	kfree(pcdev->user_formats);
 }
 
 static int pxa_camera_check_frame(u32 width, u32 height)
@@ -1280,158 +1784,72 @@ static int pxa_camera_check_frame(u32 width, u32 height)
 		(width & 0x01);
 }
 
-static int pxa_camera_set_crop(struct soc_camera_device *icd,
-			       const struct v4l2_crop *a)
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int pxac_vidioc_g_register(struct file *file, void *priv,
+				  struct v4l2_dbg_register *reg)
 {
-	const struct v4l2_rect *rect = &a->c;
-	struct device *dev = icd->parent;
-	struct soc_camera_host *ici = to_soc_camera_host(dev);
-	struct pxa_camera_dev *pcdev = ici->priv;
-	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
-	struct soc_camera_sense sense = {
-		.master_clock = pcdev->mclk,
-		.pixel_clock_max = pcdev->ciclk / 4,
-	};
-	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;
-
-	/* If PCLK is used to latch data from the sensor, check sense */
-	if (pcdev->platform_flags & PXA_CAMERA_PCLK_EN)
-		icd->sense = &sense;
-
-	ret = v4l2_subdev_call(sd, video, s_crop, a);
-
-	icd->sense = NULL;
-
-	if (ret < 0) {
-		dev_warn(dev, "Failed to crop to %ux%u@%u:%u\n",
-			 rect->width, rect->height, rect->left, rect->top);
-		return ret;
-	}
+	struct pxa_camera_dev *pcdev = video_drvdata(file);
 
-	ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, &fmt);
-	if (ret < 0)
-		return ret;
-
-	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,
-			fourcc == V4L2_PIX_FMT_YUV422P ? 4 : 0);
-		ret = v4l2_subdev_call(sd, pad, set_fmt, NULL, &fmt);
-		if (ret < 0)
-			return ret;
+	if (reg->reg > CIBR2)
+		return -ERANGE;
 
-		if (pxa_camera_check_frame(mf->width, mf->height)) {
-			dev_warn(icd->parent,
-				 "Inconsistent state. Use S_FMT to repair\n");
-			return -EINVAL;
-		}
-	}
-
-	if (sense.flags & SOCAM_SENSE_PCLK_CHANGED) {
-		if (sense.pixel_clock > sense.pixel_clock_max) {
-			dev_err(dev,
-				"pixel clock %lu set by the camera too high!",
-				sense.pixel_clock);
-			return -EIO;
-		}
-		recalculate_fifo_timeout(pcdev, sense.pixel_clock);
-	}
-
-	icd->user_width		= mf->width;
-	icd->user_height	= mf->height;
-
-	pxa_camera_setup_cicr(icd, cam->flags, fourcc);
-
-	return ret;
+	reg->val = __raw_readl(pcdev->base + reg->reg);
+	reg->size = sizeof(__u32);
+	return 0;
 }
 
-static int pxa_camera_set_fmt(struct soc_camera_device *icd,
-			      struct v4l2_format *f)
+static int pxac_vidioc_s_register(struct file *file, void *priv,
+				  const struct v4l2_dbg_register *reg)
 {
-	struct device *dev = icd->parent;
-	struct soc_camera_host *ici = to_soc_camera_host(dev);
-	struct pxa_camera_dev *pcdev = ici->priv;
-	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
-	const struct soc_camera_format_xlate *xlate = NULL;
-	struct soc_camera_sense sense = {
-		.master_clock = pcdev->mclk,
-		.pixel_clock_max = pcdev->ciclk / 4,
-	};
-	struct v4l2_pix_format *pix = &f->fmt.pix;
-	struct v4l2_subdev_format format = {
-		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
-	};
-	struct v4l2_mbus_framefmt *mf = &format.format;
-	int ret;
+	struct pxa_camera_dev *pcdev = video_drvdata(file);
 
-	xlate = soc_camera_xlate_by_fourcc(icd, pix->pixelformat);
-	if (!xlate) {
-		dev_warn(dev, "Format %x not found\n", pix->pixelformat);
+	if (reg->reg > CIBR2)
+		return -ERANGE;
+	if (reg->size != sizeof(__u32))
 		return -EINVAL;
-	}
-
-	/* If PCLK is used to latch data from the sensor, check sense */
-	if (pcdev->platform_flags & PXA_CAMERA_PCLK_EN)
-		/* 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;
+	__raw_writel(reg->val, pcdev->base + reg->reg);
+	return 0;
+}
+#endif
 
-	ret = v4l2_subdev_call(sd, pad, set_fmt, NULL, &format);
+static int pxac_vidioc_enum_fmt_vid_cap(struct file *filp, void  *priv,
+					struct v4l2_fmtdesc *f)
+{
+	struct pxa_camera_dev *pcdev = video_drvdata(filp);
+	const struct pxa_mbus_pixelfmt *format;
+	unsigned int idx;
 
-	if (mf->code != xlate->code)
+	for (idx = 0; pcdev->user_formats[idx].code; idx++);
+	if (f->index >= idx)
 		return -EINVAL;
 
-	icd->sense = NULL;
-
-	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)) {
-		dev_warn(dev,
-			 "Camera driver produced an unsupported frame %dx%d\n",
-			 mf->width, mf->height);
-		ret = -EINVAL;
-	} else if (sense.flags & SOCAM_SENSE_PCLK_CHANGED) {
-		if (sense.pixel_clock > sense.pixel_clock_max) {
-			dev_err(dev,
-				"pixel clock %lu set by the camera too high!",
-				sense.pixel_clock);
-			return -EIO;
-		}
-		recalculate_fifo_timeout(pcdev, sense.pixel_clock);
-	}
-
-	if (ret < 0)
-		return ret;
+	format = pcdev->user_formats[f->index].host_fmt;
+	f->pixelformat = format->fourcc;
+	return 0;
+}
 
-	pix->width		= mf->width;
-	pix->height		= mf->height;
-	pix->field		= mf->field;
-	pix->colorspace		= mf->colorspace;
-	icd->current_fmt	= xlate;
+static int pxac_vidioc_g_fmt_vid_cap(struct file *filp, void *priv,
+				    struct v4l2_format *f)
+{
+	struct pxa_camera_dev *pcdev = video_drvdata(filp);
+	struct v4l2_pix_format *pix = &f->fmt.pix;
 
-	return ret;
+	pix->width		= pcdev->current_pix.width;
+	pix->height		= pcdev->current_pix.height;
+	pix->bytesperline	= pcdev->current_pix.bytesperline;
+	pix->sizeimage		= pcdev->current_pix.sizeimage;
+	pix->field		= pcdev->current_pix.field;
+	pix->pixelformat	= pcdev->current_fmt->host_fmt->fourcc;
+	pix->colorspace		= pcdev->current_pix.colorspace;
+	dev_dbg(pcdev_to_dev(pcdev), "current_fmt->fourcc: 0x%08x\n",
+		pcdev->current_fmt->host_fmt->fourcc);
+	return 0;
 }
 
-static int pxa_camera_try_fmt(struct soc_camera_device *icd,
-			      struct v4l2_format *f)
+static int pxac_vidioc_try_fmt_vid_cap(struct file *filp, void *priv,
+				      struct v4l2_format *f)
 {
-	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	struct pxa_camera_dev *pcdev = video_drvdata(filp);
 	const struct soc_camera_format_xlate *xlate;
 	struct v4l2_pix_format *pix = &f->fmt.pix;
 	struct v4l2_subdev_pad_config pad_cfg;
@@ -1442,9 +1860,9 @@ static int pxa_camera_try_fmt(struct soc_camera_device *icd,
 	__u32 pixfmt = pix->pixelformat;
 	int ret;
 
-	xlate = soc_camera_xlate_by_fourcc(icd, pixfmt);
+	xlate = pxa_mbus_xlate_by_fourcc(pcdev->user_formats, pixfmt);
 	if (!xlate) {
-		dev_warn(icd->parent, "Format %x not found\n", pixfmt);
+		dev_warn(pcdev_to_dev(pcdev), "Format %x not found\n", pixfmt);
 		return -EINVAL;
 	}
 
@@ -1458,90 +1876,311 @@ static int pxa_camera_try_fmt(struct soc_camera_device *icd,
 			      &pix->height, 32, 2048, 0,
 			      pixfmt == V4L2_PIX_FMT_YUV422P ? 4 : 0);
 
-	/* limit to sensor capabilities */
-	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;
-
-	ret = v4l2_subdev_call(sd, pad, set_fmt, &pad_cfg, &format);
+	v4l2_fill_mbus_format(mf, pix, xlate->code);
+	ret = sensor_call(pcdev, pad, set_fmt, &pad_cfg, &format);
 	if (ret < 0)
 		return ret;
 
-	pix->width	= mf->width;
-	pix->height	= mf->height;
-	pix->colorspace	= mf->colorspace;
+	v4l2_fill_pix_format(pix, mf);
 
+	/* Only progressive video supported so far */
 	switch (mf->field) {
 	case V4L2_FIELD_ANY:
 	case V4L2_FIELD_NONE:
-		pix->field	= V4L2_FIELD_NONE;
+		pix->field = V4L2_FIELD_NONE;
 		break;
 	default:
 		/* TODO: support interlaced at least in pass-through mode */
-		dev_err(icd->parent, "Field type %d unsupported.\n",
+		dev_err(pcdev_to_dev(pcdev), "Field type %d unsupported.\n",
 			mf->field);
 		return -EINVAL;
 	}
 
-	return ret;
+	ret = pxa_mbus_bytes_per_line(pix->width, xlate->host_fmt);
+	if (ret < 0)
+		return ret;
+
+	pix->bytesperline = ret;
+	ret = pxa_mbus_image_size(xlate->host_fmt, pix->bytesperline,
+				  pix->height);
+	if (ret < 0)
+		return ret;
+
+	pix->sizeimage = ret;
+	return 0;
 }
 
-static int pxa_camera_reqbufs(struct soc_camera_device *icd,
-			      struct v4l2_requestbuffers *p)
+static int pxac_vidioc_s_fmt_vid_cap(struct file *filp, void *priv,
+				    struct v4l2_format *f)
 {
-	int i;
+	struct pxa_camera_dev *pcdev = video_drvdata(filp);
+	const struct soc_camera_format_xlate *xlate;
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	unsigned long flags;
+	int ret, is_busy;
 
-	/*
-	 * This is for locking debugging only. I removed spinlocks and now I
-	 * check whether .prepare is ever called on a linked buffer, or whether
-	 * a dma IRQ can occur for an in-work or unlinked buffer. Until now
-	 * it hadn't triggered
-	 */
-	for (i = 0; i < p->count; i++) {
-		struct pxa_buffer *buf = container_of(icd->vb_vidq.bufs[i],
-						      struct pxa_buffer, vb);
-		buf->inwork = 0;
-		INIT_LIST_HEAD(&buf->vb.queue);
+	dev_dbg(pcdev_to_dev(pcdev),
+		"s_fmt_vid_cap(pix=%dx%d:%x)\n",
+		pix->width, pix->height, pix->pixelformat);
+
+	spin_lock_irqsave(&pcdev->lock, flags);
+	is_busy = pcdev->active || vb2_is_busy(&pcdev->vb2_vq);
+	spin_unlock_irqrestore(&pcdev->lock, flags);
+
+	if (is_busy)
+		return -EBUSY;
+
+	ret = pxac_vidioc_try_fmt_vid_cap(filp, priv, f);
+	if (ret)
+		return ret;
+
+	xlate = pxa_mbus_xlate_by_fourcc(pcdev->user_formats,
+					 pix->pixelformat);
+	v4l2_fill_mbus_format(&format.format, pix, xlate->code);
+	ret = sensor_call(pcdev, pad, set_fmt, NULL, &format);
+	if (ret < 0) {
+		dev_warn(pcdev_to_dev(pcdev),
+			 "Failed to configure for format %x\n",
+			 pix->pixelformat);
+	} else if (pxa_camera_check_frame(pix->width, pix->height)) {
+		dev_warn(pcdev_to_dev(pcdev),
+			 "Camera driver produced an unsupported frame %dx%d\n",
+			 pix->width, pix->height);
+		return -EINVAL;
 	}
 
+	pcdev->current_fmt = xlate;
+	pcdev->current_pix = *pix;
+
+	ret = pxa_camera_set_bus_param(pcdev);
+	return ret;
+}
+
+static int pxac_vidioc_querycap(struct file *file, void *priv,
+				struct v4l2_capability *cap)
+{
+	strlcpy(cap->bus_info, "platform:pxa-camera", sizeof(cap->bus_info));
+	strlcpy(cap->driver, PXA_CAM_DRV_NAME, sizeof(cap->driver));
+	strlcpy(cap->card, pxa_cam_driver_description, sizeof(cap->card));
+	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+
 	return 0;
 }
 
-static unsigned int pxa_camera_poll(struct file *file, poll_table *pt)
+static int pxac_vidioc_enum_input(struct file *file, void *priv,
+				  struct v4l2_input *i)
 {
-	struct soc_camera_device *icd = file->private_data;
-	struct pxa_buffer *buf;
+	if (i->index > 0)
+		return -EINVAL;
 
-	buf = list_entry(icd->vb_vidq.stream.next, struct pxa_buffer,
-			 vb.stream);
+	i->type = V4L2_INPUT_TYPE_CAMERA;
+	strlcpy(i->name, "Camera", sizeof(i->name));
 
-	poll_wait(file, &buf->vb.done, pt);
+	return 0;
+}
 
-	if (buf->vb.state == VIDEOBUF_DONE ||
-	    buf->vb.state == VIDEOBUF_ERROR)
-		return POLLIN|POLLRDNORM;
+static int pxac_vidioc_g_input(struct file *file, void *priv, unsigned int *i)
+{
+	*i = 0;
 
 	return 0;
 }
 
-static int pxa_camera_querycap(struct soc_camera_host *ici,
-			       struct v4l2_capability *cap)
+static int pxac_vidioc_s_input(struct file *file, void *priv, unsigned int i)
 {
-	/* cap->name is set by the firendly caller:-> */
-	strlcpy(cap->card, pxa_cam_driver_description, sizeof(cap->card));
-	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
-	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+	if (i > 0)
+		return -EINVAL;
 
 	return 0;
 }
 
+static int pxac_fops_camera_open(struct file *filp)
+{
+	struct pxa_camera_dev *pcdev = video_drvdata(filp);
+	int ret;
+
+	mutex_lock(&pcdev->mlock);
+	ret = v4l2_fh_open(filp);
+	if (ret < 0)
+		goto out;
+
+	ret = sensor_call(pcdev, core, s_power, 1);
+	if (ret)
+		v4l2_fh_release(filp);
+out:
+	mutex_unlock(&pcdev->mlock);
+	return ret;
+}
+
+static int pxac_fops_camera_release(struct file *filp)
+{
+	struct pxa_camera_dev *pcdev = video_drvdata(filp);
+	int ret;
+
+	ret = vb2_fop_release(filp);
+	if (ret < 0)
+		return ret;
+
+	mutex_lock(&pcdev->mlock);
+	ret = sensor_call(pcdev, core, s_power, 0);
+	mutex_unlock(&pcdev->mlock);
+
+	return ret;
+}
+
+static const struct v4l2_file_operations pxa_camera_fops = {
+	.owner		= THIS_MODULE,
+	.open		= pxac_fops_camera_open,
+	.release	= pxac_fops_camera_release,
+	.read		= vb2_fop_read,
+	.poll		= vb2_fop_poll,
+	.mmap		= vb2_fop_mmap,
+	.unlocked_ioctl = video_ioctl2,
+};
+
+static const struct v4l2_ioctl_ops pxa_camera_ioctl_ops = {
+	.vidioc_querycap		= pxac_vidioc_querycap,
+
+	.vidioc_enum_input		= pxac_vidioc_enum_input,
+	.vidioc_g_input			= pxac_vidioc_g_input,
+	.vidioc_s_input			= pxac_vidioc_s_input,
+
+	.vidioc_enum_fmt_vid_cap	= pxac_vidioc_enum_fmt_vid_cap,
+	.vidioc_g_fmt_vid_cap		= pxac_vidioc_g_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap		= pxac_vidioc_s_fmt_vid_cap,
+	.vidioc_try_fmt_vid_cap		= pxac_vidioc_try_fmt_vid_cap,
+
+	.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,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.vidioc_g_register		= pxac_vidioc_g_register,
+	.vidioc_s_register		= pxac_vidioc_s_register,
+#endif
+};
+
+static struct v4l2_clk_ops pxa_camera_mclk_ops = {
+};
+
+static const struct video_device pxa_camera_videodev_template = {
+	.name = "pxa-camera",
+	.minor = -1,
+	.fops = &pxa_camera_fops,
+	.ioctl_ops = &pxa_camera_ioctl_ops,
+	.release = video_device_release_empty,
+	.device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING,
+};
+
+static int pxa_camera_sensor_bound(struct v4l2_async_notifier *notifier,
+		     struct v4l2_subdev *subdev,
+		     struct v4l2_async_subdev *asd)
+{
+	int err;
+	struct v4l2_device *v4l2_dev = notifier->v4l2_dev;
+	struct pxa_camera_dev *pcdev = v4l2_dev_to_pcdev(v4l2_dev);
+	struct video_device *vdev = &pcdev->vdev;
+	struct v4l2_pix_format *pix = &pcdev->current_pix;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	struct v4l2_mbus_framefmt *mf = &format.format;
+
+	dev_info(pcdev_to_dev(pcdev), "%s(): trying to bind a device\n",
+		 __func__);
+	mutex_lock(&pcdev->mlock);
+	*vdev = pxa_camera_videodev_template;
+	vdev->v4l2_dev = v4l2_dev;
+	vdev->lock = &pcdev->mlock;
+	pcdev->sensor = subdev;
+	pcdev->vdev.queue = &pcdev->vb2_vq;
+	pcdev->vdev.v4l2_dev = &pcdev->v4l2_dev;
+	pcdev->vdev.ctrl_handler = subdev->ctrl_handler;
+	video_set_drvdata(&pcdev->vdev, pcdev);
+
+	err = pxa_camera_build_formats(pcdev);
+	if (err) {
+		dev_err(pcdev_to_dev(pcdev), "building formats failed: %d\n",
+			err);
+		goto out;
+	}
+
+	pcdev->current_fmt = pcdev->user_formats;
+	pix->field = V4L2_FIELD_NONE;
+	pix->width = DEFAULT_WIDTH;
+	pix->height = DEFAULT_HEIGHT;
+	pix->bytesperline =
+		pxa_mbus_bytes_per_line(pix->width,
+					pcdev->current_fmt->host_fmt);
+	pix->sizeimage =
+		pxa_mbus_image_size(pcdev->current_fmt->host_fmt,
+				    pix->bytesperline, pix->height);
+	pix->pixelformat = pcdev->current_fmt->host_fmt->fourcc;
+	v4l2_fill_mbus_format(mf, pix, pcdev->current_fmt->code);
+	err = sensor_call(pcdev, pad, set_fmt, NULL, &format);
+	if (err)
+		goto out;
+
+	v4l2_fill_pix_format(pix, mf);
+	pr_info("%s(): colorspace=0x%x pixfmt=0x%x\n",
+		__func__, pix->colorspace, pix->pixelformat);
+
+	err = pxa_camera_init_videobuf2(pcdev);
+	if (err)
+		goto out;
+
+	err = video_register_device(&pcdev->vdev, VFL_TYPE_GRABBER, -1);
+	if (err) {
+		v4l2_err(v4l2_dev, "register video device failed: %d\n", err);
+		pcdev->sensor = NULL;
+	} else {
+		dev_info(pcdev_to_dev(pcdev),
+			 "PXA Camera driver attached to camera %s\n",
+			 subdev->name);
+	}
+out:
+	mutex_unlock(&pcdev->mlock);
+	return err;
+}
+
+static void pxa_camera_sensor_unbind(struct v4l2_async_notifier *notifier,
+		     struct v4l2_subdev *subdev,
+		     struct v4l2_async_subdev *asd)
+{
+	struct pxa_camera_dev *pcdev = v4l2_dev_to_pcdev(notifier->v4l2_dev);
+
+	mutex_lock(&pcdev->mlock);
+	dev_info(pcdev_to_dev(pcdev),
+		 "PXA Camera driver detached from camera %s\n",
+		 subdev->name);
+
+	/* disable capture, disable interrupts */
+	__raw_writel(0x3ff, pcdev->base + CICR0);
+
+	/* Stop DMA engine */
+	pxa_dma_stop_channels(pcdev);
+
+	pxa_camera_destroy_formats(pcdev);
+	video_unregister_device(&pcdev->vdev);
+	pcdev->sensor = NULL;
+
+	mutex_unlock(&pcdev->mlock);
+}
+
+/*
+ * Driver probe, remove, suspend and resume operations
+ */
 static int pxa_camera_suspend(struct device *dev)
 {
-	struct soc_camera_host *ici = to_soc_camera_host(dev);
-	struct pxa_camera_dev *pcdev = ici->priv;
+	struct pxa_camera_dev *pcdev = dev_get_drvdata(dev);
 	int i = 0, ret = 0;
 
 	pcdev->save_cicr[i++] = __raw_readl(pcdev->base + CICR0);
@@ -1550,9 +2189,8 @@ static int pxa_camera_suspend(struct device *dev)
 	pcdev->save_cicr[i++] = __raw_readl(pcdev->base + CICR3);
 	pcdev->save_cicr[i++] = __raw_readl(pcdev->base + CICR4);
 
-	if (pcdev->soc_host.icd) {
-		struct v4l2_subdev *sd = soc_camera_to_subdev(pcdev->soc_host.icd);
-		ret = v4l2_subdev_call(sd, core, s_power, 0);
+	if (pcdev->sensor) {
+		ret = sensor_call(pcdev, core, s_power, 0);
 		if (ret == -ENOIOCTLCMD)
 			ret = 0;
 	}
@@ -1562,8 +2200,7 @@ static int pxa_camera_suspend(struct device *dev)
 
 static int pxa_camera_resume(struct device *dev)
 {
-	struct soc_camera_host *ici = to_soc_camera_host(dev);
-	struct pxa_camera_dev *pcdev = ici->priv;
+	struct pxa_camera_dev *pcdev = dev_get_drvdata(dev);
 	int i = 0, ret = 0;
 
 	__raw_writel(pcdev->save_cicr[i++] & ~CICR0_ENB, pcdev->base + CICR0);
@@ -1572,9 +2209,8 @@ static int pxa_camera_resume(struct device *dev)
 	__raw_writel(pcdev->save_cicr[i++], pcdev->base + CICR3);
 	__raw_writel(pcdev->save_cicr[i++], pcdev->base + CICR4);
 
-	if (pcdev->soc_host.icd) {
-		struct v4l2_subdev *sd = soc_camera_to_subdev(pcdev->soc_host.icd);
-		ret = v4l2_subdev_call(sd, core, s_power, 1);
+	if (pcdev->sensor) {
+		ret = sensor_call(pcdev, core, s_power, 1);
 		if (ret == -ENOIOCTLCMD)
 			ret = 0;
 	}
@@ -1586,29 +2222,12 @@ static int pxa_camera_resume(struct device *dev)
 	return ret;
 }
 
-static struct soc_camera_host_ops pxa_soc_camera_host_ops = {
-	.owner		= THIS_MODULE,
-	.add		= pxa_camera_add_device,
-	.remove		= pxa_camera_remove_device,
-	.clock_start	= pxa_camera_clock_start,
-	.clock_stop	= pxa_camera_clock_stop,
-	.set_crop	= pxa_camera_set_crop,
-	.get_formats	= pxa_camera_get_formats,
-	.put_formats	= pxa_camera_put_formats,
-	.set_fmt	= pxa_camera_set_fmt,
-	.try_fmt	= pxa_camera_try_fmt,
-	.init_videobuf	= pxa_camera_init_videobuf,
-	.reqbufs	= pxa_camera_reqbufs,
-	.poll		= pxa_camera_poll,
-	.querycap	= pxa_camera_querycap,
-	.set_bus_param	= pxa_camera_set_bus_param,
-};
-
 static int pxa_camera_pdata_from_dt(struct device *dev,
-				    struct pxa_camera_dev *pcdev)
+				    struct pxa_camera_dev *pcdev,
+				    struct v4l2_async_subdev *asd)
 {
 	u32 mclk_rate;
-	struct device_node *np = dev->of_node;
+	struct device_node *remote, *np = dev->of_node;
 	struct v4l2_of_endpoint ep;
 	int err = of_property_read_u32(np, "clock-frequency",
 				       &mclk_rate);
@@ -1660,6 +2279,15 @@ static int pxa_camera_pdata_from_dt(struct device *dev,
 	if (ep.bus.parallel.flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)
 		pcdev->platform_flags |= PXA_CAMERA_PCLK_EN;
 
+	asd->match_type = V4L2_ASYNC_MATCH_OF;
+	remote = of_graph_get_remote_port(np);
+	if (remote) {
+		asd->match.of.node = remote;
+		of_node_put(remote);
+	} else {
+		dev_notice(dev, "no remote for %s\n", of_node_full_name(np));
+	}
+
 out:
 	of_node_put(np);
 
@@ -1678,6 +2306,7 @@ static int pxa_camera_probe(struct platform_device *pdev)
 	};
 	dma_cap_mask_t mask;
 	struct pxad_param params;
+	char clk_name[V4L2_CLK_NAME_SIZE];
 	int irq;
 	int err = 0, i;
 
@@ -1700,10 +2329,14 @@ static int pxa_camera_probe(struct platform_device *pdev)
 
 	pcdev->pdata = pdev->dev.platform_data;
 	if (&pdev->dev.of_node && !pcdev->pdata) {
-		err = pxa_camera_pdata_from_dt(&pdev->dev, pcdev);
+		err = pxa_camera_pdata_from_dt(&pdev->dev, pcdev, &pcdev->asd);
 	} else {
 		pcdev->platform_flags = pcdev->pdata->flags;
 		pcdev->mclk = pcdev->pdata->mclk_10khz * 10000;
+		pcdev->asd.match_type = V4L2_ASYNC_MATCH_I2C;
+		pcdev->asd.match.i2c.adapter_id =
+			pcdev->pdata->sensor_i2c_adapter_id;
+		pcdev->asd.match.i2c.address = pcdev->pdata->sensor_i2c_address;
 	}
 	if (err < 0)
 		return err;
@@ -1735,6 +2368,7 @@ static int pxa_camera_probe(struct platform_device *pdev)
 
 	INIT_LIST_HEAD(&pcdev->capture);
 	spin_lock_init(&pcdev->lock);
+	mutex_init(&pcdev->mlock);
 
 	/*
 	 * Request the regions.
@@ -1767,6 +2401,7 @@ static int pxa_camera_probe(struct platform_device *pdev)
 						 &params, &pdev->dev, "CI_U");
 	if (!pcdev->dma_chans[1]) {
 		dev_err(&pdev->dev, "Can't request DMA for Y\n");
+		err = -ENODEV;
 		goto exit_free_dma_y;
 	}
 
@@ -1776,6 +2411,7 @@ static int pxa_camera_probe(struct platform_device *pdev)
 						 &params, &pdev->dev, "CI_V");
 	if (!pcdev->dma_chans[2]) {
 		dev_err(&pdev->dev, "Can't request DMA for V\n");
+		err = -ENODEV;
 		goto exit_free_dma_u;
 	}
 
@@ -1797,19 +2433,50 @@ static int pxa_camera_probe(struct platform_device *pdev)
 		goto exit_free_dma;
 	}
 
-	pcdev->soc_host.drv_name	= PXA_CAM_DRV_NAME;
-	pcdev->soc_host.ops		= &pxa_soc_camera_host_ops;
-	pcdev->soc_host.priv		= pcdev;
-	pcdev->soc_host.v4l2_dev.dev	= &pdev->dev;
-	pcdev->soc_host.nr		= pdev->id;
 	tasklet_init(&pcdev->task_eof, pxa_camera_eof, (unsigned long)pcdev);
 
-	err = soc_camera_host_register(&pcdev->soc_host);
+	pxa_camera_activate(pcdev);
+
+	dev_set_drvdata(&pdev->dev, pcdev);
+	err = v4l2_device_register(&pdev->dev, &pcdev->v4l2_dev);
 	if (err)
 		goto exit_free_dma;
 
-	return 0;
+	pcdev->asds[0] = &pcdev->asd;
+	pcdev->notifier.subdevs = pcdev->asds;
+	pcdev->notifier.num_subdevs = 1;
+	pcdev->notifier.bound = pxa_camera_sensor_bound;
+	pcdev->notifier.unbind = pxa_camera_sensor_unbind;
 
+	if (!of_have_populated_dt())
+		pcdev->asd.match_type = V4L2_ASYNC_MATCH_I2C;
+
+	err = pxa_camera_init_videobuf2(pcdev);
+	if (err)
+		goto exit_free_v4l2dev;
+
+	if (pcdev->mclk) {
+		v4l2_clk_name_i2c(clk_name, sizeof(clk_name),
+				  pcdev->asd.match.i2c.adapter_id,
+				  pcdev->asd.match.i2c.address);
+
+		pcdev->mclk_clk = v4l2_clk_register(&pxa_camera_mclk_ops,
+						    clk_name, NULL);
+		if (IS_ERR(pcdev->mclk_clk)) {
+			err = PTR_ERR(pcdev->mclk_clk);
+			goto exit_free_v4l2dev;
+		}
+	}
+
+	err = v4l2_async_notifier_register(&pcdev->v4l2_dev, &pcdev->notifier);
+	if (err)
+		goto exit_free_clk;
+
+	return 0;
+exit_free_clk:
+	v4l2_clk_unregister(pcdev->mclk_clk);
+exit_free_v4l2dev:
+	v4l2_device_unregister(&pcdev->v4l2_dev);
 exit_free_dma:
 	dma_release_channel(pcdev->dma_chans[2]);
 exit_free_dma_u:
@@ -1821,15 +2488,15 @@ exit_free_dma_y:
 
 static int pxa_camera_remove(struct platform_device *pdev)
 {
-	struct soc_camera_host *soc_host = to_soc_camera_host(&pdev->dev);
-	struct pxa_camera_dev *pcdev = container_of(soc_host,
-					struct pxa_camera_dev, soc_host);
+	struct pxa_camera_dev *pcdev = dev_get_drvdata(&pdev->dev);
 
+	pxa_camera_deactivate(pcdev);
 	dma_release_channel(pcdev->dma_chans[0]);
 	dma_release_channel(pcdev->dma_chans[1]);
 	dma_release_channel(pcdev->dma_chans[2]);
 
-	soc_camera_host_unregister(soc_host);
+	v4l2_clk_unregister(pcdev->mclk_clk);
+	v4l2_device_unregister(&pcdev->v4l2_dev);
 
 	dev_info(&pdev->dev, "PXA Camera driver unloaded\n");
 
diff --git a/drivers/media/platform/rcar-fcp.c b/drivers/media/platform/rcar-fcp.c
index bc50c69ee0c5..f3a3f31cdfa9 100644
--- a/drivers/media/platform/rcar-fcp.c
+++ b/drivers/media/platform/rcar-fcp.c
@@ -99,14 +99,14 @@ EXPORT_SYMBOL_GPL(rcar_fcp_put);
  */
 int rcar_fcp_enable(struct rcar_fcp_device *fcp)
 {
-	int error;
+	int ret;
 
 	if (!fcp)
 		return 0;
 
-	error = pm_runtime_get_sync(fcp->dev);
-	if (error < 0)
-		return error;
+	ret = pm_runtime_get_sync(fcp->dev);
+	if (ret < 0)
+		return ret;
 
 	return 0;
 }
@@ -165,6 +165,7 @@ static int rcar_fcp_remove(struct platform_device *pdev)
 }
 
 static const struct of_device_id rcar_fcp_of_match[] = {
+	{ .compatible = "renesas,fcpf" },
 	{ .compatible = "renesas,fcpv" },
 	{ },
 };
diff --git a/drivers/media/platform/rcar-vin/Kconfig b/drivers/media/platform/rcar-vin/Kconfig
index b2ff2d4e8bb1..111d2a151f6a 100644
--- a/drivers/media/platform/rcar-vin/Kconfig
+++ b/drivers/media/platform/rcar-vin/Kconfig
@@ -1,6 +1,6 @@
 config VIDEO_RCAR_VIN
 	tristate "R-Car Video Input (VIN) Driver"
-	depends on VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API && OF && HAS_DMA
+	depends on VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API && OF && HAS_DMA && MEDIA_CONTROLLER
 	depends on ARCH_RENESAS || COMPILE_TEST
 	select VIDEOBUF2_DMA_CONTIG
 	---help---
diff --git a/drivers/media/platform/rcar-vin/rcar-core.c b/drivers/media/platform/rcar-vin/rcar-core.c
index 4b2007b73463..098a0b1cc10a 100644
--- a/drivers/media/platform/rcar-vin/rcar-core.c
+++ b/drivers/media/platform/rcar-vin/rcar-core.c
@@ -31,26 +31,22 @@
 
 #define notifier_to_vin(n) container_of(n, struct rvin_dev, notifier)
 
-static int rvin_mbus_supported(struct rvin_dev *vin)
+static bool rvin_mbus_supported(struct rvin_graph_entity *entity)
 {
-	struct v4l2_subdev *sd;
+	struct v4l2_subdev *sd = entity->subdev;
 	struct v4l2_subdev_mbus_code_enum code = {
 		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
 	};
 
-	sd = vin_to_source(vin);
-
 	code.index = 0;
 	while (!v4l2_subdev_call(sd, pad, enum_mbus_code, NULL, &code)) {
 		code.index++;
 		switch (code.code) {
 		case MEDIA_BUS_FMT_YUYV8_1X16:
-		case MEDIA_BUS_FMT_YUYV8_2X8:
-		case MEDIA_BUS_FMT_YUYV10_2X10:
+		case MEDIA_BUS_FMT_UYVY8_2X8:
+		case MEDIA_BUS_FMT_UYVY10_2X10:
 		case MEDIA_BUS_FMT_RGB888_1X24:
-			vin->source.code = code.code;
-			vin_dbg(vin, "Found supported media bus format: %d\n",
-				vin->source.code);
+			entity->code = code.code;
 			return true;
 		default:
 			break;
@@ -60,142 +56,168 @@ static int rvin_mbus_supported(struct rvin_dev *vin)
 	return false;
 }
 
-static int rvin_graph_notify_complete(struct v4l2_async_notifier *notifier)
+static int rvin_digital_notify_complete(struct v4l2_async_notifier *notifier)
 {
 	struct rvin_dev *vin = notifier_to_vin(notifier);
 	int ret;
 
+	/* Verify subdevices mbus format */
+	if (!rvin_mbus_supported(&vin->digital)) {
+		vin_err(vin, "Unsupported media bus format for %s\n",
+			vin->digital.subdev->name);
+		return -EINVAL;
+	}
+
+	vin_dbg(vin, "Found media bus format for %s: %d\n",
+		vin->digital.subdev->name, vin->digital.code);
+
 	ret = v4l2_device_register_subdev_nodes(&vin->v4l2_dev);
 	if (ret < 0) {
 		vin_err(vin, "Failed to register subdev nodes\n");
 		return ret;
 	}
 
-	if (!rvin_mbus_supported(vin)) {
-		vin_err(vin, "No supported mediabus format found\n");
-		return -EINVAL;
+	return rvin_v4l2_probe(vin);
+}
+
+static void rvin_digital_notify_unbind(struct v4l2_async_notifier *notifier,
+				       struct v4l2_subdev *subdev,
+				       struct v4l2_async_subdev *asd)
+{
+	struct rvin_dev *vin = notifier_to_vin(notifier);
+
+	if (vin->digital.subdev == subdev) {
+		vin_dbg(vin, "unbind digital subdev %s\n", subdev->name);
+		rvin_v4l2_remove(vin);
+		vin->digital.subdev = NULL;
+		return;
 	}
 
-	return rvin_v4l2_probe(vin);
+	vin_err(vin, "no entity for subdev %s to unbind\n", subdev->name);
 }
 
-static void rvin_graph_notify_unbind(struct v4l2_async_notifier *notifier,
-				     struct v4l2_subdev *sd,
+static int rvin_digital_notify_bound(struct v4l2_async_notifier *notifier,
+				     struct v4l2_subdev *subdev,
 				     struct v4l2_async_subdev *asd)
 {
 	struct rvin_dev *vin = notifier_to_vin(notifier);
 
-	rvin_v4l2_remove(vin);
+	v4l2_set_subdev_hostdata(subdev, vin);
+
+	if (vin->digital.asd.match.of.node == subdev->dev->of_node) {
+		vin_dbg(vin, "bound digital subdev %s\n", subdev->name);
+		vin->digital.subdev = subdev;
+		return 0;
+	}
+
+	vin_err(vin, "no entity for subdev %s to bind\n", subdev->name);
+	return -EINVAL;
 }
 
-static int rvin_graph_notify_bound(struct v4l2_async_notifier *notifier,
-				   struct v4l2_subdev *subdev,
-				   struct v4l2_async_subdev *asd)
+static int rvin_digitial_parse_v4l2(struct rvin_dev *vin,
+				    struct device_node *ep,
+				    struct v4l2_mbus_config *mbus_cfg)
 {
-	struct rvin_dev *vin = notifier_to_vin(notifier);
+	struct v4l2_of_endpoint v4l2_ep;
+	int ret;
 
-	vin_dbg(vin, "subdev %s bound\n", subdev->name);
+	ret = v4l2_of_parse_endpoint(ep, &v4l2_ep);
+	if (ret) {
+		vin_err(vin, "Could not parse v4l2 endpoint\n");
+		return -EINVAL;
+	}
 
-	vin->entity.entity = &subdev->entity;
-	vin->entity.subdev = subdev;
+	mbus_cfg->type = v4l2_ep.bus_type;
+
+	switch (mbus_cfg->type) {
+	case V4L2_MBUS_PARALLEL:
+		vin_dbg(vin, "Found PARALLEL media bus\n");
+		mbus_cfg->flags = v4l2_ep.bus.parallel.flags;
+		break;
+	case V4L2_MBUS_BT656:
+		vin_dbg(vin, "Found BT656 media bus\n");
+		mbus_cfg->flags = 0;
+		break;
+	default:
+		vin_err(vin, "Unknown media bus type\n");
+		return -EINVAL;
+	}
 
 	return 0;
 }
 
-static int rvin_graph_parse(struct rvin_dev *vin,
-			    struct device_node *node)
+static int rvin_digital_graph_parse(struct rvin_dev *vin)
 {
-	struct device_node *remote;
-	struct device_node *ep = NULL;
-	struct device_node *next;
-	int ret = 0;
-
-	while (1) {
-		next = of_graph_get_next_endpoint(node, ep);
-		if (!next)
-			break;
-
-		of_node_put(ep);
-		ep = next;
+	struct device_node *ep, *np;
+	int ret;
 
-		remote = of_graph_get_remote_port_parent(ep);
-		if (!remote) {
-			ret = -EINVAL;
-			break;
-		}
+	vin->digital.asd.match.of.node = NULL;
+	vin->digital.subdev = NULL;
 
-		/* Skip entities that we have already processed. */
-		if (remote == vin->dev->of_node) {
-			of_node_put(remote);
-			continue;
-		}
+	/*
+	 * Port 0 id 0 is local digital input, try to get it.
+	 * Not all instances can or will have this, that is OK
+	 */
+	ep = of_graph_get_endpoint_by_regs(vin->dev->of_node, 0, 0);
+	if (!ep)
+		return 0;
 
-		/* Remote node to connect */
-		if (!vin->entity.node) {
-			vin->entity.node = remote;
-			vin->entity.asd.match_type = V4L2_ASYNC_MATCH_OF;
-			vin->entity.asd.match.of.node = remote;
-			ret++;
-		}
+	np = of_graph_get_remote_port_parent(ep);
+	if (!np) {
+		vin_err(vin, "No remote parent for digital input\n");
+		of_node_put(ep);
+		return -EINVAL;
 	}
+	of_node_put(np);
 
+	ret = rvin_digitial_parse_v4l2(vin, ep, &vin->digital.mbus_cfg);
 	of_node_put(ep);
+	if (ret)
+		return ret;
 
-	return ret;
+	vin->digital.asd.match.of.node = np;
+	vin->digital.asd.match_type = V4L2_ASYNC_MATCH_OF;
+
+	return 0;
 }
 
-static int rvin_graph_init(struct rvin_dev *vin)
+static int rvin_digital_graph_init(struct rvin_dev *vin)
 {
 	struct v4l2_async_subdev **subdevs = NULL;
 	int ret;
 
-	/* Parse the graph to extract a list of subdevice DT nodes. */
-	ret = rvin_graph_parse(vin, vin->dev->of_node);
-	if (ret < 0) {
-		vin_err(vin, "Graph parsing failed\n");
-		goto done;
-	}
-
-	if (!ret) {
-		vin_err(vin, "No subdev found in graph\n");
-		goto done;
-	}
+	ret = rvin_digital_graph_parse(vin);
+	if (ret)
+		return ret;
 
-	if (ret != 1) {
-		vin_err(vin, "More then one subdev found in graph\n");
-		goto done;
+	if (!vin->digital.asd.match.of.node) {
+		vin_dbg(vin, "No digital subdevice found\n");
+		return -ENODEV;
 	}
 
 	/* Register the subdevices notifier. */
 	subdevs = devm_kzalloc(vin->dev, sizeof(*subdevs), GFP_KERNEL);
-	if (subdevs == NULL) {
-		ret = -ENOMEM;
-		goto done;
-	}
+	if (subdevs == NULL)
+		return -ENOMEM;
 
-	subdevs[0] = &vin->entity.asd;
+	subdevs[0] = &vin->digital.asd;
+
+	vin_dbg(vin, "Found digital subdevice %s\n",
+		of_node_full_name(subdevs[0]->match.of.node));
 
-	vin->notifier.subdevs = subdevs;
 	vin->notifier.num_subdevs = 1;
-	vin->notifier.bound = rvin_graph_notify_bound;
-	vin->notifier.unbind = rvin_graph_notify_unbind;
-	vin->notifier.complete = rvin_graph_notify_complete;
+	vin->notifier.subdevs = subdevs;
+	vin->notifier.bound = rvin_digital_notify_bound;
+	vin->notifier.unbind = rvin_digital_notify_unbind;
+	vin->notifier.complete = rvin_digital_notify_complete;
 
 	ret = v4l2_async_notifier_register(&vin->v4l2_dev, &vin->notifier);
 	if (ret < 0) {
 		vin_err(vin, "Notifier registration failed\n");
-		goto done;
-	}
-
-	ret = 0;
-
-done:
-	if (ret < 0) {
-		v4l2_async_notifier_unregister(&vin->notifier);
-		of_node_put(vin->entity.node);
+		return ret;
 	}
 
-	return ret;
+	return 0;
 }
 
 /* -----------------------------------------------------------------------------
@@ -209,56 +231,14 @@ static const struct of_device_id rvin_of_id_table[] = {
 	{ .compatible = "renesas,vin-r8a7790", .data = (void *)RCAR_GEN2 },
 	{ .compatible = "renesas,vin-r8a7779", .data = (void *)RCAR_H1 },
 	{ .compatible = "renesas,vin-r8a7778", .data = (void *)RCAR_M1 },
+	{ .compatible = "renesas,rcar-gen2-vin", .data = (void *)RCAR_GEN2 },
 	{ },
 };
 MODULE_DEVICE_TABLE(of, rvin_of_id_table);
 
-static int rvin_parse_dt(struct rvin_dev *vin)
-{
-	const struct of_device_id *match;
-	struct v4l2_of_endpoint ep;
-	struct device_node *np;
-	int ret;
-
-	match = of_match_device(of_match_ptr(rvin_of_id_table), vin->dev);
-	if (!match)
-		return -ENODEV;
-
-	vin->chip = (enum chip_id)match->data;
-
-	np = of_graph_get_next_endpoint(vin->dev->of_node, NULL);
-	if (!np) {
-		vin_err(vin, "Could not find endpoint\n");
-		return -EINVAL;
-	}
-
-	ret = v4l2_of_parse_endpoint(np, &ep);
-	if (ret) {
-		vin_err(vin, "Could not parse endpoint\n");
-		return ret;
-	}
-
-	of_node_put(np);
-
-	vin->mbus_cfg.type = ep.bus_type;
-
-	switch (vin->mbus_cfg.type) {
-	case V4L2_MBUS_PARALLEL:
-		vin->mbus_cfg.flags = ep.bus.parallel.flags;
-		break;
-	case V4L2_MBUS_BT656:
-		vin->mbus_cfg.flags = 0;
-		break;
-	default:
-		vin_err(vin, "Unknown media bus type\n");
-		return -EINVAL;
-	}
-
-	return 0;
-}
-
 static int rcar_vin_probe(struct platform_device *pdev)
 {
+	const struct of_device_id *match;
 	struct rvin_dev *vin;
 	struct resource *mem;
 	int irq, ret;
@@ -267,11 +247,12 @@ static int rcar_vin_probe(struct platform_device *pdev)
 	if (!vin)
 		return -ENOMEM;
 
-	vin->dev = &pdev->dev;
+	match = of_match_device(of_match_ptr(rvin_of_id_table), &pdev->dev);
+	if (!match)
+		return -ENODEV;
 
-	ret = rvin_parse_dt(vin);
-	if (ret)
-		return ret;
+	vin->dev = &pdev->dev;
+	vin->chip = (enum chip_id)match->data;
 
 	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (mem == NULL)
@@ -282,14 +263,14 @@ static int rcar_vin_probe(struct platform_device *pdev)
 		return PTR_ERR(vin->base);
 
 	irq = platform_get_irq(pdev, 0);
-	if (irq <= 0)
-		return ret;
+	if (irq < 0)
+		return irq;
 
 	ret = rvin_dma_probe(vin, irq);
 	if (ret)
 		return ret;
 
-	ret = rvin_graph_init(vin);
+	ret = rvin_digital_graph_init(vin);
 	if (ret < 0)
 		goto error;
 
diff --git a/drivers/media/platform/rcar-vin/rcar-dma.c b/drivers/media/platform/rcar-vin/rcar-dma.c
index 496aa97b6400..9ccd5ff55e19 100644
--- a/drivers/media/platform/rcar-vin/rcar-dma.c
+++ b/drivers/media/platform/rcar-vin/rcar-dma.c
@@ -95,6 +95,7 @@
 /* Video n Module Status Register bits */
 #define VNMS_FBS_MASK		(3 << 3)
 #define VNMS_FBS_SHIFT		3
+#define VNMS_FS			(1 << 2)
 #define VNMS_AV			(1 << 1)
 #define VNMS_CA			(1 << 0)
 
@@ -131,6 +132,7 @@ static u32 rvin_read(struct rvin_dev *vin, u32 offset)
 static int rvin_setup(struct rvin_dev *vin)
 {
 	u32 vnmc, dmr, dmr2, interrupts;
+	v4l2_std_id std;
 	bool progressive = false, output_is_yuv = false, input_is_yuv = false;
 
 	switch (vin->format.field) {
@@ -141,12 +143,21 @@ static int rvin_setup(struct rvin_dev *vin)
 		vnmc = VNMC_IM_EVEN;
 		break;
 	case V4L2_FIELD_INTERLACED:
+		/* Default to TB */
+		vnmc = VNMC_IM_FULL;
+		/* Use BT if video standard can be read and is 60 Hz format */
+		if (!v4l2_subdev_call(vin_to_source(vin), video, g_std, &std)) {
+			if (std & V4L2_STD_525_60)
+				vnmc = VNMC_IM_FULL | VNMC_FOC;
+		}
+		break;
 	case V4L2_FIELD_INTERLACED_TB:
 		vnmc = VNMC_IM_FULL;
 		break;
 	case V4L2_FIELD_INTERLACED_BT:
 		vnmc = VNMC_IM_FULL | VNMC_FOC;
 		break;
+	case V4L2_FIELD_ALTERNATE:
 	case V4L2_FIELD_NONE:
 		if (vin->continuous) {
 			vnmc = VNMC_IM_ODD_EVEN;
@@ -163,24 +174,24 @@ static int rvin_setup(struct rvin_dev *vin)
 	/*
 	 * Input interface
 	 */
-	switch (vin->source.code) {
+	switch (vin->digital.code) {
 	case MEDIA_BUS_FMT_YUYV8_1X16:
 		/* BT.601/BT.1358 16bit YCbCr422 */
 		vnmc |= VNMC_INF_YUV16;
 		input_is_yuv = true;
 		break;
-	case MEDIA_BUS_FMT_YUYV8_2X8:
+	case MEDIA_BUS_FMT_UYVY8_2X8:
 		/* BT.656 8bit YCbCr422 or BT.601 8bit YCbCr422 */
-		vnmc |= vin->mbus_cfg.type == V4L2_MBUS_BT656 ?
+		vnmc |= vin->digital.mbus_cfg.type == V4L2_MBUS_BT656 ?
 			VNMC_INF_YUV8_BT656 : VNMC_INF_YUV8_BT601;
 		input_is_yuv = true;
 		break;
 	case MEDIA_BUS_FMT_RGB888_1X24:
 		vnmc |= VNMC_INF_RGB888;
 		break;
-	case MEDIA_BUS_FMT_YUYV10_2X10:
+	case MEDIA_BUS_FMT_UYVY10_2X10:
 		/* BT.656 10bit YCbCr422 or BT.601 10bit YCbCr422 */
-		vnmc |= vin->mbus_cfg.type == V4L2_MBUS_BT656 ?
+		vnmc |= vin->digital.mbus_cfg.type == V4L2_MBUS_BT656 ?
 			VNMC_INF_YUV10_BT656 : VNMC_INF_YUV10_BT601;
 		input_is_yuv = true;
 		break;
@@ -192,11 +203,11 @@ static int rvin_setup(struct rvin_dev *vin)
 	dmr2 = VNDMR2_FTEV | VNDMR2_VLV(1);
 
 	/* Hsync Signal Polarity Select */
-	if (!(vin->mbus_cfg.flags & V4L2_MBUS_HSYNC_ACTIVE_LOW))
+	if (!(vin->digital.mbus_cfg.flags & V4L2_MBUS_HSYNC_ACTIVE_LOW))
 		dmr2 |= VNDMR2_HPS;
 
 	/* Vsync Signal Polarity Select */
-	if (!(vin->mbus_cfg.flags & V4L2_MBUS_VSYNC_ACTIVE_LOW))
+	if (!(vin->digital.mbus_cfg.flags & V4L2_MBUS_VSYNC_ACTIVE_LOW))
 		dmr2 |= VNDMR2_VPS;
 
 	/*
@@ -225,11 +236,9 @@ static int rvin_setup(struct rvin_dev *vin)
 		dmr = 0;
 		break;
 	case V4L2_PIX_FMT_XBGR32:
-		if (vin->chip == RCAR_GEN2 || vin->chip == RCAR_H1) {
-			dmr = VNDMR_EXRGB;
-			break;
-		}
-		/* fall through */
+		/* Note: not supported on M1 */
+		dmr = VNDMR_EXRGB;
+		break;
 	default:
 		vin_err(vin, "Invalid pixelformat (0x%x)\n",
 			vin->format.pixelformat);
@@ -322,15 +331,26 @@ static bool rvin_capture_active(struct rvin_dev *vin)
 	return rvin_read(vin, VNMS_REG) & VNMS_CA;
 }
 
-static int rvin_get_active_slot(struct rvin_dev *vin)
+static int rvin_get_active_slot(struct rvin_dev *vin, u32 vnms)
 {
 	if (vin->continuous)
-		return (rvin_read(vin, VNMS_REG) & VNMS_FBS_MASK)
-			>> VNMS_FBS_SHIFT;
+		return (vnms & VNMS_FBS_MASK) >> VNMS_FBS_SHIFT;
 
 	return 0;
 }
 
+static enum v4l2_field rvin_get_active_field(struct rvin_dev *vin, u32 vnms)
+{
+	if (vin->format.field == V4L2_FIELD_ALTERNATE) {
+		/* If FS is set it's a Even field */
+		if (vnms & VNMS_FS)
+			return V4L2_FIELD_BOTTOM;
+		return V4L2_FIELD_TOP;
+	}
+
+	return vin->format.field;
+}
+
 static void rvin_set_slot_addr(struct rvin_dev *vin, int slot, dma_addr_t addr)
 {
 	const struct rvin_video_format *fmt;
@@ -871,7 +891,7 @@ static bool rvin_fill_hw(struct rvin_dev *vin)
 static irqreturn_t rvin_irq(int irq, void *data)
 {
 	struct rvin_dev *vin = data;
-	u32 int_status;
+	u32 int_status, vnms;
 	int slot;
 	unsigned int sequence, handled = 0;
 	unsigned long flags;
@@ -898,7 +918,8 @@ static irqreturn_t rvin_irq(int irq, void *data)
 	}
 
 	/* Prepare for capture and update state */
-	slot = rvin_get_active_slot(vin);
+	vnms = rvin_read(vin, VNMS_REG);
+	slot = rvin_get_active_slot(vin, vnms);
 	sequence = vin->sequence++;
 
 	vin_dbg(vin, "IRQ %02d: %d\tbuf0: %c buf1: %c buf2: %c\tmore: %d\n",
@@ -913,7 +934,7 @@ static irqreturn_t rvin_irq(int irq, void *data)
 		goto done;
 
 	/* Capture frame */
-	vin->queue_buf[slot]->field = vin->format.field;
+	vin->queue_buf[slot]->field = rvin_get_active_field(vin, vnms);
 	vin->queue_buf[slot]->sequence = sequence;
 	vin->queue_buf[slot]->vb2_buf.timestamp = ktime_get_ns();
 	vb2_buffer_done(&vin->queue_buf[slot]->vb2_buf, VB2_BUF_STATE_DONE);
@@ -1116,7 +1137,7 @@ static void rvin_stop_streaming(struct vb2_queue *vq)
 	rvin_disable_interrupts(vin);
 }
 
-static struct vb2_ops rvin_qops = {
+static const struct vb2_ops rvin_qops = {
 	.queue_setup		= rvin_queue_setup,
 	.buf_prepare		= rvin_buffer_prepare,
 	.buf_queue		= rvin_buffer_queue,
diff --git a/drivers/media/platform/rcar-vin/rcar-v4l2.c b/drivers/media/platform/rcar-vin/rcar-v4l2.c
index 10a5c107e8b9..2bbe6d495fa6 100644
--- a/drivers/media/platform/rcar-vin/rcar-v4l2.c
+++ b/drivers/media/platform/rcar-vin/rcar-v4l2.c
@@ -92,21 +92,84 @@ static u32 rvin_format_sizeimage(struct v4l2_pix_format *pix)
  * V4L2
  */
 
+static void rvin_reset_crop_compose(struct rvin_dev *vin)
+{
+	vin->crop.top = vin->crop.left = 0;
+	vin->crop.width = vin->source.width;
+	vin->crop.height = vin->source.height;
+
+	vin->compose.top = vin->compose.left = 0;
+	vin->compose.width = vin->format.width;
+	vin->compose.height = vin->format.height;
+}
+
+static int rvin_reset_format(struct rvin_dev *vin)
+{
+	struct v4l2_subdev_format fmt = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	struct v4l2_mbus_framefmt *mf = &fmt.format;
+	int ret;
+
+	fmt.pad = vin->src_pad_idx;
+
+	ret = v4l2_subdev_call(vin_to_source(vin), pad, get_fmt, NULL, &fmt);
+	if (ret)
+		return ret;
+
+	vin->format.width	= mf->width;
+	vin->format.height	= mf->height;
+	vin->format.colorspace	= mf->colorspace;
+	vin->format.field	= mf->field;
+
+	/*
+	 * If the subdevice uses ALTERNATE field mode and G_STD is
+	 * implemented use the VIN HW to combine the two fields to
+	 * one INTERLACED frame. The ALTERNATE field mode can still
+	 * be requested in S_FMT and be respected, this is just the
+	 * default which is applied at probing or when S_STD is called.
+	 */
+	if (vin->format.field == V4L2_FIELD_ALTERNATE &&
+	    v4l2_subdev_has_op(vin_to_source(vin), video, g_std))
+		vin->format.field = V4L2_FIELD_INTERLACED;
+
+	switch (vin->format.field) {
+	case V4L2_FIELD_TOP:
+	case V4L2_FIELD_BOTTOM:
+	case V4L2_FIELD_ALTERNATE:
+		vin->format.height /= 2;
+		break;
+	case V4L2_FIELD_NONE:
+	case V4L2_FIELD_INTERLACED_TB:
+	case V4L2_FIELD_INTERLACED_BT:
+	case V4L2_FIELD_INTERLACED:
+		break;
+	default:
+		vin->format.field = V4L2_FIELD_NONE;
+		break;
+	}
+
+	rvin_reset_crop_compose(vin);
+
+	return 0;
+}
+
 static int __rvin_try_format_source(struct rvin_dev *vin,
-					u32 which,
-					struct v4l2_pix_format *pix,
-					struct rvin_source_fmt *source)
+				    u32 which,
+				    struct v4l2_pix_format *pix,
+				    struct rvin_source_fmt *source)
 {
 	struct v4l2_subdev *sd;
 	struct v4l2_subdev_pad_config *pad_cfg;
 	struct v4l2_subdev_format format = {
 		.which = which,
 	};
+	enum v4l2_field field;
 	int ret;
 
 	sd = vin_to_source(vin);
 
-	v4l2_fill_mbus_format(&format.format, pix, vin->source.code);
+	v4l2_fill_mbus_format(&format.format, pix, vin->digital.code);
 
 	pad_cfg = v4l2_subdev_alloc_pad_config(sd);
 	if (pad_cfg == NULL)
@@ -114,28 +177,31 @@ static int __rvin_try_format_source(struct rvin_dev *vin,
 
 	format.pad = vin->src_pad_idx;
 
-	ret = v4l2_device_call_until_err(sd->v4l2_dev, 0, pad, set_fmt,
-					 pad_cfg, &format);
-	if (ret < 0)
-		goto cleanup;
+	field = pix->field;
+
+	ret = v4l2_subdev_call(sd, pad, set_fmt, pad_cfg, &format);
+	if (ret < 0 && ret != -ENOIOCTLCMD)
+		goto done;
 
 	v4l2_fill_pix_format(pix, &format.format);
 
+	pix->field = field;
+
 	source->width = pix->width;
 	source->height = pix->height;
 
 	vin_dbg(vin, "Source resolution: %ux%u\n", source->width,
 		source->height);
 
-cleanup:
+done:
 	v4l2_subdev_free_pad_config(pad_cfg);
-	return 0;
+	return ret;
 }
 
 static int __rvin_try_format(struct rvin_dev *vin,
-				 u32 which,
-				 struct v4l2_pix_format *pix,
-				 struct rvin_source_fmt *source)
+			     u32 which,
+			     struct v4l2_pix_format *pix,
+			     struct rvin_source_fmt *source)
 {
 	const struct rvin_video_format *info;
 	u32 rwidth, rheight, walign;
@@ -144,6 +210,10 @@ static int __rvin_try_format(struct rvin_dev *vin,
 	rwidth = pix->width;
 	rheight = pix->height;
 
+	/* Keep current field if no specific one is asked for */
+	if (pix->field == V4L2_FIELD_ANY)
+		pix->field = vin->format.field;
+
 	/*
 	 * Retrieve format information and select the current format if the
 	 * requested format isn't supported.
@@ -164,21 +234,14 @@ static int __rvin_try_format(struct rvin_dev *vin,
 	/* Limit to source capabilities */
 	__rvin_try_format_source(vin, which, pix, source);
 
-	/* If source can't match format try if VIN can scale */
-	if (source->width != rwidth || source->height != rheight)
-		rvin_scale_try(vin, pix, rwidth, rheight);
-
-	/* HW limit width to a multiple of 32 (2^5) for NV16 else 2 (2^1) */
-	walign = vin->format.pixelformat == V4L2_PIX_FMT_NV16 ? 5 : 1;
-
-	/* Limit to VIN capabilities */
-	v4l_bound_align_image(&pix->width, 2, RVIN_MAX_WIDTH, walign,
-			      &pix->height, 4, RVIN_MAX_HEIGHT, 2, 0);
-
 	switch (pix->field) {
-	case V4L2_FIELD_NONE:
 	case V4L2_FIELD_TOP:
 	case V4L2_FIELD_BOTTOM:
+	case V4L2_FIELD_ALTERNATE:
+		pix->height /= 2;
+		source->height /= 2;
+		break;
+	case V4L2_FIELD_NONE:
 	case V4L2_FIELD_INTERLACED_TB:
 	case V4L2_FIELD_INTERLACED_BT:
 	case V4L2_FIELD_INTERLACED:
@@ -188,11 +251,27 @@ static int __rvin_try_format(struct rvin_dev *vin,
 		break;
 	}
 
+	/* If source can't match format try if VIN can scale */
+	if (source->width != rwidth || source->height != rheight)
+		rvin_scale_try(vin, pix, rwidth, rheight);
+
+	/* HW limit width to a multiple of 32 (2^5) for NV16 else 2 (2^1) */
+	walign = vin->format.pixelformat == V4L2_PIX_FMT_NV16 ? 5 : 1;
+
+	/* Limit to VIN capabilities */
+	v4l_bound_align_image(&pix->width, 2, RVIN_MAX_WIDTH, walign,
+			      &pix->height, 4, RVIN_MAX_HEIGHT, 2, 0);
+
 	pix->bytesperline = max_t(u32, pix->bytesperline,
 				  rvin_format_bytesperline(pix));
 	pix->sizeimage = max_t(u32, pix->sizeimage,
 			       rvin_format_sizeimage(pix));
 
+	if (vin->chip == RCAR_M1 && pix->pixelformat == V4L2_PIX_FMT_XBGR32) {
+		vin_err(vin, "pixel format XBGR32 not supported on M1\n");
+		return -EINVAL;
+	}
+
 	vin_dbg(vin, "Requested %ux%u Got %ux%u bpl: %d size: %d\n",
 		rwidth, rheight, pix->width, pix->height,
 		pix->bytesperline, pix->sizeimage);
@@ -219,7 +298,7 @@ static int rvin_try_fmt_vid_cap(struct file *file, void *priv,
 	struct rvin_source_fmt source;
 
 	return __rvin_try_format(vin, V4L2_SUBDEV_FORMAT_TRY, &f->fmt.pix,
-				     &source);
+				 &source);
 }
 
 static int rvin_s_fmt_vid_cap(struct file *file, void *priv,
@@ -233,7 +312,7 @@ static int rvin_s_fmt_vid_cap(struct file *file, void *priv,
 		return -EBUSY;
 
 	ret = __rvin_try_format(vin, V4L2_SUBDEV_FORMAT_ACTIVE, &f->fmt.pix,
-				    &source);
+				&source);
 	if (ret)
 		return ret;
 
@@ -242,6 +321,8 @@ static int rvin_s_fmt_vid_cap(struct file *file, void *priv,
 
 	vin->format = f->fmt.pix;
 
+	rvin_reset_crop_compose(vin);
+
 	return 0;
 }
 
@@ -334,8 +415,8 @@ static int rvin_s_selection(struct file *file, void *fh,
 		vin->crop = s->r = r;
 
 		vin_dbg(vin, "Cropped %dx%d@%d:%d of %dx%d\n",
-			 r.width, r.height, r.left, r.top,
-			 vin->source.width, vin->source.height);
+			r.width, r.height, r.left, r.top,
+			vin->source.width, vin->source.height);
 		break;
 	case V4L2_SEL_TGT_COMPOSE:
 		/* Make sure compose rect fits inside output format */
@@ -359,8 +440,8 @@ static int rvin_s_selection(struct file *file, void *fh,
 		vin->compose = s->r = r;
 
 		vin_dbg(vin, "Compose %dx%d@%d:%d in %dx%d\n",
-			 r.width, r.height, r.left, r.top,
-			 vin->format.width, vin->format.height);
+			r.width, r.height, r.left, r.top,
+			vin->format.width, vin->format.height);
 		break;
 	default:
 		return -EINVAL;
@@ -381,7 +462,7 @@ static int rvin_cropcap(struct file *file, void *priv,
 	if (crop->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 		return -EINVAL;
 
-	return v4l2_subdev_call(sd, video, cropcap, crop);
+	return v4l2_subdev_call(sd, video, g_pixelaspect, &crop->pixelaspect);
 }
 
 static int rvin_enum_input(struct file *file, void *priv,
@@ -433,35 +514,14 @@ static int rvin_querystd(struct file *file, void *priv, v4l2_std_id *a)
 static int rvin_s_std(struct file *file, void *priv, v4l2_std_id a)
 {
 	struct rvin_dev *vin = video_drvdata(file);
-	struct v4l2_subdev *sd = vin_to_source(vin);
-	struct v4l2_subdev_format fmt = {
-		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
-	};
-	struct v4l2_mbus_framefmt *mf = &fmt.format;
-	int ret = v4l2_subdev_call(sd, video, s_std, a);
+	int ret;
 
+	ret = v4l2_subdev_call(vin_to_source(vin), video, s_std, a);
 	if (ret < 0)
 		return ret;
 
 	/* Changing the standard will change the width/height */
-	ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, &fmt);
-	if (ret) {
-		vin_err(vin, "Failed to get initial format\n");
-		return ret;
-	}
-
-	vin->format.width = mf->width;
-	vin->format.height = mf->height;
-
-	vin->crop.top = vin->crop.left = 0;
-	vin->crop.width = mf->width;
-	vin->crop.height = mf->height;
-
-	vin->compose.top = vin->compose.left = 0;
-	vin->compose.width = mf->width;
-	vin->compose.height = mf->height;
-
-	return 0;
+	return rvin_reset_format(vin);
 }
 
 static int rvin_g_std(struct file *file, void *priv, v4l2_std_id *a)
@@ -483,14 +543,14 @@ static int rvin_subscribe_event(struct v4l2_fh *fh,
 }
 
 static int rvin_enum_dv_timings(struct file *file, void *priv_fh,
-				    struct v4l2_enum_dv_timings *timings)
+				struct v4l2_enum_dv_timings *timings)
 {
 	struct rvin_dev *vin = video_drvdata(file);
 	struct v4l2_subdev *sd = vin_to_source(vin);
 	int pad, ret;
 
 	pad = timings->pad;
-	timings->pad = vin->src_pad_idx;
+	timings->pad = vin->sink_pad_idx;
 
 	ret = v4l2_subdev_call(sd, pad, enum_dv_timings, timings);
 
@@ -500,52 +560,51 @@ static int rvin_enum_dv_timings(struct file *file, void *priv_fh,
 }
 
 static int rvin_s_dv_timings(struct file *file, void *priv_fh,
-				    struct v4l2_dv_timings *timings)
+			     struct v4l2_dv_timings *timings)
 {
 	struct rvin_dev *vin = video_drvdata(file);
 	struct v4l2_subdev *sd = vin_to_source(vin);
-	int err;
-
-	err = v4l2_subdev_call(sd,
-			video, s_dv_timings, timings);
-	if (!err) {
-		vin->source.width = timings->bt.width;
-		vin->source.height = timings->bt.height;
-		vin->format.width = timings->bt.width;
-		vin->format.height = timings->bt.height;
-	}
-	return err;
+	int ret;
+
+	ret = v4l2_subdev_call(sd, video, s_dv_timings, timings);
+	if (ret)
+		return ret;
+
+	vin->source.width = timings->bt.width;
+	vin->source.height = timings->bt.height;
+	vin->format.width = timings->bt.width;
+	vin->format.height = timings->bt.height;
+
+	return 0;
 }
 
 static int rvin_g_dv_timings(struct file *file, void *priv_fh,
-				    struct v4l2_dv_timings *timings)
+			     struct v4l2_dv_timings *timings)
 {
 	struct rvin_dev *vin = video_drvdata(file);
 	struct v4l2_subdev *sd = vin_to_source(vin);
 
-	return v4l2_subdev_call(sd,
-			video, g_dv_timings, timings);
+	return v4l2_subdev_call(sd, video, g_dv_timings, timings);
 }
 
 static int rvin_query_dv_timings(struct file *file, void *priv_fh,
-				    struct v4l2_dv_timings *timings)
+				 struct v4l2_dv_timings *timings)
 {
 	struct rvin_dev *vin = video_drvdata(file);
 	struct v4l2_subdev *sd = vin_to_source(vin);
 
-	return v4l2_subdev_call(sd,
-			video, query_dv_timings, timings);
+	return v4l2_subdev_call(sd, video, query_dv_timings, timings);
 }
 
 static int rvin_dv_timings_cap(struct file *file, void *priv_fh,
-				    struct v4l2_dv_timings_cap *cap)
+			       struct v4l2_dv_timings_cap *cap)
 {
 	struct rvin_dev *vin = video_drvdata(file);
 	struct v4l2_subdev *sd = vin_to_source(vin);
 	int pad, ret;
 
 	pad = cap->pad;
-	cap->pad = vin->src_pad_idx;
+	cap->pad = vin->sink_pad_idx;
 
 	ret = v4l2_subdev_call(sd, pad, dv_timings_cap, cap);
 
@@ -554,6 +613,44 @@ static int rvin_dv_timings_cap(struct file *file, void *priv_fh,
 	return ret;
 }
 
+static int rvin_g_edid(struct file *file, void *fh, struct v4l2_edid *edid)
+{
+	struct rvin_dev *vin = video_drvdata(file);
+	struct v4l2_subdev *sd = vin_to_source(vin);
+	int input, ret;
+
+	if (edid->pad)
+		return -EINVAL;
+
+	input = edid->pad;
+	edid->pad = vin->sink_pad_idx;
+
+	ret = v4l2_subdev_call(sd, pad, get_edid, edid);
+
+	edid->pad = input;
+
+	return ret;
+}
+
+static int rvin_s_edid(struct file *file, void *fh, struct v4l2_edid *edid)
+{
+	struct rvin_dev *vin = video_drvdata(file);
+	struct v4l2_subdev *sd = vin_to_source(vin);
+	int input, ret;
+
+	if (edid->pad)
+		return -EINVAL;
+
+	input = edid->pad;
+	edid->pad = vin->sink_pad_idx;
+
+	ret = v4l2_subdev_call(sd, pad, set_edid, edid);
+
+	edid->pad = input;
+
+	return ret;
+}
+
 static const struct v4l2_ioctl_ops rvin_ioctl_ops = {
 	.vidioc_querycap		= rvin_querycap,
 	.vidioc_try_fmt_vid_cap		= rvin_try_fmt_vid_cap,
@@ -576,6 +673,9 @@ static const struct v4l2_ioctl_ops rvin_ioctl_ops = {
 	.vidioc_s_dv_timings		= rvin_s_dv_timings,
 	.vidioc_query_dv_timings	= rvin_query_dv_timings,
 
+	.vidioc_g_edid			= rvin_g_edid,
+	.vidioc_s_edid			= rvin_s_edid,
+
 	.vidioc_querystd		= rvin_querystd,
 	.vidioc_g_std			= rvin_g_std,
 	.vidioc_s_std			= rvin_s_std,
@@ -767,16 +867,9 @@ static void rvin_notify(struct v4l2_subdev *sd,
 
 int rvin_v4l2_probe(struct rvin_dev *vin)
 {
-	struct v4l2_subdev_format fmt = {
-		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
-	};
-	struct v4l2_mbus_framefmt *mf = &fmt.format;
 	struct video_device *vdev = &vin->vdev;
 	struct v4l2_subdev *sd = vin_to_source(vin);
-#if defined(CONFIG_MEDIA_CONTROLLER)
-	int pad_idx;
-#endif
-	int ret;
+	int pad_idx, ret;
 
 	v4l2_set_subdev_hostdata(sd, vin);
 
@@ -823,41 +916,23 @@ int rvin_v4l2_probe(struct rvin_dev *vin)
 		V4L2_CAP_READWRITE;
 
 	vin->src_pad_idx = 0;
-#if defined(CONFIG_MEDIA_CONTROLLER)
 	for (pad_idx = 0; pad_idx < sd->entity.num_pads; pad_idx++)
-		if (sd->entity.pads[pad_idx].flags
-				== MEDIA_PAD_FL_SOURCE)
+		if (sd->entity.pads[pad_idx].flags == MEDIA_PAD_FL_SOURCE)
 			break;
 	if (pad_idx >= sd->entity.num_pads)
 		return -EINVAL;
 
 	vin->src_pad_idx = pad_idx;
-#endif
-	fmt.pad = vin->src_pad_idx;
 
-	/* Try to improve our guess of a reasonable window format */
-	ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, &fmt);
-	if (ret) {
-		vin_err(vin, "Failed to get initial format\n");
-		return ret;
-	}
+	vin->sink_pad_idx = 0;
+	for (pad_idx = 0; pad_idx < sd->entity.num_pads; pad_idx++)
+		if (sd->entity.pads[pad_idx].flags == MEDIA_PAD_FL_SINK) {
+			vin->sink_pad_idx = pad_idx;
+			break;
+		}
 
-	/* Set default format */
-	vin->format.width	= mf->width;
-	vin->format.height	= mf->height;
-	vin->format.colorspace	= mf->colorspace;
-	vin->format.field	= mf->field;
 	vin->format.pixelformat	= RVIN_DEFAULT_FORMAT;
-
-
-	/* Set initial crop and compose */
-	vin->crop.top = vin->crop.left = 0;
-	vin->crop.width = mf->width;
-	vin->crop.height = mf->height;
-
-	vin->compose.top = vin->compose.left = 0;
-	vin->compose.width = mf->width;
-	vin->compose.height = mf->height;
+	rvin_reset_format(vin);
 
 	ret = video_register_device(&vin->vdev, VFL_TYPE_GRABBER, -1);
 	if (ret) {
diff --git a/drivers/media/platform/rcar-vin/rcar-vin.h b/drivers/media/platform/rcar-vin/rcar-vin.h
index 31ad39a39937..727e215c0871 100644
--- a/drivers/media/platform/rcar-vin/rcar-vin.h
+++ b/drivers/media/platform/rcar-vin/rcar-vin.h
@@ -30,9 +30,9 @@
 #define HW_BUFFER_MASK 0x7f
 
 enum chip_id {
-	RCAR_GEN2,
 	RCAR_H1,
 	RCAR_M1,
+	RCAR_GEN2,
 };
 
 /**
@@ -50,12 +50,10 @@ enum rvin_dma_state {
 
 /**
  * struct rvin_source_fmt - Source information
- * @code:	Media bus format from source
  * @width:	Width from source
  * @height:	Height from source
  */
 struct rvin_source_fmt {
-	u32 code;
 	u32 width;
 	u32 height;
 };
@@ -70,12 +68,19 @@ struct rvin_video_format {
 	u8 bpp;
 };
 
+/**
+ * struct rvin_graph_entity - Video endpoint from async framework
+ * @asd:	sub-device descriptor for async framework
+ * @subdev:	subdevice matched using async framework
+ * @code:	Media bus format from source
+ * @mbus_cfg:	Media bus format from DT
+ */
 struct rvin_graph_entity {
-	struct device_node *node;
-	struct media_entity *entity;
-
 	struct v4l2_async_subdev asd;
 	struct v4l2_subdev *subdev;
+
+	u32 code;
+	struct v4l2_mbus_config mbus_cfg;
 };
 
 /**
@@ -83,14 +88,14 @@ struct rvin_graph_entity {
  * @dev:		(OF) device
  * @base:		device I/O register space remapped to virtual memory
  * @chip:		type of VIN chip
- * @mbus_cfg		media bus configuration
  *
  * @vdev:		V4L2 video device associated with VIN
  * @v4l2_dev:		V4L2 device
  * @src_pad_idx:	source pad index for media controller drivers
+ * @sink_pad_idx:	sink pad index for media controller drivers
  * @ctrl_handler:	V4L2 control handler
  * @notifier:		V4L2 asynchronous subdevs notifier
- * @entity:		entity in the DT for subdevice
+ * @digital:		entity in the DT for local digital subdevice
  *
  * @lock:		protects @queue
  * @queue:		vb2 buffers queue
@@ -113,14 +118,14 @@ struct rvin_dev {
 	struct device *dev;
 	void __iomem *base;
 	enum chip_id chip;
-	struct v4l2_mbus_config mbus_cfg;
 
 	struct video_device vdev;
 	struct v4l2_device v4l2_dev;
 	int src_pad_idx;
+	int sink_pad_idx;
 	struct v4l2_ctrl_handler ctrl_handler;
 	struct v4l2_async_notifier notifier;
-	struct rvin_graph_entity entity;
+	struct rvin_graph_entity digital;
 
 	struct mutex lock;
 	struct vb2_queue queue;
@@ -139,7 +144,7 @@ struct rvin_dev {
 	struct v4l2_rect compose;
 };
 
-#define vin_to_source(vin)		vin->entity.subdev
+#define vin_to_source(vin)		vin->digital.subdev
 
 /* Debug */
 #define vin_dbg(d, fmt, arg...)		dev_dbg(d->dev, fmt, ##arg)
diff --git a/drivers/media/platform/rcar_jpu.c b/drivers/media/platform/rcar_jpu.c
index 16782ceb29c3..d1746ecc645d 100644
--- a/drivers/media/platform/rcar_jpu.c
+++ b/drivers/media/platform/rcar_jpu.c
@@ -1183,7 +1183,7 @@ static void jpu_stop_streaming(struct vb2_queue *vq)
 	}
 }
 
-static struct vb2_ops jpu_qops = {
+static const struct vb2_ops jpu_qops = {
 	.queue_setup		= jpu_queue_setup,
 	.buf_prepare		= jpu_buf_prepare,
 	.buf_queue		= jpu_buf_queue,
diff --git a/drivers/media/platform/s5p-g2d/g2d.c b/drivers/media/platform/s5p-g2d/g2d.c
index 391dd7a7b362..62c0dec30b59 100644
--- a/drivers/media/platform/s5p-g2d/g2d.c
+++ b/drivers/media/platform/s5p-g2d/g2d.c
@@ -138,7 +138,7 @@ static void g2d_buf_queue(struct vb2_buffer *vb)
 	v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vbuf);
 }
 
-static struct vb2_ops g2d_qops = {
+static const struct vb2_ops g2d_qops = {
 	.queue_setup	= g2d_queue_setup,
 	.buf_prepare	= g2d_buf_prepare,
 	.buf_queue	= g2d_buf_queue,
diff --git a/drivers/media/platform/s5p-jpeg/jpeg-core.c b/drivers/media/platform/s5p-jpeg/jpeg-core.c
index 785e6936c881..52dc7941db65 100644
--- a/drivers/media/platform/s5p-jpeg/jpeg-core.c
+++ b/drivers/media/platform/s5p-jpeg/jpeg-core.c
@@ -537,6 +537,7 @@ static const u32 fourcc_to_dwngrd_schema_id[] = {
 static int s5p_jpeg_get_dwngrd_sch_id_by_fourcc(u32 fourcc)
 {
 	int i;
+
 	for (i = 0; i < ARRAY_SIZE(fourcc_to_dwngrd_schema_id); ++i) {
 		if (fourcc_to_dwngrd_schema_id[i] == fourcc)
 			return i;
@@ -1246,17 +1247,18 @@ static int s5p_jpeg_querycap(struct file *file, void *priv,
 	struct s5p_jpeg_ctx *ctx = fh_to_ctx(priv);
 
 	if (ctx->mode == S5P_JPEG_ENCODE) {
-		strlcpy(cap->driver, S5P_JPEG_M2M_NAME " encoder",
+		strlcpy(cap->driver, S5P_JPEG_M2M_NAME,
 			sizeof(cap->driver));
 		strlcpy(cap->card, S5P_JPEG_M2M_NAME " encoder",
 			sizeof(cap->card));
 	} else {
-		strlcpy(cap->driver, S5P_JPEG_M2M_NAME " decoder",
+		strlcpy(cap->driver, S5P_JPEG_M2M_NAME,
 			sizeof(cap->driver));
 		strlcpy(cap->card, S5P_JPEG_M2M_NAME " decoder",
 			sizeof(cap->card));
 	}
-	cap->bus_info[0] = 0;
+	snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s",
+		 dev_name(ctx->jpeg->dev));
 	cap->device_caps = V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_M2M;
 	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
 	return 0;
@@ -1273,7 +1275,8 @@ static int enum_fmt(struct s5p_jpeg_fmt *sjpeg_formats, int n,
 			if (num == f->index)
 				break;
 			/* Correct type but haven't reached our index yet,
-			 * just increment per-type index */
+			 * just increment per-type index
+			 */
 			++num;
 		}
 	}
@@ -1349,6 +1352,7 @@ static int s5p_jpeg_g_fmt(struct file *file, void *priv, struct v4l2_format *f)
 	pix->bytesperline = 0;
 	if (q_data->fmt->fourcc != V4L2_PIX_FMT_JPEG) {
 		u32 bpl = q_data->w;
+
 		if (q_data->fmt->colplanes == 1)
 			bpl = (bpl * q_data->fmt->depth) >> 3;
 		pix->bytesperline = bpl;
@@ -1374,6 +1378,7 @@ static struct s5p_jpeg_fmt *s5p_jpeg_find_format(struct s5p_jpeg_ctx *ctx,
 
 	for (k = 0; k < ARRAY_SIZE(sjpeg_formats); k++) {
 		struct s5p_jpeg_fmt *fmt = &sjpeg_formats[k];
+
 		if (fmt->fourcc == pixelformat &&
 		    fmt->flags & fmt_flag &&
 		    fmt->flags & ctx->jpeg->variant->fmt_ver_flag) {
@@ -1431,7 +1436,8 @@ static int vidioc_try_fmt(struct v4l2_format *f, struct s5p_jpeg_fmt *fmt,
 		return -EINVAL;
 
 	/* V4L2 specification suggests the driver corrects the format struct
-	 * if any of the dimensions is unsupported */
+	 * if any of the dimensions is unsupported
+	 */
 	if (q_type == FMT_TYPE_OUTPUT)
 		jpeg_bound_align_image(ctx, &pix->width, S5P_JPEG_MIN_WIDTH,
 				       S5P_JPEG_MAX_WIDTH, 0,
@@ -2489,6 +2495,7 @@ static void s5p_jpeg_buf_queue(struct vb2_buffer *vb)
 	if (ctx->mode == S5P_JPEG_DECODE &&
 	    vb->vb2_queue->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
 		struct s5p_jpeg_q_data tmp, *q_data;
+
 		ctx->hdr_parsed = s5p_jpeg_parse_hdr(&tmp,
 		     (unsigned long)vb2_plane_vaddr(vb, 0),
 		     min((unsigned long)ctx->out_q.size,
@@ -2538,7 +2545,7 @@ static void s5p_jpeg_stop_streaming(struct vb2_queue *q)
 	pm_runtime_put(ctx->jpeg->dev);
 }
 
-static struct vb2_ops s5p_jpeg_qops = {
+static const struct vb2_ops s5p_jpeg_qops = {
 	.queue_setup		= s5p_jpeg_queue_setup,
 	.buf_prepare		= s5p_jpeg_buf_prepare,
 	.buf_queue		= s5p_jpeg_buf_queue,
@@ -2996,27 +3003,11 @@ static int s5p_jpeg_runtime_resume(struct device *dev)
 }
 #endif /* CONFIG_PM */
 
-#ifdef CONFIG_PM_SLEEP
-static int s5p_jpeg_suspend(struct device *dev)
-{
-	if (pm_runtime_suspended(dev))
-		return 0;
-
-	return s5p_jpeg_runtime_suspend(dev);
-}
-
-static int s5p_jpeg_resume(struct device *dev)
-{
-	if (pm_runtime_suspended(dev))
-		return 0;
-
-	return s5p_jpeg_runtime_resume(dev);
-}
-#endif
-
 static const struct dev_pm_ops s5p_jpeg_pm_ops = {
-	SET_SYSTEM_SLEEP_PM_OPS(s5p_jpeg_suspend, s5p_jpeg_resume)
-	SET_RUNTIME_PM_OPS(s5p_jpeg_runtime_suspend, s5p_jpeg_runtime_resume, NULL)
+	SET_SYSTEM_SLEEP_PM_OPS(pm_runtime_force_suspend,
+				pm_runtime_force_resume)
+	SET_RUNTIME_PM_OPS(s5p_jpeg_runtime_suspend, s5p_jpeg_runtime_resume,
+			   NULL)
 };
 
 static struct s5p_jpeg_variant s5p_jpeg_drvdata = {
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc.c b/drivers/media/platform/s5p-mfc/s5p_mfc.c
index e3f104fafd0a..0a5b8f5e011e 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc.c
@@ -153,7 +153,7 @@ static void s5p_mfc_watchdog(unsigned long arg)
 		 * error. Now it is time to kill all instances and
 		 * reset the MFC. */
 		mfc_err("Time out during waiting for HW\n");
-		queue_work(dev->watchdog_workqueue, &dev->watchdog_work);
+		schedule_work(&dev->watchdog_work);
 	}
 	dev->watchdog_timer.expires = jiffies +
 					msecs_to_jiffies(MFC_WATCHDOG_INTERVAL);
@@ -494,7 +494,6 @@ static void s5p_mfc_handle_error(struct s5p_mfc_dev *dev,
 	s5p_mfc_hw_call(dev->mfc_ops, clear_int_flags, dev);
 	s5p_mfc_clock_off();
 	wake_up_dev(dev, reason, err);
-	return;
 }
 
 /* Header parsing interrupt handling */
@@ -759,7 +758,6 @@ static int s5p_mfc_open(struct file *file)
 	/* Allocate memory for context */
 	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
 	if (!ctx) {
-		mfc_err("Not enough memory\n");
 		ret = -ENOMEM;
 		goto err_alloc;
 	}
@@ -776,7 +774,7 @@ static int s5p_mfc_open(struct file *file)
 	while (dev->ctx[ctx->num]) {
 		ctx->num++;
 		if (ctx->num >= MFC_NUM_CONTEXTS) {
-			mfc_err("Too many open contexts\n");
+			mfc_debug(2, "Too many open contexts\n");
 			ret = -EBUSY;
 			goto err_no_ctx;
 		}
@@ -924,39 +922,50 @@ static int s5p_mfc_release(struct file *file)
 	struct s5p_mfc_ctx *ctx = fh_to_ctx(file->private_data);
 	struct s5p_mfc_dev *dev = ctx->dev;
 
+	/* if dev is null, do cleanup that doesn't need dev */
 	mfc_debug_enter();
-	mutex_lock(&dev->mfc_mutex);
+	if (dev)
+		mutex_lock(&dev->mfc_mutex);
 	s5p_mfc_clock_on();
 	vb2_queue_release(&ctx->vq_src);
 	vb2_queue_release(&ctx->vq_dst);
-	/* Mark context as idle */
-	clear_work_bit_irqsave(ctx);
-	/* If instance was initialised and not yet freed,
-	 * return instance and free resources */
-	if (ctx->state != MFCINST_FREE && ctx->state != MFCINST_INIT) {
-		mfc_debug(2, "Has to free instance\n");
-		s5p_mfc_close_mfc_inst(dev, ctx);
-	}
-	/* hardware locking scheme */
-	if (dev->curr_ctx == ctx->num)
-		clear_bit(0, &dev->hw_lock);
-	dev->num_inst--;
-	if (dev->num_inst == 0) {
-		mfc_debug(2, "Last instance\n");
-		s5p_mfc_deinit_hw(dev);
-		del_timer_sync(&dev->watchdog_timer);
-		if (s5p_mfc_power_off() < 0)
-			mfc_err("Power off failed\n");
+	if (dev) {
+		/* Mark context as idle */
+		clear_work_bit_irqsave(ctx);
+		/*
+		 * If instance was initialised and not yet freed,
+		 * return instance and free resources
+		*/
+		if (ctx->state != MFCINST_FREE && ctx->state != MFCINST_INIT) {
+			mfc_debug(2, "Has to free instance\n");
+			s5p_mfc_close_mfc_inst(dev, ctx);
+		}
+		/* hardware locking scheme */
+		if (dev->curr_ctx == ctx->num)
+			clear_bit(0, &dev->hw_lock);
+		dev->num_inst--;
+		if (dev->num_inst == 0) {
+			mfc_debug(2, "Last instance\n");
+			s5p_mfc_deinit_hw(dev);
+			del_timer_sync(&dev->watchdog_timer);
+			if (s5p_mfc_power_off() < 0)
+				mfc_err("Power off failed\n");
+		}
 	}
 	mfc_debug(2, "Shutting down clock\n");
 	s5p_mfc_clock_off();
-	dev->ctx[ctx->num] = NULL;
+	if (dev)
+		dev->ctx[ctx->num] = NULL;
 	s5p_mfc_dec_ctrls_delete(ctx);
 	v4l2_fh_del(&ctx->fh);
-	v4l2_fh_exit(&ctx->fh);
+	/* vdev is gone if dev is null */
+	if (dev)
+		v4l2_fh_exit(&ctx->fh);
 	kfree(ctx);
 	mfc_debug_leave();
-	mutex_unlock(&dev->mfc_mutex);
+	if (dev)
+		mutex_unlock(&dev->mfc_mutex);
+
 	return 0;
 }
 
@@ -1158,10 +1167,6 @@ static int s5p_mfc_probe(struct platform_device *pdev)
 	dev->variant = mfc_get_drv_data(pdev);
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (res == NULL) {
-		dev_err(&pdev->dev, "failed to get io resource\n");
-		return -ENOENT;
-	}
 	dev->regs_base = devm_ioremap_resource(&pdev->dev, res);
 	if (IS_ERR(dev->regs_base))
 		return PTR_ERR(dev->regs_base);
@@ -1241,7 +1246,6 @@ static int s5p_mfc_probe(struct platform_device *pdev)
 	platform_set_drvdata(pdev, dev);
 
 	dev->hw_lock = 0;
-	dev->watchdog_workqueue = create_singlethread_workqueue(S5P_MFC_NAME);
 	INIT_WORK(&dev->watchdog_work, s5p_mfc_watchdog_worker);
 	atomic_set(&dev->watchdog_cnt, 0);
 	init_timer(&dev->watchdog_timer);
@@ -1298,12 +1302,28 @@ err_dma:
 static int s5p_mfc_remove(struct platform_device *pdev)
 {
 	struct s5p_mfc_dev *dev = platform_get_drvdata(pdev);
+	struct s5p_mfc_ctx *ctx;
+	int i;
 
 	v4l2_info(&dev->v4l2_dev, "Removing %s\n", pdev->name);
 
+	/*
+	 * Clear ctx dev pointer to avoid races between s5p_mfc_remove()
+	 * and s5p_mfc_release() and s5p_mfc_release() accessing ctx->dev
+	 * after s5p_mfc_remove() is run during unbind.
+	*/
+	mutex_lock(&dev->mfc_mutex);
+	for (i = 0; i < MFC_NUM_CONTEXTS; i++) {
+		ctx = dev->ctx[i];
+		if (!ctx)
+			continue;
+		/* clear ctx->dev */
+		ctx->dev = NULL;
+	}
+	mutex_unlock(&dev->mfc_mutex);
+
 	del_timer_sync(&dev->watchdog_timer);
-	flush_workqueue(dev->watchdog_workqueue);
-	destroy_workqueue(dev->watchdog_workqueue);
+	flush_work(&dev->watchdog_work);
 
 	video_unregister_device(dev->vfd_enc);
 	video_unregister_device(dev->vfd_dec);
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_common.h b/drivers/media/platform/s5p-mfc/s5p_mfc_common.h
index 373e346fce3e..46b99f28cbd7 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_common.h
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_common.h
@@ -292,7 +292,9 @@ struct s5p_mfc_priv_buf {
  * @warn_start:		hardware error code from which warnings start
  * @mfc_ops:		ops structure holding HW operation function pointers
  * @mfc_cmds:		cmd structure holding HW commands function pointers
+ * @mfc_regs:		structure holding MFC registers
  * @fw_ver:		loaded firmware sub-version
+ * risc_on:		flag indicates RISC is on or off
  *
  */
 struct s5p_mfc_dev {
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_dec.c b/drivers/media/platform/s5p-mfc/s5p_mfc_dec.c
index 47c997d9e8cb..52081ddc9bf2 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_dec.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_dec.c
@@ -776,11 +776,12 @@ static int vidioc_g_crop(struct file *file, void *priv,
 	u32 left, right, top, bottom;
 
 	if (ctx->state != MFCINST_HEAD_PARSED &&
-	ctx->state != MFCINST_RUNNING && ctx->state != MFCINST_FINISHING
-					&& ctx->state != MFCINST_FINISHED) {
-			mfc_err("Cannont set crop\n");
-			return -EINVAL;
-		}
+	    ctx->state != MFCINST_RUNNING &&
+	    ctx->state != MFCINST_FINISHING &&
+	    ctx->state != MFCINST_FINISHED) {
+		mfc_err("Can not get crop information\n");
+		return -EINVAL;
+	}
 	if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_H264) {
 		left = s5p_mfc_hw_call(dev->mfc_ops, get_crop_info_h, ctx);
 		right = left >> S5P_FIMV_SHARED_CROP_RIGHT_SHIFT;
diff --git a/drivers/media/platform/s5p-tv/Kconfig b/drivers/media/platform/s5p-tv/Kconfig
deleted file mode 100644
index 697aaed42486..000000000000
--- a/drivers/media/platform/s5p-tv/Kconfig
+++ /dev/null
@@ -1,88 +0,0 @@
-# drivers/media/platform/s5p-tv/Kconfig
-#
-# Copyright (c) 2010-2011 Samsung Electronics Co., Ltd.
-#	http://www.samsung.com/
-# Tomasz Stanislawski <t.stanislaws@samsung.com>
-#
-# Licensed under GPL
-
-config VIDEO_SAMSUNG_S5P_TV
-	bool "Samsung TV driver for S5P platform"
-	depends on PM
-	depends on ARCH_S5PV210 || ARCH_EXYNOS || COMPILE_TEST
-	default n
-	---help---
-	  Say Y here to enable selecting the TV output devices for
-	  Samsung S5P platform.
-
-if VIDEO_SAMSUNG_S5P_TV
-
-config VIDEO_SAMSUNG_S5P_HDMI
-	tristate "Samsung HDMI Driver"
-	depends on VIDEO_V4L2
-	depends on I2C
-	depends on VIDEO_SAMSUNG_S5P_TV
-	select VIDEO_SAMSUNG_S5P_HDMIPHY
-	help
-	  Say Y here if you want support for the HDMI output
-	  interface in S5P Samsung SoC. The driver can be compiled
-	  as module. It is an auxiliary driver, that exposes a V4L2
-	  subdev for use by other drivers. This driver requires
-	  hdmiphy driver to work correctly.
-
-config VIDEO_SAMSUNG_S5P_HDMI_DEBUG
-	bool "Enable debug for HDMI Driver"
-	depends on VIDEO_SAMSUNG_S5P_HDMI
-	default n
-	help
-	  Enables debugging for HDMI driver.
-
-config VIDEO_SAMSUNG_S5P_HDMIPHY
-	tristate "Samsung HDMIPHY Driver"
-	depends on VIDEO_DEV && VIDEO_V4L2 && I2C
-	depends on VIDEO_SAMSUNG_S5P_TV
-	help
-	  Say Y here if you want support for the physical HDMI
-	  interface in S5P Samsung SoC. The driver can be compiled
-	  as module. It is an I2C driver, that exposes a V4L2
-	  subdev for use by other drivers.
-
-config VIDEO_SAMSUNG_S5P_SII9234
-	tristate "Samsung SII9234 Driver"
-	depends on VIDEO_DEV && VIDEO_V4L2 && I2C
-	depends on VIDEO_SAMSUNG_S5P_TV
-	help
-	  Say Y here if you want support for the MHL interface
-	  in S5P Samsung SoC. The driver can be compiled
-	  as module. It is an I2C driver, that exposes a V4L2
-	  subdev for use by other drivers.
-
-config VIDEO_SAMSUNG_S5P_SDO
-	tristate "Samsung Analog TV Driver"
-	depends on VIDEO_DEV && VIDEO_V4L2
-	depends on VIDEO_SAMSUNG_S5P_TV
-	help
-	  Say Y here if you want support for the analog TV output
-	  interface in S5P Samsung SoC. The driver can be compiled
-	  as module. It is an auxiliary driver, that exposes a V4L2
-	  subdev for use by other drivers. This driver requires
-	  hdmiphy driver to work correctly.
-
-config VIDEO_SAMSUNG_S5P_MIXER
-	tristate "Samsung Mixer and Video Processor Driver"
-	depends on VIDEO_DEV && VIDEO_V4L2
-	depends on VIDEO_SAMSUNG_S5P_TV
-	depends on HAS_DMA
-	select VIDEOBUF2_DMA_CONTIG
-	help
-	  Say Y here if you want support for the Mixer in Samsung S5P SoCs.
-	  This device produce image data to one of output interfaces.
-
-config VIDEO_SAMSUNG_S5P_MIXER_DEBUG
-	bool "Enable debug for Mixer Driver"
-	depends on VIDEO_SAMSUNG_S5P_MIXER
-	default n
-	help
-	  Enables debugging for Mixer driver.
-
-endif # VIDEO_SAMSUNG_S5P_TV
diff --git a/drivers/media/platform/s5p-tv/Makefile b/drivers/media/platform/s5p-tv/Makefile
deleted file mode 100644
index 7cd47902e269..000000000000
--- a/drivers/media/platform/s5p-tv/Makefile
+++ /dev/null
@@ -1,19 +0,0 @@
-# drivers/media/platform/samsung/tvout/Makefile
-#
-# Copyright (c) 2010-2011 Samsung Electronics Co., Ltd.
-#	http://www.samsung.com/
-# Tomasz Stanislawski <t.stanislaws@samsung.com>
-#
-# Licensed under GPL
-
-obj-$(CONFIG_VIDEO_SAMSUNG_S5P_HDMIPHY) += s5p-hdmiphy.o
-s5p-hdmiphy-y += hdmiphy_drv.o
-obj-$(CONFIG_VIDEO_SAMSUNG_S5P_SII9234) += s5p-sii9234.o
-s5p-sii9234-y += sii9234_drv.o
-obj-$(CONFIG_VIDEO_SAMSUNG_S5P_HDMI) += s5p-hdmi.o
-s5p-hdmi-y += hdmi_drv.o
-obj-$(CONFIG_VIDEO_SAMSUNG_S5P_SDO) += s5p-sdo.o
-s5p-sdo-y += sdo_drv.o
-obj-$(CONFIG_VIDEO_SAMSUNG_S5P_MIXER) += s5p-mixer.o
-s5p-mixer-y += mixer_drv.o mixer_video.o mixer_reg.o mixer_grp_layer.o mixer_vp_layer.o
-
diff --git a/drivers/media/platform/s5p-tv/hdmi_drv.c b/drivers/media/platform/s5p-tv/hdmi_drv.c
deleted file mode 100644
index e71b13e40f59..000000000000
--- a/drivers/media/platform/s5p-tv/hdmi_drv.c
+++ /dev/null
@@ -1,1059 +0,0 @@
-/*
- * Samsung HDMI interface driver
- *
- * Copyright (c) 2010-2011 Samsung Electronics Co., Ltd.
- *
- * Tomasz Stanislawski, <t.stanislaws@samsung.com>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published
- * by the Free Software Foundiation. either version 2 of the License,
- * or (at your option) any later version
- */
-
-#define pr_fmt(fmt) "s5p-tv (hdmi_drv): " fmt
-
-#ifdef CONFIG_VIDEO_SAMSUNG_S5P_HDMI_DEBUG
-#define DEBUG
-#endif
-
-#include <linux/kernel.h>
-#include <linux/slab.h>
-#include <linux/io.h>
-#include <linux/i2c.h>
-#include <linux/platform_device.h>
-#include <media/v4l2-subdev.h>
-#include <linux/module.h>
-#include <linux/interrupt.h>
-#include <linux/irq.h>
-#include <linux/delay.h>
-#include <linux/bug.h>
-#include <linux/pm_runtime.h>
-#include <linux/clk.h>
-#include <linux/regulator/consumer.h>
-#include <linux/v4l2-dv-timings.h>
-
-#include <linux/platform_data/media/s5p_hdmi.h>
-#include <media/v4l2-common.h>
-#include <media/v4l2-dev.h>
-#include <media/v4l2-device.h>
-#include <media/v4l2-dv-timings.h>
-
-#include "regs-hdmi.h"
-
-MODULE_AUTHOR("Tomasz Stanislawski, <t.stanislaws@samsung.com>");
-MODULE_DESCRIPTION("Samsung HDMI");
-MODULE_LICENSE("GPL");
-
-struct hdmi_pulse {
-	u32 beg;
-	u32 end;
-};
-
-struct hdmi_timings {
-	struct hdmi_pulse hact;
-	u32 hsyn_pol; /* 0 - high, 1 - low */
-	struct hdmi_pulse hsyn;
-	u32 interlaced;
-	struct hdmi_pulse vact[2];
-	u32 vsyn_pol; /* 0 - high, 1 - low */
-	u32 vsyn_off;
-	struct hdmi_pulse vsyn[2];
-};
-
-struct hdmi_resources {
-	struct clk *hdmi;
-	struct clk *sclk_hdmi;
-	struct clk *sclk_pixel;
-	struct clk *sclk_hdmiphy;
-	struct clk *hdmiphy;
-	struct regulator_bulk_data *regul_bulk;
-	int regul_count;
-};
-
-struct hdmi_device {
-	/** base address of HDMI registers */
-	void __iomem *regs;
-	/** HDMI interrupt */
-	unsigned int irq;
-	/** pointer to device parent */
-	struct device *dev;
-	/** subdev generated by HDMI device */
-	struct v4l2_subdev sd;
-	/** V4L2 device structure */
-	struct v4l2_device v4l2_dev;
-	/** subdev of HDMIPHY interface */
-	struct v4l2_subdev *phy_sd;
-	/** subdev of MHL interface */
-	struct v4l2_subdev *mhl_sd;
-	/** configuration of current graphic mode */
-	const struct hdmi_timings *cur_conf;
-	/** flag indicating that timings are dirty */
-	int cur_conf_dirty;
-	/** current timings */
-	struct v4l2_dv_timings cur_timings;
-	/** other resources */
-	struct hdmi_resources res;
-};
-
-static const struct platform_device_id hdmi_driver_types[] = {
-	{
-		.name		= "s5pv210-hdmi",
-	}, {
-		.name		= "exynos4-hdmi",
-	}, {
-		/* end node */
-	}
-};
-
-static const struct v4l2_subdev_ops hdmi_sd_ops;
-
-static struct hdmi_device *sd_to_hdmi_dev(struct v4l2_subdev *sd)
-{
-	return container_of(sd, struct hdmi_device, sd);
-}
-
-static inline
-void hdmi_write(struct hdmi_device *hdev, u32 reg_id, u32 value)
-{
-	writel(value, hdev->regs + reg_id);
-}
-
-static inline
-void hdmi_write_mask(struct hdmi_device *hdev, u32 reg_id, u32 value, u32 mask)
-{
-	u32 old = readl(hdev->regs + reg_id);
-	value = (value & mask) | (old & ~mask);
-	writel(value, hdev->regs + reg_id);
-}
-
-static inline
-void hdmi_writeb(struct hdmi_device *hdev, u32 reg_id, u8 value)
-{
-	writeb(value, hdev->regs + reg_id);
-}
-
-static inline
-void hdmi_writebn(struct hdmi_device *hdev, u32 reg_id, int n, u32 value)
-{
-	switch (n) {
-	default:
-		writeb(value >> 24, hdev->regs + reg_id + 12);
-	case 3:
-		writeb(value >> 16, hdev->regs + reg_id + 8);
-	case 2:
-		writeb(value >>  8, hdev->regs + reg_id + 4);
-	case 1:
-		writeb(value >>  0, hdev->regs + reg_id + 0);
-	}
-}
-
-static inline u32 hdmi_read(struct hdmi_device *hdev, u32 reg_id)
-{
-	return readl(hdev->regs + reg_id);
-}
-
-static irqreturn_t hdmi_irq_handler(int irq, void *dev_data)
-{
-	struct hdmi_device *hdev = dev_data;
-	u32 intc_flag;
-
-	(void)irq;
-	intc_flag = hdmi_read(hdev, HDMI_INTC_FLAG);
-	/* clearing flags for HPD plug/unplug */
-	if (intc_flag & HDMI_INTC_FLAG_HPD_UNPLUG) {
-		pr_info("unplugged\n");
-		hdmi_write_mask(hdev, HDMI_INTC_FLAG, ~0,
-			HDMI_INTC_FLAG_HPD_UNPLUG);
-	}
-	if (intc_flag & HDMI_INTC_FLAG_HPD_PLUG) {
-		pr_info("plugged\n");
-		hdmi_write_mask(hdev, HDMI_INTC_FLAG, ~0,
-			HDMI_INTC_FLAG_HPD_PLUG);
-	}
-
-	return IRQ_HANDLED;
-}
-
-static void hdmi_reg_init(struct hdmi_device *hdev)
-{
-	/* enable HPD interrupts */
-	hdmi_write_mask(hdev, HDMI_INTC_CON, ~0, HDMI_INTC_EN_GLOBAL |
-		HDMI_INTC_EN_HPD_PLUG | HDMI_INTC_EN_HPD_UNPLUG);
-	/* choose DVI mode */
-	hdmi_write_mask(hdev, HDMI_MODE_SEL,
-		HDMI_MODE_DVI_EN, HDMI_MODE_MASK);
-	hdmi_write_mask(hdev, HDMI_CON_2, ~0,
-		HDMI_DVI_PERAMBLE_EN | HDMI_DVI_BAND_EN);
-	/* disable bluescreen */
-	hdmi_write_mask(hdev, HDMI_CON_0, 0, HDMI_BLUE_SCR_EN);
-	/* choose bluescreen (fecal) color */
-	hdmi_writeb(hdev, HDMI_BLUE_SCREEN_0, 0x12);
-	hdmi_writeb(hdev, HDMI_BLUE_SCREEN_1, 0x34);
-	hdmi_writeb(hdev, HDMI_BLUE_SCREEN_2, 0x56);
-}
-
-static void hdmi_timing_apply(struct hdmi_device *hdev,
-	const struct hdmi_timings *t)
-{
-	/* setting core registers */
-	hdmi_writebn(hdev, HDMI_H_BLANK_0, 2, t->hact.beg);
-	hdmi_writebn(hdev, HDMI_H_SYNC_GEN_0, 3,
-		(t->hsyn_pol << 20) | (t->hsyn.end << 10) | t->hsyn.beg);
-	hdmi_writeb(hdev, HDMI_VSYNC_POL, t->vsyn_pol);
-	hdmi_writebn(hdev, HDMI_V_BLANK_0, 3,
-		(t->vact[0].beg << 11) | t->vact[0].end);
-	hdmi_writebn(hdev, HDMI_V_SYNC_GEN_1_0, 3,
-		(t->vsyn[0].beg << 12) | t->vsyn[0].end);
-	if (t->interlaced) {
-		u32 vsyn_trans = t->hsyn.beg + t->vsyn_off;
-
-		hdmi_writeb(hdev, HDMI_INT_PRO_MODE, 1);
-		hdmi_writebn(hdev, HDMI_H_V_LINE_0, 3,
-			(t->hact.end << 12) | t->vact[1].end);
-		hdmi_writebn(hdev, HDMI_V_BLANK_F_0, 3,
-			(t->vact[1].end << 11) | t->vact[1].beg);
-		hdmi_writebn(hdev, HDMI_V_SYNC_GEN_2_0, 3,
-			(t->vsyn[1].beg << 12) | t->vsyn[1].end);
-		hdmi_writebn(hdev, HDMI_V_SYNC_GEN_3_0, 3,
-			(vsyn_trans << 12) | vsyn_trans);
-	} else {
-		hdmi_writeb(hdev, HDMI_INT_PRO_MODE, 0);
-		hdmi_writebn(hdev, HDMI_H_V_LINE_0, 3,
-			(t->hact.end << 12) | t->vact[0].end);
-	}
-
-	/* Timing generator registers */
-	hdmi_writebn(hdev, HDMI_TG_H_FSZ_L, 2, t->hact.end);
-	hdmi_writebn(hdev, HDMI_TG_HACT_ST_L, 2, t->hact.beg);
-	hdmi_writebn(hdev, HDMI_TG_HACT_SZ_L, 2, t->hact.end - t->hact.beg);
-	hdmi_writebn(hdev, HDMI_TG_VSYNC_L, 2, t->vsyn[0].beg);
-	hdmi_writebn(hdev, HDMI_TG_VACT_ST_L, 2, t->vact[0].beg);
-	hdmi_writebn(hdev, HDMI_TG_VACT_SZ_L, 2,
-		t->vact[0].end - t->vact[0].beg);
-	hdmi_writebn(hdev, HDMI_TG_VSYNC_TOP_HDMI_L, 2, t->vsyn[0].beg);
-	hdmi_writebn(hdev, HDMI_TG_FIELD_TOP_HDMI_L, 2, t->vsyn[0].beg);
-	if (t->interlaced) {
-		hdmi_write_mask(hdev, HDMI_TG_CMD, ~0, HDMI_TG_FIELD_EN);
-		hdmi_writebn(hdev, HDMI_TG_V_FSZ_L, 2, t->vact[1].end);
-		hdmi_writebn(hdev, HDMI_TG_VSYNC2_L, 2, t->vsyn[1].beg);
-		hdmi_writebn(hdev, HDMI_TG_FIELD_CHG_L, 2, t->vact[0].end);
-		hdmi_writebn(hdev, HDMI_TG_VACT_ST2_L, 2, t->vact[1].beg);
-		hdmi_writebn(hdev, HDMI_TG_VSYNC_BOT_HDMI_L, 2, t->vsyn[1].beg);
-		hdmi_writebn(hdev, HDMI_TG_FIELD_BOT_HDMI_L, 2, t->vsyn[1].beg);
-	} else {
-		hdmi_write_mask(hdev, HDMI_TG_CMD, 0, HDMI_TG_FIELD_EN);
-		hdmi_writebn(hdev, HDMI_TG_V_FSZ_L, 2, t->vact[0].end);
-	}
-}
-
-static int hdmi_conf_apply(struct hdmi_device *hdmi_dev)
-{
-	struct device *dev = hdmi_dev->dev;
-	const struct hdmi_timings *conf = hdmi_dev->cur_conf;
-	int ret;
-
-	dev_dbg(dev, "%s\n", __func__);
-
-	/* skip if conf is already synchronized with HW */
-	if (!hdmi_dev->cur_conf_dirty)
-		return 0;
-
-	/* reset hdmiphy */
-	hdmi_write_mask(hdmi_dev, HDMI_PHY_RSTOUT, ~0, HDMI_PHY_SW_RSTOUT);
-	mdelay(10);
-	hdmi_write_mask(hdmi_dev, HDMI_PHY_RSTOUT,  0, HDMI_PHY_SW_RSTOUT);
-	mdelay(10);
-
-	/* configure timings */
-	ret = v4l2_subdev_call(hdmi_dev->phy_sd, video, s_dv_timings,
-				&hdmi_dev->cur_timings);
-	if (ret) {
-		dev_err(dev, "failed to set timings\n");
-		return ret;
-	}
-
-	/* resetting HDMI core */
-	hdmi_write_mask(hdmi_dev, HDMI_CORE_RSTOUT,  0, HDMI_CORE_SW_RSTOUT);
-	mdelay(10);
-	hdmi_write_mask(hdmi_dev, HDMI_CORE_RSTOUT, ~0, HDMI_CORE_SW_RSTOUT);
-	mdelay(10);
-
-	hdmi_reg_init(hdmi_dev);
-
-	/* setting core registers */
-	hdmi_timing_apply(hdmi_dev, conf);
-
-	hdmi_dev->cur_conf_dirty = 0;
-
-	return 0;
-}
-
-static void hdmi_dumpregs(struct hdmi_device *hdev, char *prefix)
-{
-#define DUMPREG(reg_id) \
-	dev_dbg(hdev->dev, "%s:" #reg_id " = %08x\n", prefix, \
-		readl(hdev->regs + reg_id))
-
-	dev_dbg(hdev->dev, "%s: ---- CONTROL REGISTERS ----\n", prefix);
-	DUMPREG(HDMI_INTC_FLAG);
-	DUMPREG(HDMI_INTC_CON);
-	DUMPREG(HDMI_HPD_STATUS);
-	DUMPREG(HDMI_PHY_RSTOUT);
-	DUMPREG(HDMI_PHY_VPLL);
-	DUMPREG(HDMI_PHY_CMU);
-	DUMPREG(HDMI_CORE_RSTOUT);
-
-	dev_dbg(hdev->dev, "%s: ---- CORE REGISTERS ----\n", prefix);
-	DUMPREG(HDMI_CON_0);
-	DUMPREG(HDMI_CON_1);
-	DUMPREG(HDMI_CON_2);
-	DUMPREG(HDMI_SYS_STATUS);
-	DUMPREG(HDMI_PHY_STATUS);
-	DUMPREG(HDMI_STATUS_EN);
-	DUMPREG(HDMI_HPD);
-	DUMPREG(HDMI_MODE_SEL);
-	DUMPREG(HDMI_HPD_GEN);
-	DUMPREG(HDMI_DC_CONTROL);
-	DUMPREG(HDMI_VIDEO_PATTERN_GEN);
-
-	dev_dbg(hdev->dev, "%s: ---- CORE SYNC REGISTERS ----\n", prefix);
-	DUMPREG(HDMI_H_BLANK_0);
-	DUMPREG(HDMI_H_BLANK_1);
-	DUMPREG(HDMI_V_BLANK_0);
-	DUMPREG(HDMI_V_BLANK_1);
-	DUMPREG(HDMI_V_BLANK_2);
-	DUMPREG(HDMI_H_V_LINE_0);
-	DUMPREG(HDMI_H_V_LINE_1);
-	DUMPREG(HDMI_H_V_LINE_2);
-	DUMPREG(HDMI_VSYNC_POL);
-	DUMPREG(HDMI_INT_PRO_MODE);
-	DUMPREG(HDMI_V_BLANK_F_0);
-	DUMPREG(HDMI_V_BLANK_F_1);
-	DUMPREG(HDMI_V_BLANK_F_2);
-	DUMPREG(HDMI_H_SYNC_GEN_0);
-	DUMPREG(HDMI_H_SYNC_GEN_1);
-	DUMPREG(HDMI_H_SYNC_GEN_2);
-	DUMPREG(HDMI_V_SYNC_GEN_1_0);
-	DUMPREG(HDMI_V_SYNC_GEN_1_1);
-	DUMPREG(HDMI_V_SYNC_GEN_1_2);
-	DUMPREG(HDMI_V_SYNC_GEN_2_0);
-	DUMPREG(HDMI_V_SYNC_GEN_2_1);
-	DUMPREG(HDMI_V_SYNC_GEN_2_2);
-	DUMPREG(HDMI_V_SYNC_GEN_3_0);
-	DUMPREG(HDMI_V_SYNC_GEN_3_1);
-	DUMPREG(HDMI_V_SYNC_GEN_3_2);
-
-	dev_dbg(hdev->dev, "%s: ---- TG REGISTERS ----\n", prefix);
-	DUMPREG(HDMI_TG_CMD);
-	DUMPREG(HDMI_TG_H_FSZ_L);
-	DUMPREG(HDMI_TG_H_FSZ_H);
-	DUMPREG(HDMI_TG_HACT_ST_L);
-	DUMPREG(HDMI_TG_HACT_ST_H);
-	DUMPREG(HDMI_TG_HACT_SZ_L);
-	DUMPREG(HDMI_TG_HACT_SZ_H);
-	DUMPREG(HDMI_TG_V_FSZ_L);
-	DUMPREG(HDMI_TG_V_FSZ_H);
-	DUMPREG(HDMI_TG_VSYNC_L);
-	DUMPREG(HDMI_TG_VSYNC_H);
-	DUMPREG(HDMI_TG_VSYNC2_L);
-	DUMPREG(HDMI_TG_VSYNC2_H);
-	DUMPREG(HDMI_TG_VACT_ST_L);
-	DUMPREG(HDMI_TG_VACT_ST_H);
-	DUMPREG(HDMI_TG_VACT_SZ_L);
-	DUMPREG(HDMI_TG_VACT_SZ_H);
-	DUMPREG(HDMI_TG_FIELD_CHG_L);
-	DUMPREG(HDMI_TG_FIELD_CHG_H);
-	DUMPREG(HDMI_TG_VACT_ST2_L);
-	DUMPREG(HDMI_TG_VACT_ST2_H);
-	DUMPREG(HDMI_TG_VSYNC_TOP_HDMI_L);
-	DUMPREG(HDMI_TG_VSYNC_TOP_HDMI_H);
-	DUMPREG(HDMI_TG_VSYNC_BOT_HDMI_L);
-	DUMPREG(HDMI_TG_VSYNC_BOT_HDMI_H);
-	DUMPREG(HDMI_TG_FIELD_TOP_HDMI_L);
-	DUMPREG(HDMI_TG_FIELD_TOP_HDMI_H);
-	DUMPREG(HDMI_TG_FIELD_BOT_HDMI_L);
-	DUMPREG(HDMI_TG_FIELD_BOT_HDMI_H);
-#undef DUMPREG
-}
-
-static const struct hdmi_timings hdmi_timings_480p = {
-	.hact = { .beg = 138, .end = 858 },
-	.hsyn_pol = 1,
-	.hsyn = { .beg = 16, .end = 16 + 62 },
-	.interlaced = 0,
-	.vact[0] = { .beg = 42 + 3, .end = 522 + 3 },
-	.vsyn_pol = 1,
-	.vsyn[0] = { .beg = 6 + 3, .end = 12 + 3},
-};
-
-static const struct hdmi_timings hdmi_timings_576p50 = {
-	.hact = { .beg = 144, .end = 864 },
-	.hsyn_pol = 1,
-	.hsyn = { .beg = 12, .end = 12 + 64 },
-	.interlaced = 0,
-	.vact[0] = { .beg = 44 + 5, .end = 620 + 5 },
-	.vsyn_pol = 1,
-	.vsyn[0] = { .beg = 0 + 5, .end = 5 + 5},
-};
-
-static const struct hdmi_timings hdmi_timings_720p60 = {
-	.hact = { .beg = 370, .end = 1650 },
-	.hsyn_pol = 0,
-	.hsyn = { .beg = 110, .end = 110 + 40 },
-	.interlaced = 0,
-	.vact[0] = { .beg = 25 + 5, .end = 745 + 5 },
-	.vsyn_pol = 0,
-	.vsyn[0] = { .beg = 0 + 5, .end = 5 + 5},
-};
-
-static const struct hdmi_timings hdmi_timings_720p50 = {
-	.hact = { .beg = 700, .end = 1980 },
-	.hsyn_pol = 0,
-	.hsyn = { .beg = 440, .end = 440 + 40 },
-	.interlaced = 0,
-	.vact[0] = { .beg = 25 + 5, .end = 745 + 5 },
-	.vsyn_pol = 0,
-	.vsyn[0] = { .beg = 0 + 5, .end = 5 + 5},
-};
-
-static const struct hdmi_timings hdmi_timings_1080p24 = {
-	.hact = { .beg = 830, .end = 2750 },
-	.hsyn_pol = 0,
-	.hsyn = { .beg = 638, .end = 638 + 44 },
-	.interlaced = 0,
-	.vact[0] = { .beg = 41 + 4, .end = 1121 + 4 },
-	.vsyn_pol = 0,
-	.vsyn[0] = { .beg = 0 + 4, .end = 5 + 4},
-};
-
-static const struct hdmi_timings hdmi_timings_1080p60 = {
-	.hact = { .beg = 280, .end = 2200 },
-	.hsyn_pol = 0,
-	.hsyn = { .beg = 88, .end = 88 + 44 },
-	.interlaced = 0,
-	.vact[0] = { .beg = 41 + 4, .end = 1121 + 4 },
-	.vsyn_pol = 0,
-	.vsyn[0] = { .beg = 0 + 4, .end = 5 + 4},
-};
-
-static const struct hdmi_timings hdmi_timings_1080i60 = {
-	.hact = { .beg = 280, .end = 2200 },
-	.hsyn_pol = 0,
-	.hsyn = { .beg = 88, .end = 88 + 44 },
-	.interlaced = 1,
-	.vact[0] = { .beg = 20 + 2, .end = 560 + 2 },
-	.vact[1] = { .beg = 583 + 2, .end = 1123 + 2 },
-	.vsyn_pol = 0,
-	.vsyn_off = 1100,
-	.vsyn[0] = { .beg = 0 + 2, .end = 5 + 2},
-	.vsyn[1] = { .beg = 562 + 2, .end = 567 + 2},
-};
-
-static const struct hdmi_timings hdmi_timings_1080i50 = {
-	.hact = { .beg = 720, .end = 2640 },
-	.hsyn_pol = 0,
-	.hsyn = { .beg = 528, .end = 528 + 44 },
-	.interlaced = 1,
-	.vact[0] = { .beg = 20 + 2, .end = 560 + 2 },
-	.vact[1] = { .beg = 583 + 2, .end = 1123 + 2 },
-	.vsyn_pol = 0,
-	.vsyn_off = 1320,
-	.vsyn[0] = { .beg = 0 + 2, .end = 5 + 2},
-	.vsyn[1] = { .beg = 562 + 2, .end = 567 + 2},
-};
-
-static const struct hdmi_timings hdmi_timings_1080p50 = {
-	.hact = { .beg = 720, .end = 2640 },
-	.hsyn_pol = 0,
-	.hsyn = { .beg = 528, .end = 528 + 44 },
-	.interlaced = 0,
-	.vact[0] = { .beg = 41 + 4, .end = 1121 + 4 },
-	.vsyn_pol = 0,
-	.vsyn[0] = { .beg = 0 + 4, .end = 5 + 4},
-};
-
-/* default hdmi_timings index of the timings configured on probe */
-#define HDMI_DEFAULT_TIMINGS_IDX (0)
-
-static const struct {
-	bool reduced_fps;
-	const struct v4l2_dv_timings dv_timings;
-	const struct hdmi_timings *hdmi_timings;
-} hdmi_timings[] = {
-	{ false, V4L2_DV_BT_CEA_720X480P59_94, &hdmi_timings_480p    },
-	{ false, V4L2_DV_BT_CEA_720X576P50,    &hdmi_timings_576p50  },
-	{ false, V4L2_DV_BT_CEA_1280X720P50,   &hdmi_timings_720p50  },
-	{ true,  V4L2_DV_BT_CEA_1280X720P60,   &hdmi_timings_720p60  },
-	{ false, V4L2_DV_BT_CEA_1920X1080P24,  &hdmi_timings_1080p24 },
-	{ false, V4L2_DV_BT_CEA_1920X1080P30,  &hdmi_timings_1080p60 },
-	{ false, V4L2_DV_BT_CEA_1920X1080P50,  &hdmi_timings_1080p50 },
-	{ false, V4L2_DV_BT_CEA_1920X1080I50,  &hdmi_timings_1080i50 },
-	{ false, V4L2_DV_BT_CEA_1920X1080I60,  &hdmi_timings_1080i60 },
-	{ false, V4L2_DV_BT_CEA_1920X1080P60,  &hdmi_timings_1080p60 },
-};
-
-static int hdmi_streamon(struct hdmi_device *hdev)
-{
-	struct device *dev = hdev->dev;
-	struct hdmi_resources *res = &hdev->res;
-	int ret, tries;
-
-	dev_dbg(dev, "%s\n", __func__);
-
-	ret = hdmi_conf_apply(hdev);
-	if (ret)
-		return ret;
-
-	ret = v4l2_subdev_call(hdev->phy_sd, video, s_stream, 1);
-	if (ret)
-		return ret;
-
-	/* waiting for HDMIPHY's PLL to get to steady state */
-	for (tries = 100; tries; --tries) {
-		u32 val = hdmi_read(hdev, HDMI_PHY_STATUS);
-		if (val & HDMI_PHY_STATUS_READY)
-			break;
-		mdelay(1);
-	}
-	/* steady state not achieved */
-	if (tries == 0) {
-		dev_err(dev, "hdmiphy's pll could not reach steady state.\n");
-		v4l2_subdev_call(hdev->phy_sd, video, s_stream, 0);
-		hdmi_dumpregs(hdev, "hdmiphy - s_stream");
-		return -EIO;
-	}
-
-	/* starting MHL */
-	ret = v4l2_subdev_call(hdev->mhl_sd, video, s_stream, 1);
-	if (hdev->mhl_sd && ret) {
-		v4l2_subdev_call(hdev->phy_sd, video, s_stream, 0);
-		hdmi_dumpregs(hdev, "mhl - s_stream");
-		return -EIO;
-	}
-
-	/* hdmiphy clock is used for HDMI in streaming mode */
-	clk_disable(res->sclk_hdmi);
-	clk_set_parent(res->sclk_hdmi, res->sclk_hdmiphy);
-	clk_enable(res->sclk_hdmi);
-
-	/* enable HDMI and timing generator */
-	hdmi_write_mask(hdev, HDMI_CON_0, ~0, HDMI_EN);
-	hdmi_write_mask(hdev, HDMI_TG_CMD, ~0, HDMI_TG_EN);
-	hdmi_dumpregs(hdev, "streamon");
-	return 0;
-}
-
-static int hdmi_streamoff(struct hdmi_device *hdev)
-{
-	struct device *dev = hdev->dev;
-	struct hdmi_resources *res = &hdev->res;
-
-	dev_dbg(dev, "%s\n", __func__);
-
-	hdmi_write_mask(hdev, HDMI_CON_0, 0, HDMI_EN);
-	hdmi_write_mask(hdev, HDMI_TG_CMD, 0, HDMI_TG_EN);
-
-	/* pixel(vpll) clock is used for HDMI in config mode */
-	clk_disable(res->sclk_hdmi);
-	clk_set_parent(res->sclk_hdmi, res->sclk_pixel);
-	clk_enable(res->sclk_hdmi);
-
-	v4l2_subdev_call(hdev->mhl_sd, video, s_stream, 0);
-	v4l2_subdev_call(hdev->phy_sd, video, s_stream, 0);
-
-	hdmi_dumpregs(hdev, "streamoff");
-	return 0;
-}
-
-static int hdmi_s_stream(struct v4l2_subdev *sd, int enable)
-{
-	struct hdmi_device *hdev = sd_to_hdmi_dev(sd);
-	struct device *dev = hdev->dev;
-
-	dev_dbg(dev, "%s(%d)\n", __func__, enable);
-	if (enable)
-		return hdmi_streamon(hdev);
-	return hdmi_streamoff(hdev);
-}
-
-static int hdmi_resource_poweron(struct hdmi_resources *res)
-{
-	int ret;
-
-	/* turn HDMI power on */
-	ret = regulator_bulk_enable(res->regul_count, res->regul_bulk);
-	if (ret < 0)
-		return ret;
-	/* power-on hdmi physical interface */
-	clk_enable(res->hdmiphy);
-	/* use VPP as parent clock; HDMIPHY is not working yet */
-	clk_set_parent(res->sclk_hdmi, res->sclk_pixel);
-	/* turn clocks on */
-	clk_enable(res->sclk_hdmi);
-
-	return 0;
-}
-
-static void hdmi_resource_poweroff(struct hdmi_resources *res)
-{
-	/* turn clocks off */
-	clk_disable(res->sclk_hdmi);
-	/* power-off hdmiphy */
-	clk_disable(res->hdmiphy);
-	/* turn HDMI power off */
-	regulator_bulk_disable(res->regul_count, res->regul_bulk);
-}
-
-static int hdmi_s_power(struct v4l2_subdev *sd, int on)
-{
-	struct hdmi_device *hdev = sd_to_hdmi_dev(sd);
-	int ret;
-
-	if (on)
-		ret = pm_runtime_get_sync(hdev->dev);
-	else
-		ret = pm_runtime_put_sync(hdev->dev);
-	/* only values < 0 indicate errors */
-	return ret < 0 ? ret : 0;
-}
-
-static int hdmi_s_dv_timings(struct v4l2_subdev *sd,
-	struct v4l2_dv_timings *timings)
-{
-	struct hdmi_device *hdev = sd_to_hdmi_dev(sd);
-	struct device *dev = hdev->dev;
-	int i;
-
-	for (i = 0; i < ARRAY_SIZE(hdmi_timings); i++)
-		if (v4l2_match_dv_timings(&hdmi_timings[i].dv_timings,
-					timings, 0, false))
-			break;
-	if (i == ARRAY_SIZE(hdmi_timings)) {
-		dev_err(dev, "timings not supported\n");
-		return -EINVAL;
-	}
-	hdev->cur_conf = hdmi_timings[i].hdmi_timings;
-	hdev->cur_conf_dirty = 1;
-	hdev->cur_timings = *timings;
-	if (!hdmi_timings[i].reduced_fps)
-		hdev->cur_timings.bt.flags &= ~V4L2_DV_FL_CAN_REDUCE_FPS;
-	return 0;
-}
-
-static int hdmi_g_dv_timings(struct v4l2_subdev *sd,
-	struct v4l2_dv_timings *timings)
-{
-	*timings = sd_to_hdmi_dev(sd)->cur_timings;
-	return 0;
-}
-
-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;
-	fmt->code = MEDIA_BUS_FMT_FIXED; /* means RGB888 */
-	fmt->colorspace = V4L2_COLORSPACE_SRGB;
-	if (t->interlaced) {
-		fmt->field = V4L2_FIELD_INTERLACED;
-		fmt->height *= 2;
-	} else {
-		fmt->field = V4L2_FIELD_NONE;
-	}
-	return 0;
-}
-
-static int hdmi_enum_dv_timings(struct v4l2_subdev *sd,
-	struct v4l2_enum_dv_timings *timings)
-{
-	if (timings->pad != 0)
-		return -EINVAL;
-	if (timings->index >= ARRAY_SIZE(hdmi_timings))
-		return -EINVAL;
-	timings->timings = hdmi_timings[timings->index].dv_timings;
-	if (!hdmi_timings[timings->index].reduced_fps)
-		timings->timings.bt.flags &= ~V4L2_DV_FL_CAN_REDUCE_FPS;
-	return 0;
-}
-
-static int hdmi_dv_timings_cap(struct v4l2_subdev *sd,
-	struct v4l2_dv_timings_cap *cap)
-{
-	struct hdmi_device *hdev = sd_to_hdmi_dev(sd);
-
-	if (cap->pad != 0)
-		return -EINVAL;
-
-	/* Let the phy fill in the pixelclock range */
-	v4l2_subdev_call(hdev->phy_sd, pad, dv_timings_cap, cap);
-	cap->type = V4L2_DV_BT_656_1120;
-	cap->bt.min_width = 720;
-	cap->bt.max_width = 1920;
-	cap->bt.min_height = 480;
-	cap->bt.max_height = 1080;
-	cap->bt.standards = V4L2_DV_BT_STD_CEA861;
-	cap->bt.capabilities = V4L2_DV_BT_CAP_INTERLACED |
-			       V4L2_DV_BT_CAP_PROGRESSIVE;
-	return 0;
-}
-
-static const struct v4l2_subdev_core_ops hdmi_sd_core_ops = {
-	.s_power = hdmi_s_power,
-};
-
-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,
-	.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)
-{
-	struct v4l2_subdev *sd = dev_get_drvdata(dev);
-	struct hdmi_device *hdev = sd_to_hdmi_dev(sd);
-
-	dev_dbg(dev, "%s\n", __func__);
-	v4l2_subdev_call(hdev->mhl_sd, core, s_power, 0);
-	hdmi_resource_poweroff(&hdev->res);
-	/* flag that device context is lost */
-	hdev->cur_conf_dirty = 1;
-	return 0;
-}
-
-static int hdmi_runtime_resume(struct device *dev)
-{
-	struct v4l2_subdev *sd = dev_get_drvdata(dev);
-	struct hdmi_device *hdev = sd_to_hdmi_dev(sd);
-	int ret;
-
-	dev_dbg(dev, "%s\n", __func__);
-
-	ret = hdmi_resource_poweron(&hdev->res);
-	if (ret < 0)
-		return ret;
-
-	/* starting MHL */
-	ret = v4l2_subdev_call(hdev->mhl_sd, core, s_power, 1);
-	if (hdev->mhl_sd && ret)
-		goto fail;
-
-	dev_dbg(dev, "poweron succeed\n");
-
-	return 0;
-
-fail:
-	hdmi_resource_poweroff(&hdev->res);
-	dev_err(dev, "poweron failed\n");
-
-	return ret;
-}
-
-static const struct dev_pm_ops hdmi_pm_ops = {
-	.runtime_suspend = hdmi_runtime_suspend,
-	.runtime_resume	 = hdmi_runtime_resume,
-};
-
-static void hdmi_resource_clear_clocks(struct hdmi_resources *res)
-{
-	res->hdmi	 = ERR_PTR(-EINVAL);
-	res->sclk_hdmi	 = ERR_PTR(-EINVAL);
-	res->sclk_pixel	 = ERR_PTR(-EINVAL);
-	res->sclk_hdmiphy = ERR_PTR(-EINVAL);
-	res->hdmiphy	 = ERR_PTR(-EINVAL);
-}
-
-static void hdmi_resources_cleanup(struct hdmi_device *hdev)
-{
-	struct hdmi_resources *res = &hdev->res;
-
-	dev_dbg(hdev->dev, "HDMI resource cleanup\n");
-	/* put clocks, power */
-	if (res->regul_count)
-		regulator_bulk_free(res->regul_count, res->regul_bulk);
-	/* kfree is NULL-safe */
-	kfree(res->regul_bulk);
-	if (!IS_ERR(res->hdmiphy))
-		clk_put(res->hdmiphy);
-	if (!IS_ERR(res->sclk_hdmiphy))
-		clk_put(res->sclk_hdmiphy);
-	if (!IS_ERR(res->sclk_pixel))
-		clk_put(res->sclk_pixel);
-	if (!IS_ERR(res->sclk_hdmi))
-		clk_put(res->sclk_hdmi);
-	if (!IS_ERR(res->hdmi))
-		clk_put(res->hdmi);
-	memset(res, 0, sizeof(*res));
-	hdmi_resource_clear_clocks(res);
-}
-
-static int hdmi_resources_init(struct hdmi_device *hdev)
-{
-	struct device *dev = hdev->dev;
-	struct hdmi_resources *res = &hdev->res;
-	static char *supply[] = {
-		"hdmi-en",
-		"vdd",
-		"vdd_osc",
-		"vdd_pll",
-	};
-	int i, ret;
-
-	dev_dbg(dev, "HDMI resource init\n");
-
-	memset(res, 0, sizeof(*res));
-	hdmi_resource_clear_clocks(res);
-
-	/* get clocks, power */
-	res->hdmi = clk_get(dev, "hdmi");
-	if (IS_ERR(res->hdmi)) {
-		dev_err(dev, "failed to get clock 'hdmi'\n");
-		goto fail;
-	}
-	res->sclk_hdmi = clk_get(dev, "sclk_hdmi");
-	if (IS_ERR(res->sclk_hdmi)) {
-		dev_err(dev, "failed to get clock 'sclk_hdmi'\n");
-		goto fail;
-	}
-	res->sclk_pixel = clk_get(dev, "sclk_pixel");
-	if (IS_ERR(res->sclk_pixel)) {
-		dev_err(dev, "failed to get clock 'sclk_pixel'\n");
-		goto fail;
-	}
-	res->sclk_hdmiphy = clk_get(dev, "sclk_hdmiphy");
-	if (IS_ERR(res->sclk_hdmiphy)) {
-		dev_err(dev, "failed to get clock 'sclk_hdmiphy'\n");
-		goto fail;
-	}
-	res->hdmiphy = clk_get(dev, "hdmiphy");
-	if (IS_ERR(res->hdmiphy)) {
-		dev_err(dev, "failed to get clock 'hdmiphy'\n");
-		goto fail;
-	}
-	res->regul_bulk = kcalloc(ARRAY_SIZE(supply),
-				  sizeof(res->regul_bulk[0]), GFP_KERNEL);
-	if (!res->regul_bulk) {
-		dev_err(dev, "failed to get memory for regulators\n");
-		goto fail;
-	}
-	for (i = 0; i < ARRAY_SIZE(supply); ++i) {
-		res->regul_bulk[i].supply = supply[i];
-		res->regul_bulk[i].consumer = NULL;
-	}
-
-	ret = regulator_bulk_get(dev, ARRAY_SIZE(supply), res->regul_bulk);
-	if (ret) {
-		dev_err(dev, "failed to get regulators\n");
-		goto fail;
-	}
-	res->regul_count = ARRAY_SIZE(supply);
-
-	return 0;
-fail:
-	dev_err(dev, "HDMI resource init - failed\n");
-	hdmi_resources_cleanup(hdev);
-	return -ENODEV;
-}
-
-static int hdmi_probe(struct platform_device *pdev)
-{
-	struct device *dev = &pdev->dev;
-	struct resource *res;
-	struct i2c_adapter *adapter;
-	struct v4l2_subdev *sd;
-	struct hdmi_device *hdmi_dev = NULL;
-	struct s5p_hdmi_platform_data *pdata = dev->platform_data;
-	int ret;
-
-	dev_dbg(dev, "probe start\n");
-
-	if (!pdata) {
-		dev_err(dev, "platform data is missing\n");
-		ret = -ENODEV;
-		goto fail;
-	}
-
-	hdmi_dev = devm_kzalloc(&pdev->dev, sizeof(*hdmi_dev), GFP_KERNEL);
-	if (!hdmi_dev) {
-		dev_err(dev, "out of memory\n");
-		ret = -ENOMEM;
-		goto fail;
-	}
-
-	hdmi_dev->dev = dev;
-
-	ret = hdmi_resources_init(hdmi_dev);
-	if (ret)
-		goto fail;
-
-	/* mapping HDMI registers */
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (res == NULL) {
-		dev_err(dev, "get memory resource failed.\n");
-		ret = -ENXIO;
-		goto fail_init;
-	}
-
-	hdmi_dev->regs = devm_ioremap(&pdev->dev, res->start,
-				      resource_size(res));
-	if (hdmi_dev->regs == NULL) {
-		dev_err(dev, "register mapping failed.\n");
-		ret = -ENXIO;
-		goto fail_init;
-	}
-
-	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
-	if (res == NULL) {
-		dev_err(dev, "get interrupt resource failed.\n");
-		ret = -ENXIO;
-		goto fail_init;
-	}
-
-	ret = devm_request_irq(&pdev->dev, res->start, hdmi_irq_handler, 0,
-			       "hdmi", hdmi_dev);
-	if (ret) {
-		dev_err(dev, "request interrupt failed.\n");
-		goto fail_init;
-	}
-	hdmi_dev->irq = res->start;
-
-	/* setting v4l2 name to prevent WARN_ON in v4l2_device_register */
-	strlcpy(hdmi_dev->v4l2_dev.name, dev_name(dev),
-		sizeof(hdmi_dev->v4l2_dev.name));
-	/* passing NULL owner prevents driver from erasing drvdata */
-	ret = v4l2_device_register(NULL, &hdmi_dev->v4l2_dev);
-	if (ret) {
-		dev_err(dev, "could not register v4l2 device.\n");
-		goto fail_init;
-	}
-
-	/* testing if hdmiphy info is present */
-	if (!pdata->hdmiphy_info) {
-		dev_err(dev, "hdmiphy info is missing in platform data\n");
-		ret = -ENXIO;
-		goto fail_vdev;
-	}
-
-	adapter = i2c_get_adapter(pdata->hdmiphy_bus);
-	if (adapter == NULL) {
-		dev_err(dev, "hdmiphy adapter request failed\n");
-		ret = -ENXIO;
-		goto fail_vdev;
-	}
-
-	hdmi_dev->phy_sd = v4l2_i2c_new_subdev_board(&hdmi_dev->v4l2_dev,
-		adapter, pdata->hdmiphy_info, NULL);
-	/* on failure or not adapter is no longer useful */
-	i2c_put_adapter(adapter);
-	if (hdmi_dev->phy_sd == NULL) {
-		dev_err(dev, "missing subdev for hdmiphy\n");
-		ret = -ENODEV;
-		goto fail_vdev;
-	}
-
-	/* initialization of MHL interface if present */
-	if (pdata->mhl_info) {
-		adapter = i2c_get_adapter(pdata->mhl_bus);
-		if (adapter == NULL) {
-			dev_err(dev, "MHL adapter request failed\n");
-			ret = -ENXIO;
-			goto fail_vdev;
-		}
-
-		hdmi_dev->mhl_sd = v4l2_i2c_new_subdev_board(
-			&hdmi_dev->v4l2_dev, adapter,
-			pdata->mhl_info, NULL);
-		/* on failure or not adapter is no longer useful */
-		i2c_put_adapter(adapter);
-		if (hdmi_dev->mhl_sd == NULL) {
-			dev_err(dev, "missing subdev for MHL\n");
-			ret = -ENODEV;
-			goto fail_vdev;
-		}
-	}
-
-	clk_enable(hdmi_dev->res.hdmi);
-
-	pm_runtime_enable(dev);
-
-	sd = &hdmi_dev->sd;
-	v4l2_subdev_init(sd, &hdmi_sd_ops);
-	sd->owner = THIS_MODULE;
-
-	strlcpy(sd->name, "s5p-hdmi", sizeof(sd->name));
-	hdmi_dev->cur_timings =
-		hdmi_timings[HDMI_DEFAULT_TIMINGS_IDX].dv_timings;
-	/* FIXME: missing fail timings is not supported */
-	hdmi_dev->cur_conf =
-		hdmi_timings[HDMI_DEFAULT_TIMINGS_IDX].hdmi_timings;
-	hdmi_dev->cur_conf_dirty = 1;
-
-	/* storing subdev for call that have only access to struct device */
-	dev_set_drvdata(dev, sd);
-
-	dev_info(dev, "probe successful\n");
-
-	return 0;
-
-fail_vdev:
-	v4l2_device_unregister(&hdmi_dev->v4l2_dev);
-
-fail_init:
-	hdmi_resources_cleanup(hdmi_dev);
-
-fail:
-	dev_err(dev, "probe failed\n");
-	return ret;
-}
-
-static int hdmi_remove(struct platform_device *pdev)
-{
-	struct device *dev = &pdev->dev;
-	struct v4l2_subdev *sd = dev_get_drvdata(dev);
-	struct hdmi_device *hdmi_dev = sd_to_hdmi_dev(sd);
-
-	pm_runtime_disable(dev);
-	clk_disable(hdmi_dev->res.hdmi);
-	v4l2_device_unregister(&hdmi_dev->v4l2_dev);
-	disable_irq(hdmi_dev->irq);
-	hdmi_resources_cleanup(hdmi_dev);
-	dev_info(dev, "remove successful\n");
-
-	return 0;
-}
-
-static struct platform_driver hdmi_driver __refdata = {
-	.probe = hdmi_probe,
-	.remove = hdmi_remove,
-	.id_table = hdmi_driver_types,
-	.driver = {
-		.name = "s5p-hdmi",
-		.pm = &hdmi_pm_ops,
-	}
-};
-
-module_platform_driver(hdmi_driver);
diff --git a/drivers/media/platform/s5p-tv/hdmiphy_drv.c b/drivers/media/platform/s5p-tv/hdmiphy_drv.c
deleted file mode 100644
index aae652351aa8..000000000000
--- a/drivers/media/platform/s5p-tv/hdmiphy_drv.c
+++ /dev/null
@@ -1,324 +0,0 @@
-/*
- * Samsung HDMI Physical interface driver
- *
- * Copyright (C) 2010-2011 Samsung Electronics Co.Ltd
- * Author: Tomasz Stanislawski <t.stanislaws@samsung.com>
- *
- * This program is free software; you can redistribute  it and/or modify it
- * under  the terms of  the GNU General  Public License as published by the
- * Free Software Foundation;  either version 2 of the  License, or (at your
- * option) any later version.
- */
-
-#include <linux/module.h>
-#include <linux/i2c.h>
-#include <linux/slab.h>
-#include <linux/clk.h>
-#include <linux/io.h>
-#include <linux/interrupt.h>
-#include <linux/irq.h>
-#include <linux/err.h>
-
-#include <media/v4l2-subdev.h>
-
-MODULE_AUTHOR("Tomasz Stanislawski <t.stanislaws@samsung.com>");
-MODULE_DESCRIPTION("Samsung HDMI Physical interface driver");
-MODULE_LICENSE("GPL");
-
-struct hdmiphy_conf {
-	unsigned long pixclk;
-	const u8 *data;
-};
-
-struct hdmiphy_ctx {
-	struct v4l2_subdev sd;
-	const struct hdmiphy_conf *conf_tab;
-};
-
-static const struct hdmiphy_conf hdmiphy_conf_s5pv210[] = {
-	{ .pixclk = 27000000, .data = (u8 [32]) {
-		0x01, 0x05, 0x00, 0xD8, 0x10, 0x1C, 0x30, 0x40,
-		0x6B, 0x10, 0x02, 0x52, 0xDF, 0xF2, 0x54, 0x87,
-		0x84, 0x00, 0x30, 0x38, 0x00, 0x08, 0x10, 0xE0,
-		0x22, 0x40, 0xE3, 0x26, 0x00, 0x00, 0x00, 0x00, }
-	},
-	{ .pixclk = 27027000, .data = (u8 [32]) {
-		0x01, 0x05, 0x00, 0xD4, 0x10, 0x9C, 0x09, 0x64,
-		0x6B, 0x10, 0x02, 0x52, 0xDF, 0xF2, 0x54, 0x87,
-		0x84, 0x00, 0x30, 0x38, 0x00, 0x08, 0x10, 0xE0,
-		0x22, 0x40, 0xE2, 0x26, 0x00, 0x00, 0x00, 0x00, }
-	},
-	{ .pixclk = 74176000, .data = (u8 [32]) {
-		0x01, 0x05, 0x00, 0xD8, 0x10, 0x9C, 0xEF, 0x5B,
-		0x6D, 0x10, 0x01, 0x52, 0xEF, 0xF3, 0x54, 0xB9,
-		0x84, 0x00, 0x30, 0x38, 0x00, 0x08, 0x10, 0xE0,
-		0x22, 0x40, 0xA5, 0x26, 0x01, 0x00, 0x00, 0x00, }
-	},
-	{ .pixclk = 74250000, .data = (u8 [32]) {
-		0x01, 0x05, 0x00, 0xD8, 0x10, 0x9C, 0xF8, 0x40,
-		0x6A, 0x10, 0x01, 0x52, 0xFF, 0xF1, 0x54, 0xBA,
-		0x84, 0x00, 0x10, 0x38, 0x00, 0x08, 0x10, 0xE0,
-		0x22, 0x40, 0xA4, 0x26, 0x01, 0x00, 0x00, 0x00, }
-	},
-	{ /* end marker */ }
-};
-
-static const struct hdmiphy_conf hdmiphy_conf_exynos4210[] = {
-	{ .pixclk = 27000000, .data = (u8 [32]) {
-		0x01, 0x05, 0x00, 0xD8, 0x10, 0x1C, 0x30, 0x40,
-		0x6B, 0x10, 0x02, 0x51, 0xDF, 0xF2, 0x54, 0x87,
-		0x84, 0x00, 0x30, 0x38, 0x00, 0x08, 0x10, 0xE0,
-		0x22, 0x40, 0xE3, 0x26, 0x00, 0x00, 0x00, 0x00, }
-	},
-	{ .pixclk = 27027000, .data = (u8 [32]) {
-		0x01, 0x05, 0x00, 0xD4, 0x10, 0x9C, 0x09, 0x64,
-		0x6B, 0x10, 0x02, 0x51, 0xDF, 0xF2, 0x54, 0x87,
-		0x84, 0x00, 0x30, 0x38, 0x00, 0x08, 0x10, 0xE0,
-		0x22, 0x40, 0xE2, 0x26, 0x00, 0x00, 0x00, 0x00, }
-	},
-	{ .pixclk = 74176000, .data = (u8 [32]) {
-		0x01, 0x05, 0x00, 0xD8, 0x10, 0x9C, 0xEF, 0x5B,
-		0x6D, 0x10, 0x01, 0x51, 0xEF, 0xF3, 0x54, 0xB9,
-		0x84, 0x00, 0x30, 0x38, 0x00, 0x08, 0x10, 0xE0,
-		0x22, 0x40, 0xA5, 0x26, 0x01, 0x00, 0x00, 0x00, }
-	},
-	{ .pixclk = 74250000, .data = (u8 [32]) {
-		0x01, 0x05, 0x00, 0xD8, 0x10, 0x9C, 0xF8, 0x40,
-		0x6A, 0x10, 0x01, 0x51, 0xFF, 0xF1, 0x54, 0xBA,
-		0x84, 0x00, 0x10, 0x38, 0x00, 0x08, 0x10, 0xE0,
-		0x22, 0x40, 0xA4, 0x26, 0x01, 0x00, 0x00, 0x00, }
-	},
-	{ .pixclk = 148352000, .data = (u8 [32]) {
-		0x01, 0x05, 0x00, 0xD8, 0x10, 0x9C, 0xEF, 0x5B,
-		0x6D, 0x18, 0x00, 0x51, 0xEF, 0xF3, 0x54, 0xB9,
-		0x84, 0x00, 0x30, 0x38, 0x00, 0x08, 0x10, 0xE0,
-		0x11, 0x40, 0xA5, 0x26, 0x02, 0x00, 0x00, 0x00, }
-	},
-	{ .pixclk = 148500000, .data = (u8 [32]) {
-		0x01, 0x05, 0x00, 0xD8, 0x10, 0x9C, 0xF8, 0x40,
-		0x6A, 0x18, 0x00, 0x51, 0xFF, 0xF1, 0x54, 0xBA,
-		0x84, 0x00, 0x10, 0x38, 0x00, 0x08, 0x10, 0xE0,
-		0x11, 0x40, 0xA4, 0x26, 0x02, 0x00, 0x00, 0x00, }
-	},
-	{ /* end marker */ }
-};
-
-static const struct hdmiphy_conf hdmiphy_conf_exynos4212[] = {
-	{ .pixclk = 27000000, .data = (u8 [32]) {
-		0x01, 0x11, 0x2D, 0x75, 0x00, 0x01, 0x00, 0x08,
-		0x82, 0x00, 0x0E, 0xD9, 0x45, 0xA0, 0x34, 0xC0,
-		0x0B, 0x80, 0x12, 0x87, 0x08, 0x24, 0x24, 0x71,
-		0x54, 0xE3, 0x24, 0x00, 0x00, 0x00, 0x01, 0x00, }
-	},
-	{ .pixclk = 27027000, .data = (u8 [32]) {
-		0x01, 0x91, 0x2D, 0x72, 0x00, 0x64, 0x12, 0x08,
-		0x43, 0x20, 0x0E, 0xD9, 0x45, 0xA0, 0x34, 0xC0,
-		0x0B, 0x80, 0x12, 0x87, 0x08, 0x24, 0x24, 0x71,
-		0x54, 0xE2, 0x24, 0x00, 0x00, 0x00, 0x01, 0x00, }
-	},
-	{ .pixclk = 74176000, .data = (u8 [32]) {
-		0x01, 0x91, 0x3E, 0x35, 0x00, 0x5B, 0xDE, 0x08,
-		0x82, 0x20, 0x73, 0xD9, 0x45, 0xA0, 0x34, 0xC0,
-		0x0B, 0x80, 0x12, 0x87, 0x08, 0x24, 0x24, 0x52,
-		0x54, 0xA5, 0x24, 0x01, 0x00, 0x00, 0x01, 0x00, }
-	},
-	{ .pixclk = 74250000, .data = (u8 [32]) {
-		0x01, 0x91, 0x3E, 0x35, 0x00, 0x40, 0xF0, 0x08,
-		0x82, 0x20, 0x73, 0xD9, 0x45, 0xA0, 0x34, 0xC0,
-		0x0B, 0x80, 0x12, 0x87, 0x08, 0x24, 0x24, 0x52,
-		0x54, 0xA4, 0x24, 0x01, 0x00, 0x00, 0x01, 0x00, }
-	},
-	{ .pixclk = 148500000, .data = (u8 [32]) {
-		0x01, 0x91, 0x3E, 0x15, 0x00, 0x40, 0xF0, 0x08,
-		0x82, 0x20, 0x73, 0xD9, 0x45, 0xA0, 0x34, 0xC0,
-		0x0B, 0x80, 0x12, 0x87, 0x08, 0x24, 0x24, 0xA4,
-		0x54, 0x4A, 0x25, 0x03, 0x00, 0x00, 0x01, 0x00, }
-	},
-	{ /* end marker */ }
-};
-
-static const struct hdmiphy_conf hdmiphy_conf_exynos4412[] = {
-	{ .pixclk = 27000000, .data = (u8 [32]) {
-		0x01, 0x11, 0x2D, 0x75, 0x40, 0x01, 0x00, 0x08,
-		0x82, 0x00, 0x0E, 0xD9, 0x45, 0xA0, 0xAC, 0x80,
-		0x08, 0x80, 0x11, 0x84, 0x02, 0x22, 0x44, 0x86,
-		0x54, 0xE4, 0x24, 0x00, 0x00, 0x00, 0x01, 0x00, }
-	},
-	{ .pixclk = 27027000, .data = (u8 [32]) {
-		0x01, 0x91, 0x2D, 0x72, 0x40, 0x64, 0x12, 0x08,
-		0x43, 0x20, 0x0E, 0xD9, 0x45, 0xA0, 0xAC, 0x80,
-		0x08, 0x80, 0x11, 0x84, 0x02, 0x22, 0x44, 0x86,
-		0x54, 0xE3, 0x24, 0x00, 0x00, 0x00, 0x01, 0x00, }
-	},
-	{ .pixclk = 74176000, .data = (u8 [32]) {
-		0x01, 0x91, 0x1F, 0x10, 0x40, 0x5B, 0xEF, 0x08,
-		0x81, 0x20, 0xB9, 0xD8, 0x45, 0xA0, 0xAC, 0x80,
-		0x08, 0x80, 0x11, 0x84, 0x02, 0x22, 0x44, 0x86,
-		0x54, 0xA6, 0x24, 0x01, 0x00, 0x00, 0x01, 0x00, }
-	},
-	{ .pixclk = 74250000, .data = (u8 [32]) {
-		0x01, 0x91, 0x1F, 0x10, 0x40, 0x40, 0xF8, 0x08,
-		0x81, 0x20, 0xBA, 0xD8, 0x45, 0xA0, 0xAC, 0x80,
-		0x08, 0x80, 0x11, 0x84, 0x02, 0x22, 0x44, 0x86,
-		0x54, 0xA5, 0x24, 0x01, 0x00, 0x00, 0x01, 0x00, }
-	},
-	{ .pixclk = 148500000, .data = (u8 [32]) {
-		0x01, 0x91, 0x1F, 0x00, 0x40, 0x40, 0xF8, 0x08,
-		0x81, 0x20, 0xBA, 0xD8, 0x45, 0xA0, 0xAC, 0x80,
-		0x08, 0x80, 0x11, 0x84, 0x02, 0x22, 0x44, 0x86,
-		0x54, 0x4B, 0x25, 0x03, 0x00, 0x00, 0x01, 0x00, }
-	},
-	{ /* end marker */ }
-};
-
-static inline struct hdmiphy_ctx *sd_to_ctx(struct v4l2_subdev *sd)
-{
-	return container_of(sd, struct hdmiphy_ctx, sd);
-}
-
-static const u8 *hdmiphy_find_conf(unsigned long pixclk,
-		const struct hdmiphy_conf *conf)
-{
-	for (; conf->pixclk; ++conf)
-		if (conf->pixclk == pixclk)
-			return conf->data;
-	return NULL;
-}
-
-static int hdmiphy_s_power(struct v4l2_subdev *sd, int on)
-{
-	/* to be implemented */
-	return 0;
-}
-
-static int hdmiphy_s_dv_timings(struct v4l2_subdev *sd,
-	struct v4l2_dv_timings *timings)
-{
-	const u8 *data;
-	u8 buffer[32];
-	int ret;
-	struct hdmiphy_ctx *ctx = sd_to_ctx(sd);
-	struct i2c_client *client = v4l2_get_subdevdata(sd);
-	struct device *dev = &client->dev;
-	unsigned long pixclk = timings->bt.pixelclock;
-
-	dev_info(dev, "s_dv_timings\n");
-	if ((timings->bt.flags & V4L2_DV_FL_REDUCED_FPS) && pixclk == 74250000)
-		pixclk = 74176000;
-	data = hdmiphy_find_conf(pixclk, ctx->conf_tab);
-	if (!data) {
-		dev_err(dev, "format not supported\n");
-		return -EINVAL;
-	}
-
-	/* storing configuration to the device */
-	memcpy(buffer, data, 32);
-	ret = i2c_master_send(client, buffer, 32);
-	if (ret != 32) {
-		dev_err(dev, "failed to configure HDMIPHY via I2C\n");
-		return -EIO;
-	}
-
-	return 0;
-}
-
-static int hdmiphy_dv_timings_cap(struct v4l2_subdev *sd,
-	struct v4l2_dv_timings_cap *cap)
-{
-	if (cap->pad != 0)
-		return -EINVAL;
-
-	cap->type = V4L2_DV_BT_656_1120;
-	/* The phy only determines the pixelclock, leave the other values
-	 * at 0 to signify that we have no information for them. */
-	cap->bt.min_pixelclock = 27000000;
-	cap->bt.max_pixelclock = 148500000;
-	return 0;
-}
-
-static int hdmiphy_s_stream(struct v4l2_subdev *sd, int enable)
-{
-	struct i2c_client *client = v4l2_get_subdevdata(sd);
-	struct device *dev = &client->dev;
-	u8 buffer[2];
-	int ret;
-
-	dev_info(dev, "s_stream(%d)\n", enable);
-	/* going to/from configuration from/to operation mode */
-	buffer[0] = 0x1f;
-	buffer[1] = enable ? 0x80 : 0x00;
-
-	ret = i2c_master_send(client, buffer, 2);
-	if (ret != 2) {
-		dev_err(dev, "stream (%d) failed\n", enable);
-		return -EIO;
-	}
-	return 0;
-}
-
-static const struct v4l2_subdev_core_ops hdmiphy_core_ops = {
-	.s_power =  hdmiphy_s_power,
-};
-
-static const struct v4l2_subdev_video_ops hdmiphy_video_ops = {
-	.s_dv_timings = hdmiphy_s_dv_timings,
-	.s_stream =  hdmiphy_s_stream,
-};
-
-static const struct v4l2_subdev_pad_ops hdmiphy_pad_ops = {
-	.dv_timings_cap = hdmiphy_dv_timings_cap,
-};
-
-static const struct v4l2_subdev_ops hdmiphy_ops = {
-	.core = &hdmiphy_core_ops,
-	.video = &hdmiphy_video_ops,
-	.pad = &hdmiphy_pad_ops,
-};
-
-static int hdmiphy_probe(struct i2c_client *client,
-			 const struct i2c_device_id *id)
-{
-	struct hdmiphy_ctx *ctx;
-
-	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
-	if (!ctx)
-		return -ENOMEM;
-
-	ctx->conf_tab = (struct hdmiphy_conf *)id->driver_data;
-	v4l2_i2c_subdev_init(&ctx->sd, client, &hdmiphy_ops);
-
-	dev_info(&client->dev, "probe successful\n");
-	return 0;
-}
-
-static int hdmiphy_remove(struct i2c_client *client)
-{
-	struct v4l2_subdev *sd = i2c_get_clientdata(client);
-	struct hdmiphy_ctx *ctx = sd_to_ctx(sd);
-
-	kfree(ctx);
-	dev_info(&client->dev, "remove successful\n");
-
-	return 0;
-}
-
-static const struct i2c_device_id hdmiphy_id[] = {
-	{ "hdmiphy", (unsigned long)hdmiphy_conf_exynos4210 },
-	{ "hdmiphy-s5pv210", (unsigned long)hdmiphy_conf_s5pv210 },
-	{ "hdmiphy-exynos4210", (unsigned long)hdmiphy_conf_exynos4210 },
-	{ "hdmiphy-exynos4212", (unsigned long)hdmiphy_conf_exynos4212 },
-	{ "hdmiphy-exynos4412", (unsigned long)hdmiphy_conf_exynos4412 },
-	{ },
-};
-MODULE_DEVICE_TABLE(i2c, hdmiphy_id);
-
-static struct i2c_driver hdmiphy_driver = {
-	.driver = {
-		.name	= "s5p-hdmiphy",
-	},
-	.probe		= hdmiphy_probe,
-	.remove		= hdmiphy_remove,
-	.id_table = hdmiphy_id,
-};
-
-module_i2c_driver(hdmiphy_driver);
diff --git a/drivers/media/platform/s5p-tv/mixer.h b/drivers/media/platform/s5p-tv/mixer.h
deleted file mode 100644
index 869f0ce86f6e..000000000000
--- a/drivers/media/platform/s5p-tv/mixer.h
+++ /dev/null
@@ -1,364 +0,0 @@
-/*
- * Samsung TV Mixer driver
- *
- * Copyright (c) 2010-2011 Samsung Electronics Co., Ltd.
- *
- * Tomasz Stanislawski, <t.stanislaws@samsung.com>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published
- * by the Free Software Foundiation. either version 2 of the License,
- * or (at your option) any later version
- */
-
-#ifndef SAMSUNG_MIXER_H
-#define SAMSUNG_MIXER_H
-
-#ifdef CONFIG_VIDEO_SAMSUNG_S5P_MIXER_DEBUG
-	#define DEBUG
-#endif
-
-#include <linux/fb.h>
-#include <linux/irqreturn.h>
-#include <linux/kernel.h>
-#include <linux/spinlock.h>
-#include <linux/wait.h>
-#include <media/v4l2-device.h>
-#include <media/videobuf2-v4l2.h>
-
-#include "regs-mixer.h"
-
-/** maximum number of output interfaces */
-#define MXR_MAX_OUTPUTS 2
-/** maximum number of input interfaces (layers) */
-#define MXR_MAX_LAYERS 3
-#define MXR_DRIVER_NAME "s5p-mixer"
-/** maximal number of planes for every layer */
-#define MXR_MAX_PLANES	2
-
-#define MXR_ENABLE 1
-#define MXR_DISABLE 0
-
-/** description of a macroblock for packed formats */
-struct mxr_block {
-	/** vertical number of pixels in macroblock */
-	unsigned int width;
-	/** horizontal number of pixels in macroblock */
-	unsigned int height;
-	/** size of block in bytes */
-	unsigned int size;
-};
-
-/** description of supported format */
-struct mxr_format {
-	/** format name/mnemonic */
-	const char *name;
-	/** fourcc identifier */
-	u32 fourcc;
-	/** colorspace identifier */
-	enum v4l2_colorspace colorspace;
-	/** number of planes in image data */
-	int num_planes;
-	/** description of block for each plane */
-	struct mxr_block plane[MXR_MAX_PLANES];
-	/** number of subframes in image data */
-	int num_subframes;
-	/** specifies to which subframe belong given plane */
-	int plane2subframe[MXR_MAX_PLANES];
-	/** internal code, driver dependent */
-	unsigned long cookie;
-};
-
-/** description of crop configuration for image */
-struct mxr_crop {
-	/** width of layer in pixels */
-	unsigned int full_width;
-	/** height of layer in pixels */
-	unsigned int full_height;
-	/** horizontal offset of first pixel to be displayed */
-	unsigned int x_offset;
-	/** vertical offset of first pixel to be displayed */
-	unsigned int y_offset;
-	/** width of displayed data in pixels */
-	unsigned int width;
-	/** height of displayed data in pixels */
-	unsigned int height;
-	/** indicate which fields are present in buffer */
-	unsigned int field;
-};
-
-/** stages of geometry operations */
-enum mxr_geometry_stage {
-	MXR_GEOMETRY_SINK,
-	MXR_GEOMETRY_COMPOSE,
-	MXR_GEOMETRY_CROP,
-	MXR_GEOMETRY_SOURCE,
-};
-
-/* flag indicating that offset should be 0 */
-#define MXR_NO_OFFSET	0x80000000
-
-/** description of transformation from source to destination image */
-struct mxr_geometry {
-	/** cropping for source image */
-	struct mxr_crop src;
-	/** cropping for destination image */
-	struct mxr_crop dst;
-	/** layer-dependant description of horizontal scaling */
-	unsigned int x_ratio;
-	/** layer-dependant description of vertical scaling */
-	unsigned int y_ratio;
-};
-
-/** instance of a buffer */
-struct mxr_buffer {
-	/** common v4l buffer stuff -- must be first */
-	struct vb2_v4l2_buffer vb;
-	/** node for layer's lists */
-	struct list_head	list;
-};
-
-
-/** internal states of layer */
-enum mxr_layer_state {
-	/** layers is not shown */
-	MXR_LAYER_IDLE = 0,
-	/** layer is shown */
-	MXR_LAYER_STREAMING,
-	/** state before STREAMOFF is finished */
-	MXR_LAYER_STREAMING_FINISH,
-};
-
-/** forward declarations */
-struct mxr_device;
-struct mxr_layer;
-
-/** callback for layers operation */
-struct mxr_layer_ops {
-	/* TODO: try to port it to subdev API */
-	/** handler for resource release function */
-	void (*release)(struct mxr_layer *);
-	/** setting buffer to HW */
-	void (*buffer_set)(struct mxr_layer *, struct mxr_buffer *);
-	/** setting format and geometry in HW */
-	void (*format_set)(struct mxr_layer *);
-	/** streaming stop/start */
-	void (*stream_set)(struct mxr_layer *, int);
-	/** adjusting geometry */
-	void (*fix_geometry)(struct mxr_layer *,
-		enum mxr_geometry_stage, unsigned long);
-};
-
-/** layer instance, a single window and content displayed on output */
-struct mxr_layer {
-	/** parent mixer device */
-	struct mxr_device *mdev;
-	/** layer index (unique identifier) */
-	int idx;
-	/** callbacks for layer methods */
-	struct mxr_layer_ops ops;
-	/** format array */
-	const struct mxr_format **fmt_array;
-	/** size of format array */
-	unsigned long fmt_array_size;
-
-	/** lock for protection of list and state fields */
-	spinlock_t enq_slock;
-	/** list for enqueued buffers */
-	struct list_head enq_list;
-	/** buffer currently owned by hardware in temporary registers */
-	struct mxr_buffer *update_buf;
-	/** buffer currently owned by hardware in shadow registers */
-	struct mxr_buffer *shadow_buf;
-	/** state of layer IDLE/STREAMING */
-	enum mxr_layer_state state;
-
-	/** mutex for protection of fields below */
-	struct mutex mutex;
-	/** handler for video node */
-	struct video_device vfd;
-	/** queue for output buffers */
-	struct vb2_queue vb_queue;
-	/** current image format */
-	const struct mxr_format *fmt;
-	/** current geometry of image */
-	struct mxr_geometry geo;
-};
-
-/** description of mixers output interface */
-struct mxr_output {
-	/** name of output */
-	char name[32];
-	/** output subdev */
-	struct v4l2_subdev *sd;
-	/** cookie used for configuration of registers */
-	int cookie;
-};
-
-/** specify source of output subdevs */
-struct mxr_output_conf {
-	/** name of output (connector) */
-	char *output_name;
-	/** name of module that generates output subdev */
-	char *module_name;
-	/** cookie need for mixer HW */
-	int cookie;
-};
-
-struct clk;
-struct regulator;
-
-/** auxiliary resources used my mixer */
-struct mxr_resources {
-	/** interrupt index */
-	int irq;
-	/** pointer to Mixer registers */
-	void __iomem *mxr_regs;
-	/** pointer to Video Processor registers */
-	void __iomem *vp_regs;
-	/** other resources, should used under mxr_device.mutex */
-	struct clk *mixer;
-	struct clk *vp;
-	struct clk *sclk_mixer;
-	struct clk *sclk_hdmi;
-	struct clk *sclk_dac;
-};
-
-/* event flags used  */
-enum mxr_devide_flags {
-	MXR_EVENT_VSYNC = 0,
-	MXR_EVENT_TOP = 1,
-};
-
-/** drivers instance */
-struct mxr_device {
-	/** master device */
-	struct device *dev;
-	/** state of each layer */
-	struct mxr_layer *layer[MXR_MAX_LAYERS];
-	/** state of each output */
-	struct mxr_output *output[MXR_MAX_OUTPUTS];
-	/** number of registered outputs */
-	int output_cnt;
-
-	/* video resources */
-
-	/** V4L2 device */
-	struct v4l2_device v4l2_dev;
-	/** event wait queue */
-	wait_queue_head_t event_queue;
-	/** state flags */
-	unsigned long event_flags;
-
-	/** spinlock for protection of registers */
-	spinlock_t reg_slock;
-
-	/** mutex for protection of fields below */
-	struct mutex mutex;
-	/** number of entities depndant on output configuration */
-	int n_output;
-	/** number of users that do streaming */
-	int n_streamer;
-	/** index of current output */
-	int current_output;
-	/** auxiliary resources used my mixer */
-	struct mxr_resources res;
-};
-
-/** transform device structure into mixer device */
-static inline struct mxr_device *to_mdev(struct device *dev)
-{
-	struct v4l2_device *vdev = dev_get_drvdata(dev);
-	return container_of(vdev, struct mxr_device, v4l2_dev);
-}
-
-/** get current output data, should be called under mdev's mutex */
-static inline struct mxr_output *to_output(struct mxr_device *mdev)
-{
-	return mdev->output[mdev->current_output];
-}
-
-/** get current output subdev, should be called under mdev's mutex */
-static inline struct v4l2_subdev *to_outsd(struct mxr_device *mdev)
-{
-	struct mxr_output *out = to_output(mdev);
-	return out ? out->sd : NULL;
-}
-
-/** forward declaration for mixer platform data */
-struct mxr_platform_data;
-
-/** acquiring common video resources */
-int mxr_acquire_video(struct mxr_device *mdev,
-	struct mxr_output_conf *output_cont, int output_count);
-
-/** releasing common video resources */
-void mxr_release_video(struct mxr_device *mdev);
-
-struct mxr_layer *mxr_graph_layer_create(struct mxr_device *mdev, int idx);
-struct mxr_layer *mxr_vp_layer_create(struct mxr_device *mdev, int idx);
-struct mxr_layer *mxr_base_layer_create(struct mxr_device *mdev,
-	int idx, char *name, const struct mxr_layer_ops *ops);
-
-void mxr_base_layer_release(struct mxr_layer *layer);
-void mxr_layer_release(struct mxr_layer *layer);
-
-int mxr_base_layer_register(struct mxr_layer *layer);
-void mxr_base_layer_unregister(struct mxr_layer *layer);
-
-unsigned long mxr_get_plane_size(const struct mxr_block *blk,
-	unsigned int width, unsigned int height);
-
-/** adds new consumer for mixer's power */
-int __must_check mxr_power_get(struct mxr_device *mdev);
-/** removes consumer for mixer's power */
-void mxr_power_put(struct mxr_device *mdev);
-/** add new client for output configuration */
-void mxr_output_get(struct mxr_device *mdev);
-/** removes new client for output configuration */
-void mxr_output_put(struct mxr_device *mdev);
-/** add new client for streaming */
-void mxr_streamer_get(struct mxr_device *mdev);
-/** removes new client for streaming */
-void mxr_streamer_put(struct mxr_device *mdev);
-/** returns format of data delivared to current output */
-void mxr_get_mbus_fmt(struct mxr_device *mdev,
-	struct v4l2_mbus_framefmt *mbus_fmt);
-
-/* Debug */
-
-#define mxr_err(mdev, fmt, ...)  dev_err(mdev->dev, fmt, ##__VA_ARGS__)
-#define mxr_warn(mdev, fmt, ...) dev_warn(mdev->dev, fmt, ##__VA_ARGS__)
-#define mxr_info(mdev, fmt, ...) dev_info(mdev->dev, fmt, ##__VA_ARGS__)
-
-#ifdef CONFIG_VIDEO_SAMSUNG_S5P_MIXER_DEBUG
-	#define mxr_dbg(mdev, fmt, ...)  dev_dbg(mdev->dev, fmt, ##__VA_ARGS__)
-#else
-	#define mxr_dbg(mdev, fmt, ...)  do { (void) mdev; } while (0)
-#endif
-
-/* accessing Mixer's and Video Processor's registers */
-
-void mxr_vsync_set_update(struct mxr_device *mdev, int en);
-void mxr_reg_reset(struct mxr_device *mdev);
-irqreturn_t mxr_irq_handler(int irq, void *dev_data);
-void mxr_reg_s_output(struct mxr_device *mdev, int cookie);
-void mxr_reg_streamon(struct mxr_device *mdev);
-void mxr_reg_streamoff(struct mxr_device *mdev);
-int mxr_reg_wait4vsync(struct mxr_device *mdev);
-void mxr_reg_set_mbus_fmt(struct mxr_device *mdev,
-	struct v4l2_mbus_framefmt *fmt);
-void mxr_reg_graph_layer_stream(struct mxr_device *mdev, int idx, int en);
-void mxr_reg_graph_buffer(struct mxr_device *mdev, int idx, dma_addr_t addr);
-void mxr_reg_graph_format(struct mxr_device *mdev, int idx,
-	const struct mxr_format *fmt, const struct mxr_geometry *geo);
-
-void mxr_reg_vp_layer_stream(struct mxr_device *mdev, int en);
-void mxr_reg_vp_buffer(struct mxr_device *mdev,
-	dma_addr_t luma_addr[2], dma_addr_t chroma_addr[2]);
-void mxr_reg_vp_format(struct mxr_device *mdev,
-	const struct mxr_format *fmt, const struct mxr_geometry *geo);
-void mxr_reg_dump(struct mxr_device *mdev);
-
-#endif /* SAMSUNG_MIXER_H */
-
diff --git a/drivers/media/platform/s5p-tv/mixer_drv.c b/drivers/media/platform/s5p-tv/mixer_drv.c
deleted file mode 100644
index 8a5d19469ddc..000000000000
--- a/drivers/media/platform/s5p-tv/mixer_drv.c
+++ /dev/null
@@ -1,527 +0,0 @@
-/*
- * Samsung TV Mixer driver
- *
- * Copyright (c) 2010-2011 Samsung Electronics Co., Ltd.
- *
- * Tomasz Stanislawski, <t.stanislaws@samsung.com>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published
- * by the Free Software Foundiation. either version 2 of the License,
- * or (at your option) any later version
- */
-
-#include "mixer.h"
-
-#include <linux/module.h>
-#include <linux/platform_device.h>
-#include <linux/io.h>
-#include <linux/interrupt.h>
-#include <linux/irq.h>
-#include <linux/fb.h>
-#include <linux/delay.h>
-#include <linux/pm_runtime.h>
-#include <linux/clk.h>
-
-MODULE_AUTHOR("Tomasz Stanislawski, <t.stanislaws@samsung.com>");
-MODULE_DESCRIPTION("Samsung MIXER");
-MODULE_LICENSE("GPL");
-
-/* --------- DRIVER PARAMETERS ---------- */
-
-static struct mxr_output_conf mxr_output_conf[] = {
-	{
-		.output_name = "S5P HDMI connector",
-		.module_name = "s5p-hdmi",
-		.cookie = 1,
-	},
-	{
-		.output_name = "S5P SDO connector",
-		.module_name = "s5p-sdo",
-		.cookie = 0,
-	},
-};
-
-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, 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);
-}
-
-void mxr_streamer_get(struct mxr_device *mdev)
-{
-	mutex_lock(&mdev->mutex);
-	++mdev->n_streamer;
-	mxr_dbg(mdev, "%s(%d)\n", __func__, mdev->n_streamer);
-	if (mdev->n_streamer == 1) {
-		struct v4l2_subdev *sd = to_outsd(mdev);
-		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;
-
-		if (to_output(mdev)->cookie == 0)
-			clk_set_parent(res->sclk_mixer, res->sclk_dac);
-		else
-			clk_set_parent(res->sclk_mixer, res->sclk_hdmi);
-		mxr_reg_s_output(mdev, to_output(mdev)->cookie);
-
-		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_streamon(mdev);
-		ret = mxr_reg_wait4vsync(mdev);
-		WARN(ret, "failed to get vsync (%d) from output\n", ret);
-	}
-	mutex_unlock(&mdev->mutex);
-	mxr_reg_dump(mdev);
-	/* FIXME: what to do when streaming fails? */
-}
-
-void mxr_streamer_put(struct mxr_device *mdev)
-{
-	mutex_lock(&mdev->mutex);
-	--mdev->n_streamer;
-	mxr_dbg(mdev, "%s(%d)\n", __func__, mdev->n_streamer);
-	if (mdev->n_streamer == 0) {
-		int ret;
-		struct v4l2_subdev *sd = to_outsd(mdev);
-
-		mxr_reg_streamoff(mdev);
-		/* vsync applies Mixer setup */
-		ret = mxr_reg_wait4vsync(mdev);
-		WARN(ret, "failed to get vsync (%d) from output\n", ret);
-		ret = v4l2_subdev_call(sd, video, s_stream, 0);
-		WARN(ret, "stopping stream failed for output %s\n", sd->name);
-	}
-	WARN(mdev->n_streamer < 0, "negative number of streamers (%d)\n",
-		mdev->n_streamer);
-	mutex_unlock(&mdev->mutex);
-	mxr_reg_dump(mdev);
-}
-
-void mxr_output_get(struct mxr_device *mdev)
-{
-	mutex_lock(&mdev->mutex);
-	++mdev->n_output;
-	mxr_dbg(mdev, "%s(%d)\n", __func__, mdev->n_output);
-	/* turn on auxiliary driver */
-	if (mdev->n_output == 1)
-		v4l2_subdev_call(to_outsd(mdev), core, s_power, 1);
-	mutex_unlock(&mdev->mutex);
-}
-
-void mxr_output_put(struct mxr_device *mdev)
-{
-	mutex_lock(&mdev->mutex);
-	--mdev->n_output;
-	mxr_dbg(mdev, "%s(%d)\n", __func__, mdev->n_output);
-	/* turn on auxiliary driver */
-	if (mdev->n_output == 0)
-		v4l2_subdev_call(to_outsd(mdev), core, s_power, 0);
-	WARN(mdev->n_output < 0, "negative number of output users (%d)\n",
-		mdev->n_output);
-	mutex_unlock(&mdev->mutex);
-}
-
-int mxr_power_get(struct mxr_device *mdev)
-{
-	int ret = pm_runtime_get_sync(mdev->dev);
-
-	/* returning 1 means that power is already enabled,
-	 * so zero success be returned */
-	if (ret < 0)
-		return ret;
-	return 0;
-}
-
-void mxr_power_put(struct mxr_device *mdev)
-{
-	pm_runtime_put_sync(mdev->dev);
-}
-
-/* --------- RESOURCE MANAGEMENT -------------*/
-
-static int mxr_acquire_plat_resources(struct mxr_device *mdev,
-				      struct platform_device *pdev)
-{
-	struct resource *res;
-	int ret;
-
-	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "mxr");
-	if (res == NULL) {
-		mxr_err(mdev, "get memory resource failed.\n");
-		ret = -ENXIO;
-		goto fail;
-	}
-
-	mdev->res.mxr_regs = ioremap(res->start, resource_size(res));
-	if (mdev->res.mxr_regs == NULL) {
-		mxr_err(mdev, "register mapping failed.\n");
-		ret = -ENXIO;
-		goto fail;
-	}
-
-	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "vp");
-	if (res == NULL) {
-		mxr_err(mdev, "get memory resource failed.\n");
-		ret = -ENXIO;
-		goto fail_mxr_regs;
-	}
-
-	mdev->res.vp_regs = ioremap(res->start, resource_size(res));
-	if (mdev->res.vp_regs == NULL) {
-		mxr_err(mdev, "register mapping failed.\n");
-		ret = -ENXIO;
-		goto fail_mxr_regs;
-	}
-
-	res = platform_get_resource_byname(pdev, IORESOURCE_IRQ, "irq");
-	if (res == NULL) {
-		mxr_err(mdev, "get interrupt resource failed.\n");
-		ret = -ENXIO;
-		goto fail_vp_regs;
-	}
-
-	ret = request_irq(res->start, mxr_irq_handler, 0, "s5p-mixer", mdev);
-	if (ret) {
-		mxr_err(mdev, "request interrupt failed.\n");
-		goto fail_vp_regs;
-	}
-	mdev->res.irq = res->start;
-
-	return 0;
-
-fail_vp_regs:
-	iounmap(mdev->res.vp_regs);
-
-fail_mxr_regs:
-	iounmap(mdev->res.mxr_regs);
-
-fail:
-	return ret;
-}
-
-static void mxr_resource_clear_clocks(struct mxr_resources *res)
-{
-	res->mixer	= ERR_PTR(-EINVAL);
-	res->vp		= ERR_PTR(-EINVAL);
-	res->sclk_mixer	= ERR_PTR(-EINVAL);
-	res->sclk_hdmi	= ERR_PTR(-EINVAL);
-	res->sclk_dac	= ERR_PTR(-EINVAL);
-}
-
-static void mxr_release_plat_resources(struct mxr_device *mdev)
-{
-	free_irq(mdev->res.irq, mdev);
-	iounmap(mdev->res.vp_regs);
-	iounmap(mdev->res.mxr_regs);
-}
-
-static void mxr_release_clocks(struct mxr_device *mdev)
-{
-	struct mxr_resources *res = &mdev->res;
-
-	if (!IS_ERR(res->sclk_dac))
-		clk_put(res->sclk_dac);
-	if (!IS_ERR(res->sclk_hdmi))
-		clk_put(res->sclk_hdmi);
-	if (!IS_ERR(res->sclk_mixer))
-		clk_put(res->sclk_mixer);
-	if (!IS_ERR(res->vp))
-		clk_put(res->vp);
-	if (!IS_ERR(res->mixer))
-		clk_put(res->mixer);
-}
-
-static int mxr_acquire_clocks(struct mxr_device *mdev)
-{
-	struct mxr_resources *res = &mdev->res;
-	struct device *dev = mdev->dev;
-
-	mxr_resource_clear_clocks(res);
-
-	res->mixer = clk_get(dev, "mixer");
-	if (IS_ERR(res->mixer)) {
-		mxr_err(mdev, "failed to get clock 'mixer'\n");
-		goto fail;
-	}
-	res->vp = clk_get(dev, "vp");
-	if (IS_ERR(res->vp)) {
-		mxr_err(mdev, "failed to get clock 'vp'\n");
-		goto fail;
-	}
-	res->sclk_mixer = clk_get(dev, "sclk_mixer");
-	if (IS_ERR(res->sclk_mixer)) {
-		mxr_err(mdev, "failed to get clock 'sclk_mixer'\n");
-		goto fail;
-	}
-	res->sclk_hdmi = clk_get(dev, "sclk_hdmi");
-	if (IS_ERR(res->sclk_hdmi)) {
-		mxr_err(mdev, "failed to get clock 'sclk_hdmi'\n");
-		goto fail;
-	}
-	res->sclk_dac = clk_get(dev, "sclk_dac");
-	if (IS_ERR(res->sclk_dac)) {
-		mxr_err(mdev, "failed to get clock 'sclk_dac'\n");
-		goto fail;
-	}
-
-	return 0;
-fail:
-	mxr_release_clocks(mdev);
-	return -ENODEV;
-}
-
-static int mxr_acquire_resources(struct mxr_device *mdev,
-				 struct platform_device *pdev)
-{
-	int ret;
-	ret = mxr_acquire_plat_resources(mdev, pdev);
-
-	if (ret)
-		goto fail;
-
-	ret = mxr_acquire_clocks(mdev);
-	if (ret)
-		goto fail_plat;
-
-	mxr_info(mdev, "resources acquired\n");
-	return 0;
-
-fail_plat:
-	mxr_release_plat_resources(mdev);
-fail:
-	mxr_err(mdev, "resources acquire failed\n");
-	return ret;
-}
-
-static void mxr_release_resources(struct mxr_device *mdev)
-{
-	mxr_release_clocks(mdev);
-	mxr_release_plat_resources(mdev);
-	memset(&mdev->res, 0, sizeof(mdev->res));
-	mxr_resource_clear_clocks(&mdev->res);
-}
-
-static void mxr_release_layers(struct mxr_device *mdev)
-{
-	int i;
-
-	for (i = 0; i < ARRAY_SIZE(mdev->layer); ++i)
-		if (mdev->layer[i])
-			mxr_layer_release(mdev->layer[i]);
-}
-
-static int mxr_acquire_layers(struct mxr_device *mdev,
-			      struct mxr_platform_data *pdata)
-{
-	mdev->layer[0] = mxr_graph_layer_create(mdev, 0);
-	mdev->layer[1] = mxr_graph_layer_create(mdev, 1);
-	mdev->layer[2] = mxr_vp_layer_create(mdev, 0);
-
-	if (!mdev->layer[0] || !mdev->layer[1] || !mdev->layer[2]) {
-		mxr_err(mdev, "failed to acquire layers\n");
-		goto fail;
-	}
-
-	return 0;
-
-fail:
-	mxr_release_layers(mdev);
-	return -ENODEV;
-}
-
-/* ---------- POWER MANAGEMENT ----------- */
-
-static int mxr_runtime_resume(struct device *dev)
-{
-	struct mxr_device *mdev = to_mdev(dev);
-	struct mxr_resources *res = &mdev->res;
-	int ret;
-
-	mxr_dbg(mdev, "resume - start\n");
-	mutex_lock(&mdev->mutex);
-	/* turn clocks on */
-	ret = clk_prepare_enable(res->mixer);
-	if (ret < 0) {
-		dev_err(mdev->dev, "clk_prepare_enable(mixer) failed\n");
-		goto fail;
-	}
-	ret = clk_prepare_enable(res->vp);
-	if (ret < 0) {
-		dev_err(mdev->dev, "clk_prepare_enable(vp) failed\n");
-		goto fail_mixer;
-	}
-	ret = clk_prepare_enable(res->sclk_mixer);
-	if (ret < 0) {
-		dev_err(mdev->dev, "clk_prepare_enable(sclk_mixer) failed\n");
-		goto fail_vp;
-	}
-	/* apply default configuration */
-	mxr_reg_reset(mdev);
-	mxr_dbg(mdev, "resume - finished\n");
-
-	mutex_unlock(&mdev->mutex);
-	return 0;
-
-fail_vp:
-	clk_disable_unprepare(res->vp);
-fail_mixer:
-	clk_disable_unprepare(res->mixer);
-fail:
-	mutex_unlock(&mdev->mutex);
-	dev_err(mdev->dev, "resume failed\n");
-	return ret;
-}
-
-static int mxr_runtime_suspend(struct device *dev)
-{
-	struct mxr_device *mdev = to_mdev(dev);
-	struct mxr_resources *res = &mdev->res;
-	mxr_dbg(mdev, "suspend - start\n");
-	mutex_lock(&mdev->mutex);
-	/* turn clocks off */
-	clk_disable_unprepare(res->sclk_mixer);
-	clk_disable_unprepare(res->vp);
-	clk_disable_unprepare(res->mixer);
-	mutex_unlock(&mdev->mutex);
-	mxr_dbg(mdev, "suspend - finished\n");
-	return 0;
-}
-
-static const struct dev_pm_ops mxr_pm_ops = {
-	.runtime_suspend = mxr_runtime_suspend,
-	.runtime_resume	 = mxr_runtime_resume,
-};
-
-/* --------- DRIVER INITIALIZATION ---------- */
-
-static int mxr_probe(struct platform_device *pdev)
-{
-	struct device *dev = &pdev->dev;
-	struct mxr_platform_data *pdata = dev->platform_data;
-	struct mxr_device *mdev;
-	int ret;
-
-	/* mdev does not exist yet so no mxr_dbg is used */
-	dev_info(dev, "probe start\n");
-
-	mdev = kzalloc(sizeof(*mdev), GFP_KERNEL);
-	if (!mdev) {
-		dev_err(dev, "not enough memory.\n");
-		ret = -ENOMEM;
-		goto fail;
-	}
-
-	/* setup pointer to master device */
-	mdev->dev = dev;
-
-	mutex_init(&mdev->mutex);
-	spin_lock_init(&mdev->reg_slock);
-	init_waitqueue_head(&mdev->event_queue);
-
-	/* acquire resources: regs, irqs, clocks, regulators */
-	ret = mxr_acquire_resources(mdev, pdev);
-	if (ret)
-		goto fail_mem;
-
-	/* configure resources for video output */
-	ret = mxr_acquire_video(mdev, mxr_output_conf,
-		ARRAY_SIZE(mxr_output_conf));
-	if (ret)
-		goto fail_resources;
-
-	/* configure layers */
-	ret = mxr_acquire_layers(mdev, pdata);
-	if (ret)
-		goto fail_video;
-
-	pm_runtime_enable(dev);
-
-	mxr_info(mdev, "probe successful\n");
-	return 0;
-
-fail_video:
-	mxr_release_video(mdev);
-
-fail_resources:
-	mxr_release_resources(mdev);
-
-fail_mem:
-	kfree(mdev);
-
-fail:
-	dev_info(dev, "probe failed\n");
-	return ret;
-}
-
-static int mxr_remove(struct platform_device *pdev)
-{
-	struct device *dev = &pdev->dev;
-	struct mxr_device *mdev = to_mdev(dev);
-
-	pm_runtime_disable(dev);
-
-	mxr_release_layers(mdev);
-	mxr_release_video(mdev);
-	mxr_release_resources(mdev);
-
-	kfree(mdev);
-
-	dev_info(dev, "remove successful\n");
-	return 0;
-}
-
-static struct platform_driver mxr_driver __refdata = {
-	.probe = mxr_probe,
-	.remove = mxr_remove,
-	.driver = {
-		.name = MXR_DRIVER_NAME,
-		.pm = &mxr_pm_ops,
-	}
-};
-
-static int __init mxr_init(void)
-{
-	int i, ret;
-	static const char banner[] __initconst =
-		"Samsung TV Mixer driver, "
-		"(c) 2010-2011 Samsung Electronics Co., Ltd.\n";
-	pr_info("%s\n", banner);
-
-	/* Loading auxiliary modules */
-	for (i = 0; i < ARRAY_SIZE(mxr_output_conf); ++i)
-		request_module(mxr_output_conf[i].module_name);
-
-	ret = platform_driver_register(&mxr_driver);
-	if (ret != 0) {
-		pr_err("s5p-tv: registration of MIXER driver failed\n");
-		return -ENXIO;
-	}
-
-	return 0;
-}
-module_init(mxr_init);
-
-static void __exit mxr_exit(void)
-{
-	platform_driver_unregister(&mxr_driver);
-}
-module_exit(mxr_exit);
diff --git a/drivers/media/platform/s5p-tv/mixer_grp_layer.c b/drivers/media/platform/s5p-tv/mixer_grp_layer.c
deleted file mode 100644
index d4d2564f7de7..000000000000
--- a/drivers/media/platform/s5p-tv/mixer_grp_layer.c
+++ /dev/null
@@ -1,270 +0,0 @@
-/*
- * Samsung TV Mixer driver
- *
- * Copyright (c) 2010-2011 Samsung Electronics Co., Ltd.
- *
- * Tomasz Stanislawski, <t.stanislaws@samsung.com>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published
- * by the Free Software Foundiation. either version 2 of the License,
- * or (at your option) any later version
- */
-
-#include "mixer.h"
-
-#include <media/videobuf2-dma-contig.h>
-
-/* FORMAT DEFINITIONS */
-
-static const struct mxr_format mxr_fb_fmt_rgb565 = {
-	.name = "RGB565",
-	.fourcc = V4L2_PIX_FMT_RGB565,
-	.colorspace = V4L2_COLORSPACE_SRGB,
-	.num_planes = 1,
-	.plane = {
-		{ .width = 1, .height = 1, .size = 2 },
-	},
-	.num_subframes = 1,
-	.cookie = 4,
-};
-
-static const struct mxr_format mxr_fb_fmt_argb1555 = {
-	.name = "ARGB1555",
-	.num_planes = 1,
-	.fourcc = V4L2_PIX_FMT_RGB555,
-	.colorspace = V4L2_COLORSPACE_SRGB,
-	.plane = {
-		{ .width = 1, .height = 1, .size = 2 },
-	},
-	.num_subframes = 1,
-	.cookie = 5,
-};
-
-static const struct mxr_format mxr_fb_fmt_argb4444 = {
-	.name = "ARGB4444",
-	.num_planes = 1,
-	.fourcc = V4L2_PIX_FMT_RGB444,
-	.colorspace = V4L2_COLORSPACE_SRGB,
-	.plane = {
-		{ .width = 1, .height = 1, .size = 2 },
-	},
-	.num_subframes = 1,
-	.cookie = 6,
-};
-
-static const struct mxr_format mxr_fb_fmt_argb8888 = {
-	.name = "ARGB8888",
-	.fourcc = V4L2_PIX_FMT_BGR32,
-	.colorspace = V4L2_COLORSPACE_SRGB,
-	.num_planes = 1,
-	.plane = {
-		{ .width = 1, .height = 1, .size = 4 },
-	},
-	.num_subframes = 1,
-	.cookie = 7,
-};
-
-static const struct mxr_format *mxr_graph_format[] = {
-	&mxr_fb_fmt_rgb565,
-	&mxr_fb_fmt_argb1555,
-	&mxr_fb_fmt_argb4444,
-	&mxr_fb_fmt_argb8888,
-};
-
-/* AUXILIARY CALLBACKS */
-
-static void mxr_graph_layer_release(struct mxr_layer *layer)
-{
-	mxr_base_layer_unregister(layer);
-	mxr_base_layer_release(layer);
-}
-
-static void mxr_graph_buffer_set(struct mxr_layer *layer,
-	struct mxr_buffer *buf)
-{
-	dma_addr_t addr = 0;
-
-	if (buf)
-		addr = vb2_dma_contig_plane_dma_addr(&buf->vb.vb2_buf, 0);
-	mxr_reg_graph_buffer(layer->mdev, layer->idx, addr);
-}
-
-static void mxr_graph_stream_set(struct mxr_layer *layer, int en)
-{
-	mxr_reg_graph_layer_stream(layer->mdev, layer->idx, en);
-}
-
-static void mxr_graph_format_set(struct mxr_layer *layer)
-{
-	mxr_reg_graph_format(layer->mdev, layer->idx,
-		layer->fmt, &layer->geo);
-}
-
-static inline unsigned int closest(unsigned int x, unsigned int a,
-	unsigned int b, unsigned long flags)
-{
-	unsigned int mid = (a + b) / 2;
-
-	/* choosing closest value with constraints according to table:
-	 * -------------+-----+-----+-----+-------+
-	 * flags	|  0  |  LE |  GE | LE|GE |
-	 * -------------+-----+-----+-----+-------+
-	 * x <= a	|  a  |  a  |  a  |   a   |
-	 * a < x <= mid	|  a  |  a  |  b  |   a   |
-	 * mid < x < b	|  b  |  a  |  b  |   b   |
-	 * b <= x	|  b  |  b  |  b  |   b   |
-	 * -------------+-----+-----+-----+-------+
-	 */
-
-	/* remove all non-constraint flags */
-	flags &= V4L2_SEL_FLAG_LE | V4L2_SEL_FLAG_GE;
-
-	if (x <= a)
-		return  a;
-	if (x >= b)
-		return b;
-	if (flags == V4L2_SEL_FLAG_LE)
-		return a;
-	if (flags == V4L2_SEL_FLAG_GE)
-		return b;
-	if (x <= mid)
-		return a;
-	return b;
-}
-
-static inline unsigned int do_center(unsigned int center,
-	unsigned int size, unsigned int upper, unsigned int flags)
-{
-	unsigned int lower;
-
-	if (flags & MXR_NO_OFFSET)
-		return 0;
-
-	lower = center - min(center, size / 2);
-	return min(lower, upper - size);
-}
-
-static void mxr_graph_fix_geometry(struct mxr_layer *layer,
-	enum mxr_geometry_stage stage, unsigned long flags)
-{
-	struct mxr_geometry *geo = &layer->geo;
-	struct mxr_crop *src = &geo->src;
-	struct mxr_crop *dst = &geo->dst;
-	unsigned int x_center, y_center;
-
-	switch (stage) {
-
-	case MXR_GEOMETRY_SINK: /* nothing to be fixed here */
-		flags = 0;
-		/* fall through */
-
-	case MXR_GEOMETRY_COMPOSE:
-		/* remember center of the area */
-		x_center = dst->x_offset + dst->width / 2;
-		y_center = dst->y_offset + dst->height / 2;
-		/* round up/down to 2 multiple depending on flags */
-		if (flags & V4L2_SEL_FLAG_LE) {
-			dst->width = round_down(dst->width, 2);
-			dst->height = round_down(dst->height, 2);
-		} else {
-			dst->width = round_up(dst->width, 2);
-			dst->height = round_up(dst->height, 2);
-		}
-		/* assure that compose rect is inside display area */
-		dst->width = min(dst->width, dst->full_width);
-		dst->height = min(dst->height, dst->full_height);
-
-		/* ensure that compose is reachable using 2x scaling */
-		dst->width = min(dst->width, 2 * src->full_width);
-		dst->height = min(dst->height, 2 * src->full_height);
-
-		/* setup offsets */
-		dst->x_offset = do_center(x_center, dst->width,
-			dst->full_width, flags);
-		dst->y_offset = do_center(y_center, dst->height,
-			dst->full_height, flags);
-		flags = 0;
-		/* fall through */
-
-	case MXR_GEOMETRY_CROP:
-		/* remember center of the area */
-		x_center = src->x_offset + src->width / 2;
-		y_center = src->y_offset + src->height / 2;
-		/* ensure that cropping area lies inside the buffer */
-		if (src->full_width < dst->width)
-			src->width = dst->width / 2;
-		else
-			src->width = closest(src->width, dst->width / 2,
-				dst->width, flags);
-
-		if (src->width == dst->width)
-			geo->x_ratio = 0;
-		else
-			geo->x_ratio = 1;
-
-		if (src->full_height < dst->height)
-			src->height = dst->height / 2;
-		else
-			src->height = closest(src->height, dst->height / 2,
-				dst->height, flags);
-
-		if (src->height == dst->height)
-			geo->y_ratio = 0;
-		else
-			geo->y_ratio = 1;
-
-		/* setup offsets */
-		src->x_offset = do_center(x_center, src->width,
-			src->full_width, flags);
-		src->y_offset = do_center(y_center, src->height,
-			src->full_height, flags);
-		flags = 0;
-		/* fall through */
-	case MXR_GEOMETRY_SOURCE:
-		src->full_width = clamp_val(src->full_width,
-			src->width + src->x_offset, 32767);
-		src->full_height = clamp_val(src->full_height,
-			src->height + src->y_offset, 2047);
-	}
-}
-
-/* PUBLIC API */
-
-struct mxr_layer *mxr_graph_layer_create(struct mxr_device *mdev, int idx)
-{
-	struct mxr_layer *layer;
-	int ret;
-	const struct mxr_layer_ops ops = {
-		.release = mxr_graph_layer_release,
-		.buffer_set = mxr_graph_buffer_set,
-		.stream_set = mxr_graph_stream_set,
-		.format_set = mxr_graph_format_set,
-		.fix_geometry = mxr_graph_fix_geometry,
-	};
-	char name[32];
-
-	sprintf(name, "graph%d", idx);
-
-	layer = mxr_base_layer_create(mdev, idx, name, &ops);
-	if (layer == NULL) {
-		mxr_err(mdev, "failed to initialize layer(%d) base\n", idx);
-		goto fail;
-	}
-
-	layer->fmt_array = mxr_graph_format;
-	layer->fmt_array_size = ARRAY_SIZE(mxr_graph_format);
-
-	ret = mxr_base_layer_register(layer);
-	if (ret)
-		goto fail_layer;
-
-	return layer;
-
-fail_layer:
-	mxr_base_layer_release(layer);
-
-fail:
-	return NULL;
-}
-
diff --git a/drivers/media/platform/s5p-tv/mixer_reg.c b/drivers/media/platform/s5p-tv/mixer_reg.c
deleted file mode 100644
index a0ec14a1da13..000000000000
--- a/drivers/media/platform/s5p-tv/mixer_reg.c
+++ /dev/null
@@ -1,551 +0,0 @@
-/*
- * Samsung TV Mixer driver
- *
- * Copyright (c) 2010-2011 Samsung Electronics Co., Ltd.
- *
- * Tomasz Stanislawski, <t.stanislaws@samsung.com>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published
- * by the Free Software Foundiation. either version 2 of the License,
- * or (at your option) any later version
- */
-
-#include "mixer.h"
-#include "regs-mixer.h"
-#include "regs-vp.h"
-
-#include <linux/delay.h>
-
-/* Register access subroutines */
-
-static inline u32 vp_read(struct mxr_device *mdev, u32 reg_id)
-{
-	return readl(mdev->res.vp_regs + reg_id);
-}
-
-static inline void vp_write(struct mxr_device *mdev, u32 reg_id, u32 val)
-{
-	writel(val, mdev->res.vp_regs + reg_id);
-}
-
-static inline void vp_write_mask(struct mxr_device *mdev, u32 reg_id,
-	u32 val, u32 mask)
-{
-	u32 old = vp_read(mdev, reg_id);
-
-	val = (val & mask) | (old & ~mask);
-	writel(val, mdev->res.vp_regs + reg_id);
-}
-
-static inline u32 mxr_read(struct mxr_device *mdev, u32 reg_id)
-{
-	return readl(mdev->res.mxr_regs + reg_id);
-}
-
-static inline void mxr_write(struct mxr_device *mdev, u32 reg_id, u32 val)
-{
-	writel(val, mdev->res.mxr_regs + reg_id);
-}
-
-static inline void mxr_write_mask(struct mxr_device *mdev, u32 reg_id,
-	u32 val, u32 mask)
-{
-	u32 old = mxr_read(mdev, reg_id);
-
-	val = (val & mask) | (old & ~mask);
-	writel(val, mdev->res.mxr_regs + reg_id);
-}
-
-void mxr_vsync_set_update(struct mxr_device *mdev, int en)
-{
-	/* block update on vsync */
-	mxr_write_mask(mdev, MXR_STATUS, en ? MXR_STATUS_SYNC_ENABLE : 0,
-		MXR_STATUS_SYNC_ENABLE);
-	vp_write(mdev, VP_SHADOW_UPDATE, en ? VP_SHADOW_UPDATE_ENABLE : 0);
-}
-
-static void __mxr_reg_vp_reset(struct mxr_device *mdev)
-{
-	int tries = 100;
-
-	vp_write(mdev, VP_SRESET, VP_SRESET_PROCESSING);
-	for (tries = 100; tries; --tries) {
-		/* waiting until VP_SRESET_PROCESSING is 0 */
-		if (~vp_read(mdev, VP_SRESET) & VP_SRESET_PROCESSING)
-			break;
-		mdelay(10);
-	}
-	WARN(tries == 0, "failed to reset Video Processor\n");
-}
-
-static void mxr_reg_vp_default_filter(struct mxr_device *mdev);
-
-void mxr_reg_reset(struct mxr_device *mdev)
-{
-	unsigned long flags;
-	u32 val; /* value stored to register */
-
-	spin_lock_irqsave(&mdev->reg_slock, flags);
-	mxr_vsync_set_update(mdev, MXR_DISABLE);
-
-	/* set output in RGB888 mode */
-	mxr_write(mdev, MXR_CFG, MXR_CFG_OUT_RGB888);
-
-	/* 16 beat burst in DMA */
-	mxr_write_mask(mdev, MXR_STATUS, MXR_STATUS_16_BURST,
-		MXR_STATUS_BURST_MASK);
-
-	/* setting default layer priority: layer1 > video > layer0
-	 * because typical usage scenario would be
-	 * layer0 - framebuffer
-	 * video - video overlay
-	 * layer1 - OSD
-	 */
-	val  = MXR_LAYER_CFG_GRP0_VAL(1);
-	val |= MXR_LAYER_CFG_VP_VAL(2);
-	val |= MXR_LAYER_CFG_GRP1_VAL(3);
-	mxr_write(mdev, MXR_LAYER_CFG, val);
-
-	/* use dark gray background color */
-	mxr_write(mdev, MXR_BG_COLOR0, 0x808080);
-	mxr_write(mdev, MXR_BG_COLOR1, 0x808080);
-	mxr_write(mdev, MXR_BG_COLOR2, 0x808080);
-
-	/* setting graphical layers */
-
-	val  = MXR_GRP_CFG_COLOR_KEY_DISABLE; /* no blank key */
-	val |= MXR_GRP_CFG_BLEND_PRE_MUL; /* premul mode */
-	val |= MXR_GRP_CFG_ALPHA_VAL(0xff); /* non-transparent alpha */
-
-	/* the same configuration for both layers */
-	mxr_write(mdev, MXR_GRAPHIC_CFG(0), val);
-	mxr_write(mdev, MXR_GRAPHIC_CFG(1), val);
-
-	/* configuration of Video Processor Registers */
-	__mxr_reg_vp_reset(mdev);
-	mxr_reg_vp_default_filter(mdev);
-
-	/* enable all interrupts */
-	mxr_write_mask(mdev, MXR_INT_EN, ~0, MXR_INT_EN_ALL);
-
-	mxr_vsync_set_update(mdev, MXR_ENABLE);
-	spin_unlock_irqrestore(&mdev->reg_slock, flags);
-}
-
-void mxr_reg_graph_format(struct mxr_device *mdev, int idx,
-	const struct mxr_format *fmt, const struct mxr_geometry *geo)
-{
-	u32 val;
-	unsigned long flags;
-
-	spin_lock_irqsave(&mdev->reg_slock, flags);
-	mxr_vsync_set_update(mdev, MXR_DISABLE);
-
-	/* setup format */
-	mxr_write_mask(mdev, MXR_GRAPHIC_CFG(idx),
-		MXR_GRP_CFG_FORMAT_VAL(fmt->cookie), MXR_GRP_CFG_FORMAT_MASK);
-
-	/* setup geometry */
-	mxr_write(mdev, MXR_GRAPHIC_SPAN(idx), geo->src.full_width);
-	val  = MXR_GRP_WH_WIDTH(geo->src.width);
-	val |= MXR_GRP_WH_HEIGHT(geo->src.height);
-	val |= MXR_GRP_WH_H_SCALE(geo->x_ratio);
-	val |= MXR_GRP_WH_V_SCALE(geo->y_ratio);
-	mxr_write(mdev, MXR_GRAPHIC_WH(idx), val);
-
-	/* setup offsets in source image */
-	val  = MXR_GRP_SXY_SX(geo->src.x_offset);
-	val |= MXR_GRP_SXY_SY(geo->src.y_offset);
-	mxr_write(mdev, MXR_GRAPHIC_SXY(idx), val);
-
-	/* setup offsets in display image */
-	val  = MXR_GRP_DXY_DX(geo->dst.x_offset);
-	val |= MXR_GRP_DXY_DY(geo->dst.y_offset);
-	mxr_write(mdev, MXR_GRAPHIC_DXY(idx), val);
-
-	mxr_vsync_set_update(mdev, MXR_ENABLE);
-	spin_unlock_irqrestore(&mdev->reg_slock, flags);
-}
-
-void mxr_reg_vp_format(struct mxr_device *mdev,
-	const struct mxr_format *fmt, const struct mxr_geometry *geo)
-{
-	unsigned long flags;
-
-	spin_lock_irqsave(&mdev->reg_slock, flags);
-	mxr_vsync_set_update(mdev, MXR_DISABLE);
-
-	vp_write_mask(mdev, VP_MODE, fmt->cookie, VP_MODE_FMT_MASK);
-
-	/* setting size of input image */
-	vp_write(mdev, VP_IMG_SIZE_Y, VP_IMG_HSIZE(geo->src.full_width) |
-		VP_IMG_VSIZE(geo->src.full_height));
-	/* chroma height has to reduced by 2 to avoid chroma distorions */
-	vp_write(mdev, VP_IMG_SIZE_C, VP_IMG_HSIZE(geo->src.full_width) |
-		VP_IMG_VSIZE(geo->src.full_height / 2));
-
-	vp_write(mdev, VP_SRC_WIDTH, geo->src.width);
-	vp_write(mdev, VP_SRC_HEIGHT, geo->src.height);
-	vp_write(mdev, VP_SRC_H_POSITION,
-		VP_SRC_H_POSITION_VAL(geo->src.x_offset));
-	vp_write(mdev, VP_SRC_V_POSITION, geo->src.y_offset);
-
-	vp_write(mdev, VP_DST_WIDTH, geo->dst.width);
-	vp_write(mdev, VP_DST_H_POSITION, geo->dst.x_offset);
-	if (geo->dst.field == V4L2_FIELD_INTERLACED) {
-		vp_write(mdev, VP_DST_HEIGHT, geo->dst.height / 2);
-		vp_write(mdev, VP_DST_V_POSITION, geo->dst.y_offset / 2);
-	} else {
-		vp_write(mdev, VP_DST_HEIGHT, geo->dst.height);
-		vp_write(mdev, VP_DST_V_POSITION, geo->dst.y_offset);
-	}
-
-	vp_write(mdev, VP_H_RATIO, geo->x_ratio);
-	vp_write(mdev, VP_V_RATIO, geo->y_ratio);
-
-	vp_write(mdev, VP_ENDIAN_MODE, VP_ENDIAN_MODE_LITTLE);
-
-	mxr_vsync_set_update(mdev, MXR_ENABLE);
-	spin_unlock_irqrestore(&mdev->reg_slock, flags);
-
-}
-
-void mxr_reg_graph_buffer(struct mxr_device *mdev, int idx, dma_addr_t addr)
-{
-	u32 val = addr ? ~0 : 0;
-	unsigned long flags;
-
-	spin_lock_irqsave(&mdev->reg_slock, flags);
-	mxr_vsync_set_update(mdev, MXR_DISABLE);
-
-	if (idx == 0)
-		mxr_write_mask(mdev, MXR_CFG, val, MXR_CFG_GRP0_ENABLE);
-	else
-		mxr_write_mask(mdev, MXR_CFG, val, MXR_CFG_GRP1_ENABLE);
-	mxr_write(mdev, MXR_GRAPHIC_BASE(idx), addr);
-
-	mxr_vsync_set_update(mdev, MXR_ENABLE);
-	spin_unlock_irqrestore(&mdev->reg_slock, flags);
-}
-
-void mxr_reg_vp_buffer(struct mxr_device *mdev,
-	dma_addr_t luma_addr[2], dma_addr_t chroma_addr[2])
-{
-	u32 val = luma_addr[0] ? ~0 : 0;
-	unsigned long flags;
-
-	spin_lock_irqsave(&mdev->reg_slock, flags);
-	mxr_vsync_set_update(mdev, MXR_DISABLE);
-
-	mxr_write_mask(mdev, MXR_CFG, val, MXR_CFG_VP_ENABLE);
-	vp_write_mask(mdev, VP_ENABLE, val, VP_ENABLE_ON);
-	/* TODO: fix tiled mode */
-	vp_write(mdev, VP_TOP_Y_PTR, luma_addr[0]);
-	vp_write(mdev, VP_TOP_C_PTR, chroma_addr[0]);
-	vp_write(mdev, VP_BOT_Y_PTR, luma_addr[1]);
-	vp_write(mdev, VP_BOT_C_PTR, chroma_addr[1]);
-
-	mxr_vsync_set_update(mdev, MXR_ENABLE);
-	spin_unlock_irqrestore(&mdev->reg_slock, flags);
-}
-
-static void mxr_irq_layer_handle(struct mxr_layer *layer)
-{
-	struct list_head *head = &layer->enq_list;
-	struct mxr_buffer *done;
-
-	/* skip non-existing layer */
-	if (layer == NULL)
-		return;
-
-	spin_lock(&layer->enq_slock);
-	if (layer->state == MXR_LAYER_IDLE)
-		goto done;
-
-	done = layer->shadow_buf;
-	layer->shadow_buf = layer->update_buf;
-
-	if (list_empty(head)) {
-		if (layer->state != MXR_LAYER_STREAMING)
-			layer->update_buf = NULL;
-	} else {
-		struct mxr_buffer *next;
-		next = list_first_entry(head, struct mxr_buffer, list);
-		list_del(&next->list);
-		layer->update_buf = next;
-	}
-
-	layer->ops.buffer_set(layer, layer->update_buf);
-
-	if (done && done != layer->shadow_buf)
-		vb2_buffer_done(&done->vb.vb2_buf, VB2_BUF_STATE_DONE);
-
-done:
-	spin_unlock(&layer->enq_slock);
-}
-
-irqreturn_t mxr_irq_handler(int irq, void *dev_data)
-{
-	struct mxr_device *mdev = dev_data;
-	u32 i, val;
-
-	spin_lock(&mdev->reg_slock);
-	val = mxr_read(mdev, MXR_INT_STATUS);
-
-	/* wake up process waiting for VSYNC */
-	if (val & MXR_INT_STATUS_VSYNC) {
-		set_bit(MXR_EVENT_VSYNC, &mdev->event_flags);
-		/* toggle TOP field event if working in interlaced mode */
-		if (~mxr_read(mdev, MXR_CFG) & MXR_CFG_SCAN_PROGRASSIVE)
-			change_bit(MXR_EVENT_TOP, &mdev->event_flags);
-		wake_up(&mdev->event_queue);
-		/* vsync interrupt use different bit for read and clear */
-		val &= ~MXR_INT_STATUS_VSYNC;
-		val |= MXR_INT_CLEAR_VSYNC;
-	}
-
-	/* clear interrupts */
-	mxr_write(mdev, MXR_INT_STATUS, val);
-
-	spin_unlock(&mdev->reg_slock);
-	/* leave on non-vsync event */
-	if (~val & MXR_INT_CLEAR_VSYNC)
-		return IRQ_HANDLED;
-	/* skip layer update on bottom field */
-	if (!test_bit(MXR_EVENT_TOP, &mdev->event_flags))
-		return IRQ_HANDLED;
-	for (i = 0; i < MXR_MAX_LAYERS; ++i)
-		mxr_irq_layer_handle(mdev->layer[i]);
-	return IRQ_HANDLED;
-}
-
-void mxr_reg_s_output(struct mxr_device *mdev, int cookie)
-{
-	u32 val;
-
-	val = cookie == 0 ? MXR_CFG_DST_SDO : MXR_CFG_DST_HDMI;
-	mxr_write_mask(mdev, MXR_CFG, val, MXR_CFG_DST_MASK);
-}
-
-void mxr_reg_streamon(struct mxr_device *mdev)
-{
-	unsigned long flags;
-
-	spin_lock_irqsave(&mdev->reg_slock, flags);
-	/* single write -> no need to block vsync update */
-
-	/* start MIXER */
-	mxr_write_mask(mdev, MXR_STATUS, ~0, MXR_STATUS_REG_RUN);
-	set_bit(MXR_EVENT_TOP, &mdev->event_flags);
-
-	spin_unlock_irqrestore(&mdev->reg_slock, flags);
-}
-
-void mxr_reg_streamoff(struct mxr_device *mdev)
-{
-	unsigned long flags;
-
-	spin_lock_irqsave(&mdev->reg_slock, flags);
-	/* single write -> no need to block vsync update */
-
-	/* stop MIXER */
-	mxr_write_mask(mdev, MXR_STATUS, 0, MXR_STATUS_REG_RUN);
-
-	spin_unlock_irqrestore(&mdev->reg_slock, flags);
-}
-
-int mxr_reg_wait4vsync(struct mxr_device *mdev)
-{
-	long time_left;
-
-	clear_bit(MXR_EVENT_VSYNC, &mdev->event_flags);
-	/* TODO: consider adding interruptible */
-	time_left = wait_event_timeout(mdev->event_queue,
-			test_bit(MXR_EVENT_VSYNC, &mdev->event_flags),
-				 msecs_to_jiffies(1000));
-	if (time_left > 0)
-		return 0;
-	mxr_warn(mdev, "no vsync detected - timeout\n");
-	return -ETIME;
-}
-
-void mxr_reg_set_mbus_fmt(struct mxr_device *mdev,
-	struct v4l2_mbus_framefmt *fmt)
-{
-	u32 val = 0;
-	unsigned long flags;
-
-	spin_lock_irqsave(&mdev->reg_slock, flags);
-	mxr_vsync_set_update(mdev, MXR_DISABLE);
-
-	/* selecting colorspace accepted by output */
-	if (fmt->colorspace == V4L2_COLORSPACE_JPEG)
-		val |= MXR_CFG_OUT_YUV444;
-	else
-		val |= MXR_CFG_OUT_RGB888;
-
-	/* choosing between interlace and progressive mode */
-	if (fmt->field == V4L2_FIELD_INTERLACED)
-		val |= MXR_CFG_SCAN_INTERLACE;
-	else
-		val |= MXR_CFG_SCAN_PROGRASSIVE;
-
-	/* choosing between porper HD and SD mode */
-	if (fmt->height == 480)
-		val |= MXR_CFG_SCAN_NTSC | MXR_CFG_SCAN_SD;
-	else if (fmt->height == 576)
-		val |= MXR_CFG_SCAN_PAL | MXR_CFG_SCAN_SD;
-	else if (fmt->height == 720)
-		val |= MXR_CFG_SCAN_HD_720 | MXR_CFG_SCAN_HD;
-	else if (fmt->height == 1080)
-		val |= MXR_CFG_SCAN_HD_1080 | MXR_CFG_SCAN_HD;
-	else
-		WARN(1, "unrecognized mbus height %u!\n", fmt->height);
-
-	mxr_write_mask(mdev, MXR_CFG, val, MXR_CFG_SCAN_MASK |
-		MXR_CFG_OUT_MASK);
-
-	val = (fmt->field == V4L2_FIELD_INTERLACED) ? ~0 : 0;
-	vp_write_mask(mdev, VP_MODE, val,
-		VP_MODE_LINE_SKIP | VP_MODE_FIELD_ID_AUTO_TOGGLING);
-
-	mxr_vsync_set_update(mdev, MXR_ENABLE);
-	spin_unlock_irqrestore(&mdev->reg_slock, flags);
-}
-
-void mxr_reg_graph_layer_stream(struct mxr_device *mdev, int idx, int en)
-{
-	/* no extra actions need to be done */
-}
-
-void mxr_reg_vp_layer_stream(struct mxr_device *mdev, int en)
-{
-	/* no extra actions need to be done */
-}
-
-static const u8 filter_y_horiz_tap8[] = {
-	0,	-1,	-1,	-1,	-1,	-1,	-1,	-1,
-	-1,	-1,	-1,	-1,	-1,	0,	0,	0,
-	0,	2,	4,	5,	6,	6,	6,	6,
-	6,	5,	5,	4,	3,	2,	1,	1,
-	0,	-6,	-12,	-16,	-18,	-20,	-21,	-20,
-	-20,	-18,	-16,	-13,	-10,	-8,	-5,	-2,
-	127,	126,	125,	121,	114,	107,	99,	89,
-	79,	68,	57,	46,	35,	25,	16,	8,
-};
-
-static const u8 filter_y_vert_tap4[] = {
-	0,	-3,	-6,	-8,	-8,	-8,	-8,	-7,
-	-6,	-5,	-4,	-3,	-2,	-1,	-1,	0,
-	127,	126,	124,	118,	111,	102,	92,	81,
-	70,	59,	48,	37,	27,	19,	11,	5,
-	0,	5,	11,	19,	27,	37,	48,	59,
-	70,	81,	92,	102,	111,	118,	124,	126,
-	0,	0,	-1,	-1,	-2,	-3,	-4,	-5,
-	-6,	-7,	-8,	-8,	-8,	-8,	-6,	-3,
-};
-
-static const u8 filter_cr_horiz_tap4[] = {
-	0,	-3,	-6,	-8,	-8,	-8,	-8,	-7,
-	-6,	-5,	-4,	-3,	-2,	-1,	-1,	0,
-	127,	126,	124,	118,	111,	102,	92,	81,
-	70,	59,	48,	37,	27,	19,	11,	5,
-};
-
-static inline void mxr_reg_vp_filter_set(struct mxr_device *mdev,
-	int reg_id, const u8 *data, unsigned int size)
-{
-	/* assure 4-byte align */
-	BUG_ON(size & 3);
-	for (; size; size -= 4, reg_id += 4, data += 4) {
-		u32 val = (data[0] << 24) |  (data[1] << 16) |
-			(data[2] << 8) | data[3];
-		vp_write(mdev, reg_id, val);
-	}
-}
-
-static void mxr_reg_vp_default_filter(struct mxr_device *mdev)
-{
-	mxr_reg_vp_filter_set(mdev, VP_POLY8_Y0_LL,
-		filter_y_horiz_tap8, sizeof(filter_y_horiz_tap8));
-	mxr_reg_vp_filter_set(mdev, VP_POLY4_Y0_LL,
-		filter_y_vert_tap4, sizeof(filter_y_vert_tap4));
-	mxr_reg_vp_filter_set(mdev, VP_POLY4_C0_LL,
-		filter_cr_horiz_tap4, sizeof(filter_cr_horiz_tap4));
-}
-
-static void mxr_reg_mxr_dump(struct mxr_device *mdev)
-{
-#define DUMPREG(reg_id) \
-do { \
-	mxr_dbg(mdev, #reg_id " = %08x\n", \
-		(u32)readl(mdev->res.mxr_regs + reg_id)); \
-} while (0)
-
-	DUMPREG(MXR_STATUS);
-	DUMPREG(MXR_CFG);
-	DUMPREG(MXR_INT_EN);
-	DUMPREG(MXR_INT_STATUS);
-
-	DUMPREG(MXR_LAYER_CFG);
-	DUMPREG(MXR_VIDEO_CFG);
-
-	DUMPREG(MXR_GRAPHIC0_CFG);
-	DUMPREG(MXR_GRAPHIC0_BASE);
-	DUMPREG(MXR_GRAPHIC0_SPAN);
-	DUMPREG(MXR_GRAPHIC0_WH);
-	DUMPREG(MXR_GRAPHIC0_SXY);
-	DUMPREG(MXR_GRAPHIC0_DXY);
-
-	DUMPREG(MXR_GRAPHIC1_CFG);
-	DUMPREG(MXR_GRAPHIC1_BASE);
-	DUMPREG(MXR_GRAPHIC1_SPAN);
-	DUMPREG(MXR_GRAPHIC1_WH);
-	DUMPREG(MXR_GRAPHIC1_SXY);
-	DUMPREG(MXR_GRAPHIC1_DXY);
-#undef DUMPREG
-}
-
-static void mxr_reg_vp_dump(struct mxr_device *mdev)
-{
-#define DUMPREG(reg_id) \
-do { \
-	mxr_dbg(mdev, #reg_id " = %08x\n", \
-		(u32) readl(mdev->res.vp_regs + reg_id)); \
-} while (0)
-
-
-	DUMPREG(VP_ENABLE);
-	DUMPREG(VP_SRESET);
-	DUMPREG(VP_SHADOW_UPDATE);
-	DUMPREG(VP_FIELD_ID);
-	DUMPREG(VP_MODE);
-	DUMPREG(VP_IMG_SIZE_Y);
-	DUMPREG(VP_IMG_SIZE_C);
-	DUMPREG(VP_PER_RATE_CTRL);
-	DUMPREG(VP_TOP_Y_PTR);
-	DUMPREG(VP_BOT_Y_PTR);
-	DUMPREG(VP_TOP_C_PTR);
-	DUMPREG(VP_BOT_C_PTR);
-	DUMPREG(VP_ENDIAN_MODE);
-	DUMPREG(VP_SRC_H_POSITION);
-	DUMPREG(VP_SRC_V_POSITION);
-	DUMPREG(VP_SRC_WIDTH);
-	DUMPREG(VP_SRC_HEIGHT);
-	DUMPREG(VP_DST_H_POSITION);
-	DUMPREG(VP_DST_V_POSITION);
-	DUMPREG(VP_DST_WIDTH);
-	DUMPREG(VP_DST_HEIGHT);
-	DUMPREG(VP_H_RATIO);
-	DUMPREG(VP_V_RATIO);
-
-#undef DUMPREG
-}
-
-void mxr_reg_dump(struct mxr_device *mdev)
-{
-	mxr_reg_mxr_dump(mdev);
-	mxr_reg_vp_dump(mdev);
-}
-
diff --git a/drivers/media/platform/s5p-tv/mixer_video.c b/drivers/media/platform/s5p-tv/mixer_video.c
deleted file mode 100644
index ee74e2b44d69..000000000000
--- a/drivers/media/platform/s5p-tv/mixer_video.c
+++ /dev/null
@@ -1,1130 +0,0 @@
-/*
- * Samsung TV Mixer driver
- *
- * Copyright (c) 2010-2011 Samsung Electronics Co., Ltd.
- *
- * Tomasz Stanislawski, <t.stanislaws@samsung.com>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published
- * by the Free Software Foundation. either version 2 of the License,
- * or (at your option) any later version
- */
-
-#define pr_fmt(fmt) "s5p-tv (mixer): " fmt
-
-#include "mixer.h"
-
-#include <media/v4l2-ioctl.h>
-#include <linux/videodev2.h>
-#include <linux/mm.h>
-#include <linux/module.h>
-#include <linux/platform_device.h>
-#include <linux/timer.h>
-#include <media/videobuf2-dma-contig.h>
-
-static int find_reg_callback(struct device *dev, void *p)
-{
-	struct v4l2_subdev **sd = p;
-
-	*sd = dev_get_drvdata(dev);
-	/* non-zero value stops iteration */
-	return 1;
-}
-
-static struct v4l2_subdev *find_and_register_subdev(
-	struct mxr_device *mdev, char *module_name)
-{
-	struct device_driver *drv;
-	struct v4l2_subdev *sd = NULL;
-	int ret;
-
-	/* TODO: add waiting until probe is finished */
-	drv = driver_find(module_name, &platform_bus_type);
-	if (!drv) {
-		mxr_warn(mdev, "module %s is missing\n", module_name);
-		return NULL;
-	}
-	/* driver refcnt is increased, it is safe to iterate over devices */
-	ret = driver_for_each_device(drv, NULL, &sd, find_reg_callback);
-	/* ret == 0 means that find_reg_callback was never executed */
-	if (sd == NULL) {
-		mxr_warn(mdev, "module %s provides no subdev!\n", module_name);
-		goto done;
-	}
-	/* v4l2_device_register_subdev detects if sd is NULL */
-	ret = v4l2_device_register_subdev(&mdev->v4l2_dev, sd);
-	if (ret) {
-		mxr_warn(mdev, "failed to register subdev %s\n", sd->name);
-		sd = NULL;
-	}
-
-done:
-	return sd;
-}
-
-int mxr_acquire_video(struct mxr_device *mdev,
-		      struct mxr_output_conf *output_conf, int output_count)
-{
-	struct device *dev = mdev->dev;
-	struct v4l2_device *v4l2_dev = &mdev->v4l2_dev;
-	int i;
-	int ret = 0;
-	struct v4l2_subdev *sd;
-
-	strlcpy(v4l2_dev->name, dev_name(mdev->dev), sizeof(v4l2_dev->name));
-	/* prepare context for V4L2 device */
-	ret = v4l2_device_register(dev, v4l2_dev);
-	if (ret) {
-		mxr_err(mdev, "could not register v4l2 device.\n");
-		goto fail;
-	}
-
-	vb2_dma_contig_set_max_seg_size(mdev->dev, DMA_BIT_MASK(32));
-
-	/* registering outputs */
-	mdev->output_cnt = 0;
-	for (i = 0; i < output_count; ++i) {
-		struct mxr_output_conf *conf = &output_conf[i];
-		struct mxr_output *out;
-
-		sd = find_and_register_subdev(mdev, conf->module_name);
-		/* trying to register next output */
-		if (sd == NULL)
-			continue;
-		out = kzalloc(sizeof(*out), GFP_KERNEL);
-		if (out == NULL) {
-			mxr_err(mdev, "no memory for '%s'\n",
-				conf->output_name);
-			ret = -ENOMEM;
-			/* registered subdevs are removed in fail_v4l2_dev */
-			goto fail_output;
-		}
-		strlcpy(out->name, conf->output_name, sizeof(out->name));
-		out->sd = sd;
-		out->cookie = conf->cookie;
-		mdev->output[mdev->output_cnt++] = out;
-		mxr_info(mdev, "added output '%s' from module '%s'\n",
-			conf->output_name, conf->module_name);
-		/* checking if maximal number of outputs is reached */
-		if (mdev->output_cnt >= MXR_MAX_OUTPUTS)
-			break;
-	}
-
-	if (mdev->output_cnt == 0) {
-		mxr_err(mdev, "failed to register any output\n");
-		ret = -ENODEV;
-		/* skipping fail_output because there is nothing to free */
-		goto fail_v4l2_dev;
-	}
-
-	return 0;
-
-fail_output:
-	/* kfree is NULL-safe */
-	for (i = 0; i < mdev->output_cnt; ++i)
-		kfree(mdev->output[i]);
-	memset(mdev->output, 0, sizeof(mdev->output));
-
-fail_v4l2_dev:
-	/* NOTE: automatically unregister all subdevs */
-	v4l2_device_unregister(v4l2_dev);
-
-fail:
-	return ret;
-}
-
-void mxr_release_video(struct mxr_device *mdev)
-{
-	int i;
-
-	/* kfree is NULL-safe */
-	for (i = 0; i < mdev->output_cnt; ++i)
-		kfree(mdev->output[i]);
-
-	vb2_dma_contig_clear_max_seg_size(mdev->dev);
-	v4l2_device_unregister(&mdev->v4l2_dev);
-}
-
-static int mxr_querycap(struct file *file, void *priv,
-	struct v4l2_capability *cap)
-{
-	struct mxr_layer *layer = video_drvdata(file);
-
-	mxr_dbg(layer->mdev, "%s:%d\n", __func__, __LINE__);
-
-	strlcpy(cap->driver, MXR_DRIVER_NAME, sizeof(cap->driver));
-	strlcpy(cap->card, layer->vfd.name, sizeof(cap->card));
-	sprintf(cap->bus_info, "%d", layer->idx);
-	cap->device_caps = V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_OUTPUT_MPLANE;
-	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
-
-	return 0;
-}
-
-static void mxr_geometry_dump(struct mxr_device *mdev, struct mxr_geometry *geo)
-{
-	mxr_dbg(mdev, "src.full_size = (%u, %u)\n",
-		geo->src.full_width, geo->src.full_height);
-	mxr_dbg(mdev, "src.size = (%u, %u)\n",
-		geo->src.width, geo->src.height);
-	mxr_dbg(mdev, "src.offset = (%u, %u)\n",
-		geo->src.x_offset, geo->src.y_offset);
-	mxr_dbg(mdev, "dst.full_size = (%u, %u)\n",
-		geo->dst.full_width, geo->dst.full_height);
-	mxr_dbg(mdev, "dst.size = (%u, %u)\n",
-		geo->dst.width, geo->dst.height);
-	mxr_dbg(mdev, "dst.offset = (%u, %u)\n",
-		geo->dst.x_offset, geo->dst.y_offset);
-	mxr_dbg(mdev, "ratio = (%u, %u)\n",
-		geo->x_ratio, geo->y_ratio);
-}
-
-static void mxr_layer_default_geo(struct mxr_layer *layer)
-{
-	struct mxr_device *mdev = layer->mdev;
-	struct v4l2_mbus_framefmt mbus_fmt;
-
-	memset(&layer->geo, 0, sizeof(layer->geo));
-
-	mxr_get_mbus_fmt(mdev, &mbus_fmt);
-
-	layer->geo.dst.full_width = mbus_fmt.width;
-	layer->geo.dst.full_height = mbus_fmt.height;
-	layer->geo.dst.width = layer->geo.dst.full_width;
-	layer->geo.dst.height = layer->geo.dst.full_height;
-	layer->geo.dst.field = mbus_fmt.field;
-
-	layer->geo.src.full_width = mbus_fmt.width;
-	layer->geo.src.full_height = mbus_fmt.height;
-	layer->geo.src.width = layer->geo.src.full_width;
-	layer->geo.src.height = layer->geo.src.full_height;
-
-	mxr_geometry_dump(mdev, &layer->geo);
-	layer->ops.fix_geometry(layer, MXR_GEOMETRY_SINK, 0);
-	mxr_geometry_dump(mdev, &layer->geo);
-}
-
-static void mxr_layer_update_output(struct mxr_layer *layer)
-{
-	struct mxr_device *mdev = layer->mdev;
-	struct v4l2_mbus_framefmt mbus_fmt;
-
-	mxr_get_mbus_fmt(mdev, &mbus_fmt);
-	/* checking if update is needed */
-	if (layer->geo.dst.full_width == mbus_fmt.width &&
-		layer->geo.dst.full_height == mbus_fmt.width)
-		return;
-
-	layer->geo.dst.full_width = mbus_fmt.width;
-	layer->geo.dst.full_height = mbus_fmt.height;
-	layer->geo.dst.field = mbus_fmt.field;
-	layer->ops.fix_geometry(layer, MXR_GEOMETRY_SINK, 0);
-
-	mxr_geometry_dump(mdev, &layer->geo);
-}
-
-static const struct mxr_format *find_format_by_fourcc(
-	struct mxr_layer *layer, unsigned long fourcc);
-static const struct mxr_format *find_format_by_index(
-	struct mxr_layer *layer, unsigned long index);
-
-static int mxr_enum_fmt(struct file *file, void  *priv,
-	struct v4l2_fmtdesc *f)
-{
-	struct mxr_layer *layer = video_drvdata(file);
-	struct mxr_device *mdev = layer->mdev;
-	const struct mxr_format *fmt;
-
-	mxr_dbg(mdev, "%s\n", __func__);
-	fmt = find_format_by_index(layer, f->index);
-	if (fmt == NULL)
-		return -EINVAL;
-
-	strlcpy(f->description, fmt->name, sizeof(f->description));
-	f->pixelformat = fmt->fourcc;
-
-	return 0;
-}
-
-static unsigned int divup(unsigned int divident, unsigned int divisor)
-{
-	return (divident + divisor - 1) / divisor;
-}
-
-unsigned long mxr_get_plane_size(const struct mxr_block *blk,
-	unsigned int width, unsigned int height)
-{
-	unsigned int bl_width = divup(width, blk->width);
-	unsigned int bl_height = divup(height, blk->height);
-
-	return bl_width * bl_height * blk->size;
-}
-
-static void mxr_mplane_fill(struct v4l2_plane_pix_format *planes,
-	const struct mxr_format *fmt, u32 width, u32 height)
-{
-	int i;
-
-	/* checking if nothing to fill */
-	if (!planes)
-		return;
-
-	memset(planes, 0, sizeof(*planes) * fmt->num_subframes);
-	for (i = 0; i < fmt->num_planes; ++i) {
-		struct v4l2_plane_pix_format *plane = planes
-			+ fmt->plane2subframe[i];
-		const struct mxr_block *blk = &fmt->plane[i];
-		u32 bl_width = divup(width, blk->width);
-		u32 bl_height = divup(height, blk->height);
-		u32 sizeimage = bl_width * bl_height * blk->size;
-		u32 bytesperline = bl_width * blk->size / blk->height;
-
-		plane->sizeimage += sizeimage;
-		plane->bytesperline = max(plane->bytesperline, bytesperline);
-	}
-}
-
-static int mxr_g_fmt(struct file *file, void *priv,
-			     struct v4l2_format *f)
-{
-	struct mxr_layer *layer = video_drvdata(file);
-	struct v4l2_pix_format_mplane *pix = &f->fmt.pix_mp;
-
-	mxr_dbg(layer->mdev, "%s:%d\n", __func__, __LINE__);
-
-	pix->width = layer->geo.src.full_width;
-	pix->height = layer->geo.src.full_height;
-	pix->field = V4L2_FIELD_NONE;
-	pix->pixelformat = layer->fmt->fourcc;
-	pix->colorspace = layer->fmt->colorspace;
-	mxr_mplane_fill(pix->plane_fmt, layer->fmt, pix->width, pix->height);
-
-	return 0;
-}
-
-static int mxr_s_fmt(struct file *file, void *priv,
-	struct v4l2_format *f)
-{
-	struct mxr_layer *layer = video_drvdata(file);
-	const struct mxr_format *fmt;
-	struct v4l2_pix_format_mplane *pix;
-	struct mxr_device *mdev = layer->mdev;
-	struct mxr_geometry *geo = &layer->geo;
-
-	mxr_dbg(mdev, "%s:%d\n", __func__, __LINE__);
-
-	pix = &f->fmt.pix_mp;
-	fmt = find_format_by_fourcc(layer, pix->pixelformat);
-	if (fmt == NULL) {
-		mxr_warn(mdev, "not recognized fourcc: %08x\n",
-			pix->pixelformat);
-		return -EINVAL;
-	}
-	layer->fmt = fmt;
-	/* set source size to highest accepted value */
-	geo->src.full_width = max(geo->dst.full_width, pix->width);
-	geo->src.full_height = max(geo->dst.full_height, pix->height);
-	layer->ops.fix_geometry(layer, MXR_GEOMETRY_SOURCE, 0);
-	mxr_geometry_dump(mdev, &layer->geo);
-	/* set cropping to total visible screen */
-	geo->src.width = pix->width;
-	geo->src.height = pix->height;
-	geo->src.x_offset = 0;
-	geo->src.y_offset = 0;
-	/* assure consistency of geometry */
-	layer->ops.fix_geometry(layer, MXR_GEOMETRY_CROP, MXR_NO_OFFSET);
-	mxr_geometry_dump(mdev, &layer->geo);
-	/* set full size to lowest possible value */
-	geo->src.full_width = 0;
-	geo->src.full_height = 0;
-	layer->ops.fix_geometry(layer, MXR_GEOMETRY_SOURCE, 0);
-	mxr_geometry_dump(mdev, &layer->geo);
-
-	/* returning results */
-	mxr_g_fmt(file, priv, f);
-
-	return 0;
-}
-
-static int mxr_g_selection(struct file *file, void *fh,
-	struct v4l2_selection *s)
-{
-	struct mxr_layer *layer = video_drvdata(file);
-	struct mxr_geometry *geo = &layer->geo;
-
-	mxr_dbg(layer->mdev, "%s:%d\n", __func__, __LINE__);
-
-	if (s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT &&
-		s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
-		return -EINVAL;
-
-	switch (s->target) {
-	case V4L2_SEL_TGT_CROP:
-		s->r.left = geo->src.x_offset;
-		s->r.top = geo->src.y_offset;
-		s->r.width = geo->src.width;
-		s->r.height = geo->src.height;
-		break;
-	case V4L2_SEL_TGT_CROP_DEFAULT:
-	case V4L2_SEL_TGT_CROP_BOUNDS:
-		s->r.left = 0;
-		s->r.top = 0;
-		s->r.width = geo->src.full_width;
-		s->r.height = geo->src.full_height;
-		break;
-	case V4L2_SEL_TGT_COMPOSE:
-	case V4L2_SEL_TGT_COMPOSE_PADDED:
-		s->r.left = geo->dst.x_offset;
-		s->r.top = geo->dst.y_offset;
-		s->r.width = geo->dst.width;
-		s->r.height = geo->dst.height;
-		break;
-	case V4L2_SEL_TGT_COMPOSE_DEFAULT:
-	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
-		s->r.left = 0;
-		s->r.top = 0;
-		s->r.width = geo->dst.full_width;
-		s->r.height = geo->dst.full_height;
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	return 0;
-}
-
-/* returns 1 if rectangle 'a' is inside 'b' */
-static int mxr_is_rect_inside(struct v4l2_rect *a, struct v4l2_rect *b)
-{
-	if (a->left < b->left)
-		return 0;
-	if (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 mxr_s_selection(struct file *file, void *fh,
-	struct v4l2_selection *s)
-{
-	struct mxr_layer *layer = video_drvdata(file);
-	struct mxr_geometry *geo = &layer->geo;
-	struct mxr_crop *target = NULL;
-	enum mxr_geometry_stage stage;
-	struct mxr_geometry tmp;
-	struct v4l2_rect res;
-
-	memset(&res, 0, sizeof(res));
-
-	mxr_dbg(layer->mdev, "%s: rect: %dx%d@%d,%d\n", __func__,
-		s->r.width, s->r.height, s->r.left, s->r.top);
-
-	if (s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT &&
-		s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
-		return -EINVAL;
-
-	switch (s->target) {
-	/* ignore read-only targets */
-	case V4L2_SEL_TGT_CROP_DEFAULT:
-	case V4L2_SEL_TGT_CROP_BOUNDS:
-		res.width = geo->src.full_width;
-		res.height = geo->src.full_height;
-		break;
-
-	/* ignore read-only targets */
-	case V4L2_SEL_TGT_COMPOSE_DEFAULT:
-	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
-		res.width = geo->dst.full_width;
-		res.height = geo->dst.full_height;
-		break;
-
-	case V4L2_SEL_TGT_CROP:
-		target = &geo->src;
-		stage = MXR_GEOMETRY_CROP;
-		break;
-	case V4L2_SEL_TGT_COMPOSE:
-	case V4L2_SEL_TGT_COMPOSE_PADDED:
-		target = &geo->dst;
-		stage = MXR_GEOMETRY_COMPOSE;
-		break;
-	default:
-		return -EINVAL;
-	}
-	/* apply change and update geometry if needed */
-	if (target) {
-		/* backup current geometry if setup fails */
-		memcpy(&tmp, geo, sizeof(tmp));
-
-		/* apply requested selection */
-		target->x_offset = s->r.left;
-		target->y_offset = s->r.top;
-		target->width = s->r.width;
-		target->height = s->r.height;
-
-		layer->ops.fix_geometry(layer, stage, s->flags);
-
-		/* retrieve update selection rectangle */
-		res.left = target->x_offset;
-		res.top = target->y_offset;
-		res.width = target->width;
-		res.height = target->height;
-
-		mxr_geometry_dump(layer->mdev, &layer->geo);
-	}
-
-	/* checking if the rectangle satisfies constraints */
-	if ((s->flags & V4L2_SEL_FLAG_LE) && !mxr_is_rect_inside(&res, &s->r))
-		goto fail;
-	if ((s->flags & V4L2_SEL_FLAG_GE) && !mxr_is_rect_inside(&s->r, &res))
-		goto fail;
-
-	/* return result rectangle */
-	s->r = res;
-
-	return 0;
-fail:
-	/* restore old geometry, which is not touched if target is NULL */
-	if (target)
-		memcpy(geo, &tmp, sizeof(tmp));
-	return -ERANGE;
-}
-
-static int mxr_enum_dv_timings(struct file *file, void *fh,
-	struct v4l2_enum_dv_timings *timings)
-{
-	struct mxr_layer *layer = video_drvdata(file);
-	struct mxr_device *mdev = layer->mdev;
-	int ret;
-
-	timings->pad = 0;
-
-	/* lock protects from changing sd_out */
-	mutex_lock(&mdev->mutex);
-	ret = v4l2_subdev_call(to_outsd(mdev), pad, enum_dv_timings, timings);
-	mutex_unlock(&mdev->mutex);
-
-	return ret ? -EINVAL : 0;
-}
-
-static int mxr_s_dv_timings(struct file *file, void *fh,
-	struct v4l2_dv_timings *timings)
-{
-	struct mxr_layer *layer = video_drvdata(file);
-	struct mxr_device *mdev = layer->mdev;
-	int ret;
-
-	/* lock protects from changing sd_out */
-	mutex_lock(&mdev->mutex);
-
-	/* timings change cannot be done while there is an entity
-	 * dependent on output configuration
-	 */
-	if (mdev->n_output > 0) {
-		mutex_unlock(&mdev->mutex);
-		return -EBUSY;
-	}
-
-	ret = v4l2_subdev_call(to_outsd(mdev), video, s_dv_timings, timings);
-
-	mutex_unlock(&mdev->mutex);
-
-	mxr_layer_update_output(layer);
-
-	/* any failure should return EINVAL according to V4L2 doc */
-	return ret ? -EINVAL : 0;
-}
-
-static int mxr_g_dv_timings(struct file *file, void *fh,
-	struct v4l2_dv_timings *timings)
-{
-	struct mxr_layer *layer = video_drvdata(file);
-	struct mxr_device *mdev = layer->mdev;
-	int ret;
-
-	/* lock protects from changing sd_out */
-	mutex_lock(&mdev->mutex);
-	ret = v4l2_subdev_call(to_outsd(mdev), video, g_dv_timings, timings);
-	mutex_unlock(&mdev->mutex);
-
-	return ret ? -EINVAL : 0;
-}
-
-static int mxr_dv_timings_cap(struct file *file, void *fh,
-	struct v4l2_dv_timings_cap *cap)
-{
-	struct mxr_layer *layer = video_drvdata(file);
-	struct mxr_device *mdev = layer->mdev;
-	int ret;
-
-	cap->pad = 0;
-
-	/* lock protects from changing sd_out */
-	mutex_lock(&mdev->mutex);
-	ret = v4l2_subdev_call(to_outsd(mdev), pad, dv_timings_cap, cap);
-	mutex_unlock(&mdev->mutex);
-
-	return ret ? -EINVAL : 0;
-}
-
-static int mxr_s_std(struct file *file, void *fh, v4l2_std_id norm)
-{
-	struct mxr_layer *layer = video_drvdata(file);
-	struct mxr_device *mdev = layer->mdev;
-	int ret;
-
-	/* lock protects from changing sd_out */
-	mutex_lock(&mdev->mutex);
-
-	/* standard change cannot be done while there is an entity
-	 * dependent on output configuration
-	 */
-	if (mdev->n_output > 0) {
-		mutex_unlock(&mdev->mutex);
-		return -EBUSY;
-	}
-
-	ret = v4l2_subdev_call(to_outsd(mdev), video, s_std_output, norm);
-
-	mutex_unlock(&mdev->mutex);
-
-	mxr_layer_update_output(layer);
-
-	return ret ? -EINVAL : 0;
-}
-
-static int mxr_g_std(struct file *file, void *fh, v4l2_std_id *norm)
-{
-	struct mxr_layer *layer = video_drvdata(file);
-	struct mxr_device *mdev = layer->mdev;
-	int ret;
-
-	/* lock protects from changing sd_out */
-	mutex_lock(&mdev->mutex);
-	ret = v4l2_subdev_call(to_outsd(mdev), video, g_std_output, norm);
-	mutex_unlock(&mdev->mutex);
-
-	return ret ? -EINVAL : 0;
-}
-
-static int mxr_enum_output(struct file *file, void *fh, struct v4l2_output *a)
-{
-	struct mxr_layer *layer = video_drvdata(file);
-	struct mxr_device *mdev = layer->mdev;
-	struct mxr_output *out;
-	struct v4l2_subdev *sd;
-
-	if (a->index >= mdev->output_cnt)
-		return -EINVAL;
-	out = mdev->output[a->index];
-	BUG_ON(out == NULL);
-	sd = out->sd;
-	strlcpy(a->name, out->name, sizeof(a->name));
-
-	/* try to obtain supported tv norms */
-	v4l2_subdev_call(sd, video, g_tvnorms_output, &a->std);
-	a->capabilities = 0;
-	if (sd->ops->video && sd->ops->video->s_dv_timings)
-		a->capabilities |= V4L2_OUT_CAP_DV_TIMINGS;
-	if (sd->ops->video && sd->ops->video->s_std_output)
-		a->capabilities |= V4L2_OUT_CAP_STD;
-	a->type = V4L2_OUTPUT_TYPE_ANALOG;
-
-	return 0;
-}
-
-static int mxr_s_output(struct file *file, void *fh, unsigned int i)
-{
-	struct video_device *vfd = video_devdata(file);
-	struct mxr_layer *layer = video_drvdata(file);
-	struct mxr_device *mdev = layer->mdev;
-
-	if (i >= mdev->output_cnt || mdev->output[i] == NULL)
-		return -EINVAL;
-
-	mutex_lock(&mdev->mutex);
-	if (mdev->n_output > 0) {
-		mutex_unlock(&mdev->mutex);
-		return -EBUSY;
-	}
-	mdev->current_output = i;
-	vfd->tvnorms = 0;
-	v4l2_subdev_call(to_outsd(mdev), video, g_tvnorms_output,
-		&vfd->tvnorms);
-	mutex_unlock(&mdev->mutex);
-
-	/* update layers geometry */
-	mxr_layer_update_output(layer);
-
-	mxr_dbg(mdev, "tvnorms = %08llx\n", vfd->tvnorms);
-
-	return 0;
-}
-
-static int mxr_g_output(struct file *file, void *fh, unsigned int *p)
-{
-	struct mxr_layer *layer = video_drvdata(file);
-	struct mxr_device *mdev = layer->mdev;
-
-	mutex_lock(&mdev->mutex);
-	*p = mdev->current_output;
-	mutex_unlock(&mdev->mutex);
-
-	return 0;
-}
-
-static int mxr_reqbufs(struct file *file, void *priv,
-			  struct v4l2_requestbuffers *p)
-{
-	struct mxr_layer *layer = video_drvdata(file);
-
-	mxr_dbg(layer->mdev, "%s:%d\n", __func__, __LINE__);
-	return vb2_reqbufs(&layer->vb_queue, p);
-}
-
-static int mxr_querybuf(struct file *file, void *priv, struct v4l2_buffer *p)
-{
-	struct mxr_layer *layer = video_drvdata(file);
-
-	mxr_dbg(layer->mdev, "%s:%d\n", __func__, __LINE__);
-	return vb2_querybuf(&layer->vb_queue, p);
-}
-
-static int mxr_qbuf(struct file *file, void *priv, struct v4l2_buffer *p)
-{
-	struct mxr_layer *layer = video_drvdata(file);
-
-	mxr_dbg(layer->mdev, "%s:%d(%d)\n", __func__, __LINE__, p->index);
-	return vb2_qbuf(&layer->vb_queue, p);
-}
-
-static int mxr_dqbuf(struct file *file, void *priv, struct v4l2_buffer *p)
-{
-	struct mxr_layer *layer = video_drvdata(file);
-
-	mxr_dbg(layer->mdev, "%s:%d\n", __func__, __LINE__);
-	return vb2_dqbuf(&layer->vb_queue, p, file->f_flags & O_NONBLOCK);
-}
-
-static int mxr_expbuf(struct file *file, void *priv,
-	struct v4l2_exportbuffer *eb)
-{
-	struct mxr_layer *layer = video_drvdata(file);
-
-	mxr_dbg(layer->mdev, "%s:%d\n", __func__, __LINE__);
-	return vb2_expbuf(&layer->vb_queue, eb);
-}
-
-static int mxr_streamon(struct file *file, void *priv, enum v4l2_buf_type i)
-{
-	struct mxr_layer *layer = video_drvdata(file);
-
-	mxr_dbg(layer->mdev, "%s:%d\n", __func__, __LINE__);
-	return vb2_streamon(&layer->vb_queue, i);
-}
-
-static int mxr_streamoff(struct file *file, void *priv, enum v4l2_buf_type i)
-{
-	struct mxr_layer *layer = video_drvdata(file);
-
-	mxr_dbg(layer->mdev, "%s:%d\n", __func__, __LINE__);
-	return vb2_streamoff(&layer->vb_queue, i);
-}
-
-static const struct v4l2_ioctl_ops mxr_ioctl_ops = {
-	.vidioc_querycap = mxr_querycap,
-	/* format handling */
-	.vidioc_enum_fmt_vid_out_mplane = mxr_enum_fmt,
-	.vidioc_s_fmt_vid_out_mplane = mxr_s_fmt,
-	.vidioc_g_fmt_vid_out_mplane = mxr_g_fmt,
-	/* buffer control */
-	.vidioc_reqbufs = mxr_reqbufs,
-	.vidioc_querybuf = mxr_querybuf,
-	.vidioc_qbuf = mxr_qbuf,
-	.vidioc_dqbuf = mxr_dqbuf,
-	.vidioc_expbuf = mxr_expbuf,
-	/* Streaming control */
-	.vidioc_streamon = mxr_streamon,
-	.vidioc_streamoff = mxr_streamoff,
-	/* DV Timings functions */
-	.vidioc_enum_dv_timings = mxr_enum_dv_timings,
-	.vidioc_s_dv_timings = mxr_s_dv_timings,
-	.vidioc_g_dv_timings = mxr_g_dv_timings,
-	.vidioc_dv_timings_cap = mxr_dv_timings_cap,
-	/* analog TV standard functions */
-	.vidioc_s_std = mxr_s_std,
-	.vidioc_g_std = mxr_g_std,
-	/* Output handling */
-	.vidioc_enum_output = mxr_enum_output,
-	.vidioc_s_output = mxr_s_output,
-	.vidioc_g_output = mxr_g_output,
-	/* selection ioctls */
-	.vidioc_g_selection = mxr_g_selection,
-	.vidioc_s_selection = mxr_s_selection,
-};
-
-static int mxr_video_open(struct file *file)
-{
-	struct mxr_layer *layer = video_drvdata(file);
-	struct mxr_device *mdev = layer->mdev;
-	int ret = 0;
-
-	mxr_dbg(mdev, "%s:%d\n", __func__, __LINE__);
-	if (mutex_lock_interruptible(&layer->mutex))
-		return -ERESTARTSYS;
-	/* assure device probe is finished */
-	wait_for_device_probe();
-	/* creating context for file descriptor */
-	ret = v4l2_fh_open(file);
-	if (ret) {
-		mxr_err(mdev, "v4l2_fh_open failed\n");
-		goto unlock;
-	}
-
-	/* leaving if layer is already initialized */
-	if (!v4l2_fh_is_singular_file(file))
-		goto unlock;
-
-	/* FIXME: should power be enabled on open? */
-	ret = mxr_power_get(mdev);
-	if (ret) {
-		mxr_err(mdev, "power on failed\n");
-		goto fail_fh_open;
-	}
-
-	ret = vb2_queue_init(&layer->vb_queue);
-	if (ret != 0) {
-		mxr_err(mdev, "failed to initialize vb2 queue\n");
-		goto fail_power;
-	}
-	/* set default format, first on the list */
-	layer->fmt = layer->fmt_array[0];
-	/* setup default geometry */
-	mxr_layer_default_geo(layer);
-	mutex_unlock(&layer->mutex);
-
-	return 0;
-
-fail_power:
-	mxr_power_put(mdev);
-
-fail_fh_open:
-	v4l2_fh_release(file);
-
-unlock:
-	mutex_unlock(&layer->mutex);
-
-	return ret;
-}
-
-static unsigned int
-mxr_video_poll(struct file *file, struct poll_table_struct *wait)
-{
-	struct mxr_layer *layer = video_drvdata(file);
-	unsigned int res;
-
-	mxr_dbg(layer->mdev, "%s:%d\n", __func__, __LINE__);
-
-	mutex_lock(&layer->mutex);
-	res = vb2_poll(&layer->vb_queue, file, wait);
-	mutex_unlock(&layer->mutex);
-	return res;
-}
-
-static int mxr_video_mmap(struct file *file, struct vm_area_struct *vma)
-{
-	struct mxr_layer *layer = video_drvdata(file);
-	int ret;
-
-	mxr_dbg(layer->mdev, "%s:%d\n", __func__, __LINE__);
-
-	if (mutex_lock_interruptible(&layer->mutex))
-		return -ERESTARTSYS;
-	ret = vb2_mmap(&layer->vb_queue, vma);
-	mutex_unlock(&layer->mutex);
-	return ret;
-}
-
-static int mxr_video_release(struct file *file)
-{
-	struct mxr_layer *layer = video_drvdata(file);
-
-	mxr_dbg(layer->mdev, "%s:%d\n", __func__, __LINE__);
-	mutex_lock(&layer->mutex);
-	if (v4l2_fh_is_singular_file(file)) {
-		vb2_queue_release(&layer->vb_queue);
-		mxr_power_put(layer->mdev);
-	}
-	v4l2_fh_release(file);
-	mutex_unlock(&layer->mutex);
-	return 0;
-}
-
-static const struct v4l2_file_operations mxr_fops = {
-	.owner = THIS_MODULE,
-	.open = mxr_video_open,
-	.poll = mxr_video_poll,
-	.mmap = mxr_video_mmap,
-	.release = mxr_video_release,
-	.unlocked_ioctl = video_ioctl2,
-};
-
-static int queue_setup(struct vb2_queue *vq,
-	unsigned int *nbuffers, unsigned int *nplanes, unsigned int sizes[],
-	struct device *alloc_devs[])
-{
-	struct mxr_layer *layer = vb2_get_drv_priv(vq);
-	const struct mxr_format *fmt = layer->fmt;
-	int i;
-	struct mxr_device *mdev = layer->mdev;
-	struct v4l2_plane_pix_format planes[3];
-
-	mxr_dbg(mdev, "%s\n", __func__);
-	/* checking if format was configured */
-	if (fmt == NULL)
-		return -EINVAL;
-	mxr_dbg(mdev, "fmt = %s\n", fmt->name);
-	mxr_mplane_fill(planes, fmt, layer->geo.src.full_width,
-		layer->geo.src.full_height);
-
-	*nplanes = fmt->num_subframes;
-	for (i = 0; i < fmt->num_subframes; ++i) {
-		sizes[i] = planes[i].sizeimage;
-		mxr_dbg(mdev, "size[%d] = %08x\n", i, sizes[i]);
-	}
-
-	if (*nbuffers == 0)
-		*nbuffers = 1;
-
-	return 0;
-}
-
-static void buf_queue(struct vb2_buffer *vb)
-{
-	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
-	struct mxr_buffer *buffer = container_of(vbuf, struct mxr_buffer, vb);
-	struct mxr_layer *layer = vb2_get_drv_priv(vb->vb2_queue);
-	struct mxr_device *mdev = layer->mdev;
-	unsigned long flags;
-
-	spin_lock_irqsave(&layer->enq_slock, flags);
-	list_add_tail(&buffer->list, &layer->enq_list);
-	spin_unlock_irqrestore(&layer->enq_slock, flags);
-
-	mxr_dbg(mdev, "queuing buffer\n");
-}
-
-static int start_streaming(struct vb2_queue *vq, unsigned int count)
-{
-	struct mxr_layer *layer = vb2_get_drv_priv(vq);
-	struct mxr_device *mdev = layer->mdev;
-	unsigned long flags;
-
-	mxr_dbg(mdev, "%s\n", __func__);
-
-	/* block any changes in output configuration */
-	mxr_output_get(mdev);
-
-	mxr_layer_update_output(layer);
-	layer->ops.format_set(layer);
-	/* enabling layer in hardware */
-	spin_lock_irqsave(&layer->enq_slock, flags);
-	layer->state = MXR_LAYER_STREAMING;
-	spin_unlock_irqrestore(&layer->enq_slock, flags);
-
-	layer->ops.stream_set(layer, MXR_ENABLE);
-	mxr_streamer_get(mdev);
-
-	return 0;
-}
-
-static void mxr_watchdog(unsigned long arg)
-{
-	struct mxr_layer *layer = (struct mxr_layer *) arg;
-	struct mxr_device *mdev = layer->mdev;
-	unsigned long flags;
-
-	mxr_err(mdev, "watchdog fired for layer %s\n", layer->vfd.name);
-
-	spin_lock_irqsave(&layer->enq_slock, flags);
-
-	if (layer->update_buf == layer->shadow_buf)
-		layer->update_buf = NULL;
-	if (layer->update_buf) {
-		vb2_buffer_done(&layer->update_buf->vb.vb2_buf,
-				VB2_BUF_STATE_ERROR);
-		layer->update_buf = NULL;
-	}
-	if (layer->shadow_buf) {
-		vb2_buffer_done(&layer->shadow_buf->vb.vb2_buf,
-				VB2_BUF_STATE_ERROR);
-		layer->shadow_buf = NULL;
-	}
-	spin_unlock_irqrestore(&layer->enq_slock, flags);
-}
-
-static void stop_streaming(struct vb2_queue *vq)
-{
-	struct mxr_layer *layer = vb2_get_drv_priv(vq);
-	struct mxr_device *mdev = layer->mdev;
-	unsigned long flags;
-	struct timer_list watchdog;
-	struct mxr_buffer *buf, *buf_tmp;
-
-	mxr_dbg(mdev, "%s\n", __func__);
-
-	spin_lock_irqsave(&layer->enq_slock, flags);
-
-	/* reset list */
-	layer->state = MXR_LAYER_STREAMING_FINISH;
-
-	/* set all buffer to be done */
-	list_for_each_entry_safe(buf, buf_tmp, &layer->enq_list, list) {
-		list_del(&buf->list);
-		vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
-	}
-
-	spin_unlock_irqrestore(&layer->enq_slock, flags);
-
-	/* give 1 seconds to complete to complete last buffers */
-	setup_timer_on_stack(&watchdog, mxr_watchdog,
-		(unsigned long)layer);
-	mod_timer(&watchdog, jiffies + msecs_to_jiffies(1000));
-
-	/* wait until all buffers are goes to done state */
-	vb2_wait_for_all_buffers(vq);
-
-	/* stop timer if all synchronization is done */
-	del_timer_sync(&watchdog);
-	destroy_timer_on_stack(&watchdog);
-
-	/* stopping hardware */
-	spin_lock_irqsave(&layer->enq_slock, flags);
-	layer->state = MXR_LAYER_IDLE;
-	spin_unlock_irqrestore(&layer->enq_slock, flags);
-
-	/* disabling layer in hardware */
-	layer->ops.stream_set(layer, MXR_DISABLE);
-	/* remove one streamer */
-	mxr_streamer_put(mdev);
-	/* allow changes in output configuration */
-	mxr_output_put(mdev);
-}
-
-static struct vb2_ops mxr_video_qops = {
-	.queue_setup = queue_setup,
-	.buf_queue = buf_queue,
-	.wait_prepare = vb2_ops_wait_prepare,
-	.wait_finish = vb2_ops_wait_finish,
-	.start_streaming = start_streaming,
-	.stop_streaming = stop_streaming,
-};
-
-/* FIXME: try to put this functions to mxr_base_layer_create */
-int mxr_base_layer_register(struct mxr_layer *layer)
-{
-	struct mxr_device *mdev = layer->mdev;
-	int ret;
-
-	ret = video_register_device(&layer->vfd, VFL_TYPE_GRABBER, -1);
-	if (ret)
-		mxr_err(mdev, "failed to register video device\n");
-	else
-		mxr_info(mdev, "registered layer %s as /dev/video%d\n",
-			layer->vfd.name, layer->vfd.num);
-	return ret;
-}
-
-void mxr_base_layer_unregister(struct mxr_layer *layer)
-{
-	video_unregister_device(&layer->vfd);
-}
-
-void mxr_layer_release(struct mxr_layer *layer)
-{
-	if (layer->ops.release)
-		layer->ops.release(layer);
-}
-
-void mxr_base_layer_release(struct mxr_layer *layer)
-{
-	kfree(layer);
-}
-
-static void mxr_vfd_release(struct video_device *vdev)
-{
-	pr_info("video device release\n");
-}
-
-struct mxr_layer *mxr_base_layer_create(struct mxr_device *mdev,
-	int idx, char *name, const struct mxr_layer_ops *ops)
-{
-	struct mxr_layer *layer;
-
-	layer = kzalloc(sizeof(*layer), GFP_KERNEL);
-	if (layer == NULL) {
-		mxr_err(mdev, "not enough memory for layer.\n");
-		goto fail;
-	}
-
-	layer->mdev = mdev;
-	layer->idx = idx;
-	layer->ops = *ops;
-
-	spin_lock_init(&layer->enq_slock);
-	INIT_LIST_HEAD(&layer->enq_list);
-	mutex_init(&layer->mutex);
-
-	layer->vfd = (struct video_device) {
-		.minor = -1,
-		.release = mxr_vfd_release,
-		.fops = &mxr_fops,
-		.vfl_dir = VFL_DIR_TX,
-		.ioctl_ops = &mxr_ioctl_ops,
-	};
-	strlcpy(layer->vfd.name, name, sizeof(layer->vfd.name));
-
-	video_set_drvdata(&layer->vfd, layer);
-	layer->vfd.lock = &layer->mutex;
-	layer->vfd.v4l2_dev = &mdev->v4l2_dev;
-
-	layer->vb_queue = (struct vb2_queue) {
-		.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE,
-		.io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF,
-		.drv_priv = layer,
-		.buf_struct_size = sizeof(struct mxr_buffer),
-		.ops = &mxr_video_qops,
-		.min_buffers_needed = 1,
-		.mem_ops = &vb2_dma_contig_memops,
-		.lock = &layer->mutex,
-		.dev = mdev->dev,
-	};
-
-	return layer;
-
-fail:
-	return NULL;
-}
-
-static const struct mxr_format *find_format_by_fourcc(
-	struct mxr_layer *layer, unsigned long fourcc)
-{
-	int i;
-
-	for (i = 0; i < layer->fmt_array_size; ++i)
-		if (layer->fmt_array[i]->fourcc == fourcc)
-			return layer->fmt_array[i];
-	return NULL;
-}
-
-static const struct mxr_format *find_format_by_index(
-	struct mxr_layer *layer, unsigned long index)
-{
-	if (index >= layer->fmt_array_size)
-		return NULL;
-	return layer->fmt_array[index];
-}
-
diff --git a/drivers/media/platform/s5p-tv/mixer_vp_layer.c b/drivers/media/platform/s5p-tv/mixer_vp_layer.c
deleted file mode 100644
index 6fa6f673f53b..000000000000
--- a/drivers/media/platform/s5p-tv/mixer_vp_layer.c
+++ /dev/null
@@ -1,242 +0,0 @@
-/*
- * Samsung TV Mixer driver
- *
- * Copyright (c) 2010-2011 Samsung Electronics Co., Ltd.
- *
- * Tomasz Stanislawski, <t.stanislaws@samsung.com>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published
- * by the Free Software Foundiation. either version 2 of the License,
- * or (at your option) any later version
- */
-
-#include "mixer.h"
-
-#include "regs-vp.h"
-
-#include <media/videobuf2-dma-contig.h>
-
-/* FORMAT DEFINITIONS */
-static const struct mxr_format mxr_fmt_nv12 = {
-	.name = "NV12",
-	.fourcc = V4L2_PIX_FMT_NV12,
-	.colorspace = V4L2_COLORSPACE_JPEG,
-	.num_planes = 2,
-	.plane = {
-		{ .width = 1, .height = 1, .size = 1 },
-		{ .width = 2, .height = 2, .size = 2 },
-	},
-	.num_subframes = 1,
-	.cookie = VP_MODE_NV12 | VP_MODE_MEM_LINEAR,
-};
-
-static const struct mxr_format mxr_fmt_nv21 = {
-	.name = "NV21",
-	.fourcc = V4L2_PIX_FMT_NV21,
-	.colorspace = V4L2_COLORSPACE_JPEG,
-	.num_planes = 2,
-	.plane = {
-		{ .width = 1, .height = 1, .size = 1 },
-		{ .width = 2, .height = 2, .size = 2 },
-	},
-	.num_subframes = 1,
-	.cookie = VP_MODE_NV21 | VP_MODE_MEM_LINEAR,
-};
-
-static const struct mxr_format mxr_fmt_nv12m = {
-	.name = "NV12 (mplane)",
-	.fourcc = V4L2_PIX_FMT_NV12M,
-	.colorspace = V4L2_COLORSPACE_JPEG,
-	.num_planes = 2,
-	.plane = {
-		{ .width = 1, .height = 1, .size = 1 },
-		{ .width = 2, .height = 2, .size = 2 },
-	},
-	.num_subframes = 2,
-	.plane2subframe = {0, 1},
-	.cookie = VP_MODE_NV12 | VP_MODE_MEM_LINEAR,
-};
-
-static const struct mxr_format mxr_fmt_nv12mt = {
-	.name = "NV12 tiled (mplane)",
-	.fourcc = V4L2_PIX_FMT_NV12MT,
-	.colorspace = V4L2_COLORSPACE_JPEG,
-	.num_planes = 2,
-	.plane = {
-		{ .width = 128, .height = 32, .size = 4096 },
-		{ .width = 128, .height = 32, .size = 2048 },
-	},
-	.num_subframes = 2,
-	.plane2subframe = {0, 1},
-	.cookie = VP_MODE_NV12 | VP_MODE_MEM_TILED,
-};
-
-static const struct mxr_format *mxr_video_format[] = {
-	&mxr_fmt_nv12,
-	&mxr_fmt_nv21,
-	&mxr_fmt_nv12m,
-	&mxr_fmt_nv12mt,
-};
-
-/* AUXILIARY CALLBACKS */
-
-static void mxr_vp_layer_release(struct mxr_layer *layer)
-{
-	mxr_base_layer_unregister(layer);
-	mxr_base_layer_release(layer);
-}
-
-static void mxr_vp_buffer_set(struct mxr_layer *layer,
-	struct mxr_buffer *buf)
-{
-	dma_addr_t luma_addr[2] = {0, 0};
-	dma_addr_t chroma_addr[2] = {0, 0};
-
-	if (buf == NULL) {
-		mxr_reg_vp_buffer(layer->mdev, luma_addr, chroma_addr);
-		return;
-	}
-	luma_addr[0] = vb2_dma_contig_plane_dma_addr(&buf->vb.vb2_buf, 0);
-	if (layer->fmt->num_subframes == 2) {
-		chroma_addr[0] =
-			vb2_dma_contig_plane_dma_addr(&buf->vb.vb2_buf, 1);
-	} else {
-		/* FIXME: mxr_get_plane_size compute integer division,
-		 * which is slow and should not be performed in interrupt */
-		chroma_addr[0] = luma_addr[0] + mxr_get_plane_size(
-			&layer->fmt->plane[0], layer->geo.src.full_width,
-			layer->geo.src.full_height);
-	}
-	if (layer->fmt->cookie & VP_MODE_MEM_TILED) {
-		luma_addr[1] = luma_addr[0] + 0x40;
-		chroma_addr[1] = chroma_addr[0] + 0x40;
-	} else {
-		luma_addr[1] = luma_addr[0] + layer->geo.src.full_width;
-		chroma_addr[1] = chroma_addr[0];
-	}
-	mxr_reg_vp_buffer(layer->mdev, luma_addr, chroma_addr);
-}
-
-static void mxr_vp_stream_set(struct mxr_layer *layer, int en)
-{
-	mxr_reg_vp_layer_stream(layer->mdev, en);
-}
-
-static void mxr_vp_format_set(struct mxr_layer *layer)
-{
-	mxr_reg_vp_format(layer->mdev, layer->fmt, &layer->geo);
-}
-
-static inline unsigned int do_center(unsigned int center,
-	unsigned int size, unsigned int upper, unsigned int flags)
-{
-	unsigned int lower;
-
-	if (flags & MXR_NO_OFFSET)
-		return 0;
-
-	lower = center - min(center, size / 2);
-	return min(lower, upper - size);
-}
-
-static void mxr_vp_fix_geometry(struct mxr_layer *layer,
-	enum mxr_geometry_stage stage, unsigned long flags)
-{
-	struct mxr_geometry *geo = &layer->geo;
-	struct mxr_crop *src = &geo->src;
-	struct mxr_crop *dst = &geo->dst;
-	unsigned long x_center, y_center;
-
-	switch (stage) {
-
-	case MXR_GEOMETRY_SINK: /* nothing to be fixed here */
-	case MXR_GEOMETRY_COMPOSE:
-		/* remember center of the area */
-		x_center = dst->x_offset + dst->width / 2;
-		y_center = dst->y_offset + dst->height / 2;
-
-		/* ensure that compose is reachable using 16x scaling */
-		dst->width = clamp(dst->width, 8U, 16 * src->full_width);
-		dst->height = clamp(dst->height, 1U, 16 * src->full_height);
-
-		/* setup offsets */
-		dst->x_offset = do_center(x_center, dst->width,
-			dst->full_width, flags);
-		dst->y_offset = do_center(y_center, dst->height,
-			dst->full_height, flags);
-		flags = 0; /* remove possible MXR_NO_OFFSET flag */
-		/* fall through */
-	case MXR_GEOMETRY_CROP:
-		/* remember center of the area */
-		x_center = src->x_offset + src->width / 2;
-		y_center = src->y_offset + src->height / 2;
-
-		/* ensure scaling is between 0.25x .. 16x */
-		src->width = clamp(src->width, round_up(dst->width / 16, 4),
-			dst->width * 4);
-		src->height = clamp(src->height, round_up(dst->height / 16, 4),
-			dst->height * 4);
-
-		/* hardware limits */
-		src->width = clamp(src->width, 32U, 2047U);
-		src->height = clamp(src->height, 4U, 2047U);
-
-		/* setup offsets */
-		src->x_offset = do_center(x_center, src->width,
-			src->full_width, flags);
-		src->y_offset = do_center(y_center, src->height,
-			src->full_height, flags);
-
-		/* setting scaling ratio */
-		geo->x_ratio = (src->width << 16) / dst->width;
-		geo->y_ratio = (src->height << 16) / dst->height;
-		/* fall through */
-
-	case MXR_GEOMETRY_SOURCE:
-		src->full_width = clamp(src->full_width,
-			ALIGN(src->width + src->x_offset, 8), 8192U);
-		src->full_height = clamp(src->full_height,
-			src->height + src->y_offset, 8192U);
-	}
-}
-
-/* PUBLIC API */
-
-struct mxr_layer *mxr_vp_layer_create(struct mxr_device *mdev, int idx)
-{
-	struct mxr_layer *layer;
-	int ret;
-	const struct mxr_layer_ops ops = {
-		.release = mxr_vp_layer_release,
-		.buffer_set = mxr_vp_buffer_set,
-		.stream_set = mxr_vp_stream_set,
-		.format_set = mxr_vp_format_set,
-		.fix_geometry = mxr_vp_fix_geometry,
-	};
-	char name[32];
-
-	sprintf(name, "video%d", idx);
-
-	layer = mxr_base_layer_create(mdev, idx, name, &ops);
-	if (layer == NULL) {
-		mxr_err(mdev, "failed to initialize layer(%d) base\n", idx);
-		goto fail;
-	}
-
-	layer->fmt_array = mxr_video_format;
-	layer->fmt_array_size = ARRAY_SIZE(mxr_video_format);
-
-	ret = mxr_base_layer_register(layer);
-	if (ret)
-		goto fail_layer;
-
-	return layer;
-
-fail_layer:
-	mxr_base_layer_release(layer);
-
-fail:
-	return NULL;
-}
-
diff --git a/drivers/media/platform/s5p-tv/regs-hdmi.h b/drivers/media/platform/s5p-tv/regs-hdmi.h
deleted file mode 100644
index a889d1f57f28..000000000000
--- a/drivers/media/platform/s5p-tv/regs-hdmi.h
+++ /dev/null
@@ -1,146 +0,0 @@
-/* linux/arch/arm/mach-exynos4/include/mach/regs-hdmi.h
- *
- * Copyright (c) 2010-2011 Samsung Electronics Co., Ltd.
- * http://www.samsung.com/
- *
- * HDMI register header file for Samsung TVOUT driver
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
-*/
-
-#ifndef SAMSUNG_REGS_HDMI_H
-#define SAMSUNG_REGS_HDMI_H
-
-/*
- * Register part
-*/
-
-#define HDMI_CTRL_BASE(x)		((x) + 0x00000000)
-#define HDMI_CORE_BASE(x)		((x) + 0x00010000)
-#define HDMI_TG_BASE(x)			((x) + 0x00050000)
-
-/* Control registers */
-#define HDMI_INTC_CON			HDMI_CTRL_BASE(0x0000)
-#define HDMI_INTC_FLAG			HDMI_CTRL_BASE(0x0004)
-#define HDMI_HPD_STATUS			HDMI_CTRL_BASE(0x000C)
-#define HDMI_PHY_RSTOUT			HDMI_CTRL_BASE(0x0014)
-#define HDMI_PHY_VPLL			HDMI_CTRL_BASE(0x0018)
-#define HDMI_PHY_CMU			HDMI_CTRL_BASE(0x001C)
-#define HDMI_CORE_RSTOUT		HDMI_CTRL_BASE(0x0020)
-
-/* Core registers */
-#define HDMI_CON_0			HDMI_CORE_BASE(0x0000)
-#define HDMI_CON_1			HDMI_CORE_BASE(0x0004)
-#define HDMI_CON_2			HDMI_CORE_BASE(0x0008)
-#define HDMI_SYS_STATUS			HDMI_CORE_BASE(0x0010)
-#define HDMI_PHY_STATUS			HDMI_CORE_BASE(0x0014)
-#define HDMI_STATUS_EN			HDMI_CORE_BASE(0x0020)
-#define HDMI_HPD			HDMI_CORE_BASE(0x0030)
-#define HDMI_MODE_SEL			HDMI_CORE_BASE(0x0040)
-#define HDMI_BLUE_SCREEN_0		HDMI_CORE_BASE(0x0050)
-#define HDMI_BLUE_SCREEN_1		HDMI_CORE_BASE(0x0054)
-#define HDMI_BLUE_SCREEN_2		HDMI_CORE_BASE(0x0058)
-#define HDMI_H_BLANK_0			HDMI_CORE_BASE(0x00A0)
-#define HDMI_H_BLANK_1			HDMI_CORE_BASE(0x00A4)
-#define HDMI_V_BLANK_0			HDMI_CORE_BASE(0x00B0)
-#define HDMI_V_BLANK_1			HDMI_CORE_BASE(0x00B4)
-#define HDMI_V_BLANK_2			HDMI_CORE_BASE(0x00B8)
-#define HDMI_H_V_LINE_0			HDMI_CORE_BASE(0x00C0)
-#define HDMI_H_V_LINE_1			HDMI_CORE_BASE(0x00C4)
-#define HDMI_H_V_LINE_2			HDMI_CORE_BASE(0x00C8)
-#define HDMI_VSYNC_POL			HDMI_CORE_BASE(0x00E4)
-#define HDMI_INT_PRO_MODE		HDMI_CORE_BASE(0x00E8)
-#define HDMI_V_BLANK_F_0		HDMI_CORE_BASE(0x0110)
-#define HDMI_V_BLANK_F_1		HDMI_CORE_BASE(0x0114)
-#define HDMI_V_BLANK_F_2		HDMI_CORE_BASE(0x0118)
-#define HDMI_H_SYNC_GEN_0		HDMI_CORE_BASE(0x0120)
-#define HDMI_H_SYNC_GEN_1		HDMI_CORE_BASE(0x0124)
-#define HDMI_H_SYNC_GEN_2		HDMI_CORE_BASE(0x0128)
-#define HDMI_V_SYNC_GEN_1_0		HDMI_CORE_BASE(0x0130)
-#define HDMI_V_SYNC_GEN_1_1		HDMI_CORE_BASE(0x0134)
-#define HDMI_V_SYNC_GEN_1_2		HDMI_CORE_BASE(0x0138)
-#define HDMI_V_SYNC_GEN_2_0		HDMI_CORE_BASE(0x0140)
-#define HDMI_V_SYNC_GEN_2_1		HDMI_CORE_BASE(0x0144)
-#define HDMI_V_SYNC_GEN_2_2		HDMI_CORE_BASE(0x0148)
-#define HDMI_V_SYNC_GEN_3_0		HDMI_CORE_BASE(0x0150)
-#define HDMI_V_SYNC_GEN_3_1		HDMI_CORE_BASE(0x0154)
-#define HDMI_V_SYNC_GEN_3_2		HDMI_CORE_BASE(0x0158)
-#define HDMI_AVI_CON			HDMI_CORE_BASE(0x0300)
-#define HDMI_AVI_BYTE(n)		HDMI_CORE_BASE(0x0320 + 4 * (n))
-#define	HDMI_DC_CONTROL			HDMI_CORE_BASE(0x05C0)
-#define HDMI_VIDEO_PATTERN_GEN		HDMI_CORE_BASE(0x05C4)
-#define HDMI_HPD_GEN			HDMI_CORE_BASE(0x05C8)
-
-/* Timing generator registers */
-#define HDMI_TG_CMD			HDMI_TG_BASE(0x0000)
-#define HDMI_TG_H_FSZ_L			HDMI_TG_BASE(0x0018)
-#define HDMI_TG_H_FSZ_H			HDMI_TG_BASE(0x001C)
-#define HDMI_TG_HACT_ST_L		HDMI_TG_BASE(0x0020)
-#define HDMI_TG_HACT_ST_H		HDMI_TG_BASE(0x0024)
-#define HDMI_TG_HACT_SZ_L		HDMI_TG_BASE(0x0028)
-#define HDMI_TG_HACT_SZ_H		HDMI_TG_BASE(0x002C)
-#define HDMI_TG_V_FSZ_L			HDMI_TG_BASE(0x0030)
-#define HDMI_TG_V_FSZ_H			HDMI_TG_BASE(0x0034)
-#define HDMI_TG_VSYNC_L			HDMI_TG_BASE(0x0038)
-#define HDMI_TG_VSYNC_H			HDMI_TG_BASE(0x003C)
-#define HDMI_TG_VSYNC2_L		HDMI_TG_BASE(0x0040)
-#define HDMI_TG_VSYNC2_H		HDMI_TG_BASE(0x0044)
-#define HDMI_TG_VACT_ST_L		HDMI_TG_BASE(0x0048)
-#define HDMI_TG_VACT_ST_H		HDMI_TG_BASE(0x004C)
-#define HDMI_TG_VACT_SZ_L		HDMI_TG_BASE(0x0050)
-#define HDMI_TG_VACT_SZ_H		HDMI_TG_BASE(0x0054)
-#define HDMI_TG_FIELD_CHG_L		HDMI_TG_BASE(0x0058)
-#define HDMI_TG_FIELD_CHG_H		HDMI_TG_BASE(0x005C)
-#define HDMI_TG_VACT_ST2_L		HDMI_TG_BASE(0x0060)
-#define HDMI_TG_VACT_ST2_H		HDMI_TG_BASE(0x0064)
-#define HDMI_TG_VSYNC_TOP_HDMI_L	HDMI_TG_BASE(0x0078)
-#define HDMI_TG_VSYNC_TOP_HDMI_H	HDMI_TG_BASE(0x007C)
-#define HDMI_TG_VSYNC_BOT_HDMI_L	HDMI_TG_BASE(0x0080)
-#define HDMI_TG_VSYNC_BOT_HDMI_H	HDMI_TG_BASE(0x0084)
-#define HDMI_TG_FIELD_TOP_HDMI_L	HDMI_TG_BASE(0x0088)
-#define HDMI_TG_FIELD_TOP_HDMI_H	HDMI_TG_BASE(0x008C)
-#define HDMI_TG_FIELD_BOT_HDMI_L	HDMI_TG_BASE(0x0090)
-#define HDMI_TG_FIELD_BOT_HDMI_H	HDMI_TG_BASE(0x0094)
-
-/*
- * Bit definition part
- */
-
-/* HDMI_INTC_CON */
-#define HDMI_INTC_EN_GLOBAL		(1 << 6)
-#define HDMI_INTC_EN_HPD_PLUG		(1 << 3)
-#define HDMI_INTC_EN_HPD_UNPLUG		(1 << 2)
-
-/* HDMI_INTC_FLAG */
-#define HDMI_INTC_FLAG_HPD_PLUG		(1 << 3)
-#define HDMI_INTC_FLAG_HPD_UNPLUG	(1 << 2)
-
-/* HDMI_PHY_RSTOUT */
-#define HDMI_PHY_SW_RSTOUT		(1 << 0)
-
-/* HDMI_CORE_RSTOUT */
-#define HDMI_CORE_SW_RSTOUT		(1 << 0)
-
-/* HDMI_CON_0 */
-#define HDMI_BLUE_SCR_EN		(1 << 5)
-#define HDMI_EN				(1 << 0)
-
-/* HDMI_CON_2 */
-#define HDMI_DVI_PERAMBLE_EN		(1 << 5)
-#define HDMI_DVI_BAND_EN		(1 << 1)
-
-/* HDMI_PHY_STATUS */
-#define HDMI_PHY_STATUS_READY		(1 << 0)
-
-/* HDMI_MODE_SEL */
-#define HDMI_MODE_HDMI_EN		(1 << 1)
-#define HDMI_MODE_DVI_EN		(1 << 0)
-#define HDMI_MODE_MASK			(3 << 0)
-
-/* HDMI_TG_CMD */
-#define HDMI_TG_FIELD_EN		(1 << 1)
-#define HDMI_TG_EN			(1 << 0)
-
-#endif /* SAMSUNG_REGS_HDMI_H */
diff --git a/drivers/media/platform/s5p-tv/regs-mixer.h b/drivers/media/platform/s5p-tv/regs-mixer.h
deleted file mode 100644
index 158abb43d0a4..000000000000
--- a/drivers/media/platform/s5p-tv/regs-mixer.h
+++ /dev/null
@@ -1,122 +0,0 @@
-/*
- * Copyright (c) 2010-2011 Samsung Electronics Co., Ltd.
- * http://www.samsung.com/
- *
- * Mixer register header file for Samsung Mixer driver
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
-*/
-#ifndef SAMSUNG_REGS_MIXER_H
-#define SAMSUNG_REGS_MIXER_H
-
-/*
- * Register part
- */
-#define MXR_STATUS			0x0000
-#define MXR_CFG				0x0004
-#define MXR_INT_EN			0x0008
-#define MXR_INT_STATUS			0x000C
-#define MXR_LAYER_CFG			0x0010
-#define MXR_VIDEO_CFG			0x0014
-#define MXR_GRAPHIC0_CFG		0x0020
-#define MXR_GRAPHIC0_BASE		0x0024
-#define MXR_GRAPHIC0_SPAN		0x0028
-#define MXR_GRAPHIC0_SXY		0x002C
-#define MXR_GRAPHIC0_WH			0x0030
-#define MXR_GRAPHIC0_DXY		0x0034
-#define MXR_GRAPHIC0_BLANK		0x0038
-#define MXR_GRAPHIC1_CFG		0x0040
-#define MXR_GRAPHIC1_BASE		0x0044
-#define MXR_GRAPHIC1_SPAN		0x0048
-#define MXR_GRAPHIC1_SXY		0x004C
-#define MXR_GRAPHIC1_WH			0x0050
-#define MXR_GRAPHIC1_DXY		0x0054
-#define MXR_GRAPHIC1_BLANK		0x0058
-#define MXR_BG_CFG			0x0060
-#define MXR_BG_COLOR0			0x0064
-#define MXR_BG_COLOR1			0x0068
-#define MXR_BG_COLOR2			0x006C
-
-/* for parametrized access to layer registers */
-#define MXR_GRAPHIC_CFG(i)		(0x0020 + (i) * 0x20)
-#define MXR_GRAPHIC_BASE(i)		(0x0024 + (i) * 0x20)
-#define MXR_GRAPHIC_SPAN(i)		(0x0028 + (i) * 0x20)
-#define MXR_GRAPHIC_SXY(i)		(0x002C + (i) * 0x20)
-#define MXR_GRAPHIC_WH(i)		(0x0030 + (i) * 0x20)
-#define MXR_GRAPHIC_DXY(i)		(0x0034 + (i) * 0x20)
-
-/*
- * Bit definition part
- */
-
-/* generates mask for range of bits */
-#define MXR_MASK(high_bit, low_bit) \
-	(((2 << ((high_bit) - (low_bit))) - 1) << (low_bit))
-
-#define MXR_MASK_VAL(val, high_bit, low_bit) \
-	(((val) << (low_bit)) & MXR_MASK(high_bit, low_bit))
-
-/* bits for MXR_STATUS */
-#define MXR_STATUS_16_BURST		(1 << 7)
-#define MXR_STATUS_BURST_MASK		(1 << 7)
-#define MXR_STATUS_SYNC_ENABLE		(1 << 2)
-#define MXR_STATUS_REG_RUN		(1 << 0)
-
-/* bits for MXR_CFG */
-#define MXR_CFG_OUT_YUV444		(0 << 8)
-#define MXR_CFG_OUT_RGB888		(1 << 8)
-#define MXR_CFG_OUT_MASK		(1 << 8)
-#define MXR_CFG_DST_SDO			(0 << 7)
-#define MXR_CFG_DST_HDMI		(1 << 7)
-#define MXR_CFG_DST_MASK		(1 << 7)
-#define MXR_CFG_SCAN_HD_720		(0 << 6)
-#define MXR_CFG_SCAN_HD_1080		(1 << 6)
-#define MXR_CFG_GRP1_ENABLE		(1 << 5)
-#define MXR_CFG_GRP0_ENABLE		(1 << 4)
-#define MXR_CFG_VP_ENABLE		(1 << 3)
-#define MXR_CFG_SCAN_INTERLACE		(0 << 2)
-#define MXR_CFG_SCAN_PROGRASSIVE	(1 << 2)
-#define MXR_CFG_SCAN_NTSC		(0 << 1)
-#define MXR_CFG_SCAN_PAL		(1 << 1)
-#define MXR_CFG_SCAN_SD			(0 << 0)
-#define MXR_CFG_SCAN_HD			(1 << 0)
-#define MXR_CFG_SCAN_MASK		0x47
-
-/* bits for MXR_GRAPHICn_CFG */
-#define MXR_GRP_CFG_COLOR_KEY_DISABLE	(1 << 21)
-#define MXR_GRP_CFG_BLEND_PRE_MUL	(1 << 20)
-#define MXR_GRP_CFG_FORMAT_VAL(x)	MXR_MASK_VAL(x, 11, 8)
-#define MXR_GRP_CFG_FORMAT_MASK		MXR_GRP_CFG_FORMAT_VAL(~0)
-#define MXR_GRP_CFG_ALPHA_VAL(x)	MXR_MASK_VAL(x, 7, 0)
-
-/* bits for MXR_GRAPHICn_WH */
-#define MXR_GRP_WH_H_SCALE(x)		MXR_MASK_VAL(x, 28, 28)
-#define MXR_GRP_WH_V_SCALE(x)		MXR_MASK_VAL(x, 12, 12)
-#define MXR_GRP_WH_WIDTH(x)		MXR_MASK_VAL(x, 26, 16)
-#define MXR_GRP_WH_HEIGHT(x)		MXR_MASK_VAL(x, 10, 0)
-
-/* bits for MXR_GRAPHICn_SXY */
-#define MXR_GRP_SXY_SX(x)		MXR_MASK_VAL(x, 26, 16)
-#define MXR_GRP_SXY_SY(x)		MXR_MASK_VAL(x, 10, 0)
-
-/* bits for MXR_GRAPHICn_DXY */
-#define MXR_GRP_DXY_DX(x)		MXR_MASK_VAL(x, 26, 16)
-#define MXR_GRP_DXY_DY(x)		MXR_MASK_VAL(x, 10, 0)
-
-/* bits for MXR_INT_EN */
-#define MXR_INT_EN_VSYNC		(1 << 11)
-#define MXR_INT_EN_ALL			(0x0f << 8)
-
-/* bit for MXR_INT_STATUS */
-#define MXR_INT_CLEAR_VSYNC		(1 << 11)
-#define MXR_INT_STATUS_VSYNC		(1 << 0)
-
-/* bit for MXR_LAYER_CFG */
-#define MXR_LAYER_CFG_GRP1_VAL(x)	MXR_MASK_VAL(x, 11, 8)
-#define MXR_LAYER_CFG_GRP0_VAL(x)	MXR_MASK_VAL(x, 7, 4)
-#define MXR_LAYER_CFG_VP_VAL(x)		MXR_MASK_VAL(x, 3, 0)
-
-#endif /* SAMSUNG_REGS_MIXER_H */
-
diff --git a/drivers/media/platform/s5p-tv/regs-sdo.h b/drivers/media/platform/s5p-tv/regs-sdo.h
deleted file mode 100644
index 6f22fbfe2f6c..000000000000
--- a/drivers/media/platform/s5p-tv/regs-sdo.h
+++ /dev/null
@@ -1,63 +0,0 @@
-/* drivers/media/platform/s5p-tv/regs-sdo.h
- *
- * Copyright (c) 2010-2011 Samsung Electronics Co., Ltd.
- *		http://www.samsung.com/
- *
- * SDO register description file
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- */
-
-#ifndef SAMSUNG_REGS_SDO_H
-#define SAMSUNG_REGS_SDO_H
-
-/*
- * Register part
- */
-
-#define SDO_CLKCON			0x0000
-#define SDO_CONFIG			0x0008
-#define SDO_VBI				0x0014
-#define SDO_DAC				0x003C
-#define SDO_CCCON			0x0180
-#define SDO_IRQ				0x0280
-#define SDO_IRQMASK			0x0284
-#define SDO_VERSION			0x03D8
-
-/*
- * Bit definition part
- */
-
-/* SDO Clock Control Register (SDO_CLKCON) */
-#define SDO_TVOUT_SW_RESET		(1 << 4)
-#define SDO_TVOUT_CLOCK_READY		(1 << 1)
-#define SDO_TVOUT_CLOCK_ON		(1 << 0)
-
-/* SDO Video Standard Configuration Register (SDO_CONFIG) */
-#define SDO_PROGRESSIVE			(1 << 4)
-#define SDO_NTSC_M			0
-#define SDO_PAL_M			1
-#define SDO_PAL_BGHID			2
-#define SDO_PAL_N			3
-#define SDO_PAL_NC			4
-#define SDO_NTSC_443			8
-#define SDO_PAL_60			9
-#define SDO_STANDARD_MASK		0xf
-
-/* SDO VBI Configuration Register (SDO_VBI) */
-#define SDO_CVBS_WSS_INS		(1 << 14)
-#define SDO_CVBS_CLOSED_CAPTION_MASK	(3 << 12)
-
-/* SDO DAC Configuration Register (SDO_DAC) */
-#define SDO_POWER_ON_DAC		(1 << 0)
-
-/* SDO Color Compensation On/Off Control (SDO_CCCON) */
-#define SDO_COMPENSATION_BHS_ADJ_OFF	(1 << 4)
-#define SDO_COMPENSATION_CVBS_COMP_OFF	(1 << 0)
-
-/* SDO Interrupt Request Register (SDO_IRQ) */
-#define SDO_VSYNC_IRQ_PEND		(1 << 0)
-
-#endif /* SAMSUNG_REGS_SDO_H */
diff --git a/drivers/media/platform/s5p-tv/regs-vp.h b/drivers/media/platform/s5p-tv/regs-vp.h
deleted file mode 100644
index 6c63984e11e8..000000000000
--- a/drivers/media/platform/s5p-tv/regs-vp.h
+++ /dev/null
@@ -1,88 +0,0 @@
-/*
- * Copyright (c) 2010-2011 Samsung Electronics Co., Ltd.
- *		http://www.samsung.com/
- *
- * Video processor register header file for Samsung Mixer driver
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- */
-
-#ifndef SAMSUNG_REGS_VP_H
-#define SAMSUNG_REGS_VP_H
-
-/*
- * Register part
- */
-
-#define VP_ENABLE			0x0000
-#define VP_SRESET			0x0004
-#define VP_SHADOW_UPDATE		0x0008
-#define VP_FIELD_ID			0x000C
-#define VP_MODE				0x0010
-#define VP_IMG_SIZE_Y			0x0014
-#define VP_IMG_SIZE_C			0x0018
-#define VP_PER_RATE_CTRL		0x001C
-#define VP_TOP_Y_PTR			0x0028
-#define VP_BOT_Y_PTR			0x002C
-#define VP_TOP_C_PTR			0x0030
-#define VP_BOT_C_PTR			0x0034
-#define VP_ENDIAN_MODE			0x03CC
-#define VP_SRC_H_POSITION		0x0044
-#define VP_SRC_V_POSITION		0x0048
-#define VP_SRC_WIDTH			0x004C
-#define VP_SRC_HEIGHT			0x0050
-#define VP_DST_H_POSITION		0x0054
-#define VP_DST_V_POSITION		0x0058
-#define VP_DST_WIDTH			0x005C
-#define VP_DST_HEIGHT			0x0060
-#define VP_H_RATIO			0x0064
-#define VP_V_RATIO			0x0068
-#define VP_POLY8_Y0_LL			0x006C
-#define VP_POLY4_Y0_LL			0x00EC
-#define VP_POLY4_C0_LL			0x012C
-
-/*
- * Bit definition part
- */
-
-/* generates mask for range of bits */
-
-#define VP_MASK(high_bit, low_bit) \
-	(((2 << ((high_bit) - (low_bit))) - 1) << (low_bit))
-
-#define VP_MASK_VAL(val, high_bit, low_bit) \
-	(((val) << (low_bit)) & VP_MASK(high_bit, low_bit))
-
- /* VP_ENABLE */
-#define VP_ENABLE_ON			(1 << 0)
-
-/* VP_SRESET */
-#define VP_SRESET_PROCESSING		(1 << 0)
-
-/* VP_SHADOW_UPDATE */
-#define VP_SHADOW_UPDATE_ENABLE		(1 << 0)
-
-/* VP_MODE */
-#define VP_MODE_NV12			(0 << 6)
-#define VP_MODE_NV21			(1 << 6)
-#define VP_MODE_LINE_SKIP		(1 << 5)
-#define VP_MODE_MEM_LINEAR		(0 << 4)
-#define VP_MODE_MEM_TILED		(1 << 4)
-#define VP_MODE_FMT_MASK		(5 << 4)
-#define VP_MODE_FIELD_ID_AUTO_TOGGLING	(1 << 2)
-#define VP_MODE_2D_IPC			(1 << 1)
-
-/* VP_IMG_SIZE_Y */
-/* VP_IMG_SIZE_C */
-#define VP_IMG_HSIZE(x)			VP_MASK_VAL(x, 29, 16)
-#define VP_IMG_VSIZE(x)			VP_MASK_VAL(x, 13, 0)
-
-/* VP_SRC_H_POSITION */
-#define VP_SRC_H_POSITION_VAL(x)	VP_MASK_VAL(x, 14, 4)
-
-/* VP_ENDIAN_MODE */
-#define VP_ENDIAN_MODE_LITTLE		(1 << 0)
-
-#endif /* SAMSUNG_REGS_VP_H */
diff --git a/drivers/media/platform/s5p-tv/sdo_drv.c b/drivers/media/platform/s5p-tv/sdo_drv.c
deleted file mode 100644
index c75d4354d182..000000000000
--- a/drivers/media/platform/s5p-tv/sdo_drv.c
+++ /dev/null
@@ -1,497 +0,0 @@
-/*
- * Samsung Standard Definition Output (SDO) driver
- *
- * Copyright (c) 2010-2011 Samsung Electronics Co., Ltd.
- *
- * Tomasz Stanislawski, <t.stanislaws@samsung.com>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published
- * by the Free Software Foundiation. either version 2 of the License,
- * or (at your option) any later version
- */
-
-#include <linux/clk.h>
-#include <linux/delay.h>
-#include <linux/kernel.h>
-#include <linux/module.h>
-#include <linux/interrupt.h>
-#include <linux/io.h>
-#include <linux/irq.h>
-#include <linux/platform_device.h>
-#include <linux/pm_runtime.h>
-#include <linux/regulator/consumer.h>
-#include <linux/slab.h>
-
-#include <media/v4l2-subdev.h>
-
-#include "regs-sdo.h"
-
-MODULE_AUTHOR("Tomasz Stanislawski, <t.stanislaws@samsung.com>");
-MODULE_DESCRIPTION("Samsung Standard Definition Output (SDO)");
-MODULE_LICENSE("GPL");
-
-#define SDO_DEFAULT_STD	V4L2_STD_PAL
-
-struct sdo_format {
-	v4l2_std_id id;
-	/* all modes are 720 pixels wide */
-	unsigned int height;
-	unsigned int cookie;
-};
-
-struct sdo_device {
-	/** pointer to device parent */
-	struct device *dev;
-	/** base address of SDO registers */
-	void __iomem *regs;
-	/** SDO interrupt */
-	unsigned int irq;
-	/** DAC source clock */
-	struct clk *sclk_dac;
-	/** DAC clock */
-	struct clk *dac;
-	/** DAC physical interface */
-	struct clk *dacphy;
-	/** clock for control of VPLL */
-	struct clk *fout_vpll;
-	/** vpll rate before sdo stream was on */
-	unsigned long vpll_rate;
-	/** regulator for SDO IP power */
-	struct regulator *vdac;
-	/** regulator for SDO plug detection */
-	struct regulator *vdet;
-	/** subdev used as device interface */
-	struct v4l2_subdev sd;
-	/** current format */
-	const struct sdo_format *fmt;
-};
-
-static inline struct sdo_device *sd_to_sdev(struct v4l2_subdev *sd)
-{
-	return container_of(sd, struct sdo_device, sd);
-}
-
-static inline
-void sdo_write_mask(struct sdo_device *sdev, u32 reg_id, u32 value, u32 mask)
-{
-	u32 old = readl(sdev->regs + reg_id);
-	value = (value & mask) | (old & ~mask);
-	writel(value, sdev->regs + reg_id);
-}
-
-static inline
-void sdo_write(struct sdo_device *sdev, u32 reg_id, u32 value)
-{
-	writel(value, sdev->regs + reg_id);
-}
-
-static inline
-u32 sdo_read(struct sdo_device *sdev, u32 reg_id)
-{
-	return readl(sdev->regs + reg_id);
-}
-
-static irqreturn_t sdo_irq_handler(int irq, void *dev_data)
-{
-	struct sdo_device *sdev = dev_data;
-
-	/* clear interrupt */
-	sdo_write_mask(sdev, SDO_IRQ, ~0, SDO_VSYNC_IRQ_PEND);
-	return IRQ_HANDLED;
-}
-
-static void sdo_reg_debug(struct sdo_device *sdev)
-{
-#define DBGREG(reg_id) \
-	dev_info(sdev->dev, #reg_id " = %08x\n", \
-		sdo_read(sdev, reg_id))
-
-	DBGREG(SDO_CLKCON);
-	DBGREG(SDO_CONFIG);
-	DBGREG(SDO_VBI);
-	DBGREG(SDO_DAC);
-	DBGREG(SDO_IRQ);
-	DBGREG(SDO_IRQMASK);
-	DBGREG(SDO_VERSION);
-}
-
-static const struct sdo_format sdo_format[] = {
-	{ V4L2_STD_PAL_N,	.height = 576, .cookie = SDO_PAL_N },
-	{ V4L2_STD_PAL_Nc,	.height = 576, .cookie = SDO_PAL_NC },
-	{ V4L2_STD_PAL_M,	.height = 480, .cookie = SDO_PAL_M },
-	{ V4L2_STD_PAL_60,	.height = 480, .cookie = SDO_PAL_60 },
-	{ V4L2_STD_NTSC_443,	.height = 480, .cookie = SDO_NTSC_443 },
-	{ V4L2_STD_PAL,		.height = 576, .cookie = SDO_PAL_BGHID },
-	{ V4L2_STD_NTSC_M,	.height = 480, .cookie = SDO_NTSC_M },
-};
-
-static const struct sdo_format *sdo_find_format(v4l2_std_id id)
-{
-	int i;
-	for (i = 0; i < ARRAY_SIZE(sdo_format); ++i)
-		if (sdo_format[i].id & id)
-			return &sdo_format[i];
-	return NULL;
-}
-
-static int sdo_g_tvnorms_output(struct v4l2_subdev *sd, v4l2_std_id *std)
-{
-	*std = V4L2_STD_NTSC_M | V4L2_STD_PAL_M | V4L2_STD_PAL |
-		V4L2_STD_PAL_N | V4L2_STD_PAL_Nc |
-		V4L2_STD_NTSC_443 | V4L2_STD_PAL_60;
-	return 0;
-}
-
-static int sdo_s_std_output(struct v4l2_subdev *sd, v4l2_std_id std)
-{
-	struct sdo_device *sdev = sd_to_sdev(sd);
-	const struct sdo_format *fmt;
-	fmt = sdo_find_format(std);
-	if (fmt == NULL)
-		return -EINVAL;
-	sdev->fmt = fmt;
-	return 0;
-}
-
-static int sdo_g_std_output(struct v4l2_subdev *sd, v4l2_std_id *std)
-{
-	*std = sd_to_sdev(sd)->fmt->id;
-	return 0;
-}
-
-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;
-	fmt->code = MEDIA_BUS_FMT_FIXED;
-	fmt->field = V4L2_FIELD_INTERLACED;
-	fmt->colorspace = V4L2_COLORSPACE_JPEG;
-	return 0;
-}
-
-static int sdo_s_power(struct v4l2_subdev *sd, int on)
-{
-	struct sdo_device *sdev = sd_to_sdev(sd);
-	struct device *dev = sdev->dev;
-	int ret;
-
-	dev_info(dev, "sdo_s_power(%d)\n", on);
-
-	if (on)
-		ret = pm_runtime_get_sync(dev);
-	else
-		ret = pm_runtime_put_sync(dev);
-
-	/* only values < 0 indicate errors */
-	return ret < 0 ? ret : 0;
-}
-
-static int sdo_streamon(struct sdo_device *sdev)
-{
-	int ret;
-
-	/* set proper clock for Timing Generator */
-	sdev->vpll_rate = clk_get_rate(sdev->fout_vpll);
-	ret = clk_set_rate(sdev->fout_vpll, 54000000);
-	if (ret < 0) {
-		dev_err(sdev->dev, "Failed to set vpll rate\n");
-		return ret;
-	}
-	dev_info(sdev->dev, "fout_vpll.rate = %lu\n",
-	clk_get_rate(sdev->fout_vpll));
-	/* enable clock in SDO */
-	sdo_write_mask(sdev, SDO_CLKCON, ~0, SDO_TVOUT_CLOCK_ON);
-	ret = clk_prepare_enable(sdev->dacphy);
-	if (ret < 0) {
-		dev_err(sdev->dev, "clk_prepare_enable(dacphy) failed\n");
-		goto fail;
-	}
-	/* enable DAC */
-	sdo_write_mask(sdev, SDO_DAC, ~0, SDO_POWER_ON_DAC);
-	sdo_reg_debug(sdev);
-	return 0;
-
-fail:
-	sdo_write_mask(sdev, SDO_CLKCON, 0, SDO_TVOUT_CLOCK_ON);
-	clk_set_rate(sdev->fout_vpll, sdev->vpll_rate);
-	return ret;
-}
-
-static int sdo_streamoff(struct sdo_device *sdev)
-{
-	int tries;
-
-	sdo_write_mask(sdev, SDO_DAC, 0, SDO_POWER_ON_DAC);
-	clk_disable_unprepare(sdev->dacphy);
-	sdo_write_mask(sdev, SDO_CLKCON, 0, SDO_TVOUT_CLOCK_ON);
-	for (tries = 100; tries; --tries) {
-		if (sdo_read(sdev, SDO_CLKCON) & SDO_TVOUT_CLOCK_READY)
-			break;
-		mdelay(1);
-	}
-	if (tries == 0)
-		dev_err(sdev->dev, "failed to stop streaming\n");
-	clk_set_rate(sdev->fout_vpll, sdev->vpll_rate);
-	return tries ? 0 : -EIO;
-}
-
-static int sdo_s_stream(struct v4l2_subdev *sd, int on)
-{
-	struct sdo_device *sdev = sd_to_sdev(sd);
-	return on ? sdo_streamon(sdev) : sdo_streamoff(sdev);
-}
-
-static const struct v4l2_subdev_core_ops sdo_sd_core_ops = {
-	.s_power = sdo_s_power,
-};
-
-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,
-	.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)
-{
-	struct v4l2_subdev *sd = dev_get_drvdata(dev);
-	struct sdo_device *sdev = sd_to_sdev(sd);
-
-	dev_info(dev, "suspend\n");
-	regulator_disable(sdev->vdet);
-	regulator_disable(sdev->vdac);
-	clk_disable_unprepare(sdev->sclk_dac);
-	return 0;
-}
-
-static int sdo_runtime_resume(struct device *dev)
-{
-	struct v4l2_subdev *sd = dev_get_drvdata(dev);
-	struct sdo_device *sdev = sd_to_sdev(sd);
-	int ret;
-
-	dev_info(dev, "resume\n");
-
-	ret = clk_prepare_enable(sdev->sclk_dac);
-	if (ret < 0)
-		return ret;
-
-	ret = regulator_enable(sdev->vdac);
-	if (ret < 0)
-		goto dac_clk_dis;
-
-	ret = regulator_enable(sdev->vdet);
-	if (ret < 0)
-		goto vdac_r_dis;
-
-	/* software reset */
-	sdo_write_mask(sdev, SDO_CLKCON, ~0, SDO_TVOUT_SW_RESET);
-	mdelay(10);
-	sdo_write_mask(sdev, SDO_CLKCON, 0, SDO_TVOUT_SW_RESET);
-
-	/* setting TV mode */
-	sdo_write_mask(sdev, SDO_CONFIG, sdev->fmt->cookie, SDO_STANDARD_MASK);
-	/* XXX: forcing interlaced mode using undocumented bit */
-	sdo_write_mask(sdev, SDO_CONFIG, 0, SDO_PROGRESSIVE);
-	/* turn all VBI off */
-	sdo_write_mask(sdev, SDO_VBI, 0, SDO_CVBS_WSS_INS |
-		SDO_CVBS_CLOSED_CAPTION_MASK);
-	/* turn all post processing off */
-	sdo_write_mask(sdev, SDO_CCCON, ~0, SDO_COMPENSATION_BHS_ADJ_OFF |
-		SDO_COMPENSATION_CVBS_COMP_OFF);
-	sdo_reg_debug(sdev);
-	return 0;
-
-vdac_r_dis:
-	regulator_disable(sdev->vdac);
-dac_clk_dis:
-	clk_disable_unprepare(sdev->sclk_dac);
-	return ret;
-}
-
-static const struct dev_pm_ops sdo_pm_ops = {
-	.runtime_suspend = sdo_runtime_suspend,
-	.runtime_resume	 = sdo_runtime_resume,
-};
-
-static int sdo_probe(struct platform_device *pdev)
-{
-	struct device *dev = &pdev->dev;
-	struct sdo_device *sdev;
-	struct resource *res;
-	int ret = 0;
-	struct clk *sclk_vpll;
-
-	dev_info(dev, "probe start\n");
-	sdev = devm_kzalloc(&pdev->dev, sizeof(*sdev), GFP_KERNEL);
-	if (!sdev) {
-		dev_err(dev, "not enough memory.\n");
-		ret = -ENOMEM;
-		goto fail;
-	}
-	sdev->dev = dev;
-
-	/* mapping registers */
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (res == NULL) {
-		dev_err(dev, "get memory resource failed.\n");
-		ret = -ENXIO;
-		goto fail;
-	}
-
-	sdev->regs = devm_ioremap(&pdev->dev, res->start, resource_size(res));
-	if (sdev->regs == NULL) {
-		dev_err(dev, "register mapping failed.\n");
-		ret = -ENXIO;
-		goto fail;
-	}
-
-	/* acquiring interrupt */
-	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
-	if (res == NULL) {
-		dev_err(dev, "get interrupt resource failed.\n");
-		ret = -ENXIO;
-		goto fail;
-	}
-	ret = devm_request_irq(&pdev->dev, res->start, sdo_irq_handler, 0,
-			       "s5p-sdo", sdev);
-	if (ret) {
-		dev_err(dev, "request interrupt failed.\n");
-		goto fail;
-	}
-	sdev->irq = res->start;
-
-	/* acquire clocks */
-	sdev->sclk_dac = clk_get(dev, "sclk_dac");
-	if (IS_ERR(sdev->sclk_dac)) {
-		dev_err(dev, "failed to get clock 'sclk_dac'\n");
-		ret = PTR_ERR(sdev->sclk_dac);
-		goto fail;
-	}
-	sdev->dac = clk_get(dev, "dac");
-	if (IS_ERR(sdev->dac)) {
-		dev_err(dev, "failed to get clock 'dac'\n");
-		ret = PTR_ERR(sdev->dac);
-		goto fail_sclk_dac;
-	}
-	sdev->dacphy = clk_get(dev, "dacphy");
-	if (IS_ERR(sdev->dacphy)) {
-		dev_err(dev, "failed to get clock 'dacphy'\n");
-		ret = PTR_ERR(sdev->dacphy);
-		goto fail_dac;
-	}
-	sclk_vpll = clk_get(dev, "sclk_vpll");
-	if (IS_ERR(sclk_vpll)) {
-		dev_err(dev, "failed to get clock 'sclk_vpll'\n");
-		ret = PTR_ERR(sclk_vpll);
-		goto fail_dacphy;
-	}
-	clk_set_parent(sdev->sclk_dac, sclk_vpll);
-	clk_put(sclk_vpll);
-	sdev->fout_vpll = clk_get(dev, "fout_vpll");
-	if (IS_ERR(sdev->fout_vpll)) {
-		dev_err(dev, "failed to get clock 'fout_vpll'\n");
-		ret = PTR_ERR(sdev->fout_vpll);
-		goto fail_dacphy;
-	}
-	dev_info(dev, "fout_vpll.rate = %lu\n", clk_get_rate(sclk_vpll));
-
-	/* acquire regulator */
-	sdev->vdac = devm_regulator_get(dev, "vdd33a_dac");
-	if (IS_ERR(sdev->vdac)) {
-		dev_err(dev, "failed to get regulator 'vdac'\n");
-		ret = PTR_ERR(sdev->vdac);
-		goto fail_fout_vpll;
-	}
-	sdev->vdet = devm_regulator_get(dev, "vdet");
-	if (IS_ERR(sdev->vdet)) {
-		dev_err(dev, "failed to get regulator 'vdet'\n");
-		ret = PTR_ERR(sdev->vdet);
-		goto fail_fout_vpll;
-	}
-
-	/* enable gate for dac clock, because mixer uses it */
-	ret = clk_prepare_enable(sdev->dac);
-	if (ret < 0) {
-		dev_err(dev, "clk_prepare_enable(dac) failed\n");
-		goto fail_fout_vpll;
-	}
-
-	/* configure power management */
-	pm_runtime_enable(dev);
-
-	/* configuration of interface subdevice */
-	v4l2_subdev_init(&sdev->sd, &sdo_sd_ops);
-	sdev->sd.owner = THIS_MODULE;
-	strlcpy(sdev->sd.name, "s5p-sdo", sizeof(sdev->sd.name));
-
-	/* set default format */
-	sdev->fmt = sdo_find_format(SDO_DEFAULT_STD);
-	BUG_ON(sdev->fmt == NULL);
-
-	/* keeping subdev in device's private for use by other drivers */
-	dev_set_drvdata(dev, &sdev->sd);
-
-	dev_info(dev, "probe succeeded\n");
-	return 0;
-
-fail_fout_vpll:
-	clk_put(sdev->fout_vpll);
-fail_dacphy:
-	clk_put(sdev->dacphy);
-fail_dac:
-	clk_put(sdev->dac);
-fail_sclk_dac:
-	clk_put(sdev->sclk_dac);
-fail:
-	dev_info(dev, "probe failed\n");
-	return ret;
-}
-
-static int sdo_remove(struct platform_device *pdev)
-{
-	struct v4l2_subdev *sd = dev_get_drvdata(&pdev->dev);
-	struct sdo_device *sdev = sd_to_sdev(sd);
-
-	pm_runtime_disable(&pdev->dev);
-	clk_disable_unprepare(sdev->dac);
-	clk_put(sdev->fout_vpll);
-	clk_put(sdev->dacphy);
-	clk_put(sdev->dac);
-	clk_put(sdev->sclk_dac);
-
-	dev_info(&pdev->dev, "remove successful\n");
-	return 0;
-}
-
-static struct platform_driver sdo_driver __refdata = {
-	.probe = sdo_probe,
-	.remove = sdo_remove,
-	.driver = {
-		.name = "s5p-sdo",
-		.pm = &sdo_pm_ops,
-	}
-};
-
-module_platform_driver(sdo_driver);
diff --git a/drivers/media/platform/s5p-tv/sii9234_drv.c b/drivers/media/platform/s5p-tv/sii9234_drv.c
deleted file mode 100644
index 0a97f9ab4f76..000000000000
--- a/drivers/media/platform/s5p-tv/sii9234_drv.c
+++ /dev/null
@@ -1,407 +0,0 @@
-/*
- * Samsung MHL interface driver
- *
- * Copyright (C) 2011 Samsung Electronics Co.Ltd
- * Author: Tomasz Stanislawski <t.stanislaws@samsung.com>
- *
- * This program is free software; you can redistribute  it and/or modify it
- * under  the terms of  the GNU General  Public License as published by the
- * Free Software Foundation;  either version 2 of the  License, or (at your
- * option) any later version.
- */
-
-#include <linux/delay.h>
-#include <linux/err.h>
-#include <linux/freezer.h>
-#include <linux/gpio.h>
-#include <linux/i2c.h>
-#include <linux/interrupt.h>
-#include <linux/irq.h>
-#include <linux/kthread.h>
-#include <linux/module.h>
-#include <linux/pm_runtime.h>
-#include <linux/regulator/machine.h>
-#include <linux/slab.h>
-
-#include <linux/platform_data/media/sii9234.h>
-#include <media/v4l2-subdev.h>
-
-MODULE_AUTHOR("Tomasz Stanislawski <t.stanislaws@samsung.com>");
-MODULE_DESCRIPTION("Samsung MHL interface driver");
-MODULE_LICENSE("GPL");
-
-struct sii9234_context {
-	struct i2c_client *client;
-	struct regulator *power;
-	int gpio_n_reset;
-	struct v4l2_subdev sd;
-};
-
-static inline struct sii9234_context *sd_to_context(struct v4l2_subdev *sd)
-{
-	return container_of(sd, struct sii9234_context, sd);
-}
-
-static inline int sii9234_readb(struct i2c_client *client, int addr)
-{
-	return i2c_smbus_read_byte_data(client, addr);
-}
-
-static inline int sii9234_writeb(struct i2c_client *client, int addr, int value)
-{
-	return i2c_smbus_write_byte_data(client, addr, value);
-}
-
-static inline int sii9234_writeb_mask(struct i2c_client *client, int addr,
-	int value, int mask)
-{
-	int ret;
-
-	ret = i2c_smbus_read_byte_data(client, addr);
-	if (ret < 0)
-		return ret;
-	ret = (ret & ~mask) | (value & mask);
-	return i2c_smbus_write_byte_data(client, addr, ret);
-}
-
-static inline int sii9234_readb_idx(struct i2c_client *client, int addr)
-{
-	int ret;
-	ret = i2c_smbus_write_byte_data(client, 0xbc, addr >> 8);
-	if (ret < 0)
-		return ret;
-	ret = i2c_smbus_write_byte_data(client, 0xbd, addr & 0xff);
-	if (ret < 0)
-		return ret;
-	return i2c_smbus_read_byte_data(client, 0xbe);
-}
-
-static inline int sii9234_writeb_idx(struct i2c_client *client, int addr,
-	int value)
-{
-	int ret;
-	ret = i2c_smbus_write_byte_data(client, 0xbc, addr >> 8);
-	if (ret < 0)
-		return ret;
-	ret = i2c_smbus_write_byte_data(client, 0xbd, addr & 0xff);
-	if (ret < 0)
-		return ret;
-	ret = i2c_smbus_write_byte_data(client, 0xbe, value);
-	return ret;
-}
-
-static inline int sii9234_writeb_idx_mask(struct i2c_client *client, int addr,
-	int value, int mask)
-{
-	int ret;
-
-	ret = sii9234_readb_idx(client, addr);
-	if (ret < 0)
-		return ret;
-	ret = (ret & ~mask) | (value & mask);
-	return sii9234_writeb_idx(client, addr, ret);
-}
-
-static int sii9234_reset(struct sii9234_context *ctx)
-{
-	struct i2c_client *client = ctx->client;
-	struct device *dev = &client->dev;
-	int ret, tries;
-
-	gpio_direction_output(ctx->gpio_n_reset, 1);
-	mdelay(1);
-	gpio_direction_output(ctx->gpio_n_reset, 0);
-	mdelay(1);
-	gpio_direction_output(ctx->gpio_n_reset, 1);
-	mdelay(1);
-
-	/* going to TTPI mode */
-	ret = sii9234_writeb(client, 0xc7, 0);
-	if (ret < 0) {
-		dev_err(dev, "failed to set TTPI mode\n");
-		return ret;
-	}
-	for (tries = 0; tries < 100 ; ++tries) {
-		ret = sii9234_readb(client, 0x1b);
-		if (ret > 0)
-			break;
-		if (ret < 0) {
-			dev_err(dev, "failed to reset device\n");
-			return -EIO;
-		}
-		mdelay(1);
-	}
-	if (tries == 100) {
-		dev_err(dev, "maximal number of tries reached\n");
-		return -EIO;
-	}
-
-	return 0;
-}
-
-static int sii9234_verify_version(struct i2c_client *client)
-{
-	struct device *dev = &client->dev;
-	int family, rev, tpi_rev, dev_id, sub_id, hdcp, id;
-
-	family = sii9234_readb(client, 0x1b);
-	rev = sii9234_readb(client, 0x1c) & 0x0f;
-	tpi_rev = sii9234_readb(client, 0x1d) & 0x7f;
-	dev_id = sii9234_readb_idx(client, 0x0103);
-	sub_id = sii9234_readb_idx(client, 0x0102);
-	hdcp = sii9234_readb(client, 0x30);
-
-	if (family < 0 || rev < 0 || tpi_rev < 0 || dev_id < 0 ||
-		sub_id < 0 || hdcp < 0) {
-		dev_err(dev, "failed to read chip's version\n");
-		return -EIO;
-	}
-
-	id = (dev_id << 8) | sub_id;
-
-	dev_info(dev, "chip: SiL%02x family: %02x, rev: %02x\n",
-		id, family, rev);
-	dev_info(dev, "tpi_rev:%02x, hdcp: %02x\n", tpi_rev, hdcp);
-	if (id != 0x9234) {
-		dev_err(dev, "not supported chip\n");
-		return -ENODEV;
-	}
-
-	return 0;
-}
-
-static u8 data[][3] = {
-/* setup from driver created by doonsoo45.kim */
-	{ 0x01, 0x05, 0x04 }, /* Enable Auto soft reset on SCDT = 0 */
-	{ 0x01, 0x08, 0x35 }, /* Power Up TMDS Tx Core */
-	{ 0x01, 0x0d, 0x1c }, /* HDMI Transcode mode enable */
-	{ 0x01, 0x2b, 0x01 }, /* Enable HDCP Compliance workaround */
-	{ 0x01, 0x79, 0x40 }, /* daniel test...MHL_INT */
-	{ 0x01, 0x80, 0x34 }, /* Enable Rx PLL Clock Value */
-	{ 0x01, 0x90, 0x27 }, /* Enable CBUS discovery */
-	{ 0x01, 0x91, 0xe5 }, /* Skip RGND detection */
-	{ 0x01, 0x92, 0x46 }, /* Force MHD mode */
-	{ 0x01, 0x93, 0xdc }, /* Disable CBUS pull-up during RGND measurement */
-	{ 0x01, 0x94, 0x66 }, /* 1.8V CBUS VTH & GND threshold */
-	{ 0x01, 0x95, 0x31 }, /* RGND block & single discovery attempt */
-	{ 0x01, 0x96, 0x22 }, /* use 1K and 2K setting */
-	{ 0x01, 0xa0, 0x10 }, /* SIMG: Term mode */
-	{ 0x01, 0xa1, 0xfc }, /* Disable internal Mobile HD driver */
-	{ 0x01, 0xa3, 0xfa }, /* SIMG: Output Swing  default EB, 3x Clk Mult */
-	{ 0x01, 0xa5, 0x80 }, /* SIMG: RGND Hysterisis, 3x mode for Beast */
-	{ 0x01, 0xa6, 0x0c }, /* SIMG: Swing Offset */
-	{ 0x02, 0x3d, 0x3f }, /* Power up CVCC 1.2V core */
-	{ 0x03, 0x00, 0x00 }, /* SIMG: correcting HW default */
-	{ 0x03, 0x11, 0x01 }, /* Enable TxPLL Clock */
-	{ 0x03, 0x12, 0x15 }, /* Enable Tx Clock Path & Equalizer */
-	{ 0x03, 0x13, 0x60 }, /* SIMG: Set termination value */
-	{ 0x03, 0x14, 0xf0 }, /* SIMG: Change CKDT level */
-	{ 0x03, 0x17, 0x07 }, /* SIMG: PLL Calrefsel */
-	{ 0x03, 0x1a, 0x20 }, /* VCO Cal */
-	{ 0x03, 0x22, 0xe0 }, /* SIMG: Auto EQ */
-	{ 0x03, 0x23, 0xc0 }, /* SIMG: Auto EQ */
-	{ 0x03, 0x24, 0xa0 }, /* SIMG: Auto EQ */
-	{ 0x03, 0x25, 0x80 }, /* SIMG: Auto EQ */
-	{ 0x03, 0x26, 0x60 }, /* SIMG: Auto EQ */
-	{ 0x03, 0x27, 0x40 }, /* SIMG: Auto EQ */
-	{ 0x03, 0x28, 0x20 }, /* SIMG: Auto EQ */
-	{ 0x03, 0x29, 0x00 }, /* SIMG: Auto EQ */
-	{ 0x03, 0x31, 0x0b }, /* SIMG: Rx PLL BW value from I2C BW ~ 4MHz */
-	{ 0x03, 0x45, 0x06 }, /* SIMG: DPLL Mode */
-	{ 0x03, 0x4b, 0x06 }, /* SIMG: Correcting HW default */
-	{ 0x03, 0x4c, 0xa0 }, /* Manual zone control */
-	{ 0x03, 0x4d, 0x02 }, /* SIMG: PLL Mode Value (order is important) */
-};
-
-static int sii9234_set_internal(struct sii9234_context *ctx)
-{
-	struct i2c_client *client = ctx->client;
-	int i, ret;
-
-	for (i = 0; i < ARRAY_SIZE(data); ++i) {
-		int addr = (data[i][0] << 8) | data[i][1];
-		ret = sii9234_writeb_idx(client, addr, data[i][2]);
-		if (ret < 0)
-			return ret;
-	}
-	return 0;
-}
-
-static int sii9234_runtime_suspend(struct device *dev)
-{
-	struct v4l2_subdev *sd = dev_get_drvdata(dev);
-	struct sii9234_context *ctx = sd_to_context(sd);
-	struct i2c_client *client = ctx->client;
-
-	dev_info(dev, "suspend start\n");
-
-	sii9234_writeb_mask(client, 0x1e, 3, 3);
-	regulator_disable(ctx->power);
-
-	return 0;
-}
-
-static int sii9234_runtime_resume(struct device *dev)
-{
-	struct v4l2_subdev *sd = dev_get_drvdata(dev);
-	struct sii9234_context *ctx = sd_to_context(sd);
-	struct i2c_client *client = ctx->client;
-	int ret;
-
-	dev_info(dev, "resume start\n");
-	ret = regulator_enable(ctx->power);
-	if (ret < 0)
-		return ret;
-
-	ret = sii9234_reset(ctx);
-	if (ret)
-		goto fail;
-
-	/* enable tpi */
-	ret = sii9234_writeb_mask(client, 0x1e, 1, 0);
-	if (ret < 0)
-		goto fail;
-	ret = sii9234_set_internal(ctx);
-	if (ret < 0)
-		goto fail;
-
-	return 0;
-
-fail:
-	dev_err(dev, "failed to resume\n");
-	regulator_disable(ctx->power);
-
-	return ret;
-}
-
-static const struct dev_pm_ops sii9234_pm_ops = {
-	.runtime_suspend = sii9234_runtime_suspend,
-	.runtime_resume	 = sii9234_runtime_resume,
-};
-
-static int sii9234_s_power(struct v4l2_subdev *sd, int on)
-{
-	struct sii9234_context *ctx = sd_to_context(sd);
-	int ret;
-
-	if (on)
-		ret = pm_runtime_get_sync(&ctx->client->dev);
-	else
-		ret = pm_runtime_put(&ctx->client->dev);
-	/* only values < 0 indicate errors */
-	return ret < 0 ? ret : 0;
-}
-
-static int sii9234_s_stream(struct v4l2_subdev *sd, int enable)
-{
-	struct sii9234_context *ctx = sd_to_context(sd);
-
-	/* (dis/en)able TDMS output */
-	sii9234_writeb_mask(ctx->client, 0x1a, enable ? 0 : ~0 , 1 << 4);
-	return 0;
-}
-
-static const struct v4l2_subdev_core_ops sii9234_core_ops = {
-	.s_power =  sii9234_s_power,
-};
-
-static const struct v4l2_subdev_video_ops sii9234_video_ops = {
-	.s_stream =  sii9234_s_stream,
-};
-
-static const struct v4l2_subdev_ops sii9234_ops = {
-	.core = &sii9234_core_ops,
-	.video = &sii9234_video_ops,
-};
-
-static int sii9234_probe(struct i2c_client *client,
-			 const struct i2c_device_id *id)
-{
-	struct device *dev = &client->dev;
-	struct sii9234_platform_data *pdata = dev->platform_data;
-	struct sii9234_context *ctx;
-	int ret;
-
-	ctx = devm_kzalloc(&client->dev, sizeof(*ctx), GFP_KERNEL);
-	if (!ctx) {
-		dev_err(dev, "out of memory\n");
-		ret = -ENOMEM;
-		goto fail;
-	}
-	ctx->client = client;
-
-	ctx->power = devm_regulator_get(dev, "hdmi-en");
-	if (IS_ERR(ctx->power)) {
-		dev_err(dev, "failed to acquire regulator hdmi-en\n");
-		return PTR_ERR(ctx->power);
-	}
-
-	ctx->gpio_n_reset = pdata->gpio_n_reset;
-	ret = devm_gpio_request(dev, ctx->gpio_n_reset, "MHL_RST");
-	if (ret) {
-		dev_err(dev, "failed to acquire MHL_RST gpio\n");
-		return ret;
-	}
-
-	v4l2_i2c_subdev_init(&ctx->sd, client, &sii9234_ops);
-
-	pm_runtime_enable(dev);
-
-	/* enable device */
-	ret = pm_runtime_get_sync(dev);
-	if (ret)
-		goto fail_pm;
-
-	/* verify chip version */
-	ret = sii9234_verify_version(client);
-	if (ret)
-		goto fail_pm_get;
-
-	/* stop processing */
-	pm_runtime_put(dev);
-
-	dev_info(dev, "probe successful\n");
-
-	return 0;
-
-fail_pm_get:
-	pm_runtime_put_sync(dev);
-
-fail_pm:
-	pm_runtime_disable(dev);
-
-fail:
-	dev_err(dev, "probe failed\n");
-
-	return ret;
-}
-
-static int sii9234_remove(struct i2c_client *client)
-{
-	struct device *dev = &client->dev;
-
-	pm_runtime_disable(dev);
-
-	dev_info(dev, "remove successful\n");
-
-	return 0;
-}
-
-
-static const struct i2c_device_id sii9234_id[] = {
-	{ "SII9234", 0 },
-	{ },
-};
-
-MODULE_DEVICE_TABLE(i2c, sii9234_id);
-static struct i2c_driver sii9234_driver = {
-	.driver = {
-		.name	= "sii9234",
-		.pm = &sii9234_pm_ops,
-	},
-	.probe		= sii9234_probe,
-	.remove		= sii9234_remove,
-	.id_table = sii9234_id,
-};
-
-module_i2c_driver(sii9234_driver);
diff --git a/drivers/media/platform/sh_vou.c b/drivers/media/platform/sh_vou.c
index e1f39b4cf1cd..ef2a519bcd4c 100644
--- a/drivers/media/platform/sh_vou.c
+++ b/drivers/media/platform/sh_vou.c
@@ -362,7 +362,7 @@ static void sh_vou_stop_streaming(struct vb2_queue *vq)
 	spin_unlock_irqrestore(&vou_dev->lock, flags);
 }
 
-static struct vb2_ops sh_vou_qops = {
+static const struct vb2_ops sh_vou_qops = {
 	.queue_setup		= sh_vou_queue_setup,
 	.buf_prepare		= sh_vou_buf_prepare,
 	.buf_queue		= sh_vou_buf_queue,
@@ -937,7 +937,10 @@ static int sh_vou_s_selection(struct file *file, void *fh,
 {
 	struct v4l2_rect *rect = &sel->r;
 	struct sh_vou_device *vou_dev = video_drvdata(file);
-	struct v4l2_crop sd_crop = {.type = V4L2_BUF_TYPE_VIDEO_OUTPUT};
+	struct v4l2_subdev_selection sd_sel = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		.target = V4L2_SEL_TGT_COMPOSE,
+	};
 	struct v4l2_pix_format *pix = &vou_dev->pix;
 	struct sh_vou_geometry geo;
 	struct v4l2_subdev_format format = {
@@ -978,14 +981,14 @@ static int sh_vou_s_selection(struct file *file, void *fh,
 	geo.in_height = pix->height;
 
 	/* Configure the encoder one-to-one, position at 0, ignore errors */
-	sd_crop.c.width = geo.output.width;
-	sd_crop.c.height = geo.output.height;
+	sd_sel.r.width = geo.output.width;
+	sd_sel.r.height = geo.output.height;
 	/*
-	 * We first issue a S_CROP, so that the subsequent S_FMT delivers the
+	 * We first issue a S_SELECTION, so that the subsequent S_FMT delivers the
 	 * final encoder configuration.
 	 */
-	v4l2_device_call_until_err(&vou_dev->v4l2_dev, 0, video,
-				   s_crop, &sd_crop);
+	v4l2_device_call_until_err(&vou_dev->v4l2_dev, 0, pad,
+				   set_selection, NULL, &sd_sel);
 	format.format.width = geo.output.width;
 	format.format.height = geo.output.height;
 	ret = v4l2_device_call_until_err(&vou_dev->v4l2_dev, 0, pad,
diff --git a/drivers/media/platform/soc_camera/Kconfig b/drivers/media/platform/soc_camera/Kconfig
index 39f66414f621..86d74788544f 100644
--- a/drivers/media/platform/soc_camera/Kconfig
+++ b/drivers/media/platform/soc_camera/Kconfig
@@ -17,31 +17,6 @@ config SOC_CAMERA_PLATFORM
 	help
 	  This is a generic SoC camera platform driver, useful for testing
 
-config VIDEO_PXA27x
-	tristate "PXA27x Quick Capture Interface driver"
-	depends on VIDEO_DEV && PXA27x && SOC_CAMERA
-	select VIDEOBUF_DMA_SG
-	select SG_SPLIT
-	---help---
-	  This is a v4l2 driver for the PXA27x Quick Capture Interface
-
-config VIDEO_RCAR_VIN_OLD
-	tristate "R-Car Video Input (VIN) support (DEPRECATED)"
-	depends on VIDEO_DEV && SOC_CAMERA
-	depends on ARCH_RENESAS || COMPILE_TEST
-	depends on HAS_DMA
-	select VIDEOBUF2_DMA_CONTIG
-	select SOC_CAMERA_SCALE_CROP
-	---help---
-	  This is a v4l2 driver for the R-Car VIN Interface
-
-config VIDEO_SH_MOBILE_CSI2
-	tristate "SuperH Mobile MIPI CSI-2 Interface driver"
-	depends on VIDEO_DEV && SOC_CAMERA && HAVE_CLK
-	depends on ARCH_SHMOBILE || SUPERH || COMPILE_TEST
-	---help---
-	  This is a v4l2 driver for the SuperH MIPI CSI-2 Interface
-
 config VIDEO_SH_MOBILE_CEU
 	tristate "SuperH Mobile CEU Interface driver"
 	depends on VIDEO_DEV && SOC_CAMERA && HAS_DMA && HAVE_CLK
diff --git a/drivers/media/platform/soc_camera/Makefile b/drivers/media/platform/soc_camera/Makefile
index 7703cb7ce456..7633a0f2f66f 100644
--- a/drivers/media/platform/soc_camera/Makefile
+++ b/drivers/media/platform/soc_camera/Makefile
@@ -7,7 +7,4 @@ obj-$(CONFIG_SOC_CAMERA_PLATFORM)	+= soc_camera_platform.o
 
 # soc-camera host drivers have to be linked after camera drivers
 obj-$(CONFIG_VIDEO_ATMEL_ISI)		+= atmel-isi.o
-obj-$(CONFIG_VIDEO_PXA27x)		+= pxa_camera.o
 obj-$(CONFIG_VIDEO_SH_MOBILE_CEU)	+= sh_mobile_ceu_camera.o
-obj-$(CONFIG_VIDEO_SH_MOBILE_CSI2)	+= sh_mobile_csi2.o
-obj-$(CONFIG_VIDEO_RCAR_VIN_OLD)	+= rcar_vin.o
diff --git a/drivers/media/platform/soc_camera/atmel-isi.c b/drivers/media/platform/soc_camera/atmel-isi.c
index 30211f6b4483..46de657c3e6d 100644
--- a/drivers/media/platform/soc_camera/atmel-isi.c
+++ b/drivers/media/platform/soc_camera/atmel-isi.c
@@ -536,7 +536,7 @@ static void stop_streaming(struct vb2_queue *vq)
 	pm_runtime_put(ici->v4l2_dev.dev);
 }
 
-static struct vb2_ops isi_video_qops = {
+static const struct vb2_ops isi_video_qops = {
 	.queue_setup		= queue_setup,
 	.buf_init		= buffer_init,
 	.buf_prepare		= buffer_prepare,
diff --git a/drivers/media/platform/soc_camera/rcar_vin.c b/drivers/media/platform/soc_camera/rcar_vin.c
deleted file mode 100644
index 9c137522c660..000000000000
--- a/drivers/media/platform/soc_camera/rcar_vin.c
+++ /dev/null
@@ -1,1970 +0,0 @@
-/*
- * SoC-camera host driver for Renesas R-Car VIN unit
- *
- * Copyright (C) 2011-2013 Renesas Solutions Corp.
- * Copyright (C) 2013 Cogent Embedded, Inc., <source@cogentembedded.com>
- *
- * Based on V4L2 Driver for SuperH Mobile CEU interface "sh_mobile_ceu_camera.c"
- *
- * Copyright (C) 2008 Magnus Damm
- *
- * This program is free software; you can redistribute  it and/or modify it
- * under  the terms of  the GNU General  Public License as published by the
- * Free Software Foundation;  either version 2 of the  License, or (at your
- * option) any later version.
- */
-
-#include <linux/delay.h>
-#include <linux/interrupt.h>
-#include <linux/io.h>
-#include <linux/kernel.h>
-#include <linux/module.h>
-#include <linux/of.h>
-#include <linux/of_device.h>
-#include <linux/platform_device.h>
-#include <linux/pm_runtime.h>
-#include <linux/slab.h>
-#include <linux/videodev2.h>
-
-#include <media/soc_camera.h>
-#include <media/drv-intf/soc_mediabus.h>
-#include <media/v4l2-common.h>
-#include <media/v4l2-dev.h>
-#include <media/v4l2-device.h>
-#include <media/v4l2-mediabus.h>
-#include <media/v4l2-of.h>
-#include <media/v4l2-subdev.h>
-#include <media/videobuf2-dma-contig.h>
-
-#include "soc_scale_crop.h"
-
-#define DRV_NAME "rcar_vin"
-
-/* Register offsets for R-Car VIN */
-#define VNMC_REG	0x00	/* Video n Main Control Register */
-#define VNMS_REG	0x04	/* Video n Module Status Register */
-#define VNFC_REG	0x08	/* Video n Frame Capture Register */
-#define VNSLPRC_REG	0x0C	/* Video n Start Line Pre-Clip Register */
-#define VNELPRC_REG	0x10	/* Video n End Line Pre-Clip Register */
-#define VNSPPRC_REG	0x14	/* Video n Start Pixel Pre-Clip Register */
-#define VNEPPRC_REG	0x18	/* Video n End Pixel Pre-Clip Register */
-#define VNSLPOC_REG	0x1C	/* Video n Start Line Post-Clip Register */
-#define VNELPOC_REG	0x20	/* Video n End Line Post-Clip Register */
-#define VNSPPOC_REG	0x24	/* Video n Start Pixel Post-Clip Register */
-#define VNEPPOC_REG	0x28	/* Video n End Pixel Post-Clip Register */
-#define VNIS_REG	0x2C	/* Video n Image Stride Register */
-#define VNMB_REG(m)	(0x30 + ((m) << 2)) /* Video n Memory Base m Register */
-#define VNIE_REG	0x40	/* Video n Interrupt Enable Register */
-#define VNINTS_REG	0x44	/* Video n Interrupt Status Register */
-#define VNSI_REG	0x48	/* Video n Scanline Interrupt Register */
-#define VNMTC_REG	0x4C	/* Video n Memory Transfer Control Register */
-#define VNYS_REG	0x50	/* Video n Y Scale Register */
-#define VNXS_REG	0x54	/* Video n X Scale Register */
-#define VNDMR_REG	0x58	/* Video n Data Mode Register */
-#define VNDMR2_REG	0x5C	/* Video n Data Mode Register 2 */
-#define VNUVAOF_REG	0x60	/* Video n UV Address Offset Register */
-#define VNC1A_REG	0x80	/* Video n Coefficient Set C1A Register */
-#define VNC1B_REG	0x84	/* Video n Coefficient Set C1B Register */
-#define VNC1C_REG	0x88	/* Video n Coefficient Set C1C Register */
-#define VNC2A_REG	0x90	/* Video n Coefficient Set C2A Register */
-#define VNC2B_REG	0x94	/* Video n Coefficient Set C2B Register */
-#define VNC2C_REG	0x98	/* Video n Coefficient Set C2C Register */
-#define VNC3A_REG	0xA0	/* Video n Coefficient Set C3A Register */
-#define VNC3B_REG	0xA4	/* Video n Coefficient Set C3B Register */
-#define VNC3C_REG	0xA8	/* Video n Coefficient Set C3C Register */
-#define VNC4A_REG	0xB0	/* Video n Coefficient Set C4A Register */
-#define VNC4B_REG	0xB4	/* Video n Coefficient Set C4B Register */
-#define VNC4C_REG	0xB8	/* Video n Coefficient Set C4C Register */
-#define VNC5A_REG	0xC0	/* Video n Coefficient Set C5A Register */
-#define VNC5B_REG	0xC4	/* Video n Coefficient Set C5B Register */
-#define VNC5C_REG	0xC8	/* Video n Coefficient Set C5C Register */
-#define VNC6A_REG	0xD0	/* Video n Coefficient Set C6A Register */
-#define VNC6B_REG	0xD4	/* Video n Coefficient Set C6B Register */
-#define VNC6C_REG	0xD8	/* Video n Coefficient Set C6C Register */
-#define VNC7A_REG	0xE0	/* Video n Coefficient Set C7A Register */
-#define VNC7B_REG	0xE4	/* Video n Coefficient Set C7B Register */
-#define VNC7C_REG	0xE8	/* Video n Coefficient Set C7C Register */
-#define VNC8A_REG	0xF0	/* Video n Coefficient Set C8A Register */
-#define VNC8B_REG	0xF4	/* Video n Coefficient Set C8B Register */
-#define VNC8C_REG	0xF8	/* Video n Coefficient Set C8C Register */
-
-/* Register bit fields for R-Car VIN */
-/* Video n Main Control Register bits */
-#define VNMC_FOC		(1 << 21)
-#define VNMC_YCAL		(1 << 19)
-#define VNMC_INF_YUV8_BT656	(0 << 16)
-#define VNMC_INF_YUV8_BT601	(1 << 16)
-#define VNMC_INF_YUV10_BT656	(2 << 16)
-#define VNMC_INF_YUV10_BT601	(3 << 16)
-#define VNMC_INF_YUV16		(5 << 16)
-#define VNMC_INF_RGB888		(6 << 16)
-#define VNMC_VUP		(1 << 10)
-#define VNMC_IM_ODD		(0 << 3)
-#define VNMC_IM_ODD_EVEN	(1 << 3)
-#define VNMC_IM_EVEN		(2 << 3)
-#define VNMC_IM_FULL		(3 << 3)
-#define VNMC_BPS		(1 << 1)
-#define VNMC_ME			(1 << 0)
-
-/* Video n Module Status Register bits */
-#define VNMS_FBS_MASK		(3 << 3)
-#define VNMS_FBS_SHIFT		3
-#define VNMS_AV			(1 << 1)
-#define VNMS_CA			(1 << 0)
-
-/* Video n Frame Capture Register bits */
-#define VNFC_C_FRAME		(1 << 1)
-#define VNFC_S_FRAME		(1 << 0)
-
-/* Video n Interrupt Enable Register bits */
-#define VNIE_FIE		(1 << 4)
-#define VNIE_EFE		(1 << 1)
-
-/* Video n Data Mode Register bits */
-#define VNDMR_EXRGB		(1 << 8)
-#define VNDMR_BPSM		(1 << 4)
-#define VNDMR_DTMD_YCSEP	(1 << 1)
-#define VNDMR_DTMD_ARGB		(1 << 0)
-
-/* Video n Data Mode Register 2 bits */
-#define VNDMR2_VPS		(1 << 30)
-#define VNDMR2_HPS		(1 << 29)
-#define VNDMR2_FTEV		(1 << 17)
-#define VNDMR2_VLV(n)		((n & 0xf) << 12)
-
-#define VIN_MAX_WIDTH		2048
-#define VIN_MAX_HEIGHT		2048
-
-#define TIMEOUT_MS		100
-
-#define RCAR_VIN_HSYNC_ACTIVE_LOW	(1 << 0)
-#define RCAR_VIN_VSYNC_ACTIVE_LOW	(1 << 1)
-#define RCAR_VIN_BT601			(1 << 2)
-#define RCAR_VIN_BT656			(1 << 3)
-
-enum chip_id {
-	RCAR_GEN3,
-	RCAR_GEN2,
-	RCAR_H1,
-	RCAR_M1,
-	RCAR_E1,
-};
-
-struct vin_coeff {
-	unsigned short xs_value;
-	u32 coeff_set[24];
-};
-
-static const struct vin_coeff vin_coeff_set[] = {
-	{ 0x0000, {
-		0x00000000,		0x00000000,		0x00000000,
-		0x00000000,		0x00000000,		0x00000000,
-		0x00000000,		0x00000000,		0x00000000,
-		0x00000000,		0x00000000,		0x00000000,
-		0x00000000,		0x00000000,		0x00000000,
-		0x00000000,		0x00000000,		0x00000000,
-		0x00000000,		0x00000000,		0x00000000,
-		0x00000000,		0x00000000,		0x00000000 },
-	},
-	{ 0x1000, {
-		0x000fa400,		0x000fa400,		0x09625902,
-		0x000003f8,		0x00000403,		0x3de0d9f0,
-		0x001fffed,		0x00000804,		0x3cc1f9c3,
-		0x001003de,		0x00000c01,		0x3cb34d7f,
-		0x002003d2,		0x00000c00,		0x3d24a92d,
-		0x00200bca,		0x00000bff,		0x3df600d2,
-		0x002013cc,		0x000007ff,		0x3ed70c7e,
-		0x00100fde,		0x00000000,		0x3f87c036 },
-	},
-	{ 0x1200, {
-		0x002ffff1,		0x002ffff1,		0x02a0a9c8,
-		0x002003e7,		0x001ffffa,		0x000185bc,
-		0x002007dc,		0x000003ff,		0x3e52859c,
-		0x00200bd4,		0x00000002,		0x3d53996b,
-		0x00100fd0,		0x00000403,		0x3d04ad2d,
-		0x00000bd5,		0x00000403,		0x3d35ace7,
-		0x3ff003e4,		0x00000801,		0x3dc674a1,
-		0x3fffe800,		0x00000800,		0x3e76f461 },
-	},
-	{ 0x1400, {
-		0x00100be3,		0x00100be3,		0x04d1359a,
-		0x00000fdb,		0x002003ed,		0x0211fd93,
-		0x00000fd6,		0x002003f4,		0x0002d97b,
-		0x000007d6,		0x002ffffb,		0x3e93b956,
-		0x3ff003da,		0x001003ff,		0x3db49926,
-		0x3fffefe9,		0x00100001,		0x3d655cee,
-		0x3fffd400,		0x00000003,		0x3d65f4b6,
-		0x000fb421,		0x00000402,		0x3dc6547e },
-	},
-	{ 0x1600, {
-		0x00000bdd,		0x00000bdd,		0x06519578,
-		0x3ff007da,		0x00000be3,		0x03c24973,
-		0x3ff003d9,		0x00000be9,		0x01b30d5f,
-		0x3ffff7df,		0x001003f1,		0x0003c542,
-		0x000fdfec,		0x001003f7,		0x3ec4711d,
-		0x000fc400,		0x002ffffd,		0x3df504f1,
-		0x001fa81a,		0x002ffc00,		0x3d957cc2,
-		0x002f8c3c,		0x00100000,		0x3db5c891 },
-	},
-	{ 0x1800, {
-		0x3ff003dc,		0x3ff003dc,		0x0791e558,
-		0x000ff7dd,		0x3ff007de,		0x05328554,
-		0x000fe7e3,		0x3ff00be2,		0x03232546,
-		0x000fd7ee,		0x000007e9,		0x0143bd30,
-		0x001fb800,		0x000007ee,		0x00044511,
-		0x002fa015,		0x000007f4,		0x3ef4bcee,
-		0x002f8832,		0x001003f9,		0x3e4514c7,
-		0x001f7853,		0x001003fd,		0x3de54c9f },
-	},
-	{ 0x1a00, {
-		0x000fefe0,		0x000fefe0,		0x08721d3c,
-		0x001fdbe7,		0x000ffbde,		0x0652a139,
-		0x001fcbf0,		0x000003df,		0x0463292e,
-		0x002fb3ff,		0x3ff007e3,		0x0293a91d,
-		0x002f9c12,		0x3ff00be7,		0x01241905,
-		0x001f8c29,		0x000007ed,		0x3fe470eb,
-		0x000f7c46,		0x000007f2,		0x3f04b8ca,
-		0x3fef7865,		0x000007f6,		0x3e74e4a8 },
-	},
-	{ 0x1c00, {
-		0x001fd3e9,		0x001fd3e9,		0x08f23d26,
-		0x002fbff3,		0x001fe3e4,		0x0712ad23,
-		0x002fa800,		0x000ff3e0,		0x05631d1b,
-		0x001f9810,		0x000ffbe1,		0x03b3890d,
-		0x000f8c23,		0x000003e3,		0x0233e8fa,
-		0x3fef843b,		0x000003e7,		0x00f430e4,
-		0x3fbf8456,		0x3ff00bea,		0x00046cc8,
-		0x3f8f8c72,		0x3ff00bef,		0x3f3490ac },
-	},
-	{ 0x1e00, {
-		0x001fbbf4,		0x001fbbf4,		0x09425112,
-		0x001fa800,		0x002fc7ed,		0x0792b110,
-		0x000f980e,		0x001fdbe6,		0x0613110a,
-		0x3fff8c20,		0x001fe7e3,		0x04a368fd,
-		0x3fcf8c33,		0x000ff7e2,		0x0343b8ed,
-		0x3f9f8c4a,		0x000fffe3,		0x0203f8da,
-		0x3f5f9c61,		0x000003e6,		0x00e428c5,
-		0x3f1fb07b,		0x000003eb,		0x3fe440af },
-	},
-	{ 0x2000, {
-		0x000fa400,		0x000fa400,		0x09625902,
-		0x3fff980c,		0x001fb7f5,		0x0812b0ff,
-		0x3fdf901c,		0x001fc7ed,		0x06b2fcfa,
-		0x3faf902d,		0x001fd3e8,		0x055348f1,
-		0x3f7f983f,		0x001fe3e5,		0x04038ce3,
-		0x3f3fa454,		0x001fefe3,		0x02e3c8d1,
-		0x3f0fb86a,		0x001ff7e4,		0x01c3e8c0,
-		0x3ecfd880,		0x000fffe6,		0x00c404ac },
-	},
-	{ 0x2200, {
-		0x3fdf9c0b,		0x3fdf9c0b,		0x09725cf4,
-		0x3fbf9818,		0x3fffa400,		0x0842a8f1,
-		0x3f8f9827,		0x000fb3f7,		0x0702f0ec,
-		0x3f5fa037,		0x000fc3ef,		0x05d330e4,
-		0x3f2fac49,		0x001fcfea,		0x04a364d9,
-		0x3effc05c,		0x001fdbe7,		0x038394ca,
-		0x3ecfdc6f,		0x001fe7e6,		0x0273b0bb,
-		0x3ea00083,		0x001fefe6,		0x0183c0a9 },
-	},
-	{ 0x2400, {
-		0x3f9fa014,		0x3f9fa014,		0x098260e6,
-		0x3f7f9c23,		0x3fcf9c0a,		0x08629ce5,
-		0x3f4fa431,		0x3fefa400,		0x0742d8e1,
-		0x3f1fb440,		0x3fffb3f8,		0x062310d9,
-		0x3eefc850,		0x000fbbf2,		0x050340d0,
-		0x3ecfe062,		0x000fcbec,		0x041364c2,
-		0x3ea00073,		0x001fd3ea,		0x03037cb5,
-		0x3e902086,		0x001fdfe8,		0x022388a5 },
-	},
-	{ 0x2600, {
-		0x3f5fa81e,		0x3f5fa81e,		0x096258da,
-		0x3f3fac2b,		0x3f8fa412,		0x088290d8,
-		0x3f0fbc38,		0x3fafa408,		0x0772c8d5,
-		0x3eefcc47,		0x3fcfa800,		0x0672f4ce,
-		0x3ecfe456,		0x3fefaffa,		0x05531cc6,
-		0x3eb00066,		0x3fffbbf3,		0x047334bb,
-		0x3ea01c77,		0x000fc7ee,		0x039348ae,
-		0x3ea04486,		0x000fd3eb,		0x02b350a1 },
-	},
-	{ 0x2800, {
-		0x3f2fb426,		0x3f2fb426,		0x094250ce,
-		0x3f0fc032,		0x3f4fac1b,		0x086284cd,
-		0x3eefd040,		0x3f7fa811,		0x0782acc9,
-		0x3ecfe84c,		0x3f9fa807,		0x06a2d8c4,
-		0x3eb0005b,		0x3fbfac00,		0x05b2f4bc,
-		0x3eb0186a,		0x3fdfb3fa,		0x04c308b4,
-		0x3eb04077,		0x3fefbbf4,		0x03f31ca8,
-		0x3ec06884,		0x000fbff2,		0x03031c9e },
-	},
-	{ 0x2a00, {
-		0x3f0fc42d,		0x3f0fc42d,		0x090240c4,
-		0x3eefd439,		0x3f2fb822,		0x08526cc2,
-		0x3edfe845,		0x3f4fb018,		0x078294bf,
-		0x3ec00051,		0x3f6fac0f,		0x06b2b4bb,
-		0x3ec0185f,		0x3f8fac07,		0x05e2ccb4,
-		0x3ec0386b,		0x3fafac00,		0x0502e8ac,
-		0x3ed05c77,		0x3fcfb3fb,		0x0432f0a3,
-		0x3ef08482,		0x3fdfbbf6,		0x0372f898 },
-	},
-	{ 0x2c00, {
-		0x3eefdc31,		0x3eefdc31,		0x08e238b8,
-		0x3edfec3d,		0x3f0fc828,		0x082258b9,
-		0x3ed00049,		0x3f1fc01e,		0x077278b6,
-		0x3ed01455,		0x3f3fb815,		0x06c294b2,
-		0x3ed03460,		0x3f5fb40d,		0x0602acac,
-		0x3ef0506c,		0x3f7fb006,		0x0542c0a4,
-		0x3f107476,		0x3f9fb400,		0x0472c89d,
-		0x3f309c80,		0x3fbfb7fc,		0x03b2cc94 },
-	},
-	{ 0x2e00, {
-		0x3eefec37,		0x3eefec37,		0x088220b0,
-		0x3ee00041,		0x3effdc2d,		0x07f244ae,
-		0x3ee0144c,		0x3f0fd023,		0x07625cad,
-		0x3ef02c57,		0x3f1fc81a,		0x06c274a9,
-		0x3f004861,		0x3f3fbc13,		0x060288a6,
-		0x3f20686b,		0x3f5fb80c,		0x05529c9e,
-		0x3f408c74,		0x3f6fb805,		0x04b2ac96,
-		0x3f80ac7e,		0x3f8fb800,		0x0402ac8e },
-	},
-	{ 0x3000, {
-		0x3ef0003a,		0x3ef0003a,		0x084210a6,
-		0x3ef01045,		0x3effec32,		0x07b228a7,
-		0x3f00284e,		0x3f0fdc29,		0x073244a4,
-		0x3f104058,		0x3f0fd420,		0x06a258a2,
-		0x3f305c62,		0x3f2fc818,		0x0612689d,
-		0x3f508069,		0x3f3fc011,		0x05728496,
-		0x3f80a072,		0x3f4fc00a,		0x04d28c90,
-		0x3fc0c07b,		0x3f6fbc04,		0x04429088 },
-	},
-	{ 0x3200, {
-		0x3f00103e,		0x3f00103e,		0x07f1fc9e,
-		0x3f102447,		0x3f000035,		0x0782149d,
-		0x3f203c4f,		0x3f0ff02c,		0x07122c9c,
-		0x3f405458,		0x3f0fe424,		0x06924099,
-		0x3f607061,		0x3f1fd41d,		0x06024c97,
-		0x3f909068,		0x3f2fcc16,		0x05726490,
-		0x3fc0b070,		0x3f3fc80f,		0x04f26c8a,
-		0x0000d077,		0x3f4fc409,		0x04627484 },
-	},
-	{ 0x3400, {
-		0x3f202040,		0x3f202040,		0x07a1e898,
-		0x3f303449,		0x3f100c38,		0x0741fc98,
-		0x3f504c50,		0x3f10002f,		0x06e21495,
-		0x3f706459,		0x3f1ff028,		0x06722492,
-		0x3fa08060,		0x3f1fe421,		0x05f2348f,
-		0x3fd09c67,		0x3f1fdc19,		0x05824c89,
-		0x0000bc6e,		0x3f2fd014,		0x04f25086,
-		0x0040dc74,		0x3f3fcc0d,		0x04825c7f },
-	},
-	{ 0x3600, {
-		0x3f403042,		0x3f403042,		0x0761d890,
-		0x3f504848,		0x3f301c3b,		0x0701f090,
-		0x3f805c50,		0x3f200c33,		0x06a2008f,
-		0x3fa07458,		0x3f10002b,		0x06520c8d,
-		0x3fd0905e,		0x3f1ff424,		0x05e22089,
-		0x0000ac65,		0x3f1fe81d,		0x05823483,
-		0x0030cc6a,		0x3f2fdc18,		0x04f23c81,
-		0x0080e871,		0x3f2fd412,		0x0482407c },
-	},
-	{ 0x3800, {
-		0x3f604043,		0x3f604043,		0x0721c88a,
-		0x3f80544a,		0x3f502c3c,		0x06d1d88a,
-		0x3fb06851,		0x3f301c35,		0x0681e889,
-		0x3fd08456,		0x3f30082f,		0x0611fc88,
-		0x00009c5d,		0x3f200027,		0x05d20884,
-		0x0030b863,		0x3f2ff421,		0x05621880,
-		0x0070d468,		0x3f2fe81b,		0x0502247c,
-		0x00c0ec6f,		0x3f2fe015,		0x04a22877 },
-	},
-	{ 0x3a00, {
-		0x3f904c44,		0x3f904c44,		0x06e1b884,
-		0x3fb0604a,		0x3f70383e,		0x0691c885,
-		0x3fe07451,		0x3f502c36,		0x0661d483,
-		0x00009055,		0x3f401831,		0x0601ec81,
-		0x0030a85b,		0x3f300c2a,		0x05b1f480,
-		0x0070c061,		0x3f300024,		0x0562047a,
-		0x00b0d867,		0x3f3ff41e,		0x05020c77,
-		0x00f0f46b,		0x3f2fec19,		0x04a21474 },
-	},
-	{ 0x3c00, {
-		0x3fb05c43,		0x3fb05c43,		0x06c1b07e,
-		0x3fe06c4b,		0x3f902c3f,		0x0681c081,
-		0x0000844f,		0x3f703838,		0x0631cc7d,
-		0x00309855,		0x3f602433,		0x05d1d47e,
-		0x0060b459,		0x3f50142e,		0x0581e47b,
-		0x00a0c85f,		0x3f400828,		0x0531f078,
-		0x00e0e064,		0x3f300021,		0x0501fc73,
-		0x00b0fc6a,		0x3f3ff41d,		0x04a20873 },
-	},
-	{ 0x3e00, {
-		0x3fe06444,		0x3fe06444,		0x0681a07a,
-		0x00007849,		0x3fc0503f,		0x0641b07a,
-		0x0020904d,		0x3fa0403a,		0x05f1c07a,
-		0x0060a453,		0x3f803034,		0x05c1c878,
-		0x0090b858,		0x3f70202f,		0x0571d477,
-		0x00d0d05d,		0x3f501829,		0x0531e073,
-		0x0110e462,		0x3f500825,		0x04e1e471,
-		0x01510065,		0x3f40001f,		0x04a1f06d },
-	},
-	{ 0x4000, {
-		0x00007044,		0x00007044,		0x06519476,
-		0x00208448,		0x3fe05c3f,		0x0621a476,
-		0x0050984d,		0x3fc04c3a,		0x05e1b075,
-		0x0080ac52,		0x3fa03c35,		0x05a1b875,
-		0x00c0c056,		0x3f803030,		0x0561c473,
-		0x0100d45b,		0x3f70202b,		0x0521d46f,
-		0x0140e860,		0x3f601427,		0x04d1d46e,
-		0x01810064,		0x3f500822,		0x0491dc6b },
-	},
-	{ 0x5000, {
-		0x0110a442,		0x0110a442,		0x0551545e,
-		0x0140b045,		0x00e0983f,		0x0531585f,
-		0x0160c047,		0x00c08c3c,		0x0511645e,
-		0x0190cc4a,		0x00908039,		0x04f1685f,
-		0x01c0dc4c,		0x00707436,		0x04d1705e,
-		0x0200e850,		0x00506833,		0x04b1785b,
-		0x0230f453,		0x00305c30,		0x0491805a,
-		0x02710056,		0x0010542d,		0x04718059 },
-	},
-	{ 0x6000, {
-		0x01c0bc40,		0x01c0bc40,		0x04c13052,
-		0x01e0c841,		0x01a0b43d,		0x04c13851,
-		0x0210cc44,		0x0180a83c,		0x04a13453,
-		0x0230d845,		0x0160a03a,		0x04913c52,
-		0x0260e047,		0x01409838,		0x04714052,
-		0x0280ec49,		0x01208c37,		0x04514c50,
-		0x02b0f44b,		0x01008435,		0x04414c50,
-		0x02d1004c,		0x00e07c33,		0x0431544f },
-	},
-	{ 0x7000, {
-		0x0230c83e,		0x0230c83e,		0x04711c4c,
-		0x0250d03f,		0x0210c43c,		0x0471204b,
-		0x0270d840,		0x0200b83c,		0x0451244b,
-		0x0290dc42,		0x01e0b43a,		0x0441244c,
-		0x02b0e443,		0x01c0b038,		0x0441284b,
-		0x02d0ec44,		0x01b0a438,		0x0421304a,
-		0x02f0f445,		0x0190a036,		0x04213449,
-		0x0310f847,		0x01709c34,		0x04213848 },
-	},
-	{ 0x8000, {
-		0x0280d03d,		0x0280d03d,		0x04310c48,
-		0x02a0d43e,		0x0270c83c,		0x04311047,
-		0x02b0dc3e,		0x0250c83a,		0x04311447,
-		0x02d0e040,		0x0240c03a,		0x04211446,
-		0x02e0e840,		0x0220bc39,		0x04111847,
-		0x0300e842,		0x0210b438,		0x04012445,
-		0x0310f043,		0x0200b037,		0x04012045,
-		0x0330f444,		0x01e0ac36,		0x03f12445 },
-	},
-	{ 0xefff, {
-		0x0340dc3a,		0x0340dc3a,		0x03b0ec40,
-		0x0340e03a,		0x0330e039,		0x03c0f03e,
-		0x0350e03b,		0x0330dc39,		0x03c0ec3e,
-		0x0350e43a,		0x0320dc38,		0x03c0f43e,
-		0x0360e43b,		0x0320d839,		0x03b0f03e,
-		0x0360e83b,		0x0310d838,		0x03c0fc3b,
-		0x0370e83b,		0x0310d439,		0x03a0f83d,
-		0x0370e83c,		0x0300d438,		0x03b0fc3c },
-	}
-};
-
-enum rcar_vin_state {
-	STOPPED = 0,
-	RUNNING,
-	STOPPING,
-};
-
-struct rcar_vin_priv {
-	void __iomem			*base;
-	spinlock_t			lock;
-	int				sequence;
-	/* State of the VIN module in capturing mode */
-	enum rcar_vin_state		state;
-	struct soc_camera_host		ici;
-	struct list_head		capture;
-#define MAX_BUFFER_NUM			3
-	struct vb2_v4l2_buffer		*queue_buf[MAX_BUFFER_NUM];
-	enum v4l2_field			field;
-	unsigned int			pdata_flags;
-	unsigned int			vb_count;
-	unsigned int			nr_hw_slots;
-	bool				request_to_stop;
-	struct completion		capture_stop;
-	enum chip_id			chip;
-};
-
-#define is_continuous_transfer(priv)	(priv->vb_count > MAX_BUFFER_NUM)
-
-struct rcar_vin_buffer {
-	struct vb2_v4l2_buffer vb;
-	struct list_head		list;
-};
-
-#define to_buf_list(vb2_buffer)	(&container_of(vb2_buffer, \
-						       struct rcar_vin_buffer, \
-						       vb)->list)
-
-struct rcar_vin_cam {
-	/* VIN offsets within the camera output, before the VIN scaler */
-	unsigned int			vin_left;
-	unsigned int			vin_top;
-	/* Client output, as seen by the VIN */
-	unsigned int			width;
-	unsigned int			height;
-	/* User window from S_FMT */
-	unsigned int out_width;
-	unsigned int out_height;
-	/*
-	 * User window from S_CROP / G_CROP, produced by client cropping and
-	 * scaling, VIN scaling and VIN cropping, mapped back onto the client
-	 * input window
-	 */
-	struct v4l2_rect		subrect;
-	/* Camera cropping rectangle */
-	struct v4l2_rect		rect;
-	const struct soc_mbus_pixelfmt	*extra_fmt;
-};
-
-/*
- * .queue_setup() is called to check whether the driver can accept the requested
- * number of buffers and to fill in plane sizes for the current frame format if
- * required
- */
-static int rcar_vin_videobuf_setup(struct vb2_queue *vq,
-				   unsigned int *count,
-				   unsigned int *num_planes,
-				   unsigned int sizes[], struct device *alloc_devs[])
-{
-	struct soc_camera_device *icd = soc_camera_from_vb2q(vq);
-	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
-	struct rcar_vin_priv *priv = ici->priv;
-
-	if (!vq->num_buffers)
-		priv->sequence = 0;
-
-	if (!*count)
-		*count = 2;
-	priv->vb_count = *count;
-
-	/* Number of hardware slots */
-	if (is_continuous_transfer(priv))
-		priv->nr_hw_slots = MAX_BUFFER_NUM;
-	else
-		priv->nr_hw_slots = 1;
-
-	if (*num_planes)
-		return sizes[0] < icd->sizeimage ? -EINVAL : 0;
-
-	sizes[0] = icd->sizeimage;
-	*num_planes = 1;
-
-	dev_dbg(icd->parent, "count=%d, size=%u\n", *count, sizes[0]);
-
-	return 0;
-}
-
-static int rcar_vin_setup(struct rcar_vin_priv *priv)
-{
-	struct soc_camera_device *icd = priv->ici.icd;
-	struct rcar_vin_cam *cam = icd->host_priv;
-	u32 vnmc, dmr, interrupts;
-	bool progressive = false, output_is_yuv = false, input_is_yuv = false;
-
-	switch (priv->field) {
-	case V4L2_FIELD_TOP:
-		vnmc = VNMC_IM_ODD;
-		break;
-	case V4L2_FIELD_BOTTOM:
-		vnmc = VNMC_IM_EVEN;
-		break;
-	case V4L2_FIELD_INTERLACED:
-	case V4L2_FIELD_INTERLACED_TB:
-		vnmc = VNMC_IM_FULL;
-		break;
-	case V4L2_FIELD_INTERLACED_BT:
-		vnmc = VNMC_IM_FULL | VNMC_FOC;
-		break;
-	case V4L2_FIELD_NONE:
-		if (is_continuous_transfer(priv)) {
-			vnmc = VNMC_IM_ODD_EVEN;
-			progressive = true;
-		} else {
-			vnmc = VNMC_IM_ODD;
-		}
-		break;
-	default:
-		vnmc = VNMC_IM_ODD;
-		break;
-	}
-
-	/* input interface */
-	switch (icd->current_fmt->code) {
-	case MEDIA_BUS_FMT_YUYV8_1X16:
-		/* BT.601/BT.1358 16bit YCbCr422 */
-		vnmc |= VNMC_INF_YUV16;
-		input_is_yuv = true;
-		break;
-	case MEDIA_BUS_FMT_YUYV8_2X8:
-		/* BT.656 8bit YCbCr422 or BT.601 8bit YCbCr422 */
-		vnmc |= priv->pdata_flags & RCAR_VIN_BT656 ?
-			VNMC_INF_YUV8_BT656 : VNMC_INF_YUV8_BT601;
-		input_is_yuv = true;
-		break;
-	case MEDIA_BUS_FMT_RGB888_1X24:
-		vnmc |= VNMC_INF_RGB888;
-		break;
-	case MEDIA_BUS_FMT_YUYV10_2X10:
-		/* BT.656 10bit YCbCr422 or BT.601 10bit YCbCr422 */
-		vnmc |= priv->pdata_flags & RCAR_VIN_BT656 ?
-			VNMC_INF_YUV10_BT656 : VNMC_INF_YUV10_BT601;
-		input_is_yuv = true;
-		break;
-	default:
-		break;
-	}
-
-	/* output format */
-	switch (icd->current_fmt->host_fmt->fourcc) {
-	case V4L2_PIX_FMT_NV16:
-		iowrite32(ALIGN(cam->width * cam->height, 0x80),
-			  priv->base + VNUVAOF_REG);
-		dmr = VNDMR_DTMD_YCSEP;
-		output_is_yuv = true;
-		break;
-	case V4L2_PIX_FMT_YUYV:
-		dmr = VNDMR_BPSM;
-		output_is_yuv = true;
-		break;
-	case V4L2_PIX_FMT_UYVY:
-		dmr = 0;
-		output_is_yuv = true;
-		break;
-	case V4L2_PIX_FMT_RGB555X:
-		dmr = VNDMR_DTMD_ARGB;
-		break;
-	case V4L2_PIX_FMT_RGB565:
-		dmr = 0;
-		break;
-	case V4L2_PIX_FMT_RGB32:
-		if (priv->chip != RCAR_GEN2 && priv->chip != RCAR_H1 &&
-		    priv->chip != RCAR_E1)
-			goto e_format;
-
-		dmr = VNDMR_EXRGB;
-		break;
-	case V4L2_PIX_FMT_ARGB32:
-		if (priv->chip != RCAR_GEN3)
-			goto e_format;
-
-		dmr = VNDMR_EXRGB | VNDMR_DTMD_ARGB;
-		break;
-	default:
-		goto e_format;
-	}
-
-	/* Always update on field change */
-	vnmc |= VNMC_VUP;
-
-	/* If input and output use the same colorspace, use bypass mode */
-	if (input_is_yuv == output_is_yuv)
-		vnmc |= VNMC_BPS;
-
-	/* progressive or interlaced mode */
-	interrupts = progressive ? VNIE_FIE : VNIE_EFE;
-
-	/* ack interrupts */
-	iowrite32(interrupts, priv->base + VNINTS_REG);
-	/* enable interrupts */
-	iowrite32(interrupts, priv->base + VNIE_REG);
-	/* start capturing */
-	iowrite32(dmr, priv->base + VNDMR_REG);
-	iowrite32(vnmc | VNMC_ME, priv->base + VNMC_REG);
-
-	return 0;
-
-e_format:
-	dev_warn(icd->parent, "Invalid fourcc format (0x%x)\n",
-		 icd->current_fmt->host_fmt->fourcc);
-	return -EINVAL;
-}
-
-static void rcar_vin_capture(struct rcar_vin_priv *priv)
-{
-	if (is_continuous_transfer(priv))
-		/* Continuous Frame Capture Mode */
-		iowrite32(VNFC_C_FRAME, priv->base + VNFC_REG);
-	else
-		/* Single Frame Capture Mode */
-		iowrite32(VNFC_S_FRAME, priv->base + VNFC_REG);
-}
-
-static void rcar_vin_request_capture_stop(struct rcar_vin_priv *priv)
-{
-	priv->state = STOPPING;
-
-	/* set continuous & single transfer off */
-	iowrite32(0, priv->base + VNFC_REG);
-	/* disable capture (release DMA buffer), reset */
-	iowrite32(ioread32(priv->base + VNMC_REG) & ~VNMC_ME,
-		  priv->base + VNMC_REG);
-
-	/* update the status if stopped already */
-	if (!(ioread32(priv->base + VNMS_REG) & VNMS_CA))
-		priv->state = STOPPED;
-}
-
-static int rcar_vin_get_free_hw_slot(struct rcar_vin_priv *priv)
-{
-	int slot;
-
-	for (slot = 0; slot < priv->nr_hw_slots; slot++)
-		if (priv->queue_buf[slot] == NULL)
-			return slot;
-
-	return -1;
-}
-
-static int rcar_vin_hw_ready(struct rcar_vin_priv *priv)
-{
-	/* Ensure all HW slots are filled */
-	return rcar_vin_get_free_hw_slot(priv) < 0 ? 1 : 0;
-}
-
-/* Moves a buffer from the queue to the HW slots */
-static int rcar_vin_fill_hw_slot(struct rcar_vin_priv *priv)
-{
-	struct vb2_v4l2_buffer *vbuf;
-	dma_addr_t phys_addr_top;
-	int slot;
-
-	if (list_empty(&priv->capture))
-		return 0;
-
-	/* Find a free HW slot */
-	slot = rcar_vin_get_free_hw_slot(priv);
-	if (slot < 0)
-		return 0;
-
-	vbuf = &list_entry(priv->capture.next,
-			struct rcar_vin_buffer, list)->vb;
-	list_del_init(to_buf_list(vbuf));
-	priv->queue_buf[slot] = vbuf;
-	phys_addr_top = vb2_dma_contig_plane_dma_addr(&vbuf->vb2_buf, 0);
-	iowrite32(phys_addr_top, priv->base + VNMB_REG(slot));
-
-	return 1;
-}
-
-static void rcar_vin_videobuf_queue(struct vb2_buffer *vb)
-{
-	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
-	struct soc_camera_device *icd = soc_camera_from_vb2q(vb->vb2_queue);
-	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
-	struct rcar_vin_priv *priv = ici->priv;
-	unsigned long size;
-
-	size = icd->sizeimage;
-
-	if (vb2_plane_size(vb, 0) < size) {
-		dev_err(icd->parent, "Buffer #%d too small (%lu < %lu)\n",
-			vb->index, vb2_plane_size(vb, 0), size);
-		goto error;
-	}
-
-	vb2_set_plane_payload(vb, 0, size);
-
-	dev_dbg(icd->parent, "%s (vb=0x%p) 0x%p %lu\n", __func__,
-		vb, vb2_plane_vaddr(vb, 0), vb2_get_plane_payload(vb, 0));
-
-	spin_lock_irq(&priv->lock);
-
-	list_add_tail(to_buf_list(vbuf), &priv->capture);
-	rcar_vin_fill_hw_slot(priv);
-
-	/* If we weren't running, and have enough buffers, start capturing! */
-	if (priv->state != RUNNING && rcar_vin_hw_ready(priv)) {
-		if (rcar_vin_setup(priv)) {
-			/* Submit error */
-			list_del_init(to_buf_list(vbuf));
-			spin_unlock_irq(&priv->lock);
-			goto error;
-		}
-		priv->request_to_stop = false;
-		init_completion(&priv->capture_stop);
-		priv->state = RUNNING;
-		rcar_vin_capture(priv);
-	}
-
-	spin_unlock_irq(&priv->lock);
-
-	return;
-
-error:
-	vb2_buffer_done(vb, VB2_BUF_STATE_ERROR);
-}
-
-/*
- * Wait for capture to stop and all in-flight buffers to be finished with by
- * the video hardware. This must be called under &priv->lock
- *
- */
-static void rcar_vin_wait_stop_streaming(struct rcar_vin_priv *priv)
-{
-	while (priv->state != STOPPED) {
-		/* issue stop if running */
-		if (priv->state == RUNNING)
-			rcar_vin_request_capture_stop(priv);
-
-		/* wait until capturing has been stopped */
-		if (priv->state == STOPPING) {
-			priv->request_to_stop = true;
-			spin_unlock_irq(&priv->lock);
-			if (!wait_for_completion_timeout(
-					&priv->capture_stop,
-					msecs_to_jiffies(TIMEOUT_MS)))
-				priv->state = STOPPED;
-			spin_lock_irq(&priv->lock);
-		}
-	}
-}
-
-static void rcar_vin_stop_streaming(struct vb2_queue *vq)
-{
-	struct soc_camera_device *icd = soc_camera_from_vb2q(vq);
-	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
-	struct rcar_vin_priv *priv = ici->priv;
-	struct list_head *buf_head, *tmp;
-	int i;
-
-	spin_lock_irq(&priv->lock);
-	rcar_vin_wait_stop_streaming(priv);
-
-	for (i = 0; i < MAX_BUFFER_NUM; i++) {
-		if (priv->queue_buf[i]) {
-			vb2_buffer_done(&priv->queue_buf[i]->vb2_buf,
-					VB2_BUF_STATE_ERROR);
-			priv->queue_buf[i] = NULL;
-		}
-	}
-
-	list_for_each_safe(buf_head, tmp, &priv->capture) {
-		vb2_buffer_done(&list_entry(buf_head,
-				struct rcar_vin_buffer, list)->vb.vb2_buf,
-				VB2_BUF_STATE_ERROR);
-		list_del_init(buf_head);
-	}
-	spin_unlock_irq(&priv->lock);
-}
-
-static struct vb2_ops rcar_vin_vb2_ops = {
-	.queue_setup	= rcar_vin_videobuf_setup,
-	.buf_queue	= rcar_vin_videobuf_queue,
-	.stop_streaming	= rcar_vin_stop_streaming,
-	.wait_prepare	= vb2_ops_wait_prepare,
-	.wait_finish	= vb2_ops_wait_finish,
-};
-
-static irqreturn_t rcar_vin_irq(int irq, void *data)
-{
-	struct rcar_vin_priv *priv = data;
-	u32 int_status;
-	bool can_run = false, hw_stopped;
-	int slot;
-	unsigned int handled = 0;
-
-	spin_lock(&priv->lock);
-
-	int_status = ioread32(priv->base + VNINTS_REG);
-	if (!int_status)
-		goto done;
-	/* ack interrupts */
-	iowrite32(int_status, priv->base + VNINTS_REG);
-	handled = 1;
-
-	/* nothing to do if capture status is 'STOPPED' */
-	if (priv->state == STOPPED)
-		goto done;
-
-	hw_stopped = !(ioread32(priv->base + VNMS_REG) & VNMS_CA);
-
-	if (!priv->request_to_stop) {
-		if (is_continuous_transfer(priv))
-			slot = (ioread32(priv->base + VNMS_REG) &
-				VNMS_FBS_MASK) >> VNMS_FBS_SHIFT;
-		else
-			slot = 0;
-
-		priv->queue_buf[slot]->field = priv->field;
-		priv->queue_buf[slot]->sequence = priv->sequence++;
-		priv->queue_buf[slot]->vb2_buf.timestamp = ktime_get_ns();
-		vb2_buffer_done(&priv->queue_buf[slot]->vb2_buf,
-				VB2_BUF_STATE_DONE);
-		priv->queue_buf[slot] = NULL;
-
-		if (priv->state != STOPPING)
-			can_run = rcar_vin_fill_hw_slot(priv);
-
-		if (hw_stopped || !can_run) {
-			priv->state = STOPPED;
-		} else if (is_continuous_transfer(priv) &&
-			   list_empty(&priv->capture) &&
-			   priv->state == RUNNING) {
-			/*
-			 * The continuous capturing requires an explicit stop
-			 * operation when there is no buffer to be set into
-			 * the VnMBm registers.
-			 */
-			rcar_vin_request_capture_stop(priv);
-		} else {
-			rcar_vin_capture(priv);
-		}
-
-	} else if (hw_stopped) {
-		priv->state = STOPPED;
-		priv->request_to_stop = false;
-		complete(&priv->capture_stop);
-	}
-
-done:
-	spin_unlock(&priv->lock);
-
-	return IRQ_RETVAL(handled);
-}
-
-static int rcar_vin_add_device(struct soc_camera_device *icd)
-{
-	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
-	struct rcar_vin_priv *priv = ici->priv;
-	int i;
-
-	for (i = 0; i < MAX_BUFFER_NUM; i++)
-		priv->queue_buf[i] = NULL;
-
-	pm_runtime_get_sync(ici->v4l2_dev.dev);
-
-	dev_dbg(icd->parent, "R-Car VIN driver attached to camera %d\n",
-		icd->devnum);
-
-	return 0;
-}
-
-static void rcar_vin_remove_device(struct soc_camera_device *icd)
-{
-	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
-	struct rcar_vin_priv *priv = ici->priv;
-	struct vb2_v4l2_buffer *vbuf;
-	int i;
-
-	/* disable capture, disable interrupts */
-	iowrite32(ioread32(priv->base + VNMC_REG) & ~VNMC_ME,
-		  priv->base + VNMC_REG);
-	iowrite32(0, priv->base + VNIE_REG);
-
-	priv->state = STOPPED;
-	priv->request_to_stop = false;
-
-	/* make sure active buffer is cancelled */
-	spin_lock_irq(&priv->lock);
-	for (i = 0; i < MAX_BUFFER_NUM; i++) {
-		vbuf = priv->queue_buf[i];
-		if (vbuf) {
-			list_del_init(to_buf_list(vbuf));
-			vb2_buffer_done(&vbuf->vb2_buf, VB2_BUF_STATE_ERROR);
-		}
-	}
-	spin_unlock_irq(&priv->lock);
-
-	pm_runtime_put(ici->v4l2_dev.dev);
-
-	dev_dbg(icd->parent, "R-Car VIN driver detached from camera %d\n",
-		icd->devnum);
-}
-
-static void set_coeff(struct rcar_vin_priv *priv, unsigned short xs)
-{
-	int i;
-	const struct vin_coeff *p_prev_set = NULL;
-	const struct vin_coeff *p_set = NULL;
-
-	/* Look for suitable coefficient values */
-	for (i = 0; i < ARRAY_SIZE(vin_coeff_set); i++) {
-		p_prev_set = p_set;
-		p_set = &vin_coeff_set[i];
-
-		if (xs < p_set->xs_value)
-			break;
-	}
-
-	/* Use previous value if its XS value is closer */
-	if (p_prev_set && p_set &&
-	    xs - p_prev_set->xs_value < p_set->xs_value - xs)
-		p_set = p_prev_set;
-
-	/* Set coefficient registers */
-	iowrite32(p_set->coeff_set[0], priv->base + VNC1A_REG);
-	iowrite32(p_set->coeff_set[1], priv->base + VNC1B_REG);
-	iowrite32(p_set->coeff_set[2], priv->base + VNC1C_REG);
-
-	iowrite32(p_set->coeff_set[3], priv->base + VNC2A_REG);
-	iowrite32(p_set->coeff_set[4], priv->base + VNC2B_REG);
-	iowrite32(p_set->coeff_set[5], priv->base + VNC2C_REG);
-
-	iowrite32(p_set->coeff_set[6], priv->base + VNC3A_REG);
-	iowrite32(p_set->coeff_set[7], priv->base + VNC3B_REG);
-	iowrite32(p_set->coeff_set[8], priv->base + VNC3C_REG);
-
-	iowrite32(p_set->coeff_set[9], priv->base + VNC4A_REG);
-	iowrite32(p_set->coeff_set[10], priv->base + VNC4B_REG);
-	iowrite32(p_set->coeff_set[11], priv->base + VNC4C_REG);
-
-	iowrite32(p_set->coeff_set[12], priv->base + VNC5A_REG);
-	iowrite32(p_set->coeff_set[13], priv->base + VNC5B_REG);
-	iowrite32(p_set->coeff_set[14], priv->base + VNC5C_REG);
-
-	iowrite32(p_set->coeff_set[15], priv->base + VNC6A_REG);
-	iowrite32(p_set->coeff_set[16], priv->base + VNC6B_REG);
-	iowrite32(p_set->coeff_set[17], priv->base + VNC6C_REG);
-
-	iowrite32(p_set->coeff_set[18], priv->base + VNC7A_REG);
-	iowrite32(p_set->coeff_set[19], priv->base + VNC7B_REG);
-	iowrite32(p_set->coeff_set[20], priv->base + VNC7C_REG);
-
-	iowrite32(p_set->coeff_set[21], priv->base + VNC8A_REG);
-	iowrite32(p_set->coeff_set[22], priv->base + VNC8B_REG);
-	iowrite32(p_set->coeff_set[23], priv->base + VNC8C_REG);
-}
-
-/* rect is guaranteed to not exceed the scaled camera rectangle */
-static int rcar_vin_set_rect(struct soc_camera_device *icd)
-{
-	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
-	struct rcar_vin_cam *cam = icd->host_priv;
-	struct rcar_vin_priv *priv = ici->priv;
-	unsigned int left_offset, top_offset;
-	unsigned char dsize = 0;
-	struct v4l2_rect *cam_subrect = &cam->subrect;
-	u32 value;
-
-	dev_dbg(icd->parent, "Crop %ux%u@%u:%u\n",
-		icd->user_width, icd->user_height, cam->vin_left, cam->vin_top);
-
-	left_offset = cam->vin_left;
-	top_offset = cam->vin_top;
-
-	if (icd->current_fmt->host_fmt->fourcc == V4L2_PIX_FMT_RGB32 &&
-	    priv->chip == RCAR_E1)
-		dsize = 1;
-
-	dev_dbg(icd->parent, "Cam %ux%u@%u:%u\n",
-		cam->width, cam->height, cam->vin_left, cam->vin_top);
-	dev_dbg(icd->parent, "Cam subrect %ux%u@%u:%u\n",
-		cam_subrect->width, cam_subrect->height,
-		cam_subrect->left, cam_subrect->top);
-
-	/* Set Start/End Pixel/Line Pre-Clip */
-	iowrite32(left_offset << dsize, priv->base + VNSPPRC_REG);
-	iowrite32((left_offset + cam_subrect->width - 1) << dsize,
-		  priv->base + VNEPPRC_REG);
-	switch (priv->field) {
-	case V4L2_FIELD_INTERLACED:
-	case V4L2_FIELD_INTERLACED_TB:
-	case V4L2_FIELD_INTERLACED_BT:
-		iowrite32(top_offset / 2, priv->base + VNSLPRC_REG);
-		iowrite32((top_offset + cam_subrect->height) / 2 - 1,
-			  priv->base + VNELPRC_REG);
-		break;
-	default:
-		iowrite32(top_offset, priv->base + VNSLPRC_REG);
-		iowrite32(top_offset + cam_subrect->height - 1,
-			  priv->base + VNELPRC_REG);
-		break;
-	}
-
-	/* Set scaling coefficient */
-	value = 0;
-	if (cam_subrect->height != cam->out_height)
-		value = (4096 * cam_subrect->height) / cam->out_height;
-	dev_dbg(icd->parent, "YS Value: %x\n", value);
-	iowrite32(value, priv->base + VNYS_REG);
-
-	value = 0;
-	if (cam_subrect->width != cam->out_width)
-		value = (4096 * cam_subrect->width) / cam->out_width;
-
-	/* Horizontal upscaling is up to double size */
-	if (0 < value && value < 2048)
-		value = 2048;
-
-	dev_dbg(icd->parent, "XS Value: %x\n", value);
-	iowrite32(value, priv->base + VNXS_REG);
-
-	/* Horizontal upscaling is carried out by scaling down from double size */
-	if (value < 4096)
-		value *= 2;
-
-	set_coeff(priv, value);
-
-	/* Set Start/End Pixel/Line Post-Clip */
-	iowrite32(0, priv->base + VNSPPOC_REG);
-	iowrite32(0, priv->base + VNSLPOC_REG);
-	iowrite32((cam->out_width - 1) << dsize, priv->base + VNEPPOC_REG);
-	switch (priv->field) {
-	case V4L2_FIELD_INTERLACED:
-	case V4L2_FIELD_INTERLACED_TB:
-	case V4L2_FIELD_INTERLACED_BT:
-		iowrite32(cam->out_height / 2 - 1,
-			  priv->base + VNELPOC_REG);
-		break;
-	default:
-		iowrite32(cam->out_height - 1, priv->base + VNELPOC_REG);
-		break;
-	}
-
-	iowrite32(ALIGN(cam->out_width, 0x10), priv->base + VNIS_REG);
-
-	return 0;
-}
-
-static void capture_stop_preserve(struct rcar_vin_priv *priv, u32 *vnmc)
-{
-	*vnmc = ioread32(priv->base + VNMC_REG);
-	/* module disable */
-	iowrite32(*vnmc & ~VNMC_ME, priv->base + VNMC_REG);
-}
-
-static void capture_restore(struct rcar_vin_priv *priv, u32 vnmc)
-{
-	unsigned long timeout = jiffies + 10 * HZ;
-
-	/*
-	 * Wait until the end of the current frame. It can take a long time,
-	 * but if it has been aborted by a MRST1 reset, it should exit sooner.
-	 */
-	while ((ioread32(priv->base + VNMS_REG) & VNMS_AV) &&
-		time_before(jiffies, timeout))
-		msleep(1);
-
-	if (time_after(jiffies, timeout)) {
-		dev_err(priv->ici.v4l2_dev.dev,
-			"Timeout waiting for frame end! Interface problem?\n");
-		return;
-	}
-
-	iowrite32(vnmc, priv->base + VNMC_REG);
-}
-
-#define VIN_MBUS_FLAGS	(V4L2_MBUS_MASTER |		\
-			 V4L2_MBUS_PCLK_SAMPLE_RISING |	\
-			 V4L2_MBUS_HSYNC_ACTIVE_HIGH |	\
-			 V4L2_MBUS_HSYNC_ACTIVE_LOW |	\
-			 V4L2_MBUS_VSYNC_ACTIVE_HIGH |	\
-			 V4L2_MBUS_VSYNC_ACTIVE_LOW |	\
-			 V4L2_MBUS_DATA_ACTIVE_HIGH)
-
-static int rcar_vin_set_bus_param(struct soc_camera_device *icd)
-{
-	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
-	struct rcar_vin_priv *priv = ici->priv;
-	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
-	struct v4l2_mbus_config cfg;
-	unsigned long common_flags;
-	u32 vnmc;
-	u32 val;
-	int ret;
-
-	capture_stop_preserve(priv, &vnmc);
-
-	ret = v4l2_subdev_call(sd, video, g_mbus_config, &cfg);
-	if (!ret) {
-		common_flags = soc_mbus_config_compatible(&cfg, VIN_MBUS_FLAGS);
-		if (!common_flags) {
-			dev_warn(icd->parent,
-				 "MBUS flags incompatible: camera 0x%x, host 0x%x\n",
-				 cfg.flags, VIN_MBUS_FLAGS);
-			return -EINVAL;
-		}
-	} else if (ret != -ENOIOCTLCMD) {
-		return ret;
-	} else {
-		common_flags = VIN_MBUS_FLAGS;
-	}
-
-	/* Make choises, based on platform preferences */
-	if ((common_flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH) &&
-	    (common_flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)) {
-		if (priv->pdata_flags & RCAR_VIN_HSYNC_ACTIVE_LOW)
-			common_flags &= ~V4L2_MBUS_HSYNC_ACTIVE_HIGH;
-		else
-			common_flags &= ~V4L2_MBUS_HSYNC_ACTIVE_LOW;
-	}
-
-	if ((common_flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH) &&
-	    (common_flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)) {
-		if (priv->pdata_flags & RCAR_VIN_VSYNC_ACTIVE_LOW)
-			common_flags &= ~V4L2_MBUS_VSYNC_ACTIVE_HIGH;
-		else
-			common_flags &= ~V4L2_MBUS_VSYNC_ACTIVE_LOW;
-	}
-
-	cfg.flags = common_flags;
-	ret = v4l2_subdev_call(sd, video, s_mbus_config, &cfg);
-	if (ret < 0 && ret != -ENOIOCTLCMD)
-		return ret;
-
-	val = VNDMR2_FTEV | VNDMR2_VLV(1);
-	if (!(common_flags & V4L2_MBUS_VSYNC_ACTIVE_LOW))
-		val |= VNDMR2_VPS;
-	if (!(common_flags & V4L2_MBUS_HSYNC_ACTIVE_LOW))
-		val |= VNDMR2_HPS;
-	iowrite32(val, priv->base + VNDMR2_REG);
-
-	ret = rcar_vin_set_rect(icd);
-	if (ret < 0)
-		return ret;
-
-	capture_restore(priv, vnmc);
-
-	return 0;
-}
-
-static int rcar_vin_try_bus_param(struct soc_camera_device *icd,
-				  unsigned char buswidth)
-{
-	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
-	struct v4l2_mbus_config cfg;
-	int ret;
-
-	ret = v4l2_subdev_call(sd, video, g_mbus_config, &cfg);
-	if (ret == -ENOIOCTLCMD)
-		return 0;
-	else if (ret)
-		return ret;
-
-	if (buswidth > 24)
-		return -EINVAL;
-
-	/* check is there common mbus flags */
-	ret = soc_mbus_config_compatible(&cfg, VIN_MBUS_FLAGS);
-	if (ret)
-		return 0;
-
-	dev_warn(icd->parent,
-		"MBUS flags incompatible: camera 0x%x, host 0x%x\n",
-		 cfg.flags, VIN_MBUS_FLAGS);
-
-	return -EINVAL;
-}
-
-static bool rcar_vin_packing_supported(const struct soc_mbus_pixelfmt *fmt)
-{
-	return	fmt->packing == SOC_MBUS_PACKING_NONE ||
-		(fmt->bits_per_sample > 8 &&
-		 fmt->packing == SOC_MBUS_PACKING_EXTEND16);
-}
-
-static const struct soc_mbus_pixelfmt rcar_vin_formats[] = {
-	{
-		.fourcc			= V4L2_PIX_FMT_NV16,
-		.name			= "NV16",
-		.bits_per_sample	= 8,
-		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
-		.order			= SOC_MBUS_ORDER_LE,
-		.layout			= SOC_MBUS_LAYOUT_PLANAR_Y_C,
-	},
-	{
-		.fourcc			= V4L2_PIX_FMT_YUYV,
-		.name			= "YUYV",
-		.bits_per_sample	= 16,
-		.packing		= SOC_MBUS_PACKING_NONE,
-		.order			= SOC_MBUS_ORDER_LE,
-		.layout			= SOC_MBUS_LAYOUT_PACKED,
-	},
-	{
-		.fourcc			= V4L2_PIX_FMT_UYVY,
-		.name			= "UYVY",
-		.bits_per_sample	= 16,
-		.packing		= SOC_MBUS_PACKING_NONE,
-		.order			= SOC_MBUS_ORDER_LE,
-		.layout			= SOC_MBUS_LAYOUT_PACKED,
-	},
-	{
-		.fourcc			= V4L2_PIX_FMT_RGB565,
-		.name			= "RGB565",
-		.bits_per_sample	= 16,
-		.packing		= SOC_MBUS_PACKING_NONE,
-		.order			= SOC_MBUS_ORDER_LE,
-		.layout			= SOC_MBUS_LAYOUT_PACKED,
-	},
-	{
-		.fourcc			= V4L2_PIX_FMT_RGB555X,
-		.name			= "ARGB1555",
-		.bits_per_sample	= 16,
-		.packing		= SOC_MBUS_PACKING_NONE,
-		.order			= SOC_MBUS_ORDER_LE,
-		.layout			= SOC_MBUS_LAYOUT_PACKED,
-	},
-	{
-		.fourcc			= V4L2_PIX_FMT_RGB32,
-		.name			= "RGB888",
-		.bits_per_sample	= 32,
-		.packing		= SOC_MBUS_PACKING_NONE,
-		.order			= SOC_MBUS_ORDER_LE,
-		.layout			= SOC_MBUS_LAYOUT_PACKED,
-	},
-	{
-		.fourcc			= V4L2_PIX_FMT_ARGB32,
-		.name			= "ARGB8888",
-		.bits_per_sample	= 32,
-		.packing		= SOC_MBUS_PACKING_NONE,
-		.order			= SOC_MBUS_ORDER_LE,
-		.layout			= SOC_MBUS_LAYOUT_PACKED,
-	},
-};
-
-static int rcar_vin_get_formats(struct soc_camera_device *icd, unsigned int idx,
-				struct soc_camera_format_xlate *xlate)
-{
-	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
-	struct device *dev = icd->parent;
-	int ret, k, n;
-	int formats = 0;
-	struct rcar_vin_cam *cam;
-	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, pad, enum_mbus_code, NULL, &code);
-	if (ret < 0)
-		return 0;
-
-	fmt = soc_mbus_get_fmtdesc(code.code);
-	if (!fmt) {
-		dev_warn(dev, "unsupported format code #%u: %d\n", idx, code.code);
-		return 0;
-	}
-
-	ret = rcar_vin_try_bus_param(icd, fmt->bits_per_sample);
-	if (ret < 0)
-		return 0;
-
-	if (!icd->host_priv) {
-		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, pad, get_fmt, NULL, &fmt);
-		if (ret < 0)
-			return ret;
-
-		/* Cache current client geometry */
-		ret = soc_camera_client_g_rect(sd, &rect);
-		if (ret == -ENOIOCTLCMD) {
-			/* Sensor driver doesn't support cropping */
-			rect.left = 0;
-			rect.top = 0;
-			rect.width = mf->width;
-			rect.height = mf->height;
-		} else if (ret < 0) {
-			return ret;
-		}
-
-		/*
-		 * If sensor proposes too large format then try smaller ones:
-		 * 1280x960, 640x480, 320x240
-		 */
-		for (shift = 0; shift < 3; shift++) {
-			if (mf->width <= VIN_MAX_WIDTH &&
-			    mf->height <= VIN_MAX_HEIGHT)
-				break;
-
-			mf->width = 1280 >> shift;
-			mf->height = 960 >> shift;
-			ret = v4l2_device_call_until_err(sd->v4l2_dev,
-							 soc_camera_grp_id(icd),
-							 pad, set_fmt, NULL,
-							 &fmt);
-			if (ret < 0)
-				return ret;
-		}
-
-		if (shift == 3) {
-			dev_err(dev,
-				"Failed to configure the client below %ux%u\n",
-				mf->width, mf->height);
-			return -EIO;
-		}
-
-		dev_dbg(dev, "camera fmt %ux%u\n", mf->width, mf->height);
-
-		cam = kzalloc(sizeof(*cam), GFP_KERNEL);
-		if (!cam)
-			return -ENOMEM;
-		/*
-		 * We are called with current camera crop,
-		 * initialise subrect with it
-		 */
-		cam->rect = rect;
-		cam->subrect = rect;
-		cam->width = mf->width;
-		cam->height = mf->height;
-		cam->out_width	= mf->width;
-		cam->out_height	= mf->height;
-
-		icd->host_priv = cam;
-	} else {
-		cam = icd->host_priv;
-	}
-
-	/* Beginning of a pass */
-	if (!idx)
-		cam->extra_fmt = NULL;
-
-	switch (code.code) {
-	case MEDIA_BUS_FMT_YUYV8_1X16:
-	case MEDIA_BUS_FMT_YUYV8_2X8:
-	case MEDIA_BUS_FMT_YUYV10_2X10:
-	case MEDIA_BUS_FMT_RGB888_1X24:
-		if (cam->extra_fmt)
-			break;
-
-		/* Add all our formats that can be generated by VIN */
-		cam->extra_fmt = rcar_vin_formats;
-
-		n = ARRAY_SIZE(rcar_vin_formats);
-		formats += n;
-		for (k = 0; xlate && k < n; k++, xlate++) {
-			xlate->host_fmt = &rcar_vin_formats[k];
-			xlate->code = code.code;
-			dev_dbg(dev, "Providing format %s using code %d\n",
-				rcar_vin_formats[k].name, code.code);
-		}
-		break;
-	default:
-		if (!rcar_vin_packing_supported(fmt))
-			return 0;
-
-		dev_dbg(dev, "Providing format %s in pass-through mode\n",
-			fmt->name);
-		break;
-	}
-
-	/* Generic pass-through */
-	formats++;
-	if (xlate) {
-		xlate->host_fmt = fmt;
-		xlate->code = code.code;
-		xlate++;
-	}
-
-	return formats;
-}
-
-static void rcar_vin_put_formats(struct soc_camera_device *icd)
-{
-	kfree(icd->host_priv);
-	icd->host_priv = NULL;
-}
-
-static int rcar_vin_set_crop(struct soc_camera_device *icd,
-			     const struct v4l2_crop *a)
-{
-	struct v4l2_crop a_writable = *a;
-	const struct v4l2_rect *rect = &a_writable.c;
-	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
-	struct rcar_vin_priv *priv = ici->priv;
-	struct v4l2_crop cam_crop;
-	struct rcar_vin_cam *cam = icd->host_priv;
-	struct v4l2_rect *cam_rect = &cam_crop.c;
-	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
-	struct device *dev = icd->parent;
-	struct v4l2_subdev_format fmt = {
-		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
-	};
-	struct v4l2_mbus_framefmt *mf = &fmt.format;
-	u32 vnmc;
-	int ret, i;
-
-	dev_dbg(dev, "S_CROP(%ux%u@%u:%u)\n", rect->width, rect->height,
-		rect->left, rect->top);
-
-	/* During camera cropping its output window can change too, stop VIN */
-	capture_stop_preserve(priv, &vnmc);
-	dev_dbg(dev, "VNMC_REG 0x%x\n", vnmc);
-
-	/* Apply iterative camera S_CROP for new input window. */
-	ret = soc_camera_client_s_crop(sd, &a_writable, &cam_crop,
-				       &cam->rect, &cam->subrect);
-	if (ret < 0)
-		return ret;
-
-	dev_dbg(dev, "camera cropped to %ux%u@%u:%u\n",
-		cam_rect->width, cam_rect->height,
-		cam_rect->left, cam_rect->top);
-
-	/* On success cam_crop contains current camera crop */
-
-	/* Retrieve camera output window */
-	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)
-		return -EINVAL;
-
-	/* Cache camera output window */
-	cam->width = mf->width;
-	cam->height = mf->height;
-
-	icd->user_width  = cam->width;
-	icd->user_height = cam->height;
-
-	cam->vin_left = rect->left & ~1;
-	cam->vin_top = rect->top & ~1;
-
-	/* Use VIN cropping to crop to the new window. */
-	ret = rcar_vin_set_rect(icd);
-	if (ret < 0)
-		return ret;
-
-	cam->subrect = *rect;
-
-	dev_dbg(dev, "VIN cropped to %ux%u@%u:%u\n",
-		icd->user_width, icd->user_height,
-		cam->vin_left, cam->vin_top);
-
-	/* Restore capture */
-	for (i = 0; i < MAX_BUFFER_NUM; i++) {
-		if (priv->queue_buf[i] && priv->state == STOPPED) {
-			vnmc |= VNMC_ME;
-			break;
-		}
-	}
-	capture_restore(priv, vnmc);
-
-	/* Even if only camera cropping succeeded */
-	return ret;
-}
-
-static int rcar_vin_get_crop(struct soc_camera_device *icd,
-			     struct v4l2_crop *a)
-{
-	struct rcar_vin_cam *cam = icd->host_priv;
-
-	a->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-	a->c = cam->subrect;
-
-	return 0;
-}
-
-/* Similar to set_crop multistage iterative algorithm */
-static int rcar_vin_set_fmt(struct soc_camera_device *icd,
-			    struct v4l2_format *f)
-{
-	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
-	struct rcar_vin_priv *priv = ici->priv;
-	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
-	struct rcar_vin_cam *cam = icd->host_priv;
-	struct v4l2_pix_format *pix = &f->fmt.pix;
-	struct v4l2_mbus_framefmt mf;
-	struct device *dev = icd->parent;
-	__u32 pixfmt = pix->pixelformat;
-	const struct soc_camera_format_xlate *xlate;
-	unsigned int vin_sub_width = 0, vin_sub_height = 0;
-	int ret;
-	bool can_scale;
-	enum v4l2_field field;
-	v4l2_std_id std;
-
-	dev_dbg(dev, "S_FMT(pix=0x%x, %ux%u)\n",
-		pixfmt, pix->width, pix->height);
-
-	switch (pix->field) {
-	default:
-		pix->field = V4L2_FIELD_NONE;
-		/* fall-through */
-	case V4L2_FIELD_NONE:
-	case V4L2_FIELD_TOP:
-	case V4L2_FIELD_BOTTOM:
-	case V4L2_FIELD_INTERLACED_TB:
-	case V4L2_FIELD_INTERLACED_BT:
-		field = pix->field;
-		break;
-	case V4L2_FIELD_INTERLACED:
-		/* Query for standard if not explicitly mentioned _TB/_BT */
-		ret = v4l2_subdev_call(sd, video, querystd, &std);
-		if (ret == -ENOIOCTLCMD) {
-			field = V4L2_FIELD_NONE;
-		} else if (ret < 0) {
-			return ret;
-		} else {
-			field = std & V4L2_STD_625_50 ?
-				V4L2_FIELD_INTERLACED_TB :
-				V4L2_FIELD_INTERLACED_BT;
-		}
-		break;
-	}
-
-	xlate = soc_camera_xlate_by_fourcc(icd, pixfmt);
-	if (!xlate) {
-		dev_warn(dev, "Format %x not found\n", pixfmt);
-		return -EINVAL;
-	}
-	/* Calculate client output geometry */
-	soc_camera_calc_client_output(icd, &cam->rect, &cam->subrect, pix, &mf,
-				      12);
-	mf.field = pix->field;
-	mf.colorspace = pix->colorspace;
-	mf.code	 = xlate->code;
-
-	switch (pixfmt) {
-	case V4L2_PIX_FMT_RGB32:
-		can_scale = priv->chip != RCAR_E1;
-		break;
-	case V4L2_PIX_FMT_ARGB32:
-	case V4L2_PIX_FMT_UYVY:
-	case V4L2_PIX_FMT_YUYV:
-	case V4L2_PIX_FMT_RGB565:
-	case V4L2_PIX_FMT_RGB555X:
-		can_scale = true;
-		break;
-	default:
-		can_scale = false;
-		break;
-	}
-
-	dev_dbg(dev, "request camera output %ux%u\n", mf.width, mf.height);
-
-	ret = soc_camera_client_scale(icd, &cam->rect, &cam->subrect,
-				      &mf, &vin_sub_width, &vin_sub_height,
-				      can_scale, 12);
-
-	/* Done with the camera. Now see if we can improve the result */
-	dev_dbg(dev, "Camera %d fmt %ux%u, requested %ux%u\n",
-		ret, mf.width, mf.height, pix->width, pix->height);
-
-	if (ret == -ENOIOCTLCMD)
-		dev_dbg(dev, "Sensor doesn't support scaling\n");
-	else if (ret < 0)
-		return ret;
-
-	if (mf.code != xlate->code)
-		return -EINVAL;
-
-	/* Prepare VIN crop */
-	cam->width = mf.width;
-	cam->height = mf.height;
-
-	/* Use VIN scaling to scale to the requested user window. */
-
-	/* We cannot scale up */
-	if (pix->width > vin_sub_width)
-		vin_sub_width = pix->width;
-
-	if (pix->height > vin_sub_height)
-		vin_sub_height = pix->height;
-
-	pix->colorspace = mf.colorspace;
-
-	if (!can_scale) {
-		pix->width = vin_sub_width;
-		pix->height = vin_sub_height;
-	}
-
-	/*
-	 * We have calculated CFLCR, the actual configuration will be performed
-	 * in rcar_vin_set_bus_param()
-	 */
-
-	dev_dbg(dev, "W: %u : %u, H: %u : %u\n",
-		vin_sub_width, pix->width, vin_sub_height, pix->height);
-
-	cam->out_width = pix->width;
-	cam->out_height = pix->height;
-
-	icd->current_fmt = xlate;
-
-	priv->field = field;
-
-	return 0;
-}
-
-static int rcar_vin_try_fmt(struct soc_camera_device *icd,
-			    struct v4l2_format *f)
-{
-	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_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;
-
-	xlate = soc_camera_xlate_by_fourcc(icd, pixfmt);
-	if (!xlate) {
-		xlate = icd->current_fmt;
-		dev_dbg(icd->parent, "Format %x not found, keeping %x\n",
-			pixfmt, xlate->host_fmt->fourcc);
-		pixfmt = xlate->host_fmt->fourcc;
-		pix->pixelformat = pixfmt;
-		pix->colorspace = icd->colorspace;
-	}
-
-	/* FIXME: calculate using depth and bus width */
-	v4l_bound_align_image(&pix->width, 2, VIN_MAX_WIDTH, 1,
-			      &pix->height, 4, VIN_MAX_HEIGHT, 2, 0);
-
-	width = pix->width;
-	height = pix->height;
-
-	/* let soc-camera calculate these values */
-	pix->bytesperline = 0;
-	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;
-
-	ret = v4l2_device_call_until_err(sd->v4l2_dev, soc_camera_grp_id(icd),
-					 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;
-
-	pix->field = mf->field;
-	pix->colorspace = mf->colorspace;
-
-	if (pixfmt == V4L2_PIX_FMT_NV16) {
-		/* FIXME: check against rect_max after converting soc-camera */
-		/* We can scale precisely, need a bigger image from camera */
-		if (pix->width < width || pix->height < height) {
-			/*
-			 * We presume, the sensor behaves sanely, i.e. if
-			 * requested a bigger rectangle, it will not return a
-			 * smaller one.
-			 */
-			mf->width = VIN_MAX_WIDTH;
-			mf->height = VIN_MAX_HEIGHT;
-			ret = v4l2_device_call_until_err(sd->v4l2_dev,
-							 soc_camera_grp_id(icd),
-							 pad, set_fmt, &pad_cfg,
-							 &format);
-			if (ret < 0) {
-				dev_err(icd->parent,
-					"client try_fmt() = %d\n", ret);
-				return ret;
-			}
-		}
-		/* We will scale exactly */
-		if (mf->width > width)
-			pix->width = width;
-		if (mf->height > height)
-			pix->height = height;
-	}
-
-	return ret;
-}
-
-static unsigned int rcar_vin_poll(struct file *file, poll_table *pt)
-{
-	struct soc_camera_device *icd = file->private_data;
-
-	return vb2_poll(&icd->vb2_vidq, file, pt);
-}
-
-static int rcar_vin_querycap(struct soc_camera_host *ici,
-			     struct v4l2_capability *cap)
-{
-	strlcpy(cap->card, "R_Car_VIN", sizeof(cap->card));
-	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
-	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
-	snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s%d", DRV_NAME, ici->nr);
-
-	return 0;
-}
-
-static int rcar_vin_init_videobuf2(struct vb2_queue *vq,
-				   struct soc_camera_device *icd)
-{
-	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
-
-	vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-	vq->io_modes = VB2_MMAP | VB2_USERPTR;
-	vq->drv_priv = icd;
-	vq->ops = &rcar_vin_vb2_ops;
-	vq->mem_ops = &vb2_dma_contig_memops;
-	vq->buf_struct_size = sizeof(struct rcar_vin_buffer);
-	vq->timestamp_flags  = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
-	vq->lock = &ici->host_lock;
-	vq->dev = ici->v4l2_dev.dev;
-
-	return vb2_queue_init(vq);
-}
-
-static struct soc_camera_host_ops rcar_vin_host_ops = {
-	.owner		= THIS_MODULE,
-	.add		= rcar_vin_add_device,
-	.remove		= rcar_vin_remove_device,
-	.get_formats	= rcar_vin_get_formats,
-	.put_formats	= rcar_vin_put_formats,
-	.get_crop	= rcar_vin_get_crop,
-	.set_crop	= rcar_vin_set_crop,
-	.try_fmt	= rcar_vin_try_fmt,
-	.set_fmt	= rcar_vin_set_fmt,
-	.poll		= rcar_vin_poll,
-	.querycap	= rcar_vin_querycap,
-	.set_bus_param	= rcar_vin_set_bus_param,
-	.init_videobuf2	= rcar_vin_init_videobuf2,
-};
-
-#ifdef CONFIG_OF
-static const struct of_device_id rcar_vin_of_table[] = {
-	{ .compatible = "renesas,vin-r8a7795", .data = (void *)RCAR_GEN3 },
-	{ .compatible = "renesas,vin-r8a7794", .data = (void *)RCAR_GEN2 },
-	{ .compatible = "renesas,vin-r8a7793", .data = (void *)RCAR_GEN2 },
-	{ .compatible = "renesas,vin-r8a7791", .data = (void *)RCAR_GEN2 },
-	{ .compatible = "renesas,vin-r8a7790", .data = (void *)RCAR_GEN2 },
-	{ .compatible = "renesas,vin-r8a7779", .data = (void *)RCAR_H1 },
-	{ .compatible = "renesas,vin-r8a7778", .data = (void *)RCAR_M1 },
-	{ .compatible = "renesas,rcar-gen3-vin", .data = (void *)RCAR_GEN3 },
-	{ .compatible = "renesas,rcar-gen2-vin", .data = (void *)RCAR_GEN2 },
-	{ },
-};
-MODULE_DEVICE_TABLE(of, rcar_vin_of_table);
-#endif
-
-static int rcar_vin_probe(struct platform_device *pdev)
-{
-	const struct of_device_id *match = NULL;
-	struct rcar_vin_priv *priv;
-	struct v4l2_of_endpoint ep;
-	struct device_node *np;
-	struct resource *mem;
-	unsigned int pdata_flags;
-	int irq, ret;
-
-	match = of_match_device(of_match_ptr(rcar_vin_of_table), &pdev->dev);
-
-	np = of_graph_get_next_endpoint(pdev->dev.of_node, NULL);
-	if (!np) {
-		dev_err(&pdev->dev, "could not find endpoint\n");
-		return -EINVAL;
-	}
-
-	ret = v4l2_of_parse_endpoint(np, &ep);
-	if (ret) {
-		dev_err(&pdev->dev, "could not parse endpoint\n");
-		return ret;
-	}
-
-	if (ep.bus_type == V4L2_MBUS_BT656)
-		pdata_flags = RCAR_VIN_BT656;
-	else {
-		pdata_flags = 0;
-		if (ep.bus.parallel.flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)
-			pdata_flags |= RCAR_VIN_HSYNC_ACTIVE_LOW;
-		if (ep.bus.parallel.flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)
-			pdata_flags |= RCAR_VIN_VSYNC_ACTIVE_LOW;
-	}
-
-	of_node_put(np);
-
-	dev_dbg(&pdev->dev, "pdata_flags = %08x\n", pdata_flags);
-
-	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (mem == NULL)
-		return -EINVAL;
-
-	irq = platform_get_irq(pdev, 0);
-	if (irq <= 0)
-		return -EINVAL;
-
-	priv = devm_kzalloc(&pdev->dev, sizeof(struct rcar_vin_priv),
-			    GFP_KERNEL);
-	if (!priv)
-		return -ENOMEM;
-
-	priv->base = devm_ioremap_resource(&pdev->dev, mem);
-	if (IS_ERR(priv->base))
-		return PTR_ERR(priv->base);
-
-	ret = devm_request_irq(&pdev->dev, irq, rcar_vin_irq, IRQF_SHARED,
-			       dev_name(&pdev->dev), priv);
-	if (ret)
-		return ret;
-
-	priv->ici.priv = priv;
-	priv->ici.v4l2_dev.dev = &pdev->dev;
-	priv->ici.drv_name = dev_name(&pdev->dev);
-	priv->ici.ops = &rcar_vin_host_ops;
-
-	priv->pdata_flags = pdata_flags;
-	if (!match) {
-		priv->ici.nr = pdev->id;
-		priv->chip = pdev->id_entry->driver_data;
-	} else {
-		priv->ici.nr = of_alias_get_id(pdev->dev.of_node, "vin");
-		priv->chip = (enum chip_id)match->data;
-	}
-
-	spin_lock_init(&priv->lock);
-	INIT_LIST_HEAD(&priv->capture);
-
-	priv->state = STOPPED;
-
-	pm_suspend_ignore_children(&pdev->dev, true);
-	pm_runtime_enable(&pdev->dev);
-
-	ret = soc_camera_host_register(&priv->ici);
-	if (ret)
-		goto cleanup;
-
-	return 0;
-
-cleanup:
-	pm_runtime_disable(&pdev->dev);
-
-	return ret;
-}
-
-static int rcar_vin_remove(struct platform_device *pdev)
-{
-	struct soc_camera_host *soc_host = to_soc_camera_host(&pdev->dev);
-
-	soc_camera_host_unregister(soc_host);
-	pm_runtime_disable(&pdev->dev);
-
-	return 0;
-}
-
-static struct platform_driver rcar_vin_driver = {
-	.probe		= rcar_vin_probe,
-	.remove		= rcar_vin_remove,
-	.driver		= {
-		.name		= DRV_NAME,
-		.of_match_table	= of_match_ptr(rcar_vin_of_table),
-	},
-};
-
-module_platform_driver(rcar_vin_driver);
-
-MODULE_LICENSE("GPL");
-MODULE_ALIAS("platform:rcar_vin");
-MODULE_DESCRIPTION("Renesas R-Car VIN camera host driver");
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 02b519dde42a..a15bfb5aea47 100644
--- a/drivers/media/platform/soc_camera/sh_mobile_ceu_camera.c
+++ b/drivers/media/platform/soc_camera/sh_mobile_ceu_camera.c
@@ -41,7 +41,6 @@
 #include <media/v4l2-dev.h>
 #include <media/soc_camera.h>
 #include <media/drv-intf/sh_mobile_ceu.h>
-#include <media/drv-intf/sh_mobile_csi2.h>
 #include <media/videobuf2-dma-contig.h>
 #include <media/v4l2-mediabus.h>
 #include <media/drv-intf/soc_mediabus.h>
@@ -99,11 +98,6 @@ struct sh_mobile_ceu_buffer {
 
 struct sh_mobile_ceu_dev {
 	struct soc_camera_host ici;
-	/* Asynchronous CSI2 linking */
-	struct v4l2_async_subdev *csi2_asd;
-	struct v4l2_subdev *csi2_sd;
-	/* Synchronous probing compatibility */
-	struct platform_device *csi2_pdev;
 
 	unsigned int irq;
 	void __iomem *base;
@@ -140,7 +134,7 @@ struct sh_mobile_ceu_cam {
 	unsigned int width;
 	unsigned int height;
 	/*
-	 * User window from S_CROP / G_CROP, produced by client cropping and
+	 * User window from S_SELECTION / G_SELECTION, produced by client cropping and
 	 * scaling, CEU scaling and CEU cropping, mapped back onto the client
 	 * input window
 	 */
@@ -470,7 +464,7 @@ static void sh_mobile_ceu_stop_streaming(struct vb2_queue *q)
 	sh_mobile_ceu_soft_reset(pcdev);
 }
 
-static struct vb2_ops sh_mobile_ceu_videobuf_ops = {
+static const struct vb2_ops sh_mobile_ceu_videobuf_ops = {
 	.queue_setup	= sh_mobile_ceu_videobuf_setup,
 	.buf_prepare	= sh_mobile_ceu_videobuf_prepare,
 	.buf_queue	= sh_mobile_ceu_videobuf_queue,
@@ -517,74 +511,20 @@ out:
 	return IRQ_HANDLED;
 }
 
-static struct v4l2_subdev *find_csi2(struct sh_mobile_ceu_dev *pcdev)
-{
-	struct v4l2_subdev *sd;
-
-	if (pcdev->csi2_sd)
-		return pcdev->csi2_sd;
-
-	if (pcdev->csi2_asd) {
-		char name[] = "sh-mobile-csi2";
-		v4l2_device_for_each_subdev(sd, &pcdev->ici.v4l2_dev)
-			if (!strncmp(name, sd->name, sizeof(name) - 1)) {
-				pcdev->csi2_sd = sd;
-				return sd;
-			}
-	}
-
-	return NULL;
-}
-
-static struct v4l2_subdev *csi2_subdev(struct sh_mobile_ceu_dev *pcdev,
-				       struct soc_camera_device *icd)
-{
-	struct v4l2_subdev *sd = pcdev->csi2_sd;
-
-	return sd && sd->grp_id == soc_camera_grp_id(icd) ? sd : NULL;
-}
-
 static int sh_mobile_ceu_add_device(struct soc_camera_device *icd)
 {
-	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
-	struct sh_mobile_ceu_dev *pcdev = ici->priv;
-	struct v4l2_subdev *csi2_sd = find_csi2(pcdev);
-	int ret;
-
-	if (csi2_sd) {
-		csi2_sd->grp_id = soc_camera_grp_id(icd);
-		v4l2_set_subdev_hostdata(csi2_sd, icd);
-	}
-
-	ret = v4l2_subdev_call(csi2_sd, core, s_power, 1);
-	if (ret < 0 && ret != -ENOIOCTLCMD && ret != -ENODEV)
-		return ret;
-
-	/*
-	 * -ENODEV is special: either csi2_sd == NULL or the CSI-2 driver
-	 * has not found this soc-camera device among its clients
-	 */
-	if (csi2_sd && ret == -ENODEV)
-		csi2_sd->grp_id = 0;
-
 	dev_info(icd->parent,
-		 "SuperH Mobile CEU%s driver attached to camera %d\n",
-		 csi2_sd && csi2_sd->grp_id ? "/CSI-2" : "", icd->devnum);
+		 "SuperH Mobile CEU driver attached to camera %d\n",
+		 icd->devnum);
 
 	return 0;
 }
 
 static void sh_mobile_ceu_remove_device(struct soc_camera_device *icd)
 {
-	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
-	struct sh_mobile_ceu_dev *pcdev = ici->priv;
-	struct v4l2_subdev *csi2_sd = find_csi2(pcdev);
-
 	dev_info(icd->parent,
 		 "SuperH Mobile CEU driver detached from camera %d\n",
 		 icd->devnum);
-
-	v4l2_subdev_call(csi2_sd, core, s_power, 0);
 }
 
 /* Called with .host_lock held */
@@ -704,12 +644,6 @@ static void sh_mobile_ceu_set_rect(struct soc_camera_device *icd)
 		cdwdr_width *= 2;
 	}
 
-	/* CSI2 special configuration */
-	if (csi2_subdev(pcdev, icd)) {
-		in_width = ((in_width - 2) * 2);
-		left_offset *= 2;
-	}
-
 	/* Set CAMOR, CAPWR, CFSZR, take care of CDWDR */
 	camor = left_offset | (top_offset << 16);
 
@@ -758,13 +692,6 @@ static void capture_restore(struct sh_mobile_ceu_dev *pcdev, u32 capsr)
 		ceu_write(pcdev, CAPSR, capsr);
 }
 
-/* Find the bus subdevice driver, e.g., CSI2 */
-static struct v4l2_subdev *find_bus_subdev(struct sh_mobile_ceu_dev *pcdev,
-					   struct soc_camera_device *icd)
-{
-	return csi2_subdev(pcdev, icd) ? : soc_camera_to_subdev(icd);
-}
-
 #define CEU_BUS_FLAGS (V4L2_MBUS_MASTER |	\
 		V4L2_MBUS_PCLK_SAMPLE_RISING |	\
 		V4L2_MBUS_HSYNC_ACTIVE_HIGH |	\
@@ -778,7 +705,7 @@ static int sh_mobile_ceu_set_bus_param(struct soc_camera_device *icd)
 {
 	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 	struct sh_mobile_ceu_dev *pcdev = ici->priv;
-	struct v4l2_subdev *sd = find_bus_subdev(pcdev, icd);
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 	struct sh_mobile_ceu_cam *cam = icd->host_priv;
 	struct v4l2_mbus_config cfg = {.type = V4L2_MBUS_PARALLEL,};
 	unsigned long value, common_flags = CEU_BUS_FLAGS;
@@ -866,9 +793,7 @@ static int sh_mobile_ceu_set_bus_param(struct soc_camera_device *icd)
 	value |= common_flags & V4L2_MBUS_VSYNC_ACTIVE_LOW ? 1 << 1 : 0;
 	value |= common_flags & V4L2_MBUS_HSYNC_ACTIVE_LOW ? 1 << 0 : 0;
 
-	if (csi2_subdev(pcdev, icd)) /* CSI2 mode */
-		value |= 3 << 12;
-	else if (pcdev->is_16bit)
+	if (pcdev->is_16bit)
 		value |= 1 << 12;
 	else if (pcdev->flags & SH_CEU_FLAG_LOWER_8BIT)
 		value |= 2 << 12;
@@ -923,9 +848,7 @@ static int sh_mobile_ceu_set_bus_param(struct soc_camera_device *icd)
 static int sh_mobile_ceu_try_bus_param(struct soc_camera_device *icd,
 				       unsigned char buswidth)
 {
-	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
-	struct sh_mobile_ceu_dev *pcdev = ici->priv;
-	struct v4l2_subdev *sd = find_bus_subdev(pcdev, icd);
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 	unsigned long common_flags = CEU_BUS_FLAGS;
 	struct v4l2_mbus_config cfg = {.type = V4L2_MBUS_PARALLEL,};
 	int ret;
@@ -1046,12 +969,9 @@ static int sh_mobile_ceu_get_formats(struct soc_camera_device *icd, unsigned int
 		return 0;
 	}
 
-	if (!csi2_subdev(pcdev, icd)) {
-		/* Are there any restrictions in the CSI-2 case? */
-		ret = sh_mobile_ceu_try_bus_param(icd, fmt->bits_per_sample);
-		if (ret < 0)
-			return 0;
-	}
+	ret = sh_mobile_ceu_try_bus_param(icd, fmt->bits_per_sample);
+	if (ret < 0)
+		return 0;
 
 	if (!icd->host_priv) {
 		struct v4l2_subdev_format fmt = {
@@ -1189,17 +1109,16 @@ static void sh_mobile_ceu_put_formats(struct soc_camera_device *icd)
  * Documentation/video4linux/sh_mobile_ceu_camera.txt for a description of
  * scaling and cropping algorithms and for the meaning of referenced here steps.
  */
-static int sh_mobile_ceu_set_crop(struct soc_camera_device *icd,
-				  const struct v4l2_crop *a)
+static int sh_mobile_ceu_set_selection(struct soc_camera_device *icd,
+				       struct v4l2_selection *sel)
 {
-	struct v4l2_crop a_writable = *a;
-	const struct v4l2_rect *rect = &a_writable.c;
+	struct v4l2_rect *rect = &sel->r;
 	struct device *dev = icd->parent;
 	struct soc_camera_host *ici = to_soc_camera_host(dev);
 	struct sh_mobile_ceu_dev *pcdev = ici->priv;
-	struct v4l2_crop cam_crop;
+	struct v4l2_selection cam_sel;
 	struct sh_mobile_ceu_cam *cam = icd->host_priv;
-	struct v4l2_rect *cam_rect = &cam_crop.c;
+	struct v4l2_rect *cam_rect = &cam_sel.r;
 	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 	struct v4l2_subdev_format fmt = {
 		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
@@ -1211,7 +1130,7 @@ static int sh_mobile_ceu_set_crop(struct soc_camera_device *icd,
 	u32 capsr, cflcr;
 	int ret;
 
-	dev_geo(dev, "S_CROP(%ux%u@%u:%u)\n", rect->width, rect->height,
+	dev_geo(dev, "S_SELECTION(%ux%u@%u:%u)\n", rect->width, rect->height,
 		rect->left, rect->top);
 
 	/* During camera cropping its output window can change too, stop CEU */
@@ -1219,10 +1138,10 @@ static int sh_mobile_ceu_set_crop(struct soc_camera_device *icd,
 	dev_dbg(dev, "CAPSR 0x%x, CFLCR 0x%x\n", capsr, pcdev->cflcr);
 
 	/*
-	 * 1. - 2. Apply iterative camera S_CROP for new input window, read back
+	 * 1. - 2. Apply iterative camera S_SELECTION for new input window, read back
 	 * actual camera rectangle.
 	 */
-	ret = soc_camera_client_s_crop(sd, &a_writable, &cam_crop,
+	ret = soc_camera_client_s_selection(sd, sel, &cam_sel,
 				       &cam->rect, &cam->subrect);
 	if (ret < 0)
 		return ret;
@@ -1331,13 +1250,12 @@ static int sh_mobile_ceu_set_crop(struct soc_camera_device *icd,
 	return ret;
 }
 
-static int sh_mobile_ceu_get_crop(struct soc_camera_device *icd,
-				  struct v4l2_crop *a)
+static int sh_mobile_ceu_get_selection(struct soc_camera_device *icd,
+				       struct v4l2_selection *sel)
 {
 	struct sh_mobile_ceu_cam *cam = icd->host_priv;
 
-	a->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-	a->c = cam->subrect;
+	sel->r = cam->subrect;
 
 	return 0;
 }
@@ -1579,8 +1497,8 @@ static int sh_mobile_ceu_try_fmt(struct soc_camera_device *icd,
 	return ret;
 }
 
-static int sh_mobile_ceu_set_livecrop(struct soc_camera_device *icd,
-				      const struct v4l2_crop *a)
+static int sh_mobile_ceu_set_liveselection(struct soc_camera_device *icd,
+					   struct v4l2_selection *sel)
 {
 	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
@@ -1599,7 +1517,7 @@ static int sh_mobile_ceu_set_livecrop(struct soc_camera_device *icd,
 			 "Client failed to stop the stream: %d\n", ret);
 	else
 		/* Do the crop, if it fails, there's nothing more we can do */
-		sh_mobile_ceu_set_crop(icd, a);
+		sh_mobile_ceu_set_selection(icd, sel);
 
 	dev_geo(icd->parent, "Output after crop: %ux%u\n", icd->user_width, icd->user_height);
 
@@ -1680,9 +1598,9 @@ static struct soc_camera_host_ops sh_mobile_ceu_host_ops = {
 	.clock_stop	= sh_mobile_ceu_clock_stop,
 	.get_formats	= sh_mobile_ceu_get_formats,
 	.put_formats	= sh_mobile_ceu_put_formats,
-	.get_crop	= sh_mobile_ceu_get_crop,
-	.set_crop	= sh_mobile_ceu_set_crop,
-	.set_livecrop	= sh_mobile_ceu_set_livecrop,
+	.get_selection	= sh_mobile_ceu_get_selection,
+	.set_selection	= sh_mobile_ceu_set_selection,
+	.set_liveselection	= sh_mobile_ceu_set_liveselection,
 	.set_fmt	= sh_mobile_ceu_set_fmt,
 	.try_fmt	= sh_mobile_ceu_try_fmt,
 	.poll		= sh_mobile_ceu_poll,
@@ -1721,12 +1639,11 @@ static int sh_mobile_ceu_probe(struct platform_device *pdev)
 	struct resource *res;
 	void __iomem *base;
 	unsigned int irq;
-	int err, i;
+	int err;
 	struct bus_wait wait = {
 		.completion = COMPLETION_INITIALIZER_ONSTACK(wait.completion),
 		.notifier.notifier_call = bus_notify,
 	};
-	struct sh_mobile_ceu_companion *csi2;
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	irq = platform_get_irq(pdev, 0);
@@ -1821,132 +1738,16 @@ static int sh_mobile_ceu_probe(struct platform_device *pdev)
 	pcdev->ici.capabilities = SOCAM_HOST_CAP_STRIDE;
 
 	if (pcdev->pdata && pcdev->pdata->asd_sizes) {
-		struct v4l2_async_subdev **asd;
-		char name[] = "sh-mobile-csi2";
-		int j;
-
-		/*
-		 * CSI2 interfacing: several groups can use CSI2, pick up the
-		 * first one
-		 */
-		asd = pcdev->pdata->asd;
-		for (j = 0; pcdev->pdata->asd_sizes[j]; j++) {
-			for (i = 0; i < pcdev->pdata->asd_sizes[j]; i++, asd++) {
-				dev_dbg(&pdev->dev, "%s(): subdev #%d, type %u\n",
-					__func__, i, (*asd)->match_type);
-				if ((*asd)->match_type == V4L2_ASYNC_MATCH_DEVNAME &&
-				    !strncmp(name, (*asd)->match.device_name.name,
-					     sizeof(name) - 1)) {
-					pcdev->csi2_asd = *asd;
-					break;
-				}
-			}
-			if (pcdev->csi2_asd)
-				break;
-		}
-
 		pcdev->ici.asd = pcdev->pdata->asd;
 		pcdev->ici.asd_sizes = pcdev->pdata->asd_sizes;
 	}
 
-	/* Legacy CSI2 interfacing */
-	csi2 = pcdev->pdata ? pcdev->pdata->csi2 : NULL;
-	if (csi2) {
-		/*
-		 * TODO: remove this once all users are converted to
-		 * asynchronous CSI2 probing. If it has to be kept, csi2
-		 * platform device resources have to be added, using
-		 * platform_device_add_resources()
-		 */
-		struct platform_device *csi2_pdev =
-			platform_device_alloc("sh-mobile-csi2", csi2->id);
-		struct sh_csi2_pdata *csi2_pdata = csi2->platform_data;
-
-		if (!csi2_pdev) {
-			err = -ENOMEM;
-			goto exit_free_clk;
-		}
-
-		pcdev->csi2_pdev		= csi2_pdev;
-
-		err = platform_device_add_data(csi2_pdev, csi2_pdata,
-					       sizeof(*csi2_pdata));
-		if (err < 0)
-			goto exit_pdev_put;
-
-		csi2_pdev->resource		= csi2->resource;
-		csi2_pdev->num_resources	= csi2->num_resources;
-
-		err = platform_device_add(csi2_pdev);
-		if (err < 0)
-			goto exit_pdev_put;
-
-		wait.dev = &csi2_pdev->dev;
-
-		err = bus_register_notifier(&platform_bus_type, &wait.notifier);
-		if (err < 0)
-			goto exit_pdev_unregister;
-
-		/*
-		 * From this point the driver module will not unload, until
-		 * we complete the completion.
-		 */
-
-		if (!csi2_pdev->dev.driver) {
-			complete(&wait.completion);
-			/* Either too late, or probing failed */
-			bus_unregister_notifier(&platform_bus_type, &wait.notifier);
-			err = -ENXIO;
-			goto exit_pdev_unregister;
-		}
-
-		/*
-		 * The module is still loaded, in the worst case it is hanging
-		 * in device release on our completion. So, _now_ dereferencing
-		 * the "owner" is safe!
-		 */
-
-		err = try_module_get(csi2_pdev->dev.driver->owner);
-
-		/* Let notifier complete, if it has been locked */
-		complete(&wait.completion);
-		bus_unregister_notifier(&platform_bus_type, &wait.notifier);
-		if (!err) {
-			err = -ENODEV;
-			goto exit_pdev_unregister;
-		}
-
-		pcdev->csi2_sd = platform_get_drvdata(csi2_pdev);
-	}
-
 	err = soc_camera_host_register(&pcdev->ici);
 	if (err)
-		goto exit_csi2_unregister;
-
-	if (csi2) {
-		err = v4l2_device_register_subdev(&pcdev->ici.v4l2_dev,
-						  pcdev->csi2_sd);
-		dev_dbg(&pdev->dev, "%s(): ret(register_subdev) = %d\n",
-			__func__, err);
-		if (err < 0)
-			goto exit_host_unregister;
-		/* v4l2_device_register_subdev() took a reference too */
-		module_put(pcdev->csi2_sd->owner);
-	}
+		goto exit_free_clk;
 
 	return 0;
 
-exit_host_unregister:
-	soc_camera_host_unregister(&pcdev->ici);
-exit_csi2_unregister:
-	if (csi2) {
-		module_put(pcdev->csi2_pdev->dev.driver->owner);
-exit_pdev_unregister:
-		platform_device_del(pcdev->csi2_pdev);
-exit_pdev_put:
-		pcdev->csi2_pdev->resource = NULL;
-		platform_device_put(pcdev->csi2_pdev);
-	}
 exit_free_clk:
 	pm_runtime_disable(&pdev->dev);
 exit_release_mem:
@@ -1958,21 +1759,11 @@ exit_release_mem:
 static int sh_mobile_ceu_remove(struct platform_device *pdev)
 {
 	struct soc_camera_host *soc_host = to_soc_camera_host(&pdev->dev);
-	struct sh_mobile_ceu_dev *pcdev = container_of(soc_host,
-					struct sh_mobile_ceu_dev, ici);
-	struct platform_device *csi2_pdev = pcdev->csi2_pdev;
 
 	soc_camera_host_unregister(soc_host);
 	pm_runtime_disable(&pdev->dev);
 	if (platform_get_resource(pdev, IORESOURCE_MEM, 1))
 		dma_release_declared_memory(&pdev->dev);
-	if (csi2_pdev && csi2_pdev->dev.driver) {
-		struct module *csi2_drv = csi2_pdev->dev.driver->owner;
-		platform_device_del(csi2_pdev);
-		csi2_pdev->resource = NULL;
-		platform_device_put(csi2_pdev);
-		module_put(csi2_drv);
-	}
 
 	return 0;
 }
@@ -2012,8 +1803,6 @@ static struct platform_driver sh_mobile_ceu_driver = {
 
 static int __init sh_mobile_ceu_init(void)
 {
-	/* Whatever return code */
-	request_module("sh_mobile_csi2");
 	return platform_driver_register(&sh_mobile_ceu_driver);
 }
 
diff --git a/drivers/media/platform/soc_camera/sh_mobile_csi2.c b/drivers/media/platform/soc_camera/sh_mobile_csi2.c
deleted file mode 100644
index 09b18365a4b1..000000000000
--- a/drivers/media/platform/soc_camera/sh_mobile_csi2.c
+++ /dev/null
@@ -1,400 +0,0 @@
-/*
- * Driver for the SH-Mobile MIPI CSI-2 unit
- *
- * Copyright (C) 2010, Guennadi Liakhovetski <g.liakhovetski@gmx.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/delay.h>
-#include <linux/err.h>
-#include <linux/i2c.h>
-#include <linux/io.h>
-#include <linux/platform_device.h>
-#include <linux/pm_runtime.h>
-#include <linux/slab.h>
-#include <linux/videodev2.h>
-#include <linux/module.h>
-
-#include <media/drv-intf/sh_mobile_ceu.h>
-#include <media/drv-intf/sh_mobile_csi2.h>
-#include <media/soc_camera.h>
-#include <media/drv-intf/soc_mediabus.h>
-#include <media/v4l2-common.h>
-#include <media/v4l2-dev.h>
-#include <media/v4l2-device.h>
-#include <media/v4l2-mediabus.h>
-#include <media/v4l2-subdev.h>
-
-#define SH_CSI2_TREF	0x00
-#define SH_CSI2_SRST	0x04
-#define SH_CSI2_PHYCNT	0x08
-#define SH_CSI2_CHKSUM	0x0C
-#define SH_CSI2_VCDT	0x10
-
-struct sh_csi2 {
-	struct v4l2_subdev		subdev;
-	unsigned int			irq;
-	unsigned long			mipi_flags;
-	void __iomem			*base;
-	struct platform_device		*pdev;
-	struct sh_csi2_client_config	*client;
-};
-
-static void sh_csi2_hwinit(struct sh_csi2 *priv);
-
-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;
-	else if (mf->width & 1)
-		mf->width &= ~1;
-
-	switch (pdata->type) {
-	case SH_CSI2C:
-		switch (mf->code) {
-		case MEDIA_BUS_FMT_UYVY8_2X8:		/* YUV422 */
-		case MEDIA_BUS_FMT_YUYV8_1_5X8:		/* YUV420 */
-		case MEDIA_BUS_FMT_Y8_1X8:		/* RAW8 */
-		case MEDIA_BUS_FMT_SBGGR8_1X8:
-		case MEDIA_BUS_FMT_SGRBG8_1X8:
-			break;
-		default:
-			/* All MIPI CSI-2 devices must support one of primary formats */
-			mf->code = MEDIA_BUS_FMT_YUYV8_2X8;
-		}
-		break;
-	case SH_CSI2I:
-		switch (mf->code) {
-		case MEDIA_BUS_FMT_Y8_1X8:		/* RAW8 */
-		case MEDIA_BUS_FMT_SBGGR8_1X8:
-		case MEDIA_BUS_FMT_SGRBG8_1X8:
-		case MEDIA_BUS_FMT_SBGGR10_1X10:	/* RAW10 */
-		case MEDIA_BUS_FMT_SBGGR12_1X12:	/* RAW12 */
-			break;
-		default:
-			/* All MIPI CSI-2 devices must support one of primary formats */
-			mf->code = MEDIA_BUS_FMT_SBGGR8_1X8;
-		}
-		break;
-	}
-
-	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
-		cfg->try_fmt = *mf;
-		return 0;
-	}
-
-	if (mf->width > 8188 || mf->width & 1)
-		return -EINVAL;
-
-	switch (mf->code) {
-	case MEDIA_BUS_FMT_UYVY8_2X8:
-		tmp |= 0x1e;	/* YUV422 8 bit */
-		break;
-	case MEDIA_BUS_FMT_YUYV8_1_5X8:
-		tmp |= 0x18;	/* YUV420 8 bit */
-		break;
-	case MEDIA_BUS_FMT_RGB555_2X8_PADHI_BE:
-		tmp |= 0x21;	/* RGB555 */
-		break;
-	case MEDIA_BUS_FMT_RGB565_2X8_BE:
-		tmp |= 0x22;	/* RGB565 */
-		break;
-	case MEDIA_BUS_FMT_Y8_1X8:
-	case MEDIA_BUS_FMT_SBGGR8_1X8:
-	case MEDIA_BUS_FMT_SGRBG8_1X8:
-		tmp |= 0x2a;	/* RAW8 */
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	iowrite32(tmp, priv->base + SH_CSI2_VCDT);
-
-	return 0;
-}
-
-static int sh_csi2_g_mbus_config(struct v4l2_subdev *sd,
-				 struct v4l2_mbus_config *cfg)
-{
-	struct sh_csi2 *priv = container_of(sd, struct sh_csi2, subdev);
-
-	if (!priv->mipi_flags) {
-		struct soc_camera_device *icd = v4l2_get_subdev_hostdata(sd);
-		struct v4l2_subdev *client_sd = soc_camera_to_subdev(icd);
-		struct sh_csi2_pdata *pdata = priv->pdev->dev.platform_data;
-		unsigned long common_flags, csi2_flags;
-		struct v4l2_mbus_config client_cfg = {.type = V4L2_MBUS_CSI2,};
-		int ret;
-
-		/* Check if we can support this camera */
-		csi2_flags = V4L2_MBUS_CSI2_CONTINUOUS_CLOCK |
-			V4L2_MBUS_CSI2_1_LANE;
-
-		switch (pdata->type) {
-		case SH_CSI2C:
-			if (priv->client->lanes != 1)
-				csi2_flags |= V4L2_MBUS_CSI2_2_LANE;
-			break;
-		case SH_CSI2I:
-			switch (priv->client->lanes) {
-			default:
-				csi2_flags |= V4L2_MBUS_CSI2_4_LANE;
-			case 3:
-				csi2_flags |= V4L2_MBUS_CSI2_3_LANE;
-			case 2:
-				csi2_flags |= V4L2_MBUS_CSI2_2_LANE;
-			}
-		}
-
-		ret = v4l2_subdev_call(client_sd, video, g_mbus_config, &client_cfg);
-		if (ret == -ENOIOCTLCMD)
-			common_flags = csi2_flags;
-		else if (!ret)
-			common_flags = soc_mbus_config_compatible(&client_cfg,
-								  csi2_flags);
-		else
-			common_flags = 0;
-
-		if (!common_flags)
-			return -EINVAL;
-
-		/* All good: camera MIPI configuration supported */
-		priv->mipi_flags = common_flags;
-	}
-
-	if (cfg) {
-		cfg->flags = V4L2_MBUS_PCLK_SAMPLE_RISING |
-			V4L2_MBUS_HSYNC_ACTIVE_HIGH | V4L2_MBUS_VSYNC_ACTIVE_HIGH |
-			V4L2_MBUS_MASTER | V4L2_MBUS_DATA_ACTIVE_HIGH;
-		cfg->type = V4L2_MBUS_PARALLEL;
-	}
-
-	return 0;
-}
-
-static int sh_csi2_s_mbus_config(struct v4l2_subdev *sd,
-				 const struct v4l2_mbus_config *cfg)
-{
-	struct sh_csi2 *priv = container_of(sd, struct sh_csi2, subdev);
-	struct soc_camera_device *icd = v4l2_get_subdev_hostdata(sd);
-	struct v4l2_subdev *client_sd = soc_camera_to_subdev(icd);
-	struct v4l2_mbus_config client_cfg = {.type = V4L2_MBUS_CSI2,};
-	int ret = sh_csi2_g_mbus_config(sd, NULL);
-
-	if (ret < 0)
-		return ret;
-
-	pm_runtime_get_sync(&priv->pdev->dev);
-
-	sh_csi2_hwinit(priv);
-
-	client_cfg.flags = priv->mipi_flags;
-
-	return v4l2_subdev_call(client_sd, video, s_mbus_config, &client_cfg);
-}
-
-static struct v4l2_subdev_video_ops sh_csi2_subdev_video_ops = {
-	.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;
-	__u32 tmp = 0x10; /* Enable MIPI CSI clock lane */
-
-	/* Reflect registers immediately */
-	iowrite32(0x00000001, priv->base + SH_CSI2_TREF);
-	/* reset CSI2 harware */
-	iowrite32(0x00000001, priv->base + SH_CSI2_SRST);
-	udelay(5);
-	iowrite32(0x00000000, priv->base + SH_CSI2_SRST);
-
-	switch (pdata->type) {
-	case SH_CSI2C:
-		if (priv->client->lanes == 1)
-			tmp |= 1;
-		else
-			/* Default - both lanes */
-			tmp |= 3;
-		break;
-	case SH_CSI2I:
-		if (!priv->client->lanes || priv->client->lanes > 4)
-			/* Default - all 4 lanes */
-			tmp |= 0xf;
-		else
-			tmp |= (1 << priv->client->lanes) - 1;
-	}
-
-	if (priv->client->phy == SH_CSI2_PHY_MAIN)
-		tmp |= 0x8000;
-
-	iowrite32(tmp, priv->base + SH_CSI2_PHYCNT);
-
-	tmp = 0;
-	if (pdata->flags & SH_CSI2_ECC)
-		tmp |= 2;
-	if (pdata->flags & SH_CSI2_CRC)
-		tmp |= 1;
-	iowrite32(tmp, priv->base + SH_CSI2_CHKSUM);
-}
-
-static int sh_csi2_client_connect(struct sh_csi2 *priv)
-{
-	struct device *dev = v4l2_get_subdevdata(&priv->subdev);
-	struct sh_csi2_pdata *pdata = dev->platform_data;
-	struct soc_camera_device *icd = v4l2_get_subdev_hostdata(&priv->subdev);
-	int i;
-
-	if (priv->client)
-		return -EBUSY;
-
-	for (i = 0; i < pdata->num_clients; i++)
-		if ((pdata->clients[i].pdev &&
-		     &pdata->clients[i].pdev->dev == icd->pdev) ||
-		    (icd->control &&
-		     strcmp(pdata->clients[i].name, dev_name(icd->control))))
-			break;
-
-	dev_dbg(dev, "%s(%p): found #%d\n", __func__, dev, i);
-
-	if (i == pdata->num_clients)
-		return -ENODEV;
-
-	priv->client = pdata->clients + i;
-
-	return 0;
-}
-
-static void sh_csi2_client_disconnect(struct sh_csi2 *priv)
-{
-	if (!priv->client)
-		return;
-
-	priv->client = NULL;
-
-	pm_runtime_put(v4l2_get_subdevdata(&priv->subdev));
-}
-
-static int sh_csi2_s_power(struct v4l2_subdev *sd, int on)
-{
-	struct sh_csi2 *priv = container_of(sd, struct sh_csi2, subdev);
-
-	if (on)
-		return sh_csi2_client_connect(priv);
-
-	sh_csi2_client_disconnect(priv);
-	return 0;
-}
-
-static struct v4l2_subdev_core_ops sh_csi2_subdev_core_ops = {
-	.s_power	= sh_csi2_s_power,
-};
-
-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)
-{
-	struct resource *res;
-	unsigned int irq;
-	int ret;
-	struct sh_csi2 *priv;
-	/* Platform data specify the PHY, lanes, ECC, CRC */
-	struct sh_csi2_pdata *pdata = pdev->dev.platform_data;
-
-	if (!pdata)
-		return -EINVAL;
-
-	priv = devm_kzalloc(&pdev->dev, sizeof(struct sh_csi2), GFP_KERNEL);
-	if (!priv)
-		return -ENOMEM;
-
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	/* Interrupt unused so far */
-	irq = platform_get_irq(pdev, 0);
-
-	if (!res || (int)irq <= 0) {
-		dev_err(&pdev->dev, "Not enough CSI2 platform resources.\n");
-		return -ENODEV;
-	}
-
-	/* TODO: Add support for CSI2I. Careful: different register layout! */
-	if (pdata->type != SH_CSI2C) {
-		dev_err(&pdev->dev, "Only CSI2C supported ATM.\n");
-		return -EINVAL;
-	}
-
-	priv->irq = irq;
-
-	priv->base = devm_ioremap_resource(&pdev->dev, res);
-	if (IS_ERR(priv->base))
-		return PTR_ERR(priv->base);
-
-	priv->pdev = pdev;
-	priv->subdev.owner = THIS_MODULE;
-	priv->subdev.dev = &pdev->dev;
-	platform_set_drvdata(pdev, &priv->subdev);
-
-	v4l2_subdev_init(&priv->subdev, &sh_csi2_subdev_ops);
-	v4l2_set_subdevdata(&priv->subdev, &pdev->dev);
-
-	snprintf(priv->subdev.name, V4L2_SUBDEV_NAME_SIZE, "%s.mipi-csi",
-		 dev_name(&pdev->dev));
-
-	ret = v4l2_async_register_subdev(&priv->subdev);
-	if (ret < 0)
-		return ret;
-
-	pm_runtime_enable(&pdev->dev);
-
-	dev_dbg(&pdev->dev, "CSI2 probed.\n");
-
-	return 0;
-}
-
-static int sh_csi2_remove(struct platform_device *pdev)
-{
-	struct v4l2_subdev *subdev = platform_get_drvdata(pdev);
-	struct sh_csi2 *priv = container_of(subdev, struct sh_csi2, subdev);
-
-	v4l2_async_unregister_subdev(&priv->subdev);
-	pm_runtime_disable(&pdev->dev);
-
-	return 0;
-}
-
-static struct platform_driver __refdata sh_csi2_pdrv = {
-	.remove	= sh_csi2_remove,
-	.probe	= sh_csi2_probe,
-	.driver	= {
-		.name	= "sh-mobile-csi2",
-	},
-};
-
-module_platform_driver(sh_csi2_pdrv);
-
-MODULE_DESCRIPTION("SH-Mobile MIPI CSI-2 driver");
-MODULE_AUTHOR("Guennadi Liakhovetski <g.liakhovetski@gmx.de>");
-MODULE_LICENSE("GPL v2");
-MODULE_ALIAS("platform:sh-mobile-csi2");
diff --git a/drivers/media/platform/soc_camera/soc_camera.c b/drivers/media/platform/soc_camera/soc_camera.c
index 46c7186f7867..edd1c1de4e33 100644
--- a/drivers/media/platform/soc_camera/soc_camera.c
+++ b/drivers/media/platform/soc_camera/soc_camera.c
@@ -581,7 +581,7 @@ static int soc_camera_set_fmt(struct soc_camera_device *icd,
 	dev_dbg(icd->pdev, "S_FMT(%c%c%c%c, %ux%u)\n",
 		pixfmtstr(pix->pixelformat), pix->width, pix->height);
 
-	/* We always call try_fmt() before set_fmt() or set_crop() */
+	/* We always call try_fmt() before set_fmt() or set_selection() */
 	ret = soc_camera_try_fmt(icd, f);
 	if (ret < 0)
 		return ret;
@@ -1025,72 +1025,6 @@ static int soc_camera_streamoff(struct file *file, void *priv,
 	return ret;
 }
 
-static int soc_camera_cropcap(struct file *file, void *fh,
-			      struct v4l2_cropcap *a)
-{
-	struct soc_camera_device *icd = file->private_data;
-	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
-
-	return ici->ops->cropcap(icd, a);
-}
-
-static int soc_camera_g_crop(struct file *file, void *fh,
-			     struct v4l2_crop *a)
-{
-	struct soc_camera_device *icd = file->private_data;
-	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
-	int ret;
-
-	ret = ici->ops->get_crop(icd, a);
-
-	return ret;
-}
-
-/*
- * According to the V4L2 API, drivers shall not update the struct v4l2_crop
- * argument with the actual geometry, instead, the user shall use G_CROP to
- * retrieve it.
- */
-static int soc_camera_s_crop(struct file *file, void *fh,
-			     const struct v4l2_crop *a)
-{
-	struct soc_camera_device *icd = file->private_data;
-	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
-	const struct v4l2_rect *rect = &a->c;
-	struct v4l2_crop current_crop;
-	int ret;
-
-	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
-
-	dev_dbg(icd->pdev, "S_CROP(%ux%u@%u:%u)\n",
-		rect->width, rect->height, rect->left, rect->top);
-
-	current_crop.type = a->type;
-
-	/* If get_crop fails, we'll let host and / or client drivers decide */
-	ret = ici->ops->get_crop(icd, &current_crop);
-
-	/* Prohibit window size change with initialised buffers */
-	if (ret < 0) {
-		dev_err(icd->pdev,
-			"S_CROP denied: getting current crop failed\n");
-	} else if ((a->c.width == current_crop.c.width &&
-		    a->c.height == current_crop.c.height) ||
-		   !is_streaming(ici, icd)) {
-		/* same size or not streaming - use .set_crop() */
-		ret = ici->ops->set_crop(icd, a);
-	} else if (ici->ops->set_livecrop) {
-		ret = ici->ops->set_livecrop(icd, a);
-	} else {
-		dev_err(icd->pdev,
-			"S_CROP denied: queue initialised and sizes differ\n");
-		ret = -EBUSY;
-	}
-
-	return ret;
-}
-
 static int soc_camera_g_selection(struct file *file, void *fh,
 				  struct v4l2_selection *s)
 {
@@ -1101,9 +1035,6 @@ static int soc_camera_g_selection(struct file *file, void *fh,
 	if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 		return -EINVAL;
 
-	if (!ici->ops->get_selection)
-		return -ENOTTY;
-
 	return ici->ops->get_selection(icd, s);
 }
 
@@ -1135,10 +1066,11 @@ static int soc_camera_s_selection(struct file *file, void *fh,
 			return -EBUSY;
 	}
 
-	if (!ici->ops->set_selection)
-		return -ENOTTY;
-
-	ret = ici->ops->set_selection(icd, s);
+	if (s->target == V4L2_SEL_TGT_CROP && is_streaming(ici, icd) &&
+	    ici->ops->set_liveselection)
+		ret = ici->ops->set_liveselection(icd, s);
+	else
+		ret = ici->ops->set_selection(icd, s);
 	if (!ret &&
 	    s->target == V4L2_SEL_TGT_COMPOSE) {
 		icd->user_width = s->r.width;
@@ -1881,23 +1813,40 @@ static int soc_camera_remove(struct soc_camera_device *icd)
 	return 0;
 }
 
-static int default_cropcap(struct soc_camera_device *icd,
-			   struct v4l2_cropcap *a)
+static int default_g_selection(struct soc_camera_device *icd,
+			       struct v4l2_selection *sel)
 {
 	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
-	return v4l2_subdev_call(sd, video, cropcap, a);
-}
+	struct v4l2_subdev_selection sdsel = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		.target = sel->target,
+	};
+	int ret;
 
-static int default_g_crop(struct soc_camera_device *icd, struct v4l2_crop *a)
-{
-	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
-	return v4l2_subdev_call(sd, video, g_crop, a);
+	ret = v4l2_subdev_call(sd, pad, get_selection, NULL, &sdsel);
+	if (ret)
+		return ret;
+	sel->r = sdsel.r;
+	return 0;
 }
 
-static int default_s_crop(struct soc_camera_device *icd, const struct v4l2_crop *a)
+static int default_s_selection(struct soc_camera_device *icd,
+			       struct v4l2_selection *sel)
 {
 	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
-	return v4l2_subdev_call(sd, video, s_crop, a);
+	struct v4l2_subdev_selection sdsel = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		.target = sel->target,
+		.flags = sel->flags,
+		.r = sel->r,
+	};
+	int ret;
+
+	ret = v4l2_subdev_call(sd, pad, set_selection, NULL, &sdsel);
+	if (ret)
+		return ret;
+	sel->r = sdsel.r;
+	return 0;
 }
 
 static int default_g_parm(struct soc_camera_device *icd,
@@ -1968,12 +1917,10 @@ int soc_camera_host_register(struct soc_camera_host *ici)
 	    !ici->v4l2_dev.dev)
 		return -EINVAL;
 
-	if (!ici->ops->set_crop)
-		ici->ops->set_crop = default_s_crop;
-	if (!ici->ops->get_crop)
-		ici->ops->get_crop = default_g_crop;
-	if (!ici->ops->cropcap)
-		ici->ops->cropcap = default_cropcap;
+	if (!ici->ops->set_selection)
+		ici->ops->set_selection = default_s_selection;
+	if (!ici->ops->get_selection)
+		ici->ops->get_selection = default_g_selection;
 	if (!ici->ops->set_parm)
 		ici->ops->set_parm = default_s_parm;
 	if (!ici->ops->get_parm)
@@ -2126,9 +2073,6 @@ static const struct v4l2_ioctl_ops soc_camera_ioctl_ops = {
 	.vidioc_expbuf		 = soc_camera_expbuf,
 	.vidioc_streamon	 = soc_camera_streamon,
 	.vidioc_streamoff	 = soc_camera_streamoff,
-	.vidioc_cropcap		 = soc_camera_cropcap,
-	.vidioc_g_crop		 = soc_camera_g_crop,
-	.vidioc_s_crop		 = soc_camera_s_crop,
 	.vidioc_g_selection	 = soc_camera_g_selection,
 	.vidioc_s_selection	 = soc_camera_s_selection,
 	.vidioc_g_parm		 = soc_camera_g_parm,
diff --git a/drivers/media/platform/soc_camera/soc_camera_platform.c b/drivers/media/platform/soc_camera/soc_camera_platform.c
index a51d2a42998c..534d6c3c6d60 100644
--- a/drivers/media/platform/soc_camera/soc_camera_platform.c
+++ b/drivers/media/platform/soc_camera/soc_camera_platform.c
@@ -76,35 +76,27 @@ static int soc_camera_platform_enum_mbus_code(struct v4l2_subdev *sd,
 	return 0;
 }
 
-static int soc_camera_platform_g_crop(struct v4l2_subdev *sd,
-				      struct v4l2_crop *a)
-{
-	struct soc_camera_platform_info *p = v4l2_get_subdevdata(sd);
-
-	a->c.left	= 0;
-	a->c.top	= 0;
-	a->c.width	= p->format.width;
-	a->c.height	= p->format.height;
-	a->type		= V4L2_BUF_TYPE_VIDEO_CAPTURE;
-
-	return 0;
-}
-
-static int soc_camera_platform_cropcap(struct v4l2_subdev *sd,
-				       struct v4l2_cropcap *a)
+static int soc_camera_platform_get_selection(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_selection *sel)
 {
 	struct soc_camera_platform_info *p = v4l2_get_subdevdata(sd);
 
-	a->bounds.left			= 0;
-	a->bounds.top			= 0;
-	a->bounds.width			= p->format.width;
-	a->bounds.height		= p->format.height;
-	a->defrect			= a->bounds;
-	a->type				= V4L2_BUF_TYPE_VIDEO_CAPTURE;
-	a->pixelaspect.numerator	= 1;
-	a->pixelaspect.denominator	= 1;
+	if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE)
+		return -EINVAL;
 
-	return 0;
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+	case V4L2_SEL_TGT_CROP:
+		sel->r.left = 0;
+		sel->r.top = 0;
+		sel->r.width = p->format.width;
+		sel->r.height = p->format.height;
+		return 0;
+	default:
+		return -EINVAL;
+	}
 }
 
 static int soc_camera_platform_g_mbus_config(struct v4l2_subdev *sd,
@@ -120,13 +112,12 @@ 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,
-	.cropcap	= soc_camera_platform_cropcap,
-	.g_crop		= soc_camera_platform_g_crop,
 	.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_selection	= soc_camera_platform_get_selection,
 	.get_fmt	= soc_camera_platform_fill_fmt,
 	.set_fmt	= soc_camera_platform_fill_fmt,
 };
diff --git a/drivers/media/platform/soc_camera/soc_scale_crop.c b/drivers/media/platform/soc_camera/soc_scale_crop.c
index bda29bc1b933..f77252d6ccd3 100644
--- a/drivers/media/platform/soc_camera/soc_scale_crop.c
+++ b/drivers/media/platform/soc_camera/soc_scale_crop.c
@@ -40,24 +40,22 @@ static bool is_inside(const struct v4l2_rect *r1, const struct v4l2_rect *r2)
 /* Get and store current client crop */
 int soc_camera_client_g_rect(struct v4l2_subdev *sd, struct v4l2_rect *rect)
 {
-	struct v4l2_crop crop;
-	struct v4l2_cropcap cap;
+	struct v4l2_subdev_selection sdsel = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		.target = V4L2_SEL_TGT_CROP,
+	};
 	int ret;
 
-	crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-
-	ret = v4l2_subdev_call(sd, video, g_crop, &crop);
+	ret = v4l2_subdev_call(sd, pad, get_selection, NULL, &sdsel);
 	if (!ret) {
-		*rect = crop.c;
+		*rect = sdsel.r;
 		return ret;
 	}
 
-	/* Camera driver doesn't support .g_crop(), assume default rectangle */
-	cap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-
-	ret = v4l2_subdev_call(sd, video, cropcap, &cap);
+	sdsel.target = V4L2_SEL_TGT_CROP_DEFAULT;
+	ret = v4l2_subdev_call(sd, pad, get_selection, NULL, &sdsel);
 	if (!ret)
-		*rect = cap.defrect;
+		*rect = sdsel.r;
 
 	return ret;
 }
@@ -93,17 +91,27 @@ static void update_subrect(struct v4l2_rect *rect, struct v4l2_rect *subrect)
  * 2. if (1) failed, try to double the client image until we get one big enough
  * 3. if (2) failed, try to request the maximum image
  */
-int soc_camera_client_s_crop(struct v4l2_subdev *sd,
-			struct v4l2_crop *crop, struct v4l2_crop *cam_crop,
+int soc_camera_client_s_selection(struct v4l2_subdev *sd,
+			struct v4l2_selection *sel, struct v4l2_selection *cam_sel,
 			struct v4l2_rect *target_rect, struct v4l2_rect *subrect)
 {
-	struct v4l2_rect *rect = &crop->c, *cam_rect = &cam_crop->c;
+	struct v4l2_subdev_selection sdsel = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		.target = sel->target,
+		.flags = sel->flags,
+		.r = sel->r,
+	};
+	struct v4l2_subdev_selection bounds = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		.target = V4L2_SEL_TGT_CROP_BOUNDS,
+	};
+	struct v4l2_rect *rect = &sel->r, *cam_rect = &cam_sel->r;
 	struct device *dev = sd->v4l2_dev->dev;
-	struct v4l2_cropcap cap;
 	int ret;
 	unsigned int width, height;
 
-	v4l2_subdev_call(sd, video, s_crop, crop);
+	v4l2_subdev_call(sd, pad, set_selection, NULL, &sdsel);
+	sel->r = sdsel.r;
 	ret = soc_camera_client_g_rect(sd, cam_rect);
 	if (ret < 0)
 		return ret;
@@ -113,29 +121,29 @@ int soc_camera_client_s_crop(struct v4l2_subdev *sd,
 	 * be within camera cropcap bounds
 	 */
 	if (!memcmp(rect, cam_rect, sizeof(*rect))) {
-		/* Even if camera S_CROP failed, but camera rectangle matches */
-		dev_dbg(dev, "Camera S_CROP successful for %dx%d@%d:%d\n",
+		/* Even if camera S_SELECTION failed, but camera rectangle matches */
+		dev_dbg(dev, "Camera S_SELECTION successful for %dx%d@%d:%d\n",
 			rect->width, rect->height, rect->left, rect->top);
 		*target_rect = *cam_rect;
 		return 0;
 	}
 
 	/* Try to fix cropping, that camera hasn't managed to set */
-	dev_geo(dev, "Fix camera S_CROP for %dx%d@%d:%d to %dx%d@%d:%d\n",
+	dev_geo(dev, "Fix camera S_SELECTION for %dx%d@%d:%d to %dx%d@%d:%d\n",
 		cam_rect->width, cam_rect->height,
 		cam_rect->left, cam_rect->top,
 		rect->width, rect->height, rect->left, rect->top);
 
 	/* We need sensor maximum rectangle */
-	ret = v4l2_subdev_call(sd, video, cropcap, &cap);
+	ret = v4l2_subdev_call(sd, pad, get_selection, NULL, &bounds);
 	if (ret < 0)
 		return ret;
 
 	/* Put user requested rectangle within sensor bounds */
-	soc_camera_limit_side(&rect->left, &rect->width, cap.bounds.left, 2,
-			      cap.bounds.width);
-	soc_camera_limit_side(&rect->top, &rect->height, cap.bounds.top, 4,
-			      cap.bounds.height);
+	soc_camera_limit_side(&rect->left, &rect->width, sdsel.r.left, 2,
+			      bounds.r.width);
+	soc_camera_limit_side(&rect->top, &rect->height, sdsel.r.top, 4,
+			      bounds.r.height);
 
 	/*
 	 * Popular special case - some cameras can only handle fixed sizes like
@@ -150,7 +158,7 @@ int soc_camera_client_s_crop(struct v4l2_subdev *sd,
 	 */
 	while (!ret && (is_smaller(cam_rect, rect) ||
 			is_inside(cam_rect, rect)) &&
-	       (cap.bounds.width > width || cap.bounds.height > height)) {
+	       (bounds.r.width > width || bounds.r.height > height)) {
 
 		width *= 2;
 		height *= 2;
@@ -168,36 +176,40 @@ int soc_camera_client_s_crop(struct v4l2_subdev *sd,
 		 * Instead we just drop to the left and top bounds.
 		 */
 		if (cam_rect->left > rect->left)
-			cam_rect->left = cap.bounds.left;
+			cam_rect->left = bounds.r.left;
 
 		if (cam_rect->left + cam_rect->width < rect->left + rect->width)
 			cam_rect->width = rect->left + rect->width -
 				cam_rect->left;
 
 		if (cam_rect->top > rect->top)
-			cam_rect->top = cap.bounds.top;
+			cam_rect->top = bounds.r.top;
 
 		if (cam_rect->top + cam_rect->height < rect->top + rect->height)
 			cam_rect->height = rect->top + rect->height -
 				cam_rect->top;
 
-		v4l2_subdev_call(sd, video, s_crop, cam_crop);
+		sdsel.r = *cam_rect;
+		v4l2_subdev_call(sd, pad, set_selection, NULL, &sdsel);
+		*cam_rect = sdsel.r;
 		ret = soc_camera_client_g_rect(sd, cam_rect);
-		dev_geo(dev, "Camera S_CROP %d for %dx%d@%d:%d\n", ret,
+		dev_geo(dev, "Camera S_SELECTION %d for %dx%d@%d:%d\n", ret,
 			cam_rect->width, cam_rect->height,
 			cam_rect->left, cam_rect->top);
 	}
 
-	/* S_CROP must not modify the rectangle */
+	/* S_SELECTION must not modify the rectangle */
 	if (is_smaller(cam_rect, rect) || is_inside(cam_rect, rect)) {
 		/*
 		 * The camera failed to configure a suitable cropping,
 		 * we cannot use the current rectangle, set to max
 		 */
-		*cam_rect = cap.bounds;
-		v4l2_subdev_call(sd, video, s_crop, cam_crop);
+		sdsel.r = bounds.r;
+		v4l2_subdev_call(sd, pad, set_selection, NULL, &sdsel);
+		*cam_rect = sdsel.r;
+
 		ret = soc_camera_client_g_rect(sd, cam_rect);
-		dev_geo(dev, "Camera S_CROP %d for max %dx%d@%d:%d\n", ret,
+		dev_geo(dev, "Camera S_SELECTION %d for max %dx%d@%d:%d\n", ret,
 			cam_rect->width, cam_rect->height,
 			cam_rect->left, cam_rect->top);
 	}
@@ -209,7 +221,7 @@ int soc_camera_client_s_crop(struct v4l2_subdev *sd,
 
 	return ret;
 }
-EXPORT_SYMBOL(soc_camera_client_s_crop);
+EXPORT_SYMBOL(soc_camera_client_s_selection);
 
 /* Iterative set_fmt, also updates cached client crop on success */
 static int client_set_fmt(struct soc_camera_device *icd,
@@ -221,7 +233,10 @@ static int client_set_fmt(struct soc_camera_device *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;
+	struct v4l2_subdev_selection sdsel = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		.target = V4L2_SEL_TGT_CROP_BOUNDS,
+	};
 	bool host_1to1;
 	int ret;
 
@@ -243,16 +258,14 @@ static int client_set_fmt(struct soc_camera_device *icd,
 	if (!host_can_scale)
 		goto update_cache;
 
-	cap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-
-	ret = v4l2_subdev_call(sd, video, cropcap, &cap);
+	ret = v4l2_subdev_call(sd, pad, get_selection, NULL, &sdsel);
 	if (ret < 0)
 		return ret;
 
-	if (max_width > cap.bounds.width)
-		max_width = cap.bounds.width;
-	if (max_height > cap.bounds.height)
-		max_height = cap.bounds.height;
+	if (max_width > sdsel.r.width)
+		max_width = sdsel.r.width;
+	if (max_height > sdsel.r.height)
+		max_height = sdsel.r.height;
 
 	/* Camera set a format, but geometry is not precise, try to improve */
 	tmp_w = mf->width;
diff --git a/drivers/media/platform/soc_camera/soc_scale_crop.h b/drivers/media/platform/soc_camera/soc_scale_crop.h
index 184a30dff541..9ca469312a1f 100644
--- a/drivers/media/platform/soc_camera/soc_scale_crop.h
+++ b/drivers/media/platform/soc_camera/soc_scale_crop.h
@@ -16,7 +16,7 @@
 
 struct soc_camera_device;
 
-struct v4l2_crop;
+struct v4l2_selection;
 struct v4l2_mbus_framefmt;
 struct v4l2_pix_format;
 struct v4l2_rect;
@@ -31,8 +31,8 @@ static inline unsigned int soc_camera_shift_scale(unsigned int size,
 #define soc_camera_calc_scale(in, shift, out) soc_camera_shift_scale(in, shift, out)
 
 int soc_camera_client_g_rect(struct v4l2_subdev *sd, struct v4l2_rect *rect);
-int soc_camera_client_s_crop(struct v4l2_subdev *sd,
-			struct v4l2_crop *crop, struct v4l2_crop *cam_crop,
+int soc_camera_client_s_selection(struct v4l2_subdev *sd,
+			struct v4l2_selection *sel, struct v4l2_selection *cam_sel,
 			struct v4l2_rect *target_rect, struct v4l2_rect *subrect);
 int soc_camera_client_scale(struct soc_camera_device *icd,
 			struct v4l2_rect *rect, struct v4l2_rect *subrect,
diff --git a/drivers/media/platform/sti/bdisp/bdisp-v4l2.c b/drivers/media/platform/sti/bdisp/bdisp-v4l2.c
index 3b1ac687d0df..45f82b5ddd77 100644
--- a/drivers/media/platform/sti/bdisp/bdisp-v4l2.c
+++ b/drivers/media/platform/sti/bdisp/bdisp-v4l2.c
@@ -527,7 +527,7 @@ static void bdisp_stop_streaming(struct vb2_queue *q)
 	pm_runtime_put(ctx->bdisp_dev->dev);
 }
 
-static struct vb2_ops bdisp_qops = {
+static const struct vb2_ops bdisp_qops = {
 	.queue_setup     = bdisp_queue_setup,
 	.buf_prepare     = bdisp_buf_prepare,
 	.buf_queue       = bdisp_buf_queue,
diff --git a/drivers/media/platform/sti/hva/Makefile b/drivers/media/platform/sti/hva/Makefile
new file mode 100644
index 000000000000..ffb69cebaef3
--- /dev/null
+++ b/drivers/media/platform/sti/hva/Makefile
@@ -0,0 +1,2 @@
+obj-$(CONFIG_VIDEO_STI_HVA) := st-hva.o
+st-hva-y := hva-v4l2.o hva-hw.o hva-mem.o hva-h264.o
diff --git a/drivers/media/platform/sti/hva/hva-h264.c b/drivers/media/platform/sti/hva/hva-h264.c
new file mode 100644
index 000000000000..8cc8467c0cd3
--- /dev/null
+++ b/drivers/media/platform/sti/hva/hva-h264.c
@@ -0,0 +1,1050 @@
+/*
+ * Copyright (C) STMicroelectronics SA 2015
+ * Authors: Yannick Fertre <yannick.fertre@st.com>
+ *          Hugues Fruchet <hugues.fruchet@st.com>
+ * License terms:  GNU General Public License (GPL), version 2
+ */
+
+#include "hva.h"
+#include "hva-hw.h"
+
+#define MAX_SPS_PPS_SIZE 128
+
+#define BITSTREAM_OFFSET_MASK 0x7F
+
+/* video max size*/
+#define H264_MAX_SIZE_W 1920
+#define H264_MAX_SIZE_H 1920
+
+/* macroBlocs number (width & height) */
+#define MB_W(w) ((w + 0xF)  / 0x10)
+#define MB_H(h) ((h + 0xF)  / 0x10)
+
+/* formula to get temporal or spatial data size */
+#define DATA_SIZE(w, h) (MB_W(w) * MB_H(h) * 16)
+
+#define SEARCH_WINDOW_BUFFER_MAX_SIZE(w) ((4 * MB_W(w) + 42) * 256 * 3 / 2)
+#define CABAC_CONTEXT_BUFFER_MAX_SIZE(w) (MB_W(w) * 16)
+#define CTX_MB_BUFFER_MAX_SIZE(w) (MB_W(w) * 16 * 8)
+#define SLICE_HEADER_SIZE (4 * 16)
+#define BRC_DATA_SIZE (5 * 16)
+
+/* source buffer copy in YUV 420 MB-tiled format with size=16*256*3/2 */
+#define CURRENT_WINDOW_BUFFER_MAX_SIZE (16 * 256 * 3 / 2)
+
+/*
+ * 4 lines of pixels (in Luma, Chroma blue and Chroma red) of top MB
+ * for deblocking with size=4*16*MBx*2
+ */
+#define LOCAL_RECONSTRUCTED_BUFFER_MAX_SIZE(w) (4 * 16 * MB_W(w) * 2)
+
+/* factor for bitrate and cpb buffer size max values if profile >= high */
+#define H264_FACTOR_HIGH 1200
+
+/* factor for bitrate and cpb buffer size max values if profile < high */
+#define H264_FACTOR_BASELINE 1000
+
+/* number of bytes for NALU_TYPE_FILLER_DATA header and footer */
+#define H264_FILLER_DATA_SIZE 6
+
+struct h264_profile {
+	enum v4l2_mpeg_video_h264_level level;
+	u32 max_mb_per_seconds;
+	u32 max_frame_size;
+	u32 max_bitrate;
+	u32 max_cpb_size;
+	u32 min_comp_ratio;
+};
+
+static const struct h264_profile h264_infos_list[] = {
+	{V4L2_MPEG_VIDEO_H264_LEVEL_1_0, 1485, 99, 64, 175, 2},
+	{V4L2_MPEG_VIDEO_H264_LEVEL_1B, 1485, 99, 128, 350, 2},
+	{V4L2_MPEG_VIDEO_H264_LEVEL_1_1, 3000, 396, 192, 500, 2},
+	{V4L2_MPEG_VIDEO_H264_LEVEL_1_2, 6000, 396, 384, 1000, 2},
+	{V4L2_MPEG_VIDEO_H264_LEVEL_1_3, 11880, 396, 768, 2000, 2},
+	{V4L2_MPEG_VIDEO_H264_LEVEL_2_0, 11880, 396, 2000, 2000, 2},
+	{V4L2_MPEG_VIDEO_H264_LEVEL_2_1, 19800, 792, 4000, 4000, 2},
+	{V4L2_MPEG_VIDEO_H264_LEVEL_2_2, 20250, 1620, 4000, 4000, 2},
+	{V4L2_MPEG_VIDEO_H264_LEVEL_3_0, 40500, 1620, 10000, 10000, 2},
+	{V4L2_MPEG_VIDEO_H264_LEVEL_3_1, 108000, 3600, 14000, 14000, 4},
+	{V4L2_MPEG_VIDEO_H264_LEVEL_3_2, 216000, 5120, 20000, 20000, 4},
+	{V4L2_MPEG_VIDEO_H264_LEVEL_4_0, 245760, 8192, 20000, 25000, 4},
+	{V4L2_MPEG_VIDEO_H264_LEVEL_4_1, 245760, 8192, 50000, 62500, 2},
+	{V4L2_MPEG_VIDEO_H264_LEVEL_4_2, 522240, 8704, 50000, 62500, 2},
+	{V4L2_MPEG_VIDEO_H264_LEVEL_5_0, 589824, 22080, 135000, 135000, 2},
+	{V4L2_MPEG_VIDEO_H264_LEVEL_5_1, 983040, 36864, 240000, 240000, 2}
+};
+
+enum hva_brc_type {
+	BRC_TYPE_NONE = 0,
+	BRC_TYPE_CBR = 1,
+	BRC_TYPE_VBR = 2,
+	BRC_TYPE_VBR_LOW_DELAY = 3
+};
+
+enum hva_entropy_coding_mode {
+	CAVLC = 0,
+	CABAC = 1
+};
+
+enum hva_picture_coding_type {
+	PICTURE_CODING_TYPE_I = 0,
+	PICTURE_CODING_TYPE_P = 1,
+	PICTURE_CODING_TYPE_B = 2
+};
+
+enum hva_h264_sampling_mode {
+	SAMPLING_MODE_NV12 = 0,
+	SAMPLING_MODE_UYVY = 1,
+	SAMPLING_MODE_RGB3 = 3,
+	SAMPLING_MODE_XRGB4 = 4,
+	SAMPLING_MODE_NV21 = 8,
+	SAMPLING_MODE_VYUY = 9,
+	SAMPLING_MODE_BGR3 = 11,
+	SAMPLING_MODE_XBGR4 = 12,
+	SAMPLING_MODE_RGBX4 = 20,
+	SAMPLING_MODE_BGRX4 = 28
+};
+
+enum hva_h264_nalu_type {
+	NALU_TYPE_UNKNOWN = 0,
+	NALU_TYPE_SLICE = 1,
+	NALU_TYPE_SLICE_DPA = 2,
+	NALU_TYPE_SLICE_DPB = 3,
+	NALU_TYPE_SLICE_DPC = 4,
+	NALU_TYPE_SLICE_IDR = 5,
+	NALU_TYPE_SEI = 6,
+	NALU_TYPE_SPS = 7,
+	NALU_TYPE_PPS = 8,
+	NALU_TYPE_AU_DELIMITER = 9,
+	NALU_TYPE_SEQ_END = 10,
+	NALU_TYPE_STREAM_END = 11,
+	NALU_TYPE_FILLER_DATA = 12,
+	NALU_TYPE_SPS_EXT = 13,
+	NALU_TYPE_PREFIX_UNIT = 14,
+	NALU_TYPE_SUBSET_SPS = 15,
+	NALU_TYPE_SLICE_AUX = 19,
+	NALU_TYPE_SLICE_EXT = 20
+};
+
+enum hva_h264_sei_payload_type {
+	SEI_BUFFERING_PERIOD = 0,
+	SEI_PICTURE_TIMING = 1,
+	SEI_STEREO_VIDEO_INFO = 21,
+	SEI_FRAME_PACKING_ARRANGEMENT = 45
+};
+
+/**
+ * stereo Video Info struct
+ */
+struct hva_h264_stereo_video_sei {
+	u8 field_views_flag;
+	u8 top_field_is_left_view_flag;
+	u8 current_frame_is_left_view_flag;
+	u8 next_frame_is_second_view_flag;
+	u8 left_view_self_contained_flag;
+	u8 right_view_self_contained_flag;
+};
+
+/**
+ * @frame_width: width in pixels of the buffer containing the input frame
+ * @frame_height: height in pixels of the buffer containing the input frame
+ * @frame_num: the parameter to be written in the slice header
+ * @picture_coding_type: type I, P or B
+ * @pic_order_cnt_type: POC mode, as defined in H264 std : can be 0,1,2
+ * @first_picture_in_sequence: flag telling to encoder that this is the
+ *			       first picture in a video sequence.
+ *			       Used for VBR
+ * @slice_size_type: 0 = no constraint to close the slice
+ *		     1= a slice is closed as soon as the slice_mb_size limit
+ *			is reached
+ *		     2= a slice is closed as soon as the slice_byte_size limit
+ *			is reached
+ *		     3= a slice is closed as soon as either the slice_byte_size
+ *			limit or the slice_mb_size limit is reached
+ * @slice_mb_size: defines the slice size in number of macroblocks
+ *		   (used when slice_size_type=1 or slice_size_type=3)
+ * @ir_param_option: defines the number of macroblocks per frame to be
+ *		     refreshed by AIR algorithm OR the refresh period
+ *		     by CIR algorithm
+ * @intra_refresh_type: enables the adaptive intra refresh algorithm.
+ *			Disable=0 / Adaptative=1 and Cycle=2 as intra refresh
+ * @use_constrained_intra_flag: constrained_intra_pred_flag from PPS
+ * @transform_mode: controls the use of 4x4/8x8 transform mode
+ * @disable_deblocking_filter_idc:
+ *		     0: specifies that all luma and chroma block edges of
+ *			the slice are filtered.
+ *		     1: specifies that deblocking is disabled for all block
+ *			edges of the slice.
+ *		     2: specifies that all luma and chroma block edges of
+ *			the slice are filtered with exception of the block edges
+ *			that coincide with slice boundaries
+ * @slice_alpha_c0_offset_div2: to be written in slice header,
+ *				controls deblocking
+ * @slice_beta_offset_div2: to be written in slice header,
+ *			    controls deblocking
+ * @encoder_complexity: encoder complexity control (IME).
+ *		     0 = I_16x16, P_16x16, Full ME Complexity
+ *		     1 = I_16x16, I_NxN, P_16x16, Full ME Complexity
+ *		     2 = I_16x16, I_NXN, P_16x16, P_WxH, Full ME Complexity
+ *		     4 = I_16x16, P_16x16, Reduced ME Complexity
+ *		     5 = I_16x16, I_NxN, P_16x16, Reduced ME Complexity
+ *		     6 = I_16x16, I_NXN, P_16x16, P_WxH, Reduced ME Complexity
+ *  @chroma_qp_index_offset: coming from picture parameter set
+ *			     (PPS see [H.264 STD] 7.4.2.2)
+ *  @entropy_coding_mode: entropy coding mode.
+ *			  0 = CAVLC
+ *			  1 = CABAC
+ * @brc_type: selects the bit-rate control algorithm
+ *		     0 = constant Qp, (no BRC)
+ *		     1 = CBR
+ *		     2 = VBR
+ * @quant: Quantization param used in case of fix QP encoding (no BRC)
+ * @non_VCL_NALU_Size: size of non-VCL NALUs (SPS, PPS, filler),
+ *		       used by BRC
+ * @cpb_buffer_size: size of Coded Picture Buffer, used by BRC
+ * @bit_rate: target bitrate, for BRC
+ * @qp_min: min QP threshold
+ * @qp_max: max QP threshold
+ * @framerate_num: target framerate numerator , used by BRC
+ * @framerate_den: target framerate denomurator , used by BRC
+ * @delay: End-to-End Initial Delay
+ * @strict_HRD_compliancy: flag for HDR compliancy (1)
+ *			   May impact quality encoding
+ * @addr_source_buffer: address of input frame buffer for current frame
+ * @addr_fwd_Ref_Buffer: address of reference frame buffer
+ * @addr_rec_buffer: address of reconstructed frame buffer
+ * @addr_output_bitstream_start: output bitstream start address
+ * @addr_output_bitstream_end: output bitstream end address
+ * @addr_external_sw : address of external search window
+ * @addr_lctx : address of context picture buffer
+ * @addr_local_rec_buffer: address of local reconstructed buffer
+ * @addr_spatial_context: address of spatial context buffer
+ * @bitstream_offset: offset in bits between aligned bitstream start
+ *		      address and first bit to be written by HVA.
+ *		      Range value is [0..63]
+ * @sampling_mode: Input picture format .
+ *		     0: YUV420 semi_planar Interleaved
+ *		     1: YUV422 raster Interleaved
+ * @addr_param_out: address of output parameters structure
+ * @addr_scaling_matrix: address to the coefficient of
+ *			 the inverse scaling matrix
+ * @addr_scaling_matrix_dir: address to the coefficient of
+ *			     the direct scaling matrix
+ * @addr_cabac_context_buffer: address of cabac context buffer
+ * @GmvX: Input information about the horizontal global displacement of
+ *	  the encoded frame versus the previous one
+ * @GmvY: Input information about the vertical global displacement of
+ *	  the encoded frame versus the previous one
+ * @window_width: width in pixels of the window to be encoded inside
+ *		  the input frame
+ * @window_height: width in pixels of the window to be encoded inside
+ *		   the input frame
+ * @window_horizontal_offset: horizontal offset in pels for input window
+ *			      within input frame
+ * @window_vertical_offset: vertical offset in pels for input window
+ *			    within input frame
+ * @addr_roi: Map of QP offset for the Region of Interest algorithm and
+ *	      also used for Error map.
+ *	      Bit 0-6 used for qp offset (value -64 to 63).
+ *	      Bit 7 used to force intra
+ * @addr_slice_header: address to slice header
+ * @slice_header_size_in_bits: size in bits of the Slice header
+ * @slice_header_offset0: Slice header offset where to insert
+ *			  first_Mb_in_slice
+ * @slice_header_offset1: Slice header offset where to insert
+ *			  slice_qp_delta
+ * @slice_header_offset2: Slice header offset where to insert
+ *			  num_MBs_in_slice
+ * @slice_synchro_enable: enable "slice ready" interrupt after each slice
+ * @max_slice_number: Maximum number of slice in a frame
+ *		      (0 is strictly forbidden)
+ * @rgb2_yuv_y_coeff: Four coefficients (C0C1C2C3) to convert from RGB to
+ *		      YUV for the Y component.
+ *		      Y = C0*R + C1*G + C2*B + C3 (C0 is on byte 0)
+ * @rgb2_yuv_u_coeff: four coefficients (C0C1C2C3) to convert from RGB to
+ *		      YUV for the Y component.
+ *		      Y = C0*R + C1*G + C2*B + C3 (C0 is on byte 0)
+ * @rgb2_yuv_v_coeff: Four coefficients (C0C1C2C3) to convert from RGB to
+ *		      YUV for the U (Cb) component.
+ *		      U = C0*R + C1*G + C2*B + C3 (C0 is on byte 0)
+ * @slice_byte_size: maximum slice size in bytes
+ *		     (used when slice_size_type=2 or slice_size_type=3)
+ * @max_air_intra_mb_nb: Maximum number of intra macroblock in a frame
+ *			 for the AIR algorithm
+ * @brc_no_skip: Disable skipping in the Bitrate Controller
+ * @addr_brc_in_out_parameter: address of static buffer for BRC parameters
+ */
+struct hva_h264_td {
+	u16 frame_width;
+	u16 frame_height;
+	u32 frame_num;
+	u16 picture_coding_type;
+	u16 reserved1;
+	u16 pic_order_cnt_type;
+	u16 first_picture_in_sequence;
+	u16 slice_size_type;
+	u16 reserved2;
+	u32 slice_mb_size;
+	u16 ir_param_option;
+	u16 intra_refresh_type;
+	u16 use_constrained_intra_flag;
+	u16 transform_mode;
+	u16 disable_deblocking_filter_idc;
+	s16 slice_alpha_c0_offset_div2;
+	s16 slice_beta_offset_div2;
+	u16 encoder_complexity;
+	s16 chroma_qp_index_offset;
+	u16 entropy_coding_mode;
+	u16 brc_type;
+	u16 quant;
+	u32 non_vcl_nalu_size;
+	u32 cpb_buffer_size;
+	u32 bit_rate;
+	u16 qp_min;
+	u16 qp_max;
+	u16 framerate_num;
+	u16 framerate_den;
+	u16 delay;
+	u16 strict_hrd_compliancy;
+	u32 addr_source_buffer;
+	u32 addr_fwd_ref_buffer;
+	u32 addr_rec_buffer;
+	u32 addr_output_bitstream_start;
+	u32 addr_output_bitstream_end;
+	u32 addr_external_sw;
+	u32 addr_lctx;
+	u32 addr_local_rec_buffer;
+	u32 addr_spatial_context;
+	u16 bitstream_offset;
+	u16 sampling_mode;
+	u32 addr_param_out;
+	u32 addr_scaling_matrix;
+	u32 addr_scaling_matrix_dir;
+	u32 addr_cabac_context_buffer;
+	u32 reserved3;
+	u32 reserved4;
+	s16 gmv_x;
+	s16 gmv_y;
+	u16 window_width;
+	u16 window_height;
+	u16 window_horizontal_offset;
+	u16 window_vertical_offset;
+	u32 addr_roi;
+	u32 addr_slice_header;
+	u16 slice_header_size_in_bits;
+	u16 slice_header_offset0;
+	u16 slice_header_offset1;
+	u16 slice_header_offset2;
+	u32 reserved5;
+	u32 reserved6;
+	u16 reserved7;
+	u16 reserved8;
+	u16 slice_synchro_enable;
+	u16 max_slice_number;
+	u32 rgb2_yuv_y_coeff;
+	u32 rgb2_yuv_u_coeff;
+	u32 rgb2_yuv_v_coeff;
+	u32 slice_byte_size;
+	u16 max_air_intra_mb_nb;
+	u16 brc_no_skip;
+	u32 addr_temporal_context;
+	u32 addr_brc_in_out_parameter;
+};
+
+/**
+ * @ slice_size: slice size
+ * @ slice_start_time: start time
+ * @ slice_stop_time: stop time
+ * @ slice_num: slice number
+ */
+struct hva_h264_slice_po {
+	u32 slice_size;
+	u32 slice_start_time;
+	u32 slice_end_time;
+	u32 slice_num;
+};
+
+/**
+ * @ bitstream_size: bitstream size
+ * @ dct_bitstream_size: dtc bitstream size
+ * @ stuffing_bits: number of stuffing bits inserted by the encoder
+ * @ removal_time: removal time of current frame (nb of ticks 1/framerate)
+ * @ hvc_start_time: hvc start time
+ * @ hvc_stop_time: hvc stop time
+ * @ slice_count: slice count
+ */
+struct hva_h264_po {
+	u32 bitstream_size;
+	u32 dct_bitstream_size;
+	u32 stuffing_bits;
+	u32 removal_time;
+	u32 hvc_start_time;
+	u32 hvc_stop_time;
+	u32 slice_count;
+	u32 reserved0;
+	struct hva_h264_slice_po slice_params[16];
+};
+
+struct hva_h264_task {
+	struct hva_h264_td td;
+	struct hva_h264_po po;
+};
+
+/**
+ * @seq_info:  sequence information buffer
+ * @ref_frame: reference frame buffer
+ * @rec_frame: reconstructed frame buffer
+ * @task:      task descriptor
+ */
+struct hva_h264_ctx {
+	struct hva_buffer *seq_info;
+	struct hva_buffer *ref_frame;
+	struct hva_buffer *rec_frame;
+	struct hva_buffer *task;
+};
+
+static int hva_h264_fill_slice_header(struct hva_ctx *pctx,
+				      u8 *slice_header_addr,
+				      struct hva_controls *ctrls,
+				      int frame_num,
+				      u16 *header_size,
+				      u16 *header_offset0,
+				      u16 *header_offset1,
+				      u16 *header_offset2)
+{
+	/*
+	 * with this HVA hardware version, part of the slice header is computed
+	 * on host and part by hardware.
+	 * The part of host is precomputed and available through this array.
+	 */
+	struct device *dev = ctx_to_dev(pctx);
+	int  cabac = V4L2_MPEG_VIDEO_H264_ENTROPY_MODE_CABAC;
+	const unsigned char slice_header[] = { 0x00, 0x00, 0x00, 0x01,
+					       0x41, 0x34, 0x07, 0x00};
+	int idr_pic_id = frame_num % 2;
+	enum hva_picture_coding_type type;
+	u32 frame_order = frame_num % ctrls->gop_size;
+
+	if (!(frame_num % ctrls->gop_size))
+		type = PICTURE_CODING_TYPE_I;
+	else
+		type = PICTURE_CODING_TYPE_P;
+
+	memcpy(slice_header_addr, slice_header, sizeof(slice_header));
+
+	*header_size = 56;
+	*header_offset0 = 40;
+	*header_offset1 = 13;
+	*header_offset2 = 0;
+
+	if (type == PICTURE_CODING_TYPE_I) {
+		slice_header_addr[4] = 0x65;
+		slice_header_addr[5] = 0x11;
+
+		/* toggle the I frame */
+		if ((frame_num / ctrls->gop_size) % 2) {
+			*header_size += 4;
+			*header_offset1 += 4;
+			slice_header_addr[6] = 0x04;
+			slice_header_addr[7] = 0x70;
+
+		} else {
+			*header_size += 2;
+			*header_offset1 += 2;
+			slice_header_addr[6] = 0x09;
+			slice_header_addr[7] = 0xC0;
+		}
+	} else {
+		if (ctrls->entropy_mode == cabac) {
+			*header_size += 1;
+			*header_offset1 += 1;
+			slice_header_addr[7] = 0x80;
+		}
+		/*
+		 * update slice header with P frame order
+		 * frame order is limited to 16 (coded on 4bits only)
+		 */
+		slice_header_addr[5] += ((frame_order & 0x0C) >> 2);
+		slice_header_addr[6] += ((frame_order & 0x03) << 6);
+	}
+
+	dev_dbg(dev,
+		"%s   %s slice header order %d idrPicId %d header size %d\n",
+		pctx->name, __func__, frame_order, idr_pic_id, *header_size);
+	return 0;
+}
+
+static int hva_h264_fill_data_nal(struct hva_ctx *pctx,
+				  unsigned int stuffing_bytes, u8 *addr,
+				  unsigned int stream_size, unsigned int *size)
+{
+	struct device *dev = ctx_to_dev(pctx);
+	const u8 start[] = { 0x00, 0x00, 0x00, 0x01 };
+
+	dev_dbg(dev, "%s   %s stuffing bytes %d\n", pctx->name, __func__,
+		stuffing_bytes);
+
+	if ((*size + stuffing_bytes + H264_FILLER_DATA_SIZE) > stream_size) {
+		dev_dbg(dev, "%s   %s too many stuffing bytes %d\n",
+			pctx->name, __func__, stuffing_bytes);
+		return 0;
+	}
+
+	/* start code */
+	memcpy(addr + *size, start, sizeof(start));
+	*size += sizeof(start);
+
+	/* nal_unit_type */
+	addr[*size] = NALU_TYPE_FILLER_DATA;
+	*size += 1;
+
+	memset(addr + *size, 0xff, stuffing_bytes);
+	*size += stuffing_bytes;
+
+	addr[*size] = 0x80;
+	*size += 1;
+
+	return 0;
+}
+
+static int hva_h264_fill_sei_nal(struct hva_ctx *pctx,
+				 enum hva_h264_sei_payload_type type,
+				 u8 *addr, u32 *size)
+{
+	struct device *dev = ctx_to_dev(pctx);
+	const u8 start[] = { 0x00, 0x00, 0x00, 0x01 };
+	struct hva_h264_stereo_video_sei info;
+	u8 offset = 7;
+	u8 msg = 0;
+
+	/* start code */
+	memcpy(addr + *size, start, sizeof(start));
+	*size += sizeof(start);
+
+	/* nal_unit_type */
+	addr[*size] = NALU_TYPE_SEI;
+	*size += 1;
+
+	/* payload type */
+	addr[*size] = type;
+	*size += 1;
+
+	switch (type) {
+	case SEI_STEREO_VIDEO_INFO:
+		memset(&info, 0, sizeof(info));
+
+		/* set to top/bottom frame packing arrangement */
+		info.field_views_flag = 1;
+		info.top_field_is_left_view_flag = 1;
+
+		/* payload size */
+		addr[*size] = 1;
+		*size += 1;
+
+		/* payload */
+		msg = info.field_views_flag << offset--;
+
+		if (info.field_views_flag) {
+			msg |= info.top_field_is_left_view_flag <<
+			       offset--;
+		} else {
+			msg |= info.current_frame_is_left_view_flag <<
+			       offset--;
+			msg |= info.next_frame_is_second_view_flag <<
+			       offset--;
+		}
+		msg |= info.left_view_self_contained_flag << offset--;
+		msg |= info.right_view_self_contained_flag << offset--;
+
+		addr[*size] = msg;
+		*size += 1;
+
+		addr[*size] = 0x80;
+		*size += 1;
+
+		return 0;
+	case SEI_BUFFERING_PERIOD:
+	case SEI_PICTURE_TIMING:
+	case SEI_FRAME_PACKING_ARRANGEMENT:
+	default:
+		dev_err(dev, "%s   sei nal type not supported %d\n",
+			pctx->name, type);
+		return -EINVAL;
+	}
+}
+
+static int hva_h264_prepare_task(struct hva_ctx *pctx,
+				 struct hva_h264_task *task,
+				 struct hva_frame *frame,
+				 struct hva_stream *stream)
+{
+	struct hva_dev *hva = ctx_to_hdev(pctx);
+	struct device *dev = ctx_to_dev(pctx);
+	struct hva_h264_ctx *ctx = (struct hva_h264_ctx *)pctx->priv;
+	struct hva_buffer *seq_info = ctx->seq_info;
+	struct hva_buffer *fwd_ref_frame = ctx->ref_frame;
+	struct hva_buffer *loc_rec_frame = ctx->rec_frame;
+	struct hva_h264_td *td = &task->td;
+	struct hva_controls *ctrls = &pctx->ctrls;
+	struct v4l2_fract *time_per_frame = &pctx->ctrls.time_per_frame;
+	int cavlc =  V4L2_MPEG_VIDEO_H264_ENTROPY_MODE_CAVLC;
+	u32 frame_num = pctx->stream_num;
+	u32 addr_esram = hva->esram_addr;
+	enum v4l2_mpeg_video_h264_level level;
+	dma_addr_t paddr = 0;
+	u8 *slice_header_vaddr;
+	u32 frame_width = frame->info.aligned_width;
+	u32 frame_height = frame->info.aligned_height;
+	u32 max_cpb_buffer_size;
+	unsigned int payload = stream->bytesused;
+	u32 max_bitrate;
+
+	/* check width and height parameters */
+	if ((frame_width > max(H264_MAX_SIZE_W, H264_MAX_SIZE_H)) ||
+	    (frame_height > max(H264_MAX_SIZE_W, H264_MAX_SIZE_H))) {
+		dev_err(dev,
+			"%s   width(%d) or height(%d) exceeds limits (%dx%d)\n",
+			pctx->name, frame_width, frame_height,
+			H264_MAX_SIZE_W, H264_MAX_SIZE_H);
+		return -EINVAL;
+	}
+
+	level = ctrls->level;
+
+	memset(td, 0, sizeof(struct hva_h264_td));
+
+	td->frame_width = frame_width;
+	td->frame_height = frame_height;
+
+	/* set frame alignement */
+	td->window_width =  frame_width;
+	td->window_height = frame_height;
+	td->window_horizontal_offset = 0;
+	td->window_vertical_offset = 0;
+
+	td->first_picture_in_sequence = (!frame_num) ? 1 : 0;
+
+	/* pic_order_cnt_type hard coded to '2' as only I & P frames */
+	td->pic_order_cnt_type = 2;
+
+	/* useConstrainedIntraFlag set to false for better coding efficiency */
+	td->use_constrained_intra_flag = false;
+	td->brc_type = (ctrls->bitrate_mode == V4L2_MPEG_VIDEO_BITRATE_MODE_CBR)
+			? BRC_TYPE_CBR : BRC_TYPE_VBR;
+
+	td->entropy_coding_mode = (ctrls->entropy_mode == cavlc) ? CAVLC :
+				  CABAC;
+
+	td->bit_rate = ctrls->bitrate;
+
+	/* set framerate, framerate = 1 n/ time per frame */
+	if (time_per_frame->numerator >= 536) {
+		/*
+		 * due to a hardware bug, framerate denominator can't exceed
+		 * 536 (BRC overflow). Compute nearest framerate
+		 */
+		td->framerate_den = 1;
+		td->framerate_num = (time_per_frame->denominator +
+				    (time_per_frame->numerator >> 1) - 1) /
+				    time_per_frame->numerator;
+
+		/*
+		 * update bitrate to introduce a correction due to
+		 * the new framerate
+		 * new bitrate = (old bitrate * new framerate) / old framerate
+		 */
+		td->bit_rate /= time_per_frame->numerator;
+		td->bit_rate *= time_per_frame->denominator;
+		td->bit_rate /= td->framerate_num;
+	} else {
+		td->framerate_den = time_per_frame->numerator;
+		td->framerate_num = time_per_frame->denominator;
+	}
+
+	/* compute maximum bitrate depending on profile */
+	if (ctrls->profile >= V4L2_MPEG_VIDEO_H264_PROFILE_HIGH)
+		max_bitrate = h264_infos_list[level].max_bitrate *
+			      H264_FACTOR_HIGH;
+	else
+		max_bitrate = h264_infos_list[level].max_bitrate *
+			      H264_FACTOR_BASELINE;
+
+	/* check if bitrate doesn't exceed max size */
+	if (td->bit_rate > max_bitrate) {
+		dev_dbg(dev,
+			"%s   bitrate (%d) larger than level and profile allow, clip to %d\n",
+			pctx->name, td->bit_rate, max_bitrate);
+		td->bit_rate = max_bitrate;
+	}
+
+	/* convert cpb_buffer_size in bits */
+	td->cpb_buffer_size = ctrls->cpb_size * 8000;
+
+	/* compute maximum cpb buffer size depending on profile */
+	if (ctrls->profile >= V4L2_MPEG_VIDEO_H264_PROFILE_HIGH)
+		max_cpb_buffer_size =
+		    h264_infos_list[level].max_cpb_size * H264_FACTOR_HIGH;
+	else
+		max_cpb_buffer_size =
+		    h264_infos_list[level].max_cpb_size * H264_FACTOR_BASELINE;
+
+	/* check if cpb buffer size doesn't exceed max size */
+	if (td->cpb_buffer_size > max_cpb_buffer_size) {
+		dev_dbg(dev,
+			"%s   cpb size larger than level %d allows, clip to %d\n",
+			pctx->name, td->cpb_buffer_size, max_cpb_buffer_size);
+		td->cpb_buffer_size = max_cpb_buffer_size;
+	}
+
+	/* enable skipping in the Bitrate Controller */
+	td->brc_no_skip = 0;
+
+	/* initial delay */
+	if ((ctrls->bitrate_mode == V4L2_MPEG_VIDEO_BITRATE_MODE_CBR) &&
+	    td->bit_rate)
+		td->delay = 1000 * (td->cpb_buffer_size / td->bit_rate);
+	else
+		td->delay = 0;
+
+	switch (frame->info.pixelformat) {
+	case V4L2_PIX_FMT_NV12:
+		td->sampling_mode = SAMPLING_MODE_NV12;
+		break;
+	case V4L2_PIX_FMT_NV21:
+		td->sampling_mode = SAMPLING_MODE_NV21;
+		break;
+	default:
+		dev_err(dev, "%s   invalid source pixel format\n",
+			pctx->name);
+		return -EINVAL;
+	}
+
+	/*
+	 * fill matrix color converter (RGB to YUV)
+	 * Y = 0,299 R + 0,587 G + 0,114 B
+	 * Cb = -0,1687 R -0,3313 G + 0,5 B + 128
+	 * Cr = 0,5 R - 0,4187 G - 0,0813 B + 128
+	 */
+	td->rgb2_yuv_y_coeff = 0x12031008;
+	td->rgb2_yuv_u_coeff = 0x800EF7FB;
+	td->rgb2_yuv_v_coeff = 0x80FEF40E;
+
+	/* enable/disable transform mode */
+	td->transform_mode = ctrls->dct8x8;
+
+	/* encoder complexity fix to 2, ENCODE_I_16x16_I_NxN_P_16x16_P_WxH */
+	td->encoder_complexity = 2;
+
+	/* quant fix to 28, default VBR value */
+	td->quant = 28;
+
+	if (td->framerate_den == 0) {
+		dev_err(dev, "%s   invalid framerate\n", pctx->name);
+		return -EINVAL;
+	}
+
+	/* if automatic framerate, deactivate bitrate controller */
+	if (td->framerate_num == 0)
+		td->brc_type = 0;
+
+	/* compliancy fix to true */
+	td->strict_hrd_compliancy = 1;
+
+	/* set minimum & maximum quantizers */
+	td->qp_min = clamp_val(ctrls->qpmin, 0, 51);
+	td->qp_max = clamp_val(ctrls->qpmax, 0, 51);
+
+	td->addr_source_buffer = frame->paddr;
+	td->addr_fwd_ref_buffer = fwd_ref_frame->paddr;
+	td->addr_rec_buffer = loc_rec_frame->paddr;
+
+	td->addr_output_bitstream_end = (u32)stream->paddr + stream->size;
+
+	td->addr_output_bitstream_start = (u32)stream->paddr;
+	td->bitstream_offset = (((u32)stream->paddr & 0xF) << 3) &
+			       BITSTREAM_OFFSET_MASK;
+
+	td->addr_param_out = (u32)ctx->task->paddr +
+			     offsetof(struct hva_h264_task, po);
+
+	/* swap spatial and temporal context */
+	if (frame_num % 2) {
+		paddr = seq_info->paddr;
+		td->addr_spatial_context =  ALIGN(paddr, 0x100);
+		paddr = seq_info->paddr + DATA_SIZE(frame_width,
+							frame_height);
+		td->addr_temporal_context = ALIGN(paddr, 0x100);
+	} else {
+		paddr = seq_info->paddr;
+		td->addr_temporal_context = ALIGN(paddr, 0x100);
+		paddr = seq_info->paddr + DATA_SIZE(frame_width,
+							frame_height);
+		td->addr_spatial_context =  ALIGN(paddr, 0x100);
+	}
+
+	paddr = seq_info->paddr + 2 * DATA_SIZE(frame_width, frame_height);
+
+	td->addr_brc_in_out_parameter =  ALIGN(paddr, 0x100);
+
+	paddr = td->addr_brc_in_out_parameter + BRC_DATA_SIZE;
+	td->addr_slice_header =  ALIGN(paddr, 0x100);
+	td->addr_external_sw =  ALIGN(addr_esram, 0x100);
+
+	addr_esram += SEARCH_WINDOW_BUFFER_MAX_SIZE(frame_width);
+	td->addr_local_rec_buffer = ALIGN(addr_esram, 0x100);
+
+	addr_esram += LOCAL_RECONSTRUCTED_BUFFER_MAX_SIZE(frame_width);
+	td->addr_lctx = ALIGN(addr_esram, 0x100);
+
+	addr_esram += CTX_MB_BUFFER_MAX_SIZE(max(frame_width, frame_height));
+	td->addr_cabac_context_buffer = ALIGN(addr_esram, 0x100);
+
+	if (!(frame_num % ctrls->gop_size)) {
+		td->picture_coding_type = PICTURE_CODING_TYPE_I;
+		stream->vbuf.flags |= V4L2_BUF_FLAG_KEYFRAME;
+	} else {
+		td->picture_coding_type = PICTURE_CODING_TYPE_P;
+		stream->vbuf.flags &= ~V4L2_BUF_FLAG_KEYFRAME;
+	}
+
+	/* fill the slice header part */
+	slice_header_vaddr = seq_info->vaddr + (td->addr_slice_header -
+			     seq_info->paddr);
+
+	hva_h264_fill_slice_header(pctx, slice_header_vaddr, ctrls, frame_num,
+				   &td->slice_header_size_in_bits,
+				   &td->slice_header_offset0,
+				   &td->slice_header_offset1,
+				   &td->slice_header_offset2);
+
+	td->chroma_qp_index_offset = 2;
+	td->slice_synchro_enable = 0;
+	td->max_slice_number = 1;
+
+	/*
+	 * check the sps/pps header size for key frame only
+	 * sps/pps header was previously fill by libv4l
+	 * during qbuf of stream buffer
+	 */
+	if ((stream->vbuf.flags == V4L2_BUF_FLAG_KEYFRAME) &&
+	    (payload > MAX_SPS_PPS_SIZE)) {
+		dev_err(dev, "%s   invalid sps/pps size %d\n", pctx->name,
+			payload);
+		return -EINVAL;
+	}
+
+	if (stream->vbuf.flags != V4L2_BUF_FLAG_KEYFRAME)
+		payload = 0;
+
+	/* add SEI nal (video stereo info) */
+	if (ctrls->sei_fp && hva_h264_fill_sei_nal(pctx, SEI_STEREO_VIDEO_INFO,
+						   (u8 *)stream->vaddr,
+						   &payload)) {
+		dev_err(dev, "%s   fail to get SEI nal\n", pctx->name);
+		return -EINVAL;
+	}
+
+	/* fill size of non-VCL NAL units (SPS, PPS, filler and SEI) */
+	td->non_vcl_nalu_size = payload * 8;
+
+	/* compute bitstream offset & new start address of bitstream */
+	td->addr_output_bitstream_start += ((payload >> 4) << 4);
+	td->bitstream_offset += (payload - ((payload >> 4) << 4)) * 8;
+
+	stream->bytesused = payload;
+
+	return 0;
+}
+
+static unsigned int hva_h264_get_stream_size(struct hva_h264_task *task)
+{
+	struct hva_h264_po *po = &task->po;
+
+	return po->bitstream_size;
+}
+
+static u32 hva_h264_get_stuffing_bytes(struct hva_h264_task *task)
+{
+	struct hva_h264_po *po = &task->po;
+
+	return po->stuffing_bits >> 3;
+}
+
+static int hva_h264_open(struct hva_ctx *pctx)
+{
+	struct device *dev = ctx_to_dev(pctx);
+	struct hva_h264_ctx *ctx;
+	struct hva_dev *hva = ctx_to_hdev(pctx);
+	u32 frame_width = pctx->frameinfo.aligned_width;
+	u32 frame_height = pctx->frameinfo.aligned_height;
+	u32 size;
+	int ret;
+
+	/* check esram size necessary to encode a frame */
+	size = SEARCH_WINDOW_BUFFER_MAX_SIZE(frame_width) +
+	       LOCAL_RECONSTRUCTED_BUFFER_MAX_SIZE(frame_width) +
+	       CTX_MB_BUFFER_MAX_SIZE(max(frame_width, frame_height)) +
+	       CABAC_CONTEXT_BUFFER_MAX_SIZE(frame_width);
+
+	if (hva->esram_size < size) {
+		dev_err(dev, "%s   not enough esram (max:%d request:%d)\n",
+			pctx->name, hva->esram_size, size);
+		ret = -EINVAL;
+		goto err;
+	}
+
+	/* allocate context for codec */
+	ctx = devm_kzalloc(dev, sizeof(*ctx), GFP_KERNEL);
+	if (!ctx) {
+		ret = -ENOMEM;
+		goto err;
+	}
+
+	/* allocate sequence info buffer */
+	ret = hva_mem_alloc(pctx,
+			    2 * DATA_SIZE(frame_width, frame_height) +
+			    SLICE_HEADER_SIZE +
+			    BRC_DATA_SIZE,
+			    "hva sequence info",
+			    &ctx->seq_info);
+	if (ret) {
+		dev_err(dev,
+			"%s   failed to allocate sequence info buffer\n",
+			pctx->name);
+		goto err_ctx;
+	}
+
+	/* allocate reference frame buffer */
+	ret = hva_mem_alloc(pctx,
+			    frame_width * frame_height * 3 / 2,
+			    "hva reference frame",
+			    &ctx->ref_frame);
+	if (ret) {
+		dev_err(dev, "%s   failed to allocate reference frame buffer\n",
+			pctx->name);
+		goto err_seq_info;
+	}
+
+	/* allocate reconstructed frame buffer */
+	ret = hva_mem_alloc(pctx,
+			    frame_width * frame_height * 3 / 2,
+			    "hva reconstructed frame",
+			    &ctx->rec_frame);
+	if (ret) {
+		dev_err(dev,
+			"%s   failed to allocate reconstructed frame buffer\n",
+			pctx->name);
+		goto err_ref_frame;
+	}
+
+	/* allocate task descriptor */
+	ret = hva_mem_alloc(pctx,
+			    sizeof(struct hva_h264_task),
+			    "hva task descriptor",
+			    &ctx->task);
+	if (ret) {
+		dev_err(dev,
+			"%s   failed to allocate task descriptor\n",
+			pctx->name);
+		goto err_rec_frame;
+	}
+
+	pctx->priv = (void *)ctx;
+
+	return 0;
+
+err_rec_frame:
+	hva_mem_free(pctx, ctx->rec_frame);
+err_ref_frame:
+	hva_mem_free(pctx, ctx->ref_frame);
+err_seq_info:
+	hva_mem_free(pctx, ctx->seq_info);
+err_ctx:
+	devm_kfree(dev, ctx);
+err:
+	return ret;
+}
+
+static int hva_h264_close(struct hva_ctx *pctx)
+{
+	struct hva_h264_ctx *ctx = (struct hva_h264_ctx *)pctx->priv;
+	struct device *dev = ctx_to_dev(pctx);
+
+	if (ctx->seq_info)
+		hva_mem_free(pctx, ctx->seq_info);
+
+	if (ctx->ref_frame)
+		hva_mem_free(pctx, ctx->ref_frame);
+
+	if (ctx->rec_frame)
+		hva_mem_free(pctx, ctx->rec_frame);
+
+	if (ctx->task)
+		hva_mem_free(pctx, ctx->task);
+
+	devm_kfree(dev, ctx);
+
+	return 0;
+}
+
+static int hva_h264_encode(struct hva_ctx *pctx, struct hva_frame *frame,
+			   struct hva_stream *stream)
+{
+	struct hva_h264_ctx *ctx = (struct hva_h264_ctx *)pctx->priv;
+	struct hva_h264_task *task = (struct hva_h264_task *)ctx->task->vaddr;
+	struct hva_buffer *tmp_frame;
+	u32 stuffing_bytes = 0;
+	int ret = 0;
+
+	ret = hva_h264_prepare_task(pctx, task, frame, stream);
+	if (ret)
+		goto err;
+
+	ret = hva_hw_execute_task(pctx, H264_ENC, ctx->task);
+	if (ret)
+		goto err;
+
+	pctx->stream_num++;
+	stream->bytesused += hva_h264_get_stream_size(task);
+
+	stuffing_bytes = hva_h264_get_stuffing_bytes(task);
+
+	if (stuffing_bytes)
+		hva_h264_fill_data_nal(pctx, stuffing_bytes,
+				       (u8 *)stream->vaddr,
+				       stream->size,
+				       &stream->bytesused);
+
+	/* switch reference & reconstructed frame */
+	tmp_frame = ctx->ref_frame;
+	ctx->ref_frame = ctx->rec_frame;
+	ctx->rec_frame = tmp_frame;
+
+	return 0;
+err:
+	stream->bytesused = 0;
+	return ret;
+}
+
+const struct hva_enc nv12h264enc = {
+	.name = "H264(NV12)",
+	.pixelformat = V4L2_PIX_FMT_NV12,
+	.streamformat = V4L2_PIX_FMT_H264,
+	.max_width = H264_MAX_SIZE_W,
+	.max_height = H264_MAX_SIZE_H,
+	.open = hva_h264_open,
+	.close = hva_h264_close,
+	.encode = hva_h264_encode,
+};
+
+const struct hva_enc nv21h264enc = {
+	.name = "H264(NV21)",
+	.pixelformat = V4L2_PIX_FMT_NV21,
+	.streamformat = V4L2_PIX_FMT_H264,
+	.max_width = H264_MAX_SIZE_W,
+	.max_height = H264_MAX_SIZE_H,
+	.open = hva_h264_open,
+	.close = hva_h264_close,
+	.encode = hva_h264_encode,
+};
diff --git a/drivers/media/platform/sti/hva/hva-hw.c b/drivers/media/platform/sti/hva/hva-hw.c
new file mode 100644
index 000000000000..d341d4994528
--- /dev/null
+++ b/drivers/media/platform/sti/hva/hva-hw.c
@@ -0,0 +1,538 @@
+/*
+ * Copyright (C) STMicroelectronics SA 2015
+ * Authors: Yannick Fertre <yannick.fertre@st.com>
+ *          Hugues Fruchet <hugues.fruchet@st.com>
+ * License terms:  GNU General Public License (GPL), version 2
+ */
+
+#include <linux/clk.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+
+#include "hva.h"
+#include "hva-hw.h"
+
+/* HVA register offsets */
+#define HVA_HIF_REG_RST                 0x0100U
+#define HVA_HIF_REG_RST_ACK             0x0104U
+#define HVA_HIF_REG_MIF_CFG             0x0108U
+#define HVA_HIF_REG_HEC_MIF_CFG         0x010CU
+#define HVA_HIF_REG_CFL                 0x0110U
+#define HVA_HIF_FIFO_CMD                0x0114U
+#define HVA_HIF_FIFO_STS                0x0118U
+#define HVA_HIF_REG_SFL                 0x011CU
+#define HVA_HIF_REG_IT_ACK              0x0120U
+#define HVA_HIF_REG_ERR_IT_ACK          0x0124U
+#define HVA_HIF_REG_LMI_ERR             0x0128U
+#define HVA_HIF_REG_EMI_ERR             0x012CU
+#define HVA_HIF_REG_HEC_MIF_ERR         0x0130U
+#define HVA_HIF_REG_HEC_STS             0x0134U
+#define HVA_HIF_REG_HVC_STS             0x0138U
+#define HVA_HIF_REG_HJE_STS             0x013CU
+#define HVA_HIF_REG_CNT                 0x0140U
+#define HVA_HIF_REG_HEC_CHKSYN_DIS      0x0144U
+#define HVA_HIF_REG_CLK_GATING          0x0148U
+#define HVA_HIF_REG_VERSION             0x014CU
+#define HVA_HIF_REG_BSM                 0x0150U
+
+/* define value for version id register (HVA_HIF_REG_VERSION) */
+#define VERSION_ID_MASK	0x0000FFFF
+
+/* define values for BSM register (HVA_HIF_REG_BSM) */
+#define BSM_CFG_VAL1	0x0003F000
+#define BSM_CFG_VAL2	0x003F0000
+
+/* define values for memory interface register (HVA_HIF_REG_MIF_CFG) */
+#define MIF_CFG_VAL1	0x04460446
+#define MIF_CFG_VAL2	0x04460806
+#define MIF_CFG_VAL3	0x00000000
+
+/* define value for HEC memory interface register (HVA_HIF_REG_MIF_CFG) */
+#define HEC_MIF_CFG_VAL	0x000000C4
+
+/*  Bits definition for clock gating register (HVA_HIF_REG_CLK_GATING) */
+#define CLK_GATING_HVC	BIT(0)
+#define CLK_GATING_HEC	BIT(1)
+#define CLK_GATING_HJE	BIT(2)
+
+/* fix hva clock rate */
+#define CLK_RATE		300000000
+
+/* fix delay for pmruntime */
+#define AUTOSUSPEND_DELAY_MS	3
+
+/*
+ * hw encode error values
+ * NO_ERROR: Success, Task OK
+ * H264_BITSTREAM_OVERSIZE: VECH264 Bitstream size > bitstream buffer
+ * H264_FRAME_SKIPPED: VECH264 Frame skipped (refers to CPB Buffer Size)
+ * H264_SLICE_LIMIT_SIZE: VECH264 MB > slice limit size
+ * H264_MAX_SLICE_NUMBER: VECH264 max slice number reached
+ * H264_SLICE_READY: VECH264 Slice ready
+ * TASK_LIST_FULL: HVA/FPC task list full
+		   (discard latest transform command)
+ * UNKNOWN_COMMAND: Transform command not known by HVA/FPC
+ * WRONG_CODEC_OR_RESOLUTION: Wrong Codec or Resolution Selection
+ * NO_INT_COMPLETION: Time-out on interrupt completion
+ * LMI_ERR: Local Memory Interface Error
+ * EMI_ERR: External Memory Interface Error
+ * HECMI_ERR: HEC Memory Interface Error
+ */
+enum hva_hw_error {
+	NO_ERROR = 0x0,
+	H264_BITSTREAM_OVERSIZE = 0x2,
+	H264_FRAME_SKIPPED = 0x4,
+	H264_SLICE_LIMIT_SIZE = 0x5,
+	H264_MAX_SLICE_NUMBER = 0x7,
+	H264_SLICE_READY = 0x8,
+	TASK_LIST_FULL = 0xF0,
+	UNKNOWN_COMMAND = 0xF1,
+	WRONG_CODEC_OR_RESOLUTION = 0xF4,
+	NO_INT_COMPLETION = 0x100,
+	LMI_ERR = 0x101,
+	EMI_ERR = 0x102,
+	HECMI_ERR = 0x103,
+};
+
+static irqreturn_t hva_hw_its_interrupt(int irq, void *data)
+{
+	struct hva_dev *hva = data;
+
+	/* read status registers */
+	hva->sts_reg = readl_relaxed(hva->regs + HVA_HIF_FIFO_STS);
+	hva->sfl_reg = readl_relaxed(hva->regs + HVA_HIF_REG_SFL);
+
+	/* acknowledge interruption */
+	writel_relaxed(0x1, hva->regs + HVA_HIF_REG_IT_ACK);
+
+	return IRQ_WAKE_THREAD;
+}
+
+static irqreturn_t hva_hw_its_irq_thread(int irq, void *arg)
+{
+	struct hva_dev *hva = arg;
+	struct device *dev = hva_to_dev(hva);
+	u32 status = hva->sts_reg & 0xFF;
+	u8 ctx_id = 0;
+	struct hva_ctx *ctx = NULL;
+
+	dev_dbg(dev, "%s     %s: status: 0x%02x fifo level: 0x%02x\n",
+		HVA_PREFIX, __func__, hva->sts_reg & 0xFF, hva->sfl_reg & 0xF);
+
+	/*
+	 * status: task_id[31:16] client_id[15:8] status[7:0]
+	 * the context identifier is retrieved from the client identifier
+	 */
+	ctx_id = (hva->sts_reg & 0xFF00) >> 8;
+	if (ctx_id >= HVA_MAX_INSTANCES) {
+		dev_err(dev, "%s     %s: bad context identifier: %d\n",
+			ctx->name, __func__, ctx_id);
+		ctx->hw_err = true;
+		goto out;
+	}
+
+	ctx = hva->instances[ctx_id];
+	if (!ctx)
+		goto out;
+
+	switch (status) {
+	case NO_ERROR:
+		dev_dbg(dev, "%s     %s: no error\n",
+			ctx->name, __func__);
+		ctx->hw_err = false;
+		break;
+	case H264_SLICE_READY:
+		dev_dbg(dev, "%s     %s: h264 slice ready\n",
+			ctx->name, __func__);
+		ctx->hw_err = false;
+		break;
+	case H264_FRAME_SKIPPED:
+		dev_dbg(dev, "%s     %s: h264 frame skipped\n",
+			ctx->name, __func__);
+		ctx->hw_err = false;
+		break;
+	case H264_BITSTREAM_OVERSIZE:
+		dev_err(dev, "%s     %s:h264 bitstream oversize\n",
+			ctx->name, __func__);
+		ctx->hw_err = true;
+		break;
+	case H264_SLICE_LIMIT_SIZE:
+		dev_err(dev, "%s     %s: h264 slice limit size is reached\n",
+			ctx->name, __func__);
+		ctx->hw_err = true;
+		break;
+	case H264_MAX_SLICE_NUMBER:
+		dev_err(dev, "%s     %s: h264 max slice number is reached\n",
+			ctx->name, __func__);
+		ctx->hw_err = true;
+		break;
+	case TASK_LIST_FULL:
+		dev_err(dev, "%s     %s:task list full\n",
+			ctx->name, __func__);
+		ctx->hw_err = true;
+		break;
+	case UNKNOWN_COMMAND:
+		dev_err(dev, "%s     %s: command not known\n",
+			ctx->name, __func__);
+		ctx->hw_err = true;
+		break;
+	case WRONG_CODEC_OR_RESOLUTION:
+		dev_err(dev, "%s     %s: wrong codec or resolution\n",
+			ctx->name, __func__);
+		ctx->hw_err = true;
+		break;
+	default:
+		dev_err(dev, "%s     %s: status not recognized\n",
+			ctx->name, __func__);
+		ctx->hw_err = true;
+		break;
+	}
+out:
+	complete(&hva->interrupt);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t hva_hw_err_interrupt(int irq, void *data)
+{
+	struct hva_dev *hva = data;
+
+	/* read status registers */
+	hva->sts_reg = readl_relaxed(hva->regs + HVA_HIF_FIFO_STS);
+	hva->sfl_reg = readl_relaxed(hva->regs + HVA_HIF_REG_SFL);
+
+	/* read error registers */
+	hva->lmi_err_reg = readl_relaxed(hva->regs + HVA_HIF_REG_LMI_ERR);
+	hva->emi_err_reg = readl_relaxed(hva->regs + HVA_HIF_REG_EMI_ERR);
+	hva->hec_mif_err_reg = readl_relaxed(hva->regs +
+					     HVA_HIF_REG_HEC_MIF_ERR);
+
+	/* acknowledge interruption */
+	writel_relaxed(0x1, hva->regs + HVA_HIF_REG_IT_ACK);
+
+	return IRQ_WAKE_THREAD;
+}
+
+static irqreturn_t hva_hw_err_irq_thread(int irq, void *arg)
+{
+	struct hva_dev *hva = arg;
+	struct device *dev = hva_to_dev(hva);
+	u8 ctx_id = 0;
+	struct hva_ctx *ctx;
+
+	dev_dbg(dev, "%s     status: 0x%02x fifo level: 0x%02x\n",
+		HVA_PREFIX, hva->sts_reg & 0xFF, hva->sfl_reg & 0xF);
+
+	/*
+	 * status: task_id[31:16] client_id[15:8] status[7:0]
+	 * the context identifier is retrieved from the client identifier
+	 */
+	ctx_id = (hva->sts_reg & 0xFF00) >> 8;
+	if (ctx_id >= HVA_MAX_INSTANCES) {
+		dev_err(dev, "%s     bad context identifier: %d\n", HVA_PREFIX,
+			ctx_id);
+		goto out;
+	}
+
+	ctx = hva->instances[ctx_id];
+	if (!ctx)
+		goto out;
+
+	if (hva->lmi_err_reg) {
+		dev_err(dev, "%s     local memory interface error: 0x%08x\n",
+			ctx->name, hva->lmi_err_reg);
+		ctx->hw_err = true;
+	}
+
+	if (hva->lmi_err_reg) {
+		dev_err(dev, "%s     external memory interface error: 0x%08x\n",
+			ctx->name, hva->emi_err_reg);
+		ctx->hw_err = true;
+	}
+
+	if (hva->hec_mif_err_reg) {
+		dev_err(dev, "%s     hec memory interface error: 0x%08x\n",
+			ctx->name, hva->hec_mif_err_reg);
+		ctx->hw_err = true;
+	}
+out:
+	complete(&hva->interrupt);
+
+	return IRQ_HANDLED;
+}
+
+static unsigned long int hva_hw_get_ip_version(struct hva_dev *hva)
+{
+	struct device *dev = hva_to_dev(hva);
+	unsigned long int version;
+
+	if (pm_runtime_get_sync(dev) < 0) {
+		dev_err(dev, "%s     failed to get pm_runtime\n", HVA_PREFIX);
+		mutex_unlock(&hva->protect_mutex);
+		return -EFAULT;
+	}
+
+	version = readl_relaxed(hva->regs + HVA_HIF_REG_VERSION) &
+				VERSION_ID_MASK;
+
+	pm_runtime_put_autosuspend(dev);
+
+	switch (version) {
+	case HVA_VERSION_V400:
+		dev_dbg(dev, "%s     IP hardware version 0x%lx\n",
+			HVA_PREFIX, version);
+		break;
+	default:
+		dev_err(dev, "%s     unknown IP hardware version 0x%lx\n",
+			HVA_PREFIX, version);
+		version = HVA_VERSION_UNKNOWN;
+		break;
+	}
+
+	return version;
+}
+
+int hva_hw_probe(struct platform_device *pdev, struct hva_dev *hva)
+{
+	struct device *dev = &pdev->dev;
+	struct resource *regs;
+	struct resource *esram;
+	int ret;
+
+	WARN_ON(!hva);
+
+	/* get memory for registers */
+	regs = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	hva->regs = devm_ioremap_resource(dev, regs);
+	if (IS_ERR_OR_NULL(hva->regs)) {
+		dev_err(dev, "%s     failed to get regs\n", HVA_PREFIX);
+		return PTR_ERR(hva->regs);
+	}
+
+	/* get memory for esram */
+	esram = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	if (IS_ERR_OR_NULL(esram)) {
+		dev_err(dev, "%s     failed to get esram\n", HVA_PREFIX);
+		return PTR_ERR(esram);
+	}
+	hva->esram_addr = esram->start;
+	hva->esram_size = resource_size(esram);
+
+	dev_info(dev, "%s     esram reserved for address: 0x%x size:%d\n",
+		 HVA_PREFIX, hva->esram_addr, hva->esram_size);
+
+	/* get clock resource */
+	hva->clk = devm_clk_get(dev, "clk_hva");
+	if (IS_ERR(hva->clk)) {
+		dev_err(dev, "%s     failed to get clock\n", HVA_PREFIX);
+		return PTR_ERR(hva->clk);
+	}
+
+	ret = clk_prepare(hva->clk);
+	if (ret < 0) {
+		dev_err(dev, "%s     failed to prepare clock\n", HVA_PREFIX);
+		hva->clk = ERR_PTR(-EINVAL);
+		return ret;
+	}
+
+	/* get status interruption resource */
+	ret  = platform_get_irq(pdev, 0);
+	if (ret < 0) {
+		dev_err(dev, "%s     failed to get status IRQ\n", HVA_PREFIX);
+		goto err_clk;
+	}
+	hva->irq_its = ret;
+
+	ret = devm_request_threaded_irq(dev, hva->irq_its, hva_hw_its_interrupt,
+					hva_hw_its_irq_thread,
+					IRQF_ONESHOT,
+					"hva_its_irq", hva);
+	if (ret) {
+		dev_err(dev, "%s     failed to install status IRQ 0x%x\n",
+			HVA_PREFIX, hva->irq_its);
+		goto err_clk;
+	}
+	disable_irq(hva->irq_its);
+
+	/* get error interruption resource */
+	ret = platform_get_irq(pdev, 1);
+	if (ret < 0) {
+		dev_err(dev, "%s     failed to get error IRQ\n", HVA_PREFIX);
+		goto err_clk;
+	}
+	hva->irq_err = ret;
+
+	ret = devm_request_threaded_irq(dev, hva->irq_err, hva_hw_err_interrupt,
+					hva_hw_err_irq_thread,
+					IRQF_ONESHOT,
+					"hva_err_irq", hva);
+	if (ret) {
+		dev_err(dev, "%s     failed to install error IRQ 0x%x\n",
+			HVA_PREFIX, hva->irq_err);
+		goto err_clk;
+	}
+	disable_irq(hva->irq_err);
+
+	/* initialise protection mutex */
+	mutex_init(&hva->protect_mutex);
+
+	/* initialise completion signal */
+	init_completion(&hva->interrupt);
+
+	/* initialise runtime power management */
+	pm_runtime_set_autosuspend_delay(dev, AUTOSUSPEND_DELAY_MS);
+	pm_runtime_use_autosuspend(dev);
+	pm_runtime_set_suspended(dev);
+	pm_runtime_enable(dev);
+
+	ret = pm_runtime_get_sync(dev);
+	if (ret < 0) {
+		dev_err(dev, "%s     failed to set PM\n", HVA_PREFIX);
+		goto err_clk;
+	}
+
+	/* check IP hardware version */
+	hva->ip_version = hva_hw_get_ip_version(hva);
+
+	if (hva->ip_version == HVA_VERSION_UNKNOWN) {
+		ret = -EINVAL;
+		goto err_pm;
+	}
+
+	dev_info(dev, "%s     found hva device (version 0x%lx)\n", HVA_PREFIX,
+		 hva->ip_version);
+
+	return 0;
+
+err_pm:
+	pm_runtime_put(dev);
+err_clk:
+	if (hva->clk)
+		clk_unprepare(hva->clk);
+
+	return ret;
+}
+
+void hva_hw_remove(struct hva_dev *hva)
+{
+	struct device *dev = hva_to_dev(hva);
+
+	disable_irq(hva->irq_its);
+	disable_irq(hva->irq_err);
+
+	pm_runtime_put_autosuspend(dev);
+	pm_runtime_disable(dev);
+}
+
+int hva_hw_runtime_suspend(struct device *dev)
+{
+	struct hva_dev *hva = dev_get_drvdata(dev);
+
+	clk_disable_unprepare(hva->clk);
+
+	return 0;
+}
+
+int hva_hw_runtime_resume(struct device *dev)
+{
+	struct hva_dev *hva = dev_get_drvdata(dev);
+
+	if (clk_prepare_enable(hva->clk)) {
+		dev_err(hva->dev, "%s     failed to prepare hva clk\n",
+			HVA_PREFIX);
+		return -EINVAL;
+	}
+
+	if (clk_set_rate(hva->clk, CLK_RATE)) {
+		dev_err(dev, "%s     failed to set clock frequency\n",
+			HVA_PREFIX);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+int hva_hw_execute_task(struct hva_ctx *ctx, enum hva_hw_cmd_type cmd,
+			struct hva_buffer *task)
+{
+	struct hva_dev *hva = ctx_to_hdev(ctx);
+	struct device *dev = hva_to_dev(hva);
+	u8 client_id = ctx->id;
+	int ret;
+	u32 reg = 0;
+
+	mutex_lock(&hva->protect_mutex);
+
+	/* enable irqs */
+	enable_irq(hva->irq_its);
+	enable_irq(hva->irq_err);
+
+	if (pm_runtime_get_sync(dev) < 0) {
+		dev_err(dev, "%s     failed to get pm_runtime\n", ctx->name);
+		ret = -EFAULT;
+		goto out;
+	}
+
+	reg = readl_relaxed(hva->regs + HVA_HIF_REG_CLK_GATING);
+	switch (cmd) {
+	case H264_ENC:
+		reg |= CLK_GATING_HVC;
+		break;
+	default:
+		dev_dbg(dev, "%s     unknown command 0x%x\n", ctx->name, cmd);
+		ret = -EFAULT;
+		goto out;
+	}
+	writel_relaxed(reg, hva->regs + HVA_HIF_REG_CLK_GATING);
+
+	dev_dbg(dev, "%s     %s: write configuration registers\n", ctx->name,
+		__func__);
+
+	/* byte swap config */
+	writel_relaxed(BSM_CFG_VAL1, hva->regs + HVA_HIF_REG_BSM);
+
+	/* define Max Opcode Size and Max Message Size for LMI and EMI */
+	writel_relaxed(MIF_CFG_VAL3, hva->regs + HVA_HIF_REG_MIF_CFG);
+	writel_relaxed(HEC_MIF_CFG_VAL, hva->regs + HVA_HIF_REG_HEC_MIF_CFG);
+
+	/*
+	 * command FIFO: task_id[31:16] client_id[15:8] command_type[7:0]
+	 * the context identifier is provided as client identifier to the
+	 * hardware, and is retrieved in the interrupt functions from the
+	 * status register
+	 */
+	dev_dbg(dev, "%s     %s: send task (cmd: %d, task_desc: %pad)\n",
+		ctx->name, __func__, cmd + (client_id << 8), &task->paddr);
+	writel_relaxed(cmd + (client_id << 8), hva->regs + HVA_HIF_FIFO_CMD);
+	writel_relaxed(task->paddr, hva->regs + HVA_HIF_FIFO_CMD);
+
+	if (!wait_for_completion_timeout(&hva->interrupt,
+					 msecs_to_jiffies(2000))) {
+		dev_err(dev, "%s     %s: time out on completion\n", ctx->name,
+			__func__);
+		ret = -EFAULT;
+		goto out;
+	}
+
+	/* get encoding status */
+	ret = ctx->hw_err ? -EFAULT : 0;
+
+out:
+	disable_irq(hva->irq_its);
+	disable_irq(hva->irq_err);
+
+	switch (cmd) {
+	case H264_ENC:
+		reg &= ~CLK_GATING_HVC;
+		writel_relaxed(reg, hva->regs + HVA_HIF_REG_CLK_GATING);
+		break;
+	default:
+		dev_dbg(dev, "%s     unknown command 0x%x\n", ctx->name, cmd);
+	}
+
+	pm_runtime_put_autosuspend(dev);
+	mutex_unlock(&hva->protect_mutex);
+
+	return ret;
+}
diff --git a/drivers/media/platform/sti/hva/hva-hw.h b/drivers/media/platform/sti/hva/hva-hw.h
new file mode 100644
index 000000000000..efb45b927524
--- /dev/null
+++ b/drivers/media/platform/sti/hva/hva-hw.h
@@ -0,0 +1,42 @@
+/*
+ * Copyright (C) STMicroelectronics SA 2015
+ * Authors: Yannick Fertre <yannick.fertre@st.com>
+ *          Hugues Fruchet <hugues.fruchet@st.com>
+ * License terms:  GNU General Public License (GPL), version 2
+ */
+
+#ifndef HVA_HW_H
+#define HVA_HW_H
+
+#include "hva-mem.h"
+
+/* HVA Versions */
+#define HVA_VERSION_UNKNOWN    0x000
+#define HVA_VERSION_V400       0x400
+
+/* HVA command types */
+enum hva_hw_cmd_type {
+	/* RESERVED = 0x00 */
+	/* RESERVED = 0x01 */
+	H264_ENC = 0x02,
+	/* RESERVED = 0x03 */
+	/* RESERVED = 0x04 */
+	/* RESERVED = 0x05 */
+	/* RESERVED = 0x06 */
+	/* RESERVED = 0x07 */
+	REMOVE_CLIENT = 0x08,
+	FREEZE_CLIENT = 0x09,
+	START_CLIENT = 0x0A,
+	FREEZE_ALL = 0x0B,
+	START_ALL = 0x0C,
+	REMOVE_ALL = 0x0D
+};
+
+int hva_hw_probe(struct platform_device *pdev, struct hva_dev *hva);
+void hva_hw_remove(struct hva_dev *hva);
+int hva_hw_runtime_suspend(struct device *dev);
+int hva_hw_runtime_resume(struct device *dev);
+int hva_hw_execute_task(struct hva_ctx *ctx, enum hva_hw_cmd_type cmd,
+			struct hva_buffer *task);
+
+#endif /* HVA_HW_H */
diff --git a/drivers/media/platform/sti/hva/hva-mem.c b/drivers/media/platform/sti/hva/hva-mem.c
new file mode 100644
index 000000000000..649f66007ad6
--- /dev/null
+++ b/drivers/media/platform/sti/hva/hva-mem.c
@@ -0,0 +1,59 @@
+/*
+ * Copyright (C) STMicroelectronics SA 2015
+ * Authors: Yannick Fertre <yannick.fertre@st.com>
+ *          Hugues Fruchet <hugues.fruchet@st.com>
+ * License terms:  GNU General Public License (GPL), version 2
+ */
+
+#include "hva.h"
+#include "hva-mem.h"
+
+int hva_mem_alloc(struct hva_ctx *ctx, u32 size, const char *name,
+		  struct hva_buffer **buf)
+{
+	struct device *dev = ctx_to_dev(ctx);
+	struct hva_buffer *b;
+	dma_addr_t paddr;
+	void *base;
+
+	b = devm_kzalloc(dev, sizeof(*b), GFP_KERNEL);
+	if (!b)
+		return -ENOMEM;
+
+	base = dma_alloc_attrs(dev, size, &paddr, GFP_KERNEL | GFP_DMA,
+			       DMA_ATTR_WRITE_COMBINE);
+	if (!base) {
+		dev_err(dev, "%s %s : dma_alloc_attrs failed for %s (size=%d)\n",
+			ctx->name, __func__, name, size);
+		devm_kfree(dev, b);
+		return -ENOMEM;
+	}
+
+	b->size = size;
+	b->paddr = paddr;
+	b->vaddr = base;
+	b->name = name;
+
+	dev_dbg(dev,
+		"%s allocate %d bytes of HW memory @(virt=%p, phy=%pad): %s\n",
+		ctx->name, size, b->vaddr, &b->paddr, b->name);
+
+	/* return  hva buffer to user */
+	*buf = b;
+
+	return 0;
+}
+
+void hva_mem_free(struct hva_ctx *ctx, struct hva_buffer *buf)
+{
+	struct device *dev = ctx_to_dev(ctx);
+
+	dev_dbg(dev,
+		"%s free %d bytes of HW memory @(virt=%p, phy=%pad): %s\n",
+		ctx->name, buf->size, buf->vaddr, &buf->paddr, buf->name);
+
+	dma_free_attrs(dev, buf->size, buf->vaddr, buf->paddr,
+		       DMA_ATTR_WRITE_COMBINE);
+
+	devm_kfree(dev, buf);
+}
diff --git a/drivers/media/platform/sti/hva/hva-mem.h b/drivers/media/platform/sti/hva/hva-mem.h
new file mode 100644
index 000000000000..a95c728a45e6
--- /dev/null
+++ b/drivers/media/platform/sti/hva/hva-mem.h
@@ -0,0 +1,34 @@
+/*
+ * Copyright (C) STMicroelectronics SA 2015
+ * Authors: Yannick Fertre <yannick.fertre@st.com>
+ *          Hugues Fruchet <hugues.fruchet@st.com>
+ * License terms:  GNU General Public License (GPL), version 2
+ */
+
+#ifndef HVA_MEM_H
+#define HVA_MEM_H
+
+/**
+ * struct hva_buffer - hva buffer
+ *
+ * @name:  name of requester
+ * @paddr: physical address (for hardware)
+ * @vaddr: virtual address (kernel can read/write)
+ * @size:  size of buffer
+ */
+struct hva_buffer {
+	const char		*name;
+	dma_addr_t		paddr;
+	void			*vaddr;
+	u32			size;
+};
+
+int hva_mem_alloc(struct hva_ctx *ctx,
+		  __u32 size,
+		  const char *name,
+		  struct hva_buffer **buf);
+
+void hva_mem_free(struct hva_ctx *ctx,
+		  struct hva_buffer *buf);
+
+#endif /* HVA_MEM_H */
diff --git a/drivers/media/platform/sti/hva/hva-v4l2.c b/drivers/media/platform/sti/hva/hva-v4l2.c
new file mode 100644
index 000000000000..6bf3c8588230
--- /dev/null
+++ b/drivers/media/platform/sti/hva/hva-v4l2.c
@@ -0,0 +1,1415 @@
+/*
+ * Copyright (C) STMicroelectronics SA 2015
+ * Authors: Yannick Fertre <yannick.fertre@st.com>
+ *          Hugues Fruchet <hugues.fruchet@st.com>
+ * License terms:  GNU General Public License (GPL), version 2
+ */
+
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-ioctl.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "hva.h"
+#include "hva-hw.h"
+
+#define HVA_NAME "st-hva"
+
+#define MIN_FRAMES	1
+#define MIN_STREAMS	1
+
+#define HVA_MIN_WIDTH	32
+#define HVA_MAX_WIDTH	1920
+#define HVA_MIN_HEIGHT	32
+#define HVA_MAX_HEIGHT	1920
+
+/* HVA requires a 16x16 pixels alignment for frames */
+#define HVA_WIDTH_ALIGNMENT	16
+#define HVA_HEIGHT_ALIGNMENT	16
+
+#define HVA_DEFAULT_WIDTH	HVA_MIN_WIDTH
+#define	HVA_DEFAULT_HEIGHT	HVA_MIN_HEIGHT
+#define HVA_DEFAULT_FRAME_NUM	1
+#define HVA_DEFAULT_FRAME_DEN	30
+
+#define to_type_str(type) (type == V4L2_BUF_TYPE_VIDEO_OUTPUT ? \
+			   "frame" : "stream")
+
+#define fh_to_ctx(f)    (container_of(f, struct hva_ctx, fh))
+
+/* registry of available encoders */
+static const struct hva_enc *hva_encoders[] = {
+	&nv12h264enc,
+	&nv21h264enc,
+};
+
+static inline int frame_size(u32 w, u32 h, u32 fmt)
+{
+	switch (fmt) {
+	case V4L2_PIX_FMT_NV12:
+	case V4L2_PIX_FMT_NV21:
+		return (w * h * 3) / 2;
+	default:
+		return 0;
+	}
+}
+
+static inline int frame_stride(u32 w, u32 fmt)
+{
+	switch (fmt) {
+	case V4L2_PIX_FMT_NV12:
+	case V4L2_PIX_FMT_NV21:
+		return w;
+	default:
+		return 0;
+	}
+}
+
+static inline int frame_alignment(u32 fmt)
+{
+	switch (fmt) {
+	case V4L2_PIX_FMT_NV12:
+	case V4L2_PIX_FMT_NV21:
+		/* multiple of 2 */
+		return 2;
+	default:
+		return 1;
+	}
+}
+
+static inline int estimated_stream_size(u32 w, u32 h)
+{
+	/*
+	 * HVA only encodes in YUV420 format, whatever the frame format.
+	 * A compression ratio of 2 is assumed: thus, the maximum size
+	 * of a stream is estimated to ((width x height x 3 / 2) / 2)
+	 */
+	return (w * h * 3) / 4;
+}
+
+static void set_default_params(struct hva_ctx *ctx)
+{
+	struct hva_frameinfo *frameinfo = &ctx->frameinfo;
+	struct hva_streaminfo *streaminfo = &ctx->streaminfo;
+
+	frameinfo->pixelformat = V4L2_PIX_FMT_NV12;
+	frameinfo->width = HVA_DEFAULT_WIDTH;
+	frameinfo->height = HVA_DEFAULT_HEIGHT;
+	frameinfo->aligned_width = ALIGN(frameinfo->width,
+					 HVA_WIDTH_ALIGNMENT);
+	frameinfo->aligned_height = ALIGN(frameinfo->height,
+					  HVA_HEIGHT_ALIGNMENT);
+	frameinfo->size = frame_size(frameinfo->aligned_width,
+				     frameinfo->aligned_height,
+				     frameinfo->pixelformat);
+
+	streaminfo->streamformat = V4L2_PIX_FMT_H264;
+	streaminfo->width = HVA_DEFAULT_WIDTH;
+	streaminfo->height = HVA_DEFAULT_HEIGHT;
+
+	ctx->colorspace = V4L2_COLORSPACE_REC709;
+	ctx->xfer_func = V4L2_XFER_FUNC_DEFAULT;
+	ctx->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
+	ctx->quantization = V4L2_QUANTIZATION_DEFAULT;
+
+	ctx->max_stream_size = estimated_stream_size(streaminfo->width,
+						     streaminfo->height);
+}
+
+static const struct hva_enc *hva_find_encoder(struct hva_ctx *ctx,
+					      u32 pixelformat,
+					      u32 streamformat)
+{
+	struct hva_dev *hva = ctx_to_hdev(ctx);
+	const struct hva_enc *enc;
+	unsigned int i;
+
+	for (i = 0; i < hva->nb_of_encoders; i++) {
+		enc = hva->encoders[i];
+		if ((enc->pixelformat == pixelformat) &&
+		    (enc->streamformat == streamformat))
+			return enc;
+	}
+
+	return NULL;
+}
+
+static void register_format(u32 format, u32 formats[], u32 *nb_of_formats)
+{
+	u32 i;
+	bool found = false;
+
+	for (i = 0; i < *nb_of_formats; i++) {
+		if (format == formats[i]) {
+			found = true;
+			break;
+		}
+	}
+
+	if (!found)
+		formats[(*nb_of_formats)++] = format;
+}
+
+static void register_formats(struct hva_dev *hva)
+{
+	unsigned int i;
+
+	for (i = 0; i < hva->nb_of_encoders; i++) {
+		register_format(hva->encoders[i]->pixelformat,
+				hva->pixelformats,
+				&hva->nb_of_pixelformats);
+
+		register_format(hva->encoders[i]->streamformat,
+				hva->streamformats,
+				&hva->nb_of_streamformats);
+	}
+}
+
+static void register_encoders(struct hva_dev *hva)
+{
+	struct device *dev = hva_to_dev(hva);
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(hva_encoders); i++) {
+		if (hva->nb_of_encoders >= HVA_MAX_ENCODERS) {
+			dev_dbg(dev,
+				"%s failed to register %s encoder (%d maximum reached)\n",
+				HVA_PREFIX, hva_encoders[i]->name,
+				HVA_MAX_ENCODERS);
+			return;
+		}
+
+		hva->encoders[hva->nb_of_encoders++] = hva_encoders[i];
+		dev_info(dev, "%s %s encoder registered\n", HVA_PREFIX,
+			 hva_encoders[i]->name);
+	}
+}
+
+static int hva_open_encoder(struct hva_ctx *ctx, u32 streamformat,
+			    u32 pixelformat, struct hva_enc **penc)
+{
+	struct hva_dev *hva = ctx_to_hdev(ctx);
+	struct device *dev = ctx_to_dev(ctx);
+	struct hva_enc *enc;
+	int ret;
+
+	/* find an encoder which can deal with these formats */
+	enc = (struct hva_enc *)hva_find_encoder(ctx, pixelformat,
+						 streamformat);
+	if (!enc) {
+		dev_err(dev, "%s no encoder found matching %4.4s => %4.4s\n",
+			ctx->name, (char *)&pixelformat, (char *)&streamformat);
+		return -EINVAL;
+	}
+
+	dev_dbg(dev, "%s one encoder matching %4.4s => %4.4s\n",
+		ctx->name, (char *)&pixelformat, (char *)&streamformat);
+
+	/* update instance name */
+	snprintf(ctx->name, sizeof(ctx->name), "[%3d:%4.4s]",
+		 hva->instance_id, (char *)&streamformat);
+
+	/* open encoder instance */
+	ret = enc->open(ctx);
+	if (ret) {
+		dev_err(dev, "%s failed to open encoder instance (%d)\n",
+			ctx->name, ret);
+		return ret;
+	}
+
+	dev_dbg(dev, "%s %s encoder opened\n", ctx->name, enc->name);
+
+	*penc = enc;
+
+	return ret;
+}
+
+/*
+ * V4L2 ioctl operations
+ */
+
+static int hva_querycap(struct file *file, void *priv,
+			struct v4l2_capability *cap)
+{
+	struct hva_ctx *ctx = fh_to_ctx(file->private_data);
+	struct hva_dev *hva = ctx_to_hdev(ctx);
+
+	strlcpy(cap->driver, HVA_NAME, sizeof(cap->driver));
+	strlcpy(cap->card, hva->vdev->name, sizeof(cap->card));
+	snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s",
+		 hva->pdev->name);
+
+	return 0;
+}
+
+static int hva_enum_fmt_stream(struct file *file, void *priv,
+			       struct v4l2_fmtdesc *f)
+{
+	struct hva_ctx *ctx = fh_to_ctx(file->private_data);
+	struct hva_dev *hva = ctx_to_hdev(ctx);
+
+	if (unlikely(f->index >= hva->nb_of_streamformats))
+		return -EINVAL;
+
+	f->pixelformat = hva->streamformats[f->index];
+
+	return 0;
+}
+
+static int hva_enum_fmt_frame(struct file *file, void *priv,
+			      struct v4l2_fmtdesc *f)
+{
+	struct hva_ctx *ctx = fh_to_ctx(file->private_data);
+	struct hva_dev *hva = ctx_to_hdev(ctx);
+
+	if (unlikely(f->index >= hva->nb_of_pixelformats))
+		return -EINVAL;
+
+	f->pixelformat = hva->pixelformats[f->index];
+
+	return 0;
+}
+
+static int hva_g_fmt_stream(struct file *file, void *fh, struct v4l2_format *f)
+{
+	struct hva_ctx *ctx = fh_to_ctx(file->private_data);
+	struct hva_streaminfo *streaminfo = &ctx->streaminfo;
+
+	f->fmt.pix.width = streaminfo->width;
+	f->fmt.pix.height = streaminfo->height;
+	f->fmt.pix.field = V4L2_FIELD_NONE;
+	f->fmt.pix.colorspace = ctx->colorspace;
+	f->fmt.pix.xfer_func = ctx->xfer_func;
+	f->fmt.pix.ycbcr_enc = ctx->ycbcr_enc;
+	f->fmt.pix.quantization = ctx->quantization;
+	f->fmt.pix.pixelformat = streaminfo->streamformat;
+	f->fmt.pix.bytesperline = 0;
+	f->fmt.pix.sizeimage = ctx->max_stream_size;
+
+	return 0;
+}
+
+static int hva_g_fmt_frame(struct file *file, void *fh, struct v4l2_format *f)
+{
+	struct hva_ctx *ctx = fh_to_ctx(file->private_data);
+	struct hva_frameinfo *frameinfo = &ctx->frameinfo;
+
+	f->fmt.pix.width = frameinfo->width;
+	f->fmt.pix.height = frameinfo->height;
+	f->fmt.pix.field = V4L2_FIELD_NONE;
+	f->fmt.pix.colorspace = ctx->colorspace;
+	f->fmt.pix.xfer_func = ctx->xfer_func;
+	f->fmt.pix.ycbcr_enc = ctx->ycbcr_enc;
+	f->fmt.pix.quantization = ctx->quantization;
+	f->fmt.pix.pixelformat = frameinfo->pixelformat;
+	f->fmt.pix.bytesperline = frame_stride(frameinfo->aligned_width,
+					       frameinfo->pixelformat);
+	f->fmt.pix.sizeimage = frameinfo->size;
+
+	return 0;
+}
+
+static int hva_try_fmt_stream(struct file *file, void *priv,
+			      struct v4l2_format *f)
+{
+	struct hva_ctx *ctx = fh_to_ctx(file->private_data);
+	struct device *dev = ctx_to_dev(ctx);
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	u32 streamformat = pix->pixelformat;
+	const struct hva_enc *enc;
+	u32 width, height;
+	u32 stream_size;
+
+	enc = hva_find_encoder(ctx, ctx->frameinfo.pixelformat, streamformat);
+	if (!enc) {
+		dev_dbg(dev,
+			"%s V4L2 TRY_FMT (CAPTURE): unsupported format %.4s\n",
+			ctx->name, (char *)&pix->pixelformat);
+		return -EINVAL;
+	}
+
+	width = pix->width;
+	height = pix->height;
+	if (ctx->flags & HVA_FLAG_FRAMEINFO) {
+		/*
+		 * if the frame resolution is already fixed, only allow the
+		 * same stream resolution
+		 */
+		pix->width = ctx->frameinfo.width;
+		pix->height = ctx->frameinfo.height;
+		if ((pix->width != width) || (pix->height != height))
+			dev_dbg(dev,
+				"%s V4L2 TRY_FMT (CAPTURE): resolution updated %dx%d -> %dx%d to fit frame resolution\n",
+				ctx->name, width, height,
+				pix->width, pix->height);
+	} else {
+		/* adjust width & height */
+		v4l_bound_align_image(&pix->width,
+				      HVA_MIN_WIDTH, enc->max_width,
+				      0,
+				      &pix->height,
+				      HVA_MIN_HEIGHT, enc->max_height,
+				      0,
+				      0);
+
+		if ((pix->width != width) || (pix->height != height))
+			dev_dbg(dev,
+				"%s V4L2 TRY_FMT (CAPTURE): resolution updated %dx%d -> %dx%d to fit min/max/alignment\n",
+				ctx->name, width, height,
+				pix->width, pix->height);
+	}
+
+	stream_size = estimated_stream_size(pix->width, pix->height);
+	if (pix->sizeimage < stream_size)
+		pix->sizeimage = stream_size;
+
+	pix->bytesperline = 0;
+	pix->colorspace = ctx->colorspace;
+	pix->xfer_func = ctx->xfer_func;
+	pix->ycbcr_enc = ctx->ycbcr_enc;
+	pix->quantization = ctx->quantization;
+	pix->field = V4L2_FIELD_NONE;
+
+	return 0;
+}
+
+static int hva_try_fmt_frame(struct file *file, void *priv,
+			     struct v4l2_format *f)
+{
+	struct hva_ctx *ctx = fh_to_ctx(file->private_data);
+	struct device *dev = ctx_to_dev(ctx);
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	u32 pixelformat = pix->pixelformat;
+	const struct hva_enc *enc;
+	u32 width, height;
+
+	enc = hva_find_encoder(ctx, pixelformat, ctx->streaminfo.streamformat);
+	if (!enc) {
+		dev_dbg(dev,
+			"%s V4L2 TRY_FMT (OUTPUT): unsupported format %.4s\n",
+			ctx->name, (char *)&pixelformat);
+		return -EINVAL;
+	}
+
+	/* adjust width & height */
+	width = pix->width;
+	height = pix->height;
+	v4l_bound_align_image(&pix->width,
+			      HVA_MIN_WIDTH, HVA_MAX_WIDTH,
+			      frame_alignment(pixelformat) - 1,
+			      &pix->height,
+			      HVA_MIN_HEIGHT, HVA_MAX_HEIGHT,
+			      frame_alignment(pixelformat) - 1,
+			      0);
+
+	if ((pix->width != width) || (pix->height != height))
+		dev_dbg(dev,
+			"%s V4L2 TRY_FMT (OUTPUT): resolution updated %dx%d -> %dx%d to fit min/max/alignment\n",
+			ctx->name, width, height, pix->width, pix->height);
+
+	width = ALIGN(pix->width, HVA_WIDTH_ALIGNMENT);
+	height = ALIGN(pix->height, HVA_HEIGHT_ALIGNMENT);
+
+	if (!pix->colorspace) {
+		pix->colorspace = V4L2_COLORSPACE_REC709;
+		pix->xfer_func = V4L2_XFER_FUNC_DEFAULT;
+		pix->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
+		pix->quantization = V4L2_QUANTIZATION_DEFAULT;
+	}
+
+	pix->bytesperline = frame_stride(width, pixelformat);
+	pix->sizeimage = frame_size(width, height, pixelformat);
+	pix->field = V4L2_FIELD_NONE;
+
+	return 0;
+}
+
+static int hva_s_fmt_stream(struct file *file, void *fh, struct v4l2_format *f)
+{
+	struct hva_ctx *ctx = fh_to_ctx(file->private_data);
+	struct device *dev = ctx_to_dev(ctx);
+	struct vb2_queue *vq;
+	int ret;
+
+	ret = hva_try_fmt_stream(file, fh, f);
+	if (ret) {
+		dev_dbg(dev, "%s V4L2 S_FMT (CAPTURE): unsupported format %.4s\n",
+			ctx->name, (char *)&f->fmt.pix.pixelformat);
+		return ret;
+	}
+
+	vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
+	if (vb2_is_streaming(vq)) {
+		dev_dbg(dev, "%s V4L2 S_FMT (CAPTURE): queue busy\n",
+			ctx->name);
+		return -EBUSY;
+	}
+
+	ctx->max_stream_size = f->fmt.pix.sizeimage;
+	ctx->streaminfo.width = f->fmt.pix.width;
+	ctx->streaminfo.height = f->fmt.pix.height;
+	ctx->streaminfo.streamformat = f->fmt.pix.pixelformat;
+	ctx->flags |= HVA_FLAG_STREAMINFO;
+
+	return 0;
+}
+
+static int hva_s_fmt_frame(struct file *file, void *fh, struct v4l2_format *f)
+{
+	struct hva_ctx *ctx = fh_to_ctx(file->private_data);
+	struct device *dev = ctx_to_dev(ctx);
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	struct vb2_queue *vq;
+	int ret;
+
+	ret = hva_try_fmt_frame(file, fh, f);
+	if (ret) {
+		dev_dbg(dev, "%s V4L2 S_FMT (OUTPUT): unsupported format %.4s\n",
+			ctx->name, (char *)&pix->pixelformat);
+		return ret;
+	}
+
+	vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
+	if (vb2_is_streaming(vq)) {
+		dev_dbg(dev, "%s V4L2 S_FMT (OUTPUT): queue busy\n", ctx->name);
+		return -EBUSY;
+	}
+
+	ctx->colorspace = pix->colorspace;
+	ctx->xfer_func = pix->xfer_func;
+	ctx->ycbcr_enc = pix->ycbcr_enc;
+	ctx->quantization = pix->quantization;
+
+	ctx->frameinfo.aligned_width = ALIGN(pix->width, HVA_WIDTH_ALIGNMENT);
+	ctx->frameinfo.aligned_height = ALIGN(pix->height,
+					      HVA_HEIGHT_ALIGNMENT);
+	ctx->frameinfo.size = pix->sizeimage;
+	ctx->frameinfo.pixelformat = pix->pixelformat;
+	ctx->frameinfo.width = pix->width;
+	ctx->frameinfo.height = pix->height;
+	ctx->flags |= HVA_FLAG_FRAMEINFO;
+
+	return 0;
+}
+
+static int hva_g_parm(struct file *file, void *fh, struct v4l2_streamparm *sp)
+{
+	struct hva_ctx *ctx = fh_to_ctx(file->private_data);
+	struct v4l2_fract *time_per_frame = &ctx->ctrls.time_per_frame;
+
+	if (sp->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
+		return -EINVAL;
+
+	sp->parm.output.capability = V4L2_CAP_TIMEPERFRAME;
+	sp->parm.output.timeperframe.numerator = time_per_frame->numerator;
+	sp->parm.output.timeperframe.denominator =
+		time_per_frame->denominator;
+
+	return 0;
+}
+
+static int hva_s_parm(struct file *file, void *fh, struct v4l2_streamparm *sp)
+{
+	struct hva_ctx *ctx = fh_to_ctx(file->private_data);
+	struct v4l2_fract *time_per_frame = &ctx->ctrls.time_per_frame;
+
+	if (sp->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
+		return -EINVAL;
+
+	if (!sp->parm.output.timeperframe.numerator ||
+	    !sp->parm.output.timeperframe.denominator)
+		return hva_g_parm(file, fh, sp);
+
+	sp->parm.output.capability = V4L2_CAP_TIMEPERFRAME;
+	time_per_frame->numerator = sp->parm.output.timeperframe.numerator;
+	time_per_frame->denominator =
+		sp->parm.output.timeperframe.denominator;
+
+	return 0;
+}
+
+static int hva_qbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
+{
+	struct hva_ctx *ctx = fh_to_ctx(file->private_data);
+	struct device *dev = ctx_to_dev(ctx);
+
+	if (buf->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+		/*
+		 * depending on the targeted compressed video format, the
+		 * capture buffer might contain headers (e.g. H.264 SPS/PPS)
+		 * filled in by the driver client; the size of these data is
+		 * copied from the bytesused field of the V4L2 buffer in the
+		 * payload field of the hva stream buffer
+		 */
+		struct vb2_queue *vq;
+		struct hva_stream *stream;
+
+		vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, buf->type);
+
+		if (buf->index >= vq->num_buffers) {
+			dev_dbg(dev, "%s buffer index %d out of range (%d)\n",
+				ctx->name, buf->index, vq->num_buffers);
+			return -EINVAL;
+		}
+
+		stream = (struct hva_stream *)vq->bufs[buf->index];
+		stream->bytesused = buf->bytesused;
+	}
+
+	return v4l2_m2m_qbuf(file, ctx->fh.m2m_ctx, buf);
+}
+
+/* V4L2 ioctl ops */
+static const struct v4l2_ioctl_ops hva_ioctl_ops = {
+	.vidioc_querycap		= hva_querycap,
+	.vidioc_enum_fmt_vid_cap	= hva_enum_fmt_stream,
+	.vidioc_enum_fmt_vid_out	= hva_enum_fmt_frame,
+	.vidioc_g_fmt_vid_cap		= hva_g_fmt_stream,
+	.vidioc_g_fmt_vid_out		= hva_g_fmt_frame,
+	.vidioc_try_fmt_vid_cap		= hva_try_fmt_stream,
+	.vidioc_try_fmt_vid_out		= hva_try_fmt_frame,
+	.vidioc_s_fmt_vid_cap		= hva_s_fmt_stream,
+	.vidioc_s_fmt_vid_out		= hva_s_fmt_frame,
+	.vidioc_g_parm			= hva_g_parm,
+	.vidioc_s_parm			= hva_s_parm,
+	.vidioc_reqbufs			= v4l2_m2m_ioctl_reqbufs,
+	.vidioc_create_bufs             = v4l2_m2m_ioctl_create_bufs,
+	.vidioc_querybuf		= v4l2_m2m_ioctl_querybuf,
+	.vidioc_expbuf			= v4l2_m2m_ioctl_expbuf,
+	.vidioc_qbuf			= hva_qbuf,
+	.vidioc_dqbuf			= v4l2_m2m_ioctl_dqbuf,
+	.vidioc_streamon		= v4l2_m2m_ioctl_streamon,
+	.vidioc_streamoff		= v4l2_m2m_ioctl_streamoff,
+	.vidioc_subscribe_event		= v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event	= v4l2_event_unsubscribe,
+};
+
+/*
+ * V4L2 control operations
+ */
+
+static int hva_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct hva_ctx *ctx = container_of(ctrl->handler, struct hva_ctx,
+					   ctrl_handler);
+	struct device *dev = ctx_to_dev(ctx);
+
+	dev_dbg(dev, "%s S_CTRL: id = %d, val = %d\n", ctx->name,
+		ctrl->id, ctrl->val);
+
+	switch (ctrl->id) {
+	case V4L2_CID_MPEG_VIDEO_BITRATE_MODE:
+		ctx->ctrls.bitrate_mode = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_GOP_SIZE:
+		ctx->ctrls.gop_size = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_BITRATE:
+		ctx->ctrls.bitrate = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_ASPECT:
+		ctx->ctrls.aspect = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_PROFILE:
+		ctx->ctrls.profile = ctrl->val;
+		if (ctx->flags & HVA_FLAG_STREAMINFO)
+			snprintf(ctx->streaminfo.profile,
+				 sizeof(ctx->streaminfo.profile),
+				 "%s profile",
+				 v4l2_ctrl_get_menu(ctrl->id)[ctrl->val]);
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_LEVEL:
+		ctx->ctrls.level = ctrl->val;
+		if (ctx->flags & HVA_FLAG_STREAMINFO)
+			snprintf(ctx->streaminfo.level,
+				 sizeof(ctx->streaminfo.level),
+				 "level %s",
+				 v4l2_ctrl_get_menu(ctrl->id)[ctrl->val]);
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_ENTROPY_MODE:
+		ctx->ctrls.entropy_mode = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_CPB_SIZE:
+		ctx->ctrls.cpb_size = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_8X8_TRANSFORM:
+		ctx->ctrls.dct8x8 = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_MIN_QP:
+		ctx->ctrls.qpmin = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_MAX_QP:
+		ctx->ctrls.qpmax = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_VUI_SAR_ENABLE:
+		ctx->ctrls.vui_sar = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_VUI_SAR_IDC:
+		ctx->ctrls.vui_sar_idc = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_SEI_FRAME_PACKING:
+		ctx->ctrls.sei_fp = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_SEI_FP_ARRANGEMENT_TYPE:
+		ctx->ctrls.sei_fp_type = ctrl->val;
+		break;
+	default:
+		dev_dbg(dev, "%s S_CTRL: invalid control (id = %d)\n",
+			ctx->name, ctrl->id);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/* V4L2 control ops */
+static const struct v4l2_ctrl_ops hva_ctrl_ops = {
+	.s_ctrl = hva_s_ctrl,
+};
+
+static int hva_ctrls_setup(struct hva_ctx *ctx)
+{
+	struct device *dev = ctx_to_dev(ctx);
+	u64 mask;
+	enum v4l2_mpeg_video_h264_sei_fp_arrangement_type sei_fp_type =
+		V4L2_MPEG_VIDEO_H264_SEI_FP_ARRANGEMENT_TYPE_TOP_BOTTOM;
+
+	v4l2_ctrl_handler_init(&ctx->ctrl_handler, 15);
+
+	v4l2_ctrl_new_std_menu(&ctx->ctrl_handler, &hva_ctrl_ops,
+			       V4L2_CID_MPEG_VIDEO_BITRATE_MODE,
+			       V4L2_MPEG_VIDEO_BITRATE_MODE_CBR,
+			       0,
+			       V4L2_MPEG_VIDEO_BITRATE_MODE_CBR);
+
+	v4l2_ctrl_new_std(&ctx->ctrl_handler, &hva_ctrl_ops,
+			  V4L2_CID_MPEG_VIDEO_GOP_SIZE,
+			  1, 60, 1, 16);
+
+	v4l2_ctrl_new_std(&ctx->ctrl_handler, &hva_ctrl_ops,
+			  V4L2_CID_MPEG_VIDEO_BITRATE,
+			  1000, 60000000, 1000, 20000000);
+
+	mask = ~(1 << V4L2_MPEG_VIDEO_ASPECT_1x1);
+	v4l2_ctrl_new_std_menu(&ctx->ctrl_handler, &hva_ctrl_ops,
+			       V4L2_CID_MPEG_VIDEO_ASPECT,
+			       V4L2_MPEG_VIDEO_ASPECT_1x1,
+			       mask,
+			       V4L2_MPEG_VIDEO_ASPECT_1x1);
+
+	mask = ~((1 << V4L2_MPEG_VIDEO_H264_PROFILE_BASELINE) |
+		 (1 << V4L2_MPEG_VIDEO_H264_PROFILE_MAIN) |
+		 (1 << V4L2_MPEG_VIDEO_H264_PROFILE_HIGH) |
+		 (1 << V4L2_MPEG_VIDEO_H264_PROFILE_STEREO_HIGH));
+	v4l2_ctrl_new_std_menu(&ctx->ctrl_handler, &hva_ctrl_ops,
+			       V4L2_CID_MPEG_VIDEO_H264_PROFILE,
+			       V4L2_MPEG_VIDEO_H264_PROFILE_STEREO_HIGH,
+			       mask,
+			       V4L2_MPEG_VIDEO_H264_PROFILE_HIGH);
+
+	v4l2_ctrl_new_std_menu(&ctx->ctrl_handler, &hva_ctrl_ops,
+			       V4L2_CID_MPEG_VIDEO_H264_LEVEL,
+			       V4L2_MPEG_VIDEO_H264_LEVEL_4_2,
+			       0,
+			       V4L2_MPEG_VIDEO_H264_LEVEL_4_0);
+
+	v4l2_ctrl_new_std_menu(&ctx->ctrl_handler, &hva_ctrl_ops,
+			       V4L2_CID_MPEG_VIDEO_H264_ENTROPY_MODE,
+			       V4L2_MPEG_VIDEO_H264_ENTROPY_MODE_CABAC,
+			       0,
+			       V4L2_MPEG_VIDEO_H264_ENTROPY_MODE_CAVLC);
+
+	v4l2_ctrl_new_std(&ctx->ctrl_handler, &hva_ctrl_ops,
+			  V4L2_CID_MPEG_VIDEO_H264_CPB_SIZE,
+			  1, 10000, 1, 3000);
+
+	v4l2_ctrl_new_std(&ctx->ctrl_handler, &hva_ctrl_ops,
+			  V4L2_CID_MPEG_VIDEO_H264_8X8_TRANSFORM,
+			  0, 1, 1, 0);
+
+	v4l2_ctrl_new_std(&ctx->ctrl_handler, &hva_ctrl_ops,
+			  V4L2_CID_MPEG_VIDEO_H264_MIN_QP,
+			  0, 51, 1, 5);
+
+	v4l2_ctrl_new_std(&ctx->ctrl_handler, &hva_ctrl_ops,
+			  V4L2_CID_MPEG_VIDEO_H264_MAX_QP,
+			  0, 51, 1, 51);
+
+	v4l2_ctrl_new_std(&ctx->ctrl_handler, &hva_ctrl_ops,
+			  V4L2_CID_MPEG_VIDEO_H264_VUI_SAR_ENABLE,
+			  0, 1, 1, 1);
+
+	mask = ~(1 << V4L2_MPEG_VIDEO_H264_VUI_SAR_IDC_1x1);
+	v4l2_ctrl_new_std_menu(&ctx->ctrl_handler, &hva_ctrl_ops,
+			       V4L2_CID_MPEG_VIDEO_H264_VUI_SAR_IDC,
+			       V4L2_MPEG_VIDEO_H264_VUI_SAR_IDC_1x1,
+			       mask,
+			       V4L2_MPEG_VIDEO_H264_VUI_SAR_IDC_1x1);
+
+	v4l2_ctrl_new_std(&ctx->ctrl_handler, &hva_ctrl_ops,
+			  V4L2_CID_MPEG_VIDEO_H264_SEI_FRAME_PACKING,
+			  0, 1, 1, 0);
+
+	mask = ~(1 << sei_fp_type);
+	v4l2_ctrl_new_std_menu(&ctx->ctrl_handler, &hva_ctrl_ops,
+			       V4L2_CID_MPEG_VIDEO_H264_SEI_FP_ARRANGEMENT_TYPE,
+			       sei_fp_type,
+			       mask,
+			       sei_fp_type);
+
+	if (ctx->ctrl_handler.error) {
+		int err = ctx->ctrl_handler.error;
+
+		dev_dbg(dev, "%s controls setup failed (%d)\n",
+			ctx->name, err);
+		v4l2_ctrl_handler_free(&ctx->ctrl_handler);
+		return err;
+	}
+
+	v4l2_ctrl_handler_setup(&ctx->ctrl_handler);
+
+	/* set default time per frame */
+	ctx->ctrls.time_per_frame.numerator = HVA_DEFAULT_FRAME_NUM;
+	ctx->ctrls.time_per_frame.denominator = HVA_DEFAULT_FRAME_DEN;
+
+	return 0;
+}
+
+/*
+ * mem-to-mem operations
+ */
+
+static void hva_run_work(struct work_struct *work)
+{
+	struct hva_ctx *ctx = container_of(work, struct hva_ctx, run_work);
+	struct vb2_v4l2_buffer *src_buf, *dst_buf;
+	const struct hva_enc *enc = ctx->enc;
+	struct hva_frame *frame;
+	struct hva_stream *stream;
+	int ret;
+
+	/* protect instance against reentrancy */
+	mutex_lock(&ctx->lock);
+
+	src_buf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
+	dst_buf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
+
+	frame = to_hva_frame(src_buf);
+	stream = to_hva_stream(dst_buf);
+	frame->vbuf.sequence = ctx->frame_num++;
+
+	ret = enc->encode(ctx, frame, stream);
+
+	vb2_set_plane_payload(&dst_buf->vb2_buf, 0, stream->bytesused);
+	if (ret) {
+		v4l2_m2m_buf_done(src_buf, VB2_BUF_STATE_ERROR);
+		v4l2_m2m_buf_done(dst_buf, VB2_BUF_STATE_ERROR);
+	} else {
+		/* propagate frame timestamp */
+		dst_buf->vb2_buf.timestamp = src_buf->vb2_buf.timestamp;
+		dst_buf->field = V4L2_FIELD_NONE;
+		dst_buf->sequence = ctx->stream_num - 1;
+
+		v4l2_m2m_buf_done(src_buf, VB2_BUF_STATE_DONE);
+		v4l2_m2m_buf_done(dst_buf, VB2_BUF_STATE_DONE);
+	}
+
+	mutex_unlock(&ctx->lock);
+
+	v4l2_m2m_job_finish(ctx->hva_dev->m2m_dev, ctx->fh.m2m_ctx);
+}
+
+static void hva_device_run(void *priv)
+{
+	struct hva_ctx *ctx = priv;
+	struct hva_dev *hva = ctx_to_hdev(ctx);
+
+	queue_work(hva->work_queue, &ctx->run_work);
+}
+
+static void hva_job_abort(void *priv)
+{
+	struct hva_ctx *ctx = priv;
+	struct device *dev = ctx_to_dev(ctx);
+
+	dev_dbg(dev, "%s aborting job\n", ctx->name);
+
+	ctx->aborting = true;
+}
+
+static int hva_job_ready(void *priv)
+{
+	struct hva_ctx *ctx = priv;
+	struct device *dev = ctx_to_dev(ctx);
+
+	if (!v4l2_m2m_num_src_bufs_ready(ctx->fh.m2m_ctx)) {
+		dev_dbg(dev, "%s job not ready: no frame buffers\n",
+			ctx->name);
+		return 0;
+	}
+
+	if (!v4l2_m2m_num_dst_bufs_ready(ctx->fh.m2m_ctx)) {
+		dev_dbg(dev, "%s job not ready: no stream buffers\n",
+			ctx->name);
+		return 0;
+	}
+
+	if (ctx->aborting) {
+		dev_dbg(dev, "%s job not ready: aborting\n", ctx->name);
+		return 0;
+	}
+
+	return 1;
+}
+
+/* mem-to-mem ops */
+static const struct v4l2_m2m_ops hva_m2m_ops = {
+	.device_run	= hva_device_run,
+	.job_abort	= hva_job_abort,
+	.job_ready	= hva_job_ready,
+};
+
+/*
+ * VB2 queue operations
+ */
+
+static int hva_queue_setup(struct vb2_queue *vq,
+			   unsigned int *num_buffers, unsigned int *num_planes,
+			   unsigned int sizes[], struct device *alloc_devs[])
+{
+	struct hva_ctx *ctx = vb2_get_drv_priv(vq);
+	struct device *dev = ctx_to_dev(ctx);
+	unsigned int size;
+
+	dev_dbg(dev, "%s %s queue setup: num_buffers %d\n", ctx->name,
+		to_type_str(vq->type), *num_buffers);
+
+	size = vq->type == V4L2_BUF_TYPE_VIDEO_OUTPUT ?
+		ctx->frameinfo.size : ctx->max_stream_size;
+
+	if (*num_planes)
+		return sizes[0] < size ? -EINVAL : 0;
+
+	/* only one plane supported */
+	*num_planes = 1;
+	sizes[0] = size;
+
+	return 0;
+}
+
+static int hva_buf_prepare(struct vb2_buffer *vb)
+{
+	struct hva_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+	struct device *dev = ctx_to_dev(ctx);
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+
+	if (vb->vb2_queue->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+		struct hva_frame *frame = to_hva_frame(vbuf);
+
+		if (vbuf->field == V4L2_FIELD_ANY)
+			vbuf->field = V4L2_FIELD_NONE;
+		if (vbuf->field != V4L2_FIELD_NONE) {
+			dev_dbg(dev,
+				"%s frame[%d] prepare: %d field not supported\n",
+				ctx->name, vb->index, vbuf->field);
+			return -EINVAL;
+		}
+
+		if (!frame->prepared) {
+			/* get memory addresses */
+			frame->vaddr = vb2_plane_vaddr(&vbuf->vb2_buf, 0);
+			frame->paddr = vb2_dma_contig_plane_dma_addr(
+					&vbuf->vb2_buf, 0);
+			frame->info = ctx->frameinfo;
+			frame->prepared = true;
+
+			dev_dbg(dev,
+				"%s frame[%d] prepared; virt=%p, phy=%pad\n",
+				ctx->name, vb->index,
+				frame->vaddr, &frame->paddr);
+		}
+	} else {
+		struct hva_stream *stream = to_hva_stream(vbuf);
+
+		if (!stream->prepared) {
+			/* get memory addresses */
+			stream->vaddr = vb2_plane_vaddr(&vbuf->vb2_buf, 0);
+			stream->paddr = vb2_dma_contig_plane_dma_addr(
+					&vbuf->vb2_buf, 0);
+			stream->size = vb2_plane_size(&vbuf->vb2_buf, 0);
+			stream->prepared = true;
+
+			dev_dbg(dev,
+				"%s stream[%d] prepared; virt=%p, phy=%pad\n",
+				ctx->name, vb->index,
+				stream->vaddr, &stream->paddr);
+		}
+	}
+
+	return 0;
+}
+
+static void hva_buf_queue(struct vb2_buffer *vb)
+{
+	struct hva_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+
+	if (ctx->fh.m2m_ctx)
+		v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vbuf);
+}
+
+static int hva_start_streaming(struct vb2_queue *vq, unsigned int count)
+{
+	struct hva_ctx *ctx = vb2_get_drv_priv(vq);
+	struct hva_dev *hva = ctx_to_hdev(ctx);
+	struct device *dev = ctx_to_dev(ctx);
+	struct vb2_v4l2_buffer *vbuf;
+	int ret;
+	unsigned int i;
+	bool found = false;
+
+	dev_dbg(dev, "%s %s start streaming\n", ctx->name,
+		to_type_str(vq->type));
+
+	/* open encoder when both start_streaming have been called */
+	if (V4L2_TYPE_IS_OUTPUT(vq->type)) {
+		if (!vb2_start_streaming_called(&ctx->fh.m2m_ctx->cap_q_ctx.q))
+			return 0;
+	} else {
+		if (!vb2_start_streaming_called(&ctx->fh.m2m_ctx->out_q_ctx.q))
+			return 0;
+	}
+
+	/* store the instance context in the instances array */
+	for (i = 0; i < HVA_MAX_INSTANCES; i++) {
+		if (!hva->instances[i]) {
+			hva->instances[i] = ctx;
+			/* save the context identifier in the context */
+			ctx->id = i;
+			found = true;
+			break;
+		}
+	}
+
+	if (!found) {
+		dev_err(dev, "%s maximum instances reached\n", ctx->name);
+		ret = -ENOMEM;
+		goto err;
+	}
+
+	hva->nb_of_instances++;
+
+	if (!ctx->enc) {
+		ret = hva_open_encoder(ctx,
+				       ctx->streaminfo.streamformat,
+				       ctx->frameinfo.pixelformat,
+				       &ctx->enc);
+		if (ret < 0)
+			goto err_ctx;
+	}
+
+	return 0;
+
+err_ctx:
+	hva->instances[ctx->id] = NULL;
+	hva->nb_of_instances--;
+err:
+	if (vq->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+		/* return of all pending buffers to vb2 (in queued state) */
+		while ((vbuf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx)))
+			v4l2_m2m_buf_done(vbuf, VB2_BUF_STATE_QUEUED);
+	} else {
+		/* return of all pending buffers to vb2 (in queued state) */
+		while ((vbuf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx)))
+			v4l2_m2m_buf_done(vbuf, VB2_BUF_STATE_QUEUED);
+	}
+
+	return ret;
+}
+
+static void hva_stop_streaming(struct vb2_queue *vq)
+{
+	struct hva_ctx *ctx = vb2_get_drv_priv(vq);
+	struct hva_dev *hva = ctx_to_hdev(ctx);
+	struct device *dev = ctx_to_dev(ctx);
+	const struct hva_enc *enc = ctx->enc;
+	struct vb2_v4l2_buffer *vbuf;
+
+	dev_dbg(dev, "%s %s stop streaming\n", ctx->name,
+		to_type_str(vq->type));
+
+	if (vq->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+		/* return of all pending buffers to vb2 (in error state) */
+		ctx->frame_num = 0;
+		while ((vbuf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx)))
+			v4l2_m2m_buf_done(vbuf, VB2_BUF_STATE_ERROR);
+	} else {
+		/* return of all pending buffers to vb2 (in error state) */
+		ctx->stream_num = 0;
+		while ((vbuf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx)))
+			v4l2_m2m_buf_done(vbuf, VB2_BUF_STATE_ERROR);
+	}
+
+	if ((V4L2_TYPE_IS_OUTPUT(vq->type) &&
+	     vb2_is_streaming(&ctx->fh.m2m_ctx->cap_q_ctx.q)) ||
+	    (!V4L2_TYPE_IS_OUTPUT(vq->type) &&
+	     vb2_is_streaming(&ctx->fh.m2m_ctx->out_q_ctx.q))) {
+		dev_dbg(dev, "%s %s out=%d cap=%d\n",
+			ctx->name, to_type_str(vq->type),
+			vb2_is_streaming(&ctx->fh.m2m_ctx->out_q_ctx.q),
+			vb2_is_streaming(&ctx->fh.m2m_ctx->cap_q_ctx.q));
+		return;
+	}
+
+	/* close encoder when both stop_streaming have been called */
+	if (enc) {
+		dev_dbg(dev, "%s %s encoder closed\n", ctx->name, enc->name);
+		enc->close(ctx);
+		ctx->enc = NULL;
+
+		/* clear instance context in instances array */
+		hva->instances[ctx->id] = NULL;
+		hva->nb_of_instances--;
+	}
+
+	ctx->aborting = false;
+}
+
+/* VB2 queue ops */
+static const struct vb2_ops hva_qops = {
+	.queue_setup		= hva_queue_setup,
+	.buf_prepare		= hva_buf_prepare,
+	.buf_queue		= hva_buf_queue,
+	.start_streaming	= hva_start_streaming,
+	.stop_streaming		= hva_stop_streaming,
+	.wait_prepare		= vb2_ops_wait_prepare,
+	.wait_finish		= vb2_ops_wait_finish,
+};
+
+/*
+ * V4L2 file operations
+ */
+
+static int queue_init(struct hva_ctx *ctx, struct vb2_queue *vq)
+{
+	vq->io_modes = VB2_MMAP | VB2_DMABUF;
+	vq->drv_priv = ctx;
+	vq->ops = &hva_qops;
+	vq->mem_ops = &vb2_dma_contig_memops;
+	vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	vq->lock = &ctx->hva_dev->lock;
+
+	return vb2_queue_init(vq);
+}
+
+static int hva_queue_init(void *priv, struct vb2_queue *src_vq,
+			  struct vb2_queue *dst_vq)
+{
+	struct hva_ctx *ctx = priv;
+	int ret;
+
+	src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
+	src_vq->buf_struct_size = sizeof(struct hva_frame);
+	src_vq->min_buffers_needed = MIN_FRAMES;
+	src_vq->dev = ctx->hva_dev->dev;
+
+	ret = queue_init(ctx, src_vq);
+	if (ret)
+		return ret;
+
+	dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	dst_vq->buf_struct_size = sizeof(struct hva_stream);
+	dst_vq->min_buffers_needed = MIN_STREAMS;
+	dst_vq->dev = ctx->hva_dev->dev;
+
+	return queue_init(ctx, dst_vq);
+}
+
+static int hva_open(struct file *file)
+{
+	struct hva_dev *hva = video_drvdata(file);
+	struct device *dev = hva_to_dev(hva);
+	struct hva_ctx *ctx;
+	int ret;
+
+	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
+	if (!ctx) {
+		ret = -ENOMEM;
+		goto out;
+	}
+	ctx->hva_dev = hva;
+
+	INIT_WORK(&ctx->run_work, hva_run_work);
+	v4l2_fh_init(&ctx->fh, video_devdata(file));
+	file->private_data = &ctx->fh;
+	v4l2_fh_add(&ctx->fh);
+
+	ret = hva_ctrls_setup(ctx);
+	if (ret) {
+		dev_err(dev, "%s [x:x] failed to setup controls\n",
+			HVA_PREFIX);
+		goto err_fh;
+	}
+	ctx->fh.ctrl_handler = &ctx->ctrl_handler;
+
+	mutex_init(&ctx->lock);
+
+	ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(hva->m2m_dev, ctx,
+					    &hva_queue_init);
+	if (IS_ERR(ctx->fh.m2m_ctx)) {
+		ret = PTR_ERR(ctx->fh.m2m_ctx);
+		dev_err(dev, "%s failed to initialize m2m context (%d)\n",
+			HVA_PREFIX, ret);
+		goto err_ctrls;
+	}
+
+	/* set the instance name */
+	mutex_lock(&hva->lock);
+	hva->instance_id++;
+	snprintf(ctx->name, sizeof(ctx->name), "[%3d:----]",
+		 hva->instance_id);
+	mutex_unlock(&hva->lock);
+
+	/* default parameters for frame and stream */
+	set_default_params(ctx);
+
+	dev_info(dev, "%s encoder instance created\n", ctx->name);
+
+	return 0;
+
+err_ctrls:
+	v4l2_ctrl_handler_free(&ctx->ctrl_handler);
+err_fh:
+	v4l2_fh_del(&ctx->fh);
+	v4l2_fh_exit(&ctx->fh);
+	kfree(ctx);
+out:
+	return ret;
+}
+
+static int hva_release(struct file *file)
+{
+	struct hva_ctx *ctx = fh_to_ctx(file->private_data);
+	struct hva_dev *hva = ctx_to_hdev(ctx);
+	struct device *dev = ctx_to_dev(ctx);
+	const struct hva_enc *enc = ctx->enc;
+
+	if (enc) {
+		dev_dbg(dev, "%s %s encoder closed\n", ctx->name, enc->name);
+		enc->close(ctx);
+		ctx->enc = NULL;
+
+		/* clear instance context in instances array */
+		hva->instances[ctx->id] = NULL;
+		hva->nb_of_instances--;
+	}
+
+	v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
+
+	v4l2_ctrl_handler_free(&ctx->ctrl_handler);
+
+	v4l2_fh_del(&ctx->fh);
+	v4l2_fh_exit(&ctx->fh);
+
+	dev_info(dev, "%s encoder instance released\n", ctx->name);
+
+	kfree(ctx);
+
+	return 0;
+}
+
+/* V4L2 file ops */
+static const struct v4l2_file_operations hva_fops = {
+	.owner			= THIS_MODULE,
+	.open			= hva_open,
+	.release		= hva_release,
+	.unlocked_ioctl		= video_ioctl2,
+	.mmap			= v4l2_m2m_fop_mmap,
+	.poll			= v4l2_m2m_fop_poll,
+};
+
+/*
+ * Platform device operations
+ */
+
+static int hva_register_device(struct hva_dev *hva)
+{
+	int ret;
+	struct video_device *vdev;
+	struct device *dev;
+
+	if (!hva)
+		return -ENODEV;
+	dev = hva_to_dev(hva);
+
+	hva->m2m_dev = v4l2_m2m_init(&hva_m2m_ops);
+	if (IS_ERR(hva->m2m_dev)) {
+		dev_err(dev, "%s failed to initialize v4l2-m2m device\n",
+			HVA_PREFIX);
+		ret = PTR_ERR(hva->m2m_dev);
+		goto err;
+	}
+
+	vdev = video_device_alloc();
+	if (!vdev) {
+		dev_err(dev, "%s failed to allocate video device\n",
+			HVA_PREFIX);
+		ret = -ENOMEM;
+		goto err_m2m_release;
+	}
+
+	vdev->fops = &hva_fops;
+	vdev->ioctl_ops = &hva_ioctl_ops;
+	vdev->release = video_device_release;
+	vdev->lock = &hva->lock;
+	vdev->vfl_dir = VFL_DIR_M2M;
+	vdev->device_caps = V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_M2M;
+	vdev->v4l2_dev = &hva->v4l2_dev;
+	snprintf(vdev->name, sizeof(vdev->name), "%s%lx", HVA_NAME,
+		 hva->ip_version);
+
+	ret = video_register_device(vdev, VFL_TYPE_GRABBER, -1);
+	if (ret) {
+		dev_err(dev, "%s failed to register video device\n",
+			HVA_PREFIX);
+		goto err_vdev_release;
+	}
+
+	hva->vdev = vdev;
+	video_set_drvdata(vdev, hva);
+	return 0;
+
+err_vdev_release:
+	video_device_release(vdev);
+err_m2m_release:
+	v4l2_m2m_release(hva->m2m_dev);
+err:
+	return ret;
+}
+
+static void hva_unregister_device(struct hva_dev *hva)
+{
+	if (!hva)
+		return;
+
+	if (hva->m2m_dev)
+		v4l2_m2m_release(hva->m2m_dev);
+
+	video_unregister_device(hva->vdev);
+}
+
+static int hva_probe(struct platform_device *pdev)
+{
+	struct hva_dev *hva;
+	struct device *dev = &pdev->dev;
+	int ret;
+
+	hva = devm_kzalloc(dev, sizeof(*hva), GFP_KERNEL);
+	if (!hva) {
+		ret = -ENOMEM;
+		goto err;
+	}
+
+	hva->dev = dev;
+	hva->pdev = pdev;
+	platform_set_drvdata(pdev, hva);
+
+	mutex_init(&hva->lock);
+
+	/* probe hardware */
+	ret = hva_hw_probe(pdev, hva);
+	if (ret)
+		goto err;
+
+	/* register all available encoders */
+	register_encoders(hva);
+
+	/* register all supported formats */
+	register_formats(hva);
+
+	/* register on V4L2 */
+	ret = v4l2_device_register(dev, &hva->v4l2_dev);
+	if (ret) {
+		dev_err(dev, "%s %s failed to register V4L2 device\n",
+			HVA_PREFIX, HVA_NAME);
+		goto err_hw;
+	}
+
+	hva->work_queue = create_workqueue(HVA_NAME);
+	if (!hva->work_queue) {
+		dev_err(dev, "%s %s failed to allocate work queue\n",
+			HVA_PREFIX, HVA_NAME);
+		ret = -ENOMEM;
+		goto err_v4l2;
+	}
+
+	/* register device */
+	ret = hva_register_device(hva);
+	if (ret)
+		goto err_work_queue;
+
+	dev_info(dev, "%s %s registered as /dev/video%d\n", HVA_PREFIX,
+		 HVA_NAME, hva->vdev->num);
+
+	return 0;
+
+err_work_queue:
+	destroy_workqueue(hva->work_queue);
+err_v4l2:
+	v4l2_device_unregister(&hva->v4l2_dev);
+err_hw:
+	hva_hw_remove(hva);
+err:
+	return ret;
+}
+
+static int hva_remove(struct platform_device *pdev)
+{
+	struct hva_dev *hva = platform_get_drvdata(pdev);
+	struct device *dev = hva_to_dev(hva);
+
+	hva_unregister_device(hva);
+
+	destroy_workqueue(hva->work_queue);
+
+	hva_hw_remove(hva);
+
+	v4l2_device_unregister(&hva->v4l2_dev);
+
+	dev_info(dev, "%s %s removed\n", HVA_PREFIX, pdev->name);
+
+	return 0;
+}
+
+/* PM ops */
+static const struct dev_pm_ops hva_pm_ops = {
+	.runtime_suspend	= hva_hw_runtime_suspend,
+	.runtime_resume		= hva_hw_runtime_resume,
+};
+
+static const struct of_device_id hva_match_types[] = {
+	{
+	 .compatible = "st,st-hva",
+	},
+	{ /* end node */ }
+};
+
+MODULE_DEVICE_TABLE(of, hva_match_types);
+
+static struct platform_driver hva_driver = {
+	.probe  = hva_probe,
+	.remove = hva_remove,
+	.driver = {
+		.name		= HVA_NAME,
+		.of_match_table	= hva_match_types,
+		.pm		= &hva_pm_ops,
+		},
+};
+
+module_platform_driver(hva_driver);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Yannick Fertre <yannick.fertre@st.com>");
+MODULE_DESCRIPTION("STMicroelectronics HVA video encoder V4L2 driver");
diff --git a/drivers/media/platform/sti/hva/hva.h b/drivers/media/platform/sti/hva/hva.h
new file mode 100644
index 000000000000..caa580825541
--- /dev/null
+++ b/drivers/media/platform/sti/hva/hva.h
@@ -0,0 +1,315 @@
+/*
+ * Copyright (C) STMicroelectronics SA 2015
+ * Authors: Yannick Fertre <yannick.fertre@st.com>
+ *          Hugues Fruchet <hugues.fruchet@st.com>
+ * License terms:  GNU General Public License (GPL), version 2
+ */
+
+#ifndef HVA_H
+#define HVA_H
+
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/videobuf2-v4l2.h>
+#include <media/v4l2-mem2mem.h>
+
+#define fh_to_ctx(f)    (container_of(f, struct hva_ctx, fh))
+
+#define hva_to_dev(h)   (h->dev)
+
+#define ctx_to_dev(c)   (c->hva_dev->dev)
+
+#define ctx_to_hdev(c)  (c->hva_dev)
+
+#define HVA_PREFIX "[---:----]"
+
+extern const struct hva_enc nv12h264enc;
+extern const struct hva_enc nv21h264enc;
+
+/**
+ * struct hva_frameinfo - information about hva frame
+ *
+ * @pixelformat:    fourcc code for uncompressed video format
+ * @width:          width of frame
+ * @height:         height of frame
+ * @aligned_width:  width of frame (with encoder alignment constraint)
+ * @aligned_height: height of frame (with encoder alignment constraint)
+ * @size:           maximum size in bytes required for data
+*/
+struct hva_frameinfo {
+	u32	pixelformat;
+	u32	width;
+	u32	height;
+	u32	aligned_width;
+	u32	aligned_height;
+	u32	size;
+};
+
+/**
+ * struct hva_streaminfo - information about hva stream
+ *
+ * @streamformat: fourcc code of compressed video format (H.264...)
+ * @width:        width of stream
+ * @height:       height of stream
+ * @profile:      profile string
+ * @level:        level string
+ */
+struct hva_streaminfo {
+	u32	streamformat;
+	u32	width;
+	u32	height;
+	u8	profile[32];
+	u8	level[32];
+};
+
+/**
+ * struct hva_controls - hva controls set
+ *
+ * @time_per_frame: time per frame in seconds
+ * @bitrate_mode:   bitrate mode (constant bitrate or variable bitrate)
+ * @gop_size:       groupe of picture size
+ * @bitrate:        bitrate (in bps)
+ * @aspect:         video aspect
+ * @profile:        H.264 profile
+ * @level:          H.264 level
+ * @entropy_mode:   H.264 entropy mode (CABAC or CVLC)
+ * @cpb_size:       coded picture buffer size (in kB)
+ * @dct8x8:         transform mode 8x8 enable
+ * @qpmin:          minimum quantizer
+ * @qpmax:          maximum quantizer
+ * @vui_sar:        pixel aspect ratio enable
+ * @vui_sar_idc:    pixel aspect ratio identifier
+ * @sei_fp:         sei frame packing arrangement enable
+ * @sei_fp_type:    sei frame packing arrangement type
+ */
+struct hva_controls {
+	struct v4l2_fract					time_per_frame;
+	enum v4l2_mpeg_video_bitrate_mode			bitrate_mode;
+	u32							gop_size;
+	u32							bitrate;
+	enum v4l2_mpeg_video_aspect				aspect;
+	enum v4l2_mpeg_video_h264_profile			profile;
+	enum v4l2_mpeg_video_h264_level				level;
+	enum v4l2_mpeg_video_h264_entropy_mode			entropy_mode;
+	u32							cpb_size;
+	bool							dct8x8;
+	u32							qpmin;
+	u32							qpmax;
+	bool							vui_sar;
+	enum v4l2_mpeg_video_h264_vui_sar_idc			vui_sar_idc;
+	bool							sei_fp;
+	enum v4l2_mpeg_video_h264_sei_fp_arrangement_type	sei_fp_type;
+};
+
+/**
+ * struct hva_frame - hva frame buffer (output)
+ *
+ * @vbuf:     video buffer information for V4L2
+ * @list:     V4L2 m2m list that the frame belongs to
+ * @info:     frame information (width, height, format, alignment...)
+ * @paddr:    physical address (for hardware)
+ * @vaddr:    virtual address (kernel can read/write)
+ * @prepared: true if vaddr/paddr are resolved
+ */
+struct hva_frame {
+	struct vb2_v4l2_buffer	vbuf;
+	struct list_head	list;
+	struct hva_frameinfo	info;
+	dma_addr_t		paddr;
+	void			*vaddr;
+	bool			prepared;
+};
+
+/*
+ * to_hva_frame() - cast struct vb2_v4l2_buffer * to struct hva_frame *
+ */
+#define to_hva_frame(vb) \
+	container_of(vb, struct hva_frame, vbuf)
+
+/**
+ * struct hva_stream - hva stream buffer (capture)
+ *
+ * @v4l2:       video buffer information for V4L2
+ * @list:       V4L2 m2m list that the frame belongs to
+ * @paddr:      physical address (for hardware)
+ * @vaddr:      virtual address (kernel can read/write)
+ * @prepared:   true if vaddr/paddr are resolved
+ * @size:       size of the buffer in bytes
+ * @bytesused:  number of bytes occupied by data in the buffer
+ */
+struct hva_stream {
+	struct vb2_v4l2_buffer	vbuf;
+	struct list_head	list;
+	dma_addr_t		paddr;
+	void			*vaddr;
+	bool			prepared;
+	unsigned int		size;
+	unsigned int		bytesused;
+};
+
+/*
+ * to_hva_stream() - cast struct vb2_v4l2_buffer * to struct hva_stream *
+ */
+#define to_hva_stream(vb) \
+	container_of(vb, struct hva_stream, vbuf)
+
+struct hva_dev;
+struct hva_enc;
+
+/**
+ * struct hva_ctx - context of hva instance
+ *
+ * @hva_dev:         the device that this instance is associated with
+ * @fh:              V4L2 file handle
+ * @ctrl_handler:    V4L2 controls handler
+ * @ctrls:           hva controls set
+ * @id:              instance identifier
+ * @aborting:        true if current job aborted
+ * @name:            instance name (debug purpose)
+ * @run_work:        encode work
+ * @lock:            mutex used to lock access of this context
+ * @flags:           validity of streaminfo and frameinfo fields
+ * @frame_num:       frame number
+ * @stream_num:      stream number
+ * @max_stream_size: maximum size in bytes required for stream data
+ * @colorspace:      colorspace identifier
+ * @xfer_func:       transfer function identifier
+ * @ycbcr_enc:       Y'CbCr encoding identifier
+ * @quantization:    quantization identifier
+ * @streaminfo:      stream properties
+ * @frameinfo:       frame properties
+ * @enc:             current encoder
+ * @priv:            private codec data for this instance, allocated
+ *                   by encoder @open time
+ * @hw_err:          true if hardware error detected
+ */
+struct hva_ctx {
+	struct hva_dev		        *hva_dev;
+	struct v4l2_fh			fh;
+	struct v4l2_ctrl_handler	ctrl_handler;
+	struct hva_controls		ctrls;
+	u8				id;
+	bool				aborting;
+	char				name[100];
+	struct work_struct		run_work;
+	/* mutex protecting this data structure */
+	struct mutex			lock;
+	u32				flags;
+	u32				frame_num;
+	u32				stream_num;
+	u32				max_stream_size;
+	enum v4l2_colorspace		colorspace;
+	enum v4l2_xfer_func		xfer_func;
+	enum v4l2_ycbcr_encoding	ycbcr_enc;
+	enum v4l2_quantization		quantization;
+	struct hva_streaminfo		streaminfo;
+	struct hva_frameinfo		frameinfo;
+	struct hva_enc			*enc;
+	void				*priv;
+	bool				hw_err;
+};
+
+#define HVA_FLAG_STREAMINFO	0x0001
+#define HVA_FLAG_FRAMEINFO	0x0002
+
+#define HVA_MAX_INSTANCES	16
+#define HVA_MAX_ENCODERS	10
+#define HVA_MAX_FORMATS		HVA_MAX_ENCODERS
+
+/**
+ * struct hva_dev - abstraction for hva entity
+ *
+ * @v4l2_dev:            V4L2 device
+ * @vdev:                video device
+ * @pdev:                platform device
+ * @dev:                 device
+ * @lock:                mutex used for critical sections & V4L2 ops
+ *                       serialization
+ * @m2m_dev:             memory-to-memory V4L2 device information
+ * @instances:           opened instances
+ * @nb_of_instances:     number of opened instances
+ * @instance_id:         rolling counter identifying an instance (debug purpose)
+ * @regs:                register io memory access
+ * @esram_addr:          esram address
+ * @esram_size:          esram size
+ * @clk:                 hva clock
+ * @irq_its:             status interruption
+ * @irq_err:             error interruption
+ * @work_queue:          work queue to handle the encode jobs
+ * @protect_mutex:       mutex used to lock access of hardware
+ * @interrupt:           completion interrupt
+ * @ip_version:          IP hardware version
+ * @encoders:            registered encoders
+ * @nb_of_encoders:      number of registered encoders
+ * @pixelformats:        supported uncompressed video formats
+ * @nb_of_pixelformats:  number of supported umcompressed video formats
+ * @streamformats:       supported compressed video formats
+ * @nb_of_streamformats: number of supported compressed video formats
+ * @sfl_reg:             status fifo level register value
+ * @sts_reg:             status register value
+ * @lmi_err_reg:         local memory interface error register value
+ * @emi_err_reg:         external memory interface error register value
+ * @hec_mif_err_reg:     HEC memory interface error register value
+ */
+struct hva_dev {
+	struct v4l2_device	v4l2_dev;
+	struct video_device	*vdev;
+	struct platform_device	*pdev;
+	struct device		*dev;
+	/* mutex protecting vb2_queue structure */
+	struct mutex		lock;
+	struct v4l2_m2m_dev	*m2m_dev;
+	struct hva_ctx		*instances[HVA_MAX_INSTANCES];
+	unsigned int		nb_of_instances;
+	unsigned int		instance_id;
+	void __iomem		*regs;
+	u32			esram_addr;
+	u32			esram_size;
+	struct clk		*clk;
+	int			irq_its;
+	int			irq_err;
+	struct workqueue_struct *work_queue;
+	/* mutex protecting hardware access */
+	struct mutex		protect_mutex;
+	struct completion	interrupt;
+	unsigned long int	ip_version;
+	const struct hva_enc	*encoders[HVA_MAX_ENCODERS];
+	u32			nb_of_encoders;
+	u32			pixelformats[HVA_MAX_FORMATS];
+	u32			nb_of_pixelformats;
+	u32			streamformats[HVA_MAX_FORMATS];
+	u32			nb_of_streamformats;
+	u32			sfl_reg;
+	u32			sts_reg;
+	u32			lmi_err_reg;
+	u32			emi_err_reg;
+	u32			hec_mif_err_reg;
+};
+
+/**
+ * struct hva_enc - hva encoder
+ *
+ * @name:         encoder name
+ * @streamformat: fourcc code for compressed video format (H.264...)
+ * @pixelformat:  fourcc code for uncompressed video format
+ * @max_width:    maximum width of frame for this encoder
+ * @max_height:   maximum height of frame for this encoder
+ * @open:         open encoder
+ * @close:        close encoder
+ * @encode:       encode a frame (struct hva_frame) in a stream
+ *                (struct hva_stream)
+ */
+
+struct hva_enc {
+	const char	*name;
+	u32		streamformat;
+	u32		pixelformat;
+	u32		max_width;
+	u32		max_height;
+	int		(*open)(struct hva_ctx *ctx);
+	int		(*close)(struct hva_ctx *ctx);
+	int		(*encode)(struct hva_ctx *ctx, struct hva_frame *frame,
+				  struct hva_stream *stream);
+};
+
+#endif /* HVA_H */
diff --git a/drivers/media/platform/ti-vpe/cal.c b/drivers/media/platform/ti-vpe/cal.c
index e967fcfdc1d8..44323cb5d287 100644
--- a/drivers/media/platform/ti-vpe/cal.c
+++ b/drivers/media/platform/ti-vpe/cal.c
@@ -1379,7 +1379,7 @@ static void cal_stop_streaming(struct vb2_queue *vq)
 	cal_runtime_put(ctx->dev);
 }
 
-static struct vb2_ops cal_video_qops = {
+static const struct vb2_ops cal_video_qops = {
 	.queue_setup		= cal_queue_setup,
 	.buf_prepare		= cal_buffer_prepare,
 	.buf_queue		= cal_buffer_queue,
diff --git a/drivers/media/platform/ti-vpe/vpe.c b/drivers/media/platform/ti-vpe/vpe.c
index 55a1458ac783..0189f7f7cb03 100644
--- a/drivers/media/platform/ti-vpe/vpe.c
+++ b/drivers/media/platform/ti-vpe/vpe.c
@@ -1878,7 +1878,7 @@ static void vpe_stop_streaming(struct vb2_queue *q)
 	vpdma_dump_regs(ctx->dev->vpdma);
 }
 
-static struct vb2_ops vpe_qops = {
+static const struct vb2_ops vpe_qops = {
 	.queue_setup	 = vpe_queue_setup,
 	.buf_prepare	 = vpe_buf_prepare,
 	.buf_queue	 = vpe_buf_queue,
diff --git a/drivers/media/platform/vim2m.c b/drivers/media/platform/vim2m.c
index cd0ff4a66fdc..a98f679bd88d 100644
--- a/drivers/media/platform/vim2m.c
+++ b/drivers/media/platform/vim2m.c
@@ -815,7 +815,7 @@ static void vim2m_stop_streaming(struct vb2_queue *q)
 	}
 }
 
-static struct vb2_ops vim2m_qops = {
+static const struct vb2_ops vim2m_qops = {
 	.queue_setup	 = vim2m_queue_setup,
 	.buf_prepare	 = vim2m_buf_prepare,
 	.buf_queue	 = vim2m_buf_queue,
diff --git a/drivers/media/platform/vivid/vivid-core.c b/drivers/media/platform/vivid/vivid-core.c
index 7f937136c3f5..5464fefbaab9 100644
--- a/drivers/media/platform/vivid/vivid-core.c
+++ b/drivers/media/platform/vivid/vivid-core.c
@@ -163,38 +163,38 @@ const struct v4l2_rect vivid_max_rect = {
 
 static const u8 vivid_hdmi_edid[256] = {
 	0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00,
-	0x63, 0x3a, 0xaa, 0x55, 0x00, 0x00, 0x00, 0x00,
-	0x0a, 0x18, 0x01, 0x03, 0x80, 0x10, 0x09, 0x78,
-	0x0e, 0x00, 0xb2, 0xa0, 0x57, 0x49, 0x9b, 0x26,
-	0x10, 0x48, 0x4f, 0x2f, 0xcf, 0x00, 0x31, 0x59,
+	0x31, 0xd8, 0x34, 0x12, 0x00, 0x00, 0x00, 0x00,
+	0x22, 0x1a, 0x01, 0x03, 0x80, 0x60, 0x36, 0x78,
+	0x0f, 0xee, 0x91, 0xa3, 0x54, 0x4c, 0x99, 0x26,
+	0x0f, 0x50, 0x54, 0x2f, 0xcf, 0x00, 0x31, 0x59,
 	0x45, 0x59, 0x81, 0x80, 0x81, 0x40, 0x90, 0x40,
-	0x95, 0x00, 0xa9, 0x40, 0xb3, 0x00, 0x02, 0x3a,
-	0x80, 0x18, 0x71, 0x38, 0x2d, 0x40, 0x58, 0x2c,
-	0x46, 0x00, 0x10, 0x09, 0x00, 0x00, 0x00, 0x1e,
+	0x95, 0x00, 0xa9, 0x40, 0xb3, 0x00, 0x08, 0xe8,
+	0x00, 0x30, 0xf2, 0x70, 0x5a, 0x80, 0xb0, 0x58,
+	0x8a, 0x00, 0xc0, 0x1c, 0x32, 0x00, 0x00, 0x1e,
 	0x00, 0x00, 0x00, 0xfd, 0x00, 0x18, 0x55, 0x18,
-	0x5e, 0x11, 0x00, 0x0a, 0x20, 0x20, 0x20, 0x20,
-	0x20, 0x20, 0x00, 0x00, 0x00, 0xfc, 0x00,  'v',
-	'4',   'l',  '2',  '-',  'h',  'd',  'm',  'i',
-	0x0a, 0x0a, 0x0a, 0x0a, 0x00, 0x00, 0x00, 0x10,
+	0x87, 0x3c, 0x00, 0x0a, 0x20, 0x20, 0x20, 0x20,
+	0x20, 0x20, 0x00, 0x00, 0x00, 0xfc, 0x00, 0x76,
+	0x69, 0x76, 0x69, 0x64, 0x0a, 0x20, 0x20, 0x20,
+	0x20, 0x20, 0x20, 0x20, 0x00, 0x00, 0x00, 0x10,
 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf0,
-
-	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
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x7b,
+
+	0x02, 0x03, 0x3f, 0xf0, 0x51, 0x61, 0x60, 0x5f,
+	0x5e, 0x5d, 0x10, 0x1f, 0x04, 0x13, 0x22, 0x21,
+	0x20, 0x05, 0x14, 0x02, 0x11, 0x01, 0x23, 0x09,
+	0x07, 0x07, 0x83, 0x01, 0x00, 0x00, 0x6d, 0x03,
+	0x0c, 0x00, 0x10, 0x00, 0x00, 0x78, 0x21, 0x00,
+	0x60, 0x01, 0x02, 0x03, 0x67, 0xd8, 0x5d, 0xc4,
+	0x01, 0x78, 0x00, 0x00, 0xe2, 0x00, 0xea, 0xe3,
+	0x05, 0x00, 0x00, 0xe3, 0x06, 0x01, 0x00, 0x4d,
+	0xd0, 0x00, 0xa0, 0xf0, 0x70, 0x3e, 0x80, 0x30,
+	0x20, 0x35, 0x00, 0xc0, 0x1c, 0x32, 0x00, 0x00,
+	0x1e, 0x1a, 0x36, 0x80, 0xa0, 0x70, 0x38, 0x1f,
+	0x40, 0x30, 0x20, 0x35, 0x00, 0xc0, 0x1c, 0x32,
+	0x00, 0x00, 0x1a, 0x1a, 0x1d, 0x00, 0x80, 0x51,
+	0xd0, 0x1c, 0x20, 0x40, 0x80, 0x35, 0x00, 0xc0,
+	0x1c, 0x32, 0x00, 0x00, 0x1c, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x27,
 };
 
 static int vidioc_querycap(struct file *file, void  *priv,
@@ -839,6 +839,7 @@ static int vivid_create_instance(struct platform_device *pdev, int inst)
 		dev->radio_tx_caps = V4L2_CAP_RDS_OUTPUT | V4L2_CAP_MODULATOR |
 				     V4L2_CAP_READWRITE;
 
+	ret = -ENOMEM;
 	/* initialize the test pattern generator */
 	tpg_init(&dev->tpg, 640, 360);
 	if (tpg_alloc(&dev->tpg, MAX_ZOOM * MAX_WIDTH))
@@ -1033,8 +1034,10 @@ static int vivid_create_instance(struct platform_device *pdev, int inst)
 	 */
 	dev->cec_workqueue =
 		alloc_ordered_workqueue("vivid-%03d-cec", WQ_MEM_RECLAIM, inst);
-	if (!dev->cec_workqueue)
+	if (!dev->cec_workqueue) {
+		ret = -ENOMEM;
 		goto unreg_dev;
+	}
 
 	/* start creating the vb2 queues */
 	if (dev->has_vid_cap) {
diff --git a/drivers/media/platform/vivid/vivid-ctrls.c b/drivers/media/platform/vivid/vivid-ctrls.c
index b98089c95ef5..aceb38d9f7e7 100644
--- a/drivers/media/platform/vivid/vivid-ctrls.c
+++ b/drivers/media/platform/vivid/vivid-ctrls.c
@@ -761,7 +761,7 @@ static const char * const vivid_ctrl_ycbcr_enc_strings[] = {
 	"Rec. 709",
 	"xvYCC 601",
 	"xvYCC 709",
-	"sYCC",
+	"",
 	"BT.2020",
 	"BT.2020 Constant Luminance",
 	"SMPTE 240M",
@@ -773,6 +773,7 @@ static const struct v4l2_ctrl_config vivid_ctrl_ycbcr_enc = {
 	.id = VIVID_CID_YCBCR_ENC,
 	.name = "Y'CbCr Encoding",
 	.type = V4L2_CTRL_TYPE_MENU,
+	.menu_skip_mask = 1 << 5,
 	.max = ARRAY_SIZE(vivid_ctrl_ycbcr_enc_strings) - 2,
 	.qmenu = vivid_ctrl_ycbcr_enc_strings,
 };
diff --git a/drivers/media/platform/vivid/vivid-vid-cap.c b/drivers/media/platform/vivid/vivid-vid-cap.c
index d404a7ce33a4..d5c84ecf2027 100644
--- a/drivers/media/platform/vivid/vivid-vid-cap.c
+++ b/drivers/media/platform/vivid/vivid-vid-cap.c
@@ -823,7 +823,7 @@ int vivid_vid_cap_g_selection(struct file *file, void *priv,
 	if (sel->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 		return -EINVAL;
 	if (vivid_is_webcam(dev))
-		return -EINVAL;
+		return -ENODATA;
 
 	sel->r.left = sel->r.top = 0;
 	switch (sel->target) {
@@ -872,7 +872,7 @@ int vivid_vid_cap_s_selection(struct file *file, void *fh, struct v4l2_selection
 	if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 		return -EINVAL;
 	if (vivid_is_webcam(dev))
-		return -EINVAL;
+		return -ENODATA;
 
 	switch (s->target) {
 	case V4L2_SEL_TGT_CROP:
diff --git a/drivers/media/platform/vsp1/vsp1.h b/drivers/media/platform/vsp1/vsp1.h
index 06a2ec7e5ad4..b23fa879a9aa 100644
--- a/drivers/media/platform/vsp1/vsp1.h
+++ b/drivers/media/platform/vsp1/vsp1.h
@@ -53,6 +53,7 @@ struct vsp1_uds;
 
 struct vsp1_device_info {
 	u32 version;
+	const char *model;
 	unsigned int gen;
 	unsigned int features;
 	unsigned int rpf_count;
@@ -65,6 +66,7 @@ struct vsp1_device_info {
 struct vsp1_device {
 	struct device *dev;
 	const struct vsp1_device_info *info;
+	u32 version;
 
 	void __iomem *mmio;
 	struct rcar_fcp_device *fcp;
diff --git a/drivers/media/platform/vsp1/vsp1_bru.c b/drivers/media/platform/vsp1/vsp1_bru.c
index 8268b87727a7..ee8355c28f94 100644
--- a/drivers/media/platform/vsp1/vsp1_bru.c
+++ b/drivers/media/platform/vsp1/vsp1_bru.c
@@ -142,10 +142,15 @@ static int bru_set_format(struct v4l2_subdev *subdev,
 	struct vsp1_bru *bru = to_bru(subdev);
 	struct v4l2_subdev_pad_config *config;
 	struct v4l2_mbus_framefmt *format;
+	int ret = 0;
+
+	mutex_lock(&bru->entity.lock);
 
 	config = vsp1_entity_get_pad_config(&bru->entity, cfg, fmt->which);
-	if (!config)
-		return -EINVAL;
+	if (!config) {
+		ret = -EINVAL;
+		goto done;
+	}
 
 	bru_try_format(bru, config, fmt->pad, &fmt->format);
 
@@ -174,7 +179,9 @@ static int bru_set_format(struct v4l2_subdev *subdev,
 		}
 	}
 
-	return 0;
+done:
+	mutex_unlock(&bru->entity.lock);
+	return ret;
 }
 
 static int bru_get_selection(struct v4l2_subdev *subdev,
@@ -201,7 +208,9 @@ static int bru_get_selection(struct v4l2_subdev *subdev,
 		if (!config)
 			return -EINVAL;
 
+		mutex_lock(&bru->entity.lock);
 		sel->r = *bru_get_compose(bru, config, sel->pad);
+		mutex_unlock(&bru->entity.lock);
 		return 0;
 
 	default:
@@ -217,6 +226,7 @@ static int bru_set_selection(struct v4l2_subdev *subdev,
 	struct v4l2_subdev_pad_config *config;
 	struct v4l2_mbus_framefmt *format;
 	struct v4l2_rect *compose;
+	int ret = 0;
 
 	if (sel->pad == bru->entity.source_pad)
 		return -EINVAL;
@@ -224,11 +234,16 @@ static int bru_set_selection(struct v4l2_subdev *subdev,
 	if (sel->target != V4L2_SEL_TGT_COMPOSE)
 		return -EINVAL;
 
+	mutex_lock(&bru->entity.lock);
+
 	config = vsp1_entity_get_pad_config(&bru->entity, cfg, sel->which);
-	if (!config)
-		return -EINVAL;
+	if (!config) {
+		ret = -EINVAL;
+		goto done;
+	}
 
-	/* The compose rectangle top left corner must be inside the output
+	/*
+	 * The compose rectangle top left corner must be inside the output
 	 * frame.
 	 */
 	format = vsp1_entity_get_pad_format(&bru->entity, config,
@@ -246,7 +261,9 @@ static int bru_set_selection(struct v4l2_subdev *subdev,
 	compose = bru_get_compose(bru, config, sel->pad);
 	*compose = sel->r;
 
-	return 0;
+done:
+	mutex_unlock(&bru->entity.lock);
+	return ret;
 }
 
 static const struct v4l2_subdev_pad_ops bru_pad_ops = {
@@ -269,14 +286,15 @@ static const struct v4l2_subdev_ops bru_ops = {
 
 static void bru_configure(struct vsp1_entity *entity,
 			  struct vsp1_pipeline *pipe,
-			  struct vsp1_dl_list *dl, bool full)
+			  struct vsp1_dl_list *dl,
+			  enum vsp1_entity_params params)
 {
 	struct vsp1_bru *bru = to_bru(&entity->subdev);
 	struct v4l2_mbus_framefmt *format;
 	unsigned int flags;
 	unsigned int i;
 
-	if (!full)
+	if (params != VSP1_ENTITY_PARAMS_INIT)
 		return;
 
 	format = vsp1_entity_get_pad_format(&bru->entity, bru->entity.config,
diff --git a/drivers/media/platform/vsp1/vsp1_clu.c b/drivers/media/platform/vsp1/vsp1_clu.c
index b63d2dbe5ea3..f2fb26e5ab4e 100644
--- a/drivers/media/platform/vsp1/vsp1_clu.c
+++ b/drivers/media/platform/vsp1/vsp1_clu.c
@@ -148,10 +148,15 @@ static int clu_set_format(struct v4l2_subdev *subdev,
 	struct vsp1_clu *clu = to_clu(subdev);
 	struct v4l2_subdev_pad_config *config;
 	struct v4l2_mbus_framefmt *format;
+	int ret = 0;
+
+	mutex_lock(&clu->entity.lock);
 
 	config = vsp1_entity_get_pad_config(&clu->entity, cfg, fmt->which);
-	if (!config)
-		return -EINVAL;
+	if (!config) {
+		ret = -EINVAL;
+		goto done;
+	}
 
 	/* Default to YUV if the requested format is not supported. */
 	if (fmt->format.code != MEDIA_BUS_FMT_ARGB8888_1X32 &&
@@ -164,7 +169,7 @@ static int clu_set_format(struct v4l2_subdev *subdev,
 	if (fmt->pad == CLU_PAD_SOURCE) {
 		/* The CLU output format can't be modified. */
 		fmt->format = *format;
-		return 0;
+		goto done;
 	}
 
 	format->code = fmt->format.code;
@@ -182,7 +187,9 @@ static int clu_set_format(struct v4l2_subdev *subdev,
 					    CLU_PAD_SOURCE);
 	*format = fmt->format;
 
-	return 0;
+done:
+	mutex_unlock(&clu->entity.lock);
+	return ret;
 }
 
 /* -----------------------------------------------------------------------------
@@ -207,42 +214,51 @@ static const struct v4l2_subdev_ops clu_ops = {
 
 static void clu_configure(struct vsp1_entity *entity,
 			  struct vsp1_pipeline *pipe,
-			  struct vsp1_dl_list *dl, bool full)
+			  struct vsp1_dl_list *dl,
+			  enum vsp1_entity_params params)
 {
 	struct vsp1_clu *clu = to_clu(&entity->subdev);
 	struct vsp1_dl_body *dlb;
 	unsigned long flags;
 	u32 ctrl = VI6_CLU_CTRL_AAI | VI6_CLU_CTRL_MVS | VI6_CLU_CTRL_EN;
 
-	/* The format can't be changed during streaming, only verify it at
-	 * stream start and store the information internally for future partial
-	 * reconfiguration calls.
-	 */
-	if (full) {
+	switch (params) {
+	case VSP1_ENTITY_PARAMS_INIT: {
+		/*
+		 * The format can't be changed during streaming, only verify it
+		 * at setup time and store the information internally for future
+		 * runtime configuration calls.
+		 */
 		struct v4l2_mbus_framefmt *format;
 
 		format = vsp1_entity_get_pad_format(&clu->entity,
 						    clu->entity.config,
 						    CLU_PAD_SINK);
 		clu->yuv_mode = format->code == MEDIA_BUS_FMT_AYUV8_1X32;
-		return;
+		break;
 	}
 
-	/* 2D mode can only be used with the YCbCr pixel encoding. */
-	if (clu->mode == V4L2_CID_VSP1_CLU_MODE_2D && clu->yuv_mode)
-		ctrl |= VI6_CLU_CTRL_AX1I_2D | VI6_CLU_CTRL_AX2I_2D
-		     |  VI6_CLU_CTRL_OS0_2D | VI6_CLU_CTRL_OS1_2D
-		     |  VI6_CLU_CTRL_OS2_2D | VI6_CLU_CTRL_M2D;
+	case VSP1_ENTITY_PARAMS_PARTITION:
+		break;
+
+	case VSP1_ENTITY_PARAMS_RUNTIME:
+		/* 2D mode can only be used with the YCbCr pixel encoding. */
+		if (clu->mode == V4L2_CID_VSP1_CLU_MODE_2D && clu->yuv_mode)
+			ctrl |= VI6_CLU_CTRL_AX1I_2D | VI6_CLU_CTRL_AX2I_2D
+			     |  VI6_CLU_CTRL_OS0_2D | VI6_CLU_CTRL_OS1_2D
+			     |  VI6_CLU_CTRL_OS2_2D | VI6_CLU_CTRL_M2D;
 
-	vsp1_clu_write(clu, dl, VI6_CLU_CTRL, ctrl);
+		vsp1_clu_write(clu, dl, VI6_CLU_CTRL, ctrl);
 
-	spin_lock_irqsave(&clu->lock, flags);
-	dlb = clu->clu;
-	clu->clu = NULL;
-	spin_unlock_irqrestore(&clu->lock, flags);
+		spin_lock_irqsave(&clu->lock, flags);
+		dlb = clu->clu;
+		clu->clu = NULL;
+		spin_unlock_irqrestore(&clu->lock, flags);
 
-	if (dlb)
-		vsp1_dl_list_add_fragment(dl, dlb);
+		if (dlb)
+			vsp1_dl_list_add_fragment(dl, dlb);
+		break;
+	}
 }
 
 static const struct vsp1_entity_operations clu_entity_ops = {
diff --git a/drivers/media/platform/vsp1/vsp1_dl.c b/drivers/media/platform/vsp1/vsp1_dl.c
index 37c3518aa2a8..ad545aff4e35 100644
--- a/drivers/media/platform/vsp1/vsp1_dl.c
+++ b/drivers/media/platform/vsp1/vsp1_dl.c
@@ -21,7 +21,6 @@
 #include "vsp1_dl.h"
 
 #define VSP1_DL_NUM_ENTRIES		256
-#define VSP1_DL_NUM_LISTS		3
 
 #define VSP1_DLH_INT_ENABLE		(1 << 1)
 #define VSP1_DLH_AUTO_START		(1 << 0)
@@ -71,6 +70,7 @@ struct vsp1_dl_body {
  * @dma: DMA address for the header
  * @body0: first display list body
  * @fragments: list of extra display list bodies
+ * @chain: entry in the display list partition chain
  */
 struct vsp1_dl_list {
 	struct list_head list;
@@ -81,6 +81,9 @@ struct vsp1_dl_list {
 
 	struct vsp1_dl_body body0;
 	struct list_head fragments;
+
+	bool has_chain;
+	struct list_head chain;
 };
 
 enum vsp1_dl_mode {
@@ -262,7 +265,6 @@ static struct vsp1_dl_list *vsp1_dl_list_alloc(struct vsp1_dl_manager *dlm)
 
 		memset(dl->header, 0, sizeof(*dl->header));
 		dl->header->lists[0].addr = dl->body0.dma;
-		dl->header->flags = VSP1_DLH_INT_ENABLE;
 	}
 
 	return dl;
@@ -293,6 +295,12 @@ struct vsp1_dl_list *vsp1_dl_list_get(struct vsp1_dl_manager *dlm)
 	if (!list_empty(&dlm->free)) {
 		dl = list_first_entry(&dlm->free, struct vsp1_dl_list, list);
 		list_del(&dl->list);
+
+		/*
+		 * The display list chain must be initialised to ensure every
+		 * display list can assert list_empty() if it is not in a chain.
+		 */
+		INIT_LIST_HEAD(&dl->chain);
 	}
 
 	spin_unlock_irqrestore(&dlm->lock, flags);
@@ -303,10 +311,24 @@ struct vsp1_dl_list *vsp1_dl_list_get(struct vsp1_dl_manager *dlm)
 /* This function must be called with the display list manager lock held.*/
 static void __vsp1_dl_list_put(struct vsp1_dl_list *dl)
 {
+	struct vsp1_dl_list *dl_child;
+
 	if (!dl)
 		return;
 
-	/* We can't free fragments here as DMA memory can only be freed in
+	/*
+	 * Release any linked display-lists which were chained for a single
+	 * hardware operation.
+	 */
+	if (dl->has_chain) {
+		list_for_each_entry(dl_child, &dl->chain, chain)
+			__vsp1_dl_list_put(dl_child);
+	}
+
+	dl->has_chain = false;
+
+	/*
+	 * We can't free fragments here as DMA memory can only be freed in
 	 * interruptible context. Move all fragments to the display list
 	 * manager's list of fragments to be freed, they will be
 	 * garbage-collected by the work queue.
@@ -383,6 +405,76 @@ int vsp1_dl_list_add_fragment(struct vsp1_dl_list *dl,
 	return 0;
 }
 
+/**
+ * vsp1_dl_list_add_chain - Add a display list to a chain
+ * @head: The head display list
+ * @dl: The new display list
+ *
+ * Add a display list to an existing display list chain. The chained lists
+ * will be automatically processed by the hardware without intervention from
+ * the CPU. A display list end interrupt will only complete after the last
+ * display list in the chain has completed processing.
+ *
+ * Adding a display list to a chain passes ownership of the display list to
+ * the head display list item. The chain is released when the head dl item is
+ * put back with __vsp1_dl_list_put().
+ *
+ * Chained display lists are only usable in header mode. Attempts to add a
+ * display list to a chain in header-less mode will return an error.
+ */
+int vsp1_dl_list_add_chain(struct vsp1_dl_list *head,
+			   struct vsp1_dl_list *dl)
+{
+	/* Chained lists are only available in header mode. */
+	if (head->dlm->mode != VSP1_DL_MODE_HEADER)
+		return -EINVAL;
+
+	head->has_chain = true;
+	list_add_tail(&dl->chain, &head->chain);
+	return 0;
+}
+
+static void vsp1_dl_list_fill_header(struct vsp1_dl_list *dl, bool is_last)
+{
+	struct vsp1_dl_header_list *hdr = dl->header->lists;
+	struct vsp1_dl_body *dlb;
+	unsigned int num_lists = 0;
+
+	/*
+	 * Fill the header with the display list bodies addresses and sizes. The
+	 * address of the first body has already been filled when the display
+	 * list was allocated.
+	 */
+
+	hdr->num_bytes = dl->body0.num_entries
+		       * sizeof(*dl->header->lists);
+
+	list_for_each_entry(dlb, &dl->fragments, list) {
+		num_lists++;
+		hdr++;
+
+		hdr->addr = dlb->dma;
+		hdr->num_bytes = dlb->num_entries
+			       * sizeof(*dl->header->lists);
+	}
+
+	dl->header->num_lists = num_lists;
+
+	/*
+	 * If this display list's chain is not empty, we are on a list, where
+	 * the next item in the list is the display list entity which should be
+	 * automatically queued by the hardware.
+	 */
+	if (!list_empty(&dl->chain) && !is_last) {
+		struct vsp1_dl_list *next = list_next_entry(dl, chain);
+
+		dl->header->next_header = next->dma;
+		dl->header->flags = VSP1_DLH_AUTO_START;
+	} else {
+		dl->header->flags = VSP1_DLH_INT_ENABLE;
+	}
+}
+
 void vsp1_dl_list_commit(struct vsp1_dl_list *dl)
 {
 	struct vsp1_dl_manager *dlm = dl->dlm;
@@ -393,30 +485,26 @@ void vsp1_dl_list_commit(struct vsp1_dl_list *dl)
 	spin_lock_irqsave(&dlm->lock, flags);
 
 	if (dl->dlm->mode == VSP1_DL_MODE_HEADER) {
-		struct vsp1_dl_header_list *hdr = dl->header->lists;
-		struct vsp1_dl_body *dlb;
-		unsigned int num_lists = 0;
+		struct vsp1_dl_list *dl_child;
 
-		/* Fill the header with the display list bodies addresses and
-		 * sizes. The address of the first body has already been filled
-		 * when the display list was allocated.
-		 *
+		/*
 		 * In header mode the caller guarantees that the hardware is
 		 * idle at this point.
 		 */
-		hdr->num_bytes = dl->body0.num_entries
-			       * sizeof(*dl->header->lists);
 
-		list_for_each_entry(dlb, &dl->fragments, list) {
-			num_lists++;
-			hdr++;
+		/* Fill the header for the head and chained display lists. */
+		vsp1_dl_list_fill_header(dl, list_empty(&dl->chain));
 
-			hdr->addr = dlb->dma;
-			hdr->num_bytes = dlb->num_entries
-				       * sizeof(*dl->header->lists);
+		list_for_each_entry(dl_child, &dl->chain, chain) {
+			bool last = list_is_last(&dl_child->chain, &dl->chain);
+
+			vsp1_dl_list_fill_header(dl_child, last);
 		}
 
-		dl->header->num_lists = num_lists;
+		/*
+		 * Commit the head display list to hardware. Chained headers
+		 * will auto-start.
+		 */
 		vsp1_write(vsp1, VI6_DL_HDR_ADDR(dlm->index), dl->dma);
 
 		dlm->active = dl;
diff --git a/drivers/media/platform/vsp1/vsp1_dl.h b/drivers/media/platform/vsp1/vsp1_dl.h
index de387cd4d745..7131aa3c5978 100644
--- a/drivers/media/platform/vsp1/vsp1_dl.h
+++ b/drivers/media/platform/vsp1/vsp1_dl.h
@@ -41,5 +41,6 @@ void vsp1_dl_fragment_free(struct vsp1_dl_body *dlb);
 void vsp1_dl_fragment_write(struct vsp1_dl_body *dlb, u32 reg, u32 data);
 int vsp1_dl_list_add_fragment(struct vsp1_dl_list *dl,
 			      struct vsp1_dl_body *dlb);
+int vsp1_dl_list_add_chain(struct vsp1_dl_list *head, struct vsp1_dl_list *dl);
 
 #endif /* __VSP1_DL_H__ */
diff --git a/drivers/media/platform/vsp1/vsp1_drm.c b/drivers/media/platform/vsp1/vsp1_drm.c
index fe9665e57b3b..cd209dccff1b 100644
--- a/drivers/media/platform/vsp1/vsp1_drm.c
+++ b/drivers/media/platform/vsp1/vsp1_drm.c
@@ -276,17 +276,18 @@ int vsp1_du_atomic_update(struct device *dev, unsigned int rpf_index,
 	}
 
 	dev_dbg(vsp1->dev,
-		"%s: RPF%u: (%u,%u)/%ux%u -> (%u,%u)/%ux%u (%08x), pitch %u dma { %pad, %pad } zpos %u\n",
+		"%s: RPF%u: (%u,%u)/%ux%u -> (%u,%u)/%ux%u (%08x), pitch %u dma { %pad, %pad, %pad } zpos %u\n",
 		__func__, rpf_index,
 		cfg->src.left, cfg->src.top, cfg->src.width, cfg->src.height,
 		cfg->dst.left, cfg->dst.top, cfg->dst.width, cfg->dst.height,
 		cfg->pixelformat, cfg->pitch, &cfg->mem[0], &cfg->mem[1],
-		cfg->zpos);
+		&cfg->mem[2], cfg->zpos);
 
-	/* Store the format, stride, memory buffer address, crop and compose
+	/*
+	 * Store the format, stride, memory buffer address, crop and compose
 	 * rectangles and Z-order position and for the input.
 	 */
-	fmtinfo = vsp1_get_format_info(cfg->pixelformat);
+	fmtinfo = vsp1_get_format_info(vsp1, cfg->pixelformat);
 	if (!fmtinfo) {
 		dev_dbg(vsp1->dev, "Unsupport pixel format %08x for RPF\n",
 			cfg->pixelformat);
@@ -301,7 +302,7 @@ int vsp1_du_atomic_update(struct device *dev, unsigned int rpf_index,
 
 	rpf->mem.addr[0] = cfg->mem[0];
 	rpf->mem.addr[1] = cfg->mem[1];
-	rpf->mem.addr[2] = 0;
+	rpf->mem.addr[2] = cfg->mem[2];
 
 	vsp1->drm->inputs[rpf_index].crop = cfg->src;
 	vsp1->drm->inputs[rpf_index].compose = cfg->dst;
@@ -492,16 +493,13 @@ void vsp1_du_atomic_flush(struct device *dev)
 		vsp1_entity_route_setup(entity, pipe->dl);
 
 		if (entity->ops->configure) {
-			entity->ops->configure(entity, pipe, pipe->dl, true);
-			entity->ops->configure(entity, pipe, pipe->dl, false);
+			entity->ops->configure(entity, pipe, pipe->dl,
+					       VSP1_ENTITY_PARAMS_INIT);
+			entity->ops->configure(entity, pipe, pipe->dl,
+					       VSP1_ENTITY_PARAMS_RUNTIME);
+			entity->ops->configure(entity, pipe, pipe->dl,
+					       VSP1_ENTITY_PARAMS_PARTITION);
 		}
-
-		/* The memory buffer address must be applied after configuring
-		 * the RPF to make sure the crop offset are computed.
-		 */
-		if (entity->type == VSP1_ENTITY_RPF)
-			vsp1_rwpf_set_memory(to_rwpf(&entity->subdev),
-					     pipe->dl);
 	}
 
 	vsp1_dl_list_commit(pipe->dl);
diff --git a/drivers/media/platform/vsp1/vsp1_drv.c b/drivers/media/platform/vsp1/vsp1_drv.c
index cc316d281687..57c713a4e1df 100644
--- a/drivers/media/platform/vsp1/vsp1_drv.c
+++ b/drivers/media/platform/vsp1/vsp1_drv.c
@@ -60,7 +60,7 @@ static irqreturn_t vsp1_irq_handler(int irq, void *data)
 		status = vsp1_read(vsp1, VI6_WPF_IRQ_STA(i));
 		vsp1_write(vsp1, VI6_WPF_IRQ_STA(i), ~status & mask);
 
-		if (status & VI6_WFP_IRQ_STA_FRE) {
+		if (status & VI6_WFP_IRQ_STA_DFE) {
 			vsp1_pipeline_frame_end(wpf->pipe);
 			ret = IRQ_HANDLED;
 		}
@@ -220,7 +220,8 @@ static int vsp1_create_entities(struct vsp1_device *vsp1)
 	int ret;
 
 	mdev->dev = vsp1->dev;
-	strlcpy(mdev->model, "VSP1", sizeof(mdev->model));
+	mdev->hw_revision = vsp1->version;
+	strlcpy(mdev->model, vsp1->info->model, sizeof(mdev->model));
 	snprintf(mdev->bus_info, sizeof(mdev->bus_info), "platform:%s",
 		 dev_name(mdev->dev));
 	media_device_init(mdev);
@@ -559,6 +560,7 @@ static const struct dev_pm_ops vsp1_pm_ops = {
 static const struct vsp1_device_info vsp1_device_infos[] = {
 	{
 		.version = VI6_IP_VERSION_MODEL_VSPS_H2,
+		.model = "VSP1-S",
 		.gen = 2,
 		.features = VSP1_HAS_BRU | VSP1_HAS_CLU | VSP1_HAS_LUT
 			  | VSP1_HAS_SRU | VSP1_HAS_WPF_VFLIP,
@@ -569,6 +571,7 @@ static const struct vsp1_device_info vsp1_device_infos[] = {
 		.uapi = true,
 	}, {
 		.version = VI6_IP_VERSION_MODEL_VSPR_H2,
+		.model = "VSP1-R",
 		.gen = 2,
 		.features = VSP1_HAS_BRU | VSP1_HAS_SRU | VSP1_HAS_WPF_VFLIP,
 		.rpf_count = 5,
@@ -578,6 +581,7 @@ static const struct vsp1_device_info vsp1_device_infos[] = {
 		.uapi = true,
 	}, {
 		.version = VI6_IP_VERSION_MODEL_VSPD_GEN2,
+		.model = "VSP1-D",
 		.gen = 2,
 		.features = VSP1_HAS_BRU | VSP1_HAS_LIF | VSP1_HAS_LUT,
 		.rpf_count = 4,
@@ -587,6 +591,7 @@ static const struct vsp1_device_info vsp1_device_infos[] = {
 		.uapi = true,
 	}, {
 		.version = VI6_IP_VERSION_MODEL_VSPS_M2,
+		.model = "VSP1-S",
 		.gen = 2,
 		.features = VSP1_HAS_BRU | VSP1_HAS_CLU | VSP1_HAS_LUT
 			  | VSP1_HAS_SRU | VSP1_HAS_WPF_VFLIP,
@@ -596,7 +601,30 @@ static const struct vsp1_device_info vsp1_device_infos[] = {
 		.num_bru_inputs = 4,
 		.uapi = true,
 	}, {
+		.version = VI6_IP_VERSION_MODEL_VSPS_V2H,
+		.model = "VSP1V-S",
+		.gen = 2,
+		.features = VSP1_HAS_BRU | VSP1_HAS_CLU | VSP1_HAS_LUT
+			  | VSP1_HAS_SRU | VSP1_HAS_WPF_VFLIP,
+		.rpf_count = 4,
+		.uds_count = 1,
+		.wpf_count = 4,
+		.num_bru_inputs = 4,
+		.uapi = true,
+	}, {
+		.version = VI6_IP_VERSION_MODEL_VSPD_V2H,
+		.model = "VSP1V-D",
+		.gen = 2,
+		.features = VSP1_HAS_BRU | VSP1_HAS_CLU | VSP1_HAS_LUT
+			  | VSP1_HAS_LIF,
+		.rpf_count = 4,
+		.uds_count = 1,
+		.wpf_count = 1,
+		.num_bru_inputs = 4,
+		.uapi = true,
+	}, {
 		.version = VI6_IP_VERSION_MODEL_VSPI_GEN3,
+		.model = "VSP2-I",
 		.gen = 3,
 		.features = VSP1_HAS_CLU | VSP1_HAS_LUT | VSP1_HAS_SRU
 			  | VSP1_HAS_WPF_HFLIP | VSP1_HAS_WPF_VFLIP,
@@ -606,6 +634,7 @@ static const struct vsp1_device_info vsp1_device_infos[] = {
 		.uapi = true,
 	}, {
 		.version = VI6_IP_VERSION_MODEL_VSPBD_GEN3,
+		.model = "VSP2-BD",
 		.gen = 3,
 		.features = VSP1_HAS_BRU | VSP1_HAS_WPF_VFLIP,
 		.rpf_count = 5,
@@ -614,6 +643,7 @@ static const struct vsp1_device_info vsp1_device_infos[] = {
 		.uapi = true,
 	}, {
 		.version = VI6_IP_VERSION_MODEL_VSPBC_GEN3,
+		.model = "VSP2-BC",
 		.gen = 3,
 		.features = VSP1_HAS_BRU | VSP1_HAS_CLU | VSP1_HAS_LUT
 			  | VSP1_HAS_WPF_VFLIP,
@@ -623,6 +653,7 @@ static const struct vsp1_device_info vsp1_device_infos[] = {
 		.uapi = true,
 	}, {
 		.version = VI6_IP_VERSION_MODEL_VSPD_GEN3,
+		.model = "VSP2-D",
 		.gen = 3,
 		.features = VSP1_HAS_BRU | VSP1_HAS_LIF | VSP1_HAS_WPF_VFLIP,
 		.rpf_count = 5,
@@ -638,7 +669,6 @@ static int vsp1_probe(struct platform_device *pdev)
 	struct resource *irq;
 	struct resource *io;
 	unsigned int i;
-	u32 version;
 	int ret;
 
 	vsp1 = devm_kzalloc(&pdev->dev, sizeof(*vsp1), GFP_KERNEL);
@@ -689,11 +719,11 @@ static int vsp1_probe(struct platform_device *pdev)
 	if (ret < 0)
 		goto done;
 
-	version = vsp1_read(vsp1, VI6_IP_VERSION);
+	vsp1->version = vsp1_read(vsp1, VI6_IP_VERSION);
 	pm_runtime_put_sync(&pdev->dev);
 
 	for (i = 0; i < ARRAY_SIZE(vsp1_device_infos); ++i) {
-		if ((version & VI6_IP_VERSION_MODEL_MASK) ==
+		if ((vsp1->version & VI6_IP_VERSION_MODEL_MASK) ==
 		    vsp1_device_infos[i].version) {
 			vsp1->info = &vsp1_device_infos[i];
 			break;
@@ -701,12 +731,13 @@ static int vsp1_probe(struct platform_device *pdev)
 	}
 
 	if (!vsp1->info) {
-		dev_err(&pdev->dev, "unsupported IP version 0x%08x\n", version);
+		dev_err(&pdev->dev, "unsupported IP version 0x%08x\n",
+			vsp1->version);
 		ret = -ENXIO;
 		goto done;
 	}
 
-	dev_dbg(&pdev->dev, "IP version 0x%08x\n", version);
+	dev_dbg(&pdev->dev, "IP version 0x%08x\n", vsp1->version);
 
 	/* Instanciate entities */
 	ret = vsp1_create_entities(vsp1);
diff --git a/drivers/media/platform/vsp1/vsp1_entity.c b/drivers/media/platform/vsp1/vsp1_entity.c
index 4cf6cc719c00..da673495c222 100644
--- a/drivers/media/platform/vsp1/vsp1_entity.c
+++ b/drivers/media/platform/vsp1/vsp1_entity.c
@@ -51,6 +51,9 @@ void vsp1_entity_route_setup(struct vsp1_entity *source,
  * @cfg: the TRY pad configuration
  * @which: configuration selector (ACTIVE or TRY)
  *
+ * When called with which set to V4L2_SUBDEV_FORMAT_ACTIVE the caller must hold
+ * the entity lock to access the returned configuration.
+ *
  * Return the pad configuration requested by the which argument. The TRY
  * configuration is passed explicitly to the function through the cfg argument
  * and simply returned when requested. The ACTIVE configuration comes from the
@@ -160,7 +163,9 @@ int vsp1_subdev_get_pad_format(struct v4l2_subdev *subdev,
 	if (!config)
 		return -EINVAL;
 
+	mutex_lock(&entity->lock);
 	fmt->format = *vsp1_entity_get_pad_format(entity, config, fmt->pad);
+	mutex_unlock(&entity->lock);
 
 	return 0;
 }
@@ -204,8 +209,10 @@ int vsp1_subdev_enum_mbus_code(struct v4l2_subdev *subdev,
 		if (!config)
 			return -EINVAL;
 
+		mutex_lock(&entity->lock);
 		format = vsp1_entity_get_pad_format(entity, config, 0);
 		code->code = format->code;
+		mutex_unlock(&entity->lock);
 	}
 
 	return 0;
@@ -235,6 +242,7 @@ int vsp1_subdev_enum_frame_size(struct v4l2_subdev *subdev,
 	struct vsp1_entity *entity = to_vsp1_entity(subdev);
 	struct v4l2_subdev_pad_config *config;
 	struct v4l2_mbus_framefmt *format;
+	int ret = 0;
 
 	config = vsp1_entity_get_pad_config(entity, cfg, fse->which);
 	if (!config)
@@ -242,8 +250,12 @@ int vsp1_subdev_enum_frame_size(struct v4l2_subdev *subdev,
 
 	format = vsp1_entity_get_pad_format(entity, config, fse->pad);
 
-	if (fse->index || fse->code != format->code)
-		return -EINVAL;
+	mutex_lock(&entity->lock);
+
+	if (fse->index || fse->code != format->code) {
+		ret = -EINVAL;
+		goto done;
+	}
 
 	if (fse->pad == 0) {
 		fse->min_width = min_width;
@@ -260,7 +272,9 @@ int vsp1_subdev_enum_frame_size(struct v4l2_subdev *subdev,
 		fse->max_height = format->height;
 	}
 
-	return 0;
+done:
+	mutex_unlock(&entity->lock);
+	return ret;
 }
 
 /* -----------------------------------------------------------------------------
@@ -358,6 +372,8 @@ int vsp1_entity_init(struct vsp1_device *vsp1, struct vsp1_entity *entity,
 	if (i == ARRAY_SIZE(vsp1_routes))
 		return -EINVAL;
 
+	mutex_init(&entity->lock);
+
 	entity->vsp1 = vsp1;
 	entity->source_pad = num_pads - 1;
 
diff --git a/drivers/media/platform/vsp1/vsp1_entity.h b/drivers/media/platform/vsp1/vsp1_entity.h
index b43457fd2c43..901146f807b9 100644
--- a/drivers/media/platform/vsp1/vsp1_entity.h
+++ b/drivers/media/platform/vsp1/vsp1_entity.h
@@ -14,7 +14,7 @@
 #define __VSP1_ENTITY_H__
 
 #include <linux/list.h>
-#include <linux/spinlock.h>
+#include <linux/mutex.h>
 
 #include <media/v4l2-subdev.h>
 
@@ -35,6 +35,18 @@ enum vsp1_entity_type {
 	VSP1_ENTITY_WPF,
 };
 
+/**
+ * enum vsp1_entity_params - Entity configuration parameters class
+ * @VSP1_ENTITY_PARAMS_INIT - Initial parameters
+ * @VSP1_ENTITY_PARAMS_PARTITION - Per-image partition parameters
+ * @VSP1_ENTITY_PARAMS_RUNTIME - Runtime-configurable parameters
+ */
+enum vsp1_entity_params {
+	VSP1_ENTITY_PARAMS_INIT,
+	VSP1_ENTITY_PARAMS_PARTITION,
+	VSP1_ENTITY_PARAMS_RUNTIME,
+};
+
 #define VSP1_ENTITY_MAX_INPUTS		5	/* For the BRU */
 
 /*
@@ -63,17 +75,16 @@ struct vsp1_route {
 /**
  * struct vsp1_entity_operations - Entity operations
  * @destroy:	Destroy the entity.
- * @set_memory:	Setup memory buffer access. This operation applies the settings
- *		stored in the rwpf mem field to the display list. Valid for RPF
- *		and WPF only.
  * @configure:	Setup the hardware based on the entity state (pipeline, formats,
  *		selection rectangles, ...)
+ * @max_width:	Return the max supported width of data that the entity can
+ *		process in a single operation.
  */
 struct vsp1_entity_operations {
 	void (*destroy)(struct vsp1_entity *);
-	void (*set_memory)(struct vsp1_entity *, struct vsp1_dl_list *dl);
 	void (*configure)(struct vsp1_entity *, struct vsp1_pipeline *,
-			  struct vsp1_dl_list *, bool);
+			  struct vsp1_dl_list *, enum vsp1_entity_params);
+	unsigned int (*max_width)(struct vsp1_entity *, struct vsp1_pipeline *);
 };
 
 struct vsp1_entity {
@@ -96,6 +107,8 @@ struct vsp1_entity {
 
 	struct v4l2_subdev subdev;
 	struct v4l2_subdev_pad_config *config;
+
+	struct mutex lock;	/* Protects the pad config */
 };
 
 static inline struct vsp1_entity *to_vsp1_entity(struct v4l2_subdev *subdev)
diff --git a/drivers/media/platform/vsp1/vsp1_hsit.c b/drivers/media/platform/vsp1/vsp1_hsit.c
index 6e5077beb38c..94316afc54ff 100644
--- a/drivers/media/platform/vsp1/vsp1_hsit.c
+++ b/drivers/media/platform/vsp1/vsp1_hsit.c
@@ -71,10 +71,15 @@ static int hsit_set_format(struct v4l2_subdev *subdev,
 	struct vsp1_hsit *hsit = to_hsit(subdev);
 	struct v4l2_subdev_pad_config *config;
 	struct v4l2_mbus_framefmt *format;
+	int ret = 0;
+
+	mutex_lock(&hsit->entity.lock);
 
 	config = vsp1_entity_get_pad_config(&hsit->entity, cfg, fmt->which);
-	if (!config)
-		return -EINVAL;
+	if (!config) {
+		ret = -EINVAL;
+		goto done;
+	}
 
 	format = vsp1_entity_get_pad_format(&hsit->entity, config, fmt->pad);
 
@@ -83,7 +88,7 @@ static int hsit_set_format(struct v4l2_subdev *subdev,
 		 * modified.
 		 */
 		fmt->format = *format;
-		return 0;
+		goto done;
 	}
 
 	format->code = hsit->inverse ? MEDIA_BUS_FMT_AHSV8888_1X32
@@ -104,7 +109,9 @@ static int hsit_set_format(struct v4l2_subdev *subdev,
 	format->code = hsit->inverse ? MEDIA_BUS_FMT_ARGB8888_1X32
 		     : MEDIA_BUS_FMT_AHSV8888_1X32;
 
-	return 0;
+done:
+	mutex_unlock(&hsit->entity.lock);
+	return ret;
 }
 
 static const struct v4l2_subdev_pad_ops hsit_pad_ops = {
@@ -125,11 +132,12 @@ static const struct v4l2_subdev_ops hsit_ops = {
 
 static void hsit_configure(struct vsp1_entity *entity,
 			   struct vsp1_pipeline *pipe,
-			   struct vsp1_dl_list *dl, bool full)
+			   struct vsp1_dl_list *dl,
+			   enum vsp1_entity_params params)
 {
 	struct vsp1_hsit *hsit = to_hsit(&entity->subdev);
 
-	if (!full)
+	if (params != VSP1_ENTITY_PARAMS_INIT)
 		return;
 
 	if (hsit->inverse)
diff --git a/drivers/media/platform/vsp1/vsp1_lif.c b/drivers/media/platform/vsp1/vsp1_lif.c
index a720063f38c5..e32acae1fc6e 100644
--- a/drivers/media/platform/vsp1/vsp1_lif.c
+++ b/drivers/media/platform/vsp1/vsp1_lif.c
@@ -66,10 +66,15 @@ static int lif_set_format(struct v4l2_subdev *subdev,
 	struct vsp1_lif *lif = to_lif(subdev);
 	struct v4l2_subdev_pad_config *config;
 	struct v4l2_mbus_framefmt *format;
+	int ret = 0;
+
+	mutex_lock(&lif->entity.lock);
 
 	config = vsp1_entity_get_pad_config(&lif->entity, cfg, fmt->which);
-	if (!config)
-		return -EINVAL;
+	if (!config) {
+		ret = -EINVAL;
+		goto done;
+	}
 
 	/* Default to YUV if the requested format is not supported. */
 	if (fmt->format.code != MEDIA_BUS_FMT_ARGB8888_1X32 &&
@@ -83,7 +88,7 @@ static int lif_set_format(struct v4l2_subdev *subdev,
 		 * format.
 		 */
 		fmt->format = *format;
-		return 0;
+		goto done;
 	}
 
 	format->code = fmt->format.code;
@@ -101,7 +106,9 @@ static int lif_set_format(struct v4l2_subdev *subdev,
 					    LIF_PAD_SOURCE);
 	*format = fmt->format;
 
-	return 0;
+done:
+	mutex_unlock(&lif->entity.lock);
+	return ret;
 }
 
 static const struct v4l2_subdev_pad_ops lif_pad_ops = {
@@ -122,7 +129,8 @@ static const struct v4l2_subdev_ops lif_ops = {
 
 static void lif_configure(struct vsp1_entity *entity,
 			  struct vsp1_pipeline *pipe,
-			  struct vsp1_dl_list *dl, bool full)
+			  struct vsp1_dl_list *dl,
+			  enum vsp1_entity_params params)
 {
 	const struct v4l2_mbus_framefmt *format;
 	struct vsp1_lif *lif = to_lif(&entity->subdev);
@@ -130,7 +138,7 @@ static void lif_configure(struct vsp1_entity *entity,
 	unsigned int obth = 400;
 	unsigned int lbth = 200;
 
-	if (!full)
+	if (params != VSP1_ENTITY_PARAMS_INIT)
 		return;
 
 	format = vsp1_entity_get_pad_format(&lif->entity, lif->entity.config,
diff --git a/drivers/media/platform/vsp1/vsp1_lut.c b/drivers/media/platform/vsp1/vsp1_lut.c
index dc31de9602ba..c67cc60db0db 100644
--- a/drivers/media/platform/vsp1/vsp1_lut.c
+++ b/drivers/media/platform/vsp1/vsp1_lut.c
@@ -124,10 +124,15 @@ static int lut_set_format(struct v4l2_subdev *subdev,
 	struct vsp1_lut *lut = to_lut(subdev);
 	struct v4l2_subdev_pad_config *config;
 	struct v4l2_mbus_framefmt *format;
+	int ret = 0;
+
+	mutex_lock(&lut->entity.lock);
 
 	config = vsp1_entity_get_pad_config(&lut->entity, cfg, fmt->which);
-	if (!config)
-		return -EINVAL;
+	if (!config) {
+		ret = -EINVAL;
+		goto done;
+	}
 
 	/* Default to YUV if the requested format is not supported. */
 	if (fmt->format.code != MEDIA_BUS_FMT_ARGB8888_1X32 &&
@@ -140,7 +145,7 @@ static int lut_set_format(struct v4l2_subdev *subdev,
 	if (fmt->pad == LUT_PAD_SOURCE) {
 		/* The LUT output format can't be modified. */
 		fmt->format = *format;
-		return 0;
+		goto done;
 	}
 
 	format->code = fmt->format.code;
@@ -158,7 +163,9 @@ static int lut_set_format(struct v4l2_subdev *subdev,
 					    LUT_PAD_SOURCE);
 	*format = fmt->format;
 
-	return 0;
+done:
+	mutex_unlock(&lut->entity.lock);
+	return ret;
 }
 
 /* -----------------------------------------------------------------------------
@@ -183,24 +190,31 @@ static const struct v4l2_subdev_ops lut_ops = {
 
 static void lut_configure(struct vsp1_entity *entity,
 			  struct vsp1_pipeline *pipe,
-			  struct vsp1_dl_list *dl, bool full)
+			  struct vsp1_dl_list *dl,
+			  enum vsp1_entity_params params)
 {
 	struct vsp1_lut *lut = to_lut(&entity->subdev);
 	struct vsp1_dl_body *dlb;
 	unsigned long flags;
 
-	if (full) {
+	switch (params) {
+	case VSP1_ENTITY_PARAMS_INIT:
 		vsp1_lut_write(lut, dl, VI6_LUT_CTRL, VI6_LUT_CTRL_EN);
-		return;
-	}
+		break;
 
-	spin_lock_irqsave(&lut->lock, flags);
-	dlb = lut->lut;
-	lut->lut = NULL;
-	spin_unlock_irqrestore(&lut->lock, flags);
+	case VSP1_ENTITY_PARAMS_PARTITION:
+		break;
+
+	case VSP1_ENTITY_PARAMS_RUNTIME:
+		spin_lock_irqsave(&lut->lock, flags);
+		dlb = lut->lut;
+		lut->lut = NULL;
+		spin_unlock_irqrestore(&lut->lock, flags);
 
-	if (dlb)
-		vsp1_dl_list_add_fragment(dl, dlb);
+		if (dlb)
+			vsp1_dl_list_add_fragment(dl, dlb);
+		break;
+	}
 }
 
 static const struct vsp1_entity_operations lut_entity_ops = {
diff --git a/drivers/media/platform/vsp1/vsp1_pipe.c b/drivers/media/platform/vsp1/vsp1_pipe.c
index 3e75fb3fcace..756ca4ea7668 100644
--- a/drivers/media/platform/vsp1/vsp1_pipe.c
+++ b/drivers/media/platform/vsp1/vsp1_pipe.c
@@ -136,17 +136,23 @@ static const struct vsp1_format_info vsp1_video_formats[] = {
 	  3, { 8, 8, 8 }, false, true, 1, 1, false },
 };
 
-/*
+/**
  * vsp1_get_format_info - Retrieve format information for a 4CC
+ * @vsp1: the VSP1 device
  * @fourcc: the format 4CC
  *
  * Return a pointer to the format information structure corresponding to the
  * given V4L2 format 4CC, or NULL if no corresponding format can be found.
  */
-const struct vsp1_format_info *vsp1_get_format_info(u32 fourcc)
+const struct vsp1_format_info *vsp1_get_format_info(struct vsp1_device *vsp1,
+						    u32 fourcc)
 {
 	unsigned int i;
 
+	/* Special case, the VYUY format is supported on Gen2 only. */
+	if (vsp1->info->gen != 2 && fourcc == V4L2_PIX_FMT_VYUY)
+		return NULL;
+
 	for (i = 0; i < ARRAY_SIZE(vsp1_video_formats); ++i) {
 		const struct vsp1_format_info *info = &vsp1_video_formats[i];
 
@@ -365,6 +371,7 @@ void vsp1_pipelines_suspend(struct vsp1_device *vsp1)
 
 void vsp1_pipelines_resume(struct vsp1_device *vsp1)
 {
+	unsigned long flags;
 	unsigned int i;
 
 	/* Resume all running pipelines. */
@@ -379,7 +386,9 @@ void vsp1_pipelines_resume(struct vsp1_device *vsp1)
 		if (pipe == NULL)
 			continue;
 
+		spin_lock_irqsave(&pipe->irqlock, flags);
 		if (vsp1_pipeline_ready(pipe))
 			vsp1_pipeline_run(pipe);
+		spin_unlock_irqrestore(&pipe->irqlock, flags);
 	}
 }
diff --git a/drivers/media/platform/vsp1/vsp1_pipe.h b/drivers/media/platform/vsp1/vsp1_pipe.h
index d20d997b1fda..ac4ad2655551 100644
--- a/drivers/media/platform/vsp1/vsp1_pipe.h
+++ b/drivers/media/platform/vsp1/vsp1_pipe.h
@@ -77,6 +77,9 @@ enum vsp1_pipeline_state {
  * @uds_input: entity at the input of the UDS, if the UDS is present
  * @entities: list of entities in the pipeline
  * @dl: display list associated with the pipeline
+ * @div_size: The maximum allowed partition size for the pipeline
+ * @partitions: The number of partitions used to process one frame
+ * @current_partition: The partition number currently being configured
  */
 struct vsp1_pipeline {
 	struct media_pipeline pipe;
@@ -104,6 +107,11 @@ struct vsp1_pipeline {
 	struct list_head entities;
 
 	struct vsp1_dl_list *dl;
+
+	unsigned int div_size;
+	unsigned int partitions;
+	struct v4l2_rect partition;
+	unsigned int current_partition;
 };
 
 void vsp1_pipeline_reset(struct vsp1_pipeline *pipe);
@@ -122,6 +130,7 @@ void vsp1_pipeline_propagate_alpha(struct vsp1_pipeline *pipe,
 void vsp1_pipelines_suspend(struct vsp1_device *vsp1);
 void vsp1_pipelines_resume(struct vsp1_device *vsp1);
 
-const struct vsp1_format_info *vsp1_get_format_info(u32 fourcc);
+const struct vsp1_format_info *vsp1_get_format_info(struct vsp1_device *vsp1,
+						    u32 fourcc);
 
 #endif /* __VSP1_PIPE_H__ */
diff --git a/drivers/media/platform/vsp1/vsp1_regs.h b/drivers/media/platform/vsp1/vsp1_regs.h
index 3b03007ba625..47b1dee044fb 100644
--- a/drivers/media/platform/vsp1/vsp1_regs.h
+++ b/drivers/media/platform/vsp1/vsp1_regs.h
@@ -660,6 +660,8 @@
 #define VI6_IP_VERSION_MODEL_VSPR_H2	(0x0a << 8)
 #define VI6_IP_VERSION_MODEL_VSPD_GEN2	(0x0b << 8)
 #define VI6_IP_VERSION_MODEL_VSPS_M2	(0x0c << 8)
+#define VI6_IP_VERSION_MODEL_VSPS_V2H	(0x12 << 8)
+#define VI6_IP_VERSION_MODEL_VSPD_V2H	(0x13 << 8)
 #define VI6_IP_VERSION_MODEL_VSPI_GEN3	(0x14 << 8)
 #define VI6_IP_VERSION_MODEL_VSPBD_GEN3	(0x15 << 8)
 #define VI6_IP_VERSION_MODEL_VSPBC_GEN3	(0x16 << 8)
diff --git a/drivers/media/platform/vsp1/vsp1_rpf.c b/drivers/media/platform/vsp1/vsp1_rpf.c
index 388838913205..b2e34a800ffa 100644
--- a/drivers/media/platform/vsp1/vsp1_rpf.c
+++ b/drivers/media/platform/vsp1/vsp1_rpf.c
@@ -46,34 +46,22 @@ static const struct v4l2_subdev_ops rpf_ops = {
  * VSP1 Entity Operations
  */
 
-static void rpf_set_memory(struct vsp1_entity *entity, struct vsp1_dl_list *dl)
-{
-	struct vsp1_rwpf *rpf = entity_to_rwpf(entity);
-
-	vsp1_rpf_write(rpf, dl, VI6_RPF_SRCM_ADDR_Y,
-		       rpf->mem.addr[0] + rpf->offsets[0]);
-	vsp1_rpf_write(rpf, dl, VI6_RPF_SRCM_ADDR_C0,
-		       rpf->mem.addr[1] + rpf->offsets[1]);
-	vsp1_rpf_write(rpf, dl, VI6_RPF_SRCM_ADDR_C1,
-		       rpf->mem.addr[2] + rpf->offsets[1]);
-}
-
 static void rpf_configure(struct vsp1_entity *entity,
 			  struct vsp1_pipeline *pipe,
-			  struct vsp1_dl_list *dl, bool full)
+			  struct vsp1_dl_list *dl,
+			  enum vsp1_entity_params params)
 {
 	struct vsp1_rwpf *rpf = to_rwpf(&entity->subdev);
 	const struct vsp1_format_info *fmtinfo = rpf->fmtinfo;
 	const struct v4l2_pix_format_mplane *format = &rpf->format;
 	const struct v4l2_mbus_framefmt *source_format;
 	const struct v4l2_mbus_framefmt *sink_format;
-	const struct v4l2_rect *crop;
 	unsigned int left = 0;
 	unsigned int top = 0;
 	u32 pstride;
 	u32 infmt;
 
-	if (!full) {
+	if (params == VSP1_ENTITY_PARAMS_RUNTIME) {
 		vsp1_rpf_write(rpf, dl, VI6_RPF_VRTCOL_SET,
 			       rpf->alpha << VI6_RPF_VRTCOL_SET_LAYA_SHIFT);
 		vsp1_rpf_write(rpf, dl, VI6_RPF_MULT_ALPHA, rpf->mult_alpha |
@@ -83,34 +71,80 @@ static void rpf_configure(struct vsp1_entity *entity,
 		return;
 	}
 
-	/* Source size, stride and crop offsets.
-	 *
-	 * The crop offsets correspond to the location of the crop rectangle top
-	 * left corner in the plane buffer. Only two offsets are needed, as
-	 * planes 2 and 3 always have identical strides.
-	 */
-	crop = vsp1_rwpf_get_crop(rpf, rpf->entity.config);
+	if (params == VSP1_ENTITY_PARAMS_PARTITION) {
+		unsigned int offsets[2];
+		struct v4l2_rect crop;
+
+		/*
+		 * Source size and crop offsets.
+		 *
+		 * The crop offsets correspond to the location of the crop
+		 * rectangle top left corner in the plane buffer. Only two
+		 * offsets are needed, as planes 2 and 3 always have identical
+		 * strides.
+		 */
+		crop = *vsp1_rwpf_get_crop(rpf, rpf->entity.config);
+
+		/*
+		 * Partition Algorithm Control
+		 *
+		 * The partition algorithm can split this frame into multiple
+		 * slices. We must scale our partition window based on the pipe
+		 * configuration to match the destination partition window.
+		 * To achieve this, we adjust our crop to provide a 'sub-crop'
+		 * matching the expected partition window. Only 'left' and
+		 * 'width' need to be adjusted.
+		 */
+		if (pipe->partitions > 1) {
+			const struct v4l2_mbus_framefmt *output;
+			struct vsp1_entity *wpf = &pipe->output->entity;
+			unsigned int input_width = crop.width;
+
+			/*
+			 * Scale the partition window based on the configuration
+			 * of the pipeline.
+			 */
+			output = vsp1_entity_get_pad_format(wpf, wpf->config,
+							    RWPF_PAD_SOURCE);
 
-	vsp1_rpf_write(rpf, dl, VI6_RPF_SRC_BSIZE,
-		       (crop->width << VI6_RPF_SRC_BSIZE_BHSIZE_SHIFT) |
-		       (crop->height << VI6_RPF_SRC_BSIZE_BVSIZE_SHIFT));
-	vsp1_rpf_write(rpf, dl, VI6_RPF_SRC_ESIZE,
-		       (crop->width << VI6_RPF_SRC_ESIZE_EHSIZE_SHIFT) |
-		       (crop->height << VI6_RPF_SRC_ESIZE_EVSIZE_SHIFT));
+			crop.width = pipe->partition.width * input_width
+				   / output->width;
+			crop.left += pipe->partition.left * input_width
+				   / output->width;
+		}
+
+		vsp1_rpf_write(rpf, dl, VI6_RPF_SRC_BSIZE,
+			       (crop.width << VI6_RPF_SRC_BSIZE_BHSIZE_SHIFT) |
+			       (crop.height << VI6_RPF_SRC_BSIZE_BVSIZE_SHIFT));
+		vsp1_rpf_write(rpf, dl, VI6_RPF_SRC_ESIZE,
+			       (crop.width << VI6_RPF_SRC_ESIZE_EHSIZE_SHIFT) |
+			       (crop.height << VI6_RPF_SRC_ESIZE_EVSIZE_SHIFT));
+
+		offsets[0] = crop.top * format->plane_fmt[0].bytesperline
+			   + crop.left * fmtinfo->bpp[0] / 8;
+
+		if (format->num_planes > 1)
+			offsets[1] = crop.top * format->plane_fmt[1].bytesperline
+				   + crop.left / fmtinfo->hsub
+				   * fmtinfo->bpp[1] / 8;
+		else
+			offsets[1] = 0;
+
+		vsp1_rpf_write(rpf, dl, VI6_RPF_SRCM_ADDR_Y,
+			       rpf->mem.addr[0] + offsets[0]);
+		vsp1_rpf_write(rpf, dl, VI6_RPF_SRCM_ADDR_C0,
+			       rpf->mem.addr[1] + offsets[1]);
+		vsp1_rpf_write(rpf, dl, VI6_RPF_SRCM_ADDR_C1,
+			       rpf->mem.addr[2] + offsets[1]);
+		return;
+	}
 
-	rpf->offsets[0] = crop->top * format->plane_fmt[0].bytesperline
-			+ crop->left * fmtinfo->bpp[0] / 8;
+	/* Stride */
 	pstride = format->plane_fmt[0].bytesperline
 		<< VI6_RPF_SRCM_PSTRIDE_Y_SHIFT;
-
-	if (format->num_planes > 1) {
-		rpf->offsets[1] = crop->top * format->plane_fmt[1].bytesperline
-				+ crop->left * fmtinfo->bpp[1] / 8;
+	if (format->num_planes > 1)
 		pstride |= format->plane_fmt[1].bytesperline
 			<< VI6_RPF_SRCM_PSTRIDE_C_SHIFT;
-	} else {
-		rpf->offsets[1] = 0;
-	}
 
 	vsp1_rpf_write(rpf, dl, VI6_RPF_SRCM_PSTRIDE, pstride);
 
@@ -215,7 +249,6 @@ static void rpf_configure(struct vsp1_entity *entity,
 }
 
 static const struct vsp1_entity_operations rpf_entity_ops = {
-	.set_memory = rpf_set_memory,
 	.configure = rpf_configure,
 };
 
diff --git a/drivers/media/platform/vsp1/vsp1_rwpf.c b/drivers/media/platform/vsp1/vsp1_rwpf.c
index 8d461b375e91..66e4d7ea31d6 100644
--- a/drivers/media/platform/vsp1/vsp1_rwpf.c
+++ b/drivers/media/platform/vsp1/vsp1_rwpf.c
@@ -66,11 +66,15 @@ static int vsp1_rwpf_set_format(struct v4l2_subdev *subdev,
 	struct vsp1_rwpf *rwpf = to_rwpf(subdev);
 	struct v4l2_subdev_pad_config *config;
 	struct v4l2_mbus_framefmt *format;
-	struct v4l2_rect *crop;
+	int ret = 0;
+
+	mutex_lock(&rwpf->entity.lock);
 
 	config = vsp1_entity_get_pad_config(&rwpf->entity, cfg, fmt->which);
-	if (!config)
-		return -EINVAL;
+	if (!config) {
+		ret = -EINVAL;
+		goto done;
+	}
 
 	/* Default to YUV if the requested format is not supported. */
 	if (fmt->format.code != MEDIA_BUS_FMT_ARGB8888_1X32 &&
@@ -85,7 +89,7 @@ static int vsp1_rwpf_set_format(struct v4l2_subdev *subdev,
 		 */
 		format->code = fmt->format.code;
 		fmt->format = *format;
-		return 0;
+		goto done;
 	}
 
 	format->code = fmt->format.code;
@@ -98,19 +102,25 @@ static int vsp1_rwpf_set_format(struct v4l2_subdev *subdev,
 
 	fmt->format = *format;
 
-	/* Update the sink crop rectangle. */
-	crop = vsp1_rwpf_get_crop(rwpf, config);
-	crop->left = 0;
-	crop->top = 0;
-	crop->width = fmt->format.width;
-	crop->height = fmt->format.height;
+	if (rwpf->entity.type == VSP1_ENTITY_RPF) {
+		struct v4l2_rect *crop;
+
+		/* Update the sink crop rectangle. */
+		crop = vsp1_rwpf_get_crop(rwpf, config);
+		crop->left = 0;
+		crop->top = 0;
+		crop->width = fmt->format.width;
+		crop->height = fmt->format.height;
+	}
 
 	/* Propagate the format to the source pad. */
 	format = vsp1_entity_get_pad_format(&rwpf->entity, config,
 					    RWPF_PAD_SOURCE);
 	*format = fmt->format;
 
-	return 0;
+done:
+	mutex_unlock(&rwpf->entity.lock);
+	return ret;
 }
 
 static int vsp1_rwpf_get_selection(struct v4l2_subdev *subdev,
@@ -120,14 +130,22 @@ static int vsp1_rwpf_get_selection(struct v4l2_subdev *subdev,
 	struct vsp1_rwpf *rwpf = to_rwpf(subdev);
 	struct v4l2_subdev_pad_config *config;
 	struct v4l2_mbus_framefmt *format;
+	int ret = 0;
 
-	/* Cropping is implemented on the sink pad. */
-	if (sel->pad != RWPF_PAD_SINK)
+	/*
+	 * Cropping is only supported on the RPF and is implemented on the sink
+	 * pad.
+	 */
+	if (rwpf->entity.type == VSP1_ENTITY_WPF || sel->pad != RWPF_PAD_SINK)
 		return -EINVAL;
 
+	mutex_lock(&rwpf->entity.lock);
+
 	config = vsp1_entity_get_pad_config(&rwpf->entity, cfg, sel->which);
-	if (!config)
-		return -EINVAL;
+	if (!config) {
+		ret = -EINVAL;
+		goto done;
+	}
 
 	switch (sel->target) {
 	case V4L2_SEL_TGT_CROP:
@@ -144,10 +162,13 @@ static int vsp1_rwpf_get_selection(struct v4l2_subdev *subdev,
 		break;
 
 	default:
-		return -EINVAL;
+		ret = -EINVAL;
+		break;
 	}
 
-	return 0;
+done:
+	mutex_unlock(&rwpf->entity.lock);
+	return ret;
 }
 
 static int vsp1_rwpf_set_selection(struct v4l2_subdev *subdev,
@@ -158,21 +179,27 @@ static int vsp1_rwpf_set_selection(struct v4l2_subdev *subdev,
 	struct v4l2_subdev_pad_config *config;
 	struct v4l2_mbus_framefmt *format;
 	struct v4l2_rect *crop;
+	int ret = 0;
 
-	/* Cropping is implemented on the sink pad. */
-	if (sel->pad != RWPF_PAD_SINK)
+	/*
+	 * Cropping is only supported on the RPF and is implemented on the sink
+	 * pad.
+	 */
+	if (rwpf->entity.type == VSP1_ENTITY_WPF || sel->pad != RWPF_PAD_SINK)
 		return -EINVAL;
 
 	if (sel->target != V4L2_SEL_TGT_CROP)
 		return -EINVAL;
 
+	mutex_lock(&rwpf->entity.lock);
+
 	config = vsp1_entity_get_pad_config(&rwpf->entity, cfg, sel->which);
-	if (!config)
-		return -EINVAL;
+	if (!config) {
+		ret = -EINVAL;
+		goto done;
+	}
 
-	/* Make sure the crop rectangle is entirely contained in the image. The
-	 * WPF top and left offsets are limited to 255.
-	 */
+	/* Make sure the crop rectangle is entirely contained in the image. */
 	format = vsp1_entity_get_pad_format(&rwpf->entity, config,
 					    RWPF_PAD_SINK);
 
@@ -188,10 +215,6 @@ static int vsp1_rwpf_set_selection(struct v4l2_subdev *subdev,
 
 	sel->r.left = min_t(unsigned int, sel->r.left, format->width - 2);
 	sel->r.top = min_t(unsigned int, sel->r.top, format->height - 2);
-	if (rwpf->entity.type == VSP1_ENTITY_WPF) {
-		sel->r.left = min_t(unsigned int, sel->r.left, 255);
-		sel->r.top = min_t(unsigned int, sel->r.top, 255);
-	}
 	sel->r.width = min_t(unsigned int, sel->r.width,
 			     format->width - sel->r.left);
 	sel->r.height = min_t(unsigned int, sel->r.height,
@@ -206,7 +229,9 @@ static int vsp1_rwpf_set_selection(struct v4l2_subdev *subdev,
 	format->width = crop->width;
 	format->height = crop->height;
 
-	return 0;
+done:
+	mutex_unlock(&rwpf->entity.lock);
+	return ret;
 }
 
 const struct v4l2_subdev_pad_ops vsp1_rwpf_pad_ops = {
diff --git a/drivers/media/platform/vsp1/vsp1_rwpf.h b/drivers/media/platform/vsp1/vsp1_rwpf.h
index cb20484e80da..1c98aff3da5d 100644
--- a/drivers/media/platform/vsp1/vsp1_rwpf.h
+++ b/drivers/media/platform/vsp1/vsp1_rwpf.h
@@ -61,7 +61,6 @@ struct vsp1_rwpf {
 		unsigned int active;
 	} flip;
 
-	unsigned int offsets[2];
 	struct vsp1_rwpf_memory mem;
 
 	struct vsp1_dl_manager *dlm;
@@ -86,17 +85,5 @@ extern const struct v4l2_subdev_pad_ops vsp1_rwpf_pad_ops;
 
 struct v4l2_rect *vsp1_rwpf_get_crop(struct vsp1_rwpf *rwpf,
 				     struct v4l2_subdev_pad_config *config);
-/**
- * vsp1_rwpf_set_memory - Configure DMA addresses for a [RW]PF
- * @rwpf: the [RW]PF instance
- * @dl: the display list
- *
- * This function applies the cached memory buffer address to the display list.
- */
-static inline void vsp1_rwpf_set_memory(struct vsp1_rwpf *rwpf,
-					struct vsp1_dl_list *dl)
-{
-	rwpf->entity.ops->set_memory(&rwpf->entity, dl);
-}
 
 #endif /* __VSP1_RWPF_H__ */
diff --git a/drivers/media/platform/vsp1/vsp1_sru.c b/drivers/media/platform/vsp1/vsp1_sru.c
index 47f5e0cea2ce..b4e568a3b4ed 100644
--- a/drivers/media/platform/vsp1/vsp1_sru.c
+++ b/drivers/media/platform/vsp1/vsp1_sru.c
@@ -128,6 +128,7 @@ static int sru_enum_frame_size(struct v4l2_subdev *subdev,
 	struct vsp1_sru *sru = to_sru(subdev);
 	struct v4l2_subdev_pad_config *config;
 	struct v4l2_mbus_framefmt *format;
+	int ret = 0;
 
 	config = vsp1_entity_get_pad_config(&sru->entity, cfg, fse->which);
 	if (!config)
@@ -135,8 +136,12 @@ static int sru_enum_frame_size(struct v4l2_subdev *subdev,
 
 	format = vsp1_entity_get_pad_format(&sru->entity, config, SRU_PAD_SINK);
 
-	if (fse->index || fse->code != format->code)
-		return -EINVAL;
+	mutex_lock(&sru->entity.lock);
+
+	if (fse->index || fse->code != format->code) {
+		ret = -EINVAL;
+		goto done;
+	}
 
 	if (fse->pad == SRU_PAD_SINK) {
 		fse->min_width = SRU_MIN_SIZE;
@@ -156,7 +161,9 @@ static int sru_enum_frame_size(struct v4l2_subdev *subdev,
 		}
 	}
 
-	return 0;
+done:
+	mutex_unlock(&sru->entity.lock);
+	return ret;
 }
 
 static void sru_try_format(struct vsp1_sru *sru,
@@ -217,10 +224,15 @@ static int sru_set_format(struct v4l2_subdev *subdev,
 	struct vsp1_sru *sru = to_sru(subdev);
 	struct v4l2_subdev_pad_config *config;
 	struct v4l2_mbus_framefmt *format;
+	int ret = 0;
+
+	mutex_lock(&sru->entity.lock);
 
 	config = vsp1_entity_get_pad_config(&sru->entity, cfg, fmt->which);
-	if (!config)
-		return -EINVAL;
+	if (!config) {
+		ret = -EINVAL;
+		goto done;
+	}
 
 	sru_try_format(sru, config, fmt->pad, &fmt->format);
 
@@ -236,7 +248,9 @@ static int sru_set_format(struct v4l2_subdev *subdev,
 		sru_try_format(sru, config, SRU_PAD_SOURCE, format);
 	}
 
-	return 0;
+done:
+	mutex_unlock(&sru->entity.lock);
+	return ret;
 }
 
 static const struct v4l2_subdev_pad_ops sru_pad_ops = {
@@ -257,7 +271,8 @@ static const struct v4l2_subdev_ops sru_ops = {
 
 static void sru_configure(struct vsp1_entity *entity,
 			  struct vsp1_pipeline *pipe,
-			  struct vsp1_dl_list *dl, bool full)
+			  struct vsp1_dl_list *dl,
+			  enum vsp1_entity_params params)
 {
 	const struct vsp1_sru_param *param;
 	struct vsp1_sru *sru = to_sru(&entity->subdev);
@@ -265,7 +280,7 @@ static void sru_configure(struct vsp1_entity *entity,
 	struct v4l2_mbus_framefmt *output;
 	u32 ctrl0;
 
-	if (!full)
+	if (params != VSP1_ENTITY_PARAMS_INIT)
 		return;
 
 	input = vsp1_entity_get_pad_format(&sru->entity, sru->entity.config,
@@ -291,8 +306,27 @@ static void sru_configure(struct vsp1_entity *entity,
 	vsp1_sru_write(sru, dl, VI6_SRU_CTRL2, param->ctrl2);
 }
 
+static unsigned int sru_max_width(struct vsp1_entity *entity,
+				  struct vsp1_pipeline *pipe)
+{
+	struct vsp1_sru *sru = to_sru(&entity->subdev);
+	struct v4l2_mbus_framefmt *input;
+	struct v4l2_mbus_framefmt *output;
+
+	input = vsp1_entity_get_pad_format(&sru->entity, sru->entity.config,
+					   SRU_PAD_SINK);
+	output = vsp1_entity_get_pad_format(&sru->entity, sru->entity.config,
+					    SRU_PAD_SOURCE);
+
+	if (input->width != output->width)
+		return 512;
+	else
+		return 256;
+}
+
 static const struct vsp1_entity_operations sru_entity_ops = {
 	.configure = sru_configure,
+	.max_width = sru_max_width,
 };
 
 /* -----------------------------------------------------------------------------
diff --git a/drivers/media/platform/vsp1/vsp1_uds.c b/drivers/media/platform/vsp1/vsp1_uds.c
index 652dcd895022..da8f89a31ea4 100644
--- a/drivers/media/platform/vsp1/vsp1_uds.c
+++ b/drivers/media/platform/vsp1/vsp1_uds.c
@@ -18,6 +18,7 @@
 
 #include "vsp1.h"
 #include "vsp1_dl.h"
+#include "vsp1_pipe.h"
 #include "vsp1_uds.h"
 
 #define UDS_MIN_SIZE				4U
@@ -133,6 +134,7 @@ static int uds_enum_frame_size(struct v4l2_subdev *subdev,
 	struct vsp1_uds *uds = to_uds(subdev);
 	struct v4l2_subdev_pad_config *config;
 	struct v4l2_mbus_framefmt *format;
+	int ret = 0;
 
 	config = vsp1_entity_get_pad_config(&uds->entity, cfg, fse->which);
 	if (!config)
@@ -141,8 +143,12 @@ static int uds_enum_frame_size(struct v4l2_subdev *subdev,
 	format = vsp1_entity_get_pad_format(&uds->entity, config,
 					    UDS_PAD_SINK);
 
-	if (fse->index || fse->code != format->code)
-		return -EINVAL;
+	mutex_lock(&uds->entity.lock);
+
+	if (fse->index || fse->code != format->code) {
+		ret = -EINVAL;
+		goto done;
+	}
 
 	if (fse->pad == UDS_PAD_SINK) {
 		fse->min_width = UDS_MIN_SIZE;
@@ -156,7 +162,9 @@ static int uds_enum_frame_size(struct v4l2_subdev *subdev,
 				  &fse->max_height);
 	}
 
-	return 0;
+done:
+	mutex_unlock(&uds->entity.lock);
+	return ret;
 }
 
 static void uds_try_format(struct vsp1_uds *uds,
@@ -202,10 +210,15 @@ static int uds_set_format(struct v4l2_subdev *subdev,
 	struct vsp1_uds *uds = to_uds(subdev);
 	struct v4l2_subdev_pad_config *config;
 	struct v4l2_mbus_framefmt *format;
+	int ret = 0;
+
+	mutex_lock(&uds->entity.lock);
 
 	config = vsp1_entity_get_pad_config(&uds->entity, cfg, fmt->which);
-	if (!config)
-		return -EINVAL;
+	if (!config) {
+		ret = -EINVAL;
+		goto done;
+	}
 
 	uds_try_format(uds, config, fmt->pad, &fmt->format);
 
@@ -221,7 +234,9 @@ static int uds_set_format(struct v4l2_subdev *subdev,
 		uds_try_format(uds, config, UDS_PAD_SOURCE, format);
 	}
 
-	return 0;
+done:
+	mutex_unlock(&uds->entity.lock);
+	return ret;
 }
 
 /* -----------------------------------------------------------------------------
@@ -246,7 +261,8 @@ static const struct v4l2_subdev_ops uds_ops = {
 
 static void uds_configure(struct vsp1_entity *entity,
 			  struct vsp1_pipeline *pipe,
-			  struct vsp1_dl_list *dl, bool full)
+			  struct vsp1_dl_list *dl,
+			  enum vsp1_entity_params params)
 {
 	struct vsp1_uds *uds = to_uds(&entity->subdev);
 	const struct v4l2_mbus_framefmt *output;
@@ -255,7 +271,16 @@ static void uds_configure(struct vsp1_entity *entity,
 	unsigned int vscale;
 	bool multitap;
 
-	if (!full)
+	if (params == VSP1_ENTITY_PARAMS_PARTITION) {
+		const struct v4l2_rect *clip = &pipe->partition;
+
+		vsp1_uds_write(uds, dl, VI6_UDS_CLIP_SIZE,
+			       (clip->width << VI6_UDS_CLIP_SIZE_HSIZE_SHIFT) |
+			       (clip->height << VI6_UDS_CLIP_SIZE_VSIZE_SHIFT));
+		return;
+	}
+
+	if (params != VSP1_ENTITY_PARAMS_INIT)
 		return;
 
 	input = vsp1_entity_get_pad_format(&uds->entity, uds->entity.config,
@@ -287,17 +312,39 @@ static void uds_configure(struct vsp1_entity *entity,
 		       (uds_passband_width(vscale)
 				<< VI6_UDS_PASS_BWIDTH_V_SHIFT));
 
-	/* Set the scaling ratios and the output size. */
+	/* Set the scaling ratios. */
 	vsp1_uds_write(uds, dl, VI6_UDS_SCALE,
 		       (hscale << VI6_UDS_SCALE_HFRAC_SHIFT) |
 		       (vscale << VI6_UDS_SCALE_VFRAC_SHIFT));
-	vsp1_uds_write(uds, dl, VI6_UDS_CLIP_SIZE,
-		       (output->width << VI6_UDS_CLIP_SIZE_HSIZE_SHIFT) |
-		       (output->height << VI6_UDS_CLIP_SIZE_VSIZE_SHIFT));
+}
+
+static unsigned int uds_max_width(struct vsp1_entity *entity,
+				  struct vsp1_pipeline *pipe)
+{
+	struct vsp1_uds *uds = to_uds(&entity->subdev);
+	const struct v4l2_mbus_framefmt *output;
+	const struct v4l2_mbus_framefmt *input;
+	unsigned int hscale;
+
+	input = vsp1_entity_get_pad_format(&uds->entity, uds->entity.config,
+					   UDS_PAD_SINK);
+	output = vsp1_entity_get_pad_format(&uds->entity, uds->entity.config,
+					    UDS_PAD_SOURCE);
+	hscale = output->width / input->width;
+
+	if (hscale <= 2)
+		return 256;
+	else if (hscale <= 4)
+		return 512;
+	else if (hscale <= 8)
+		return 1024;
+	else
+		return 2048;
 }
 
 static const struct vsp1_entity_operations uds_entity_ops = {
 	.configure = uds_configure,
+	.max_width = uds_max_width,
 };
 
 /* -----------------------------------------------------------------------------
diff --git a/drivers/media/platform/vsp1/vsp1_video.c b/drivers/media/platform/vsp1/vsp1_video.c
index 9fb4fc26a359..d351b9c768d2 100644
--- a/drivers/media/platform/vsp1/vsp1_video.c
+++ b/drivers/media/platform/vsp1/vsp1_video.c
@@ -117,9 +117,9 @@ static int __vsp1_video_try_format(struct vsp1_video *video,
 	/* Retrieve format information and select the default format if the
 	 * requested format isn't supported.
 	 */
-	info = vsp1_get_format_info(pix->pixelformat);
+	info = vsp1_get_format_info(video->vsp1, pix->pixelformat);
 	if (info == NULL)
-		info = vsp1_get_format_info(VSP1_VIDEO_DEF_FORMAT);
+		info = vsp1_get_format_info(video->vsp1, VSP1_VIDEO_DEF_FORMAT);
 
 	pix->pixelformat = info->fourcc;
 	pix->colorspace = V4L2_COLORSPACE_SRGB;
@@ -169,6 +169,113 @@ static int __vsp1_video_try_format(struct vsp1_video *video,
 }
 
 /* -----------------------------------------------------------------------------
+ * VSP1 Partition Algorithm support
+ */
+
+static void vsp1_video_pipeline_setup_partitions(struct vsp1_pipeline *pipe)
+{
+	struct vsp1_device *vsp1 = pipe->output->entity.vsp1;
+	const struct v4l2_mbus_framefmt *format;
+	struct vsp1_entity *entity;
+	unsigned int div_size;
+
+	format = vsp1_entity_get_pad_format(&pipe->output->entity,
+					    pipe->output->entity.config,
+					    RWPF_PAD_SOURCE);
+	div_size = format->width;
+
+	/* Gen2 hardware doesn't require image partitioning. */
+	if (vsp1->info->gen == 2) {
+		pipe->div_size = div_size;
+		pipe->partitions = 1;
+		return;
+	}
+
+	list_for_each_entry(entity, &pipe->entities, list_pipe) {
+		unsigned int entity_max = VSP1_VIDEO_MAX_WIDTH;
+
+		if (entity->ops->max_width) {
+			entity_max = entity->ops->max_width(entity, pipe);
+			if (entity_max)
+				div_size = min(div_size, entity_max);
+		}
+	}
+
+	pipe->div_size = div_size;
+	pipe->partitions = DIV_ROUND_UP(format->width, div_size);
+}
+
+/**
+ * vsp1_video_partition - Calculate the active partition output window
+ *
+ * @div_size: pre-determined maximum partition division size
+ * @index: partition index
+ *
+ * Returns a v4l2_rect describing the partition window.
+ */
+static struct v4l2_rect vsp1_video_partition(struct vsp1_pipeline *pipe,
+					     unsigned int div_size,
+					     unsigned int index)
+{
+	const struct v4l2_mbus_framefmt *format;
+	struct v4l2_rect partition;
+	unsigned int modulus;
+
+	format = vsp1_entity_get_pad_format(&pipe->output->entity,
+					    pipe->output->entity.config,
+					    RWPF_PAD_SOURCE);
+
+	/* A single partition simply processes the output size in full. */
+	if (pipe->partitions <= 1) {
+		partition.left = 0;
+		partition.top = 0;
+		partition.width = format->width;
+		partition.height = format->height;
+		return partition;
+	}
+
+	/* Initialise the partition with sane starting conditions. */
+	partition.left = index * div_size;
+	partition.top = 0;
+	partition.width = div_size;
+	partition.height = format->height;
+
+	modulus = format->width % div_size;
+
+	/*
+	 * We need to prevent the last partition from being smaller than the
+	 * *minimum* width of the hardware capabilities.
+	 *
+	 * If the modulus is less than half of the partition size,
+	 * the penultimate partition is reduced to half, which is added
+	 * to the final partition: |1234|1234|1234|12|341|
+	 * to prevents this:       |1234|1234|1234|1234|1|.
+	 */
+	if (modulus) {
+		/*
+		 * pipe->partitions is 1 based, whilst index is a 0 based index.
+		 * Normalise this locally.
+		 */
+		unsigned int partitions = pipe->partitions - 1;
+
+		if (modulus < div_size / 2) {
+			if (index == partitions - 1) {
+				/* Halve the penultimate partition. */
+				partition.width = div_size / 2;
+			} else if (index == partitions) {
+				/* Increase the final partition. */
+				partition.width = (div_size / 2) + modulus;
+				partition.left -= div_size / 2;
+			}
+		} else if (index == partitions) {
+			partition.width = modulus;
+		}
+	}
+
+	return partition;
+}
+
+/* -----------------------------------------------------------------------------
  * Pipeline Management
  */
 
@@ -234,44 +341,81 @@ static void vsp1_video_frame_end(struct vsp1_pipeline *pipe,
 {
 	struct vsp1_video *video = rwpf->video;
 	struct vsp1_vb2_buffer *buf;
-	unsigned long flags;
 
 	buf = vsp1_video_complete_buffer(video);
 	if (buf == NULL)
 		return;
 
-	spin_lock_irqsave(&pipe->irqlock, flags);
-
 	video->rwpf->mem = buf->mem;
 	pipe->buffers_ready |= 1 << video->pipe_index;
+}
 
-	spin_unlock_irqrestore(&pipe->irqlock, flags);
+static void vsp1_video_pipeline_run_partition(struct vsp1_pipeline *pipe,
+					      struct vsp1_dl_list *dl)
+{
+	struct vsp1_entity *entity;
+
+	pipe->partition = vsp1_video_partition(pipe, pipe->div_size,
+					       pipe->current_partition);
+
+	list_for_each_entry(entity, &pipe->entities, list_pipe) {
+		if (entity->ops->configure)
+			entity->ops->configure(entity, pipe, dl,
+					       VSP1_ENTITY_PARAMS_PARTITION);
+	}
 }
 
 static void vsp1_video_pipeline_run(struct vsp1_pipeline *pipe)
 {
 	struct vsp1_device *vsp1 = pipe->output->entity.vsp1;
 	struct vsp1_entity *entity;
-	unsigned int i;
 
 	if (!pipe->dl)
 		pipe->dl = vsp1_dl_list_get(pipe->output->dlm);
 
+	/*
+	 * Start with the runtime parameters as the configure operation can
+	 * compute/cache information needed when configuring partitions. This
+	 * is the case with flipping in the WPF.
+	 */
 	list_for_each_entry(entity, &pipe->entities, list_pipe) {
 		if (entity->ops->configure)
-			entity->ops->configure(entity, pipe, pipe->dl, false);
+			entity->ops->configure(entity, pipe, pipe->dl,
+					       VSP1_ENTITY_PARAMS_RUNTIME);
 	}
 
-	for (i = 0; i < vsp1->info->rpf_count; ++i) {
-		struct vsp1_rwpf *rwpf = pipe->inputs[i];
+	/* Run the first partition */
+	pipe->current_partition = 0;
+	vsp1_video_pipeline_run_partition(pipe, pipe->dl);
 
-		if (rwpf)
-			vsp1_rwpf_set_memory(rwpf, pipe->dl);
-	}
+	/* Process consecutive partitions as necessary */
+	for (pipe->current_partition = 1;
+	     pipe->current_partition < pipe->partitions;
+	     pipe->current_partition++) {
+		struct vsp1_dl_list *dl;
 
-	if (!pipe->lif)
-		vsp1_rwpf_set_memory(pipe->output, pipe->dl);
+		/*
+		 * Partition configuration operations will utilise
+		 * the pipe->current_partition variable to determine
+		 * the work they should complete.
+		 */
+		dl = vsp1_dl_list_get(pipe->output->dlm);
+
+		/*
+		 * An incomplete chain will still function, but output only
+		 * the partitions that had a dl available. The frame end
+		 * interrupt will be marked on the last dl in the chain.
+		 */
+		if (!dl) {
+			dev_err(vsp1->dev, "Failed to obtain a dl list. Frame will be incomplete\n");
+			break;
+		}
+
+		vsp1_video_pipeline_run_partition(pipe, dl);
+		vsp1_dl_list_add_chain(pipe->dl, dl);
+	}
 
+	/* Complete, and commit the head display list. */
 	vsp1_dl_list_commit(pipe->dl);
 	pipe->dl = NULL;
 
@@ -285,6 +429,8 @@ static void vsp1_video_pipeline_frame_end(struct vsp1_pipeline *pipe)
 	unsigned long flags;
 	unsigned int i;
 
+	spin_lock_irqsave(&pipe->irqlock, flags);
+
 	/* Complete buffers on all video nodes. */
 	for (i = 0; i < vsp1->info->rpf_count; ++i) {
 		if (!pipe->inputs[i])
@@ -295,8 +441,6 @@ static void vsp1_video_pipeline_frame_end(struct vsp1_pipeline *pipe)
 
 	vsp1_video_frame_end(pipe, pipe->output);
 
-	spin_lock_irqsave(&pipe->irqlock, flags);
-
 	state = pipe->state;
 	pipe->state = VSP1_PIPELINE_STOPPED;
 
@@ -607,6 +751,9 @@ static int vsp1_video_setup_pipeline(struct vsp1_pipeline *pipe)
 {
 	struct vsp1_entity *entity;
 
+	/* Determine this pipelines sizes for image partitioning support. */
+	vsp1_video_pipeline_setup_partitions(pipe);
+
 	/* Prepare the display list. */
 	pipe->dl = vsp1_dl_list_get(pipe->output->dlm);
 	if (!pipe->dl)
@@ -634,7 +781,8 @@ static int vsp1_video_setup_pipeline(struct vsp1_pipeline *pipe)
 		vsp1_entity_route_setup(entity, pipe->dl);
 
 		if (entity->ops->configure)
-			entity->ops->configure(entity, pipe, pipe->dl, true);
+			entity->ops->configure(entity, pipe, pipe->dl,
+					       VSP1_ENTITY_PARAMS_INIT);
 	}
 
 	return 0;
@@ -675,6 +823,14 @@ static void vsp1_video_stop_streaming(struct vb2_queue *vq)
 	unsigned long flags;
 	int ret;
 
+	/*
+	 * Clear the buffers ready flag to make sure the device won't be started
+	 * by a QBUF on the video node on the other side of the pipeline.
+	 */
+	spin_lock_irqsave(&video->irqlock, flags);
+	pipe->buffers_ready &= ~(1 << video->pipe_index);
+	spin_unlock_irqrestore(&video->irqlock, flags);
+
 	mutex_lock(&pipe->lock);
 	if (--pipe->stream_count == pipe->num_inputs) {
 		/* Stop the pipeline. */
diff --git a/drivers/media/platform/vsp1/vsp1_wpf.c b/drivers/media/platform/vsp1/vsp1_wpf.c
index 31983169c24a..7c48f81cd5c1 100644
--- a/drivers/media/platform/vsp1/vsp1_wpf.c
+++ b/drivers/media/platform/vsp1/vsp1_wpf.c
@@ -173,58 +173,28 @@ static void vsp1_wpf_destroy(struct vsp1_entity *entity)
 	vsp1_dlm_destroy(wpf->dlm);
 }
 
-static void wpf_set_memory(struct vsp1_entity *entity, struct vsp1_dl_list *dl)
-{
-	struct vsp1_rwpf *wpf = entity_to_rwpf(entity);
-	const struct v4l2_pix_format_mplane *format = &wpf->format;
-	struct vsp1_rwpf_memory mem = wpf->mem;
-	unsigned int flip = wpf->flip.active;
-	unsigned int offset;
-
-	/* Update the memory offsets based on flipping configuration. The
-	 * destination addresses point to the locations where the VSP starts
-	 * writing to memory, which can be different corners of the image
-	 * depending on vertical flipping. Horizontal flipping is handled
-	 * through a line buffer and doesn't modify the start address.
-	 */
-	if (flip & BIT(WPF_CTRL_VFLIP)) {
-		mem.addr[0] += (format->height - 1)
-			     * format->plane_fmt[0].bytesperline;
-
-		if (format->num_planes > 1) {
-			offset = (format->height / wpf->fmtinfo->vsub - 1)
-			       * format->plane_fmt[1].bytesperline;
-			mem.addr[1] += offset;
-			mem.addr[2] += offset;
-		}
-	}
-
-	vsp1_wpf_write(wpf, dl, VI6_WPF_DSTM_ADDR_Y, mem.addr[0]);
-	vsp1_wpf_write(wpf, dl, VI6_WPF_DSTM_ADDR_C0, mem.addr[1]);
-	vsp1_wpf_write(wpf, dl, VI6_WPF_DSTM_ADDR_C1, mem.addr[2]);
-}
-
 static void wpf_configure(struct vsp1_entity *entity,
 			  struct vsp1_pipeline *pipe,
-			  struct vsp1_dl_list *dl, bool full)
+			  struct vsp1_dl_list *dl,
+			  enum vsp1_entity_params params)
 {
 	struct vsp1_rwpf *wpf = to_rwpf(&entity->subdev);
 	struct vsp1_device *vsp1 = wpf->entity.vsp1;
 	const struct v4l2_mbus_framefmt *source_format;
 	const struct v4l2_mbus_framefmt *sink_format;
-	const struct v4l2_rect *crop;
 	unsigned int i;
 	u32 outfmt = 0;
 	u32 srcrpf = 0;
 
-	if (!full) {
+	if (params == VSP1_ENTITY_PARAMS_RUNTIME) {
 		const unsigned int mask = BIT(WPF_CTRL_VFLIP)
 					| BIT(WPF_CTRL_HFLIP);
+		unsigned long flags;
 
-		spin_lock(&wpf->flip.lock);
+		spin_lock_irqsave(&wpf->flip.lock, flags);
 		wpf->flip.active = (wpf->flip.active & ~mask)
 				 | (wpf->flip.pending & mask);
-		spin_unlock(&wpf->flip.lock);
+		spin_unlock_irqrestore(&wpf->flip.lock, flags);
 
 		outfmt = (wpf->alpha << VI6_WPF_OUTFMT_PDV_SHIFT) | wpf->outfmt;
 
@@ -237,17 +207,6 @@ static void wpf_configure(struct vsp1_entity *entity,
 		return;
 	}
 
-	/* Cropping */
-	crop = vsp1_rwpf_get_crop(wpf, wpf->entity.config);
-
-	vsp1_wpf_write(wpf, dl, VI6_WPF_HSZCLIP, VI6_WPF_SZCLIP_EN |
-		       (crop->left << VI6_WPF_SZCLIP_OFST_SHIFT) |
-		       (crop->width << VI6_WPF_SZCLIP_SIZE_SHIFT));
-	vsp1_wpf_write(wpf, dl, VI6_WPF_VSZCLIP, VI6_WPF_SZCLIP_EN |
-		       (crop->top << VI6_WPF_SZCLIP_OFST_SHIFT) |
-		       (crop->height << VI6_WPF_SZCLIP_SIZE_SHIFT));
-
-	/* Format */
 	sink_format = vsp1_entity_get_pad_format(&wpf->entity,
 						 wpf->entity.config,
 						 RWPF_PAD_SINK);
@@ -255,6 +214,80 @@ static void wpf_configure(struct vsp1_entity *entity,
 						   wpf->entity.config,
 						   RWPF_PAD_SOURCE);
 
+	if (params == VSP1_ENTITY_PARAMS_PARTITION) {
+		const struct v4l2_pix_format_mplane *format = &wpf->format;
+		struct vsp1_rwpf_memory mem = wpf->mem;
+		unsigned int flip = wpf->flip.active;
+		unsigned int width = source_format->width;
+		unsigned int height = source_format->height;
+		unsigned int offset;
+
+		/*
+		 * Cropping. The partition algorithm can split the image into
+		 * multiple slices.
+		 */
+		if (pipe->partitions > 1)
+			width = pipe->partition.width;
+
+		vsp1_wpf_write(wpf, dl, VI6_WPF_HSZCLIP, VI6_WPF_SZCLIP_EN |
+			       (0 << VI6_WPF_SZCLIP_OFST_SHIFT) |
+			       (width << VI6_WPF_SZCLIP_SIZE_SHIFT));
+		vsp1_wpf_write(wpf, dl, VI6_WPF_VSZCLIP, VI6_WPF_SZCLIP_EN |
+			       (0 << VI6_WPF_SZCLIP_OFST_SHIFT) |
+			       (height << VI6_WPF_SZCLIP_SIZE_SHIFT));
+
+		if (pipe->lif)
+			return;
+
+		/*
+		 * Update the memory offsets based on flipping configuration.
+		 * The destination addresses point to the locations where the
+		 * VSP starts writing to memory, which can be different corners
+		 * of the image depending on vertical flipping.
+		 */
+		if (pipe->partitions > 1) {
+			const struct vsp1_format_info *fmtinfo = wpf->fmtinfo;
+
+			/*
+			 * Horizontal flipping is handled through a line buffer
+			 * and doesn't modify the start address, but still needs
+			 * to be handled when image partitioning is in effect to
+			 * order the partitions correctly.
+			 */
+			if (flip & BIT(WPF_CTRL_HFLIP))
+				offset = format->width - pipe->partition.left
+					- pipe->partition.width;
+			else
+				offset = pipe->partition.left;
+
+			mem.addr[0] += offset * fmtinfo->bpp[0] / 8;
+			if (format->num_planes > 1) {
+				mem.addr[1] += offset / fmtinfo->hsub
+					     * fmtinfo->bpp[1] / 8;
+				mem.addr[2] += offset / fmtinfo->hsub
+					     * fmtinfo->bpp[2] / 8;
+			}
+		}
+
+		if (flip & BIT(WPF_CTRL_VFLIP)) {
+			mem.addr[0] += (format->height - 1)
+				     * format->plane_fmt[0].bytesperline;
+
+			if (format->num_planes > 1) {
+				offset = (format->height / wpf->fmtinfo->vsub - 1)
+				       * format->plane_fmt[1].bytesperline;
+				mem.addr[1] += offset;
+				mem.addr[2] += offset;
+			}
+		}
+
+		vsp1_wpf_write(wpf, dl, VI6_WPF_DSTM_ADDR_Y, mem.addr[0]);
+		vsp1_wpf_write(wpf, dl, VI6_WPF_DSTM_ADDR_C0, mem.addr[1]);
+		vsp1_wpf_write(wpf, dl, VI6_WPF_DSTM_ADDR_C1, mem.addr[2]);
+		return;
+	}
+
+	/* Format */
 	if (!pipe->lif) {
 		const struct v4l2_pix_format_mplane *format = &wpf->format;
 		const struct vsp1_format_info *fmtinfo = wpf->fmtinfo;
@@ -318,12 +351,11 @@ static void wpf_configure(struct vsp1_entity *entity,
 	/* Enable interrupts */
 	vsp1_dl_list_write(dl, VI6_WPF_IRQ_STA(wpf->entity.index), 0);
 	vsp1_dl_list_write(dl, VI6_WPF_IRQ_ENB(wpf->entity.index),
-			   VI6_WFP_IRQ_ENB_FREE);
+			   VI6_WFP_IRQ_ENB_DFEE);
 }
 
 static const struct vsp1_entity_operations wpf_entity_ops = {
 	.destroy = vsp1_wpf_destroy,
-	.set_memory = wpf_set_memory,
 	.configure = wpf_configure,
 };
 
@@ -360,7 +392,7 @@ struct vsp1_rwpf *vsp1_wpf_create(struct vsp1_device *vsp1, unsigned int index)
 		return ERR_PTR(ret);
 
 	/* Initialize the display list manager. */
-	wpf->dlm = vsp1_dlm_create(vsp1, index, 4);
+	wpf->dlm = vsp1_dlm_create(vsp1, index, 64);
 	if (!wpf->dlm) {
 		ret = -ENOMEM;
 		goto error;
diff --git a/drivers/media/platform/xilinx/xilinx-dma.c b/drivers/media/platform/xilinx/xilinx-dma.c
index 7ae1a134b1ff..1d5836c3fb7a 100644
--- a/drivers/media/platform/xilinx/xilinx-dma.c
+++ b/drivers/media/platform/xilinx/xilinx-dma.c
@@ -474,7 +474,7 @@ static void xvip_dma_stop_streaming(struct vb2_queue *vq)
 	spin_unlock_irq(&dma->queued_lock);
 }
 
-static struct vb2_ops xvip_dma_queue_qops = {
+static const struct vb2_ops xvip_dma_queue_qops = {
 	.queue_setup = xvip_dma_queue_setup,
 	.buf_prepare = xvip_dma_buffer_prepare,
 	.buf_queue = xvip_dma_buffer_queue,
diff --git a/drivers/media/radio/si470x/radio-si470x-i2c.c b/drivers/media/radio/si470x/radio-si470x-i2c.c
index 471d6a8ae8a4..ee0470a3196b 100644
--- a/drivers/media/radio/si470x/radio-si470x-i2c.c
+++ b/drivers/media/radio/si470x/radio-si470x-i2c.c
@@ -509,7 +509,6 @@ static SIMPLE_DEV_PM_OPS(si470x_i2c_pm, si470x_i2c_suspend, si470x_i2c_resume);
 static struct i2c_driver si470x_i2c_driver = {
 	.driver = {
 		.name		= "si470x",
-		.owner		= THIS_MODULE,
 #ifdef CONFIG_PM_SLEEP
 		.pm		= &si470x_i2c_pm,
 #endif
diff --git a/drivers/media/radio/si470x/radio-si470x-usb.c b/drivers/media/radio/si470x/radio-si470x-usb.c
index 091d793f6583..4b132c29f290 100644
--- a/drivers/media/radio/si470x/radio-si470x-usb.c
+++ b/drivers/media/radio/si470x/radio-si470x-usb.c
@@ -627,7 +627,6 @@ static int si470x_usb_driver_probe(struct usb_interface *intf,
 
 	radio->int_in_urb = usb_alloc_urb(0, GFP_KERNEL);
 	if (!radio->int_in_urb) {
-		dev_info(&intf->dev, "could not allocate int_in_urb");
 		retval = -ENOMEM;
 		goto err_intbuffer;
 	}
diff --git a/drivers/media/radio/si4713/radio-usb-si4713.c b/drivers/media/radio/si4713/radio-usb-si4713.c
index 5146be2a1a50..e5e5a1672bdb 100644
--- a/drivers/media/radio/si4713/radio-usb-si4713.c
+++ b/drivers/media/radio/si4713/radio-usb-si4713.c
@@ -402,7 +402,7 @@ static u32 si4713_functionality(struct i2c_adapter *adapter)
 	return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL;
 }
 
-static struct i2c_algorithm si4713_algo = {
+static const struct i2c_algorithm si4713_algo = {
 	.master_xfer   = si4713_transfer,
 	.functionality = si4713_functionality,
 };
diff --git a/drivers/media/rc/igorplugusb.c b/drivers/media/rc/igorplugusb.c
index e0c531fa01da..5cf983be07a2 100644
--- a/drivers/media/rc/igorplugusb.c
+++ b/drivers/media/rc/igorplugusb.c
@@ -203,7 +203,8 @@ static int igorplugusb_probe(struct usb_interface *intf,
 	 * This device can only store 36 pulses + spaces, which is not enough
 	 * for the NEC protocol and many others.
 	 */
-	rc->allowed_protocols = RC_BIT_ALL & ~(RC_BIT_NEC | RC_BIT_RC6_6A_20 |
+	rc->allowed_protocols = RC_BIT_ALL & ~(RC_BIT_NEC | RC_BIT_NECX |
+			RC_BIT_NEC32 | RC_BIT_RC6_6A_20 |
 			RC_BIT_RC6_6A_24 | RC_BIT_RC6_6A_32 | RC_BIT_RC6_MCE |
 			RC_BIT_SONY20 | RC_BIT_MCE_KBD | RC_BIT_SANYO);
 
diff --git a/drivers/media/rc/img-ir/img-ir-nec.c b/drivers/media/rc/img-ir/img-ir-nec.c
index 27a7ea8f1260..09314933ea08 100644
--- a/drivers/media/rc/img-ir/img-ir-nec.c
+++ b/drivers/media/rc/img-ir/img-ir-nec.c
@@ -34,19 +34,21 @@ static int img_ir_nec_scancode(int len, u64 raw, u64 enabled_protocols,
 				bitrev8(addr_inv) << 16 |
 				bitrev8(data)     <<  8 |
 				bitrev8(data_inv);
+		request->protocol = RC_TYPE_NEC32;
 	} else if ((addr_inv ^ addr) != 0xff) {
 		/* Extended NEC */
 		/* scan encoding: AAaaDD */
 		request->scancode = addr     << 16 |
 				addr_inv <<  8 |
 				data;
+		request->protocol = RC_TYPE_NECX;
 	} else {
 		/* Normal NEC */
 		/* scan encoding: AADD */
 		request->scancode = addr << 8 |
 				data;
+		request->protocol = RC_TYPE_NEC;
 	}
-	request->protocol = RC_TYPE_NEC;
 	return IMG_IR_SCANCODE;
 }
 
@@ -109,7 +111,7 @@ static int img_ir_nec_filter(const struct rc_scancode_filter *in,
  *        http://wiki.altium.com/display/ADOH/NEC+Infrared+Transmission+Protocol
  */
 struct img_ir_decoder img_ir_nec = {
-	.type = RC_BIT_NEC,
+	.type = RC_BIT_NEC | RC_BIT_NECX | RC_BIT_NEC32,
 	.control = {
 		.decoden = 1,
 		.code_type = IMG_IR_CODETYPE_PULSEDIST,
diff --git a/drivers/media/rc/imon.c b/drivers/media/rc/imon.c
index 65f80b8b9f7a..86cc70fe2534 100644
--- a/drivers/media/rc/imon.c
+++ b/drivers/media/rc/imon.c
@@ -2211,16 +2211,11 @@ static struct imon_context *imon_init_intf0(struct usb_interface *intf,
 		goto exit;
 	}
 	rx_urb = usb_alloc_urb(0, GFP_KERNEL);
-	if (!rx_urb) {
-		dev_err(dev, "%s: usb_alloc_urb failed for IR urb", __func__);
+	if (!rx_urb)
 		goto rx_urb_alloc_failed;
-	}
 	tx_urb = usb_alloc_urb(0, GFP_KERNEL);
-	if (!tx_urb) {
-		dev_err(dev, "%s: usb_alloc_urb failed for display urb",
-			__func__);
+	if (!tx_urb)
 		goto tx_urb_alloc_failed;
-	}
 
 	mutex_init(&ictx->lock);
 	spin_lock_init(&ictx->kc_lock);
@@ -2305,10 +2300,8 @@ static struct imon_context *imon_init_intf1(struct usb_interface *intf,
 	int ret = -ENOMEM;
 
 	rx_urb = usb_alloc_urb(0, GFP_KERNEL);
-	if (!rx_urb) {
-		pr_err("usb_alloc_urb failed for IR urb\n");
+	if (!rx_urb)
 		goto rx_urb_alloc_failed;
-	}
 
 	mutex_lock(&ictx->lock);
 
diff --git a/drivers/media/rc/ir-nec-decoder.c b/drivers/media/rc/ir-nec-decoder.c
index bea0d1eedee0..2a9d155548ab 100644
--- a/drivers/media/rc/ir-nec-decoder.c
+++ b/drivers/media/rc/ir-nec-decoder.c
@@ -49,6 +49,7 @@ static int ir_nec_decode(struct rc_dev *dev, struct ir_raw_event ev)
 {
 	struct nec_dec *data = &dev->raw->nec;
 	u32 scancode;
+	enum rc_type rc_type;
 	u8 address, not_address, command, not_command;
 	bool send_32bits = false;
 
@@ -171,22 +172,25 @@ static int ir_nec_decode(struct rc_dev *dev, struct ir_raw_event ev)
 			 * least Apple and TiVo remotes */
 			scancode = data->bits;
 			IR_dprintk(1, "NEC (modified) scancode 0x%08x\n", scancode);
+			rc_type = RC_TYPE_NEC32;
 		} else if ((address ^ not_address) != 0xff) {
 			/* Extended NEC */
 			scancode = address     << 16 |
 				   not_address <<  8 |
 				   command;
 			IR_dprintk(1, "NEC (Ext) scancode 0x%06x\n", scancode);
+			rc_type = RC_TYPE_NECX;
 		} else {
 			/* Normal NEC */
 			scancode = address << 8 | command;
 			IR_dprintk(1, "NEC scancode 0x%04x\n", scancode);
+			rc_type = RC_TYPE_NEC;
 		}
 
 		if (data->is_nec_x)
 			data->necx_repeat = true;
 
-		rc_keydown(dev, RC_TYPE_NEC, scancode, 0);
+		rc_keydown(dev, rc_type, scancode, 0);
 		data->state = STATE_INACTIVE;
 		return 0;
 	}
@@ -198,7 +202,7 @@ static int ir_nec_decode(struct rc_dev *dev, struct ir_raw_event ev)
 }
 
 static struct ir_raw_handler nec_handler = {
-	.protocols	= RC_BIT_NEC,
+	.protocols	= RC_BIT_NEC | RC_BIT_NECX | RC_BIT_NEC32,
 	.decode		= ir_nec_decode,
 };
 
diff --git a/drivers/media/rc/ir-rc6-decoder.c b/drivers/media/rc/ir-rc6-decoder.c
index e0e2edefa651..5cc54c967a80 100644
--- a/drivers/media/rc/ir-rc6-decoder.c
+++ b/drivers/media/rc/ir-rc6-decoder.c
@@ -248,7 +248,7 @@ again:
 				toggle = 0;
 				break;
 			case 24:
-				protocol = RC_BIT_RC6_6A_24;
+				protocol = RC_TYPE_RC6_6A_24;
 				toggle = 0;
 				break;
 			case 32:
@@ -257,7 +257,7 @@ again:
 					toggle = !!(scancode & RC6_6A_MCE_TOGGLE_MASK);
 					scancode &= ~RC6_6A_MCE_TOGGLE_MASK;
 				} else {
-					protocol = RC_BIT_RC6_6A_32;
+					protocol = RC_TYPE_RC6_6A_32;
 					toggle = 0;
 				}
 				break;
diff --git a/drivers/media/rc/meson-ir.c b/drivers/media/rc/meson-ir.c
index fcc3b82d1454..003fff07ade2 100644
--- a/drivers/media/rc/meson-ir.c
+++ b/drivers/media/rc/meson-ir.c
@@ -24,6 +24,7 @@
 
 #define DRIVER_NAME		"meson-ir"
 
+/* valid on all Meson platforms */
 #define IR_DEC_LDR_ACTIVE	0x00
 #define IR_DEC_LDR_IDLE		0x04
 #define IR_DEC_LDR_REPEAT	0x08
@@ -32,12 +33,21 @@
 #define IR_DEC_FRAME		0x14
 #define IR_DEC_STATUS		0x18
 #define IR_DEC_REG1		0x1c
+/* only available on Meson 8b and newer */
+#define IR_DEC_REG2		0x20
 
 #define REG0_RATE_MASK		(BIT(11) - 1)
 
-#define REG1_MODE_MASK		(BIT(7) | BIT(8))
-#define REG1_MODE_NEC		(0 << 7)
-#define REG1_MODE_GENERAL	(2 << 7)
+#define DECODE_MODE_NEC		0x0
+#define DECODE_MODE_RAW		0x2
+
+/* Meson 6b uses REG1 to configure the mode */
+#define REG1_MODE_MASK		GENMASK(8, 7)
+#define REG1_MODE_SHIFT		7
+
+/* Meson 8b / GXBB use REG2 to configure the mode */
+#define REG2_MODE_MASK		GENMASK(3, 0)
+#define REG2_MODE_SHIFT		0
 
 #define REG1_TIME_IV_SHIFT	16
 #define REG1_TIME_IV_MASK	((BIT(13) - 1) << REG1_TIME_IV_SHIFT)
@@ -158,8 +168,15 @@ static int meson_ir_probe(struct platform_device *pdev)
 	/* Reset the decoder */
 	meson_ir_set_mask(ir, IR_DEC_REG1, REG1_RESET, REG1_RESET);
 	meson_ir_set_mask(ir, IR_DEC_REG1, REG1_RESET, 0);
-	/* Set general operation mode */
-	meson_ir_set_mask(ir, IR_DEC_REG1, REG1_MODE_MASK, REG1_MODE_GENERAL);
+
+	/* Set general operation mode (= raw/software decoding) */
+	if (of_device_is_compatible(node, "amlogic,meson6-ir"))
+		meson_ir_set_mask(ir, IR_DEC_REG1, REG1_MODE_MASK,
+				  DECODE_MODE_RAW << REG1_MODE_SHIFT);
+	else
+		meson_ir_set_mask(ir, IR_DEC_REG2, REG2_MODE_MASK,
+				  DECODE_MODE_RAW << REG2_MODE_SHIFT);
+
 	/* Set rate */
 	meson_ir_set_mask(ir, IR_DEC_REG0, REG0_RATE_MASK, MESON_TRATE - 1);
 	/* IRQ on rising and falling edges */
@@ -197,6 +214,8 @@ static int meson_ir_remove(struct platform_device *pdev)
 
 static const struct of_device_id meson_ir_match[] = {
 	{ .compatible = "amlogic,meson6-ir" },
+	{ .compatible = "amlogic,meson8b-ir" },
+	{ .compatible = "amlogic,meson-gxbb-ir" },
 	{ },
 };
 
diff --git a/drivers/media/rc/nuvoton-cir.c b/drivers/media/rc/nuvoton-cir.c
index 00215f343819..04fedaa75612 100644
--- a/drivers/media/rc/nuvoton-cir.c
+++ b/drivers/media/rc/nuvoton-cir.c
@@ -769,21 +769,11 @@ static void nvt_process_rx_ir_data(struct nvt_dev *nvt)
 			rawir.pulse ? "pulse" : "space", rawir.duration);
 
 		ir_raw_event_store_with_filter(nvt->rdev, &rawir);
-
-		/*
-		 * BUF_PULSE_BIT indicates end of IR data, BUF_REPEAT_BYTE
-		 * indicates end of IR signal, but new data incoming. In both
-		 * cases, it means we're ready to call ir_raw_event_handle
-		 */
-		if ((sample == BUF_PULSE_BIT) && (i + 1 < nvt->pkts)) {
-			nvt_dbg("Calling ir_raw_event_handle (signal end)\n");
-			ir_raw_event_handle(nvt->rdev);
-		}
 	}
 
 	nvt->pkts = 0;
 
-	nvt_dbg("Calling ir_raw_event_handle (buffer empty)\n");
+	nvt_dbg("Calling ir_raw_event_handle\n");
 	ir_raw_event_handle(nvt->rdev);
 
 	nvt_dbg_verbose("%s done", __func__);
@@ -801,8 +791,7 @@ static void nvt_handle_rx_fifo_overrun(struct nvt_dev *nvt)
 /* copy data from hardware rx fifo into driver buffer */
 static void nvt_get_rx_ir_data(struct nvt_dev *nvt)
 {
-	u8 fifocount, val;
-	unsigned int b_idx;
+	u8 fifocount;
 	int i;
 
 	/* Get count of how many bytes to read from RX FIFO */
@@ -810,21 +799,11 @@ static void nvt_get_rx_ir_data(struct nvt_dev *nvt)
 
 	nvt_dbg("attempting to fetch %u bytes from hw rx fifo", fifocount);
 
-	b_idx = nvt->pkts;
-
-	/* This should never happen, but lets check anyway... */
-	if (b_idx + fifocount > RX_BUF_LEN) {
-		nvt_process_rx_ir_data(nvt);
-		b_idx = 0;
-	}
-
 	/* Read fifocount bytes from CIR Sample RX FIFO register */
-	for (i = 0; i < fifocount; i++) {
-		val = nvt_cir_reg_read(nvt, CIR_SRXFIFO);
-		nvt->buf[b_idx + i] = val;
-	}
+	for (i = 0; i < fifocount; i++)
+		nvt->buf[i] = nvt_cir_reg_read(nvt, CIR_SRXFIFO);
 
-	nvt->pkts += fifocount;
+	nvt->pkts = fifocount;
 	nvt_dbg("%s: pkts now %d", __func__, nvt->pkts);
 
 	nvt_process_rx_ir_data(nvt);
@@ -886,6 +865,15 @@ static irqreturn_t nvt_cir_isr(int irq, void *data)
 	status = nvt_cir_reg_read(nvt, CIR_IRSTS);
 	iren = nvt_cir_reg_read(nvt, CIR_IREN);
 
+	/* At least NCT6779D creates a spurious interrupt when the
+	 * logical device is being disabled.
+	 */
+	if (status == 0xff && iren == 0xff) {
+		spin_unlock_irqrestore(&nvt->nvt_lock, flags);
+		nvt_dbg_verbose("Spurious interrupt detected");
+		return IRQ_HANDLED;
+	}
+
 	/* IRQ may be shared with CIR WAKE, therefore check for each
 	 * status bit whether the related interrupt source is enabled
 	 */
diff --git a/drivers/media/rc/rc-ir-raw.c b/drivers/media/rc/rc-ir-raw.c
index 144304c94606..205ecc602e34 100644
--- a/drivers/media/rc/rc-ir-raw.c
+++ b/drivers/media/rc/rc-ir-raw.c
@@ -26,6 +26,7 @@ static LIST_HEAD(ir_raw_client_list);
 /* Used to handle IR raw handler extensions */
 static DEFINE_MUTEX(ir_raw_handler_lock);
 static LIST_HEAD(ir_raw_handler_list);
+static DEFINE_MUTEX(available_protocols_lock);
 static u64 available_protocols;
 
 static int ir_raw_event_thread(void *data)
@@ -234,9 +235,9 @@ u64
 ir_raw_get_allowed_protocols(void)
 {
 	u64 protocols;
-	mutex_lock(&ir_raw_handler_lock);
+	mutex_lock(&available_protocols_lock);
 	protocols = available_protocols;
-	mutex_unlock(&ir_raw_handler_lock);
+	mutex_unlock(&available_protocols_lock);
 	return protocols;
 }
 
@@ -330,7 +331,9 @@ int ir_raw_handler_register(struct ir_raw_handler *ir_raw_handler)
 	if (ir_raw_handler->raw_register)
 		list_for_each_entry(raw, &ir_raw_client_list, list)
 			ir_raw_handler->raw_register(raw->dev);
+	mutex_lock(&available_protocols_lock);
 	available_protocols |= ir_raw_handler->protocols;
+	mutex_unlock(&available_protocols_lock);
 	mutex_unlock(&ir_raw_handler_lock);
 
 	return 0;
@@ -349,7 +352,9 @@ void ir_raw_handler_unregister(struct ir_raw_handler *ir_raw_handler)
 		if (ir_raw_handler->raw_unregister)
 			ir_raw_handler->raw_unregister(raw->dev);
 	}
+	mutex_lock(&available_protocols_lock);
 	available_protocols &= ~protocols;
+	mutex_unlock(&available_protocols_lock);
 	mutex_unlock(&ir_raw_handler_lock);
 }
 EXPORT_SYMBOL(ir_raw_handler_unregister);
diff --git a/drivers/media/rc/rc-main.c b/drivers/media/rc/rc-main.c
index 8e7f2929fa6f..d9c1f2ff7119 100644
--- a/drivers/media/rc/rc-main.c
+++ b/drivers/media/rc/rc-main.c
@@ -795,7 +795,9 @@ static const struct {
 	{ RC_BIT_UNKNOWN,	"unknown",	NULL			},
 	{ RC_BIT_RC5 |
 	  RC_BIT_RC5X,		"rc-5",		"ir-rc5-decoder"	},
-	{ RC_BIT_NEC,		"nec",		"ir-nec-decoder"	},
+	{ RC_BIT_NEC |
+	  RC_BIT_NECX |
+	  RC_BIT_NEC32,		"nec",		"ir-nec-decoder"	},
 	{ RC_BIT_RC6_0 |
 	  RC_BIT_RC6_6A_20 |
 	  RC_BIT_RC6_6A_24 |
@@ -1460,6 +1462,10 @@ int rc_register_device(struct rc_dev *dev)
 	dev->input_dev->phys = dev->input_phys;
 	dev->input_dev->name = dev->input_name;
 
+	rc = input_register_device(dev->input_dev);
+	if (rc)
+		goto out_table;
+
 	/*
 	 * Default delay of 250ms is too short for some protocols, especially
 	 * since the timeout is currently set to 250ms. Increase it to 500ms,
@@ -1475,11 +1481,6 @@ int rc_register_device(struct rc_dev *dev)
 	 */
 	dev->input_dev->rep[REP_PERIOD] = 125;
 
-	/* rc_open will be called here */
-	rc = input_register_device(dev->input_dev);
-	if (rc)
-		goto out_table;
-
 	path = kobject_get_path(&dev->dev.kobj, GFP_KERNEL);
 	dev_info(&dev->dev, "%s as %s\n",
 		dev->input_name ?: "Unspecified device", path ?: "N/A");
diff --git a/drivers/media/rc/redrat3.c b/drivers/media/rc/redrat3.c
index 399f44d89a29..05ba47bc0b61 100644
--- a/drivers/media/rc/redrat3.c
+++ b/drivers/media/rc/redrat3.c
@@ -124,6 +124,41 @@
 #define USB_RR3USB_PRODUCT_ID	0x0001
 #define USB_RR3IIUSB_PRODUCT_ID	0x0005
 
+
+/*
+ * The redrat3 encodes an IR signal as set of different lengths and a set
+ * of indices into those lengths. This sets how much two lengths must
+ * differ before they are considered distinct, the value is specified
+ * in microseconds.
+ * Default 5, value 0 to 127.
+ */
+static int length_fuzz = 5;
+module_param(length_fuzz, uint, 0644);
+MODULE_PARM_DESC(length_fuzz, "Length Fuzz (0-127)");
+
+/*
+ * When receiving a continuous ir stream (for example when a user is
+ * holding a button down on a remote), this specifies the minimum size
+ * of a space when the redrat3 sends a irdata packet to the host. Specified
+ * in miliseconds. Default value 18ms.
+ * The value can be between 2 and 30 inclusive.
+ */
+static int minimum_pause = 18;
+module_param(minimum_pause, uint, 0644);
+MODULE_PARM_DESC(minimum_pause, "Minimum Pause in ms (2-30)");
+
+/*
+ * The carrier frequency is measured during the first pulse of the IR
+ * signal. The larger the number of periods used To measure, the more
+ * accurate the result is likely to be, however some signals have short
+ * initial pulses, so in some case it may be necessary to reduce this value.
+ * Default 8, value 1 to 255.
+ */
+static int periods_measure_carrier = 8;
+module_param(periods_measure_carrier, uint, 0644);
+MODULE_PARM_DESC(periods_measure_carrier, "Number of Periods to Measure Carrier (1-255)");
+
+
 struct redrat3_header {
 	__be16 length;
 	__be16 transfer_type;
@@ -188,9 +223,6 @@ struct redrat3_dev {
 	/* usb dma */
 	dma_addr_t dma_in;
 
-	/* rx signal timeout */
-	u32 hw_timeout;
-
 	/* Is the device currently transmitting?*/
 	bool transmitting;
 
@@ -372,7 +404,7 @@ static void redrat3_process_ir_data(struct redrat3_dev *rr3)
 	/* add a trailing space */
 	rawir.pulse = false;
 	rawir.timeout = true;
-	rawir.duration = US_TO_NS(rr3->hw_timeout);
+	rawir.duration = rr3->rc->timeout;
 	dev_dbg(dev, "storing trailing timeout with duration %d\n",
 							rawir.duration);
 	ir_raw_event_store_with_filter(rr3->rc, &rawir);
@@ -480,7 +512,7 @@ static int redrat3_set_timeout(struct rc_dev *rc_dev, unsigned int timeoutns)
 	struct redrat3_dev *rr3 = rc_dev->priv;
 	struct usb_device *udev = rr3->udev;
 	struct device *dev = rr3->dev;
-	u32 *timeout;
+	__be32 *timeout;
 	int ret;
 
 	timeout = kmalloc(sizeof(*timeout), GFP_KERNEL);
@@ -495,10 +527,9 @@ static int redrat3_set_timeout(struct rc_dev *rc_dev, unsigned int timeoutns)
 	dev_dbg(dev, "set ir parm timeout %d ret 0x%02x\n",
 						be32_to_cpu(*timeout), ret);
 
-	if (ret == sizeof(*timeout)) {
-		rr3->hw_timeout = timeoutns / 1000;
+	if (ret == sizeof(*timeout))
 		ret = 0;
-	} else if (ret >= 0)
+	else if (ret >= 0)
 		ret = -EIO;
 
 	kfree(timeout);
@@ -529,12 +560,25 @@ static void redrat3_reset(struct redrat3_dev *rr3)
 			     RR3_CPUCS_REG_ADDR, 0, val, len, HZ * 25);
 	dev_dbg(dev, "reset returned 0x%02x\n", rc);
 
-	*val = 5;
+	*val = length_fuzz;
 	rc = usb_control_msg(udev, txpipe, RR3_SET_IR_PARAM,
 			     USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_OUT,
 			     RR3_IR_IO_LENGTH_FUZZ, 0, val, len, HZ * 25);
 	dev_dbg(dev, "set ir parm len fuzz %d rc 0x%02x\n", *val, rc);
 
+	*val = (65536 - (minimum_pause * 2000)) / 256;
+	rc = usb_control_msg(udev, txpipe, RR3_SET_IR_PARAM,
+			     USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_OUT,
+			     RR3_IR_IO_MIN_PAUSE, 0, val, len, HZ * 25);
+	dev_dbg(dev, "set ir parm min pause %d rc 0x%02x\n", *val, rc);
+
+	*val = periods_measure_carrier;
+	rc = usb_control_msg(udev, txpipe, RR3_SET_IR_PARAM,
+			     USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_OUT,
+			     RR3_IR_IO_PERIODS_MF, 0, val, len, HZ * 25);
+	dev_dbg(dev, "set ir parm periods measure carrier %d rc 0x%02x", *val,
+									rc);
+
 	*val = RR3_DRIVER_MAXLENS;
 	rc = usb_control_msg(udev, txpipe, RR3_SET_IR_PARAM,
 			     USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_OUT,
@@ -889,7 +933,7 @@ static struct rc_dev *redrat3_init_rc_dev(struct redrat3_dev *rr3)
 	rc->allowed_protocols = RC_BIT_ALL;
 	rc->min_timeout = MS_TO_NS(RR3_RX_MIN_TIMEOUT);
 	rc->max_timeout = MS_TO_NS(RR3_RX_MAX_TIMEOUT);
-	rc->timeout = US_TO_NS(rr3->hw_timeout);
+	rc->timeout = US_TO_NS(redrat3_get_timeout(rr3));
 	rc->s_timeout = redrat3_set_timeout;
 	rc->tx_ir = redrat3_transmit_ir;
 	rc->s_tx_carrier = redrat3_set_tx_carrier;
@@ -970,10 +1014,8 @@ static int redrat3_dev_probe(struct usb_interface *intf,
 
 	/* set up bulk-in endpoint */
 	rr3->read_urb = usb_alloc_urb(0, GFP_KERNEL);
-	if (!rr3->read_urb) {
-		dev_err(dev, "Read urb allocation failure\n");
+	if (!rr3->read_urb)
 		goto error;
-	}
 
 	rr3->ep_in = ep_in;
 	rr3->bulk_in_buf = usb_alloc_coherent(udev,
@@ -1000,9 +1042,6 @@ static int redrat3_dev_probe(struct usb_interface *intf,
 	if (retval < 0)
 		goto error;
 
-	/* store current hardware timeout, in µs */
-	rr3->hw_timeout = redrat3_get_timeout(rr3);
-
 	/* default.. will get overridden by any sends with a freq defined */
 	rr3->carrier = 38000;
 
diff --git a/drivers/media/rc/streamzap.c b/drivers/media/rc/streamzap.c
index 815243c65bc3..4004260a7c69 100644
--- a/drivers/media/rc/streamzap.c
+++ b/drivers/media/rc/streamzap.c
@@ -499,7 +499,7 @@ static int streamzap_resume(struct usb_interface *intf)
 	struct streamzap_ir *sz = usb_get_intfdata(intf);
 
 	if (usb_submit_urb(sz->urb_in, GFP_ATOMIC)) {
-		dev_err(sz->dev, "Error sumbiting urb\n");
+		dev_err(sz->dev, "Error submitting urb\n");
 		return -EIO;
 	}
 
diff --git a/drivers/media/spi/Kconfig b/drivers/media/spi/Kconfig
new file mode 100644
index 000000000000..a21f5a39a440
--- /dev/null
+++ b/drivers/media/spi/Kconfig
@@ -0,0 +1,14 @@
+if VIDEO_V4L2
+
+menu "SPI helper chips"
+	visible if !MEDIA_SUBDRV_AUTOSELECT || COMPILE_TEST
+
+config VIDEO_GS1662
+	tristate "Gennum Serializers video"
+	depends on SPI && VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API
+	---help---
+	  Enable the GS1662 driver which serializes video streams.
+
+endmenu
+
+endif
diff --git a/drivers/media/spi/Makefile b/drivers/media/spi/Makefile
new file mode 100644
index 000000000000..ea64013d16cc
--- /dev/null
+++ b/drivers/media/spi/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_VIDEO_GS1662) += gs1662.o
diff --git a/drivers/media/spi/gs1662.c b/drivers/media/spi/gs1662.c
new file mode 100644
index 000000000000..d76f36233f43
--- /dev/null
+++ b/drivers/media/spi/gs1662.c
@@ -0,0 +1,478 @@
+/*
+ * GS1662 device registration.
+ *
+ * Copyright (C) 2015-2016 Nexvision
+ * Author: Charles-Antoine Couret <charles-antoine.couret@nexvision.fr>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation; either version 2 of the License, or (at your
+ * option) any later version.
+ */
+
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/spi/spi.h>
+#include <linux/platform_device.h>
+#include <linux/ctype.h>
+#include <linux/err.h>
+#include <linux/device.h>
+#include <linux/module.h>
+
+#include <linux/videodev2.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-subdev.h>
+#include <media/v4l2-dv-timings.h>
+#include <linux/v4l2-dv-timings.h>
+
+#define REG_STATUS			0x04
+#define REG_FORCE_FMT			0x06
+#define REG_LINES_PER_FRAME		0x12
+#define REG_WORDS_PER_LINE		0x13
+#define REG_WORDS_PER_ACT_LINE		0x14
+#define REG_ACT_LINES_PER_FRAME	0x15
+
+#define MASK_H_LOCK			0x001
+#define MASK_V_LOCK			0x002
+#define MASK_STD_LOCK			0x004
+#define MASK_FORCE_STD			0x020
+#define MASK_STD_STATUS		0x3E0
+
+#define GS_WIDTH_MIN			720
+#define GS_WIDTH_MAX			2048
+#define GS_HEIGHT_MIN			487
+#define GS_HEIGHT_MAX			1080
+#define GS_PIXELCLOCK_MIN		10519200
+#define GS_PIXELCLOCK_MAX		74250000
+
+struct gs {
+	struct spi_device *pdev;
+	struct v4l2_subdev sd;
+	struct v4l2_dv_timings current_timings;
+	int enabled;
+};
+
+struct gs_reg_fmt {
+	u16 reg_value;
+	struct v4l2_dv_timings format;
+};
+
+struct gs_reg_fmt_custom {
+	u16 reg_value;
+	__u32 width;
+	__u32 height;
+	__u64 pixelclock;
+	__u32 interlaced;
+};
+
+static const struct spi_device_id gs_id[] = {
+	{ "gs1662", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(spi, gs_id);
+
+static const struct v4l2_dv_timings fmt_cap[] = {
+	V4L2_DV_BT_SDI_720X487I60,
+	V4L2_DV_BT_CEA_720X576P50,
+	V4L2_DV_BT_CEA_1280X720P24,
+	V4L2_DV_BT_CEA_1280X720P25,
+	V4L2_DV_BT_CEA_1280X720P30,
+	V4L2_DV_BT_CEA_1280X720P50,
+	V4L2_DV_BT_CEA_1280X720P60,
+	V4L2_DV_BT_CEA_1920X1080P24,
+	V4L2_DV_BT_CEA_1920X1080P25,
+	V4L2_DV_BT_CEA_1920X1080P30,
+	V4L2_DV_BT_CEA_1920X1080I50,
+	V4L2_DV_BT_CEA_1920X1080I60,
+};
+
+static const struct gs_reg_fmt reg_fmt[] = {
+	{ 0x00, V4L2_DV_BT_CEA_1280X720P60 },
+	{ 0x01, V4L2_DV_BT_CEA_1280X720P60 },
+	{ 0x02, V4L2_DV_BT_CEA_1280X720P30 },
+	{ 0x03, V4L2_DV_BT_CEA_1280X720P30 },
+	{ 0x04, V4L2_DV_BT_CEA_1280X720P50 },
+	{ 0x05, V4L2_DV_BT_CEA_1280X720P50 },
+	{ 0x06, V4L2_DV_BT_CEA_1280X720P25 },
+	{ 0x07, V4L2_DV_BT_CEA_1280X720P25 },
+	{ 0x08, V4L2_DV_BT_CEA_1280X720P24 },
+	{ 0x09, V4L2_DV_BT_CEA_1280X720P24 },
+	{ 0x0A, V4L2_DV_BT_CEA_1920X1080I60 },
+	{ 0x0B, V4L2_DV_BT_CEA_1920X1080P30 },
+
+	/* Default value: keep this field before 0xC */
+	{ 0x14, V4L2_DV_BT_CEA_1920X1080I50 },
+	{ 0x0C, V4L2_DV_BT_CEA_1920X1080I50 },
+	{ 0x0D, V4L2_DV_BT_CEA_1920X1080P25 },
+	{ 0x0E, V4L2_DV_BT_CEA_1920X1080P25 },
+	{ 0x10, V4L2_DV_BT_CEA_1920X1080P24 },
+	{ 0x12, V4L2_DV_BT_CEA_1920X1080P24 },
+	{ 0x16, V4L2_DV_BT_SDI_720X487I60 },
+	{ 0x19, V4L2_DV_BT_SDI_720X487I60 },
+	{ 0x18, V4L2_DV_BT_CEA_720X576P50 },
+	{ 0x1A, V4L2_DV_BT_CEA_720X576P50 },
+
+	/* Implement following timings before enable it.
+	 * Because of we don't have access to these theoretical timings yet.
+	 * Workaround: use functions to get and set registers for these formats.
+	 */
+#if 0
+	{ 0x0F, V4L2_DV_BT_XXX_1920X1080I25 }, /* SMPTE 274M */
+	{ 0x11, V4L2_DV_BT_XXX_1920X1080I24 }, /* SMPTE 274M */
+	{ 0x13, V4L2_DV_BT_XXX_1920X1080I25 }, /* SMPTE 274M */
+	{ 0x15, V4L2_DV_BT_XXX_1920X1035I60 }, /* SMPTE 260M */
+	{ 0x17, V4L2_DV_BT_SDI_720X507I60 }, /* SMPTE 125M */
+	{ 0x1B, V4L2_DV_BT_SDI_720X507I60 }, /* SMPTE 125M */
+	{ 0x1C, V4L2_DV_BT_XXX_2048X1080P25 }, /* SMPTE 428.1M */
+#endif
+};
+
+static const struct v4l2_dv_timings_cap gs_timings_cap = {
+	.type = V4L2_DV_BT_656_1120,
+	/* keep this initialization for compatibility with GCC < 4.4.6 */
+	.reserved = { 0 },
+	V4L2_INIT_BT_TIMINGS(GS_WIDTH_MIN, GS_WIDTH_MAX, GS_HEIGHT_MIN,
+			     GS_HEIGHT_MAX, GS_PIXELCLOCK_MIN,
+			     GS_PIXELCLOCK_MAX,
+			     V4L2_DV_BT_STD_CEA861 | V4L2_DV_BT_STD_SDI,
+			     V4L2_DV_BT_CAP_PROGRESSIVE
+			     | V4L2_DV_BT_CAP_INTERLACED)
+};
+
+static int gs_read_register(struct spi_device *spi, u16 addr, u16 *value)
+{
+	int ret;
+	u16 buf_addr = (0x8000 | (0x0FFF & addr));
+	u16 buf_value = 0;
+	struct spi_message msg;
+	struct spi_transfer tx[] = {
+		{
+			.tx_buf = &buf_addr,
+			.len = 2,
+			.delay_usecs = 1,
+		}, {
+			.rx_buf = &buf_value,
+			.len = 2,
+			.delay_usecs = 1,
+		},
+	};
+
+	spi_message_init(&msg);
+	spi_message_add_tail(&tx[0], &msg);
+	spi_message_add_tail(&tx[1], &msg);
+	ret = spi_sync(spi, &msg);
+
+	*value = buf_value;
+
+	return ret;
+}
+
+static int gs_write_register(struct spi_device *spi, u16 addr, u16 value)
+{
+	int ret;
+	u16 buf_addr = addr;
+	u16 buf_value = value;
+	struct spi_message msg;
+	struct spi_transfer tx[] = {
+		{
+			.tx_buf = &buf_addr,
+			.len = 2,
+			.delay_usecs = 1,
+		}, {
+			.tx_buf = &buf_value,
+			.len = 2,
+			.delay_usecs = 1,
+		},
+	};
+
+	spi_message_init(&msg);
+	spi_message_add_tail(&tx[0], &msg);
+	spi_message_add_tail(&tx[1], &msg);
+	ret = spi_sync(spi, &msg);
+
+	return ret;
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int gs_g_register(struct v4l2_subdev *sd,
+		  struct v4l2_dbg_register *reg)
+{
+	struct spi_device *spi = v4l2_get_subdevdata(sd);
+	u16 val;
+	int ret;
+
+	ret = gs_read_register(spi, reg->reg & 0xFFFF, &val);
+	reg->val = val;
+	reg->size = 2;
+	return ret;
+}
+
+static int gs_s_register(struct v4l2_subdev *sd,
+		  const struct v4l2_dbg_register *reg)
+{
+	struct spi_device *spi = v4l2_get_subdevdata(sd);
+
+	return gs_write_register(spi, reg->reg & 0xFFFF, reg->val & 0xFFFF);
+}
+#endif
+
+static int gs_status_format(u16 status, struct v4l2_dv_timings *timings)
+{
+	int std = (status & MASK_STD_STATUS) >> 5;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(reg_fmt); i++) {
+		if (reg_fmt[i].reg_value == std) {
+			*timings = reg_fmt[i].format;
+			return 0;
+		}
+	}
+
+	return -ERANGE;
+}
+
+static u16 get_register_timings(struct v4l2_dv_timings *timings)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(reg_fmt); i++) {
+		if (v4l2_match_dv_timings(timings, &reg_fmt[i].format, 0,
+					  false))
+			return reg_fmt[i].reg_value | MASK_FORCE_STD;
+	}
+
+	return 0x0;
+}
+
+static inline struct gs *to_gs(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct gs, sd);
+}
+
+static int gs_s_dv_timings(struct v4l2_subdev *sd,
+		    struct v4l2_dv_timings *timings)
+{
+	struct gs *gs = to_gs(sd);
+	int reg_value;
+
+	reg_value = get_register_timings(timings);
+	if (reg_value == 0x0)
+		return -EINVAL;
+
+	gs->current_timings = *timings;
+	return 0;
+}
+
+static int gs_g_dv_timings(struct v4l2_subdev *sd,
+		    struct v4l2_dv_timings *timings)
+{
+	struct gs *gs = to_gs(sd);
+
+	*timings = gs->current_timings;
+	return 0;
+}
+
+static int gs_query_dv_timings(struct v4l2_subdev *sd,
+			struct v4l2_dv_timings *timings)
+{
+	struct gs *gs = to_gs(sd);
+	struct v4l2_dv_timings fmt;
+	u16 reg_value, i;
+	int ret;
+
+	if (gs->enabled)
+		return -EBUSY;
+
+	/*
+	 * Check if the component detect a line, a frame or something else
+	 * which looks like a video signal activity.
+	 */
+	for (i = 0; i < 4; i++) {
+		gs_read_register(gs->pdev, REG_LINES_PER_FRAME + i, &reg_value);
+		if (reg_value)
+			break;
+	}
+
+	/* If no register reports a video signal */
+	if (i >= 4)
+		return -ENOLINK;
+
+	gs_read_register(gs->pdev, REG_STATUS, &reg_value);
+	if (!(reg_value & MASK_H_LOCK) || !(reg_value & MASK_V_LOCK))
+		return -ENOLCK;
+	if (!(reg_value & MASK_STD_LOCK))
+		return -ERANGE;
+
+	ret = gs_status_format(reg_value, &fmt);
+
+	if (ret < 0)
+		return ret;
+
+	*timings = fmt;
+	return 0;
+}
+
+static int gs_enum_dv_timings(struct v4l2_subdev *sd,
+		       struct v4l2_enum_dv_timings *timings)
+{
+	if (timings->index >= ARRAY_SIZE(fmt_cap))
+		return -EINVAL;
+
+	if (timings->pad != 0)
+		return -EINVAL;
+
+	timings->timings = fmt_cap[timings->index];
+	return 0;
+}
+
+static int gs_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct gs *gs = to_gs(sd);
+	int reg_value;
+
+	if (gs->enabled == enable)
+		return 0;
+
+	gs->enabled = enable;
+
+	if (enable) {
+		/* To force the specific format */
+		reg_value = get_register_timings(&gs->current_timings);
+		return gs_write_register(gs->pdev, REG_FORCE_FMT, reg_value);
+	}
+
+	/* To renable auto-detection mode */
+	return gs_write_register(gs->pdev, REG_FORCE_FMT, 0x0);
+}
+
+static int gs_g_input_status(struct v4l2_subdev *sd, u32 *status)
+{
+	struct gs *gs = to_gs(sd);
+	u16 reg_value, i;
+	int ret;
+
+	/*
+	 * Check if the component detect a line, a frame or something else
+	 * which looks like a video signal activity.
+	 */
+	for (i = 0; i < 4; i++) {
+		ret = gs_read_register(gs->pdev,
+				       REG_LINES_PER_FRAME + i, &reg_value);
+		if (reg_value)
+			break;
+		if (ret) {
+			*status = V4L2_IN_ST_NO_POWER;
+			return ret;
+		}
+	}
+
+	/* If no register reports a video signal */
+	if (i >= 4)
+		*status |= V4L2_IN_ST_NO_SIGNAL;
+
+	ret = gs_read_register(gs->pdev, REG_STATUS, &reg_value);
+	if (!(reg_value & MASK_H_LOCK))
+		*status |=  V4L2_IN_ST_NO_H_LOCK;
+	if (!(reg_value & MASK_V_LOCK))
+		*status |=  V4L2_IN_ST_NO_V_LOCK;
+	if (!(reg_value & MASK_STD_LOCK))
+		*status |=  V4L2_IN_ST_NO_STD_LOCK;
+
+	return ret;
+}
+
+static int gs_dv_timings_cap(struct v4l2_subdev *sd,
+			     struct v4l2_dv_timings_cap *cap)
+{
+	if (cap->pad != 0)
+		return -EINVAL;
+
+	*cap = gs_timings_cap;
+	return 0;
+}
+
+/* V4L2 core operation handlers */
+static const struct v4l2_subdev_core_ops gs_core_ops = {
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register = gs_g_register,
+	.s_register = gs_s_register,
+#endif
+};
+
+static const struct v4l2_subdev_video_ops gs_video_ops = {
+	.s_dv_timings = gs_s_dv_timings,
+	.g_dv_timings = gs_g_dv_timings,
+	.s_stream = gs_s_stream,
+	.g_input_status = gs_g_input_status,
+	.query_dv_timings = gs_query_dv_timings,
+};
+
+static const struct v4l2_subdev_pad_ops gs_pad_ops = {
+	.enum_dv_timings = gs_enum_dv_timings,
+	.dv_timings_cap = gs_dv_timings_cap,
+};
+
+/* V4L2 top level operation handlers */
+static const struct v4l2_subdev_ops gs_ops = {
+	.core = &gs_core_ops,
+	.video = &gs_video_ops,
+	.pad = &gs_pad_ops,
+};
+
+static int gs_probe(struct spi_device *spi)
+{
+	int ret;
+	struct gs *gs;
+	struct v4l2_subdev *sd;
+
+	gs = devm_kzalloc(&spi->dev, sizeof(struct gs), GFP_KERNEL);
+	if (!gs)
+		return -ENOMEM;
+
+	gs->pdev = spi;
+	sd = &gs->sd;
+
+	spi->mode = SPI_MODE_0;
+	spi->irq = -1;
+	spi->max_speed_hz = 10000000;
+	spi->bits_per_word = 16;
+	ret = spi_setup(spi);
+	v4l2_spi_subdev_init(sd, spi, &gs_ops);
+
+	gs->current_timings = reg_fmt[0].format;
+	gs->enabled = 0;
+
+	/* Set H_CONFIG to SMPTE timings */
+	gs_write_register(spi, 0x0, 0x300);
+
+	return ret;
+}
+
+static int gs_remove(struct spi_device *spi)
+{
+	struct v4l2_subdev *sd = spi_get_drvdata(spi);
+	struct gs *gs = to_gs(sd);
+
+	v4l2_device_unregister_subdev(sd);
+	kfree(gs);
+	return 0;
+}
+
+static struct spi_driver gs_driver = {
+	.driver = {
+		.name		= "gs1662",
+		.owner		= THIS_MODULE,
+	},
+
+	.probe		= gs_probe,
+	.remove		= gs_remove,
+	.id_table	= gs_id,
+};
+
+module_spi_driver(gs_driver);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Charles-Antoine Couret <charles-antoine.couret@nexvision.fr>");
+MODULE_DESCRIPTION("Gennum GS1662 HD/SD-SDI Serializer driver");
diff --git a/drivers/media/tuners/fc0011.h b/drivers/media/tuners/fc0011.h
index 81bb568d6943..438cf897acd1 100644
--- a/drivers/media/tuners/fc0011.h
+++ b/drivers/media/tuners/fc0011.h
@@ -1,7 +1,6 @@
 #ifndef LINUX_FC0011_H_
 #define LINUX_FC0011_H_
 
-#include <linux/kconfig.h>
 #include "dvb_frontend.h"
 
 
diff --git a/drivers/media/tuners/fc0012.h b/drivers/media/tuners/fc0012.h
index 9ad32859bab0..4a23e418daf0 100644
--- a/drivers/media/tuners/fc0012.h
+++ b/drivers/media/tuners/fc0012.h
@@ -21,7 +21,6 @@
 #ifndef _FC0012_H_
 #define _FC0012_H_
 
-#include <linux/kconfig.h>
 #include "dvb_frontend.h"
 #include "fc001x-common.h"
 
diff --git a/drivers/media/tuners/fc0013.h b/drivers/media/tuners/fc0013.h
index e130bd7a3230..8c34105c9383 100644
--- a/drivers/media/tuners/fc0013.h
+++ b/drivers/media/tuners/fc0013.h
@@ -22,7 +22,6 @@
 #ifndef _FC0013_H_
 #define _FC0013_H_
 
-#include <linux/kconfig.h>
 #include "dvb_frontend.h"
 #include "fc001x-common.h"
 
diff --git a/drivers/media/tuners/max2165.h b/drivers/media/tuners/max2165.h
index 5054f01a78fb..aadd9fea59e4 100644
--- a/drivers/media/tuners/max2165.h
+++ b/drivers/media/tuners/max2165.h
@@ -22,8 +22,6 @@
 #ifndef __MAX2165_H__
 #define __MAX2165_H__
 
-#include <linux/kconfig.h>
-
 struct dvb_frontend;
 struct i2c_adapter;
 
diff --git a/drivers/media/tuners/mc44s803.h b/drivers/media/tuners/mc44s803.h
index b3e614be657d..6b40df339284 100644
--- a/drivers/media/tuners/mc44s803.h
+++ b/drivers/media/tuners/mc44s803.h
@@ -22,8 +22,6 @@
 #ifndef MC44S803_H
 #define MC44S803_H
 
-#include <linux/kconfig.h>
-
 struct dvb_frontend;
 struct i2c_adapter;
 
diff --git a/drivers/media/tuners/mt2063.c b/drivers/media/tuners/mt2063.c
index 7f0b9d5940db..dfec23743afe 100644
--- a/drivers/media/tuners/mt2063.c
+++ b/drivers/media/tuners/mt2063.c
@@ -2201,7 +2201,7 @@ static int mt2063_get_bandwidth(struct dvb_frontend *fe, u32 *bw)
 	return 0;
 }
 
-static struct dvb_tuner_ops mt2063_ops = {
+static const struct dvb_tuner_ops mt2063_ops = {
 	.info = {
 		 .name = "MT2063 Silicon Tuner",
 		 .frequency_min = 45000000,
diff --git a/drivers/media/tuners/mt20xx.c b/drivers/media/tuners/mt20xx.c
index 9e031040c13f..52da4671b0e0 100644
--- a/drivers/media/tuners/mt20xx.c
+++ b/drivers/media/tuners/mt20xx.c
@@ -363,7 +363,7 @@ static int mt2032_set_params(struct dvb_frontend *fe,
 	return ret;
 }
 
-static struct dvb_tuner_ops mt2032_tuner_ops = {
+static const struct dvb_tuner_ops mt2032_tuner_ops = {
 	.set_analog_params = mt2032_set_params,
 	.release           = microtune_release,
 	.get_frequency     = microtune_get_frequency,
@@ -563,7 +563,7 @@ static int mt2050_set_params(struct dvb_frontend *fe,
 	return ret;
 }
 
-static struct dvb_tuner_ops mt2050_tuner_ops = {
+static const struct dvb_tuner_ops mt2050_tuner_ops = {
 	.set_analog_params = mt2050_set_params,
 	.release           = microtune_release,
 	.get_frequency     = microtune_get_frequency,
diff --git a/drivers/media/tuners/mxl5005s.h b/drivers/media/tuners/mxl5005s.h
index 5764b12c5c7c..d842734f2dcd 100644
--- a/drivers/media/tuners/mxl5005s.h
+++ b/drivers/media/tuners/mxl5005s.h
@@ -23,8 +23,6 @@
 #ifndef __MXL5005S_H
 #define __MXL5005S_H
 
-#include <linux/kconfig.h>
-
 #include <linux/i2c.h>
 #include "dvb_frontend.h"
 
diff --git a/drivers/media/tuners/mxl5007t.c b/drivers/media/tuners/mxl5007t.c
index f4ae04c3328a..42569c6811e6 100644
--- a/drivers/media/tuners/mxl5007t.c
+++ b/drivers/media/tuners/mxl5007t.c
@@ -794,7 +794,7 @@ static int mxl5007t_release(struct dvb_frontend *fe)
 
 /* ------------------------------------------------------------------------- */
 
-static struct dvb_tuner_ops mxl5007t_tuner_ops = {
+static const struct dvb_tuner_ops mxl5007t_tuner_ops = {
 	.info = {
 		.name = "MaxLinear MxL5007T",
 	},
diff --git a/drivers/media/tuners/r820t.h b/drivers/media/tuners/r820t.h
index b1e5661af1c7..fdcab91405de 100644
--- a/drivers/media/tuners/r820t.h
+++ b/drivers/media/tuners/r820t.h
@@ -21,7 +21,6 @@
 #ifndef R820T_H
 #define R820T_H
 
-#include <linux/kconfig.h>
 #include "dvb_frontend.h"
 
 enum r820t_chip {
diff --git a/drivers/media/tuners/si2157.h b/drivers/media/tuners/si2157.h
index 5f1a60bf7ced..76807f5b3cf8 100644
--- a/drivers/media/tuners/si2157.h
+++ b/drivers/media/tuners/si2157.h
@@ -17,7 +17,6 @@
 #ifndef SI2157_H
 #define SI2157_H
 
-#include <linux/kconfig.h>
 #include <media/media-device.h>
 #include "dvb_frontend.h"
 
diff --git a/drivers/media/tuners/tda18212.h b/drivers/media/tuners/tda18212.h
index e58c9096d79c..6391dafd0c9d 100644
--- a/drivers/media/tuners/tda18212.h
+++ b/drivers/media/tuners/tda18212.h
@@ -21,7 +21,6 @@
 #ifndef TDA18212_H
 #define TDA18212_H
 
-#include <linux/kconfig.h>
 #include "dvb_frontend.h"
 
 struct tda18212_config {
diff --git a/drivers/media/tuners/tda18218.h b/drivers/media/tuners/tda18218.h
index 1eacb4f84e93..076b5f2e888d 100644
--- a/drivers/media/tuners/tda18218.h
+++ b/drivers/media/tuners/tda18218.h
@@ -21,7 +21,6 @@
 #ifndef TDA18218_H
 #define TDA18218_H
 
-#include <linux/kconfig.h>
 #include "dvb_frontend.h"
 
 struct tda18218_config {
diff --git a/drivers/media/tuners/tda18271-fe.c b/drivers/media/tuners/tda18271-fe.c
index f8620741bb5f..2d50e8b1dce1 100644
--- a/drivers/media/tuners/tda18271-fe.c
+++ b/drivers/media/tuners/tda18271-fe.c
@@ -18,11 +18,12 @@
     Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 */
 
-#include <linux/delay.h>
-#include <linux/videodev2.h>
 #include "tda18271-priv.h"
 #include "tda8290.h"
 
+#include <linux/delay.h>
+#include <linux/videodev2.h>
+
 int tda18271_debug;
 module_param_named(debug, tda18271_debug, int, 0644);
 MODULE_PARM_DESC(debug, "set debug level "
@@ -646,7 +647,7 @@ static int tda18271_calc_rf_filter_curve(struct dvb_frontend *fe)
 	unsigned int i;
 	int ret;
 
-	tda_info("tda18271: performing RF tracking filter calibration\n");
+	tda_info("performing RF tracking filter calibration\n");
 
 	/* wait for die temperature stabilization */
 	msleep(200);
@@ -692,12 +693,12 @@ static int tda18271c2_rf_cal_init(struct dvb_frontend *fe)
 	if (tda_fail(ret))
 		goto fail;
 
-	tda_info("tda18271: RF tracking filter calibration complete\n");
+	tda_info("RF tracking filter calibration complete\n");
 
 	priv->cal_initialized = true;
 	goto end;
 fail:
-	tda_info("tda18271: RF tracking filter calibration failed!\n");
+	tda_info("RF tracking filter calibration failed!\n");
 end:
 	return ret;
 }
diff --git a/drivers/media/tuners/tda18271-priv.h b/drivers/media/tuners/tda18271-priv.h
index cc80f544af34..0bcc735a0427 100644
--- a/drivers/media/tuners/tda18271-priv.h
+++ b/drivers/media/tuners/tda18271-priv.h
@@ -21,6 +21,8 @@
 #ifndef __TDA18271_PRIV_H__
 #define __TDA18271_PRIV_H__
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/kernel.h>
 #include <linux/types.h>
 #include <linux/mutex.h>
diff --git a/drivers/media/tuners/tda827x.c b/drivers/media/tuners/tda827x.c
index edcb4a723aa1..5050ce9be423 100644
--- a/drivers/media/tuners/tda827x.c
+++ b/drivers/media/tuners/tda827x.c
@@ -818,7 +818,7 @@ static int tda827x_initial_sleep(struct dvb_frontend *fe)
 	return fe->ops.tuner_ops.sleep(fe);
 }
 
-static struct dvb_tuner_ops tda827xo_tuner_ops = {
+static const struct dvb_tuner_ops tda827xo_tuner_ops = {
 	.info = {
 		.name = "Philips TDA827X",
 		.frequency_min  =  55000000,
@@ -834,7 +834,7 @@ static struct dvb_tuner_ops tda827xo_tuner_ops = {
 	.get_bandwidth = tda827x_get_bandwidth,
 };
 
-static struct dvb_tuner_ops tda827xa_tuner_ops = {
+static const struct dvb_tuner_ops tda827xa_tuner_ops = {
 	.info = {
 		.name = "Philips TDA827XA",
 		.frequency_min  =  44000000,
diff --git a/drivers/media/tuners/tea5761.c b/drivers/media/tuners/tea5761.c
index bf78cb9fc52c..36b0b1e1d05b 100644
--- a/drivers/media/tuners/tea5761.c
+++ b/drivers/media/tuners/tea5761.c
@@ -301,7 +301,7 @@ static int tea5761_get_frequency(struct dvb_frontend *fe, u32 *frequency)
 	return 0;
 }
 
-static struct dvb_tuner_ops tea5761_tuner_ops = {
+static const struct dvb_tuner_ops tea5761_tuner_ops = {
 	.info = {
 		.name           = "tea5761", // Philips TEA5761HN FM Radio
 	},
diff --git a/drivers/media/tuners/tea5767.c b/drivers/media/tuners/tea5767.c
index 36e85d81acb2..d62a6d6b1f42 100644
--- a/drivers/media/tuners/tea5767.c
+++ b/drivers/media/tuners/tea5767.c
@@ -10,6 +10,8 @@
  * from their contributions on DScaler.
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/i2c.h>
 #include <linux/slab.h>
 #include <linux/delay.h>
@@ -370,17 +372,18 @@ int tea5767_autodetection(struct i2c_adapter* i2c_adap, u8 i2c_addr)
 {
 	struct tuner_i2c_props i2c = { .adap = i2c_adap, .addr = i2c_addr };
 	unsigned char buffer[7] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
+
 	int rc;
 
 	if ((rc = tuner_i2c_xfer_recv(&i2c, buffer, 7))< 5) {
-		printk(KERN_WARNING "It is not a TEA5767. Received %i bytes.\n", rc);
+		pr_warn("It is not a TEA5767. Received %i bytes.\n", rc);
 		return -EINVAL;
 	}
 
 	/* If all bytes are the same then it's a TV tuner and not a tea5767 */
 	if (buffer[0] == buffer[1] && buffer[0] == buffer[2] &&
 	    buffer[0] == buffer[3] && buffer[0] == buffer[4]) {
-		printk(KERN_WARNING "All bytes are equal. It is not a TEA5767\n");
+		pr_warn("All bytes are equal. It is not a TEA5767\n");
 		return -EINVAL;
 	}
 
@@ -390,7 +393,7 @@ int tea5767_autodetection(struct i2c_adapter* i2c_adap, u8 i2c_addr)
 	 *  Byte 5: bit 7:0 : == 0
 	 */
 	if (((buffer[3] & 0x0f) != 0x00) || (buffer[4] != 0x00)) {
-		printk(KERN_WARNING "Chip ID is not zero. It is not a TEA5767\n");
+		pr_warn("Chip ID is not zero. It is not a TEA5767\n");
 		return -EINVAL;
 	}
 
@@ -423,7 +426,7 @@ static int tea5767_set_config (struct dvb_frontend *fe, void *priv_cfg)
 	return 0;
 }
 
-static struct dvb_tuner_ops tea5767_tuner_ops = {
+static const struct dvb_tuner_ops tea5767_tuner_ops = {
 	.info = {
 		.name           = "tea5767", // Philips TEA5767HN FM Radio
 	},
diff --git a/drivers/media/tuners/tuner-simple.c b/drivers/media/tuners/tuner-simple.c
index 8e9ce144da9a..9ba9582e7765 100644
--- a/drivers/media/tuners/tuner-simple.c
+++ b/drivers/media/tuners/tuner-simple.c
@@ -1035,7 +1035,7 @@ static int simple_get_bandwidth(struct dvb_frontend *fe, u32 *bandwidth)
 	return 0;
 }
 
-static struct dvb_tuner_ops simple_tuner_ops = {
+static const struct dvb_tuner_ops simple_tuner_ops = {
 	.init              = simple_init,
 	.sleep             = simple_sleep,
 	.set_analog_params = simple_set_params,
diff --git a/drivers/media/tuners/xc5000.h b/drivers/media/tuners/xc5000.h
index 00ba29e21fb9..336bd49eb09b 100644
--- a/drivers/media/tuners/xc5000.h
+++ b/drivers/media/tuners/xc5000.h
@@ -22,7 +22,6 @@
 #ifndef __XC5000_H__
 #define __XC5000_H__
 
-#include <linux/kconfig.h>
 #include <linux/firmware.h>
 
 struct dvb_frontend;
diff --git a/drivers/media/usb/airspy/airspy.c b/drivers/media/usb/airspy/airspy.c
index fe031b06935f..8251942bcd12 100644
--- a/drivers/media/usb/airspy/airspy.c
+++ b/drivers/media/usb/airspy/airspy.c
@@ -426,7 +426,6 @@ static int airspy_alloc_urbs(struct airspy *s)
 		dev_dbg(s->dev, "alloc urb=%d\n", i);
 		s->urb_list[i] = usb_alloc_urb(0, GFP_ATOMIC);
 		if (!s->urb_list[i]) {
-			dev_dbg(s->dev, "failed\n");
 			for (j = 0; j < i; j++)
 				usb_free_urb(s->urb_list[j]);
 			return -ENOMEM;
@@ -606,7 +605,7 @@ static void airspy_stop_streaming(struct vb2_queue *vq)
 	mutex_unlock(&s->v4l2_lock);
 }
 
-static struct vb2_ops airspy_vb2_ops = {
+static const struct vb2_ops airspy_vb2_ops = {
 	.queue_setup            = airspy_queue_setup,
 	.buf_queue              = airspy_buf_queue,
 	.start_streaming        = airspy_start_streaming,
diff --git a/drivers/media/usb/as102/as102_usb_drv.c b/drivers/media/usb/as102/as102_usb_drv.c
index 0e8030c071b8..68c3a80ce349 100644
--- a/drivers/media/usb/as102/as102_usb_drv.c
+++ b/drivers/media/usb/as102/as102_usb_drv.c
@@ -270,8 +270,6 @@ static int as102_alloc_usb_stream_buffer(struct as102_dev_t *dev)
 
 		urb = usb_alloc_urb(0, GFP_ATOMIC);
 		if (urb == NULL) {
-			dev_dbg(&dev->bus_adap.usb_dev->dev,
-				"%s: usb_alloc_urb failed\n", __func__);
 			as102_free_usb_stream_buffer(dev);
 			return -ENOMEM;
 		}
diff --git a/drivers/media/usb/au0828/au0828-input.c b/drivers/media/usb/au0828/au0828-input.c
index 3d6687f0407d..1e66e7828d8f 100644
--- a/drivers/media/usb/au0828/au0828-input.c
+++ b/drivers/media/usb/au0828/au0828-input.c
@@ -344,7 +344,8 @@ int au0828_rc_register(struct au0828_dev *dev)
 	rc->dev.parent = &dev->usbdev->dev;
 	rc->driver_name = "au0828-input";
 	rc->driver_type = RC_DRIVER_IR_RAW;
-	rc->allowed_protocols = RC_BIT_NEC | RC_BIT_RC5;
+	rc->allowed_protocols = RC_BIT_NEC | RC_BIT_NECX | RC_BIT_NEC32 |
+								RC_BIT_RC5;
 
 	/* all done */
 	err = rc_register_device(rc);
diff --git a/drivers/media/usb/au0828/au0828-video.c b/drivers/media/usb/au0828/au0828-video.c
index 82b026985868..85dd9a8e83ff 100644
--- a/drivers/media/usb/au0828/au0828-video.c
+++ b/drivers/media/usb/au0828/au0828-video.c
@@ -245,7 +245,6 @@ static int au0828_init_isoc(struct au0828_dev *dev, int max_packets,
 	for (i = 0; i < dev->isoc_ctl.num_bufs; i++) {
 		urb = usb_alloc_urb(max_packets, GFP_KERNEL);
 		if (!urb) {
-			au0828_isocdbg("cannot alloc isoc_ctl.urb %i\n", i);
 			au0828_uninit_isoc(dev);
 			return -ENOMEM;
 		}
@@ -929,7 +928,7 @@ void au0828_stop_vbi_streaming(struct vb2_queue *vq)
 	del_timer_sync(&dev->vbi_timeout);
 }
 
-static struct vb2_ops au0828_video_qops = {
+static const struct vb2_ops au0828_video_qops = {
 	.queue_setup     = queue_setup,
 	.buf_prepare     = buffer_prepare,
 	.buf_queue       = buffer_queue,
diff --git a/drivers/media/usb/cpia2/cpia2_usb.c b/drivers/media/usb/cpia2/cpia2_usb.c
index c1aa1ab2ece9..13620cdf0599 100644
--- a/drivers/media/usb/cpia2/cpia2_usb.c
+++ b/drivers/media/usb/cpia2/cpia2_usb.c
@@ -662,7 +662,6 @@ static int submit_urbs(struct camera_data *cam)
 		}
 		urb = usb_alloc_urb(FRAMES_PER_DESC, GFP_KERNEL);
 		if (!urb) {
-			ERR("%s: usb_alloc_urb error!\n", __func__);
 			for (j = 0; j < i; j++)
 				usb_free_urb(cam->sbuf[j].urb);
 			return -ENOMEM;
diff --git a/drivers/media/usb/cx231xx/cx231xx-audio.c b/drivers/media/usb/cx231xx/cx231xx-audio.c
index a6a9508418f8..8263c4b0610b 100644
--- a/drivers/media/usb/cx231xx/cx231xx-audio.c
+++ b/drivers/media/usb/cx231xx/cx231xx-audio.c
@@ -293,7 +293,6 @@ static int cx231xx_init_audio_isoc(struct cx231xx *dev)
 		memset(dev->adev.transfer_buffer[i], 0x80, sb_size);
 		urb = usb_alloc_urb(CX231XX_ISO_NUM_AUDIO_PACKETS, GFP_ATOMIC);
 		if (!urb) {
-			dev_err(dev->dev, "usb_alloc_urb failed!\n");
 			for (j = 0; j < i; j++) {
 				usb_free_urb(dev->adev.urb[j]);
 				kfree(dev->adev.transfer_buffer[j]);
@@ -355,7 +354,6 @@ static int cx231xx_init_audio_bulk(struct cx231xx *dev)
 		memset(dev->adev.transfer_buffer[i], 0x80, sb_size);
 		urb = usb_alloc_urb(CX231XX_NUM_AUDIO_PACKETS, GFP_ATOMIC);
 		if (!urb) {
-			dev_err(dev->dev, "usb_alloc_urb failed!\n");
 			for (j = 0; j < i; j++) {
 				usb_free_urb(dev->adev.urb[j]);
 				kfree(dev->adev.transfer_buffer[j]);
@@ -637,7 +635,7 @@ static struct page *snd_pcm_get_vmalloc_page(struct snd_pcm_substream *subs,
 	return vmalloc_to_page(pageptr);
 }
 
-static struct snd_pcm_ops snd_cx231xx_pcm_capture = {
+static const struct snd_pcm_ops snd_cx231xx_pcm_capture = {
 	.open = snd_cx231xx_capture_open,
 	.close = snd_cx231xx_pcm_close,
 	.ioctl = snd_pcm_lib_ioctl,
diff --git a/drivers/media/usb/cx231xx/cx231xx-avcore.c b/drivers/media/usb/cx231xx/cx231xx-avcore.c
index 491913778bcc..2f52d66b4dae 100644
--- a/drivers/media/usb/cx231xx/cx231xx-avcore.c
+++ b/drivers/media/usb/cx231xx/cx231xx-avcore.c
@@ -1264,7 +1264,10 @@ int cx231xx_set_agc_analog_digital_mux_select(struct cx231xx *dev,
 				   dev->board.agc_analog_digital_select_gpio,
 				   analog_or_digital);
 
-	return status;
+	if (status < 0)
+		return status;
+
+	return 0;
 }
 
 int cx231xx_enable_i2c_port_3(struct cx231xx *dev, bool is_port_3)
diff --git a/drivers/media/usb/cx231xx/cx231xx-cards.c b/drivers/media/usb/cx231xx/cx231xx-cards.c
index c63248a18823..36bc25494319 100644
--- a/drivers/media/usb/cx231xx/cx231xx-cards.c
+++ b/drivers/media/usb/cx231xx/cx231xx-cards.c
@@ -486,7 +486,7 @@ struct cx231xx_board cx231xx_boards[] = {
 		.output_mode = OUT_MODE_VIP11,
 		.demod_xfer_mode = 0,
 		.ctl_pin_status_mask = 0xFFFFFFC4,
-		.agc_analog_digital_select_gpio = 0x00,	/* According with PV cxPolaris.inf file */
+		.agc_analog_digital_select_gpio = 0x1c,
 		.tuner_sif_gpio = -1,
 		.tuner_scl_gpio = -1,
 		.tuner_sda_gpio = -1,
@@ -1186,12 +1186,12 @@ static void cx231xx_unregister_media_device(struct cx231xx *dev)
 */
 void cx231xx_release_resources(struct cx231xx *dev)
 {
+	cx231xx_ir_exit(dev);
+
 	cx231xx_release_analog_resources(dev);
 
 	cx231xx_remove_from_devlist(dev);
 
-	cx231xx_ir_exit(dev);
-
 	/* Release I2C buses */
 	cx231xx_dev_uninit(dev);
 
diff --git a/drivers/media/usb/cx231xx/cx231xx-core.c b/drivers/media/usb/cx231xx/cx231xx-core.c
index 630f4fc5155f..8b099fe1d592 100644
--- a/drivers/media/usb/cx231xx/cx231xx-core.c
+++ b/drivers/media/usb/cx231xx/cx231xx-core.c
@@ -712,6 +712,7 @@ int cx231xx_set_mode(struct cx231xx *dev, enum cx231xx_mode set_mode)
 			break;
 		case CX231XX_BOARD_CNXT_RDE_253S:
 		case CX231XX_BOARD_CNXT_RDU_253S:
+		case CX231XX_BOARD_PV_PLAYTV_USB_HYBRID:
 			errCode = cx231xx_set_agc_analog_digital_mux_select(dev, 1);
 			break;
 		case CX231XX_BOARD_HAUPPAUGE_EXETER:
@@ -738,14 +739,21 @@ int cx231xx_set_mode(struct cx231xx *dev, enum cx231xx_mode set_mode)
 		case CX231XX_BOARD_PV_PLAYTV_USB_HYBRID:
 		case CX231XX_BOARD_HAUPPAUGE_USB2_FM_PAL:
 		case CX231XX_BOARD_HAUPPAUGE_USB2_FM_NTSC:
-		errCode = cx231xx_set_agc_analog_digital_mux_select(dev, 0);
+			errCode = cx231xx_set_agc_analog_digital_mux_select(dev, 0);
 			break;
 		default:
 			break;
 		}
 	}
 
-	return errCode ? -EINVAL : 0;
+	if (errCode < 0) {
+		dev_err(dev->dev, "Failed to set devmode to %s: error: %i",
+			dev->mode == CX231XX_DIGITAL_MODE ? "digital" : "analog",
+			errCode);
+		return errCode;
+	}
+
+	return 0;
 }
 EXPORT_SYMBOL_GPL(cx231xx_set_mode);
 
@@ -799,7 +807,7 @@ static void cx231xx_isoc_irq_callback(struct urb *urb)
 	case -ESHUTDOWN:
 		return;
 	default:		/* error */
-		cx231xx_isocdbg("urb completition error %d.\n", urb->status);
+		cx231xx_isocdbg("urb completion error %d.\n", urb->status);
 		break;
 	}
 
@@ -842,8 +850,11 @@ static void cx231xx_bulk_irq_callback(struct urb *urb)
 	case -ENOENT:
 	case -ESHUTDOWN:
 		return;
+	case -EPIPE:		/* stall */
+		cx231xx_isocdbg("urb completion error - device is stalled.\n");
+		return;
 	default:		/* error */
-		cx231xx_isocdbg("urb completition error %d.\n", urb->status);
+		cx231xx_isocdbg("urb completion error %d.\n", urb->status);
 		break;
 	}
 
@@ -867,6 +878,7 @@ void cx231xx_uninit_isoc(struct cx231xx *dev)
 	struct cx231xx_dmaqueue *dma_q = &dev->video_mode.vidq;
 	struct urb *urb;
 	int i;
+	bool broken_pipe = false;
 
 	cx231xx_isocdbg("cx231xx: called cx231xx_uninit_isoc\n");
 
@@ -886,12 +898,19 @@ void cx231xx_uninit_isoc(struct cx231xx *dev)
 						  transfer_buffer[i],
 						  urb->transfer_dma);
 			}
+			if (urb->status == -EPIPE) {
+				broken_pipe = true;
+			}
 			usb_free_urb(urb);
 			dev->video_mode.isoc_ctl.urb[i] = NULL;
 		}
 		dev->video_mode.isoc_ctl.transfer_buffer[i] = NULL;
 	}
 
+	if (broken_pipe) {
+		cx231xx_isocdbg("Reset endpoint to recover broken pipe.");
+		usb_reset_endpoint(dev->udev, dev->video_mode.end_point_addr);
+	}
 	kfree(dev->video_mode.isoc_ctl.urb);
 	kfree(dev->video_mode.isoc_ctl.transfer_buffer);
 	kfree(dma_q->p_left_data);
@@ -918,6 +937,7 @@ void cx231xx_uninit_bulk(struct cx231xx *dev)
 	struct cx231xx_dmaqueue *dma_q = &dev->video_mode.vidq;
 	struct urb *urb;
 	int i;
+	bool broken_pipe = false;
 
 	cx231xx_isocdbg("cx231xx: called cx231xx_uninit_bulk\n");
 
@@ -937,12 +957,19 @@ void cx231xx_uninit_bulk(struct cx231xx *dev)
 						transfer_buffer[i],
 						urb->transfer_dma);
 			}
+			if (urb->status == -EPIPE) {
+				broken_pipe = true;
+			}
 			usb_free_urb(urb);
 			dev->video_mode.bulk_ctl.urb[i] = NULL;
 		}
 		dev->video_mode.bulk_ctl.transfer_buffer[i] = NULL;
 	}
 
+	if (broken_pipe) {
+		cx231xx_isocdbg("Reset endpoint to recover broken pipe.");
+		usb_reset_endpoint(dev->udev, dev->video_mode.end_point_addr);
+	}
 	kfree(dev->video_mode.bulk_ctl.urb);
 	kfree(dev->video_mode.bulk_ctl.transfer_buffer);
 	kfree(dma_q->p_left_data);
@@ -1035,8 +1062,6 @@ int cx231xx_init_isoc(struct cx231xx *dev, int max_packets,
 	for (i = 0; i < dev->video_mode.isoc_ctl.num_bufs; i++) {
 		urb = usb_alloc_urb(max_packets, GFP_KERNEL);
 		if (!urb) {
-			dev_err(dev->dev,
-				"cannot alloc isoc_ctl.urb %i\n", i);
 			cx231xx_uninit_isoc(dev);
 			return -ENOMEM;
 		}
@@ -1172,8 +1197,6 @@ int cx231xx_init_bulk(struct cx231xx *dev, int max_packets,
 	for (i = 0; i < dev->video_mode.bulk_ctl.num_bufs; i++) {
 		urb = usb_alloc_urb(0, GFP_KERNEL);
 		if (!urb) {
-			dev_err(dev->dev,
-				"cannot alloc bulk_ctl.urb %i\n", i);
 			cx231xx_uninit_bulk(dev);
 			return -ENOMEM;
 		}
@@ -1301,15 +1324,29 @@ int cx231xx_dev_init(struct cx231xx *dev)
 	dev->i2c_bus[2].i2c_reserve = 0;
 
 	/* register I2C buses */
-	cx231xx_i2c_register(&dev->i2c_bus[0]);
-	cx231xx_i2c_register(&dev->i2c_bus[1]);
-	cx231xx_i2c_register(&dev->i2c_bus[2]);
+	errCode = cx231xx_i2c_register(&dev->i2c_bus[0]);
+	if (errCode < 0)
+		return errCode;
+	errCode = cx231xx_i2c_register(&dev->i2c_bus[1]);
+	if (errCode < 0)
+		return errCode;
+	errCode = cx231xx_i2c_register(&dev->i2c_bus[2]);
+	if (errCode < 0)
+		return errCode;
 
 	errCode = cx231xx_i2c_mux_create(dev);
+	if (errCode < 0) {
+		dev_err(dev->dev,
+			"%s: Failed to create I2C mux\n", __func__);
+		return errCode;
+	}
+	errCode = cx231xx_i2c_mux_register(dev, 0);
+	if (errCode < 0)
+		return errCode;
+
+	errCode = cx231xx_i2c_mux_register(dev, 1);
 	if (errCode < 0)
 		return errCode;
-	cx231xx_i2c_mux_register(dev, 0);
-	cx231xx_i2c_mux_register(dev, 1);
 
 	/* scan the real bus segments in the order of physical port numbers */
 	cx231xx_do_i2c_scan(dev, I2C_0);
@@ -1452,14 +1489,14 @@ int cx231xx_send_gpio_cmd(struct cx231xx *dev, u32 gpio_bit, u8 *gpio_val,
 	/* set request */
 	if (!request) {
 		if (direction)
-			ven_req.bRequest = VRT_GET_GPIO;	/* 0x8 gpio */
+			ven_req.bRequest = VRT_GET_GPIO;	/* 0x9 gpio */
 		else
-			ven_req.bRequest = VRT_SET_GPIO;	/* 0x9 gpio */
+			ven_req.bRequest = VRT_SET_GPIO;	/* 0x8 gpio */
 	} else {
 		if (direction)
-			ven_req.bRequest = VRT_GET_GPIE;	/* 0xa gpie */
+			ven_req.bRequest = VRT_GET_GPIE;	/* 0xb gpie */
 		else
-			ven_req.bRequest = VRT_SET_GPIE;	/* 0xb gpie */
+			ven_req.bRequest = VRT_SET_GPIE;	/* 0xa gpie */
 	}
 
 	/* set index value */
diff --git a/drivers/media/usb/cx231xx/cx231xx-dvb.c b/drivers/media/usb/cx231xx/cx231xx-dvb.c
index ab2fb9fa0cd1..1417515d30eb 100644
--- a/drivers/media/usb/cx231xx/cx231xx-dvb.c
+++ b/drivers/media/usb/cx231xx/cx231xx-dvb.c
@@ -65,6 +65,7 @@ struct cx231xx_dvb {
 	struct dmx_frontend fe_hw;
 	struct dmx_frontend fe_mem;
 	struct dvb_net net;
+	struct i2c_client *i2c_client_demod;
 	struct i2c_client *i2c_client_tuner;
 };
 
@@ -150,18 +151,6 @@ static struct tda18271_config pv_tda18271_config = {
 	.small_i2c = TDA18271_03_BYTE_CHUNK_INIT,
 };
 
-static const struct si2165_config hauppauge_930C_HD_1113xx_si2165_config = {
-	.i2c_addr	= 0x64,
-	.chip_mode	= SI2165_MODE_PLL_XTAL,
-	.ref_freq_Hz	= 16000000,
-};
-
-static const struct si2165_config pctv_quatro_stick_1114xx_si2165_config = {
-	.i2c_addr	= 0x64,
-	.chip_mode	= SI2165_MODE_PLL_EXT,
-	.ref_freq_Hz	= 24000000,
-};
-
 static struct lgdt3306a_config hauppauge_955q_lgdt3306a_config = {
 	.i2c_addr           = 0x59,
 	.qam_if_khz         = 4000,
@@ -586,8 +575,14 @@ static void unregister_dvb(struct cx231xx_dvb *dvb)
 	dvb->demux.dmx.remove_frontend(&dvb->demux.dmx, &dvb->fe_hw);
 	dvb_dmxdev_release(&dvb->dmxdev);
 	dvb_dmx_release(&dvb->demux);
-	client = dvb->i2c_client_tuner;
 	/* remove I2C tuner */
+	client = dvb->i2c_client_tuner;
+	if (client) {
+		module_put(client->dev.driver->owner);
+		i2c_unregister_device(client);
+	}
+	/* remove I2C demod */
+	client = dvb->i2c_client_demod;
 	if (client) {
 		module_put(client->dev.driver->owner);
 		i2c_unregister_device(client);
@@ -749,19 +744,38 @@ static int dvb_init(struct cx231xx *dev)
 		break;
 
 	case CX231XX_BOARD_HAUPPAUGE_930C_HD_1113xx:
+	{
+		struct i2c_client *client;
+		struct i2c_board_info info;
+		struct si2165_platform_data si2165_pdata;
 
-		dev->dvb->frontend = dvb_attach(si2165_attach,
-			&hauppauge_930C_HD_1113xx_si2165_config,
-			demod_i2c
-			);
+		/* attach demod */
+		memset(&si2165_pdata, 0, sizeof(si2165_pdata));
+		si2165_pdata.fe = &dev->dvb->frontend;
+		si2165_pdata.chip_mode = SI2165_MODE_PLL_XTAL,
+		si2165_pdata.ref_freq_Hz = 16000000,
 
-		if (dev->dvb->frontend == NULL) {
+		memset(&info, 0, sizeof(struct i2c_board_info));
+		strlcpy(info.type, "si2165", I2C_NAME_SIZE);
+		info.addr = 0x64;
+		info.platform_data = &si2165_pdata;
+		request_module(info.type);
+		client = i2c_new_device(demod_i2c, &info);
+		if (client == NULL || client->dev.driver == NULL || dev->dvb->frontend == NULL) {
 			dev_err(dev->dev,
 				"Failed to attach SI2165 front end\n");
 			result = -EINVAL;
 			goto out_free;
 		}
 
+		if (!try_module_get(client->dev.driver->owner)) {
+			i2c_unregister_device(client);
+			result = -ENODEV;
+			goto out_free;
+		}
+
+		dvb->i2c_client_demod = client;
+
 		dev->dvb->frontend->ops.i2c_gate_ctrl = NULL;
 
 		/* define general-purpose callback pointer */
@@ -774,27 +788,43 @@ static int dvb_init(struct cx231xx *dev)
 
 		dev->cx231xx_reset_analog_tuner = NULL;
 		break;
-
+	}
 	case CX231XX_BOARD_HAUPPAUGE_930C_HD_1114xx:
 	{
 		struct i2c_client *client;
 		struct i2c_board_info info;
+		struct si2165_platform_data si2165_pdata;
 		struct si2157_config si2157_config;
 
-		memset(&info, 0, sizeof(struct i2c_board_info));
+		/* attach demod */
+		memset(&si2165_pdata, 0, sizeof(si2165_pdata));
+		si2165_pdata.fe = &dev->dvb->frontend;
+		si2165_pdata.chip_mode = SI2165_MODE_PLL_EXT,
+		si2165_pdata.ref_freq_Hz = 24000000,
 
-		dev->dvb->frontend = dvb_attach(si2165_attach,
-			&pctv_quatro_stick_1114xx_si2165_config,
-			demod_i2c
-			);
-
-		if (dev->dvb->frontend == NULL) {
+		memset(&info, 0, sizeof(struct i2c_board_info));
+		strlcpy(info.type, "si2165", I2C_NAME_SIZE);
+		info.addr = 0x64;
+		info.platform_data = &si2165_pdata;
+		request_module(info.type);
+		client = i2c_new_device(demod_i2c, &info);
+		if (client == NULL || client->dev.driver == NULL || dev->dvb->frontend == NULL) {
 			dev_err(dev->dev,
 				"Failed to attach SI2165 front end\n");
 			result = -EINVAL;
 			goto out_free;
 		}
 
+		if (!try_module_get(client->dev.driver->owner)) {
+			i2c_unregister_device(client);
+			result = -ENODEV;
+			goto out_free;
+		}
+
+		dvb->i2c_client_demod = client;
+
+		memset(&info, 0, sizeof(struct i2c_board_info));
+
 		dev->dvb->frontend->ops.i2c_gate_ctrl = NULL;
 
 		/* define general-purpose callback pointer */
diff --git a/drivers/media/usb/cx231xx/cx231xx-i2c.c b/drivers/media/usb/cx231xx/cx231xx-i2c.c
index 473cd3433fe5..35e9acfe63d3 100644
--- a/drivers/media/usb/cx231xx/cx231xx-i2c.c
+++ b/drivers/media/usb/cx231xx/cx231xx-i2c.c
@@ -454,7 +454,7 @@ static u32 functionality(struct i2c_adapter *adap)
 	return I2C_FUNC_SMBUS_EMUL | I2C_FUNC_I2C;
 }
 
-static struct i2c_algorithm cx231xx_algo = {
+static const struct i2c_algorithm cx231xx_algo = {
 	.master_xfer = cx231xx_i2c_xfer,
 	.functionality = functionality,
 };
@@ -608,7 +608,7 @@ struct i2c_adapter *cx231xx_get_i2c_adap(struct cx231xx *dev, int i2c_port)
 	case I2C_1_MUX_3:
 		return dev->muxc->adapter[1];
 	default:
-		return NULL;
+		BUG();
 	}
 }
 EXPORT_SYMBOL_GPL(cx231xx_get_i2c_adap);
diff --git a/drivers/media/usb/cx231xx/cx231xx-vbi.c b/drivers/media/usb/cx231xx/cx231xx-vbi.c
index 15bb573b78ac..76e901920f6f 100644
--- a/drivers/media/usb/cx231xx/cx231xx-vbi.c
+++ b/drivers/media/usb/cx231xx/cx231xx-vbi.c
@@ -442,8 +442,6 @@ int cx231xx_init_vbi_isoc(struct cx231xx *dev, int max_packets,
 
 		urb = usb_alloc_urb(0, GFP_KERNEL);
 		if (!urb) {
-			dev_err(dev->dev,
-				"cannot alloc bulk_ctl.urb %i\n", i);
 			cx231xx_uninit_vbi_isoc(dev);
 			return -ENOMEM;
 		}
diff --git a/drivers/media/usb/dvb-usb-v2/af9015.c b/drivers/media/usb/dvb-usb-v2/af9015.c
index 09e0f58f6bb7..941ceff9b268 100644
--- a/drivers/media/usb/dvb-usb-v2/af9015.c
+++ b/drivers/media/usb/dvb-usb-v2/af9015.c
@@ -1222,6 +1222,7 @@ static int af9015_rc_query(struct dvb_usb_device *d)
 
 	/* Only process key if canary killed */
 	if (buf[16] != 0xff && buf[0] != 0x01) {
+		enum rc_type proto;
 		dev_dbg(&d->udev->dev, "%s: key pressed %*ph\n",
 				__func__, 4, buf + 12);
 
@@ -1237,11 +1238,13 @@ static int af9015_rc_query(struct dvb_usb_device *d)
 				/* NEC */
 				state->rc_keycode = RC_SCANCODE_NEC(buf[12],
 								    buf[14]);
+				proto = RC_TYPE_NEC;
 			} else {
 				/* NEC extended*/
 				state->rc_keycode = RC_SCANCODE_NECX(buf[12] << 8 |
 								     buf[13],
 								     buf[14]);
+				proto = RC_TYPE_NECX;
 			}
 		} else {
 			/* 32 bit NEC */
@@ -1249,8 +1252,9 @@ static int af9015_rc_query(struct dvb_usb_device *d)
 							      buf[13] << 16 |
 							      buf[14] << 8  |
 							      buf[15]);
+			proto = RC_TYPE_NEC32;
 		}
-		rc_keydown(d->rc_dev, RC_TYPE_NEC, state->rc_keycode, 0);
+		rc_keydown(d->rc_dev, proto, state->rc_keycode, 0);
 	} else {
 		dev_dbg(&d->udev->dev, "%s: no key press\n", __func__);
 		/* Invalidate last keypress */
@@ -1317,7 +1321,7 @@ static int af9015_get_rc_config(struct dvb_usb_device *d, struct dvb_usb_rc *rc)
 	if (!rc->map_name)
 		rc->map_name = RC_MAP_EMPTY;
 
-	rc->allowed_protos = RC_BIT_NEC;
+	rc->allowed_protos = RC_BIT_NEC | RC_BIT_NECX | RC_BIT_NEC32;
 	rc->query = af9015_rc_query;
 	rc->interval = 500;
 
diff --git a/drivers/media/usb/dvb-usb-v2/af9035.c b/drivers/media/usb/dvb-usb-v2/af9035.c
index ca018cd3fcd4..8961dd732522 100644
--- a/drivers/media/usb/dvb-usb-v2/af9035.c
+++ b/drivers/media/usb/dvb-usb-v2/af9035.c
@@ -1828,6 +1828,7 @@ static int af9035_rc_query(struct dvb_usb_device *d)
 {
 	struct usb_interface *intf = d->intf;
 	int ret;
+	enum rc_type proto;
 	u32 key;
 	u8 buf[4];
 	struct usb_req req = { CMD_IR_GET, 0, 0, NULL, 4, buf };
@@ -1842,19 +1843,22 @@ static int af9035_rc_query(struct dvb_usb_device *d)
 		if ((buf[0] + buf[1]) == 0xff) {
 			/* NEC standard 16bit */
 			key = RC_SCANCODE_NEC(buf[0], buf[2]);
+			proto = RC_TYPE_NEC;
 		} else {
 			/* NEC extended 24bit */
 			key = RC_SCANCODE_NECX(buf[0] << 8 | buf[1], buf[2]);
+			proto = RC_TYPE_NECX;
 		}
 	} else {
 		/* NEC full code 32bit */
 		key = RC_SCANCODE_NEC32(buf[0] << 24 | buf[1] << 16 |
 					buf[2] << 8  | buf[3]);
+		proto = RC_TYPE_NEC32;
 	}
 
 	dev_dbg(&intf->dev, "%*ph\n", 4, buf);
 
-	rc_keydown(d->rc_dev, RC_TYPE_NEC, key, 0);
+	rc_keydown(d->rc_dev, proto, key, 0);
 
 	return 0;
 
@@ -1889,7 +1893,8 @@ static int af9035_get_rc_config(struct dvb_usb_device *d, struct dvb_usb_rc *rc)
 		switch (tmp) {
 		case 0: /* NEC */
 		default:
-			rc->allowed_protos = RC_BIT_NEC;
+			rc->allowed_protos = RC_BIT_NEC | RC_BIT_NECX |
+								RC_BIT_NEC32;
 			break;
 		case 1: /* RC6 */
 			rc->allowed_protos = RC_BIT_RC6_MCE;
diff --git a/drivers/media/usb/dvb-usb-v2/az6007.c b/drivers/media/usb/dvb-usb-v2/az6007.c
index 935dbaa80ef0..50c07fe7dacb 100644
--- a/drivers/media/usb/dvb-usb-v2/az6007.c
+++ b/drivers/media/usb/dvb-usb-v2/az6007.c
@@ -208,6 +208,7 @@ static int az6007_rc_query(struct dvb_usb_device *d)
 {
 	struct az6007_device_state *st = d_to_priv(d);
 	unsigned code;
+	enum rc_type proto;
 
 	az6007_read(d, AZ6007_READ_IR, 0, 0, st->data, 10);
 
@@ -215,19 +216,23 @@ static int az6007_rc_query(struct dvb_usb_device *d)
 		return 0;
 
 	if ((st->data[3] ^ st->data[4]) == 0xff) {
-		if ((st->data[1] ^ st->data[2]) == 0xff)
+		if ((st->data[1] ^ st->data[2]) == 0xff) {
 			code = RC_SCANCODE_NEC(st->data[1], st->data[3]);
-		else
+			proto = RC_TYPE_NEC;
+		} else {
 			code = RC_SCANCODE_NECX(st->data[1] << 8 | st->data[2],
 						st->data[3]);
+			proto = RC_TYPE_NECX;
+		}
 	} else {
 		code = RC_SCANCODE_NEC32(st->data[1] << 24 |
 					 st->data[2] << 16 |
 					 st->data[3] << 8  |
 					 st->data[4]);
+		proto = RC_TYPE_NEC32;
 	}
 
-	rc_keydown(d->rc_dev, RC_TYPE_NEC, code, st->data[5]);
+	rc_keydown(d->rc_dev, proto, code, st->data[5]);
 
 	return 0;
 }
@@ -236,7 +241,7 @@ static int az6007_get_rc_config(struct dvb_usb_device *d, struct dvb_usb_rc *rc)
 {
 	pr_debug("Getting az6007 Remote Control properties\n");
 
-	rc->allowed_protos = RC_BIT_NEC;
+	rc->allowed_protos = RC_BIT_NEC | RC_BIT_NECX | RC_BIT_NEC32;
 	rc->query          = az6007_rc_query;
 	rc->interval       = 400;
 
diff --git a/drivers/media/usb/dvb-usb-v2/dvb_usb_core.c b/drivers/media/usb/dvb-usb-v2/dvb_usb_core.c
index 3fbb2cd19f5e..a8e6624fbe83 100644
--- a/drivers/media/usb/dvb-usb-v2/dvb_usb_core.c
+++ b/drivers/media/usb/dvb-usb-v2/dvb_usb_core.c
@@ -82,8 +82,6 @@ static int dvb_usbv2_i2c_init(struct dvb_usb_device *d)
 	ret = i2c_add_adapter(&d->i2c_adap);
 	if (ret < 0) {
 		d->i2c_adap.algo = NULL;
-		dev_err(&d->udev->dev, "%s: i2c_add_adapter() failed=%d\n",
-				KBUILD_MODNAME, ret);
 		goto err;
 	}
 
diff --git a/drivers/media/usb/dvb-usb-v2/lmedm04.c b/drivers/media/usb/dvb-usb-v2/lmedm04.c
index 3721ee63b8fb..0e8fb89896c4 100644
--- a/drivers/media/usb/dvb-usb-v2/lmedm04.c
+++ b/drivers/media/usb/dvb-usb-v2/lmedm04.c
@@ -357,7 +357,8 @@ static void lme2510_int_response(struct urb *lme_urb)
 						ibuf[5]);
 
 			deb_info(1, "INT Key = 0x%08x", key);
-			rc_keydown(adap_to_d(adap)->rc_dev, RC_TYPE_NEC, key, 0);
+			rc_keydown(adap_to_d(adap)->rc_dev, RC_TYPE_NEC32, key,
+									0);
 			break;
 		case 0xbb:
 			switch (st->tuner_config) {
@@ -1242,7 +1243,7 @@ static int lme2510_get_stream_config(struct dvb_frontend *fe, u8 *ts_type,
 static int lme2510_get_rc_config(struct dvb_usb_device *d,
 	struct dvb_usb_rc *rc)
 {
-	rc->allowed_protos = RC_BIT_NEC;
+	rc->allowed_protos = RC_BIT_NEC32;
 	return 0;
 }
 
diff --git a/drivers/media/usb/dvb-usb-v2/mxl111sf-demod.h b/drivers/media/usb/dvb-usb-v2/mxl111sf-demod.h
index 7065aca81252..e6eae9d88e9f 100644
--- a/drivers/media/usb/dvb-usb-v2/mxl111sf-demod.h
+++ b/drivers/media/usb/dvb-usb-v2/mxl111sf-demod.h
@@ -21,7 +21,6 @@
 #ifndef __MXL111SF_DEMOD_H__
 #define __MXL111SF_DEMOD_H__
 
-#include <linux/kconfig.h>
 #include "dvb_frontend.h"
 #include "mxl111sf.h"
 
diff --git a/drivers/media/usb/dvb-usb-v2/mxl111sf-tuner.c b/drivers/media/usb/dvb-usb-v2/mxl111sf-tuner.c
index 7d16252dbb71..f141dcc55cc9 100644
--- a/drivers/media/usb/dvb-usb-v2/mxl111sf-tuner.c
+++ b/drivers/media/usb/dvb-usb-v2/mxl111sf-tuner.c
@@ -466,7 +466,7 @@ static int mxl111sf_tuner_release(struct dvb_frontend *fe)
 
 /* ------------------------------------------------------------------------- */
 
-static struct dvb_tuner_ops mxl111sf_tuner_tuner_ops = {
+static const struct dvb_tuner_ops mxl111sf_tuner_tuner_ops = {
 	.info = {
 		.name = "MaxLinear MxL111SF",
 #if 0
diff --git a/drivers/media/usb/dvb-usb-v2/mxl111sf-tuner.h b/drivers/media/usb/dvb-usb-v2/mxl111sf-tuner.h
index 509b55071218..e96d9a444ed1 100644
--- a/drivers/media/usb/dvb-usb-v2/mxl111sf-tuner.h
+++ b/drivers/media/usb/dvb-usb-v2/mxl111sf-tuner.h
@@ -21,7 +21,6 @@
 #ifndef __MXL111SF_TUNER_H__
 #define __MXL111SF_TUNER_H__
 
-#include <linux/kconfig.h>
 #include "dvb_frontend.h"
 #include "mxl111sf.h"
 
diff --git a/drivers/media/usb/dvb-usb-v2/rtl28xxu.c b/drivers/media/usb/dvb-usb-v2/rtl28xxu.c
index 6643762a9ff7..c583c638e468 100644
--- a/drivers/media/usb/dvb-usb-v2/rtl28xxu.c
+++ b/drivers/media/usb/dvb-usb-v2/rtl28xxu.c
@@ -1631,22 +1631,27 @@ static int rtl2831u_rc_query(struct dvb_usb_device *d)
 		goto err;
 
 	if (buf[4] & 0x01) {
+		enum rc_type proto;
+
 		if (buf[2] == (u8) ~buf[3]) {
 			if (buf[0] == (u8) ~buf[1]) {
 				/* NEC standard (16 bit) */
 				rc_code = RC_SCANCODE_NEC(buf[0], buf[2]);
+				proto = RC_TYPE_NEC;
 			} else {
 				/* NEC extended (24 bit) */
 				rc_code = RC_SCANCODE_NECX(buf[0] << 8 | buf[1],
 							   buf[2]);
+				proto = RC_TYPE_NECX;
 			}
 		} else {
 			/* NEC full (32 bit) */
 			rc_code = RC_SCANCODE_NEC32(buf[0] << 24 | buf[1] << 16 |
 						    buf[2] << 8  | buf[3]);
+			proto = RC_TYPE_NEC32;
 		}
 
-		rc_keydown(d->rc_dev, RC_TYPE_NEC, rc_code, 0);
+		rc_keydown(d->rc_dev, proto, rc_code, 0);
 
 		ret = rtl28xxu_wr_reg(d, SYS_IRRC_SR, 1);
 		if (ret)
@@ -1668,7 +1673,7 @@ static int rtl2831u_get_rc_config(struct dvb_usb_device *d,
 		struct dvb_usb_rc *rc)
 {
 	rc->map_name = RC_MAP_EMPTY;
-	rc->allowed_protos = RC_BIT_NEC;
+	rc->allowed_protos = RC_BIT_NEC | RC_BIT_NECX | RC_BIT_NEC32;
 	rc->query = rtl2831u_rc_query;
 	rc->interval = 400;
 
diff --git a/drivers/media/usb/dvb-usb/Kconfig b/drivers/media/usb/dvb-usb/Kconfig
index f03b0b70c901..959fa09dfd92 100644
--- a/drivers/media/usb/dvb-usb/Kconfig
+++ b/drivers/media/usb/dvb-usb/Kconfig
@@ -20,10 +20,20 @@ config DVB_USB_DEBUG
 	  Say Y if you want to enable debugging. See modinfo dvb-usb (and the
 	  appropriate drivers) for debug levels.
 
+config DVB_USB_DIB3000MC
+	tristate
+	depends on DVB_USB
+	select DVB_DIB3000MC
+	help
+	  This is a module with helper functions for accessing the
+	  DIB3000MC from USB DVB devices. It must be a separate module
+	  in case DVB_USB is built-in and DVB_DIB3000MC is a module,
+	  and gets selected automatically when needed.
+
 config DVB_USB_A800
 	tristate "AVerMedia AverTV DVB-T USB 2.0 (A800)"
 	depends on DVB_USB
-	select DVB_DIB3000MC
+	select DVB_USB_DIB3000MC
 	select DVB_PLL if MEDIA_SUBDRV_AUTOSELECT
 	select MEDIA_TUNER_MT2060 if MEDIA_SUBDRV_AUTOSELECT
 	help
@@ -34,6 +44,7 @@ config DVB_USB_DIBUSB_MB
 	depends on DVB_USB
 	select DVB_PLL if MEDIA_SUBDRV_AUTOSELECT
 	select DVB_DIB3000MB
+	depends on DVB_DIB3000MC || !DVB_DIB3000MC
 	select MEDIA_TUNER_MT2060 if MEDIA_SUBDRV_AUTOSELECT
 	help
 	  Support for USB 1.1 and 2.0 DVB-T receivers based on reference designs made by
@@ -54,7 +65,7 @@ config DVB_USB_DIBUSB_MB_FAULTY
 config DVB_USB_DIBUSB_MC
 	tristate "DiBcom USB DVB-T devices (based on the DiB3000M-C/P) (see help for device list)"
 	depends on DVB_USB
-	select DVB_DIB3000MC
+	select DVB_USB_DIB3000MC
 	select MEDIA_TUNER_MT2060 if MEDIA_SUBDRV_AUTOSELECT
 	help
 	  Support for USB2.0 DVB-T receivers based on reference designs made by
@@ -72,7 +83,7 @@ config DVB_USB_DIB0700
 	select DVB_DIB7000P if MEDIA_SUBDRV_AUTOSELECT
 	select DVB_DIB7000M if MEDIA_SUBDRV_AUTOSELECT
 	select DVB_DIB8000 if MEDIA_SUBDRV_AUTOSELECT
-	select DVB_DIB3000MC if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_USB_DIB3000MC if MEDIA_SUBDRV_AUTOSELECT
 	select DVB_S5H1411 if MEDIA_SUBDRV_AUTOSELECT
 	select DVB_LGDT3305 if MEDIA_SUBDRV_AUTOSELECT
 	select DVB_TUNER_DIB0070 if MEDIA_SUBDRV_AUTOSELECT
@@ -99,7 +110,7 @@ config DVB_USB_UMT_010
 	tristate "HanfTek UMT-010 DVB-T USB2.0 support"
 	depends on DVB_USB
 	select DVB_PLL if MEDIA_SUBDRV_AUTOSELECT
-	select DVB_DIB3000MC
+	select DVB_USB_DIB3000MC
 	select MEDIA_TUNER_MT2060 if MEDIA_SUBDRV_AUTOSELECT
 	select DVB_MT352 if MEDIA_SUBDRV_AUTOSELECT
 	help
@@ -192,7 +203,7 @@ config DVB_USB_GP8PSK
 config DVB_USB_NOVA_T_USB2
 	tristate "Hauppauge WinTV-NOVA-T usb2 DVB-T USB2.0 support"
 	depends on DVB_USB
-	select DVB_DIB3000MC
+	select DVB_USB_DIB3000MC
 	select DVB_PLL if MEDIA_SUBDRV_AUTOSELECT
 	select MEDIA_TUNER_MT2060 if MEDIA_SUBDRV_AUTOSELECT
 	help
diff --git a/drivers/media/usb/dvb-usb/Makefile b/drivers/media/usb/dvb-usb/Makefile
index acdd1efd4e74..2a7b5a963acf 100644
--- a/drivers/media/usb/dvb-usb/Makefile
+++ b/drivers/media/usb/dvb-usb/Makefile
@@ -16,20 +16,23 @@ obj-$(CONFIG_DVB_USB_DTT200U) += dvb-usb-dtt200u.o
 
 dvb-usb-dibusb-common-objs := dibusb-common.o
 
+dvb-usb-dibusb-mc-common-objs := dibusb-mc-common.o
+obj-$(CONFIG_DVB_USB_DIB3000MC)	+= dvb-usb-dibusb-common.o dvb-usb-dibusb-mc-common.o
+
 dvb-usb-a800-objs := a800.o
-obj-$(CONFIG_DVB_USB_A800) += dvb-usb-dibusb-common.o dvb-usb-a800.o
+obj-$(CONFIG_DVB_USB_A800) += dvb-usb-a800.o
 
 dvb-usb-dibusb-mb-objs := dibusb-mb.o
 obj-$(CONFIG_DVB_USB_DIBUSB_MB) += dvb-usb-dibusb-common.o dvb-usb-dibusb-mb.o
 
 dvb-usb-dibusb-mc-objs := dibusb-mc.o
-obj-$(CONFIG_DVB_USB_DIBUSB_MC) += dvb-usb-dibusb-common.o dvb-usb-dibusb-mc.o
+obj-$(CONFIG_DVB_USB_DIBUSB_MC) += dvb-usb-dibusb-mc.o
 
 dvb-usb-nova-t-usb2-objs := nova-t-usb2.o
-obj-$(CONFIG_DVB_USB_NOVA_T_USB2) += dvb-usb-dibusb-common.o dvb-usb-nova-t-usb2.o
+obj-$(CONFIG_DVB_USB_NOVA_T_USB2) += dvb-usb-nova-t-usb2.o
 
 dvb-usb-umt-010-objs := umt-010.o
-obj-$(CONFIG_DVB_USB_UMT_010) += dvb-usb-dibusb-common.o dvb-usb-umt-010.o
+obj-$(CONFIG_DVB_USB_UMT_010) += dvb-usb-umt-010.o
 
 dvb-usb-m920x-objs := m920x.o
 obj-$(CONFIG_DVB_USB_M920X) += dvb-usb-m920x.o
diff --git a/drivers/media/usb/dvb-usb/dib0700_core.c b/drivers/media/usb/dvb-usb/dib0700_core.c
index bf890c3d9cda..f3196658fb70 100644
--- a/drivers/media/usb/dvb-usb/dib0700_core.c
+++ b/drivers/media/usb/dvb-usb/dib0700_core.c
@@ -710,7 +710,6 @@ static void dib0700_rc_urb_completion(struct urb *purb)
 
 	switch (d->props.rc.core.protocol) {
 	case RC_BIT_NEC:
-		protocol = RC_TYPE_NEC;
 		toggle = 0;
 
 		/* NEC protocol sends repeat code as 0 0 0 FF */
@@ -728,16 +727,19 @@ static void dib0700_rc_urb_completion(struct urb *purb)
 						     poll_reply->nec.not_system << 16 |
 						     poll_reply->nec.data       << 8  |
 						     poll_reply->nec.not_data);
+			protocol = RC_TYPE_NEC32;
 		} else if ((poll_reply->nec.system ^ poll_reply->nec.not_system) != 0xff) {
 			deb_data("NEC extended protocol\n");
 			keycode = RC_SCANCODE_NECX(poll_reply->nec.system << 8 |
 						    poll_reply->nec.not_system,
 						    poll_reply->nec.data);
 
+			protocol = RC_TYPE_NECX;
 		} else {
 			deb_data("NEC normal protocol\n");
 			keycode = RC_SCANCODE_NEC(poll_reply->nec.system,
 						   poll_reply->nec.data);
+			protocol = RC_TYPE_NEC;
 		}
 
 		break;
@@ -783,10 +785,8 @@ int dib0700_rc_setup(struct dvb_usb_device *d, struct usb_interface *intf)
 	/* Starting in firmware 1.20, the RC info is provided on a bulk pipe */
 
 	purb = usb_alloc_urb(0, GFP_KERNEL);
-	if (purb == NULL) {
-		err("rc usb alloc urb failed");
+	if (purb == NULL)
 		return -ENOMEM;
-	}
 
 	purb->transfer_buffer = kzalloc(RC_MSG_SIZE_V1_20, GFP_KERNEL);
 	if (purb->transfer_buffer == NULL) {
diff --git a/drivers/media/usb/dvb-usb/dibusb-common.c b/drivers/media/usb/dvb-usb/dibusb-common.c
index 6eea4e68891d..18ed3bfbb5e2 100644
--- a/drivers/media/usb/dvb-usb/dibusb-common.c
+++ b/drivers/media/usb/dvb-usb/dibusb-common.c
@@ -9,7 +9,6 @@
  * see Documentation/dvb/README.dvb-usb for more information
  */
 
-#include <linux/kconfig.h>
 #include "dibusb.h"
 
 /* Max transfer size done by I2C transfer functions */
@@ -184,164 +183,6 @@ int dibusb_read_eeprom_byte(struct dvb_usb_device *d, u8 offs, u8 *val)
 }
 EXPORT_SYMBOL(dibusb_read_eeprom_byte);
 
-#if IS_ENABLED(CONFIG_DVB_DIB3000MC)
-
-/* 3000MC/P stuff */
-// Config Adjacent channels  Perf -cal22
-static struct dibx000_agc_config dib3000p_mt2060_agc_config = {
-	.band_caps = BAND_VHF | BAND_UHF,
-	.setup     = (1 << 8) | (5 << 5) | (1 << 4) | (1 << 3) | (0 << 2) | (2 << 0),
-
-	.agc1_max = 48497,
-	.agc1_min = 23593,
-	.agc2_max = 46531,
-	.agc2_min = 24904,
-
-	.agc1_pt1 = 0x65,
-	.agc1_pt2 = 0x69,
-
-	.agc1_slope1 = 0x51,
-	.agc1_slope2 = 0x27,
-
-	.agc2_pt1 = 0,
-	.agc2_pt2 = 0x33,
-
-	.agc2_slope1 = 0x35,
-	.agc2_slope2 = 0x37,
-};
-
-static struct dib3000mc_config stk3000p_dib3000p_config = {
-	&dib3000p_mt2060_agc_config,
-
-	.max_time     = 0x196,
-	.ln_adc_level = 0x1cc7,
-
-	.output_mpeg2_in_188_bytes = 1,
-
-	.agc_command1 = 1,
-	.agc_command2 = 1,
-};
-
-static struct dibx000_agc_config dib3000p_panasonic_agc_config = {
-	.band_caps = BAND_VHF | BAND_UHF,
-	.setup     = (1 << 8) | (5 << 5) | (1 << 4) | (1 << 3) | (0 << 2) | (2 << 0),
-
-	.agc1_max = 56361,
-	.agc1_min = 22282,
-	.agc2_max = 47841,
-	.agc2_min = 36045,
-
-	.agc1_pt1 = 0x3b,
-	.agc1_pt2 = 0x6b,
-
-	.agc1_slope1 = 0x55,
-	.agc1_slope2 = 0x1d,
-
-	.agc2_pt1 = 0,
-	.agc2_pt2 = 0x0a,
-
-	.agc2_slope1 = 0x95,
-	.agc2_slope2 = 0x1e,
-};
-
-static struct dib3000mc_config mod3000p_dib3000p_config = {
-	&dib3000p_panasonic_agc_config,
-
-	.max_time     = 0x51,
-	.ln_adc_level = 0x1cc7,
-
-	.output_mpeg2_in_188_bytes = 1,
-
-	.agc_command1 = 1,
-	.agc_command2 = 1,
-};
-
-int dibusb_dib3000mc_frontend_attach(struct dvb_usb_adapter *adap)
-{
-	if (le16_to_cpu(adap->dev->udev->descriptor.idVendor) == USB_VID_LITEON &&
-	    le16_to_cpu(adap->dev->udev->descriptor.idProduct) ==
-			USB_PID_LITEON_DVB_T_WARM) {
-		msleep(1000);
-	}
-
-	adap->fe_adap[0].fe = dvb_attach(dib3000mc_attach,
-					 &adap->dev->i2c_adap,
-					 DEFAULT_DIB3000P_I2C_ADDRESS,
-					 &mod3000p_dib3000p_config);
-	if ((adap->fe_adap[0].fe) == NULL)
-		adap->fe_adap[0].fe = dvb_attach(dib3000mc_attach,
-						 &adap->dev->i2c_adap,
-						 DEFAULT_DIB3000MC_I2C_ADDRESS,
-						 &mod3000p_dib3000p_config);
-	if ((adap->fe_adap[0].fe) != NULL) {
-		if (adap->priv != NULL) {
-			struct dibusb_state *st = adap->priv;
-			st->ops.pid_parse = dib3000mc_pid_parse;
-			st->ops.pid_ctrl  = dib3000mc_pid_control;
-		}
-		return 0;
-	}
-	return -ENODEV;
-}
-EXPORT_SYMBOL(dibusb_dib3000mc_frontend_attach);
-
-static struct mt2060_config stk3000p_mt2060_config = {
-	0x60
-};
-
-int dibusb_dib3000mc_tuner_attach(struct dvb_usb_adapter *adap)
-{
-	struct dibusb_state *st = adap->priv;
-	u8 a,b;
-	u16 if1 = 1220;
-	struct i2c_adapter *tun_i2c;
-
-	// First IF calibration for Liteon Sticks
-	if (le16_to_cpu(adap->dev->udev->descriptor.idVendor) == USB_VID_LITEON &&
-	    le16_to_cpu(adap->dev->udev->descriptor.idProduct) == USB_PID_LITEON_DVB_T_WARM) {
-
-		dibusb_read_eeprom_byte(adap->dev,0x7E,&a);
-		dibusb_read_eeprom_byte(adap->dev,0x7F,&b);
-
-		if (a == 0x00)
-			if1 += b;
-		else if (a == 0x80)
-			if1 -= b;
-		else
-			warn("LITE-ON DVB-T: Strange IF1 calibration :%2X %2X\n", a, b);
-
-	} else if (le16_to_cpu(adap->dev->udev->descriptor.idVendor) == USB_VID_DIBCOM &&
-		   le16_to_cpu(adap->dev->udev->descriptor.idProduct) == USB_PID_DIBCOM_MOD3001_WARM) {
-		u8 desc;
-		dibusb_read_eeprom_byte(adap->dev, 7, &desc);
-		if (desc == 2) {
-			a = 127;
-			do {
-				dibusb_read_eeprom_byte(adap->dev, a, &desc);
-				a--;
-			} while (a > 7 && (desc == 0xff || desc == 0x00));
-			if (desc & 0x80)
-				if1 -= (0xff - desc);
-			else
-				if1 += desc;
-		}
-	}
-
-	tun_i2c = dib3000mc_get_tuner_i2c_master(adap->fe_adap[0].fe, 1);
-	if (dvb_attach(mt2060_attach, adap->fe_adap[0].fe, tun_i2c, &stk3000p_mt2060_config, if1) == NULL) {
-		/* not found - use panasonic pll parameters */
-		if (dvb_attach(dvb_pll_attach, adap->fe_adap[0].fe, 0x60, tun_i2c, DVB_PLL_ENV57H1XD5) == NULL)
-			return -ENOMEM;
-	} else {
-		st->mt2060_present = 1;
-		/* set the correct parameters for the dib3000p */
-		dib3000mc_set_config(adap->fe_adap[0].fe, &stk3000p_dib3000p_config);
-	}
-	return 0;
-}
-EXPORT_SYMBOL(dibusb_dib3000mc_tuner_attach);
-#endif
-
 /*
  * common remote control stuff
  */
diff --git a/drivers/media/usb/dvb-usb/dibusb-mc-common.c b/drivers/media/usb/dvb-usb/dibusb-mc-common.c
new file mode 100644
index 000000000000..d66f56cc46a5
--- /dev/null
+++ b/drivers/media/usb/dvb-usb/dibusb-mc-common.c
@@ -0,0 +1,168 @@
+/* Common methods for dibusb-based-receivers.
+ *
+ * Copyright (C) 2004-5 Patrick Boettcher (patrick.boettcher@desy.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.
+ *
+ * see Documentation/dvb/README.dvb-usb for more information
+ */
+
+#include <linux/kconfig.h>
+#include "dibusb.h"
+
+/* 3000MC/P stuff */
+// Config Adjacent channels  Perf -cal22
+static struct dibx000_agc_config dib3000p_mt2060_agc_config = {
+	.band_caps = BAND_VHF | BAND_UHF,
+	.setup     = (1 << 8) | (5 << 5) | (1 << 4) | (1 << 3) | (0 << 2) | (2 << 0),
+
+	.agc1_max = 48497,
+	.agc1_min = 23593,
+	.agc2_max = 46531,
+	.agc2_min = 24904,
+
+	.agc1_pt1 = 0x65,
+	.agc1_pt2 = 0x69,
+
+	.agc1_slope1 = 0x51,
+	.agc1_slope2 = 0x27,
+
+	.agc2_pt1 = 0,
+	.agc2_pt2 = 0x33,
+
+	.agc2_slope1 = 0x35,
+	.agc2_slope2 = 0x37,
+};
+
+static struct dib3000mc_config stk3000p_dib3000p_config = {
+	&dib3000p_mt2060_agc_config,
+
+	.max_time     = 0x196,
+	.ln_adc_level = 0x1cc7,
+
+	.output_mpeg2_in_188_bytes = 1,
+
+	.agc_command1 = 1,
+	.agc_command2 = 1,
+};
+
+static struct dibx000_agc_config dib3000p_panasonic_agc_config = {
+	.band_caps = BAND_VHF | BAND_UHF,
+	.setup     = (1 << 8) | (5 << 5) | (1 << 4) | (1 << 3) | (0 << 2) | (2 << 0),
+
+	.agc1_max = 56361,
+	.agc1_min = 22282,
+	.agc2_max = 47841,
+	.agc2_min = 36045,
+
+	.agc1_pt1 = 0x3b,
+	.agc1_pt2 = 0x6b,
+
+	.agc1_slope1 = 0x55,
+	.agc1_slope2 = 0x1d,
+
+	.agc2_pt1 = 0,
+	.agc2_pt2 = 0x0a,
+
+	.agc2_slope1 = 0x95,
+	.agc2_slope2 = 0x1e,
+};
+
+static struct dib3000mc_config mod3000p_dib3000p_config = {
+	&dib3000p_panasonic_agc_config,
+
+	.max_time     = 0x51,
+	.ln_adc_level = 0x1cc7,
+
+	.output_mpeg2_in_188_bytes = 1,
+
+	.agc_command1 = 1,
+	.agc_command2 = 1,
+};
+
+int dibusb_dib3000mc_frontend_attach(struct dvb_usb_adapter *adap)
+{
+	if (le16_to_cpu(adap->dev->udev->descriptor.idVendor) == USB_VID_LITEON &&
+	    le16_to_cpu(adap->dev->udev->descriptor.idProduct) ==
+			USB_PID_LITEON_DVB_T_WARM) {
+		msleep(1000);
+	}
+
+	adap->fe_adap[0].fe = dvb_attach(dib3000mc_attach,
+					 &adap->dev->i2c_adap,
+					 DEFAULT_DIB3000P_I2C_ADDRESS,
+					 &mod3000p_dib3000p_config);
+	if ((adap->fe_adap[0].fe) == NULL)
+		adap->fe_adap[0].fe = dvb_attach(dib3000mc_attach,
+						 &adap->dev->i2c_adap,
+						 DEFAULT_DIB3000MC_I2C_ADDRESS,
+						 &mod3000p_dib3000p_config);
+	if ((adap->fe_adap[0].fe) != NULL) {
+		if (adap->priv != NULL) {
+			struct dibusb_state *st = adap->priv;
+			st->ops.pid_parse = dib3000mc_pid_parse;
+			st->ops.pid_ctrl  = dib3000mc_pid_control;
+		}
+		return 0;
+	}
+	return -ENODEV;
+}
+EXPORT_SYMBOL(dibusb_dib3000mc_frontend_attach);
+
+static struct mt2060_config stk3000p_mt2060_config = {
+	0x60
+};
+
+int dibusb_dib3000mc_tuner_attach(struct dvb_usb_adapter *adap)
+{
+	struct dibusb_state *st = adap->priv;
+	u8 a,b;
+	u16 if1 = 1220;
+	struct i2c_adapter *tun_i2c;
+
+	// First IF calibration for Liteon Sticks
+	if (le16_to_cpu(adap->dev->udev->descriptor.idVendor) == USB_VID_LITEON &&
+	    le16_to_cpu(adap->dev->udev->descriptor.idProduct) == USB_PID_LITEON_DVB_T_WARM) {
+
+		dibusb_read_eeprom_byte(adap->dev,0x7E,&a);
+		dibusb_read_eeprom_byte(adap->dev,0x7F,&b);
+
+		if (a == 0x00)
+			if1 += b;
+		else if (a == 0x80)
+			if1 -= b;
+		else
+			warn("LITE-ON DVB-T: Strange IF1 calibration :%2X %2X\n", a, b);
+
+	} else if (le16_to_cpu(adap->dev->udev->descriptor.idVendor) == USB_VID_DIBCOM &&
+		   le16_to_cpu(adap->dev->udev->descriptor.idProduct) == USB_PID_DIBCOM_MOD3001_WARM) {
+		u8 desc;
+		dibusb_read_eeprom_byte(adap->dev, 7, &desc);
+		if (desc == 2) {
+			a = 127;
+			do {
+				dibusb_read_eeprom_byte(adap->dev, a, &desc);
+				a--;
+			} while (a > 7 && (desc == 0xff || desc == 0x00));
+			if (desc & 0x80)
+				if1 -= (0xff - desc);
+			else
+				if1 += desc;
+		}
+	}
+
+	tun_i2c = dib3000mc_get_tuner_i2c_master(adap->fe_adap[0].fe, 1);
+	if (dvb_attach(mt2060_attach, adap->fe_adap[0].fe, tun_i2c, &stk3000p_mt2060_config, if1) == NULL) {
+		/* not found - use panasonic pll parameters */
+		if (dvb_attach(dvb_pll_attach, adap->fe_adap[0].fe, 0x60, tun_i2c, DVB_PLL_ENV57H1XD5) == NULL)
+			return -ENOMEM;
+	} else {
+		st->mt2060_present = 1;
+		/* set the correct parameters for the dib3000p */
+		dib3000mc_set_config(adap->fe_adap[0].fe, &stk3000p_dib3000p_config);
+	}
+	return 0;
+}
+EXPORT_SYMBOL(dibusb_dib3000mc_tuner_attach);
diff --git a/drivers/media/usb/dvb-usb/dtt200u.c b/drivers/media/usb/dvb-usb/dtt200u.c
index be633ece4194..d2a01b50af0d 100644
--- a/drivers/media/usb/dvb-usb/dtt200u.c
+++ b/drivers/media/usb/dvb-usb/dtt200u.c
@@ -62,18 +62,21 @@ static int dtt200u_rc_query(struct dvb_usb_device *d)
 
 	dvb_usb_generic_rw(d,&cmd,1,key,5,0);
 	if (key[0] == 1) {
+		enum rc_type proto = RC_TYPE_NEC;
+
 		scancode = key[1];
 		if ((u8) ~key[1] != key[2]) {
 			/* Extended NEC */
 			scancode = scancode << 8;
 			scancode |= key[2];
+			proto = RC_TYPE_NECX;
 		}
 		scancode = scancode << 8;
 		scancode |= key[3];
 
 		/* Check command checksum is ok */
 		if ((u8) ~key[3] == key[4])
-			rc_keydown(d->rc_dev, RC_TYPE_NEC, scancode, 0);
+			rc_keydown(d->rc_dev, proto, scancode, 0);
 		else
 			rc_keyup(d->rc_dev);
 	} else if (key[0] == 2) {
diff --git a/drivers/media/usb/em28xx/em28xx-audio.c b/drivers/media/usb/em28xx/em28xx-audio.c
index 49a5f9532bd8..e11fe46a547c 100644
--- a/drivers/media/usb/em28xx/em28xx-audio.c
+++ b/drivers/media/usb/em28xx/em28xx-audio.c
@@ -695,7 +695,7 @@ static int em28xx_cvol_new(struct snd_card *card, struct em28xx *dev,
 /*
  * register/unregister code and data
  */
-static struct snd_pcm_ops snd_em28xx_pcm_capture = {
+static const struct snd_pcm_ops snd_em28xx_pcm_capture = {
 	.open      = snd_em28xx_capture_open,
 	.close     = snd_em28xx_pcm_close,
 	.ioctl     = snd_pcm_lib_ioctl,
@@ -850,7 +850,6 @@ static int em28xx_audio_urb_init(struct em28xx *dev)
 
 		urb = usb_alloc_urb(npackets, GFP_ATOMIC);
 		if (!urb) {
-			em28xx_errdev("usb_alloc_urb failed!\n");
 			em28xx_audio_free_urb(dev);
 			return -ENOMEM;
 		}
diff --git a/drivers/media/usb/em28xx/em28xx-core.c b/drivers/media/usb/em28xx/em28xx-core.c
index 37456079f490..eebd5d7088d0 100644
--- a/drivers/media/usb/em28xx/em28xx-core.c
+++ b/drivers/media/usb/em28xx/em28xx-core.c
@@ -934,7 +934,6 @@ int em28xx_alloc_urbs(struct em28xx *dev, enum em28xx_mode mode, int xfer_bulk,
 	for (i = 0; i < usb_bufs->num_bufs; i++) {
 		urb = usb_alloc_urb(usb_bufs->num_packets, GFP_KERNEL);
 		if (!urb) {
-			em28xx_err("cannot alloc usb_ctl.urb %i\n", i);
 			em28xx_uninit_usb_xfer(dev, mode);
 			return -ENOMEM;
 		}
diff --git a/drivers/media/usb/em28xx/em28xx-i2c.c b/drivers/media/usb/em28xx/em28xx-i2c.c
index 1a9e1e556706..8b690ac908a4 100644
--- a/drivers/media/usb/em28xx/em28xx-i2c.c
+++ b/drivers/media/usb/em28xx/em28xx-i2c.c
@@ -855,7 +855,7 @@ static u32 functionality(struct i2c_adapter *i2c_adap)
 	return 0;
 }
 
-static struct i2c_algorithm em28xx_algo = {
+static const struct i2c_algorithm em28xx_algo = {
 	.master_xfer   = em28xx_i2c_xfer,
 	.functionality = functionality,
 };
diff --git a/drivers/media/usb/em28xx/em28xx-video.c b/drivers/media/usb/em28xx/em28xx-video.c
index 7968695217f3..1f7fa059eb34 100644
--- a/drivers/media/usb/em28xx/em28xx-video.c
+++ b/drivers/media/usb/em28xx/em28xx-video.c
@@ -1204,7 +1204,7 @@ buffer_queue(struct vb2_buffer *vb)
 	spin_unlock_irqrestore(&dev->slock, flags);
 }
 
-static struct vb2_ops em28xx_video_qops = {
+static const struct vb2_ops em28xx_video_qops = {
 	.queue_setup    = queue_setup,
 	.buf_prepare    = buffer_prepare,
 	.buf_queue      = buffer_queue,
diff --git a/drivers/media/usb/go7007/go7007-i2c.c b/drivers/media/usb/go7007/go7007-i2c.c
index 55addfa855d4..c084bf794b56 100644
--- a/drivers/media/usb/go7007/go7007-i2c.c
+++ b/drivers/media/usb/go7007/go7007-i2c.c
@@ -191,7 +191,7 @@ static u32 go7007_functionality(struct i2c_adapter *adapter)
 	return I2C_FUNC_SMBUS_BYTE_DATA;
 }
 
-static struct i2c_algorithm go7007_algo = {
+static const struct i2c_algorithm go7007_algo = {
 	.smbus_xfer	= go7007_smbus_xfer,
 	.master_xfer	= go7007_i2c_master_xfer,
 	.functionality	= go7007_functionality,
diff --git a/drivers/media/usb/go7007/go7007-usb.c b/drivers/media/usb/go7007/go7007-usb.c
index 14d3f8c1ce4a..ed9bcaf08d5e 100644
--- a/drivers/media/usb/go7007/go7007-usb.c
+++ b/drivers/media/usb/go7007/go7007-usb.c
@@ -1032,7 +1032,7 @@ static u32 go7007_usb_functionality(struct i2c_adapter *adapter)
 	return (I2C_FUNC_SMBUS_EMUL) & ~I2C_FUNC_SMBUS_QUICK;
 }
 
-static struct i2c_algorithm go7007_usb_algo = {
+static const struct i2c_algorithm go7007_usb_algo = {
 	.master_xfer	= go7007_usb_i2c_master_xfer,
 	.functionality	= go7007_usb_functionality,
 };
diff --git a/drivers/media/usb/go7007/go7007-v4l2.c b/drivers/media/usb/go7007/go7007-v4l2.c
index af8458996d91..4eaba0c24629 100644
--- a/drivers/media/usb/go7007/go7007-v4l2.c
+++ b/drivers/media/usb/go7007/go7007-v4l2.c
@@ -477,7 +477,7 @@ static void go7007_stop_streaming(struct vb2_queue *q)
 		go7007_write_addr(go, 0x3c82, 0x000d);
 }
 
-static struct vb2_ops go7007_video_qops = {
+static const struct vb2_ops go7007_video_qops = {
 	.queue_setup    = go7007_queue_setup,
 	.buf_queue      = go7007_buf_queue,
 	.buf_prepare    = go7007_buf_prepare,
diff --git a/drivers/media/usb/go7007/snd-go7007.c b/drivers/media/usb/go7007/snd-go7007.c
index d22d7d574672..070871fb1fc4 100644
--- a/drivers/media/usb/go7007/snd-go7007.c
+++ b/drivers/media/usb/go7007/snd-go7007.c
@@ -198,7 +198,7 @@ static struct page *go7007_snd_pcm_page(struct snd_pcm_substream *substream,
 	return vmalloc_to_page(substream->runtime->dma_area + offset);
 }
 
-static struct snd_pcm_ops go7007_snd_capture_ops = {
+static const struct snd_pcm_ops go7007_snd_capture_ops = {
 	.open		= go7007_snd_capture_open,
 	.close		= go7007_snd_capture_close,
 	.ioctl		= snd_pcm_lib_ioctl,
diff --git a/drivers/media/usb/gspca/benq.c b/drivers/media/usb/gspca/benq.c
index 790baed33963..5fa67b78ad49 100644
--- a/drivers/media/usb/gspca/benq.c
+++ b/drivers/media/usb/gspca/benq.c
@@ -95,10 +95,8 @@ static int sd_start(struct gspca_dev *gspca_dev)
 #define SD_NPKT 32
 	for (n = 0; n < 4; n++) {
 		urb = usb_alloc_urb(SD_NPKT, GFP_KERNEL);
-		if (!urb) {
-			pr_err("usb_alloc_urb failed\n");
+		if (!urb)
 			return -ENOMEM;
-		}
 		gspca_dev->urb[n] = urb;
 		urb->transfer_buffer = usb_alloc_coherent(gspca_dev->dev,
 						SD_PKT_SZ * SD_NPKT,
diff --git a/drivers/media/usb/gspca/finepix.c b/drivers/media/usb/gspca/finepix.c
index 52bdb569760b..ae9a55d7bbbb 100644
--- a/drivers/media/usb/gspca/finepix.c
+++ b/drivers/media/usb/gspca/finepix.c
@@ -41,7 +41,6 @@ struct usb_fpix {
 	struct gspca_dev gspca_dev;	/* !! must be the first item */
 
 	struct work_struct work_struct;
-	struct workqueue_struct *work_thread;
 };
 
 /* Delay after which claim the next frame. If the delay is too small,
@@ -226,9 +225,7 @@ static int sd_start(struct gspca_dev *gspca_dev)
 	/* Again, reset bulk in endpoint */
 	usb_clear_halt(gspca_dev->dev, gspca_dev->urb[0]->pipe);
 
-	/* Start the workqueue function to do the streaming */
-	dev->work_thread = create_singlethread_workqueue(MODULE_NAME);
-	queue_work(dev->work_thread, &dev->work_struct);
+	schedule_work(&dev->work_struct);
 
 	return 0;
 }
@@ -241,9 +238,8 @@ static void sd_stop0(struct gspca_dev *gspca_dev)
 
 	/* wait for the work queue to terminate */
 	mutex_unlock(&gspca_dev->usb_lock);
-	destroy_workqueue(dev->work_thread);
+	flush_work(&dev->work_struct);
 	mutex_lock(&gspca_dev->usb_lock);
-	dev->work_thread = NULL;
 }
 
 /* Table of supported USB devices */
diff --git a/drivers/media/usb/gspca/gspca.c b/drivers/media/usb/gspca/gspca.c
index b17bd7ebcb47..af2395a76d8b 100644
--- a/drivers/media/usb/gspca/gspca.c
+++ b/drivers/media/usb/gspca/gspca.c
@@ -795,10 +795,8 @@ static int create_urbs(struct gspca_dev *gspca_dev,
 
 	for (n = 0; n < nurbs; n++) {
 		urb = usb_alloc_urb(npkt, GFP_KERNEL);
-		if (!urb) {
-			pr_err("usb_alloc_urb failed\n");
+		if (!urb)
 			return -ENOMEM;
-		}
 		gspca_dev->urb[n] = urb;
 		urb->transfer_buffer = usb_alloc_coherent(gspca_dev->dev,
 						bsize,
diff --git a/drivers/media/usb/gspca/jl2005bcd.c b/drivers/media/usb/gspca/jl2005bcd.c
index 5b481fa43099..ac295f04bd18 100644
--- a/drivers/media/usb/gspca/jl2005bcd.c
+++ b/drivers/media/usb/gspca/jl2005bcd.c
@@ -45,7 +45,6 @@ struct sd {
 	const struct v4l2_pix_format *cap_mode;
 	/* Driver stuff */
 	struct work_struct work_struct;
-	struct workqueue_struct *work_thread;
 	u8 frame_brightness;
 	int block_size;	/* block size of camera */
 	int vga;	/* 1 if vga cam, 0 if cif cam */
@@ -477,9 +476,7 @@ static int sd_start(struct gspca_dev *gspca_dev)
 		return -1;
 	}
 
-	/* Start the workqueue function to do the streaming */
-	sd->work_thread = create_singlethread_workqueue(MODULE_NAME);
-	queue_work(sd->work_thread, &sd->work_struct);
+	schedule_work(&sd->work_struct);
 
 	return 0;
 }
@@ -493,8 +490,7 @@ static void sd_stop0(struct gspca_dev *gspca_dev)
 	/* wait for the work queue to terminate */
 	mutex_unlock(&gspca_dev->usb_lock);
 	/* This waits for sq905c_dostream to finish */
-	destroy_workqueue(dev->work_thread);
-	dev->work_thread = NULL;
+	flush_work(&dev->work_struct);
 	mutex_lock(&gspca_dev->usb_lock);
 }
 
diff --git a/drivers/media/usb/gspca/konica.c b/drivers/media/usb/gspca/konica.c
index 0712b1bc90b4..40aaaa9c5f30 100644
--- a/drivers/media/usb/gspca/konica.c
+++ b/drivers/media/usb/gspca/konica.c
@@ -208,10 +208,8 @@ static int sd_start(struct gspca_dev *gspca_dev)
 		packet_size =
 			le16_to_cpu(alt->endpoint[i].desc.wMaxPacketSize);
 		urb = usb_alloc_urb(SD_NPKT, GFP_KERNEL);
-		if (!urb) {
-			pr_err("usb_alloc_urb failed\n");
+		if (!urb)
 			return -ENOMEM;
-		}
 		gspca_dev->urb[n] = urb;
 		urb->transfer_buffer = usb_alloc_coherent(gspca_dev->dev,
 						packet_size * SD_NPKT,
diff --git a/drivers/media/usb/gspca/sonixj.c b/drivers/media/usb/gspca/sonixj.c
index fd1c8706d86a..d49d76ec1421 100644
--- a/drivers/media/usb/gspca/sonixj.c
+++ b/drivers/media/usb/gspca/sonixj.c
@@ -54,7 +54,6 @@ struct sd {
 	u32 exposure;
 
 	struct work_struct work;
-	struct workqueue_struct *work_thread;
 
 	u32 pktsz;			/* (used by pkt_scan) */
 	u16 npkt;
@@ -2485,7 +2484,6 @@ static int sd_start(struct gspca_dev *gspca_dev)
 
 	sd->pktsz = sd->npkt = 0;
 	sd->nchg = sd->short_mark = 0;
-	sd->work_thread = create_singlethread_workqueue(MODULE_NAME);
 
 	return gspca_dev->usb_err;
 }
@@ -2569,12 +2567,9 @@ static void sd_stop0(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 
-	if (sd->work_thread != NULL) {
-		mutex_unlock(&gspca_dev->usb_lock);
-		destroy_workqueue(sd->work_thread);
-		mutex_lock(&gspca_dev->usb_lock);
-		sd->work_thread = NULL;
-	}
+	mutex_unlock(&gspca_dev->usb_lock);
+	flush_work(&sd->work);
+	mutex_lock(&gspca_dev->usb_lock);
 }
 
 static void do_autogain(struct gspca_dev *gspca_dev)
@@ -2785,7 +2780,7 @@ marker_found:
 				new_qual = QUALITY_MAX;
 			if (new_qual != sd->quality) {
 				sd->quality = new_qual;
-				queue_work(sd->work_thread, &sd->work);
+				schedule_work(&sd->work);
 			}
 		}
 	} else {
diff --git a/drivers/media/usb/gspca/vicam.c b/drivers/media/usb/gspca/vicam.c
index 103f6c4236b0..8860510c2f9c 100644
--- a/drivers/media/usb/gspca/vicam.c
+++ b/drivers/media/usb/gspca/vicam.c
@@ -47,7 +47,6 @@ MODULE_FIRMWARE(VICAM_FIRMWARE);
 struct sd {
 	struct gspca_dev gspca_dev;	/* !! must be the first item */
 	struct work_struct work_struct;
-	struct workqueue_struct *work_thread;
 };
 
 /* The vicam sensor has a resolution of 512 x 244, with I believe square
@@ -278,9 +277,7 @@ static int sd_start(struct gspca_dev *gspca_dev)
 	if (ret < 0)
 		return ret;
 
-	/* Start the workqueue function to do the streaming */
-	sd->work_thread = create_singlethread_workqueue(MODULE_NAME);
-	queue_work(sd->work_thread, &sd->work_struct);
+	schedule_work(&sd->work_struct);
 
 	return 0;
 }
@@ -294,8 +291,7 @@ static void sd_stop0(struct gspca_dev *gspca_dev)
 	/* wait for the work queue to terminate */
 	mutex_unlock(&gspca_dev->usb_lock);
 	/* This waits for vicam_dostream to finish */
-	destroy_workqueue(dev->work_thread);
-	dev->work_thread = NULL;
+	flush_work(&dev->work_struct);
 	mutex_lock(&gspca_dev->usb_lock);
 
 	if (gspca_dev->present)
diff --git a/drivers/media/usb/hackrf/hackrf.c b/drivers/media/usb/hackrf/hackrf.c
index b1e229a44192..d9a525260511 100644
--- a/drivers/media/usb/hackrf/hackrf.c
+++ b/drivers/media/usb/hackrf/hackrf.c
@@ -129,7 +129,7 @@ struct hackrf_dev {
 	struct list_head rx_buffer_list;
 	struct list_head tx_buffer_list;
 	spinlock_t buffer_list_lock; /* Protects buffer_list */
-	unsigned sequence;	     /* Buffer sequence counter */
+	unsigned int sequence;	     /* Buffer sequence counter */
 	unsigned int vb_full;        /* vb is full and packets dropped */
 	unsigned int vb_empty;       /* vb is empty and packets dropped */
 
@@ -691,7 +691,6 @@ static int hackrf_alloc_urbs(struct hackrf_dev *dev, bool rcv)
 		dev_dbg(dev->dev, "alloc urb=%d\n", i);
 		dev->urb_list[i] = usb_alloc_urb(0, GFP_ATOMIC);
 		if (!dev->urb_list[i]) {
-			dev_dbg(dev->dev, "failed\n");
 			for (j = 0; j < i; j++)
 				usb_free_urb(dev->urb_list[j]);
 			return -ENOMEM;
@@ -892,7 +891,7 @@ static void hackrf_stop_streaming(struct vb2_queue *vq)
 	mutex_unlock(&dev->v4l2_lock);
 }
 
-static struct vb2_ops hackrf_vb2_ops = {
+static const struct vb2_ops hackrf_vb2_ops = {
 	.queue_setup            = hackrf_queue_setup,
 	.buf_queue              = hackrf_buf_queue,
 	.start_streaming        = hackrf_start_streaming,
diff --git a/drivers/media/usb/hdpvr/hdpvr-i2c.c b/drivers/media/usb/hdpvr/hdpvr-i2c.c
index a38f58c4c6bf..9b641c4d4431 100644
--- a/drivers/media/usb/hdpvr/hdpvr-i2c.c
+++ b/drivers/media/usb/hdpvr/hdpvr-i2c.c
@@ -55,7 +55,7 @@ struct i2c_client *hdpvr_register_ir_rx_i2c(struct hdpvr_device *dev)
 	/* Our default information for ir-kbd-i2c.c to use */
 	init_data->ir_codes = RC_MAP_HAUPPAUGE;
 	init_data->internal_get_key_func = IR_KBD_GET_KEY_HAUP_XVR;
-	init_data->type = RC_BIT_RC5;
+	init_data->type = RC_BIT_RC5 | RC_BIT_RC6_MCE | RC_BIT_RC6_6A_32;
 	init_data->name = "HD-PVR";
 	init_data->polling_interval = 405; /* ms, duplicated from Windows */
 	hdpvr_ir_rx_i2c_board_info.platform_data = init_data;
@@ -180,7 +180,7 @@ static u32 hdpvr_functionality(struct i2c_adapter *adapter)
 	return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL;
 }
 
-static struct i2c_algorithm hdpvr_algo = {
+static const struct i2c_algorithm hdpvr_algo = {
 	.master_xfer   = hdpvr_transfer,
 	.functionality = hdpvr_functionality,
 };
diff --git a/drivers/media/usb/hdpvr/hdpvr-video.c b/drivers/media/usb/hdpvr/hdpvr-video.c
index 2a3a8b470555..474c11e1d495 100644
--- a/drivers/media/usb/hdpvr/hdpvr-video.c
+++ b/drivers/media/usb/hdpvr/hdpvr-video.c
@@ -10,7 +10,6 @@
  */
 
 #include <linux/kernel.h>
-#include <linux/kconfig.h>
 #include <linux/errno.h>
 #include <linux/init.h>
 #include <linux/slab.h>
@@ -155,10 +154,8 @@ int hdpvr_alloc_buffers(struct hdpvr_device *dev, uint count)
 		buf->dev = dev;
 
 		urb = usb_alloc_urb(0, GFP_KERNEL);
-		if (!urb) {
-			v4l2_err(&dev->v4l2_dev, "cannot allocate urb\n");
+		if (!urb)
 			goto exit_urb;
-		}
 		buf->urb = urb;
 
 		mem = usb_alloc_coherent(dev->udev, dev->bulk_in_size, GFP_KERNEL,
diff --git a/drivers/media/usb/msi2500/msi2500.c b/drivers/media/usb/msi2500/msi2500.c
index e7f167d44c61..bb3d31e2a0b5 100644
--- a/drivers/media/usb/msi2500/msi2500.c
+++ b/drivers/media/usb/msi2500/msi2500.c
@@ -509,7 +509,6 @@ static int msi2500_isoc_init(struct msi2500_dev *dev)
 	for (i = 0; i < MAX_ISO_BUFS; i++) {
 		urb = usb_alloc_urb(ISO_FRAMES_PER_DESC, GFP_KERNEL);
 		if (urb == NULL) {
-			dev_err(dev->dev, "Failed to allocate urb %d\n", i);
 			msi2500_isoc_cleanup(dev);
 			return -ENOMEM;
 		}
@@ -898,7 +897,7 @@ static void msi2500_stop_streaming(struct vb2_queue *vq)
 	mutex_unlock(&dev->v4l2_lock);
 }
 
-static struct vb2_ops msi2500_vb2_ops = {
+static const struct vb2_ops msi2500_vb2_ops = {
 	.queue_setup            = msi2500_queue_setup,
 	.buf_queue              = msi2500_buf_queue,
 	.start_streaming        = msi2500_start_streaming,
diff --git a/drivers/media/usb/pvrusb2/pvrusb2-hdw-internal.h b/drivers/media/usb/pvrusb2/pvrusb2-hdw-internal.h
index 60141b16d731..23473a21319c 100644
--- a/drivers/media/usb/pvrusb2/pvrusb2-hdw-internal.h
+++ b/drivers/media/usb/pvrusb2/pvrusb2-hdw-internal.h
@@ -170,7 +170,6 @@ struct pvr2_hdw {
 	const struct pvr2_device_desc *hdw_desc;
 
 	/* Kernel worker thread handling */
-	struct workqueue_struct *workqueue;
 	struct work_struct workpoll;     /* Update driver state */
 
 	/* Video spigot */
diff --git a/drivers/media/usb/pvrusb2/pvrusb2-hdw.c b/drivers/media/usb/pvrusb2/pvrusb2-hdw.c
index fe20fe4f2330..1eb4f7ba2967 100644
--- a/drivers/media/usb/pvrusb2/pvrusb2-hdw.c
+++ b/drivers/media/usb/pvrusb2/pvrusb2-hdw.c
@@ -2624,7 +2624,6 @@ struct pvr2_hdw *pvr2_hdw_create(struct usb_interface *intf,
 	if (cnt1 >= sizeof(hdw->name)) cnt1 = sizeof(hdw->name)-1;
 	hdw->name[cnt1] = 0;
 
-	hdw->workqueue = create_singlethread_workqueue(hdw->name);
 	INIT_WORK(&hdw->workpoll,pvr2_hdw_worker_poll);
 
 	pvr2_trace(PVR2_TRACE_INIT,"Driver unit number is %d, name is %s",
@@ -2651,11 +2650,7 @@ struct pvr2_hdw *pvr2_hdw_create(struct usb_interface *intf,
 		del_timer_sync(&hdw->decoder_stabilization_timer);
 		del_timer_sync(&hdw->encoder_run_timer);
 		del_timer_sync(&hdw->encoder_wait_timer);
-		if (hdw->workqueue) {
-			flush_workqueue(hdw->workqueue);
-			destroy_workqueue(hdw->workqueue);
-			hdw->workqueue = NULL;
-		}
+		flush_work(&hdw->workpoll);
 		usb_free_urb(hdw->ctl_read_urb);
 		usb_free_urb(hdw->ctl_write_urb);
 		kfree(hdw->ctl_read_buffer);
@@ -2712,11 +2707,7 @@ void pvr2_hdw_destroy(struct pvr2_hdw *hdw)
 {
 	if (!hdw) return;
 	pvr2_trace(PVR2_TRACE_INIT,"pvr2_hdw_destroy: hdw=%p",hdw);
-	if (hdw->workqueue) {
-		flush_workqueue(hdw->workqueue);
-		destroy_workqueue(hdw->workqueue);
-		hdw->workqueue = NULL;
-	}
+	flush_work(&hdw->workpoll);
 	del_timer_sync(&hdw->quiescent_timer);
 	del_timer_sync(&hdw->decoder_stabilization_timer);
 	del_timer_sync(&hdw->encoder_run_timer);
@@ -4443,7 +4434,7 @@ static void pvr2_hdw_quiescent_timeout(unsigned long data)
 	hdw->state_decoder_quiescent = !0;
 	trace_stbit("state_decoder_quiescent",hdw->state_decoder_quiescent);
 	hdw->state_stale = !0;
-	queue_work(hdw->workqueue,&hdw->workpoll);
+	schedule_work(&hdw->workpoll);
 }
 
 
@@ -4454,7 +4445,7 @@ static void pvr2_hdw_decoder_stabilization_timeout(unsigned long data)
 	hdw->state_decoder_ready = !0;
 	trace_stbit("state_decoder_ready", hdw->state_decoder_ready);
 	hdw->state_stale = !0;
-	queue_work(hdw->workqueue, &hdw->workpoll);
+	schedule_work(&hdw->workpoll);
 }
 
 
@@ -4465,7 +4456,7 @@ static void pvr2_hdw_encoder_wait_timeout(unsigned long data)
 	hdw->state_encoder_waitok = !0;
 	trace_stbit("state_encoder_waitok",hdw->state_encoder_waitok);
 	hdw->state_stale = !0;
-	queue_work(hdw->workqueue,&hdw->workpoll);
+	schedule_work(&hdw->workpoll);
 }
 
 
@@ -4477,7 +4468,7 @@ static void pvr2_hdw_encoder_run_timeout(unsigned long data)
 		hdw->state_encoder_runok = !0;
 		trace_stbit("state_encoder_runok",hdw->state_encoder_runok);
 		hdw->state_stale = !0;
-		queue_work(hdw->workqueue,&hdw->workpoll);
+		schedule_work(&hdw->workpoll);
 	}
 }
 
@@ -4991,7 +4982,7 @@ static void pvr2_hdw_state_sched(struct pvr2_hdw *hdw)
 	if (hdw->state_stale) return;
 	hdw->state_stale = !0;
 	trace_stbit("state_stale",hdw->state_stale);
-	queue_work(hdw->workqueue,&hdw->workpoll);
+	schedule_work(&hdw->workpoll);
 }
 
 
diff --git a/drivers/media/usb/pvrusb2/pvrusb2-i2c-core.c b/drivers/media/usb/pvrusb2/pvrusb2-i2c-core.c
index 14321d0a1833..6da5fb544817 100644
--- a/drivers/media/usb/pvrusb2/pvrusb2-i2c-core.c
+++ b/drivers/media/usb/pvrusb2/pvrusb2-i2c-core.c
@@ -596,7 +596,8 @@ static void pvr2_i2c_register_ir(struct pvr2_hdw *hdw)
 	case PVR2_IR_SCHEME_24XXX_MCE: /* 24xxx MCE device */
 		init_data->ir_codes              = RC_MAP_HAUPPAUGE;
 		init_data->internal_get_key_func = IR_KBD_GET_KEY_HAUP_XVR;
-		init_data->type                  = RC_BIT_RC5;
+		init_data->type                  = RC_BIT_RC5 | RC_BIT_RC6_MCE |
+							RC_BIT_RC6_6A_32;
 		init_data->name                  = hdw->hdw_desc->description;
 		/* IR Receiver */
 		info.addr          = 0x71;
diff --git a/drivers/media/usb/pvrusb2/pvrusb2-v4l2.c b/drivers/media/usb/pvrusb2/pvrusb2-v4l2.c
index 81f788b7b242..2cc4d2b6f810 100644
--- a/drivers/media/usb/pvrusb2/pvrusb2-v4l2.c
+++ b/drivers/media/usb/pvrusb2/pvrusb2-v4l2.c
@@ -719,64 +719,85 @@ static int pvr2_cropcap(struct file *file, void *priv, struct v4l2_cropcap *cap)
 	return ret;
 }
 
-static int pvr2_g_crop(struct file *file, void *priv, struct v4l2_crop *crop)
+static int pvr2_g_selection(struct file *file, void *priv,
+			    struct v4l2_selection *sel)
 {
 	struct pvr2_v4l2_fh *fh = file->private_data;
 	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
+	struct v4l2_cropcap cap;
 	int val = 0;
 	int ret;
 
-	if (crop->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+	if (sel->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 		return -EINVAL;
-	ret = pvr2_ctrl_get_value(
-			pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_CROPL), &val);
-	if (ret != 0)
-		return -EINVAL;
-	crop->c.left = val;
-	ret = pvr2_ctrl_get_value(
-			pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_CROPT), &val);
-	if (ret != 0)
-		return -EINVAL;
-	crop->c.top = val;
-	ret = pvr2_ctrl_get_value(
-			pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_CROPW), &val);
-	if (ret != 0)
-		return -EINVAL;
-	crop->c.width = val;
-	ret = pvr2_ctrl_get_value(
-			pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_CROPH), &val);
-	if (ret != 0)
+
+	cap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP:
+		ret = pvr2_ctrl_get_value(
+			  pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_CROPL), &val);
+		if (ret != 0)
+			return -EINVAL;
+		sel->r.left = val;
+		ret = pvr2_ctrl_get_value(
+			  pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_CROPT), &val);
+		if (ret != 0)
+			return -EINVAL;
+		sel->r.top = val;
+		ret = pvr2_ctrl_get_value(
+			  pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_CROPW), &val);
+		if (ret != 0)
+			return -EINVAL;
+		sel->r.width = val;
+		ret = pvr2_ctrl_get_value(
+			  pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_CROPH), &val);
+		if (ret != 0)
+			return -EINVAL;
+		sel->r.height = val;
+		break;
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+		ret = pvr2_hdw_get_cropcap(hdw, &cap);
+		sel->r = cap.defrect;
+		break;
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		ret = pvr2_hdw_get_cropcap(hdw, &cap);
+		sel->r = cap.bounds;
+		break;
+	default:
 		return -EINVAL;
-	crop->c.height = val;
-	return 0;
+	}
+	return ret;
 }
 
-static int pvr2_s_crop(struct file *file, void *priv, const struct v4l2_crop *crop)
+static int pvr2_s_selection(struct file *file, void *priv,
+			    struct v4l2_selection *sel)
 {
 	struct pvr2_v4l2_fh *fh = file->private_data;
 	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
 	int ret;
 
-	if (crop->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+	if (sel->type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
+	    sel->target != V4L2_SEL_TGT_CROP)
 		return -EINVAL;
 	ret = pvr2_ctrl_set_value(
 			pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_CROPL),
-			crop->c.left);
+			sel->r.left);
 	if (ret != 0)
 		return -EINVAL;
 	ret = pvr2_ctrl_set_value(
 			pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_CROPT),
-			crop->c.top);
+			sel->r.top);
 	if (ret != 0)
 		return -EINVAL;
 	ret = pvr2_ctrl_set_value(
 			pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_CROPW),
-			crop->c.width);
+			sel->r.width);
 	if (ret != 0)
 		return -EINVAL;
 	ret = pvr2_ctrl_set_value(
 			pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_CROPH),
-			crop->c.height);
+			sel->r.height);
 	if (ret != 0)
 		return -EINVAL;
 	return 0;
@@ -798,8 +819,8 @@ static const struct v4l2_ioctl_ops pvr2_ioctl_ops = {
 	.vidioc_enumaudio		    = pvr2_enumaudio,
 	.vidioc_enum_input		    = pvr2_enum_input,
 	.vidioc_cropcap			    = pvr2_cropcap,
-	.vidioc_s_crop			    = pvr2_s_crop,
-	.vidioc_g_crop			    = pvr2_g_crop,
+	.vidioc_s_selection		    = pvr2_s_selection,
+	.vidioc_g_selection		    = pvr2_g_selection,
 	.vidioc_g_input			    = pvr2_g_input,
 	.vidioc_s_input			    = pvr2_s_input,
 	.vidioc_g_frequency		    = pvr2_g_frequency,
diff --git a/drivers/media/usb/pwc/pwc-if.c b/drivers/media/usb/pwc/pwc-if.c
index b51b27a3fd61..ff657644b6b3 100644
--- a/drivers/media/usb/pwc/pwc-if.c
+++ b/drivers/media/usb/pwc/pwc-if.c
@@ -410,7 +410,6 @@ retry:
 	for (i = 0; i < MAX_ISO_BUFS; i++) {
 		urb = usb_alloc_urb(ISO_FRAMES_PER_DESC, GFP_KERNEL);
 		if (urb == NULL) {
-			PWC_ERROR("Failed to allocate urb %d\n", i);
 			pwc_isoc_cleanup(pdev);
 			return -ENOMEM;
 		}
@@ -708,7 +707,7 @@ static void stop_streaming(struct vb2_queue *vq)
 	mutex_unlock(&pdev->v4l2_lock);
 }
 
-static struct vb2_ops pwc_vb_queue_ops = {
+static const struct vb2_ops pwc_vb_queue_ops = {
 	.queue_setup		= queue_setup,
 	.buf_init		= buffer_init,
 	.buf_prepare		= buffer_prepare,
diff --git a/drivers/media/usb/s2255/s2255drv.c b/drivers/media/usb/s2255/s2255drv.c
index 43ba71a7d02b..c3a0e87066eb 100644
--- a/drivers/media/usb/s2255/s2255drv.c
+++ b/drivers/media/usb/s2255/s2255drv.c
@@ -717,7 +717,7 @@ static void buffer_queue(struct vb2_buffer *vb)
 static int start_streaming(struct vb2_queue *vq, unsigned int count);
 static void stop_streaming(struct vb2_queue *vq);
 
-static struct vb2_ops s2255_video_qops = {
+static const struct vb2_ops s2255_video_qops = {
 	.queue_setup = queue_setup,
 	.buf_prepare = buffer_prepare,
 	.buf_queue = buffer_queue,
@@ -2113,11 +2113,8 @@ static int s2255_start_readpipe(struct s2255_dev *dev)
 	pipe_info->state = 1;
 	pipe_info->err_count = 0;
 	pipe_info->stream_urb = usb_alloc_urb(0, GFP_KERNEL);
-	if (!pipe_info->stream_urb) {
-		dev_err(&dev->udev->dev,
-			"ReadStream: Unable to alloc URB\n");
+	if (!pipe_info->stream_urb)
 		return -ENOMEM;
-	}
 	/* transfer buffer allocated in board_init */
 	usb_fill_bulk_urb(pipe_info->stream_urb, dev->udev,
 			  pipe,
@@ -2290,10 +2287,8 @@ static int s2255_probe(struct usb_interface *interface,
 	}
 
 	dev->fw_data->fw_urb = usb_alloc_urb(0, GFP_KERNEL);
-	if (!dev->fw_data->fw_urb) {
-		dev_err(&interface->dev, "out of memory!\n");
+	if (!dev->fw_data->fw_urb)
 		goto errorFWURB;
-	}
 
 	dev->fw_data->pfw_data = kzalloc(CHUNK_SIZE, GFP_KERNEL);
 	if (!dev->fw_data->pfw_data) {
diff --git a/drivers/media/usb/stk1160/stk1160-i2c.c b/drivers/media/usb/stk1160/stk1160-i2c.c
index 850cf285ada8..3f2517be02bb 100644
--- a/drivers/media/usb/stk1160/stk1160-i2c.c
+++ b/drivers/media/usb/stk1160/stk1160-i2c.c
@@ -235,7 +235,7 @@ static u32 functionality(struct i2c_adapter *adap)
 	return I2C_FUNC_SMBUS_EMUL;
 }
 
-static struct i2c_algorithm algo = {
+static const struct i2c_algorithm algo = {
 	.master_xfer   = stk1160_i2c_xfer,
 	.functionality = functionality,
 };
diff --git a/drivers/media/usb/stk1160/stk1160-v4l.c b/drivers/media/usb/stk1160/stk1160-v4l.c
index 5fab3bee8c74..a005d262392a 100644
--- a/drivers/media/usb/stk1160/stk1160-v4l.c
+++ b/drivers/media/usb/stk1160/stk1160-v4l.c
@@ -742,7 +742,7 @@ static void stop_streaming(struct vb2_queue *vq)
 	stk1160_stop_streaming(dev);
 }
 
-static struct vb2_ops stk1160_video_qops = {
+static const struct vb2_ops stk1160_video_qops = {
 	.queue_setup		= queue_setup,
 	.buf_queue		= buffer_queue,
 	.start_streaming	= start_streaming,
diff --git a/drivers/media/usb/stk1160/stk1160-video.c b/drivers/media/usb/stk1160/stk1160-video.c
index 6ecb0b48423f..ce8ebbe395a6 100644
--- a/drivers/media/usb/stk1160/stk1160-video.c
+++ b/drivers/media/usb/stk1160/stk1160-video.c
@@ -457,10 +457,8 @@ int stk1160_alloc_isoc(struct stk1160 *dev)
 	for (i = 0; i < num_bufs; i++) {
 
 		urb = usb_alloc_urb(max_packets, GFP_KERNEL);
-		if (!urb) {
-			stk1160_err("cannot alloc urb[%d]\n", i);
+		if (!urb)
 			goto free_i_bufs;
-		}
 		dev->isoc_ctl.urb[i] = urb;
 
 #ifndef CONFIG_DMA_NONCOHERENT
diff --git a/drivers/media/usb/stkwebcam/stk-webcam.c b/drivers/media/usb/stkwebcam/stk-webcam.c
index c21c4c004f97..db200c9d796d 100644
--- a/drivers/media/usb/stkwebcam/stk-webcam.c
+++ b/drivers/media/usb/stkwebcam/stk-webcam.c
@@ -452,10 +452,8 @@ static int stk_prepare_iso(struct stk_camera *dev)
 			STK_ERROR("isobuf data already allocated\n");
 		if (dev->isobufs[i].urb == NULL) {
 			urb = usb_alloc_urb(ISO_FRAMES_PER_DESC, GFP_KERNEL);
-			if (urb == NULL) {
-				STK_ERROR("Failed to allocate URB %d\n", i);
+			if (urb == NULL)
 				goto isobufs_out;
-			}
 			dev->isobufs[i].urb = urb;
 		} else {
 			STK_ERROR("Killing URB\n");
diff --git a/drivers/media/usb/tm6000/tm6000-alsa.c b/drivers/media/usb/tm6000/tm6000-alsa.c
index e21c7aacecb6..f16fbd1f9f51 100644
--- a/drivers/media/usb/tm6000/tm6000-alsa.c
+++ b/drivers/media/usb/tm6000/tm6000-alsa.c
@@ -388,7 +388,7 @@ static struct page *snd_pcm_get_vmalloc_page(struct snd_pcm_substream *subs,
 /*
  * operators
  */
-static struct snd_pcm_ops snd_tm6000_pcm_ops = {
+static const struct snd_pcm_ops snd_tm6000_pcm_ops = {
 	.open = snd_tm6000_pcm_open,
 	.close = snd_tm6000_close,
 	.ioctl = snd_pcm_lib_ioctl,
diff --git a/drivers/media/usb/tm6000/tm6000-dvb.c b/drivers/media/usb/tm6000/tm6000-dvb.c
index 095f5db1a790..0426b210383b 100644
--- a/drivers/media/usb/tm6000/tm6000-dvb.c
+++ b/drivers/media/usb/tm6000/tm6000-dvb.c
@@ -129,10 +129,8 @@ static int tm6000_start_stream(struct tm6000_core *dev)
 	}
 
 	dvb->bulk_urb = usb_alloc_urb(0, GFP_KERNEL);
-	if (dvb->bulk_urb == NULL) {
-		printk(KERN_ERR "tm6000: couldn't allocate urb\n");
+	if (dvb->bulk_urb == NULL)
 		return -ENOMEM;
-	}
 
 	pipe = usb_rcvbulkpipe(dev->udev, dev->bulk_in.endp->desc.bEndpointAddress
 							  & USB_ENDPOINT_NUMBER_MASK);
diff --git a/drivers/media/usb/tm6000/tm6000-video.c b/drivers/media/usb/tm6000/tm6000-video.c
index fa5e8bda2ae4..dee7e7d3d47d 100644
--- a/drivers/media/usb/tm6000/tm6000-video.c
+++ b/drivers/media/usb/tm6000/tm6000-video.c
@@ -635,7 +635,6 @@ static int tm6000_prepare_isoc(struct tm6000_core *dev)
 	for (i = 0; i < dev->isoc_ctl.num_bufs; i++) {
 		urb = usb_alloc_urb(max_packets, GFP_KERNEL);
 		if (!urb) {
-			tm6000_err("cannot alloc isoc_ctl.urb %i\n", i);
 			tm6000_uninit_isoc(dev);
 			usb_free_urb(urb);
 			return -ENOMEM;
diff --git a/drivers/media/usb/ttusb-dec/ttusb_dec.c b/drivers/media/usb/ttusb-dec/ttusb_dec.c
index 4e36e24cb3a6..4e7671a3a1e4 100644
--- a/drivers/media/usb/ttusb-dec/ttusb_dec.c
+++ b/drivers/media/usb/ttusb-dec/ttusb_dec.c
@@ -206,7 +206,7 @@ static void ttusb_dec_set_model(struct ttusb_dec *dec,
 
 static void ttusb_dec_handle_irq( struct urb *urb)
 {
-	struct ttusb_dec * dec = urb->context;
+	struct ttusb_dec *dec = urb->context;
 	char *buffer = dec->irq_buffer;
 	int retval;
 
@@ -227,25 +227,31 @@ static void ttusb_dec_handle_irq( struct urb *urb)
 			goto exit;
 	}
 
-	if( (buffer[0] == 0x1) && (buffer[2] == 0x15) )  {
-		/* IR - Event */
-		/* this is an fact a bit too simple implementation;
+	if ((buffer[0] == 0x1) && (buffer[2] == 0x15))  {
+		/*
+		 * IR - Event
+		 *
+		 * this is an fact a bit too simple implementation;
 		 * the box also reports a keyrepeat signal
 		 * (with buffer[3] == 0x40) in an intervall of ~100ms.
 		 * But to handle this correctly we had to imlemenent some
 		 * kind of timer which signals a 'key up' event if no
 		 * keyrepeat signal is received for lets say 200ms.
 		 * this should/could be added later ...
-		 * for now lets report each signal as a key down and up*/
-		dprintk("%s:rc signal:%d\n", __func__, buffer[4]);
-		input_report_key(dec->rc_input_dev, rc_keys[buffer[4] - 1], 1);
-		input_sync(dec->rc_input_dev);
-		input_report_key(dec->rc_input_dev, rc_keys[buffer[4] - 1], 0);
-		input_sync(dec->rc_input_dev);
+		 * for now lets report each signal as a key down and up
+		 */
+		if (buffer[4] - 1 < ARRAY_SIZE(rc_keys)) {
+			dprintk("%s:rc signal:%d\n", __func__, buffer[4]);
+			input_report_key(dec->rc_input_dev, rc_keys[buffer[4] - 1], 1);
+			input_sync(dec->rc_input_dev);
+			input_report_key(dec->rc_input_dev, rc_keys[buffer[4] - 1], 0);
+			input_sync(dec->rc_input_dev);
+		}
 	}
 
-exit:	retval = usb_submit_urb(urb, GFP_ATOMIC);
-	if(retval)
+exit:
+	retval = usb_submit_urb(urb, GFP_ATOMIC);
+	if (retval)
 		printk("%s - usb_commit_urb failed with result: %d\n",
 			__func__, retval);
 }
diff --git a/drivers/media/usb/usbtv/usbtv-audio.c b/drivers/media/usb/usbtv/usbtv-audio.c
index 1965ff1b1f12..9db31db7d9ac 100644
--- a/drivers/media/usb/usbtv/usbtv-audio.c
+++ b/drivers/media/usb/usbtv/usbtv-audio.c
@@ -332,7 +332,7 @@ static snd_pcm_uframes_t snd_usbtv_pointer(struct snd_pcm_substream *substream)
 	return chip->snd_buffer_pos;
 }
 
-static struct snd_pcm_ops snd_usbtv_pcm_ops = {
+static const struct snd_pcm_ops snd_usbtv_pcm_ops = {
 	.open = snd_usbtv_pcm_open,
 	.close = snd_usbtv_pcm_close,
 	.ioctl = snd_pcm_lib_ioctl,
diff --git a/drivers/media/usb/usbtv/usbtv-video.c b/drivers/media/usb/usbtv/usbtv-video.c
index 2a089756c988..6cbe4a245c9f 100644
--- a/drivers/media/usb/usbtv/usbtv-video.c
+++ b/drivers/media/usb/usbtv/usbtv-video.c
@@ -689,7 +689,7 @@ static void usbtv_stop_streaming(struct vb2_queue *vq)
 		usbtv_stop(usbtv);
 }
 
-static struct vb2_ops usbtv_vb2_ops = {
+static const struct vb2_ops usbtv_vb2_ops = {
 	.queue_setup = usbtv_queue_setup,
 	.buf_queue = usbtv_buf_queue,
 	.start_streaming = usbtv_start_streaming,
diff --git a/drivers/media/usb/usbvision/usbvision-core.c b/drivers/media/usb/usbvision/usbvision-core.c
index 52ac4391582c..c23bf73a68ea 100644
--- a/drivers/media/usb/usbvision/usbvision-core.c
+++ b/drivers/media/usb/usbvision/usbvision-core.c
@@ -2303,11 +2303,8 @@ int usbvision_init_isoc(struct usb_usbvision *usbvision)
 		struct urb *urb;
 
 		urb = usb_alloc_urb(USBVISION_URB_FRAMES, GFP_KERNEL);
-		if (urb == NULL) {
-			dev_err(&usbvision->dev->dev,
-				"%s: usb_alloc_urb() failed\n", __func__);
+		if (urb == NULL)
 			return -ENOMEM;
-		}
 		usbvision->sbuf[buf_idx].urb = urb;
 		usbvision->sbuf[buf_idx].data =
 			usb_alloc_coherent(usbvision->dev,
diff --git a/drivers/media/usb/uvc/uvc_queue.c b/drivers/media/usb/uvc/uvc_queue.c
index 773fefb52d7a..77edd206d345 100644
--- a/drivers/media/usb/uvc/uvc_queue.c
+++ b/drivers/media/usb/uvc/uvc_queue.c
@@ -177,7 +177,7 @@ static void uvc_stop_streaming(struct vb2_queue *vq)
 	spin_unlock_irqrestore(&queue->irqlock, flags);
 }
 
-static struct vb2_ops uvc_queue_qops = {
+static const struct vb2_ops uvc_queue_qops = {
 	.queue_setup = uvc_queue_setup,
 	.buf_prepare = uvc_buffer_prepare,
 	.buf_queue = uvc_buffer_queue,
diff --git a/drivers/media/usb/zr364xx/zr364xx.c b/drivers/media/usb/zr364xx/zr364xx.c
index 7433ba5c4bad..cc128db85723 100644
--- a/drivers/media/usb/zr364xx/zr364xx.c
+++ b/drivers/media/usb/zr364xx/zr364xx.c
@@ -1045,10 +1045,8 @@ static int zr364xx_start_readpipe(struct zr364xx_camera *cam)
 	pipe_info->state = 1;
 	pipe_info->err_count = 0;
 	pipe_info->stream_urb = usb_alloc_urb(0, GFP_KERNEL);
-	if (!pipe_info->stream_urb) {
-		dev_err(&cam->udev->dev, "ReadStream: Unable to alloc URB\n");
+	if (!pipe_info->stream_urb)
 		return -ENOMEM;
-	}
 	/* transfer buffer allocated in board_init */
 	usb_fill_bulk_urb(pipe_info->stream_urb, cam->udev,
 			  pipe,
diff --git a/drivers/media/v4l2-core/Kconfig b/drivers/media/v4l2-core/Kconfig
index 29b3436d0910..367523a3c774 100644
--- a/drivers/media/v4l2-core/Kconfig
+++ b/drivers/media/v4l2-core/Kconfig
@@ -27,7 +27,7 @@ config VIDEO_FIXED_MINOR_RANGES
 
 config VIDEO_PCI_SKELETON
 	tristate "Skeleton PCI V4L2 driver"
-	depends on PCI && BUILD_DOCSRC
+	depends on PCI
 	depends on VIDEO_V4L2 && VIDEOBUF2_CORE
 	depends on VIDEOBUF2_MEMOPS && VIDEOBUF2_DMA_CONTIG
 	---help---
diff --git a/drivers/media/v4l2-core/v4l2-async.c b/drivers/media/v4l2-core/v4l2-async.c
index a4b224d92572..5bada202b2d3 100644
--- a/drivers/media/v4l2-core/v4l2-async.c
+++ b/drivers/media/v4l2-core/v4l2-async.c
@@ -119,13 +119,6 @@ static int v4l2_async_test_notify(struct v4l2_async_notifier *notifier,
 		return ret;
 	}
 
-	ret = v4l2_subdev_call(sd, core, registered_async);
-	if (ret < 0 && ret != -ENOIOCTLCMD) {
-		if (notifier->unbind)
-			notifier->unbind(notifier, sd, asd);
-		return ret;
-	}
-
 	if (list_empty(&notifier->waiting) && notifier->complete)
 		return notifier->complete(notifier);
 
diff --git a/drivers/media/v4l2-core/v4l2-common.c b/drivers/media/v4l2-core/v4l2-common.c
index 5b808500e7e7..57cfe26a393f 100644
--- a/drivers/media/v4l2-core/v4l2-common.c
+++ b/drivers/media/v4l2-core/v4l2-common.c
@@ -291,7 +291,7 @@ struct v4l2_subdev *v4l2_spi_new_subdev(struct v4l2_device *v4l2_dev,
 error:
 	/* If we have a client but no subdev, then something went wrong and
 	   we must unregister the client. */
-	if (spi && sd == NULL)
+	if (!sd)
 		spi_unregister_device(spi);
 
 	return sd;
diff --git a/drivers/media/v4l2-core/v4l2-ctrls.c b/drivers/media/v4l2-core/v4l2-ctrls.c
index f7abfad9ad23..adc2147fcff7 100644
--- a/drivers/media/v4l2-core/v4l2-ctrls.c
+++ b/drivers/media/v4l2-core/v4l2-ctrls.c
@@ -361,6 +361,7 @@ const char * const *v4l2_ctrl_get_menu(u32 id)
 		"Scalable Baseline",
 		"Scalable High",
 		"Scalable High Intra",
+		"Stereo High",
 		"Multiview High",
 		NULL,
 	};
diff --git a/drivers/media/v4l2-core/v4l2-dev.c b/drivers/media/v4l2-core/v4l2-dev.c
index e6da353b39bc..8be561ab2615 100644
--- a/drivers/media/v4l2-core/v4l2-dev.c
+++ b/drivers/media/v4l2-core/v4l2-dev.c
@@ -527,6 +527,7 @@ static void determine_valid_ioctls(struct video_device *vdev)
 	bool is_vbi = vdev->vfl_type == VFL_TYPE_VBI;
 	bool is_radio = vdev->vfl_type == VFL_TYPE_RADIO;
 	bool is_sdr = vdev->vfl_type == VFL_TYPE_SDR;
+	bool is_tch = vdev->vfl_type == VFL_TYPE_TOUCH;
 	bool is_rx = vdev->vfl_dir != VFL_DIR_TX;
 	bool is_tx = vdev->vfl_dir != VFL_DIR_RX;
 
@@ -573,7 +574,7 @@ static void determine_valid_ioctls(struct video_device *vdev)
 	if (ops->vidioc_enum_freq_bands || ops->vidioc_g_tuner || ops->vidioc_g_modulator)
 		set_bit(_IOC_NR(VIDIOC_ENUM_FREQ_BANDS), valid_ioctls);
 
-	if (is_vid) {
+	if (is_vid || is_tch) {
 		/* video specific ioctls */
 		if ((is_rx && (ops->vidioc_enum_fmt_vid_cap ||
 			       ops->vidioc_enum_fmt_vid_cap_mplane ||
@@ -662,7 +663,7 @@ static void determine_valid_ioctls(struct video_device *vdev)
 			set_bit(_IOC_NR(VIDIOC_TRY_FMT), valid_ioctls);
 	}
 
-	if (is_vid || is_vbi || is_sdr) {
+	if (is_vid || is_vbi || is_sdr || is_tch) {
 		/* ioctls valid for video, vbi or sdr */
 		SET_VALID_IOCTL(ops, VIDIOC_REQBUFS, vidioc_reqbufs);
 		SET_VALID_IOCTL(ops, VIDIOC_QUERYBUF, vidioc_querybuf);
@@ -675,7 +676,7 @@ static void determine_valid_ioctls(struct video_device *vdev)
 		SET_VALID_IOCTL(ops, VIDIOC_STREAMOFF, vidioc_streamoff);
 	}
 
-	if (is_vid || is_vbi) {
+	if (is_vid || is_vbi || is_tch) {
 		/* ioctls valid for video or vbi */
 		if (ops->vidioc_s_std)
 			set_bit(_IOC_NR(VIDIOC_ENUMSTD), valid_ioctls);
@@ -751,6 +752,10 @@ static int video_register_media_controller(struct video_device *vdev, int type)
 		intf_type = MEDIA_INTF_T_V4L_SWRADIO;
 		vdev->entity.function = MEDIA_ENT_F_IO_SWRADIO;
 		break;
+	case VFL_TYPE_TOUCH:
+		intf_type = MEDIA_INTF_T_V4L_TOUCH;
+		vdev->entity.function = MEDIA_ENT_F_IO_V4L;
+		break;
 	case VFL_TYPE_RADIO:
 		intf_type = MEDIA_INTF_T_V4L_RADIO;
 		/*
@@ -854,6 +859,9 @@ int __video_register_device(struct video_device *vdev, int type, int nr,
 		/* Use device name 'swradio' because 'sdr' was already taken. */
 		name_base = "swradio";
 		break;
+	case VFL_TYPE_TOUCH:
+		name_base = "v4l-touch";
+		break;
 	default:
 		printk(KERN_ERR "%s called with unknown type: %d\n",
 		       __func__, type);
diff --git a/drivers/media/v4l2-core/v4l2-device.c b/drivers/media/v4l2-core/v4l2-device.c
index 06fa5f1b2cff..62bbed76dbbc 100644
--- a/drivers/media/v4l2-core/v4l2-device.c
+++ b/drivers/media/v4l2-core/v4l2-device.c
@@ -160,12 +160,9 @@ int v4l2_device_register_subdev(struct v4l2_device *v4l2_dev,
 	int err;
 
 	/* Check for valid input */
-	if (v4l2_dev == NULL || sd == NULL || !sd->name[0])
+	if (!v4l2_dev || !sd || sd->v4l2_dev || !sd->name[0])
 		return -EINVAL;
 
-	/* Warn if we apparently re-register a subdev */
-	WARN_ON(sd->v4l2_dev != NULL);
-
 	/*
 	 * The reason to acquire the module here is to avoid unloading
 	 * a module of sub-device which is registered to a media
diff --git a/drivers/media/v4l2-core/v4l2-dv-timings.c b/drivers/media/v4l2-core/v4l2-dv-timings.c
index 889de0a32152..730a7c392c1d 100644
--- a/drivers/media/v4l2-core/v4l2-dv-timings.c
+++ b/drivers/media/v4l2-core/v4l2-dv-timings.c
@@ -306,7 +306,7 @@ void v4l2_print_dv_timings(const char *dev_prefix, const char *prefix,
 			(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%s\n", dev_prefix, bt->flags,
+	pr_info("%s: flags (0x%x):%s%s%s%s%s%s%s\n", dev_prefix, bt->flags,
 			(bt->flags & V4L2_DV_FL_REDUCED_BLANKING) ?
 			" REDUCED_BLANKING" : "",
 			((bt->flags & V4L2_DV_FL_REDUCED_BLANKING) &&
@@ -318,12 +318,15 @@ void v4l2_print_dv_timings(const char *dev_prefix, const char *prefix,
 			(bt->flags & V4L2_DV_FL_HALF_LINE) ?
 			" HALF_LINE" : "",
 			(bt->flags & V4L2_DV_FL_IS_CE_VIDEO) ?
-			" CE_VIDEO" : "");
-	pr_info("%s: standards (0x%x):%s%s%s%s\n", dev_prefix, bt->standards,
+			" CE_VIDEO" : "",
+			(bt->flags & V4L2_DV_FL_FIRST_FIELD_EXTRA_LINE) ?
+			" FIRST_FIELD_EXTRA_LINE" : "");
+	pr_info("%s: standards (0x%x):%s%s%s%s%s\n", dev_prefix, bt->standards,
 			(bt->standards & V4L2_DV_BT_STD_CEA861) ?  " CEA" : "",
 			(bt->standards & V4L2_DV_BT_STD_DMT) ?  " DMT" : "",
 			(bt->standards & V4L2_DV_BT_STD_CVT) ?  " CVT" : "",
-			(bt->standards & V4L2_DV_BT_STD_GTF) ?  " GTF" : "");
+			(bt->standards & V4L2_DV_BT_STD_GTF) ?  " GTF" : "",
+			(bt->standards & V4L2_DV_BT_STD_SDI) ?  " SDI" : "");
 }
 EXPORT_SYMBOL_GPL(v4l2_print_dv_timings);
 
diff --git a/drivers/media/v4l2-core/v4l2-ioctl.c b/drivers/media/v4l2-core/v4l2-ioctl.c
index 51a0fa144392..c52d94c018bb 100644
--- a/drivers/media/v4l2-core/v4l2-ioctl.c
+++ b/drivers/media/v4l2-core/v4l2-ioctl.c
@@ -924,6 +924,7 @@ static int check_fmt(struct file *file, enum v4l2_buf_type type)
 	bool is_vid = vfd->vfl_type == VFL_TYPE_GRABBER;
 	bool is_vbi = vfd->vfl_type == VFL_TYPE_VBI;
 	bool is_sdr = vfd->vfl_type == VFL_TYPE_SDR;
+	bool is_tch = vfd->vfl_type == VFL_TYPE_TOUCH;
 	bool is_rx = vfd->vfl_dir != VFL_DIR_TX;
 	bool is_tx = vfd->vfl_dir != VFL_DIR_RX;
 
@@ -932,7 +933,7 @@ static int check_fmt(struct file *file, enum v4l2_buf_type type)
 
 	switch (type) {
 	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
-		if (is_vid && is_rx &&
+		if ((is_vid || is_tch) && is_rx &&
 		    (ops->vidioc_g_fmt_vid_cap || ops->vidioc_g_fmt_vid_cap_mplane))
 			return 0;
 		break;
@@ -1243,6 +1244,10 @@ static void v4l_fill_fmtdesc(struct v4l2_fmtdesc *fmt)
 	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;
+	case V4L2_TCH_FMT_DELTA_TD16:	descr = "16-bit signed deltas"; break;
+	case V4L2_TCH_FMT_DELTA_TD08:	descr = "8-bit signed deltas"; break;
+	case V4L2_TCH_FMT_TU16:		descr = "16-bit unsigned touch data"; break;
+	case V4L2_TCH_FMT_TU08:		descr = "8-bit unsigned touch data"; break;
 
 	default:
 		/* Compressed formats */
@@ -1309,13 +1314,14 @@ static int v4l_enum_fmt(const struct v4l2_ioctl_ops *ops,
 	struct video_device *vfd = video_devdata(file);
 	bool is_vid = vfd->vfl_type == VFL_TYPE_GRABBER;
 	bool is_sdr = vfd->vfl_type == VFL_TYPE_SDR;
+	bool is_tch = vfd->vfl_type == VFL_TYPE_TOUCH;
 	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))
+		if (unlikely(!is_rx || (!is_vid && !is_tch) || !ops->vidioc_enum_fmt_vid_cap))
 			break;
 		ret = ops->vidioc_enum_fmt_vid_cap(file, fh, arg);
 		break;
@@ -1362,6 +1368,7 @@ static int v4l_g_fmt(const struct v4l2_ioctl_ops *ops,
 	struct video_device *vfd = video_devdata(file);
 	bool is_vid = vfd->vfl_type == VFL_TYPE_GRABBER;
 	bool is_sdr = vfd->vfl_type == VFL_TYPE_SDR;
+	bool is_tch = vfd->vfl_type == VFL_TYPE_TOUCH;
 	bool is_rx = vfd->vfl_dir != VFL_DIR_TX;
 	bool is_tx = vfd->vfl_dir != VFL_DIR_RX;
 	int ret;
@@ -1392,7 +1399,7 @@ static int v4l_g_fmt(const struct v4l2_ioctl_ops *ops,
 
 	switch (p->type) {
 	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
-		if (unlikely(!is_rx || !is_vid || !ops->vidioc_g_fmt_vid_cap))
+		if (unlikely(!is_rx || (!is_vid && !is_tch) || !ops->vidioc_g_fmt_vid_cap))
 			break;
 		p->fmt.pix.priv = V4L2_PIX_FMT_PRIV_MAGIC;
 		ret = ops->vidioc_g_fmt_vid_cap(file, fh, arg);
@@ -1451,6 +1458,21 @@ static int v4l_g_fmt(const struct v4l2_ioctl_ops *ops,
 	return -EINVAL;
 }
 
+static void v4l_pix_format_touch(struct v4l2_pix_format *p)
+{
+	/*
+	 * The v4l2_pix_format structure contains fields that make no sense for
+	 * touch. Set them to default values in this case.
+	 */
+
+	p->field = V4L2_FIELD_NONE;
+	p->colorspace = V4L2_COLORSPACE_RAW;
+	p->flags = 0;
+	p->ycbcr_enc = 0;
+	p->quantization = 0;
+	p->xfer_func = 0;
+}
+
 static int v4l_s_fmt(const struct v4l2_ioctl_ops *ops,
 				struct file *file, void *fh, void *arg)
 {
@@ -1458,6 +1480,7 @@ static int v4l_s_fmt(const struct v4l2_ioctl_ops *ops,
 	struct video_device *vfd = video_devdata(file);
 	bool is_vid = vfd->vfl_type == VFL_TYPE_GRABBER;
 	bool is_sdr = vfd->vfl_type == VFL_TYPE_SDR;
+	bool is_tch = vfd->vfl_type == VFL_TYPE_TOUCH;
 	bool is_rx = vfd->vfl_dir != VFL_DIR_TX;
 	bool is_tx = vfd->vfl_dir != VFL_DIR_RX;
 	int ret;
@@ -1469,17 +1492,19 @@ static int v4l_s_fmt(const struct v4l2_ioctl_ops *ops,
 
 	switch (p->type) {
 	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
-		if (unlikely(!is_rx || !is_vid || !ops->vidioc_s_fmt_vid_cap))
+		if (unlikely(!is_rx || (!is_vid && !is_tch) || !ops->vidioc_s_fmt_vid_cap))
 			break;
 		CLEAR_AFTER_FIELD(p, fmt.pix);
 		ret = ops->vidioc_s_fmt_vid_cap(file, fh, arg);
 		/* just in case the driver zeroed it again */
 		p->fmt.pix.priv = V4L2_PIX_FMT_PRIV_MAGIC;
+		if (is_tch)
+			v4l_pix_format_touch(&p->fmt.pix);
 		return ret;
 	case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
 		if (unlikely(!is_rx || !is_vid || !ops->vidioc_s_fmt_vid_cap_mplane))
 			break;
-		CLEAR_AFTER_FIELD(p, fmt.pix_mp);
+		CLEAR_AFTER_FIELD(p, fmt.pix_mp.xfer_func);
 		return ops->vidioc_s_fmt_vid_cap_mplane(file, fh, arg);
 	case V4L2_BUF_TYPE_VIDEO_OVERLAY:
 		if (unlikely(!is_rx || !is_vid || !ops->vidioc_s_fmt_vid_overlay))
@@ -1507,7 +1532,7 @@ static int v4l_s_fmt(const struct v4l2_ioctl_ops *ops,
 	case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
 		if (unlikely(!is_tx || !is_vid || !ops->vidioc_s_fmt_vid_out_mplane))
 			break;
-		CLEAR_AFTER_FIELD(p, fmt.pix_mp);
+		CLEAR_AFTER_FIELD(p, fmt.pix_mp.xfer_func);
 		return ops->vidioc_s_fmt_vid_out_mplane(file, fh, arg);
 	case V4L2_BUF_TYPE_VIDEO_OUTPUT_OVERLAY:
 		if (unlikely(!is_tx || !is_vid || !ops->vidioc_s_fmt_vid_out_overlay))
@@ -1545,6 +1570,7 @@ static int v4l_try_fmt(const struct v4l2_ioctl_ops *ops,
 	struct video_device *vfd = video_devdata(file);
 	bool is_vid = vfd->vfl_type == VFL_TYPE_GRABBER;
 	bool is_sdr = vfd->vfl_type == VFL_TYPE_SDR;
+	bool is_tch = vfd->vfl_type == VFL_TYPE_TOUCH;
 	bool is_rx = vfd->vfl_dir != VFL_DIR_TX;
 	bool is_tx = vfd->vfl_dir != VFL_DIR_RX;
 	int ret;
@@ -1553,7 +1579,7 @@ static int v4l_try_fmt(const struct v4l2_ioctl_ops *ops,
 
 	switch (p->type) {
 	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
-		if (unlikely(!is_rx || !is_vid || !ops->vidioc_try_fmt_vid_cap))
+		if (unlikely(!is_rx || (!is_vid && !is_tch) || !ops->vidioc_try_fmt_vid_cap))
 			break;
 		CLEAR_AFTER_FIELD(p, fmt.pix);
 		ret = ops->vidioc_try_fmt_vid_cap(file, fh, arg);
@@ -1563,7 +1589,7 @@ static int v4l_try_fmt(const struct v4l2_ioctl_ops *ops,
 	case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
 		if (unlikely(!is_rx || !is_vid || !ops->vidioc_try_fmt_vid_cap_mplane))
 			break;
-		CLEAR_AFTER_FIELD(p, fmt.pix_mp);
+		CLEAR_AFTER_FIELD(p, fmt.pix_mp.xfer_func);
 		return ops->vidioc_try_fmt_vid_cap_mplane(file, fh, arg);
 	case V4L2_BUF_TYPE_VIDEO_OVERLAY:
 		if (unlikely(!is_rx || !is_vid || !ops->vidioc_try_fmt_vid_overlay))
@@ -1591,7 +1617,7 @@ static int v4l_try_fmt(const struct v4l2_ioctl_ops *ops,
 	case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
 		if (unlikely(!is_tx || !is_vid || !ops->vidioc_try_fmt_vid_out_mplane))
 			break;
-		CLEAR_AFTER_FIELD(p, fmt.pix_mp);
+		CLEAR_AFTER_FIELD(p, fmt.pix_mp.xfer_func);
 		return ops->vidioc_try_fmt_vid_out_mplane(file, fh, arg);
 	case V4L2_BUF_TYPE_VIDEO_OUTPUT_OVERLAY:
 		if (unlikely(!is_tx || !is_vid || !ops->vidioc_try_fmt_vid_out_overlay))
diff --git a/drivers/media/v4l2-core/v4l2-mem2mem.c b/drivers/media/v4l2-core/v4l2-mem2mem.c
index 61d56c940f80..6bc27e7b2a33 100644
--- a/drivers/media/v4l2-core/v4l2-mem2mem.c
+++ b/drivers/media/v4l2-core/v4l2-mem2mem.c
@@ -76,9 +76,6 @@ static struct v4l2_m2m_queue_ctx *get_queue_ctx(struct v4l2_m2m_ctx *m2m_ctx,
 		return &m2m_ctx->cap_q_ctx;
 }
 
-/**
- * v4l2_m2m_get_vq() - return vb2_queue for the given type
- */
 struct vb2_queue *v4l2_m2m_get_vq(struct v4l2_m2m_ctx *m2m_ctx,
 				       enum v4l2_buf_type type)
 {
@@ -92,9 +89,6 @@ struct vb2_queue *v4l2_m2m_get_vq(struct v4l2_m2m_ctx *m2m_ctx,
 }
 EXPORT_SYMBOL(v4l2_m2m_get_vq);
 
-/**
- * v4l2_m2m_next_buf() - return next buffer from the list of ready buffers
- */
 void *v4l2_m2m_next_buf(struct v4l2_m2m_queue_ctx *q_ctx)
 {
 	struct v4l2_m2m_buffer *b;
@@ -113,10 +107,6 @@ void *v4l2_m2m_next_buf(struct v4l2_m2m_queue_ctx *q_ctx)
 }
 EXPORT_SYMBOL_GPL(v4l2_m2m_next_buf);
 
-/**
- * v4l2_m2m_buf_remove() - take off a buffer from the list of ready buffers and
- * return it
- */
 void *v4l2_m2m_buf_remove(struct v4l2_m2m_queue_ctx *q_ctx)
 {
 	struct v4l2_m2m_buffer *b;
@@ -140,10 +130,6 @@ EXPORT_SYMBOL_GPL(v4l2_m2m_buf_remove);
  * Scheduling handlers
  */
 
-/**
- * v4l2_m2m_get_curr_priv() - return driver private data for the currently
- * running instance or NULL if no instance is running
- */
 void *v4l2_m2m_get_curr_priv(struct v4l2_m2m_dev *m2m_dev)
 {
 	unsigned long flags;
@@ -188,26 +174,6 @@ static void v4l2_m2m_try_run(struct v4l2_m2m_dev *m2m_dev)
 	m2m_dev->m2m_ops->device_run(m2m_dev->curr_ctx->priv);
 }
 
-/**
- * v4l2_m2m_try_schedule() - check whether an instance is ready to be added to
- * the pending job queue and add it if so.
- * @m2m_ctx:	m2m context assigned to the instance to be checked
- *
- * There are three basic requirements an instance has to meet to be able to run:
- * 1) at least one source buffer has to be queued,
- * 2) at least one destination buffer has to be queued,
- * 3) streaming has to be on.
- *
- * If a queue is buffered (for example a decoder hardware ringbuffer that has
- * to be drained before doing streamoff), allow scheduling without v4l2 buffers
- * on that queue.
- *
- * There may also be additional, custom requirements. In such case the driver
- * should supply a custom callback (job_ready in v4l2_m2m_ops) that should
- * return 1 if the instance is ready.
- * An example of the above could be an instance that requires more than one
- * src/dst buffer per transaction.
- */
 void v4l2_m2m_try_schedule(struct v4l2_m2m_ctx *m2m_ctx)
 {
 	struct v4l2_m2m_dev *m2m_dev;
@@ -311,18 +277,6 @@ static void v4l2_m2m_cancel_job(struct v4l2_m2m_ctx *m2m_ctx)
 	}
 }
 
-/**
- * v4l2_m2m_job_finish() - inform the framework that a job has been finished
- * and have it clean up
- *
- * Called by a driver to yield back the device after it has finished with it.
- * Should be called as soon as possible after reaching a state which allows
- * other instances to take control of the device.
- *
- * This function has to be called only after device_run() callback has been
- * called on the driver. To prevent recursion, it should not be called directly
- * from the device_run() callback though.
- */
 void v4l2_m2m_job_finish(struct v4l2_m2m_dev *m2m_dev,
 			 struct v4l2_m2m_ctx *m2m_ctx)
 {
@@ -350,9 +304,6 @@ void v4l2_m2m_job_finish(struct v4l2_m2m_dev *m2m_dev,
 }
 EXPORT_SYMBOL(v4l2_m2m_job_finish);
 
-/**
- * v4l2_m2m_reqbufs() - multi-queue-aware REQBUFS multiplexer
- */
 int v4l2_m2m_reqbufs(struct file *file, struct v4l2_m2m_ctx *m2m_ctx,
 		     struct v4l2_requestbuffers *reqbufs)
 {
@@ -370,11 +321,6 @@ int v4l2_m2m_reqbufs(struct file *file, struct v4l2_m2m_ctx *m2m_ctx,
 }
 EXPORT_SYMBOL_GPL(v4l2_m2m_reqbufs);
 
-/**
- * v4l2_m2m_querybuf() - multi-queue-aware QUERYBUF multiplexer
- *
- * See v4l2_m2m_mmap() documentation for details.
- */
 int v4l2_m2m_querybuf(struct file *file, struct v4l2_m2m_ctx *m2m_ctx,
 		      struct v4l2_buffer *buf)
 {
@@ -400,10 +346,6 @@ int v4l2_m2m_querybuf(struct file *file, struct v4l2_m2m_ctx *m2m_ctx,
 }
 EXPORT_SYMBOL_GPL(v4l2_m2m_querybuf);
 
-/**
- * v4l2_m2m_qbuf() - enqueue a source or destination buffer, depending on
- * the type
- */
 int v4l2_m2m_qbuf(struct file *file, struct v4l2_m2m_ctx *m2m_ctx,
 		  struct v4l2_buffer *buf)
 {
@@ -419,10 +361,6 @@ int v4l2_m2m_qbuf(struct file *file, struct v4l2_m2m_ctx *m2m_ctx,
 }
 EXPORT_SYMBOL_GPL(v4l2_m2m_qbuf);
 
-/**
- * v4l2_m2m_dqbuf() - dequeue a source or destination buffer, depending on
- * the type
- */
 int v4l2_m2m_dqbuf(struct file *file, struct v4l2_m2m_ctx *m2m_ctx,
 		   struct v4l2_buffer *buf)
 {
@@ -433,10 +371,6 @@ 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)
 {
@@ -452,10 +386,6 @@ int v4l2_m2m_prepare_buf(struct file *file, struct v4l2_m2m_ctx *m2m_ctx,
 }
 EXPORT_SYMBOL_GPL(v4l2_m2m_prepare_buf);
 
-/**
- * v4l2_m2m_create_bufs() - create a source or destination buffer, depending
- * on the type
- */
 int v4l2_m2m_create_bufs(struct file *file, struct v4l2_m2m_ctx *m2m_ctx,
 			 struct v4l2_create_buffers *create)
 {
@@ -466,10 +396,6 @@ int v4l2_m2m_create_bufs(struct file *file, struct v4l2_m2m_ctx *m2m_ctx,
 }
 EXPORT_SYMBOL_GPL(v4l2_m2m_create_bufs);
 
-/**
- * v4l2_m2m_expbuf() - export a source or destination buffer, depending on
- * the type
- */
 int v4l2_m2m_expbuf(struct file *file, struct v4l2_m2m_ctx *m2m_ctx,
 		  struct v4l2_exportbuffer *eb)
 {
@@ -479,9 +405,7 @@ int v4l2_m2m_expbuf(struct file *file, struct v4l2_m2m_ctx *m2m_ctx,
 	return vb2_expbuf(vq, eb);
 }
 EXPORT_SYMBOL_GPL(v4l2_m2m_expbuf);
-/**
- * v4l2_m2m_streamon() - turn on streaming for a video queue
- */
+
 int v4l2_m2m_streamon(struct file *file, struct v4l2_m2m_ctx *m2m_ctx,
 		      enum v4l2_buf_type type)
 {
@@ -497,9 +421,6 @@ int v4l2_m2m_streamon(struct file *file, struct v4l2_m2m_ctx *m2m_ctx,
 }
 EXPORT_SYMBOL_GPL(v4l2_m2m_streamon);
 
-/**
- * v4l2_m2m_streamoff() - turn off streaming for a video queue
- */
 int v4l2_m2m_streamoff(struct file *file, struct v4l2_m2m_ctx *m2m_ctx,
 		       enum v4l2_buf_type type)
 {
@@ -540,14 +461,6 @@ int v4l2_m2m_streamoff(struct file *file, struct v4l2_m2m_ctx *m2m_ctx,
 }
 EXPORT_SYMBOL_GPL(v4l2_m2m_streamoff);
 
-/**
- * v4l2_m2m_poll() - poll replacement, for destination buffers only
- *
- * Call from the driver's poll() function. Will poll both queues. If a buffer
- * is available to dequeue (with dqbuf) from the source queue, this will
- * indicate that a non-blocking write can be performed, while read will be
- * returned in case of the destination queue.
- */
 unsigned int v4l2_m2m_poll(struct file *file, struct v4l2_m2m_ctx *m2m_ctx,
 			   struct poll_table_struct *wait)
 {
@@ -626,16 +539,6 @@ end:
 }
 EXPORT_SYMBOL_GPL(v4l2_m2m_poll);
 
-/**
- * v4l2_m2m_mmap() - source and destination queues-aware mmap multiplexer
- *
- * Call from driver's mmap() function. Will handle mmap() for both queues
- * seamlessly for videobuffer, which will receive normal per-queue offsets and
- * proper videobuf queue pointers. The differentiation is made outside videobuf
- * by adding a predefined offset to buffers from one of the queues and
- * subtracting it before passing it back to videobuf. Only drivers (and
- * thus applications) receive modified offsets.
- */
 int v4l2_m2m_mmap(struct file *file, struct v4l2_m2m_ctx *m2m_ctx,
 			 struct vm_area_struct *vma)
 {
@@ -653,11 +556,6 @@ int v4l2_m2m_mmap(struct file *file, struct v4l2_m2m_ctx *m2m_ctx,
 }
 EXPORT_SYMBOL(v4l2_m2m_mmap);
 
-/**
- * v4l2_m2m_init() - initialize per-driver m2m data
- *
- * Usually called from driver's probe() function.
- */
 struct v4l2_m2m_dev *v4l2_m2m_init(const struct v4l2_m2m_ops *m2m_ops)
 {
 	struct v4l2_m2m_dev *m2m_dev;
@@ -679,26 +577,12 @@ struct v4l2_m2m_dev *v4l2_m2m_init(const struct v4l2_m2m_ops *m2m_ops)
 }
 EXPORT_SYMBOL_GPL(v4l2_m2m_init);
 
-/**
- * v4l2_m2m_release() - cleans up and frees a m2m_dev structure
- *
- * Usually called from driver's remove() function.
- */
 void v4l2_m2m_release(struct v4l2_m2m_dev *m2m_dev)
 {
 	kfree(m2m_dev);
 }
 EXPORT_SYMBOL_GPL(v4l2_m2m_release);
 
-/**
- * v4l2_m2m_ctx_init() - allocate and initialize a m2m context
- * @priv - driver's instance private data
- * @m2m_dev - a previously initialized m2m_dev struct
- * @vq_init - a callback for queue type-specific initialization function to be
- * used for initializing videobuf_queues
- *
- * Usually called from driver's open() function.
- */
 struct v4l2_m2m_ctx *v4l2_m2m_ctx_init(struct v4l2_m2m_dev *m2m_dev,
 		void *drv_priv,
 		int (*queue_init)(void *priv, struct vb2_queue *src_vq, struct vb2_queue *dst_vq))
@@ -744,11 +628,6 @@ err:
 }
 EXPORT_SYMBOL_GPL(v4l2_m2m_ctx_init);
 
-/**
- * v4l2_m2m_ctx_release() - release m2m context
- *
- * Usually called from driver's release() function.
- */
 void v4l2_m2m_ctx_release(struct v4l2_m2m_ctx *m2m_ctx)
 {
 	/* wait until the current context is dequeued from job_queue */
@@ -761,11 +640,6 @@ void v4l2_m2m_ctx_release(struct v4l2_m2m_ctx *m2m_ctx)
 }
 EXPORT_SYMBOL_GPL(v4l2_m2m_ctx_release);
 
-/**
- * v4l2_m2m_buf_queue() - add a buffer to the proper ready buffers list.
- *
- * Call from buf_queue(), videobuf_queue_ops callback.
- */
 void v4l2_m2m_buf_queue(struct v4l2_m2m_ctx *m2m_ctx,
 		struct vb2_v4l2_buffer *vbuf)
 {
diff --git a/drivers/media/v4l2-core/videobuf-dma-sg.c b/drivers/media/v4l2-core/videobuf-dma-sg.c
index f300f060b3f3..1db0af6c7f94 100644
--- a/drivers/media/v4l2-core/videobuf-dma-sg.c
+++ b/drivers/media/v4l2-core/videobuf-dma-sg.c
@@ -156,6 +156,7 @@ static int videobuf_dma_init_user_locked(struct videobuf_dmabuf *dma,
 {
 	unsigned long first, last;
 	int err, rw = 0;
+	unsigned int flags = FOLL_FORCE;
 
 	dma->direction = direction;
 	switch (dma->direction) {
@@ -178,12 +179,14 @@ static int videobuf_dma_init_user_locked(struct videobuf_dmabuf *dma,
 	if (NULL == dma->pages)
 		return -ENOMEM;
 
+	if (rw == READ)
+		flags |= FOLL_WRITE;
+
 	dprintk(1, "init user [0x%lx+0x%lx => %d pages]\n",
 		data, size, dma->nr_pages);
 
 	err = get_user_pages(data & PAGE_MASK, dma->nr_pages,
-			     rw == READ, 1, /* force */
-			     dma->pages, NULL);
+			     flags, dma->pages, NULL);
 
 	if (err != dma->nr_pages) {
 		dma->nr_pages = (err >= 0) ? err : 0;
diff --git a/drivers/media/v4l2-core/videobuf2-core.c b/drivers/media/v4l2-core/videobuf2-core.c
index ca8ffeb56d72..21900202ff83 100644
--- a/drivers/media/v4l2-core/videobuf2-core.c
+++ b/drivers/media/v4l2-core/videobuf2-core.c
@@ -198,6 +198,7 @@ static int __vb2_buf_mem_alloc(struct vb2_buffer *vb)
 		q->is_output ? DMA_TO_DEVICE : DMA_FROM_DEVICE;
 	void *mem_priv;
 	int plane;
+	int ret = -ENOMEM;
 
 	/*
 	 * Allocate memory for all planes in this buffer
@@ -209,8 +210,11 @@ static int __vb2_buf_mem_alloc(struct vb2_buffer *vb)
 		mem_priv = call_ptr_memop(vb, alloc,
 				q->alloc_devs[plane] ? : q->dev,
 				q->dma_attrs, size, dma_dir, q->gfp_flags);
-		if (IS_ERR_OR_NULL(mem_priv))
+		if (IS_ERR(mem_priv)) {
+			if (mem_priv)
+				ret = PTR_ERR(mem_priv);
 			goto free;
+		}
 
 		/* Associate allocator private data with this plane */
 		vb->planes[plane].mem_priv = mem_priv;
@@ -224,7 +228,7 @@ free:
 		vb->planes[plane - 1].mem_priv = NULL;
 	}
 
-	return -ENOMEM;
+	return ret;
 }
 
 /**
@@ -524,10 +528,6 @@ static int __vb2_queue_free(struct vb2_queue *q, unsigned int buffers)
 	return 0;
 }
 
-/**
- * vb2_buffer_in_use() - return true if the buffer is in use and
- * the queue cannot be freed (by the means of REQBUFS(0)) call
- */
 bool vb2_buffer_in_use(struct vb2_queue *q, struct vb2_buffer *vb)
 {
 	unsigned int plane;
@@ -560,16 +560,6 @@ static bool __buffers_in_use(struct vb2_queue *q)
 	return false;
 }
 
-/**
- * vb2_core_querybuf() - query video buffer information
- * @q:		videobuf queue
- * @index:	id number of the buffer
- * @pb:		buffer struct passed from userspace
- *
- * Should be called from vidioc_querybuf ioctl handler in driver.
- * The passed buffer should have been verified.
- * This function fills the relevant information for the userspace.
- */
 void vb2_core_querybuf(struct vb2_queue *q, unsigned int index, void *pb)
 {
 	call_void_bufop(q, fill_user_buffer, q->bufs[index], pb);
@@ -616,10 +606,6 @@ static int __verify_dmabuf_ops(struct vb2_queue *q)
 	return 0;
 }
 
-/**
- * vb2_verify_memory_type() - Check whether the memory type and buffer type
- * passed to a buffer operation are compatible with the queue.
- */
 int vb2_verify_memory_type(struct vb2_queue *q,
 		enum vb2_memory memory, unsigned int type)
 {
@@ -666,30 +652,6 @@ int vb2_verify_memory_type(struct vb2_queue *q,
 }
 EXPORT_SYMBOL(vb2_verify_memory_type);
 
-/**
- * vb2_core_reqbufs() - Initiate streaming
- * @q:		videobuf2 queue
- * @memory: memory type
- * @count: requested buffer count
- *
- * Should be called from vidioc_reqbufs ioctl handler of a driver.
- * This function:
- * 1) verifies streaming parameters passed from the userspace,
- * 2) sets up the queue,
- * 3) negotiates number of buffers and planes per buffer with the driver
- *    to be used during streaming,
- * 4) allocates internal buffer structures (struct vb2_buffer), according to
- *    the agreed parameters,
- * 5) for MMAP memory type, allocates actual video memory, using the
- *    memory handling/allocation routines provided during queue initialization
- *
- * If req->count is 0, all the memory will be freed instead.
- * If the queue has been allocated previously (by a previous vb2_reqbufs) call
- * and the queue is not busy, memory will be reallocated.
- *
- * The return values from this function are intended to be directly returned
- * from vidioc_reqbufs handler in driver.
- */
 int vb2_core_reqbufs(struct vb2_queue *q, enum vb2_memory memory,
 		unsigned int *count)
 {
@@ -815,22 +777,6 @@ int vb2_core_reqbufs(struct vb2_queue *q, enum vb2_memory memory,
 }
 EXPORT_SYMBOL_GPL(vb2_core_reqbufs);
 
-/**
- * vb2_core_create_bufs() - Allocate buffers and any required auxiliary structs
- * @q:		videobuf2 queue
- * @memory: memory type
- * @count: requested buffer count
- * @parg: parameter passed to device driver
- *
- * Should be called from vidioc_create_bufs ioctl handler of a driver.
- * This function:
- * 1) verifies parameter sanity
- * 2) calls the .queue_setup() queue operation
- * 3) performs any necessary memory allocations
- *
- * The return values from this function are intended to be directly returned
- * from vidioc_create_bufs handler in driver.
- */
 int vb2_core_create_bufs(struct vb2_queue *q, enum vb2_memory memory,
 		unsigned int *count, unsigned requested_planes,
 		const unsigned requested_sizes[])
@@ -920,14 +866,6 @@ int vb2_core_create_bufs(struct vb2_queue *q, enum vb2_memory memory,
 }
 EXPORT_SYMBOL_GPL(vb2_core_create_bufs);
 
-/**
- * vb2_plane_vaddr() - Return a kernel virtual address of a given plane
- * @vb:		vb2_buffer to which the plane in question belongs to
- * @plane_no:	plane number for which the address is to be returned
- *
- * This function returns a kernel virtual address of a given plane if
- * such a mapping exist, NULL otherwise.
- */
 void *vb2_plane_vaddr(struct vb2_buffer *vb, unsigned int plane_no)
 {
 	if (plane_no > vb->num_planes || !vb->planes[plane_no].mem_priv)
@@ -938,17 +876,6 @@ void *vb2_plane_vaddr(struct vb2_buffer *vb, unsigned int plane_no)
 }
 EXPORT_SYMBOL_GPL(vb2_plane_vaddr);
 
-/**
- * vb2_plane_cookie() - Return allocator specific cookie for the given plane
- * @vb:		vb2_buffer to which the plane in question belongs to
- * @plane_no:	plane number for which the cookie is to be returned
- *
- * This function returns an allocator specific cookie for a given plane if
- * available, NULL otherwise. The allocator should provide some simple static
- * inline function, which would convert this cookie to the allocator specific
- * type that can be used directly by the driver to access the buffer. This can
- * be for example physical address, pointer to scatter list or IOMMU mapping.
- */
 void *vb2_plane_cookie(struct vb2_buffer *vb, unsigned int plane_no)
 {
 	if (plane_no >= vb->num_planes || !vb->planes[plane_no].mem_priv)
@@ -958,26 +885,6 @@ void *vb2_plane_cookie(struct vb2_buffer *vb, unsigned int plane_no)
 }
 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,
- *		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.
- *
- * This function should be called by the driver after a hardware operation on
- * a buffer is finished and the buffer may be returned to userspace. The driver
- * cannot use this buffer anymore until it is queued back to it by videobuf
- * by the means of buf_queue callback. Only buffers previously queued to the
- * driver by buf_queue can be passed to this function.
- *
- * While streaming a buffer can only be returned in state DONE or ERROR.
- * The start_streaming op can also return them in case the DMA engine cannot
- * be started for some reason. In that case the buffers should be returned with
- * state QUEUED.
- */
 void vb2_buffer_done(struct vb2_buffer *vb, enum vb2_buffer_state state)
 {
 	struct vb2_queue *q = vb->vb2_queue;
@@ -1036,18 +943,6 @@ void vb2_buffer_done(struct vb2_buffer *vb, enum vb2_buffer_state state)
 }
 EXPORT_SYMBOL_GPL(vb2_buffer_done);
 
-/**
- * vb2_discard_done() - discard all buffers marked as DONE
- * @q:		videobuf2 queue
- *
- * This function is intended to be used with suspend/resume operations. It
- * discards all 'done' buffers as they would be too old to be requested after
- * resume.
- *
- * Drivers must stop the hardware and synchronize with interrupt handlers and/or
- * delayed works before calling this function to make sure no buffer will be
- * touched by the driver and/or hardware.
- */
 void vb2_discard_done(struct vb2_queue *q)
 {
 	struct vb2_buffer *vb;
@@ -1136,10 +1031,10 @@ static int __qbuf_userptr(struct vb2_buffer *vb, const void *pb)
 				q->alloc_devs[plane] ? : q->dev,
 				planes[plane].m.userptr,
 				planes[plane].length, dma_dir);
-		if (IS_ERR_OR_NULL(mem_priv)) {
+		if (IS_ERR(mem_priv)) {
 			dprintk(1, "failed acquiring userspace "
 						"memory for plane %d\n", plane);
-			ret = mem_priv ? PTR_ERR(mem_priv) : -EINVAL;
+			ret = PTR_ERR(mem_priv);
 			goto err;
 		}
 		vb->planes[plane].mem_priv = mem_priv;
@@ -1228,8 +1123,10 @@ static int __qbuf_dmabuf(struct vb2_buffer *vb, const void *pb)
 			planes[plane].length = dbuf->size;
 
 		if (planes[plane].length < vb->planes[plane].min_length) {
-			dprintk(1, "invalid dmabuf length for plane %d\n",
-				plane);
+			dprintk(1, "invalid dmabuf length %u for plane %d, "
+				"minimum length %u\n",
+				planes[plane].length, plane,
+				vb->planes[plane].min_length);
 			dma_buf_put(dbuf);
 			ret = -EINVAL;
 			goto err;
@@ -1271,9 +1168,10 @@ static int __qbuf_dmabuf(struct vb2_buffer *vb, const void *pb)
 		vb->planes[plane].mem_priv = mem_priv;
 	}
 
-	/* TODO: This pins the buffer(s) with  dma_buf_map_attachment()).. but
-	 * really we want to do this just before the DMA, not while queueing
-	 * the buffer(s)..
+	/*
+	 * This pins the buffer(s) with dma_buf_map_attachment()). It's done
+	 * here instead just before the DMA, while queueing the buffer(s) so
+	 * userspace knows sooner rather than later if the dma-buf map fails.
 	 */
 	for (plane = 0; plane < vb->num_planes; ++plane) {
 		ret = call_memop(vb, map_dmabuf, vb->planes[plane].mem_priv);
@@ -1377,22 +1275,6 @@ static int __buf_prepare(struct vb2_buffer *vb, const void *pb)
 	return ret;
 }
 
-/**
- * vb2_core_prepare_buf() - Pass ownership of a buffer from userspace
- *			to the kernel
- * @q:		videobuf2 queue
- * @index:	id number of the buffer
- * @pb:		buffer structure passed from userspace to vidioc_prepare_buf
- *		handler in driver
- *
- * Should be called from vidioc_prepare_buf ioctl handler of a driver.
- * The passed buffer should have been verified.
- * This function calls buf_prepare callback in the driver (if provided),
- * in which driver-specific buffer initialization can be performed,
- *
- * The return values from this function are intended to be directly returned
- * from vidioc_prepare_buf handler in driver.
- */
 int vb2_core_prepare_buf(struct vb2_queue *q, unsigned int index, void *pb)
 {
 	struct vb2_buffer *vb;
@@ -1481,24 +1363,6 @@ static int vb2_start_streaming(struct vb2_queue *q)
 	return ret;
 }
 
-/**
- * vb2_core_qbuf() - Queue a buffer from userspace
- * @q:		videobuf2 queue
- * @index:	id number of the buffer
- * @pb:		buffer structure passed from userspace to vidioc_qbuf handler
- *		in driver
- *
- * Should be called from vidioc_qbuf ioctl handler of a driver.
- * The passed buffer should have been verified.
- * This function:
- * 1) if necessary, calls buf_prepare callback in the driver (if provided), in
- *    which driver-specific buffer initialization can be performed,
- * 2) if streaming is on, queues the buffer in driver by the means of buf_queue
- *    callback for processing.
- *
- * The return values from this function are intended to be directly returned
- * from vidioc_qbuf handler in driver.
- */
 int vb2_core_qbuf(struct vb2_queue *q, unsigned int index, void *pb)
 {
 	struct vb2_buffer *vb;
@@ -1679,15 +1543,6 @@ static int __vb2_get_done_vb(struct vb2_queue *q, struct vb2_buffer **vb,
 	return ret;
 }
 
-/**
- * vb2_wait_for_all_buffers() - wait until all buffers are given back to vb2
- * @q:		videobuf2 queue
- *
- * This function will wait until all buffers that have been given to the driver
- * by buf_queue() are given back to vb2 with vb2_buffer_done(). It doesn't call
- * wait_prepare, wait_finish pair. It is intended to be called with all locks
- * taken, for example from stop_streaming() callback.
- */
 int vb2_wait_for_all_buffers(struct vb2_queue *q)
 {
 	if (!q->streaming) {
@@ -1725,27 +1580,6 @@ static void __vb2_dqbuf(struct vb2_buffer *vb)
 		}
 }
 
-/**
- * vb2_dqbuf() - Dequeue a buffer to the userspace
- * @q:		videobuf2 queue
- * @pb:		buffer structure passed from userspace to vidioc_dqbuf handler
- *		in driver
- * @nonblocking: if true, this call will not sleep waiting for a buffer if no
- *		 buffers ready for dequeuing are present. Normally the driver
- *		 would be passing (file->f_flags & O_NONBLOCK) here
- *
- * Should be called from vidioc_dqbuf ioctl handler of a driver.
- * The passed buffer should have been verified.
- * This function:
- * 1) calls buf_finish callback in the driver (if provided), in which
- *    driver can perform any additional operations that may be required before
- *    returning the buffer to userspace, such as cache sync,
- * 2) the buffer struct members are filled with relevant information for
- *    the userspace.
- *
- * The return values from this function are intended to be directly returned
- * from vidioc_dqbuf handler in driver.
- */
 int vb2_core_dqbuf(struct vb2_queue *q, unsigned int *pindex, void *pb,
 		   bool nonblocking)
 {
@@ -1909,19 +1743,6 @@ int vb2_core_streamon(struct vb2_queue *q, unsigned int type)
 }
 EXPORT_SYMBOL_GPL(vb2_core_streamon);
 
-/**
- * vb2_queue_error() - signal a fatal error on the queue
- * @q:		videobuf2 queue
- *
- * Flag that a fatal unrecoverable error has occurred and wake up all processes
- * waiting on the queue. Polling will now set POLLERR and queuing and dequeuing
- * buffers will return -EIO.
- *
- * The error flag will be cleared when cancelling the queue, either from
- * vb2_streamoff or vb2_queue_release. Drivers should thus not call this
- * function before starting the stream, otherwise the error flag will remain set
- * until the queue is released when closing the device node.
- */
 void vb2_queue_error(struct vb2_queue *q)
 {
 	q->error = 1;
@@ -1984,19 +1805,6 @@ static int __find_plane_by_offset(struct vb2_queue *q, unsigned long off,
 	return -EINVAL;
 }
 
-/**
- * vb2_core_expbuf() - Export a buffer as a file descriptor
- * @q:		videobuf2 queue
- * @fd:		file descriptor associated with DMABUF (set by driver) *
- * @type:	buffer type
- * @index:	id number of the buffer
- * @plane:	index of the plane to be exported, 0 for single plane queues
- * @flags:	flags for newly created file, currently only O_CLOEXEC is
- *		supported, refer to manual of open syscall for more details
- *
- * The return values from this function are intended to be directly returned
- * from vidioc_expbuf handler in driver.
- */
 int vb2_core_expbuf(struct vb2_queue *q, int *fd, unsigned int type,
 		unsigned int index, unsigned int plane, unsigned int flags)
 {
@@ -2068,25 +1876,6 @@ int vb2_core_expbuf(struct vb2_queue *q, int *fd, unsigned int type,
 }
 EXPORT_SYMBOL_GPL(vb2_core_expbuf);
 
-/**
- * vb2_mmap() - map video buffers into application address space
- * @q:		videobuf2 queue
- * @vma:	vma passed to the mmap file operation handler in the driver
- *
- * Should be called from mmap file operation handler of a driver.
- * This function maps one plane of one of the available video buffers to
- * userspace. To map whole video memory allocated on reqbufs, this function
- * has to be called once per each plane per each buffer previously allocated.
- *
- * When the userspace application calls mmap, it passes to it an offset returned
- * to it earlier by the means of vidioc_querybuf handler. That offset acts as
- * a "cookie", which is then used to identify the plane to be mapped.
- * This function finds a plane with a matching offset and a mapping is performed
- * by the means of a provided memory operation.
- *
- * The return values from this function are intended to be directly returned
- * from the mmap handler in driver.
- */
 int vb2_mmap(struct vb2_queue *q, struct vm_area_struct *vma)
 {
 	unsigned long off = vma->vm_pgoff << PAGE_SHIFT;
@@ -2188,17 +1977,6 @@ unsigned long vb2_get_unmapped_area(struct vb2_queue *q,
 EXPORT_SYMBOL_GPL(vb2_get_unmapped_area);
 #endif
 
-/**
- * vb2_core_queue_init() - initialize a videobuf2 queue
- * @q:		videobuf2 queue; this structure should be allocated in driver
- *
- * The vb2_queue structure should be allocated by the driver. The driver is
- * responsible of clearing it's content and setting initial values for some
- * required entries before calling this function.
- * q->ops, q->mem_ops, q->type and q->io_modes are mandatory. Please refer
- * to the struct vb2_queue description in include/media/videobuf2-core.h
- * for more information.
- */
 int vb2_core_queue_init(struct vb2_queue *q)
 {
 	/*
@@ -2228,14 +2006,6 @@ EXPORT_SYMBOL_GPL(vb2_core_queue_init);
 
 static int __vb2_init_fileio(struct vb2_queue *q, int read);
 static int __vb2_cleanup_fileio(struct vb2_queue *q);
-/**
- * vb2_core_queue_release() - stop streaming, release the queue and free memory
- * @q:		videobuf2 queue
- *
- * This function stops streaming and performs necessary clean ups, including
- * freeing video buffer memory. The driver is responsible for freeing
- * the vb2_queue structure itself.
- */
 void vb2_core_queue_release(struct vb2_queue *q)
 {
 	__vb2_cleanup_fileio(q);
@@ -2246,22 +2016,6 @@ void vb2_core_queue_release(struct vb2_queue *q)
 }
 EXPORT_SYMBOL_GPL(vb2_core_queue_release);
 
-/**
- * vb2_core_poll() - implements poll userspace operation
- * @q:		videobuf2 queue
- * @file:	file argument passed to the poll file operation handler
- * @wait:	wait argument passed to the poll file operation handler
- *
- * This function implements poll file operation handler for a driver.
- * For CAPTURE queues, if a buffer is ready to be dequeued, the userspace will
- * be informed that the file descriptor of a video device is available for
- * reading.
- * For OUTPUT queues, if a buffer is ready to be dequeued, the file descriptor
- * will be reported as available for writing.
- *
- * The return values from this function are intended to be directly returned
- * from poll handler in driver.
- */
 unsigned int vb2_core_poll(struct vb2_queue *q, struct file *file,
 		poll_table *wait)
 {
diff --git a/drivers/media/v4l2-core/videobuf2-dma-contig.c b/drivers/media/v4l2-core/videobuf2-dma-contig.c
index 59fa204b15f3..fb6a177be461 100644
--- a/drivers/media/v4l2-core/videobuf2-dma-contig.c
+++ b/drivers/media/v4l2-core/videobuf2-dma-contig.c
@@ -141,6 +141,9 @@ static void *vb2_dc_alloc(struct device *dev, unsigned long attrs,
 {
 	struct vb2_dc_buf *buf;
 
+	if (WARN_ON(!dev))
+		return ERR_PTR(-EINVAL);
+
 	buf = kzalloc(sizeof *buf, GFP_KERNEL);
 	if (!buf)
 		return ERR_PTR(-ENOMEM);
@@ -493,6 +496,9 @@ static void *vb2_dc_get_userptr(struct device *dev, unsigned long vaddr,
 		return ERR_PTR(-EINVAL);
 	}
 
+	if (WARN_ON(!dev))
+		return ERR_PTR(-EINVAL);
+
 	buf = kzalloc(sizeof *buf, GFP_KERNEL);
 	if (!buf)
 		return ERR_PTR(-ENOMEM);
@@ -673,6 +679,9 @@ static void *vb2_dc_attach_dmabuf(struct device *dev, struct dma_buf *dbuf,
 	if (dbuf->size < size)
 		return ERR_PTR(-EFAULT);
 
+	if (WARN_ON(!dev))
+		return ERR_PTR(-EINVAL);
+
 	buf = kzalloc(sizeof(*buf), GFP_KERNEL);
 	if (!buf)
 		return ERR_PTR(-ENOMEM);
diff --git a/drivers/media/v4l2-core/videobuf2-dma-sg.c b/drivers/media/v4l2-core/videobuf2-dma-sg.c
index bd82d709ee82..ecff8f492c4f 100644
--- a/drivers/media/v4l2-core/videobuf2-dma-sg.c
+++ b/drivers/media/v4l2-core/videobuf2-dma-sg.c
@@ -104,11 +104,12 @@ static void *vb2_dma_sg_alloc(struct device *dev, unsigned long dma_attrs,
 	int ret;
 	int num_pages;
 
-	if (WARN_ON(dev == NULL))
-		return NULL;
+	if (WARN_ON(!dev))
+		return ERR_PTR(-EINVAL);
+
 	buf = kzalloc(sizeof *buf, GFP_KERNEL);
 	if (!buf)
-		return NULL;
+		return ERR_PTR(-ENOMEM);
 
 	buf->vaddr = NULL;
 	buf->dma_dir = dma_dir;
@@ -166,7 +167,7 @@ fail_pages_alloc:
 	kfree(buf->pages);
 fail_pages_array_alloc:
 	kfree(buf);
-	return NULL;
+	return ERR_PTR(-ENOMEM);
 }
 
 static void vb2_dma_sg_put(void *buf_priv)
@@ -224,9 +225,12 @@ static void *vb2_dma_sg_get_userptr(struct device *dev, unsigned long vaddr,
 	struct sg_table *sgt;
 	struct frame_vector *vec;
 
+	if (WARN_ON(!dev))
+		return ERR_PTR(-EINVAL);
+
 	buf = kzalloc(sizeof *buf, GFP_KERNEL);
 	if (!buf)
-		return NULL;
+		return ERR_PTR(-ENOMEM);
 
 	buf->vaddr = NULL;
 	buf->dev = dev;
@@ -266,7 +270,7 @@ userptr_fail_sgtable:
 	vb2_destroy_framevec(vec);
 userptr_fail_pfnvec:
 	kfree(buf);
-	return NULL;
+	return ERR_PTR(-ENOMEM);
 }
 
 /*
@@ -606,6 +610,9 @@ static void *vb2_dma_sg_attach_dmabuf(struct device *dev, struct dma_buf *dbuf,
 	struct vb2_dma_sg_buf *buf;
 	struct dma_buf_attachment *dba;
 
+	if (WARN_ON(!dev))
+		return ERR_PTR(-EINVAL);
+
 	if (dbuf->size < size)
 		return ERR_PTR(-EFAULT);
 
diff --git a/drivers/media/v4l2-core/videobuf2-memops.c b/drivers/media/v4l2-core/videobuf2-memops.c
index 3c3b517f1d1c..1cd322e939c7 100644
--- a/drivers/media/v4l2-core/videobuf2-memops.c
+++ b/drivers/media/v4l2-core/videobuf2-memops.c
@@ -42,6 +42,10 @@ struct frame_vector *vb2_create_framevec(unsigned long start,
 	unsigned long first, last;
 	unsigned long nr;
 	struct frame_vector *vec;
+	unsigned int flags = FOLL_FORCE;
+
+	if (write)
+		flags |= FOLL_WRITE;
 
 	first = start >> PAGE_SHIFT;
 	last = (start + length - 1) >> PAGE_SHIFT;
@@ -49,7 +53,7 @@ struct frame_vector *vb2_create_framevec(unsigned long start,
 	vec = frame_vector_create(nr);
 	if (!vec)
 		return ERR_PTR(-ENOMEM);
-	ret = get_vaddr_frames(start & PAGE_MASK, nr, write, true, vec);
+	ret = get_vaddr_frames(start & PAGE_MASK, nr, flags, vec);
 	if (ret < 0)
 		goto out_destroy;
 	/* We accept only complete set of PFNs */
diff --git a/drivers/media/v4l2-core/videobuf2-v4l2.c b/drivers/media/v4l2-core/videobuf2-v4l2.c
index 9cfbb6e4bc28..52ef8833f6b6 100644
--- a/drivers/media/v4l2-core/videobuf2-v4l2.c
+++ b/drivers/media/v4l2-core/videobuf2-v4l2.c
@@ -483,13 +483,6 @@ int vb2_querybuf(struct vb2_queue *q, struct v4l2_buffer *b)
 }
 EXPORT_SYMBOL(vb2_querybuf);
 
-/**
- * vb2_reqbufs() - Wrapper for vb2_core_reqbufs() that also verifies
- * the memory and type values.
- * @q:		videobuf2 queue
- * @req:	struct passed from userspace to vidioc_reqbufs handler
- *		in driver
- */
 int vb2_reqbufs(struct vb2_queue *q, struct v4l2_requestbuffers *req)
 {
 	int ret = vb2_verify_memory_type(q, req->memory, req->type);
@@ -498,21 +491,6 @@ int vb2_reqbufs(struct vb2_queue *q, struct v4l2_requestbuffers *req)
 }
 EXPORT_SYMBOL_GPL(vb2_reqbufs);
 
-/**
- * vb2_prepare_buf() - Pass ownership of a buffer from userspace to the kernel
- * @q:		videobuf2 queue
- * @b:		buffer structure passed from userspace to vidioc_prepare_buf
- *		handler in driver
- *
- * Should be called from vidioc_prepare_buf ioctl handler of a driver.
- * This function:
- * 1) verifies the passed buffer,
- * 2) calls buf_prepare callback in the driver (if provided), in which
- *    driver-specific buffer initialization can be performed,
- *
- * The return values from this function are intended to be directly returned
- * from vidioc_prepare_buf handler in driver.
- */
 int vb2_prepare_buf(struct vb2_queue *q, struct v4l2_buffer *b)
 {
 	int ret;
@@ -528,13 +506,6 @@ int vb2_prepare_buf(struct vb2_queue *q, struct v4l2_buffer *b)
 }
 EXPORT_SYMBOL_GPL(vb2_prepare_buf);
 
-/**
- * vb2_create_bufs() - Wrapper for vb2_core_create_bufs() that also verifies
- * the memory and type values.
- * @q:		videobuf2 queue
- * @create:	creation parameters, passed from userspace to vidioc_create_bufs
- *		handler in driver
- */
 int vb2_create_bufs(struct vb2_queue *q, struct v4l2_create_buffers *create)
 {
 	unsigned requested_planes = 1;
@@ -586,23 +557,6 @@ int vb2_create_bufs(struct vb2_queue *q, struct v4l2_create_buffers *create)
 }
 EXPORT_SYMBOL_GPL(vb2_create_bufs);
 
-/**
- * vb2_qbuf() - Queue a buffer from userspace
- * @q:		videobuf2 queue
- * @b:		buffer structure passed from userspace to vidioc_qbuf handler
- *		in driver
- *
- * Should be called from vidioc_qbuf ioctl handler of a driver.
- * This function:
- * 1) verifies the passed buffer,
- * 2) if necessary, calls buf_prepare callback in the driver (if provided), in
- *    which driver-specific buffer initialization can be performed,
- * 3) if streaming is on, queues the buffer in driver by the means of buf_queue
- *    callback for processing.
- *
- * The return values from this function are intended to be directly returned
- * from vidioc_qbuf handler in driver.
- */
 int vb2_qbuf(struct vb2_queue *q, struct v4l2_buffer *b)
 {
 	int ret;
@@ -617,27 +571,6 @@ int vb2_qbuf(struct vb2_queue *q, struct v4l2_buffer *b)
 }
 EXPORT_SYMBOL_GPL(vb2_qbuf);
 
-/**
- * vb2_dqbuf() - Dequeue a buffer to the userspace
- * @q:		videobuf2 queue
- * @b:		buffer structure passed from userspace to vidioc_dqbuf handler
- *		in driver
- * @nonblocking: if true, this call will not sleep waiting for a buffer if no
- *		 buffers ready for dequeuing are present. Normally the driver
- *		 would be passing (file->f_flags & O_NONBLOCK) here
- *
- * Should be called from vidioc_dqbuf ioctl handler of a driver.
- * This function:
- * 1) verifies the passed buffer,
- * 2) calls buf_finish callback in the driver (if provided), in which
- *    driver can perform any additional operations that may be required before
- *    returning the buffer to userspace, such as cache sync,
- * 3) the buffer struct members are filled with relevant information for
- *    the userspace.
- *
- * The return values from this function are intended to be directly returned
- * from vidioc_dqbuf handler in driver.
- */
 int vb2_dqbuf(struct vb2_queue *q, struct v4l2_buffer *b, bool nonblocking)
 {
 	int ret;
@@ -664,19 +597,6 @@ int vb2_dqbuf(struct vb2_queue *q, struct v4l2_buffer *b, bool nonblocking)
 }
 EXPORT_SYMBOL_GPL(vb2_dqbuf);
 
-/**
- * vb2_streamon - start streaming
- * @q:		videobuf2 queue
- * @type:	type argument passed from userspace to vidioc_streamon handler
- *
- * Should be called from vidioc_streamon handler of a driver.
- * This function:
- * 1) verifies current state
- * 2) passes any previously queued buffers to the driver and starts streaming
- *
- * The return values from this function are intended to be directly returned
- * from vidioc_streamon handler in the driver.
- */
 int vb2_streamon(struct vb2_queue *q, enum v4l2_buf_type type)
 {
 	if (vb2_fileio_is_active(q)) {
@@ -687,21 +607,6 @@ int vb2_streamon(struct vb2_queue *q, enum v4l2_buf_type type)
 }
 EXPORT_SYMBOL_GPL(vb2_streamon);
 
-/**
- * vb2_streamoff - stop streaming
- * @q:		videobuf2 queue
- * @type:	type argument passed from userspace to vidioc_streamoff handler
- *
- * Should be called from vidioc_streamoff handler of a driver.
- * This function:
- * 1) verifies current state,
- * 2) stop streaming and dequeues any queued buffers, including those previously
- *    passed to the driver (after waiting for the driver to finish).
- *
- * This call can be used for pausing playback.
- * The return values from this function are intended to be directly returned
- * from vidioc_streamoff handler in the driver
- */
 int vb2_streamoff(struct vb2_queue *q, enum v4l2_buf_type type)
 {
 	if (vb2_fileio_is_active(q)) {
@@ -712,15 +617,6 @@ int vb2_streamoff(struct vb2_queue *q, enum v4l2_buf_type type)
 }
 EXPORT_SYMBOL_GPL(vb2_streamoff);
 
-/**
- * vb2_expbuf() - Export a buffer as a file descriptor
- * @q:		videobuf2 queue
- * @eb:		export buffer structure passed from userspace to vidioc_expbuf
- *		handler in driver
- *
- * The return values from this function are intended to be directly returned
- * from vidioc_expbuf handler in driver.
- */
 int vb2_expbuf(struct vb2_queue *q, struct v4l2_exportbuffer *eb)
 {
 	return vb2_core_expbuf(q, &eb->fd, eb->type, eb->index,
@@ -728,17 +624,6 @@ int vb2_expbuf(struct vb2_queue *q, struct v4l2_exportbuffer *eb)
 }
 EXPORT_SYMBOL_GPL(vb2_expbuf);
 
-/**
- * vb2_queue_init() - initialize a videobuf2 queue
- * @q:		videobuf2 queue; this structure should be allocated in driver
- *
- * The vb2_queue structure should be allocated by the driver. The driver is
- * responsible of clearing it's content and setting initial values for some
- * required entries before calling this function.
- * q->ops, q->mem_ops, q->type and q->io_modes are mandatory. Please refer
- * to the struct vb2_queue description in include/media/videobuf2-core.h
- * for more information.
- */
 int vb2_queue_init(struct vb2_queue *q)
 {
 	/*
@@ -779,39 +664,12 @@ int vb2_queue_init(struct vb2_queue *q)
 }
 EXPORT_SYMBOL_GPL(vb2_queue_init);
 
-/**
- * vb2_queue_release() - stop streaming, release the queue and free memory
- * @q:		videobuf2 queue
- *
- * This function stops streaming and performs necessary clean ups, including
- * freeing video buffer memory. The driver is responsible for freeing
- * the vb2_queue structure itself.
- */
 void vb2_queue_release(struct vb2_queue *q)
 {
 	vb2_core_queue_release(q);
 }
 EXPORT_SYMBOL_GPL(vb2_queue_release);
 
-/**
- * vb2_poll() - implements poll userspace operation
- * @q:		videobuf2 queue
- * @file:	file argument passed to the poll file operation handler
- * @wait:	wait argument passed to the poll file operation handler
- *
- * This function implements poll file operation handler for a driver.
- * For CAPTURE queues, if a buffer is ready to be dequeued, the userspace will
- * be informed that the file descriptor of a video device is available for
- * reading.
- * For OUTPUT queues, if a buffer is ready to be dequeued, the file descriptor
- * will be reported as available for writing.
- *
- * If the driver uses struct v4l2_fh, then vb2_poll() will also check for any
- * pending events.
- *
- * The return values from this function are intended to be directly returned
- * from poll handler in driver.
- */
 unsigned int vb2_poll(struct vb2_queue *q, struct file *file, poll_table *wait)
 {
 	struct video_device *vfd = video_devdata(file);
diff --git a/drivers/media/v4l2-core/videobuf2-vmalloc.c b/drivers/media/v4l2-core/videobuf2-vmalloc.c
index c2820a6e164d..ab3227b75c84 100644
--- a/drivers/media/v4l2-core/videobuf2-vmalloc.c
+++ b/drivers/media/v4l2-core/videobuf2-vmalloc.c
@@ -41,7 +41,7 @@ static void *vb2_vmalloc_alloc(struct device *dev, unsigned long attrs,
 
 	buf = kzalloc(sizeof(*buf), GFP_KERNEL | gfp_flags);
 	if (!buf)
-		return NULL;
+		return ERR_PTR(-ENOMEM);
 
 	buf->size = size;
 	buf->vaddr = vmalloc_user(buf->size);
@@ -53,7 +53,7 @@ static void *vb2_vmalloc_alloc(struct device *dev, unsigned long attrs,
 	if (!buf->vaddr) {
 		pr_debug("vmalloc of size %ld failed\n", buf->size);
 		kfree(buf);
-		return NULL;
+		return ERR_PTR(-ENOMEM);
 	}
 
 	atomic_inc(&buf->refcount);
@@ -77,17 +77,20 @@ static void *vb2_vmalloc_get_userptr(struct device *dev, unsigned long vaddr,
 	struct vb2_vmalloc_buf *buf;
 	struct frame_vector *vec;
 	int n_pages, offset, i;
+	int ret = -ENOMEM;
 
 	buf = kzalloc(sizeof(*buf), GFP_KERNEL);
 	if (!buf)
-		return NULL;
+		return ERR_PTR(-ENOMEM);
 
 	buf->dma_dir = dma_dir;
 	offset = vaddr & ~PAGE_MASK;
 	buf->size = size;
 	vec = vb2_create_framevec(vaddr, size, dma_dir == DMA_FROM_DEVICE);
-	if (IS_ERR(vec))
+	if (IS_ERR(vec)) {
+		ret = PTR_ERR(vec);
 		goto fail_pfnvec_create;
+	}
 	buf->vec = vec;
 	n_pages = frame_vector_count(vec);
 	if (frame_vector_to_pages(vec) < 0) {
@@ -117,7 +120,7 @@ fail_map:
 fail_pfnvec_create:
 	kfree(buf);
 
-	return NULL;
+	return ERR_PTR(ret);
 }
 
 static void vb2_vmalloc_put_userptr(void *buf_priv)
diff --git a/drivers/memory/atmel-ebi.c b/drivers/memory/atmel-ebi.c
index f87ad6f5d2dc..b5ed3bd082b5 100644
--- a/drivers/memory/atmel-ebi.c
+++ b/drivers/memory/atmel-ebi.c
@@ -410,10 +410,7 @@ static int at91sam9_ebi_init(struct at91_ebi *ebi)
 
 	field.reg = AT91SAM9_SMC_MODE(AT91SAM9_SMC_GENERIC);
 	fields->mode = devm_regmap_field_alloc(ebi->dev, ebi->smc, field);
-	if (IS_ERR(fields->mode))
-		return PTR_ERR(fields->mode);
-
-	return 0;
+	return PTR_ERR_OR_ZERO(fields->mode);
 }
 
 static int sama5d3_ebi_init(struct at91_ebi *ebi)
@@ -441,10 +438,7 @@ static int sama5d3_ebi_init(struct at91_ebi *ebi)
 
 	field.reg = SAMA5_SMC_MODE(SAMA5_SMC_GENERIC);
 	fields->mode = devm_regmap_field_alloc(ebi->dev, ebi->smc, field);
-	if (IS_ERR(fields->mode))
-		return PTR_ERR(fields->mode);
-
-	return 0;
+	return PTR_ERR_OR_ZERO(fields->mode);
 }
 
 static int at91_ebi_dev_setup(struct at91_ebi *ebi, struct device_node *np,
diff --git a/drivers/memory/atmel-sdramc.c b/drivers/memory/atmel-sdramc.c
index 53a341f3b305..12080b05e3e6 100644
--- a/drivers/memory/atmel-sdramc.c
+++ b/drivers/memory/atmel-sdramc.c
@@ -53,12 +53,10 @@ static const struct of_device_id atmel_ramc_of_match[] = {
 
 static int atmel_ramc_probe(struct platform_device *pdev)
 {
-	const struct of_device_id *match;
 	const struct at91_ramc_caps *caps;
 	struct clk *clk;
 
-	match = of_match_device(atmel_ramc_of_match, &pdev->dev);
-	caps = match->data;
+	caps = of_device_get_match_data(&pdev->dev);
 
 	if (caps->has_ddrck) {
 		clk = devm_clk_get(&pdev->dev, "ddrck");
diff --git a/drivers/memory/of_memory.c b/drivers/memory/of_memory.c
index 9daf94bb8f27..568f05ed961a 100644
--- a/drivers/memory/of_memory.c
+++ b/drivers/memory/of_memory.c
@@ -16,6 +16,7 @@
 #include <linux/gfp.h>
 #include <memory/jedec_ddr.h>
 #include <linux/export.h>
+#include "of_memory.h"
 
 /**
  * of_get_min_tck() - extract min timing values for ddr
diff --git a/drivers/memory/omap-gpmc.c b/drivers/memory/omap-gpmc.c
index f00f3e742265..5457c361ad58 100644
--- a/drivers/memory/omap-gpmc.c
+++ b/drivers/memory/omap-gpmc.c
@@ -350,8 +350,8 @@ static unsigned int gpmc_ps_to_ticks(unsigned int time_ps)
 	return (time_ps + tick_ps - 1) / tick_ps;
 }
 
-unsigned int gpmc_clk_ticks_to_ns(unsigned ticks, int cs,
-				  enum gpmc_clk_domain cd)
+static unsigned int gpmc_clk_ticks_to_ns(unsigned int ticks, int cs,
+					 enum gpmc_clk_domain cd)
 {
 	return ticks * gpmc_get_clk_period(cs, cd) / 1000;
 }
@@ -2143,9 +2143,7 @@ err_child_fail:
 	ret = -ENODEV;
 
 err_cs:
-	if (waitpin_desc)
-		gpiochip_free_own_desc(waitpin_desc);
-
+	gpiochip_free_own_desc(waitpin_desc);
 err:
 	gpmc_cs_free(cs);
 
@@ -2265,7 +2263,7 @@ static int gpmc_gpio_init(struct gpmc_device *gpmc)
 	gpmc->gpio_chip.get = gpmc_gpio_get;
 	gpmc->gpio_chip.base = -1;
 
-	ret = gpiochip_add(&gpmc->gpio_chip);
+	ret = devm_gpiochip_add_data(gpmc->dev, &gpmc->gpio_chip, NULL);
 	if (ret < 0) {
 		dev_err(gpmc->dev, "could not register gpio chip: %d\n", ret);
 		return ret;
@@ -2274,11 +2272,6 @@ static int gpmc_gpio_init(struct gpmc_device *gpmc)
 	return 0;
 }
 
-static void gpmc_gpio_exit(struct gpmc_device *gpmc)
-{
-	gpiochip_remove(&gpmc->gpio_chip);
-}
-
 static int gpmc_probe(struct platform_device *pdev)
 {
 	int rc;
@@ -2365,15 +2358,13 @@ static int gpmc_probe(struct platform_device *pdev)
 	rc = gpmc_setup_irq(gpmc);
 	if (rc) {
 		dev_err(gpmc->dev, "gpmc_setup_irq failed\n");
-		goto setup_irq_failed;
+		goto gpio_init_failed;
 	}
 
 	gpmc_probe_dt_children(pdev);
 
 	return 0;
 
-setup_irq_failed:
-	gpmc_gpio_exit(gpmc);
 gpio_init_failed:
 	gpmc_mem_exit();
 	pm_runtime_put_sync(&pdev->dev);
@@ -2387,7 +2378,6 @@ static int gpmc_remove(struct platform_device *pdev)
 	struct gpmc_device *gpmc = platform_get_drvdata(pdev);
 
 	gpmc_free_irq(gpmc);
-	gpmc_gpio_exit(gpmc);
 	gpmc_mem_exit();
 	pm_runtime_put_sync(&pdev->dev);
 	pm_runtime_disable(&pdev->dev);
diff --git a/drivers/memstick/host/rtsx_usb_ms.c b/drivers/memstick/host/rtsx_usb_ms.c
index d34bc3530385..2e3cf012ef48 100644
--- a/drivers/memstick/host/rtsx_usb_ms.c
+++ b/drivers/memstick/host/rtsx_usb_ms.c
@@ -524,6 +524,7 @@ static void rtsx_usb_ms_handle_req(struct work_struct *work)
 	int rc;
 
 	if (!host->req) {
+		pm_runtime_get_sync(ms_dev(host));
 		do {
 			rc = memstick_next_req(msh, &host->req);
 			dev_dbg(ms_dev(host), "next req %d\n", rc);
@@ -544,6 +545,7 @@ static void rtsx_usb_ms_handle_req(struct work_struct *work)
 						host->req->error);
 			}
 		} while (!rc);
+		pm_runtime_put(ms_dev(host));
 	}
 
 }
@@ -570,6 +572,7 @@ static int rtsx_usb_ms_set_param(struct memstick_host *msh,
 	dev_dbg(ms_dev(host), "%s: param = %d, value = %d\n",
 			__func__, param, value);
 
+	pm_runtime_get_sync(ms_dev(host));
 	mutex_lock(&ucr->dev_mutex);
 
 	err = rtsx_usb_card_exclusive_check(ucr, RTSX_USB_MS_CARD);
@@ -635,6 +638,7 @@ static int rtsx_usb_ms_set_param(struct memstick_host *msh,
 	}
 out:
 	mutex_unlock(&ucr->dev_mutex);
+	pm_runtime_put(ms_dev(host));
 
 	/* power-on delay */
 	if (param == MEMSTICK_POWER && value == MEMSTICK_POWER_ON)
@@ -681,6 +685,7 @@ static int rtsx_usb_detect_ms_card(void *__host)
 	int err;
 
 	for (;;) {
+		pm_runtime_get_sync(ms_dev(host));
 		mutex_lock(&ucr->dev_mutex);
 
 		/* Check pending MS card changes */
@@ -703,6 +708,7 @@ static int rtsx_usb_detect_ms_card(void *__host)
 		}
 
 poll_again:
+		pm_runtime_put(ms_dev(host));
 		if (host->eject)
 			break;
 
diff --git a/drivers/message/fusion/mptbase.c b/drivers/message/fusion/mptbase.c
index 5537f8df8512..89c7ed16b4df 100644
--- a/drivers/message/fusion/mptbase.c
+++ b/drivers/message/fusion/mptbase.c
@@ -1865,8 +1865,8 @@ mpt_attach(struct pci_dev *pdev, const struct pci_device_id *id)
 
 	snprintf(ioc->reset_work_q_name, MPT_KOBJ_NAME_LEN,
 		 "mpt_poll_%d", ioc->id);
-	ioc->reset_work_q =
-		create_singlethread_workqueue(ioc->reset_work_q_name);
+	ioc->reset_work_q = alloc_workqueue(ioc->reset_work_q_name,
+					    WQ_MEM_RECLAIM, 0);
 	if (!ioc->reset_work_q) {
 		printk(MYIOC_s_ERR_FMT "Insufficient memory to add adapter!\n",
 		    ioc->name);
@@ -1992,7 +1992,8 @@ mpt_attach(struct pci_dev *pdev, const struct pci_device_id *id)
 	INIT_LIST_HEAD(&ioc->fw_event_list);
 	spin_lock_init(&ioc->fw_event_lock);
 	snprintf(ioc->fw_event_q_name, MPT_KOBJ_NAME_LEN, "mpt/%d", ioc->id);
-	ioc->fw_event_q = create_singlethread_workqueue(ioc->fw_event_q_name);
+	ioc->fw_event_q = alloc_workqueue(ioc->fw_event_q_name,
+					  WQ_MEM_RECLAIM, 0);
 	if (!ioc->fw_event_q) {
 		printk(MYIOC_s_ERR_FMT "Insufficient memory to add adapter!\n",
 		    ioc->name);
diff --git a/drivers/message/fusion/mptfc.c b/drivers/message/fusion/mptfc.c
index d8bf84aef602..add6a3a6ef0d 100644
--- a/drivers/message/fusion/mptfc.c
+++ b/drivers/message/fusion/mptfc.c
@@ -1324,9 +1324,12 @@ mptfc_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	snprintf(ioc->fc_rescan_work_q_name, sizeof(ioc->fc_rescan_work_q_name),
 		 "mptfc_wq_%d", sh->host_no);
 	ioc->fc_rescan_work_q =
-		create_singlethread_workqueue(ioc->fc_rescan_work_q_name);
-	if (!ioc->fc_rescan_work_q)
+		alloc_ordered_workqueue(ioc->fc_rescan_work_q_name,
+					WQ_MEM_RECLAIM);
+	if (!ioc->fc_rescan_work_q) {
+		error = -ENOMEM;
 		goto out_mptfc_probe;
+	}
 
 	/*
 	 *  Pre-fetch FC port WWN and stuff...
diff --git a/drivers/mfd/Kconfig b/drivers/mfd/Kconfig
index 2d1fb6420592..c6df6442ba2b 100644
--- a/drivers/mfd/Kconfig
+++ b/drivers/mfd/Kconfig
@@ -50,7 +50,7 @@ config MFD_AS3711
 	  Support for the AS3711 PMIC from AMS
 
 config MFD_AS3722
-	bool "ams AS3722 Power Management IC"
+	tristate "ams AS3722 Power Management IC"
 	select MFD_CORE
 	select REGMAP_I2C
 	select REGMAP_IRQ
@@ -112,6 +112,16 @@ config MFD_BCM590XX
 	help
 	  Support for the BCM590xx PMUs from Broadcom
 
+config MFD_AC100
+	tristate "X-Powers AC100"
+	select MFD_CORE
+	depends on SUNXI_RSB
+	help
+	  If you say Y here you get support for the X-Powers AC100 audio codec
+	  IC.
+	  This driver include only the core APIs. You have to select individual
+	  components like codecs or RTC under the corresponding menus.
+
 config MFD_AXP20X
 	tristate
 	select MFD_CORE
@@ -281,6 +291,14 @@ config MFD_DLN2
 	  etc. must be enabled in order to use the functionality of
 	  the device.
 
+config MFD_EXYNOS_LPASS
+	tristate "Samsung Exynos SoC Low Power Audio Subsystem"
+	select MFD_CORE
+	select REGMAP_MMIO
+	help
+	  Select this option to enable support for Samsung Exynos Low Power
+	  Audio Subsystem.
+
 config MFD_MC13XXX
 	tristate
 	depends on (SPI_MASTER || I2C)
@@ -340,14 +358,6 @@ config MFD_HI655X_PMIC
 	help
 	  Select this option to enable Hisilicon hi655x series pmic driver.
 
-config HTC_EGPIO
-	bool "HTC EGPIO support"
-	depends on GPIOLIB && ARM
-	help
-	    This driver supports the CPLD egpio chip present on
-	    several HTC phones.  It provides basic support for input
-	    pins, output pins, and irqs.
-
 config HTC_PASIC3
 	tristate "HTC PASIC3 LED/DS1WM chip support"
 	select MFD_CORE
@@ -852,13 +862,13 @@ config MFD_RC5T583
 	  different functionality of the device.
 
 config MFD_RK808
-	tristate "Rockchip RK808 Power Management chip"
+	tristate "Rockchip RK808/RK818 Power Management Chip"
 	depends on I2C && OF
 	select MFD_CORE
 	select REGMAP_I2C
 	select REGMAP_IRQ
 	help
-	  If you say yes here you get support for the RK808
+	  If you say yes here you get support for the RK808 and RK818
 	  Power Management chips.
 	  This driver provides common support for accessing the device
 	  through I2C interface. The device supports multiple sub-devices
@@ -1214,6 +1224,7 @@ config MFD_TPS65217
 	depends on I2C
 	select MFD_CORE
 	select REGMAP_I2C
+	select IRQ_DOMAIN
 	help
 	  If you say yes here you get support for the TPS65217 series of
 	  Power Management / White LED chips.
@@ -1224,6 +1235,20 @@ config MFD_TPS65217
 	  This driver can also be built as a module.  If so, the module
 	  will be called tps65217.
 
+config MFD_TI_LP873X
+	tristate "TI LP873X Power Management IC"
+	depends on I2C
+	select MFD_CORE
+	select REGMAP_I2C
+	help
+	  If you say yes here then you get support for the LP873X series of
+	  Power Management Integrated Circuits (PMIC).
+	  These include voltage regulators, thermal protection, configurable
+	  General Purpose Outputs (GPO) that are used in portable devices.
+
+	  This driver can also be built as a module. If so, the module
+	  will be called lp873x.
+
 config MFD_TPS65218
 	tristate "TI TPS65218 Power Management chips"
 	depends on I2C
@@ -1549,6 +1574,7 @@ config MFD_WM8350
 config MFD_WM8350_I2C
 	bool "Wolfson Microelectronics WM8350 with I2C"
 	select MFD_WM8350
+	select REGMAP_I2C
 	depends on I2C=y
 	help
 	  The WM8350 is an integrated audio and power management
diff --git a/drivers/mfd/Makefile b/drivers/mfd/Makefile
index 2ba3ba35f745..9834e669d985 100644
--- a/drivers/mfd/Makefile
+++ b/drivers/mfd/Makefile
@@ -13,15 +13,17 @@ obj-$(CONFIG_MFD_BCM590XX)	+= bcm590xx.o
 obj-$(CONFIG_MFD_CROS_EC)	+= cros_ec.o
 obj-$(CONFIG_MFD_CROS_EC_I2C)	+= cros_ec_i2c.o
 obj-$(CONFIG_MFD_CROS_EC_SPI)	+= cros_ec_spi.o
+obj-$(CONFIG_MFD_EXYNOS_LPASS)	+= exynos-lpass.o
 
 rtsx_pci-objs			:= rtsx_pcr.o rts5209.o rts5229.o rtl8411.o rts5227.o rts5249.o
 obj-$(CONFIG_MFD_RTSX_PCI)	+= rtsx_pci.o
 obj-$(CONFIG_MFD_RTSX_USB)	+= rtsx_usb.o
 
-obj-$(CONFIG_HTC_EGPIO)		+= htc-egpio.o
 obj-$(CONFIG_HTC_PASIC3)	+= htc-pasic3.o
 obj-$(CONFIG_HTC_I2CPLD)	+= htc-i2cpld.o
 
+obj-$(CONFIG_MFD_TI_LP873X)	+= lp873x.o
+
 obj-$(CONFIG_MFD_DAVINCI_VOICECODEC)	+= davinci_voicecodec.o
 obj-$(CONFIG_MFD_DM355EVM_MSP)	+= dm355evm_msp.o
 obj-$(CONFIG_MFD_TI_AM335X_TSCADC)	+= ti_am335x_tscadc.o
@@ -113,6 +115,8 @@ obj-$(CONFIG_PMIC_DA9052)	+= da9052-irq.o
 obj-$(CONFIG_PMIC_DA9052)	+= da9052-core.o
 obj-$(CONFIG_MFD_DA9052_SPI)	+= da9052-spi.o
 obj-$(CONFIG_MFD_DA9052_I2C)	+= da9052-i2c.o
+
+obj-$(CONFIG_MFD_AC100)		+= ac100.o
 obj-$(CONFIG_MFD_AXP20X)	+= axp20x.o
 obj-$(CONFIG_MFD_AXP20X_I2C)	+= axp20x-i2c.o
 obj-$(CONFIG_MFD_AXP20X_RSB)	+= axp20x-rsb.o
diff --git a/drivers/mfd/ab8500-debugfs.c b/drivers/mfd/ab8500-debugfs.c
index 0aecd7bd35f8..acf6c00b14b9 100644
--- a/drivers/mfd/ab8500-debugfs.c
+++ b/drivers/mfd/ab8500-debugfs.c
@@ -153,14 +153,14 @@ static struct hwreg_cfg hwreg_cfg = {
 
 #define AB8500_NAME_STRING "ab8500"
 #define AB8500_ADC_NAME_STRING "gpadc"
-#define AB8500_NUM_BANKS 24
+#define AB8500_NUM_BANKS AB8500_DEBUG_FIELD_LAST
 
 #define AB8500_REV_REG 0x80
 
 static struct ab8500_prcmu_ranges *debug_ranges;
 
 static struct ab8500_prcmu_ranges ab8500_debug_ranges[AB8500_NUM_BANKS] = {
-	[0x0] = {
+	[AB8500_M_FSM_RANK] = {
 		.num_ranges = 0,
 		.range = NULL,
 	},
@@ -315,7 +315,7 @@ static struct ab8500_prcmu_ranges ab8500_debug_ranges[AB8500_NUM_BANKS] = {
 			},
 		},
 	},
-	[0x9] = {
+	[AB8500_RESERVED] = {
 		.num_ranges = 0,
 		.range = NULL,
 	},
@@ -386,24 +386,6 @@ static struct ab8500_prcmu_ranges ab8500_debug_ranges[AB8500_NUM_BANKS] = {
 			},
 		},
 	},
-	[AB8500_DEVELOPMENT] = {
-		.num_ranges = 1,
-		.range = (struct ab8500_reg_range[]) {
-			{
-				.first = 0x00,
-				.last = 0x00,
-			},
-		},
-	},
-	[AB8500_DEBUG] = {
-		.num_ranges = 1,
-		.range = (struct ab8500_reg_range[]) {
-			{
-				.first = 0x05,
-				.last = 0x07,
-			},
-		},
-	},
 	[AB8500_AUDIO] = {
 		.num_ranges = 1,
 		.range = (struct ab8500_reg_range[]) {
@@ -463,19 +445,29 @@ static struct ab8500_prcmu_ranges ab8500_debug_ranges[AB8500_NUM_BANKS] = {
 			},
 		},
 	},
-	[0x11] = {
-		.num_ranges = 0,
-		.range = NULL,
+	[AB8500_DEVELOPMENT] = {
+		.num_ranges = 1,
+		.range = (struct ab8500_reg_range[]) {
+			{
+				.first = 0x00,
+				.last = 0x00,
+			},
+		},
 	},
-	[0x12] = {
-		.num_ranges = 0,
-		.range = NULL,
+	[AB8500_DEBUG] = {
+		.num_ranges = 1,
+		.range = (struct ab8500_reg_range[]) {
+			{
+				.first = 0x05,
+				.last = 0x07,
+			},
+		},
 	},
-	[0x13] = {
+	[AB8500_PROD_TEST] = {
 		.num_ranges = 0,
 		.range = NULL,
 	},
-	[0x14] = {
+	[AB8500_STE_TEST] = {
 		.num_ranges = 0,
 		.range = NULL,
 	},
@@ -1382,60 +1374,6 @@ void ab8500_dump_all_banks(struct device *dev)
 	}
 }
 
-/* Space for 500 registers. */
-#define DUMP_MAX_REGS 700
-static struct ab8500_register_dump
-{
-	u8 bank;
-	u8 reg;
-	u8 value;
-} ab8500_complete_register_dump[DUMP_MAX_REGS];
-
-/* This shall only be called upon kernel panic! */
-void ab8500_dump_all_banks_to_mem(void)
-{
-	int i, r = 0;
-	u8 bank;
-	int err = 0;
-
-	pr_info("Saving all ABB registers for crash analysis.\n");
-
-	for (bank = 0; bank < AB8500_NUM_BANKS; bank++) {
-		for (i = 0; i < debug_ranges[bank].num_ranges; i++) {
-			u8 reg;
-
-			for (reg = debug_ranges[bank].range[i].first;
-			     reg <= debug_ranges[bank].range[i].last;
-			     reg++) {
-				u8 value;
-
-				err = prcmu_abb_read(bank, reg, &value, 1);
-
-				if (err < 0)
-					goto out;
-
-				ab8500_complete_register_dump[r].bank = bank;
-				ab8500_complete_register_dump[r].reg = reg;
-				ab8500_complete_register_dump[r].value = value;
-
-				r++;
-
-				if (r >= DUMP_MAX_REGS) {
-					pr_err("%s: too many register to dump!\n",
-						__func__);
-					err = -EINVAL;
-					goto out;
-				}
-			}
-		}
-	}
-out:
-	if (err >= 0)
-		pr_info("Saved all ABB registers.\n");
-	else
-		pr_info("Failed to save all ABB registers.\n");
-}
-
 static int ab8500_all_banks_open(struct inode *inode, struct file *file)
 {
 	struct seq_file *s;
@@ -1584,18 +1522,10 @@ static u32 num_interrupts[AB8500_MAX_NR_IRQS];
 static u32 num_wake_interrupts[AB8500_MAX_NR_IRQS];
 static int num_interrupt_lines;
 
-bool __attribute__((weak)) suspend_test_wake_cause_interrupt_is_mine(u32 my_int)
-{
-	return false;
-}
-
 void ab8500_debug_register_interrupt(int line)
 {
-	if (line < num_interrupt_lines) {
+	if (line < num_interrupt_lines)
 		num_interrupts[line]++;
-		if (suspend_test_wake_cause_interrupt_is_mine(irq_ab8500))
-			num_wake_interrupts[line]++;
-	}
 }
 
 static int ab8500_interrupts_print(struct seq_file *s, void *p)
diff --git a/drivers/mfd/ac100.c b/drivers/mfd/ac100.c
new file mode 100644
index 000000000000..9bc69cd7807d
--- /dev/null
+++ b/drivers/mfd/ac100.c
@@ -0,0 +1,137 @@
+/*
+ * MFD core driver for X-Powers' AC100 Audio Codec IC
+ *
+ * The AC100 is a highly integrated audio codec and RTC subsystem designed
+ * for mobile applications. It has 3 I2S/PCM interfaces, a 2 channel DAC,
+ * a 2 channel ADC with 5 inputs and a builtin mixer. The RTC subsystem has
+ * 3 clock outputs.
+ *
+ * The audio codec and RTC parts are completely separate, sharing only the
+ * host interface for access to its registers.
+ *
+ * Copyright (2016) Chen-Yu Tsai
+ *
+ * Author: Chen-Yu Tsai <wens@csie.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/mfd/core.h>
+#include <linux/mfd/ac100.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/regmap.h>
+#include <linux/sunxi-rsb.h>
+
+static const struct regmap_range ac100_writeable_ranges[] = {
+	regmap_reg_range(AC100_CHIP_AUDIO_RST, AC100_I2S_SR_CTRL),
+	regmap_reg_range(AC100_I2S1_CLK_CTRL, AC100_I2S1_MXR_GAIN),
+	regmap_reg_range(AC100_I2S2_CLK_CTRL, AC100_I2S2_MXR_GAIN),
+	regmap_reg_range(AC100_I2S3_CLK_CTRL, AC100_I2S3_SIG_PATH_CTRL),
+	regmap_reg_range(AC100_ADC_DIG_CTRL, AC100_ADC_VOL_CTRL),
+	regmap_reg_range(AC100_HMIC_CTRL1, AC100_HMIC_STATUS),
+	regmap_reg_range(AC100_DAC_DIG_CTRL, AC100_DAC_MXR_GAIN),
+	regmap_reg_range(AC100_ADC_APC_CTRL, AC100_LINEOUT_CTRL),
+	regmap_reg_range(AC100_ADC_DAP_L_CTRL, AC100_ADC_DAP_OPT),
+	regmap_reg_range(AC100_DAC_DAP_CTRL, AC100_DAC_DAP_OPT),
+	regmap_reg_range(AC100_ADC_DAP_ENA, AC100_DAC_DAP_ENA),
+	regmap_reg_range(AC100_SRC1_CTRL1, AC100_SRC1_CTRL2),
+	regmap_reg_range(AC100_SRC2_CTRL1, AC100_SRC2_CTRL2),
+	regmap_reg_range(AC100_CLK32K_ANALOG_CTRL, AC100_CLKOUT_CTRL3),
+	regmap_reg_range(AC100_RTC_RST, AC100_RTC_UPD),
+	regmap_reg_range(AC100_ALM_INT_ENA, AC100_ALM_INT_STA),
+	regmap_reg_range(AC100_ALM_SEC, AC100_RTC_GP(15)),
+};
+
+static const struct regmap_range ac100_volatile_ranges[] = {
+	regmap_reg_range(AC100_CHIP_AUDIO_RST, AC100_PLL_CTRL2),
+	regmap_reg_range(AC100_HMIC_STATUS, AC100_HMIC_STATUS),
+	regmap_reg_range(AC100_ADC_DAP_L_STA, AC100_ADC_DAP_L_STA),
+	regmap_reg_range(AC100_SRC1_CTRL1, AC100_SRC1_CTRL1),
+	regmap_reg_range(AC100_SRC1_CTRL3, AC100_SRC2_CTRL1),
+	regmap_reg_range(AC100_SRC2_CTRL3, AC100_SRC2_CTRL4),
+	regmap_reg_range(AC100_RTC_RST, AC100_RTC_RST),
+	regmap_reg_range(AC100_RTC_SEC, AC100_ALM_INT_STA),
+	regmap_reg_range(AC100_ALM_SEC, AC100_ALM_UPD),
+};
+
+static const struct regmap_access_table ac100_writeable_table = {
+	.yes_ranges	= ac100_writeable_ranges,
+	.n_yes_ranges	= ARRAY_SIZE(ac100_writeable_ranges),
+};
+
+static const struct regmap_access_table ac100_volatile_table = {
+	.yes_ranges	= ac100_volatile_ranges,
+	.n_yes_ranges	= ARRAY_SIZE(ac100_volatile_ranges),
+};
+
+static const struct regmap_config ac100_regmap_config = {
+	.reg_bits	= 8,
+	.val_bits	= 16,
+	.wr_table	= &ac100_writeable_table,
+	.volatile_table	= &ac100_volatile_table,
+	.max_register	= AC100_RTC_GP(15),
+	.cache_type	= REGCACHE_RBTREE,
+};
+
+static struct mfd_cell ac100_cells[] = {
+	{
+		.name		= "ac100-codec",
+		.of_compatible	= "x-powers,ac100-codec",
+	}, {
+		.name		= "ac100-rtc",
+		.of_compatible	= "x-powers,ac100-rtc",
+	},
+};
+
+static int ac100_rsb_probe(struct sunxi_rsb_device *rdev)
+{
+	struct ac100_dev *ac100;
+	int ret;
+
+	ac100 = devm_kzalloc(&rdev->dev, sizeof(*ac100), GFP_KERNEL);
+	if (!ac100)
+		return -ENOMEM;
+
+	ac100->dev = &rdev->dev;
+	sunxi_rsb_device_set_drvdata(rdev, ac100);
+
+	ac100->regmap = devm_regmap_init_sunxi_rsb(rdev, &ac100_regmap_config);
+	if (IS_ERR(ac100->regmap)) {
+		ret = PTR_ERR(ac100->regmap);
+		dev_err(ac100->dev, "regmap init failed: %d\n", ret);
+		return ret;
+	}
+
+	ret = devm_mfd_add_devices(ac100->dev, PLATFORM_DEVID_NONE, ac100_cells,
+				   ARRAY_SIZE(ac100_cells), NULL, 0, NULL);
+	if (ret) {
+		dev_err(ac100->dev, "failed to add MFD devices: %d\n", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static const struct of_device_id ac100_of_match[] = {
+	{ .compatible = "x-powers,ac100" },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, ac100_of_match);
+
+static struct sunxi_rsb_driver ac100_rsb_driver = {
+	.driver = {
+		.name	= "ac100",
+		.of_match_table	= of_match_ptr(ac100_of_match),
+	},
+	.probe	= ac100_rsb_probe,
+};
+module_sunxi_rsb_driver(ac100_rsb_driver);
+
+MODULE_DESCRIPTION("Audio codec MFD core driver for AC100");
+MODULE_AUTHOR("Chen-Yu Tsai <wens@csie.org>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/mfd/act8945a.c b/drivers/mfd/act8945a.c
index 10c6d2da8822..a4024d91da01 100644
--- a/drivers/mfd/act8945a.c
+++ b/drivers/mfd/act8945a.c
@@ -23,6 +23,7 @@ static const struct mfd_cell act8945a_devs[] = {
 	},
 	{
 		.name = "act8945a-charger",
+		.of_compatible = "active-semi,act8945a-charger",
 	},
 };
 
diff --git a/drivers/mfd/altera-a10sr.c b/drivers/mfd/altera-a10sr.c
index c05aa4ff57fd..06e1f7fc5605 100644
--- a/drivers/mfd/altera-a10sr.c
+++ b/drivers/mfd/altera-a10sr.c
@@ -1,4 +1,8 @@
 /*
+ * Altera Arria10 DevKit System Resource MFD Driver
+ *
+ * Author: Thor Thayer <tthayer@opensource.altera.com>
+ *
  * Copyright Intel Corporation (C) 2014-2016. All Rights Reserved
  *
  * This program is free software; you can redistribute it and/or modify it
@@ -20,7 +24,7 @@
 
 #include <linux/mfd/altera-a10sr.h>
 #include <linux/mfd/core.h>
-#include <linux/module.h>
+#include <linux/init.h>
 #include <linux/of.h>
 #include <linux/spi/spi.h>
 
@@ -94,7 +98,7 @@ static bool altr_a10sr_reg_volatile(struct device *dev, unsigned int reg)
 	}
 }
 
-const struct regmap_config altr_a10sr_regmap_config = {
+static const struct regmap_config altr_a10sr_regmap_config = {
 	.reg_bits = 8,
 	.val_bits = 8,
 
@@ -152,7 +156,6 @@ static const struct of_device_id altr_a10sr_spi_of_match[] = {
 	{ .compatible = "altr,a10sr" },
 	{ },
 };
-MODULE_DEVICE_TABLE(of, altr_a10sr_spi_of_match);
 
 static struct spi_driver altr_a10sr_spi_driver = {
 	.probe = altr_a10sr_spi_probe,
@@ -161,9 +164,4 @@ static struct spi_driver altr_a10sr_spi_driver = {
 		.of_match_table = of_match_ptr(altr_a10sr_spi_of_match),
 	},
 };
-
-module_spi_driver(altr_a10sr_spi_driver);
-
-MODULE_LICENSE("GPL v2");
-MODULE_AUTHOR("Thor Thayer <tthayer@opensource.altera.com>");
-MODULE_DESCRIPTION("Altera Arria10 DevKit System Resource MFD Driver");
+builtin_driver(altr_a10sr_spi_driver, spi_register_driver)
diff --git a/drivers/mfd/arizona-core.c b/drivers/mfd/arizona-core.c
index e4f97b3c824b..41767f7239bb 100644
--- a/drivers/mfd/arizona-core.c
+++ b/drivers/mfd/arizona-core.c
@@ -10,6 +10,7 @@
  * published by the Free Software Foundation.
  */
 
+#include <linux/clk.h>
 #include <linux/delay.h>
 #include <linux/err.h>
 #include <linux/gpio.h>
@@ -49,7 +50,15 @@ int arizona_clk32k_enable(struct arizona *arizona)
 		case ARIZONA_32KZ_MCLK1:
 			ret = pm_runtime_get_sync(arizona->dev);
 			if (ret != 0)
-				goto out;
+				goto err_ref;
+			ret = clk_prepare_enable(arizona->mclk[ARIZONA_MCLK1]);
+			if (ret != 0)
+				goto err_pm;
+			break;
+		case ARIZONA_32KZ_MCLK2:
+			ret = clk_prepare_enable(arizona->mclk[ARIZONA_MCLK2]);
+			if (ret != 0)
+				goto err_ref;
 			break;
 		}
 
@@ -58,7 +67,9 @@ int arizona_clk32k_enable(struct arizona *arizona)
 					 ARIZONA_CLK_32K_ENA);
 	}
 
-out:
+err_pm:
+	pm_runtime_put_sync(arizona->dev);
+err_ref:
 	if (ret != 0)
 		arizona->clk32k_ref--;
 
@@ -83,6 +94,10 @@ int arizona_clk32k_disable(struct arizona *arizona)
 		switch (arizona->pdata.clk32k_src) {
 		case ARIZONA_32KZ_MCLK1:
 			pm_runtime_put_sync(arizona->dev);
+			clk_disable_unprepare(arizona->mclk[ARIZONA_MCLK1]);
+			break;
+		case ARIZONA_32KZ_MCLK2:
+			clk_disable_unprepare(arizona->mclk[ARIZONA_MCLK2]);
 			break;
 		}
 	}
@@ -735,7 +750,7 @@ static int arizona_suspend(struct device *dev)
 	return 0;
 }
 
-static int arizona_suspend_late(struct device *dev)
+static int arizona_suspend_noirq(struct device *dev)
 {
 	struct arizona *arizona = dev_get_drvdata(dev);
 
@@ -759,7 +774,7 @@ static int arizona_resume(struct device *dev)
 {
 	struct arizona *arizona = dev_get_drvdata(dev);
 
-	dev_dbg(arizona->dev, "Late resume, reenabling IRQ\n");
+	dev_dbg(arizona->dev, "Resume, reenabling IRQ\n");
 	enable_irq(arizona->irq);
 
 	return 0;
@@ -771,10 +786,8 @@ const struct dev_pm_ops arizona_pm_ops = {
 			   arizona_runtime_resume,
 			   NULL)
 	SET_SYSTEM_SLEEP_PM_OPS(arizona_suspend, arizona_resume)
-#ifdef CONFIG_PM_SLEEP
-	.suspend_late = arizona_suspend_late,
-	.resume_noirq = arizona_resume_noirq,
-#endif
+	SET_NOIRQ_SYSTEM_SLEEP_PM_OPS(arizona_suspend_noirq,
+				      arizona_resume_noirq)
 };
 EXPORT_SYMBOL_GPL(arizona_pm_ops);
 
@@ -790,35 +803,25 @@ unsigned long arizona_of_get_type(struct device *dev)
 }
 EXPORT_SYMBOL_GPL(arizona_of_get_type);
 
-int arizona_of_get_named_gpio(struct arizona *arizona, const char *prop,
-			      bool mandatory)
-{
-	int gpio;
-
-	gpio = of_get_named_gpio(arizona->dev->of_node, prop, 0);
-	if (gpio < 0) {
-		if (mandatory)
-			dev_err(arizona->dev,
-				"Mandatory DT gpio %s missing/malformed: %d\n",
-				prop, gpio);
-
-		gpio = 0;
-	}
-
-	return gpio;
-}
-EXPORT_SYMBOL_GPL(arizona_of_get_named_gpio);
-
 static int arizona_of_get_core_pdata(struct arizona *arizona)
 {
 	struct arizona_pdata *pdata = &arizona->pdata;
 	struct property *prop;
 	const __be32 *cur;
 	u32 val;
+	u32 pdm_val[ARIZONA_MAX_PDM_SPK];
 	int ret, i;
 	int count = 0;
 
-	pdata->reset = arizona_of_get_named_gpio(arizona, "wlf,reset", true);
+	pdata->reset = of_get_named_gpio(arizona->dev->of_node, "wlf,reset", 0);
+	if (pdata->reset == -EPROBE_DEFER) {
+		return pdata->reset;
+	} else if (pdata->reset < 0) {
+		dev_err(arizona->dev, "Reset GPIO missing/malformed: %d\n",
+			pdata->reset);
+
+		pdata->reset = 0;
+	}
 
 	ret = of_property_read_u32_array(arizona->dev->of_node,
 					 "wlf,gpio-defaults",
@@ -871,6 +874,35 @@ static int arizona_of_get_core_pdata(struct arizona *arizona)
 		count++;
 	}
 
+	count = 0;
+	of_property_for_each_u32(arizona->dev->of_node,
+				 "wlf,max-channels-clocked",
+				 prop, cur, val) {
+		if (count == ARRAY_SIZE(pdata->max_channels_clocked))
+			break;
+
+		pdata->max_channels_clocked[count] = val;
+		count++;
+	}
+
+	ret = of_property_read_u32_array(arizona->dev->of_node,
+					 "wlf,spk-fmt",
+					 pdm_val,
+					 ARRAY_SIZE(pdm_val));
+
+	if (ret >= 0)
+		for (count = 0; count < ARRAY_SIZE(pdata->spk_fmt); ++count)
+			pdata->spk_fmt[count] = pdm_val[count];
+
+	ret = of_property_read_u32_array(arizona->dev->of_node,
+					 "wlf,spk-mute",
+					 pdm_val,
+					 ARRAY_SIZE(pdm_val));
+
+	if (ret >= 0)
+		for (count = 0; count < ARRAY_SIZE(pdata->spk_mute); ++count)
+			pdata->spk_mute[count] = pdm_val[count];
+
 	return 0;
 }
 
@@ -1000,6 +1032,7 @@ static const struct mfd_cell wm8998_devs[] = {
 
 int arizona_dev_init(struct arizona *arizona)
 {
+	const char * const mclk_name[] = { "mclk1", "mclk2" };
 	struct device *dev = arizona->dev;
 	const char *type_name = NULL;
 	unsigned int reg, val, mask;
@@ -1010,11 +1043,24 @@ int arizona_dev_init(struct arizona *arizona)
 	dev_set_drvdata(arizona->dev, arizona);
 	mutex_init(&arizona->clk_lock);
 
-	if (dev_get_platdata(arizona->dev))
+	if (dev_get_platdata(arizona->dev)) {
 		memcpy(&arizona->pdata, dev_get_platdata(arizona->dev),
 		       sizeof(arizona->pdata));
-	else
-		arizona_of_get_core_pdata(arizona);
+	} else {
+		ret = arizona_of_get_core_pdata(arizona);
+		if (ret < 0)
+			return ret;
+	}
+
+	BUILD_BUG_ON(ARRAY_SIZE(arizona->mclk) != ARRAY_SIZE(mclk_name));
+	for (i = 0; i < ARRAY_SIZE(arizona->mclk); i++) {
+		arizona->mclk[i] = devm_clk_get(arizona->dev, mclk_name[i]);
+		if (IS_ERR(arizona->mclk[i])) {
+			dev_info(arizona->dev, "Failed to get %s: %ld\n",
+				 mclk_name[i], PTR_ERR(arizona->mclk[i]));
+			arizona->mclk[i] = NULL;
+		}
+	}
 
 	regcache_cache_only(arizona->regmap, true);
 
@@ -1035,7 +1081,7 @@ int arizona_dev_init(struct arizona *arizona)
 	default:
 		dev_err(arizona->dev, "Unknown device type %d\n",
 			arizona->type);
-		return -EINVAL;
+		return -ENODEV;
 	}
 
 	/* Mark DCVDD as external, LDO1 driver will clear if internal */
@@ -1121,6 +1167,7 @@ int arizona_dev_init(struct arizona *arizona)
 		break;
 	default:
 		dev_err(arizona->dev, "Unknown device ID: %x\n", reg);
+		ret = -ENODEV;
 		goto err_reset;
 	}
 
@@ -1280,12 +1327,14 @@ int arizona_dev_init(struct arizona *arizona)
 		break;
 	default:
 		dev_err(arizona->dev, "Unknown device ID %x\n", reg);
+		ret = -ENODEV;
 		goto err_reset;
 	}
 
 	if (!subdevs) {
 		dev_err(arizona->dev,
 			"No kernel support for device ID %x\n", reg);
+		ret = -ENODEV;
 		goto err_reset;
 	}
 
diff --git a/drivers/mfd/atmel-hlcdc.c b/drivers/mfd/atmel-hlcdc.c
index eca7ea69b81c..4b15b0840f16 100644
--- a/drivers/mfd/atmel-hlcdc.c
+++ b/drivers/mfd/atmel-hlcdc.c
@@ -50,8 +50,9 @@ static int regmap_atmel_hlcdc_reg_write(void *context, unsigned int reg,
 	if (reg <= ATMEL_HLCDC_DIS) {
 		u32 status;
 
-		readl_poll_timeout(hregmap->regs + ATMEL_HLCDC_SR, status,
-				   !(status & ATMEL_HLCDC_SIP), 1, 100);
+		readl_poll_timeout_atomic(hregmap->regs + ATMEL_HLCDC_SR,
+					  status, !(status & ATMEL_HLCDC_SIP),
+					  1, 100);
 	}
 
 	writel(val, hregmap->regs + reg);
diff --git a/drivers/mfd/axp20x-rsb.c b/drivers/mfd/axp20x-rsb.c
index a407527bcd09..a732cb50bcff 100644
--- a/drivers/mfd/axp20x-rsb.c
+++ b/drivers/mfd/axp20x-rsb.c
@@ -61,6 +61,7 @@ static int axp20x_rsb_remove(struct sunxi_rsb_device *rdev)
 
 static const struct of_device_id axp20x_rsb_of_match[] = {
 	{ .compatible = "x-powers,axp223", .data = (void *)AXP223_ID },
+	{ .compatible = "x-powers,axp806", .data = (void *)AXP806_ID },
 	{ .compatible = "x-powers,axp809", .data = (void *)AXP809_ID },
 	{ },
 };
diff --git a/drivers/mfd/axp20x.c b/drivers/mfd/axp20x.c
index fd80b0981f0f..ba130be32e61 100644
--- a/drivers/mfd/axp20x.c
+++ b/drivers/mfd/axp20x.c
@@ -38,6 +38,7 @@ static const char * const axp20x_model_names[] = {
 	"AXP221",
 	"AXP223",
 	"AXP288",
+	"AXP806",
 	"AXP809",
 };
 
@@ -129,6 +130,27 @@ static const struct regmap_access_table axp288_volatile_table = {
 	.n_yes_ranges	= ARRAY_SIZE(axp288_volatile_ranges),
 };
 
+static const struct regmap_range axp806_writeable_ranges[] = {
+	regmap_reg_range(AXP20X_DATACACHE(0), AXP20X_DATACACHE(3)),
+	regmap_reg_range(AXP806_PWR_OUT_CTRL1, AXP806_CLDO3_V_CTRL),
+	regmap_reg_range(AXP20X_IRQ1_EN, AXP20X_IRQ2_EN),
+	regmap_reg_range(AXP20X_IRQ1_STATE, AXP20X_IRQ2_STATE),
+};
+
+static const struct regmap_range axp806_volatile_ranges[] = {
+	regmap_reg_range(AXP20X_IRQ1_STATE, AXP20X_IRQ2_STATE),
+};
+
+static const struct regmap_access_table axp806_writeable_table = {
+	.yes_ranges	= axp806_writeable_ranges,
+	.n_yes_ranges	= ARRAY_SIZE(axp806_writeable_ranges),
+};
+
+static const struct regmap_access_table axp806_volatile_table = {
+	.yes_ranges	= axp806_volatile_ranges,
+	.n_yes_ranges	= ARRAY_SIZE(axp806_volatile_ranges),
+};
+
 static struct resource axp152_pek_resources[] = {
 	DEFINE_RES_IRQ_NAMED(AXP152_IRQ_PEK_RIS_EDGE, "PEK_DBR"),
 	DEFINE_RES_IRQ_NAMED(AXP152_IRQ_PEK_FAL_EDGE, "PEK_DBF"),
@@ -278,6 +300,15 @@ static const struct regmap_config axp288_regmap_config = {
 	.cache_type	= REGCACHE_RBTREE,
 };
 
+static const struct regmap_config axp806_regmap_config = {
+	.reg_bits	= 8,
+	.val_bits	= 8,
+	.wr_table	= &axp806_writeable_table,
+	.volatile_table	= &axp806_volatile_table,
+	.max_register	= AXP806_VREF_TEMP_WARN_L,
+	.cache_type	= REGCACHE_RBTREE,
+};
+
 #define INIT_REGMAP_IRQ(_variant, _irq, _off, _mask)			\
 	[_variant##_IRQ_##_irq] = { .reg_offset = (_off), .mask = BIT(_mask) }
 
@@ -409,6 +440,21 @@ static const struct regmap_irq axp288_regmap_irqs[] = {
 	INIT_REGMAP_IRQ(AXP288, BC_USB_CHNG,            5, 1),
 };
 
+static const struct regmap_irq axp806_regmap_irqs[] = {
+	INIT_REGMAP_IRQ(AXP806, DIE_TEMP_HIGH_LV1,	0, 0),
+	INIT_REGMAP_IRQ(AXP806, DIE_TEMP_HIGH_LV2,	0, 1),
+	INIT_REGMAP_IRQ(AXP806, DCDCA_V_LOW,		0, 3),
+	INIT_REGMAP_IRQ(AXP806, DCDCB_V_LOW,		0, 4),
+	INIT_REGMAP_IRQ(AXP806, DCDCC_V_LOW,		0, 5),
+	INIT_REGMAP_IRQ(AXP806, DCDCD_V_LOW,		0, 6),
+	INIT_REGMAP_IRQ(AXP806, DCDCE_V_LOW,		0, 7),
+	INIT_REGMAP_IRQ(AXP806, PWROK_LONG,		1, 0),
+	INIT_REGMAP_IRQ(AXP806, PWROK_SHORT,		1, 1),
+	INIT_REGMAP_IRQ(AXP806, WAKEUP,			1, 4),
+	INIT_REGMAP_IRQ(AXP806, PWROK_FALL,		1, 5),
+	INIT_REGMAP_IRQ(AXP806, PWROK_RISE,		1, 6),
+};
+
 static const struct regmap_irq axp809_regmap_irqs[] = {
 	INIT_REGMAP_IRQ(AXP809, ACIN_OVER_V,		0, 7),
 	INIT_REGMAP_IRQ(AXP809, ACIN_PLUGIN,		0, 6),
@@ -494,6 +540,18 @@ static const struct regmap_irq_chip axp288_regmap_irq_chip = {
 
 };
 
+static const struct regmap_irq_chip axp806_regmap_irq_chip = {
+	.name			= "axp806",
+	.status_base		= AXP20X_IRQ1_STATE,
+	.ack_base		= AXP20X_IRQ1_STATE,
+	.mask_base		= AXP20X_IRQ1_EN,
+	.mask_invert		= true,
+	.init_ack_masked	= true,
+	.irqs			= axp806_regmap_irqs,
+	.num_irqs		= ARRAY_SIZE(axp806_regmap_irqs),
+	.num_regs		= 2,
+};
+
 static const struct regmap_irq_chip axp809_regmap_irq_chip = {
 	.name			= "axp809",
 	.status_base		= AXP20X_IRQ1_STATE,
@@ -508,6 +566,9 @@ static const struct regmap_irq_chip axp809_regmap_irq_chip = {
 
 static struct mfd_cell axp20x_cells[] = {
 	{
+		.name		= "axp20x-gpio",
+		.of_compatible	= "x-powers,axp209-gpio",
+	}, {
 		.name		= "axp20x-pek",
 		.num_resources	= ARRAY_SIZE(axp20x_pek_resources),
 		.resources	= axp20x_pek_resources,
@@ -660,12 +721,20 @@ static struct mfd_cell axp288_cells[] = {
 	},
 };
 
+static struct mfd_cell axp806_cells[] = {
+	{
+		.id			= 2,
+		.name			= "axp20x-regulator",
+	},
+};
+
 static struct mfd_cell axp809_cells[] = {
 	{
 		.name			= "axp20x-pek",
 		.num_resources		= ARRAY_SIZE(axp809_pek_resources),
 		.resources		= axp809_pek_resources,
 	}, {
+		.id			= 1,
 		.name			= "axp20x-regulator",
 	},
 };
@@ -732,6 +801,12 @@ int axp20x_match_device(struct axp20x_dev *axp20x)
 		axp20x->regmap_cfg = &axp288_regmap_config;
 		axp20x->regmap_irq_chip = &axp288_regmap_irq_chip;
 		break;
+	case AXP806_ID:
+		axp20x->nr_cells = ARRAY_SIZE(axp806_cells);
+		axp20x->cells = axp806_cells;
+		axp20x->regmap_cfg = &axp806_regmap_config;
+		axp20x->regmap_irq_chip = &axp806_regmap_irq_chip;
+		break;
 	case AXP809_ID:
 		axp20x->nr_cells = ARRAY_SIZE(axp809_cells);
 		axp20x->cells = axp809_cells;
diff --git a/drivers/mfd/cros_ec.c b/drivers/mfd/cros_ec.c
index 0eee63542038..abd83424b498 100644
--- a/drivers/mfd/cros_ec.c
+++ b/drivers/mfd/cros_ec.c
@@ -23,6 +23,7 @@
 #include <linux/module.h>
 #include <linux/mfd/core.h>
 #include <linux/mfd/cros_ec.h>
+#include <asm/unaligned.h>
 
 #define CROS_EC_DEV_EC_INDEX 0
 #define CROS_EC_DEV_PD_INDEX 1
@@ -49,11 +50,28 @@ static const struct mfd_cell ec_pd_cell = {
 	.pdata_size = sizeof(pd_p),
 };
 
+static irqreturn_t ec_irq_thread(int irq, void *data)
+{
+	struct cros_ec_device *ec_dev = data;
+	int ret;
+
+	if (device_may_wakeup(ec_dev->dev))
+		pm_wakeup_event(ec_dev->dev, 0);
+
+	ret = cros_ec_get_next_event(ec_dev);
+	if (ret > 0)
+		blocking_notifier_call_chain(&ec_dev->event_notifier,
+					     0, ec_dev);
+	return IRQ_HANDLED;
+}
+
 int cros_ec_register(struct cros_ec_device *ec_dev)
 {
 	struct device *dev = ec_dev->dev;
 	int err = 0;
 
+	BLOCKING_INIT_NOTIFIER_HEAD(&ec_dev->event_notifier);
+
 	ec_dev->max_request = sizeof(struct ec_params_hello);
 	ec_dev->max_response = sizeof(struct ec_response_get_protocol_info);
 	ec_dev->max_passthru = 0;
@@ -70,13 +88,24 @@ int cros_ec_register(struct cros_ec_device *ec_dev)
 
 	cros_ec_query_all(ec_dev);
 
+	if (ec_dev->irq) {
+		err = request_threaded_irq(ec_dev->irq, NULL, ec_irq_thread,
+					   IRQF_TRIGGER_LOW | IRQF_ONESHOT,
+					   "chromeos-ec", ec_dev);
+		if (err) {
+			dev_err(dev, "Failed to request IRQ %d: %d",
+				ec_dev->irq, err);
+			return err;
+		}
+	}
+
 	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 register Embedded Controller subdevice %d\n",
 			err);
-		return err;
+		goto fail_mfd;
 	}
 
 	if (ec_dev->max_passthru) {
@@ -94,7 +123,7 @@ int cros_ec_register(struct cros_ec_device *ec_dev)
 			dev_err(dev,
 				"Failed to register Power Delivery subdevice %d\n",
 				err);
-			return err;
+			goto fail_mfd;
 		}
 	}
 
@@ -103,13 +132,18 @@ int cros_ec_register(struct cros_ec_device *ec_dev)
 		if (err) {
 			mfd_remove_devices(dev);
 			dev_err(dev, "Failed to register sub-devices\n");
-			return err;
+			goto fail_mfd;
 		}
 	}
 
 	dev_info(dev, "Chrome EC device registered\n");
 
 	return 0;
+
+fail_mfd:
+	if (ec_dev->irq)
+		free_irq(ec_dev->irq, ec_dev);
+	return err;
 }
 EXPORT_SYMBOL(cros_ec_register);
 
@@ -136,13 +170,31 @@ int cros_ec_suspend(struct cros_ec_device *ec_dev)
 }
 EXPORT_SYMBOL(cros_ec_suspend);
 
+static void cros_ec_drain_events(struct cros_ec_device *ec_dev)
+{
+	while (cros_ec_get_next_event(ec_dev) > 0)
+		blocking_notifier_call_chain(&ec_dev->event_notifier,
+					     1, ec_dev);
+}
+
 int cros_ec_resume(struct cros_ec_device *ec_dev)
 {
 	enable_irq(ec_dev->irq);
 
+	/*
+	 * In some cases, we need to distinguish between events that occur
+	 * during suspend if the EC is not a wake source. For example,
+	 * keypresses during suspend should be discarded if it does not wake
+	 * the system.
+	 *
+	 * If the EC is not a wake source, drain the event queue and mark them
+	 * as "queued during suspend".
+	 */
 	if (ec_dev->wake_enabled) {
 		disable_irq_wake(ec_dev->irq);
 		ec_dev->wake_enabled = 0;
+	} else {
+		cros_ec_drain_events(ec_dev);
 	}
 
 	return 0;
diff --git a/drivers/mfd/cros_ec_spi.c b/drivers/mfd/cros_ec_spi.c
index ebe9b9477cb2..a518832ed5f5 100644
--- a/drivers/mfd/cros_ec_spi.c
+++ b/drivers/mfd/cros_ec_spi.c
@@ -366,7 +366,6 @@ static int cros_ec_spi_receive_response(struct cros_ec_device *ec_dev,
 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;
@@ -378,7 +377,6 @@ static int cros_ec_pkt_xfer_spi(struct cros_ec_device *ec_dev,
 	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 */
diff --git a/drivers/mfd/da9052-core.c b/drivers/mfd/da9052-core.c
index c0bf68a3e614..a88c2065d8ab 100644
--- a/drivers/mfd/da9052-core.c
+++ b/drivers/mfd/da9052-core.c
@@ -167,6 +167,7 @@ static bool da9052_reg_writeable(struct device *dev, unsigned int reg)
 	case DA9052_EVENT_B_REG:
 	case DA9052_EVENT_C_REG:
 	case DA9052_EVENT_D_REG:
+	case DA9052_FAULTLOG_REG:
 	case DA9052_IRQ_MASK_A_REG:
 	case DA9052_IRQ_MASK_B_REG:
 	case DA9052_IRQ_MASK_C_REG:
@@ -541,6 +542,52 @@ const struct regmap_config da9052_regmap_config = {
 };
 EXPORT_SYMBOL_GPL(da9052_regmap_config);
 
+static int da9052_clear_fault_log(struct da9052 *da9052)
+{
+	int ret = 0;
+	int fault_log = 0;
+
+	fault_log = da9052_reg_read(da9052, DA9052_FAULTLOG_REG);
+	if (fault_log < 0) {
+		dev_err(da9052->dev,
+			"Cannot read FAULT_LOG %d\n", fault_log);
+		return fault_log;
+	}
+
+	if (fault_log) {
+		if (fault_log & DA9052_FAULTLOG_TWDERROR)
+			dev_dbg(da9052->dev,
+				"Fault log entry detected: TWD_ERROR\n");
+		if (fault_log & DA9052_FAULTLOG_VDDFAULT)
+			dev_dbg(da9052->dev,
+				"Fault log entry detected: VDD_FAULT\n");
+		if (fault_log & DA9052_FAULTLOG_VDDSTART)
+			dev_dbg(da9052->dev,
+				"Fault log entry detected: VDD_START\n");
+		if (fault_log & DA9052_FAULTLOG_TEMPOVER)
+			dev_dbg(da9052->dev,
+				"Fault log entry detected: TEMP_OVER\n");
+		if (fault_log & DA9052_FAULTLOG_KEYSHUT)
+			dev_dbg(da9052->dev,
+				"Fault log entry detected: KEY_SHUT\n");
+		if (fault_log & DA9052_FAULTLOG_NSDSET)
+			dev_dbg(da9052->dev,
+				"Fault log entry detected: nSD_SHUT\n");
+		if (fault_log & DA9052_FAULTLOG_WAITSET)
+			dev_dbg(da9052->dev,
+				"Fault log entry detected: WAIT_SHUT\n");
+
+		ret = da9052_reg_write(da9052,
+					DA9052_FAULTLOG_REG,
+					0xFF);
+		if (ret < 0)
+			dev_err(da9052->dev,
+				"Cannot reset FAULT_LOG values %d\n", ret);
+	}
+
+	return ret;
+}
+
 int da9052_device_init(struct da9052 *da9052, u8 chip_id)
 {
 	struct da9052_pdata *pdata = dev_get_platdata(da9052->dev);
@@ -549,6 +596,10 @@ int da9052_device_init(struct da9052 *da9052, u8 chip_id)
 	mutex_init(&da9052->auxadc_lock);
 	init_completion(&da9052->done);
 
+	ret = da9052_clear_fault_log(da9052);
+	if (ret < 0)
+		dev_warn(da9052->dev, "Cannot clear FAULT_LOG\n");
+
 	if (pdata && pdata->init != NULL)
 		pdata->init(da9052);
 
diff --git a/drivers/mfd/da9063-core.c b/drivers/mfd/da9063-core.c
index af841c165787..6c2870d4e754 100644
--- a/drivers/mfd/da9063-core.c
+++ b/drivers/mfd/da9063-core.c
@@ -4,8 +4,8 @@
  * Copyright 2012 Dialog Semiconductors Ltd.
  * Copyright 2013 Philipp Zabel, Pengutronix
  *
- * Author: Krystian Garbaciak <krystian.garbaciak@diasemi.com>,
- *         Michal Hajduk <michal.hajduk@diasemi.com>
+ * Author: Krystian Garbaciak, Dialog Semiconductor
+ * Author: Michal Hajduk, Dialog Semiconductor
  *
  *  This program is free software; you can redistribute  it and/or modify it
  *  under  the terms of  the GNU General  Public License as published by the
@@ -242,5 +242,6 @@ void da9063_device_exit(struct da9063 *da9063)
 }
 
 MODULE_DESCRIPTION("PMIC driver for Dialog DA9063");
-MODULE_AUTHOR("Krystian Garbaciak <krystian.garbaciak@diasemi.com>, Michal Hajduk <michal.hajduk@diasemi.com>");
+MODULE_AUTHOR("Krystian Garbaciak");
+MODULE_AUTHOR("Michal Hajduk");
 MODULE_LICENSE("GPL");
diff --git a/drivers/mfd/da9063-i2c.c b/drivers/mfd/da9063-i2c.c
index 73901084945f..981805a2c521 100644
--- a/drivers/mfd/da9063-i2c.c
+++ b/drivers/mfd/da9063-i2c.c
@@ -3,7 +3,7 @@
  * Copyright 2012 Dialog Semiconductor Ltd.
  * Copyright 2013 Philipp Zabel, Pengutronix
  *
- * Author: Krystian Garbaciak <krystian.garbaciak@diasemi.com>
+ * Author: Krystian Garbaciak, Dialog Semiconductor
  *
  *  This program is free software; you can redistribute  it and/or modify it
  *  under  the terms of  the GNU General  Public License as published by the
diff --git a/drivers/mfd/da9063-irq.c b/drivers/mfd/da9063-irq.c
index 7e903fcb8813..207bbfe55449 100644
--- a/drivers/mfd/da9063-irq.c
+++ b/drivers/mfd/da9063-irq.c
@@ -3,7 +3,7 @@
  * Copyright 2012 Dialog Semiconductor Ltd.
  * Copyright 2013 Philipp Zabel, Pengutronix
  *
- * Author: Michal Hajduk <michal.hajduk@diasemi.com>
+ * Author: Michal Hajduk, Dialog Semiconductor
  *
  *  This program is free software; you can redistribute  it and/or modify it
  *  under  the terms of  the GNU General  Public License as published by the
diff --git a/drivers/mfd/db8500-prcmu.c b/drivers/mfd/db8500-prcmu.c
index 388e268b9bcf..ca38a6a14110 100644
--- a/drivers/mfd/db8500-prcmu.c
+++ b/drivers/mfd/db8500-prcmu.c
@@ -938,25 +938,6 @@ int db8500_prcmu_get_ddr_opp(void)
 	return readb(PRCM_DDR_SUBSYS_APE_MINBW);
 }
 
-/**
- * db8500_set_ddr_opp - set the appropriate DDR OPP
- * @opp: The new DDR operating point to which transition is to be made
- * Returns: 0 on success, non-zero on failure
- *
- * This function sets the operating point of the DDR.
- */
-static bool enable_set_ddr_opp;
-int db8500_prcmu_set_ddr_opp(u8 opp)
-{
-	if (opp < DDR_100_OPP || opp > DDR_25_OPP)
-		return -EINVAL;
-	/* Changing the DDR OPP can hang the hardware pre-v21 */
-	if (enable_set_ddr_opp)
-		writeb(opp, PRCM_DDR_SUBSYS_APE_MINBW);
-
-	return 0;
-}
-
 /* Divide the frequency of certain clocks by 2 for APE_50_PARTLY_25_OPP. */
 static void request_even_slower_clocks(bool enable)
 {
diff --git a/drivers/mfd/dm355evm_msp.c b/drivers/mfd/dm355evm_msp.c
index 270e19c0bba1..86eca614507b 100644
--- a/drivers/mfd/dm355evm_msp.c
+++ b/drivers/mfd/dm355evm_msp.c
@@ -209,7 +209,7 @@ static struct device *add_child(struct i2c_client *client, const char *name,
 		status = platform_device_add_data(pdev, pdata, pdata_len);
 		if (status < 0) {
 			dev_dbg(&pdev->dev, "can't add platform_data\n");
-			goto err;
+			goto put_device;
 		}
 	}
 
@@ -222,19 +222,20 @@ static struct device *add_child(struct i2c_client *client, const char *name,
 		status = platform_device_add_resources(pdev, &r, 1);
 		if (status < 0) {
 			dev_dbg(&pdev->dev, "can't add irq\n");
-			goto err;
+			goto put_device;
 		}
 	}
 
 	status = platform_device_add(pdev);
+	if (status)
+		goto put_device;
 
-err:
-	if (status < 0) {
-		platform_device_put(pdev);
-		dev_err(&client->dev, "can't add %s dev\n", name);
-		return ERR_PTR(status);
-	}
 	return &pdev->dev;
+
+put_device:
+	platform_device_put(pdev);
+	dev_err(&client->dev, "failed to add device %s\n", name);
+	return ERR_PTR(status);
 }
 
 static int add_children(struct i2c_client *client)
diff --git a/drivers/mfd/exynos-lpass.c b/drivers/mfd/exynos-lpass.c
new file mode 100644
index 000000000000..2e064fb8826f
--- /dev/null
+++ b/drivers/mfd/exynos-lpass.c
@@ -0,0 +1,185 @@
+/*
+ * Copyright (C) 2015 - 2016 Samsung Electronics Co., Ltd.
+ *
+ * Authors: Inha Song <ideal.song@samsung.com>
+ *          Sylwester Nawrocki <s.nawrocki@samsung.com>
+ *
+ * Samsung Exynos SoC series Low Power Audio Subsystem driver.
+ *
+ * This module provides regmap for the Top SFR region and instantiates
+ * devices for IP blocks like DMAC, I2S, UART.
+ *
+ * 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.
+ */
+
+#include <linux/delay.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/mfd/syscon.h>
+#include <linux/mfd/syscon/exynos5-pmu.h>
+#include <linux/of.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+#include <linux/types.h>
+
+/* LPASS Top register definitions */
+#define SFR_LPASS_CORE_SW_RESET		0x08
+#define  LPASS_SB_SW_RESET		BIT(11)
+#define  LPASS_UART_SW_RESET		BIT(10)
+#define  LPASS_PCM_SW_RESET		BIT(9)
+#define  LPASS_I2S_SW_RESET		BIT(8)
+#define  LPASS_WDT1_SW_RESET		BIT(4)
+#define  LPASS_WDT0_SW_RESET		BIT(3)
+#define  LPASS_TIMER_SW_RESET		BIT(2)
+#define  LPASS_MEM_SW_RESET		BIT(1)
+#define  LPASS_DMA_SW_RESET		BIT(0)
+
+#define SFR_LPASS_INTR_CA5_MASK		0x48
+#define SFR_LPASS_INTR_CPU_MASK		0x58
+#define  LPASS_INTR_APM			BIT(9)
+#define  LPASS_INTR_MIF			BIT(8)
+#define  LPASS_INTR_TIMER		BIT(7)
+#define  LPASS_INTR_DMA			BIT(6)
+#define  LPASS_INTR_GPIO		BIT(5)
+#define  LPASS_INTR_I2S			BIT(4)
+#define  LPASS_INTR_PCM			BIT(3)
+#define  LPASS_INTR_SLIMBUS		BIT(2)
+#define  LPASS_INTR_UART		BIT(1)
+#define  LPASS_INTR_SFR			BIT(0)
+
+struct exynos_lpass {
+	/* pointer to the Power Management Unit regmap */
+	struct regmap *pmu;
+	/* pointer to the LPASS TOP regmap */
+	struct regmap *top;
+};
+
+static void exynos_lpass_core_sw_reset(struct exynos_lpass *lpass, int mask)
+{
+	unsigned int val = 0;
+
+	regmap_read(lpass->top, SFR_LPASS_CORE_SW_RESET, &val);
+
+	val &= ~mask;
+	regmap_write(lpass->top, SFR_LPASS_CORE_SW_RESET, val);
+
+	usleep_range(100, 150);
+
+	val |= mask;
+	regmap_write(lpass->top, SFR_LPASS_CORE_SW_RESET, val);
+}
+
+static void exynos_lpass_enable(struct exynos_lpass *lpass)
+{
+	/* Unmask SFR, DMA and I2S interrupt */
+	regmap_write(lpass->top, SFR_LPASS_INTR_CA5_MASK,
+		     LPASS_INTR_SFR | LPASS_INTR_DMA | LPASS_INTR_I2S);
+
+	regmap_write(lpass->top, SFR_LPASS_INTR_CPU_MASK,
+		     LPASS_INTR_SFR | LPASS_INTR_DMA | LPASS_INTR_I2S);
+
+	/* Activate related PADs from retention state */
+	regmap_write(lpass->pmu, EXYNOS5433_PAD_RETENTION_AUD_OPTION,
+		     EXYNOS5433_PAD_INITIATE_WAKEUP_FROM_LOWPWR);
+
+	exynos_lpass_core_sw_reset(lpass, LPASS_I2S_SW_RESET);
+	exynos_lpass_core_sw_reset(lpass, LPASS_DMA_SW_RESET);
+	exynos_lpass_core_sw_reset(lpass, LPASS_MEM_SW_RESET);
+}
+
+static void exynos_lpass_disable(struct exynos_lpass *lpass)
+{
+	/* Mask any unmasked IP interrupt sources */
+	regmap_write(lpass->top, SFR_LPASS_INTR_CPU_MASK, 0);
+	regmap_write(lpass->top, SFR_LPASS_INTR_CA5_MASK, 0);
+
+	/* Deactivate related PADs from retention state */
+	regmap_write(lpass->pmu, EXYNOS5433_PAD_RETENTION_AUD_OPTION, 0);
+}
+
+static const struct regmap_config exynos_lpass_reg_conf = {
+	.reg_bits	= 32,
+	.reg_stride	= 4,
+	.val_bits	= 32,
+	.max_register	= 0xfc,
+	.fast_io	= true,
+};
+
+static int exynos_lpass_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct exynos_lpass *lpass;
+	void __iomem *base_top;
+	struct resource *res;
+
+	lpass = devm_kzalloc(dev, sizeof(*lpass), GFP_KERNEL);
+	if (!lpass)
+		return -ENOMEM;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	base_top = devm_ioremap_resource(dev, res);
+	if (IS_ERR(base_top))
+		return PTR_ERR(base_top);
+
+	lpass->top = regmap_init_mmio(dev, base_top,
+					&exynos_lpass_reg_conf);
+	if (IS_ERR(lpass->top)) {
+		dev_err(dev, "LPASS top regmap initialization failed\n");
+		return PTR_ERR(lpass->top);
+	}
+
+	lpass->pmu = syscon_regmap_lookup_by_phandle(dev->of_node,
+						"samsung,pmu-syscon");
+	if (IS_ERR(lpass->pmu)) {
+		dev_err(dev, "Failed to lookup PMU regmap\n");
+		return PTR_ERR(lpass->pmu);
+	}
+
+	platform_set_drvdata(pdev, lpass);
+	exynos_lpass_enable(lpass);
+
+	return of_platform_populate(dev->of_node, NULL, NULL, dev);
+}
+
+static int __maybe_unused exynos_lpass_suspend(struct device *dev)
+{
+	struct exynos_lpass *lpass = dev_get_drvdata(dev);
+
+	exynos_lpass_disable(lpass);
+
+	return 0;
+}
+
+static int __maybe_unused exynos_lpass_resume(struct device *dev)
+{
+	struct exynos_lpass *lpass = dev_get_drvdata(dev);
+
+	exynos_lpass_enable(lpass);
+
+	return 0;
+}
+
+static SIMPLE_DEV_PM_OPS(lpass_pm_ops, exynos_lpass_suspend,
+					exynos_lpass_resume);
+
+static const struct of_device_id exynos_lpass_of_match[] = {
+	{ .compatible = "samsung,exynos5433-lpass" },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, exynos_lpass_of_match);
+
+static struct platform_driver exynos_lpass_driver = {
+	.driver = {
+		.name		= "exynos-lpass",
+		.pm		= &lpass_pm_ops,
+		.of_match_table	= exynos_lpass_of_match,
+	},
+	.probe	= exynos_lpass_probe,
+};
+module_platform_driver(exynos_lpass_driver);
+
+MODULE_DESCRIPTION("Samsung Low Power Audio Subsystem driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/mfd/intel-lpss-acpi.c b/drivers/mfd/intel-lpss-acpi.c
index 7ddc4a9563ea..6bf8d643d942 100644
--- a/drivers/mfd/intel-lpss-acpi.c
+++ b/drivers/mfd/intel-lpss-acpi.c
@@ -52,6 +52,18 @@ static const struct intel_lpss_platform_info bxt_i2c_info = {
 	.properties = bxt_i2c_properties,
 };
 
+static struct property_entry apl_i2c_properties[] = {
+	PROPERTY_ENTRY_U32("i2c-sda-hold-time-ns", 207),
+	PROPERTY_ENTRY_U32("i2c-sda-falling-time-ns", 171),
+	PROPERTY_ENTRY_U32("i2c-scl-falling-time-ns", 208),
+	{ },
+};
+
+static const struct intel_lpss_platform_info apl_i2c_info = {
+	.clk_rate = 133000000,
+	.properties = apl_i2c_properties,
+};
+
 static const struct acpi_device_id intel_lpss_acpi_ids[] = {
 	/* SPT */
 	{ "INT3446", (kernel_ulong_t)&spt_i2c_info },
@@ -61,7 +73,7 @@ static const struct acpi_device_id intel_lpss_acpi_ids[] = {
 	{ "80860ABC", (kernel_ulong_t)&bxt_info },
 	{ "80860AC2", (kernel_ulong_t)&bxt_info },
 	/* APL */
-	{ "80865AAC", (kernel_ulong_t)&bxt_i2c_info },
+	{ "80865AAC", (kernel_ulong_t)&apl_i2c_info },
 	{ "80865ABC", (kernel_ulong_t)&bxt_info },
 	{ "80865AC2", (kernel_ulong_t)&bxt_info },
 	{ }
diff --git a/drivers/mfd/intel-lpss-pci.c b/drivers/mfd/intel-lpss-pci.c
index 1d79a3c9370f..3228fd182a99 100644
--- a/drivers/mfd/intel-lpss-pci.c
+++ b/drivers/mfd/intel-lpss-pci.c
@@ -111,6 +111,31 @@ static const struct intel_lpss_platform_info bxt_i2c_info = {
 	.properties = bxt_i2c_properties,
 };
 
+static struct property_entry apl_i2c_properties[] = {
+	PROPERTY_ENTRY_U32("i2c-sda-hold-time-ns", 207),
+	PROPERTY_ENTRY_U32("i2c-sda-falling-time-ns", 171),
+	PROPERTY_ENTRY_U32("i2c-scl-falling-time-ns", 208),
+	{ },
+};
+
+static const struct intel_lpss_platform_info apl_i2c_info = {
+	.clk_rate = 133000000,
+	.properties = apl_i2c_properties,
+};
+
+static const struct intel_lpss_platform_info kbl_info = {
+	.clk_rate = 120000000,
+};
+
+static const struct intel_lpss_platform_info kbl_uart_info = {
+	.clk_rate = 120000000,
+	.clk_con_id = "baudclk",
+};
+
+static const struct intel_lpss_platform_info kbl_i2c_info = {
+	.clk_rate = 133000000,
+};
+
 static const struct pci_device_id intel_lpss_pci_ids[] = {
 	/* BXT A-Step */
 	{ PCI_VDEVICE(INTEL, 0x0aac), (kernel_ulong_t)&bxt_i2c_info },
@@ -146,14 +171,14 @@ static const struct pci_device_id intel_lpss_pci_ids[] = {
 	{ PCI_VDEVICE(INTEL, 0x1aee), (kernel_ulong_t)&bxt_uart_info },
 
 	/* APL */
-	{ PCI_VDEVICE(INTEL, 0x5aac), (kernel_ulong_t)&bxt_i2c_info },
-	{ PCI_VDEVICE(INTEL, 0x5aae), (kernel_ulong_t)&bxt_i2c_info },
-	{ PCI_VDEVICE(INTEL, 0x5ab0), (kernel_ulong_t)&bxt_i2c_info },
-	{ PCI_VDEVICE(INTEL, 0x5ab2), (kernel_ulong_t)&bxt_i2c_info },
-	{ PCI_VDEVICE(INTEL, 0x5ab4), (kernel_ulong_t)&bxt_i2c_info },
-	{ PCI_VDEVICE(INTEL, 0x5ab6), (kernel_ulong_t)&bxt_i2c_info },
-	{ PCI_VDEVICE(INTEL, 0x5ab8), (kernel_ulong_t)&bxt_i2c_info },
-	{ PCI_VDEVICE(INTEL, 0x5aba), (kernel_ulong_t)&bxt_i2c_info },
+	{ PCI_VDEVICE(INTEL, 0x5aac), (kernel_ulong_t)&apl_i2c_info },
+	{ PCI_VDEVICE(INTEL, 0x5aae), (kernel_ulong_t)&apl_i2c_info },
+	{ PCI_VDEVICE(INTEL, 0x5ab0), (kernel_ulong_t)&apl_i2c_info },
+	{ PCI_VDEVICE(INTEL, 0x5ab2), (kernel_ulong_t)&apl_i2c_info },
+	{ PCI_VDEVICE(INTEL, 0x5ab4), (kernel_ulong_t)&apl_i2c_info },
+	{ PCI_VDEVICE(INTEL, 0x5ab6), (kernel_ulong_t)&apl_i2c_info },
+	{ PCI_VDEVICE(INTEL, 0x5ab8), (kernel_ulong_t)&apl_i2c_info },
+	{ PCI_VDEVICE(INTEL, 0x5aba), (kernel_ulong_t)&apl_i2c_info },
 	{ PCI_VDEVICE(INTEL, 0x5abc), (kernel_ulong_t)&bxt_uart_info },
 	{ PCI_VDEVICE(INTEL, 0x5abe), (kernel_ulong_t)&bxt_uart_info },
 	{ PCI_VDEVICE(INTEL, 0x5ac0), (kernel_ulong_t)&bxt_uart_info },
@@ -181,6 +206,16 @@ static const struct pci_device_id intel_lpss_pci_ids[] = {
 	{ PCI_VDEVICE(INTEL, 0xa160), (kernel_ulong_t)&spt_i2c_info },
 	{ PCI_VDEVICE(INTEL, 0xa161), (kernel_ulong_t)&spt_i2c_info },
 	{ PCI_VDEVICE(INTEL, 0xa166), (kernel_ulong_t)&spt_uart_info },
+	/* KBL-H */
+	{ PCI_VDEVICE(INTEL, 0xa2a7), (kernel_ulong_t)&kbl_uart_info },
+	{ PCI_VDEVICE(INTEL, 0xa2a8), (kernel_ulong_t)&kbl_uart_info },
+	{ PCI_VDEVICE(INTEL, 0xa2a9), (kernel_ulong_t)&kbl_info },
+	{ PCI_VDEVICE(INTEL, 0xa2aa), (kernel_ulong_t)&kbl_info },
+	{ PCI_VDEVICE(INTEL, 0xa2e0), (kernel_ulong_t)&kbl_i2c_info },
+	{ PCI_VDEVICE(INTEL, 0xa2e1), (kernel_ulong_t)&kbl_i2c_info },
+	{ PCI_VDEVICE(INTEL, 0xa2e2), (kernel_ulong_t)&kbl_i2c_info },
+	{ PCI_VDEVICE(INTEL, 0xa2e3), (kernel_ulong_t)&kbl_i2c_info },
+	{ PCI_VDEVICE(INTEL, 0xa2e6), (kernel_ulong_t)&kbl_uart_info },
 	{ }
 };
 MODULE_DEVICE_TABLE(pci, intel_lpss_pci_ids);
diff --git a/drivers/mfd/intel_msic.c b/drivers/mfd/intel_msic.c
index 25d486c543cb..2017446c5b4b 100644
--- a/drivers/mfd/intel_msic.c
+++ b/drivers/mfd/intel_msic.c
@@ -12,7 +12,7 @@
 #include <linux/err.h>
 #include <linux/gpio.h>
 #include <linux/io.h>
-#include <linux/module.h>
+#include <linux/init.h>
 #include <linux/mfd/core.h>
 #include <linux/mfd/intel_msic.h>
 #include <linux/platform_device.h>
@@ -449,9 +449,4 @@ static struct platform_driver intel_msic_driver = {
 		.name	= "intel_msic",
 	},
 };
-
-module_platform_driver(intel_msic_driver);
-
-MODULE_DESCRIPTION("Driver for Intel MSIC");
-MODULE_AUTHOR("Mika Westerberg <mika.westerberg@linux.intel.com>");
-MODULE_LICENSE("GPL");
+builtin_platform_driver(intel_msic_driver);
diff --git a/drivers/mfd/intel_soc_pmic_bxtwc.c b/drivers/mfd/intel_soc_pmic_bxtwc.c
index b9428767e615..43e54b7e908f 100644
--- a/drivers/mfd/intel_soc_pmic_bxtwc.c
+++ b/drivers/mfd/intel_soc_pmic_bxtwc.c
@@ -47,6 +47,8 @@
 #define BXTWC_MIRQLVL1		0x4E0E
 #define BXTWC_MPWRTNIRQ		0x4E0F
 
+#define BXTWC_MIRQLVL1_MCHGR	BIT(5)
+
 #define BXTWC_MTHRM0IRQ		0x4E12
 #define BXTWC_MTHRM1IRQ		0x4E13
 #define BXTWC_MTHRM2IRQ		0x4E14
@@ -109,7 +111,7 @@ static const struct regmap_irq bxtwc_regmap_irqs_level2[] = {
 	REGMAP_IRQ_REG(BXTWC_THRM2_IRQ, 2, 0xff),
 	REGMAP_IRQ_REG(BXTWC_BCU_IRQ, 3, 0x1f),
 	REGMAP_IRQ_REG(BXTWC_ADC_IRQ, 4, 0xff),
-	REGMAP_IRQ_REG(BXTWC_CHGR0_IRQ, 5, 0x1f),
+	REGMAP_IRQ_REG(BXTWC_CHGR0_IRQ, 5, 0x3f),
 	REGMAP_IRQ_REG(BXTWC_CHGR1_IRQ, 6, 0x1f),
 	REGMAP_IRQ_REG(BXTWC_GPIO0_IRQ, 7, 0xff),
 	REGMAP_IRQ_REG(BXTWC_GPIO1_IRQ, 8, 0x3f),
@@ -143,6 +145,10 @@ static struct resource adc_resources[] = {
 	DEFINE_RES_IRQ_NAMED(BXTWC_ADC_IRQ, "ADC"),
 };
 
+static struct resource usbc_resources[] = {
+	DEFINE_RES_IRQ_NAMED(BXTWC_CHGR0_IRQ, "USBC"),
+};
+
 static struct resource charger_resources[] = {
 	DEFINE_RES_IRQ_NAMED(BXTWC_CHGR0_IRQ, "CHARGER"),
 	DEFINE_RES_IRQ_NAMED(BXTWC_CHGR1_IRQ, "CHARGER1"),
@@ -170,6 +176,11 @@ static struct mfd_cell bxt_wc_dev[] = {
 		.resources = thermal_resources,
 	},
 	{
+		.name = "bxt_wcove_usbc",
+		.num_resources = ARRAY_SIZE(usbc_resources),
+		.resources = usbc_resources,
+	},
+	{
 		.name = "bxt_wcove_ext_charger",
 		.num_resources = ARRAY_SIZE(charger_resources),
 		.resources = charger_resources,
@@ -403,6 +414,16 @@ static int bxtwc_probe(struct platform_device *pdev)
 		goto err_sysfs;
 	}
 
+	/*
+	 * There is known hw bug. Upon reset BIT 5 of register
+	 * BXTWC_CHGR_LVL1_IRQ is 0 which is the expected value. However,
+	 * later it's set to 1(masked) automatically by hardware. So we
+	 * have the software workaround here to unmaksed it in order to let
+	 * charger interrutp work.
+	 */
+	regmap_update_bits(pmic->regmap, BXTWC_MIRQLVL1,
+				BXTWC_MIRQLVL1_MCHGR, 0);
+
 	return 0;
 
 err_sysfs:
diff --git a/drivers/mfd/lp873x.c b/drivers/mfd/lp873x.c
new file mode 100644
index 000000000000..873c608e6a5d
--- /dev/null
+++ b/drivers/mfd/lp873x.c
@@ -0,0 +1,97 @@
+/*
+ * Copyright (C) 2016 Texas Instruments Incorporated - http://www.ti.com/
+ *
+ * Author: Keerthy <j-keerthy@ti.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.
+ *
+ * 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/interrupt.h>
+#include <linux/mfd/core.h>
+#include <linux/module.h>
+#include <linux/of_device.h>
+#include <linux/regmap.h>
+
+#include <linux/mfd/lp873x.h>
+
+static const struct regmap_config lp873x_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
+	.max_register = LP873X_REG_MAX,
+};
+
+static const struct mfd_cell lp873x_cells[] = {
+	{ .name = "lp873x-regulator", },
+	{ .name = "lp873x-gpio", },
+};
+
+static int lp873x_probe(struct i2c_client *client,
+			const struct i2c_device_id *ids)
+{
+	struct lp873x *lp873;
+	int ret;
+	unsigned int otpid;
+
+	lp873 = devm_kzalloc(&client->dev, sizeof(*lp873), GFP_KERNEL);
+	if (!lp873)
+		return -ENOMEM;
+
+	lp873->dev = &client->dev;
+
+	lp873->regmap = devm_regmap_init_i2c(client, &lp873x_regmap_config);
+	if (IS_ERR(lp873->regmap)) {
+		ret = PTR_ERR(lp873->regmap);
+		dev_err(lp873->dev,
+			"Failed to initialize register map: %d\n", ret);
+		return ret;
+	}
+
+	ret = regmap_read(lp873->regmap, LP873X_REG_OTP_REV, &otpid);
+	if (ret) {
+		dev_err(lp873->dev, "Failed to read OTP ID\n");
+		return ret;
+	}
+
+	lp873->rev = otpid & LP873X_OTP_REV_OTP_ID;
+
+	i2c_set_clientdata(client, lp873);
+
+	ret = mfd_add_devices(lp873->dev, PLATFORM_DEVID_AUTO, lp873x_cells,
+			      ARRAY_SIZE(lp873x_cells), NULL, 0, NULL);
+
+	return ret;
+}
+
+static const struct of_device_id of_lp873x_match_table[] = {
+	{ .compatible = "ti,lp8733", },
+	{ .compatible = "ti,lp8732", },
+	{}
+};
+MODULE_DEVICE_TABLE(of, of_lp873x_match_table);
+
+static const struct i2c_device_id lp873x_id_table[] = {
+	{ "lp873x", 0 },
+	{ },
+};
+MODULE_DEVICE_TABLE(i2c, lp873x_id_table);
+
+static struct i2c_driver lp873x_driver = {
+	.driver	= {
+		.name	= "lp873x",
+		.of_match_table = of_lp873x_match_table,
+	},
+	.probe		= lp873x_probe,
+	.id_table	= lp873x_id_table,
+};
+module_i2c_driver(lp873x_driver);
+
+MODULE_AUTHOR("J Keerthy <j-keerthy@ti.com>");
+MODULE_DESCRIPTION("LP873X chip family Multi-Function Device driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/mfd/lpc_ich.c b/drivers/mfd/lpc_ich.c
index bd3aa4578346..c8dee47b45d9 100644
--- a/drivers/mfd/lpc_ich.c
+++ b/drivers/mfd/lpc_ich.c
@@ -984,6 +984,10 @@ static int lpc_ich_init_wdt(struct pci_dev *dev)
 	int ret;
 	struct resource *res;
 
+	/* If we have ACPI based watchdog use that instead */
+	if (acpi_has_watchdog())
+		return -ENODEV;
+
 	/* Setup power management base register */
 	pci_read_config_dword(dev, priv->abase, &base_addr_cfg);
 	base_addr = base_addr_cfg & 0x0000ff80;
diff --git a/drivers/mfd/max14577.c b/drivers/mfd/max14577.c
index 6c245128ab2e..6cbe96b28f42 100644
--- a/drivers/mfd/max14577.c
+++ b/drivers/mfd/max14577.c
@@ -3,7 +3,7 @@
  *
  * Copyright (C) 2014 Samsung Electronics
  * Chanwoo Choi <cw00.choi@samsung.com>
- * Krzysztof Kozlowski <k.kozlowski@samsung.com>
+ * Krzysztof Kozlowski <krzk@kernel.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
@@ -569,6 +569,6 @@ static void __exit max14577_i2c_exit(void)
 }
 module_exit(max14577_i2c_exit);
 
-MODULE_AUTHOR("Chanwoo Choi <cw00.choi@samsung.com>, Krzysztof Kozlowski <k.kozlowski@samsung.com>");
+MODULE_AUTHOR("Chanwoo Choi <cw00.choi@samsung.com>, Krzysztof Kozlowski <krzk@kernel.org>");
 MODULE_DESCRIPTION("Maxim 14577/77836 multi-function core driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/mfd/max8997-irq.c b/drivers/mfd/max8997-irq.c
index b95a46d79b9d..326f17b632a7 100644
--- a/drivers/mfd/max8997-irq.c
+++ b/drivers/mfd/max8997-irq.c
@@ -139,7 +139,7 @@ static void max8997_irq_sync_unlock(struct irq_data *data)
 	mutex_unlock(&max8997->irqlock);
 }
 
-static const inline struct max8997_irq_data *
+inline static const struct max8997_irq_data *
 irq_to_max8997_irq(struct max8997_dev *max8997, struct irq_data *data)
 {
 	return &max8997_irqs[data->hwirq];
diff --git a/drivers/mfd/omap-usb-host.c b/drivers/mfd/omap-usb-host.c
index 1d924d1533c0..7aab376ecb84 100644
--- a/drivers/mfd/omap-usb-host.c
+++ b/drivers/mfd/omap-usb-host.c
@@ -162,7 +162,7 @@ static const char * const port_modes[] = {
  * provided port mode string as per the port_modes table.
  * If no match is found it returns -ENODEV
  */
-static const int omap_usbhs_get_dt_port_mode(const char *mode)
+static int omap_usbhs_get_dt_port_mode(const char *mode)
 {
 	int i;
 
diff --git a/drivers/mfd/pm8921-core.c b/drivers/mfd/pm8921-core.c
index 1b7ec0870c2a..0e3a2ea25942 100644
--- a/drivers/mfd/pm8921-core.c
+++ b/drivers/mfd/pm8921-core.c
@@ -309,6 +309,7 @@ static const struct regmap_config ssbi_regmap_config = {
 };
 
 static const struct of_device_id pm8921_id_table[] = {
+	{ .compatible = "qcom,pm8018", },
 	{ .compatible = "qcom,pm8058", },
 	{ .compatible = "qcom,pm8921", },
 	{ }
diff --git a/drivers/mfd/qcom_rpm.c b/drivers/mfd/qcom_rpm.c
index 2e44323455dd..52fafea06067 100644
--- a/drivers/mfd/qcom_rpm.c
+++ b/drivers/mfd/qcom_rpm.c
@@ -21,6 +21,7 @@
 #include <linux/mfd/qcom_rpm.h>
 #include <linux/mfd/syscon.h>
 #include <linux/regmap.h>
+#include <linux/clk.h>
 
 #include <dt-bindings/mfd/qcom-rpm.h>
 
@@ -48,6 +49,7 @@ struct qcom_rpm {
 	struct regmap *ipc_regmap;
 	unsigned ipc_offset;
 	unsigned ipc_bit;
+	struct clk *ramclk;
 
 	struct completion ack;
 	struct mutex lock;
@@ -388,11 +390,62 @@ static const struct qcom_rpm_data ipq806x_template = {
 	.ack_sel_size = 7,
 };
 
+static const struct qcom_rpm_resource mdm9615_rpm_resource_table[] = {
+	[QCOM_RPM_CXO_CLK] =			{ 25, 9, 5, 1 },
+	[QCOM_RPM_SYS_FABRIC_CLK] =		{ 26, 10, 9, 1 },
+	[QCOM_RPM_DAYTONA_FABRIC_CLK] =		{ 27, 11, 11, 1 },
+	[QCOM_RPM_SFPB_CLK] =			{ 28, 12, 12, 1 },
+	[QCOM_RPM_CFPB_CLK] =			{ 29, 13, 13, 1 },
+	[QCOM_RPM_EBI1_CLK] =			{ 30, 14, 16, 1 },
+	[QCOM_RPM_APPS_FABRIC_HALT] =		{ 31, 15, 22, 2 },
+	[QCOM_RPM_APPS_FABRIC_MODE] =		{ 33, 16, 23, 3 },
+	[QCOM_RPM_APPS_FABRIC_IOCTL] =		{ 36, 17, 24, 1 },
+	[QCOM_RPM_APPS_FABRIC_ARB] =		{ 37, 18, 25, 27 },
+	[QCOM_RPM_PM8018_SMPS1] =		{ 64, 19, 30, 2 },
+	[QCOM_RPM_PM8018_SMPS2] =		{ 66, 21, 31, 2 },
+	[QCOM_RPM_PM8018_SMPS3] =		{ 68, 23, 32, 2 },
+	[QCOM_RPM_PM8018_SMPS4] =		{ 70, 25, 33, 2 },
+	[QCOM_RPM_PM8018_SMPS5] =		{ 72, 27, 34, 2 },
+	[QCOM_RPM_PM8018_LDO1] =		{ 74, 29, 35, 2 },
+	[QCOM_RPM_PM8018_LDO2] =		{ 76, 31, 36, 2 },
+	[QCOM_RPM_PM8018_LDO3] =		{ 78, 33, 37, 2 },
+	[QCOM_RPM_PM8018_LDO4] =		{ 80, 35, 38, 2 },
+	[QCOM_RPM_PM8018_LDO5] =		{ 82, 37, 39, 2 },
+	[QCOM_RPM_PM8018_LDO6] =		{ 84, 39, 40, 2 },
+	[QCOM_RPM_PM8018_LDO7] =		{ 86, 41, 41, 2 },
+	[QCOM_RPM_PM8018_LDO8] =		{ 88, 43, 42, 2 },
+	[QCOM_RPM_PM8018_LDO9] =		{ 90, 45, 43, 2 },
+	[QCOM_RPM_PM8018_LDO10] =		{ 92, 47, 44, 2 },
+	[QCOM_RPM_PM8018_LDO11] =		{ 94, 49, 45, 2 },
+	[QCOM_RPM_PM8018_LDO12] =		{ 96, 51, 46, 2 },
+	[QCOM_RPM_PM8018_LDO13] =		{ 98, 53, 47, 2 },
+	[QCOM_RPM_PM8018_LDO14] =		{ 100, 55, 48, 2 },
+	[QCOM_RPM_PM8018_LVS1] =		{ 102, 57, 49, 1 },
+	[QCOM_RPM_PM8018_NCP] =			{ 103, 58, 80, 2 },
+	[QCOM_RPM_CXO_BUFFERS] =		{ 105, 60, 81, 1 },
+	[QCOM_RPM_USB_OTG_SWITCH] =		{ 106, 61, 82, 1 },
+	[QCOM_RPM_HDMI_SWITCH] =		{ 107, 62, 83, 1 },
+	[QCOM_RPM_VOLTAGE_CORNER] =		{ 109, 64, 87, 1 },
+};
+
+static const struct qcom_rpm_data mdm9615_template = {
+	.version = 3,
+	.resource_table = mdm9615_rpm_resource_table,
+	.n_resources = ARRAY_SIZE(mdm9615_rpm_resource_table),
+	.req_ctx_off = 3,
+	.req_sel_off = 11,
+	.ack_ctx_off = 15,
+	.ack_sel_off = 23,
+	.req_sel_size = 4,
+	.ack_sel_size = 7,
+};
+
 static const struct of_device_id qcom_rpm_of_match[] = {
 	{ .compatible = "qcom,rpm-apq8064", .data = &apq8064_template },
 	{ .compatible = "qcom,rpm-msm8660", .data = &msm8660_template },
 	{ .compatible = "qcom,rpm-msm8960", .data = &msm8960_template },
 	{ .compatible = "qcom,rpm-ipq8064", .data = &ipq806x_template },
+	{ .compatible = "qcom,rpm-mdm9615", .data = &mdm9615_template },
 	{ }
 };
 MODULE_DEVICE_TABLE(of, qcom_rpm_of_match);
@@ -501,6 +554,20 @@ static int qcom_rpm_probe(struct platform_device *pdev)
 	mutex_init(&rpm->lock);
 	init_completion(&rpm->ack);
 
+	/* Enable message RAM clock */
+	rpm->ramclk = devm_clk_get(&pdev->dev, "ram");
+	if (IS_ERR(rpm->ramclk)) {
+		ret = PTR_ERR(rpm->ramclk);
+		if (ret == -EPROBE_DEFER)
+			return ret;
+		/*
+		 * Fall through in all other cases, as the clock is
+		 * optional. (Does not exist on all platforms.)
+		 */
+		rpm->ramclk = NULL;
+	}
+	clk_prepare_enable(rpm->ramclk); /* Accepts NULL */
+
 	irq_ack = platform_get_irq_byname(pdev, "ack");
 	if (irq_ack < 0) {
 		dev_err(&pdev->dev, "required ack interrupt missing\n");
@@ -538,6 +605,7 @@ static int qcom_rpm_probe(struct platform_device *pdev)
 	}
 
 	rpm->ipc_regmap = syscon_node_to_regmap(syscon_np);
+	of_node_put(syscon_np);
 	if (IS_ERR(rpm->ipc_regmap))
 		return PTR_ERR(rpm->ipc_regmap);
 
@@ -620,7 +688,11 @@ static int qcom_rpm_probe(struct platform_device *pdev)
 
 static int qcom_rpm_remove(struct platform_device *pdev)
 {
+	struct qcom_rpm *rpm = dev_get_drvdata(&pdev->dev);
+
 	of_platform_depopulate(&pdev->dev);
+	clk_disable_unprepare(rpm->ramclk);
+
 	return 0;
 }
 
diff --git a/drivers/mfd/rk808.c b/drivers/mfd/rk808.c
index 49d7f624fc94..0f8acc5882a4 100644
--- a/drivers/mfd/rk808.c
+++ b/drivers/mfd/rk808.c
@@ -1,11 +1,15 @@
 /*
- * MFD core driver for Rockchip RK808
+ * MFD core driver for Rockchip RK808/RK818
  *
  * Copyright (c) 2014, Fuzhou Rockchip Electronics Co., Ltd
  *
  * Author: Chris Zhong <zyw@rock-chips.com>
  * Author: Zhang Qing <zhangqing@rock-chips.com>
  *
+ * Copyright (C) 2016 PHYTEC Messtechnik GmbH
+ *
+ * Author: Wadim Egorov <w.egorov@phytec.de>
+ *
  * 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.
@@ -21,6 +25,7 @@
 #include <linux/mfd/rk808.h>
 #include <linux/mfd/core.h>
 #include <linux/module.h>
+#include <linux/of_device.h>
 #include <linux/regmap.h>
 
 struct rk808_reg_data {
@@ -57,6 +62,14 @@ static bool rk808_is_volatile_reg(struct device *dev, unsigned int reg)
 	return false;
 }
 
+static const struct regmap_config rk818_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
+	.max_register = RK818_USB_CTRL_REG,
+	.cache_type = REGCACHE_RBTREE,
+	.volatile_reg = rk808_is_volatile_reg,
+};
+
 static const struct regmap_config rk808_regmap_config = {
 	.reg_bits = 8,
 	.val_bits = 8,
@@ -79,11 +92,21 @@ static const struct mfd_cell rk808s[] = {
 	{
 		.name = "rk808-rtc",
 		.num_resources = ARRAY_SIZE(rtc_resources),
-		.resources = &rtc_resources[0],
+		.resources = rtc_resources,
 	},
 };
 
-static const struct rk808_reg_data pre_init_reg[] = {
+static const struct mfd_cell rk818s[] = {
+	{ .name = "rk808-clkout", },
+	{ .name = "rk808-regulator", },
+	{
+		.name = "rk808-rtc",
+		.num_resources = ARRAY_SIZE(rtc_resources),
+		.resources = rtc_resources,
+	},
+};
+
+static const struct rk808_reg_data rk808_pre_init_reg[] = {
 	{ RK808_BUCK3_CONFIG_REG, BUCK_ILMIN_MASK,  BUCK_ILMIN_150MA },
 	{ RK808_BUCK4_CONFIG_REG, BUCK_ILMIN_MASK,  BUCK_ILMIN_200MA },
 	{ RK808_BOOST_CONFIG_REG, BOOST_ILMIN_MASK, BOOST_ILMIN_100MA },
@@ -94,6 +117,24 @@ static const struct rk808_reg_data pre_init_reg[] = {
 						    VB_LO_SEL_3500MV },
 };
 
+static const struct rk808_reg_data rk818_pre_init_reg[] = {
+	/* improve efficiency */
+	{ RK818_BUCK2_CONFIG_REG, BUCK2_RATE_MASK,  BUCK_ILMIN_250MA },
+	{ RK818_BUCK4_CONFIG_REG, BUCK_ILMIN_MASK,  BUCK_ILMIN_250MA },
+	{ RK818_BOOST_CONFIG_REG, BOOST_ILMIN_MASK, BOOST_ILMIN_100MA },
+	{ RK818_USB_CTRL_REG,	  RK818_USB_ILIM_SEL_MASK,
+						    RK818_USB_ILMIN_2000MA },
+	/* close charger when usb lower then 3.4V */
+	{ RK818_USB_CTRL_REG,	  RK818_USB_CHG_SD_VSEL_MASK,
+						    (0x7 << 4) },
+	/* no action when vref */
+	{ RK818_H5V_EN_REG,	  BIT(1),	    RK818_REF_RDY_CTRL },
+	/* enable HDMI 5V */
+	{ RK818_H5V_EN_REG,	  BIT(0),	    RK818_H5V_EN },
+	{ RK808_VB_MON_REG,	  MASK_ALL,	    VB_LO_ACT |
+						    VB_LO_SEL_3500MV },
+};
+
 static const struct regmap_irq rk808_irqs[] = {
 	/* INT_STS */
 	[RK808_IRQ_VOUT_LO] = {
@@ -136,6 +177,76 @@ static const struct regmap_irq rk808_irqs[] = {
 	},
 };
 
+static const struct regmap_irq rk818_irqs[] = {
+	/* INT_STS */
+	[RK818_IRQ_VOUT_LO] = {
+		.mask = RK818_IRQ_VOUT_LO_MSK,
+		.reg_offset = 0,
+	},
+	[RK818_IRQ_VB_LO] = {
+		.mask = RK818_IRQ_VB_LO_MSK,
+		.reg_offset = 0,
+	},
+	[RK818_IRQ_PWRON] = {
+		.mask = RK818_IRQ_PWRON_MSK,
+		.reg_offset = 0,
+	},
+	[RK818_IRQ_PWRON_LP] = {
+		.mask = RK818_IRQ_PWRON_LP_MSK,
+		.reg_offset = 0,
+	},
+	[RK818_IRQ_HOTDIE] = {
+		.mask = RK818_IRQ_HOTDIE_MSK,
+		.reg_offset = 0,
+	},
+	[RK818_IRQ_RTC_ALARM] = {
+		.mask = RK818_IRQ_RTC_ALARM_MSK,
+		.reg_offset = 0,
+	},
+	[RK818_IRQ_RTC_PERIOD] = {
+		.mask = RK818_IRQ_RTC_PERIOD_MSK,
+		.reg_offset = 0,
+	},
+	[RK818_IRQ_USB_OV] = {
+		.mask = RK818_IRQ_USB_OV_MSK,
+		.reg_offset = 0,
+	},
+
+	/* INT_STS2 */
+	[RK818_IRQ_PLUG_IN] = {
+		.mask = RK818_IRQ_PLUG_IN_MSK,
+		.reg_offset = 1,
+	},
+	[RK818_IRQ_PLUG_OUT] = {
+		.mask = RK818_IRQ_PLUG_OUT_MSK,
+		.reg_offset = 1,
+	},
+	[RK818_IRQ_CHG_OK] = {
+		.mask = RK818_IRQ_CHG_OK_MSK,
+		.reg_offset = 1,
+	},
+	[RK818_IRQ_CHG_TE] = {
+		.mask = RK818_IRQ_CHG_TE_MSK,
+		.reg_offset = 1,
+	},
+	[RK818_IRQ_CHG_TS1] = {
+		.mask = RK818_IRQ_CHG_TS1_MSK,
+		.reg_offset = 1,
+	},
+	[RK818_IRQ_TS2] = {
+		.mask = RK818_IRQ_TS2_MSK,
+		.reg_offset = 1,
+	},
+	[RK818_IRQ_CHG_CVTLIM] = {
+		.mask = RK818_IRQ_CHG_CVTLIM_MSK,
+		.reg_offset = 1,
+	},
+	[RK818_IRQ_DISCHG_ILIM] = {
+		.mask = RK818_IRQ_DISCHG_ILIM_MSK,
+		.reg_offset = 1,
+	},
+};
+
 static struct regmap_irq_chip rk808_irq_chip = {
 	.name = "rk808",
 	.irqs = rk808_irqs,
@@ -148,6 +259,18 @@ static struct regmap_irq_chip rk808_irq_chip = {
 	.init_ack_masked = true,
 };
 
+static struct regmap_irq_chip rk818_irq_chip = {
+	.name = "rk818",
+	.irqs = rk818_irqs,
+	.num_irqs = ARRAY_SIZE(rk818_irqs),
+	.num_regs = 2,
+	.irq_reg_stride = 2,
+	.status_base = RK818_INT_STS_REG1,
+	.mask_base = RK818_INT_STS_MSK_REG1,
+	.ack_base = RK818_INT_STS_REG1,
+	.init_ack_masked = true,
+};
+
 static struct i2c_client *rk808_i2c_client;
 static void rk808_device_shutdown(void)
 {
@@ -167,55 +290,100 @@ static void rk808_device_shutdown(void)
 		dev_err(&rk808_i2c_client->dev, "power off error!\n");
 }
 
+static const struct of_device_id rk808_of_match[] = {
+	{ .compatible = "rockchip,rk808" },
+	{ .compatible = "rockchip,rk818" },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, rk808_of_match);
+
 static int rk808_probe(struct i2c_client *client,
 		       const struct i2c_device_id *id)
 {
 	struct device_node *np = client->dev.of_node;
 	struct rk808 *rk808;
+	const struct rk808_reg_data *pre_init_reg;
+	const struct mfd_cell *cells;
+	int nr_pre_init_regs;
+	int nr_cells;
 	int pm_off = 0;
 	int ret;
 	int i;
 
-	if (!client->irq) {
-		dev_err(&client->dev, "No interrupt support, no core IRQ\n");
-		return -EINVAL;
-	}
-
 	rk808 = devm_kzalloc(&client->dev, sizeof(*rk808), GFP_KERNEL);
 	if (!rk808)
 		return -ENOMEM;
 
-	rk808->regmap = devm_regmap_init_i2c(client, &rk808_regmap_config);
+	rk808->variant = i2c_smbus_read_word_data(client, RK808_ID_MSB);
+	if (rk808->variant < 0) {
+		dev_err(&client->dev, "Failed to read the chip id at 0x%02x\n",
+			RK808_ID_MSB);
+		return rk808->variant;
+	}
+
+	dev_dbg(&client->dev, "Chip id: 0x%x\n", (unsigned int)rk808->variant);
+
+	switch (rk808->variant) {
+	case RK808_ID:
+		rk808->regmap_cfg = &rk808_regmap_config;
+		rk808->regmap_irq_chip = &rk808_irq_chip;
+		pre_init_reg = rk808_pre_init_reg;
+		nr_pre_init_regs = ARRAY_SIZE(rk808_pre_init_reg);
+		cells = rk808s;
+		nr_cells = ARRAY_SIZE(rk808s);
+		break;
+	case RK818_ID:
+		rk808->regmap_cfg = &rk818_regmap_config;
+		rk808->regmap_irq_chip = &rk818_irq_chip;
+		pre_init_reg = rk818_pre_init_reg;
+		nr_pre_init_regs = ARRAY_SIZE(rk818_pre_init_reg);
+		cells = rk818s;
+		nr_cells = ARRAY_SIZE(rk818s);
+		break;
+	default:
+		dev_err(&client->dev, "Unsupported RK8XX ID %lu\n",
+			rk808->variant);
+		return -EINVAL;
+	}
+
+	rk808->i2c = client;
+	i2c_set_clientdata(client, rk808);
+
+	rk808->regmap = devm_regmap_init_i2c(client, rk808->regmap_cfg);
 	if (IS_ERR(rk808->regmap)) {
 		dev_err(&client->dev, "regmap initialization failed\n");
 		return PTR_ERR(rk808->regmap);
 	}
 
-	for (i = 0; i < ARRAY_SIZE(pre_init_reg); i++) {
-		ret = regmap_update_bits(rk808->regmap, pre_init_reg[i].addr,
-					 pre_init_reg[i].mask,
-					 pre_init_reg[i].value);
-		if (ret) {
-			dev_err(&client->dev,
-				"0x%x write err\n", pre_init_reg[i].addr);
-			return ret;
-		}
+	if (!client->irq) {
+		dev_err(&client->dev, "No interrupt support, no core IRQ\n");
+		return -EINVAL;
 	}
 
 	ret = regmap_add_irq_chip(rk808->regmap, client->irq,
 				  IRQF_ONESHOT, -1,
-				  &rk808_irq_chip, &rk808->irq_data);
+				  rk808->regmap_irq_chip, &rk808->irq_data);
 	if (ret) {
 		dev_err(&client->dev, "Failed to add irq_chip %d\n", ret);
 		return ret;
 	}
 
-	rk808->i2c = client;
-	i2c_set_clientdata(client, rk808);
+	for (i = 0; i < nr_pre_init_regs; i++) {
+		ret = regmap_update_bits(rk808->regmap,
+					pre_init_reg[i].addr,
+					pre_init_reg[i].mask,
+					pre_init_reg[i].value);
+		if (ret) {
+			dev_err(&client->dev,
+				"0x%x write err\n",
+				pre_init_reg[i].addr);
+			return ret;
+		}
+	}
 
-	ret = devm_mfd_add_devices(&client->dev, -1,
-				   rk808s, ARRAY_SIZE(rk808s), NULL, 0,
-				   regmap_irq_get_domain(rk808->irq_data));
+	ret = devm_mfd_add_devices(&client->dev, PLATFORM_DEVID_NONE,
+			      cells, nr_cells, NULL, 0,
+			      regmap_irq_get_domain(rk808->irq_data));
 	if (ret) {
 		dev_err(&client->dev, "failed to add MFD devices %d\n", ret);
 		goto err_irq;
@@ -245,14 +413,9 @@ static int rk808_remove(struct i2c_client *client)
 	return 0;
 }
 
-static const struct of_device_id rk808_of_match[] = {
-	{ .compatible = "rockchip,rk808" },
-	{ },
-};
-MODULE_DEVICE_TABLE(of, rk808_of_match);
-
 static const struct i2c_device_id rk808_ids[] = {
 	{ "rk808" },
+	{ "rk818" },
 	{ },
 };
 MODULE_DEVICE_TABLE(i2c, rk808_ids);
@@ -272,4 +435,5 @@ module_i2c_driver(rk808_i2c_driver);
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Chris Zhong <zyw@rock-chips.com>");
 MODULE_AUTHOR("Zhang Qing <zhangqing@rock-chips.com>");
-MODULE_DESCRIPTION("RK808 PMIC driver");
+MODULE_AUTHOR("Wadim Egorov <w.egorov@phytec.de>");
+MODULE_DESCRIPTION("RK808/RK818 PMIC driver");
diff --git a/drivers/mfd/rtsx_usb.c b/drivers/mfd/rtsx_usb.c
index dbd907d7170e..691dab791f7a 100644
--- a/drivers/mfd/rtsx_usb.c
+++ b/drivers/mfd/rtsx_usb.c
@@ -46,9 +46,6 @@ static void rtsx_usb_sg_timed_out(unsigned long data)
 
 	dev_dbg(&ucr->pusb_intf->dev, "%s: sg transfer timed out", __func__);
 	usb_sg_cancel(&ucr->current_sg);
-
-	/* we know the cancellation is caused by time-out */
-	ucr->current_sg.status = -ETIMEDOUT;
 }
 
 static int rtsx_usb_bulk_transfer_sglist(struct rtsx_ucr *ucr,
@@ -67,12 +64,15 @@ static int rtsx_usb_bulk_transfer_sglist(struct rtsx_ucr *ucr,
 	ucr->sg_timer.expires = jiffies + msecs_to_jiffies(timeout);
 	add_timer(&ucr->sg_timer);
 	usb_sg_wait(&ucr->current_sg);
-	del_timer_sync(&ucr->sg_timer);
+	if (!del_timer_sync(&ucr->sg_timer))
+		ret = -ETIMEDOUT;
+	else
+		ret = ucr->current_sg.status;
 
 	if (act_len)
 		*act_len = ucr->current_sg.bytes;
 
-	return ucr->current_sg.status;
+	return ret;
 }
 
 int rtsx_usb_transfer_data(struct rtsx_ucr *ucr, unsigned int pipe,
diff --git a/drivers/mfd/sm501.c b/drivers/mfd/sm501.c
index 65cd0d2a822a..40534352e574 100644
--- a/drivers/mfd/sm501.c
+++ b/drivers/mfd/sm501.c
@@ -1001,7 +1001,7 @@ static int sm501_gpio_output(struct gpio_chip *chip,
 	return 0;
 }
 
-static struct gpio_chip gpio_chip_template = {
+static const struct gpio_chip gpio_chip_template = {
 	.ngpio			= 32,
 	.direction_input	= sm501_gpio_input,
 	.direction_output	= sm501_gpio_output,
diff --git a/drivers/mfd/smsc-ece1099.c b/drivers/mfd/smsc-ece1099.c
index cd18c09827ef..1f40baf1234e 100644
--- a/drivers/mfd/smsc-ece1099.c
+++ b/drivers/mfd/smsc-ece1099.c
@@ -11,8 +11,7 @@
  *
  */
 
-#include <linux/module.h>
-#include <linux/moduleparam.h>
+#include <linux/init.h>
 #include <linux/slab.h>
 #include <linux/i2c.h>
 #include <linux/gpio.h>
@@ -81,7 +80,6 @@ static const struct i2c_device_id smsc_i2c_id[] = {
 	{ "smscece1099", 0},
 	{},
 };
-MODULE_DEVICE_TABLE(i2c, smsc_i2c_id);
 
 static struct i2c_driver smsc_i2c_driver = {
 	.driver = {
@@ -90,9 +88,4 @@ static struct i2c_driver smsc_i2c_driver = {
 	.probe = smsc_i2c_probe,
 	.id_table = smsc_i2c_id,
 };
-
-module_i2c_driver(smsc_i2c_driver);
-
-MODULE_AUTHOR("Sourav Poddar <sourav.poddar@ti.com>");
-MODULE_DESCRIPTION("SMSC chip multi-function driver");
-MODULE_LICENSE("GPL v2");
+builtin_i2c_driver(smsc_i2c_driver);
diff --git a/drivers/mfd/stmpe-i2c.c b/drivers/mfd/stmpe-i2c.c
index c3f4aab53b07..863c39a3353c 100644
--- a/drivers/mfd/stmpe-i2c.c
+++ b/drivers/mfd/stmpe-i2c.c
@@ -57,6 +57,7 @@ static const struct of_device_id stmpe_of_match[] = {
 	{ .compatible = "st,stmpe610", .data = (void *)STMPE610, },
 	{ .compatible = "st,stmpe801", .data = (void *)STMPE801, },
 	{ .compatible = "st,stmpe811", .data = (void *)STMPE811, },
+	{ .compatible = "st,stmpe1600", .data = (void *)STMPE1600, },
 	{ .compatible = "st,stmpe1601", .data = (void *)STMPE1601, },
 	{ .compatible = "st,stmpe1801", .data = (void *)STMPE1801, },
 	{ .compatible = "st,stmpe2401", .data = (void *)STMPE2401, },
@@ -101,6 +102,7 @@ static const struct i2c_device_id stmpe_i2c_id[] = {
 	{ "stmpe610", STMPE610 },
 	{ "stmpe801", STMPE801 },
 	{ "stmpe811", STMPE811 },
+	{ "stmpe1600", STMPE1600 },
 	{ "stmpe1601", STMPE1601 },
 	{ "stmpe1801", STMPE1801 },
 	{ "stmpe2401", STMPE2401 },
diff --git a/drivers/mfd/stmpe.c b/drivers/mfd/stmpe.c
index 94c7cc02fdab..cfdae8a3d779 100644
--- a/drivers/mfd/stmpe.c
+++ b/drivers/mfd/stmpe.c
@@ -469,6 +469,8 @@ static const struct mfd_cell stmpe_ts_cell = {
 
 static const u8 stmpe811_regs[] = {
 	[STMPE_IDX_CHIP_ID]	= STMPE811_REG_CHIP_ID,
+	[STMPE_IDX_SYS_CTRL]	= STMPE811_REG_SYS_CTRL,
+	[STMPE_IDX_SYS_CTRL2]	= STMPE811_REG_SYS_CTRL2,
 	[STMPE_IDX_ICR_LSB]	= STMPE811_REG_INT_CTRL,
 	[STMPE_IDX_IER_LSB]	= STMPE811_REG_INT_EN,
 	[STMPE_IDX_ISR_MSB]	= STMPE811_REG_INT_STA,
@@ -481,7 +483,7 @@ static const u8 stmpe811_regs[] = {
 	[STMPE_IDX_GPAFR_U_MSB]	= STMPE811_REG_GPIO_AF,
 	[STMPE_IDX_IEGPIOR_LSB]	= STMPE811_REG_GPIO_INT_EN,
 	[STMPE_IDX_ISGPIOR_MSB]	= STMPE811_REG_GPIO_INT_STA,
-	[STMPE_IDX_GPEDR_MSB]	= STMPE811_REG_GPIO_ED,
+	[STMPE_IDX_GPEDR_LSB]	= STMPE811_REG_GPIO_ED,
 };
 
 static struct stmpe_variant_block stmpe811_blocks[] = {
@@ -511,7 +513,7 @@ static int stmpe811_enable(struct stmpe *stmpe, unsigned int blocks,
 	if (blocks & STMPE_BLOCK_TOUCHSCREEN)
 		mask |= STMPE811_SYS_CTRL2_TSC_OFF;
 
-	return __stmpe_set_bits(stmpe, STMPE811_REG_SYS_CTRL2, mask,
+	return __stmpe_set_bits(stmpe, stmpe->regs[STMPE_IDX_SYS_CTRL2], mask,
 				enable ? 0 : mask);
 }
 
@@ -551,25 +553,89 @@ static struct stmpe_variant_info stmpe610 = {
 };
 
 /*
+ * STMPE1600
+ * Compared to all others STMPE variant, LSB and MSB regs are located in this
+ * order :	LSB   addr
+ *		MSB   addr + 1
+ * As there is only 2 * 8bits registers for GPMR/GPSR/IEGPIOPR, CSB index is MSB registers
+ */
+
+static const u8 stmpe1600_regs[] = {
+	[STMPE_IDX_CHIP_ID]	= STMPE1600_REG_CHIP_ID,
+	[STMPE_IDX_SYS_CTRL]	= STMPE1600_REG_SYS_CTRL,
+	[STMPE_IDX_ICR_LSB]	= STMPE1600_REG_SYS_CTRL,
+	[STMPE_IDX_GPMR_LSB]	= STMPE1600_REG_GPMR_LSB,
+	[STMPE_IDX_GPMR_CSB]	= STMPE1600_REG_GPMR_MSB,
+	[STMPE_IDX_GPSR_LSB]	= STMPE1600_REG_GPSR_LSB,
+	[STMPE_IDX_GPSR_CSB]	= STMPE1600_REG_GPSR_MSB,
+	[STMPE_IDX_GPDR_LSB]	= STMPE1600_REG_GPDR_LSB,
+	[STMPE_IDX_GPDR_CSB]	= STMPE1600_REG_GPDR_MSB,
+	[STMPE_IDX_IEGPIOR_LSB]	= STMPE1600_REG_IEGPIOR_LSB,
+	[STMPE_IDX_IEGPIOR_CSB]	= STMPE1600_REG_IEGPIOR_MSB,
+	[STMPE_IDX_ISGPIOR_LSB]	= STMPE1600_REG_ISGPIOR_LSB,
+};
+
+static struct stmpe_variant_block stmpe1600_blocks[] = {
+	{
+		.cell	= &stmpe_gpio_cell,
+		.irq	= 0,
+		.block	= STMPE_BLOCK_GPIO,
+	},
+};
+
+static int stmpe1600_enable(struct stmpe *stmpe, unsigned int blocks,
+			   bool enable)
+{
+	if (blocks & STMPE_BLOCK_GPIO)
+		return 0;
+	else
+		return -EINVAL;
+}
+
+static struct stmpe_variant_info stmpe1600 = {
+	.name		= "stmpe1600",
+	.id_val		= STMPE1600_ID,
+	.id_mask	= 0xffff,
+	.num_gpios	= 16,
+	.af_bits	= 0,
+	.regs		= stmpe1600_regs,
+	.blocks		= stmpe1600_blocks,
+	.num_blocks	= ARRAY_SIZE(stmpe1600_blocks),
+	.num_irqs	= STMPE1600_NR_INTERNAL_IRQS,
+	.enable		= stmpe1600_enable,
+};
+
+/*
  * STMPE1601
  */
 
 static const u8 stmpe1601_regs[] = {
 	[STMPE_IDX_CHIP_ID]	= STMPE1601_REG_CHIP_ID,
+	[STMPE_IDX_SYS_CTRL]	= STMPE1601_REG_SYS_CTRL,
+	[STMPE_IDX_SYS_CTRL2]	= STMPE1601_REG_SYS_CTRL2,
 	[STMPE_IDX_ICR_LSB]	= STMPE1601_REG_ICR_LSB,
+	[STMPE_IDX_IER_MSB]	= STMPE1601_REG_IER_MSB,
 	[STMPE_IDX_IER_LSB]	= STMPE1601_REG_IER_LSB,
 	[STMPE_IDX_ISR_MSB]	= STMPE1601_REG_ISR_MSB,
 	[STMPE_IDX_GPMR_LSB]	= STMPE1601_REG_GPIO_MP_LSB,
+	[STMPE_IDX_GPMR_CSB]	= STMPE1601_REG_GPIO_MP_MSB,
 	[STMPE_IDX_GPSR_LSB]	= STMPE1601_REG_GPIO_SET_LSB,
+	[STMPE_IDX_GPSR_CSB]	= STMPE1601_REG_GPIO_SET_MSB,
 	[STMPE_IDX_GPCR_LSB]	= STMPE1601_REG_GPIO_CLR_LSB,
+	[STMPE_IDX_GPCR_CSB]	= STMPE1601_REG_GPIO_CLR_MSB,
 	[STMPE_IDX_GPDR_LSB]	= STMPE1601_REG_GPIO_SET_DIR_LSB,
+	[STMPE_IDX_GPDR_CSB]	= STMPE1601_REG_GPIO_SET_DIR_MSB,
+	[STMPE_IDX_GPEDR_LSB]	= STMPE1601_REG_GPIO_ED_LSB,
+	[STMPE_IDX_GPEDR_CSB]	= STMPE1601_REG_GPIO_ED_MSB,
 	[STMPE_IDX_GPRER_LSB]	= STMPE1601_REG_GPIO_RE_LSB,
+	[STMPE_IDX_GPRER_CSB]	= STMPE1601_REG_GPIO_RE_MSB,
 	[STMPE_IDX_GPFER_LSB]	= STMPE1601_REG_GPIO_FE_LSB,
+	[STMPE_IDX_GPFER_CSB]	= STMPE1601_REG_GPIO_FE_MSB,
 	[STMPE_IDX_GPPUR_LSB]	= STMPE1601_REG_GPIO_PU_LSB,
 	[STMPE_IDX_GPAFR_U_MSB]	= STMPE1601_REG_GPIO_AF_U_MSB,
 	[STMPE_IDX_IEGPIOR_LSB]	= STMPE1601_REG_INT_EN_GPIO_MASK_LSB,
+	[STMPE_IDX_IEGPIOR_CSB]	= STMPE1601_REG_INT_EN_GPIO_MASK_MSB,
 	[STMPE_IDX_ISGPIOR_MSB]	= STMPE1601_REG_INT_STA_GPIO_MSB,
-	[STMPE_IDX_GPEDR_MSB]	= STMPE1601_REG_GPIO_ED_MSB,
 };
 
 static struct stmpe_variant_block stmpe1601_blocks[] = {
@@ -640,13 +706,13 @@ static int stmpe1601_autosleep(struct stmpe *stmpe,
 		return timeout;
 	}
 
-	ret = __stmpe_set_bits(stmpe, STMPE1601_REG_SYS_CTRL2,
+	ret = __stmpe_set_bits(stmpe, stmpe->regs[STMPE_IDX_SYS_CTRL2],
 			STMPE1601_AUTOSLEEP_TIMEOUT_MASK,
 			timeout);
 	if (ret < 0)
 		return ret;
 
-	return __stmpe_set_bits(stmpe, STMPE1601_REG_SYS_CTRL2,
+	return __stmpe_set_bits(stmpe, stmpe->regs[STMPE_IDX_SYS_CTRL2],
 			STPME1601_AUTOSLEEP_ENABLE,
 			STPME1601_AUTOSLEEP_ENABLE);
 }
@@ -671,7 +737,7 @@ static int stmpe1601_enable(struct stmpe *stmpe, unsigned int blocks,
 	else
 		mask &= ~STMPE1601_SYS_CTRL_ENABLE_SPWM;
 
-	return __stmpe_set_bits(stmpe, STMPE1601_REG_SYS_CTRL, mask,
+	return __stmpe_set_bits(stmpe, stmpe->regs[STMPE_IDX_SYS_CTRL], mask,
 				enable ? mask : 0);
 }
 
@@ -710,18 +776,33 @@ static struct stmpe_variant_info stmpe1601 = {
  */
 static const u8 stmpe1801_regs[] = {
 	[STMPE_IDX_CHIP_ID]	= STMPE1801_REG_CHIP_ID,
+	[STMPE_IDX_SYS_CTRL]	= STMPE1801_REG_SYS_CTRL,
 	[STMPE_IDX_ICR_LSB]	= STMPE1801_REG_INT_CTRL_LOW,
 	[STMPE_IDX_IER_LSB]	= STMPE1801_REG_INT_EN_MASK_LOW,
 	[STMPE_IDX_ISR_LSB]	= STMPE1801_REG_INT_STA_LOW,
 	[STMPE_IDX_GPMR_LSB]	= STMPE1801_REG_GPIO_MP_LOW,
+	[STMPE_IDX_GPMR_CSB]	= STMPE1801_REG_GPIO_MP_MID,
+	[STMPE_IDX_GPMR_MSB]	= STMPE1801_REG_GPIO_MP_HIGH,
 	[STMPE_IDX_GPSR_LSB]	= STMPE1801_REG_GPIO_SET_LOW,
+	[STMPE_IDX_GPSR_CSB]	= STMPE1801_REG_GPIO_SET_MID,
+	[STMPE_IDX_GPSR_MSB]	= STMPE1801_REG_GPIO_SET_HIGH,
 	[STMPE_IDX_GPCR_LSB]	= STMPE1801_REG_GPIO_CLR_LOW,
+	[STMPE_IDX_GPCR_CSB]	= STMPE1801_REG_GPIO_CLR_MID,
+	[STMPE_IDX_GPCR_MSB]	= STMPE1801_REG_GPIO_CLR_HIGH,
 	[STMPE_IDX_GPDR_LSB]	= STMPE1801_REG_GPIO_SET_DIR_LOW,
+	[STMPE_IDX_GPDR_CSB]	= STMPE1801_REG_GPIO_SET_DIR_MID,
+	[STMPE_IDX_GPDR_MSB]	= STMPE1801_REG_GPIO_SET_DIR_HIGH,
 	[STMPE_IDX_GPRER_LSB]	= STMPE1801_REG_GPIO_RE_LOW,
+	[STMPE_IDX_GPRER_CSB]	= STMPE1801_REG_GPIO_RE_MID,
+	[STMPE_IDX_GPRER_MSB]	= STMPE1801_REG_GPIO_RE_HIGH,
 	[STMPE_IDX_GPFER_LSB]	= STMPE1801_REG_GPIO_FE_LOW,
+	[STMPE_IDX_GPFER_CSB]	= STMPE1801_REG_GPIO_FE_MID,
+	[STMPE_IDX_GPFER_MSB]	= STMPE1801_REG_GPIO_FE_HIGH,
 	[STMPE_IDX_GPPUR_LSB]	= STMPE1801_REG_GPIO_PULL_UP_LOW,
 	[STMPE_IDX_IEGPIOR_LSB]	= STMPE1801_REG_INT_EN_GPIO_MASK_LOW,
-	[STMPE_IDX_ISGPIOR_LSB]	= STMPE1801_REG_INT_STA_GPIO_LOW,
+	[STMPE_IDX_IEGPIOR_CSB]	= STMPE1801_REG_INT_EN_GPIO_MASK_MID,
+	[STMPE_IDX_IEGPIOR_MSB]	= STMPE1801_REG_INT_EN_GPIO_MASK_HIGH,
+	[STMPE_IDX_ISGPIOR_MSB]	= STMPE1801_REG_INT_STA_GPIO_HIGH,
 };
 
 static struct stmpe_variant_block stmpe1801_blocks[] = {
@@ -751,22 +832,31 @@ static int stmpe1801_enable(struct stmpe *stmpe, unsigned int blocks,
 				enable ? mask : 0);
 }
 
-static int stmpe1801_reset(struct stmpe *stmpe)
+static int stmpe_reset(struct stmpe *stmpe)
 {
+	u16 id_val = stmpe->variant->id_val;
 	unsigned long timeout;
 	int ret = 0;
+	u8 reset_bit;
+
+	if (id_val == STMPE811_ID)
+		/* STMPE801 and STMPE610 use bit 1 of SYS_CTRL register */
+		reset_bit = STMPE811_SYS_CTRL_RESET;
+	else
+		/* all other STMPE variant use bit 7 of SYS_CTRL register */
+		reset_bit = STMPE_SYS_CTRL_RESET;
 
-	ret = __stmpe_set_bits(stmpe, STMPE1801_REG_SYS_CTRL,
-		STMPE1801_MSK_SYS_CTRL_RESET, STMPE1801_MSK_SYS_CTRL_RESET);
+	ret = __stmpe_set_bits(stmpe, stmpe->regs[STMPE_IDX_SYS_CTRL],
+			       reset_bit, reset_bit);
 	if (ret < 0)
 		return ret;
 
 	timeout = jiffies + msecs_to_jiffies(100);
 	while (time_before(jiffies, timeout)) {
-		ret = __stmpe_reg_read(stmpe, STMPE1801_REG_SYS_CTRL);
+		ret = __stmpe_reg_read(stmpe, stmpe->regs[STMPE_IDX_SYS_CTRL]);
 		if (ret < 0)
 			return ret;
-		if (!(ret & STMPE1801_MSK_SYS_CTRL_RESET))
+		if (!(ret & reset_bit))
 			return 0;
 		usleep_range(100, 200);
 	}
@@ -794,20 +884,39 @@ static struct stmpe_variant_info stmpe1801 = {
 
 static const u8 stmpe24xx_regs[] = {
 	[STMPE_IDX_CHIP_ID]	= STMPE24XX_REG_CHIP_ID,
+	[STMPE_IDX_SYS_CTRL]	= STMPE24XX_REG_SYS_CTRL,
+	[STMPE_IDX_SYS_CTRL2]	= STMPE24XX_REG_SYS_CTRL2,
 	[STMPE_IDX_ICR_LSB]	= STMPE24XX_REG_ICR_LSB,
+	[STMPE_IDX_IER_MSB]	= STMPE24XX_REG_IER_MSB,
 	[STMPE_IDX_IER_LSB]	= STMPE24XX_REG_IER_LSB,
 	[STMPE_IDX_ISR_MSB]	= STMPE24XX_REG_ISR_MSB,
 	[STMPE_IDX_GPMR_LSB]	= STMPE24XX_REG_GPMR_LSB,
+	[STMPE_IDX_GPMR_CSB]	= STMPE24XX_REG_GPMR_CSB,
+	[STMPE_IDX_GPMR_MSB]	= STMPE24XX_REG_GPMR_MSB,
 	[STMPE_IDX_GPSR_LSB]	= STMPE24XX_REG_GPSR_LSB,
+	[STMPE_IDX_GPSR_CSB]	= STMPE24XX_REG_GPSR_CSB,
+	[STMPE_IDX_GPSR_MSB]	= STMPE24XX_REG_GPSR_MSB,
 	[STMPE_IDX_GPCR_LSB]	= STMPE24XX_REG_GPCR_LSB,
+	[STMPE_IDX_GPCR_CSB]	= STMPE24XX_REG_GPCR_CSB,
+	[STMPE_IDX_GPCR_MSB]	= STMPE24XX_REG_GPCR_MSB,
 	[STMPE_IDX_GPDR_LSB]	= STMPE24XX_REG_GPDR_LSB,
+	[STMPE_IDX_GPDR_CSB]	= STMPE24XX_REG_GPDR_CSB,
+	[STMPE_IDX_GPDR_MSB]	= STMPE24XX_REG_GPDR_MSB,
 	[STMPE_IDX_GPRER_LSB]	= STMPE24XX_REG_GPRER_LSB,
+	[STMPE_IDX_GPRER_CSB]	= STMPE24XX_REG_GPRER_CSB,
+	[STMPE_IDX_GPRER_MSB]	= STMPE24XX_REG_GPRER_MSB,
 	[STMPE_IDX_GPFER_LSB]	= STMPE24XX_REG_GPFER_LSB,
+	[STMPE_IDX_GPFER_CSB]	= STMPE24XX_REG_GPFER_CSB,
+	[STMPE_IDX_GPFER_MSB]	= STMPE24XX_REG_GPFER_MSB,
 	[STMPE_IDX_GPPUR_LSB]	= STMPE24XX_REG_GPPUR_LSB,
 	[STMPE_IDX_GPPDR_LSB]	= STMPE24XX_REG_GPPDR_LSB,
 	[STMPE_IDX_GPAFR_U_MSB]	= STMPE24XX_REG_GPAFR_U_MSB,
 	[STMPE_IDX_IEGPIOR_LSB]	= STMPE24XX_REG_IEGPIOR_LSB,
+	[STMPE_IDX_IEGPIOR_CSB]	= STMPE24XX_REG_IEGPIOR_CSB,
+	[STMPE_IDX_IEGPIOR_MSB]	= STMPE24XX_REG_IEGPIOR_MSB,
 	[STMPE_IDX_ISGPIOR_MSB]	= STMPE24XX_REG_ISGPIOR_MSB,
+	[STMPE_IDX_GPEDR_LSB]	= STMPE24XX_REG_GPEDR_LSB,
+	[STMPE_IDX_GPEDR_CSB]	= STMPE24XX_REG_GPEDR_CSB,
 	[STMPE_IDX_GPEDR_MSB]	= STMPE24XX_REG_GPEDR_MSB,
 };
 
@@ -840,7 +949,7 @@ static int stmpe24xx_enable(struct stmpe *stmpe, unsigned int blocks,
 	if (blocks & STMPE_BLOCK_KEYPAD)
 		mask |= STMPE24XX_SYS_CTRL_ENABLE_KPC;
 
-	return __stmpe_set_bits(stmpe, STMPE24XX_REG_SYS_CTRL, mask,
+	return __stmpe_set_bits(stmpe, stmpe->regs[STMPE_IDX_SYS_CTRL], mask,
 				enable ? mask : 0);
 }
 
@@ -893,6 +1002,7 @@ static struct stmpe_variant_info *stmpe_variant_info[STMPE_NBR_PARTS] = {
 	[STMPE610]	= &stmpe610,
 	[STMPE801]	= &stmpe801,
 	[STMPE811]	= &stmpe811,
+	[STMPE1600]	= &stmpe1600,
 	[STMPE1601]	= &stmpe1601,
 	[STMPE1801]	= &stmpe1801,
 	[STMPE2401]	= &stmpe2401,
@@ -919,7 +1029,8 @@ static irqreturn_t stmpe_irq(int irq, void *data)
 	int ret;
 	int i;
 
-	if (variant->id_val == STMPE801_ID) {
+	if (variant->id_val == STMPE801_ID ||
+	    variant->id_val == STMPE1600_ID) {
 		int base = irq_create_mapping(stmpe->domain, 0);
 
 		handle_nested_irq(base);
@@ -982,7 +1093,7 @@ static void stmpe_irq_sync_unlock(struct irq_data *data)
 			continue;
 
 		stmpe->oldier[i] = new;
-		stmpe_reg_write(stmpe, stmpe->regs[STMPE_IDX_IER_LSB] - i, new);
+		stmpe_reg_write(stmpe, stmpe->regs[STMPE_IDX_IER_LSB + i], new);
 	}
 
 	mutex_unlock(&stmpe->irq_lock);
@@ -1088,20 +1199,18 @@ static int stmpe_chip_init(struct stmpe *stmpe)
 	if (ret)
 		return ret;
 
-	if (id == STMPE1801_ID)	{
-		ret =  stmpe1801_reset(stmpe);
-		if (ret < 0)
-			return ret;
-	}
+	ret =  stmpe_reset(stmpe);
+	if (ret < 0)
+		return ret;
 
 	if (stmpe->irq >= 0) {
-		if (id == STMPE801_ID)
-			icr = STMPE801_REG_SYS_CTRL_INT_EN;
+		if (id == STMPE801_ID || id == STMPE1600_ID)
+			icr = STMPE_SYS_CTRL_INT_EN;
 		else
 			icr = STMPE_ICR_LSB_GIM;
 
-		/* STMPE801 doesn't support Edge interrupts */
-		if (id != STMPE801_ID) {
+		/* STMPE801 and STMPE1600 don't support Edge interrupts */
+		if (id != STMPE801_ID && id != STMPE1600_ID) {
 			if (irq_trigger == IRQF_TRIGGER_FALLING ||
 					irq_trigger == IRQF_TRIGGER_RISING)
 				icr |= STMPE_ICR_LSB_EDGE;
@@ -1109,8 +1218,8 @@ static int stmpe_chip_init(struct stmpe *stmpe)
 
 		if (irq_trigger == IRQF_TRIGGER_RISING ||
 				irq_trigger == IRQF_TRIGGER_HIGH) {
-			if (id == STMPE801_ID)
-				icr |= STMPE801_REG_SYS_CTRL_INT_HI;
+			if (id == STMPE801_ID || id == STMPE1600_ID)
+				icr |= STMPE_SYS_CTRL_INT_HI;
 			else
 				icr |= STMPE_ICR_LSB_HIGH;
 		}
diff --git a/drivers/mfd/stmpe.h b/drivers/mfd/stmpe.h
index 84adb46b3e2f..f7efdd8a5fc6 100644
--- a/drivers/mfd/stmpe.h
+++ b/drivers/mfd/stmpe.h
@@ -104,6 +104,10 @@ int stmpe_remove(struct stmpe *stmpe);
 #define STMPE_ICR_LSB_EDGE	(1 << 1)
 #define STMPE_ICR_LSB_GIM	(1 << 0)
 
+#define STMPE_SYS_CTRL_RESET	(1 << 7)
+#define STMPE_SYS_CTRL_INT_EN	(1 << 2)
+#define STMPE_SYS_CTRL_INT_HI	(1 << 0)
+
 /*
  * STMPE801
  */
@@ -119,13 +123,10 @@ int stmpe_remove(struct stmpe *stmpe);
 #define STMPE801_REG_GPIO_SET_PIN	0x11
 #define STMPE801_REG_GPIO_DIR		0x12
 
-#define STMPE801_REG_SYS_CTRL_RESET	(1 << 7)
-#define STMPE801_REG_SYS_CTRL_INT_EN	(1 << 2)
-#define STMPE801_REG_SYS_CTRL_INT_HI	(1 << 0)
-
 /*
  * STMPE811
  */
+#define STMPE811_ID			0x0811
 
 #define STMPE811_IRQ_TOUCH_DET		0
 #define STMPE811_IRQ_FIFO_TH		1
@@ -138,6 +139,7 @@ int stmpe_remove(struct stmpe *stmpe);
 #define STMPE811_NR_INTERNAL_IRQS	8
 
 #define STMPE811_REG_CHIP_ID		0x00
+#define STMPE811_REG_SYS_CTRL		0x03
 #define STMPE811_REG_SYS_CTRL2		0x04
 #define STMPE811_REG_SPI_CFG		0x08
 #define STMPE811_REG_INT_CTRL		0x09
@@ -154,12 +156,35 @@ int stmpe_remove(struct stmpe *stmpe);
 #define STMPE811_REG_GPIO_FE		0x16
 #define STMPE811_REG_GPIO_AF		0x17
 
+#define STMPE811_SYS_CTRL_RESET		(1 << 1)
+
 #define STMPE811_SYS_CTRL2_ADC_OFF	(1 << 0)
 #define STMPE811_SYS_CTRL2_TSC_OFF	(1 << 1)
 #define STMPE811_SYS_CTRL2_GPIO_OFF	(1 << 2)
 #define STMPE811_SYS_CTRL2_TS_OFF	(1 << 3)
 
 /*
+ * STMPE1600
+ */
+#define STMPE1600_ID			0x0016
+#define STMPE1600_NR_INTERNAL_IRQS	16
+
+#define STMPE1600_REG_CHIP_ID		0x00
+#define STMPE1600_REG_SYS_CTRL		0x03
+#define STMPE1600_REG_IEGPIOR_LSB	0x08
+#define STMPE1600_REG_IEGPIOR_MSB	0x09
+#define STMPE1600_REG_ISGPIOR_LSB	0x0A
+#define STMPE1600_REG_ISGPIOR_MSB	0x0B
+#define STMPE1600_REG_GPMR_LSB		0x10
+#define STMPE1600_REG_GPMR_MSB		0x11
+#define STMPE1600_REG_GPSR_LSB		0x12
+#define STMPE1600_REG_GPSR_MSB		0x13
+#define STMPE1600_REG_GPDR_LSB		0x14
+#define STMPE1600_REG_GPDR_MSB		0x15
+#define STMPE1600_REG_GPPIR_LSB		0x16
+#define STMPE1600_REG_GPPIR_MSB		0x17
+
+/*
  * STMPE1601
  */
 
@@ -175,19 +200,32 @@ int stmpe_remove(struct stmpe *stmpe);
 
 #define STMPE1601_REG_SYS_CTRL			0x02
 #define STMPE1601_REG_SYS_CTRL2			0x03
+#define STMPE1601_REG_ICR_MSB			0x10
 #define STMPE1601_REG_ICR_LSB			0x11
+#define STMPE1601_REG_IER_MSB			0x12
 #define STMPE1601_REG_IER_LSB			0x13
 #define STMPE1601_REG_ISR_MSB			0x14
-#define STMPE1601_REG_CHIP_ID			0x80
+#define STMPE1601_REG_ISR_LSB			0x15
+#define STMPE1601_REG_INT_EN_GPIO_MASK_MSB	0x16
 #define STMPE1601_REG_INT_EN_GPIO_MASK_LSB	0x17
 #define STMPE1601_REG_INT_STA_GPIO_MSB		0x18
-#define STMPE1601_REG_GPIO_MP_LSB		0x87
+#define STMPE1601_REG_INT_STA_GPIO_LSB		0x19
+#define STMPE1601_REG_CHIP_ID			0x80
+#define STMPE1601_REG_GPIO_SET_MSB		0x82
 #define STMPE1601_REG_GPIO_SET_LSB		0x83
+#define STMPE1601_REG_GPIO_CLR_MSB		0x84
 #define STMPE1601_REG_GPIO_CLR_LSB		0x85
+#define STMPE1601_REG_GPIO_MP_MSB		0x86
+#define STMPE1601_REG_GPIO_MP_LSB		0x87
+#define STMPE1601_REG_GPIO_SET_DIR_MSB		0x88
 #define STMPE1601_REG_GPIO_SET_DIR_LSB		0x89
 #define STMPE1601_REG_GPIO_ED_MSB		0x8A
+#define STMPE1601_REG_GPIO_ED_LSB		0x8B
+#define STMPE1601_REG_GPIO_RE_MSB		0x8C
 #define STMPE1601_REG_GPIO_RE_LSB		0x8D
+#define STMPE1601_REG_GPIO_FE_MSB		0x8E
 #define STMPE1601_REG_GPIO_FE_LSB		0x8F
+#define STMPE1601_REG_GPIO_PU_MSB		0x90
 #define STMPE1601_REG_GPIO_PU_LSB		0x91
 #define STMPE1601_REG_GPIO_AF_U_MSB		0x92
 
@@ -243,8 +281,6 @@ int stmpe_remove(struct stmpe *stmpe);
 #define STMPE1801_REG_GPIO_PULL_UP_MID		0x23
 #define STMPE1801_REG_GPIO_PULL_UP_HIGH		0x24
 
-#define STMPE1801_MSK_SYS_CTRL_RESET		(1 << 7)
-
 #define STMPE1801_MSK_INT_EN_KPC		(1 << 1)
 #define STMPE1801_MSK_INT_EN_GPIO		(1 << 3)
 
@@ -264,23 +300,48 @@ int stmpe_remove(struct stmpe *stmpe);
 #define STMPE24XX_NR_INTERNAL_IRQS	9
 
 #define STMPE24XX_REG_SYS_CTRL		0x02
+#define STMPE24XX_REG_SYS_CTRL2		0x03
+#define STMPE24XX_REG_ICR_MSB		0x10
 #define STMPE24XX_REG_ICR_LSB		0x11
+#define STMPE24XX_REG_IER_MSB		0x12
 #define STMPE24XX_REG_IER_LSB		0x13
 #define STMPE24XX_REG_ISR_MSB		0x14
-#define STMPE24XX_REG_CHIP_ID		0x80
+#define STMPE24XX_REG_ISR_LSB		0x15
+#define STMPE24XX_REG_IEGPIOR_MSB	0x16
+#define STMPE24XX_REG_IEGPIOR_CSB	0x17
 #define STMPE24XX_REG_IEGPIOR_LSB	0x18
 #define STMPE24XX_REG_ISGPIOR_MSB	0x19
-#define STMPE24XX_REG_GPMR_LSB		0xA4
+#define STMPE24XX_REG_ISGPIOR_CSB	0x1A
+#define STMPE24XX_REG_ISGPIOR_LSB	0x1B
+#define STMPE24XX_REG_CHIP_ID		0x80
+#define STMPE24XX_REG_GPSR_MSB		0x83
+#define STMPE24XX_REG_GPSR_CSB		0x84
 #define STMPE24XX_REG_GPSR_LSB		0x85
+#define STMPE24XX_REG_GPCR_MSB		0x86
+#define STMPE24XX_REG_GPCR_CSB		0x87
 #define STMPE24XX_REG_GPCR_LSB		0x88
+#define STMPE24XX_REG_GPDR_MSB		0x89
+#define STMPE24XX_REG_GPDR_CSB		0x8A
 #define STMPE24XX_REG_GPDR_LSB		0x8B
 #define STMPE24XX_REG_GPEDR_MSB		0x8C
+#define STMPE24XX_REG_GPEDR_CSB		0x8D
+#define STMPE24XX_REG_GPEDR_LSB		0x8E
+#define STMPE24XX_REG_GPRER_MSB		0x8F
+#define STMPE24XX_REG_GPRER_CSB		0x90
 #define STMPE24XX_REG_GPRER_LSB		0x91
+#define STMPE24XX_REG_GPFER_MSB		0x92
+#define STMPE24XX_REG_GPFER_CSB		0x93
 #define STMPE24XX_REG_GPFER_LSB		0x94
+#define STMPE24XX_REG_GPPUR_MSB		0x95
+#define STMPE24XX_REG_GPPUR_CSB		0x96
 #define STMPE24XX_REG_GPPUR_LSB		0x97
-#define STMPE24XX_REG_GPPDR_LSB		0x9a
+#define STMPE24XX_REG_GPPDR_MSB		0x98
+#define STMPE24XX_REG_GPPDR_CSB		0x99
+#define STMPE24XX_REG_GPPDR_LSB		0x9A
 #define STMPE24XX_REG_GPAFR_U_MSB	0x9B
-
+#define STMPE24XX_REG_GPMR_MSB		0xA2
+#define STMPE24XX_REG_GPMR_CSB		0xA3
+#define STMPE24XX_REG_GPMR_LSB		0xA4
 #define STMPE24XX_SYS_CTRL_ENABLE_GPIO		(1 << 3)
 #define STMPE24XX_SYSCON_ENABLE_PWM		(1 << 2)
 #define STMPE24XX_SYS_CTRL_ENABLE_KPC		(1 << 1)
diff --git a/drivers/mfd/sun6i-prcm.c b/drivers/mfd/sun6i-prcm.c
index 191173166d65..011fcc555945 100644
--- a/drivers/mfd/sun6i-prcm.c
+++ b/drivers/mfd/sun6i-prcm.c
@@ -9,7 +9,7 @@
  */
 
 #include <linux/mfd/core.h>
-#include <linux/module.h>
+#include <linux/init.h>
 #include <linux/of.h>
 
 struct prcm_data {
@@ -170,8 +170,4 @@ static struct platform_driver sun6i_prcm_driver = {
 	},
 	.probe = sun6i_prcm_probe,
 };
-module_platform_driver(sun6i_prcm_driver);
-
-MODULE_AUTHOR("Boris BREZILLON <boris.brezillon@free-electrons.com>");
-MODULE_DESCRIPTION("Allwinner sun6i PRCM driver");
-MODULE_LICENSE("GPL v2");
+builtin_platform_driver(sun6i_prcm_driver);
diff --git a/drivers/mfd/tps65217.c b/drivers/mfd/tps65217.c
index 049a6fcac651..9a4d8684dd32 100644
--- a/drivers/mfd/tps65217.c
+++ b/drivers/mfd/tps65217.c
@@ -15,22 +15,103 @@
  * GNU General Public License for more details.
  */
 
-#include <linux/kernel.h>
 #include <linux/device.h>
-#include <linux/module.h>
-#include <linux/platform_device.h>
+#include <linux/err.h>
 #include <linux/init.h>
+#include <linux/interrupt.h>
 #include <linux/i2c.h>
-#include <linux/slab.h>
-#include <linux/regmap.h>
-#include <linux/err.h>
+#include <linux/irq.h>
+#include <linux/irqdomain.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
 #include <linux/of.h>
 #include <linux/of_device.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+#include <linux/slab.h>
 
 #include <linux/mfd/core.h>
 #include <linux/mfd/tps65217.h>
 
-static const struct mfd_cell tps65217s[] = {
+static struct resource charger_resources[] = {
+	DEFINE_RES_IRQ_NAMED(TPS65217_IRQ_AC, "AC"),
+	DEFINE_RES_IRQ_NAMED(TPS65217_IRQ_USB, "USB"),
+};
+
+static struct resource pb_resources[] = {
+	DEFINE_RES_IRQ_NAMED(TPS65217_IRQ_PB, "PB"),
+};
+
+struct tps65217_irq {
+	int mask;
+	int interrupt;
+};
+
+static const struct tps65217_irq tps65217_irqs[] = {
+	[TPS65217_IRQ_PB] = {
+		.mask = TPS65217_INT_PBM,
+		.interrupt = TPS65217_INT_PBI,
+	},
+	[TPS65217_IRQ_AC] = {
+		.mask = TPS65217_INT_ACM,
+		.interrupt = TPS65217_INT_ACI,
+	},
+	[TPS65217_IRQ_USB] = {
+		.mask = TPS65217_INT_USBM,
+		.interrupt = TPS65217_INT_USBI,
+	},
+};
+
+static void tps65217_irq_lock(struct irq_data *data)
+{
+	struct tps65217 *tps = irq_data_get_irq_chip_data(data);
+
+	mutex_lock(&tps->irq_lock);
+}
+
+static void tps65217_irq_sync_unlock(struct irq_data *data)
+{
+	struct tps65217 *tps = irq_data_get_irq_chip_data(data);
+	int ret;
+
+	ret = tps65217_reg_write(tps, TPS65217_REG_INT, tps->irq_mask,
+				TPS65217_PROTECT_NONE);
+	if (ret != 0)
+		dev_err(tps->dev, "Failed to sync IRQ masks\n");
+
+	mutex_unlock(&tps->irq_lock);
+}
+
+static inline const struct tps65217_irq *
+irq_to_tps65217_irq(struct tps65217 *tps, struct irq_data *data)
+{
+	return &tps65217_irqs[data->hwirq];
+}
+
+static void tps65217_irq_enable(struct irq_data *data)
+{
+	struct tps65217 *tps = irq_data_get_irq_chip_data(data);
+	const struct tps65217_irq *irq_data = irq_to_tps65217_irq(tps, data);
+
+	tps->irq_mask &= ~irq_data->mask;
+}
+
+static void tps65217_irq_disable(struct irq_data *data)
+{
+	struct tps65217 *tps = irq_data_get_irq_chip_data(data);
+	const struct tps65217_irq *irq_data = irq_to_tps65217_irq(tps, data);
+
+	tps->irq_mask |= irq_data->mask;
+}
+
+static struct irq_chip tps65217_irq_chip = {
+	.irq_bus_lock		= tps65217_irq_lock,
+	.irq_bus_sync_unlock	= tps65217_irq_sync_unlock,
+	.irq_enable		= tps65217_irq_enable,
+	.irq_disable		= tps65217_irq_disable,
+};
+
+static struct mfd_cell tps65217s[] = {
 	{
 		.name = "tps65217-pmic",
 		.of_compatible = "ti,tps65217-pmic",
@@ -41,10 +122,96 @@ static const struct mfd_cell tps65217s[] = {
 	},
 	{
 		.name = "tps65217-charger",
+		.num_resources = ARRAY_SIZE(charger_resources),
+		.resources = charger_resources,
 		.of_compatible = "ti,tps65217-charger",
 	},
+	{
+		.name = "tps65217-pwrbutton",
+		.num_resources = ARRAY_SIZE(pb_resources),
+		.resources = pb_resources,
+		.of_compatible = "ti,tps65217-pwrbutton",
+	},
+};
+
+static irqreturn_t tps65217_irq_thread(int irq, void *data)
+{
+	struct tps65217 *tps = data;
+	unsigned int status;
+	bool handled = false;
+	int i;
+	int ret;
+
+	ret = tps65217_reg_read(tps, TPS65217_REG_INT, &status);
+	if (ret < 0) {
+		dev_err(tps->dev, "Failed to read IRQ status: %d\n",
+			ret);
+		return IRQ_NONE;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(tps65217_irqs); i++) {
+		if (status & tps65217_irqs[i].interrupt) {
+			handle_nested_irq(irq_find_mapping(tps->irq_domain, i));
+			handled = true;
+		}
+	}
+
+	if (handled)
+		return IRQ_HANDLED;
+
+	return IRQ_NONE;
+}
+
+static int tps65217_irq_map(struct irq_domain *h, unsigned int virq,
+			irq_hw_number_t hw)
+{
+	struct tps65217 *tps = h->host_data;
+
+	irq_set_chip_data(virq, tps);
+	irq_set_chip_and_handler(virq, &tps65217_irq_chip, handle_edge_irq);
+	irq_set_nested_thread(virq, 1);
+	irq_set_parent(virq, tps->irq);
+	irq_set_noprobe(virq);
+
+	return 0;
+}
+
+static const struct irq_domain_ops tps65217_irq_domain_ops = {
+	.map = tps65217_irq_map,
 };
 
+static int tps65217_irq_init(struct tps65217 *tps, int irq)
+{
+	int ret;
+
+	mutex_init(&tps->irq_lock);
+	tps->irq = irq;
+
+	/* Mask all interrupt sources */
+	tps->irq_mask = (TPS65217_INT_RESERVEDM | TPS65217_INT_PBM
+			| TPS65217_INT_ACM | TPS65217_INT_USBM);
+	tps65217_reg_write(tps, TPS65217_REG_INT, tps->irq_mask,
+			TPS65217_PROTECT_NONE);
+
+	tps->irq_domain = irq_domain_add_linear(tps->dev->of_node,
+		TPS65217_NUM_IRQ, &tps65217_irq_domain_ops, tps);
+	if (!tps->irq_domain) {
+		dev_err(tps->dev, "Could not create IRQ domain\n");
+		return -ENOMEM;
+	}
+
+	ret = devm_request_threaded_irq(tps->dev, irq, NULL,
+					tps65217_irq_thread, IRQF_ONESHOT,
+					"tps65217-irq", tps);
+	if (ret) {
+		dev_err(tps->dev, "Failed to request IRQ %d: %d\n",
+			irq, ret);
+		return ret;
+	}
+
+	return 0;
+}
+
 /**
  * tps65217_reg_read: Read a single tps65217 register.
  *
@@ -149,11 +316,22 @@ int tps65217_clear_bits(struct tps65217 *tps, unsigned int reg,
 }
 EXPORT_SYMBOL_GPL(tps65217_clear_bits);
 
+static bool tps65217_volatile_reg(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case TPS65217_REG_INT:
+		return true;
+	default:
+		return false;
+	}
+}
+
 static const struct regmap_config tps65217_regmap_config = {
 	.reg_bits = 8,
 	.val_bits = 8,
 
 	.max_register = TPS65217_REG_MAX,
+	.volatile_reg = tps65217_volatile_reg,
 };
 
 static const struct of_device_id tps65217_of_match[] = {
@@ -205,8 +383,19 @@ static int tps65217_probe(struct i2c_client *client,
 		return ret;
 	}
 
+	if (client->irq) {
+		tps65217_irq_init(tps, client->irq);
+	} else {
+		int i;
+
+		/* Don't tell children about IRQ resources which won't fire */
+		for (i = 0; i < ARRAY_SIZE(tps65217s); i++)
+			tps65217s[i].num_resources = 0;
+	}
+
 	ret = devm_mfd_add_devices(tps->dev, -1, tps65217s,
-				   ARRAY_SIZE(tps65217s), NULL, 0, NULL);
+				   ARRAY_SIZE(tps65217s), NULL, 0,
+				   tps->irq_domain);
 	if (ret < 0) {
 		dev_err(tps->dev, "mfd_add_devices failed: %d\n", ret);
 		return ret;
diff --git a/drivers/mfd/tps65218.c b/drivers/mfd/tps65218.c
index 80b9dc363cd8..ba610adbdbff 100644
--- a/drivers/mfd/tps65218.c
+++ b/drivers/mfd/tps65218.c
@@ -219,6 +219,7 @@ static int tps65218_probe(struct i2c_client *client,
 	struct tps65218 *tps;
 	const struct of_device_id *match;
 	int ret;
+	unsigned int chipid;
 
 	match = of_match_device(of_tps65218_match_table, &client->dev);
 	if (!match) {
@@ -250,6 +251,14 @@ static int tps65218_probe(struct i2c_client *client,
 	if (ret < 0)
 		return ret;
 
+	ret = tps65218_reg_read(tps, TPS65218_REG_CHIPID, &chipid);
+	if (ret) {
+		dev_err(tps->dev, "Failed to read chipid: %d\n", ret);
+		return ret;
+	}
+
+	tps->rev = chipid & TPS65218_CHIPID_REV_MASK;
+
 	ret = of_platform_populate(client->dev.of_node, NULL, NULL,
 				   &client->dev);
 	if (ret < 0)
diff --git a/drivers/mfd/twl-core.c b/drivers/mfd/twl-core.c
index a49d3db6d936..c64615dca2bd 100644
--- a/drivers/mfd/twl-core.c
+++ b/drivers/mfd/twl-core.c
@@ -30,7 +30,6 @@
 
 #include <linux/init.h>
 #include <linux/mutex.h>
-#include <linux/module.h>
 #include <linux/platform_device.h>
 #include <linux/regmap.h>
 #include <linux/clk.h>
@@ -1258,7 +1257,6 @@ static const struct i2c_device_id twl_ids[] = {
 	{ "twl6032", TWL6030_CLASS | TWL6032_SUBCLASS }, /* "Phoenix lite" */
 	{ /* end of list */ },
 };
-MODULE_DEVICE_TABLE(i2c, twl_ids);
 
 /* One Client Driver , 4 Clients */
 static struct i2c_driver twl_driver = {
@@ -1267,9 +1265,4 @@ static struct i2c_driver twl_driver = {
 	.probe		= twl_probe,
 	.remove		= twl_remove,
 };
-
-module_i2c_driver(twl_driver);
-
-MODULE_AUTHOR("Texas Instruments, Inc.");
-MODULE_DESCRIPTION("I2C Core interface for TWL");
-MODULE_LICENSE("GPL");
+builtin_i2c_driver(twl_driver);
diff --git a/drivers/mfd/twl6040.c b/drivers/mfd/twl6040.c
index ab328ec49353..d66502d36ba0 100644
--- a/drivers/mfd/twl6040.c
+++ b/drivers/mfd/twl6040.c
@@ -609,6 +609,7 @@ static const struct regmap_config twl6040_regmap_config = {
 	.writeable_reg = twl6040_writeable_reg,
 
 	.cache_type = REGCACHE_RBTREE,
+	.use_single_rw = true,
 };
 
 static const struct regmap_irq twl6040_irqs[] = {
@@ -782,6 +783,11 @@ static int twl6040_probe(struct i2c_client *client,
 	cell->name = "twl6040-gpo";
 	children++;
 
+	/* PDM clock support  */
+	cell = &twl6040->cells[children];
+	cell->name = "twl6040-pdmclk";
+	children++;
+
 	/* The chip is powered down so mark regmap to cache only and dirty */
 	regcache_cache_only(twl6040->regmap, true);
 	regcache_mark_dirty(twl6040->regmap);
diff --git a/drivers/mfd/ucb1x00-core.c b/drivers/mfd/ucb1x00-core.c
index 9ab9ec47ea75..d6fb2e1a759a 100644
--- a/drivers/mfd/ucb1x00-core.c
+++ b/drivers/mfd/ucb1x00-core.c
@@ -446,10 +446,6 @@ static int ucb1x00_detect_irq(struct ucb1x00 *ucb)
 	unsigned long mask;
 
 	mask = probe_irq_on();
-	if (!mask) {
-		probe_irq_off(mask);
-		return NO_IRQ;
-	}
 
 	/*
 	 * Enable the ADC interrupt.
@@ -541,7 +537,7 @@ static int ucb1x00_probe(struct mcp *mcp)
 	ucb1x00_enable(ucb);
 	ucb->irq = ucb1x00_detect_irq(ucb);
 	ucb1x00_disable(ucb);
-	if (ucb->irq == NO_IRQ) {
+	if (!ucb->irq) {
 		dev_err(&ucb->dev, "IRQ probe failed\n");
 		ret = -ENODEV;
 		goto err_no_irq;
diff --git a/drivers/misc/Kconfig b/drivers/misc/Kconfig
index d00252828966..64971baf11fa 100644
--- a/drivers/misc/Kconfig
+++ b/drivers/misc/Kconfig
@@ -429,34 +429,6 @@ config ARM_CHARLCD
 	  line and the Linux version on the second line, but that's
 	  still useful.
 
-config BMP085
-	tristate
-	depends on SYSFS
-
-config BMP085_I2C
-	tristate "BMP085 digital pressure sensor on I2C"
-	select BMP085
-	select REGMAP_I2C
-	depends on I2C && SYSFS
-	help
-	  Say Y here if you want to support Bosch Sensortec's digital pressure
-	  sensor hooked to an I2C bus.
-
-	  To compile this driver as a module, choose M here: the
-	  module will be called bmp085-i2c.
-
-config BMP085_SPI
-	tristate "BMP085 digital pressure sensor on SPI"
-	select BMP085
-	select REGMAP_SPI
-	depends on SPI_MASTER && SYSFS
-	help
-	  Say Y here if you want to support Bosch Sensortec's digital pressure
-	  sensor hooked to an SPI bus.
-
-	  To compile this driver as a module, choose M here: the
-	  module will be called bmp085-spi.
-
 config PCH_PHUB
 	tristate "Intel EG20T PCH/LAPIS Semicon IOH(ML7213/ML7223/ML7831) PHUB"
 	select GENERIC_NET_UTILS
diff --git a/drivers/misc/Makefile b/drivers/misc/Makefile
index fb32516ddfe2..31983366090a 100644
--- a/drivers/misc/Makefile
+++ b/drivers/misc/Makefile
@@ -9,9 +9,6 @@ obj-$(CONFIG_AD525X_DPOT_SPI)	+= ad525x_dpot-spi.o
 obj-$(CONFIG_INTEL_MID_PTI)	+= pti.o
 obj-$(CONFIG_ATMEL_SSC)		+= atmel-ssc.o
 obj-$(CONFIG_ATMEL_TCLIB)	+= atmel_tclib.o
-obj-$(CONFIG_BMP085)		+= bmp085.o
-obj-$(CONFIG_BMP085_I2C)	+= bmp085-i2c.o
-obj-$(CONFIG_BMP085_SPI)	+= bmp085-spi.o
 obj-$(CONFIG_DUMMY_IRQ)		+= dummy-irq.o
 obj-$(CONFIG_ICS932S401)	+= ics932s401.o
 obj-$(CONFIG_LKDTM)		+= lkdtm.o
diff --git a/drivers/misc/bmp085-i2c.c b/drivers/misc/bmp085-i2c.c
deleted file mode 100644
index f35c218aaa1a..000000000000
--- a/drivers/misc/bmp085-i2c.c
+++ /dev/null
@@ -1,83 +0,0 @@
-/*
- * Copyright (c) 2012  Bosch Sensortec GmbH
- * Copyright (c) 2012  Unixphere AB
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
- */
-
-#include <linux/module.h>
-#include <linux/i2c.h>
-#include <linux/err.h>
-#include "bmp085.h"
-
-#define BMP085_I2C_ADDRESS	0x77
-
-static const unsigned short normal_i2c[] = { BMP085_I2C_ADDRESS,
-							I2C_CLIENT_END };
-
-static int bmp085_i2c_detect(struct i2c_client *client,
-			     struct i2c_board_info *info)
-{
-	if (client->addr != BMP085_I2C_ADDRESS)
-		return -ENODEV;
-
-	return bmp085_detect(&client->dev);
-}
-
-static int bmp085_i2c_probe(struct i2c_client *client,
-				      const struct i2c_device_id *id)
-{
-	int err;
-	struct regmap *regmap = devm_regmap_init_i2c(client,
-						     &bmp085_regmap_config);
-
-	if (IS_ERR(regmap)) {
-		err = PTR_ERR(regmap);
-		dev_err(&client->dev, "Failed to init regmap: %d\n", err);
-		return err;
-	}
-
-	return bmp085_probe(&client->dev, regmap, client->irq);
-}
-
-static int bmp085_i2c_remove(struct i2c_client *client)
-{
-	return bmp085_remove(&client->dev);
-}
-
-static const struct i2c_device_id bmp085_id[] = {
-	{ BMP085_NAME, 0 },
-	{ "bmp180", 0 },
-	{ }
-};
-MODULE_DEVICE_TABLE(i2c, bmp085_id);
-
-static struct i2c_driver bmp085_i2c_driver = {
-	.driver = {
-		.name	= BMP085_NAME,
-	},
-	.id_table	= bmp085_id,
-	.probe		= bmp085_i2c_probe,
-	.remove		= bmp085_i2c_remove,
-
-	.detect		= bmp085_i2c_detect,
-	.address_list	= normal_i2c
-};
-
-module_i2c_driver(bmp085_i2c_driver);
-
-MODULE_AUTHOR("Eric Andersson <eric.andersson@unixphere.com>");
-MODULE_DESCRIPTION("BMP085 I2C bus driver");
-MODULE_LICENSE("GPL");
diff --git a/drivers/misc/bmp085-spi.c b/drivers/misc/bmp085-spi.c
deleted file mode 100644
index 17ecbf95ff15..000000000000
--- a/drivers/misc/bmp085-spi.c
+++ /dev/null
@@ -1,79 +0,0 @@
-/*
- * Copyright (c) 2012  Bosch Sensortec GmbH
- * Copyright (c) 2012  Unixphere AB
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
- */
-
-#include <linux/module.h>
-#include <linux/spi/spi.h>
-#include <linux/err.h>
-#include "bmp085.h"
-
-static int bmp085_spi_probe(struct spi_device *client)
-{
-	int err;
-	struct regmap *regmap;
-
-	client->bits_per_word = 8;
-	err = spi_setup(client);
-	if (err < 0) {
-		dev_err(&client->dev, "spi_setup failed!\n");
-		return err;
-	}
-
-	regmap = devm_regmap_init_spi(client, &bmp085_regmap_config);
-	if (IS_ERR(regmap)) {
-		err = PTR_ERR(regmap);
-		dev_err(&client->dev, "Failed to init regmap: %d\n", err);
-		return err;
-	}
-
-	return bmp085_probe(&client->dev, regmap, client->irq);
-}
-
-static int bmp085_spi_remove(struct spi_device *client)
-{
-	return bmp085_remove(&client->dev);
-}
-
-static const struct of_device_id bmp085_of_match[] = {
-	{ .compatible = "bosch,bmp085", },
-	{ },
-};
-MODULE_DEVICE_TABLE(of, bmp085_of_match);
-
-static const struct spi_device_id bmp085_id[] = {
-	{ "bmp180", 0 },
-	{ "bmp181", 0 },
-	{ }
-};
-MODULE_DEVICE_TABLE(spi, bmp085_id);
-
-static struct spi_driver bmp085_spi_driver = {
-	.driver = {
-		.name	= BMP085_NAME,
-		.of_match_table = bmp085_of_match
-	},
-	.id_table	= bmp085_id,
-	.probe		= bmp085_spi_probe,
-	.remove		= bmp085_spi_remove
-};
-
-module_spi_driver(bmp085_spi_driver);
-
-MODULE_AUTHOR("Eric Andersson <eric.andersson@unixphere.com>");
-MODULE_DESCRIPTION("BMP085 SPI bus driver");
-MODULE_LICENSE("GPL");
diff --git a/drivers/misc/bmp085.c b/drivers/misc/bmp085.c
deleted file mode 100644
index 9b313f7810f5..000000000000
--- a/drivers/misc/bmp085.c
+++ /dev/null
@@ -1,506 +0,0 @@
-/*  Copyright (c) 2010  Christoph Mair <christoph.mair@gmail.com>
- *  Copyright (c) 2012  Bosch Sensortec GmbH
- *  Copyright (c) 2012  Unixphere AB
- *
- *  This driver supports the bmp085 and bmp18x digital barometric pressure
- *  and temperature sensors from Bosch Sensortec. The datasheets
- *  are available from their website:
- *  http://www.bosch-sensortec.com/content/language1/downloads/BST-BMP085-DS000-05.pdf
- *  http://www.bosch-sensortec.com/content/language1/downloads/BST-BMP180-DS000-07.pdf
- *
- *  A pressure measurement is issued by reading from pressure0_input.
- *  The return value ranges from 30000 to 110000 pascal with a resulution
- *  of 1 pascal (0.01 millibar) which enables measurements from 9000m above
- *  to 500m below sea level.
- *
- *  The temperature can be read from temp0_input. Values range from
- *  -400 to 850 representing the ambient temperature in degree celsius
- *  multiplied by 10.The resolution is 0.1 celsius.
- *
- *  Because ambient pressure is temperature dependent, a temperature
- *  measurement will be executed automatically even if the user is reading
- *  from pressure0_input. This happens if the last temperature measurement
- *  has been executed more then one second ago.
- *
- *  To decrease RMS noise from pressure measurements, the bmp085 can
- *  autonomously calculate the average of up to eight samples. This is
- *  set up by writing to the oversampling sysfs file. Accepted values
- *  are 0, 1, 2 and 3. 2^x when x is the value written to this file
- *  specifies the number of samples used to calculate the ambient pressure.
- *  RMS noise is specified with six pascal (without averaging) and decreases
- *  down to 3 pascal when using an oversampling setting of 3.
- *
- *  This program is free software; you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation; either version 2 of the License, or
- *  (at your option) any later version.
- *
- *  This program is distributed in the hope that it will be useful,
- *  but WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with this program; if not, write to the Free Software
- *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
- */
-
-#include <linux/module.h>
-#include <linux/device.h>
-#include <linux/slab.h>
-#include <linux/of.h>
-#include "bmp085.h"
-#include <linux/interrupt.h>
-#include <linux/completion.h>
-#include <linux/gpio.h>
-
-#define BMP085_CHIP_ID			0x55
-#define BMP085_CALIBRATION_DATA_START	0xAA
-#define BMP085_CALIBRATION_DATA_LENGTH	11	/* 16 bit values */
-#define BMP085_CHIP_ID_REG		0xD0
-#define BMP085_CTRL_REG			0xF4
-#define BMP085_TEMP_MEASUREMENT		0x2E
-#define BMP085_PRESSURE_MEASUREMENT	0x34
-#define BMP085_CONVERSION_REGISTER_MSB	0xF6
-#define BMP085_CONVERSION_REGISTER_LSB	0xF7
-#define BMP085_CONVERSION_REGISTER_XLSB	0xF8
-#define BMP085_TEMP_CONVERSION_TIME	5
-
-struct bmp085_calibration_data {
-	s16 AC1, AC2, AC3;
-	u16 AC4, AC5, AC6;
-	s16 B1, B2;
-	s16 MB, MC, MD;
-};
-
-struct bmp085_data {
-	struct	device *dev;
-	struct  regmap *regmap;
-	struct	mutex lock;
-	struct	bmp085_calibration_data calibration;
-	u8	oversampling_setting;
-	u32	raw_temperature;
-	u32	raw_pressure;
-	u32	temp_measurement_period;
-	unsigned long last_temp_measurement;
-	u8	chip_id;
-	s32	b6; /* calculated temperature correction coefficient */
-	int	irq;
-	struct	completion done;
-};
-
-static irqreturn_t bmp085_eoc_isr(int irq, void *devid)
-{
-	struct bmp085_data *data = devid;
-
-	complete(&data->done);
-
-	return IRQ_HANDLED;
-}
-
-static s32 bmp085_read_calibration_data(struct bmp085_data *data)
-{
-	u16 tmp[BMP085_CALIBRATION_DATA_LENGTH];
-	struct bmp085_calibration_data *cali = &(data->calibration);
-	s32 status = regmap_bulk_read(data->regmap,
-				BMP085_CALIBRATION_DATA_START, (u8 *)tmp,
-				(BMP085_CALIBRATION_DATA_LENGTH << 1));
-	if (status < 0)
-		return status;
-
-	cali->AC1 =  be16_to_cpu(tmp[0]);
-	cali->AC2 =  be16_to_cpu(tmp[1]);
-	cali->AC3 =  be16_to_cpu(tmp[2]);
-	cali->AC4 =  be16_to_cpu(tmp[3]);
-	cali->AC5 =  be16_to_cpu(tmp[4]);
-	cali->AC6 = be16_to_cpu(tmp[5]);
-	cali->B1 = be16_to_cpu(tmp[6]);
-	cali->B2 = be16_to_cpu(tmp[7]);
-	cali->MB = be16_to_cpu(tmp[8]);
-	cali->MC = be16_to_cpu(tmp[9]);
-	cali->MD = be16_to_cpu(tmp[10]);
-	return 0;
-}
-
-static s32 bmp085_update_raw_temperature(struct bmp085_data *data)
-{
-	u16 tmp;
-	s32 status;
-
-	mutex_lock(&data->lock);
-
-	init_completion(&data->done);
-
-	status = regmap_write(data->regmap, BMP085_CTRL_REG,
-			      BMP085_TEMP_MEASUREMENT);
-	if (status < 0) {
-		dev_err(data->dev,
-			"Error while requesting temperature measurement.\n");
-		goto exit;
-	}
-	wait_for_completion_timeout(&data->done, 1 + msecs_to_jiffies(
-					    BMP085_TEMP_CONVERSION_TIME));
-
-	status = regmap_bulk_read(data->regmap, BMP085_CONVERSION_REGISTER_MSB,
-				 &tmp, sizeof(tmp));
-	if (status < 0) {
-		dev_err(data->dev,
-			"Error while reading temperature measurement result\n");
-		goto exit;
-	}
-	data->raw_temperature = be16_to_cpu(tmp);
-	data->last_temp_measurement = jiffies;
-	status = 0;	/* everything ok, return 0 */
-
-exit:
-	mutex_unlock(&data->lock);
-	return status;
-}
-
-static s32 bmp085_update_raw_pressure(struct bmp085_data *data)
-{
-	u32 tmp = 0;
-	s32 status;
-
-	mutex_lock(&data->lock);
-
-	init_completion(&data->done);
-
-	status = regmap_write(data->regmap, BMP085_CTRL_REG,
-			BMP085_PRESSURE_MEASUREMENT +
-			(data->oversampling_setting << 6));
-	if (status < 0) {
-		dev_err(data->dev,
-			"Error while requesting pressure measurement.\n");
-		goto exit;
-	}
-
-	/* wait for the end of conversion */
-	wait_for_completion_timeout(&data->done, 1 + msecs_to_jiffies(
-					2+(3 << data->oversampling_setting)));
-	/* copy data into a u32 (4 bytes), but skip the first byte. */
-	status = regmap_bulk_read(data->regmap, BMP085_CONVERSION_REGISTER_MSB,
-				 ((u8 *)&tmp)+1, 3);
-	if (status < 0) {
-		dev_err(data->dev,
-			"Error while reading pressure measurement results\n");
-		goto exit;
-	}
-	data->raw_pressure = be32_to_cpu((tmp));
-	data->raw_pressure >>= (8-data->oversampling_setting);
-	status = 0;	/* everything ok, return 0 */
-
-exit:
-	mutex_unlock(&data->lock);
-	return status;
-}
-
-/*
- * This function starts the temperature measurement and returns the value
- * in tenth of a degree celsius.
- */
-static s32 bmp085_get_temperature(struct bmp085_data *data, int *temperature)
-{
-	struct bmp085_calibration_data *cali = &data->calibration;
-	long x1, x2;
-	int status;
-
-	status = bmp085_update_raw_temperature(data);
-	if (status < 0)
-		goto exit;
-
-	x1 = ((data->raw_temperature - cali->AC6) * cali->AC5) >> 15;
-	x2 = (cali->MC << 11) / (x1 + cali->MD);
-	data->b6 = x1 + x2 - 4000;
-	/* if NULL just update b6. Used for pressure only measurements */
-	if (temperature != NULL)
-		*temperature = (x1+x2+8) >> 4;
-
-exit:
-	return status;
-}
-
-/*
- * This function starts the pressure measurement and returns the value
- * in millibar. Since the pressure depends on the ambient temperature,
- * a temperature measurement is executed according to the given temperature
- * measurement period (default is 1 sec boundary). This period could vary
- * and needs to be adjusted according to the sensor environment, i.e. if big
- * temperature variations then the temperature needs to be read out often.
- */
-static s32 bmp085_get_pressure(struct bmp085_data *data, int *pressure)
-{
-	struct bmp085_calibration_data *cali = &data->calibration;
-	s32 x1, x2, x3, b3;
-	u32 b4, b7;
-	s32 p;
-	int status;
-
-	/* alt least every second force an update of the ambient temperature */
-	if ((data->last_temp_measurement == 0) ||
-	    time_is_before_jiffies(data->last_temp_measurement + 1*HZ)) {
-		status = bmp085_get_temperature(data, NULL);
-		if (status < 0)
-			return status;
-	}
-
-	status = bmp085_update_raw_pressure(data);
-	if (status < 0)
-		return status;
-
-	x1 = (data->b6 * data->b6) >> 12;
-	x1 *= cali->B2;
-	x1 >>= 11;
-
-	x2 = cali->AC2 * data->b6;
-	x2 >>= 11;
-
-	x3 = x1 + x2;
-
-	b3 = (((((s32)cali->AC1) * 4 + x3) << data->oversampling_setting) + 2);
-	b3 >>= 2;
-
-	x1 = (cali->AC3 * data->b6) >> 13;
-	x2 = (cali->B1 * ((data->b6 * data->b6) >> 12)) >> 16;
-	x3 = (x1 + x2 + 2) >> 2;
-	b4 = (cali->AC4 * (u32)(x3 + 32768)) >> 15;
-
-	b7 = ((u32)data->raw_pressure - b3) *
-					(50000 >> data->oversampling_setting);
-	p = ((b7 < 0x80000000) ? ((b7 << 1) / b4) : ((b7 / b4) * 2));
-
-	x1 = p >> 8;
-	x1 *= x1;
-	x1 = (x1 * 3038) >> 16;
-	x2 = (-7357 * p) >> 16;
-	p += (x1 + x2 + 3791) >> 4;
-
-	*pressure = p;
-
-	return 0;
-}
-
-/*
- * This function sets the chip-internal oversampling. Valid values are 0..3.
- * The chip will use 2^oversampling samples for internal averaging.
- * This influences the measurement time and the accuracy; larger values
- * increase both. The datasheet gives an overview on how measurement time,
- * accuracy and noise correlate.
- */
-static void bmp085_set_oversampling(struct bmp085_data *data,
-						unsigned char oversampling)
-{
-	if (oversampling > 3)
-		oversampling = 3;
-	data->oversampling_setting = oversampling;
-}
-
-/*
- * Returns the currently selected oversampling. Range: 0..3
- */
-static unsigned char bmp085_get_oversampling(struct bmp085_data *data)
-{
-	return data->oversampling_setting;
-}
-
-/* sysfs callbacks */
-static ssize_t set_oversampling(struct device *dev,
-				struct device_attribute *attr,
-				const char *buf, size_t count)
-{
-	struct bmp085_data *data = dev_get_drvdata(dev);
-	unsigned long oversampling;
-	int err = kstrtoul(buf, 10, &oversampling);
-
-	if (err == 0) {
-		mutex_lock(&data->lock);
-		bmp085_set_oversampling(data, oversampling);
-		mutex_unlock(&data->lock);
-		return count;
-	}
-
-	return err;
-}
-
-static ssize_t show_oversampling(struct device *dev,
-				 struct device_attribute *attr, char *buf)
-{
-	struct bmp085_data *data = dev_get_drvdata(dev);
-
-	return sprintf(buf, "%u\n", bmp085_get_oversampling(data));
-}
-static DEVICE_ATTR(oversampling, S_IWUSR | S_IRUGO,
-					show_oversampling, set_oversampling);
-
-
-static ssize_t show_temperature(struct device *dev,
-				struct device_attribute *attr, char *buf)
-{
-	int temperature;
-	int status;
-	struct bmp085_data *data = dev_get_drvdata(dev);
-
-	status = bmp085_get_temperature(data, &temperature);
-	if (status < 0)
-		return status;
-	else
-		return sprintf(buf, "%d\n", temperature);
-}
-static DEVICE_ATTR(temp0_input, S_IRUGO, show_temperature, NULL);
-
-
-static ssize_t show_pressure(struct device *dev,
-			     struct device_attribute *attr, char *buf)
-{
-	int pressure;
-	int status;
-	struct bmp085_data *data = dev_get_drvdata(dev);
-
-	status = bmp085_get_pressure(data, &pressure);
-	if (status < 0)
-		return status;
-	else
-		return sprintf(buf, "%d\n", pressure);
-}
-static DEVICE_ATTR(pressure0_input, S_IRUGO, show_pressure, NULL);
-
-
-static struct attribute *bmp085_attributes[] = {
-	&dev_attr_temp0_input.attr,
-	&dev_attr_pressure0_input.attr,
-	&dev_attr_oversampling.attr,
-	NULL
-};
-
-static const struct attribute_group bmp085_attr_group = {
-	.attrs = bmp085_attributes,
-};
-
-int bmp085_detect(struct device *dev)
-{
-	struct bmp085_data *data = dev_get_drvdata(dev);
-	unsigned int id;
-	int ret;
-
-	ret = regmap_read(data->regmap, BMP085_CHIP_ID_REG, &id);
-	if (ret < 0)
-		return ret;
-
-	if (id != data->chip_id)
-		return -ENODEV;
-
-	return 0;
-}
-EXPORT_SYMBOL_GPL(bmp085_detect);
-
-static void bmp085_get_of_properties(struct bmp085_data *data)
-{
-#ifdef CONFIG_OF
-	struct device_node *np = data->dev->of_node;
-	u32 prop;
-
-	if (!np)
-		return;
-
-	if (!of_property_read_u32(np, "chip-id", &prop))
-		data->chip_id = prop & 0xff;
-
-	if (!of_property_read_u32(np, "temp-measurement-period", &prop))
-		data->temp_measurement_period = (prop/100)*HZ;
-
-	if (!of_property_read_u32(np, "default-oversampling", &prop))
-		data->oversampling_setting = prop & 0xff;
-#endif
-}
-
-static int bmp085_init_client(struct bmp085_data *data)
-{
-	int status = bmp085_read_calibration_data(data);
-
-	if (status < 0)
-		return status;
-
-	/* default settings */
-	data->chip_id = BMP085_CHIP_ID;
-	data->last_temp_measurement = 0;
-	data->temp_measurement_period = 1*HZ;
-	data->oversampling_setting = 3;
-
-	bmp085_get_of_properties(data);
-
-	mutex_init(&data->lock);
-
-	return 0;
-}
-
-struct regmap_config bmp085_regmap_config = {
-	.reg_bits = 8,
-	.val_bits = 8
-};
-EXPORT_SYMBOL_GPL(bmp085_regmap_config);
-
-int bmp085_probe(struct device *dev, struct regmap *regmap, int irq)
-{
-	struct bmp085_data *data;
-	int err = 0;
-
-	data = kzalloc(sizeof(struct bmp085_data), GFP_KERNEL);
-	if (!data) {
-		err = -ENOMEM;
-		goto exit;
-	}
-
-	dev_set_drvdata(dev, data);
-	data->dev = dev;
-	data->regmap = regmap;
-	data->irq = irq;
-
-	if (data->irq > 0) {
-		err = devm_request_irq(dev, data->irq, bmp085_eoc_isr,
-					      IRQF_TRIGGER_RISING, "bmp085",
-					      data);
-		if (err < 0)
-			goto exit_free;
-	}
-
-	/* Initialize the BMP085 chip */
-	err = bmp085_init_client(data);
-	if (err < 0)
-		goto exit_free;
-
-	err = bmp085_detect(dev);
-	if (err < 0) {
-		dev_err(dev, "%s: chip_id failed!\n", BMP085_NAME);
-		goto exit_free;
-	}
-
-	/* Register sysfs hooks */
-	err = sysfs_create_group(&dev->kobj, &bmp085_attr_group);
-	if (err)
-		goto exit_free;
-
-	dev_info(dev, "Successfully initialized %s!\n", BMP085_NAME);
-
-	return 0;
-
-exit_free:
-	kfree(data);
-exit:
-	return err;
-}
-EXPORT_SYMBOL_GPL(bmp085_probe);
-
-int bmp085_remove(struct device *dev)
-{
-	struct bmp085_data *data = dev_get_drvdata(dev);
-
-	sysfs_remove_group(&data->dev->kobj, &bmp085_attr_group);
-	kfree(data);
-
-	return 0;
-}
-EXPORT_SYMBOL_GPL(bmp085_remove);
-
-MODULE_AUTHOR("Christoph Mair <christoph.mair@gmail.com>");
-MODULE_DESCRIPTION("BMP085 driver");
-MODULE_LICENSE("GPL");
diff --git a/drivers/misc/bmp085.h b/drivers/misc/bmp085.h
deleted file mode 100644
index 8b8e3b1f5ca5..000000000000
--- a/drivers/misc/bmp085.h
+++ /dev/null
@@ -1,33 +0,0 @@
-/*
- * Copyright (c) 2012  Bosch Sensortec GmbH
- * Copyright (c) 2012  Unixphere AB
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
- */
-
-#ifndef _BMP085_H
-#define _BMP085_H
-
-#include <linux/regmap.h>
-
-#define BMP085_NAME		"bmp085"
-
-extern struct regmap_config bmp085_regmap_config;
-
-int bmp085_probe(struct device *dev, struct regmap *regmap, int irq);
-int bmp085_remove(struct device *dev);
-int bmp085_detect(struct device *dev);
-
-#endif
diff --git a/drivers/misc/cxl/api.c b/drivers/misc/cxl/api.c
index f3d34b941f85..af23d7dfe752 100644
--- a/drivers/misc/cxl/api.c
+++ b/drivers/misc/cxl/api.c
@@ -229,6 +229,14 @@ int cxl_start_context(struct cxl_context *ctx, u64 wed,
 	if (ctx->status == STARTED)
 		goto out; /* already started */
 
+	/*
+	 * Increment the mapped context count for adapter. This also checks
+	 * if adapter_context_lock is taken.
+	 */
+	rc = cxl_adapter_context_get(ctx->afu->adapter);
+	if (rc)
+		goto out;
+
 	if (task) {
 		ctx->pid = get_task_pid(task, PIDTYPE_PID);
 		ctx->glpid = get_task_pid(task->group_leader, PIDTYPE_PID);
@@ -240,6 +248,7 @@ int cxl_start_context(struct cxl_context *ctx, u64 wed,
 
 	if ((rc = cxl_ops->attach_process(ctx, kernel, wed, 0))) {
 		put_pid(ctx->pid);
+		cxl_adapter_context_put(ctx->afu->adapter);
 		cxl_ctx_put();
 		goto out;
 	}
diff --git a/drivers/misc/cxl/context.c b/drivers/misc/cxl/context.c
index c466ee2b0c97..5e506c19108a 100644
--- a/drivers/misc/cxl/context.c
+++ b/drivers/misc/cxl/context.c
@@ -238,6 +238,9 @@ int __detach_context(struct cxl_context *ctx)
 	put_pid(ctx->glpid);
 
 	cxl_ctx_put();
+
+	/* Decrease the attached context count on the adapter */
+	cxl_adapter_context_put(ctx->afu->adapter);
 	return 0;
 }
 
diff --git a/drivers/misc/cxl/cxl.h b/drivers/misc/cxl/cxl.h
index 344a0ff8f8c7..a144073593fa 100644
--- a/drivers/misc/cxl/cxl.h
+++ b/drivers/misc/cxl/cxl.h
@@ -162,7 +162,10 @@ static const cxl_p2n_reg_t CXL_PSL_WED_An     = {0x0A0};
 #define CXL_PSL_SPAP_V    0x0000000000000001ULL
 
 /****** CXL_PSL_Control ****************************************************/
-#define CXL_PSL_Control_tb 0x0000000000000001ULL
+#define CXL_PSL_Control_tb              (0x1ull << (63-63))
+#define CXL_PSL_Control_Fr              (0x1ull << (63-31))
+#define CXL_PSL_Control_Fs_MASK         (0x3ull << (63-29))
+#define CXL_PSL_Control_Fs_Complete     (0x3ull << (63-29))
 
 /****** CXL_PSL_DLCNTL *****************************************************/
 #define CXL_PSL_DLCNTL_D (0x1ull << (63-28))
@@ -615,6 +618,14 @@ struct cxl {
 	bool perst_select_user;
 	bool perst_same_image;
 	bool psl_timebase_synced;
+
+	/*
+	 * number of contexts mapped on to this card. Possible values are:
+	 * >0: Number of contexts mapped and new one can be mapped.
+	 *  0: No active contexts and new ones can be mapped.
+	 * -1: No contexts mapped and new ones cannot be mapped.
+	 */
+	atomic_t contexts_num;
 };
 
 int cxl_pci_alloc_one_irq(struct cxl *adapter);
@@ -854,6 +865,7 @@ int cxl_register_one_irq(struct cxl *adapter, irq_handler_t handler,
 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_data_cache_flush(struct cxl *adapter);
 int cxl_afu_disable(struct cxl_afu *afu);
 int cxl_psl_purge(struct cxl_afu *afu);
 
@@ -940,4 +952,20 @@ bool cxl_pci_is_vphb_device(struct pci_dev *dev);
 
 /* decode AFU error bits in the PSL register PSL_SERR_An */
 void cxl_afu_decode_psl_serr(struct cxl_afu *afu, u64 serr);
+
+/*
+ * Increments the number of attached contexts on an adapter.
+ * In case an adapter_context_lock is taken the return -EBUSY.
+ */
+int cxl_adapter_context_get(struct cxl *adapter);
+
+/* Decrements the number of attached contexts on an adapter */
+void cxl_adapter_context_put(struct cxl *adapter);
+
+/* If no active contexts then prevents contexts from being attached */
+int cxl_adapter_context_lock(struct cxl *adapter);
+
+/* Unlock the contexts-lock if taken. Warn and force unlock otherwise */
+void cxl_adapter_context_unlock(struct cxl *adapter);
+
 #endif
diff --git a/drivers/misc/cxl/file.c b/drivers/misc/cxl/file.c
index 5fb9894b157f..d0b421f49b39 100644
--- a/drivers/misc/cxl/file.c
+++ b/drivers/misc/cxl/file.c
@@ -205,11 +205,22 @@ static long afu_ioctl_start_work(struct cxl_context *ctx,
 	ctx->pid = get_task_pid(current, PIDTYPE_PID);
 	ctx->glpid = get_task_pid(current->group_leader, PIDTYPE_PID);
 
+	/*
+	 * Increment the mapped context count for adapter. This also checks
+	 * if adapter_context_lock is taken.
+	 */
+	rc = cxl_adapter_context_get(ctx->afu->adapter);
+	if (rc) {
+		afu_release_irqs(ctx, ctx);
+		goto out;
+	}
+
 	trace_cxl_attach(ctx, work.work_element_descriptor, work.num_interrupts, amr);
 
 	if ((rc = cxl_ops->attach_process(ctx, false, work.work_element_descriptor,
 							amr))) {
 		afu_release_irqs(ctx, ctx);
+		cxl_adapter_context_put(ctx->afu->adapter);
 		goto out;
 	}
 
diff --git a/drivers/misc/cxl/guest.c b/drivers/misc/cxl/guest.c
index 9aa58a77a24d..3e102cd6ed91 100644
--- a/drivers/misc/cxl/guest.c
+++ b/drivers/misc/cxl/guest.c
@@ -1152,6 +1152,9 @@ struct cxl *cxl_guest_init_adapter(struct device_node *np, struct platform_devic
 	if ((rc = cxl_sysfs_adapter_add(adapter)))
 		goto err_put1;
 
+	/* release the context lock as the adapter is configured */
+	cxl_adapter_context_unlock(adapter);
+
 	return adapter;
 
 err_put1:
diff --git a/drivers/misc/cxl/main.c b/drivers/misc/cxl/main.c
index d9be23b24aa3..62e0dfb5f15b 100644
--- a/drivers/misc/cxl/main.c
+++ b/drivers/misc/cxl/main.c
@@ -243,8 +243,10 @@ struct cxl *cxl_alloc_adapter(void)
 	if (dev_set_name(&adapter->dev, "card%i", adapter->adapter_num))
 		goto err2;
 
-	return adapter;
+	/* start with context lock taken */
+	atomic_set(&adapter->contexts_num, -1);
 
+	return adapter;
 err2:
 	cxl_remove_adapter_nr(adapter);
 err1:
@@ -286,6 +288,44 @@ int cxl_afu_select_best_mode(struct cxl_afu *afu)
 	return 0;
 }
 
+int cxl_adapter_context_get(struct cxl *adapter)
+{
+	int rc;
+
+	rc = atomic_inc_unless_negative(&adapter->contexts_num);
+	return rc >= 0 ? 0 : -EBUSY;
+}
+
+void cxl_adapter_context_put(struct cxl *adapter)
+{
+	atomic_dec_if_positive(&adapter->contexts_num);
+}
+
+int cxl_adapter_context_lock(struct cxl *adapter)
+{
+	int rc;
+	/* no active contexts -> contexts_num == 0 */
+	rc = atomic_cmpxchg(&adapter->contexts_num, 0, -1);
+	return rc ? -EBUSY : 0;
+}
+
+void cxl_adapter_context_unlock(struct cxl *adapter)
+{
+	int val = atomic_cmpxchg(&adapter->contexts_num, -1, 0);
+
+	/*
+	 * contexts lock taken -> contexts_num == -1
+	 * If not true then show a warning and force reset the lock.
+	 * This will happen when context_unlock was requested without
+	 * doing a context_lock.
+	 */
+	if (val != -1) {
+		atomic_set(&adapter->contexts_num, 0);
+		WARN(1, "Adapter context unlocked with %d active contexts",
+		     val);
+	}
+}
+
 static int __init init_cxl(void)
 {
 	int rc = 0;
diff --git a/drivers/misc/cxl/native.c b/drivers/misc/cxl/native.c
index e606fdc4bc9c..a217a74ccc98 100644
--- a/drivers/misc/cxl/native.c
+++ b/drivers/misc/cxl/native.c
@@ -290,6 +290,37 @@ int cxl_tlb_slb_invalidate(struct cxl *adapter)
 	return 0;
 }
 
+int cxl_data_cache_flush(struct cxl *adapter)
+{
+	u64 reg;
+	unsigned long timeout = jiffies + (HZ * CXL_TIMEOUT);
+
+	pr_devel("Flushing data cache\n");
+
+	reg = cxl_p1_read(adapter, CXL_PSL_Control);
+	reg |= CXL_PSL_Control_Fr;
+	cxl_p1_write(adapter, CXL_PSL_Control, reg);
+
+	reg = cxl_p1_read(adapter, CXL_PSL_Control);
+	while ((reg & CXL_PSL_Control_Fs_MASK) != CXL_PSL_Control_Fs_Complete) {
+		if (time_after_eq(jiffies, timeout)) {
+			dev_warn(&adapter->dev, "WARNING: cache flush timed out!\n");
+			return -EBUSY;
+		}
+
+		if (!cxl_ops->link_ok(adapter, NULL)) {
+			dev_warn(&adapter->dev, "WARNING: link down when flushing cache\n");
+			return -EIO;
+		}
+		cpu_relax();
+		reg = cxl_p1_read(adapter, CXL_PSL_Control);
+	}
+
+	reg &= ~CXL_PSL_Control_Fr;
+	cxl_p1_write(adapter, CXL_PSL_Control, reg);
+	return 0;
+}
+
 static int cxl_write_sstp(struct cxl_afu *afu, u64 sstp0, u64 sstp1)
 {
 	int rc;
diff --git a/drivers/misc/cxl/of.c b/drivers/misc/cxl/of.c
index edc458395f68..ec175ea5dfba 100644
--- a/drivers/misc/cxl/of.c
+++ b/drivers/misc/cxl/of.c
@@ -460,7 +460,7 @@ int cxl_of_probe(struct platform_device *pdev)
 	struct device_node *afu_np = NULL;
 	struct cxl *adapter = NULL;
 	int ret;
-	int slice, slice_ok;
+	int slice = 0, slice_ok = 0;
 
 	pr_devel("in %s\n", __func__);
 
@@ -476,13 +476,13 @@ int cxl_of_probe(struct platform_device *pdev)
 	}
 
 	/* init afu */
-	slice_ok = 0;
-	for (afu_np = NULL, slice = 0; (afu_np = of_get_next_child(np, afu_np)); slice++) {
+	for_each_child_of_node(np, afu_np) {
 		if ((ret = cxl_guest_init_afu(adapter, slice, afu_np)))
 			dev_err(&pdev->dev, "AFU %i failed to initialise: %i\n",
 				slice, ret);
 		else
 			slice_ok++;
+		slice++;
 	}
 
 	if (slice_ok == 0) {
@@ -490,8 +490,6 @@ int cxl_of_probe(struct platform_device *pdev)
 		adapter->slices = 0;
 	}
 
-	if (afu_np)
-		of_node_put(afu_np);
 	return 0;
 }
 
diff --git a/drivers/misc/cxl/pci.c b/drivers/misc/cxl/pci.c
index 6f0c4ac4b649..e96be9ca4e60 100644
--- a/drivers/misc/cxl/pci.c
+++ b/drivers/misc/cxl/pci.c
@@ -1239,6 +1239,9 @@ int cxl_pci_reset(struct cxl *adapter)
 
 	dev_info(&dev->dev, "CXL reset\n");
 
+	/* the adapter is about to be reset, so ignore errors */
+	cxl_data_cache_flush(adapter);
+
 	/* pcie_warm_reset requests a fundamental pci reset which includes a
 	 * PERST assert/deassert.  PERST triggers a loading of the image
 	 * if "user" or "factory" is selected in sysfs */
@@ -1484,6 +1487,8 @@ static int cxl_configure_adapter(struct cxl *adapter, struct pci_dev *dev)
 	if ((rc = cxl_native_register_psl_err_irq(adapter)))
 		goto err;
 
+	/* Release the context lock as adapter is configured */
+	cxl_adapter_context_unlock(adapter);
 	return 0;
 
 err:
@@ -1530,11 +1535,11 @@ static void set_sl_ops(struct cxl *adapter, struct pci_dev *dev)
 {
 	if (dev->vendor == PCI_VENDOR_ID_MELLANOX && dev->device == 0x1013) {
 		/* Mellanox CX-4 */
-		dev_info(&adapter->dev, "Device uses an XSL\n");
+		dev_info(&dev->dev, "Device uses an XSL\n");
 		adapter->native->sl_ops = &xsl_ops;
 		adapter->min_pe = 1; /* Workaround for CX-4 hardware bug */
 	} else {
-		dev_info(&adapter->dev, "Device uses a PSL\n");
+		dev_info(&dev->dev, "Device uses a PSL\n");
 		adapter->native->sl_ops = &psl_ops;
 	}
 }
diff --git a/drivers/misc/cxl/sysfs.c b/drivers/misc/cxl/sysfs.c
index b043c20f158f..a8b6d6a635e9 100644
--- a/drivers/misc/cxl/sysfs.c
+++ b/drivers/misc/cxl/sysfs.c
@@ -75,12 +75,31 @@ static ssize_t reset_adapter_store(struct device *device,
 	int val;
 
 	rc = sscanf(buf, "%i", &val);
-	if ((rc != 1) || (val != 1))
+	if ((rc != 1) || (val != 1 && val != -1))
 		return -EINVAL;
 
-	if ((rc = cxl_ops->adapter_reset(adapter)))
-		return rc;
-	return count;
+	/*
+	 * See if we can lock the context mapping that's only allowed
+	 * when there are no contexts attached to the adapter. Once
+	 * taken this will also prevent any context from getting activated.
+	 */
+	if (val == 1) {
+		rc =  cxl_adapter_context_lock(adapter);
+		if (rc)
+			goto out;
+
+		rc = cxl_ops->adapter_reset(adapter);
+		/* In case reset failed release context lock */
+		if (rc)
+			cxl_adapter_context_unlock(adapter);
+
+	} else if (val == -1) {
+		/* Perform a forced adapter reset */
+		rc = cxl_ops->adapter_reset(adapter);
+	}
+
+out:
+	return rc ? rc : count;
 }
 
 static ssize_t load_image_on_perst_show(struct device *device,
diff --git a/drivers/misc/eeprom/at24.c b/drivers/misc/eeprom/at24.c
index 3cdf8e1ca0ad..051b14766ef9 100644
--- a/drivers/misc/eeprom/at24.c
+++ b/drivers/misc/eeprom/at24.c
@@ -593,6 +593,7 @@ static int at24_probe(struct i2c_client *client, const struct i2c_device_id *id)
 	struct at24_data *at24;
 	int err;
 	unsigned i, num_addresses;
+	u8 test_byte;
 
 	if (client->dev.platform_data) {
 		chip = *(struct at24_platform_data *)client->dev.platform_data;
@@ -743,6 +744,18 @@ static int at24_probe(struct i2c_client *client, const struct i2c_device_id *id)
 		}
 	}
 
+	i2c_set_clientdata(client, at24);
+
+	/*
+	 * Perform a one-byte test read to verify that the
+	 * chip is functional.
+	 */
+	err = at24_read(at24, 0, &test_byte, 1);
+	if (err) {
+		err = -ENODEV;
+		goto err_clients;
+	}
+
 	at24->nvmem_config.name = dev_name(&client->dev);
 	at24->nvmem_config.dev = &client->dev;
 	at24->nvmem_config.read_only = !writable;
@@ -764,8 +777,6 @@ static int at24_probe(struct i2c_client *client, const struct i2c_device_id *id)
 		goto err_clients;
 	}
 
-	i2c_set_clientdata(client, at24);
-
 	dev_info(&client->dev, "%u byte %s EEPROM, %s, %u bytes/write\n",
 		chip.byte_len, client->name,
 		writable ? "writable" : "read-only", at24->write_max);
diff --git a/drivers/misc/eeprom/at25.c b/drivers/misc/eeprom/at25.c
index 2c6c7c8e3ead..5afe4cd16569 100644
--- a/drivers/misc/eeprom/at25.c
+++ b/drivers/misc/eeprom/at25.c
@@ -121,9 +121,8 @@ static int at25_ee_read(void *priv, unsigned int offset,
 	 * this chip is clocked very slowly
 	 */
 	status = spi_sync(at25->spi, &m);
-	dev_dbg(&at25->spi->dev,
-		"read %Zd bytes at %d --> %d\n",
-		count, offset, (int) status);
+	dev_dbg(&at25->spi->dev, "read %zu bytes at %d --> %zd\n",
+		count, offset, status);
 
 	mutex_unlock(&at25->lock);
 	return status;
@@ -167,8 +166,7 @@ static int at25_ee_write(void *priv, unsigned int off, void *val, size_t count)
 		*cp = AT25_WREN;
 		status = spi_write(at25->spi, cp, 1);
 		if (status < 0) {
-			dev_dbg(&at25->spi->dev, "WREN --> %d\n",
-					(int) status);
+			dev_dbg(&at25->spi->dev, "WREN --> %d\n", status);
 			break;
 		}
 
@@ -196,9 +194,8 @@ static int at25_ee_write(void *priv, unsigned int off, void *val, size_t count)
 		memcpy(cp, buf, segment);
 		status = spi_write(at25->spi, bounce,
 				segment + at25->addrlen + 1);
-		dev_dbg(&at25->spi->dev,
-				"write %u bytes at %u --> %d\n",
-				segment, offset, (int) status);
+		dev_dbg(&at25->spi->dev, "write %u bytes at %u --> %d\n",
+			segment, offset, status);
 		if (status < 0)
 			break;
 
@@ -225,8 +222,7 @@ static int at25_ee_write(void *priv, unsigned int off, void *val, size_t count)
 
 		if ((sr < 0) || (sr & AT25_SR_nRDY)) {
 			dev_err(&at25->spi->dev,
-				"write %d bytes offset %d, "
-				"timeout after %u msecs\n",
+				"write %u bytes offset %u, timeout after %u msecs\n",
 				segment, offset,
 				jiffies_to_msecs(jiffies -
 					(timeout - EE_TIMEOUT)));
@@ -368,9 +364,7 @@ static int at25_probe(struct spi_device *spi)
 		return PTR_ERR(at25->nvmem);
 
 	dev_info(&spi->dev, "%d %s %s eeprom%s, pagesize %u\n",
-		(chip.byte_len < 1024)
-			? chip.byte_len
-			: (chip.byte_len / 1024),
+		(chip.byte_len < 1024) ? chip.byte_len : (chip.byte_len / 1024),
 		(chip.byte_len < 1024) ? "Byte" : "KByte",
 		at25->chip.name,
 		(chip.flags & EE_READONLY) ? " (readonly)" : "",
diff --git a/drivers/misc/genwqe/card_base.c b/drivers/misc/genwqe/card_base.c
index a70b853fa2c9..6c1f49a85023 100644
--- a/drivers/misc/genwqe/card_base.c
+++ b/drivers/misc/genwqe/card_base.c
@@ -1351,6 +1351,19 @@ static struct pci_driver genwqe_driver = {
 };
 
 /**
+ * genwqe_devnode() - Set default access mode for genwqe devices.
+ *
+ * Default mode should be rw for everybody. Do not change default
+ * device name.
+ */
+static char *genwqe_devnode(struct device *dev, umode_t *mode)
+{
+	if (mode)
+		*mode = 0666;
+	return NULL;
+}
+
+/**
  * genwqe_init_module() - Driver registration and initialization
  */
 static int __init genwqe_init_module(void)
@@ -1363,6 +1376,8 @@ static int __init genwqe_init_module(void)
 		return -ENOMEM;
 	}
 
+	class_genwqe->devnode = genwqe_devnode;
+
 	debugfs_genwqe = debugfs_create_dir(GENWQE_DEVNAME, NULL);
 	if (!debugfs_genwqe) {
 		rc = -ENOMEM;
diff --git a/drivers/misc/genwqe/card_ddcb.c b/drivers/misc/genwqe/card_ddcb.c
index 353ee0cc733d..ddfeefe39540 100644
--- a/drivers/misc/genwqe/card_ddcb.c
+++ b/drivers/misc/genwqe/card_ddcb.c
@@ -1048,8 +1048,6 @@ static int setup_ddcb_queue(struct genwqe_dev *cd, struct ddcb_queue *queue)
 			"[%s] **err: could not allocate DDCB **\n", __func__);
 		return -ENOMEM;
 	}
-	memset(queue->ddcb_vaddr, 0, queue_size);
-
 	queue->ddcb_req = kzalloc(sizeof(struct ddcb_requ *) *
 				  queue->ddcb_max, GFP_KERNEL);
 	if (!queue->ddcb_req) {
diff --git a/drivers/misc/genwqe/card_utils.c b/drivers/misc/genwqe/card_utils.c
index 222367cc8c81..8a679ecc8fd1 100644
--- a/drivers/misc/genwqe/card_utils.c
+++ b/drivers/misc/genwqe/card_utils.c
@@ -220,8 +220,8 @@ void *__genwqe_alloc_consistent(struct genwqe_dev *cd, size_t size,
 	if (get_order(size) > MAX_ORDER)
 		return NULL;
 
-	return dma_alloc_coherent(&cd->pci_dev->dev, size, dma_handle,
-				  GFP_KERNEL);
+	return dma_zalloc_coherent(&cd->pci_dev->dev, size, dma_handle,
+				   GFP_KERNEL);
 }
 
 void __genwqe_free_consistent(struct genwqe_dev *cd, size_t size,
diff --git a/drivers/misc/hpilo.c b/drivers/misc/hpilo.c
index d6a901cd4222..fea8ff40440f 100644
--- a/drivers/misc/hpilo.c
+++ b/drivers/misc/hpilo.c
@@ -688,7 +688,8 @@ static void ilo_unmap_device(struct pci_dev *pdev, struct ilo_hwinfo *hw)
 
 static int ilo_map_device(struct pci_dev *pdev, struct ilo_hwinfo *hw)
 {
-	int error = -ENOMEM;
+	int bar;
+	unsigned long off;
 
 	/* map the memory mapped i/o registers */
 	hw->mmio_vaddr = pci_iomap(pdev, 1, 0);
@@ -698,7 +699,15 @@ static int ilo_map_device(struct pci_dev *pdev, struct ilo_hwinfo *hw)
 	}
 
 	/* map the adapter shared memory region */
-	hw->ram_vaddr = pci_iomap(pdev, 2, max_ccb * ILOHW_CCB_SZ);
+	if (pdev->subsystem_device == 0x00E4) {
+		bar = 5;
+		/* Last 8k is reserved for CCBs */
+		off = pci_resource_len(pdev, bar) - 0x2000;
+	} else {
+		bar = 2;
+		off = 0;
+	}
+	hw->ram_vaddr = pci_iomap_range(pdev, bar, off, max_ccb * ILOHW_CCB_SZ);
 	if (hw->ram_vaddr == NULL) {
 		dev_err(&pdev->dev, "Error mapping shared mem\n");
 		goto mmio_free;
@@ -717,7 +726,7 @@ ram_free:
 mmio_free:
 	pci_iounmap(pdev, hw->mmio_vaddr);
 out:
-	return error;
+	return -ENOMEM;
 }
 
 static void ilo_remove(struct pci_dev *pdev)
@@ -899,7 +908,7 @@ static void __exit ilo_exit(void)
 	class_destroy(ilo_class);
 }
 
-MODULE_VERSION("1.4.1");
+MODULE_VERSION("1.5.0");
 MODULE_ALIAS(ILO_NAME);
 MODULE_DESCRIPTION(ILO_NAME);
 MODULE_AUTHOR("David Altobelli <david.altobelli@hpe.com>");
diff --git a/drivers/misc/ibmasm/ibmasmfs.c b/drivers/misc/ibmasm/ibmasmfs.c
index 9c677f3f3c26..520f58439080 100644
--- a/drivers/misc/ibmasm/ibmasmfs.c
+++ b/drivers/misc/ibmasm/ibmasmfs.c
@@ -144,7 +144,7 @@ static struct inode *ibmasmfs_make_inode(struct super_block *sb, int mode)
 	if (ret) {
 		ret->i_ino = get_next_ino();
 		ret->i_mode = mode;
-		ret->i_atime = ret->i_mtime = ret->i_ctime = CURRENT_TIME;
+		ret->i_atime = ret->i_mtime = ret->i_ctime = current_time(ret);
 	}
 	return ret;
 }
diff --git a/drivers/misc/mei/amthif.c b/drivers/misc/mei/amthif.c
index a039a5df6f21..7ae89b4a21d5 100644
--- a/drivers/misc/mei/amthif.c
+++ b/drivers/misc/mei/amthif.c
@@ -47,7 +47,6 @@ const uuid_le mei_amthif_guid  = UUID_LE(0x12f80028, 0xb4b7, 0x4b2d,
 void mei_amthif_reset_params(struct mei_device *dev)
 {
 	/* reset iamthif parameters. */
-	dev->iamthif_current_cb = NULL;
 	dev->iamthif_canceled = false;
 	dev->iamthif_state = MEI_IAMTHIF_IDLE;
 	dev->iamthif_stall_timer = 0;
@@ -67,8 +66,12 @@ int mei_amthif_host_init(struct mei_device *dev, struct mei_me_client *me_cl)
 	struct mei_cl *cl = &dev->iamthif_cl;
 	int ret;
 
-	if (mei_cl_is_connected(cl))
-		return 0;
+	mutex_lock(&dev->device_lock);
+
+	if (mei_cl_is_connected(cl)) {
+		ret = 0;
+		goto out;
+	}
 
 	dev->iamthif_state = MEI_IAMTHIF_IDLE;
 
@@ -77,179 +80,37 @@ int mei_amthif_host_init(struct mei_device *dev, struct mei_me_client *me_cl)
 	ret = mei_cl_link(cl);
 	if (ret < 0) {
 		dev_err(dev->dev, "amthif: failed cl_link %d\n", ret);
-		return ret;
+		goto out;
 	}
 
 	ret = mei_cl_connect(cl, me_cl, NULL);
 
-	return ret;
-}
-
-/**
- * mei_amthif_read - read data from AMTHIF client
- *
- * @dev: the device structure
- * @file: pointer to file object
- * @ubuf: pointer to user data in user space
- * @length: data length to read
- * @offset: data read offset
- *
- * Locking: called under "dev->device_lock" lock
- *
- * Return:
- *  returned data length on success,
- *  zero if no data to read,
- *  negative on failure.
- */
-int mei_amthif_read(struct mei_device *dev, struct file *file,
-	       char __user *ubuf, size_t length, loff_t *offset)
-{
-	struct mei_cl *cl = file->private_data;
-	struct mei_cl_cb *cb;
-	int rets;
-	int wait_ret;
-
-	dev_dbg(dev->dev, "checking amthif data\n");
-	cb = mei_cl_read_cb(cl, file);
-
-	/* Check for if we can block or not*/
-	if (cb == NULL && file->f_flags & O_NONBLOCK)
-		return -EAGAIN;
-
-
-	dev_dbg(dev->dev, "waiting for amthif data\n");
-	while (cb == NULL) {
-		/* unlock the Mutex */
-		mutex_unlock(&dev->device_lock);
-
-		wait_ret = wait_event_interruptible(cl->rx_wait,
-					!list_empty(&cl->rd_completed) ||
-					!mei_cl_is_connected(cl));
-
-		/* Locking again the Mutex */
-		mutex_lock(&dev->device_lock);
-
-		if (wait_ret)
-			return -ERESTARTSYS;
-
-		if (!mei_cl_is_connected(cl)) {
-			rets = -EBUSY;
-			goto out;
-		}
-
-		cb = mei_cl_read_cb(cl, file);
-	}
-
-	if (cb->status) {
-		rets = cb->status;
-		dev_dbg(dev->dev, "read operation failed %d\n", rets);
-		goto free;
-	}
-
-	dev_dbg(dev->dev, "Got amthif data\n");
-	/* if the whole message will fit remove it from the list */
-	if (cb->buf_idx >= *offset && length >= (cb->buf_idx - *offset))
-		list_del_init(&cb->list);
-	else if (cb->buf_idx <= *offset) {
-		/* end of the message has been reached */
-		list_del_init(&cb->list);
-		rets = 0;
-		goto free;
-	}
-		/* else means that not full buffer will be read and do not
-		 * remove message from deletion list
-		 */
-
-	dev_dbg(dev->dev, "amthif cb->buf.size - %zu cb->buf_idx - %zu\n",
-		cb->buf.size, cb->buf_idx);
-
-	/* length is being truncated to PAGE_SIZE, however,
-	 * the buf_idx may point beyond */
-	length = min_t(size_t, length, (cb->buf_idx - *offset));
-
-	if (copy_to_user(ubuf, cb->buf.data + *offset, length)) {
-		dev_dbg(dev->dev, "failed to copy data to userland\n");
-		rets = -EFAULT;
-	} else {
-		rets = length;
-		if ((*offset + length) < cb->buf_idx) {
-			*offset += length;
-			goto out;
-		}
-	}
-free:
-	dev_dbg(dev->dev, "free amthif cb memory.\n");
-	*offset = 0;
-	mei_io_cb_free(cb);
 out:
-	return rets;
+	mutex_unlock(&dev->device_lock);
+	return ret;
 }
 
 /**
  * mei_amthif_read_start - queue message for sending read credential
  *
  * @cl: host client
- * @file: file pointer of message recipient
+ * @fp: file pointer of message recipient
  *
  * Return: 0 on success, <0 on failure.
  */
-static int mei_amthif_read_start(struct mei_cl *cl, const struct file *file)
+static int mei_amthif_read_start(struct mei_cl *cl, const struct file *fp)
 {
 	struct mei_device *dev = cl->dev;
 	struct mei_cl_cb *cb;
-	int rets;
-
-	cb = mei_io_cb_init(cl, MEI_FOP_READ, file);
-	if (!cb) {
-		rets = -ENOMEM;
-		goto err;
-	}
 
-	rets = mei_io_cb_alloc_buf(cb, mei_cl_mtu(cl));
-	if (rets)
-		goto err;
+	cb = mei_cl_enqueue_ctrl_wr_cb(cl, mei_cl_mtu(cl), MEI_FOP_READ, fp);
+	if (!cb)
+		return -ENOMEM;
 
-	list_add_tail(&cb->list, &dev->ctrl_wr_list.list);
+	cl->rx_flow_ctrl_creds++;
 
 	dev->iamthif_state = MEI_IAMTHIF_READING;
-	dev->iamthif_fp = cb->fp;
-	dev->iamthif_current_cb = cb;
-
-	return 0;
-err:
-	mei_io_cb_free(cb);
-	return rets;
-}
-
-/**
- * mei_amthif_send_cmd - send amthif command to the ME
- *
- * @cl: the host client
- * @cb: mei call back struct
- *
- * Return: 0 on success, <0 on failure.
- */
-static int mei_amthif_send_cmd(struct mei_cl *cl, struct mei_cl_cb *cb)
-{
-	struct mei_device *dev;
-	int ret;
-
-	if (!cl->dev || !cb)
-		return -ENODEV;
-
-	dev = cl->dev;
-
-	dev->iamthif_state = MEI_IAMTHIF_WRITING;
-	dev->iamthif_current_cb = cb;
-	dev->iamthif_fp = cb->fp;
-	dev->iamthif_canceled = false;
-
-	ret = mei_cl_write(cl, cb, false);
-	if (ret < 0)
-		return ret;
-
-	if (cb->completed)
-		cb->status = mei_amthif_read_start(cl, cb->fp);
+	cl->fp = cb->fp;
 
 	return 0;
 }
@@ -265,20 +126,32 @@ int mei_amthif_run_next_cmd(struct mei_device *dev)
 {
 	struct mei_cl *cl = &dev->iamthif_cl;
 	struct mei_cl_cb *cb;
+	int ret;
 
 	dev->iamthif_canceled = false;
-	dev->iamthif_state = MEI_IAMTHIF_IDLE;
-	dev->iamthif_fp = NULL;
 
 	dev_dbg(dev->dev, "complete amthif cmd_list cb.\n");
 
 	cb = list_first_entry_or_null(&dev->amthif_cmd_list.list,
 					typeof(*cb), list);
-	if (!cb)
+	if (!cb) {
+		dev->iamthif_state = MEI_IAMTHIF_IDLE;
+		cl->fp = NULL;
 		return 0;
+	}
 
 	list_del_init(&cb->list);
-	return mei_amthif_send_cmd(cl, cb);
+	dev->iamthif_state = MEI_IAMTHIF_WRITING;
+	cl->fp = cb->fp;
+
+	ret = mei_cl_write(cl, cb, false);
+	if (ret < 0)
+		return ret;
+
+	if (cb->completed)
+		cb->status = mei_amthif_read_start(cl, cb->fp);
+
+	return 0;
 }
 
 /**
@@ -299,8 +172,7 @@ int mei_amthif_write(struct mei_cl *cl, struct mei_cl_cb *cb)
 	/*
 	 * The previous request is still in processing, queue this one.
 	 */
-	if (dev->iamthif_state > MEI_IAMTHIF_IDLE &&
-	    dev->iamthif_state < MEI_IAMTHIF_READ_COMPLETE)
+	if (dev->iamthif_state != MEI_IAMTHIF_IDLE)
 		return 0;
 
 	return mei_amthif_run_next_cmd(dev);
@@ -309,7 +181,6 @@ int mei_amthif_write(struct mei_cl *cl, struct mei_cl_cb *cb)
 /**
  * mei_amthif_poll - the amthif poll function
  *
- * @dev: the device structure
  * @file: pointer to file structure
  * @wait: pointer to poll_table structure
  *
@@ -317,26 +188,19 @@ int mei_amthif_write(struct mei_cl *cl, struct mei_cl_cb *cb)
  *
  * Locking: called under "dev->device_lock" lock
  */
-
-unsigned int mei_amthif_poll(struct mei_device *dev,
-		struct file *file, poll_table *wait)
+unsigned int mei_amthif_poll(struct file *file, poll_table *wait)
 {
+	struct mei_cl *cl = file->private_data;
+	struct mei_cl_cb *cb = mei_cl_read_cb(cl, file);
 	unsigned int mask = 0;
 
-	poll_wait(file, &dev->iamthif_cl.rx_wait, wait);
-
-	if (dev->iamthif_state == MEI_IAMTHIF_READ_COMPLETE &&
-	    dev->iamthif_fp == file) {
-
+	poll_wait(file, &cl->rx_wait, wait);
+	if (cb)
 		mask |= POLLIN | POLLRDNORM;
-		mei_amthif_run_next_cmd(dev);
-	}
 
 	return mask;
 }
 
-
-
 /**
  * mei_amthif_irq_write - write iamthif command in irq thread context.
  *
@@ -393,7 +257,6 @@ int mei_amthif_irq_read_msg(struct mei_cl *cl,
 		return 0;
 
 	dev_dbg(dev->dev, "completed amthif read.\n ");
-	dev->iamthif_current_cb = NULL;
 	dev->iamthif_stall_timer = 0;
 
 	return 0;
@@ -409,115 +272,63 @@ void mei_amthif_complete(struct mei_cl *cl, struct mei_cl_cb *cb)
 {
 	struct mei_device *dev = cl->dev;
 
-	if (cb->fop_type == MEI_FOP_WRITE) {
+	dev_dbg(dev->dev, "completing amthif call back.\n");
+	switch (cb->fop_type) {
+	case MEI_FOP_WRITE:
 		if (!cb->status) {
 			dev->iamthif_stall_timer = MEI_IAMTHIF_STALL_TIMER;
+			mei_schedule_stall_timer(dev);
 			mei_io_cb_free(cb);
 			return;
 		}
-		/*
-		 * in case of error enqueue the write cb to complete read list
-		 * so it can be propagated to the reader
-		 */
-		list_add_tail(&cb->list, &cl->rd_completed);
-		wake_up_interruptible(&cl->rx_wait);
-		return;
-	}
+		dev->iamthif_state = MEI_IAMTHIF_IDLE;
+		cl->fp = NULL;
+		if (!dev->iamthif_canceled) {
+			/*
+			 * in case of error enqueue the write cb to complete
+			 * read list so it can be propagated to the reader
+			 */
+			list_add_tail(&cb->list, &cl->rd_completed);
+			wake_up_interruptible(&cl->rx_wait);
+		} else {
+			mei_io_cb_free(cb);
+		}
+		break;
+	case MEI_FOP_READ:
+		if (!dev->iamthif_canceled) {
+			list_add_tail(&cb->list, &cl->rd_completed);
+			dev_dbg(dev->dev, "amthif read completed\n");
+			wake_up_interruptible(&cl->rx_wait);
+		} else {
+			mei_io_cb_free(cb);
+		}
 
-	if (!dev->iamthif_canceled) {
-		dev->iamthif_state = MEI_IAMTHIF_READ_COMPLETE;
 		dev->iamthif_stall_timer = 0;
-		list_add_tail(&cb->list, &cl->rd_completed);
-		dev_dbg(dev->dev, "amthif read completed\n");
-	} else {
 		mei_amthif_run_next_cmd(dev);
+		break;
+	default:
+		WARN_ON(1);
 	}
-
-	dev_dbg(dev->dev, "completing amthif call back.\n");
-	wake_up_interruptible(&cl->rx_wait);
 }
 
 /**
  * mei_clear_list - removes all callbacks associated with file
  *		from mei_cb_list
  *
- * @dev: device structure.
  * @file: file structure
  * @mei_cb_list: callbacks list
  *
  * mei_clear_list is called to clear resources associated with file
  * when application calls close function or Ctrl-C was pressed
- *
- * Return: true if callback removed from the list, false otherwise
  */
-static bool mei_clear_list(struct mei_device *dev,
-		const struct file *file, struct list_head *mei_cb_list)
+static void mei_clear_list(const struct file *file,
+			   struct list_head *mei_cb_list)
 {
-	struct mei_cl *cl = &dev->iamthif_cl;
 	struct mei_cl_cb *cb, *next;
-	bool removed = false;
-
-	/* list all list member */
-	list_for_each_entry_safe(cb, next, mei_cb_list, list) {
-		/* check if list member associated with a file */
-		if (file == cb->fp) {
-			/* check if cb equal to current iamthif cb */
-			if (dev->iamthif_current_cb == cb) {
-				dev->iamthif_current_cb = NULL;
-				/* send flow control to iamthif client */
-				mei_hbm_cl_flow_control_req(dev, cl);
-			}
-			/* free all allocated buffers */
-			mei_io_cb_free(cb);
-			removed = true;
-		}
-	}
-	return removed;
-}
 
-/**
- * mei_clear_lists - removes all callbacks associated with file
- *
- * @dev: device structure
- * @file: file structure
- *
- * mei_clear_lists is called to clear resources associated with file
- * when application calls close function or Ctrl-C was pressed
- *
- * Return: true if callback removed from the list, false otherwise
- */
-static bool mei_clear_lists(struct mei_device *dev, const struct file *file)
-{
-	bool removed = false;
-	struct mei_cl *cl = &dev->iamthif_cl;
-
-	/* remove callbacks associated with a file */
-	mei_clear_list(dev, file, &dev->amthif_cmd_list.list);
-	if (mei_clear_list(dev, file, &cl->rd_completed))
-		removed = true;
-
-	mei_clear_list(dev, file, &dev->ctrl_rd_list.list);
-
-	if (mei_clear_list(dev, file, &dev->ctrl_wr_list.list))
-		removed = true;
-
-	if (mei_clear_list(dev, file, &dev->write_waiting_list.list))
-		removed = true;
-
-	if (mei_clear_list(dev, file, &dev->write_list.list))
-		removed = true;
-
-	/* check if iamthif_current_cb not NULL */
-	if (dev->iamthif_current_cb && !removed) {
-		/* check file and iamthif current cb association */
-		if (dev->iamthif_current_cb->fp == file) {
-			/* remove cb */
-			mei_io_cb_free(dev->iamthif_current_cb);
-			dev->iamthif_current_cb = NULL;
-			removed = true;
-		}
-	}
-	return removed;
+	list_for_each_entry_safe(cb, next, mei_cb_list, list)
+		if (file == cb->fp)
+			mei_io_cb_free(cb);
 }
 
 /**
@@ -530,23 +341,21 @@ static bool mei_clear_lists(struct mei_device *dev, const struct file *file)
 */
 int mei_amthif_release(struct mei_device *dev, struct file *file)
 {
+	struct mei_cl *cl = file->private_data;
+
 	if (dev->iamthif_open_count > 0)
 		dev->iamthif_open_count--;
 
-	if (dev->iamthif_fp == file &&
-	    dev->iamthif_state != MEI_IAMTHIF_IDLE) {
+	if (cl->fp == file && dev->iamthif_state != MEI_IAMTHIF_IDLE) {
 
 		dev_dbg(dev->dev, "amthif canceled iamthif state %d\n",
 		    dev->iamthif_state);
 		dev->iamthif_canceled = true;
-		if (dev->iamthif_state == MEI_IAMTHIF_READ_COMPLETE) {
-			dev_dbg(dev->dev, "run next amthif iamthif cb\n");
-			mei_amthif_run_next_cmd(dev);
-		}
 	}
 
-	if (mei_clear_lists(dev, file))
-		dev->iamthif_state = MEI_IAMTHIF_IDLE;
+	mei_clear_list(file, &dev->amthif_cmd_list.list);
+	mei_clear_list(file, &cl->rd_completed);
+	mei_clear_list(file, &dev->ctrl_rd_list.list);
 
 	return 0;
 }
diff --git a/drivers/misc/mei/bus.c b/drivers/misc/mei/bus.c
index 1f33fea9299f..8cac7ef9ad0d 100644
--- a/drivers/misc/mei/bus.c
+++ b/drivers/misc/mei/bus.c
@@ -126,7 +126,8 @@ ssize_t __mei_cl_recv(struct mei_cl *cl, u8 *buf, size_t length)
 		goto out;
 
 	/* wait on event only if there is no other waiter */
-	if (list_empty(&cl->rd_completed) && !waitqueue_active(&cl->rx_wait)) {
+	/* synchronized under device mutex */
+	if (!waitqueue_active(&cl->rx_wait)) {
 
 		mutex_unlock(&bus->device_lock);
 
@@ -142,7 +143,7 @@ ssize_t __mei_cl_recv(struct mei_cl *cl, u8 *buf, size_t length)
 		mutex_lock(&bus->device_lock);
 
 		if (!mei_cl_is_connected(cl)) {
-			rets = -EBUSY;
+			rets = -ENODEV;
 			goto out;
 		}
 	}
@@ -234,7 +235,7 @@ static void mei_cl_bus_event_work(struct work_struct *work)
 	/* Prepare for the next read */
 	if (cldev->events_mask & BIT(MEI_CL_EVENT_RX)) {
 		mutex_lock(&bus->device_lock);
-		mei_cl_read_start(cldev->cl, 0, NULL);
+		mei_cl_read_start(cldev->cl, mei_cl_mtu(cldev->cl), NULL);
 		mutex_unlock(&bus->device_lock);
 	}
 }
@@ -324,7 +325,7 @@ int mei_cldev_register_event_cb(struct mei_cl_device *cldev,
 
 	if (cldev->events_mask & BIT(MEI_CL_EVENT_RX)) {
 		mutex_lock(&bus->device_lock);
-		ret = mei_cl_read_start(cldev->cl, 0, NULL);
+		ret = mei_cl_read_start(cldev->cl, mei_cl_mtu(cldev->cl), NULL);
 		mutex_unlock(&bus->device_lock);
 		if (ret && ret != -EBUSY)
 			return ret;
@@ -983,12 +984,10 @@ void mei_cl_bus_rescan_work(struct work_struct *work)
 		container_of(work, struct mei_device, bus_rescan_work);
 	struct mei_me_client *me_cl;
 
-	mutex_lock(&bus->device_lock);
 	me_cl = mei_me_cl_by_uuid(bus, &mei_amthif_guid);
 	if (me_cl)
 		mei_amthif_host_init(bus, me_cl);
 	mei_me_cl_put(me_cl);
-	mutex_unlock(&bus->device_lock);
 
 	mei_cl_bus_rescan(bus);
 }
diff --git a/drivers/misc/mei/client.c b/drivers/misc/mei/client.c
index 641c1a566687..6fe02350578d 100644
--- a/drivers/misc/mei/client.c
+++ b/drivers/misc/mei/client.c
@@ -358,8 +358,9 @@ void mei_io_cb_free(struct mei_cl_cb *cb)
  *
  * Return: mei_cl_cb pointer or NULL;
  */
-struct mei_cl_cb *mei_io_cb_init(struct mei_cl *cl, enum mei_cb_file_ops type,
-				 const struct file *fp)
+static struct mei_cl_cb *mei_io_cb_init(struct mei_cl *cl,
+					enum mei_cb_file_ops type,
+					const struct file *fp)
 {
 	struct mei_cl_cb *cb;
 
@@ -420,32 +421,41 @@ static inline void mei_io_list_free(struct mei_cl_cb *list, struct mei_cl *cl)
 }
 
 /**
- * mei_io_cb_alloc_buf - allocate callback buffer
+ * mei_cl_alloc_cb - a convenient wrapper for allocating read cb
  *
- * @cb: io callback structure
+ * @cl: host client
  * @length: size of the buffer
+ * @type: operation type
+ * @fp: associated file pointer (might be NULL)
  *
- * Return: 0 on success
- *         -EINVAL if cb is NULL
- *         -ENOMEM if allocation failed
+ * Return: cb on success and NULL on failure
  */
-int mei_io_cb_alloc_buf(struct mei_cl_cb *cb, size_t length)
+struct mei_cl_cb *mei_cl_alloc_cb(struct mei_cl *cl, size_t length,
+				  enum mei_cb_file_ops fop_type,
+				  const struct file *fp)
 {
+	struct mei_cl_cb *cb;
+
+	cb = mei_io_cb_init(cl, fop_type, fp);
 	if (!cb)
-		return -EINVAL;
+		return NULL;
 
 	if (length == 0)
-		return 0;
+		return cb;
 
 	cb->buf.data = kmalloc(length, GFP_KERNEL);
-	if (!cb->buf.data)
-		return -ENOMEM;
+	if (!cb->buf.data) {
+		mei_io_cb_free(cb);
+		return NULL;
+	}
 	cb->buf.size = length;
-	return 0;
+
+	return cb;
 }
 
 /**
- * mei_cl_alloc_cb - a convenient wrapper for allocating read cb
+ * mei_cl_enqueue_ctrl_wr_cb - a convenient wrapper for allocating
+ *     and enqueuing of the control commands cb
  *
  * @cl: host client
  * @length: size of the buffer
@@ -453,22 +463,23 @@ int mei_io_cb_alloc_buf(struct mei_cl_cb *cb, size_t length)
  * @fp: associated file pointer (might be NULL)
  *
  * Return: cb on success and NULL on failure
+ * Locking: called under "dev->device_lock" lock
  */
-struct mei_cl_cb *mei_cl_alloc_cb(struct mei_cl *cl, size_t length,
-				  enum mei_cb_file_ops type,
-				  const struct file *fp)
+struct mei_cl_cb *mei_cl_enqueue_ctrl_wr_cb(struct mei_cl *cl, size_t length,
+					    enum mei_cb_file_ops fop_type,
+					    const struct file *fp)
 {
 	struct mei_cl_cb *cb;
 
-	cb = mei_io_cb_init(cl, type, fp);
-	if (!cb)
-		return NULL;
+	/* for RX always allocate at least client's mtu */
+	if (length)
+		length = max_t(size_t, length, mei_cl_mtu(cl));
 
-	if (mei_io_cb_alloc_buf(cb, length)) {
-		mei_io_cb_free(cb);
+	cb = mei_cl_alloc_cb(cl, length, fop_type, fp);
+	if (!cb)
 		return NULL;
-	}
 
+	list_add_tail(&cb->list, &cl->dev->ctrl_wr_list.list);
 	return cb;
 }
 
@@ -754,7 +765,8 @@ void mei_cl_set_disconnected(struct mei_cl *cl)
 	mei_io_list_flush(&dev->ctrl_rd_list, cl);
 	mei_io_list_flush(&dev->ctrl_wr_list, cl);
 	mei_cl_wake_all(cl);
-	cl->mei_flow_ctrl_creds = 0;
+	cl->rx_flow_ctrl_creds = 0;
+	cl->tx_flow_ctrl_creds = 0;
 	cl->timer_count = 0;
 
 	if (!cl->me_cl)
@@ -764,7 +776,7 @@ void mei_cl_set_disconnected(struct mei_cl *cl)
 		cl->me_cl->connect_count--;
 
 	if (cl->me_cl->connect_count == 0)
-		cl->me_cl->mei_flow_ctrl_creds = 0;
+		cl->me_cl->tx_flow_ctrl_creds = 0;
 
 	mei_me_cl_put(cl->me_cl);
 	cl->me_cl = NULL;
@@ -814,6 +826,7 @@ static int mei_cl_send_disconnect(struct mei_cl *cl, struct mei_cl_cb *cb)
 
 	list_move_tail(&cb->list, &dev->ctrl_rd_list.list);
 	cl->timer_count = MEI_CONNECT_TIMEOUT;
+	mei_schedule_stall_timer(dev);
 
 	return 0;
 }
@@ -867,13 +880,11 @@ static int __mei_cl_disconnect(struct mei_cl *cl)
 
 	cl->state = MEI_FILE_DISCONNECTING;
 
-	cb = mei_io_cb_init(cl, MEI_FOP_DISCONNECT, NULL);
-	rets = cb ? 0 : -ENOMEM;
-	if (rets)
+	cb = mei_cl_enqueue_ctrl_wr_cb(cl, 0, MEI_FOP_DISCONNECT, NULL);
+	if (!cb) {
+		rets = -ENOMEM;
 		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)) {
 		rets = mei_cl_send_disconnect(cl, cb);
@@ -1001,6 +1012,7 @@ static int mei_cl_send_connect(struct mei_cl *cl, struct mei_cl_cb *cb)
 
 	list_move_tail(&cb->list, &dev->ctrl_rd_list.list);
 	cl->timer_count = MEI_CONNECT_TIMEOUT;
+	mei_schedule_stall_timer(dev);
 	return 0;
 }
 
@@ -1042,14 +1054,14 @@ int mei_cl_irq_connect(struct mei_cl *cl, struct mei_cl_cb *cb,
  *
  * @cl: host client
  * @me_cl: me client
- * @file: pointer to file structure
+ * @fp: 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 mei_me_client *me_cl,
-		  const struct file *file)
+		   const struct file *fp)
 {
 	struct mei_device *dev;
 	struct mei_cl_cb *cb;
@@ -1076,12 +1088,11 @@ int mei_cl_connect(struct mei_cl *cl, struct mei_me_client *me_cl,
 		goto nortpm;
 	}
 
-	cb = mei_io_cb_init(cl, MEI_FOP_CONNECT, file);
-	rets = cb ? 0 : -ENOMEM;
-	if (rets)
+	cb = mei_cl_enqueue_ctrl_wr_cb(cl, 0, MEI_FOP_CONNECT, fp);
+	if (!cb) {
+		rets = -ENOMEM;
 		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)) {
@@ -1159,50 +1170,42 @@ err:
 	return ERR_PTR(ret);
 }
 
-
-
 /**
- * mei_cl_flow_ctrl_creds - checks flow_control credits for cl.
+ * mei_cl_tx_flow_ctrl_creds - checks flow_control credits for cl.
  *
  * @cl: host client
- * @fp: the file pointer associated with the pointer
  *
- * Return: 1 if mei_flow_ctrl_creds >0, 0 - otherwise.
+ * Return: 1 if tx_flow_ctrl_creds >0, 0 - otherwise.
  */
-static int mei_cl_flow_ctrl_creds(struct mei_cl *cl, const struct file *fp)
+static int mei_cl_tx_flow_ctrl_creds(struct mei_cl *cl)
 {
-	int rets;
-
 	if (WARN_ON(!cl || !cl->me_cl))
 		return -EINVAL;
 
-	if (cl->mei_flow_ctrl_creds > 0)
+	if (cl->tx_flow_ctrl_creds > 0)
 		return 1;
 
-	if (mei_cl_is_fixed_address(cl)) {
-		rets = mei_cl_read_start(cl, mei_cl_mtu(cl), fp);
-		if (rets && rets != -EBUSY)
-			return rets;
+	if (mei_cl_is_fixed_address(cl))
 		return 1;
-	}
 
 	if (mei_cl_is_single_recv_buf(cl)) {
-		if (cl->me_cl->mei_flow_ctrl_creds > 0)
+		if (cl->me_cl->tx_flow_ctrl_creds > 0)
 			return 1;
 	}
 	return 0;
 }
 
 /**
- * mei_cl_flow_ctrl_reduce - reduces flow_control.
+ * mei_cl_tx_flow_ctrl_creds_reduce - reduces transmit flow control credits
+ *   for a client
  *
- * @cl: private data of the file object
+ * @cl: host client
  *
  * Return:
  *	0 on success
  *	-EINVAL when ctrl credits are <= 0
  */
-static int mei_cl_flow_ctrl_reduce(struct mei_cl *cl)
+static int mei_cl_tx_flow_ctrl_creds_reduce(struct mei_cl *cl)
 {
 	if (WARN_ON(!cl || !cl->me_cl))
 		return -EINVAL;
@@ -1211,13 +1214,13 @@ static int mei_cl_flow_ctrl_reduce(struct mei_cl *cl)
 		return 0;
 
 	if (mei_cl_is_single_recv_buf(cl)) {
-		if (WARN_ON(cl->me_cl->mei_flow_ctrl_creds <= 0))
+		if (WARN_ON(cl->me_cl->tx_flow_ctrl_creds <= 0))
 			return -EINVAL;
-		cl->me_cl->mei_flow_ctrl_creds--;
+		cl->me_cl->tx_flow_ctrl_creds--;
 	} else {
-		if (WARN_ON(cl->mei_flow_ctrl_creds <= 0))
+		if (WARN_ON(cl->tx_flow_ctrl_creds <= 0))
 			return -EINVAL;
-		cl->mei_flow_ctrl_creds--;
+		cl->tx_flow_ctrl_creds--;
 	}
 	return 0;
 }
@@ -1292,7 +1295,7 @@ int mei_cl_irq_notify(struct mei_cl *cl, struct mei_cl_cb *cb,
  * mei_cl_notify_request - send notification stop/start request
  *
  * @cl: host client
- * @file: associate request with file
+ * @fp: associate request with file
  * @request: 1 for start or 0 for stop
  *
  * Locking: called under "dev->device_lock" lock
@@ -1300,7 +1303,7 @@ int mei_cl_irq_notify(struct mei_cl *cl, struct mei_cl_cb *cb,
  * Return: 0 on such and error otherwise.
  */
 int mei_cl_notify_request(struct mei_cl *cl,
-			  const struct file *file, u8 request)
+			  const struct file *fp, u8 request)
 {
 	struct mei_device *dev;
 	struct mei_cl_cb *cb;
@@ -1325,7 +1328,7 @@ int mei_cl_notify_request(struct mei_cl *cl,
 	}
 
 	fop_type = mei_cl_notify_req2fop(request);
-	cb = mei_io_cb_init(cl, fop_type, file);
+	cb = mei_cl_enqueue_ctrl_wr_cb(cl, 0, fop_type, fp);
 	if (!cb) {
 		rets = -ENOMEM;
 		goto out;
@@ -1336,9 +1339,7 @@ int mei_cl_notify_request(struct mei_cl *cl,
 			rets = -ENODEV;
 			goto out;
 		}
-		list_add_tail(&cb->list, &dev->ctrl_rd_list.list);
-	} else {
-		list_add_tail(&cb->list, &dev->ctrl_wr_list.list);
+		list_move_tail(&cb->list, &dev->ctrl_rd_list.list);
 	}
 
 	mutex_unlock(&dev->device_lock);
@@ -1436,25 +1437,6 @@ out:
 }
 
 /**
- * mei_cl_is_read_fc_cb - check if read cb is waiting for flow control
- *                        for given host client
- *
- * @cl: host client
- *
- * Return: true, if found at least one cb.
- */
-static bool mei_cl_is_read_fc_cb(struct mei_cl *cl)
-{
-	struct mei_device *dev = cl->dev;
-	struct mei_cl_cb *cb;
-
-	list_for_each_entry(cb, &dev->ctrl_wr_list.list, list)
-		if (cb->fop_type == MEI_FOP_READ && cb->cl == cl)
-			return true;
-	return false;
-}
-
-/**
  * mei_cl_read_start - the start read client message function.
  *
  * @cl: host client
@@ -1477,26 +1459,22 @@ int mei_cl_read_start(struct mei_cl *cl, size_t length, const struct file *fp)
 	if (!mei_cl_is_connected(cl))
 		return -ENODEV;
 
-	/* HW currently supports only one pending read */
-	if (!list_empty(&cl->rd_pending) || mei_cl_is_read_fc_cb(cl))
-		return -EBUSY;
-
 	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, mei_cl_mtu(cl));
-	cb = mei_cl_alloc_cb(cl, length, MEI_FOP_READ, fp);
+	if (mei_cl_is_fixed_address(cl) || cl == &dev->iamthif_cl)
+		return 0;
+
+	/* HW currently supports only one pending read */
+	if (cl->rx_flow_ctrl_creds)
+		return -EBUSY;
+
+	cb = mei_cl_enqueue_ctrl_wr_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);
@@ -1504,16 +1482,15 @@ int mei_cl_read_start(struct mei_cl *cl, size_t length, const struct file *fp)
 		goto nortpm;
 	}
 
+	rets = 0;
 	if (mei_hbuf_acquire(dev)) {
 		rets = mei_hbm_cl_flow_control_req(dev, cl);
 		if (rets < 0)
 			goto out;
 
-		list_add_tail(&cb->list, &cl->rd_pending);
-	} else {
-		rets = 0;
-		list_add_tail(&cb->list, &dev->ctrl_wr_list.list);
+		list_move_tail(&cb->list, &cl->rd_pending);
 	}
+	cl->rx_flow_ctrl_creds++;
 
 out:
 	cl_dbg(dev, cl, "rpm: autosuspend\n");
@@ -1557,7 +1534,7 @@ int mei_cl_irq_write(struct mei_cl *cl, struct mei_cl_cb *cb,
 
 	first_chunk = cb->buf_idx == 0;
 
-	rets = first_chunk ? mei_cl_flow_ctrl_creds(cl, cb->fp) : 1;
+	rets = first_chunk ? mei_cl_tx_flow_ctrl_creds(cl) : 1;
 	if (rets < 0)
 		return rets;
 
@@ -1605,7 +1582,7 @@ int mei_cl_irq_write(struct mei_cl *cl, struct mei_cl_cb *cb,
 	cb->completed = mei_hdr.msg_complete == 1;
 
 	if (first_chunk) {
-		if (mei_cl_flow_ctrl_reduce(cl))
+		if (mei_cl_tx_flow_ctrl_creds_reduce(cl))
 			return -EIO;
 	}
 
@@ -1663,7 +1640,7 @@ int mei_cl_write(struct mei_cl *cl, struct mei_cl_cb *cb, bool blocking)
 	mei_hdr.msg_complete = 0;
 	mei_hdr.internal = cb->internal;
 
-	rets = mei_cl_flow_ctrl_creds(cl, cb->fp);
+	rets = mei_cl_tx_flow_ctrl_creds(cl);
 	if (rets < 0)
 		goto err;
 
@@ -1691,7 +1668,7 @@ 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);
+	rets = mei_cl_tx_flow_ctrl_creds_reduce(cl);
 	if (rets)
 		goto err;
 
@@ -1761,6 +1738,9 @@ void mei_cl_complete(struct mei_cl *cl, struct mei_cl_cb *cb)
 
 	case MEI_FOP_READ:
 		list_add_tail(&cb->list, &cl->rd_completed);
+		if (!mei_cl_is_fixed_address(cl) &&
+		    !WARN_ON(!cl->rx_flow_ctrl_creds))
+			cl->rx_flow_ctrl_creds--;
 		if (!mei_cl_bus_rx_event(cl))
 			wake_up_interruptible(&cl->rx_wait);
 		break;
diff --git a/drivers/misc/mei/client.h b/drivers/misc/mei/client.h
index 0d7a3a1fef78..d2bfabecd882 100644
--- a/drivers/misc/mei/client.h
+++ b/drivers/misc/mei/client.h
@@ -82,11 +82,7 @@ static inline u8 mei_me_cl_ver(const struct mei_me_client *me_cl)
 /*
  * MEI IO Functions
  */
-struct mei_cl_cb *mei_io_cb_init(struct mei_cl *cl, enum mei_cb_file_ops type,
-				 const struct file *fp);
 void mei_io_cb_free(struct mei_cl_cb *priv_cb);
-int mei_io_cb_alloc_buf(struct mei_cl_cb *cb, size_t length);
-
 
 /**
  * mei_io_list_init - Sets up a queue list.
@@ -118,6 +114,9 @@ void mei_cl_read_cb_flush(const struct mei_cl *cl, const struct file *fp);
 struct mei_cl_cb *mei_cl_alloc_cb(struct mei_cl *cl, size_t length,
 				  enum mei_cb_file_ops type,
 				  const struct file *fp);
+struct mei_cl_cb *mei_cl_enqueue_ctrl_wr_cb(struct mei_cl *cl, size_t length,
+					    enum mei_cb_file_ops type,
+					    const struct file *fp);
 int mei_cl_flush_queues(struct mei_cl *cl, const struct file *fp);
 
 /*
diff --git a/drivers/misc/mei/hbm.c b/drivers/misc/mei/hbm.c
index 085f3aafe6fa..dd7f15a65eed 100644
--- a/drivers/misc/mei/hbm.c
+++ b/drivers/misc/mei/hbm.c
@@ -161,6 +161,7 @@ void mei_hbm_cl_hdr(struct mei_cl *cl, u8 hbm_cmd, void *buf, size_t len)
  * @dev: the device structure
  * @cl: client
  * @hbm_cmd: host bus message command
+ * @buf: message buffer
  * @len: buffer length
  *
  * Return: 0 on success, <0 on failure.
@@ -276,6 +277,7 @@ int mei_hbm_start_req(struct mei_device *dev)
 
 	dev->hbm_state = MEI_HBM_STARTING;
 	dev->init_clients_timer = MEI_CLIENTS_INIT_TIMEOUT;
+	mei_schedule_stall_timer(dev);
 	return 0;
 }
 
@@ -311,6 +313,7 @@ static int mei_hbm_enum_clients_req(struct mei_device *dev)
 	}
 	dev->hbm_state = MEI_HBM_ENUM_CLIENTS;
 	dev->init_clients_timer = MEI_CLIENTS_INIT_TIMEOUT;
+	mei_schedule_stall_timer(dev);
 	return 0;
 }
 
@@ -339,7 +342,7 @@ static int mei_hbm_me_cl_add(struct mei_device *dev,
 
 	me_cl->props = res->client_properties;
 	me_cl->client_id = res->me_addr;
-	me_cl->mei_flow_ctrl_creds = 0;
+	me_cl->tx_flow_ctrl_creds = 0;
 
 	mei_me_cl_add(dev, me_cl);
 
@@ -561,6 +564,7 @@ static int mei_hbm_prop_req(struct mei_device *dev, unsigned long start_idx)
 	}
 
 	dev->init_clients_timer = MEI_CLIENTS_INIT_TIMEOUT;
+	mei_schedule_stall_timer(dev);
 
 	return 0;
 }
@@ -636,23 +640,22 @@ int mei_hbm_cl_flow_control_req(struct mei_device *dev, struct mei_cl *cl)
 }
 
 /**
- * mei_hbm_add_single_flow_creds - adds single buffer credentials.
+ * mei_hbm_add_single_tx_flow_ctrl_creds - adds single buffer credentials.
  *
  * @dev: the device structure
- * @flow: flow control.
+ * @fctrl: flow control response bus message
  *
  * Return: 0 on success, < 0 otherwise
  */
-static int mei_hbm_add_single_flow_creds(struct mei_device *dev,
-				  struct hbm_flow_control *flow)
+static int mei_hbm_add_single_tx_flow_ctrl_creds(struct mei_device *dev,
+						 struct hbm_flow_control *fctrl)
 {
 	struct mei_me_client *me_cl;
 	int rets;
 
-	me_cl = mei_me_cl_by_id(dev, flow->me_addr);
+	me_cl = mei_me_cl_by_id(dev, fctrl->me_addr);
 	if (!me_cl) {
-		dev_err(dev->dev, "no such me client %d\n",
-			flow->me_addr);
+		dev_err(dev->dev, "no such me client %d\n", fctrl->me_addr);
 		return -ENOENT;
 	}
 
@@ -661,9 +664,9 @@ static int mei_hbm_add_single_flow_creds(struct mei_device *dev,
 		goto out;
 	}
 
-	me_cl->mei_flow_ctrl_creds++;
+	me_cl->tx_flow_ctrl_creds++;
 	dev_dbg(dev->dev, "recv flow ctrl msg ME %d (single) creds = %d.\n",
-	    flow->me_addr, me_cl->mei_flow_ctrl_creds);
+		fctrl->me_addr, me_cl->tx_flow_ctrl_creds);
 
 	rets = 0;
 out:
@@ -675,24 +678,24 @@ out:
  * mei_hbm_cl_flow_control_res - flow control response from me
  *
  * @dev: the device structure
- * @flow_control: flow control response bus message
+ * @fctrl: flow control response bus message
  */
-static void mei_hbm_cl_flow_control_res(struct mei_device *dev,
-					struct hbm_flow_control *flow_control)
+static void mei_hbm_cl_tx_flow_ctrl_creds_res(struct mei_device *dev,
+					       struct hbm_flow_control *fctrl)
 {
 	struct mei_cl *cl;
 
-	if (!flow_control->host_addr) {
+	if (!fctrl->host_addr) {
 		/* single receive buffer */
-		mei_hbm_add_single_flow_creds(dev, flow_control);
+		mei_hbm_add_single_tx_flow_ctrl_creds(dev, fctrl);
 		return;
 	}
 
-	cl = mei_hbm_cl_find_by_cmd(dev, flow_control);
+	cl = mei_hbm_cl_find_by_cmd(dev, fctrl);
 	if (cl) {
-		cl->mei_flow_ctrl_creds++;
+		cl->tx_flow_ctrl_creds++;
 		cl_dbg(dev, cl, "flow control creds = %d.\n",
-				cl->mei_flow_ctrl_creds);
+				cl->tx_flow_ctrl_creds);
 	}
 }
 
@@ -871,10 +874,10 @@ static int mei_hbm_fw_disconnect_req(struct mei_device *dev,
 		cl->state = MEI_FILE_DISCONNECTING;
 		cl->timer_count = 0;
 
-		cb = mei_io_cb_init(cl, MEI_FOP_DISCONNECT_RSP, NULL);
+		cb = mei_cl_enqueue_ctrl_wr_cb(cl, 0, MEI_FOP_DISCONNECT_RSP,
+					       NULL);
 		if (!cb)
 			return -ENOMEM;
-		list_add_tail(&cb->list, &dev->ctrl_wr_list.list);
 	}
 	return 0;
 }
@@ -1022,7 +1025,7 @@ int mei_hbm_dispatch(struct mei_device *dev, struct mei_msg_hdr *hdr)
 
 	struct mei_hbm_cl_cmd *cl_cmd;
 	struct hbm_client_connect_request *disconnect_req;
-	struct hbm_flow_control *flow_control;
+	struct hbm_flow_control *fctrl;
 
 	/* read the message to our buffer */
 	BUG_ON(hdr->length >= sizeof(dev->rd_msg_buf));
@@ -1102,8 +1105,8 @@ int mei_hbm_dispatch(struct mei_device *dev, struct mei_msg_hdr *hdr)
 	case MEI_FLOW_CONTROL_CMD:
 		dev_dbg(dev->dev, "hbm: client flow control response: message received.\n");
 
-		flow_control = (struct hbm_flow_control *) mei_msg;
-		mei_hbm_cl_flow_control_res(dev, flow_control);
+		fctrl = (struct hbm_flow_control *)mei_msg;
+		mei_hbm_cl_tx_flow_ctrl_creds_res(dev, fctrl);
 		break;
 
 	case MEI_PG_ISOLATION_ENTRY_RES_CMD:
diff --git a/drivers/misc/mei/hw-me-regs.h b/drivers/misc/mei/hw-me-regs.h
index 0dcb854b4bfc..7ad15d678878 100644
--- a/drivers/misc/mei/hw-me-regs.h
+++ b/drivers/misc/mei/hw-me-regs.h
@@ -125,6 +125,9 @@
 #define MEI_DEV_ID_BXT_M      0x1A9A  /* Broxton M */
 #define MEI_DEV_ID_APL_I      0x5A9A  /* Apollo Lake I */
 
+#define MEI_DEV_ID_KBP        0xA2BA  /* Kaby Point */
+#define MEI_DEV_ID_KBP_2      0xA2BB  /* Kaby Point 2 */
+
 /*
  * MEI HW Section
  */
diff --git a/drivers/misc/mei/hw-me.c b/drivers/misc/mei/hw-me.c
index dc3a854e02d3..56c2101e80ad 100644
--- a/drivers/misc/mei/hw-me.c
+++ b/drivers/misc/mei/hw-me.c
@@ -18,6 +18,7 @@
 
 #include <linux/kthread.h>
 #include <linux/interrupt.h>
+#include <linux/pm_runtime.h>
 
 #include "mei_dev.h"
 #include "hbm.h"
@@ -1063,6 +1064,8 @@ static int mei_me_hw_reset(struct mei_device *dev, bool intr_enable)
 		}
 	}
 
+	pm_runtime_set_active(dev->dev);
+
 	hcsr = mei_hcsr_read(dev);
 	/* H_RST may be found lit before reset is started,
 	 * for example if preceding reset flow hasn't completed.
diff --git a/drivers/misc/mei/hw-txe.c b/drivers/misc/mei/hw-txe.c
index 4a6c1b85f11e..e6e5e55a12ed 100644
--- a/drivers/misc/mei/hw-txe.c
+++ b/drivers/misc/mei/hw-txe.c
@@ -20,6 +20,7 @@
 #include <linux/delay.h>
 #include <linux/kthread.h>
 #include <linux/irqreturn.h>
+#include <linux/pm_runtime.h>
 
 #include <linux/mei.h>
 
@@ -935,6 +936,8 @@ static int mei_txe_hw_start(struct mei_device *dev)
 		return ret;
 	}
 
+	pm_runtime_set_active(dev->dev);
+
 	/* enable input ready interrupts:
 	 * SEC_IPC_HOST_INT_MASK.IPC_INPUT_READY_INT_MASK
 	 */
diff --git a/drivers/misc/mei/init.c b/drivers/misc/mei/init.c
index f7c8dfdb6a12..9a9c2484d107 100644
--- a/drivers/misc/mei/init.c
+++ b/drivers/misc/mei/init.c
@@ -94,7 +94,7 @@ void mei_cancel_work(struct mei_device *dev)
 	cancel_work_sync(&dev->reset_work);
 	cancel_work_sync(&dev->bus_rescan_work);
 
-	cancel_delayed_work(&dev->timer_work);
+	cancel_delayed_work_sync(&dev->timer_work);
 }
 EXPORT_SYMBOL_GPL(mei_cancel_work);
 
diff --git a/drivers/misc/mei/interrupt.c b/drivers/misc/mei/interrupt.c
index 3831a7ba2531..5a4893ce9c24 100644
--- a/drivers/misc/mei/interrupt.c
+++ b/drivers/misc/mei/interrupt.c
@@ -102,26 +102,25 @@ int mei_cl_irq_read_msg(struct mei_cl *cl,
 {
 	struct mei_device *dev = cl->dev;
 	struct mei_cl_cb *cb;
-	unsigned char *buffer = NULL;
 	size_t buf_sz;
 
 	cb = list_first_entry_or_null(&cl->rd_pending, struct mei_cl_cb, list);
 	if (!cb) {
-		cl_err(dev, cl, "pending read cb not found\n");
-		goto out;
+		if (!mei_cl_is_fixed_address(cl)) {
+			cl_err(dev, cl, "pending read cb not found\n");
+			goto discard;
+		}
+		cb = mei_cl_alloc_cb(cl, mei_cl_mtu(cl), MEI_FOP_READ, cl->fp);
+		if (!cb)
+			goto discard;
+		list_add_tail(&cb->list, &cl->rd_pending);
 	}
 
 	if (!mei_cl_is_connected(cl)) {
 		cl_dbg(dev, cl, "not connected\n");
-		cb->status = -ENODEV;
-		goto out;
-	}
-
-	if (cb->buf.size == 0 || cb->buf.data == NULL) {
-		cl_err(dev, cl, "response buffer is not allocated.\n");
 		list_move_tail(&cb->list, &complete_list->list);
-		cb->status = -ENOMEM;
-		goto out;
+		cb->status = -ENODEV;
+		goto discard;
 	}
 
 	buf_sz = mei_hdr->length + cb->buf_idx;
@@ -132,25 +131,19 @@ int mei_cl_irq_read_msg(struct mei_cl *cl,
 
 		list_move_tail(&cb->list, &complete_list->list);
 		cb->status = -EMSGSIZE;
-		goto out;
+		goto discard;
 	}
 
 	if (cb->buf.size < buf_sz) {
 		cl_dbg(dev, cl, "message overflow. size %zu len %d idx %zu\n",
 			cb->buf.size, mei_hdr->length, cb->buf_idx);
-		buffer = krealloc(cb->buf.data, buf_sz, GFP_KERNEL);
 
-		if (!buffer) {
-			cb->status = -ENOMEM;
-			list_move_tail(&cb->list, &complete_list->list);
-			goto out;
-		}
-		cb->buf.data = buffer;
-		cb->buf.size = buf_sz;
+		list_move_tail(&cb->list, &complete_list->list);
+		cb->status = -EMSGSIZE;
+		goto discard;
 	}
 
-	buffer = cb->buf.data + cb->buf_idx;
-	mei_read_slots(dev, buffer, mei_hdr->length);
+	mei_read_slots(dev, cb->buf.data + cb->buf_idx, mei_hdr->length);
 
 	cb->buf_idx += mei_hdr->length;
 
@@ -162,10 +155,10 @@ int mei_cl_irq_read_msg(struct mei_cl *cl,
 		pm_request_autosuspend(dev->dev);
 	}
 
-out:
-	if (!buffer)
-		mei_irq_discard_msg(dev, mei_hdr);
+	return 0;
 
+discard:
+	mei_irq_discard_msg(dev, mei_hdr);
 	return 0;
 }
 
@@ -216,6 +209,9 @@ static int mei_cl_irq_read(struct mei_cl *cl, struct mei_cl_cb *cb,
 	int slots;
 	int ret;
 
+	if (!list_empty(&cl->rd_pending))
+		return 0;
+
 	msg_slots = mei_data2slots(sizeof(struct hbm_flow_control));
 	slots = mei_hbuf_empty_slots(dev);
 
@@ -463,6 +459,19 @@ static void mei_connect_timeout(struct mei_cl *cl)
 	mei_reset(dev);
 }
 
+#define MEI_STALL_TIMER_FREQ (2 * HZ)
+/**
+ * mei_schedule_stall_timer - re-arm stall_timer work
+ *
+ * Schedule stall timer
+ *
+ * @dev: the device structure
+ */
+void mei_schedule_stall_timer(struct mei_device *dev)
+{
+	schedule_delayed_work(&dev->timer_work, MEI_STALL_TIMER_FREQ);
+}
+
 /**
  * mei_timer - timer function.
  *
@@ -472,10 +481,9 @@ static void mei_connect_timeout(struct mei_cl *cl)
 void mei_timer(struct work_struct *work)
 {
 	struct mei_cl *cl;
-
 	struct mei_device *dev = container_of(work,
 					struct mei_device, timer_work.work);
-
+	bool reschedule_timer = false;
 
 	mutex_lock(&dev->device_lock);
 
@@ -490,6 +498,7 @@ void mei_timer(struct work_struct *work)
 				mei_reset(dev);
 				goto out;
 			}
+			reschedule_timer = true;
 		}
 	}
 
@@ -504,6 +513,7 @@ void mei_timer(struct work_struct *work)
 				mei_connect_timeout(cl);
 				goto out;
 			}
+			reschedule_timer = true;
 		}
 	}
 
@@ -514,19 +524,16 @@ void mei_timer(struct work_struct *work)
 		if (--dev->iamthif_stall_timer == 0) {
 			dev_err(dev->dev, "timer: amthif  hanged.\n");
 			mei_reset(dev);
-			dev->iamthif_canceled = false;
-			dev->iamthif_state = MEI_IAMTHIF_IDLE;
 
-			mei_io_cb_free(dev->iamthif_current_cb);
-			dev->iamthif_current_cb = NULL;
-
-			dev->iamthif_fp = NULL;
 			mei_amthif_run_next_cmd(dev);
+			goto out;
 		}
+		reschedule_timer = true;
 	}
 
 out:
-	if (dev->dev_state != MEI_DEV_DISABLED)
-		schedule_delayed_work(&dev->timer_work, 2 * HZ);
+	if (dev->dev_state != MEI_DEV_DISABLED && reschedule_timer)
+		mei_schedule_stall_timer(dev);
+
 	mutex_unlock(&dev->device_lock);
 }
diff --git a/drivers/misc/mei/main.c b/drivers/misc/mei/main.c
index 52635b063873..fa50635512e8 100644
--- a/drivers/misc/mei/main.c
+++ b/drivers/misc/mei/main.c
@@ -71,6 +71,7 @@ static int mei_open(struct inode *inode, struct file *file)
 		goto err_unlock;
 	}
 
+	cl->fp = file;
 	file->private_data = cl;
 
 	mutex_unlock(&dev->device_lock);
@@ -138,9 +139,8 @@ static ssize_t mei_read(struct file *file, char __user *ubuf,
 	struct mei_cl *cl = file->private_data;
 	struct mei_device *dev;
 	struct mei_cl_cb *cb = NULL;
+	bool nonblock = !!(file->f_flags & O_NONBLOCK);
 	int rets;
-	int err;
-
 
 	if (WARN_ON(!cl || !cl->dev))
 		return -ENODEV;
@@ -164,11 +164,6 @@ static ssize_t mei_read(struct file *file, char __user *ubuf,
 		goto out;
 	}
 
-	if (cl == &dev->iamthif_cl) {
-		rets = mei_amthif_read(dev, file, ubuf, length, offset);
-		goto out;
-	}
-
 	cb = mei_cl_read_cb(cl, file);
 	if (cb)
 		goto copy_buffer;
@@ -176,24 +171,29 @@ static ssize_t mei_read(struct file *file, char __user *ubuf,
 	if (*offset > 0)
 		*offset = 0;
 
-	err = mei_cl_read_start(cl, length, file);
-	if (err && err != -EBUSY) {
-		cl_dbg(dev, cl, "mei start read failure status = %d\n", err);
-		rets = err;
+	rets = mei_cl_read_start(cl, length, file);
+	if (rets && rets != -EBUSY) {
+		cl_dbg(dev, cl, "mei start read failure status = %d\n", rets);
 		goto out;
 	}
 
-	if (list_empty(&cl->rd_completed) && !waitqueue_active(&cl->rx_wait)) {
-		if (file->f_flags & O_NONBLOCK) {
-			rets = -EAGAIN;
-			goto out;
-		}
+	if (nonblock) {
+		rets = -EAGAIN;
+		goto out;
+	}
 
+	if (rets == -EBUSY &&
+	    !mei_cl_enqueue_ctrl_wr_cb(cl, length, MEI_FOP_READ, file)) {
+		rets = -ENOMEM;
+		goto out;
+	}
+
+	do {
 		mutex_unlock(&dev->device_lock);
 
 		if (wait_event_interruptible(cl->rx_wait,
-				(!list_empty(&cl->rd_completed)) ||
-				(!mei_cl_is_connected(cl)))) {
+					     (!list_empty(&cl->rd_completed)) ||
+					     (!mei_cl_is_connected(cl)))) {
 
 			if (signal_pending(current))
 				return -EINTR;
@@ -202,16 +202,12 @@ static ssize_t mei_read(struct file *file, char __user *ubuf,
 
 		mutex_lock(&dev->device_lock);
 		if (!mei_cl_is_connected(cl)) {
-			rets = -EBUSY;
+			rets = -ENODEV;
 			goto out;
 		}
-	}
 
-	cb = mei_cl_read_cb(cl, file);
-	if (!cb) {
-		rets = 0;
-		goto out;
-	}
+		cb = mei_cl_read_cb(cl, file);
+	} while (!cb);
 
 copy_buffer:
 	/* now copy the data to user space */
@@ -609,24 +605,24 @@ static unsigned int mei_poll(struct file *file, poll_table *wait)
 		goto out;
 	}
 
-	if (cl == &dev->iamthif_cl) {
-		mask = mei_amthif_poll(dev, file, wait);
-		goto out;
-	}
-
 	if (notify_en) {
 		poll_wait(file, &cl->ev_wait, wait);
 		if (cl->notify_ev)
 			mask |= POLLPRI;
 	}
 
+	if (cl == &dev->iamthif_cl) {
+		mask |= mei_amthif_poll(file, wait);
+		goto out;
+	}
+
 	if (req_events & (POLLIN | POLLRDNORM)) {
 		poll_wait(file, &cl->rx_wait, wait);
 
 		if (!list_empty(&cl->rd_completed))
 			mask |= POLLIN | POLLRDNORM;
 		else
-			mei_cl_read_start(cl, 0, file);
+			mei_cl_read_start(cl, mei_cl_mtu(cl), file);
 	}
 
 out:
diff --git a/drivers/misc/mei/mei_dev.h b/drivers/misc/mei/mei_dev.h
index e5e32503d4bc..1169fd9e7d02 100644
--- a/drivers/misc/mei/mei_dev.h
+++ b/drivers/misc/mei/mei_dev.h
@@ -80,18 +80,13 @@ const char *mei_dev_state_str(int state);
 enum iamthif_states {
 	MEI_IAMTHIF_IDLE,
 	MEI_IAMTHIF_WRITING,
-	MEI_IAMTHIF_FLOW_CONTROL,
 	MEI_IAMTHIF_READING,
-	MEI_IAMTHIF_READ_COMPLETE
 };
 
 enum mei_file_transaction_states {
 	MEI_IDLE,
 	MEI_WRITING,
 	MEI_WRITE_COMPLETE,
-	MEI_FLOW_CONTROL,
-	MEI_READING,
-	MEI_READ_COMPLETE
 };
 
 /**
@@ -146,7 +141,7 @@ struct mei_fw_status {
  * @refcnt: struct reference count
  * @props: client properties
  * @client_id: me client id
- * @mei_flow_ctrl_creds: flow control credits
+ * @tx_flow_ctrl_creds: flow control credits
  * @connect_count: number connections to this client
  * @bus_added: added to bus
  */
@@ -155,7 +150,7 @@ struct mei_me_client {
 	struct kref refcnt;
 	struct mei_client_properties props;
 	u8 client_id;
-	u8 mei_flow_ctrl_creds;
+	u8 tx_flow_ctrl_creds;
 	u8 connect_count;
 	u8 bus_added;
 };
@@ -202,10 +197,11 @@ struct mei_cl_cb {
  * @ev_async: event async notification
  * @status: connection status
  * @me_cl: fw client connected
+ * @fp: file associated with client
  * @host_client_id: host id
- * @mei_flow_ctrl_creds: transmit flow credentials
+ * @tx_flow_ctrl_creds: transmit flow credentials
+ * @rx_flow_ctrl_creds: receive flow credentials
  * @timer_count:  watchdog timer for operation completion
- * @reserved: reserved for alignment
  * @notify_en: notification - enabled/disabled
  * @notify_ev: pending notification event
  * @writing_state: state of the tx
@@ -225,10 +221,11 @@ struct mei_cl {
 	struct fasync_struct *ev_async;
 	int status;
 	struct mei_me_client *me_cl;
+	const struct file *fp;
 	u8 host_client_id;
-	u8 mei_flow_ctrl_creds;
+	u8 tx_flow_ctrl_creds;
+	u8 rx_flow_ctrl_creds;
 	u8 timer_count;
-	u8 reserved;
 	u8 notify_en;
 	u8 notify_ev;
 	enum mei_file_transaction_states writing_state;
@@ -400,9 +397,7 @@ const char *mei_pg_state_str(enum mei_pg_state state);
  * @override_fixed_address: force allow fixed address behavior
  *
  * @amthif_cmd_list : amthif list for cmd waiting
- * @iamthif_fp : file for current amthif operation
  * @iamthif_cl  : amthif host client
- * @iamthif_current_cb : amthif current operation callback
  * @iamthif_open_count : number of opened amthif connections
  * @iamthif_stall_timer : timer to detect amthif hang
  * @iamthif_state : amthif processor state
@@ -484,10 +479,7 @@ struct mei_device {
 
 	/* amthif list for cmd waiting */
 	struct mei_cl_cb amthif_cmd_list;
-	/* driver managed amthif list for reading completed amthif cmd data */
-	const struct file *iamthif_fp;
 	struct mei_cl iamthif_cl;
-	struct mei_cl_cb *iamthif_current_cb;
 	long iamthif_open_count;
 	u32 iamthif_stall_timer;
 	enum iamthif_states iamthif_state;
@@ -556,6 +548,7 @@ void mei_cancel_work(struct mei_device *dev);
  */
 
 void mei_timer(struct work_struct *work);
+void mei_schedule_stall_timer(struct mei_device *dev);
 int mei_irq_read_handler(struct mei_device *dev,
 		struct mei_cl_cb *cmpl_list, s32 *slots);
 
@@ -569,11 +562,7 @@ void mei_amthif_reset_params(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);
-
-unsigned int mei_amthif_poll(struct mei_device *dev,
-		struct file *file, poll_table *wait);
+unsigned int mei_amthif_poll(struct file *file, poll_table *wait);
 
 int mei_amthif_release(struct mei_device *dev, struct file *file);
 
diff --git a/drivers/misc/mei/pci-me.c b/drivers/misc/mei/pci-me.c
index 71cea9b296b2..f3ffd883b232 100644
--- a/drivers/misc/mei/pci-me.c
+++ b/drivers/misc/mei/pci-me.c
@@ -91,6 +91,9 @@ static const struct pci_device_id mei_me_pci_tbl[] = {
 	{MEI_PCI_DEVICE(MEI_DEV_ID_BXT_M, mei_me_pch8_cfg)},
 	{MEI_PCI_DEVICE(MEI_DEV_ID_APL_I, mei_me_pch8_cfg)},
 
+	{MEI_PCI_DEVICE(MEI_DEV_ID_KBP, mei_me_pch8_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_KBP_2, mei_me_pch8_cfg)},
+
 	/* required last entry */
 	{0, }
 };
@@ -217,8 +220,6 @@ static int mei_me_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 
 	pci_set_drvdata(pdev, dev);
 
-	schedule_delayed_work(&dev->timer_work, HZ);
-
 	/*
 	* For not wake-able HW runtime pm framework
 	* can't be used on pci device level.
@@ -400,6 +401,9 @@ static int mei_me_pm_runtime_suspend(struct device *device)
 
 	dev_dbg(&pdev->dev, "rpm: me: runtime suspend ret=%d\n", ret);
 
+	if (ret && ret != -EAGAIN)
+		schedule_work(&dev->reset_work);
+
 	return ret;
 }
 
@@ -423,6 +427,9 @@ static int mei_me_pm_runtime_resume(struct device *device)
 
 	dev_dbg(&pdev->dev, "rpm: me: runtime resume ret = %d\n", ret);
 
+	if (ret)
+		schedule_work(&dev->reset_work);
+
 	return ret;
 }
 
diff --git a/drivers/misc/mei/pci-txe.c b/drivers/misc/mei/pci-txe.c
index 30cc30683c07..58ffd30dcc91 100644
--- a/drivers/misc/mei/pci-txe.c
+++ b/drivers/misc/mei/pci-txe.c
@@ -347,6 +347,10 @@ static int mei_txe_pm_runtime_suspend(struct device *device)
 	dev_dbg(&pdev->dev, "rpm: txe: runtime suspend ret=%d\n", ret);
 
 	mutex_unlock(&dev->device_lock);
+
+	if (ret && ret != -EAGAIN)
+		schedule_work(&dev->reset_work);
+
 	return ret;
 }
 
@@ -372,6 +376,9 @@ static int mei_txe_pm_runtime_resume(struct device *device)
 
 	dev_dbg(&pdev->dev, "rpm: txe: runtime resume ret = %d\n", ret);
 
+	if (ret)
+		schedule_work(&dev->reset_work);
+
 	return ret;
 }
 
diff --git a/drivers/misc/mic/scif/scif_dma.c b/drivers/misc/mic/scif/scif_dma.c
index cd01a0efda6b..64d5760d069a 100644
--- a/drivers/misc/mic/scif/scif_dma.c
+++ b/drivers/misc/mic/scif/scif_dma.c
@@ -115,7 +115,6 @@ int scif_reserve_dma_chan(struct scif_endpt *ep)
  */
 static
 void __scif_rma_destroy_tcw(struct scif_mmu_notif *mmn,
-			    struct scif_endpt *ep,
 			    u64 start, u64 len)
 {
 	struct list_head *item, *tmp;
@@ -128,7 +127,6 @@ void __scif_rma_destroy_tcw(struct scif_mmu_notif *mmn,
 
 	list_for_each_safe(item, tmp, &mmn->tc_reg_list) {
 		window = list_entry(item, struct scif_window, list);
-		ep = (struct scif_endpt *)window->ep;
 		if (!len)
 			break;
 		start_va = window->va_for_temp;
@@ -146,7 +144,7 @@ static void scif_rma_destroy_tcw(struct scif_mmu_notif *mmn, u64 start, u64 len)
 	struct scif_endpt *ep = mmn->ep;
 
 	spin_lock(&ep->rma_info.tc_lock);
-	__scif_rma_destroy_tcw(mmn, ep, start, len);
+	__scif_rma_destroy_tcw(mmn, start, len);
 	spin_unlock(&ep->rma_info.tc_lock);
 }
 
@@ -169,7 +167,7 @@ static void __scif_rma_destroy_tcw_ep(struct scif_endpt *ep)
 	spin_lock(&ep->rma_info.tc_lock);
 	list_for_each_safe(item, tmp, &ep->rma_info.mmn_list) {
 		mmn = list_entry(item, struct scif_mmu_notif, list);
-		__scif_rma_destroy_tcw(mmn, ep, 0, ULONG_MAX);
+		__scif_rma_destroy_tcw(mmn, 0, ULONG_MAX);
 	}
 	spin_unlock(&ep->rma_info.tc_lock);
 }
diff --git a/drivers/misc/mic/scif/scif_mmap.c b/drivers/misc/mic/scif/scif_mmap.c
index 49cb8f7b4672..928211677079 100644
--- a/drivers/misc/mic/scif/scif_mmap.c
+++ b/drivers/misc/mic/scif/scif_mmap.c
@@ -552,7 +552,7 @@ static void scif_munmap(struct vm_area_struct *vma)
 {
 	struct scif_endpt *ep;
 	struct vma_pvt *vmapvt = vma->vm_private_data;
-	int nr_pages = (vma->vm_end - vma->vm_start) >> PAGE_SHIFT;
+	int nr_pages = vma_pages(vma);
 	s64 offset;
 	struct scif_rma_req req;
 	struct scif_window *window = NULL;
@@ -614,7 +614,7 @@ int scif_mmap(struct vm_area_struct *vma, scif_epd_t epd)
 	struct scif_window *window = NULL;
 	struct scif_endpt *ep = (struct scif_endpt *)epd;
 	s64 start_offset = vma->vm_pgoff << PAGE_SHIFT;
-	int nr_pages = (vma->vm_end - vma->vm_start) >> PAGE_SHIFT;
+	int nr_pages = vma_pages(vma);
 	int err;
 	struct vma_pvt *vmapvt;
 
diff --git a/drivers/misc/mic/scif/scif_rma.c b/drivers/misc/mic/scif/scif_rma.c
index e0203b1a20fd..f806a4471eb9 100644
--- a/drivers/misc/mic/scif/scif_rma.c
+++ b/drivers/misc/mic/scif/scif_rma.c
@@ -1396,8 +1396,7 @@ retry:
 		pinned_pages->nr_pages = get_user_pages(
 				(u64)addr,
 				nr_pages,
-				!!(prot & SCIF_PROT_WRITE),
-				0,
+				(prot & SCIF_PROT_WRITE) ? FOLL_WRITE : 0,
 				pinned_pages->pages,
 				NULL);
 		up_write(&mm->mmap_sem);
diff --git a/drivers/misc/pch_phub.c b/drivers/misc/pch_phub.c
index 4810e039bbec..e42bdc90fa27 100644
--- a/drivers/misc/pch_phub.c
+++ b/drivers/misc/pch_phub.c
@@ -28,6 +28,7 @@
 #include <linux/if_ether.h>
 #include <linux/ctype.h>
 #include <linux/dmi.h>
+#include <linux/of.h>
 
 #define PHUB_STATUS 0x00		/* Status Register offset */
 #define PHUB_CONTROL 0x04		/* Control Register offset */
@@ -57,6 +58,7 @@
 
 /* CM-iTC */
 #define CLKCFG_UART_48MHZ			(1 << 16)
+#define CLKCFG_UART_25MHZ			(2 << 16)
 #define CLKCFG_BAUDDIV				(2 << 20)
 #define CLKCFG_PLL2VCO				(8 << 9)
 #define CLKCFG_UARTCLKSEL			(1 << 18)
@@ -711,6 +713,12 @@ static int pch_phub_probe(struct pci_dev *pdev,
 
 	if (id->driver_data == 1) { /* EG20T PCH */
 		const char *board_name;
+		unsigned int prefetch = 0x000affaa;
+
+		if (pdev->dev.of_node)
+			of_property_read_u32(pdev->dev.of_node,
+						  "intel,eg20t-prefetch",
+						  &prefetch);
 
 		ret = sysfs_create_file(&pdev->dev.kobj,
 					&dev_attr_pch_mac.attr);
@@ -736,11 +744,21 @@ static int pch_phub_probe(struct pci_dev *pdev,
 						CLKCFG_UART_MASK);
 
 		/* set the prefech value */
-		iowrite32(0x000affaa, chip->pch_phub_base_address + 0x14);
+		iowrite32(prefetch, chip->pch_phub_base_address + 0x14);
 		/* set the interrupt delay value */
 		iowrite32(0x25, chip->pch_phub_base_address + 0x44);
 		chip->pch_opt_rom_start_address = PCH_PHUB_ROM_START_ADDR_EG20T;
 		chip->pch_mac_start_address = PCH_PHUB_MAC_START_ADDR_EG20T;
+
+		/* quirk for MIPS Boston platform */
+		if (pdev->dev.of_node) {
+			if (of_machine_is_compatible("img,boston")) {
+				pch_phub_read_modify_write_reg(chip,
+					(unsigned int)CLKCFG_REG_OFFSET,
+					CLKCFG_UART_25MHZ,
+					CLKCFG_UART_MASK);
+			}
+		}
 	} else if (id->driver_data == 2) { /* ML7213 IOH */
 		ret = sysfs_create_bin_file(&pdev->dev.kobj, &pch_bin_attr);
 		if (ret)
diff --git a/drivers/misc/sgi-gru/grufault.c b/drivers/misc/sgi-gru/grufault.c
index a2d97b9b17e3..6fb773dbcd0c 100644
--- a/drivers/misc/sgi-gru/grufault.c
+++ b/drivers/misc/sgi-gru/grufault.c
@@ -198,7 +198,7 @@ static int non_atomic_pte_lookup(struct vm_area_struct *vma,
 #else
 	*pageshift = PAGE_SHIFT;
 #endif
-	if (get_user_pages(vaddr, 1, write, 0, &page, NULL) <= 0)
+	if (get_user_pages(vaddr, 1, write ? FOLL_WRITE : 0, &page, NULL) <= 0)
 		return -EFAULT;
 	*paddr = page_to_phys(page);
 	put_page(page);
diff --git a/drivers/misc/vmw_vmci/vmci_host.c b/drivers/misc/vmw_vmci/vmci_host.c
index 9ec262a52656..ec090105eb4b 100644
--- a/drivers/misc/vmw_vmci/vmci_host.c
+++ b/drivers/misc/vmw_vmci/vmci_host.c
@@ -381,18 +381,12 @@ static int vmci_host_do_send_datagram(struct vmci_host_dev *vmci_host_dev,
 		return -EINVAL;
 	}
 
-	dg = kmalloc(send_info.len, GFP_KERNEL);
-	if (!dg) {
+	dg = memdup_user((void __user *)(uintptr_t)send_info.addr,
+			 send_info.len);
+	if (IS_ERR(dg)) {
 		vmci_ioctl_err(
 			"cannot allocate memory to dispatch datagram\n");
-		return -ENOMEM;
-	}
-
-	if (copy_from_user(dg, (void __user *)(uintptr_t)send_info.addr,
-			   send_info.len)) {
-		vmci_ioctl_err("error getting datagram\n");
-		kfree(dg);
-		return -EFAULT;
+		return PTR_ERR(dg);
 	}
 
 	if (VMCI_DG_SIZE(dg) != send_info.len) {
diff --git a/drivers/mmc/card/block.c b/drivers/mmc/card/block.c
index 2206d4477dbb..709a872ed484 100644
--- a/drivers/mmc/card/block.c
+++ b/drivers/mmc/card/block.c
@@ -46,6 +46,7 @@
 #include <asm/uaccess.h>
 
 #include "queue.h"
+#include "block.h"
 
 MODULE_ALIAS("mmc:block");
 #ifdef MODULE_PARAM_PREFIX
@@ -142,8 +143,6 @@ static inline void mmc_blk_clear_packed(struct mmc_queue_req *mqrq)
 {
 	struct mmc_packed *packed = mqrq->packed;
 
-	BUG_ON(!packed);
-
 	mqrq->cmd_type = MMC_PACKED_NONE;
 	packed->nr_entries = MMC_PACKED_NR_ZERO;
 	packed->idx_failure = MMC_PACKED_NR_IDX;
@@ -1443,8 +1442,6 @@ static int mmc_blk_packed_err_check(struct mmc_card *card,
 	int err, check, status;
 	u8 *ext_csd;
 
-	BUG_ON(!packed);
-
 	packed->retries--;
 	check = mmc_blk_err_check(card, areq);
 	err = get_card_status(card, &status, 0);
@@ -1673,6 +1670,18 @@ static u8 mmc_blk_prep_packed_list(struct mmc_queue *mq, struct request *req)
 	u8 max_packed_rw = 0;
 	u8 reqs = 0;
 
+	/*
+	 * We don't need to check packed for any further
+	 * operation of packed stuff as we set MMC_PACKED_NONE
+	 * and return zero for reqs if geting null packed. Also
+	 * we clean the flag of MMC_BLK_PACKED_CMD to avoid doing
+	 * it again when removing blk req.
+	 */
+	if (!mqrq->packed) {
+		md->flags &= (~MMC_BLK_PACKED_CMD);
+		goto no_packed;
+	}
+
 	if (!(md->flags & MMC_BLK_PACKED_CMD))
 		goto no_packed;
 
@@ -1778,12 +1787,10 @@ static void mmc_blk_packed_hdr_wrq_prep(struct mmc_queue_req *mqrq,
 	struct mmc_blk_data *md = mq->data;
 	struct mmc_packed *packed = mqrq->packed;
 	bool do_rel_wr, do_data_tag;
-	u32 *packed_cmd_hdr;
+	__le32 *packed_cmd_hdr;
 	u8 hdr_blocks;
 	u8 i = 1;
 
-	BUG_ON(!packed);
-
 	mqrq->cmd_type = MMC_PACKED_WRITE;
 	packed->blocks = 0;
 	packed->idx_failure = MMC_PACKED_NR_IDX;
@@ -1887,8 +1894,6 @@ static int mmc_blk_end_packed_req(struct mmc_queue_req *mq_rq)
 	int idx = packed->idx_failure, i = 0;
 	int ret = 0;
 
-	BUG_ON(!packed);
-
 	while (!list_empty(&packed->list)) {
 		prq = list_entry_rq(packed->list.next);
 		if (idx == i) {
@@ -1917,8 +1922,6 @@ static void mmc_blk_abort_packed_req(struct mmc_queue_req *mq_rq)
 	struct request *prq;
 	struct mmc_packed *packed = mq_rq->packed;
 
-	BUG_ON(!packed);
-
 	while (!list_empty(&packed->list)) {
 		prq = list_entry_rq(packed->list.next);
 		list_del_init(&prq->queuelist);
@@ -1935,8 +1938,6 @@ static void mmc_blk_revert_packed_req(struct mmc_queue *mq,
 	struct request_queue *q = mq->queue;
 	struct mmc_packed *packed = mq_rq->packed;
 
-	BUG_ON(!packed);
-
 	while (!list_empty(&packed->list)) {
 		prq = list_entry_rq(packed->list.prev);
 		if (prq->queuelist.prev != &packed->list) {
@@ -2144,7 +2145,7 @@ static int mmc_blk_issue_rw_rq(struct mmc_queue *mq, struct request *rqc)
 	return 0;
 }
 
-static int mmc_blk_issue_rq(struct mmc_queue *mq, struct request *req)
+int mmc_blk_issue_rq(struct mmc_queue *mq, struct request *req)
 {
 	int ret;
 	struct mmc_blk_data *md = mq->data;
@@ -2265,7 +2266,6 @@ again:
 	if (ret)
 		goto err_putdisk;
 
-	md->queue.issue_fn = mmc_blk_issue_rq;
 	md->queue.data = md;
 
 	md->disk->major	= MMC_BLOCK_MAJOR;
@@ -2303,7 +2303,8 @@ again:
 	set_capacity(md->disk, size);
 
 	if (mmc_host_cmd23(card->host)) {
-		if (mmc_card_mmc(card) ||
+		if ((mmc_card_mmc(card) &&
+		     card->csd.mmca_vsn >= CSD_SPEC_VER_3) ||
 		    (mmc_card_sd(card) &&
 		     card->scr.cmds & SD_SCR_CMD23_SUPPORT))
 			md->flags |= MMC_BLK_CMD23;
diff --git a/drivers/mmc/card/block.h b/drivers/mmc/card/block.h
new file mode 100644
index 000000000000..cdabb2ee74be
--- /dev/null
+++ b/drivers/mmc/card/block.h
@@ -0,0 +1 @@
+int mmc_blk_issue_rq(struct mmc_queue *mq, struct request *req);
diff --git a/drivers/mmc/card/mmc_test.c b/drivers/mmc/card/mmc_test.c
index c032eef45762..5a8dc5a76e0d 100644
--- a/drivers/mmc/card/mmc_test.c
+++ b/drivers/mmc/card/mmc_test.c
@@ -184,6 +184,29 @@ static int mmc_test_set_blksize(struct mmc_test_card *test, unsigned size)
 	return mmc_set_blocklen(test->card, size);
 }
 
+static bool mmc_test_card_cmd23(struct mmc_card *card)
+{
+	return mmc_card_mmc(card) ||
+	       (mmc_card_sd(card) && card->scr.cmds & SD_SCR_CMD23_SUPPORT);
+}
+
+static void mmc_test_prepare_sbc(struct mmc_test_card *test,
+				 struct mmc_request *mrq, unsigned int blocks)
+{
+	struct mmc_card *card = test->card;
+
+	if (!mrq->sbc || !mmc_host_cmd23(card->host) ||
+	    !mmc_test_card_cmd23(card) || !mmc_op_multi(mrq->cmd->opcode) ||
+	    (card->quirks & MMC_QUIRK_BLK_NO_CMD23)) {
+		mrq->sbc = NULL;
+		return;
+	}
+
+	mrq->sbc->opcode = MMC_SET_BLOCK_COUNT;
+	mrq->sbc->arg = blocks;
+	mrq->sbc->flags = MMC_RSP_R1 | MMC_CMD_AC;
+}
+
 /*
  * Fill in the mmc_request structure given a set of transfer parameters.
  */
@@ -221,6 +244,8 @@ static void mmc_test_prepare_mrq(struct mmc_test_card *test,
 	mrq->data->sg = sg;
 	mrq->data->sg_len = sg_len;
 
+	mmc_test_prepare_sbc(test, mrq, blocks);
+
 	mmc_set_data_timeout(mrq->data, test->card);
 }
 
@@ -693,6 +718,8 @@ static int mmc_test_check_result(struct mmc_test_card *test,
 
 	ret = 0;
 
+	if (mrq->sbc && mrq->sbc->error)
+		ret = mrq->sbc->error;
 	if (!ret && mrq->cmd->error)
 		ret = mrq->cmd->error;
 	if (!ret && mrq->data->error)
@@ -2278,6 +2305,245 @@ static int mmc_test_reset(struct mmc_test_card *test)
 	return RESULT_FAIL;
 }
 
+struct mmc_test_req {
+	struct mmc_request mrq;
+	struct mmc_command sbc;
+	struct mmc_command cmd;
+	struct mmc_command stop;
+	struct mmc_command status;
+	struct mmc_data data;
+};
+
+static struct mmc_test_req *mmc_test_req_alloc(void)
+{
+	struct mmc_test_req *rq = kzalloc(sizeof(*rq), GFP_KERNEL);
+
+	if (rq) {
+		rq->mrq.cmd = &rq->cmd;
+		rq->mrq.data = &rq->data;
+		rq->mrq.stop = &rq->stop;
+	}
+
+	return rq;
+}
+
+static int mmc_test_send_status(struct mmc_test_card *test,
+				struct mmc_command *cmd)
+{
+	memset(cmd, 0, sizeof(*cmd));
+
+	cmd->opcode = MMC_SEND_STATUS;
+	if (!mmc_host_is_spi(test->card->host))
+		cmd->arg = test->card->rca << 16;
+	cmd->flags = MMC_RSP_SPI_R2 | MMC_RSP_R1 | MMC_CMD_AC;
+
+	return mmc_wait_for_cmd(test->card->host, cmd, 0);
+}
+
+static int mmc_test_ongoing_transfer(struct mmc_test_card *test,
+				     unsigned int dev_addr, int use_sbc,
+				     int repeat_cmd, int write, int use_areq)
+{
+	struct mmc_test_req *rq = mmc_test_req_alloc();
+	struct mmc_host *host = test->card->host;
+	struct mmc_test_area *t = &test->area;
+	struct mmc_async_req areq;
+	struct mmc_request *mrq;
+	unsigned long timeout;
+	bool expired = false;
+	int ret = 0, cmd_ret;
+	u32 status = 0;
+	int count = 0;
+
+	if (!rq)
+		return -ENOMEM;
+
+	mrq = &rq->mrq;
+	if (use_sbc)
+		mrq->sbc = &rq->sbc;
+	mrq->cap_cmd_during_tfr = true;
+
+	areq.mrq = mrq;
+	areq.err_check = mmc_test_check_result_async;
+
+	mmc_test_prepare_mrq(test, mrq, t->sg, t->sg_len, dev_addr, t->blocks,
+			     512, write);
+
+	if (use_sbc && t->blocks > 1 && !mrq->sbc) {
+		ret =  mmc_host_cmd23(host) ?
+		       RESULT_UNSUP_CARD :
+		       RESULT_UNSUP_HOST;
+		goto out_free;
+	}
+
+	/* Start ongoing data request */
+	if (use_areq) {
+		mmc_start_req(host, &areq, &ret);
+		if (ret)
+			goto out_free;
+	} else {
+		mmc_wait_for_req(host, mrq);
+	}
+
+	timeout = jiffies + msecs_to_jiffies(3000);
+	do {
+		count += 1;
+
+		/* Send status command while data transfer in progress */
+		cmd_ret = mmc_test_send_status(test, &rq->status);
+		if (cmd_ret)
+			break;
+
+		status = rq->status.resp[0];
+		if (status & R1_ERROR) {
+			cmd_ret = -EIO;
+			break;
+		}
+
+		if (mmc_is_req_done(host, mrq))
+			break;
+
+		expired = time_after(jiffies, timeout);
+		if (expired) {
+			pr_info("%s: timeout waiting for Tran state status %#x\n",
+				mmc_hostname(host), status);
+			cmd_ret = -ETIMEDOUT;
+			break;
+		}
+	} while (repeat_cmd && R1_CURRENT_STATE(status) != R1_STATE_TRAN);
+
+	/* Wait for data request to complete */
+	if (use_areq)
+		mmc_start_req(host, NULL, &ret);
+	else
+		mmc_wait_for_req_done(test->card->host, mrq);
+
+	/*
+	 * For cap_cmd_during_tfr request, upper layer must send stop if
+	 * required.
+	 */
+	if (mrq->data->stop && (mrq->data->error || !mrq->sbc)) {
+		if (ret)
+			mmc_wait_for_cmd(host, mrq->data->stop, 0);
+		else
+			ret = mmc_wait_for_cmd(host, mrq->data->stop, 0);
+	}
+
+	if (ret)
+		goto out_free;
+
+	if (cmd_ret) {
+		pr_info("%s: Send Status failed: status %#x, error %d\n",
+			mmc_hostname(test->card->host), status, cmd_ret);
+	}
+
+	ret = mmc_test_check_result(test, mrq);
+	if (ret)
+		goto out_free;
+
+	ret = mmc_test_wait_busy(test);
+	if (ret)
+		goto out_free;
+
+	if (repeat_cmd && (t->blocks + 1) << 9 > t->max_tfr)
+		pr_info("%s: %d commands completed during transfer of %u blocks\n",
+			mmc_hostname(test->card->host), count, t->blocks);
+
+	if (cmd_ret)
+		ret = cmd_ret;
+out_free:
+	kfree(rq);
+
+	return ret;
+}
+
+static int __mmc_test_cmds_during_tfr(struct mmc_test_card *test,
+				      unsigned long sz, int use_sbc, int write,
+				      int use_areq)
+{
+	struct mmc_test_area *t = &test->area;
+	int ret;
+
+	if (!(test->card->host->caps & MMC_CAP_CMD_DURING_TFR))
+		return RESULT_UNSUP_HOST;
+
+	ret = mmc_test_area_map(test, sz, 0, 0);
+	if (ret)
+		return ret;
+
+	ret = mmc_test_ongoing_transfer(test, t->dev_addr, use_sbc, 0, write,
+					use_areq);
+	if (ret)
+		return ret;
+
+	return mmc_test_ongoing_transfer(test, t->dev_addr, use_sbc, 1, write,
+					 use_areq);
+}
+
+static int mmc_test_cmds_during_tfr(struct mmc_test_card *test, int use_sbc,
+				    int write, int use_areq)
+{
+	struct mmc_test_area *t = &test->area;
+	unsigned long sz;
+	int ret;
+
+	for (sz = 512; sz <= t->max_tfr; sz += 512) {
+		ret = __mmc_test_cmds_during_tfr(test, sz, use_sbc, write,
+						 use_areq);
+		if (ret)
+			return ret;
+	}
+	return 0;
+}
+
+/*
+ * Commands during read - no Set Block Count (CMD23).
+ */
+static int mmc_test_cmds_during_read(struct mmc_test_card *test)
+{
+	return mmc_test_cmds_during_tfr(test, 0, 0, 0);
+}
+
+/*
+ * Commands during write - no Set Block Count (CMD23).
+ */
+static int mmc_test_cmds_during_write(struct mmc_test_card *test)
+{
+	return mmc_test_cmds_during_tfr(test, 0, 1, 0);
+}
+
+/*
+ * Commands during read - use Set Block Count (CMD23).
+ */
+static int mmc_test_cmds_during_read_cmd23(struct mmc_test_card *test)
+{
+	return mmc_test_cmds_during_tfr(test, 1, 0, 0);
+}
+
+/*
+ * Commands during write - use Set Block Count (CMD23).
+ */
+static int mmc_test_cmds_during_write_cmd23(struct mmc_test_card *test)
+{
+	return mmc_test_cmds_during_tfr(test, 1, 1, 0);
+}
+
+/*
+ * Commands during non-blocking read - use Set Block Count (CMD23).
+ */
+static int mmc_test_cmds_during_read_cmd23_nonblock(struct mmc_test_card *test)
+{
+	return mmc_test_cmds_during_tfr(test, 1, 0, 1);
+}
+
+/*
+ * Commands during non-blocking write - use Set Block Count (CMD23).
+ */
+static int mmc_test_cmds_during_write_cmd23_nonblock(struct mmc_test_card *test)
+{
+	return mmc_test_cmds_during_tfr(test, 1, 1, 1);
+}
+
 static const struct mmc_test_case mmc_test_cases[] = {
 	{
 		.name = "Basic write (no data verification)",
@@ -2605,6 +2871,48 @@ static const struct mmc_test_case mmc_test_cases[] = {
 		.name = "Reset test",
 		.run = mmc_test_reset,
 	},
+
+	{
+		.name = "Commands during read - no Set Block Count (CMD23)",
+		.prepare = mmc_test_area_prepare,
+		.run = mmc_test_cmds_during_read,
+		.cleanup = mmc_test_area_cleanup,
+	},
+
+	{
+		.name = "Commands during write - no Set Block Count (CMD23)",
+		.prepare = mmc_test_area_prepare,
+		.run = mmc_test_cmds_during_write,
+		.cleanup = mmc_test_area_cleanup,
+	},
+
+	{
+		.name = "Commands during read - use Set Block Count (CMD23)",
+		.prepare = mmc_test_area_prepare,
+		.run = mmc_test_cmds_during_read_cmd23,
+		.cleanup = mmc_test_area_cleanup,
+	},
+
+	{
+		.name = "Commands during write - use Set Block Count (CMD23)",
+		.prepare = mmc_test_area_prepare,
+		.run = mmc_test_cmds_during_write_cmd23,
+		.cleanup = mmc_test_area_cleanup,
+	},
+
+	{
+		.name = "Commands during non-blocking read - use Set Block Count (CMD23)",
+		.prepare = mmc_test_area_prepare,
+		.run = mmc_test_cmds_during_read_cmd23_nonblock,
+		.cleanup = mmc_test_area_cleanup,
+	},
+
+	{
+		.name = "Commands during non-blocking write - use Set Block Count (CMD23)",
+		.prepare = mmc_test_area_prepare,
+		.run = mmc_test_cmds_during_write_cmd23_nonblock,
+		.cleanup = mmc_test_area_cleanup,
+	},
 };
 
 static DEFINE_MUTEX(mmc_test_lock);
diff --git a/drivers/mmc/card/queue.c b/drivers/mmc/card/queue.c
index 708057261b38..8037f73a109a 100644
--- a/drivers/mmc/card/queue.c
+++ b/drivers/mmc/card/queue.c
@@ -19,7 +19,9 @@
 
 #include <linux/mmc/card.h>
 #include <linux/mmc/host.h>
+
 #include "queue.h"
+#include "block.h"
 
 #define MMC_QUEUE_BOUNCESZ	65536
 
@@ -68,7 +70,7 @@ static int mmc_queue_thread(void *d)
 			bool req_is_special = mmc_req_is_special(req);
 
 			set_current_state(TASK_RUNNING);
-			mq->issue_fn(mq, req);
+			mmc_blk_issue_rq(mq, req);
 			cond_resched();
 			if (mq->flags & MMC_QUEUE_NEW_REQUEST) {
 				mq->flags &= ~MMC_QUEUE_NEW_REQUEST;
diff --git a/drivers/mmc/card/queue.h b/drivers/mmc/card/queue.h
index fee5e1271465..342f1e3f301e 100644
--- a/drivers/mmc/card/queue.h
+++ b/drivers/mmc/card/queue.h
@@ -31,7 +31,7 @@ enum mmc_packed_type {
 
 struct mmc_packed {
 	struct list_head	list;
-	u32			cmd_hdr[1024];
+	__le32			cmd_hdr[1024];
 	unsigned int		blocks;
 	u8			nr_entries;
 	u8			retries;
@@ -57,8 +57,6 @@ struct mmc_queue {
 	unsigned int		flags;
 #define MMC_QUEUE_SUSPENDED	(1 << 0)
 #define MMC_QUEUE_NEW_REQUEST	(1 << 1)
-
-	int			(*issue_fn)(struct mmc_queue *, struct request *);
 	void			*data;
 	struct request_queue	*queue;
 	struct mmc_queue_req	mqrq[2];
diff --git a/drivers/mmc/core/core.c b/drivers/mmc/core/core.c
index e55cde6d436d..2553d903a82b 100644
--- a/drivers/mmc/core/core.c
+++ b/drivers/mmc/core/core.c
@@ -58,6 +58,9 @@
  */
 #define MMC_BKOPS_MAX_TIMEOUT	(4 * 60 * 1000) /* max time to wait in ms */
 
+/* The max erase timeout, used when host->max_busy_timeout isn't specified */
+#define MMC_ERASE_TIMEOUT_MS	(60 * 1000) /* 60 s */
+
 static const unsigned freqs[] = { 400000, 300000, 200000, 100000 };
 
 /*
@@ -117,6 +120,24 @@ static inline void mmc_should_fail_request(struct mmc_host *host,
 
 #endif /* CONFIG_FAIL_MMC_REQUEST */
 
+static inline void mmc_complete_cmd(struct mmc_request *mrq)
+{
+	if (mrq->cap_cmd_during_tfr && !completion_done(&mrq->cmd_completion))
+		complete_all(&mrq->cmd_completion);
+}
+
+void mmc_command_done(struct mmc_host *host, struct mmc_request *mrq)
+{
+	if (!mrq->cap_cmd_during_tfr)
+		return;
+
+	mmc_complete_cmd(mrq);
+
+	pr_debug("%s: cmd done, tfr ongoing (CMD%u)\n",
+		 mmc_hostname(host), mrq->cmd->opcode);
+}
+EXPORT_SYMBOL(mmc_command_done);
+
 /**
  *	mmc_request_done - finish processing an MMC request
  *	@host: MMC host which completed request
@@ -143,6 +164,11 @@ void mmc_request_done(struct mmc_host *host, struct mmc_request *mrq)
 			cmd->retries = 0;
 	}
 
+	if (host->ongoing_mrq == mrq)
+		host->ongoing_mrq = NULL;
+
+	mmc_complete_cmd(mrq);
+
 	trace_mmc_request_done(host, mrq);
 
 	if (err && cmd->retries && !mmc_card_removed(host->card)) {
@@ -155,7 +181,8 @@ void mmc_request_done(struct mmc_host *host, struct mmc_request *mrq)
 	} else {
 		mmc_should_fail_request(host, mrq);
 
-		led_trigger_event(host->led, LED_OFF);
+		if (!host->ongoing_mrq)
+			led_trigger_event(host->led, LED_OFF);
 
 		if (mrq->sbc) {
 			pr_debug("%s: req done <CMD%u>: %d: %08x %08x %08x %08x\n",
@@ -220,6 +247,15 @@ static void __mmc_start_request(struct mmc_host *host, struct mmc_request *mrq)
 		}
 	}
 
+	if (mrq->cap_cmd_during_tfr) {
+		host->ongoing_mrq = mrq;
+		/*
+		 * Retry path could come through here without having waiting on
+		 * cmd_completion, so ensure it is reinitialised.
+		 */
+		reinit_completion(&mrq->cmd_completion);
+	}
+
 	trace_mmc_request_start(host, mrq);
 
 	host->ops->request(host, mrq);
@@ -386,6 +422,18 @@ static void mmc_wait_done(struct mmc_request *mrq)
 	complete(&mrq->completion);
 }
 
+static inline void mmc_wait_ongoing_tfr_cmd(struct mmc_host *host)
+{
+	struct mmc_request *ongoing_mrq = READ_ONCE(host->ongoing_mrq);
+
+	/*
+	 * If there is an ongoing transfer, wait for the command line to become
+	 * available.
+	 */
+	if (ongoing_mrq && !completion_done(&ongoing_mrq->cmd_completion))
+		wait_for_completion(&ongoing_mrq->cmd_completion);
+}
+
 /*
  *__mmc_start_data_req() - starts data request
  * @host: MMC host to start the request
@@ -393,17 +441,24 @@ static void mmc_wait_done(struct mmc_request *mrq)
  *
  * Sets the done callback to be called when request is completed by the card.
  * Starts data mmc request execution
+ * If an ongoing transfer is already in progress, wait for the command line
+ * to become available before sending another command.
  */
 static int __mmc_start_data_req(struct mmc_host *host, struct mmc_request *mrq)
 {
 	int err;
 
+	mmc_wait_ongoing_tfr_cmd(host);
+
 	mrq->done = mmc_wait_data_done;
 	mrq->host = host;
 
+	init_completion(&mrq->cmd_completion);
+
 	err = mmc_start_request(host, mrq);
 	if (err) {
 		mrq->cmd->error = err;
+		mmc_complete_cmd(mrq);
 		mmc_wait_data_done(mrq);
 	}
 
@@ -414,12 +469,17 @@ static int __mmc_start_req(struct mmc_host *host, struct mmc_request *mrq)
 {
 	int err;
 
+	mmc_wait_ongoing_tfr_cmd(host);
+
 	init_completion(&mrq->completion);
 	mrq->done = mmc_wait_done;
 
+	init_completion(&mrq->cmd_completion);
+
 	err = mmc_start_request(host, mrq);
 	if (err) {
 		mrq->cmd->error = err;
+		mmc_complete_cmd(mrq);
 		complete(&mrq->completion);
 	}
 
@@ -483,8 +543,7 @@ static int mmc_wait_for_data_req_done(struct mmc_host *host,
 	return err;
 }
 
-static void mmc_wait_for_req_done(struct mmc_host *host,
-				  struct mmc_request *mrq)
+void mmc_wait_for_req_done(struct mmc_host *host, struct mmc_request *mrq)
 {
 	struct mmc_command *cmd;
 
@@ -525,6 +584,28 @@ static void mmc_wait_for_req_done(struct mmc_host *host,
 
 	mmc_retune_release(host);
 }
+EXPORT_SYMBOL(mmc_wait_for_req_done);
+
+/**
+ *	mmc_is_req_done - Determine if a 'cap_cmd_during_tfr' request is done
+ *	@host: MMC host
+ *	@mrq: MMC request
+ *
+ *	mmc_is_req_done() is used with requests that have
+ *	mrq->cap_cmd_during_tfr = true. mmc_is_req_done() must be called after
+ *	starting a request and before waiting for it to complete. That is,
+ *	either in between calls to mmc_start_req(), or after mmc_wait_for_req()
+ *	and before mmc_wait_for_req_done(). If it is called at other times the
+ *	result is not meaningful.
+ */
+bool mmc_is_req_done(struct mmc_host *host, struct mmc_request *mrq)
+{
+	if (host->areq)
+		return host->context_info.is_done_rcv;
+	else
+		return completion_done(&mrq->completion);
+}
+EXPORT_SYMBOL(mmc_is_req_done);
 
 /**
  *	mmc_pre_req - Prepare for a new request
@@ -645,13 +726,18 @@ EXPORT_SYMBOL(mmc_start_req);
  *	@mrq: MMC request to start
  *
  *	Start a new MMC custom command request for a host, and wait
- *	for the command to complete. Does not attempt to parse the
- *	response.
+ *	for the command to complete. In the case of 'cap_cmd_during_tfr'
+ *	requests, the transfer is ongoing and the caller can issue further
+ *	commands that do not use the data lines, and then wait by calling
+ *	mmc_wait_for_req_done().
+ *	Does not attempt to parse the response.
  */
 void mmc_wait_for_req(struct mmc_host *host, struct mmc_request *mrq)
 {
 	__mmc_start_req(host, mrq);
-	mmc_wait_for_req_done(host, mrq);
+
+	if (!mrq->cap_cmd_during_tfr)
+		mmc_wait_for_req_done(host, mrq);
 }
 EXPORT_SYMBOL(mmc_wait_for_req);
 
@@ -2202,6 +2288,54 @@ out:
 	return err;
 }
 
+static unsigned int mmc_align_erase_size(struct mmc_card *card,
+					 unsigned int *from,
+					 unsigned int *to,
+					 unsigned int nr)
+{
+	unsigned int from_new = *from, nr_new = nr, rem;
+
+	/*
+	 * When the 'card->erase_size' is power of 2, we can use round_up/down()
+	 * to align the erase size efficiently.
+	 */
+	if (is_power_of_2(card->erase_size)) {
+		unsigned int temp = from_new;
+
+		from_new = round_up(temp, card->erase_size);
+		rem = from_new - temp;
+
+		if (nr_new > rem)
+			nr_new -= rem;
+		else
+			return 0;
+
+		nr_new = round_down(nr_new, card->erase_size);
+	} else {
+		rem = from_new % card->erase_size;
+		if (rem) {
+			rem = card->erase_size - rem;
+			from_new += rem;
+			if (nr_new > rem)
+				nr_new -= rem;
+			else
+				return 0;
+		}
+
+		rem = nr_new % card->erase_size;
+		if (rem)
+			nr_new -= rem;
+	}
+
+	if (nr_new == 0)
+		return 0;
+
+	*to = from_new + nr_new;
+	*from = from_new;
+
+	return nr_new;
+}
+
 /**
  * mmc_erase - erase sectors.
  * @card: card to erase
@@ -2240,26 +2374,12 @@ int mmc_erase(struct mmc_card *card, unsigned int from, unsigned int nr,
 			return -EINVAL;
 	}
 
-	if (arg == MMC_ERASE_ARG) {
-		rem = from % card->erase_size;
-		if (rem) {
-			rem = card->erase_size - rem;
-			from += rem;
-			if (nr > rem)
-				nr -= rem;
-			else
-				return 0;
-		}
-		rem = nr % card->erase_size;
-		if (rem)
-			nr -= rem;
-	}
+	if (arg == MMC_ERASE_ARG)
+		nr = mmc_align_erase_size(card, &from, &to, nr);
 
 	if (nr == 0)
 		return 0;
 
-	to = from + nr;
-
 	if (to <= from)
 		return -EINVAL;
 
@@ -2352,6 +2472,8 @@ static unsigned int mmc_do_calc_max_discard(struct mmc_card *card,
 	struct mmc_host *host = card->host;
 	unsigned int max_discard, x, y, qty = 0, max_qty, min_qty, timeout;
 	unsigned int last_timeout = 0;
+	unsigned int max_busy_timeout = host->max_busy_timeout ?
+			host->max_busy_timeout : MMC_ERASE_TIMEOUT_MS;
 
 	if (card->erase_shift) {
 		max_qty = UINT_MAX >> card->erase_shift;
@@ -2374,15 +2496,15 @@ static unsigned int mmc_do_calc_max_discard(struct mmc_card *card,
 	 * matter what size of 'host->max_busy_timeout', but if the
 	 * 'host->max_busy_timeout' is large enough for more discard sectors,
 	 * then we can continue to increase the max discard sectors until we
-	 * get a balance value.
+	 * get a balance value. In cases when the 'host->max_busy_timeout'
+	 * isn't specified, use the default max erase timeout.
 	 */
 	do {
 		y = 0;
 		for (x = 1; x && x <= max_qty && max_qty - x >= qty; x <<= 1) {
 			timeout = mmc_erase_timeout(card, arg, qty + x);
 
-			if (qty + x > min_qty &&
-			    timeout > host->max_busy_timeout)
+			if (qty + x > min_qty && timeout > max_busy_timeout)
 				break;
 
 			if (timeout < last_timeout)
@@ -2427,9 +2549,6 @@ unsigned int mmc_calc_max_discard(struct mmc_card *card)
 	struct mmc_host *host = card->host;
 	unsigned int max_discard, max_trim;
 
-	if (!host->max_busy_timeout)
-		return UINT_MAX;
-
 	/*
 	 * Without erase_group_def set, MMC erase timeout depends on clock
 	 * frequence which can change.  In that case, the best choice is
@@ -2447,7 +2566,8 @@ unsigned int mmc_calc_max_discard(struct mmc_card *card)
 		max_discard = 0;
 	}
 	pr_debug("%s: calculated max. discard sectors %u for timeout %u ms\n",
-		 mmc_hostname(host), max_discard, host->max_busy_timeout);
+		mmc_hostname(host), max_discard, host->max_busy_timeout ?
+		host->max_busy_timeout : MMC_ERASE_TIMEOUT_MS);
 	return max_discard;
 }
 EXPORT_SYMBOL(mmc_calc_max_discard);
@@ -2456,7 +2576,8 @@ int mmc_set_blocklen(struct mmc_card *card, unsigned int blocklen)
 {
 	struct mmc_command cmd = {0};
 
-	if (mmc_card_blockaddr(card) || mmc_card_ddr52(card))
+	if (mmc_card_blockaddr(card) || mmc_card_ddr52(card) ||
+	    mmc_card_hs400(card) || mmc_card_hs400es(card))
 		return 0;
 
 	cmd.opcode = MMC_SET_BLOCKLEN;
diff --git a/drivers/mmc/core/mmc.c b/drivers/mmc/core/mmc.c
index f2d185cf8a8b..39fc5b2b96c5 100644
--- a/drivers/mmc/core/mmc.c
+++ b/drivers/mmc/core/mmc.c
@@ -1029,6 +1029,10 @@ static int mmc_select_hs(struct mmc_card *card)
 		err = mmc_switch_status(card);
 	}
 
+	if (err)
+		pr_warn("%s: switch to high-speed failed, err:%d\n",
+			mmc_hostname(card->host), err);
+
 	return err;
 }
 
@@ -1259,17 +1263,26 @@ static int mmc_select_hs400es(struct mmc_card *card)
 		goto out_err;
 	}
 
+	if (card->mmc_avail_type & EXT_CSD_CARD_TYPE_HS400_1_2V)
+		err = __mmc_set_signal_voltage(host, MMC_SIGNAL_VOLTAGE_120);
+
+	if (err && card->mmc_avail_type & EXT_CSD_CARD_TYPE_HS400_1_8V)
+		err = __mmc_set_signal_voltage(host, MMC_SIGNAL_VOLTAGE_180);
+
+	/* If fails try again during next card power cycle */
+	if (err)
+		goto out_err;
+
 	err = mmc_select_bus_width(card);
 	if (err < 0)
 		goto out_err;
 
 	/* Switch card to HS mode */
 	err = mmc_select_hs(card);
-	if (err) {
-		pr_err("%s: switch to high-speed failed, err:%d\n",
-			mmc_hostname(host), err);
+	if (err)
 		goto out_err;
-	}
+
+	mmc_set_clock(host, card->ext_csd.hs_max_dtr);
 
 	err = mmc_switch_status(card);
 	if (err)
diff --git a/drivers/mmc/core/pwrseq_simple.c b/drivers/mmc/core/pwrseq_simple.c
index 450d907c6e6c..1304160de168 100644
--- a/drivers/mmc/core/pwrseq_simple.c
+++ b/drivers/mmc/core/pwrseq_simple.c
@@ -16,6 +16,8 @@
 #include <linux/device.h>
 #include <linux/err.h>
 #include <linux/gpio/consumer.h>
+#include <linux/delay.h>
+#include <linux/property.h>
 
 #include <linux/mmc/host.h>
 
@@ -24,6 +26,7 @@
 struct mmc_pwrseq_simple {
 	struct mmc_pwrseq pwrseq;
 	bool clk_enabled;
+	u32 post_power_on_delay_ms;
 	struct clk *ext_clk;
 	struct gpio_descs *reset_gpios;
 };
@@ -64,6 +67,9 @@ static void mmc_pwrseq_simple_post_power_on(struct mmc_host *host)
 	struct mmc_pwrseq_simple *pwrseq = to_pwrseq_simple(host->pwrseq);
 
 	mmc_pwrseq_simple_set_gpios_value(pwrseq, 0);
+
+	if (pwrseq->post_power_on_delay_ms)
+		msleep(pwrseq->post_power_on_delay_ms);
 }
 
 static void mmc_pwrseq_simple_power_off(struct mmc_host *host)
@@ -111,6 +117,9 @@ static int mmc_pwrseq_simple_probe(struct platform_device *pdev)
 		return PTR_ERR(pwrseq->reset_gpios);
 	}
 
+	device_property_read_u32(dev, "post-power-on-delay-ms",
+				 &pwrseq->post_power_on_delay_ms);
+
 	pwrseq->pwrseq.dev = dev;
 	pwrseq->pwrseq.ops = &mmc_pwrseq_simple_ops;
 	pwrseq->pwrseq.owner = THIS_MODULE;
diff --git a/drivers/mmc/core/sd.c b/drivers/mmc/core/sd.c
index 0123936241b0..73c762a28dfe 100644
--- a/drivers/mmc/core/sd.c
+++ b/drivers/mmc/core/sd.c
@@ -223,8 +223,7 @@ static int mmc_decode_scr(struct mmc_card *card)
 static int mmc_read_ssr(struct mmc_card *card)
 {
 	unsigned int au, es, et, eo;
-	int err, i;
-	u32 *ssr;
+	int i;
 
 	if (!(card->csd.cmdclass & CCC_APP_SPEC)) {
 		pr_warn("%s: card lacks mandatory SD Status function\n",
@@ -232,33 +231,27 @@ static int mmc_read_ssr(struct mmc_card *card)
 		return 0;
 	}
 
-	ssr = kmalloc(64, GFP_KERNEL);
-	if (!ssr)
-		return -ENOMEM;
-
-	err = mmc_app_sd_status(card, ssr);
-	if (err) {
+	if (mmc_app_sd_status(card, card->raw_ssr)) {
 		pr_warn("%s: problem reading SD Status register\n",
 			mmc_hostname(card->host));
-		err = 0;
-		goto out;
+		return 0;
 	}
 
 	for (i = 0; i < 16; i++)
-		ssr[i] = be32_to_cpu(ssr[i]);
+		card->raw_ssr[i] = be32_to_cpu(card->raw_ssr[i]);
 
 	/*
 	 * UNSTUFF_BITS only works with four u32s so we have to offset the
 	 * bitfield positions accordingly.
 	 */
-	au = UNSTUFF_BITS(ssr, 428 - 384, 4);
+	au = UNSTUFF_BITS(card->raw_ssr, 428 - 384, 4);
 	if (au) {
 		if (au <= 9 || card->scr.sda_spec3) {
 			card->ssr.au = sd_au_size[au];
-			es = UNSTUFF_BITS(ssr, 408 - 384, 16);
-			et = UNSTUFF_BITS(ssr, 402 - 384, 6);
+			es = UNSTUFF_BITS(card->raw_ssr, 408 - 384, 16);
+			et = UNSTUFF_BITS(card->raw_ssr, 402 - 384, 6);
 			if (es && et) {
-				eo = UNSTUFF_BITS(ssr, 400 - 384, 2);
+				eo = UNSTUFF_BITS(card->raw_ssr, 400 - 384, 2);
 				card->ssr.erase_timeout = (et * 1000) / es;
 				card->ssr.erase_offset = eo * 1000;
 			}
@@ -267,9 +260,8 @@ static int mmc_read_ssr(struct mmc_card *card)
 				mmc_hostname(card->host));
 		}
 	}
-out:
-	kfree(ssr);
-	return err;
+
+	return 0;
 }
 
 /*
@@ -666,6 +658,14 @@ MMC_DEV_ATTR(cid, "%08x%08x%08x%08x\n", card->raw_cid[0], card->raw_cid[1],
 MMC_DEV_ATTR(csd, "%08x%08x%08x%08x\n", card->raw_csd[0], card->raw_csd[1],
 	card->raw_csd[2], card->raw_csd[3]);
 MMC_DEV_ATTR(scr, "%08x%08x\n", card->raw_scr[0], card->raw_scr[1]);
+MMC_DEV_ATTR(ssr,
+	"%08x%08x%08x%08x%08x%08x%08x%08x%08x%08x%08x%08x%08x%08x%08x%08x\n",
+		card->raw_ssr[0], card->raw_ssr[1], card->raw_ssr[2],
+		card->raw_ssr[3], card->raw_ssr[4], card->raw_ssr[5],
+		card->raw_ssr[6], card->raw_ssr[7], card->raw_ssr[8],
+		card->raw_ssr[9], card->raw_ssr[10], card->raw_ssr[11],
+		card->raw_ssr[12], card->raw_ssr[13], card->raw_ssr[14],
+		card->raw_ssr[15]);
 MMC_DEV_ATTR(date, "%02d/%04d\n", card->cid.month, card->cid.year);
 MMC_DEV_ATTR(erase_size, "%u\n", card->erase_size << 9);
 MMC_DEV_ATTR(preferred_erase_size, "%u\n", card->pref_erase << 9);
@@ -698,6 +698,7 @@ static struct attribute *sd_std_attrs[] = {
 	&dev_attr_cid.attr,
 	&dev_attr_csd.attr,
 	&dev_attr_scr.attr,
+	&dev_attr_ssr.attr,
 	&dev_attr_date.attr,
 	&dev_attr_erase_size.attr,
 	&dev_attr_preferred_erase_size.attr,
diff --git a/drivers/mmc/core/sdio_io.c b/drivers/mmc/core/sdio_io.c
index 78cb4d5d9d58..406e5f037e32 100644
--- a/drivers/mmc/core/sdio_io.c
+++ b/drivers/mmc/core/sdio_io.c
@@ -26,8 +26,8 @@
  */
 void sdio_claim_host(struct sdio_func *func)
 {
-	BUG_ON(!func);
-	BUG_ON(!func->card);
+	if (WARN_ON(!func))
+		return;
 
 	mmc_claim_host(func->card->host);
 }
@@ -42,8 +42,8 @@ EXPORT_SYMBOL_GPL(sdio_claim_host);
  */
 void sdio_release_host(struct sdio_func *func)
 {
-	BUG_ON(!func);
-	BUG_ON(!func->card);
+	if (WARN_ON(!func))
+		return;
 
 	mmc_release_host(func->card->host);
 }
@@ -62,8 +62,8 @@ int sdio_enable_func(struct sdio_func *func)
 	unsigned char reg;
 	unsigned long timeout;
 
-	BUG_ON(!func);
-	BUG_ON(!func->card);
+	if (!func)
+		return -EINVAL;
 
 	pr_debug("SDIO: Enabling device %s...\n", sdio_func_id(func));
 
@@ -112,8 +112,8 @@ int sdio_disable_func(struct sdio_func *func)
 	int ret;
 	unsigned char reg;
 
-	BUG_ON(!func);
-	BUG_ON(!func->card);
+	if (!func)
+		return -EINVAL;
 
 	pr_debug("SDIO: Disabling device %s...\n", sdio_func_id(func));
 
@@ -307,6 +307,9 @@ static int sdio_io_rw_ext_helper(struct sdio_func *func, int write,
 	unsigned max_blocks;
 	int ret;
 
+	if (!func || (func->num > 7))
+		return -EINVAL;
+
 	/* Do the bulk of the transfer using block mode (if supported). */
 	if (func->card->cccr.multi_block && (size > sdio_max_byte_size(func))) {
 		/* Blocks per command is limited by host count, host transfer
@@ -367,7 +370,10 @@ u8 sdio_readb(struct sdio_func *func, unsigned int addr, int *err_ret)
 	int ret;
 	u8 val;
 
-	BUG_ON(!func);
+	if (!func) {
+		*err_ret = -EINVAL;
+		return 0xFF;
+	}
 
 	if (err_ret)
 		*err_ret = 0;
@@ -398,7 +404,10 @@ void sdio_writeb(struct sdio_func *func, u8 b, unsigned int addr, int *err_ret)
 {
 	int ret;
 
-	BUG_ON(!func);
+	if (!func) {
+		*err_ret = -EINVAL;
+		return;
+	}
 
 	ret = mmc_io_rw_direct(func->card, 1, func->num, addr, b, NULL);
 	if (err_ret)
@@ -623,7 +632,10 @@ unsigned char sdio_f0_readb(struct sdio_func *func, unsigned int addr,
 	int ret;
 	unsigned char val;
 
-	BUG_ON(!func);
+	if (!func) {
+		*err_ret = -EINVAL;
+		return 0xFF;
+	}
 
 	if (err_ret)
 		*err_ret = 0;
@@ -658,7 +670,10 @@ void sdio_f0_writeb(struct sdio_func *func, unsigned char b, unsigned int addr,
 {
 	int ret;
 
-	BUG_ON(!func);
+	if (!func) {
+		*err_ret = -EINVAL;
+		return;
+	}
 
 	if ((addr < 0xF0 || addr > 0xFF) && (!mmc_card_lenient_fn0(func->card))) {
 		if (err_ret)
@@ -684,8 +699,8 @@ EXPORT_SYMBOL_GPL(sdio_f0_writeb);
  */
 mmc_pm_flag_t sdio_get_host_pm_caps(struct sdio_func *func)
 {
-	BUG_ON(!func);
-	BUG_ON(!func->card);
+	if (!func)
+		return 0;
 
 	return func->card->host->pm_caps;
 }
@@ -707,8 +722,8 @@ int sdio_set_host_pm_flags(struct sdio_func *func, mmc_pm_flag_t flags)
 {
 	struct mmc_host *host;
 
-	BUG_ON(!func);
-	BUG_ON(!func->card);
+	if (!func)
+		return -EINVAL;
 
 	host = func->card->host;
 
diff --git a/drivers/mmc/core/sdio_ops.c b/drivers/mmc/core/sdio_ops.c
index 34f6e8015306..90fe5545c677 100644
--- a/drivers/mmc/core/sdio_ops.c
+++ b/drivers/mmc/core/sdio_ops.c
@@ -24,8 +24,6 @@ int mmc_send_io_op_cond(struct mmc_host *host, u32 ocr, u32 *rocr)
 	struct mmc_command cmd = {0};
 	int i, err = 0;
 
-	BUG_ON(!host);
-
 	cmd.opcode = SD_IO_SEND_OP_COND;
 	cmd.arg = ocr;
 	cmd.flags = MMC_RSP_SPI_R4 | MMC_RSP_R4 | MMC_CMD_BCR;
@@ -71,8 +69,8 @@ static int mmc_io_rw_direct_host(struct mmc_host *host, int write, unsigned fn,
 	struct mmc_command cmd = {0};
 	int err;
 
-	BUG_ON(!host);
-	BUG_ON(fn > 7);
+	if (fn > 7)
+		return -EINVAL;
 
 	/* sanity check */
 	if (addr & ~0x1FFFF)
@@ -114,7 +112,6 @@ static int mmc_io_rw_direct_host(struct mmc_host *host, int write, unsigned fn,
 int mmc_io_rw_direct(struct mmc_card *card, int write, unsigned fn,
 	unsigned addr, u8 in, u8 *out)
 {
-	BUG_ON(!card);
 	return mmc_io_rw_direct_host(card->host, write, fn, addr, in, out);
 }
 
@@ -129,8 +126,6 @@ int mmc_io_rw_extended(struct mmc_card *card, int write, unsigned fn,
 	unsigned int nents, left_size, i;
 	unsigned int seg_size = card->host->max_seg_size;
 
-	BUG_ON(!card);
-	BUG_ON(fn > 7);
 	WARN_ON(blksz == 0);
 
 	/* sanity check */
diff --git a/drivers/mmc/host/davinci_mmc.c b/drivers/mmc/host/davinci_mmc.c
index a56373c75983..8fa478c3b0db 100644
--- a/drivers/mmc/host/davinci_mmc.c
+++ b/drivers/mmc/host/davinci_mmc.c
@@ -1216,9 +1216,11 @@ static int __init davinci_mmcsd_probe(struct platform_device *pdev)
 	}
 
 	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	irq = platform_get_irq(pdev, 0);
-	if (!r || irq == NO_IRQ)
+	if (!r)
 		return -ENODEV;
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0)
+		return irq;
 
 	mem_size = resource_size(r);
 	mem = devm_request_mem_region(&pdev->dev, r->start, mem_size,
diff --git a/drivers/mmc/host/dw_mmc-exynos.c b/drivers/mmc/host/dw_mmc-exynos.c
index da0ef1765735..7ab3d749b5ae 100644
--- a/drivers/mmc/host/dw_mmc-exynos.c
+++ b/drivers/mmc/host/dw_mmc-exynos.c
@@ -225,8 +225,12 @@ static void dw_mci_exynos_config_hs400(struct dw_mci *host, u32 timing)
 	 * Not supported to configure register
 	 * related to HS400
 	 */
-	if (priv->ctrl_type < DW_MCI_TYPE_EXYNOS5420)
+	if (priv->ctrl_type < DW_MCI_TYPE_EXYNOS5420) {
+		if (timing == MMC_TIMING_MMC_HS400)
+			dev_warn(host->dev,
+				 "cannot configure HS400, unsupported chipset\n");
 		return;
+	}
 
 	dqs = priv->saved_dqs_en;
 	strobe = priv->saved_strobe_ctrl;
diff --git a/drivers/mmc/host/dw_mmc-k3.c b/drivers/mmc/host/dw_mmc-k3.c
index 8e9d886bfcda..624789496dce 100644
--- a/drivers/mmc/host/dw_mmc-k3.c
+++ b/drivers/mmc/host/dw_mmc-k3.c
@@ -131,11 +131,17 @@ static void dw_mci_hi6220_set_ios(struct dw_mci *host, struct mmc_ios *ios)
 	host->bus_hz = clk_get_rate(host->biu_clk);
 }
 
+static int dw_mci_hi6220_execute_tuning(struct dw_mci_slot *slot, u32 opcode)
+{
+	return 0;
+}
+
 static const struct dw_mci_drv_data hi6220_data = {
 	.caps			= dw_mci_hi6220_caps,
 	.switch_voltage		= dw_mci_hi6220_switch_voltage,
 	.set_ios		= dw_mci_hi6220_set_ios,
 	.parse_dt		= dw_mci_hi6220_parse_dt,
+	.execute_tuning		= dw_mci_hi6220_execute_tuning,
 };
 
 static const struct of_device_id dw_mci_k3_match[] = {
diff --git a/drivers/mmc/host/dw_mmc.c b/drivers/mmc/host/dw_mmc.c
index 767af2026f8b..4fcbc4012ed0 100644
--- a/drivers/mmc/host/dw_mmc.c
+++ b/drivers/mmc/host/dw_mmc.c
@@ -61,6 +61,8 @@
 				 SDMMC_IDMAC_INT_FBE | SDMMC_IDMAC_INT_RI | \
 				 SDMMC_IDMAC_INT_TI)
 
+#define DESC_RING_BUF_SZ	PAGE_SIZE
+
 struct idmac_desc_64addr {
 	u32		des0;	/* Control Descriptor */
 
@@ -467,136 +469,6 @@ static void dw_mci_dmac_complete_dma(void *arg)
 	}
 }
 
-static void dw_mci_translate_sglist(struct dw_mci *host, struct mmc_data *data,
-				    unsigned int sg_len)
-{
-	unsigned int desc_len;
-	int i;
-
-	if (host->dma_64bit_address == 1) {
-		struct idmac_desc_64addr *desc_first, *desc_last, *desc;
-
-		desc_first = desc_last = desc = host->sg_cpu;
-
-		for (i = 0; i < sg_len; i++) {
-			unsigned int length = sg_dma_len(&data->sg[i]);
-
-			u64 mem_addr = sg_dma_address(&data->sg[i]);
-
-			for ( ; length ; desc++) {
-				desc_len = (length <= DW_MCI_DESC_DATA_LENGTH) ?
-					   length : DW_MCI_DESC_DATA_LENGTH;
-
-				length -= desc_len;
-
-				/*
-				 * Set the OWN bit and disable interrupts
-				 * for this descriptor
-				 */
-				desc->des0 = IDMAC_DES0_OWN | IDMAC_DES0_DIC |
-							IDMAC_DES0_CH;
-
-				/* Buffer length */
-				IDMAC_64ADDR_SET_BUFFER1_SIZE(desc, desc_len);
-
-				/* Physical address to DMA to/from */
-				desc->des4 = mem_addr & 0xffffffff;
-				desc->des5 = mem_addr >> 32;
-
-				/* Update physical address for the next desc */
-				mem_addr += desc_len;
-
-				/* Save pointer to the last descriptor */
-				desc_last = desc;
-			}
-		}
-
-		/* Set first descriptor */
-		desc_first->des0 |= IDMAC_DES0_FD;
-
-		/* Set last descriptor */
-		desc_last->des0 &= ~(IDMAC_DES0_CH | IDMAC_DES0_DIC);
-		desc_last->des0 |= IDMAC_DES0_LD;
-
-	} else {
-		struct idmac_desc *desc_first, *desc_last, *desc;
-
-		desc_first = desc_last = desc = host->sg_cpu;
-
-		for (i = 0; i < sg_len; i++) {
-			unsigned int length = sg_dma_len(&data->sg[i]);
-
-			u32 mem_addr = sg_dma_address(&data->sg[i]);
-
-			for ( ; length ; desc++) {
-				desc_len = (length <= DW_MCI_DESC_DATA_LENGTH) ?
-					   length : DW_MCI_DESC_DATA_LENGTH;
-
-				length -= desc_len;
-
-				/*
-				 * Set the OWN bit and disable interrupts
-				 * for this descriptor
-				 */
-				desc->des0 = cpu_to_le32(IDMAC_DES0_OWN |
-							 IDMAC_DES0_DIC |
-							 IDMAC_DES0_CH);
-
-				/* Buffer length */
-				IDMAC_SET_BUFFER1_SIZE(desc, desc_len);
-
-				/* Physical address to DMA to/from */
-				desc->des2 = cpu_to_le32(mem_addr);
-
-				/* Update physical address for the next desc */
-				mem_addr += desc_len;
-
-				/* Save pointer to the last descriptor */
-				desc_last = desc;
-			}
-		}
-
-		/* Set first descriptor */
-		desc_first->des0 |= cpu_to_le32(IDMAC_DES0_FD);
-
-		/* Set last descriptor */
-		desc_last->des0 &= cpu_to_le32(~(IDMAC_DES0_CH |
-					       IDMAC_DES0_DIC));
-		desc_last->des0 |= cpu_to_le32(IDMAC_DES0_LD);
-	}
-
-	wmb(); /* drain writebuffer */
-}
-
-static int dw_mci_idmac_start_dma(struct dw_mci *host, unsigned int sg_len)
-{
-	u32 temp;
-
-	dw_mci_translate_sglist(host, host->data, sg_len);
-
-	/* Make sure to reset DMA in case we did PIO before this */
-	dw_mci_ctrl_reset(host, SDMMC_CTRL_DMA_RESET);
-	dw_mci_idmac_reset(host);
-
-	/* Select IDMAC interface */
-	temp = mci_readl(host, CTRL);
-	temp |= SDMMC_CTRL_USE_IDMAC;
-	mci_writel(host, CTRL, temp);
-
-	/* drain writebuffer */
-	wmb();
-
-	/* Enable the IDMAC */
-	temp = mci_readl(host, BMOD);
-	temp |= SDMMC_IDMAC_ENABLE | SDMMC_IDMAC_FB;
-	mci_writel(host, BMOD, temp);
-
-	/* Start it running */
-	mci_writel(host, PLDMND, 1);
-
-	return 0;
-}
-
 static int dw_mci_idmac_init(struct dw_mci *host)
 {
 	int i;
@@ -604,7 +476,8 @@ static int dw_mci_idmac_init(struct dw_mci *host)
 	if (host->dma_64bit_address == 1) {
 		struct idmac_desc_64addr *p;
 		/* Number of descriptors in the ring buffer */
-		host->ring_size = PAGE_SIZE / sizeof(struct idmac_desc_64addr);
+		host->ring_size =
+			DESC_RING_BUF_SZ / sizeof(struct idmac_desc_64addr);
 
 		/* Forward link the descriptor list */
 		for (i = 0, p = host->sg_cpu; i < host->ring_size - 1;
@@ -630,7 +503,8 @@ static int dw_mci_idmac_init(struct dw_mci *host)
 	} else {
 		struct idmac_desc *p;
 		/* Number of descriptors in the ring buffer */
-		host->ring_size = PAGE_SIZE / sizeof(struct idmac_desc);
+		host->ring_size =
+			DESC_RING_BUF_SZ / sizeof(struct idmac_desc);
 
 		/* Forward link the descriptor list */
 		for (i = 0, p = host->sg_cpu;
@@ -671,6 +545,195 @@ static int dw_mci_idmac_init(struct dw_mci *host)
 	return 0;
 }
 
+static inline int dw_mci_prepare_desc64(struct dw_mci *host,
+					 struct mmc_data *data,
+					 unsigned int sg_len)
+{
+	unsigned int desc_len;
+	struct idmac_desc_64addr *desc_first, *desc_last, *desc;
+	unsigned long timeout;
+	int i;
+
+	desc_first = desc_last = desc = host->sg_cpu;
+
+	for (i = 0; i < sg_len; i++) {
+		unsigned int length = sg_dma_len(&data->sg[i]);
+
+		u64 mem_addr = sg_dma_address(&data->sg[i]);
+
+		for ( ; length ; desc++) {
+			desc_len = (length <= DW_MCI_DESC_DATA_LENGTH) ?
+				   length : DW_MCI_DESC_DATA_LENGTH;
+
+			length -= desc_len;
+
+			/*
+			 * Wait for the former clear OWN bit operation
+			 * of IDMAC to make sure that this descriptor
+			 * isn't still owned by IDMAC as IDMAC's write
+			 * ops and CPU's read ops are asynchronous.
+			 */
+			timeout = jiffies + msecs_to_jiffies(100);
+			while (readl(&desc->des0) & IDMAC_DES0_OWN) {
+				if (time_after(jiffies, timeout))
+					goto err_own_bit;
+				udelay(10);
+			}
+
+			/*
+			 * Set the OWN bit and disable interrupts
+			 * for this descriptor
+			 */
+			desc->des0 = IDMAC_DES0_OWN | IDMAC_DES0_DIC |
+						IDMAC_DES0_CH;
+
+			/* Buffer length */
+			IDMAC_64ADDR_SET_BUFFER1_SIZE(desc, desc_len);
+
+			/* Physical address to DMA to/from */
+			desc->des4 = mem_addr & 0xffffffff;
+			desc->des5 = mem_addr >> 32;
+
+			/* Update physical address for the next desc */
+			mem_addr += desc_len;
+
+			/* Save pointer to the last descriptor */
+			desc_last = desc;
+		}
+	}
+
+	/* Set first descriptor */
+	desc_first->des0 |= IDMAC_DES0_FD;
+
+	/* Set last descriptor */
+	desc_last->des0 &= ~(IDMAC_DES0_CH | IDMAC_DES0_DIC);
+	desc_last->des0 |= IDMAC_DES0_LD;
+
+	return 0;
+err_own_bit:
+	/* restore the descriptor chain as it's polluted */
+	dev_dbg(host->dev, "desciptor is still owned by IDMAC.\n");
+	memset(host->sg_cpu, 0, DESC_RING_BUF_SZ);
+	dw_mci_idmac_init(host);
+	return -EINVAL;
+}
+
+
+static inline int dw_mci_prepare_desc32(struct dw_mci *host,
+					 struct mmc_data *data,
+					 unsigned int sg_len)
+{
+	unsigned int desc_len;
+	struct idmac_desc *desc_first, *desc_last, *desc;
+	unsigned long timeout;
+	int i;
+
+	desc_first = desc_last = desc = host->sg_cpu;
+
+	for (i = 0; i < sg_len; i++) {
+		unsigned int length = sg_dma_len(&data->sg[i]);
+
+		u32 mem_addr = sg_dma_address(&data->sg[i]);
+
+		for ( ; length ; desc++) {
+			desc_len = (length <= DW_MCI_DESC_DATA_LENGTH) ?
+				   length : DW_MCI_DESC_DATA_LENGTH;
+
+			length -= desc_len;
+
+			/*
+			 * Wait for the former clear OWN bit operation
+			 * of IDMAC to make sure that this descriptor
+			 * isn't still owned by IDMAC as IDMAC's write
+			 * ops and CPU's read ops are asynchronous.
+			 */
+			timeout = jiffies + msecs_to_jiffies(100);
+			while (readl(&desc->des0) &
+			       cpu_to_le32(IDMAC_DES0_OWN)) {
+				if (time_after(jiffies, timeout))
+					goto err_own_bit;
+				udelay(10);
+			}
+
+			/*
+			 * Set the OWN bit and disable interrupts
+			 * for this descriptor
+			 */
+			desc->des0 = cpu_to_le32(IDMAC_DES0_OWN |
+						 IDMAC_DES0_DIC |
+						 IDMAC_DES0_CH);
+
+			/* Buffer length */
+			IDMAC_SET_BUFFER1_SIZE(desc, desc_len);
+
+			/* Physical address to DMA to/from */
+			desc->des2 = cpu_to_le32(mem_addr);
+
+			/* Update physical address for the next desc */
+			mem_addr += desc_len;
+
+			/* Save pointer to the last descriptor */
+			desc_last = desc;
+		}
+	}
+
+	/* Set first descriptor */
+	desc_first->des0 |= cpu_to_le32(IDMAC_DES0_FD);
+
+	/* Set last descriptor */
+	desc_last->des0 &= cpu_to_le32(~(IDMAC_DES0_CH |
+				       IDMAC_DES0_DIC));
+	desc_last->des0 |= cpu_to_le32(IDMAC_DES0_LD);
+
+	return 0;
+err_own_bit:
+	/* restore the descriptor chain as it's polluted */
+	dev_dbg(host->dev, "desciptor is still owned by IDMAC.\n");
+	memset(host->sg_cpu, 0, DESC_RING_BUF_SZ);
+	dw_mci_idmac_init(host);
+	return -EINVAL;
+}
+
+static int dw_mci_idmac_start_dma(struct dw_mci *host, unsigned int sg_len)
+{
+	u32 temp;
+	int ret;
+
+	if (host->dma_64bit_address == 1)
+		ret = dw_mci_prepare_desc64(host, host->data, sg_len);
+	else
+		ret = dw_mci_prepare_desc32(host, host->data, sg_len);
+
+	if (ret)
+		goto out;
+
+	/* drain writebuffer */
+	wmb();
+
+	/* Make sure to reset DMA in case we did PIO before this */
+	dw_mci_ctrl_reset(host, SDMMC_CTRL_DMA_RESET);
+	dw_mci_idmac_reset(host);
+
+	/* Select IDMAC interface */
+	temp = mci_readl(host, CTRL);
+	temp |= SDMMC_CTRL_USE_IDMAC;
+	mci_writel(host, CTRL, temp);
+
+	/* drain writebuffer */
+	wmb();
+
+	/* Enable the IDMAC */
+	temp = mci_readl(host, BMOD);
+	temp |= SDMMC_IDMAC_ENABLE | SDMMC_IDMAC_FB;
+	mci_writel(host, BMOD, temp);
+
+	/* Start it running */
+	mci_writel(host, PLDMND, 1);
+
+out:
+	return ret;
+}
+
 static const struct dw_mci_dma_ops dw_mci_idmac_ops = {
 	.init = dw_mci_idmac_init,
 	.start = dw_mci_idmac_start_dma,
@@ -876,11 +939,8 @@ static void dw_mci_adjust_fifoth(struct dw_mci *host, struct mmc_data *data)
 	 * MSIZE is '1',
 	 * if blksz is not a multiple of the FIFO width
 	 */
-	if (blksz % fifo_width) {
-		msize = 0;
-		rx_wmark = 1;
+	if (blksz % fifo_width)
 		goto done;
-	}
 
 	do {
 		if (!((blksz_depth % mszs[idx]) ||
@@ -998,8 +1058,10 @@ static int dw_mci_submit_data_dma(struct dw_mci *host, struct mmc_data *data)
 	spin_unlock_irqrestore(&host->irq_lock, irqflags);
 
 	if (host->dma_ops->start(host, sg_len)) {
-		/* We can't do DMA */
-		dev_err(host->dev, "%s: failed to start DMA.\n", __func__);
+		/* We can't do DMA, try PIO for this one */
+		dev_dbg(host->dev,
+			"%s: fall back to PIO mode for current transfer\n",
+			__func__);
 		return -ENODEV;
 	}
 
@@ -1695,11 +1757,11 @@ static int dw_mci_data_complete(struct dw_mci *host, struct mmc_data *data)
 				data->error = -ETIMEDOUT;
 			} else if (host->dir_status ==
 					DW_MCI_RECV_STATUS) {
-				data->error = -EIO;
+				data->error = -EILSEQ;
 			}
 		} else {
 			/* SDMMC_INT_SBE is included */
-			data->error = -EIO;
+			data->error = -EILSEQ;
 		}
 
 		dev_dbg(host->dev, "data error, status 0x%08x\n", status);
@@ -2527,47 +2589,6 @@ static irqreturn_t dw_mci_interrupt(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
-#ifdef CONFIG_OF
-/* 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;
-
-	if (!dev || !dev->of_node)
-		return NULL;
-
-	for_each_child_of_node(dev->of_node, np) {
-		addr = of_get_property(np, "reg", &len);
-		if (!addr || (len < sizeof(int)))
-			continue;
-		if (be32_to_cpup(addr) == slot->id)
-			return np;
-	}
-	return NULL;
-}
-
-static void dw_mci_slot_of_parse(struct dw_mci_slot *slot)
-{
-	struct device_node *np = dw_mci_of_find_slot_node(slot);
-
-	if (!np)
-		return;
-
-	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 void dw_mci_slot_of_parse(struct dw_mci_slot *slot)
-{
-}
-#endif /* CONFIG_OF */
-
 static int dw_mci_init_slot(struct dw_mci *host, unsigned int id)
 {
 	struct mmc_host *mmc;
@@ -2630,8 +2651,6 @@ 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;
@@ -2736,7 +2755,8 @@ static void dw_mci_init_dma(struct dw_mci *host)
 		}
 
 		/* Alloc memory for sg translation */
-		host->sg_cpu = dmam_alloc_coherent(host->dev, PAGE_SIZE,
+		host->sg_cpu = dmam_alloc_coherent(host->dev,
+						   DESC_RING_BUF_SZ,
 						   &host->sg_dma, GFP_KERNEL);
 		if (!host->sg_cpu) {
 			dev_err(host->dev,
@@ -2919,6 +2939,13 @@ static struct dw_mci_board *dw_mci_parse_dt(struct dw_mci *host)
 	if (!pdata)
 		return ERR_PTR(-ENOMEM);
 
+	/* find reset controller when exist */
+	pdata->rstc = devm_reset_control_get_optional(dev, NULL);
+	if (IS_ERR(pdata->rstc)) {
+		if (PTR_ERR(pdata->rstc) == -EPROBE_DEFER)
+			return ERR_PTR(-EPROBE_DEFER);
+	}
+
 	/* find out number of slots supported */
 	of_property_read_u32(np, "num-slots", &pdata->num_slots);
 
@@ -2937,11 +2964,6 @@ static struct dw_mci_board *dw_mci_parse_dt(struct dw_mci *host)
 			return ERR_PTR(ret);
 	}
 
-	if (of_find_property(np, "supports-highspeed", NULL)) {
-		dev_info(dev, "supports-highspeed property is deprecated.\n");
-		pdata->caps |= MMC_CAP_SD_HIGHSPEED | MMC_CAP_MMC_HIGHSPEED;
-	}
-
 	return pdata;
 }
 
@@ -2990,7 +3012,9 @@ int dw_mci_probe(struct dw_mci *host)
 
 	if (!host->pdata) {
 		host->pdata = dw_mci_parse_dt(host);
-		if (IS_ERR(host->pdata)) {
+		if (PTR_ERR(host->pdata) == -EPROBE_DEFER) {
+			return -EPROBE_DEFER;
+		} else if (IS_ERR(host->pdata)) {
 			dev_err(host->dev, "platform data not available\n");
 			return -EINVAL;
 		}
@@ -3044,6 +3068,12 @@ int dw_mci_probe(struct dw_mci *host)
 		}
 	}
 
+	if (!IS_ERR(host->pdata->rstc)) {
+		reset_control_assert(host->pdata->rstc);
+		usleep_range(10, 50);
+		reset_control_deassert(host->pdata->rstc);
+	}
+
 	setup_timer(&host->cmd11_timer,
 		    dw_mci_cmd11_timer, (unsigned long)host);
 
@@ -3193,13 +3223,14 @@ err_dmaunmap:
 	if (host->use_dma && host->dma_ops->exit)
 		host->dma_ops->exit(host);
 
+	if (!IS_ERR(host->pdata->rstc))
+		reset_control_assert(host->pdata->rstc);
+
 err_clk_ciu:
-	if (!IS_ERR(host->ciu_clk))
-		clk_disable_unprepare(host->ciu_clk);
+	clk_disable_unprepare(host->ciu_clk);
 
 err_clk_biu:
-	if (!IS_ERR(host->biu_clk))
-		clk_disable_unprepare(host->biu_clk);
+	clk_disable_unprepare(host->biu_clk);
 
 	return ret;
 }
@@ -3225,11 +3256,11 @@ void dw_mci_remove(struct dw_mci *host)
 	if (host->use_dma && host->dma_ops->exit)
 		host->dma_ops->exit(host);
 
-	if (!IS_ERR(host->ciu_clk))
-		clk_disable_unprepare(host->ciu_clk);
+	if (!IS_ERR(host->pdata->rstc))
+		reset_control_assert(host->pdata->rstc);
 
-	if (!IS_ERR(host->biu_clk))
-		clk_disable_unprepare(host->biu_clk);
+	clk_disable_unprepare(host->ciu_clk);
+	clk_disable_unprepare(host->biu_clk);
 }
 EXPORT_SYMBOL(dw_mci_remove);
 
diff --git a/drivers/mmc/host/moxart-mmc.c b/drivers/mmc/host/moxart-mmc.c
index 79905ce895ad..bbad309679cf 100644
--- a/drivers/mmc/host/moxart-mmc.c
+++ b/drivers/mmc/host/moxart-mmc.c
@@ -257,7 +257,7 @@ static void moxart_dma_complete(void *param)
 static void moxart_transfer_dma(struct mmc_data *data, struct moxart_host *host)
 {
 	u32 len, dir_data, dir_slave;
-	unsigned long dma_time;
+	long dma_time;
 	struct dma_async_tx_descriptor *desc = NULL;
 	struct dma_chan *dma_chan;
 
@@ -397,7 +397,8 @@ static void moxart_prepare_data(struct moxart_host *host)
 static void moxart_request(struct mmc_host *mmc, struct mmc_request *mrq)
 {
 	struct moxart_host *host = mmc_priv(mmc);
-	unsigned long pio_time, flags;
+	long pio_time;
+	unsigned long flags;
 	u32 status;
 
 	spin_lock_irqsave(&host->lock, flags);
diff --git a/drivers/mmc/host/rtsx_pci_sdmmc.c b/drivers/mmc/host/rtsx_pci_sdmmc.c
index 396c9b7e4121..3ccaa1415f33 100644
--- a/drivers/mmc/host/rtsx_pci_sdmmc.c
+++ b/drivers/mmc/host/rtsx_pci_sdmmc.c
@@ -126,7 +126,7 @@ static int sd_response_type(struct mmc_command *cmd)
 		return SD_RSP_TYPE_R0;
 	case MMC_RSP_R1:
 		return SD_RSP_TYPE_R1;
-	case MMC_RSP_R1 & ~MMC_RSP_CRC:
+	case MMC_RSP_R1_NO_CRC:
 		return SD_RSP_TYPE_R1 | SD_NO_CHECK_CRC7;
 	case MMC_RSP_R1B:
 		return SD_RSP_TYPE_R1b;
diff --git a/drivers/mmc/host/rtsx_usb_sdmmc.c b/drivers/mmc/host/rtsx_usb_sdmmc.c
index 6c71fc9f76c7..6e9c0f8fddb1 100644
--- a/drivers/mmc/host/rtsx_usb_sdmmc.c
+++ b/drivers/mmc/host/rtsx_usb_sdmmc.c
@@ -324,7 +324,7 @@ static void sd_send_cmd_get_rsp(struct rtsx_usb_sdmmc *host,
 	case MMC_RSP_R1:
 		rsp_type = SD_RSP_TYPE_R1;
 		break;
-	case MMC_RSP_R1 & ~MMC_RSP_CRC:
+	case MMC_RSP_R1_NO_CRC:
 		rsp_type = SD_RSP_TYPE_R1 | SD_NO_CHECK_CRC7;
 		break;
 	case MMC_RSP_R1B:
@@ -1138,11 +1138,6 @@ static void sdmmc_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 	dev_dbg(sdmmc_dev(host), "%s\n", __func__);
 	mutex_lock(&ucr->dev_mutex);
 
-	if (rtsx_usb_card_exclusive_check(ucr, RTSX_USB_SD_CARD)) {
-		mutex_unlock(&ucr->dev_mutex);
-		return;
-	}
-
 	sd_set_power_mode(host, ios->power_mode);
 	sd_set_bus_width(host, ios->bus_width);
 	sd_set_timing(host, ios->timing, &host->ddr_mode);
@@ -1314,6 +1309,7 @@ static void rtsx_usb_update_led(struct work_struct *work)
 		container_of(work, struct rtsx_usb_sdmmc, led_work);
 	struct rtsx_ucr *ucr = host->ucr;
 
+	pm_runtime_get_sync(sdmmc_dev(host));
 	mutex_lock(&ucr->dev_mutex);
 
 	if (host->led.brightness == LED_OFF)
@@ -1322,6 +1318,7 @@ static void rtsx_usb_update_led(struct work_struct *work)
 		rtsx_usb_turn_on_led(ucr);
 
 	mutex_unlock(&ucr->dev_mutex);
+	pm_runtime_put(sdmmc_dev(host));
 }
 #endif
 
diff --git a/drivers/mmc/host/sdhci-acpi.c b/drivers/mmc/host/sdhci-acpi.c
index 8fe0756c8e1e..81d4dc034793 100644
--- a/drivers/mmc/host/sdhci-acpi.c
+++ b/drivers/mmc/host/sdhci-acpi.c
@@ -275,7 +275,7 @@ static const struct sdhci_acpi_slot sdhci_acpi_slot_int_emmc = {
 	.chip    = &sdhci_acpi_chip_int,
 	.caps    = MMC_CAP_8_BIT_DATA | MMC_CAP_NONREMOVABLE |
 		   MMC_CAP_HW_RESET | MMC_CAP_1_8V_DDR |
-		   MMC_CAP_WAIT_WHILE_BUSY,
+		   MMC_CAP_CMD_DURING_TFR | MMC_CAP_WAIT_WHILE_BUSY,
 	.caps2   = MMC_CAP2_HC_ERASE_SZ,
 	.flags   = SDHCI_ACPI_RUNTIME_PM,
 	.quirks  = SDHCI_QUIRK_NO_ENDATTR_IN_NOPDESC,
diff --git a/drivers/mmc/host/sdhci-bcm-kona.c b/drivers/mmc/host/sdhci-bcm-kona.c
index e5c634bdfdd9..51dd2fd65000 100644
--- a/drivers/mmc/host/sdhci-bcm-kona.c
+++ b/drivers/mmc/host/sdhci-bcm-kona.c
@@ -253,12 +253,14 @@ static int sdhci_bcm_kona_probe(struct platform_device *pdev)
 		goto err_pltfm_free;
 	}
 
-	if (clk_set_rate(pltfm_priv->clk, host->mmc->f_max) != 0) {
+	ret = clk_set_rate(pltfm_priv->clk, host->mmc->f_max);
+	if (ret) {
 		dev_err(dev, "Failed to set rate core clock\n");
 		goto err_pltfm_free;
 	}
 
-	if (clk_prepare_enable(pltfm_priv->clk) != 0) {
+	ret = clk_prepare_enable(pltfm_priv->clk);
+	if (ret) {
 		dev_err(dev, "Failed to enable core clock\n");
 		goto err_pltfm_free;
 	}
diff --git a/drivers/mmc/host/sdhci-brcmstb.c b/drivers/mmc/host/sdhci-brcmstb.c
index cce10fe3e19e..159f6f64c68e 100644
--- a/drivers/mmc/host/sdhci-brcmstb.c
+++ b/drivers/mmc/host/sdhci-brcmstb.c
@@ -98,6 +98,8 @@ static int sdhci_brcmstb_probe(struct platform_device *pdev)
 	 * properties through mmc_of_parse().
 	 */
 	host->caps = sdhci_readl(host, SDHCI_CAPABILITIES);
+	if (of_device_is_compatible(pdev->dev.of_node, "brcm,bcm7425-sdhci"))
+		host->caps &= ~SDHCI_CAN_64BIT;
 	host->caps1 = sdhci_readl(host, SDHCI_CAPABILITIES_1);
 	host->caps1 &= ~(SDHCI_SUPPORT_SDR50 | SDHCI_SUPPORT_SDR104 |
 			SDHCI_SUPPORT_DDR50);
@@ -121,6 +123,7 @@ err_clk:
 
 static const struct of_device_id sdhci_brcm_of_match[] = {
 	{ .compatible = "brcm,bcm7425-sdhci" },
+	{ .compatible = "brcm,bcm7445-sdhci" },
 	{},
 };
 MODULE_DEVICE_TABLE(of, sdhci_brcm_of_match);
@@ -128,7 +131,6 @@ MODULE_DEVICE_TABLE(of, sdhci_brcm_of_match);
 static struct platform_driver sdhci_brcmstb_driver = {
 	.driver		= {
 		.name	= "sdhci-brcmstb",
-		.owner	= THIS_MODULE,
 		.pm	= &sdhci_brcmstb_pmops,
 		.of_match_table = of_match_ptr(sdhci_brcm_of_match),
 	},
diff --git a/drivers/mmc/host/sdhci-esdhc-imx.c b/drivers/mmc/host/sdhci-esdhc-imx.c
index 99e0b334f9df..7123ef96ed18 100644
--- a/drivers/mmc/host/sdhci-esdhc-imx.c
+++ b/drivers/mmc/host/sdhci-esdhc-imx.c
@@ -31,6 +31,7 @@
 #include "sdhci-pltfm.h"
 #include "sdhci-esdhc.h"
 
+#define ESDHC_SYS_CTRL_DTOCV_MASK	0x0f
 #define	ESDHC_CTRL_D3CD			0x08
 #define ESDHC_BURST_LEN_EN_INCR		(1 << 27)
 /* VENDOR SPEC register */
@@ -345,7 +346,8 @@ static void esdhc_writel_le(struct sdhci_host *host, u32 val, int reg)
 	struct pltfm_imx_data *imx_data = sdhci_pltfm_priv(pltfm_host);
 	u32 data;
 
-	if (unlikely(reg == SDHCI_INT_ENABLE || reg == SDHCI_SIGNAL_ENABLE)) {
+	if (unlikely(reg == SDHCI_INT_ENABLE || reg == SDHCI_SIGNAL_ENABLE ||
+			reg == SDHCI_INT_STATUS)) {
 		if ((val & SDHCI_INT_CARD_INT) && !esdhc_is_usdhc(imx_data)) {
 			/*
 			 * Clear and then set D3CD bit to avoid missing the
@@ -554,6 +556,25 @@ static void esdhc_writew_le(struct sdhci_host *host, u16 val, int reg)
 	esdhc_clrset_le(host, 0xffff, val, reg);
 }
 
+static u8 esdhc_readb_le(struct sdhci_host *host, int reg)
+{
+	u8 ret;
+	u32 val;
+
+	switch (reg) {
+	case SDHCI_HOST_CONTROL:
+		val = readl(host->ioaddr + reg);
+
+		ret = val & SDHCI_CTRL_LED;
+		ret |= (val >> 5) & SDHCI_CTRL_DMA_MASK;
+		ret |= (val & ESDHC_CTRL_4BITBUS);
+		ret |= (val & ESDHC_CTRL_8BITBUS) << 3;
+		return ret;
+	}
+
+	return readb(host->ioaddr + reg);
+}
+
 static void esdhc_writeb_le(struct sdhci_host *host, u8 val, int reg)
 {
 	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
@@ -928,7 +949,8 @@ static unsigned int esdhc_get_max_timeout_count(struct sdhci_host *host)
 	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
 	struct pltfm_imx_data *imx_data = sdhci_pltfm_priv(pltfm_host);
 
-	return esdhc_is_usdhc(imx_data) ? 1 << 28 : 1 << 27;
+	/* Doc Errata: the uSDHC actual maximum timeout count is 1 << 29 */
+	return esdhc_is_usdhc(imx_data) ? 1 << 29 : 1 << 27;
 }
 
 static void esdhc_set_timeout(struct sdhci_host *host, struct mmc_command *cmd)
@@ -937,13 +959,15 @@ static void esdhc_set_timeout(struct sdhci_host *host, struct mmc_command *cmd)
 	struct pltfm_imx_data *imx_data = sdhci_pltfm_priv(pltfm_host);
 
 	/* use maximum timeout counter */
-	sdhci_writeb(host, esdhc_is_usdhc(imx_data) ? 0xF : 0xE,
+	esdhc_clrset_le(host, ESDHC_SYS_CTRL_DTOCV_MASK,
+			esdhc_is_usdhc(imx_data) ? 0xF : 0xE,
 			SDHCI_TIMEOUT_CONTROL);
 }
 
 static struct sdhci_ops sdhci_esdhc_ops = {
 	.read_l = esdhc_readl_le,
 	.read_w = esdhc_readw_le,
+	.read_b = esdhc_readb_le,
 	.write_l = esdhc_writel_le,
 	.write_w = esdhc_writew_le,
 	.write_b = esdhc_writeb_le,
diff --git a/drivers/mmc/host/sdhci-of-arasan.c b/drivers/mmc/host/sdhci-of-arasan.c
index e0f193f7e3e5..410a55b1c25f 100644
--- a/drivers/mmc/host/sdhci-of-arasan.c
+++ b/drivers/mmc/host/sdhci-of-arasan.c
@@ -26,6 +26,7 @@
 #include <linux/phy/phy.h>
 #include <linux/regmap.h>
 #include "sdhci-pltfm.h"
+#include <linux/of.h>
 
 #define SDHCI_ARASAN_CLK_CTRL_OFFSET	0x2c
 #define SDHCI_ARASAN_VENDOR_REGISTER	0x78
@@ -35,6 +36,8 @@
 #define CLK_CTRL_TIMEOUT_MASK		(0xf << CLK_CTRL_TIMEOUT_SHIFT)
 #define CLK_CTRL_TIMEOUT_MIN_EXP	13
 
+#define PHY_CLK_TOO_SLOW_HZ		400000
+
 /*
  * On some SoCs the syscon area has a feature where the upper 16-bits of
  * each 32-bit register act as a write mask for the lower 16-bits.  This allows
@@ -65,10 +68,12 @@ struct sdhci_arasan_soc_ctl_field {
  * accessible via the syscon API.
  *
  * @baseclkfreq:	Where to find corecfg_baseclkfreq
+ * @clockmultiplier:	Where to find corecfg_clockmultiplier
  * @hiword_update:	If true, use HIWORD_UPDATE to access the syscon
  */
 struct sdhci_arasan_soc_ctl_map {
 	struct sdhci_arasan_soc_ctl_field	baseclkfreq;
+	struct sdhci_arasan_soc_ctl_field	clockmultiplier;
 	bool					hiword_update;
 };
 
@@ -77,6 +82,7 @@ struct sdhci_arasan_soc_ctl_map {
  * @host:		Pointer to the main SDHCI host structure.
  * @clk_ahb:		Pointer to the AHB clock
  * @phy:		Pointer to the generic phy
+ * @is_phy_on:		True if the PHY is on; false if not.
  * @sdcardclk_hw:	Struct for the clock we might provide to a PHY.
  * @sdcardclk:		Pointer to normal 'struct clock' for sdcardclk_hw.
  * @soc_ctl_base:	Pointer to regmap for syscon for soc_ctl registers.
@@ -86,16 +92,22 @@ struct sdhci_arasan_data {
 	struct sdhci_host *host;
 	struct clk	*clk_ahb;
 	struct phy	*phy;
+	bool		is_phy_on;
 
 	struct clk_hw	sdcardclk_hw;
 	struct clk      *sdcardclk;
 
 	struct regmap	*soc_ctl_base;
 	const struct sdhci_arasan_soc_ctl_map *soc_ctl_map;
+	unsigned int	quirks; /* Arasan deviations from spec */
+
+/* Controller does not have CD wired and will not function normally without */
+#define SDHCI_ARASAN_QUIRK_FORCE_CDTEST	BIT(0)
 };
 
 static const struct sdhci_arasan_soc_ctl_map rk3399_soc_ctl_map = {
 	.baseclkfreq = { .reg = 0xf000, .width = 8, .shift = 8 },
+	.clockmultiplier = { .reg = 0xf02c, .width = 8, .shift = 0},
 	.hiword_update = true,
 };
 
@@ -170,13 +182,47 @@ static void sdhci_arasan_set_clock(struct sdhci_host *host, unsigned int clock)
 	struct sdhci_arasan_data *sdhci_arasan = sdhci_pltfm_priv(pltfm_host);
 	bool ctrl_phy = false;
 
-	if (clock > MMC_HIGH_52_MAX_DTR && (!IS_ERR(sdhci_arasan->phy)))
-		ctrl_phy = true;
+	if (!IS_ERR(sdhci_arasan->phy)) {
+		if (!sdhci_arasan->is_phy_on && clock <= PHY_CLK_TOO_SLOW_HZ) {
+			/*
+			 * If PHY off, set clock to max speed and power PHY on.
+			 *
+			 * Although PHY docs apparently suggest power cycling
+			 * when changing the clock the PHY doesn't like to be
+			 * powered on while at low speeds like those used in ID
+			 * mode.  Even worse is powering the PHY on while the
+			 * clock is off.
+			 *
+			 * To workaround the PHY limitations, the best we can
+			 * do is to power it on at a faster speed and then slam
+			 * through low speeds without power cycling.
+			 */
+			sdhci_set_clock(host, host->max_clk);
+			spin_unlock_irq(&host->lock);
+			phy_power_on(sdhci_arasan->phy);
+			spin_lock_irq(&host->lock);
+			sdhci_arasan->is_phy_on = true;
+
+			/*
+			 * We'll now fall through to the below case with
+			 * ctrl_phy = false (so we won't turn off/on).  The
+			 * sdhci_set_clock() will set the real clock.
+			 */
+		} else if (clock > PHY_CLK_TOO_SLOW_HZ) {
+			/*
+			 * At higher clock speeds the PHY is fine being power
+			 * cycled and docs say you _should_ power cycle when
+			 * changing clock speeds.
+			 */
+			ctrl_phy = true;
+		}
+	}
 
-	if (ctrl_phy) {
+	if (ctrl_phy && sdhci_arasan->is_phy_on) {
 		spin_unlock_irq(&host->lock);
 		phy_power_off(sdhci_arasan->phy);
 		spin_lock_irq(&host->lock);
+		sdhci_arasan->is_phy_on = false;
 	}
 
 	sdhci_set_clock(host, clock);
@@ -185,6 +231,7 @@ static void sdhci_arasan_set_clock(struct sdhci_host *host, unsigned int clock)
 		spin_unlock_irq(&host->lock);
 		phy_power_on(sdhci_arasan->phy);
 		spin_lock_irq(&host->lock);
+		sdhci_arasan->is_phy_on = true;
 	}
 }
 
@@ -203,12 +250,49 @@ static void sdhci_arasan_hs400_enhanced_strobe(struct mmc_host *mmc,
 	writel(vendor, host->ioaddr + SDHCI_ARASAN_VENDOR_REGISTER);
 }
 
+static void sdhci_arasan_reset(struct sdhci_host *host, u8 mask)
+{
+	u8 ctrl;
+	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
+	struct sdhci_arasan_data *sdhci_arasan = sdhci_pltfm_priv(pltfm_host);
+
+	sdhci_reset(host, mask);
+
+	if (sdhci_arasan->quirks & SDHCI_ARASAN_QUIRK_FORCE_CDTEST) {
+		ctrl = sdhci_readb(host, SDHCI_HOST_CONTROL);
+		ctrl |= SDHCI_CTRL_CDTEST_INS | SDHCI_CTRL_CDTEST_EN;
+		sdhci_writeb(host, ctrl, SDHCI_HOST_CONTROL);
+	}
+}
+
+static int sdhci_arasan_voltage_switch(struct mmc_host *mmc,
+				       struct mmc_ios *ios)
+{
+	switch (ios->signal_voltage) {
+	case MMC_SIGNAL_VOLTAGE_180:
+		/*
+		 * Plese don't switch to 1V8 as arasan,5.1 doesn't
+		 * actually refer to this setting to indicate the
+		 * signal voltage and the state machine will be broken
+		 * actually if we force to enable 1V8. That's something
+		 * like broken quirk but we could work around here.
+		 */
+		return 0;
+	case MMC_SIGNAL_VOLTAGE_330:
+	case MMC_SIGNAL_VOLTAGE_120:
+		/* We don't support 3V3 and 1V2 */
+		break;
+	}
+
+	return -EINVAL;
+}
+
 static struct sdhci_ops sdhci_arasan_ops = {
 	.set_clock = sdhci_arasan_set_clock,
 	.get_max_clock = sdhci_pltfm_clk_get_max_clock,
 	.get_timeout_clock = sdhci_arasan_get_timeout_clock,
 	.set_bus_width = sdhci_set_bus_width,
-	.reset = sdhci_reset,
+	.reset = sdhci_arasan_reset,
 	.set_uhs_signaling = sdhci_set_uhs_signaling,
 };
 
@@ -239,13 +323,14 @@ static int sdhci_arasan_suspend(struct device *dev)
 	if (ret)
 		return ret;
 
-	if (!IS_ERR(sdhci_arasan->phy)) {
+	if (!IS_ERR(sdhci_arasan->phy) && sdhci_arasan->is_phy_on) {
 		ret = phy_power_off(sdhci_arasan->phy);
 		if (ret) {
 			dev_err(dev, "Cannot power off phy.\n");
 			sdhci_resume_host(host);
 			return ret;
 		}
+		sdhci_arasan->is_phy_on = false;
 	}
 
 	clk_disable(pltfm_host->clk);
@@ -281,12 +366,13 @@ static int sdhci_arasan_resume(struct device *dev)
 		return ret;
 	}
 
-	if (!IS_ERR(sdhci_arasan->phy)) {
+	if (!IS_ERR(sdhci_arasan->phy) && host->mmc->actual_clock) {
 		ret = phy_power_on(sdhci_arasan->phy);
 		if (ret) {
 			dev_err(dev, "Cannot power on phy.\n");
 			return ret;
 		}
+		sdhci_arasan->is_phy_on = true;
 	}
 
 	return sdhci_resume_host(host);
@@ -338,6 +424,45 @@ static const struct clk_ops arasan_sdcardclk_ops = {
 };
 
 /**
+ * sdhci_arasan_update_clockmultiplier - Set corecfg_clockmultiplier
+ *
+ * The corecfg_clockmultiplier is supposed to contain clock multiplier
+ * value of programmable clock generator.
+ *
+ * NOTES:
+ * - Many existing devices don't seem to do this and work fine.  To keep
+ *   compatibility for old hardware where the device tree doesn't provide a
+ *   register map, this function is a noop if a soc_ctl_map hasn't been provided
+ *   for this platform.
+ * - The value of corecfg_clockmultiplier should sync with that of corresponding
+ *   value reading from sdhci_capability_register. So this function is called
+ *   once at probe time and never called again.
+ *
+ * @host:		The sdhci_host
+ */
+static void sdhci_arasan_update_clockmultiplier(struct sdhci_host *host,
+						u32 value)
+{
+	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
+	struct sdhci_arasan_data *sdhci_arasan = sdhci_pltfm_priv(pltfm_host);
+	const struct sdhci_arasan_soc_ctl_map *soc_ctl_map =
+		sdhci_arasan->soc_ctl_map;
+
+	/* Having a map is optional */
+	if (!soc_ctl_map)
+		return;
+
+	/* If we have a map, we expect to have a syscon */
+	if (!sdhci_arasan->soc_ctl_base) {
+		pr_warn("%s: Have regmap, but no soc-ctl-syscon\n",
+			mmc_hostname(host->mmc));
+		return;
+	}
+
+	sdhci_arasan_syscon_write(host, &soc_ctl_map->clockmultiplier, value);
+}
+
+/**
  * sdhci_arasan_update_baseclkfreq - Set corecfg_baseclkfreq
  *
  * The corecfg_baseclkfreq is supposed to contain the MHz of clk_xin.  This
@@ -462,6 +587,7 @@ static int sdhci_arasan_probe(struct platform_device *pdev)
 	struct sdhci_host *host;
 	struct sdhci_pltfm_host *pltfm_host;
 	struct sdhci_arasan_data *sdhci_arasan;
+	struct device_node *np = pdev->dev.of_node;
 
 	host = sdhci_pltfm_init(pdev, &sdhci_arasan_pdata,
 				sizeof(*sdhci_arasan));
@@ -516,8 +642,16 @@ static int sdhci_arasan_probe(struct platform_device *pdev)
 	}
 
 	sdhci_get_of_property(pdev);
+
+	if (of_property_read_bool(np, "xlnx,fails-without-test-cd"))
+		sdhci_arasan->quirks |= SDHCI_ARASAN_QUIRK_FORCE_CDTEST;
+
 	pltfm_host->clk = clk_xin;
 
+	if (of_device_is_compatible(pdev->dev.of_node,
+				    "rockchip,rk3399-sdhci-5.1"))
+		sdhci_arasan_update_clockmultiplier(host, 0x0);
+
 	sdhci_arasan_update_baseclkfreq(host);
 
 	ret = sdhci_arasan_register_sdclk(sdhci_arasan, clk_xin, &pdev->dev);
@@ -547,14 +681,10 @@ static int sdhci_arasan_probe(struct platform_device *pdev)
 			goto unreg_clk;
 		}
 
-		ret = phy_power_on(sdhci_arasan->phy);
-		if (ret < 0) {
-			dev_err(&pdev->dev, "phy_power_on err.\n");
-			goto err_phy_power;
-		}
-
 		host->mmc_host_ops.hs400_enhanced_strobe =
 					sdhci_arasan_hs400_enhanced_strobe;
+		host->mmc_host_ops.start_signal_voltage_switch =
+					sdhci_arasan_voltage_switch;
 	}
 
 	ret = sdhci_add_host(host);
@@ -565,9 +695,6 @@ static int sdhci_arasan_probe(struct platform_device *pdev)
 
 err_add_host:
 	if (!IS_ERR(sdhci_arasan->phy))
-		phy_power_off(sdhci_arasan->phy);
-err_phy_power:
-	if (!IS_ERR(sdhci_arasan->phy))
 		phy_exit(sdhci_arasan->phy);
 unreg_clk:
 	sdhci_arasan_unregister_sdclk(&pdev->dev);
@@ -589,7 +716,8 @@ static int sdhci_arasan_remove(struct platform_device *pdev)
 	struct clk *clk_ahb = sdhci_arasan->clk_ahb;
 
 	if (!IS_ERR(sdhci_arasan->phy)) {
-		phy_power_off(sdhci_arasan->phy);
+		if (sdhci_arasan->is_phy_on)
+			phy_power_off(sdhci_arasan->phy);
 		phy_exit(sdhci_arasan->phy);
 	}
 
diff --git a/drivers/mmc/host/sdhci-of-esdhc.c b/drivers/mmc/host/sdhci-of-esdhc.c
index 239be2fde242..fb71c866eacc 100644
--- a/drivers/mmc/host/sdhci-of-esdhc.c
+++ b/drivers/mmc/host/sdhci-of-esdhc.c
@@ -583,7 +583,7 @@ static int sdhci_esdhc_probe(struct platform_device *pdev)
 
 	np = pdev->dev.of_node;
 
-	if (of_get_property(np, "little-endian", NULL))
+	if (of_property_read_bool(np, "little-endian"))
 		host = sdhci_pltfm_init(pdev, &sdhci_esdhc_le_pdata,
 					sizeof(struct sdhci_esdhc));
 	else
diff --git a/drivers/mmc/host/sdhci-pci-core.c b/drivers/mmc/host/sdhci-pci-core.c
index 897cfd24ca2e..1d9e00a00e9f 100644
--- a/drivers/mmc/host/sdhci-pci-core.c
+++ b/drivers/mmc/host/sdhci-pci-core.c
@@ -32,6 +32,14 @@
 #include "sdhci-pci.h"
 #include "sdhci-pci-o2micro.h"
 
+static int sdhci_pci_enable_dma(struct sdhci_host *host);
+static void sdhci_pci_set_bus_width(struct sdhci_host *host, int width);
+static void sdhci_pci_hw_reset(struct sdhci_host *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);
+
 /*****************************************************************************\
  *                                                                           *
  * Hardware specific quirk handling                                          *
@@ -156,7 +164,7 @@ static void sdhci_pci_add_own_cd(struct sdhci_pci_slot *slot)
 	if (!gpio_is_valid(gpio))
 		return;
 
-	err = gpio_request(gpio, "sd_cd");
+	err = devm_gpio_request(&slot->chip->pdev->dev, gpio, "sd_cd");
 	if (err < 0)
 		goto out;
 
@@ -179,7 +187,7 @@ static void sdhci_pci_add_own_cd(struct sdhci_pci_slot *slot)
 	return;
 
 out_free:
-	gpio_free(gpio);
+	devm_gpio_free(&slot->chip->pdev->dev, gpio);
 out:
 	dev_warn(&slot->chip->pdev->dev, "failed to setup card detect wake up\n");
 }
@@ -188,8 +196,6 @@ static void sdhci_pci_remove_own_cd(struct sdhci_pci_slot *slot)
 {
 	if (slot->cd_irq >= 0)
 		free_irq(slot->cd_irq, slot);
-	if (gpio_is_valid(slot->cd_gpio))
-		gpio_free(slot->cd_gpio);
 }
 
 #else
@@ -356,6 +362,7 @@ static int byt_emmc_probe_slot(struct sdhci_pci_slot *slot)
 {
 	slot->host->mmc->caps |= MMC_CAP_8_BIT_DATA | MMC_CAP_NONREMOVABLE |
 				 MMC_CAP_HW_RESET | MMC_CAP_1_8V_DDR |
+				 MMC_CAP_CMD_DURING_TFR |
 				 MMC_CAP_WAIT_WHILE_BUSY;
 	slot->host->mmc->caps2 |= MMC_CAP2_HC_ERASE_SZ;
 	slot->hw_reset = sdhci_pci_int_hw_reset;
@@ -391,6 +398,45 @@ static int byt_sd_probe_slot(struct sdhci_pci_slot *slot)
 	return 0;
 }
 
+#define SDHCI_INTEL_PWR_TIMEOUT_CNT	20
+#define SDHCI_INTEL_PWR_TIMEOUT_UDELAY	100
+
+static void sdhci_intel_set_power(struct sdhci_host *host, unsigned char mode,
+				  unsigned short vdd)
+{
+	int cntr;
+	u8 reg;
+
+	sdhci_set_power(host, mode, vdd);
+
+	if (mode == MMC_POWER_OFF)
+		return;
+
+	/*
+	 * Bus power might not enable after D3 -> D0 transition due to the
+	 * present state not yet having propagated. Retry for up to 2ms.
+	 */
+	for (cntr = 0; cntr < SDHCI_INTEL_PWR_TIMEOUT_CNT; cntr++) {
+		reg = sdhci_readb(host, SDHCI_POWER_CONTROL);
+		if (reg & SDHCI_POWER_ON)
+			break;
+		udelay(SDHCI_INTEL_PWR_TIMEOUT_UDELAY);
+		reg |= SDHCI_POWER_ON;
+		sdhci_writeb(host, reg, SDHCI_POWER_CONTROL);
+	}
+}
+
+static const struct sdhci_ops sdhci_intel_byt_ops = {
+	.set_clock		= sdhci_set_clock,
+	.set_power		= sdhci_intel_set_power,
+	.enable_dma		= sdhci_pci_enable_dma,
+	.set_bus_width		= sdhci_pci_set_bus_width,
+	.reset			= sdhci_reset,
+	.set_uhs_signaling	= sdhci_set_uhs_signaling,
+	.hw_reset		= sdhci_pci_hw_reset,
+	.select_drive_strength	= sdhci_pci_select_drive_strength,
+};
+
 static const struct sdhci_pci_fixes sdhci_intel_byt_emmc = {
 	.allow_runtime_pm = true,
 	.probe_slot	= byt_emmc_probe_slot,
@@ -398,6 +444,7 @@ static const struct sdhci_pci_fixes sdhci_intel_byt_emmc = {
 	.quirks2	= SDHCI_QUIRK2_PRESET_VALUE_BROKEN |
 			  SDHCI_QUIRK2_CAPS_BIT63_FOR_HS400 |
 			  SDHCI_QUIRK2_STOP_WITH_TC,
+	.ops		= &sdhci_intel_byt_ops,
 };
 
 static const struct sdhci_pci_fixes sdhci_intel_byt_sdio = {
@@ -406,6 +453,7 @@ static const struct sdhci_pci_fixes sdhci_intel_byt_sdio = {
 			SDHCI_QUIRK2_PRESET_VALUE_BROKEN,
 	.allow_runtime_pm = true,
 	.probe_slot	= byt_sdio_probe_slot,
+	.ops		= &sdhci_intel_byt_ops,
 };
 
 static const struct sdhci_pci_fixes sdhci_intel_byt_sd = {
@@ -416,22 +464,36 @@ static const struct sdhci_pci_fixes sdhci_intel_byt_sd = {
 	.allow_runtime_pm = true,
 	.own_cd_for_runtime_pm = true,
 	.probe_slot	= byt_sd_probe_slot,
+	.ops		= &sdhci_intel_byt_ops,
 };
 
 /* Define Host controllers for Intel Merrifield platform */
 #define INTEL_MRFLD_EMMC_0	0
 #define INTEL_MRFLD_EMMC_1	1
+#define INTEL_MRFLD_SD		2
+#define INTEL_MRFLD_SDIO	3
 
 static int intel_mrfld_mmc_probe_slot(struct sdhci_pci_slot *slot)
 {
-	if ((PCI_FUNC(slot->chip->pdev->devfn) != INTEL_MRFLD_EMMC_0) &&
-	    (PCI_FUNC(slot->chip->pdev->devfn) != INTEL_MRFLD_EMMC_1))
-		/* SD support is not ready yet */
+	unsigned int func = PCI_FUNC(slot->chip->pdev->devfn);
+
+	switch (func) {
+	case INTEL_MRFLD_EMMC_0:
+	case INTEL_MRFLD_EMMC_1:
+		slot->host->mmc->caps |= MMC_CAP_NONREMOVABLE |
+					 MMC_CAP_8_BIT_DATA |
+					 MMC_CAP_1_8V_DDR;
+		break;
+	case INTEL_MRFLD_SD:
+		slot->host->quirks2 |= SDHCI_QUIRK2_NO_1_8_V;
+		break;
+	case INTEL_MRFLD_SDIO:
+		slot->host->mmc->caps |= MMC_CAP_NONREMOVABLE |
+					 MMC_CAP_POWER_OFF_CARD;
+		break;
+	default:
 		return -ENODEV;
-
-	slot->host->mmc->caps |= MMC_CAP_8_BIT_DATA | MMC_CAP_NONREMOVABLE |
-				 MMC_CAP_1_8V_DDR;
-
+	}
 	return 0;
 }
 
@@ -1615,7 +1677,6 @@ static struct sdhci_pci_slot *sdhci_pci_probe_slot(
 
 	slot->chip = chip;
 	slot->host = host;
-	slot->pci_bar = bar;
 	slot->rst_n_gpio = -EINVAL;
 	slot->cd_gpio = -EINVAL;
 	slot->cd_idx = -1;
@@ -1637,33 +1698,30 @@ static struct sdhci_pci_slot *sdhci_pci_probe_slot(
 	}
 
 	host->hw_name = "PCI";
-	host->ops = &sdhci_pci_ops;
+	host->ops = chip->fixes && chip->fixes->ops ?
+		    chip->fixes->ops :
+		    &sdhci_pci_ops;
 	host->quirks = chip->quirks;
 	host->quirks2 = chip->quirks2;
 
 	host->irq = pdev->irq;
 
-	ret = pci_request_region(pdev, bar, mmc_hostname(host->mmc));
+	ret = pcim_iomap_regions(pdev, BIT(bar), mmc_hostname(host->mmc));
 	if (ret) {
 		dev_err(&pdev->dev, "cannot request region\n");
 		goto cleanup;
 	}
 
-	host->ioaddr = pci_ioremap_bar(pdev, bar);
-	if (!host->ioaddr) {
-		dev_err(&pdev->dev, "failed to remap registers\n");
-		ret = -ENOMEM;
-		goto release;
-	}
+	host->ioaddr = pcim_iomap_table(pdev)[bar];
 
 	if (chip->fixes && chip->fixes->probe_slot) {
 		ret = chip->fixes->probe_slot(slot);
 		if (ret)
-			goto unmap;
+			goto cleanup;
 	}
 
 	if (gpio_is_valid(slot->rst_n_gpio)) {
-		if (!gpio_request(slot->rst_n_gpio, "eMMC_reset")) {
+		if (!devm_gpio_request(&pdev->dev, slot->rst_n_gpio, "eMMC_reset")) {
 			gpio_direction_output(slot->rst_n_gpio, 1);
 			slot->host->mmc->caps |= MMC_CAP_HW_RESET;
 			slot->hw_reset = sdhci_pci_gpio_hw_reset;
@@ -1702,18 +1760,9 @@ static struct sdhci_pci_slot *sdhci_pci_probe_slot(
 	return slot;
 
 remove:
-	if (gpio_is_valid(slot->rst_n_gpio))
-		gpio_free(slot->rst_n_gpio);
-
 	if (chip->fixes && chip->fixes->remove_slot)
 		chip->fixes->remove_slot(slot, 0);
 
-unmap:
-	iounmap(host->ioaddr);
-
-release:
-	pci_release_region(pdev, bar);
-
 cleanup:
 	if (slot->data && slot->data->cleanup)
 		slot->data->cleanup(slot->data);
@@ -1738,17 +1787,12 @@ static void sdhci_pci_remove_slot(struct sdhci_pci_slot *slot)
 
 	sdhci_remove_host(slot->host, dead);
 
-	if (gpio_is_valid(slot->rst_n_gpio))
-		gpio_free(slot->rst_n_gpio);
-
 	if (slot->chip->fixes && slot->chip->fixes->remove_slot)
 		slot->chip->fixes->remove_slot(slot, dead);
 
 	if (slot->data && slot->data->cleanup)
 		slot->data->cleanup(slot->data);
 
-	pci_release_region(slot->chip->pdev, slot->pci_bar);
-
 	sdhci_free_host(slot->host);
 }
 
diff --git a/drivers/mmc/host/sdhci-pci.h b/drivers/mmc/host/sdhci-pci.h
index 7e0788712e1a..6bccf56bc5ff 100644
--- a/drivers/mmc/host/sdhci-pci.h
+++ b/drivers/mmc/host/sdhci-pci.h
@@ -65,6 +65,8 @@ struct sdhci_pci_fixes {
 
 	int			(*suspend) (struct sdhci_pci_chip *);
 	int			(*resume) (struct sdhci_pci_chip *);
+
+	const struct sdhci_ops	*ops;
 };
 
 struct sdhci_pci_slot {
@@ -72,7 +74,6 @@ struct sdhci_pci_slot {
 	struct sdhci_host	*host;
 	struct sdhci_pci_data	*data;
 
-	int			pci_bar;
 	int			rst_n_gpio;
 	int			cd_gpio;
 	int			cd_irq;
diff --git a/drivers/mmc/host/sdhci-pltfm.c b/drivers/mmc/host/sdhci-pltfm.c
index 1d17dcfc3ffb..ad49bfaf5bf8 100644
--- a/drivers/mmc/host/sdhci-pltfm.c
+++ b/drivers/mmc/host/sdhci-pltfm.c
@@ -156,13 +156,6 @@ struct sdhci_host *sdhci_pltfm_init(struct platform_device *pdev,
 		host->quirks2 = pdata->quirks2;
 	}
 
-	/*
-	 * Some platforms need to probe the controller to be able to
-	 * determine which caps should be used.
-	 */
-	if (host->ops && host->ops->platform_init)
-		host->ops->platform_init(host);
-
 	platform_set_drvdata(pdev, host);
 
 	return host;
diff --git a/drivers/mmc/host/sdhci-pxav3.c b/drivers/mmc/host/sdhci-pxav3.c
index dd1938d341f7..d0f5c05fbc19 100644
--- a/drivers/mmc/host/sdhci-pxav3.c
+++ b/drivers/mmc/host/sdhci-pxav3.c
@@ -315,7 +315,7 @@ static void pxav3_set_power(struct sdhci_host *host, unsigned char mode,
 	struct mmc_host *mmc = host->mmc;
 	u8 pwr = host->pwr;
 
-	sdhci_set_power(host, mode, vdd);
+	sdhci_set_power_noreg(host, mode, vdd);
 
 	if (host->pwr == pwr)
 		return;
diff --git a/drivers/mmc/host/sdhci-tegra.c b/drivers/mmc/host/sdhci-tegra.c
index 1e93dc4e303e..20b6ff5b4af1 100644
--- a/drivers/mmc/host/sdhci-tegra.c
+++ b/drivers/mmc/host/sdhci-tegra.c
@@ -391,6 +391,31 @@ static const struct sdhci_tegra_soc_data soc_data_tegra114 = {
 	.pdata = &sdhci_tegra114_pdata,
 };
 
+static const struct sdhci_pltfm_data sdhci_tegra124_pdata = {
+	.quirks = SDHCI_QUIRK_BROKEN_TIMEOUT_VAL |
+		  SDHCI_QUIRK_DATA_TIMEOUT_USES_SDCLK |
+		  SDHCI_QUIRK_SINGLE_POWER_WRITE |
+		  SDHCI_QUIRK_NO_HISPD_BIT |
+		  SDHCI_QUIRK_BROKEN_ADMA_ZEROLEN_DESC |
+		  SDHCI_QUIRK_CAP_CLOCK_BASE_BROKEN,
+	.quirks2 = SDHCI_QUIRK2_PRESET_VALUE_BROKEN |
+		   /*
+		    * The TRM states that the SD/MMC controller found on
+		    * Tegra124 can address 34 bits (the maximum supported by
+		    * the Tegra memory controller), but tests show that DMA
+		    * to or from above 4 GiB doesn't work. This is possibly
+		    * caused by missing programming, though it's not obvious
+		    * what sequence is required. Mark 64-bit DMA broken for
+		    * now to fix this for existing users (e.g. Nyan boards).
+		    */
+		   SDHCI_QUIRK2_BROKEN_64_BIT_DMA,
+	.ops  = &tegra114_sdhci_ops,
+};
+
+static const struct sdhci_tegra_soc_data soc_data_tegra124 = {
+	.pdata = &sdhci_tegra124_pdata,
+};
+
 static const struct sdhci_pltfm_data sdhci_tegra210_pdata = {
 	.quirks = SDHCI_QUIRK_BROKEN_TIMEOUT_VAL |
 		  SDHCI_QUIRK_DATA_TIMEOUT_USES_SDCLK |
@@ -408,7 +433,7 @@ static const struct sdhci_tegra_soc_data soc_data_tegra210 = {
 
 static const struct of_device_id sdhci_tegra_dt_match[] = {
 	{ .compatible = "nvidia,tegra210-sdhci", .data = &soc_data_tegra210 },
-	{ .compatible = "nvidia,tegra124-sdhci", .data = &soc_data_tegra114 },
+	{ .compatible = "nvidia,tegra124-sdhci", .data = &soc_data_tegra124 },
 	{ .compatible = "nvidia,tegra114-sdhci", .data = &soc_data_tegra114 },
 	{ .compatible = "nvidia,tegra30-sdhci", .data = &soc_data_tegra30 },
 	{ .compatible = "nvidia,tegra20-sdhci", .data = &soc_data_tegra20 },
diff --git a/drivers/mmc/host/sdhci.c b/drivers/mmc/host/sdhci.c
index cd65d474afa2..71654b90227f 100644
--- a/drivers/mmc/host/sdhci.c
+++ b/drivers/mmc/host/sdhci.c
@@ -687,7 +687,7 @@ static u8 sdhci_calc_timeout(struct sdhci_host *host, struct mmc_command *cmd)
 			 * host->clock is in Hz.  target_timeout is in us.
 			 * Hence, us = 1000000 * cycles / Hz.  Round up.
 			 */
-			val = 1000000 * data->timeout_clks;
+			val = 1000000ULL * data->timeout_clks;
 			if (do_div(val, host->clock))
 				target_timeout++;
 			target_timeout += val;
@@ -888,7 +888,8 @@ static void sdhci_prepare_data(struct sdhci_host *host, struct mmc_command *cmd)
 static inline bool sdhci_auto_cmd12(struct sdhci_host *host,
 				    struct mmc_request *mrq)
 {
-	return !mrq->sbc && (host->flags & SDHCI_AUTO_CMD12);
+	return !mrq->sbc && (host->flags & SDHCI_AUTO_CMD12) &&
+	       !mrq->cap_cmd_during_tfr;
 }
 
 static void sdhci_set_transfer_mode(struct sdhci_host *host,
@@ -1031,9 +1032,18 @@ static void sdhci_finish_data(struct sdhci_host *host)
 			sdhci_do_reset(host, SDHCI_RESET_DATA);
 		}
 
-		/* Avoid triggering warning in sdhci_send_command() */
-		host->cmd = NULL;
-		sdhci_send_command(host, data->stop);
+		/*
+		 * 'cap_cmd_during_tfr' request must not use the command line
+		 * after mmc_command_done() has been called. It is upper layer's
+		 * responsibility to send the stop command if required.
+		 */
+		if (data->mrq->cap_cmd_during_tfr) {
+			sdhci_finish_mrq(host, data->mrq);
+		} else {
+			/* Avoid triggering warning in sdhci_send_command() */
+			host->cmd = NULL;
+			sdhci_send_command(host, data->stop);
+		}
 	} else {
 		sdhci_finish_mrq(host, data->mrq);
 	}
@@ -1067,6 +1077,10 @@ void sdhci_send_command(struct sdhci_host *host, struct mmc_command *cmd)
 	/* Initially, a command has no error */
 	cmd->error = 0;
 
+	if ((host->quirks2 & SDHCI_QUIRK2_STOP_WITH_TC) &&
+	    cmd->opcode == MMC_STOP_TRANSMISSION)
+		cmd->flags |= MMC_RSP_BUSY;
+
 	/* Wait max 10 ms */
 	timeout = 10;
 
@@ -1165,6 +1179,9 @@ static void sdhci_finish_command(struct sdhci_host *host)
 		}
 	}
 
+	if (cmd->mrq->cap_cmd_during_tfr && cmd == cmd->mrq->cmd)
+		mmc_command_done(host->mmc, cmd->mrq);
+
 	/*
 	 * The host can send and interrupt when the busy state has
 	 * ended, allowing us to wait without wasting CPU cycles.
@@ -1377,8 +1394,8 @@ static void sdhci_set_power_reg(struct sdhci_host *host, unsigned char mode,
 		sdhci_writeb(host, 0, SDHCI_POWER_CONTROL);
 }
 
-void sdhci_set_power(struct sdhci_host *host, unsigned char mode,
-		     unsigned short vdd)
+void sdhci_set_power_noreg(struct sdhci_host *host, unsigned char mode,
+			   unsigned short vdd)
 {
 	u8 pwr = 0;
 
@@ -1442,20 +1459,17 @@ void sdhci_set_power(struct sdhci_host *host, unsigned char mode,
 			mdelay(10);
 	}
 }
-EXPORT_SYMBOL_GPL(sdhci_set_power);
+EXPORT_SYMBOL_GPL(sdhci_set_power_noreg);
 
-static void __sdhci_set_power(struct sdhci_host *host, unsigned char mode,
-			      unsigned short vdd)
+void sdhci_set_power(struct sdhci_host *host, unsigned char mode,
+		     unsigned short vdd)
 {
-	struct mmc_host *mmc = host->mmc;
-
-	if (host->ops->set_power)
-		host->ops->set_power(host, mode, vdd);
-	else if (!IS_ERR(mmc->supply.vmmc))
-		sdhci_set_power_reg(host, mode, vdd);
+	if (IS_ERR(host->mmc->supply.vmmc))
+		sdhci_set_power_noreg(host, mode, vdd);
 	else
-		sdhci_set_power(host, mode, vdd);
+		sdhci_set_power_reg(host, mode, vdd);
 }
+EXPORT_SYMBOL_GPL(sdhci_set_power);
 
 /*****************************************************************************\
  *                                                                           *
@@ -1596,7 +1610,10 @@ static void sdhci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 		}
 	}
 
-	__sdhci_set_power(host, ios->power_mode, ios->vdd);
+	if (host->ops->set_power)
+		host->ops->set_power(host, ios->power_mode, ios->vdd);
+	else
+		sdhci_set_power(host, ios->power_mode, ios->vdd);
 
 	if (host->ops->platform_send_init_74_clocks)
 		host->ops->platform_send_init_74_clocks(host, ios->power_mode);
@@ -2062,7 +2079,7 @@ static int sdhci_execute_tuning(struct mmc_host *mmc, u32 opcode)
 
 		spin_unlock_irqrestore(&host->lock, flags);
 		/* Wait for Buffer Read Ready interrupt */
-		wait_event_interruptible_timeout(host->buf_ready_int,
+		wait_event_timeout(host->buf_ready_int,
 					(host->tuning_done == 1),
 					msecs_to_jiffies(50));
 		spin_lock_irqsave(&host->lock, flags);
@@ -2396,7 +2413,7 @@ static void sdhci_timeout_data_timer(unsigned long data)
  *                                                                           *
 \*****************************************************************************/
 
-static void sdhci_cmd_irq(struct sdhci_host *host, u32 intmask, u32 *mask)
+static void sdhci_cmd_irq(struct sdhci_host *host, u32 intmask)
 {
 	if (!host->cmd) {
 		/*
@@ -2440,11 +2457,6 @@ static void sdhci_cmd_irq(struct sdhci_host *host, u32 intmask, u32 *mask)
 		return;
 	}
 
-	if ((host->quirks2 & SDHCI_QUIRK2_STOP_WITH_TC) &&
-	    !(host->cmd->flags & MMC_RSP_BUSY) && !host->data &&
-	    host->cmd->opcode == MMC_STOP_TRANSMISSION)
-		*mask &= ~SDHCI_INT_DATA_END;
-
 	if (intmask & SDHCI_INT_RESPONSE)
 		sdhci_finish_command(host);
 }
@@ -2667,8 +2679,7 @@ static irqreturn_t sdhci_irq(int irq, void *dev_id)
 		}
 
 		if (intmask & SDHCI_INT_CMD_MASK)
-			sdhci_cmd_irq(host, intmask & SDHCI_INT_CMD_MASK,
-				      &intmask);
+			sdhci_cmd_irq(host, intmask & SDHCI_INT_CMD_MASK);
 
 		if (intmask & SDHCI_INT_DATA_MASK)
 			sdhci_data_irq(host, intmask & SDHCI_INT_DATA_MASK);
diff --git a/drivers/mmc/host/sdhci.h b/drivers/mmc/host/sdhci.h
index 0411c9f36461..766df17fb7eb 100644
--- a/drivers/mmc/host/sdhci.h
+++ b/drivers/mmc/host/sdhci.h
@@ -84,6 +84,8 @@
 #define   SDHCI_CTRL_ADMA32	0x10
 #define   SDHCI_CTRL_ADMA64	0x18
 #define   SDHCI_CTRL_8BITBUS	0x20
+#define  SDHCI_CTRL_CDTEST_INS	0x40
+#define  SDHCI_CTRL_CDTEST_EN	0x80
 
 #define SDHCI_POWER_CONTROL	0x29
 #define  SDHCI_POWER_ON		0x01
@@ -555,7 +557,6 @@ struct sdhci_ops {
 	void	(*set_uhs_signaling)(struct sdhci_host *host, unsigned int uhs);
 	void	(*hw_reset)(struct sdhci_host *host);
 	void    (*adma_workaround)(struct sdhci_host *host, u32 intmask);
-	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,
@@ -682,6 +683,8 @@ u16 sdhci_calc_clk(struct sdhci_host *host, unsigned int clock,
 void sdhci_set_clock(struct sdhci_host *host, unsigned int clock);
 void sdhci_set_power(struct sdhci_host *host, unsigned char mode,
 		     unsigned short vdd);
+void sdhci_set_power_noreg(struct sdhci_host *host, unsigned char mode,
+			   unsigned short vdd);
 void sdhci_set_bus_width(struct sdhci_host *host, int width);
 void sdhci_reset(struct sdhci_host *host, u8 mask);
 void sdhci_set_uhs_signaling(struct sdhci_host *host, unsigned timing);
diff --git a/drivers/mmc/host/sh_mobile_sdhi.c b/drivers/mmc/host/sh_mobile_sdhi.c
index c3b651bf89cb..49edff7fee49 100644
--- a/drivers/mmc/host/sh_mobile_sdhi.c
+++ b/drivers/mmc/host/sh_mobile_sdhi.c
@@ -94,6 +94,7 @@ static const struct of_device_id sh_mobile_sdhi_of_match[] = {
 	{ .compatible = "renesas,sdhi-r8a7793", .data = &of_rcar_gen2_compatible, },
 	{ .compatible = "renesas,sdhi-r8a7794", .data = &of_rcar_gen2_compatible, },
 	{ .compatible = "renesas,sdhi-r8a7795", .data = &of_rcar_gen3_compatible, },
+	{ .compatible = "renesas,sdhi-r8a7796", .data = &of_rcar_gen3_compatible, },
 	{},
 };
 MODULE_DEVICE_TABLE(of, sh_mobile_sdhi_of_match);
@@ -213,6 +214,13 @@ static void sh_mobile_sdhi_clk_disable(struct tmio_mmc_host *host)
 	clk_disable_unprepare(priv->clk);
 }
 
+static int sh_mobile_sdhi_card_busy(struct mmc_host *mmc)
+{
+	struct tmio_mmc_host *host = mmc_priv(mmc);
+
+	return !(sd_ctrl_read16_and_16_as_32(host, CTL_STATUS) & TMIO_STAT_DAT0);
+}
+
 static int sh_mobile_sdhi_start_signal_voltage_switch(struct mmc_host *mmc,
 						      struct mmc_ios *ios)
 {
@@ -369,7 +377,14 @@ static int sh_mobile_sdhi_probe(struct platform_device *pdev)
 	host->clk_update	= sh_mobile_sdhi_clk_update;
 	host->clk_disable	= sh_mobile_sdhi_clk_disable;
 	host->multi_io_quirk	= sh_mobile_sdhi_multi_io_quirk;
-	host->start_signal_voltage_switch = sh_mobile_sdhi_start_signal_voltage_switch;
+
+	/* SDR speeds are only available on Gen2+ */
+	if (mmc_data->flags & TMIO_MMC_MIN_RCAR2) {
+		/* card_busy caused issues on r8a73a4 (pre-Gen2) CD-less SDHI */
+		host->card_busy	= sh_mobile_sdhi_card_busy;
+		host->start_signal_voltage_switch =
+			sh_mobile_sdhi_start_signal_voltage_switch;
+	}
 
 	/* Orginally registers were 16 bit apart, could be 32 or 64 nowadays */
 	if (!host->bus_shift && resource_size(res) > 0x100) /* old way to determine the shift */
diff --git a/drivers/mmc/host/sunxi-mmc.c b/drivers/mmc/host/sunxi-mmc.c
index 2ee4c21ec55e..c0a5c676d0e8 100644
--- a/drivers/mmc/host/sunxi-mmc.c
+++ b/drivers/mmc/host/sunxi-mmc.c
@@ -72,6 +72,13 @@
 #define SDXC_REG_CHDA	(0x90)
 #define SDXC_REG_CBDA	(0x94)
 
+/* New registers introduced in A64 */
+#define SDXC_REG_A12A		0x058 /* SMC Auto Command 12 Register */
+#define SDXC_REG_SD_NTSR	0x05C /* SMC New Timing Set Register */
+#define SDXC_REG_DRV_DL		0x140 /* Drive Delay Control Register */
+#define SDXC_REG_SAMP_DL_REG	0x144 /* SMC sample delay control */
+#define SDXC_REG_DS_DL_REG	0x148 /* SMC data strobe delay control */
+
 #define mmc_readl(host, reg) \
 	readl((host)->reg_base + SDXC_##reg)
 #define mmc_writel(host, reg, value) \
@@ -217,21 +224,41 @@
 #define SDXC_CLK_50M_DDR	3
 #define SDXC_CLK_50M_DDR_8BIT	4
 
+#define SDXC_2X_TIMING_MODE	BIT(31)
+
+#define SDXC_CAL_START		BIT(15)
+#define SDXC_CAL_DONE		BIT(14)
+#define SDXC_CAL_DL_SHIFT	8
+#define SDXC_CAL_DL_SW_EN	BIT(7)
+#define SDXC_CAL_DL_SW_SHIFT	0
+#define SDXC_CAL_DL_MASK	0x3f
+
+#define SDXC_CAL_TIMEOUT	3	/* in seconds, 3s is enough*/
+
 struct sunxi_mmc_clk_delay {
 	u32 output;
 	u32 sample;
 };
 
 struct sunxi_idma_des {
-	u32	config;
-	u32	buf_size;
-	u32	buf_addr_ptr1;
-	u32	buf_addr_ptr2;
+	__le32 config;
+	__le32 buf_size;
+	__le32 buf_addr_ptr1;
+	__le32 buf_addr_ptr2;
+};
+
+struct sunxi_mmc_cfg {
+	u32 idma_des_size_bits;
+	const struct sunxi_mmc_clk_delay *clk_delays;
+
+	/* does the IP block support autocalibration? */
+	bool can_calibrate;
 };
 
 struct sunxi_mmc_host {
 	struct mmc_host	*mmc;
 	struct reset_control *reset;
+	const struct sunxi_mmc_cfg *cfg;
 
 	/* IO mapping base */
 	void __iomem	*reg_base;
@@ -241,7 +268,6 @@ struct sunxi_mmc_host {
 	struct clk	*clk_mmc;
 	struct clk	*clk_sample;
 	struct clk	*clk_output;
-	const struct sunxi_mmc_clk_delay *clk_delays;
 
 	/* irq */
 	spinlock_t	lock;
@@ -250,7 +276,6 @@ struct sunxi_mmc_host {
 	u32		sdio_imask;
 
 	/* dma */
-	u32		idma_des_size_bits;
 	dma_addr_t	sg_dma;
 	void		*sg_cpu;
 	bool		wait_dma;
@@ -322,25 +347,28 @@ static void sunxi_mmc_init_idma_des(struct sunxi_mmc_host *host,
 {
 	struct sunxi_idma_des *pdes = (struct sunxi_idma_des *)host->sg_cpu;
 	dma_addr_t next_desc = host->sg_dma;
-	int i, max_len = (1 << host->idma_des_size_bits);
+	int i, max_len = (1 << host->cfg->idma_des_size_bits);
 
 	for (i = 0; i < data->sg_len; i++) {
-		pdes[i].config = SDXC_IDMAC_DES0_CH | SDXC_IDMAC_DES0_OWN |
-				 SDXC_IDMAC_DES0_DIC;
+		pdes[i].config = cpu_to_le32(SDXC_IDMAC_DES0_CH |
+					     SDXC_IDMAC_DES0_OWN |
+					     SDXC_IDMAC_DES0_DIC);
 
 		if (data->sg[i].length == max_len)
 			pdes[i].buf_size = 0; /* 0 == max_len */
 		else
-			pdes[i].buf_size = data->sg[i].length;
+			pdes[i].buf_size = cpu_to_le32(data->sg[i].length);
 
 		next_desc += sizeof(struct sunxi_idma_des);
-		pdes[i].buf_addr_ptr1 = sg_dma_address(&data->sg[i]);
-		pdes[i].buf_addr_ptr2 = (u32)next_desc;
+		pdes[i].buf_addr_ptr1 =
+			cpu_to_le32(sg_dma_address(&data->sg[i]));
+		pdes[i].buf_addr_ptr2 = cpu_to_le32((u32)next_desc);
 	}
 
-	pdes[0].config |= SDXC_IDMAC_DES0_FD;
-	pdes[i - 1].config |= SDXC_IDMAC_DES0_LD | SDXC_IDMAC_DES0_ER;
-	pdes[i - 1].config &= ~SDXC_IDMAC_DES0_DIC;
+	pdes[0].config |= cpu_to_le32(SDXC_IDMAC_DES0_FD);
+	pdes[i - 1].config |= cpu_to_le32(SDXC_IDMAC_DES0_LD |
+					  SDXC_IDMAC_DES0_ER);
+	pdes[i - 1].config &= cpu_to_le32(~SDXC_IDMAC_DES0_DIC);
 	pdes[i - 1].buf_addr_ptr2 = 0;
 
 	/*
@@ -653,11 +681,84 @@ static int sunxi_mmc_oclk_onoff(struct sunxi_mmc_host *host, u32 oclk_en)
 	return 0;
 }
 
+static int sunxi_mmc_calibrate(struct sunxi_mmc_host *host, int reg_off)
+{
+	u32 reg = readl(host->reg_base + reg_off);
+	u32 delay;
+	unsigned long timeout;
+
+	if (!host->cfg->can_calibrate)
+		return 0;
+
+	reg &= ~(SDXC_CAL_DL_MASK << SDXC_CAL_DL_SW_SHIFT);
+	reg &= ~SDXC_CAL_DL_SW_EN;
+
+	writel(reg | SDXC_CAL_START, host->reg_base + reg_off);
+
+	dev_dbg(mmc_dev(host->mmc), "calibration started\n");
+
+	timeout = jiffies + HZ * SDXC_CAL_TIMEOUT;
+
+	while (!((reg = readl(host->reg_base + reg_off)) & SDXC_CAL_DONE)) {
+		if (time_before(jiffies, timeout))
+			cpu_relax();
+		else {
+			reg &= ~SDXC_CAL_START;
+			writel(reg, host->reg_base + reg_off);
+
+			return -ETIMEDOUT;
+		}
+	}
+
+	delay = (reg >> SDXC_CAL_DL_SHIFT) & SDXC_CAL_DL_MASK;
+
+	reg &= ~SDXC_CAL_START;
+	reg |= (delay << SDXC_CAL_DL_SW_SHIFT) | SDXC_CAL_DL_SW_EN;
+
+	writel(reg, host->reg_base + reg_off);
+
+	dev_dbg(mmc_dev(host->mmc), "calibration ended, reg is 0x%x\n", reg);
+
+	return 0;
+}
+
+static int sunxi_mmc_clk_set_phase(struct sunxi_mmc_host *host,
+				   struct mmc_ios *ios, u32 rate)
+{
+	int index;
+
+	if (!host->cfg->clk_delays)
+		return 0;
+
+	/* determine delays */
+	if (rate <= 400000) {
+		index = SDXC_CLK_400K;
+	} else if (rate <= 25000000) {
+		index = SDXC_CLK_25M;
+	} else if (rate <= 52000000) {
+		if (ios->timing != MMC_TIMING_UHS_DDR50 &&
+		    ios->timing != MMC_TIMING_MMC_DDR52) {
+			index = SDXC_CLK_50M;
+		} else if (ios->bus_width == MMC_BUS_WIDTH_8) {
+			index = SDXC_CLK_50M_DDR_8BIT;
+		} else {
+			index = SDXC_CLK_50M_DDR;
+		}
+	} else {
+		return -EINVAL;
+	}
+
+	clk_set_phase(host->clk_sample, host->cfg->clk_delays[index].sample);
+	clk_set_phase(host->clk_output, host->cfg->clk_delays[index].output);
+
+	return 0;
+}
+
 static int sunxi_mmc_clk_set_rate(struct sunxi_mmc_host *host,
 				  struct mmc_ios *ios)
 {
-	u32 rate, oclk_dly, rval, sclk_dly;
-	u32 clock = ios->clock;
+	long rate;
+	u32 rval, clock = ios->clock;
 	int ret;
 
 	/* 8 bit DDR requires a higher module clock */
@@ -666,13 +767,18 @@ static int sunxi_mmc_clk_set_rate(struct sunxi_mmc_host *host,
 		clock <<= 1;
 
 	rate = clk_round_rate(host->clk_mmc, clock);
-	dev_dbg(mmc_dev(host->mmc), "setting clk to %d, rounded %d\n",
+	if (rate < 0) {
+		dev_err(mmc_dev(host->mmc), "error rounding clk to %d: %ld\n",
+			clock, rate);
+		return rate;
+	}
+	dev_dbg(mmc_dev(host->mmc), "setting clk to %d, rounded %ld\n",
 		clock, rate);
 
 	/* setting clock rate */
 	ret = clk_set_rate(host->clk_mmc, rate);
 	if (ret) {
-		dev_err(mmc_dev(host->mmc), "error setting clk to %d: %d\n",
+		dev_err(mmc_dev(host->mmc), "error setting clk to %ld: %d\n",
 			rate, ret);
 		return ret;
 	}
@@ -692,31 +798,15 @@ static int sunxi_mmc_clk_set_rate(struct sunxi_mmc_host *host,
 	}
 	mmc_writel(host, REG_CLKCR, rval);
 
-	/* determine delays */
-	if (rate <= 400000) {
-		oclk_dly = host->clk_delays[SDXC_CLK_400K].output;
-		sclk_dly = host->clk_delays[SDXC_CLK_400K].sample;
-	} else if (rate <= 25000000) {
-		oclk_dly = host->clk_delays[SDXC_CLK_25M].output;
-		sclk_dly = host->clk_delays[SDXC_CLK_25M].sample;
-	} else if (rate <= 52000000) {
-		if (ios->timing != MMC_TIMING_UHS_DDR50 &&
-		    ios->timing != MMC_TIMING_MMC_DDR52) {
-			oclk_dly = host->clk_delays[SDXC_CLK_50M].output;
-			sclk_dly = host->clk_delays[SDXC_CLK_50M].sample;
-		} else if (ios->bus_width == MMC_BUS_WIDTH_8) {
-			oclk_dly = host->clk_delays[SDXC_CLK_50M_DDR_8BIT].output;
-			sclk_dly = host->clk_delays[SDXC_CLK_50M_DDR_8BIT].sample;
-		} else {
-			oclk_dly = host->clk_delays[SDXC_CLK_50M_DDR].output;
-			sclk_dly = host->clk_delays[SDXC_CLK_50M_DDR].sample;
-		}
-	} else {
-		return -EINVAL;
-	}
+	ret = sunxi_mmc_clk_set_phase(host, ios, rate);
+	if (ret)
+		return ret;
+
+	ret = sunxi_mmc_calibrate(host, SDXC_REG_SAMP_DL_REG);
+	if (ret)
+		return ret;
 
-	clk_set_phase(host->clk_sample, sclk_dly);
-	clk_set_phase(host->clk_output, oclk_dly);
+	/* TODO: enable calibrate on sdc2 SDXC_REG_DS_DL_REG of A64 */
 
 	return sunxi_mmc_oclk_onoff(host, 1);
 }
@@ -938,14 +1028,6 @@ static int sunxi_mmc_card_busy(struct mmc_host *mmc)
 	return !!(mmc_readl(host, REG_STAS) & SDXC_CARD_DATA_BUSY);
 }
 
-static const struct of_device_id sunxi_mmc_of_match[] = {
-	{ .compatible = "allwinner,sun4i-a10-mmc", },
-	{ .compatible = "allwinner,sun5i-a13-mmc", },
-	{ .compatible = "allwinner,sun9i-a80-mmc", },
-	{ /* sentinel */ }
-};
-MODULE_DEVICE_TABLE(of, sunxi_mmc_of_match);
-
 static struct mmc_host_ops sunxi_mmc_ops = {
 	.request	 = sunxi_mmc_request,
 	.set_ios	 = sunxi_mmc_set_ios,
@@ -974,21 +1056,54 @@ static const struct sunxi_mmc_clk_delay sun9i_mmc_clk_delays[] = {
 	[SDXC_CLK_50M_DDR_8BIT]	= { .output =  72, .sample =  72 },
 };
 
+static const struct sunxi_mmc_cfg sun4i_a10_cfg = {
+	.idma_des_size_bits = 13,
+	.clk_delays = NULL,
+	.can_calibrate = false,
+};
+
+static const struct sunxi_mmc_cfg sun5i_a13_cfg = {
+	.idma_des_size_bits = 16,
+	.clk_delays = NULL,
+	.can_calibrate = false,
+};
+
+static const struct sunxi_mmc_cfg sun7i_a20_cfg = {
+	.idma_des_size_bits = 16,
+	.clk_delays = sunxi_mmc_clk_delays,
+	.can_calibrate = false,
+};
+
+static const struct sunxi_mmc_cfg sun9i_a80_cfg = {
+	.idma_des_size_bits = 16,
+	.clk_delays = sun9i_mmc_clk_delays,
+	.can_calibrate = false,
+};
+
+static const struct sunxi_mmc_cfg sun50i_a64_cfg = {
+	.idma_des_size_bits = 16,
+	.clk_delays = NULL,
+	.can_calibrate = true,
+};
+
+static const struct of_device_id sunxi_mmc_of_match[] = {
+	{ .compatible = "allwinner,sun4i-a10-mmc", .data = &sun4i_a10_cfg },
+	{ .compatible = "allwinner,sun5i-a13-mmc", .data = &sun5i_a13_cfg },
+	{ .compatible = "allwinner,sun7i-a20-mmc", .data = &sun7i_a20_cfg },
+	{ .compatible = "allwinner,sun9i-a80-mmc", .data = &sun9i_a80_cfg },
+	{ .compatible = "allwinner,sun50i-a64-mmc", .data = &sun50i_a64_cfg },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, sunxi_mmc_of_match);
+
 static int sunxi_mmc_resource_request(struct sunxi_mmc_host *host,
 				      struct platform_device *pdev)
 {
-	struct device_node *np = pdev->dev.of_node;
 	int ret;
 
-	if (of_device_is_compatible(np, "allwinner,sun4i-a10-mmc"))
-		host->idma_des_size_bits = 13;
-	else
-		host->idma_des_size_bits = 16;
-
-	if (of_device_is_compatible(np, "allwinner,sun9i-a80-mmc"))
-		host->clk_delays = sun9i_mmc_clk_delays;
-	else
-		host->clk_delays = sunxi_mmc_clk_delays;
+	host->cfg = of_device_get_match_data(&pdev->dev);
+	if (!host->cfg)
+		return -EINVAL;
 
 	ret = mmc_regulator_get_supply(host->mmc);
 	if (ret) {
@@ -1014,16 +1129,18 @@ static int sunxi_mmc_resource_request(struct sunxi_mmc_host *host,
 		return PTR_ERR(host->clk_mmc);
 	}
 
-	host->clk_output = devm_clk_get(&pdev->dev, "output");
-	if (IS_ERR(host->clk_output)) {
-		dev_err(&pdev->dev, "Could not get output clock\n");
-		return PTR_ERR(host->clk_output);
-	}
+	if (host->cfg->clk_delays) {
+		host->clk_output = devm_clk_get(&pdev->dev, "output");
+		if (IS_ERR(host->clk_output)) {
+			dev_err(&pdev->dev, "Could not get output clock\n");
+			return PTR_ERR(host->clk_output);
+		}
 
-	host->clk_sample = devm_clk_get(&pdev->dev, "sample");
-	if (IS_ERR(host->clk_sample)) {
-		dev_err(&pdev->dev, "Could not get sample clock\n");
-		return PTR_ERR(host->clk_sample);
+		host->clk_sample = devm_clk_get(&pdev->dev, "sample");
+		if (IS_ERR(host->clk_sample)) {
+			dev_err(&pdev->dev, "Could not get sample clock\n");
+			return PTR_ERR(host->clk_sample);
+		}
 	}
 
 	host->reset = devm_reset_control_get_optional(&pdev->dev, "ahb");
@@ -1120,15 +1237,17 @@ static int sunxi_mmc_probe(struct platform_device *pdev)
 	mmc->max_blk_count	= 8192;
 	mmc->max_blk_size	= 4096;
 	mmc->max_segs		= PAGE_SIZE / sizeof(struct sunxi_idma_des);
-	mmc->max_seg_size	= (1 << host->idma_des_size_bits);
+	mmc->max_seg_size	= (1 << host->cfg->idma_des_size_bits);
 	mmc->max_req_size	= mmc->max_seg_size * mmc->max_segs;
 	/* 400kHz ~ 52MHz */
 	mmc->f_min		=   400000;
 	mmc->f_max		= 52000000;
 	mmc->caps	       |= MMC_CAP_MMC_HIGHSPEED | MMC_CAP_SD_HIGHSPEED |
-				  MMC_CAP_1_8V_DDR |
 				  MMC_CAP_ERASE | MMC_CAP_SDIO_IRQ;
 
+	if (host->cfg->clk_delays)
+		mmc->caps      |= MMC_CAP_1_8V_DDR;
+
 	ret = mmc_of_parse(mmc);
 	if (ret)
 		goto error_free_dma;
@@ -1160,6 +1279,8 @@ static int sunxi_mmc_remove(struct platform_device *pdev)
 	if (!IS_ERR(host->reset))
 		reset_control_assert(host->reset);
 
+	clk_disable_unprepare(host->clk_sample);
+	clk_disable_unprepare(host->clk_output);
 	clk_disable_unprepare(host->clk_mmc);
 	clk_disable_unprepare(host->clk_ahb);
 
diff --git a/drivers/mmc/host/tmio_mmc.h b/drivers/mmc/host/tmio_mmc.h
index 7f63ec05bdf4..8e126afd988c 100644
--- a/drivers/mmc/host/tmio_mmc.h
+++ b/drivers/mmc/host/tmio_mmc.h
@@ -79,6 +79,9 @@
 #define	CLK_CTL_DIV_MASK	0xff
 #define	CLK_CTL_SCLKEN		BIT(8)
 
+#define CARD_OPT_WIDTH8		BIT(13)
+#define CARD_OPT_WIDTH		BIT(15)
+
 #define TMIO_BBS		512		/* Boot block size */
 
 /* Definitions for values the CTRL_SDIO_STATUS register can take. */
@@ -158,6 +161,7 @@ struct tmio_mmc_host {
 	void (*clk_disable)(struct tmio_mmc_host *host);
 	int (*multi_io_quirk)(struct mmc_card *card,
 			      unsigned int direction, int blk_size);
+	int (*card_busy)(struct mmc_host *mmc);
 	int (*start_signal_voltage_switch)(struct mmc_host *mmc,
 					   struct mmc_ios *ios);
 };
diff --git a/drivers/mmc/host/tmio_mmc_pio.c b/drivers/mmc/host/tmio_mmc_pio.c
index 92467efc4e2c..700567603107 100644
--- a/drivers/mmc/host/tmio_mmc_pio.c
+++ b/drivers/mmc/host/tmio_mmc_pio.c
@@ -336,7 +336,9 @@ static int tmio_mmc_start_command(struct tmio_mmc_host *host, struct mmc_command
 
 	switch (mmc_resp_type(cmd)) {
 	case MMC_RSP_NONE: c |= RESP_NONE; break;
-	case MMC_RSP_R1:   c |= RESP_R1;   break;
+	case MMC_RSP_R1:
+	case MMC_RSP_R1_NO_CRC:
+			   c |= RESP_R1;   break;
 	case MMC_RSP_R1B:  c |= RESP_R1B;  break;
 	case MMC_RSP_R2:   c |= RESP_R2;   break;
 	case MMC_RSP_R3:   c |= RESP_R3;   break;
@@ -730,12 +732,13 @@ static int tmio_mmc_start_data(struct tmio_mmc_host *host,
 	pr_debug("setup data transfer: blocksize %08x  nr_blocks %d\n",
 		 data->blksz, data->blocks);
 
-	/* Some hardware cannot perform 2 byte requests in 4 bit mode */
-	if (host->mmc->ios.bus_width == MMC_BUS_WIDTH_4) {
+	/* Some hardware cannot perform 2 byte requests in 4/8 bit mode */
+	if (host->mmc->ios.bus_width == MMC_BUS_WIDTH_4 ||
+	    host->mmc->ios.bus_width == MMC_BUS_WIDTH_8) {
 		int blksz_2bytes = pdata->flags & TMIO_MMC_BLKSZ_2BYTES;
 
 		if (data->blksz < 2 || (data->blksz < 4 && !blksz_2bytes)) {
-			pr_err("%s: %d byte block unsupported in 4 bit mode\n",
+			pr_err("%s: %d byte block unsupported in 4/8 bit mode\n",
 			       mmc_hostname(host->mmc), data->blksz);
 			return -EINVAL;
 		}
@@ -857,14 +860,16 @@ static void tmio_mmc_power_off(struct tmio_mmc_host *host)
 static void tmio_mmc_set_bus_width(struct tmio_mmc_host *host,
 				unsigned char bus_width)
 {
-	switch (bus_width) {
-	case MMC_BUS_WIDTH_1:
-		sd_ctrl_write16(host, CTL_SD_MEM_CARD_OPT, 0x80e0);
-		break;
-	case MMC_BUS_WIDTH_4:
-		sd_ctrl_write16(host, CTL_SD_MEM_CARD_OPT, 0x00e0);
-		break;
-	}
+	u16 reg = sd_ctrl_read16(host, CTL_SD_MEM_CARD_OPT)
+				& ~(CARD_OPT_WIDTH | CARD_OPT_WIDTH8);
+
+	/* reg now applies to MMC_BUS_WIDTH_4 */
+	if (bus_width == MMC_BUS_WIDTH_1)
+		reg |= CARD_OPT_WIDTH;
+	else if (bus_width == MMC_BUS_WIDTH_8)
+		reg |= CARD_OPT_WIDTH8;
+
+	sd_ctrl_write16(host, CTL_SD_MEM_CARD_OPT, reg);
 }
 
 /* Set MMC clock / power.
@@ -960,20 +965,12 @@ static int tmio_multi_io_quirk(struct mmc_card *card,
 	return blk_size;
 }
 
-static int tmio_mmc_card_busy(struct mmc_host *mmc)
-{
-	struct tmio_mmc_host *host = mmc_priv(mmc);
-
-	return !(sd_ctrl_read16_and_16_as_32(host, CTL_STATUS) & TMIO_STAT_DAT0);
-}
-
 static struct mmc_host_ops tmio_mmc_ops = {
 	.request	= tmio_mmc_request,
 	.set_ios	= tmio_mmc_set_ios,
 	.get_ro         = tmio_mmc_get_ro,
 	.get_cd		= mmc_gpio_get_cd,
 	.enable_sdio_irq = tmio_mmc_enable_sdio_irq,
-	.card_busy	= tmio_mmc_card_busy,
 	.multi_io_quirk	= tmio_multi_io_quirk,
 };
 
@@ -1072,6 +1069,7 @@ int tmio_mmc_host_probe(struct tmio_mmc_host *_host,
 		goto host_free;
 	}
 
+	tmio_mmc_ops.card_busy = _host->card_busy;
 	tmio_mmc_ops.start_signal_voltage_switch = _host->start_signal_voltage_switch;
 	mmc->ops = &tmio_mmc_ops;
 
@@ -1089,6 +1087,15 @@ int tmio_mmc_host_probe(struct tmio_mmc_host *_host,
 				  !mmc_card_is_removable(mmc) ||
 				  mmc->slot.cd_irq >= 0);
 
+	/*
+	 * On Gen2+, eMMC with NONREMOVABLE currently fails because native
+	 * hotplug gets disabled. It seems RuntimePM related yet we need further
+	 * research. Since we are planning a PM overhaul anyway, let's enforce
+	 * for now the device being active by enabling native hotplug always.
+	 */
+	if (pdata->flags & TMIO_MMC_MIN_RCAR2)
+		_host->native_hotplug = true;
+
 	if (tmio_mmc_clk_enable(_host) < 0) {
 		mmc->f_max = pdata->hclk;
 		mmc->f_min = mmc->f_max / 512;
diff --git a/drivers/mmc/host/vub300.c b/drivers/mmc/host/vub300.c
index 1e819f98b94f..bb3e0d1dd355 100644
--- a/drivers/mmc/host/vub300.c
+++ b/drivers/mmc/host/vub300.c
@@ -2116,13 +2116,11 @@ static int vub300_probe(struct usb_interface *interface,
 	command_out_urb = usb_alloc_urb(0, GFP_KERNEL);
 	if (!command_out_urb) {
 		retval = -ENOMEM;
-		dev_err(&udev->dev, "not enough memory for command_out_urb\n");
 		goto error0;
 	}
 	command_res_urb = usb_alloc_urb(0, GFP_KERNEL);
 	if (!command_res_urb) {
 		retval = -ENOMEM;
-		dev_err(&udev->dev, "not enough memory for command_res_urb\n");
 		goto error1;
 	}
 	/* this also allocates memory for our VUB300 mmc host device */
diff --git a/drivers/mtd/maps/Kconfig b/drivers/mtd/maps/Kconfig
index 392f9eff5fb7..5bcc896a48c3 100644
--- a/drivers/mtd/maps/Kconfig
+++ b/drivers/mtd/maps/Kconfig
@@ -78,7 +78,7 @@ config MTD_PHYSMAP_OF_VERSATILE
 	bool "Support ARM Versatile physmap OF"
 	depends on MTD_PHYSMAP_OF
 	depends on MFD_SYSCON
-	default y if (ARCH_INTEGRATOR || ARCH_VERSATILE || REALVIEW_DT)
+	default y if (ARCH_INTEGRATOR || ARCH_VERSATILE || ARCH_REALVIEW)
 	help
 	  This provides some extra DT physmap parsing for the ARM Versatile
 	  platforms, basically to add a VPP (write protection) callback so
diff --git a/drivers/mtd/mtdcore.c b/drivers/mtd/mtdcore.c
index e3936b847c6b..d46e4adf6d2b 100644
--- a/drivers/mtd/mtdcore.c
+++ b/drivers/mtd/mtdcore.c
@@ -39,7 +39,6 @@
 #include <linux/gfp.h>
 #include <linux/slab.h>
 #include <linux/reboot.h>
-#include <linux/kconfig.h>
 #include <linux/leds.h>
 
 #include <linux/mtd/mtd.h>
@@ -376,6 +375,110 @@ static int mtd_reboot_notifier(struct notifier_block *n, unsigned long state,
 }
 
 /**
+ * mtd_wunit_to_pairing_info - get pairing information of a wunit
+ * @mtd: pointer to new MTD device info structure
+ * @wunit: write unit we are interested in
+ * @info: returned pairing information
+ *
+ * Retrieve pairing information associated to the wunit.
+ * This is mainly useful when dealing with MLC/TLC NANDs where pages can be
+ * paired together, and where programming a page may influence the page it is
+ * paired with.
+ * The notion of page is replaced by the term wunit (write-unit) to stay
+ * consistent with the ->writesize field.
+ *
+ * The @wunit argument can be extracted from an absolute offset using
+ * mtd_offset_to_wunit(). @info is filled with the pairing information attached
+ * to @wunit.
+ *
+ * From the pairing info the MTD user can find all the wunits paired with
+ * @wunit using the following loop:
+ *
+ * for (i = 0; i < mtd_pairing_groups(mtd); i++) {
+ *	info.pair = i;
+ *	mtd_pairing_info_to_wunit(mtd, &info);
+ *	...
+ * }
+ */
+int mtd_wunit_to_pairing_info(struct mtd_info *mtd, int wunit,
+			      struct mtd_pairing_info *info)
+{
+	int npairs = mtd_wunit_per_eb(mtd) / mtd_pairing_groups(mtd);
+
+	if (wunit < 0 || wunit >= npairs)
+		return -EINVAL;
+
+	if (mtd->pairing && mtd->pairing->get_info)
+		return mtd->pairing->get_info(mtd, wunit, info);
+
+	info->group = 0;
+	info->pair = wunit;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(mtd_wunit_to_pairing_info);
+
+/**
+ * mtd_wunit_to_pairing_info - get wunit from pairing information
+ * @mtd: pointer to new MTD device info structure
+ * @info: pairing information struct
+ *
+ * Returns a positive number representing the wunit associated to the info
+ * struct, or a negative error code.
+ *
+ * This is the reverse of mtd_wunit_to_pairing_info(), and can help one to
+ * iterate over all wunits of a given pair (see mtd_wunit_to_pairing_info()
+ * doc).
+ *
+ * It can also be used to only program the first page of each pair (i.e.
+ * page attached to group 0), which allows one to use an MLC NAND in
+ * software-emulated SLC mode:
+ *
+ * info.group = 0;
+ * npairs = mtd_wunit_per_eb(mtd) / mtd_pairing_groups(mtd);
+ * for (info.pair = 0; info.pair < npairs; info.pair++) {
+ *	wunit = mtd_pairing_info_to_wunit(mtd, &info);
+ *	mtd_write(mtd, mtd_wunit_to_offset(mtd, blkoffs, wunit),
+ *		  mtd->writesize, &retlen, buf + (i * mtd->writesize));
+ * }
+ */
+int mtd_pairing_info_to_wunit(struct mtd_info *mtd,
+			      const struct mtd_pairing_info *info)
+{
+	int ngroups = mtd_pairing_groups(mtd);
+	int npairs = mtd_wunit_per_eb(mtd) / ngroups;
+
+	if (!info || info->pair < 0 || info->pair >= npairs ||
+	    info->group < 0 || info->group >= ngroups)
+		return -EINVAL;
+
+	if (mtd->pairing && mtd->pairing->get_wunit)
+		return mtd->pairing->get_wunit(mtd, info);
+
+	return info->pair;
+}
+EXPORT_SYMBOL_GPL(mtd_pairing_info_to_wunit);
+
+/**
+ * mtd_pairing_groups - get the number of pairing groups
+ * @mtd: pointer to new MTD device info structure
+ *
+ * Returns the number of pairing groups.
+ *
+ * This number is usually equal to the number of bits exposed by a single
+ * cell, and can be used in conjunction with mtd_pairing_info_to_wunit()
+ * to iterate over all pages of a given pair.
+ */
+int mtd_pairing_groups(struct mtd_info *mtd)
+{
+	if (!mtd->pairing || !mtd->pairing->ngroups)
+		return 1;
+
+	return mtd->pairing->ngroups;
+}
+EXPORT_SYMBOL_GPL(mtd_pairing_groups);
+
+/**
  *	add_mtd_device - register an MTD device
  *	@mtd: pointer to new MTD device info structure
  *
diff --git a/drivers/mtd/mtdpart.c b/drivers/mtd/mtdpart.c
index 1f13e32556f8..fccdd49bb964 100644
--- a/drivers/mtd/mtdpart.c
+++ b/drivers/mtd/mtdpart.c
@@ -30,7 +30,6 @@
 #include <linux/mtd/mtd.h>
 #include <linux/mtd/partitions.h>
 #include <linux/err.h>
-#include <linux/kconfig.h>
 
 #include "mtdcore.h"
 
@@ -317,6 +316,18 @@ static int part_block_markbad(struct mtd_info *mtd, loff_t ofs)
 	return res;
 }
 
+static int part_get_device(struct mtd_info *mtd)
+{
+	struct mtd_part *part = mtd_to_part(mtd);
+	return part->master->_get_device(part->master);
+}
+
+static void part_put_device(struct mtd_info *mtd)
+{
+	struct mtd_part *part = mtd_to_part(mtd);
+	part->master->_put_device(part->master);
+}
+
 static int part_ooblayout_ecc(struct mtd_info *mtd, int section,
 			      struct mtd_oob_region *oobregion)
 {
@@ -397,6 +408,7 @@ static struct mtd_part *allocate_partition(struct mtd_info *master,
 	slave->mtd.oobsize = master->oobsize;
 	slave->mtd.oobavail = master->oobavail;
 	slave->mtd.subpage_sft = master->subpage_sft;
+	slave->mtd.pairing = master->pairing;
 
 	slave->mtd.name = name;
 	slave->mtd.owner = master->owner;
@@ -463,6 +475,12 @@ static struct mtd_part *allocate_partition(struct mtd_info *master,
 		slave->mtd._block_isbad = part_block_isbad;
 	if (master->_block_markbad)
 		slave->mtd._block_markbad = part_block_markbad;
+
+	if (master->_get_device)
+		slave->mtd._get_device = part_get_device;
+	if (master->_put_device)
+		slave->mtd._put_device = part_put_device;
+
 	slave->mtd._erase = part_erase;
 	slave->master = master;
 	slave->offset = part->offset;
diff --git a/drivers/mtd/nand/Kconfig b/drivers/mtd/nand/Kconfig
index 21ff58099f3b..7b7a887b4709 100644
--- a/drivers/mtd/nand/Kconfig
+++ b/drivers/mtd/nand/Kconfig
@@ -88,11 +88,11 @@ config MTD_NAND_AMS_DELTA
 	  Support for NAND flash on Amstrad E3 (Delta).
 
 config MTD_NAND_OMAP2
-	tristate "NAND Flash device on OMAP2, OMAP3 and OMAP4"
-	depends on ARCH_OMAP2PLUS
+	tristate "NAND Flash device on OMAP2, OMAP3, OMAP4 and Keystone"
+	depends on (ARCH_OMAP2PLUS || ARCH_KEYSTONE)
 	help
-          Support for NAND flash on Texas Instruments OMAP2, OMAP3 and OMAP4
-	  platforms.
+          Support for NAND flash on Texas Instruments OMAP2, OMAP3, OMAP4
+	  and Keystone platforms.
 
 config MTD_NAND_OMAP_BCH
 	depends on MTD_NAND_OMAP2
@@ -428,7 +428,7 @@ config MTD_NAND_ORION
 
 config MTD_NAND_FSL_ELBC
 	tristate "NAND support for Freescale eLBC controllers"
-	depends on PPC
+	depends on FSL_SOC
 	select FSL_LBC
 	help
 	  Various Freescale chips, including the 8313, include a NAND Flash
@@ -438,7 +438,7 @@ config MTD_NAND_FSL_ELBC
 
 config MTD_NAND_FSL_IFC
 	tristate "NAND support for Freescale IFC controller"
-	depends on MTD_NAND && (FSL_SOC || ARCH_LAYERSCAPE)
+	depends on FSL_SOC || ARCH_LAYERSCAPE
 	select FSL_IFC
 	select MEMORY
 	help
diff --git a/drivers/mtd/nand/bf5xx_nand.c b/drivers/mtd/nand/bf5xx_nand.c
index 37da4236ab90..3962f55bd034 100644
--- a/drivers/mtd/nand/bf5xx_nand.c
+++ b/drivers/mtd/nand/bf5xx_nand.c
@@ -761,8 +761,7 @@ static int bf5xx_nand_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, info);
 
-	spin_lock_init(&info->controller.lock);
-	init_waitqueue_head(&info->controller.wq);
+	nand_hw_control_init(&info->controller);
 
 	info->device     = &pdev->dev;
 	info->platform   = plat;
diff --git a/drivers/mtd/nand/brcmnand/brcmnand.c b/drivers/mtd/nand/brcmnand/brcmnand.c
index 8eb2c64df38c..9d2424bfdbf5 100644
--- a/drivers/mtd/nand/brcmnand/brcmnand.c
+++ b/drivers/mtd/nand/brcmnand/brcmnand.c
@@ -1336,7 +1336,7 @@ static void brcmnand_cmdfunc(struct mtd_info *mtd, unsigned command,
 		u32 *flash_cache = (u32 *)ctrl->flash_cache;
 		int i;
 
-		brcmnand_soc_data_bus_prepare(ctrl->soc);
+		brcmnand_soc_data_bus_prepare(ctrl->soc, true);
 
 		/*
 		 * Must cache the FLASH_CACHE now, since changes in
@@ -1349,7 +1349,7 @@ static void brcmnand_cmdfunc(struct mtd_info *mtd, unsigned command,
 			 */
 			flash_cache[i] = be32_to_cpu(brcmnand_read_fc(ctrl, i));
 
-		brcmnand_soc_data_bus_unprepare(ctrl->soc);
+		brcmnand_soc_data_bus_unprepare(ctrl->soc, true);
 
 		/* Cleanup from HW quirk: restore SECTOR_SIZE_1K */
 		if (host->hwcfg.sector_size_1k)
@@ -1565,12 +1565,12 @@ static int brcmnand_read_by_pio(struct mtd_info *mtd, struct nand_chip *chip,
 		brcmnand_waitfunc(mtd, chip);
 
 		if (likely(buf)) {
-			brcmnand_soc_data_bus_prepare(ctrl->soc);
+			brcmnand_soc_data_bus_prepare(ctrl->soc, false);
 
 			for (j = 0; j < FC_WORDS; j++, buf++)
 				*buf = brcmnand_read_fc(ctrl, j);
 
-			brcmnand_soc_data_bus_unprepare(ctrl->soc);
+			brcmnand_soc_data_bus_unprepare(ctrl->soc, false);
 		}
 
 		if (oob)
@@ -1815,12 +1815,12 @@ static int brcmnand_write(struct mtd_info *mtd, struct nand_chip *chip,
 		(void)brcmnand_read_reg(ctrl, BRCMNAND_CMD_ADDRESS);
 
 		if (buf) {
-			brcmnand_soc_data_bus_prepare(ctrl->soc);
+			brcmnand_soc_data_bus_prepare(ctrl->soc, false);
 
 			for (j = 0; j < FC_WORDS; j++, buf++)
 				brcmnand_write_fc(ctrl, j, *buf);
 
-			brcmnand_soc_data_bus_unprepare(ctrl->soc);
+			brcmnand_soc_data_bus_unprepare(ctrl->soc, false);
 		} else if (oob) {
 			for (j = 0; j < FC_WORDS; j++)
 				brcmnand_write_fc(ctrl, j, 0xffffffff);
@@ -2370,8 +2370,7 @@ int brcmnand_probe(struct platform_device *pdev, struct brcmnand_soc *soc)
 
 	init_completion(&ctrl->done);
 	init_completion(&ctrl->dma_done);
-	spin_lock_init(&ctrl->controller.lock);
-	init_waitqueue_head(&ctrl->controller.wq);
+	nand_hw_control_init(&ctrl->controller);
 	INIT_LIST_HEAD(&ctrl->host_list);
 
 	/* NAND register range */
diff --git a/drivers/mtd/nand/brcmnand/brcmnand.h b/drivers/mtd/nand/brcmnand/brcmnand.h
index ef5eabba88e5..5c44cd4aba87 100644
--- a/drivers/mtd/nand/brcmnand/brcmnand.h
+++ b/drivers/mtd/nand/brcmnand/brcmnand.h
@@ -23,19 +23,22 @@ struct dev_pm_ops;
 struct brcmnand_soc {
 	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);
+	void (*prepare_data_bus)(struct brcmnand_soc *soc, bool prepare,
+				 bool is_param);
 };
 
-static inline void brcmnand_soc_data_bus_prepare(struct brcmnand_soc *soc)
+static inline void brcmnand_soc_data_bus_prepare(struct brcmnand_soc *soc,
+						 bool is_param)
 {
 	if (soc && soc->prepare_data_bus)
-		soc->prepare_data_bus(soc, true);
+		soc->prepare_data_bus(soc, true, is_param);
 }
 
-static inline void brcmnand_soc_data_bus_unprepare(struct brcmnand_soc *soc)
+static inline void brcmnand_soc_data_bus_unprepare(struct brcmnand_soc *soc,
+						   bool is_param)
 {
 	if (soc && soc->prepare_data_bus)
-		soc->prepare_data_bus(soc, false);
+		soc->prepare_data_bus(soc, false, is_param);
 }
 
 static inline u32 brcmnand_readl(void __iomem *addr)
diff --git a/drivers/mtd/nand/brcmnand/iproc_nand.c b/drivers/mtd/nand/brcmnand/iproc_nand.c
index 585596c549b2..4c6ae113664d 100644
--- a/drivers/mtd/nand/brcmnand/iproc_nand.c
+++ b/drivers/mtd/nand/brcmnand/iproc_nand.c
@@ -74,7 +74,8 @@ static void iproc_nand_intc_set(struct brcmnand_soc *soc, bool en)
 	spin_unlock_irqrestore(&priv->idm_lock, flags);
 }
 
-static void iproc_nand_apb_access(struct brcmnand_soc *soc, bool prepare)
+static void iproc_nand_apb_access(struct brcmnand_soc *soc, bool prepare,
+				  bool is_param)
 {
 	struct iproc_nand_soc *priv =
 			container_of(soc, struct iproc_nand_soc, soc);
@@ -86,10 +87,19 @@ static void iproc_nand_apb_access(struct brcmnand_soc *soc, bool prepare)
 
 	val = brcmnand_readl(mmio);
 
-	if (prepare)
-		val |= IPROC_NAND_APB_LE_MODE;
-	else
+	/*
+	 * In the case of BE or when dealing with NAND data, alway configure
+	 * the APB bus to LE mode before accessing the FIFO and back to BE mode
+	 * after the access is done
+	 */
+	if (IS_ENABLED(CONFIG_CPU_BIG_ENDIAN) || !is_param) {
+		if (prepare)
+			val |= IPROC_NAND_APB_LE_MODE;
+		else
+			val &= ~IPROC_NAND_APB_LE_MODE;
+	} else { /* when in LE accessing the parameter page, keep APB in BE */
 		val &= ~IPROC_NAND_APB_LE_MODE;
+	}
 
 	brcmnand_writel(val, mmio);
 
diff --git a/drivers/mtd/nand/davinci_nand.c b/drivers/mtd/nand/davinci_nand.c
index cc07ba0f044d..27fa8b87cd5f 100644
--- a/drivers/mtd/nand/davinci_nand.c
+++ b/drivers/mtd/nand/davinci_nand.c
@@ -240,6 +240,9 @@ static void nand_davinci_hwctl_4bit(struct mtd_info *mtd, int mode)
 	unsigned long flags;
 	u32 val;
 
+	/* Reset ECC hardware */
+	davinci_nand_readl(info, NAND_4BIT_ECC1_OFFSET);
+
 	spin_lock_irqsave(&davinci_nand_lock, flags);
 
 	/* Start 4-bit ECC calculation for read/write */
diff --git a/drivers/mtd/nand/docg4.c b/drivers/mtd/nand/docg4.c
index 47316998017f..7af2a3cd949e 100644
--- a/drivers/mtd/nand/docg4.c
+++ b/drivers/mtd/nand/docg4.c
@@ -1249,8 +1249,7 @@ static void __init init_mtd_structs(struct mtd_info *mtd)
 	nand->options = NAND_BUSWIDTH_16 | NAND_NO_SUBPAGE_WRITE;
 	nand->IO_ADDR_R = nand->IO_ADDR_W = doc->virtadr + DOC_IOSPACE_DATA;
 	nand->controller = &nand->hwcontrol;
-	spin_lock_init(&nand->controller->lock);
-	init_waitqueue_head(&nand->controller->wq);
+	nand_hw_control_init(nand->controller);
 
 	/* methods */
 	nand->cmdfunc = docg4_command;
diff --git a/drivers/mtd/nand/fsl_elbc_nand.c b/drivers/mtd/nand/fsl_elbc_nand.c
index 60a88f24c6b3..113f76e59937 100644
--- a/drivers/mtd/nand/fsl_elbc_nand.c
+++ b/drivers/mtd/nand/fsl_elbc_nand.c
@@ -879,8 +879,7 @@ static int fsl_elbc_nand_probe(struct platform_device *pdev)
 		}
 		elbc_fcm_ctrl->counter++;
 
-		spin_lock_init(&elbc_fcm_ctrl->controller.lock);
-		init_waitqueue_head(&elbc_fcm_ctrl->controller.wq);
+		nand_hw_control_init(&elbc_fcm_ctrl->controller);
 		fsl_lbc_ctrl_dev->nand = elbc_fcm_ctrl;
 	} else {
 		elbc_fcm_ctrl = fsl_lbc_ctrl_dev->nand;
diff --git a/drivers/mtd/nand/fsl_ifc_nand.c b/drivers/mtd/nand/fsl_ifc_nand.c
index 4e9e5fd8faf3..0a177b1bfe3e 100644
--- a/drivers/mtd/nand/fsl_ifc_nand.c
+++ b/drivers/mtd/nand/fsl_ifc_nand.c
@@ -987,8 +987,7 @@ static int fsl_ifc_nand_probe(struct platform_device *dev)
 		ifc_nand_ctrl->addr = NULL;
 		fsl_ifc_ctrl_dev->nand = ifc_nand_ctrl;
 
-		spin_lock_init(&ifc_nand_ctrl->controller.lock);
-		init_waitqueue_head(&ifc_nand_ctrl->controller.wq);
+		nand_hw_control_init(&ifc_nand_ctrl->controller);
 	} else {
 		ifc_nand_ctrl = fsl_ifc_ctrl_dev->nand;
 	}
diff --git a/drivers/mtd/nand/gpmi-nand/gpmi-nand.c b/drivers/mtd/nand/gpmi-nand/gpmi-nand.c
index 6e461560c6a8..6c062b8251d2 100644
--- a/drivers/mtd/nand/gpmi-nand/gpmi-nand.c
+++ b/drivers/mtd/nand/gpmi-nand/gpmi-nand.c
@@ -318,7 +318,8 @@ static int legacy_set_geometry(struct gpmi_nand_data *this)
 		return -EINVAL;
 	}
 
-	geo->page_size = mtd->writesize + mtd->oobsize;
+	geo->page_size = mtd->writesize + geo->metadata_size +
+		(geo->gf_len * geo->ecc_strength * geo->ecc_chunk_count) / 8;
 	geo->payload_size = mtd->writesize;
 
 	/*
diff --git a/drivers/mtd/nand/jz4780_nand.c b/drivers/mtd/nand/jz4780_nand.c
index 175f67da25af..a39bb70175ee 100644
--- a/drivers/mtd/nand/jz4780_nand.c
+++ b/drivers/mtd/nand/jz4780_nand.c
@@ -368,9 +368,8 @@ static int jz4780_nand_probe(struct platform_device *pdev)
 	nfc->dev = dev;
 	nfc->num_banks = num_banks;
 
-	spin_lock_init(&nfc->controller.lock);
+	nand_hw_control_init(&nfc->controller);
 	INIT_LIST_HEAD(&nfc->chips);
-	init_waitqueue_head(&nfc->controller.wq);
 
 	ret = jz4780_nand_init_chips(nfc, pdev);
 	if (ret) {
diff --git a/drivers/mtd/nand/mxc_nand.c b/drivers/mtd/nand/mxc_nand.c
index 57cbe2b83849..d7f724b24fd7 100644
--- a/drivers/mtd/nand/mxc_nand.c
+++ b/drivers/mtd/nand/mxc_nand.c
@@ -152,6 +152,9 @@ struct mxc_nand_devtype_data {
 	void (*select_chip)(struct mtd_info *mtd, int chip);
 	int (*correct_data)(struct mtd_info *mtd, u_char *dat,
 			u_char *read_ecc, u_char *calc_ecc);
+	int (*setup_data_interface)(struct mtd_info *mtd,
+				    const struct nand_data_interface *conf,
+				    bool check_only);
 
 	/*
 	 * On i.MX21 the CONFIG2:INT bit cannot be read if interrupts are masked
@@ -1012,6 +1015,82 @@ static void preset_v1(struct mtd_info *mtd)
 	writew(0x4, NFC_V1_V2_WRPROT);
 }
 
+static int mxc_nand_v2_setup_data_interface(struct mtd_info *mtd,
+					const struct nand_data_interface *conf,
+					bool check_only)
+{
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct mxc_nand_host *host = nand_get_controller_data(nand_chip);
+	int tRC_min_ns, tRC_ps, ret;
+	unsigned long rate, rate_round;
+	const struct nand_sdr_timings *timings;
+	u16 config1;
+
+	timings = nand_get_sdr_timings(conf);
+	if (IS_ERR(timings))
+		return -ENOTSUPP;
+
+	config1 = readw(NFC_V1_V2_CONFIG1);
+
+	tRC_min_ns = timings->tRC_min / 1000;
+	rate = 1000000000 / tRC_min_ns;
+
+	/*
+	 * For tRC < 30ns we have to use EDO mode. In this case the controller
+	 * does one access per clock cycle. Otherwise the controller does one
+	 * access in two clock cycles, thus we have to double the rate to the
+	 * controller.
+	 */
+	if (tRC_min_ns < 30) {
+		rate_round = clk_round_rate(host->clk, rate);
+		config1 |= NFC_V2_CONFIG1_ONE_CYCLE;
+		tRC_ps = 1000000000 / (rate_round / 1000);
+	} else {
+		rate *= 2;
+		rate_round = clk_round_rate(host->clk, rate);
+		config1 &= ~NFC_V2_CONFIG1_ONE_CYCLE;
+		tRC_ps = 1000000000 / (rate_round / 1000 / 2);
+	}
+
+	/*
+	 * The timing values compared against are from the i.MX25 Automotive
+	 * datasheet, Table 50. NFC Timing Parameters
+	 */
+	if (timings->tCLS_min > tRC_ps - 1000 ||
+	    timings->tCLH_min > tRC_ps - 2000 ||
+	    timings->tCS_min > tRC_ps - 1000 ||
+	    timings->tCH_min > tRC_ps - 2000 ||
+	    timings->tWP_min > tRC_ps - 1500 ||
+	    timings->tALS_min > tRC_ps ||
+	    timings->tALH_min > tRC_ps - 3000 ||
+	    timings->tDS_min > tRC_ps ||
+	    timings->tDH_min > tRC_ps - 5000 ||
+	    timings->tWC_min > 2 * tRC_ps ||
+	    timings->tWH_min > tRC_ps - 2500 ||
+	    timings->tRR_min > 6 * tRC_ps ||
+	    timings->tRP_min > 3 * tRC_ps / 2 ||
+	    timings->tRC_min > 2 * tRC_ps ||
+	    timings->tREH_min > (tRC_ps / 2) - 2500) {
+		dev_dbg(host->dev, "Timing out of bounds\n");
+		return -EINVAL;
+	}
+
+	if (check_only)
+		return 0;
+
+	ret = clk_set_rate(host->clk, rate);
+	if (ret)
+		return ret;
+
+	writew(config1, NFC_V1_V2_CONFIG1);
+
+	dev_dbg(host->dev, "Setting rate to %ldHz, %s mode\n", rate_round,
+		config1 & NFC_V2_CONFIG1_ONE_CYCLE ? "One cycle (EDO)" :
+		"normal");
+
+	return 0;
+}
+
 static void preset_v2(struct mtd_info *mtd)
 {
 	struct nand_chip *nand_chip = mtd_to_nand(mtd);
@@ -1239,6 +1318,57 @@ static void mxc_nand_command(struct mtd_info *mtd, unsigned command,
 	}
 }
 
+static int mxc_nand_onfi_set_features(struct mtd_info *mtd,
+				      struct nand_chip *chip, int addr,
+				      u8 *subfeature_param)
+{
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct mxc_nand_host *host = nand_get_controller_data(nand_chip);
+	int i;
+
+	if (!chip->onfi_version ||
+	    !(le16_to_cpu(chip->onfi_params.opt_cmd)
+	      & ONFI_OPT_CMD_SET_GET_FEATURES))
+		return -EINVAL;
+
+	host->buf_start = 0;
+
+	for (i = 0; i < ONFI_SUBFEATURE_PARAM_LEN; ++i)
+		chip->write_byte(mtd, subfeature_param[i]);
+
+	memcpy32_toio(host->main_area0, host->data_buf, mtd->writesize);
+	host->devtype_data->send_cmd(host, NAND_CMD_SET_FEATURES, false);
+	mxc_do_addr_cycle(mtd, addr, -1);
+	host->devtype_data->send_page(mtd, NFC_INPUT);
+
+	return 0;
+}
+
+static int mxc_nand_onfi_get_features(struct mtd_info *mtd,
+				      struct nand_chip *chip, int addr,
+				      u8 *subfeature_param)
+{
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct mxc_nand_host *host = nand_get_controller_data(nand_chip);
+	int i;
+
+	if (!chip->onfi_version ||
+	    !(le16_to_cpu(chip->onfi_params.opt_cmd)
+	      & ONFI_OPT_CMD_SET_GET_FEATURES))
+		return -EINVAL;
+
+	host->devtype_data->send_cmd(host, NAND_CMD_GET_FEATURES, false);
+	mxc_do_addr_cycle(mtd, addr, -1);
+	host->devtype_data->send_page(mtd, NFC_OUTPUT);
+	memcpy32_fromio(host->data_buf, host->main_area0, 512);
+	host->buf_start = 0;
+
+	for (i = 0; i < ONFI_SUBFEATURE_PARAM_LEN; ++i)
+		*subfeature_param++ = chip->read_byte(mtd);
+
+	return 0;
+}
+
 /*
  * The generic flash bbt decriptors overlap with our ecc
  * hardware, so define some i.MX specific ones.
@@ -1327,6 +1457,7 @@ static const struct mxc_nand_devtype_data imx25_nand_devtype_data = {
 	.ooblayout = &mxc_v2_ooblayout_ops,
 	.select_chip = mxc_nand_select_chip_v2,
 	.correct_data = mxc_nand_correct_data_v2_v3,
+	.setup_data_interface = mxc_nand_v2_setup_data_interface,
 	.irqpending_quirk = 0,
 	.needs_ip = 0,
 	.regs_offset = 0x1e00,
@@ -1434,7 +1565,7 @@ static const struct platform_device_id mxcnd_devtype[] = {
 };
 MODULE_DEVICE_TABLE(platform, mxcnd_devtype);
 
-#ifdef CONFIG_OF_MTD
+#ifdef CONFIG_OF
 static const struct of_device_id mxcnd_dt_ids[] = {
 	{
 		.compatible = "fsl,imx21-nand",
@@ -1513,6 +1644,8 @@ static int mxcnd_probe(struct platform_device *pdev)
 	this->read_word = mxc_nand_read_word;
 	this->write_buf = mxc_nand_write_buf;
 	this->read_buf = mxc_nand_read_buf;
+	this->onfi_set_features = mxc_nand_onfi_set_features;
+	this->onfi_get_features = mxc_nand_onfi_get_features;
 
 	host->clk = devm_clk_get(&pdev->dev, NULL);
 	if (IS_ERR(host->clk))
@@ -1533,6 +1666,8 @@ static int mxcnd_probe(struct platform_device *pdev)
 	if (err < 0)
 		return err;
 
+	this->setup_data_interface = host->devtype_data->setup_data_interface;
+
 	if (host->devtype_data->needs_ip) {
 		res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 		host->regs_ip = devm_ioremap_resource(&pdev->dev, res);
diff --git a/drivers/mtd/nand/nand_base.c b/drivers/mtd/nand/nand_base.c
index 77533f7f2429..e5718e5ecf92 100644
--- a/drivers/mtd/nand/nand_base.c
+++ b/drivers/mtd/nand/nand_base.c
@@ -745,7 +745,10 @@ static void nand_command_lp(struct mtd_info *mtd, unsigned int command,
 				column >>= 1;
 			chip->cmd_ctrl(mtd, column, ctrl);
 			ctrl &= ~NAND_CTRL_CHANGE;
-			chip->cmd_ctrl(mtd, column >> 8, ctrl);
+
+			/* Only output a single addr cycle for 8bits opcodes. */
+			if (!nand_opcode_8bits(command))
+				chip->cmd_ctrl(mtd, column >> 8, ctrl);
 		}
 		if (page_addr != -1) {
 			chip->cmd_ctrl(mtd, page_addr, ctrl);
@@ -948,6 +951,172 @@ static int nand_wait(struct mtd_info *mtd, struct nand_chip *chip)
 }
 
 /**
+ * nand_reset_data_interface - Reset data interface and timings
+ * @chip: The NAND chip
+ *
+ * Reset the Data interface and timings to ONFI mode 0.
+ *
+ * Returns 0 for success or negative error code otherwise.
+ */
+static int nand_reset_data_interface(struct nand_chip *chip)
+{
+	struct mtd_info *mtd = nand_to_mtd(chip);
+	const struct nand_data_interface *conf;
+	int ret;
+
+	if (!chip->setup_data_interface)
+		return 0;
+
+	/*
+	 * The ONFI specification says:
+	 * "
+	 * To transition from NV-DDR or NV-DDR2 to the SDR data
+	 * interface, the host shall use the Reset (FFh) command
+	 * using SDR timing mode 0. A device in any timing mode is
+	 * required to recognize Reset (FFh) command issued in SDR
+	 * timing mode 0.
+	 * "
+	 *
+	 * Configure the data interface in SDR mode and set the
+	 * timings to timing mode 0.
+	 */
+
+	conf = nand_get_default_data_interface();
+	ret = chip->setup_data_interface(mtd, conf, false);
+	if (ret)
+		pr_err("Failed to configure data interface to SDR timing mode 0\n");
+
+	return ret;
+}
+
+/**
+ * nand_setup_data_interface - Setup the best data interface and timings
+ * @chip: The NAND chip
+ *
+ * Find and configure the best data interface and NAND timings supported by
+ * the chip and the driver.
+ * First tries to retrieve supported timing modes from ONFI information,
+ * and if the NAND chip does not support ONFI, relies on the
+ * ->onfi_timing_mode_default specified in the nand_ids table.
+ *
+ * Returns 0 for success or negative error code otherwise.
+ */
+static int nand_setup_data_interface(struct nand_chip *chip)
+{
+	struct mtd_info *mtd = nand_to_mtd(chip);
+	int ret;
+
+	if (!chip->setup_data_interface || !chip->data_interface)
+		return 0;
+
+	/*
+	 * Ensure the timing mode has been changed on the chip side
+	 * before changing timings on the controller side.
+	 */
+	if (chip->onfi_version) {
+		u8 tmode_param[ONFI_SUBFEATURE_PARAM_LEN] = {
+			chip->onfi_timing_mode_default,
+		};
+
+		ret = chip->onfi_set_features(mtd, chip,
+				ONFI_FEATURE_ADDR_TIMING_MODE,
+				tmode_param);
+		if (ret)
+			goto err;
+	}
+
+	ret = chip->setup_data_interface(mtd, chip->data_interface, false);
+err:
+	return ret;
+}
+
+/**
+ * nand_init_data_interface - find the best data interface and timings
+ * @chip: The NAND chip
+ *
+ * Find the best data interface and NAND timings supported by the chip
+ * and the driver.
+ * First tries to retrieve supported timing modes from ONFI information,
+ * and if the NAND chip does not support ONFI, relies on the
+ * ->onfi_timing_mode_default specified in the nand_ids table. After this
+ * function nand_chip->data_interface is initialized with the best timing mode
+ * available.
+ *
+ * Returns 0 for success or negative error code otherwise.
+ */
+static int nand_init_data_interface(struct nand_chip *chip)
+{
+	struct mtd_info *mtd = nand_to_mtd(chip);
+	int modes, mode, ret;
+
+	if (!chip->setup_data_interface)
+		return 0;
+
+	/*
+	 * First try to identify the best timings from ONFI parameters and
+	 * if the NAND does not support ONFI, fallback to the default ONFI
+	 * timing mode.
+	 */
+	modes = onfi_get_async_timing_mode(chip);
+	if (modes == ONFI_TIMING_MODE_UNKNOWN) {
+		if (!chip->onfi_timing_mode_default)
+			return 0;
+
+		modes = GENMASK(chip->onfi_timing_mode_default, 0);
+	}
+
+	chip->data_interface = kzalloc(sizeof(*chip->data_interface),
+				       GFP_KERNEL);
+	if (!chip->data_interface)
+		return -ENOMEM;
+
+	for (mode = fls(modes) - 1; mode >= 0; mode--) {
+		ret = onfi_init_data_interface(chip, chip->data_interface,
+					       NAND_SDR_IFACE, mode);
+		if (ret)
+			continue;
+
+		ret = chip->setup_data_interface(mtd, chip->data_interface,
+						 true);
+		if (!ret) {
+			chip->onfi_timing_mode_default = mode;
+			break;
+		}
+	}
+
+	return 0;
+}
+
+static void nand_release_data_interface(struct nand_chip *chip)
+{
+	kfree(chip->data_interface);
+}
+
+/**
+ * nand_reset - Reset and initialize a NAND device
+ * @chip: The NAND chip
+ *
+ * Returns 0 for success or negative error code otherwise
+ */
+int nand_reset(struct nand_chip *chip)
+{
+	struct mtd_info *mtd = nand_to_mtd(chip);
+	int ret;
+
+	ret = nand_reset_data_interface(chip);
+	if (ret)
+		return ret;
+
+	chip->cmdfunc(mtd, NAND_CMD_RESET, -1, -1);
+
+	ret = nand_setup_data_interface(chip);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+/**
  * __nand_unlock - [REPLACEABLE] unlocks specified locked blocks
  * @mtd: mtd info
  * @ofs: offset to start unlock from
@@ -1025,7 +1194,7 @@ int nand_unlock(struct mtd_info *mtd, loff_t ofs, uint64_t len)
 	 * some operation can also clear the bit 7 of status register
 	 * eg. erase/program a locked block
 	 */
-	chip->cmdfunc(mtd, NAND_CMD_RESET, -1, -1);
+	nand_reset(chip);
 
 	/* Check, if it is write protected */
 	if (nand_check_wp(mtd)) {
@@ -1084,7 +1253,7 @@ int nand_lock(struct mtd_info *mtd, loff_t ofs, uint64_t len)
 	 * some operation can also clear the bit 7 of status register
 	 * eg. erase/program a locked block
 	 */
-	chip->cmdfunc(mtd, NAND_CMD_RESET, -1, -1);
+	nand_reset(chip);
 
 	/* Check, if it is write protected */
 	if (nand_check_wp(mtd)) {
@@ -2162,7 +2331,7 @@ static int nand_do_read_oob(struct mtd_info *mtd, loff_t from,
 static int nand_read_oob(struct mtd_info *mtd, loff_t from,
 			 struct mtd_oob_ops *ops)
 {
-	int ret = -ENOTSUPP;
+	int ret;
 
 	ops->retlen = 0;
 
@@ -2173,24 +2342,18 @@ static int nand_read_oob(struct mtd_info *mtd, loff_t from,
 		return -EINVAL;
 	}
 
-	nand_get_device(mtd, FL_READING);
-
-	switch (ops->mode) {
-	case MTD_OPS_PLACE_OOB:
-	case MTD_OPS_AUTO_OOB:
-	case MTD_OPS_RAW:
-		break;
+	if (ops->mode != MTD_OPS_PLACE_OOB &&
+	    ops->mode != MTD_OPS_AUTO_OOB &&
+	    ops->mode != MTD_OPS_RAW)
+		return -ENOTSUPP;
 
-	default:
-		goto out;
-	}
+	nand_get_device(mtd, FL_READING);
 
 	if (!ops->datbuf)
 		ret = nand_do_read_oob(mtd, from, ops);
 	else
 		ret = nand_do_read_ops(mtd, from, ops);
 
-out:
 	nand_release_device(mtd);
 	return ret;
 }
@@ -2788,7 +2951,7 @@ static int nand_do_write_oob(struct mtd_info *mtd, loff_t to,
 	 * if we don't do this. I have no clue why, but I seem to have 'fixed'
 	 * it in the doc2000 driver in August 1999.  dwmw2.
 	 */
-	chip->cmdfunc(mtd, NAND_CMD_RESET, -1, -1);
+	nand_reset(chip);
 
 	/* Check, if it is write protected */
 	if (nand_check_wp(mtd)) {
@@ -3191,8 +3354,7 @@ static void nand_set_defaults(struct nand_chip *chip, int busw)
 
 	if (!chip->controller) {
 		chip->controller = &chip->hwcontrol;
-		spin_lock_init(&chip->controller->lock);
-		init_waitqueue_head(&chip->controller->wq);
+		nand_hw_control_init(chip->controller);
 	}
 
 }
@@ -3829,7 +3991,7 @@ static struct nand_flash_dev *nand_get_flash_type(struct mtd_info *mtd,
 	 * Reset the chip, required by some chips (e.g. Micron MT29FxGxxxxx)
 	 * after power-up.
 	 */
-	chip->cmdfunc(mtd, NAND_CMD_RESET, -1, -1);
+	nand_reset(chip);
 
 	/* Send the command for reading device ID */
 	chip->cmdfunc(mtd, NAND_CMD_READID, 0x00, -1);
@@ -4113,6 +4275,9 @@ static int nand_dt_init(struct nand_chip *chip)
 	if (ecc_step > 0)
 		chip->ecc.size = ecc_step;
 
+	if (of_property_read_bool(dn, "nand-ecc-maximize"))
+		chip->ecc.options |= NAND_ECC_MAXIMIZE;
+
 	return 0;
 }
 
@@ -4141,6 +4306,15 @@ int nand_scan_ident(struct mtd_info *mtd, int maxchips,
 	if (!mtd->name && mtd->dev.parent)
 		mtd->name = dev_name(mtd->dev.parent);
 
+	if ((!chip->cmdfunc || !chip->select_chip) && !chip->cmd_ctrl) {
+		/*
+		 * Default functions assigned for chip_select() and
+		 * cmdfunc() both expect cmd_ctrl() to be populated,
+		 * so we need to check that that's the case
+		 */
+		pr_err("chip.cmd_ctrl() callback is not provided");
+		return -EINVAL;
+	}
 	/* Set the default functions */
 	nand_set_defaults(chip, chip->options & NAND_BUSWIDTH_16);
 
@@ -4155,13 +4329,17 @@ int nand_scan_ident(struct mtd_info *mtd, int maxchips,
 		return PTR_ERR(type);
 	}
 
+	ret = nand_init_data_interface(chip);
+	if (ret)
+		return ret;
+
 	chip->select_chip(mtd, -1);
 
 	/* Check for a chip array */
 	for (i = 1; i < maxchips; i++) {
 		chip->select_chip(mtd, i);
 		/* See comment in nand_get_flash_type for reset */
-		chip->cmdfunc(mtd, NAND_CMD_RESET, -1, -1);
+		nand_reset(chip);
 		/* Send the command for reading device ID */
 		chip->cmdfunc(mtd, NAND_CMD_READID, 0x00, -1);
 		/* Read manufacturer and device IDs */
@@ -4221,6 +4399,7 @@ static int nand_set_ecc_soft_ops(struct mtd_info *mtd)
 		ecc->write_page_raw = nand_write_page_raw;
 		ecc->read_oob = nand_read_oob_std;
 		ecc->write_oob = nand_write_oob_std;
+
 		/*
 		* Board driver should supply ecc.size and ecc.strength
 		* values to select how many bits are correctable.
@@ -4243,6 +4422,25 @@ static int nand_set_ecc_soft_ops(struct mtd_info *mtd)
 			}
 
 			mtd_set_ooblayout(mtd, &nand_ooblayout_lp_ops);
+
+		}
+
+		/*
+		 * We can only maximize ECC config when the default layout is
+		 * used, otherwise we don't know how many bytes can really be
+		 * used.
+		 */
+		if (mtd->ooblayout == &nand_ooblayout_lp_ops &&
+		    ecc->options & NAND_ECC_MAXIMIZE) {
+			int steps, bytes;
+
+			/* Always prefer 1k blocks over 512bytes ones */
+			ecc->size = 1024;
+			steps = mtd->writesize / ecc->size;
+
+			/* Reserve 2 bytes for the BBM */
+			bytes = (mtd->oobsize - 2) / steps;
+			ecc->strength = bytes * 8 / fls(8 * ecc->size);
 		}
 
 		/* See nand_bch_init() for details. */
@@ -4601,18 +4799,16 @@ int nand_scan(struct mtd_info *mtd, int maxchips)
 EXPORT_SYMBOL(nand_scan);
 
 /**
- * nand_release - [NAND Interface] Free resources held by the NAND device
- * @mtd: MTD device structure
+ * nand_cleanup - [NAND Interface] Free resources held by the NAND device
+ * @chip: NAND chip object
  */
-void nand_release(struct mtd_info *mtd)
+void nand_cleanup(struct nand_chip *chip)
 {
-	struct nand_chip *chip = mtd_to_nand(mtd);
-
 	if (chip->ecc.mode == NAND_ECC_SOFT &&
 	    chip->ecc.algo == NAND_ECC_BCH)
 		nand_bch_free((struct nand_bch_control *)chip->ecc.priv);
 
-	mtd_device_unregister(mtd);
+	nand_release_data_interface(chip);
 
 	/* Free bad block table memory */
 	kfree(chip->bbt);
@@ -4624,6 +4820,18 @@ void nand_release(struct mtd_info *mtd)
 			& NAND_BBT_DYNAMICSTRUCT)
 		kfree(chip->badblock_pattern);
 }
+EXPORT_SYMBOL_GPL(nand_cleanup);
+
+/**
+ * nand_release - [NAND Interface] Unregister the MTD device and free resources
+ *		  held by the NAND device
+ * @mtd: MTD device structure
+ */
+void nand_release(struct mtd_info *mtd)
+{
+	mtd_device_unregister(mtd);
+	nand_cleanup(mtd_to_nand(mtd));
+}
 EXPORT_SYMBOL_GPL(nand_release);
 
 MODULE_LICENSE("GPL");
diff --git a/drivers/mtd/nand/nand_bbt.c b/drivers/mtd/nand/nand_bbt.c
index 2fbb523df066..7695efea65f2 100644
--- a/drivers/mtd/nand/nand_bbt.c
+++ b/drivers/mtd/nand/nand_bbt.c
@@ -605,6 +605,100 @@ static void search_read_bbts(struct mtd_info *mtd, uint8_t *buf,
 }
 
 /**
+ * get_bbt_block - Get the first valid eraseblock suitable to store a BBT
+ * @this: the NAND device
+ * @td: the BBT description
+ * @md: the mirror BBT descriptor
+ * @chip: the CHIP selector
+ *
+ * This functions returns a positive block number pointing a valid eraseblock
+ * suitable to store a BBT (i.e. in the range reserved for BBT), or -ENOSPC if
+ * all blocks are already used of marked bad. If td->pages[chip] was already
+ * pointing to a valid block we re-use it, otherwise we search for the next
+ * valid one.
+ */
+static int get_bbt_block(struct nand_chip *this, struct nand_bbt_descr *td,
+			 struct nand_bbt_descr *md, int chip)
+{
+	int startblock, dir, page, numblocks, i;
+
+	/*
+	 * There was already a version of the table, reuse the page. This
+	 * applies for absolute placement too, as we have the page number in
+	 * td->pages.
+	 */
+	if (td->pages[chip] != -1)
+		return td->pages[chip] >>
+				(this->bbt_erase_shift - this->page_shift);
+
+	numblocks = (int)(this->chipsize >> this->bbt_erase_shift);
+	if (!(td->options & NAND_BBT_PERCHIP))
+		numblocks *= this->numchips;
+
+	/*
+	 * Automatic placement of the bad block table. Search direction
+	 * top -> down?
+	 */
+	if (td->options & NAND_BBT_LASTBLOCK) {
+		startblock = numblocks * (chip + 1) - 1;
+		dir = -1;
+	} else {
+		startblock = chip * numblocks;
+		dir = 1;
+	}
+
+	for (i = 0; i < td->maxblocks; i++) {
+		int block = startblock + dir * i;
+
+		/* Check, if the block is bad */
+		switch (bbt_get_entry(this, block)) {
+		case BBT_BLOCK_WORN:
+		case BBT_BLOCK_FACTORY_BAD:
+			continue;
+		}
+
+		page = block << (this->bbt_erase_shift - this->page_shift);
+
+		/* Check, if the block is used by the mirror table */
+		if (!md || md->pages[chip] != page)
+			return block;
+	}
+
+	return -ENOSPC;
+}
+
+/**
+ * mark_bbt_block_bad - Mark one of the block reserved for BBT bad
+ * @this: the NAND device
+ * @td: the BBT description
+ * @chip: the CHIP selector
+ * @block: the BBT block to mark
+ *
+ * Blocks reserved for BBT can become bad. This functions is an helper to mark
+ * such blocks as bad. It takes care of updating the in-memory BBT, marking the
+ * block as bad using a bad block marker and invalidating the associated
+ * td->pages[] entry.
+ */
+static void mark_bbt_block_bad(struct nand_chip *this,
+			       struct nand_bbt_descr *td,
+			       int chip, int block)
+{
+	struct mtd_info *mtd = nand_to_mtd(this);
+	loff_t to;
+	int res;
+
+	bbt_mark_entry(this, block, BBT_BLOCK_WORN);
+
+	to = (loff_t)block << this->bbt_erase_shift;
+	res = this->block_markbad(mtd, to);
+	if (res)
+		pr_warn("nand_bbt: error %d while marking block %d bad\n",
+			res, block);
+
+	td->pages[chip] = -1;
+}
+
+/**
  * write_bbt - [GENERIC] (Re)write the bad block table
  * @mtd: MTD device structure
  * @buf: temporary buffer
@@ -621,7 +715,7 @@ static int write_bbt(struct mtd_info *mtd, uint8_t *buf,
 	struct nand_chip *this = mtd_to_nand(mtd);
 	struct erase_info einfo;
 	int i, res, chip = 0;
-	int bits, startblock, dir, page, offs, numblocks, sft, sftmsk;
+	int bits, page, offs, numblocks, sft, sftmsk;
 	int nrchips, pageoffs, ooboffs;
 	uint8_t msk[4];
 	uint8_t rcode = td->reserved_block_code;
@@ -652,46 +746,21 @@ static int write_bbt(struct mtd_info *mtd, uint8_t *buf,
 	}
 
 	/* Loop through the chips */
-	for (; chip < nrchips; chip++) {
-		/*
-		 * There was already a version of the table, reuse the page
-		 * This applies for absolute placement too, as we have the
-		 * page nr. in td->pages.
-		 */
-		if (td->pages[chip] != -1) {
-			page = td->pages[chip];
-			goto write;
+	while (chip < nrchips) {
+		int block;
+
+		block = get_bbt_block(this, td, md, chip);
+		if (block < 0) {
+			pr_err("No space left to write bad block table\n");
+			res = block;
+			goto outerr;
 		}
 
 		/*
-		 * Automatic placement of the bad block table. Search direction
-		 * top -> down?
+		 * get_bbt_block() returns a block number, shift the value to
+		 * get a page number.
 		 */
-		if (td->options & NAND_BBT_LASTBLOCK) {
-			startblock = numblocks * (chip + 1) - 1;
-			dir = -1;
-		} else {
-			startblock = chip * numblocks;
-			dir = 1;
-		}
-
-		for (i = 0; i < td->maxblocks; i++) {
-			int block = startblock + dir * i;
-			/* Check, if the block is bad */
-			switch (bbt_get_entry(this, block)) {
-			case BBT_BLOCK_WORN:
-			case BBT_BLOCK_FACTORY_BAD:
-				continue;
-			}
-			page = block <<
-				(this->bbt_erase_shift - this->page_shift);
-			/* Check, if the block is used by the mirror table */
-			if (!md || md->pages[chip] != page)
-				goto write;
-		}
-		pr_err("No space left to write bad block table\n");
-		return -ENOSPC;
-	write:
+		page = block << (this->bbt_erase_shift - this->page_shift);
 
 		/* Set up shift count and masks for the flash table */
 		bits = td->options & NAND_BBT_NRBITS_MSK;
@@ -787,20 +856,28 @@ static int write_bbt(struct mtd_info *mtd, uint8_t *buf,
 		einfo.addr = to;
 		einfo.len = 1 << this->bbt_erase_shift;
 		res = nand_erase_nand(mtd, &einfo, 1);
-		if (res < 0)
-			goto outerr;
+		if (res < 0) {
+			pr_warn("nand_bbt: error while erasing BBT block %d\n",
+				res);
+			mark_bbt_block_bad(this, td, chip, block);
+			continue;
+		}
 
 		res = scan_write_bbt(mtd, to, len, buf,
 				td->options & NAND_BBT_NO_OOB ? NULL :
 				&buf[len]);
-		if (res < 0)
-			goto outerr;
+		if (res < 0) {
+			pr_warn("nand_bbt: error while writing BBT block %d\n",
+				res);
+			mark_bbt_block_bad(this, td, chip, block);
+			continue;
+		}
 
 		pr_info("Bad block table written to 0x%012llx, version 0x%02X\n",
 			 (unsigned long long)to, td->version[chip]);
 
 		/* Mark it as used */
-		td->pages[chip] = page;
+		td->pages[chip++] = page;
 	}
 	return 0;
 
diff --git a/drivers/mtd/nand/nand_timings.c b/drivers/mtd/nand/nand_timings.c
index e81470a8ac67..13a587407be3 100644
--- a/drivers/mtd/nand/nand_timings.c
+++ b/drivers/mtd/nand/nand_timings.c
@@ -13,228 +13,246 @@
 #include <linux/export.h>
 #include <linux/mtd/nand.h>
 
-static const struct nand_sdr_timings onfi_sdr_timings[] = {
+static const struct nand_data_interface onfi_sdr_timings[] = {
 	/* Mode 0 */
 	{
-		.tADL_min = 200000,
-		.tALH_min = 20000,
-		.tALS_min = 50000,
-		.tAR_min = 25000,
-		.tCEA_max = 100000,
-		.tCEH_min = 20000,
-		.tCH_min = 20000,
-		.tCHZ_max = 100000,
-		.tCLH_min = 20000,
-		.tCLR_min = 20000,
-		.tCLS_min = 50000,
-		.tCOH_min = 0,
-		.tCS_min = 70000,
-		.tDH_min = 20000,
-		.tDS_min = 40000,
-		.tFEAT_max = 1000000,
-		.tIR_min = 10000,
-		.tITC_max = 1000000,
-		.tRC_min = 100000,
-		.tREA_max = 40000,
-		.tREH_min = 30000,
-		.tRHOH_min = 0,
-		.tRHW_min = 200000,
-		.tRHZ_max = 200000,
-		.tRLOH_min = 0,
-		.tRP_min = 50000,
-		.tRST_max = 250000000000ULL,
-		.tWB_max = 200000,
-		.tRR_min = 40000,
-		.tWC_min = 100000,
-		.tWH_min = 30000,
-		.tWHR_min = 120000,
-		.tWP_min = 50000,
-		.tWW_min = 100000,
+		.type = NAND_SDR_IFACE,
+		.timings.sdr = {
+			.tADL_min = 400000,
+			.tALH_min = 20000,
+			.tALS_min = 50000,
+			.tAR_min = 25000,
+			.tCEA_max = 100000,
+			.tCEH_min = 20000,
+			.tCH_min = 20000,
+			.tCHZ_max = 100000,
+			.tCLH_min = 20000,
+			.tCLR_min = 20000,
+			.tCLS_min = 50000,
+			.tCOH_min = 0,
+			.tCS_min = 70000,
+			.tDH_min = 20000,
+			.tDS_min = 40000,
+			.tFEAT_max = 1000000,
+			.tIR_min = 10000,
+			.tITC_max = 1000000,
+			.tRC_min = 100000,
+			.tREA_max = 40000,
+			.tREH_min = 30000,
+			.tRHOH_min = 0,
+			.tRHW_min = 200000,
+			.tRHZ_max = 200000,
+			.tRLOH_min = 0,
+			.tRP_min = 50000,
+			.tRR_min = 40000,
+			.tRST_max = 250000000000ULL,
+			.tWB_max = 200000,
+			.tWC_min = 100000,
+			.tWH_min = 30000,
+			.tWHR_min = 120000,
+			.tWP_min = 50000,
+			.tWW_min = 100000,
+		},
 	},
 	/* Mode 1 */
 	{
-		.tADL_min = 100000,
-		.tALH_min = 10000,
-		.tALS_min = 25000,
-		.tAR_min = 10000,
-		.tCEA_max = 45000,
-		.tCEH_min = 20000,
-		.tCH_min = 10000,
-		.tCHZ_max = 50000,
-		.tCLH_min = 10000,
-		.tCLR_min = 10000,
-		.tCLS_min = 25000,
-		.tCOH_min = 15000,
-		.tCS_min = 35000,
-		.tDH_min = 10000,
-		.tDS_min = 20000,
-		.tFEAT_max = 1000000,
-		.tIR_min = 0,
-		.tITC_max = 1000000,
-		.tRC_min = 50000,
-		.tREA_max = 30000,
-		.tREH_min = 15000,
-		.tRHOH_min = 15000,
-		.tRHW_min = 100000,
-		.tRHZ_max = 100000,
-		.tRLOH_min = 0,
-		.tRP_min = 25000,
-		.tRR_min = 20000,
-		.tRST_max = 500000000,
-		.tWB_max = 100000,
-		.tWC_min = 45000,
-		.tWH_min = 15000,
-		.tWHR_min = 80000,
-		.tWP_min = 25000,
-		.tWW_min = 100000,
+		.type = NAND_SDR_IFACE,
+		.timings.sdr = {
+			.tADL_min = 400000,
+			.tALH_min = 10000,
+			.tALS_min = 25000,
+			.tAR_min = 10000,
+			.tCEA_max = 45000,
+			.tCEH_min = 20000,
+			.tCH_min = 10000,
+			.tCHZ_max = 50000,
+			.tCLH_min = 10000,
+			.tCLR_min = 10000,
+			.tCLS_min = 25000,
+			.tCOH_min = 15000,
+			.tCS_min = 35000,
+			.tDH_min = 10000,
+			.tDS_min = 20000,
+			.tFEAT_max = 1000000,
+			.tIR_min = 0,
+			.tITC_max = 1000000,
+			.tRC_min = 50000,
+			.tREA_max = 30000,
+			.tREH_min = 15000,
+			.tRHOH_min = 15000,
+			.tRHW_min = 100000,
+			.tRHZ_max = 100000,
+			.tRLOH_min = 0,
+			.tRP_min = 25000,
+			.tRR_min = 20000,
+			.tRST_max = 500000000,
+			.tWB_max = 100000,
+			.tWC_min = 45000,
+			.tWH_min = 15000,
+			.tWHR_min = 80000,
+			.tWP_min = 25000,
+			.tWW_min = 100000,
+		},
 	},
 	/* Mode 2 */
 	{
-		.tADL_min = 100000,
-		.tALH_min = 10000,
-		.tALS_min = 15000,
-		.tAR_min = 10000,
-		.tCEA_max = 30000,
-		.tCEH_min = 20000,
-		.tCH_min = 10000,
-		.tCHZ_max = 50000,
-		.tCLH_min = 10000,
-		.tCLR_min = 10000,
-		.tCLS_min = 15000,
-		.tCOH_min = 15000,
-		.tCS_min = 25000,
-		.tDH_min = 5000,
-		.tDS_min = 15000,
-		.tFEAT_max = 1000000,
-		.tIR_min = 0,
-		.tITC_max = 1000000,
-		.tRC_min = 35000,
-		.tREA_max = 25000,
-		.tREH_min = 15000,
-		.tRHOH_min = 15000,
-		.tRHW_min = 100000,
-		.tRHZ_max = 100000,
-		.tRLOH_min = 0,
-		.tRR_min = 20000,
-		.tRST_max = 500000000,
-		.tWB_max = 100000,
-		.tRP_min = 17000,
-		.tWC_min = 35000,
-		.tWH_min = 15000,
-		.tWHR_min = 80000,
-		.tWP_min = 17000,
-		.tWW_min = 100000,
+		.type = NAND_SDR_IFACE,
+		.timings.sdr = {
+			.tADL_min = 400000,
+			.tALH_min = 10000,
+			.tALS_min = 15000,
+			.tAR_min = 10000,
+			.tCEA_max = 30000,
+			.tCEH_min = 20000,
+			.tCH_min = 10000,
+			.tCHZ_max = 50000,
+			.tCLH_min = 10000,
+			.tCLR_min = 10000,
+			.tCLS_min = 15000,
+			.tCOH_min = 15000,
+			.tCS_min = 25000,
+			.tDH_min = 5000,
+			.tDS_min = 15000,
+			.tFEAT_max = 1000000,
+			.tIR_min = 0,
+			.tITC_max = 1000000,
+			.tRC_min = 35000,
+			.tREA_max = 25000,
+			.tREH_min = 15000,
+			.tRHOH_min = 15000,
+			.tRHW_min = 100000,
+			.tRHZ_max = 100000,
+			.tRLOH_min = 0,
+			.tRR_min = 20000,
+			.tRST_max = 500000000,
+			.tWB_max = 100000,
+			.tRP_min = 17000,
+			.tWC_min = 35000,
+			.tWH_min = 15000,
+			.tWHR_min = 80000,
+			.tWP_min = 17000,
+			.tWW_min = 100000,
+		},
 	},
 	/* Mode 3 */
 	{
-		.tADL_min = 100000,
-		.tALH_min = 5000,
-		.tALS_min = 10000,
-		.tAR_min = 10000,
-		.tCEA_max = 25000,
-		.tCEH_min = 20000,
-		.tCH_min = 5000,
-		.tCHZ_max = 50000,
-		.tCLH_min = 5000,
-		.tCLR_min = 10000,
-		.tCLS_min = 10000,
-		.tCOH_min = 15000,
-		.tCS_min = 25000,
-		.tDH_min = 5000,
-		.tDS_min = 10000,
-		.tFEAT_max = 1000000,
-		.tIR_min = 0,
-		.tITC_max = 1000000,
-		.tRC_min = 30000,
-		.tREA_max = 20000,
-		.tREH_min = 10000,
-		.tRHOH_min = 15000,
-		.tRHW_min = 100000,
-		.tRHZ_max = 100000,
-		.tRLOH_min = 0,
-		.tRP_min = 15000,
-		.tRR_min = 20000,
-		.tRST_max = 500000000,
-		.tWB_max = 100000,
-		.tWC_min = 30000,
-		.tWH_min = 10000,
-		.tWHR_min = 80000,
-		.tWP_min = 15000,
-		.tWW_min = 100000,
+		.type = NAND_SDR_IFACE,
+		.timings.sdr = {
+			.tADL_min = 400000,
+			.tALH_min = 5000,
+			.tALS_min = 10000,
+			.tAR_min = 10000,
+			.tCEA_max = 25000,
+			.tCEH_min = 20000,
+			.tCH_min = 5000,
+			.tCHZ_max = 50000,
+			.tCLH_min = 5000,
+			.tCLR_min = 10000,
+			.tCLS_min = 10000,
+			.tCOH_min = 15000,
+			.tCS_min = 25000,
+			.tDH_min = 5000,
+			.tDS_min = 10000,
+			.tFEAT_max = 1000000,
+			.tIR_min = 0,
+			.tITC_max = 1000000,
+			.tRC_min = 30000,
+			.tREA_max = 20000,
+			.tREH_min = 10000,
+			.tRHOH_min = 15000,
+			.tRHW_min = 100000,
+			.tRHZ_max = 100000,
+			.tRLOH_min = 0,
+			.tRP_min = 15000,
+			.tRR_min = 20000,
+			.tRST_max = 500000000,
+			.tWB_max = 100000,
+			.tWC_min = 30000,
+			.tWH_min = 10000,
+			.tWHR_min = 80000,
+			.tWP_min = 15000,
+			.tWW_min = 100000,
+		},
 	},
 	/* Mode 4 */
 	{
-		.tADL_min = 70000,
-		.tALH_min = 5000,
-		.tALS_min = 10000,
-		.tAR_min = 10000,
-		.tCEA_max = 25000,
-		.tCEH_min = 20000,
-		.tCH_min = 5000,
-		.tCHZ_max = 30000,
-		.tCLH_min = 5000,
-		.tCLR_min = 10000,
-		.tCLS_min = 10000,
-		.tCOH_min = 15000,
-		.tCS_min = 20000,
-		.tDH_min = 5000,
-		.tDS_min = 10000,
-		.tFEAT_max = 1000000,
-		.tIR_min = 0,
-		.tITC_max = 1000000,
-		.tRC_min = 25000,
-		.tREA_max = 20000,
-		.tREH_min = 10000,
-		.tRHOH_min = 15000,
-		.tRHW_min = 100000,
-		.tRHZ_max = 100000,
-		.tRLOH_min = 5000,
-		.tRP_min = 12000,
-		.tRR_min = 20000,
-		.tRST_max = 500000000,
-		.tWB_max = 100000,
-		.tWC_min = 25000,
-		.tWH_min = 10000,
-		.tWHR_min = 80000,
-		.tWP_min = 12000,
-		.tWW_min = 100000,
+		.type = NAND_SDR_IFACE,
+		.timings.sdr = {
+			.tADL_min = 400000,
+			.tALH_min = 5000,
+			.tALS_min = 10000,
+			.tAR_min = 10000,
+			.tCEA_max = 25000,
+			.tCEH_min = 20000,
+			.tCH_min = 5000,
+			.tCHZ_max = 30000,
+			.tCLH_min = 5000,
+			.tCLR_min = 10000,
+			.tCLS_min = 10000,
+			.tCOH_min = 15000,
+			.tCS_min = 20000,
+			.tDH_min = 5000,
+			.tDS_min = 10000,
+			.tFEAT_max = 1000000,
+			.tIR_min = 0,
+			.tITC_max = 1000000,
+			.tRC_min = 25000,
+			.tREA_max = 20000,
+			.tREH_min = 10000,
+			.tRHOH_min = 15000,
+			.tRHW_min = 100000,
+			.tRHZ_max = 100000,
+			.tRLOH_min = 5000,
+			.tRP_min = 12000,
+			.tRR_min = 20000,
+			.tRST_max = 500000000,
+			.tWB_max = 100000,
+			.tWC_min = 25000,
+			.tWH_min = 10000,
+			.tWHR_min = 80000,
+			.tWP_min = 12000,
+			.tWW_min = 100000,
+		},
 	},
 	/* Mode 5 */
 	{
-		.tADL_min = 70000,
-		.tALH_min = 5000,
-		.tALS_min = 10000,
-		.tAR_min = 10000,
-		.tCEA_max = 25000,
-		.tCEH_min = 20000,
-		.tCH_min = 5000,
-		.tCHZ_max = 30000,
-		.tCLH_min = 5000,
-		.tCLR_min = 10000,
-		.tCLS_min = 10000,
-		.tCOH_min = 15000,
-		.tCS_min = 15000,
-		.tDH_min = 5000,
-		.tDS_min = 7000,
-		.tFEAT_max = 1000000,
-		.tIR_min = 0,
-		.tITC_max = 1000000,
-		.tRC_min = 20000,
-		.tREA_max = 16000,
-		.tREH_min = 7000,
-		.tRHOH_min = 15000,
-		.tRHW_min = 100000,
-		.tRHZ_max = 100000,
-		.tRLOH_min = 5000,
-		.tRP_min = 10000,
-		.tRR_min = 20000,
-		.tRST_max = 500000000,
-		.tWB_max = 100000,
-		.tWC_min = 20000,
-		.tWH_min = 7000,
-		.tWHR_min = 80000,
-		.tWP_min = 10000,
-		.tWW_min = 100000,
+		.type = NAND_SDR_IFACE,
+		.timings.sdr = {
+			.tADL_min = 400000,
+			.tALH_min = 5000,
+			.tALS_min = 10000,
+			.tAR_min = 10000,
+			.tCEA_max = 25000,
+			.tCEH_min = 20000,
+			.tCH_min = 5000,
+			.tCHZ_max = 30000,
+			.tCLH_min = 5000,
+			.tCLR_min = 10000,
+			.tCLS_min = 10000,
+			.tCOH_min = 15000,
+			.tCS_min = 15000,
+			.tDH_min = 5000,
+			.tDS_min = 7000,
+			.tFEAT_max = 1000000,
+			.tIR_min = 0,
+			.tITC_max = 1000000,
+			.tRC_min = 20000,
+			.tREA_max = 16000,
+			.tREH_min = 7000,
+			.tRHOH_min = 15000,
+			.tRHW_min = 100000,
+			.tRHZ_max = 100000,
+			.tRLOH_min = 5000,
+			.tRP_min = 10000,
+			.tRR_min = 20000,
+			.tRST_max = 500000000,
+			.tWB_max = 100000,
+			.tWC_min = 20000,
+			.tWH_min = 7000,
+			.tWHR_min = 80000,
+			.tWP_min = 10000,
+			.tWW_min = 100000,
+		},
 	},
 };
 
@@ -248,6 +266,46 @@ const struct nand_sdr_timings *onfi_async_timing_mode_to_sdr_timings(int mode)
 	if (mode < 0 || mode >= ARRAY_SIZE(onfi_sdr_timings))
 		return ERR_PTR(-EINVAL);
 
-	return &onfi_sdr_timings[mode];
+	return &onfi_sdr_timings[mode].timings.sdr;
 }
 EXPORT_SYMBOL(onfi_async_timing_mode_to_sdr_timings);
+
+/**
+ * onfi_init_data_interface - [NAND Interface] Initialize a data interface from
+ * given ONFI mode
+ * @iface: The data interface to be initialized
+ * @mode: The ONFI timing mode
+ */
+int onfi_init_data_interface(struct nand_chip *chip,
+			     struct nand_data_interface *iface,
+			     enum nand_data_interface_type type,
+			     int timing_mode)
+{
+	if (type != NAND_SDR_IFACE)
+		return -EINVAL;
+
+	if (timing_mode < 0 || timing_mode >= ARRAY_SIZE(onfi_sdr_timings))
+		return -EINVAL;
+
+	*iface = onfi_sdr_timings[timing_mode];
+
+	/*
+	 * TODO: initialize timings that cannot be deduced from timing mode:
+	 * tR, tPROG, tCCS, ...
+	 * These information are part of the ONFI parameter page.
+	 */
+
+	return 0;
+}
+EXPORT_SYMBOL(onfi_init_data_interface);
+
+/**
+ * nand_get_default_data_interface - [NAND Interface] Retrieve NAND
+ * data interface for mode 0. This is used as default timing after
+ * reset.
+ */
+const struct nand_data_interface *nand_get_default_data_interface(void)
+{
+	return &onfi_sdr_timings[0];
+}
+EXPORT_SYMBOL(nand_get_default_data_interface);
diff --git a/drivers/mtd/nand/ndfc.c b/drivers/mtd/nand/ndfc.c
index 218c789ca7ab..28e6118362f7 100644
--- a/drivers/mtd/nand/ndfc.c
+++ b/drivers/mtd/nand/ndfc.c
@@ -218,8 +218,7 @@ static int ndfc_probe(struct platform_device *ofdev)
 	ndfc = &ndfc_ctrl[cs];
 	ndfc->chip_select = cs;
 
-	spin_lock_init(&ndfc->ndfc_control.lock);
-	init_waitqueue_head(&ndfc->ndfc_control.wq);
+	nand_hw_control_init(&ndfc->ndfc_control);
 	ndfc->ofdev = ofdev;
 	dev_set_drvdata(&ofdev->dev, ndfc);
 
diff --git a/drivers/mtd/nand/omap2.c b/drivers/mtd/nand/omap2.c
index a59361c36f40..5513bfd9cdc9 100644
--- a/drivers/mtd/nand/omap2.c
+++ b/drivers/mtd/nand/omap2.c
@@ -2169,7 +2169,7 @@ scan_tail:
 	return 0;
 
 return_error:
-	if (info->dma)
+	if (!IS_ERR_OR_NULL(info->dma))
 		dma_release_channel(info->dma);
 	if (nand_chip->ecc.priv) {
 		nand_bch_free(nand_chip->ecc.priv);
diff --git a/drivers/mtd/nand/pxa3xx_nand.c b/drivers/mtd/nand/pxa3xx_nand.c
index 436dd6dc11f4..b121bf4ed73a 100644
--- a/drivers/mtd/nand/pxa3xx_nand.c
+++ b/drivers/mtd/nand/pxa3xx_nand.c
@@ -1810,8 +1810,7 @@ static int alloc_nand_resource(struct platform_device *pdev)
 		chip->cmdfunc		= nand_cmdfunc;
 	}
 
-	spin_lock_init(&chip->controller->lock);
-	init_waitqueue_head(&chip->controller->wq);
+	nand_hw_control_init(chip->controller);
 	info->clk = devm_clk_get(&pdev->dev, NULL);
 	if (IS_ERR(info->clk)) {
 		dev_err(&pdev->dev, "failed to get nand clock\n");
diff --git a/drivers/mtd/nand/qcom_nandc.c b/drivers/mtd/nand/qcom_nandc.c
index de7d28e62d4e..57d483ac5765 100644
--- a/drivers/mtd/nand/qcom_nandc.c
+++ b/drivers/mtd/nand/qcom_nandc.c
@@ -1957,8 +1957,7 @@ static int qcom_nandc_alloc(struct qcom_nand_controller *nandc)
 	INIT_LIST_HEAD(&nandc->desc_list);
 	INIT_LIST_HEAD(&nandc->host_list);
 
-	spin_lock_init(&nandc->controller.lock);
-	init_waitqueue_head(&nandc->controller.wq);
+	nand_hw_control_init(&nandc->controller);
 
 	return 0;
 }
diff --git a/drivers/mtd/nand/s3c2410.c b/drivers/mtd/nand/s3c2410.c
index d9309cf0ce2e..d459c19d78de 100644
--- a/drivers/mtd/nand/s3c2410.c
+++ b/drivers/mtd/nand/s3c2410.c
@@ -180,7 +180,7 @@ struct s3c2410_nand_info {
 
 	enum s3c_cpu_type		cpu_type;
 
-#ifdef CONFIG_CPU_FREQ
+#ifdef CONFIG_ARM_S3C24XX_CPUFREQ
 	struct notifier_block	freq_transition;
 #endif
 };
@@ -701,7 +701,7 @@ static void s3c2440_nand_write_buf(struct mtd_info *mtd, const u_char *buf,
 
 /* cpufreq driver support */
 
-#ifdef CONFIG_CPU_FREQ
+#ifdef CONFIG_ARM_S3C24XX_CPUFREQ
 
 static int s3c2410_nand_cpufreq_transition(struct notifier_block *nb,
 					  unsigned long val, void *data)
@@ -977,8 +977,7 @@ static int s3c24xx_nand_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, info);
 
-	spin_lock_init(&info->controller.lock);
-	init_waitqueue_head(&info->controller.wq);
+	nand_hw_control_init(&info->controller);
 
 	/* get the clock source and enable it */
 
diff --git a/drivers/mtd/nand/sh_flctl.c b/drivers/mtd/nand/sh_flctl.c
index 6fa3bcd59769..442ce619b3b6 100644
--- a/drivers/mtd/nand/sh_flctl.c
+++ b/drivers/mtd/nand/sh_flctl.c
@@ -397,7 +397,7 @@ static int flctl_dma_fifo0_transfer(struct sh_flctl *flctl, unsigned long *buf,
 	struct dma_chan *chan;
 	enum dma_transfer_direction tr_dir;
 	dma_addr_t dma_addr;
-	dma_cookie_t cookie = -EINVAL;
+	dma_cookie_t cookie;
 	uint32_t reg;
 	int ret;
 
@@ -423,6 +423,12 @@ static int flctl_dma_fifo0_transfer(struct sh_flctl *flctl, unsigned long *buf,
 		desc->callback = flctl_dma_complete;
 		desc->callback_param = flctl;
 		cookie = dmaengine_submit(desc);
+		if (dma_submit_error(cookie)) {
+			ret = dma_submit_error(cookie);
+			dev_warn(&flctl->pdev->dev,
+				 "DMA submit failed, falling back to PIO\n");
+			goto out;
+		}
 
 		dma_async_issue_pending(chan);
 	} else {
diff --git a/drivers/mtd/nand/sunxi_nand.c b/drivers/mtd/nand/sunxi_nand.c
index e414b31b71c1..8b8470c4e6d0 100644
--- a/drivers/mtd/nand/sunxi_nand.c
+++ b/drivers/mtd/nand/sunxi_nand.c
@@ -1572,14 +1572,22 @@ static int _sunxi_nand_lookup_timing(const s32 *lut, int lut_size, u32 duration,
 #define sunxi_nand_lookup_timing(l, p, c) \
 			_sunxi_nand_lookup_timing(l, ARRAY_SIZE(l), p, c)
 
-static int sunxi_nand_chip_set_timings(struct sunxi_nand_chip *chip,
-				       const struct nand_sdr_timings *timings)
+static int sunxi_nfc_setup_data_interface(struct mtd_info *mtd,
+					const struct nand_data_interface *conf,
+					bool check_only)
 {
+	struct nand_chip *nand = mtd_to_nand(mtd);
+	struct sunxi_nand_chip *chip = to_sunxi_nand(nand);
 	struct sunxi_nfc *nfc = to_sunxi_nfc(chip->nand.controller);
+	const struct nand_sdr_timings *timings;
 	u32 min_clk_period = 0;
 	s32 tWB, tADL, tWHR, tRHW, tCAD;
 	long real_clk_rate;
 
+	timings = nand_get_sdr_timings(conf);
+	if (IS_ERR(timings))
+		return -ENOTSUPP;
+
 	/* T1 <=> tCLS */
 	if (timings->tCLS_min > min_clk_period)
 		min_clk_period = timings->tCLS_min;
@@ -1679,6 +1687,9 @@ static int sunxi_nand_chip_set_timings(struct sunxi_nand_chip *chip,
 		return tRHW;
 	}
 
+	if (check_only)
+		return 0;
+
 	/*
 	 * TODO: according to ONFI specs this value only applies for DDR NAND,
 	 * but Allwinner seems to set this to 0x7. Mimic them for now.
@@ -1712,44 +1723,6 @@ static int sunxi_nand_chip_set_timings(struct sunxi_nand_chip *chip,
 	return 0;
 }
 
-static int sunxi_nand_chip_init_timings(struct sunxi_nand_chip *chip,
-					struct device_node *np)
-{
-	struct mtd_info *mtd = nand_to_mtd(&chip->nand);
-	const struct nand_sdr_timings *timings;
-	int ret;
-	int mode;
-
-	mode = onfi_get_async_timing_mode(&chip->nand);
-	if (mode == ONFI_TIMING_MODE_UNKNOWN) {
-		mode = chip->nand.onfi_timing_mode_default;
-	} else {
-		uint8_t feature[ONFI_SUBFEATURE_PARAM_LEN] = {};
-		int i;
-
-		mode = fls(mode) - 1;
-		if (mode < 0)
-			mode = 0;
-
-		feature[0] = mode;
-		for (i = 0; i < chip->nsels; i++) {
-			chip->nand.select_chip(mtd, i);
-			ret = chip->nand.onfi_set_features(mtd,	&chip->nand,
-						ONFI_FEATURE_ADDR_TIMING_MODE,
-						feature);
-			chip->nand.select_chip(mtd, -1);
-			if (ret)
-				return ret;
-		}
-	}
-
-	timings = onfi_async_timing_mode_to_sdr_timings(mode);
-	if (IS_ERR(timings))
-		return PTR_ERR(timings);
-
-	return sunxi_nand_chip_set_timings(chip, timings);
-}
-
 static int sunxi_nand_ooblayout_ecc(struct mtd_info *mtd, int section,
 				    struct mtd_oob_region *oobregion)
 {
@@ -1814,6 +1787,35 @@ static int sunxi_nand_hw_common_ecc_ctrl_init(struct mtd_info *mtd,
 	int ret;
 	int i;
 
+	if (ecc->options & NAND_ECC_MAXIMIZE) {
+		int bytes;
+
+		ecc->size = 1024;
+		nsectors = mtd->writesize / ecc->size;
+
+		/* Reserve 2 bytes for the BBM */
+		bytes = (mtd->oobsize - 2) / nsectors;
+
+		/* 4 non-ECC bytes are added before each ECC bytes section */
+		bytes -= 4;
+
+		/* and bytes has to be even. */
+		if (bytes % 2)
+			bytes--;
+
+		ecc->strength = bytes * 8 / fls(8 * ecc->size);
+
+		for (i = 0; i < ARRAY_SIZE(strengths); i++) {
+			if (strengths[i] > ecc->strength)
+				break;
+		}
+
+		if (!i)
+			ecc->strength = 0;
+		else
+			ecc->strength = strengths[i - 1];
+	}
+
 	if (ecc->size != 512 && ecc->size != 1024)
 		return -EINVAL;
 
@@ -1975,7 +1977,6 @@ static int sunxi_nand_ecc_init(struct mtd_info *mtd, struct nand_ecc_ctrl *ecc,
 static int sunxi_nand_chip_init(struct device *dev, struct sunxi_nfc *nfc,
 				struct device_node *np)
 {
-	const struct nand_sdr_timings *timings;
 	struct sunxi_nand_chip *chip;
 	struct mtd_info *mtd;
 	struct nand_chip *nand;
@@ -2065,25 +2066,11 @@ static int sunxi_nand_chip_init(struct device *dev, struct sunxi_nfc *nfc,
 	nand->read_buf = sunxi_nfc_read_buf;
 	nand->write_buf = sunxi_nfc_write_buf;
 	nand->read_byte = sunxi_nfc_read_byte;
+	nand->setup_data_interface = sunxi_nfc_setup_data_interface;
 
 	mtd = nand_to_mtd(nand);
 	mtd->dev.parent = dev;
 
-	timings = onfi_async_timing_mode_to_sdr_timings(0);
-	if (IS_ERR(timings)) {
-		ret = PTR_ERR(timings);
-		dev_err(dev,
-			"could not retrieve timings for ONFI mode 0: %d\n",
-			ret);
-		return ret;
-	}
-
-	ret = sunxi_nand_chip_set_timings(chip, timings);
-	if (ret) {
-		dev_err(dev, "could not configure chip timings: %d\n", ret);
-		return ret;
-	}
-
 	ret = nand_scan_ident(mtd, nsels, NULL);
 	if (ret)
 		return ret;
@@ -2096,12 +2083,6 @@ static int sunxi_nand_chip_init(struct device *dev, struct sunxi_nfc *nfc,
 
 	nand->options |= NAND_SUBPAGE_READ;
 
-	ret = sunxi_nand_chip_init_timings(chip, np);
-	if (ret) {
-		dev_err(dev, "could not configure chip timings: %d\n", ret);
-		return ret;
-	}
-
 	ret = sunxi_nand_ecc_init(mtd, &nand->ecc, np);
 	if (ret) {
 		dev_err(dev, "ECC init failed: %d\n", ret);
@@ -2175,8 +2156,7 @@ static int sunxi_nfc_probe(struct platform_device *pdev)
 		return -ENOMEM;
 
 	nfc->dev = dev;
-	spin_lock_init(&nfc->controller.lock);
-	init_waitqueue_head(&nfc->controller.wq);
+	nand_hw_control_init(&nfc->controller);
 	INIT_LIST_HEAD(&nfc->chips);
 
 	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
diff --git a/drivers/mtd/nand/txx9ndfmc.c b/drivers/mtd/nand/txx9ndfmc.c
index 04d63f56baa4..0a14fda2e41b 100644
--- a/drivers/mtd/nand/txx9ndfmc.c
+++ b/drivers/mtd/nand/txx9ndfmc.c
@@ -303,8 +303,7 @@ static int __init txx9ndfmc_probe(struct platform_device *dev)
 	dev_info(&dev->dev, "CLK:%ldMHz HOLD:%d SPW:%d\n",
 		 (gbusclk + 500000) / 1000000, hold, spw);
 
-	spin_lock_init(&drvdata->hw_control.lock);
-	init_waitqueue_head(&drvdata->hw_control.wq);
+	nand_hw_control_init(&drvdata->hw_control);
 
 	platform_set_drvdata(dev, drvdata);
 	txx9ndfmc_initialize(dev);
diff --git a/drivers/mtd/ubi/attach.c b/drivers/mtd/ubi/attach.c
index 903becd31410..93ceea4f27d5 100644
--- a/drivers/mtd/ubi/attach.c
+++ b/drivers/mtd/ubi/attach.c
@@ -91,9 +91,132 @@
 
 static int self_check_ai(struct ubi_device *ubi, struct ubi_attach_info *ai);
 
-/* Temporary variables used during scanning */
-static struct ubi_ec_hdr *ech;
-static struct ubi_vid_hdr *vidh;
+#define AV_FIND		BIT(0)
+#define AV_ADD		BIT(1)
+#define AV_FIND_OR_ADD	(AV_FIND | AV_ADD)
+
+/**
+ * find_or_add_av - internal function to find a volume, add a volume or do
+ *		    both (find and add if missing).
+ * @ai: attaching information
+ * @vol_id: the requested volume ID
+ * @flags: a combination of the %AV_FIND and %AV_ADD flags describing the
+ *	   expected operation. If only %AV_ADD is set, -EEXIST is returned
+ *	   if the volume already exists. If only %AV_FIND is set, NULL is
+ *	   returned if the volume does not exist. And if both flags are
+ *	   set, the helper first tries to find an existing volume, and if
+ *	   it does not exist it creates a new one.
+ * @created: in value used to inform the caller whether it"s a newly created
+ *	     volume or not.
+ *
+ * This function returns a pointer to a volume description or an ERR_PTR if
+ * the operation failed. It can also return NULL if only %AV_FIND is set and
+ * the volume does not exist.
+ */
+static struct ubi_ainf_volume *find_or_add_av(struct ubi_attach_info *ai,
+					      int vol_id, unsigned int flags,
+					      bool *created)
+{
+	struct ubi_ainf_volume *av;
+	struct rb_node **p = &ai->volumes.rb_node, *parent = NULL;
+
+	/* Walk the volume RB-tree to look if this volume is already present */
+	while (*p) {
+		parent = *p;
+		av = rb_entry(parent, struct ubi_ainf_volume, rb);
+
+		if (vol_id == av->vol_id) {
+			*created = false;
+
+			if (!(flags & AV_FIND))
+				return ERR_PTR(-EEXIST);
+
+			return av;
+		}
+
+		if (vol_id > av->vol_id)
+			p = &(*p)->rb_left;
+		else
+			p = &(*p)->rb_right;
+	}
+
+	if (!(flags & AV_ADD))
+		return NULL;
+
+	/* The volume is absent - add it */
+	av = kzalloc(sizeof(*av), GFP_KERNEL);
+	if (!av)
+		return ERR_PTR(-ENOMEM);
+
+	av->vol_id = vol_id;
+
+	if (vol_id > ai->highest_vol_id)
+		ai->highest_vol_id = vol_id;
+
+	rb_link_node(&av->rb, parent, p);
+	rb_insert_color(&av->rb, &ai->volumes);
+	ai->vols_found += 1;
+	*created = true;
+	dbg_bld("added volume %d", vol_id);
+	return av;
+}
+
+/**
+ * ubi_find_or_add_av - search for a volume in the attaching information and
+ *			add one if it does not exist.
+ * @ai: attaching information
+ * @vol_id: the requested volume ID
+ * @created: whether the volume has been created or not
+ *
+ * This function returns a pointer to the new volume description or an
+ * ERR_PTR if the operation failed.
+ */
+static struct ubi_ainf_volume *ubi_find_or_add_av(struct ubi_attach_info *ai,
+						  int vol_id, bool *created)
+{
+	return find_or_add_av(ai, vol_id, AV_FIND_OR_ADD, created);
+}
+
+/**
+ * ubi_alloc_aeb - allocate an aeb element
+ * @ai: attaching information
+ * @pnum: physical eraseblock number
+ * @ec: erase counter of the physical eraseblock
+ *
+ * Allocate an aeb object and initialize the pnum and ec information.
+ * vol_id and lnum are set to UBI_UNKNOWN, and the other fields are
+ * initialized to zero.
+ * Note that the element is not added in any list or RB tree.
+ */
+struct ubi_ainf_peb *ubi_alloc_aeb(struct ubi_attach_info *ai, int pnum,
+				   int ec)
+{
+	struct ubi_ainf_peb *aeb;
+
+	aeb = kmem_cache_zalloc(ai->aeb_slab_cache, GFP_KERNEL);
+	if (!aeb)
+		return NULL;
+
+	aeb->pnum = pnum;
+	aeb->ec = ec;
+	aeb->vol_id = UBI_UNKNOWN;
+	aeb->lnum = UBI_UNKNOWN;
+
+	return aeb;
+}
+
+/**
+ * ubi_free_aeb - free an aeb element
+ * @ai: attaching information
+ * @aeb: the element to free
+ *
+ * Free an aeb object. The caller must have removed the element from any list
+ * or RB tree.
+ */
+void ubi_free_aeb(struct ubi_attach_info *ai, struct ubi_ainf_peb *aeb)
+{
+	kmem_cache_free(ai->aeb_slab_cache, aeb);
+}
 
 /**
  * add_to_list - add physical eraseblock to a list.
@@ -131,14 +254,12 @@ static int add_to_list(struct ubi_attach_info *ai, int pnum, int vol_id,
 	} else
 		BUG();
 
-	aeb = kmem_cache_alloc(ai->aeb_slab_cache, GFP_KERNEL);
+	aeb = ubi_alloc_aeb(ai, pnum, ec);
 	if (!aeb)
 		return -ENOMEM;
 
-	aeb->pnum = pnum;
 	aeb->vol_id = vol_id;
 	aeb->lnum = lnum;
-	aeb->ec = ec;
 	if (to_head)
 		list_add(&aeb->u.list, list);
 	else
@@ -163,13 +284,11 @@ static int add_corrupted(struct ubi_attach_info *ai, int pnum, int ec)
 
 	dbg_bld("add to corrupted: PEB %d, EC %d", pnum, ec);
 
-	aeb = kmem_cache_alloc(ai->aeb_slab_cache, GFP_KERNEL);
+	aeb = ubi_alloc_aeb(ai, pnum, ec);
 	if (!aeb)
 		return -ENOMEM;
 
 	ai->corr_peb_count += 1;
-	aeb->pnum = pnum;
-	aeb->ec = ec;
 	list_add(&aeb->u.list, &ai->corr);
 	return 0;
 }
@@ -192,14 +311,12 @@ static int add_fastmap(struct ubi_attach_info *ai, int pnum,
 {
 	struct ubi_ainf_peb *aeb;
 
-	aeb = kmem_cache_alloc(ai->aeb_slab_cache, GFP_KERNEL);
+	aeb = ubi_alloc_aeb(ai, pnum, ec);
 	if (!aeb)
 		return -ENOMEM;
 
-	aeb->pnum = pnum;
-	aeb->vol_id = be32_to_cpu(vidh->vol_id);
-	aeb->sqnum = be64_to_cpu(vidh->sqnum);
-	aeb->ec = ec;
+	aeb->vol_id = be32_to_cpu(vid_hdr->vol_id);
+	aeb->sqnum = be64_to_cpu(vid_hdr->sqnum);
 	list_add(&aeb->u.list, &ai->fastmap);
 
 	dbg_bld("add to fastmap list: PEB %d, vol_id %d, sqnum: %llu", pnum,
@@ -294,44 +411,20 @@ static struct ubi_ainf_volume *add_volume(struct ubi_attach_info *ai,
 					  const struct ubi_vid_hdr *vid_hdr)
 {
 	struct ubi_ainf_volume *av;
-	struct rb_node **p = &ai->volumes.rb_node, *parent = NULL;
+	bool created;
 
 	ubi_assert(vol_id == be32_to_cpu(vid_hdr->vol_id));
 
-	/* Walk the volume RB-tree to look if this volume is already present */
-	while (*p) {
-		parent = *p;
-		av = rb_entry(parent, struct ubi_ainf_volume, rb);
-
-		if (vol_id == av->vol_id)
-			return av;
-
-		if (vol_id > av->vol_id)
-			p = &(*p)->rb_left;
-		else
-			p = &(*p)->rb_right;
-	}
-
-	/* The volume is absent - add it */
-	av = kmalloc(sizeof(struct ubi_ainf_volume), GFP_KERNEL);
-	if (!av)
-		return ERR_PTR(-ENOMEM);
+	av = ubi_find_or_add_av(ai, vol_id, &created);
+	if (IS_ERR(av) || !created)
+		return av;
 
-	av->highest_lnum = av->leb_count = 0;
-	av->vol_id = vol_id;
-	av->root = RB_ROOT;
 	av->used_ebs = be32_to_cpu(vid_hdr->used_ebs);
 	av->data_pad = be32_to_cpu(vid_hdr->data_pad);
 	av->compat = vid_hdr->compat;
 	av->vol_type = vid_hdr->vol_type == UBI_VID_DYNAMIC ? UBI_DYNAMIC_VOLUME
 							    : UBI_STATIC_VOLUME;
-	if (vol_id > ai->highest_vol_id)
-		ai->highest_vol_id = vol_id;
 
-	rb_link_node(&av->rb, parent, p);
-	rb_insert_color(&av->rb, &ai->volumes);
-	ai->vols_found += 1;
-	dbg_bld("added volume %d", vol_id);
 	return av;
 }
 
@@ -360,7 +453,7 @@ int ubi_compare_lebs(struct ubi_device *ubi, const struct ubi_ainf_peb *aeb,
 {
 	int len, err, second_is_newer, bitflips = 0, corrupted = 0;
 	uint32_t data_crc, crc;
-	struct ubi_vid_hdr *vh = NULL;
+	struct ubi_vid_io_buf *vidb = NULL;
 	unsigned long long sqnum2 = be64_to_cpu(vid_hdr->sqnum);
 
 	if (sqnum2 == aeb->sqnum) {
@@ -403,12 +496,12 @@ int ubi_compare_lebs(struct ubi_device *ubi, const struct ubi_ainf_peb *aeb,
 			return bitflips << 1;
 		}
 
-		vh = ubi_zalloc_vid_hdr(ubi, GFP_KERNEL);
-		if (!vh)
+		vidb = ubi_alloc_vid_buf(ubi, GFP_KERNEL);
+		if (!vidb)
 			return -ENOMEM;
 
 		pnum = aeb->pnum;
-		err = ubi_io_read_vid_hdr(ubi, pnum, vh, 0);
+		err = ubi_io_read_vid_hdr(ubi, pnum, vidb, 0);
 		if (err) {
 			if (err == UBI_IO_BITFLIPS)
 				bitflips = 1;
@@ -422,7 +515,7 @@ int ubi_compare_lebs(struct ubi_device *ubi, const struct ubi_ainf_peb *aeb,
 			}
 		}
 
-		vid_hdr = vh;
+		vid_hdr = ubi_get_vid_hdr(vidb);
 	}
 
 	/* Read the data of the copy and check the CRC */
@@ -448,7 +541,7 @@ int ubi_compare_lebs(struct ubi_device *ubi, const struct ubi_ainf_peb *aeb,
 	}
 	mutex_unlock(&ubi->buf_mutex);
 
-	ubi_free_vid_hdr(ubi, vh);
+	ubi_free_vid_buf(vidb);
 
 	if (second_is_newer)
 		dbg_bld("second PEB %d is newer, copy_flag is set", pnum);
@@ -460,7 +553,7 @@ int ubi_compare_lebs(struct ubi_device *ubi, const struct ubi_ainf_peb *aeb,
 out_unlock:
 	mutex_unlock(&ubi->buf_mutex);
 out_free_vidh:
-	ubi_free_vid_hdr(ubi, vh);
+	ubi_free_vid_buf(vidb);
 	return err;
 }
 
@@ -605,12 +698,10 @@ int ubi_add_to_av(struct ubi_device *ubi, struct ubi_attach_info *ai, int pnum,
 	if (err)
 		return err;
 
-	aeb = kmem_cache_alloc(ai->aeb_slab_cache, GFP_KERNEL);
+	aeb = ubi_alloc_aeb(ai, pnum, ec);
 	if (!aeb)
 		return -ENOMEM;
 
-	aeb->ec = ec;
-	aeb->pnum = pnum;
 	aeb->vol_id = vol_id;
 	aeb->lnum = lnum;
 	aeb->scrub = bitflips;
@@ -629,6 +720,21 @@ int ubi_add_to_av(struct ubi_device *ubi, struct ubi_attach_info *ai, int pnum,
 }
 
 /**
+ * ubi_add_av - add volume to the attaching information.
+ * @ai: attaching information
+ * @vol_id: the requested volume ID
+ *
+ * This function returns a pointer to the new volume description or an
+ * ERR_PTR if the operation failed.
+ */
+struct ubi_ainf_volume *ubi_add_av(struct ubi_attach_info *ai, int vol_id)
+{
+	bool created;
+
+	return find_or_add_av(ai, vol_id, AV_ADD, &created);
+}
+
+/**
  * ubi_find_av - find volume in the attaching information.
  * @ai: attaching information
  * @vol_id: the requested volume ID
@@ -639,24 +745,15 @@ int ubi_add_to_av(struct ubi_device *ubi, struct ubi_attach_info *ai, int pnum,
 struct ubi_ainf_volume *ubi_find_av(const struct ubi_attach_info *ai,
 				    int vol_id)
 {
-	struct ubi_ainf_volume *av;
-	struct rb_node *p = ai->volumes.rb_node;
-
-	while (p) {
-		av = rb_entry(p, struct ubi_ainf_volume, rb);
-
-		if (vol_id == av->vol_id)
-			return av;
-
-		if (vol_id > av->vol_id)
-			p = p->rb_left;
-		else
-			p = p->rb_right;
-	}
+	bool created;
 
-	return NULL;
+	return find_or_add_av((struct ubi_attach_info *)ai, vol_id, AV_FIND,
+			      &created);
 }
 
+static void destroy_av(struct ubi_attach_info *ai, struct ubi_ainf_volume *av,
+		       struct list_head *list);
+
 /**
  * ubi_remove_av - delete attaching information about a volume.
  * @ai: attaching information
@@ -664,19 +761,10 @@ struct ubi_ainf_volume *ubi_find_av(const struct ubi_attach_info *ai,
  */
 void ubi_remove_av(struct ubi_attach_info *ai, struct ubi_ainf_volume *av)
 {
-	struct rb_node *rb;
-	struct ubi_ainf_peb *aeb;
-
 	dbg_bld("remove attaching information about volume %d", av->vol_id);
 
-	while ((rb = rb_first(&av->root))) {
-		aeb = rb_entry(rb, struct ubi_ainf_peb, u.rb);
-		rb_erase(&aeb->u.rb, &av->root);
-		list_add_tail(&aeb->u.list, &ai->erase);
-	}
-
 	rb_erase(&av->rb, &ai->volumes);
-	kfree(av);
+	destroy_av(ai, av, &ai->erase);
 	ai->vols_found -= 1;
 }
 
@@ -866,6 +954,9 @@ static bool vol_ignored(int vol_id)
 static int scan_peb(struct ubi_device *ubi, struct ubi_attach_info *ai,
 		    int pnum, bool fast)
 {
+	struct ubi_ec_hdr *ech = ai->ech;
+	struct ubi_vid_io_buf *vidb = ai->vidb;
+	struct ubi_vid_hdr *vidh = ubi_get_vid_hdr(vidb);
 	long long ec;
 	int err, bitflips = 0, vol_id = -1, ec_err = 0;
 
@@ -963,7 +1054,7 @@ static int scan_peb(struct ubi_device *ubi, struct ubi_attach_info *ai,
 
 	/* OK, we've done with the EC header, let's look at the VID header */
 
-	err = ubi_io_read_vid_hdr(ubi, pnum, vidh, 0);
+	err = ubi_io_read_vid_hdr(ubi, pnum, vidb, 0);
 	if (err < 0)
 		return err;
 	switch (err) {
@@ -1191,10 +1282,12 @@ static int late_analysis(struct ubi_device *ubi, struct ubi_attach_info *ai)
  * destroy_av - free volume attaching information.
  * @av: volume attaching information
  * @ai: attaching information
+ * @list: put the aeb elements in there if !NULL, otherwise free them
  *
  * This function destroys the volume attaching information.
  */
-static void destroy_av(struct ubi_attach_info *ai, struct ubi_ainf_volume *av)
+static void destroy_av(struct ubi_attach_info *ai, struct ubi_ainf_volume *av,
+		       struct list_head *list)
 {
 	struct ubi_ainf_peb *aeb;
 	struct rb_node *this = av->root.rb_node;
@@ -1214,7 +1307,10 @@ static void destroy_av(struct ubi_attach_info *ai, struct ubi_ainf_volume *av)
 					this->rb_right = NULL;
 			}
 
-			kmem_cache_free(ai->aeb_slab_cache, aeb);
+			if (list)
+				list_add_tail(&aeb->u.list, list);
+			else
+				ubi_free_aeb(ai, aeb);
 		}
 	}
 	kfree(av);
@@ -1232,23 +1328,23 @@ static void destroy_ai(struct ubi_attach_info *ai)
 
 	list_for_each_entry_safe(aeb, aeb_tmp, &ai->alien, u.list) {
 		list_del(&aeb->u.list);
-		kmem_cache_free(ai->aeb_slab_cache, aeb);
+		ubi_free_aeb(ai, aeb);
 	}
 	list_for_each_entry_safe(aeb, aeb_tmp, &ai->erase, u.list) {
 		list_del(&aeb->u.list);
-		kmem_cache_free(ai->aeb_slab_cache, aeb);
+		ubi_free_aeb(ai, aeb);
 	}
 	list_for_each_entry_safe(aeb, aeb_tmp, &ai->corr, u.list) {
 		list_del(&aeb->u.list);
-		kmem_cache_free(ai->aeb_slab_cache, aeb);
+		ubi_free_aeb(ai, aeb);
 	}
 	list_for_each_entry_safe(aeb, aeb_tmp, &ai->free, u.list) {
 		list_del(&aeb->u.list);
-		kmem_cache_free(ai->aeb_slab_cache, aeb);
+		ubi_free_aeb(ai, aeb);
 	}
 	list_for_each_entry_safe(aeb, aeb_tmp, &ai->fastmap, u.list) {
 		list_del(&aeb->u.list);
-		kmem_cache_free(ai->aeb_slab_cache, aeb);
+		ubi_free_aeb(ai, aeb);
 	}
 
 	/* Destroy the volume RB-tree */
@@ -1269,7 +1365,7 @@ static void destroy_ai(struct ubi_attach_info *ai)
 					rb->rb_right = NULL;
 			}
 
-			destroy_av(ai, av);
+			destroy_av(ai, av, NULL);
 		}
 	}
 
@@ -1297,12 +1393,12 @@ static int scan_all(struct ubi_device *ubi, struct ubi_attach_info *ai,
 
 	err = -ENOMEM;
 
-	ech = kzalloc(ubi->ec_hdr_alsize, GFP_KERNEL);
-	if (!ech)
+	ai->ech = kzalloc(ubi->ec_hdr_alsize, GFP_KERNEL);
+	if (!ai->ech)
 		return err;
 
-	vidh = ubi_zalloc_vid_hdr(ubi, GFP_KERNEL);
-	if (!vidh)
+	ai->vidb = ubi_alloc_vid_buf(ubi, GFP_KERNEL);
+	if (!ai->vidb)
 		goto out_ech;
 
 	for (pnum = start; pnum < ubi->peb_count; pnum++) {
@@ -1351,15 +1447,15 @@ static int scan_all(struct ubi_device *ubi, struct ubi_attach_info *ai,
 	if (err)
 		goto out_vidh;
 
-	ubi_free_vid_hdr(ubi, vidh);
-	kfree(ech);
+	ubi_free_vid_buf(ai->vidb);
+	kfree(ai->ech);
 
 	return 0;
 
 out_vidh:
-	ubi_free_vid_hdr(ubi, vidh);
+	ubi_free_vid_buf(ai->vidb);
 out_ech:
-	kfree(ech);
+	kfree(ai->ech);
 	return err;
 }
 
@@ -1411,12 +1507,12 @@ static int scan_fast(struct ubi_device *ubi, struct ubi_attach_info **ai)
 	if (!scan_ai)
 		goto out;
 
-	ech = kzalloc(ubi->ec_hdr_alsize, GFP_KERNEL);
-	if (!ech)
+	scan_ai->ech = kzalloc(ubi->ec_hdr_alsize, GFP_KERNEL);
+	if (!scan_ai->ech)
 		goto out_ai;
 
-	vidh = ubi_zalloc_vid_hdr(ubi, GFP_KERNEL);
-	if (!vidh)
+	scan_ai->vidb = ubi_alloc_vid_buf(ubi, GFP_KERNEL);
+	if (!scan_ai->vidb)
 		goto out_ech;
 
 	for (pnum = 0; pnum < UBI_FM_MAX_START; pnum++) {
@@ -1428,8 +1524,8 @@ static int scan_fast(struct ubi_device *ubi, struct ubi_attach_info **ai)
 			goto out_vidh;
 	}
 
-	ubi_free_vid_hdr(ubi, vidh);
-	kfree(ech);
+	ubi_free_vid_buf(scan_ai->vidb);
+	kfree(scan_ai->ech);
 
 	if (scan_ai->force_full_scan)
 		err = UBI_NO_FASTMAP;
@@ -1449,9 +1545,9 @@ static int scan_fast(struct ubi_device *ubi, struct ubi_attach_info **ai)
 	return err;
 
 out_vidh:
-	ubi_free_vid_hdr(ubi, vidh);
+	ubi_free_vid_buf(scan_ai->vidb);
 out_ech:
-	kfree(ech);
+	kfree(scan_ai->ech);
 out_ai:
 	destroy_ai(scan_ai);
 out:
@@ -1573,6 +1669,8 @@ out_ai:
  */
 static int self_check_ai(struct ubi_device *ubi, struct ubi_attach_info *ai)
 {
+	struct ubi_vid_io_buf *vidb = ai->vidb;
+	struct ubi_vid_hdr *vidh = ubi_get_vid_hdr(vidb);
 	int pnum, err, vols_found = 0;
 	struct rb_node *rb1, *rb2;
 	struct ubi_ainf_volume *av;
@@ -1708,7 +1806,7 @@ static int self_check_ai(struct ubi_device *ubi, struct ubi_attach_info *ai)
 
 			last_aeb = aeb;
 
-			err = ubi_io_read_vid_hdr(ubi, aeb->pnum, vidh, 1);
+			err = ubi_io_read_vid_hdr(ubi, aeb->pnum, vidb, 1);
 			if (err && err != UBI_IO_BITFLIPS) {
 				ubi_err(ubi, "VID header is not OK (%d)",
 					err);
diff --git a/drivers/mtd/ubi/block.c b/drivers/mtd/ubi/block.c
index ebf46ad2d513..d1e6931c132f 100644
--- a/drivers/mtd/ubi/block.c
+++ b/drivers/mtd/ubi/block.c
@@ -351,7 +351,6 @@ static int ubiblock_init_request(void *data, struct request *req,
 static struct blk_mq_ops ubiblock_mq_ops = {
 	.queue_rq       = ubiblock_queue_rq,
 	.init_request	= ubiblock_init_request,
-	.map_queue      = blk_mq_map_queue,
 };
 
 static DEFINE_IDR(ubiblock_minor_idr);
diff --git a/drivers/mtd/ubi/build.c b/drivers/mtd/ubi/build.c
index 0680516bb472..85d54f37e28f 100644
--- a/drivers/mtd/ubi/build.c
+++ b/drivers/mtd/ubi/build.c
@@ -574,7 +574,7 @@ void ubi_free_internal_volumes(struct ubi_device *ubi)
 
 	for (i = ubi->vtbl_slots;
 	     i < ubi->vtbl_slots + UBI_INT_VOL_COUNT; i++) {
-		kfree(ubi->volumes[i]->eba_tbl);
+		ubi_eba_replace_table(ubi->volumes[i], NULL);
 		kfree(ubi->volumes[i]);
 	}
 }
diff --git a/drivers/mtd/ubi/cdev.c b/drivers/mtd/ubi/cdev.c
index ee2b74d1d1b5..45c329694a5e 100644
--- a/drivers/mtd/ubi/cdev.c
+++ b/drivers/mtd/ubi/cdev.c
@@ -416,7 +416,7 @@ static long vol_cdev_ioctl(struct file *file, unsigned int cmd,
 		}
 
 		rsvd_bytes = (long long)vol->reserved_pebs *
-					ubi->leb_size-vol->data_pad;
+					vol->usable_leb_size;
 		if (bytes < 0 || bytes > rsvd_bytes) {
 			err = -EINVAL;
 			break;
@@ -454,7 +454,7 @@ static long vol_cdev_ioctl(struct file *file, unsigned int cmd,
 
 		/* Validate the request */
 		err = -EINVAL;
-		if (req.lnum < 0 || req.lnum >= vol->reserved_pebs ||
+		if (!ubi_leb_valid(vol, req.lnum) ||
 		    req.bytes < 0 || req.bytes > vol->usable_leb_size)
 			break;
 
@@ -485,7 +485,7 @@ static long vol_cdev_ioctl(struct file *file, unsigned int cmd,
 			break;
 		}
 
-		if (lnum < 0 || lnum >= vol->reserved_pebs) {
+		if (!ubi_leb_valid(vol, lnum)) {
 			err = -EINVAL;
 			break;
 		}
diff --git a/drivers/mtd/ubi/eba.c b/drivers/mtd/ubi/eba.c
index ebf517271d29..388e46be6ad9 100644
--- a/drivers/mtd/ubi/eba.c
+++ b/drivers/mtd/ubi/eba.c
@@ -50,6 +50,30 @@
 #define EBA_RESERVED_PEBS 1
 
 /**
+ * struct ubi_eba_entry - structure encoding a single LEB -> PEB association
+ * @pnum: the physical eraseblock number attached to the LEB
+ *
+ * This structure is encoding a LEB -> PEB association. Note that the LEB
+ * number is not stored here, because it is the index used to access the
+ * entries table.
+ */
+struct ubi_eba_entry {
+	int pnum;
+};
+
+/**
+ * struct ubi_eba_table - LEB -> PEB association information
+ * @entries: the LEB to PEB mapping (one entry per LEB).
+ *
+ * This structure is private to the EBA logic and should be kept here.
+ * It is encoding the LEB to PEB association table, and is subject to
+ * changes.
+ */
+struct ubi_eba_table {
+	struct ubi_eba_entry *entries;
+};
+
+/**
  * next_sqnum - get next sequence number.
  * @ubi: UBI device description object
  *
@@ -84,6 +108,110 @@ static int ubi_get_compat(const struct ubi_device *ubi, int vol_id)
 }
 
 /**
+ * ubi_eba_get_ldesc - get information about a LEB
+ * @vol: volume description object
+ * @lnum: logical eraseblock number
+ * @ldesc: the LEB descriptor to fill
+ *
+ * Used to query information about a specific LEB.
+ * It is currently only returning the physical position of the LEB, but will be
+ * extended to provide more information.
+ */
+void ubi_eba_get_ldesc(struct ubi_volume *vol, int lnum,
+		       struct ubi_eba_leb_desc *ldesc)
+{
+	ldesc->lnum = lnum;
+	ldesc->pnum = vol->eba_tbl->entries[lnum].pnum;
+}
+
+/**
+ * ubi_eba_create_table - allocate a new EBA table and initialize it with all
+ *			  LEBs unmapped
+ * @vol: volume containing the EBA table to copy
+ * @nentries: number of entries in the table
+ *
+ * Allocate a new EBA table and initialize it with all LEBs unmapped.
+ * Returns a valid pointer if it succeed, an ERR_PTR() otherwise.
+ */
+struct ubi_eba_table *ubi_eba_create_table(struct ubi_volume *vol,
+					   int nentries)
+{
+	struct ubi_eba_table *tbl;
+	int err = -ENOMEM;
+	int i;
+
+	tbl = kzalloc(sizeof(*tbl), GFP_KERNEL);
+	if (!tbl)
+		return ERR_PTR(-ENOMEM);
+
+	tbl->entries = kmalloc_array(nentries, sizeof(*tbl->entries),
+				     GFP_KERNEL);
+	if (!tbl->entries)
+		goto err;
+
+	for (i = 0; i < nentries; i++)
+		tbl->entries[i].pnum = UBI_LEB_UNMAPPED;
+
+	return tbl;
+
+err:
+	kfree(tbl->entries);
+	kfree(tbl);
+
+	return ERR_PTR(err);
+}
+
+/**
+ * ubi_eba_destroy_table - destroy an EBA table
+ * @tbl: the table to destroy
+ *
+ * Destroy an EBA table.
+ */
+void ubi_eba_destroy_table(struct ubi_eba_table *tbl)
+{
+	if (!tbl)
+		return;
+
+	kfree(tbl->entries);
+	kfree(tbl);
+}
+
+/**
+ * ubi_eba_copy_table - copy the EBA table attached to vol into another table
+ * @vol: volume containing the EBA table to copy
+ * @dst: destination
+ * @nentries: number of entries to copy
+ *
+ * Copy the EBA table stored in vol into the one pointed by dst.
+ */
+void ubi_eba_copy_table(struct ubi_volume *vol, struct ubi_eba_table *dst,
+			int nentries)
+{
+	struct ubi_eba_table *src;
+	int i;
+
+	ubi_assert(dst && vol && vol->eba_tbl);
+
+	src = vol->eba_tbl;
+
+	for (i = 0; i < nentries; i++)
+		dst->entries[i].pnum = src->entries[i].pnum;
+}
+
+/**
+ * ubi_eba_replace_table - assign a new EBA table to a volume
+ * @vol: volume containing the EBA table to copy
+ * @tbl: new EBA table
+ *
+ * Assign a new EBA table to the volume and release the old one.
+ */
+void ubi_eba_replace_table(struct ubi_volume *vol, struct ubi_eba_table *tbl)
+{
+	ubi_eba_destroy_table(vol->eba_tbl);
+	vol->eba_tbl = tbl;
+}
+
+/**
  * ltree_lookup - look up the lock tree.
  * @ubi: UBI device description object
  * @vol_id: volume ID
@@ -312,6 +440,18 @@ static void leb_write_unlock(struct ubi_device *ubi, int vol_id, int lnum)
 }
 
 /**
+ * ubi_eba_is_mapped - check if a LEB is mapped.
+ * @vol: volume description object
+ * @lnum: logical eraseblock number
+ *
+ * This function returns true if the LEB is mapped, false otherwise.
+ */
+bool ubi_eba_is_mapped(struct ubi_volume *vol, int lnum)
+{
+	return vol->eba_tbl->entries[lnum].pnum >= 0;
+}
+
+/**
  * ubi_eba_unmap_leb - un-map logical eraseblock.
  * @ubi: UBI device description object
  * @vol: volume description object
@@ -333,7 +473,7 @@ int ubi_eba_unmap_leb(struct ubi_device *ubi, struct ubi_volume *vol,
 	if (err)
 		return err;
 
-	pnum = vol->eba_tbl[lnum];
+	pnum = vol->eba_tbl->entries[lnum].pnum;
 	if (pnum < 0)
 		/* This logical eraseblock is already unmapped */
 		goto out_unlock;
@@ -341,7 +481,7 @@ int ubi_eba_unmap_leb(struct ubi_device *ubi, struct ubi_volume *vol,
 	dbg_eba("erase LEB %d:%d, PEB %d", vol_id, lnum, pnum);
 
 	down_read(&ubi->fm_eba_sem);
-	vol->eba_tbl[lnum] = UBI_LEB_UNMAPPED;
+	vol->eba_tbl->entries[lnum].pnum = UBI_LEB_UNMAPPED;
 	up_read(&ubi->fm_eba_sem);
 	err = ubi_wl_put_peb(ubi, vol_id, lnum, pnum, 0);
 
@@ -373,6 +513,7 @@ int ubi_eba_read_leb(struct ubi_device *ubi, struct ubi_volume *vol, int lnum,
 		     void *buf, int offset, int len, int check)
 {
 	int err, pnum, scrub = 0, vol_id = vol->vol_id;
+	struct ubi_vid_io_buf *vidb;
 	struct ubi_vid_hdr *vid_hdr;
 	uint32_t uninitialized_var(crc);
 
@@ -380,7 +521,7 @@ int ubi_eba_read_leb(struct ubi_device *ubi, struct ubi_volume *vol, int lnum,
 	if (err)
 		return err;
 
-	pnum = vol->eba_tbl[lnum];
+	pnum = vol->eba_tbl->entries[lnum].pnum;
 	if (pnum < 0) {
 		/*
 		 * The logical eraseblock is not mapped, fill the whole buffer
@@ -403,13 +544,15 @@ int ubi_eba_read_leb(struct ubi_device *ubi, struct ubi_volume *vol, int lnum,
 
 retry:
 	if (check) {
-		vid_hdr = ubi_zalloc_vid_hdr(ubi, GFP_NOFS);
-		if (!vid_hdr) {
+		vidb = ubi_alloc_vid_buf(ubi, GFP_NOFS);
+		if (!vidb) {
 			err = -ENOMEM;
 			goto out_unlock;
 		}
 
-		err = ubi_io_read_vid_hdr(ubi, pnum, vid_hdr, 1);
+		vid_hdr = ubi_get_vid_hdr(vidb);
+
+		err = ubi_io_read_vid_hdr(ubi, pnum, vidb, 1);
 		if (err && err != UBI_IO_BITFLIPS) {
 			if (err > 0) {
 				/*
@@ -455,7 +598,7 @@ retry:
 		ubi_assert(len == be32_to_cpu(vid_hdr->data_size));
 
 		crc = be32_to_cpu(vid_hdr->data_crc);
-		ubi_free_vid_hdr(ubi, vid_hdr);
+		ubi_free_vid_buf(vidb);
 	}
 
 	err = ubi_io_read_data(ubi, buf, pnum, offset, len);
@@ -492,7 +635,7 @@ retry:
 	return err;
 
 out_free:
-	ubi_free_vid_hdr(ubi, vid_hdr);
+	ubi_free_vid_buf(vidb);
 out_unlock:
 	leb_read_unlock(ubi, vol_id, lnum);
 	return err;
@@ -554,52 +697,51 @@ int ubi_eba_read_leb_sg(struct ubi_device *ubi, struct ubi_volume *vol,
 }
 
 /**
- * recover_peb - recover from write failure.
- * @ubi: UBI device description object
+ * try_recover_peb - try to recover from write failure.
+ * @vol: volume description object
  * @pnum: the physical eraseblock to recover
- * @vol_id: volume ID
  * @lnum: logical eraseblock number
  * @buf: data which was not written because of the write failure
  * @offset: offset of the failed write
  * @len: how many bytes should have been written
+ * @vidb: VID buffer
+ * @retry: whether the caller should retry in case of failure
  *
  * This function is called in case of a write failure and moves all good data
  * from the potentially bad physical eraseblock to a good physical eraseblock.
  * This function also writes the data which was not written due to the failure.
- * Returns new physical eraseblock number in case of success, and a negative
- * error code in case of failure.
+ * Returns 0 in case of success, and a negative error code in case of failure.
+ * In case of failure, the %retry parameter is set to false if this is a fatal
+ * error (retrying won't help), and true otherwise.
  */
-static int recover_peb(struct ubi_device *ubi, int pnum, int vol_id, int lnum,
-		       const void *buf, int offset, int len)
+static int try_recover_peb(struct ubi_volume *vol, int pnum, int lnum,
+			   const void *buf, int offset, int len,
+			   struct ubi_vid_io_buf *vidb, bool *retry)
 {
-	int err, idx = vol_id2idx(ubi, vol_id), new_pnum, data_size, tries = 0;
-	struct ubi_volume *vol = ubi->volumes[idx];
+	struct ubi_device *ubi = vol->ubi;
 	struct ubi_vid_hdr *vid_hdr;
+	int new_pnum, err, vol_id = vol->vol_id, data_size;
 	uint32_t crc;
 
-	vid_hdr = ubi_zalloc_vid_hdr(ubi, GFP_NOFS);
-	if (!vid_hdr)
-		return -ENOMEM;
+	*retry = false;
 
-retry:
 	new_pnum = ubi_wl_get_peb(ubi);
 	if (new_pnum < 0) {
-		ubi_free_vid_hdr(ubi, vid_hdr);
-		up_read(&ubi->fm_eba_sem);
-		return new_pnum;
+		err = new_pnum;
+		goto out_put;
 	}
 
 	ubi_msg(ubi, "recover PEB %d, move data to PEB %d",
 		pnum, new_pnum);
 
-	err = ubi_io_read_vid_hdr(ubi, pnum, vid_hdr, 1);
+	err = ubi_io_read_vid_hdr(ubi, pnum, vidb, 1);
 	if (err && err != UBI_IO_BITFLIPS) {
 		if (err > 0)
 			err = -EIO;
-		up_read(&ubi->fm_eba_sem);
 		goto out_put;
 	}
 
+	vid_hdr = ubi_get_vid_hdr(vidb);
 	ubi_assert(vid_hdr->vol_type == UBI_VID_DYNAMIC);
 
 	mutex_lock(&ubi->buf_mutex);
@@ -608,12 +750,12 @@ retry:
 	/* Read everything before the area where the write failure happened */
 	if (offset > 0) {
 		err = ubi_io_read_data(ubi, ubi->peb_buf, pnum, 0, offset);
-		if (err && err != UBI_IO_BITFLIPS) {
-			up_read(&ubi->fm_eba_sem);
+		if (err && err != UBI_IO_BITFLIPS)
 			goto out_unlock;
-		}
 	}
 
+	*retry = true;
+
 	memcpy(ubi->peb_buf + offset, buf, len);
 
 	data_size = offset + len;
@@ -622,50 +764,140 @@ retry:
 	vid_hdr->copy_flag = 1;
 	vid_hdr->data_size = cpu_to_be32(data_size);
 	vid_hdr->data_crc = cpu_to_be32(crc);
-	err = ubi_io_write_vid_hdr(ubi, new_pnum, vid_hdr);
-	if (err) {
-		mutex_unlock(&ubi->buf_mutex);
-		up_read(&ubi->fm_eba_sem);
-		goto write_error;
-	}
+	err = ubi_io_write_vid_hdr(ubi, new_pnum, vidb);
+	if (err)
+		goto out_unlock;
 
 	err = ubi_io_write_data(ubi, ubi->peb_buf, new_pnum, 0, data_size);
-	if (err) {
-		mutex_unlock(&ubi->buf_mutex);
-		up_read(&ubi->fm_eba_sem);
-		goto write_error;
-	}
 
+out_unlock:
 	mutex_unlock(&ubi->buf_mutex);
-	ubi_free_vid_hdr(ubi, vid_hdr);
 
-	vol->eba_tbl[lnum] = new_pnum;
+	if (!err)
+		vol->eba_tbl->entries[lnum].pnum = new_pnum;
+
+out_put:
 	up_read(&ubi->fm_eba_sem);
-	ubi_wl_put_peb(ubi, vol_id, lnum, pnum, 1);
 
-	ubi_msg(ubi, "data was successfully recovered");
-	return 0;
+	if (!err) {
+		ubi_wl_put_peb(ubi, vol_id, lnum, pnum, 1);
+		ubi_msg(ubi, "data was successfully recovered");
+	} else if (new_pnum >= 0) {
+		/*
+		 * Bad luck? This physical eraseblock is bad too? Crud. Let's
+		 * try to get another one.
+		 */
+		ubi_wl_put_peb(ubi, vol_id, lnum, new_pnum, 1);
+		ubi_warn(ubi, "failed to write to PEB %d", new_pnum);
+	}
 
-out_unlock:
-	mutex_unlock(&ubi->buf_mutex);
-out_put:
-	ubi_wl_put_peb(ubi, vol_id, lnum, new_pnum, 1);
-	ubi_free_vid_hdr(ubi, vid_hdr);
 	return err;
+}
 
-write_error:
-	/*
-	 * Bad luck? This physical eraseblock is bad too? Crud. Let's try to
-	 * get another one.
-	 */
-	ubi_warn(ubi, "failed to write to PEB %d", new_pnum);
-	ubi_wl_put_peb(ubi, vol_id, lnum, new_pnum, 1);
-	if (++tries > UBI_IO_RETRIES) {
-		ubi_free_vid_hdr(ubi, vid_hdr);
-		return err;
+/**
+ * recover_peb - recover from write failure.
+ * @ubi: UBI device description object
+ * @pnum: the physical eraseblock to recover
+ * @vol_id: volume ID
+ * @lnum: logical eraseblock number
+ * @buf: data which was not written because of the write failure
+ * @offset: offset of the failed write
+ * @len: how many bytes should have been written
+ *
+ * This function is called in case of a write failure and moves all good data
+ * from the potentially bad physical eraseblock to a good physical eraseblock.
+ * This function also writes the data which was not written due to the failure.
+ * Returns 0 in case of success, and a negative error code in case of failure.
+ * This function tries %UBI_IO_RETRIES before giving up.
+ */
+static int recover_peb(struct ubi_device *ubi, int pnum, int vol_id, int lnum,
+		       const void *buf, int offset, int len)
+{
+	int err, idx = vol_id2idx(ubi, vol_id), tries;
+	struct ubi_volume *vol = ubi->volumes[idx];
+	struct ubi_vid_io_buf *vidb;
+
+	vidb = ubi_alloc_vid_buf(ubi, GFP_NOFS);
+	if (!vidb)
+		return -ENOMEM;
+
+	for (tries = 0; tries <= UBI_IO_RETRIES; tries++) {
+		bool retry;
+
+		err = try_recover_peb(vol, pnum, lnum, buf, offset, len, vidb,
+				      &retry);
+		if (!err || !retry)
+			break;
+
+		ubi_msg(ubi, "try again");
 	}
-	ubi_msg(ubi, "try again");
-	goto retry;
+
+	ubi_free_vid_buf(vidb);
+
+	return err;
+}
+
+/**
+ * try_write_vid_and_data - try to write VID header and data to a new PEB.
+ * @vol: volume description object
+ * @lnum: logical eraseblock number
+ * @vidb: the VID buffer to write
+ * @buf: buffer containing the data
+ * @offset: where to start writing data
+ * @len: how many bytes should be written
+ *
+ * This function tries to write VID header and data belonging to logical
+ * eraseblock @lnum of volume @vol to a new physical eraseblock. Returns zero
+ * in case of success and a negative error code in case of failure.
+ * In case of error, it is possible that something was still written to the
+ * flash media, but may be some garbage.
+ */
+static int try_write_vid_and_data(struct ubi_volume *vol, int lnum,
+				  struct ubi_vid_io_buf *vidb, const void *buf,
+				  int offset, int len)
+{
+	struct ubi_device *ubi = vol->ubi;
+	int pnum, opnum, err, vol_id = vol->vol_id;
+
+	pnum = ubi_wl_get_peb(ubi);
+	if (pnum < 0) {
+		err = pnum;
+		goto out_put;
+	}
+
+	opnum = vol->eba_tbl->entries[lnum].pnum;
+
+	dbg_eba("write VID hdr and %d bytes at offset %d of LEB %d:%d, PEB %d",
+		len, offset, vol_id, lnum, pnum);
+
+	err = ubi_io_write_vid_hdr(ubi, pnum, vidb);
+	if (err) {
+		ubi_warn(ubi, "failed to write VID header to LEB %d:%d, PEB %d",
+			 vol_id, lnum, pnum);
+		goto out_put;
+	}
+
+	if (len) {
+		err = ubi_io_write_data(ubi, buf, pnum, offset, len);
+		if (err) {
+			ubi_warn(ubi,
+				 "failed to write %d bytes at offset %d of LEB %d:%d, PEB %d",
+				 len, offset, vol_id, lnum, pnum);
+			goto out_put;
+		}
+	}
+
+	vol->eba_tbl->entries[lnum].pnum = pnum;
+
+out_put:
+	up_read(&ubi->fm_eba_sem);
+
+	if (err && pnum >= 0)
+		err = ubi_wl_put_peb(ubi, vol_id, lnum, pnum, 1);
+	else if (!err && opnum >= 0)
+		err = ubi_wl_put_peb(ubi, vol_id, lnum, opnum, 0);
+
+	return err;
 }
 
 /**
@@ -681,11 +913,13 @@ write_error:
  * @vol. Returns zero in case of success and a negative error code in case
  * of failure. In case of error, it is possible that something was still
  * written to the flash media, but may be some garbage.
+ * This function retries %UBI_IO_RETRIES times before giving up.
  */
 int ubi_eba_write_leb(struct ubi_device *ubi, struct ubi_volume *vol, int lnum,
 		      const void *buf, int offset, int len)
 {
-	int err, pnum, tries = 0, vol_id = vol->vol_id;
+	int err, pnum, tries, vol_id = vol->vol_id;
+	struct ubi_vid_io_buf *vidb;
 	struct ubi_vid_hdr *vid_hdr;
 
 	if (ubi->ro_mode)
@@ -695,7 +929,7 @@ int ubi_eba_write_leb(struct ubi_device *ubi, struct ubi_volume *vol, int lnum,
 	if (err)
 		return err;
 
-	pnum = vol->eba_tbl[lnum];
+	pnum = vol->eba_tbl->entries[lnum].pnum;
 	if (pnum >= 0) {
 		dbg_eba("write %d bytes at offset %d of LEB %d:%d, PEB %d",
 			len, offset, vol_id, lnum, pnum);
@@ -706,23 +940,23 @@ int ubi_eba_write_leb(struct ubi_device *ubi, struct ubi_volume *vol, int lnum,
 			if (err == -EIO && ubi->bad_allowed)
 				err = recover_peb(ubi, pnum, vol_id, lnum, buf,
 						  offset, len);
-			if (err)
-				ubi_ro_mode(ubi);
 		}
-		leb_write_unlock(ubi, vol_id, lnum);
-		return err;
+
+		goto out;
 	}
 
 	/*
 	 * The logical eraseblock is not mapped. We have to get a free physical
 	 * eraseblock and write the volume identifier header there first.
 	 */
-	vid_hdr = ubi_zalloc_vid_hdr(ubi, GFP_NOFS);
-	if (!vid_hdr) {
+	vidb = ubi_alloc_vid_buf(ubi, GFP_NOFS);
+	if (!vidb) {
 		leb_write_unlock(ubi, vol_id, lnum);
 		return -ENOMEM;
 	}
 
+	vid_hdr = ubi_get_vid_hdr(vidb);
+
 	vid_hdr->vol_type = UBI_VID_DYNAMIC;
 	vid_hdr->sqnum = cpu_to_be64(ubi_next_sqnum(ubi));
 	vid_hdr->vol_id = cpu_to_be32(vol_id);
@@ -730,67 +964,30 @@ int ubi_eba_write_leb(struct ubi_device *ubi, struct ubi_volume *vol, int lnum,
 	vid_hdr->compat = ubi_get_compat(ubi, vol_id);
 	vid_hdr->data_pad = cpu_to_be32(vol->data_pad);
 
-retry:
-	pnum = ubi_wl_get_peb(ubi);
-	if (pnum < 0) {
-		ubi_free_vid_hdr(ubi, vid_hdr);
-		leb_write_unlock(ubi, vol_id, lnum);
-		up_read(&ubi->fm_eba_sem);
-		return pnum;
-	}
-
-	dbg_eba("write VID hdr and %d bytes at offset %d of LEB %d:%d, PEB %d",
-		len, offset, vol_id, lnum, pnum);
-
-	err = ubi_io_write_vid_hdr(ubi, pnum, vid_hdr);
-	if (err) {
-		ubi_warn(ubi, "failed to write VID header to LEB %d:%d, PEB %d",
-			 vol_id, lnum, pnum);
-		up_read(&ubi->fm_eba_sem);
-		goto write_error;
-	}
+	for (tries = 0; tries <= UBI_IO_RETRIES; tries++) {
+		err = try_write_vid_and_data(vol, lnum, vidb, buf, offset, len);
+		if (err != -EIO || !ubi->bad_allowed)
+			break;
 
-	if (len) {
-		err = ubi_io_write_data(ubi, buf, pnum, offset, len);
-		if (err) {
-			ubi_warn(ubi, "failed to write %d bytes at offset %d of LEB %d:%d, PEB %d",
-				 len, offset, vol_id, lnum, pnum);
-			up_read(&ubi->fm_eba_sem);
-			goto write_error;
-		}
+		/*
+		 * Fortunately, this is the first write operation to this
+		 * physical eraseblock, so just put it and request a new one.
+		 * We assume that if this physical eraseblock went bad, the
+		 * erase code will handle that.
+		 */
+		vid_hdr->sqnum = cpu_to_be64(ubi_next_sqnum(ubi));
+		ubi_msg(ubi, "try another PEB");
 	}
 
-	vol->eba_tbl[lnum] = pnum;
-	up_read(&ubi->fm_eba_sem);
-
-	leb_write_unlock(ubi, vol_id, lnum);
-	ubi_free_vid_hdr(ubi, vid_hdr);
-	return 0;
+	ubi_free_vid_buf(vidb);
 
-write_error:
-	if (err != -EIO || !ubi->bad_allowed) {
+out:
+	if (err)
 		ubi_ro_mode(ubi);
-		leb_write_unlock(ubi, vol_id, lnum);
-		ubi_free_vid_hdr(ubi, vid_hdr);
-		return err;
-	}
 
-	/*
-	 * Fortunately, this is the first write operation to this physical
-	 * eraseblock, so just put it and request a new one. We assume that if
-	 * this physical eraseblock went bad, the erase code will handle that.
-	 */
-	err = ubi_wl_put_peb(ubi, vol_id, lnum, pnum, 1);
-	if (err || ++tries > UBI_IO_RETRIES) {
-		ubi_ro_mode(ubi);
-		leb_write_unlock(ubi, vol_id, lnum);
-		ubi_free_vid_hdr(ubi, vid_hdr);
-		return err;
-	}
+	leb_write_unlock(ubi, vol_id, lnum);
 
-	vid_hdr->sqnum = cpu_to_be64(ubi_next_sqnum(ubi));
-	ubi_msg(ubi, "try another PEB");
-	goto retry;
+	return err;
 }
 
 /**
@@ -818,7 +1015,8 @@ write_error:
 int ubi_eba_write_leb_st(struct ubi_device *ubi, struct ubi_volume *vol,
 			 int lnum, const void *buf, int len, int used_ebs)
 {
-	int err, pnum, tries = 0, data_size = len, vol_id = vol->vol_id;
+	int err, tries, data_size = len, vol_id = vol->vol_id;
+	struct ubi_vid_io_buf *vidb;
 	struct ubi_vid_hdr *vid_hdr;
 	uint32_t crc;
 
@@ -831,15 +1029,15 @@ int ubi_eba_write_leb_st(struct ubi_device *ubi, struct ubi_volume *vol,
 	else
 		ubi_assert(!(len & (ubi->min_io_size - 1)));
 
-	vid_hdr = ubi_zalloc_vid_hdr(ubi, GFP_NOFS);
-	if (!vid_hdr)
+	vidb = ubi_alloc_vid_buf(ubi, GFP_NOFS);
+	if (!vidb)
 		return -ENOMEM;
 
+	vid_hdr = ubi_get_vid_hdr(vidb);
+
 	err = leb_write_lock(ubi, vol_id, lnum);
-	if (err) {
-		ubi_free_vid_hdr(ubi, vid_hdr);
-		return err;
-	}
+	if (err)
+		goto out;
 
 	vid_hdr->sqnum = cpu_to_be64(ubi_next_sqnum(ubi));
 	vid_hdr->vol_id = cpu_to_be32(vol_id);
@@ -853,66 +1051,26 @@ int ubi_eba_write_leb_st(struct ubi_device *ubi, struct ubi_volume *vol,
 	vid_hdr->used_ebs = cpu_to_be32(used_ebs);
 	vid_hdr->data_crc = cpu_to_be32(crc);
 
-retry:
-	pnum = ubi_wl_get_peb(ubi);
-	if (pnum < 0) {
-		ubi_free_vid_hdr(ubi, vid_hdr);
-		leb_write_unlock(ubi, vol_id, lnum);
-		up_read(&ubi->fm_eba_sem);
-		return pnum;
-	}
+	ubi_assert(vol->eba_tbl->entries[lnum].pnum < 0);
 
-	dbg_eba("write VID hdr and %d bytes at LEB %d:%d, PEB %d, used_ebs %d",
-		len, vol_id, lnum, pnum, used_ebs);
+	for (tries = 0; tries <= UBI_IO_RETRIES; tries++) {
+		err = try_write_vid_and_data(vol, lnum, vidb, buf, 0, len);
+		if (err != -EIO || !ubi->bad_allowed)
+			break;
 
-	err = ubi_io_write_vid_hdr(ubi, pnum, vid_hdr);
-	if (err) {
-		ubi_warn(ubi, "failed to write VID header to LEB %d:%d, PEB %d",
-			 vol_id, lnum, pnum);
-		up_read(&ubi->fm_eba_sem);
-		goto write_error;
+		vid_hdr->sqnum = cpu_to_be64(ubi_next_sqnum(ubi));
+		ubi_msg(ubi, "try another PEB");
 	}
 
-	err = ubi_io_write_data(ubi, buf, pnum, 0, len);
-	if (err) {
-		ubi_warn(ubi, "failed to write %d bytes of data to PEB %d",
-			 len, pnum);
-		up_read(&ubi->fm_eba_sem);
-		goto write_error;
-	}
-
-	ubi_assert(vol->eba_tbl[lnum] < 0);
-	vol->eba_tbl[lnum] = pnum;
-	up_read(&ubi->fm_eba_sem);
+	if (err)
+		ubi_ro_mode(ubi);
 
 	leb_write_unlock(ubi, vol_id, lnum);
-	ubi_free_vid_hdr(ubi, vid_hdr);
-	return 0;
-
-write_error:
-	if (err != -EIO || !ubi->bad_allowed) {
-		/*
-		 * This flash device does not admit of bad eraseblocks or
-		 * something nasty and unexpected happened. Switch to read-only
-		 * mode just in case.
-		 */
-		ubi_ro_mode(ubi);
-		leb_write_unlock(ubi, vol_id, lnum);
-		ubi_free_vid_hdr(ubi, vid_hdr);
-		return err;
-	}
 
-	err = ubi_wl_put_peb(ubi, vol_id, lnum, pnum, 1);
-	if (err || ++tries > UBI_IO_RETRIES) {
-		ubi_ro_mode(ubi);
-		leb_write_unlock(ubi, vol_id, lnum);
-		ubi_free_vid_hdr(ubi, vid_hdr);
-		return err;
-	}
+out:
+	ubi_free_vid_buf(vidb);
 
-	vid_hdr->sqnum = cpu_to_be64(ubi_next_sqnum(ubi));
-	ubi_msg(ubi, "try another PEB");
-	goto retry;
+	return err;
 }
 
 /*
@@ -935,7 +1093,8 @@ write_error:
 int ubi_eba_atomic_leb_change(struct ubi_device *ubi, struct ubi_volume *vol,
 			      int lnum, const void *buf, int len)
 {
-	int err, pnum, old_pnum, tries = 0, vol_id = vol->vol_id;
+	int err, tries, vol_id = vol->vol_id;
+	struct ubi_vid_io_buf *vidb;
 	struct ubi_vid_hdr *vid_hdr;
 	uint32_t crc;
 
@@ -953,10 +1112,12 @@ int ubi_eba_atomic_leb_change(struct ubi_device *ubi, struct ubi_volume *vol,
 		return ubi_eba_write_leb(ubi, vol, lnum, NULL, 0, 0);
 	}
 
-	vid_hdr = ubi_zalloc_vid_hdr(ubi, GFP_NOFS);
-	if (!vid_hdr)
+	vidb = ubi_alloc_vid_buf(ubi, GFP_NOFS);
+	if (!vidb)
 		return -ENOMEM;
 
+	vid_hdr = ubi_get_vid_hdr(vidb);
+
 	mutex_lock(&ubi->alc_mutex);
 	err = leb_write_lock(ubi, vol_id, lnum);
 	if (err)
@@ -974,70 +1135,31 @@ int ubi_eba_atomic_leb_change(struct ubi_device *ubi, struct ubi_volume *vol,
 	vid_hdr->copy_flag = 1;
 	vid_hdr->data_crc = cpu_to_be32(crc);
 
-retry:
-	pnum = ubi_wl_get_peb(ubi);
-	if (pnum < 0) {
-		err = pnum;
-		up_read(&ubi->fm_eba_sem);
-		goto out_leb_unlock;
-	}
-
-	dbg_eba("change LEB %d:%d, PEB %d, write VID hdr to PEB %d",
-		vol_id, lnum, vol->eba_tbl[lnum], pnum);
+	dbg_eba("change LEB %d:%d", vol_id, lnum);
 
-	err = ubi_io_write_vid_hdr(ubi, pnum, vid_hdr);
-	if (err) {
-		ubi_warn(ubi, "failed to write VID header to LEB %d:%d, PEB %d",
-			 vol_id, lnum, pnum);
-		up_read(&ubi->fm_eba_sem);
-		goto write_error;
-	}
+	for (tries = 0; tries <= UBI_IO_RETRIES; tries++) {
+		err = try_write_vid_and_data(vol, lnum, vidb, buf, 0, len);
+		if (err != -EIO || !ubi->bad_allowed)
+			break;
 
-	err = ubi_io_write_data(ubi, buf, pnum, 0, len);
-	if (err) {
-		ubi_warn(ubi, "failed to write %d bytes of data to PEB %d",
-			 len, pnum);
-		up_read(&ubi->fm_eba_sem);
-		goto write_error;
+		vid_hdr->sqnum = cpu_to_be64(ubi_next_sqnum(ubi));
+		ubi_msg(ubi, "try another PEB");
 	}
 
-	old_pnum = vol->eba_tbl[lnum];
-	vol->eba_tbl[lnum] = pnum;
-	up_read(&ubi->fm_eba_sem);
-
-	if (old_pnum >= 0) {
-		err = ubi_wl_put_peb(ubi, vol_id, lnum, old_pnum, 0);
-		if (err)
-			goto out_leb_unlock;
-	}
+	/*
+	 * This flash device does not admit of bad eraseblocks or
+	 * something nasty and unexpected happened. Switch to read-only
+	 * mode just in case.
+	 */
+	if (err)
+		ubi_ro_mode(ubi);
 
-out_leb_unlock:
 	leb_write_unlock(ubi, vol_id, lnum);
+
 out_mutex:
 	mutex_unlock(&ubi->alc_mutex);
-	ubi_free_vid_hdr(ubi, vid_hdr);
+	ubi_free_vid_buf(vidb);
 	return err;
-
-write_error:
-	if (err != -EIO || !ubi->bad_allowed) {
-		/*
-		 * This flash device does not admit of bad eraseblocks or
-		 * something nasty and unexpected happened. Switch to read-only
-		 * mode just in case.
-		 */
-		ubi_ro_mode(ubi);
-		goto out_leb_unlock;
-	}
-
-	err = ubi_wl_put_peb(ubi, vol_id, lnum, pnum, 1);
-	if (err || ++tries > UBI_IO_RETRIES) {
-		ubi_ro_mode(ubi);
-		goto out_leb_unlock;
-	}
-
-	vid_hdr->sqnum = cpu_to_be64(ubi_next_sqnum(ubi));
-	ubi_msg(ubi, "try another PEB");
-	goto retry;
 }
 
 /**
@@ -1082,12 +1204,15 @@ static int is_error_sane(int err)
  *   o a negative error code in case of failure.
  */
 int ubi_eba_copy_leb(struct ubi_device *ubi, int from, int to,
-		     struct ubi_vid_hdr *vid_hdr)
+		     struct ubi_vid_io_buf *vidb)
 {
 	int err, vol_id, lnum, data_size, aldata_size, idx;
+	struct ubi_vid_hdr *vid_hdr = ubi_get_vid_hdr(vidb);
 	struct ubi_volume *vol;
 	uint32_t crc;
 
+	ubi_assert(rwsem_is_locked(&ubi->fm_eba_sem));
+
 	vol_id = be32_to_cpu(vid_hdr->vol_id);
 	lnum = be32_to_cpu(vid_hdr->lnum);
 
@@ -1142,9 +1267,9 @@ int ubi_eba_copy_leb(struct ubi_device *ubi, int from, int to,
 	 * probably waiting on @ubi->move_mutex. No need to continue the work,
 	 * cancel it.
 	 */
-	if (vol->eba_tbl[lnum] != from) {
+	if (vol->eba_tbl->entries[lnum].pnum != from) {
 		dbg_wl("LEB %d:%d is no longer mapped to PEB %d, mapped to PEB %d, cancel",
-		       vol_id, lnum, from, vol->eba_tbl[lnum]);
+		       vol_id, lnum, from, vol->eba_tbl->entries[lnum].pnum);
 		err = MOVE_CANCEL_RACE;
 		goto out_unlock_leb;
 	}
@@ -1196,7 +1321,7 @@ int ubi_eba_copy_leb(struct ubi_device *ubi, int from, int to,
 	}
 	vid_hdr->sqnum = cpu_to_be64(ubi_next_sqnum(ubi));
 
-	err = ubi_io_write_vid_hdr(ubi, to, vid_hdr);
+	err = ubi_io_write_vid_hdr(ubi, to, vidb);
 	if (err) {
 		if (err == -EIO)
 			err = MOVE_TARGET_WR_ERR;
@@ -1206,7 +1331,7 @@ int ubi_eba_copy_leb(struct ubi_device *ubi, int from, int to,
 	cond_resched();
 
 	/* Read the VID header back and check if it was written correctly */
-	err = ubi_io_read_vid_hdr(ubi, to, vid_hdr, 1);
+	err = ubi_io_read_vid_hdr(ubi, to, vidb, 1);
 	if (err) {
 		if (err != UBI_IO_BITFLIPS) {
 			ubi_warn(ubi, "error %d while reading VID header back from PEB %d",
@@ -1229,10 +1354,8 @@ int ubi_eba_copy_leb(struct ubi_device *ubi, int from, int to,
 		cond_resched();
 	}
 
-	ubi_assert(vol->eba_tbl[lnum] == from);
-	down_read(&ubi->fm_eba_sem);
-	vol->eba_tbl[lnum] = to;
-	up_read(&ubi->fm_eba_sem);
+	ubi_assert(vol->eba_tbl->entries[lnum].pnum == from);
+	vol->eba_tbl->entries[lnum].pnum = to;
 
 out_unlock_buf:
 	mutex_unlock(&ubi->buf_mutex);
@@ -1388,7 +1511,7 @@ out_free:
  */
 int ubi_eba_init(struct ubi_device *ubi, struct ubi_attach_info *ai)
 {
-	int i, j, err, num_volumes;
+	int i, err, num_volumes;
 	struct ubi_ainf_volume *av;
 	struct ubi_volume *vol;
 	struct ubi_ainf_peb *aeb;
@@ -1404,35 +1527,39 @@ int ubi_eba_init(struct ubi_device *ubi, struct ubi_attach_info *ai)
 	num_volumes = ubi->vtbl_slots + UBI_INT_VOL_COUNT;
 
 	for (i = 0; i < num_volumes; i++) {
+		struct ubi_eba_table *tbl;
+
 		vol = ubi->volumes[i];
 		if (!vol)
 			continue;
 
 		cond_resched();
 
-		vol->eba_tbl = kmalloc(vol->reserved_pebs * sizeof(int),
-				       GFP_KERNEL);
-		if (!vol->eba_tbl) {
-			err = -ENOMEM;
+		tbl = ubi_eba_create_table(vol, vol->reserved_pebs);
+		if (IS_ERR(tbl)) {
+			err = PTR_ERR(tbl);
 			goto out_free;
 		}
 
-		for (j = 0; j < vol->reserved_pebs; j++)
-			vol->eba_tbl[j] = UBI_LEB_UNMAPPED;
+		ubi_eba_replace_table(vol, tbl);
 
 		av = ubi_find_av(ai, idx2vol_id(ubi, i));
 		if (!av)
 			continue;
 
 		ubi_rb_for_each_entry(rb, aeb, &av->root, u.rb) {
-			if (aeb->lnum >= vol->reserved_pebs)
+			if (aeb->lnum >= vol->reserved_pebs) {
 				/*
 				 * This may happen in case of an unclean reboot
 				 * during re-size.
 				 */
 				ubi_move_aeb_to_list(av, aeb, &ai->erase);
-			else
-				vol->eba_tbl[aeb->lnum] = aeb->pnum;
+			} else {
+				struct ubi_eba_entry *entry;
+
+				entry = &vol->eba_tbl->entries[aeb->lnum];
+				entry->pnum = aeb->pnum;
+			}
 		}
 	}
 
@@ -1469,8 +1596,7 @@ out_free:
 	for (i = 0; i < num_volumes; i++) {
 		if (!ubi->volumes[i])
 			continue;
-		kfree(ubi->volumes[i]->eba_tbl);
-		ubi->volumes[i]->eba_tbl = NULL;
+		ubi_eba_replace_table(ubi->volumes[i], NULL);
 	}
 	return err;
 }
diff --git a/drivers/mtd/ubi/fastmap-wl.c b/drivers/mtd/ubi/fastmap-wl.c
index 30d3999dddba..4f0bd6b4422a 100644
--- a/drivers/mtd/ubi/fastmap-wl.c
+++ b/drivers/mtd/ubi/fastmap-wl.c
@@ -262,6 +262,8 @@ static struct ubi_wl_entry *get_peb_for_wl(struct ubi_device *ubi)
 	struct ubi_fm_pool *pool = &ubi->fm_wl_pool;
 	int pnum;
 
+	ubi_assert(rwsem_is_locked(&ubi->fm_eba_sem));
+
 	if (pool->used == pool->size) {
 		/* We cannot update the fastmap here because this
 		 * function is called in atomic context.
@@ -303,7 +305,7 @@ int ubi_ensure_anchor_pebs(struct ubi_device *ubi)
 
 	wrk->anchor = 1;
 	wrk->func = &wear_leveling_worker;
-	schedule_ubi_work(ubi, wrk);
+	__schedule_ubi_work(ubi, wrk);
 	return 0;
 }
 
@@ -344,7 +346,7 @@ int ubi_wl_put_fm_peb(struct ubi_device *ubi, struct ubi_wl_entry *fm_e,
 	spin_unlock(&ubi->wl_lock);
 
 	vol_id = lnum ? UBI_FM_DATA_VOLUME_ID : UBI_FM_SB_VOLUME_ID;
-	return schedule_erase(ubi, e, vol_id, lnum, torture);
+	return schedule_erase(ubi, e, vol_id, lnum, torture, true);
 }
 
 /**
diff --git a/drivers/mtd/ubi/fastmap.c b/drivers/mtd/ubi/fastmap.c
index 48eb55f344eb..2ff62157d3bb 100644
--- a/drivers/mtd/ubi/fastmap.c
+++ b/drivers/mtd/ubi/fastmap.c
@@ -110,21 +110,23 @@ size_t ubi_calc_fm_size(struct ubi_device *ubi)
  * Returns a new struct ubi_vid_hdr on success.
  * NULL indicates out of memory.
  */
-static struct ubi_vid_hdr *new_fm_vhdr(struct ubi_device *ubi, int vol_id)
+static struct ubi_vid_io_buf *new_fm_vbuf(struct ubi_device *ubi, int vol_id)
 {
-	struct ubi_vid_hdr *new;
+	struct ubi_vid_io_buf *new;
+	struct ubi_vid_hdr *vh;
 
-	new = ubi_zalloc_vid_hdr(ubi, GFP_KERNEL);
+	new = ubi_alloc_vid_buf(ubi, GFP_KERNEL);
 	if (!new)
 		goto out;
 
-	new->vol_type = UBI_VID_DYNAMIC;
-	new->vol_id = cpu_to_be32(vol_id);
+	vh = ubi_get_vid_hdr(new);
+	vh->vol_type = UBI_VID_DYNAMIC;
+	vh->vol_id = cpu_to_be32(vol_id);
 
 	/* UBI implementations without fastmap support have to delete the
 	 * fastmap.
 	 */
-	new->compat = UBI_COMPAT_DELETE;
+	vh->compat = UBI_COMPAT_DELETE;
 
 out:
 	return new;
@@ -145,12 +147,10 @@ static int add_aeb(struct ubi_attach_info *ai, struct list_head *list,
 {
 	struct ubi_ainf_peb *aeb;
 
-	aeb = kmem_cache_alloc(ai->aeb_slab_cache, GFP_KERNEL);
+	aeb = ubi_alloc_aeb(ai, pnum, ec);
 	if (!aeb)
 		return -ENOMEM;
 
-	aeb->pnum = pnum;
-	aeb->ec = ec;
 	aeb->lnum = -1;
 	aeb->scrub = scrub;
 	aeb->copy_flag = aeb->sqnum = 0;
@@ -186,40 +186,19 @@ static struct ubi_ainf_volume *add_vol(struct ubi_attach_info *ai, int vol_id,
 				       int last_eb_bytes)
 {
 	struct ubi_ainf_volume *av;
-	struct rb_node **p = &ai->volumes.rb_node, *parent = NULL;
-
-	while (*p) {
-		parent = *p;
-		av = rb_entry(parent, struct ubi_ainf_volume, rb);
-
-		if (vol_id > av->vol_id)
-			p = &(*p)->rb_left;
-		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);
-	if (!av)
-		goto out;
+	av = ubi_add_av(ai, vol_id);
+	if (IS_ERR(av))
+		return av;
 
-	av->highest_lnum = av->leb_count = av->used_ebs = 0;
-	av->vol_id = vol_id;
 	av->data_pad = data_pad;
 	av->last_data_size = last_eb_bytes;
 	av->compat = 0;
 	av->vol_type = vol_type;
-	av->root = RB_ROOT;
 	if (av->vol_type == UBI_STATIC_VOLUME)
 		av->used_ebs = used_ebs;
 
 	dbg_bld("found volume (ID %i)", vol_id);
-
-	rb_link_node(&av->rb, parent, p);
-	rb_insert_color(&av->rb, &ai->volumes);
-
-out:
 	return av;
 }
 
@@ -297,7 +276,7 @@ static int update_vol(struct ubi_device *ubi, struct ubi_attach_info *ai,
 		 */
 		if (aeb->pnum == new_aeb->pnum) {
 			ubi_assert(aeb->lnum == new_aeb->lnum);
-			kmem_cache_free(ai->aeb_slab_cache, new_aeb);
+			ubi_free_aeb(ai, new_aeb);
 
 			return 0;
 		}
@@ -308,13 +287,10 @@ static int update_vol(struct ubi_device *ubi, struct ubi_attach_info *ai,
 
 		/* new_aeb is newer */
 		if (cmp_res & 1) {
-			victim = kmem_cache_alloc(ai->aeb_slab_cache,
-				GFP_KERNEL);
+			victim = ubi_alloc_aeb(ai, aeb->pnum, aeb->ec);
 			if (!victim)
 				return -ENOMEM;
 
-			victim->ec = aeb->ec;
-			victim->pnum = aeb->pnum;
 			list_add_tail(&victim->u.list, &ai->erase);
 
 			if (av->highest_lnum == be32_to_cpu(new_vh->lnum))
@@ -328,7 +304,8 @@ static int update_vol(struct ubi_device *ubi, struct ubi_attach_info *ai,
 			aeb->pnum = new_aeb->pnum;
 			aeb->copy_flag = new_vh->copy_flag;
 			aeb->scrub = new_aeb->scrub;
-			kmem_cache_free(ai->aeb_slab_cache, new_aeb);
+			aeb->sqnum = new_aeb->sqnum;
+			ubi_free_aeb(ai, new_aeb);
 
 		/* new_aeb is older */
 		} else {
@@ -370,41 +347,24 @@ static int process_pool_aeb(struct ubi_device *ubi, struct ubi_attach_info *ai,
 			    struct ubi_vid_hdr *new_vh,
 			    struct ubi_ainf_peb *new_aeb)
 {
-	struct ubi_ainf_volume *av, *tmp_av = NULL;
-	struct rb_node **p = &ai->volumes.rb_node, *parent = NULL;
-	int found = 0;
+	int vol_id = be32_to_cpu(new_vh->vol_id);
+	struct ubi_ainf_volume *av;
 
-	if (be32_to_cpu(new_vh->vol_id) == UBI_FM_SB_VOLUME_ID ||
-		be32_to_cpu(new_vh->vol_id) == UBI_FM_DATA_VOLUME_ID) {
-		kmem_cache_free(ai->aeb_slab_cache, new_aeb);
+	if (vol_id == UBI_FM_SB_VOLUME_ID || vol_id == UBI_FM_DATA_VOLUME_ID) {
+		ubi_free_aeb(ai, new_aeb);
 
 		return 0;
 	}
 
 	/* Find the volume this SEB belongs to */
-	while (*p) {
-		parent = *p;
-		tmp_av = rb_entry(parent, struct ubi_ainf_volume, rb);
-
-		if (be32_to_cpu(new_vh->vol_id) > tmp_av->vol_id)
-			p = &(*p)->rb_left;
-		else if (be32_to_cpu(new_vh->vol_id) < tmp_av->vol_id)
-			p = &(*p)->rb_right;
-		else {
-			found = 1;
-			break;
-		}
-	}
-
-	if (found)
-		av = tmp_av;
-	else {
+	av = ubi_find_av(ai, vol_id);
+	if (!av) {
 		ubi_err(ubi, "orphaned volume in fastmap pool!");
-		kmem_cache_free(ai->aeb_slab_cache, new_aeb);
+		ubi_free_aeb(ai, new_aeb);
 		return UBI_BAD_FASTMAP;
 	}
 
-	ubi_assert(be32_to_cpu(new_vh->vol_id) == av->vol_id);
+	ubi_assert(vol_id == av->vol_id);
 
 	return update_vol(ubi, ai, av, new_vh, new_aeb);
 }
@@ -423,16 +383,12 @@ static void unmap_peb(struct ubi_attach_info *ai, int pnum)
 	struct rb_node *node, *node2;
 	struct ubi_ainf_peb *aeb;
 
-	for (node = rb_first(&ai->volumes); node; node = rb_next(node)) {
-		av = rb_entry(node, struct ubi_ainf_volume, rb);
-
-		for (node2 = rb_first(&av->root); node2;
-		     node2 = rb_next(node2)) {
-			aeb = rb_entry(node2, struct ubi_ainf_peb, u.rb);
+	ubi_rb_for_each_entry(node, av, &ai->volumes, rb) {
+		ubi_rb_for_each_entry(node2, aeb, &av->root, u.rb) {
 			if (aeb->pnum == pnum) {
 				rb_erase(&aeb->u.rb, &av->root);
 				av->leb_count--;
-				kmem_cache_free(ai->aeb_slab_cache, aeb);
+				ubi_free_aeb(ai, aeb);
 				return;
 			}
 		}
@@ -455,6 +411,7 @@ static int scan_pool(struct ubi_device *ubi, struct ubi_attach_info *ai,
 		     __be32 *pebs, int pool_size, unsigned long long *max_sqnum,
 		     struct list_head *free)
 {
+	struct ubi_vid_io_buf *vb;
 	struct ubi_vid_hdr *vh;
 	struct ubi_ec_hdr *ech;
 	struct ubi_ainf_peb *new_aeb;
@@ -464,12 +421,14 @@ static int scan_pool(struct ubi_device *ubi, struct ubi_attach_info *ai,
 	if (!ech)
 		return -ENOMEM;
 
-	vh = ubi_zalloc_vid_hdr(ubi, GFP_KERNEL);
-	if (!vh) {
+	vb = ubi_alloc_vid_buf(ubi, GFP_KERNEL);
+	if (!vb) {
 		kfree(ech);
 		return -ENOMEM;
 	}
 
+	vh = ubi_get_vid_hdr(vb);
+
 	dbg_bld("scanning fastmap pool: size = %i", pool_size);
 
 	/*
@@ -510,15 +469,16 @@ static int scan_pool(struct ubi_device *ubi, struct ubi_attach_info *ai,
 			goto out;
 		}
 
-		err = ubi_io_read_vid_hdr(ubi, pnum, vh, 0);
+		err = ubi_io_read_vid_hdr(ubi, pnum, vb, 0);
 		if (err == UBI_IO_FF || err == UBI_IO_FF_BITFLIPS) {
 			unsigned long long ec = be64_to_cpu(ech->ec);
 			unmap_peb(ai, pnum);
 			dbg_bld("Adding PEB to free: %i", pnum);
+
 			if (err == UBI_IO_FF_BITFLIPS)
-				add_aeb(ai, free, pnum, ec, 1);
-			else
-				add_aeb(ai, free, pnum, ec, 0);
+				scrub = 1;
+
+			add_aeb(ai, free, pnum, ec, scrub);
 			continue;
 		} else if (err == 0 || err == UBI_IO_BITFLIPS) {
 			dbg_bld("Found non empty PEB:%i in pool", pnum);
@@ -526,15 +486,12 @@ static int scan_pool(struct ubi_device *ubi, struct ubi_attach_info *ai,
 			if (err == UBI_IO_BITFLIPS)
 				scrub = 1;
 
-			new_aeb = kmem_cache_alloc(ai->aeb_slab_cache,
-						   GFP_KERNEL);
+			new_aeb = ubi_alloc_aeb(ai, pnum, be64_to_cpu(ech->ec));
 			if (!new_aeb) {
 				ret = -ENOMEM;
 				goto out;
 			}
 
-			new_aeb->ec = be64_to_cpu(ech->ec);
-			new_aeb->pnum = pnum;
 			new_aeb->lnum = be32_to_cpu(vh->lnum);
 			new_aeb->sqnum = be64_to_cpu(vh->sqnum);
 			new_aeb->copy_flag = vh->copy_flag;
@@ -558,7 +515,7 @@ static int scan_pool(struct ubi_device *ubi, struct ubi_attach_info *ai,
 	}
 
 out:
-	ubi_free_vid_hdr(ubi, vh);
+	ubi_free_vid_buf(vb);
 	kfree(ech);
 	return ret;
 }
@@ -841,11 +798,11 @@ fail_bad:
 fail:
 	list_for_each_entry_safe(tmp_aeb, _tmp_aeb, &used, u.list) {
 		list_del(&tmp_aeb->u.list);
-		kmem_cache_free(ai->aeb_slab_cache, tmp_aeb);
+		ubi_free_aeb(ai, tmp_aeb);
 	}
 	list_for_each_entry_safe(tmp_aeb, _tmp_aeb, &free, u.list) {
 		list_del(&tmp_aeb->u.list);
-		kmem_cache_free(ai->aeb_slab_cache, tmp_aeb);
+		ubi_free_aeb(ai, tmp_aeb);
 	}
 
 	return ret;
@@ -886,6 +843,7 @@ int ubi_scan_fastmap(struct ubi_device *ubi, struct ubi_attach_info *ai,
 		     struct ubi_attach_info *scan_ai)
 {
 	struct ubi_fm_sb *fmsb, *fmsb2;
+	struct ubi_vid_io_buf *vb;
 	struct ubi_vid_hdr *vh;
 	struct ubi_ec_hdr *ech;
 	struct ubi_fastmap_layout *fm;
@@ -919,7 +877,7 @@ int ubi_scan_fastmap(struct ubi_device *ubi, struct ubi_attach_info *ai,
 		goto out;
 	}
 
-	ret = ubi_io_read(ubi, fmsb, fm_anchor, ubi->leb_start, sizeof(*fmsb));
+	ret = ubi_io_read_data(ubi, fmsb, fm_anchor, 0, sizeof(*fmsb));
 	if (ret && ret != UBI_IO_BITFLIPS)
 		goto free_fm_sb;
 	else if (ret == UBI_IO_BITFLIPS)
@@ -961,12 +919,14 @@ int ubi_scan_fastmap(struct ubi_device *ubi, struct ubi_attach_info *ai,
 		goto free_fm_sb;
 	}
 
-	vh = ubi_zalloc_vid_hdr(ubi, GFP_KERNEL);
-	if (!vh) {
+	vb = ubi_alloc_vid_buf(ubi, GFP_KERNEL);
+	if (!vb) {
 		ret = -ENOMEM;
 		goto free_hdr;
 	}
 
+	vh = ubi_get_vid_hdr(vb);
+
 	for (i = 0; i < used_blocks; i++) {
 		int image_seq;
 
@@ -1009,7 +969,7 @@ int ubi_scan_fastmap(struct ubi_device *ubi, struct ubi_attach_info *ai,
 			goto free_hdr;
 		}
 
-		ret = ubi_io_read_vid_hdr(ubi, pnum, vh, 0);
+		ret = ubi_io_read_vid_hdr(ubi, pnum, vb, 0);
 		if (ret && ret != UBI_IO_BITFLIPS) {
 			ubi_err(ubi, "unable to read fastmap block# %i (PEB: %i)",
 				i, pnum);
@@ -1037,8 +997,8 @@ int ubi_scan_fastmap(struct ubi_device *ubi, struct ubi_attach_info *ai,
 		if (sqnum < be64_to_cpu(vh->sqnum))
 			sqnum = be64_to_cpu(vh->sqnum);
 
-		ret = ubi_io_read(ubi, ubi->fm_buf + (ubi->leb_size * i), pnum,
-				  ubi->leb_start, ubi->leb_size);
+		ret = ubi_io_read_data(ubi, ubi->fm_buf + (ubi->leb_size * i),
+				       pnum, 0, ubi->leb_size);
 		if (ret && ret != UBI_IO_BITFLIPS) {
 			ubi_err(ubi, "unable to read fastmap block# %i (PEB: %i, "
 				"err: %i)", i, pnum, ret);
@@ -1099,7 +1059,7 @@ int ubi_scan_fastmap(struct ubi_device *ubi, struct ubi_attach_info *ai,
 	ubi->fm_disabled = 0;
 	ubi->fast_attach = 1;
 
-	ubi_free_vid_hdr(ubi, vh);
+	ubi_free_vid_buf(vb);
 	kfree(ech);
 out:
 	up_write(&ubi->fm_protect);
@@ -1108,7 +1068,7 @@ out:
 	return ret;
 
 free_hdr:
-	ubi_free_vid_hdr(ubi, vh);
+	ubi_free_vid_buf(vb);
 	kfree(ech);
 free_fm_sb:
 	kfree(fmsb);
@@ -1136,6 +1096,7 @@ static int ubi_write_fastmap(struct ubi_device *ubi,
 	struct ubi_fm_eba *feba;
 	struct ubi_wl_entry *wl_e;
 	struct ubi_volume *vol;
+	struct ubi_vid_io_buf *avbuf, *dvbuf;
 	struct ubi_vid_hdr *avhdr, *dvhdr;
 	struct ubi_work *ubi_wrk;
 	struct rb_node *tmp_rb;
@@ -1146,18 +1107,21 @@ static int ubi_write_fastmap(struct ubi_device *ubi,
 	fm_raw = ubi->fm_buf;
 	memset(ubi->fm_buf, 0, ubi->fm_size);
 
-	avhdr = new_fm_vhdr(ubi, UBI_FM_SB_VOLUME_ID);
-	if (!avhdr) {
+	avbuf = new_fm_vbuf(ubi, UBI_FM_SB_VOLUME_ID);
+	if (!avbuf) {
 		ret = -ENOMEM;
 		goto out;
 	}
 
-	dvhdr = new_fm_vhdr(ubi, UBI_FM_DATA_VOLUME_ID);
-	if (!dvhdr) {
+	dvbuf = new_fm_vbuf(ubi, UBI_FM_DATA_VOLUME_ID);
+	if (!dvbuf) {
 		ret = -ENOMEM;
 		goto out_kfree;
 	}
 
+	avhdr = ubi_get_vid_hdr(avbuf);
+	dvhdr = ubi_get_vid_hdr(dvbuf);
+
 	seen_pebs = init_seen(ubi);
 	if (IS_ERR(seen_pebs)) {
 		ret = PTR_ERR(seen_pebs);
@@ -1306,8 +1270,12 @@ static int ubi_write_fastmap(struct ubi_device *ubi,
 		fm_pos += sizeof(*feba) + (sizeof(__be32) * vol->reserved_pebs);
 		ubi_assert(fm_pos <= ubi->fm_size);
 
-		for (j = 0; j < vol->reserved_pebs; j++)
-			feba->pnum[j] = cpu_to_be32(vol->eba_tbl[j]);
+		for (j = 0; j < vol->reserved_pebs; j++) {
+			struct ubi_eba_leb_desc ldesc;
+
+			ubi_eba_get_ldesc(vol, j, &ldesc);
+			feba->pnum[j] = cpu_to_be32(ldesc.pnum);
+		}
 
 		feba->reserved_pebs = cpu_to_be32(j);
 		feba->magic = cpu_to_be32(UBI_FM_EBA_MAGIC);
@@ -1322,7 +1290,7 @@ static int ubi_write_fastmap(struct ubi_device *ubi,
 	spin_unlock(&ubi->volumes_lock);
 
 	dbg_bld("writing fastmap SB to PEB %i", new_fm->e[0]->pnum);
-	ret = ubi_io_write_vid_hdr(ubi, new_fm->e[0]->pnum, avhdr);
+	ret = ubi_io_write_vid_hdr(ubi, new_fm->e[0]->pnum, avbuf);
 	if (ret) {
 		ubi_err(ubi, "unable to write vid_hdr to fastmap SB!");
 		goto out_kfree;
@@ -1343,7 +1311,7 @@ static int ubi_write_fastmap(struct ubi_device *ubi,
 		dvhdr->lnum = cpu_to_be32(i);
 		dbg_bld("writing fastmap data to PEB %i sqnum %llu",
 			new_fm->e[i]->pnum, be64_to_cpu(dvhdr->sqnum));
-		ret = ubi_io_write_vid_hdr(ubi, new_fm->e[i]->pnum, dvhdr);
+		ret = ubi_io_write_vid_hdr(ubi, new_fm->e[i]->pnum, dvbuf);
 		if (ret) {
 			ubi_err(ubi, "unable to write vid_hdr to PEB %i!",
 				new_fm->e[i]->pnum);
@@ -1352,8 +1320,8 @@ static int ubi_write_fastmap(struct ubi_device *ubi,
 	}
 
 	for (i = 0; i < new_fm->used_blocks; i++) {
-		ret = ubi_io_write(ubi, fm_raw + (i * ubi->leb_size),
-			new_fm->e[i]->pnum, ubi->leb_start, ubi->leb_size);
+		ret = ubi_io_write_data(ubi, fm_raw + (i * ubi->leb_size),
+					new_fm->e[i]->pnum, 0, ubi->leb_size);
 		if (ret) {
 			ubi_err(ubi, "unable to write fastmap to PEB %i!",
 				new_fm->e[i]->pnum);
@@ -1368,8 +1336,8 @@ static int ubi_write_fastmap(struct ubi_device *ubi,
 	dbg_bld("fastmap written!");
 
 out_kfree:
-	ubi_free_vid_hdr(ubi, avhdr);
-	ubi_free_vid_hdr(ubi, dvhdr);
+	ubi_free_vid_buf(avbuf);
+	ubi_free_vid_buf(dvbuf);
 	free_seen(seen_pebs);
 out:
 	return ret;
@@ -1439,7 +1407,8 @@ static int invalidate_fastmap(struct ubi_device *ubi)
 	int ret;
 	struct ubi_fastmap_layout *fm;
 	struct ubi_wl_entry *e;
-	struct ubi_vid_hdr *vh = NULL;
+	struct ubi_vid_io_buf *vb = NULL;
+	struct ubi_vid_hdr *vh;
 
 	if (!ubi->fm)
 		return 0;
@@ -1451,10 +1420,12 @@ static int invalidate_fastmap(struct ubi_device *ubi)
 	if (!fm)
 		goto out;
 
-	vh = new_fm_vhdr(ubi, UBI_FM_SB_VOLUME_ID);
-	if (!vh)
+	vb = new_fm_vbuf(ubi, UBI_FM_SB_VOLUME_ID);
+	if (!vb)
 		goto out_free_fm;
 
+	vh = ubi_get_vid_hdr(vb);
+
 	ret = -ENOSPC;
 	e = ubi_wl_get_fm_peb(ubi, 1);
 	if (!e)
@@ -1465,7 +1436,7 @@ static int invalidate_fastmap(struct ubi_device *ubi)
 	 * to scanning mode.
 	 */
 	vh->sqnum = cpu_to_be64(ubi_next_sqnum(ubi));
-	ret = ubi_io_write_vid_hdr(ubi, e->pnum, vh);
+	ret = ubi_io_write_vid_hdr(ubi, e->pnum, vb);
 	if (ret < 0) {
 		ubi_wl_put_fm_peb(ubi, e, 0, 0);
 		goto out_free_fm;
@@ -1477,7 +1448,7 @@ static int invalidate_fastmap(struct ubi_device *ubi)
 	ubi->fm = fm;
 
 out:
-	ubi_free_vid_hdr(ubi, vh);
+	ubi_free_vid_buf(vb);
 	return ret;
 
 out_free_fm:
@@ -1522,22 +1493,30 @@ int ubi_update_fastmap(struct ubi_device *ubi)
 	struct ubi_wl_entry *tmp_e;
 
 	down_write(&ubi->fm_protect);
+	down_write(&ubi->work_sem);
+	down_write(&ubi->fm_eba_sem);
 
 	ubi_refill_pools(ubi);
 
 	if (ubi->ro_mode || ubi->fm_disabled) {
+		up_write(&ubi->fm_eba_sem);
+		up_write(&ubi->work_sem);
 		up_write(&ubi->fm_protect);
 		return 0;
 	}
 
 	ret = ubi_ensure_anchor_pebs(ubi);
 	if (ret) {
+		up_write(&ubi->fm_eba_sem);
+		up_write(&ubi->work_sem);
 		up_write(&ubi->fm_protect);
 		return ret;
 	}
 
 	new_fm = kzalloc(sizeof(*new_fm), GFP_KERNEL);
 	if (!new_fm) {
+		up_write(&ubi->fm_eba_sem);
+		up_write(&ubi->work_sem);
 		up_write(&ubi->fm_protect);
 		return -ENOMEM;
 	}
@@ -1646,16 +1625,14 @@ int ubi_update_fastmap(struct ubi_device *ubi)
 		new_fm->e[0] = tmp_e;
 	}
 
-	down_write(&ubi->work_sem);
-	down_write(&ubi->fm_eba_sem);
 	ret = ubi_write_fastmap(ubi, new_fm);
-	up_write(&ubi->fm_eba_sem);
-	up_write(&ubi->work_sem);
 
 	if (ret)
 		goto err;
 
 out_unlock:
+	up_write(&ubi->fm_eba_sem);
+	up_write(&ubi->work_sem);
 	up_write(&ubi->fm_protect);
 	kfree(old_fm);
 	return ret;
diff --git a/drivers/mtd/ubi/io.c b/drivers/mtd/ubi/io.c
index ff8cafe1e5cd..b6fb8f945c21 100644
--- a/drivers/mtd/ubi/io.c
+++ b/drivers/mtd/ubi/io.c
@@ -502,6 +502,7 @@ static int nor_erase_prepare(struct ubi_device *ubi, int pnum)
 	loff_t addr;
 	uint32_t data = 0;
 	struct ubi_ec_hdr ec_hdr;
+	struct ubi_vid_io_buf vidb;
 
 	/*
 	 * Note, we cannot generally define VID header buffers on stack,
@@ -528,7 +529,10 @@ static int nor_erase_prepare(struct ubi_device *ubi, int pnum)
 			goto error;
 	}
 
-	err = ubi_io_read_vid_hdr(ubi, pnum, &vid_hdr, 0);
+	ubi_init_vid_buf(ubi, &vidb, &vid_hdr);
+	ubi_assert(&vid_hdr == ubi_get_vid_hdr(&vidb));
+
+	err = ubi_io_read_vid_hdr(ubi, pnum, &vidb, 0);
 	if (err != UBI_IO_BAD_HDR_EBADMSG && err != UBI_IO_BAD_HDR &&
 	    err != UBI_IO_FF){
 		addr += ubi->vid_hdr_aloffset;
@@ -995,12 +999,11 @@ bad:
  * ubi_io_read_vid_hdr - read and check a volume identifier header.
  * @ubi: UBI device description object
  * @pnum: physical eraseblock number to read from
- * @vid_hdr: &struct ubi_vid_hdr object where to store the read volume
- * identifier header
+ * @vidb: the volume identifier buffer to store data in
  * @verbose: be verbose if the header is corrupted or wasn't found
  *
  * This function reads the volume identifier header from physical eraseblock
- * @pnum and stores it in @vid_hdr. It also checks CRC checksum of the read
+ * @pnum and stores it in @vidb. It also checks CRC checksum of the read
  * volume identifier header. The error codes are the same as in
  * 'ubi_io_read_ec_hdr()'.
  *
@@ -1008,16 +1011,16 @@ bad:
  * 'ubi_io_read_ec_hdr()', so refer commentaries in 'ubi_io_read_ec_hdr()'.
  */
 int ubi_io_read_vid_hdr(struct ubi_device *ubi, int pnum,
-			struct ubi_vid_hdr *vid_hdr, int verbose)
+			struct ubi_vid_io_buf *vidb, int verbose)
 {
 	int err, read_err;
 	uint32_t crc, magic, hdr_crc;
-	void *p;
+	struct ubi_vid_hdr *vid_hdr = ubi_get_vid_hdr(vidb);
+	void *p = vidb->buffer;
 
 	dbg_io("read VID header from PEB %d", pnum);
 	ubi_assert(pnum >= 0 &&  pnum < ubi->peb_count);
 
-	p = (char *)vid_hdr - ubi->vid_hdr_shift;
 	read_err = ubi_io_read(ubi, p, pnum, ubi->vid_hdr_aloffset,
 			  ubi->vid_hdr_shift + UBI_VID_HDR_SIZE);
 	if (read_err && read_err != UBI_IO_BITFLIPS && !mtd_is_eccerr(read_err))
@@ -1080,23 +1083,24 @@ int ubi_io_read_vid_hdr(struct ubi_device *ubi, int pnum,
  * ubi_io_write_vid_hdr - write a volume identifier header.
  * @ubi: UBI device description object
  * @pnum: the physical eraseblock number to write to
- * @vid_hdr: the volume identifier header to write
+ * @vidb: the volume identifier buffer to write
  *
  * This function writes the volume identifier header described by @vid_hdr to
  * physical eraseblock @pnum. This function automatically fills the
- * @vid_hdr->magic and the @vid_hdr->version fields, as well as calculates
- * header CRC checksum and stores it at vid_hdr->hdr_crc.
+ * @vidb->hdr->magic and the @vidb->hdr->version fields, as well as calculates
+ * header CRC checksum and stores it at vidb->hdr->hdr_crc.
  *
  * This function returns zero in case of success and a negative error code in
  * case of failure. If %-EIO is returned, the physical eraseblock probably went
  * bad.
  */
 int ubi_io_write_vid_hdr(struct ubi_device *ubi, int pnum,
-			 struct ubi_vid_hdr *vid_hdr)
+			 struct ubi_vid_io_buf *vidb)
 {
+	struct ubi_vid_hdr *vid_hdr = ubi_get_vid_hdr(vidb);
 	int err;
 	uint32_t crc;
-	void *p;
+	void *p = vidb->buffer;
 
 	dbg_io("write VID header to PEB %d", pnum);
 	ubi_assert(pnum >= 0 &&  pnum < ubi->peb_count);
@@ -1117,7 +1121,6 @@ int ubi_io_write_vid_hdr(struct ubi_device *ubi, int pnum,
 	if (ubi_dbg_power_cut(ubi, POWER_CUT_VID_WRITE))
 		return -EROFS;
 
-	p = (char *)vid_hdr - ubi->vid_hdr_shift;
 	err = ubi_io_write(ubi, p, pnum, ubi->vid_hdr_aloffset,
 			   ubi->vid_hdr_alsize);
 	return err;
@@ -1283,17 +1286,19 @@ static int self_check_peb_vid_hdr(const struct ubi_device *ubi, int pnum)
 {
 	int err;
 	uint32_t crc, hdr_crc;
+	struct ubi_vid_io_buf *vidb;
 	struct ubi_vid_hdr *vid_hdr;
 	void *p;
 
 	if (!ubi_dbg_chk_io(ubi))
 		return 0;
 
-	vid_hdr = ubi_zalloc_vid_hdr(ubi, GFP_NOFS);
-	if (!vid_hdr)
+	vidb = ubi_alloc_vid_buf(ubi, GFP_NOFS);
+	if (!vidb)
 		return -ENOMEM;
 
-	p = (char *)vid_hdr - ubi->vid_hdr_shift;
+	vid_hdr = ubi_get_vid_hdr(vidb);
+	p = vidb->buffer;
 	err = ubi_io_read(ubi, p, pnum, ubi->vid_hdr_aloffset,
 			  ubi->vid_hdr_alsize);
 	if (err && err != UBI_IO_BITFLIPS && !mtd_is_eccerr(err))
@@ -1314,7 +1319,7 @@ static int self_check_peb_vid_hdr(const struct ubi_device *ubi, int pnum)
 	err = self_check_vid_hdr(ubi, pnum, vid_hdr);
 
 exit:
-	ubi_free_vid_hdr(ubi, vid_hdr);
+	ubi_free_vid_buf(vidb);
 	return err;
 }
 
diff --git a/drivers/mtd/ubi/kapi.c b/drivers/mtd/ubi/kapi.c
index a9e2cef7c95c..88b1897aeb40 100644
--- a/drivers/mtd/ubi/kapi.c
+++ b/drivers/mtd/ubi/kapi.c
@@ -538,7 +538,7 @@ int ubi_leb_write(struct ubi_volume_desc *desc, int lnum, const void *buf,
 	if (desc->mode == UBI_READONLY || vol->vol_type == UBI_STATIC_VOLUME)
 		return -EROFS;
 
-	if (lnum < 0 || lnum >= vol->reserved_pebs || offset < 0 || len < 0 ||
+	if (!ubi_leb_valid(vol, lnum) || offset < 0 || len < 0 ||
 	    offset + len > vol->usable_leb_size ||
 	    offset & (ubi->min_io_size - 1) || len & (ubi->min_io_size - 1))
 		return -EINVAL;
@@ -583,7 +583,7 @@ int ubi_leb_change(struct ubi_volume_desc *desc, int lnum, const void *buf,
 	if (desc->mode == UBI_READONLY || vol->vol_type == UBI_STATIC_VOLUME)
 		return -EROFS;
 
-	if (lnum < 0 || lnum >= vol->reserved_pebs || len < 0 ||
+	if (!ubi_leb_valid(vol, lnum) || len < 0 ||
 	    len > vol->usable_leb_size || len & (ubi->min_io_size - 1))
 		return -EINVAL;
 
@@ -620,7 +620,7 @@ int ubi_leb_erase(struct ubi_volume_desc *desc, int lnum)
 	if (desc->mode == UBI_READONLY || vol->vol_type == UBI_STATIC_VOLUME)
 		return -EROFS;
 
-	if (lnum < 0 || lnum >= vol->reserved_pebs)
+	if (!ubi_leb_valid(vol, lnum))
 		return -EINVAL;
 
 	if (vol->upd_marker)
@@ -680,7 +680,7 @@ int ubi_leb_unmap(struct ubi_volume_desc *desc, int lnum)
 	if (desc->mode == UBI_READONLY || vol->vol_type == UBI_STATIC_VOLUME)
 		return -EROFS;
 
-	if (lnum < 0 || lnum >= vol->reserved_pebs)
+	if (!ubi_leb_valid(vol, lnum))
 		return -EINVAL;
 
 	if (vol->upd_marker)
@@ -716,13 +716,13 @@ int ubi_leb_map(struct ubi_volume_desc *desc, int lnum)
 	if (desc->mode == UBI_READONLY || vol->vol_type == UBI_STATIC_VOLUME)
 		return -EROFS;
 
-	if (lnum < 0 || lnum >= vol->reserved_pebs)
+	if (!ubi_leb_valid(vol, lnum))
 		return -EINVAL;
 
 	if (vol->upd_marker)
 		return -EBADF;
 
-	if (vol->eba_tbl[lnum] >= 0)
+	if (ubi_eba_is_mapped(vol, lnum))
 		return -EBADMSG;
 
 	return ubi_eba_write_leb(ubi, vol, lnum, NULL, 0, 0);
@@ -751,13 +751,13 @@ int ubi_is_mapped(struct ubi_volume_desc *desc, int lnum)
 
 	dbg_gen("test LEB %d:%d", vol->vol_id, lnum);
 
-	if (lnum < 0 || lnum >= vol->reserved_pebs)
+	if (!ubi_leb_valid(vol, lnum))
 		return -EINVAL;
 
 	if (vol->upd_marker)
 		return -EBADF;
 
-	return vol->eba_tbl[lnum] >= 0;
+	return ubi_eba_is_mapped(vol, lnum);
 }
 EXPORT_SYMBOL_GPL(ubi_is_mapped);
 
diff --git a/drivers/mtd/ubi/ubi.h b/drivers/mtd/ubi/ubi.h
index b616a115c9d3..697dbcba7371 100644
--- a/drivers/mtd/ubi/ubi.h
+++ b/drivers/mtd/ubi/ubi.h
@@ -167,6 +167,17 @@ enum {
 };
 
 /**
+ * struct ubi_vid_io_buf - VID buffer used to read/write VID info to/from the
+ *			   flash.
+ * @hdr: a pointer to the VID header stored in buffer
+ * @buffer: underlying buffer
+ */
+struct ubi_vid_io_buf {
+	struct ubi_vid_hdr *hdr;
+	void *buffer;
+};
+
+/**
  * struct ubi_wl_entry - wear-leveling entry.
  * @u.rb: link in the corresponding (free/used) RB-tree
  * @u.list: link in the protection queue
@@ -267,6 +278,21 @@ struct ubi_fm_pool {
 };
 
 /**
+ * struct ubi_eba_leb_desc - EBA logical eraseblock descriptor
+ * @lnum: the logical eraseblock number
+ * @pnum: the physical eraseblock where the LEB can be found
+ *
+ * This structure is here to hide EBA's internal from other part of the
+ * UBI implementation.
+ *
+ * One can query the position of a LEB by calling ubi_eba_get_ldesc().
+ */
+struct ubi_eba_leb_desc {
+	int lnum;
+	int pnum;
+};
+
+/**
  * struct ubi_volume - UBI volume description data structure.
  * @dev: device object to make use of the the Linux device model
  * @cdev: character device object to create character device
@@ -344,7 +370,7 @@ struct ubi_volume {
 	long long upd_received;
 	void *upd_buf;
 
-	int *eba_tbl;
+	struct ubi_eba_table *eba_tbl;
 	unsigned int checked:1;
 	unsigned int corrupted:1;
 	unsigned int upd_marker:1;
@@ -724,6 +750,8 @@ struct ubi_ainf_volume {
  * @ec_sum: a temporary variable used when calculating @mean_ec
  * @ec_count: a temporary variable used when calculating @mean_ec
  * @aeb_slab_cache: slab cache for &struct ubi_ainf_peb objects
+ * @ech: temporary EC header. Only available during scan
+ * @vidh: temporary VID buffer. Only available during scan
  *
  * This data structure contains the result of attaching an MTD device and may
  * be used by other UBI sub-systems to build final UBI data structures, further
@@ -752,6 +780,8 @@ struct ubi_attach_info {
 	uint64_t ec_sum;
 	int ec_count;
 	struct kmem_cache *aeb_slab_cache;
+	struct ubi_ec_hdr *ech;
+	struct ubi_vid_io_buf *vidb;
 };
 
 /**
@@ -792,8 +822,12 @@ extern struct mutex ubi_devices_mutex;
 extern struct blocking_notifier_head ubi_notifiers;
 
 /* attach.c */
+struct ubi_ainf_peb *ubi_alloc_aeb(struct ubi_attach_info *ai, int pnum,
+				   int ec);
+void ubi_free_aeb(struct ubi_attach_info *ai, struct ubi_ainf_peb *aeb);
 int ubi_add_to_av(struct ubi_device *ubi, struct ubi_attach_info *ai, int pnum,
 		  int ec, const struct ubi_vid_hdr *vid_hdr, int bitflips);
+struct ubi_ainf_volume *ubi_add_av(struct ubi_attach_info *ai, int vol_id);
 struct ubi_ainf_volume *ubi_find_av(const struct ubi_attach_info *ai,
 				    int vol_id);
 void ubi_remove_av(struct ubi_attach_info *ai, struct ubi_ainf_volume *av);
@@ -835,7 +869,21 @@ void ubi_update_reserved(struct ubi_device *ubi);
 void ubi_calculate_reserved(struct ubi_device *ubi);
 int ubi_check_pattern(const void *buf, uint8_t patt, int size);
 
+static inline bool ubi_leb_valid(struct ubi_volume *vol, int lnum)
+{
+	return lnum >= 0 && lnum < vol->reserved_pebs;
+}
+
 /* eba.c */
+struct ubi_eba_table *ubi_eba_create_table(struct ubi_volume *vol,
+					   int nentries);
+void ubi_eba_destroy_table(struct ubi_eba_table *tbl);
+void ubi_eba_copy_table(struct ubi_volume *vol, struct ubi_eba_table *dst,
+			int nentries);
+void ubi_eba_replace_table(struct ubi_volume *vol, struct ubi_eba_table *tbl);
+void ubi_eba_get_ldesc(struct ubi_volume *vol, int lnum,
+		       struct ubi_eba_leb_desc *ldesc);
+bool ubi_eba_is_mapped(struct ubi_volume *vol, int lnum);
 int ubi_eba_unmap_leb(struct ubi_device *ubi, struct ubi_volume *vol,
 		      int lnum);
 int ubi_eba_read_leb(struct ubi_device *ubi, struct ubi_volume *vol, int lnum,
@@ -850,7 +898,7 @@ int ubi_eba_write_leb_st(struct ubi_device *ubi, struct ubi_volume *vol,
 int ubi_eba_atomic_leb_change(struct ubi_device *ubi, struct ubi_volume *vol,
 			      int lnum, const void *buf, int len);
 int ubi_eba_copy_leb(struct ubi_device *ubi, int from, int to,
-		     struct ubi_vid_hdr *vid_hdr);
+		     struct ubi_vid_io_buf *vidb);
 int ubi_eba_init(struct ubi_device *ubi, struct ubi_attach_info *ai);
 unsigned long long ubi_next_sqnum(struct ubi_device *ubi);
 int self_check_eba(struct ubi_device *ubi, struct ubi_attach_info *ai_fastmap,
@@ -885,9 +933,9 @@ int ubi_io_read_ec_hdr(struct ubi_device *ubi, int pnum,
 int ubi_io_write_ec_hdr(struct ubi_device *ubi, int pnum,
 			struct ubi_ec_hdr *ec_hdr);
 int ubi_io_read_vid_hdr(struct ubi_device *ubi, int pnum,
-			struct ubi_vid_hdr *vid_hdr, int verbose);
+			struct ubi_vid_io_buf *vidb, int verbose);
 int ubi_io_write_vid_hdr(struct ubi_device *ubi, int pnum,
-			 struct ubi_vid_hdr *vid_hdr);
+			 struct ubi_vid_io_buf *vidb);
 
 /* build.c */
 int ubi_attach_mtd_dev(struct mtd_info *mtd, int ubi_num,
@@ -1008,44 +1056,68 @@ static inline void ubi_move_aeb_to_list(struct ubi_ainf_volume *av,
 }
 
 /**
- * ubi_zalloc_vid_hdr - allocate a volume identifier header object.
- * @ubi: UBI device description object
- * @gfp_flags: GFP flags to allocate with
- *
- * This function returns a pointer to the newly allocated and zero-filled
- * volume identifier header object in case of success and %NULL in case of
- * failure.
+ * ubi_init_vid_buf - Initialize a VID buffer
+ * @ubi: the UBI device
+ * @vidb: the VID buffer to initialize
+ * @buf: the underlying buffer
  */
-static inline struct ubi_vid_hdr *
-ubi_zalloc_vid_hdr(const struct ubi_device *ubi, gfp_t gfp_flags)
+static inline void ubi_init_vid_buf(const struct ubi_device *ubi,
+				    struct ubi_vid_io_buf *vidb,
+				    void *buf)
 {
-	void *vid_hdr;
+	if (buf)
+		memset(buf, 0, ubi->vid_hdr_alsize);
 
-	vid_hdr = kzalloc(ubi->vid_hdr_alsize, gfp_flags);
-	if (!vid_hdr)
+	vidb->buffer = buf;
+	vidb->hdr = buf + ubi->vid_hdr_shift;
+}
+
+/**
+ * ubi_init_vid_buf - Allocate a VID buffer
+ * @ubi: the UBI device
+ * @gfp_flags: GFP flags to use for the allocation
+ */
+static inline struct ubi_vid_io_buf *
+ubi_alloc_vid_buf(const struct ubi_device *ubi, gfp_t gfp_flags)
+{
+	struct ubi_vid_io_buf *vidb;
+	void *buf;
+
+	vidb = kzalloc(sizeof(*vidb), gfp_flags);
+	if (!vidb)
+		return NULL;
+
+	buf = kmalloc(ubi->vid_hdr_alsize, gfp_flags);
+	if (!buf) {
+		kfree(vidb);
 		return NULL;
+	}
 
-	/*
-	 * VID headers may be stored at un-aligned flash offsets, so we shift
-	 * the pointer.
-	 */
-	return vid_hdr + ubi->vid_hdr_shift;
+	ubi_init_vid_buf(ubi, vidb, buf);
+
+	return vidb;
 }
 
 /**
- * ubi_free_vid_hdr - free a volume identifier header object.
- * @ubi: UBI device description object
- * @vid_hdr: the object to free
+ * ubi_free_vid_buf - Free a VID buffer
+ * @vidb: the VID buffer to free
  */
-static inline void ubi_free_vid_hdr(const struct ubi_device *ubi,
-				    struct ubi_vid_hdr *vid_hdr)
+static inline void ubi_free_vid_buf(struct ubi_vid_io_buf *vidb)
 {
-	void *p = vid_hdr;
-
-	if (!p)
+	if (!vidb)
 		return;
 
-	kfree(p - ubi->vid_hdr_shift);
+	kfree(vidb->buffer);
+	kfree(vidb);
+}
+
+/**
+ * ubi_get_vid_hdr - Get the VID header attached to a VID buffer
+ * @vidb: VID buffer
+ */
+static inline struct ubi_vid_hdr *ubi_get_vid_hdr(struct ubi_vid_io_buf *vidb)
+{
+	return vidb->hdr;
 }
 
 /*
diff --git a/drivers/mtd/ubi/vmt.c b/drivers/mtd/ubi/vmt.c
index 0138f526474a..7ac78c13dd1c 100644
--- a/drivers/mtd/ubi/vmt.c
+++ b/drivers/mtd/ubi/vmt.c
@@ -138,7 +138,7 @@ static void vol_release(struct device *dev)
 {
 	struct ubi_volume *vol = container_of(dev, struct ubi_volume, dev);
 
-	kfree(vol->eba_tbl);
+	ubi_eba_replace_table(vol, NULL);
 	kfree(vol);
 }
 
@@ -158,6 +158,7 @@ int ubi_create_volume(struct ubi_device *ubi, struct ubi_mkvol_req *req)
 	int i, err, vol_id = req->vol_id, do_free = 1;
 	struct ubi_volume *vol;
 	struct ubi_vtbl_record vtbl_rec;
+	struct ubi_eba_table *eba_tbl = NULL;
 	dev_t dev;
 
 	if (ubi->ro_mode)
@@ -241,14 +242,13 @@ int ubi_create_volume(struct ubi_device *ubi, struct ubi_mkvol_req *req)
 	if (err)
 		goto out_acc;
 
-	vol->eba_tbl = kmalloc(vol->reserved_pebs * sizeof(int), GFP_KERNEL);
-	if (!vol->eba_tbl) {
-		err = -ENOMEM;
+	eba_tbl = ubi_eba_create_table(vol, vol->reserved_pebs);
+	if (IS_ERR(eba_tbl)) {
+		err = PTR_ERR(eba_tbl);
 		goto out_acc;
 	}
 
-	for (i = 0; i < vol->reserved_pebs; i++)
-		vol->eba_tbl[i] = UBI_LEB_UNMAPPED;
+	ubi_eba_replace_table(vol, eba_tbl);
 
 	if (vol->vol_type == UBI_DYNAMIC_VOLUME) {
 		vol->used_ebs = vol->reserved_pebs;
@@ -329,7 +329,7 @@ out_cdev:
 	cdev_del(&vol->cdev);
 out_mapping:
 	if (do_free)
-		kfree(vol->eba_tbl);
+		ubi_eba_destroy_table(eba_tbl);
 out_acc:
 	spin_lock(&ubi->volumes_lock);
 	ubi->rsvd_pebs -= vol->reserved_pebs;
@@ -427,10 +427,11 @@ out_unlock:
  */
 int ubi_resize_volume(struct ubi_volume_desc *desc, int reserved_pebs)
 {
-	int i, err, pebs, *new_mapping;
+	int i, err, pebs;
 	struct ubi_volume *vol = desc->vol;
 	struct ubi_device *ubi = vol->ubi;
 	struct ubi_vtbl_record vtbl_rec;
+	struct ubi_eba_table *new_eba_tbl = NULL;
 	int vol_id = vol->vol_id;
 
 	if (ubi->ro_mode)
@@ -450,12 +451,9 @@ int ubi_resize_volume(struct ubi_volume_desc *desc, int reserved_pebs)
 	if (reserved_pebs == vol->reserved_pebs)
 		return 0;
 
-	new_mapping = kmalloc(reserved_pebs * sizeof(int), GFP_KERNEL);
-	if (!new_mapping)
-		return -ENOMEM;
-
-	for (i = 0; i < reserved_pebs; i++)
-		new_mapping[i] = UBI_LEB_UNMAPPED;
+	new_eba_tbl = ubi_eba_create_table(vol, reserved_pebs);
+	if (IS_ERR(new_eba_tbl))
+		return PTR_ERR(new_eba_tbl);
 
 	spin_lock(&ubi->volumes_lock);
 	if (vol->ref_count > 1) {
@@ -481,10 +479,8 @@ int ubi_resize_volume(struct ubi_volume_desc *desc, int reserved_pebs)
 		}
 		ubi->avail_pebs -= pebs;
 		ubi->rsvd_pebs += pebs;
-		for (i = 0; i < vol->reserved_pebs; i++)
-			new_mapping[i] = vol->eba_tbl[i];
-		kfree(vol->eba_tbl);
-		vol->eba_tbl = new_mapping;
+		ubi_eba_copy_table(vol, new_eba_tbl, vol->reserved_pebs);
+		ubi_eba_replace_table(vol, new_eba_tbl);
 		spin_unlock(&ubi->volumes_lock);
 	}
 
@@ -498,10 +494,8 @@ int ubi_resize_volume(struct ubi_volume_desc *desc, int reserved_pebs)
 		ubi->rsvd_pebs += pebs;
 		ubi->avail_pebs -= pebs;
 		ubi_update_reserved(ubi);
-		for (i = 0; i < reserved_pebs; i++)
-			new_mapping[i] = vol->eba_tbl[i];
-		kfree(vol->eba_tbl);
-		vol->eba_tbl = new_mapping;
+		ubi_eba_copy_table(vol, new_eba_tbl, reserved_pebs);
+		ubi_eba_replace_table(vol, new_eba_tbl);
 		spin_unlock(&ubi->volumes_lock);
 	}
 
@@ -543,7 +537,7 @@ out_acc:
 		spin_unlock(&ubi->volumes_lock);
 	}
 out_free:
-	kfree(new_mapping);
+	kfree(new_eba_tbl);
 	return err;
 }
 
diff --git a/drivers/mtd/ubi/vtbl.c b/drivers/mtd/ubi/vtbl.c
index d85c19762160..263743e7b741 100644
--- a/drivers/mtd/ubi/vtbl.c
+++ b/drivers/mtd/ubi/vtbl.c
@@ -299,15 +299,18 @@ static int create_vtbl(struct ubi_device *ubi, struct ubi_attach_info *ai,
 		       int copy, void *vtbl)
 {
 	int err, tries = 0;
+	struct ubi_vid_io_buf *vidb;
 	struct ubi_vid_hdr *vid_hdr;
 	struct ubi_ainf_peb *new_aeb;
 
 	dbg_gen("create volume table (copy #%d)", copy + 1);
 
-	vid_hdr = ubi_zalloc_vid_hdr(ubi, GFP_KERNEL);
-	if (!vid_hdr)
+	vidb = ubi_alloc_vid_buf(ubi, GFP_KERNEL);
+	if (!vidb)
 		return -ENOMEM;
 
+	vid_hdr = ubi_get_vid_hdr(vidb);
+
 retry:
 	new_aeb = ubi_early_get_peb(ubi, ai);
 	if (IS_ERR(new_aeb)) {
@@ -324,7 +327,7 @@ retry:
 	vid_hdr->sqnum = cpu_to_be64(++ai->max_sqnum);
 
 	/* The EC header is already there, write the VID header */
-	err = ubi_io_write_vid_hdr(ubi, new_aeb->pnum, vid_hdr);
+	err = ubi_io_write_vid_hdr(ubi, new_aeb->pnum, vidb);
 	if (err)
 		goto write_error;
 
@@ -338,8 +341,8 @@ retry:
 	 * of this LEB as it will be deleted and freed in 'ubi_add_to_av()'.
 	 */
 	err = ubi_add_to_av(ubi, ai, new_aeb->pnum, new_aeb->ec, vid_hdr, 0);
-	kmem_cache_free(ai->aeb_slab_cache, new_aeb);
-	ubi_free_vid_hdr(ubi, vid_hdr);
+	ubi_free_aeb(ai, new_aeb);
+	ubi_free_vid_buf(vidb);
 	return err;
 
 write_error:
@@ -351,9 +354,9 @@ write_error:
 		list_add(&new_aeb->u.list, &ai->erase);
 		goto retry;
 	}
-	kmem_cache_free(ai->aeb_slab_cache, new_aeb);
+	ubi_free_aeb(ai, new_aeb);
 out_free:
-	ubi_free_vid_hdr(ubi, vid_hdr);
+	ubi_free_vid_buf(vidb);
 	return err;
 
 }
diff --git a/drivers/mtd/ubi/wl.c b/drivers/mtd/ubi/wl.c
index f4533266d7b2..b5b8cd6f481c 100644
--- a/drivers/mtd/ubi/wl.c
+++ b/drivers/mtd/ubi/wl.c
@@ -580,7 +580,7 @@ static int erase_worker(struct ubi_device *ubi, struct ubi_work *wl_wrk,
  * failure.
  */
 static int schedule_erase(struct ubi_device *ubi, struct ubi_wl_entry *e,
-			  int vol_id, int lnum, int torture)
+			  int vol_id, int lnum, int torture, bool nested)
 {
 	struct ubi_work *wl_wrk;
 
@@ -599,7 +599,10 @@ static int schedule_erase(struct ubi_device *ubi, struct ubi_wl_entry *e,
 	wl_wrk->lnum = lnum;
 	wl_wrk->torture = torture;
 
-	schedule_ubi_work(ubi, wl_wrk);
+	if (nested)
+		__schedule_ubi_work(ubi, wl_wrk);
+	else
+		schedule_ubi_work(ubi, wl_wrk);
 	return 0;
 }
 
@@ -644,11 +647,12 @@ static int wear_leveling_worker(struct ubi_device *ubi, struct ubi_work *wrk,
 				int shutdown)
 {
 	int err, scrubbing = 0, torture = 0, protect = 0, erroneous = 0;
-	int vol_id = -1, lnum = -1;
+	int erase = 0, keep = 0, vol_id = -1, lnum = -1;
 #ifdef CONFIG_MTD_UBI_FASTMAP
 	int anchor = wrk->anchor;
 #endif
 	struct ubi_wl_entry *e1, *e2;
+	struct ubi_vid_io_buf *vidb;
 	struct ubi_vid_hdr *vid_hdr;
 	int dst_leb_clean = 0;
 
@@ -656,10 +660,13 @@ static int wear_leveling_worker(struct ubi_device *ubi, struct ubi_work *wrk,
 	if (shutdown)
 		return 0;
 
-	vid_hdr = ubi_zalloc_vid_hdr(ubi, GFP_NOFS);
-	if (!vid_hdr)
+	vidb = ubi_alloc_vid_buf(ubi, GFP_NOFS);
+	if (!vidb)
 		return -ENOMEM;
 
+	vid_hdr = ubi_get_vid_hdr(vidb);
+
+	down_read(&ubi->fm_eba_sem);
 	mutex_lock(&ubi->move_mutex);
 	spin_lock(&ubi->wl_lock);
 	ubi_assert(!ubi->move_from && !ubi->move_to);
@@ -753,7 +760,7 @@ static int wear_leveling_worker(struct ubi_device *ubi, struct ubi_work *wrk,
 	 * which is being moved was unmapped.
 	 */
 
-	err = ubi_io_read_vid_hdr(ubi, e1->pnum, vid_hdr, 0);
+	err = ubi_io_read_vid_hdr(ubi, e1->pnum, vidb, 0);
 	if (err && err != UBI_IO_BITFLIPS) {
 		dst_leb_clean = 1;
 		if (err == UBI_IO_FF) {
@@ -780,6 +787,16 @@ static int wear_leveling_worker(struct ubi_device *ubi, struct ubi_work *wrk,
 			       e1->pnum);
 			scrubbing = 1;
 			goto out_not_moved;
+		} else if (ubi->fast_attach && err == UBI_IO_BAD_HDR_EBADMSG) {
+			/*
+			 * While a full scan would detect interrupted erasures
+			 * at attach time we can face them here when attached from
+			 * Fastmap.
+			 */
+			dbg_wl("PEB %d has ECC errors, maybe from an interrupted erasure",
+			       e1->pnum);
+			erase = 1;
+			goto out_not_moved;
 		}
 
 		ubi_err(ubi, "error %d while reading VID header from PEB %d",
@@ -790,7 +807,7 @@ static int wear_leveling_worker(struct ubi_device *ubi, struct ubi_work *wrk,
 	vol_id = be32_to_cpu(vid_hdr->vol_id);
 	lnum = be32_to_cpu(vid_hdr->lnum);
 
-	err = ubi_eba_copy_leb(ubi, e1->pnum, e2->pnum, vid_hdr);
+	err = ubi_eba_copy_leb(ubi, e1->pnum, e2->pnum, vidb);
 	if (err) {
 		if (err == MOVE_CANCEL_RACE) {
 			/*
@@ -815,6 +832,7 @@ static int wear_leveling_worker(struct ubi_device *ubi, struct ubi_work *wrk,
 			 * Target PEB had bit-flips or write error - torture it.
 			 */
 			torture = 1;
+			keep = 1;
 			goto out_not_moved;
 		}
 
@@ -847,7 +865,7 @@ static int wear_leveling_worker(struct ubi_device *ubi, struct ubi_work *wrk,
 	if (scrubbing)
 		ubi_msg(ubi, "scrubbed PEB %d (LEB %d:%d), data moved to PEB %d",
 			e1->pnum, vol_id, lnum, e2->pnum);
-	ubi_free_vid_hdr(ubi, vid_hdr);
+	ubi_free_vid_buf(vidb);
 
 	spin_lock(&ubi->wl_lock);
 	if (!ubi->move_to_put) {
@@ -879,6 +897,7 @@ static int wear_leveling_worker(struct ubi_device *ubi, struct ubi_work *wrk,
 
 	dbg_wl("done");
 	mutex_unlock(&ubi->move_mutex);
+	up_read(&ubi->fm_eba_sem);
 	return 0;
 
 	/*
@@ -901,7 +920,7 @@ out_not_moved:
 		ubi->erroneous_peb_count += 1;
 	} else if (scrubbing)
 		wl_tree_add(e1, &ubi->scrub);
-	else
+	else if (keep)
 		wl_tree_add(e1, &ubi->used);
 	if (dst_leb_clean) {
 		wl_tree_add(e2, &ubi->free);
@@ -913,7 +932,7 @@ out_not_moved:
 	ubi->wl_scheduled = 0;
 	spin_unlock(&ubi->wl_lock);
 
-	ubi_free_vid_hdr(ubi, vid_hdr);
+	ubi_free_vid_buf(vidb);
 	if (dst_leb_clean) {
 		ensure_wear_leveling(ubi, 1);
 	} else {
@@ -922,7 +941,14 @@ out_not_moved:
 			goto out_ro;
 	}
 
+	if (erase) {
+		err = do_sync_erase(ubi, e1, vol_id, lnum, 1);
+		if (err)
+			goto out_ro;
+	}
+
 	mutex_unlock(&ubi->move_mutex);
+	up_read(&ubi->fm_eba_sem);
 	return 0;
 
 out_error:
@@ -937,13 +963,14 @@ out_error:
 	ubi->move_to_put = ubi->wl_scheduled = 0;
 	spin_unlock(&ubi->wl_lock);
 
-	ubi_free_vid_hdr(ubi, vid_hdr);
+	ubi_free_vid_buf(vidb);
 	wl_entry_destroy(ubi, e1);
 	wl_entry_destroy(ubi, e2);
 
 out_ro:
 	ubi_ro_mode(ubi);
 	mutex_unlock(&ubi->move_mutex);
+	up_read(&ubi->fm_eba_sem);
 	ubi_assert(err != 0);
 	return err < 0 ? err : -EIO;
 
@@ -951,7 +978,8 @@ out_cancel:
 	ubi->wl_scheduled = 0;
 	spin_unlock(&ubi->wl_lock);
 	mutex_unlock(&ubi->move_mutex);
-	ubi_free_vid_hdr(ubi, vid_hdr);
+	up_read(&ubi->fm_eba_sem);
+	ubi_free_vid_buf(vidb);
 	return 0;
 }
 
@@ -1073,7 +1101,7 @@ static int __erase_worker(struct ubi_device *ubi, struct ubi_work *wl_wrk)
 		int err1;
 
 		/* Re-schedule the LEB for erasure */
-		err1 = schedule_erase(ubi, e, vol_id, lnum, 0);
+		err1 = schedule_erase(ubi, e, vol_id, lnum, 0, false);
 		if (err1) {
 			wl_entry_destroy(ubi, e);
 			err = err1;
@@ -1254,7 +1282,7 @@ retry:
 	}
 	spin_unlock(&ubi->wl_lock);
 
-	err = schedule_erase(ubi, e, vol_id, lnum, torture);
+	err = schedule_erase(ubi, e, vol_id, lnum, torture, false);
 	if (err) {
 		spin_lock(&ubi->wl_lock);
 		wl_tree_add(e, &ubi->used);
@@ -1545,7 +1573,7 @@ int ubi_wl_init(struct ubi_device *ubi, struct ubi_attach_info *ai)
 		e->pnum = aeb->pnum;
 		e->ec = aeb->ec;
 		ubi->lookuptbl[e->pnum] = e;
-		if (schedule_erase(ubi, e, aeb->vol_id, aeb->lnum, 0)) {
+		if (schedule_erase(ubi, e, aeb->vol_id, aeb->lnum, 0, false)) {
 			wl_entry_destroy(ubi, e);
 			goto out_free;
 		}
@@ -1624,7 +1652,7 @@ int ubi_wl_init(struct ubi_device *ubi, struct ubi_attach_info *ai)
 			e->ec = aeb->ec;
 			ubi_assert(!ubi->lookuptbl[e->pnum]);
 			ubi->lookuptbl[e->pnum] = e;
-			if (schedule_erase(ubi, e, aeb->vol_id, aeb->lnum, 0)) {
+			if (schedule_erase(ubi, e, aeb->vol_id, aeb->lnum, 0, false)) {
 				wl_entry_destroy(ubi, e);
 				goto out_free;
 			}
diff --git a/drivers/net/Kconfig b/drivers/net/Kconfig
index 0c5415b05ea9..95c32f2d7601 100644
--- a/drivers/net/Kconfig
+++ b/drivers/net/Kconfig
@@ -149,6 +149,8 @@ config IPVLAN
     tristate "IP-VLAN support"
     depends on INET
     depends on IPV6
+    depends on NETFILTER
+    depends on NET_L3_MASTER_DEV
     ---help---
       This allows one to create virtual devices off of a main interface
       and packets will be delivered based on the dest L3 (IPv6/IPv4 addr)
diff --git a/drivers/net/bonding/bond_main.c b/drivers/net/bonding/bond_main.c
index 9599ed6f1213..5fa36ebc0640 100644
--- a/drivers/net/bonding/bond_main.c
+++ b/drivers/net/bonding/bond_main.c
@@ -471,9 +471,9 @@ static int bond_check_dev_link(struct bonding *bond,
 		/* Yes, the mii is overlaid on the ifreq.ifr_ifru */
 		strncpy(ifr.ifr_name, slave_dev->name, IFNAMSIZ);
 		mii = if_mii(&ifr);
-		if (IOCTL(slave_dev, &ifr, SIOCGMIIPHY) == 0) {
+		if (ioctl(slave_dev, &ifr, SIOCGMIIPHY) == 0) {
 			mii->reg_num = MII_BMSR;
-			if (IOCTL(slave_dev, &ifr, SIOCGMIIREG) == 0)
+			if (ioctl(slave_dev, &ifr, SIOCGMIIREG) == 0)
 				return mii->val_out & BMSR_LSTATUS;
 		}
 	}
@@ -4628,7 +4628,7 @@ static int bond_init(struct net_device *bond_dev)
 
 	netdev_dbg(bond_dev, "Begin bond_init\n");
 
-	bond->wq = create_singlethread_workqueue(bond_dev->name);
+	bond->wq = alloc_ordered_workqueue(bond_dev->name, WQ_MEM_RECLAIM);
 	if (!bond->wq)
 		return -ENOMEM;
 
diff --git a/drivers/net/can/dev.c b/drivers/net/can/dev.c
index e21f7cc5ae4d..8d6208c0b400 100644
--- a/drivers/net/can/dev.c
+++ b/drivers/net/can/dev.c
@@ -21,6 +21,7 @@
 #include <linux/slab.h>
 #include <linux/netdevice.h>
 #include <linux/if_arp.h>
+#include <linux/workqueue.h>
 #include <linux/can.h>
 #include <linux/can/dev.h>
 #include <linux/can/skb.h>
@@ -501,9 +502,8 @@ EXPORT_SYMBOL_GPL(can_free_echo_skb);
 /*
  * CAN device restart for bus-off recovery
  */
-static void can_restart(unsigned long data)
+static void can_restart(struct net_device *dev)
 {
-	struct net_device *dev = (struct net_device *)data;
 	struct can_priv *priv = netdev_priv(dev);
 	struct net_device_stats *stats = &dev->stats;
 	struct sk_buff *skb;
@@ -543,6 +543,14 @@ restart:
 		netdev_err(dev, "Error %d during restart", err);
 }
 
+static void can_restart_work(struct work_struct *work)
+{
+	struct delayed_work *dwork = to_delayed_work(work);
+	struct can_priv *priv = container_of(dwork, struct can_priv, restart_work);
+
+	can_restart(priv->dev);
+}
+
 int can_restart_now(struct net_device *dev)
 {
 	struct can_priv *priv = netdev_priv(dev);
@@ -556,8 +564,8 @@ int can_restart_now(struct net_device *dev)
 	if (priv->state != CAN_STATE_BUS_OFF)
 		return -EBUSY;
 
-	/* Runs as soon as possible in the timer context */
-	mod_timer(&priv->restart_timer, jiffies);
+	cancel_delayed_work_sync(&priv->restart_work);
+	can_restart(dev);
 
 	return 0;
 }
@@ -578,8 +586,8 @@ void can_bus_off(struct net_device *dev)
 	netif_carrier_off(dev);
 
 	if (priv->restart_ms)
-		mod_timer(&priv->restart_timer,
-			  jiffies + (priv->restart_ms * HZ) / 1000);
+		schedule_delayed_work(&priv->restart_work,
+				      msecs_to_jiffies(priv->restart_ms));
 }
 EXPORT_SYMBOL_GPL(can_bus_off);
 
@@ -688,6 +696,7 @@ struct net_device *alloc_candev(int sizeof_priv, unsigned int echo_skb_max)
 		return NULL;
 
 	priv = netdev_priv(dev);
+	priv->dev = dev;
 
 	if (echo_skb_max) {
 		priv->echo_skb_max = echo_skb_max;
@@ -697,7 +706,7 @@ struct net_device *alloc_candev(int sizeof_priv, unsigned int echo_skb_max)
 
 	priv->state = CAN_STATE_STOPPED;
 
-	init_timer(&priv->restart_timer);
+	INIT_DELAYED_WORK(&priv->restart_work, can_restart_work);
 
 	return dev;
 }
@@ -778,8 +787,6 @@ int open_candev(struct net_device *dev)
 	if (!netif_carrier_ok(dev))
 		netif_carrier_on(dev);
 
-	setup_timer(&priv->restart_timer, can_restart, (unsigned long)dev);
-
 	return 0;
 }
 EXPORT_SYMBOL_GPL(open_candev);
@@ -794,7 +801,7 @@ void close_candev(struct net_device *dev)
 {
 	struct can_priv *priv = netdev_priv(dev);
 
-	del_timer_sync(&priv->restart_timer);
+	cancel_delayed_work_sync(&priv->restart_work);
 	can_flush_echo_skb(dev);
 }
 EXPORT_SYMBOL_GPL(close_candev);
diff --git a/drivers/net/can/usb/ems_usb.c b/drivers/net/can/usb/ems_usb.c
index 71f0e791355b..b3d02759c226 100644
--- a/drivers/net/can/usb/ems_usb.c
+++ b/drivers/net/can/usb/ems_usb.c
@@ -600,7 +600,6 @@ static int ems_usb_start(struct ems_usb *dev)
 		/* create a URB, and a buffer for it */
 		urb = usb_alloc_urb(0, GFP_KERNEL);
 		if (!urb) {
-			netdev_err(netdev, "No memory left for URBs\n");
 			err = -ENOMEM;
 			break;
 		}
@@ -752,10 +751,8 @@ static netdev_tx_t ems_usb_start_xmit(struct sk_buff *skb, struct net_device *ne
 
 	/* create a URB, and a buffer for it, and copy the data to the URB */
 	urb = usb_alloc_urb(0, GFP_ATOMIC);
-	if (!urb) {
-		netdev_err(netdev, "No memory left for URBs\n");
+	if (!urb)
 		goto nomem;
-	}
 
 	buf = usb_alloc_coherent(dev->udev, size, GFP_ATOMIC, &urb->transfer_dma);
 	if (!buf) {
@@ -1007,10 +1004,8 @@ static int ems_usb_probe(struct usb_interface *intf,
 		dev->tx_contexts[i].echo_index = MAX_TX_URBS;
 
 	dev->intr_urb = usb_alloc_urb(0, GFP_KERNEL);
-	if (!dev->intr_urb) {
-		dev_err(&intf->dev, "Couldn't alloc intr URB\n");
+	if (!dev->intr_urb)
 		goto cleanup_candev;
-	}
 
 	dev->intr_in_buffer = kzalloc(INTR_IN_BUFFER_SIZE, GFP_KERNEL);
 	if (!dev->intr_in_buffer)
diff --git a/drivers/net/can/usb/esd_usb2.c b/drivers/net/can/usb/esd_usb2.c
index 784a9002fbb9..be928ce62d32 100644
--- a/drivers/net/can/usb/esd_usb2.c
+++ b/drivers/net/can/usb/esd_usb2.c
@@ -558,8 +558,6 @@ static int esd_usb2_setup_rx_urbs(struct esd_usb2 *dev)
 		/* create a URB, and a buffer for it */
 		urb = usb_alloc_urb(0, GFP_KERNEL);
 		if (!urb) {
-			dev_warn(dev->udev->dev.parent,
-				 "No memory left for URBs\n");
 			err = -ENOMEM;
 			break;
 		}
@@ -730,7 +728,6 @@ static netdev_tx_t esd_usb2_start_xmit(struct sk_buff *skb,
 	/* create a URB, and a buffer for it, and copy the data to the URB */
 	urb = usb_alloc_urb(0, GFP_ATOMIC);
 	if (!urb) {
-		netdev_err(netdev, "No memory left for URBs\n");
 		stats->tx_dropped++;
 		dev_kfree_skb(skb);
 		goto nourbmem;
diff --git a/drivers/net/can/usb/gs_usb.c b/drivers/net/can/usb/gs_usb.c
index 6f0cbc38782e..77e3cc06a30c 100644
--- a/drivers/net/can/usb/gs_usb.c
+++ b/drivers/net/can/usb/gs_usb.c
@@ -493,10 +493,8 @@ static netdev_tx_t gs_can_start_xmit(struct sk_buff *skb,
 
 	/* create a URB, and a buffer for it */
 	urb = usb_alloc_urb(0, GFP_ATOMIC);
-	if (!urb) {
-		netdev_err(netdev, "No memory left for URB\n");
+	if (!urb)
 		goto nomem_urb;
-	}
 
 	hf = usb_alloc_coherent(dev->udev, sizeof(*hf), GFP_ATOMIC,
 				&urb->transfer_dma);
@@ -600,11 +598,8 @@ static int gs_can_open(struct net_device *netdev)
 
 			/* alloc rx urb */
 			urb = usb_alloc_urb(0, GFP_KERNEL);
-			if (!urb) {
-				netdev_err(netdev,
-					   "No memory left for URB\n");
+			if (!urb)
 				return -ENOMEM;
-			}
 
 			/* alloc rx buffer */
 			buf = usb_alloc_coherent(dev->udev,
diff --git a/drivers/net/can/usb/kvaser_usb.c b/drivers/net/can/usb/kvaser_usb.c
index 6f1f3b675ff5..d51e0c401b48 100644
--- a/drivers/net/can/usb/kvaser_usb.c
+++ b/drivers/net/can/usb/kvaser_usb.c
@@ -787,10 +787,8 @@ static int kvaser_usb_simple_msg_async(struct kvaser_usb_net_priv *priv,
 	int err;
 
 	urb = usb_alloc_urb(0, GFP_ATOMIC);
-	if (!urb) {
-		netdev_err(netdev, "No memory left for URBs\n");
+	if (!urb)
 		return -ENOMEM;
-	}
 
 	buf = kmalloc(sizeof(struct kvaser_msg), GFP_ATOMIC);
 	if (!buf) {
@@ -1393,8 +1391,6 @@ static int kvaser_usb_setup_rx_urbs(struct kvaser_usb *dev)
 
 		urb = usb_alloc_urb(0, GFP_KERNEL);
 		if (!urb) {
-			dev_warn(dev->udev->dev.parent,
-				 "No memory left for URBs\n");
 			err = -ENOMEM;
 			break;
 		}
@@ -1670,7 +1666,6 @@ static netdev_tx_t kvaser_usb_start_xmit(struct sk_buff *skb,
 
 	urb = usb_alloc_urb(0, GFP_ATOMIC);
 	if (!urb) {
-		netdev_err(netdev, "No memory left for URBs\n");
 		stats->tx_dropped++;
 		dev_kfree_skb(skb);
 		return NETDEV_TX_OK;
diff --git a/drivers/net/can/usb/peak_usb/pcan_usb_core.c b/drivers/net/can/usb/peak_usb/pcan_usb_core.c
index bfb91d8fa460..c06382cdfdfe 100644
--- a/drivers/net/can/usb/peak_usb/pcan_usb_core.c
+++ b/drivers/net/can/usb/peak_usb/pcan_usb_core.c
@@ -399,7 +399,6 @@ static int peak_usb_start(struct peak_usb_device *dev)
 		/* create a URB, and a buffer for it, to receive usb messages */
 		urb = usb_alloc_urb(0, GFP_KERNEL);
 		if (!urb) {
-			netdev_err(netdev, "No memory left for URBs\n");
 			err = -ENOMEM;
 			break;
 		}
@@ -454,7 +453,6 @@ static int peak_usb_start(struct peak_usb_device *dev)
 		/* create a URB and a buffer for it, to transmit usb messages */
 		urb = usb_alloc_urb(0, GFP_KERNEL);
 		if (!urb) {
-			netdev_err(netdev, "No memory left for URBs\n");
 			err = -ENOMEM;
 			break;
 		}
@@ -651,10 +649,8 @@ static int peak_usb_restart(struct peak_usb_device *dev)
 
 	/* first allocate a urb to handle the asynchronous steps */
 	urb = usb_alloc_urb(0, GFP_ATOMIC);
-	if (!urb) {
-		netdev_err(dev->netdev, "no memory left for urb\n");
+	if (!urb)
 		return -ENOMEM;
-	}
 
 	/* also allocate enough space for the commands to send */
 	buf = kmalloc(PCAN_USB_MAX_CMD_LEN, GFP_ATOMIC);
diff --git a/drivers/net/can/usb/usb_8dev.c b/drivers/net/can/usb/usb_8dev.c
index a731720f1d13..108a30e15097 100644
--- a/drivers/net/can/usb/usb_8dev.c
+++ b/drivers/net/can/usb/usb_8dev.c
@@ -623,10 +623,8 @@ static netdev_tx_t usb_8dev_start_xmit(struct sk_buff *skb,
 
 	/* create a URB, and a buffer for it, and copy the data to the URB */
 	urb = usb_alloc_urb(0, GFP_ATOMIC);
-	if (!urb) {
-		netdev_err(netdev, "No memory left for URBs\n");
+	if (!urb)
 		goto nomem;
-	}
 
 	buf = usb_alloc_coherent(priv->udev, size, GFP_ATOMIC,
 				 &urb->transfer_dma);
@@ -748,7 +746,6 @@ static int usb_8dev_start(struct usb_8dev_priv *priv)
 		/* create a URB, and a buffer for it */
 		urb = usb_alloc_urb(0, GFP_KERNEL);
 		if (!urb) {
-			netdev_err(netdev, "No memory left for URBs\n");
 			err = -ENOMEM;
 			break;
 		}
diff --git a/drivers/net/dsa/Kconfig b/drivers/net/dsa/Kconfig
index 8f4544394f44..065984670ff1 100644
--- a/drivers/net/dsa/Kconfig
+++ b/drivers/net/dsa/Kconfig
@@ -16,6 +16,7 @@ config NET_DSA_BCM_SF2
 	select FIXED_PHY
 	select BCM7XXX_PHY
 	select MDIO_BCM_UNIMAC
+	select B53
 	---help---
 	  This enables support for the Broadcom Starfighter 2 Ethernet
 	  switch chips.
@@ -24,4 +25,13 @@ source "drivers/net/dsa/b53/Kconfig"
 
 source "drivers/net/dsa/mv88e6xxx/Kconfig"
 
+config NET_DSA_QCA8K
+	tristate "Qualcomm Atheros QCA8K Ethernet switch family support"
+	depends on NET_DSA
+	select NET_DSA_TAG_QCA
+	select REGMAP
+	---help---
+	  This enables support for the Qualcomm Atheros QCA8K Ethernet
+	  switch chips.
+
 endmenu
diff --git a/drivers/net/dsa/Makefile b/drivers/net/dsa/Makefile
index ca1e71b853a6..8346e4f9737a 100644
--- a/drivers/net/dsa/Makefile
+++ b/drivers/net/dsa/Makefile
@@ -1,5 +1,6 @@
 obj-$(CONFIG_NET_DSA_MV88E6060) += mv88e6060.o
 obj-$(CONFIG_NET_DSA_BCM_SF2)	+= bcm_sf2.o
+obj-$(CONFIG_NET_DSA_QCA8K)	+= qca8k.o
 
 obj-y				+= b53/
 obj-y				+= mv88e6xxx/
diff --git a/drivers/net/dsa/b53/b53_common.c b/drivers/net/dsa/b53/b53_common.c
index bda37d336736..7717b19dc806 100644
--- a/drivers/net/dsa/b53/b53_common.c
+++ b/drivers/net/dsa/b53/b53_common.c
@@ -167,6 +167,65 @@ static const struct b53_mib_desc b53_mibs[] = {
 
 #define B53_MIBS_SIZE	ARRAY_SIZE(b53_mibs)
 
+static const struct b53_mib_desc b53_mibs_58xx[] = {
+	{ 8, 0x00, "TxOctets" },
+	{ 4, 0x08, "TxDropPkts" },
+	{ 4, 0x0c, "TxQPKTQ0" },
+	{ 4, 0x10, "TxBroadcastPkts" },
+	{ 4, 0x14, "TxMulticastPkts" },
+	{ 4, 0x18, "TxUnicastPKts" },
+	{ 4, 0x1c, "TxCollisions" },
+	{ 4, 0x20, "TxSingleCollision" },
+	{ 4, 0x24, "TxMultipleCollision" },
+	{ 4, 0x28, "TxDeferredCollision" },
+	{ 4, 0x2c, "TxLateCollision" },
+	{ 4, 0x30, "TxExcessiveCollision" },
+	{ 4, 0x34, "TxFrameInDisc" },
+	{ 4, 0x38, "TxPausePkts" },
+	{ 4, 0x3c, "TxQPKTQ1" },
+	{ 4, 0x40, "TxQPKTQ2" },
+	{ 4, 0x44, "TxQPKTQ3" },
+	{ 4, 0x48, "TxQPKTQ4" },
+	{ 4, 0x4c, "TxQPKTQ5" },
+	{ 8, 0x50, "RxOctets" },
+	{ 4, 0x58, "RxUndersizePkts" },
+	{ 4, 0x5c, "RxPausePkts" },
+	{ 4, 0x60, "RxPkts64Octets" },
+	{ 4, 0x64, "RxPkts65to127Octets" },
+	{ 4, 0x68, "RxPkts128to255Octets" },
+	{ 4, 0x6c, "RxPkts256to511Octets" },
+	{ 4, 0x70, "RxPkts512to1023Octets" },
+	{ 4, 0x74, "RxPkts1024toMaxPktsOctets" },
+	{ 4, 0x78, "RxOversizePkts" },
+	{ 4, 0x7c, "RxJabbers" },
+	{ 4, 0x80, "RxAlignmentErrors" },
+	{ 4, 0x84, "RxFCSErrors" },
+	{ 8, 0x88, "RxGoodOctets" },
+	{ 4, 0x90, "RxDropPkts" },
+	{ 4, 0x94, "RxUnicastPkts" },
+	{ 4, 0x98, "RxMulticastPkts" },
+	{ 4, 0x9c, "RxBroadcastPkts" },
+	{ 4, 0xa0, "RxSAChanges" },
+	{ 4, 0xa4, "RxFragments" },
+	{ 4, 0xa8, "RxJumboPkt" },
+	{ 4, 0xac, "RxSymblErr" },
+	{ 4, 0xb0, "InRangeErrCount" },
+	{ 4, 0xb4, "OutRangeErrCount" },
+	{ 4, 0xb8, "EEELpiEvent" },
+	{ 4, 0xbc, "EEELpiDuration" },
+	{ 4, 0xc0, "RxDiscard" },
+	{ 4, 0xc8, "TxQPKTQ6" },
+	{ 4, 0xcc, "TxQPKTQ7" },
+	{ 4, 0xd0, "TxPkts64Octets" },
+	{ 4, 0xd4, "TxPkts65to127Octets" },
+	{ 4, 0xd8, "TxPkts128to255Octets" },
+	{ 4, 0xdc, "TxPkts256to511Ocets" },
+	{ 4, 0xe0, "TxPkts512to1023Ocets" },
+	{ 4, 0xe4, "TxPkts1024toMaxPktOcets" },
+};
+
+#define B53_MIBS_58XX_SIZE	ARRAY_SIZE(b53_mibs_58xx)
+
 static int b53_do_vlan_op(struct b53_device *dev, u8 op)
 {
 	unsigned int i;
@@ -418,7 +477,7 @@ static int b53_fast_age_vlan(struct b53_device *dev, u16 vid)
 
 static void b53_imp_vlan_setup(struct dsa_switch *ds, int cpu_port)
 {
-	struct b53_device *dev = ds_to_priv(ds);
+	struct b53_device *dev = ds->priv;
 	unsigned int i;
 	u16 pvlan;
 
@@ -436,7 +495,7 @@ static void b53_imp_vlan_setup(struct dsa_switch *ds, int cpu_port)
 static int b53_enable_port(struct dsa_switch *ds, int port,
 			   struct phy_device *phy)
 {
-	struct b53_device *dev = ds_to_priv(ds);
+	struct b53_device *dev = ds->priv;
 	unsigned int cpu_port = dev->cpu_port;
 	u16 pvlan;
 
@@ -461,7 +520,7 @@ static int b53_enable_port(struct dsa_switch *ds, int port,
 static void b53_disable_port(struct dsa_switch *ds, int port,
 			     struct phy_device *phy)
 {
-	struct b53_device *dev = ds_to_priv(ds);
+	struct b53_device *dev = ds->priv;
 	u8 reg;
 
 	/* Disable Tx/Rx for the port */
@@ -570,7 +629,7 @@ static int b53_switch_reset(struct b53_device *dev)
 
 static int b53_phy_read16(struct dsa_switch *ds, int addr, int reg)
 {
-	struct b53_device *priv = ds_to_priv(ds);
+	struct b53_device *priv = ds->priv;
 	u16 value = 0;
 	int ret;
 
@@ -585,7 +644,7 @@ static int b53_phy_read16(struct dsa_switch *ds, int addr, int reg)
 
 static int b53_phy_write16(struct dsa_switch *ds, int addr, int reg, u16 val)
 {
-	struct b53_device *priv = ds_to_priv(ds);
+	struct b53_device *priv = ds->priv;
 
 	if (priv->ops->phy_write16)
 		return priv->ops->phy_write16(priv, addr, reg, val);
@@ -635,6 +694,8 @@ static const struct b53_mib_desc *b53_get_mib(struct b53_device *dev)
 		return b53_mibs_65;
 	else if (is63xx(dev))
 		return b53_mibs_63xx;
+	else if (is58xx(dev))
+		return b53_mibs_58xx;
 	else
 		return b53_mibs;
 }
@@ -645,13 +706,15 @@ static unsigned int b53_get_mib_size(struct b53_device *dev)
 		return B53_MIBS_65_SIZE;
 	else if (is63xx(dev))
 		return B53_MIBS_63XX_SIZE;
+	else if (is58xx(dev))
+		return B53_MIBS_58XX_SIZE;
 	else
 		return B53_MIBS_SIZE;
 }
 
 static void b53_get_strings(struct dsa_switch *ds, int port, uint8_t *data)
 {
-	struct b53_device *dev = ds_to_priv(ds);
+	struct b53_device *dev = ds->priv;
 	const struct b53_mib_desc *mibs = b53_get_mib(dev);
 	unsigned int mib_size = b53_get_mib_size(dev);
 	unsigned int i;
@@ -664,7 +727,7 @@ static void b53_get_strings(struct dsa_switch *ds, int port, uint8_t *data)
 static void b53_get_ethtool_stats(struct dsa_switch *ds, int port,
 				  uint64_t *data)
 {
-	struct b53_device *dev = ds_to_priv(ds);
+	struct b53_device *dev = ds->priv;
 	const struct b53_mib_desc *mibs = b53_get_mib(dev);
 	unsigned int mib_size = b53_get_mib_size(dev);
 	const struct b53_mib_desc *s;
@@ -696,19 +759,14 @@ static void b53_get_ethtool_stats(struct dsa_switch *ds, int port,
 
 static int b53_get_sset_count(struct dsa_switch *ds)
 {
-	struct b53_device *dev = ds_to_priv(ds);
+	struct b53_device *dev = ds->priv;
 
 	return b53_get_mib_size(dev);
 }
 
-static int b53_set_addr(struct dsa_switch *ds, u8 *addr)
-{
-	return 0;
-}
-
 static int b53_setup(struct dsa_switch *ds)
 {
-	struct b53_device *dev = ds_to_priv(ds);
+	struct b53_device *dev = ds->priv;
 	unsigned int port;
 	int ret;
 
@@ -739,7 +797,7 @@ static int b53_setup(struct dsa_switch *ds)
 static void b53_adjust_link(struct dsa_switch *ds, int port,
 			    struct phy_device *phydev)
 {
-	struct b53_device *dev = ds_to_priv(ds);
+	struct b53_device *dev = ds->priv;
 	u8 rgmii_ctrl = 0, reg = 0, off;
 
 	if (!phy_is_pseudo_fixed_link(phydev))
@@ -873,7 +931,7 @@ static int b53_vlan_prepare(struct dsa_switch *ds, int port,
 			    const struct switchdev_obj_port_vlan *vlan,
 			    struct switchdev_trans *trans)
 {
-	struct b53_device *dev = ds_to_priv(ds);
+	struct b53_device *dev = ds->priv;
 
 	if ((is5325(dev) || is5365(dev)) && vlan->vid_begin == 0)
 		return -EOPNOTSUPP;
@@ -890,7 +948,7 @@ static void b53_vlan_add(struct dsa_switch *ds, int port,
 			 const struct switchdev_obj_port_vlan *vlan,
 			 struct switchdev_trans *trans)
 {
-	struct b53_device *dev = ds_to_priv(ds);
+	struct b53_device *dev = ds->priv;
 	bool untagged = vlan->flags & BRIDGE_VLAN_INFO_UNTAGGED;
 	bool pvid = vlan->flags & BRIDGE_VLAN_INFO_PVID;
 	unsigned int cpu_port = dev->cpu_port;
@@ -924,7 +982,7 @@ static void b53_vlan_add(struct dsa_switch *ds, int port,
 static int b53_vlan_del(struct dsa_switch *ds, int port,
 			const struct switchdev_obj_port_vlan *vlan)
 {
-	struct b53_device *dev = ds_to_priv(ds);
+	struct b53_device *dev = ds->priv;
 	bool untagged = vlan->flags & BRIDGE_VLAN_INFO_UNTAGGED;
 	unsigned int cpu_port = dev->cpu_port;
 	struct b53_vlan *vl;
@@ -970,7 +1028,7 @@ static int b53_vlan_dump(struct dsa_switch *ds, int port,
 			 struct switchdev_obj_port_vlan *vlan,
 			 int (*cb)(struct switchdev_obj *obj))
 {
-	struct b53_device *dev = ds_to_priv(ds);
+	struct b53_device *dev = ds->priv;
 	u16 vid, vid_start = 0, pvid;
 	struct b53_vlan *vl;
 	int err = 0;
@@ -1129,7 +1187,7 @@ static int b53_fdb_prepare(struct dsa_switch *ds, int port,
 			   const struct switchdev_obj_port_fdb *fdb,
 			   struct switchdev_trans *trans)
 {
-	struct b53_device *priv = ds_to_priv(ds);
+	struct b53_device *priv = ds->priv;
 
 	/* 5325 and 5365 require some more massaging, but could
 	 * be supported eventually
@@ -1144,7 +1202,7 @@ static void b53_fdb_add(struct dsa_switch *ds, int port,
 			const struct switchdev_obj_port_fdb *fdb,
 			struct switchdev_trans *trans)
 {
-	struct b53_device *priv = ds_to_priv(ds);
+	struct b53_device *priv = ds->priv;
 
 	if (b53_arl_op(priv, 0, port, fdb->addr, fdb->vid, true))
 		pr_err("%s: failed to add MAC address\n", __func__);
@@ -1153,7 +1211,7 @@ static void b53_fdb_add(struct dsa_switch *ds, int port,
 static int b53_fdb_del(struct dsa_switch *ds, int port,
 		       const struct switchdev_obj_port_fdb *fdb)
 {
-	struct b53_device *priv = ds_to_priv(ds);
+	struct b53_device *priv = ds->priv;
 
 	return b53_arl_op(priv, 0, port, fdb->addr, fdb->vid, false);
 }
@@ -1212,7 +1270,7 @@ static int b53_fdb_dump(struct dsa_switch *ds, int port,
 			struct switchdev_obj_port_fdb *fdb,
 			int (*cb)(struct switchdev_obj *obj))
 {
-	struct b53_device *priv = ds_to_priv(ds);
+	struct b53_device *priv = ds->priv;
 	struct net_device *dev = ds->ports[port].netdev;
 	struct b53_arl_entry results[2];
 	unsigned int count = 0;
@@ -1251,10 +1309,22 @@ static int b53_fdb_dump(struct dsa_switch *ds, int port,
 static int b53_br_join(struct dsa_switch *ds, int port,
 		       struct net_device *bridge)
 {
-	struct b53_device *dev = ds_to_priv(ds);
+	struct b53_device *dev = ds->priv;
+	s8 cpu_port = ds->dst->cpu_port;
 	u16 pvlan, reg;
 	unsigned int i;
 
+	/* Make this port leave the all VLANs join since we will have proper
+	 * VLAN entries from now on
+	 */
+	if (is58xx(dev)) {
+		b53_read16(dev, B53_VLAN_PAGE, B53_JOIN_ALL_VLAN_EN, &reg);
+		reg &= ~BIT(port);
+		if ((reg & BIT(cpu_port)) == BIT(cpu_port))
+			reg &= ~BIT(cpu_port);
+		b53_write16(dev, B53_VLAN_PAGE, B53_JOIN_ALL_VLAN_EN, reg);
+	}
+
 	dev->ports[port].bridge_dev = bridge;
 	b53_read16(dev, B53_PVLAN_PAGE, B53_PVLAN_PORT_MASK(port), &pvlan);
 
@@ -1284,9 +1354,10 @@ static int b53_br_join(struct dsa_switch *ds, int port,
 
 static void b53_br_leave(struct dsa_switch *ds, int port)
 {
-	struct b53_device *dev = ds_to_priv(ds);
+	struct b53_device *dev = ds->priv;
 	struct net_device *bridge = dev->ports[port].bridge_dev;
 	struct b53_vlan *vl = &dev->vlans[0];
+	s8 cpu_port = ds->dst->cpu_port;
 	unsigned int i;
 	u16 pvlan, reg, pvid;
 
@@ -1316,22 +1387,27 @@ static void b53_br_leave(struct dsa_switch *ds, int port)
 	else
 		pvid = 0;
 
-	b53_get_vlan_entry(dev, pvid, vl);
-	vl->members |= BIT(port) | BIT(dev->cpu_port);
-	vl->untag |= BIT(port) | BIT(dev->cpu_port);
-	b53_set_vlan_entry(dev, pvid, vl);
+	/* Make this port join all VLANs without VLAN entries */
+	if (is58xx(dev)) {
+		b53_read16(dev, B53_VLAN_PAGE, B53_JOIN_ALL_VLAN_EN, &reg);
+		reg |= BIT(port);
+		if (!(reg & BIT(cpu_port)))
+			reg |= BIT(cpu_port);
+		b53_write16(dev, B53_VLAN_PAGE, B53_JOIN_ALL_VLAN_EN, reg);
+	} else {
+		b53_get_vlan_entry(dev, pvid, vl);
+		vl->members |= BIT(port) | BIT(dev->cpu_port);
+		vl->untag |= BIT(port) | BIT(dev->cpu_port);
+		b53_set_vlan_entry(dev, pvid, vl);
+	}
 }
 
-static void b53_br_set_stp_state(struct dsa_switch *ds, int port,
-				 u8 state)
+static void b53_br_set_stp_state(struct dsa_switch *ds, int port, u8 state)
 {
-	struct b53_device *dev = ds_to_priv(ds);
-	u8 hw_state, cur_hw_state;
+	struct b53_device *dev = ds->priv;
+	u8 hw_state;
 	u8 reg;
 
-	b53_read8(dev, B53_CTRL_PAGE, B53_PORT_CTRL(port), &reg);
-	cur_hw_state = reg & PORT_CTRL_STP_STATE_MASK;
-
 	switch (state) {
 	case BR_STATE_DISABLED:
 		hw_state = PORT_CTRL_DIS_STATE;
@@ -1353,30 +1429,28 @@ static void b53_br_set_stp_state(struct dsa_switch *ds, int port,
 		return;
 	}
 
-	/* Fast-age ARL entries if we are moving a port from Learning or
-	 * Forwarding (cur_hw_state) state to Disabled, Blocking or Listening
-	 * state (hw_state)
-	 */
-	if (cur_hw_state != hw_state) {
-		if (cur_hw_state >= PORT_CTRL_LEARN_STATE &&
-		    hw_state <= PORT_CTRL_LISTEN_STATE) {
-			if (b53_fast_age_port(dev, port)) {
-				dev_err(ds->dev, "fast ageing failed\n");
-				return;
-			}
-		}
-	}
-
 	b53_read8(dev, B53_CTRL_PAGE, B53_PORT_CTRL(port), &reg);
 	reg &= ~PORT_CTRL_STP_STATE_MASK;
 	reg |= hw_state;
 	b53_write8(dev, B53_CTRL_PAGE, B53_PORT_CTRL(port), reg);
 }
 
-static struct dsa_switch_driver b53_switch_ops = {
-	.tag_protocol		= DSA_TAG_PROTO_NONE,
+static void b53_br_fast_age(struct dsa_switch *ds, int port)
+{
+	struct b53_device *dev = ds->priv;
+
+	if (b53_fast_age_port(dev, port))
+		dev_err(ds->dev, "fast ageing failed\n");
+}
+
+static enum dsa_tag_protocol b53_get_tag_protocol(struct dsa_switch *ds)
+{
+	return DSA_TAG_PROTO_NONE;
+}
+
+static struct dsa_switch_ops b53_switch_ops = {
+	.get_tag_protocol	= b53_get_tag_protocol,
 	.setup			= b53_setup,
-	.set_addr		= b53_set_addr,
 	.get_strings		= b53_get_strings,
 	.get_ethtool_stats	= b53_get_ethtool_stats,
 	.get_sset_count		= b53_get_sset_count,
@@ -1388,6 +1462,7 @@ static struct dsa_switch_driver b53_switch_ops = {
 	.port_bridge_join	= b53_br_join,
 	.port_bridge_leave	= b53_br_leave,
 	.port_stp_state_set	= b53_br_set_stp_state,
+	.port_fast_age		= b53_br_fast_age,
 	.port_vlan_filtering	= b53_vlan_filtering,
 	.port_vlan_prepare	= b53_vlan_prepare,
 	.port_vlan_add		= b53_vlan_add,
@@ -1593,11 +1668,22 @@ static const struct b53_chip_data b53_switch_chips[] = {
 		.jumbo_pm_reg = B53_JUMBO_PORT_MASK,
 		.jumbo_size_reg = B53_JUMBO_MAX_SIZE,
 	},
+	{
+		.chip_id = BCM7445_DEVICE_ID,
+		.dev_name = "BCM7445",
+		.vlans	= 4096,
+		.enabled_ports = 0x1ff,
+		.arl_entries = 4,
+		.cpu_port = B53_CPU_PORT,
+		.vta_regs = B53_VTA_REGS,
+		.duplex_reg = B53_DUPLEX_STAT_GE,
+		.jumbo_pm_reg = B53_JUMBO_PORT_MASK,
+		.jumbo_size_reg = B53_JUMBO_MAX_SIZE,
+	},
 };
 
 static int b53_switch_init(struct b53_device *dev)
 {
-	struct dsa_switch *ds = dev->ds;
 	unsigned int i;
 	int ret;
 
@@ -1613,7 +1699,6 @@ static int b53_switch_init(struct b53_device *dev)
 			dev->vta_regs[1] = chip->vta_regs[1];
 			dev->vta_regs[2] = chip->vta_regs[2];
 			dev->jumbo_pm_reg = chip->jumbo_pm_reg;
-			ds->drv = &b53_switch_ops;
 			dev->cpu_port = chip->cpu_port;
 			dev->num_vlans = chip->vlans;
 			dev->num_arl_entries = chip->arl_entries;
@@ -1681,7 +1766,8 @@ static int b53_switch_init(struct b53_device *dev)
 	return 0;
 }
 
-struct b53_device *b53_switch_alloc(struct device *base, struct b53_io_ops *ops,
+struct b53_device *b53_switch_alloc(struct device *base,
+				    const struct b53_io_ops *ops,
 				    void *priv)
 {
 	struct dsa_switch *ds;
@@ -1700,6 +1786,7 @@ struct b53_device *b53_switch_alloc(struct device *base, struct b53_io_ops *ops,
 	dev->ds = ds;
 	dev->priv = priv;
 	dev->ops = ops;
+	ds->ops = &b53_switch_ops;
 	mutex_init(&dev->reg_mutex);
 	mutex_init(&dev->stats_mutex);
 
diff --git a/drivers/net/dsa/b53/b53_mdio.c b/drivers/net/dsa/b53/b53_mdio.c
index aa87c3fffdac..477a16b5660a 100644
--- a/drivers/net/dsa/b53/b53_mdio.c
+++ b/drivers/net/dsa/b53/b53_mdio.c
@@ -267,7 +267,7 @@ static int b53_mdio_phy_write16(struct b53_device *dev, int addr, int reg,
 	return mdiobus_write_nested(bus, addr, reg, value);
 }
 
-static struct b53_io_ops b53_mdio_ops = {
+static const struct b53_io_ops b53_mdio_ops = {
 	.read8 = b53_mdio_read8,
 	.read16 = b53_mdio_read16,
 	.read32 = b53_mdio_read32,
diff --git a/drivers/net/dsa/b53/b53_mmap.c b/drivers/net/dsa/b53/b53_mmap.c
index 77ffc4312808..76fb8552c9d9 100644
--- a/drivers/net/dsa/b53/b53_mmap.c
+++ b/drivers/net/dsa/b53/b53_mmap.c
@@ -17,7 +17,6 @@
  */
 
 #include <linux/kernel.h>
-#include <linux/kconfig.h>
 #include <linux/module.h>
 #include <linux/io.h>
 #include <linux/platform_device.h>
@@ -208,7 +207,7 @@ static int b53_mmap_write64(struct b53_device *dev, u8 page, u8 reg,
 	return 0;
 }
 
-static struct b53_io_ops b53_mmap_ops = {
+static const struct b53_io_ops b53_mmap_ops = {
 	.read8 = b53_mmap_read8,
 	.read16 = b53_mmap_read16,
 	.read32 = b53_mmap_read32,
diff --git a/drivers/net/dsa/b53/b53_priv.h b/drivers/net/dsa/b53/b53_priv.h
index 835a744f206e..f192a673caba 100644
--- a/drivers/net/dsa/b53/b53_priv.h
+++ b/drivers/net/dsa/b53/b53_priv.h
@@ -60,6 +60,7 @@ enum {
 	BCM53018_DEVICE_ID = 0x53018,
 	BCM53019_DEVICE_ID = 0x53019,
 	BCM58XX_DEVICE_ID = 0x5800,
+	BCM7445_DEVICE_ID = 0x7445,
 };
 
 #define B53_N_PORTS	9
@@ -174,6 +175,12 @@ static inline int is5301x(struct b53_device *dev)
 		dev->chip_id == BCM53019_DEVICE_ID;
 }
 
+static inline int is58xx(struct b53_device *dev)
+{
+	return dev->chip_id == BCM58XX_DEVICE_ID ||
+		dev->chip_id == BCM7445_DEVICE_ID;
+}
+
 #define B53_CPU_PORT_25	5
 #define B53_CPU_PORT	8
 
@@ -182,7 +189,8 @@ static inline int is_cpu_port(struct b53_device *dev, int port)
 	return dev->cpu_port;
 }
 
-struct b53_device *b53_switch_alloc(struct device *base, struct b53_io_ops *ops,
+struct b53_device *b53_switch_alloc(struct device *base,
+				    const struct b53_io_ops *ops,
 				    void *priv);
 
 int b53_switch_detect(struct b53_device *dev);
@@ -364,7 +372,6 @@ static inline void b53_arl_from_entry(u64 *mac_vid, u32 *fwd_entry,
 
 #ifdef CONFIG_BCM47XX
 
-#include <linux/version.h>
 #include <linux/bcm47xx_nvram.h>
 #include <bcm47xx_board.h>
 static inline int b53_switch_get_reset_gpio(struct b53_device *dev)
diff --git a/drivers/net/dsa/b53/b53_regs.h b/drivers/net/dsa/b53/b53_regs.h
index a0b453ea34c9..dac0af4e2cd0 100644
--- a/drivers/net/dsa/b53/b53_regs.h
+++ b/drivers/net/dsa/b53/b53_regs.h
@@ -309,6 +309,9 @@
 /* Port VLAN mask (16 bit) IMP port is always 8, also on 5325 & co */
 #define B53_PVLAN_PORT_MASK(i)		((i) * 2)
 
+/* Join all VLANs register (16 bit) */
+#define B53_JOIN_ALL_VLAN_EN		0x50
+
 /*************************************************************************
  * 802.1Q Page Registers
  *************************************************************************/
diff --git a/drivers/net/dsa/b53/b53_spi.c b/drivers/net/dsa/b53/b53_spi.c
index 2bda0b5f1578..f89f5308a99b 100644
--- a/drivers/net/dsa/b53/b53_spi.c
+++ b/drivers/net/dsa/b53/b53_spi.c
@@ -270,7 +270,7 @@ static int b53_spi_write64(struct b53_device *dev, u8 page, u8 reg, u64 value)
 	return spi_write(spi, txbuf, sizeof(txbuf));
 }
 
-static struct b53_io_ops b53_spi_ops = {
+static const struct b53_io_ops b53_spi_ops = {
 	.read8 = b53_spi_read8,
 	.read16 = b53_spi_read16,
 	.read32 = b53_spi_read32,
@@ -317,8 +317,6 @@ static int b53_spi_remove(struct spi_device *spi)
 static struct spi_driver b53_spi_driver = {
 	.driver = {
 		.name	= "b53-switch",
-		.bus	= &spi_bus_type,
-		.owner	= THIS_MODULE,
 	},
 	.probe	= b53_spi_probe,
 	.remove	= b53_spi_remove,
diff --git a/drivers/net/dsa/b53/b53_srab.c b/drivers/net/dsa/b53/b53_srab.c
index 3e2d4a5fcd5a..8a62b6a69703 100644
--- a/drivers/net/dsa/b53/b53_srab.c
+++ b/drivers/net/dsa/b53/b53_srab.c
@@ -344,7 +344,7 @@ err:
 	return ret;
 }
 
-static struct b53_io_ops b53_srab_ops = {
+static const struct b53_io_ops b53_srab_ops = {
 	.read8 = b53_srab_read8,
 	.read16 = b53_srab_read16,
 	.read32 = b53_srab_read32,
diff --git a/drivers/net/dsa/bcm_sf2.c b/drivers/net/dsa/bcm_sf2.c
index b2b838724a9b..e218887f18b7 100644
--- a/drivers/net/dsa/bcm_sf2.c
+++ b/drivers/net/dsa/bcm_sf2.c
@@ -29,130 +29,21 @@
 #include <linux/brcmphy.h>
 #include <linux/etherdevice.h>
 #include <net/switchdev.h>
+#include <linux/platform_data/b53.h>
 
 #include "bcm_sf2.h"
 #include "bcm_sf2_regs.h"
+#include "b53/b53_priv.h"
+#include "b53/b53_regs.h"
 
-/* String, offset, and register size in bytes if different from 4 bytes */
-static const struct bcm_sf2_hw_stats bcm_sf2_mib[] = {
-	{ "TxOctets",		0x000, 8	},
-	{ "TxDropPkts",		0x020		},
-	{ "TxQPKTQ0",		0x030		},
-	{ "TxBroadcastPkts",	0x040		},
-	{ "TxMulticastPkts",	0x050		},
-	{ "TxUnicastPKts",	0x060		},
-	{ "TxCollisions",	0x070		},
-	{ "TxSingleCollision",	0x080		},
-	{ "TxMultipleCollision", 0x090		},
-	{ "TxDeferredCollision", 0x0a0		},
-	{ "TxLateCollision",	0x0b0		},
-	{ "TxExcessiveCollision", 0x0c0		},
-	{ "TxFrameInDisc",	0x0d0		},
-	{ "TxPausePkts",	0x0e0		},
-	{ "TxQPKTQ1",		0x0f0		},
-	{ "TxQPKTQ2",		0x100		},
-	{ "TxQPKTQ3",		0x110		},
-	{ "TxQPKTQ4",		0x120		},
-	{ "TxQPKTQ5",		0x130		},
-	{ "RxOctets",		0x140, 8	},
-	{ "RxUndersizePkts",	0x160		},
-	{ "RxPausePkts",	0x170		},
-	{ "RxPkts64Octets",	0x180		},
-	{ "RxPkts65to127Octets", 0x190		},
-	{ "RxPkts128to255Octets", 0x1a0		},
-	{ "RxPkts256to511Octets", 0x1b0		},
-	{ "RxPkts512to1023Octets", 0x1c0	},
-	{ "RxPkts1024toMaxPktsOctets", 0x1d0	},
-	{ "RxOversizePkts",	0x1e0		},
-	{ "RxJabbers",		0x1f0		},
-	{ "RxAlignmentErrors",	0x200		},
-	{ "RxFCSErrors",	0x210		},
-	{ "RxGoodOctets",	0x220, 8	},
-	{ "RxDropPkts",		0x240		},
-	{ "RxUnicastPkts",	0x250		},
-	{ "RxMulticastPkts",	0x260		},
-	{ "RxBroadcastPkts",	0x270		},
-	{ "RxSAChanges",	0x280		},
-	{ "RxFragments",	0x290		},
-	{ "RxJumboPkt",		0x2a0		},
-	{ "RxSymblErr",		0x2b0		},
-	{ "InRangeErrCount",	0x2c0		},
-	{ "OutRangeErrCount",	0x2d0		},
-	{ "EEELpiEvent",	0x2e0		},
-	{ "EEELpiDuration",	0x2f0		},
-	{ "RxDiscard",		0x300, 8	},
-	{ "TxQPKTQ6",		0x320		},
-	{ "TxQPKTQ7",		0x330		},
-	{ "TxPkts64Octets",	0x340		},
-	{ "TxPkts65to127Octets", 0x350		},
-	{ "TxPkts128to255Octets", 0x360		},
-	{ "TxPkts256to511Ocets", 0x370		},
-	{ "TxPkts512to1023Ocets", 0x380		},
-	{ "TxPkts1024toMaxPktOcets", 0x390	},
-};
-
-#define BCM_SF2_STATS_SIZE	ARRAY_SIZE(bcm_sf2_mib)
-
-static void bcm_sf2_sw_get_strings(struct dsa_switch *ds,
-				   int port, uint8_t *data)
-{
-	unsigned int i;
-
-	for (i = 0; i < BCM_SF2_STATS_SIZE; i++)
-		memcpy(data + i * ETH_GSTRING_LEN,
-		       bcm_sf2_mib[i].string, ETH_GSTRING_LEN);
-}
-
-static void bcm_sf2_sw_get_ethtool_stats(struct dsa_switch *ds,
-					 int port, uint64_t *data)
-{
-	struct bcm_sf2_priv *priv = ds_to_priv(ds);
-	const struct bcm_sf2_hw_stats *s;
-	unsigned int i;
-	u64 val = 0;
-	u32 offset;
-
-	mutex_lock(&priv->stats_mutex);
-
-	/* Now fetch the per-port counters */
-	for (i = 0; i < BCM_SF2_STATS_SIZE; i++) {
-		s = &bcm_sf2_mib[i];
-
-		/* Do a latched 64-bit read if needed */
-		offset = s->reg + CORE_P_MIB_OFFSET(port);
-		if (s->sizeof_stat == 8)
-			val = core_readq(priv, offset);
-		else
-			val = core_readl(priv, offset);
-
-		data[i] = (u64)val;
-	}
-
-	mutex_unlock(&priv->stats_mutex);
-}
-
-static int bcm_sf2_sw_get_sset_count(struct dsa_switch *ds)
-{
-	return BCM_SF2_STATS_SIZE;
-}
-
-static const char *bcm_sf2_sw_drv_probe(struct device *dsa_dev,
-					struct device *host_dev, int sw_addr,
-					void **_priv)
+static enum dsa_tag_protocol bcm_sf2_sw_get_tag_protocol(struct dsa_switch *ds)
 {
-	struct bcm_sf2_priv *priv;
-
-	priv = devm_kzalloc(dsa_dev, sizeof(*priv), GFP_KERNEL);
-	if (!priv)
-		return NULL;
-	*_priv = priv;
-
-	return "Broadcom Starfighter 2";
+	return DSA_TAG_PROTO_BRCM;
 }
 
 static void bcm_sf2_imp_vlan_setup(struct dsa_switch *ds, int cpu_port)
 {
-	struct bcm_sf2_priv *priv = ds_to_priv(ds);
+	struct bcm_sf2_priv *priv = bcm_sf2_to_priv(ds);
 	unsigned int i;
 	u32 reg;
 
@@ -172,7 +63,7 @@ static void bcm_sf2_imp_vlan_setup(struct dsa_switch *ds, int cpu_port)
 
 static void bcm_sf2_imp_setup(struct dsa_switch *ds, int port)
 {
-	struct bcm_sf2_priv *priv = ds_to_priv(ds);
+	struct bcm_sf2_priv *priv = bcm_sf2_to_priv(ds);
 	u32 reg, val;
 
 	/* Enable the port memories */
@@ -237,7 +128,7 @@ static void bcm_sf2_imp_setup(struct dsa_switch *ds, int port)
 
 static void bcm_sf2_eee_enable_set(struct dsa_switch *ds, int port, bool enable)
 {
-	struct bcm_sf2_priv *priv = ds_to_priv(ds);
+	struct bcm_sf2_priv *priv = bcm_sf2_to_priv(ds);
 	u32 reg;
 
 	reg = core_readl(priv, CORE_EEE_EN_CTRL);
@@ -250,7 +141,7 @@ static void bcm_sf2_eee_enable_set(struct dsa_switch *ds, int port, bool enable)
 
 static void bcm_sf2_gphy_enable_set(struct dsa_switch *ds, bool enable)
 {
-	struct bcm_sf2_priv *priv = ds_to_priv(ds);
+	struct bcm_sf2_priv *priv = bcm_sf2_to_priv(ds);
 	u32 reg;
 
 	reg = reg_readl(priv, REG_SPHY_CNTRL);
@@ -324,7 +215,7 @@ static inline void bcm_sf2_port_intr_disable(struct bcm_sf2_priv *priv,
 static int bcm_sf2_port_setup(struct dsa_switch *ds, int port,
 			      struct phy_device *phy)
 {
-	struct bcm_sf2_priv *priv = ds_to_priv(ds);
+	struct bcm_sf2_priv *priv = bcm_sf2_to_priv(ds);
 	s8 cpu_port = ds->dst[ds->index].cpu_port;
 	u32 reg;
 
@@ -365,7 +256,7 @@ static int bcm_sf2_port_setup(struct dsa_switch *ds, int port,
 	reg = core_readl(priv, CORE_PORT_VLAN_CTL_PORT(port));
 	reg &= ~PORT_VLAN_CTRL_MASK;
 	reg |= (1 << port);
-	reg |= priv->port_sts[port].vlan_ctl_mask;
+	reg |= priv->dev->ports[port].vlan_ctl_mask;
 	core_writel(priv, reg, CORE_PORT_VLAN_CTL_PORT(port));
 
 	bcm_sf2_imp_vlan_setup(ds, cpu_port);
@@ -380,7 +271,7 @@ static int bcm_sf2_port_setup(struct dsa_switch *ds, int port,
 static void bcm_sf2_port_disable(struct dsa_switch *ds, int port,
 				 struct phy_device *phy)
 {
-	struct bcm_sf2_priv *priv = ds_to_priv(ds);
+	struct bcm_sf2_priv *priv = bcm_sf2_to_priv(ds);
 	u32 off, reg;
 
 	if (priv->wol_ports_mask & (1 << port))
@@ -412,7 +303,7 @@ static void bcm_sf2_port_disable(struct dsa_switch *ds, int port,
 static int bcm_sf2_eee_init(struct dsa_switch *ds, int port,
 			    struct phy_device *phy)
 {
-	struct bcm_sf2_priv *priv = ds_to_priv(ds);
+	struct bcm_sf2_priv *priv = bcm_sf2_to_priv(ds);
 	struct ethtool_eee *p = &priv->port_sts[port].eee;
 	int ret;
 
@@ -430,7 +321,7 @@ static int bcm_sf2_eee_init(struct dsa_switch *ds, int port,
 static int bcm_sf2_sw_get_eee(struct dsa_switch *ds, int port,
 			      struct ethtool_eee *e)
 {
-	struct bcm_sf2_priv *priv = ds_to_priv(ds);
+	struct bcm_sf2_priv *priv = bcm_sf2_to_priv(ds);
 	struct ethtool_eee *p = &priv->port_sts[port].eee;
 	u32 reg;
 
@@ -445,7 +336,7 @@ static int bcm_sf2_sw_set_eee(struct dsa_switch *ds, int port,
 			      struct phy_device *phydev,
 			      struct ethtool_eee *e)
 {
-	struct bcm_sf2_priv *priv = ds_to_priv(ds);
+	struct bcm_sf2_priv *priv = bcm_sf2_to_priv(ds);
 	struct ethtool_eee *p = &priv->port_sts[port].eee;
 
 	p->eee_enabled = e->eee_enabled;
@@ -461,469 +352,6 @@ static int bcm_sf2_sw_set_eee(struct dsa_switch *ds, int port,
 	return 0;
 }
 
-static int bcm_sf2_fast_age_op(struct bcm_sf2_priv *priv)
-{
-	unsigned int timeout = 1000;
-	u32 reg;
-
-	reg = core_readl(priv, CORE_FAST_AGE_CTRL);
-	reg |= EN_AGE_PORT | EN_AGE_VLAN | EN_AGE_DYNAMIC | FAST_AGE_STR_DONE;
-	core_writel(priv, reg, CORE_FAST_AGE_CTRL);
-
-	do {
-		reg = core_readl(priv, CORE_FAST_AGE_CTRL);
-		if (!(reg & FAST_AGE_STR_DONE))
-			break;
-
-		cpu_relax();
-	} while (timeout--);
-
-	if (!timeout)
-		return -ETIMEDOUT;
-
-	core_writel(priv, 0, CORE_FAST_AGE_CTRL);
-
-	return 0;
-}
-
-/* Fast-ageing of ARL entries for a given port, equivalent to an ARL
- * flush for that port.
- */
-static int bcm_sf2_sw_fast_age_port(struct dsa_switch *ds, int port)
-{
-	struct bcm_sf2_priv *priv = ds_to_priv(ds);
-
-	core_writel(priv, port, CORE_FAST_AGE_PORT);
-
-	return bcm_sf2_fast_age_op(priv);
-}
-
-static int bcm_sf2_sw_fast_age_vlan(struct bcm_sf2_priv *priv, u16 vid)
-{
-	core_writel(priv, vid, CORE_FAST_AGE_VID);
-
-	return bcm_sf2_fast_age_op(priv);
-}
-
-static int bcm_sf2_vlan_op_wait(struct bcm_sf2_priv *priv)
-{
-	unsigned int timeout = 10;
-	u32 reg;
-
-	do {
-		reg = core_readl(priv, CORE_ARLA_VTBL_RWCTRL);
-		if (!(reg & ARLA_VTBL_STDN))
-			return 0;
-
-		usleep_range(1000, 2000);
-	} while (timeout--);
-
-	return -ETIMEDOUT;
-}
-
-static int bcm_sf2_vlan_op(struct bcm_sf2_priv *priv, u8 op)
-{
-	core_writel(priv, ARLA_VTBL_STDN | op, CORE_ARLA_VTBL_RWCTRL);
-
-	return bcm_sf2_vlan_op_wait(priv);
-}
-
-static void bcm_sf2_set_vlan_entry(struct bcm_sf2_priv *priv, u16 vid,
-				   struct bcm_sf2_vlan *vlan)
-{
-	int ret;
-
-	core_writel(priv, vid & VTBL_ADDR_INDEX_MASK, CORE_ARLA_VTBL_ADDR);
-	core_writel(priv, vlan->untag << UNTAG_MAP_SHIFT | vlan->members,
-		    CORE_ARLA_VTBL_ENTRY);
-
-	ret = bcm_sf2_vlan_op(priv, ARLA_VTBL_CMD_WRITE);
-	if (ret)
-		pr_err("failed to write VLAN entry\n");
-}
-
-static int bcm_sf2_get_vlan_entry(struct bcm_sf2_priv *priv, u16 vid,
-				  struct bcm_sf2_vlan *vlan)
-{
-	u32 entry;
-	int ret;
-
-	core_writel(priv, vid & VTBL_ADDR_INDEX_MASK, CORE_ARLA_VTBL_ADDR);
-
-	ret = bcm_sf2_vlan_op(priv, ARLA_VTBL_CMD_READ);
-	if (ret)
-		return ret;
-
-	entry = core_readl(priv, CORE_ARLA_VTBL_ENTRY);
-	vlan->members = entry & FWD_MAP_MASK;
-	vlan->untag = (entry >> UNTAG_MAP_SHIFT) & UNTAG_MAP_MASK;
-
-	return 0;
-}
-
-static int bcm_sf2_sw_br_join(struct dsa_switch *ds, int port,
-			      struct net_device *bridge)
-{
-	struct bcm_sf2_priv *priv = ds_to_priv(ds);
-	s8 cpu_port = ds->dst->cpu_port;
-	unsigned int i;
-	u32 reg, p_ctl;
-
-	/* Make this port leave the all VLANs join since we will have proper
-	 * VLAN entries from now on
-	 */
-	reg = core_readl(priv, CORE_JOIN_ALL_VLAN_EN);
-	reg &= ~BIT(port);
-	if ((reg & BIT(cpu_port)) == BIT(cpu_port))
-		reg &= ~BIT(cpu_port);
-	core_writel(priv, reg, CORE_JOIN_ALL_VLAN_EN);
-
-	priv->port_sts[port].bridge_dev = bridge;
-	p_ctl = core_readl(priv, CORE_PORT_VLAN_CTL_PORT(port));
-
-	for (i = 0; i < priv->hw_params.num_ports; i++) {
-		if (priv->port_sts[i].bridge_dev != bridge)
-			continue;
-
-		/* Add this local port to the remote port VLAN control
-		 * membership and update the remote port bitmask
-		 */
-		reg = core_readl(priv, CORE_PORT_VLAN_CTL_PORT(i));
-		reg |= 1 << port;
-		core_writel(priv, reg, CORE_PORT_VLAN_CTL_PORT(i));
-		priv->port_sts[i].vlan_ctl_mask = reg;
-
-		p_ctl |= 1 << i;
-	}
-
-	/* Configure the local port VLAN control membership to include
-	 * remote ports and update the local port bitmask
-	 */
-	core_writel(priv, p_ctl, CORE_PORT_VLAN_CTL_PORT(port));
-	priv->port_sts[port].vlan_ctl_mask = p_ctl;
-
-	return 0;
-}
-
-static void bcm_sf2_sw_br_leave(struct dsa_switch *ds, int port)
-{
-	struct bcm_sf2_priv *priv = ds_to_priv(ds);
-	struct net_device *bridge = priv->port_sts[port].bridge_dev;
-	s8 cpu_port = ds->dst->cpu_port;
-	unsigned int i;
-	u32 reg, p_ctl;
-
-	p_ctl = core_readl(priv, CORE_PORT_VLAN_CTL_PORT(port));
-
-	for (i = 0; i < priv->hw_params.num_ports; i++) {
-		/* Don't touch the remaining ports */
-		if (priv->port_sts[i].bridge_dev != bridge)
-			continue;
-
-		reg = core_readl(priv, CORE_PORT_VLAN_CTL_PORT(i));
-		reg &= ~(1 << port);
-		core_writel(priv, reg, CORE_PORT_VLAN_CTL_PORT(i));
-		priv->port_sts[port].vlan_ctl_mask = reg;
-
-		/* Prevent self removal to preserve isolation */
-		if (port != i)
-			p_ctl &= ~(1 << i);
-	}
-
-	core_writel(priv, p_ctl, CORE_PORT_VLAN_CTL_PORT(port));
-	priv->port_sts[port].vlan_ctl_mask = p_ctl;
-	priv->port_sts[port].bridge_dev = NULL;
-
-	/* Make this port join all VLANs without VLAN entries */
-	reg = core_readl(priv, CORE_JOIN_ALL_VLAN_EN);
-	reg |= BIT(port);
-	if (!(reg & BIT(cpu_port)))
-		reg |= BIT(cpu_port);
-	core_writel(priv, reg, CORE_JOIN_ALL_VLAN_EN);
-}
-
-static void bcm_sf2_sw_br_set_stp_state(struct dsa_switch *ds, int port,
-					u8 state)
-{
-	struct bcm_sf2_priv *priv = ds_to_priv(ds);
-	u8 hw_state, cur_hw_state;
-	u32 reg;
-
-	reg = core_readl(priv, CORE_G_PCTL_PORT(port));
-	cur_hw_state = reg & (G_MISTP_STATE_MASK << G_MISTP_STATE_SHIFT);
-
-	switch (state) {
-	case BR_STATE_DISABLED:
-		hw_state = G_MISTP_DIS_STATE;
-		break;
-	case BR_STATE_LISTENING:
-		hw_state = G_MISTP_LISTEN_STATE;
-		break;
-	case BR_STATE_LEARNING:
-		hw_state = G_MISTP_LEARN_STATE;
-		break;
-	case BR_STATE_FORWARDING:
-		hw_state = G_MISTP_FWD_STATE;
-		break;
-	case BR_STATE_BLOCKING:
-		hw_state = G_MISTP_BLOCK_STATE;
-		break;
-	default:
-		pr_err("%s: invalid STP state: %d\n", __func__, state);
-		return;
-	}
-
-	/* Fast-age ARL entries if we are moving a port from Learning or
-	 * Forwarding (cur_hw_state) state to Disabled, Blocking or Listening
-	 * state (hw_state)
-	 */
-	if (cur_hw_state != hw_state) {
-		if (cur_hw_state >= G_MISTP_LEARN_STATE &&
-		    hw_state <= G_MISTP_LISTEN_STATE) {
-			if (bcm_sf2_sw_fast_age_port(ds, port)) {
-				pr_err("%s: fast-ageing failed\n", __func__);
-				return;
-			}
-		}
-	}
-
-	reg = core_readl(priv, CORE_G_PCTL_PORT(port));
-	reg &= ~(G_MISTP_STATE_MASK << G_MISTP_STATE_SHIFT);
-	reg |= hw_state;
-	core_writel(priv, reg, CORE_G_PCTL_PORT(port));
-}
-
-/* Address Resolution Logic routines */
-static int bcm_sf2_arl_op_wait(struct bcm_sf2_priv *priv)
-{
-	unsigned int timeout = 10;
-	u32 reg;
-
-	do {
-		reg = core_readl(priv, CORE_ARLA_RWCTL);
-		if (!(reg & ARL_STRTDN))
-			return 0;
-
-		usleep_range(1000, 2000);
-	} while (timeout--);
-
-	return -ETIMEDOUT;
-}
-
-static int bcm_sf2_arl_rw_op(struct bcm_sf2_priv *priv, unsigned int op)
-{
-	u32 cmd;
-
-	if (op > ARL_RW)
-		return -EINVAL;
-
-	cmd = core_readl(priv, CORE_ARLA_RWCTL);
-	cmd &= ~IVL_SVL_SELECT;
-	cmd |= ARL_STRTDN;
-	if (op)
-		cmd |= ARL_RW;
-	else
-		cmd &= ~ARL_RW;
-	core_writel(priv, cmd, CORE_ARLA_RWCTL);
-
-	return bcm_sf2_arl_op_wait(priv);
-}
-
-static int bcm_sf2_arl_read(struct bcm_sf2_priv *priv, u64 mac,
-			    u16 vid, struct bcm_sf2_arl_entry *ent, u8 *idx,
-			    bool is_valid)
-{
-	unsigned int i;
-	int ret;
-
-	ret = bcm_sf2_arl_op_wait(priv);
-	if (ret)
-		return ret;
-
-	/* Read the 4 bins */
-	for (i = 0; i < 4; i++) {
-		u64 mac_vid;
-		u32 fwd_entry;
-
-		mac_vid = core_readq(priv, CORE_ARLA_MACVID_ENTRY(i));
-		fwd_entry = core_readl(priv, CORE_ARLA_FWD_ENTRY(i));
-		bcm_sf2_arl_to_entry(ent, mac_vid, fwd_entry);
-
-		if (ent->is_valid && is_valid) {
-			*idx = i;
-			return 0;
-		}
-
-		/* This is the MAC we just deleted */
-		if (!is_valid && (mac_vid & mac))
-			return 0;
-	}
-
-	return -ENOENT;
-}
-
-static int bcm_sf2_arl_op(struct bcm_sf2_priv *priv, int op, int port,
-			  const unsigned char *addr, u16 vid, bool is_valid)
-{
-	struct bcm_sf2_arl_entry ent;
-	u32 fwd_entry;
-	u64 mac, mac_vid = 0;
-	u8 idx = 0;
-	int ret;
-
-	/* Convert the array into a 64-bit MAC */
-	mac = bcm_sf2_mac_to_u64(addr);
-
-	/* Perform a read for the given MAC and VID */
-	core_writeq(priv, mac, CORE_ARLA_MAC);
-	core_writel(priv, vid, CORE_ARLA_VID);
-
-	/* Issue a read operation for this MAC */
-	ret = bcm_sf2_arl_rw_op(priv, 1);
-	if (ret)
-		return ret;
-
-	ret = bcm_sf2_arl_read(priv, mac, vid, &ent, &idx, is_valid);
-	/* If this is a read, just finish now */
-	if (op)
-		return ret;
-
-	/* We could not find a matching MAC, so reset to a new entry */
-	if (ret) {
-		fwd_entry = 0;
-		idx = 0;
-	}
-
-	memset(&ent, 0, sizeof(ent));
-	ent.port = port;
-	ent.is_valid = is_valid;
-	ent.vid = vid;
-	ent.is_static = true;
-	memcpy(ent.mac, addr, ETH_ALEN);
-	bcm_sf2_arl_from_entry(&mac_vid, &fwd_entry, &ent);
-
-	core_writeq(priv, mac_vid, CORE_ARLA_MACVID_ENTRY(idx));
-	core_writel(priv, fwd_entry, CORE_ARLA_FWD_ENTRY(idx));
-
-	ret = bcm_sf2_arl_rw_op(priv, 0);
-	if (ret)
-		return ret;
-
-	/* Re-read the entry to check */
-	return bcm_sf2_arl_read(priv, mac, vid, &ent, &idx, is_valid);
-}
-
-static int bcm_sf2_sw_fdb_prepare(struct dsa_switch *ds, int port,
-				  const struct switchdev_obj_port_fdb *fdb,
-				  struct switchdev_trans *trans)
-{
-	/* We do not need to do anything specific here yet */
-	return 0;
-}
-
-static void bcm_sf2_sw_fdb_add(struct dsa_switch *ds, int port,
-			       const struct switchdev_obj_port_fdb *fdb,
-			       struct switchdev_trans *trans)
-{
-	struct bcm_sf2_priv *priv = ds_to_priv(ds);
-
-	if (bcm_sf2_arl_op(priv, 0, port, fdb->addr, fdb->vid, true))
-		pr_err("%s: failed to add MAC address\n", __func__);
-}
-
-static int bcm_sf2_sw_fdb_del(struct dsa_switch *ds, int port,
-			      const struct switchdev_obj_port_fdb *fdb)
-{
-	struct bcm_sf2_priv *priv = ds_to_priv(ds);
-
-	return bcm_sf2_arl_op(priv, 0, port, fdb->addr, fdb->vid, false);
-}
-
-static int bcm_sf2_arl_search_wait(struct bcm_sf2_priv *priv)
-{
-	unsigned timeout = 1000;
-	u32 reg;
-
-	do {
-		reg = core_readl(priv, CORE_ARLA_SRCH_CTL);
-		if (!(reg & ARLA_SRCH_STDN))
-			return 0;
-
-		if (reg & ARLA_SRCH_VLID)
-			return 0;
-
-		usleep_range(1000, 2000);
-	} while (timeout--);
-
-	return -ETIMEDOUT;
-}
-
-static void bcm_sf2_arl_search_rd(struct bcm_sf2_priv *priv, u8 idx,
-				  struct bcm_sf2_arl_entry *ent)
-{
-	u64 mac_vid;
-	u32 fwd_entry;
-
-	mac_vid = core_readq(priv, CORE_ARLA_SRCH_RSLT_MACVID(idx));
-	fwd_entry = core_readl(priv, CORE_ARLA_SRCH_RSLT(idx));
-	bcm_sf2_arl_to_entry(ent, mac_vid, fwd_entry);
-}
-
-static int bcm_sf2_sw_fdb_copy(struct net_device *dev, int port,
-			       const struct bcm_sf2_arl_entry *ent,
-			       struct switchdev_obj_port_fdb *fdb,
-			       int (*cb)(struct switchdev_obj *obj))
-{
-	if (!ent->is_valid)
-		return 0;
-
-	if (port != ent->port)
-		return 0;
-
-	ether_addr_copy(fdb->addr, ent->mac);
-	fdb->vid = ent->vid;
-	fdb->ndm_state = ent->is_static ? NUD_NOARP : NUD_REACHABLE;
-
-	return cb(&fdb->obj);
-}
-
-static int bcm_sf2_sw_fdb_dump(struct dsa_switch *ds, int port,
-			       struct switchdev_obj_port_fdb *fdb,
-			       int (*cb)(struct switchdev_obj *obj))
-{
-	struct bcm_sf2_priv *priv = ds_to_priv(ds);
-	struct net_device *dev = ds->ports[port].netdev;
-	struct bcm_sf2_arl_entry results[2];
-	unsigned int count = 0;
-	int ret;
-
-	/* Start search operation */
-	core_writel(priv, ARLA_SRCH_STDN, CORE_ARLA_SRCH_CTL);
-
-	do {
-		ret = bcm_sf2_arl_search_wait(priv);
-		if (ret)
-			return ret;
-
-		/* Read both entries, then return their values back */
-		bcm_sf2_arl_search_rd(priv, 0, &results[0]);
-		ret = bcm_sf2_sw_fdb_copy(dev, port, &results[0], fdb, cb);
-		if (ret)
-			return ret;
-
-		bcm_sf2_arl_search_rd(priv, 1, &results[1]);
-		ret = bcm_sf2_sw_fdb_copy(dev, port, &results[1], fdb, cb);
-		if (ret)
-			return ret;
-
-		if (!results[0].is_valid && !results[1].is_valid)
-			break;
-
-	} while (count++ < CORE_ARLA_NUM_ENTRIES);
-
-	return 0;
-}
-
 static int bcm_sf2_sw_indir_rw(struct bcm_sf2_priv *priv, int op, int addr,
 			       int regnum, u16 val)
 {
@@ -1036,12 +464,10 @@ static int bcm_sf2_sw_rst(struct bcm_sf2_priv *priv)
 
 static void bcm_sf2_intr_disable(struct bcm_sf2_priv *priv)
 {
-	intrl2_0_writel(priv, 0xffffffff, INTRL2_CPU_MASK_SET);
+	intrl2_0_mask_set(priv, 0xffffffff);
 	intrl2_0_writel(priv, 0xffffffff, INTRL2_CPU_CLEAR);
-	intrl2_0_writel(priv, 0, INTRL2_CPU_MASK_CLEAR);
-	intrl2_1_writel(priv, 0xffffffff, INTRL2_CPU_MASK_SET);
+	intrl2_1_mask_set(priv, 0xffffffff);
 	intrl2_1_writel(priv, 0xffffffff, INTRL2_CPU_CLEAR);
-	intrl2_1_writel(priv, 0, INTRL2_CPU_MASK_CLEAR);
 }
 
 static void bcm_sf2_identify_ports(struct bcm_sf2_priv *priv,
@@ -1082,7 +508,7 @@ static void bcm_sf2_identify_ports(struct bcm_sf2_priv *priv,
 
 static int bcm_sf2_mdio_register(struct dsa_switch *ds)
 {
-	struct bcm_sf2_priv *priv = ds_to_priv(ds);
+	struct bcm_sf2_priv *priv = bcm_sf2_to_priv(ds);
 	struct device_node *dn;
 	static int index;
 	int err;
@@ -1146,14 +572,9 @@ static void bcm_sf2_mdio_unregister(struct bcm_sf2_priv *priv)
 		of_node_put(priv->master_mii_dn);
 }
 
-static int bcm_sf2_sw_set_addr(struct dsa_switch *ds, u8 *addr)
-{
-	return 0;
-}
-
 static u32 bcm_sf2_sw_get_phy_flags(struct dsa_switch *ds, int port)
 {
-	struct bcm_sf2_priv *priv = ds_to_priv(ds);
+	struct bcm_sf2_priv *priv = bcm_sf2_to_priv(ds);
 
 	/* The BCM7xxx PHY driver expects to find the integrated PHY revision
 	 * in bits 15:8 and the patch level in bits 7:0 which is exactly what
@@ -1166,7 +587,7 @@ static u32 bcm_sf2_sw_get_phy_flags(struct dsa_switch *ds, int port)
 static void bcm_sf2_sw_adjust_link(struct dsa_switch *ds, int port,
 				   struct phy_device *phydev)
 {
-	struct bcm_sf2_priv *priv = ds_to_priv(ds);
+	struct bcm_sf2_priv *priv = bcm_sf2_to_priv(ds);
 	u32 id_mode_dis = 0, port_mode;
 	const char *str = NULL;
 	u32 reg;
@@ -1246,7 +667,7 @@ force_link:
 static void bcm_sf2_sw_fixed_link_update(struct dsa_switch *ds, int port,
 					 struct fixed_phy_status *status)
 {
-	struct bcm_sf2_priv *priv = ds_to_priv(ds);
+	struct bcm_sf2_priv *priv = bcm_sf2_to_priv(ds);
 	u32 duplex, pause;
 	u32 reg;
 
@@ -1298,7 +719,7 @@ static void bcm_sf2_sw_fixed_link_update(struct dsa_switch *ds, int port,
 
 static int bcm_sf2_sw_suspend(struct dsa_switch *ds)
 {
-	struct bcm_sf2_priv *priv = ds_to_priv(ds);
+	struct bcm_sf2_priv *priv = bcm_sf2_to_priv(ds);
 	unsigned int port;
 
 	bcm_sf2_intr_disable(priv);
@@ -1318,7 +739,7 @@ static int bcm_sf2_sw_suspend(struct dsa_switch *ds)
 
 static int bcm_sf2_sw_resume(struct dsa_switch *ds)
 {
-	struct bcm_sf2_priv *priv = ds_to_priv(ds);
+	struct bcm_sf2_priv *priv = bcm_sf2_to_priv(ds);
 	unsigned int port;
 	int ret;
 
@@ -1345,7 +766,7 @@ static void bcm_sf2_sw_get_wol(struct dsa_switch *ds, int port,
 			       struct ethtool_wolinfo *wol)
 {
 	struct net_device *p = ds->dst[ds->index].master_netdev;
-	struct bcm_sf2_priv *priv = ds_to_priv(ds);
+	struct bcm_sf2_priv *priv = bcm_sf2_to_priv(ds);
 	struct ethtool_wolinfo pwol;
 
 	/* Get the parent device WoL settings */
@@ -1368,7 +789,7 @@ static int bcm_sf2_sw_set_wol(struct dsa_switch *ds, int port,
 			      struct ethtool_wolinfo *wol)
 {
 	struct net_device *p = ds->dst[ds->index].master_netdev;
-	struct bcm_sf2_priv *priv = ds_to_priv(ds);
+	struct bcm_sf2_priv *priv = bcm_sf2_to_priv(ds);
 	s8 cpu_port = ds->dst[ds->index].cpu_port;
 	struct ethtool_wolinfo pwol;
 
@@ -1393,43 +814,32 @@ static int bcm_sf2_sw_set_wol(struct dsa_switch *ds, int port,
 	return p->ethtool_ops->set_wol(p, wol);
 }
 
-static void bcm_sf2_enable_vlan(struct bcm_sf2_priv *priv, bool enable)
+static int bcm_sf2_vlan_op_wait(struct bcm_sf2_priv *priv)
 {
-	u32 mgmt, vc0, vc1, vc4, vc5;
+	unsigned int timeout = 10;
+	u32 reg;
 
-	mgmt = core_readl(priv, CORE_SWMODE);
-	vc0 = core_readl(priv, CORE_VLAN_CTRL0);
-	vc1 = core_readl(priv, CORE_VLAN_CTRL1);
-	vc4 = core_readl(priv, CORE_VLAN_CTRL4);
-	vc5 = core_readl(priv, CORE_VLAN_CTRL5);
+	do {
+		reg = core_readl(priv, CORE_ARLA_VTBL_RWCTRL);
+		if (!(reg & ARLA_VTBL_STDN))
+			return 0;
 
-	mgmt &= ~SW_FWDG_MODE;
+		usleep_range(1000, 2000);
+	} while (timeout--);
 
-	if (enable) {
-		vc0 |= VLAN_EN | VLAN_LEARN_MODE_IVL;
-		vc1 |= EN_RSV_MCAST_UNTAG | EN_RSV_MCAST_FWDMAP;
-		vc4 &= ~(INGR_VID_CHK_MASK << INGR_VID_CHK_SHIFT);
-		vc4 |= INGR_VID_CHK_DROP;
-		vc5 |= DROP_VTABLE_MISS | EN_VID_FFF_FWD;
-	} else {
-		vc0 &= ~(VLAN_EN | VLAN_LEARN_MODE_IVL);
-		vc1 &= ~(EN_RSV_MCAST_UNTAG | EN_RSV_MCAST_FWDMAP);
-		vc4 &= ~(INGR_VID_CHK_MASK << INGR_VID_CHK_SHIFT);
-		vc5 &= ~(DROP_VTABLE_MISS | EN_VID_FFF_FWD);
-		vc4 |= INGR_VID_CHK_VID_VIOL_IMP;
-	}
+	return -ETIMEDOUT;
+}
+
+static int bcm_sf2_vlan_op(struct bcm_sf2_priv *priv, u8 op)
+{
+	core_writel(priv, ARLA_VTBL_STDN | op, CORE_ARLA_VTBL_RWCTRL);
 
-	core_writel(priv, vc0, CORE_VLAN_CTRL0);
-	core_writel(priv, vc1, CORE_VLAN_CTRL1);
-	core_writel(priv, 0, CORE_VLAN_CTRL3);
-	core_writel(priv, vc4, CORE_VLAN_CTRL4);
-	core_writel(priv, vc5, CORE_VLAN_CTRL5);
-	core_writel(priv, mgmt, CORE_SWMODE);
+	return bcm_sf2_vlan_op_wait(priv);
 }
 
 static void bcm_sf2_sw_configure_vlan(struct dsa_switch *ds)
 {
-	struct bcm_sf2_priv *priv = ds_to_priv(ds);
+	struct bcm_sf2_priv *priv = bcm_sf2_to_priv(ds);
 	unsigned int port;
 
 	/* Clear all VLANs */
@@ -1443,162 +853,199 @@ static void bcm_sf2_sw_configure_vlan(struct dsa_switch *ds)
 	}
 }
 
-static int bcm_sf2_sw_vlan_filtering(struct dsa_switch *ds, int port,
-				     bool vlan_filtering)
+static int bcm_sf2_sw_setup(struct dsa_switch *ds)
 {
+	struct bcm_sf2_priv *priv = bcm_sf2_to_priv(ds);
+	unsigned int port;
+
+	/* Enable all valid ports and disable those unused */
+	for (port = 0; port < priv->hw_params.num_ports; port++) {
+		/* IMP port receives special treatment */
+		if ((1 << port) & ds->enabled_port_mask)
+			bcm_sf2_port_setup(ds, port, NULL);
+		else if (dsa_is_cpu_port(ds, port))
+			bcm_sf2_imp_setup(ds, port);
+		else
+			bcm_sf2_port_disable(ds, port, NULL);
+	}
+
+	bcm_sf2_sw_configure_vlan(ds);
+
 	return 0;
 }
 
-static int bcm_sf2_sw_vlan_prepare(struct dsa_switch *ds, int port,
-				   const struct switchdev_obj_port_vlan *vlan,
-				   struct switchdev_trans *trans)
+/* The SWITCH_CORE register space is managed by b53 but operates on a page +
+ * register basis so we need to translate that into an address that the
+ * bus-glue understands.
+ */
+#define SF2_PAGE_REG_MKADDR(page, reg)	((page) << 10 | (reg) << 2)
+
+static int bcm_sf2_core_read8(struct b53_device *dev, u8 page, u8 reg,
+			      u8 *val)
 {
-	struct bcm_sf2_priv *priv = ds_to_priv(ds);
+	struct bcm_sf2_priv *priv = dev->priv;
 
-	bcm_sf2_enable_vlan(priv, true);
+	*val = core_readl(priv, SF2_PAGE_REG_MKADDR(page, reg));
 
 	return 0;
 }
 
-static void bcm_sf2_sw_vlan_add(struct dsa_switch *ds, int port,
-				const struct switchdev_obj_port_vlan *vlan,
-				struct switchdev_trans *trans)
+static int bcm_sf2_core_read16(struct b53_device *dev, u8 page, u8 reg,
+			       u16 *val)
 {
-	struct bcm_sf2_priv *priv = ds_to_priv(ds);
-	bool untagged = vlan->flags & BRIDGE_VLAN_INFO_UNTAGGED;
-	bool pvid = vlan->flags & BRIDGE_VLAN_INFO_PVID;
-	s8 cpu_port = ds->dst->cpu_port;
-	struct bcm_sf2_vlan *vl;
-	u16 vid;
+	struct bcm_sf2_priv *priv = dev->priv;
 
-	for (vid = vlan->vid_begin; vid <= vlan->vid_end; ++vid) {
-		vl = &priv->vlans[vid];
+	*val = core_readl(priv, SF2_PAGE_REG_MKADDR(page, reg));
 
-		bcm_sf2_get_vlan_entry(priv, vid, vl);
+	return 0;
+}
 
-		vl->members |= BIT(port) | BIT(cpu_port);
-		if (untagged)
-			vl->untag |= BIT(port) | BIT(cpu_port);
-		else
-			vl->untag &= ~(BIT(port) | BIT(cpu_port));
+static int bcm_sf2_core_read32(struct b53_device *dev, u8 page, u8 reg,
+			       u32 *val)
+{
+	struct bcm_sf2_priv *priv = dev->priv;
 
-		bcm_sf2_set_vlan_entry(priv, vid, vl);
-		bcm_sf2_sw_fast_age_vlan(priv, vid);
-	}
+	*val = core_readl(priv, SF2_PAGE_REG_MKADDR(page, reg));
 
-	if (pvid) {
-		core_writel(priv, vlan->vid_end, CORE_DEFAULT_1Q_TAG_P(port));
-		core_writel(priv, vlan->vid_end,
-			    CORE_DEFAULT_1Q_TAG_P(cpu_port));
-		bcm_sf2_sw_fast_age_vlan(priv, vid);
-	}
+	return 0;
 }
 
-static int bcm_sf2_sw_vlan_del(struct dsa_switch *ds, int port,
-			       const struct switchdev_obj_port_vlan *vlan)
+static int bcm_sf2_core_read64(struct b53_device *dev, u8 page, u8 reg,
+			       u64 *val)
 {
-	struct bcm_sf2_priv *priv = ds_to_priv(ds);
-	bool untagged = vlan->flags & BRIDGE_VLAN_INFO_UNTAGGED;
-	s8 cpu_port = ds->dst->cpu_port;
-	struct bcm_sf2_vlan *vl;
-	u16 vid, pvid;
-	int ret;
+	struct bcm_sf2_priv *priv = dev->priv;
 
-	pvid = core_readl(priv, CORE_DEFAULT_1Q_TAG_P(port));
+	*val = core_readq(priv, SF2_PAGE_REG_MKADDR(page, reg));
 
-	for (vid = vlan->vid_begin; vid <= vlan->vid_end; ++vid) {
-		vl = &priv->vlans[vid];
-
-		ret = bcm_sf2_get_vlan_entry(priv, vid, vl);
-		if (ret)
-			return ret;
-
-		vl->members &= ~BIT(port);
-		if ((vl->members & BIT(cpu_port)) == BIT(cpu_port))
-			vl->members = 0;
-		if (pvid == vid)
-			pvid = 0;
-		if (untagged) {
-			vl->untag &= ~BIT(port);
-			if ((vl->untag & BIT(port)) == BIT(cpu_port))
-				vl->untag = 0;
-		}
+	return 0;
+}
 
-		bcm_sf2_set_vlan_entry(priv, vid, vl);
-		bcm_sf2_sw_fast_age_vlan(priv, vid);
-	}
+static int bcm_sf2_core_write8(struct b53_device *dev, u8 page, u8 reg,
+			       u8 value)
+{
+	struct bcm_sf2_priv *priv = dev->priv;
 
-	core_writel(priv, pvid, CORE_DEFAULT_1Q_TAG_P(port));
-	core_writel(priv, pvid, CORE_DEFAULT_1Q_TAG_P(cpu_port));
-	bcm_sf2_sw_fast_age_vlan(priv, vid);
+	core_writel(priv, value, SF2_PAGE_REG_MKADDR(page, reg));
 
 	return 0;
 }
 
-static int bcm_sf2_sw_vlan_dump(struct dsa_switch *ds, int port,
-				struct switchdev_obj_port_vlan *vlan,
-				int (*cb)(struct switchdev_obj *obj))
+static int bcm_sf2_core_write16(struct b53_device *dev, u8 page, u8 reg,
+				u16 value)
 {
-	struct bcm_sf2_priv *priv = ds_to_priv(ds);
-	struct bcm_sf2_port_status *p = &priv->port_sts[port];
-	struct bcm_sf2_vlan *vl;
-	u16 vid, pvid;
-	int err = 0;
+	struct bcm_sf2_priv *priv = dev->priv;
 
-	pvid = core_readl(priv, CORE_DEFAULT_1Q_TAG_P(port));
+	core_writel(priv, value, SF2_PAGE_REG_MKADDR(page, reg));
 
-	for (vid = 0; vid < VLAN_N_VID; vid++) {
-		vl = &priv->vlans[vid];
+	return 0;
+}
 
-		if (!(vl->members & BIT(port)))
-			continue;
+static int bcm_sf2_core_write32(struct b53_device *dev, u8 page, u8 reg,
+				u32 value)
+{
+	struct bcm_sf2_priv *priv = dev->priv;
 
-		vlan->vid_begin = vlan->vid_end = vid;
-		vlan->flags = 0;
+	core_writel(priv, value, SF2_PAGE_REG_MKADDR(page, reg));
 
-		if (vl->untag & BIT(port))
-			vlan->flags |= BRIDGE_VLAN_INFO_UNTAGGED;
-		if (p->pvid == vid)
-			vlan->flags |= BRIDGE_VLAN_INFO_PVID;
+	return 0;
+}
 
-		err = cb(&vlan->obj);
-		if (err)
-			break;
-	}
+static int bcm_sf2_core_write64(struct b53_device *dev, u8 page, u8 reg,
+				u64 value)
+{
+	struct bcm_sf2_priv *priv = dev->priv;
 
-	return err;
+	core_writeq(priv, value, SF2_PAGE_REG_MKADDR(page, reg));
+
+	return 0;
 }
 
-static int bcm_sf2_sw_setup(struct dsa_switch *ds)
+static struct b53_io_ops bcm_sf2_io_ops = {
+	.read8	= bcm_sf2_core_read8,
+	.read16	= bcm_sf2_core_read16,
+	.read32	= bcm_sf2_core_read32,
+	.read48	= bcm_sf2_core_read64,
+	.read64	= bcm_sf2_core_read64,
+	.write8	= bcm_sf2_core_write8,
+	.write16 = bcm_sf2_core_write16,
+	.write32 = bcm_sf2_core_write32,
+	.write48 = bcm_sf2_core_write64,
+	.write64 = bcm_sf2_core_write64,
+};
+
+static int bcm_sf2_sw_probe(struct platform_device *pdev)
 {
 	const char *reg_names[BCM_SF2_REGS_NUM] = BCM_SF2_REGS_NAME;
-	struct bcm_sf2_priv *priv = ds_to_priv(ds);
-	struct device_node *dn;
+	struct device_node *dn = pdev->dev.of_node;
+	struct b53_platform_data *pdata;
+	struct bcm_sf2_priv *priv;
+	struct b53_device *dev;
+	struct dsa_switch *ds;
 	void __iomem **base;
-	unsigned int port;
+	struct resource *r;
 	unsigned int i;
 	u32 reg, rev;
 	int ret;
 
+	priv = devm_kzalloc(&pdev->dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	dev = b53_switch_alloc(&pdev->dev, &bcm_sf2_io_ops, priv);
+	if (!dev)
+		return -ENOMEM;
+
+	pdata = devm_kzalloc(&pdev->dev, sizeof(*pdata), GFP_KERNEL);
+	if (!pdata)
+		return -ENOMEM;
+
+	/* Auto-detection using standard registers will not work, so
+	 * provide an indication of what kind of device we are for
+	 * b53_common to work with
+	 */
+	pdata->chip_id = BCM7445_DEVICE_ID;
+	dev->pdata = pdata;
+
+	priv->dev = dev;
+	ds = dev->ds;
+
+	/* Override the parts that are non-standard wrt. normal b53 devices */
+	ds->ops->get_tag_protocol = bcm_sf2_sw_get_tag_protocol;
+	ds->ops->setup = bcm_sf2_sw_setup;
+	ds->ops->get_phy_flags = bcm_sf2_sw_get_phy_flags;
+	ds->ops->adjust_link = bcm_sf2_sw_adjust_link;
+	ds->ops->fixed_link_update = bcm_sf2_sw_fixed_link_update;
+	ds->ops->suspend = bcm_sf2_sw_suspend;
+	ds->ops->resume = bcm_sf2_sw_resume;
+	ds->ops->get_wol = bcm_sf2_sw_get_wol;
+	ds->ops->set_wol = bcm_sf2_sw_set_wol;
+	ds->ops->port_enable = bcm_sf2_port_setup;
+	ds->ops->port_disable = bcm_sf2_port_disable;
+	ds->ops->get_eee = bcm_sf2_sw_get_eee;
+	ds->ops->set_eee = bcm_sf2_sw_set_eee;
+
+	/* Avoid having DSA free our slave MDIO bus (checking for
+	 * ds->slave_mii_bus and ds->ops->phy_read being non-NULL)
+	 */
+	ds->ops->phy_read = NULL;
+
+	dev_set_drvdata(&pdev->dev, priv);
+
 	spin_lock_init(&priv->indir_lock);
 	mutex_init(&priv->stats_mutex);
 
-	/* All the interesting properties are at the parent device_node
-	 * level
-	 */
-	dn = ds->cd->of_node->parent;
-	bcm_sf2_identify_ports(priv, ds->cd->of_node);
+	bcm_sf2_identify_ports(priv, dn->child);
 
 	priv->irq0 = irq_of_parse_and_map(dn, 0);
 	priv->irq1 = irq_of_parse_and_map(dn, 1);
 
 	base = &priv->core;
 	for (i = 0; i < BCM_SF2_REGS_NUM; i++) {
-		*base = of_iomap(dn, i);
-		if (*base == NULL) {
+		r = platform_get_resource(pdev, IORESOURCE_MEM, i);
+		*base = devm_ioremap_resource(&pdev->dev, r);
+		if (IS_ERR(*base)) {
 			pr_err("unable to find register: %s\n", reg_names[i]);
-			ret = -ENOMEM;
-			goto out_unmap;
+			return PTR_ERR(*base);
 		}
 		base++;
 	}
@@ -1606,30 +1053,30 @@ static int bcm_sf2_sw_setup(struct dsa_switch *ds)
 	ret = bcm_sf2_sw_rst(priv);
 	if (ret) {
 		pr_err("unable to software reset switch: %d\n", ret);
-		goto out_unmap;
+		return ret;
 	}
 
 	ret = bcm_sf2_mdio_register(ds);
 	if (ret) {
 		pr_err("failed to register MDIO bus\n");
-		goto out_unmap;
+		return ret;
 	}
 
 	/* Disable all interrupts and request them */
 	bcm_sf2_intr_disable(priv);
 
-	ret = request_irq(priv->irq0, bcm_sf2_switch_0_isr, 0,
-			  "switch_0", priv);
+	ret = devm_request_irq(&pdev->dev, priv->irq0, bcm_sf2_switch_0_isr, 0,
+			       "switch_0", priv);
 	if (ret < 0) {
 		pr_err("failed to request switch_0 IRQ\n");
 		goto out_mdio;
 	}
 
-	ret = request_irq(priv->irq1, bcm_sf2_switch_1_isr, 0,
-			  "switch_1", priv);
+	ret = devm_request_irq(&pdev->dev, priv->irq1, bcm_sf2_switch_1_isr, 0,
+			       "switch_1", priv);
 	if (ret < 0) {
 		pr_err("failed to request switch_1 IRQ\n");
-		goto out_free_irq0;
+		goto out_mdio;
 	}
 
 	/* Reset the MIB counters */
@@ -1649,19 +1096,6 @@ static int bcm_sf2_sw_setup(struct dsa_switch *ds)
 				 &priv->hw_params.num_gphy))
 		priv->hw_params.num_gphy = 1;
 
-	/* Enable all valid ports and disable those unused */
-	for (port = 0; port < priv->hw_params.num_ports; port++) {
-		/* IMP port receives special treatment */
-		if ((1 << port) & ds->enabled_port_mask)
-			bcm_sf2_port_setup(ds, port, NULL);
-		else if (dsa_is_cpu_port(ds, port))
-			bcm_sf2_imp_setup(ds, port);
-		else
-			bcm_sf2_port_disable(ds, port, NULL);
-	}
-
-	bcm_sf2_sw_configure_vlan(ds);
-
 	rev = reg_readl(priv, REG_SWITCH_REVISION);
 	priv->hw_params.top_rev = (rev >> SWITCH_TOP_REV_SHIFT) &
 					SWITCH_TOP_REV_MASK;
@@ -1670,6 +1104,10 @@ static int bcm_sf2_sw_setup(struct dsa_switch *ds)
 	rev = reg_readl(priv, REG_PHY_REVISION);
 	priv->hw_params.gphy_rev = rev & PHY_REVISION_MASK;
 
+	ret = b53_switch_register(dev);
+	if (ret)
+		goto out_mdio;
+
 	pr_info("Starfighter 2 top: %x.%02x, core: %x.%02x base: 0x%p, IRQs: %d, %d\n",
 		priv->hw_params.top_rev >> 8, priv->hw_params.top_rev & 0xff,
 		priv->hw_params.core_rev >> 8, priv->hw_params.core_rev & 0xff,
@@ -1677,66 +1115,60 @@ static int bcm_sf2_sw_setup(struct dsa_switch *ds)
 
 	return 0;
 
-out_free_irq0:
-	free_irq(priv->irq0, priv);
 out_mdio:
 	bcm_sf2_mdio_unregister(priv);
-out_unmap:
-	base = &priv->core;
-	for (i = 0; i < BCM_SF2_REGS_NUM; i++) {
-		if (*base)
-			iounmap(*base);
-		base++;
-	}
 	return ret;
 }
 
-static struct dsa_switch_driver bcm_sf2_switch_driver = {
-	.tag_protocol		= DSA_TAG_PROTO_BRCM,
-	.probe			= bcm_sf2_sw_drv_probe,
-	.setup			= bcm_sf2_sw_setup,
-	.set_addr		= bcm_sf2_sw_set_addr,
-	.get_phy_flags		= bcm_sf2_sw_get_phy_flags,
-	.get_strings		= bcm_sf2_sw_get_strings,
-	.get_ethtool_stats	= bcm_sf2_sw_get_ethtool_stats,
-	.get_sset_count		= bcm_sf2_sw_get_sset_count,
-	.adjust_link		= bcm_sf2_sw_adjust_link,
-	.fixed_link_update	= bcm_sf2_sw_fixed_link_update,
-	.suspend		= bcm_sf2_sw_suspend,
-	.resume			= bcm_sf2_sw_resume,
-	.get_wol		= bcm_sf2_sw_get_wol,
-	.set_wol		= bcm_sf2_sw_set_wol,
-	.port_enable		= bcm_sf2_port_setup,
-	.port_disable		= bcm_sf2_port_disable,
-	.get_eee		= bcm_sf2_sw_get_eee,
-	.set_eee		= bcm_sf2_sw_set_eee,
-	.port_bridge_join	= bcm_sf2_sw_br_join,
-	.port_bridge_leave	= bcm_sf2_sw_br_leave,
-	.port_stp_state_set	= bcm_sf2_sw_br_set_stp_state,
-	.port_fdb_prepare	= bcm_sf2_sw_fdb_prepare,
-	.port_fdb_add		= bcm_sf2_sw_fdb_add,
-	.port_fdb_del		= bcm_sf2_sw_fdb_del,
-	.port_fdb_dump		= bcm_sf2_sw_fdb_dump,
-	.port_vlan_filtering	= bcm_sf2_sw_vlan_filtering,
-	.port_vlan_prepare	= bcm_sf2_sw_vlan_prepare,
-	.port_vlan_add		= bcm_sf2_sw_vlan_add,
-	.port_vlan_del		= bcm_sf2_sw_vlan_del,
-	.port_vlan_dump		= bcm_sf2_sw_vlan_dump,
-};
-
-static int __init bcm_sf2_init(void)
+static int bcm_sf2_sw_remove(struct platform_device *pdev)
 {
-	register_switch_driver(&bcm_sf2_switch_driver);
+	struct bcm_sf2_priv *priv = platform_get_drvdata(pdev);
+
+	/* Disable all ports and interrupts */
+	priv->wol_ports_mask = 0;
+	bcm_sf2_sw_suspend(priv->dev->ds);
+	dsa_unregister_switch(priv->dev->ds);
+	bcm_sf2_mdio_unregister(priv);
 
 	return 0;
 }
-module_init(bcm_sf2_init);
 
-static void __exit bcm_sf2_exit(void)
+#ifdef CONFIG_PM_SLEEP
+static int bcm_sf2_suspend(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct bcm_sf2_priv *priv = platform_get_drvdata(pdev);
+
+	return dsa_switch_suspend(priv->dev->ds);
+}
+
+static int bcm_sf2_resume(struct device *dev)
 {
-	unregister_switch_driver(&bcm_sf2_switch_driver);
+	struct platform_device *pdev = to_platform_device(dev);
+	struct bcm_sf2_priv *priv = platform_get_drvdata(pdev);
+
+	return dsa_switch_resume(priv->dev->ds);
 }
-module_exit(bcm_sf2_exit);
+#endif /* CONFIG_PM_SLEEP */
+
+static SIMPLE_DEV_PM_OPS(bcm_sf2_pm_ops,
+			 bcm_sf2_suspend, bcm_sf2_resume);
+
+static const struct of_device_id bcm_sf2_of_match[] = {
+	{ .compatible = "brcm,bcm7445-switch-v4.0" },
+	{ /* sentinel */ },
+};
+
+static struct platform_driver bcm_sf2_driver = {
+	.probe	= bcm_sf2_sw_probe,
+	.remove	= bcm_sf2_sw_remove,
+	.driver = {
+		.name = "brcm-sf2",
+		.of_match_table = bcm_sf2_of_match,
+		.pm = &bcm_sf2_pm_ops,
+	},
+};
+module_platform_driver(bcm_sf2_driver);
 
 MODULE_AUTHOR("Broadcom Corporation");
 MODULE_DESCRIPTION("Driver for Broadcom Starfighter 2 ethernet switch chip");
diff --git a/drivers/net/dsa/bcm_sf2.h b/drivers/net/dsa/bcm_sf2.h
index dd446e466699..44692673e1d5 100644
--- a/drivers/net/dsa/bcm_sf2.h
+++ b/drivers/net/dsa/bcm_sf2.h
@@ -26,6 +26,7 @@
 #include <net/dsa.h>
 
 #include "bcm_sf2_regs.h"
+#include "b53/b53_priv.h"
 
 struct bcm_sf2_hw_params {
 	u16	top_rev;
@@ -49,72 +50,8 @@ struct bcm_sf2_port_status {
 	unsigned int link;
 
 	struct ethtool_eee eee;
-
-	u32 vlan_ctl_mask;
-	u16 pvid;
-
-	struct net_device *bridge_dev;
-};
-
-struct bcm_sf2_arl_entry {
-	u8 port;
-	u8 mac[ETH_ALEN];
-	u16 vid;
-	u8 is_valid:1;
-	u8 is_age:1;
-	u8 is_static:1;
 };
 
-struct bcm_sf2_vlan {
-	u16 members;
-	u16 untag;
-};
-
-static inline void bcm_sf2_mac_from_u64(u64 src, u8 *dst)
-{
-	unsigned int i;
-
-	for (i = 0; i < ETH_ALEN; i++)
-		dst[ETH_ALEN - 1 - i] = (src >> (8 * i)) & 0xff;
-}
-
-static inline u64 bcm_sf2_mac_to_u64(const u8 *src)
-{
-	unsigned int i;
-	u64 dst = 0;
-
-	for (i = 0; i < ETH_ALEN; i++)
-		dst |= (u64)src[ETH_ALEN - 1 - i] << (8 * i);
-
-	return dst;
-}
-
-static inline void bcm_sf2_arl_to_entry(struct bcm_sf2_arl_entry *ent,
-					u64 mac_vid, u32 fwd_entry)
-{
-	memset(ent, 0, sizeof(*ent));
-	ent->port = fwd_entry & PORTID_MASK;
-	ent->is_valid = !!(fwd_entry & ARL_VALID);
-	ent->is_age = !!(fwd_entry & ARL_AGE);
-	ent->is_static = !!(fwd_entry & ARL_STATIC);
-	bcm_sf2_mac_from_u64(mac_vid, ent->mac);
-	ent->vid = mac_vid >> VID_SHIFT;
-}
-
-static inline void bcm_sf2_arl_from_entry(u64 *mac_vid, u32 *fwd_entry,
-					  const struct bcm_sf2_arl_entry *ent)
-{
-	*mac_vid = bcm_sf2_mac_to_u64(ent->mac);
-	*mac_vid |= (u64)(ent->vid & VID_MASK) << VID_SHIFT;
-	*fwd_entry = ent->port & PORTID_MASK;
-	if (ent->is_valid)
-		*fwd_entry |= ARL_VALID;
-	if (ent->is_static)
-		*fwd_entry |= ARL_STATIC;
-	if (ent->is_age)
-		*fwd_entry |= ARL_AGE;
-}
-
 struct bcm_sf2_priv {
 	/* Base registers, keep those in order with BCM_SF2_REGS_NAME */
 	void __iomem			*core;
@@ -134,6 +71,9 @@ struct bcm_sf2_priv {
 	u32				irq1_stat;
 	u32				irq1_mask;
 
+	/* Backing b53_device */
+	struct b53_device		*dev;
+
 	/* Mutex protecting access to the MIB counters */
 	struct mutex			stats_mutex;
 
@@ -155,16 +95,14 @@ struct bcm_sf2_priv {
 	struct device_node		*master_mii_dn;
 	struct mii_bus			*slave_mii_bus;
 	struct mii_bus			*master_mii_bus;
-
-	/* Cache of programmed VLANs */
-	struct bcm_sf2_vlan		vlans[VLAN_N_VID];
 };
 
-struct bcm_sf2_hw_stats {
-	const char	*string;
-	u16		reg;
-	u8		sizeof_stat;
-};
+static inline struct bcm_sf2_priv *bcm_sf2_to_priv(struct dsa_switch *ds)
+{
+	struct b53_device *dev = ds->priv;
+
+	return dev->priv;
+}
 
 #define SF2_IO_MACRO(name) \
 static inline u32 name##_readl(struct bcm_sf2_priv *priv, u32 off)	\
diff --git a/drivers/net/dsa/bcm_sf2_regs.h b/drivers/net/dsa/bcm_sf2_regs.h
index 9f2a9cb42074..838fe373cd6f 100644
--- a/drivers/net/dsa/bcm_sf2_regs.h
+++ b/drivers/net/dsa/bcm_sf2_regs.h
@@ -115,14 +115,6 @@
 #define  RX_BCST_EN			(1 << 2)
 #define  RX_MCST_EN			(1 << 3)
 #define  RX_UCST_EN			(1 << 4)
-#define  G_MISTP_STATE_SHIFT		5
-#define  G_MISTP_NO_STP			(0 << G_MISTP_STATE_SHIFT)
-#define  G_MISTP_DIS_STATE		(1 << G_MISTP_STATE_SHIFT)
-#define  G_MISTP_BLOCK_STATE		(2 << G_MISTP_STATE_SHIFT)
-#define  G_MISTP_LISTEN_STATE		(3 << G_MISTP_STATE_SHIFT)
-#define  G_MISTP_LEARN_STATE		(4 << G_MISTP_STATE_SHIFT)
-#define  G_MISTP_FWD_STATE		(5 << G_MISTP_STATE_SHIFT)
-#define  G_MISTP_STATE_MASK		0x7
 
 #define CORE_SWMODE			0x0002c
 #define  SW_FWDG_MODE			(1 << 0)
@@ -205,75 +197,11 @@
 #define  BRCM_HDR_EN_P5			(1 << 1)
 #define  BRCM_HDR_EN_P7			(1 << 2)
 
-#define CORE_BRCM_HDR_CTRL2		0x0828
-
-#define CORE_HL_PRTC_CTRL		0x0940
-#define  ARP_EN				(1 << 0)
-#define  RARP_EN			(1 << 1)
-#define  DHCP_EN			(1 << 2)
-#define  ICMPV4_EN			(1 << 3)
-#define  ICMPV6_EN			(1 << 4)
-#define  ICMPV6_FWD_MODE		(1 << 5)
-#define  IGMP_DIP_EN			(1 << 8)
-#define  IGMP_RPTLVE_EN			(1 << 9)
-#define  IGMP_RTPLVE_FWD_MODE		(1 << 10)
-#define  IGMP_QRY_EN			(1 << 11)
-#define  IGMP_QRY_FWD_MODE		(1 << 12)
-#define  IGMP_UKN_EN			(1 << 13)
-#define  IGMP_UKN_FWD_MODE		(1 << 14)
-#define  MLD_RPTDONE_EN			(1 << 15)
-#define  MLD_RPTDONE_FWD_MODE		(1 << 16)
-#define  MLD_QRY_EN			(1 << 17)
-#define  MLD_QRY_FWD_MODE		(1 << 18)
-
 #define CORE_RST_MIB_CNT_EN		0x0950
 
 #define CORE_BRCM_HDR_RX_DIS		0x0980
 #define CORE_BRCM_HDR_TX_DIS		0x0988
 
-#define CORE_ARLA_NUM_ENTRIES		1024
-
-#define CORE_ARLA_RWCTL			0x1400
-#define  ARL_RW				(1 << 0)
-#define  IVL_SVL_SELECT			(1 << 6)
-#define  ARL_STRTDN			(1 << 7)
-
-#define CORE_ARLA_MAC			0x1408
-#define CORE_ARLA_VID			0x1420
-#define  ARLA_VIDTAB_INDX_MASK		0x1fff
-
-#define CORE_ARLA_MACVID0		0x1440
-#define  MAC_MASK			0xffffffffff
-#define  VID_SHIFT			48
-#define  VID_MASK			0xfff
-
-#define CORE_ARLA_FWD_ENTRY0		0x1460
-#define  PORTID_MASK			0x1ff
-#define  ARL_CON_SHIFT			9
-#define  ARL_CON_MASK			0x3
-#define  ARL_PRI_SHIFT			11
-#define  ARL_PRI_MASK			0x7
-#define  ARL_AGE			(1 << 14)
-#define  ARL_STATIC			(1 << 15)
-#define  ARL_VALID			(1 << 16)
-
-#define CORE_ARLA_MACVID_ENTRY(x)	(CORE_ARLA_MACVID0 + ((x) * 0x40))
-#define CORE_ARLA_FWD_ENTRY(x)		(CORE_ARLA_FWD_ENTRY0 + ((x) * 0x40))
-
-#define CORE_ARLA_SRCH_CTL		0x1540
-#define  ARLA_SRCH_VLID			(1 << 0)
-#define  IVL_SVL_SELECT			(1 << 6)
-#define  ARLA_SRCH_STDN			(1 << 7)
-
-#define CORE_ARLA_SRCH_ADR		0x1544
-#define  ARLA_SRCH_ADR_VALID		(1 << 15)
-
-#define CORE_ARLA_SRCH_RSLT_0_MACVID	0x1580
-#define CORE_ARLA_SRCH_RSLT_0		0x15a0
-
-#define CORE_ARLA_SRCH_RSLT_MACVID(x)	(CORE_ARLA_SRCH_RSLT_0_MACVID + ((x) * 0x40))
-#define CORE_ARLA_SRCH_RSLT(x)		(CORE_ARLA_SRCH_RSLT_0 + ((x) * 0x40))
-
 #define CORE_ARLA_VTBL_RWCTRL		0x1600
 #define  ARLA_VTBL_CMD_WRITE		0
 #define  ARLA_VTBL_CMD_READ		1
@@ -297,59 +225,9 @@
 #define  P_TXQ_PSM_VDD(x)		(P_TXQ_PSM_VDD_MASK << \
 					((x) * P_TXQ_PSM_VDD_SHIFT))
 
-#define	CORE_P0_MIB_OFFSET		0x8000
-#define P_MIB_SIZE			0x400
-#define CORE_P_MIB_OFFSET(x)		(CORE_P0_MIB_OFFSET + (x) * P_MIB_SIZE)
-
 #define CORE_PORT_VLAN_CTL_PORT(x)	(0xc400 + ((x) * 0x8))
 #define  PORT_VLAN_CTRL_MASK		0x1ff
 
-#define CORE_VLAN_CTRL0			0xd000
-#define  CHANGE_1P_VID_INNER		(1 << 0)
-#define  CHANGE_1P_VID_OUTER		(1 << 1)
-#define  CHANGE_1Q_VID			(1 << 3)
-#define  VLAN_LEARN_MODE_SVL		(0 << 5)
-#define  VLAN_LEARN_MODE_IVL		(3 << 5)
-#define  VLAN_EN			(1 << 7)
-
-#define CORE_VLAN_CTRL1			0xd004
-#define  EN_RSV_MCAST_FWDMAP		(1 << 2)
-#define  EN_RSV_MCAST_UNTAG		(1 << 3)
-#define  EN_IPMC_BYPASS_FWDMAP		(1 << 5)
-#define  EN_IPMC_BYPASS_UNTAG		(1 << 6)
-
-#define CORE_VLAN_CTRL2			0xd008
-#define  EN_MIIM_BYPASS_V_FWDMAP	(1 << 2)
-#define  EN_GMRP_GVRP_V_FWDMAP		(1 << 5)
-#define  EN_GMRP_GVRP_UNTAG_MAP		(1 << 6)
-
-#define CORE_VLAN_CTRL3			0xd00c
-#define  EN_DROP_NON1Q_MASK		0x1ff
-
-#define CORE_VLAN_CTRL4			0xd014
-#define  RESV_MCAST_FLOOD		(1 << 1)
-#define  EN_DOUBLE_TAG_MASK		0x3
-#define  EN_DOUBLE_TAG_SHIFT		2
-#define  EN_MGE_REV_GMRP		(1 << 4)
-#define  EN_MGE_REV_GVRP		(1 << 5)
-#define  INGR_VID_CHK_SHIFT		6
-#define  INGR_VID_CHK_MASK		0x3
-#define  INGR_VID_CHK_FWD		(0 << INGR_VID_CHK_SHIFT)
-#define  INGR_VID_CHK_DROP		(1 << INGR_VID_CHK_SHIFT)
-#define  INGR_VID_CHK_NO_CHK		(2 << INGR_VID_CHK_SHIFT)
-#define  INGR_VID_CHK_VID_VIOL_IMP	(3 << INGR_VID_CHK_SHIFT)
-
-#define CORE_VLAN_CTRL5			0xd018
-#define  EN_CPU_RX_BYP_INNER_CRCCHCK	(1 << 0)
-#define  EN_VID_FFF_FWD			(1 << 2)
-#define  DROP_VTABLE_MISS		(1 << 3)
-#define  EGRESS_DIR_FRM_BYP_TRUNK_EN	(1 << 4)
-#define  PRESV_NON1Q			(1 << 6)
-
-#define CORE_VLAN_CTRL6			0xd01c
-#define  STRICT_SFD_DETECT		(1 << 0)
-#define  DIS_ARL_BUST_LMIT		(1 << 4)
-
 #define CORE_DEFAULT_1Q_TAG_P(x)	(0xd040 + ((x) * 8))
 #define  CFI_SHIFT			12
 #define  PRI_SHIFT			13
diff --git a/drivers/net/dsa/mv88e6060.c b/drivers/net/dsa/mv88e6060.c
index e36b40886bd8..7ce36dbd9b62 100644
--- a/drivers/net/dsa/mv88e6060.c
+++ b/drivers/net/dsa/mv88e6060.c
@@ -19,7 +19,7 @@
 
 static int reg_read(struct dsa_switch *ds, int addr, int reg)
 {
-	struct mv88e6060_priv *priv = ds_to_priv(ds);
+	struct mv88e6060_priv *priv = ds->priv;
 
 	return mdiobus_read_nested(priv->bus, priv->sw_addr + addr, reg);
 }
@@ -37,7 +37,7 @@ static int reg_read(struct dsa_switch *ds, int addr, int reg)
 
 static int reg_write(struct dsa_switch *ds, int addr, int reg, u16 val)
 {
-	struct mv88e6060_priv *priv = ds_to_priv(ds);
+	struct mv88e6060_priv *priv = ds->priv;
 
 	return mdiobus_write_nested(priv->bus, priv->sw_addr + addr, reg, val);
 }
@@ -69,6 +69,11 @@ static const char *mv88e6060_get_name(struct mii_bus *bus, int sw_addr)
 	return NULL;
 }
 
+static enum dsa_tag_protocol mv88e6060_get_tag_protocol(struct dsa_switch *ds)
+{
+	return DSA_TAG_PROTO_TRAILER;
+}
+
 static const char *mv88e6060_drv_probe(struct device *dsa_dev,
 				       struct device *host_dev, int sw_addr,
 				       void **_priv)
@@ -247,8 +252,8 @@ mv88e6060_phy_write(struct dsa_switch *ds, int port, int regnum, u16 val)
 	return reg_write(ds, addr, regnum, val);
 }
 
-static struct dsa_switch_driver mv88e6060_switch_driver = {
-	.tag_protocol	= DSA_TAG_PROTO_TRAILER,
+static struct dsa_switch_ops mv88e6060_switch_ops = {
+	.get_tag_protocol = mv88e6060_get_tag_protocol,
 	.probe		= mv88e6060_drv_probe,
 	.setup		= mv88e6060_setup,
 	.set_addr	= mv88e6060_set_addr,
@@ -258,14 +263,14 @@ static struct dsa_switch_driver mv88e6060_switch_driver = {
 
 static int __init mv88e6060_init(void)
 {
-	register_switch_driver(&mv88e6060_switch_driver);
+	register_switch_driver(&mv88e6060_switch_ops);
 	return 0;
 }
 module_init(mv88e6060_init);
 
 static void __exit mv88e6060_cleanup(void)
 {
-	unregister_switch_driver(&mv88e6060_switch_driver);
+	unregister_switch_driver(&mv88e6060_switch_ops);
 }
 module_exit(mv88e6060_cleanup);
 
diff --git a/drivers/net/dsa/mv88e6xxx/Kconfig b/drivers/net/dsa/mv88e6xxx/Kconfig
index 490bc06f993e..486668813e15 100644
--- a/drivers/net/dsa/mv88e6xxx/Kconfig
+++ b/drivers/net/dsa/mv88e6xxx/Kconfig
@@ -2,6 +2,18 @@ config NET_DSA_MV88E6XXX
 	tristate "Marvell 88E6xxx Ethernet switch fabric support"
 	depends on NET_DSA
 	select NET_DSA_TAG_EDSA
+	select NET_DSA_TAG_DSA
 	help
 	  This driver adds support for most of the Marvell 88E6xxx models of
 	  Ethernet switch chips, except 88E6060.
+
+config NET_DSA_MV88E6XXX_GLOBAL2
+	bool "Switch Global 2 Registers support"
+	default y
+	depends on NET_DSA_MV88E6XXX
+	help
+	  This registers set at internal SMI address 0x1C provides extended
+	  features like EEPROM interface, trunking, cross-chip setup, etc.
+
+	  It is required on most chips. If the chip you compile the support for
+	  doesn't have such registers set, say N here. In doubt, say Y.
diff --git a/drivers/net/dsa/mv88e6xxx/Makefile b/drivers/net/dsa/mv88e6xxx/Makefile
index 6e29a75ee2f7..10ce820daa48 100644
--- a/drivers/net/dsa/mv88e6xxx/Makefile
+++ b/drivers/net/dsa/mv88e6xxx/Makefile
@@ -1 +1,4 @@
-obj-$(CONFIG_NET_DSA_MV88E6XXX) += chip.o
+obj-$(CONFIG_NET_DSA_MV88E6XXX) += mv88e6xxx.o
+mv88e6xxx-objs := chip.o
+mv88e6xxx-objs += global1.o
+mv88e6xxx-$(CONFIG_NET_DSA_MV88E6XXX_GLOBAL2) += global2.o
diff --git a/drivers/net/dsa/mv88e6xxx/chip.c b/drivers/net/dsa/mv88e6xxx/chip.c
index 710679067594..883fd9809dd2 100644
--- a/drivers/net/dsa/mv88e6xxx/chip.c
+++ b/drivers/net/dsa/mv88e6xxx/chip.c
@@ -29,7 +29,10 @@
 #include <linux/phy.h>
 #include <net/dsa.h>
 #include <net/switchdev.h>
+
 #include "mv88e6xxx.h"
+#include "global1.h"
+#include "global2.h"
 
 static void assert_reg_lock(struct mv88e6xxx_chip *chip)
 {
@@ -95,7 +98,7 @@ static int mv88e6xxx_smi_single_chip_write(struct mv88e6xxx_chip *chip,
 	return 0;
 }
 
-static const struct mv88e6xxx_ops mv88e6xxx_smi_single_chip_ops = {
+static const struct mv88e6xxx_bus_ops mv88e6xxx_smi_single_chip_ops = {
 	.read = mv88e6xxx_smi_single_chip_read,
 	.write = mv88e6xxx_smi_single_chip_write,
 };
@@ -177,13 +180,12 @@ static int mv88e6xxx_smi_multi_chip_write(struct mv88e6xxx_chip *chip,
 	return 0;
 }
 
-static const struct mv88e6xxx_ops mv88e6xxx_smi_multi_chip_ops = {
+static const struct mv88e6xxx_bus_ops mv88e6xxx_smi_multi_chip_ops = {
 	.read = mv88e6xxx_smi_multi_chip_read,
 	.write = mv88e6xxx_smi_multi_chip_write,
 };
 
-static int mv88e6xxx_read(struct mv88e6xxx_chip *chip,
-			  int addr, int reg, u16 *val)
+int mv88e6xxx_read(struct mv88e6xxx_chip *chip, int addr, int reg, u16 *val)
 {
 	int err;
 
@@ -199,8 +201,7 @@ static int mv88e6xxx_read(struct mv88e6xxx_chip *chip,
 	return 0;
 }
 
-static int mv88e6xxx_write(struct mv88e6xxx_chip *chip,
-			   int addr, int reg, u16 val)
+int mv88e6xxx_write(struct mv88e6xxx_chip *chip, int addr, int reg, u16 val)
 {
 	int err;
 
@@ -216,89 +217,172 @@ static int mv88e6xxx_write(struct mv88e6xxx_chip *chip,
 	return 0;
 }
 
-/* Indirect write to single pointer-data register with an Update bit */
-static int mv88e6xxx_update(struct mv88e6xxx_chip *chip, int addr, int reg,
-			    u16 update)
+static int mv88e6xxx_port_read(struct mv88e6xxx_chip *chip, int port, int reg,
+			       u16 *val)
 {
-	u16 val;
-	int i, err;
+	int addr = chip->info->port_base_addr + port;
 
-	/* Wait until the previous operation is completed */
-	for (i = 0; i < 16; ++i) {
-		err = mv88e6xxx_read(chip, addr, reg, &val);
-		if (err)
-			return err;
+	return mv88e6xxx_read(chip, addr, reg, val);
+}
 
-		if (!(val & BIT(15)))
-			break;
+static int mv88e6xxx_port_write(struct mv88e6xxx_chip *chip, int port, int reg,
+				u16 val)
+{
+	int addr = chip->info->port_base_addr + port;
+
+	return mv88e6xxx_write(chip, addr, reg, val);
+}
+
+static int mv88e6xxx_phy_read(struct mv88e6xxx_chip *chip, int phy,
+			      int reg, u16 *val)
+{
+	int addr = phy; /* PHY devices addresses start at 0x0 */
+
+	if (!chip->info->ops->phy_read)
+		return -EOPNOTSUPP;
+
+	return chip->info->ops->phy_read(chip, addr, reg, val);
+}
+
+static int mv88e6xxx_phy_write(struct mv88e6xxx_chip *chip, int phy,
+			       int reg, u16 val)
+{
+	int addr = phy; /* PHY devices addresses start at 0x0 */
+
+	if (!chip->info->ops->phy_write)
+		return -EOPNOTSUPP;
+
+	return chip->info->ops->phy_write(chip, addr, reg, val);
+}
+
+static int mv88e6xxx_phy_page_get(struct mv88e6xxx_chip *chip, int phy, u8 page)
+{
+	if (!mv88e6xxx_has(chip, MV88E6XXX_FLAG_PHY_PAGE))
+		return -EOPNOTSUPP;
+
+	return mv88e6xxx_phy_write(chip, phy, PHY_PAGE, page);
+}
+
+static void mv88e6xxx_phy_page_put(struct mv88e6xxx_chip *chip, int phy)
+{
+	int err;
+
+	/* Restore PHY page Copper 0x0 for access via the registered MDIO bus */
+	err = mv88e6xxx_phy_write(chip, phy, PHY_PAGE, PHY_PAGE_COPPER);
+	if (unlikely(err)) {
+		dev_err(chip->dev, "failed to restore PHY %d page Copper (%d)\n",
+			phy, err);
 	}
+}
+
+static int mv88e6xxx_phy_page_read(struct mv88e6xxx_chip *chip, int phy,
+				   u8 page, int reg, u16 *val)
+{
+	int err;
 
-	if (i == 16)
-		return -ETIMEDOUT;
+	/* There is no paging for registers 22 */
+	if (reg == PHY_PAGE)
+		return -EINVAL;
 
-	/* Set the Update bit to trigger a write operation */
-	val = BIT(15) | update;
+	err = mv88e6xxx_phy_page_get(chip, phy, page);
+	if (!err) {
+		err = mv88e6xxx_phy_read(chip, phy, reg, val);
+		mv88e6xxx_phy_page_put(chip, phy);
+	}
 
-	return mv88e6xxx_write(chip, addr, reg, val);
+	return err;
 }
 
-static int _mv88e6xxx_reg_read(struct mv88e6xxx_chip *chip, int addr, int reg)
+static int mv88e6xxx_phy_page_write(struct mv88e6xxx_chip *chip, int phy,
+				    u8 page, int reg, u16 val)
 {
-	u16 val;
 	int err;
 
-	err = mv88e6xxx_read(chip, addr, reg, &val);
-	if (err)
-		return err;
+	/* There is no paging for registers 22 */
+	if (reg == PHY_PAGE)
+		return -EINVAL;
 
-	return val;
+	err = mv88e6xxx_phy_page_get(chip, phy, page);
+	if (!err) {
+		err = mv88e6xxx_phy_write(chip, phy, PHY_PAGE, page);
+		mv88e6xxx_phy_page_put(chip, phy);
+	}
+
+	return err;
 }
 
-static int _mv88e6xxx_reg_write(struct mv88e6xxx_chip *chip, int addr,
-				int reg, u16 val)
+static int mv88e6xxx_serdes_read(struct mv88e6xxx_chip *chip, int reg, u16 *val)
 {
-	return mv88e6xxx_write(chip, addr, reg, val);
+	return mv88e6xxx_phy_page_read(chip, ADDR_SERDES, SERDES_PAGE_FIBER,
+				       reg, val);
 }
 
-static int mv88e6xxx_mdio_read_direct(struct mv88e6xxx_chip *chip,
-				      int addr, int regnum)
+static int mv88e6xxx_serdes_write(struct mv88e6xxx_chip *chip, int reg, u16 val)
 {
-	if (addr >= 0)
-		return _mv88e6xxx_reg_read(chip, addr, regnum);
-	return 0xffff;
+	return mv88e6xxx_phy_page_write(chip, ADDR_SERDES, SERDES_PAGE_FIBER,
+					reg, val);
 }
 
-static int mv88e6xxx_mdio_write_direct(struct mv88e6xxx_chip *chip,
-				       int addr, int regnum, u16 val)
+int mv88e6xxx_wait(struct mv88e6xxx_chip *chip, int addr, int reg, u16 mask)
 {
-	if (addr >= 0)
-		return _mv88e6xxx_reg_write(chip, addr, regnum, val);
-	return 0;
+	int i;
+
+	for (i = 0; i < 16; i++) {
+		u16 val;
+		int err;
+
+		err = mv88e6xxx_read(chip, addr, reg, &val);
+		if (err)
+			return err;
+
+		if (!(val & mask))
+			return 0;
+
+		usleep_range(1000, 2000);
+	}
+
+	dev_err(chip->dev, "Timeout while waiting for switch\n");
+	return -ETIMEDOUT;
+}
+
+/* Indirect write to single pointer-data register with an Update bit */
+int mv88e6xxx_update(struct mv88e6xxx_chip *chip, int addr, int reg, u16 update)
+{
+	u16 val;
+	int err;
+
+	/* Wait until the previous operation is completed */
+	err = mv88e6xxx_wait(chip, addr, reg, BIT(15));
+	if (err)
+		return err;
+
+	/* Set the Update bit to trigger a write operation */
+	val = BIT(15) | update;
+
+	return mv88e6xxx_write(chip, addr, reg, val);
 }
 
 static int mv88e6xxx_ppu_disable(struct mv88e6xxx_chip *chip)
 {
-	int ret;
-	unsigned long timeout;
+	u16 val;
+	int i, err;
 
-	ret = _mv88e6xxx_reg_read(chip, REG_GLOBAL, GLOBAL_CONTROL);
-	if (ret < 0)
-		return ret;
+	err = mv88e6xxx_g1_read(chip, GLOBAL_CONTROL, &val);
+	if (err)
+		return err;
 
-	ret = _mv88e6xxx_reg_write(chip, REG_GLOBAL, GLOBAL_CONTROL,
-				   ret & ~GLOBAL_CONTROL_PPU_ENABLE);
-	if (ret)
-		return ret;
+	err = mv88e6xxx_g1_write(chip, GLOBAL_CONTROL,
+				 val & ~GLOBAL_CONTROL_PPU_ENABLE);
+	if (err)
+		return err;
 
-	timeout = jiffies + 1 * HZ;
-	while (time_before(jiffies, timeout)) {
-		ret = _mv88e6xxx_reg_read(chip, REG_GLOBAL, GLOBAL_STATUS);
-		if (ret < 0)
-			return ret;
+	for (i = 0; i < 16; i++) {
+		err = mv88e6xxx_g1_read(chip, GLOBAL_STATUS, &val);
+		if (err)
+			return err;
 
 		usleep_range(1000, 2000);
-		if ((ret & GLOBAL_STATUS_PPU_MASK) !=
-		    GLOBAL_STATUS_PPU_POLLING)
+		if ((val & GLOBAL_STATUS_PPU_MASK) != GLOBAL_STATUS_PPU_POLLING)
 			return 0;
 	}
 
@@ -307,27 +391,25 @@ static int mv88e6xxx_ppu_disable(struct mv88e6xxx_chip *chip)
 
 static int mv88e6xxx_ppu_enable(struct mv88e6xxx_chip *chip)
 {
-	int ret, err;
-	unsigned long timeout;
+	u16 val;
+	int i, err;
 
-	ret = _mv88e6xxx_reg_read(chip, REG_GLOBAL, GLOBAL_CONTROL);
-	if (ret < 0)
-		return ret;
+	err = mv88e6xxx_g1_read(chip, GLOBAL_CONTROL, &val);
+	if (err)
+		return err;
 
-	err = _mv88e6xxx_reg_write(chip, REG_GLOBAL, GLOBAL_CONTROL,
-				   ret | GLOBAL_CONTROL_PPU_ENABLE);
+	err = mv88e6xxx_g1_write(chip, GLOBAL_CONTROL,
+				 val | GLOBAL_CONTROL_PPU_ENABLE);
 	if (err)
 		return err;
 
-	timeout = jiffies + 1 * HZ;
-	while (time_before(jiffies, timeout)) {
-		ret = _mv88e6xxx_reg_read(chip, REG_GLOBAL, GLOBAL_STATUS);
-		if (ret < 0)
-			return ret;
+	for (i = 0; i < 16; i++) {
+		err = mv88e6xxx_g1_read(chip, GLOBAL_STATUS, &val);
+		if (err)
+			return err;
 
 		usleep_range(1000, 2000);
-		if ((ret & GLOBAL_STATUS_PPU_MASK) ==
-		    GLOBAL_STATUS_PPU_POLLING)
+		if ((val & GLOBAL_STATUS_PPU_MASK) == GLOBAL_STATUS_PPU_POLLING)
 			return 0;
 	}
 
@@ -400,32 +482,37 @@ static void mv88e6xxx_ppu_state_init(struct mv88e6xxx_chip *chip)
 	chip->ppu_timer.function = mv88e6xxx_ppu_reenable_timer;
 }
 
-static int mv88e6xxx_mdio_read_ppu(struct mv88e6xxx_chip *chip, int addr,
-				   int regnum)
+static void mv88e6xxx_ppu_state_destroy(struct mv88e6xxx_chip *chip)
 {
-	int ret;
+	del_timer_sync(&chip->ppu_timer);
+}
+
+static int mv88e6xxx_phy_ppu_read(struct mv88e6xxx_chip *chip, int addr,
+				  int reg, u16 *val)
+{
+	int err;
 
-	ret = mv88e6xxx_ppu_access_get(chip);
-	if (ret >= 0) {
-		ret = _mv88e6xxx_reg_read(chip, addr, regnum);
+	err = mv88e6xxx_ppu_access_get(chip);
+	if (!err) {
+		err = mv88e6xxx_read(chip, addr, reg, val);
 		mv88e6xxx_ppu_access_put(chip);
 	}
 
-	return ret;
+	return err;
 }
 
-static int mv88e6xxx_mdio_write_ppu(struct mv88e6xxx_chip *chip, int addr,
-				    int regnum, u16 val)
+static int mv88e6xxx_phy_ppu_write(struct mv88e6xxx_chip *chip, int addr,
+				   int reg, u16 val)
 {
-	int ret;
+	int err;
 
-	ret = mv88e6xxx_ppu_access_get(chip);
-	if (ret >= 0) {
-		ret = _mv88e6xxx_reg_write(chip, addr, regnum, val);
+	err = mv88e6xxx_ppu_access_get(chip);
+	if (!err) {
+		err = mv88e6xxx_write(chip, addr, reg, val);
 		mv88e6xxx_ppu_access_put(chip);
 	}
 
-	return ret;
+	return err;
 }
 
 static bool mv88e6xxx_6065_family(struct mv88e6xxx_chip *chip)
@@ -468,21 +555,6 @@ static bool mv88e6xxx_6352_family(struct mv88e6xxx_chip *chip)
 	return chip->info->family == MV88E6XXX_FAMILY_6352;
 }
 
-static unsigned int mv88e6xxx_num_databases(struct mv88e6xxx_chip *chip)
-{
-	return chip->info->num_databases;
-}
-
-static bool mv88e6xxx_has_fid_reg(struct mv88e6xxx_chip *chip)
-{
-	/* Does the device have dedicated FID registers for ATU and VTU ops? */
-	if (mv88e6xxx_6097_family(chip) || mv88e6xxx_6165_family(chip) ||
-	    mv88e6xxx_6351_family(chip) || mv88e6xxx_6352_family(chip))
-		return true;
-
-	return false;
-}
-
 /* We expect the switch to perform auto negotiation if there is a real
  * phy. However, in the case of a fixed link phy, we force the port
  * settings from the fixed link settings.
@@ -490,24 +562,24 @@ static bool mv88e6xxx_has_fid_reg(struct mv88e6xxx_chip *chip)
 static void mv88e6xxx_adjust_link(struct dsa_switch *ds, int port,
 				  struct phy_device *phydev)
 {
-	struct mv88e6xxx_chip *chip = ds_to_priv(ds);
-	u32 reg;
-	int ret;
+	struct mv88e6xxx_chip *chip = ds->priv;
+	u16 reg;
+	int err;
 
 	if (!phy_is_pseudo_fixed_link(phydev))
 		return;
 
 	mutex_lock(&chip->reg_lock);
 
-	ret = _mv88e6xxx_reg_read(chip, REG_PORT(port), PORT_PCS_CTRL);
-	if (ret < 0)
+	err = mv88e6xxx_port_read(chip, port, PORT_PCS_CTRL, &reg);
+	if (err)
 		goto out;
 
-	reg = ret & ~(PORT_PCS_CTRL_LINK_UP |
-		      PORT_PCS_CTRL_FORCE_LINK |
-		      PORT_PCS_CTRL_DUPLEX_FULL |
-		      PORT_PCS_CTRL_FORCE_DUPLEX |
-		      PORT_PCS_CTRL_UNFORCED);
+	reg &= ~(PORT_PCS_CTRL_LINK_UP |
+		 PORT_PCS_CTRL_FORCE_LINK |
+		 PORT_PCS_CTRL_DUPLEX_FULL |
+		 PORT_PCS_CTRL_FORCE_DUPLEX |
+		 PORT_PCS_CTRL_UNFORCED);
 
 	reg |= PORT_PCS_CTRL_FORCE_LINK;
 	if (phydev->link)
@@ -536,7 +608,7 @@ static void mv88e6xxx_adjust_link(struct dsa_switch *ds, int port,
 		reg |= PORT_PCS_CTRL_DUPLEX_FULL;
 
 	if ((mv88e6xxx_6352_family(chip) || mv88e6xxx_6351_family(chip)) &&
-	    (port >= chip->info->num_ports - 2)) {
+	    (port >= mv88e6xxx_num_ports(chip) - 2)) {
 		if (phydev->interface == PHY_INTERFACE_MODE_RGMII_RXID)
 			reg |= PORT_PCS_CTRL_RGMII_DELAY_RXCLK;
 		if (phydev->interface == PHY_INTERFACE_MODE_RGMII_TXID)
@@ -545,7 +617,7 @@ static void mv88e6xxx_adjust_link(struct dsa_switch *ds, int port,
 			reg |= (PORT_PCS_CTRL_RGMII_DELAY_RXCLK |
 				PORT_PCS_CTRL_RGMII_DELAY_TXCLK);
 	}
-	_mv88e6xxx_reg_write(chip, REG_PORT(port), PORT_PCS_CTRL, reg);
+	mv88e6xxx_port_write(chip, port, PORT_PCS_CTRL, reg);
 
 out:
 	mutex_unlock(&chip->reg_lock);
@@ -553,12 +625,12 @@ out:
 
 static int _mv88e6xxx_stats_wait(struct mv88e6xxx_chip *chip)
 {
-	int ret;
-	int i;
+	u16 val;
+	int i, err;
 
 	for (i = 0; i < 10; i++) {
-		ret = _mv88e6xxx_reg_read(chip, REG_GLOBAL, GLOBAL_STATS_OP);
-		if ((ret & GLOBAL_STATS_OP_BUSY) == 0)
+		err = mv88e6xxx_g1_read(chip, GLOBAL_STATS_OP, &val);
+		if ((val & GLOBAL_STATS_OP_BUSY) == 0)
 			return 0;
 	}
 
@@ -567,55 +639,52 @@ static int _mv88e6xxx_stats_wait(struct mv88e6xxx_chip *chip)
 
 static int _mv88e6xxx_stats_snapshot(struct mv88e6xxx_chip *chip, int port)
 {
-	int ret;
+	int err;
 
 	if (mv88e6xxx_6320_family(chip) || mv88e6xxx_6352_family(chip))
 		port = (port + 1) << 5;
 
 	/* Snapshot the hardware statistics counters for this port. */
-	ret = _mv88e6xxx_reg_write(chip, REG_GLOBAL, GLOBAL_STATS_OP,
-				   GLOBAL_STATS_OP_CAPTURE_PORT |
-				   GLOBAL_STATS_OP_HIST_RX_TX | port);
-	if (ret < 0)
-		return ret;
+	err = mv88e6xxx_g1_write(chip, GLOBAL_STATS_OP,
+				 GLOBAL_STATS_OP_CAPTURE_PORT |
+				 GLOBAL_STATS_OP_HIST_RX_TX | port);
+	if (err)
+		return err;
 
 	/* Wait for the snapshotting to complete. */
-	ret = _mv88e6xxx_stats_wait(chip);
-	if (ret < 0)
-		return ret;
-
-	return 0;
+	return _mv88e6xxx_stats_wait(chip);
 }
 
 static void _mv88e6xxx_stats_read(struct mv88e6xxx_chip *chip,
 				  int stat, u32 *val)
 {
-	u32 _val;
-	int ret;
+	u32 value;
+	u16 reg;
+	int err;
 
 	*val = 0;
 
-	ret = _mv88e6xxx_reg_write(chip, REG_GLOBAL, GLOBAL_STATS_OP,
-				   GLOBAL_STATS_OP_READ_CAPTURED |
-				   GLOBAL_STATS_OP_HIST_RX_TX | stat);
-	if (ret < 0)
+	err = mv88e6xxx_g1_write(chip, GLOBAL_STATS_OP,
+				 GLOBAL_STATS_OP_READ_CAPTURED |
+				 GLOBAL_STATS_OP_HIST_RX_TX | stat);
+	if (err)
 		return;
 
-	ret = _mv88e6xxx_stats_wait(chip);
-	if (ret < 0)
+	err = _mv88e6xxx_stats_wait(chip);
+	if (err)
 		return;
 
-	ret = _mv88e6xxx_reg_read(chip, REG_GLOBAL, GLOBAL_STATS_COUNTER_32);
-	if (ret < 0)
+	err = mv88e6xxx_g1_read(chip, GLOBAL_STATS_COUNTER_32, &reg);
+	if (err)
 		return;
 
-	_val = ret << 16;
+	value = reg << 16;
 
-	ret = _mv88e6xxx_reg_read(chip, REG_GLOBAL, GLOBAL_STATS_COUNTER_01);
-	if (ret < 0)
+	err = mv88e6xxx_g1_read(chip, GLOBAL_STATS_COUNTER_01, &reg);
+	if (err)
 		return;
 
-	*val = _val | ret;
+	*val = value | reg;
 }
 
 static struct mv88e6xxx_hw_stat mv88e6xxx_hw_stats[] = {
@@ -705,22 +774,22 @@ static uint64_t _mv88e6xxx_get_ethtool_stat(struct mv88e6xxx_chip *chip,
 {
 	u32 low;
 	u32 high = 0;
-	int ret;
+	int err;
+	u16 reg;
 	u64 value;
 
 	switch (s->type) {
 	case PORT:
-		ret = _mv88e6xxx_reg_read(chip, REG_PORT(port), s->reg);
-		if (ret < 0)
+		err = mv88e6xxx_port_read(chip, port, s->reg, &reg);
+		if (err)
 			return UINT64_MAX;
 
-		low = ret;
+		low = reg;
 		if (s->sizeof_stat == 4) {
-			ret = _mv88e6xxx_reg_read(chip, REG_PORT(port),
-						  s->reg + 1);
-			if (ret < 0)
+			err = mv88e6xxx_port_read(chip, port, s->reg + 1, &reg);
+			if (err)
 				return UINT64_MAX;
-			high = ret;
+			high = reg;
 		}
 		break;
 	case BANK0:
@@ -736,7 +805,7 @@ static uint64_t _mv88e6xxx_get_ethtool_stat(struct mv88e6xxx_chip *chip,
 static void mv88e6xxx_get_strings(struct dsa_switch *ds, int port,
 				  uint8_t *data)
 {
-	struct mv88e6xxx_chip *chip = ds_to_priv(ds);
+	struct mv88e6xxx_chip *chip = ds->priv;
 	struct mv88e6xxx_hw_stat *stat;
 	int i, j;
 
@@ -752,7 +821,7 @@ static void mv88e6xxx_get_strings(struct dsa_switch *ds, int port,
 
 static int mv88e6xxx_get_sset_count(struct dsa_switch *ds)
 {
-	struct mv88e6xxx_chip *chip = ds_to_priv(ds);
+	struct mv88e6xxx_chip *chip = ds->priv;
 	struct mv88e6xxx_hw_stat *stat;
 	int i, j;
 
@@ -767,7 +836,7 @@ static int mv88e6xxx_get_sset_count(struct dsa_switch *ds)
 static void mv88e6xxx_get_ethtool_stats(struct dsa_switch *ds, int port,
 					uint64_t *data)
 {
-	struct mv88e6xxx_chip *chip = ds_to_priv(ds);
+	struct mv88e6xxx_chip *chip = ds->priv;
 	struct mv88e6xxx_hw_stat *stat;
 	int ret;
 	int i, j;
@@ -798,7 +867,9 @@ static int mv88e6xxx_get_regs_len(struct dsa_switch *ds, int port)
 static void mv88e6xxx_get_regs(struct dsa_switch *ds, int port,
 			       struct ethtool_regs *regs, void *_p)
 {
-	struct mv88e6xxx_chip *chip = ds_to_priv(ds);
+	struct mv88e6xxx_chip *chip = ds->priv;
+	int err;
+	u16 reg;
 	u16 *p = _p;
 	int i;
 
@@ -809,170 +880,106 @@ static void mv88e6xxx_get_regs(struct dsa_switch *ds, int port,
 	mutex_lock(&chip->reg_lock);
 
 	for (i = 0; i < 32; i++) {
-		int ret;
 
-		ret = _mv88e6xxx_reg_read(chip, REG_PORT(port), i);
-		if (ret >= 0)
-			p[i] = ret;
+		err = mv88e6xxx_port_read(chip, port, i, &reg);
+		if (!err)
+			p[i] = reg;
 	}
 
 	mutex_unlock(&chip->reg_lock);
 }
 
-static int _mv88e6xxx_wait(struct mv88e6xxx_chip *chip, int reg, int offset,
-			   u16 mask)
-{
-	unsigned long timeout = jiffies + HZ / 10;
-
-	while (time_before(jiffies, timeout)) {
-		int ret;
-
-		ret = _mv88e6xxx_reg_read(chip, reg, offset);
-		if (ret < 0)
-			return ret;
-		if (!(ret & mask))
-			return 0;
-
-		usleep_range(1000, 2000);
-	}
-	return -ETIMEDOUT;
-}
-
-static int mv88e6xxx_mdio_wait(struct mv88e6xxx_chip *chip)
-{
-	return _mv88e6xxx_wait(chip, REG_GLOBAL2, GLOBAL2_SMI_OP,
-			       GLOBAL2_SMI_OP_BUSY);
-}
-
 static int _mv88e6xxx_atu_wait(struct mv88e6xxx_chip *chip)
 {
-	return _mv88e6xxx_wait(chip, REG_GLOBAL, GLOBAL_ATU_OP,
-			       GLOBAL_ATU_OP_BUSY);
-}
-
-static int mv88e6xxx_mdio_read_indirect(struct mv88e6xxx_chip *chip,
-					int addr, int regnum)
-{
-	int ret;
-
-	ret = _mv88e6xxx_reg_write(chip, REG_GLOBAL2, GLOBAL2_SMI_OP,
-				   GLOBAL2_SMI_OP_22_READ | (addr << 5) |
-				   regnum);
-	if (ret < 0)
-		return ret;
-
-	ret = mv88e6xxx_mdio_wait(chip);
-	if (ret < 0)
-		return ret;
-
-	ret = _mv88e6xxx_reg_read(chip, REG_GLOBAL2, GLOBAL2_SMI_DATA);
-
-	return ret;
-}
-
-static int mv88e6xxx_mdio_write_indirect(struct mv88e6xxx_chip *chip,
-					 int addr, int regnum, u16 val)
-{
-	int ret;
-
-	ret = _mv88e6xxx_reg_write(chip, REG_GLOBAL2, GLOBAL2_SMI_DATA, val);
-	if (ret < 0)
-		return ret;
-
-	ret = _mv88e6xxx_reg_write(chip, REG_GLOBAL2, GLOBAL2_SMI_OP,
-				   GLOBAL2_SMI_OP_22_WRITE | (addr << 5) |
-				   regnum);
-
-	return mv88e6xxx_mdio_wait(chip);
+	return mv88e6xxx_g1_wait(chip, GLOBAL_ATU_OP, GLOBAL_ATU_OP_BUSY);
 }
 
 static int mv88e6xxx_get_eee(struct dsa_switch *ds, int port,
 			     struct ethtool_eee *e)
 {
-	struct mv88e6xxx_chip *chip = ds_to_priv(ds);
-	int reg;
+	struct mv88e6xxx_chip *chip = ds->priv;
+	u16 reg;
+	int err;
 
 	if (!mv88e6xxx_has(chip, MV88E6XXX_FLAG_EEE))
 		return -EOPNOTSUPP;
 
 	mutex_lock(&chip->reg_lock);
 
-	reg = mv88e6xxx_mdio_read_indirect(chip, port, 16);
-	if (reg < 0)
+	err = mv88e6xxx_phy_read(chip, port, 16, &reg);
+	if (err)
 		goto out;
 
 	e->eee_enabled = !!(reg & 0x0200);
 	e->tx_lpi_enabled = !!(reg & 0x0100);
 
-	reg = _mv88e6xxx_reg_read(chip, REG_PORT(port), PORT_STATUS);
-	if (reg < 0)
+	err = mv88e6xxx_port_read(chip, port, PORT_STATUS, &reg);
+	if (err)
 		goto out;
 
 	e->eee_active = !!(reg & PORT_STATUS_EEE);
-	reg = 0;
-
 out:
 	mutex_unlock(&chip->reg_lock);
-	return reg;
+
+	return err;
 }
 
 static int mv88e6xxx_set_eee(struct dsa_switch *ds, int port,
 			     struct phy_device *phydev, struct ethtool_eee *e)
 {
-	struct mv88e6xxx_chip *chip = ds_to_priv(ds);
-	int reg;
-	int ret;
+	struct mv88e6xxx_chip *chip = ds->priv;
+	u16 reg;
+	int err;
 
 	if (!mv88e6xxx_has(chip, MV88E6XXX_FLAG_EEE))
 		return -EOPNOTSUPP;
 
 	mutex_lock(&chip->reg_lock);
 
-	ret = mv88e6xxx_mdio_read_indirect(chip, port, 16);
-	if (ret < 0)
+	err = mv88e6xxx_phy_read(chip, port, 16, &reg);
+	if (err)
 		goto out;
 
-	reg = ret & ~0x0300;
+	reg &= ~0x0300;
 	if (e->eee_enabled)
 		reg |= 0x0200;
 	if (e->tx_lpi_enabled)
 		reg |= 0x0100;
 
-	ret = mv88e6xxx_mdio_write_indirect(chip, port, 16, reg);
+	err = mv88e6xxx_phy_write(chip, port, 16, reg);
 out:
 	mutex_unlock(&chip->reg_lock);
 
-	return ret;
+	return err;
 }
 
 static int _mv88e6xxx_atu_cmd(struct mv88e6xxx_chip *chip, u16 fid, u16 cmd)
 {
-	int ret;
+	u16 val;
+	int err;
 
-	if (mv88e6xxx_has_fid_reg(chip)) {
-		ret = _mv88e6xxx_reg_write(chip, REG_GLOBAL, GLOBAL_ATU_FID,
-					   fid);
-		if (ret < 0)
-			return ret;
+	if (mv88e6xxx_has(chip, MV88E6XXX_FLAG_G1_ATU_FID)) {
+		err = mv88e6xxx_g1_write(chip, GLOBAL_ATU_FID, fid);
+		if (err)
+			return err;
 	} else if (mv88e6xxx_num_databases(chip) == 256) {
 		/* ATU DBNum[7:4] are located in ATU Control 15:12 */
-		ret = _mv88e6xxx_reg_read(chip, REG_GLOBAL, GLOBAL_ATU_CONTROL);
-		if (ret < 0)
-			return ret;
+		err = mv88e6xxx_g1_read(chip, GLOBAL_ATU_CONTROL, &val);
+		if (err)
+			return err;
 
-		ret = _mv88e6xxx_reg_write(chip, REG_GLOBAL, GLOBAL_ATU_CONTROL,
-					   (ret & 0xfff) |
-					   ((fid << 8) & 0xf000));
-		if (ret < 0)
-			return ret;
+		err = mv88e6xxx_g1_write(chip, GLOBAL_ATU_CONTROL,
+					 (val & 0xfff) | ((fid << 8) & 0xf000));
+		if (err)
+			return err;
 
 		/* ATU DBNum[3:0] are located in ATU Operation 3:0 */
 		cmd |= fid & 0xf;
 	}
 
-	ret = _mv88e6xxx_reg_write(chip, REG_GLOBAL, GLOBAL_ATU_OP, cmd);
-	if (ret < 0)
-		return ret;
+	err = mv88e6xxx_g1_write(chip, GLOBAL_ATU_OP, cmd);
+	if (err)
+		return err;
 
 	return _mv88e6xxx_atu_wait(chip);
 }
@@ -997,7 +1004,7 @@ static int _mv88e6xxx_atu_data_write(struct mv88e6xxx_chip *chip,
 		data |= (entry->portv_trunkid << shift) & mask;
 	}
 
-	return _mv88e6xxx_reg_write(chip, REG_GLOBAL, GLOBAL_ATU_DATA, data);
+	return mv88e6xxx_g1_write(chip, GLOBAL_ATU_DATA, data);
 }
 
 static int _mv88e6xxx_atu_flush_move(struct mv88e6xxx_chip *chip,
@@ -1073,57 +1080,45 @@ static int _mv88e6xxx_port_state(struct mv88e6xxx_chip *chip, int port,
 				 u8 state)
 {
 	struct dsa_switch *ds = chip->ds;
-	int reg, ret = 0;
+	u16 reg;
+	int err;
 	u8 oldstate;
 
-	reg = _mv88e6xxx_reg_read(chip, REG_PORT(port), PORT_CONTROL);
-	if (reg < 0)
-		return reg;
+	err = mv88e6xxx_port_read(chip, port, PORT_CONTROL, &reg);
+	if (err)
+		return err;
 
 	oldstate = reg & PORT_CONTROL_STATE_MASK;
 
-	if (oldstate != state) {
-		/* Flush forwarding database if we're moving a port
-		 * from Learning or Forwarding state to Disabled or
-		 * Blocking or Listening state.
-		 */
-		if ((oldstate == PORT_CONTROL_STATE_LEARNING ||
-		     oldstate == PORT_CONTROL_STATE_FORWARDING) &&
-		    (state == PORT_CONTROL_STATE_DISABLED ||
-		     state == PORT_CONTROL_STATE_BLOCKING)) {
-			ret = _mv88e6xxx_atu_remove(chip, 0, port, false);
-			if (ret)
-				return ret;
-		}
+	reg &= ~PORT_CONTROL_STATE_MASK;
+	reg |= state;
 
-		reg = (reg & ~PORT_CONTROL_STATE_MASK) | state;
-		ret = _mv88e6xxx_reg_write(chip, REG_PORT(port), PORT_CONTROL,
-					   reg);
-		if (ret)
-			return ret;
+	err = mv88e6xxx_port_write(chip, port, PORT_CONTROL, reg);
+	if (err)
+		return err;
 
-		netdev_dbg(ds->ports[port].netdev, "PortState %s (was %s)\n",
-			   mv88e6xxx_port_state_names[state],
-			   mv88e6xxx_port_state_names[oldstate]);
-	}
+	netdev_dbg(ds->ports[port].netdev, "PortState %s (was %s)\n",
+		   mv88e6xxx_port_state_names[state],
+		   mv88e6xxx_port_state_names[oldstate]);
 
-	return ret;
+	return 0;
 }
 
 static int _mv88e6xxx_port_based_vlan_map(struct mv88e6xxx_chip *chip, int port)
 {
 	struct net_device *bridge = chip->ports[port].bridge_dev;
-	const u16 mask = (1 << chip->info->num_ports) - 1;
+	const u16 mask = (1 << mv88e6xxx_num_ports(chip)) - 1;
 	struct dsa_switch *ds = chip->ds;
 	u16 output_ports = 0;
-	int reg;
+	u16 reg;
+	int err;
 	int i;
 
 	/* allow CPU port or DSA link(s) to send frames to every port */
 	if (dsa_is_cpu_port(ds, port) || dsa_is_dsa_port(ds, port)) {
 		output_ports = mask;
 	} else {
-		for (i = 0; i < chip->info->num_ports; ++i) {
+		for (i = 0; i < mv88e6xxx_num_ports(chip); ++i) {
 			/* allow sending frames to every group member */
 			if (bridge && chip->ports[i].bridge_dev == bridge)
 				output_ports |= BIT(i);
@@ -1137,20 +1132,20 @@ static int _mv88e6xxx_port_based_vlan_map(struct mv88e6xxx_chip *chip, int port)
 	/* prevent frames from going back out of the port they came in on */
 	output_ports &= ~BIT(port);
 
-	reg = _mv88e6xxx_reg_read(chip, REG_PORT(port), PORT_BASE_VLAN);
-	if (reg < 0)
-		return reg;
+	err = mv88e6xxx_port_read(chip, port, PORT_BASE_VLAN, &reg);
+	if (err)
+		return err;
 
 	reg &= ~mask;
 	reg |= output_ports & mask;
 
-	return _mv88e6xxx_reg_write(chip, REG_PORT(port), PORT_BASE_VLAN, reg);
+	return mv88e6xxx_port_write(chip, port, PORT_BASE_VLAN, reg);
 }
 
 static void mv88e6xxx_port_stp_state_set(struct dsa_switch *ds, int port,
 					 u8 state)
 {
-	struct mv88e6xxx_chip *chip = ds_to_priv(ds);
+	struct mv88e6xxx_chip *chip = ds->priv;
 	int stp_state;
 	int err;
 
@@ -1181,27 +1176,39 @@ static void mv88e6xxx_port_stp_state_set(struct dsa_switch *ds, int port,
 			   mv88e6xxx_port_state_names[stp_state]);
 }
 
+static void mv88e6xxx_port_fast_age(struct dsa_switch *ds, int port)
+{
+	struct mv88e6xxx_chip *chip = ds->priv;
+	int err;
+
+	mutex_lock(&chip->reg_lock);
+	err = _mv88e6xxx_atu_remove(chip, 0, port, false);
+	mutex_unlock(&chip->reg_lock);
+
+	if (err)
+		netdev_err(ds->ports[port].netdev, "failed to flush ATU\n");
+}
+
 static int _mv88e6xxx_port_pvid(struct mv88e6xxx_chip *chip, int port,
 				u16 *new, u16 *old)
 {
 	struct dsa_switch *ds = chip->ds;
-	u16 pvid;
-	int ret;
+	u16 pvid, reg;
+	int err;
 
-	ret = _mv88e6xxx_reg_read(chip, REG_PORT(port), PORT_DEFAULT_VLAN);
-	if (ret < 0)
-		return ret;
+	err = mv88e6xxx_port_read(chip, port, PORT_DEFAULT_VLAN, &reg);
+	if (err)
+		return err;
 
-	pvid = ret & PORT_DEFAULT_VLAN_MASK;
+	pvid = reg & PORT_DEFAULT_VLAN_MASK;
 
 	if (new) {
-		ret &= ~PORT_DEFAULT_VLAN_MASK;
-		ret |= *new & PORT_DEFAULT_VLAN_MASK;
+		reg &= ~PORT_DEFAULT_VLAN_MASK;
+		reg |= *new & PORT_DEFAULT_VLAN_MASK;
 
-		ret = _mv88e6xxx_reg_write(chip, REG_PORT(port),
-					   PORT_DEFAULT_VLAN, ret);
-		if (ret < 0)
-			return ret;
+		err = mv88e6xxx_port_write(chip, port, PORT_DEFAULT_VLAN, reg);
+		if (err)
+			return err;
 
 		netdev_dbg(ds->ports[port].netdev,
 			   "DefaultVID %d (was %d)\n", *new, pvid);
@@ -1227,17 +1234,16 @@ static int _mv88e6xxx_port_pvid_set(struct mv88e6xxx_chip *chip,
 
 static int _mv88e6xxx_vtu_wait(struct mv88e6xxx_chip *chip)
 {
-	return _mv88e6xxx_wait(chip, REG_GLOBAL, GLOBAL_VTU_OP,
-			       GLOBAL_VTU_OP_BUSY);
+	return mv88e6xxx_g1_wait(chip, GLOBAL_VTU_OP, GLOBAL_VTU_OP_BUSY);
 }
 
 static int _mv88e6xxx_vtu_cmd(struct mv88e6xxx_chip *chip, u16 op)
 {
-	int ret;
+	int err;
 
-	ret = _mv88e6xxx_reg_write(chip, REG_GLOBAL, GLOBAL_VTU_OP, op);
-	if (ret < 0)
-		return ret;
+	err = mv88e6xxx_g1_write(chip, GLOBAL_VTU_OP, op);
+	if (err)
+		return err;
 
 	return _mv88e6xxx_vtu_wait(chip);
 }
@@ -1254,23 +1260,21 @@ static int _mv88e6xxx_vtu_stu_flush(struct mv88e6xxx_chip *chip)
 }
 
 static int _mv88e6xxx_vtu_stu_data_read(struct mv88e6xxx_chip *chip,
-					struct mv88e6xxx_vtu_stu_entry *entry,
+					struct mv88e6xxx_vtu_entry *entry,
 					unsigned int nibble_offset)
 {
 	u16 regs[3];
-	int i;
-	int ret;
+	int i, err;
 
 	for (i = 0; i < 3; ++i) {
-		ret = _mv88e6xxx_reg_read(chip, REG_GLOBAL,
-					  GLOBAL_VTU_DATA_0_3 + i);
-		if (ret < 0)
-			return ret;
+		u16 *reg = &regs[i];
 
-		regs[i] = ret;
+		err = mv88e6xxx_g1_read(chip, GLOBAL_VTU_DATA_0_3 + i, reg);
+		if (err)
+			return err;
 	}
 
-	for (i = 0; i < chip->info->num_ports; ++i) {
+	for (i = 0; i < mv88e6xxx_num_ports(chip); ++i) {
 		unsigned int shift = (i % 4) * 4 + nibble_offset;
 		u16 reg = regs[i / 4];
 
@@ -1281,26 +1285,25 @@ static int _mv88e6xxx_vtu_stu_data_read(struct mv88e6xxx_chip *chip,
 }
 
 static int mv88e6xxx_vtu_data_read(struct mv88e6xxx_chip *chip,
-				   struct mv88e6xxx_vtu_stu_entry *entry)
+				   struct mv88e6xxx_vtu_entry *entry)
 {
 	return _mv88e6xxx_vtu_stu_data_read(chip, entry, 0);
 }
 
 static int mv88e6xxx_stu_data_read(struct mv88e6xxx_chip *chip,
-				   struct mv88e6xxx_vtu_stu_entry *entry)
+				   struct mv88e6xxx_vtu_entry *entry)
 {
 	return _mv88e6xxx_vtu_stu_data_read(chip, entry, 2);
 }
 
 static int _mv88e6xxx_vtu_stu_data_write(struct mv88e6xxx_chip *chip,
-					 struct mv88e6xxx_vtu_stu_entry *entry,
+					 struct mv88e6xxx_vtu_entry *entry,
 					 unsigned int nibble_offset)
 {
 	u16 regs[3] = { 0 };
-	int i;
-	int ret;
+	int i, err;
 
-	for (i = 0; i < chip->info->num_ports; ++i) {
+	for (i = 0; i < mv88e6xxx_num_ports(chip); ++i) {
 		unsigned int shift = (i % 4) * 4 + nibble_offset;
 		u8 data = entry->data[i];
 
@@ -1308,86 +1311,85 @@ static int _mv88e6xxx_vtu_stu_data_write(struct mv88e6xxx_chip *chip,
 	}
 
 	for (i = 0; i < 3; ++i) {
-		ret = _mv88e6xxx_reg_write(chip, REG_GLOBAL,
-					   GLOBAL_VTU_DATA_0_3 + i, regs[i]);
-		if (ret < 0)
-			return ret;
+		u16 reg = regs[i];
+
+		err = mv88e6xxx_g1_write(chip, GLOBAL_VTU_DATA_0_3 + i, reg);
+		if (err)
+			return err;
 	}
 
 	return 0;
 }
 
 static int mv88e6xxx_vtu_data_write(struct mv88e6xxx_chip *chip,
-				    struct mv88e6xxx_vtu_stu_entry *entry)
+				    struct mv88e6xxx_vtu_entry *entry)
 {
 	return _mv88e6xxx_vtu_stu_data_write(chip, entry, 0);
 }
 
 static int mv88e6xxx_stu_data_write(struct mv88e6xxx_chip *chip,
-				    struct mv88e6xxx_vtu_stu_entry *entry)
+				    struct mv88e6xxx_vtu_entry *entry)
 {
 	return _mv88e6xxx_vtu_stu_data_write(chip, entry, 2);
 }
 
 static int _mv88e6xxx_vtu_vid_write(struct mv88e6xxx_chip *chip, u16 vid)
 {
-	return _mv88e6xxx_reg_write(chip, REG_GLOBAL, GLOBAL_VTU_VID,
-				    vid & GLOBAL_VTU_VID_MASK);
+	return mv88e6xxx_g1_write(chip, GLOBAL_VTU_VID,
+				  vid & GLOBAL_VTU_VID_MASK);
 }
 
 static int _mv88e6xxx_vtu_getnext(struct mv88e6xxx_chip *chip,
-				  struct mv88e6xxx_vtu_stu_entry *entry)
+				  struct mv88e6xxx_vtu_entry *entry)
 {
-	struct mv88e6xxx_vtu_stu_entry next = { 0 };
-	int ret;
+	struct mv88e6xxx_vtu_entry next = { 0 };
+	u16 val;
+	int err;
 
-	ret = _mv88e6xxx_vtu_wait(chip);
-	if (ret < 0)
-		return ret;
+	err = _mv88e6xxx_vtu_wait(chip);
+	if (err)
+		return err;
 
-	ret = _mv88e6xxx_vtu_cmd(chip, GLOBAL_VTU_OP_VTU_GET_NEXT);
-	if (ret < 0)
-		return ret;
+	err = _mv88e6xxx_vtu_cmd(chip, GLOBAL_VTU_OP_VTU_GET_NEXT);
+	if (err)
+		return err;
 
-	ret = _mv88e6xxx_reg_read(chip, REG_GLOBAL, GLOBAL_VTU_VID);
-	if (ret < 0)
-		return ret;
+	err = mv88e6xxx_g1_read(chip, GLOBAL_VTU_VID, &val);
+	if (err)
+		return err;
 
-	next.vid = ret & GLOBAL_VTU_VID_MASK;
-	next.valid = !!(ret & GLOBAL_VTU_VID_VALID);
+	next.vid = val & GLOBAL_VTU_VID_MASK;
+	next.valid = !!(val & GLOBAL_VTU_VID_VALID);
 
 	if (next.valid) {
-		ret = mv88e6xxx_vtu_data_read(chip, &next);
-		if (ret < 0)
-			return ret;
+		err = mv88e6xxx_vtu_data_read(chip, &next);
+		if (err)
+			return err;
 
-		if (mv88e6xxx_has_fid_reg(chip)) {
-			ret = _mv88e6xxx_reg_read(chip, REG_GLOBAL,
-						  GLOBAL_VTU_FID);
-			if (ret < 0)
-				return ret;
+		if (mv88e6xxx_has(chip, MV88E6XXX_FLAG_G1_VTU_FID)) {
+			err = mv88e6xxx_g1_read(chip, GLOBAL_VTU_FID, &val);
+			if (err)
+				return err;
 
-			next.fid = ret & GLOBAL_VTU_FID_MASK;
+			next.fid = val & GLOBAL_VTU_FID_MASK;
 		} else if (mv88e6xxx_num_databases(chip) == 256) {
 			/* VTU DBNum[7:4] are located in VTU Operation 11:8, and
 			 * VTU DBNum[3:0] are located in VTU Operation 3:0
 			 */
-			ret = _mv88e6xxx_reg_read(chip, REG_GLOBAL,
-						  GLOBAL_VTU_OP);
-			if (ret < 0)
-				return ret;
+			err = mv88e6xxx_g1_read(chip, GLOBAL_VTU_OP, &val);
+			if (err)
+				return err;
 
-			next.fid = (ret & 0xf00) >> 4;
-			next.fid |= ret & 0xf;
+			next.fid = (val & 0xf00) >> 4;
+			next.fid |= val & 0xf;
 		}
 
 		if (mv88e6xxx_has(chip, MV88E6XXX_FLAG_STU)) {
-			ret = _mv88e6xxx_reg_read(chip, REG_GLOBAL,
-						  GLOBAL_VTU_SID);
-			if (ret < 0)
-				return ret;
+			err = mv88e6xxx_g1_read(chip, GLOBAL_VTU_SID, &val);
+			if (err)
+				return err;
 
-			next.sid = ret & GLOBAL_VTU_SID_MASK;
+			next.sid = val & GLOBAL_VTU_SID_MASK;
 		}
 	}
 
@@ -1399,8 +1401,8 @@ static int mv88e6xxx_port_vlan_dump(struct dsa_switch *ds, int port,
 				    struct switchdev_obj_port_vlan *vlan,
 				    int (*cb)(struct switchdev_obj *obj))
 {
-	struct mv88e6xxx_chip *chip = ds_to_priv(ds);
-	struct mv88e6xxx_vtu_stu_entry next;
+	struct mv88e6xxx_chip *chip = ds->priv;
+	struct mv88e6xxx_vtu_entry next;
 	u16 pvid;
 	int err;
 
@@ -1451,38 +1453,36 @@ unlock:
 }
 
 static int _mv88e6xxx_vtu_loadpurge(struct mv88e6xxx_chip *chip,
-				    struct mv88e6xxx_vtu_stu_entry *entry)
+				    struct mv88e6xxx_vtu_entry *entry)
 {
 	u16 op = GLOBAL_VTU_OP_VTU_LOAD_PURGE;
 	u16 reg = 0;
-	int ret;
+	int err;
 
-	ret = _mv88e6xxx_vtu_wait(chip);
-	if (ret < 0)
-		return ret;
+	err = _mv88e6xxx_vtu_wait(chip);
+	if (err)
+		return err;
 
 	if (!entry->valid)
 		goto loadpurge;
 
 	/* Write port member tags */
-	ret = mv88e6xxx_vtu_data_write(chip, entry);
-	if (ret < 0)
-		return ret;
+	err = mv88e6xxx_vtu_data_write(chip, entry);
+	if (err)
+		return err;
 
 	if (mv88e6xxx_has(chip, MV88E6XXX_FLAG_STU)) {
 		reg = entry->sid & GLOBAL_VTU_SID_MASK;
-		ret = _mv88e6xxx_reg_write(chip, REG_GLOBAL, GLOBAL_VTU_SID,
-					   reg);
-		if (ret < 0)
-			return ret;
+		err = mv88e6xxx_g1_write(chip, GLOBAL_VTU_SID, reg);
+		if (err)
+			return err;
 	}
 
-	if (mv88e6xxx_has_fid_reg(chip)) {
+	if (mv88e6xxx_has(chip, MV88E6XXX_FLAG_G1_VTU_FID)) {
 		reg = entry->fid & GLOBAL_VTU_FID_MASK;
-		ret = _mv88e6xxx_reg_write(chip, REG_GLOBAL, GLOBAL_VTU_FID,
-					   reg);
-		if (ret < 0)
-			return ret;
+		err = mv88e6xxx_g1_write(chip, GLOBAL_VTU_FID, reg);
+		if (err)
+			return err;
 	} else if (mv88e6xxx_num_databases(chip) == 256) {
 		/* VTU DBNum[7:4] are located in VTU Operation 11:8, and
 		 * VTU DBNum[3:0] are located in VTU Operation 3:0
@@ -1494,48 +1494,49 @@ static int _mv88e6xxx_vtu_loadpurge(struct mv88e6xxx_chip *chip,
 	reg = GLOBAL_VTU_VID_VALID;
 loadpurge:
 	reg |= entry->vid & GLOBAL_VTU_VID_MASK;
-	ret = _mv88e6xxx_reg_write(chip, REG_GLOBAL, GLOBAL_VTU_VID, reg);
-	if (ret < 0)
-		return ret;
+	err = mv88e6xxx_g1_write(chip, GLOBAL_VTU_VID, reg);
+	if (err)
+		return err;
 
 	return _mv88e6xxx_vtu_cmd(chip, op);
 }
 
 static int _mv88e6xxx_stu_getnext(struct mv88e6xxx_chip *chip, u8 sid,
-				  struct mv88e6xxx_vtu_stu_entry *entry)
+				  struct mv88e6xxx_vtu_entry *entry)
 {
-	struct mv88e6xxx_vtu_stu_entry next = { 0 };
-	int ret;
+	struct mv88e6xxx_vtu_entry next = { 0 };
+	u16 val;
+	int err;
 
-	ret = _mv88e6xxx_vtu_wait(chip);
-	if (ret < 0)
-		return ret;
+	err = _mv88e6xxx_vtu_wait(chip);
+	if (err)
+		return err;
 
-	ret = _mv88e6xxx_reg_write(chip, REG_GLOBAL, GLOBAL_VTU_SID,
-				   sid & GLOBAL_VTU_SID_MASK);
-	if (ret < 0)
-		return ret;
+	err = mv88e6xxx_g1_write(chip, GLOBAL_VTU_SID,
+				 sid & GLOBAL_VTU_SID_MASK);
+	if (err)
+		return err;
 
-	ret = _mv88e6xxx_vtu_cmd(chip, GLOBAL_VTU_OP_STU_GET_NEXT);
-	if (ret < 0)
-		return ret;
+	err = _mv88e6xxx_vtu_cmd(chip, GLOBAL_VTU_OP_STU_GET_NEXT);
+	if (err)
+		return err;
 
-	ret = _mv88e6xxx_reg_read(chip, REG_GLOBAL, GLOBAL_VTU_SID);
-	if (ret < 0)
-		return ret;
+	err = mv88e6xxx_g1_read(chip, GLOBAL_VTU_SID, &val);
+	if (err)
+		return err;
 
-	next.sid = ret & GLOBAL_VTU_SID_MASK;
+	next.sid = val & GLOBAL_VTU_SID_MASK;
 
-	ret = _mv88e6xxx_reg_read(chip, REG_GLOBAL, GLOBAL_VTU_VID);
-	if (ret < 0)
-		return ret;
+	err = mv88e6xxx_g1_read(chip, GLOBAL_VTU_VID, &val);
+	if (err)
+		return err;
 
-	next.valid = !!(ret & GLOBAL_VTU_VID_VALID);
+	next.valid = !!(val & GLOBAL_VTU_VID_VALID);
 
 	if (next.valid) {
-		ret = mv88e6xxx_stu_data_read(chip, &next);
-		if (ret < 0)
-			return ret;
+		err = mv88e6xxx_stu_data_read(chip, &next);
+		if (err)
+			return err;
 	}
 
 	*entry = next;
@@ -1543,33 +1544,33 @@ static int _mv88e6xxx_stu_getnext(struct mv88e6xxx_chip *chip, u8 sid,
 }
 
 static int _mv88e6xxx_stu_loadpurge(struct mv88e6xxx_chip *chip,
-				    struct mv88e6xxx_vtu_stu_entry *entry)
+				    struct mv88e6xxx_vtu_entry *entry)
 {
 	u16 reg = 0;
-	int ret;
+	int err;
 
-	ret = _mv88e6xxx_vtu_wait(chip);
-	if (ret < 0)
-		return ret;
+	err = _mv88e6xxx_vtu_wait(chip);
+	if (err)
+		return err;
 
 	if (!entry->valid)
 		goto loadpurge;
 
 	/* Write port states */
-	ret = mv88e6xxx_stu_data_write(chip, entry);
-	if (ret < 0)
-		return ret;
+	err = mv88e6xxx_stu_data_write(chip, entry);
+	if (err)
+		return err;
 
 	reg = GLOBAL_VTU_VID_VALID;
 loadpurge:
-	ret = _mv88e6xxx_reg_write(chip, REG_GLOBAL, GLOBAL_VTU_VID, reg);
-	if (ret < 0)
-		return ret;
+	err = mv88e6xxx_g1_write(chip, GLOBAL_VTU_VID, reg);
+	if (err)
+		return err;
 
 	reg = entry->sid & GLOBAL_VTU_SID_MASK;
-	ret = _mv88e6xxx_reg_write(chip, REG_GLOBAL, GLOBAL_VTU_SID, reg);
-	if (ret < 0)
-		return ret;
+	err = mv88e6xxx_g1_write(chip, GLOBAL_VTU_SID, reg);
+	if (err)
+		return err;
 
 	return _mv88e6xxx_vtu_cmd(chip, GLOBAL_VTU_OP_STU_LOAD_PURGE);
 }
@@ -1580,7 +1581,8 @@ static int _mv88e6xxx_port_fid(struct mv88e6xxx_chip *chip, int port,
 	struct dsa_switch *ds = chip->ds;
 	u16 upper_mask;
 	u16 fid;
-	int ret;
+	u16 reg;
+	int err;
 
 	if (mv88e6xxx_num_databases(chip) == 4096)
 		upper_mask = 0xff;
@@ -1590,37 +1592,35 @@ static int _mv88e6xxx_port_fid(struct mv88e6xxx_chip *chip, int port,
 		return -EOPNOTSUPP;
 
 	/* Port's default FID bits 3:0 are located in reg 0x06, offset 12 */
-	ret = _mv88e6xxx_reg_read(chip, REG_PORT(port), PORT_BASE_VLAN);
-	if (ret < 0)
-		return ret;
+	err = mv88e6xxx_port_read(chip, port, PORT_BASE_VLAN, &reg);
+	if (err)
+		return err;
 
-	fid = (ret & PORT_BASE_VLAN_FID_3_0_MASK) >> 12;
+	fid = (reg & PORT_BASE_VLAN_FID_3_0_MASK) >> 12;
 
 	if (new) {
-		ret &= ~PORT_BASE_VLAN_FID_3_0_MASK;
-		ret |= (*new << 12) & PORT_BASE_VLAN_FID_3_0_MASK;
+		reg &= ~PORT_BASE_VLAN_FID_3_0_MASK;
+		reg |= (*new << 12) & PORT_BASE_VLAN_FID_3_0_MASK;
 
-		ret = _mv88e6xxx_reg_write(chip, REG_PORT(port), PORT_BASE_VLAN,
-					   ret);
-		if (ret < 0)
-			return ret;
+		err = mv88e6xxx_port_write(chip, port, PORT_BASE_VLAN, reg);
+		if (err)
+			return err;
 	}
 
 	/* Port's default FID bits 11:4 are located in reg 0x05, offset 0 */
-	ret = _mv88e6xxx_reg_read(chip, REG_PORT(port), PORT_CONTROL_1);
-	if (ret < 0)
-		return ret;
+	err = mv88e6xxx_port_read(chip, port, PORT_CONTROL_1, &reg);
+	if (err)
+		return err;
 
-	fid |= (ret & upper_mask) << 4;
+	fid |= (reg & upper_mask) << 4;
 
 	if (new) {
-		ret &= ~upper_mask;
-		ret |= (*new >> 4) & upper_mask;
+		reg &= ~upper_mask;
+		reg |= (*new >> 4) & upper_mask;
 
-		ret = _mv88e6xxx_reg_write(chip, REG_PORT(port), PORT_CONTROL_1,
-					   ret);
-		if (ret < 0)
-			return ret;
+		err = mv88e6xxx_port_write(chip, port, PORT_CONTROL_1, reg);
+		if (err)
+			return err;
 
 		netdev_dbg(ds->ports[port].netdev,
 			   "FID %d (was %d)\n", *new, fid);
@@ -1647,13 +1647,13 @@ static int _mv88e6xxx_port_fid_set(struct mv88e6xxx_chip *chip,
 static int _mv88e6xxx_fid_new(struct mv88e6xxx_chip *chip, u16 *fid)
 {
 	DECLARE_BITMAP(fid_bitmap, MV88E6XXX_N_FID);
-	struct mv88e6xxx_vtu_stu_entry vlan;
+	struct mv88e6xxx_vtu_entry vlan;
 	int i, err;
 
 	bitmap_zero(fid_bitmap, MV88E6XXX_N_FID);
 
 	/* Set every FID bit used by the (un)bridged ports */
-	for (i = 0; i < chip->info->num_ports; ++i) {
+	for (i = 0; i < mv88e6xxx_num_ports(chip); ++i) {
 		err = _mv88e6xxx_port_fid_get(chip, i, fid);
 		if (err)
 			return err;
@@ -1689,10 +1689,10 @@ static int _mv88e6xxx_fid_new(struct mv88e6xxx_chip *chip, u16 *fid)
 }
 
 static int _mv88e6xxx_vtu_new(struct mv88e6xxx_chip *chip, u16 vid,
-			      struct mv88e6xxx_vtu_stu_entry *entry)
+			      struct mv88e6xxx_vtu_entry *entry)
 {
 	struct dsa_switch *ds = chip->ds;
-	struct mv88e6xxx_vtu_stu_entry vlan = {
+	struct mv88e6xxx_vtu_entry vlan = {
 		.valid = true,
 		.vid = vid,
 	};
@@ -1703,14 +1703,14 @@ static int _mv88e6xxx_vtu_new(struct mv88e6xxx_chip *chip, u16 vid,
 		return err;
 
 	/* exclude all ports except the CPU and DSA ports */
-	for (i = 0; i < chip->info->num_ports; ++i)
+	for (i = 0; i < mv88e6xxx_num_ports(chip); ++i)
 		vlan.data[i] = dsa_is_cpu_port(ds, i) || dsa_is_dsa_port(ds, i)
 			? GLOBAL_VTU_DATA_MEMBER_TAG_UNMODIFIED
 			: GLOBAL_VTU_DATA_MEMBER_TAG_NON_MEMBER;
 
 	if (mv88e6xxx_6097_family(chip) || mv88e6xxx_6165_family(chip) ||
 	    mv88e6xxx_6351_family(chip) || mv88e6xxx_6352_family(chip)) {
-		struct mv88e6xxx_vtu_stu_entry vstp;
+		struct mv88e6xxx_vtu_entry vstp;
 
 		/* Adding a VTU entry requires a valid STU entry. As VSTP is not
 		 * implemented, only one STU entry is needed to cover all VTU
@@ -1737,7 +1737,7 @@ static int _mv88e6xxx_vtu_new(struct mv88e6xxx_chip *chip, u16 vid,
 }
 
 static int _mv88e6xxx_vtu_get(struct mv88e6xxx_chip *chip, u16 vid,
-			      struct mv88e6xxx_vtu_stu_entry *entry, bool creat)
+			      struct mv88e6xxx_vtu_entry *entry, bool creat)
 {
 	int err;
 
@@ -1768,8 +1768,8 @@ static int _mv88e6xxx_vtu_get(struct mv88e6xxx_chip *chip, u16 vid,
 static int mv88e6xxx_port_check_hw_vlan(struct dsa_switch *ds, int port,
 					u16 vid_begin, u16 vid_end)
 {
-	struct mv88e6xxx_chip *chip = ds_to_priv(ds);
-	struct mv88e6xxx_vtu_stu_entry vlan;
+	struct mv88e6xxx_chip *chip = ds->priv;
+	struct mv88e6xxx_vtu_entry vlan;
 	int i, err;
 
 	if (!vid_begin)
@@ -1792,7 +1792,7 @@ static int mv88e6xxx_port_check_hw_vlan(struct dsa_switch *ds, int port,
 		if (vlan.vid > vid_end)
 			break;
 
-		for (i = 0; i < chip->info->num_ports; ++i) {
+		for (i = 0; i < mv88e6xxx_num_ports(chip); ++i) {
 			if (dsa_is_dsa_port(ds, i) || dsa_is_cpu_port(ds, i))
 				continue;
 
@@ -1829,29 +1829,29 @@ static const char * const mv88e6xxx_port_8021q_mode_names[] = {
 static int mv88e6xxx_port_vlan_filtering(struct dsa_switch *ds, int port,
 					 bool vlan_filtering)
 {
-	struct mv88e6xxx_chip *chip = ds_to_priv(ds);
+	struct mv88e6xxx_chip *chip = ds->priv;
 	u16 old, new = vlan_filtering ? PORT_CONTROL_2_8021Q_SECURE :
 		PORT_CONTROL_2_8021Q_DISABLED;
-	int ret;
+	u16 reg;
+	int err;
 
 	if (!mv88e6xxx_has(chip, MV88E6XXX_FLAG_VTU))
 		return -EOPNOTSUPP;
 
 	mutex_lock(&chip->reg_lock);
 
-	ret = _mv88e6xxx_reg_read(chip, REG_PORT(port), PORT_CONTROL_2);
-	if (ret < 0)
+	err = mv88e6xxx_port_read(chip, port, PORT_CONTROL_2, &reg);
+	if (err)
 		goto unlock;
 
-	old = ret & PORT_CONTROL_2_8021Q_MASK;
+	old = reg & PORT_CONTROL_2_8021Q_MASK;
 
 	if (new != old) {
-		ret &= ~PORT_CONTROL_2_8021Q_MASK;
-		ret |= new & PORT_CONTROL_2_8021Q_MASK;
+		reg &= ~PORT_CONTROL_2_8021Q_MASK;
+		reg |= new & PORT_CONTROL_2_8021Q_MASK;
 
-		ret = _mv88e6xxx_reg_write(chip, REG_PORT(port), PORT_CONTROL_2,
-					   ret);
-		if (ret < 0)
+		err = mv88e6xxx_port_write(chip, port, PORT_CONTROL_2, reg);
+		if (err)
 			goto unlock;
 
 		netdev_dbg(ds->ports[port].netdev, "802.1Q Mode %s (was %s)\n",
@@ -1859,11 +1859,11 @@ static int mv88e6xxx_port_vlan_filtering(struct dsa_switch *ds, int port,
 			   mv88e6xxx_port_8021q_mode_names[old]);
 	}
 
-	ret = 0;
+	err = 0;
 unlock:
 	mutex_unlock(&chip->reg_lock);
 
-	return ret;
+	return err;
 }
 
 static int
@@ -1871,7 +1871,7 @@ mv88e6xxx_port_vlan_prepare(struct dsa_switch *ds, int port,
 			    const struct switchdev_obj_port_vlan *vlan,
 			    struct switchdev_trans *trans)
 {
-	struct mv88e6xxx_chip *chip = ds_to_priv(ds);
+	struct mv88e6xxx_chip *chip = ds->priv;
 	int err;
 
 	if (!mv88e6xxx_has(chip, MV88E6XXX_FLAG_VTU))
@@ -1894,7 +1894,7 @@ mv88e6xxx_port_vlan_prepare(struct dsa_switch *ds, int port,
 static int _mv88e6xxx_port_vlan_add(struct mv88e6xxx_chip *chip, int port,
 				    u16 vid, bool untagged)
 {
-	struct mv88e6xxx_vtu_stu_entry vlan;
+	struct mv88e6xxx_vtu_entry vlan;
 	int err;
 
 	err = _mv88e6xxx_vtu_get(chip, vid, &vlan, true);
@@ -1912,7 +1912,7 @@ static void mv88e6xxx_port_vlan_add(struct dsa_switch *ds, int port,
 				    const struct switchdev_obj_port_vlan *vlan,
 				    struct switchdev_trans *trans)
 {
-	struct mv88e6xxx_chip *chip = ds_to_priv(ds);
+	struct mv88e6xxx_chip *chip = ds->priv;
 	bool untagged = vlan->flags & BRIDGE_VLAN_INFO_UNTAGGED;
 	bool pvid = vlan->flags & BRIDGE_VLAN_INFO_PVID;
 	u16 vid;
@@ -1939,7 +1939,7 @@ static int _mv88e6xxx_port_vlan_del(struct mv88e6xxx_chip *chip,
 				    int port, u16 vid)
 {
 	struct dsa_switch *ds = chip->ds;
-	struct mv88e6xxx_vtu_stu_entry vlan;
+	struct mv88e6xxx_vtu_entry vlan;
 	int i, err;
 
 	err = _mv88e6xxx_vtu_get(chip, vid, &vlan, false);
@@ -1954,7 +1954,7 @@ static int _mv88e6xxx_port_vlan_del(struct mv88e6xxx_chip *chip,
 
 	/* keep the VLAN unless all ports are excluded */
 	vlan.valid = false;
-	for (i = 0; i < chip->info->num_ports; ++i) {
+	for (i = 0; i < mv88e6xxx_num_ports(chip); ++i) {
 		if (dsa_is_cpu_port(ds, i) || dsa_is_dsa_port(ds, i))
 			continue;
 
@@ -1974,7 +1974,7 @@ static int _mv88e6xxx_port_vlan_del(struct mv88e6xxx_chip *chip,
 static int mv88e6xxx_port_vlan_del(struct dsa_switch *ds, int port,
 				   const struct switchdev_obj_port_vlan *vlan)
 {
-	struct mv88e6xxx_chip *chip = ds_to_priv(ds);
+	struct mv88e6xxx_chip *chip = ds->priv;
 	u16 pvid, vid;
 	int err = 0;
 
@@ -2008,14 +2008,13 @@ unlock:
 static int _mv88e6xxx_atu_mac_write(struct mv88e6xxx_chip *chip,
 				    const unsigned char *addr)
 {
-	int i, ret;
+	int i, err;
 
 	for (i = 0; i < 3; i++) {
-		ret = _mv88e6xxx_reg_write(
-			chip, REG_GLOBAL, GLOBAL_ATU_MAC_01 + i,
-			(addr[i * 2] << 8) | addr[i * 2 + 1]);
-		if (ret < 0)
-			return ret;
+		err = mv88e6xxx_g1_write(chip, GLOBAL_ATU_MAC_01 + i,
+					 (addr[i * 2] << 8) | addr[i * 2 + 1]);
+		if (err)
+			return err;
 	}
 
 	return 0;
@@ -2024,15 +2023,16 @@ static int _mv88e6xxx_atu_mac_write(struct mv88e6xxx_chip *chip,
 static int _mv88e6xxx_atu_mac_read(struct mv88e6xxx_chip *chip,
 				   unsigned char *addr)
 {
-	int i, ret;
+	u16 val;
+	int i, err;
 
 	for (i = 0; i < 3; i++) {
-		ret = _mv88e6xxx_reg_read(chip, REG_GLOBAL,
-					  GLOBAL_ATU_MAC_01 + i);
-		if (ret < 0)
-			return ret;
-		addr[i * 2] = ret >> 8;
-		addr[i * 2 + 1] = ret & 0xff;
+		err = mv88e6xxx_g1_read(chip, GLOBAL_ATU_MAC_01 + i, &val);
+		if (err)
+			return err;
+
+		addr[i * 2] = val >> 8;
+		addr[i * 2 + 1] = val & 0xff;
 	}
 
 	return 0;
@@ -2058,12 +2058,48 @@ static int _mv88e6xxx_atu_load(struct mv88e6xxx_chip *chip,
 	return _mv88e6xxx_atu_cmd(chip, entry->fid, GLOBAL_ATU_OP_LOAD_DB);
 }
 
-static int _mv88e6xxx_port_fdb_load(struct mv88e6xxx_chip *chip, int port,
-				    const unsigned char *addr, u16 vid,
-				    u8 state)
+static int _mv88e6xxx_atu_getnext(struct mv88e6xxx_chip *chip, u16 fid,
+				  struct mv88e6xxx_atu_entry *entry);
+
+static int mv88e6xxx_atu_get(struct mv88e6xxx_chip *chip, int fid,
+			     const u8 *addr, struct mv88e6xxx_atu_entry *entry)
 {
-	struct mv88e6xxx_atu_entry entry = { 0 };
-	struct mv88e6xxx_vtu_stu_entry vlan;
+	struct mv88e6xxx_atu_entry next;
+	int err;
+
+	eth_broadcast_addr(next.mac);
+
+	err = _mv88e6xxx_atu_mac_write(chip, next.mac);
+	if (err)
+		return err;
+
+	do {
+		err = _mv88e6xxx_atu_getnext(chip, fid, &next);
+		if (err)
+			return err;
+
+		if (next.state == GLOBAL_ATU_DATA_STATE_UNUSED)
+			break;
+
+		if (ether_addr_equal(next.mac, addr)) {
+			*entry = next;
+			return 0;
+		}
+	} while (!is_broadcast_ether_addr(next.mac));
+
+	memset(entry, 0, sizeof(*entry));
+	entry->fid = fid;
+	ether_addr_copy(entry->mac, addr);
+
+	return 0;
+}
+
+static int mv88e6xxx_port_db_load_purge(struct mv88e6xxx_chip *chip, int port,
+					const unsigned char *addr, u16 vid,
+					u8 state)
+{
+	struct mv88e6xxx_vtu_entry vlan;
+	struct mv88e6xxx_atu_entry entry;
 	int err;
 
 	/* Null VLAN ID corresponds to the port private database */
@@ -2074,12 +2110,18 @@ static int _mv88e6xxx_port_fdb_load(struct mv88e6xxx_chip *chip, int port,
 	if (err)
 		return err;
 
-	entry.fid = vlan.fid;
-	entry.state = state;
-	ether_addr_copy(entry.mac, addr);
-	if (state != GLOBAL_ATU_DATA_STATE_UNUSED) {
-		entry.trunk = false;
-		entry.portv_trunkid = BIT(port);
+	err = mv88e6xxx_atu_get(chip, vlan.fid, addr, &entry);
+	if (err)
+		return err;
+
+	/* Purge the ATU entry only if no port is using it anymore */
+	if (state == GLOBAL_ATU_DATA_STATE_UNUSED) {
+		entry.portv_trunkid &= ~BIT(port);
+		if (!entry.portv_trunkid)
+			entry.state = GLOBAL_ATU_DATA_STATE_UNUSED;
+	} else {
+		entry.portv_trunkid |= BIT(port);
+		entry.state = state;
 	}
 
 	return _mv88e6xxx_atu_load(chip, &entry);
@@ -2099,61 +2141,59 @@ static void mv88e6xxx_port_fdb_add(struct dsa_switch *ds, int port,
 				   const struct switchdev_obj_port_fdb *fdb,
 				   struct switchdev_trans *trans)
 {
-	int state = is_multicast_ether_addr(fdb->addr) ?
-		GLOBAL_ATU_DATA_STATE_MC_STATIC :
-		GLOBAL_ATU_DATA_STATE_UC_STATIC;
-	struct mv88e6xxx_chip *chip = ds_to_priv(ds);
+	struct mv88e6xxx_chip *chip = ds->priv;
 
 	mutex_lock(&chip->reg_lock);
-	if (_mv88e6xxx_port_fdb_load(chip, port, fdb->addr, fdb->vid, state))
-		netdev_err(ds->ports[port].netdev,
-			   "failed to load MAC address\n");
+	if (mv88e6xxx_port_db_load_purge(chip, port, fdb->addr, fdb->vid,
+					 GLOBAL_ATU_DATA_STATE_UC_STATIC))
+		netdev_err(ds->ports[port].netdev, "failed to load unicast MAC address\n");
 	mutex_unlock(&chip->reg_lock);
 }
 
 static int mv88e6xxx_port_fdb_del(struct dsa_switch *ds, int port,
 				  const struct switchdev_obj_port_fdb *fdb)
 {
-	struct mv88e6xxx_chip *chip = ds_to_priv(ds);
-	int ret;
+	struct mv88e6xxx_chip *chip = ds->priv;
+	int err;
 
 	mutex_lock(&chip->reg_lock);
-	ret = _mv88e6xxx_port_fdb_load(chip, port, fdb->addr, fdb->vid,
-				       GLOBAL_ATU_DATA_STATE_UNUSED);
+	err = mv88e6xxx_port_db_load_purge(chip, port, fdb->addr, fdb->vid,
+					   GLOBAL_ATU_DATA_STATE_UNUSED);
 	mutex_unlock(&chip->reg_lock);
 
-	return ret;
+	return err;
 }
 
 static int _mv88e6xxx_atu_getnext(struct mv88e6xxx_chip *chip, u16 fid,
 				  struct mv88e6xxx_atu_entry *entry)
 {
 	struct mv88e6xxx_atu_entry next = { 0 };
-	int ret;
+	u16 val;
+	int err;
 
 	next.fid = fid;
 
-	ret = _mv88e6xxx_atu_wait(chip);
-	if (ret < 0)
-		return ret;
+	err = _mv88e6xxx_atu_wait(chip);
+	if (err)
+		return err;
 
-	ret = _mv88e6xxx_atu_cmd(chip, fid, GLOBAL_ATU_OP_GET_NEXT_DB);
-	if (ret < 0)
-		return ret;
+	err = _mv88e6xxx_atu_cmd(chip, fid, GLOBAL_ATU_OP_GET_NEXT_DB);
+	if (err)
+		return err;
 
-	ret = _mv88e6xxx_atu_mac_read(chip, next.mac);
-	if (ret < 0)
-		return ret;
+	err = _mv88e6xxx_atu_mac_read(chip, next.mac);
+	if (err)
+		return err;
 
-	ret = _mv88e6xxx_reg_read(chip, REG_GLOBAL, GLOBAL_ATU_DATA);
-	if (ret < 0)
-		return ret;
+	err = mv88e6xxx_g1_read(chip, GLOBAL_ATU_DATA, &val);
+	if (err)
+		return err;
 
-	next.state = ret & GLOBAL_ATU_DATA_STATE_MASK;
+	next.state = val & GLOBAL_ATU_DATA_STATE_MASK;
 	if (next.state != GLOBAL_ATU_DATA_STATE_UNUSED) {
 		unsigned int mask, shift;
 
-		if (ret & GLOBAL_ATU_DATA_TRUNK) {
+		if (val & GLOBAL_ATU_DATA_TRUNK) {
 			next.trunk = true;
 			mask = GLOBAL_ATU_DATA_TRUNK_ID_MASK;
 			shift = GLOBAL_ATU_DATA_TRUNK_ID_SHIFT;
@@ -2163,17 +2203,17 @@ static int _mv88e6xxx_atu_getnext(struct mv88e6xxx_chip *chip, u16 fid,
 			shift = GLOBAL_ATU_DATA_PORT_VECTOR_SHIFT;
 		}
 
-		next.portv_trunkid = (ret & mask) >> shift;
+		next.portv_trunkid = (val & mask) >> shift;
 	}
 
 	*entry = next;
 	return 0;
 }
 
-static int _mv88e6xxx_port_fdb_dump_one(struct mv88e6xxx_chip *chip,
-					u16 fid, u16 vid, int port,
-					struct switchdev_obj_port_fdb *fdb,
-					int (*cb)(struct switchdev_obj *obj))
+static int mv88e6xxx_port_db_dump_fid(struct mv88e6xxx_chip *chip,
+				      u16 fid, u16 vid, int port,
+				      struct switchdev_obj *obj,
+				      int (*cb)(struct switchdev_obj *obj))
 {
 	struct mv88e6xxx_atu_entry addr = {
 		.mac = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff },
@@ -2187,72 +2227,98 @@ static int _mv88e6xxx_port_fdb_dump_one(struct mv88e6xxx_chip *chip,
 	do {
 		err = _mv88e6xxx_atu_getnext(chip, fid, &addr);
 		if (err)
-			break;
+			return err;
 
 		if (addr.state == GLOBAL_ATU_DATA_STATE_UNUSED)
 			break;
 
-		if (!addr.trunk && addr.portv_trunkid & BIT(port)) {
-			bool is_static = addr.state ==
-				(is_multicast_ether_addr(addr.mac) ?
-				 GLOBAL_ATU_DATA_STATE_MC_STATIC :
-				 GLOBAL_ATU_DATA_STATE_UC_STATIC);
+		if (addr.trunk || (addr.portv_trunkid & BIT(port)) == 0)
+			continue;
+
+		if (obj->id == SWITCHDEV_OBJ_ID_PORT_FDB) {
+			struct switchdev_obj_port_fdb *fdb;
+
+			if (!is_unicast_ether_addr(addr.mac))
+				continue;
 
+			fdb = SWITCHDEV_OBJ_PORT_FDB(obj);
 			fdb->vid = vid;
 			ether_addr_copy(fdb->addr, addr.mac);
-			fdb->ndm_state = is_static ? NUD_NOARP : NUD_REACHABLE;
+			if (addr.state == GLOBAL_ATU_DATA_STATE_UC_STATIC)
+				fdb->ndm_state = NUD_NOARP;
+			else
+				fdb->ndm_state = NUD_REACHABLE;
+		} else if (obj->id == SWITCHDEV_OBJ_ID_PORT_MDB) {
+			struct switchdev_obj_port_mdb *mdb;
 
-			err = cb(&fdb->obj);
-			if (err)
-				break;
+			if (!is_multicast_ether_addr(addr.mac))
+				continue;
+
+			mdb = SWITCHDEV_OBJ_PORT_MDB(obj);
+			mdb->vid = vid;
+			ether_addr_copy(mdb->addr, addr.mac);
+		} else {
+			return -EOPNOTSUPP;
 		}
+
+		err = cb(obj);
+		if (err)
+			return err;
 	} while (!is_broadcast_ether_addr(addr.mac));
 
 	return err;
 }
 
-static int mv88e6xxx_port_fdb_dump(struct dsa_switch *ds, int port,
-				   struct switchdev_obj_port_fdb *fdb,
-				   int (*cb)(struct switchdev_obj *obj))
+static int mv88e6xxx_port_db_dump(struct mv88e6xxx_chip *chip, int port,
+				  struct switchdev_obj *obj,
+				  int (*cb)(struct switchdev_obj *obj))
 {
-	struct mv88e6xxx_chip *chip = ds_to_priv(ds);
-	struct mv88e6xxx_vtu_stu_entry vlan = {
+	struct mv88e6xxx_vtu_entry vlan = {
 		.vid = GLOBAL_VTU_VID_MASK, /* all ones */
 	};
 	u16 fid;
 	int err;
 
-	mutex_lock(&chip->reg_lock);
-
 	/* Dump port's default Filtering Information Database (VLAN ID 0) */
 	err = _mv88e6xxx_port_fid_get(chip, port, &fid);
 	if (err)
-		goto unlock;
+		return err;
 
-	err = _mv88e6xxx_port_fdb_dump_one(chip, fid, 0, port, fdb, cb);
+	err = mv88e6xxx_port_db_dump_fid(chip, fid, 0, port, obj, cb);
 	if (err)
-		goto unlock;
+		return err;
 
 	/* Dump VLANs' Filtering Information Databases */
 	err = _mv88e6xxx_vtu_vid_write(chip, vlan.vid);
 	if (err)
-		goto unlock;
+		return err;
 
 	do {
 		err = _mv88e6xxx_vtu_getnext(chip, &vlan);
 		if (err)
-			break;
+			return err;
 
 		if (!vlan.valid)
 			break;
 
-		err = _mv88e6xxx_port_fdb_dump_one(chip, vlan.fid, vlan.vid,
-						   port, fdb, cb);
+		err = mv88e6xxx_port_db_dump_fid(chip, vlan.fid, vlan.vid, port,
+						 obj, cb);
 		if (err)
-			break;
+			return err;
 	} while (vlan.vid < GLOBAL_VTU_VID_MASK);
 
-unlock:
+	return err;
+}
+
+static int mv88e6xxx_port_fdb_dump(struct dsa_switch *ds, int port,
+				   struct switchdev_obj_port_fdb *fdb,
+				   int (*cb)(struct switchdev_obj *obj))
+{
+	struct mv88e6xxx_chip *chip = ds->priv;
+	int err;
+
+	mutex_lock(&chip->reg_lock);
+	err = mv88e6xxx_port_db_dump(chip, port, &fdb->obj, cb);
 	mutex_unlock(&chip->reg_lock);
 
 	return err;
@@ -2261,7 +2327,7 @@ unlock:
 static int mv88e6xxx_port_bridge_join(struct dsa_switch *ds, int port,
 				      struct net_device *bridge)
 {
-	struct mv88e6xxx_chip *chip = ds_to_priv(ds);
+	struct mv88e6xxx_chip *chip = ds->priv;
 	int i, err = 0;
 
 	mutex_lock(&chip->reg_lock);
@@ -2269,7 +2335,7 @@ static int mv88e6xxx_port_bridge_join(struct dsa_switch *ds, int port,
 	/* Assign the bridge and remap each port's VLANTable */
 	chip->ports[port].bridge_dev = bridge;
 
-	for (i = 0; i < chip->info->num_ports; ++i) {
+	for (i = 0; i < mv88e6xxx_num_ports(chip); ++i) {
 		if (chip->ports[i].bridge_dev == bridge) {
 			err = _mv88e6xxx_port_based_vlan_map(chip, i);
 			if (err)
@@ -2284,7 +2350,7 @@ static int mv88e6xxx_port_bridge_join(struct dsa_switch *ds, int port,
 
 static void mv88e6xxx_port_bridge_leave(struct dsa_switch *ds, int port)
 {
-	struct mv88e6xxx_chip *chip = ds_to_priv(ds);
+	struct mv88e6xxx_chip *chip = ds->priv;
 	struct net_device *bridge = chip->ports[port].bridge_dev;
 	int i;
 
@@ -2293,7 +2359,7 @@ static void mv88e6xxx_port_bridge_leave(struct dsa_switch *ds, int port)
 	/* Unassign the bridge and remap each port's VLANTable */
 	chip->ports[port].bridge_dev = NULL;
 
-	for (i = 0; i < chip->info->num_ports; ++i)
+	for (i = 0; i < mv88e6xxx_num_ports(chip); ++i)
 		if (i == port || chip->ports[i].bridge_dev == bridge)
 			if (_mv88e6xxx_port_based_vlan_map(chip, i))
 				netdev_warn(ds->ports[i].netdev,
@@ -2302,57 +2368,26 @@ static void mv88e6xxx_port_bridge_leave(struct dsa_switch *ds, int port)
 	mutex_unlock(&chip->reg_lock);
 }
 
-static int _mv88e6xxx_mdio_page_write(struct mv88e6xxx_chip *chip,
-				      int port, int page, int reg, int val)
-{
-	int ret;
-
-	ret = mv88e6xxx_mdio_write_indirect(chip, port, 0x16, page);
-	if (ret < 0)
-		goto restore_page_0;
-
-	ret = mv88e6xxx_mdio_write_indirect(chip, port, reg, val);
-restore_page_0:
-	mv88e6xxx_mdio_write_indirect(chip, port, 0x16, 0x0);
-
-	return ret;
-}
-
-static int _mv88e6xxx_mdio_page_read(struct mv88e6xxx_chip *chip,
-				     int port, int page, int reg)
-{
-	int ret;
-
-	ret = mv88e6xxx_mdio_write_indirect(chip, port, 0x16, page);
-	if (ret < 0)
-		goto restore_page_0;
-
-	ret = mv88e6xxx_mdio_read_indirect(chip, port, reg);
-restore_page_0:
-	mv88e6xxx_mdio_write_indirect(chip, port, 0x16, 0x0);
-
-	return ret;
-}
-
 static int mv88e6xxx_switch_reset(struct mv88e6xxx_chip *chip)
 {
 	bool ppu_active = mv88e6xxx_has(chip, MV88E6XXX_FLAG_PPU_ACTIVE);
 	u16 is_reset = (ppu_active ? 0x8800 : 0xc800);
 	struct gpio_desc *gpiod = chip->reset;
 	unsigned long timeout;
-	int ret;
+	u16 reg;
+	int err;
 	int i;
 
 	/* Set all ports to the disabled state. */
-	for (i = 0; i < chip->info->num_ports; i++) {
-		ret = _mv88e6xxx_reg_read(chip, REG_PORT(i), PORT_CONTROL);
-		if (ret < 0)
-			return ret;
+	for (i = 0; i < mv88e6xxx_num_ports(chip); i++) {
+		err = mv88e6xxx_port_read(chip, i, PORT_CONTROL, &reg);
+		if (err)
+			return err;
 
-		ret = _mv88e6xxx_reg_write(chip, REG_PORT(i), PORT_CONTROL,
-					   ret & 0xfffc);
-		if (ret)
-			return ret;
+		err = mv88e6xxx_port_write(chip, i, PORT_CONTROL,
+					   reg & 0xfffc);
+		if (err)
+			return err;
 	}
 
 	/* Wait for transmit queues to drain. */
@@ -2371,65 +2406,53 @@ static int mv88e6xxx_switch_reset(struct mv88e6xxx_chip *chip)
 	 * through global registers 0x18 and 0x19.
 	 */
 	if (ppu_active)
-		ret = _mv88e6xxx_reg_write(chip, REG_GLOBAL, 0x04, 0xc000);
+		err = mv88e6xxx_g1_write(chip, 0x04, 0xc000);
 	else
-		ret = _mv88e6xxx_reg_write(chip, REG_GLOBAL, 0x04, 0xc400);
-	if (ret)
-		return ret;
+		err = mv88e6xxx_g1_write(chip, 0x04, 0xc400);
+	if (err)
+		return err;
 
 	/* Wait up to one second for reset to complete. */
 	timeout = jiffies + 1 * HZ;
 	while (time_before(jiffies, timeout)) {
-		ret = _mv88e6xxx_reg_read(chip, REG_GLOBAL, 0x00);
-		if (ret < 0)
-			return ret;
+		err = mv88e6xxx_g1_read(chip, 0x00, &reg);
+		if (err)
+			return err;
 
-		if ((ret & is_reset) == is_reset)
+		if ((reg & is_reset) == is_reset)
 			break;
 		usleep_range(1000, 2000);
 	}
 	if (time_after(jiffies, timeout))
-		ret = -ETIMEDOUT;
+		err = -ETIMEDOUT;
 	else
-		ret = 0;
+		err = 0;
 
-	return ret;
+	return err;
 }
 
-static int mv88e6xxx_power_on_serdes(struct mv88e6xxx_chip *chip)
+static int mv88e6xxx_serdes_power_on(struct mv88e6xxx_chip *chip)
 {
-	int ret;
+	u16 val;
+	int err;
 
-	ret = _mv88e6xxx_mdio_page_read(chip, REG_FIBER_SERDES,
-					PAGE_FIBER_SERDES, MII_BMCR);
-	if (ret < 0)
-		return ret;
+	/* Clear Power Down bit */
+	err = mv88e6xxx_serdes_read(chip, MII_BMCR, &val);
+	if (err)
+		return err;
 
-	if (ret & BMCR_PDOWN) {
-		ret &= ~BMCR_PDOWN;
-		ret = _mv88e6xxx_mdio_page_write(chip, REG_FIBER_SERDES,
-						 PAGE_FIBER_SERDES, MII_BMCR,
-						 ret);
+	if (val & BMCR_PDOWN) {
+		val &= ~BMCR_PDOWN;
+		err = mv88e6xxx_serdes_write(chip, MII_BMCR, val);
 	}
 
-	return ret;
-}
-
-static int mv88e6xxx_port_read(struct mv88e6xxx_chip *chip, int port,
-			       int reg, u16 *val)
-{
-	int addr = chip->info->port_base_addr + port;
-
-	if (port >= chip->info->num_ports)
-		return -EINVAL;
-
-	return mv88e6xxx_read(chip, addr, reg, val);
+	return err;
 }
 
 static int mv88e6xxx_setup_port(struct mv88e6xxx_chip *chip, int port)
 {
 	struct dsa_switch *ds = chip->ds;
-	int ret;
+	int err;
 	u16 reg;
 
 	if (mv88e6xxx_6352_family(chip) || mv88e6xxx_6351_family(chip) ||
@@ -2442,7 +2465,7 @@ static int mv88e6xxx_setup_port(struct mv88e6xxx_chip *chip, int port)
 		 * and all DSA ports to their maximum bandwidth and
 		 * full duplex.
 		 */
-		reg = _mv88e6xxx_reg_read(chip, REG_PORT(port), PORT_PCS_CTRL);
+		err = mv88e6xxx_port_read(chip, port, PORT_PCS_CTRL, &reg);
 		if (dsa_is_cpu_port(ds, port) || dsa_is_dsa_port(ds, port)) {
 			reg &= ~PORT_PCS_CTRL_UNFORCED;
 			reg |= PORT_PCS_CTRL_FORCE_LINK |
@@ -2457,10 +2480,9 @@ static int mv88e6xxx_setup_port(struct mv88e6xxx_chip *chip, int port)
 			reg |= PORT_PCS_CTRL_UNFORCED;
 		}
 
-		ret = _mv88e6xxx_reg_write(chip, REG_PORT(port),
-					   PORT_PCS_CTRL, reg);
-		if (ret)
-			return ret;
+		err = mv88e6xxx_port_write(chip, port, PORT_PCS_CTRL, reg);
+		if (err)
+			return err;
 	}
 
 	/* Port Control: disable Drop-on-Unlock, disable Drop-on-Lock,
@@ -2486,28 +2508,13 @@ static int mv88e6xxx_setup_port(struct mv88e6xxx_chip *chip, int port)
 		PORT_CONTROL_USE_TAG | PORT_CONTROL_USE_IP |
 		PORT_CONTROL_STATE_FORWARDING;
 	if (dsa_is_cpu_port(ds, port)) {
-		if (mv88e6xxx_6095_family(chip) || mv88e6xxx_6185_family(chip))
-			reg |= PORT_CONTROL_DSA_TAG;
-		if (mv88e6xxx_6352_family(chip) ||
-		    mv88e6xxx_6351_family(chip) ||
-		    mv88e6xxx_6165_family(chip) ||
-		    mv88e6xxx_6097_family(chip) ||
-		    mv88e6xxx_6320_family(chip)) {
+		if (mv88e6xxx_has(chip, MV88E6XXX_FLAG_EDSA))
 			reg |= PORT_CONTROL_FRAME_ETHER_TYPE_DSA |
-				PORT_CONTROL_FORWARD_UNKNOWN |
 				PORT_CONTROL_FORWARD_UNKNOWN_MC;
-		}
-
-		if (mv88e6xxx_6352_family(chip) ||
-		    mv88e6xxx_6351_family(chip) ||
-		    mv88e6xxx_6165_family(chip) ||
-		    mv88e6xxx_6097_family(chip) ||
-		    mv88e6xxx_6095_family(chip) ||
-		    mv88e6xxx_6065_family(chip) ||
-		    mv88e6xxx_6185_family(chip) ||
-		    mv88e6xxx_6320_family(chip)) {
-			reg |= PORT_CONTROL_EGRESS_ADD_TAG;
-		}
+		else
+			reg |= PORT_CONTROL_DSA_TAG;
+		reg |= PORT_CONTROL_EGRESS_ADD_TAG |
+			PORT_CONTROL_FORWARD_UNKNOWN;
 	}
 	if (dsa_is_dsa_port(ds, port)) {
 		if (mv88e6xxx_6095_family(chip) ||
@@ -2526,26 +2533,25 @@ static int mv88e6xxx_setup_port(struct mv88e6xxx_chip *chip, int port)
 				PORT_CONTROL_FORWARD_UNKNOWN_MC;
 	}
 	if (reg) {
-		ret = _mv88e6xxx_reg_write(chip, REG_PORT(port),
-					   PORT_CONTROL, reg);
-		if (ret)
-			return ret;
+		err = mv88e6xxx_port_write(chip, port, PORT_CONTROL, reg);
+		if (err)
+			return err;
 	}
 
 	/* If this port is connected to a SerDes, make sure the SerDes is not
 	 * powered down.
 	 */
-	if (mv88e6xxx_6352_family(chip)) {
-		ret = _mv88e6xxx_reg_read(chip, REG_PORT(port), PORT_STATUS);
-		if (ret < 0)
-			return ret;
-		ret &= PORT_STATUS_CMODE_MASK;
-		if ((ret == PORT_STATUS_CMODE_100BASE_X) ||
-		    (ret == PORT_STATUS_CMODE_1000BASE_X) ||
-		    (ret == PORT_STATUS_CMODE_SGMII)) {
-			ret = mv88e6xxx_power_on_serdes(chip);
-			if (ret < 0)
-				return ret;
+	if (mv88e6xxx_has(chip, MV88E6XXX_FLAGS_SERDES)) {
+		err = mv88e6xxx_port_read(chip, port, PORT_STATUS, &reg);
+		if (err)
+			return err;
+		reg &= PORT_STATUS_CMODE_MASK;
+		if ((reg == PORT_STATUS_CMODE_100BASE_X) ||
+		    (reg == PORT_STATUS_CMODE_1000BASE_X) ||
+		    (reg == PORT_STATUS_CMODE_SGMII)) {
+			err = mv88e6xxx_serdes_power_on(chip);
+			if (err < 0)
+				return err;
 		}
 	}
 
@@ -2579,10 +2585,9 @@ static int mv88e6xxx_setup_port(struct mv88e6xxx_chip *chip, int port)
 	reg |= PORT_CONTROL_2_8021Q_DISABLED;
 
 	if (reg) {
-		ret = _mv88e6xxx_reg_write(chip, REG_PORT(port),
-					   PORT_CONTROL_2, reg);
-		if (ret)
-			return ret;
+		err = mv88e6xxx_port_write(chip, port, PORT_CONTROL_2, reg);
+		if (err)
+			return err;
 	}
 
 	/* Port Association Vector: when learning source addresses
@@ -2595,16 +2600,14 @@ static int mv88e6xxx_setup_port(struct mv88e6xxx_chip *chip, int port)
 	if (dsa_is_cpu_port(ds, port))
 		reg = 0;
 
-	ret = _mv88e6xxx_reg_write(chip, REG_PORT(port), PORT_ASSOC_VECTOR,
-				   reg);
-	if (ret)
-		return ret;
+	err = mv88e6xxx_port_write(chip, port, PORT_ASSOC_VECTOR, reg);
+	if (err)
+		return err;
 
 	/* Egress rate control 2: disable egress rate control. */
-	ret = _mv88e6xxx_reg_write(chip, REG_PORT(port), PORT_RATE_CONTROL_2,
-				   0x0000);
-	if (ret)
-		return ret;
+	err = mv88e6xxx_port_write(chip, port, PORT_RATE_CONTROL_2, 0x0000);
+	if (err)
+		return err;
 
 	if (mv88e6xxx_6352_family(chip) || mv88e6xxx_6351_family(chip) ||
 	    mv88e6xxx_6165_family(chip) || mv88e6xxx_6097_family(chip) ||
@@ -2613,111 +2616,108 @@ static int mv88e6xxx_setup_port(struct mv88e6xxx_chip *chip, int port)
 		 * be paused for by the remote end or the period of
 		 * time that this port can pause the remote end.
 		 */
-		ret = _mv88e6xxx_reg_write(chip, REG_PORT(port),
-					   PORT_PAUSE_CTRL, 0x0000);
-		if (ret)
-			return ret;
+		err = mv88e6xxx_port_write(chip, port, PORT_PAUSE_CTRL, 0x0000);
+		if (err)
+			return err;
 
 		/* Port ATU control: disable limiting the number of
 		 * address database entries that this port is allowed
 		 * to use.
 		 */
-		ret = _mv88e6xxx_reg_write(chip, REG_PORT(port),
-					   PORT_ATU_CONTROL, 0x0000);
+		err = mv88e6xxx_port_write(chip, port, PORT_ATU_CONTROL,
+					   0x0000);
 		/* Priority Override: disable DA, SA and VTU priority
 		 * override.
 		 */
-		ret = _mv88e6xxx_reg_write(chip, REG_PORT(port),
-					   PORT_PRI_OVERRIDE, 0x0000);
-		if (ret)
-			return ret;
+		err = mv88e6xxx_port_write(chip, port, PORT_PRI_OVERRIDE,
+					   0x0000);
+		if (err)
+			return err;
 
 		/* Port Ethertype: use the Ethertype DSA Ethertype
 		 * value.
 		 */
-		ret = _mv88e6xxx_reg_write(chip, REG_PORT(port),
-					   PORT_ETH_TYPE, ETH_P_EDSA);
-		if (ret)
-			return ret;
+		if (mv88e6xxx_has(chip, MV88E6XXX_FLAG_EDSA)) {
+			err = mv88e6xxx_port_write(chip, port, PORT_ETH_TYPE,
+						   ETH_P_EDSA);
+			if (err)
+				return err;
+		}
+
 		/* Tag Remap: use an identity 802.1p prio -> switch
 		 * prio mapping.
 		 */
-		ret = _mv88e6xxx_reg_write(chip, REG_PORT(port),
-					   PORT_TAG_REGMAP_0123, 0x3210);
-		if (ret)
-			return ret;
+		err = mv88e6xxx_port_write(chip, port, PORT_TAG_REGMAP_0123,
+					   0x3210);
+		if (err)
+			return err;
 
 		/* Tag Remap 2: use an identity 802.1p prio -> switch
 		 * prio mapping.
 		 */
-		ret = _mv88e6xxx_reg_write(chip, REG_PORT(port),
-					   PORT_TAG_REGMAP_4567, 0x7654);
-		if (ret)
-			return ret;
+		err = mv88e6xxx_port_write(chip, port, PORT_TAG_REGMAP_4567,
+					   0x7654);
+		if (err)
+			return err;
 	}
 
 	/* Rate Control: disable ingress rate limiting. */
 	if (mv88e6xxx_6352_family(chip) || mv88e6xxx_6351_family(chip) ||
 	    mv88e6xxx_6165_family(chip) || mv88e6xxx_6097_family(chip) ||
 	    mv88e6xxx_6320_family(chip)) {
-		ret = _mv88e6xxx_reg_write(chip, REG_PORT(port),
-					   PORT_RATE_CONTROL, 0x0001);
-		if (ret)
-			return ret;
+		err = mv88e6xxx_port_write(chip, port, PORT_RATE_CONTROL,
+					   0x0001);
+		if (err)
+			return err;
 	} else if (mv88e6xxx_6185_family(chip) || mv88e6xxx_6095_family(chip)) {
-		ret = _mv88e6xxx_reg_write(chip, REG_PORT(port),
-					   PORT_RATE_CONTROL, 0x0000);
-		if (ret)
-			return ret;
+		err = mv88e6xxx_port_write(chip, port, PORT_RATE_CONTROL,
+					   0x0000);
+		if (err)
+			return err;
 	}
 
 	/* Port Control 1: disable trunking, disable sending
 	 * learning messages to this port.
 	 */
-	ret = _mv88e6xxx_reg_write(chip, REG_PORT(port), PORT_CONTROL_1,
-				   0x0000);
-	if (ret)
-		return ret;
+	err = mv88e6xxx_port_write(chip, port, PORT_CONTROL_1, 0x0000);
+	if (err)
+		return err;
 
 	/* Port based VLAN map: give each port the same default address
 	 * database, and allow bidirectional communication between the
 	 * CPU and DSA port(s), and the other ports.
 	 */
-	ret = _mv88e6xxx_port_fid_set(chip, port, 0);
-	if (ret)
-		return ret;
+	err = _mv88e6xxx_port_fid_set(chip, port, 0);
+	if (err)
+		return err;
 
-	ret = _mv88e6xxx_port_based_vlan_map(chip, port);
-	if (ret)
-		return ret;
+	err = _mv88e6xxx_port_based_vlan_map(chip, port);
+	if (err)
+		return err;
 
 	/* Default VLAN ID and priority: don't set a default VLAN
 	 * ID, and set the default packet priority to zero.
 	 */
-	ret = _mv88e6xxx_reg_write(chip, REG_PORT(port), PORT_DEFAULT_VLAN,
-				   0x0000);
-	if (ret)
-		return ret;
-
-	return 0;
+	return mv88e6xxx_port_write(chip, port, PORT_DEFAULT_VLAN, 0x0000);
 }
 
-static int mv88e6xxx_g1_set_switch_mac(struct mv88e6xxx_chip *chip, u8 *addr)
+int mv88e6xxx_g1_set_switch_mac(struct mv88e6xxx_chip *chip, u8 *addr)
 {
 	int err;
 
-	err = mv88e6xxx_write(chip, REG_GLOBAL, GLOBAL_MAC_01,
-			      (addr[0] << 8) | addr[1]);
+	err = mv88e6xxx_g1_write(chip, GLOBAL_MAC_01, (addr[0] << 8) | addr[1]);
 	if (err)
 		return err;
 
-	err = mv88e6xxx_write(chip, REG_GLOBAL, GLOBAL_MAC_23,
-			      (addr[2] << 8) | addr[3]);
+	err = mv88e6xxx_g1_write(chip, GLOBAL_MAC_23, (addr[2] << 8) | addr[3]);
 	if (err)
 		return err;
 
-	return mv88e6xxx_write(chip, REG_GLOBAL, GLOBAL_MAC_45,
-			       (addr[4] << 8) | addr[5]);
+	err = mv88e6xxx_g1_write(chip, GLOBAL_MAC_45, (addr[4] << 8) | addr[5]);
+	if (err)
+		return err;
+
+	return 0;
 }
 
 static int mv88e6xxx_g1_set_age_time(struct mv88e6xxx_chip *chip,
@@ -2736,7 +2736,7 @@ static int mv88e6xxx_g1_set_age_time(struct mv88e6xxx_chip *chip,
 	/* Round to nearest multiple of coeff */
 	age_time = (msecs + coeff / 2) / coeff;
 
-	err = mv88e6xxx_read(chip, REG_GLOBAL, GLOBAL_ATU_CONTROL, &val);
+	err = mv88e6xxx_g1_read(chip, GLOBAL_ATU_CONTROL, &val);
 	if (err)
 		return err;
 
@@ -2744,13 +2744,13 @@ static int mv88e6xxx_g1_set_age_time(struct mv88e6xxx_chip *chip,
 	val &= ~0xff0;
 	val |= age_time << 4;
 
-	return mv88e6xxx_write(chip, REG_GLOBAL, GLOBAL_ATU_CONTROL, val);
+	return mv88e6xxx_g1_write(chip, GLOBAL_ATU_CONTROL, val);
 }
 
 static int mv88e6xxx_set_ageing_time(struct dsa_switch *ds,
 				     unsigned int ageing_time)
 {
-	struct mv88e6xxx_chip *chip = ds_to_priv(ds);
+	struct mv88e6xxx_chip *chip = ds->priv;
 	int err;
 
 	mutex_lock(&chip->reg_lock);
@@ -2775,7 +2775,7 @@ static int mv88e6xxx_g1_setup(struct mv88e6xxx_chip *chip)
 	    mv88e6xxx_has(chip, MV88E6XXX_FLAG_PPU_ACTIVE))
 		reg |= GLOBAL_CONTROL_PPU_ENABLE;
 
-	err = _mv88e6xxx_reg_write(chip, REG_GLOBAL, GLOBAL_CONTROL, reg);
+	err = mv88e6xxx_g1_write(chip, GLOBAL_CONTROL, reg);
 	if (err)
 		return err;
 
@@ -2785,15 +2785,14 @@ static int mv88e6xxx_g1_setup(struct mv88e6xxx_chip *chip)
 	reg = upstream_port << GLOBAL_MONITOR_CONTROL_INGRESS_SHIFT |
 		upstream_port << GLOBAL_MONITOR_CONTROL_EGRESS_SHIFT |
 		upstream_port << GLOBAL_MONITOR_CONTROL_ARP_SHIFT;
-	err = _mv88e6xxx_reg_write(chip, REG_GLOBAL, GLOBAL_MONITOR_CONTROL,
-				   reg);
+	err = mv88e6xxx_g1_write(chip, GLOBAL_MONITOR_CONTROL, reg);
 	if (err)
 		return err;
 
 	/* Disable remote management, and set the switch's DSA device number. */
-	err = _mv88e6xxx_reg_write(chip, REG_GLOBAL, GLOBAL_CONTROL_2,
-				   GLOBAL_CONTROL_2_MULTIPLE_CASCADE |
-				   (ds->index & 0x1f));
+	err = mv88e6xxx_g1_write(chip, GLOBAL_CONTROL_2,
+				 GLOBAL_CONTROL_2_MULTIPLE_CASCADE |
+				 (ds->index & 0x1f));
 	if (err)
 		return err;
 
@@ -2806,8 +2805,8 @@ static int mv88e6xxx_g1_setup(struct mv88e6xxx_chip *chip)
 	 * enable address learn messages to be sent to all message
 	 * ports.
 	 */
-	err = mv88e6xxx_write(chip, REG_GLOBAL, GLOBAL_ATU_CONTROL,
-			      GLOBAL_ATU_CONTROL_LEARN2ALL);
+	err = mv88e6xxx_g1_write(chip, GLOBAL_ATU_CONTROL,
+				 GLOBAL_ATU_CONTROL_LEARN2ALL);
 	if (err)
 		return err;
 
@@ -2821,39 +2820,39 @@ static int mv88e6xxx_g1_setup(struct mv88e6xxx_chip *chip)
 		return err;
 
 	/* Configure the IP ToS mapping registers. */
-	err = _mv88e6xxx_reg_write(chip, REG_GLOBAL, GLOBAL_IP_PRI_0, 0x0000);
+	err = mv88e6xxx_g1_write(chip, GLOBAL_IP_PRI_0, 0x0000);
 	if (err)
 		return err;
-	err = _mv88e6xxx_reg_write(chip, REG_GLOBAL, GLOBAL_IP_PRI_1, 0x0000);
+	err = mv88e6xxx_g1_write(chip, GLOBAL_IP_PRI_1, 0x0000);
 	if (err)
 		return err;
-	err = _mv88e6xxx_reg_write(chip, REG_GLOBAL, GLOBAL_IP_PRI_2, 0x5555);
+	err = mv88e6xxx_g1_write(chip, GLOBAL_IP_PRI_2, 0x5555);
 	if (err)
 		return err;
-	err = _mv88e6xxx_reg_write(chip, REG_GLOBAL, GLOBAL_IP_PRI_3, 0x5555);
+	err = mv88e6xxx_g1_write(chip, GLOBAL_IP_PRI_3, 0x5555);
 	if (err)
 		return err;
-	err = _mv88e6xxx_reg_write(chip, REG_GLOBAL, GLOBAL_IP_PRI_4, 0xaaaa);
+	err = mv88e6xxx_g1_write(chip, GLOBAL_IP_PRI_4, 0xaaaa);
 	if (err)
 		return err;
-	err = _mv88e6xxx_reg_write(chip, REG_GLOBAL, GLOBAL_IP_PRI_5, 0xaaaa);
+	err = mv88e6xxx_g1_write(chip, GLOBAL_IP_PRI_5, 0xaaaa);
 	if (err)
 		return err;
-	err = _mv88e6xxx_reg_write(chip, REG_GLOBAL, GLOBAL_IP_PRI_6, 0xffff);
+	err = mv88e6xxx_g1_write(chip, GLOBAL_IP_PRI_6, 0xffff);
 	if (err)
 		return err;
-	err = _mv88e6xxx_reg_write(chip, REG_GLOBAL, GLOBAL_IP_PRI_7, 0xffff);
+	err = mv88e6xxx_g1_write(chip, GLOBAL_IP_PRI_7, 0xffff);
 	if (err)
 		return err;
 
 	/* Configure the IEEE 802.1p priority mapping register. */
-	err = _mv88e6xxx_reg_write(chip, REG_GLOBAL, GLOBAL_IEEE_PRI, 0xfa41);
+	err = mv88e6xxx_g1_write(chip, GLOBAL_IEEE_PRI, 0xfa41);
 	if (err)
 		return err;
 
 	/* Clear the statistics counters for all ports */
-	err = _mv88e6xxx_reg_write(chip, REG_GLOBAL, GLOBAL_STATS_OP,
-				   GLOBAL_STATS_OP_FLUSH_ALL);
+	err = mv88e6xxx_g1_write(chip, GLOBAL_STATS_OP,
+				 GLOBAL_STATS_OP_FLUSH_ALL);
 	if (err)
 		return err;
 
@@ -2865,277 +2864,9 @@ static int mv88e6xxx_g1_setup(struct mv88e6xxx_chip *chip)
 	return 0;
 }
 
-static int mv88e6xxx_g2_device_mapping_write(struct mv88e6xxx_chip *chip,
-					     int target, int port)
-{
-	u16 val = (target << 8) | (port & 0xf);
-
-	return mv88e6xxx_update(chip, REG_GLOBAL2, GLOBAL2_DEVICE_MAPPING, val);
-}
-
-static int mv88e6xxx_g2_set_device_mapping(struct mv88e6xxx_chip *chip)
-{
-	int target, port;
-	int err;
-
-	/* Initialize the routing port to the 32 possible target devices */
-	for (target = 0; target < 32; ++target) {
-		port = 0xf;
-
-		if (target < DSA_MAX_SWITCHES) {
-			port = chip->ds->rtable[target];
-			if (port == DSA_RTABLE_NONE)
-				port = 0xf;
-		}
-
-		err = mv88e6xxx_g2_device_mapping_write(chip, target, port);
-		if (err)
-			break;
-	}
-
-	return err;
-}
-
-static int mv88e6xxx_g2_trunk_mask_write(struct mv88e6xxx_chip *chip, int num,
-					 bool hask, u16 mask)
-{
-	const u16 port_mask = BIT(chip->info->num_ports) - 1;
-	u16 val = (num << 12) | (mask & port_mask);
-
-	if (hask)
-		val |= GLOBAL2_TRUNK_MASK_HASK;
-
-	return mv88e6xxx_update(chip, REG_GLOBAL2, GLOBAL2_TRUNK_MASK, val);
-}
-
-static int mv88e6xxx_g2_trunk_mapping_write(struct mv88e6xxx_chip *chip, int id,
-					    u16 map)
-{
-	const u16 port_mask = BIT(chip->info->num_ports) - 1;
-	u16 val = (id << 11) | (map & port_mask);
-
-	return mv88e6xxx_update(chip, REG_GLOBAL2, GLOBAL2_TRUNK_MAPPING, val);
-}
-
-static int mv88e6xxx_g2_clear_trunk(struct mv88e6xxx_chip *chip)
-{
-	const u16 port_mask = BIT(chip->info->num_ports) - 1;
-	int i, err;
-
-	/* Clear all eight possible Trunk Mask vectors */
-	for (i = 0; i < 8; ++i) {
-		err = mv88e6xxx_g2_trunk_mask_write(chip, i, false, port_mask);
-		if (err)
-			return err;
-	}
-
-	/* Clear all sixteen possible Trunk ID routing vectors */
-	for (i = 0; i < 16; ++i) {
-		err = mv88e6xxx_g2_trunk_mapping_write(chip, i, 0);
-		if (err)
-			return err;
-	}
-
-	return 0;
-}
-
-static int mv88e6xxx_g2_clear_irl(struct mv88e6xxx_chip *chip)
-{
-	int port, err;
-
-	/* Init all Ingress Rate Limit resources of all ports */
-	for (port = 0; port < chip->info->num_ports; ++port) {
-		/* XXX newer chips (like 88E6390) have different 2-bit ops */
-		err = mv88e6xxx_write(chip, REG_GLOBAL2, GLOBAL2_IRL_CMD,
-				      GLOBAL2_IRL_CMD_OP_INIT_ALL |
-				      (port << 8));
-		if (err)
-			break;
-
-		/* Wait for the operation to complete */
-		err = _mv88e6xxx_wait(chip, REG_GLOBAL2, GLOBAL2_IRL_CMD,
-				      GLOBAL2_IRL_CMD_BUSY);
-		if (err)
-			break;
-	}
-
-	return err;
-}
-
-/* Indirect write to the Switch MAC/WoL/WoF register */
-static int mv88e6xxx_g2_switch_mac_write(struct mv88e6xxx_chip *chip,
-					 unsigned int pointer, u8 data)
-{
-	u16 val = (pointer << 8) | data;
-
-	return mv88e6xxx_update(chip, REG_GLOBAL2, GLOBAL2_SWITCH_MAC, val);
-}
-
-static int mv88e6xxx_g2_set_switch_mac(struct mv88e6xxx_chip *chip, u8 *addr)
-{
-	int i, err;
-
-	for (i = 0; i < 6; i++) {
-		err = mv88e6xxx_g2_switch_mac_write(chip, i, addr[i]);
-		if (err)
-			break;
-	}
-
-	return err;
-}
-
-static int mv88e6xxx_g2_pot_write(struct mv88e6xxx_chip *chip, int pointer,
-				  u8 data)
-{
-	u16 val = (pointer << 8) | (data & 0x7);
-
-	return mv88e6xxx_update(chip, REG_GLOBAL2, GLOBAL2_PRIO_OVERRIDE, val);
-}
-
-static int mv88e6xxx_g2_clear_pot(struct mv88e6xxx_chip *chip)
-{
-	int i, err;
-
-	/* Clear all sixteen possible Priority Override entries */
-	for (i = 0; i < 16; i++) {
-		err = mv88e6xxx_g2_pot_write(chip, i, 0);
-		if (err)
-			break;
-	}
-
-	return err;
-}
-
-static int mv88e6xxx_g2_eeprom_wait(struct mv88e6xxx_chip *chip)
-{
-	return _mv88e6xxx_wait(chip, REG_GLOBAL2, GLOBAL2_EEPROM_CMD,
-			       GLOBAL2_EEPROM_CMD_BUSY |
-			       GLOBAL2_EEPROM_CMD_RUNNING);
-}
-
-static int mv88e6xxx_g2_eeprom_cmd(struct mv88e6xxx_chip *chip, u16 cmd)
-{
-	int err;
-
-	err = mv88e6xxx_write(chip, REG_GLOBAL2, GLOBAL2_EEPROM_CMD, cmd);
-	if (err)
-		return err;
-
-	return mv88e6xxx_g2_eeprom_wait(chip);
-}
-
-static int mv88e6xxx_g2_eeprom_read16(struct mv88e6xxx_chip *chip,
-				      u8 addr, u16 *data)
-{
-	u16 cmd = GLOBAL2_EEPROM_CMD_OP_READ | addr;
-	int err;
-
-	err = mv88e6xxx_g2_eeprom_wait(chip);
-	if (err)
-		return err;
-
-	err = mv88e6xxx_g2_eeprom_cmd(chip, cmd);
-	if (err)
-		return err;
-
-	return mv88e6xxx_read(chip, REG_GLOBAL2, GLOBAL2_EEPROM_DATA, data);
-}
-
-static int mv88e6xxx_g2_eeprom_write16(struct mv88e6xxx_chip *chip,
-				       u8 addr, u16 data)
-{
-	u16 cmd = GLOBAL2_EEPROM_CMD_OP_WRITE | addr;
-	int err;
-
-	err = mv88e6xxx_g2_eeprom_wait(chip);
-	if (err)
-		return err;
-
-	err = mv88e6xxx_write(chip, REG_GLOBAL2, GLOBAL2_EEPROM_DATA, data);
-	if (err)
-		return err;
-
-	return mv88e6xxx_g2_eeprom_cmd(chip, cmd);
-}
-
-static int mv88e6xxx_g2_setup(struct mv88e6xxx_chip *chip)
-{
-	u16 reg;
-	int err;
-
-	if (mv88e6xxx_has(chip, MV88E6XXX_FLAG_G2_MGMT_EN_2X)) {
-		/* Consider the frames with reserved multicast destination
-		 * addresses matching 01:80:c2:00:00:2x as MGMT.
-		 */
-		err = mv88e6xxx_write(chip, REG_GLOBAL2, GLOBAL2_MGMT_EN_2X,
-				      0xffff);
-		if (err)
-			return err;
-	}
-
-	if (mv88e6xxx_has(chip, MV88E6XXX_FLAG_G2_MGMT_EN_0X)) {
-		/* Consider the frames with reserved multicast destination
-		 * addresses matching 01:80:c2:00:00:0x as MGMT.
-		 */
-		err = mv88e6xxx_write(chip, REG_GLOBAL2, GLOBAL2_MGMT_EN_0X,
-				      0xffff);
-		if (err)
-			return err;
-	}
-
-	/* 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 = GLOBAL2_SWITCH_MGMT_FORCE_FLOW_CTRL_PRI | (0x7 << 4);
-	if (mv88e6xxx_has(chip, MV88E6XXX_FLAG_G2_MGMT_EN_0X) ||
-	    mv88e6xxx_has(chip, MV88E6XXX_FLAG_G2_MGMT_EN_2X))
-		reg |= GLOBAL2_SWITCH_MGMT_RSVD2CPU | 0x7;
-	err = mv88e6xxx_write(chip, REG_GLOBAL2, GLOBAL2_SWITCH_MGMT, reg);
-	if (err)
-		return err;
-
-	/* Program the DSA routing table. */
-	err = mv88e6xxx_g2_set_device_mapping(chip);
-	if (err)
-		return err;
-
-	/* Clear all trunk masks and mapping. */
-	err = mv88e6xxx_g2_clear_trunk(chip);
-	if (err)
-		return err;
-
-	if (mv88e6xxx_has(chip, MV88E6XXX_FLAGS_IRL)) {
-		/* Disable ingress rate limiting by resetting all per port
-		 * ingress rate limit resources to their initial state.
-		 */
-		err = mv88e6xxx_g2_clear_irl(chip);
-			if (err)
-				return err;
-	}
-
-	if (mv88e6xxx_has(chip, MV88E6XXX_FLAGS_PVT)) {
-		/* Initialize Cross-chip Port VLAN Table to reset defaults */
-		err = mv88e6xxx_write(chip, REG_GLOBAL2, GLOBAL2_PVT_ADDR,
-				      GLOBAL2_PVT_ADDR_OP_INIT_ONES);
-		if (err)
-			return err;
-	}
-
-	if (mv88e6xxx_has(chip, MV88E6XXX_FLAG_G2_POT)) {
-		/* Clear the priority override table. */
-		err = mv88e6xxx_g2_clear_pot(chip);
-		if (err)
-			return err;
-	}
-
-	return 0;
-}
-
 static int mv88e6xxx_setup(struct dsa_switch *ds)
 {
-	struct mv88e6xxx_chip *chip = ds_to_priv(ds);
+	struct mv88e6xxx_chip *chip = ds->priv;
 	int err;
 	int i;
 
@@ -3149,7 +2880,7 @@ static int mv88e6xxx_setup(struct dsa_switch *ds)
 		goto unlock;
 
 	/* Setup Switch Port Registers */
-	for (i = 0; i < chip->info->num_ports; i++) {
+	for (i = 0; i < mv88e6xxx_num_ports(chip); i++) {
 		err = mv88e6xxx_setup_port(chip, i);
 		if (err)
 			goto unlock;
@@ -3175,100 +2906,48 @@ unlock:
 
 static int mv88e6xxx_set_addr(struct dsa_switch *ds, u8 *addr)
 {
-	struct mv88e6xxx_chip *chip = ds_to_priv(ds);
+	struct mv88e6xxx_chip *chip = ds->priv;
 	int err;
 
-	mutex_lock(&chip->reg_lock);
-
-	/* Has an indirect Switch MAC/WoL/WoF register in Global 2? */
-	if (mv88e6xxx_has(chip, MV88E6XXX_FLAG_G2_SWITCH_MAC))
-		err = mv88e6xxx_g2_set_switch_mac(chip, addr);
-	else
-		err = mv88e6xxx_g1_set_switch_mac(chip, addr);
-
-	mutex_unlock(&chip->reg_lock);
-
-	return err;
-}
-
-#ifdef CONFIG_NET_DSA_HWMON
-static int mv88e6xxx_mdio_page_read(struct dsa_switch *ds, int port, int page,
-				    int reg)
-{
-	struct mv88e6xxx_chip *chip = ds_to_priv(ds);
-	int ret;
-
-	mutex_lock(&chip->reg_lock);
-	ret = _mv88e6xxx_mdio_page_read(chip, port, page, reg);
-	mutex_unlock(&chip->reg_lock);
-
-	return ret;
-}
-
-static int mv88e6xxx_mdio_page_write(struct dsa_switch *ds, int port, int page,
-				     int reg, int val)
-{
-	struct mv88e6xxx_chip *chip = ds_to_priv(ds);
-	int ret;
+	if (!chip->info->ops->set_switch_mac)
+		return -EOPNOTSUPP;
 
 	mutex_lock(&chip->reg_lock);
-	ret = _mv88e6xxx_mdio_page_write(chip, port, page, reg, val);
+	err = chip->info->ops->set_switch_mac(chip, addr);
 	mutex_unlock(&chip->reg_lock);
 
-	return ret;
-}
-#endif
-
-static int mv88e6xxx_port_to_mdio_addr(struct mv88e6xxx_chip *chip, int port)
-{
-	if (port >= 0 && port < chip->info->num_ports)
-		return port;
-	return -EINVAL;
+	return err;
 }
 
-static int mv88e6xxx_mdio_read(struct mii_bus *bus, int port, int regnum)
+static int mv88e6xxx_mdio_read(struct mii_bus *bus, int phy, int reg)
 {
 	struct mv88e6xxx_chip *chip = bus->priv;
-	int addr = mv88e6xxx_port_to_mdio_addr(chip, port);
-	int ret;
+	u16 val;
+	int err;
 
-	if (addr < 0)
+	if (phy >= mv88e6xxx_num_ports(chip))
 		return 0xffff;
 
 	mutex_lock(&chip->reg_lock);
-
-	if (mv88e6xxx_has(chip, MV88E6XXX_FLAG_PPU))
-		ret = mv88e6xxx_mdio_read_ppu(chip, addr, regnum);
-	else if (mv88e6xxx_has(chip, MV88E6XXX_FLAG_SMI_PHY))
-		ret = mv88e6xxx_mdio_read_indirect(chip, addr, regnum);
-	else
-		ret = mv88e6xxx_mdio_read_direct(chip, addr, regnum);
-
+	err = mv88e6xxx_phy_read(chip, phy, reg, &val);
 	mutex_unlock(&chip->reg_lock);
-	return ret;
+
+	return err ? err : val;
 }
 
-static int mv88e6xxx_mdio_write(struct mii_bus *bus, int port, int regnum,
-				u16 val)
+static int mv88e6xxx_mdio_write(struct mii_bus *bus, int phy, int reg, u16 val)
 {
 	struct mv88e6xxx_chip *chip = bus->priv;
-	int addr = mv88e6xxx_port_to_mdio_addr(chip, port);
-	int ret;
+	int err;
 
-	if (addr < 0)
+	if (phy >= mv88e6xxx_num_ports(chip))
 		return 0xffff;
 
 	mutex_lock(&chip->reg_lock);
-
-	if (mv88e6xxx_has(chip, MV88E6XXX_FLAG_PPU))
-		ret = mv88e6xxx_mdio_write_ppu(chip, addr, regnum, val);
-	else if (mv88e6xxx_has(chip, MV88E6XXX_FLAG_SMI_PHY))
-		ret = mv88e6xxx_mdio_write_indirect(chip, addr, regnum, val);
-	else
-		ret = mv88e6xxx_mdio_write_direct(chip, addr, regnum, val);
-
+	err = mv88e6xxx_phy_write(chip, phy, reg, val);
 	mutex_unlock(&chip->reg_lock);
-	return ret;
+
+	return err;
 }
 
 static int mv88e6xxx_mdio_register(struct mv88e6xxx_chip *chip,
@@ -3278,9 +2957,6 @@ static int mv88e6xxx_mdio_register(struct mv88e6xxx_chip *chip,
 	struct mii_bus *bus;
 	int err;
 
-	if (mv88e6xxx_has(chip, MV88E6XXX_FLAG_PPU))
-		mv88e6xxx_ppu_state_init(chip);
-
 	if (np)
 		chip->mdio_np = of_get_child_by_name(np, "mdio");
 
@@ -3335,69 +3011,70 @@ static void mv88e6xxx_mdio_unregister(struct mv88e6xxx_chip *chip)
 
 static int mv88e61xx_get_temp(struct dsa_switch *ds, int *temp)
 {
-	struct mv88e6xxx_chip *chip = ds_to_priv(ds);
+	struct mv88e6xxx_chip *chip = ds->priv;
+	u16 val;
 	int ret;
-	int val;
 
 	*temp = 0;
 
 	mutex_lock(&chip->reg_lock);
 
-	ret = mv88e6xxx_mdio_write_direct(chip, 0x0, 0x16, 0x6);
+	ret = mv88e6xxx_phy_write(chip, 0x0, 0x16, 0x6);
 	if (ret < 0)
 		goto error;
 
 	/* Enable temperature sensor */
-	ret = mv88e6xxx_mdio_read_direct(chip, 0x0, 0x1a);
+	ret = mv88e6xxx_phy_read(chip, 0x0, 0x1a, &val);
 	if (ret < 0)
 		goto error;
 
-	ret = mv88e6xxx_mdio_write_direct(chip, 0x0, 0x1a, ret | (1 << 5));
+	ret = mv88e6xxx_phy_write(chip, 0x0, 0x1a, val | (1 << 5));
 	if (ret < 0)
 		goto error;
 
 	/* Wait for temperature to stabilize */
 	usleep_range(10000, 12000);
 
-	val = mv88e6xxx_mdio_read_direct(chip, 0x0, 0x1a);
-	if (val < 0) {
-		ret = val;
+	ret = mv88e6xxx_phy_read(chip, 0x0, 0x1a, &val);
+	if (ret < 0)
 		goto error;
-	}
 
 	/* Disable temperature sensor */
-	ret = mv88e6xxx_mdio_write_direct(chip, 0x0, 0x1a, ret & ~(1 << 5));
+	ret = mv88e6xxx_phy_write(chip, 0x0, 0x1a, val & ~(1 << 5));
 	if (ret < 0)
 		goto error;
 
 	*temp = ((val & 0x1f) - 5) * 5;
 
 error:
-	mv88e6xxx_mdio_write_direct(chip, 0x0, 0x16, 0x0);
+	mv88e6xxx_phy_write(chip, 0x0, 0x16, 0x0);
 	mutex_unlock(&chip->reg_lock);
 	return ret;
 }
 
 static int mv88e63xx_get_temp(struct dsa_switch *ds, int *temp)
 {
-	struct mv88e6xxx_chip *chip = ds_to_priv(ds);
+	struct mv88e6xxx_chip *chip = ds->priv;
 	int phy = mv88e6xxx_6320_family(chip) ? 3 : 0;
+	u16 val;
 	int ret;
 
 	*temp = 0;
 
-	ret = mv88e6xxx_mdio_page_read(ds, phy, 6, 27);
+	mutex_lock(&chip->reg_lock);
+	ret = mv88e6xxx_phy_page_read(chip, phy, 6, 27, &val);
+	mutex_unlock(&chip->reg_lock);
 	if (ret < 0)
 		return ret;
 
-	*temp = (ret & 0xff) - 25;
+	*temp = (val & 0xff) - 25;
 
 	return 0;
 }
 
 static int mv88e6xxx_get_temp(struct dsa_switch *ds, int *temp)
 {
-	struct mv88e6xxx_chip *chip = ds_to_priv(ds);
+	struct mv88e6xxx_chip *chip = ds->priv;
 
 	if (!mv88e6xxx_has(chip, MV88E6XXX_FLAG_TEMP))
 		return -EOPNOTSUPP;
@@ -3410,8 +3087,9 @@ static int mv88e6xxx_get_temp(struct dsa_switch *ds, int *temp)
 
 static int mv88e6xxx_get_temp_limit(struct dsa_switch *ds, int *temp)
 {
-	struct mv88e6xxx_chip *chip = ds_to_priv(ds);
+	struct mv88e6xxx_chip *chip = ds->priv;
 	int phy = mv88e6xxx_6320_family(chip) ? 3 : 0;
+	u16 val;
 	int ret;
 
 	if (!mv88e6xxx_has(chip, MV88E6XXX_FLAG_TEMP_LIMIT))
@@ -3419,36 +3097,45 @@ static int mv88e6xxx_get_temp_limit(struct dsa_switch *ds, int *temp)
 
 	*temp = 0;
 
-	ret = mv88e6xxx_mdio_page_read(ds, phy, 6, 26);
+	mutex_lock(&chip->reg_lock);
+	ret = mv88e6xxx_phy_page_read(chip, phy, 6, 26, &val);
+	mutex_unlock(&chip->reg_lock);
 	if (ret < 0)
 		return ret;
 
-	*temp = (((ret >> 8) & 0x1f) * 5) - 25;
+	*temp = (((val >> 8) & 0x1f) * 5) - 25;
 
 	return 0;
 }
 
 static int mv88e6xxx_set_temp_limit(struct dsa_switch *ds, int temp)
 {
-	struct mv88e6xxx_chip *chip = ds_to_priv(ds);
+	struct mv88e6xxx_chip *chip = ds->priv;
 	int phy = mv88e6xxx_6320_family(chip) ? 3 : 0;
-	int ret;
+	u16 val;
+	int err;
 
 	if (!mv88e6xxx_has(chip, MV88E6XXX_FLAG_TEMP_LIMIT))
 		return -EOPNOTSUPP;
 
-	ret = mv88e6xxx_mdio_page_read(ds, phy, 6, 26);
-	if (ret < 0)
-		return ret;
+	mutex_lock(&chip->reg_lock);
+	err = mv88e6xxx_phy_page_read(chip, phy, 6, 26, &val);
+	if (err)
+		goto unlock;
 	temp = clamp_val(DIV_ROUND_CLOSEST(temp, 5) + 5, 0, 0x1f);
-	return mv88e6xxx_mdio_page_write(ds, phy, 6, 26,
-					 (ret & 0xe0ff) | (temp << 8));
+	err = mv88e6xxx_phy_page_write(chip, phy, 6, 26,
+				       (val & 0xe0ff) | (temp << 8));
+unlock:
+	mutex_unlock(&chip->reg_lock);
+
+	return err;
 }
 
 static int mv88e6xxx_get_temp_alarm(struct dsa_switch *ds, bool *alarm)
 {
-	struct mv88e6xxx_chip *chip = ds_to_priv(ds);
+	struct mv88e6xxx_chip *chip = ds->priv;
 	int phy = mv88e6xxx_6320_family(chip) ? 3 : 0;
+	u16 val;
 	int ret;
 
 	if (!mv88e6xxx_has(chip, MV88E6XXX_FLAG_TEMP_LIMIT))
@@ -3456,11 +3143,13 @@ static int mv88e6xxx_get_temp_alarm(struct dsa_switch *ds, bool *alarm)
 
 	*alarm = false;
 
-	ret = mv88e6xxx_mdio_page_read(ds, phy, 6, 26);
+	mutex_lock(&chip->reg_lock);
+	ret = mv88e6xxx_phy_page_read(chip, phy, 6, 26, &val);
+	mutex_unlock(&chip->reg_lock);
 	if (ret < 0)
 		return ret;
 
-	*alarm = !!(ret & 0x40);
+	*alarm = !!(val & 0x40);
 
 	return 0;
 }
@@ -3468,74 +3157,22 @@ static int mv88e6xxx_get_temp_alarm(struct dsa_switch *ds, bool *alarm)
 
 static int mv88e6xxx_get_eeprom_len(struct dsa_switch *ds)
 {
-	struct mv88e6xxx_chip *chip = ds_to_priv(ds);
+	struct mv88e6xxx_chip *chip = ds->priv;
 
 	return chip->eeprom_len;
 }
 
-static int mv88e6xxx_get_eeprom16(struct mv88e6xxx_chip *chip,
-				  struct ethtool_eeprom *eeprom, u8 *data)
-{
-	unsigned int offset = eeprom->offset;
-	unsigned int len = eeprom->len;
-	u16 val;
-	int err;
-
-	eeprom->len = 0;
-
-	if (offset & 1) {
-		err = mv88e6xxx_g2_eeprom_read16(chip, offset >> 1, &val);
-		if (err)
-			return err;
-
-		*data++ = (val >> 8) & 0xff;
-
-		offset++;
-		len--;
-		eeprom->len++;
-	}
-
-	while (len >= 2) {
-		err = mv88e6xxx_g2_eeprom_read16(chip, offset >> 1, &val);
-		if (err)
-			return err;
-
-		*data++ = val & 0xff;
-		*data++ = (val >> 8) & 0xff;
-
-		offset += 2;
-		len -= 2;
-		eeprom->len += 2;
-	}
-
-	if (len) {
-		err = mv88e6xxx_g2_eeprom_read16(chip, offset >> 1, &val);
-		if (err)
-			return err;
-
-		*data++ = val & 0xff;
-
-		offset++;
-		len--;
-		eeprom->len++;
-	}
-
-	return 0;
-}
-
 static int mv88e6xxx_get_eeprom(struct dsa_switch *ds,
 				struct ethtool_eeprom *eeprom, u8 *data)
 {
-	struct mv88e6xxx_chip *chip = ds_to_priv(ds);
+	struct mv88e6xxx_chip *chip = ds->priv;
 	int err;
 
-	mutex_lock(&chip->reg_lock);
-
-	if (mv88e6xxx_has(chip, MV88E6XXX_FLAGS_EEPROM16))
-		err = mv88e6xxx_get_eeprom16(chip, eeprom, data);
-	else
-		err = -EOPNOTSUPP;
+	if (!chip->info->ops->get_eeprom)
+		return -EOPNOTSUPP;
 
+	mutex_lock(&chip->reg_lock);
+	err = chip->info->ops->get_eeprom(chip, eeprom, data);
 	mutex_unlock(&chip->reg_lock);
 
 	if (err)
@@ -3546,92 +3183,138 @@ static int mv88e6xxx_get_eeprom(struct dsa_switch *ds,
 	return 0;
 }
 
-static int mv88e6xxx_set_eeprom16(struct mv88e6xxx_chip *chip,
-				  struct ethtool_eeprom *eeprom, u8 *data)
+static int mv88e6xxx_set_eeprom(struct dsa_switch *ds,
+				struct ethtool_eeprom *eeprom, u8 *data)
 {
-	unsigned int offset = eeprom->offset;
-	unsigned int len = eeprom->len;
-	u16 val;
+	struct mv88e6xxx_chip *chip = ds->priv;
 	int err;
 
-	/* Ensure the RO WriteEn bit is set */
-	err = mv88e6xxx_read(chip, REG_GLOBAL2, GLOBAL2_EEPROM_CMD, &val);
-	if (err)
-		return err;
+	if (!chip->info->ops->set_eeprom)
+		return -EOPNOTSUPP;
 
-	if (!(val & GLOBAL2_EEPROM_CMD_WRITE_EN))
-		return -EROFS;
+	if (eeprom->magic != 0xc3ec4951)
+		return -EINVAL;
 
-	eeprom->len = 0;
+	mutex_lock(&chip->reg_lock);
+	err = chip->info->ops->set_eeprom(chip, eeprom, data);
+	mutex_unlock(&chip->reg_lock);
 
-	if (offset & 1) {
-		err = mv88e6xxx_g2_eeprom_read16(chip, offset >> 1, &val);
-		if (err)
-			return err;
+	return err;
+}
 
-		val = (*data++ << 8) | (val & 0xff);
+static const struct mv88e6xxx_ops mv88e6085_ops = {
+	.set_switch_mac = mv88e6xxx_g1_set_switch_mac,
+	.phy_read = mv88e6xxx_phy_ppu_read,
+	.phy_write = mv88e6xxx_phy_ppu_write,
+};
 
-		err = mv88e6xxx_g2_eeprom_write16(chip, offset >> 1, val);
-		if (err)
-			return err;
+static const struct mv88e6xxx_ops mv88e6095_ops = {
+	.set_switch_mac = mv88e6xxx_g1_set_switch_mac,
+	.phy_read = mv88e6xxx_phy_ppu_read,
+	.phy_write = mv88e6xxx_phy_ppu_write,
+};
 
-		offset++;
-		len--;
-		eeprom->len++;
-	}
+static const struct mv88e6xxx_ops mv88e6123_ops = {
+	.set_switch_mac = mv88e6xxx_g2_set_switch_mac,
+	.phy_read = mv88e6xxx_read,
+	.phy_write = mv88e6xxx_write,
+};
 
-	while (len >= 2) {
-		val = *data++;
-		val |= *data++ << 8;
+static const struct mv88e6xxx_ops mv88e6131_ops = {
+	.set_switch_mac = mv88e6xxx_g1_set_switch_mac,
+	.phy_read = mv88e6xxx_phy_ppu_read,
+	.phy_write = mv88e6xxx_phy_ppu_write,
+};
 
-		err = mv88e6xxx_g2_eeprom_write16(chip, offset >> 1, val);
-		if (err)
-			return err;
+static const struct mv88e6xxx_ops mv88e6161_ops = {
+	.set_switch_mac = mv88e6xxx_g2_set_switch_mac,
+	.phy_read = mv88e6xxx_read,
+	.phy_write = mv88e6xxx_write,
+};
 
-		offset += 2;
-		len -= 2;
-		eeprom->len += 2;
-	}
+static const struct mv88e6xxx_ops mv88e6165_ops = {
+	.set_switch_mac = mv88e6xxx_g2_set_switch_mac,
+	.phy_read = mv88e6xxx_read,
+	.phy_write = mv88e6xxx_write,
+};
 
-	if (len) {
-		err = mv88e6xxx_g2_eeprom_read16(chip, offset >> 1, &val);
-		if (err)
-			return err;
+static const struct mv88e6xxx_ops mv88e6171_ops = {
+	.set_switch_mac = mv88e6xxx_g2_set_switch_mac,
+	.phy_read = mv88e6xxx_g2_smi_phy_read,
+	.phy_write = mv88e6xxx_g2_smi_phy_write,
+};
 
-		val = (val & 0xff00) | *data++;
+static const struct mv88e6xxx_ops mv88e6172_ops = {
+	.get_eeprom = mv88e6xxx_g2_get_eeprom16,
+	.set_eeprom = mv88e6xxx_g2_set_eeprom16,
+	.set_switch_mac = mv88e6xxx_g2_set_switch_mac,
+	.phy_read = mv88e6xxx_g2_smi_phy_read,
+	.phy_write = mv88e6xxx_g2_smi_phy_write,
+};
 
-		err = mv88e6xxx_g2_eeprom_write16(chip, offset >> 1, val);
-		if (err)
-			return err;
+static const struct mv88e6xxx_ops mv88e6175_ops = {
+	.set_switch_mac = mv88e6xxx_g2_set_switch_mac,
+	.phy_read = mv88e6xxx_g2_smi_phy_read,
+	.phy_write = mv88e6xxx_g2_smi_phy_write,
+};
 
-		offset++;
-		len--;
-		eeprom->len++;
-	}
+static const struct mv88e6xxx_ops mv88e6176_ops = {
+	.get_eeprom = mv88e6xxx_g2_get_eeprom16,
+	.set_eeprom = mv88e6xxx_g2_set_eeprom16,
+	.set_switch_mac = mv88e6xxx_g2_set_switch_mac,
+	.phy_read = mv88e6xxx_g2_smi_phy_read,
+	.phy_write = mv88e6xxx_g2_smi_phy_write,
+};
 
-	return 0;
-}
+static const struct mv88e6xxx_ops mv88e6185_ops = {
+	.set_switch_mac = mv88e6xxx_g1_set_switch_mac,
+	.phy_read = mv88e6xxx_phy_ppu_read,
+	.phy_write = mv88e6xxx_phy_ppu_write,
+};
 
-static int mv88e6xxx_set_eeprom(struct dsa_switch *ds,
-				struct ethtool_eeprom *eeprom, u8 *data)
-{
-	struct mv88e6xxx_chip *chip = ds_to_priv(ds);
-	int err;
+static const struct mv88e6xxx_ops mv88e6240_ops = {
+	.get_eeprom = mv88e6xxx_g2_get_eeprom16,
+	.set_eeprom = mv88e6xxx_g2_set_eeprom16,
+	.set_switch_mac = mv88e6xxx_g2_set_switch_mac,
+	.phy_read = mv88e6xxx_g2_smi_phy_read,
+	.phy_write = mv88e6xxx_g2_smi_phy_write,
+};
 
-	if (eeprom->magic != 0xc3ec4951)
-		return -EINVAL;
+static const struct mv88e6xxx_ops mv88e6320_ops = {
+	.get_eeprom = mv88e6xxx_g2_get_eeprom16,
+	.set_eeprom = mv88e6xxx_g2_set_eeprom16,
+	.set_switch_mac = mv88e6xxx_g2_set_switch_mac,
+	.phy_read = mv88e6xxx_g2_smi_phy_read,
+	.phy_write = mv88e6xxx_g2_smi_phy_write,
+};
 
-	mutex_lock(&chip->reg_lock);
+static const struct mv88e6xxx_ops mv88e6321_ops = {
+	.get_eeprom = mv88e6xxx_g2_get_eeprom16,
+	.set_eeprom = mv88e6xxx_g2_set_eeprom16,
+	.set_switch_mac = mv88e6xxx_g2_set_switch_mac,
+	.phy_read = mv88e6xxx_g2_smi_phy_read,
+	.phy_write = mv88e6xxx_g2_smi_phy_write,
+};
 
-	if (mv88e6xxx_has(chip, MV88E6XXX_FLAGS_EEPROM16))
-		err = mv88e6xxx_set_eeprom16(chip, eeprom, data);
-	else
-		err = -EOPNOTSUPP;
+static const struct mv88e6xxx_ops mv88e6350_ops = {
+	.set_switch_mac = mv88e6xxx_g2_set_switch_mac,
+	.phy_read = mv88e6xxx_g2_smi_phy_read,
+	.phy_write = mv88e6xxx_g2_smi_phy_write,
+};
 
-	mutex_unlock(&chip->reg_lock);
+static const struct mv88e6xxx_ops mv88e6351_ops = {
+	.set_switch_mac = mv88e6xxx_g2_set_switch_mac,
+	.phy_read = mv88e6xxx_g2_smi_phy_read,
+	.phy_write = mv88e6xxx_g2_smi_phy_write,
+};
 
-	return err;
-}
+static const struct mv88e6xxx_ops mv88e6352_ops = {
+	.get_eeprom = mv88e6xxx_g2_get_eeprom16,
+	.set_eeprom = mv88e6xxx_g2_set_eeprom16,
+	.set_switch_mac = mv88e6xxx_g2_set_switch_mac,
+	.phy_read = mv88e6xxx_g2_smi_phy_read,
+	.phy_write = mv88e6xxx_g2_smi_phy_write,
+};
 
 static const struct mv88e6xxx_info mv88e6xxx_table[] = {
 	[MV88E6085] = {
@@ -3641,8 +3324,10 @@ static const struct mv88e6xxx_info mv88e6xxx_table[] = {
 		.num_databases = 4096,
 		.num_ports = 10,
 		.port_base_addr = 0x10,
+		.global1_addr = 0x1b,
 		.age_time_coeff = 15000,
 		.flags = MV88E6XXX_FLAGS_FAMILY_6097,
+		.ops = &mv88e6085_ops,
 	},
 
 	[MV88E6095] = {
@@ -3652,8 +3337,10 @@ static const struct mv88e6xxx_info mv88e6xxx_table[] = {
 		.num_databases = 256,
 		.num_ports = 11,
 		.port_base_addr = 0x10,
+		.global1_addr = 0x1b,
 		.age_time_coeff = 15000,
 		.flags = MV88E6XXX_FLAGS_FAMILY_6095,
+		.ops = &mv88e6095_ops,
 	},
 
 	[MV88E6123] = {
@@ -3663,8 +3350,10 @@ static const struct mv88e6xxx_info mv88e6xxx_table[] = {
 		.num_databases = 4096,
 		.num_ports = 3,
 		.port_base_addr = 0x10,
+		.global1_addr = 0x1b,
 		.age_time_coeff = 15000,
 		.flags = MV88E6XXX_FLAGS_FAMILY_6165,
+		.ops = &mv88e6123_ops,
 	},
 
 	[MV88E6131] = {
@@ -3674,8 +3363,10 @@ static const struct mv88e6xxx_info mv88e6xxx_table[] = {
 		.num_databases = 256,
 		.num_ports = 8,
 		.port_base_addr = 0x10,
+		.global1_addr = 0x1b,
 		.age_time_coeff = 15000,
 		.flags = MV88E6XXX_FLAGS_FAMILY_6185,
+		.ops = &mv88e6131_ops,
 	},
 
 	[MV88E6161] = {
@@ -3685,8 +3376,10 @@ static const struct mv88e6xxx_info mv88e6xxx_table[] = {
 		.num_databases = 4096,
 		.num_ports = 6,
 		.port_base_addr = 0x10,
+		.global1_addr = 0x1b,
 		.age_time_coeff = 15000,
 		.flags = MV88E6XXX_FLAGS_FAMILY_6165,
+		.ops = &mv88e6161_ops,
 	},
 
 	[MV88E6165] = {
@@ -3696,8 +3389,10 @@ static const struct mv88e6xxx_info mv88e6xxx_table[] = {
 		.num_databases = 4096,
 		.num_ports = 6,
 		.port_base_addr = 0x10,
+		.global1_addr = 0x1b,
 		.age_time_coeff = 15000,
 		.flags = MV88E6XXX_FLAGS_FAMILY_6165,
+		.ops = &mv88e6165_ops,
 	},
 
 	[MV88E6171] = {
@@ -3707,8 +3402,10 @@ static const struct mv88e6xxx_info mv88e6xxx_table[] = {
 		.num_databases = 4096,
 		.num_ports = 7,
 		.port_base_addr = 0x10,
+		.global1_addr = 0x1b,
 		.age_time_coeff = 15000,
 		.flags = MV88E6XXX_FLAGS_FAMILY_6351,
+		.ops = &mv88e6171_ops,
 	},
 
 	[MV88E6172] = {
@@ -3718,8 +3415,10 @@ static const struct mv88e6xxx_info mv88e6xxx_table[] = {
 		.num_databases = 4096,
 		.num_ports = 7,
 		.port_base_addr = 0x10,
+		.global1_addr = 0x1b,
 		.age_time_coeff = 15000,
 		.flags = MV88E6XXX_FLAGS_FAMILY_6352,
+		.ops = &mv88e6172_ops,
 	},
 
 	[MV88E6175] = {
@@ -3729,8 +3428,10 @@ static const struct mv88e6xxx_info mv88e6xxx_table[] = {
 		.num_databases = 4096,
 		.num_ports = 7,
 		.port_base_addr = 0x10,
+		.global1_addr = 0x1b,
 		.age_time_coeff = 15000,
 		.flags = MV88E6XXX_FLAGS_FAMILY_6351,
+		.ops = &mv88e6175_ops,
 	},
 
 	[MV88E6176] = {
@@ -3740,8 +3441,10 @@ static const struct mv88e6xxx_info mv88e6xxx_table[] = {
 		.num_databases = 4096,
 		.num_ports = 7,
 		.port_base_addr = 0x10,
+		.global1_addr = 0x1b,
 		.age_time_coeff = 15000,
 		.flags = MV88E6XXX_FLAGS_FAMILY_6352,
+		.ops = &mv88e6176_ops,
 	},
 
 	[MV88E6185] = {
@@ -3751,8 +3454,10 @@ static const struct mv88e6xxx_info mv88e6xxx_table[] = {
 		.num_databases = 256,
 		.num_ports = 10,
 		.port_base_addr = 0x10,
+		.global1_addr = 0x1b,
 		.age_time_coeff = 15000,
 		.flags = MV88E6XXX_FLAGS_FAMILY_6185,
+		.ops = &mv88e6185_ops,
 	},
 
 	[MV88E6240] = {
@@ -3762,8 +3467,10 @@ static const struct mv88e6xxx_info mv88e6xxx_table[] = {
 		.num_databases = 4096,
 		.num_ports = 7,
 		.port_base_addr = 0x10,
+		.global1_addr = 0x1b,
 		.age_time_coeff = 15000,
 		.flags = MV88E6XXX_FLAGS_FAMILY_6352,
+		.ops = &mv88e6240_ops,
 	},
 
 	[MV88E6320] = {
@@ -3773,8 +3480,10 @@ static const struct mv88e6xxx_info mv88e6xxx_table[] = {
 		.num_databases = 4096,
 		.num_ports = 7,
 		.port_base_addr = 0x10,
+		.global1_addr = 0x1b,
 		.age_time_coeff = 15000,
 		.flags = MV88E6XXX_FLAGS_FAMILY_6320,
+		.ops = &mv88e6320_ops,
 	},
 
 	[MV88E6321] = {
@@ -3784,8 +3493,10 @@ static const struct mv88e6xxx_info mv88e6xxx_table[] = {
 		.num_databases = 4096,
 		.num_ports = 7,
 		.port_base_addr = 0x10,
+		.global1_addr = 0x1b,
 		.age_time_coeff = 15000,
 		.flags = MV88E6XXX_FLAGS_FAMILY_6320,
+		.ops = &mv88e6321_ops,
 	},
 
 	[MV88E6350] = {
@@ -3795,8 +3506,10 @@ static const struct mv88e6xxx_info mv88e6xxx_table[] = {
 		.num_databases = 4096,
 		.num_ports = 7,
 		.port_base_addr = 0x10,
+		.global1_addr = 0x1b,
 		.age_time_coeff = 15000,
 		.flags = MV88E6XXX_FLAGS_FAMILY_6351,
+		.ops = &mv88e6350_ops,
 	},
 
 	[MV88E6351] = {
@@ -3806,8 +3519,10 @@ static const struct mv88e6xxx_info mv88e6xxx_table[] = {
 		.num_databases = 4096,
 		.num_ports = 7,
 		.port_base_addr = 0x10,
+		.global1_addr = 0x1b,
 		.age_time_coeff = 15000,
 		.flags = MV88E6XXX_FLAGS_FAMILY_6351,
+		.ops = &mv88e6351_ops,
 	},
 
 	[MV88E6352] = {
@@ -3817,8 +3532,10 @@ static const struct mv88e6xxx_info mv88e6xxx_table[] = {
 		.num_databases = 4096,
 		.num_ports = 7,
 		.port_base_addr = 0x10,
+		.global1_addr = 0x1b,
 		.age_time_coeff = 15000,
 		.flags = MV88E6XXX_FLAGS_FAMILY_6352,
+		.ops = &mv88e6352_ops,
 	},
 };
 
@@ -3856,6 +3573,10 @@ static int mv88e6xxx_detect(struct mv88e6xxx_chip *chip)
 	/* Update the compatible info with the probed one */
 	chip->info = info;
 
+	err = mv88e6xxx_g2_require(chip);
+	if (err)
+		return err;
+
 	dev_info(chip->dev, "switch 0x%x detected: %s, revision %u\n",
 		 chip->info->prod_num, chip->info->name, rev);
 
@@ -3877,6 +3598,18 @@ static struct mv88e6xxx_chip *mv88e6xxx_alloc_chip(struct device *dev)
 	return chip;
 }
 
+static void mv88e6xxx_phy_init(struct mv88e6xxx_chip *chip)
+{
+	if (mv88e6xxx_has(chip, MV88E6XXX_FLAG_PPU))
+		mv88e6xxx_ppu_state_init(chip);
+}
+
+static void mv88e6xxx_phy_destroy(struct mv88e6xxx_chip *chip)
+{
+	if (mv88e6xxx_has(chip, MV88E6XXX_FLAG_PPU))
+		mv88e6xxx_ppu_state_destroy(chip);
+}
+
 static int mv88e6xxx_smi_init(struct mv88e6xxx_chip *chip,
 			      struct mii_bus *bus, int sw_addr)
 {
@@ -3886,7 +3619,7 @@ static int mv88e6xxx_smi_init(struct mv88e6xxx_chip *chip,
 
 	if (sw_addr == 0)
 		chip->smi_ops = &mv88e6xxx_smi_single_chip_ops;
-	else if (mv88e6xxx_has(chip, MV88E6XXX_FLAG_MULTI_CHIP))
+	else if (mv88e6xxx_has(chip, MV88E6XXX_FLAGS_MULTI_CHIP))
 		chip->smi_ops = &mv88e6xxx_smi_multi_chip_ops;
 	else
 		return -EINVAL;
@@ -3897,6 +3630,16 @@ static int mv88e6xxx_smi_init(struct mv88e6xxx_chip *chip,
 	return 0;
 }
 
+static enum dsa_tag_protocol mv88e6xxx_get_tag_protocol(struct dsa_switch *ds)
+{
+	struct mv88e6xxx_chip *chip = ds->priv;
+
+	if (mv88e6xxx_has(chip, MV88E6XXX_FLAG_EDSA))
+		return DSA_TAG_PROTO_EDSA;
+
+	return DSA_TAG_PROTO_DSA;
+}
+
 static const char *mv88e6xxx_drv_probe(struct device *dsa_dev,
 				       struct device *host_dev, int sw_addr,
 				       void **priv)
@@ -3924,6 +3667,8 @@ static const char *mv88e6xxx_drv_probe(struct device *dsa_dev,
 	if (err)
 		goto free;
 
+	mv88e6xxx_phy_init(chip);
+
 	err = mv88e6xxx_mdio_register(chip, NULL);
 	if (err)
 		goto free;
@@ -3937,9 +3682,61 @@ free:
 	return NULL;
 }
 
-static struct dsa_switch_driver mv88e6xxx_switch_driver = {
-	.tag_protocol		= DSA_TAG_PROTO_EDSA,
+static int mv88e6xxx_port_mdb_prepare(struct dsa_switch *ds, int port,
+				      const struct switchdev_obj_port_mdb *mdb,
+				      struct switchdev_trans *trans)
+{
+	/* We don't need any dynamic resource from the kernel (yet),
+	 * so skip the prepare phase.
+	 */
+
+	return 0;
+}
+
+static void mv88e6xxx_port_mdb_add(struct dsa_switch *ds, int port,
+				   const struct switchdev_obj_port_mdb *mdb,
+				   struct switchdev_trans *trans)
+{
+	struct mv88e6xxx_chip *chip = ds->priv;
+
+	mutex_lock(&chip->reg_lock);
+	if (mv88e6xxx_port_db_load_purge(chip, port, mdb->addr, mdb->vid,
+					 GLOBAL_ATU_DATA_STATE_MC_STATIC))
+		netdev_err(ds->ports[port].netdev, "failed to load multicast MAC address\n");
+	mutex_unlock(&chip->reg_lock);
+}
+
+static int mv88e6xxx_port_mdb_del(struct dsa_switch *ds, int port,
+				  const struct switchdev_obj_port_mdb *mdb)
+{
+	struct mv88e6xxx_chip *chip = ds->priv;
+	int err;
+
+	mutex_lock(&chip->reg_lock);
+	err = mv88e6xxx_port_db_load_purge(chip, port, mdb->addr, mdb->vid,
+					   GLOBAL_ATU_DATA_STATE_UNUSED);
+	mutex_unlock(&chip->reg_lock);
+
+	return err;
+}
+
+static int mv88e6xxx_port_mdb_dump(struct dsa_switch *ds, int port,
+				   struct switchdev_obj_port_mdb *mdb,
+				   int (*cb)(struct switchdev_obj *obj))
+{
+	struct mv88e6xxx_chip *chip = ds->priv;
+	int err;
+
+	mutex_lock(&chip->reg_lock);
+	err = mv88e6xxx_port_db_dump(chip, port, &mdb->obj, cb);
+	mutex_unlock(&chip->reg_lock);
+
+	return err;
+}
+
+static struct dsa_switch_ops mv88e6xxx_switch_ops = {
 	.probe			= mv88e6xxx_drv_probe,
+	.get_tag_protocol	= mv88e6xxx_get_tag_protocol,
 	.setup			= mv88e6xxx_setup,
 	.set_addr		= mv88e6xxx_set_addr,
 	.adjust_link		= mv88e6xxx_adjust_link,
@@ -3963,6 +3760,7 @@ static struct dsa_switch_driver mv88e6xxx_switch_driver = {
 	.port_bridge_join	= mv88e6xxx_port_bridge_join,
 	.port_bridge_leave	= mv88e6xxx_port_bridge_leave,
 	.port_stp_state_set	= mv88e6xxx_port_stp_state_set,
+	.port_fast_age		= mv88e6xxx_port_fast_age,
 	.port_vlan_filtering	= mv88e6xxx_port_vlan_filtering,
 	.port_vlan_prepare	= mv88e6xxx_port_vlan_prepare,
 	.port_vlan_add		= mv88e6xxx_port_vlan_add,
@@ -3972,6 +3770,10 @@ static struct dsa_switch_driver mv88e6xxx_switch_driver = {
 	.port_fdb_add           = mv88e6xxx_port_fdb_add,
 	.port_fdb_del           = mv88e6xxx_port_fdb_del,
 	.port_fdb_dump          = mv88e6xxx_port_fdb_dump,
+	.port_mdb_prepare       = mv88e6xxx_port_mdb_prepare,
+	.port_mdb_add           = mv88e6xxx_port_mdb_add,
+	.port_mdb_del           = mv88e6xxx_port_mdb_del,
+	.port_mdb_dump          = mv88e6xxx_port_mdb_dump,
 };
 
 static int mv88e6xxx_register_switch(struct mv88e6xxx_chip *chip,
@@ -3986,7 +3788,7 @@ static int mv88e6xxx_register_switch(struct mv88e6xxx_chip *chip,
 
 	ds->dev = dev;
 	ds->priv = chip;
-	ds->drv = &mv88e6xxx_switch_driver;
+	ds->ops = &mv88e6xxx_switch_ops;
 
 	dev_set_drvdata(dev, ds);
 
@@ -4025,11 +3827,13 @@ static int mv88e6xxx_probe(struct mdio_device *mdiodev)
 	if (err)
 		return err;
 
+	mv88e6xxx_phy_init(chip);
+
 	chip->reset = devm_gpiod_get_optional(dev, "reset", GPIOD_ASIS);
 	if (IS_ERR(chip->reset))
 		return PTR_ERR(chip->reset);
 
-	if (mv88e6xxx_has(chip, MV88E6XXX_FLAGS_EEPROM16) &&
+	if (chip->info->ops->get_eeprom &&
 	    !of_property_read_u32(np, "eeprom-length", &eeprom_len))
 		chip->eeprom_len = eeprom_len;
 
@@ -4049,8 +3853,9 @@ static int mv88e6xxx_probe(struct mdio_device *mdiodev)
 static void mv88e6xxx_remove(struct mdio_device *mdiodev)
 {
 	struct dsa_switch *ds = dev_get_drvdata(&mdiodev->dev);
-	struct mv88e6xxx_chip *chip = ds_to_priv(ds);
+	struct mv88e6xxx_chip *chip = ds->priv;
 
+	mv88e6xxx_phy_destroy(chip);
 	mv88e6xxx_unregister_switch(chip);
 	mv88e6xxx_mdio_unregister(chip);
 }
@@ -4076,7 +3881,7 @@ static struct mdio_driver mv88e6xxx_driver = {
 
 static int __init mv88e6xxx_init(void)
 {
-	register_switch_driver(&mv88e6xxx_switch_driver);
+	register_switch_driver(&mv88e6xxx_switch_ops);
 	return mdio_driver_register(&mv88e6xxx_driver);
 }
 module_init(mv88e6xxx_init);
@@ -4084,7 +3889,7 @@ module_init(mv88e6xxx_init);
 static void __exit mv88e6xxx_cleanup(void)
 {
 	mdio_driver_unregister(&mv88e6xxx_driver);
-	unregister_switch_driver(&mv88e6xxx_switch_driver);
+	unregister_switch_driver(&mv88e6xxx_switch_ops);
 }
 module_exit(mv88e6xxx_cleanup);
 
diff --git a/drivers/net/dsa/mv88e6xxx/global1.c b/drivers/net/dsa/mv88e6xxx/global1.c
new file mode 100644
index 000000000000..d358720b6c2d
--- /dev/null
+++ b/drivers/net/dsa/mv88e6xxx/global1.c
@@ -0,0 +1,34 @@
+/*
+ * Marvell 88E6xxx Switch Global (1) Registers support
+ *
+ * Copyright (c) 2008 Marvell Semiconductor
+ *
+ * Copyright (c) 2016 Vivien Didelot <vivien.didelot@savoirfairelinux.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 "mv88e6xxx.h"
+#include "global1.h"
+
+int mv88e6xxx_g1_read(struct mv88e6xxx_chip *chip, int reg, u16 *val)
+{
+	int addr = chip->info->global1_addr;
+
+	return mv88e6xxx_read(chip, addr, reg, val);
+}
+
+int mv88e6xxx_g1_write(struct mv88e6xxx_chip *chip, int reg, u16 val)
+{
+	int addr = chip->info->global1_addr;
+
+	return mv88e6xxx_write(chip, addr, reg, val);
+}
+
+int mv88e6xxx_g1_wait(struct mv88e6xxx_chip *chip, int reg, u16 mask)
+{
+	return mv88e6xxx_wait(chip, chip->info->global1_addr, reg, mask);
+}
diff --git a/drivers/net/dsa/mv88e6xxx/global1.h b/drivers/net/dsa/mv88e6xxx/global1.h
new file mode 100644
index 000000000000..62291e6fe3a3
--- /dev/null
+++ b/drivers/net/dsa/mv88e6xxx/global1.h
@@ -0,0 +1,23 @@
+/*
+ * Marvell 88E6xxx Switch Global (1) Registers support
+ *
+ * Copyright (c) 2008 Marvell Semiconductor
+ *
+ * Copyright (c) 2016 Vivien Didelot <vivien.didelot@savoirfairelinux.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#ifndef _MV88E6XXX_GLOBAL1_H
+#define _MV88E6XXX_GLOBAL1_H
+
+#include "mv88e6xxx.h"
+
+int mv88e6xxx_g1_read(struct mv88e6xxx_chip *chip, int reg, u16 *val);
+int mv88e6xxx_g1_write(struct mv88e6xxx_chip *chip, int reg, u16 val);
+int mv88e6xxx_g1_wait(struct mv88e6xxx_chip *chip, int reg, u16 mask);
+
+#endif /* _MV88E6XXX_GLOBAL1_H */
diff --git a/drivers/net/dsa/mv88e6xxx/global2.c b/drivers/net/dsa/mv88e6xxx/global2.c
new file mode 100644
index 000000000000..cf686e7506a9
--- /dev/null
+++ b/drivers/net/dsa/mv88e6xxx/global2.c
@@ -0,0 +1,491 @@
+/*
+ * Marvell 88E6xxx Switch Global 2 Registers support (device address 0x1C)
+ *
+ * Copyright (c) 2008 Marvell Semiconductor
+ *
+ * Copyright (c) 2016 Vivien Didelot <vivien.didelot@savoirfairelinux.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 "mv88e6xxx.h"
+#include "global2.h"
+
+#define ADDR_GLOBAL2	0x1c
+
+static int mv88e6xxx_g2_read(struct mv88e6xxx_chip *chip, int reg, u16 *val)
+{
+	return mv88e6xxx_read(chip, ADDR_GLOBAL2, reg, val);
+}
+
+static int mv88e6xxx_g2_write(struct mv88e6xxx_chip *chip, int reg, u16 val)
+{
+	return mv88e6xxx_write(chip, ADDR_GLOBAL2, reg, val);
+}
+
+static int mv88e6xxx_g2_update(struct mv88e6xxx_chip *chip, int reg, u16 update)
+{
+	return mv88e6xxx_update(chip, ADDR_GLOBAL2, reg, update);
+}
+
+static int mv88e6xxx_g2_wait(struct mv88e6xxx_chip *chip, int reg, u16 mask)
+{
+	return mv88e6xxx_wait(chip, ADDR_GLOBAL2, reg, mask);
+}
+
+/* Offset 0x06: Device Mapping Table register */
+
+static int mv88e6xxx_g2_device_mapping_write(struct mv88e6xxx_chip *chip,
+					     int target, int port)
+{
+	u16 val = (target << 8) | (port & 0xf);
+
+	return mv88e6xxx_g2_update(chip, GLOBAL2_DEVICE_MAPPING, val);
+}
+
+static int mv88e6xxx_g2_set_device_mapping(struct mv88e6xxx_chip *chip)
+{
+	int target, port;
+	int err;
+
+	/* Initialize the routing port to the 32 possible target devices */
+	for (target = 0; target < 32; ++target) {
+		port = 0xf;
+
+		if (target < DSA_MAX_SWITCHES) {
+			port = chip->ds->rtable[target];
+			if (port == DSA_RTABLE_NONE)
+				port = 0xf;
+		}
+
+		err = mv88e6xxx_g2_device_mapping_write(chip, target, port);
+		if (err)
+			break;
+	}
+
+	return err;
+}
+
+/* Offset 0x07: Trunk Mask Table register */
+
+static int mv88e6xxx_g2_trunk_mask_write(struct mv88e6xxx_chip *chip, int num,
+					 bool hask, u16 mask)
+{
+	const u16 port_mask = BIT(mv88e6xxx_num_ports(chip)) - 1;
+	u16 val = (num << 12) | (mask & port_mask);
+
+	if (hask)
+		val |= GLOBAL2_TRUNK_MASK_HASK;
+
+	return mv88e6xxx_g2_update(chip, GLOBAL2_TRUNK_MASK, val);
+}
+
+/* Offset 0x08: Trunk Mapping Table register */
+
+static int mv88e6xxx_g2_trunk_mapping_write(struct mv88e6xxx_chip *chip, int id,
+					    u16 map)
+{
+	const u16 port_mask = BIT(mv88e6xxx_num_ports(chip)) - 1;
+	u16 val = (id << 11) | (map & port_mask);
+
+	return mv88e6xxx_g2_update(chip, GLOBAL2_TRUNK_MAPPING, val);
+}
+
+static int mv88e6xxx_g2_clear_trunk(struct mv88e6xxx_chip *chip)
+{
+	const u16 port_mask = BIT(mv88e6xxx_num_ports(chip)) - 1;
+	int i, err;
+
+	/* Clear all eight possible Trunk Mask vectors */
+	for (i = 0; i < 8; ++i) {
+		err = mv88e6xxx_g2_trunk_mask_write(chip, i, false, port_mask);
+		if (err)
+			return err;
+	}
+
+	/* Clear all sixteen possible Trunk ID routing vectors */
+	for (i = 0; i < 16; ++i) {
+		err = mv88e6xxx_g2_trunk_mapping_write(chip, i, 0);
+		if (err)
+			return err;
+	}
+
+	return 0;
+}
+
+/* Offset 0x09: Ingress Rate Command register
+ * Offset 0x0A: Ingress Rate Data register
+ */
+
+static int mv88e6xxx_g2_clear_irl(struct mv88e6xxx_chip *chip)
+{
+	int port, err;
+
+	/* Init all Ingress Rate Limit resources of all ports */
+	for (port = 0; port < mv88e6xxx_num_ports(chip); ++port) {
+		/* XXX newer chips (like 88E6390) have different 2-bit ops */
+		err = mv88e6xxx_g2_write(chip, GLOBAL2_IRL_CMD,
+					 GLOBAL2_IRL_CMD_OP_INIT_ALL |
+					 (port << 8));
+		if (err)
+			break;
+
+		/* Wait for the operation to complete */
+		err = mv88e6xxx_g2_wait(chip, GLOBAL2_IRL_CMD,
+					GLOBAL2_IRL_CMD_BUSY);
+		if (err)
+			break;
+	}
+
+	return err;
+}
+
+/* Offset 0x0D: Switch MAC/WoL/WoF register */
+
+static int mv88e6xxx_g2_switch_mac_write(struct mv88e6xxx_chip *chip,
+					 unsigned int pointer, u8 data)
+{
+	u16 val = (pointer << 8) | data;
+
+	return mv88e6xxx_g2_update(chip, GLOBAL2_SWITCH_MAC, val);
+}
+
+int mv88e6xxx_g2_set_switch_mac(struct mv88e6xxx_chip *chip, u8 *addr)
+{
+	int i, err;
+
+	for (i = 0; i < 6; i++) {
+		err = mv88e6xxx_g2_switch_mac_write(chip, i, addr[i]);
+		if (err)
+			break;
+	}
+
+	return err;
+}
+
+/* Offset 0x0F: Priority Override Table */
+
+static int mv88e6xxx_g2_pot_write(struct mv88e6xxx_chip *chip, int pointer,
+				  u8 data)
+{
+	u16 val = (pointer << 8) | (data & 0x7);
+
+	return mv88e6xxx_g2_update(chip, GLOBAL2_PRIO_OVERRIDE, val);
+}
+
+static int mv88e6xxx_g2_clear_pot(struct mv88e6xxx_chip *chip)
+{
+	int i, err;
+
+	/* Clear all sixteen possible Priority Override entries */
+	for (i = 0; i < 16; i++) {
+		err = mv88e6xxx_g2_pot_write(chip, i, 0);
+		if (err)
+			break;
+	}
+
+	return err;
+}
+
+/* Offset 0x14: EEPROM Command
+ * Offset 0x15: EEPROM Data
+ */
+
+static int mv88e6xxx_g2_eeprom_wait(struct mv88e6xxx_chip *chip)
+{
+	return mv88e6xxx_g2_wait(chip, GLOBAL2_EEPROM_CMD,
+				 GLOBAL2_EEPROM_CMD_BUSY |
+				 GLOBAL2_EEPROM_CMD_RUNNING);
+}
+
+static int mv88e6xxx_g2_eeprom_cmd(struct mv88e6xxx_chip *chip, u16 cmd)
+{
+	int err;
+
+	err = mv88e6xxx_g2_write(chip, GLOBAL2_EEPROM_CMD, cmd);
+	if (err)
+		return err;
+
+	return mv88e6xxx_g2_eeprom_wait(chip);
+}
+
+static int mv88e6xxx_g2_eeprom_read16(struct mv88e6xxx_chip *chip,
+				      u8 addr, u16 *data)
+{
+	u16 cmd = GLOBAL2_EEPROM_CMD_OP_READ | addr;
+	int err;
+
+	err = mv88e6xxx_g2_eeprom_wait(chip);
+	if (err)
+		return err;
+
+	err = mv88e6xxx_g2_eeprom_cmd(chip, cmd);
+	if (err)
+		return err;
+
+	return mv88e6xxx_g2_read(chip, GLOBAL2_EEPROM_DATA, data);
+}
+
+static int mv88e6xxx_g2_eeprom_write16(struct mv88e6xxx_chip *chip,
+				       u8 addr, u16 data)
+{
+	u16 cmd = GLOBAL2_EEPROM_CMD_OP_WRITE | addr;
+	int err;
+
+	err = mv88e6xxx_g2_eeprom_wait(chip);
+	if (err)
+		return err;
+
+	err = mv88e6xxx_g2_write(chip, GLOBAL2_EEPROM_DATA, data);
+	if (err)
+		return err;
+
+	return mv88e6xxx_g2_eeprom_cmd(chip, cmd);
+}
+
+int mv88e6xxx_g2_get_eeprom16(struct mv88e6xxx_chip *chip,
+			      struct ethtool_eeprom *eeprom, u8 *data)
+{
+	unsigned int offset = eeprom->offset;
+	unsigned int len = eeprom->len;
+	u16 val;
+	int err;
+
+	eeprom->len = 0;
+
+	if (offset & 1) {
+		err = mv88e6xxx_g2_eeprom_read16(chip, offset >> 1, &val);
+		if (err)
+			return err;
+
+		*data++ = (val >> 8) & 0xff;
+
+		offset++;
+		len--;
+		eeprom->len++;
+	}
+
+	while (len >= 2) {
+		err = mv88e6xxx_g2_eeprom_read16(chip, offset >> 1, &val);
+		if (err)
+			return err;
+
+		*data++ = val & 0xff;
+		*data++ = (val >> 8) & 0xff;
+
+		offset += 2;
+		len -= 2;
+		eeprom->len += 2;
+	}
+
+	if (len) {
+		err = mv88e6xxx_g2_eeprom_read16(chip, offset >> 1, &val);
+		if (err)
+			return err;
+
+		*data++ = val & 0xff;
+
+		offset++;
+		len--;
+		eeprom->len++;
+	}
+
+	return 0;
+}
+
+int mv88e6xxx_g2_set_eeprom16(struct mv88e6xxx_chip *chip,
+			      struct ethtool_eeprom *eeprom, u8 *data)
+{
+	unsigned int offset = eeprom->offset;
+	unsigned int len = eeprom->len;
+	u16 val;
+	int err;
+
+	/* Ensure the RO WriteEn bit is set */
+	err = mv88e6xxx_g2_read(chip, GLOBAL2_EEPROM_CMD, &val);
+	if (err)
+		return err;
+
+	if (!(val & GLOBAL2_EEPROM_CMD_WRITE_EN))
+		return -EROFS;
+
+	eeprom->len = 0;
+
+	if (offset & 1) {
+		err = mv88e6xxx_g2_eeprom_read16(chip, offset >> 1, &val);
+		if (err)
+			return err;
+
+		val = (*data++ << 8) | (val & 0xff);
+
+		err = mv88e6xxx_g2_eeprom_write16(chip, offset >> 1, val);
+		if (err)
+			return err;
+
+		offset++;
+		len--;
+		eeprom->len++;
+	}
+
+	while (len >= 2) {
+		val = *data++;
+		val |= *data++ << 8;
+
+		err = mv88e6xxx_g2_eeprom_write16(chip, offset >> 1, val);
+		if (err)
+			return err;
+
+		offset += 2;
+		len -= 2;
+		eeprom->len += 2;
+	}
+
+	if (len) {
+		err = mv88e6xxx_g2_eeprom_read16(chip, offset >> 1, &val);
+		if (err)
+			return err;
+
+		val = (val & 0xff00) | *data++;
+
+		err = mv88e6xxx_g2_eeprom_write16(chip, offset >> 1, val);
+		if (err)
+			return err;
+
+		offset++;
+		len--;
+		eeprom->len++;
+	}
+
+	return 0;
+}
+
+/* Offset 0x18: SMI PHY Command Register
+ * Offset 0x19: SMI PHY Data Register
+ */
+
+static int mv88e6xxx_g2_smi_phy_wait(struct mv88e6xxx_chip *chip)
+{
+	return mv88e6xxx_g2_wait(chip, GLOBAL2_SMI_PHY_CMD,
+				 GLOBAL2_SMI_PHY_CMD_BUSY);
+}
+
+static int mv88e6xxx_g2_smi_phy_cmd(struct mv88e6xxx_chip *chip, u16 cmd)
+{
+	int err;
+
+	err = mv88e6xxx_g2_write(chip, GLOBAL2_SMI_PHY_CMD, cmd);
+	if (err)
+		return err;
+
+	return mv88e6xxx_g2_smi_phy_wait(chip);
+}
+
+int mv88e6xxx_g2_smi_phy_read(struct mv88e6xxx_chip *chip, int addr, int reg,
+			      u16 *val)
+{
+	u16 cmd = GLOBAL2_SMI_PHY_CMD_OP_22_READ_DATA | (addr << 5) | reg;
+	int err;
+
+	err = mv88e6xxx_g2_smi_phy_wait(chip);
+	if (err)
+		return err;
+
+	err = mv88e6xxx_g2_smi_phy_cmd(chip, cmd);
+	if (err)
+		return err;
+
+	return mv88e6xxx_g2_read(chip, GLOBAL2_SMI_PHY_DATA, val);
+}
+
+int mv88e6xxx_g2_smi_phy_write(struct mv88e6xxx_chip *chip, int addr, int reg,
+			       u16 val)
+{
+	u16 cmd = GLOBAL2_SMI_PHY_CMD_OP_22_WRITE_DATA | (addr << 5) | reg;
+	int err;
+
+	err = mv88e6xxx_g2_smi_phy_wait(chip);
+	if (err)
+		return err;
+
+	err = mv88e6xxx_g2_write(chip, GLOBAL2_SMI_PHY_DATA, val);
+	if (err)
+		return err;
+
+	return mv88e6xxx_g2_smi_phy_cmd(chip, cmd);
+}
+
+int mv88e6xxx_g2_setup(struct mv88e6xxx_chip *chip)
+{
+	u16 reg;
+	int err;
+
+	if (mv88e6xxx_has(chip, MV88E6XXX_FLAG_G2_MGMT_EN_2X)) {
+		/* Consider the frames with reserved multicast destination
+		 * addresses matching 01:80:c2:00:00:2x as MGMT.
+		 */
+		err = mv88e6xxx_g2_write(chip, GLOBAL2_MGMT_EN_2X, 0xffff);
+		if (err)
+			return err;
+	}
+
+	if (mv88e6xxx_has(chip, MV88E6XXX_FLAG_G2_MGMT_EN_0X)) {
+		/* Consider the frames with reserved multicast destination
+		 * addresses matching 01:80:c2:00:00:0x as MGMT.
+		 */
+		err = mv88e6xxx_g2_write(chip, GLOBAL2_MGMT_EN_0X, 0xffff);
+		if (err)
+			return err;
+	}
+
+	/* 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 = GLOBAL2_SWITCH_MGMT_FORCE_FLOW_CTRL_PRI | (0x7 << 4);
+	if (mv88e6xxx_has(chip, MV88E6XXX_FLAG_G2_MGMT_EN_0X) ||
+	    mv88e6xxx_has(chip, MV88E6XXX_FLAG_G2_MGMT_EN_2X))
+		reg |= GLOBAL2_SWITCH_MGMT_RSVD2CPU | 0x7;
+	err = mv88e6xxx_g2_write(chip, GLOBAL2_SWITCH_MGMT, reg);
+	if (err)
+		return err;
+
+	/* Program the DSA routing table. */
+	err = mv88e6xxx_g2_set_device_mapping(chip);
+	if (err)
+		return err;
+
+	/* Clear all trunk masks and mapping. */
+	err = mv88e6xxx_g2_clear_trunk(chip);
+	if (err)
+		return err;
+
+	if (mv88e6xxx_has(chip, MV88E6XXX_FLAGS_IRL)) {
+		/* Disable ingress rate limiting by resetting all per port
+		 * ingress rate limit resources to their initial state.
+		 */
+		err = mv88e6xxx_g2_clear_irl(chip);
+			if (err)
+				return err;
+	}
+
+	if (mv88e6xxx_has(chip, MV88E6XXX_FLAGS_PVT)) {
+		/* Initialize Cross-chip Port VLAN Table to reset defaults */
+		err = mv88e6xxx_g2_write(chip, GLOBAL2_PVT_ADDR,
+					 GLOBAL2_PVT_ADDR_OP_INIT_ONES);
+		if (err)
+			return err;
+	}
+
+	if (mv88e6xxx_has(chip, MV88E6XXX_FLAG_G2_POT)) {
+		/* Clear the priority override table. */
+		err = mv88e6xxx_g2_clear_pot(chip);
+		if (err)
+			return err;
+	}
+
+	return 0;
+}
diff --git a/drivers/net/dsa/mv88e6xxx/global2.h b/drivers/net/dsa/mv88e6xxx/global2.h
new file mode 100644
index 000000000000..c4bb9035ee3a
--- /dev/null
+++ b/drivers/net/dsa/mv88e6xxx/global2.h
@@ -0,0 +1,88 @@
+/*
+ * Marvell 88E6xxx Switch Global 2 Registers support (device address 0x1C)
+ *
+ * Copyright (c) 2008 Marvell Semiconductor
+ *
+ * Copyright (c) 2016 Vivien Didelot <vivien.didelot@savoirfairelinux.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#ifndef _MV88E6XXX_GLOBAL2_H
+#define _MV88E6XXX_GLOBAL2_H
+
+#include "mv88e6xxx.h"
+
+#ifdef CONFIG_NET_DSA_MV88E6XXX_GLOBAL2
+
+static inline int mv88e6xxx_g2_require(struct mv88e6xxx_chip *chip)
+{
+	return 0;
+}
+
+int mv88e6xxx_g2_smi_phy_read(struct mv88e6xxx_chip *chip, int addr, int reg,
+			      u16 *val);
+int mv88e6xxx_g2_smi_phy_write(struct mv88e6xxx_chip *chip, int addr, int reg,
+			       u16 val);
+int mv88e6xxx_g2_set_switch_mac(struct mv88e6xxx_chip *chip, u8 *addr);
+int mv88e6xxx_g2_get_eeprom16(struct mv88e6xxx_chip *chip,
+			      struct ethtool_eeprom *eeprom, u8 *data);
+int mv88e6xxx_g2_set_eeprom16(struct mv88e6xxx_chip *chip,
+			      struct ethtool_eeprom *eeprom, u8 *data);
+int mv88e6xxx_g2_setup(struct mv88e6xxx_chip *chip);
+
+#else /* !CONFIG_NET_DSA_MV88E6XXX_GLOBAL2 */
+
+static inline int mv88e6xxx_g2_require(struct mv88e6xxx_chip *chip)
+{
+	if (mv88e6xxx_has(chip, MV88E6XXX_FLAG_GLOBAL2)) {
+		dev_err(chip->dev, "this chip requires CONFIG_NET_DSA_MV88E6XXX_GLOBAL2 enabled\n");
+		return -EOPNOTSUPP;
+	}
+
+	return 0;
+}
+
+static inline int mv88e6xxx_g2_smi_phy_read(struct mv88e6xxx_chip *chip,
+					    int addr, int reg, u16 *val)
+{
+	return -EOPNOTSUPP;
+}
+
+static inline int mv88e6xxx_g2_smi_phy_write(struct mv88e6xxx_chip *chip,
+					     int addr, int reg, u16 val)
+{
+	return -EOPNOTSUPP;
+}
+
+static inline int mv88e6xxx_g2_set_switch_mac(struct mv88e6xxx_chip *chip,
+					      u8 *addr)
+{
+	return -EOPNOTSUPP;
+}
+
+static inline int mv88e6xxx_g2_get_eeprom16(struct mv88e6xxx_chip *chip,
+					    struct ethtool_eeprom *eeprom,
+					    u8 *data)
+{
+	return -EOPNOTSUPP;
+}
+
+static inline int mv88e6xxx_g2_set_eeprom16(struct mv88e6xxx_chip *chip,
+					    struct ethtool_eeprom *eeprom,
+					    u8 *data)
+{
+	return -EOPNOTSUPP;
+}
+
+static inline int mv88e6xxx_g2_setup(struct mv88e6xxx_chip *chip)
+{
+	return -EOPNOTSUPP;
+}
+
+#endif /* CONFIG_NET_DSA_MV88E6XXX_GLOBAL2 */
+
+#endif /* _MV88E6XXX_GLOBAL2_H */
diff --git a/drivers/net/dsa/mv88e6xxx/mv88e6xxx.h b/drivers/net/dsa/mv88e6xxx/mv88e6xxx.h
index 48d6ea77f9bd..e572121c196e 100644
--- a/drivers/net/dsa/mv88e6xxx/mv88e6xxx.h
+++ b/drivers/net/dsa/mv88e6xxx/mv88e6xxx.h
@@ -30,11 +30,13 @@
 #define SMI_CMD_OP_45_READ_DATA_INC	((3 << 10) | SMI_CMD_BUSY)
 #define SMI_DATA		0x01
 
-/* Fiber/SERDES Registers are located at SMI address F, page 1 */
-#define REG_FIBER_SERDES	0x0f
-#define PAGE_FIBER_SERDES	0x01
+/* PHY Registers */
+#define PHY_PAGE		0x16
+#define PHY_PAGE_COPPER		0x00
+
+#define ADDR_SERDES		0x0f
+#define SERDES_PAGE_FIBER	0x01
 
-#define REG_PORT(p)		(0x10 + (p))
 #define PORT_STATUS		0x00
 #define PORT_STATUS_PAUSE_EN	BIT(15)
 #define PORT_STATUS_MY_PAUSE	BIT(14)
@@ -157,7 +159,6 @@
 #define PORT_TAG_REGMAP_0123	0x18
 #define PORT_TAG_REGMAP_4567	0x19
 
-#define REG_GLOBAL		0x1b
 #define GLOBAL_STATUS		0x00
 #define GLOBAL_STATUS_PPU_STATE BIT(15) /* 6351 and 6171 */
 /* Two bits for 6165, 6185 etc */
@@ -169,8 +170,8 @@
 #define GLOBAL_MAC_01		0x01
 #define GLOBAL_MAC_23		0x02
 #define GLOBAL_MAC_45		0x03
-#define GLOBAL_ATU_FID		0x01	/* 6097 6165 6351 6352 */
-#define GLOBAL_VTU_FID		0x02	/* 6097 6165 6351 6352 */
+#define GLOBAL_ATU_FID		0x01
+#define GLOBAL_VTU_FID		0x02
 #define GLOBAL_VTU_FID_MASK	0xfff
 #define GLOBAL_VTU_SID		0x03	/* 6097 6165 6351 6352 */
 #define GLOBAL_VTU_SID_MASK	0x3f
@@ -275,7 +276,6 @@
 #define GLOBAL_STATS_COUNTER_32	0x1e
 #define GLOBAL_STATS_COUNTER_01	0x1f
 
-#define REG_GLOBAL2		0x1c
 #define GLOBAL2_INT_SOURCE	0x00
 #define GLOBAL2_INT_MASK	0x01
 #define GLOBAL2_MGMT_EN_2X	0x02
@@ -329,17 +329,16 @@
 #define GLOBAL2_EEPROM_DATA	0x15
 #define GLOBAL2_PTP_AVB_OP	0x16
 #define GLOBAL2_PTP_AVB_DATA	0x17
-#define GLOBAL2_SMI_OP		0x18
-#define GLOBAL2_SMI_OP_BUSY		BIT(15)
-#define GLOBAL2_SMI_OP_CLAUSE_22	BIT(12)
-#define GLOBAL2_SMI_OP_22_WRITE		((1 << 10) | GLOBAL2_SMI_OP_BUSY | \
-					 GLOBAL2_SMI_OP_CLAUSE_22)
-#define GLOBAL2_SMI_OP_22_READ		((2 << 10) | GLOBAL2_SMI_OP_BUSY | \
-					 GLOBAL2_SMI_OP_CLAUSE_22)
-#define GLOBAL2_SMI_OP_45_WRITE_ADDR	((0 << 10) | GLOBAL2_SMI_OP_BUSY)
-#define GLOBAL2_SMI_OP_45_WRITE_DATA	((1 << 10) | GLOBAL2_SMI_OP_BUSY)
-#define GLOBAL2_SMI_OP_45_READ_DATA	((2 << 10) | GLOBAL2_SMI_OP_BUSY)
-#define GLOBAL2_SMI_DATA	0x19
+#define GLOBAL2_SMI_PHY_CMD			0x18
+#define GLOBAL2_SMI_PHY_CMD_BUSY		BIT(15)
+#define GLOBAL2_SMI_PHY_CMD_MODE_22		BIT(12)
+#define GLOBAL2_SMI_PHY_CMD_OP_22_WRITE_DATA	((0x1 << 10) | \
+						 GLOBAL2_SMI_PHY_CMD_MODE_22 | \
+						 GLOBAL2_SMI_PHY_CMD_BUSY)
+#define GLOBAL2_SMI_PHY_CMD_OP_22_READ_DATA	((0x2 << 10) | \
+						 GLOBAL2_SMI_PHY_CMD_MODE_22 | \
+						 GLOBAL2_SMI_PHY_CMD_BUSY)
+#define GLOBAL2_SMI_PHY_DATA			0x19
 #define GLOBAL2_SCRATCH_MISC	0x1a
 #define GLOBAL2_SCRATCH_BUSY		BIT(15)
 #define GLOBAL2_SCRATCH_REGISTER_SHIFT	8
@@ -384,10 +383,36 @@ enum mv88e6xxx_family {
 };
 
 enum mv88e6xxx_cap {
+	/* Two different tag protocols can be used by the driver. All
+	 * switches support DSA, but only later generations support
+	 * EDSA.
+	 */
+	MV88E6XXX_CAP_EDSA,
+
 	/* Energy Efficient Ethernet.
 	 */
 	MV88E6XXX_CAP_EEE,
 
+	/* Multi-chip Addressing Mode.
+	 * Some chips respond to only 2 registers of its own SMI device address
+	 * when it is non-zero, and use indirect access to internal registers.
+	 */
+	MV88E6XXX_CAP_SMI_CMD,		/* (0x00) SMI Command */
+	MV88E6XXX_CAP_SMI_DATA,		/* (0x01) SMI Data */
+
+	/* PHY Registers.
+	 */
+	MV88E6XXX_CAP_PHY_PAGE,		/* (0x16) Page Register */
+
+	/* Fiber/SERDES Registers (SMI address F).
+	 */
+	MV88E6XXX_CAP_SERDES,
+
+	/* Switch Global (1) Registers.
+	 */
+	MV88E6XXX_CAP_G1_ATU_FID,	/* (0x01) ATU FID Register */
+	MV88E6XXX_CAP_G1_VTU_FID,	/* (0x02) VTU FID Register */
+
 	/* Switch Global 2 Registers.
 	 * The device contains a second set of global 16-bit registers.
 	 */
@@ -398,16 +423,7 @@ enum mv88e6xxx_cap {
 	MV88E6XXX_CAP_G2_IRL_DATA,	/* (0x0a) Ingress Rate Data */
 	MV88E6XXX_CAP_G2_PVT_ADDR,	/* (0x0b) Cross Chip Port VLAN Addr */
 	MV88E6XXX_CAP_G2_PVT_DATA,	/* (0x0c) Cross Chip Port VLAN Data */
-	MV88E6XXX_CAP_G2_SWITCH_MAC,	/* (0x0d) Switch MAC/WoL/WoF */
 	MV88E6XXX_CAP_G2_POT,		/* (0x0f) Priority Override Table */
-	MV88E6XXX_CAP_G2_EEPROM_CMD,	/* (0x14) EEPROM Command */
-	MV88E6XXX_CAP_G2_EEPROM_DATA,	/* (0x15) EEPROM Data */
-
-	/* Multi-chip Addressing Mode.
-	 * Some chips require an indirect SMI access when their SMI device
-	 * address is not zero. See SMI_CMD and SMI_DATA.
-	 */
-	MV88E6XXX_CAP_MULTI_CHIP,
 
 	/* PHY Polling Unit.
 	 * See GLOBAL_CONTROL_PPU_ENABLE and GLOBAL_STATUS_PPU_POLLING.
@@ -415,12 +431,6 @@ enum mv88e6xxx_cap {
 	MV88E6XXX_CAP_PPU,
 	MV88E6XXX_CAP_PPU_ACTIVE,
 
-	/* SMI PHY Command and Data registers.
-	 * This requires an indirect access to PHY registers through
-	 * GLOBAL2_SMI_OP, otherwise direct access to PHY registers is done.
-	 */
-	MV88E6XXX_CAP_SMI_PHY,
-
 	/* Per VLAN Spanning Tree Unit (STU).
 	 * The Port State database, if present, is accessed through VTU
 	 * operations and dedicated SID registers. See GLOBAL_VTU_SID.
@@ -440,130 +450,148 @@ enum mv88e6xxx_cap {
 };
 
 /* Bitmask of capabilities */
-#define MV88E6XXX_FLAG_EEE		BIT(MV88E6XXX_CAP_EEE)
-#define MV88E6XXX_FLAG_GLOBAL2		BIT(MV88E6XXX_CAP_GLOBAL2)
-#define MV88E6XXX_FLAG_G2_MGMT_EN_2X	BIT(MV88E6XXX_CAP_G2_MGMT_EN_2X)
-#define MV88E6XXX_FLAG_G2_MGMT_EN_0X	BIT(MV88E6XXX_CAP_G2_MGMT_EN_0X)
-#define MV88E6XXX_FLAG_G2_IRL_CMD	BIT(MV88E6XXX_CAP_G2_IRL_CMD)
-#define MV88E6XXX_FLAG_G2_IRL_DATA	BIT(MV88E6XXX_CAP_G2_IRL_DATA)
-#define MV88E6XXX_FLAG_G2_PVT_ADDR	BIT(MV88E6XXX_CAP_G2_PVT_ADDR)
-#define MV88E6XXX_FLAG_G2_PVT_DATA	BIT(MV88E6XXX_CAP_G2_PVT_DATA)
-#define MV88E6XXX_FLAG_G2_SWITCH_MAC	BIT(MV88E6XXX_CAP_G2_SWITCH_MAC)
-#define MV88E6XXX_FLAG_G2_POT		BIT(MV88E6XXX_CAP_G2_POT)
-#define MV88E6XXX_FLAG_G2_EEPROM_CMD	BIT(MV88E6XXX_CAP_G2_EEPROM_CMD)
-#define MV88E6XXX_FLAG_G2_EEPROM_DATA	BIT(MV88E6XXX_CAP_G2_EEPROM_DATA)
-#define MV88E6XXX_FLAG_MULTI_CHIP	BIT(MV88E6XXX_CAP_MULTI_CHIP)
-#define MV88E6XXX_FLAG_PPU		BIT(MV88E6XXX_CAP_PPU)
-#define MV88E6XXX_FLAG_PPU_ACTIVE	BIT(MV88E6XXX_CAP_PPU_ACTIVE)
-#define MV88E6XXX_FLAG_SMI_PHY		BIT(MV88E6XXX_CAP_SMI_PHY)
-#define MV88E6XXX_FLAG_STU		BIT(MV88E6XXX_CAP_STU)
-#define MV88E6XXX_FLAG_TEMP		BIT(MV88E6XXX_CAP_TEMP)
-#define MV88E6XXX_FLAG_TEMP_LIMIT	BIT(MV88E6XXX_CAP_TEMP_LIMIT)
-#define MV88E6XXX_FLAG_VTU		BIT(MV88E6XXX_CAP_VTU)
-
-/* EEPROM Programming via Global2 with 16-bit data */
-#define MV88E6XXX_FLAGS_EEPROM16	\
-	(MV88E6XXX_FLAG_G2_EEPROM_CMD |	\
-	 MV88E6XXX_FLAG_G2_EEPROM_DATA)
+#define MV88E6XXX_FLAG_EDSA		BIT_ULL(MV88E6XXX_CAP_EDSA)
+#define MV88E6XXX_FLAG_EEE		BIT_ULL(MV88E6XXX_CAP_EEE)
+
+#define MV88E6XXX_FLAG_SMI_CMD		BIT_ULL(MV88E6XXX_CAP_SMI_CMD)
+#define MV88E6XXX_FLAG_SMI_DATA		BIT_ULL(MV88E6XXX_CAP_SMI_DATA)
+
+#define MV88E6XXX_FLAG_PHY_PAGE		BIT_ULL(MV88E6XXX_CAP_PHY_PAGE)
+
+#define MV88E6XXX_FLAG_SERDES		BIT_ULL(MV88E6XXX_CAP_SERDES)
+
+#define MV88E6XXX_FLAG_G1_ATU_FID	BIT_ULL(MV88E6XXX_CAP_G1_ATU_FID)
+#define MV88E6XXX_FLAG_G1_VTU_FID	BIT_ULL(MV88E6XXX_CAP_G1_VTU_FID)
+
+#define MV88E6XXX_FLAG_GLOBAL2		BIT_ULL(MV88E6XXX_CAP_GLOBAL2)
+#define MV88E6XXX_FLAG_G2_MGMT_EN_2X	BIT_ULL(MV88E6XXX_CAP_G2_MGMT_EN_2X)
+#define MV88E6XXX_FLAG_G2_MGMT_EN_0X	BIT_ULL(MV88E6XXX_CAP_G2_MGMT_EN_0X)
+#define MV88E6XXX_FLAG_G2_IRL_CMD	BIT_ULL(MV88E6XXX_CAP_G2_IRL_CMD)
+#define MV88E6XXX_FLAG_G2_IRL_DATA	BIT_ULL(MV88E6XXX_CAP_G2_IRL_DATA)
+#define MV88E6XXX_FLAG_G2_PVT_ADDR	BIT_ULL(MV88E6XXX_CAP_G2_PVT_ADDR)
+#define MV88E6XXX_FLAG_G2_PVT_DATA	BIT_ULL(MV88E6XXX_CAP_G2_PVT_DATA)
+#define MV88E6XXX_FLAG_G2_POT		BIT_ULL(MV88E6XXX_CAP_G2_POT)
+
+#define MV88E6XXX_FLAG_PPU		BIT_ULL(MV88E6XXX_CAP_PPU)
+#define MV88E6XXX_FLAG_PPU_ACTIVE	BIT_ULL(MV88E6XXX_CAP_PPU_ACTIVE)
+#define MV88E6XXX_FLAG_STU		BIT_ULL(MV88E6XXX_CAP_STU)
+#define MV88E6XXX_FLAG_TEMP		BIT_ULL(MV88E6XXX_CAP_TEMP)
+#define MV88E6XXX_FLAG_TEMP_LIMIT	BIT_ULL(MV88E6XXX_CAP_TEMP_LIMIT)
+#define MV88E6XXX_FLAG_VTU		BIT_ULL(MV88E6XXX_CAP_VTU)
 
 /* Ingress Rate Limit unit */
 #define MV88E6XXX_FLAGS_IRL		\
 	(MV88E6XXX_FLAG_G2_IRL_CMD |	\
 	 MV88E6XXX_FLAG_G2_IRL_DATA)
 
+/* Multi-chip Addressing Mode */
+#define MV88E6XXX_FLAGS_MULTI_CHIP	\
+	(MV88E6XXX_FLAG_SMI_CMD |	\
+	 MV88E6XXX_FLAG_SMI_DATA)
+
 /* Cross-chip Port VLAN Table */
 #define MV88E6XXX_FLAGS_PVT		\
 	(MV88E6XXX_FLAG_G2_PVT_ADDR |	\
 	 MV88E6XXX_FLAG_G2_PVT_DATA)
 
+/* Fiber/SERDES Registers at SMI address F, page 1 */
+#define MV88E6XXX_FLAGS_SERDES		\
+	(MV88E6XXX_FLAG_PHY_PAGE |	\
+	 MV88E6XXX_FLAG_SERDES)
+
 #define MV88E6XXX_FLAGS_FAMILY_6095	\
 	(MV88E6XXX_FLAG_GLOBAL2 |	\
 	 MV88E6XXX_FLAG_G2_MGMT_EN_0X |	\
-	 MV88E6XXX_FLAG_MULTI_CHIP |	\
 	 MV88E6XXX_FLAG_PPU |		\
-	 MV88E6XXX_FLAG_VTU)
+	 MV88E6XXX_FLAG_VTU |		\
+	 MV88E6XXX_FLAGS_MULTI_CHIP)
 
 #define MV88E6XXX_FLAGS_FAMILY_6097	\
-	(MV88E6XXX_FLAG_GLOBAL2 |	\
+	(MV88E6XXX_FLAG_G1_ATU_FID |	\
+	 MV88E6XXX_FLAG_G1_VTU_FID |	\
+	 MV88E6XXX_FLAG_GLOBAL2 |	\
 	 MV88E6XXX_FLAG_G2_MGMT_EN_2X |	\
 	 MV88E6XXX_FLAG_G2_MGMT_EN_0X |	\
 	 MV88E6XXX_FLAG_G2_POT |	\
-	 MV88E6XXX_FLAG_MULTI_CHIP |	\
 	 MV88E6XXX_FLAG_PPU |		\
 	 MV88E6XXX_FLAG_STU |		\
 	 MV88E6XXX_FLAG_VTU |		\
 	 MV88E6XXX_FLAGS_IRL |		\
+	 MV88E6XXX_FLAGS_MULTI_CHIP |	\
 	 MV88E6XXX_FLAGS_PVT)
 
 #define MV88E6XXX_FLAGS_FAMILY_6165	\
-	(MV88E6XXX_FLAG_GLOBAL2 |	\
+	(MV88E6XXX_FLAG_G1_ATU_FID |	\
+	 MV88E6XXX_FLAG_G1_VTU_FID |	\
+	 MV88E6XXX_FLAG_GLOBAL2 |	\
 	 MV88E6XXX_FLAG_G2_MGMT_EN_2X |	\
 	 MV88E6XXX_FLAG_G2_MGMT_EN_0X |	\
-	 MV88E6XXX_FLAG_G2_SWITCH_MAC |	\
 	 MV88E6XXX_FLAG_G2_POT |	\
-	 MV88E6XXX_FLAG_MULTI_CHIP |	\
 	 MV88E6XXX_FLAG_STU |		\
 	 MV88E6XXX_FLAG_TEMP |		\
 	 MV88E6XXX_FLAG_VTU |		\
 	 MV88E6XXX_FLAGS_IRL |		\
+	 MV88E6XXX_FLAGS_MULTI_CHIP |	\
 	 MV88E6XXX_FLAGS_PVT)
 
 #define MV88E6XXX_FLAGS_FAMILY_6185	\
 	(MV88E6XXX_FLAG_GLOBAL2 |	\
 	 MV88E6XXX_FLAG_G2_MGMT_EN_0X |	\
-	 MV88E6XXX_FLAG_MULTI_CHIP |	\
+	 MV88E6XXX_FLAGS_MULTI_CHIP |	\
 	 MV88E6XXX_FLAG_PPU |		\
 	 MV88E6XXX_FLAG_VTU)
 
 #define MV88E6XXX_FLAGS_FAMILY_6320	\
-	(MV88E6XXX_FLAG_EEE |		\
+	(MV88E6XXX_FLAG_EDSA |		\
+	 MV88E6XXX_FLAG_EEE |		\
 	 MV88E6XXX_FLAG_GLOBAL2 |	\
 	 MV88E6XXX_FLAG_G2_MGMT_EN_2X |	\
 	 MV88E6XXX_FLAG_G2_MGMT_EN_0X |	\
-	 MV88E6XXX_FLAG_G2_SWITCH_MAC |	\
 	 MV88E6XXX_FLAG_G2_POT |	\
-	 MV88E6XXX_FLAG_MULTI_CHIP |	\
 	 MV88E6XXX_FLAG_PPU_ACTIVE |	\
-	 MV88E6XXX_FLAG_SMI_PHY |	\
 	 MV88E6XXX_FLAG_TEMP |		\
 	 MV88E6XXX_FLAG_TEMP_LIMIT |	\
 	 MV88E6XXX_FLAG_VTU |		\
-	 MV88E6XXX_FLAGS_EEPROM16 |	\
 	 MV88E6XXX_FLAGS_IRL |		\
+	 MV88E6XXX_FLAGS_MULTI_CHIP |	\
 	 MV88E6XXX_FLAGS_PVT)
 
 #define MV88E6XXX_FLAGS_FAMILY_6351	\
-	(MV88E6XXX_FLAG_GLOBAL2 |	\
+	(MV88E6XXX_FLAG_EDSA |		\
+	 MV88E6XXX_FLAG_G1_ATU_FID |	\
+	 MV88E6XXX_FLAG_G1_VTU_FID |	\
+	 MV88E6XXX_FLAG_GLOBAL2 |	\
 	 MV88E6XXX_FLAG_G2_MGMT_EN_2X |	\
 	 MV88E6XXX_FLAG_G2_MGMT_EN_0X |	\
-	 MV88E6XXX_FLAG_G2_SWITCH_MAC |	\
 	 MV88E6XXX_FLAG_G2_POT |	\
-	 MV88E6XXX_FLAG_MULTI_CHIP |	\
 	 MV88E6XXX_FLAG_PPU_ACTIVE |	\
-	 MV88E6XXX_FLAG_SMI_PHY |	\
 	 MV88E6XXX_FLAG_STU |		\
 	 MV88E6XXX_FLAG_TEMP |		\
 	 MV88E6XXX_FLAG_VTU |		\
 	 MV88E6XXX_FLAGS_IRL |		\
+	 MV88E6XXX_FLAGS_MULTI_CHIP |	\
 	 MV88E6XXX_FLAGS_PVT)
 
 #define MV88E6XXX_FLAGS_FAMILY_6352	\
-	(MV88E6XXX_FLAG_EEE |		\
+	(MV88E6XXX_FLAG_EDSA |		\
+	 MV88E6XXX_FLAG_EEE |		\
+	 MV88E6XXX_FLAG_G1_ATU_FID |	\
+	 MV88E6XXX_FLAG_G1_VTU_FID |	\
 	 MV88E6XXX_FLAG_GLOBAL2 |	\
 	 MV88E6XXX_FLAG_G2_MGMT_EN_2X |	\
 	 MV88E6XXX_FLAG_G2_MGMT_EN_0X |	\
-	 MV88E6XXX_FLAG_G2_SWITCH_MAC |	\
 	 MV88E6XXX_FLAG_G2_POT |	\
-	 MV88E6XXX_FLAG_MULTI_CHIP |	\
 	 MV88E6XXX_FLAG_PPU_ACTIVE |	\
-	 MV88E6XXX_FLAG_SMI_PHY |	\
 	 MV88E6XXX_FLAG_STU |		\
 	 MV88E6XXX_FLAG_TEMP |		\
 	 MV88E6XXX_FLAG_TEMP_LIMIT |	\
 	 MV88E6XXX_FLAG_VTU |		\
-	 MV88E6XXX_FLAGS_EEPROM16 |	\
 	 MV88E6XXX_FLAGS_IRL |		\
-	 MV88E6XXX_FLAGS_PVT)
+	 MV88E6XXX_FLAGS_MULTI_CHIP |	\
+	 MV88E6XXX_FLAGS_PVT |		\
+	 MV88E6XXX_FLAGS_SERDES)
+
+struct mv88e6xxx_ops;
 
 struct mv88e6xxx_info {
 	enum mv88e6xxx_family family;
@@ -572,8 +600,10 @@ struct mv88e6xxx_info {
 	unsigned int num_databases;
 	unsigned int num_ports;
 	unsigned int port_base_addr;
+	unsigned int global1_addr;
 	unsigned int age_time_coeff;
-	unsigned long flags;
+	unsigned long long flags;
+	const struct mv88e6xxx_ops *ops;
 };
 
 struct mv88e6xxx_atu_entry {
@@ -584,18 +614,15 @@ struct mv88e6xxx_atu_entry {
 	u8	mac[ETH_ALEN];
 };
 
-struct mv88e6xxx_vtu_stu_entry {
-	/* VTU only */
+struct mv88e6xxx_vtu_entry {
 	u16	vid;
 	u16	fid;
-
-	/* VTU and STU */
 	u8	sid;
 	bool	valid;
 	u8	data[DSA_MAX_PORTS];
 };
 
-struct mv88e6xxx_ops;
+struct mv88e6xxx_bus_ops;
 
 struct mv88e6xxx_priv_port {
 	struct net_device *bridge_dev;
@@ -616,13 +643,14 @@ struct mv88e6xxx_chip {
 	/* The MII bus and the address on the bus that is used to
 	 * communication with the switch
 	 */
-	const struct mv88e6xxx_ops *smi_ops;
+	const struct mv88e6xxx_bus_ops *smi_ops;
 	struct mii_bus *bus;
 	int sw_addr;
 
 	/* Handles automatic disabling and re-enabling of the PHY
 	 * polling unit.
 	 */
+	const struct mv88e6xxx_bus_ops *phy_ops;
 	struct mutex		ppu_mutex;
 	int			ppu_disabled;
 	struct work_struct	ppu_work;
@@ -651,11 +679,25 @@ struct mv88e6xxx_chip {
 	struct mii_bus *mdio_bus;
 };
 
-struct mv88e6xxx_ops {
+struct mv88e6xxx_bus_ops {
 	int (*read)(struct mv88e6xxx_chip *chip, int addr, int reg, u16 *val);
 	int (*write)(struct mv88e6xxx_chip *chip, int addr, int reg, u16 val);
 };
 
+struct mv88e6xxx_ops {
+	int (*get_eeprom)(struct mv88e6xxx_chip *chip,
+			  struct ethtool_eeprom *eeprom, u8 *data);
+	int (*set_eeprom)(struct mv88e6xxx_chip *chip,
+			  struct ethtool_eeprom *eeprom, u8 *data);
+
+	int (*set_switch_mac)(struct mv88e6xxx_chip *chip, u8 *addr);
+
+	int (*phy_read)(struct mv88e6xxx_chip *chip, int addr, int reg,
+			u16 *val);
+	int (*phy_write)(struct mv88e6xxx_chip *chip, int addr, int reg,
+			 u16 val);
+};
+
 enum stat_type {
 	BANK0,
 	BANK1,
@@ -675,4 +717,20 @@ static inline bool mv88e6xxx_has(struct mv88e6xxx_chip *chip,
 	return (chip->info->flags & flags) == flags;
 }
 
+static inline unsigned int mv88e6xxx_num_databases(struct mv88e6xxx_chip *chip)
+{
+	return chip->info->num_databases;
+}
+
+static inline unsigned int mv88e6xxx_num_ports(struct mv88e6xxx_chip *chip)
+{
+	return chip->info->num_ports;
+}
+
+int mv88e6xxx_read(struct mv88e6xxx_chip *chip, int addr, int reg, u16 *val);
+int mv88e6xxx_write(struct mv88e6xxx_chip *chip, int addr, int reg, u16 val);
+int mv88e6xxx_update(struct mv88e6xxx_chip *chip, int addr, int reg,
+		     u16 update);
+int mv88e6xxx_wait(struct mv88e6xxx_chip *chip, int addr, int reg, u16 mask);
+
 #endif
diff --git a/drivers/net/dsa/qca8k.c b/drivers/net/dsa/qca8k.c
new file mode 100644
index 000000000000..b3df70d07ff6
--- /dev/null
+++ b/drivers/net/dsa/qca8k.c
@@ -0,0 +1,1040 @@
+/*
+ * Copyright (C) 2009 Felix Fietkau <nbd@nbd.name>
+ * Copyright (C) 2011-2012 Gabor Juhos <juhosg@openwrt.org>
+ * Copyright (c) 2015, The Linux Foundation. All rights reserved.
+ * Copyright (c) 2016 John Crispin <john@phrozen.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 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/phy.h>
+#include <linux/netdevice.h>
+#include <net/dsa.h>
+#include <net/switchdev.h>
+#include <linux/of_net.h>
+#include <linux/of_platform.h>
+#include <linux/if_bridge.h>
+#include <linux/mdio.h>
+#include <linux/etherdevice.h>
+
+#include "qca8k.h"
+
+#define MIB_DESC(_s, _o, _n)	\
+	{			\
+		.size = (_s),	\
+		.offset = (_o),	\
+		.name = (_n),	\
+	}
+
+static const struct qca8k_mib_desc ar8327_mib[] = {
+	MIB_DESC(1, 0x00, "RxBroad"),
+	MIB_DESC(1, 0x04, "RxPause"),
+	MIB_DESC(1, 0x08, "RxMulti"),
+	MIB_DESC(1, 0x0c, "RxFcsErr"),
+	MIB_DESC(1, 0x10, "RxAlignErr"),
+	MIB_DESC(1, 0x14, "RxRunt"),
+	MIB_DESC(1, 0x18, "RxFragment"),
+	MIB_DESC(1, 0x1c, "Rx64Byte"),
+	MIB_DESC(1, 0x20, "Rx128Byte"),
+	MIB_DESC(1, 0x24, "Rx256Byte"),
+	MIB_DESC(1, 0x28, "Rx512Byte"),
+	MIB_DESC(1, 0x2c, "Rx1024Byte"),
+	MIB_DESC(1, 0x30, "Rx1518Byte"),
+	MIB_DESC(1, 0x34, "RxMaxByte"),
+	MIB_DESC(1, 0x38, "RxTooLong"),
+	MIB_DESC(2, 0x3c, "RxGoodByte"),
+	MIB_DESC(2, 0x44, "RxBadByte"),
+	MIB_DESC(1, 0x4c, "RxOverFlow"),
+	MIB_DESC(1, 0x50, "Filtered"),
+	MIB_DESC(1, 0x54, "TxBroad"),
+	MIB_DESC(1, 0x58, "TxPause"),
+	MIB_DESC(1, 0x5c, "TxMulti"),
+	MIB_DESC(1, 0x60, "TxUnderRun"),
+	MIB_DESC(1, 0x64, "Tx64Byte"),
+	MIB_DESC(1, 0x68, "Tx128Byte"),
+	MIB_DESC(1, 0x6c, "Tx256Byte"),
+	MIB_DESC(1, 0x70, "Tx512Byte"),
+	MIB_DESC(1, 0x74, "Tx1024Byte"),
+	MIB_DESC(1, 0x78, "Tx1518Byte"),
+	MIB_DESC(1, 0x7c, "TxMaxByte"),
+	MIB_DESC(1, 0x80, "TxOverSize"),
+	MIB_DESC(2, 0x84, "TxByte"),
+	MIB_DESC(1, 0x8c, "TxCollision"),
+	MIB_DESC(1, 0x90, "TxAbortCol"),
+	MIB_DESC(1, 0x94, "TxMultiCol"),
+	MIB_DESC(1, 0x98, "TxSingleCol"),
+	MIB_DESC(1, 0x9c, "TxExcDefer"),
+	MIB_DESC(1, 0xa0, "TxDefer"),
+	MIB_DESC(1, 0xa4, "TxLateCol"),
+};
+
+/* The 32bit switch registers are accessed indirectly. To achieve this we need
+ * to set the page of the register. Track the last page that was set to reduce
+ * mdio writes
+ */
+static u16 qca8k_current_page = 0xffff;
+
+static void
+qca8k_split_addr(u32 regaddr, u16 *r1, u16 *r2, u16 *page)
+{
+	regaddr >>= 1;
+	*r1 = regaddr & 0x1e;
+
+	regaddr >>= 5;
+	*r2 = regaddr & 0x7;
+
+	regaddr >>= 3;
+	*page = regaddr & 0x3ff;
+}
+
+static u32
+qca8k_mii_read32(struct mii_bus *bus, int phy_id, u32 regnum)
+{
+	u32 val;
+	int ret;
+
+	ret = bus->read(bus, phy_id, regnum);
+	if (ret >= 0) {
+		val = ret;
+		ret = bus->read(bus, phy_id, regnum + 1);
+		val |= ret << 16;
+	}
+
+	if (ret < 0) {
+		dev_err_ratelimited(&bus->dev,
+				    "failed to read qca8k 32bit register\n");
+		return ret;
+	}
+
+	return val;
+}
+
+static void
+qca8k_mii_write32(struct mii_bus *bus, int phy_id, u32 regnum, u32 val)
+{
+	u16 lo, hi;
+	int ret;
+
+	lo = val & 0xffff;
+	hi = (u16)(val >> 16);
+
+	ret = bus->write(bus, phy_id, regnum, lo);
+	if (ret >= 0)
+		ret = bus->write(bus, phy_id, regnum + 1, hi);
+	if (ret < 0)
+		dev_err_ratelimited(&bus->dev,
+				    "failed to write qca8k 32bit register\n");
+}
+
+static void
+qca8k_set_page(struct mii_bus *bus, u16 page)
+{
+	if (page == qca8k_current_page)
+		return;
+
+	if (bus->write(bus, 0x18, 0, page) < 0)
+		dev_err_ratelimited(&bus->dev,
+				    "failed to set qca8k page\n");
+	qca8k_current_page = page;
+}
+
+static u32
+qca8k_read(struct qca8k_priv *priv, u32 reg)
+{
+	u16 r1, r2, page;
+	u32 val;
+
+	qca8k_split_addr(reg, &r1, &r2, &page);
+
+	mutex_lock_nested(&priv->bus->mdio_lock, MDIO_MUTEX_NESTED);
+
+	qca8k_set_page(priv->bus, page);
+	val = qca8k_mii_read32(priv->bus, 0x10 | r2, r1);
+
+	mutex_unlock(&priv->bus->mdio_lock);
+
+	return val;
+}
+
+static void
+qca8k_write(struct qca8k_priv *priv, u32 reg, u32 val)
+{
+	u16 r1, r2, page;
+
+	qca8k_split_addr(reg, &r1, &r2, &page);
+
+	mutex_lock_nested(&priv->bus->mdio_lock, MDIO_MUTEX_NESTED);
+
+	qca8k_set_page(priv->bus, page);
+	qca8k_mii_write32(priv->bus, 0x10 | r2, r1, val);
+
+	mutex_unlock(&priv->bus->mdio_lock);
+}
+
+static u32
+qca8k_rmw(struct qca8k_priv *priv, u32 reg, u32 mask, u32 val)
+{
+	u16 r1, r2, page;
+	u32 ret;
+
+	qca8k_split_addr(reg, &r1, &r2, &page);
+
+	mutex_lock_nested(&priv->bus->mdio_lock, MDIO_MUTEX_NESTED);
+
+	qca8k_set_page(priv->bus, page);
+	ret = qca8k_mii_read32(priv->bus, 0x10 | r2, r1);
+	ret &= ~mask;
+	ret |= val;
+	qca8k_mii_write32(priv->bus, 0x10 | r2, r1, ret);
+
+	mutex_unlock(&priv->bus->mdio_lock);
+
+	return ret;
+}
+
+static void
+qca8k_reg_set(struct qca8k_priv *priv, u32 reg, u32 val)
+{
+	qca8k_rmw(priv, reg, 0, val);
+}
+
+static void
+qca8k_reg_clear(struct qca8k_priv *priv, u32 reg, u32 val)
+{
+	qca8k_rmw(priv, reg, val, 0);
+}
+
+static int
+qca8k_regmap_read(void *ctx, uint32_t reg, uint32_t *val)
+{
+	struct qca8k_priv *priv = (struct qca8k_priv *)ctx;
+
+	*val = qca8k_read(priv, reg);
+
+	return 0;
+}
+
+static int
+qca8k_regmap_write(void *ctx, uint32_t reg, uint32_t val)
+{
+	struct qca8k_priv *priv = (struct qca8k_priv *)ctx;
+
+	qca8k_write(priv, reg, val);
+
+	return 0;
+}
+
+static const struct regmap_range qca8k_readable_ranges[] = {
+	regmap_reg_range(0x0000, 0x00e4), /* Global control */
+	regmap_reg_range(0x0100, 0x0168), /* EEE control */
+	regmap_reg_range(0x0200, 0x0270), /* Parser control */
+	regmap_reg_range(0x0400, 0x0454), /* ACL */
+	regmap_reg_range(0x0600, 0x0718), /* Lookup */
+	regmap_reg_range(0x0800, 0x0b70), /* QM */
+	regmap_reg_range(0x0c00, 0x0c80), /* PKT */
+	regmap_reg_range(0x0e00, 0x0e98), /* L3 */
+	regmap_reg_range(0x1000, 0x10ac), /* MIB - Port0 */
+	regmap_reg_range(0x1100, 0x11ac), /* MIB - Port1 */
+	regmap_reg_range(0x1200, 0x12ac), /* MIB - Port2 */
+	regmap_reg_range(0x1300, 0x13ac), /* MIB - Port3 */
+	regmap_reg_range(0x1400, 0x14ac), /* MIB - Port4 */
+	regmap_reg_range(0x1500, 0x15ac), /* MIB - Port5 */
+	regmap_reg_range(0x1600, 0x16ac), /* MIB - Port6 */
+
+};
+
+static struct regmap_access_table qca8k_readable_table = {
+	.yes_ranges = qca8k_readable_ranges,
+	.n_yes_ranges = ARRAY_SIZE(qca8k_readable_ranges),
+};
+
+static struct regmap_config qca8k_regmap_config = {
+	.reg_bits = 16,
+	.val_bits = 32,
+	.reg_stride = 4,
+	.max_register = 0x16ac, /* end MIB - Port6 range */
+	.reg_read = qca8k_regmap_read,
+	.reg_write = qca8k_regmap_write,
+	.rd_table = &qca8k_readable_table,
+};
+
+static int
+qca8k_busy_wait(struct qca8k_priv *priv, u32 reg, u32 mask)
+{
+	unsigned long timeout;
+
+	timeout = jiffies + msecs_to_jiffies(20);
+
+	/* loop until the busy flag has cleared */
+	do {
+		u32 val = qca8k_read(priv, reg);
+		int busy = val & mask;
+
+		if (!busy)
+			break;
+		cond_resched();
+	} while (!time_after_eq(jiffies, timeout));
+
+	return time_after_eq(jiffies, timeout);
+}
+
+static void
+qca8k_fdb_read(struct qca8k_priv *priv, struct qca8k_fdb *fdb)
+{
+	u32 reg[4];
+	int i;
+
+	/* load the ARL table into an array */
+	for (i = 0; i < 4; i++)
+		reg[i] = qca8k_read(priv, QCA8K_REG_ATU_DATA0 + (i * 4));
+
+	/* vid - 83:72 */
+	fdb->vid = (reg[2] >> QCA8K_ATU_VID_S) & QCA8K_ATU_VID_M;
+	/* aging - 67:64 */
+	fdb->aging = reg[2] & QCA8K_ATU_STATUS_M;
+	/* portmask - 54:48 */
+	fdb->port_mask = (reg[1] >> QCA8K_ATU_PORT_S) & QCA8K_ATU_PORT_M;
+	/* mac - 47:0 */
+	fdb->mac[0] = (reg[1] >> QCA8K_ATU_ADDR0_S) & 0xff;
+	fdb->mac[1] = reg[1] & 0xff;
+	fdb->mac[2] = (reg[0] >> QCA8K_ATU_ADDR2_S) & 0xff;
+	fdb->mac[3] = (reg[0] >> QCA8K_ATU_ADDR3_S) & 0xff;
+	fdb->mac[4] = (reg[0] >> QCA8K_ATU_ADDR4_S) & 0xff;
+	fdb->mac[5] = reg[0] & 0xff;
+}
+
+static void
+qca8k_fdb_write(struct qca8k_priv *priv, u16 vid, u8 port_mask, const u8 *mac,
+		u8 aging)
+{
+	u32 reg[3] = { 0 };
+	int i;
+
+	/* vid - 83:72 */
+	reg[2] = (vid & QCA8K_ATU_VID_M) << QCA8K_ATU_VID_S;
+	/* aging - 67:64 */
+	reg[2] |= aging & QCA8K_ATU_STATUS_M;
+	/* portmask - 54:48 */
+	reg[1] = (port_mask & QCA8K_ATU_PORT_M) << QCA8K_ATU_PORT_S;
+	/* mac - 47:0 */
+	reg[1] |= mac[0] << QCA8K_ATU_ADDR0_S;
+	reg[1] |= mac[1];
+	reg[0] |= mac[2] << QCA8K_ATU_ADDR2_S;
+	reg[0] |= mac[3] << QCA8K_ATU_ADDR3_S;
+	reg[0] |= mac[4] << QCA8K_ATU_ADDR4_S;
+	reg[0] |= mac[5];
+
+	/* load the array into the ARL table */
+	for (i = 0; i < 3; i++)
+		qca8k_write(priv, QCA8K_REG_ATU_DATA0 + (i * 4), reg[i]);
+}
+
+static int
+qca8k_fdb_access(struct qca8k_priv *priv, enum qca8k_fdb_cmd cmd, int port)
+{
+	u32 reg;
+
+	/* Set the command and FDB index */
+	reg = QCA8K_ATU_FUNC_BUSY;
+	reg |= cmd;
+	if (port >= 0) {
+		reg |= QCA8K_ATU_FUNC_PORT_EN;
+		reg |= (port & QCA8K_ATU_FUNC_PORT_M) << QCA8K_ATU_FUNC_PORT_S;
+	}
+
+	/* Write the function register triggering the table access */
+	qca8k_write(priv, QCA8K_REG_ATU_FUNC, reg);
+
+	/* wait for completion */
+	if (qca8k_busy_wait(priv, QCA8K_REG_ATU_FUNC, QCA8K_ATU_FUNC_BUSY))
+		return -1;
+
+	/* Check for table full violation when adding an entry */
+	if (cmd == QCA8K_FDB_LOAD) {
+		reg = qca8k_read(priv, QCA8K_REG_ATU_FUNC);
+		if (reg & QCA8K_ATU_FUNC_FULL)
+			return -1;
+	}
+
+	return 0;
+}
+
+static int
+qca8k_fdb_next(struct qca8k_priv *priv, struct qca8k_fdb *fdb, int port)
+{
+	int ret;
+
+	qca8k_fdb_write(priv, fdb->vid, fdb->port_mask, fdb->mac, fdb->aging);
+	ret = qca8k_fdb_access(priv, QCA8K_FDB_NEXT, port);
+	if (ret >= 0)
+		qca8k_fdb_read(priv, fdb);
+
+	return ret;
+}
+
+static int
+qca8k_fdb_add(struct qca8k_priv *priv, const u8 *mac, u16 port_mask,
+	      u16 vid, u8 aging)
+{
+	int ret;
+
+	mutex_lock(&priv->reg_mutex);
+	qca8k_fdb_write(priv, vid, port_mask, mac, aging);
+	ret = qca8k_fdb_access(priv, QCA8K_FDB_LOAD, -1);
+	mutex_unlock(&priv->reg_mutex);
+
+	return ret;
+}
+
+static int
+qca8k_fdb_del(struct qca8k_priv *priv, const u8 *mac, u16 port_mask, u16 vid)
+{
+	int ret;
+
+	mutex_lock(&priv->reg_mutex);
+	qca8k_fdb_write(priv, vid, port_mask, mac, 0);
+	ret = qca8k_fdb_access(priv, QCA8K_FDB_PURGE, -1);
+	mutex_unlock(&priv->reg_mutex);
+
+	return ret;
+}
+
+static void
+qca8k_fdb_flush(struct qca8k_priv *priv)
+{
+	mutex_lock(&priv->reg_mutex);
+	qca8k_fdb_access(priv, QCA8K_FDB_FLUSH, -1);
+	mutex_unlock(&priv->reg_mutex);
+}
+
+static void
+qca8k_mib_init(struct qca8k_priv *priv)
+{
+	mutex_lock(&priv->reg_mutex);
+	qca8k_reg_set(priv, QCA8K_REG_MIB, QCA8K_MIB_FLUSH | QCA8K_MIB_BUSY);
+	qca8k_busy_wait(priv, QCA8K_REG_MIB, QCA8K_MIB_BUSY);
+	qca8k_reg_set(priv, QCA8K_REG_MIB, QCA8K_MIB_CPU_KEEP);
+	qca8k_write(priv, QCA8K_REG_MODULE_EN, QCA8K_MODULE_EN_MIB);
+	mutex_unlock(&priv->reg_mutex);
+}
+
+static int
+qca8k_set_pad_ctrl(struct qca8k_priv *priv, int port, int mode)
+{
+	u32 reg;
+
+	switch (port) {
+	case 0:
+		reg = QCA8K_REG_PORT0_PAD_CTRL;
+		break;
+	case 6:
+		reg = QCA8K_REG_PORT6_PAD_CTRL;
+		break;
+	default:
+		pr_err("Can't set PAD_CTRL on port %d\n", port);
+		return -EINVAL;
+	}
+
+	/* Configure a port to be directly connected to an external
+	 * PHY or MAC.
+	 */
+	switch (mode) {
+	case PHY_INTERFACE_MODE_RGMII:
+		qca8k_write(priv, reg,
+			    QCA8K_PORT_PAD_RGMII_EN |
+			    QCA8K_PORT_PAD_RGMII_TX_DELAY(3) |
+			    QCA8K_PORT_PAD_RGMII_RX_DELAY(3));
+
+		/* According to the datasheet, RGMII delay is enabled through
+		 * PORT5_PAD_CTRL for all ports, rather than individual port
+		 * registers
+		 */
+		qca8k_write(priv, QCA8K_REG_PORT5_PAD_CTRL,
+			    QCA8K_PORT_PAD_RGMII_RX_DELAY_EN);
+		break;
+	case PHY_INTERFACE_MODE_SGMII:
+		qca8k_write(priv, reg, QCA8K_PORT_PAD_SGMII_EN);
+		break;
+	default:
+		pr_err("xMII mode %d not supported\n", mode);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static void
+qca8k_port_set_status(struct qca8k_priv *priv, int port, int enable)
+{
+	u32 mask = QCA8K_PORT_STATUS_TXMAC;
+
+	/* Port 0 and 6 have no internal PHY */
+	if ((port > 0) && (port < 6))
+		mask |= QCA8K_PORT_STATUS_LINK_AUTO;
+
+	if (enable)
+		qca8k_reg_set(priv, QCA8K_REG_PORT_STATUS(port), mask);
+	else
+		qca8k_reg_clear(priv, QCA8K_REG_PORT_STATUS(port), mask);
+}
+
+static int
+qca8k_setup(struct dsa_switch *ds)
+{
+	struct qca8k_priv *priv = (struct qca8k_priv *)ds->priv;
+	int ret, i, phy_mode = -1;
+
+	/* Make sure that port 0 is the cpu port */
+	if (!dsa_is_cpu_port(ds, 0)) {
+		pr_err("port 0 is not the CPU port\n");
+		return -EINVAL;
+	}
+
+	mutex_init(&priv->reg_mutex);
+
+	/* Start by setting up the register mapping */
+	priv->regmap = devm_regmap_init(ds->dev, NULL, priv,
+					&qca8k_regmap_config);
+	if (IS_ERR(priv->regmap))
+		pr_warn("regmap initialization failed");
+
+	/* Initialize CPU port pad mode (xMII type, delays...) */
+	phy_mode = of_get_phy_mode(ds->ports[ds->dst->cpu_port].dn);
+	if (phy_mode < 0) {
+		pr_err("Can't find phy-mode for master device\n");
+		return phy_mode;
+	}
+	ret = qca8k_set_pad_ctrl(priv, QCA8K_CPU_PORT, phy_mode);
+	if (ret < 0)
+		return ret;
+
+	/* Enable CPU Port */
+	qca8k_reg_set(priv, QCA8K_REG_GLOBAL_FW_CTRL0,
+		      QCA8K_GLOBAL_FW_CTRL0_CPU_PORT_EN);
+	qca8k_port_set_status(priv, QCA8K_CPU_PORT, 1);
+	priv->port_sts[QCA8K_CPU_PORT].enabled = 1;
+
+	/* Enable MIB counters */
+	qca8k_mib_init(priv);
+
+	/* Enable QCA header mode on the cpu port */
+	qca8k_write(priv, QCA8K_REG_PORT_HDR_CTRL(QCA8K_CPU_PORT),
+		    QCA8K_PORT_HDR_CTRL_ALL << QCA8K_PORT_HDR_CTRL_TX_S |
+		    QCA8K_PORT_HDR_CTRL_ALL << QCA8K_PORT_HDR_CTRL_RX_S);
+
+	/* Disable forwarding by default on all ports */
+	for (i = 0; i < QCA8K_NUM_PORTS; i++)
+		qca8k_rmw(priv, QCA8K_PORT_LOOKUP_CTRL(i),
+			  QCA8K_PORT_LOOKUP_MEMBER, 0);
+
+	/* Disable MAC by default on all user ports */
+	for (i = 1; i < QCA8K_NUM_PORTS; i++)
+		if (ds->enabled_port_mask & BIT(i))
+			qca8k_port_set_status(priv, i, 0);
+
+	/* Forward all unknown frames to CPU port for Linux processing */
+	qca8k_write(priv, QCA8K_REG_GLOBAL_FW_CTRL1,
+		    BIT(0) << QCA8K_GLOBAL_FW_CTRL1_IGMP_DP_S |
+		    BIT(0) << QCA8K_GLOBAL_FW_CTRL1_BC_DP_S |
+		    BIT(0) << QCA8K_GLOBAL_FW_CTRL1_MC_DP_S |
+		    BIT(0) << QCA8K_GLOBAL_FW_CTRL1_UC_DP_S);
+
+	/* Setup connection between CPU port & user ports */
+	for (i = 0; i < DSA_MAX_PORTS; i++) {
+		/* CPU port gets connected to all user ports of the switch */
+		if (dsa_is_cpu_port(ds, i)) {
+			qca8k_rmw(priv, QCA8K_PORT_LOOKUP_CTRL(QCA8K_CPU_PORT),
+				  QCA8K_PORT_LOOKUP_MEMBER,
+				  ds->enabled_port_mask);
+		}
+
+		/* Invividual user ports get connected to CPU port only */
+		if (ds->enabled_port_mask & BIT(i)) {
+			int shift = 16 * (i % 2);
+
+			qca8k_rmw(priv, QCA8K_PORT_LOOKUP_CTRL(i),
+				  QCA8K_PORT_LOOKUP_MEMBER,
+				  BIT(QCA8K_CPU_PORT));
+
+			/* Enable ARP Auto-learning by default */
+			qca8k_reg_set(priv, QCA8K_PORT_LOOKUP_CTRL(i),
+				      QCA8K_PORT_LOOKUP_LEARN);
+
+			/* For port based vlans to work we need to set the
+			 * default egress vid
+			 */
+			qca8k_rmw(priv, QCA8K_EGRESS_VLAN(i),
+				  0xffff << shift, 1 << shift);
+			qca8k_write(priv, QCA8K_REG_PORT_VLAN_CTRL0(i),
+				    QCA8K_PORT_VLAN_CVID(1) |
+				    QCA8K_PORT_VLAN_SVID(1));
+		}
+	}
+
+	/* Flush the FDB table */
+	qca8k_fdb_flush(priv);
+
+	return 0;
+}
+
+static int
+qca8k_phy_read(struct dsa_switch *ds, int phy, int regnum)
+{
+	struct qca8k_priv *priv = (struct qca8k_priv *)ds->priv;
+
+	return mdiobus_read(priv->bus, phy, regnum);
+}
+
+static int
+qca8k_phy_write(struct dsa_switch *ds, int phy, int regnum, u16 val)
+{
+	struct qca8k_priv *priv = (struct qca8k_priv *)ds->priv;
+
+	return mdiobus_write(priv->bus, phy, regnum, val);
+}
+
+static void
+qca8k_get_strings(struct dsa_switch *ds, int port, uint8_t *data)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(ar8327_mib); i++)
+		strncpy(data + i * ETH_GSTRING_LEN, ar8327_mib[i].name,
+			ETH_GSTRING_LEN);
+}
+
+static void
+qca8k_get_ethtool_stats(struct dsa_switch *ds, int port,
+			uint64_t *data)
+{
+	struct qca8k_priv *priv = (struct qca8k_priv *)ds->priv;
+	const struct qca8k_mib_desc *mib;
+	u32 reg, i;
+	u64 hi;
+
+	for (i = 0; i < ARRAY_SIZE(ar8327_mib); i++) {
+		mib = &ar8327_mib[i];
+		reg = QCA8K_PORT_MIB_COUNTER(port) + mib->offset;
+
+		data[i] = qca8k_read(priv, reg);
+		if (mib->size == 2) {
+			hi = qca8k_read(priv, reg + 4);
+			data[i] |= hi << 32;
+		}
+	}
+}
+
+static int
+qca8k_get_sset_count(struct dsa_switch *ds)
+{
+	return ARRAY_SIZE(ar8327_mib);
+}
+
+static void
+qca8k_eee_enable_set(struct dsa_switch *ds, int port, bool enable)
+{
+	struct qca8k_priv *priv = (struct qca8k_priv *)ds->priv;
+	u32 lpi_en = QCA8K_REG_EEE_CTRL_LPI_EN(port);
+	u32 reg;
+
+	mutex_lock(&priv->reg_mutex);
+	reg = qca8k_read(priv, QCA8K_REG_EEE_CTRL);
+	if (enable)
+		reg |= lpi_en;
+	else
+		reg &= ~lpi_en;
+	qca8k_write(priv, QCA8K_REG_EEE_CTRL, reg);
+	mutex_unlock(&priv->reg_mutex);
+}
+
+static int
+qca8k_eee_init(struct dsa_switch *ds, int port,
+	       struct phy_device *phy)
+{
+	struct qca8k_priv *priv = (struct qca8k_priv *)ds->priv;
+	struct ethtool_eee *p = &priv->port_sts[port].eee;
+	int ret;
+
+	p->supported = (SUPPORTED_1000baseT_Full | SUPPORTED_100baseT_Full);
+
+	ret = phy_init_eee(phy, 0);
+	if (ret)
+		return ret;
+
+	qca8k_eee_enable_set(ds, port, true);
+
+	return 0;
+}
+
+static int
+qca8k_set_eee(struct dsa_switch *ds, int port,
+	      struct phy_device *phydev,
+	      struct ethtool_eee *e)
+{
+	struct qca8k_priv *priv = (struct qca8k_priv *)ds->priv;
+	struct ethtool_eee *p = &priv->port_sts[port].eee;
+	int ret = 0;
+
+	p->eee_enabled = e->eee_enabled;
+
+	if (e->eee_enabled) {
+		p->eee_enabled = qca8k_eee_init(ds, port, phydev);
+		if (!p->eee_enabled)
+			ret = -EOPNOTSUPP;
+	}
+	qca8k_eee_enable_set(ds, port, p->eee_enabled);
+
+	return ret;
+}
+
+static int
+qca8k_get_eee(struct dsa_switch *ds, int port,
+	      struct ethtool_eee *e)
+{
+	struct qca8k_priv *priv = (struct qca8k_priv *)ds->priv;
+	struct ethtool_eee *p = &priv->port_sts[port].eee;
+	struct net_device *netdev = ds->ports[port].netdev;
+	int ret;
+
+	ret = phy_ethtool_get_eee(netdev->phydev, p);
+	if (!ret)
+		e->eee_active =
+			!!(p->supported & p->advertised & p->lp_advertised);
+	else
+		e->eee_active = 0;
+
+	e->eee_enabled = p->eee_enabled;
+
+	return ret;
+}
+
+static void
+qca8k_port_stp_state_set(struct dsa_switch *ds, int port, u8 state)
+{
+	struct qca8k_priv *priv = (struct qca8k_priv *)ds->priv;
+	u32 stp_state;
+
+	switch (state) {
+	case BR_STATE_DISABLED:
+		stp_state = QCA8K_PORT_LOOKUP_STATE_DISABLED;
+		break;
+	case BR_STATE_BLOCKING:
+		stp_state = QCA8K_PORT_LOOKUP_STATE_BLOCKING;
+		break;
+	case BR_STATE_LISTENING:
+		stp_state = QCA8K_PORT_LOOKUP_STATE_LISTENING;
+		break;
+	case BR_STATE_LEARNING:
+		stp_state = QCA8K_PORT_LOOKUP_STATE_LEARNING;
+		break;
+	case BR_STATE_FORWARDING:
+	default:
+		stp_state = QCA8K_PORT_LOOKUP_STATE_FORWARD;
+		break;
+	}
+
+	qca8k_rmw(priv, QCA8K_PORT_LOOKUP_CTRL(port),
+		  QCA8K_PORT_LOOKUP_STATE_MASK, stp_state);
+}
+
+static int
+qca8k_port_bridge_join(struct dsa_switch *ds, int port,
+		       struct net_device *bridge)
+{
+	struct qca8k_priv *priv = (struct qca8k_priv *)ds->priv;
+	int port_mask = BIT(QCA8K_CPU_PORT);
+	int i;
+
+	priv->port_sts[port].bridge_dev = bridge;
+
+	for (i = 1; i < QCA8K_NUM_PORTS; i++) {
+		if (priv->port_sts[i].bridge_dev != bridge)
+			continue;
+		/* Add this port to the portvlan mask of the other ports
+		 * in the bridge
+		 */
+		qca8k_reg_set(priv,
+			      QCA8K_PORT_LOOKUP_CTRL(i),
+			      BIT(port));
+		if (i != port)
+			port_mask |= BIT(i);
+	}
+	/* Add all other ports to this ports portvlan mask */
+	qca8k_rmw(priv, QCA8K_PORT_LOOKUP_CTRL(port),
+		  QCA8K_PORT_LOOKUP_MEMBER, port_mask);
+
+	return 0;
+}
+
+static void
+qca8k_port_bridge_leave(struct dsa_switch *ds, int port)
+{
+	struct qca8k_priv *priv = (struct qca8k_priv *)ds->priv;
+	int i;
+
+	for (i = 1; i < QCA8K_NUM_PORTS; i++) {
+		if (priv->port_sts[i].bridge_dev !=
+		    priv->port_sts[port].bridge_dev)
+			continue;
+		/* Remove this port to the portvlan mask of the other ports
+		 * in the bridge
+		 */
+		qca8k_reg_clear(priv,
+				QCA8K_PORT_LOOKUP_CTRL(i),
+				BIT(port));
+	}
+	priv->port_sts[port].bridge_dev = NULL;
+	/* Set the cpu port to be the only one in the portvlan mask of
+	 * this port
+	 */
+	qca8k_rmw(priv, QCA8K_PORT_LOOKUP_CTRL(port),
+		  QCA8K_PORT_LOOKUP_MEMBER, BIT(QCA8K_CPU_PORT));
+}
+
+static int
+qca8k_port_enable(struct dsa_switch *ds, int port,
+		  struct phy_device *phy)
+{
+	struct qca8k_priv *priv = (struct qca8k_priv *)ds->priv;
+
+	qca8k_port_set_status(priv, port, 1);
+	priv->port_sts[port].enabled = 1;
+
+	return 0;
+}
+
+static void
+qca8k_port_disable(struct dsa_switch *ds, int port,
+		   struct phy_device *phy)
+{
+	struct qca8k_priv *priv = (struct qca8k_priv *)ds->priv;
+
+	qca8k_port_set_status(priv, port, 0);
+	priv->port_sts[port].enabled = 0;
+}
+
+static int
+qca8k_port_fdb_insert(struct qca8k_priv *priv, const u8 *addr,
+		      u16 port_mask, u16 vid)
+{
+	/* Set the vid to the port vlan id if no vid is set */
+	if (!vid)
+		vid = 1;
+
+	return qca8k_fdb_add(priv, addr, port_mask, vid,
+			     QCA8K_ATU_STATUS_STATIC);
+}
+
+static int
+qca8k_port_fdb_prepare(struct dsa_switch *ds, int port,
+		       const struct switchdev_obj_port_fdb *fdb,
+		       struct switchdev_trans *trans)
+{
+	struct qca8k_priv *priv = (struct qca8k_priv *)ds->priv;
+
+	/* The FDB table for static and auto learned entries is the same. We
+	 * need to reserve an entry with no port_mask set to make sure that
+	 * when port_fdb_add is called an entry is still available. Otherwise
+	 * the last free entry might have been used up by auto learning
+	 */
+	return qca8k_port_fdb_insert(priv, fdb->addr, 0, fdb->vid);
+}
+
+static void
+qca8k_port_fdb_add(struct dsa_switch *ds, int port,
+		   const struct switchdev_obj_port_fdb *fdb,
+		   struct switchdev_trans *trans)
+{
+	struct qca8k_priv *priv = (struct qca8k_priv *)ds->priv;
+	u16 port_mask = BIT(port);
+
+	/* Update the FDB entry adding the port_mask */
+	qca8k_port_fdb_insert(priv, fdb->addr, port_mask, fdb->vid);
+}
+
+static int
+qca8k_port_fdb_del(struct dsa_switch *ds, int port,
+		   const struct switchdev_obj_port_fdb *fdb)
+{
+	struct qca8k_priv *priv = (struct qca8k_priv *)ds->priv;
+	u16 port_mask = BIT(port);
+	u16 vid = fdb->vid;
+
+	if (!vid)
+		vid = 1;
+
+	return qca8k_fdb_del(priv, fdb->addr, port_mask, vid);
+}
+
+static int
+qca8k_port_fdb_dump(struct dsa_switch *ds, int port,
+		    struct switchdev_obj_port_fdb *fdb,
+		    int (*cb)(struct switchdev_obj *obj))
+{
+	struct qca8k_priv *priv = (struct qca8k_priv *)ds->priv;
+	struct qca8k_fdb _fdb = { 0 };
+	int cnt = QCA8K_NUM_FDB_RECORDS;
+	int ret = 0;
+
+	mutex_lock(&priv->reg_mutex);
+	while (cnt-- && !qca8k_fdb_next(priv, &_fdb, port)) {
+		if (!_fdb.aging)
+			break;
+
+		ether_addr_copy(fdb->addr, _fdb.mac);
+		fdb->vid = _fdb.vid;
+		if (_fdb.aging == QCA8K_ATU_STATUS_STATIC)
+			fdb->ndm_state = NUD_NOARP;
+		else
+			fdb->ndm_state = NUD_REACHABLE;
+
+		ret = cb(&fdb->obj);
+		if (ret)
+			break;
+	}
+	mutex_unlock(&priv->reg_mutex);
+
+	return 0;
+}
+
+static enum dsa_tag_protocol
+qca8k_get_tag_protocol(struct dsa_switch *ds)
+{
+	return DSA_TAG_PROTO_QCA;
+}
+
+static struct dsa_switch_ops qca8k_switch_ops = {
+	.get_tag_protocol	= qca8k_get_tag_protocol,
+	.setup			= qca8k_setup,
+	.get_strings		= qca8k_get_strings,
+	.phy_read		= qca8k_phy_read,
+	.phy_write		= qca8k_phy_write,
+	.get_ethtool_stats	= qca8k_get_ethtool_stats,
+	.get_sset_count		= qca8k_get_sset_count,
+	.get_eee		= qca8k_get_eee,
+	.set_eee		= qca8k_set_eee,
+	.port_enable		= qca8k_port_enable,
+	.port_disable		= qca8k_port_disable,
+	.port_stp_state_set	= qca8k_port_stp_state_set,
+	.port_bridge_join	= qca8k_port_bridge_join,
+	.port_bridge_leave	= qca8k_port_bridge_leave,
+	.port_fdb_prepare	= qca8k_port_fdb_prepare,
+	.port_fdb_add		= qca8k_port_fdb_add,
+	.port_fdb_del		= qca8k_port_fdb_del,
+	.port_fdb_dump		= qca8k_port_fdb_dump,
+};
+
+static int
+qca8k_sw_probe(struct mdio_device *mdiodev)
+{
+	struct qca8k_priv *priv;
+	u32 id;
+
+	/* allocate the private data struct so that we can probe the switches
+	 * ID register
+	 */
+	priv = devm_kzalloc(&mdiodev->dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->bus = mdiodev->bus;
+
+	/* read the switches ID register */
+	id = qca8k_read(priv, QCA8K_REG_MASK_CTRL);
+	id >>= QCA8K_MASK_CTRL_ID_S;
+	id &= QCA8K_MASK_CTRL_ID_M;
+	if (id != QCA8K_ID_QCA8337)
+		return -ENODEV;
+
+	priv->ds = devm_kzalloc(&mdiodev->dev, sizeof(*priv->ds), GFP_KERNEL);
+	if (!priv->ds)
+		return -ENOMEM;
+
+	priv->ds->priv = priv;
+	priv->ds->dev = &mdiodev->dev;
+	priv->ds->ops = &qca8k_switch_ops;
+	mutex_init(&priv->reg_mutex);
+	dev_set_drvdata(&mdiodev->dev, priv);
+
+	return dsa_register_switch(priv->ds, priv->ds->dev->of_node);
+}
+
+static void
+qca8k_sw_remove(struct mdio_device *mdiodev)
+{
+	struct qca8k_priv *priv = dev_get_drvdata(&mdiodev->dev);
+	int i;
+
+	for (i = 0; i < QCA8K_NUM_PORTS; i++)
+		qca8k_port_set_status(priv, i, 0);
+
+	dsa_unregister_switch(priv->ds);
+}
+
+#ifdef CONFIG_PM_SLEEP
+static void
+qca8k_set_pm(struct qca8k_priv *priv, int enable)
+{
+	int i;
+
+	for (i = 0; i < QCA8K_NUM_PORTS; i++) {
+		if (!priv->port_sts[i].enabled)
+			continue;
+
+		qca8k_port_set_status(priv, i, enable);
+	}
+}
+
+static int qca8k_suspend(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct qca8k_priv *priv = platform_get_drvdata(pdev);
+
+	qca8k_set_pm(priv, 0);
+
+	return dsa_switch_suspend(priv->ds);
+}
+
+static int qca8k_resume(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct qca8k_priv *priv = platform_get_drvdata(pdev);
+
+	qca8k_set_pm(priv, 1);
+
+	return dsa_switch_resume(priv->ds);
+}
+#endif /* CONFIG_PM_SLEEP */
+
+static SIMPLE_DEV_PM_OPS(qca8k_pm_ops,
+			 qca8k_suspend, qca8k_resume);
+
+static const struct of_device_id qca8k_of_match[] = {
+	{ .compatible = "qca,qca8337" },
+	{ /* sentinel */ },
+};
+
+static struct mdio_driver qca8kmdio_driver = {
+	.probe  = qca8k_sw_probe,
+	.remove = qca8k_sw_remove,
+	.mdiodrv.driver = {
+		.name = "qca8k",
+		.of_match_table = qca8k_of_match,
+		.pm = &qca8k_pm_ops,
+	},
+};
+
+mdio_module_driver(qca8kmdio_driver);
+
+MODULE_AUTHOR("Mathieu Olivari, John Crispin <john@phrozen.org>");
+MODULE_DESCRIPTION("Driver for QCA8K ethernet switch family");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:qca8k");
diff --git a/drivers/net/dsa/qca8k.h b/drivers/net/dsa/qca8k.h
new file mode 100644
index 000000000000..201464719531
--- /dev/null
+++ b/drivers/net/dsa/qca8k.h
@@ -0,0 +1,185 @@
+/*
+ * Copyright (C) 2009 Felix Fietkau <nbd@nbd.name>
+ * Copyright (C) 2011-2012 Gabor Juhos <juhosg@openwrt.org>
+ * Copyright (c) 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 __QCA8K_H
+#define __QCA8K_H
+
+#include <linux/delay.h>
+#include <linux/regmap.h>
+
+#define QCA8K_NUM_PORTS					7
+
+#define PHY_ID_QCA8337					0x004dd036
+#define QCA8K_ID_QCA8337				0x13
+
+#define QCA8K_NUM_FDB_RECORDS				2048
+
+#define QCA8K_CPU_PORT					0
+
+/* Global control registers */
+#define QCA8K_REG_MASK_CTRL				0x000
+#define   QCA8K_MASK_CTRL_ID_M				0xff
+#define   QCA8K_MASK_CTRL_ID_S				8
+#define QCA8K_REG_PORT0_PAD_CTRL			0x004
+#define QCA8K_REG_PORT5_PAD_CTRL			0x008
+#define QCA8K_REG_PORT6_PAD_CTRL			0x00c
+#define   QCA8K_PORT_PAD_RGMII_EN			BIT(26)
+#define   QCA8K_PORT_PAD_RGMII_TX_DELAY(x)		\
+						((0x8 + (x & 0x3)) << 22)
+#define   QCA8K_PORT_PAD_RGMII_RX_DELAY(x)		\
+						((0x10 + (x & 0x3)) << 20)
+#define   QCA8K_PORT_PAD_RGMII_RX_DELAY_EN		BIT(24)
+#define   QCA8K_PORT_PAD_SGMII_EN			BIT(7)
+#define QCA8K_REG_MODULE_EN				0x030
+#define   QCA8K_MODULE_EN_MIB				BIT(0)
+#define QCA8K_REG_MIB					0x034
+#define   QCA8K_MIB_FLUSH				BIT(24)
+#define   QCA8K_MIB_CPU_KEEP				BIT(20)
+#define   QCA8K_MIB_BUSY				BIT(17)
+#define QCA8K_GOL_MAC_ADDR0				0x60
+#define QCA8K_GOL_MAC_ADDR1				0x64
+#define QCA8K_REG_PORT_STATUS(_i)			(0x07c + (_i) * 4)
+#define   QCA8K_PORT_STATUS_SPEED			GENMASK(2, 0)
+#define   QCA8K_PORT_STATUS_SPEED_S			0
+#define   QCA8K_PORT_STATUS_TXMAC			BIT(2)
+#define   QCA8K_PORT_STATUS_RXMAC			BIT(3)
+#define   QCA8K_PORT_STATUS_TXFLOW			BIT(4)
+#define   QCA8K_PORT_STATUS_RXFLOW			BIT(5)
+#define   QCA8K_PORT_STATUS_DUPLEX			BIT(6)
+#define   QCA8K_PORT_STATUS_LINK_UP			BIT(8)
+#define   QCA8K_PORT_STATUS_LINK_AUTO			BIT(9)
+#define   QCA8K_PORT_STATUS_LINK_PAUSE			BIT(10)
+#define QCA8K_REG_PORT_HDR_CTRL(_i)			(0x9c + (_i * 4))
+#define   QCA8K_PORT_HDR_CTRL_RX_MASK			GENMASK(3, 2)
+#define   QCA8K_PORT_HDR_CTRL_RX_S			2
+#define   QCA8K_PORT_HDR_CTRL_TX_MASK			GENMASK(1, 0)
+#define   QCA8K_PORT_HDR_CTRL_TX_S			0
+#define   QCA8K_PORT_HDR_CTRL_ALL			2
+#define   QCA8K_PORT_HDR_CTRL_MGMT			1
+#define   QCA8K_PORT_HDR_CTRL_NONE			0
+
+/* EEE control registers */
+#define QCA8K_REG_EEE_CTRL				0x100
+#define  QCA8K_REG_EEE_CTRL_LPI_EN(_i)			((_i + 1) * 2)
+
+/* ACL registers */
+#define QCA8K_REG_PORT_VLAN_CTRL0(_i)			(0x420 + (_i * 8))
+#define   QCA8K_PORT_VLAN_CVID(x)			(x << 16)
+#define   QCA8K_PORT_VLAN_SVID(x)			x
+#define QCA8K_REG_PORT_VLAN_CTRL1(_i)			(0x424 + (_i * 8))
+#define QCA8K_REG_IPV4_PRI_BASE_ADDR			0x470
+#define QCA8K_REG_IPV4_PRI_ADDR_MASK			0x474
+
+/* Lookup registers */
+#define QCA8K_REG_ATU_DATA0				0x600
+#define   QCA8K_ATU_ADDR2_S				24
+#define   QCA8K_ATU_ADDR3_S				16
+#define   QCA8K_ATU_ADDR4_S				8
+#define QCA8K_REG_ATU_DATA1				0x604
+#define   QCA8K_ATU_PORT_M				0x7f
+#define   QCA8K_ATU_PORT_S				16
+#define   QCA8K_ATU_ADDR0_S				8
+#define QCA8K_REG_ATU_DATA2				0x608
+#define   QCA8K_ATU_VID_M				0xfff
+#define   QCA8K_ATU_VID_S				8
+#define   QCA8K_ATU_STATUS_M				0xf
+#define   QCA8K_ATU_STATUS_STATIC			0xf
+#define QCA8K_REG_ATU_FUNC				0x60c
+#define   QCA8K_ATU_FUNC_BUSY				BIT(31)
+#define   QCA8K_ATU_FUNC_PORT_EN			BIT(14)
+#define   QCA8K_ATU_FUNC_MULTI_EN			BIT(13)
+#define   QCA8K_ATU_FUNC_FULL				BIT(12)
+#define   QCA8K_ATU_FUNC_PORT_M				0xf
+#define   QCA8K_ATU_FUNC_PORT_S				8
+#define QCA8K_REG_GLOBAL_FW_CTRL0			0x620
+#define   QCA8K_GLOBAL_FW_CTRL0_CPU_PORT_EN		BIT(10)
+#define QCA8K_REG_GLOBAL_FW_CTRL1			0x624
+#define   QCA8K_GLOBAL_FW_CTRL1_IGMP_DP_S		24
+#define   QCA8K_GLOBAL_FW_CTRL1_BC_DP_S			16
+#define   QCA8K_GLOBAL_FW_CTRL1_MC_DP_S			8
+#define   QCA8K_GLOBAL_FW_CTRL1_UC_DP_S			0
+#define QCA8K_PORT_LOOKUP_CTRL(_i)			(0x660 + (_i) * 0xc)
+#define   QCA8K_PORT_LOOKUP_MEMBER			GENMASK(6, 0)
+#define   QCA8K_PORT_LOOKUP_STATE_MASK			GENMASK(18, 16)
+#define   QCA8K_PORT_LOOKUP_STATE_DISABLED		(0 << 16)
+#define   QCA8K_PORT_LOOKUP_STATE_BLOCKING		(1 << 16)
+#define   QCA8K_PORT_LOOKUP_STATE_LISTENING		(2 << 16)
+#define   QCA8K_PORT_LOOKUP_STATE_LEARNING		(3 << 16)
+#define   QCA8K_PORT_LOOKUP_STATE_FORWARD		(4 << 16)
+#define   QCA8K_PORT_LOOKUP_STATE			GENMASK(18, 16)
+#define   QCA8K_PORT_LOOKUP_LEARN			BIT(20)
+
+/* Pkt edit registers */
+#define QCA8K_EGRESS_VLAN(x)				(0x0c70 + (4 * (x / 2)))
+
+/* L3 registers */
+#define QCA8K_HROUTER_CONTROL				0xe00
+#define   QCA8K_HROUTER_CONTROL_GLB_LOCKTIME_M		GENMASK(17, 16)
+#define   QCA8K_HROUTER_CONTROL_GLB_LOCKTIME_S		16
+#define   QCA8K_HROUTER_CONTROL_ARP_AGE_MODE		1
+#define QCA8K_HROUTER_PBASED_CONTROL1			0xe08
+#define QCA8K_HROUTER_PBASED_CONTROL2			0xe0c
+#define QCA8K_HNAT_CONTROL				0xe38
+
+/* MIB registers */
+#define QCA8K_PORT_MIB_COUNTER(_i)			(0x1000 + (_i) * 0x100)
+
+/* QCA specific MII registers */
+#define MII_ATH_MMD_ADDR				0x0d
+#define MII_ATH_MMD_DATA				0x0e
+
+enum {
+	QCA8K_PORT_SPEED_10M = 0,
+	QCA8K_PORT_SPEED_100M = 1,
+	QCA8K_PORT_SPEED_1000M = 2,
+	QCA8K_PORT_SPEED_ERR = 3,
+};
+
+enum qca8k_fdb_cmd {
+	QCA8K_FDB_FLUSH	= 1,
+	QCA8K_FDB_LOAD = 2,
+	QCA8K_FDB_PURGE = 3,
+	QCA8K_FDB_NEXT = 6,
+	QCA8K_FDB_SEARCH = 7,
+};
+
+struct ar8xxx_port_status {
+	struct ethtool_eee eee;
+	struct net_device *bridge_dev;
+	int enabled;
+};
+
+struct qca8k_priv {
+	struct regmap *regmap;
+	struct mii_bus *bus;
+	struct ar8xxx_port_status port_sts[QCA8K_NUM_PORTS];
+	struct dsa_switch *ds;
+	struct mutex reg_mutex;
+};
+
+struct qca8k_mib_desc {
+	unsigned int size;
+	unsigned int offset;
+	const char *name;
+};
+
+struct qca8k_fdb {
+	u16 vid;
+	u8 port_mask;
+	u8 aging;
+	u8 mac[6];
+};
+
+#endif /* __QCA8K_H */
diff --git a/drivers/net/ethernet/3com/3c59x.c b/drivers/net/ethernet/3com/3c59x.c
index 25c55ab05c7d..9133e7926da5 100644
--- a/drivers/net/ethernet/3com/3c59x.c
+++ b/drivers/net/ethernet/3com/3c59x.c
@@ -3089,7 +3089,7 @@ static void set_rx_mode(struct net_device *dev)
 	iowrite16(new_mode, ioaddr + EL3_CMD);
 }
 
-#if defined(CONFIG_VLAN_8021Q) || defined(CONFIG_VLAN_8021Q_MODULE)
+#if IS_ENABLED(CONFIG_VLAN_8021Q)
 /* Setup the card so that it can receive frames with an 802.1q VLAN tag.
    Note that this must be done after each RxReset due to some backwards
    compatibility logic in the Cyclone and Tornado ASICs */
diff --git a/drivers/net/ethernet/Kconfig b/drivers/net/ethernet/Kconfig
index 2ffd63463299..8cc7467b6c1f 100644
--- a/drivers/net/ethernet/Kconfig
+++ b/drivers/net/ethernet/Kconfig
@@ -24,6 +24,7 @@ source "drivers/net/ethernet/agere/Kconfig"
 source "drivers/net/ethernet/allwinner/Kconfig"
 source "drivers/net/ethernet/alteon/Kconfig"
 source "drivers/net/ethernet/altera/Kconfig"
+source "drivers/net/ethernet/amazon/Kconfig"
 source "drivers/net/ethernet/amd/Kconfig"
 source "drivers/net/ethernet/apm/Kconfig"
 source "drivers/net/ethernet/apple/Kconfig"
diff --git a/drivers/net/ethernet/Makefile b/drivers/net/ethernet/Makefile
index 1d349e9aa9a6..a09423df83f2 100644
--- a/drivers/net/ethernet/Makefile
+++ b/drivers/net/ethernet/Makefile
@@ -10,6 +10,7 @@ obj-$(CONFIG_NET_VENDOR_AGERE) += agere/
 obj-$(CONFIG_NET_VENDOR_ALLWINNER) += allwinner/
 obj-$(CONFIG_NET_VENDOR_ALTEON) += alteon/
 obj-$(CONFIG_ALTERA_TSE) += altera/
+obj-$(CONFIG_NET_VENDOR_AMAZON) += amazon/
 obj-$(CONFIG_NET_VENDOR_AMD) += amd/
 obj-$(CONFIG_NET_XGENE) += apm/
 obj-$(CONFIG_NET_VENDOR_APPLE) += apple/
diff --git a/drivers/net/ethernet/adaptec/starfire.c b/drivers/net/ethernet/adaptec/starfire.c
index 1d1069641d81..8af2c88d5b33 100644
--- a/drivers/net/ethernet/adaptec/starfire.c
+++ b/drivers/net/ethernet/adaptec/starfire.c
@@ -66,7 +66,7 @@
  */
 #define ZEROCOPY
 
-#if defined(CONFIG_VLAN_8021Q) || defined(CONFIG_VLAN_8021Q_MODULE)
+#if IS_ENABLED(CONFIG_VLAN_8021Q)
 #define VLAN_SUPPORT
 #endif
 
diff --git a/drivers/net/ethernet/adi/bfin_mac.c b/drivers/net/ethernet/adi/bfin_mac.c
index 38eaea18da23..00f9ee3fc3e5 100644
--- a/drivers/net/ethernet/adi/bfin_mac.c
+++ b/drivers/net/ethernet/adi/bfin_mac.c
@@ -192,8 +192,8 @@ static int desc_list_init(struct net_device *dev)
 			goto init_error;
 
 		skb_reserve(new_skb, NET_IP_ALIGN);
-		/* Invidate the data cache of skb->data range when it is write back
-		 * cache. It will prevent overwritting the new data from DMA
+		/* Invalidate the data cache of skb->data range when it is write back
+		 * cache. It will prevent overwriting the new data from DMA
 		 */
 		blackfin_dcache_invalidate_range((unsigned long)new_skb->head,
 					 (unsigned long)new_skb->end);
@@ -1205,7 +1205,7 @@ static void bfin_mac_rx(struct bfin_mac_local *lp)
 	}
 	/* reserve 2 bytes for RXDWA padding */
 	skb_reserve(new_skb, NET_IP_ALIGN);
-	/* Invidate the data cache of skb->data range when it is write back
+	/* Invalidate the data cache of skb->data range when it is write back
 	 * cache. It will prevent overwritting the new data from DMA
 	 */
 	blackfin_dcache_invalidate_range((unsigned long)new_skb->head,
@@ -1599,7 +1599,7 @@ static int bfin_mac_probe(struct platform_device *pdev)
 	*(__le16 *) (&(ndev->dev_addr[4])) = cpu_to_le16((u16) bfin_read_EMAC_ADDRHI());
 
 	/* probe mac */
-	/*todo: how to proble? which is revision_register */
+	/*todo: how to probe? which is revision_register */
 	bfin_write_EMAC_ADDRLO(0x12345678);
 	if (bfin_read_EMAC_ADDRLO() != 0x12345678) {
 		dev_err(&pdev->dev, "Cannot detect Blackfin on-chip ethernet MAC controller!\n");
diff --git a/drivers/net/ethernet/aeroflex/greth.c b/drivers/net/ethernet/aeroflex/greth.c
index bca07c5c94bd..f8df8248035e 100644
--- a/drivers/net/ethernet/aeroflex/greth.c
+++ b/drivers/net/ethernet/aeroflex/greth.c
@@ -1105,27 +1105,6 @@ static void greth_set_msglevel(struct net_device *dev, u32 value)
 	struct greth_private *greth = netdev_priv(dev);
 	greth->msg_enable = value;
 }
-static int greth_get_settings(struct net_device *dev, struct ethtool_cmd *cmd)
-{
-	struct greth_private *greth = netdev_priv(dev);
-	struct phy_device *phy = greth->phy;
-
-	if (!phy)
-		return -ENODEV;
-
-	return phy_ethtool_gset(phy, cmd);
-}
-
-static int greth_set_settings(struct net_device *dev, struct ethtool_cmd *cmd)
-{
-	struct greth_private *greth = netdev_priv(dev);
-	struct phy_device *phy = greth->phy;
-
-	if (!phy)
-		return -ENODEV;
-
-	return phy_ethtool_sset(phy, cmd);
-}
 
 static int greth_get_regs_len(struct net_device *dev)
 {
@@ -1157,12 +1136,12 @@ static void greth_get_regs(struct net_device *dev, struct ethtool_regs *regs, vo
 static const struct ethtool_ops greth_ethtool_ops = {
 	.get_msglevel		= greth_get_msglevel,
 	.set_msglevel		= greth_set_msglevel,
-	.get_settings		= greth_get_settings,
-	.set_settings		= greth_set_settings,
 	.get_drvinfo		= greth_get_drvinfo,
 	.get_regs_len           = greth_get_regs_len,
 	.get_regs               = greth_get_regs,
 	.get_link		= ethtool_op_get_link,
+	.get_link_ksettings	= phy_ethtool_get_link_ksettings,
+	.set_link_ksettings	= phy_ethtool_set_link_ksettings,
 };
 
 static struct net_device_ops greth_netdev_ops = {
@@ -1224,7 +1203,7 @@ static int greth_mdio_write(struct mii_bus *bus, int phy, int reg, u16 val)
 static void greth_link_change(struct net_device *dev)
 {
 	struct greth_private *greth = netdev_priv(dev);
-	struct phy_device *phydev = greth->phy;
+	struct phy_device *phydev = dev->phydev;
 	unsigned long flags;
 	int status_change = 0;
 	u32 ctrl;
@@ -1307,7 +1286,6 @@ static int greth_mdio_probe(struct net_device *dev)
 	greth->link = 0;
 	greth->speed = 0;
 	greth->duplex = -1;
-	greth->phy = phy;
 
 	return 0;
 }
@@ -1325,6 +1303,7 @@ static int greth_mdio_init(struct greth_private *greth)
 {
 	int ret;
 	unsigned long timeout;
+	struct net_device *ndev = greth->netdev;
 
 	greth->mdio = mdiobus_alloc();
 	if (!greth->mdio) {
@@ -1349,15 +1328,16 @@ static int greth_mdio_init(struct greth_private *greth)
 		goto unreg_mdio;
 	}
 
-	phy_start(greth->phy);
+	phy_start(ndev->phydev);
 
 	/* If Ethernet debug link is used make autoneg happen right away */
 	if (greth->edcl && greth_edcl == 1) {
-		phy_start_aneg(greth->phy);
+		phy_start_aneg(ndev->phydev);
 		timeout = jiffies + 6*HZ;
-		while (!phy_aneg_done(greth->phy) && time_before(jiffies, timeout)) {
+		while (!phy_aneg_done(ndev->phydev) &&
+		       time_before(jiffies, timeout)) {
 		}
-		phy_read_status(greth->phy);
+		phy_read_status(ndev->phydev);
 		greth_link_change(greth->netdev);
 	}
 
@@ -1569,8 +1549,8 @@ static int greth_of_remove(struct platform_device *of_dev)
 
 	dma_free_coherent(&of_dev->dev, 1024, greth->tx_bd_base, greth->tx_bd_base_phys);
 
-	if (greth->phy)
-		phy_stop(greth->phy);
+	if (ndev->phydev)
+		phy_stop(ndev->phydev);
 	mdiobus_unregister(greth->mdio);
 
 	unregister_netdev(ndev);
diff --git a/drivers/net/ethernet/aeroflex/greth.h b/drivers/net/ethernet/aeroflex/greth.h
index 92dd918e4a83..9c07140a5d8d 100644
--- a/drivers/net/ethernet/aeroflex/greth.h
+++ b/drivers/net/ethernet/aeroflex/greth.h
@@ -123,7 +123,6 @@ struct greth_private {
 	struct napi_struct napi;
 	spinlock_t devlock;
 
-	struct phy_device *phy;
 	struct mii_bus *mdio;
 	unsigned int link;
 	unsigned int speed;
diff --git a/drivers/net/ethernet/agere/et131x.c b/drivers/net/ethernet/agere/et131x.c
index c83ebae73d91..906683851c7d 100644
--- a/drivers/net/ethernet/agere/et131x.c
+++ b/drivers/net/ethernet/agere/et131x.c
@@ -2961,7 +2961,7 @@ static void et131x_get_drvinfo(struct net_device *netdev,
 		sizeof(info->bus_info));
 }
 
-static struct ethtool_ops et131x_ethtool_ops = {
+static const struct ethtool_ops et131x_ethtool_ops = {
 	.get_drvinfo	= et131x_get_drvinfo,
 	.get_regs_len	= et131x_get_regs_len,
 	.get_regs	= et131x_get_regs,
diff --git a/drivers/net/ethernet/amazon/Kconfig b/drivers/net/ethernet/amazon/Kconfig
new file mode 100644
index 000000000000..99b30353541a
--- /dev/null
+++ b/drivers/net/ethernet/amazon/Kconfig
@@ -0,0 +1,27 @@
+#
+# Amazon network device configuration
+#
+
+config NET_VENDOR_AMAZON
+	bool "Amazon 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 Amazon devices. If you say Y, you will be asked
+	  for your specific device in the following questions.
+
+if NET_VENDOR_AMAZON
+
+config ENA_ETHERNET
+	tristate "Elastic Network Adapter (ENA) support"
+	depends on (PCI_MSI && X86)
+	---help---
+	  This driver supports Elastic Network Adapter (ENA)"
+
+	  To compile this driver as a module, choose M here.
+	  The module will be called ena.
+
+endif #NET_VENDOR_AMAZON
diff --git a/drivers/net/ethernet/amazon/Makefile b/drivers/net/ethernet/amazon/Makefile
new file mode 100644
index 000000000000..8e0b73f60d51
--- /dev/null
+++ b/drivers/net/ethernet/amazon/Makefile
@@ -0,0 +1,5 @@
+#
+# Makefile for the Amazon network device drivers.
+#
+
+obj-$(CONFIG_ENA_ETHERNET) += ena/
diff --git a/drivers/net/ethernet/amazon/ena/Makefile b/drivers/net/ethernet/amazon/ena/Makefile
new file mode 100644
index 000000000000..eaeeae06c5d9
--- /dev/null
+++ b/drivers/net/ethernet/amazon/ena/Makefile
@@ -0,0 +1,7 @@
+#
+# Makefile for the Elastic Network Adapter (ENA) device drivers.
+#
+
+obj-$(CONFIG_ENA_ETHERNET) += ena.o
+
+ena-y := ena_netdev.o ena_com.o ena_eth_com.o ena_ethtool.o
diff --git a/drivers/net/ethernet/amazon/ena/ena_admin_defs.h b/drivers/net/ethernet/amazon/ena/ena_admin_defs.h
new file mode 100644
index 000000000000..a46e749bf226
--- /dev/null
+++ b/drivers/net/ethernet/amazon/ena/ena_admin_defs.h
@@ -0,0 +1,973 @@
+/*
+ * Copyright 2015 - 2016 Amazon.com, Inc. or its affiliates.
+ *
+ * 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
+ * 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 _ENA_ADMIN_H_
+#define _ENA_ADMIN_H_
+
+enum ena_admin_aq_opcode {
+	ENA_ADMIN_CREATE_SQ	= 1,
+
+	ENA_ADMIN_DESTROY_SQ	= 2,
+
+	ENA_ADMIN_CREATE_CQ	= 3,
+
+	ENA_ADMIN_DESTROY_CQ	= 4,
+
+	ENA_ADMIN_GET_FEATURE	= 8,
+
+	ENA_ADMIN_SET_FEATURE	= 9,
+
+	ENA_ADMIN_GET_STATS	= 11,
+};
+
+enum ena_admin_aq_completion_status {
+	ENA_ADMIN_SUCCESS			= 0,
+
+	ENA_ADMIN_RESOURCE_ALLOCATION_FAILURE	= 1,
+
+	ENA_ADMIN_BAD_OPCODE			= 2,
+
+	ENA_ADMIN_UNSUPPORTED_OPCODE		= 3,
+
+	ENA_ADMIN_MALFORMED_REQUEST		= 4,
+
+	/* Additional status is provided in ACQ entry extended_status */
+	ENA_ADMIN_ILLEGAL_PARAMETER		= 5,
+
+	ENA_ADMIN_UNKNOWN_ERROR			= 6,
+};
+
+enum ena_admin_aq_feature_id {
+	ENA_ADMIN_DEVICE_ATTRIBUTES		= 1,
+
+	ENA_ADMIN_MAX_QUEUES_NUM		= 2,
+
+	ENA_ADMIN_RSS_HASH_FUNCTION		= 10,
+
+	ENA_ADMIN_STATELESS_OFFLOAD_CONFIG	= 11,
+
+	ENA_ADMIN_RSS_REDIRECTION_TABLE_CONFIG	= 12,
+
+	ENA_ADMIN_MTU				= 14,
+
+	ENA_ADMIN_RSS_HASH_INPUT		= 18,
+
+	ENA_ADMIN_INTERRUPT_MODERATION		= 20,
+
+	ENA_ADMIN_AENQ_CONFIG			= 26,
+
+	ENA_ADMIN_LINK_CONFIG			= 27,
+
+	ENA_ADMIN_HOST_ATTR_CONFIG		= 28,
+
+	ENA_ADMIN_FEATURES_OPCODE_NUM		= 32,
+};
+
+enum ena_admin_placement_policy_type {
+	/* descriptors and headers are in host memory */
+	ENA_ADMIN_PLACEMENT_POLICY_HOST	= 1,
+
+	/* descriptors and headers are in device memory (a.k.a Low Latency
+	 * Queue)
+	 */
+	ENA_ADMIN_PLACEMENT_POLICY_DEV	= 3,
+};
+
+enum ena_admin_link_types {
+	ENA_ADMIN_LINK_SPEED_1G		= 0x1,
+
+	ENA_ADMIN_LINK_SPEED_2_HALF_G	= 0x2,
+
+	ENA_ADMIN_LINK_SPEED_5G		= 0x4,
+
+	ENA_ADMIN_LINK_SPEED_10G	= 0x8,
+
+	ENA_ADMIN_LINK_SPEED_25G	= 0x10,
+
+	ENA_ADMIN_LINK_SPEED_40G	= 0x20,
+
+	ENA_ADMIN_LINK_SPEED_50G	= 0x40,
+
+	ENA_ADMIN_LINK_SPEED_100G	= 0x80,
+
+	ENA_ADMIN_LINK_SPEED_200G	= 0x100,
+
+	ENA_ADMIN_LINK_SPEED_400G	= 0x200,
+};
+
+enum ena_admin_completion_policy_type {
+	/* completion queue entry for each sq descriptor */
+	ENA_ADMIN_COMPLETION_POLICY_DESC		= 0,
+
+	/* completion queue entry upon request in sq descriptor */
+	ENA_ADMIN_COMPLETION_POLICY_DESC_ON_DEMAND	= 1,
+
+	/* current queue head pointer is updated in OS memory upon sq
+	 * descriptor request
+	 */
+	ENA_ADMIN_COMPLETION_POLICY_HEAD_ON_DEMAND	= 2,
+
+	/* current queue head pointer is updated in OS memory for each sq
+	 * descriptor
+	 */
+	ENA_ADMIN_COMPLETION_POLICY_HEAD		= 3,
+};
+
+/* basic stats return ena_admin_basic_stats while extanded stats return a
+ * buffer (string format) with additional statistics per queue and per
+ * device id
+ */
+enum ena_admin_get_stats_type {
+	ENA_ADMIN_GET_STATS_TYPE_BASIC		= 0,
+
+	ENA_ADMIN_GET_STATS_TYPE_EXTENDED	= 1,
+};
+
+enum ena_admin_get_stats_scope {
+	ENA_ADMIN_SPECIFIC_QUEUE	= 0,
+
+	ENA_ADMIN_ETH_TRAFFIC		= 1,
+};
+
+struct ena_admin_aq_common_desc {
+	/* 11:0 : command_id
+	 * 15:12 : reserved12
+	 */
+	u16 command_id;
+
+	/* as appears in ena_admin_aq_opcode */
+	u8 opcode;
+
+	/* 0 : phase
+	 * 1 : ctrl_data - control buffer address valid
+	 * 2 : ctrl_data_indirect - control buffer address
+	 *    points to list of pages with addresses of control
+	 *    buffers
+	 * 7:3 : reserved3
+	 */
+	u8 flags;
+};
+
+/* used in ena_admin_aq_entry. Can point directly to control data, or to a
+ * page list chunk. Used also at the end of indirect mode page list chunks,
+ * for chaining.
+ */
+struct ena_admin_ctrl_buff_info {
+	u32 length;
+
+	struct ena_common_mem_addr address;
+};
+
+struct ena_admin_sq {
+	u16 sq_idx;
+
+	/* 4:0 : reserved
+	 * 7:5 : sq_direction - 0x1 - Tx; 0x2 - Rx
+	 */
+	u8 sq_identity;
+
+	u8 reserved1;
+};
+
+struct ena_admin_aq_entry {
+	struct ena_admin_aq_common_desc aq_common_descriptor;
+
+	union {
+		u32 inline_data_w1[3];
+
+		struct ena_admin_ctrl_buff_info control_buffer;
+	} u;
+
+	u32 inline_data_w4[12];
+};
+
+struct ena_admin_acq_common_desc {
+	/* command identifier to associate it with the aq descriptor
+	 * 11:0 : command_id
+	 * 15:12 : reserved12
+	 */
+	u16 command;
+
+	u8 status;
+
+	/* 0 : phase
+	 * 7:1 : reserved1
+	 */
+	u8 flags;
+
+	u16 extended_status;
+
+	/* serves as a hint what AQ entries can be revoked */
+	u16 sq_head_indx;
+};
+
+struct ena_admin_acq_entry {
+	struct ena_admin_acq_common_desc acq_common_descriptor;
+
+	u32 response_specific_data[14];
+};
+
+struct ena_admin_aq_create_sq_cmd {
+	struct ena_admin_aq_common_desc aq_common_descriptor;
+
+	/* 4:0 : reserved0_w1
+	 * 7:5 : sq_direction - 0x1 - Tx, 0x2 - Rx
+	 */
+	u8 sq_identity;
+
+	u8 reserved8_w1;
+
+	/* 3:0 : placement_policy - Describing where the SQ
+	 *    descriptor ring and the SQ packet headers reside:
+	 *    0x1 - descriptors and headers are in OS memory,
+	 *    0x3 - descriptors and headers in device memory
+	 *    (a.k.a Low Latency Queue)
+	 * 6:4 : completion_policy - Describing what policy
+	 *    to use for generation completion entry (cqe) in
+	 *    the CQ associated with this SQ: 0x0 - cqe for each
+	 *    sq descriptor, 0x1 - cqe upon request in sq
+	 *    descriptor, 0x2 - current queue head pointer is
+	 *    updated in OS memory upon sq descriptor request
+	 *    0x3 - current queue head pointer is updated in OS
+	 *    memory for each sq descriptor
+	 * 7 : reserved15_w1
+	 */
+	u8 sq_caps_2;
+
+	/* 0 : is_physically_contiguous - Described if the
+	 *    queue ring memory is allocated in physical
+	 *    contiguous pages or split.
+	 * 7:1 : reserved17_w1
+	 */
+	u8 sq_caps_3;
+
+	/* associated completion queue id. This CQ must be created prior to
+	 *    SQ creation
+	 */
+	u16 cq_idx;
+
+	/* submission queue depth in entries */
+	u16 sq_depth;
+
+	/* SQ physical base address in OS memory. This field should not be
+	 * used for Low Latency queues. Has to be page aligned.
+	 */
+	struct ena_common_mem_addr sq_ba;
+
+	/* specifies queue head writeback location in OS memory. Valid if
+	 * completion_policy is set to completion_policy_head_on_demand or
+	 * completion_policy_head. Has to be cache aligned
+	 */
+	struct ena_common_mem_addr sq_head_writeback;
+
+	u32 reserved0_w7;
+
+	u32 reserved0_w8;
+};
+
+enum ena_admin_sq_direction {
+	ENA_ADMIN_SQ_DIRECTION_TX	= 1,
+
+	ENA_ADMIN_SQ_DIRECTION_RX	= 2,
+};
+
+struct ena_admin_acq_create_sq_resp_desc {
+	struct ena_admin_acq_common_desc acq_common_desc;
+
+	u16 sq_idx;
+
+	u16 reserved;
+
+	/* queue doorbell address as an offset to PCIe MMIO REG BAR */
+	u32 sq_doorbell_offset;
+
+	/* low latency queue ring base address as an offset to PCIe MMIO
+	 * LLQ_MEM BAR
+	 */
+	u32 llq_descriptors_offset;
+
+	/* low latency queue headers' memory as an offset to PCIe MMIO
+	 * LLQ_MEM BAR
+	 */
+	u32 llq_headers_offset;
+};
+
+struct ena_admin_aq_destroy_sq_cmd {
+	struct ena_admin_aq_common_desc aq_common_descriptor;
+
+	struct ena_admin_sq sq;
+};
+
+struct ena_admin_acq_destroy_sq_resp_desc {
+	struct ena_admin_acq_common_desc acq_common_desc;
+};
+
+struct ena_admin_aq_create_cq_cmd {
+	struct ena_admin_aq_common_desc aq_common_descriptor;
+
+	/* 4:0 : reserved5
+	 * 5 : interrupt_mode_enabled - if set, cq operates
+	 *    in interrupt mode, otherwise - polling
+	 * 7:6 : reserved6
+	 */
+	u8 cq_caps_1;
+
+	/* 4:0 : cq_entry_size_words - size of CQ entry in
+	 *    32-bit words, valid values: 4, 8.
+	 * 7:5 : reserved7
+	 */
+	u8 cq_caps_2;
+
+	/* completion queue depth in # of entries. must be power of 2 */
+	u16 cq_depth;
+
+	/* msix vector assigned to this cq */
+	u32 msix_vector;
+
+	/* cq physical base address in OS memory. CQ must be physically
+	 * contiguous
+	 */
+	struct ena_common_mem_addr cq_ba;
+};
+
+struct ena_admin_acq_create_cq_resp_desc {
+	struct ena_admin_acq_common_desc acq_common_desc;
+
+	u16 cq_idx;
+
+	/* actual cq depth in number of entries */
+	u16 cq_actual_depth;
+
+	u32 numa_node_register_offset;
+
+	u32 cq_head_db_register_offset;
+
+	u32 cq_interrupt_unmask_register_offset;
+};
+
+struct ena_admin_aq_destroy_cq_cmd {
+	struct ena_admin_aq_common_desc aq_common_descriptor;
+
+	u16 cq_idx;
+
+	u16 reserved1;
+};
+
+struct ena_admin_acq_destroy_cq_resp_desc {
+	struct ena_admin_acq_common_desc acq_common_desc;
+};
+
+/* ENA AQ Get Statistics command. Extended statistics are placed in control
+ * buffer pointed by AQ entry
+ */
+struct ena_admin_aq_get_stats_cmd {
+	struct ena_admin_aq_common_desc aq_common_descriptor;
+
+	union {
+		/* command specific inline data */
+		u32 inline_data_w1[3];
+
+		struct ena_admin_ctrl_buff_info control_buffer;
+	} u;
+
+	/* stats type as defined in enum ena_admin_get_stats_type */
+	u8 type;
+
+	/* stats scope defined in enum ena_admin_get_stats_scope */
+	u8 scope;
+
+	u16 reserved3;
+
+	/* queue id. used when scope is specific_queue */
+	u16 queue_idx;
+
+	/* device id, value 0xFFFF means mine. only privileged device can get
+	 *    stats of other device
+	 */
+	u16 device_id;
+};
+
+/* Basic Statistics Command. */
+struct ena_admin_basic_stats {
+	u32 tx_bytes_low;
+
+	u32 tx_bytes_high;
+
+	u32 tx_pkts_low;
+
+	u32 tx_pkts_high;
+
+	u32 rx_bytes_low;
+
+	u32 rx_bytes_high;
+
+	u32 rx_pkts_low;
+
+	u32 rx_pkts_high;
+
+	u32 rx_drops_low;
+
+	u32 rx_drops_high;
+};
+
+struct ena_admin_acq_get_stats_resp {
+	struct ena_admin_acq_common_desc acq_common_desc;
+
+	struct ena_admin_basic_stats basic_stats;
+};
+
+struct ena_admin_get_set_feature_common_desc {
+	/* 1:0 : select - 0x1 - current value; 0x3 - default
+	 *    value
+	 * 7:3 : reserved3
+	 */
+	u8 flags;
+
+	/* as appears in ena_admin_aq_feature_id */
+	u8 feature_id;
+
+	u16 reserved16;
+};
+
+struct ena_admin_device_attr_feature_desc {
+	u32 impl_id;
+
+	u32 device_version;
+
+	/* bitmap of ena_admin_aq_feature_id */
+	u32 supported_features;
+
+	u32 reserved3;
+
+	/* Indicates how many bits are used physical address access. */
+	u32 phys_addr_width;
+
+	/* Indicates how many bits are used virtual address access. */
+	u32 virt_addr_width;
+
+	/* unicast MAC address (in Network byte order) */
+	u8 mac_addr[6];
+
+	u8 reserved7[2];
+
+	u32 max_mtu;
+};
+
+struct ena_admin_queue_feature_desc {
+	/* including LLQs */
+	u32 max_sq_num;
+
+	u32 max_sq_depth;
+
+	u32 max_cq_num;
+
+	u32 max_cq_depth;
+
+	u32 max_llq_num;
+
+	u32 max_llq_depth;
+
+	u32 max_header_size;
+
+	/* Maximum Descriptors number, including meta descriptor, allowed for
+	 *    a single Tx packet
+	 */
+	u16 max_packet_tx_descs;
+
+	/* Maximum Descriptors number allowed for a single Rx packet */
+	u16 max_packet_rx_descs;
+};
+
+struct ena_admin_set_feature_mtu_desc {
+	/* exclude L2 */
+	u32 mtu;
+};
+
+struct ena_admin_set_feature_host_attr_desc {
+	/* host OS info base address in OS memory. host info is 4KB of
+	 * physically contiguous
+	 */
+	struct ena_common_mem_addr os_info_ba;
+
+	/* host debug area base address in OS memory. debug area must be
+	 * physically contiguous
+	 */
+	struct ena_common_mem_addr debug_ba;
+
+	/* debug area size */
+	u32 debug_area_size;
+};
+
+struct ena_admin_feature_intr_moder_desc {
+	/* interrupt delay granularity in usec */
+	u16 intr_delay_resolution;
+
+	u16 reserved;
+};
+
+struct ena_admin_get_feature_link_desc {
+	/* Link speed in Mb */
+	u32 speed;
+
+	/* bit field of enum ena_admin_link types */
+	u32 supported;
+
+	/* 0 : autoneg
+	 * 1 : duplex - Full Duplex
+	 * 31:2 : reserved2
+	 */
+	u32 flags;
+};
+
+struct ena_admin_feature_aenq_desc {
+	/* bitmask for AENQ groups the device can report */
+	u32 supported_groups;
+
+	/* bitmask for AENQ groups to report */
+	u32 enabled_groups;
+};
+
+struct ena_admin_feature_offload_desc {
+	/* 0 : TX_L3_csum_ipv4
+	 * 1 : TX_L4_ipv4_csum_part - The checksum field
+	 *    should be initialized with pseudo header checksum
+	 * 2 : TX_L4_ipv4_csum_full
+	 * 3 : TX_L4_ipv6_csum_part - The checksum field
+	 *    should be initialized with pseudo header checksum
+	 * 4 : TX_L4_ipv6_csum_full
+	 * 5 : tso_ipv4
+	 * 6 : tso_ipv6
+	 * 7 : tso_ecn
+	 */
+	u32 tx;
+
+	/* Receive side supported stateless offload
+	 * 0 : RX_L3_csum_ipv4 - IPv4 checksum
+	 * 1 : RX_L4_ipv4_csum - TCP/UDP/IPv4 checksum
+	 * 2 : RX_L4_ipv6_csum - TCP/UDP/IPv6 checksum
+	 * 3 : RX_hash - Hash calculation
+	 */
+	u32 rx_supported;
+
+	u32 rx_enabled;
+};
+
+enum ena_admin_hash_functions {
+	ENA_ADMIN_TOEPLITZ	= 1,
+
+	ENA_ADMIN_CRC32		= 2,
+};
+
+struct ena_admin_feature_rss_flow_hash_control {
+	u32 keys_num;
+
+	u32 reserved;
+
+	u32 key[10];
+};
+
+struct ena_admin_feature_rss_flow_hash_function {
+	/* 7:0 : funcs - bitmask of ena_admin_hash_functions */
+	u32 supported_func;
+
+	/* 7:0 : selected_func - bitmask of
+	 *    ena_admin_hash_functions
+	 */
+	u32 selected_func;
+
+	/* initial value */
+	u32 init_val;
+};
+
+/* RSS flow hash protocols */
+enum ena_admin_flow_hash_proto {
+	ENA_ADMIN_RSS_TCP4	= 0,
+
+	ENA_ADMIN_RSS_UDP4	= 1,
+
+	ENA_ADMIN_RSS_TCP6	= 2,
+
+	ENA_ADMIN_RSS_UDP6	= 3,
+
+	ENA_ADMIN_RSS_IP4	= 4,
+
+	ENA_ADMIN_RSS_IP6	= 5,
+
+	ENA_ADMIN_RSS_IP4_FRAG	= 6,
+
+	ENA_ADMIN_RSS_NOT_IP	= 7,
+
+	ENA_ADMIN_RSS_PROTO_NUM	= 16,
+};
+
+/* RSS flow hash fields */
+enum ena_admin_flow_hash_fields {
+	/* Ethernet Dest Addr */
+	ENA_ADMIN_RSS_L2_DA	= 0,
+
+	/* Ethernet Src Addr */
+	ENA_ADMIN_RSS_L2_SA	= 1,
+
+	/* ipv4/6 Dest Addr */
+	ENA_ADMIN_RSS_L3_DA	= 2,
+
+	/* ipv4/6 Src Addr */
+	ENA_ADMIN_RSS_L3_SA	= 5,
+
+	/* tcp/udp Dest Port */
+	ENA_ADMIN_RSS_L4_DP	= 6,
+
+	/* tcp/udp Src Port */
+	ENA_ADMIN_RSS_L4_SP	= 7,
+};
+
+struct ena_admin_proto_input {
+	/* flow hash fields (bitwise according to ena_admin_flow_hash_fields) */
+	u16 fields;
+
+	u16 reserved2;
+};
+
+struct ena_admin_feature_rss_hash_control {
+	struct ena_admin_proto_input supported_fields[ENA_ADMIN_RSS_PROTO_NUM];
+
+	struct ena_admin_proto_input selected_fields[ENA_ADMIN_RSS_PROTO_NUM];
+
+	struct ena_admin_proto_input reserved2[ENA_ADMIN_RSS_PROTO_NUM];
+
+	struct ena_admin_proto_input reserved3[ENA_ADMIN_RSS_PROTO_NUM];
+};
+
+struct ena_admin_feature_rss_flow_hash_input {
+	/* supported hash input sorting
+	 * 1 : L3_sort - support swap L3 addresses if DA is
+	 *    smaller than SA
+	 * 2 : L4_sort - support swap L4 ports if DP smaller
+	 *    SP
+	 */
+	u16 supported_input_sort;
+
+	/* enabled hash input sorting
+	 * 1 : enable_L3_sort - enable swap L3 addresses if
+	 *    DA smaller than SA
+	 * 2 : enable_L4_sort - enable swap L4 ports if DP
+	 *    smaller than SP
+	 */
+	u16 enabled_input_sort;
+};
+
+enum ena_admin_os_type {
+	ENA_ADMIN_OS_LINUX	= 1,
+
+	ENA_ADMIN_OS_WIN	= 2,
+
+	ENA_ADMIN_OS_DPDK	= 3,
+
+	ENA_ADMIN_OS_FREEBSD	= 4,
+
+	ENA_ADMIN_OS_IPXE	= 5,
+};
+
+struct ena_admin_host_info {
+	/* defined in enum ena_admin_os_type */
+	u32 os_type;
+
+	/* os distribution string format */
+	u8 os_dist_str[128];
+
+	/* OS distribution numeric format */
+	u32 os_dist;
+
+	/* kernel version string format */
+	u8 kernel_ver_str[32];
+
+	/* Kernel version numeric format */
+	u32 kernel_ver;
+
+	/* 7:0 : major
+	 * 15:8 : minor
+	 * 23:16 : sub_minor
+	 */
+	u32 driver_version;
+
+	/* features bitmap */
+	u32 supported_network_features[4];
+};
+
+struct ena_admin_rss_ind_table_entry {
+	u16 cq_idx;
+
+	u16 reserved;
+};
+
+struct ena_admin_feature_rss_ind_table {
+	/* min supported table size (2^min_size) */
+	u16 min_size;
+
+	/* max supported table size (2^max_size) */
+	u16 max_size;
+
+	/* table size (2^size) */
+	u16 size;
+
+	u16 reserved;
+
+	/* index of the inline entry. 0xFFFFFFFF means invalid */
+	u32 inline_index;
+
+	/* used for updating single entry, ignored when setting the entire
+	 * table through the control buffer.
+	 */
+	struct ena_admin_rss_ind_table_entry inline_entry;
+};
+
+struct ena_admin_get_feat_cmd {
+	struct ena_admin_aq_common_desc aq_common_descriptor;
+
+	struct ena_admin_ctrl_buff_info control_buffer;
+
+	struct ena_admin_get_set_feature_common_desc feat_common;
+
+	u32 raw[11];
+};
+
+struct ena_admin_get_feat_resp {
+	struct ena_admin_acq_common_desc acq_common_desc;
+
+	union {
+		u32 raw[14];
+
+		struct ena_admin_device_attr_feature_desc dev_attr;
+
+		struct ena_admin_queue_feature_desc max_queue;
+
+		struct ena_admin_feature_aenq_desc aenq;
+
+		struct ena_admin_get_feature_link_desc link;
+
+		struct ena_admin_feature_offload_desc offload;
+
+		struct ena_admin_feature_rss_flow_hash_function flow_hash_func;
+
+		struct ena_admin_feature_rss_flow_hash_input flow_hash_input;
+
+		struct ena_admin_feature_rss_ind_table ind_table;
+
+		struct ena_admin_feature_intr_moder_desc intr_moderation;
+	} u;
+};
+
+struct ena_admin_set_feat_cmd {
+	struct ena_admin_aq_common_desc aq_common_descriptor;
+
+	struct ena_admin_ctrl_buff_info control_buffer;
+
+	struct ena_admin_get_set_feature_common_desc feat_common;
+
+	union {
+		u32 raw[11];
+
+		/* mtu size */
+		struct ena_admin_set_feature_mtu_desc mtu;
+
+		/* host attributes */
+		struct ena_admin_set_feature_host_attr_desc host_attr;
+
+		/* AENQ configuration */
+		struct ena_admin_feature_aenq_desc aenq;
+
+		/* rss flow hash function */
+		struct ena_admin_feature_rss_flow_hash_function flow_hash_func;
+
+		/* rss flow hash input */
+		struct ena_admin_feature_rss_flow_hash_input flow_hash_input;
+
+		/* rss indirection table */
+		struct ena_admin_feature_rss_ind_table ind_table;
+	} u;
+};
+
+struct ena_admin_set_feat_resp {
+	struct ena_admin_acq_common_desc acq_common_desc;
+
+	union {
+		u32 raw[14];
+	} u;
+};
+
+struct ena_admin_aenq_common_desc {
+	u16 group;
+
+	u16 syndrom;
+
+	/* 0 : phase */
+	u8 flags;
+
+	u8 reserved1[3];
+
+	u32 timestamp_low;
+
+	u32 timestamp_high;
+};
+
+/* asynchronous event notification groups */
+enum ena_admin_aenq_group {
+	ENA_ADMIN_LINK_CHANGE		= 0,
+
+	ENA_ADMIN_FATAL_ERROR		= 1,
+
+	ENA_ADMIN_WARNING		= 2,
+
+	ENA_ADMIN_NOTIFICATION		= 3,
+
+	ENA_ADMIN_KEEP_ALIVE		= 4,
+
+	ENA_ADMIN_AENQ_GROUPS_NUM	= 5,
+};
+
+enum ena_admin_aenq_notification_syndrom {
+	ENA_ADMIN_SUSPEND	= 0,
+
+	ENA_ADMIN_RESUME	= 1,
+};
+
+struct ena_admin_aenq_entry {
+	struct ena_admin_aenq_common_desc aenq_common_desc;
+
+	/* command specific inline data */
+	u32 inline_data_w4[12];
+};
+
+struct ena_admin_aenq_link_change_desc {
+	struct ena_admin_aenq_common_desc aenq_common_desc;
+
+	/* 0 : link_status */
+	u32 flags;
+};
+
+struct ena_admin_ena_mmio_req_read_less_resp {
+	u16 req_id;
+
+	u16 reg_off;
+
+	/* value is valid when poll is cleared */
+	u32 reg_val;
+};
+
+/* aq_common_desc */
+#define ENA_ADMIN_AQ_COMMON_DESC_COMMAND_ID_MASK GENMASK(11, 0)
+#define ENA_ADMIN_AQ_COMMON_DESC_PHASE_MASK BIT(0)
+#define ENA_ADMIN_AQ_COMMON_DESC_CTRL_DATA_SHIFT 1
+#define ENA_ADMIN_AQ_COMMON_DESC_CTRL_DATA_MASK BIT(1)
+#define ENA_ADMIN_AQ_COMMON_DESC_CTRL_DATA_INDIRECT_SHIFT 2
+#define ENA_ADMIN_AQ_COMMON_DESC_CTRL_DATA_INDIRECT_MASK BIT(2)
+
+/* sq */
+#define ENA_ADMIN_SQ_SQ_DIRECTION_SHIFT 5
+#define ENA_ADMIN_SQ_SQ_DIRECTION_MASK GENMASK(7, 5)
+
+/* acq_common_desc */
+#define ENA_ADMIN_ACQ_COMMON_DESC_COMMAND_ID_MASK GENMASK(11, 0)
+#define ENA_ADMIN_ACQ_COMMON_DESC_PHASE_MASK BIT(0)
+
+/* aq_create_sq_cmd */
+#define ENA_ADMIN_AQ_CREATE_SQ_CMD_SQ_DIRECTION_SHIFT 5
+#define ENA_ADMIN_AQ_CREATE_SQ_CMD_SQ_DIRECTION_MASK GENMASK(7, 5)
+#define ENA_ADMIN_AQ_CREATE_SQ_CMD_PLACEMENT_POLICY_MASK GENMASK(3, 0)
+#define ENA_ADMIN_AQ_CREATE_SQ_CMD_COMPLETION_POLICY_SHIFT 4
+#define ENA_ADMIN_AQ_CREATE_SQ_CMD_COMPLETION_POLICY_MASK GENMASK(6, 4)
+#define ENA_ADMIN_AQ_CREATE_SQ_CMD_IS_PHYSICALLY_CONTIGUOUS_MASK BIT(0)
+
+/* aq_create_cq_cmd */
+#define ENA_ADMIN_AQ_CREATE_CQ_CMD_INTERRUPT_MODE_ENABLED_SHIFT 5
+#define ENA_ADMIN_AQ_CREATE_CQ_CMD_INTERRUPT_MODE_ENABLED_MASK BIT(5)
+#define ENA_ADMIN_AQ_CREATE_CQ_CMD_CQ_ENTRY_SIZE_WORDS_MASK GENMASK(4, 0)
+
+/* get_set_feature_common_desc */
+#define ENA_ADMIN_GET_SET_FEATURE_COMMON_DESC_SELECT_MASK GENMASK(1, 0)
+
+/* get_feature_link_desc */
+#define ENA_ADMIN_GET_FEATURE_LINK_DESC_AUTONEG_MASK BIT(0)
+#define ENA_ADMIN_GET_FEATURE_LINK_DESC_DUPLEX_SHIFT 1
+#define ENA_ADMIN_GET_FEATURE_LINK_DESC_DUPLEX_MASK BIT(1)
+
+/* feature_offload_desc */
+#define ENA_ADMIN_FEATURE_OFFLOAD_DESC_TX_L3_CSUM_IPV4_MASK BIT(0)
+#define ENA_ADMIN_FEATURE_OFFLOAD_DESC_TX_L4_IPV4_CSUM_PART_SHIFT 1
+#define ENA_ADMIN_FEATURE_OFFLOAD_DESC_TX_L4_IPV4_CSUM_PART_MASK BIT(1)
+#define ENA_ADMIN_FEATURE_OFFLOAD_DESC_TX_L4_IPV4_CSUM_FULL_SHIFT 2
+#define ENA_ADMIN_FEATURE_OFFLOAD_DESC_TX_L4_IPV4_CSUM_FULL_MASK BIT(2)
+#define ENA_ADMIN_FEATURE_OFFLOAD_DESC_TX_L4_IPV6_CSUM_PART_SHIFT 3
+#define ENA_ADMIN_FEATURE_OFFLOAD_DESC_TX_L4_IPV6_CSUM_PART_MASK BIT(3)
+#define ENA_ADMIN_FEATURE_OFFLOAD_DESC_TX_L4_IPV6_CSUM_FULL_SHIFT 4
+#define ENA_ADMIN_FEATURE_OFFLOAD_DESC_TX_L4_IPV6_CSUM_FULL_MASK BIT(4)
+#define ENA_ADMIN_FEATURE_OFFLOAD_DESC_TSO_IPV4_SHIFT 5
+#define ENA_ADMIN_FEATURE_OFFLOAD_DESC_TSO_IPV4_MASK BIT(5)
+#define ENA_ADMIN_FEATURE_OFFLOAD_DESC_TSO_IPV6_SHIFT 6
+#define ENA_ADMIN_FEATURE_OFFLOAD_DESC_TSO_IPV6_MASK BIT(6)
+#define ENA_ADMIN_FEATURE_OFFLOAD_DESC_TSO_ECN_SHIFT 7
+#define ENA_ADMIN_FEATURE_OFFLOAD_DESC_TSO_ECN_MASK BIT(7)
+#define ENA_ADMIN_FEATURE_OFFLOAD_DESC_RX_L3_CSUM_IPV4_MASK BIT(0)
+#define ENA_ADMIN_FEATURE_OFFLOAD_DESC_RX_L4_IPV4_CSUM_SHIFT 1
+#define ENA_ADMIN_FEATURE_OFFLOAD_DESC_RX_L4_IPV4_CSUM_MASK BIT(1)
+#define ENA_ADMIN_FEATURE_OFFLOAD_DESC_RX_L4_IPV6_CSUM_SHIFT 2
+#define ENA_ADMIN_FEATURE_OFFLOAD_DESC_RX_L4_IPV6_CSUM_MASK BIT(2)
+#define ENA_ADMIN_FEATURE_OFFLOAD_DESC_RX_HASH_SHIFT 3
+#define ENA_ADMIN_FEATURE_OFFLOAD_DESC_RX_HASH_MASK BIT(3)
+
+/* feature_rss_flow_hash_function */
+#define ENA_ADMIN_FEATURE_RSS_FLOW_HASH_FUNCTION_FUNCS_MASK GENMASK(7, 0)
+#define ENA_ADMIN_FEATURE_RSS_FLOW_HASH_FUNCTION_SELECTED_FUNC_MASK GENMASK(7, 0)
+
+/* feature_rss_flow_hash_input */
+#define ENA_ADMIN_FEATURE_RSS_FLOW_HASH_INPUT_L3_SORT_SHIFT 1
+#define ENA_ADMIN_FEATURE_RSS_FLOW_HASH_INPUT_L3_SORT_MASK BIT(1)
+#define ENA_ADMIN_FEATURE_RSS_FLOW_HASH_INPUT_L4_SORT_SHIFT 2
+#define ENA_ADMIN_FEATURE_RSS_FLOW_HASH_INPUT_L4_SORT_MASK BIT(2)
+#define ENA_ADMIN_FEATURE_RSS_FLOW_HASH_INPUT_ENABLE_L3_SORT_SHIFT 1
+#define ENA_ADMIN_FEATURE_RSS_FLOW_HASH_INPUT_ENABLE_L3_SORT_MASK BIT(1)
+#define ENA_ADMIN_FEATURE_RSS_FLOW_HASH_INPUT_ENABLE_L4_SORT_SHIFT 2
+#define ENA_ADMIN_FEATURE_RSS_FLOW_HASH_INPUT_ENABLE_L4_SORT_MASK BIT(2)
+
+/* host_info */
+#define ENA_ADMIN_HOST_INFO_MAJOR_MASK GENMASK(7, 0)
+#define ENA_ADMIN_HOST_INFO_MINOR_SHIFT 8
+#define ENA_ADMIN_HOST_INFO_MINOR_MASK GENMASK(15, 8)
+#define ENA_ADMIN_HOST_INFO_SUB_MINOR_SHIFT 16
+#define ENA_ADMIN_HOST_INFO_SUB_MINOR_MASK GENMASK(23, 16)
+
+/* aenq_common_desc */
+#define ENA_ADMIN_AENQ_COMMON_DESC_PHASE_MASK BIT(0)
+
+/* aenq_link_change_desc */
+#define ENA_ADMIN_AENQ_LINK_CHANGE_DESC_LINK_STATUS_MASK BIT(0)
+
+#endif /*_ENA_ADMIN_H_ */
diff --git a/drivers/net/ethernet/amazon/ena/ena_com.c b/drivers/net/ethernet/amazon/ena/ena_com.c
new file mode 100644
index 000000000000..3066d9c99984
--- /dev/null
+++ b/drivers/net/ethernet/amazon/ena/ena_com.c
@@ -0,0 +1,2666 @@
+/*
+ * Copyright 2015 Amazon.com, Inc. or its affiliates.
+ *
+ * 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
+ * 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 "ena_com.h"
+
+/*****************************************************************************/
+/*****************************************************************************/
+
+/* Timeout in micro-sec */
+#define ADMIN_CMD_TIMEOUT_US (1000000)
+
+#define ENA_ASYNC_QUEUE_DEPTH 4
+#define ENA_ADMIN_QUEUE_DEPTH 32
+
+#define MIN_ENA_VER (((ENA_COMMON_SPEC_VERSION_MAJOR) << \
+		ENA_REGS_VERSION_MAJOR_VERSION_SHIFT) \
+		| (ENA_COMMON_SPEC_VERSION_MINOR))
+
+#define ENA_CTRL_MAJOR		0
+#define ENA_CTRL_MINOR		0
+#define ENA_CTRL_SUB_MINOR	1
+
+#define MIN_ENA_CTRL_VER \
+	(((ENA_CTRL_MAJOR) << \
+	(ENA_REGS_CONTROLLER_VERSION_MAJOR_VERSION_SHIFT)) | \
+	((ENA_CTRL_MINOR) << \
+	(ENA_REGS_CONTROLLER_VERSION_MINOR_VERSION_SHIFT)) | \
+	(ENA_CTRL_SUB_MINOR))
+
+#define ENA_DMA_ADDR_TO_UINT32_LOW(x)	((u32)((u64)(x)))
+#define ENA_DMA_ADDR_TO_UINT32_HIGH(x)	((u32)(((u64)(x)) >> 32))
+
+#define ENA_MMIO_READ_TIMEOUT 0xFFFFFFFF
+
+/*****************************************************************************/
+/*****************************************************************************/
+/*****************************************************************************/
+
+enum ena_cmd_status {
+	ENA_CMD_SUBMITTED,
+	ENA_CMD_COMPLETED,
+	/* Abort - canceled by the driver */
+	ENA_CMD_ABORTED,
+};
+
+struct ena_comp_ctx {
+	struct completion wait_event;
+	struct ena_admin_acq_entry *user_cqe;
+	u32 comp_size;
+	enum ena_cmd_status status;
+	/* status from the device */
+	u8 comp_status;
+	u8 cmd_opcode;
+	bool occupied;
+};
+
+struct ena_com_stats_ctx {
+	struct ena_admin_aq_get_stats_cmd get_cmd;
+	struct ena_admin_acq_get_stats_resp get_resp;
+};
+
+static inline int ena_com_mem_addr_set(struct ena_com_dev *ena_dev,
+				       struct ena_common_mem_addr *ena_addr,
+				       dma_addr_t addr)
+{
+	if ((addr & GENMASK_ULL(ena_dev->dma_addr_bits - 1, 0)) != addr) {
+		pr_err("dma address has more bits that the device supports\n");
+		return -EINVAL;
+	}
+
+	ena_addr->mem_addr_low = (u32)addr;
+	ena_addr->mem_addr_high = (u64)addr >> 32;
+
+	return 0;
+}
+
+static int ena_com_admin_init_sq(struct ena_com_admin_queue *queue)
+{
+	struct ena_com_admin_sq *sq = &queue->sq;
+	u16 size = ADMIN_SQ_SIZE(queue->q_depth);
+
+	sq->entries = dma_zalloc_coherent(queue->q_dmadev, size, &sq->dma_addr,
+					  GFP_KERNEL);
+
+	if (!sq->entries) {
+		pr_err("memory allocation failed");
+		return -ENOMEM;
+	}
+
+	sq->head = 0;
+	sq->tail = 0;
+	sq->phase = 1;
+
+	sq->db_addr = NULL;
+
+	return 0;
+}
+
+static int ena_com_admin_init_cq(struct ena_com_admin_queue *queue)
+{
+	struct ena_com_admin_cq *cq = &queue->cq;
+	u16 size = ADMIN_CQ_SIZE(queue->q_depth);
+
+	cq->entries = dma_zalloc_coherent(queue->q_dmadev, size, &cq->dma_addr,
+					  GFP_KERNEL);
+
+	if (!cq->entries) {
+		pr_err("memory allocation failed");
+		return -ENOMEM;
+	}
+
+	cq->head = 0;
+	cq->phase = 1;
+
+	return 0;
+}
+
+static int ena_com_admin_init_aenq(struct ena_com_dev *dev,
+				   struct ena_aenq_handlers *aenq_handlers)
+{
+	struct ena_com_aenq *aenq = &dev->aenq;
+	u32 addr_low, addr_high, aenq_caps;
+	u16 size;
+
+	dev->aenq.q_depth = ENA_ASYNC_QUEUE_DEPTH;
+	size = ADMIN_AENQ_SIZE(ENA_ASYNC_QUEUE_DEPTH);
+	aenq->entries = dma_zalloc_coherent(dev->dmadev, size, &aenq->dma_addr,
+					    GFP_KERNEL);
+
+	if (!aenq->entries) {
+		pr_err("memory allocation failed");
+		return -ENOMEM;
+	}
+
+	aenq->head = aenq->q_depth;
+	aenq->phase = 1;
+
+	addr_low = ENA_DMA_ADDR_TO_UINT32_LOW(aenq->dma_addr);
+	addr_high = ENA_DMA_ADDR_TO_UINT32_HIGH(aenq->dma_addr);
+
+	writel(addr_low, dev->reg_bar + ENA_REGS_AENQ_BASE_LO_OFF);
+	writel(addr_high, dev->reg_bar + ENA_REGS_AENQ_BASE_HI_OFF);
+
+	aenq_caps = 0;
+	aenq_caps |= dev->aenq.q_depth & ENA_REGS_AENQ_CAPS_AENQ_DEPTH_MASK;
+	aenq_caps |= (sizeof(struct ena_admin_aenq_entry)
+		      << ENA_REGS_AENQ_CAPS_AENQ_ENTRY_SIZE_SHIFT) &
+		     ENA_REGS_AENQ_CAPS_AENQ_ENTRY_SIZE_MASK;
+	writel(aenq_caps, dev->reg_bar + ENA_REGS_AENQ_CAPS_OFF);
+
+	if (unlikely(!aenq_handlers)) {
+		pr_err("aenq handlers pointer is NULL\n");
+		return -EINVAL;
+	}
+
+	aenq->aenq_handlers = aenq_handlers;
+
+	return 0;
+}
+
+static inline void comp_ctxt_release(struct ena_com_admin_queue *queue,
+				     struct ena_comp_ctx *comp_ctx)
+{
+	comp_ctx->occupied = false;
+	atomic_dec(&queue->outstanding_cmds);
+}
+
+static struct ena_comp_ctx *get_comp_ctxt(struct ena_com_admin_queue *queue,
+					  u16 command_id, bool capture)
+{
+	if (unlikely(command_id >= queue->q_depth)) {
+		pr_err("command id is larger than the queue size. cmd_id: %u queue size %d\n",
+		       command_id, queue->q_depth);
+		return NULL;
+	}
+
+	if (unlikely(queue->comp_ctx[command_id].occupied && capture)) {
+		pr_err("Completion context is occupied\n");
+		return NULL;
+	}
+
+	if (capture) {
+		atomic_inc(&queue->outstanding_cmds);
+		queue->comp_ctx[command_id].occupied = true;
+	}
+
+	return &queue->comp_ctx[command_id];
+}
+
+static struct ena_comp_ctx *__ena_com_submit_admin_cmd(struct ena_com_admin_queue *admin_queue,
+						       struct ena_admin_aq_entry *cmd,
+						       size_t cmd_size_in_bytes,
+						       struct ena_admin_acq_entry *comp,
+						       size_t comp_size_in_bytes)
+{
+	struct ena_comp_ctx *comp_ctx;
+	u16 tail_masked, cmd_id;
+	u16 queue_size_mask;
+	u16 cnt;
+
+	queue_size_mask = admin_queue->q_depth - 1;
+
+	tail_masked = admin_queue->sq.tail & queue_size_mask;
+
+	/* In case of queue FULL */
+	cnt = admin_queue->sq.tail - admin_queue->sq.head;
+	if (cnt >= admin_queue->q_depth) {
+		pr_debug("admin queue is FULL (tail %d head %d depth: %d)\n",
+			 admin_queue->sq.tail, admin_queue->sq.head,
+			 admin_queue->q_depth);
+		admin_queue->stats.out_of_space++;
+		return ERR_PTR(-ENOSPC);
+	}
+
+	cmd_id = admin_queue->curr_cmd_id;
+
+	cmd->aq_common_descriptor.flags |= admin_queue->sq.phase &
+		ENA_ADMIN_AQ_COMMON_DESC_PHASE_MASK;
+
+	cmd->aq_common_descriptor.command_id |= cmd_id &
+		ENA_ADMIN_AQ_COMMON_DESC_COMMAND_ID_MASK;
+
+	comp_ctx = get_comp_ctxt(admin_queue, cmd_id, true);
+	if (unlikely(!comp_ctx))
+		return ERR_PTR(-EINVAL);
+
+	comp_ctx->status = ENA_CMD_SUBMITTED;
+	comp_ctx->comp_size = (u32)comp_size_in_bytes;
+	comp_ctx->user_cqe = comp;
+	comp_ctx->cmd_opcode = cmd->aq_common_descriptor.opcode;
+
+	reinit_completion(&comp_ctx->wait_event);
+
+	memcpy(&admin_queue->sq.entries[tail_masked], cmd, cmd_size_in_bytes);
+
+	admin_queue->curr_cmd_id = (admin_queue->curr_cmd_id + 1) &
+		queue_size_mask;
+
+	admin_queue->sq.tail++;
+	admin_queue->stats.submitted_cmd++;
+
+	if (unlikely((admin_queue->sq.tail & queue_size_mask) == 0))
+		admin_queue->sq.phase = !admin_queue->sq.phase;
+
+	writel(admin_queue->sq.tail, admin_queue->sq.db_addr);
+
+	return comp_ctx;
+}
+
+static inline int ena_com_init_comp_ctxt(struct ena_com_admin_queue *queue)
+{
+	size_t size = queue->q_depth * sizeof(struct ena_comp_ctx);
+	struct ena_comp_ctx *comp_ctx;
+	u16 i;
+
+	queue->comp_ctx = devm_kzalloc(queue->q_dmadev, size, GFP_KERNEL);
+	if (unlikely(!queue->comp_ctx)) {
+		pr_err("memory allocation failed");
+		return -ENOMEM;
+	}
+
+	for (i = 0; i < queue->q_depth; i++) {
+		comp_ctx = get_comp_ctxt(queue, i, false);
+		if (comp_ctx)
+			init_completion(&comp_ctx->wait_event);
+	}
+
+	return 0;
+}
+
+static struct ena_comp_ctx *ena_com_submit_admin_cmd(struct ena_com_admin_queue *admin_queue,
+						     struct ena_admin_aq_entry *cmd,
+						     size_t cmd_size_in_bytes,
+						     struct ena_admin_acq_entry *comp,
+						     size_t comp_size_in_bytes)
+{
+	unsigned long flags;
+	struct ena_comp_ctx *comp_ctx;
+
+	spin_lock_irqsave(&admin_queue->q_lock, flags);
+	if (unlikely(!admin_queue->running_state)) {
+		spin_unlock_irqrestore(&admin_queue->q_lock, flags);
+		return ERR_PTR(-ENODEV);
+	}
+	comp_ctx = __ena_com_submit_admin_cmd(admin_queue, cmd,
+					      cmd_size_in_bytes,
+					      comp,
+					      comp_size_in_bytes);
+	if (unlikely(IS_ERR(comp_ctx)))
+		admin_queue->running_state = false;
+	spin_unlock_irqrestore(&admin_queue->q_lock, flags);
+
+	return comp_ctx;
+}
+
+static int ena_com_init_io_sq(struct ena_com_dev *ena_dev,
+			      struct ena_com_create_io_ctx *ctx,
+			      struct ena_com_io_sq *io_sq)
+{
+	size_t size;
+	int dev_node = 0;
+
+	memset(&io_sq->desc_addr, 0x0, sizeof(struct ena_com_io_desc_addr));
+
+	io_sq->desc_entry_size =
+		(io_sq->direction == ENA_COM_IO_QUEUE_DIRECTION_TX) ?
+		sizeof(struct ena_eth_io_tx_desc) :
+		sizeof(struct ena_eth_io_rx_desc);
+
+	size = io_sq->desc_entry_size * io_sq->q_depth;
+
+	if (io_sq->mem_queue_type == ENA_ADMIN_PLACEMENT_POLICY_HOST) {
+		dev_node = dev_to_node(ena_dev->dmadev);
+		set_dev_node(ena_dev->dmadev, ctx->numa_node);
+		io_sq->desc_addr.virt_addr =
+			dma_zalloc_coherent(ena_dev->dmadev, size,
+					    &io_sq->desc_addr.phys_addr,
+					    GFP_KERNEL);
+		set_dev_node(ena_dev->dmadev, dev_node);
+		if (!io_sq->desc_addr.virt_addr) {
+			io_sq->desc_addr.virt_addr =
+				dma_zalloc_coherent(ena_dev->dmadev, size,
+						    &io_sq->desc_addr.phys_addr,
+						    GFP_KERNEL);
+		}
+	} else {
+		dev_node = dev_to_node(ena_dev->dmadev);
+		set_dev_node(ena_dev->dmadev, ctx->numa_node);
+		io_sq->desc_addr.virt_addr =
+			devm_kzalloc(ena_dev->dmadev, size, GFP_KERNEL);
+		set_dev_node(ena_dev->dmadev, dev_node);
+		if (!io_sq->desc_addr.virt_addr) {
+			io_sq->desc_addr.virt_addr =
+				devm_kzalloc(ena_dev->dmadev, size, GFP_KERNEL);
+		}
+	}
+
+	if (!io_sq->desc_addr.virt_addr) {
+		pr_err("memory allocation failed");
+		return -ENOMEM;
+	}
+
+	io_sq->tail = 0;
+	io_sq->next_to_comp = 0;
+	io_sq->phase = 1;
+
+	return 0;
+}
+
+static int ena_com_init_io_cq(struct ena_com_dev *ena_dev,
+			      struct ena_com_create_io_ctx *ctx,
+			      struct ena_com_io_cq *io_cq)
+{
+	size_t size;
+	int prev_node = 0;
+
+	memset(&io_cq->cdesc_addr, 0x0, sizeof(struct ena_com_io_desc_addr));
+
+	/* Use the basic completion descriptor for Rx */
+	io_cq->cdesc_entry_size_in_bytes =
+		(io_cq->direction == ENA_COM_IO_QUEUE_DIRECTION_TX) ?
+		sizeof(struct ena_eth_io_tx_cdesc) :
+		sizeof(struct ena_eth_io_rx_cdesc_base);
+
+	size = io_cq->cdesc_entry_size_in_bytes * io_cq->q_depth;
+
+	prev_node = dev_to_node(ena_dev->dmadev);
+	set_dev_node(ena_dev->dmadev, ctx->numa_node);
+	io_cq->cdesc_addr.virt_addr =
+		dma_zalloc_coherent(ena_dev->dmadev, size,
+				    &io_cq->cdesc_addr.phys_addr, GFP_KERNEL);
+	set_dev_node(ena_dev->dmadev, prev_node);
+	if (!io_cq->cdesc_addr.virt_addr) {
+		io_cq->cdesc_addr.virt_addr =
+			dma_zalloc_coherent(ena_dev->dmadev, size,
+					    &io_cq->cdesc_addr.phys_addr,
+					    GFP_KERNEL);
+	}
+
+	if (!io_cq->cdesc_addr.virt_addr) {
+		pr_err("memory allocation failed");
+		return -ENOMEM;
+	}
+
+	io_cq->phase = 1;
+	io_cq->head = 0;
+
+	return 0;
+}
+
+static void ena_com_handle_single_admin_completion(struct ena_com_admin_queue *admin_queue,
+						   struct ena_admin_acq_entry *cqe)
+{
+	struct ena_comp_ctx *comp_ctx;
+	u16 cmd_id;
+
+	cmd_id = cqe->acq_common_descriptor.command &
+		ENA_ADMIN_ACQ_COMMON_DESC_COMMAND_ID_MASK;
+
+	comp_ctx = get_comp_ctxt(admin_queue, cmd_id, false);
+	if (unlikely(!comp_ctx)) {
+		pr_err("comp_ctx is NULL. Changing the admin queue running state\n");
+		admin_queue->running_state = false;
+		return;
+	}
+
+	comp_ctx->status = ENA_CMD_COMPLETED;
+	comp_ctx->comp_status = cqe->acq_common_descriptor.status;
+
+	if (comp_ctx->user_cqe)
+		memcpy(comp_ctx->user_cqe, (void *)cqe, comp_ctx->comp_size);
+
+	if (!admin_queue->polling)
+		complete(&comp_ctx->wait_event);
+}
+
+static void ena_com_handle_admin_completion(struct ena_com_admin_queue *admin_queue)
+{
+	struct ena_admin_acq_entry *cqe = NULL;
+	u16 comp_num = 0;
+	u16 head_masked;
+	u8 phase;
+
+	head_masked = admin_queue->cq.head & (admin_queue->q_depth - 1);
+	phase = admin_queue->cq.phase;
+
+	cqe = &admin_queue->cq.entries[head_masked];
+
+	/* Go over all the completions */
+	while ((cqe->acq_common_descriptor.flags &
+			ENA_ADMIN_ACQ_COMMON_DESC_PHASE_MASK) == phase) {
+		/* Do not read the rest of the completion entry before the
+		 * phase bit was validated
+		 */
+		rmb();
+		ena_com_handle_single_admin_completion(admin_queue, cqe);
+
+		head_masked++;
+		comp_num++;
+		if (unlikely(head_masked == admin_queue->q_depth)) {
+			head_masked = 0;
+			phase = !phase;
+		}
+
+		cqe = &admin_queue->cq.entries[head_masked];
+	}
+
+	admin_queue->cq.head += comp_num;
+	admin_queue->cq.phase = phase;
+	admin_queue->sq.head += comp_num;
+	admin_queue->stats.completed_cmd += comp_num;
+}
+
+static int ena_com_comp_status_to_errno(u8 comp_status)
+{
+	if (unlikely(comp_status != 0))
+		pr_err("admin command failed[%u]\n", comp_status);
+
+	if (unlikely(comp_status > ENA_ADMIN_UNKNOWN_ERROR))
+		return -EINVAL;
+
+	switch (comp_status) {
+	case ENA_ADMIN_SUCCESS:
+		return 0;
+	case ENA_ADMIN_RESOURCE_ALLOCATION_FAILURE:
+		return -ENOMEM;
+	case ENA_ADMIN_UNSUPPORTED_OPCODE:
+		return -EPERM;
+	case ENA_ADMIN_BAD_OPCODE:
+	case ENA_ADMIN_MALFORMED_REQUEST:
+	case ENA_ADMIN_ILLEGAL_PARAMETER:
+	case ENA_ADMIN_UNKNOWN_ERROR:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int ena_com_wait_and_process_admin_cq_polling(struct ena_comp_ctx *comp_ctx,
+						     struct ena_com_admin_queue *admin_queue)
+{
+	unsigned long flags;
+	u32 start_time;
+	int ret;
+
+	start_time = ((u32)jiffies_to_usecs(jiffies));
+
+	while (comp_ctx->status == ENA_CMD_SUBMITTED) {
+		if ((((u32)jiffies_to_usecs(jiffies)) - start_time) >
+		    ADMIN_CMD_TIMEOUT_US) {
+			pr_err("Wait for completion (polling) timeout\n");
+			/* ENA didn't have any completion */
+			spin_lock_irqsave(&admin_queue->q_lock, flags);
+			admin_queue->stats.no_completion++;
+			admin_queue->running_state = false;
+			spin_unlock_irqrestore(&admin_queue->q_lock, flags);
+
+			ret = -ETIME;
+			goto err;
+		}
+
+		spin_lock_irqsave(&admin_queue->q_lock, flags);
+		ena_com_handle_admin_completion(admin_queue);
+		spin_unlock_irqrestore(&admin_queue->q_lock, flags);
+
+		msleep(100);
+	}
+
+	if (unlikely(comp_ctx->status == ENA_CMD_ABORTED)) {
+		pr_err("Command was aborted\n");
+		spin_lock_irqsave(&admin_queue->q_lock, flags);
+		admin_queue->stats.aborted_cmd++;
+		spin_unlock_irqrestore(&admin_queue->q_lock, flags);
+		ret = -ENODEV;
+		goto err;
+	}
+
+	WARN(comp_ctx->status != ENA_CMD_COMPLETED, "Invalid comp status %d\n",
+	     comp_ctx->status);
+
+	ret = ena_com_comp_status_to_errno(comp_ctx->comp_status);
+err:
+	comp_ctxt_release(admin_queue, comp_ctx);
+	return ret;
+}
+
+static int ena_com_wait_and_process_admin_cq_interrupts(struct ena_comp_ctx *comp_ctx,
+							struct ena_com_admin_queue *admin_queue)
+{
+	unsigned long flags;
+	int ret;
+
+	wait_for_completion_timeout(&comp_ctx->wait_event,
+				    usecs_to_jiffies(ADMIN_CMD_TIMEOUT_US));
+
+	/* In case the command wasn't completed find out the root cause.
+	 * There might be 2 kinds of errors
+	 * 1) No completion (timeout reached)
+	 * 2) There is completion but the device didn't get any msi-x interrupt.
+	 */
+	if (unlikely(comp_ctx->status == ENA_CMD_SUBMITTED)) {
+		spin_lock_irqsave(&admin_queue->q_lock, flags);
+		ena_com_handle_admin_completion(admin_queue);
+		admin_queue->stats.no_completion++;
+		spin_unlock_irqrestore(&admin_queue->q_lock, flags);
+
+		if (comp_ctx->status == ENA_CMD_COMPLETED)
+			pr_err("The ena device have completion but the driver didn't receive any MSI-X interrupt (cmd %d)\n",
+			       comp_ctx->cmd_opcode);
+		else
+			pr_err("The ena device doesn't send any completion for the admin cmd %d status %d\n",
+			       comp_ctx->cmd_opcode, comp_ctx->status);
+
+		admin_queue->running_state = false;
+		ret = -ETIME;
+		goto err;
+	}
+
+	ret = ena_com_comp_status_to_errno(comp_ctx->comp_status);
+err:
+	comp_ctxt_release(admin_queue, comp_ctx);
+	return ret;
+}
+
+/* This method read the hardware device register through posting writes
+ * and waiting for response
+ * On timeout the function will return ENA_MMIO_READ_TIMEOUT
+ */
+static u32 ena_com_reg_bar_read32(struct ena_com_dev *ena_dev, u16 offset)
+{
+	struct ena_com_mmio_read *mmio_read = &ena_dev->mmio_read;
+	volatile struct ena_admin_ena_mmio_req_read_less_resp *read_resp =
+		mmio_read->read_resp;
+	u32 mmio_read_reg, ret;
+	unsigned long flags;
+	int i;
+
+	might_sleep();
+
+	/* If readless is disabled, perform regular read */
+	if (!mmio_read->readless_supported)
+		return readl(ena_dev->reg_bar + offset);
+
+	spin_lock_irqsave(&mmio_read->lock, flags);
+	mmio_read->seq_num++;
+
+	read_resp->req_id = mmio_read->seq_num + 0xDEAD;
+	mmio_read_reg = (offset << ENA_REGS_MMIO_REG_READ_REG_OFF_SHIFT) &
+			ENA_REGS_MMIO_REG_READ_REG_OFF_MASK;
+	mmio_read_reg |= mmio_read->seq_num &
+			ENA_REGS_MMIO_REG_READ_REQ_ID_MASK;
+
+	/* make sure read_resp->req_id get updated before the hw can write
+	 * there
+	 */
+	wmb();
+
+	writel(mmio_read_reg, ena_dev->reg_bar + ENA_REGS_MMIO_REG_READ_OFF);
+
+	for (i = 0; i < ENA_REG_READ_TIMEOUT; i++) {
+		if (read_resp->req_id == mmio_read->seq_num)
+			break;
+
+		udelay(1);
+	}
+
+	if (unlikely(i == ENA_REG_READ_TIMEOUT)) {
+		pr_err("reading reg failed for timeout. expected: req id[%hu] offset[%hu] actual: req id[%hu] offset[%hu]\n",
+		       mmio_read->seq_num, offset, read_resp->req_id,
+		       read_resp->reg_off);
+		ret = ENA_MMIO_READ_TIMEOUT;
+		goto err;
+	}
+
+	if (read_resp->reg_off != offset) {
+		pr_err("Read failure: wrong offset provided");
+		ret = ENA_MMIO_READ_TIMEOUT;
+	} else {
+		ret = read_resp->reg_val;
+	}
+err:
+	spin_unlock_irqrestore(&mmio_read->lock, flags);
+
+	return ret;
+}
+
+/* There are two types to wait for completion.
+ * Polling mode - wait until the completion is available.
+ * Async mode - wait on wait queue until the completion is ready
+ * (or the timeout expired).
+ * It is expected that the IRQ called ena_com_handle_admin_completion
+ * to mark the completions.
+ */
+static int ena_com_wait_and_process_admin_cq(struct ena_comp_ctx *comp_ctx,
+					     struct ena_com_admin_queue *admin_queue)
+{
+	if (admin_queue->polling)
+		return ena_com_wait_and_process_admin_cq_polling(comp_ctx,
+								 admin_queue);
+
+	return ena_com_wait_and_process_admin_cq_interrupts(comp_ctx,
+							    admin_queue);
+}
+
+static int ena_com_destroy_io_sq(struct ena_com_dev *ena_dev,
+				 struct ena_com_io_sq *io_sq)
+{
+	struct ena_com_admin_queue *admin_queue = &ena_dev->admin_queue;
+	struct ena_admin_aq_destroy_sq_cmd destroy_cmd;
+	struct ena_admin_acq_destroy_sq_resp_desc destroy_resp;
+	u8 direction;
+	int ret;
+
+	memset(&destroy_cmd, 0x0, sizeof(struct ena_admin_aq_destroy_sq_cmd));
+
+	if (io_sq->direction == ENA_COM_IO_QUEUE_DIRECTION_TX)
+		direction = ENA_ADMIN_SQ_DIRECTION_TX;
+	else
+		direction = ENA_ADMIN_SQ_DIRECTION_RX;
+
+	destroy_cmd.sq.sq_identity |= (direction <<
+		ENA_ADMIN_SQ_SQ_DIRECTION_SHIFT) &
+		ENA_ADMIN_SQ_SQ_DIRECTION_MASK;
+
+	destroy_cmd.sq.sq_idx = io_sq->idx;
+	destroy_cmd.aq_common_descriptor.opcode = ENA_ADMIN_DESTROY_SQ;
+
+	ret = ena_com_execute_admin_command(admin_queue,
+					    (struct ena_admin_aq_entry *)&destroy_cmd,
+					    sizeof(destroy_cmd),
+					    (struct ena_admin_acq_entry *)&destroy_resp,
+					    sizeof(destroy_resp));
+
+	if (unlikely(ret && (ret != -ENODEV)))
+		pr_err("failed to destroy io sq error: %d\n", ret);
+
+	return ret;
+}
+
+static void ena_com_io_queue_free(struct ena_com_dev *ena_dev,
+				  struct ena_com_io_sq *io_sq,
+				  struct ena_com_io_cq *io_cq)
+{
+	size_t size;
+
+	if (io_cq->cdesc_addr.virt_addr) {
+		size = io_cq->cdesc_entry_size_in_bytes * io_cq->q_depth;
+
+		dma_free_coherent(ena_dev->dmadev, size,
+				  io_cq->cdesc_addr.virt_addr,
+				  io_cq->cdesc_addr.phys_addr);
+
+		io_cq->cdesc_addr.virt_addr = NULL;
+	}
+
+	if (io_sq->desc_addr.virt_addr) {
+		size = io_sq->desc_entry_size * io_sq->q_depth;
+
+		if (io_sq->mem_queue_type == ENA_ADMIN_PLACEMENT_POLICY_HOST)
+			dma_free_coherent(ena_dev->dmadev, size,
+					  io_sq->desc_addr.virt_addr,
+					  io_sq->desc_addr.phys_addr);
+		else
+			devm_kfree(ena_dev->dmadev, io_sq->desc_addr.virt_addr);
+
+		io_sq->desc_addr.virt_addr = NULL;
+	}
+}
+
+static int wait_for_reset_state(struct ena_com_dev *ena_dev, u32 timeout,
+				u16 exp_state)
+{
+	u32 val, i;
+
+	for (i = 0; i < timeout; i++) {
+		val = ena_com_reg_bar_read32(ena_dev, ENA_REGS_DEV_STS_OFF);
+
+		if (unlikely(val == ENA_MMIO_READ_TIMEOUT)) {
+			pr_err("Reg read timeout occurred\n");
+			return -ETIME;
+		}
+
+		if ((val & ENA_REGS_DEV_STS_RESET_IN_PROGRESS_MASK) ==
+			exp_state)
+			return 0;
+
+		/* The resolution of the timeout is 100ms */
+		msleep(100);
+	}
+
+	return -ETIME;
+}
+
+static bool ena_com_check_supported_feature_id(struct ena_com_dev *ena_dev,
+					       enum ena_admin_aq_feature_id feature_id)
+{
+	u32 feature_mask = 1 << feature_id;
+
+	/* Device attributes is always supported */
+	if ((feature_id != ENA_ADMIN_DEVICE_ATTRIBUTES) &&
+	    !(ena_dev->supported_features & feature_mask))
+		return false;
+
+	return true;
+}
+
+static int ena_com_get_feature_ex(struct ena_com_dev *ena_dev,
+				  struct ena_admin_get_feat_resp *get_resp,
+				  enum ena_admin_aq_feature_id feature_id,
+				  dma_addr_t control_buf_dma_addr,
+				  u32 control_buff_size)
+{
+	struct ena_com_admin_queue *admin_queue;
+	struct ena_admin_get_feat_cmd get_cmd;
+	int ret;
+
+	if (!ena_com_check_supported_feature_id(ena_dev, feature_id)) {
+		pr_info("Feature %d isn't supported\n", feature_id);
+		return -EPERM;
+	}
+
+	memset(&get_cmd, 0x0, sizeof(get_cmd));
+	admin_queue = &ena_dev->admin_queue;
+
+	get_cmd.aq_common_descriptor.opcode = ENA_ADMIN_GET_FEATURE;
+
+	if (control_buff_size)
+		get_cmd.aq_common_descriptor.flags =
+			ENA_ADMIN_AQ_COMMON_DESC_CTRL_DATA_INDIRECT_MASK;
+	else
+		get_cmd.aq_common_descriptor.flags = 0;
+
+	ret = ena_com_mem_addr_set(ena_dev,
+				   &get_cmd.control_buffer.address,
+				   control_buf_dma_addr);
+	if (unlikely(ret)) {
+		pr_err("memory address set failed\n");
+		return ret;
+	}
+
+	get_cmd.control_buffer.length = control_buff_size;
+
+	get_cmd.feat_common.feature_id = feature_id;
+
+	ret = ena_com_execute_admin_command(admin_queue,
+					    (struct ena_admin_aq_entry *)
+					    &get_cmd,
+					    sizeof(get_cmd),
+					    (struct ena_admin_acq_entry *)
+					    get_resp,
+					    sizeof(*get_resp));
+
+	if (unlikely(ret))
+		pr_err("Failed to submit get_feature command %d error: %d\n",
+		       feature_id, ret);
+
+	return ret;
+}
+
+static int ena_com_get_feature(struct ena_com_dev *ena_dev,
+			       struct ena_admin_get_feat_resp *get_resp,
+			       enum ena_admin_aq_feature_id feature_id)
+{
+	return ena_com_get_feature_ex(ena_dev,
+				      get_resp,
+				      feature_id,
+				      0,
+				      0);
+}
+
+static int ena_com_hash_key_allocate(struct ena_com_dev *ena_dev)
+{
+	struct ena_rss *rss = &ena_dev->rss;
+
+	rss->hash_key =
+		dma_zalloc_coherent(ena_dev->dmadev, sizeof(*rss->hash_key),
+				    &rss->hash_key_dma_addr, GFP_KERNEL);
+
+	if (unlikely(!rss->hash_key))
+		return -ENOMEM;
+
+	return 0;
+}
+
+static void ena_com_hash_key_destroy(struct ena_com_dev *ena_dev)
+{
+	struct ena_rss *rss = &ena_dev->rss;
+
+	if (rss->hash_key)
+		dma_free_coherent(ena_dev->dmadev, sizeof(*rss->hash_key),
+				  rss->hash_key, rss->hash_key_dma_addr);
+	rss->hash_key = NULL;
+}
+
+static int ena_com_hash_ctrl_init(struct ena_com_dev *ena_dev)
+{
+	struct ena_rss *rss = &ena_dev->rss;
+
+	rss->hash_ctrl =
+		dma_zalloc_coherent(ena_dev->dmadev, sizeof(*rss->hash_ctrl),
+				    &rss->hash_ctrl_dma_addr, GFP_KERNEL);
+
+	if (unlikely(!rss->hash_ctrl))
+		return -ENOMEM;
+
+	return 0;
+}
+
+static void ena_com_hash_ctrl_destroy(struct ena_com_dev *ena_dev)
+{
+	struct ena_rss *rss = &ena_dev->rss;
+
+	if (rss->hash_ctrl)
+		dma_free_coherent(ena_dev->dmadev, sizeof(*rss->hash_ctrl),
+				  rss->hash_ctrl, rss->hash_ctrl_dma_addr);
+	rss->hash_ctrl = NULL;
+}
+
+static int ena_com_indirect_table_allocate(struct ena_com_dev *ena_dev,
+					   u16 log_size)
+{
+	struct ena_rss *rss = &ena_dev->rss;
+	struct ena_admin_get_feat_resp get_resp;
+	size_t tbl_size;
+	int ret;
+
+	ret = ena_com_get_feature(ena_dev, &get_resp,
+				  ENA_ADMIN_RSS_REDIRECTION_TABLE_CONFIG);
+	if (unlikely(ret))
+		return ret;
+
+	if ((get_resp.u.ind_table.min_size > log_size) ||
+	    (get_resp.u.ind_table.max_size < log_size)) {
+		pr_err("indirect table size doesn't fit. requested size: %d while min is:%d and max %d\n",
+		       1 << log_size, 1 << get_resp.u.ind_table.min_size,
+		       1 << get_resp.u.ind_table.max_size);
+		return -EINVAL;
+	}
+
+	tbl_size = (1ULL << log_size) *
+		sizeof(struct ena_admin_rss_ind_table_entry);
+
+	rss->rss_ind_tbl =
+		dma_zalloc_coherent(ena_dev->dmadev, tbl_size,
+				    &rss->rss_ind_tbl_dma_addr, GFP_KERNEL);
+	if (unlikely(!rss->rss_ind_tbl))
+		goto mem_err1;
+
+	tbl_size = (1ULL << log_size) * sizeof(u16);
+	rss->host_rss_ind_tbl =
+		devm_kzalloc(ena_dev->dmadev, tbl_size, GFP_KERNEL);
+	if (unlikely(!rss->host_rss_ind_tbl))
+		goto mem_err2;
+
+	rss->tbl_log_size = log_size;
+
+	return 0;
+
+mem_err2:
+	tbl_size = (1ULL << log_size) *
+		sizeof(struct ena_admin_rss_ind_table_entry);
+
+	dma_free_coherent(ena_dev->dmadev, tbl_size, rss->rss_ind_tbl,
+			  rss->rss_ind_tbl_dma_addr);
+	rss->rss_ind_tbl = NULL;
+mem_err1:
+	rss->tbl_log_size = 0;
+	return -ENOMEM;
+}
+
+static void ena_com_indirect_table_destroy(struct ena_com_dev *ena_dev)
+{
+	struct ena_rss *rss = &ena_dev->rss;
+	size_t tbl_size = (1ULL << rss->tbl_log_size) *
+		sizeof(struct ena_admin_rss_ind_table_entry);
+
+	if (rss->rss_ind_tbl)
+		dma_free_coherent(ena_dev->dmadev, tbl_size, rss->rss_ind_tbl,
+				  rss->rss_ind_tbl_dma_addr);
+	rss->rss_ind_tbl = NULL;
+
+	if (rss->host_rss_ind_tbl)
+		devm_kfree(ena_dev->dmadev, rss->host_rss_ind_tbl);
+	rss->host_rss_ind_tbl = NULL;
+}
+
+static int ena_com_create_io_sq(struct ena_com_dev *ena_dev,
+				struct ena_com_io_sq *io_sq, u16 cq_idx)
+{
+	struct ena_com_admin_queue *admin_queue = &ena_dev->admin_queue;
+	struct ena_admin_aq_create_sq_cmd create_cmd;
+	struct ena_admin_acq_create_sq_resp_desc cmd_completion;
+	u8 direction;
+	int ret;
+
+	memset(&create_cmd, 0x0, sizeof(struct ena_admin_aq_create_sq_cmd));
+
+	create_cmd.aq_common_descriptor.opcode = ENA_ADMIN_CREATE_SQ;
+
+	if (io_sq->direction == ENA_COM_IO_QUEUE_DIRECTION_TX)
+		direction = ENA_ADMIN_SQ_DIRECTION_TX;
+	else
+		direction = ENA_ADMIN_SQ_DIRECTION_RX;
+
+	create_cmd.sq_identity |= (direction <<
+		ENA_ADMIN_AQ_CREATE_SQ_CMD_SQ_DIRECTION_SHIFT) &
+		ENA_ADMIN_AQ_CREATE_SQ_CMD_SQ_DIRECTION_MASK;
+
+	create_cmd.sq_caps_2 |= io_sq->mem_queue_type &
+		ENA_ADMIN_AQ_CREATE_SQ_CMD_PLACEMENT_POLICY_MASK;
+
+	create_cmd.sq_caps_2 |= (ENA_ADMIN_COMPLETION_POLICY_DESC <<
+		ENA_ADMIN_AQ_CREATE_SQ_CMD_COMPLETION_POLICY_SHIFT) &
+		ENA_ADMIN_AQ_CREATE_SQ_CMD_COMPLETION_POLICY_MASK;
+
+	create_cmd.sq_caps_3 |=
+		ENA_ADMIN_AQ_CREATE_SQ_CMD_IS_PHYSICALLY_CONTIGUOUS_MASK;
+
+	create_cmd.cq_idx = cq_idx;
+	create_cmd.sq_depth = io_sq->q_depth;
+
+	if (io_sq->mem_queue_type == ENA_ADMIN_PLACEMENT_POLICY_HOST) {
+		ret = ena_com_mem_addr_set(ena_dev,
+					   &create_cmd.sq_ba,
+					   io_sq->desc_addr.phys_addr);
+		if (unlikely(ret)) {
+			pr_err("memory address set failed\n");
+			return ret;
+		}
+	}
+
+	ret = ena_com_execute_admin_command(admin_queue,
+					    (struct ena_admin_aq_entry *)&create_cmd,
+					    sizeof(create_cmd),
+					    (struct ena_admin_acq_entry *)&cmd_completion,
+					    sizeof(cmd_completion));
+	if (unlikely(ret)) {
+		pr_err("Failed to create IO SQ. error: %d\n", ret);
+		return ret;
+	}
+
+	io_sq->idx = cmd_completion.sq_idx;
+
+	io_sq->db_addr = (u32 __iomem *)((uintptr_t)ena_dev->reg_bar +
+		(uintptr_t)cmd_completion.sq_doorbell_offset);
+
+	if (io_sq->mem_queue_type == ENA_ADMIN_PLACEMENT_POLICY_DEV) {
+		io_sq->header_addr = (u8 __iomem *)((uintptr_t)ena_dev->mem_bar
+				+ cmd_completion.llq_headers_offset);
+
+		io_sq->desc_addr.pbuf_dev_addr =
+			(u8 __iomem *)((uintptr_t)ena_dev->mem_bar +
+			cmd_completion.llq_descriptors_offset);
+	}
+
+	pr_debug("created sq[%u], depth[%u]\n", io_sq->idx, io_sq->q_depth);
+
+	return ret;
+}
+
+static int ena_com_ind_tbl_convert_to_device(struct ena_com_dev *ena_dev)
+{
+	struct ena_rss *rss = &ena_dev->rss;
+	struct ena_com_io_sq *io_sq;
+	u16 qid;
+	int i;
+
+	for (i = 0; i < 1 << rss->tbl_log_size; i++) {
+		qid = rss->host_rss_ind_tbl[i];
+		if (qid >= ENA_TOTAL_NUM_QUEUES)
+			return -EINVAL;
+
+		io_sq = &ena_dev->io_sq_queues[qid];
+
+		if (io_sq->direction != ENA_COM_IO_QUEUE_DIRECTION_RX)
+			return -EINVAL;
+
+		rss->rss_ind_tbl[i].cq_idx = io_sq->idx;
+	}
+
+	return 0;
+}
+
+static int ena_com_ind_tbl_convert_from_device(struct ena_com_dev *ena_dev)
+{
+	u16 dev_idx_to_host_tbl[ENA_TOTAL_NUM_QUEUES] = { (u16)-1 };
+	struct ena_rss *rss = &ena_dev->rss;
+	u8 idx;
+	u16 i;
+
+	for (i = 0; i < ENA_TOTAL_NUM_QUEUES; i++)
+		dev_idx_to_host_tbl[ena_dev->io_sq_queues[i].idx] = i;
+
+	for (i = 0; i < 1 << rss->tbl_log_size; i++) {
+		if (rss->rss_ind_tbl[i].cq_idx > ENA_TOTAL_NUM_QUEUES)
+			return -EINVAL;
+		idx = (u8)rss->rss_ind_tbl[i].cq_idx;
+
+		if (dev_idx_to_host_tbl[idx] > ENA_TOTAL_NUM_QUEUES)
+			return -EINVAL;
+
+		rss->host_rss_ind_tbl[i] = dev_idx_to_host_tbl[idx];
+	}
+
+	return 0;
+}
+
+static int ena_com_init_interrupt_moderation_table(struct ena_com_dev *ena_dev)
+{
+	size_t size;
+
+	size = sizeof(struct ena_intr_moder_entry) * ENA_INTR_MAX_NUM_OF_LEVELS;
+
+	ena_dev->intr_moder_tbl =
+		devm_kzalloc(ena_dev->dmadev, size, GFP_KERNEL);
+	if (!ena_dev->intr_moder_tbl)
+		return -ENOMEM;
+
+	ena_com_config_default_interrupt_moderation_table(ena_dev);
+
+	return 0;
+}
+
+static void ena_com_update_intr_delay_resolution(struct ena_com_dev *ena_dev,
+						 u16 intr_delay_resolution)
+{
+	struct ena_intr_moder_entry *intr_moder_tbl = ena_dev->intr_moder_tbl;
+	unsigned int i;
+
+	if (!intr_delay_resolution) {
+		pr_err("Illegal intr_delay_resolution provided. Going to use default 1 usec resolution\n");
+		intr_delay_resolution = 1;
+	}
+	ena_dev->intr_delay_resolution = intr_delay_resolution;
+
+	/* update Rx */
+	for (i = 0; i < ENA_INTR_MAX_NUM_OF_LEVELS; i++)
+		intr_moder_tbl[i].intr_moder_interval /= intr_delay_resolution;
+
+	/* update Tx */
+	ena_dev->intr_moder_tx_interval /= intr_delay_resolution;
+}
+
+/*****************************************************************************/
+/*******************************      API       ******************************/
+/*****************************************************************************/
+
+int ena_com_execute_admin_command(struct ena_com_admin_queue *admin_queue,
+				  struct ena_admin_aq_entry *cmd,
+				  size_t cmd_size,
+				  struct ena_admin_acq_entry *comp,
+				  size_t comp_size)
+{
+	struct ena_comp_ctx *comp_ctx;
+	int ret;
+
+	comp_ctx = ena_com_submit_admin_cmd(admin_queue, cmd, cmd_size,
+					    comp, comp_size);
+	if (unlikely(IS_ERR(comp_ctx))) {
+		pr_err("Failed to submit command [%ld]\n", PTR_ERR(comp_ctx));
+		return PTR_ERR(comp_ctx);
+	}
+
+	ret = ena_com_wait_and_process_admin_cq(comp_ctx, admin_queue);
+	if (unlikely(ret)) {
+		if (admin_queue->running_state)
+			pr_err("Failed to process command. ret = %d\n", ret);
+		else
+			pr_debug("Failed to process command. ret = %d\n", ret);
+	}
+	return ret;
+}
+
+int ena_com_create_io_cq(struct ena_com_dev *ena_dev,
+			 struct ena_com_io_cq *io_cq)
+{
+	struct ena_com_admin_queue *admin_queue = &ena_dev->admin_queue;
+	struct ena_admin_aq_create_cq_cmd create_cmd;
+	struct ena_admin_acq_create_cq_resp_desc cmd_completion;
+	int ret;
+
+	memset(&create_cmd, 0x0, sizeof(struct ena_admin_aq_create_cq_cmd));
+
+	create_cmd.aq_common_descriptor.opcode = ENA_ADMIN_CREATE_CQ;
+
+	create_cmd.cq_caps_2 |= (io_cq->cdesc_entry_size_in_bytes / 4) &
+		ENA_ADMIN_AQ_CREATE_CQ_CMD_CQ_ENTRY_SIZE_WORDS_MASK;
+	create_cmd.cq_caps_1 |=
+		ENA_ADMIN_AQ_CREATE_CQ_CMD_INTERRUPT_MODE_ENABLED_MASK;
+
+	create_cmd.msix_vector = io_cq->msix_vector;
+	create_cmd.cq_depth = io_cq->q_depth;
+
+	ret = ena_com_mem_addr_set(ena_dev,
+				   &create_cmd.cq_ba,
+				   io_cq->cdesc_addr.phys_addr);
+	if (unlikely(ret)) {
+		pr_err("memory address set failed\n");
+		return ret;
+	}
+
+	ret = ena_com_execute_admin_command(admin_queue,
+					    (struct ena_admin_aq_entry *)&create_cmd,
+					    sizeof(create_cmd),
+					    (struct ena_admin_acq_entry *)&cmd_completion,
+					    sizeof(cmd_completion));
+	if (unlikely(ret)) {
+		pr_err("Failed to create IO CQ. error: %d\n", ret);
+		return ret;
+	}
+
+	io_cq->idx = cmd_completion.cq_idx;
+
+	io_cq->unmask_reg = (u32 __iomem *)((uintptr_t)ena_dev->reg_bar +
+		cmd_completion.cq_interrupt_unmask_register_offset);
+
+	if (cmd_completion.cq_head_db_register_offset)
+		io_cq->cq_head_db_reg =
+			(u32 __iomem *)((uintptr_t)ena_dev->reg_bar +
+			cmd_completion.cq_head_db_register_offset);
+
+	if (cmd_completion.numa_node_register_offset)
+		io_cq->numa_node_cfg_reg =
+			(u32 __iomem *)((uintptr_t)ena_dev->reg_bar +
+			cmd_completion.numa_node_register_offset);
+
+	pr_debug("created cq[%u], depth[%u]\n", io_cq->idx, io_cq->q_depth);
+
+	return ret;
+}
+
+int ena_com_get_io_handlers(struct ena_com_dev *ena_dev, u16 qid,
+			    struct ena_com_io_sq **io_sq,
+			    struct ena_com_io_cq **io_cq)
+{
+	if (qid >= ENA_TOTAL_NUM_QUEUES) {
+		pr_err("Invalid queue number %d but the max is %d\n", qid,
+		       ENA_TOTAL_NUM_QUEUES);
+		return -EINVAL;
+	}
+
+	*io_sq = &ena_dev->io_sq_queues[qid];
+	*io_cq = &ena_dev->io_cq_queues[qid];
+
+	return 0;
+}
+
+void ena_com_abort_admin_commands(struct ena_com_dev *ena_dev)
+{
+	struct ena_com_admin_queue *admin_queue = &ena_dev->admin_queue;
+	struct ena_comp_ctx *comp_ctx;
+	u16 i;
+
+	if (!admin_queue->comp_ctx)
+		return;
+
+	for (i = 0; i < admin_queue->q_depth; i++) {
+		comp_ctx = get_comp_ctxt(admin_queue, i, false);
+		if (unlikely(!comp_ctx))
+			break;
+
+		comp_ctx->status = ENA_CMD_ABORTED;
+
+		complete(&comp_ctx->wait_event);
+	}
+}
+
+void ena_com_wait_for_abort_completion(struct ena_com_dev *ena_dev)
+{
+	struct ena_com_admin_queue *admin_queue = &ena_dev->admin_queue;
+	unsigned long flags;
+
+	spin_lock_irqsave(&admin_queue->q_lock, flags);
+	while (atomic_read(&admin_queue->outstanding_cmds) != 0) {
+		spin_unlock_irqrestore(&admin_queue->q_lock, flags);
+		msleep(20);
+		spin_lock_irqsave(&admin_queue->q_lock, flags);
+	}
+	spin_unlock_irqrestore(&admin_queue->q_lock, flags);
+}
+
+int ena_com_destroy_io_cq(struct ena_com_dev *ena_dev,
+			  struct ena_com_io_cq *io_cq)
+{
+	struct ena_com_admin_queue *admin_queue = &ena_dev->admin_queue;
+	struct ena_admin_aq_destroy_cq_cmd destroy_cmd;
+	struct ena_admin_acq_destroy_cq_resp_desc destroy_resp;
+	int ret;
+
+	memset(&destroy_cmd, 0x0, sizeof(struct ena_admin_aq_destroy_sq_cmd));
+
+	destroy_cmd.cq_idx = io_cq->idx;
+	destroy_cmd.aq_common_descriptor.opcode = ENA_ADMIN_DESTROY_CQ;
+
+	ret = ena_com_execute_admin_command(admin_queue,
+					    (struct ena_admin_aq_entry *)&destroy_cmd,
+					    sizeof(destroy_cmd),
+					    (struct ena_admin_acq_entry *)&destroy_resp,
+					    sizeof(destroy_resp));
+
+	if (unlikely(ret && (ret != -ENODEV)))
+		pr_err("Failed to destroy IO CQ. error: %d\n", ret);
+
+	return ret;
+}
+
+bool ena_com_get_admin_running_state(struct ena_com_dev *ena_dev)
+{
+	return ena_dev->admin_queue.running_state;
+}
+
+void ena_com_set_admin_running_state(struct ena_com_dev *ena_dev, bool state)
+{
+	struct ena_com_admin_queue *admin_queue = &ena_dev->admin_queue;
+	unsigned long flags;
+
+	spin_lock_irqsave(&admin_queue->q_lock, flags);
+	ena_dev->admin_queue.running_state = state;
+	spin_unlock_irqrestore(&admin_queue->q_lock, flags);
+}
+
+void ena_com_admin_aenq_enable(struct ena_com_dev *ena_dev)
+{
+	u16 depth = ena_dev->aenq.q_depth;
+
+	WARN(ena_dev->aenq.head != depth, "Invalid AENQ state\n");
+
+	/* Init head_db to mark that all entries in the queue
+	 * are initially available
+	 */
+	writel(depth, ena_dev->reg_bar + ENA_REGS_AENQ_HEAD_DB_OFF);
+}
+
+int ena_com_set_aenq_config(struct ena_com_dev *ena_dev, u32 groups_flag)
+{
+	struct ena_com_admin_queue *admin_queue;
+	struct ena_admin_set_feat_cmd cmd;
+	struct ena_admin_set_feat_resp resp;
+	struct ena_admin_get_feat_resp get_resp;
+	int ret;
+
+	ret = ena_com_get_feature(ena_dev, &get_resp, ENA_ADMIN_AENQ_CONFIG);
+	if (ret) {
+		pr_info("Can't get aenq configuration\n");
+		return ret;
+	}
+
+	if ((get_resp.u.aenq.supported_groups & groups_flag) != groups_flag) {
+		pr_warn("Trying to set unsupported aenq events. supported flag: %x asked flag: %x\n",
+			get_resp.u.aenq.supported_groups, groups_flag);
+		return -EPERM;
+	}
+
+	memset(&cmd, 0x0, sizeof(cmd));
+	admin_queue = &ena_dev->admin_queue;
+
+	cmd.aq_common_descriptor.opcode = ENA_ADMIN_SET_FEATURE;
+	cmd.aq_common_descriptor.flags = 0;
+	cmd.feat_common.feature_id = ENA_ADMIN_AENQ_CONFIG;
+	cmd.u.aenq.enabled_groups = groups_flag;
+
+	ret = ena_com_execute_admin_command(admin_queue,
+					    (struct ena_admin_aq_entry *)&cmd,
+					    sizeof(cmd),
+					    (struct ena_admin_acq_entry *)&resp,
+					    sizeof(resp));
+
+	if (unlikely(ret))
+		pr_err("Failed to config AENQ ret: %d\n", ret);
+
+	return ret;
+}
+
+int ena_com_get_dma_width(struct ena_com_dev *ena_dev)
+{
+	u32 caps = ena_com_reg_bar_read32(ena_dev, ENA_REGS_CAPS_OFF);
+	int width;
+
+	if (unlikely(caps == ENA_MMIO_READ_TIMEOUT)) {
+		pr_err("Reg read timeout occurred\n");
+		return -ETIME;
+	}
+
+	width = (caps & ENA_REGS_CAPS_DMA_ADDR_WIDTH_MASK) >>
+		ENA_REGS_CAPS_DMA_ADDR_WIDTH_SHIFT;
+
+	pr_debug("ENA dma width: %d\n", width);
+
+	if ((width < 32) || width > ENA_MAX_PHYS_ADDR_SIZE_BITS) {
+		pr_err("DMA width illegal value: %d\n", width);
+		return -EINVAL;
+	}
+
+	ena_dev->dma_addr_bits = width;
+
+	return width;
+}
+
+int ena_com_validate_version(struct ena_com_dev *ena_dev)
+{
+	u32 ver;
+	u32 ctrl_ver;
+	u32 ctrl_ver_masked;
+
+	/* Make sure the ENA version and the controller version are at least
+	 * as the driver expects
+	 */
+	ver = ena_com_reg_bar_read32(ena_dev, ENA_REGS_VERSION_OFF);
+	ctrl_ver = ena_com_reg_bar_read32(ena_dev,
+					  ENA_REGS_CONTROLLER_VERSION_OFF);
+
+	if (unlikely((ver == ENA_MMIO_READ_TIMEOUT) ||
+		     (ctrl_ver == ENA_MMIO_READ_TIMEOUT))) {
+		pr_err("Reg read timeout occurred\n");
+		return -ETIME;
+	}
+
+	pr_info("ena device version: %d.%d\n",
+		(ver & ENA_REGS_VERSION_MAJOR_VERSION_MASK) >>
+			ENA_REGS_VERSION_MAJOR_VERSION_SHIFT,
+		ver & ENA_REGS_VERSION_MINOR_VERSION_MASK);
+
+	if (ver < MIN_ENA_VER) {
+		pr_err("ENA version is lower than the minimal version the driver supports\n");
+		return -1;
+	}
+
+	pr_info("ena controller version: %d.%d.%d implementation version %d\n",
+		(ctrl_ver & ENA_REGS_CONTROLLER_VERSION_MAJOR_VERSION_MASK) >>
+			ENA_REGS_CONTROLLER_VERSION_MAJOR_VERSION_SHIFT,
+		(ctrl_ver & ENA_REGS_CONTROLLER_VERSION_MINOR_VERSION_MASK) >>
+			ENA_REGS_CONTROLLER_VERSION_MINOR_VERSION_SHIFT,
+		(ctrl_ver & ENA_REGS_CONTROLLER_VERSION_SUBMINOR_VERSION_MASK),
+		(ctrl_ver & ENA_REGS_CONTROLLER_VERSION_IMPL_ID_MASK) >>
+			ENA_REGS_CONTROLLER_VERSION_IMPL_ID_SHIFT);
+
+	ctrl_ver_masked =
+		(ctrl_ver & ENA_REGS_CONTROLLER_VERSION_MAJOR_VERSION_MASK) |
+		(ctrl_ver & ENA_REGS_CONTROLLER_VERSION_MINOR_VERSION_MASK) |
+		(ctrl_ver & ENA_REGS_CONTROLLER_VERSION_SUBMINOR_VERSION_MASK);
+
+	/* Validate the ctrl version without the implementation ID */
+	if (ctrl_ver_masked < MIN_ENA_CTRL_VER) {
+		pr_err("ENA ctrl version is lower than the minimal ctrl version the driver supports\n");
+		return -1;
+	}
+
+	return 0;
+}
+
+void ena_com_admin_destroy(struct ena_com_dev *ena_dev)
+{
+	struct ena_com_admin_queue *admin_queue = &ena_dev->admin_queue;
+	struct ena_com_admin_cq *cq = &admin_queue->cq;
+	struct ena_com_admin_sq *sq = &admin_queue->sq;
+	struct ena_com_aenq *aenq = &ena_dev->aenq;
+	u16 size;
+
+	if (admin_queue->comp_ctx)
+		devm_kfree(ena_dev->dmadev, admin_queue->comp_ctx);
+	admin_queue->comp_ctx = NULL;
+	size = ADMIN_SQ_SIZE(admin_queue->q_depth);
+	if (sq->entries)
+		dma_free_coherent(ena_dev->dmadev, size, sq->entries,
+				  sq->dma_addr);
+	sq->entries = NULL;
+
+	size = ADMIN_CQ_SIZE(admin_queue->q_depth);
+	if (cq->entries)
+		dma_free_coherent(ena_dev->dmadev, size, cq->entries,
+				  cq->dma_addr);
+	cq->entries = NULL;
+
+	size = ADMIN_AENQ_SIZE(aenq->q_depth);
+	if (ena_dev->aenq.entries)
+		dma_free_coherent(ena_dev->dmadev, size, aenq->entries,
+				  aenq->dma_addr);
+	aenq->entries = NULL;
+}
+
+void ena_com_set_admin_polling_mode(struct ena_com_dev *ena_dev, bool polling)
+{
+	ena_dev->admin_queue.polling = polling;
+}
+
+int ena_com_mmio_reg_read_request_init(struct ena_com_dev *ena_dev)
+{
+	struct ena_com_mmio_read *mmio_read = &ena_dev->mmio_read;
+
+	spin_lock_init(&mmio_read->lock);
+	mmio_read->read_resp =
+		dma_zalloc_coherent(ena_dev->dmadev,
+				    sizeof(*mmio_read->read_resp),
+				    &mmio_read->read_resp_dma_addr, GFP_KERNEL);
+	if (unlikely(!mmio_read->read_resp))
+		return -ENOMEM;
+
+	ena_com_mmio_reg_read_request_write_dev_addr(ena_dev);
+
+	mmio_read->read_resp->req_id = 0x0;
+	mmio_read->seq_num = 0x0;
+	mmio_read->readless_supported = true;
+
+	return 0;
+}
+
+void ena_com_set_mmio_read_mode(struct ena_com_dev *ena_dev, bool readless_supported)
+{
+	struct ena_com_mmio_read *mmio_read = &ena_dev->mmio_read;
+
+	mmio_read->readless_supported = readless_supported;
+}
+
+void ena_com_mmio_reg_read_request_destroy(struct ena_com_dev *ena_dev)
+{
+	struct ena_com_mmio_read *mmio_read = &ena_dev->mmio_read;
+
+	writel(0x0, ena_dev->reg_bar + ENA_REGS_MMIO_RESP_LO_OFF);
+	writel(0x0, ena_dev->reg_bar + ENA_REGS_MMIO_RESP_HI_OFF);
+
+	dma_free_coherent(ena_dev->dmadev, sizeof(*mmio_read->read_resp),
+			  mmio_read->read_resp, mmio_read->read_resp_dma_addr);
+
+	mmio_read->read_resp = NULL;
+}
+
+void ena_com_mmio_reg_read_request_write_dev_addr(struct ena_com_dev *ena_dev)
+{
+	struct ena_com_mmio_read *mmio_read = &ena_dev->mmio_read;
+	u32 addr_low, addr_high;
+
+	addr_low = ENA_DMA_ADDR_TO_UINT32_LOW(mmio_read->read_resp_dma_addr);
+	addr_high = ENA_DMA_ADDR_TO_UINT32_HIGH(mmio_read->read_resp_dma_addr);
+
+	writel(addr_low, ena_dev->reg_bar + ENA_REGS_MMIO_RESP_LO_OFF);
+	writel(addr_high, ena_dev->reg_bar + ENA_REGS_MMIO_RESP_HI_OFF);
+}
+
+int ena_com_admin_init(struct ena_com_dev *ena_dev,
+		       struct ena_aenq_handlers *aenq_handlers,
+		       bool init_spinlock)
+{
+	struct ena_com_admin_queue *admin_queue = &ena_dev->admin_queue;
+	u32 aq_caps, acq_caps, dev_sts, addr_low, addr_high;
+	int ret;
+
+	dev_sts = ena_com_reg_bar_read32(ena_dev, ENA_REGS_DEV_STS_OFF);
+
+	if (unlikely(dev_sts == ENA_MMIO_READ_TIMEOUT)) {
+		pr_err("Reg read timeout occurred\n");
+		return -ETIME;
+	}
+
+	if (!(dev_sts & ENA_REGS_DEV_STS_READY_MASK)) {
+		pr_err("Device isn't ready, abort com init\n");
+		return -ENODEV;
+	}
+
+	admin_queue->q_depth = ENA_ADMIN_QUEUE_DEPTH;
+
+	admin_queue->q_dmadev = ena_dev->dmadev;
+	admin_queue->polling = false;
+	admin_queue->curr_cmd_id = 0;
+
+	atomic_set(&admin_queue->outstanding_cmds, 0);
+
+	if (init_spinlock)
+		spin_lock_init(&admin_queue->q_lock);
+
+	ret = ena_com_init_comp_ctxt(admin_queue);
+	if (ret)
+		goto error;
+
+	ret = ena_com_admin_init_sq(admin_queue);
+	if (ret)
+		goto error;
+
+	ret = ena_com_admin_init_cq(admin_queue);
+	if (ret)
+		goto error;
+
+	admin_queue->sq.db_addr = (u32 __iomem *)((uintptr_t)ena_dev->reg_bar +
+		ENA_REGS_AQ_DB_OFF);
+
+	addr_low = ENA_DMA_ADDR_TO_UINT32_LOW(admin_queue->sq.dma_addr);
+	addr_high = ENA_DMA_ADDR_TO_UINT32_HIGH(admin_queue->sq.dma_addr);
+
+	writel(addr_low, ena_dev->reg_bar + ENA_REGS_AQ_BASE_LO_OFF);
+	writel(addr_high, ena_dev->reg_bar + ENA_REGS_AQ_BASE_HI_OFF);
+
+	addr_low = ENA_DMA_ADDR_TO_UINT32_LOW(admin_queue->cq.dma_addr);
+	addr_high = ENA_DMA_ADDR_TO_UINT32_HIGH(admin_queue->cq.dma_addr);
+
+	writel(addr_low, ena_dev->reg_bar + ENA_REGS_ACQ_BASE_LO_OFF);
+	writel(addr_high, ena_dev->reg_bar + ENA_REGS_ACQ_BASE_HI_OFF);
+
+	aq_caps = 0;
+	aq_caps |= admin_queue->q_depth & ENA_REGS_AQ_CAPS_AQ_DEPTH_MASK;
+	aq_caps |= (sizeof(struct ena_admin_aq_entry) <<
+			ENA_REGS_AQ_CAPS_AQ_ENTRY_SIZE_SHIFT) &
+			ENA_REGS_AQ_CAPS_AQ_ENTRY_SIZE_MASK;
+
+	acq_caps = 0;
+	acq_caps |= admin_queue->q_depth & ENA_REGS_ACQ_CAPS_ACQ_DEPTH_MASK;
+	acq_caps |= (sizeof(struct ena_admin_acq_entry) <<
+		ENA_REGS_ACQ_CAPS_ACQ_ENTRY_SIZE_SHIFT) &
+		ENA_REGS_ACQ_CAPS_ACQ_ENTRY_SIZE_MASK;
+
+	writel(aq_caps, ena_dev->reg_bar + ENA_REGS_AQ_CAPS_OFF);
+	writel(acq_caps, ena_dev->reg_bar + ENA_REGS_ACQ_CAPS_OFF);
+	ret = ena_com_admin_init_aenq(ena_dev, aenq_handlers);
+	if (ret)
+		goto error;
+
+	admin_queue->running_state = true;
+
+	return 0;
+error:
+	ena_com_admin_destroy(ena_dev);
+
+	return ret;
+}
+
+int ena_com_create_io_queue(struct ena_com_dev *ena_dev,
+			    struct ena_com_create_io_ctx *ctx)
+{
+	struct ena_com_io_sq *io_sq;
+	struct ena_com_io_cq *io_cq;
+	int ret;
+
+	if (ctx->qid >= ENA_TOTAL_NUM_QUEUES) {
+		pr_err("Qid (%d) is bigger than max num of queues (%d)\n",
+		       ctx->qid, ENA_TOTAL_NUM_QUEUES);
+		return -EINVAL;
+	}
+
+	io_sq = &ena_dev->io_sq_queues[ctx->qid];
+	io_cq = &ena_dev->io_cq_queues[ctx->qid];
+
+	memset(io_sq, 0x0, sizeof(struct ena_com_io_sq));
+	memset(io_cq, 0x0, sizeof(struct ena_com_io_cq));
+
+	/* Init CQ */
+	io_cq->q_depth = ctx->queue_size;
+	io_cq->direction = ctx->direction;
+	io_cq->qid = ctx->qid;
+
+	io_cq->msix_vector = ctx->msix_vector;
+
+	io_sq->q_depth = ctx->queue_size;
+	io_sq->direction = ctx->direction;
+	io_sq->qid = ctx->qid;
+
+	io_sq->mem_queue_type = ctx->mem_queue_type;
+
+	if (ctx->direction == ENA_COM_IO_QUEUE_DIRECTION_TX)
+		/* header length is limited to 8 bits */
+		io_sq->tx_max_header_size =
+			min_t(u32, ena_dev->tx_max_header_size, SZ_256);
+
+	ret = ena_com_init_io_sq(ena_dev, ctx, io_sq);
+	if (ret)
+		goto error;
+	ret = ena_com_init_io_cq(ena_dev, ctx, io_cq);
+	if (ret)
+		goto error;
+
+	ret = ena_com_create_io_cq(ena_dev, io_cq);
+	if (ret)
+		goto error;
+
+	ret = ena_com_create_io_sq(ena_dev, io_sq, io_cq->idx);
+	if (ret)
+		goto destroy_io_cq;
+
+	return 0;
+
+destroy_io_cq:
+	ena_com_destroy_io_cq(ena_dev, io_cq);
+error:
+	ena_com_io_queue_free(ena_dev, io_sq, io_cq);
+	return ret;
+}
+
+void ena_com_destroy_io_queue(struct ena_com_dev *ena_dev, u16 qid)
+{
+	struct ena_com_io_sq *io_sq;
+	struct ena_com_io_cq *io_cq;
+
+	if (qid >= ENA_TOTAL_NUM_QUEUES) {
+		pr_err("Qid (%d) is bigger than max num of queues (%d)\n", qid,
+		       ENA_TOTAL_NUM_QUEUES);
+		return;
+	}
+
+	io_sq = &ena_dev->io_sq_queues[qid];
+	io_cq = &ena_dev->io_cq_queues[qid];
+
+	ena_com_destroy_io_sq(ena_dev, io_sq);
+	ena_com_destroy_io_cq(ena_dev, io_cq);
+
+	ena_com_io_queue_free(ena_dev, io_sq, io_cq);
+}
+
+int ena_com_get_link_params(struct ena_com_dev *ena_dev,
+			    struct ena_admin_get_feat_resp *resp)
+{
+	return ena_com_get_feature(ena_dev, resp, ENA_ADMIN_LINK_CONFIG);
+}
+
+int ena_com_get_dev_attr_feat(struct ena_com_dev *ena_dev,
+			      struct ena_com_dev_get_features_ctx *get_feat_ctx)
+{
+	struct ena_admin_get_feat_resp get_resp;
+	int rc;
+
+	rc = ena_com_get_feature(ena_dev, &get_resp,
+				 ENA_ADMIN_DEVICE_ATTRIBUTES);
+	if (rc)
+		return rc;
+
+	memcpy(&get_feat_ctx->dev_attr, &get_resp.u.dev_attr,
+	       sizeof(get_resp.u.dev_attr));
+	ena_dev->supported_features = get_resp.u.dev_attr.supported_features;
+
+	rc = ena_com_get_feature(ena_dev, &get_resp,
+				 ENA_ADMIN_MAX_QUEUES_NUM);
+	if (rc)
+		return rc;
+
+	memcpy(&get_feat_ctx->max_queues, &get_resp.u.max_queue,
+	       sizeof(get_resp.u.max_queue));
+	ena_dev->tx_max_header_size = get_resp.u.max_queue.max_header_size;
+
+	rc = ena_com_get_feature(ena_dev, &get_resp,
+				 ENA_ADMIN_AENQ_CONFIG);
+	if (rc)
+		return rc;
+
+	memcpy(&get_feat_ctx->aenq, &get_resp.u.aenq,
+	       sizeof(get_resp.u.aenq));
+
+	rc = ena_com_get_feature(ena_dev, &get_resp,
+				 ENA_ADMIN_STATELESS_OFFLOAD_CONFIG);
+	if (rc)
+		return rc;
+
+	memcpy(&get_feat_ctx->offload, &get_resp.u.offload,
+	       sizeof(get_resp.u.offload));
+
+	return 0;
+}
+
+void ena_com_admin_q_comp_intr_handler(struct ena_com_dev *ena_dev)
+{
+	ena_com_handle_admin_completion(&ena_dev->admin_queue);
+}
+
+/* ena_handle_specific_aenq_event:
+ * return the handler that is relevant to the specific event group
+ */
+static ena_aenq_handler ena_com_get_specific_aenq_cb(struct ena_com_dev *dev,
+						     u16 group)
+{
+	struct ena_aenq_handlers *aenq_handlers = dev->aenq.aenq_handlers;
+
+	if ((group < ENA_MAX_HANDLERS) && aenq_handlers->handlers[group])
+		return aenq_handlers->handlers[group];
+
+	return aenq_handlers->unimplemented_handler;
+}
+
+/* ena_aenq_intr_handler:
+ * handles the aenq incoming events.
+ * pop events from the queue and apply the specific handler
+ */
+void ena_com_aenq_intr_handler(struct ena_com_dev *dev, void *data)
+{
+	struct ena_admin_aenq_entry *aenq_e;
+	struct ena_admin_aenq_common_desc *aenq_common;
+	struct ena_com_aenq *aenq  = &dev->aenq;
+	ena_aenq_handler handler_cb;
+	u16 masked_head, processed = 0;
+	u8 phase;
+
+	masked_head = aenq->head & (aenq->q_depth - 1);
+	phase = aenq->phase;
+	aenq_e = &aenq->entries[masked_head]; /* Get first entry */
+	aenq_common = &aenq_e->aenq_common_desc;
+
+	/* Go over all the events */
+	while ((aenq_common->flags & ENA_ADMIN_AENQ_COMMON_DESC_PHASE_MASK) ==
+	       phase) {
+		pr_debug("AENQ! Group[%x] Syndrom[%x] timestamp: [%llus]\n",
+			 aenq_common->group, aenq_common->syndrom,
+			 (u64)aenq_common->timestamp_low +
+				 ((u64)aenq_common->timestamp_high << 32));
+
+		/* Handle specific event*/
+		handler_cb = ena_com_get_specific_aenq_cb(dev,
+							  aenq_common->group);
+		handler_cb(data, aenq_e); /* call the actual event handler*/
+
+		/* Get next event entry */
+		masked_head++;
+		processed++;
+
+		if (unlikely(masked_head == aenq->q_depth)) {
+			masked_head = 0;
+			phase = !phase;
+		}
+		aenq_e = &aenq->entries[masked_head];
+		aenq_common = &aenq_e->aenq_common_desc;
+	}
+
+	aenq->head += processed;
+	aenq->phase = phase;
+
+	/* Don't update aenq doorbell if there weren't any processed events */
+	if (!processed)
+		return;
+
+	/* write the aenq doorbell after all AENQ descriptors were read */
+	mb();
+	writel((u32)aenq->head, dev->reg_bar + ENA_REGS_AENQ_HEAD_DB_OFF);
+}
+
+int ena_com_dev_reset(struct ena_com_dev *ena_dev)
+{
+	u32 stat, timeout, cap, reset_val;
+	int rc;
+
+	stat = ena_com_reg_bar_read32(ena_dev, ENA_REGS_DEV_STS_OFF);
+	cap = ena_com_reg_bar_read32(ena_dev, ENA_REGS_CAPS_OFF);
+
+	if (unlikely((stat == ENA_MMIO_READ_TIMEOUT) ||
+		     (cap == ENA_MMIO_READ_TIMEOUT))) {
+		pr_err("Reg read32 timeout occurred\n");
+		return -ETIME;
+	}
+
+	if ((stat & ENA_REGS_DEV_STS_READY_MASK) == 0) {
+		pr_err("Device isn't ready, can't reset device\n");
+		return -EINVAL;
+	}
+
+	timeout = (cap & ENA_REGS_CAPS_RESET_TIMEOUT_MASK) >>
+			ENA_REGS_CAPS_RESET_TIMEOUT_SHIFT;
+	if (timeout == 0) {
+		pr_err("Invalid timeout value\n");
+		return -EINVAL;
+	}
+
+	/* start reset */
+	reset_val = ENA_REGS_DEV_CTL_DEV_RESET_MASK;
+	writel(reset_val, ena_dev->reg_bar + ENA_REGS_DEV_CTL_OFF);
+
+	/* Write again the MMIO read request address */
+	ena_com_mmio_reg_read_request_write_dev_addr(ena_dev);
+
+	rc = wait_for_reset_state(ena_dev, timeout,
+				  ENA_REGS_DEV_STS_RESET_IN_PROGRESS_MASK);
+	if (rc != 0) {
+		pr_err("Reset indication didn't turn on\n");
+		return rc;
+	}
+
+	/* reset done */
+	writel(0, ena_dev->reg_bar + ENA_REGS_DEV_CTL_OFF);
+	rc = wait_for_reset_state(ena_dev, timeout, 0);
+	if (rc != 0) {
+		pr_err("Reset indication didn't turn off\n");
+		return rc;
+	}
+
+	return 0;
+}
+
+static int ena_get_dev_stats(struct ena_com_dev *ena_dev,
+			     struct ena_com_stats_ctx *ctx,
+			     enum ena_admin_get_stats_type type)
+{
+	struct ena_admin_aq_get_stats_cmd *get_cmd = &ctx->get_cmd;
+	struct ena_admin_acq_get_stats_resp *get_resp = &ctx->get_resp;
+	struct ena_com_admin_queue *admin_queue;
+	int ret;
+
+	admin_queue = &ena_dev->admin_queue;
+
+	get_cmd->aq_common_descriptor.opcode = ENA_ADMIN_GET_STATS;
+	get_cmd->aq_common_descriptor.flags = 0;
+	get_cmd->type = type;
+
+	ret =  ena_com_execute_admin_command(admin_queue,
+					     (struct ena_admin_aq_entry *)get_cmd,
+					     sizeof(*get_cmd),
+					     (struct ena_admin_acq_entry *)get_resp,
+					     sizeof(*get_resp));
+
+	if (unlikely(ret))
+		pr_err("Failed to get stats. error: %d\n", ret);
+
+	return ret;
+}
+
+int ena_com_get_dev_basic_stats(struct ena_com_dev *ena_dev,
+				struct ena_admin_basic_stats *stats)
+{
+	struct ena_com_stats_ctx ctx;
+	int ret;
+
+	memset(&ctx, 0x0, sizeof(ctx));
+	ret = ena_get_dev_stats(ena_dev, &ctx, ENA_ADMIN_GET_STATS_TYPE_BASIC);
+	if (likely(ret == 0))
+		memcpy(stats, &ctx.get_resp.basic_stats,
+		       sizeof(ctx.get_resp.basic_stats));
+
+	return ret;
+}
+
+int ena_com_set_dev_mtu(struct ena_com_dev *ena_dev, int mtu)
+{
+	struct ena_com_admin_queue *admin_queue;
+	struct ena_admin_set_feat_cmd cmd;
+	struct ena_admin_set_feat_resp resp;
+	int ret;
+
+	if (!ena_com_check_supported_feature_id(ena_dev, ENA_ADMIN_MTU)) {
+		pr_info("Feature %d isn't supported\n", ENA_ADMIN_MTU);
+		return -EPERM;
+	}
+
+	memset(&cmd, 0x0, sizeof(cmd));
+	admin_queue = &ena_dev->admin_queue;
+
+	cmd.aq_common_descriptor.opcode = ENA_ADMIN_SET_FEATURE;
+	cmd.aq_common_descriptor.flags = 0;
+	cmd.feat_common.feature_id = ENA_ADMIN_MTU;
+	cmd.u.mtu.mtu = mtu;
+
+	ret = ena_com_execute_admin_command(admin_queue,
+					    (struct ena_admin_aq_entry *)&cmd,
+					    sizeof(cmd),
+					    (struct ena_admin_acq_entry *)&resp,
+					    sizeof(resp));
+
+	if (unlikely(ret))
+		pr_err("Failed to set mtu %d. error: %d\n", mtu, ret);
+
+	return ret;
+}
+
+int ena_com_get_offload_settings(struct ena_com_dev *ena_dev,
+				 struct ena_admin_feature_offload_desc *offload)
+{
+	int ret;
+	struct ena_admin_get_feat_resp resp;
+
+	ret = ena_com_get_feature(ena_dev, &resp,
+				  ENA_ADMIN_STATELESS_OFFLOAD_CONFIG);
+	if (unlikely(ret)) {
+		pr_err("Failed to get offload capabilities %d\n", ret);
+		return ret;
+	}
+
+	memcpy(offload, &resp.u.offload, sizeof(resp.u.offload));
+
+	return 0;
+}
+
+int ena_com_set_hash_function(struct ena_com_dev *ena_dev)
+{
+	struct ena_com_admin_queue *admin_queue = &ena_dev->admin_queue;
+	struct ena_rss *rss = &ena_dev->rss;
+	struct ena_admin_set_feat_cmd cmd;
+	struct ena_admin_set_feat_resp resp;
+	struct ena_admin_get_feat_resp get_resp;
+	int ret;
+
+	if (!ena_com_check_supported_feature_id(ena_dev,
+						ENA_ADMIN_RSS_HASH_FUNCTION)) {
+		pr_info("Feature %d isn't supported\n",
+			ENA_ADMIN_RSS_HASH_FUNCTION);
+		return -EPERM;
+	}
+
+	/* Validate hash function is supported */
+	ret = ena_com_get_feature(ena_dev, &get_resp,
+				  ENA_ADMIN_RSS_HASH_FUNCTION);
+	if (unlikely(ret))
+		return ret;
+
+	if (get_resp.u.flow_hash_func.supported_func & (1 << rss->hash_func)) {
+		pr_err("Func hash %d isn't supported by device, abort\n",
+		       rss->hash_func);
+		return -EPERM;
+	}
+
+	memset(&cmd, 0x0, sizeof(cmd));
+
+	cmd.aq_common_descriptor.opcode = ENA_ADMIN_SET_FEATURE;
+	cmd.aq_common_descriptor.flags =
+		ENA_ADMIN_AQ_COMMON_DESC_CTRL_DATA_INDIRECT_MASK;
+	cmd.feat_common.feature_id = ENA_ADMIN_RSS_HASH_FUNCTION;
+	cmd.u.flow_hash_func.init_val = rss->hash_init_val;
+	cmd.u.flow_hash_func.selected_func = 1 << rss->hash_func;
+
+	ret = ena_com_mem_addr_set(ena_dev,
+				   &cmd.control_buffer.address,
+				   rss->hash_key_dma_addr);
+	if (unlikely(ret)) {
+		pr_err("memory address set failed\n");
+		return ret;
+	}
+
+	cmd.control_buffer.length = sizeof(*rss->hash_key);
+
+	ret = ena_com_execute_admin_command(admin_queue,
+					    (struct ena_admin_aq_entry *)&cmd,
+					    sizeof(cmd),
+					    (struct ena_admin_acq_entry *)&resp,
+					    sizeof(resp));
+	if (unlikely(ret)) {
+		pr_err("Failed to set hash function %d. error: %d\n",
+		       rss->hash_func, ret);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+int ena_com_fill_hash_function(struct ena_com_dev *ena_dev,
+			       enum ena_admin_hash_functions func,
+			       const u8 *key, u16 key_len, u32 init_val)
+{
+	struct ena_rss *rss = &ena_dev->rss;
+	struct ena_admin_get_feat_resp get_resp;
+	struct ena_admin_feature_rss_flow_hash_control *hash_key =
+		rss->hash_key;
+	int rc;
+
+	/* Make sure size is a mult of DWs */
+	if (unlikely(key_len & 0x3))
+		return -EINVAL;
+
+	rc = ena_com_get_feature_ex(ena_dev, &get_resp,
+				    ENA_ADMIN_RSS_HASH_FUNCTION,
+				    rss->hash_key_dma_addr,
+				    sizeof(*rss->hash_key));
+	if (unlikely(rc))
+		return rc;
+
+	if (!((1 << func) & get_resp.u.flow_hash_func.supported_func)) {
+		pr_err("Flow hash function %d isn't supported\n", func);
+		return -EPERM;
+	}
+
+	switch (func) {
+	case ENA_ADMIN_TOEPLITZ:
+		if (key_len > sizeof(hash_key->key)) {
+			pr_err("key len (%hu) is bigger than the max supported (%zu)\n",
+			       key_len, sizeof(hash_key->key));
+			return -EINVAL;
+		}
+
+		memcpy(hash_key->key, key, key_len);
+		rss->hash_init_val = init_val;
+		hash_key->keys_num = key_len >> 2;
+		break;
+	case ENA_ADMIN_CRC32:
+		rss->hash_init_val = init_val;
+		break;
+	default:
+		pr_err("Invalid hash function (%d)\n", func);
+		return -EINVAL;
+	}
+
+	rc = ena_com_set_hash_function(ena_dev);
+
+	/* Restore the old function */
+	if (unlikely(rc))
+		ena_com_get_hash_function(ena_dev, NULL, NULL);
+
+	return rc;
+}
+
+int ena_com_get_hash_function(struct ena_com_dev *ena_dev,
+			      enum ena_admin_hash_functions *func,
+			      u8 *key)
+{
+	struct ena_rss *rss = &ena_dev->rss;
+	struct ena_admin_get_feat_resp get_resp;
+	struct ena_admin_feature_rss_flow_hash_control *hash_key =
+		rss->hash_key;
+	int rc;
+
+	rc = ena_com_get_feature_ex(ena_dev, &get_resp,
+				    ENA_ADMIN_RSS_HASH_FUNCTION,
+				    rss->hash_key_dma_addr,
+				    sizeof(*rss->hash_key));
+	if (unlikely(rc))
+		return rc;
+
+	rss->hash_func = get_resp.u.flow_hash_func.selected_func;
+	if (func)
+		*func = rss->hash_func;
+
+	if (key)
+		memcpy(key, hash_key->key, (size_t)(hash_key->keys_num) << 2);
+
+	return 0;
+}
+
+int ena_com_get_hash_ctrl(struct ena_com_dev *ena_dev,
+			  enum ena_admin_flow_hash_proto proto,
+			  u16 *fields)
+{
+	struct ena_rss *rss = &ena_dev->rss;
+	struct ena_admin_get_feat_resp get_resp;
+	int rc;
+
+	rc = ena_com_get_feature_ex(ena_dev, &get_resp,
+				    ENA_ADMIN_RSS_HASH_INPUT,
+				    rss->hash_ctrl_dma_addr,
+				    sizeof(*rss->hash_ctrl));
+	if (unlikely(rc))
+		return rc;
+
+	if (fields)
+		*fields = rss->hash_ctrl->selected_fields[proto].fields;
+
+	return 0;
+}
+
+int ena_com_set_hash_ctrl(struct ena_com_dev *ena_dev)
+{
+	struct ena_com_admin_queue *admin_queue = &ena_dev->admin_queue;
+	struct ena_rss *rss = &ena_dev->rss;
+	struct ena_admin_feature_rss_hash_control *hash_ctrl = rss->hash_ctrl;
+	struct ena_admin_set_feat_cmd cmd;
+	struct ena_admin_set_feat_resp resp;
+	int ret;
+
+	if (!ena_com_check_supported_feature_id(ena_dev,
+						ENA_ADMIN_RSS_HASH_INPUT)) {
+		pr_info("Feature %d isn't supported\n", ENA_ADMIN_RSS_HASH_INPUT);
+		return -EPERM;
+	}
+
+	memset(&cmd, 0x0, sizeof(cmd));
+
+	cmd.aq_common_descriptor.opcode = ENA_ADMIN_SET_FEATURE;
+	cmd.aq_common_descriptor.flags =
+		ENA_ADMIN_AQ_COMMON_DESC_CTRL_DATA_INDIRECT_MASK;
+	cmd.feat_common.feature_id = ENA_ADMIN_RSS_HASH_INPUT;
+	cmd.u.flow_hash_input.enabled_input_sort =
+		ENA_ADMIN_FEATURE_RSS_FLOW_HASH_INPUT_L3_SORT_MASK |
+		ENA_ADMIN_FEATURE_RSS_FLOW_HASH_INPUT_L4_SORT_MASK;
+
+	ret = ena_com_mem_addr_set(ena_dev,
+				   &cmd.control_buffer.address,
+				   rss->hash_ctrl_dma_addr);
+	if (unlikely(ret)) {
+		pr_err("memory address set failed\n");
+		return ret;
+	}
+	cmd.control_buffer.length = sizeof(*hash_ctrl);
+
+	ret = ena_com_execute_admin_command(admin_queue,
+					    (struct ena_admin_aq_entry *)&cmd,
+					    sizeof(cmd),
+					    (struct ena_admin_acq_entry *)&resp,
+					    sizeof(resp));
+	if (unlikely(ret))
+		pr_err("Failed to set hash input. error: %d\n", ret);
+
+	return ret;
+}
+
+int ena_com_set_default_hash_ctrl(struct ena_com_dev *ena_dev)
+{
+	struct ena_rss *rss = &ena_dev->rss;
+	struct ena_admin_feature_rss_hash_control *hash_ctrl =
+		rss->hash_ctrl;
+	u16 available_fields = 0;
+	int rc, i;
+
+	/* Get the supported hash input */
+	rc = ena_com_get_hash_ctrl(ena_dev, 0, NULL);
+	if (unlikely(rc))
+		return rc;
+
+	hash_ctrl->selected_fields[ENA_ADMIN_RSS_TCP4].fields =
+		ENA_ADMIN_RSS_L3_SA | ENA_ADMIN_RSS_L3_DA |
+		ENA_ADMIN_RSS_L4_DP | ENA_ADMIN_RSS_L4_SP;
+
+	hash_ctrl->selected_fields[ENA_ADMIN_RSS_UDP4].fields =
+		ENA_ADMIN_RSS_L3_SA | ENA_ADMIN_RSS_L3_DA |
+		ENA_ADMIN_RSS_L4_DP | ENA_ADMIN_RSS_L4_SP;
+
+	hash_ctrl->selected_fields[ENA_ADMIN_RSS_TCP6].fields =
+		ENA_ADMIN_RSS_L3_SA | ENA_ADMIN_RSS_L3_DA |
+		ENA_ADMIN_RSS_L4_DP | ENA_ADMIN_RSS_L4_SP;
+
+	hash_ctrl->selected_fields[ENA_ADMIN_RSS_UDP6].fields =
+		ENA_ADMIN_RSS_L3_SA | ENA_ADMIN_RSS_L3_DA |
+		ENA_ADMIN_RSS_L4_DP | ENA_ADMIN_RSS_L4_SP;
+
+	hash_ctrl->selected_fields[ENA_ADMIN_RSS_IP4].fields =
+		ENA_ADMIN_RSS_L3_SA | ENA_ADMIN_RSS_L3_DA;
+
+	hash_ctrl->selected_fields[ENA_ADMIN_RSS_IP6].fields =
+		ENA_ADMIN_RSS_L3_SA | ENA_ADMIN_RSS_L3_DA;
+
+	hash_ctrl->selected_fields[ENA_ADMIN_RSS_IP4_FRAG].fields =
+		ENA_ADMIN_RSS_L3_SA | ENA_ADMIN_RSS_L3_DA;
+
+	hash_ctrl->selected_fields[ENA_ADMIN_RSS_IP4_FRAG].fields =
+		ENA_ADMIN_RSS_L2_DA | ENA_ADMIN_RSS_L2_SA;
+
+	for (i = 0; i < ENA_ADMIN_RSS_PROTO_NUM; i++) {
+		available_fields = hash_ctrl->selected_fields[i].fields &
+				hash_ctrl->supported_fields[i].fields;
+		if (available_fields != hash_ctrl->selected_fields[i].fields) {
+			pr_err("hash control doesn't support all the desire configuration. proto %x supported %x selected %x\n",
+			       i, hash_ctrl->supported_fields[i].fields,
+			       hash_ctrl->selected_fields[i].fields);
+			return -EPERM;
+		}
+	}
+
+	rc = ena_com_set_hash_ctrl(ena_dev);
+
+	/* In case of failure, restore the old hash ctrl */
+	if (unlikely(rc))
+		ena_com_get_hash_ctrl(ena_dev, 0, NULL);
+
+	return rc;
+}
+
+int ena_com_fill_hash_ctrl(struct ena_com_dev *ena_dev,
+			   enum ena_admin_flow_hash_proto proto,
+			   u16 hash_fields)
+{
+	struct ena_rss *rss = &ena_dev->rss;
+	struct ena_admin_feature_rss_hash_control *hash_ctrl = rss->hash_ctrl;
+	u16 supported_fields;
+	int rc;
+
+	if (proto >= ENA_ADMIN_RSS_PROTO_NUM) {
+		pr_err("Invalid proto num (%u)\n", proto);
+		return -EINVAL;
+	}
+
+	/* Get the ctrl table */
+	rc = ena_com_get_hash_ctrl(ena_dev, proto, NULL);
+	if (unlikely(rc))
+		return rc;
+
+	/* Make sure all the fields are supported */
+	supported_fields = hash_ctrl->supported_fields[proto].fields;
+	if ((hash_fields & supported_fields) != hash_fields) {
+		pr_err("proto %d doesn't support the required fields %x. supports only: %x\n",
+		       proto, hash_fields, supported_fields);
+	}
+
+	hash_ctrl->selected_fields[proto].fields = hash_fields;
+
+	rc = ena_com_set_hash_ctrl(ena_dev);
+
+	/* In case of failure, restore the old hash ctrl */
+	if (unlikely(rc))
+		ena_com_get_hash_ctrl(ena_dev, 0, NULL);
+
+	return 0;
+}
+
+int ena_com_indirect_table_fill_entry(struct ena_com_dev *ena_dev,
+				      u16 entry_idx, u16 entry_value)
+{
+	struct ena_rss *rss = &ena_dev->rss;
+
+	if (unlikely(entry_idx >= (1 << rss->tbl_log_size)))
+		return -EINVAL;
+
+	if (unlikely((entry_value > ENA_TOTAL_NUM_QUEUES)))
+		return -EINVAL;
+
+	rss->host_rss_ind_tbl[entry_idx] = entry_value;
+
+	return 0;
+}
+
+int ena_com_indirect_table_set(struct ena_com_dev *ena_dev)
+{
+	struct ena_com_admin_queue *admin_queue = &ena_dev->admin_queue;
+	struct ena_rss *rss = &ena_dev->rss;
+	struct ena_admin_set_feat_cmd cmd;
+	struct ena_admin_set_feat_resp resp;
+	int ret;
+
+	if (!ena_com_check_supported_feature_id(
+		    ena_dev, ENA_ADMIN_RSS_REDIRECTION_TABLE_CONFIG)) {
+		pr_info("Feature %d isn't supported\n",
+			ENA_ADMIN_RSS_REDIRECTION_TABLE_CONFIG);
+		return -EPERM;
+	}
+
+	ret = ena_com_ind_tbl_convert_to_device(ena_dev);
+	if (ret) {
+		pr_err("Failed to convert host indirection table to device table\n");
+		return ret;
+	}
+
+	memset(&cmd, 0x0, sizeof(cmd));
+
+	cmd.aq_common_descriptor.opcode = ENA_ADMIN_SET_FEATURE;
+	cmd.aq_common_descriptor.flags =
+		ENA_ADMIN_AQ_COMMON_DESC_CTRL_DATA_INDIRECT_MASK;
+	cmd.feat_common.feature_id = ENA_ADMIN_RSS_REDIRECTION_TABLE_CONFIG;
+	cmd.u.ind_table.size = rss->tbl_log_size;
+	cmd.u.ind_table.inline_index = 0xFFFFFFFF;
+
+	ret = ena_com_mem_addr_set(ena_dev,
+				   &cmd.control_buffer.address,
+				   rss->rss_ind_tbl_dma_addr);
+	if (unlikely(ret)) {
+		pr_err("memory address set failed\n");
+		return ret;
+	}
+
+	cmd.control_buffer.length = (1ULL << rss->tbl_log_size) *
+		sizeof(struct ena_admin_rss_ind_table_entry);
+
+	ret = ena_com_execute_admin_command(admin_queue,
+					    (struct ena_admin_aq_entry *)&cmd,
+					    sizeof(cmd),
+					    (struct ena_admin_acq_entry *)&resp,
+					    sizeof(resp));
+
+	if (unlikely(ret))
+		pr_err("Failed to set indirect table. error: %d\n", ret);
+
+	return ret;
+}
+
+int ena_com_indirect_table_get(struct ena_com_dev *ena_dev, u32 *ind_tbl)
+{
+	struct ena_rss *rss = &ena_dev->rss;
+	struct ena_admin_get_feat_resp get_resp;
+	u32 tbl_size;
+	int i, rc;
+
+	tbl_size = (1ULL << rss->tbl_log_size) *
+		sizeof(struct ena_admin_rss_ind_table_entry);
+
+	rc = ena_com_get_feature_ex(ena_dev, &get_resp,
+				    ENA_ADMIN_RSS_REDIRECTION_TABLE_CONFIG,
+				    rss->rss_ind_tbl_dma_addr,
+				    tbl_size);
+	if (unlikely(rc))
+		return rc;
+
+	if (!ind_tbl)
+		return 0;
+
+	rc = ena_com_ind_tbl_convert_from_device(ena_dev);
+	if (unlikely(rc))
+		return rc;
+
+	for (i = 0; i < (1 << rss->tbl_log_size); i++)
+		ind_tbl[i] = rss->host_rss_ind_tbl[i];
+
+	return 0;
+}
+
+int ena_com_rss_init(struct ena_com_dev *ena_dev, u16 indr_tbl_log_size)
+{
+	int rc;
+
+	memset(&ena_dev->rss, 0x0, sizeof(ena_dev->rss));
+
+	rc = ena_com_indirect_table_allocate(ena_dev, indr_tbl_log_size);
+	if (unlikely(rc))
+		goto err_indr_tbl;
+
+	rc = ena_com_hash_key_allocate(ena_dev);
+	if (unlikely(rc))
+		goto err_hash_key;
+
+	rc = ena_com_hash_ctrl_init(ena_dev);
+	if (unlikely(rc))
+		goto err_hash_ctrl;
+
+	return 0;
+
+err_hash_ctrl:
+	ena_com_hash_key_destroy(ena_dev);
+err_hash_key:
+	ena_com_indirect_table_destroy(ena_dev);
+err_indr_tbl:
+
+	return rc;
+}
+
+void ena_com_rss_destroy(struct ena_com_dev *ena_dev)
+{
+	ena_com_indirect_table_destroy(ena_dev);
+	ena_com_hash_key_destroy(ena_dev);
+	ena_com_hash_ctrl_destroy(ena_dev);
+
+	memset(&ena_dev->rss, 0x0, sizeof(ena_dev->rss));
+}
+
+int ena_com_allocate_host_info(struct ena_com_dev *ena_dev)
+{
+	struct ena_host_attribute *host_attr = &ena_dev->host_attr;
+
+	host_attr->host_info =
+		dma_zalloc_coherent(ena_dev->dmadev, SZ_4K,
+				    &host_attr->host_info_dma_addr, GFP_KERNEL);
+	if (unlikely(!host_attr->host_info))
+		return -ENOMEM;
+
+	return 0;
+}
+
+int ena_com_allocate_debug_area(struct ena_com_dev *ena_dev,
+				u32 debug_area_size)
+{
+	struct ena_host_attribute *host_attr = &ena_dev->host_attr;
+
+	host_attr->debug_area_virt_addr =
+		dma_zalloc_coherent(ena_dev->dmadev, debug_area_size,
+				    &host_attr->debug_area_dma_addr, GFP_KERNEL);
+	if (unlikely(!host_attr->debug_area_virt_addr)) {
+		host_attr->debug_area_size = 0;
+		return -ENOMEM;
+	}
+
+	host_attr->debug_area_size = debug_area_size;
+
+	return 0;
+}
+
+void ena_com_delete_host_info(struct ena_com_dev *ena_dev)
+{
+	struct ena_host_attribute *host_attr = &ena_dev->host_attr;
+
+	if (host_attr->host_info) {
+		dma_free_coherent(ena_dev->dmadev, SZ_4K, host_attr->host_info,
+				  host_attr->host_info_dma_addr);
+		host_attr->host_info = NULL;
+	}
+}
+
+void ena_com_delete_debug_area(struct ena_com_dev *ena_dev)
+{
+	struct ena_host_attribute *host_attr = &ena_dev->host_attr;
+
+	if (host_attr->debug_area_virt_addr) {
+		dma_free_coherent(ena_dev->dmadev, host_attr->debug_area_size,
+				  host_attr->debug_area_virt_addr,
+				  host_attr->debug_area_dma_addr);
+		host_attr->debug_area_virt_addr = NULL;
+	}
+}
+
+int ena_com_set_host_attributes(struct ena_com_dev *ena_dev)
+{
+	struct ena_host_attribute *host_attr = &ena_dev->host_attr;
+	struct ena_com_admin_queue *admin_queue;
+	struct ena_admin_set_feat_cmd cmd;
+	struct ena_admin_set_feat_resp resp;
+
+	int ret;
+
+	if (!ena_com_check_supported_feature_id(ena_dev,
+						ENA_ADMIN_HOST_ATTR_CONFIG)) {
+		pr_warn("Set host attribute isn't supported\n");
+		return -EPERM;
+	}
+
+	memset(&cmd, 0x0, sizeof(cmd));
+	admin_queue = &ena_dev->admin_queue;
+
+	cmd.aq_common_descriptor.opcode = ENA_ADMIN_SET_FEATURE;
+	cmd.feat_common.feature_id = ENA_ADMIN_HOST_ATTR_CONFIG;
+
+	ret = ena_com_mem_addr_set(ena_dev,
+				   &cmd.u.host_attr.debug_ba,
+				   host_attr->debug_area_dma_addr);
+	if (unlikely(ret)) {
+		pr_err("memory address set failed\n");
+		return ret;
+	}
+
+	ret = ena_com_mem_addr_set(ena_dev,
+				   &cmd.u.host_attr.os_info_ba,
+				   host_attr->host_info_dma_addr);
+	if (unlikely(ret)) {
+		pr_err("memory address set failed\n");
+		return ret;
+	}
+
+	cmd.u.host_attr.debug_area_size = host_attr->debug_area_size;
+
+	ret = ena_com_execute_admin_command(admin_queue,
+					    (struct ena_admin_aq_entry *)&cmd,
+					    sizeof(cmd),
+					    (struct ena_admin_acq_entry *)&resp,
+					    sizeof(resp));
+
+	if (unlikely(ret))
+		pr_err("Failed to set host attributes: %d\n", ret);
+
+	return ret;
+}
+
+/* Interrupt moderation */
+bool ena_com_interrupt_moderation_supported(struct ena_com_dev *ena_dev)
+{
+	return ena_com_check_supported_feature_id(ena_dev,
+						  ENA_ADMIN_INTERRUPT_MODERATION);
+}
+
+int ena_com_update_nonadaptive_moderation_interval_tx(struct ena_com_dev *ena_dev,
+						      u32 tx_coalesce_usecs)
+{
+	if (!ena_dev->intr_delay_resolution) {
+		pr_err("Illegal interrupt delay granularity value\n");
+		return -EFAULT;
+	}
+
+	ena_dev->intr_moder_tx_interval = tx_coalesce_usecs /
+		ena_dev->intr_delay_resolution;
+
+	return 0;
+}
+
+int ena_com_update_nonadaptive_moderation_interval_rx(struct ena_com_dev *ena_dev,
+						      u32 rx_coalesce_usecs)
+{
+	if (!ena_dev->intr_delay_resolution) {
+		pr_err("Illegal interrupt delay granularity value\n");
+		return -EFAULT;
+	}
+
+	/* We use LOWEST entry of moderation table for storing
+	 * nonadaptive interrupt coalescing values
+	 */
+	ena_dev->intr_moder_tbl[ENA_INTR_MODER_LOWEST].intr_moder_interval =
+		rx_coalesce_usecs / ena_dev->intr_delay_resolution;
+
+	return 0;
+}
+
+void ena_com_destroy_interrupt_moderation(struct ena_com_dev *ena_dev)
+{
+	if (ena_dev->intr_moder_tbl)
+		devm_kfree(ena_dev->dmadev, ena_dev->intr_moder_tbl);
+	ena_dev->intr_moder_tbl = NULL;
+}
+
+int ena_com_init_interrupt_moderation(struct ena_com_dev *ena_dev)
+{
+	struct ena_admin_get_feat_resp get_resp;
+	u16 delay_resolution;
+	int rc;
+
+	rc = ena_com_get_feature(ena_dev, &get_resp,
+				 ENA_ADMIN_INTERRUPT_MODERATION);
+
+	if (rc) {
+		if (rc == -EPERM) {
+			pr_info("Feature %d isn't supported\n",
+				ENA_ADMIN_INTERRUPT_MODERATION);
+			rc = 0;
+		} else {
+			pr_err("Failed to get interrupt moderation admin cmd. rc: %d\n",
+			       rc);
+		}
+
+		/* no moderation supported, disable adaptive support */
+		ena_com_disable_adaptive_moderation(ena_dev);
+		return rc;
+	}
+
+	rc = ena_com_init_interrupt_moderation_table(ena_dev);
+	if (rc)
+		goto err;
+
+	/* if moderation is supported by device we set adaptive moderation */
+	delay_resolution = get_resp.u.intr_moderation.intr_delay_resolution;
+	ena_com_update_intr_delay_resolution(ena_dev, delay_resolution);
+	ena_com_enable_adaptive_moderation(ena_dev);
+
+	return 0;
+err:
+	ena_com_destroy_interrupt_moderation(ena_dev);
+	return rc;
+}
+
+void ena_com_config_default_interrupt_moderation_table(struct ena_com_dev *ena_dev)
+{
+	struct ena_intr_moder_entry *intr_moder_tbl = ena_dev->intr_moder_tbl;
+
+	if (!intr_moder_tbl)
+		return;
+
+	intr_moder_tbl[ENA_INTR_MODER_LOWEST].intr_moder_interval =
+		ENA_INTR_LOWEST_USECS;
+	intr_moder_tbl[ENA_INTR_MODER_LOWEST].pkts_per_interval =
+		ENA_INTR_LOWEST_PKTS;
+	intr_moder_tbl[ENA_INTR_MODER_LOWEST].bytes_per_interval =
+		ENA_INTR_LOWEST_BYTES;
+
+	intr_moder_tbl[ENA_INTR_MODER_LOW].intr_moder_interval =
+		ENA_INTR_LOW_USECS;
+	intr_moder_tbl[ENA_INTR_MODER_LOW].pkts_per_interval =
+		ENA_INTR_LOW_PKTS;
+	intr_moder_tbl[ENA_INTR_MODER_LOW].bytes_per_interval =
+		ENA_INTR_LOW_BYTES;
+
+	intr_moder_tbl[ENA_INTR_MODER_MID].intr_moder_interval =
+		ENA_INTR_MID_USECS;
+	intr_moder_tbl[ENA_INTR_MODER_MID].pkts_per_interval =
+		ENA_INTR_MID_PKTS;
+	intr_moder_tbl[ENA_INTR_MODER_MID].bytes_per_interval =
+		ENA_INTR_MID_BYTES;
+
+	intr_moder_tbl[ENA_INTR_MODER_HIGH].intr_moder_interval =
+		ENA_INTR_HIGH_USECS;
+	intr_moder_tbl[ENA_INTR_MODER_HIGH].pkts_per_interval =
+		ENA_INTR_HIGH_PKTS;
+	intr_moder_tbl[ENA_INTR_MODER_HIGH].bytes_per_interval =
+		ENA_INTR_HIGH_BYTES;
+
+	intr_moder_tbl[ENA_INTR_MODER_HIGHEST].intr_moder_interval =
+		ENA_INTR_HIGHEST_USECS;
+	intr_moder_tbl[ENA_INTR_MODER_HIGHEST].pkts_per_interval =
+		ENA_INTR_HIGHEST_PKTS;
+	intr_moder_tbl[ENA_INTR_MODER_HIGHEST].bytes_per_interval =
+		ENA_INTR_HIGHEST_BYTES;
+}
+
+unsigned int ena_com_get_nonadaptive_moderation_interval_tx(struct ena_com_dev *ena_dev)
+{
+	return ena_dev->intr_moder_tx_interval;
+}
+
+unsigned int ena_com_get_nonadaptive_moderation_interval_rx(struct ena_com_dev *ena_dev)
+{
+	struct ena_intr_moder_entry *intr_moder_tbl = ena_dev->intr_moder_tbl;
+
+	if (intr_moder_tbl)
+		return intr_moder_tbl[ENA_INTR_MODER_LOWEST].intr_moder_interval;
+
+	return 0;
+}
+
+void ena_com_init_intr_moderation_entry(struct ena_com_dev *ena_dev,
+					enum ena_intr_moder_level level,
+					struct ena_intr_moder_entry *entry)
+{
+	struct ena_intr_moder_entry *intr_moder_tbl = ena_dev->intr_moder_tbl;
+
+	if (level >= ENA_INTR_MAX_NUM_OF_LEVELS)
+		return;
+
+	intr_moder_tbl[level].intr_moder_interval = entry->intr_moder_interval;
+	if (ena_dev->intr_delay_resolution)
+		intr_moder_tbl[level].intr_moder_interval /=
+			ena_dev->intr_delay_resolution;
+	intr_moder_tbl[level].pkts_per_interval = entry->pkts_per_interval;
+
+	/* use hardcoded value until ethtool supports bytecount parameter */
+	if (entry->bytes_per_interval != ENA_INTR_BYTE_COUNT_NOT_SUPPORTED)
+		intr_moder_tbl[level].bytes_per_interval = entry->bytes_per_interval;
+}
+
+void ena_com_get_intr_moderation_entry(struct ena_com_dev *ena_dev,
+				       enum ena_intr_moder_level level,
+				       struct ena_intr_moder_entry *entry)
+{
+	struct ena_intr_moder_entry *intr_moder_tbl = ena_dev->intr_moder_tbl;
+
+	if (level >= ENA_INTR_MAX_NUM_OF_LEVELS)
+		return;
+
+	entry->intr_moder_interval = intr_moder_tbl[level].intr_moder_interval;
+	if (ena_dev->intr_delay_resolution)
+		entry->intr_moder_interval *= ena_dev->intr_delay_resolution;
+	entry->pkts_per_interval =
+	intr_moder_tbl[level].pkts_per_interval;
+	entry->bytes_per_interval = intr_moder_tbl[level].bytes_per_interval;
+}
diff --git a/drivers/net/ethernet/amazon/ena/ena_com.h b/drivers/net/ethernet/amazon/ena/ena_com.h
new file mode 100644
index 000000000000..509d7b8e15ab
--- /dev/null
+++ b/drivers/net/ethernet/amazon/ena/ena_com.h
@@ -0,0 +1,1038 @@
+/*
+ * Copyright 2015 Amazon.com, Inc. or its affiliates.
+ *
+ * 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
+ * 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 ENA_COM
+#define ENA_COM
+
+#include <linux/delay.h>
+#include <linux/dma-mapping.h>
+#include <linux/gfp.h>
+#include <linux/sched.h>
+#include <linux/sizes.h>
+#include <linux/spinlock.h>
+#include <linux/types.h>
+#include <linux/wait.h>
+
+#include "ena_common_defs.h"
+#include "ena_admin_defs.h"
+#include "ena_eth_io_defs.h"
+#include "ena_regs_defs.h"
+
+#undef pr_fmt
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#define ENA_MAX_NUM_IO_QUEUES		128U
+/* We need to queues for each IO (on for Tx and one for Rx) */
+#define ENA_TOTAL_NUM_QUEUES		(2 * (ENA_MAX_NUM_IO_QUEUES))
+
+#define ENA_MAX_HANDLERS 256
+
+#define ENA_MAX_PHYS_ADDR_SIZE_BITS 48
+
+/* Unit in usec */
+#define ENA_REG_READ_TIMEOUT 200000
+
+#define ADMIN_SQ_SIZE(depth)	((depth) * sizeof(struct ena_admin_aq_entry))
+#define ADMIN_CQ_SIZE(depth)	((depth) * sizeof(struct ena_admin_acq_entry))
+#define ADMIN_AENQ_SIZE(depth)	((depth) * sizeof(struct ena_admin_aenq_entry))
+
+/*****************************************************************************/
+/*****************************************************************************/
+/* ENA adaptive interrupt moderation settings */
+
+#define ENA_INTR_LOWEST_USECS           (0)
+#define ENA_INTR_LOWEST_PKTS            (3)
+#define ENA_INTR_LOWEST_BYTES           (2 * 1524)
+
+#define ENA_INTR_LOW_USECS              (32)
+#define ENA_INTR_LOW_PKTS               (12)
+#define ENA_INTR_LOW_BYTES              (16 * 1024)
+
+#define ENA_INTR_MID_USECS              (80)
+#define ENA_INTR_MID_PKTS               (48)
+#define ENA_INTR_MID_BYTES              (64 * 1024)
+
+#define ENA_INTR_HIGH_USECS             (128)
+#define ENA_INTR_HIGH_PKTS              (96)
+#define ENA_INTR_HIGH_BYTES             (128 * 1024)
+
+#define ENA_INTR_HIGHEST_USECS          (192)
+#define ENA_INTR_HIGHEST_PKTS           (128)
+#define ENA_INTR_HIGHEST_BYTES          (192 * 1024)
+
+#define ENA_INTR_INITIAL_TX_INTERVAL_USECS		196
+#define ENA_INTR_INITIAL_RX_INTERVAL_USECS		4
+#define ENA_INTR_DELAY_OLD_VALUE_WEIGHT			6
+#define ENA_INTR_DELAY_NEW_VALUE_WEIGHT			4
+#define ENA_INTR_MODER_LEVEL_STRIDE			2
+#define ENA_INTR_BYTE_COUNT_NOT_SUPPORTED		0xFFFFFF
+
+enum ena_intr_moder_level {
+	ENA_INTR_MODER_LOWEST = 0,
+	ENA_INTR_MODER_LOW,
+	ENA_INTR_MODER_MID,
+	ENA_INTR_MODER_HIGH,
+	ENA_INTR_MODER_HIGHEST,
+	ENA_INTR_MAX_NUM_OF_LEVELS,
+};
+
+struct ena_intr_moder_entry {
+	unsigned int intr_moder_interval;
+	unsigned int pkts_per_interval;
+	unsigned int bytes_per_interval;
+};
+
+enum queue_direction {
+	ENA_COM_IO_QUEUE_DIRECTION_TX,
+	ENA_COM_IO_QUEUE_DIRECTION_RX
+};
+
+struct ena_com_buf {
+	dma_addr_t paddr; /**< Buffer physical address */
+	u16 len; /**< Buffer length in bytes */
+};
+
+struct ena_com_rx_buf_info {
+	u16 len;
+	u16 req_id;
+};
+
+struct ena_com_io_desc_addr {
+	u8 __iomem *pbuf_dev_addr; /* LLQ address */
+	u8 *virt_addr;
+	dma_addr_t phys_addr;
+};
+
+struct ena_com_tx_meta {
+	u16 mss;
+	u16 l3_hdr_len;
+	u16 l3_hdr_offset;
+	u16 l4_hdr_len; /* In words */
+};
+
+struct ena_com_io_cq {
+	struct ena_com_io_desc_addr cdesc_addr;
+
+	/* Interrupt unmask register */
+	u32 __iomem *unmask_reg;
+
+	/* The completion queue head doorbell register */
+	u32 __iomem *cq_head_db_reg;
+
+	/* numa configuration register (for TPH) */
+	u32 __iomem *numa_node_cfg_reg;
+
+	/* The value to write to the above register to unmask
+	 * the interrupt of this queue
+	 */
+	u32 msix_vector;
+
+	enum queue_direction direction;
+
+	/* holds the number of cdesc of the current packet */
+	u16 cur_rx_pkt_cdesc_count;
+	/* save the firt cdesc idx of the current packet */
+	u16 cur_rx_pkt_cdesc_start_idx;
+
+	u16 q_depth;
+	/* Caller qid */
+	u16 qid;
+
+	/* Device queue index */
+	u16 idx;
+	u16 head;
+	u16 last_head_update;
+	u8 phase;
+	u8 cdesc_entry_size_in_bytes;
+
+} ____cacheline_aligned;
+
+struct ena_com_io_sq {
+	struct ena_com_io_desc_addr desc_addr;
+
+	u32 __iomem *db_addr;
+	u8 __iomem *header_addr;
+
+	enum queue_direction direction;
+	enum ena_admin_placement_policy_type mem_queue_type;
+
+	u32 msix_vector;
+	struct ena_com_tx_meta cached_tx_meta;
+
+	u16 q_depth;
+	u16 qid;
+
+	u16 idx;
+	u16 tail;
+	u16 next_to_comp;
+	u32 tx_max_header_size;
+	u8 phase;
+	u8 desc_entry_size;
+	u8 dma_addr_bits;
+} ____cacheline_aligned;
+
+struct ena_com_admin_cq {
+	struct ena_admin_acq_entry *entries;
+	dma_addr_t dma_addr;
+
+	u16 head;
+	u8 phase;
+};
+
+struct ena_com_admin_sq {
+	struct ena_admin_aq_entry *entries;
+	dma_addr_t dma_addr;
+
+	u32 __iomem *db_addr;
+
+	u16 head;
+	u16 tail;
+	u8 phase;
+
+};
+
+struct ena_com_stats_admin {
+	u32 aborted_cmd;
+	u32 submitted_cmd;
+	u32 completed_cmd;
+	u32 out_of_space;
+	u32 no_completion;
+};
+
+struct ena_com_admin_queue {
+	void *q_dmadev;
+	spinlock_t q_lock; /* spinlock for the admin queue */
+	struct ena_comp_ctx *comp_ctx;
+	u16 q_depth;
+	struct ena_com_admin_cq cq;
+	struct ena_com_admin_sq sq;
+
+	/* Indicate if the admin queue should poll for completion */
+	bool polling;
+
+	u16 curr_cmd_id;
+
+	/* Indicate that the ena was initialized and can
+	 * process new admin commands
+	 */
+	bool running_state;
+
+	/* Count the number of outstanding admin commands */
+	atomic_t outstanding_cmds;
+
+	struct ena_com_stats_admin stats;
+};
+
+struct ena_aenq_handlers;
+
+struct ena_com_aenq {
+	u16 head;
+	u8 phase;
+	struct ena_admin_aenq_entry *entries;
+	dma_addr_t dma_addr;
+	u16 q_depth;
+	struct ena_aenq_handlers *aenq_handlers;
+};
+
+struct ena_com_mmio_read {
+	struct ena_admin_ena_mmio_req_read_less_resp *read_resp;
+	dma_addr_t read_resp_dma_addr;
+	u16 seq_num;
+	bool readless_supported;
+	/* spin lock to ensure a single outstanding read */
+	spinlock_t lock;
+};
+
+struct ena_rss {
+	/* Indirect table */
+	u16 *host_rss_ind_tbl;
+	struct ena_admin_rss_ind_table_entry *rss_ind_tbl;
+	dma_addr_t rss_ind_tbl_dma_addr;
+	u16 tbl_log_size;
+
+	/* Hash key */
+	enum ena_admin_hash_functions hash_func;
+	struct ena_admin_feature_rss_flow_hash_control *hash_key;
+	dma_addr_t hash_key_dma_addr;
+	u32 hash_init_val;
+
+	/* Flow Control */
+	struct ena_admin_feature_rss_hash_control *hash_ctrl;
+	dma_addr_t hash_ctrl_dma_addr;
+
+};
+
+struct ena_host_attribute {
+	/* Debug area */
+	u8 *debug_area_virt_addr;
+	dma_addr_t debug_area_dma_addr;
+	u32 debug_area_size;
+
+	/* Host information */
+	struct ena_admin_host_info *host_info;
+	dma_addr_t host_info_dma_addr;
+};
+
+/* Each ena_dev is a PCI function. */
+struct ena_com_dev {
+	struct ena_com_admin_queue admin_queue;
+	struct ena_com_aenq aenq;
+	struct ena_com_io_cq io_cq_queues[ENA_TOTAL_NUM_QUEUES];
+	struct ena_com_io_sq io_sq_queues[ENA_TOTAL_NUM_QUEUES];
+	u8 __iomem *reg_bar;
+	void __iomem *mem_bar;
+	void *dmadev;
+
+	enum ena_admin_placement_policy_type tx_mem_queue_type;
+	u32 tx_max_header_size;
+	u16 stats_func; /* Selected function for extended statistic dump */
+	u16 stats_queue; /* Selected queue for extended statistic dump */
+
+	struct ena_com_mmio_read mmio_read;
+
+	struct ena_rss rss;
+	u32 supported_features;
+	u32 dma_addr_bits;
+
+	struct ena_host_attribute host_attr;
+	bool adaptive_coalescing;
+	u16 intr_delay_resolution;
+	u32 intr_moder_tx_interval;
+	struct ena_intr_moder_entry *intr_moder_tbl;
+};
+
+struct ena_com_dev_get_features_ctx {
+	struct ena_admin_queue_feature_desc max_queues;
+	struct ena_admin_device_attr_feature_desc dev_attr;
+	struct ena_admin_feature_aenq_desc aenq;
+	struct ena_admin_feature_offload_desc offload;
+};
+
+struct ena_com_create_io_ctx {
+	enum ena_admin_placement_policy_type mem_queue_type;
+	enum queue_direction direction;
+	int numa_node;
+	u32 msix_vector;
+	u16 queue_size;
+	u16 qid;
+};
+
+typedef void (*ena_aenq_handler)(void *data,
+	struct ena_admin_aenq_entry *aenq_e);
+
+/* Holds aenq handlers. Indexed by AENQ event group */
+struct ena_aenq_handlers {
+	ena_aenq_handler handlers[ENA_MAX_HANDLERS];
+	ena_aenq_handler unimplemented_handler;
+};
+
+/*****************************************************************************/
+/*****************************************************************************/
+
+/* ena_com_mmio_reg_read_request_init - Init the mmio reg read mechanism
+ * @ena_dev: ENA communication layer struct
+ *
+ * Initialize the register read mechanism.
+ *
+ * @note: This method must be the first stage in the initialization sequence.
+ *
+ * @return - 0 on success, negative value on failure.
+ */
+int ena_com_mmio_reg_read_request_init(struct ena_com_dev *ena_dev);
+
+/* ena_com_set_mmio_read_mode - Enable/disable the mmio reg read mechanism
+ * @ena_dev: ENA communication layer struct
+ * @readless_supported: readless mode (enable/disable)
+ */
+void ena_com_set_mmio_read_mode(struct ena_com_dev *ena_dev,
+				bool readless_supported);
+
+/* ena_com_mmio_reg_read_request_write_dev_addr - Write the mmio reg read return
+ * value physical address.
+ * @ena_dev: ENA communication layer struct
+ */
+void ena_com_mmio_reg_read_request_write_dev_addr(struct ena_com_dev *ena_dev);
+
+/* ena_com_mmio_reg_read_request_destroy - Destroy the mmio reg read mechanism
+ * @ena_dev: ENA communication layer struct
+ */
+void ena_com_mmio_reg_read_request_destroy(struct ena_com_dev *ena_dev);
+
+/* ena_com_admin_init - Init the admin and the async queues
+ * @ena_dev: ENA communication layer struct
+ * @aenq_handlers: Those handlers to be called upon event.
+ * @init_spinlock: Indicate if this method should init the admin spinlock or
+ * the spinlock was init before (for example, in a case of FLR).
+ *
+ * Initialize the admin submission and completion queues.
+ * Initialize the asynchronous events notification queues.
+ *
+ * @return - 0 on success, negative value on failure.
+ */
+int ena_com_admin_init(struct ena_com_dev *ena_dev,
+		       struct ena_aenq_handlers *aenq_handlers,
+		       bool init_spinlock);
+
+/* ena_com_admin_destroy - Destroy the admin and the async events queues.
+ * @ena_dev: ENA communication layer struct
+ *
+ * @note: Before calling this method, the caller must validate that the device
+ * won't send any additional admin completions/aenq.
+ * To achieve that, a FLR is recommended.
+ */
+void ena_com_admin_destroy(struct ena_com_dev *ena_dev);
+
+/* ena_com_dev_reset - Perform device FLR to the device.
+ * @ena_dev: ENA communication layer struct
+ *
+ * @return - 0 on success, negative value on failure.
+ */
+int ena_com_dev_reset(struct ena_com_dev *ena_dev);
+
+/* ena_com_create_io_queue - Create io queue.
+ * @ena_dev: ENA communication layer struct
+ * @ctx - create context structure
+ *
+ * Create the submission and the completion queues.
+ *
+ * @return - 0 on success, negative value on failure.
+ */
+int ena_com_create_io_queue(struct ena_com_dev *ena_dev,
+			    struct ena_com_create_io_ctx *ctx);
+
+/* ena_com_destroy_io_queue - Destroy IO queue with the queue id - qid.
+ * @ena_dev: ENA communication layer struct
+ * @qid - the caller virtual queue id.
+ */
+void ena_com_destroy_io_queue(struct ena_com_dev *ena_dev, u16 qid);
+
+/* ena_com_get_io_handlers - Return the io queue handlers
+ * @ena_dev: ENA communication layer struct
+ * @qid - the caller virtual queue id.
+ * @io_sq - IO submission queue handler
+ * @io_cq - IO completion queue handler.
+ *
+ * @return - 0 on success, negative value on failure.
+ */
+int ena_com_get_io_handlers(struct ena_com_dev *ena_dev, u16 qid,
+			    struct ena_com_io_sq **io_sq,
+			    struct ena_com_io_cq **io_cq);
+
+/* ena_com_admin_aenq_enable - ENAble asynchronous event notifications
+ * @ena_dev: ENA communication layer struct
+ *
+ * After this method, aenq event can be received via AENQ.
+ */
+void ena_com_admin_aenq_enable(struct ena_com_dev *ena_dev);
+
+/* ena_com_set_admin_running_state - Set the state of the admin queue
+ * @ena_dev: ENA communication layer struct
+ *
+ * Change the state of the admin queue (enable/disable)
+ */
+void ena_com_set_admin_running_state(struct ena_com_dev *ena_dev, bool state);
+
+/* ena_com_get_admin_running_state - Get the admin queue state
+ * @ena_dev: ENA communication layer struct
+ *
+ * Retrieve the state of the admin queue (enable/disable)
+ *
+ * @return - current polling mode (enable/disable)
+ */
+bool ena_com_get_admin_running_state(struct ena_com_dev *ena_dev);
+
+/* ena_com_set_admin_polling_mode - Set the admin completion queue polling mode
+ * @ena_dev: ENA communication layer struct
+ * @polling: ENAble/Disable polling mode
+ *
+ * Set the admin completion mode.
+ */
+void ena_com_set_admin_polling_mode(struct ena_com_dev *ena_dev, bool polling);
+
+/* ena_com_set_admin_polling_mode - Get the admin completion queue polling mode
+ * @ena_dev: ENA communication layer struct
+ *
+ * Get the admin completion mode.
+ * If polling mode is on, ena_com_execute_admin_command will perform a
+ * polling on the admin completion queue for the commands completion,
+ * otherwise it will wait on wait event.
+ *
+ * @return state
+ */
+bool ena_com_get_ena_admin_polling_mode(struct ena_com_dev *ena_dev);
+
+/* ena_com_admin_q_comp_intr_handler - admin queue interrupt handler
+ * @ena_dev: ENA communication layer struct
+ *
+ * This method go over the admin completion queue and wake up all the pending
+ * threads that wait on the commands wait event.
+ *
+ * @note: Should be called after MSI-X interrupt.
+ */
+void ena_com_admin_q_comp_intr_handler(struct ena_com_dev *ena_dev);
+
+/* ena_com_aenq_intr_handler - AENQ interrupt handler
+ * @ena_dev: ENA communication layer struct
+ *
+ * This method go over the async event notification queue and call the proper
+ * aenq handler.
+ */
+void ena_com_aenq_intr_handler(struct ena_com_dev *dev, void *data);
+
+/* ena_com_abort_admin_commands - Abort all the outstanding admin commands.
+ * @ena_dev: ENA communication layer struct
+ *
+ * This method aborts all the outstanding admin commands.
+ * The caller should then call ena_com_wait_for_abort_completion to make sure
+ * all the commands were completed.
+ */
+void ena_com_abort_admin_commands(struct ena_com_dev *ena_dev);
+
+/* ena_com_wait_for_abort_completion - Wait for admin commands abort.
+ * @ena_dev: ENA communication layer struct
+ *
+ * This method wait until all the outstanding admin commands will be completed.
+ */
+void ena_com_wait_for_abort_completion(struct ena_com_dev *ena_dev);
+
+/* ena_com_validate_version - Validate the device parameters
+ * @ena_dev: ENA communication layer struct
+ *
+ * This method validate the device parameters are the same as the saved
+ * parameters in ena_dev.
+ * This method is useful after device reset, to validate the device mac address
+ * and the device offloads are the same as before the reset.
+ *
+ * @return - 0 on success negative value otherwise.
+ */
+int ena_com_validate_version(struct ena_com_dev *ena_dev);
+
+/* ena_com_get_link_params - Retrieve physical link parameters.
+ * @ena_dev: ENA communication layer struct
+ * @resp: Link parameters
+ *
+ * Retrieve the physical link parameters,
+ * like speed, auto-negotiation and full duplex support.
+ *
+ * @return - 0 on Success negative value otherwise.
+ */
+int ena_com_get_link_params(struct ena_com_dev *ena_dev,
+			    struct ena_admin_get_feat_resp *resp);
+
+/* ena_com_get_dma_width - Retrieve physical dma address width the device
+ * supports.
+ * @ena_dev: ENA communication layer struct
+ *
+ * Retrieve the maximum physical address bits the device can handle.
+ *
+ * @return: > 0 on Success and negative value otherwise.
+ */
+int ena_com_get_dma_width(struct ena_com_dev *ena_dev);
+
+/* ena_com_set_aenq_config - Set aenq groups configurations
+ * @ena_dev: ENA communication layer struct
+ * @groups flag: bit fields flags of enum ena_admin_aenq_group.
+ *
+ * Configure which aenq event group the driver would like to receive.
+ *
+ * @return: 0 on Success and negative value otherwise.
+ */
+int ena_com_set_aenq_config(struct ena_com_dev *ena_dev, u32 groups_flag);
+
+/* ena_com_get_dev_attr_feat - Get device features
+ * @ena_dev: ENA communication layer struct
+ * @get_feat_ctx: returned context that contain the get features.
+ *
+ * @return: 0 on Success and negative value otherwise.
+ */
+int ena_com_get_dev_attr_feat(struct ena_com_dev *ena_dev,
+			      struct ena_com_dev_get_features_ctx *get_feat_ctx);
+
+/* ena_com_get_dev_basic_stats - Get device basic statistics
+ * @ena_dev: ENA communication layer struct
+ * @stats: stats return value
+ *
+ * @return: 0 on Success and negative value otherwise.
+ */
+int ena_com_get_dev_basic_stats(struct ena_com_dev *ena_dev,
+				struct ena_admin_basic_stats *stats);
+
+/* ena_com_set_dev_mtu - Configure the device mtu.
+ * @ena_dev: ENA communication layer struct
+ * @mtu: mtu value
+ *
+ * @return: 0 on Success and negative value otherwise.
+ */
+int ena_com_set_dev_mtu(struct ena_com_dev *ena_dev, int mtu);
+
+/* ena_com_get_offload_settings - Retrieve the device offloads capabilities
+ * @ena_dev: ENA communication layer struct
+ * @offlad: offload return value
+ *
+ * @return: 0 on Success and negative value otherwise.
+ */
+int ena_com_get_offload_settings(struct ena_com_dev *ena_dev,
+				 struct ena_admin_feature_offload_desc *offload);
+
+/* ena_com_rss_init - Init RSS
+ * @ena_dev: ENA communication layer struct
+ * @log_size: indirection log size
+ *
+ * Allocate RSS/RFS resources.
+ * The caller then can configure rss using ena_com_set_hash_function,
+ * ena_com_set_hash_ctrl and ena_com_indirect_table_set.
+ *
+ * @return: 0 on Success and negative value otherwise.
+ */
+int ena_com_rss_init(struct ena_com_dev *ena_dev, u16 log_size);
+
+/* ena_com_rss_destroy - Destroy rss
+ * @ena_dev: ENA communication layer struct
+ *
+ * Free all the RSS/RFS resources.
+ */
+void ena_com_rss_destroy(struct ena_com_dev *ena_dev);
+
+/* ena_com_fill_hash_function - Fill RSS hash function
+ * @ena_dev: ENA communication layer struct
+ * @func: The hash function (Toeplitz or crc)
+ * @key: Hash key (for toeplitz hash)
+ * @key_len: key length (max length 10 DW)
+ * @init_val: initial value for the hash function
+ *
+ * Fill the ena_dev resources with the desire hash function, hash key, key_len
+ * and key initial value (if needed by the hash function).
+ * To flush the key into the device the caller should call
+ * ena_com_set_hash_function.
+ *
+ * @return: 0 on Success and negative value otherwise.
+ */
+int ena_com_fill_hash_function(struct ena_com_dev *ena_dev,
+			       enum ena_admin_hash_functions func,
+			       const u8 *key, u16 key_len, u32 init_val);
+
+/* ena_com_set_hash_function - Flush the hash function and it dependencies to
+ * the device.
+ * @ena_dev: ENA communication layer struct
+ *
+ * Flush the hash function and it dependencies (key, key length and
+ * initial value) if needed.
+ *
+ * @note: Prior to this method the caller should call ena_com_fill_hash_function
+ *
+ * @return: 0 on Success and negative value otherwise.
+ */
+int ena_com_set_hash_function(struct ena_com_dev *ena_dev);
+
+/* ena_com_get_hash_function - Retrieve the hash function and the hash key
+ * from the device.
+ * @ena_dev: ENA communication layer struct
+ * @func: hash function
+ * @key: hash key
+ *
+ * Retrieve the hash function and the hash key from the device.
+ *
+ * @note: If the caller called ena_com_fill_hash_function but didn't flash
+ * it to the device, the new configuration will be lost.
+ *
+ * @return: 0 on Success and negative value otherwise.
+ */
+int ena_com_get_hash_function(struct ena_com_dev *ena_dev,
+			      enum ena_admin_hash_functions *func,
+			      u8 *key);
+
+/* ena_com_fill_hash_ctrl - Fill RSS hash control
+ * @ena_dev: ENA communication layer struct.
+ * @proto: The protocol to configure.
+ * @hash_fields: bit mask of ena_admin_flow_hash_fields
+ *
+ * Fill the ena_dev resources with the desire hash control (the ethernet
+ * fields that take part of the hash) for a specific protocol.
+ * To flush the hash control to the device, the caller should call
+ * ena_com_set_hash_ctrl.
+ *
+ * @return: 0 on Success and negative value otherwise.
+ */
+int ena_com_fill_hash_ctrl(struct ena_com_dev *ena_dev,
+			   enum ena_admin_flow_hash_proto proto,
+			   u16 hash_fields);
+
+/* ena_com_set_hash_ctrl - Flush the hash control resources to the device.
+ * @ena_dev: ENA communication layer struct
+ *
+ * Flush the hash control (the ethernet fields that take part of the hash)
+ *
+ * @note: Prior to this method the caller should call ena_com_fill_hash_ctrl.
+ *
+ * @return: 0 on Success and negative value otherwise.
+ */
+int ena_com_set_hash_ctrl(struct ena_com_dev *ena_dev);
+
+/* ena_com_get_hash_ctrl - Retrieve the hash control from the device.
+ * @ena_dev: ENA communication layer struct
+ * @proto: The protocol to retrieve.
+ * @fields: bit mask of ena_admin_flow_hash_fields.
+ *
+ * Retrieve the hash control from the device.
+ *
+ * @note, If the caller called ena_com_fill_hash_ctrl but didn't flash
+ * it to the device, the new configuration will be lost.
+ *
+ * @return: 0 on Success and negative value otherwise.
+ */
+int ena_com_get_hash_ctrl(struct ena_com_dev *ena_dev,
+			  enum ena_admin_flow_hash_proto proto,
+			  u16 *fields);
+
+/* ena_com_set_default_hash_ctrl - Set the hash control to a default
+ * configuration.
+ * @ena_dev: ENA communication layer struct
+ *
+ * Fill the ena_dev resources with the default hash control configuration.
+ * To flush the hash control to the device, the caller should call
+ * ena_com_set_hash_ctrl.
+ *
+ * @return: 0 on Success and negative value otherwise.
+ */
+int ena_com_set_default_hash_ctrl(struct ena_com_dev *ena_dev);
+
+/* ena_com_indirect_table_fill_entry - Fill a single entry in the RSS
+ * indirection table
+ * @ena_dev: ENA communication layer struct.
+ * @entry_idx - indirection table entry.
+ * @entry_value - redirection value
+ *
+ * Fill a single entry of the RSS indirection table in the ena_dev resources.
+ * To flush the indirection table to the device, the called should call
+ * ena_com_indirect_table_set.
+ *
+ * @return: 0 on Success and negative value otherwise.
+ */
+int ena_com_indirect_table_fill_entry(struct ena_com_dev *ena_dev,
+				      u16 entry_idx, u16 entry_value);
+
+/* ena_com_indirect_table_set - Flush the indirection table to the device.
+ * @ena_dev: ENA communication layer struct
+ *
+ * Flush the indirection hash control to the device.
+ * Prior to this method the caller should call ena_com_indirect_table_fill_entry
+ *
+ * @return: 0 on Success and negative value otherwise.
+ */
+int ena_com_indirect_table_set(struct ena_com_dev *ena_dev);
+
+/* ena_com_indirect_table_get - Retrieve the indirection table from the device.
+ * @ena_dev: ENA communication layer struct
+ * @ind_tbl: indirection table
+ *
+ * Retrieve the RSS indirection table from the device.
+ *
+ * @note: If the caller called ena_com_indirect_table_fill_entry but didn't flash
+ * it to the device, the new configuration will be lost.
+ *
+ * @return: 0 on Success and negative value otherwise.
+ */
+int ena_com_indirect_table_get(struct ena_com_dev *ena_dev, u32 *ind_tbl);
+
+/* ena_com_allocate_host_info - Allocate host info resources.
+ * @ena_dev: ENA communication layer struct
+ *
+ * @return: 0 on Success and negative value otherwise.
+ */
+int ena_com_allocate_host_info(struct ena_com_dev *ena_dev);
+
+/* ena_com_allocate_debug_area - Allocate debug area.
+ * @ena_dev: ENA communication layer struct
+ * @debug_area_size - debug area size.
+ *
+ * @return: 0 on Success and negative value otherwise.
+ */
+int ena_com_allocate_debug_area(struct ena_com_dev *ena_dev,
+				u32 debug_area_size);
+
+/* ena_com_delete_debug_area - Free the debug area resources.
+ * @ena_dev: ENA communication layer struct
+ *
+ * Free the allocate debug area.
+ */
+void ena_com_delete_debug_area(struct ena_com_dev *ena_dev);
+
+/* ena_com_delete_host_info - Free the host info resources.
+ * @ena_dev: ENA communication layer struct
+ *
+ * Free the allocate host info.
+ */
+void ena_com_delete_host_info(struct ena_com_dev *ena_dev);
+
+/* ena_com_set_host_attributes - Update the device with the host
+ * attributes (debug area and host info) base address.
+ * @ena_dev: ENA communication layer struct
+ *
+ * @return: 0 on Success and negative value otherwise.
+ */
+int ena_com_set_host_attributes(struct ena_com_dev *ena_dev);
+
+/* ena_com_create_io_cq - Create io completion queue.
+ * @ena_dev: ENA communication layer struct
+ * @io_cq - io completion queue handler
+
+ * Create IO completion queue.
+ *
+ * @return - 0 on success, negative value on failure.
+ */
+int ena_com_create_io_cq(struct ena_com_dev *ena_dev,
+			 struct ena_com_io_cq *io_cq);
+
+/* ena_com_destroy_io_cq - Destroy io completion queue.
+ * @ena_dev: ENA communication layer struct
+ * @io_cq - io completion queue handler
+
+ * Destroy IO completion queue.
+ *
+ * @return - 0 on success, negative value on failure.
+ */
+int ena_com_destroy_io_cq(struct ena_com_dev *ena_dev,
+			  struct ena_com_io_cq *io_cq);
+
+/* ena_com_execute_admin_command - Execute admin command
+ * @admin_queue: admin queue.
+ * @cmd: the admin command to execute.
+ * @cmd_size: the command size.
+ * @cmd_completion: command completion return value.
+ * @cmd_comp_size: command completion size.
+
+ * Submit an admin command and then wait until the device will return a
+ * completion.
+ * The completion will be copyed into cmd_comp.
+ *
+ * @return - 0 on success, negative value on failure.
+ */
+int ena_com_execute_admin_command(struct ena_com_admin_queue *admin_queue,
+				  struct ena_admin_aq_entry *cmd,
+				  size_t cmd_size,
+				  struct ena_admin_acq_entry *cmd_comp,
+				  size_t cmd_comp_size);
+
+/* ena_com_init_interrupt_moderation - Init interrupt moderation
+ * @ena_dev: ENA communication layer struct
+ *
+ * @return - 0 on success, negative value on failure.
+ */
+int ena_com_init_interrupt_moderation(struct ena_com_dev *ena_dev);
+
+/* ena_com_destroy_interrupt_moderation - Destroy interrupt moderation resources
+ * @ena_dev: ENA communication layer struct
+ */
+void ena_com_destroy_interrupt_moderation(struct ena_com_dev *ena_dev);
+
+/* ena_com_interrupt_moderation_supported - Return if interrupt moderation
+ * capability is supported by the device.
+ *
+ * @return - supported or not.
+ */
+bool ena_com_interrupt_moderation_supported(struct ena_com_dev *ena_dev);
+
+/* ena_com_config_default_interrupt_moderation_table - Restore the interrupt
+ * moderation table back to the default parameters.
+ * @ena_dev: ENA communication layer struct
+ */
+void ena_com_config_default_interrupt_moderation_table(struct ena_com_dev *ena_dev);
+
+/* ena_com_update_nonadaptive_moderation_interval_tx - Update the
+ * non-adaptive interval in Tx direction.
+ * @ena_dev: ENA communication layer struct
+ * @tx_coalesce_usecs: Interval in usec.
+ *
+ * @return - 0 on success, negative value on failure.
+ */
+int ena_com_update_nonadaptive_moderation_interval_tx(struct ena_com_dev *ena_dev,
+						      u32 tx_coalesce_usecs);
+
+/* ena_com_update_nonadaptive_moderation_interval_rx - Update the
+ * non-adaptive interval in Rx direction.
+ * @ena_dev: ENA communication layer struct
+ * @rx_coalesce_usecs: Interval in usec.
+ *
+ * @return - 0 on success, negative value on failure.
+ */
+int ena_com_update_nonadaptive_moderation_interval_rx(struct ena_com_dev *ena_dev,
+						      u32 rx_coalesce_usecs);
+
+/* ena_com_get_nonadaptive_moderation_interval_tx - Retrieve the
+ * non-adaptive interval in Tx direction.
+ * @ena_dev: ENA communication layer struct
+ *
+ * @return - interval in usec
+ */
+unsigned int ena_com_get_nonadaptive_moderation_interval_tx(struct ena_com_dev *ena_dev);
+
+/* ena_com_get_nonadaptive_moderation_interval_rx - Retrieve the
+ * non-adaptive interval in Rx direction.
+ * @ena_dev: ENA communication layer struct
+ *
+ * @return - interval in usec
+ */
+unsigned int ena_com_get_nonadaptive_moderation_interval_rx(struct ena_com_dev *ena_dev);
+
+/* ena_com_init_intr_moderation_entry - Update a single entry in the interrupt
+ * moderation table.
+ * @ena_dev: ENA communication layer struct
+ * @level: Interrupt moderation table level
+ * @entry: Entry value
+ *
+ * Update a single entry in the interrupt moderation table.
+ */
+void ena_com_init_intr_moderation_entry(struct ena_com_dev *ena_dev,
+					enum ena_intr_moder_level level,
+					struct ena_intr_moder_entry *entry);
+
+/* ena_com_get_intr_moderation_entry - Init ena_intr_moder_entry.
+ * @ena_dev: ENA communication layer struct
+ * @level: Interrupt moderation table level
+ * @entry: Entry to fill.
+ *
+ * Initialize the entry according to the adaptive interrupt moderation table.
+ */
+void ena_com_get_intr_moderation_entry(struct ena_com_dev *ena_dev,
+				       enum ena_intr_moder_level level,
+				       struct ena_intr_moder_entry *entry);
+
+static inline bool ena_com_get_adaptive_moderation_enabled(struct ena_com_dev *ena_dev)
+{
+	return ena_dev->adaptive_coalescing;
+}
+
+static inline void ena_com_enable_adaptive_moderation(struct ena_com_dev *ena_dev)
+{
+	ena_dev->adaptive_coalescing = true;
+}
+
+static inline void ena_com_disable_adaptive_moderation(struct ena_com_dev *ena_dev)
+{
+	ena_dev->adaptive_coalescing = false;
+}
+
+/* ena_com_calculate_interrupt_delay - Calculate new interrupt delay
+ * @ena_dev: ENA communication layer struct
+ * @pkts: Number of packets since the last update
+ * @bytes: Number of bytes received since the last update.
+ * @smoothed_interval: Returned interval
+ * @moder_tbl_idx: Current table level as input update new level as return
+ * value.
+ */
+static inline void ena_com_calculate_interrupt_delay(struct ena_com_dev *ena_dev,
+						     unsigned int pkts,
+						     unsigned int bytes,
+						     unsigned int *smoothed_interval,
+						     unsigned int *moder_tbl_idx)
+{
+	enum ena_intr_moder_level curr_moder_idx, new_moder_idx;
+	struct ena_intr_moder_entry *curr_moder_entry;
+	struct ena_intr_moder_entry *pred_moder_entry;
+	struct ena_intr_moder_entry *new_moder_entry;
+	struct ena_intr_moder_entry *intr_moder_tbl = ena_dev->intr_moder_tbl;
+	unsigned int interval;
+
+	/* We apply adaptive moderation on Rx path only.
+	 * Tx uses static interrupt moderation.
+	 */
+	if (!pkts || !bytes)
+		/* Tx interrupt, or spurious interrupt,
+		 * in both cases we just use same delay values
+		 */
+		return;
+
+	curr_moder_idx = (enum ena_intr_moder_level)(*moder_tbl_idx);
+	if (unlikely(curr_moder_idx >= ENA_INTR_MAX_NUM_OF_LEVELS)) {
+		pr_err("Wrong moderation index %u\n", curr_moder_idx);
+		return;
+	}
+
+	curr_moder_entry = &intr_moder_tbl[curr_moder_idx];
+	new_moder_idx = curr_moder_idx;
+
+	if (curr_moder_idx == ENA_INTR_MODER_LOWEST) {
+		if ((pkts > curr_moder_entry->pkts_per_interval) ||
+		    (bytes > curr_moder_entry->bytes_per_interval))
+			new_moder_idx =
+				(enum ena_intr_moder_level)(curr_moder_idx + ENA_INTR_MODER_LEVEL_STRIDE);
+	} else {
+		pred_moder_entry = &intr_moder_tbl[curr_moder_idx - ENA_INTR_MODER_LEVEL_STRIDE];
+
+		if ((pkts <= pred_moder_entry->pkts_per_interval) ||
+		    (bytes <= pred_moder_entry->bytes_per_interval))
+			new_moder_idx =
+				(enum ena_intr_moder_level)(curr_moder_idx - ENA_INTR_MODER_LEVEL_STRIDE);
+		else if ((pkts > curr_moder_entry->pkts_per_interval) ||
+			 (bytes > curr_moder_entry->bytes_per_interval)) {
+			if (curr_moder_idx != ENA_INTR_MODER_HIGHEST)
+				new_moder_idx =
+					(enum ena_intr_moder_level)(curr_moder_idx + ENA_INTR_MODER_LEVEL_STRIDE);
+		}
+	}
+	new_moder_entry = &intr_moder_tbl[new_moder_idx];
+
+	interval = new_moder_entry->intr_moder_interval;
+	*smoothed_interval = (
+		(interval * ENA_INTR_DELAY_NEW_VALUE_WEIGHT +
+		ENA_INTR_DELAY_OLD_VALUE_WEIGHT * (*smoothed_interval)) + 5) /
+		10;
+
+	*moder_tbl_idx = new_moder_idx;
+}
+
+/* ena_com_update_intr_reg - Prepare interrupt register
+ * @intr_reg: interrupt register to update.
+ * @rx_delay_interval: Rx interval in usecs
+ * @tx_delay_interval: Tx interval in usecs
+ * @unmask: unask enable/disable
+ *
+ * Prepare interrupt update register with the supplied parameters.
+ */
+static inline void ena_com_update_intr_reg(struct ena_eth_io_intr_reg *intr_reg,
+					   u32 rx_delay_interval,
+					   u32 tx_delay_interval,
+					   bool unmask)
+{
+	intr_reg->intr_control = 0;
+	intr_reg->intr_control |= rx_delay_interval &
+		ENA_ETH_IO_INTR_REG_RX_INTR_DELAY_MASK;
+
+	intr_reg->intr_control |=
+		(tx_delay_interval << ENA_ETH_IO_INTR_REG_TX_INTR_DELAY_SHIFT)
+		& ENA_ETH_IO_INTR_REG_TX_INTR_DELAY_MASK;
+
+	if (unmask)
+		intr_reg->intr_control |= ENA_ETH_IO_INTR_REG_INTR_UNMASK_MASK;
+}
+
+#endif /* !(ENA_COM) */
diff --git a/drivers/net/ethernet/amazon/ena/ena_common_defs.h b/drivers/net/ethernet/amazon/ena/ena_common_defs.h
new file mode 100644
index 000000000000..bb8d73676eab
--- /dev/null
+++ b/drivers/net/ethernet/amazon/ena/ena_common_defs.h
@@ -0,0 +1,48 @@
+/*
+ * Copyright 2015 - 2016 Amazon.com, Inc. or its affiliates.
+ *
+ * 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
+ * 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 _ENA_COMMON_H_
+#define _ENA_COMMON_H_
+
+#define ENA_COMMON_SPEC_VERSION_MAJOR	0 /*  */
+#define ENA_COMMON_SPEC_VERSION_MINOR	10 /*  */
+
+/* ENA operates with 48-bit memory addresses. ena_mem_addr_t */
+struct ena_common_mem_addr {
+	u32 mem_addr_low;
+
+	u16 mem_addr_high;
+
+	/* MBZ */
+	u16 reserved16;
+};
+
+#endif /*_ENA_COMMON_H_ */
diff --git a/drivers/net/ethernet/amazon/ena/ena_eth_com.c b/drivers/net/ethernet/amazon/ena/ena_eth_com.c
new file mode 100644
index 000000000000..539c536464a5
--- /dev/null
+++ b/drivers/net/ethernet/amazon/ena/ena_eth_com.c
@@ -0,0 +1,501 @@
+/*
+ * Copyright 2015 Amazon.com, Inc. or its affiliates.
+ *
+ * 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
+ * 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 "ena_eth_com.h"
+
+static inline struct ena_eth_io_rx_cdesc_base *ena_com_get_next_rx_cdesc(
+	struct ena_com_io_cq *io_cq)
+{
+	struct ena_eth_io_rx_cdesc_base *cdesc;
+	u16 expected_phase, head_masked;
+	u16 desc_phase;
+
+	head_masked = io_cq->head & (io_cq->q_depth - 1);
+	expected_phase = io_cq->phase;
+
+	cdesc = (struct ena_eth_io_rx_cdesc_base *)(io_cq->cdesc_addr.virt_addr
+			+ (head_masked * io_cq->cdesc_entry_size_in_bytes));
+
+	desc_phase = (cdesc->status & ENA_ETH_IO_RX_CDESC_BASE_PHASE_MASK) >>
+			ENA_ETH_IO_RX_CDESC_BASE_PHASE_SHIFT;
+
+	if (desc_phase != expected_phase)
+		return NULL;
+
+	return cdesc;
+}
+
+static inline void ena_com_cq_inc_head(struct ena_com_io_cq *io_cq)
+{
+	io_cq->head++;
+
+	/* Switch phase bit in case of wrap around */
+	if (unlikely((io_cq->head & (io_cq->q_depth - 1)) == 0))
+		io_cq->phase ^= 1;
+}
+
+static inline void *get_sq_desc(struct ena_com_io_sq *io_sq)
+{
+	u16 tail_masked;
+	u32 offset;
+
+	tail_masked = io_sq->tail & (io_sq->q_depth - 1);
+
+	offset = tail_masked * io_sq->desc_entry_size;
+
+	return (void *)((uintptr_t)io_sq->desc_addr.virt_addr + offset);
+}
+
+static inline void ena_com_copy_curr_sq_desc_to_dev(struct ena_com_io_sq *io_sq)
+{
+	u16 tail_masked = io_sq->tail & (io_sq->q_depth - 1);
+	u32 offset = tail_masked * io_sq->desc_entry_size;
+
+	/* In case this queue isn't a LLQ */
+	if (io_sq->mem_queue_type == ENA_ADMIN_PLACEMENT_POLICY_HOST)
+		return;
+
+	memcpy_toio(io_sq->desc_addr.pbuf_dev_addr + offset,
+		    io_sq->desc_addr.virt_addr + offset,
+		    io_sq->desc_entry_size);
+}
+
+static inline void ena_com_sq_update_tail(struct ena_com_io_sq *io_sq)
+{
+	io_sq->tail++;
+
+	/* Switch phase bit in case of wrap around */
+	if (unlikely((io_sq->tail & (io_sq->q_depth - 1)) == 0))
+		io_sq->phase ^= 1;
+}
+
+static inline int ena_com_write_header(struct ena_com_io_sq *io_sq,
+				       u8 *head_src, u16 header_len)
+{
+	u16 tail_masked = io_sq->tail & (io_sq->q_depth - 1);
+	u8 __iomem *dev_head_addr =
+		io_sq->header_addr + (tail_masked * io_sq->tx_max_header_size);
+
+	if (io_sq->mem_queue_type == ENA_ADMIN_PLACEMENT_POLICY_HOST)
+		return 0;
+
+	if (unlikely(!io_sq->header_addr)) {
+		pr_err("Push buffer header ptr is NULL\n");
+		return -EINVAL;
+	}
+
+	memcpy_toio(dev_head_addr, head_src, header_len);
+
+	return 0;
+}
+
+static inline struct ena_eth_io_rx_cdesc_base *
+	ena_com_rx_cdesc_idx_to_ptr(struct ena_com_io_cq *io_cq, u16 idx)
+{
+	idx &= (io_cq->q_depth - 1);
+	return (struct ena_eth_io_rx_cdesc_base *)
+		((uintptr_t)io_cq->cdesc_addr.virt_addr +
+		idx * io_cq->cdesc_entry_size_in_bytes);
+}
+
+static inline u16 ena_com_cdesc_rx_pkt_get(struct ena_com_io_cq *io_cq,
+					   u16 *first_cdesc_idx)
+{
+	struct ena_eth_io_rx_cdesc_base *cdesc;
+	u16 count = 0, head_masked;
+	u32 last = 0;
+
+	do {
+		cdesc = ena_com_get_next_rx_cdesc(io_cq);
+		if (!cdesc)
+			break;
+
+		ena_com_cq_inc_head(io_cq);
+		count++;
+		last = (cdesc->status & ENA_ETH_IO_RX_CDESC_BASE_LAST_MASK) >>
+			ENA_ETH_IO_RX_CDESC_BASE_LAST_SHIFT;
+	} while (!last);
+
+	if (last) {
+		*first_cdesc_idx = io_cq->cur_rx_pkt_cdesc_start_idx;
+		count += io_cq->cur_rx_pkt_cdesc_count;
+
+		head_masked = io_cq->head & (io_cq->q_depth - 1);
+
+		io_cq->cur_rx_pkt_cdesc_count = 0;
+		io_cq->cur_rx_pkt_cdesc_start_idx = head_masked;
+
+		pr_debug("ena q_id: %d packets were completed. first desc idx %u descs# %d\n",
+			 io_cq->qid, *first_cdesc_idx, count);
+	} else {
+		io_cq->cur_rx_pkt_cdesc_count += count;
+		count = 0;
+	}
+
+	return count;
+}
+
+static inline bool ena_com_meta_desc_changed(struct ena_com_io_sq *io_sq,
+					     struct ena_com_tx_ctx *ena_tx_ctx)
+{
+	int rc;
+
+	if (ena_tx_ctx->meta_valid) {
+		rc = memcmp(&io_sq->cached_tx_meta,
+			    &ena_tx_ctx->ena_meta,
+			    sizeof(struct ena_com_tx_meta));
+
+		if (unlikely(rc != 0))
+			return true;
+	}
+
+	return false;
+}
+
+static inline void ena_com_create_and_store_tx_meta_desc(struct ena_com_io_sq *io_sq,
+							 struct ena_com_tx_ctx *ena_tx_ctx)
+{
+	struct ena_eth_io_tx_meta_desc *meta_desc = NULL;
+	struct ena_com_tx_meta *ena_meta = &ena_tx_ctx->ena_meta;
+
+	meta_desc = get_sq_desc(io_sq);
+	memset(meta_desc, 0x0, sizeof(struct ena_eth_io_tx_meta_desc));
+
+	meta_desc->len_ctrl |= ENA_ETH_IO_TX_META_DESC_META_DESC_MASK;
+
+	meta_desc->len_ctrl |= ENA_ETH_IO_TX_META_DESC_EXT_VALID_MASK;
+
+	/* bits 0-9 of the mss */
+	meta_desc->word2 |= (ena_meta->mss <<
+		ENA_ETH_IO_TX_META_DESC_MSS_LO_SHIFT) &
+		ENA_ETH_IO_TX_META_DESC_MSS_LO_MASK;
+	/* bits 10-13 of the mss */
+	meta_desc->len_ctrl |= ((ena_meta->mss >> 10) <<
+		ENA_ETH_IO_TX_META_DESC_MSS_HI_SHIFT) &
+		ENA_ETH_IO_TX_META_DESC_MSS_HI_MASK;
+
+	/* Extended meta desc */
+	meta_desc->len_ctrl |= ENA_ETH_IO_TX_META_DESC_ETH_META_TYPE_MASK;
+	meta_desc->len_ctrl |= ENA_ETH_IO_TX_META_DESC_META_STORE_MASK;
+	meta_desc->len_ctrl |= (io_sq->phase <<
+		ENA_ETH_IO_TX_META_DESC_PHASE_SHIFT) &
+		ENA_ETH_IO_TX_META_DESC_PHASE_MASK;
+
+	meta_desc->len_ctrl |= ENA_ETH_IO_TX_META_DESC_FIRST_MASK;
+	meta_desc->word2 |= ena_meta->l3_hdr_len &
+		ENA_ETH_IO_TX_META_DESC_L3_HDR_LEN_MASK;
+	meta_desc->word2 |= (ena_meta->l3_hdr_offset <<
+		ENA_ETH_IO_TX_META_DESC_L3_HDR_OFF_SHIFT) &
+		ENA_ETH_IO_TX_META_DESC_L3_HDR_OFF_MASK;
+
+	meta_desc->word2 |= (ena_meta->l4_hdr_len <<
+		ENA_ETH_IO_TX_META_DESC_L4_HDR_LEN_IN_WORDS_SHIFT) &
+		ENA_ETH_IO_TX_META_DESC_L4_HDR_LEN_IN_WORDS_MASK;
+
+	meta_desc->len_ctrl |= ENA_ETH_IO_TX_META_DESC_META_STORE_MASK;
+
+	/* Cached the meta desc */
+	memcpy(&io_sq->cached_tx_meta, ena_meta,
+	       sizeof(struct ena_com_tx_meta));
+
+	ena_com_copy_curr_sq_desc_to_dev(io_sq);
+	ena_com_sq_update_tail(io_sq);
+}
+
+static inline void ena_com_rx_set_flags(struct ena_com_rx_ctx *ena_rx_ctx,
+					struct ena_eth_io_rx_cdesc_base *cdesc)
+{
+	ena_rx_ctx->l3_proto = cdesc->status &
+		ENA_ETH_IO_RX_CDESC_BASE_L3_PROTO_IDX_MASK;
+	ena_rx_ctx->l4_proto =
+		(cdesc->status & ENA_ETH_IO_RX_CDESC_BASE_L4_PROTO_IDX_MASK) >>
+		ENA_ETH_IO_RX_CDESC_BASE_L4_PROTO_IDX_SHIFT;
+	ena_rx_ctx->l3_csum_err =
+		(cdesc->status & ENA_ETH_IO_RX_CDESC_BASE_L3_CSUM_ERR_MASK) >>
+		ENA_ETH_IO_RX_CDESC_BASE_L3_CSUM_ERR_SHIFT;
+	ena_rx_ctx->l4_csum_err =
+		(cdesc->status & ENA_ETH_IO_RX_CDESC_BASE_L4_CSUM_ERR_MASK) >>
+		ENA_ETH_IO_RX_CDESC_BASE_L4_CSUM_ERR_SHIFT;
+	ena_rx_ctx->hash = cdesc->hash;
+	ena_rx_ctx->frag =
+		(cdesc->status & ENA_ETH_IO_RX_CDESC_BASE_IPV4_FRAG_MASK) >>
+		ENA_ETH_IO_RX_CDESC_BASE_IPV4_FRAG_SHIFT;
+
+	pr_debug("ena_rx_ctx->l3_proto %d ena_rx_ctx->l4_proto %d\nena_rx_ctx->l3_csum_err %d ena_rx_ctx->l4_csum_err %d\nhash frag %d frag: %d cdesc_status: %x\n",
+		 ena_rx_ctx->l3_proto, ena_rx_ctx->l4_proto,
+		 ena_rx_ctx->l3_csum_err, ena_rx_ctx->l4_csum_err,
+		 ena_rx_ctx->hash, ena_rx_ctx->frag, cdesc->status);
+}
+
+/*****************************************************************************/
+/*****************************     API      **********************************/
+/*****************************************************************************/
+
+int ena_com_prepare_tx(struct ena_com_io_sq *io_sq,
+		       struct ena_com_tx_ctx *ena_tx_ctx,
+		       int *nb_hw_desc)
+{
+	struct ena_eth_io_tx_desc *desc = NULL;
+	struct ena_com_buf *ena_bufs = ena_tx_ctx->ena_bufs;
+	void *push_header = ena_tx_ctx->push_header;
+	u16 header_len = ena_tx_ctx->header_len;
+	u16 num_bufs = ena_tx_ctx->num_bufs;
+	int total_desc, i, rc;
+	bool have_meta;
+	u64 addr_hi;
+
+	WARN(io_sq->direction != ENA_COM_IO_QUEUE_DIRECTION_TX, "wrong Q type");
+
+	/* num_bufs +1 for potential meta desc */
+	if (ena_com_sq_empty_space(io_sq) < (num_bufs + 1)) {
+		pr_err("Not enough space in the tx queue\n");
+		return -ENOMEM;
+	}
+
+	if (unlikely(header_len > io_sq->tx_max_header_size)) {
+		pr_err("header size is too large %d max header: %d\n",
+		       header_len, io_sq->tx_max_header_size);
+		return -EINVAL;
+	}
+
+	/* start with pushing the header (if needed) */
+	rc = ena_com_write_header(io_sq, push_header, header_len);
+	if (unlikely(rc))
+		return rc;
+
+	have_meta = ena_tx_ctx->meta_valid && ena_com_meta_desc_changed(io_sq,
+			ena_tx_ctx);
+	if (have_meta)
+		ena_com_create_and_store_tx_meta_desc(io_sq, ena_tx_ctx);
+
+	/* If the caller doesn't want send packets */
+	if (unlikely(!num_bufs && !header_len)) {
+		*nb_hw_desc = have_meta ? 0 : 1;
+		return 0;
+	}
+
+	desc = get_sq_desc(io_sq);
+	memset(desc, 0x0, sizeof(struct ena_eth_io_tx_desc));
+
+	/* Set first desc when we don't have meta descriptor */
+	if (!have_meta)
+		desc->len_ctrl |= ENA_ETH_IO_TX_DESC_FIRST_MASK;
+
+	desc->buff_addr_hi_hdr_sz |= (header_len <<
+		ENA_ETH_IO_TX_DESC_HEADER_LENGTH_SHIFT) &
+		ENA_ETH_IO_TX_DESC_HEADER_LENGTH_MASK;
+	desc->len_ctrl |= (io_sq->phase << ENA_ETH_IO_TX_DESC_PHASE_SHIFT) &
+		ENA_ETH_IO_TX_DESC_PHASE_MASK;
+
+	desc->len_ctrl |= ENA_ETH_IO_TX_DESC_COMP_REQ_MASK;
+
+	/* Bits 0-9 */
+	desc->meta_ctrl |= (ena_tx_ctx->req_id <<
+		ENA_ETH_IO_TX_DESC_REQ_ID_LO_SHIFT) &
+		ENA_ETH_IO_TX_DESC_REQ_ID_LO_MASK;
+
+	desc->meta_ctrl |= (ena_tx_ctx->df <<
+		ENA_ETH_IO_TX_DESC_DF_SHIFT) &
+		ENA_ETH_IO_TX_DESC_DF_MASK;
+
+	/* Bits 10-15 */
+	desc->len_ctrl |= ((ena_tx_ctx->req_id >> 10) <<
+		ENA_ETH_IO_TX_DESC_REQ_ID_HI_SHIFT) &
+		ENA_ETH_IO_TX_DESC_REQ_ID_HI_MASK;
+
+	if (ena_tx_ctx->meta_valid) {
+		desc->meta_ctrl |= (ena_tx_ctx->tso_enable <<
+			ENA_ETH_IO_TX_DESC_TSO_EN_SHIFT) &
+			ENA_ETH_IO_TX_DESC_TSO_EN_MASK;
+		desc->meta_ctrl |= ena_tx_ctx->l3_proto &
+			ENA_ETH_IO_TX_DESC_L3_PROTO_IDX_MASK;
+		desc->meta_ctrl |= (ena_tx_ctx->l4_proto <<
+			ENA_ETH_IO_TX_DESC_L4_PROTO_IDX_SHIFT) &
+			ENA_ETH_IO_TX_DESC_L4_PROTO_IDX_MASK;
+		desc->meta_ctrl |= (ena_tx_ctx->l3_csum_enable <<
+			ENA_ETH_IO_TX_DESC_L3_CSUM_EN_SHIFT) &
+			ENA_ETH_IO_TX_DESC_L3_CSUM_EN_MASK;
+		desc->meta_ctrl |= (ena_tx_ctx->l4_csum_enable <<
+			ENA_ETH_IO_TX_DESC_L4_CSUM_EN_SHIFT) &
+			ENA_ETH_IO_TX_DESC_L4_CSUM_EN_MASK;
+		desc->meta_ctrl |= (ena_tx_ctx->l4_csum_partial <<
+			ENA_ETH_IO_TX_DESC_L4_CSUM_PARTIAL_SHIFT) &
+			ENA_ETH_IO_TX_DESC_L4_CSUM_PARTIAL_MASK;
+	}
+
+	for (i = 0; i < num_bufs; i++) {
+		/* The first desc share the same desc as the header */
+		if (likely(i != 0)) {
+			ena_com_copy_curr_sq_desc_to_dev(io_sq);
+			ena_com_sq_update_tail(io_sq);
+
+			desc = get_sq_desc(io_sq);
+			memset(desc, 0x0, sizeof(struct ena_eth_io_tx_desc));
+
+			desc->len_ctrl |= (io_sq->phase <<
+				ENA_ETH_IO_TX_DESC_PHASE_SHIFT) &
+				ENA_ETH_IO_TX_DESC_PHASE_MASK;
+		}
+
+		desc->len_ctrl |= ena_bufs->len &
+			ENA_ETH_IO_TX_DESC_LENGTH_MASK;
+
+		addr_hi = ((ena_bufs->paddr &
+			GENMASK_ULL(io_sq->dma_addr_bits - 1, 32)) >> 32);
+
+		desc->buff_addr_lo = (u32)ena_bufs->paddr;
+		desc->buff_addr_hi_hdr_sz |= addr_hi &
+			ENA_ETH_IO_TX_DESC_ADDR_HI_MASK;
+		ena_bufs++;
+	}
+
+	/* set the last desc indicator */
+	desc->len_ctrl |= ENA_ETH_IO_TX_DESC_LAST_MASK;
+
+	ena_com_copy_curr_sq_desc_to_dev(io_sq);
+
+	ena_com_sq_update_tail(io_sq);
+
+	total_desc = max_t(u16, num_bufs, 1);
+	total_desc += have_meta ? 1 : 0;
+
+	*nb_hw_desc = total_desc;
+	return 0;
+}
+
+int ena_com_rx_pkt(struct ena_com_io_cq *io_cq,
+		   struct ena_com_io_sq *io_sq,
+		   struct ena_com_rx_ctx *ena_rx_ctx)
+{
+	struct ena_com_rx_buf_info *ena_buf = &ena_rx_ctx->ena_bufs[0];
+	struct ena_eth_io_rx_cdesc_base *cdesc = NULL;
+	u16 cdesc_idx = 0;
+	u16 nb_hw_desc;
+	u16 i;
+
+	WARN(io_cq->direction != ENA_COM_IO_QUEUE_DIRECTION_RX, "wrong Q type");
+
+	nb_hw_desc = ena_com_cdesc_rx_pkt_get(io_cq, &cdesc_idx);
+	if (nb_hw_desc == 0) {
+		ena_rx_ctx->descs = nb_hw_desc;
+		return 0;
+	}
+
+	pr_debug("fetch rx packet: queue %d completed desc: %d\n", io_cq->qid,
+		 nb_hw_desc);
+
+	if (unlikely(nb_hw_desc > ena_rx_ctx->max_bufs)) {
+		pr_err("Too many RX cdescs (%d) > MAX(%d)\n", nb_hw_desc,
+		       ena_rx_ctx->max_bufs);
+		return -ENOSPC;
+	}
+
+	for (i = 0; i < nb_hw_desc; i++) {
+		cdesc = ena_com_rx_cdesc_idx_to_ptr(io_cq, cdesc_idx + i);
+
+		ena_buf->len = cdesc->length;
+		ena_buf->req_id = cdesc->req_id;
+		ena_buf++;
+	}
+
+	/* Update SQ head ptr */
+	io_sq->next_to_comp += nb_hw_desc;
+
+	pr_debug("[%s][QID#%d] Updating SQ head to: %d\n", __func__, io_sq->qid,
+		 io_sq->next_to_comp);
+
+	/* Get rx flags from the last pkt */
+	ena_com_rx_set_flags(ena_rx_ctx, cdesc);
+
+	ena_rx_ctx->descs = nb_hw_desc;
+	return 0;
+}
+
+int ena_com_add_single_rx_desc(struct ena_com_io_sq *io_sq,
+			       struct ena_com_buf *ena_buf,
+			       u16 req_id)
+{
+	struct ena_eth_io_rx_desc *desc;
+
+	WARN(io_sq->direction != ENA_COM_IO_QUEUE_DIRECTION_RX, "wrong Q type");
+
+	if (unlikely(ena_com_sq_empty_space(io_sq) == 0))
+		return -ENOSPC;
+
+	desc = get_sq_desc(io_sq);
+	memset(desc, 0x0, sizeof(struct ena_eth_io_rx_desc));
+
+	desc->length = ena_buf->len;
+
+	desc->ctrl |= ENA_ETH_IO_RX_DESC_FIRST_MASK;
+	desc->ctrl |= ENA_ETH_IO_RX_DESC_LAST_MASK;
+	desc->ctrl |= io_sq->phase & ENA_ETH_IO_RX_DESC_PHASE_MASK;
+	desc->ctrl |= ENA_ETH_IO_RX_DESC_COMP_REQ_MASK;
+
+	desc->req_id = req_id;
+
+	desc->buff_addr_lo = (u32)ena_buf->paddr;
+	desc->buff_addr_hi =
+		((ena_buf->paddr & GENMASK_ULL(io_sq->dma_addr_bits - 1, 32)) >> 32);
+
+	ena_com_sq_update_tail(io_sq);
+
+	return 0;
+}
+
+int ena_com_tx_comp_req_id_get(struct ena_com_io_cq *io_cq, u16 *req_id)
+{
+	u8 expected_phase, cdesc_phase;
+	struct ena_eth_io_tx_cdesc *cdesc;
+	u16 masked_head;
+
+	masked_head = io_cq->head & (io_cq->q_depth - 1);
+	expected_phase = io_cq->phase;
+
+	cdesc = (struct ena_eth_io_tx_cdesc *)
+		((uintptr_t)io_cq->cdesc_addr.virt_addr +
+		(masked_head * io_cq->cdesc_entry_size_in_bytes));
+
+	/* When the current completion descriptor phase isn't the same as the
+	 * expected, it mean that the device still didn't update
+	 * this completion.
+	 */
+	cdesc_phase = cdesc->flags & ENA_ETH_IO_TX_CDESC_PHASE_MASK;
+	if (cdesc_phase != expected_phase)
+		return -EAGAIN;
+
+	ena_com_cq_inc_head(io_cq);
+
+	*req_id = cdesc->req_id;
+
+	return 0;
+}
diff --git a/drivers/net/ethernet/amazon/ena/ena_eth_com.h b/drivers/net/ethernet/amazon/ena/ena_eth_com.h
new file mode 100644
index 000000000000..bb53c3a4f8e9
--- /dev/null
+++ b/drivers/net/ethernet/amazon/ena/ena_eth_com.h
@@ -0,0 +1,160 @@
+/*
+ * Copyright 2015 Amazon.com, Inc. or its affiliates.
+ *
+ * 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
+ * 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 ENA_ETH_COM_H_
+#define ENA_ETH_COM_H_
+
+#include "ena_com.h"
+
+/* head update threshold in units of (queue size / ENA_COMP_HEAD_THRESH) */
+#define ENA_COMP_HEAD_THRESH 4
+
+struct ena_com_tx_ctx {
+	struct ena_com_tx_meta ena_meta;
+	struct ena_com_buf *ena_bufs;
+	/* For LLQ, header buffer - pushed to the device mem space */
+	void *push_header;
+
+	enum ena_eth_io_l3_proto_index l3_proto;
+	enum ena_eth_io_l4_proto_index l4_proto;
+	u16 num_bufs;
+	u16 req_id;
+	/* For regular queue, indicate the size of the header
+	 * For LLQ, indicate the size of the pushed buffer
+	 */
+	u16 header_len;
+
+	u8 meta_valid;
+	u8 tso_enable;
+	u8 l3_csum_enable;
+	u8 l4_csum_enable;
+	u8 l4_csum_partial;
+	u8 df; /* Don't fragment */
+};
+
+struct ena_com_rx_ctx {
+	struct ena_com_rx_buf_info *ena_bufs;
+	enum ena_eth_io_l3_proto_index l3_proto;
+	enum ena_eth_io_l4_proto_index l4_proto;
+	bool l3_csum_err;
+	bool l4_csum_err;
+	/* fragmented packet */
+	bool frag;
+	u32 hash;
+	u16 descs;
+	int max_bufs;
+};
+
+int ena_com_prepare_tx(struct ena_com_io_sq *io_sq,
+		       struct ena_com_tx_ctx *ena_tx_ctx,
+		       int *nb_hw_desc);
+
+int ena_com_rx_pkt(struct ena_com_io_cq *io_cq,
+		   struct ena_com_io_sq *io_sq,
+		   struct ena_com_rx_ctx *ena_rx_ctx);
+
+int ena_com_add_single_rx_desc(struct ena_com_io_sq *io_sq,
+			       struct ena_com_buf *ena_buf,
+			       u16 req_id);
+
+int ena_com_tx_comp_req_id_get(struct ena_com_io_cq *io_cq, u16 *req_id);
+
+static inline void ena_com_unmask_intr(struct ena_com_io_cq *io_cq,
+				       struct ena_eth_io_intr_reg *intr_reg)
+{
+	writel(intr_reg->intr_control, io_cq->unmask_reg);
+}
+
+static inline int ena_com_sq_empty_space(struct ena_com_io_sq *io_sq)
+{
+	u16 tail, next_to_comp, cnt;
+
+	next_to_comp = io_sq->next_to_comp;
+	tail = io_sq->tail;
+	cnt = tail - next_to_comp;
+
+	return io_sq->q_depth - 1 - cnt;
+}
+
+static inline int ena_com_write_sq_doorbell(struct ena_com_io_sq *io_sq)
+{
+	u16 tail;
+
+	tail = io_sq->tail;
+
+	pr_debug("write submission queue doorbell for queue: %d tail: %d\n",
+		 io_sq->qid, tail);
+
+	writel(tail, io_sq->db_addr);
+
+	return 0;
+}
+
+static inline int ena_com_update_dev_comp_head(struct ena_com_io_cq *io_cq)
+{
+	u16 unreported_comp, head;
+	bool need_update;
+
+	head = io_cq->head;
+	unreported_comp = head - io_cq->last_head_update;
+	need_update = unreported_comp > (io_cq->q_depth / ENA_COMP_HEAD_THRESH);
+
+	if (io_cq->cq_head_db_reg && need_update) {
+		pr_debug("Write completion queue doorbell for queue %d: head: %d\n",
+			 io_cq->qid, head);
+		writel(head, io_cq->cq_head_db_reg);
+		io_cq->last_head_update = head;
+	}
+
+	return 0;
+}
+
+static inline void ena_com_update_numa_node(struct ena_com_io_cq *io_cq,
+					    u8 numa_node)
+{
+	struct ena_eth_io_numa_node_cfg_reg numa_cfg;
+
+	if (!io_cq->numa_node_cfg_reg)
+		return;
+
+	numa_cfg.numa_cfg = (numa_node & ENA_ETH_IO_NUMA_NODE_CFG_REG_NUMA_MASK)
+		| ENA_ETH_IO_NUMA_NODE_CFG_REG_ENABLED_MASK;
+
+	writel(numa_cfg.numa_cfg, io_cq->numa_node_cfg_reg);
+}
+
+static inline void ena_com_comp_ack(struct ena_com_io_sq *io_sq, u16 elem)
+{
+	io_sq->next_to_comp += elem;
+}
+
+#endif /* ENA_ETH_COM_H_ */
diff --git a/drivers/net/ethernet/amazon/ena/ena_eth_io_defs.h b/drivers/net/ethernet/amazon/ena/ena_eth_io_defs.h
new file mode 100644
index 000000000000..f320c58793a5
--- /dev/null
+++ b/drivers/net/ethernet/amazon/ena/ena_eth_io_defs.h
@@ -0,0 +1,416 @@
+/*
+ * Copyright 2015 - 2016 Amazon.com, Inc. or its affiliates.
+ *
+ * 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
+ * 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 _ENA_ETH_IO_H_
+#define _ENA_ETH_IO_H_
+
+enum ena_eth_io_l3_proto_index {
+	ENA_ETH_IO_L3_PROTO_UNKNOWN	= 0,
+
+	ENA_ETH_IO_L3_PROTO_IPV4	= 8,
+
+	ENA_ETH_IO_L3_PROTO_IPV6	= 11,
+
+	ENA_ETH_IO_L3_PROTO_FCOE	= 21,
+
+	ENA_ETH_IO_L3_PROTO_ROCE	= 22,
+};
+
+enum ena_eth_io_l4_proto_index {
+	ENA_ETH_IO_L4_PROTO_UNKNOWN		= 0,
+
+	ENA_ETH_IO_L4_PROTO_TCP			= 12,
+
+	ENA_ETH_IO_L4_PROTO_UDP			= 13,
+
+	ENA_ETH_IO_L4_PROTO_ROUTEABLE_ROCE	= 23,
+};
+
+struct ena_eth_io_tx_desc {
+	/* 15:0 : length - Buffer length in bytes, must
+	 *    include any packet trailers that the ENA supposed
+	 *    to update like End-to-End CRC, Authentication GMAC
+	 *    etc. This length must not include the
+	 *    'Push_Buffer' length. This length must not include
+	 *    the 4-byte added in the end for 802.3 Ethernet FCS
+	 * 21:16 : req_id_hi - Request ID[15:10]
+	 * 22 : reserved22 - MBZ
+	 * 23 : meta_desc - MBZ
+	 * 24 : phase
+	 * 25 : reserved1 - MBZ
+	 * 26 : first - Indicates first descriptor in
+	 *    transaction
+	 * 27 : last - Indicates last descriptor in
+	 *    transaction
+	 * 28 : comp_req - Indicates whether completion
+	 *    should be posted, after packet is transmitted.
+	 *    Valid only for first descriptor
+	 * 30:29 : reserved29 - MBZ
+	 * 31 : reserved31 - MBZ
+	 */
+	u32 len_ctrl;
+
+	/* 3:0 : l3_proto_idx - L3 protocol. This field
+	 *    required when l3_csum_en,l3_csum or tso_en are set.
+	 * 4 : DF - IPv4 DF, must be 0 if packet is IPv4 and
+	 *    DF flags of the IPv4 header is 0. Otherwise must
+	 *    be set to 1
+	 * 6:5 : reserved5
+	 * 7 : tso_en - Enable TSO, For TCP only.
+	 * 12:8 : l4_proto_idx - L4 protocol. This field need
+	 *    to be set when l4_csum_en or tso_en are set.
+	 * 13 : l3_csum_en - enable IPv4 header checksum.
+	 * 14 : l4_csum_en - enable TCP/UDP checksum.
+	 * 15 : ethernet_fcs_dis - when set, the controller
+	 *    will not append the 802.3 Ethernet Frame Check
+	 *    Sequence to the packet
+	 * 16 : reserved16
+	 * 17 : l4_csum_partial - L4 partial checksum. when
+	 *    set to 0, the ENA calculates the L4 checksum,
+	 *    where the Destination Address required for the
+	 *    TCP/UDP pseudo-header is taken from the actual
+	 *    packet L3 header. when set to 1, the ENA doesn't
+	 *    calculate the sum of the pseudo-header, instead,
+	 *    the checksum field of the L4 is used instead. When
+	 *    TSO enabled, the checksum of the pseudo-header
+	 *    must not include the tcp length field. L4 partial
+	 *    checksum should be used for IPv6 packet that
+	 *    contains Routing Headers.
+	 * 20:18 : reserved18 - MBZ
+	 * 21 : reserved21 - MBZ
+	 * 31:22 : req_id_lo - Request ID[9:0]
+	 */
+	u32 meta_ctrl;
+
+	u32 buff_addr_lo;
+
+	/* address high and header size
+	 * 15:0 : addr_hi - Buffer Pointer[47:32]
+	 * 23:16 : reserved16_w2
+	 * 31:24 : header_length - Header length. For Low
+	 *    Latency Queues, this fields indicates the number
+	 *    of bytes written to the headers' memory. For
+	 *    normal queues, if packet is TCP or UDP, and longer
+	 *    than max_header_size, then this field should be
+	 *    set to the sum of L4 header offset and L4 header
+	 *    size(without options), otherwise, this field
+	 *    should be set to 0. For both modes, this field
+	 *    must not exceed the max_header_size.
+	 *    max_header_size value is reported by the Max
+	 *    Queues Feature descriptor
+	 */
+	u32 buff_addr_hi_hdr_sz;
+};
+
+struct ena_eth_io_tx_meta_desc {
+	/* 9:0 : req_id_lo - Request ID[9:0]
+	 * 11:10 : reserved10 - MBZ
+	 * 12 : reserved12 - MBZ
+	 * 13 : reserved13 - MBZ
+	 * 14 : ext_valid - if set, offset fields in Word2
+	 *    are valid Also MSS High in Word 0 and bits [31:24]
+	 *    in Word 3
+	 * 15 : reserved15
+	 * 19:16 : mss_hi
+	 * 20 : eth_meta_type - 0: Tx Metadata Descriptor, 1:
+	 *    Extended Metadata Descriptor
+	 * 21 : meta_store - Store extended metadata in queue
+	 *    cache
+	 * 22 : reserved22 - MBZ
+	 * 23 : meta_desc - MBO
+	 * 24 : phase
+	 * 25 : reserved25 - MBZ
+	 * 26 : first - Indicates first descriptor in
+	 *    transaction
+	 * 27 : last - Indicates last descriptor in
+	 *    transaction
+	 * 28 : comp_req - Indicates whether completion
+	 *    should be posted, after packet is transmitted.
+	 *    Valid only for first descriptor
+	 * 30:29 : reserved29 - MBZ
+	 * 31 : reserved31 - MBZ
+	 */
+	u32 len_ctrl;
+
+	/* 5:0 : req_id_hi
+	 * 31:6 : reserved6 - MBZ
+	 */
+	u32 word1;
+
+	/* 7:0 : l3_hdr_len
+	 * 15:8 : l3_hdr_off
+	 * 21:16 : l4_hdr_len_in_words - counts the L4 header
+	 *    length in words. there is an explicit assumption
+	 *    that L4 header appears right after L3 header and
+	 *    L4 offset is based on l3_hdr_off+l3_hdr_len
+	 * 31:22 : mss_lo
+	 */
+	u32 word2;
+
+	u32 reserved;
+};
+
+struct ena_eth_io_tx_cdesc {
+	/* Request ID[15:0] */
+	u16 req_id;
+
+	u8 status;
+
+	/* flags
+	 * 0 : phase
+	 * 7:1 : reserved1
+	 */
+	u8 flags;
+
+	u16 sub_qid;
+
+	u16 sq_head_idx;
+};
+
+struct ena_eth_io_rx_desc {
+	/* In bytes. 0 means 64KB */
+	u16 length;
+
+	/* MBZ */
+	u8 reserved2;
+
+	/* 0 : phase
+	 * 1 : reserved1 - MBZ
+	 * 2 : first - Indicates first descriptor in
+	 *    transaction
+	 * 3 : last - Indicates last descriptor in transaction
+	 * 4 : comp_req
+	 * 5 : reserved5 - MBO
+	 * 7:6 : reserved6 - MBZ
+	 */
+	u8 ctrl;
+
+	u16 req_id;
+
+	/* MBZ */
+	u16 reserved6;
+
+	u32 buff_addr_lo;
+
+	u16 buff_addr_hi;
+
+	/* MBZ */
+	u16 reserved16_w3;
+};
+
+/* 4-word format Note: all ethernet parsing information are valid only when
+ * last=1
+ */
+struct ena_eth_io_rx_cdesc_base {
+	/* 4:0 : l3_proto_idx
+	 * 6:5 : src_vlan_cnt
+	 * 7 : reserved7 - MBZ
+	 * 12:8 : l4_proto_idx
+	 * 13 : l3_csum_err - when set, either the L3
+	 *    checksum error detected, or, the controller didn't
+	 *    validate the checksum. This bit is valid only when
+	 *    l3_proto_idx indicates IPv4 packet
+	 * 14 : l4_csum_err - when set, either the L4
+	 *    checksum error detected, or, the controller didn't
+	 *    validate the checksum. This bit is valid only when
+	 *    l4_proto_idx indicates TCP/UDP packet, and,
+	 *    ipv4_frag is not set
+	 * 15 : ipv4_frag - Indicates IPv4 fragmented packet
+	 * 23:16 : reserved16
+	 * 24 : phase
+	 * 25 : l3_csum2 - second checksum engine result
+	 * 26 : first - Indicates first descriptor in
+	 *    transaction
+	 * 27 : last - Indicates last descriptor in
+	 *    transaction
+	 * 29:28 : reserved28
+	 * 30 : buffer - 0: Metadata descriptor. 1: Buffer
+	 *    Descriptor was used
+	 * 31 : reserved31
+	 */
+	u32 status;
+
+	u16 length;
+
+	u16 req_id;
+
+	/* 32-bit hash result */
+	u32 hash;
+
+	u16 sub_qid;
+
+	u16 reserved;
+};
+
+/* 8-word format */
+struct ena_eth_io_rx_cdesc_ext {
+	struct ena_eth_io_rx_cdesc_base base;
+
+	u32 buff_addr_lo;
+
+	u16 buff_addr_hi;
+
+	u16 reserved16;
+
+	u32 reserved_w6;
+
+	u32 reserved_w7;
+};
+
+struct ena_eth_io_intr_reg {
+	/* 14:0 : rx_intr_delay
+	 * 29:15 : tx_intr_delay
+	 * 30 : intr_unmask
+	 * 31 : reserved
+	 */
+	u32 intr_control;
+};
+
+struct ena_eth_io_numa_node_cfg_reg {
+	/* 7:0 : numa
+	 * 30:8 : reserved
+	 * 31 : enabled
+	 */
+	u32 numa_cfg;
+};
+
+/* tx_desc */
+#define ENA_ETH_IO_TX_DESC_LENGTH_MASK GENMASK(15, 0)
+#define ENA_ETH_IO_TX_DESC_REQ_ID_HI_SHIFT 16
+#define ENA_ETH_IO_TX_DESC_REQ_ID_HI_MASK GENMASK(21, 16)
+#define ENA_ETH_IO_TX_DESC_META_DESC_SHIFT 23
+#define ENA_ETH_IO_TX_DESC_META_DESC_MASK BIT(23)
+#define ENA_ETH_IO_TX_DESC_PHASE_SHIFT 24
+#define ENA_ETH_IO_TX_DESC_PHASE_MASK BIT(24)
+#define ENA_ETH_IO_TX_DESC_FIRST_SHIFT 26
+#define ENA_ETH_IO_TX_DESC_FIRST_MASK BIT(26)
+#define ENA_ETH_IO_TX_DESC_LAST_SHIFT 27
+#define ENA_ETH_IO_TX_DESC_LAST_MASK BIT(27)
+#define ENA_ETH_IO_TX_DESC_COMP_REQ_SHIFT 28
+#define ENA_ETH_IO_TX_DESC_COMP_REQ_MASK BIT(28)
+#define ENA_ETH_IO_TX_DESC_L3_PROTO_IDX_MASK GENMASK(3, 0)
+#define ENA_ETH_IO_TX_DESC_DF_SHIFT 4
+#define ENA_ETH_IO_TX_DESC_DF_MASK BIT(4)
+#define ENA_ETH_IO_TX_DESC_TSO_EN_SHIFT 7
+#define ENA_ETH_IO_TX_DESC_TSO_EN_MASK BIT(7)
+#define ENA_ETH_IO_TX_DESC_L4_PROTO_IDX_SHIFT 8
+#define ENA_ETH_IO_TX_DESC_L4_PROTO_IDX_MASK GENMASK(12, 8)
+#define ENA_ETH_IO_TX_DESC_L3_CSUM_EN_SHIFT 13
+#define ENA_ETH_IO_TX_DESC_L3_CSUM_EN_MASK BIT(13)
+#define ENA_ETH_IO_TX_DESC_L4_CSUM_EN_SHIFT 14
+#define ENA_ETH_IO_TX_DESC_L4_CSUM_EN_MASK BIT(14)
+#define ENA_ETH_IO_TX_DESC_ETHERNET_FCS_DIS_SHIFT 15
+#define ENA_ETH_IO_TX_DESC_ETHERNET_FCS_DIS_MASK BIT(15)
+#define ENA_ETH_IO_TX_DESC_L4_CSUM_PARTIAL_SHIFT 17
+#define ENA_ETH_IO_TX_DESC_L4_CSUM_PARTIAL_MASK BIT(17)
+#define ENA_ETH_IO_TX_DESC_REQ_ID_LO_SHIFT 22
+#define ENA_ETH_IO_TX_DESC_REQ_ID_LO_MASK GENMASK(31, 22)
+#define ENA_ETH_IO_TX_DESC_ADDR_HI_MASK GENMASK(15, 0)
+#define ENA_ETH_IO_TX_DESC_HEADER_LENGTH_SHIFT 24
+#define ENA_ETH_IO_TX_DESC_HEADER_LENGTH_MASK GENMASK(31, 24)
+
+/* tx_meta_desc */
+#define ENA_ETH_IO_TX_META_DESC_REQ_ID_LO_MASK GENMASK(9, 0)
+#define ENA_ETH_IO_TX_META_DESC_EXT_VALID_SHIFT 14
+#define ENA_ETH_IO_TX_META_DESC_EXT_VALID_MASK BIT(14)
+#define ENA_ETH_IO_TX_META_DESC_MSS_HI_SHIFT 16
+#define ENA_ETH_IO_TX_META_DESC_MSS_HI_MASK GENMASK(19, 16)
+#define ENA_ETH_IO_TX_META_DESC_ETH_META_TYPE_SHIFT 20
+#define ENA_ETH_IO_TX_META_DESC_ETH_META_TYPE_MASK BIT(20)
+#define ENA_ETH_IO_TX_META_DESC_META_STORE_SHIFT 21
+#define ENA_ETH_IO_TX_META_DESC_META_STORE_MASK BIT(21)
+#define ENA_ETH_IO_TX_META_DESC_META_DESC_SHIFT 23
+#define ENA_ETH_IO_TX_META_DESC_META_DESC_MASK BIT(23)
+#define ENA_ETH_IO_TX_META_DESC_PHASE_SHIFT 24
+#define ENA_ETH_IO_TX_META_DESC_PHASE_MASK BIT(24)
+#define ENA_ETH_IO_TX_META_DESC_FIRST_SHIFT 26
+#define ENA_ETH_IO_TX_META_DESC_FIRST_MASK BIT(26)
+#define ENA_ETH_IO_TX_META_DESC_LAST_SHIFT 27
+#define ENA_ETH_IO_TX_META_DESC_LAST_MASK BIT(27)
+#define ENA_ETH_IO_TX_META_DESC_COMP_REQ_SHIFT 28
+#define ENA_ETH_IO_TX_META_DESC_COMP_REQ_MASK BIT(28)
+#define ENA_ETH_IO_TX_META_DESC_REQ_ID_HI_MASK GENMASK(5, 0)
+#define ENA_ETH_IO_TX_META_DESC_L3_HDR_LEN_MASK GENMASK(7, 0)
+#define ENA_ETH_IO_TX_META_DESC_L3_HDR_OFF_SHIFT 8
+#define ENA_ETH_IO_TX_META_DESC_L3_HDR_OFF_MASK GENMASK(15, 8)
+#define ENA_ETH_IO_TX_META_DESC_L4_HDR_LEN_IN_WORDS_SHIFT 16
+#define ENA_ETH_IO_TX_META_DESC_L4_HDR_LEN_IN_WORDS_MASK GENMASK(21, 16)
+#define ENA_ETH_IO_TX_META_DESC_MSS_LO_SHIFT 22
+#define ENA_ETH_IO_TX_META_DESC_MSS_LO_MASK GENMASK(31, 22)
+
+/* tx_cdesc */
+#define ENA_ETH_IO_TX_CDESC_PHASE_MASK BIT(0)
+
+/* rx_desc */
+#define ENA_ETH_IO_RX_DESC_PHASE_MASK BIT(0)
+#define ENA_ETH_IO_RX_DESC_FIRST_SHIFT 2
+#define ENA_ETH_IO_RX_DESC_FIRST_MASK BIT(2)
+#define ENA_ETH_IO_RX_DESC_LAST_SHIFT 3
+#define ENA_ETH_IO_RX_DESC_LAST_MASK BIT(3)
+#define ENA_ETH_IO_RX_DESC_COMP_REQ_SHIFT 4
+#define ENA_ETH_IO_RX_DESC_COMP_REQ_MASK BIT(4)
+
+/* rx_cdesc_base */
+#define ENA_ETH_IO_RX_CDESC_BASE_L3_PROTO_IDX_MASK GENMASK(4, 0)
+#define ENA_ETH_IO_RX_CDESC_BASE_SRC_VLAN_CNT_SHIFT 5
+#define ENA_ETH_IO_RX_CDESC_BASE_SRC_VLAN_CNT_MASK GENMASK(6, 5)
+#define ENA_ETH_IO_RX_CDESC_BASE_L4_PROTO_IDX_SHIFT 8
+#define ENA_ETH_IO_RX_CDESC_BASE_L4_PROTO_IDX_MASK GENMASK(12, 8)
+#define ENA_ETH_IO_RX_CDESC_BASE_L3_CSUM_ERR_SHIFT 13
+#define ENA_ETH_IO_RX_CDESC_BASE_L3_CSUM_ERR_MASK BIT(13)
+#define ENA_ETH_IO_RX_CDESC_BASE_L4_CSUM_ERR_SHIFT 14
+#define ENA_ETH_IO_RX_CDESC_BASE_L4_CSUM_ERR_MASK BIT(14)
+#define ENA_ETH_IO_RX_CDESC_BASE_IPV4_FRAG_SHIFT 15
+#define ENA_ETH_IO_RX_CDESC_BASE_IPV4_FRAG_MASK BIT(15)
+#define ENA_ETH_IO_RX_CDESC_BASE_PHASE_SHIFT 24
+#define ENA_ETH_IO_RX_CDESC_BASE_PHASE_MASK BIT(24)
+#define ENA_ETH_IO_RX_CDESC_BASE_L3_CSUM2_SHIFT 25
+#define ENA_ETH_IO_RX_CDESC_BASE_L3_CSUM2_MASK BIT(25)
+#define ENA_ETH_IO_RX_CDESC_BASE_FIRST_SHIFT 26
+#define ENA_ETH_IO_RX_CDESC_BASE_FIRST_MASK BIT(26)
+#define ENA_ETH_IO_RX_CDESC_BASE_LAST_SHIFT 27
+#define ENA_ETH_IO_RX_CDESC_BASE_LAST_MASK BIT(27)
+#define ENA_ETH_IO_RX_CDESC_BASE_BUFFER_SHIFT 30
+#define ENA_ETH_IO_RX_CDESC_BASE_BUFFER_MASK BIT(30)
+
+/* intr_reg */
+#define ENA_ETH_IO_INTR_REG_RX_INTR_DELAY_MASK GENMASK(14, 0)
+#define ENA_ETH_IO_INTR_REG_TX_INTR_DELAY_SHIFT 15
+#define ENA_ETH_IO_INTR_REG_TX_INTR_DELAY_MASK GENMASK(29, 15)
+#define ENA_ETH_IO_INTR_REG_INTR_UNMASK_SHIFT 30
+#define ENA_ETH_IO_INTR_REG_INTR_UNMASK_MASK BIT(30)
+
+/* numa_node_cfg_reg */
+#define ENA_ETH_IO_NUMA_NODE_CFG_REG_NUMA_MASK GENMASK(7, 0)
+#define ENA_ETH_IO_NUMA_NODE_CFG_REG_ENABLED_SHIFT 31
+#define ENA_ETH_IO_NUMA_NODE_CFG_REG_ENABLED_MASK BIT(31)
+
+#endif /*_ENA_ETH_IO_H_ */
diff --git a/drivers/net/ethernet/amazon/ena/ena_ethtool.c b/drivers/net/ethernet/amazon/ena/ena_ethtool.c
new file mode 100644
index 000000000000..67b2338f8fb3
--- /dev/null
+++ b/drivers/net/ethernet/amazon/ena/ena_ethtool.c
@@ -0,0 +1,895 @@
+/*
+ * Copyright 2015 Amazon.com, Inc. or its affiliates.
+ *
+ * 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
+ * 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/pci.h>
+
+#include "ena_netdev.h"
+
+struct ena_stats {
+	char name[ETH_GSTRING_LEN];
+	int stat_offset;
+};
+
+#define ENA_STAT_ENA_COM_ENTRY(stat) { \
+	.name = #stat, \
+	.stat_offset = offsetof(struct ena_com_stats_admin, stat) \
+}
+
+#define ENA_STAT_ENTRY(stat, stat_type) { \
+	.name = #stat, \
+	.stat_offset = offsetof(struct ena_stats_##stat_type, stat) \
+}
+
+#define ENA_STAT_RX_ENTRY(stat) \
+	ENA_STAT_ENTRY(stat, rx)
+
+#define ENA_STAT_TX_ENTRY(stat) \
+	ENA_STAT_ENTRY(stat, tx)
+
+#define ENA_STAT_GLOBAL_ENTRY(stat) \
+	ENA_STAT_ENTRY(stat, dev)
+
+static const struct ena_stats ena_stats_global_strings[] = {
+	ENA_STAT_GLOBAL_ENTRY(tx_timeout),
+	ENA_STAT_GLOBAL_ENTRY(io_suspend),
+	ENA_STAT_GLOBAL_ENTRY(io_resume),
+	ENA_STAT_GLOBAL_ENTRY(wd_expired),
+	ENA_STAT_GLOBAL_ENTRY(interface_up),
+	ENA_STAT_GLOBAL_ENTRY(interface_down),
+	ENA_STAT_GLOBAL_ENTRY(admin_q_pause),
+};
+
+static const struct ena_stats ena_stats_tx_strings[] = {
+	ENA_STAT_TX_ENTRY(cnt),
+	ENA_STAT_TX_ENTRY(bytes),
+	ENA_STAT_TX_ENTRY(queue_stop),
+	ENA_STAT_TX_ENTRY(queue_wakeup),
+	ENA_STAT_TX_ENTRY(dma_mapping_err),
+	ENA_STAT_TX_ENTRY(linearize),
+	ENA_STAT_TX_ENTRY(linearize_failed),
+	ENA_STAT_TX_ENTRY(napi_comp),
+	ENA_STAT_TX_ENTRY(tx_poll),
+	ENA_STAT_TX_ENTRY(doorbells),
+	ENA_STAT_TX_ENTRY(prepare_ctx_err),
+	ENA_STAT_TX_ENTRY(missing_tx_comp),
+	ENA_STAT_TX_ENTRY(bad_req_id),
+};
+
+static const struct ena_stats ena_stats_rx_strings[] = {
+	ENA_STAT_RX_ENTRY(cnt),
+	ENA_STAT_RX_ENTRY(bytes),
+	ENA_STAT_RX_ENTRY(refil_partial),
+	ENA_STAT_RX_ENTRY(bad_csum),
+	ENA_STAT_RX_ENTRY(page_alloc_fail),
+	ENA_STAT_RX_ENTRY(skb_alloc_fail),
+	ENA_STAT_RX_ENTRY(dma_mapping_err),
+	ENA_STAT_RX_ENTRY(bad_desc_num),
+	ENA_STAT_RX_ENTRY(rx_copybreak_pkt),
+};
+
+static const struct ena_stats ena_stats_ena_com_strings[] = {
+	ENA_STAT_ENA_COM_ENTRY(aborted_cmd),
+	ENA_STAT_ENA_COM_ENTRY(submitted_cmd),
+	ENA_STAT_ENA_COM_ENTRY(completed_cmd),
+	ENA_STAT_ENA_COM_ENTRY(out_of_space),
+	ENA_STAT_ENA_COM_ENTRY(no_completion),
+};
+
+#define ENA_STATS_ARRAY_GLOBAL	ARRAY_SIZE(ena_stats_global_strings)
+#define ENA_STATS_ARRAY_TX	ARRAY_SIZE(ena_stats_tx_strings)
+#define ENA_STATS_ARRAY_RX	ARRAY_SIZE(ena_stats_rx_strings)
+#define ENA_STATS_ARRAY_ENA_COM	ARRAY_SIZE(ena_stats_ena_com_strings)
+
+static void ena_safe_update_stat(u64 *src, u64 *dst,
+				 struct u64_stats_sync *syncp)
+{
+	unsigned int start;
+
+	do {
+		start = u64_stats_fetch_begin_irq(syncp);
+		*(dst) = *src;
+	} while (u64_stats_fetch_retry_irq(syncp, start));
+}
+
+static void ena_queue_stats(struct ena_adapter *adapter, u64 **data)
+{
+	const struct ena_stats *ena_stats;
+	struct ena_ring *ring;
+
+	u64 *ptr;
+	int i, j;
+
+	for (i = 0; i < adapter->num_queues; i++) {
+		/* Tx stats */
+		ring = &adapter->tx_ring[i];
+
+		for (j = 0; j < ENA_STATS_ARRAY_TX; j++) {
+			ena_stats = &ena_stats_tx_strings[j];
+
+			ptr = (u64 *)((uintptr_t)&ring->tx_stats +
+				(uintptr_t)ena_stats->stat_offset);
+
+			ena_safe_update_stat(ptr, (*data)++, &ring->syncp);
+		}
+
+		/* Rx stats */
+		ring = &adapter->rx_ring[i];
+
+		for (j = 0; j < ENA_STATS_ARRAY_RX; j++) {
+			ena_stats = &ena_stats_rx_strings[j];
+
+			ptr = (u64 *)((uintptr_t)&ring->rx_stats +
+				(uintptr_t)ena_stats->stat_offset);
+
+			ena_safe_update_stat(ptr, (*data)++, &ring->syncp);
+		}
+	}
+}
+
+static void ena_dev_admin_queue_stats(struct ena_adapter *adapter, u64 **data)
+{
+	const struct ena_stats *ena_stats;
+	u32 *ptr;
+	int i;
+
+	for (i = 0; i < ENA_STATS_ARRAY_ENA_COM; i++) {
+		ena_stats = &ena_stats_ena_com_strings[i];
+
+		ptr = (u32 *)((uintptr_t)&adapter->ena_dev->admin_queue.stats +
+			(uintptr_t)ena_stats->stat_offset);
+
+		*(*data)++ = *ptr;
+	}
+}
+
+static void ena_get_ethtool_stats(struct net_device *netdev,
+				  struct ethtool_stats *stats,
+				  u64 *data)
+{
+	struct ena_adapter *adapter = netdev_priv(netdev);
+	const struct ena_stats *ena_stats;
+	u64 *ptr;
+	int i;
+
+	for (i = 0; i < ENA_STATS_ARRAY_GLOBAL; i++) {
+		ena_stats = &ena_stats_global_strings[i];
+
+		ptr = (u64 *)((uintptr_t)&adapter->dev_stats +
+			(uintptr_t)ena_stats->stat_offset);
+
+		ena_safe_update_stat(ptr, data++, &adapter->syncp);
+	}
+
+	ena_queue_stats(adapter, &data);
+	ena_dev_admin_queue_stats(adapter, &data);
+}
+
+int ena_get_sset_count(struct net_device *netdev, int sset)
+{
+	struct ena_adapter *adapter = netdev_priv(netdev);
+
+	if (sset != ETH_SS_STATS)
+		return -EOPNOTSUPP;
+
+	return  adapter->num_queues * (ENA_STATS_ARRAY_TX + ENA_STATS_ARRAY_RX)
+		+ ENA_STATS_ARRAY_GLOBAL + ENA_STATS_ARRAY_ENA_COM;
+}
+
+static void ena_queue_strings(struct ena_adapter *adapter, u8 **data)
+{
+	const struct ena_stats *ena_stats;
+	int i, j;
+
+	for (i = 0; i < adapter->num_queues; i++) {
+		/* Tx stats */
+		for (j = 0; j < ENA_STATS_ARRAY_TX; j++) {
+			ena_stats = &ena_stats_tx_strings[j];
+
+			snprintf(*data, ETH_GSTRING_LEN,
+				 "queue_%u_tx_%s", i, ena_stats->name);
+			 (*data) += ETH_GSTRING_LEN;
+		}
+		/* Rx stats */
+		for (j = 0; j < ENA_STATS_ARRAY_RX; j++) {
+			ena_stats = &ena_stats_rx_strings[j];
+
+			snprintf(*data, ETH_GSTRING_LEN,
+				 "queue_%u_rx_%s", i, ena_stats->name);
+			(*data) += ETH_GSTRING_LEN;
+		}
+	}
+}
+
+static void ena_com_dev_strings(u8 **data)
+{
+	const struct ena_stats *ena_stats;
+	int i;
+
+	for (i = 0; i < ENA_STATS_ARRAY_ENA_COM; i++) {
+		ena_stats = &ena_stats_ena_com_strings[i];
+
+		snprintf(*data, ETH_GSTRING_LEN,
+			 "ena_admin_q_%s", ena_stats->name);
+		(*data) += ETH_GSTRING_LEN;
+	}
+}
+
+static void ena_get_strings(struct net_device *netdev, u32 sset, u8 *data)
+{
+	struct ena_adapter *adapter = netdev_priv(netdev);
+	const struct ena_stats *ena_stats;
+	int i;
+
+	if (sset != ETH_SS_STATS)
+		return;
+
+	for (i = 0; i < ENA_STATS_ARRAY_GLOBAL; i++) {
+		ena_stats = &ena_stats_global_strings[i];
+
+		memcpy(data, ena_stats->name, ETH_GSTRING_LEN);
+		data += ETH_GSTRING_LEN;
+	}
+
+	ena_queue_strings(adapter, &data);
+	ena_com_dev_strings(&data);
+}
+
+static int ena_get_link_ksettings(struct net_device *netdev,
+				  struct ethtool_link_ksettings *link_ksettings)
+{
+	struct ena_adapter *adapter = netdev_priv(netdev);
+	struct ena_com_dev *ena_dev = adapter->ena_dev;
+	struct ena_admin_get_feature_link_desc *link;
+	struct ena_admin_get_feat_resp feat_resp;
+	int rc;
+
+	rc = ena_com_get_link_params(ena_dev, &feat_resp);
+	if (rc)
+		return rc;
+
+	link = &feat_resp.u.link;
+	link_ksettings->base.speed = link->speed;
+
+	if (link->flags & ENA_ADMIN_GET_FEATURE_LINK_DESC_AUTONEG_MASK) {
+		ethtool_link_ksettings_add_link_mode(link_ksettings,
+						     supported, Autoneg);
+		ethtool_link_ksettings_add_link_mode(link_ksettings,
+						     supported, Autoneg);
+	}
+
+	link_ksettings->base.autoneg =
+		(link->flags & ENA_ADMIN_GET_FEATURE_LINK_DESC_AUTONEG_MASK) ?
+		AUTONEG_ENABLE : AUTONEG_DISABLE;
+
+	link_ksettings->base.duplex = DUPLEX_FULL;
+
+	return 0;
+}
+
+static int ena_get_coalesce(struct net_device *net_dev,
+			    struct ethtool_coalesce *coalesce)
+{
+	struct ena_adapter *adapter = netdev_priv(net_dev);
+	struct ena_com_dev *ena_dev = adapter->ena_dev;
+	struct ena_intr_moder_entry intr_moder_entry;
+
+	if (!ena_com_interrupt_moderation_supported(ena_dev)) {
+		/* the devie doesn't support interrupt moderation */
+		return -EOPNOTSUPP;
+	}
+	coalesce->tx_coalesce_usecs =
+		ena_com_get_nonadaptive_moderation_interval_tx(ena_dev) /
+			ena_dev->intr_delay_resolution;
+	if (!ena_com_get_adaptive_moderation_enabled(ena_dev)) {
+		coalesce->rx_coalesce_usecs =
+			ena_com_get_nonadaptive_moderation_interval_rx(ena_dev)
+			/ ena_dev->intr_delay_resolution;
+	} else {
+		ena_com_get_intr_moderation_entry(adapter->ena_dev, ENA_INTR_MODER_LOWEST, &intr_moder_entry);
+		coalesce->rx_coalesce_usecs_low = intr_moder_entry.intr_moder_interval;
+		coalesce->rx_max_coalesced_frames_low = intr_moder_entry.pkts_per_interval;
+
+		ena_com_get_intr_moderation_entry(adapter->ena_dev, ENA_INTR_MODER_MID, &intr_moder_entry);
+		coalesce->rx_coalesce_usecs = intr_moder_entry.intr_moder_interval;
+		coalesce->rx_max_coalesced_frames = intr_moder_entry.pkts_per_interval;
+
+		ena_com_get_intr_moderation_entry(adapter->ena_dev, ENA_INTR_MODER_HIGHEST, &intr_moder_entry);
+		coalesce->rx_coalesce_usecs_high = intr_moder_entry.intr_moder_interval;
+		coalesce->rx_max_coalesced_frames_high = intr_moder_entry.pkts_per_interval;
+	}
+	coalesce->use_adaptive_rx_coalesce =
+		ena_com_get_adaptive_moderation_enabled(ena_dev);
+
+	return 0;
+}
+
+static void ena_update_tx_rings_intr_moderation(struct ena_adapter *adapter)
+{
+	unsigned int val;
+	int i;
+
+	val = ena_com_get_nonadaptive_moderation_interval_tx(adapter->ena_dev);
+
+	for (i = 0; i < adapter->num_queues; i++)
+		adapter->tx_ring[i].smoothed_interval = val;
+}
+
+static int ena_set_coalesce(struct net_device *net_dev,
+			    struct ethtool_coalesce *coalesce)
+{
+	struct ena_adapter *adapter = netdev_priv(net_dev);
+	struct ena_com_dev *ena_dev = adapter->ena_dev;
+	struct ena_intr_moder_entry intr_moder_entry;
+	int rc;
+
+	if (!ena_com_interrupt_moderation_supported(ena_dev)) {
+		/* the devie doesn't support interrupt moderation */
+		return -EOPNOTSUPP;
+	}
+
+	if (coalesce->rx_coalesce_usecs_irq ||
+	    coalesce->rx_max_coalesced_frames_irq ||
+	    coalesce->tx_coalesce_usecs_irq ||
+	    coalesce->tx_max_coalesced_frames ||
+	    coalesce->tx_max_coalesced_frames_irq ||
+	    coalesce->stats_block_coalesce_usecs ||
+	    coalesce->use_adaptive_tx_coalesce ||
+	    coalesce->pkt_rate_low ||
+	    coalesce->tx_coalesce_usecs_low ||
+	    coalesce->tx_max_coalesced_frames_low ||
+	    coalesce->pkt_rate_high ||
+	    coalesce->tx_coalesce_usecs_high ||
+	    coalesce->tx_max_coalesced_frames_high ||
+	    coalesce->rate_sample_interval)
+		return -EINVAL;
+
+	rc = ena_com_update_nonadaptive_moderation_interval_tx(ena_dev,
+							       coalesce->tx_coalesce_usecs);
+	if (rc)
+		return rc;
+
+	ena_update_tx_rings_intr_moderation(adapter);
+
+	if (ena_com_get_adaptive_moderation_enabled(ena_dev)) {
+		if (!coalesce->use_adaptive_rx_coalesce) {
+			ena_com_disable_adaptive_moderation(ena_dev);
+			rc = ena_com_update_nonadaptive_moderation_interval_rx(ena_dev,
+									       coalesce->rx_coalesce_usecs);
+			return rc;
+		}
+	} else { /* was in non-adaptive mode */
+		if (coalesce->use_adaptive_rx_coalesce) {
+			ena_com_enable_adaptive_moderation(ena_dev);
+		} else {
+			rc = ena_com_update_nonadaptive_moderation_interval_rx(ena_dev,
+									       coalesce->rx_coalesce_usecs);
+			return rc;
+		}
+	}
+
+	intr_moder_entry.intr_moder_interval = coalesce->rx_coalesce_usecs_low;
+	intr_moder_entry.pkts_per_interval = coalesce->rx_max_coalesced_frames_low;
+	intr_moder_entry.bytes_per_interval = ENA_INTR_BYTE_COUNT_NOT_SUPPORTED;
+	ena_com_init_intr_moderation_entry(adapter->ena_dev, ENA_INTR_MODER_LOWEST, &intr_moder_entry);
+
+	intr_moder_entry.intr_moder_interval = coalesce->rx_coalesce_usecs;
+	intr_moder_entry.pkts_per_interval = coalesce->rx_max_coalesced_frames;
+	intr_moder_entry.bytes_per_interval = ENA_INTR_BYTE_COUNT_NOT_SUPPORTED;
+	ena_com_init_intr_moderation_entry(adapter->ena_dev, ENA_INTR_MODER_MID, &intr_moder_entry);
+
+	intr_moder_entry.intr_moder_interval = coalesce->rx_coalesce_usecs_high;
+	intr_moder_entry.pkts_per_interval = coalesce->rx_max_coalesced_frames_high;
+	intr_moder_entry.bytes_per_interval = ENA_INTR_BYTE_COUNT_NOT_SUPPORTED;
+	ena_com_init_intr_moderation_entry(adapter->ena_dev, ENA_INTR_MODER_HIGHEST, &intr_moder_entry);
+
+	return 0;
+}
+
+static u32 ena_get_msglevel(struct net_device *netdev)
+{
+	struct ena_adapter *adapter = netdev_priv(netdev);
+
+	return adapter->msg_enable;
+}
+
+static void ena_set_msglevel(struct net_device *netdev, u32 value)
+{
+	struct ena_adapter *adapter = netdev_priv(netdev);
+
+	adapter->msg_enable = value;
+}
+
+static void ena_get_drvinfo(struct net_device *dev,
+			    struct ethtool_drvinfo *info)
+{
+	struct ena_adapter *adapter = netdev_priv(dev);
+
+	strlcpy(info->driver, DRV_MODULE_NAME, sizeof(info->driver));
+	strlcpy(info->version, DRV_MODULE_VERSION, sizeof(info->version));
+	strlcpy(info->bus_info, pci_name(adapter->pdev),
+		sizeof(info->bus_info));
+}
+
+static void ena_get_ringparam(struct net_device *netdev,
+			      struct ethtool_ringparam *ring)
+{
+	struct ena_adapter *adapter = netdev_priv(netdev);
+	struct ena_ring *tx_ring = &adapter->tx_ring[0];
+	struct ena_ring *rx_ring = &adapter->rx_ring[0];
+
+	ring->rx_max_pending = rx_ring->ring_size;
+	ring->tx_max_pending = tx_ring->ring_size;
+	ring->rx_pending = rx_ring->ring_size;
+	ring->tx_pending = tx_ring->ring_size;
+}
+
+static u32 ena_flow_hash_to_flow_type(u16 hash_fields)
+{
+	u32 data = 0;
+
+	if (hash_fields & ENA_ADMIN_RSS_L2_DA)
+		data |= RXH_L2DA;
+
+	if (hash_fields & ENA_ADMIN_RSS_L3_DA)
+		data |= RXH_IP_DST;
+
+	if (hash_fields & ENA_ADMIN_RSS_L3_SA)
+		data |= RXH_IP_SRC;
+
+	if (hash_fields & ENA_ADMIN_RSS_L4_DP)
+		data |= RXH_L4_B_2_3;
+
+	if (hash_fields & ENA_ADMIN_RSS_L4_SP)
+		data |= RXH_L4_B_0_1;
+
+	return data;
+}
+
+static u16 ena_flow_data_to_flow_hash(u32 hash_fields)
+{
+	u16 data = 0;
+
+	if (hash_fields & RXH_L2DA)
+		data |= ENA_ADMIN_RSS_L2_DA;
+
+	if (hash_fields & RXH_IP_DST)
+		data |= ENA_ADMIN_RSS_L3_DA;
+
+	if (hash_fields & RXH_IP_SRC)
+		data |= ENA_ADMIN_RSS_L3_SA;
+
+	if (hash_fields & RXH_L4_B_2_3)
+		data |= ENA_ADMIN_RSS_L4_DP;
+
+	if (hash_fields & RXH_L4_B_0_1)
+		data |= ENA_ADMIN_RSS_L4_SP;
+
+	return data;
+}
+
+static int ena_get_rss_hash(struct ena_com_dev *ena_dev,
+			    struct ethtool_rxnfc *cmd)
+{
+	enum ena_admin_flow_hash_proto proto;
+	u16 hash_fields;
+	int rc;
+
+	cmd->data = 0;
+
+	switch (cmd->flow_type) {
+	case TCP_V4_FLOW:
+		proto = ENA_ADMIN_RSS_TCP4;
+		break;
+	case UDP_V4_FLOW:
+		proto = ENA_ADMIN_RSS_UDP4;
+		break;
+	case TCP_V6_FLOW:
+		proto = ENA_ADMIN_RSS_TCP6;
+		break;
+	case UDP_V6_FLOW:
+		proto = ENA_ADMIN_RSS_UDP6;
+		break;
+	case IPV4_FLOW:
+		proto = ENA_ADMIN_RSS_IP4;
+		break;
+	case IPV6_FLOW:
+		proto = ENA_ADMIN_RSS_IP6;
+		break;
+	case ETHER_FLOW:
+		proto = ENA_ADMIN_RSS_NOT_IP;
+		break;
+	case AH_V4_FLOW:
+	case ESP_V4_FLOW:
+	case AH_V6_FLOW:
+	case ESP_V6_FLOW:
+	case SCTP_V4_FLOW:
+	case AH_ESP_V4_FLOW:
+		return -EOPNOTSUPP;
+	default:
+		return -EINVAL;
+	}
+
+	rc = ena_com_get_hash_ctrl(ena_dev, proto, &hash_fields);
+	if (rc) {
+		/* If device don't have permission, return unsupported */
+		if (rc == -EPERM)
+			rc = -EOPNOTSUPP;
+		return rc;
+	}
+
+	cmd->data = ena_flow_hash_to_flow_type(hash_fields);
+
+	return 0;
+}
+
+static int ena_set_rss_hash(struct ena_com_dev *ena_dev,
+			    struct ethtool_rxnfc *cmd)
+{
+	enum ena_admin_flow_hash_proto proto;
+	u16 hash_fields;
+
+	switch (cmd->flow_type) {
+	case TCP_V4_FLOW:
+		proto = ENA_ADMIN_RSS_TCP4;
+		break;
+	case UDP_V4_FLOW:
+		proto = ENA_ADMIN_RSS_UDP4;
+		break;
+	case TCP_V6_FLOW:
+		proto = ENA_ADMIN_RSS_TCP6;
+		break;
+	case UDP_V6_FLOW:
+		proto = ENA_ADMIN_RSS_UDP6;
+		break;
+	case IPV4_FLOW:
+		proto = ENA_ADMIN_RSS_IP4;
+		break;
+	case IPV6_FLOW:
+		proto = ENA_ADMIN_RSS_IP6;
+		break;
+	case ETHER_FLOW:
+		proto = ENA_ADMIN_RSS_NOT_IP;
+		break;
+	case AH_V4_FLOW:
+	case ESP_V4_FLOW:
+	case AH_V6_FLOW:
+	case ESP_V6_FLOW:
+	case SCTP_V4_FLOW:
+	case AH_ESP_V4_FLOW:
+		return -EOPNOTSUPP;
+	default:
+		return -EINVAL;
+	}
+
+	hash_fields = ena_flow_data_to_flow_hash(cmd->data);
+
+	return ena_com_fill_hash_ctrl(ena_dev, proto, hash_fields);
+}
+
+static int ena_set_rxnfc(struct net_device *netdev, struct ethtool_rxnfc *info)
+{
+	struct ena_adapter *adapter = netdev_priv(netdev);
+	int rc = 0;
+
+	switch (info->cmd) {
+	case ETHTOOL_SRXFH:
+		rc = ena_set_rss_hash(adapter->ena_dev, info);
+		break;
+	case ETHTOOL_SRXCLSRLDEL:
+	case ETHTOOL_SRXCLSRLINS:
+	default:
+		netif_err(adapter, drv, netdev,
+			  "Command parameter %d is not supported\n", info->cmd);
+		rc = -EOPNOTSUPP;
+	}
+
+	return (rc == -EPERM) ? -EOPNOTSUPP : rc;
+}
+
+static int ena_get_rxnfc(struct net_device *netdev, struct ethtool_rxnfc *info,
+			 u32 *rules)
+{
+	struct ena_adapter *adapter = netdev_priv(netdev);
+	int rc = 0;
+
+	switch (info->cmd) {
+	case ETHTOOL_GRXRINGS:
+		info->data = adapter->num_queues;
+		rc = 0;
+		break;
+	case ETHTOOL_GRXFH:
+		rc = ena_get_rss_hash(adapter->ena_dev, info);
+		break;
+	case ETHTOOL_GRXCLSRLCNT:
+	case ETHTOOL_GRXCLSRULE:
+	case ETHTOOL_GRXCLSRLALL:
+	default:
+		netif_err(adapter, drv, netdev,
+			  "Command parameter %d is not supported\n", info->cmd);
+		rc = -EOPNOTSUPP;
+	}
+
+	return (rc == -EPERM) ? -EOPNOTSUPP : rc;
+}
+
+static u32 ena_get_rxfh_indir_size(struct net_device *netdev)
+{
+	return ENA_RX_RSS_TABLE_SIZE;
+}
+
+static u32 ena_get_rxfh_key_size(struct net_device *netdev)
+{
+	return ENA_HASH_KEY_SIZE;
+}
+
+static int ena_get_rxfh(struct net_device *netdev, u32 *indir, u8 *key,
+			u8 *hfunc)
+{
+	struct ena_adapter *adapter = netdev_priv(netdev);
+	enum ena_admin_hash_functions ena_func;
+	u8 func;
+	int rc;
+
+	rc = ena_com_indirect_table_get(adapter->ena_dev, indir);
+	if (rc)
+		return rc;
+
+	rc = ena_com_get_hash_function(adapter->ena_dev, &ena_func, key);
+	if (rc)
+		return rc;
+
+	switch (ena_func) {
+	case ENA_ADMIN_TOEPLITZ:
+		func = ETH_RSS_HASH_TOP;
+		break;
+	case ENA_ADMIN_CRC32:
+		func = ETH_RSS_HASH_XOR;
+		break;
+	default:
+		netif_err(adapter, drv, netdev,
+			  "Command parameter is not supported\n");
+		return -EOPNOTSUPP;
+	}
+
+	if (hfunc)
+		*hfunc = func;
+
+	return rc;
+}
+
+static int ena_set_rxfh(struct net_device *netdev, const u32 *indir,
+			const u8 *key, const u8 hfunc)
+{
+	struct ena_adapter *adapter = netdev_priv(netdev);
+	struct ena_com_dev *ena_dev = adapter->ena_dev;
+	enum ena_admin_hash_functions func;
+	int rc, i;
+
+	if (indir) {
+		for (i = 0; i < ENA_RX_RSS_TABLE_SIZE; i++) {
+			rc = ena_com_indirect_table_fill_entry(ena_dev,
+							       ENA_IO_RXQ_IDX(indir[i]),
+							       i);
+			if (unlikely(rc)) {
+				netif_err(adapter, drv, netdev,
+					  "Cannot fill indirect table (index is too large)\n");
+				return rc;
+			}
+		}
+
+		rc = ena_com_indirect_table_set(ena_dev);
+		if (rc) {
+			netif_err(adapter, drv, netdev,
+				  "Cannot set indirect table\n");
+			return rc == -EPERM ? -EOPNOTSUPP : rc;
+		}
+	}
+
+	switch (hfunc) {
+	case ETH_RSS_HASH_TOP:
+		func = ENA_ADMIN_TOEPLITZ;
+		break;
+	case ETH_RSS_HASH_XOR:
+		func = ENA_ADMIN_CRC32;
+		break;
+	default:
+		netif_err(adapter, drv, netdev, "Unsupported hfunc %d\n",
+			  hfunc);
+		return -EOPNOTSUPP;
+	}
+
+	if (key) {
+		rc = ena_com_fill_hash_function(ena_dev, func, key,
+						ENA_HASH_KEY_SIZE,
+						0xFFFFFFFF);
+		if (unlikely(rc)) {
+			netif_err(adapter, drv, netdev, "Cannot fill key\n");
+			return rc == -EPERM ? -EOPNOTSUPP : rc;
+		}
+	}
+
+	return 0;
+}
+
+static void ena_get_channels(struct net_device *netdev,
+			     struct ethtool_channels *channels)
+{
+	struct ena_adapter *adapter = netdev_priv(netdev);
+
+	channels->max_rx = ENA_MAX_NUM_IO_QUEUES;
+	channels->max_tx = ENA_MAX_NUM_IO_QUEUES;
+	channels->max_other = 0;
+	channels->max_combined = 0;
+	channels->rx_count = adapter->num_queues;
+	channels->tx_count = adapter->num_queues;
+	channels->other_count = 0;
+	channels->combined_count = 0;
+}
+
+static int ena_get_tunable(struct net_device *netdev,
+			   const struct ethtool_tunable *tuna, void *data)
+{
+	struct ena_adapter *adapter = netdev_priv(netdev);
+	int ret = 0;
+
+	switch (tuna->id) {
+	case ETHTOOL_RX_COPYBREAK:
+		*(u32 *)data = adapter->rx_copybreak;
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
+static int ena_set_tunable(struct net_device *netdev,
+			   const struct ethtool_tunable *tuna,
+			   const void *data)
+{
+	struct ena_adapter *adapter = netdev_priv(netdev);
+	int ret = 0;
+	u32 len;
+
+	switch (tuna->id) {
+	case ETHTOOL_RX_COPYBREAK:
+		len = *(u32 *)data;
+		if (len > adapter->netdev->mtu) {
+			ret = -EINVAL;
+			break;
+		}
+		adapter->rx_copybreak = len;
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
+static const struct ethtool_ops ena_ethtool_ops = {
+	.get_link_ksettings	= ena_get_link_ksettings,
+	.get_drvinfo		= ena_get_drvinfo,
+	.get_msglevel		= ena_get_msglevel,
+	.set_msglevel		= ena_set_msglevel,
+	.get_link		= ethtool_op_get_link,
+	.get_coalesce		= ena_get_coalesce,
+	.set_coalesce		= ena_set_coalesce,
+	.get_ringparam		= ena_get_ringparam,
+	.get_sset_count         = ena_get_sset_count,
+	.get_strings		= ena_get_strings,
+	.get_ethtool_stats      = ena_get_ethtool_stats,
+	.get_rxnfc		= ena_get_rxnfc,
+	.set_rxnfc		= ena_set_rxnfc,
+	.get_rxfh_indir_size    = ena_get_rxfh_indir_size,
+	.get_rxfh_key_size	= ena_get_rxfh_key_size,
+	.get_rxfh		= ena_get_rxfh,
+	.set_rxfh		= ena_set_rxfh,
+	.get_channels		= ena_get_channels,
+	.get_tunable		= ena_get_tunable,
+	.set_tunable		= ena_set_tunable,
+};
+
+void ena_set_ethtool_ops(struct net_device *netdev)
+{
+	netdev->ethtool_ops = &ena_ethtool_ops;
+}
+
+static void ena_dump_stats_ex(struct ena_adapter *adapter, u8 *buf)
+{
+	struct net_device *netdev = adapter->netdev;
+	u8 *strings_buf;
+	u64 *data_buf;
+	int strings_num;
+	int i, rc;
+
+	strings_num = ena_get_sset_count(netdev, ETH_SS_STATS);
+	if (strings_num <= 0) {
+		netif_err(adapter, drv, netdev, "Can't get stats num\n");
+		return;
+	}
+
+	strings_buf = devm_kzalloc(&adapter->pdev->dev,
+				   strings_num * ETH_GSTRING_LEN,
+				   GFP_ATOMIC);
+	if (!strings_buf) {
+		netif_err(adapter, drv, netdev,
+			  "failed to alloc strings_buf\n");
+		return;
+	}
+
+	data_buf = devm_kzalloc(&adapter->pdev->dev,
+				strings_num * sizeof(u64),
+				GFP_ATOMIC);
+	if (!data_buf) {
+		netif_err(adapter, drv, netdev,
+			  "failed to allocate data buf\n");
+		devm_kfree(&adapter->pdev->dev, strings_buf);
+		return;
+	}
+
+	ena_get_strings(netdev, ETH_SS_STATS, strings_buf);
+	ena_get_ethtool_stats(netdev, NULL, data_buf);
+
+	/* If there is a buffer, dump stats, otherwise print them to dmesg */
+	if (buf)
+		for (i = 0; i < strings_num; i++) {
+			rc = snprintf(buf, ETH_GSTRING_LEN + sizeof(u64),
+				      "%s %llu\n",
+				      strings_buf + i * ETH_GSTRING_LEN,
+				      data_buf[i]);
+			buf += rc;
+		}
+	else
+		for (i = 0; i < strings_num; i++)
+			netif_err(adapter, drv, netdev, "%s: %llu\n",
+				  strings_buf + i * ETH_GSTRING_LEN,
+				  data_buf[i]);
+
+	devm_kfree(&adapter->pdev->dev, strings_buf);
+	devm_kfree(&adapter->pdev->dev, data_buf);
+}
+
+void ena_dump_stats_to_buf(struct ena_adapter *adapter, u8 *buf)
+{
+	if (!buf)
+		return;
+
+	ena_dump_stats_ex(adapter, buf);
+}
+
+void ena_dump_stats_to_dmesg(struct ena_adapter *adapter)
+{
+	ena_dump_stats_ex(adapter, NULL);
+}
diff --git a/drivers/net/ethernet/amazon/ena/ena_netdev.c b/drivers/net/ethernet/amazon/ena/ena_netdev.c
new file mode 100644
index 000000000000..bfeaec5bd7b9
--- /dev/null
+++ b/drivers/net/ethernet/amazon/ena/ena_netdev.c
@@ -0,0 +1,3272 @@
+/*
+ * Copyright 2015 Amazon.com, Inc. or its affiliates.
+ *
+ * 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
+ * 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.
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#ifdef CONFIG_RFS_ACCEL
+#include <linux/cpu_rmap.h>
+#endif /* CONFIG_RFS_ACCEL */
+#include <linux/ethtool.h>
+#include <linux/if_vlan.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/numa.h>
+#include <linux/pci.h>
+#include <linux/utsname.h>
+#include <linux/version.h>
+#include <linux/vmalloc.h>
+#include <net/ip.h>
+
+#include "ena_netdev.h"
+#include "ena_pci_id_tbl.h"
+
+static char version[] = DEVICE_NAME " v" DRV_MODULE_VERSION "\n";
+
+MODULE_AUTHOR("Amazon.com, Inc. or its affiliates");
+MODULE_DESCRIPTION(DEVICE_NAME);
+MODULE_LICENSE("GPL");
+MODULE_VERSION(DRV_MODULE_VERSION);
+
+/* Time in jiffies before concluding the transmitter is hung. */
+#define TX_TIMEOUT  (5 * HZ)
+
+#define ENA_NAPI_BUDGET 64
+
+#define DEFAULT_MSG_ENABLE (NETIF_MSG_DRV | NETIF_MSG_PROBE | NETIF_MSG_IFUP | \
+		NETIF_MSG_TX_DONE | NETIF_MSG_TX_ERR | NETIF_MSG_RX_ERR)
+static int debug = -1;
+module_param(debug, int, 0);
+MODULE_PARM_DESC(debug, "Debug level (0=none,...,16=all)");
+
+static struct ena_aenq_handlers aenq_handlers;
+
+static struct workqueue_struct *ena_wq;
+
+MODULE_DEVICE_TABLE(pci, ena_pci_tbl);
+
+static int ena_rss_init_default(struct ena_adapter *adapter);
+
+static void ena_tx_timeout(struct net_device *dev)
+{
+	struct ena_adapter *adapter = netdev_priv(dev);
+
+	u64_stats_update_begin(&adapter->syncp);
+	adapter->dev_stats.tx_timeout++;
+	u64_stats_update_end(&adapter->syncp);
+
+	netif_err(adapter, tx_err, dev, "Transmit time out\n");
+
+	/* Change the state of the device to trigger reset */
+	set_bit(ENA_FLAG_TRIGGER_RESET, &adapter->flags);
+}
+
+static void update_rx_ring_mtu(struct ena_adapter *adapter, int mtu)
+{
+	int i;
+
+	for (i = 0; i < adapter->num_queues; i++)
+		adapter->rx_ring[i].mtu = mtu;
+}
+
+static int ena_change_mtu(struct net_device *dev, int new_mtu)
+{
+	struct ena_adapter *adapter = netdev_priv(dev);
+	int ret;
+
+	if ((new_mtu > adapter->max_mtu) || (new_mtu < ENA_MIN_MTU)) {
+		netif_err(adapter, drv, dev,
+			  "Invalid MTU setting. new_mtu: %d\n", new_mtu);
+
+		return -EINVAL;
+	}
+
+	ret = ena_com_set_dev_mtu(adapter->ena_dev, new_mtu);
+	if (!ret) {
+		netif_dbg(adapter, drv, dev, "set MTU to %d\n", new_mtu);
+		update_rx_ring_mtu(adapter, new_mtu);
+		dev->mtu = new_mtu;
+	} else {
+		netif_err(adapter, drv, dev, "Failed to set MTU to %d\n",
+			  new_mtu);
+	}
+
+	return ret;
+}
+
+static int ena_init_rx_cpu_rmap(struct ena_adapter *adapter)
+{
+#ifdef CONFIG_RFS_ACCEL
+	u32 i;
+	int rc;
+
+	adapter->netdev->rx_cpu_rmap = alloc_irq_cpu_rmap(adapter->num_queues);
+	if (!adapter->netdev->rx_cpu_rmap)
+		return -ENOMEM;
+	for (i = 0; i < adapter->num_queues; i++) {
+		int irq_idx = ENA_IO_IRQ_IDX(i);
+
+		rc = irq_cpu_rmap_add(adapter->netdev->rx_cpu_rmap,
+				      adapter->msix_entries[irq_idx].vector);
+		if (rc) {
+			free_irq_cpu_rmap(adapter->netdev->rx_cpu_rmap);
+			adapter->netdev->rx_cpu_rmap = NULL;
+			return rc;
+		}
+	}
+#endif /* CONFIG_RFS_ACCEL */
+	return 0;
+}
+
+static void ena_init_io_rings_common(struct ena_adapter *adapter,
+				     struct ena_ring *ring, u16 qid)
+{
+	ring->qid = qid;
+	ring->pdev = adapter->pdev;
+	ring->dev = &adapter->pdev->dev;
+	ring->netdev = adapter->netdev;
+	ring->napi = &adapter->ena_napi[qid].napi;
+	ring->adapter = adapter;
+	ring->ena_dev = adapter->ena_dev;
+	ring->per_napi_packets = 0;
+	ring->per_napi_bytes = 0;
+	ring->cpu = 0;
+	u64_stats_init(&ring->syncp);
+}
+
+static void ena_init_io_rings(struct ena_adapter *adapter)
+{
+	struct ena_com_dev *ena_dev;
+	struct ena_ring *txr, *rxr;
+	int i;
+
+	ena_dev = adapter->ena_dev;
+
+	for (i = 0; i < adapter->num_queues; i++) {
+		txr = &adapter->tx_ring[i];
+		rxr = &adapter->rx_ring[i];
+
+		/* TX/RX common ring state */
+		ena_init_io_rings_common(adapter, txr, i);
+		ena_init_io_rings_common(adapter, rxr, i);
+
+		/* TX specific ring state */
+		txr->ring_size = adapter->tx_ring_size;
+		txr->tx_max_header_size = ena_dev->tx_max_header_size;
+		txr->tx_mem_queue_type = ena_dev->tx_mem_queue_type;
+		txr->sgl_size = adapter->max_tx_sgl_size;
+		txr->smoothed_interval =
+			ena_com_get_nonadaptive_moderation_interval_tx(ena_dev);
+
+		/* RX specific ring state */
+		rxr->ring_size = adapter->rx_ring_size;
+		rxr->rx_copybreak = adapter->rx_copybreak;
+		rxr->sgl_size = adapter->max_rx_sgl_size;
+		rxr->smoothed_interval =
+			ena_com_get_nonadaptive_moderation_interval_rx(ena_dev);
+	}
+}
+
+/* ena_setup_tx_resources - allocate I/O Tx resources (Descriptors)
+ * @adapter: network interface device structure
+ * @qid: queue index
+ *
+ * Return 0 on success, negative on failure
+ */
+static int ena_setup_tx_resources(struct ena_adapter *adapter, int qid)
+{
+	struct ena_ring *tx_ring = &adapter->tx_ring[qid];
+	struct ena_irq *ena_irq = &adapter->irq_tbl[ENA_IO_IRQ_IDX(qid)];
+	int size, i, node;
+
+	if (tx_ring->tx_buffer_info) {
+		netif_err(adapter, ifup,
+			  adapter->netdev, "tx_buffer_info info is not NULL");
+		return -EEXIST;
+	}
+
+	size = sizeof(struct ena_tx_buffer) * tx_ring->ring_size;
+	node = cpu_to_node(ena_irq->cpu);
+
+	tx_ring->tx_buffer_info = vzalloc_node(size, node);
+	if (!tx_ring->tx_buffer_info) {
+		tx_ring->tx_buffer_info = vzalloc(size);
+		if (!tx_ring->tx_buffer_info)
+			return -ENOMEM;
+	}
+
+	size = sizeof(u16) * tx_ring->ring_size;
+	tx_ring->free_tx_ids = vzalloc_node(size, node);
+	if (!tx_ring->free_tx_ids) {
+		tx_ring->free_tx_ids = vzalloc(size);
+		if (!tx_ring->free_tx_ids) {
+			vfree(tx_ring->tx_buffer_info);
+			return -ENOMEM;
+		}
+	}
+
+	/* Req id ring for TX out of order completions */
+	for (i = 0; i < tx_ring->ring_size; i++)
+		tx_ring->free_tx_ids[i] = i;
+
+	/* Reset tx statistics */
+	memset(&tx_ring->tx_stats, 0x0, sizeof(tx_ring->tx_stats));
+
+	tx_ring->next_to_use = 0;
+	tx_ring->next_to_clean = 0;
+	tx_ring->cpu = ena_irq->cpu;
+	return 0;
+}
+
+/* ena_free_tx_resources - Free I/O Tx Resources per Queue
+ * @adapter: network interface device structure
+ * @qid: queue index
+ *
+ * Free all transmit software resources
+ */
+static void ena_free_tx_resources(struct ena_adapter *adapter, int qid)
+{
+	struct ena_ring *tx_ring = &adapter->tx_ring[qid];
+
+	vfree(tx_ring->tx_buffer_info);
+	tx_ring->tx_buffer_info = NULL;
+
+	vfree(tx_ring->free_tx_ids);
+	tx_ring->free_tx_ids = NULL;
+}
+
+/* ena_setup_all_tx_resources - allocate I/O Tx queues resources for All queues
+ * @adapter: private structure
+ *
+ * Return 0 on success, negative on failure
+ */
+static int ena_setup_all_tx_resources(struct ena_adapter *adapter)
+{
+	int i, rc = 0;
+
+	for (i = 0; i < adapter->num_queues; i++) {
+		rc = ena_setup_tx_resources(adapter, i);
+		if (rc)
+			goto err_setup_tx;
+	}
+
+	return 0;
+
+err_setup_tx:
+
+	netif_err(adapter, ifup, adapter->netdev,
+		  "Tx queue %d: allocation failed\n", i);
+
+	/* rewind the index freeing the rings as we go */
+	while (i--)
+		ena_free_tx_resources(adapter, i);
+	return rc;
+}
+
+/* ena_free_all_io_tx_resources - Free I/O Tx Resources for All Queues
+ * @adapter: board private structure
+ *
+ * Free all transmit software resources
+ */
+static void ena_free_all_io_tx_resources(struct ena_adapter *adapter)
+{
+	int i;
+
+	for (i = 0; i < adapter->num_queues; i++)
+		ena_free_tx_resources(adapter, i);
+}
+
+/* ena_setup_rx_resources - allocate I/O Rx resources (Descriptors)
+ * @adapter: network interface device structure
+ * @qid: queue index
+ *
+ * Returns 0 on success, negative on failure
+ */
+static int ena_setup_rx_resources(struct ena_adapter *adapter,
+				  u32 qid)
+{
+	struct ena_ring *rx_ring = &adapter->rx_ring[qid];
+	struct ena_irq *ena_irq = &adapter->irq_tbl[ENA_IO_IRQ_IDX(qid)];
+	int size, node;
+
+	if (rx_ring->rx_buffer_info) {
+		netif_err(adapter, ifup, adapter->netdev,
+			  "rx_buffer_info is not NULL");
+		return -EEXIST;
+	}
+
+	/* alloc extra element so in rx path
+	 * we can always prefetch rx_info + 1
+	 */
+	size = sizeof(struct ena_rx_buffer) * (rx_ring->ring_size + 1);
+	node = cpu_to_node(ena_irq->cpu);
+
+	rx_ring->rx_buffer_info = vzalloc_node(size, node);
+	if (!rx_ring->rx_buffer_info) {
+		rx_ring->rx_buffer_info = vzalloc(size);
+		if (!rx_ring->rx_buffer_info)
+			return -ENOMEM;
+	}
+
+	/* Reset rx statistics */
+	memset(&rx_ring->rx_stats, 0x0, sizeof(rx_ring->rx_stats));
+
+	rx_ring->next_to_clean = 0;
+	rx_ring->next_to_use = 0;
+	rx_ring->cpu = ena_irq->cpu;
+
+	return 0;
+}
+
+/* ena_free_rx_resources - Free I/O Rx Resources
+ * @adapter: network interface device structure
+ * @qid: queue index
+ *
+ * Free all receive software resources
+ */
+static void ena_free_rx_resources(struct ena_adapter *adapter,
+				  u32 qid)
+{
+	struct ena_ring *rx_ring = &adapter->rx_ring[qid];
+
+	vfree(rx_ring->rx_buffer_info);
+	rx_ring->rx_buffer_info = NULL;
+}
+
+/* ena_setup_all_rx_resources - allocate I/O Rx queues resources for all queues
+ * @adapter: board private structure
+ *
+ * Return 0 on success, negative on failure
+ */
+static int ena_setup_all_rx_resources(struct ena_adapter *adapter)
+{
+	int i, rc = 0;
+
+	for (i = 0; i < adapter->num_queues; i++) {
+		rc = ena_setup_rx_resources(adapter, i);
+		if (rc)
+			goto err_setup_rx;
+	}
+
+	return 0;
+
+err_setup_rx:
+
+	netif_err(adapter, ifup, adapter->netdev,
+		  "Rx queue %d: allocation failed\n", i);
+
+	/* rewind the index freeing the rings as we go */
+	while (i--)
+		ena_free_rx_resources(adapter, i);
+	return rc;
+}
+
+/* ena_free_all_io_rx_resources - Free I/O Rx Resources for All Queues
+ * @adapter: board private structure
+ *
+ * Free all receive software resources
+ */
+static void ena_free_all_io_rx_resources(struct ena_adapter *adapter)
+{
+	int i;
+
+	for (i = 0; i < adapter->num_queues; i++)
+		ena_free_rx_resources(adapter, i);
+}
+
+static inline int ena_alloc_rx_page(struct ena_ring *rx_ring,
+				    struct ena_rx_buffer *rx_info, gfp_t gfp)
+{
+	struct ena_com_buf *ena_buf;
+	struct page *page;
+	dma_addr_t dma;
+
+	/* if previous allocated page is not used */
+	if (unlikely(rx_info->page))
+		return 0;
+
+	page = alloc_page(gfp);
+	if (unlikely(!page)) {
+		u64_stats_update_begin(&rx_ring->syncp);
+		rx_ring->rx_stats.page_alloc_fail++;
+		u64_stats_update_end(&rx_ring->syncp);
+		return -ENOMEM;
+	}
+
+	dma = dma_map_page(rx_ring->dev, page, 0, PAGE_SIZE,
+			   DMA_FROM_DEVICE);
+	if (unlikely(dma_mapping_error(rx_ring->dev, dma))) {
+		u64_stats_update_begin(&rx_ring->syncp);
+		rx_ring->rx_stats.dma_mapping_err++;
+		u64_stats_update_end(&rx_ring->syncp);
+
+		__free_page(page);
+		return -EIO;
+	}
+	netif_dbg(rx_ring->adapter, rx_status, rx_ring->netdev,
+		  "alloc page %p, rx_info %p\n", page, rx_info);
+
+	rx_info->page = page;
+	rx_info->page_offset = 0;
+	ena_buf = &rx_info->ena_buf;
+	ena_buf->paddr = dma;
+	ena_buf->len = PAGE_SIZE;
+
+	return 0;
+}
+
+static void ena_free_rx_page(struct ena_ring *rx_ring,
+			     struct ena_rx_buffer *rx_info)
+{
+	struct page *page = rx_info->page;
+	struct ena_com_buf *ena_buf = &rx_info->ena_buf;
+
+	if (unlikely(!page)) {
+		netif_warn(rx_ring->adapter, rx_err, rx_ring->netdev,
+			   "Trying to free unallocated buffer\n");
+		return;
+	}
+
+	dma_unmap_page(rx_ring->dev, ena_buf->paddr, PAGE_SIZE,
+		       DMA_FROM_DEVICE);
+
+	__free_page(page);
+	rx_info->page = NULL;
+}
+
+static int ena_refill_rx_bufs(struct ena_ring *rx_ring, u32 num)
+{
+	u16 next_to_use;
+	u32 i;
+	int rc;
+
+	next_to_use = rx_ring->next_to_use;
+
+	for (i = 0; i < num; i++) {
+		struct ena_rx_buffer *rx_info =
+			&rx_ring->rx_buffer_info[next_to_use];
+
+		rc = ena_alloc_rx_page(rx_ring, rx_info,
+				       __GFP_COLD | GFP_ATOMIC | __GFP_COMP);
+		if (unlikely(rc < 0)) {
+			netif_warn(rx_ring->adapter, rx_err, rx_ring->netdev,
+				   "failed to alloc buffer for rx queue %d\n",
+				   rx_ring->qid);
+			break;
+		}
+		rc = ena_com_add_single_rx_desc(rx_ring->ena_com_io_sq,
+						&rx_info->ena_buf,
+						next_to_use);
+		if (unlikely(rc)) {
+			netif_warn(rx_ring->adapter, rx_status, rx_ring->netdev,
+				   "failed to add buffer for rx queue %d\n",
+				   rx_ring->qid);
+			break;
+		}
+		next_to_use = ENA_RX_RING_IDX_NEXT(next_to_use,
+						   rx_ring->ring_size);
+	}
+
+	if (unlikely(i < num)) {
+		u64_stats_update_begin(&rx_ring->syncp);
+		rx_ring->rx_stats.refil_partial++;
+		u64_stats_update_end(&rx_ring->syncp);
+		netdev_warn(rx_ring->netdev,
+			    "refilled rx qid %d with only %d buffers (from %d)\n",
+			    rx_ring->qid, i, num);
+	}
+
+	if (likely(i)) {
+		/* Add memory barrier to make sure the desc were written before
+		 * issue a doorbell
+		 */
+		wmb();
+		ena_com_write_sq_doorbell(rx_ring->ena_com_io_sq);
+	}
+
+	rx_ring->next_to_use = next_to_use;
+
+	return i;
+}
+
+static void ena_free_rx_bufs(struct ena_adapter *adapter,
+			     u32 qid)
+{
+	struct ena_ring *rx_ring = &adapter->rx_ring[qid];
+	u32 i;
+
+	for (i = 0; i < rx_ring->ring_size; i++) {
+		struct ena_rx_buffer *rx_info = &rx_ring->rx_buffer_info[i];
+
+		if (rx_info->page)
+			ena_free_rx_page(rx_ring, rx_info);
+	}
+}
+
+/* ena_refill_all_rx_bufs - allocate all queues Rx buffers
+ * @adapter: board private structure
+ *
+ */
+static void ena_refill_all_rx_bufs(struct ena_adapter *adapter)
+{
+	struct ena_ring *rx_ring;
+	int i, rc, bufs_num;
+
+	for (i = 0; i < adapter->num_queues; i++) {
+		rx_ring = &adapter->rx_ring[i];
+		bufs_num = rx_ring->ring_size - 1;
+		rc = ena_refill_rx_bufs(rx_ring, bufs_num);
+
+		if (unlikely(rc != bufs_num))
+			netif_warn(rx_ring->adapter, rx_status, rx_ring->netdev,
+				   "refilling Queue %d failed. allocated %d buffers from: %d\n",
+				   i, rc, bufs_num);
+	}
+}
+
+static void ena_free_all_rx_bufs(struct ena_adapter *adapter)
+{
+	int i;
+
+	for (i = 0; i < adapter->num_queues; i++)
+		ena_free_rx_bufs(adapter, i);
+}
+
+/* ena_free_tx_bufs - Free Tx Buffers per Queue
+ * @tx_ring: TX ring for which buffers be freed
+ */
+static void ena_free_tx_bufs(struct ena_ring *tx_ring)
+{
+	u32 i;
+
+	for (i = 0; i < tx_ring->ring_size; i++) {
+		struct ena_tx_buffer *tx_info = &tx_ring->tx_buffer_info[i];
+		struct ena_com_buf *ena_buf;
+		int nr_frags;
+		int j;
+
+		if (!tx_info->skb)
+			continue;
+
+		netdev_notice(tx_ring->netdev,
+			      "free uncompleted tx skb qid %d idx 0x%x\n",
+			      tx_ring->qid, i);
+
+		ena_buf = tx_info->bufs;
+		dma_unmap_single(tx_ring->dev,
+				 ena_buf->paddr,
+				 ena_buf->len,
+				 DMA_TO_DEVICE);
+
+		/* unmap remaining mapped pages */
+		nr_frags = tx_info->num_of_bufs - 1;
+		for (j = 0; j < nr_frags; j++) {
+			ena_buf++;
+			dma_unmap_page(tx_ring->dev,
+				       ena_buf->paddr,
+				       ena_buf->len,
+				       DMA_TO_DEVICE);
+		}
+
+		dev_kfree_skb_any(tx_info->skb);
+	}
+	netdev_tx_reset_queue(netdev_get_tx_queue(tx_ring->netdev,
+						  tx_ring->qid));
+}
+
+static void ena_free_all_tx_bufs(struct ena_adapter *adapter)
+{
+	struct ena_ring *tx_ring;
+	int i;
+
+	for (i = 0; i < adapter->num_queues; i++) {
+		tx_ring = &adapter->tx_ring[i];
+		ena_free_tx_bufs(tx_ring);
+	}
+}
+
+static void ena_destroy_all_tx_queues(struct ena_adapter *adapter)
+{
+	u16 ena_qid;
+	int i;
+
+	for (i = 0; i < adapter->num_queues; i++) {
+		ena_qid = ENA_IO_TXQ_IDX(i);
+		ena_com_destroy_io_queue(adapter->ena_dev, ena_qid);
+	}
+}
+
+static void ena_destroy_all_rx_queues(struct ena_adapter *adapter)
+{
+	u16 ena_qid;
+	int i;
+
+	for (i = 0; i < adapter->num_queues; i++) {
+		ena_qid = ENA_IO_RXQ_IDX(i);
+		ena_com_destroy_io_queue(adapter->ena_dev, ena_qid);
+	}
+}
+
+static void ena_destroy_all_io_queues(struct ena_adapter *adapter)
+{
+	ena_destroy_all_tx_queues(adapter);
+	ena_destroy_all_rx_queues(adapter);
+}
+
+static int validate_tx_req_id(struct ena_ring *tx_ring, u16 req_id)
+{
+	struct ena_tx_buffer *tx_info = NULL;
+
+	if (likely(req_id < tx_ring->ring_size)) {
+		tx_info = &tx_ring->tx_buffer_info[req_id];
+		if (likely(tx_info->skb))
+			return 0;
+	}
+
+	if (tx_info)
+		netif_err(tx_ring->adapter, tx_done, tx_ring->netdev,
+			  "tx_info doesn't have valid skb\n");
+	else
+		netif_err(tx_ring->adapter, tx_done, tx_ring->netdev,
+			  "Invalid req_id: %hu\n", req_id);
+
+	u64_stats_update_begin(&tx_ring->syncp);
+	tx_ring->tx_stats.bad_req_id++;
+	u64_stats_update_end(&tx_ring->syncp);
+
+	/* Trigger device reset */
+	set_bit(ENA_FLAG_TRIGGER_RESET, &tx_ring->adapter->flags);
+	return -EFAULT;
+}
+
+static int ena_clean_tx_irq(struct ena_ring *tx_ring, u32 budget)
+{
+	struct netdev_queue *txq;
+	bool above_thresh;
+	u32 tx_bytes = 0;
+	u32 total_done = 0;
+	u16 next_to_clean;
+	u16 req_id;
+	int tx_pkts = 0;
+	int rc;
+
+	next_to_clean = tx_ring->next_to_clean;
+	txq = netdev_get_tx_queue(tx_ring->netdev, tx_ring->qid);
+
+	while (tx_pkts < budget) {
+		struct ena_tx_buffer *tx_info;
+		struct sk_buff *skb;
+		struct ena_com_buf *ena_buf;
+		int i, nr_frags;
+
+		rc = ena_com_tx_comp_req_id_get(tx_ring->ena_com_io_cq,
+						&req_id);
+		if (rc)
+			break;
+
+		rc = validate_tx_req_id(tx_ring, req_id);
+		if (rc)
+			break;
+
+		tx_info = &tx_ring->tx_buffer_info[req_id];
+		skb = tx_info->skb;
+
+		/* prefetch skb_end_pointer() to speedup skb_shinfo(skb) */
+		prefetch(&skb->end);
+
+		tx_info->skb = NULL;
+		tx_info->last_jiffies = 0;
+
+		if (likely(tx_info->num_of_bufs != 0)) {
+			ena_buf = tx_info->bufs;
+
+			dma_unmap_single(tx_ring->dev,
+					 dma_unmap_addr(ena_buf, paddr),
+					 dma_unmap_len(ena_buf, len),
+					 DMA_TO_DEVICE);
+
+			/* unmap remaining mapped pages */
+			nr_frags = tx_info->num_of_bufs - 1;
+			for (i = 0; i < nr_frags; i++) {
+				ena_buf++;
+				dma_unmap_page(tx_ring->dev,
+					       dma_unmap_addr(ena_buf, paddr),
+					       dma_unmap_len(ena_buf, len),
+					       DMA_TO_DEVICE);
+			}
+		}
+
+		netif_dbg(tx_ring->adapter, tx_done, tx_ring->netdev,
+			  "tx_poll: q %d skb %p completed\n", tx_ring->qid,
+			  skb);
+
+		tx_bytes += skb->len;
+		dev_kfree_skb(skb);
+		tx_pkts++;
+		total_done += tx_info->tx_descs;
+
+		tx_ring->free_tx_ids[next_to_clean] = req_id;
+		next_to_clean = ENA_TX_RING_IDX_NEXT(next_to_clean,
+						     tx_ring->ring_size);
+	}
+
+	tx_ring->next_to_clean = next_to_clean;
+	ena_com_comp_ack(tx_ring->ena_com_io_sq, total_done);
+	ena_com_update_dev_comp_head(tx_ring->ena_com_io_cq);
+
+	netdev_tx_completed_queue(txq, tx_pkts, tx_bytes);
+
+	netif_dbg(tx_ring->adapter, tx_done, tx_ring->netdev,
+		  "tx_poll: q %d done. total pkts: %d\n",
+		  tx_ring->qid, tx_pkts);
+
+	/* need to make the rings circular update visible to
+	 * ena_start_xmit() before checking for netif_queue_stopped().
+	 */
+	smp_mb();
+
+	above_thresh = ena_com_sq_empty_space(tx_ring->ena_com_io_sq) >
+		ENA_TX_WAKEUP_THRESH;
+	if (unlikely(netif_tx_queue_stopped(txq) && above_thresh)) {
+		__netif_tx_lock(txq, smp_processor_id());
+		above_thresh = ena_com_sq_empty_space(tx_ring->ena_com_io_sq) >
+			ENA_TX_WAKEUP_THRESH;
+		if (netif_tx_queue_stopped(txq) && above_thresh) {
+			netif_tx_wake_queue(txq);
+			u64_stats_update_begin(&tx_ring->syncp);
+			tx_ring->tx_stats.queue_wakeup++;
+			u64_stats_update_end(&tx_ring->syncp);
+		}
+		__netif_tx_unlock(txq);
+	}
+
+	tx_ring->per_napi_bytes += tx_bytes;
+	tx_ring->per_napi_packets += tx_pkts;
+
+	return tx_pkts;
+}
+
+static struct sk_buff *ena_rx_skb(struct ena_ring *rx_ring,
+				  struct ena_com_rx_buf_info *ena_bufs,
+				  u32 descs,
+				  u16 *next_to_clean)
+{
+	struct sk_buff *skb;
+	struct ena_rx_buffer *rx_info =
+		&rx_ring->rx_buffer_info[*next_to_clean];
+	u32 len;
+	u32 buf = 0;
+	void *va;
+
+	len = ena_bufs[0].len;
+	if (unlikely(!rx_info->page)) {
+		netif_err(rx_ring->adapter, rx_err, rx_ring->netdev,
+			  "Page is NULL\n");
+		return NULL;
+	}
+
+	netif_dbg(rx_ring->adapter, rx_status, rx_ring->netdev,
+		  "rx_info %p page %p\n",
+		  rx_info, rx_info->page);
+
+	/* save virt address of first buffer */
+	va = page_address(rx_info->page) + rx_info->page_offset;
+	prefetch(va + NET_IP_ALIGN);
+
+	if (len <= rx_ring->rx_copybreak) {
+		skb = netdev_alloc_skb_ip_align(rx_ring->netdev,
+						rx_ring->rx_copybreak);
+		if (unlikely(!skb)) {
+			u64_stats_update_begin(&rx_ring->syncp);
+			rx_ring->rx_stats.skb_alloc_fail++;
+			u64_stats_update_end(&rx_ring->syncp);
+			netif_err(rx_ring->adapter, rx_err, rx_ring->netdev,
+				  "Failed to allocate skb\n");
+			return NULL;
+		}
+
+		netif_dbg(rx_ring->adapter, rx_status, rx_ring->netdev,
+			  "rx allocated small packet. len %d. data_len %d\n",
+			  skb->len, skb->data_len);
+
+		/* sync this buffer for CPU use */
+		dma_sync_single_for_cpu(rx_ring->dev,
+					dma_unmap_addr(&rx_info->ena_buf, paddr),
+					len,
+					DMA_FROM_DEVICE);
+		skb_copy_to_linear_data(skb, va, len);
+		dma_sync_single_for_device(rx_ring->dev,
+					   dma_unmap_addr(&rx_info->ena_buf, paddr),
+					   len,
+					   DMA_FROM_DEVICE);
+
+		skb_put(skb, len);
+		skb->protocol = eth_type_trans(skb, rx_ring->netdev);
+		*next_to_clean = ENA_RX_RING_IDX_ADD(*next_to_clean, descs,
+						     rx_ring->ring_size);
+		return skb;
+	}
+
+	skb = napi_get_frags(rx_ring->napi);
+	if (unlikely(!skb)) {
+		netif_dbg(rx_ring->adapter, rx_status, rx_ring->netdev,
+			  "Failed allocating skb\n");
+		u64_stats_update_begin(&rx_ring->syncp);
+		rx_ring->rx_stats.skb_alloc_fail++;
+		u64_stats_update_end(&rx_ring->syncp);
+		return NULL;
+	}
+
+	do {
+		dma_unmap_page(rx_ring->dev,
+			       dma_unmap_addr(&rx_info->ena_buf, paddr),
+			       PAGE_SIZE, DMA_FROM_DEVICE);
+
+		skb_add_rx_frag(skb, skb_shinfo(skb)->nr_frags, rx_info->page,
+				rx_info->page_offset, len, PAGE_SIZE);
+
+		netif_dbg(rx_ring->adapter, rx_status, rx_ring->netdev,
+			  "rx skb updated. len %d. data_len %d\n",
+			  skb->len, skb->data_len);
+
+		rx_info->page = NULL;
+		*next_to_clean =
+			ENA_RX_RING_IDX_NEXT(*next_to_clean,
+					     rx_ring->ring_size);
+		if (likely(--descs == 0))
+			break;
+		rx_info = &rx_ring->rx_buffer_info[*next_to_clean];
+		len = ena_bufs[++buf].len;
+	} while (1);
+
+	return skb;
+}
+
+/* ena_rx_checksum - indicate in skb if hw indicated a good cksum
+ * @adapter: structure containing adapter specific data
+ * @ena_rx_ctx: received packet context/metadata
+ * @skb: skb currently being received and modified
+ */
+static inline void ena_rx_checksum(struct ena_ring *rx_ring,
+				   struct ena_com_rx_ctx *ena_rx_ctx,
+				   struct sk_buff *skb)
+{
+	/* Rx csum disabled */
+	if (unlikely(!(rx_ring->netdev->features & NETIF_F_RXCSUM))) {
+		skb->ip_summed = CHECKSUM_NONE;
+		return;
+	}
+
+	/* For fragmented packets the checksum isn't valid */
+	if (ena_rx_ctx->frag) {
+		skb->ip_summed = CHECKSUM_NONE;
+		return;
+	}
+
+	/* if IP and error */
+	if (unlikely((ena_rx_ctx->l3_proto == ENA_ETH_IO_L3_PROTO_IPV4) &&
+		     (ena_rx_ctx->l3_csum_err))) {
+		/* ipv4 checksum error */
+		skb->ip_summed = CHECKSUM_NONE;
+		u64_stats_update_begin(&rx_ring->syncp);
+		rx_ring->rx_stats.bad_csum++;
+		u64_stats_update_end(&rx_ring->syncp);
+		netif_err(rx_ring->adapter, rx_err, rx_ring->netdev,
+			  "RX IPv4 header checksum error\n");
+		return;
+	}
+
+	/* if TCP/UDP */
+	if (likely((ena_rx_ctx->l4_proto == ENA_ETH_IO_L4_PROTO_TCP) ||
+		   (ena_rx_ctx->l4_proto == ENA_ETH_IO_L4_PROTO_UDP))) {
+		if (unlikely(ena_rx_ctx->l4_csum_err)) {
+			/* TCP/UDP checksum error */
+			u64_stats_update_begin(&rx_ring->syncp);
+			rx_ring->rx_stats.bad_csum++;
+			u64_stats_update_end(&rx_ring->syncp);
+			netif_err(rx_ring->adapter, rx_err, rx_ring->netdev,
+				  "RX L4 checksum error\n");
+			skb->ip_summed = CHECKSUM_NONE;
+			return;
+		}
+
+		skb->ip_summed = CHECKSUM_UNNECESSARY;
+	}
+}
+
+static void ena_set_rx_hash(struct ena_ring *rx_ring,
+			    struct ena_com_rx_ctx *ena_rx_ctx,
+			    struct sk_buff *skb)
+{
+	enum pkt_hash_types hash_type;
+
+	if (likely(rx_ring->netdev->features & NETIF_F_RXHASH)) {
+		if (likely((ena_rx_ctx->l4_proto == ENA_ETH_IO_L4_PROTO_TCP) ||
+			   (ena_rx_ctx->l4_proto == ENA_ETH_IO_L4_PROTO_UDP)))
+
+			hash_type = PKT_HASH_TYPE_L4;
+		else
+			hash_type = PKT_HASH_TYPE_NONE;
+
+		/* Override hash type if the packet is fragmented */
+		if (ena_rx_ctx->frag)
+			hash_type = PKT_HASH_TYPE_NONE;
+
+		skb_set_hash(skb, ena_rx_ctx->hash, hash_type);
+	}
+}
+
+/* ena_clean_rx_irq - Cleanup RX irq
+ * @rx_ring: RX ring to clean
+ * @napi: napi handler
+ * @budget: how many packets driver is allowed to clean
+ *
+ * Returns the number of cleaned buffers.
+ */
+static int ena_clean_rx_irq(struct ena_ring *rx_ring, struct napi_struct *napi,
+			    u32 budget)
+{
+	u16 next_to_clean = rx_ring->next_to_clean;
+	u32 res_budget, work_done;
+
+	struct ena_com_rx_ctx ena_rx_ctx;
+	struct ena_adapter *adapter;
+	struct sk_buff *skb;
+	int refill_required;
+	int refill_threshold;
+	int rc = 0;
+	int total_len = 0;
+	int rx_copybreak_pkt = 0;
+
+	netif_dbg(rx_ring->adapter, rx_status, rx_ring->netdev,
+		  "%s qid %d\n", __func__, rx_ring->qid);
+	res_budget = budget;
+
+	do {
+		ena_rx_ctx.ena_bufs = rx_ring->ena_bufs;
+		ena_rx_ctx.max_bufs = rx_ring->sgl_size;
+		ena_rx_ctx.descs = 0;
+		rc = ena_com_rx_pkt(rx_ring->ena_com_io_cq,
+				    rx_ring->ena_com_io_sq,
+				    &ena_rx_ctx);
+		if (unlikely(rc))
+			goto error;
+
+		if (unlikely(ena_rx_ctx.descs == 0))
+			break;
+
+		netif_dbg(rx_ring->adapter, rx_status, rx_ring->netdev,
+			  "rx_poll: q %d got packet from ena. descs #: %d l3 proto %d l4 proto %d hash: %x\n",
+			  rx_ring->qid, ena_rx_ctx.descs, ena_rx_ctx.l3_proto,
+			  ena_rx_ctx.l4_proto, ena_rx_ctx.hash);
+
+		/* allocate skb and fill it */
+		skb = ena_rx_skb(rx_ring, rx_ring->ena_bufs, ena_rx_ctx.descs,
+				 &next_to_clean);
+
+		/* exit if we failed to retrieve a buffer */
+		if (unlikely(!skb)) {
+			next_to_clean = ENA_RX_RING_IDX_ADD(next_to_clean,
+							    ena_rx_ctx.descs,
+							    rx_ring->ring_size);
+			break;
+		}
+
+		ena_rx_checksum(rx_ring, &ena_rx_ctx, skb);
+
+		ena_set_rx_hash(rx_ring, &ena_rx_ctx, skb);
+
+		skb_record_rx_queue(skb, rx_ring->qid);
+
+		if (rx_ring->ena_bufs[0].len <= rx_ring->rx_copybreak) {
+			total_len += rx_ring->ena_bufs[0].len;
+			rx_copybreak_pkt++;
+			napi_gro_receive(napi, skb);
+		} else {
+			total_len += skb->len;
+			napi_gro_frags(napi);
+		}
+
+		res_budget--;
+	} while (likely(res_budget));
+
+	work_done = budget - res_budget;
+	rx_ring->per_napi_bytes += total_len;
+	rx_ring->per_napi_packets += work_done;
+	u64_stats_update_begin(&rx_ring->syncp);
+	rx_ring->rx_stats.bytes += total_len;
+	rx_ring->rx_stats.cnt += work_done;
+	rx_ring->rx_stats.rx_copybreak_pkt += rx_copybreak_pkt;
+	u64_stats_update_end(&rx_ring->syncp);
+
+	rx_ring->next_to_clean = next_to_clean;
+
+	refill_required = ena_com_sq_empty_space(rx_ring->ena_com_io_sq);
+	refill_threshold = rx_ring->ring_size / ENA_RX_REFILL_THRESH_DIVIDER;
+
+	/* Optimization, try to batch new rx buffers */
+	if (refill_required > refill_threshold) {
+		ena_com_update_dev_comp_head(rx_ring->ena_com_io_cq);
+		ena_refill_rx_bufs(rx_ring, refill_required);
+	}
+
+	return work_done;
+
+error:
+	adapter = netdev_priv(rx_ring->netdev);
+
+	u64_stats_update_begin(&rx_ring->syncp);
+	rx_ring->rx_stats.bad_desc_num++;
+	u64_stats_update_end(&rx_ring->syncp);
+
+	/* Too many desc from the device. Trigger reset */
+	set_bit(ENA_FLAG_TRIGGER_RESET, &adapter->flags);
+
+	return 0;
+}
+
+inline void ena_adjust_intr_moderation(struct ena_ring *rx_ring,
+				       struct ena_ring *tx_ring)
+{
+	/* We apply adaptive moderation on Rx path only.
+	 * Tx uses static interrupt moderation.
+	 */
+	ena_com_calculate_interrupt_delay(rx_ring->ena_dev,
+					  rx_ring->per_napi_packets,
+					  rx_ring->per_napi_bytes,
+					  &rx_ring->smoothed_interval,
+					  &rx_ring->moder_tbl_idx);
+
+	/* Reset per napi packets/bytes */
+	tx_ring->per_napi_packets = 0;
+	tx_ring->per_napi_bytes = 0;
+	rx_ring->per_napi_packets = 0;
+	rx_ring->per_napi_bytes = 0;
+}
+
+static inline void ena_update_ring_numa_node(struct ena_ring *tx_ring,
+					     struct ena_ring *rx_ring)
+{
+	int cpu = get_cpu();
+	int numa_node;
+
+	/* Check only one ring since the 2 rings are running on the same cpu */
+	if (likely(tx_ring->cpu == cpu))
+		goto out;
+
+	numa_node = cpu_to_node(cpu);
+	put_cpu();
+
+	if (numa_node != NUMA_NO_NODE) {
+		ena_com_update_numa_node(tx_ring->ena_com_io_cq, numa_node);
+		ena_com_update_numa_node(rx_ring->ena_com_io_cq, numa_node);
+	}
+
+	tx_ring->cpu = cpu;
+	rx_ring->cpu = cpu;
+
+	return;
+out:
+	put_cpu();
+}
+
+static int ena_io_poll(struct napi_struct *napi, int budget)
+{
+	struct ena_napi *ena_napi = container_of(napi, struct ena_napi, napi);
+	struct ena_ring *tx_ring, *rx_ring;
+	struct ena_eth_io_intr_reg intr_reg;
+
+	u32 tx_work_done;
+	u32 rx_work_done;
+	int tx_budget;
+	int napi_comp_call = 0;
+	int ret;
+
+	tx_ring = ena_napi->tx_ring;
+	rx_ring = ena_napi->rx_ring;
+
+	tx_budget = tx_ring->ring_size / ENA_TX_POLL_BUDGET_DIVIDER;
+
+	if (!test_bit(ENA_FLAG_DEV_UP, &tx_ring->adapter->flags)) {
+		napi_complete_done(napi, 0);
+		return 0;
+	}
+
+	tx_work_done = ena_clean_tx_irq(tx_ring, tx_budget);
+	rx_work_done = ena_clean_rx_irq(rx_ring, napi, budget);
+
+	if ((budget > rx_work_done) && (tx_budget > tx_work_done)) {
+		napi_complete_done(napi, rx_work_done);
+
+		napi_comp_call = 1;
+		/* Tx and Rx share the same interrupt vector */
+		if (ena_com_get_adaptive_moderation_enabled(rx_ring->ena_dev))
+			ena_adjust_intr_moderation(rx_ring, tx_ring);
+
+		/* Update intr register: rx intr delay, tx intr delay and
+		 * interrupt unmask
+		 */
+		ena_com_update_intr_reg(&intr_reg,
+					rx_ring->smoothed_interval,
+					tx_ring->smoothed_interval,
+					true);
+
+		/* It is a shared MSI-X. Tx and Rx CQ have pointer to it.
+		 * So we use one of them to reach the intr reg
+		 */
+		ena_com_unmask_intr(rx_ring->ena_com_io_cq, &intr_reg);
+
+		ena_update_ring_numa_node(tx_ring, rx_ring);
+
+		ret = rx_work_done;
+	} else {
+		ret = budget;
+	}
+
+	u64_stats_update_begin(&tx_ring->syncp);
+	tx_ring->tx_stats.napi_comp += napi_comp_call;
+	tx_ring->tx_stats.tx_poll++;
+	u64_stats_update_end(&tx_ring->syncp);
+
+	return ret;
+}
+
+static irqreturn_t ena_intr_msix_mgmnt(int irq, void *data)
+{
+	struct ena_adapter *adapter = (struct ena_adapter *)data;
+
+	ena_com_admin_q_comp_intr_handler(adapter->ena_dev);
+
+	/* Don't call the aenq handler before probe is done */
+	if (likely(test_bit(ENA_FLAG_DEVICE_RUNNING, &adapter->flags)))
+		ena_com_aenq_intr_handler(adapter->ena_dev, data);
+
+	return IRQ_HANDLED;
+}
+
+/* ena_intr_msix_io - MSI-X Interrupt Handler for Tx/Rx
+ * @irq: interrupt number
+ * @data: pointer to a network interface private napi device structure
+ */
+static irqreturn_t ena_intr_msix_io(int irq, void *data)
+{
+	struct ena_napi *ena_napi = data;
+
+	napi_schedule(&ena_napi->napi);
+
+	return IRQ_HANDLED;
+}
+
+static int ena_enable_msix(struct ena_adapter *adapter, int num_queues)
+{
+	int i, msix_vecs, rc;
+
+	if (test_bit(ENA_FLAG_MSIX_ENABLED, &adapter->flags)) {
+		netif_err(adapter, probe, adapter->netdev,
+			  "Error, MSI-X is already enabled\n");
+		return -EPERM;
+	}
+
+	/* Reserved the max msix vectors we might need */
+	msix_vecs = ENA_MAX_MSIX_VEC(num_queues);
+
+	netif_dbg(adapter, probe, adapter->netdev,
+		  "trying to enable MSI-X, vectors %d\n", msix_vecs);
+
+	adapter->msix_entries = vzalloc(msix_vecs * sizeof(struct msix_entry));
+
+	if (!adapter->msix_entries)
+		return -ENOMEM;
+
+	for (i = 0; i < msix_vecs; i++)
+		adapter->msix_entries[i].entry = i;
+
+	rc = pci_enable_msix(adapter->pdev, adapter->msix_entries, msix_vecs);
+	if (rc != 0) {
+		netif_err(adapter, probe, adapter->netdev,
+			  "Failed to enable MSI-X, vectors %d rc %d\n",
+			  msix_vecs, rc);
+		return -ENOSPC;
+	}
+
+	netif_dbg(adapter, probe, adapter->netdev, "enable MSI-X, vectors %d\n",
+		  msix_vecs);
+
+	if (msix_vecs >= 1) {
+		if (ena_init_rx_cpu_rmap(adapter))
+			netif_warn(adapter, probe, adapter->netdev,
+				   "Failed to map IRQs to CPUs\n");
+	}
+
+	adapter->msix_vecs = msix_vecs;
+	set_bit(ENA_FLAG_MSIX_ENABLED, &adapter->flags);
+
+	return 0;
+}
+
+static void ena_setup_mgmnt_intr(struct ena_adapter *adapter)
+{
+	u32 cpu;
+
+	snprintf(adapter->irq_tbl[ENA_MGMNT_IRQ_IDX].name,
+		 ENA_IRQNAME_SIZE, "ena-mgmnt@pci:%s",
+		 pci_name(adapter->pdev));
+	adapter->irq_tbl[ENA_MGMNT_IRQ_IDX].handler =
+		ena_intr_msix_mgmnt;
+	adapter->irq_tbl[ENA_MGMNT_IRQ_IDX].data = adapter;
+	adapter->irq_tbl[ENA_MGMNT_IRQ_IDX].vector =
+		adapter->msix_entries[ENA_MGMNT_IRQ_IDX].vector;
+	cpu = cpumask_first(cpu_online_mask);
+	adapter->irq_tbl[ENA_MGMNT_IRQ_IDX].cpu = cpu;
+	cpumask_set_cpu(cpu,
+			&adapter->irq_tbl[ENA_MGMNT_IRQ_IDX].affinity_hint_mask);
+}
+
+static void ena_setup_io_intr(struct ena_adapter *adapter)
+{
+	struct net_device *netdev;
+	int irq_idx, i, cpu;
+
+	netdev = adapter->netdev;
+
+	for (i = 0; i < adapter->num_queues; i++) {
+		irq_idx = ENA_IO_IRQ_IDX(i);
+		cpu = i % num_online_cpus();
+
+		snprintf(adapter->irq_tbl[irq_idx].name, ENA_IRQNAME_SIZE,
+			 "%s-Tx-Rx-%d", netdev->name, i);
+		adapter->irq_tbl[irq_idx].handler = ena_intr_msix_io;
+		adapter->irq_tbl[irq_idx].data = &adapter->ena_napi[i];
+		adapter->irq_tbl[irq_idx].vector =
+			adapter->msix_entries[irq_idx].vector;
+		adapter->irq_tbl[irq_idx].cpu = cpu;
+
+		cpumask_set_cpu(cpu,
+				&adapter->irq_tbl[irq_idx].affinity_hint_mask);
+	}
+}
+
+static int ena_request_mgmnt_irq(struct ena_adapter *adapter)
+{
+	unsigned long flags = 0;
+	struct ena_irq *irq;
+	int rc;
+
+	irq = &adapter->irq_tbl[ENA_MGMNT_IRQ_IDX];
+	rc = request_irq(irq->vector, irq->handler, flags, irq->name,
+			 irq->data);
+	if (rc) {
+		netif_err(adapter, probe, adapter->netdev,
+			  "failed to request admin irq\n");
+		return rc;
+	}
+
+	netif_dbg(adapter, probe, adapter->netdev,
+		  "set affinity hint of mgmnt irq.to 0x%lx (irq vector: %d)\n",
+		  irq->affinity_hint_mask.bits[0], irq->vector);
+
+	irq_set_affinity_hint(irq->vector, &irq->affinity_hint_mask);
+
+	return rc;
+}
+
+static int ena_request_io_irq(struct ena_adapter *adapter)
+{
+	unsigned long flags = 0;
+	struct ena_irq *irq;
+	int rc = 0, i, k;
+
+	if (!test_bit(ENA_FLAG_MSIX_ENABLED, &adapter->flags)) {
+		netif_err(adapter, ifup, adapter->netdev,
+			  "Failed to request I/O IRQ: MSI-X is not enabled\n");
+		return -EINVAL;
+	}
+
+	for (i = ENA_IO_IRQ_FIRST_IDX; i < adapter->msix_vecs; i++) {
+		irq = &adapter->irq_tbl[i];
+		rc = request_irq(irq->vector, irq->handler, flags, irq->name,
+				 irq->data);
+		if (rc) {
+			netif_err(adapter, ifup, adapter->netdev,
+				  "Failed to request I/O IRQ. index %d rc %d\n",
+				   i, rc);
+			goto err;
+		}
+
+		netif_dbg(adapter, ifup, adapter->netdev,
+			  "set affinity hint of irq. index %d to 0x%lx (irq vector: %d)\n",
+			  i, irq->affinity_hint_mask.bits[0], irq->vector);
+
+		irq_set_affinity_hint(irq->vector, &irq->affinity_hint_mask);
+	}
+
+	return rc;
+
+err:
+	for (k = ENA_IO_IRQ_FIRST_IDX; k < i; k++) {
+		irq = &adapter->irq_tbl[k];
+		free_irq(irq->vector, irq->data);
+	}
+
+	return rc;
+}
+
+static void ena_free_mgmnt_irq(struct ena_adapter *adapter)
+{
+	struct ena_irq *irq;
+
+	irq = &adapter->irq_tbl[ENA_MGMNT_IRQ_IDX];
+	synchronize_irq(irq->vector);
+	irq_set_affinity_hint(irq->vector, NULL);
+	free_irq(irq->vector, irq->data);
+}
+
+static void ena_free_io_irq(struct ena_adapter *adapter)
+{
+	struct ena_irq *irq;
+	int i;
+
+#ifdef CONFIG_RFS_ACCEL
+	if (adapter->msix_vecs >= 1) {
+		free_irq_cpu_rmap(adapter->netdev->rx_cpu_rmap);
+		adapter->netdev->rx_cpu_rmap = NULL;
+	}
+#endif /* CONFIG_RFS_ACCEL */
+
+	for (i = ENA_IO_IRQ_FIRST_IDX; i < adapter->msix_vecs; i++) {
+		irq = &adapter->irq_tbl[i];
+		irq_set_affinity_hint(irq->vector, NULL);
+		free_irq(irq->vector, irq->data);
+	}
+}
+
+static void ena_disable_msix(struct ena_adapter *adapter)
+{
+	if (test_and_clear_bit(ENA_FLAG_MSIX_ENABLED, &adapter->flags))
+		pci_disable_msix(adapter->pdev);
+
+	if (adapter->msix_entries)
+		vfree(adapter->msix_entries);
+	adapter->msix_entries = NULL;
+}
+
+static void ena_disable_io_intr_sync(struct ena_adapter *adapter)
+{
+	int i;
+
+	if (!netif_running(adapter->netdev))
+		return;
+
+	for (i = ENA_IO_IRQ_FIRST_IDX; i < adapter->msix_vecs; i++)
+		synchronize_irq(adapter->irq_tbl[i].vector);
+}
+
+static void ena_del_napi(struct ena_adapter *adapter)
+{
+	int i;
+
+	for (i = 0; i < adapter->num_queues; i++)
+		netif_napi_del(&adapter->ena_napi[i].napi);
+}
+
+static void ena_init_napi(struct ena_adapter *adapter)
+{
+	struct ena_napi *napi;
+	int i;
+
+	for (i = 0; i < adapter->num_queues; i++) {
+		napi = &adapter->ena_napi[i];
+
+		netif_napi_add(adapter->netdev,
+			       &adapter->ena_napi[i].napi,
+			       ena_io_poll,
+			       ENA_NAPI_BUDGET);
+		napi->rx_ring = &adapter->rx_ring[i];
+		napi->tx_ring = &adapter->tx_ring[i];
+		napi->qid = i;
+	}
+}
+
+static void ena_napi_disable_all(struct ena_adapter *adapter)
+{
+	int i;
+
+	for (i = 0; i < adapter->num_queues; i++)
+		napi_disable(&adapter->ena_napi[i].napi);
+}
+
+static void ena_napi_enable_all(struct ena_adapter *adapter)
+{
+	int i;
+
+	for (i = 0; i < adapter->num_queues; i++)
+		napi_enable(&adapter->ena_napi[i].napi);
+}
+
+static void ena_restore_ethtool_params(struct ena_adapter *adapter)
+{
+	adapter->tx_usecs = 0;
+	adapter->rx_usecs = 0;
+	adapter->tx_frames = 1;
+	adapter->rx_frames = 1;
+}
+
+/* Configure the Rx forwarding */
+static int ena_rss_configure(struct ena_adapter *adapter)
+{
+	struct ena_com_dev *ena_dev = adapter->ena_dev;
+	int rc;
+
+	/* In case the RSS table wasn't initialized by probe */
+	if (!ena_dev->rss.tbl_log_size) {
+		rc = ena_rss_init_default(adapter);
+		if (rc && (rc != -EPERM)) {
+			netif_err(adapter, ifup, adapter->netdev,
+				  "Failed to init RSS rc: %d\n", rc);
+			return rc;
+		}
+	}
+
+	/* Set indirect table */
+	rc = ena_com_indirect_table_set(ena_dev);
+	if (unlikely(rc && rc != -EPERM))
+		return rc;
+
+	/* Configure hash function (if supported) */
+	rc = ena_com_set_hash_function(ena_dev);
+	if (unlikely(rc && (rc != -EPERM)))
+		return rc;
+
+	/* Configure hash inputs (if supported) */
+	rc = ena_com_set_hash_ctrl(ena_dev);
+	if (unlikely(rc && (rc != -EPERM)))
+		return rc;
+
+	return 0;
+}
+
+static int ena_up_complete(struct ena_adapter *adapter)
+{
+	int rc, i;
+
+	rc = ena_rss_configure(adapter);
+	if (rc)
+		return rc;
+
+	ena_init_napi(adapter);
+
+	ena_change_mtu(adapter->netdev, adapter->netdev->mtu);
+
+	ena_refill_all_rx_bufs(adapter);
+
+	/* enable transmits */
+	netif_tx_start_all_queues(adapter->netdev);
+
+	ena_restore_ethtool_params(adapter);
+
+	ena_napi_enable_all(adapter);
+
+	/* schedule napi in case we had pending packets
+	 * from the last time we disable napi
+	 */
+	for (i = 0; i < adapter->num_queues; i++)
+		napi_schedule(&adapter->ena_napi[i].napi);
+
+	return 0;
+}
+
+static int ena_create_io_tx_queue(struct ena_adapter *adapter, int qid)
+{
+	struct ena_com_create_io_ctx ctx = { 0 };
+	struct ena_com_dev *ena_dev;
+	struct ena_ring *tx_ring;
+	u32 msix_vector;
+	u16 ena_qid;
+	int rc;
+
+	ena_dev = adapter->ena_dev;
+
+	tx_ring = &adapter->tx_ring[qid];
+	msix_vector = ENA_IO_IRQ_IDX(qid);
+	ena_qid = ENA_IO_TXQ_IDX(qid);
+
+	ctx.direction = ENA_COM_IO_QUEUE_DIRECTION_TX;
+	ctx.qid = ena_qid;
+	ctx.mem_queue_type = ena_dev->tx_mem_queue_type;
+	ctx.msix_vector = msix_vector;
+	ctx.queue_size = adapter->tx_ring_size;
+	ctx.numa_node = cpu_to_node(tx_ring->cpu);
+
+	rc = ena_com_create_io_queue(ena_dev, &ctx);
+	if (rc) {
+		netif_err(adapter, ifup, adapter->netdev,
+			  "Failed to create I/O TX queue num %d rc: %d\n",
+			  qid, rc);
+		return rc;
+	}
+
+	rc = ena_com_get_io_handlers(ena_dev, ena_qid,
+				     &tx_ring->ena_com_io_sq,
+				     &tx_ring->ena_com_io_cq);
+	if (rc) {
+		netif_err(adapter, ifup, adapter->netdev,
+			  "Failed to get TX queue handlers. TX queue num %d rc: %d\n",
+			  qid, rc);
+		ena_com_destroy_io_queue(ena_dev, ena_qid);
+	}
+
+	ena_com_update_numa_node(tx_ring->ena_com_io_cq, ctx.numa_node);
+	return rc;
+}
+
+static int ena_create_all_io_tx_queues(struct ena_adapter *adapter)
+{
+	struct ena_com_dev *ena_dev = adapter->ena_dev;
+	int rc, i;
+
+	for (i = 0; i < adapter->num_queues; i++) {
+		rc = ena_create_io_tx_queue(adapter, i);
+		if (rc)
+			goto create_err;
+	}
+
+	return 0;
+
+create_err:
+	while (i--)
+		ena_com_destroy_io_queue(ena_dev, ENA_IO_TXQ_IDX(i));
+
+	return rc;
+}
+
+static int ena_create_io_rx_queue(struct ena_adapter *adapter, int qid)
+{
+	struct ena_com_dev *ena_dev;
+	struct ena_com_create_io_ctx ctx = { 0 };
+	struct ena_ring *rx_ring;
+	u32 msix_vector;
+	u16 ena_qid;
+	int rc;
+
+	ena_dev = adapter->ena_dev;
+
+	rx_ring = &adapter->rx_ring[qid];
+	msix_vector = ENA_IO_IRQ_IDX(qid);
+	ena_qid = ENA_IO_RXQ_IDX(qid);
+
+	ctx.qid = ena_qid;
+	ctx.direction = ENA_COM_IO_QUEUE_DIRECTION_RX;
+	ctx.mem_queue_type = ENA_ADMIN_PLACEMENT_POLICY_HOST;
+	ctx.msix_vector = msix_vector;
+	ctx.queue_size = adapter->rx_ring_size;
+	ctx.numa_node = cpu_to_node(rx_ring->cpu);
+
+	rc = ena_com_create_io_queue(ena_dev, &ctx);
+	if (rc) {
+		netif_err(adapter, ifup, adapter->netdev,
+			  "Failed to create I/O RX queue num %d rc: %d\n",
+			  qid, rc);
+		return rc;
+	}
+
+	rc = ena_com_get_io_handlers(ena_dev, ena_qid,
+				     &rx_ring->ena_com_io_sq,
+				     &rx_ring->ena_com_io_cq);
+	if (rc) {
+		netif_err(adapter, ifup, adapter->netdev,
+			  "Failed to get RX queue handlers. RX queue num %d rc: %d\n",
+			  qid, rc);
+		ena_com_destroy_io_queue(ena_dev, ena_qid);
+	}
+
+	ena_com_update_numa_node(rx_ring->ena_com_io_cq, ctx.numa_node);
+
+	return rc;
+}
+
+static int ena_create_all_io_rx_queues(struct ena_adapter *adapter)
+{
+	struct ena_com_dev *ena_dev = adapter->ena_dev;
+	int rc, i;
+
+	for (i = 0; i < adapter->num_queues; i++) {
+		rc = ena_create_io_rx_queue(adapter, i);
+		if (rc)
+			goto create_err;
+	}
+
+	return 0;
+
+create_err:
+	while (i--)
+		ena_com_destroy_io_queue(ena_dev, ENA_IO_RXQ_IDX(i));
+
+	return rc;
+}
+
+static int ena_up(struct ena_adapter *adapter)
+{
+	int rc;
+
+	netdev_dbg(adapter->netdev, "%s\n", __func__);
+
+	ena_setup_io_intr(adapter);
+
+	rc = ena_request_io_irq(adapter);
+	if (rc)
+		goto err_req_irq;
+
+	/* allocate transmit descriptors */
+	rc = ena_setup_all_tx_resources(adapter);
+	if (rc)
+		goto err_setup_tx;
+
+	/* allocate receive descriptors */
+	rc = ena_setup_all_rx_resources(adapter);
+	if (rc)
+		goto err_setup_rx;
+
+	/* Create TX queues */
+	rc = ena_create_all_io_tx_queues(adapter);
+	if (rc)
+		goto err_create_tx_queues;
+
+	/* Create RX queues */
+	rc = ena_create_all_io_rx_queues(adapter);
+	if (rc)
+		goto err_create_rx_queues;
+
+	rc = ena_up_complete(adapter);
+	if (rc)
+		goto err_up;
+
+	if (test_bit(ENA_FLAG_LINK_UP, &adapter->flags))
+		netif_carrier_on(adapter->netdev);
+
+	u64_stats_update_begin(&adapter->syncp);
+	adapter->dev_stats.interface_up++;
+	u64_stats_update_end(&adapter->syncp);
+
+	set_bit(ENA_FLAG_DEV_UP, &adapter->flags);
+
+	return rc;
+
+err_up:
+	ena_destroy_all_rx_queues(adapter);
+err_create_rx_queues:
+	ena_destroy_all_tx_queues(adapter);
+err_create_tx_queues:
+	ena_free_all_io_rx_resources(adapter);
+err_setup_rx:
+	ena_free_all_io_tx_resources(adapter);
+err_setup_tx:
+	ena_free_io_irq(adapter);
+err_req_irq:
+
+	return rc;
+}
+
+static void ena_down(struct ena_adapter *adapter)
+{
+	netif_info(adapter, ifdown, adapter->netdev, "%s\n", __func__);
+
+	clear_bit(ENA_FLAG_DEV_UP, &adapter->flags);
+
+	u64_stats_update_begin(&adapter->syncp);
+	adapter->dev_stats.interface_down++;
+	u64_stats_update_end(&adapter->syncp);
+
+	/* After this point the napi handler won't enable the tx queue */
+	ena_napi_disable_all(adapter);
+	netif_carrier_off(adapter->netdev);
+	netif_tx_disable(adapter->netdev);
+
+	/* After destroy the queue there won't be any new interrupts */
+	ena_destroy_all_io_queues(adapter);
+
+	ena_disable_io_intr_sync(adapter);
+	ena_free_io_irq(adapter);
+	ena_del_napi(adapter);
+
+	ena_free_all_tx_bufs(adapter);
+	ena_free_all_rx_bufs(adapter);
+	ena_free_all_io_tx_resources(adapter);
+	ena_free_all_io_rx_resources(adapter);
+}
+
+/* ena_open - Called when a network interface is made active
+ * @netdev: network interface device structure
+ *
+ * Returns 0 on success, negative value on failure
+ *
+ * The open entry point is called when a network interface is made
+ * active by the system (IFF_UP).  At this point all resources needed
+ * for transmit and receive operations are allocated, the interrupt
+ * handler is registered with the OS, the watchdog timer is started,
+ * and the stack is notified that the interface is ready.
+ */
+static int ena_open(struct net_device *netdev)
+{
+	struct ena_adapter *adapter = netdev_priv(netdev);
+	int rc;
+
+	/* Notify the stack of the actual queue counts. */
+	rc = netif_set_real_num_tx_queues(netdev, adapter->num_queues);
+	if (rc) {
+		netif_err(adapter, ifup, netdev, "Can't set num tx queues\n");
+		return rc;
+	}
+
+	rc = netif_set_real_num_rx_queues(netdev, adapter->num_queues);
+	if (rc) {
+		netif_err(adapter, ifup, netdev, "Can't set num rx queues\n");
+		return rc;
+	}
+
+	rc = ena_up(adapter);
+	if (rc)
+		return rc;
+
+	return rc;
+}
+
+/* ena_close - Disables a network interface
+ * @netdev: network interface device structure
+ *
+ * Returns 0, this is not allowed to fail
+ *
+ * The close entry point is called when an interface is de-activated
+ * by the OS.  The hardware is still under the drivers control, but
+ * needs to be disabled.  A global MAC reset is issued to stop the
+ * hardware, and all transmit and receive resources are freed.
+ */
+static int ena_close(struct net_device *netdev)
+{
+	struct ena_adapter *adapter = netdev_priv(netdev);
+
+	netif_dbg(adapter, ifdown, netdev, "%s\n", __func__);
+
+	if (test_bit(ENA_FLAG_DEV_UP, &adapter->flags))
+		ena_down(adapter);
+
+	return 0;
+}
+
+static void ena_tx_csum(struct ena_com_tx_ctx *ena_tx_ctx, struct sk_buff *skb)
+{
+	u32 mss = skb_shinfo(skb)->gso_size;
+	struct ena_com_tx_meta *ena_meta = &ena_tx_ctx->ena_meta;
+	u8 l4_protocol = 0;
+
+	if ((skb->ip_summed == CHECKSUM_PARTIAL) || mss) {
+		ena_tx_ctx->l4_csum_enable = 1;
+		if (mss) {
+			ena_tx_ctx->tso_enable = 1;
+			ena_meta->l4_hdr_len = tcp_hdr(skb)->doff;
+			ena_tx_ctx->l4_csum_partial = 0;
+		} else {
+			ena_tx_ctx->tso_enable = 0;
+			ena_meta->l4_hdr_len = 0;
+			ena_tx_ctx->l4_csum_partial = 1;
+		}
+
+		switch (ip_hdr(skb)->version) {
+		case IPVERSION:
+			ena_tx_ctx->l3_proto = ENA_ETH_IO_L3_PROTO_IPV4;
+			if (ip_hdr(skb)->frag_off & htons(IP_DF))
+				ena_tx_ctx->df = 1;
+			if (mss)
+				ena_tx_ctx->l3_csum_enable = 1;
+			l4_protocol = ip_hdr(skb)->protocol;
+			break;
+		case 6:
+			ena_tx_ctx->l3_proto = ENA_ETH_IO_L3_PROTO_IPV6;
+			l4_protocol = ipv6_hdr(skb)->nexthdr;
+			break;
+		default:
+			break;
+		}
+
+		if (l4_protocol == IPPROTO_TCP)
+			ena_tx_ctx->l4_proto = ENA_ETH_IO_L4_PROTO_TCP;
+		else
+			ena_tx_ctx->l4_proto = ENA_ETH_IO_L4_PROTO_UDP;
+
+		ena_meta->mss = mss;
+		ena_meta->l3_hdr_len = skb_network_header_len(skb);
+		ena_meta->l3_hdr_offset = skb_network_offset(skb);
+		ena_tx_ctx->meta_valid = 1;
+
+	} else {
+		ena_tx_ctx->meta_valid = 0;
+	}
+}
+
+static int ena_check_and_linearize_skb(struct ena_ring *tx_ring,
+				       struct sk_buff *skb)
+{
+	int num_frags, header_len, rc;
+
+	num_frags = skb_shinfo(skb)->nr_frags;
+	header_len = skb_headlen(skb);
+
+	if (num_frags < tx_ring->sgl_size)
+		return 0;
+
+	if ((num_frags == tx_ring->sgl_size) &&
+	    (header_len < tx_ring->tx_max_header_size))
+		return 0;
+
+	u64_stats_update_begin(&tx_ring->syncp);
+	tx_ring->tx_stats.linearize++;
+	u64_stats_update_end(&tx_ring->syncp);
+
+	rc = skb_linearize(skb);
+	if (unlikely(rc)) {
+		u64_stats_update_begin(&tx_ring->syncp);
+		tx_ring->tx_stats.linearize_failed++;
+		u64_stats_update_end(&tx_ring->syncp);
+	}
+
+	return rc;
+}
+
+/* Called with netif_tx_lock. */
+static netdev_tx_t ena_start_xmit(struct sk_buff *skb, struct net_device *dev)
+{
+	struct ena_adapter *adapter = netdev_priv(dev);
+	struct ena_tx_buffer *tx_info;
+	struct ena_com_tx_ctx ena_tx_ctx;
+	struct ena_ring *tx_ring;
+	struct netdev_queue *txq;
+	struct ena_com_buf *ena_buf;
+	void *push_hdr;
+	u32 len, last_frag;
+	u16 next_to_use;
+	u16 req_id;
+	u16 push_len;
+	u16 header_len;
+	dma_addr_t dma;
+	int qid, rc, nb_hw_desc;
+	int i = -1;
+
+	netif_dbg(adapter, tx_queued, dev, "%s skb %p\n", __func__, skb);
+	/*  Determine which tx ring we will be placed on */
+	qid = skb_get_queue_mapping(skb);
+	tx_ring = &adapter->tx_ring[qid];
+	txq = netdev_get_tx_queue(dev, qid);
+
+	rc = ena_check_and_linearize_skb(tx_ring, skb);
+	if (unlikely(rc))
+		goto error_drop_packet;
+
+	skb_tx_timestamp(skb);
+	len = skb_headlen(skb);
+
+	next_to_use = tx_ring->next_to_use;
+	req_id = tx_ring->free_tx_ids[next_to_use];
+	tx_info = &tx_ring->tx_buffer_info[req_id];
+	tx_info->num_of_bufs = 0;
+
+	WARN(tx_info->skb, "SKB isn't NULL req_id %d\n", req_id);
+	ena_buf = tx_info->bufs;
+	tx_info->skb = skb;
+
+	if (tx_ring->tx_mem_queue_type == ENA_ADMIN_PLACEMENT_POLICY_DEV) {
+		/* prepared the push buffer */
+		push_len = min_t(u32, len, tx_ring->tx_max_header_size);
+		header_len = push_len;
+		push_hdr = skb->data;
+	} else {
+		push_len = 0;
+		header_len = min_t(u32, len, tx_ring->tx_max_header_size);
+		push_hdr = NULL;
+	}
+
+	netif_dbg(adapter, tx_queued, dev,
+		  "skb: %p header_buf->vaddr: %p push_len: %d\n", skb,
+		  push_hdr, push_len);
+
+	if (len > push_len) {
+		dma = dma_map_single(tx_ring->dev, skb->data + push_len,
+				     len - push_len, DMA_TO_DEVICE);
+		if (dma_mapping_error(tx_ring->dev, dma))
+			goto error_report_dma_error;
+
+		ena_buf->paddr = dma;
+		ena_buf->len = len - push_len;
+
+		ena_buf++;
+		tx_info->num_of_bufs++;
+	}
+
+	last_frag = skb_shinfo(skb)->nr_frags;
+
+	for (i = 0; i < last_frag; i++) {
+		const skb_frag_t *frag = &skb_shinfo(skb)->frags[i];
+
+		len = skb_frag_size(frag);
+		dma = skb_frag_dma_map(tx_ring->dev, frag, 0, len,
+				       DMA_TO_DEVICE);
+		if (dma_mapping_error(tx_ring->dev, dma))
+			goto error_report_dma_error;
+
+		ena_buf->paddr = dma;
+		ena_buf->len = len;
+		ena_buf++;
+	}
+
+	tx_info->num_of_bufs += last_frag;
+
+	memset(&ena_tx_ctx, 0x0, sizeof(struct ena_com_tx_ctx));
+	ena_tx_ctx.ena_bufs = tx_info->bufs;
+	ena_tx_ctx.push_header = push_hdr;
+	ena_tx_ctx.num_bufs = tx_info->num_of_bufs;
+	ena_tx_ctx.req_id = req_id;
+	ena_tx_ctx.header_len = header_len;
+
+	/* set flags and meta data */
+	ena_tx_csum(&ena_tx_ctx, skb);
+
+	/* prepare the packet's descriptors to dma engine */
+	rc = ena_com_prepare_tx(tx_ring->ena_com_io_sq, &ena_tx_ctx,
+				&nb_hw_desc);
+
+	if (unlikely(rc)) {
+		netif_err(adapter, tx_queued, dev,
+			  "failed to prepare tx bufs\n");
+		u64_stats_update_begin(&tx_ring->syncp);
+		tx_ring->tx_stats.queue_stop++;
+		tx_ring->tx_stats.prepare_ctx_err++;
+		u64_stats_update_end(&tx_ring->syncp);
+		netif_tx_stop_queue(txq);
+		goto error_unmap_dma;
+	}
+
+	netdev_tx_sent_queue(txq, skb->len);
+
+	u64_stats_update_begin(&tx_ring->syncp);
+	tx_ring->tx_stats.cnt++;
+	tx_ring->tx_stats.bytes += skb->len;
+	u64_stats_update_end(&tx_ring->syncp);
+
+	tx_info->tx_descs = nb_hw_desc;
+	tx_info->last_jiffies = jiffies;
+
+	tx_ring->next_to_use = ENA_TX_RING_IDX_NEXT(next_to_use,
+		tx_ring->ring_size);
+
+	/* This WMB is aimed to:
+	 * 1 - perform smp barrier before reading next_to_completion
+	 * 2 - make sure the desc were written before trigger DB
+	 */
+	wmb();
+
+	/* stop the queue when no more space available, the packet can have up
+	 * to sgl_size + 2. one for the meta descriptor and one for header
+	 * (if the header is larger than tx_max_header_size).
+	 */
+	if (unlikely(ena_com_sq_empty_space(tx_ring->ena_com_io_sq) <
+		     (tx_ring->sgl_size + 2))) {
+		netif_dbg(adapter, tx_queued, dev, "%s stop queue %d\n",
+			  __func__, qid);
+
+		netif_tx_stop_queue(txq);
+		u64_stats_update_begin(&tx_ring->syncp);
+		tx_ring->tx_stats.queue_stop++;
+		u64_stats_update_end(&tx_ring->syncp);
+
+		/* There is a rare condition where this function decide to
+		 * stop the queue but meanwhile clean_tx_irq updates
+		 * next_to_completion and terminates.
+		 * The queue will remain stopped forever.
+		 * To solve this issue this function perform rmb, check
+		 * the wakeup condition and wake up the queue if needed.
+		 */
+		smp_rmb();
+
+		if (ena_com_sq_empty_space(tx_ring->ena_com_io_sq)
+				> ENA_TX_WAKEUP_THRESH) {
+			netif_tx_wake_queue(txq);
+			u64_stats_update_begin(&tx_ring->syncp);
+			tx_ring->tx_stats.queue_wakeup++;
+			u64_stats_update_end(&tx_ring->syncp);
+		}
+	}
+
+	if (netif_xmit_stopped(txq) || !skb->xmit_more) {
+		/* trigger the dma engine */
+		ena_com_write_sq_doorbell(tx_ring->ena_com_io_sq);
+		u64_stats_update_begin(&tx_ring->syncp);
+		tx_ring->tx_stats.doorbells++;
+		u64_stats_update_end(&tx_ring->syncp);
+	}
+
+	return NETDEV_TX_OK;
+
+error_report_dma_error:
+	u64_stats_update_begin(&tx_ring->syncp);
+	tx_ring->tx_stats.dma_mapping_err++;
+	u64_stats_update_end(&tx_ring->syncp);
+	netdev_warn(adapter->netdev, "failed to map skb\n");
+
+	tx_info->skb = NULL;
+
+error_unmap_dma:
+	if (i >= 0) {
+		/* save value of frag that failed */
+		last_frag = i;
+
+		/* start back at beginning and unmap skb */
+		tx_info->skb = NULL;
+		ena_buf = tx_info->bufs;
+		dma_unmap_single(tx_ring->dev, dma_unmap_addr(ena_buf, paddr),
+				 dma_unmap_len(ena_buf, len), DMA_TO_DEVICE);
+
+		/* unmap remaining mapped pages */
+		for (i = 0; i < last_frag; i++) {
+			ena_buf++;
+			dma_unmap_page(tx_ring->dev, dma_unmap_addr(ena_buf, paddr),
+				       dma_unmap_len(ena_buf, len), DMA_TO_DEVICE);
+		}
+	}
+
+error_drop_packet:
+
+	dev_kfree_skb(skb);
+	return NETDEV_TX_OK;
+}
+
+#ifdef CONFIG_NET_POLL_CONTROLLER
+static void ena_netpoll(struct net_device *netdev)
+{
+	struct ena_adapter *adapter = netdev_priv(netdev);
+	int i;
+
+	for (i = 0; i < adapter->num_queues; i++)
+		napi_schedule(&adapter->ena_napi[i].napi);
+}
+#endif /* CONFIG_NET_POLL_CONTROLLER */
+
+static u16 ena_select_queue(struct net_device *dev, struct sk_buff *skb,
+			    void *accel_priv, select_queue_fallback_t fallback)
+{
+	u16 qid;
+	/* we suspect that this is good for in--kernel network services that
+	 * want to loop incoming skb rx to tx in normal user generated traffic,
+	 * most probably we will not get to this
+	 */
+	if (skb_rx_queue_recorded(skb))
+		qid = skb_get_rx_queue(skb);
+	else
+		qid = fallback(dev, skb);
+
+	return qid;
+}
+
+static void ena_config_host_info(struct ena_com_dev *ena_dev)
+{
+	struct ena_admin_host_info *host_info;
+	int rc;
+
+	/* Allocate only the host info */
+	rc = ena_com_allocate_host_info(ena_dev);
+	if (rc) {
+		pr_err("Cannot allocate host info\n");
+		return;
+	}
+
+	host_info = ena_dev->host_attr.host_info;
+
+	host_info->os_type = ENA_ADMIN_OS_LINUX;
+	host_info->kernel_ver = LINUX_VERSION_CODE;
+	strncpy(host_info->kernel_ver_str, utsname()->version,
+		sizeof(host_info->kernel_ver_str) - 1);
+	host_info->os_dist = 0;
+	strncpy(host_info->os_dist_str, utsname()->release,
+		sizeof(host_info->os_dist_str) - 1);
+	host_info->driver_version =
+		(DRV_MODULE_VER_MAJOR) |
+		(DRV_MODULE_VER_MINOR << ENA_ADMIN_HOST_INFO_MINOR_SHIFT) |
+		(DRV_MODULE_VER_SUBMINOR << ENA_ADMIN_HOST_INFO_SUB_MINOR_SHIFT);
+
+	rc = ena_com_set_host_attributes(ena_dev);
+	if (rc) {
+		if (rc == -EPERM)
+			pr_warn("Cannot set host attributes\n");
+		else
+			pr_err("Cannot set host attributes\n");
+
+		goto err;
+	}
+
+	return;
+
+err:
+	ena_com_delete_host_info(ena_dev);
+}
+
+static void ena_config_debug_area(struct ena_adapter *adapter)
+{
+	u32 debug_area_size;
+	int rc, ss_count;
+
+	ss_count = ena_get_sset_count(adapter->netdev, ETH_SS_STATS);
+	if (ss_count <= 0) {
+		netif_err(adapter, drv, adapter->netdev,
+			  "SS count is negative\n");
+		return;
+	}
+
+	/* allocate 32 bytes for each string and 64bit for the value */
+	debug_area_size = ss_count * ETH_GSTRING_LEN + sizeof(u64) * ss_count;
+
+	rc = ena_com_allocate_debug_area(adapter->ena_dev, debug_area_size);
+	if (rc) {
+		pr_err("Cannot allocate debug area\n");
+		return;
+	}
+
+	rc = ena_com_set_host_attributes(adapter->ena_dev);
+	if (rc) {
+		if (rc == -EPERM)
+			netif_warn(adapter, drv, adapter->netdev,
+				   "Cannot set host attributes\n");
+		else
+			netif_err(adapter, drv, adapter->netdev,
+				  "Cannot set host attributes\n");
+		goto err;
+	}
+
+	return;
+err:
+	ena_com_delete_debug_area(adapter->ena_dev);
+}
+
+static struct rtnl_link_stats64 *ena_get_stats64(struct net_device *netdev,
+						 struct rtnl_link_stats64 *stats)
+{
+	struct ena_adapter *adapter = netdev_priv(netdev);
+	struct ena_admin_basic_stats ena_stats;
+	int rc;
+
+	if (!test_bit(ENA_FLAG_DEV_UP, &adapter->flags))
+		return NULL;
+
+	rc = ena_com_get_dev_basic_stats(adapter->ena_dev, &ena_stats);
+	if (rc)
+		return NULL;
+
+	stats->tx_bytes = ((u64)ena_stats.tx_bytes_high << 32) |
+		ena_stats.tx_bytes_low;
+	stats->rx_bytes = ((u64)ena_stats.rx_bytes_high << 32) |
+		ena_stats.rx_bytes_low;
+
+	stats->rx_packets = ((u64)ena_stats.rx_pkts_high << 32) |
+		ena_stats.rx_pkts_low;
+	stats->tx_packets = ((u64)ena_stats.tx_pkts_high << 32) |
+		ena_stats.tx_pkts_low;
+
+	stats->rx_dropped = ((u64)ena_stats.rx_drops_high << 32) |
+		ena_stats.rx_drops_low;
+
+	stats->multicast = 0;
+	stats->collisions = 0;
+
+	stats->rx_length_errors = 0;
+	stats->rx_crc_errors = 0;
+	stats->rx_frame_errors = 0;
+	stats->rx_fifo_errors = 0;
+	stats->rx_missed_errors = 0;
+	stats->tx_window_errors = 0;
+
+	stats->rx_errors = 0;
+	stats->tx_errors = 0;
+
+	return stats;
+}
+
+static const struct net_device_ops ena_netdev_ops = {
+	.ndo_open		= ena_open,
+	.ndo_stop		= ena_close,
+	.ndo_start_xmit		= ena_start_xmit,
+	.ndo_select_queue	= ena_select_queue,
+	.ndo_get_stats64	= ena_get_stats64,
+	.ndo_tx_timeout		= ena_tx_timeout,
+	.ndo_change_mtu		= ena_change_mtu,
+	.ndo_set_mac_address	= NULL,
+	.ndo_validate_addr	= eth_validate_addr,
+#ifdef CONFIG_NET_POLL_CONTROLLER
+	.ndo_poll_controller	= ena_netpoll,
+#endif /* CONFIG_NET_POLL_CONTROLLER */
+};
+
+static void ena_device_io_suspend(struct work_struct *work)
+{
+	struct ena_adapter *adapter =
+		container_of(work, struct ena_adapter, suspend_io_task);
+	struct net_device *netdev = adapter->netdev;
+
+	/* ena_napi_disable_all disables only the IO handling.
+	 * We are still subject to AENQ keep alive watchdog.
+	 */
+	u64_stats_update_begin(&adapter->syncp);
+	adapter->dev_stats.io_suspend++;
+	u64_stats_update_begin(&adapter->syncp);
+	ena_napi_disable_all(adapter);
+	netif_tx_lock(netdev);
+	netif_device_detach(netdev);
+	netif_tx_unlock(netdev);
+}
+
+static void ena_device_io_resume(struct work_struct *work)
+{
+	struct ena_adapter *adapter =
+		container_of(work, struct ena_adapter, resume_io_task);
+	struct net_device *netdev = adapter->netdev;
+
+	u64_stats_update_begin(&adapter->syncp);
+	adapter->dev_stats.io_resume++;
+	u64_stats_update_end(&adapter->syncp);
+
+	netif_device_attach(netdev);
+	ena_napi_enable_all(adapter);
+}
+
+static int ena_device_validate_params(struct ena_adapter *adapter,
+				      struct ena_com_dev_get_features_ctx *get_feat_ctx)
+{
+	struct net_device *netdev = adapter->netdev;
+	int rc;
+
+	rc = ether_addr_equal(get_feat_ctx->dev_attr.mac_addr,
+			      adapter->mac_addr);
+	if (!rc) {
+		netif_err(adapter, drv, netdev,
+			  "Error, mac address are different\n");
+		return -EINVAL;
+	}
+
+	if ((get_feat_ctx->max_queues.max_cq_num < adapter->num_queues) ||
+	    (get_feat_ctx->max_queues.max_sq_num < adapter->num_queues)) {
+		netif_err(adapter, drv, netdev,
+			  "Error, device doesn't support enough queues\n");
+		return -EINVAL;
+	}
+
+	if (get_feat_ctx->dev_attr.max_mtu < netdev->mtu) {
+		netif_err(adapter, drv, netdev,
+			  "Error, device max mtu is smaller than netdev MTU\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int ena_device_init(struct ena_com_dev *ena_dev, struct pci_dev *pdev,
+			   struct ena_com_dev_get_features_ctx *get_feat_ctx,
+			   bool *wd_state)
+{
+	struct device *dev = &pdev->dev;
+	bool readless_supported;
+	u32 aenq_groups;
+	int dma_width;
+	int rc;
+
+	rc = ena_com_mmio_reg_read_request_init(ena_dev);
+	if (rc) {
+		dev_err(dev, "failed to init mmio read less\n");
+		return rc;
+	}
+
+	/* The PCIe configuration space revision id indicate if mmio reg
+	 * read is disabled
+	 */
+	readless_supported = !(pdev->revision & ENA_MMIO_DISABLE_REG_READ);
+	ena_com_set_mmio_read_mode(ena_dev, readless_supported);
+
+	rc = ena_com_dev_reset(ena_dev);
+	if (rc) {
+		dev_err(dev, "Can not reset device\n");
+		goto err_mmio_read_less;
+	}
+
+	rc = ena_com_validate_version(ena_dev);
+	if (rc) {
+		dev_err(dev, "device version is too low\n");
+		goto err_mmio_read_less;
+	}
+
+	dma_width = ena_com_get_dma_width(ena_dev);
+	if (dma_width < 0) {
+		dev_err(dev, "Invalid dma width value %d", dma_width);
+		rc = dma_width;
+		goto err_mmio_read_less;
+	}
+
+	rc = pci_set_dma_mask(pdev, DMA_BIT_MASK(dma_width));
+	if (rc) {
+		dev_err(dev, "pci_set_dma_mask failed 0x%x\n", rc);
+		goto err_mmio_read_less;
+	}
+
+	rc = pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(dma_width));
+	if (rc) {
+		dev_err(dev, "err_pci_set_consistent_dma_mask failed 0x%x\n",
+			rc);
+		goto err_mmio_read_less;
+	}
+
+	/* ENA admin level init */
+	rc = ena_com_admin_init(ena_dev, &aenq_handlers, true);
+	if (rc) {
+		dev_err(dev,
+			"Can not initialize ena admin queue with device\n");
+		goto err_mmio_read_less;
+	}
+
+	/* To enable the msix interrupts the driver needs to know the number
+	 * of queues. So the driver uses polling mode to retrieve this
+	 * information
+	 */
+	ena_com_set_admin_polling_mode(ena_dev, true);
+
+	/* Get Device Attributes*/
+	rc = ena_com_get_dev_attr_feat(ena_dev, get_feat_ctx);
+	if (rc) {
+		dev_err(dev, "Cannot get attribute for ena device rc=%d\n", rc);
+		goto err_admin_init;
+	}
+
+	/* Try to turn all the available aenq groups */
+	aenq_groups = BIT(ENA_ADMIN_LINK_CHANGE) |
+		BIT(ENA_ADMIN_FATAL_ERROR) |
+		BIT(ENA_ADMIN_WARNING) |
+		BIT(ENA_ADMIN_NOTIFICATION) |
+		BIT(ENA_ADMIN_KEEP_ALIVE);
+
+	aenq_groups &= get_feat_ctx->aenq.supported_groups;
+
+	rc = ena_com_set_aenq_config(ena_dev, aenq_groups);
+	if (rc) {
+		dev_err(dev, "Cannot configure aenq groups rc= %d\n", rc);
+		goto err_admin_init;
+	}
+
+	*wd_state = !!(aenq_groups & BIT(ENA_ADMIN_KEEP_ALIVE));
+
+	ena_config_host_info(ena_dev);
+
+	return 0;
+
+err_admin_init:
+	ena_com_admin_destroy(ena_dev);
+err_mmio_read_less:
+	ena_com_mmio_reg_read_request_destroy(ena_dev);
+
+	return rc;
+}
+
+static int ena_enable_msix_and_set_admin_interrupts(struct ena_adapter *adapter,
+						    int io_vectors)
+{
+	struct ena_com_dev *ena_dev = adapter->ena_dev;
+	struct device *dev = &adapter->pdev->dev;
+	int rc;
+
+	rc = ena_enable_msix(adapter, io_vectors);
+	if (rc) {
+		dev_err(dev, "Can not reserve msix vectors\n");
+		return rc;
+	}
+
+	ena_setup_mgmnt_intr(adapter);
+
+	rc = ena_request_mgmnt_irq(adapter);
+	if (rc) {
+		dev_err(dev, "Can not setup management interrupts\n");
+		goto err_disable_msix;
+	}
+
+	ena_com_set_admin_polling_mode(ena_dev, false);
+
+	ena_com_admin_aenq_enable(ena_dev);
+
+	return 0;
+
+err_disable_msix:
+	ena_disable_msix(adapter);
+
+	return rc;
+}
+
+static void ena_fw_reset_device(struct work_struct *work)
+{
+	struct ena_com_dev_get_features_ctx get_feat_ctx;
+	struct ena_adapter *adapter =
+		container_of(work, struct ena_adapter, reset_task);
+	struct net_device *netdev = adapter->netdev;
+	struct ena_com_dev *ena_dev = adapter->ena_dev;
+	struct pci_dev *pdev = adapter->pdev;
+	bool dev_up, wd_state;
+	int rc;
+
+	del_timer_sync(&adapter->timer_service);
+
+	rtnl_lock();
+
+	dev_up = test_bit(ENA_FLAG_DEV_UP, &adapter->flags);
+	ena_com_set_admin_running_state(ena_dev, false);
+
+	/* After calling ena_close the tx queues and the napi
+	 * are disabled so no one can interfere or touch the
+	 * data structures
+	 */
+	ena_close(netdev);
+
+	rc = ena_com_dev_reset(ena_dev);
+	if (rc) {
+		dev_err(&pdev->dev, "Device reset failed\n");
+		goto err;
+	}
+
+	ena_free_mgmnt_irq(adapter);
+
+	ena_disable_msix(adapter);
+
+	ena_com_abort_admin_commands(ena_dev);
+
+	ena_com_wait_for_abort_completion(ena_dev);
+
+	ena_com_admin_destroy(ena_dev);
+
+	ena_com_mmio_reg_read_request_destroy(ena_dev);
+
+	/* Finish with the destroy part. Start the init part */
+
+	rc = ena_device_init(ena_dev, adapter->pdev, &get_feat_ctx, &wd_state);
+	if (rc) {
+		dev_err(&pdev->dev, "Can not initialize device\n");
+		goto err;
+	}
+	adapter->wd_state = wd_state;
+
+	rc = ena_device_validate_params(adapter, &get_feat_ctx);
+	if (rc) {
+		dev_err(&pdev->dev, "Validation of device parameters failed\n");
+		goto err_device_destroy;
+	}
+
+	rc = ena_enable_msix_and_set_admin_interrupts(adapter,
+						      adapter->num_queues);
+	if (rc) {
+		dev_err(&pdev->dev, "Enable MSI-X failed\n");
+		goto err_device_destroy;
+	}
+	/* If the interface was up before the reset bring it up */
+	if (dev_up) {
+		rc = ena_up(adapter);
+		if (rc) {
+			dev_err(&pdev->dev, "Failed to create I/O queues\n");
+			goto err_disable_msix;
+		}
+	}
+
+	mod_timer(&adapter->timer_service, round_jiffies(jiffies + HZ));
+
+	rtnl_unlock();
+
+	dev_err(&pdev->dev, "Device reset completed successfully\n");
+
+	return;
+err_disable_msix:
+	ena_free_mgmnt_irq(adapter);
+	ena_disable_msix(adapter);
+err_device_destroy:
+	ena_com_admin_destroy(ena_dev);
+err:
+	rtnl_unlock();
+
+	dev_err(&pdev->dev,
+		"Reset attempt failed. Can not reset the device\n");
+}
+
+static void check_for_missing_tx_completions(struct ena_adapter *adapter)
+{
+	struct ena_tx_buffer *tx_buf;
+	unsigned long last_jiffies;
+	struct ena_ring *tx_ring;
+	int i, j, budget;
+	u32 missed_tx;
+
+	/* Make sure the driver doesn't turn the device in other process */
+	smp_rmb();
+
+	if (!test_bit(ENA_FLAG_DEV_UP, &adapter->flags))
+		return;
+
+	budget = ENA_MONITORED_TX_QUEUES;
+
+	for (i = adapter->last_monitored_tx_qid; i < adapter->num_queues; i++) {
+		tx_ring = &adapter->tx_ring[i];
+
+		for (j = 0; j < tx_ring->ring_size; j++) {
+			tx_buf = &tx_ring->tx_buffer_info[j];
+			last_jiffies = tx_buf->last_jiffies;
+			if (unlikely(last_jiffies && time_is_before_jiffies(last_jiffies + TX_TIMEOUT))) {
+				netif_notice(adapter, tx_err, adapter->netdev,
+					     "Found a Tx that wasn't completed on time, qid %d, index %d.\n",
+					     tx_ring->qid, j);
+
+				u64_stats_update_begin(&tx_ring->syncp);
+				missed_tx = tx_ring->tx_stats.missing_tx_comp++;
+				u64_stats_update_end(&tx_ring->syncp);
+
+				/* Clear last jiffies so the lost buffer won't
+				 * be counted twice.
+				 */
+				tx_buf->last_jiffies = 0;
+
+				if (unlikely(missed_tx > MAX_NUM_OF_TIMEOUTED_PACKETS)) {
+					netif_err(adapter, tx_err, adapter->netdev,
+						  "The number of lost tx completion is above the threshold (%d > %d). Reset the device\n",
+						  missed_tx, MAX_NUM_OF_TIMEOUTED_PACKETS);
+					set_bit(ENA_FLAG_TRIGGER_RESET, &adapter->flags);
+				}
+			}
+		}
+
+		budget--;
+		if (!budget)
+			break;
+	}
+
+	adapter->last_monitored_tx_qid = i % adapter->num_queues;
+}
+
+/* Check for keep alive expiration */
+static void check_for_missing_keep_alive(struct ena_adapter *adapter)
+{
+	unsigned long keep_alive_expired;
+
+	if (!adapter->wd_state)
+		return;
+
+	keep_alive_expired = round_jiffies(adapter->last_keep_alive_jiffies
+					   + ENA_DEVICE_KALIVE_TIMEOUT);
+	if (unlikely(time_is_before_jiffies(keep_alive_expired))) {
+		netif_err(adapter, drv, adapter->netdev,
+			  "Keep alive watchdog timeout.\n");
+		u64_stats_update_begin(&adapter->syncp);
+		adapter->dev_stats.wd_expired++;
+		u64_stats_update_end(&adapter->syncp);
+		set_bit(ENA_FLAG_TRIGGER_RESET, &adapter->flags);
+	}
+}
+
+static void check_for_admin_com_state(struct ena_adapter *adapter)
+{
+	if (unlikely(!ena_com_get_admin_running_state(adapter->ena_dev))) {
+		netif_err(adapter, drv, adapter->netdev,
+			  "ENA admin queue is not in running state!\n");
+		u64_stats_update_begin(&adapter->syncp);
+		adapter->dev_stats.admin_q_pause++;
+		u64_stats_update_end(&adapter->syncp);
+		set_bit(ENA_FLAG_TRIGGER_RESET, &adapter->flags);
+	}
+}
+
+static void ena_update_host_info(struct ena_admin_host_info *host_info,
+				 struct net_device *netdev)
+{
+	host_info->supported_network_features[0] =
+		netdev->features & GENMASK_ULL(31, 0);
+	host_info->supported_network_features[1] =
+		(netdev->features & GENMASK_ULL(63, 32)) >> 32;
+}
+
+static void ena_timer_service(unsigned long data)
+{
+	struct ena_adapter *adapter = (struct ena_adapter *)data;
+	u8 *debug_area = adapter->ena_dev->host_attr.debug_area_virt_addr;
+	struct ena_admin_host_info *host_info =
+		adapter->ena_dev->host_attr.host_info;
+
+	check_for_missing_keep_alive(adapter);
+
+	check_for_admin_com_state(adapter);
+
+	check_for_missing_tx_completions(adapter);
+
+	if (debug_area)
+		ena_dump_stats_to_buf(adapter, debug_area);
+
+	if (host_info)
+		ena_update_host_info(host_info, adapter->netdev);
+
+	if (unlikely(test_and_clear_bit(ENA_FLAG_TRIGGER_RESET, &adapter->flags))) {
+		netif_err(adapter, drv, adapter->netdev,
+			  "Trigger reset is on\n");
+		ena_dump_stats_to_dmesg(adapter);
+		queue_work(ena_wq, &adapter->reset_task);
+		return;
+	}
+
+	/* Reset the timer */
+	mod_timer(&adapter->timer_service, jiffies + HZ);
+}
+
+static int ena_calc_io_queue_num(struct pci_dev *pdev,
+				 struct ena_com_dev *ena_dev,
+				 struct ena_com_dev_get_features_ctx *get_feat_ctx)
+{
+	int io_sq_num, io_queue_num;
+
+	/* In case of LLQ use the llq number in the get feature cmd */
+	if (ena_dev->tx_mem_queue_type == ENA_ADMIN_PLACEMENT_POLICY_DEV) {
+		io_sq_num = get_feat_ctx->max_queues.max_llq_num;
+
+		if (io_sq_num == 0) {
+			dev_err(&pdev->dev,
+				"Trying to use LLQ but llq_num is 0. Fall back into regular queues\n");
+
+			ena_dev->tx_mem_queue_type =
+				ENA_ADMIN_PLACEMENT_POLICY_HOST;
+			io_sq_num = get_feat_ctx->max_queues.max_sq_num;
+		}
+	} else {
+		io_sq_num = get_feat_ctx->max_queues.max_sq_num;
+	}
+
+	io_queue_num = min_t(int, num_possible_cpus(), ENA_MAX_NUM_IO_QUEUES);
+	io_queue_num = min_t(int, io_queue_num, io_sq_num);
+	io_queue_num = min_t(int, io_queue_num,
+			     get_feat_ctx->max_queues.max_cq_num);
+	/* 1 IRQ for for mgmnt and 1 IRQs for each IO direction */
+	io_queue_num = min_t(int, io_queue_num, pci_msix_vec_count(pdev) - 1);
+	if (unlikely(!io_queue_num)) {
+		dev_err(&pdev->dev, "The device doesn't have io queues\n");
+		return -EFAULT;
+	}
+
+	return io_queue_num;
+}
+
+static void ena_set_push_mode(struct pci_dev *pdev, struct ena_com_dev *ena_dev,
+			      struct ena_com_dev_get_features_ctx *get_feat_ctx)
+{
+	bool has_mem_bar;
+
+	has_mem_bar = pci_select_bars(pdev, IORESOURCE_MEM) & BIT(ENA_MEM_BAR);
+
+	/* Enable push mode if device supports LLQ */
+	if (has_mem_bar && (get_feat_ctx->max_queues.max_llq_num > 0))
+		ena_dev->tx_mem_queue_type = ENA_ADMIN_PLACEMENT_POLICY_DEV;
+	else
+		ena_dev->tx_mem_queue_type = ENA_ADMIN_PLACEMENT_POLICY_HOST;
+}
+
+static void ena_set_dev_offloads(struct ena_com_dev_get_features_ctx *feat,
+				 struct net_device *netdev)
+{
+	netdev_features_t dev_features = 0;
+
+	/* Set offload features */
+	if (feat->offload.tx &
+		ENA_ADMIN_FEATURE_OFFLOAD_DESC_TX_L4_IPV4_CSUM_PART_MASK)
+		dev_features |= NETIF_F_IP_CSUM;
+
+	if (feat->offload.tx &
+		ENA_ADMIN_FEATURE_OFFLOAD_DESC_TX_L4_IPV6_CSUM_PART_MASK)
+		dev_features |= NETIF_F_IPV6_CSUM;
+
+	if (feat->offload.tx & ENA_ADMIN_FEATURE_OFFLOAD_DESC_TSO_IPV4_MASK)
+		dev_features |= NETIF_F_TSO;
+
+	if (feat->offload.tx & ENA_ADMIN_FEATURE_OFFLOAD_DESC_TSO_IPV6_MASK)
+		dev_features |= NETIF_F_TSO6;
+
+	if (feat->offload.tx & ENA_ADMIN_FEATURE_OFFLOAD_DESC_TSO_ECN_MASK)
+		dev_features |= NETIF_F_TSO_ECN;
+
+	if (feat->offload.rx_supported &
+		ENA_ADMIN_FEATURE_OFFLOAD_DESC_RX_L4_IPV4_CSUM_MASK)
+		dev_features |= NETIF_F_RXCSUM;
+
+	if (feat->offload.rx_supported &
+		ENA_ADMIN_FEATURE_OFFLOAD_DESC_RX_L4_IPV6_CSUM_MASK)
+		dev_features |= NETIF_F_RXCSUM;
+
+	netdev->features =
+		dev_features |
+		NETIF_F_SG |
+		NETIF_F_NTUPLE |
+		NETIF_F_RXHASH |
+		NETIF_F_HIGHDMA;
+
+	netdev->hw_features |= netdev->features;
+	netdev->vlan_features |= netdev->features;
+}
+
+static void ena_set_conf_feat_params(struct ena_adapter *adapter,
+				     struct ena_com_dev_get_features_ctx *feat)
+{
+	struct net_device *netdev = adapter->netdev;
+
+	/* Copy mac address */
+	if (!is_valid_ether_addr(feat->dev_attr.mac_addr)) {
+		eth_hw_addr_random(netdev);
+		ether_addr_copy(adapter->mac_addr, netdev->dev_addr);
+	} else {
+		ether_addr_copy(adapter->mac_addr, feat->dev_attr.mac_addr);
+		ether_addr_copy(netdev->dev_addr, adapter->mac_addr);
+	}
+
+	/* Set offload features */
+	ena_set_dev_offloads(feat, netdev);
+
+	adapter->max_mtu = feat->dev_attr.max_mtu;
+}
+
+static int ena_rss_init_default(struct ena_adapter *adapter)
+{
+	struct ena_com_dev *ena_dev = adapter->ena_dev;
+	struct device *dev = &adapter->pdev->dev;
+	int rc, i;
+	u32 val;
+
+	rc = ena_com_rss_init(ena_dev, ENA_RX_RSS_TABLE_LOG_SIZE);
+	if (unlikely(rc)) {
+		dev_err(dev, "Cannot init indirect table\n");
+		goto err_rss_init;
+	}
+
+	for (i = 0; i < ENA_RX_RSS_TABLE_SIZE; i++) {
+		val = ethtool_rxfh_indir_default(i, adapter->num_queues);
+		rc = ena_com_indirect_table_fill_entry(ena_dev, i,
+						       ENA_IO_RXQ_IDX(val));
+		if (unlikely(rc && (rc != -EPERM))) {
+			dev_err(dev, "Cannot fill indirect table\n");
+			goto err_fill_indir;
+		}
+	}
+
+	rc = ena_com_fill_hash_function(ena_dev, ENA_ADMIN_CRC32, NULL,
+					ENA_HASH_KEY_SIZE, 0xFFFFFFFF);
+	if (unlikely(rc && (rc != -EPERM))) {
+		dev_err(dev, "Cannot fill hash function\n");
+		goto err_fill_indir;
+	}
+
+	rc = ena_com_set_default_hash_ctrl(ena_dev);
+	if (unlikely(rc && (rc != -EPERM))) {
+		dev_err(dev, "Cannot fill hash control\n");
+		goto err_fill_indir;
+	}
+
+	return 0;
+
+err_fill_indir:
+	ena_com_rss_destroy(ena_dev);
+err_rss_init:
+
+	return rc;
+}
+
+static void ena_release_bars(struct ena_com_dev *ena_dev, struct pci_dev *pdev)
+{
+	int release_bars;
+
+	release_bars = pci_select_bars(pdev, IORESOURCE_MEM) & ENA_BAR_MASK;
+	pci_release_selected_regions(pdev, release_bars);
+}
+
+static int ena_calc_queue_size(struct pci_dev *pdev,
+			       struct ena_com_dev *ena_dev,
+			       u16 *max_tx_sgl_size,
+			       u16 *max_rx_sgl_size,
+			       struct ena_com_dev_get_features_ctx *get_feat_ctx)
+{
+	u32 queue_size = ENA_DEFAULT_RING_SIZE;
+
+	queue_size = min_t(u32, queue_size,
+			   get_feat_ctx->max_queues.max_cq_depth);
+	queue_size = min_t(u32, queue_size,
+			   get_feat_ctx->max_queues.max_sq_depth);
+
+	if (ena_dev->tx_mem_queue_type == ENA_ADMIN_PLACEMENT_POLICY_DEV)
+		queue_size = min_t(u32, queue_size,
+				   get_feat_ctx->max_queues.max_llq_depth);
+
+	queue_size = rounddown_pow_of_two(queue_size);
+
+	if (unlikely(!queue_size)) {
+		dev_err(&pdev->dev, "Invalid queue size\n");
+		return -EFAULT;
+	}
+
+	*max_tx_sgl_size = min_t(u16, ENA_PKT_MAX_BUFS,
+				 get_feat_ctx->max_queues.max_packet_tx_descs);
+	*max_rx_sgl_size = min_t(u16, ENA_PKT_MAX_BUFS,
+				 get_feat_ctx->max_queues.max_packet_rx_descs);
+
+	return queue_size;
+}
+
+/* ena_probe - Device Initialization Routine
+ * @pdev: PCI device information struct
+ * @ent: entry in ena_pci_tbl
+ *
+ * Returns 0 on success, negative on failure
+ *
+ * ena_probe initializes an adapter identified by a pci_dev structure.
+ * The OS initialization, configuring of the adapter private structure,
+ * and a hardware reset occur.
+ */
+static int ena_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
+{
+	struct ena_com_dev_get_features_ctx get_feat_ctx;
+	static int version_printed;
+	struct net_device *netdev;
+	struct ena_adapter *adapter;
+	struct ena_com_dev *ena_dev = NULL;
+	static int adapters_found;
+	int io_queue_num, bars, rc;
+	int queue_size;
+	u16 tx_sgl_size = 0;
+	u16 rx_sgl_size = 0;
+	bool wd_state;
+
+	dev_dbg(&pdev->dev, "%s\n", __func__);
+
+	if (version_printed++ == 0)
+		dev_info(&pdev->dev, "%s", version);
+
+	rc = pci_enable_device_mem(pdev);
+	if (rc) {
+		dev_err(&pdev->dev, "pci_enable_device_mem() failed!\n");
+		return rc;
+	}
+
+	pci_set_master(pdev);
+
+	ena_dev = vzalloc(sizeof(*ena_dev));
+	if (!ena_dev) {
+		rc = -ENOMEM;
+		goto err_disable_device;
+	}
+
+	bars = pci_select_bars(pdev, IORESOURCE_MEM) & ENA_BAR_MASK;
+	rc = pci_request_selected_regions(pdev, bars, DRV_MODULE_NAME);
+	if (rc) {
+		dev_err(&pdev->dev, "pci_request_selected_regions failed %d\n",
+			rc);
+		goto err_free_ena_dev;
+	}
+
+	ena_dev->reg_bar = ioremap(pci_resource_start(pdev, ENA_REG_BAR),
+				   pci_resource_len(pdev, ENA_REG_BAR));
+	if (!ena_dev->reg_bar) {
+		dev_err(&pdev->dev, "failed to remap regs bar\n");
+		rc = -EFAULT;
+		goto err_free_region;
+	}
+
+	ena_dev->dmadev = &pdev->dev;
+
+	rc = ena_device_init(ena_dev, pdev, &get_feat_ctx, &wd_state);
+	if (rc) {
+		dev_err(&pdev->dev, "ena device init failed\n");
+		if (rc == -ETIME)
+			rc = -EPROBE_DEFER;
+		goto err_free_region;
+	}
+
+	ena_set_push_mode(pdev, ena_dev, &get_feat_ctx);
+
+	if (ena_dev->tx_mem_queue_type == ENA_ADMIN_PLACEMENT_POLICY_DEV) {
+		ena_dev->mem_bar = ioremap_wc(pci_resource_start(pdev, ENA_MEM_BAR),
+					      pci_resource_len(pdev, ENA_MEM_BAR));
+		if (!ena_dev->mem_bar) {
+			rc = -EFAULT;
+			goto err_device_destroy;
+		}
+	}
+
+	/* initial Tx interrupt delay, Assumes 1 usec granularity.
+	* Updated during device initialization with the real granularity
+	*/
+	ena_dev->intr_moder_tx_interval = ENA_INTR_INITIAL_TX_INTERVAL_USECS;
+	io_queue_num = ena_calc_io_queue_num(pdev, ena_dev, &get_feat_ctx);
+	queue_size = ena_calc_queue_size(pdev, ena_dev, &tx_sgl_size,
+					 &rx_sgl_size, &get_feat_ctx);
+	if ((queue_size <= 0) || (io_queue_num <= 0)) {
+		rc = -EFAULT;
+		goto err_device_destroy;
+	}
+
+	dev_info(&pdev->dev, "creating %d io queues. queue size: %d\n",
+		 io_queue_num, queue_size);
+
+	/* dev zeroed in init_etherdev */
+	netdev = alloc_etherdev_mq(sizeof(struct ena_adapter), io_queue_num);
+	if (!netdev) {
+		dev_err(&pdev->dev, "alloc_etherdev_mq failed\n");
+		rc = -ENOMEM;
+		goto err_device_destroy;
+	}
+
+	SET_NETDEV_DEV(netdev, &pdev->dev);
+
+	adapter = netdev_priv(netdev);
+	pci_set_drvdata(pdev, adapter);
+
+	adapter->ena_dev = ena_dev;
+	adapter->netdev = netdev;
+	adapter->pdev = pdev;
+
+	ena_set_conf_feat_params(adapter, &get_feat_ctx);
+
+	adapter->msg_enable = netif_msg_init(debug, DEFAULT_MSG_ENABLE);
+
+	adapter->tx_ring_size = queue_size;
+	adapter->rx_ring_size = queue_size;
+
+	adapter->max_tx_sgl_size = tx_sgl_size;
+	adapter->max_rx_sgl_size = rx_sgl_size;
+
+	adapter->num_queues = io_queue_num;
+	adapter->last_monitored_tx_qid = 0;
+
+	adapter->rx_copybreak = ENA_DEFAULT_RX_COPYBREAK;
+	adapter->wd_state = wd_state;
+
+	snprintf(adapter->name, ENA_NAME_MAX_LEN, "ena_%d", adapters_found);
+
+	rc = ena_com_init_interrupt_moderation(adapter->ena_dev);
+	if (rc) {
+		dev_err(&pdev->dev,
+			"Failed to query interrupt moderation feature\n");
+		goto err_netdev_destroy;
+	}
+	ena_init_io_rings(adapter);
+
+	netdev->netdev_ops = &ena_netdev_ops;
+	netdev->watchdog_timeo = TX_TIMEOUT;
+	ena_set_ethtool_ops(netdev);
+
+	netdev->priv_flags |= IFF_UNICAST_FLT;
+
+	u64_stats_init(&adapter->syncp);
+
+	rc = ena_enable_msix_and_set_admin_interrupts(adapter, io_queue_num);
+	if (rc) {
+		dev_err(&pdev->dev,
+			"Failed to enable and set the admin interrupts\n");
+		goto err_worker_destroy;
+	}
+	rc = ena_rss_init_default(adapter);
+	if (rc && (rc != -EPERM)) {
+		dev_err(&pdev->dev, "Cannot init RSS rc: %d\n", rc);
+		goto err_free_msix;
+	}
+
+	ena_config_debug_area(adapter);
+
+	memcpy(adapter->netdev->perm_addr, adapter->mac_addr, netdev->addr_len);
+
+	netif_carrier_off(netdev);
+
+	rc = register_netdev(netdev);
+	if (rc) {
+		dev_err(&pdev->dev, "Cannot register net device\n");
+		goto err_rss;
+	}
+
+	INIT_WORK(&adapter->suspend_io_task, ena_device_io_suspend);
+	INIT_WORK(&adapter->resume_io_task, ena_device_io_resume);
+	INIT_WORK(&adapter->reset_task, ena_fw_reset_device);
+
+	adapter->last_keep_alive_jiffies = jiffies;
+
+	init_timer(&adapter->timer_service);
+	adapter->timer_service.expires = round_jiffies(jiffies + HZ);
+	adapter->timer_service.function = ena_timer_service;
+	adapter->timer_service.data = (unsigned long)adapter;
+
+	add_timer(&adapter->timer_service);
+
+	dev_info(&pdev->dev, "%s found at mem %lx, mac addr %pM Queues %d\n",
+		 DEVICE_NAME, (long)pci_resource_start(pdev, 0),
+		 netdev->dev_addr, io_queue_num);
+
+	set_bit(ENA_FLAG_DEVICE_RUNNING, &adapter->flags);
+
+	adapters_found++;
+
+	return 0;
+
+err_rss:
+	ena_com_delete_debug_area(ena_dev);
+	ena_com_rss_destroy(ena_dev);
+err_free_msix:
+	ena_com_dev_reset(ena_dev);
+	ena_free_mgmnt_irq(adapter);
+	ena_disable_msix(adapter);
+err_worker_destroy:
+	ena_com_destroy_interrupt_moderation(ena_dev);
+	del_timer(&adapter->timer_service);
+	cancel_work_sync(&adapter->suspend_io_task);
+	cancel_work_sync(&adapter->resume_io_task);
+err_netdev_destroy:
+	free_netdev(netdev);
+err_device_destroy:
+	ena_com_delete_host_info(ena_dev);
+	ena_com_admin_destroy(ena_dev);
+err_free_region:
+	ena_release_bars(ena_dev, pdev);
+err_free_ena_dev:
+	vfree(ena_dev);
+err_disable_device:
+	pci_disable_device(pdev);
+	return rc;
+}
+
+/*****************************************************************************/
+static int ena_sriov_configure(struct pci_dev *dev, int numvfs)
+{
+	int rc;
+
+	if (numvfs > 0) {
+		rc = pci_enable_sriov(dev, numvfs);
+		if (rc != 0) {
+			dev_err(&dev->dev,
+				"pci_enable_sriov failed to enable: %d vfs with the error: %d\n",
+				numvfs, rc);
+			return rc;
+		}
+
+		return numvfs;
+	}
+
+	if (numvfs == 0) {
+		pci_disable_sriov(dev);
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+/*****************************************************************************/
+/*****************************************************************************/
+
+/* ena_remove - Device Removal Routine
+ * @pdev: PCI device information struct
+ *
+ * ena_remove is called by the PCI subsystem to alert the driver
+ * that it should release a PCI device.
+ */
+static void ena_remove(struct pci_dev *pdev)
+{
+	struct ena_adapter *adapter = pci_get_drvdata(pdev);
+	struct ena_com_dev *ena_dev;
+	struct net_device *netdev;
+
+	if (!adapter)
+		/* This device didn't load properly and it's resources
+		 * already released, nothing to do
+		 */
+		return;
+
+	ena_dev = adapter->ena_dev;
+	netdev = adapter->netdev;
+
+#ifdef CONFIG_RFS_ACCEL
+	if ((adapter->msix_vecs >= 1) && (netdev->rx_cpu_rmap)) {
+		free_irq_cpu_rmap(netdev->rx_cpu_rmap);
+		netdev->rx_cpu_rmap = NULL;
+	}
+#endif /* CONFIG_RFS_ACCEL */
+
+	unregister_netdev(netdev);
+	del_timer_sync(&adapter->timer_service);
+
+	cancel_work_sync(&adapter->reset_task);
+
+	cancel_work_sync(&adapter->suspend_io_task);
+
+	cancel_work_sync(&adapter->resume_io_task);
+
+	ena_com_dev_reset(ena_dev);
+
+	ena_free_mgmnt_irq(adapter);
+
+	ena_disable_msix(adapter);
+
+	free_netdev(netdev);
+
+	ena_com_mmio_reg_read_request_destroy(ena_dev);
+
+	ena_com_abort_admin_commands(ena_dev);
+
+	ena_com_wait_for_abort_completion(ena_dev);
+
+	ena_com_admin_destroy(ena_dev);
+
+	ena_com_rss_destroy(ena_dev);
+
+	ena_com_delete_debug_area(ena_dev);
+
+	ena_com_delete_host_info(ena_dev);
+
+	ena_release_bars(ena_dev, pdev);
+
+	pci_disable_device(pdev);
+
+	ena_com_destroy_interrupt_moderation(ena_dev);
+
+	vfree(ena_dev);
+}
+
+static struct pci_driver ena_pci_driver = {
+	.name		= DRV_MODULE_NAME,
+	.id_table	= ena_pci_tbl,
+	.probe		= ena_probe,
+	.remove		= ena_remove,
+	.sriov_configure = ena_sriov_configure,
+};
+
+static int __init ena_init(void)
+{
+	pr_info("%s", version);
+
+	ena_wq = create_singlethread_workqueue(DRV_MODULE_NAME);
+	if (!ena_wq) {
+		pr_err("Failed to create workqueue\n");
+		return -ENOMEM;
+	}
+
+	return pci_register_driver(&ena_pci_driver);
+}
+
+static void __exit ena_cleanup(void)
+{
+	pci_unregister_driver(&ena_pci_driver);
+
+	if (ena_wq) {
+		destroy_workqueue(ena_wq);
+		ena_wq = NULL;
+	}
+}
+
+/******************************************************************************
+ ******************************** AENQ Handlers *******************************
+ *****************************************************************************/
+/* ena_update_on_link_change:
+ * Notify the network interface about the change in link status
+ */
+static void ena_update_on_link_change(void *adapter_data,
+				      struct ena_admin_aenq_entry *aenq_e)
+{
+	struct ena_adapter *adapter = (struct ena_adapter *)adapter_data;
+	struct ena_admin_aenq_link_change_desc *aenq_desc =
+		(struct ena_admin_aenq_link_change_desc *)aenq_e;
+	int status = aenq_desc->flags &
+		ENA_ADMIN_AENQ_LINK_CHANGE_DESC_LINK_STATUS_MASK;
+
+	if (status) {
+		netdev_dbg(adapter->netdev, "%s\n", __func__);
+		set_bit(ENA_FLAG_LINK_UP, &adapter->flags);
+		netif_carrier_on(adapter->netdev);
+	} else {
+		clear_bit(ENA_FLAG_LINK_UP, &adapter->flags);
+		netif_carrier_off(adapter->netdev);
+	}
+}
+
+static void ena_keep_alive_wd(void *adapter_data,
+			      struct ena_admin_aenq_entry *aenq_e)
+{
+	struct ena_adapter *adapter = (struct ena_adapter *)adapter_data;
+
+	adapter->last_keep_alive_jiffies = jiffies;
+}
+
+static void ena_notification(void *adapter_data,
+			     struct ena_admin_aenq_entry *aenq_e)
+{
+	struct ena_adapter *adapter = (struct ena_adapter *)adapter_data;
+
+	WARN(aenq_e->aenq_common_desc.group != ENA_ADMIN_NOTIFICATION,
+	     "Invalid group(%x) expected %x\n",
+	     aenq_e->aenq_common_desc.group,
+	     ENA_ADMIN_NOTIFICATION);
+
+	switch (aenq_e->aenq_common_desc.syndrom) {
+	case ENA_ADMIN_SUSPEND:
+		/* Suspend just the IO queues.
+		 * We deliberately don't suspend admin so the timer and
+		 * the keep_alive events should remain.
+		 */
+		queue_work(ena_wq, &adapter->suspend_io_task);
+		break;
+	case ENA_ADMIN_RESUME:
+		queue_work(ena_wq, &adapter->resume_io_task);
+		break;
+	default:
+		netif_err(adapter, drv, adapter->netdev,
+			  "Invalid aenq notification link state %d\n",
+			  aenq_e->aenq_common_desc.syndrom);
+	}
+}
+
+/* This handler will called for unknown event group or unimplemented handlers*/
+static void unimplemented_aenq_handler(void *data,
+				       struct ena_admin_aenq_entry *aenq_e)
+{
+	struct ena_adapter *adapter = (struct ena_adapter *)data;
+
+	netif_err(adapter, drv, adapter->netdev,
+		  "Unknown event was received or event with unimplemented handler\n");
+}
+
+static struct ena_aenq_handlers aenq_handlers = {
+	.handlers = {
+		[ENA_ADMIN_LINK_CHANGE] = ena_update_on_link_change,
+		[ENA_ADMIN_NOTIFICATION] = ena_notification,
+		[ENA_ADMIN_KEEP_ALIVE] = ena_keep_alive_wd,
+	},
+	.unimplemented_handler = unimplemented_aenq_handler
+};
+
+module_init(ena_init);
+module_exit(ena_cleanup);
diff --git a/drivers/net/ethernet/amazon/ena/ena_netdev.h b/drivers/net/ethernet/amazon/ena/ena_netdev.h
new file mode 100644
index 000000000000..69d7e9ed5bc8
--- /dev/null
+++ b/drivers/net/ethernet/amazon/ena/ena_netdev.h
@@ -0,0 +1,324 @@
+/*
+ * Copyright 2015 Amazon.com, Inc. or its affiliates.
+ *
+ * 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
+ * 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 ENA_H
+#define ENA_H
+
+#include <linux/bitops.h>
+#include <linux/etherdevice.h>
+#include <linux/inetdevice.h>
+#include <linux/interrupt.h>
+#include <linux/netdevice.h>
+#include <linux/skbuff.h>
+
+#include "ena_com.h"
+#include "ena_eth_com.h"
+
+#define DRV_MODULE_VER_MAJOR	1
+#define DRV_MODULE_VER_MINOR	0
+#define DRV_MODULE_VER_SUBMINOR 2
+
+#define DRV_MODULE_NAME		"ena"
+#ifndef DRV_MODULE_VERSION
+#define DRV_MODULE_VERSION \
+	__stringify(DRV_MODULE_VER_MAJOR) "."	\
+	__stringify(DRV_MODULE_VER_MINOR) "."	\
+	__stringify(DRV_MODULE_VER_SUBMINOR)
+#endif
+
+#define DEVICE_NAME	"Elastic Network Adapter (ENA)"
+
+/* 1 for AENQ + ADMIN */
+#define ENA_MAX_MSIX_VEC(io_queues)	(1 + (io_queues))
+
+#define ENA_REG_BAR			0
+#define ENA_MEM_BAR			2
+#define ENA_BAR_MASK (BIT(ENA_REG_BAR) | BIT(ENA_MEM_BAR))
+
+#define ENA_DEFAULT_RING_SIZE	(1024)
+
+#define ENA_TX_WAKEUP_THRESH		(MAX_SKB_FRAGS + 2)
+#define ENA_DEFAULT_RX_COPYBREAK	(128 - NET_IP_ALIGN)
+
+/* limit the buffer size to 600 bytes to handle MTU changes from very
+ * small to very large, in which case the number of buffers per packet
+ * could exceed ENA_PKT_MAX_BUFS
+ */
+#define ENA_DEFAULT_MIN_RX_BUFF_ALLOC_SIZE 600
+
+#define ENA_MIN_MTU		128
+
+#define ENA_NAME_MAX_LEN	20
+#define ENA_IRQNAME_SIZE	40
+
+#define ENA_PKT_MAX_BUFS	19
+
+#define ENA_RX_RSS_TABLE_LOG_SIZE  7
+#define ENA_RX_RSS_TABLE_SIZE	(1 << ENA_RX_RSS_TABLE_LOG_SIZE)
+
+#define ENA_HASH_KEY_SIZE	40
+
+/* The number of tx packet completions that will be handled each NAPI poll
+ * cycle is ring_size / ENA_TX_POLL_BUDGET_DIVIDER.
+ */
+#define ENA_TX_POLL_BUDGET_DIVIDER	4
+
+/* Refill Rx queue when number of available descriptors is below
+ * QUEUE_SIZE / ENA_RX_REFILL_THRESH_DIVIDER
+ */
+#define ENA_RX_REFILL_THRESH_DIVIDER	8
+
+/* Number of queues to check for missing queues per timer service */
+#define ENA_MONITORED_TX_QUEUES	4
+/* Max timeout packets before device reset */
+#define MAX_NUM_OF_TIMEOUTED_PACKETS 32
+
+#define ENA_TX_RING_IDX_NEXT(idx, ring_size) (((idx) + 1) & ((ring_size) - 1))
+
+#define ENA_RX_RING_IDX_NEXT(idx, ring_size) (((idx) + 1) & ((ring_size) - 1))
+#define ENA_RX_RING_IDX_ADD(idx, n, ring_size) \
+	(((idx) + (n)) & ((ring_size) - 1))
+
+#define ENA_IO_TXQ_IDX(q)	(2 * (q))
+#define ENA_IO_RXQ_IDX(q)	(2 * (q) + 1)
+
+#define ENA_MGMNT_IRQ_IDX		0
+#define ENA_IO_IRQ_FIRST_IDX		1
+#define ENA_IO_IRQ_IDX(q)		(ENA_IO_IRQ_FIRST_IDX + (q))
+
+/* ENA device should send keep alive msg every 1 sec.
+ * We wait for 3 sec just to be on the safe side.
+ */
+#define ENA_DEVICE_KALIVE_TIMEOUT	(3 * HZ)
+
+#define ENA_MMIO_DISABLE_REG_READ	BIT(0)
+
+struct ena_irq {
+	irq_handler_t handler;
+	void *data;
+	int cpu;
+	u32 vector;
+	cpumask_t affinity_hint_mask;
+	char name[ENA_IRQNAME_SIZE];
+};
+
+struct ena_napi {
+	struct napi_struct napi ____cacheline_aligned;
+	struct ena_ring *tx_ring;
+	struct ena_ring *rx_ring;
+	u32 qid;
+};
+
+struct ena_tx_buffer {
+	struct sk_buff *skb;
+	/* num of ena desc for this specific skb
+	 * (includes data desc and metadata desc)
+	 */
+	u32 tx_descs;
+	/* num of buffers used by this skb */
+	u32 num_of_bufs;
+	/* Save the last jiffies to detect missing tx packets */
+	unsigned long last_jiffies;
+	struct ena_com_buf bufs[ENA_PKT_MAX_BUFS];
+} ____cacheline_aligned;
+
+struct ena_rx_buffer {
+	struct sk_buff *skb;
+	struct page *page;
+	u32 page_offset;
+	struct ena_com_buf ena_buf;
+} ____cacheline_aligned;
+
+struct ena_stats_tx {
+	u64 cnt;
+	u64 bytes;
+	u64 queue_stop;
+	u64 prepare_ctx_err;
+	u64 queue_wakeup;
+	u64 dma_mapping_err;
+	u64 linearize;
+	u64 linearize_failed;
+	u64 napi_comp;
+	u64 tx_poll;
+	u64 doorbells;
+	u64 missing_tx_comp;
+	u64 bad_req_id;
+};
+
+struct ena_stats_rx {
+	u64 cnt;
+	u64 bytes;
+	u64 refil_partial;
+	u64 bad_csum;
+	u64 page_alloc_fail;
+	u64 skb_alloc_fail;
+	u64 dma_mapping_err;
+	u64 bad_desc_num;
+	u64 rx_copybreak_pkt;
+};
+
+struct ena_ring {
+	/* Holds the empty requests for TX out of order completions */
+	u16 *free_tx_ids;
+	union {
+		struct ena_tx_buffer *tx_buffer_info;
+		struct ena_rx_buffer *rx_buffer_info;
+	};
+
+	/* cache ptr to avoid using the adapter */
+	struct device *dev;
+	struct pci_dev *pdev;
+	struct napi_struct *napi;
+	struct net_device *netdev;
+	struct ena_com_dev *ena_dev;
+	struct ena_adapter *adapter;
+	struct ena_com_io_cq *ena_com_io_cq;
+	struct ena_com_io_sq *ena_com_io_sq;
+
+	u16 next_to_use;
+	u16 next_to_clean;
+	u16 rx_copybreak;
+	u16 qid;
+	u16 mtu;
+	u16 sgl_size;
+
+	/* The maximum header length the device can handle */
+	u8 tx_max_header_size;
+
+	/* cpu for TPH */
+	int cpu;
+	 /* number of tx/rx_buffer_info's entries */
+	int ring_size;
+
+	enum ena_admin_placement_policy_type tx_mem_queue_type;
+
+	struct ena_com_rx_buf_info ena_bufs[ENA_PKT_MAX_BUFS];
+	u32  smoothed_interval;
+	u32  per_napi_packets;
+	u32  per_napi_bytes;
+	enum ena_intr_moder_level moder_tbl_idx;
+	struct u64_stats_sync syncp;
+	union {
+		struct ena_stats_tx tx_stats;
+		struct ena_stats_rx rx_stats;
+	};
+} ____cacheline_aligned;
+
+struct ena_stats_dev {
+	u64 tx_timeout;
+	u64 io_suspend;
+	u64 io_resume;
+	u64 wd_expired;
+	u64 interface_up;
+	u64 interface_down;
+	u64 admin_q_pause;
+};
+
+enum ena_flags_t {
+	ENA_FLAG_DEVICE_RUNNING,
+	ENA_FLAG_DEV_UP,
+	ENA_FLAG_LINK_UP,
+	ENA_FLAG_MSIX_ENABLED,
+	ENA_FLAG_TRIGGER_RESET
+};
+
+/* adapter specific private data structure */
+struct ena_adapter {
+	struct ena_com_dev *ena_dev;
+	/* OS defined structs */
+	struct net_device *netdev;
+	struct pci_dev *pdev;
+
+	/* rx packets that shorter that this len will be copied to the skb
+	 * header
+	 */
+	u32 rx_copybreak;
+	u32 max_mtu;
+
+	int num_queues;
+
+	struct msix_entry *msix_entries;
+	int msix_vecs;
+
+	u32 tx_usecs, rx_usecs; /* interrupt moderation */
+	u32 tx_frames, rx_frames; /* interrupt moderation */
+
+	u32 tx_ring_size;
+	u32 rx_ring_size;
+
+	u32 msg_enable;
+
+	u16 max_tx_sgl_size;
+	u16 max_rx_sgl_size;
+
+	u8 mac_addr[ETH_ALEN];
+
+	char name[ENA_NAME_MAX_LEN];
+
+	unsigned long flags;
+	/* TX */
+	struct ena_ring tx_ring[ENA_MAX_NUM_IO_QUEUES]
+		____cacheline_aligned_in_smp;
+
+	/* RX */
+	struct ena_ring rx_ring[ENA_MAX_NUM_IO_QUEUES]
+		____cacheline_aligned_in_smp;
+
+	struct ena_napi ena_napi[ENA_MAX_NUM_IO_QUEUES];
+
+	struct ena_irq irq_tbl[ENA_MAX_MSIX_VEC(ENA_MAX_NUM_IO_QUEUES)];
+
+	/* timer service */
+	struct work_struct reset_task;
+	struct work_struct suspend_io_task;
+	struct work_struct resume_io_task;
+	struct timer_list timer_service;
+
+	bool wd_state;
+	unsigned long last_keep_alive_jiffies;
+
+	struct u64_stats_sync syncp;
+	struct ena_stats_dev dev_stats;
+
+	/* last queue index that was checked for uncompleted tx packets */
+	u32 last_monitored_tx_qid;
+};
+
+void ena_set_ethtool_ops(struct net_device *netdev);
+
+void ena_dump_stats_to_dmesg(struct ena_adapter *adapter);
+
+void ena_dump_stats_to_buf(struct ena_adapter *adapter, u8 *buf);
+
+int ena_get_sset_count(struct net_device *netdev, int sset);
+
+#endif /* !(ENA_H) */
diff --git a/drivers/net/ethernet/amazon/ena/ena_pci_id_tbl.h b/drivers/net/ethernet/amazon/ena/ena_pci_id_tbl.h
new file mode 100644
index 000000000000..f80d2a47fa94
--- /dev/null
+++ b/drivers/net/ethernet/amazon/ena/ena_pci_id_tbl.h
@@ -0,0 +1,67 @@
+/*
+ * Copyright 2015 Amazon.com, Inc. or its affiliates.
+ *
+ * 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
+ * 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 ENA_PCI_ID_TBL_H_
+#define ENA_PCI_ID_TBL_H_
+
+#ifndef PCI_VENDOR_ID_AMAZON
+#define PCI_VENDOR_ID_AMAZON 0x1d0f
+#endif
+
+#ifndef PCI_DEV_ID_ENA_PF
+#define PCI_DEV_ID_ENA_PF	0x0ec2
+#endif
+
+#ifndef PCI_DEV_ID_ENA_LLQ_PF
+#define PCI_DEV_ID_ENA_LLQ_PF	0x1ec2
+#endif
+
+#ifndef PCI_DEV_ID_ENA_VF
+#define PCI_DEV_ID_ENA_VF	0xec20
+#endif
+
+#ifndef PCI_DEV_ID_ENA_LLQ_VF
+#define PCI_DEV_ID_ENA_LLQ_VF	0xec21
+#endif
+
+#define ENA_PCI_ID_TABLE_ENTRY(devid) \
+	{PCI_DEVICE(PCI_VENDOR_ID_AMAZON, devid)},
+
+static const struct pci_device_id ena_pci_tbl[] = {
+	ENA_PCI_ID_TABLE_ENTRY(PCI_DEV_ID_ENA_PF)
+	ENA_PCI_ID_TABLE_ENTRY(PCI_DEV_ID_ENA_LLQ_PF)
+	ENA_PCI_ID_TABLE_ENTRY(PCI_DEV_ID_ENA_VF)
+	ENA_PCI_ID_TABLE_ENTRY(PCI_DEV_ID_ENA_LLQ_VF)
+	{ }
+};
+
+#endif /* ENA_PCI_ID_TBL_H_ */
diff --git a/drivers/net/ethernet/amazon/ena/ena_regs_defs.h b/drivers/net/ethernet/amazon/ena/ena_regs_defs.h
new file mode 100644
index 000000000000..26097a2b6030
--- /dev/null
+++ b/drivers/net/ethernet/amazon/ena/ena_regs_defs.h
@@ -0,0 +1,133 @@
+/*
+ * Copyright 2015 - 2016 Amazon.com, Inc. or its affiliates.
+ *
+ * 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
+ * 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 _ENA_REGS_H_
+#define _ENA_REGS_H_
+
+/* ena_registers offsets */
+#define ENA_REGS_VERSION_OFF		0x0
+#define ENA_REGS_CONTROLLER_VERSION_OFF		0x4
+#define ENA_REGS_CAPS_OFF		0x8
+#define ENA_REGS_CAPS_EXT_OFF		0xc
+#define ENA_REGS_AQ_BASE_LO_OFF		0x10
+#define ENA_REGS_AQ_BASE_HI_OFF		0x14
+#define ENA_REGS_AQ_CAPS_OFF		0x18
+#define ENA_REGS_ACQ_BASE_LO_OFF		0x20
+#define ENA_REGS_ACQ_BASE_HI_OFF		0x24
+#define ENA_REGS_ACQ_CAPS_OFF		0x28
+#define ENA_REGS_AQ_DB_OFF		0x2c
+#define ENA_REGS_ACQ_TAIL_OFF		0x30
+#define ENA_REGS_AENQ_CAPS_OFF		0x34
+#define ENA_REGS_AENQ_BASE_LO_OFF		0x38
+#define ENA_REGS_AENQ_BASE_HI_OFF		0x3c
+#define ENA_REGS_AENQ_HEAD_DB_OFF		0x40
+#define ENA_REGS_AENQ_TAIL_OFF		0x44
+#define ENA_REGS_INTR_MASK_OFF		0x4c
+#define ENA_REGS_DEV_CTL_OFF		0x54
+#define ENA_REGS_DEV_STS_OFF		0x58
+#define ENA_REGS_MMIO_REG_READ_OFF		0x5c
+#define ENA_REGS_MMIO_RESP_LO_OFF		0x60
+#define ENA_REGS_MMIO_RESP_HI_OFF		0x64
+#define ENA_REGS_RSS_IND_ENTRY_UPDATE_OFF		0x68
+
+/* version register */
+#define ENA_REGS_VERSION_MINOR_VERSION_MASK		0xff
+#define ENA_REGS_VERSION_MAJOR_VERSION_SHIFT		8
+#define ENA_REGS_VERSION_MAJOR_VERSION_MASK		0xff00
+
+/* controller_version register */
+#define ENA_REGS_CONTROLLER_VERSION_SUBMINOR_VERSION_MASK		0xff
+#define ENA_REGS_CONTROLLER_VERSION_MINOR_VERSION_SHIFT		8
+#define ENA_REGS_CONTROLLER_VERSION_MINOR_VERSION_MASK		0xff00
+#define ENA_REGS_CONTROLLER_VERSION_MAJOR_VERSION_SHIFT		16
+#define ENA_REGS_CONTROLLER_VERSION_MAJOR_VERSION_MASK		0xff0000
+#define ENA_REGS_CONTROLLER_VERSION_IMPL_ID_SHIFT		24
+#define ENA_REGS_CONTROLLER_VERSION_IMPL_ID_MASK		0xff000000
+
+/* caps register */
+#define ENA_REGS_CAPS_CONTIGUOUS_QUEUE_REQUIRED_MASK		0x1
+#define ENA_REGS_CAPS_RESET_TIMEOUT_SHIFT		1
+#define ENA_REGS_CAPS_RESET_TIMEOUT_MASK		0x3e
+#define ENA_REGS_CAPS_DMA_ADDR_WIDTH_SHIFT		8
+#define ENA_REGS_CAPS_DMA_ADDR_WIDTH_MASK		0xff00
+
+/* aq_caps register */
+#define ENA_REGS_AQ_CAPS_AQ_DEPTH_MASK		0xffff
+#define ENA_REGS_AQ_CAPS_AQ_ENTRY_SIZE_SHIFT		16
+#define ENA_REGS_AQ_CAPS_AQ_ENTRY_SIZE_MASK		0xffff0000
+
+/* acq_caps register */
+#define ENA_REGS_ACQ_CAPS_ACQ_DEPTH_MASK		0xffff
+#define ENA_REGS_ACQ_CAPS_ACQ_ENTRY_SIZE_SHIFT		16
+#define ENA_REGS_ACQ_CAPS_ACQ_ENTRY_SIZE_MASK		0xffff0000
+
+/* aenq_caps register */
+#define ENA_REGS_AENQ_CAPS_AENQ_DEPTH_MASK		0xffff
+#define ENA_REGS_AENQ_CAPS_AENQ_ENTRY_SIZE_SHIFT		16
+#define ENA_REGS_AENQ_CAPS_AENQ_ENTRY_SIZE_MASK		0xffff0000
+
+/* dev_ctl register */
+#define ENA_REGS_DEV_CTL_DEV_RESET_MASK		0x1
+#define ENA_REGS_DEV_CTL_AQ_RESTART_SHIFT		1
+#define ENA_REGS_DEV_CTL_AQ_RESTART_MASK		0x2
+#define ENA_REGS_DEV_CTL_QUIESCENT_SHIFT		2
+#define ENA_REGS_DEV_CTL_QUIESCENT_MASK		0x4
+#define ENA_REGS_DEV_CTL_IO_RESUME_SHIFT		3
+#define ENA_REGS_DEV_CTL_IO_RESUME_MASK		0x8
+
+/* dev_sts register */
+#define ENA_REGS_DEV_STS_READY_MASK		0x1
+#define ENA_REGS_DEV_STS_AQ_RESTART_IN_PROGRESS_SHIFT		1
+#define ENA_REGS_DEV_STS_AQ_RESTART_IN_PROGRESS_MASK		0x2
+#define ENA_REGS_DEV_STS_AQ_RESTART_FINISHED_SHIFT		2
+#define ENA_REGS_DEV_STS_AQ_RESTART_FINISHED_MASK		0x4
+#define ENA_REGS_DEV_STS_RESET_IN_PROGRESS_SHIFT		3
+#define ENA_REGS_DEV_STS_RESET_IN_PROGRESS_MASK		0x8
+#define ENA_REGS_DEV_STS_RESET_FINISHED_SHIFT		4
+#define ENA_REGS_DEV_STS_RESET_FINISHED_MASK		0x10
+#define ENA_REGS_DEV_STS_FATAL_ERROR_SHIFT		5
+#define ENA_REGS_DEV_STS_FATAL_ERROR_MASK		0x20
+#define ENA_REGS_DEV_STS_QUIESCENT_STATE_IN_PROGRESS_SHIFT		6
+#define ENA_REGS_DEV_STS_QUIESCENT_STATE_IN_PROGRESS_MASK		0x40
+#define ENA_REGS_DEV_STS_QUIESCENT_STATE_ACHIEVED_SHIFT		7
+#define ENA_REGS_DEV_STS_QUIESCENT_STATE_ACHIEVED_MASK		0x80
+
+/* mmio_reg_read register */
+#define ENA_REGS_MMIO_REG_READ_REQ_ID_MASK		0xffff
+#define ENA_REGS_MMIO_REG_READ_REG_OFF_SHIFT		16
+#define ENA_REGS_MMIO_REG_READ_REG_OFF_MASK		0xffff0000
+
+/* rss_ind_entry_update register */
+#define ENA_REGS_RSS_IND_ENTRY_UPDATE_INDEX_MASK		0xffff
+#define ENA_REGS_RSS_IND_ENTRY_UPDATE_CQ_IDX_SHIFT		16
+#define ENA_REGS_RSS_IND_ENTRY_UPDATE_CQ_IDX_MASK		0xffff0000
+
+#endif /*_ENA_REGS_H_ */
diff --git a/drivers/net/ethernet/amd/7990.c b/drivers/net/ethernet/amd/7990.c
index dcf2a1f3643d..dc57f2759f44 100644
--- a/drivers/net/ethernet/amd/7990.c
+++ b/drivers/net/ethernet/amd/7990.c
@@ -45,14 +45,14 @@
 #define WRITERDP(lp, x)	out_be16(lp->base + LANCE_RDP, (x))
 #define READRDP(lp)	in_be16(lp->base + LANCE_RDP)
 
-#if defined(CONFIG_HPLANCE) || defined(CONFIG_HPLANCE_MODULE)
+#if IS_ENABLED(CONFIG_HPLANCE)
 #include "hplance.h"
 
 #undef WRITERAP
 #undef WRITERDP
 #undef READRDP
 
-#if defined(CONFIG_MVME147_NET) || defined(CONFIG_MVME147_NET_MODULE)
+#if IS_ENABLED(CONFIG_MVME147_NET)
 
 /* Lossage Factor Nine, Mr Sulu. */
 #define WRITERAP(lp, x)	(lp->writerap(lp, x))
@@ -86,7 +86,7 @@ static inline __u16 READRDP(struct lance_private *lp)
 }
 
 #endif
-#endif /* CONFIG_HPLANCE || CONFIG_HPLANCE_MODULE */
+#endif /* IS_ENABLED(CONFIG_HPLANCE) */
 
 /* debugging output macros, various flavours */
 /* #define TEST_HITS */
diff --git a/drivers/net/ethernet/amd/amd8111e.c b/drivers/net/ethernet/amd/amd8111e.c
index 94960055fa1f..f92cc97151ec 100644
--- a/drivers/net/ethernet/amd/amd8111e.c
+++ b/drivers/net/ethernet/amd/amd8111e.c
@@ -89,7 +89,7 @@ Revision History:
 #include <asm/byteorder.h>
 #include <asm/uaccess.h>
 
-#if defined(CONFIG_VLAN_8021Q) || defined(CONFIG_VLAN_8021Q_MODULE)
+#if IS_ENABLED(CONFIG_VLAN_8021Q)
 #define AMD8111E_VLAN_TAG_USED 1
 #else
 #define AMD8111E_VLAN_TAG_USED 0
diff --git a/drivers/net/ethernet/amd/xgbe/xgbe-drv.c b/drivers/net/ethernet/amd/xgbe/xgbe-drv.c
index a9b2709567ec..7f9216db026f 100644
--- a/drivers/net/ethernet/amd/xgbe/xgbe-drv.c
+++ b/drivers/net/ethernet/amd/xgbe/xgbe-drv.c
@@ -1708,9 +1708,9 @@ static const struct net_device_ops xgbe_netdev_ops = {
 	.ndo_set_features	= xgbe_set_features,
 };
 
-struct net_device_ops *xgbe_get_netdev_ops(void)
+const struct net_device_ops *xgbe_get_netdev_ops(void)
 {
-	return (struct net_device_ops *)&xgbe_netdev_ops;
+	return &xgbe_netdev_ops;
 }
 
 static void xgbe_rx_refresh(struct xgbe_channel *channel)
diff --git a/drivers/net/ethernet/amd/xgbe/xgbe-ethtool.c b/drivers/net/ethernet/amd/xgbe/xgbe-ethtool.c
index 11d9f0c5b78b..4007b429c80c 100644
--- a/drivers/net/ethernet/amd/xgbe/xgbe-ethtool.c
+++ b/drivers/net/ethernet/amd/xgbe/xgbe-ethtool.c
@@ -623,7 +623,7 @@ static const struct ethtool_ops xgbe_ethtool_ops = {
 	.get_ts_info = xgbe_get_ts_info,
 };
 
-struct ethtool_ops *xgbe_get_ethtool_ops(void)
+const struct ethtool_ops *xgbe_get_ethtool_ops(void)
 {
-	return (struct ethtool_ops *)&xgbe_ethtool_ops;
+	return &xgbe_ethtool_ops;
 }
diff --git a/drivers/net/ethernet/amd/xgbe/xgbe-main.c b/drivers/net/ethernet/amd/xgbe/xgbe-main.c
index 3eee3201b58f..9de078819aa6 100644
--- a/drivers/net/ethernet/amd/xgbe/xgbe-main.c
+++ b/drivers/net/ethernet/amd/xgbe/xgbe-main.c
@@ -861,9 +861,15 @@ static int xgbe_resume(struct device *dev)
 	pdata->lpm_ctrl &= ~MDIO_CTRL1_LPOWER;
 	XMDIO_WRITE(pdata, MDIO_MMD_PCS, MDIO_CTRL1, pdata->lpm_ctrl);
 
-	if (netif_running(netdev))
+	if (netif_running(netdev)) {
 		ret = xgbe_powerup(netdev, XGMAC_DRIVER_CONTEXT);
 
+		/* Schedule a restart in case the link or phy state changed
+		 * while we were powered down.
+		 */
+		schedule_work(&pdata->restart_work);
+	}
+
 	DBGPR("<--xgbe_resume\n");
 
 	return ret;
diff --git a/drivers/net/ethernet/amd/xgbe/xgbe.h b/drivers/net/ethernet/amd/xgbe/xgbe.h
index 98d9d63c4353..5dd17dcea2f8 100644
--- a/drivers/net/ethernet/amd/xgbe/xgbe.h
+++ b/drivers/net/ethernet/amd/xgbe/xgbe.h
@@ -956,8 +956,9 @@ struct xgbe_prv_data {
 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);
+const struct net_device_ops *xgbe_get_netdev_ops(void);
+const struct ethtool_ops *xgbe_get_ethtool_ops(void);
+
 #ifdef CONFIG_AMD_XGBE_DCB
 const struct dcbnl_rtnl_ops *xgbe_get_dcbnl_ops(void);
 #endif
diff --git a/drivers/net/ethernet/apm/xgene/Kconfig b/drivers/net/ethernet/apm/xgene/Kconfig
index 300e3b5c54e0..afccb033177b 100644
--- a/drivers/net/ethernet/apm/xgene/Kconfig
+++ b/drivers/net/ethernet/apm/xgene/Kconfig
@@ -4,6 +4,7 @@ config NET_XGENE
 	depends on ARCH_XGENE || COMPILE_TEST
 	select PHYLIB
 	select MDIO_XGENE
+	select GPIOLIB
 	help
 	  This is the Ethernet driver for the on-chip ethernet interface on the
 	  APM X-Gene SoC.
diff --git a/drivers/net/ethernet/apm/xgene/xgene_enet_cle.c b/drivers/net/ethernet/apm/xgene/xgene_enet_cle.c
index 472c0fb3f4c4..23d72af83d82 100644
--- a/drivers/net/ethernet/apm/xgene/xgene_enet_cle.c
+++ b/drivers/net/ethernet/apm/xgene/xgene_enet_cle.c
@@ -32,12 +32,19 @@ static void xgene_cle_sband_to_hw(u8 frag, enum xgene_cle_prot_version ver,
 		SET_VAL(SB_HDRLEN, len);
 }
 
-static void xgene_cle_idt_to_hw(u32 dstqid, u32 fpsel,
+static void xgene_cle_idt_to_hw(struct xgene_enet_pdata *pdata,
+				u32 dstqid, u32 fpsel,
 				u32 nfpsel, u32 *idt_reg)
 {
-	*idt_reg =  SET_VAL(IDT_DSTQID, dstqid) |
-		    SET_VAL(IDT_FPSEL, fpsel) |
-		    SET_VAL(IDT_NFPSEL, nfpsel);
+	if (pdata->enet_id == XGENE_ENET1) {
+		*idt_reg = SET_VAL(IDT_DSTQID, dstqid) |
+			   SET_VAL(IDT_FPSEL1, fpsel)  |
+			   SET_VAL(IDT_NFPSEL1, nfpsel);
+	} else {
+		*idt_reg = SET_VAL(IDT_DSTQID, dstqid) |
+			   SET_VAL(IDT_FPSEL, fpsel)   |
+			   SET_VAL(IDT_NFPSEL, nfpsel);
+	}
 }
 
 static void xgene_cle_dbptr_to_hw(struct xgene_enet_pdata *pdata,
@@ -344,7 +351,7 @@ static int xgene_cle_set_rss_idt(struct xgene_enet_pdata *pdata)
 		nfpsel = 0;
 		idt_reg = 0;
 
-		xgene_cle_idt_to_hw(dstqid, fpsel, nfpsel, &idt_reg);
+		xgene_cle_idt_to_hw(pdata, dstqid, fpsel, nfpsel, &idt_reg);
 		ret = xgene_cle_dram_wr(&pdata->cle, &idt_reg, 1, i,
 					RSS_IDT, CLE_CMD_WR);
 		if (ret)
diff --git a/drivers/net/ethernet/apm/xgene/xgene_enet_cle.h b/drivers/net/ethernet/apm/xgene/xgene_enet_cle.h
index 33c5f6b25824..9ac9f8e145ec 100644
--- a/drivers/net/ethernet/apm/xgene/xgene_enet_cle.h
+++ b/drivers/net/ethernet/apm/xgene/xgene_enet_cle.h
@@ -196,9 +196,13 @@ enum xgene_cle_ptree_dbptrs {
 #define IDT_DSTQID_POS		0
 #define IDT_DSTQID_LEN		12
 #define IDT_FPSEL_POS		12
-#define IDT_FPSEL_LEN		4
-#define IDT_NFPSEL_POS		16
-#define IDT_NFPSEL_LEN		4
+#define IDT_FPSEL_LEN		5
+#define IDT_NFPSEL_POS		17
+#define IDT_NFPSEL_LEN		5
+#define IDT_FPSEL1_POS		12
+#define IDT_FPSEL1_LEN		4
+#define IDT_NFPSEL1_POS		16
+#define IDT_NFPSEL1_LEN		4
 
 struct xgene_cle_ptree_branch {
 	bool valid;
diff --git a/drivers/net/ethernet/apm/xgene/xgene_enet_ethtool.c b/drivers/net/ethernet/apm/xgene/xgene_enet_ethtool.c
index 22a7b26ca1d6..d372d4235c81 100644
--- a/drivers/net/ethernet/apm/xgene/xgene_enet_ethtool.c
+++ b/drivers/net/ethernet/apm/xgene/xgene_enet_ethtool.c
@@ -54,55 +54,68 @@ static void xgene_get_drvinfo(struct net_device *ndev,
 	sprintf(info->bus_info, "%s", pdev->name);
 }
 
-static int xgene_get_settings(struct net_device *ndev, struct ethtool_cmd *cmd)
+static int xgene_get_link_ksettings(struct net_device *ndev,
+				    struct ethtool_link_ksettings *cmd)
 {
 	struct xgene_enet_pdata *pdata = netdev_priv(ndev);
-	struct phy_device *phydev = pdata->phy_dev;
+	struct phy_device *phydev = ndev->phydev;
+	u32 supported;
 
 	if (pdata->phy_mode == PHY_INTERFACE_MODE_RGMII) {
 		if (phydev == NULL)
 			return -ENODEV;
 
-		return phy_ethtool_gset(phydev, cmd);
+		return phy_ethtool_ksettings_get(phydev, cmd);
 	} else if (pdata->phy_mode == PHY_INTERFACE_MODE_SGMII) {
 		if (pdata->mdio_driver) {
 			if (!phydev)
 				return -ENODEV;
 
-			return phy_ethtool_gset(phydev, cmd);
+			return phy_ethtool_ksettings_get(phydev, cmd);
 		}
 
-		cmd->supported = SUPPORTED_1000baseT_Full | SUPPORTED_Autoneg |
-				 SUPPORTED_MII;
-		cmd->advertising = cmd->supported;
-		ethtool_cmd_speed_set(cmd, SPEED_1000);
-		cmd->duplex = DUPLEX_FULL;
-		cmd->port = PORT_MII;
-		cmd->transceiver = XCVR_INTERNAL;
-		cmd->autoneg = AUTONEG_ENABLE;
+		supported = SUPPORTED_1000baseT_Full | SUPPORTED_Autoneg |
+			SUPPORTED_MII;
+		ethtool_convert_legacy_u32_to_link_mode(
+			cmd->link_modes.supported,
+			supported);
+		ethtool_convert_legacy_u32_to_link_mode(
+			cmd->link_modes.advertising,
+			supported);
+
+		cmd->base.speed = SPEED_1000;
+		cmd->base.duplex = DUPLEX_FULL;
+		cmd->base.port = PORT_MII;
+		cmd->base.autoneg = AUTONEG_ENABLE;
 	} else {
-		cmd->supported = SUPPORTED_10000baseT_Full | SUPPORTED_FIBRE;
-		cmd->advertising = cmd->supported;
-		ethtool_cmd_speed_set(cmd, SPEED_10000);
-		cmd->duplex = DUPLEX_FULL;
-		cmd->port = PORT_FIBRE;
-		cmd->transceiver = XCVR_INTERNAL;
-		cmd->autoneg = AUTONEG_DISABLE;
+		supported = SUPPORTED_10000baseT_Full | SUPPORTED_FIBRE;
+		ethtool_convert_legacy_u32_to_link_mode(
+			cmd->link_modes.supported,
+			supported);
+		ethtool_convert_legacy_u32_to_link_mode(
+			cmd->link_modes.advertising,
+			supported);
+
+		cmd->base.speed = SPEED_10000;
+		cmd->base.duplex = DUPLEX_FULL;
+		cmd->base.port = PORT_FIBRE;
+		cmd->base.autoneg = AUTONEG_DISABLE;
 	}
 
 	return 0;
 }
 
-static int xgene_set_settings(struct net_device *ndev, struct ethtool_cmd *cmd)
+static int xgene_set_link_ksettings(struct net_device *ndev,
+				    const struct ethtool_link_ksettings *cmd)
 {
 	struct xgene_enet_pdata *pdata = netdev_priv(ndev);
-	struct phy_device *phydev = pdata->phy_dev;
+	struct phy_device *phydev = ndev->phydev;
 
 	if (pdata->phy_mode == PHY_INTERFACE_MODE_RGMII) {
 		if (!phydev)
 			return -ENODEV;
 
-		return phy_ethtool_sset(phydev, cmd);
+		return phy_ethtool_ksettings_set(phydev, cmd);
 	}
 
 	if (pdata->phy_mode == PHY_INTERFACE_MODE_SGMII) {
@@ -110,7 +123,7 @@ static int xgene_set_settings(struct net_device *ndev, struct ethtool_cmd *cmd)
 			if (!phydev)
 				return -ENODEV;
 
-			return phy_ethtool_sset(phydev, cmd);
+			return phy_ethtool_ksettings_set(phydev, cmd);
 		}
 	}
 
@@ -152,12 +165,12 @@ static void xgene_get_ethtool_stats(struct net_device *ndev,
 
 static const struct ethtool_ops xgene_ethtool_ops = {
 	.get_drvinfo = xgene_get_drvinfo,
-	.get_settings = xgene_get_settings,
-	.set_settings = xgene_set_settings,
 	.get_link = ethtool_op_get_link,
 	.get_strings = xgene_get_strings,
 	.get_sset_count = xgene_get_sset_count,
-	.get_ethtool_stats = xgene_get_ethtool_stats
+	.get_ethtool_stats = xgene_get_ethtool_stats,
+	.get_link_ksettings = xgene_get_link_ksettings,
+	.set_link_ksettings = xgene_set_link_ksettings,
 };
 
 void xgene_enet_set_ethtool_ops(struct net_device *ndev)
diff --git a/drivers/net/ethernet/apm/xgene/xgene_enet_hw.c b/drivers/net/ethernet/apm/xgene/xgene_enet_hw.c
index 18bb9556dd00..c481f104a8fe 100644
--- a/drivers/net/ethernet/apm/xgene/xgene_enet_hw.c
+++ b/drivers/net/ethernet/apm/xgene/xgene_enet_hw.c
@@ -713,7 +713,7 @@ static void xgene_enet_adjust_link(struct net_device *ndev)
 {
 	struct xgene_enet_pdata *pdata = netdev_priv(ndev);
 	const struct xgene_mac_ops *mac_ops = pdata->mac_ops;
-	struct phy_device *phydev = pdata->phy_dev;
+	struct phy_device *phydev = ndev->phydev;
 
 	if (phydev->link) {
 		if (pdata->phy_speed != phydev->speed) {
@@ -761,31 +761,25 @@ int xgene_enet_phy_connect(struct net_device *ndev)
 	if (dev->of_node) {
 		for (i = 0 ; i < 2; i++) {
 			np = of_parse_phandle(dev->of_node, "phy-handle", i);
-			if (np)
+			phy_dev = of_phy_connect(ndev, np,
+						 &xgene_enet_adjust_link,
+						 0, pdata->phy_mode);
+			of_node_put(np);
+			if (phy_dev)
 				break;
 		}
 
-		if (!np) {
-			netdev_dbg(ndev, "No phy-handle found in DT\n");
-			return -ENODEV;
-		}
-
-		phy_dev = of_phy_connect(ndev, np, &xgene_enet_adjust_link,
-					 0, pdata->phy_mode);
-		of_node_put(np);
 		if (!phy_dev) {
 			netdev_err(ndev, "Could not connect to PHY\n");
 			return -ENODEV;
 		}
-
-		pdata->phy_dev = phy_dev;
 	} else {
 #ifdef CONFIG_ACPI
 		struct acpi_device *adev = acpi_phy_find_device(dev);
 		if (adev)
-			pdata->phy_dev =  adev->driver_data;
-
-		phy_dev = pdata->phy_dev;
+			phy_dev = adev->driver_data;
+		else
+			phy_dev = NULL;
 
 		if (!phy_dev ||
 		    phy_connect_direct(ndev, phy_dev, &xgene_enet_adjust_link,
@@ -853,8 +847,6 @@ static int xgene_mdiobus_register(struct xgene_enet_pdata *pdata,
 	if (!phy)
 		return -EIO;
 
-	pdata->phy_dev = phy;
-
 	return ret;
 }
 
@@ -894,14 +886,18 @@ int xgene_enet_mdio_config(struct xgene_enet_pdata *pdata)
 
 void xgene_enet_phy_disconnect(struct xgene_enet_pdata *pdata)
 {
-	if (pdata->phy_dev)
-		phy_disconnect(pdata->phy_dev);
+	struct net_device *ndev = pdata->ndev;
+
+	if (ndev->phydev)
+		phy_disconnect(ndev->phydev);
 }
 
 void xgene_enet_mdio_remove(struct xgene_enet_pdata *pdata)
 {
-	if (pdata->phy_dev)
-		phy_disconnect(pdata->phy_dev);
+	struct net_device *ndev = pdata->ndev;
+
+	if (ndev->phydev)
+		phy_disconnect(ndev->phydev);
 
 	mdiobus_unregister(pdata->mdio_bus);
 	mdiobus_free(pdata->mdio_bus);
diff --git a/drivers/net/ethernet/apm/xgene/xgene_enet_hw.h b/drivers/net/ethernet/apm/xgene/xgene_enet_hw.h
index 179a44dceb29..8456337a237d 100644
--- a/drivers/net/ethernet/apm/xgene/xgene_enet_hw.h
+++ b/drivers/net/ethernet/apm/xgene/xgene_enet_hw.h
@@ -124,6 +124,12 @@ enum xgene_enet_rm {
 #define MAC_READ_REG_OFFSET		0x0c
 #define MAC_COMMAND_DONE_REG_OFFSET	0x10
 
+#define PCS_ADDR_REG_OFFSET		0x00
+#define PCS_COMMAND_REG_OFFSET		0x04
+#define PCS_WRITE_REG_OFFSET		0x08
+#define PCS_READ_REG_OFFSET		0x0c
+#define PCS_COMMAND_DONE_REG_OFFSET	0x10
+
 #define MII_MGMT_CONFIG_ADDR		0x20
 #define MII_MGMT_COMMAND_ADDR		0x24
 #define MII_MGMT_ADDRESS_ADDR		0x28
@@ -231,6 +237,8 @@ enum xgene_enet_rm {
 #define TCPHDR_LEN			6
 #define IPHDR_POS			6
 #define IPHDR_LEN			6
+#define MSS_POS				20
+#define MSS_LEN				2
 #define EC_POS				22	/* Enable checksum */
 #define EC_LEN				1
 #define ET_POS				23	/* Enable TSO */
@@ -247,6 +255,11 @@ enum xgene_enet_rm {
 
 #define LAST_BUFFER			(0x7800ULL << BUFDATALEN_POS)
 
+#define TSO_MSS0_POS			0
+#define TSO_MSS0_LEN			14
+#define TSO_MSS1_POS			16
+#define TSO_MSS1_LEN			14
+
 struct xgene_enet_raw_desc {
 	__le64 m0;
 	__le64 m1;
diff --git a/drivers/net/ethernet/apm/xgene/xgene_enet_main.c b/drivers/net/ethernet/apm/xgene/xgene_enet_main.c
index d1d6b5eeb613..429f18fc5503 100644
--- a/drivers/net/ethernet/apm/xgene/xgene_enet_main.c
+++ b/drivers/net/ethernet/apm/xgene/xgene_enet_main.c
@@ -19,6 +19,7 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#include <linux/gpio.h>
 #include "xgene_enet_main.h"
 #include "xgene_enet_hw.h"
 #include "xgene_enet_sgmac.h"
@@ -72,7 +73,6 @@ static int xgene_enet_refill_bufpool(struct xgene_enet_desc_ring *buf_pool,
 		skb = netdev_alloc_skb_ip_align(ndev, len);
 		if (unlikely(!skb))
 			return -ENOMEM;
-		buf_pool->rx_skb[tail] = skb;
 
 		dma_addr = dma_map_single(dev, skb->data, len, DMA_FROM_DEVICE);
 		if (dma_mapping_error(dev, dma_addr)) {
@@ -81,6 +81,8 @@ static int xgene_enet_refill_bufpool(struct xgene_enet_desc_ring *buf_pool,
 			return -EINVAL;
 		}
 
+		buf_pool->rx_skb[tail] = skb;
+
 		raw_desc->m1 = cpu_to_le64(SET_VAL(DATAADDR, dma_addr) |
 					   SET_VAL(BUFDATALEN, bufdatalen) |
 					   SET_BIT(COHERENT));
@@ -102,12 +104,21 @@ static u8 xgene_enet_hdr_len(const void *data)
 
 static void xgene_enet_delete_bufpool(struct xgene_enet_desc_ring *buf_pool)
 {
+	struct device *dev = ndev_to_dev(buf_pool->ndev);
+	struct xgene_enet_raw_desc16 *raw_desc;
+	dma_addr_t dma_addr;
 	int i;
 
 	/* Free up the buffers held by hardware */
 	for (i = 0; i < buf_pool->slots; i++) {
-		if (buf_pool->rx_skb[i])
+		if (buf_pool->rx_skb[i]) {
 			dev_kfree_skb_any(buf_pool->rx_skb[i]);
+
+			raw_desc = &buf_pool->raw_desc16[i];
+			dma_addr = GET_VAL(DATAADDR, le64_to_cpu(raw_desc->m1));
+			dma_unmap_single(dev, dma_addr, XGENE_ENET_MAX_MTU,
+					 DMA_FROM_DEVICE);
+		}
 	}
 }
 
@@ -126,6 +137,7 @@ static irqreturn_t xgene_enet_rx_irq(const int irq, void *data)
 static int xgene_enet_tx_completion(struct xgene_enet_desc_ring *cp_ring,
 				    struct xgene_enet_raw_desc *raw_desc)
 {
+	struct xgene_enet_pdata *pdata = netdev_priv(cp_ring->ndev);
 	struct sk_buff *skb;
 	struct device *dev;
 	skb_frag_t *frag;
@@ -133,6 +145,7 @@ static int xgene_enet_tx_completion(struct xgene_enet_desc_ring *cp_ring,
 	u16 skb_index;
 	u8 status;
 	int i, ret = 0;
+	u8 mss_index;
 
 	skb_index = GET_VAL(USERINFO, le64_to_cpu(raw_desc->m0));
 	skb = cp_ring->cp_skb[skb_index];
@@ -149,6 +162,13 @@ static int xgene_enet_tx_completion(struct xgene_enet_desc_ring *cp_ring,
 			       DMA_TO_DEVICE);
 	}
 
+	if (GET_BIT(ET, le64_to_cpu(raw_desc->m3))) {
+		mss_index = GET_VAL(MSS, le64_to_cpu(raw_desc->m3));
+		spin_lock(&pdata->mss_lock);
+		pdata->mss_refcnt[mss_index]--;
+		spin_unlock(&pdata->mss_lock);
+	}
+
 	/* Checking for error */
 	status = GET_VAL(LERR, le64_to_cpu(raw_desc->m0));
 	if (unlikely(status > 2)) {
@@ -167,15 +187,53 @@ static int xgene_enet_tx_completion(struct xgene_enet_desc_ring *cp_ring,
 	return ret;
 }
 
-static u64 xgene_enet_work_msg(struct sk_buff *skb)
+static int xgene_enet_setup_mss(struct net_device *ndev, u32 mss)
+{
+	struct xgene_enet_pdata *pdata = netdev_priv(ndev);
+	bool mss_index_found = false;
+	int mss_index;
+	int i;
+
+	spin_lock(&pdata->mss_lock);
+
+	/* Reuse the slot if MSS matches */
+	for (i = 0; !mss_index_found && i < NUM_MSS_REG; i++) {
+		if (pdata->mss[i] == mss) {
+			pdata->mss_refcnt[i]++;
+			mss_index = i;
+			mss_index_found = true;
+		}
+	}
+
+	/* Overwrite the slot with ref_count = 0 */
+	for (i = 0; !mss_index_found && i < NUM_MSS_REG; i++) {
+		if (!pdata->mss_refcnt[i]) {
+			pdata->mss_refcnt[i]++;
+			pdata->mac_ops->set_mss(pdata, mss, i);
+			pdata->mss[i] = mss;
+			mss_index = i;
+			mss_index_found = true;
+		}
+	}
+
+	spin_unlock(&pdata->mss_lock);
+
+	/* No slots with ref_count = 0 available, return busy */
+	if (!mss_index_found)
+		return -EBUSY;
+
+	return mss_index;
+}
+
+static int xgene_enet_work_msg(struct sk_buff *skb, u64 *hopinfo)
 {
 	struct net_device *ndev = skb->dev;
 	struct iphdr *iph;
 	u8 l3hlen = 0, l4hlen = 0;
 	u8 ethhdr, proto = 0, csum_enable = 0;
-	u64 hopinfo = 0;
 	u32 hdr_len, mss = 0;
 	u32 i, len, nr_frags;
+	int mss_index;
 
 	ethhdr = xgene_enet_hdr_len(skb->data);
 
@@ -215,7 +273,11 @@ static u64 xgene_enet_work_msg(struct sk_buff *skb)
 			if (!mss || ((skb->len - hdr_len) <= mss))
 				goto out;
 
-			hopinfo |= SET_BIT(ET);
+			mss_index = xgene_enet_setup_mss(ndev, mss);
+			if (unlikely(mss_index < 0))
+				return -EBUSY;
+
+			*hopinfo |= SET_BIT(ET) | SET_VAL(MSS, mss_index);
 		}
 	} else if (iph->protocol == IPPROTO_UDP) {
 		l4hlen = UDP_HDR_SIZE;
@@ -223,15 +285,15 @@ static u64 xgene_enet_work_msg(struct sk_buff *skb)
 	}
 out:
 	l3hlen = ip_hdrlen(skb) >> 2;
-	hopinfo |= SET_VAL(TCPHDR, l4hlen) |
-		  SET_VAL(IPHDR, l3hlen) |
-		  SET_VAL(ETHHDR, ethhdr) |
-		  SET_VAL(EC, csum_enable) |
-		  SET_VAL(IS, proto) |
-		  SET_BIT(IC) |
-		  SET_BIT(TYPE_ETH_WORK_MESSAGE);
-
-	return hopinfo;
+	*hopinfo |= SET_VAL(TCPHDR, l4hlen) |
+		    SET_VAL(IPHDR, l3hlen) |
+		    SET_VAL(ETHHDR, ethhdr) |
+		    SET_VAL(EC, csum_enable) |
+		    SET_VAL(IS, proto) |
+		    SET_BIT(IC) |
+		    SET_BIT(TYPE_ETH_WORK_MESSAGE);
+
+	return 0;
 }
 
 static u16 xgene_enet_encode_len(u16 len)
@@ -271,20 +333,22 @@ static int xgene_enet_setup_tx_desc(struct xgene_enet_desc_ring *tx_ring,
 	dma_addr_t dma_addr, pbuf_addr, *frag_dma_addr;
 	skb_frag_t *frag;
 	u16 tail = tx_ring->tail;
-	u64 hopinfo;
+	u64 hopinfo = 0;
 	u32 len, hw_len;
 	u8 ll = 0, nv = 0, idx = 0;
 	bool split = false;
 	u32 size, offset, ell_bytes = 0;
 	u32 i, fidx, nr_frags, count = 1;
+	int ret;
 
 	raw_desc = &tx_ring->raw_desc[tail];
 	tail = (tail + 1) & (tx_ring->slots - 1);
 	memset(raw_desc, 0, sizeof(struct xgene_enet_raw_desc));
 
-	hopinfo = xgene_enet_work_msg(skb);
-	if (!hopinfo)
-		return -EINVAL;
+	ret = xgene_enet_work_msg(skb, &hopinfo);
+	if (ret)
+		return ret;
+
 	raw_desc->m3 = cpu_to_le64(SET_VAL(HENQNUM, tx_ring->dst_ring_num) |
 				   hopinfo);
 
@@ -424,6 +488,9 @@ static netdev_tx_t xgene_enet_start_xmit(struct sk_buff *skb,
 		return NETDEV_TX_OK;
 
 	count = xgene_enet_setup_tx_desc(tx_ring, skb);
+	if (count == -EBUSY)
+		return NETDEV_TX_BUSY;
+
 	if (count <= 0) {
 		dev_kfree_skb_any(skb);
 		return NETDEV_TX_OK;
@@ -452,7 +519,6 @@ static int xgene_enet_rx_frame(struct xgene_enet_desc_ring *rx_ring,
 			       struct xgene_enet_raw_desc *raw_desc)
 {
 	struct net_device *ndev;
-	struct xgene_enet_pdata *pdata;
 	struct device *dev;
 	struct xgene_enet_desc_ring *buf_pool;
 	u32 datalen, skb_index;
@@ -461,7 +527,6 @@ static int xgene_enet_rx_frame(struct xgene_enet_desc_ring *rx_ring,
 	int ret = 0;
 
 	ndev = rx_ring->ndev;
-	pdata = netdev_priv(ndev);
 	dev = ndev_to_dev(rx_ring->ndev);
 	buf_pool = rx_ring->buf_pool;
 
@@ -739,8 +804,8 @@ static int xgene_enet_open(struct net_device *ndev)
 	if (ret)
 		return ret;
 
-	if (pdata->phy_dev) {
-		phy_start(pdata->phy_dev);
+	if (ndev->phydev) {
+		phy_start(ndev->phydev);
 	} else {
 		schedule_delayed_work(&pdata->link_work, PHY_POLL_LINK_OFF);
 		netif_carrier_off(ndev);
@@ -763,8 +828,8 @@ static int xgene_enet_close(struct net_device *ndev)
 	mac_ops->tx_disable(pdata);
 	mac_ops->rx_disable(pdata);
 
-	if (pdata->phy_dev)
-		phy_stop(pdata->phy_dev);
+	if (ndev->phydev)
+		phy_stop(ndev->phydev);
 	else
 		cancel_delayed_work_sync(&pdata->link_work);
 
@@ -1312,6 +1377,18 @@ static int xgene_enet_check_phy_handle(struct xgene_enet_pdata *pdata)
 	return 0;
 }
 
+static void xgene_enet_gpiod_get(struct xgene_enet_pdata *pdata)
+{
+	struct device *dev = &pdata->pdev->dev;
+
+	if (pdata->phy_mode != PHY_INTERFACE_MODE_XGMII)
+		return;
+
+	pdata->sfp_rdy = gpiod_get(dev, "rxlos", GPIOD_IN);
+	if (IS_ERR(pdata->sfp_rdy))
+		pdata->sfp_rdy = gpiod_get(dev, "sfp", GPIOD_IN);
+}
+
 static int xgene_enet_get_resources(struct xgene_enet_pdata *pdata)
 {
 	struct platform_device *pdev;
@@ -1401,6 +1478,8 @@ static int xgene_enet_get_resources(struct xgene_enet_pdata *pdata)
 	if (ret)
 		return ret;
 
+	xgene_enet_gpiod_get(pdata);
+
 	pdata->clk = devm_clk_get(&pdev->dev, NULL);
 	if (IS_ERR(pdata->clk)) {
 		/* Firmware may have set up the clock already. */
@@ -1425,6 +1504,7 @@ static int xgene_enet_get_resources(struct xgene_enet_pdata *pdata)
 	} else {
 		pdata->mcx_mac_addr = base_addr + BLOCK_AXG_MAC_OFFSET;
 		pdata->mcx_mac_csr_addr = base_addr + BLOCK_AXG_MAC_CSR_OFFSET;
+		pdata->pcs_addr = base_addr + BLOCK_PCS_OFFSET;
 	}
 	pdata->rx_buff_cnt = NUM_PKT_BUF;
 
@@ -1454,10 +1534,8 @@ static int xgene_enet_init_hw(struct xgene_enet_pdata *pdata)
 		buf_pool = pdata->rx_ring[i]->buf_pool;
 		xgene_enet_init_bufpool(buf_pool);
 		ret = xgene_enet_refill_bufpool(buf_pool, pdata->rx_buff_cnt);
-		if (ret) {
-			xgene_enet_delete_desc_rings(pdata);
-			return ret;
-		}
+		if (ret)
+			goto err;
 	}
 
 	dst_ring_num = xgene_enet_dst_ring_num(pdata->rx_ring[0]);
@@ -1474,7 +1552,7 @@ static int xgene_enet_init_hw(struct xgene_enet_pdata *pdata)
 		ret = pdata->cle_ops->cle_init(pdata);
 		if (ret) {
 			netdev_err(ndev, "Preclass Tree init error\n");
-			return ret;
+			goto err;
 		}
 	} else {
 		pdata->port_ops->cle_bypass(pdata, dst_ring_num, buf_pool->id);
@@ -1484,6 +1562,10 @@ static int xgene_enet_init_hw(struct xgene_enet_pdata *pdata)
 	pdata->mac_ops->init(pdata);
 
 	return ret;
+
+err:
+	xgene_enet_delete_desc_rings(pdata);
+	return ret;
 }
 
 static void xgene_enet_setup_ops(struct xgene_enet_pdata *pdata)
@@ -1631,8 +1713,8 @@ static int xgene_enet_probe(struct platform_device *pdev)
 	}
 #endif
 	if (!pdata->enet_id) {
-		free_netdev(ndev);
-		return -ENODEV;
+		ret = -ENODEV;
+		goto err;
 	}
 
 	ret = xgene_enet_get_resources(pdata);
@@ -1643,7 +1725,7 @@ static int xgene_enet_probe(struct platform_device *pdev)
 
 	if (pdata->phy_mode == PHY_INTERFACE_MODE_XGMII) {
 		ndev->features |= NETIF_F_TSO;
-		pdata->mss = XGENE_ENET_MSS;
+		spin_lock_init(&pdata->mss_lock);
 	}
 	ndev->hw_features = ndev->features;
 
@@ -1655,7 +1737,7 @@ static int xgene_enet_probe(struct platform_device *pdev)
 
 	ret = xgene_enet_init_hw(pdata);
 	if (ret)
-		goto err_netdev;
+		goto err;
 
 	link_state = pdata->mac_ops->link_state;
 	if (pdata->phy_mode == PHY_INTERFACE_MODE_XGMII) {
@@ -1665,21 +1747,32 @@ static int xgene_enet_probe(struct platform_device *pdev)
 			ret = xgene_enet_mdio_config(pdata);
 		else
 			INIT_DELAYED_WORK(&pdata->link_work, link_state);
+
+		if (ret)
+			goto err1;
 	}
-	if (ret)
-		goto err;
 
 	xgene_enet_napi_add(pdata);
 	ret = register_netdev(ndev);
 	if (ret) {
 		netdev_err(ndev, "Failed to register netdev\n");
-		goto err;
+		goto err2;
 	}
 
 	return 0;
 
-err_netdev:
-	unregister_netdev(ndev);
+err2:
+	/*
+	 * If necessary, free_netdev() will call netif_napi_del() and undo
+	 * the effects of xgene_enet_napi_add()'s calls to netif_napi_add().
+	 */
+
+	if (pdata->mdio_driver)
+		xgene_enet_phy_disconnect(pdata);
+	else if (pdata->phy_mode == PHY_INTERFACE_MODE_RGMII)
+		xgene_enet_mdio_remove(pdata);
+err1:
+	xgene_enet_delete_desc_rings(pdata);
 err:
 	free_netdev(ndev);
 	return ret;
@@ -1688,11 +1781,9 @@ err:
 static int xgene_enet_remove(struct platform_device *pdev)
 {
 	struct xgene_enet_pdata *pdata;
-	const struct xgene_mac_ops *mac_ops;
 	struct net_device *ndev;
 
 	pdata = platform_get_drvdata(pdev);
-	mac_ops = pdata->mac_ops;
 	ndev = pdata->ndev;
 
 	rtnl_lock();
diff --git a/drivers/net/ethernet/apm/xgene/xgene_enet_main.h b/drivers/net/ethernet/apm/xgene/xgene_enet_main.h
index 217546e5714a..0cda58f5a840 100644
--- a/drivers/net/ethernet/apm/xgene/xgene_enet_main.h
+++ b/drivers/net/ethernet/apm/xgene/xgene_enet_main.h
@@ -47,7 +47,7 @@
 #define NUM_PKT_BUF	64
 #define NUM_BUFPOOL	32
 #define MAX_EXP_BUFFS	256
-#define XGENE_ENET_MSS	1448
+#define NUM_MSS_REG	4
 #define XGENE_MIN_ENET_FRAME_SIZE	60
 
 #define XGENE_MAX_ENET_IRQ	16
@@ -143,7 +143,7 @@ struct xgene_mac_ops {
 	void (*rx_disable)(struct xgene_enet_pdata *pdata);
 	void (*set_speed)(struct xgene_enet_pdata *pdata);
 	void (*set_mac_addr)(struct xgene_enet_pdata *pdata);
-	void (*set_mss)(struct xgene_enet_pdata *pdata);
+	void (*set_mss)(struct xgene_enet_pdata *pdata, u16 mss, u8 index);
 	void (*link_state)(struct work_struct *work);
 };
 
@@ -174,7 +174,6 @@ struct xgene_cle_ops {
 struct xgene_enet_pdata {
 	struct net_device *ndev;
 	struct mii_bus *mdio_bus;
-	struct phy_device *phy_dev;
 	int phy_speed;
 	struct clk *clk;
 	struct platform_device *pdev;
@@ -196,6 +195,7 @@ struct xgene_enet_pdata {
 	void __iomem *mcx_mac_addr;
 	void __iomem *mcx_mac_csr_addr;
 	void __iomem *base_addr;
+	void __iomem *pcs_addr;
 	void __iomem *ring_csr_addr;
 	void __iomem *ring_cmd_addr;
 	int phy_mode;
@@ -212,10 +212,13 @@ struct xgene_enet_pdata {
 	u8 eth_bufnum;
 	u8 bp_bufnum;
 	u16 ring_num;
-	u32 mss;
+	u32 mss[NUM_MSS_REG];
+	u32 mss_refcnt[NUM_MSS_REG];
+	spinlock_t mss_lock;  /* mss lock */
 	u8 tx_delay;
 	u8 rx_delay;
 	bool mdio_driver;
+	struct gpio_desc *sfp_rdy;
 };
 
 struct xgene_indirect_ctl {
diff --git a/drivers/net/ethernet/apm/xgene/xgene_enet_xgmac.c b/drivers/net/ethernet/apm/xgene/xgene_enet_xgmac.c
index 9c6ad0dce00f..6475f383ba83 100644
--- a/drivers/net/ethernet/apm/xgene/xgene_enet_xgmac.c
+++ b/drivers/net/ethernet/apm/xgene/xgene_enet_xgmac.c
@@ -18,6 +18,8 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#include <linux/of_gpio.h>
+#include <linux/gpio.h>
 #include "xgene_enet_main.h"
 #include "xgene_enet_hw.h"
 #include "xgene_enet_xgmac.h"
@@ -84,6 +86,21 @@ static void xgene_enet_wr_mac(struct xgene_enet_pdata *pdata,
 			   wr_addr);
 }
 
+static void xgene_enet_wr_pcs(struct xgene_enet_pdata *pdata,
+			      u32 wr_addr, u32 wr_data)
+{
+	void __iomem *addr, *wr, *cmd, *cmd_done;
+
+	addr = pdata->pcs_addr + PCS_ADDR_REG_OFFSET;
+	wr = pdata->pcs_addr + PCS_WRITE_REG_OFFSET;
+	cmd = pdata->pcs_addr + PCS_COMMAND_REG_OFFSET;
+	cmd_done = pdata->pcs_addr + PCS_COMMAND_DONE_REG_OFFSET;
+
+	if (!xgene_enet_wr_indirect(addr, wr, cmd, cmd_done, wr_addr, wr_data))
+		netdev_err(pdata->ndev, "PCS write failed, addr: %04x\n",
+			   wr_addr);
+}
+
 static void xgene_enet_rd_csr(struct xgene_enet_pdata *pdata,
 			      u32 offset, u32 *val)
 {
@@ -122,6 +139,7 @@ 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)
 {
@@ -137,6 +155,25 @@ static void xgene_enet_rd_mac(struct xgene_enet_pdata *pdata,
 			   rd_addr);
 }
 
+static bool xgene_enet_rd_pcs(struct xgene_enet_pdata *pdata,
+			      u32 rd_addr, u32 *rd_data)
+{
+	void __iomem *addr, *rd, *cmd, *cmd_done;
+	bool success;
+
+	addr = pdata->pcs_addr + PCS_ADDR_REG_OFFSET;
+	rd = pdata->pcs_addr + PCS_READ_REG_OFFSET;
+	cmd = pdata->pcs_addr + PCS_COMMAND_REG_OFFSET;
+	cmd_done = pdata->pcs_addr + PCS_COMMAND_DONE_REG_OFFSET;
+
+	success = xgene_enet_rd_indirect(addr, rd, cmd, cmd_done, rd_addr, rd_data);
+	if (!success)
+		netdev_err(pdata->ndev, "PCS read failed, addr: %04x\n",
+			   rd_addr);
+
+	return success;
+}
+
 static int xgene_enet_ecc_init(struct xgene_enet_pdata *pdata)
 {
 	struct net_device *ndev = pdata->ndev;
@@ -171,6 +208,17 @@ static void xgene_xgmac_reset(struct xgene_enet_pdata *pdata)
 	xgene_enet_wr_mac(pdata, AXGMAC_CONFIG_0, 0);
 }
 
+static void xgene_pcs_reset(struct xgene_enet_pdata *pdata)
+{
+	u32 data;
+
+	if (!xgene_enet_rd_pcs(pdata, PCS_CONTROL_1, &data))
+		return;
+
+	xgene_enet_wr_pcs(pdata, PCS_CONTROL_1, data | PCS_CTRL_PCS_RST);
+	xgene_enet_wr_pcs(pdata, PCS_CONTROL_1, data & ~PCS_CTRL_PCS_RST);
+}
+
 static void xgene_xgmac_set_mac_addr(struct xgene_enet_pdata *pdata)
 {
 	u32 addr0, addr1;
@@ -184,9 +232,22 @@ static void xgene_xgmac_set_mac_addr(struct xgene_enet_pdata *pdata)
 	xgene_enet_wr_mac(pdata, HSTMACADR_MSW_ADDR, addr1);
 }
 
-static void xgene_xgmac_set_mss(struct xgene_enet_pdata *pdata)
+static void xgene_xgmac_set_mss(struct xgene_enet_pdata *pdata,
+				u16 mss, u8 index)
 {
-	xgene_enet_wr_csr(pdata, XG_TSIF_MSS_REG0_ADDR, pdata->mss);
+	u8 offset;
+	u32 data;
+
+	offset = (index < 2) ? 0 : 4;
+	xgene_enet_rd_csr(pdata, XG_TSIF_MSS_REG0_ADDR + offset, &data);
+
+	if (!(index & 0x1))
+		data = SET_VAL(TSO_MSS1, data >> TSO_MSS1_POS) |
+			SET_VAL(TSO_MSS0, mss);
+	else
+		data = SET_VAL(TSO_MSS1, mss) | SET_VAL(TSO_MSS0, data);
+
+	xgene_enet_wr_csr(pdata, XG_TSIF_MSS_REG0_ADDR + offset, data);
 }
 
 static u32 xgene_enet_link_status(struct xgene_enet_pdata *pdata)
@@ -210,18 +271,17 @@ static void xgene_xgmac_init(struct xgene_enet_pdata *pdata)
 	xgene_enet_wr_mac(pdata, AXGMAC_CONFIG_1, data);
 
 	xgene_xgmac_set_mac_addr(pdata);
-	xgene_xgmac_set_mss(pdata);
 
 	xgene_enet_rd_csr(pdata, XG_RSIF_CONFIG_REG_ADDR, &data);
 	data |= CFG_RSIF_FPBUFF_TIMEOUT_EN;
 	xgene_enet_wr_csr(pdata, XG_RSIF_CONFIG_REG_ADDR, data);
 
-	xgene_enet_wr_csr(pdata, XG_CFG_BYPASS_ADDR, RESUME_TX);
-	xgene_enet_wr_csr(pdata, XGENET_RX_DV_GATE_REG_0_ADDR, 0);
 	xgene_enet_rd_csr(pdata, XG_ENET_SPARE_CFG_REG_ADDR, &data);
 	data |= BIT(12);
 	xgene_enet_wr_csr(pdata, XG_ENET_SPARE_CFG_REG_ADDR, data);
 	xgene_enet_wr_csr(pdata, XG_ENET_SPARE_CFG_REG_1_ADDR, 0x82);
+	xgene_enet_wr_csr(pdata, XGENET_RX_DV_GATE_REG_0_ADDR, 0);
+	xgene_enet_wr_csr(pdata, XG_CFG_BYPASS_ADDR, RESUME_TX);
 }
 
 static void xgene_xgmac_rx_enable(struct xgene_enet_pdata *pdata)
@@ -359,14 +419,17 @@ static void xgene_enet_link_state(struct work_struct *work)
 {
 	struct xgene_enet_pdata *pdata = container_of(to_delayed_work(work),
 					 struct xgene_enet_pdata, link_work);
+	struct gpio_desc *sfp_rdy = pdata->sfp_rdy;
 	struct net_device *ndev = pdata->ndev;
 	u32 link_status, poll_interval;
 
 	link_status = xgene_enet_link_status(pdata);
+	if (link_status && !IS_ERR(sfp_rdy) && !gpiod_get_value(sfp_rdy))
+		link_status = 0;
+
 	if (link_status) {
 		if (!netif_carrier_ok(ndev)) {
 			netif_carrier_on(ndev);
-			xgene_xgmac_init(pdata);
 			xgene_xgmac_rx_enable(pdata);
 			xgene_xgmac_tx_enable(pdata);
 			netdev_info(ndev, "Link is Up - 10Gbps\n");
@@ -380,6 +443,8 @@ static void xgene_enet_link_state(struct work_struct *work)
 			netdev_info(ndev, "Link is Down\n");
 		}
 		poll_interval = PHY_POLL_LINK_OFF;
+
+		xgene_pcs_reset(pdata);
 	}
 
 	schedule_delayed_work(&pdata->link_work, poll_interval);
diff --git a/drivers/net/ethernet/apm/xgene/xgene_enet_xgmac.h b/drivers/net/ethernet/apm/xgene/xgene_enet_xgmac.h
index f1ea485f916b..360ccbd95566 100644
--- a/drivers/net/ethernet/apm/xgene/xgene_enet_xgmac.h
+++ b/drivers/net/ethernet/apm/xgene/xgene_enet_xgmac.h
@@ -24,6 +24,7 @@
 #define X2_BLOCK_ETH_MAC_CSR_OFFSET	0x3000
 #define BLOCK_AXG_MAC_OFFSET		0x0800
 #define BLOCK_AXG_MAC_CSR_OFFSET	0x2000
+#define BLOCK_PCS_OFFSET		0x3800
 
 #define XGENET_CONFIG_REG_ADDR		0x20
 #define XGENET_SRST_ADDR		0x00
@@ -72,6 +73,9 @@
 #define XG_MCX_ICM_CONFIG0_REG_0_ADDR	0x00e0
 #define XG_MCX_ICM_CONFIG2_REG_0_ADDR	0x00e8
 
+#define PCS_CONTROL_1			0x0000
+#define PCS_CTRL_PCS_RST		BIT(15)
+
 extern const struct xgene_mac_ops xgene_xgmac_ops;
 extern const struct xgene_port_ops xgene_xgport_ops;
 
diff --git a/drivers/net/ethernet/arc/emac_mdio.c b/drivers/net/ethernet/arc/emac_mdio.c
index 058460bdd5a6..a22403c688c9 100644
--- a/drivers/net/ethernet/arc/emac_mdio.c
+++ b/drivers/net/ethernet/arc/emac_mdio.c
@@ -104,7 +104,7 @@ static int arc_mdio_write(struct mii_bus *bus, int phy_addr,
  * @bus: points to the mii_bus structure
  * Description: reset the MII bus
  */
-int arc_mdio_reset(struct mii_bus *bus)
+static int arc_mdio_reset(struct mii_bus *bus)
 {
 	struct arc_emac_priv *priv = bus->priv;
 	struct arc_emac_mdio_bus_data *data = &priv->bus_data;
diff --git a/drivers/net/ethernet/atheros/alx/alx.h b/drivers/net/ethernet/atheros/alx/alx.h
index 8fc93c5f6abc..6cac919272ea 100644
--- a/drivers/net/ethernet/atheros/alx/alx.h
+++ b/drivers/net/ethernet/atheros/alx/alx.h
@@ -76,11 +76,19 @@ enum alx_device_quirks {
 	ALX_DEV_QUIRK_MSI_INTX_DISABLE_BUG = BIT(0),
 };
 
+#define ALX_FLAG_USING_MSIX	BIT(0)
+#define ALX_FLAG_USING_MSI	BIT(1)
+
 struct alx_priv {
 	struct net_device *dev;
 
 	struct alx_hw hw;
 
+	/* msi-x vectors */
+	int num_vec;
+	struct msix_entry *msix_entries;
+	char irq_lbl[IFNAMSIZ + 8];
+
 	/* all descriptor memory */
 	struct {
 		dma_addr_t dma;
@@ -105,7 +113,7 @@ struct alx_priv {
 
 	u16 msg_enable;
 
-	bool msi;
+	int flags;
 
 	/* protects hw.stats */
 	spinlock_t stats_lock;
diff --git a/drivers/net/ethernet/atheros/alx/hw.c b/drivers/net/ethernet/atheros/alx/hw.c
index 1fe35e453d43..6ac40b0003a3 100644
--- a/drivers/net/ethernet/atheros/alx/hw.c
+++ b/drivers/net/ethernet/atheros/alx/hw.c
@@ -1031,6 +1031,20 @@ void alx_configure_basic(struct alx_hw *hw)
 	alx_write_mem32(hw, ALX_WRR, val);
 }
 
+void alx_mask_msix(struct alx_hw *hw, int index, bool mask)
+{
+	u32 reg, val;
+
+	reg = ALX_MSIX_ENTRY_BASE + index * PCI_MSIX_ENTRY_SIZE +
+		PCI_MSIX_ENTRY_VECTOR_CTRL;
+
+	val = mask ? PCI_MSIX_ENTRY_CTRL_MASKBIT : 0;
+
+	alx_write_mem32(hw, reg, val);
+	alx_post_write(hw);
+}
+
+
 bool alx_get_phy_info(struct alx_hw *hw)
 {
 	u16  devs1, devs2;
diff --git a/drivers/net/ethernet/atheros/alx/hw.h b/drivers/net/ethernet/atheros/alx/hw.h
index f289c05f5cb4..0191477ace51 100644
--- a/drivers/net/ethernet/atheros/alx/hw.h
+++ b/drivers/net/ethernet/atheros/alx/hw.h
@@ -562,6 +562,7 @@ int alx_reset_mac(struct alx_hw *hw);
 void alx_set_macaddr(struct alx_hw *hw, const u8 *addr);
 bool alx_phy_configured(struct alx_hw *hw);
 void alx_configure_basic(struct alx_hw *hw);
+void alx_mask_msix(struct alx_hw *hw, int index, bool mask);
 void alx_disable_rss(struct alx_hw *hw);
 bool alx_get_phy_info(struct alx_hw *hw);
 void alx_update_hw_stats(struct alx_hw *hw);
diff --git a/drivers/net/ethernet/atheros/alx/main.c b/drivers/net/ethernet/atheros/alx/main.c
index 4eb17daefc4f..c0f84b73574d 100644
--- a/drivers/net/ethernet/atheros/alx/main.c
+++ b/drivers/net/ethernet/atheros/alx/main.c
@@ -51,6 +51,9 @@
 
 const char alx_drv_name[] = "alx";
 
+static bool msix = false;
+module_param(msix, bool, 0);
+MODULE_PARM_DESC(msix, "Enable msi-x interrupt support");
 
 static void alx_free_txbuf(struct alx_priv *alx, int entry)
 {
@@ -292,32 +295,29 @@ static int alx_poll(struct napi_struct *napi, int budget)
 	napi_complete(&alx->napi);
 
 	/* enable interrupt */
-	spin_lock_irqsave(&alx->irq_lock, flags);
-	alx->int_mask |= ALX_ISR_TX_Q0 | ALX_ISR_RX_Q0;
-	alx_write_mem32(hw, ALX_IMR, alx->int_mask);
-	spin_unlock_irqrestore(&alx->irq_lock, flags);
+	if (alx->flags & ALX_FLAG_USING_MSIX) {
+		alx_mask_msix(hw, 1, false);
+	} else {
+		spin_lock_irqsave(&alx->irq_lock, flags);
+		alx->int_mask |= ALX_ISR_TX_Q0 | ALX_ISR_RX_Q0;
+		alx_write_mem32(hw, ALX_IMR, alx->int_mask);
+		spin_unlock_irqrestore(&alx->irq_lock, flags);
+	}
 
 	alx_post_write(hw);
 
 	return work;
 }
 
-static irqreturn_t alx_intr_handle(struct alx_priv *alx, u32 intr)
+static bool alx_intr_handle_misc(struct alx_priv *alx, u32 intr)
 {
 	struct alx_hw *hw = &alx->hw;
-	bool write_int_mask = false;
-
-	spin_lock(&alx->irq_lock);
-
-	/* ACK interrupt */
-	alx_write_mem32(hw, ALX_ISR, intr | ALX_ISR_DIS);
-	intr &= alx->int_mask;
 
 	if (intr & ALX_ISR_FATAL) {
 		netif_warn(alx, hw, alx->dev,
 			   "fatal interrupt 0x%x, resetting\n", intr);
 		alx_schedule_reset(alx);
-		goto out;
+		return true;
 	}
 
 	if (intr & ALX_ISR_ALERT)
@@ -329,19 +329,32 @@ static irqreturn_t alx_intr_handle(struct alx_priv *alx, u32 intr)
 		 * is cleared, the interrupt status could be cleared.
 		 */
 		alx->int_mask &= ~ALX_ISR_PHY;
-		write_int_mask = true;
+		alx_write_mem32(hw, ALX_IMR, alx->int_mask);
 		alx_schedule_link_check(alx);
 	}
 
+	return false;
+}
+
+static irqreturn_t alx_intr_handle(struct alx_priv *alx, u32 intr)
+{
+	struct alx_hw *hw = &alx->hw;
+
+	spin_lock(&alx->irq_lock);
+
+	/* ACK interrupt */
+	alx_write_mem32(hw, ALX_ISR, intr | ALX_ISR_DIS);
+	intr &= alx->int_mask;
+
+	if (alx_intr_handle_misc(alx, intr))
+		goto out;
+
 	if (intr & (ALX_ISR_TX_Q0 | ALX_ISR_RX_Q0)) {
 		napi_schedule(&alx->napi);
 		/* mask rx/tx interrupt, enable them when napi complete */
 		alx->int_mask &= ~ALX_ISR_ALL_QUEUES;
-		write_int_mask = true;
-	}
-
-	if (write_int_mask)
 		alx_write_mem32(hw, ALX_IMR, alx->int_mask);
+	}
 
 	alx_write_mem32(hw, ALX_ISR, 0);
 
@@ -350,6 +363,46 @@ static irqreturn_t alx_intr_handle(struct alx_priv *alx, u32 intr)
 	return IRQ_HANDLED;
 }
 
+static irqreturn_t alx_intr_msix_ring(int irq, void *data)
+{
+	struct alx_priv *alx = data;
+	struct alx_hw *hw = &alx->hw;
+
+	/* mask interrupt to ACK chip */
+	alx_mask_msix(hw, 1, true);
+	/* clear interrupt status */
+	alx_write_mem32(hw, ALX_ISR, (ALX_ISR_TX_Q0 | ALX_ISR_RX_Q0));
+
+	napi_schedule(&alx->napi);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t alx_intr_msix_misc(int irq, void *data)
+{
+	struct alx_priv *alx = data;
+	struct alx_hw *hw = &alx->hw;
+	u32 intr;
+
+	/* mask interrupt to ACK chip */
+	alx_mask_msix(hw, 0, true);
+
+	/* read interrupt status */
+	intr = alx_read_mem32(hw, ALX_ISR);
+	intr &= (alx->int_mask & ~ALX_ISR_ALL_QUEUES);
+
+	if (alx_intr_handle_misc(alx, intr))
+		return IRQ_HANDLED;
+
+	/* clear interrupt status */
+	alx_write_mem32(hw, ALX_ISR, intr);
+
+	/* enable interrupt again */
+	alx_mask_msix(hw, 0, false);
+
+	return IRQ_HANDLED;
+}
+
 static irqreturn_t alx_intr_msi(int irq, void *data)
 {
 	struct alx_priv *alx = data;
@@ -614,31 +667,136 @@ static void alx_free_rings(struct alx_priv *alx)
 static void alx_config_vector_mapping(struct alx_priv *alx)
 {
 	struct alx_hw *hw = &alx->hw;
+	u32 tbl = 0;
 
-	alx_write_mem32(hw, ALX_MSI_MAP_TBL1, 0);
+	if (alx->flags & ALX_FLAG_USING_MSIX) {
+		tbl |= 1 << ALX_MSI_MAP_TBL1_TXQ0_SHIFT;
+		tbl |= 1 << ALX_MSI_MAP_TBL1_RXQ0_SHIFT;
+	}
+
+	alx_write_mem32(hw, ALX_MSI_MAP_TBL1, tbl);
 	alx_write_mem32(hw, ALX_MSI_MAP_TBL2, 0);
 	alx_write_mem32(hw, ALX_MSI_ID_MAP, 0);
 }
 
+static bool alx_enable_msix(struct alx_priv *alx)
+{
+	int i, err, num_vec = 2;
+
+	alx->msix_entries = kcalloc(num_vec, sizeof(struct msix_entry),
+				    GFP_KERNEL);
+	if (!alx->msix_entries) {
+		netdev_warn(alx->dev, "Allocation of msix entries failed!\n");
+		return false;
+	}
+
+	for (i = 0; i < num_vec; i++)
+		alx->msix_entries[i].entry = i;
+
+	err = pci_enable_msix(alx->hw.pdev, alx->msix_entries, num_vec);
+	if (err) {
+		kfree(alx->msix_entries);
+		netdev_warn(alx->dev, "Enabling MSI-X interrupts failed!\n");
+		return false;
+	}
+
+	alx->num_vec = num_vec;
+	return true;
+}
+
+static int alx_request_msix(struct alx_priv *alx)
+{
+	struct net_device *netdev = alx->dev;
+	int i, err, vector = 0, free_vector = 0;
+
+	err = request_irq(alx->msix_entries[0].vector, alx_intr_msix_misc,
+			  0, netdev->name, alx);
+	if (err)
+		goto out_err;
+
+	vector++;
+	sprintf(alx->irq_lbl, "%s-TxRx-0", netdev->name);
+
+	err = request_irq(alx->msix_entries[vector].vector,
+			  alx_intr_msix_ring, 0, alx->irq_lbl, alx);
+		if (err)
+			goto out_free;
+
+	return 0;
+
+out_free:
+	free_irq(alx->msix_entries[free_vector++].vector, alx);
+
+	vector--;
+	for (i = 0; i < vector; i++)
+		free_irq(alx->msix_entries[free_vector++].vector, alx);
+
+out_err:
+	return err;
+}
+
+static void alx_init_intr(struct alx_priv *alx, bool msix)
+{
+	if (msix) {
+		if (alx_enable_msix(alx))
+			alx->flags |= ALX_FLAG_USING_MSIX;
+	}
+
+	if (!(alx->flags & ALX_FLAG_USING_MSIX)) {
+		alx->num_vec = 1;
+
+		if (!pci_enable_msi(alx->hw.pdev))
+			alx->flags |= ALX_FLAG_USING_MSI;
+	}
+}
+
+static void alx_disable_advanced_intr(struct alx_priv *alx)
+{
+	if (alx->flags & ALX_FLAG_USING_MSIX) {
+		kfree(alx->msix_entries);
+		pci_disable_msix(alx->hw.pdev);
+		alx->flags &= ~ALX_FLAG_USING_MSIX;
+	}
+
+	if (alx->flags & ALX_FLAG_USING_MSI) {
+		pci_disable_msi(alx->hw.pdev);
+		alx->flags &= ~ALX_FLAG_USING_MSI;
+	}
+}
+
 static void alx_irq_enable(struct alx_priv *alx)
 {
 	struct alx_hw *hw = &alx->hw;
+	int i;
 
 	/* level-1 interrupt switch */
 	alx_write_mem32(hw, ALX_ISR, 0);
 	alx_write_mem32(hw, ALX_IMR, alx->int_mask);
 	alx_post_write(hw);
+
+	if (alx->flags & ALX_FLAG_USING_MSIX)
+		/* enable all msix irqs */
+		for (i = 0; i < alx->num_vec; i++)
+			alx_mask_msix(hw, i, false);
 }
 
 static void alx_irq_disable(struct alx_priv *alx)
 {
 	struct alx_hw *hw = &alx->hw;
+	int i;
 
 	alx_write_mem32(hw, ALX_ISR, ALX_ISR_DIS);
 	alx_write_mem32(hw, ALX_IMR, 0);
 	alx_post_write(hw);
 
-	synchronize_irq(alx->hw.pdev->irq);
+	if (alx->flags & ALX_FLAG_USING_MSIX) {
+		for (i = 0; i < alx->num_vec; i++) {
+			alx_mask_msix(hw, i, true);
+			synchronize_irq(alx->msix_entries[i].vector);
+		}
+	} else {
+		synchronize_irq(alx->hw.pdev->irq);
+	}
 }
 
 static int alx_request_irq(struct alx_priv *alx)
@@ -650,9 +808,18 @@ static int alx_request_irq(struct alx_priv *alx)
 
 	msi_ctrl = (hw->imt >> 1) << ALX_MSI_RETRANS_TM_SHIFT;
 
-	if (!pci_enable_msi(alx->hw.pdev)) {
-		alx->msi = true;
+	if (alx->flags & ALX_FLAG_USING_MSIX) {
+		alx_write_mem32(hw, ALX_MSI_RETRANS_TIMER, msi_ctrl);
+		err = alx_request_msix(alx);
+		if (!err)
+			goto out;
+
+		/* msix request failed, realloc resources */
+		alx_disable_advanced_intr(alx);
+		alx_init_intr(alx, false);
+	}
 
+	if (alx->flags & ALX_FLAG_USING_MSI) {
 		alx_write_mem32(hw, ALX_MSI_RETRANS_TIMER,
 				msi_ctrl | ALX_MSI_MASK_SEL_LINE);
 		err = request_irq(pdev->irq, alx_intr_msi, 0,
@@ -660,6 +827,7 @@ static int alx_request_irq(struct alx_priv *alx)
 		if (!err)
 			goto out;
 		/* fall back to legacy interrupt */
+		alx->flags &= ~ALX_FLAG_USING_MSI;
 		pci_disable_msi(alx->hw.pdev);
 	}
 
@@ -669,19 +837,25 @@ static int alx_request_irq(struct alx_priv *alx)
 out:
 	if (!err)
 		alx_config_vector_mapping(alx);
+	else
+		netdev_err(alx->dev, "IRQ registration failed!\n");
 	return err;
 }
 
 static void alx_free_irq(struct alx_priv *alx)
 {
 	struct pci_dev *pdev = alx->hw.pdev;
+	int i;
 
-	free_irq(pdev->irq, alx);
-
-	if (alx->msi) {
-		pci_disable_msi(alx->hw.pdev);
-		alx->msi = false;
+	if (alx->flags & ALX_FLAG_USING_MSIX) {
+		/* we have only 2 vectors without multi queue support */
+		for (i = 0; i < 2; i++)
+			free_irq(alx->msix_entries[i].vector, alx);
+	} else {
+		free_irq(pdev->irq, alx);
 	}
+
+	alx_disable_advanced_intr(alx);
 }
 
 static int alx_identify_hw(struct alx_priv *alx)
@@ -847,12 +1021,14 @@ static int __alx_open(struct alx_priv *alx, bool resume)
 {
 	int err;
 
+	alx_init_intr(alx, msix);
+
 	if (!resume)
 		netif_carrier_off(alx->dev);
 
 	err = alx_alloc_rings(alx);
 	if (err)
-		return err;
+		goto out_disable_adv_intr;
 
 	alx_configure(alx);
 
@@ -873,6 +1049,8 @@ static int __alx_open(struct alx_priv *alx, bool resume)
 
 out_free_rings:
 	alx_free_rings(alx);
+out_disable_adv_intr:
+	alx_disable_advanced_intr(alx);
 	return err;
 }
 
@@ -993,6 +1171,18 @@ static void alx_reset(struct work_struct *work)
 	rtnl_unlock();
 }
 
+static int alx_tpd_req(struct sk_buff *skb)
+{
+	int num;
+
+	num = skb_shinfo(skb)->nr_frags + 1;
+	/* we need one extra descriptor for LSOv2 */
+	if (skb_is_gso(skb) && skb_shinfo(skb)->gso_type & SKB_GSO_TCPV6)
+		num++;
+
+	return num;
+}
+
 static int alx_tx_csum(struct sk_buff *skb, struct alx_txd *first)
 {
 	u8 cso, css;
@@ -1012,6 +1202,45 @@ static int alx_tx_csum(struct sk_buff *skb, struct alx_txd *first)
 	return 0;
 }
 
+static int alx_tso(struct sk_buff *skb, struct alx_txd *first)
+{
+	int err;
+
+	if (skb->ip_summed != CHECKSUM_PARTIAL)
+		return 0;
+
+	if (!skb_is_gso(skb))
+		return 0;
+
+	err = skb_cow_head(skb, 0);
+	if (err < 0)
+		return err;
+
+	if (skb->protocol == htons(ETH_P_IP)) {
+		struct iphdr *iph = ip_hdr(skb);
+
+		iph->check = 0;
+		tcp_hdr(skb)->check = ~csum_tcpudp_magic(iph->saddr, iph->daddr,
+							 0, IPPROTO_TCP, 0);
+		first->word1 |= 1 << TPD_IPV4_SHIFT;
+	} else if (skb_is_gso_v6(skb)) {
+		ipv6_hdr(skb)->payload_len = 0;
+		tcp_hdr(skb)->check = ~csum_ipv6_magic(&ipv6_hdr(skb)->saddr,
+						       &ipv6_hdr(skb)->daddr,
+						       0, IPPROTO_TCP, 0);
+		/* LSOv2: the first TPD only provides the packet length */
+		first->adrl.l.pkt_len = skb->len;
+		first->word1 |= 1 << TPD_LSO_V2_SHIFT;
+	}
+
+	first->word1 |= 1 << TPD_LSO_EN_SHIFT;
+	first->word1 |= (skb_transport_offset(skb) &
+			 TPD_L4HDROFFSET_MASK) << TPD_L4HDROFFSET_SHIFT;
+	first->word1 |= (skb_shinfo(skb)->gso_size &
+			 TPD_MSS_MASK) << TPD_MSS_SHIFT;
+	return 1;
+}
+
 static int alx_map_tx_skb(struct alx_priv *alx, struct sk_buff *skb)
 {
 	struct alx_tx_queue *txq = &alx->txq;
@@ -1022,6 +1251,16 @@ static int alx_map_tx_skb(struct alx_priv *alx, struct sk_buff *skb)
 	first_tpd = &txq->tpd[txq->write_idx];
 	tpd = first_tpd;
 
+	if (tpd->word1 & (1 << TPD_LSO_V2_SHIFT)) {
+		if (++txq->write_idx == alx->tx_ringsz)
+			txq->write_idx = 0;
+
+		tpd = &txq->tpd[txq->write_idx];
+		tpd->len = first_tpd->len;
+		tpd->vlan_tag = first_tpd->vlan_tag;
+		tpd->word1 = first_tpd->word1;
+	}
+
 	maplen = skb_headlen(skb);
 	dma = dma_map_single(&alx->hw.pdev->dev, skb->data, maplen,
 			     DMA_TO_DEVICE);
@@ -1082,9 +1321,9 @@ static netdev_tx_t alx_start_xmit(struct sk_buff *skb,
 	struct alx_priv *alx = netdev_priv(netdev);
 	struct alx_tx_queue *txq = &alx->txq;
 	struct alx_txd *first;
-	int tpdreq = skb_shinfo(skb)->nr_frags + 1;
+	int tso;
 
-	if (alx_tpd_avail(alx) < tpdreq) {
+	if (alx_tpd_avail(alx) < alx_tpd_req(skb)) {
 		netif_stop_queue(alx->dev);
 		goto drop;
 	}
@@ -1092,7 +1331,10 @@ static netdev_tx_t alx_start_xmit(struct sk_buff *skb,
 	first = &txq->tpd[txq->write_idx];
 	memset(first, 0, sizeof(*first));
 
-	if (alx_tx_csum(skb, first))
+	tso = alx_tso(skb, first);
+	if (tso < 0)
+		goto drop;
+	else if (!tso && alx_tx_csum(skb, first))
 		goto drop;
 
 	if (alx_map_tx_skb(alx, skb) < 0)
@@ -1172,7 +1414,10 @@ static void alx_poll_controller(struct net_device *netdev)
 {
 	struct alx_priv *alx = netdev_priv(netdev);
 
-	if (alx->msi)
+	if (alx->flags & ALX_FLAG_USING_MSIX) {
+		alx_intr_msix_misc(0, alx);
+		alx_intr_msix_ring(0, alx);
+	} else if (alx->flags & ALX_FLAG_USING_MSI)
 		alx_intr_msi(0, alx);
 	else
 		alx_intr_legacy(0, alx);
@@ -1351,7 +1596,10 @@ static int alx_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 		}
 	}
 
-	netdev->hw_features = NETIF_F_SG | NETIF_F_HW_CSUM;
+	netdev->hw_features = NETIF_F_SG |
+			      NETIF_F_HW_CSUM |
+			      NETIF_F_TSO |
+			      NETIF_F_TSO6;
 
 	if (alx_get_perm_macaddr(hw, hw->perm_addr)) {
 		dev_warn(&pdev->dev,
diff --git a/drivers/net/ethernet/broadcom/b44.c b/drivers/net/ethernet/broadcom/b44.c
index 74f0a37c4eb6..17aa33c5567d 100644
--- a/drivers/net/ethernet/broadcom/b44.c
+++ b/drivers/net/ethernet/broadcom/b44.c
@@ -1486,7 +1486,7 @@ static int b44_open(struct net_device *dev)
 	b44_enable_ints(bp);
 
 	if (bp->flags & B44_FLAG_EXTERNAL_PHY)
-		phy_start(bp->phydev);
+		phy_start(dev->phydev);
 
 	netif_start_queue(dev);
 out:
@@ -1651,7 +1651,7 @@ static int b44_close(struct net_device *dev)
 	netif_stop_queue(dev);
 
 	if (bp->flags & B44_FLAG_EXTERNAL_PHY)
-		phy_stop(bp->phydev);
+		phy_stop(dev->phydev);
 
 	napi_disable(&bp->napi);
 
@@ -1832,90 +1832,100 @@ static int b44_nway_reset(struct net_device *dev)
 	return r;
 }
 
-static int b44_get_settings(struct net_device *dev, struct ethtool_cmd *cmd)
+static int b44_get_link_ksettings(struct net_device *dev,
+				  struct ethtool_link_ksettings *cmd)
 {
 	struct b44 *bp = netdev_priv(dev);
+	u32 supported, advertising;
 
 	if (bp->flags & B44_FLAG_EXTERNAL_PHY) {
-		BUG_ON(!bp->phydev);
-		return phy_ethtool_gset(bp->phydev, cmd);
+		BUG_ON(!dev->phydev);
+		return phy_ethtool_ksettings_get(dev->phydev, cmd);
 	}
 
-	cmd->supported = (SUPPORTED_Autoneg);
-	cmd->supported |= (SUPPORTED_100baseT_Half |
-			  SUPPORTED_100baseT_Full |
-			  SUPPORTED_10baseT_Half |
-			  SUPPORTED_10baseT_Full |
-			  SUPPORTED_MII);
+	supported = (SUPPORTED_Autoneg);
+	supported |= (SUPPORTED_100baseT_Half |
+		      SUPPORTED_100baseT_Full |
+		      SUPPORTED_10baseT_Half |
+		      SUPPORTED_10baseT_Full |
+		      SUPPORTED_MII);
 
-	cmd->advertising = 0;
+	advertising = 0;
 	if (bp->flags & B44_FLAG_ADV_10HALF)
-		cmd->advertising |= ADVERTISED_10baseT_Half;
+		advertising |= ADVERTISED_10baseT_Half;
 	if (bp->flags & B44_FLAG_ADV_10FULL)
-		cmd->advertising |= ADVERTISED_10baseT_Full;
+		advertising |= ADVERTISED_10baseT_Full;
 	if (bp->flags & B44_FLAG_ADV_100HALF)
-		cmd->advertising |= ADVERTISED_100baseT_Half;
+		advertising |= ADVERTISED_100baseT_Half;
 	if (bp->flags & B44_FLAG_ADV_100FULL)
-		cmd->advertising |= ADVERTISED_100baseT_Full;
-	cmd->advertising |= ADVERTISED_Pause | ADVERTISED_Asym_Pause;
-	ethtool_cmd_speed_set(cmd, ((bp->flags & B44_FLAG_100_BASE_T) ?
-				    SPEED_100 : SPEED_10));
-	cmd->duplex = (bp->flags & B44_FLAG_FULL_DUPLEX) ?
+		advertising |= ADVERTISED_100baseT_Full;
+	advertising |= ADVERTISED_Pause | ADVERTISED_Asym_Pause;
+	cmd->base.speed = (bp->flags & B44_FLAG_100_BASE_T) ?
+		SPEED_100 : SPEED_10;
+	cmd->base.duplex = (bp->flags & B44_FLAG_FULL_DUPLEX) ?
 		DUPLEX_FULL : DUPLEX_HALF;
-	cmd->port = 0;
-	cmd->phy_address = bp->phy_addr;
-	cmd->transceiver = (bp->flags & B44_FLAG_EXTERNAL_PHY) ?
-		XCVR_EXTERNAL : XCVR_INTERNAL;
-	cmd->autoneg = (bp->flags & B44_FLAG_FORCE_LINK) ?
+	cmd->base.port = 0;
+	cmd->base.phy_address = bp->phy_addr;
+	cmd->base.autoneg = (bp->flags & B44_FLAG_FORCE_LINK) ?
 		AUTONEG_DISABLE : AUTONEG_ENABLE;
-	if (cmd->autoneg == AUTONEG_ENABLE)
-		cmd->advertising |= ADVERTISED_Autoneg;
+	if (cmd->base.autoneg == AUTONEG_ENABLE)
+		advertising |= ADVERTISED_Autoneg;
+
+	ethtool_convert_legacy_u32_to_link_mode(cmd->link_modes.supported,
+						supported);
+	ethtool_convert_legacy_u32_to_link_mode(cmd->link_modes.advertising,
+						advertising);
+
 	if (!netif_running(dev)){
-		ethtool_cmd_speed_set(cmd, 0);
-		cmd->duplex = 0xff;
+		cmd->base.speed = 0;
+		cmd->base.duplex = 0xff;
 	}
-	cmd->maxtxpkt = 0;
-	cmd->maxrxpkt = 0;
+
 	return 0;
 }
 
-static int b44_set_settings(struct net_device *dev, struct ethtool_cmd *cmd)
+static int b44_set_link_ksettings(struct net_device *dev,
+				  const struct ethtool_link_ksettings *cmd)
 {
 	struct b44 *bp = netdev_priv(dev);
 	u32 speed;
 	int ret;
+	u32 advertising;
 
 	if (bp->flags & B44_FLAG_EXTERNAL_PHY) {
-		BUG_ON(!bp->phydev);
+		BUG_ON(!dev->phydev);
 		spin_lock_irq(&bp->lock);
 		if (netif_running(dev))
 			b44_setup_phy(bp);
 
-		ret = phy_ethtool_sset(bp->phydev, cmd);
+		ret = phy_ethtool_ksettings_set(dev->phydev, cmd);
 
 		spin_unlock_irq(&bp->lock);
 
 		return ret;
 	}
 
-	speed = ethtool_cmd_speed(cmd);
+	speed = cmd->base.speed;
+
+	ethtool_convert_link_mode_to_legacy_u32(&advertising,
+						cmd->link_modes.advertising);
 
 	/* We do not support gigabit. */
-	if (cmd->autoneg == AUTONEG_ENABLE) {
-		if (cmd->advertising &
+	if (cmd->base.autoneg == AUTONEG_ENABLE) {
+		if (advertising &
 		    (ADVERTISED_1000baseT_Half |
 		     ADVERTISED_1000baseT_Full))
 			return -EINVAL;
 	} else if ((speed != SPEED_100 &&
 		    speed != SPEED_10) ||
-		   (cmd->duplex != DUPLEX_HALF &&
-		    cmd->duplex != DUPLEX_FULL)) {
+		   (cmd->base.duplex != DUPLEX_HALF &&
+		    cmd->base.duplex != DUPLEX_FULL)) {
 			return -EINVAL;
 	}
 
 	spin_lock_irq(&bp->lock);
 
-	if (cmd->autoneg == AUTONEG_ENABLE) {
+	if (cmd->base.autoneg == AUTONEG_ENABLE) {
 		bp->flags &= ~(B44_FLAG_FORCE_LINK |
 			       B44_FLAG_100_BASE_T |
 			       B44_FLAG_FULL_DUPLEX |
@@ -1923,19 +1933,19 @@ static int b44_set_settings(struct net_device *dev, struct ethtool_cmd *cmd)
 			       B44_FLAG_ADV_10FULL |
 			       B44_FLAG_ADV_100HALF |
 			       B44_FLAG_ADV_100FULL);
-		if (cmd->advertising == 0) {
+		if (advertising == 0) {
 			bp->flags |= (B44_FLAG_ADV_10HALF |
 				      B44_FLAG_ADV_10FULL |
 				      B44_FLAG_ADV_100HALF |
 				      B44_FLAG_ADV_100FULL);
 		} else {
-			if (cmd->advertising & ADVERTISED_10baseT_Half)
+			if (advertising & ADVERTISED_10baseT_Half)
 				bp->flags |= B44_FLAG_ADV_10HALF;
-			if (cmd->advertising & ADVERTISED_10baseT_Full)
+			if (advertising & ADVERTISED_10baseT_Full)
 				bp->flags |= B44_FLAG_ADV_10FULL;
-			if (cmd->advertising & ADVERTISED_100baseT_Half)
+			if (advertising & ADVERTISED_100baseT_Half)
 				bp->flags |= B44_FLAG_ADV_100HALF;
-			if (cmd->advertising & ADVERTISED_100baseT_Full)
+			if (advertising & ADVERTISED_100baseT_Full)
 				bp->flags |= B44_FLAG_ADV_100FULL;
 		}
 	} else {
@@ -1943,7 +1953,7 @@ static int b44_set_settings(struct net_device *dev, struct ethtool_cmd *cmd)
 		bp->flags &= ~(B44_FLAG_100_BASE_T | B44_FLAG_FULL_DUPLEX);
 		if (speed == SPEED_100)
 			bp->flags |= B44_FLAG_100_BASE_T;
-		if (cmd->duplex == DUPLEX_FULL)
+		if (cmd->base.duplex == DUPLEX_FULL)
 			bp->flags |= B44_FLAG_FULL_DUPLEX;
 	}
 
@@ -2110,8 +2120,6 @@ static int b44_set_wol(struct net_device *dev, struct ethtool_wolinfo *wol)
 
 static const struct ethtool_ops b44_ethtool_ops = {
 	.get_drvinfo		= b44_get_drvinfo,
-	.get_settings		= b44_get_settings,
-	.set_settings		= b44_set_settings,
 	.nway_reset		= b44_nway_reset,
 	.get_link		= ethtool_op_get_link,
 	.get_wol		= b44_get_wol,
@@ -2125,6 +2133,8 @@ static const struct ethtool_ops b44_ethtool_ops = {
 	.get_strings		= b44_get_strings,
 	.get_sset_count		= b44_get_sset_count,
 	.get_ethtool_stats	= b44_get_ethtool_stats,
+	.get_link_ksettings	= b44_get_link_ksettings,
+	.set_link_ksettings	= b44_set_link_ksettings,
 };
 
 static int b44_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
@@ -2137,8 +2147,8 @@ static int b44_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
 
 	spin_lock_irq(&bp->lock);
 	if (bp->flags & B44_FLAG_EXTERNAL_PHY) {
-		BUG_ON(!bp->phydev);
-		err = phy_mii_ioctl(bp->phydev, ifr, cmd);
+		BUG_ON(!dev->phydev);
+		err = phy_mii_ioctl(dev->phydev, ifr, cmd);
 	} else {
 		err = generic_mii_ioctl(&bp->mii_if, if_mii(ifr), cmd, NULL);
 	}
@@ -2206,7 +2216,7 @@ static const struct net_device_ops b44_netdev_ops = {
 static void b44_adjust_link(struct net_device *dev)
 {
 	struct b44 *bp = netdev_priv(dev);
-	struct phy_device *phydev = bp->phydev;
+	struct phy_device *phydev = dev->phydev;
 	bool status_changed = 0;
 
 	BUG_ON(!phydev);
@@ -2303,7 +2313,6 @@ static int b44_register_phy_one(struct b44 *bp)
 			      SUPPORTED_MII);
 	phydev->advertising = phydev->supported;
 
-	bp->phydev = phydev;
 	bp->old_link = 0;
 	bp->phy_addr = phydev->mdio.addr;
 
@@ -2323,9 +2332,10 @@ err_out:
 
 static void b44_unregister_phy_one(struct b44 *bp)
 {
+	struct net_device *dev = bp->dev;
 	struct mii_bus *mii_bus = bp->mii_bus;
 
-	phy_disconnect(bp->phydev);
+	phy_disconnect(dev->phydev);
 	mdiobus_unregister(mii_bus);
 	mdiobus_free(mii_bus);
 }
diff --git a/drivers/net/ethernet/broadcom/b44.h b/drivers/net/ethernet/broadcom/b44.h
index 65d88d7c5581..89d2cf341163 100644
--- a/drivers/net/ethernet/broadcom/b44.h
+++ b/drivers/net/ethernet/broadcom/b44.h
@@ -404,7 +404,6 @@ struct b44 {
 	u32			tx_pending;
 	u8			phy_addr;
 	u8			force_copybreak;
-	struct phy_device	*phydev;
 	struct mii_bus		*mii_bus;
 	int			old_link;
 	struct mii_if_info	mii_if;
diff --git a/drivers/net/ethernet/broadcom/bcm63xx_enet.c b/drivers/net/ethernet/broadcom/bcm63xx_enet.c
index 6c8bc5fadac7..ae364c74baf3 100644
--- a/drivers/net/ethernet/broadcom/bcm63xx_enet.c
+++ b/drivers/net/ethernet/broadcom/bcm63xx_enet.c
@@ -791,7 +791,7 @@ static void bcm_enet_adjust_phy_link(struct net_device *dev)
 	int status_changed;
 
 	priv = netdev_priv(dev);
-	phydev = priv->phydev;
+	phydev = dev->phydev;
 	status_changed = 0;
 
 	if (priv->old_link != phydev->link) {
@@ -913,7 +913,6 @@ static int bcm_enet_open(struct net_device *dev)
 		priv->old_link = 0;
 		priv->old_duplex = -1;
 		priv->old_pause = -1;
-		priv->phydev = phydev;
 	}
 
 	/* mask all interrupts and request them */
@@ -1085,7 +1084,7 @@ static int bcm_enet_open(struct net_device *dev)
 			 ENETDMAC_IRMASK, priv->tx_chan);
 
 	if (priv->has_phy)
-		phy_start(priv->phydev);
+		phy_start(phydev);
 	else
 		bcm_enet_adjust_link(dev);
 
@@ -1127,7 +1126,7 @@ out_freeirq:
 	free_irq(dev->irq, dev);
 
 out_phy_disconnect:
-	phy_disconnect(priv->phydev);
+	phy_disconnect(phydev);
 
 	return ret;
 }
@@ -1190,7 +1189,7 @@ static int bcm_enet_stop(struct net_device *dev)
 	netif_stop_queue(dev);
 	napi_disable(&priv->napi);
 	if (priv->has_phy)
-		phy_stop(priv->phydev);
+		phy_stop(dev->phydev);
 	del_timer_sync(&priv->rx_timeout);
 
 	/* mask all interrupts */
@@ -1234,10 +1233,8 @@ static int bcm_enet_stop(struct net_device *dev)
 	free_irq(dev->irq, dev);
 
 	/* release phy */
-	if (priv->has_phy) {
-		phy_disconnect(priv->phydev);
-		priv->phydev = NULL;
-	}
+	if (priv->has_phy)
+		phy_disconnect(dev->phydev);
 
 	return 0;
 }
@@ -1437,64 +1434,68 @@ static int bcm_enet_nway_reset(struct net_device *dev)
 
 	priv = netdev_priv(dev);
 	if (priv->has_phy) {
-		if (!priv->phydev)
+		if (!dev->phydev)
 			return -ENODEV;
-		return genphy_restart_aneg(priv->phydev);
+		return genphy_restart_aneg(dev->phydev);
 	}
 
 	return -EOPNOTSUPP;
 }
 
-static int bcm_enet_get_settings(struct net_device *dev,
-				 struct ethtool_cmd *cmd)
+static int bcm_enet_get_link_ksettings(struct net_device *dev,
+				       struct ethtool_link_ksettings *cmd)
 {
 	struct bcm_enet_priv *priv;
+	u32 supported, advertising;
 
 	priv = netdev_priv(dev);
 
-	cmd->maxrxpkt = 0;
-	cmd->maxtxpkt = 0;
-
 	if (priv->has_phy) {
-		if (!priv->phydev)
+		if (!dev->phydev)
 			return -ENODEV;
-		return phy_ethtool_gset(priv->phydev, cmd);
+		return phy_ethtool_ksettings_get(dev->phydev, cmd);
 	} else {
-		cmd->autoneg = 0;
-		ethtool_cmd_speed_set(cmd, ((priv->force_speed_100)
-					    ? SPEED_100 : SPEED_10));
-		cmd->duplex = (priv->force_duplex_full) ?
+		cmd->base.autoneg = 0;
+		cmd->base.speed = (priv->force_speed_100) ?
+			SPEED_100 : SPEED_10;
+		cmd->base.duplex = (priv->force_duplex_full) ?
 			DUPLEX_FULL : DUPLEX_HALF;
-		cmd->supported = ADVERTISED_10baseT_Half  |
+		supported = ADVERTISED_10baseT_Half |
 			ADVERTISED_10baseT_Full |
 			ADVERTISED_100baseT_Half |
 			ADVERTISED_100baseT_Full;
-		cmd->advertising = 0;
-		cmd->port = PORT_MII;
-		cmd->transceiver = XCVR_EXTERNAL;
+		advertising = 0;
+		ethtool_convert_legacy_u32_to_link_mode(
+			cmd->link_modes.supported, supported);
+		ethtool_convert_legacy_u32_to_link_mode(
+			cmd->link_modes.advertising, advertising);
+		cmd->base.port = PORT_MII;
 	}
 	return 0;
 }
 
-static int bcm_enet_set_settings(struct net_device *dev,
-				 struct ethtool_cmd *cmd)
+static int bcm_enet_set_link_ksettings(struct net_device *dev,
+				       const struct ethtool_link_ksettings *cmd)
 {
 	struct bcm_enet_priv *priv;
 
 	priv = netdev_priv(dev);
 	if (priv->has_phy) {
-		if (!priv->phydev)
+		if (!dev->phydev)
 			return -ENODEV;
-		return phy_ethtool_sset(priv->phydev, cmd);
+		return phy_ethtool_ksettings_set(dev->phydev, cmd);
 	} else {
 
-		if (cmd->autoneg ||
-		    (cmd->speed != SPEED_100 && cmd->speed != SPEED_10) ||
-		    cmd->port != PORT_MII)
+		if (cmd->base.autoneg ||
+		    (cmd->base.speed != SPEED_100 &&
+		     cmd->base.speed != SPEED_10) ||
+		    cmd->base.port != PORT_MII)
 			return -EINVAL;
 
-		priv->force_speed_100 = (cmd->speed == SPEED_100) ? 1 : 0;
-		priv->force_duplex_full = (cmd->duplex == DUPLEX_FULL) ? 1 : 0;
+		priv->force_speed_100 =
+			(cmd->base.speed == SPEED_100) ? 1 : 0;
+		priv->force_duplex_full =
+			(cmd->base.duplex == DUPLEX_FULL) ? 1 : 0;
 
 		if (netif_running(dev))
 			bcm_enet_adjust_link(dev);
@@ -1588,14 +1589,14 @@ static const struct ethtool_ops bcm_enet_ethtool_ops = {
 	.get_sset_count		= bcm_enet_get_sset_count,
 	.get_ethtool_stats      = bcm_enet_get_ethtool_stats,
 	.nway_reset		= bcm_enet_nway_reset,
-	.get_settings		= bcm_enet_get_settings,
-	.set_settings		= bcm_enet_set_settings,
 	.get_drvinfo		= bcm_enet_get_drvinfo,
 	.get_link		= ethtool_op_get_link,
 	.get_ringparam		= bcm_enet_get_ringparam,
 	.set_ringparam		= bcm_enet_set_ringparam,
 	.get_pauseparam		= bcm_enet_get_pauseparam,
 	.set_pauseparam		= bcm_enet_set_pauseparam,
+	.get_link_ksettings	= bcm_enet_get_link_ksettings,
+	.set_link_ksettings	= bcm_enet_set_link_ksettings,
 };
 
 static int bcm_enet_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
@@ -1604,9 +1605,9 @@ static int bcm_enet_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
 
 	priv = netdev_priv(dev);
 	if (priv->has_phy) {
-		if (!priv->phydev)
+		if (!dev->phydev)
 			return -ENODEV;
-		return phy_mii_ioctl(priv->phydev, rq, cmd);
+		return phy_mii_ioctl(dev->phydev, rq, cmd);
 	} else {
 		struct mii_if_info mii;
 
diff --git a/drivers/net/ethernet/broadcom/bcm63xx_enet.h b/drivers/net/ethernet/broadcom/bcm63xx_enet.h
index f55af4310085..0a1b7b2e55bd 100644
--- a/drivers/net/ethernet/broadcom/bcm63xx_enet.h
+++ b/drivers/net/ethernet/broadcom/bcm63xx_enet.h
@@ -290,7 +290,6 @@ struct bcm_enet_priv {
 
 	/* used when a phy is connected (phylib used) */
 	struct mii_bus *mii_bus;
-	struct phy_device *phydev;
 	int old_link;
 	int old_duplex;
 	int old_pause;
diff --git a/drivers/net/ethernet/broadcom/bcmsysport.c b/drivers/net/ethernet/broadcom/bcmsysport.c
index b2d30863caeb..c3354b9941d1 100644
--- a/drivers/net/ethernet/broadcom/bcmsysport.c
+++ b/drivers/net/ethernet/broadcom/bcmsysport.c
@@ -58,8 +58,8 @@ BCM_SYSPORT_IO_MACRO(topctrl, SYS_PORT_TOPCTRL_OFFSET);
 static inline void intrl2_##which##_mask_clear(struct bcm_sysport_priv *priv, \
 						u32 mask)		\
 {									\
-	intrl2_##which##_writel(priv, mask, INTRL2_CPU_MASK_CLEAR);	\
 	priv->irq##which##_mask &= ~(mask);				\
+	intrl2_##which##_writel(priv, mask, INTRL2_CPU_MASK_CLEAR);	\
 }									\
 static inline void intrl2_##which##_mask_set(struct bcm_sysport_priv *priv, \
 						u32 mask)		\
@@ -1692,7 +1692,7 @@ static int bcm_sysport_stop(struct net_device *dev)
 	return 0;
 }
 
-static struct ethtool_ops bcm_sysport_ethtool_ops = {
+static const struct ethtool_ops bcm_sysport_ethtool_ops = {
 	.get_drvinfo		= bcm_sysport_get_drvinfo,
 	.get_msglevel		= bcm_sysport_get_msglvl,
 	.set_msglevel		= bcm_sysport_set_msglvl,
diff --git a/drivers/net/ethernet/broadcom/bgmac-bcma.c b/drivers/net/ethernet/broadcom/bgmac-bcma.c
index 625235db644f..c16ec3a51876 100644
--- a/drivers/net/ethernet/broadcom/bgmac-bcma.c
+++ b/drivers/net/ethernet/broadcom/bgmac-bcma.c
@@ -92,6 +92,7 @@ MODULE_DEVICE_TABLE(bcma, bgmac_bcma_tbl);
 /* http://bcm-v4.sipsolutions.net/mac-gbit/gmac/chipattach */
 static int bgmac_probe(struct bcma_device *core)
 {
+	struct bcma_chipinfo *ci = &core->bus->chipinfo;
 	struct ssb_sprom *sprom = &core->bus->sprom;
 	struct mii_bus *mii_bus;
 	struct bgmac *bgmac;
@@ -157,7 +158,8 @@ static int bgmac_probe(struct bcma_device *core)
 	dev_info(bgmac->dev, "Found PHY addr: %d%s\n", bgmac->phyaddr,
 		 bgmac->phyaddr == BGMAC_PHY_NOREGS ? " (NOREGS)" : "");
 
-	if (!bgmac_is_bcm4707_family(core)) {
+	if (!bgmac_is_bcm4707_family(core) &&
+	    !(ci->id == BCMA_CHIP_ID_BCM53573 && core->core_unit == 1)) {
 		mii_bus = bcma_mdio_mii_register(core, bgmac->phyaddr);
 		if (IS_ERR(mii_bus)) {
 			err = PTR_ERR(mii_bus);
@@ -230,6 +232,21 @@ static int bgmac_probe(struct bcma_device *core)
 		bgmac->feature_flags |= BGMAC_FEAT_NO_RESET;
 		bgmac->feature_flags |= BGMAC_FEAT_FORCE_SPEED_2500;
 		break;
+	case BCMA_CHIP_ID_BCM53573:
+		bgmac->feature_flags |= BGMAC_FEAT_CLKCTLST;
+		bgmac->feature_flags |= BGMAC_FEAT_SET_RXQ_CLK;
+		if (ci->pkg == BCMA_PKG_ID_BCM47189)
+			bgmac->feature_flags |= BGMAC_FEAT_IOST_ATTACHED;
+		if (core->core_unit == 0) {
+			bgmac->feature_flags |= BGMAC_FEAT_CC4_IF_SW_TYPE;
+			if (ci->pkg == BCMA_PKG_ID_BCM47189)
+				bgmac->feature_flags |=
+					BGMAC_FEAT_CC4_IF_SW_TYPE_RGMII;
+		} else if (core->core_unit == 1) {
+			bgmac->feature_flags |= BGMAC_FEAT_IRQ_ID_OOB_6;
+			bgmac->feature_flags |= BGMAC_FEAT_CC7_IF_TYPE_RGMII;
+		}
+		break;
 	default:
 		bgmac->feature_flags |= BGMAC_FEAT_CLKCTLST;
 		bgmac->feature_flags |= BGMAC_FEAT_SET_RXQ_CLK;
diff --git a/drivers/net/ethernet/broadcom/bgmac.c b/drivers/net/ethernet/broadcom/bgmac.c
index c4751ece76f6..856379cbb402 100644
--- a/drivers/net/ethernet/broadcom/bgmac.c
+++ b/drivers/net/ethernet/broadcom/bgmac.c
@@ -932,7 +932,8 @@ static void bgmac_chip_reset(struct bgmac *bgmac)
 			et_swtype <<= 4;
 			sw_type = et_swtype;
 		} else if (bgmac->feature_flags & BGMAC_FEAT_SW_TYPE_EPHYRMII) {
-			sw_type = BGMAC_CHIPCTL_1_SW_TYPE_EPHYRMII;
+			sw_type = BGMAC_CHIPCTL_1_IF_TYPE_RMII |
+				  BGMAC_CHIPCTL_1_SW_TYPE_EPHYRMII;
 		} else if (bgmac->feature_flags & BGMAC_FEAT_SW_TYPE_RGMII) {
 			sw_type = BGMAC_CHIPCTL_1_IF_TYPE_RGMII |
 				  BGMAC_CHIPCTL_1_SW_TYPE_RGMII;
@@ -940,6 +941,27 @@ static void bgmac_chip_reset(struct bgmac *bgmac)
 		bgmac_cco_ctl_maskset(bgmac, 1, ~(BGMAC_CHIPCTL_1_IF_TYPE_MASK |
 						  BGMAC_CHIPCTL_1_SW_TYPE_MASK),
 				      sw_type);
+	} else if (bgmac->feature_flags & BGMAC_FEAT_CC4_IF_SW_TYPE) {
+		u32 sw_type = BGMAC_CHIPCTL_4_IF_TYPE_MII |
+			      BGMAC_CHIPCTL_4_SW_TYPE_EPHY;
+		u8 et_swtype = 0;
+		char buf[4];
+
+		if (bcm47xx_nvram_getenv("et_swtype", buf, sizeof(buf)) > 0) {
+			if (kstrtou8(buf, 0, &et_swtype))
+				dev_err(bgmac->dev, "Failed to parse et_swtype (%s)\n",
+					buf);
+			sw_type = (et_swtype & 0x0f) << 12;
+		} else if (bgmac->feature_flags & BGMAC_FEAT_CC4_IF_SW_TYPE_RGMII) {
+			sw_type = BGMAC_CHIPCTL_4_IF_TYPE_RGMII |
+				  BGMAC_CHIPCTL_4_SW_TYPE_RGMII;
+		}
+		bgmac_cco_ctl_maskset(bgmac, 4, ~(BGMAC_CHIPCTL_4_IF_TYPE_MASK |
+						  BGMAC_CHIPCTL_4_SW_TYPE_MASK),
+				      sw_type);
+	} else if (bgmac->feature_flags & BGMAC_FEAT_CC7_IF_TYPE_RGMII) {
+		bgmac_cco_ctl_maskset(bgmac, 7, ~BGMAC_CHIPCTL_7_IF_TYPE_MASK,
+				      BGMAC_CHIPCTL_7_IF_TYPE_RGMII);
 	}
 
 	if (iost & BGMAC_BCMA_IOST_ATTACHED && !bgmac->has_robosw)
@@ -1024,7 +1046,7 @@ static void bgmac_enable(struct bgmac *bgmac)
 
 	mode = (bgmac_read(bgmac, BGMAC_DEV_STATUS) & BGMAC_DS_MM_MASK) >>
 		BGMAC_DS_MM_SHIFT;
-	if (bgmac->feature_flags & BGMAC_FEAT_CLKCTLST || mode != 0)
+	if (!(bgmac->feature_flags & BGMAC_FEAT_CLKCTLST) || mode != 0)
 		bgmac_set(bgmac, BCMA_CLKCTLST, BCMA_CLKCTLST_FORCEHT);
 	if (bgmac->feature_flags & BGMAC_FEAT_CLKCTLST && mode == 2)
 		bgmac_cco_ctl_maskset(bgmac, 1, ~0,
@@ -1467,6 +1489,10 @@ int bgmac_enet_probe(struct bgmac *info)
 	 */
 	bgmac_clk_enable(bgmac, 0);
 
+	/* This seems to be fixing IRQ by assigning OOB #6 to the core */
+	if (bgmac->feature_flags & BGMAC_FEAT_IRQ_ID_OOB_6)
+		bgmac_idm_write(bgmac, BCMA_OOB_SEL_OUT_A30, 0x86);
+
 	bgmac_chip_reset(bgmac);
 
 	err = bgmac_dma_alloc(bgmac);
diff --git a/drivers/net/ethernet/broadcom/bgmac.h b/drivers/net/ethernet/broadcom/bgmac.h
index 24a250267b88..80836b4c9f38 100644
--- a/drivers/net/ethernet/broadcom/bgmac.h
+++ b/drivers/net/ethernet/broadcom/bgmac.h
@@ -369,6 +369,21 @@
 #define BGMAC_CHIPCTL_1_SW_TYPE_RGMII		0x000000C0
 #define BGMAC_CHIPCTL_1_RXC_DLL_BYPASS		0x00010000
 
+#define BGMAC_CHIPCTL_4_IF_TYPE_MASK		0x00003000
+#define BGMAC_CHIPCTL_4_IF_TYPE_RMII		0x00000000
+#define BGMAC_CHIPCTL_4_IF_TYPE_MII		0x00001000
+#define BGMAC_CHIPCTL_4_IF_TYPE_RGMII		0x00002000
+#define BGMAC_CHIPCTL_4_SW_TYPE_MASK		0x0000C000
+#define BGMAC_CHIPCTL_4_SW_TYPE_EPHY		0x00000000
+#define BGMAC_CHIPCTL_4_SW_TYPE_EPHYMII		0x00004000
+#define BGMAC_CHIPCTL_4_SW_TYPE_EPHYRMII	0x00008000
+#define BGMAC_CHIPCTL_4_SW_TYPE_RGMII		0x0000C000
+
+#define BGMAC_CHIPCTL_7_IF_TYPE_MASK		0x000000C0
+#define BGMAC_CHIPCTL_7_IF_TYPE_RMII		0x00000000
+#define BGMAC_CHIPCTL_7_IF_TYPE_MII		0x00000040
+#define BGMAC_CHIPCTL_7_IF_TYPE_RGMII		0x00000080
+
 #define BGMAC_WEIGHT	64
 
 #define ETHER_MAX_LEN   1518
@@ -390,6 +405,10 @@
 #define BGMAC_FEAT_NO_CLR_MIB		BIT(13)
 #define BGMAC_FEAT_FORCE_SPEED_2500	BIT(14)
 #define BGMAC_FEAT_CMDCFG_SR_REV4	BIT(15)
+#define BGMAC_FEAT_IRQ_ID_OOB_6		BIT(16)
+#define BGMAC_FEAT_CC4_IF_SW_TYPE	BIT(17)
+#define BGMAC_FEAT_CC4_IF_SW_TYPE_RGMII	BIT(18)
+#define BGMAC_FEAT_CC7_IF_TYPE_RGMII	BIT(19)
 
 struct bgmac_slot_info {
 	union {
diff --git a/drivers/net/ethernet/broadcom/bnx2.c b/drivers/net/ethernet/broadcom/bnx2.c
index 505ceaf451e2..27f11a5d5fe2 100644
--- a/drivers/net/ethernet/broadcom/bnx2.c
+++ b/drivers/net/ethernet/broadcom/bnx2.c
@@ -50,7 +50,7 @@
 #include <linux/log2.h>
 #include <linux/aer.h>
 
-#if defined(CONFIG_CNIC) || defined(CONFIG_CNIC_MODULE)
+#if IS_ENABLED(CONFIG_CNIC)
 #define BCM_CNIC 1
 #include "cnic_if.h"
 #endif
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.h
index 0e68fadecfdb..243cb9748d35 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.h
@@ -492,7 +492,8 @@ int __bnx2x_setup_tc(struct net_device *dev, u32 handle, __be16 proto,
 int bnx2x_get_vf_config(struct net_device *dev, int vf,
 			struct ifla_vf_info *ivi);
 int bnx2x_set_vf_mac(struct net_device *dev, int queue, u8 *mac);
-int bnx2x_set_vf_vlan(struct net_device *netdev, int vf, u16 vlan, u8 qos);
+int bnx2x_set_vf_vlan(struct net_device *netdev, int vf, u16 vlan, u8 qos,
+		      __be16 vlan_proto);
 
 /* select_queue callback */
 u16 bnx2x_select_queue(struct net_device *dev, struct sk_buff *skb,
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c
index fa3386bb14f7..20fe6a8c35c1 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c
@@ -12563,41 +12563,64 @@ static int bnx2x_close(struct net_device *dev)
 	return 0;
 }
 
+struct bnx2x_mcast_list_elem_group
+{
+	struct list_head mcast_group_link;
+	struct bnx2x_mcast_list_elem mcast_elems[];
+};
+
+#define MCAST_ELEMS_PER_PG \
+	((PAGE_SIZE - sizeof(struct bnx2x_mcast_list_elem_group)) / \
+	sizeof(struct bnx2x_mcast_list_elem))
+
+static void bnx2x_free_mcast_macs_list(struct list_head *mcast_group_list)
+{
+	struct bnx2x_mcast_list_elem_group *current_mcast_group;
+
+	while (!list_empty(mcast_group_list)) {
+		current_mcast_group = list_first_entry(mcast_group_list,
+				      struct bnx2x_mcast_list_elem_group,
+				      mcast_group_link);
+		list_del(&current_mcast_group->mcast_group_link);
+		free_page((unsigned long)current_mcast_group);
+	}
+}
+
 static int bnx2x_init_mcast_macs_list(struct bnx2x *bp,
-				      struct bnx2x_mcast_ramrod_params *p)
+				      struct bnx2x_mcast_ramrod_params *p,
+				      struct list_head *mcast_group_list)
 {
-	int mc_count = netdev_mc_count(bp->dev);
-	struct bnx2x_mcast_list_elem *mc_mac =
-		kcalloc(mc_count, sizeof(*mc_mac), GFP_ATOMIC);
+	struct bnx2x_mcast_list_elem *mc_mac;
 	struct netdev_hw_addr *ha;
-
-	if (!mc_mac)
-		return -ENOMEM;
+	struct bnx2x_mcast_list_elem_group *current_mcast_group = NULL;
+	int mc_count = netdev_mc_count(bp->dev);
+	int offset = 0;
 
 	INIT_LIST_HEAD(&p->mcast_list);
-
 	netdev_for_each_mc_addr(ha, bp->dev) {
+		if (!offset) {
+			current_mcast_group =
+				(struct bnx2x_mcast_list_elem_group *)
+				__get_free_page(GFP_ATOMIC);
+			if (!current_mcast_group) {
+				bnx2x_free_mcast_macs_list(mcast_group_list);
+				BNX2X_ERR("Failed to allocate mc MAC list\n");
+				return -ENOMEM;
+			}
+			list_add(&current_mcast_group->mcast_group_link,
+				 mcast_group_list);
+		}
+		mc_mac = &current_mcast_group->mcast_elems[offset];
 		mc_mac->mac = bnx2x_mc_addr(ha);
 		list_add_tail(&mc_mac->link, &p->mcast_list);
-		mc_mac++;
+		offset++;
+		if (offset == MCAST_ELEMS_PER_PG)
+			offset = 0;
 	}
-
 	p->mcast_list_len = mc_count;
-
 	return 0;
 }
 
-static void bnx2x_free_mcast_macs_list(
-	struct bnx2x_mcast_ramrod_params *p)
-{
-	struct bnx2x_mcast_list_elem *mc_mac =
-		list_first_entry(&p->mcast_list, struct bnx2x_mcast_list_elem,
-				 link);
-
-	WARN_ON(!mc_mac);
-	kfree(mc_mac);
-}
-
 /**
  * bnx2x_set_uc_list - configure a new unicast MACs list.
  *
@@ -12643,8 +12666,9 @@ static int bnx2x_set_uc_list(struct bnx2x *bp)
 				 BNX2X_UC_LIST_MAC, &ramrod_flags);
 }
 
-static int bnx2x_set_mc_list(struct bnx2x *bp)
+static int bnx2x_set_mc_list_e1x(struct bnx2x *bp)
 {
+	LIST_HEAD(mcast_group_list);
 	struct net_device *dev = bp->dev;
 	struct bnx2x_mcast_ramrod_params rparam = {NULL};
 	int rc = 0;
@@ -12660,12 +12684,9 @@ static int bnx2x_set_mc_list(struct bnx2x *bp)
 
 	/* then, configure a new MACs list */
 	if (netdev_mc_count(dev)) {
-		rc = bnx2x_init_mcast_macs_list(bp, &rparam);
-		if (rc) {
-			BNX2X_ERR("Failed to create multicast MACs list: %d\n",
-				  rc);
+		rc = bnx2x_init_mcast_macs_list(bp, &rparam, &mcast_group_list);
+		if (rc)
 			return rc;
-		}
 
 		/* Now add the new MACs */
 		rc = bnx2x_config_mcast(bp, &rparam,
@@ -12674,7 +12695,44 @@ static int bnx2x_set_mc_list(struct bnx2x *bp)
 			BNX2X_ERR("Failed to set a new multicast configuration: %d\n",
 				  rc);
 
-		bnx2x_free_mcast_macs_list(&rparam);
+		bnx2x_free_mcast_macs_list(&mcast_group_list);
+	}
+
+	return rc;
+}
+
+static int bnx2x_set_mc_list(struct bnx2x *bp)
+{
+	LIST_HEAD(mcast_group_list);
+	struct bnx2x_mcast_ramrod_params rparam = {NULL};
+	struct net_device *dev = bp->dev;
+	int rc = 0;
+
+	/* On older adapters, we need to flush and re-add filters */
+	if (CHIP_IS_E1x(bp))
+		return bnx2x_set_mc_list_e1x(bp);
+
+	rparam.mcast_obj = &bp->mcast_obj;
+
+	if (netdev_mc_count(dev)) {
+		rc = bnx2x_init_mcast_macs_list(bp, &rparam, &mcast_group_list);
+		if (rc)
+			return rc;
+
+		/* Override the curently configured set of mc filters */
+		rc = bnx2x_config_mcast(bp, &rparam,
+					BNX2X_MCAST_CMD_SET);
+		if (rc < 0)
+			BNX2X_ERR("Failed to set a new multicast configuration: %d\n",
+				  rc);
+
+		bnx2x_free_mcast_macs_list(&mcast_group_list);
+	} else {
+		/* If no mc addresses are required, flush the configuration */
+		rc = bnx2x_config_mcast(bp, &rparam, BNX2X_MCAST_CMD_DEL);
+		if (rc)
+			BNX2X_ERR("Failed to clear multicast configuration %d\n",
+				  rc);
 	}
 
 	return rc;
@@ -13214,13 +13272,22 @@ static int bnx2x_init_dev(struct bnx2x *bp, struct pci_dev *pdev,
 		NETIF_F_RXCSUM | NETIF_F_LRO | NETIF_F_GRO |
 		NETIF_F_RXHASH | NETIF_F_HW_VLAN_CTAG_TX;
 	if (!chip_is_e1x) {
-		dev->hw_features |= NETIF_F_GSO_GRE | NETIF_F_GSO_UDP_TUNNEL |
-				    NETIF_F_GSO_IPXIP4;
+		dev->hw_features |= NETIF_F_GSO_GRE | NETIF_F_GSO_GRE_CSUM |
+				    NETIF_F_GSO_IPXIP4 |
+				    NETIF_F_GSO_UDP_TUNNEL |
+				    NETIF_F_GSO_UDP_TUNNEL_CSUM |
+				    NETIF_F_GSO_PARTIAL;
+
 		dev->hw_enc_features =
 			NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM | NETIF_F_SG |
 			NETIF_F_TSO | NETIF_F_TSO_ECN | NETIF_F_TSO6 |
 			NETIF_F_GSO_IPXIP4 |
-			NETIF_F_GSO_GRE | NETIF_F_GSO_UDP_TUNNEL;
+			NETIF_F_GSO_GRE | NETIF_F_GSO_GRE_CSUM |
+			NETIF_F_GSO_UDP_TUNNEL | NETIF_F_GSO_UDP_TUNNEL_CSUM |
+			NETIF_F_GSO_PARTIAL;
+
+		dev->gso_partial_features = NETIF_F_GSO_GRE_CSUM |
+					    NETIF_F_GSO_UDP_TUNNEL_CSUM;
 	}
 
 	dev->vlan_features = NETIF_F_SG | NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM |
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.c
index ff702a707a91..cea6bdcde33f 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.c
@@ -2600,8 +2600,29 @@ struct bnx2x_mcast_mac_elem {
 	u8 pad[2]; /* For a natural alignment of the following buffer */
 };
 
+struct bnx2x_mcast_bin_elem {
+	struct list_head link;
+	int bin;
+	int type; /* BNX2X_MCAST_CMD_SET_{ADD, DEL} */
+};
+
+union bnx2x_mcast_elem {
+	struct bnx2x_mcast_bin_elem bin_elem;
+	struct bnx2x_mcast_mac_elem mac_elem;
+};
+
+struct bnx2x_mcast_elem_group {
+	struct list_head mcast_group_link;
+	union bnx2x_mcast_elem mcast_elems[];
+};
+
+#define MCAST_MAC_ELEMS_PER_PG \
+	((PAGE_SIZE - sizeof(struct bnx2x_mcast_elem_group)) / \
+	sizeof(union bnx2x_mcast_elem))
+
 struct bnx2x_pending_mcast_cmd {
 	struct list_head link;
+	struct list_head group_head;
 	int type; /* BNX2X_MCAST_CMD_X */
 	union {
 		struct list_head macs_head;
@@ -2609,6 +2630,11 @@ struct bnx2x_pending_mcast_cmd {
 		int next_bin; /* Needed for RESTORE flow with aprox match */
 	} data;
 
+	bool set_convert; /* in case type == BNX2X_MCAST_CMD_SET, this is set
+			   * when macs_head had been converted to a list of
+			   * bnx2x_mcast_bin_elem.
+			   */
+
 	bool done; /* set to true, when the command has been handled,
 		    * practically used in 57712 handling only, where one pending
 		    * command may be handled in a few operations. As long as for
@@ -2627,53 +2653,93 @@ static int bnx2x_mcast_wait(struct bnx2x *bp,
 	return 0;
 }
 
+static void bnx2x_free_groups(struct list_head *mcast_group_list)
+{
+	struct bnx2x_mcast_elem_group *current_mcast_group;
+
+	while (!list_empty(mcast_group_list)) {
+		current_mcast_group = list_first_entry(mcast_group_list,
+				      struct bnx2x_mcast_elem_group,
+				      mcast_group_link);
+		list_del(&current_mcast_group->mcast_group_link);
+		free_page((unsigned long)current_mcast_group);
+	}
+}
+
 static int bnx2x_mcast_enqueue_cmd(struct bnx2x *bp,
 				   struct bnx2x_mcast_obj *o,
 				   struct bnx2x_mcast_ramrod_params *p,
 				   enum bnx2x_mcast_cmd cmd)
 {
-	int total_sz;
 	struct bnx2x_pending_mcast_cmd *new_cmd;
-	struct bnx2x_mcast_mac_elem *cur_mac = NULL;
 	struct bnx2x_mcast_list_elem *pos;
-	int macs_list_len = ((cmd == BNX2X_MCAST_CMD_ADD) ?
-			     p->mcast_list_len : 0);
+	struct bnx2x_mcast_elem_group *elem_group;
+	struct bnx2x_mcast_mac_elem *mac_elem;
+	int total_elems = 0, macs_list_len = 0, offset = 0;
+
+	/* When adding MACs we'll need to store their values */
+	if (cmd == BNX2X_MCAST_CMD_ADD || cmd == BNX2X_MCAST_CMD_SET)
+		macs_list_len = p->mcast_list_len;
 
 	/* If the command is empty ("handle pending commands only"), break */
 	if (!p->mcast_list_len)
 		return 0;
 
-	total_sz = sizeof(*new_cmd) +
-		macs_list_len * sizeof(struct bnx2x_mcast_mac_elem);
-
 	/* Add mcast is called under spin_lock, thus calling with GFP_ATOMIC */
-	new_cmd = kzalloc(total_sz, GFP_ATOMIC);
-
+	new_cmd = kzalloc(sizeof(*new_cmd), GFP_ATOMIC);
 	if (!new_cmd)
 		return -ENOMEM;
 
-	DP(BNX2X_MSG_SP, "About to enqueue a new %d command. macs_list_len=%d\n",
-	   cmd, macs_list_len);
-
 	INIT_LIST_HEAD(&new_cmd->data.macs_head);
-
+	INIT_LIST_HEAD(&new_cmd->group_head);
 	new_cmd->type = cmd;
 	new_cmd->done = false;
 
+	DP(BNX2X_MSG_SP, "About to enqueue a new %d command. macs_list_len=%d\n",
+	   cmd, macs_list_len);
+
 	switch (cmd) {
 	case BNX2X_MCAST_CMD_ADD:
-		cur_mac = (struct bnx2x_mcast_mac_elem *)
-			  ((u8 *)new_cmd + sizeof(*new_cmd));
-
-		/* Push the MACs of the current command into the pending command
-		 * MACs list: FIFO
+	case BNX2X_MCAST_CMD_SET:
+		/* For a set command, we need to allocate sufficient memory for
+		 * all the bins, since we can't analyze at this point how much
+		 * memory would be required.
 		 */
+		total_elems = macs_list_len;
+		if (cmd == BNX2X_MCAST_CMD_SET) {
+			if (total_elems < BNX2X_MCAST_BINS_NUM)
+				total_elems = BNX2X_MCAST_BINS_NUM;
+		}
+		while (total_elems > 0) {
+			elem_group = (struct bnx2x_mcast_elem_group *)
+				     __get_free_page(GFP_ATOMIC | __GFP_ZERO);
+			if (!elem_group) {
+				bnx2x_free_groups(&new_cmd->group_head);
+				kfree(new_cmd);
+				return -ENOMEM;
+			}
+			total_elems -= MCAST_MAC_ELEMS_PER_PG;
+			list_add_tail(&elem_group->mcast_group_link,
+				      &new_cmd->group_head);
+		}
+		elem_group = list_first_entry(&new_cmd->group_head,
+					      struct bnx2x_mcast_elem_group,
+					      mcast_group_link);
 		list_for_each_entry(pos, &p->mcast_list, link) {
-			memcpy(cur_mac->mac, pos->mac, ETH_ALEN);
-			list_add_tail(&cur_mac->link, &new_cmd->data.macs_head);
-			cur_mac++;
+			mac_elem = &elem_group->mcast_elems[offset].mac_elem;
+			memcpy(mac_elem->mac, pos->mac, ETH_ALEN);
+			/* Push the MACs of the current command into the pending
+			 * command MACs list: FIFO
+			 */
+			list_add_tail(&mac_elem->link,
+				      &new_cmd->data.macs_head);
+			offset++;
+			if (offset == MCAST_MAC_ELEMS_PER_PG) {
+				offset = 0;
+				elem_group = list_next_entry(elem_group,
+							     mcast_group_link);
+			}
 		}
-
 		break;
 
 	case BNX2X_MCAST_CMD_DEL:
@@ -2771,7 +2837,8 @@ static void bnx2x_mcast_set_one_rule_e2(struct bnx2x *bp,
 	u8 rx_tx_add_flag = bnx2x_mcast_get_rx_tx_flag(o);
 	int bin;
 
-	if ((cmd == BNX2X_MCAST_CMD_ADD) || (cmd == BNX2X_MCAST_CMD_RESTORE))
+	if ((cmd == BNX2X_MCAST_CMD_ADD) || (cmd == BNX2X_MCAST_CMD_RESTORE) ||
+	    (cmd == BNX2X_MCAST_CMD_SET_ADD))
 		rx_tx_add_flag |= ETH_MULTICAST_RULES_CMD_IS_ADD;
 
 	data->rules[idx].cmd_general_data |= rx_tx_add_flag;
@@ -2797,6 +2864,16 @@ static void bnx2x_mcast_set_one_rule_e2(struct bnx2x *bp,
 		bin = cfg_data->bin;
 		break;
 
+	case BNX2X_MCAST_CMD_SET_ADD:
+		bin = cfg_data->bin;
+		BIT_VEC64_SET_BIT(o->registry.aprox_match.vec, bin);
+		break;
+
+	case BNX2X_MCAST_CMD_SET_DEL:
+		bin = cfg_data->bin;
+		BIT_VEC64_CLEAR_BIT(o->registry.aprox_match.vec, bin);
+		break;
+
 	default:
 		BNX2X_ERR("Unknown command: %d\n", cmd);
 		return;
@@ -2932,6 +3009,110 @@ static inline void bnx2x_mcast_hdl_pending_restore_e2(struct bnx2x *bp,
 		cmd_pos->data.next_bin++;
 }
 
+static void
+bnx2x_mcast_hdl_pending_set_e2_convert(struct bnx2x *bp,
+				       struct bnx2x_mcast_obj *o,
+				       struct bnx2x_pending_mcast_cmd *cmd_pos)
+{
+	u64 cur[BNX2X_MCAST_VEC_SZ], req[BNX2X_MCAST_VEC_SZ];
+	struct bnx2x_mcast_mac_elem *pmac_pos, *pmac_pos_n;
+	struct bnx2x_mcast_bin_elem *p_item;
+	struct bnx2x_mcast_elem_group *elem_group;
+	int cnt = 0, mac_cnt = 0, offset = 0, i;
+
+	memset(req, 0, sizeof(u64) * BNX2X_MCAST_VEC_SZ);
+	memcpy(cur, o->registry.aprox_match.vec,
+	       sizeof(u64) * BNX2X_MCAST_VEC_SZ);
+
+	/* Fill `current' with the required set of bins to configure */
+	list_for_each_entry_safe(pmac_pos, pmac_pos_n, &cmd_pos->data.macs_head,
+				 link) {
+		int bin = bnx2x_mcast_bin_from_mac(pmac_pos->mac);
+
+		DP(BNX2X_MSG_SP, "Set contains %pM mcast MAC\n",
+		   pmac_pos->mac);
+
+		BIT_VEC64_SET_BIT(req, bin);
+		list_del(&pmac_pos->link);
+		mac_cnt++;
+	}
+
+	/* We no longer have use for the MACs; Need to re-use memory for
+	 * a list that will be used to configure bins.
+	 */
+	cmd_pos->set_convert = true;
+	INIT_LIST_HEAD(&cmd_pos->data.macs_head);
+	elem_group = list_first_entry(&cmd_pos->group_head,
+				      struct bnx2x_mcast_elem_group,
+				      mcast_group_link);
+	for (i = 0; i < BNX2X_MCAST_BINS_NUM; i++) {
+		bool b_current = !!BIT_VEC64_TEST_BIT(cur, i);
+		bool b_required = !!BIT_VEC64_TEST_BIT(req, i);
+
+		if (b_current == b_required)
+			continue;
+
+		p_item = &elem_group->mcast_elems[offset].bin_elem;
+		p_item->bin = i;
+		p_item->type = b_required ? BNX2X_MCAST_CMD_SET_ADD
+					  : BNX2X_MCAST_CMD_SET_DEL;
+		list_add_tail(&p_item->link , &cmd_pos->data.macs_head);
+		cnt++;
+		offset++;
+		if (offset == MCAST_MAC_ELEMS_PER_PG) {
+			offset = 0;
+			elem_group = list_next_entry(elem_group,
+						     mcast_group_link);
+		}
+	}
+
+	/* We now definitely know how many commands are hiding here.
+	 * Also need to correct the disruption we've added to guarantee this
+	 * would be enqueued.
+	 */
+	o->total_pending_num -= (o->max_cmd_len + mac_cnt);
+	o->total_pending_num += cnt;
+
+	DP(BNX2X_MSG_SP, "o->total_pending_num=%d\n", o->total_pending_num);
+}
+
+static void
+bnx2x_mcast_hdl_pending_set_e2(struct bnx2x *bp,
+			       struct bnx2x_mcast_obj *o,
+			       struct bnx2x_pending_mcast_cmd *cmd_pos,
+			       int *cnt)
+{
+	union bnx2x_mcast_config_data cfg_data = {NULL};
+	struct bnx2x_mcast_bin_elem *p_item, *p_item_n;
+
+	/* This is actually a 2-part scheme - it starts by converting the MACs
+	 * into a list of bins to be added/removed, and correcting the numbers
+	 * on the object. this is now allowed, as we're now sure that all
+	 * previous configured requests have already applied.
+	 * The second part is actually adding rules for the newly introduced
+	 * entries [like all the rest of the hdl_pending functions].
+	 */
+	if (!cmd_pos->set_convert)
+		bnx2x_mcast_hdl_pending_set_e2_convert(bp, o, cmd_pos);
+
+	list_for_each_entry_safe(p_item, p_item_n, &cmd_pos->data.macs_head,
+				 link) {
+		cfg_data.bin = (u8)p_item->bin;
+		o->set_one_rule(bp, o, *cnt, &cfg_data, p_item->type);
+		(*cnt)++;
+
+		list_del(&p_item->link);
+
+		/* Break if we reached the maximum number of rules. */
+		if (*cnt >= o->max_cmd_len)
+			break;
+	}
+
+	/* if no more MACs to configure - we are done */
+	if (list_empty(&cmd_pos->data.macs_head))
+		cmd_pos->done = true;
+}
+
 static inline int bnx2x_mcast_handle_pending_cmds_e2(struct bnx2x *bp,
 				struct bnx2x_mcast_ramrod_params *p)
 {
@@ -2955,6 +3136,10 @@ static inline int bnx2x_mcast_handle_pending_cmds_e2(struct bnx2x *bp,
 							   &cnt);
 			break;
 
+		case BNX2X_MCAST_CMD_SET:
+			bnx2x_mcast_hdl_pending_set_e2(bp, o, cmd_pos, &cnt);
+			break;
+
 		default:
 			BNX2X_ERR("Unknown command: %d\n", cmd_pos->type);
 			return -EINVAL;
@@ -2965,6 +3150,7 @@ static inline int bnx2x_mcast_handle_pending_cmds_e2(struct bnx2x *bp,
 		 */
 		if (cmd_pos->done) {
 			list_del(&cmd_pos->link);
+			bnx2x_free_groups(&cmd_pos->group_head);
 			kfree(cmd_pos);
 		}
 
@@ -3095,6 +3281,19 @@ static int bnx2x_mcast_validate_e2(struct bnx2x *bp,
 		o->set_registry_size(o, reg_sz + p->mcast_list_len);
 		break;
 
+	case BNX2X_MCAST_CMD_SET:
+		/* We can only learn how many commands would actually be used
+		 * when this is being configured. So for now, simply guarantee
+		 * the command will be enqueued [to refrain from adding logic
+		 * that handles this and THEN learns it needs several ramrods].
+		 * Just like for ADD/Cont, the mcast_list_len might be an over
+		 * estimation; or even more so, since we don't take into
+		 * account the possibility of removal of existing bins.
+		 */
+		o->set_registry_size(o, reg_sz + p->mcast_list_len);
+		o->total_pending_num += o->max_cmd_len;
+		break;
+
 	default:
 		BNX2X_ERR("Unknown command: %d\n", cmd);
 		return -EINVAL;
@@ -3108,12 +3307,16 @@ static int bnx2x_mcast_validate_e2(struct bnx2x *bp,
 
 static void bnx2x_mcast_revert_e2(struct bnx2x *bp,
 				      struct bnx2x_mcast_ramrod_params *p,
-				      int old_num_bins)
+				  int old_num_bins,
+				  enum bnx2x_mcast_cmd cmd)
 {
 	struct bnx2x_mcast_obj *o = p->mcast_obj;
 
 	o->set_registry_size(o, old_num_bins);
 	o->total_pending_num -= p->mcast_list_len;
+
+	if (cmd == BNX2X_MCAST_CMD_SET)
+		o->total_pending_num -= o->max_cmd_len;
 }
 
 /**
@@ -3223,9 +3426,11 @@ static int bnx2x_mcast_setup_e2(struct bnx2x *bp,
 		bnx2x_mcast_refresh_registry_e2(bp, o);
 
 	/* If CLEAR_ONLY was requested - don't send a ramrod and clear
-	 * RAMROD_PENDING status immediately.
+	 * RAMROD_PENDING status immediately. due to the SET option, it's also
+	 * possible that after evaluating the differences there's no need for
+	 * a ramrod. In that case, we can skip it as well.
 	 */
-	if (test_bit(RAMROD_DRV_CLR_ONLY, &p->ramrod_flags)) {
+	if (test_bit(RAMROD_DRV_CLR_ONLY, &p->ramrod_flags) || !cnt) {
 		raw->clear_pending(raw);
 		return 0;
 	} else {
@@ -3253,6 +3458,11 @@ static int bnx2x_mcast_validate_e1h(struct bnx2x *bp,
 				    struct bnx2x_mcast_ramrod_params *p,
 				    enum bnx2x_mcast_cmd cmd)
 {
+	if (cmd == BNX2X_MCAST_CMD_SET) {
+		BNX2X_ERR("Can't use `set' command on e1h!\n");
+		return -EINVAL;
+	}
+
 	/* Mark, that there is a work to do */
 	if ((cmd == BNX2X_MCAST_CMD_DEL) || (cmd == BNX2X_MCAST_CMD_RESTORE))
 		p->mcast_list_len = 1;
@@ -3262,7 +3472,8 @@ static int bnx2x_mcast_validate_e1h(struct bnx2x *bp,
 
 static void bnx2x_mcast_revert_e1h(struct bnx2x *bp,
 				       struct bnx2x_mcast_ramrod_params *p,
-				       int old_num_bins)
+				       int old_num_bins,
+				       enum bnx2x_mcast_cmd cmd)
 {
 	/* Do nothing */
 }
@@ -3372,6 +3583,11 @@ static int bnx2x_mcast_validate_e1(struct bnx2x *bp,
 	struct bnx2x_mcast_obj *o = p->mcast_obj;
 	int reg_sz = o->get_registry_size(o);
 
+	if (cmd == BNX2X_MCAST_CMD_SET) {
+		BNX2X_ERR("Can't use `set' command on e1!\n");
+		return -EINVAL;
+	}
+
 	switch (cmd) {
 	/* DEL command deletes all currently configured MACs */
 	case BNX2X_MCAST_CMD_DEL:
@@ -3422,7 +3638,8 @@ static int bnx2x_mcast_validate_e1(struct bnx2x *bp,
 
 static void bnx2x_mcast_revert_e1(struct bnx2x *bp,
 				      struct bnx2x_mcast_ramrod_params *p,
-				      int old_num_macs)
+				   int old_num_macs,
+				   enum bnx2x_mcast_cmd cmd)
 {
 	struct bnx2x_mcast_obj *o = p->mcast_obj;
 
@@ -3572,6 +3789,7 @@ static inline int bnx2x_mcast_handle_pending_cmds_e1(
 	}
 
 	list_del(&cmd_pos->link);
+	bnx2x_free_groups(&cmd_pos->group_head);
 	kfree(cmd_pos);
 
 	return cnt;
@@ -3816,7 +4034,7 @@ error_exit2:
 	r->clear_pending(r);
 
 error_exit1:
-	o->revert(bp, p, old_reg_size);
+	o->revert(bp, p, old_reg_size, cmd);
 
 	return rc;
 }
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.h
index 4048fc594cce..0bf2fd470819 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.h
@@ -536,6 +536,15 @@ enum bnx2x_mcast_cmd {
 	BNX2X_MCAST_CMD_CONT,
 	BNX2X_MCAST_CMD_DEL,
 	BNX2X_MCAST_CMD_RESTORE,
+
+	/* Following this, multicast configuration should equal to approx
+	 * the set of MACs provided [i.e., remove all else].
+	 * The two sub-commands are used internally to decide whether a given
+	 * bin is to be added or removed
+	 */
+	BNX2X_MCAST_CMD_SET,
+	BNX2X_MCAST_CMD_SET_ADD,
+	BNX2X_MCAST_CMD_SET_DEL,
 };
 
 struct bnx2x_mcast_obj {
@@ -635,7 +644,8 @@ struct bnx2x_mcast_obj {
 	 */
 	void (*revert)(struct bnx2x *bp,
 		       struct bnx2x_mcast_ramrod_params *p,
-		       int old_num_bins);
+		       int old_num_bins,
+		       enum bnx2x_mcast_cmd cmd);
 
 	int (*get_registry_size)(struct bnx2x_mcast_obj *o);
 	void (*set_registry_size)(struct bnx2x_mcast_obj *o, int n);
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.c
index 632daff117d3..3f77d0863543 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.c
@@ -573,17 +573,6 @@ int bnx2x_vf_mcast(struct bnx2x *bp, struct bnx2x_virtf *vf,
 		}
 	}
 
-	/* clear existing mcasts */
-	mcast.mcast_list_len = vf->mcast_list_len;
-	vf->mcast_list_len = mc_num;
-	rc = bnx2x_config_mcast(bp, &mcast, BNX2X_MCAST_CMD_DEL);
-	if (rc) {
-		BNX2X_ERR("Failed to remove multicasts\n");
-		kfree(mc);
-		return rc;
-	}
-
-	/* update mcast list on the ramrod params */
 	if (mc_num) {
 		INIT_LIST_HEAD(&mcast.mcast_list);
 		for (i = 0; i < mc_num; i++) {
@@ -594,12 +583,18 @@ int bnx2x_vf_mcast(struct bnx2x *bp, struct bnx2x_virtf *vf,
 
 		/* add new mcasts */
 		mcast.mcast_list_len = mc_num;
-		rc = bnx2x_config_mcast(bp, &mcast, BNX2X_MCAST_CMD_ADD);
+		rc = bnx2x_config_mcast(bp, &mcast, BNX2X_MCAST_CMD_SET);
 		if (rc)
-			BNX2X_ERR("Faled to add multicasts\n");
-		kfree(mc);
+			BNX2X_ERR("Faled to set multicasts\n");
+	} else {
+		/* clear existing mcasts */
+		rc = bnx2x_config_mcast(bp, &mcast, BNX2X_MCAST_CMD_DEL);
+		if (rc)
+			BNX2X_ERR("Failed to remove multicasts\n");
 	}
 
+	kfree(mc);
+
 	return rc;
 }
 
@@ -1583,7 +1578,6 @@ int bnx2x_iov_nic_init(struct bnx2x *bp)
 		 *  It needs to be initialized here so that it can be safely
 		 *  handled by a subsequent FLR flow.
 		 */
-		vf->mcast_list_len = 0;
 		bnx2x_init_mcast_obj(bp, &vf->mcast_obj, 0xFF,
 				     0xFF, 0xFF, 0xFF,
 				     bnx2x_vf_sp(bp, vf, mcast_rdata),
@@ -2527,7 +2521,8 @@ void bnx2x_pf_set_vfs_vlan(struct bnx2x *bp)
 	for_each_vf(bp, vfidx) {
 		bulletin = BP_VF_BULLETIN(bp, vfidx);
 		if (bulletin->valid_bitmap & (1 << VLAN_VALID))
-			bnx2x_set_vf_vlan(bp->dev, vfidx, bulletin->vlan, 0);
+			bnx2x_set_vf_vlan(bp->dev, vfidx, bulletin->vlan, 0,
+					  htons(ETH_P_8021Q));
 	}
 }
 
@@ -2787,7 +2782,8 @@ static int bnx2x_set_vf_vlan_filter(struct bnx2x *bp, struct bnx2x_virtf *vf,
 	return 0;
 }
 
-int bnx2x_set_vf_vlan(struct net_device *dev, int vfidx, u16 vlan, u8 qos)
+int bnx2x_set_vf_vlan(struct net_device *dev, int vfidx, u16 vlan, u8 qos,
+		      __be16 vlan_proto)
 {
 	struct pf_vf_bulletin_content *bulletin = NULL;
 	struct bnx2x *bp = netdev_priv(dev);
@@ -2802,6 +2798,9 @@ int bnx2x_set_vf_vlan(struct net_device *dev, int vfidx, u16 vlan, u8 qos)
 		return -EINVAL;
 	}
 
+	if (vlan_proto != htons(ETH_P_8021Q))
+		return -EPROTONOSUPPORT;
+
 	DP(BNX2X_MSG_IOV, "configuring VF %d with VLAN %d qos %d\n",
 	   vfidx, vlan, 0);
 
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.h
index 670a581ffabc..7a6d406f4c11 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.h
@@ -195,7 +195,6 @@ struct bnx2x_virtf {
 	int leading_rss;
 
 	/* MCAST object */
-	int mcast_list_len;
 	struct bnx2x_mcast_obj		mcast_obj;
 
 	/* RSS configuration object */
diff --git a/drivers/net/ethernet/broadcom/bnxt/bnxt.c b/drivers/net/ethernet/broadcom/bnxt/bnxt.c
index 228c964e709a..a9f9f3738022 100644
--- a/drivers/net/ethernet/broadcom/bnxt/bnxt.c
+++ b/drivers/net/ethernet/broadcom/bnxt/bnxt.c
@@ -32,6 +32,7 @@
 #include <linux/mii.h>
 #include <linux/if.h>
 #include <linux/if_vlan.h>
+#include <linux/rtc.h>
 #include <net/ip.h>
 #include <net/tcp.h>
 #include <net/udp.h>
@@ -93,50 +94,49 @@ enum board_idx {
 	BCM57404_NPAR,
 	BCM57406_NPAR,
 	BCM57407_SFP,
+	BCM57407_NPAR,
 	BCM57414_NPAR,
 	BCM57416_NPAR,
-	BCM57304_VF,
-	BCM57404_VF,
-	BCM57414_VF,
-	BCM57314_VF,
+	NETXTREME_E_VF,
+	NETXTREME_C_VF,
 };
 
 /* indexed by enum above */
 static const struct {
 	char *name;
 } board_info[] = {
-	{ "Broadcom BCM57301 NetXtreme-C Single-port 10Gb Ethernet" },
-	{ "Broadcom BCM57302 NetXtreme-C Dual-port 10Gb/25Gb Ethernet" },
-	{ "Broadcom BCM57304 NetXtreme-C Dual-port 10Gb/25Gb/40Gb/50Gb Ethernet" },
+	{ "Broadcom BCM57301 NetXtreme-C 10Gb Ethernet" },
+	{ "Broadcom BCM57302 NetXtreme-C 10Gb/25Gb Ethernet" },
+	{ "Broadcom BCM57304 NetXtreme-C 10Gb/25Gb/40Gb/50Gb Ethernet" },
 	{ "Broadcom BCM57417 NetXtreme-E Ethernet Partition" },
-	{ "Broadcom BCM58700 Nitro 4-port 1Gb/2.5Gb/10Gb Ethernet" },
-	{ "Broadcom BCM57311 NetXtreme-C Single-port 10Gb Ethernet" },
-	{ "Broadcom BCM57312 NetXtreme-C Dual-port 10Gb/25Gb Ethernet" },
-	{ "Broadcom BCM57402 NetXtreme-E Dual-port 10Gb Ethernet" },
-	{ "Broadcom BCM57404 NetXtreme-E Dual-port 10Gb/25Gb Ethernet" },
-	{ "Broadcom BCM57406 NetXtreme-E Dual-port 10GBase-T Ethernet" },
+	{ "Broadcom BCM58700 Nitro 1Gb/2.5Gb/10Gb Ethernet" },
+	{ "Broadcom BCM57311 NetXtreme-C 10Gb Ethernet" },
+	{ "Broadcom BCM57312 NetXtreme-C 10Gb/25Gb Ethernet" },
+	{ "Broadcom BCM57402 NetXtreme-E 10Gb Ethernet" },
+	{ "Broadcom BCM57404 NetXtreme-E 10Gb/25Gb Ethernet" },
+	{ "Broadcom BCM57406 NetXtreme-E 10GBase-T Ethernet" },
 	{ "Broadcom BCM57402 NetXtreme-E Ethernet Partition" },
-	{ "Broadcom BCM57407 NetXtreme-E Dual-port 10GBase-T Ethernet" },
-	{ "Broadcom BCM57412 NetXtreme-E Dual-port 10Gb Ethernet" },
-	{ "Broadcom BCM57414 NetXtreme-E Dual-port 10Gb/25Gb Ethernet" },
-	{ "Broadcom BCM57416 NetXtreme-E Dual-port 10GBase-T Ethernet" },
-	{ "Broadcom BCM57417 NetXtreme-E Dual-port 10GBase-T Ethernet" },
+	{ "Broadcom BCM57407 NetXtreme-E 10GBase-T Ethernet" },
+	{ "Broadcom BCM57412 NetXtreme-E 10Gb Ethernet" },
+	{ "Broadcom BCM57414 NetXtreme-E 10Gb/25Gb Ethernet" },
+	{ "Broadcom BCM57416 NetXtreme-E 10GBase-T Ethernet" },
+	{ "Broadcom BCM57417 NetXtreme-E 10GBase-T Ethernet" },
 	{ "Broadcom BCM57412 NetXtreme-E Ethernet Partition" },
-	{ "Broadcom BCM57314 NetXtreme-C Dual-port 10Gb/25Gb/40Gb/50Gb Ethernet" },
-	{ "Broadcom BCM57417 NetXtreme-E Dual-port 10Gb/25Gb Ethernet" },
-	{ "Broadcom BCM57416 NetXtreme-E Dual-port 10Gb Ethernet" },
+	{ "Broadcom BCM57314 NetXtreme-C 10Gb/25Gb/40Gb/50Gb Ethernet" },
+	{ "Broadcom BCM57417 NetXtreme-E 10Gb/25Gb Ethernet" },
+	{ "Broadcom BCM57416 NetXtreme-E 10Gb Ethernet" },
 	{ "Broadcom BCM57404 NetXtreme-E Ethernet Partition" },
 	{ "Broadcom BCM57406 NetXtreme-E Ethernet Partition" },
-	{ "Broadcom BCM57407 NetXtreme-E Dual-port 25Gb Ethernet" },
+	{ "Broadcom BCM57407 NetXtreme-E 25Gb Ethernet" },
+	{ "Broadcom BCM57407 NetXtreme-E Ethernet Partition" },
 	{ "Broadcom BCM57414 NetXtreme-E Ethernet Partition" },
 	{ "Broadcom BCM57416 NetXtreme-E Ethernet Partition" },
-	{ "Broadcom BCM57304 NetXtreme-C Ethernet Virtual Function" },
-	{ "Broadcom BCM57404 NetXtreme-E Ethernet Virtual Function" },
-	{ "Broadcom BCM57414 NetXtreme-E Ethernet Virtual Function" },
-	{ "Broadcom BCM57314 NetXtreme-E Ethernet Virtual Function" },
+	{ "Broadcom NetXtreme-E Ethernet Virtual Function" },
+	{ "Broadcom NetXtreme-C Ethernet Virtual Function" },
 };
 
 static const struct pci_device_id bnxt_pci_tbl[] = {
+	{ PCI_VDEVICE(BROADCOM, 0x16c0), .driver_data = BCM57417_NPAR },
 	{ PCI_VDEVICE(BROADCOM, 0x16c8), .driver_data = BCM57301 },
 	{ PCI_VDEVICE(BROADCOM, 0x16c9), .driver_data = BCM57302 },
 	{ PCI_VDEVICE(BROADCOM, 0x16ca), .driver_data = BCM57304 },
@@ -160,13 +160,19 @@ static const struct pci_device_id bnxt_pci_tbl[] = {
 	{ PCI_VDEVICE(BROADCOM, 0x16e7), .driver_data = BCM57404_NPAR },
 	{ PCI_VDEVICE(BROADCOM, 0x16e8), .driver_data = BCM57406_NPAR },
 	{ PCI_VDEVICE(BROADCOM, 0x16e9), .driver_data = BCM57407_SFP },
+	{ PCI_VDEVICE(BROADCOM, 0x16ea), .driver_data = BCM57407_NPAR },
+	{ PCI_VDEVICE(BROADCOM, 0x16eb), .driver_data = BCM57412_NPAR },
 	{ PCI_VDEVICE(BROADCOM, 0x16ec), .driver_data = BCM57414_NPAR },
+	{ PCI_VDEVICE(BROADCOM, 0x16ed), .driver_data = BCM57414_NPAR },
 	{ PCI_VDEVICE(BROADCOM, 0x16ee), .driver_data = BCM57416_NPAR },
+	{ PCI_VDEVICE(BROADCOM, 0x16ef), .driver_data = BCM57416_NPAR },
 #ifdef CONFIG_BNXT_SRIOV
-	{ PCI_VDEVICE(BROADCOM, 0x16cb), .driver_data = BCM57304_VF },
-	{ PCI_VDEVICE(BROADCOM, 0x16d3), .driver_data = BCM57404_VF },
-	{ PCI_VDEVICE(BROADCOM, 0x16dc), .driver_data = BCM57414_VF },
-	{ PCI_VDEVICE(BROADCOM, 0x16e1), .driver_data = BCM57314_VF },
+	{ PCI_VDEVICE(BROADCOM, 0x16c1), .driver_data = NETXTREME_E_VF },
+	{ PCI_VDEVICE(BROADCOM, 0x16cb), .driver_data = NETXTREME_C_VF },
+	{ PCI_VDEVICE(BROADCOM, 0x16d3), .driver_data = NETXTREME_E_VF },
+	{ PCI_VDEVICE(BROADCOM, 0x16dc), .driver_data = NETXTREME_E_VF },
+	{ PCI_VDEVICE(BROADCOM, 0x16e1), .driver_data = NETXTREME_C_VF },
+	{ PCI_VDEVICE(BROADCOM, 0x16e5), .driver_data = NETXTREME_C_VF },
 #endif
 	{ 0 }
 };
@@ -189,8 +195,7 @@ static const u16 bnxt_async_events_arr[] = {
 
 static bool bnxt_vf_pciid(enum board_idx idx)
 {
-	return (idx == BCM57304_VF || idx == BCM57404_VF ||
-		idx == BCM57314_VF || idx == BCM57414_VF);
+	return (idx == NETXTREME_C_VF || idx == NETXTREME_E_VF);
 }
 
 #define DB_CP_REARM_FLAGS	(DB_KEY_CP | DB_IDX_VALID)
@@ -3419,10 +3424,10 @@ static int bnxt_hwrm_vnic_set_rss(struct bnxt *bp, u16 vnic_id, bool set_rss)
 
 	bnxt_hwrm_cmd_hdr_init(bp, &req, HWRM_VNIC_RSS_CFG, -1, -1);
 	if (set_rss) {
-		vnic->hash_type = BNXT_RSS_HASH_TYPE_FLAG_IPV4 |
-				 BNXT_RSS_HASH_TYPE_FLAG_TCP_IPV4 |
-				 BNXT_RSS_HASH_TYPE_FLAG_IPV6 |
-				 BNXT_RSS_HASH_TYPE_FLAG_TCP_IPV6;
+		vnic->hash_type = VNIC_RSS_CFG_REQ_HASH_TYPE_IPV4 |
+				  VNIC_RSS_CFG_REQ_HASH_TYPE_TCP_IPV4 |
+				  VNIC_RSS_CFG_REQ_HASH_TYPE_IPV6 |
+				  VNIC_RSS_CFG_REQ_HASH_TYPE_TCP_IPV6;
 
 		req.hash_type = cpu_to_le32(vnic->hash_type);
 
@@ -4156,6 +4161,11 @@ int bnxt_hwrm_func_qcaps(struct bnxt *bp)
 	if (rc)
 		goto hwrm_func_qcaps_exit;
 
+	bp->tx_push_thresh = 0;
+	if (resp->flags &
+	    cpu_to_le32(FUNC_QCAPS_RESP_FLAGS_PUSH_MODE_SUPPORTED))
+		bp->tx_push_thresh = BNXT_TX_PUSH_THRESH;
+
 	if (BNXT_PF(bp)) {
 		struct bnxt_pf_info *pf = &bp->pf;
 
@@ -4187,12 +4197,6 @@ int bnxt_hwrm_func_qcaps(struct bnxt *bp)
 		struct bnxt_vf_info *vf = &bp->vf;
 
 		vf->fw_fid = le16_to_cpu(resp->fid);
-		memcpy(vf->mac_addr, resp->mac_address, ETH_ALEN);
-		if (is_valid_ether_addr(vf->mac_addr))
-			/* overwrite netdev dev_adr with admin VF MAC */
-			memcpy(bp->dev->dev_addr, vf->mac_addr, ETH_ALEN);
-		else
-			random_ether_addr(bp->dev->dev_addr);
 
 		vf->max_rsscos_ctxs = le16_to_cpu(resp->max_rsscos_ctx);
 		vf->max_cp_rings = le16_to_cpu(resp->max_cmpl_rings);
@@ -4204,14 +4208,21 @@ int bnxt_hwrm_func_qcaps(struct bnxt *bp)
 		vf->max_l2_ctxs = le16_to_cpu(resp->max_l2_ctxs);
 		vf->max_vnics = le16_to_cpu(resp->max_vnics);
 		vf->max_stat_ctxs = le16_to_cpu(resp->max_stat_ctx);
+
+		memcpy(vf->mac_addr, resp->mac_address, ETH_ALEN);
+		mutex_unlock(&bp->hwrm_cmd_lock);
+
+		if (is_valid_ether_addr(vf->mac_addr)) {
+			/* overwrite netdev dev_adr with admin VF MAC */
+			memcpy(bp->dev->dev_addr, vf->mac_addr, ETH_ALEN);
+		} else {
+			random_ether_addr(bp->dev->dev_addr);
+			rc = bnxt_approve_mac(bp, bp->dev->dev_addr);
+		}
+		return rc;
 #endif
 	}
 
-	bp->tx_push_thresh = 0;
-	if (resp->flags &
-	    cpu_to_le32(FUNC_QCAPS_RESP_FLAGS_PUSH_MODE_SUPPORTED))
-		bp->tx_push_thresh = BNXT_TX_PUSH_THRESH;
-
 hwrm_func_qcaps_exit:
 	mutex_unlock(&bp->hwrm_cmd_lock);
 	return rc;
@@ -4249,6 +4260,9 @@ static int bnxt_hwrm_queue_qportcfg(struct bnxt *bp)
 	if (bp->max_tc > BNXT_MAX_QUEUE)
 		bp->max_tc = BNXT_MAX_QUEUE;
 
+	if (resp->queue_cfg_info & QUEUE_QPORTCFG_RESP_QUEUE_CFG_INFO_ASYM_CFG)
+		bp->max_tc = 1;
+
 	qptr = &resp->queue_id0;
 	for (i = 0; i < bp->max_tc; i++) {
 		bp->q_info[i].queue_id = *qptr++;
@@ -4307,6 +4321,31 @@ hwrm_ver_get_exit:
 	return rc;
 }
 
+int bnxt_hwrm_fw_set_time(struct bnxt *bp)
+{
+#if IS_ENABLED(CONFIG_RTC_LIB)
+	struct hwrm_fw_set_time_input req = {0};
+	struct rtc_time tm;
+	struct timeval tv;
+
+	if (bp->hwrm_spec_code < 0x10400)
+		return -EOPNOTSUPP;
+
+	do_gettimeofday(&tv);
+	rtc_time_to_tm(tv.tv_sec, &tm);
+	bnxt_hwrm_cmd_hdr_init(bp, &req, HWRM_FW_SET_TIME, -1, -1);
+	req.year = cpu_to_le16(1900 + tm.tm_year);
+	req.month = 1 + tm.tm_mon;
+	req.day = tm.tm_mday;
+	req.hour = tm.tm_hour;
+	req.minute = tm.tm_min;
+	req.second = tm.tm_sec;
+	return hwrm_send_message(bp, &req, sizeof(req), HWRM_CMD_TIMEOUT);
+#else
+	return -EOPNOTSUPP;
+#endif
+}
+
 static int bnxt_hwrm_port_qstats(struct bnxt *bp)
 {
 	int rc;
@@ -6804,6 +6843,8 @@ static int bnxt_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	if (rc)
 		goto init_err;
 
+	bnxt_hwrm_fw_set_time(bp);
+
 	dev->hw_features = NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM | NETIF_F_SG |
 			   NETIF_F_TSO | NETIF_F_TSO6 |
 			   NETIF_F_GSO_UDP_TUNNEL | NETIF_F_GSO_GRE |
diff --git a/drivers/net/ethernet/broadcom/bnxt/bnxt.h b/drivers/net/ethernet/broadcom/bnxt/bnxt.h
index 23e04a6142fb..51b164a0e844 100644
--- a/drivers/net/ethernet/broadcom/bnxt/bnxt.h
+++ b/drivers/net/ethernet/broadcom/bnxt/bnxt.h
@@ -11,10 +11,10 @@
 #define BNXT_H
 
 #define DRV_MODULE_NAME		"bnxt_en"
-#define DRV_MODULE_VERSION	"1.3.0"
+#define DRV_MODULE_VERSION	"1.5.0"
 
 #define DRV_VER_MAJ	1
-#define DRV_VER_MIN	3
+#define DRV_VER_MIN	5
 #define DRV_VER_UPD	0
 
 struct tx_bd {
@@ -106,11 +106,11 @@ struct tx_cmp {
 	 #define CMP_TYPE_REMOTE_DRIVER_REQ			 34
 	 #define CMP_TYPE_REMOTE_DRIVER_RESP			 36
 	 #define CMP_TYPE_ERROR_STATUS				 48
-	 #define CMPL_BASE_TYPE_STAT_EJECT			 (0x1aUL << 0)
-	 #define CMPL_BASE_TYPE_HWRM_DONE			 (0x20UL << 0)
-	 #define CMPL_BASE_TYPE_HWRM_FWD_REQ			 (0x22UL << 0)
-	 #define CMPL_BASE_TYPE_HWRM_FWD_RESP			 (0x24UL << 0)
-	 #define CMPL_BASE_TYPE_HWRM_ASYNC_EVENT		 (0x2eUL << 0)
+	 #define CMPL_BASE_TYPE_STAT_EJECT			 0x1aUL
+	 #define CMPL_BASE_TYPE_HWRM_DONE			 0x20UL
+	 #define CMPL_BASE_TYPE_HWRM_FWD_REQ			 0x22UL
+	 #define CMPL_BASE_TYPE_HWRM_FWD_RESP			 0x24UL
+	 #define CMPL_BASE_TYPE_HWRM_ASYNC_EVENT		 0x2eUL
 
 	#define TX_CMP_FLAGS_ERROR				(1 << 6)
 	#define TX_CMP_FLAGS_PUSH				(1 << 7)
@@ -389,11 +389,6 @@ struct rx_tpa_end_cmp_ext {
 
 #define INVALID_HW_RING_ID	((u16)-1)
 
-#define BNXT_RSS_HASH_TYPE_FLAG_IPV4		0x01
-#define BNXT_RSS_HASH_TYPE_FLAG_TCP_IPV4	0x02
-#define BNXT_RSS_HASH_TYPE_FLAG_IPV6		0x04
-#define BNXT_RSS_HASH_TYPE_FLAG_TCP_IPV6	0x08
-
 /* The hardware supports certain page sizes.  Use the supported page sizes
  * to allocate the rings.
  */
@@ -418,7 +413,7 @@ struct rx_tpa_end_cmp_ext {
 
 #define BNXT_RX_PAGE_SIZE (1 << BNXT_RX_PAGE_SHIFT)
 
-#define BNXT_MIN_PKT_SIZE	45
+#define BNXT_MIN_PKT_SIZE	52
 
 #define BNXT_NUM_TESTS(bp)	0
 
@@ -1225,6 +1220,7 @@ int bnxt_hwrm_set_coal(struct bnxt *);
 int bnxt_hwrm_func_qcaps(struct bnxt *);
 int bnxt_hwrm_set_pause(struct bnxt *);
 int bnxt_hwrm_set_link_setting(struct bnxt *, bool, bool);
+int bnxt_hwrm_fw_set_time(struct bnxt *);
 int bnxt_open_nic(struct bnxt *, bool, bool);
 int bnxt_close_nic(struct bnxt *, bool, bool);
 int bnxt_get_max_rings(struct bnxt *, int *, int *, bool);
diff --git a/drivers/net/ethernet/broadcom/bnxt/bnxt_ethtool.c b/drivers/net/ethernet/broadcom/bnxt/bnxt_ethtool.c
index b83e17403d6c..a7e04ff4eaed 100644
--- a/drivers/net/ethernet/broadcom/bnxt/bnxt_ethtool.c
+++ b/drivers/net/ethernet/broadcom/bnxt/bnxt_ethtool.c
@@ -21,6 +21,8 @@
 #include "bnxt_nvm_defs.h"	/* NVRAM content constant and structure defs */
 #include "bnxt_fw_hdr.h"	/* Firmware hdr constant and structure defs */
 #define FLASH_NVRAM_TIMEOUT	((HWRM_CMD_TIMEOUT) * 100)
+#define FLASH_PACKAGE_TIMEOUT	((HWRM_CMD_TIMEOUT) * 200)
+#define INSTALL_PACKAGE_TIMEOUT	((HWRM_CMD_TIMEOUT) * 200)
 
 static char *bnxt_get_pkgver(struct net_device *dev, char *buf, size_t buflen);
 
@@ -346,7 +348,7 @@ static void bnxt_get_channels(struct net_device *dev,
 	int max_rx_rings, max_tx_rings, tcs;
 
 	bnxt_get_max_rings(bp, &max_rx_rings, &max_tx_rings, true);
-	channel->max_combined = max_rx_rings;
+	channel->max_combined = max_t(int, max_rx_rings, max_tx_rings);
 
 	if (bnxt_get_max_rings(bp, &max_rx_rings, &max_tx_rings, false)) {
 		max_rx_rings = 0;
@@ -404,8 +406,8 @@ static int bnxt_set_channels(struct net_device *dev,
 	if (tcs > 1)
 		max_tx_rings /= tcs;
 
-	if (sh && (channel->combined_count > max_rx_rings ||
-		   channel->combined_count > max_tx_rings))
+	if (sh &&
+	    channel->combined_count > max_t(int, max_rx_rings, max_tx_rings))
 		return -ENOMEM;
 
 	if (!sh && (channel->rx_count > max_rx_rings ||
@@ -428,8 +430,10 @@ static int bnxt_set_channels(struct net_device *dev,
 
 	if (sh) {
 		bp->flags |= BNXT_FLAG_SHARED_RINGS;
-		bp->rx_nr_rings = channel->combined_count;
-		bp->tx_nr_rings_per_tc = channel->combined_count;
+		bp->rx_nr_rings = min_t(int, channel->combined_count,
+					max_rx_rings);
+		bp->tx_nr_rings_per_tc = min_t(int, channel->combined_count,
+					       max_tx_rings);
 	} else {
 		bp->flags &= ~BNXT_FLAG_SHARED_RINGS;
 		bp->rx_nr_rings = channel->rx_count;
@@ -1028,6 +1032,10 @@ static u32 bnxt_get_link(struct net_device *dev)
 	return bp->link_info.link_up;
 }
 
+static int bnxt_find_nvram_item(struct net_device *dev, u16 type, u16 ordinal,
+				u16 ext, u16 *index, u32 *item_length,
+				u32 *data_length);
+
 static int bnxt_flash_nvram(struct net_device *dev,
 			    u16 dir_type,
 			    u16 dir_ordinal,
@@ -1179,7 +1187,6 @@ static int bnxt_flash_firmware(struct net_device *dev,
 			   (unsigned long)calculated_crc);
 		return -EINVAL;
 	}
-	/* TODO: Validate digital signature (RSA-encrypted SHA-256 hash) here */
 	rc = bnxt_flash_nvram(dev, dir_type, BNX_DIR_ORDINAL_FIRST,
 			      0, 0, fw_data, fw_size);
 	if (rc == 0)	/* Firmware update successful */
@@ -1188,6 +1195,57 @@ static int bnxt_flash_firmware(struct net_device *dev,
 	return rc;
 }
 
+static int bnxt_flash_microcode(struct net_device *dev,
+				u16 dir_type,
+				const u8 *fw_data,
+				size_t fw_size)
+{
+	struct bnxt_ucode_trailer *trailer;
+	u32 calculated_crc;
+	u32 stored_crc;
+	int rc = 0;
+
+	if (fw_size < sizeof(struct bnxt_ucode_trailer)) {
+		netdev_err(dev, "Invalid microcode file size: %u\n",
+			   (unsigned int)fw_size);
+		return -EINVAL;
+	}
+	trailer = (struct bnxt_ucode_trailer *)(fw_data + (fw_size -
+						sizeof(*trailer)));
+	if (trailer->sig != cpu_to_le32(BNXT_UCODE_TRAILER_SIGNATURE)) {
+		netdev_err(dev, "Invalid microcode trailer signature: %08X\n",
+			   le32_to_cpu(trailer->sig));
+		return -EINVAL;
+	}
+	if (le16_to_cpu(trailer->dir_type) != dir_type) {
+		netdev_err(dev, "Expected microcode type: %d, read: %d\n",
+			   dir_type, le16_to_cpu(trailer->dir_type));
+		return -EINVAL;
+	}
+	if (le16_to_cpu(trailer->trailer_length) <
+		sizeof(struct bnxt_ucode_trailer)) {
+		netdev_err(dev, "Invalid microcode trailer length: %d\n",
+			   le16_to_cpu(trailer->trailer_length));
+		return -EINVAL;
+	}
+
+	/* Confirm the CRC32 checksum of the file: */
+	stored_crc = le32_to_cpu(*(__le32 *)(fw_data + fw_size -
+					     sizeof(stored_crc)));
+	calculated_crc = ~crc32(~0, fw_data, fw_size - sizeof(stored_crc));
+	if (calculated_crc != stored_crc) {
+		netdev_err(dev,
+			   "CRC32 (%08lX) does not match calculated: %08lX\n",
+			   (unsigned long)stored_crc,
+			   (unsigned long)calculated_crc);
+		return -EINVAL;
+	}
+	rc = bnxt_flash_nvram(dev, dir_type, BNX_DIR_ORDINAL_FIRST,
+			      0, 0, fw_data, fw_size);
+
+	return rc;
+}
+
 static bool bnxt_dir_type_is_ape_bin_format(u16 dir_type)
 {
 	switch (dir_type) {
@@ -1206,7 +1264,7 @@ static bool bnxt_dir_type_is_ape_bin_format(u16 dir_type)
 	return false;
 }
 
-static bool bnxt_dir_type_is_unprotected_exec_format(u16 dir_type)
+static bool bnxt_dir_type_is_other_exec_format(u16 dir_type)
 {
 	switch (dir_type) {
 	case BNX_DIR_TYPE_AVS:
@@ -1227,7 +1285,7 @@ static bool bnxt_dir_type_is_unprotected_exec_format(u16 dir_type)
 static bool bnxt_dir_type_is_executable(u16 dir_type)
 {
 	return bnxt_dir_type_is_ape_bin_format(dir_type) ||
-		bnxt_dir_type_is_unprotected_exec_format(dir_type);
+		bnxt_dir_type_is_other_exec_format(dir_type);
 }
 
 static int bnxt_flash_firmware_from_file(struct net_device *dev,
@@ -1237,10 +1295,6 @@ static int bnxt_flash_firmware_from_file(struct net_device *dev,
 	const struct firmware  *fw;
 	int			rc;
 
-	if (dir_type != BNX_DIR_TYPE_UPDATE &&
-	    bnxt_dir_type_is_executable(dir_type) == false)
-		return -EINVAL;
-
 	rc = request_firmware(&fw, filename, &dev->dev);
 	if (rc != 0) {
 		netdev_err(dev, "Error %d requesting firmware file: %s\n",
@@ -1249,6 +1303,8 @@ static int bnxt_flash_firmware_from_file(struct net_device *dev,
 	}
 	if (bnxt_dir_type_is_ape_bin_format(dir_type) == true)
 		rc = bnxt_flash_firmware(dev, dir_type, fw->data, fw->size);
+	else if (bnxt_dir_type_is_other_exec_format(dir_type) == true)
+		rc = bnxt_flash_microcode(dev, dir_type, fw->data, fw->size);
 	else
 		rc = bnxt_flash_nvram(dev, dir_type, BNX_DIR_ORDINAL_FIRST,
 				      0, 0, fw->data, fw->size);
@@ -1257,10 +1313,83 @@ static int bnxt_flash_firmware_from_file(struct net_device *dev,
 }
 
 static int bnxt_flash_package_from_file(struct net_device *dev,
-					char *filename)
+					char *filename, u32 install_type)
 {
-	netdev_err(dev, "packages are not yet supported\n");
-	return -EINVAL;
+	struct bnxt *bp = netdev_priv(dev);
+	struct hwrm_nvm_install_update_output *resp = bp->hwrm_cmd_resp_addr;
+	struct hwrm_nvm_install_update_input install = {0};
+	const struct firmware *fw;
+	u32 item_len;
+	u16 index;
+	int rc;
+
+	bnxt_hwrm_fw_set_time(bp);
+
+	if (bnxt_find_nvram_item(dev, BNX_DIR_TYPE_UPDATE,
+				 BNX_DIR_ORDINAL_FIRST, BNX_DIR_EXT_NONE,
+				 &index, &item_len, NULL) != 0) {
+		netdev_err(dev, "PKG update area not created in nvram\n");
+		return -ENOBUFS;
+	}
+
+	rc = request_firmware(&fw, filename, &dev->dev);
+	if (rc != 0) {
+		netdev_err(dev, "PKG error %d requesting file: %s\n",
+			   rc, filename);
+		return rc;
+	}
+
+	if (fw->size > item_len) {
+		netdev_err(dev, "PKG insufficient update area in nvram: %lu",
+			   (unsigned long)fw->size);
+		rc = -EFBIG;
+	} else {
+		dma_addr_t dma_handle;
+		u8 *kmem;
+		struct hwrm_nvm_modify_input modify = {0};
+
+		bnxt_hwrm_cmd_hdr_init(bp, &modify, HWRM_NVM_MODIFY, -1, -1);
+
+		modify.dir_idx = cpu_to_le16(index);
+		modify.len = cpu_to_le32(fw->size);
+
+		kmem = dma_alloc_coherent(&bp->pdev->dev, fw->size,
+					  &dma_handle, GFP_KERNEL);
+		if (!kmem) {
+			netdev_err(dev,
+				   "dma_alloc_coherent failure, length = %u\n",
+				   (unsigned int)fw->size);
+			rc = -ENOMEM;
+		} else {
+			memcpy(kmem, fw->data, fw->size);
+			modify.host_src_addr = cpu_to_le64(dma_handle);
+
+			rc = hwrm_send_message(bp, &modify, sizeof(modify),
+					       FLASH_PACKAGE_TIMEOUT);
+			dma_free_coherent(&bp->pdev->dev, fw->size, kmem,
+					  dma_handle);
+		}
+	}
+	release_firmware(fw);
+	if (rc)
+		return rc;
+
+	if ((install_type & 0xffff) == 0)
+		install_type >>= 16;
+	bnxt_hwrm_cmd_hdr_init(bp, &install, HWRM_NVM_INSTALL_UPDATE, -1, -1);
+	install.install_type = cpu_to_le32(install_type);
+
+	rc = hwrm_send_message(bp, &install, sizeof(install),
+			       INSTALL_PACKAGE_TIMEOUT);
+	if (rc)
+		return -EOPNOTSUPP;
+
+	if (resp->result) {
+		netdev_err(dev, "PKG install error = %d, problem_item = %d\n",
+			   (s8)resp->result, (int)resp->problem_item);
+		return -ENOPKG;
+	}
+	return 0;
 }
 
 static int bnxt_flash_device(struct net_device *dev,
@@ -1271,8 +1400,10 @@ static int bnxt_flash_device(struct net_device *dev,
 		return -EINVAL;
 	}
 
-	if (flash->region == ETHTOOL_FLASH_ALL_REGIONS)
-		return bnxt_flash_package_from_file(dev, flash->data);
+	if (flash->region == ETHTOOL_FLASH_ALL_REGIONS ||
+	    flash->region > 0xffff)
+		return bnxt_flash_package_from_file(dev, flash->data,
+						    flash->region);
 
 	return bnxt_flash_firmware_from_file(dev, flash->region, flash->data);
 }
@@ -1516,7 +1647,7 @@ static int bnxt_set_eeprom(struct net_device *dev,
 
 	/* Create or re-write an NVM item: */
 	if (bnxt_dir_type_is_executable(type) == true)
-		return -EINVAL;
+		return -EOPNOTSUPP;
 	ext = eeprom->magic & 0xffff;
 	ordinal = eeprom->offset >> 16;
 	attr = eeprom->offset & 0xffff;
@@ -1718,6 +1849,25 @@ static int bnxt_get_module_eeprom(struct net_device *dev,
 	return rc;
 }
 
+static int bnxt_nway_reset(struct net_device *dev)
+{
+	int rc = 0;
+
+	struct bnxt *bp = netdev_priv(dev);
+	struct bnxt_link_info *link_info = &bp->link_info;
+
+	if (!BNXT_SINGLE_PF(bp))
+		return -EOPNOTSUPP;
+
+	if (!(link_info->autoneg & BNXT_AUTONEG_SPEED))
+		return -EINVAL;
+
+	if (netif_running(dev))
+		rc = bnxt_hwrm_set_link_setting(bp, true, false);
+
+	return rc;
+}
+
 const struct ethtool_ops bnxt_ethtool_ops = {
 	.get_link_ksettings	= bnxt_get_link_ksettings,
 	.set_link_ksettings	= bnxt_set_link_ksettings,
@@ -1750,4 +1900,5 @@ const struct ethtool_ops bnxt_ethtool_ops = {
 	.set_eee		= bnxt_set_eee,
 	.get_module_info	= bnxt_get_module_info,
 	.get_module_eeprom	= bnxt_get_module_eeprom,
+	.nway_reset		= bnxt_nway_reset
 };
diff --git a/drivers/net/ethernet/broadcom/bnxt/bnxt_fw_hdr.h b/drivers/net/ethernet/broadcom/bnxt/bnxt_fw_hdr.h
index 82bf44ab811b..cad30ddc6936 100644
--- a/drivers/net/ethernet/broadcom/bnxt/bnxt_fw_hdr.h
+++ b/drivers/net/ethernet/broadcom/bnxt/bnxt_fw_hdr.h
@@ -11,6 +11,7 @@
 #define __BNXT_FW_HDR_H__
 
 #define BNXT_FIRMWARE_BIN_SIGNATURE     0x1a4d4342	/* "BCM"+0x1a */
+#define BNXT_UCODE_TRAILER_SIGNATURE	0x726c7254	/* "Trlr" */
 
 enum SUPPORTED_FAMILY {
 	DEVICE_5702_3_4_FAMILY,		/* 0  - Denali, Vinson, K2 */
@@ -85,7 +86,7 @@ enum SUPPORTED_MEDIA {
 
 struct bnxt_fw_header {
 	__le32 signature;	/* constains the constant value of
-				 * BNXT_Firmware_Bin_Signatures
+				 * BNXT_FIRMWARE_BIN_SIGNATURE
 				 */
 	u8 flags;		/* reserved for ChiMP use */
 	u8 code_type;		/* enum SUPPORTED_CODE */
@@ -102,4 +103,17 @@ struct bnxt_fw_header {
 	u8 major_ver;
 };
 
+/* Microcode and pre-boot software/firmware trailer: */
+struct bnxt_ucode_trailer {
+	u8 rsa_sig[256];
+	__le16 flags;
+	u8 version_format;
+	u8 version_length;
+	u8 version[16];
+	__le16 dir_type;
+	__le16 trailer_length;
+	__le32 sig;		/* BNXT_UCODE_TRAILER_SIGNATURE */
+	__le32 chksum;		/* CRC-32 */
+};
+
 #endif
diff --git a/drivers/net/ethernet/broadcom/bnxt/bnxt_hsi.h b/drivers/net/ethernet/broadcom/bnxt/bnxt_hsi.h
index 517567f6d651..04a96cc3498a 100644
--- a/drivers/net/ethernet/broadcom/bnxt/bnxt_hsi.h
+++ b/drivers/net/ethernet/broadcom/bnxt/bnxt_hsi.h
@@ -39,7 +39,7 @@ struct eject_cmpl {
 	__le16 type;
 	#define EJECT_CMPL_TYPE_MASK				    0x3fUL
 	#define EJECT_CMPL_TYPE_SFT				    0
-	#define EJECT_CMPL_TYPE_STAT_EJECT			   (0x1aUL << 0)
+	#define EJECT_CMPL_TYPE_STAT_EJECT			   0x1aUL
 	__le16 len;
 	__le32 opaque;
 	__le32 v;
@@ -52,7 +52,7 @@ struct hwrm_cmpl {
 	__le16 type;
 	#define HWRM_CMPL_TYPE_MASK				    0x3fUL
 	#define HWRM_CMPL_TYPE_SFT				    0
-	#define HWRM_CMPL_TYPE_HWRM_DONE			   (0x20UL << 0)
+	#define HWRM_CMPL_TYPE_HWRM_DONE			   0x20UL
 	__le16 sequence_id;
 	__le32 unused_1;
 	__le32 v;
@@ -65,7 +65,7 @@ struct hwrm_fwd_req_cmpl {
 	__le16 req_len_type;
 	#define HWRM_FWD_REQ_CMPL_TYPE_MASK			    0x3fUL
 	#define HWRM_FWD_REQ_CMPL_TYPE_SFT			    0
-	#define HWRM_FWD_REQ_CMPL_TYPE_HWRM_FWD_REQ		   (0x22UL << 0)
+	#define HWRM_FWD_REQ_CMPL_TYPE_HWRM_FWD_REQ		   0x22UL
 	#define HWRM_FWD_REQ_CMPL_REQ_LEN_MASK			    0xffc0UL
 	#define HWRM_FWD_REQ_CMPL_REQ_LEN_SFT			    6
 	__le16 source_id;
@@ -81,7 +81,7 @@ struct hwrm_fwd_resp_cmpl {
 	__le16 type;
 	#define HWRM_FWD_RESP_CMPL_TYPE_MASK			    0x3fUL
 	#define HWRM_FWD_RESP_CMPL_TYPE_SFT			    0
-	#define HWRM_FWD_RESP_CMPL_TYPE_HWRM_FWD_RESP		   (0x24UL << 0)
+	#define HWRM_FWD_RESP_CMPL_TYPE_HWRM_FWD_RESP		   0x24UL
 	__le16 source_id;
 	__le16 resp_len;
 	__le16 unused_1;
@@ -96,25 +96,26 @@ struct hwrm_async_event_cmpl {
 	__le16 type;
 	#define HWRM_ASYNC_EVENT_CMPL_TYPE_MASK		    0x3fUL
 	#define HWRM_ASYNC_EVENT_CMPL_TYPE_SFT			    0
-	#define HWRM_ASYNC_EVENT_CMPL_TYPE_HWRM_ASYNC_EVENT       (0x2eUL << 0)
+	#define HWRM_ASYNC_EVENT_CMPL_TYPE_HWRM_ASYNC_EVENT       0x2eUL
 	__le16 event_id;
-	#define HWRM_ASYNC_EVENT_CMPL_EVENT_ID_LINK_STATUS_CHANGE (0x0UL << 0)
-	#define HWRM_ASYNC_EVENT_CMPL_EVENT_ID_LINK_MTU_CHANGE    (0x1UL << 0)
-	#define HWRM_ASYNC_EVENT_CMPL_EVENT_ID_LINK_SPEED_CHANGE  (0x2UL << 0)
-	#define HWRM_ASYNC_EVENT_CMPL_EVENT_ID_DCB_CONFIG_CHANGE  (0x3UL << 0)
-	#define HWRM_ASYNC_EVENT_CMPL_EVENT_ID_PORT_CONN_NOT_ALLOWED (0x4UL << 0)
-	#define HWRM_ASYNC_EVENT_CMPL_EVENT_ID_LINK_SPEED_CFG_NOT_ALLOWED (0x5UL << 0)
-	#define HWRM_ASYNC_EVENT_CMPL_EVENT_ID_LINK_SPEED_CFG_CHANGE (0x6UL << 0)
-	#define HWRM_ASYNC_EVENT_CMPL_EVENT_ID_PORT_PHY_CFG_CHANGE (0x7UL << 0)
-	#define HWRM_ASYNC_EVENT_CMPL_EVENT_ID_FUNC_DRVR_UNLOAD   (0x10UL << 0)
-	#define HWRM_ASYNC_EVENT_CMPL_EVENT_ID_FUNC_DRVR_LOAD     (0x11UL << 0)
-	#define HWRM_ASYNC_EVENT_CMPL_EVENT_ID_PF_DRVR_UNLOAD     (0x20UL << 0)
-	#define HWRM_ASYNC_EVENT_CMPL_EVENT_ID_PF_DRVR_LOAD       (0x21UL << 0)
-	#define HWRM_ASYNC_EVENT_CMPL_EVENT_ID_VF_FLR		   (0x30UL << 0)
-	#define HWRM_ASYNC_EVENT_CMPL_EVENT_ID_VF_MAC_ADDR_CHANGE (0x31UL << 0)
-	#define HWRM_ASYNC_EVENT_CMPL_EVENT_ID_PF_VF_COMM_STATUS_CHANGE (0x32UL << 0)
-	#define HWRM_ASYNC_EVENT_CMPL_EVENT_ID_VF_CFG_CHANGE      (0x33UL << 0)
-	#define HWRM_ASYNC_EVENT_CMPL_EVENT_ID_HWRM_ERROR	   (0xffUL << 0)
+	#define HWRM_ASYNC_EVENT_CMPL_EVENT_ID_LINK_STATUS_CHANGE 0x0UL
+	#define HWRM_ASYNC_EVENT_CMPL_EVENT_ID_LINK_MTU_CHANGE    0x1UL
+	#define HWRM_ASYNC_EVENT_CMPL_EVENT_ID_LINK_SPEED_CHANGE  0x2UL
+	#define HWRM_ASYNC_EVENT_CMPL_EVENT_ID_DCB_CONFIG_CHANGE  0x3UL
+	#define HWRM_ASYNC_EVENT_CMPL_EVENT_ID_PORT_CONN_NOT_ALLOWED 0x4UL
+	#define HWRM_ASYNC_EVENT_CMPL_EVENT_ID_LINK_SPEED_CFG_NOT_ALLOWED 0x5UL
+	#define HWRM_ASYNC_EVENT_CMPL_EVENT_ID_LINK_SPEED_CFG_CHANGE 0x6UL
+	#define HWRM_ASYNC_EVENT_CMPL_EVENT_ID_PORT_PHY_CFG_CHANGE 0x7UL
+	#define HWRM_ASYNC_EVENT_CMPL_EVENT_ID_FUNC_DRVR_UNLOAD   0x10UL
+	#define HWRM_ASYNC_EVENT_CMPL_EVENT_ID_FUNC_DRVR_LOAD     0x11UL
+	#define HWRM_ASYNC_EVENT_CMPL_EVENT_ID_FUNC_FLR_PROC_CMPLT 0x12UL
+	#define HWRM_ASYNC_EVENT_CMPL_EVENT_ID_PF_DRVR_UNLOAD     0x20UL
+	#define HWRM_ASYNC_EVENT_CMPL_EVENT_ID_PF_DRVR_LOAD       0x21UL
+	#define HWRM_ASYNC_EVENT_CMPL_EVENT_ID_VF_FLR		   0x30UL
+	#define HWRM_ASYNC_EVENT_CMPL_EVENT_ID_VF_MAC_ADDR_CHANGE 0x31UL
+	#define HWRM_ASYNC_EVENT_CMPL_EVENT_ID_PF_VF_COMM_STATUS_CHANGE 0x32UL
+	#define HWRM_ASYNC_EVENT_CMPL_EVENT_ID_VF_CFG_CHANGE      0x33UL
+	#define HWRM_ASYNC_EVENT_CMPL_EVENT_ID_HWRM_ERROR	   0xffUL
 	__le32 event_data2;
 	u8 opaque_v;
 	#define HWRM_ASYNC_EVENT_CMPL_V			    0x1UL
@@ -130,9 +131,9 @@ struct hwrm_async_event_cmpl_link_status_change {
 	__le16 type;
 	#define HWRM_ASYNC_EVENT_CMPL_LINK_STATUS_CHANGE_TYPE_MASK 0x3fUL
 	#define HWRM_ASYNC_EVENT_CMPL_LINK_STATUS_CHANGE_TYPE_SFT  0
-	#define HWRM_ASYNC_EVENT_CMPL_LINK_STATUS_CHANGE_TYPE_HWRM_ASYNC_EVENT (0x2eUL << 0)
+	#define HWRM_ASYNC_EVENT_CMPL_LINK_STATUS_CHANGE_TYPE_HWRM_ASYNC_EVENT 0x2eUL
 	__le16 event_id;
-	#define HWRM_ASYNC_EVENT_CMPL_LINK_STATUS_CHANGE_EVENT_ID_LINK_STATUS_CHANGE (0x0UL << 0)
+	#define HWRM_ASYNC_EVENT_CMPL_LINK_STATUS_CHANGE_EVENT_ID_LINK_STATUS_CHANGE 0x0UL
 	__le32 event_data2;
 	u8 opaque_v;
 	#define HWRM_ASYNC_EVENT_CMPL_LINK_STATUS_CHANGE_V	    0x1UL
@@ -156,9 +157,9 @@ struct hwrm_async_event_cmpl_link_mtu_change {
 	__le16 type;
 	#define HWRM_ASYNC_EVENT_CMPL_LINK_MTU_CHANGE_TYPE_MASK    0x3fUL
 	#define HWRM_ASYNC_EVENT_CMPL_LINK_MTU_CHANGE_TYPE_SFT     0
-	#define HWRM_ASYNC_EVENT_CMPL_LINK_MTU_CHANGE_TYPE_HWRM_ASYNC_EVENT (0x2eUL << 0)
+	#define HWRM_ASYNC_EVENT_CMPL_LINK_MTU_CHANGE_TYPE_HWRM_ASYNC_EVENT 0x2eUL
 	__le16 event_id;
-	#define HWRM_ASYNC_EVENT_CMPL_LINK_MTU_CHANGE_EVENT_ID_LINK_MTU_CHANGE (0x1UL << 0)
+	#define HWRM_ASYNC_EVENT_CMPL_LINK_MTU_CHANGE_EVENT_ID_LINK_MTU_CHANGE 0x1UL
 	__le32 event_data2;
 	u8 opaque_v;
 	#define HWRM_ASYNC_EVENT_CMPL_LINK_MTU_CHANGE_V	    0x1UL
@@ -176,9 +177,9 @@ struct hwrm_async_event_cmpl_link_speed_change {
 	__le16 type;
 	#define HWRM_ASYNC_EVENT_CMPL_LINK_SPEED_CHANGE_TYPE_MASK  0x3fUL
 	#define HWRM_ASYNC_EVENT_CMPL_LINK_SPEED_CHANGE_TYPE_SFT   0
-	#define HWRM_ASYNC_EVENT_CMPL_LINK_SPEED_CHANGE_TYPE_HWRM_ASYNC_EVENT (0x2eUL << 0)
+	#define HWRM_ASYNC_EVENT_CMPL_LINK_SPEED_CHANGE_TYPE_HWRM_ASYNC_EVENT 0x2eUL
 	__le16 event_id;
-	#define HWRM_ASYNC_EVENT_CMPL_LINK_SPEED_CHANGE_EVENT_ID_LINK_SPEED_CHANGE (0x2UL << 0)
+	#define HWRM_ASYNC_EVENT_CMPL_LINK_SPEED_CHANGE_EVENT_ID_LINK_SPEED_CHANGE 0x2UL
 	__le32 event_data2;
 	u8 opaque_v;
 	#define HWRM_ASYNC_EVENT_CMPL_LINK_SPEED_CHANGE_V	    0x1UL
@@ -200,8 +201,7 @@ struct hwrm_async_event_cmpl_link_speed_change {
 	#define HWRM_ASYNC_EVENT_CMPL_LINK_SPEED_CHANGE_EVENT_DATA1_NEW_LINK_SPEED_100MBPS_40GB (0x190UL << 1)
 	#define HWRM_ASYNC_EVENT_CMPL_LINK_SPEED_CHANGE_EVENT_DATA1_NEW_LINK_SPEED_100MBPS_50GB (0x1f4UL << 1)
 	#define HWRM_ASYNC_EVENT_CMPL_LINK_SPEED_CHANGE_EVENT_DATA1_NEW_LINK_SPEED_100MBPS_100GB (0x3e8UL << 1)
-	#define HWRM_ASYNC_EVENT_CMPL_LINK_SPEED_CHANGE_EVENT_DATA1_NEW_LINK_SPEED_100MBPS_10MB (0xffffUL << 1)
-	#define HWRM_ASYNC_EVENT_CMPL_LINK_SPEED_CHANGE_EVENT_DATA1_NEW_LINK_SPEED_100MBPS_LAST    HWRM_ASYNC_EVENT_CMPL_LINK_SPEED_CHANGE_EVENT_DATA1_NEW_LINK_SPEED_100MBPS_10MB
+	#define HWRM_ASYNC_EVENT_CMPL_LINK_SPEED_CHANGE_EVENT_DATA1_NEW_LINK_SPEED_100MBPS_LAST    HWRM_ASYNC_EVENT_CMPL_LINK_SPEED_CHANGE_EVENT_DATA1_NEW_LINK_SPEED_100MBPS_100GB
 	#define HWRM_ASYNC_EVENT_CMPL_LINK_SPEED_CHANGE_EVENT_DATA1_PORT_ID_MASK 0xffff0000UL
 	#define HWRM_ASYNC_EVENT_CMPL_LINK_SPEED_CHANGE_EVENT_DATA1_PORT_ID_SFT 16
 };
@@ -211,9 +211,9 @@ struct hwrm_async_event_cmpl_dcb_config_change {
 	__le16 type;
 	#define HWRM_ASYNC_EVENT_CMPL_DCB_CONFIG_CHANGE_TYPE_MASK  0x3fUL
 	#define HWRM_ASYNC_EVENT_CMPL_DCB_CONFIG_CHANGE_TYPE_SFT   0
-	#define HWRM_ASYNC_EVENT_CMPL_DCB_CONFIG_CHANGE_TYPE_HWRM_ASYNC_EVENT (0x2eUL << 0)
+	#define HWRM_ASYNC_EVENT_CMPL_DCB_CONFIG_CHANGE_TYPE_HWRM_ASYNC_EVENT 0x2eUL
 	__le16 event_id;
-	#define HWRM_ASYNC_EVENT_CMPL_DCB_CONFIG_CHANGE_EVENT_ID_DCB_CONFIG_CHANGE (0x3UL << 0)
+	#define HWRM_ASYNC_EVENT_CMPL_DCB_CONFIG_CHANGE_EVENT_ID_DCB_CONFIG_CHANGE 0x3UL
 	__le32 event_data2;
 	u8 opaque_v;
 	#define HWRM_ASYNC_EVENT_CMPL_DCB_CONFIG_CHANGE_V	    0x1UL
@@ -231,9 +231,9 @@ struct hwrm_async_event_cmpl_port_conn_not_allowed {
 	__le16 type;
 	#define HWRM_ASYNC_EVENT_CMPL_PORT_CONN_NOT_ALLOWED_TYPE_MASK 0x3fUL
 	#define HWRM_ASYNC_EVENT_CMPL_PORT_CONN_NOT_ALLOWED_TYPE_SFT 0
-	#define HWRM_ASYNC_EVENT_CMPL_PORT_CONN_NOT_ALLOWED_TYPE_HWRM_ASYNC_EVENT (0x2eUL << 0)
+	#define HWRM_ASYNC_EVENT_CMPL_PORT_CONN_NOT_ALLOWED_TYPE_HWRM_ASYNC_EVENT 0x2eUL
 	__le16 event_id;
-	#define HWRM_ASYNC_EVENT_CMPL_PORT_CONN_NOT_ALLOWED_EVENT_ID_PORT_CONN_NOT_ALLOWED (0x4UL << 0)
+	#define HWRM_ASYNC_EVENT_CMPL_PORT_CONN_NOT_ALLOWED_EVENT_ID_PORT_CONN_NOT_ALLOWED 0x4UL
 	__le32 event_data2;
 	u8 opaque_v;
 	#define HWRM_ASYNC_EVENT_CMPL_PORT_CONN_NOT_ALLOWED_V      0x1UL
@@ -258,9 +258,9 @@ struct hwrm_async_event_cmpl_link_speed_cfg_not_allowed {
 	__le16 type;
 	#define HWRM_ASYNC_EVENT_CMPL_LINK_SPEED_CFG_NOT_ALLOWED_TYPE_MASK 0x3fUL
 	#define HWRM_ASYNC_EVENT_CMPL_LINK_SPEED_CFG_NOT_ALLOWED_TYPE_SFT 0
-	#define HWRM_ASYNC_EVENT_CMPL_LINK_SPEED_CFG_NOT_ALLOWED_TYPE_HWRM_ASYNC_EVENT (0x2eUL << 0)
+	#define HWRM_ASYNC_EVENT_CMPL_LINK_SPEED_CFG_NOT_ALLOWED_TYPE_HWRM_ASYNC_EVENT 0x2eUL
 	__le16 event_id;
-	#define HWRM_ASYNC_EVENT_CMPL_LINK_SPEED_CFG_NOT_ALLOWED_EVENT_ID_LINK_SPEED_CFG_NOT_ALLOWED (0x5UL << 0)
+	#define HWRM_ASYNC_EVENT_CMPL_LINK_SPEED_CFG_NOT_ALLOWED_EVENT_ID_LINK_SPEED_CFG_NOT_ALLOWED 0x5UL
 	__le32 event_data2;
 	u8 opaque_v;
 	#define HWRM_ASYNC_EVENT_CMPL_LINK_SPEED_CFG_NOT_ALLOWED_V 0x1UL
@@ -278,9 +278,9 @@ struct hwrm_async_event_cmpl_link_speed_cfg_change {
 	__le16 type;
 	#define HWRM_ASYNC_EVENT_CMPL_LINK_SPEED_CFG_CHANGE_TYPE_MASK 0x3fUL
 	#define HWRM_ASYNC_EVENT_CMPL_LINK_SPEED_CFG_CHANGE_TYPE_SFT 0
-	#define HWRM_ASYNC_EVENT_CMPL_LINK_SPEED_CFG_CHANGE_TYPE_HWRM_ASYNC_EVENT (0x2eUL << 0)
+	#define HWRM_ASYNC_EVENT_CMPL_LINK_SPEED_CFG_CHANGE_TYPE_HWRM_ASYNC_EVENT 0x2eUL
 	__le16 event_id;
-	#define HWRM_ASYNC_EVENT_CMPL_LINK_SPEED_CFG_CHANGE_EVENT_ID_LINK_SPEED_CFG_CHANGE (0x6UL << 0)
+	#define HWRM_ASYNC_EVENT_CMPL_LINK_SPEED_CFG_CHANGE_EVENT_ID_LINK_SPEED_CFG_CHANGE 0x6UL
 	__le32 event_data2;
 	u8 opaque_v;
 	#define HWRM_ASYNC_EVENT_CMPL_LINK_SPEED_CFG_CHANGE_V      0x1UL
@@ -300,9 +300,9 @@ struct hwrm_async_event_cmpl_func_drvr_unload {
 	__le16 type;
 	#define HWRM_ASYNC_EVENT_CMPL_FUNC_DRVR_UNLOAD_TYPE_MASK   0x3fUL
 	#define HWRM_ASYNC_EVENT_CMPL_FUNC_DRVR_UNLOAD_TYPE_SFT    0
-	#define HWRM_ASYNC_EVENT_CMPL_FUNC_DRVR_UNLOAD_TYPE_HWRM_ASYNC_EVENT (0x2eUL << 0)
+	#define HWRM_ASYNC_EVENT_CMPL_FUNC_DRVR_UNLOAD_TYPE_HWRM_ASYNC_EVENT 0x2eUL
 	__le16 event_id;
-	#define HWRM_ASYNC_EVENT_CMPL_FUNC_DRVR_UNLOAD_EVENT_ID_FUNC_DRVR_UNLOAD (0x10UL << 0)
+	#define HWRM_ASYNC_EVENT_CMPL_FUNC_DRVR_UNLOAD_EVENT_ID_FUNC_DRVR_UNLOAD 0x10UL
 	__le32 event_data2;
 	u8 opaque_v;
 	#define HWRM_ASYNC_EVENT_CMPL_FUNC_DRVR_UNLOAD_V	    0x1UL
@@ -320,9 +320,9 @@ struct hwrm_async_event_cmpl_func_drvr_load {
 	__le16 type;
 	#define HWRM_ASYNC_EVENT_CMPL_FUNC_DRVR_LOAD_TYPE_MASK     0x3fUL
 	#define HWRM_ASYNC_EVENT_CMPL_FUNC_DRVR_LOAD_TYPE_SFT      0
-	#define HWRM_ASYNC_EVENT_CMPL_FUNC_DRVR_LOAD_TYPE_HWRM_ASYNC_EVENT (0x2eUL << 0)
+	#define HWRM_ASYNC_EVENT_CMPL_FUNC_DRVR_LOAD_TYPE_HWRM_ASYNC_EVENT 0x2eUL
 	__le16 event_id;
-	#define HWRM_ASYNC_EVENT_CMPL_FUNC_DRVR_LOAD_EVENT_ID_FUNC_DRVR_LOAD (0x11UL << 0)
+	#define HWRM_ASYNC_EVENT_CMPL_FUNC_DRVR_LOAD_EVENT_ID_FUNC_DRVR_LOAD 0x11UL
 	__le32 event_data2;
 	u8 opaque_v;
 	#define HWRM_ASYNC_EVENT_CMPL_FUNC_DRVR_LOAD_V		    0x1UL
@@ -340,9 +340,9 @@ struct hwrm_async_event_cmpl_pf_drvr_unload {
 	__le16 type;
 	#define HWRM_ASYNC_EVENT_CMPL_PF_DRVR_UNLOAD_TYPE_MASK     0x3fUL
 	#define HWRM_ASYNC_EVENT_CMPL_PF_DRVR_UNLOAD_TYPE_SFT      0
-	#define HWRM_ASYNC_EVENT_CMPL_PF_DRVR_UNLOAD_TYPE_HWRM_ASYNC_EVENT (0x2eUL << 0)
+	#define HWRM_ASYNC_EVENT_CMPL_PF_DRVR_UNLOAD_TYPE_HWRM_ASYNC_EVENT 0x2eUL
 	__le16 event_id;
-	#define HWRM_ASYNC_EVENT_CMPL_PF_DRVR_UNLOAD_EVENT_ID_PF_DRVR_UNLOAD (0x20UL << 0)
+	#define HWRM_ASYNC_EVENT_CMPL_PF_DRVR_UNLOAD_EVENT_ID_PF_DRVR_UNLOAD 0x20UL
 	__le32 event_data2;
 	u8 opaque_v;
 	#define HWRM_ASYNC_EVENT_CMPL_PF_DRVR_UNLOAD_V		    0x1UL
@@ -362,9 +362,9 @@ struct hwrm_async_event_cmpl_pf_drvr_load {
 	__le16 type;
 	#define HWRM_ASYNC_EVENT_CMPL_PF_DRVR_LOAD_TYPE_MASK       0x3fUL
 	#define HWRM_ASYNC_EVENT_CMPL_PF_DRVR_LOAD_TYPE_SFT	    0
-	#define HWRM_ASYNC_EVENT_CMPL_PF_DRVR_LOAD_TYPE_HWRM_ASYNC_EVENT (0x2eUL << 0)
+	#define HWRM_ASYNC_EVENT_CMPL_PF_DRVR_LOAD_TYPE_HWRM_ASYNC_EVENT 0x2eUL
 	__le16 event_id;
-	#define HWRM_ASYNC_EVENT_CMPL_PF_DRVR_LOAD_EVENT_ID_PF_DRVR_LOAD (0x21UL << 0)
+	#define HWRM_ASYNC_EVENT_CMPL_PF_DRVR_LOAD_EVENT_ID_PF_DRVR_LOAD 0x21UL
 	__le32 event_data2;
 	u8 opaque_v;
 	#define HWRM_ASYNC_EVENT_CMPL_PF_DRVR_LOAD_V		    0x1UL
@@ -384,9 +384,9 @@ struct hwrm_async_event_cmpl_vf_flr {
 	__le16 type;
 	#define HWRM_ASYNC_EVENT_CMPL_VF_FLR_TYPE_MASK		    0x3fUL
 	#define HWRM_ASYNC_EVENT_CMPL_VF_FLR_TYPE_SFT		    0
-	#define HWRM_ASYNC_EVENT_CMPL_VF_FLR_TYPE_HWRM_ASYNC_EVENT (0x2eUL << 0)
+	#define HWRM_ASYNC_EVENT_CMPL_VF_FLR_TYPE_HWRM_ASYNC_EVENT 0x2eUL
 	__le16 event_id;
-	#define HWRM_ASYNC_EVENT_CMPL_VF_FLR_EVENT_ID_VF_FLR      (0x30UL << 0)
+	#define HWRM_ASYNC_EVENT_CMPL_VF_FLR_EVENT_ID_VF_FLR      0x30UL
 	__le32 event_data2;
 	u8 opaque_v;
 	#define HWRM_ASYNC_EVENT_CMPL_VF_FLR_V			    0x1UL
@@ -404,9 +404,9 @@ struct hwrm_async_event_cmpl_vf_mac_addr_change {
 	__le16 type;
 	#define HWRM_ASYNC_EVENT_CMPL_VF_MAC_ADDR_CHANGE_TYPE_MASK 0x3fUL
 	#define HWRM_ASYNC_EVENT_CMPL_VF_MAC_ADDR_CHANGE_TYPE_SFT  0
-	#define HWRM_ASYNC_EVENT_CMPL_VF_MAC_ADDR_CHANGE_TYPE_HWRM_ASYNC_EVENT (0x2eUL << 0)
+	#define HWRM_ASYNC_EVENT_CMPL_VF_MAC_ADDR_CHANGE_TYPE_HWRM_ASYNC_EVENT 0x2eUL
 	__le16 event_id;
-	#define HWRM_ASYNC_EVENT_CMPL_VF_MAC_ADDR_CHANGE_EVENT_ID_VF_MAC_ADDR_CHANGE (0x31UL << 0)
+	#define HWRM_ASYNC_EVENT_CMPL_VF_MAC_ADDR_CHANGE_EVENT_ID_VF_MAC_ADDR_CHANGE 0x31UL
 	__le32 event_data2;
 	u8 opaque_v;
 	#define HWRM_ASYNC_EVENT_CMPL_VF_MAC_ADDR_CHANGE_V	    0x1UL
@@ -424,9 +424,9 @@ struct hwrm_async_event_cmpl_pf_vf_comm_status_change {
 	__le16 type;
 	#define HWRM_ASYNC_EVENT_CMPL_PF_VF_COMM_STATUS_CHANGE_TYPE_MASK 0x3fUL
 	#define HWRM_ASYNC_EVENT_CMPL_PF_VF_COMM_STATUS_CHANGE_TYPE_SFT 0
-	#define HWRM_ASYNC_EVENT_CMPL_PF_VF_COMM_STATUS_CHANGE_TYPE_HWRM_ASYNC_EVENT (0x2eUL << 0)
+	#define HWRM_ASYNC_EVENT_CMPL_PF_VF_COMM_STATUS_CHANGE_TYPE_HWRM_ASYNC_EVENT 0x2eUL
 	__le16 event_id;
-	#define HWRM_ASYNC_EVENT_CMPL_PF_VF_COMM_STATUS_CHANGE_EVENT_ID_PF_VF_COMM_STATUS_CHANGE (0x32UL << 0)
+	#define HWRM_ASYNC_EVENT_CMPL_PF_VF_COMM_STATUS_CHANGE_EVENT_ID_PF_VF_COMM_STATUS_CHANGE 0x32UL
 	__le32 event_data2;
 	u8 opaque_v;
 	#define HWRM_ASYNC_EVENT_CMPL_PF_VF_COMM_STATUS_CHANGE_V   0x1UL
@@ -443,9 +443,9 @@ struct hwrm_async_event_cmpl_vf_cfg_change {
 	__le16 type;
 	#define HWRM_ASYNC_EVENT_CMPL_VF_CFG_CHANGE_TYPE_MASK      0x3fUL
 	#define HWRM_ASYNC_EVENT_CMPL_VF_CFG_CHANGE_TYPE_SFT       0
-	#define HWRM_ASYNC_EVENT_CMPL_VF_CFG_CHANGE_TYPE_HWRM_ASYNC_EVENT (0x2eUL << 0)
+	#define HWRM_ASYNC_EVENT_CMPL_VF_CFG_CHANGE_TYPE_HWRM_ASYNC_EVENT 0x2eUL
 	__le16 event_id;
-	#define HWRM_ASYNC_EVENT_CMPL_VF_CFG_CHANGE_EVENT_ID_VF_CFG_CHANGE (0x33UL << 0)
+	#define HWRM_ASYNC_EVENT_CMPL_VF_CFG_CHANGE_EVENT_ID_VF_CFG_CHANGE 0x33UL
 	__le32 event_data2;
 	u8 opaque_v;
 	#define HWRM_ASYNC_EVENT_CMPL_VF_CFG_CHANGE_V		    0x1UL
@@ -465,15 +465,15 @@ struct hwrm_async_event_cmpl_hwrm_error {
 	__le16 type;
 	#define HWRM_ASYNC_EVENT_CMPL_HWRM_ERROR_TYPE_MASK	    0x3fUL
 	#define HWRM_ASYNC_EVENT_CMPL_HWRM_ERROR_TYPE_SFT	    0
-	#define HWRM_ASYNC_EVENT_CMPL_HWRM_ERROR_TYPE_HWRM_ASYNC_EVENT (0x2eUL << 0)
+	#define HWRM_ASYNC_EVENT_CMPL_HWRM_ERROR_TYPE_HWRM_ASYNC_EVENT 0x2eUL
 	__le16 event_id;
-	#define HWRM_ASYNC_EVENT_CMPL_HWRM_ERROR_EVENT_ID_HWRM_ERROR (0xffUL << 0)
+	#define HWRM_ASYNC_EVENT_CMPL_HWRM_ERROR_EVENT_ID_HWRM_ERROR 0xffUL
 	__le32 event_data2;
 	#define HWRM_ASYNC_EVENT_CMPL_HWRM_ERROR_EVENT_DATA2_SEVERITY_MASK 0xffUL
 	#define HWRM_ASYNC_EVENT_CMPL_HWRM_ERROR_EVENT_DATA2_SEVERITY_SFT 0
-	#define HWRM_ASYNC_EVENT_CMPL_HWRM_ERROR_EVENT_DATA2_SEVERITY_WARNING (0x0UL << 0)
-	#define HWRM_ASYNC_EVENT_CMPL_HWRM_ERROR_EVENT_DATA2_SEVERITY_NONFATAL (0x1UL << 0)
-	#define HWRM_ASYNC_EVENT_CMPL_HWRM_ERROR_EVENT_DATA2_SEVERITY_FATAL (0x2UL << 0)
+	#define HWRM_ASYNC_EVENT_CMPL_HWRM_ERROR_EVENT_DATA2_SEVERITY_WARNING 0x0UL
+	#define HWRM_ASYNC_EVENT_CMPL_HWRM_ERROR_EVENT_DATA2_SEVERITY_NONFATAL 0x1UL
+	#define HWRM_ASYNC_EVENT_CMPL_HWRM_ERROR_EVENT_DATA2_SEVERITY_FATAL 0x2UL
 	#define HWRM_ASYNC_EVENT_CMPL_HWRM_ERROR_EVENT_DATA2_SEVERITY_LAST    HWRM_ASYNC_EVENT_CMPL_HWRM_ERROR_EVENT_DATA2_SEVERITY_FATAL
 	u8 opaque_v;
 	#define HWRM_ASYNC_EVENT_CMPL_HWRM_ERROR_V		    0x1UL
@@ -485,12 +485,12 @@ struct hwrm_async_event_cmpl_hwrm_error {
 	#define HWRM_ASYNC_EVENT_CMPL_HWRM_ERROR_EVENT_DATA1_TIMESTAMP 0x1UL
 };
 
-/* HW Resource Manager Specification 1.3.0 */
+/* HW Resource Manager Specification 1.5.1 */
 #define HWRM_VERSION_MAJOR	1
-#define HWRM_VERSION_MINOR	3
-#define HWRM_VERSION_UPDATE	0
+#define HWRM_VERSION_MINOR	5
+#define HWRM_VERSION_UPDATE	1
 
-#define HWRM_VERSION_STR	"1.3.0"
+#define HWRM_VERSION_STR	"1.5.1"
 /*
  * Following is the signature for HWRM message field that indicates not
  * applicable (All F's). Need to cast it the size of the field if needed.
@@ -556,8 +556,8 @@ struct cmd_nums {
 	#define HWRM_QUEUE_QPORTCFG				   (0x30UL)
 	#define HWRM_QUEUE_QCFG				   (0x31UL)
 	#define HWRM_QUEUE_CFG					   (0x32UL)
-	#define HWRM_QUEUE_BUFFERS_QCFG			   (0x33UL)
-	#define HWRM_QUEUE_BUFFERS_CFG				   (0x34UL)
+	#define RESERVED2					   (0x33UL)
+	#define RESERVED3					   (0x34UL)
 	#define HWRM_QUEUE_PFCENABLE_QCFG			   (0x35UL)
 	#define HWRM_QUEUE_PFCENABLE_CFG			   (0x36UL)
 	#define HWRM_QUEUE_PRI2COS_QCFG			   (0x37UL)
@@ -574,6 +574,7 @@ struct cmd_nums {
 	#define HWRM_VNIC_RSS_QCFG				   (0x47UL)
 	#define HWRM_VNIC_PLCMODES_CFG				   (0x48UL)
 	#define HWRM_VNIC_PLCMODES_QCFG			   (0x49UL)
+	#define HWRM_VNIC_QCAPS				   (0x4aUL)
 	#define HWRM_RING_ALLOC				   (0x50UL)
 	#define HWRM_RING_FREE					   (0x51UL)
 	#define HWRM_RING_CMPL_RING_QAGGINT_PARAMS		   (0x52UL)
@@ -581,13 +582,15 @@ struct cmd_nums {
 	#define HWRM_RING_RESET				   (0x5eUL)
 	#define HWRM_RING_GRP_ALLOC				   (0x60UL)
 	#define HWRM_RING_GRP_FREE				   (0x61UL)
+	#define RESERVED5					   (0x64UL)
+	#define RESERVED6					   (0x65UL)
 	#define HWRM_VNIC_RSS_COS_LB_CTX_ALLOC			   (0x70UL)
 	#define HWRM_VNIC_RSS_COS_LB_CTX_FREE			   (0x71UL)
 	#define HWRM_CFA_L2_FILTER_ALLOC			   (0x90UL)
 	#define HWRM_CFA_L2_FILTER_FREE			   (0x91UL)
 	#define HWRM_CFA_L2_FILTER_CFG				   (0x92UL)
 	#define HWRM_CFA_L2_SET_RX_MASK			   (0x93UL)
-	#define RESERVED3					   (0x94UL)
+	#define RESERVED4					   (0x94UL)
 	#define HWRM_CFA_TUNNEL_FILTER_ALLOC			   (0x95UL)
 	#define HWRM_CFA_TUNNEL_FILTER_FREE			   (0x96UL)
 	#define HWRM_CFA_ENCAP_RECORD_ALLOC			   (0x97UL)
@@ -607,6 +610,8 @@ struct cmd_nums {
 	#define HWRM_STAT_CTX_CLR_STATS			   (0xb3UL)
 	#define HWRM_FW_RESET					   (0xc0UL)
 	#define HWRM_FW_QSTATUS				   (0xc1UL)
+	#define HWRM_FW_SET_TIME				   (0xc8UL)
+	#define HWRM_FW_GET_TIME				   (0xc9UL)
 	#define HWRM_EXEC_FWD_RESP				   (0xd0UL)
 	#define HWRM_REJECT_FWD_RESP				   (0xd1UL)
 	#define HWRM_FWD_RESP					   (0xd2UL)
@@ -615,11 +620,13 @@ struct cmd_nums {
 	#define HWRM_WOL_FILTER_ALLOC				   (0xf0UL)
 	#define HWRM_WOL_FILTER_FREE				   (0xf1UL)
 	#define HWRM_WOL_FILTER_QCFG				   (0xf2UL)
+	#define HWRM_WOL_REASON_QCFG				   (0xf3UL)
 	#define HWRM_DBG_READ_DIRECT				   (0xff10UL)
 	#define HWRM_DBG_READ_INDIRECT				   (0xff11UL)
 	#define HWRM_DBG_WRITE_DIRECT				   (0xff12UL)
 	#define HWRM_DBG_WRITE_INDIRECT			   (0xff13UL)
 	#define HWRM_DBG_DUMP					   (0xff14UL)
+	#define HWRM_NVM_INSTALL_UPDATE			   (0xfff3UL)
 	#define HWRM_NVM_MODIFY				   (0xfff4UL)
 	#define HWRM_NVM_VERIFY_UPDATE				   (0xfff5UL)
 	#define HWRM_NVM_GET_DEV_INFO				   (0xfff6UL)
@@ -824,7 +831,9 @@ struct hwrm_ver_get_output {
 	u8 netctrl_fw_min;
 	u8 netctrl_fw_bld;
 	u8 netctrl_fw_rsvd;
-	__le32 reserved1;
+	__le32 dev_caps_cfg;
+	#define VER_GET_RESP_DEV_CAPS_CFG_SECURE_FW_UPD_SUPPORTED  0x1UL
+	#define VER_GET_RESP_DEV_CAPS_CFG_FW_DCBX_AGENT_SUPPORTED  0x2UL
 	u8 roce_fw_maj;
 	u8 roce_fw_min;
 	u8 roce_fw_bld;
@@ -839,9 +848,9 @@ struct hwrm_ver_get_output {
 	u8 chip_metal;
 	u8 chip_bond_id;
 	u8 chip_platform_type;
-	#define VER_GET_RESP_CHIP_PLATFORM_TYPE_ASIC		   (0x0UL << 0)
-	#define VER_GET_RESP_CHIP_PLATFORM_TYPE_FPGA		   (0x1UL << 0)
-	#define VER_GET_RESP_CHIP_PLATFORM_TYPE_PALLADIUM	   (0x2UL << 0)
+	#define VER_GET_RESP_CHIP_PLATFORM_TYPE_ASIC		   0x0UL
+	#define VER_GET_RESP_CHIP_PLATFORM_TYPE_FPGA		   0x1UL
+	#define VER_GET_RESP_CHIP_PLATFORM_TYPE_PALLADIUM	   0x2UL
 	__le16 max_req_win_len;
 	__le16 max_resp_len;
 	__le16 def_req_timeout;
@@ -863,10 +872,10 @@ struct hwrm_func_reset_input {
 	#define FUNC_RESET_REQ_ENABLES_VF_ID_VALID		    0x1UL
 	__le16 vf_id;
 	u8 func_reset_level;
-	#define FUNC_RESET_REQ_FUNC_RESET_LEVEL_RESETALL	   (0x0UL << 0)
-	#define FUNC_RESET_REQ_FUNC_RESET_LEVEL_RESETME	   (0x1UL << 0)
-	#define FUNC_RESET_REQ_FUNC_RESET_LEVEL_RESETCHILDREN     (0x2UL << 0)
-	#define FUNC_RESET_REQ_FUNC_RESET_LEVEL_RESETVF	   (0x3UL << 0)
+	#define FUNC_RESET_REQ_FUNC_RESET_LEVEL_RESETALL	   0x0UL
+	#define FUNC_RESET_REQ_FUNC_RESET_LEVEL_RESETME	   0x1UL
+	#define FUNC_RESET_REQ_FUNC_RESET_LEVEL_RESETCHILDREN     0x2UL
+	#define FUNC_RESET_REQ_FUNC_RESET_LEVEL_RESETVF	   0x3UL
 	u8 unused_0;
 };
 
@@ -1028,6 +1037,10 @@ struct hwrm_func_qcaps_output {
 	#define FUNC_QCAPS_RESP_FLAGS_ROCE_V2_SUPPORTED	    0x10UL
 	#define FUNC_QCAPS_RESP_FLAGS_WOL_MAGICPKT_SUPPORTED       0x20UL
 	#define FUNC_QCAPS_RESP_FLAGS_WOL_BMP_SUPPORTED	    0x40UL
+	#define FUNC_QCAPS_RESP_FLAGS_TX_RING_RL_SUPPORTED	    0x80UL
+	#define FUNC_QCAPS_RESP_FLAGS_TX_BW_CFG_SUPPORTED	    0x100UL
+	#define FUNC_QCAPS_RESP_FLAGS_VF_TX_RING_RL_SUPPORTED      0x200UL
+	#define FUNC_QCAPS_RESP_FLAGS_VF_BW_CFG_SUPPORTED	    0x400UL
 	u8 mac_address[6];
 	__le16 max_rsscos_ctx;
 	__le16 max_cmpl_rings;
@@ -1047,9 +1060,8 @@ struct hwrm_func_qcaps_output {
 	__le32 max_mcast_filters;
 	__le32 max_flow_id;
 	__le32 max_hw_ring_grps;
+	__le16 max_sp_tx_rings;
 	u8 unused_0;
-	u8 unused_1;
-	u8 unused_2;
 	u8 valid;
 };
 
@@ -1077,6 +1089,7 @@ struct hwrm_func_qcfg_output {
 	__le16 flags;
 	#define FUNC_QCFG_RESP_FLAGS_OOB_WOL_MAGICPKT_ENABLED      0x1UL
 	#define FUNC_QCFG_RESP_FLAGS_OOB_WOL_BMP_ENABLED	    0x2UL
+	#define FUNC_QCFG_RESP_FLAGS_FW_DCBX_AGENT_ENABLED	    0x4UL
 	u8 mac_address[6];
 	__le16 pci_id;
 	__le16 alloc_rsscos_ctx;
@@ -1089,29 +1102,46 @@ struct hwrm_func_qcfg_output {
 	__le16 mru;
 	__le16 stat_ctx_id;
 	u8 port_partition_type;
-	#define FUNC_QCFG_RESP_PORT_PARTITION_TYPE_SPF		   (0x0UL << 0)
-	#define FUNC_QCFG_RESP_PORT_PARTITION_TYPE_MPFS	   (0x1UL << 0)
-	#define FUNC_QCFG_RESP_PORT_PARTITION_TYPE_NPAR1_0	   (0x2UL << 0)
-	#define FUNC_QCFG_RESP_PORT_PARTITION_TYPE_NPAR1_5	   (0x3UL << 0)
-	#define FUNC_QCFG_RESP_PORT_PARTITION_TYPE_NPAR2_0	   (0x4UL << 0)
-	#define FUNC_QCFG_RESP_PORT_PARTITION_TYPE_UNKNOWN	   (0xffUL << 0)
+	#define FUNC_QCFG_RESP_PORT_PARTITION_TYPE_SPF		   0x0UL
+	#define FUNC_QCFG_RESP_PORT_PARTITION_TYPE_MPFS	   0x1UL
+	#define FUNC_QCFG_RESP_PORT_PARTITION_TYPE_NPAR1_0	   0x2UL
+	#define FUNC_QCFG_RESP_PORT_PARTITION_TYPE_NPAR1_5	   0x3UL
+	#define FUNC_QCFG_RESP_PORT_PARTITION_TYPE_NPAR2_0	   0x4UL
+	#define FUNC_QCFG_RESP_PORT_PARTITION_TYPE_UNKNOWN	   0xffUL
 	u8 unused_0;
 	__le16 dflt_vnic_id;
 	u8 unused_1;
 	u8 unused_2;
 	__le32 min_bw;
+	#define FUNC_QCFG_RESP_MIN_BW_BW_VALUE_MASK		    0xfffffffUL
+	#define FUNC_QCFG_RESP_MIN_BW_BW_VALUE_SFT		    0
+	#define FUNC_QCFG_RESP_MIN_BW_RSVD			    0x10000000UL
+	#define FUNC_QCFG_RESP_MIN_BW_BW_VALUE_UNIT_MASK	    0xe0000000UL
+	#define FUNC_QCFG_RESP_MIN_BW_BW_VALUE_UNIT_SFT	    29
+	#define FUNC_QCFG_RESP_MIN_BW_BW_VALUE_UNIT_MBPS	   (0x0UL << 29)
+	#define FUNC_QCFG_RESP_MIN_BW_BW_VALUE_UNIT_PERCENT1_100  (0x1UL << 29)
+	#define FUNC_QCFG_RESP_MIN_BW_BW_VALUE_UNIT_INVALID       (0x7UL << 29)
+	#define FUNC_QCFG_RESP_MIN_BW_BW_VALUE_UNIT_LAST    FUNC_QCFG_RESP_MIN_BW_BW_VALUE_UNIT_INVALID
 	__le32 max_bw;
+	#define FUNC_QCFG_RESP_MAX_BW_BW_VALUE_MASK		    0xfffffffUL
+	#define FUNC_QCFG_RESP_MAX_BW_BW_VALUE_SFT		    0
+	#define FUNC_QCFG_RESP_MAX_BW_RSVD			    0x10000000UL
+	#define FUNC_QCFG_RESP_MAX_BW_BW_VALUE_UNIT_MASK	    0xe0000000UL
+	#define FUNC_QCFG_RESP_MAX_BW_BW_VALUE_UNIT_SFT	    29
+	#define FUNC_QCFG_RESP_MAX_BW_BW_VALUE_UNIT_MBPS	   (0x0UL << 29)
+	#define FUNC_QCFG_RESP_MAX_BW_BW_VALUE_UNIT_PERCENT1_100  (0x1UL << 29)
+	#define FUNC_QCFG_RESP_MAX_BW_BW_VALUE_UNIT_INVALID       (0x7UL << 29)
+	#define FUNC_QCFG_RESP_MAX_BW_BW_VALUE_UNIT_LAST    FUNC_QCFG_RESP_MAX_BW_BW_VALUE_UNIT_INVALID
 	u8 evb_mode;
-	#define FUNC_QCFG_RESP_EVB_MODE_NO_EVB			   (0x0UL << 0)
-	#define FUNC_QCFG_RESP_EVB_MODE_VEB			   (0x1UL << 0)
-	#define FUNC_QCFG_RESP_EVB_MODE_VEPA			   (0x2UL << 0)
+	#define FUNC_QCFG_RESP_EVB_MODE_NO_EVB			   0x0UL
+	#define FUNC_QCFG_RESP_EVB_MODE_VEB			   0x1UL
+	#define FUNC_QCFG_RESP_EVB_MODE_VEPA			   0x2UL
 	u8 unused_3;
-	__le16 unused_4;
+	__le16 alloc_vfs;
 	__le32 alloc_mcast_filters;
 	__le32 alloc_hw_ring_grps;
-	u8 unused_5;
-	u8 unused_6;
-	u8 unused_7;
+	__le16 alloc_sp_tx_rings;
+	u8 unused_4;
 	u8 valid;
 };
 
@@ -1171,18 +1201,36 @@ struct hwrm_func_cfg_input {
 	__le16 dflt_vlan;
 	__be32 dflt_ip_addr[4];
 	__le32 min_bw;
+	#define FUNC_CFG_REQ_MIN_BW_BW_VALUE_MASK		    0xfffffffUL
+	#define FUNC_CFG_REQ_MIN_BW_BW_VALUE_SFT		    0
+	#define FUNC_CFG_REQ_MIN_BW_RSVD			    0x10000000UL
+	#define FUNC_CFG_REQ_MIN_BW_BW_VALUE_UNIT_MASK		    0xe0000000UL
+	#define FUNC_CFG_REQ_MIN_BW_BW_VALUE_UNIT_SFT		    29
+	#define FUNC_CFG_REQ_MIN_BW_BW_VALUE_UNIT_MBPS		   (0x0UL << 29)
+	#define FUNC_CFG_REQ_MIN_BW_BW_VALUE_UNIT_PERCENT1_100    (0x1UL << 29)
+	#define FUNC_CFG_REQ_MIN_BW_BW_VALUE_UNIT_INVALID	   (0x7UL << 29)
+	#define FUNC_CFG_REQ_MIN_BW_BW_VALUE_UNIT_LAST    FUNC_CFG_REQ_MIN_BW_BW_VALUE_UNIT_INVALID
 	__le32 max_bw;
+	#define FUNC_CFG_REQ_MAX_BW_BW_VALUE_MASK		    0xfffffffUL
+	#define FUNC_CFG_REQ_MAX_BW_BW_VALUE_SFT		    0
+	#define FUNC_CFG_REQ_MAX_BW_RSVD			    0x10000000UL
+	#define FUNC_CFG_REQ_MAX_BW_BW_VALUE_UNIT_MASK		    0xe0000000UL
+	#define FUNC_CFG_REQ_MAX_BW_BW_VALUE_UNIT_SFT		    29
+	#define FUNC_CFG_REQ_MAX_BW_BW_VALUE_UNIT_MBPS		   (0x0UL << 29)
+	#define FUNC_CFG_REQ_MAX_BW_BW_VALUE_UNIT_PERCENT1_100    (0x1UL << 29)
+	#define FUNC_CFG_REQ_MAX_BW_BW_VALUE_UNIT_INVALID	   (0x7UL << 29)
+	#define FUNC_CFG_REQ_MAX_BW_BW_VALUE_UNIT_LAST    FUNC_CFG_REQ_MAX_BW_BW_VALUE_UNIT_INVALID
 	__le16 async_event_cr;
 	u8 vlan_antispoof_mode;
-	#define FUNC_CFG_REQ_VLAN_ANTISPOOF_MODE_NOCHECK	   (0x0UL << 0)
-	#define FUNC_CFG_REQ_VLAN_ANTISPOOF_MODE_VALIDATE_VLAN    (0x1UL << 0)
-	#define FUNC_CFG_REQ_VLAN_ANTISPOOF_MODE_INSERT_IF_VLANDNE (0x2UL << 0)
-	#define FUNC_CFG_REQ_VLAN_ANTISPOOF_MODE_INSERT_OR_OVERRIDE_VLAN (0x3UL << 0)
+	#define FUNC_CFG_REQ_VLAN_ANTISPOOF_MODE_NOCHECK	   0x0UL
+	#define FUNC_CFG_REQ_VLAN_ANTISPOOF_MODE_VALIDATE_VLAN    0x1UL
+	#define FUNC_CFG_REQ_VLAN_ANTISPOOF_MODE_INSERT_IF_VLANDNE 0x2UL
+	#define FUNC_CFG_REQ_VLAN_ANTISPOOF_MODE_INSERT_OR_OVERRIDE_VLAN 0x3UL
 	u8 allowed_vlan_pris;
 	u8 evb_mode;
-	#define FUNC_CFG_REQ_EVB_MODE_NO_EVB			   (0x0UL << 0)
-	#define FUNC_CFG_REQ_EVB_MODE_VEB			   (0x1UL << 0)
-	#define FUNC_CFG_REQ_EVB_MODE_VEPA			   (0x2UL << 0)
+	#define FUNC_CFG_REQ_EVB_MODE_NO_EVB			   0x0UL
+	#define FUNC_CFG_REQ_EVB_MODE_VEB			   0x1UL
+	#define FUNC_CFG_REQ_EVB_MODE_VEPA			   0x2UL
 	u8 unused_2;
 	__le16 num_mcast_filters;
 };
@@ -1341,16 +1389,16 @@ struct hwrm_func_drv_rgtr_input {
 	#define FUNC_DRV_RGTR_REQ_ENABLES_VF_REQ_FWD		    0x8UL
 	#define FUNC_DRV_RGTR_REQ_ENABLES_ASYNC_EVENT_FWD	    0x10UL
 	__le16 os_type;
-	#define FUNC_DRV_RGTR_REQ_OS_TYPE_UNKNOWN		   (0x0UL << 0)
-	#define FUNC_DRV_RGTR_REQ_OS_TYPE_OTHER		   (0x1UL << 0)
-	#define FUNC_DRV_RGTR_REQ_OS_TYPE_MSDOS		   (0xeUL << 0)
-	#define FUNC_DRV_RGTR_REQ_OS_TYPE_WINDOWS		   (0x12UL << 0)
-	#define FUNC_DRV_RGTR_REQ_OS_TYPE_SOLARIS		   (0x1dUL << 0)
-	#define FUNC_DRV_RGTR_REQ_OS_TYPE_LINUX		   (0x24UL << 0)
-	#define FUNC_DRV_RGTR_REQ_OS_TYPE_FREEBSD		   (0x2aUL << 0)
-	#define FUNC_DRV_RGTR_REQ_OS_TYPE_ESXI			   (0x68UL << 0)
-	#define FUNC_DRV_RGTR_REQ_OS_TYPE_WIN864		   (0x73UL << 0)
-	#define FUNC_DRV_RGTR_REQ_OS_TYPE_WIN2012R2		   (0x74UL << 0)
+	#define FUNC_DRV_RGTR_REQ_OS_TYPE_UNKNOWN		   0x0UL
+	#define FUNC_DRV_RGTR_REQ_OS_TYPE_OTHER		   0x1UL
+	#define FUNC_DRV_RGTR_REQ_OS_TYPE_MSDOS		   0xeUL
+	#define FUNC_DRV_RGTR_REQ_OS_TYPE_WINDOWS		   0x12UL
+	#define FUNC_DRV_RGTR_REQ_OS_TYPE_SOLARIS		   0x1dUL
+	#define FUNC_DRV_RGTR_REQ_OS_TYPE_LINUX		   0x24UL
+	#define FUNC_DRV_RGTR_REQ_OS_TYPE_FREEBSD		   0x2aUL
+	#define FUNC_DRV_RGTR_REQ_OS_TYPE_ESXI			   0x68UL
+	#define FUNC_DRV_RGTR_REQ_OS_TYPE_WIN864		   0x73UL
+	#define FUNC_DRV_RGTR_REQ_OS_TYPE_WIN2012R2		   0x74UL
 	u8 ver_maj;
 	u8 ver_min;
 	u8 ver_upd;
@@ -1415,13 +1463,13 @@ struct hwrm_func_buf_rgtr_input {
 	__le16 vf_id;
 	__le16 req_buf_num_pages;
 	__le16 req_buf_page_size;
-	#define FUNC_BUF_RGTR_REQ_REQ_BUF_PAGE_SIZE_16B	   (0x4UL << 0)
-	#define FUNC_BUF_RGTR_REQ_REQ_BUF_PAGE_SIZE_4K		   (0xcUL << 0)
-	#define FUNC_BUF_RGTR_REQ_REQ_BUF_PAGE_SIZE_8K		   (0xdUL << 0)
-	#define FUNC_BUF_RGTR_REQ_REQ_BUF_PAGE_SIZE_64K	   (0x10UL << 0)
-	#define FUNC_BUF_RGTR_REQ_REQ_BUF_PAGE_SIZE_2M		   (0x15UL << 0)
-	#define FUNC_BUF_RGTR_REQ_REQ_BUF_PAGE_SIZE_4M		   (0x16UL << 0)
-	#define FUNC_BUF_RGTR_REQ_REQ_BUF_PAGE_SIZE_1G		   (0x1eUL << 0)
+	#define FUNC_BUF_RGTR_REQ_REQ_BUF_PAGE_SIZE_16B	   0x4UL
+	#define FUNC_BUF_RGTR_REQ_REQ_BUF_PAGE_SIZE_4K		   0xcUL
+	#define FUNC_BUF_RGTR_REQ_REQ_BUF_PAGE_SIZE_8K		   0xdUL
+	#define FUNC_BUF_RGTR_REQ_REQ_BUF_PAGE_SIZE_64K	   0x10UL
+	#define FUNC_BUF_RGTR_REQ_REQ_BUF_PAGE_SIZE_2M		   0x15UL
+	#define FUNC_BUF_RGTR_REQ_REQ_BUF_PAGE_SIZE_4M		   0x16UL
+	#define FUNC_BUF_RGTR_REQ_REQ_BUF_PAGE_SIZE_1G		   0x1eUL
 	__le16 req_buf_len;
 	__le16 resp_buf_len;
 	u8 unused_0;
@@ -1473,16 +1521,16 @@ struct hwrm_func_drv_qver_output {
 	__le16 seq_id;
 	__le16 resp_len;
 	__le16 os_type;
-	#define FUNC_DRV_QVER_RESP_OS_TYPE_UNKNOWN		   (0x0UL << 0)
-	#define FUNC_DRV_QVER_RESP_OS_TYPE_OTHER		   (0x1UL << 0)
-	#define FUNC_DRV_QVER_RESP_OS_TYPE_MSDOS		   (0xeUL << 0)
-	#define FUNC_DRV_QVER_RESP_OS_TYPE_WINDOWS		   (0x12UL << 0)
-	#define FUNC_DRV_QVER_RESP_OS_TYPE_SOLARIS		   (0x1dUL << 0)
-	#define FUNC_DRV_QVER_RESP_OS_TYPE_LINUX		   (0x24UL << 0)
-	#define FUNC_DRV_QVER_RESP_OS_TYPE_FREEBSD		   (0x2aUL << 0)
-	#define FUNC_DRV_QVER_RESP_OS_TYPE_ESXI		   (0x68UL << 0)
-	#define FUNC_DRV_QVER_RESP_OS_TYPE_WIN864		   (0x73UL << 0)
-	#define FUNC_DRV_QVER_RESP_OS_TYPE_WIN2012R2		   (0x74UL << 0)
+	#define FUNC_DRV_QVER_RESP_OS_TYPE_UNKNOWN		   0x0UL
+	#define FUNC_DRV_QVER_RESP_OS_TYPE_OTHER		   0x1UL
+	#define FUNC_DRV_QVER_RESP_OS_TYPE_MSDOS		   0xeUL
+	#define FUNC_DRV_QVER_RESP_OS_TYPE_WINDOWS		   0x12UL
+	#define FUNC_DRV_QVER_RESP_OS_TYPE_SOLARIS		   0x1dUL
+	#define FUNC_DRV_QVER_RESP_OS_TYPE_LINUX		   0x24UL
+	#define FUNC_DRV_QVER_RESP_OS_TYPE_FREEBSD		   0x2aUL
+	#define FUNC_DRV_QVER_RESP_OS_TYPE_ESXI		   0x68UL
+	#define FUNC_DRV_QVER_RESP_OS_TYPE_WIN864		   0x73UL
+	#define FUNC_DRV_QVER_RESP_OS_TYPE_WIN2012R2		   0x74UL
 	u8 ver_maj;
 	u8 ver_min;
 	u8 ver_upd;
@@ -1528,44 +1576,44 @@ struct hwrm_port_phy_cfg_input {
 	#define PORT_PHY_CFG_REQ_ENABLES_TX_LPI_TIMER		    0x400UL
 	__le16 port_id;
 	__le16 force_link_speed;
-	#define PORT_PHY_CFG_REQ_FORCE_LINK_SPEED_100MB	   (0x1UL << 0)
-	#define PORT_PHY_CFG_REQ_FORCE_LINK_SPEED_1GB		   (0xaUL << 0)
-	#define PORT_PHY_CFG_REQ_FORCE_LINK_SPEED_2GB		   (0x14UL << 0)
-	#define PORT_PHY_CFG_REQ_FORCE_LINK_SPEED_2_5GB	   (0x19UL << 0)
-	#define PORT_PHY_CFG_REQ_FORCE_LINK_SPEED_10GB		   (0x64UL << 0)
-	#define PORT_PHY_CFG_REQ_FORCE_LINK_SPEED_20GB		   (0xc8UL << 0)
-	#define PORT_PHY_CFG_REQ_FORCE_LINK_SPEED_25GB		   (0xfaUL << 0)
-	#define PORT_PHY_CFG_REQ_FORCE_LINK_SPEED_40GB		   (0x190UL << 0)
-	#define PORT_PHY_CFG_REQ_FORCE_LINK_SPEED_50GB		   (0x1f4UL << 0)
-	#define PORT_PHY_CFG_REQ_FORCE_LINK_SPEED_100GB	   (0x3e8UL << 0)
-	#define PORT_PHY_CFG_REQ_FORCE_LINK_SPEED_10MB		   (0xffffUL << 0)
+	#define PORT_PHY_CFG_REQ_FORCE_LINK_SPEED_100MB	   0x1UL
+	#define PORT_PHY_CFG_REQ_FORCE_LINK_SPEED_1GB		   0xaUL
+	#define PORT_PHY_CFG_REQ_FORCE_LINK_SPEED_2GB		   0x14UL
+	#define PORT_PHY_CFG_REQ_FORCE_LINK_SPEED_2_5GB	   0x19UL
+	#define PORT_PHY_CFG_REQ_FORCE_LINK_SPEED_10GB		   0x64UL
+	#define PORT_PHY_CFG_REQ_FORCE_LINK_SPEED_20GB		   0xc8UL
+	#define PORT_PHY_CFG_REQ_FORCE_LINK_SPEED_25GB		   0xfaUL
+	#define PORT_PHY_CFG_REQ_FORCE_LINK_SPEED_40GB		   0x190UL
+	#define PORT_PHY_CFG_REQ_FORCE_LINK_SPEED_50GB		   0x1f4UL
+	#define PORT_PHY_CFG_REQ_FORCE_LINK_SPEED_100GB	   0x3e8UL
+	#define PORT_PHY_CFG_REQ_FORCE_LINK_SPEED_10MB		   0xffffUL
 	u8 auto_mode;
-	#define PORT_PHY_CFG_REQ_AUTO_MODE_NONE		   (0x0UL << 0)
-	#define PORT_PHY_CFG_REQ_AUTO_MODE_ALL_SPEEDS		   (0x1UL << 0)
-	#define PORT_PHY_CFG_REQ_AUTO_MODE_ONE_SPEED		   (0x2UL << 0)
-	#define PORT_PHY_CFG_REQ_AUTO_MODE_ONE_OR_BELOW	   (0x3UL << 0)
-	#define PORT_PHY_CFG_REQ_AUTO_MODE_SPEED_MASK		   (0x4UL << 0)
+	#define PORT_PHY_CFG_REQ_AUTO_MODE_NONE		   0x0UL
+	#define PORT_PHY_CFG_REQ_AUTO_MODE_ALL_SPEEDS		   0x1UL
+	#define PORT_PHY_CFG_REQ_AUTO_MODE_ONE_SPEED		   0x2UL
+	#define PORT_PHY_CFG_REQ_AUTO_MODE_ONE_OR_BELOW	   0x3UL
+	#define PORT_PHY_CFG_REQ_AUTO_MODE_SPEED_MASK		   0x4UL
 	u8 auto_duplex;
-	#define PORT_PHY_CFG_REQ_AUTO_DUPLEX_HALF		   (0x0UL << 0)
-	#define PORT_PHY_CFG_REQ_AUTO_DUPLEX_FULL		   (0x1UL << 0)
-	#define PORT_PHY_CFG_REQ_AUTO_DUPLEX_BOTH		   (0x2UL << 0)
+	#define PORT_PHY_CFG_REQ_AUTO_DUPLEX_HALF		   0x0UL
+	#define PORT_PHY_CFG_REQ_AUTO_DUPLEX_FULL		   0x1UL
+	#define PORT_PHY_CFG_REQ_AUTO_DUPLEX_BOTH		   0x2UL
 	u8 auto_pause;
 	#define PORT_PHY_CFG_REQ_AUTO_PAUSE_TX			    0x1UL
 	#define PORT_PHY_CFG_REQ_AUTO_PAUSE_RX			    0x2UL
 	#define PORT_PHY_CFG_REQ_AUTO_PAUSE_AUTONEG_PAUSE	    0x4UL
 	u8 unused_0;
 	__le16 auto_link_speed;
-	#define PORT_PHY_CFG_REQ_AUTO_LINK_SPEED_100MB		   (0x1UL << 0)
-	#define PORT_PHY_CFG_REQ_AUTO_LINK_SPEED_1GB		   (0xaUL << 0)
-	#define PORT_PHY_CFG_REQ_AUTO_LINK_SPEED_2GB		   (0x14UL << 0)
-	#define PORT_PHY_CFG_REQ_AUTO_LINK_SPEED_2_5GB		   (0x19UL << 0)
-	#define PORT_PHY_CFG_REQ_AUTO_LINK_SPEED_10GB		   (0x64UL << 0)
-	#define PORT_PHY_CFG_REQ_AUTO_LINK_SPEED_20GB		   (0xc8UL << 0)
-	#define PORT_PHY_CFG_REQ_AUTO_LINK_SPEED_25GB		   (0xfaUL << 0)
-	#define PORT_PHY_CFG_REQ_AUTO_LINK_SPEED_40GB		   (0x190UL << 0)
-	#define PORT_PHY_CFG_REQ_AUTO_LINK_SPEED_50GB		   (0x1f4UL << 0)
-	#define PORT_PHY_CFG_REQ_AUTO_LINK_SPEED_100GB		   (0x3e8UL << 0)
-	#define PORT_PHY_CFG_REQ_AUTO_LINK_SPEED_10MB		   (0xffffUL << 0)
+	#define PORT_PHY_CFG_REQ_AUTO_LINK_SPEED_100MB		   0x1UL
+	#define PORT_PHY_CFG_REQ_AUTO_LINK_SPEED_1GB		   0xaUL
+	#define PORT_PHY_CFG_REQ_AUTO_LINK_SPEED_2GB		   0x14UL
+	#define PORT_PHY_CFG_REQ_AUTO_LINK_SPEED_2_5GB		   0x19UL
+	#define PORT_PHY_CFG_REQ_AUTO_LINK_SPEED_10GB		   0x64UL
+	#define PORT_PHY_CFG_REQ_AUTO_LINK_SPEED_20GB		   0xc8UL
+	#define PORT_PHY_CFG_REQ_AUTO_LINK_SPEED_25GB		   0xfaUL
+	#define PORT_PHY_CFG_REQ_AUTO_LINK_SPEED_40GB		   0x190UL
+	#define PORT_PHY_CFG_REQ_AUTO_LINK_SPEED_50GB		   0x1f4UL
+	#define PORT_PHY_CFG_REQ_AUTO_LINK_SPEED_100GB		   0x3e8UL
+	#define PORT_PHY_CFG_REQ_AUTO_LINK_SPEED_10MB		   0xffffUL
 	__le16 auto_link_speed_mask;
 	#define PORT_PHY_CFG_REQ_AUTO_LINK_SPEED_MASK_100MBHD      0x1UL
 	#define PORT_PHY_CFG_REQ_AUTO_LINK_SPEED_MASK_100MB	    0x2UL
@@ -1582,12 +1630,12 @@ struct hwrm_port_phy_cfg_input {
 	#define PORT_PHY_CFG_REQ_AUTO_LINK_SPEED_MASK_10MBHD       0x1000UL
 	#define PORT_PHY_CFG_REQ_AUTO_LINK_SPEED_MASK_10MB	    0x2000UL
 	u8 wirespeed;
-	#define PORT_PHY_CFG_REQ_WIRESPEED_OFF			   (0x0UL << 0)
-	#define PORT_PHY_CFG_REQ_WIRESPEED_ON			   (0x1UL << 0)
+	#define PORT_PHY_CFG_REQ_WIRESPEED_OFF			   0x0UL
+	#define PORT_PHY_CFG_REQ_WIRESPEED_ON			   0x1UL
 	u8 lpbk;
-	#define PORT_PHY_CFG_REQ_LPBK_NONE			   (0x0UL << 0)
-	#define PORT_PHY_CFG_REQ_LPBK_LOCAL			   (0x1UL << 0)
-	#define PORT_PHY_CFG_REQ_LPBK_REMOTE			   (0x2UL << 0)
+	#define PORT_PHY_CFG_REQ_LPBK_NONE			   0x0UL
+	#define PORT_PHY_CFG_REQ_LPBK_LOCAL			   0x1UL
+	#define PORT_PHY_CFG_REQ_LPBK_REMOTE			   0x2UL
 	u8 force_pause;
 	#define PORT_PHY_CFG_REQ_FORCE_PAUSE_TX		    0x1UL
 	#define PORT_PHY_CFG_REQ_FORCE_PAUSE_RX		    0x2UL
@@ -1641,25 +1689,25 @@ struct hwrm_port_phy_qcfg_output {
 	__le16 seq_id;
 	__le16 resp_len;
 	u8 link;
-	#define PORT_PHY_QCFG_RESP_LINK_NO_LINK		   (0x0UL << 0)
-	#define PORT_PHY_QCFG_RESP_LINK_SIGNAL			   (0x1UL << 0)
-	#define PORT_PHY_QCFG_RESP_LINK_LINK			   (0x2UL << 0)
+	#define PORT_PHY_QCFG_RESP_LINK_NO_LINK		   0x0UL
+	#define PORT_PHY_QCFG_RESP_LINK_SIGNAL			   0x1UL
+	#define PORT_PHY_QCFG_RESP_LINK_LINK			   0x2UL
 	u8 unused_0;
 	__le16 link_speed;
-	#define PORT_PHY_QCFG_RESP_LINK_SPEED_100MB		   (0x1UL << 0)
-	#define PORT_PHY_QCFG_RESP_LINK_SPEED_1GB		   (0xaUL << 0)
-	#define PORT_PHY_QCFG_RESP_LINK_SPEED_2GB		   (0x14UL << 0)
-	#define PORT_PHY_QCFG_RESP_LINK_SPEED_2_5GB		   (0x19UL << 0)
-	#define PORT_PHY_QCFG_RESP_LINK_SPEED_10GB		   (0x64UL << 0)
-	#define PORT_PHY_QCFG_RESP_LINK_SPEED_20GB		   (0xc8UL << 0)
-	#define PORT_PHY_QCFG_RESP_LINK_SPEED_25GB		   (0xfaUL << 0)
-	#define PORT_PHY_QCFG_RESP_LINK_SPEED_40GB		   (0x190UL << 0)
-	#define PORT_PHY_QCFG_RESP_LINK_SPEED_50GB		   (0x1f4UL << 0)
-	#define PORT_PHY_QCFG_RESP_LINK_SPEED_100GB		   (0x3e8UL << 0)
-	#define PORT_PHY_QCFG_RESP_LINK_SPEED_10MB		   (0xffffUL << 0)
+	#define PORT_PHY_QCFG_RESP_LINK_SPEED_100MB		   0x1UL
+	#define PORT_PHY_QCFG_RESP_LINK_SPEED_1GB		   0xaUL
+	#define PORT_PHY_QCFG_RESP_LINK_SPEED_2GB		   0x14UL
+	#define PORT_PHY_QCFG_RESP_LINK_SPEED_2_5GB		   0x19UL
+	#define PORT_PHY_QCFG_RESP_LINK_SPEED_10GB		   0x64UL
+	#define PORT_PHY_QCFG_RESP_LINK_SPEED_20GB		   0xc8UL
+	#define PORT_PHY_QCFG_RESP_LINK_SPEED_25GB		   0xfaUL
+	#define PORT_PHY_QCFG_RESP_LINK_SPEED_40GB		   0x190UL
+	#define PORT_PHY_QCFG_RESP_LINK_SPEED_50GB		   0x1f4UL
+	#define PORT_PHY_QCFG_RESP_LINK_SPEED_100GB		   0x3e8UL
+	#define PORT_PHY_QCFG_RESP_LINK_SPEED_10MB		   0xffffUL
 	u8 duplex;
-	#define PORT_PHY_QCFG_RESP_DUPLEX_HALF			   (0x0UL << 0)
-	#define PORT_PHY_QCFG_RESP_DUPLEX_FULL			   (0x1UL << 0)
+	#define PORT_PHY_QCFG_RESP_DUPLEX_HALF			   0x0UL
+	#define PORT_PHY_QCFG_RESP_DUPLEX_FULL			   0x1UL
 	u8 pause;
 	#define PORT_PHY_QCFG_RESP_PAUSE_TX			    0x1UL
 	#define PORT_PHY_QCFG_RESP_PAUSE_RX			    0x2UL
@@ -1679,39 +1727,39 @@ struct hwrm_port_phy_qcfg_output {
 	#define PORT_PHY_QCFG_RESP_SUPPORT_SPEEDS_10MBHD	    0x1000UL
 	#define PORT_PHY_QCFG_RESP_SUPPORT_SPEEDS_10MB		    0x2000UL
 	__le16 force_link_speed;
-	#define PORT_PHY_QCFG_RESP_FORCE_LINK_SPEED_100MB	   (0x1UL << 0)
-	#define PORT_PHY_QCFG_RESP_FORCE_LINK_SPEED_1GB	   (0xaUL << 0)
-	#define PORT_PHY_QCFG_RESP_FORCE_LINK_SPEED_2GB	   (0x14UL << 0)
-	#define PORT_PHY_QCFG_RESP_FORCE_LINK_SPEED_2_5GB	   (0x19UL << 0)
-	#define PORT_PHY_QCFG_RESP_FORCE_LINK_SPEED_10GB	   (0x64UL << 0)
-	#define PORT_PHY_QCFG_RESP_FORCE_LINK_SPEED_20GB	   (0xc8UL << 0)
-	#define PORT_PHY_QCFG_RESP_FORCE_LINK_SPEED_25GB	   (0xfaUL << 0)
-	#define PORT_PHY_QCFG_RESP_FORCE_LINK_SPEED_40GB	   (0x190UL << 0)
-	#define PORT_PHY_QCFG_RESP_FORCE_LINK_SPEED_50GB	   (0x1f4UL << 0)
-	#define PORT_PHY_QCFG_RESP_FORCE_LINK_SPEED_100GB	   (0x3e8UL << 0)
-	#define PORT_PHY_QCFG_RESP_FORCE_LINK_SPEED_10MB	   (0xffffUL << 0)
+	#define PORT_PHY_QCFG_RESP_FORCE_LINK_SPEED_100MB	   0x1UL
+	#define PORT_PHY_QCFG_RESP_FORCE_LINK_SPEED_1GB	   0xaUL
+	#define PORT_PHY_QCFG_RESP_FORCE_LINK_SPEED_2GB	   0x14UL
+	#define PORT_PHY_QCFG_RESP_FORCE_LINK_SPEED_2_5GB	   0x19UL
+	#define PORT_PHY_QCFG_RESP_FORCE_LINK_SPEED_10GB	   0x64UL
+	#define PORT_PHY_QCFG_RESP_FORCE_LINK_SPEED_20GB	   0xc8UL
+	#define PORT_PHY_QCFG_RESP_FORCE_LINK_SPEED_25GB	   0xfaUL
+	#define PORT_PHY_QCFG_RESP_FORCE_LINK_SPEED_40GB	   0x190UL
+	#define PORT_PHY_QCFG_RESP_FORCE_LINK_SPEED_50GB	   0x1f4UL
+	#define PORT_PHY_QCFG_RESP_FORCE_LINK_SPEED_100GB	   0x3e8UL
+	#define PORT_PHY_QCFG_RESP_FORCE_LINK_SPEED_10MB	   0xffffUL
 	u8 auto_mode;
-	#define PORT_PHY_QCFG_RESP_AUTO_MODE_NONE		   (0x0UL << 0)
-	#define PORT_PHY_QCFG_RESP_AUTO_MODE_ALL_SPEEDS	   (0x1UL << 0)
-	#define PORT_PHY_QCFG_RESP_AUTO_MODE_ONE_SPEED		   (0x2UL << 0)
-	#define PORT_PHY_QCFG_RESP_AUTO_MODE_ONE_OR_BELOW	   (0x3UL << 0)
-	#define PORT_PHY_QCFG_RESP_AUTO_MODE_SPEED_MASK	   (0x4UL << 0)
+	#define PORT_PHY_QCFG_RESP_AUTO_MODE_NONE		   0x0UL
+	#define PORT_PHY_QCFG_RESP_AUTO_MODE_ALL_SPEEDS	   0x1UL
+	#define PORT_PHY_QCFG_RESP_AUTO_MODE_ONE_SPEED		   0x2UL
+	#define PORT_PHY_QCFG_RESP_AUTO_MODE_ONE_OR_BELOW	   0x3UL
+	#define PORT_PHY_QCFG_RESP_AUTO_MODE_SPEED_MASK	   0x4UL
 	u8 auto_pause;
 	#define PORT_PHY_QCFG_RESP_AUTO_PAUSE_TX		    0x1UL
 	#define PORT_PHY_QCFG_RESP_AUTO_PAUSE_RX		    0x2UL
 	#define PORT_PHY_QCFG_RESP_AUTO_PAUSE_AUTONEG_PAUSE	    0x4UL
 	__le16 auto_link_speed;
-	#define PORT_PHY_QCFG_RESP_AUTO_LINK_SPEED_100MB	   (0x1UL << 0)
-	#define PORT_PHY_QCFG_RESP_AUTO_LINK_SPEED_1GB		   (0xaUL << 0)
-	#define PORT_PHY_QCFG_RESP_AUTO_LINK_SPEED_2GB		   (0x14UL << 0)
-	#define PORT_PHY_QCFG_RESP_AUTO_LINK_SPEED_2_5GB	   (0x19UL << 0)
-	#define PORT_PHY_QCFG_RESP_AUTO_LINK_SPEED_10GB	   (0x64UL << 0)
-	#define PORT_PHY_QCFG_RESP_AUTO_LINK_SPEED_20GB	   (0xc8UL << 0)
-	#define PORT_PHY_QCFG_RESP_AUTO_LINK_SPEED_25GB	   (0xfaUL << 0)
-	#define PORT_PHY_QCFG_RESP_AUTO_LINK_SPEED_40GB	   (0x190UL << 0)
-	#define PORT_PHY_QCFG_RESP_AUTO_LINK_SPEED_50GB	   (0x1f4UL << 0)
-	#define PORT_PHY_QCFG_RESP_AUTO_LINK_SPEED_100GB	   (0x3e8UL << 0)
-	#define PORT_PHY_QCFG_RESP_AUTO_LINK_SPEED_10MB	   (0xffffUL << 0)
+	#define PORT_PHY_QCFG_RESP_AUTO_LINK_SPEED_100MB	   0x1UL
+	#define PORT_PHY_QCFG_RESP_AUTO_LINK_SPEED_1GB		   0xaUL
+	#define PORT_PHY_QCFG_RESP_AUTO_LINK_SPEED_2GB		   0x14UL
+	#define PORT_PHY_QCFG_RESP_AUTO_LINK_SPEED_2_5GB	   0x19UL
+	#define PORT_PHY_QCFG_RESP_AUTO_LINK_SPEED_10GB	   0x64UL
+	#define PORT_PHY_QCFG_RESP_AUTO_LINK_SPEED_20GB	   0xc8UL
+	#define PORT_PHY_QCFG_RESP_AUTO_LINK_SPEED_25GB	   0xfaUL
+	#define PORT_PHY_QCFG_RESP_AUTO_LINK_SPEED_40GB	   0x190UL
+	#define PORT_PHY_QCFG_RESP_AUTO_LINK_SPEED_50GB	   0x1f4UL
+	#define PORT_PHY_QCFG_RESP_AUTO_LINK_SPEED_100GB	   0x3e8UL
+	#define PORT_PHY_QCFG_RESP_AUTO_LINK_SPEED_10MB	   0xffffUL
 	__le16 auto_link_speed_mask;
 	#define PORT_PHY_QCFG_RESP_AUTO_LINK_SPEED_MASK_100MBHD    0x1UL
 	#define PORT_PHY_QCFG_RESP_AUTO_LINK_SPEED_MASK_100MB      0x2UL
@@ -1728,46 +1776,46 @@ struct hwrm_port_phy_qcfg_output {
 	#define PORT_PHY_QCFG_RESP_AUTO_LINK_SPEED_MASK_10MBHD     0x1000UL
 	#define PORT_PHY_QCFG_RESP_AUTO_LINK_SPEED_MASK_10MB       0x2000UL
 	u8 wirespeed;
-	#define PORT_PHY_QCFG_RESP_WIRESPEED_OFF		   (0x0UL << 0)
-	#define PORT_PHY_QCFG_RESP_WIRESPEED_ON		   (0x1UL << 0)
+	#define PORT_PHY_QCFG_RESP_WIRESPEED_OFF		   0x0UL
+	#define PORT_PHY_QCFG_RESP_WIRESPEED_ON		   0x1UL
 	u8 lpbk;
-	#define PORT_PHY_QCFG_RESP_LPBK_NONE			   (0x0UL << 0)
-	#define PORT_PHY_QCFG_RESP_LPBK_LOCAL			   (0x1UL << 0)
-	#define PORT_PHY_QCFG_RESP_LPBK_REMOTE			   (0x2UL << 0)
+	#define PORT_PHY_QCFG_RESP_LPBK_NONE			   0x0UL
+	#define PORT_PHY_QCFG_RESP_LPBK_LOCAL			   0x1UL
+	#define PORT_PHY_QCFG_RESP_LPBK_REMOTE			   0x2UL
 	u8 force_pause;
 	#define PORT_PHY_QCFG_RESP_FORCE_PAUSE_TX		    0x1UL
 	#define PORT_PHY_QCFG_RESP_FORCE_PAUSE_RX		    0x2UL
 	u8 module_status;
-	#define PORT_PHY_QCFG_RESP_MODULE_STATUS_NONE		   (0x0UL << 0)
-	#define PORT_PHY_QCFG_RESP_MODULE_STATUS_DISABLETX	   (0x1UL << 0)
-	#define PORT_PHY_QCFG_RESP_MODULE_STATUS_WARNINGMSG       (0x2UL << 0)
-	#define PORT_PHY_QCFG_RESP_MODULE_STATUS_PWRDOWN	   (0x3UL << 0)
-	#define PORT_PHY_QCFG_RESP_MODULE_STATUS_NOTINSERTED      (0x4UL << 0)
-	#define PORT_PHY_QCFG_RESP_MODULE_STATUS_NOTAPPLICABLE    (0xffUL << 0)
+	#define PORT_PHY_QCFG_RESP_MODULE_STATUS_NONE		   0x0UL
+	#define PORT_PHY_QCFG_RESP_MODULE_STATUS_DISABLETX	   0x1UL
+	#define PORT_PHY_QCFG_RESP_MODULE_STATUS_WARNINGMSG       0x2UL
+	#define PORT_PHY_QCFG_RESP_MODULE_STATUS_PWRDOWN	   0x3UL
+	#define PORT_PHY_QCFG_RESP_MODULE_STATUS_NOTINSERTED      0x4UL
+	#define PORT_PHY_QCFG_RESP_MODULE_STATUS_NOTAPPLICABLE    0xffUL
 	__le32 preemphasis;
 	u8 phy_maj;
 	u8 phy_min;
 	u8 phy_bld;
 	u8 phy_type;
-	#define PORT_PHY_QCFG_RESP_PHY_TYPE_UNKNOWN		   (0x0UL << 0)
-	#define PORT_PHY_QCFG_RESP_PHY_TYPE_BASECR		   (0x1UL << 0)
-	#define PORT_PHY_QCFG_RESP_PHY_TYPE_BASEKR4		   (0x2UL << 0)
-	#define PORT_PHY_QCFG_RESP_PHY_TYPE_BASELR		   (0x3UL << 0)
-	#define PORT_PHY_QCFG_RESP_PHY_TYPE_BASESR		   (0x4UL << 0)
-	#define PORT_PHY_QCFG_RESP_PHY_TYPE_BASEKR2		   (0x5UL << 0)
-	#define PORT_PHY_QCFG_RESP_PHY_TYPE_BASEKX		   (0x6UL << 0)
-	#define PORT_PHY_QCFG_RESP_PHY_TYPE_BASEKR		   (0x7UL << 0)
-	#define PORT_PHY_QCFG_RESP_PHY_TYPE_BASET		   (0x8UL << 0)
-	#define PORT_PHY_QCFG_RESP_PHY_TYPE_BASETE		   (0x9UL << 0)
-	#define PORT_PHY_QCFG_RESP_PHY_TYPE_SGMIIEXTPHY	   (0xaUL << 0)
+	#define PORT_PHY_QCFG_RESP_PHY_TYPE_UNKNOWN		   0x0UL
+	#define PORT_PHY_QCFG_RESP_PHY_TYPE_BASECR		   0x1UL
+	#define PORT_PHY_QCFG_RESP_PHY_TYPE_BASEKR4		   0x2UL
+	#define PORT_PHY_QCFG_RESP_PHY_TYPE_BASELR		   0x3UL
+	#define PORT_PHY_QCFG_RESP_PHY_TYPE_BASESR		   0x4UL
+	#define PORT_PHY_QCFG_RESP_PHY_TYPE_BASEKR2		   0x5UL
+	#define PORT_PHY_QCFG_RESP_PHY_TYPE_BASEKX		   0x6UL
+	#define PORT_PHY_QCFG_RESP_PHY_TYPE_BASEKR		   0x7UL
+	#define PORT_PHY_QCFG_RESP_PHY_TYPE_BASET		   0x8UL
+	#define PORT_PHY_QCFG_RESP_PHY_TYPE_BASETE		   0x9UL
+	#define PORT_PHY_QCFG_RESP_PHY_TYPE_SGMIIEXTPHY	   0xaUL
 	u8 media_type;
-	#define PORT_PHY_QCFG_RESP_MEDIA_TYPE_UNKNOWN		   (0x0UL << 0)
-	#define PORT_PHY_QCFG_RESP_MEDIA_TYPE_TP		   (0x1UL << 0)
-	#define PORT_PHY_QCFG_RESP_MEDIA_TYPE_DAC		   (0x2UL << 0)
-	#define PORT_PHY_QCFG_RESP_MEDIA_TYPE_FIBRE		   (0x3UL << 0)
+	#define PORT_PHY_QCFG_RESP_MEDIA_TYPE_UNKNOWN		   0x0UL
+	#define PORT_PHY_QCFG_RESP_MEDIA_TYPE_TP		   0x1UL
+	#define PORT_PHY_QCFG_RESP_MEDIA_TYPE_DAC		   0x2UL
+	#define PORT_PHY_QCFG_RESP_MEDIA_TYPE_FIBRE		   0x3UL
 	u8 xcvr_pkg_type;
-	#define PORT_PHY_QCFG_RESP_XCVR_PKG_TYPE_XCVR_INTERNAL    (0x1UL << 0)
-	#define PORT_PHY_QCFG_RESP_XCVR_PKG_TYPE_XCVR_EXTERNAL    (0x2UL << 0)
+	#define PORT_PHY_QCFG_RESP_XCVR_PKG_TYPE_XCVR_INTERNAL    0x1UL
+	#define PORT_PHY_QCFG_RESP_XCVR_PKG_TYPE_XCVR_EXTERNAL    0x2UL
 	u8 eee_config_phy_addr;
 	#define PORT_PHY_QCFG_RESP_PHY_ADDR_MASK		    0x1fUL
 	#define PORT_PHY_QCFG_RESP_PHY_ADDR_SFT		    0
@@ -1796,11 +1844,11 @@ struct hwrm_port_phy_qcfg_output {
 	#define PORT_PHY_QCFG_RESP_LINK_PARTNER_ADV_SPEEDS_10MBHD  0x1000UL
 	#define PORT_PHY_QCFG_RESP_LINK_PARTNER_ADV_SPEEDS_10MB    0x2000UL
 	u8 link_partner_adv_auto_mode;
-	#define PORT_PHY_QCFG_RESP_LINK_PARTNER_ADV_AUTO_MODE_NONE (0x0UL << 0)
-	#define PORT_PHY_QCFG_RESP_LINK_PARTNER_ADV_AUTO_MODE_ALL_SPEEDS (0x1UL << 0)
-	#define PORT_PHY_QCFG_RESP_LINK_PARTNER_ADV_AUTO_MODE_ONE_SPEED (0x2UL << 0)
-	#define PORT_PHY_QCFG_RESP_LINK_PARTNER_ADV_AUTO_MODE_ONE_OR_BELOW (0x3UL << 0)
-	#define PORT_PHY_QCFG_RESP_LINK_PARTNER_ADV_AUTO_MODE_SPEED_MASK (0x4UL << 0)
+	#define PORT_PHY_QCFG_RESP_LINK_PARTNER_ADV_AUTO_MODE_NONE 0x0UL
+	#define PORT_PHY_QCFG_RESP_LINK_PARTNER_ADV_AUTO_MODE_ALL_SPEEDS 0x1UL
+	#define PORT_PHY_QCFG_RESP_LINK_PARTNER_ADV_AUTO_MODE_ONE_SPEED 0x2UL
+	#define PORT_PHY_QCFG_RESP_LINK_PARTNER_ADV_AUTO_MODE_ONE_OR_BELOW 0x3UL
+	#define PORT_PHY_QCFG_RESP_LINK_PARTNER_ADV_AUTO_MODE_SPEED_MASK 0x4UL
 	u8 link_partner_adv_pause;
 	#define PORT_PHY_QCFG_RESP_LINK_PARTNER_ADV_PAUSE_TX       0x1UL
 	#define PORT_PHY_QCFG_RESP_LINK_PARTNER_ADV_PAUSE_RX       0x2UL
@@ -1859,7 +1907,7 @@ struct hwrm_port_mac_cfg_input {
 	__le64 resp_addr;
 	__le32 flags;
 	#define PORT_MAC_CFG_REQ_FLAGS_MATCH_LINK		    0x1UL
-	#define PORT_MAC_CFG_REQ_FLAGS_COS_ASSIGNMENT_ENABLE       0x2UL
+	#define PORT_MAC_CFG_REQ_FLAGS_VLAN_PRI2COS_ENABLE	    0x2UL
 	#define PORT_MAC_CFG_REQ_FLAGS_TUNNEL_PRI2COS_ENABLE       0x4UL
 	#define PORT_MAC_CFG_REQ_FLAGS_IP_DSCP2COS_ENABLE	    0x8UL
 	#define PORT_MAC_CFG_REQ_FLAGS_PTP_RX_TS_CAPTURE_ENABLE    0x10UL
@@ -1868,28 +1916,50 @@ struct hwrm_port_mac_cfg_input {
 	#define PORT_MAC_CFG_REQ_FLAGS_PTP_TX_TS_CAPTURE_DISABLE   0x80UL
 	#define PORT_MAC_CFG_REQ_FLAGS_OOB_WOL_ENABLE		    0x100UL
 	#define PORT_MAC_CFG_REQ_FLAGS_OOB_WOL_DISABLE		    0x200UL
+	#define PORT_MAC_CFG_REQ_FLAGS_VLAN_PRI2COS_DISABLE	    0x400UL
+	#define PORT_MAC_CFG_REQ_FLAGS_TUNNEL_PRI2COS_DISABLE      0x800UL
+	#define PORT_MAC_CFG_REQ_FLAGS_IP_DSCP2COS_DISABLE	    0x1000UL
 	__le32 enables;
 	#define PORT_MAC_CFG_REQ_ENABLES_IPG			    0x1UL
 	#define PORT_MAC_CFG_REQ_ENABLES_LPBK			    0x2UL
-	#define PORT_MAC_CFG_REQ_ENABLES_IVLAN_PRI2COS_MAP_PRI     0x4UL
-	#define PORT_MAC_CFG_REQ_ENABLES_LCOS_MAP_PRI		    0x8UL
+	#define PORT_MAC_CFG_REQ_ENABLES_VLAN_PRI2COS_MAP_PRI      0x4UL
+	#define PORT_MAC_CFG_REQ_ENABLES_RESERVED1		    0x8UL
 	#define PORT_MAC_CFG_REQ_ENABLES_TUNNEL_PRI2COS_MAP_PRI    0x10UL
 	#define PORT_MAC_CFG_REQ_ENABLES_DSCP2COS_MAP_PRI	    0x20UL
 	#define PORT_MAC_CFG_REQ_ENABLES_RX_TS_CAPTURE_PTP_MSG_TYPE 0x40UL
 	#define PORT_MAC_CFG_REQ_ENABLES_TX_TS_CAPTURE_PTP_MSG_TYPE 0x80UL
+	#define PORT_MAC_CFG_REQ_ENABLES_COS_FIELD_CFG		    0x100UL
 	__le16 port_id;
 	u8 ipg;
 	u8 lpbk;
-	#define PORT_MAC_CFG_REQ_LPBK_NONE			   (0x0UL << 0)
-	#define PORT_MAC_CFG_REQ_LPBK_LOCAL			   (0x1UL << 0)
-	#define PORT_MAC_CFG_REQ_LPBK_REMOTE			   (0x2UL << 0)
-	u8 ivlan_pri2cos_map_pri;
-	u8 lcos_map_pri;
+	#define PORT_MAC_CFG_REQ_LPBK_NONE			   0x0UL
+	#define PORT_MAC_CFG_REQ_LPBK_LOCAL			   0x1UL
+	#define PORT_MAC_CFG_REQ_LPBK_REMOTE			   0x2UL
+	u8 vlan_pri2cos_map_pri;
+	u8 reserved1;
 	u8 tunnel_pri2cos_map_pri;
 	u8 dscp2pri_map_pri;
 	__le16 rx_ts_capture_ptp_msg_type;
 	__le16 tx_ts_capture_ptp_msg_type;
-	__le32 unused_0;
+	u8 cos_field_cfg;
+	#define PORT_MAC_CFG_REQ_COS_FIELD_CFG_RSVD1		    0x1UL
+	#define PORT_MAC_CFG_REQ_COS_FIELD_CFG_VLAN_PRI_SEL_MASK   0x6UL
+	#define PORT_MAC_CFG_REQ_COS_FIELD_CFG_VLAN_PRI_SEL_SFT    1
+	#define PORT_MAC_CFG_REQ_COS_FIELD_CFG_VLAN_PRI_SEL_INNERMOST (0x0UL << 1)
+	#define PORT_MAC_CFG_REQ_COS_FIELD_CFG_VLAN_PRI_SEL_OUTER (0x1UL << 1)
+	#define PORT_MAC_CFG_REQ_COS_FIELD_CFG_VLAN_PRI_SEL_OUTERMOST (0x2UL << 1)
+	#define PORT_MAC_CFG_REQ_COS_FIELD_CFG_VLAN_PRI_SEL_UNSPECIFIED (0x3UL << 1)
+	#define PORT_MAC_CFG_REQ_COS_FIELD_CFG_VLAN_PRI_SEL_LAST    PORT_MAC_CFG_REQ_COS_FIELD_CFG_VLAN_PRI_SEL_UNSPECIFIED
+	#define PORT_MAC_CFG_REQ_COS_FIELD_CFG_T_VLAN_PRI_SEL_MASK 0x18UL
+	#define PORT_MAC_CFG_REQ_COS_FIELD_CFG_T_VLAN_PRI_SEL_SFT  3
+	#define PORT_MAC_CFG_REQ_COS_FIELD_CFG_T_VLAN_PRI_SEL_INNERMOST (0x0UL << 3)
+	#define PORT_MAC_CFG_REQ_COS_FIELD_CFG_T_VLAN_PRI_SEL_OUTER (0x1UL << 3)
+	#define PORT_MAC_CFG_REQ_COS_FIELD_CFG_T_VLAN_PRI_SEL_OUTERMOST (0x2UL << 3)
+	#define PORT_MAC_CFG_REQ_COS_FIELD_CFG_T_VLAN_PRI_SEL_UNSPECIFIED (0x3UL << 3)
+	#define PORT_MAC_CFG_REQ_COS_FIELD_CFG_T_VLAN_PRI_SEL_LAST    PORT_MAC_CFG_REQ_COS_FIELD_CFG_T_VLAN_PRI_SEL_UNSPECIFIED
+	#define PORT_MAC_CFG_REQ_COS_FIELD_CFG_DEFAULT_COS_MASK    0xe0UL
+	#define PORT_MAC_CFG_REQ_COS_FIELD_CFG_DEFAULT_COS_SFT     5
+	u8 unused_0[3];
 };
 
 /* Output (16 bytes) */
@@ -1902,9 +1972,9 @@ struct hwrm_port_mac_cfg_output {
 	__le16 mtu;
 	u8 ipg;
 	u8 lpbk;
-	#define PORT_MAC_CFG_RESP_LPBK_NONE			   (0x0UL << 0)
-	#define PORT_MAC_CFG_RESP_LPBK_LOCAL			   (0x1UL << 0)
-	#define PORT_MAC_CFG_RESP_LPBK_REMOTE			   (0x2UL << 0)
+	#define PORT_MAC_CFG_RESP_LPBK_NONE			   0x0UL
+	#define PORT_MAC_CFG_RESP_LPBK_LOCAL			   0x1UL
+	#define PORT_MAC_CFG_RESP_LPBK_REMOTE			   0x2UL
 	u8 unused_0;
 	u8 valid;
 };
@@ -2163,8 +2233,8 @@ struct hwrm_queue_qportcfg_input {
 	__le64 resp_addr;
 	__le32 flags;
 	#define QUEUE_QPORTCFG_REQ_FLAGS_PATH			    0x1UL
-	#define QUEUE_QPORTCFG_REQ_FLAGS_PATH_TX		   (0x0UL << 0)
-	#define QUEUE_QPORTCFG_REQ_FLAGS_PATH_RX		   (0x1UL << 0)
+	#define QUEUE_QPORTCFG_REQ_FLAGS_PATH_TX		   0x0UL
+	#define QUEUE_QPORTCFG_REQ_FLAGS_PATH_RX		   0x1UL
 	#define QUEUE_QPORTCFG_REQ_FLAGS_PATH_LAST    QUEUE_QPORTCFG_REQ_FLAGS_PATH_RX
 	__le16 port_id;
 	__le16 unused_0;
@@ -2179,50 +2249,51 @@ struct hwrm_queue_qportcfg_output {
 	u8 max_configurable_queues;
 	u8 max_configurable_lossless_queues;
 	u8 queue_cfg_allowed;
-	u8 queue_buffers_cfg_allowed;
+	u8 queue_cfg_info;
+	#define QUEUE_QPORTCFG_RESP_QUEUE_CFG_INFO_ASYM_CFG	    0x1UL
 	u8 queue_pfcenable_cfg_allowed;
 	u8 queue_pri2cos_cfg_allowed;
 	u8 queue_cos2bw_cfg_allowed;
 	u8 queue_id0;
 	u8 queue_id0_service_profile;
-	#define QUEUE_QPORTCFG_RESP_QUEUE_ID0_SERVICE_PROFILE_LOSSY (0x0UL << 0)
-	#define QUEUE_QPORTCFG_RESP_QUEUE_ID0_SERVICE_PROFILE_LOSSLESS (0x1UL << 0)
-	#define QUEUE_QPORTCFG_RESP_QUEUE_ID0_SERVICE_PROFILE_UNKNOWN (0xffUL << 0)
+	#define QUEUE_QPORTCFG_RESP_QUEUE_ID0_SERVICE_PROFILE_LOSSY 0x0UL
+	#define QUEUE_QPORTCFG_RESP_QUEUE_ID0_SERVICE_PROFILE_LOSSLESS 0x1UL
+	#define QUEUE_QPORTCFG_RESP_QUEUE_ID0_SERVICE_PROFILE_UNKNOWN 0xffUL
 	u8 queue_id1;
 	u8 queue_id1_service_profile;
-	#define QUEUE_QPORTCFG_RESP_QUEUE_ID1_SERVICE_PROFILE_LOSSY (0x0UL << 0)
-	#define QUEUE_QPORTCFG_RESP_QUEUE_ID1_SERVICE_PROFILE_LOSSLESS (0x1UL << 0)
-	#define QUEUE_QPORTCFG_RESP_QUEUE_ID1_SERVICE_PROFILE_UNKNOWN (0xffUL << 0)
+	#define QUEUE_QPORTCFG_RESP_QUEUE_ID1_SERVICE_PROFILE_LOSSY 0x0UL
+	#define QUEUE_QPORTCFG_RESP_QUEUE_ID1_SERVICE_PROFILE_LOSSLESS 0x1UL
+	#define QUEUE_QPORTCFG_RESP_QUEUE_ID1_SERVICE_PROFILE_UNKNOWN 0xffUL
 	u8 queue_id2;
 	u8 queue_id2_service_profile;
-	#define QUEUE_QPORTCFG_RESP_QUEUE_ID2_SERVICE_PROFILE_LOSSY (0x0UL << 0)
-	#define QUEUE_QPORTCFG_RESP_QUEUE_ID2_SERVICE_PROFILE_LOSSLESS (0x1UL << 0)
-	#define QUEUE_QPORTCFG_RESP_QUEUE_ID2_SERVICE_PROFILE_UNKNOWN (0xffUL << 0)
+	#define QUEUE_QPORTCFG_RESP_QUEUE_ID2_SERVICE_PROFILE_LOSSY 0x0UL
+	#define QUEUE_QPORTCFG_RESP_QUEUE_ID2_SERVICE_PROFILE_LOSSLESS 0x1UL
+	#define QUEUE_QPORTCFG_RESP_QUEUE_ID2_SERVICE_PROFILE_UNKNOWN 0xffUL
 	u8 queue_id3;
 	u8 queue_id3_service_profile;
-	#define QUEUE_QPORTCFG_RESP_QUEUE_ID3_SERVICE_PROFILE_LOSSY (0x0UL << 0)
-	#define QUEUE_QPORTCFG_RESP_QUEUE_ID3_SERVICE_PROFILE_LOSSLESS (0x1UL << 0)
-	#define QUEUE_QPORTCFG_RESP_QUEUE_ID3_SERVICE_PROFILE_UNKNOWN (0xffUL << 0)
+	#define QUEUE_QPORTCFG_RESP_QUEUE_ID3_SERVICE_PROFILE_LOSSY 0x0UL
+	#define QUEUE_QPORTCFG_RESP_QUEUE_ID3_SERVICE_PROFILE_LOSSLESS 0x1UL
+	#define QUEUE_QPORTCFG_RESP_QUEUE_ID3_SERVICE_PROFILE_UNKNOWN 0xffUL
 	u8 queue_id4;
 	u8 queue_id4_service_profile;
-	#define QUEUE_QPORTCFG_RESP_QUEUE_ID4_SERVICE_PROFILE_LOSSY (0x0UL << 0)
-	#define QUEUE_QPORTCFG_RESP_QUEUE_ID4_SERVICE_PROFILE_LOSSLESS (0x1UL << 0)
-	#define QUEUE_QPORTCFG_RESP_QUEUE_ID4_SERVICE_PROFILE_UNKNOWN (0xffUL << 0)
+	#define QUEUE_QPORTCFG_RESP_QUEUE_ID4_SERVICE_PROFILE_LOSSY 0x0UL
+	#define QUEUE_QPORTCFG_RESP_QUEUE_ID4_SERVICE_PROFILE_LOSSLESS 0x1UL
+	#define QUEUE_QPORTCFG_RESP_QUEUE_ID4_SERVICE_PROFILE_UNKNOWN 0xffUL
 	u8 queue_id5;
 	u8 queue_id5_service_profile;
-	#define QUEUE_QPORTCFG_RESP_QUEUE_ID5_SERVICE_PROFILE_LOSSY (0x0UL << 0)
-	#define QUEUE_QPORTCFG_RESP_QUEUE_ID5_SERVICE_PROFILE_LOSSLESS (0x1UL << 0)
-	#define QUEUE_QPORTCFG_RESP_QUEUE_ID5_SERVICE_PROFILE_UNKNOWN (0xffUL << 0)
+	#define QUEUE_QPORTCFG_RESP_QUEUE_ID5_SERVICE_PROFILE_LOSSY 0x0UL
+	#define QUEUE_QPORTCFG_RESP_QUEUE_ID5_SERVICE_PROFILE_LOSSLESS 0x1UL
+	#define QUEUE_QPORTCFG_RESP_QUEUE_ID5_SERVICE_PROFILE_UNKNOWN 0xffUL
 	u8 queue_id6;
 	u8 queue_id6_service_profile;
-	#define QUEUE_QPORTCFG_RESP_QUEUE_ID6_SERVICE_PROFILE_LOSSY (0x0UL << 0)
-	#define QUEUE_QPORTCFG_RESP_QUEUE_ID6_SERVICE_PROFILE_LOSSLESS (0x1UL << 0)
-	#define QUEUE_QPORTCFG_RESP_QUEUE_ID6_SERVICE_PROFILE_UNKNOWN (0xffUL << 0)
+	#define QUEUE_QPORTCFG_RESP_QUEUE_ID6_SERVICE_PROFILE_LOSSY 0x0UL
+	#define QUEUE_QPORTCFG_RESP_QUEUE_ID6_SERVICE_PROFILE_LOSSLESS 0x1UL
+	#define QUEUE_QPORTCFG_RESP_QUEUE_ID6_SERVICE_PROFILE_UNKNOWN 0xffUL
 	u8 queue_id7;
 	u8 queue_id7_service_profile;
-	#define QUEUE_QPORTCFG_RESP_QUEUE_ID7_SERVICE_PROFILE_LOSSY (0x0UL << 0)
-	#define QUEUE_QPORTCFG_RESP_QUEUE_ID7_SERVICE_PROFILE_LOSSLESS (0x1UL << 0)
-	#define QUEUE_QPORTCFG_RESP_QUEUE_ID7_SERVICE_PROFILE_UNKNOWN (0xffUL << 0)
+	#define QUEUE_QPORTCFG_RESP_QUEUE_ID7_SERVICE_PROFILE_LOSSY 0x0UL
+	#define QUEUE_QPORTCFG_RESP_QUEUE_ID7_SERVICE_PROFILE_LOSSLESS 0x1UL
+	#define QUEUE_QPORTCFG_RESP_QUEUE_ID7_SERVICE_PROFILE_UNKNOWN 0xffUL
 	u8 valid;
 };
 
@@ -2235,19 +2306,21 @@ struct hwrm_queue_cfg_input {
 	__le16 target_id;
 	__le64 resp_addr;
 	__le32 flags;
-	#define QUEUE_CFG_REQ_FLAGS_PATH			    0x1UL
-	#define QUEUE_CFG_REQ_FLAGS_PATH_TX			   (0x0UL << 0)
-	#define QUEUE_CFG_REQ_FLAGS_PATH_RX			   (0x1UL << 0)
-	#define QUEUE_CFG_REQ_FLAGS_PATH_LAST    QUEUE_CFG_REQ_FLAGS_PATH_RX
+	#define QUEUE_CFG_REQ_FLAGS_PATH_MASK			    0x3UL
+	#define QUEUE_CFG_REQ_FLAGS_PATH_SFT			    0
+	#define QUEUE_CFG_REQ_FLAGS_PATH_TX			   0x0UL
+	#define QUEUE_CFG_REQ_FLAGS_PATH_RX			   0x1UL
+	#define QUEUE_CFG_REQ_FLAGS_PATH_BIDIR			   0x2UL
+	#define QUEUE_CFG_REQ_FLAGS_PATH_LAST    QUEUE_CFG_REQ_FLAGS_PATH_BIDIR
 	__le32 enables;
 	#define QUEUE_CFG_REQ_ENABLES_DFLT_LEN			    0x1UL
 	#define QUEUE_CFG_REQ_ENABLES_SERVICE_PROFILE		    0x2UL
 	__le32 queue_id;
 	__le32 dflt_len;
 	u8 service_profile;
-	#define QUEUE_CFG_REQ_SERVICE_PROFILE_LOSSY		   (0x0UL << 0)
-	#define QUEUE_CFG_REQ_SERVICE_PROFILE_LOSSLESS		   (0x1UL << 0)
-	#define QUEUE_CFG_REQ_SERVICE_PROFILE_UNKNOWN		   (0xffUL << 0)
+	#define QUEUE_CFG_REQ_SERVICE_PROFILE_LOSSY		   0x0UL
+	#define QUEUE_CFG_REQ_SERVICE_PROFILE_LOSSLESS		   0x1UL
+	#define QUEUE_CFG_REQ_SERVICE_PROFILE_UNKNOWN		   0xffUL
 	u8 unused_0[7];
 };
 
@@ -2264,50 +2337,6 @@ struct hwrm_queue_cfg_output {
 	u8 valid;
 };
 
-/* hwrm_queue_buffers_cfg */
-/* Input (56 bytes) */
-struct hwrm_queue_buffers_cfg_input {
-	__le16 req_type;
-	__le16 cmpl_ring;
-	__le16 seq_id;
-	__le16 target_id;
-	__le64 resp_addr;
-	__le32 flags;
-	#define QUEUE_BUFFERS_CFG_REQ_FLAGS_PATH		    0x1UL
-	#define QUEUE_BUFFERS_CFG_REQ_FLAGS_PATH_TX		   (0x0UL << 0)
-	#define QUEUE_BUFFERS_CFG_REQ_FLAGS_PATH_RX		   (0x1UL << 0)
-	#define QUEUE_BUFFERS_CFG_REQ_FLAGS_PATH_LAST    QUEUE_BUFFERS_CFG_REQ_FLAGS_PATH_RX
-	__le32 enables;
-	#define QUEUE_BUFFERS_CFG_REQ_ENABLES_RESERVED		    0x1UL
-	#define QUEUE_BUFFERS_CFG_REQ_ENABLES_SHARED		    0x2UL
-	#define QUEUE_BUFFERS_CFG_REQ_ENABLES_XOFF		    0x4UL
-	#define QUEUE_BUFFERS_CFG_REQ_ENABLES_XON		    0x8UL
-	#define QUEUE_BUFFERS_CFG_REQ_ENABLES_FULL		    0x10UL
-	#define QUEUE_BUFFERS_CFG_REQ_ENABLES_NOTFULL		    0x20UL
-	#define QUEUE_BUFFERS_CFG_REQ_ENABLES_MAX		    0x40UL
-	__le32 queue_id;
-	__le32 reserved;
-	__le32 shared;
-	__le32 xoff;
-	__le32 xon;
-	__le32 full;
-	__le32 notfull;
-	__le32 max;
-};
-
-/* Output (16 bytes) */
-struct hwrm_queue_buffers_cfg_output {
-	__le16 error_code;
-	__le16 req_type;
-	__le16 seq_id;
-	__le16 resp_len;
-	__le32 unused_0;
-	u8 unused_1;
-	u8 unused_2;
-	u8 unused_3;
-	u8 valid;
-};
-
 /* hwrm_queue_pfcenable_cfg */
 /* Input (24 bytes) */
 struct hwrm_queue_pfcenable_cfg_input {
@@ -2351,12 +2380,22 @@ struct hwrm_queue_pri2cos_cfg_input {
 	__le16 target_id;
 	__le64 resp_addr;
 	__le32 flags;
-	#define QUEUE_PRI2COS_CFG_REQ_FLAGS_PATH		    0x1UL
+	#define QUEUE_PRI2COS_CFG_REQ_FLAGS_PATH_MASK		    0x3UL
+	#define QUEUE_PRI2COS_CFG_REQ_FLAGS_PATH_SFT		    0
 	#define QUEUE_PRI2COS_CFG_REQ_FLAGS_PATH_TX		   (0x0UL << 0)
 	#define QUEUE_PRI2COS_CFG_REQ_FLAGS_PATH_RX		   (0x1UL << 0)
-	#define QUEUE_PRI2COS_CFG_REQ_FLAGS_PATH_LAST    QUEUE_PRI2COS_CFG_REQ_FLAGS_PATH_RX
-	#define QUEUE_PRI2COS_CFG_REQ_FLAGS_IVLAN		    0x2UL
+	#define QUEUE_PRI2COS_CFG_REQ_FLAGS_PATH_BIDIR		   (0x2UL << 0)
+	#define QUEUE_PRI2COS_CFG_REQ_FLAGS_PATH_LAST    QUEUE_PRI2COS_CFG_REQ_FLAGS_PATH_BIDIR
+	#define QUEUE_PRI2COS_CFG_REQ_FLAGS_IVLAN		    0x4UL
 	__le32 enables;
+	#define QUEUE_PRI2COS_CFG_REQ_ENABLES_PRI0_COS_QUEUE_ID    0x1UL
+	#define QUEUE_PRI2COS_CFG_REQ_ENABLES_PRI1_COS_QUEUE_ID    0x2UL
+	#define QUEUE_PRI2COS_CFG_REQ_ENABLES_PRI2_COS_QUEUE_ID    0x4UL
+	#define QUEUE_PRI2COS_CFG_REQ_ENABLES_PRI3_COS_QUEUE_ID    0x8UL
+	#define QUEUE_PRI2COS_CFG_REQ_ENABLES_PRI4_COS_QUEUE_ID    0x10UL
+	#define QUEUE_PRI2COS_CFG_REQ_ENABLES_PRI5_COS_QUEUE_ID    0x20UL
+	#define QUEUE_PRI2COS_CFG_REQ_ENABLES_PRI6_COS_QUEUE_ID    0x40UL
+	#define QUEUE_PRI2COS_CFG_REQ_ENABLES_PRI7_COS_QUEUE_ID    0x80UL
 	u8 port_id;
 	u8 pri0_cos_queue_id;
 	u8 pri1_cos_queue_id;
@@ -2404,82 +2443,226 @@ struct hwrm_queue_cos2bw_cfg_input {
 	u8 queue_id0;
 	u8 unused_0;
 	__le32 queue_id0_min_bw;
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID0_MIN_BW_BW_VALUE_MASK 0xfffffffUL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID0_MIN_BW_BW_VALUE_SFT 0
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID0_MIN_BW_RSVD	    0x10000000UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID0_MIN_BW_BW_VALUE_UNIT_MASK 0xe0000000UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID0_MIN_BW_BW_VALUE_UNIT_SFT 29
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID0_MIN_BW_BW_VALUE_UNIT_MBPS (0x0UL << 29)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID0_MIN_BW_BW_VALUE_UNIT_PERCENT1_100 (0x1UL << 29)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID0_MIN_BW_BW_VALUE_UNIT_INVALID (0x7UL << 29)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID0_MIN_BW_BW_VALUE_UNIT_LAST    QUEUE_COS2BW_CFG_REQ_QUEUE_ID0_MIN_BW_BW_VALUE_UNIT_INVALID
 	__le32 queue_id0_max_bw;
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID0_MAX_BW_BW_VALUE_MASK 0xfffffffUL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID0_MAX_BW_BW_VALUE_SFT 0
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID0_MAX_BW_RSVD	    0x10000000UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID0_MAX_BW_BW_VALUE_UNIT_MASK 0xe0000000UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID0_MAX_BW_BW_VALUE_UNIT_SFT 29
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID0_MAX_BW_BW_VALUE_UNIT_MBPS (0x0UL << 29)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID0_MAX_BW_BW_VALUE_UNIT_PERCENT1_100 (0x1UL << 29)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID0_MAX_BW_BW_VALUE_UNIT_INVALID (0x7UL << 29)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID0_MAX_BW_BW_VALUE_UNIT_LAST    QUEUE_COS2BW_CFG_REQ_QUEUE_ID0_MAX_BW_BW_VALUE_UNIT_INVALID
 	u8 queue_id0_tsa_assign;
-	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID0_TSA_ASSIGN_SP      (0x0UL << 0)
-	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID0_TSA_ASSIGN_ETS     (0x1UL << 0)
-	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID0_TSA_ASSIGN_RESERVED_FIRST (0x2UL << 0)
-	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID0_TSA_ASSIGN_RESERVED_LAST (0xffUL << 0)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID0_TSA_ASSIGN_SP      0x0UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID0_TSA_ASSIGN_ETS     0x1UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID0_TSA_ASSIGN_RESERVED_FIRST 0x2UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID0_TSA_ASSIGN_RESERVED_LAST 0xffUL
 	u8 queue_id0_pri_lvl;
 	u8 queue_id0_bw_weight;
 	u8 queue_id1;
 	__le32 queue_id1_min_bw;
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID1_MIN_BW_BW_VALUE_MASK 0xfffffffUL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID1_MIN_BW_BW_VALUE_SFT 0
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID1_MIN_BW_RSVD	    0x10000000UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID1_MIN_BW_BW_VALUE_UNIT_MASK 0xe0000000UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID1_MIN_BW_BW_VALUE_UNIT_SFT 29
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID1_MIN_BW_BW_VALUE_UNIT_MBPS (0x0UL << 29)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID1_MIN_BW_BW_VALUE_UNIT_PERCENT1_100 (0x1UL << 29)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID1_MIN_BW_BW_VALUE_UNIT_INVALID (0x7UL << 29)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID1_MIN_BW_BW_VALUE_UNIT_LAST    QUEUE_COS2BW_CFG_REQ_QUEUE_ID1_MIN_BW_BW_VALUE_UNIT_INVALID
 	__le32 queue_id1_max_bw;
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID1_MAX_BW_BW_VALUE_MASK 0xfffffffUL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID1_MAX_BW_BW_VALUE_SFT 0
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID1_MAX_BW_RSVD	    0x10000000UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID1_MAX_BW_BW_VALUE_UNIT_MASK 0xe0000000UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID1_MAX_BW_BW_VALUE_UNIT_SFT 29
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID1_MAX_BW_BW_VALUE_UNIT_MBPS (0x0UL << 29)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID1_MAX_BW_BW_VALUE_UNIT_PERCENT1_100 (0x1UL << 29)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID1_MAX_BW_BW_VALUE_UNIT_INVALID (0x7UL << 29)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID1_MAX_BW_BW_VALUE_UNIT_LAST    QUEUE_COS2BW_CFG_REQ_QUEUE_ID1_MAX_BW_BW_VALUE_UNIT_INVALID
 	u8 queue_id1_tsa_assign;
-	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID1_TSA_ASSIGN_SP      (0x0UL << 0)
-	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID1_TSA_ASSIGN_ETS     (0x1UL << 0)
-	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID1_TSA_ASSIGN_RESERVED_FIRST (0x2UL << 0)
-	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID1_TSA_ASSIGN_RESERVED_LAST (0xffUL << 0)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID1_TSA_ASSIGN_SP      0x0UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID1_TSA_ASSIGN_ETS     0x1UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID1_TSA_ASSIGN_RESERVED_FIRST 0x2UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID1_TSA_ASSIGN_RESERVED_LAST 0xffUL
 	u8 queue_id1_pri_lvl;
 	u8 queue_id1_bw_weight;
 	u8 queue_id2;
 	__le32 queue_id2_min_bw;
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID2_MIN_BW_BW_VALUE_MASK 0xfffffffUL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID2_MIN_BW_BW_VALUE_SFT 0
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID2_MIN_BW_RSVD	    0x10000000UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID2_MIN_BW_BW_VALUE_UNIT_MASK 0xe0000000UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID2_MIN_BW_BW_VALUE_UNIT_SFT 29
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID2_MIN_BW_BW_VALUE_UNIT_MBPS (0x0UL << 29)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID2_MIN_BW_BW_VALUE_UNIT_PERCENT1_100 (0x1UL << 29)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID2_MIN_BW_BW_VALUE_UNIT_INVALID (0x7UL << 29)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID2_MIN_BW_BW_VALUE_UNIT_LAST    QUEUE_COS2BW_CFG_REQ_QUEUE_ID2_MIN_BW_BW_VALUE_UNIT_INVALID
 	__le32 queue_id2_max_bw;
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID2_MAX_BW_BW_VALUE_MASK 0xfffffffUL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID2_MAX_BW_BW_VALUE_SFT 0
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID2_MAX_BW_RSVD	    0x10000000UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID2_MAX_BW_BW_VALUE_UNIT_MASK 0xe0000000UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID2_MAX_BW_BW_VALUE_UNIT_SFT 29
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID2_MAX_BW_BW_VALUE_UNIT_MBPS (0x0UL << 29)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID2_MAX_BW_BW_VALUE_UNIT_PERCENT1_100 (0x1UL << 29)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID2_MAX_BW_BW_VALUE_UNIT_INVALID (0x7UL << 29)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID2_MAX_BW_BW_VALUE_UNIT_LAST    QUEUE_COS2BW_CFG_REQ_QUEUE_ID2_MAX_BW_BW_VALUE_UNIT_INVALID
 	u8 queue_id2_tsa_assign;
-	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID2_TSA_ASSIGN_SP      (0x0UL << 0)
-	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID2_TSA_ASSIGN_ETS     (0x1UL << 0)
-	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID2_TSA_ASSIGN_RESERVED_FIRST (0x2UL << 0)
-	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID2_TSA_ASSIGN_RESERVED_LAST (0xffUL << 0)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID2_TSA_ASSIGN_SP      0x0UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID2_TSA_ASSIGN_ETS     0x1UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID2_TSA_ASSIGN_RESERVED_FIRST 0x2UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID2_TSA_ASSIGN_RESERVED_LAST 0xffUL
 	u8 queue_id2_pri_lvl;
 	u8 queue_id2_bw_weight;
 	u8 queue_id3;
 	__le32 queue_id3_min_bw;
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID3_MIN_BW_BW_VALUE_MASK 0xfffffffUL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID3_MIN_BW_BW_VALUE_SFT 0
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID3_MIN_BW_RSVD	    0x10000000UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID3_MIN_BW_BW_VALUE_UNIT_MASK 0xe0000000UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID3_MIN_BW_BW_VALUE_UNIT_SFT 29
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID3_MIN_BW_BW_VALUE_UNIT_MBPS (0x0UL << 29)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID3_MIN_BW_BW_VALUE_UNIT_PERCENT1_100 (0x1UL << 29)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID3_MIN_BW_BW_VALUE_UNIT_INVALID (0x7UL << 29)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID3_MIN_BW_BW_VALUE_UNIT_LAST    QUEUE_COS2BW_CFG_REQ_QUEUE_ID3_MIN_BW_BW_VALUE_UNIT_INVALID
 	__le32 queue_id3_max_bw;
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID3_MAX_BW_BW_VALUE_MASK 0xfffffffUL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID3_MAX_BW_BW_VALUE_SFT 0
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID3_MAX_BW_RSVD	    0x10000000UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID3_MAX_BW_BW_VALUE_UNIT_MASK 0xe0000000UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID3_MAX_BW_BW_VALUE_UNIT_SFT 29
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID3_MAX_BW_BW_VALUE_UNIT_MBPS (0x0UL << 29)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID3_MAX_BW_BW_VALUE_UNIT_PERCENT1_100 (0x1UL << 29)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID3_MAX_BW_BW_VALUE_UNIT_INVALID (0x7UL << 29)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID3_MAX_BW_BW_VALUE_UNIT_LAST    QUEUE_COS2BW_CFG_REQ_QUEUE_ID3_MAX_BW_BW_VALUE_UNIT_INVALID
 	u8 queue_id3_tsa_assign;
-	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID3_TSA_ASSIGN_SP      (0x0UL << 0)
-	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID3_TSA_ASSIGN_ETS     (0x1UL << 0)
-	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID3_TSA_ASSIGN_RESERVED_FIRST (0x2UL << 0)
-	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID3_TSA_ASSIGN_RESERVED_LAST (0xffUL << 0)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID3_TSA_ASSIGN_SP      0x0UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID3_TSA_ASSIGN_ETS     0x1UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID3_TSA_ASSIGN_RESERVED_FIRST 0x2UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID3_TSA_ASSIGN_RESERVED_LAST 0xffUL
 	u8 queue_id3_pri_lvl;
 	u8 queue_id3_bw_weight;
 	u8 queue_id4;
 	__le32 queue_id4_min_bw;
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID4_MIN_BW_BW_VALUE_MASK 0xfffffffUL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID4_MIN_BW_BW_VALUE_SFT 0
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID4_MIN_BW_RSVD	    0x10000000UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID4_MIN_BW_BW_VALUE_UNIT_MASK 0xe0000000UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID4_MIN_BW_BW_VALUE_UNIT_SFT 29
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID4_MIN_BW_BW_VALUE_UNIT_MBPS (0x0UL << 29)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID4_MIN_BW_BW_VALUE_UNIT_PERCENT1_100 (0x1UL << 29)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID4_MIN_BW_BW_VALUE_UNIT_INVALID (0x7UL << 29)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID4_MIN_BW_BW_VALUE_UNIT_LAST    QUEUE_COS2BW_CFG_REQ_QUEUE_ID4_MIN_BW_BW_VALUE_UNIT_INVALID
 	__le32 queue_id4_max_bw;
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID4_MAX_BW_BW_VALUE_MASK 0xfffffffUL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID4_MAX_BW_BW_VALUE_SFT 0
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID4_MAX_BW_RSVD	    0x10000000UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID4_MAX_BW_BW_VALUE_UNIT_MASK 0xe0000000UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID4_MAX_BW_BW_VALUE_UNIT_SFT 29
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID4_MAX_BW_BW_VALUE_UNIT_MBPS (0x0UL << 29)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID4_MAX_BW_BW_VALUE_UNIT_PERCENT1_100 (0x1UL << 29)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID4_MAX_BW_BW_VALUE_UNIT_INVALID (0x7UL << 29)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID4_MAX_BW_BW_VALUE_UNIT_LAST    QUEUE_COS2BW_CFG_REQ_QUEUE_ID4_MAX_BW_BW_VALUE_UNIT_INVALID
 	u8 queue_id4_tsa_assign;
-	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID4_TSA_ASSIGN_SP      (0x0UL << 0)
-	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID4_TSA_ASSIGN_ETS     (0x1UL << 0)
-	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID4_TSA_ASSIGN_RESERVED_FIRST (0x2UL << 0)
-	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID4_TSA_ASSIGN_RESERVED_LAST (0xffUL << 0)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID4_TSA_ASSIGN_SP      0x0UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID4_TSA_ASSIGN_ETS     0x1UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID4_TSA_ASSIGN_RESERVED_FIRST 0x2UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID4_TSA_ASSIGN_RESERVED_LAST 0xffUL
 	u8 queue_id4_pri_lvl;
 	u8 queue_id4_bw_weight;
 	u8 queue_id5;
 	__le32 queue_id5_min_bw;
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID5_MIN_BW_BW_VALUE_MASK 0xfffffffUL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID5_MIN_BW_BW_VALUE_SFT 0
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID5_MIN_BW_RSVD	    0x10000000UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID5_MIN_BW_BW_VALUE_UNIT_MASK 0xe0000000UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID5_MIN_BW_BW_VALUE_UNIT_SFT 29
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID5_MIN_BW_BW_VALUE_UNIT_MBPS (0x0UL << 29)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID5_MIN_BW_BW_VALUE_UNIT_PERCENT1_100 (0x1UL << 29)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID5_MIN_BW_BW_VALUE_UNIT_INVALID (0x7UL << 29)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID5_MIN_BW_BW_VALUE_UNIT_LAST    QUEUE_COS2BW_CFG_REQ_QUEUE_ID5_MIN_BW_BW_VALUE_UNIT_INVALID
 	__le32 queue_id5_max_bw;
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID5_MAX_BW_BW_VALUE_MASK 0xfffffffUL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID5_MAX_BW_BW_VALUE_SFT 0
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID5_MAX_BW_RSVD	    0x10000000UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID5_MAX_BW_BW_VALUE_UNIT_MASK 0xe0000000UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID5_MAX_BW_BW_VALUE_UNIT_SFT 29
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID5_MAX_BW_BW_VALUE_UNIT_MBPS (0x0UL << 29)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID5_MAX_BW_BW_VALUE_UNIT_PERCENT1_100 (0x1UL << 29)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID5_MAX_BW_BW_VALUE_UNIT_INVALID (0x7UL << 29)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID5_MAX_BW_BW_VALUE_UNIT_LAST    QUEUE_COS2BW_CFG_REQ_QUEUE_ID5_MAX_BW_BW_VALUE_UNIT_INVALID
 	u8 queue_id5_tsa_assign;
-	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID5_TSA_ASSIGN_SP      (0x0UL << 0)
-	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID5_TSA_ASSIGN_ETS     (0x1UL << 0)
-	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID5_TSA_ASSIGN_RESERVED_FIRST (0x2UL << 0)
-	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID5_TSA_ASSIGN_RESERVED_LAST (0xffUL << 0)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID5_TSA_ASSIGN_SP      0x0UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID5_TSA_ASSIGN_ETS     0x1UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID5_TSA_ASSIGN_RESERVED_FIRST 0x2UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID5_TSA_ASSIGN_RESERVED_LAST 0xffUL
 	u8 queue_id5_pri_lvl;
 	u8 queue_id5_bw_weight;
 	u8 queue_id6;
 	__le32 queue_id6_min_bw;
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID6_MIN_BW_BW_VALUE_MASK 0xfffffffUL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID6_MIN_BW_BW_VALUE_SFT 0
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID6_MIN_BW_RSVD	    0x10000000UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID6_MIN_BW_BW_VALUE_UNIT_MASK 0xe0000000UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID6_MIN_BW_BW_VALUE_UNIT_SFT 29
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID6_MIN_BW_BW_VALUE_UNIT_MBPS (0x0UL << 29)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID6_MIN_BW_BW_VALUE_UNIT_PERCENT1_100 (0x1UL << 29)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID6_MIN_BW_BW_VALUE_UNIT_INVALID (0x7UL << 29)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID6_MIN_BW_BW_VALUE_UNIT_LAST    QUEUE_COS2BW_CFG_REQ_QUEUE_ID6_MIN_BW_BW_VALUE_UNIT_INVALID
 	__le32 queue_id6_max_bw;
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID6_MAX_BW_BW_VALUE_MASK 0xfffffffUL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID6_MAX_BW_BW_VALUE_SFT 0
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID6_MAX_BW_RSVD	    0x10000000UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID6_MAX_BW_BW_VALUE_UNIT_MASK 0xe0000000UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID6_MAX_BW_BW_VALUE_UNIT_SFT 29
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID6_MAX_BW_BW_VALUE_UNIT_MBPS (0x0UL << 29)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID6_MAX_BW_BW_VALUE_UNIT_PERCENT1_100 (0x1UL << 29)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID6_MAX_BW_BW_VALUE_UNIT_INVALID (0x7UL << 29)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID6_MAX_BW_BW_VALUE_UNIT_LAST    QUEUE_COS2BW_CFG_REQ_QUEUE_ID6_MAX_BW_BW_VALUE_UNIT_INVALID
 	u8 queue_id6_tsa_assign;
-	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID6_TSA_ASSIGN_SP      (0x0UL << 0)
-	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID6_TSA_ASSIGN_ETS     (0x1UL << 0)
-	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID6_TSA_ASSIGN_RESERVED_FIRST (0x2UL << 0)
-	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID6_TSA_ASSIGN_RESERVED_LAST (0xffUL << 0)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID6_TSA_ASSIGN_SP      0x0UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID6_TSA_ASSIGN_ETS     0x1UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID6_TSA_ASSIGN_RESERVED_FIRST 0x2UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID6_TSA_ASSIGN_RESERVED_LAST 0xffUL
 	u8 queue_id6_pri_lvl;
 	u8 queue_id6_bw_weight;
 	u8 queue_id7;
 	__le32 queue_id7_min_bw;
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID7_MIN_BW_BW_VALUE_MASK 0xfffffffUL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID7_MIN_BW_BW_VALUE_SFT 0
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID7_MIN_BW_RSVD	    0x10000000UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID7_MIN_BW_BW_VALUE_UNIT_MASK 0xe0000000UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID7_MIN_BW_BW_VALUE_UNIT_SFT 29
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID7_MIN_BW_BW_VALUE_UNIT_MBPS (0x0UL << 29)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID7_MIN_BW_BW_VALUE_UNIT_PERCENT1_100 (0x1UL << 29)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID7_MIN_BW_BW_VALUE_UNIT_INVALID (0x7UL << 29)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID7_MIN_BW_BW_VALUE_UNIT_LAST    QUEUE_COS2BW_CFG_REQ_QUEUE_ID7_MIN_BW_BW_VALUE_UNIT_INVALID
 	__le32 queue_id7_max_bw;
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID7_MAX_BW_BW_VALUE_MASK 0xfffffffUL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID7_MAX_BW_BW_VALUE_SFT 0
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID7_MAX_BW_RSVD	    0x10000000UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID7_MAX_BW_BW_VALUE_UNIT_MASK 0xe0000000UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID7_MAX_BW_BW_VALUE_UNIT_SFT 29
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID7_MAX_BW_BW_VALUE_UNIT_MBPS (0x0UL << 29)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID7_MAX_BW_BW_VALUE_UNIT_PERCENT1_100 (0x1UL << 29)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID7_MAX_BW_BW_VALUE_UNIT_INVALID (0x7UL << 29)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID7_MAX_BW_BW_VALUE_UNIT_LAST    QUEUE_COS2BW_CFG_REQ_QUEUE_ID7_MAX_BW_BW_VALUE_UNIT_INVALID
 	u8 queue_id7_tsa_assign;
-	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID7_TSA_ASSIGN_SP      (0x0UL << 0)
-	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID7_TSA_ASSIGN_ETS     (0x1UL << 0)
-	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID7_TSA_ASSIGN_RESERVED_FIRST (0x2UL << 0)
-	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID7_TSA_ASSIGN_RESERVED_LAST (0xffUL << 0)
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID7_TSA_ASSIGN_SP      0x0UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID7_TSA_ASSIGN_ETS     0x1UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID7_TSA_ASSIGN_RESERVED_FIRST 0x2UL
+	#define QUEUE_COS2BW_CFG_REQ_QUEUE_ID7_TSA_ASSIGN_RESERVED_LAST 0xffUL
 	u8 queue_id7_pri_lvl;
 	u8 queue_id7_bw_weight;
 	u8 unused_1[5];
@@ -2563,6 +2746,7 @@ struct hwrm_vnic_cfg_input {
 	#define VNIC_CFG_REQ_FLAGS_BD_STALL_MODE		    0x4UL
 	#define VNIC_CFG_REQ_FLAGS_ROCE_DUAL_VNIC_MODE		    0x8UL
 	#define VNIC_CFG_REQ_FLAGS_ROCE_ONLY_VNIC_MODE		    0x10UL
+	#define VNIC_CFG_REQ_FLAGS_RSS_DFLT_CR_MODE		    0x20UL
 	__le32 enables;
 	#define VNIC_CFG_REQ_ENABLES_DFLT_RING_GRP		    0x1UL
 	#define VNIC_CFG_REQ_ENABLES_RSS_RULE			    0x2UL
@@ -2615,18 +2799,18 @@ struct hwrm_vnic_tpa_cfg_input {
 	#define VNIC_TPA_CFG_REQ_ENABLES_MIN_AGG_LEN		    0x8UL
 	__le16 vnic_id;
 	__le16 max_agg_segs;
-	#define VNIC_TPA_CFG_REQ_MAX_AGG_SEGS_1		   (0x0UL << 0)
-	#define VNIC_TPA_CFG_REQ_MAX_AGG_SEGS_2		   (0x1UL << 0)
-	#define VNIC_TPA_CFG_REQ_MAX_AGG_SEGS_4		   (0x2UL << 0)
-	#define VNIC_TPA_CFG_REQ_MAX_AGG_SEGS_8		   (0x3UL << 0)
-	#define VNIC_TPA_CFG_REQ_MAX_AGG_SEGS_MAX		   (0x1fUL << 0)
+	#define VNIC_TPA_CFG_REQ_MAX_AGG_SEGS_1		   0x0UL
+	#define VNIC_TPA_CFG_REQ_MAX_AGG_SEGS_2		   0x1UL
+	#define VNIC_TPA_CFG_REQ_MAX_AGG_SEGS_4		   0x2UL
+	#define VNIC_TPA_CFG_REQ_MAX_AGG_SEGS_8		   0x3UL
+	#define VNIC_TPA_CFG_REQ_MAX_AGG_SEGS_MAX		   0x1fUL
 	__le16 max_aggs;
-	#define VNIC_TPA_CFG_REQ_MAX_AGGS_1			   (0x0UL << 0)
-	#define VNIC_TPA_CFG_REQ_MAX_AGGS_2			   (0x1UL << 0)
-	#define VNIC_TPA_CFG_REQ_MAX_AGGS_4			   (0x2UL << 0)
-	#define VNIC_TPA_CFG_REQ_MAX_AGGS_8			   (0x3UL << 0)
-	#define VNIC_TPA_CFG_REQ_MAX_AGGS_16			   (0x4UL << 0)
-	#define VNIC_TPA_CFG_REQ_MAX_AGGS_MAX			   (0x7UL << 0)
+	#define VNIC_TPA_CFG_REQ_MAX_AGGS_1			   0x0UL
+	#define VNIC_TPA_CFG_REQ_MAX_AGGS_2			   0x1UL
+	#define VNIC_TPA_CFG_REQ_MAX_AGGS_4			   0x2UL
+	#define VNIC_TPA_CFG_REQ_MAX_AGGS_8			   0x3UL
+	#define VNIC_TPA_CFG_REQ_MAX_AGGS_16			   0x4UL
+	#define VNIC_TPA_CFG_REQ_MAX_AGGS_MAX			   0x7UL
 	u8 unused_0;
 	u8 unused_1;
 	__le32 max_agg_timer;
@@ -2780,15 +2964,15 @@ struct hwrm_ring_alloc_input {
 	__le64 resp_addr;
 	__le32 enables;
 	#define RING_ALLOC_REQ_ENABLES_RESERVED1		    0x1UL
-	#define RING_ALLOC_REQ_ENABLES_RESERVED2		    0x2UL
+	#define RING_ALLOC_REQ_ENABLES_RING_ARB_CFG		    0x2UL
 	#define RING_ALLOC_REQ_ENABLES_RESERVED3		    0x4UL
 	#define RING_ALLOC_REQ_ENABLES_STAT_CTX_ID_VALID	    0x8UL
 	#define RING_ALLOC_REQ_ENABLES_RESERVED4		    0x10UL
 	#define RING_ALLOC_REQ_ENABLES_MAX_BW_VALID		    0x20UL
 	u8 ring_type;
-	#define RING_ALLOC_REQ_RING_TYPE_CMPL			   (0x0UL << 0)
-	#define RING_ALLOC_REQ_RING_TYPE_TX			   (0x1UL << 0)
-	#define RING_ALLOC_REQ_RING_TYPE_RX			   (0x2UL << 0)
+	#define RING_ALLOC_REQ_RING_TYPE_CMPL			   0x0UL
+	#define RING_ALLOC_REQ_RING_TYPE_TX			   0x1UL
+	#define RING_ALLOC_REQ_RING_TYPE_RX			   0x2UL
 	u8 unused_0;
 	__le16 unused_1;
 	__le64 page_tbl_addr;
@@ -2804,18 +2988,36 @@ struct hwrm_ring_alloc_input {
 	u8 unused_4;
 	u8 unused_5;
 	__le32 reserved1;
-	__le16 reserved2;
+	__le16 ring_arb_cfg;
+	#define RING_ALLOC_REQ_RING_ARB_CFG_ARB_POLICY_MASK	    0xfUL
+	#define RING_ALLOC_REQ_RING_ARB_CFG_ARB_POLICY_SFT	    0
+	#define RING_ALLOC_REQ_RING_ARB_CFG_ARB_POLICY_SP	   (0x1UL << 0)
+	#define RING_ALLOC_REQ_RING_ARB_CFG_ARB_POLICY_WFQ	   (0x2UL << 0)
+	#define RING_ALLOC_REQ_RING_ARB_CFG_ARB_POLICY_LAST    RING_ALLOC_REQ_RING_ARB_CFG_ARB_POLICY_WFQ
+	#define RING_ALLOC_REQ_RING_ARB_CFG_RSVD_MASK		    0xf0UL
+	#define RING_ALLOC_REQ_RING_ARB_CFG_RSVD_SFT		    4
+	#define RING_ALLOC_REQ_RING_ARB_CFG_ARB_POLICY_PARAM_MASK  0xff00UL
+	#define RING_ALLOC_REQ_RING_ARB_CFG_ARB_POLICY_PARAM_SFT   8
 	u8 unused_6;
 	u8 unused_7;
 	__le32 reserved3;
 	__le32 stat_ctx_id;
 	__le32 reserved4;
 	__le32 max_bw;
+	#define RING_ALLOC_REQ_MAX_BW_BW_VALUE_MASK		    0xfffffffUL
+	#define RING_ALLOC_REQ_MAX_BW_BW_VALUE_SFT		    0
+	#define RING_ALLOC_REQ_MAX_BW_RSVD			    0x10000000UL
+	#define RING_ALLOC_REQ_MAX_BW_BW_VALUE_UNIT_MASK	    0xe0000000UL
+	#define RING_ALLOC_REQ_MAX_BW_BW_VALUE_UNIT_SFT	    29
+	#define RING_ALLOC_REQ_MAX_BW_BW_VALUE_UNIT_MBPS	   (0x0UL << 29)
+	#define RING_ALLOC_REQ_MAX_BW_BW_VALUE_UNIT_PERCENT1_100  (0x1UL << 29)
+	#define RING_ALLOC_REQ_MAX_BW_BW_VALUE_UNIT_INVALID       (0x7UL << 29)
+	#define RING_ALLOC_REQ_MAX_BW_BW_VALUE_UNIT_LAST    RING_ALLOC_REQ_MAX_BW_BW_VALUE_UNIT_INVALID
 	u8 int_mode;
-	#define RING_ALLOC_REQ_INT_MODE_LEGACY			   (0x0UL << 0)
-	#define RING_ALLOC_REQ_INT_MODE_RSVD			   (0x1UL << 0)
-	#define RING_ALLOC_REQ_INT_MODE_MSIX			   (0x2UL << 0)
-	#define RING_ALLOC_REQ_INT_MODE_POLL			   (0x3UL << 0)
+	#define RING_ALLOC_REQ_INT_MODE_LEGACY			   0x0UL
+	#define RING_ALLOC_REQ_INT_MODE_RSVD			   0x1UL
+	#define RING_ALLOC_REQ_INT_MODE_MSIX			   0x2UL
+	#define RING_ALLOC_REQ_INT_MODE_POLL			   0x3UL
 	u8 unused_8[3];
 };
 
@@ -2842,9 +3044,9 @@ struct hwrm_ring_free_input {
 	__le16 target_id;
 	__le64 resp_addr;
 	u8 ring_type;
-	#define RING_FREE_REQ_RING_TYPE_CMPL			   (0x0UL << 0)
-	#define RING_FREE_REQ_RING_TYPE_TX			   (0x1UL << 0)
-	#define RING_FREE_REQ_RING_TYPE_RX			   (0x2UL << 0)
+	#define RING_FREE_REQ_RING_TYPE_CMPL			   0x0UL
+	#define RING_FREE_REQ_RING_TYPE_TX			   0x1UL
+	#define RING_FREE_REQ_RING_TYPE_RX			   0x2UL
 	u8 unused_0;
 	__le16 ring_id;
 	__le32 unused_1;
@@ -2942,9 +3144,9 @@ struct hwrm_ring_reset_input {
 	__le16 target_id;
 	__le64 resp_addr;
 	u8 ring_type;
-	#define RING_RESET_REQ_RING_TYPE_CMPL			   (0x0UL << 0)
-	#define RING_RESET_REQ_RING_TYPE_TX			   (0x1UL << 0)
-	#define RING_RESET_REQ_RING_TYPE_RX			   (0x2UL << 0)
+	#define RING_RESET_REQ_RING_TYPE_CMPL			   0x0UL
+	#define RING_RESET_REQ_RING_TYPE_TX			   0x1UL
+	#define RING_RESET_REQ_RING_TYPE_RX			   0x2UL
 	u8 unused_0;
 	__le16 ring_id;
 	__le32 unused_1;
@@ -3068,36 +3270,36 @@ struct hwrm_cfa_l2_filter_alloc_input {
 	__le16 t_l2_ivlan;
 	__le16 t_l2_ivlan_mask;
 	u8 src_type;
-	#define CFA_L2_FILTER_ALLOC_REQ_SRC_TYPE_NPORT		   (0x0UL << 0)
-	#define CFA_L2_FILTER_ALLOC_REQ_SRC_TYPE_PF		   (0x1UL << 0)
-	#define CFA_L2_FILTER_ALLOC_REQ_SRC_TYPE_VF		   (0x2UL << 0)
-	#define CFA_L2_FILTER_ALLOC_REQ_SRC_TYPE_VNIC		   (0x3UL << 0)
-	#define CFA_L2_FILTER_ALLOC_REQ_SRC_TYPE_KONG		   (0x4UL << 0)
-	#define CFA_L2_FILTER_ALLOC_REQ_SRC_TYPE_APE		   (0x5UL << 0)
-	#define CFA_L2_FILTER_ALLOC_REQ_SRC_TYPE_BONO		   (0x6UL << 0)
-	#define CFA_L2_FILTER_ALLOC_REQ_SRC_TYPE_TANG		   (0x7UL << 0)
+	#define CFA_L2_FILTER_ALLOC_REQ_SRC_TYPE_NPORT		   0x0UL
+	#define CFA_L2_FILTER_ALLOC_REQ_SRC_TYPE_PF		   0x1UL
+	#define CFA_L2_FILTER_ALLOC_REQ_SRC_TYPE_VF		   0x2UL
+	#define CFA_L2_FILTER_ALLOC_REQ_SRC_TYPE_VNIC		   0x3UL
+	#define CFA_L2_FILTER_ALLOC_REQ_SRC_TYPE_KONG		   0x4UL
+	#define CFA_L2_FILTER_ALLOC_REQ_SRC_TYPE_APE		   0x5UL
+	#define CFA_L2_FILTER_ALLOC_REQ_SRC_TYPE_BONO		   0x6UL
+	#define CFA_L2_FILTER_ALLOC_REQ_SRC_TYPE_TANG		   0x7UL
 	u8 unused_6;
 	__le32 src_id;
 	u8 tunnel_type;
-	#define CFA_L2_FILTER_ALLOC_REQ_TUNNEL_TYPE_NONTUNNEL     (0x0UL << 0)
-	#define CFA_L2_FILTER_ALLOC_REQ_TUNNEL_TYPE_VXLAN	   (0x1UL << 0)
-	#define CFA_L2_FILTER_ALLOC_REQ_TUNNEL_TYPE_NVGRE	   (0x2UL << 0)
-	#define CFA_L2_FILTER_ALLOC_REQ_TUNNEL_TYPE_L2GRE	   (0x3UL << 0)
-	#define CFA_L2_FILTER_ALLOC_REQ_TUNNEL_TYPE_IPIP	   (0x4UL << 0)
-	#define CFA_L2_FILTER_ALLOC_REQ_TUNNEL_TYPE_GENEVE	   (0x5UL << 0)
-	#define CFA_L2_FILTER_ALLOC_REQ_TUNNEL_TYPE_MPLS	   (0x6UL << 0)
-	#define CFA_L2_FILTER_ALLOC_REQ_TUNNEL_TYPE_STT	   (0x7UL << 0)
-	#define CFA_L2_FILTER_ALLOC_REQ_TUNNEL_TYPE_IPGRE	   (0x8UL << 0)
-	#define CFA_L2_FILTER_ALLOC_REQ_TUNNEL_TYPE_ANYTUNNEL     (0xffUL << 0)
+	#define CFA_L2_FILTER_ALLOC_REQ_TUNNEL_TYPE_NONTUNNEL     0x0UL
+	#define CFA_L2_FILTER_ALLOC_REQ_TUNNEL_TYPE_VXLAN	   0x1UL
+	#define CFA_L2_FILTER_ALLOC_REQ_TUNNEL_TYPE_NVGRE	   0x2UL
+	#define CFA_L2_FILTER_ALLOC_REQ_TUNNEL_TYPE_L2GRE	   0x3UL
+	#define CFA_L2_FILTER_ALLOC_REQ_TUNNEL_TYPE_IPIP	   0x4UL
+	#define CFA_L2_FILTER_ALLOC_REQ_TUNNEL_TYPE_GENEVE	   0x5UL
+	#define CFA_L2_FILTER_ALLOC_REQ_TUNNEL_TYPE_MPLS	   0x6UL
+	#define CFA_L2_FILTER_ALLOC_REQ_TUNNEL_TYPE_STT	   0x7UL
+	#define CFA_L2_FILTER_ALLOC_REQ_TUNNEL_TYPE_IPGRE	   0x8UL
+	#define CFA_L2_FILTER_ALLOC_REQ_TUNNEL_TYPE_ANYTUNNEL     0xffUL
 	u8 unused_7;
 	__le16 dst_id;
 	__le16 mirror_vnic_id;
 	u8 pri_hint;
-	#define CFA_L2_FILTER_ALLOC_REQ_PRI_HINT_NO_PREFER	   (0x0UL << 0)
-	#define CFA_L2_FILTER_ALLOC_REQ_PRI_HINT_ABOVE_FILTER     (0x1UL << 0)
-	#define CFA_L2_FILTER_ALLOC_REQ_PRI_HINT_BELOW_FILTER     (0x2UL << 0)
-	#define CFA_L2_FILTER_ALLOC_REQ_PRI_HINT_MAX		   (0x3UL << 0)
-	#define CFA_L2_FILTER_ALLOC_REQ_PRI_HINT_MIN		   (0x4UL << 0)
+	#define CFA_L2_FILTER_ALLOC_REQ_PRI_HINT_NO_PREFER	   0x0UL
+	#define CFA_L2_FILTER_ALLOC_REQ_PRI_HINT_ABOVE_FILTER     0x1UL
+	#define CFA_L2_FILTER_ALLOC_REQ_PRI_HINT_BELOW_FILTER     0x2UL
+	#define CFA_L2_FILTER_ALLOC_REQ_PRI_HINT_MAX		   0x3UL
+	#define CFA_L2_FILTER_ALLOC_REQ_PRI_HINT_MIN		   0x4UL
 	u8 unused_8;
 	__le32 unused_9;
 	__le64 l2_filter_id_hint;
@@ -3246,16 +3448,16 @@ struct hwrm_cfa_tunnel_filter_alloc_input {
 	u8 l3_addr_type;
 	u8 t_l3_addr_type;
 	u8 tunnel_type;
-	#define CFA_TUNNEL_FILTER_ALLOC_REQ_TUNNEL_TYPE_NONTUNNEL (0x0UL << 0)
-	#define CFA_TUNNEL_FILTER_ALLOC_REQ_TUNNEL_TYPE_VXLAN     (0x1UL << 0)
-	#define CFA_TUNNEL_FILTER_ALLOC_REQ_TUNNEL_TYPE_NVGRE     (0x2UL << 0)
-	#define CFA_TUNNEL_FILTER_ALLOC_REQ_TUNNEL_TYPE_L2GRE     (0x3UL << 0)
-	#define CFA_TUNNEL_FILTER_ALLOC_REQ_TUNNEL_TYPE_IPIP      (0x4UL << 0)
-	#define CFA_TUNNEL_FILTER_ALLOC_REQ_TUNNEL_TYPE_GENEVE    (0x5UL << 0)
-	#define CFA_TUNNEL_FILTER_ALLOC_REQ_TUNNEL_TYPE_MPLS      (0x6UL << 0)
-	#define CFA_TUNNEL_FILTER_ALLOC_REQ_TUNNEL_TYPE_STT       (0x7UL << 0)
-	#define CFA_TUNNEL_FILTER_ALLOC_REQ_TUNNEL_TYPE_IPGRE     (0x8UL << 0)
-	#define CFA_TUNNEL_FILTER_ALLOC_REQ_TUNNEL_TYPE_ANYTUNNEL (0xffUL << 0)
+	#define CFA_TUNNEL_FILTER_ALLOC_REQ_TUNNEL_TYPE_NONTUNNEL 0x0UL
+	#define CFA_TUNNEL_FILTER_ALLOC_REQ_TUNNEL_TYPE_VXLAN     0x1UL
+	#define CFA_TUNNEL_FILTER_ALLOC_REQ_TUNNEL_TYPE_NVGRE     0x2UL
+	#define CFA_TUNNEL_FILTER_ALLOC_REQ_TUNNEL_TYPE_L2GRE     0x3UL
+	#define CFA_TUNNEL_FILTER_ALLOC_REQ_TUNNEL_TYPE_IPIP      0x4UL
+	#define CFA_TUNNEL_FILTER_ALLOC_REQ_TUNNEL_TYPE_GENEVE    0x5UL
+	#define CFA_TUNNEL_FILTER_ALLOC_REQ_TUNNEL_TYPE_MPLS      0x6UL
+	#define CFA_TUNNEL_FILTER_ALLOC_REQ_TUNNEL_TYPE_STT       0x7UL
+	#define CFA_TUNNEL_FILTER_ALLOC_REQ_TUNNEL_TYPE_IPGRE     0x8UL
+	#define CFA_TUNNEL_FILTER_ALLOC_REQ_TUNNEL_TYPE_ANYTUNNEL 0xffUL
 	u8 unused_0;
 	__le32 vni;
 	__le32 dst_vnic_id;
@@ -3311,14 +3513,14 @@ struct hwrm_cfa_encap_record_alloc_input {
 	__le32 flags;
 	#define CFA_ENCAP_RECORD_ALLOC_REQ_FLAGS_LOOPBACK	    0x1UL
 	u8 encap_type;
-	#define CFA_ENCAP_RECORD_ALLOC_REQ_ENCAP_TYPE_VXLAN       (0x1UL << 0)
-	#define CFA_ENCAP_RECORD_ALLOC_REQ_ENCAP_TYPE_NVGRE       (0x2UL << 0)
-	#define CFA_ENCAP_RECORD_ALLOC_REQ_ENCAP_TYPE_L2GRE       (0x3UL << 0)
-	#define CFA_ENCAP_RECORD_ALLOC_REQ_ENCAP_TYPE_IPIP	   (0x4UL << 0)
-	#define CFA_ENCAP_RECORD_ALLOC_REQ_ENCAP_TYPE_GENEVE      (0x5UL << 0)
-	#define CFA_ENCAP_RECORD_ALLOC_REQ_ENCAP_TYPE_MPLS	   (0x6UL << 0)
-	#define CFA_ENCAP_RECORD_ALLOC_REQ_ENCAP_TYPE_VLAN	   (0x7UL << 0)
-	#define CFA_ENCAP_RECORD_ALLOC_REQ_ENCAP_TYPE_IPGRE       (0x8UL << 0)
+	#define CFA_ENCAP_RECORD_ALLOC_REQ_ENCAP_TYPE_VXLAN       0x1UL
+	#define CFA_ENCAP_RECORD_ALLOC_REQ_ENCAP_TYPE_NVGRE       0x2UL
+	#define CFA_ENCAP_RECORD_ALLOC_REQ_ENCAP_TYPE_L2GRE       0x3UL
+	#define CFA_ENCAP_RECORD_ALLOC_REQ_ENCAP_TYPE_IPIP	   0x4UL
+	#define CFA_ENCAP_RECORD_ALLOC_REQ_ENCAP_TYPE_GENEVE      0x5UL
+	#define CFA_ENCAP_RECORD_ALLOC_REQ_ENCAP_TYPE_MPLS	   0x6UL
+	#define CFA_ENCAP_RECORD_ALLOC_REQ_ENCAP_TYPE_VLAN	   0x7UL
+	#define CFA_ENCAP_RECORD_ALLOC_REQ_ENCAP_TYPE_IPGRE       0x8UL
 	u8 unused_0;
 	__le16 unused_1;
 	__le32 encap_data[16];
@@ -3397,32 +3599,32 @@ struct hwrm_cfa_ntuple_filter_alloc_input {
 	u8 src_macaddr[6];
 	__be16 ethertype;
 	u8 ip_addr_type;
-	#define CFA_NTUPLE_FILTER_ALLOC_REQ_IP_ADDR_TYPE_UNKNOWN  (0x0UL << 0)
-	#define CFA_NTUPLE_FILTER_ALLOC_REQ_IP_ADDR_TYPE_IPV4     (0x4UL << 0)
-	#define CFA_NTUPLE_FILTER_ALLOC_REQ_IP_ADDR_TYPE_IPV6     (0x6UL << 0)
+	#define CFA_NTUPLE_FILTER_ALLOC_REQ_IP_ADDR_TYPE_UNKNOWN  0x0UL
+	#define CFA_NTUPLE_FILTER_ALLOC_REQ_IP_ADDR_TYPE_IPV4     0x4UL
+	#define CFA_NTUPLE_FILTER_ALLOC_REQ_IP_ADDR_TYPE_IPV6     0x6UL
 	u8 ip_protocol;
-	#define CFA_NTUPLE_FILTER_ALLOC_REQ_IP_PROTOCOL_UNKNOWN   (0x0UL << 0)
-	#define CFA_NTUPLE_FILTER_ALLOC_REQ_IP_PROTOCOL_UDP       (0x6UL << 0)
-	#define CFA_NTUPLE_FILTER_ALLOC_REQ_IP_PROTOCOL_TCP       (0x11UL << 0)
+	#define CFA_NTUPLE_FILTER_ALLOC_REQ_IP_PROTOCOL_UNKNOWN   0x0UL
+	#define CFA_NTUPLE_FILTER_ALLOC_REQ_IP_PROTOCOL_UDP       0x6UL
+	#define CFA_NTUPLE_FILTER_ALLOC_REQ_IP_PROTOCOL_TCP       0x11UL
 	__le16 dst_id;
 	__le16 mirror_vnic_id;
 	u8 tunnel_type;
-	#define CFA_NTUPLE_FILTER_ALLOC_REQ_TUNNEL_TYPE_NONTUNNEL (0x0UL << 0)
-	#define CFA_NTUPLE_FILTER_ALLOC_REQ_TUNNEL_TYPE_VXLAN     (0x1UL << 0)
-	#define CFA_NTUPLE_FILTER_ALLOC_REQ_TUNNEL_TYPE_NVGRE     (0x2UL << 0)
-	#define CFA_NTUPLE_FILTER_ALLOC_REQ_TUNNEL_TYPE_L2GRE     (0x3UL << 0)
-	#define CFA_NTUPLE_FILTER_ALLOC_REQ_TUNNEL_TYPE_IPIP      (0x4UL << 0)
-	#define CFA_NTUPLE_FILTER_ALLOC_REQ_TUNNEL_TYPE_GENEVE    (0x5UL << 0)
-	#define CFA_NTUPLE_FILTER_ALLOC_REQ_TUNNEL_TYPE_MPLS      (0x6UL << 0)
-	#define CFA_NTUPLE_FILTER_ALLOC_REQ_TUNNEL_TYPE_STT       (0x7UL << 0)
-	#define CFA_NTUPLE_FILTER_ALLOC_REQ_TUNNEL_TYPE_IPGRE     (0x8UL << 0)
-	#define CFA_NTUPLE_FILTER_ALLOC_REQ_TUNNEL_TYPE_ANYTUNNEL (0xffUL << 0)
+	#define CFA_NTUPLE_FILTER_ALLOC_REQ_TUNNEL_TYPE_NONTUNNEL 0x0UL
+	#define CFA_NTUPLE_FILTER_ALLOC_REQ_TUNNEL_TYPE_VXLAN     0x1UL
+	#define CFA_NTUPLE_FILTER_ALLOC_REQ_TUNNEL_TYPE_NVGRE     0x2UL
+	#define CFA_NTUPLE_FILTER_ALLOC_REQ_TUNNEL_TYPE_L2GRE     0x3UL
+	#define CFA_NTUPLE_FILTER_ALLOC_REQ_TUNNEL_TYPE_IPIP      0x4UL
+	#define CFA_NTUPLE_FILTER_ALLOC_REQ_TUNNEL_TYPE_GENEVE    0x5UL
+	#define CFA_NTUPLE_FILTER_ALLOC_REQ_TUNNEL_TYPE_MPLS      0x6UL
+	#define CFA_NTUPLE_FILTER_ALLOC_REQ_TUNNEL_TYPE_STT       0x7UL
+	#define CFA_NTUPLE_FILTER_ALLOC_REQ_TUNNEL_TYPE_IPGRE     0x8UL
+	#define CFA_NTUPLE_FILTER_ALLOC_REQ_TUNNEL_TYPE_ANYTUNNEL 0xffUL
 	u8 pri_hint;
-	#define CFA_NTUPLE_FILTER_ALLOC_REQ_PRI_HINT_NO_PREFER    (0x0UL << 0)
-	#define CFA_NTUPLE_FILTER_ALLOC_REQ_PRI_HINT_ABOVE	   (0x1UL << 0)
-	#define CFA_NTUPLE_FILTER_ALLOC_REQ_PRI_HINT_BELOW	   (0x2UL << 0)
-	#define CFA_NTUPLE_FILTER_ALLOC_REQ_PRI_HINT_HIGHEST      (0x3UL << 0)
-	#define CFA_NTUPLE_FILTER_ALLOC_REQ_PRI_HINT_LOWEST       (0x4UL << 0)
+	#define CFA_NTUPLE_FILTER_ALLOC_REQ_PRI_HINT_NO_PREFER    0x0UL
+	#define CFA_NTUPLE_FILTER_ALLOC_REQ_PRI_HINT_ABOVE	   0x1UL
+	#define CFA_NTUPLE_FILTER_ALLOC_REQ_PRI_HINT_BELOW	   0x2UL
+	#define CFA_NTUPLE_FILTER_ALLOC_REQ_PRI_HINT_HIGHEST      0x3UL
+	#define CFA_NTUPLE_FILTER_ALLOC_REQ_PRI_HINT_LOWEST       0x4UL
 	__be32 src_ipaddr[4];
 	__be32 src_ipaddr_mask[4];
 	__be32 dst_ipaddr[4];
@@ -3511,8 +3713,8 @@ struct hwrm_tunnel_dst_port_query_input {
 	__le16 target_id;
 	__le64 resp_addr;
 	u8 tunnel_type;
-	#define TUNNEL_DST_PORT_QUERY_REQ_TUNNEL_TYPE_VXLAN       (0x1UL << 0)
-	#define TUNNEL_DST_PORT_QUERY_REQ_TUNNEL_TYPE_GENEVE      (0x5UL << 0)
+	#define TUNNEL_DST_PORT_QUERY_REQ_TUNNEL_TYPE_VXLAN       0x1UL
+	#define TUNNEL_DST_PORT_QUERY_REQ_TUNNEL_TYPE_GENEVE      0x5UL
 	u8 unused_0[7];
 };
 
@@ -3539,8 +3741,8 @@ struct hwrm_tunnel_dst_port_alloc_input {
 	__le16 target_id;
 	__le64 resp_addr;
 	u8 tunnel_type;
-	#define TUNNEL_DST_PORT_ALLOC_REQ_TUNNEL_TYPE_VXLAN       (0x1UL << 0)
-	#define TUNNEL_DST_PORT_ALLOC_REQ_TUNNEL_TYPE_GENEVE      (0x5UL << 0)
+	#define TUNNEL_DST_PORT_ALLOC_REQ_TUNNEL_TYPE_VXLAN       0x1UL
+	#define TUNNEL_DST_PORT_ALLOC_REQ_TUNNEL_TYPE_GENEVE      0x5UL
 	u8 unused_0;
 	__be16 tunnel_dst_port_val;
 	__le32 unused_1;
@@ -3570,8 +3772,8 @@ struct hwrm_tunnel_dst_port_free_input {
 	__le16 target_id;
 	__le64 resp_addr;
 	u8 tunnel_type;
-	#define TUNNEL_DST_PORT_FREE_REQ_TUNNEL_TYPE_VXLAN	   (0x1UL << 0)
-	#define TUNNEL_DST_PORT_FREE_REQ_TUNNEL_TYPE_GENEVE       (0x5UL << 0)
+	#define TUNNEL_DST_PORT_FREE_REQ_TUNNEL_TYPE_VXLAN	   0x1UL
+	#define TUNNEL_DST_PORT_FREE_REQ_TUNNEL_TYPE_GENEVE       0x5UL
 	u8 unused_0;
 	__le16 tunnel_dst_port_id;
 	__le32 unused_1;
@@ -3720,15 +3922,15 @@ struct hwrm_fw_reset_input {
 	__le16 target_id;
 	__le64 resp_addr;
 	u8 embedded_proc_type;
-	#define FW_RESET_REQ_EMBEDDED_PROC_TYPE_BOOT		   (0x0UL << 0)
-	#define FW_RESET_REQ_EMBEDDED_PROC_TYPE_MGMT		   (0x1UL << 0)
-	#define FW_RESET_REQ_EMBEDDED_PROC_TYPE_NETCTRL	   (0x2UL << 0)
-	#define FW_RESET_REQ_EMBEDDED_PROC_TYPE_ROCE		   (0x3UL << 0)
-	#define FW_RESET_REQ_EMBEDDED_PROC_TYPE_RSVD		   (0x4UL << 0)
+	#define FW_RESET_REQ_EMBEDDED_PROC_TYPE_BOOT		   0x0UL
+	#define FW_RESET_REQ_EMBEDDED_PROC_TYPE_MGMT		   0x1UL
+	#define FW_RESET_REQ_EMBEDDED_PROC_TYPE_NETCTRL	   0x2UL
+	#define FW_RESET_REQ_EMBEDDED_PROC_TYPE_ROCE		   0x3UL
+	#define FW_RESET_REQ_EMBEDDED_PROC_TYPE_RSVD		   0x4UL
 	u8 selfrst_status;
-	#define FW_RESET_REQ_SELFRST_STATUS_SELFRSTNONE	   (0x0UL << 0)
-	#define FW_RESET_REQ_SELFRST_STATUS_SELFRSTASAP	   (0x1UL << 0)
-	#define FW_RESET_REQ_SELFRST_STATUS_SELFRSTPCIERST	   (0x2UL << 0)
+	#define FW_RESET_REQ_SELFRST_STATUS_SELFRSTNONE	   0x0UL
+	#define FW_RESET_REQ_SELFRST_STATUS_SELFRSTASAP	   0x1UL
+	#define FW_RESET_REQ_SELFRST_STATUS_SELFRSTPCIERST	   0x2UL
 	__le16 unused_0[3];
 };
 
@@ -3739,9 +3941,9 @@ struct hwrm_fw_reset_output {
 	__le16 seq_id;
 	__le16 resp_len;
 	u8 selfrst_status;
-	#define FW_RESET_RESP_SELFRST_STATUS_SELFRSTNONE	   (0x0UL << 0)
-	#define FW_RESET_RESP_SELFRST_STATUS_SELFRSTASAP	   (0x1UL << 0)
-	#define FW_RESET_RESP_SELFRST_STATUS_SELFRSTPCIERST       (0x2UL << 0)
+	#define FW_RESET_RESP_SELFRST_STATUS_SELFRSTNONE	   0x0UL
+	#define FW_RESET_RESP_SELFRST_STATUS_SELFRSTASAP	   0x1UL
+	#define FW_RESET_RESP_SELFRST_STATUS_SELFRSTPCIERST       0x2UL
 	u8 unused_0;
 	__le16 unused_1;
 	u8 unused_2;
@@ -3759,11 +3961,11 @@ struct hwrm_fw_qstatus_input {
 	__le16 target_id;
 	__le64 resp_addr;
 	u8 embedded_proc_type;
-	#define FW_QSTATUS_REQ_EMBEDDED_PROC_TYPE_BOOT		   (0x0UL << 0)
-	#define FW_QSTATUS_REQ_EMBEDDED_PROC_TYPE_MGMT		   (0x1UL << 0)
-	#define FW_QSTATUS_REQ_EMBEDDED_PROC_TYPE_NETCTRL	   (0x2UL << 0)
-	#define FW_QSTATUS_REQ_EMBEDDED_PROC_TYPE_ROCE		   (0x3UL << 0)
-	#define FW_QSTATUS_REQ_EMBEDDED_PROC_TYPE_RSVD		   (0x4UL << 0)
+	#define FW_QSTATUS_REQ_EMBEDDED_PROC_TYPE_BOOT		   0x0UL
+	#define FW_QSTATUS_REQ_EMBEDDED_PROC_TYPE_MGMT		   0x1UL
+	#define FW_QSTATUS_REQ_EMBEDDED_PROC_TYPE_NETCTRL	   0x2UL
+	#define FW_QSTATUS_REQ_EMBEDDED_PROC_TYPE_ROCE		   0x3UL
+	#define FW_QSTATUS_REQ_EMBEDDED_PROC_TYPE_RSVD		   0x4UL
 	u8 unused_0[7];
 };
 
@@ -3774,9 +3976,9 @@ struct hwrm_fw_qstatus_output {
 	__le16 seq_id;
 	__le16 resp_len;
 	u8 selfrst_status;
-	#define FW_QSTATUS_RESP_SELFRST_STATUS_SELFRSTNONE	   (0x0UL << 0)
-	#define FW_QSTATUS_RESP_SELFRST_STATUS_SELFRSTASAP	   (0x1UL << 0)
-	#define FW_QSTATUS_RESP_SELFRST_STATUS_SELFRSTPCIERST     (0x2UL << 0)
+	#define FW_QSTATUS_RESP_SELFRST_STATUS_SELFRSTNONE	   0x0UL
+	#define FW_QSTATUS_RESP_SELFRST_STATUS_SELFRSTASAP	   0x1UL
+	#define FW_QSTATUS_RESP_SELFRST_STATUS_SELFRSTPCIERST     0x2UL
 	u8 unused_0;
 	__le16 unused_1;
 	u8 unused_2;
@@ -3785,6 +3987,42 @@ struct hwrm_fw_qstatus_output {
 	u8 valid;
 };
 
+/* hwrm_fw_set_time */
+/* Input (32 bytes) */
+struct hwrm_fw_set_time_input {
+	__le16 req_type;
+	__le16 cmpl_ring;
+	__le16 seq_id;
+	__le16 target_id;
+	__le64 resp_addr;
+	__le16 year;
+	#define FW_SET_TIME_REQ_YEAR_UNKNOWN			   0x0UL
+	u8 month;
+	u8 day;
+	u8 hour;
+	u8 minute;
+	u8 second;
+	u8 unused_0;
+	__le16 millisecond;
+	__le16 zone;
+	#define FW_SET_TIME_REQ_ZONE_UTC			   0x0UL
+	#define FW_SET_TIME_REQ_ZONE_UNKNOWN			   0xffffUL
+	__le32 unused_1;
+};
+
+/* Output (16 bytes) */
+struct hwrm_fw_set_time_output {
+	__le16 error_code;
+	__le16 req_type;
+	__le16 seq_id;
+	__le16 resp_len;
+	__le32 unused_0;
+	u8 unused_1;
+	u8 unused_2;
+	u8 unused_3;
+	u8 valid;
+};
+
 /* hwrm_exec_fwd_resp */
 /* Input (128 bytes) */
 struct hwrm_exec_fwd_resp_input {
@@ -3921,32 +4159,6 @@ struct hwrm_temp_monitor_query_output {
 	u8 valid;
 };
 
-/* hwrm_nvm_raw_write_blk */
-/* Input (32 bytes) */
-struct hwrm_nvm_raw_write_blk_input {
-	__le16 req_type;
-	__le16 cmpl_ring;
-	__le16 seq_id;
-	__le16 target_id;
-	__le64 resp_addr;
-	__le64 host_src_addr;
-	__le32 dest_addr;
-	__le32 len;
-};
-
-/* Output (16 bytes) */
-struct hwrm_nvm_raw_write_blk_output {
-	__le16 error_code;
-	__le16 req_type;
-	__le16 seq_id;
-	__le16 resp_len;
-	__le32 unused_0;
-	u8 unused_1;
-	u8 unused_2;
-	u8 unused_3;
-	u8 valid;
-};
-
 /* hwrm_nvm_read */
 /* Input (40 bytes) */
 struct hwrm_nvm_read_input {
@@ -4132,9 +4344,9 @@ struct hwrm_nvm_find_dir_entry_input {
 	u8 opt_ordinal;
 	#define NVM_FIND_DIR_ENTRY_REQ_OPT_ORDINAL_MASK	    0x3UL
 	#define NVM_FIND_DIR_ENTRY_REQ_OPT_ORDINAL_SFT		    0
-	#define NVM_FIND_DIR_ENTRY_REQ_OPT_ORDINAL_EQ		   (0x0UL << 0)
-	#define NVM_FIND_DIR_ENTRY_REQ_OPT_ORDINAL_GE		   (0x1UL << 0)
-	#define NVM_FIND_DIR_ENTRY_REQ_OPT_ORDINAL_GT		   (0x2UL << 0)
+	#define NVM_FIND_DIR_ENTRY_REQ_OPT_ORDINAL_EQ		   0x0UL
+	#define NVM_FIND_DIR_ENTRY_REQ_OPT_ORDINAL_GE		   0x1UL
+	#define NVM_FIND_DIR_ENTRY_REQ_OPT_ORDINAL_GT		   0x2UL
 	u8 unused_1[3];
 };
 
@@ -4266,4 +4478,41 @@ struct hwrm_nvm_verify_update_output {
 	u8 valid;
 };
 
+/* hwrm_nvm_install_update */
+/* Input (24 bytes) */
+struct hwrm_nvm_install_update_input {
+	__le16 req_type;
+	__le16 cmpl_ring;
+	__le16 seq_id;
+	__le16 target_id;
+	__le64 resp_addr;
+	__le32 install_type;
+	#define NVM_INSTALL_UPDATE_REQ_INSTALL_TYPE_NORMAL	   0x0UL
+	#define NVM_INSTALL_UPDATE_REQ_INSTALL_TYPE_ALL	   0xffffffffUL
+	__le32 unused_0;
+};
+
+/* Output (24 bytes) */
+struct hwrm_nvm_install_update_output {
+	__le16 error_code;
+	__le16 req_type;
+	__le16 seq_id;
+	__le16 resp_len;
+	__le64 installed_items;
+	u8 result;
+	#define NVM_INSTALL_UPDATE_RESP_RESULT_SUCCESS		   0x0UL
+	u8 problem_item;
+	#define NVM_INSTALL_UPDATE_RESP_PROBLEM_ITEM_NONE	   0x0UL
+	#define NVM_INSTALL_UPDATE_RESP_PROBLEM_ITEM_PACKAGE      0xffUL
+	u8 reset_required;
+	#define NVM_INSTALL_UPDATE_RESP_RESET_REQUIRED_NONE       0x0UL
+	#define NVM_INSTALL_UPDATE_RESP_RESET_REQUIRED_PCI	   0x1UL
+	#define NVM_INSTALL_UPDATE_RESP_RESET_REQUIRED_POWER      0x2UL
+	u8 unused_0;
+	u8 unused_1;
+	u8 unused_2;
+	u8 unused_3;
+	u8 valid;
+};
+
 #endif
diff --git a/drivers/net/ethernet/broadcom/bnxt/bnxt_sriov.c b/drivers/net/ethernet/broadcom/bnxt/bnxt_sriov.c
index 50d2007a2640..ec6cd18842c3 100644
--- a/drivers/net/ethernet/broadcom/bnxt/bnxt_sriov.c
+++ b/drivers/net/ethernet/broadcom/bnxt/bnxt_sriov.c
@@ -19,6 +19,45 @@
 #include "bnxt_ethtool.h"
 
 #ifdef CONFIG_BNXT_SRIOV
+static int bnxt_hwrm_fwd_async_event_cmpl(struct bnxt *bp,
+					  struct bnxt_vf_info *vf, u16 event_id)
+{
+	struct hwrm_fwd_async_event_cmpl_output *resp = bp->hwrm_cmd_resp_addr;
+	struct hwrm_fwd_async_event_cmpl_input req = {0};
+	struct hwrm_async_event_cmpl *async_cmpl;
+	int rc = 0;
+
+	bnxt_hwrm_cmd_hdr_init(bp, &req, HWRM_FWD_ASYNC_EVENT_CMPL, -1, -1);
+	if (vf)
+		req.encap_async_event_target_id = cpu_to_le16(vf->fw_fid);
+	else
+		/* broadcast this async event to all VFs */
+		req.encap_async_event_target_id = cpu_to_le16(0xffff);
+	async_cmpl = (struct hwrm_async_event_cmpl *)req.encap_async_event_cmpl;
+	async_cmpl->type =
+		cpu_to_le16(HWRM_ASYNC_EVENT_CMPL_TYPE_HWRM_ASYNC_EVENT);
+	async_cmpl->event_id = cpu_to_le16(event_id);
+
+	mutex_lock(&bp->hwrm_cmd_lock);
+	rc = _hwrm_send_message(bp, &req, sizeof(req), HWRM_CMD_TIMEOUT);
+
+	if (rc) {
+		netdev_err(bp->dev, "hwrm_fwd_async_event_cmpl failed. rc:%d\n",
+			   rc);
+		goto fwd_async_event_cmpl_exit;
+	}
+
+	if (resp->error_code) {
+		netdev_err(bp->dev, "hwrm_fwd_async_event_cmpl error %d\n",
+			   resp->error_code);
+		rc = -1;
+	}
+
+fwd_async_event_cmpl_exit:
+	mutex_unlock(&bp->hwrm_cmd_lock);
+	return rc;
+}
+
 static int bnxt_vf_ndo_prep(struct bnxt *bp, int vf_id)
 {
 	if (!test_bit(BNXT_STATE_OPEN, &bp->state)) {
@@ -135,7 +174,8 @@ int bnxt_set_vf_mac(struct net_device *dev, int vf_id, u8 *mac)
 	return hwrm_send_message(bp, &req, sizeof(req), HWRM_CMD_TIMEOUT);
 }
 
-int bnxt_set_vf_vlan(struct net_device *dev, int vf_id, u16 vlan_id, u8 qos)
+int bnxt_set_vf_vlan(struct net_device *dev, int vf_id, u16 vlan_id, u8 qos,
+		     __be16 vlan_proto)
 {
 	struct hwrm_func_cfg_input req = {0};
 	struct bnxt *bp = netdev_priv(dev);
@@ -146,6 +186,9 @@ int bnxt_set_vf_vlan(struct net_device *dev, int vf_id, u16 vlan_id, u8 qos)
 	if (bp->hwrm_spec_code < 0x10201)
 		return -ENOTSUPP;
 
+	if (vlan_proto != htons(ETH_P_8021Q))
+		return -EPROTONOSUPPORT;
+
 	rc = bnxt_vf_ndo_prep(bp, vf_id);
 	if (rc)
 		return rc;
@@ -243,8 +286,9 @@ int bnxt_set_vf_link_state(struct net_device *dev, int vf_id, int link)
 		rc = -EINVAL;
 		break;
 	}
-	/* CHIMP TODO: send msg to VF to update new link state */
-
+	if (vf->flags & (BNXT_VF_LINK_UP | BNXT_VF_LINK_FORCED))
+		rc = bnxt_hwrm_fwd_async_event_cmpl(bp, vf,
+			HWRM_ASYNC_EVENT_CMPL_EVENT_ID_LINK_STATUS_CHANGE);
 	return rc;
 }
 
@@ -525,46 +569,6 @@ err_out1:
 	return rc;
 }
 
-static int bnxt_hwrm_fwd_async_event_cmpl(struct bnxt *bp,
-					  struct bnxt_vf_info *vf,
-					  u16 event_id)
-{
-	int rc = 0;
-	struct hwrm_fwd_async_event_cmpl_input req = {0};
-	struct hwrm_fwd_async_event_cmpl_output *resp = bp->hwrm_cmd_resp_addr;
-	struct hwrm_async_event_cmpl *async_cmpl;
-
-	bnxt_hwrm_cmd_hdr_init(bp, &req, HWRM_FWD_ASYNC_EVENT_CMPL, -1, -1);
-	if (vf)
-		req.encap_async_event_target_id = cpu_to_le16(vf->fw_fid);
-	else
-		/* broadcast this async event to all VFs */
-		req.encap_async_event_target_id = cpu_to_le16(0xffff);
-	async_cmpl = (struct hwrm_async_event_cmpl *)req.encap_async_event_cmpl;
-	async_cmpl->type =
-		cpu_to_le16(HWRM_ASYNC_EVENT_CMPL_TYPE_HWRM_ASYNC_EVENT);
-	async_cmpl->event_id = cpu_to_le16(event_id);
-
-	mutex_lock(&bp->hwrm_cmd_lock);
-	rc = _hwrm_send_message(bp, &req, sizeof(req), HWRM_CMD_TIMEOUT);
-
-	if (rc) {
-		netdev_err(bp->dev, "hwrm_fwd_async_event_cmpl failed. rc:%d\n",
-			   rc);
-		goto fwd_async_event_cmpl_exit;
-	}
-
-	if (resp->error_code) {
-		netdev_err(bp->dev, "hwrm_fwd_async_event_cmpl error %d\n",
-			   resp->error_code);
-		rc = -1;
-	}
-
-fwd_async_event_cmpl_exit:
-	mutex_unlock(&bp->hwrm_cmd_lock);
-	return rc;
-}
-
 void bnxt_sriov_disable(struct bnxt *bp)
 {
 	u16 num_vfs = pci_num_vf(bp->pdev);
diff --git a/drivers/net/ethernet/broadcom/bnxt/bnxt_sriov.h b/drivers/net/ethernet/broadcom/bnxt/bnxt_sriov.h
index 0392670ab49c..1ab72e4820af 100644
--- a/drivers/net/ethernet/broadcom/bnxt/bnxt_sriov.h
+++ b/drivers/net/ethernet/broadcom/bnxt/bnxt_sriov.h
@@ -12,7 +12,7 @@
 
 int bnxt_get_vf_config(struct net_device *, int, struct ifla_vf_info *);
 int bnxt_set_vf_mac(struct net_device *, int, u8 *);
-int bnxt_set_vf_vlan(struct net_device *, int, u16, u8);
+int bnxt_set_vf_vlan(struct net_device *, int, u16, u8, __be16);
 int bnxt_set_vf_bw(struct net_device *, int, int, int);
 int bnxt_set_vf_link_state(struct net_device *, int, int);
 int bnxt_set_vf_spoofchk(struct net_device *, int, bool);
diff --git a/drivers/net/ethernet/broadcom/genet/bcmgenet.c b/drivers/net/ethernet/broadcom/genet/bcmgenet.c
index 8d4f8495dbb3..4464bc5db934 100644
--- a/drivers/net/ethernet/broadcom/genet/bcmgenet.c
+++ b/drivers/net/ethernet/broadcom/genet/bcmgenet.c
@@ -450,28 +450,32 @@ static inline void bcmgenet_rdma_ring_writel(struct bcmgenet_priv *priv,
 			genet_dma_ring_regs[r]);
 }
 
-static int bcmgenet_get_settings(struct net_device *dev,
-				 struct ethtool_cmd *cmd)
+static int bcmgenet_get_link_ksettings(struct net_device *dev,
+				       struct ethtool_link_ksettings *cmd)
 {
+	struct bcmgenet_priv *priv = netdev_priv(dev);
+
 	if (!netif_running(dev))
 		return -EINVAL;
 
-	if (!dev->phydev)
+	if (!priv->phydev)
 		return -ENODEV;
 
-	return phy_ethtool_gset(dev->phydev, cmd);
+	return phy_ethtool_ksettings_get(priv->phydev, cmd);
 }
 
-static int bcmgenet_set_settings(struct net_device *dev,
-				 struct ethtool_cmd *cmd)
+static int bcmgenet_set_link_ksettings(struct net_device *dev,
+				       const struct ethtool_link_ksettings *cmd)
 {
+	struct bcmgenet_priv *priv = netdev_priv(dev);
+
 	if (!netif_running(dev))
 		return -EINVAL;
 
-	if (!dev->phydev)
+	if (!priv->phydev)
 		return -ENODEV;
 
-	return phy_ethtool_sset(dev->phydev, cmd);
+	return phy_ethtool_ksettings_set(priv->phydev, cmd);
 }
 
 static int bcmgenet_set_rx_csum(struct net_device *dev,
@@ -937,7 +941,7 @@ static int bcmgenet_get_eee(struct net_device *dev, struct ethtool_eee *e)
 	e->eee_active = p->eee_active;
 	e->tx_lpi_timer = bcmgenet_umac_readl(priv, UMAC_EEE_LPI_TIMER);
 
-	return phy_ethtool_get_eee(dev->phydev, e);
+	return phy_ethtool_get_eee(priv->phydev, e);
 }
 
 static int bcmgenet_set_eee(struct net_device *dev, struct ethtool_eee *e)
@@ -954,7 +958,7 @@ static int bcmgenet_set_eee(struct net_device *dev, struct ethtool_eee *e)
 	if (!p->eee_enabled) {
 		bcmgenet_eee_enable_set(dev, false);
 	} else {
-		ret = phy_init_eee(dev->phydev, 0);
+		ret = phy_init_eee(priv->phydev, 0);
 		if (ret) {
 			netif_err(priv, hw, dev, "EEE initialization failed\n");
 			return ret;
@@ -964,21 +968,21 @@ static int bcmgenet_set_eee(struct net_device *dev, struct ethtool_eee *e)
 		bcmgenet_eee_enable_set(dev, true);
 	}
 
-	return phy_ethtool_set_eee(dev->phydev, e);
+	return phy_ethtool_set_eee(priv->phydev, e);
 }
 
 static int bcmgenet_nway_reset(struct net_device *dev)
 {
-	return genphy_restart_aneg(dev->phydev);
+	struct bcmgenet_priv *priv = netdev_priv(dev);
+
+	return genphy_restart_aneg(priv->phydev);
 }
 
 /* standard ethtool support functions. */
-static struct ethtool_ops bcmgenet_ethtool_ops = {
+static const struct ethtool_ops bcmgenet_ethtool_ops = {
 	.get_strings		= bcmgenet_get_strings,
 	.get_sset_count		= bcmgenet_get_sset_count,
 	.get_ethtool_stats	= bcmgenet_get_ethtool_stats,
-	.get_settings		= bcmgenet_get_settings,
-	.set_settings		= bcmgenet_set_settings,
 	.get_drvinfo		= bcmgenet_get_drvinfo,
 	.get_link		= ethtool_op_get_link,
 	.get_msglevel		= bcmgenet_get_msglevel,
@@ -990,19 +994,20 @@ static struct ethtool_ops bcmgenet_ethtool_ops = {
 	.nway_reset		= bcmgenet_nway_reset,
 	.get_coalesce		= bcmgenet_get_coalesce,
 	.set_coalesce		= bcmgenet_set_coalesce,
+	.get_link_ksettings	= bcmgenet_get_link_ksettings,
+	.set_link_ksettings	= bcmgenet_set_link_ksettings,
 };
 
 /* Power down the unimac, based on mode. */
 static int bcmgenet_power_down(struct bcmgenet_priv *priv,
 				enum bcmgenet_power_mode mode)
 {
-	struct net_device *ndev = priv->dev;
 	int ret = 0;
 	u32 reg;
 
 	switch (mode) {
 	case GENET_POWER_CABLE_SENSE:
-		phy_detach(ndev->phydev);
+		phy_detach(priv->phydev);
 		break;
 
 	case GENET_POWER_WOL_MAGIC:
@@ -1063,6 +1068,7 @@ static void bcmgenet_power_up(struct bcmgenet_priv *priv,
 /* ioctl handle special commands that are not present in ethtool. */
 static int bcmgenet_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
 {
+	struct bcmgenet_priv *priv = netdev_priv(dev);
 	int val = 0;
 
 	if (!netif_running(dev))
@@ -1072,10 +1078,10 @@ static int bcmgenet_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
 	case SIOCGMIIPHY:
 	case SIOCGMIIREG:
 	case SIOCSMIIREG:
-		if (!dev->phydev)
+		if (!priv->phydev)
 			val = -ENODEV;
 		else
-			val = phy_mii_ioctl(dev->phydev, rq, cmd);
+			val = phy_mii_ioctl(priv->phydev, rq, cmd);
 		break;
 
 	default:
@@ -2458,7 +2464,6 @@ static void bcmgenet_irq_task(struct work_struct *work)
 {
 	struct bcmgenet_priv *priv = container_of(
 			work, struct bcmgenet_priv, bcmgenet_irq_work);
-	struct net_device *ndev = priv->dev;
 
 	netif_dbg(priv, intr, priv->dev, "%s\n", __func__);
 
@@ -2471,7 +2476,7 @@ static void bcmgenet_irq_task(struct work_struct *work)
 
 	/* Link UP/DOWN event */
 	if (priv->irq0_stat & UMAC_IRQ_LINK_EVENT) {
-		phy_mac_interrupt(ndev->phydev,
+		phy_mac_interrupt(priv->phydev,
 				  !!(priv->irq0_stat & UMAC_IRQ_LINK_UP));
 		priv->irq0_stat &= ~UMAC_IRQ_LINK_EVENT;
 	}
@@ -2664,128 +2669,6 @@ static void bcmgenet_enable_dma(struct bcmgenet_priv *priv, u32 dma_ctrl)
 	bcmgenet_tdma_writel(priv, reg, DMA_CTRL);
 }
 
-static bool bcmgenet_hfb_is_filter_enabled(struct bcmgenet_priv *priv,
-					   u32 f_index)
-{
-	u32 offset;
-	u32 reg;
-
-	offset = HFB_FLT_ENABLE_V3PLUS + (f_index < 32) * sizeof(u32);
-	reg = bcmgenet_hfb_reg_readl(priv, offset);
-	return !!(reg & (1 << (f_index % 32)));
-}
-
-static void bcmgenet_hfb_enable_filter(struct bcmgenet_priv *priv, u32 f_index)
-{
-	u32 offset;
-	u32 reg;
-
-	offset = HFB_FLT_ENABLE_V3PLUS + (f_index < 32) * sizeof(u32);
-	reg = bcmgenet_hfb_reg_readl(priv, offset);
-	reg |= (1 << (f_index % 32));
-	bcmgenet_hfb_reg_writel(priv, reg, offset);
-}
-
-static void bcmgenet_hfb_set_filter_rx_queue_mapping(struct bcmgenet_priv *priv,
-						     u32 f_index, u32 rx_queue)
-{
-	u32 offset;
-	u32 reg;
-
-	offset = f_index / 8;
-	reg = bcmgenet_rdma_readl(priv, DMA_INDEX2RING_0 + offset);
-	reg &= ~(0xF << (4 * (f_index % 8)));
-	reg |= ((rx_queue & 0xF) << (4 * (f_index % 8)));
-	bcmgenet_rdma_writel(priv, reg, DMA_INDEX2RING_0 + offset);
-}
-
-static void bcmgenet_hfb_set_filter_length(struct bcmgenet_priv *priv,
-					   u32 f_index, u32 f_length)
-{
-	u32 offset;
-	u32 reg;
-
-	offset = HFB_FLT_LEN_V3PLUS +
-		 ((priv->hw_params->hfb_filter_cnt - 1 - f_index) / 4) *
-		 sizeof(u32);
-	reg = bcmgenet_hfb_reg_readl(priv, offset);
-	reg &= ~(0xFF << (8 * (f_index % 4)));
-	reg |= ((f_length & 0xFF) << (8 * (f_index % 4)));
-	bcmgenet_hfb_reg_writel(priv, reg, offset);
-}
-
-static int bcmgenet_hfb_find_unused_filter(struct bcmgenet_priv *priv)
-{
-	u32 f_index;
-
-	for (f_index = 0; f_index < priv->hw_params->hfb_filter_cnt; f_index++)
-		if (!bcmgenet_hfb_is_filter_enabled(priv, f_index))
-			return f_index;
-
-	return -ENOMEM;
-}
-
-/* bcmgenet_hfb_add_filter
- *
- * Add new filter to Hardware Filter Block to match and direct Rx traffic to
- * desired Rx queue.
- *
- * f_data is an array of unsigned 32-bit integers where each 32-bit integer
- * provides filter data for 2 bytes (4 nibbles) of Rx frame:
- *
- * bits 31:20 - unused
- * bit  19    - nibble 0 match enable
- * bit  18    - nibble 1 match enable
- * bit  17    - nibble 2 match enable
- * bit  16    - nibble 3 match enable
- * bits 15:12 - nibble 0 data
- * bits 11:8  - nibble 1 data
- * bits 7:4   - nibble 2 data
- * bits 3:0   - nibble 3 data
- *
- * Example:
- * In order to match:
- * - Ethernet frame type = 0x0800 (IP)
- * - IP version field = 4
- * - IP protocol field = 0x11 (UDP)
- *
- * The following filter is needed:
- * u32 hfb_filter_ipv4_udp[] = {
- *   Rx frame offset 0x00: 0x00000000, 0x00000000, 0x00000000, 0x00000000,
- *   Rx frame offset 0x08: 0x00000000, 0x00000000, 0x000F0800, 0x00084000,
- *   Rx frame offset 0x10: 0x00000000, 0x00000000, 0x00000000, 0x00030011,
- * };
- *
- * To add the filter to HFB and direct the traffic to Rx queue 0, call:
- * bcmgenet_hfb_add_filter(priv, hfb_filter_ipv4_udp,
- *                         ARRAY_SIZE(hfb_filter_ipv4_udp), 0);
- */
-int bcmgenet_hfb_add_filter(struct bcmgenet_priv *priv, u32 *f_data,
-			    u32 f_length, u32 rx_queue)
-{
-	int f_index;
-	u32 i;
-
-	f_index = bcmgenet_hfb_find_unused_filter(priv);
-	if (f_index < 0)
-		return -ENOMEM;
-
-	if (f_length > priv->hw_params->hfb_filter_size)
-		return -EINVAL;
-
-	for (i = 0; i < f_length; i++)
-		bcmgenet_hfb_writel(priv, f_data[i],
-			(f_index * priv->hw_params->hfb_filter_size + i) *
-			sizeof(u32));
-
-	bcmgenet_hfb_set_filter_length(priv, f_index, 2 * f_length);
-	bcmgenet_hfb_set_filter_rx_queue_mapping(priv, f_index, rx_queue);
-	bcmgenet_hfb_enable_filter(priv, f_index);
-	bcmgenet_hfb_reg_writel(priv, 0x1, HFB_CTRL);
-
-	return 0;
-}
-
 /* bcmgenet_hfb_clear
  *
  * Clear Hardware Filter Block and disable all filtering.
@@ -2833,7 +2716,7 @@ static void bcmgenet_netif_start(struct net_device *dev)
 	/* Monitor link interrupts now */
 	bcmgenet_link_intr_enable(priv);
 
-	phy_start(dev->phydev);
+	phy_start(priv->phydev);
 }
 
 static int bcmgenet_open(struct net_device *dev)
@@ -2932,7 +2815,7 @@ static void bcmgenet_netif_stop(struct net_device *dev)
 	struct bcmgenet_priv *priv = netdev_priv(dev);
 
 	netif_tx_stop_all_queues(dev);
-	phy_stop(dev->phydev);
+	phy_stop(priv->phydev);
 	bcmgenet_intr_disable(priv);
 	bcmgenet_disable_rx_napi(priv);
 	bcmgenet_disable_tx_napi(priv);
@@ -2958,7 +2841,7 @@ static int bcmgenet_close(struct net_device *dev)
 	bcmgenet_netif_stop(dev);
 
 	/* Really kill the PHY state machine and disconnect from it */
-	phy_disconnect(dev->phydev);
+	phy_disconnect(priv->phydev);
 
 	/* Disable MAC receive */
 	umac_enable_set(priv, CMD_RX_EN, false);
@@ -3517,7 +3400,7 @@ static int bcmgenet_suspend(struct device *d)
 
 	bcmgenet_netif_stop(dev);
 
-	phy_suspend(dev->phydev);
+	phy_suspend(priv->phydev);
 
 	netif_device_detach(dev);
 
@@ -3581,7 +3464,7 @@ static int bcmgenet_resume(struct device *d)
 	if (priv->wolopts)
 		clk_disable_unprepare(priv->clk_wol);
 
-	phy_init_hw(dev->phydev);
+	phy_init_hw(priv->phydev);
 	/* Speed settings must be restored */
 	bcmgenet_mii_config(priv->dev);
 
@@ -3614,7 +3497,7 @@ static int bcmgenet_resume(struct device *d)
 
 	netif_device_attach(dev);
 
-	phy_resume(dev->phydev);
+	phy_resume(priv->phydev);
 
 	if (priv->eee.eee_enabled)
 		bcmgenet_eee_enable_set(dev, true);
diff --git a/drivers/net/ethernet/broadcom/genet/bcmgenet.h b/drivers/net/ethernet/broadcom/genet/bcmgenet.h
index 0f0868c56f05..1e2dc34d331a 100644
--- a/drivers/net/ethernet/broadcom/genet/bcmgenet.h
+++ b/drivers/net/ethernet/broadcom/genet/bcmgenet.h
@@ -597,6 +597,7 @@ struct bcmgenet_priv {
 
 	/* MDIO bus variables */
 	wait_queue_head_t wq;
+	struct phy_device *phydev;
 	bool internal_phy;
 	struct device_node *phy_dn;
 	struct device_node *mdio_dn;
diff --git a/drivers/net/ethernet/broadcom/genet/bcmmii.c b/drivers/net/ethernet/broadcom/genet/bcmmii.c
index e907acd81da9..457c3bc8cfff 100644
--- a/drivers/net/ethernet/broadcom/genet/bcmmii.c
+++ b/drivers/net/ethernet/broadcom/genet/bcmmii.c
@@ -86,7 +86,7 @@ static int bcmgenet_mii_write(struct mii_bus *bus, int phy_id,
 void bcmgenet_mii_setup(struct net_device *dev)
 {
 	struct bcmgenet_priv *priv = netdev_priv(dev);
-	struct phy_device *phydev = dev->phydev;
+	struct phy_device *phydev = priv->phydev;
 	u32 reg, cmd_bits = 0;
 	bool status_changed = false;
 
@@ -183,9 +183,9 @@ void bcmgenet_mii_reset(struct net_device *dev)
 	if (GENET_IS_V4(priv))
 		return;
 
-	if (dev->phydev) {
-		phy_init_hw(dev->phydev);
-		phy_start_aneg(dev->phydev);
+	if (priv->phydev) {
+		phy_init_hw(priv->phydev);
+		phy_start_aneg(priv->phydev);
 	}
 }
 
@@ -236,7 +236,6 @@ static void bcmgenet_internal_phy_setup(struct net_device *dev)
 
 static void bcmgenet_moca_phy_setup(struct bcmgenet_priv *priv)
 {
-	struct net_device *ndev = priv->dev;
 	u32 reg;
 
 	/* Speed settings are set in bcmgenet_mii_setup() */
@@ -245,14 +244,14 @@ static void bcmgenet_moca_phy_setup(struct bcmgenet_priv *priv)
 	bcmgenet_sys_writel(priv, reg, SYS_PORT_CTRL);
 
 	if (priv->hw_params->flags & GENET_HAS_MOCA_LINK_DET)
-		fixed_phy_set_link_update(ndev->phydev,
+		fixed_phy_set_link_update(priv->phydev,
 					  bcmgenet_fixed_phy_link_update);
 }
 
 int bcmgenet_mii_config(struct net_device *dev)
 {
 	struct bcmgenet_priv *priv = netdev_priv(dev);
-	struct phy_device *phydev = dev->phydev;
+	struct phy_device *phydev = priv->phydev;
 	struct device *kdev = &priv->pdev->dev;
 	const char *phy_name = NULL;
 	u32 id_mode_dis = 0;
@@ -303,7 +302,7 @@ int bcmgenet_mii_config(struct net_device *dev)
 		 * capabilities, use that knowledge to also configure the
 		 * Reverse MII interface correctly.
 		 */
-		if ((phydev->supported & PHY_BASIC_FEATURES) ==
+		if ((priv->phydev->supported & PHY_BASIC_FEATURES) ==
 				PHY_BASIC_FEATURES)
 			port_ctrl = PORT_MODE_EXT_RVMII_25;
 		else
@@ -372,7 +371,7 @@ int bcmgenet_mii_probe(struct net_device *dev)
 			return -ENODEV;
 		}
 	} else {
-		phydev = dev->phydev;
+		phydev = priv->phydev;
 		phydev->dev_flags = phy_flags;
 
 		ret = phy_connect_direct(dev, phydev, bcmgenet_mii_setup,
@@ -383,6 +382,8 @@ int bcmgenet_mii_probe(struct net_device *dev)
 		}
 	}
 
+	priv->phydev = phydev;
+
 	/* Configure port multiplexer based on what the probed PHY device since
 	 * reading the 'max-speed' property determines the maximum supported
 	 * PHY speed which is needed for bcmgenet_mii_config() to configure
@@ -390,7 +391,7 @@ int bcmgenet_mii_probe(struct net_device *dev)
 	 */
 	ret = bcmgenet_mii_config(dev);
 	if (ret) {
-		phy_disconnect(phydev);
+		phy_disconnect(priv->phydev);
 		return ret;
 	}
 
@@ -400,7 +401,7 @@ int bcmgenet_mii_probe(struct net_device *dev)
 	 * Ethernet MAC ISRs
 	 */
 	if (priv->internal_phy)
-		phydev->irq = PHY_IGNORE_INTERRUPT;
+		priv->phydev->irq = PHY_IGNORE_INTERRUPT;
 
 	return 0;
 }
@@ -605,6 +606,7 @@ static int bcmgenet_mii_pd_init(struct bcmgenet_priv *priv)
 
 	}
 
+	priv->phydev = phydev;
 	priv->phy_interface = pd->phy_interface;
 
 	return 0;
diff --git a/drivers/net/ethernet/broadcom/tg3.c b/drivers/net/ethernet/broadcom/tg3.c
index a2551bcd1027..a927a730da10 100644
--- a/drivers/net/ethernet/broadcom/tg3.c
+++ b/drivers/net/ethernet/broadcom/tg3.c
@@ -12079,95 +12079,107 @@ static int tg3_set_eeprom(struct net_device *dev, struct ethtool_eeprom *eeprom,
 	return ret;
 }
 
-static int tg3_get_settings(struct net_device *dev, struct ethtool_cmd *cmd)
+static int tg3_get_link_ksettings(struct net_device *dev,
+				  struct ethtool_link_ksettings *cmd)
 {
 	struct tg3 *tp = netdev_priv(dev);
+	u32 supported, advertising;
 
 	if (tg3_flag(tp, USE_PHYLIB)) {
 		struct phy_device *phydev;
 		if (!(tp->phy_flags & TG3_PHYFLG_IS_CONNECTED))
 			return -EAGAIN;
 		phydev = mdiobus_get_phy(tp->mdio_bus, tp->phy_addr);
-		return phy_ethtool_gset(phydev, cmd);
+		return phy_ethtool_ksettings_get(phydev, cmd);
 	}
 
-	cmd->supported = (SUPPORTED_Autoneg);
+	supported = (SUPPORTED_Autoneg);
 
 	if (!(tp->phy_flags & TG3_PHYFLG_10_100_ONLY))
-		cmd->supported |= (SUPPORTED_1000baseT_Half |
-				   SUPPORTED_1000baseT_Full);
+		supported |= (SUPPORTED_1000baseT_Half |
+			      SUPPORTED_1000baseT_Full);
 
 	if (!(tp->phy_flags & TG3_PHYFLG_ANY_SERDES)) {
-		cmd->supported |= (SUPPORTED_100baseT_Half |
-				  SUPPORTED_100baseT_Full |
-				  SUPPORTED_10baseT_Half |
-				  SUPPORTED_10baseT_Full |
-				  SUPPORTED_TP);
-		cmd->port = PORT_TP;
+		supported |= (SUPPORTED_100baseT_Half |
+			      SUPPORTED_100baseT_Full |
+			      SUPPORTED_10baseT_Half |
+			      SUPPORTED_10baseT_Full |
+			      SUPPORTED_TP);
+		cmd->base.port = PORT_TP;
 	} else {
-		cmd->supported |= SUPPORTED_FIBRE;
-		cmd->port = PORT_FIBRE;
+		supported |= SUPPORTED_FIBRE;
+		cmd->base.port = PORT_FIBRE;
 	}
+	ethtool_convert_legacy_u32_to_link_mode(cmd->link_modes.supported,
+						supported);
 
-	cmd->advertising = tp->link_config.advertising;
+	advertising = tp->link_config.advertising;
 	if (tg3_flag(tp, PAUSE_AUTONEG)) {
 		if (tp->link_config.flowctrl & FLOW_CTRL_RX) {
 			if (tp->link_config.flowctrl & FLOW_CTRL_TX) {
-				cmd->advertising |= ADVERTISED_Pause;
+				advertising |= ADVERTISED_Pause;
 			} else {
-				cmd->advertising |= ADVERTISED_Pause |
-						    ADVERTISED_Asym_Pause;
+				advertising |= ADVERTISED_Pause |
+					ADVERTISED_Asym_Pause;
 			}
 		} else if (tp->link_config.flowctrl & FLOW_CTRL_TX) {
-			cmd->advertising |= ADVERTISED_Asym_Pause;
+			advertising |= ADVERTISED_Asym_Pause;
 		}
 	}
+	ethtool_convert_legacy_u32_to_link_mode(cmd->link_modes.advertising,
+						advertising);
+
 	if (netif_running(dev) && tp->link_up) {
-		ethtool_cmd_speed_set(cmd, tp->link_config.active_speed);
-		cmd->duplex = tp->link_config.active_duplex;
-		cmd->lp_advertising = tp->link_config.rmt_adv;
+		cmd->base.speed = tp->link_config.active_speed;
+		cmd->base.duplex = tp->link_config.active_duplex;
+		ethtool_convert_legacy_u32_to_link_mode(
+			cmd->link_modes.lp_advertising,
+			tp->link_config.rmt_adv);
+
 		if (!(tp->phy_flags & TG3_PHYFLG_ANY_SERDES)) {
 			if (tp->phy_flags & TG3_PHYFLG_MDIX_STATE)
-				cmd->eth_tp_mdix = ETH_TP_MDI_X;
+				cmd->base.eth_tp_mdix = ETH_TP_MDI_X;
 			else
-				cmd->eth_tp_mdix = ETH_TP_MDI;
+				cmd->base.eth_tp_mdix = ETH_TP_MDI;
 		}
 	} else {
-		ethtool_cmd_speed_set(cmd, SPEED_UNKNOWN);
-		cmd->duplex = DUPLEX_UNKNOWN;
-		cmd->eth_tp_mdix = ETH_TP_MDI_INVALID;
-	}
-	cmd->phy_address = tp->phy_addr;
-	cmd->transceiver = XCVR_INTERNAL;
-	cmd->autoneg = tp->link_config.autoneg;
-	cmd->maxtxpkt = 0;
-	cmd->maxrxpkt = 0;
+		cmd->base.speed = SPEED_UNKNOWN;
+		cmd->base.duplex = DUPLEX_UNKNOWN;
+		cmd->base.eth_tp_mdix = ETH_TP_MDI_INVALID;
+	}
+	cmd->base.phy_address = tp->phy_addr;
+	cmd->base.autoneg = tp->link_config.autoneg;
 	return 0;
 }
 
-static int tg3_set_settings(struct net_device *dev, struct ethtool_cmd *cmd)
+static int tg3_set_link_ksettings(struct net_device *dev,
+				  const struct ethtool_link_ksettings *cmd)
 {
 	struct tg3 *tp = netdev_priv(dev);
-	u32 speed = ethtool_cmd_speed(cmd);
+	u32 speed = cmd->base.speed;
+	u32 advertising;
 
 	if (tg3_flag(tp, USE_PHYLIB)) {
 		struct phy_device *phydev;
 		if (!(tp->phy_flags & TG3_PHYFLG_IS_CONNECTED))
 			return -EAGAIN;
 		phydev = mdiobus_get_phy(tp->mdio_bus, tp->phy_addr);
-		return phy_ethtool_sset(phydev, cmd);
+		return phy_ethtool_ksettings_set(phydev, cmd);
 	}
 
-	if (cmd->autoneg != AUTONEG_ENABLE &&
-	    cmd->autoneg != AUTONEG_DISABLE)
+	if (cmd->base.autoneg != AUTONEG_ENABLE &&
+	    cmd->base.autoneg != AUTONEG_DISABLE)
 		return -EINVAL;
 
-	if (cmd->autoneg == AUTONEG_DISABLE &&
-	    cmd->duplex != DUPLEX_FULL &&
-	    cmd->duplex != DUPLEX_HALF)
+	if (cmd->base.autoneg == AUTONEG_DISABLE &&
+	    cmd->base.duplex != DUPLEX_FULL &&
+	    cmd->base.duplex != DUPLEX_HALF)
 		return -EINVAL;
 
-	if (cmd->autoneg == AUTONEG_ENABLE) {
+	ethtool_convert_link_mode_to_legacy_u32(&advertising,
+						cmd->link_modes.advertising);
+
+	if (cmd->base.autoneg == AUTONEG_ENABLE) {
 		u32 mask = ADVERTISED_Autoneg |
 			   ADVERTISED_Pause |
 			   ADVERTISED_Asym_Pause;
@@ -12185,7 +12197,7 @@ static int tg3_set_settings(struct net_device *dev, struct ethtool_cmd *cmd)
 		else
 			mask |= ADVERTISED_FIBRE;
 
-		if (cmd->advertising & ~mask)
+		if (advertising & ~mask)
 			return -EINVAL;
 
 		mask &= (ADVERTISED_1000baseT_Half |
@@ -12195,13 +12207,13 @@ static int tg3_set_settings(struct net_device *dev, struct ethtool_cmd *cmd)
 			 ADVERTISED_10baseT_Half |
 			 ADVERTISED_10baseT_Full);
 
-		cmd->advertising &= mask;
+		advertising &= mask;
 	} else {
 		if (tp->phy_flags & TG3_PHYFLG_ANY_SERDES) {
 			if (speed != SPEED_1000)
 				return -EINVAL;
 
-			if (cmd->duplex != DUPLEX_FULL)
+			if (cmd->base.duplex != DUPLEX_FULL)
 				return -EINVAL;
 		} else {
 			if (speed != SPEED_100 &&
@@ -12212,16 +12224,16 @@ static int tg3_set_settings(struct net_device *dev, struct ethtool_cmd *cmd)
 
 	tg3_full_lock(tp, 0);
 
-	tp->link_config.autoneg = cmd->autoneg;
-	if (cmd->autoneg == AUTONEG_ENABLE) {
-		tp->link_config.advertising = (cmd->advertising |
+	tp->link_config.autoneg = cmd->base.autoneg;
+	if (cmd->base.autoneg == AUTONEG_ENABLE) {
+		tp->link_config.advertising = (advertising |
 					      ADVERTISED_Autoneg);
 		tp->link_config.speed = SPEED_UNKNOWN;
 		tp->link_config.duplex = DUPLEX_UNKNOWN;
 	} else {
 		tp->link_config.advertising = 0;
 		tp->link_config.speed = speed;
-		tp->link_config.duplex = cmd->duplex;
+		tp->link_config.duplex = cmd->base.duplex;
 	}
 
 	tp->phy_flags |= TG3_PHYFLG_USER_CONFIGURED;
@@ -14094,8 +14106,6 @@ static int tg3_get_eee(struct net_device *dev, struct ethtool_eee *edata)
 }
 
 static const struct ethtool_ops tg3_ethtool_ops = {
-	.get_settings		= tg3_get_settings,
-	.set_settings		= tg3_set_settings,
 	.get_drvinfo		= tg3_get_drvinfo,
 	.get_regs_len		= tg3_get_regs_len,
 	.get_regs		= tg3_get_regs,
@@ -14128,6 +14138,8 @@ static const struct ethtool_ops tg3_ethtool_ops = {
 	.get_ts_info		= tg3_get_ts_info,
 	.get_eee		= tg3_get_eee,
 	.set_eee		= tg3_set_eee,
+	.get_link_ksettings	= tg3_get_link_ksettings,
+	.set_link_ksettings	= tg3_set_link_ksettings,
 };
 
 static struct rtnl_link_stats64 *tg3_get_stats64(struct net_device *dev,
@@ -18122,14 +18134,14 @@ static pci_ers_result_t tg3_io_error_detected(struct pci_dev *pdev,
 
 	rtnl_lock();
 
-	/* We needn't recover from permanent error */
-	if (state == pci_channel_io_frozen)
-		tp->pcierr_recovery = true;
-
 	/* We probably don't have netdev yet */
 	if (!netdev || !netif_running(netdev))
 		goto done;
 
+	/* We needn't recover from permanent error */
+	if (state == pci_channel_io_frozen)
+		tp->pcierr_recovery = true;
+
 	tg3_phy_stop(tp);
 
 	tg3_netif_stop(tp);
@@ -18226,7 +18238,7 @@ static void tg3_io_resume(struct pci_dev *pdev)
 
 	rtnl_lock();
 
-	if (!netif_running(netdev))
+	if (!netdev || !netif_running(netdev))
 		goto done;
 
 	tg3_full_lock(tp, 0);
diff --git a/drivers/net/ethernet/brocade/bna/bnad.c b/drivers/net/ethernet/brocade/bna/bnad.c
index 771cc267f217..f9df4b5ae90e 100644
--- a/drivers/net/ethernet/brocade/bna/bnad.c
+++ b/drivers/net/ethernet/brocade/bna/bnad.c
@@ -54,9 +54,7 @@ MODULE_PARM_DESC(bna_debugfs_enable, "Enables debugfs feature, default=1,"
  * Global variables
  */
 static u32 bnad_rxqs_per_cq = 2;
-static u32 bna_id;
-static struct mutex bnad_list_mutex;
-static LIST_HEAD(bnad_list);
+static atomic_t bna_id;
 static const u8 bnad_bcast_addr[] __aligned(2) =
 	{ 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
 
@@ -76,23 +74,6 @@ do {								\
 	(_res_info)->res_u.mem_info.len = (_size);		\
 } while (0)
 
-static void
-bnad_add_to_list(struct bnad *bnad)
-{
-	mutex_lock(&bnad_list_mutex);
-	list_add_tail(&bnad->list_entry, &bnad_list);
-	bnad->id = bna_id++;
-	mutex_unlock(&bnad_list_mutex);
-}
-
-static void
-bnad_remove_from_list(struct bnad *bnad)
-{
-	mutex_lock(&bnad_list_mutex);
-	list_del(&bnad->list_entry);
-	mutex_unlock(&bnad_list_mutex);
-}
-
 /*
  * Reinitialize completions in CQ, once Rx is taken down
  */
@@ -3573,14 +3554,12 @@ bnad_lock_init(struct bnad *bnad)
 {
 	spin_lock_init(&bnad->bna_lock);
 	mutex_init(&bnad->conf_mutex);
-	mutex_init(&bnad_list_mutex);
 }
 
 static void
 bnad_lock_uninit(struct bnad *bnad)
 {
 	mutex_destroy(&bnad->conf_mutex);
-	mutex_destroy(&bnad_list_mutex);
 }
 
 /* PCI Initialization */
@@ -3653,7 +3632,7 @@ bnad_pci_probe(struct pci_dev *pdev,
 	}
 	bnad = netdev_priv(netdev);
 	bnad_lock_init(bnad);
-	bnad_add_to_list(bnad);
+	bnad->id = atomic_inc_return(&bna_id) - 1;
 
 	mutex_lock(&bnad->conf_mutex);
 	/*
@@ -3807,7 +3786,6 @@ pci_uninit:
 	bnad_pci_uninit(pdev);
 unlock_mutex:
 	mutex_unlock(&bnad->conf_mutex);
-	bnad_remove_from_list(bnad);
 	bnad_lock_uninit(bnad);
 	free_netdev(netdev);
 	return err;
@@ -3845,7 +3823,6 @@ bnad_pci_remove(struct pci_dev *pdev)
 	bnad_disable_msix(bnad);
 	bnad_pci_uninit(pdev);
 	mutex_unlock(&bnad->conf_mutex);
-	bnad_remove_from_list(bnad);
 	bnad_lock_uninit(bnad);
 	/* Remove the debugfs node for this bnad */
 	kfree(bnad->regdata);
diff --git a/drivers/net/ethernet/brocade/bna/bnad.h b/drivers/net/ethernet/brocade/bna/bnad.h
index f4ed816b93ee..46f7b842b39c 100644
--- a/drivers/net/ethernet/brocade/bna/bnad.h
+++ b/drivers/net/ethernet/brocade/bna/bnad.h
@@ -288,7 +288,6 @@ struct bnad_rx_unmap_q {
 struct bnad {
 	struct net_device	*netdev;
 	u32			id;
-	struct list_head	list_entry;
 
 	/* Data path */
 	struct bnad_tx_info tx_info[BNAD_MAX_TX];
diff --git a/drivers/net/ethernet/cadence/macb.c b/drivers/net/ethernet/cadence/macb.c
index d954a97b0b0b..b32444a3ed79 100644
--- a/drivers/net/ethernet/cadence/macb.c
+++ b/drivers/net/ethernet/cadence/macb.c
@@ -541,6 +541,14 @@ static void macb_tx_unmap(struct macb *bp, struct macb_tx_skb *tx_skb)
 	}
 }
 
+static inline void macb_set_addr(struct macb_dma_desc *desc, dma_addr_t addr)
+{
+	desc->addr = (u32)addr;
+#ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
+	desc->addrh = (u32)(addr >> 32);
+#endif
+}
+
 static void macb_tx_error_task(struct work_struct *work)
 {
 	struct macb_queue	*queue = container_of(work, struct macb_queue,
@@ -621,14 +629,17 @@ static void macb_tx_error_task(struct work_struct *work)
 
 	/* Set end of TX queue */
 	desc = macb_tx_desc(queue, 0);
-	desc->addr = 0;
+	macb_set_addr(desc, 0);
 	desc->ctrl = MACB_BIT(TX_USED);
 
 	/* Make descriptor updates visible to hardware */
 	wmb();
 
 	/* Reinitialize the TX desc queue */
-	queue_writel(queue, TBQP, queue->tx_ring_dma);
+	queue_writel(queue, TBQP, (u32)(queue->tx_ring_dma));
+#ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
+	queue_writel(queue, TBQPH, (u32)(queue->tx_ring_dma >> 32));
+#endif
 	/* Make TX ring reflect state of hardware */
 	queue->tx_head = 0;
 	queue->tx_tail = 0;
@@ -750,7 +761,7 @@ static void gem_rx_refill(struct macb *bp)
 
 			if (entry == RX_RING_SIZE - 1)
 				paddr |= MACB_BIT(RX_WRAP);
-			bp->rx_ring[entry].addr = paddr;
+			macb_set_addr(&(bp->rx_ring[entry]), paddr);
 			bp->rx_ring[entry].ctrl = 0;
 
 			/* properly align Ethernet header */
@@ -798,7 +809,9 @@ static int gem_rx(struct macb *bp, int budget)
 	int			count = 0;
 
 	while (count < budget) {
-		u32 addr, ctrl;
+		u32 ctrl;
+		dma_addr_t addr;
+		bool rxused;
 
 		entry = macb_rx_ring_wrap(bp->rx_tail);
 		desc = &bp->rx_ring[entry];
@@ -806,10 +819,14 @@ static int gem_rx(struct macb *bp, int budget)
 		/* Make hw descriptor updates visible to CPU */
 		rmb();
 
-		addr = desc->addr;
+		rxused = (desc->addr & MACB_BIT(RX_USED)) ? true : false;
+		addr = MACB_BF(RX_WADDR, MACB_BFEXT(RX_WADDR, desc->addr));
+#ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
+		addr |= ((u64)(desc->addrh) << 32);
+#endif
 		ctrl = desc->ctrl;
 
-		if (!(addr & MACB_BIT(RX_USED)))
+		if (!rxused)
 			break;
 
 		bp->rx_tail++;
@@ -835,7 +852,6 @@ static int gem_rx(struct macb *bp, int budget)
 		netdev_vdbg(bp->dev, "gem_rx %u (len %u)\n", entry, len);
 
 		skb_put(skb, len);
-		addr = MACB_BF(RX_WADDR, MACB_BFEXT(RX_WADDR, addr));
 		dma_unmap_single(&bp->pdev->dev, addr,
 				 bp->rx_buffer_size, DMA_FROM_DEVICE);
 
@@ -1299,7 +1315,7 @@ static unsigned int macb_tx_map(struct macb *bp,
 			ctrl |= MACB_BIT(TX_WRAP);
 
 		/* Set TX buffer descriptor */
-		desc->addr = tx_skb->mapping;
+		macb_set_addr(desc, tx_skb->mapping);
 		/* desc->addr must be visible to hardware before clearing
 		 * 'TX_USED' bit in desc->ctrl.
 		 */
@@ -1382,7 +1398,7 @@ static int macb_start_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	if (macb_clear_csum(skb)) {
 		dev_kfree_skb_any(skb);
-		return NETDEV_TX_OK;
+		goto unlock;
 	}
 
 	/* Map socket buffer for DMA transfer */
@@ -1445,6 +1461,9 @@ static void gem_free_rx_buffers(struct macb *bp)
 
 		desc = &bp->rx_ring[i];
 		addr = MACB_BF(RX_WADDR, MACB_BFEXT(RX_WADDR, desc->addr));
+#ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
+		addr |= ((u64)(desc->addrh) << 32);
+#endif
 		dma_unmap_single(&bp->pdev->dev, addr, bp->rx_buffer_size,
 				 DMA_FROM_DEVICE);
 		dev_kfree_skb_any(skb);
@@ -1570,7 +1589,7 @@ static void gem_init_rings(struct macb *bp)
 
 	for (q = 0, queue = bp->queues; q < bp->num_queues; ++q, ++queue) {
 		for (i = 0; i < TX_RING_SIZE; i++) {
-			queue->tx_ring[i].addr = 0;
+			macb_set_addr(&(queue->tx_ring[i]), 0);
 			queue->tx_ring[i].ctrl = MACB_BIT(TX_USED);
 		}
 		queue->tx_ring[TX_RING_SIZE - 1].ctrl |= MACB_BIT(TX_WRAP);
@@ -1717,6 +1736,10 @@ static void macb_configure_dma(struct macb *bp)
 			dmacfg |= GEM_BIT(TXCOEN);
 		else
 			dmacfg &= ~GEM_BIT(TXCOEN);
+
+#ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
+		dmacfg |= GEM_BIT(ADDR64);
+#endif
 		netdev_dbg(bp->dev, "Cadence configure DMA with 0x%08x\n",
 			   dmacfg);
 		gem_writel(bp, DMACFG, dmacfg);
@@ -1762,9 +1785,15 @@ static void macb_init_hw(struct macb *bp)
 	macb_configure_dma(bp);
 
 	/* Initialize TX and RX buffers */
-	macb_writel(bp, RBQP, bp->rx_ring_dma);
+	macb_writel(bp, RBQP, (u32)(bp->rx_ring_dma));
+#ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
+	macb_writel(bp, RBQPH, (u32)(bp->rx_ring_dma >> 32));
+#endif
 	for (q = 0, queue = bp->queues; q < bp->num_queues; ++q, ++queue) {
-		queue_writel(queue, TBQP, queue->tx_ring_dma);
+		queue_writel(queue, TBQP, (u32)(queue->tx_ring_dma));
+#ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
+		queue_writel(queue, TBQPH, (u32)(queue->tx_ring_dma >> 32));
+#endif
 
 		/* Enable interrupts */
 		queue_writel(queue, IER,
@@ -2326,7 +2355,8 @@ static void macb_probe_queues(void __iomem *mem,
 }
 
 static int macb_clk_init(struct platform_device *pdev, struct clk **pclk,
-			 struct clk **hclk, struct clk **tx_clk)
+			 struct clk **hclk, struct clk **tx_clk,
+			 struct clk **rx_clk)
 {
 	int err;
 
@@ -2348,6 +2378,10 @@ static int macb_clk_init(struct platform_device *pdev, struct clk **pclk,
 	if (IS_ERR(*tx_clk))
 		*tx_clk = NULL;
 
+	*rx_clk = devm_clk_get(&pdev->dev, "rx_clk");
+	if (IS_ERR(*rx_clk))
+		*rx_clk = NULL;
+
 	err = clk_prepare_enable(*pclk);
 	if (err) {
 		dev_err(&pdev->dev, "failed to enable pclk (%u)\n", err);
@@ -2366,8 +2400,17 @@ static int macb_clk_init(struct platform_device *pdev, struct clk **pclk,
 		goto err_disable_hclk;
 	}
 
+	err = clk_prepare_enable(*rx_clk);
+	if (err) {
+		dev_err(&pdev->dev, "failed to enable rx_clk (%u)\n", err);
+		goto err_disable_txclk;
+	}
+
 	return 0;
 
+err_disable_txclk:
+	clk_disable_unprepare(*tx_clk);
+
 err_disable_hclk:
 	clk_disable_unprepare(*hclk);
 
@@ -2402,6 +2445,9 @@ static int macb_init(struct platform_device *pdev)
 			queue->IDR  = GEM_IDR(hw_q - 1);
 			queue->IMR  = GEM_IMR(hw_q - 1);
 			queue->TBQP = GEM_TBQP(hw_q - 1);
+#ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
+			queue->TBQPH = GEM_TBQPH(hw_q -1);
+#endif
 		} else {
 			/* queue0 uses legacy registers */
 			queue->ISR  = MACB_ISR;
@@ -2409,6 +2455,9 @@ static int macb_init(struct platform_device *pdev)
 			queue->IDR  = MACB_IDR;
 			queue->IMR  = MACB_IMR;
 			queue->TBQP = MACB_TBQP;
+#ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
+			queue->TBQPH = MACB_TBQPH;
+#endif
 		}
 
 		/* get irq: here we use the linux queue index, not the hardware
@@ -2751,12 +2800,14 @@ static const struct net_device_ops at91ether_netdev_ops = {
 };
 
 static int at91ether_clk_init(struct platform_device *pdev, struct clk **pclk,
-			      struct clk **hclk, struct clk **tx_clk)
+			      struct clk **hclk, struct clk **tx_clk,
+			      struct clk **rx_clk)
 {
 	int err;
 
 	*hclk = NULL;
 	*tx_clk = NULL;
+	*rx_clk = NULL;
 
 	*pclk = devm_clk_get(&pdev->dev, "ether_clk");
 	if (IS_ERR(*pclk))
@@ -2880,13 +2931,13 @@ MODULE_DEVICE_TABLE(of, macb_dt_ids);
 static int macb_probe(struct platform_device *pdev)
 {
 	int (*clk_init)(struct platform_device *, struct clk **,
-			struct clk **, struct clk **)
+			struct clk **, struct clk **,  struct clk **)
 					      = macb_clk_init;
 	int (*init)(struct platform_device *) = macb_init;
 	struct device_node *np = pdev->dev.of_node;
 	struct device_node *phy_node;
 	const struct macb_config *macb_config = NULL;
-	struct clk *pclk, *hclk = NULL, *tx_clk = NULL;
+	struct clk *pclk, *hclk = NULL, *tx_clk = NULL, *rx_clk = NULL;
 	unsigned int queue_mask, num_queues;
 	struct macb_platform_data *pdata;
 	bool native_io;
@@ -2914,7 +2965,7 @@ static int macb_probe(struct platform_device *pdev)
 		}
 	}
 
-	err = clk_init(pdev, &pclk, &hclk, &tx_clk);
+	err = clk_init(pdev, &pclk, &hclk, &tx_clk, &rx_clk);
 	if (err)
 		return err;
 
@@ -2950,6 +3001,7 @@ static int macb_probe(struct platform_device *pdev)
 	bp->pclk = pclk;
 	bp->hclk = hclk;
 	bp->tx_clk = tx_clk;
+	bp->rx_clk = rx_clk;
 	if (macb_config)
 		bp->jumbo_max_len = macb_config->jumbo_max_len;
 
@@ -2958,6 +3010,11 @@ static int macb_probe(struct platform_device *pdev)
 		bp->wol |= MACB_WOL_HAS_MAGIC_PACKET;
 	device_init_wakeup(&pdev->dev, bp->wol & MACB_WOL_HAS_MAGIC_PACKET);
 
+#ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
+	if (GEM_BFEXT(DBWDEF, gem_readl(bp, DCFG1)) > GEM_DBW32)
+		dma_set_mask(&pdev->dev, DMA_BIT_MASK(44));
+#endif
+
 	spin_lock_init(&bp->lock);
 
 	/* setup capabilities */
@@ -2968,7 +3025,7 @@ static int macb_probe(struct platform_device *pdev)
 	dev->irq = platform_get_irq(pdev, 0);
 	if (dev->irq < 0) {
 		err = dev->irq;
-		goto err_disable_clocks;
+		goto err_out_free_netdev;
 	}
 
 	mac = of_get_mac_address(np);
@@ -3043,6 +3100,7 @@ err_disable_clocks:
 	clk_disable_unprepare(tx_clk);
 	clk_disable_unprepare(hclk);
 	clk_disable_unprepare(pclk);
+	clk_disable_unprepare(rx_clk);
 
 	return err;
 }
@@ -3059,6 +3117,7 @@ static int macb_remove(struct platform_device *pdev)
 		if (dev->phydev)
 			phy_disconnect(dev->phydev);
 		mdiobus_unregister(bp->mii_bus);
+		dev->phydev = NULL;
 		mdiobus_free(bp->mii_bus);
 
 		/* Shutdown the PHY if there is a GPIO reset */
@@ -3069,6 +3128,7 @@ static int macb_remove(struct platform_device *pdev)
 		clk_disable_unprepare(bp->tx_clk);
 		clk_disable_unprepare(bp->hclk);
 		clk_disable_unprepare(bp->pclk);
+		clk_disable_unprepare(bp->rx_clk);
 		free_netdev(dev);
 	}
 
@@ -3092,6 +3152,7 @@ static int __maybe_unused macb_suspend(struct device *dev)
 		clk_disable_unprepare(bp->tx_clk);
 		clk_disable_unprepare(bp->hclk);
 		clk_disable_unprepare(bp->pclk);
+		clk_disable_unprepare(bp->rx_clk);
 	}
 
 	return 0;
@@ -3111,6 +3172,7 @@ static int __maybe_unused macb_resume(struct device *dev)
 		clk_prepare_enable(bp->pclk);
 		clk_prepare_enable(bp->hclk);
 		clk_prepare_enable(bp->tx_clk);
+		clk_prepare_enable(bp->rx_clk);
 	}
 
 	netif_device_attach(netdev);
diff --git a/drivers/net/ethernet/cadence/macb.h b/drivers/net/ethernet/cadence/macb.h
index b6fcf10621b6..8bed4b52fef5 100644
--- a/drivers/net/ethernet/cadence/macb.h
+++ b/drivers/net/ethernet/cadence/macb.h
@@ -66,6 +66,8 @@
 #define MACB_USRIO		0x00c0
 #define MACB_WOL		0x00c4
 #define MACB_MID		0x00fc
+#define MACB_TBQPH		0x04C8
+#define MACB_RBQPH		0x04D4
 
 /* GEM register offsets. */
 #define GEM_NCFGR		0x0004 /* Network Config */
@@ -139,6 +141,7 @@
 
 #define GEM_ISR(hw_q)		(0x0400 + ((hw_q) << 2))
 #define GEM_TBQP(hw_q)		(0x0440 + ((hw_q) << 2))
+#define GEM_TBQPH(hw_q)		(0x04C8)
 #define GEM_RBQP(hw_q)		(0x0480 + ((hw_q) << 2))
 #define GEM_IER(hw_q)		(0x0600 + ((hw_q) << 2))
 #define GEM_IDR(hw_q)		(0x0620 + ((hw_q) << 2))
@@ -249,6 +252,8 @@
 #define GEM_RXBS_SIZE		8
 #define GEM_DDRP_OFFSET		24 /* disc_when_no_ahb */
 #define GEM_DDRP_SIZE		1
+#define GEM_ADDR64_OFFSET	30 /* Address bus width - 64b or 32b */
+#define GEM_ADDR64_SIZE		1
 
 
 /* Bitfields in NSR */
@@ -474,6 +479,10 @@
 struct macb_dma_desc {
 	u32	addr;
 	u32	ctrl;
+#ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
+	u32     addrh;
+	u32     resvd;
+#endif
 };
 
 /* DMA descriptor bitfields */
@@ -763,7 +772,8 @@ struct macb_config {
 	u32			caps;
 	unsigned int		dma_burst_length;
 	int	(*clk_init)(struct platform_device *pdev, struct clk **pclk,
-			    struct clk **hclk, struct clk **tx_clk);
+			    struct clk **hclk, struct clk **tx_clk,
+			    struct clk **rx_clk);
 	int	(*init)(struct platform_device *pdev);
 	int	jumbo_max_len;
 };
@@ -777,6 +787,7 @@ struct macb_queue {
 	unsigned int		IDR;
 	unsigned int		IMR;
 	unsigned int		TBQP;
+	unsigned int		TBQPH;
 
 	unsigned int		tx_head, tx_tail;
 	struct macb_dma_desc	*tx_ring;
@@ -809,6 +820,7 @@ struct macb {
 	struct clk		*pclk;
 	struct clk		*hclk;
 	struct clk		*tx_clk;
+	struct clk		*rx_clk;
 	struct net_device	*dev;
 	struct napi_struct	napi;
 	struct net_device_stats	stats;
diff --git a/drivers/net/ethernet/cavium/Kconfig b/drivers/net/ethernet/cavium/Kconfig
index 0ef232d3331e..92f411c9f0df 100644
--- a/drivers/net/ethernet/cavium/Kconfig
+++ b/drivers/net/ethernet/cavium/Kconfig
@@ -36,10 +36,20 @@ config	THUNDER_NIC_BGX
 	depends on 64BIT
 	select PHYLIB
 	select MDIO_THUNDER
+	select THUNDER_NIC_RGX
 	---help---
 	  This driver supports programming and controlling of MAC
 	  interface from NIC physical function driver.
 
+config	THUNDER_NIC_RGX
+	tristate "Thunder MAC interface driver (RGX)"
+	depends on 64BIT
+	select PHYLIB
+	select MDIO_THUNDER
+	---help---
+	  This driver supports configuring XCV block of RGX interface
+	  present on CN81XX chip.
+
 config LIQUIDIO
 	tristate "Cavium LiquidIO support"
 	depends on 64BIT
@@ -48,7 +58,7 @@ config LIQUIDIO
 	select LIBCRC32C
 	---help---
 	  This driver supports Cavium LiquidIO Intelligent Server Adapters
-	  based on CN66XX and CN68XX chips.
+	  based on CN66XX, CN68XX and CN23XX chips.
 
 	  To compile this driver as a module, choose M here: the module
 	  will be called liquidio.  This is recommended.
diff --git a/drivers/net/ethernet/cavium/liquidio/Makefile b/drivers/net/ethernet/cavium/liquidio/Makefile
index 2f366806835d..5a27b2a44039 100644
--- a/drivers/net/ethernet/cavium/liquidio/Makefile
+++ b/drivers/net/ethernet/cavium/liquidio/Makefile
@@ -3,14 +3,16 @@
 #
 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
+liquidio-$(CONFIG_LIQUIDIO) += lio_ethtool.o \
+			lio_core.o         \
+			request_manager.o  \
+			response_manager.o \
+			octeon_device.o    \
+			cn66xx_device.o    \
+			cn68xx_device.o    \
+			cn23xx_pf_device.o \
+			octeon_mem_ops.o   \
+			octeon_droq.o      \
+			octeon_nic.o
+
+liquidio-objs := lio_main.o octeon_console.o $(liquidio-y)
diff --git a/drivers/net/ethernet/cavium/liquidio/cn23xx_pf_device.c b/drivers/net/ethernet/cavium/liquidio/cn23xx_pf_device.c
new file mode 100644
index 000000000000..380a64115a98
--- /dev/null
+++ b/drivers/net/ethernet/cavium/liquidio/cn23xx_pf_device.c
@@ -0,0 +1,1237 @@
+/**********************************************************************
+* 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/pci.h>
+#include <linux/netdevice.h>
+#include <linux/vmalloc.h>
+#include "liquidio_common.h"
+#include "octeon_droq.h"
+#include "octeon_iq.h"
+#include "response_manager.h"
+#include "octeon_device.h"
+#include "cn23xx_pf_device.h"
+#include "octeon_main.h"
+
+#define RESET_NOTDONE 0
+#define RESET_DONE 1
+
+/* Change the value of SLI Packet Input Jabber Register to allow
+ * VXLAN TSO packets which can be 64424 bytes, exceeding the
+ * MAX_GSO_SIZE we supplied to the kernel
+ */
+#define CN23XX_INPUT_JABBER 64600
+
+#define LIOLUT_RING_DISTRIBUTION 9
+const int liolut_num_vfs_to_rings_per_vf[LIOLUT_RING_DISTRIBUTION] = {
+	0, 8, 4, 2, 2, 2, 1, 1, 1
+};
+
+void cn23xx_dump_pf_initialized_regs(struct octeon_device *oct)
+{
+	int i = 0;
+	u32 regval = 0;
+	struct octeon_cn23xx_pf *cn23xx = (struct octeon_cn23xx_pf *)oct->chip;
+
+	/*In cn23xx_soft_reset*/
+	dev_dbg(&oct->pci_dev->dev, "%s[%llx] : 0x%llx\n",
+		"CN23XX_WIN_WR_MASK_REG", CVM_CAST64(CN23XX_WIN_WR_MASK_REG),
+		CVM_CAST64(octeon_read_csr64(oct, CN23XX_WIN_WR_MASK_REG)));
+	dev_dbg(&oct->pci_dev->dev, "%s[%llx] : 0x%016llx\n",
+		"CN23XX_SLI_SCRATCH1", CVM_CAST64(CN23XX_SLI_SCRATCH1),
+		CVM_CAST64(octeon_read_csr64(oct, CN23XX_SLI_SCRATCH1)));
+	dev_dbg(&oct->pci_dev->dev, "%s[%llx] : 0x%016llx\n",
+		"CN23XX_RST_SOFT_RST", CN23XX_RST_SOFT_RST,
+		lio_pci_readq(oct, CN23XX_RST_SOFT_RST));
+
+	/*In cn23xx_set_dpi_regs*/
+	dev_dbg(&oct->pci_dev->dev, "%s[%llx] : 0x%016llx\n",
+		"CN23XX_DPI_DMA_CONTROL", CN23XX_DPI_DMA_CONTROL,
+		lio_pci_readq(oct, CN23XX_DPI_DMA_CONTROL));
+
+	for (i = 0; i < 6; i++) {
+		dev_dbg(&oct->pci_dev->dev, "%s(%d)[%llx] : 0x%016llx\n",
+			"CN23XX_DPI_DMA_ENG_ENB", i,
+			CN23XX_DPI_DMA_ENG_ENB(i),
+			lio_pci_readq(oct, CN23XX_DPI_DMA_ENG_ENB(i)));
+		dev_dbg(&oct->pci_dev->dev, "%s(%d)[%llx] : 0x%016llx\n",
+			"CN23XX_DPI_DMA_ENG_BUF", i,
+			CN23XX_DPI_DMA_ENG_BUF(i),
+			lio_pci_readq(oct, CN23XX_DPI_DMA_ENG_BUF(i)));
+	}
+
+	dev_dbg(&oct->pci_dev->dev, "%s[%llx] : 0x%016llx\n", "CN23XX_DPI_CTL",
+		CN23XX_DPI_CTL, lio_pci_readq(oct, CN23XX_DPI_CTL));
+
+	/*In cn23xx_setup_pcie_mps and cn23xx_setup_pcie_mrrs */
+	pci_read_config_dword(oct->pci_dev, CN23XX_CONFIG_PCIE_DEVCTL, &regval);
+	dev_dbg(&oct->pci_dev->dev, "%s[%llx] : 0x%016llx\n",
+		"CN23XX_CONFIG_PCIE_DEVCTL",
+		CVM_CAST64(CN23XX_CONFIG_PCIE_DEVCTL), CVM_CAST64(regval));
+
+	dev_dbg(&oct->pci_dev->dev, "%s(%d)[%llx] : 0x%016llx\n",
+		"CN23XX_DPI_SLI_PRTX_CFG", oct->pcie_port,
+		CN23XX_DPI_SLI_PRTX_CFG(oct->pcie_port),
+		lio_pci_readq(oct, CN23XX_DPI_SLI_PRTX_CFG(oct->pcie_port)));
+
+	/*In cn23xx_specific_regs_setup */
+	dev_dbg(&oct->pci_dev->dev, "%s(%d)[%llx] : 0x%016llx\n",
+		"CN23XX_SLI_S2M_PORTX_CTL", oct->pcie_port,
+		CVM_CAST64(CN23XX_SLI_S2M_PORTX_CTL(oct->pcie_port)),
+		CVM_CAST64(octeon_read_csr64(
+			oct, CN23XX_SLI_S2M_PORTX_CTL(oct->pcie_port))));
+
+	dev_dbg(&oct->pci_dev->dev, "%s[%llx] : 0x%016llx\n",
+		"CN23XX_SLI_RING_RST", CVM_CAST64(CN23XX_SLI_PKT_IOQ_RING_RST),
+		(u64)octeon_read_csr64(oct, CN23XX_SLI_PKT_IOQ_RING_RST));
+
+	/*In cn23xx_setup_global_mac_regs*/
+	for (i = 0; i < CN23XX_MAX_MACS; i++) {
+		dev_dbg(&oct->pci_dev->dev, "%s(%d)[%llx] : 0x%016llx\n",
+			"CN23XX_SLI_PKT_MAC_RINFO64", i,
+			CVM_CAST64(CN23XX_SLI_PKT_MAC_RINFO64(i, oct->pf_num)),
+			CVM_CAST64(octeon_read_csr64
+				(oct, CN23XX_SLI_PKT_MAC_RINFO64
+					(i, oct->pf_num))));
+	}
+
+	/*In cn23xx_setup_global_input_regs*/
+	for (i = 0; i < CN23XX_MAX_INPUT_QUEUES; i++) {
+		dev_dbg(&oct->pci_dev->dev, "%s(%d)[%llx] : 0x%016llx\n",
+			"CN23XX_SLI_IQ_PKT_CONTROL64", i,
+			CVM_CAST64(CN23XX_SLI_IQ_PKT_CONTROL64(i)),
+			CVM_CAST64(octeon_read_csr64
+				(oct, CN23XX_SLI_IQ_PKT_CONTROL64(i))));
+	}
+
+	/*In cn23xx_setup_global_output_regs*/
+	dev_dbg(&oct->pci_dev->dev, "%s[%llx] : 0x%016llx\n",
+		"CN23XX_SLI_OQ_WMARK", CVM_CAST64(CN23XX_SLI_OQ_WMARK),
+		CVM_CAST64(octeon_read_csr64(oct, CN23XX_SLI_OQ_WMARK)));
+
+	for (i = 0; i < CN23XX_MAX_OUTPUT_QUEUES; i++) {
+		dev_dbg(&oct->pci_dev->dev, "%s(%d)[%llx] : 0x%016llx\n",
+			"CN23XX_SLI_OQ_PKT_CONTROL", i,
+			CVM_CAST64(CN23XX_SLI_OQ_PKT_CONTROL(i)),
+			CVM_CAST64(octeon_read_csr(
+				oct, CN23XX_SLI_OQ_PKT_CONTROL(i))));
+		dev_dbg(&oct->pci_dev->dev, "%s(%d)[%llx] : 0x%016llx\n",
+			"CN23XX_SLI_OQ_PKT_INT_LEVELS", i,
+			CVM_CAST64(CN23XX_SLI_OQ_PKT_INT_LEVELS(i)),
+			CVM_CAST64(octeon_read_csr64(
+				oct, CN23XX_SLI_OQ_PKT_INT_LEVELS(i))));
+	}
+
+	/*In cn23xx_enable_interrupt and cn23xx_disable_interrupt*/
+	dev_dbg(&oct->pci_dev->dev, "%s[%llx] : 0x%016llx\n",
+		"cn23xx->intr_enb_reg64",
+		CVM_CAST64((long)(cn23xx->intr_enb_reg64)),
+		CVM_CAST64(readq(cn23xx->intr_enb_reg64)));
+
+	dev_dbg(&oct->pci_dev->dev, "%s[%llx] : 0x%016llx\n",
+		"cn23xx->intr_sum_reg64",
+		CVM_CAST64((long)(cn23xx->intr_sum_reg64)),
+		CVM_CAST64(readq(cn23xx->intr_sum_reg64)));
+
+	/*In cn23xx_setup_iq_regs*/
+	for (i = 0; i < CN23XX_MAX_INPUT_QUEUES; i++) {
+		dev_dbg(&oct->pci_dev->dev, "%s(%d)[%llx] : 0x%016llx\n",
+			"CN23XX_SLI_IQ_BASE_ADDR64", i,
+			CVM_CAST64(CN23XX_SLI_IQ_BASE_ADDR64(i)),
+			CVM_CAST64(octeon_read_csr64(
+				oct, CN23XX_SLI_IQ_BASE_ADDR64(i))));
+		dev_dbg(&oct->pci_dev->dev, "%s(%d)[%llx] : 0x%016llx\n",
+			"CN23XX_SLI_IQ_SIZE", i,
+			CVM_CAST64(CN23XX_SLI_IQ_SIZE(i)),
+			CVM_CAST64(octeon_read_csr
+				(oct, CN23XX_SLI_IQ_SIZE(i))));
+		dev_dbg(&oct->pci_dev->dev, "%s(%d)[%llx] : 0x%016llx\n",
+			"CN23XX_SLI_IQ_DOORBELL", i,
+			CVM_CAST64(CN23XX_SLI_IQ_DOORBELL(i)),
+			CVM_CAST64(octeon_read_csr64(
+				oct, CN23XX_SLI_IQ_DOORBELL(i))));
+		dev_dbg(&oct->pci_dev->dev, "%s(%d)[%llx] : 0x%016llx\n",
+			"CN23XX_SLI_IQ_INSTR_COUNT64", i,
+			CVM_CAST64(CN23XX_SLI_IQ_INSTR_COUNT64(i)),
+			CVM_CAST64(octeon_read_csr64(
+				oct, CN23XX_SLI_IQ_INSTR_COUNT64(i))));
+	}
+
+	/*In cn23xx_setup_oq_regs*/
+	for (i = 0; i < CN23XX_MAX_OUTPUT_QUEUES; i++) {
+		dev_dbg(&oct->pci_dev->dev, "%s(%d)[%llx] : 0x%016llx\n",
+			"CN23XX_SLI_OQ_BASE_ADDR64", i,
+			CVM_CAST64(CN23XX_SLI_OQ_BASE_ADDR64(i)),
+			CVM_CAST64(octeon_read_csr64(
+				oct, CN23XX_SLI_OQ_BASE_ADDR64(i))));
+		dev_dbg(&oct->pci_dev->dev, "%s(%d)[%llx] : 0x%016llx\n",
+			"CN23XX_SLI_OQ_SIZE", i,
+			CVM_CAST64(CN23XX_SLI_OQ_SIZE(i)),
+			CVM_CAST64(octeon_read_csr
+				(oct, CN23XX_SLI_OQ_SIZE(i))));
+		dev_dbg(&oct->pci_dev->dev, "%s(%d)[%llx] : 0x%016llx\n",
+			"CN23XX_SLI_OQ_BUFF_INFO_SIZE", i,
+			CVM_CAST64(CN23XX_SLI_OQ_BUFF_INFO_SIZE(i)),
+			CVM_CAST64(octeon_read_csr(
+				oct, CN23XX_SLI_OQ_BUFF_INFO_SIZE(i))));
+		dev_dbg(&oct->pci_dev->dev, "%s(%d)[%llx] : 0x%016llx\n",
+			"CN23XX_SLI_OQ_PKTS_SENT", i,
+			CVM_CAST64(CN23XX_SLI_OQ_PKTS_SENT(i)),
+			CVM_CAST64(octeon_read_csr64(
+				oct, CN23XX_SLI_OQ_PKTS_SENT(i))));
+		dev_dbg(&oct->pci_dev->dev, "%s(%d)[%llx] : 0x%016llx\n",
+			"CN23XX_SLI_OQ_PKTS_CREDIT", i,
+			CVM_CAST64(CN23XX_SLI_OQ_PKTS_CREDIT(i)),
+			CVM_CAST64(octeon_read_csr64(
+				oct, CN23XX_SLI_OQ_PKTS_CREDIT(i))));
+	}
+
+	dev_dbg(&oct->pci_dev->dev, "%s[%llx] : 0x%016llx\n",
+		"CN23XX_SLI_PKT_TIME_INT",
+		CVM_CAST64(CN23XX_SLI_PKT_TIME_INT),
+		CVM_CAST64(octeon_read_csr64(oct, CN23XX_SLI_PKT_TIME_INT)));
+	dev_dbg(&oct->pci_dev->dev, "%s[%llx] : 0x%016llx\n",
+		"CN23XX_SLI_PKT_CNT_INT",
+		CVM_CAST64(CN23XX_SLI_PKT_CNT_INT),
+		CVM_CAST64(octeon_read_csr64(oct, CN23XX_SLI_PKT_CNT_INT)));
+}
+
+static int cn23xx_pf_soft_reset(struct octeon_device *oct)
+{
+	octeon_write_csr64(oct, CN23XX_WIN_WR_MASK_REG, 0xFF);
+
+	dev_dbg(&oct->pci_dev->dev, "OCTEON[%d]: BIST enabled for CN23XX soft reset\n",
+		oct->octeon_id);
+
+	octeon_write_csr64(oct, CN23XX_SLI_SCRATCH1, 0x1234ULL);
+
+	/* Initiate chip-wide soft reset */
+	lio_pci_readq(oct, CN23XX_RST_SOFT_RST);
+	lio_pci_writeq(oct, 1, CN23XX_RST_SOFT_RST);
+
+	/* Wait for 100ms as Octeon resets. */
+	mdelay(100);
+
+	if (octeon_read_csr64(oct, CN23XX_SLI_SCRATCH1) == 0x1234ULL) {
+		dev_err(&oct->pci_dev->dev, "OCTEON[%d]: Soft reset failed\n",
+			oct->octeon_id);
+		return 1;
+	}
+
+	dev_dbg(&oct->pci_dev->dev, "OCTEON[%d]: Reset completed\n",
+		oct->octeon_id);
+
+	/* restore the  reset value*/
+	octeon_write_csr64(oct, CN23XX_WIN_WR_MASK_REG, 0xFF);
+
+	return 0;
+}
+
+static void cn23xx_enable_error_reporting(struct octeon_device *oct)
+{
+	u32 regval;
+	u32 uncorrectable_err_mask, corrtable_err_status;
+
+	pci_read_config_dword(oct->pci_dev, CN23XX_CONFIG_PCIE_DEVCTL, &regval);
+	if (regval & CN23XX_CONFIG_PCIE_DEVCTL_MASK) {
+		uncorrectable_err_mask = 0;
+		corrtable_err_status = 0;
+		pci_read_config_dword(oct->pci_dev,
+				      CN23XX_CONFIG_PCIE_UNCORRECT_ERR_MASK,
+				      &uncorrectable_err_mask);
+		pci_read_config_dword(oct->pci_dev,
+				      CN23XX_CONFIG_PCIE_CORRECT_ERR_STATUS,
+				      &corrtable_err_status);
+		dev_err(&oct->pci_dev->dev, "PCI-E Fatal error detected;\n"
+				 "\tdev_ctl_status_reg = 0x%08x\n"
+				 "\tuncorrectable_error_mask_reg = 0x%08x\n"
+				 "\tcorrectable_error_status_reg = 0x%08x\n",
+			    regval, uncorrectable_err_mask,
+			    corrtable_err_status);
+	}
+
+	regval |= 0xf; /* Enable Link error reporting */
+
+	dev_dbg(&oct->pci_dev->dev, "OCTEON[%d]: Enabling PCI-E error reporting..\n",
+		oct->octeon_id);
+	pci_write_config_dword(oct->pci_dev, CN23XX_CONFIG_PCIE_DEVCTL, regval);
+}
+
+static u32 cn23xx_coprocessor_clock(struct octeon_device *oct)
+{
+	/* Bits 29:24 of RST_BOOT[PNR_MUL] holds the ref.clock MULTIPLIER
+	 * for SLI.
+	 */
+
+	/* TBD: get the info in Hand-shake */
+	return (((lio_pci_readq(oct, CN23XX_RST_BOOT) >> 24) & 0x3f) * 50);
+}
+
+u32 cn23xx_pf_get_oq_ticks(struct octeon_device *oct, u32 time_intr_in_us)
+{
+	/* This gives the SLI clock per microsec */
+	u32 oqticks_per_us = cn23xx_coprocessor_clock(oct);
+
+	oct->pfvf_hsword.coproc_tics_per_us = oqticks_per_us;
+
+	/* 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;
+}
+
+static void cn23xx_setup_global_mac_regs(struct octeon_device *oct)
+{
+	u64 reg_val;
+	u16 mac_no = oct->pcie_port;
+	u16 pf_num = oct->pf_num;
+
+	/* programming SRN and TRS for each MAC(0..3)  */
+
+	dev_dbg(&oct->pci_dev->dev, "%s:Using pcie port %d\n",
+		__func__, mac_no);
+	/* By default, mapping all 64 IOQs to  a single MACs */
+
+	reg_val =
+	    octeon_read_csr64(oct, CN23XX_SLI_PKT_MAC_RINFO64(mac_no, pf_num));
+
+	if (oct->rev_id == OCTEON_CN23XX_REV_1_1) {
+		/* setting SRN <6:0>  */
+		reg_val = pf_num * CN23XX_MAX_RINGS_PER_PF_PASS_1_1;
+	} else {
+		/* setting SRN <6:0>  */
+		reg_val = pf_num * CN23XX_MAX_RINGS_PER_PF;
+	}
+
+	/* setting TRS <23:16> */
+	reg_val = reg_val |
+		  (oct->sriov_info.trs << CN23XX_PKT_MAC_CTL_RINFO_TRS_BIT_POS);
+	/* write these settings to MAC register */
+	octeon_write_csr64(oct, CN23XX_SLI_PKT_MAC_RINFO64(mac_no, pf_num),
+			   reg_val);
+
+	dev_dbg(&oct->pci_dev->dev, "SLI_PKT_MAC(%d)_PF(%d)_RINFO : 0x%016llx\n",
+		mac_no, pf_num, (u64)octeon_read_csr64
+		(oct, CN23XX_SLI_PKT_MAC_RINFO64(mac_no, pf_num)));
+}
+
+static int cn23xx_reset_io_queues(struct octeon_device *oct)
+{
+	int ret_val = 0;
+	u64 d64;
+	u32 q_no, srn, ern;
+	u32 loop = 1000;
+
+	srn = oct->sriov_info.pf_srn;
+	ern = srn + oct->sriov_info.num_pf_rings;
+
+	/*As per HRM reg description, s/w cant write 0 to ENB. */
+	/*to make the queue off, need to set the RST bit. */
+
+	/* Reset the Enable bit for all the 64 IQs.  */
+	for (q_no = srn; q_no < ern; q_no++) {
+		/* set RST bit to 1. This bit applies to both IQ and OQ */
+		d64 = octeon_read_csr64(oct, CN23XX_SLI_IQ_PKT_CONTROL64(q_no));
+		d64 = d64 | CN23XX_PKT_INPUT_CTL_RST;
+		octeon_write_csr64(oct, CN23XX_SLI_IQ_PKT_CONTROL64(q_no), d64);
+	}
+
+	/*wait until the RST bit is clear or the RST and quite bits are set*/
+	for (q_no = srn; q_no < ern; q_no++) {
+		u64 reg_val = octeon_read_csr64(oct,
+					CN23XX_SLI_IQ_PKT_CONTROL64(q_no));
+		while ((READ_ONCE(reg_val) & CN23XX_PKT_INPUT_CTL_RST) &&
+		       !(READ_ONCE(reg_val) & CN23XX_PKT_INPUT_CTL_QUIET) &&
+		       loop--) {
+			WRITE_ONCE(reg_val, octeon_read_csr64(
+			    oct, CN23XX_SLI_IQ_PKT_CONTROL64(q_no)));
+		}
+		if (!loop) {
+			dev_err(&oct->pci_dev->dev,
+				"clearing the reset reg failed or setting the quiet reg failed for qno: %u\n",
+				q_no);
+			return -1;
+		}
+		WRITE_ONCE(reg_val, READ_ONCE(reg_val) &
+			~CN23XX_PKT_INPUT_CTL_RST);
+		octeon_write_csr64(oct, CN23XX_SLI_IQ_PKT_CONTROL64(q_no),
+				   READ_ONCE(reg_val));
+
+		WRITE_ONCE(reg_val, octeon_read_csr64(
+			   oct, CN23XX_SLI_IQ_PKT_CONTROL64(q_no)));
+		if (READ_ONCE(reg_val) & CN23XX_PKT_INPUT_CTL_RST) {
+			dev_err(&oct->pci_dev->dev,
+				"clearing the reset failed for qno: %u\n",
+				q_no);
+			ret_val = -1;
+		}
+	}
+
+	return ret_val;
+}
+
+static int cn23xx_pf_setup_global_input_regs(struct octeon_device *oct)
+{
+	u32 q_no, ern, srn;
+	u64 pf_num;
+	u64 intr_threshold, reg_val;
+	struct octeon_instr_queue *iq;
+	struct octeon_cn23xx_pf *cn23xx = (struct octeon_cn23xx_pf *)oct->chip;
+
+	pf_num = oct->pf_num;
+
+	srn = oct->sriov_info.pf_srn;
+	ern = srn + oct->sriov_info.num_pf_rings;
+
+	if (cn23xx_reset_io_queues(oct))
+		return -1;
+
+	/** Set the MAC_NUM and PVF_NUM in IQ_PKT_CONTROL reg
+	* for all queues.Only PF can set these bits.
+	* bits 29:30 indicate the MAC num.
+	* bits 32:47 indicate the PVF num.
+	*/
+	for (q_no = 0; q_no < ern; q_no++) {
+		reg_val = oct->pcie_port << CN23XX_PKT_INPUT_CTL_MAC_NUM_POS;
+		reg_val |= pf_num << CN23XX_PKT_INPUT_CTL_PF_NUM_POS;
+
+		octeon_write_csr64(oct, CN23XX_SLI_IQ_PKT_CONTROL64(q_no),
+				   reg_val);
+	}
+
+	/* Select ES, RO, NS, RDSIZE,DPTR Fomat#0 for
+	 * pf queues
+	 */
+	for (q_no = srn; q_no < ern; q_no++) {
+		void __iomem *inst_cnt_reg;
+
+		iq = oct->instr_queue[q_no];
+		if (iq)
+			inst_cnt_reg = iq->inst_cnt_reg;
+		else
+			inst_cnt_reg = (u8 *)oct->mmio[0].hw_addr +
+				       CN23XX_SLI_IQ_INSTR_COUNT64(q_no);
+
+		reg_val =
+		    octeon_read_csr64(oct, CN23XX_SLI_IQ_PKT_CONTROL64(q_no));
+
+		reg_val |= CN23XX_PKT_INPUT_CTL_MASK;
+
+		octeon_write_csr64(oct, CN23XX_SLI_IQ_PKT_CONTROL64(q_no),
+				   reg_val);
+
+		/* Set WMARK level for triggering PI_INT */
+		/* intr_threshold = CN23XX_DEF_IQ_INTR_THRESHOLD & */
+		intr_threshold = CFG_GET_IQ_INTR_PKT(cn23xx->conf) &
+				 CN23XX_PKT_IN_DONE_WMARK_MASK;
+
+		writeq((readq(inst_cnt_reg) &
+			~(CN23XX_PKT_IN_DONE_WMARK_MASK <<
+			  CN23XX_PKT_IN_DONE_WMARK_BIT_POS)) |
+		       (intr_threshold << CN23XX_PKT_IN_DONE_WMARK_BIT_POS),
+		       inst_cnt_reg);
+	}
+	return 0;
+}
+
+static void cn23xx_pf_setup_global_output_regs(struct octeon_device *oct)
+{
+	u32 reg_val;
+	u32 q_no, ern, srn;
+	u64 time_threshold;
+
+	struct octeon_cn23xx_pf *cn23xx = (struct octeon_cn23xx_pf *)oct->chip;
+
+	srn = oct->sriov_info.pf_srn;
+	ern = srn + oct->sriov_info.num_pf_rings;
+
+	if (CFG_GET_IS_SLI_BP_ON(cn23xx->conf)) {
+		octeon_write_csr64(oct, CN23XX_SLI_OQ_WMARK, 32);
+	} else {
+		/** Set Output queue watermark to 0 to disable backpressure */
+		octeon_write_csr64(oct, CN23XX_SLI_OQ_WMARK, 0);
+	}
+
+	for (q_no = srn; q_no < ern; q_no++) {
+		reg_val = octeon_read_csr(oct, CN23XX_SLI_OQ_PKT_CONTROL(q_no));
+
+		/* set IPTR & DPTR */
+		reg_val |=
+		    (CN23XX_PKT_OUTPUT_CTL_IPTR | CN23XX_PKT_OUTPUT_CTL_DPTR);
+
+		/* reset BMODE */
+		reg_val &= ~(CN23XX_PKT_OUTPUT_CTL_BMODE);
+
+		/* No Relaxed Ordering, No Snoop, 64-bit Byte swap
+		 * for Output Queue ScatterList
+		 * reset ROR_P, NSR_P
+		 */
+		reg_val &= ~(CN23XX_PKT_OUTPUT_CTL_ROR_P);
+		reg_val &= ~(CN23XX_PKT_OUTPUT_CTL_NSR_P);
+
+#ifdef __LITTLE_ENDIAN_BITFIELD
+		reg_val &= ~(CN23XX_PKT_OUTPUT_CTL_ES_P);
+#else
+		reg_val |= (CN23XX_PKT_OUTPUT_CTL_ES_P);
+#endif
+		/* No Relaxed Ordering, No Snoop, 64-bit Byte swap
+		 * for Output Queue Data
+		 * reset ROR, NSR
+		 */
+		reg_val &= ~(CN23XX_PKT_OUTPUT_CTL_ROR);
+		reg_val &= ~(CN23XX_PKT_OUTPUT_CTL_NSR);
+		/* set the ES bit */
+		reg_val |= (CN23XX_PKT_OUTPUT_CTL_ES);
+
+		/* write all the selected settings */
+		octeon_write_csr(oct, CN23XX_SLI_OQ_PKT_CONTROL(q_no), reg_val);
+
+		/* Enabling these interrupt in oct->fn_list.enable_interrupt()
+		 * routine which called after IOQ init.
+		 * Set up interrupt packet and time thresholds
+		 * for all the OQs
+		 */
+		time_threshold = cn23xx_pf_get_oq_ticks(
+		    oct, (u32)CFG_GET_OQ_INTR_TIME(cn23xx->conf));
+
+		octeon_write_csr64(oct, CN23XX_SLI_OQ_PKT_INT_LEVELS(q_no),
+				   (CFG_GET_OQ_INTR_PKT(cn23xx->conf) |
+				    (time_threshold << 32)));
+	}
+
+	/** Setting the water mark level for pko back pressure **/
+	writeq(0x40, (u8 *)oct->mmio[0].hw_addr + CN23XX_SLI_OQ_WMARK);
+
+	/** Disabling setting OQs in reset when ring has no dorebells
+	  * enabling this will cause of head of line blocking
+	  */
+	/* Do it only for pass1.1. and pass1.2 */
+	if ((oct->rev_id == OCTEON_CN23XX_REV_1_0) ||
+	    (oct->rev_id == OCTEON_CN23XX_REV_1_1))
+		writeq(readq((u8 *)oct->mmio[0].hw_addr +
+				     CN23XX_SLI_GBL_CONTROL) | 0x2,
+		       (u8 *)oct->mmio[0].hw_addr + CN23XX_SLI_GBL_CONTROL);
+
+	/** Enable channel-level backpressure */
+	if (oct->pf_num)
+		writeq(0xffffffffffffffffULL,
+		       (u8 *)oct->mmio[0].hw_addr + CN23XX_SLI_OUT_BP_EN2_W1S);
+	else
+		writeq(0xffffffffffffffffULL,
+		       (u8 *)oct->mmio[0].hw_addr + CN23XX_SLI_OUT_BP_EN_W1S);
+}
+
+static int cn23xx_setup_pf_device_regs(struct octeon_device *oct)
+{
+	cn23xx_enable_error_reporting(oct);
+
+	/* program the MAC(0..3)_RINFO before setting up input/output regs */
+	cn23xx_setup_global_mac_regs(oct);
+
+	if (cn23xx_pf_setup_global_input_regs(oct))
+		return -1;
+
+	cn23xx_pf_setup_global_output_regs(oct);
+
+	/* Default error timeout value should be 0x200000 to avoid host hang
+	 * when reads invalid register
+	 */
+	octeon_write_csr64(oct, CN23XX_SLI_WINDOW_CTL,
+			   CN23XX_SLI_WINDOW_CTL_DEFAULT);
+
+	/* set SLI_PKT_IN_JABBER to handle large VXLAN packets */
+	octeon_write_csr64(oct, CN23XX_SLI_PKT_IN_JABBER, CN23XX_INPUT_JABBER);
+	return 0;
+}
+
+static void cn23xx_setup_iq_regs(struct octeon_device *oct, u32 iq_no)
+{
+	struct octeon_instr_queue *iq = oct->instr_queue[iq_no];
+	u64 pkt_in_done;
+
+	iq_no += oct->sriov_info.pf_srn;
+
+	/* Write the start of the input queue's ring and its size  */
+	octeon_write_csr64(oct, CN23XX_SLI_IQ_BASE_ADDR64(iq_no),
+			   iq->base_addr_dma);
+	octeon_write_csr(oct, CN23XX_SLI_IQ_SIZE(iq_no), iq->max_count);
+
+	/* Remember the doorbell & instruction count register addr
+	 * for this queue
+	 */
+	iq->doorbell_reg =
+	    (u8 *)oct->mmio[0].hw_addr + CN23XX_SLI_IQ_DOORBELL(iq_no);
+	iq->inst_cnt_reg =
+	    (u8 *)oct->mmio[0].hw_addr + CN23XX_SLI_IQ_INSTR_COUNT64(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)
+	 */
+	pkt_in_done = readq(iq->inst_cnt_reg);
+
+	if (oct->msix_on) {
+		/* Set CINT_ENB to enable IQ interrupt   */
+		writeq((pkt_in_done | CN23XX_INTR_CINT_ENB),
+		       iq->inst_cnt_reg);
+	} else {
+		/* Clear the count by writing back what we read, but don't
+		 * enable interrupts
+		 */
+		writeq(pkt_in_done, iq->inst_cnt_reg);
+	}
+
+	iq->reset_instr_cnt = 0;
+}
+
+static void cn23xx_setup_oq_regs(struct octeon_device *oct, u32 oq_no)
+{
+	u32 reg_val;
+	struct octeon_droq *droq = oct->droq[oq_no];
+	struct octeon_cn23xx_pf *cn23xx = (struct octeon_cn23xx_pf *)oct->chip;
+	u64 time_threshold;
+	u64 cnt_threshold;
+
+	oq_no += oct->sriov_info.pf_srn;
+
+	octeon_write_csr64(oct, CN23XX_SLI_OQ_BASE_ADDR64(oq_no),
+			   droq->desc_ring_dma);
+	octeon_write_csr(oct, CN23XX_SLI_OQ_SIZE(oq_no), droq->max_count);
+
+	octeon_write_csr(oct, CN23XX_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 =
+	    (u8 *)oct->mmio[0].hw_addr + CN23XX_SLI_OQ_PKTS_SENT(oq_no);
+	droq->pkts_credit_reg =
+	    (u8 *)oct->mmio[0].hw_addr + CN23XX_SLI_OQ_PKTS_CREDIT(oq_no);
+
+	if (!oct->msix_on) {
+		/* Enable this output queue to generate Packet Timer Interrupt
+		 */
+		reg_val =
+		    octeon_read_csr(oct, CN23XX_SLI_OQ_PKT_CONTROL(oq_no));
+		reg_val |= CN23XX_PKT_OUTPUT_CTL_TENB;
+		octeon_write_csr(oct, CN23XX_SLI_OQ_PKT_CONTROL(oq_no),
+				 reg_val);
+
+		/* Enable this output queue to generate Packet Count Interrupt
+		 */
+		reg_val =
+		    octeon_read_csr(oct, CN23XX_SLI_OQ_PKT_CONTROL(oq_no));
+		reg_val |= CN23XX_PKT_OUTPUT_CTL_CENB;
+		octeon_write_csr(oct, CN23XX_SLI_OQ_PKT_CONTROL(oq_no),
+				 reg_val);
+	} else {
+		time_threshold = cn23xx_pf_get_oq_ticks(
+		    oct, (u32)CFG_GET_OQ_INTR_TIME(cn23xx->conf));
+		cnt_threshold = (u32)CFG_GET_OQ_INTR_PKT(cn23xx->conf);
+
+		octeon_write_csr64(
+		    oct, CN23XX_SLI_OQ_PKT_INT_LEVELS(oq_no),
+		    ((time_threshold << 32 | cnt_threshold)));
+	}
+}
+
+static int cn23xx_enable_io_queues(struct octeon_device *oct)
+{
+	u64 reg_val;
+	u32 srn, ern, q_no;
+	u32 loop = 1000;
+
+	srn = oct->sriov_info.pf_srn;
+	ern = srn + oct->num_iqs;
+
+	for (q_no = srn; q_no < ern; q_no++) {
+		/* set the corresponding IQ IS_64B bit */
+		if (oct->io_qmask.iq64B & BIT_ULL(q_no - srn)) {
+			reg_val = octeon_read_csr64(
+			    oct, CN23XX_SLI_IQ_PKT_CONTROL64(q_no));
+			reg_val = reg_val | CN23XX_PKT_INPUT_CTL_IS_64B;
+			octeon_write_csr64(
+			    oct, CN23XX_SLI_IQ_PKT_CONTROL64(q_no), reg_val);
+		}
+
+		/* set the corresponding IQ ENB bit */
+		if (oct->io_qmask.iq & BIT_ULL(q_no - srn)) {
+			/* IOQs are in reset by default in PEM2 mode,
+			 * clearing reset bit
+			 */
+			reg_val = octeon_read_csr64(
+			    oct, CN23XX_SLI_IQ_PKT_CONTROL64(q_no));
+
+			if (reg_val & CN23XX_PKT_INPUT_CTL_RST) {
+				while ((reg_val & CN23XX_PKT_INPUT_CTL_RST) &&
+				       !(reg_val &
+					 CN23XX_PKT_INPUT_CTL_QUIET) &&
+				       --loop) {
+					reg_val = octeon_read_csr64(
+					    oct,
+					    CN23XX_SLI_IQ_PKT_CONTROL64(q_no));
+				}
+				if (!loop) {
+					dev_err(&oct->pci_dev->dev,
+						"clearing the reset reg failed or setting the quiet reg failed for qno: %u\n",
+						q_no);
+					return -1;
+				}
+				reg_val = reg_val & ~CN23XX_PKT_INPUT_CTL_RST;
+				octeon_write_csr64(
+				    oct, CN23XX_SLI_IQ_PKT_CONTROL64(q_no),
+				    reg_val);
+
+				reg_val = octeon_read_csr64(
+				    oct, CN23XX_SLI_IQ_PKT_CONTROL64(q_no));
+				if (reg_val & CN23XX_PKT_INPUT_CTL_RST) {
+					dev_err(&oct->pci_dev->dev,
+						"clearing the reset failed for qno: %u\n",
+						q_no);
+					return -1;
+				}
+			}
+			reg_val = octeon_read_csr64(
+			    oct, CN23XX_SLI_IQ_PKT_CONTROL64(q_no));
+			reg_val = reg_val | CN23XX_PKT_INPUT_CTL_RING_ENB;
+			octeon_write_csr64(
+			    oct, CN23XX_SLI_IQ_PKT_CONTROL64(q_no), reg_val);
+		}
+	}
+	for (q_no = srn; q_no < ern; q_no++) {
+		u32 reg_val;
+		/* set the corresponding OQ ENB bit */
+		if (oct->io_qmask.oq & BIT_ULL(q_no - srn)) {
+			reg_val = octeon_read_csr(
+			    oct, CN23XX_SLI_OQ_PKT_CONTROL(q_no));
+			reg_val = reg_val | CN23XX_PKT_OUTPUT_CTL_RING_ENB;
+			octeon_write_csr(oct, CN23XX_SLI_OQ_PKT_CONTROL(q_no),
+					 reg_val);
+		}
+	}
+	return 0;
+}
+
+static void cn23xx_disable_io_queues(struct octeon_device *oct)
+{
+	int q_no, loop;
+	u64 d64;
+	u32 d32;
+	u32 srn, ern;
+
+	srn = oct->sriov_info.pf_srn;
+	ern = srn + oct->num_iqs;
+
+	/*** Disable Input Queues. ***/
+	for (q_no = srn; q_no < ern; q_no++) {
+		loop = HZ;
+
+		/* start the Reset for a particular ring */
+		WRITE_ONCE(d64, octeon_read_csr64(
+			   oct, CN23XX_SLI_IQ_PKT_CONTROL64(q_no)));
+		WRITE_ONCE(d64, READ_ONCE(d64) &
+					(~(CN23XX_PKT_INPUT_CTL_RING_ENB)));
+		WRITE_ONCE(d64, READ_ONCE(d64) | CN23XX_PKT_INPUT_CTL_RST);
+		octeon_write_csr64(oct, CN23XX_SLI_IQ_PKT_CONTROL64(q_no),
+				   READ_ONCE(d64));
+
+		/* Wait until hardware indicates that the particular IQ
+		 * is out of reset.
+		 */
+		WRITE_ONCE(d64, octeon_read_csr64(
+					oct, CN23XX_SLI_PKT_IOQ_RING_RST));
+		while (!(READ_ONCE(d64) & BIT_ULL(q_no)) && loop--) {
+			WRITE_ONCE(d64, octeon_read_csr64(
+					oct, CN23XX_SLI_PKT_IOQ_RING_RST));
+			schedule_timeout_uninterruptible(1);
+		}
+
+		/* Reset the doorbell register for this Input Queue. */
+		octeon_write_csr(oct, CN23XX_SLI_IQ_DOORBELL(q_no), 0xFFFFFFFF);
+		while (octeon_read_csr64(oct, CN23XX_SLI_IQ_DOORBELL(q_no)) &&
+		       loop--) {
+			schedule_timeout_uninterruptible(1);
+		}
+	}
+
+	/*** Disable Output Queues. ***/
+	for (q_no = srn; q_no < ern; q_no++) {
+		loop = HZ;
+
+		/* Wait until hardware indicates that the particular IQ
+		 * is out of reset.It given that SLI_PKT_RING_RST is
+		 * common for both IQs and OQs
+		 */
+		WRITE_ONCE(d64, octeon_read_csr64(
+					oct, CN23XX_SLI_PKT_IOQ_RING_RST));
+		while (!(READ_ONCE(d64) & BIT_ULL(q_no)) && loop--) {
+			WRITE_ONCE(d64, octeon_read_csr64(
+					oct, CN23XX_SLI_PKT_IOQ_RING_RST));
+			schedule_timeout_uninterruptible(1);
+		}
+
+		/* Reset the doorbell register for this Output Queue. */
+		octeon_write_csr(oct, CN23XX_SLI_OQ_PKTS_CREDIT(q_no),
+				 0xFFFFFFFF);
+		while (octeon_read_csr64(oct,
+					 CN23XX_SLI_OQ_PKTS_CREDIT(q_no)) &&
+		       loop--) {
+			schedule_timeout_uninterruptible(1);
+		}
+
+		/* clear the SLI_PKT(0..63)_CNTS[CNT] reg value */
+		WRITE_ONCE(d32, octeon_read_csr(
+					oct, CN23XX_SLI_OQ_PKTS_SENT(q_no)));
+		octeon_write_csr(oct, CN23XX_SLI_OQ_PKTS_SENT(q_no),
+				 READ_ONCE(d32));
+	}
+}
+
+static u64 cn23xx_pf_msix_interrupt_handler(void *dev)
+{
+	struct octeon_ioq_vector *ioq_vector = (struct octeon_ioq_vector *)dev;
+	struct octeon_device *oct = ioq_vector->oct_dev;
+	u64 pkts_sent;
+	u64 ret = 0;
+	struct octeon_droq *droq = oct->droq[ioq_vector->droq_index];
+
+	dev_dbg(&oct->pci_dev->dev, "In %s octeon_dev @ %p\n", __func__, oct);
+
+	if (!droq) {
+		dev_err(&oct->pci_dev->dev, "23XX bringup FIXME: oct pfnum:%d ioq_vector->ioq_num :%d droq is NULL\n",
+			oct->pf_num, ioq_vector->ioq_num);
+		return 0;
+	}
+
+	pkts_sent = readq(droq->pkts_sent_reg);
+
+	/* 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 (!pkts_sent || (pkts_sent == 0xFFFFFFFFFFFFFFFFULL))
+		return ret;
+
+	/* Write count reg in sli_pkt_cnts to clear these int.*/
+	if ((pkts_sent & CN23XX_INTR_PO_INT) ||
+	    (pkts_sent & CN23XX_INTR_PI_INT)) {
+		if (pkts_sent & CN23XX_INTR_PO_INT)
+			ret |= MSIX_PO_INT;
+	}
+
+	if (pkts_sent & CN23XX_INTR_PI_INT)
+		/* We will clear the count when we update the read_index. */
+		ret |= MSIX_PI_INT;
+
+	/* Never need to handle msix mbox intr for pf. They arrive on the last
+	 * msix
+	 */
+	return ret;
+}
+
+static irqreturn_t cn23xx_interrupt_handler(void *dev)
+{
+	struct octeon_device *oct = (struct octeon_device *)dev;
+	struct octeon_cn23xx_pf *cn23xx = (struct octeon_cn23xx_pf *)oct->chip;
+	u64 intr64;
+
+	dev_dbg(&oct->pci_dev->dev, "In %s octeon_dev @ %p\n", __func__, oct);
+	intr64 = readq(cn23xx->intr_sum_reg64);
+
+	oct->int_status = 0;
+
+	if (intr64 & CN23XX_INTR_ERR)
+		dev_err(&oct->pci_dev->dev, "OCTEON[%d]: Error Intr: 0x%016llx\n",
+			oct->octeon_id, CVM_CAST64(intr64));
+
+	if (oct->msix_on != LIO_FLAG_MSIX_ENABLED) {
+		if (intr64 & CN23XX_INTR_PKT_DATA)
+			oct->int_status |= OCT_DEV_INTR_PKT_DATA;
+	}
+
+	if (intr64 & (CN23XX_INTR_DMA0_FORCE))
+		oct->int_status |= OCT_DEV_INTR_DMA0_FORCE;
+	if (intr64 & (CN23XX_INTR_DMA1_FORCE))
+		oct->int_status |= OCT_DEV_INTR_DMA1_FORCE;
+
+	/* Clear the current interrupts */
+	writeq(intr64, cn23xx->intr_sum_reg64);
+
+	return IRQ_HANDLED;
+}
+
+static void cn23xx_bar1_idx_setup(struct octeon_device *oct, u64 core_addr,
+				  u32 idx, int valid)
+{
+	u64 bar1;
+	u64 reg_adr;
+
+	if (!valid) {
+		reg_adr = lio_pci_readq(
+			oct, CN23XX_PEM_BAR1_INDEX_REG(oct->pcie_port, idx));
+		WRITE_ONCE(bar1, reg_adr);
+		lio_pci_writeq(oct, (READ_ONCE(bar1) & 0xFFFFFFFEULL),
+			       CN23XX_PEM_BAR1_INDEX_REG(oct->pcie_port, idx));
+		reg_adr = lio_pci_readq(
+			oct, CN23XX_PEM_BAR1_INDEX_REG(oct->pcie_port, idx));
+		WRITE_ONCE(bar1, reg_adr);
+		return;
+	}
+
+	/*  The PEM(0..3)_BAR1_INDEX(0..15)[ADDR_IDX]<23:4> stores
+	 *  bits <41:22> of the Core Addr
+	 */
+	lio_pci_writeq(oct, (((core_addr >> 22) << 4) | PCI_BAR1_MASK),
+		       CN23XX_PEM_BAR1_INDEX_REG(oct->pcie_port, idx));
+
+	WRITE_ONCE(bar1, lio_pci_readq(
+		   oct, CN23XX_PEM_BAR1_INDEX_REG(oct->pcie_port, idx)));
+}
+
+static void cn23xx_bar1_idx_write(struct octeon_device *oct, u32 idx, u32 mask)
+{
+	lio_pci_writeq(oct, mask,
+		       CN23XX_PEM_BAR1_INDEX_REG(oct->pcie_port, idx));
+}
+
+static u32 cn23xx_bar1_idx_read(struct octeon_device *oct, u32 idx)
+{
+	return (u32)lio_pci_readq(
+	    oct, CN23XX_PEM_BAR1_INDEX_REG(oct->pcie_port, idx));
+}
+
+/* always call with lock held */
+static u32 cn23xx_update_read_index(struct octeon_instr_queue *iq)
+{
+	u32 new_idx;
+	u32 last_done;
+	u32 pkt_in_done = readl(iq->inst_cnt_reg);
+
+	last_done = pkt_in_done - iq->pkt_in_done;
+	iq->pkt_in_done = pkt_in_done;
+
+	/* Modulo of the new index with the IQ size will give us
+	 * the new index.  The iq->reset_instr_cnt is always zero for
+	 * cn23xx, so no extra adjustments are needed.
+	 */
+	new_idx = (iq->octeon_read_index +
+		   (u32)(last_done & CN23XX_PKT_IN_DONE_CNT_MASK)) %
+		  iq->max_count;
+
+	return new_idx;
+}
+
+static void cn23xx_enable_pf_interrupt(struct octeon_device *oct, u8 intr_flag)
+{
+	struct octeon_cn23xx_pf *cn23xx = (struct octeon_cn23xx_pf *)oct->chip;
+	u64 intr_val = 0;
+
+	/*  Divide the single write to multiple writes based on the flag. */
+	/* Enable Interrupt */
+	if (intr_flag == OCTEON_ALL_INTR) {
+		writeq(cn23xx->intr_mask64, cn23xx->intr_enb_reg64);
+	} else if (intr_flag & OCTEON_OUTPUT_INTR) {
+		intr_val = readq(cn23xx->intr_enb_reg64);
+		intr_val |= CN23XX_INTR_PKT_DATA;
+		writeq(intr_val, cn23xx->intr_enb_reg64);
+	}
+}
+
+static void cn23xx_disable_pf_interrupt(struct octeon_device *oct, u8 intr_flag)
+{
+	struct octeon_cn23xx_pf *cn23xx = (struct octeon_cn23xx_pf *)oct->chip;
+	u64 intr_val = 0;
+
+	/* Disable Interrupts */
+	if (intr_flag == OCTEON_ALL_INTR) {
+		writeq(0, cn23xx->intr_enb_reg64);
+	} else if (intr_flag & OCTEON_OUTPUT_INTR) {
+		intr_val = readq(cn23xx->intr_enb_reg64);
+		intr_val &= ~CN23XX_INTR_PKT_DATA;
+		writeq(intr_val, cn23xx->intr_enb_reg64);
+	}
+}
+
+static void cn23xx_get_pcie_qlmport(struct octeon_device *oct)
+{
+	oct->pcie_port = (octeon_read_csr(oct, CN23XX_SLI_MAC_NUMBER)) & 0xff;
+
+	dev_dbg(&oct->pci_dev->dev, "OCTEON: CN23xx uses PCIE Port %d\n",
+		oct->pcie_port);
+}
+
+static void cn23xx_get_pf_num(struct octeon_device *oct)
+{
+	u32 fdl_bit = 0;
+
+	/** Read Function Dependency Link reg to get the function number */
+	pci_read_config_dword(oct->pci_dev, CN23XX_PCIE_SRIOV_FDL, &fdl_bit);
+	oct->pf_num = ((fdl_bit >> CN23XX_PCIE_SRIOV_FDL_BIT_POS) &
+		       CN23XX_PCIE_SRIOV_FDL_MASK);
+}
+
+static void cn23xx_setup_reg_address(struct octeon_device *oct)
+{
+	u8 __iomem *bar0_pciaddr = oct->mmio[0].hw_addr;
+	struct octeon_cn23xx_pf *cn23xx = (struct octeon_cn23xx_pf *)oct->chip;
+
+	oct->reg_list.pci_win_wr_addr_hi =
+	    (u32 __iomem *)(bar0_pciaddr + CN23XX_WIN_WR_ADDR_HI);
+	oct->reg_list.pci_win_wr_addr_lo =
+	    (u32 __iomem *)(bar0_pciaddr + CN23XX_WIN_WR_ADDR_LO);
+	oct->reg_list.pci_win_wr_addr =
+	    (u64 __iomem *)(bar0_pciaddr + CN23XX_WIN_WR_ADDR64);
+
+	oct->reg_list.pci_win_rd_addr_hi =
+	    (u32 __iomem *)(bar0_pciaddr + CN23XX_WIN_RD_ADDR_HI);
+	oct->reg_list.pci_win_rd_addr_lo =
+	    (u32 __iomem *)(bar0_pciaddr + CN23XX_WIN_RD_ADDR_LO);
+	oct->reg_list.pci_win_rd_addr =
+	    (u64 __iomem *)(bar0_pciaddr + CN23XX_WIN_RD_ADDR64);
+
+	oct->reg_list.pci_win_wr_data_hi =
+	    (u32 __iomem *)(bar0_pciaddr + CN23XX_WIN_WR_DATA_HI);
+	oct->reg_list.pci_win_wr_data_lo =
+	    (u32 __iomem *)(bar0_pciaddr + CN23XX_WIN_WR_DATA_LO);
+	oct->reg_list.pci_win_wr_data =
+	    (u64 __iomem *)(bar0_pciaddr + CN23XX_WIN_WR_DATA64);
+
+	oct->reg_list.pci_win_rd_data_hi =
+	    (u32 __iomem *)(bar0_pciaddr + CN23XX_WIN_RD_DATA_HI);
+	oct->reg_list.pci_win_rd_data_lo =
+	    (u32 __iomem *)(bar0_pciaddr + CN23XX_WIN_RD_DATA_LO);
+	oct->reg_list.pci_win_rd_data =
+	    (u64 __iomem *)(bar0_pciaddr + CN23XX_WIN_RD_DATA64);
+
+	cn23xx_get_pcie_qlmport(oct);
+
+	cn23xx->intr_mask64 = CN23XX_INTR_MASK;
+	if (!oct->msix_on)
+		cn23xx->intr_mask64 |= CN23XX_INTR_PKT_TIME;
+	if (oct->rev_id >= OCTEON_CN23XX_REV_1_1)
+		cn23xx->intr_mask64 |= CN23XX_INTR_VF_MBOX;
+
+	cn23xx->intr_sum_reg64 =
+	    bar0_pciaddr +
+	    CN23XX_SLI_MAC_PF_INT_SUM64(oct->pcie_port, oct->pf_num);
+	cn23xx->intr_enb_reg64 =
+	    bar0_pciaddr +
+	    CN23XX_SLI_MAC_PF_INT_ENB64(oct->pcie_port, oct->pf_num);
+}
+
+static int cn23xx_sriov_config(struct octeon_device *oct)
+{
+	u32 total_rings;
+	struct octeon_cn23xx_pf *cn23xx = (struct octeon_cn23xx_pf *)oct->chip;
+	/* num_vfs is already filled for us */
+	u32 pf_srn, num_pf_rings;
+
+	cn23xx->conf =
+	    (struct octeon_config *)oct_get_config_info(oct, LIO_23XX);
+	switch (oct->rev_id) {
+	case OCTEON_CN23XX_REV_1_0:
+		total_rings = CN23XX_MAX_RINGS_PER_PF_PASS_1_0;
+		break;
+	case OCTEON_CN23XX_REV_1_1:
+		total_rings = CN23XX_MAX_RINGS_PER_PF_PASS_1_1;
+		break;
+	default:
+		total_rings = CN23XX_MAX_RINGS_PER_PF;
+		break;
+	}
+	if (!oct->sriov_info.num_pf_rings) {
+		if (total_rings > num_present_cpus())
+			num_pf_rings = num_present_cpus();
+		else
+			num_pf_rings = total_rings;
+	} else {
+		num_pf_rings = oct->sriov_info.num_pf_rings;
+
+		if (num_pf_rings > total_rings) {
+			dev_warn(&oct->pci_dev->dev,
+				 "num_queues_per_pf requested %u is more than available rings. Reducing to %u\n",
+				 num_pf_rings, total_rings);
+			num_pf_rings = total_rings;
+		}
+	}
+
+	total_rings = num_pf_rings;
+	/* the first ring of the pf */
+	pf_srn = total_rings - num_pf_rings;
+
+	oct->sriov_info.trs = total_rings;
+	oct->sriov_info.pf_srn = pf_srn;
+	oct->sriov_info.num_pf_rings = num_pf_rings;
+	dev_dbg(&oct->pci_dev->dev, "trs:%d pf_srn:%d num_pf_rings:%d\n",
+		oct->sriov_info.trs, oct->sriov_info.pf_srn,
+		oct->sriov_info.num_pf_rings);
+	return 0;
+}
+
+int setup_cn23xx_octeon_pf_device(struct octeon_device *oct)
+{
+	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 CN23XX BAR1 map failed\n",
+			__func__);
+		octeon_unmap_pci_barx(oct, 0);
+		return 1;
+	}
+
+	cn23xx_get_pf_num(oct);
+
+	if (cn23xx_sriov_config(oct)) {
+		octeon_unmap_pci_barx(oct, 0);
+		octeon_unmap_pci_barx(oct, 1);
+		return 1;
+	}
+
+	octeon_write_csr64(oct, CN23XX_SLI_MAC_CREDIT_CNT, 0x3F802080802080ULL);
+
+	oct->fn_list.setup_iq_regs = cn23xx_setup_iq_regs;
+	oct->fn_list.setup_oq_regs = cn23xx_setup_oq_regs;
+	oct->fn_list.process_interrupt_regs = cn23xx_interrupt_handler;
+	oct->fn_list.msix_interrupt_handler = cn23xx_pf_msix_interrupt_handler;
+
+	oct->fn_list.soft_reset = cn23xx_pf_soft_reset;
+	oct->fn_list.setup_device_regs = cn23xx_setup_pf_device_regs;
+	oct->fn_list.update_iq_read_idx = cn23xx_update_read_index;
+
+	oct->fn_list.bar1_idx_setup = cn23xx_bar1_idx_setup;
+	oct->fn_list.bar1_idx_write = cn23xx_bar1_idx_write;
+	oct->fn_list.bar1_idx_read = cn23xx_bar1_idx_read;
+
+	oct->fn_list.enable_interrupt = cn23xx_enable_pf_interrupt;
+	oct->fn_list.disable_interrupt = cn23xx_disable_pf_interrupt;
+
+	oct->fn_list.enable_io_queues = cn23xx_enable_io_queues;
+	oct->fn_list.disable_io_queues = cn23xx_disable_io_queues;
+
+	cn23xx_setup_reg_address(oct);
+
+	oct->coproc_clock_rate = 1000000ULL * cn23xx_coprocessor_clock(oct);
+
+	return 0;
+}
+
+int validate_cn23xx_pf_config_info(struct octeon_device *oct,
+				   struct octeon_config *conf23xx)
+{
+	if (CFG_GET_IQ_MAX_Q(conf23xx) > CN23XX_MAX_INPUT_QUEUES) {
+		dev_err(&oct->pci_dev->dev, "%s: Num IQ (%d) exceeds Max (%d)\n",
+			__func__, CFG_GET_IQ_MAX_Q(conf23xx),
+			CN23XX_MAX_INPUT_QUEUES);
+		return 1;
+	}
+
+	if (CFG_GET_OQ_MAX_Q(conf23xx) > CN23XX_MAX_OUTPUT_QUEUES) {
+		dev_err(&oct->pci_dev->dev, "%s: Num OQ (%d) exceeds Max (%d)\n",
+			__func__, CFG_GET_OQ_MAX_Q(conf23xx),
+			CN23XX_MAX_OUTPUT_QUEUES);
+		return 1;
+	}
+
+	if (CFG_GET_IQ_INSTR_TYPE(conf23xx) != OCTEON_32BYTE_INSTR &&
+	    CFG_GET_IQ_INSTR_TYPE(conf23xx) != 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(conf23xx)) ||
+	    !(CFG_GET_OQ_REFILL_THRESHOLD(conf23xx))) {
+		dev_err(&oct->pci_dev->dev, "%s: Invalid parameter for OQ\n",
+			__func__);
+		return 1;
+	}
+
+	if (!(CFG_GET_OQ_INTR_TIME(conf23xx))) {
+		dev_err(&oct->pci_dev->dev, "%s: Invalid parameter for OQ\n",
+			__func__);
+		return 1;
+	}
+
+	return 0;
+}
+
+void cn23xx_dump_iq_regs(struct octeon_device *oct)
+{
+	u32 regval, q_no;
+
+	dev_dbg(&oct->pci_dev->dev, "SLI_IQ_DOORBELL_0 [0x%x]: 0x%016llx\n",
+		CN23XX_SLI_IQ_DOORBELL(0),
+		CVM_CAST64(octeon_read_csr64
+			(oct, CN23XX_SLI_IQ_DOORBELL(0))));
+
+	dev_dbg(&oct->pci_dev->dev, "SLI_IQ_BASEADDR_0 [0x%x]: 0x%016llx\n",
+		CN23XX_SLI_IQ_BASE_ADDR64(0),
+		CVM_CAST64(octeon_read_csr64
+			(oct, CN23XX_SLI_IQ_BASE_ADDR64(0))));
+
+	dev_dbg(&oct->pci_dev->dev, "SLI_IQ_FIFO_RSIZE_0 [0x%x]: 0x%016llx\n",
+		CN23XX_SLI_IQ_SIZE(0),
+		CVM_CAST64(octeon_read_csr64(oct, CN23XX_SLI_IQ_SIZE(0))));
+
+	dev_dbg(&oct->pci_dev->dev, "SLI_CTL_STATUS [0x%x]: 0x%016llx\n",
+		CN23XX_SLI_CTL_STATUS,
+		CVM_CAST64(octeon_read_csr64(oct, CN23XX_SLI_CTL_STATUS)));
+
+	for (q_no = 0; q_no < CN23XX_MAX_INPUT_QUEUES; q_no++) {
+		dev_dbg(&oct->pci_dev->dev, "SLI_PKT[%d]_INPUT_CTL [0x%x]: 0x%016llx\n",
+			q_no, CN23XX_SLI_IQ_PKT_CONTROL64(q_no),
+			CVM_CAST64(octeon_read_csr64
+				(oct,
+					CN23XX_SLI_IQ_PKT_CONTROL64(q_no))));
+	}
+
+	pci_read_config_dword(oct->pci_dev, CN23XX_CONFIG_PCIE_DEVCTL, &regval);
+	dev_dbg(&oct->pci_dev->dev, "Config DevCtl [0x%x]: 0x%08x\n",
+		CN23XX_CONFIG_PCIE_DEVCTL, regval);
+
+	dev_dbg(&oct->pci_dev->dev, "SLI_PRT[%d]_CFG [0x%llx]: 0x%016llx\n",
+		oct->pcie_port, CN23XX_DPI_SLI_PRTX_CFG(oct->pcie_port),
+		CVM_CAST64(lio_pci_readq(
+			oct, CN23XX_DPI_SLI_PRTX_CFG(oct->pcie_port))));
+
+	dev_dbg(&oct->pci_dev->dev, "SLI_S2M_PORT[%d]_CTL [0x%x]: 0x%016llx\n",
+		oct->pcie_port, CN23XX_SLI_S2M_PORTX_CTL(oct->pcie_port),
+		CVM_CAST64(octeon_read_csr64(
+			oct, CN23XX_SLI_S2M_PORTX_CTL(oct->pcie_port))));
+}
+
+int cn23xx_fw_loaded(struct octeon_device *oct)
+{
+	u64 val;
+
+	val = octeon_read_csr64(oct, CN23XX_SLI_SCRATCH1);
+	return (val >> 1) & 1ULL;
+}
diff --git a/drivers/net/ethernet/cavium/liquidio/cn23xx_pf_device.h b/drivers/net/ethernet/cavium/liquidio/cn23xx_pf_device.h
new file mode 100644
index 000000000000..21b5c9051967
--- /dev/null
+++ b/drivers/net/ethernet/cavium/liquidio/cn23xx_pf_device.h
@@ -0,0 +1,59 @@
+/**********************************************************************
+* 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  cn23xx_device.h
+ * \brief Host Driver: Routines that perform CN23XX specific operations.
+*/
+
+#ifndef __CN23XX_PF_DEVICE_H__
+#define __CN23XX_PF_DEVICE_H__
+
+#include "cn23xx_pf_regs.h"
+
+/* Register address and configuration for a CN23XX 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_cn23xx_pf {
+	/** 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;
+};
+
+int setup_cn23xx_octeon_pf_device(struct octeon_device *oct);
+
+int validate_cn23xx_pf_config_info(struct octeon_device *oct,
+				   struct octeon_config *conf23xx);
+
+u32 cn23xx_pf_get_oq_ticks(struct octeon_device *oct, u32 time_intr_in_us);
+
+void cn23xx_dump_pf_initialized_regs(struct octeon_device *oct);
+
+int cn23xx_fw_loaded(struct octeon_device *oct);
+#endif
diff --git a/drivers/net/ethernet/cavium/liquidio/cn23xx_pf_regs.h b/drivers/net/ethernet/cavium/liquidio/cn23xx_pf_regs.h
new file mode 100644
index 000000000000..03d79d95ab75
--- /dev/null
+++ b/drivers/net/ethernet/cavium/liquidio/cn23xx_pf_regs.h
@@ -0,0 +1,604 @@
+/**********************************************************************
+* 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 cn23xx_regs.h
+ * \brief Host Driver: Register Address and Register Mask values for
+ * Octeon CN23XX devices.
+*/
+
+#ifndef __CN23XX_PF_REGS_H__
+#define __CN23XX_PF_REGS_H__
+
+#define     CN23XX_CONFIG_VENDOR_ID	0x00
+#define     CN23XX_CONFIG_DEVICE_ID	0x02
+
+#define     CN23XX_CONFIG_XPANSION_BAR             0x38
+
+#define     CN23XX_CONFIG_MSIX_CAP		   0x50
+#define     CN23XX_CONFIG_MSIX_LMSI		   0x54
+#define     CN23XX_CONFIG_MSIX_UMSI		   0x58
+#define     CN23XX_CONFIG_MSIX_MSIMD		   0x5C
+#define     CN23XX_CONFIG_MSIX_MSIMM		   0x60
+#define     CN23XX_CONFIG_MSIX_MSIMP		   0x64
+
+#define     CN23XX_CONFIG_PCIE_CAP                 0x70
+#define     CN23XX_CONFIG_PCIE_DEVCAP              0x74
+#define     CN23XX_CONFIG_PCIE_DEVCTL              0x78
+#define     CN23XX_CONFIG_PCIE_LINKCAP             0x7C
+#define     CN23XX_CONFIG_PCIE_LINKCTL             0x80
+#define     CN23XX_CONFIG_PCIE_SLOTCAP             0x84
+#define     CN23XX_CONFIG_PCIE_SLOTCTL             0x88
+#define     CN23XX_CONFIG_PCIE_DEVCTL2             0x98
+#define     CN23XX_CONFIG_PCIE_LINKCTL2            0xA0
+#define     CN23XX_CONFIG_PCIE_UNCORRECT_ERR_MASK  0x108
+#define     CN23XX_CONFIG_PCIE_CORRECT_ERR_STATUS  0x110
+#define     CN23XX_CONFIG_PCIE_DEVCTL_MASK         0x00040000
+
+#define     CN23XX_PCIE_SRIOV_FDL		   0x188
+#define     CN23XX_PCIE_SRIOV_FDL_BIT_POS	   0x10
+#define     CN23XX_PCIE_SRIOV_FDL_MASK		   0xFF
+
+#define     CN23XX_CONFIG_PCIE_FLTMSK              0x720
+
+#define     CN23XX_CONFIG_SRIOV_VFDEVID            0x190
+
+#define     CN23XX_CONFIG_SRIOV_BAR_START	   0x19C
+#define     CN23XX_CONFIG_SRIOV_BARX(i)		\
+		(CN23XX_CONFIG_SRIOV_BAR_START + (i * 4))
+#define     CN23XX_CONFIG_SRIOV_BAR_PF		   0x08
+#define     CN23XX_CONFIG_SRIOV_BAR_64BIT	   0x04
+#define     CN23XX_CONFIG_SRIOV_BAR_IO		   0x01
+
+/* ##############  BAR0 Registers ################ */
+
+#define    CN23XX_SLI_CTL_PORT_START               0x286E0
+#define    CN23XX_PORT_OFFSET                      0x10
+
+#define    CN23XX_SLI_CTL_PORT(p)                  \
+		(CN23XX_SLI_CTL_PORT_START + ((p) * CN23XX_PORT_OFFSET))
+
+/* 2 scatch registers (64-bit)  */
+#define    CN23XX_SLI_WINDOW_CTL                   0x282E0
+#define    CN23XX_SLI_SCRATCH1                     0x283C0
+#define    CN23XX_SLI_SCRATCH2                     0x283D0
+#define    CN23XX_SLI_WINDOW_CTL_DEFAULT           0x200000ULL
+
+/* 1 registers (64-bit)  - SLI_CTL_STATUS */
+#define    CN23XX_SLI_CTL_STATUS                   0x28570
+
+/* SLI Packet Input Jabber Register (64 bit register)
+ * <31:0> for Byte count for limiting sizes of packet sizes
+ * that are allowed for sli packet inbound packets.
+ * the default value is 0xFA00(=64000).
+ */
+#define    CN23XX_SLI_PKT_IN_JABBER                0x29170
+/* The input jabber is used to determine the TSO max size.
+ * Due to H/W limitation, this need to be reduced to 60000
+ * in order to to H/W TSO and avoid the WQE malfarmation
+ * PKO_BUG_24989_WQE_LEN
+ */
+#define    CN23XX_DEFAULT_INPUT_JABBER             0xEA60 /*60000*/
+
+#define    CN23XX_WIN_WR_ADDR_LO                   0x20000
+#define    CN23XX_WIN_WR_ADDR_HI                   0x20004
+#define    CN23XX_WIN_WR_ADDR64                    CN23XX_WIN_WR_ADDR_LO
+
+#define    CN23XX_WIN_RD_ADDR_LO                   0x20010
+#define    CN23XX_WIN_RD_ADDR_HI                   0x20014
+#define    CN23XX_WIN_RD_ADDR64                    CN23XX_WIN_RD_ADDR_LO
+
+#define    CN23XX_WIN_WR_DATA_LO                   0x20020
+#define    CN23XX_WIN_WR_DATA_HI                   0x20024
+#define    CN23XX_WIN_WR_DATA64                    CN23XX_WIN_WR_DATA_LO
+
+#define    CN23XX_WIN_RD_DATA_LO                   0x20040
+#define    CN23XX_WIN_RD_DATA_HI                   0x20044
+#define    CN23XX_WIN_RD_DATA64                    CN23XX_WIN_RD_DATA_LO
+
+#define    CN23XX_WIN_WR_MASK_LO                   0x20030
+#define    CN23XX_WIN_WR_MASK_HI                   0x20034
+#define    CN23XX_WIN_WR_MASK_REG                  CN23XX_WIN_WR_MASK_LO
+#define    CN23XX_SLI_MAC_CREDIT_CNT               0x23D70
+
+/* 4 registers (64-bit) for mapping IOQs to MACs(PEMs)-
+ * SLI_PKT_MAC(0..3)_PF(0..1)_RINFO
+ */
+#define    CN23XX_SLI_PKT_MAC_RINFO_START64       0x29030
+
+/*1 register (64-bit) to determine whether IOQs are in reset. */
+#define    CN23XX_SLI_PKT_IOQ_RING_RST            0x291E0
+
+/* Each Input Queue register is at a 16-byte Offset in BAR0 */
+#define    CN23XX_IQ_OFFSET                       0x20000
+
+#define    CN23XX_MAC_RINFO_OFFSET                0x20
+#define    CN23XX_PF_RINFO_OFFSET                 0x10
+
+#define CN23XX_SLI_PKT_MAC_RINFO64(mac, pf)		\
+		(CN23XX_SLI_PKT_MAC_RINFO_START64 +     \
+		 ((mac) * CN23XX_MAC_RINFO_OFFSET) +	\
+		 ((pf) * CN23XX_PF_RINFO_OFFSET))
+
+/** mask for total rings, setting TRS to base */
+#define    CN23XX_PKT_MAC_CTL_RINFO_TRS               BIT_ULL(16)
+/** mask for starting ring number: setting SRN <6:0> = 0x7F */
+#define    CN23XX_PKT_MAC_CTL_RINFO_SRN               (0x7F)
+
+/* Starting bit of the TRS field in CN23XX_SLI_PKT_MAC_RINFO64 register */
+#define    CN23XX_PKT_MAC_CTL_RINFO_TRS_BIT_POS     16
+/* Starting bit of SRN field in CN23XX_SLI_PKT_MAC_RINFO64 register */
+#define    CN23XX_PKT_MAC_CTL_RINFO_SRN_BIT_POS     0
+/* Starting bit of RPVF field in CN23XX_SLI_PKT_MAC_RINFO64 register */
+#define    CN23XX_PKT_MAC_CTL_RINFO_RPVF_BIT_POS     32
+/* Starting bit of NVFS field in CN23XX_SLI_PKT_MAC_RINFO64 register */
+#define    CN23XX_PKT_MAC_CTL_RINFO_NVFS_BIT_POS     48
+
+/*###################### REQUEST QUEUE #########################*/
+
+/* 64 registers for Input Queue Instr Count - SLI_PKT_IN_DONE0_CNTS */
+#define    CN23XX_SLI_IQ_INSTR_COUNT_START64     0x10040
+
+/* 64 registers for Input Queues Start Addr - SLI_PKT0_INSTR_BADDR */
+#define    CN23XX_SLI_IQ_BASE_ADDR_START64       0x10010
+
+/* 64 registers for Input Doorbell - SLI_PKT0_INSTR_BAOFF_DBELL */
+#define    CN23XX_SLI_IQ_DOORBELL_START          0x10020
+
+/* 64 registers for Input Queue size - SLI_PKT0_INSTR_FIFO_RSIZE */
+#define    CN23XX_SLI_IQ_SIZE_START              0x10030
+
+/* 64 registers (64-bit) - ES, RO, NS, Arbitration for Input Queue Data &
+ * gather list fetches. SLI_PKT(0..63)_INPUT_CONTROL.
+ */
+#define    CN23XX_SLI_IQ_PKT_CONTROL_START64    0x10000
+
+/*------- Request Queue Macros ---------*/
+#define    CN23XX_SLI_IQ_PKT_CONTROL64(iq)          \
+		(CN23XX_SLI_IQ_PKT_CONTROL_START64 + ((iq) * CN23XX_IQ_OFFSET))
+
+#define    CN23XX_SLI_IQ_BASE_ADDR64(iq)          \
+		(CN23XX_SLI_IQ_BASE_ADDR_START64 + ((iq) * CN23XX_IQ_OFFSET))
+
+#define    CN23XX_SLI_IQ_SIZE(iq)                 \
+		(CN23XX_SLI_IQ_SIZE_START + ((iq) * CN23XX_IQ_OFFSET))
+
+#define    CN23XX_SLI_IQ_DOORBELL(iq)             \
+		(CN23XX_SLI_IQ_DOORBELL_START + ((iq) * CN23XX_IQ_OFFSET))
+
+#define    CN23XX_SLI_IQ_INSTR_COUNT64(iq)          \
+		(CN23XX_SLI_IQ_INSTR_COUNT_START64 + ((iq) * CN23XX_IQ_OFFSET))
+
+/*------------------ Masks ----------------*/
+#define    CN23XX_PKT_INPUT_CTL_VF_NUM                  BIT_ULL(32)
+#define    CN23XX_PKT_INPUT_CTL_MAC_NUM                 BIT(29)
+/* Number of instructions to be read in one MAC read request.
+ * setting to Max value(4)
+ */
+#define    CN23XX_PKT_INPUT_CTL_RDSIZE                  (3 << 25)
+#define    CN23XX_PKT_INPUT_CTL_IS_64B                  BIT(24)
+#define    CN23XX_PKT_INPUT_CTL_RST                     BIT(23)
+#define    CN23XX_PKT_INPUT_CTL_QUIET                   BIT(28)
+#define    CN23XX_PKT_INPUT_CTL_RING_ENB                BIT(22)
+#define    CN23XX_PKT_INPUT_CTL_DATA_NS                 BIT(8)
+#define    CN23XX_PKT_INPUT_CTL_DATA_ES_64B_SWAP        BIT(6)
+#define    CN23XX_PKT_INPUT_CTL_DATA_RO                 BIT(5)
+#define    CN23XX_PKT_INPUT_CTL_USE_CSR                 BIT(4)
+#define    CN23XX_PKT_INPUT_CTL_GATHER_NS               BIT(3)
+#define    CN23XX_PKT_INPUT_CTL_GATHER_ES_64B_SWAP      (2)
+#define    CN23XX_PKT_INPUT_CTL_GATHER_RO               (1)
+
+/** Rings per Virtual Function **/
+#define    CN23XX_PKT_INPUT_CTL_RPVF_MASK               (0x3F)
+#define    CN23XX_PKT_INPUT_CTL_RPVF_POS                (48)
+/** These bits[47:44] select the Physical function number within the MAC */
+#define    CN23XX_PKT_INPUT_CTL_PF_NUM_MASK             (0x7)
+#define    CN23XX_PKT_INPUT_CTL_PF_NUM_POS              (45)
+/** These bits[43:32] select the function number within the PF */
+#define    CN23XX_PKT_INPUT_CTL_VF_NUM_MASK             (0x1FFF)
+#define    CN23XX_PKT_INPUT_CTL_VF_NUM_POS              (32)
+#define    CN23XX_PKT_INPUT_CTL_MAC_NUM_MASK            (0x3)
+#define    CN23XX_PKT_INPUT_CTL_MAC_NUM_POS             (29)
+#define    CN23XX_PKT_IN_DONE_WMARK_MASK                (0xFFFFULL)
+#define    CN23XX_PKT_IN_DONE_WMARK_BIT_POS             (32)
+#define    CN23XX_PKT_IN_DONE_CNT_MASK                  (0x00000000FFFFFFFFULL)
+
+#ifdef __LITTLE_ENDIAN_BITFIELD
+#define    CN23XX_PKT_INPUT_CTL_MASK				\
+		(CN23XX_PKT_INPUT_CTL_RDSIZE		|	\
+		 CN23XX_PKT_INPUT_CTL_DATA_ES_64B_SWAP	|	\
+		 CN23XX_PKT_INPUT_CTL_USE_CSR)
+#else
+#define    CN23XX_PKT_INPUT_CTL_MASK				\
+		(CN23XX_PKT_INPUT_CTL_RDSIZE		|	\
+		 CN23XX_PKT_INPUT_CTL_DATA_ES_64B_SWAP	|	\
+		 CN23XX_PKT_INPUT_CTL_USE_CSR		|	\
+		 CN23XX_PKT_INPUT_CTL_GATHER_ES_64B_SWAP)
+#endif
+
+/** Masks for SLI_PKT_IN_DONE(0..63)_CNTS Register */
+#define    CN23XX_IN_DONE_CNTS_PI_INT               BIT_ULL(62)
+#define    CN23XX_IN_DONE_CNTS_CINT_ENB             BIT_ULL(48)
+
+/*############################ OUTPUT QUEUE #########################*/
+
+/* 64 registers for Output queue control - SLI_PKT(0..63)_OUTPUT_CONTROL */
+#define    CN23XX_SLI_OQ_PKT_CONTROL_START       0x10050
+
+/* 64 registers for Output queue buffer and info size - SLI_PKT0_OUT_SIZE */
+#define    CN23XX_SLI_OQ0_BUFF_INFO_SIZE         0x10060
+
+/* 64 registers for Output Queue Start Addr - SLI_PKT0_SLIST_BADDR */
+#define    CN23XX_SLI_OQ_BASE_ADDR_START64       0x10070
+
+/* 64 registers for Output Queue Packet Credits - SLI_PKT0_SLIST_BAOFF_DBELL */
+#define    CN23XX_SLI_OQ_PKT_CREDITS_START       0x10080
+
+/* 64 registers for Output Queue size - SLI_PKT0_SLIST_FIFO_RSIZE */
+#define    CN23XX_SLI_OQ_SIZE_START              0x10090
+
+/* 64 registers for Output Queue Packet Count - SLI_PKT0_CNTS */
+#define    CN23XX_SLI_OQ_PKT_SENT_START          0x100B0
+
+/* 64 registers for Output Queue INT Levels - SLI_PKT0_INT_LEVELS */
+#define    CN23XX_SLI_OQ_PKT_INT_LEVELS_START64   0x100A0
+
+/* Each Output Queue register is at a 16-byte Offset in BAR0 */
+#define    CN23XX_OQ_OFFSET                      0x20000
+
+/* 1 (64-bit register) for Output Queue backpressure across all rings. */
+#define    CN23XX_SLI_OQ_WMARK                   0x29180
+
+/* Global pkt control register */
+#define    CN23XX_SLI_GBL_CONTROL                0x29210
+
+/* Backpressure enable register for PF0  */
+#define    CN23XX_SLI_OUT_BP_EN_W1S              0x29260
+
+/* Backpressure enable register for PF1  */
+#define    CN23XX_SLI_OUT_BP_EN2_W1S             0x29270
+
+/* Backpressure disable register for PF0  */
+#define    CN23XX_SLI_OUT_BP_EN_W1C              0x29280
+
+/* Backpressure disable register for PF1  */
+#define    CN23XX_SLI_OUT_BP_EN2_W1C             0x29290
+
+/*------- Output Queue Macros ---------*/
+
+#define    CN23XX_SLI_OQ_PKT_CONTROL(oq)          \
+		(CN23XX_SLI_OQ_PKT_CONTROL_START + ((oq) * CN23XX_OQ_OFFSET))
+
+#define    CN23XX_SLI_OQ_BASE_ADDR64(oq)          \
+		(CN23XX_SLI_OQ_BASE_ADDR_START64 + ((oq) * CN23XX_OQ_OFFSET))
+
+#define    CN23XX_SLI_OQ_SIZE(oq)                 \
+		(CN23XX_SLI_OQ_SIZE_START + ((oq) * CN23XX_OQ_OFFSET))
+
+#define    CN23XX_SLI_OQ_BUFF_INFO_SIZE(oq)                 \
+		(CN23XX_SLI_OQ0_BUFF_INFO_SIZE + ((oq) * CN23XX_OQ_OFFSET))
+
+#define    CN23XX_SLI_OQ_PKTS_SENT(oq)            \
+		(CN23XX_SLI_OQ_PKT_SENT_START + ((oq) * CN23XX_OQ_OFFSET))
+
+#define    CN23XX_SLI_OQ_PKTS_CREDIT(oq)          \
+		(CN23XX_SLI_OQ_PKT_CREDITS_START + ((oq) * CN23XX_OQ_OFFSET))
+
+#define    CN23XX_SLI_OQ_PKT_INT_LEVELS(oq)		\
+		(CN23XX_SLI_OQ_PKT_INT_LEVELS_START64 +	\
+		 ((oq) * CN23XX_OQ_OFFSET))
+
+/*Macro's for accessing CNT and TIME separately from INT_LEVELS*/
+#define    CN23XX_SLI_OQ_PKT_INT_LEVELS_CNT(oq)		\
+		(CN23XX_SLI_OQ_PKT_INT_LEVELS_START64 + \
+		 ((oq) * CN23XX_OQ_OFFSET))
+
+#define    CN23XX_SLI_OQ_PKT_INT_LEVELS_TIME(oq)	\
+		(CN23XX_SLI_OQ_PKT_INT_LEVELS_START64 +	\
+		 ((oq) * CN23XX_OQ_OFFSET) + 4)
+
+/*------------------ Masks ----------------*/
+#define    CN23XX_PKT_OUTPUT_CTL_TENB                  BIT(13)
+#define    CN23XX_PKT_OUTPUT_CTL_CENB                  BIT(12)
+#define    CN23XX_PKT_OUTPUT_CTL_IPTR                  BIT(11)
+#define    CN23XX_PKT_OUTPUT_CTL_ES                    BIT(9)
+#define    CN23XX_PKT_OUTPUT_CTL_NSR                   BIT(8)
+#define    CN23XX_PKT_OUTPUT_CTL_ROR                   BIT(7)
+#define    CN23XX_PKT_OUTPUT_CTL_DPTR                  BIT(6)
+#define    CN23XX_PKT_OUTPUT_CTL_BMODE                 BIT(5)
+#define    CN23XX_PKT_OUTPUT_CTL_ES_P                  BIT(3)
+#define    CN23XX_PKT_OUTPUT_CTL_NSR_P                 BIT(2)
+#define    CN23XX_PKT_OUTPUT_CTL_ROR_P                 BIT(1)
+#define    CN23XX_PKT_OUTPUT_CTL_RING_ENB              BIT(0)
+
+/*######################### Mailbox Reg Macros ########################*/
+#define    CN23XX_SLI_PKT_MBOX_INT_START             0x10210
+#define    CN23XX_SLI_PKT_PF_VF_MBOX_SIG_START       0x10200
+#define    CN23XX_SLI_MAC_PF_MBOX_INT_START          0x27380
+
+#define    CN23XX_SLI_MBOX_OFFSET		     0x20000
+#define    CN23XX_SLI_MBOX_SIG_IDX_OFFSET	     0x8
+
+#define    CN23XX_SLI_PKT_MBOX_INT(q)          \
+		(CN23XX_SLI_PKT_MBOX_INT_START + ((q) * CN23XX_SLI_MBOX_OFFSET))
+
+#define    CN23XX_SLI_PKT_PF_VF_MBOX_SIG(q, idx)		\
+		(CN23XX_SLI_PKT_PF_VF_MBOX_SIG_START +		\
+		 ((q) * CN23XX_SLI_MBOX_OFFSET +		\
+		  (idx) * CN23XX_SLI_MBOX_SIG_IDX_OFFSET))
+
+#define    CN23XX_SLI_MAC_PF_MBOX_INT(mac, pf)		\
+		(CN23XX_SLI_MAC_PF_MBOX_INT_START +	\
+		 ((mac) * CN23XX_MAC_INT_OFFSET +	\
+		  (pf) * CN23XX_PF_INT_OFFSET))
+
+/*######################### DMA Counters #########################*/
+
+/* 2 registers (64-bit) - DMA Count - 1 for each DMA counter 0/1. */
+#define    CN23XX_DMA_CNT_START                   0x28400
+
+/* 2 registers (64-bit) - DMA Timer 0/1, contains DMA timer values */
+/* SLI_DMA_0_TIM */
+#define    CN23XX_DMA_TIM_START                   0x28420
+
+/* 2 registers (64-bit) - DMA count & Time Interrupt threshold -
+ * SLI_DMA_0_INT_LEVEL
+ */
+#define    CN23XX_DMA_INT_LEVEL_START             0x283E0
+
+/* Each DMA register is at a 16-byte Offset in BAR0 */
+#define    CN23XX_DMA_OFFSET                      0x10
+
+/*---------- DMA Counter Macros ---------*/
+#define    CN23XX_DMA_CNT(dq)                      \
+		(CN23XX_DMA_CNT_START + ((dq) * CN23XX_DMA_OFFSET))
+
+#define    CN23XX_DMA_INT_LEVEL(dq)                \
+		(CN23XX_DMA_INT_LEVEL_START + ((dq) * CN23XX_DMA_OFFSET))
+
+#define    CN23XX_DMA_PKT_INT_LEVEL(dq)            \
+		(CN23XX_DMA_INT_LEVEL_START + ((dq) * CN23XX_DMA_OFFSET))
+
+#define    CN23XX_DMA_TIME_INT_LEVEL(dq)           \
+		(CN23XX_DMA_INT_LEVEL_START + 4 + ((dq) * CN23XX_DMA_OFFSET))
+
+#define    CN23XX_DMA_TIM(dq)                     \
+		(CN23XX_DMA_TIM_START + ((dq) * CN23XX_DMA_OFFSET))
+
+/*######################## MSIX TABLE #########################*/
+
+#define	CN23XX_MSIX_TABLE_ADDR_START		0x0
+#define	CN23XX_MSIX_TABLE_DATA_START		0x8
+
+#define	CN23XX_MSIX_TABLE_SIZE			0x10
+#define	CN23XX_MSIX_TABLE_ENTRIES		0x41
+
+#define CN23XX_MSIX_ENTRY_VECTOR_CTL	BIT_ULL(32)
+
+#define	CN23XX_MSIX_TABLE_ADDR(idx)		\
+	(CN23XX_MSIX_TABLE_ADDR_START + ((idx) * CN23XX_MSIX_TABLE_SIZE))
+
+#define	CN23XX_MSIX_TABLE_DATA(idx)		\
+	(CN23XX_MSIX_TABLE_DATA_START + ((idx) * CN23XX_MSIX_TABLE_SIZE))
+
+/*######################## INTERRUPTS #########################*/
+#define CN23XX_MAC_INT_OFFSET   0x20
+#define CN23XX_PF_INT_OFFSET    0x10
+
+/* 1 register (64-bit) for Interrupt Summary */
+#define    CN23XX_SLI_INT_SUM64            0x27000
+
+/* 4 registers (64-bit) for Interrupt Enable for each Port */
+#define    CN23XX_SLI_INT_ENB64            0x27080
+
+#define    CN23XX_SLI_MAC_PF_INT_SUM64(mac, pf)			\
+		(CN23XX_SLI_INT_SUM64 +				\
+		 ((mac) * CN23XX_MAC_INT_OFFSET) +		\
+		 ((pf) * CN23XX_PF_INT_OFFSET))
+
+#define    CN23XX_SLI_MAC_PF_INT_ENB64(mac, pf)		\
+		(CN23XX_SLI_INT_ENB64 +			\
+		 ((mac) * CN23XX_MAC_INT_OFFSET) +	\
+		 ((pf) * CN23XX_PF_INT_OFFSET))
+
+/* 1 register (64-bit) to indicate which Output Queue reached pkt threshold */
+#define    CN23XX_SLI_PKT_CNT_INT                0x29130
+
+/* 1 register (64-bit) to indicate which Output Queue reached time threshold */
+#define    CN23XX_SLI_PKT_TIME_INT               0x29140
+
+/*------------------ Interrupt Masks ----------------*/
+
+#define    CN23XX_INTR_PO_INT			BIT_ULL(63)
+#define    CN23XX_INTR_PI_INT			BIT_ULL(62)
+#define    CN23XX_INTR_MBOX_INT			BIT_ULL(61)
+#define    CN23XX_INTR_RESEND			BIT_ULL(60)
+
+#define    CN23XX_INTR_CINT_ENB                 BIT_ULL(48)
+#define    CN23XX_INTR_MBOX_ENB                 BIT(0)
+
+#define    CN23XX_INTR_RML_TIMEOUT_ERR           (1)
+
+#define    CN23XX_INTR_MIO_INT                   BIT(1)
+
+#define    CN23XX_INTR_RESERVED1                 (3 << 2)
+
+#define    CN23XX_INTR_PKT_COUNT                 BIT(4)
+#define    CN23XX_INTR_PKT_TIME                  BIT(5)
+
+#define    CN23XX_INTR_RESERVED2                 (3 << 6)
+
+#define    CN23XX_INTR_M0UPB0_ERR                BIT(8)
+#define    CN23XX_INTR_M0UPWI_ERR                BIT(9)
+#define    CN23XX_INTR_M0UNB0_ERR                BIT(10)
+#define    CN23XX_INTR_M0UNWI_ERR                BIT(11)
+
+#define    CN23XX_INTR_RESERVED3                 (0xFFFFFULL << 12)
+
+#define    CN23XX_INTR_DMA0_FORCE                BIT_ULL(32)
+#define    CN23XX_INTR_DMA1_FORCE                BIT_ULL(33)
+
+#define    CN23XX_INTR_DMA0_COUNT                BIT_ULL(34)
+#define    CN23XX_INTR_DMA1_COUNT                BIT_ULL(35)
+
+#define    CN23XX_INTR_DMA0_TIME                 BIT_ULL(36)
+#define    CN23XX_INTR_DMA1_TIME                 BIT_ULL(37)
+
+#define    CN23XX_INTR_RESERVED4                 (0x7FFFFULL << 38)
+
+#define    CN23XX_INTR_VF_MBOX                   BIT_ULL(57)
+#define    CN23XX_INTR_DMAVF_ERR                 BIT_ULL(58)
+#define    CN23XX_INTR_DMAPF_ERR                 BIT_ULL(59)
+
+#define    CN23XX_INTR_PKTVF_ERR                 BIT_ULL(60)
+#define    CN23XX_INTR_PKTPF_ERR                 BIT_ULL(61)
+#define    CN23XX_INTR_PPVF_ERR                  BIT_ULL(62)
+#define    CN23XX_INTR_PPPF_ERR                  BIT_ULL(63)
+
+#define    CN23XX_INTR_DMA0_DATA                 (CN23XX_INTR_DMA0_TIME)
+#define    CN23XX_INTR_DMA1_DATA                 (CN23XX_INTR_DMA1_TIME)
+
+#define    CN23XX_INTR_DMA_DATA                  \
+		(CN23XX_INTR_DMA0_DATA | CN23XX_INTR_DMA1_DATA)
+
+/* By fault only TIME based */
+#define    CN23XX_INTR_PKT_DATA                  (CN23XX_INTR_PKT_TIME)
+/* For both COUNT and TIME based */
+/* #define    CN23XX_INTR_PKT_DATA                  \
+ * (CN23XX_INTR_PKT_COUNT | CN23XX_INTR_PKT_TIME)
+ */
+
+/* Sum of interrupts for all PCI-Express Data Interrupts */
+#define    CN23XX_INTR_PCIE_DATA                 \
+		(CN23XX_INTR_DMA_DATA | CN23XX_INTR_PKT_DAT)
+
+/* Sum of interrupts for error events */
+#define    CN23XX_INTR_ERR			\
+		(CN23XX_INTR_M0UPB0_ERR	|	\
+		 CN23XX_INTR_M0UPWI_ERR	|	\
+		 CN23XX_INTR_M0UNB0_ERR	|	\
+		 CN23XX_INTR_M0UNWI_ERR	|	\
+		 CN23XX_INTR_DMAVF_ERR	|	\
+		 CN23XX_INTR_DMAPF_ERR	|	\
+		 CN23XX_INTR_PKTPF_ERR	|	\
+		 CN23XX_INTR_PPPF_ERR	|	\
+		 CN23XX_INTR_PPVF_ERR)
+
+/* Programmed Mask for Interrupt Sum */
+#define    CN23XX_INTR_MASK			\
+		(CN23XX_INTR_DMA_DATA	|	\
+		 CN23XX_INTR_DMA0_FORCE	|	\
+		 CN23XX_INTR_DMA1_FORCE	|	\
+		 CN23XX_INTR_MIO_INT	|	\
+		 CN23XX_INTR_ERR)
+
+/* 4 Registers (64 - bit) */
+#define    CN23XX_SLI_S2M_PORT_CTL_START         0x23D80
+#define    CN23XX_SLI_S2M_PORTX_CTL(port)	\
+		(CN23XX_SLI_S2M_PORT_CTL_START + (port * 0x10))
+
+#define    CN23XX_SLI_MAC_NUMBER                 0x20050
+
+/** PEM(0..3)_BAR1_INDEX(0..15)address is defined as
+ *  addr = (0x00011800C0000100  |port <<24 |idx <<3 )
+ *  Here, port is PEM(0..3) & idx is INDEX(0..15)
+ */
+#define    CN23XX_PEM_BAR1_INDEX_START             0x00011800C0000100ULL
+#define    CN23XX_PEM_OFFSET                       24
+#define    CN23XX_BAR1_INDEX_OFFSET                3
+
+#define    CN23XX_PEM_BAR1_INDEX_REG(port, idx)		\
+		(CN23XX_PEM_BAR1_INDEX_START + ((port) << CN23XX_PEM_OFFSET) + \
+		 ((idx) << CN23XX_BAR1_INDEX_OFFSET))
+
+/*############################ DPI #########################*/
+
+/* 1 register (64-bit) - provides DMA Enable */
+#define    CN23XX_DPI_CTL                 0x0001df0000000040ULL
+
+/* 1 register (64-bit) - Controls the DMA IO Operation */
+#define    CN23XX_DPI_DMA_CONTROL         0x0001df0000000048ULL
+
+/* 1 register (64-bit) - Provides DMA Instr'n Queue Enable  */
+#define    CN23XX_DPI_REQ_GBL_ENB         0x0001df0000000050ULL
+
+/* 1 register (64-bit) - DPI_REQ_ERR_RSP
+ * Indicates which Instr'n Queue received error response from the IO sub-system
+ */
+#define    CN23XX_DPI_REQ_ERR_RSP         0x0001df0000000058ULL
+
+/* 1 register (64-bit) - DPI_REQ_ERR_RST
+ * Indicates which Instr'n Queue dropped an Instr'n
+ */
+#define    CN23XX_DPI_REQ_ERR_RST         0x0001df0000000060ULL
+
+/* 6 register (64-bit) - DPI_DMA_ENG(0..5)_EN
+ * Provides DMA Engine Queue Enable
+ */
+#define    CN23XX_DPI_DMA_ENG0_ENB        0x0001df0000000080ULL
+#define    CN23XX_DPI_DMA_ENG_ENB(eng) (CN23XX_DPI_DMA_ENG0_ENB + (eng * 8))
+
+/* 8 register (64-bit) - DPI_DMA(0..7)_REQQ_CTL
+ * Provides control bits for transaction on 8 Queues
+ */
+#define    CN23XX_DPI_DMA_REQQ0_CTL       0x0001df0000000180ULL
+#define    CN23XX_DPI_DMA_REQQ_CTL(q_no)	\
+		(CN23XX_DPI_DMA_REQQ0_CTL + (q_no * 8))
+
+/* 6 register (64-bit) - DPI_ENG(0..5)_BUF
+ * Provides DMA Engine FIFO (Queue) Size
+ */
+#define    CN23XX_DPI_DMA_ENG0_BUF        0x0001df0000000880ULL
+#define    CN23XX_DPI_DMA_ENG_BUF(eng)   \
+		(CN23XX_DPI_DMA_ENG0_BUF + (eng * 8))
+
+/* 4 Registers (64-bit) */
+#define    CN23XX_DPI_SLI_PRT_CFG_START   0x0001df0000000900ULL
+#define    CN23XX_DPI_SLI_PRTX_CFG(port)        \
+		(CN23XX_DPI_SLI_PRT_CFG_START + (port * 0x8))
+
+/* Masks for DPI_DMA_CONTROL Register */
+#define    CN23XX_DPI_DMA_COMMIT_MODE     BIT_ULL(58)
+#define    CN23XX_DPI_DMA_PKT_EN          BIT_ULL(56)
+#define    CN23XX_DPI_DMA_ENB             (0x0FULL << 48)
+/* Set the DMA Control, to update packet count not byte count sent by DMA,
+ * when we use Interrupt Coalescing (CA mode)
+ */
+#define    CN23XX_DPI_DMA_O_ADD1          BIT(19)
+/*selecting 64-bit Byte Swap Mode */
+#define    CN23XX_DPI_DMA_O_ES            BIT(15)
+#define    CN23XX_DPI_DMA_O_MODE          BIT(14)
+
+#define    CN23XX_DPI_DMA_CTL_MASK			\
+		(CN23XX_DPI_DMA_COMMIT_MODE	|	\
+		 CN23XX_DPI_DMA_PKT_EN		|	\
+		 CN23XX_DPI_DMA_O_ES		|	\
+		 CN23XX_DPI_DMA_O_MODE)
+
+/*############################ RST #########################*/
+
+#define    CN23XX_RST_BOOT            0x0001180006001600ULL
+#define    CN23XX_RST_SOFT_RST        0x0001180006001680ULL
+
+#define    CN23XX_LMC0_RESET_CTL               0x0001180088000180ULL
+#define    CN23XX_LMC0_RESET_CTL_DDR3RST_MASK  0x0000000000000001ULL
+
+#endif
diff --git a/drivers/net/ethernet/cavium/liquidio/cn66xx_device.c b/drivers/net/ethernet/cavium/liquidio/cn66xx_device.c
index c03d37016a48..e779af88621b 100644
--- a/drivers/net/ethernet/cavium/liquidio/cn66xx_device.c
+++ b/drivers/net/ethernet/cavium/liquidio/cn66xx_device.c
@@ -338,7 +338,7 @@ void lio_cn6xxx_setup_oq_regs(struct octeon_device *oct, u32 oq_no)
 	octeon_write_csr(oct, CN6XXX_SLI_PKT_CNT_INT_ENB, intr);
 }
 
-void lio_cn6xxx_enable_io_queues(struct octeon_device *oct)
+int lio_cn6xxx_enable_io_queues(struct octeon_device *oct)
 {
 	u32 mask;
 
@@ -353,6 +353,8 @@ void lio_cn6xxx_enable_io_queues(struct octeon_device *oct)
 	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);
+
+	return 0;
 }
 
 void lio_cn6xxx_disable_io_queues(struct octeon_device *oct)
@@ -418,36 +420,6 @@ void lio_cn6xxx_disable_io_queues(struct octeon_device *oct)
 		octeon_write_csr(oct, CN6XXX_SLI_PKT_TIME_INT, d32);
 }
 
-void lio_cn6xxx_reinit_regs(struct octeon_device *oct)
-{
-	int i;
-
-	for (i = 0; i < MAX_OCTEON_INSTR_QUEUES(oct); i++) {
-		if (!(oct->io_qmask.iq & (1ULL << i)))
-			continue;
-		oct->fn_list.setup_iq_regs(oct, i);
-	}
-
-	for (i = 0; i < MAX_OCTEON_OUTPUT_QUEUES(oct); i++) {
-		if (!(oct->io_qmask.oq & (1ULL << 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(oct); i++) {
-		if (!(oct->io_qmask.oq & (1ULL << 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,
@@ -507,18 +479,20 @@ lio_cn6xxx_update_read_index(struct octeon_instr_queue *iq)
 	return new_idx;
 }
 
-void lio_cn6xxx_enable_interrupt(void *chip)
+void lio_cn6xxx_enable_interrupt(struct octeon_device *oct,
+				 u8 unused __attribute__((unused)))
 {
-	struct octeon_cn6xxx *cn6xxx = (struct octeon_cn6xxx *)chip;
+	struct octeon_cn6xxx *cn6xxx = (struct octeon_cn6xxx *)oct->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)
+void lio_cn6xxx_disable_interrupt(struct octeon_device *oct,
+				  u8 unused __attribute__((unused)))
 {
-	struct octeon_cn6xxx *cn6xxx = (struct octeon_cn6xxx *)chip;
+	struct octeon_cn6xxx *cn6xxx = (struct octeon_cn6xxx *)oct->chip;
 
 	/* Disable Interrupts */
 	writeq(0, cn6xxx->intr_enb_reg64);
@@ -714,7 +688,6 @@ int lio_setup_cn66xx_octeon_device(struct octeon_device *oct)
 
 	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;
diff --git a/drivers/net/ethernet/cavium/liquidio/cn66xx_device.h b/drivers/net/ethernet/cavium/liquidio/cn66xx_device.h
index 28c47224221a..a40a91394079 100644
--- a/drivers/net/ethernet/cavium/liquidio/cn66xx_device.h
+++ b/drivers/net/ethernet/cavium/liquidio/cn66xx_device.h
@@ -80,18 +80,17 @@ 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);
+int lio_cn6xxx_enable_io_queues(struct octeon_device *oct);
 void lio_cn6xxx_disable_io_queues(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_instr_queue *iq);
-void lio_cn6xxx_enable_interrupt(void *chip);
-void lio_cn6xxx_disable_interrupt(void *chip);
+void lio_cn6xxx_enable_interrupt(struct octeon_device *oct, u8 unused);
+void lio_cn6xxx_disable_interrupt(struct octeon_device *oct, u8 unused);
 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);
diff --git a/drivers/net/ethernet/cavium/liquidio/cn68xx_device.c b/drivers/net/ethernet/cavium/liquidio/cn68xx_device.c
index 29755bc68f12..dbf3566ead53 100644
--- a/drivers/net/ethernet/cavium/liquidio/cn68xx_device.c
+++ b/drivers/net/ethernet/cavium/liquidio/cn68xx_device.c
@@ -148,7 +148,6 @@ int lio_setup_cn68xx_octeon_device(struct octeon_device *oct)
 	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;
diff --git a/drivers/net/ethernet/cavium/liquidio/lio_core.c b/drivers/net/ethernet/cavium/liquidio/lio_core.c
new file mode 100644
index 000000000000..201eddb3013a
--- /dev/null
+++ b/drivers/net/ethernet/cavium/liquidio/lio_core.c
@@ -0,0 +1,266 @@
+/**********************************************************************
+* 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/pci.h>
+#include <linux/if_vlan.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"
+
+int liquidio_set_feature(struct net_device *netdev, int cmd, u16 param1)
+{
+	struct lio *lio = GET_LIO(netdev);
+	struct octeon_device *oct = lio->oct_dev;
+	struct octnic_ctrl_pkt nctrl;
+	int ret = 0;
+
+	memset(&nctrl, 0, sizeof(struct octnic_ctrl_pkt));
+
+	nctrl.ncmd.u64 = 0;
+	nctrl.ncmd.s.cmd = cmd;
+	nctrl.ncmd.s.param1 = param1;
+	nctrl.iq_no = lio->linfo.txpciq[0].s.q_no;
+	nctrl.wait_time = 100;
+	nctrl.netpndev = (u64)netdev;
+	nctrl.cb_fn = liquidio_link_ctrl_cmd_completion;
+
+	ret = octnet_send_nic_ctrl_pkt(lio->oct_dev, &nctrl);
+	if (ret < 0) {
+		dev_err(&oct->pci_dev->dev, "Feature change failed in core (ret: 0x%x)\n",
+			ret);
+	}
+	return ret;
+}
+
+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)++;
+/*TODO, Use some other pound define to suggest
+ * the fact that iqs are not tied to netdevs
+ * and can take traffic from different netdevs
+ * hence bql reporting is done per packet
+ * than in bulk. Usage of NO_NAPI in txq completion is
+ * a little confusing
+ */
+	*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);
+}
+
+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;
+	u8 *mac;
+
+	switch (nctrl->ncmd.s.cmd) {
+	case OCTNET_CMD_CHANGE_DEVFLAGS:
+	case OCTNET_CMD_SET_MULTI_LIST:
+		break;
+
+	case OCTNET_CMD_CHANGE_MACADDR:
+		mac = ((u8 *)&nctrl->udd[0]) + 2;
+		netif_info(lio, probe, lio->netdev,
+			   "MACAddr changed to %2.2x:%2.2x:%2.2x:%2.2x:%2.2x:%2.2x\n",
+			   mac[0], mac[1],
+			   mac[2], mac[3],
+			   mac[4], mac[5]);
+		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.param1);
+		dev_info(&oct->pci_dev->dev, "%s MTU Changed from %d to %d\n",
+			 netdev->name, netdev->mtu,
+			 nctrl->ncmd.s.param1);
+		netdev->mtu = nctrl->ncmd.s.param1;
+		queue_delayed_work(lio->link_status_wq.wq,
+				   &lio->link_status_wq.wk.work, 0);
+		break;
+
+	case OCTNET_CMD_GPIO_ACCESS:
+		netif_info(lio, probe, lio->netdev, "LED Flashing visual identification\n");
+
+		break;
+
+	case OCTNET_CMD_ID_ACTIVE:
+		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 Firmware debug enabled\n",
+			 netdev->name);
+		break;
+
+	case OCTNET_CMD_VERBOSE_DISABLE:
+		dev_info(&oct->pci_dev->dev, "%s Firmware debug disabled\n",
+			 netdev->name);
+		break;
+
+	case OCTNET_CMD_ENABLE_VLAN_FILTER:
+		dev_info(&oct->pci_dev->dev, "%s VLAN filter enabled\n",
+			 netdev->name);
+		break;
+
+	case OCTNET_CMD_ADD_VLAN_FILTER:
+		dev_info(&oct->pci_dev->dev, "%s VLAN filter %d added\n",
+			 netdev->name, nctrl->ncmd.s.param1);
+		break;
+
+	case OCTNET_CMD_DEL_VLAN_FILTER:
+		dev_info(&oct->pci_dev->dev, "%s VLAN filter %d removed\n",
+			 netdev->name, nctrl->ncmd.s.param1);
+		break;
+
+	case OCTNET_CMD_SET_SETTINGS:
+		dev_info(&oct->pci_dev->dev, "%s settings changed\n",
+			 netdev->name);
+
+		break;
+
+	/* Case to handle "OCTNET_CMD_TNL_RX_CSUM_CTL"
+	 * Command passed by NIC driver
+	 */
+	case OCTNET_CMD_TNL_RX_CSUM_CTL:
+		if (nctrl->ncmd.s.param1 == OCTNET_CMD_RXCSUM_ENABLE) {
+			netif_info(lio, probe, lio->netdev,
+				   "RX Checksum Offload Enabled\n");
+		} else if (nctrl->ncmd.s.param1 ==
+			   OCTNET_CMD_RXCSUM_DISABLE) {
+			netif_info(lio, probe, lio->netdev,
+				   "RX Checksum Offload Disabled\n");
+		}
+		break;
+
+		/* Case to handle "OCTNET_CMD_TNL_TX_CSUM_CTL"
+		 * Command passed by NIC driver
+		 */
+	case OCTNET_CMD_TNL_TX_CSUM_CTL:
+		if (nctrl->ncmd.s.param1 == OCTNET_CMD_TXCSUM_ENABLE) {
+			netif_info(lio, probe, lio->netdev,
+				   "TX Checksum Offload Enabled\n");
+		} else if (nctrl->ncmd.s.param1 ==
+			   OCTNET_CMD_TXCSUM_DISABLE) {
+			netif_info(lio, probe, lio->netdev,
+				   "TX Checksum Offload Disabled\n");
+		}
+		break;
+
+		/* Case to handle "OCTNET_CMD_VXLAN_PORT_CONFIG"
+		 * Command passed by NIC driver
+		 */
+	case OCTNET_CMD_VXLAN_PORT_CONFIG:
+		if (nctrl->ncmd.s.more == OCTNET_CMD_VXLAN_PORT_ADD) {
+			netif_info(lio, probe, lio->netdev,
+				   "VxLAN Destination UDP PORT:%d ADDED\n",
+				   nctrl->ncmd.s.param1);
+		} else if (nctrl->ncmd.s.more ==
+			   OCTNET_CMD_VXLAN_PORT_DEL) {
+			netif_info(lio, probe, lio->netdev,
+				   "VxLAN Destination UDP PORT:%d DELETED\n",
+				   nctrl->ncmd.s.param1);
+		}
+		break;
+
+	case OCTNET_CMD_SET_FLOW_CTL:
+		netif_info(lio, probe, lio->netdev, "Set RX/TX flow control parameters\n");
+		break;
+
+	default:
+		dev_err(&oct->pci_dev->dev, "%s Unknown cmd %d\n", __func__,
+			nctrl->ncmd.s.cmd);
+	}
+}
diff --git a/drivers/net/ethernet/cavium/liquidio/lio_ethtool.c b/drivers/net/ethernet/cavium/liquidio/lio_ethtool.c
index 289eb8907922..f163e0abbeb2 100644
--- a/drivers/net/ethernet/cavium/liquidio/lio_ethtool.c
+++ b/drivers/net/ethernet/cavium/liquidio/lio_ethtool.c
@@ -32,6 +32,7 @@
 #include "octeon_network.h"
 #include "cn66xx_regs.h"
 #include "cn66xx_device.h"
+#include "cn23xx_pf_device.h"
 
 static int octnet_get_link_stats(struct net_device *netdev);
 
@@ -75,6 +76,7 @@ enum {
 
 #define ARRAY_LENGTH(a) (sizeof(a) / sizeof((a)[0]))
 #define OCT_ETHTOOL_REGDUMP_LEN  4096
+#define OCT_ETHTOOL_REGDUMP_LEN_23XX  (4096 * 11)
 #define OCT_ETHTOOL_REGSVER  1
 
 /* statistics of PF */
@@ -188,6 +190,10 @@ static const char oct_droq_stats_strings[][ETH_GSTRING_LEN] = {
 	"buffer_alloc_failure",
 };
 
+/* LiquidIO driver private flags */
+static const char oct_priv_flags_strings[][ETH_GSTRING_LEN] = {
+};
+
 #define OCTNIC_NCMD_AUTONEG_ON  0x1
 #define OCTNIC_NCMD_PHY_ON      0x2
 
@@ -259,6 +265,13 @@ lio_ethtool_get_channels(struct net_device *dev,
 		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);
+	} else if (OCTEON_CN23XX_PF(oct)) {
+		struct octeon_config *conf23 = CHIP_FIELD(oct, cn23xx_pf, conf);
+
+		max_rx = CFG_GET_OQ_MAX_Q(conf23);
+		max_tx = CFG_GET_IQ_MAX_Q(conf23);
+		rx_count = CFG_GET_NUM_RXQS_NIC_IF(conf23, lio->ifidx);
+		tx_count = CFG_GET_NUM_TXQS_NIC_IF(conf23, lio->ifidx);
 	}
 
 	channel->max_rx = max_rx;
@@ -290,18 +303,16 @@ lio_get_eeprom(struct net_device *netdev, struct ethtool_eeprom *eeprom,
 	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)
+	if (eeprom->offset)
 		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);
+	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;
 }
@@ -333,6 +344,32 @@ static int octnet_gpio_access(struct net_device *netdev, int addr, int val)
 	return 0;
 }
 
+static int octnet_id_active(struct net_device *netdev, int val)
+{
+	struct lio *lio = GET_LIO(netdev);
+	struct octeon_device *oct = lio->oct_dev;
+	struct octnic_ctrl_pkt nctrl;
+	int ret = 0;
+
+	memset(&nctrl, 0, sizeof(struct octnic_ctrl_pkt));
+
+	nctrl.ncmd.u64 = 0;
+	nctrl.ncmd.s.cmd = OCTNET_CMD_ID_ACTIVE;
+	nctrl.ncmd.s.param1 = val;
+	nctrl.iq_no = lio->linfo.txpciq[0].s.q_no;
+	nctrl.wait_time = 100;
+	nctrl.netpndev = (u64)netdev;
+	nctrl.cb_fn = liquidio_link_ctrl_cmd_completion;
+
+	ret = octnet_send_nic_ctrl_pkt(lio->oct_dev, &nctrl);
+	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,
@@ -406,7 +443,7 @@ octnet_mdio45_access(struct lio *lio, int op, int loc, int *value)
 		dev_err(&oct_dev->pci_dev->dev,
 			"octnet_mdio45_access instruction failed status: %x\n",
 			retval);
-		retval =  -EBUSY;
+		retval = -EBUSY;
 	} else {
 		/* Sleep on a wait queue till the cond flag indicates that the
 		 * response arrived
@@ -476,6 +513,11 @@ static int lio_set_phys_id(struct net_device *netdev,
 						   &value);
 			if (ret)
 				return ret;
+		} else if (oct->chip_id == OCTEON_CN23XX_PF_VID) {
+			octnet_id_active(netdev, LED_IDENTIFICATION_ON);
+
+			/* returns 0 since updates are asynchronous */
+			return 0;
 		} else {
 			return -EINVAL;
 		}
@@ -521,7 +563,10 @@ static int lio_set_phys_id(struct net_device *netdev,
 						   &lio->phy_beacon_val);
 			if (ret)
 				return ret;
+		} else if (oct->chip_id == OCTEON_CN23XX_PF_VID) {
+			octnet_id_active(netdev, LED_IDENTIFICATION_OFF);
 
+			return 0;
 		} else {
 			return -EINVAL;
 		}
@@ -550,6 +595,13 @@ lio_ethtool_get_ringparam(struct net_device *netdev,
 		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);
+	} else if (OCTEON_CN23XX_PF(oct)) {
+		struct octeon_config *conf23 = CHIP_FIELD(oct, cn23xx_pf, conf);
+
+		tx_max_pending = CN23XX_MAX_IQ_DESCRIPTORS;
+		rx_max_pending = CN23XX_MAX_OQ_DESCRIPTORS;
+		rx_pending = CFG_GET_NUM_RX_DESCS_NIC_IF(conf23, lio->ifidx);
+		tx_pending = CFG_GET_NUM_TX_DESCS_NIC_IF(conf23, lio->ifidx);
 	}
 
 	if (lio->mtu > OCTNET_DEFAULT_FRM_SIZE - OCTNET_FRM_HEADER_SIZE) {
@@ -610,6 +662,69 @@ lio_get_pauseparam(struct net_device *netdev, struct ethtool_pauseparam *pause)
 	pause->rx_pause = oct->rx_pause;
 }
 
+static int
+lio_set_pauseparam(struct net_device *netdev, struct ethtool_pauseparam *pause)
+{
+	/* Notes: Not supporting any auto negotiation in these
+	 * drivers.
+	 */
+	struct lio *lio = GET_LIO(netdev);
+	struct octeon_device *oct = lio->oct_dev;
+	struct octnic_ctrl_pkt nctrl;
+	struct oct_link_info *linfo = &lio->linfo;
+
+	int ret = 0;
+
+	if (oct->chip_id != OCTEON_CN23XX_PF_VID)
+		return -EINVAL;
+
+	if (linfo->link.s.duplex == 0) {
+		/*no flow control for half duplex*/
+		if (pause->rx_pause || pause->tx_pause)
+			return -EINVAL;
+	}
+
+	/*do not support autoneg of link flow control*/
+	if (pause->autoneg == AUTONEG_ENABLE)
+		return -EINVAL;
+
+	memset(&nctrl, 0, sizeof(struct octnic_ctrl_pkt));
+
+	nctrl.ncmd.u64 = 0;
+	nctrl.ncmd.s.cmd = OCTNET_CMD_SET_FLOW_CTL;
+	nctrl.iq_no = lio->linfo.txpciq[0].s.q_no;
+	nctrl.wait_time = 100;
+	nctrl.netpndev = (u64)netdev;
+	nctrl.cb_fn = liquidio_link_ctrl_cmd_completion;
+
+	if (pause->rx_pause) {
+		/*enable rx pause*/
+		nctrl.ncmd.s.param1 = 1;
+	} else {
+		/*disable rx pause*/
+		nctrl.ncmd.s.param1 = 0;
+	}
+
+	if (pause->tx_pause) {
+		/*enable tx pause*/
+		nctrl.ncmd.s.param2 = 1;
+	} else {
+		/*disable tx pause*/
+		nctrl.ncmd.s.param2 = 0;
+	}
+
+	ret = octnet_send_nic_ctrl_pkt(lio->oct_dev, &nctrl);
+	if (ret < 0) {
+		dev_err(&oct->pci_dev->dev, "Failed to set pause parameter\n");
+		return -EINVAL;
+	}
+
+	oct->rx_pause = pause->rx_pause;
+	oct->tx_pause = pause->tx_pause;
+
+	return 0;
+}
+
 static void
 lio_get_ethtool_stats(struct net_device *netdev,
 		      struct ethtool_stats *stats  __attribute__((unused)),
@@ -877,6 +992,27 @@ lio_get_ethtool_stats(struct net_device *netdev,
 	}
 }
 
+static void lio_get_priv_flags_strings(struct lio *lio, u8 *data)
+{
+	struct octeon_device *oct_dev = lio->oct_dev;
+	int i;
+
+	switch (oct_dev->chip_id) {
+	case OCTEON_CN23XX_PF_VID:
+		for (i = 0; i < ARRAY_SIZE(oct_priv_flags_strings); i++) {
+			sprintf(data, "%s", oct_priv_flags_strings[i]);
+			data += ETH_GSTRING_LEN;
+		}
+		break;
+	case OCTEON_CN68XX:
+	case OCTEON_CN66XX:
+		break;
+	default:
+		netif_info(lio, drv, lio->netdev, "Unknown Chip !!\n");
+		break;
+	}
+}
+
 static void lio_get_strings(struct net_device *netdev, u32 stringset, u8 *data)
 {
 	struct lio *lio = GET_LIO(netdev);
@@ -916,12 +1052,31 @@ static void lio_get_strings(struct net_device *netdev, u32 stringset, u8 *data)
 		}
 		break;
 
+	case ETH_SS_PRIV_FLAGS:
+		lio_get_priv_flags_strings(lio, data);
+		break;
 	default:
 		netif_info(lio, drv, lio->netdev, "Unknown Stringset !!\n");
 		break;
 	}
 }
 
+static int lio_get_priv_flags_ss_count(struct lio *lio)
+{
+	struct octeon_device *oct_dev = lio->oct_dev;
+
+	switch (oct_dev->chip_id) {
+	case OCTEON_CN23XX_PF_VID:
+		return ARRAY_SIZE(oct_priv_flags_strings);
+	case OCTEON_CN68XX:
+	case OCTEON_CN66XX:
+		return -EOPNOTSUPP;
+	default:
+		netif_info(lio, drv, lio->netdev, "Unknown Chip !!\n");
+		return -EOPNOTSUPP;
+	}
+}
+
 static int lio_get_sset_count(struct net_device *netdev, int sset)
 {
 	struct lio *lio = GET_LIO(netdev);
@@ -932,6 +1087,8 @@ static int lio_get_sset_count(struct net_device *netdev, int sset)
 		return (ARRAY_SIZE(oct_stats_strings) +
 			ARRAY_SIZE(oct_iq_stats_strings) * oct_dev->num_iqs +
 			ARRAY_SIZE(oct_droq_stats_strings) * oct_dev->num_oqs);
+	case ETH_SS_PRIV_FLAGS:
+		return lio_get_priv_flags_ss_count(lio);
 	default:
 		return -EOPNOTSUPP;
 	}
@@ -948,6 +1105,16 @@ static int lio_get_intr_coalesce(struct net_device *netdev,
 	intrmod_cfg = &oct->intrmod;
 
 	switch (oct->chip_id) {
+	case OCTEON_CN23XX_PF_VID:
+		if (!intrmod_cfg->rx_enable) {
+			intr_coal->rx_coalesce_usecs = intrmod_cfg->rx_usecs;
+			intr_coal->rx_max_coalesced_frames =
+				intrmod_cfg->rx_frames;
+		}
+		if (!intrmod_cfg->tx_enable)
+			intr_coal->tx_max_coalesced_frames =
+				intrmod_cfg->tx_frames;
+		break;
 	case OCTEON_CN68XX:
 	case OCTEON_CN66XX: {
 		struct octeon_cn6xxx *cn6xxx =
@@ -983,7 +1150,15 @@ static int lio_get_intr_coalesce(struct net_device *netdev,
 		intr_coal->rx_coalesce_usecs_low =
 			intrmod_cfg->rx_mintmr_trigger;
 		intr_coal->rx_max_coalesced_frames_low =
-			intrmod_cfg->rx_mincnt_trigger;
+		    intrmod_cfg->rx_mincnt_trigger;
+	}
+	if (OCTEON_CN23XX_PF(oct) &&
+	    (intrmod_cfg->tx_enable)) {
+		intr_coal->use_adaptive_tx_coalesce = intrmod_cfg->tx_enable;
+		intr_coal->tx_max_coalesced_frames_high =
+		    intrmod_cfg->tx_maxcnt_trigger;
+		intr_coal->tx_max_coalesced_frames_low =
+		    intrmod_cfg->tx_mincnt_trigger;
 	}
 	return 0;
 }
@@ -1060,11 +1235,11 @@ static void
 octnet_nic_stats_callback(struct octeon_device *oct_dev,
 			  u32 status, void *ptr)
 {
-	struct octeon_soft_command  *sc = (struct octeon_soft_command  *)ptr;
-	struct oct_nic_stats_resp *resp = (struct oct_nic_stats_resp *)
-		sc->virtrptr;
-	struct oct_nic_stats_ctrl *ctrl = (struct oct_nic_stats_ctrl *)
-		sc->ctxptr;
+	struct octeon_soft_command *sc = (struct octeon_soft_command *)ptr;
+	struct oct_nic_stats_resp *resp =
+	    (struct oct_nic_stats_resp *)sc->virtrptr;
+	struct oct_nic_stats_ctrl *ctrl =
+	    (struct oct_nic_stats_ctrl *)sc->ctxptr;
 	struct nic_rx_stats *rsp_rstats = &resp->stats.fromwire;
 	struct nic_tx_stats *rsp_tstats = &resp->stats.fromhost;
 
@@ -1314,14 +1489,35 @@ oct_cfg_rx_intrcnt(struct lio *lio, struct ethtool_coalesce *intr_coal)
 		CFG_SET_OQ_INTR_PKT(cn6xxx->conf, rx_max_coalesced_frames);
 		break;
 	}
+	case OCTEON_CN23XX_PF_VID: {
+		int q_no;
+
+		if (!intr_coal->rx_max_coalesced_frames)
+			rx_max_coalesced_frames = oct->intrmod.rx_frames;
+		else
+			rx_max_coalesced_frames =
+			    intr_coal->rx_max_coalesced_frames;
+		for (q_no = 0; q_no < oct->num_oqs; q_no++) {
+			q_no += oct->sriov_info.pf_srn;
+			octeon_write_csr64(
+			    oct, CN23XX_SLI_OQ_PKT_INT_LEVELS(q_no),
+			    (octeon_read_csr64(
+				 oct, CN23XX_SLI_OQ_PKT_INT_LEVELS(q_no)) &
+			     (0x3fffff00000000UL)) |
+				rx_max_coalesced_frames);
+			/*consider setting resend bit*/
+		}
+		oct->intrmod.rx_frames = rx_max_coalesced_frames;
+		break;
+	}
 	default:
 		return -EINVAL;
 	}
 	return 0;
 }
 
-static int oct_cfg_rx_intrtime(struct lio *lio, struct ethtool_coalesce
-			       *intr_coal)
+static int oct_cfg_rx_intrtime(struct lio *lio,
+			       struct ethtool_coalesce *intr_coal)
 {
 	struct octeon_device *oct = lio->oct_dev;
 	u32 time_threshold, rx_coalesce_usecs;
@@ -1346,6 +1542,27 @@ static int oct_cfg_rx_intrtime(struct lio *lio, struct ethtool_coalesce
 		CFG_SET_OQ_INTR_TIME(cn6xxx->conf, rx_coalesce_usecs);
 		break;
 	}
+	case OCTEON_CN23XX_PF_VID: {
+		u64 time_threshold;
+		int q_no;
+
+		if (!intr_coal->rx_coalesce_usecs)
+			rx_coalesce_usecs = oct->intrmod.rx_usecs;
+		else
+			rx_coalesce_usecs = intr_coal->rx_coalesce_usecs;
+		time_threshold =
+		    cn23xx_pf_get_oq_ticks(oct, (u32)rx_coalesce_usecs);
+		for (q_no = 0; q_no < oct->num_oqs; q_no++) {
+			q_no += oct->sriov_info.pf_srn;
+			octeon_write_csr64(oct,
+					   CN23XX_SLI_OQ_PKT_INT_LEVELS(q_no),
+					   (oct->intrmod.rx_frames |
+					    (time_threshold << 32)));
+			/*consider writing to resend bit here*/
+		}
+		oct->intrmod.rx_usecs = rx_coalesce_usecs;
+		break;
+	}
 	default:
 		return -EINVAL;
 	}
@@ -1358,12 +1575,37 @@ oct_cfg_tx_intrcnt(struct lio *lio, struct ethtool_coalesce *intr_coal
 		   __attribute__((unused)))
 {
 	struct octeon_device *oct = lio->oct_dev;
+	u32 iq_intr_pkt;
+	void __iomem *inst_cnt_reg;
+	u64 val;
 
 	/* Config Cnt based interrupt values */
 	switch (oct->chip_id) {
 	case OCTEON_CN68XX:
 	case OCTEON_CN66XX:
 		break;
+	case OCTEON_CN23XX_PF_VID: {
+		int q_no;
+
+		if (!intr_coal->tx_max_coalesced_frames)
+			iq_intr_pkt = CN23XX_DEF_IQ_INTR_THRESHOLD &
+				      CN23XX_PKT_IN_DONE_WMARK_MASK;
+		else
+			iq_intr_pkt = intr_coal->tx_max_coalesced_frames &
+				      CN23XX_PKT_IN_DONE_WMARK_MASK;
+		for (q_no = 0; q_no < oct->num_iqs; q_no++) {
+			inst_cnt_reg = (oct->instr_queue[q_no])->inst_cnt_reg;
+			val = readq(inst_cnt_reg);
+			/*clear wmark and count.dont want to write count back*/
+			val = (val & 0xFFFF000000000000ULL) |
+			      ((u64)iq_intr_pkt
+			       << CN23XX_PKT_IN_DONE_WMARK_BIT_POS);
+			writeq(val, inst_cnt_reg);
+			/*consider setting resend bit*/
+		}
+		oct->intrmod.tx_frames = iq_intr_pkt;
+		break;
+	}
 	default:
 		return -EINVAL;
 	}
@@ -1399,6 +1641,8 @@ static int lio_set_intr_coalesce(struct net_device *netdev,
 			return -EINVAL;
 		}
 		break;
+	case OCTEON_CN23XX_PF_VID:
+		break;
 	default:
 		return -EINVAL;
 	}
@@ -1541,9 +1785,237 @@ static int lio_nway_reset(struct net_device *netdev)
 }
 
 /* Return register dump len. */
-static int lio_get_regs_len(struct net_device *dev __attribute__((unused)))
+static int lio_get_regs_len(struct net_device *dev)
 {
-	return OCT_ETHTOOL_REGDUMP_LEN;
+	struct lio *lio = GET_LIO(dev);
+	struct octeon_device *oct = lio->oct_dev;
+
+	switch (oct->chip_id) {
+	case OCTEON_CN23XX_PF_VID:
+		return OCT_ETHTOOL_REGDUMP_LEN_23XX;
+	default:
+		return OCT_ETHTOOL_REGDUMP_LEN;
+	}
+}
+
+static int cn23xx_read_csr_reg(char *s, struct octeon_device *oct)
+{
+	u32 reg;
+	u8 pf_num = oct->pf_num;
+	int len = 0;
+	int i;
+
+	/* PCI  Window Registers */
+
+	len += sprintf(s + len, "\n\t Octeon CSR Registers\n\n");
+
+	/*0x29030 or 0x29040*/
+	reg = CN23XX_SLI_PKT_MAC_RINFO64(oct->pcie_port, oct->pf_num);
+	len += sprintf(s + len,
+		       "\n[%08x] (SLI_PKT_MAC%d_PF%d_RINFO): %016llx\n",
+		       reg, oct->pcie_port, oct->pf_num,
+		       (u64)octeon_read_csr64(oct, reg));
+
+	/*0x27080 or 0x27090*/
+	reg = CN23XX_SLI_MAC_PF_INT_ENB64(oct->pcie_port, oct->pf_num);
+	len +=
+	    sprintf(s + len, "\n[%08x] (SLI_MAC%d_PF%d_INT_ENB): %016llx\n",
+		    reg, oct->pcie_port, oct->pf_num,
+		    (u64)octeon_read_csr64(oct, reg));
+
+	/*0x27000 or 0x27010*/
+	reg = CN23XX_SLI_MAC_PF_INT_SUM64(oct->pcie_port, oct->pf_num);
+	len +=
+	    sprintf(s + len, "\n[%08x] (SLI_MAC%d_PF%d_INT_SUM): %016llx\n",
+		    reg, oct->pcie_port, oct->pf_num,
+		    (u64)octeon_read_csr64(oct, reg));
+
+	/*0x29120*/
+	reg = 0x29120;
+	len += sprintf(s + len, "\n[%08x] (SLI_PKT_MEM_CTL): %016llx\n", reg,
+		       (u64)octeon_read_csr64(oct, reg));
+
+	/*0x27300*/
+	reg = 0x27300 + oct->pcie_port * CN23XX_MAC_INT_OFFSET +
+	      (oct->pf_num) * CN23XX_PF_INT_OFFSET;
+	len += sprintf(
+	    s + len, "\n[%08x] (SLI_MAC%d_PF%d_PKT_VF_INT): %016llx\n", reg,
+	    oct->pcie_port, oct->pf_num, (u64)octeon_read_csr64(oct, reg));
+
+	/*0x27200*/
+	reg = 0x27200 + oct->pcie_port * CN23XX_MAC_INT_OFFSET +
+	      (oct->pf_num) * CN23XX_PF_INT_OFFSET;
+	len += sprintf(s + len,
+		       "\n[%08x] (SLI_MAC%d_PF%d_PP_VF_INT): %016llx\n",
+		       reg, oct->pcie_port, oct->pf_num,
+		       (u64)octeon_read_csr64(oct, reg));
+
+	/*29130*/
+	reg = CN23XX_SLI_PKT_CNT_INT;
+	len += sprintf(s + len, "\n[%08x] (SLI_PKT_CNT_INT): %016llx\n", reg,
+		       (u64)octeon_read_csr64(oct, reg));
+
+	/*0x29140*/
+	reg = CN23XX_SLI_PKT_TIME_INT;
+	len += sprintf(s + len, "\n[%08x] (SLI_PKT_TIME_INT): %016llx\n", reg,
+		       (u64)octeon_read_csr64(oct, reg));
+
+	/*0x29160*/
+	reg = 0x29160;
+	len += sprintf(s + len, "\n[%08x] (SLI_PKT_INT): %016llx\n", reg,
+		       (u64)octeon_read_csr64(oct, reg));
+
+	/*0x29180*/
+	reg = CN23XX_SLI_OQ_WMARK;
+	len += sprintf(s + len, "\n[%08x] (SLI_PKT_OUTPUT_WMARK): %016llx\n",
+		       reg, (u64)octeon_read_csr64(oct, reg));
+
+	/*0x291E0*/
+	reg = CN23XX_SLI_PKT_IOQ_RING_RST;
+	len += sprintf(s + len, "\n[%08x] (SLI_PKT_RING_RST): %016llx\n", reg,
+		       (u64)octeon_read_csr64(oct, reg));
+
+	/*0x29210*/
+	reg = CN23XX_SLI_GBL_CONTROL;
+	len += sprintf(s + len,
+		       "\n[%08x] (SLI_PKT_GBL_CONTROL): %016llx\n", reg,
+		       (u64)octeon_read_csr64(oct, reg));
+
+	/*0x29220*/
+	reg = 0x29220;
+	len += sprintf(s + len, "\n[%08x] (SLI_PKT_BIST_STATUS): %016llx\n",
+		       reg, (u64)octeon_read_csr64(oct, reg));
+
+	/*PF only*/
+	if (pf_num == 0) {
+		/*0x29260*/
+		reg = CN23XX_SLI_OUT_BP_EN_W1S;
+		len += sprintf(s + len,
+			       "\n[%08x] (SLI_PKT_OUT_BP_EN_W1S):  %016llx\n",
+			       reg, (u64)octeon_read_csr64(oct, reg));
+	} else if (pf_num == 1) {
+		/*0x29270*/
+		reg = CN23XX_SLI_OUT_BP_EN2_W1S;
+		len += sprintf(s + len,
+			       "\n[%08x] (SLI_PKT_OUT_BP_EN2_W1S): %016llx\n",
+			       reg, (u64)octeon_read_csr64(oct, reg));
+	}
+
+	for (i = 0; i < CN23XX_MAX_OUTPUT_QUEUES; i++) {
+		reg = CN23XX_SLI_OQ_BUFF_INFO_SIZE(i);
+		len +=
+		    sprintf(s + len, "\n[%08x] (SLI_PKT%d_OUT_SIZE): %016llx\n",
+			    reg, i, (u64)octeon_read_csr64(oct, reg));
+	}
+
+	/*0x10040*/
+	for (i = 0; i < CN23XX_MAX_INPUT_QUEUES; i++) {
+		reg = CN23XX_SLI_IQ_INSTR_COUNT64(i);
+		len += sprintf(s + len,
+			       "\n[%08x] (SLI_PKT_IN_DONE%d_CNTS): %016llx\n",
+			       reg, i, (u64)octeon_read_csr64(oct, reg));
+	}
+
+	/*0x10080*/
+	for (i = 0; i < CN23XX_MAX_OUTPUT_QUEUES; i++) {
+		reg = CN23XX_SLI_OQ_PKTS_CREDIT(i);
+		len += sprintf(s + len,
+			       "\n[%08x] (SLI_PKT%d_SLIST_BAOFF_DBELL): %016llx\n",
+			       reg, i, (u64)octeon_read_csr64(oct, reg));
+	}
+
+	/*0x10090*/
+	for (i = 0; i < CN23XX_MAX_OUTPUT_QUEUES; i++) {
+		reg = CN23XX_SLI_OQ_SIZE(i);
+		len += sprintf(
+		    s + len, "\n[%08x] (SLI_PKT%d_SLIST_FIFO_RSIZE): %016llx\n",
+		    reg, i, (u64)octeon_read_csr64(oct, reg));
+	}
+
+	/*0x10050*/
+	for (i = 0; i < CN23XX_MAX_OUTPUT_QUEUES; i++) {
+		reg = CN23XX_SLI_OQ_PKT_CONTROL(i);
+		len += sprintf(
+			s + len,
+			"\n[%08x] (SLI_PKT%d__OUTPUT_CONTROL): %016llx\n",
+			reg, i, (u64)octeon_read_csr64(oct, reg));
+	}
+
+	/*0x10070*/
+	for (i = 0; i < CN23XX_MAX_OUTPUT_QUEUES; i++) {
+		reg = CN23XX_SLI_OQ_BASE_ADDR64(i);
+		len += sprintf(s + len,
+			       "\n[%08x] (SLI_PKT%d_SLIST_BADDR): %016llx\n",
+			       reg, i, (u64)octeon_read_csr64(oct, reg));
+	}
+
+	/*0x100a0*/
+	for (i = 0; i < CN23XX_MAX_OUTPUT_QUEUES; i++) {
+		reg = CN23XX_SLI_OQ_PKT_INT_LEVELS(i);
+		len += sprintf(s + len,
+			       "\n[%08x] (SLI_PKT%d_INT_LEVELS): %016llx\n",
+			       reg, i, (u64)octeon_read_csr64(oct, reg));
+	}
+
+	/*0x100b0*/
+	for (i = 0; i < CN23XX_MAX_OUTPUT_QUEUES; i++) {
+		reg = CN23XX_SLI_OQ_PKTS_SENT(i);
+		len += sprintf(s + len, "\n[%08x] (SLI_PKT%d_CNTS): %016llx\n",
+			       reg, i, (u64)octeon_read_csr64(oct, reg));
+	}
+
+	/*0x100c0*/
+	for (i = 0; i < CN23XX_MAX_OUTPUT_QUEUES; i++) {
+		reg = 0x100c0 + i * CN23XX_OQ_OFFSET;
+		len += sprintf(s + len,
+			       "\n[%08x] (SLI_PKT%d_ERROR_INFO): %016llx\n",
+			       reg, i, (u64)octeon_read_csr64(oct, reg));
+
+		/*0x10000*/
+		for (i = 0; i < CN23XX_MAX_INPUT_QUEUES; i++) {
+			reg = CN23XX_SLI_IQ_PKT_CONTROL64(i);
+			len += sprintf(
+				s + len,
+				"\n[%08x] (SLI_PKT%d_INPUT_CONTROL): %016llx\n",
+				reg, i, (u64)octeon_read_csr64(oct, reg));
+		}
+
+		/*0x10010*/
+		for (i = 0; i < CN23XX_MAX_INPUT_QUEUES; i++) {
+			reg = CN23XX_SLI_IQ_BASE_ADDR64(i);
+			len += sprintf(
+			    s + len,
+			    "\n[%08x] (SLI_PKT%d_INSTR_BADDR): %016llx\n", reg,
+			    i, (u64)octeon_read_csr64(oct, reg));
+		}
+
+		/*0x10020*/
+		for (i = 0; i < CN23XX_MAX_INPUT_QUEUES; i++) {
+			reg = CN23XX_SLI_IQ_DOORBELL(i);
+			len += sprintf(
+			    s + len,
+			    "\n[%08x] (SLI_PKT%d_INSTR_BAOFF_DBELL): %016llx\n",
+			    reg, i, (u64)octeon_read_csr64(oct, reg));
+		}
+
+		/*0x10030*/
+		for (i = 0; i < CN23XX_MAX_INPUT_QUEUES; i++) {
+			reg = CN23XX_SLI_IQ_SIZE(i);
+			len += sprintf(
+			    s + len,
+			    "\n[%08x] (SLI_PKT%d_INSTR_FIFO_RSIZE): %016llx\n",
+			    reg, i, (u64)octeon_read_csr64(oct, reg));
+		}
+
+		/*0x10040*/
+		for (i = 0; i < CN23XX_MAX_INPUT_QUEUES; i++)
+			reg = CN23XX_SLI_IQ_INSTR_COUNT64(i);
+		len += sprintf(s + len,
+			       "\n[%08x] (SLI_PKT_IN_DONE%d_CNTS): %016llx\n",
+			       reg, i, (u64)octeon_read_csr64(oct, reg));
+	}
+
+	return len;
 }
 
 static int cn6xxx_read_csr_reg(char *s, struct octeon_device *oct)
@@ -1688,6 +2160,10 @@ static void lio_get_regs(struct net_device *dev,
 	regs->version = OCT_ETHTOOL_REGSVER;
 
 	switch (oct->chip_id) {
+	case OCTEON_CN23XX_PF_VID:
+		memset(regbuf, 0, OCT_ETHTOOL_REGDUMP_LEN_23XX);
+		len += cn23xx_read_csr_reg(regbuf + len, oct);
+		break;
 	case OCTEON_CN68XX:
 	case OCTEON_CN66XX:
 		memset(regbuf, 0, OCT_ETHTOOL_REGDUMP_LEN);
@@ -1729,6 +2205,7 @@ static const struct ethtool_ops lio_ethtool_ops = {
 	.get_strings		= lio_get_strings,
 	.get_ethtool_stats	= lio_get_ethtool_stats,
 	.get_pauseparam		= lio_get_pauseparam,
+	.set_pauseparam		= lio_set_pauseparam,
 	.get_regs_len		= lio_get_regs_len,
 	.get_regs		= lio_get_regs,
 	.get_msglevel		= lio_get_msglevel,
diff --git a/drivers/net/ethernet/cavium/liquidio/lio_main.c b/drivers/net/ethernet/cavium/liquidio/lio_main.c
index 20d6942edf40..afc6f9dc8119 100644
--- a/drivers/net/ethernet/cavium/liquidio/lio_main.c
+++ b/drivers/net/ethernet/cavium/liquidio/lio_main.c
@@ -21,11 +21,10 @@
 **********************************************************************/
 #include <linux/version.h>
 #include <linux/pci.h>
-#include <linux/net_tstamp.h>
-#include <linux/if_vlan.h>
 #include <linux/firmware.h>
 #include <linux/ptp_clock_kernel.h>
 #include <net/vxlan.h>
+#include <linux/kthread.h>
 #include "liquidio_common.h"
 #include "octeon_droq.h"
 #include "octeon_iq.h"
@@ -37,6 +36,7 @@
 #include "cn66xx_regs.h"
 #include "cn66xx_device.h"
 #include "cn68xx_device.h"
+#include "cn23xx_pf_device.h"
 #include "liquidio_image.h"
 
 MODULE_AUTHOR("Cavium Networks, <support@cavium.com>");
@@ -52,11 +52,6 @@ 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)
 
 #define INCR_INSTRQUEUE_PKT_COUNT(octeon_dev_ptr, iq_no, field, count)  \
@@ -102,6 +97,14 @@ struct liquidio_if_cfg_resp {
 	u64 status;
 };
 
+struct liquidio_rx_ctl_context {
+	int octeon_id;
+
+	wait_queue_head_t wc;
+
+	int cond;
+};
+
 struct oct_link_status_resp {
 	u64 rh;
 	struct oct_link_info link_info;
@@ -139,7 +142,8 @@ union tx_info {
 #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)
+#define OCTNIC_GSO_MAX_SIZE                                                    \
+	(CN23XX_DEFAULT_INPUT_JABBER - OCTNIC_GSO_MAX_HEADER_SIZE)
 
 /** Structure of a node in list of gather components maintained by
  * NIC driver for each network device.
@@ -162,27 +166,6 @@ struct octnic_gather {
 	u64 sg_dma_ptr;
 };
 
-/** 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;
@@ -198,6 +181,7 @@ struct octeon_device_priv {
 };
 
 static int octeon_device_init(struct octeon_device *);
+static int liquidio_stop(struct net_device *netdev);
 static void liquidio_remove(struct pci_dev *pdev);
 static int liquidio_probe(struct pci_dev *pdev,
 			  const struct pci_device_id *ent);
@@ -219,6 +203,20 @@ static void octeon_droq_bh(unsigned long pdev)
 			continue;
 		reschedule |= octeon_droq_process_packets(oct, oct->droq[q_no],
 							  MAX_PACKET_BUDGET);
+		lio_enable_irq(oct->droq[q_no], NULL);
+
+		if (OCTEON_CN23XX_PF(oct) && oct->msix_on) {
+			/* set time and cnt interrupt thresholds for this DROQ
+			 * for NAPI
+			 */
+			int adjusted_q_no = q_no + oct->sriov_info.pf_srn;
+
+			octeon_write_csr64(
+			    oct, CN23XX_SLI_OQ_PKT_INT_LEVELS(adjusted_q_no),
+			    0x5700000040ULL);
+			octeon_write_csr64(
+			    oct, CN23XX_SLI_OQ_PKTS_SENT(adjusted_q_no), 0);
+		}
 	}
 
 	if (reschedule)
@@ -252,76 +250,6 @@ static int lio_wait_for_oq_pkts(struct octeon_device *oct)
 	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
@@ -441,7 +369,7 @@ static void stop_pci_io(struct octeon_device *oct)
 	pci_disable_device(oct->pci_dev);
 
 	/* Disable interrupts  */
-	oct->fn_list.disable_interrupt(oct->chip);
+	oct->fn_list.disable_interrupt(oct, OCTEON_ALL_INTR);
 
 	pcierror_quiesce_device(oct);
 
@@ -570,6 +498,9 @@ static const struct pci_device_id liquidio_pci_tbl[] = {
 	{       /* 66xx */
 		PCI_VENDOR_ID_CAVIUM, 0x92, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0
 	},
+	{       /* 23xx pf */
+		PCI_VENDOR_ID_CAVIUM, 0x9702, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0
+	},
 	{
 		0, 0, 0, 0, 0, 0, 0
 	}
@@ -587,7 +518,6 @@ static struct pci_driver liquidio_pci_driver = {
 	.suspend	= liquidio_suspend,
 	.resume		= liquidio_resume,
 #endif
-
 };
 
 /**
@@ -936,6 +866,52 @@ static void print_link_info(struct net_device *netdev)
 }
 
 /**
+ * \brief Routine to notify MTU change
+ * @param work work_struct data structure
+ */
+static void octnet_link_status_change(struct work_struct *work)
+{
+	struct cavium_wk *wk = (struct cavium_wk *)work;
+	struct lio *lio = (struct lio *)wk->ctxptr;
+
+	rtnl_lock();
+	call_netdevice_notifiers(NETDEV_CHANGEMTU, lio->netdev);
+	rtnl_unlock();
+}
+
+/**
+ * \brief Sets up the mtu status change work
+ * @param netdev network device
+ */
+static inline int setup_link_status_change_wq(struct net_device *netdev)
+{
+	struct lio *lio = GET_LIO(netdev);
+	struct octeon_device *oct = lio->oct_dev;
+
+	lio->link_status_wq.wq = alloc_workqueue("link-status",
+						 WQ_MEM_RECLAIM, 0);
+	if (!lio->link_status_wq.wq) {
+		dev_err(&oct->pci_dev->dev, "unable to create cavium link status wq\n");
+		return -1;
+	}
+	INIT_DELAYED_WORK(&lio->link_status_wq.wk.work,
+			  octnet_link_status_change);
+	lio->link_status_wq.wk.ctxptr = lio;
+
+	return 0;
+}
+
+static inline void cleanup_link_status_change_wq(struct net_device *netdev)
+{
+	struct lio *lio = GET_LIO(netdev);
+
+	if (lio->link_status_wq.wq) {
+		cancel_delayed_work_sync(&lio->link_status_wq.wk.work);
+		destroy_workqueue(lio->link_status_wq.wq);
+	}
+}
+
+/**
  * \brief Update link status
  * @param netdev network device
  * @param ls link status structure
@@ -973,8 +949,6 @@ static void update_txq_status(struct octeon_device *oct, int iq_num)
 	struct lio *lio;
 	struct octeon_instr_queue *iq = oct->instr_queue[iq_num];
 
-	/*octeon_update_iq_read_idx(oct, iq);*/
-
 	netdev = oct->props[iq->ifidx].netdev;
 
 	/* This is needed because the first IQ does not have
@@ -1002,12 +976,32 @@ static void update_txq_status(struct octeon_device *oct, int iq_num)
 	}
 }
 
+static
+int liquidio_schedule_msix_droq_pkt_handler(struct octeon_droq *droq, u64 ret)
+{
+	struct octeon_device *oct = droq->oct_dev;
+	struct octeon_device_priv *oct_priv =
+	    (struct octeon_device_priv *)oct->priv;
+
+	if (droq->ops.poll_mode) {
+		droq->ops.napi_fn(droq);
+	} else {
+		if (ret & MSIX_PO_INT) {
+			tasklet_schedule(&oct_priv->droq_tasklet);
+			return 1;
+		}
+		/* this will be flushed periodically by check iq db */
+		if (ret & MSIX_PI_INT)
+			return 0;
+	}
+	return 0;
+}
+
 /**
  * \brief Droq packet processor sceduler
  * @param oct octeon device
  */
-static
-void liquidio_schedule_droq_pkt_handlers(struct octeon_device *oct)
+static void liquidio_schedule_droq_pkt_handlers(struct octeon_device *oct)
 {
 	struct octeon_device_priv *oct_priv =
 		(struct octeon_device_priv *)oct->priv;
@@ -1032,19 +1026,36 @@ void liquidio_schedule_droq_pkt_handlers(struct octeon_device *oct)
 	}
 }
 
+static irqreturn_t
+liquidio_msix_intr_handler(int irq __attribute__((unused)), void *dev)
+{
+	u64 ret;
+	struct octeon_ioq_vector *ioq_vector = (struct octeon_ioq_vector *)dev;
+	struct octeon_device *oct = ioq_vector->oct_dev;
+	struct octeon_droq *droq = oct->droq[ioq_vector->droq_index];
+
+	ret = oct->fn_list.msix_interrupt_handler(ioq_vector);
+
+	if ((ret & MSIX_PO_INT) || (ret & MSIX_PI_INT))
+		liquidio_schedule_msix_droq_pkt_handler(droq, ret);
+
+	return IRQ_HANDLED;
+}
+
 /**
  * \brief Interrupt handler for octeon
  * @param irq unused
  * @param dev octeon device
  */
 static
-irqreturn_t liquidio_intr_handler(int irq __attribute__((unused)), void *dev)
+irqreturn_t liquidio_legacy_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);
+	oct->fn_list.disable_interrupt(oct, OCTEON_ALL_INTR);
 
 	ret = oct->fn_list.process_interrupt_regs(oct);
 
@@ -1053,7 +1064,7 @@ irqreturn_t liquidio_intr_handler(int irq __attribute__((unused)), void *dev)
 
 	/* Re-enable our interrupts  */
 	if (!(atomic_read(&oct->status) == OCT_DEV_IN_RESET))
-		oct->fn_list.enable_interrupt(oct->chip);
+		oct->fn_list.enable_interrupt(oct, OCTEON_ALL_INTR);
 
 	return ret;
 }
@@ -1067,22 +1078,204 @@ irqreturn_t liquidio_intr_handler(int irq __attribute__((unused)), void *dev)
 static int octeon_setup_interrupt(struct octeon_device *oct)
 {
 	int irqret, err;
+	struct msix_entry *msix_entries;
+	int i;
+	int num_ioq_vectors;
+	int num_alloc_ioq_vectors;
 
-	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;
+	if (OCTEON_CN23XX_PF(oct) && oct->msix_on) {
+		oct->num_msix_irqs = oct->sriov_info.num_pf_rings;
+		/* one non ioq interrupt for handling sli_mac_pf_int_sum */
+		oct->num_msix_irqs += 1;
+
+		oct->msix_entries = kcalloc(
+		    oct->num_msix_irqs, sizeof(struct msix_entry), GFP_KERNEL);
+		if (!oct->msix_entries)
+			return 1;
+
+		msix_entries = (struct msix_entry *)oct->msix_entries;
+		/*Assumption is that pf msix vectors start from pf srn to pf to
+		 * trs and not from 0. if not change this code
+		 */
+		for (i = 0; i < oct->num_msix_irqs - 1; i++)
+			msix_entries[i].entry = oct->sriov_info.pf_srn + i;
+		msix_entries[oct->num_msix_irqs - 1].entry =
+		    oct->sriov_info.trs;
+		num_alloc_ioq_vectors = pci_enable_msix_range(
+						oct->pci_dev, msix_entries,
+						oct->num_msix_irqs,
+						oct->num_msix_irqs);
+		if (num_alloc_ioq_vectors < 0) {
+			dev_err(&oct->pci_dev->dev, "unable to Allocate MSI-X interrupts\n");
+			kfree(oct->msix_entries);
+			oct->msix_entries = NULL;
+			return 1;
+		}
+		dev_dbg(&oct->pci_dev->dev, "OCTEON: Enough MSI-X interrupts are allocated...\n");
+
+		num_ioq_vectors = oct->num_msix_irqs;
+
+		/** For PF, there is one non-ioq interrupt handler */
+		num_ioq_vectors -= 1;
+		irqret = request_irq(msix_entries[num_ioq_vectors].vector,
+				     liquidio_legacy_intr_handler, 0, "octeon",
+				     oct);
+		if (irqret) {
+			dev_err(&oct->pci_dev->dev,
+				"OCTEON: Request_irq failed for MSIX interrupt Error: %d\n",
+				irqret);
+			pci_disable_msix(oct->pci_dev);
+			kfree(oct->msix_entries);
+			oct->msix_entries = NULL;
+			return 1;
+		}
+
+		for (i = 0; i < num_ioq_vectors; i++) {
+			irqret = request_irq(msix_entries[i].vector,
+					     liquidio_msix_intr_handler, 0,
+					     "octeon", &oct->ioq_vector[i]);
+			if (irqret) {
+				dev_err(&oct->pci_dev->dev,
+					"OCTEON: Request_irq failed for MSIX interrupt Error: %d\n",
+					irqret);
+				/** Freeing the non-ioq irq vector here . */
+				free_irq(msix_entries[num_ioq_vectors].vector,
+					 oct);
+
+				while (i) {
+					i--;
+					/** clearing affinity mask. */
+					irq_set_affinity_hint(
+						msix_entries[i].vector, NULL);
+					free_irq(msix_entries[i].vector,
+						 &oct->ioq_vector[i]);
+				}
+				pci_disable_msix(oct->pci_dev);
+				kfree(oct->msix_entries);
+				oct->msix_entries = NULL;
+				return 1;
+			}
+			oct->ioq_vector[i].vector = msix_entries[i].vector;
+			/* assign the cpu mask for this msix interrupt vector */
+			irq_set_affinity_hint(
+					msix_entries[i].vector,
+					(&oct->ioq_vector[i].affinity_mask));
+		}
+		dev_dbg(&oct->pci_dev->dev, "OCTEON[%d]: MSI-X enabled\n",
+			oct->octeon_id);
+	} else {
+		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_legacy_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;
+}
+
+static int liquidio_watchdog(void *param)
+{
+	u64 wdog;
+	u16 mask_of_stuck_cores = 0;
+	u16 mask_of_crashed_cores = 0;
+	int core_num;
+	u8 core_is_stuck[LIO_MAX_CORES];
+	u8 core_crashed[LIO_MAX_CORES];
+	struct octeon_device *oct = param;
+
+	memset(core_is_stuck, 0, sizeof(core_is_stuck));
+	memset(core_crashed, 0, sizeof(core_crashed));
+
+	while (!kthread_should_stop()) {
+		mask_of_crashed_cores =
+		    (u16)octeon_read_csr64(oct, CN23XX_SLI_SCRATCH2);
+
+		for (core_num = 0; core_num < LIO_MAX_CORES; core_num++) {
+			if (!core_is_stuck[core_num]) {
+				wdog = lio_pci_readq(oct, CIU3_WDOG(core_num));
+
+				/* look at watchdog state field */
+				wdog &= CIU3_WDOG_MASK;
+				if (wdog) {
+					/* this watchdog timer has expired */
+					core_is_stuck[core_num] =
+						LIO_MONITOR_WDOG_EXPIRE;
+					mask_of_stuck_cores |= (1 << core_num);
+				}
+			}
+
+			if (!core_crashed[core_num])
+				core_crashed[core_num] =
+				    (mask_of_crashed_cores >> core_num) & 1;
+		}
+
+		if (mask_of_stuck_cores) {
+			for (core_num = 0; core_num < LIO_MAX_CORES;
+			     core_num++) {
+				if (core_is_stuck[core_num] == 1) {
+					dev_err(&oct->pci_dev->dev,
+						"ERROR: Octeon core %d is stuck!\n",
+						core_num);
+					/* 2 means we have printk'd  an error
+					 * so no need to repeat the same printk
+					 */
+					core_is_stuck[core_num] =
+						LIO_MONITOR_CORE_STUCK_MSGD;
+				}
+			}
+		}
+
+		if (mask_of_crashed_cores) {
+			for (core_num = 0; core_num < LIO_MAX_CORES;
+			     core_num++) {
+				if (core_crashed[core_num] == 1) {
+					dev_err(&oct->pci_dev->dev,
+						"ERROR: Octeon core %d crashed!  See oct-fwdump for details.\n",
+						core_num);
+					/* 2 means we have printk'd  an error
+					 * so no need to repeat the same printk
+					 */
+					core_crashed[core_num] =
+						LIO_MONITOR_CORE_STUCK_MSGD;
+				}
+			}
+		}
+#ifdef CONFIG_MODULE_UNLOAD
+		if (mask_of_stuck_cores || mask_of_crashed_cores) {
+			/* make module refcount=0 so that rmmod will work */
+			long refcount;
+
+			refcount = module_refcount(THIS_MODULE);
+
+			while (refcount > 0) {
+				module_put(THIS_MODULE);
+				refcount = module_refcount(THIS_MODULE);
+			}
+
+			/* compensate for and withstand an unlikely (but still
+			 * possible) race condition
+			 */
+			while (refcount < 0) {
+				try_module_get(THIS_MODULE);
+				refcount = module_refcount(THIS_MODULE);
+			}
+		}
+#endif
+		/* sleep for two seconds */
+		set_current_state(TASK_INTERRUPTIBLE);
+		schedule_timeout(2 * HZ);
 	}
 
 	return 0;
@@ -1107,6 +1300,9 @@ liquidio_probe(struct pci_dev *pdev,
 		return -ENOMEM;
 	}
 
+	if (pdev->device == OCTEON_CN23XX_PF_VID)
+		oct_dev->msix_on = LIO_FLAG_MSIX_ENABLED;
+
 	dev_info(&pdev->dev, "Initializing device %x:%x.\n",
 		 (u32)pdev->vendor, (u32)pdev->device);
 
@@ -1130,6 +1326,30 @@ liquidio_probe(struct pci_dev *pdev,
 		return -ENOMEM;
 	}
 
+	if (OCTEON_CN23XX_PF(oct_dev)) {
+		u64 scratch1;
+		u8 bus, device, function;
+
+		scratch1 = octeon_read_csr64(oct_dev, CN23XX_SLI_SCRATCH1);
+		if (!(scratch1 & 4ULL)) {
+			/* Bit 2 of SLI_SCRATCH_1 is a flag that indicates that
+			 * the lio watchdog kernel thread is running for this
+			 * NIC.  Each NIC gets one watchdog kernel thread.
+			 */
+			scratch1 |= 4ULL;
+			octeon_write_csr64(oct_dev, CN23XX_SLI_SCRATCH1,
+					   scratch1);
+
+			bus = pdev->bus->number;
+			device = PCI_SLOT(pdev->devfn);
+			function = PCI_FUNC(pdev->devfn);
+			oct_dev->watchdog_task = kthread_create(
+			    liquidio_watchdog, oct_dev,
+			    "liowd/%02hhx:%02hhx.%hhx", bus, device, function);
+			wake_up_process(oct_dev->watchdog_task);
+		}
+	}
+
 	oct_dev->rx_pause = 1;
 	oct_dev->tx_pause = 1;
 
@@ -1146,6 +1366,7 @@ liquidio_probe(struct pci_dev *pdev,
 static void octeon_destroy_resources(struct octeon_device *oct)
 {
 	int i;
+	struct msix_entry *msix_entries;
 	struct octeon_device_priv *oct_priv =
 		(struct octeon_device_priv *)oct->priv;
 
@@ -1190,21 +1411,40 @@ static void octeon_destroy_resources(struct octeon_device *oct)
 			dev_err(&oct->pci_dev->dev, "OQ had pending packets\n");
 
 		/* Disable interrupts  */
-		oct->fn_list.disable_interrupt(oct->chip);
+		oct->fn_list.disable_interrupt(oct, OCTEON_ALL_INTR);
+
+		if (oct->msix_on) {
+			msix_entries = (struct msix_entry *)oct->msix_entries;
+			for (i = 0; i < oct->num_msix_irqs - 1; i++) {
+				/* clear the affinity_cpumask */
+				irq_set_affinity_hint(msix_entries[i].vector,
+						      NULL);
+				free_irq(msix_entries[i].vector,
+					 &oct->ioq_vector[i]);
+			}
+			/* non-iov vector's argument is oct struct */
+			free_irq(msix_entries[i].vector, oct);
 
-		/* Release the interrupt line */
-		free_irq(oct->pci_dev->irq, oct);
+			pci_disable_msix(oct->pci_dev);
+			kfree(oct->msix_entries);
+			oct->msix_entries = NULL;
+		} else {
+			/* Release the interrupt line */
+			free_irq(oct->pci_dev->irq, oct);
 
-		if (oct->flags & LIO_FLAG_MSI_ENABLED)
-			pci_disable_msi(oct->pci_dev);
+			if (oct->flags & LIO_FLAG_MSI_ENABLED)
+				pci_disable_msi(oct->pci_dev);
+		}
 
-		/* fallthrough */
+		if (OCTEON_CN23XX_PF(oct))
+			octeon_free_ioq_vector(oct);
+	/* 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(oct); i++) {
-			if (!(oct->io_qmask.oq & (1ULL << i)))
+			if (!(oct->io_qmask.oq & BIT_ULL(i)))
 				continue;
 			octeon_delete_droq(oct, i);
 		}
@@ -1226,16 +1466,15 @@ static void octeon_destroy_resources(struct octeon_device *oct)
 		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(oct); i++) {
-			if (!(oct->io_qmask.iq & (1ULL << i)))
+			if (!(oct->io_qmask.iq & BIT_ULL(i)))
 				continue;
 			octeon_delete_instr_queue(oct, i);
 		}
+		/* fallthrough */
+	case OCT_DEV_SC_BUFF_POOL_INIT_DONE:
+		octeon_free_sc_buffer_pool(oct);
 
 		/* fallthrough */
 	case OCT_DEV_DISPATCH_INIT_DONE:
@@ -1244,9 +1483,9 @@ static void octeon_destroy_resources(struct octeon_device *oct)
 
 		/* fallthrough */
 	case OCT_DEV_PCI_MAP_DONE:
-
 		/* Soft reset the octeon device before exiting */
-		oct->fn_list.soft_reset(oct);
+		if ((!OCTEON_CN23XX_PF(oct)) || !oct->octeon_id)
+			oct->fn_list.soft_reset(oct);
 
 		octeon_unmap_pci_barx(oct, 0);
 		octeon_unmap_pci_barx(oct, 1);
@@ -1264,23 +1503,89 @@ static void octeon_destroy_resources(struct octeon_device *oct)
 }
 
 /**
+ * \brief Callback for rx ctrl
+ * @param status status of request
+ * @param buf pointer to resp structure
+ */
+static void rx_ctl_callback(struct octeon_device *oct,
+			    u32 status,
+			    void *buf)
+{
+	struct octeon_soft_command *sc = (struct octeon_soft_command *)buf;
+	struct liquidio_rx_ctl_context *ctx;
+
+	ctx  = (struct liquidio_rx_ctl_context *)sc->ctxptr;
+
+	oct = lio_get_device(ctx->octeon_id);
+	if (status)
+		dev_err(&oct->pci_dev->dev, "rx ctl instruction failed. Status: %llx\n",
+			CVM_CAST64(status));
+	WRITE_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 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 octeon_soft_command *sc;
+	struct liquidio_rx_ctl_context *ctx;
+	union octnet_cmd *ncmd;
+	int ctx_size = sizeof(struct liquidio_rx_ctl_context);
+	struct octeon_device *oct = (struct octeon_device *)lio->oct_dev;
+	int retval;
 
-	memset(&nctrl, 0, sizeof(struct octnic_ctrl_pkt));
+	if (oct->props[lio->ifidx].rx_on == start_stop)
+		return;
 
-	nctrl.ncmd.s.cmd = OCTNET_CMD_RX_CTL;
-	nctrl.ncmd.s.param1 = start_stop;
-	nctrl.iq_no = lio->linfo.txpciq[0].s.q_no;
-	nctrl.netpndev = (u64)lio->netdev;
+	sc = (struct octeon_soft_command *)
+		octeon_alloc_soft_command(oct, OCTNET_CMD_SIZE,
+					  16, ctx_size);
 
-	if (octnet_send_nic_ctrl_pkt(lio->oct_dev, &nctrl) < 0)
+	ncmd = (union octnet_cmd *)sc->virtdptr;
+	ctx  = (struct liquidio_rx_ctl_context *)sc->ctxptr;
+
+	WRITE_ONCE(ctx->cond, 0);
+	ctx->octeon_id = lio_get_device_id(oct);
+	init_waitqueue_head(&ctx->wc);
+
+	ncmd->u64 = 0;
+	ncmd->s.cmd = OCTNET_CMD_RX_CTL;
+	ncmd->s.param1 = start_stop;
+
+	octeon_swap_8B_data((u64 *)ncmd, (OCTNET_CMD_SIZE >> 3));
+
+	sc->iq_no = lio->linfo.txpciq[0].s.q_no;
+
+	octeon_prepare_soft_command(oct, sc, OPCODE_NIC,
+				    OPCODE_NIC_CMD, 0, 0, 0);
+
+	sc->callback = rx_ctl_callback;
+	sc->callback_arg = sc;
+	sc->wait_time = 5000;
+
+	retval = octeon_send_soft_command(oct, sc);
+	if (retval == IQ_SEND_FAILED) {
 		netif_info(lio, rx_err, lio->netdev, "Failed to send RX Control message\n");
+	} else {
+		/* Sleep on a wait queue till the cond flag indicates that the
+		 * response arrived or timed-out.
+		 */
+		if (sleep_cond(&ctx->wc, &ctx->cond) == -EINTR)
+			return;
+		oct->props[lio->ifidx].rx_on = start_stop;
+	}
+
+	octeon_free_soft_command(oct, sc);
 }
 
 /**
@@ -1307,21 +1612,24 @@ static void liquidio_destroy_nic_device(struct octeon_device *oct, int ifidx)
 
 	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);
+		liquidio_stop(netdev);
 
 	if (oct->props[lio->ifidx].napi_enabled == 1) {
 		list_for_each_entry_safe(napi, n, &netdev->napi_list, dev_list)
 			napi_disable(napi);
 
 		oct->props[lio->ifidx].napi_enabled = 0;
+
+		if (OCTEON_CN23XX_PF(oct))
+			oct->droq[0]->ops.poll_mode = 0;
 	}
 
 	if (atomic_read(&lio->ifstate) & LIO_IFSTATE_REGISTERED)
 		unregister_netdev(netdev);
 
+	cleanup_link_status_change_wq(netdev);
+
 	delete_glists(lio);
 
 	free_netdev(netdev);
@@ -1374,6 +1682,9 @@ static void liquidio_remove(struct pci_dev *pdev)
 
 	dev_dbg(&oct_dev->pci_dev->dev, "Stopping device\n");
 
+	if (oct_dev->watchdog_task)
+		kthread_stop(oct_dev->watchdog_task);
+
 	if (oct_dev->app_mode && (oct_dev->app_mode == CVM_DRV_NIC_APP))
 		liquidio_stop_nic_module(oct_dev);
 
@@ -1417,6 +1728,12 @@ static int octeon_chip_specific_setup(struct octeon_device *oct)
 		s = "CN66XX";
 		break;
 
+	case OCTEON_CN23XX_PCIID_PF:
+		oct->chip_id = OCTEON_CN23XX_PF_VID;
+		ret = setup_cn23xx_octeon_pf_device(oct);
+		s = "CN23XX";
+		break;
+
 	default:
 		s = "?";
 		dev_err(&oct->pci_dev->dev, "Unknown device found (dev_id: %x)\n",
@@ -1867,7 +2184,7 @@ static void if_cfg_callback(struct octeon_device *oct,
 	struct liquidio_if_cfg_context *ctx;
 
 	resp = (struct liquidio_if_cfg_resp *)sc->virtrptr;
-	ctx  = (struct liquidio_if_cfg_context *)sc->ctxptr;
+	ctx = (struct liquidio_if_cfg_context *)sc->ctxptr;
 
 	oct = lio_get_device(ctx->octeon_id);
 	if (resp->status)
@@ -2060,11 +2377,14 @@ static void napi_schedule_wrapper(void *param)
  */
 static void liquidio_napi_drv_callback(void *arg)
 {
+	struct octeon_device *oct;
 	struct octeon_droq *droq = arg;
 	int this_cpu = smp_processor_id();
 
-	if (droq->cpu_id == this_cpu) {
-		napi_schedule(&droq->napi);
+	oct = droq->oct_dev;
+
+	if (OCTEON_CN23XX_PF(oct) || droq->cpu_id == this_cpu) {
+		napi_schedule_irqoff(&droq->napi);
 	} else {
 		struct call_single_data *csd = &droq->csd;
 
@@ -2173,17 +2493,15 @@ static inline int setup_io_queues(struct octeon_device *octeon_dev,
 						   lio->ifidx), NULL);
 		if (retval) {
 			dev_err(&octeon_dev->pci_dev->dev,
-				" %s : Runtime DROQ(RxQ) creation failed.\n",
+				"%s : Runtime DROQ(RxQ) creation failed.\n",
 				__func__);
 			return 1;
 		}
 
 		droq = octeon_dev->droq[q_no];
 		napi = &droq->napi;
-		dev_dbg(&octeon_dev->pci_dev->dev,
-			"netif_napi_add netdev:%llx oct:%llx\n",
-			(u64)netdev,
-			(u64)octeon_dev);
+		dev_dbg(&octeon_dev->pci_dev->dev, "netif_napi_add netdev:%llx oct:%llx pf_num:%d\n",
+			(u64)netdev, (u64)octeon_dev, octeon_dev->pf_num);
 		netif_napi_add(netdev, napi, liquidio_napi_poll, 64);
 
 		/* designate a CPU for this droq */
@@ -2195,6 +2513,14 @@ static inline int setup_io_queues(struct octeon_device *octeon_dev,
 		octeon_register_droq_ops(octeon_dev, q_no, &droq_ops);
 	}
 
+	if (OCTEON_CN23XX_PF(octeon_dev)) {
+		/* 23XX PF can receive control messages (via the first PF-owned
+		 * droq) from the firmware even if the ethX interface is down,
+		 * so that's why poll_mode must be off for the first droq.
+		 */
+		octeon_dev->droq[0]->ops.poll_mode = 0;
+	}
+
 	/* 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
@@ -2235,7 +2561,7 @@ static void octnet_poll_check_txq_status(struct work_struct *work)
  * \brief Sets up the txq poll check
  * @param netdev network device
  */
-static inline void setup_tx_poll_fn(struct net_device *netdev)
+static inline int setup_tx_poll_fn(struct net_device *netdev)
 {
 	struct lio *lio = GET_LIO(netdev);
 	struct octeon_device *oct = lio->oct_dev;
@@ -2244,21 +2570,24 @@ static inline void setup_tx_poll_fn(struct net_device *netdev)
 						WQ_MEM_RECLAIM, 0);
 	if (!lio->txq_status_wq.wq) {
 		dev_err(&oct->pci_dev->dev, "unable to create cavium txq status wq\n");
-		return;
+		return -1;
 	}
 	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));
+	return 0;
 }
 
 static inline void cleanup_tx_poll_fn(struct net_device *netdev)
 {
 	struct lio *lio = GET_LIO(netdev);
 
-	cancel_delayed_work_sync(&lio->txq_status_wq.wk.work);
-	destroy_workqueue(lio->txq_status_wq.wq);
+	if (lio->txq_status_wq.wq) {
+		cancel_delayed_work_sync(&lio->txq_status_wq.wk.work);
+		destroy_workqueue(lio->txq_status_wq.wq);
+	}
 }
 
 /**
@@ -2276,24 +2605,34 @@ static int liquidio_open(struct net_device *netdev)
 			napi_enable(napi);
 
 		oct->props[lio->ifidx].napi_enabled = 1;
+
+		if (OCTEON_CN23XX_PF(oct))
+			oct->droq[0]->ops.poll_mode = 1;
 	}
 
 	oct_ptp_open(netdev);
 
 	ifstate_set(lio, LIO_IFSTATE_RUNNING);
 
-	setup_tx_poll_fn(netdev);
-
-	start_txq(netdev);
+	/* Ready for link status updates */
+	lio->intf_open = 1;
 
 	netif_info(lio, ifup, lio->netdev, "Interface Open, ready for traffic\n");
 
+	if (OCTEON_CN23XX_PF(oct)) {
+		if (!oct->msix_on)
+			if (setup_tx_poll_fn(netdev))
+				return -1;
+	} else {
+		if (setup_tx_poll_fn(netdev))
+			return -1;
+	}
+
+	start_txq(netdev);
+
 	/* 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);
 
@@ -2328,7 +2667,12 @@ static int liquidio_stop(struct net_device *netdev)
 	/* Now it should be safe to tell Octeon that nic interface is down. */
 	send_rx_ctrl_cmd(lio, 0);
 
-	cleanup_tx_poll_fn(netdev);
+	if (OCTEON_CN23XX_PF(oct)) {
+		if (!oct->msix_on)
+			cleanup_tx_poll_fn(netdev);
+	} else {
+		cleanup_tx_poll_fn(netdev);
+	}
 
 	if (lio->ptp_clock) {
 		ptp_clock_unregister(lio->ptp_clock);
@@ -2340,143 +2684,6 @@ static int liquidio_stop(struct net_device *netdev)
 	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;
-	u8 *mac;
-
-	switch (nctrl->ncmd.s.cmd) {
-	case OCTNET_CMD_CHANGE_DEVFLAGS:
-	case OCTNET_CMD_SET_MULTI_LIST:
-		break;
-
-	case OCTNET_CMD_CHANGE_MACADDR:
-		mac = ((u8 *)&nctrl->udd[0]) + 2;
-		netif_info(lio, probe, lio->netdev,
-			   "%s %2.2x:%2.2x:%2.2x:%2.2x:%2.2x:%2.2x\n",
-			   "MACAddr changed to", mac[0], mac[1],
-			   mac[2], mac[3], mac[4], mac[5]);
-		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.param1);
-		dev_info(&oct->pci_dev->dev, "%s MTU Changed from %d to %d\n",
-			 netdev->name, netdev->mtu,
-			 nctrl->ncmd.s.param1);
-		rtnl_lock();
-		netdev->mtu = nctrl->ncmd.s.param1;
-		call_netdevice_notifiers(NETDEV_CHANGEMTU, netdev);
-		rtnl_unlock();
-		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_ENABLE_VLAN_FILTER:
-		dev_info(&oct->pci_dev->dev, "%s VLAN filter enabled\n",
-			 netdev->name);
-		break;
-
-	case OCTNET_CMD_ADD_VLAN_FILTER:
-		dev_info(&oct->pci_dev->dev, "%s VLAN filter %d added\n",
-			 netdev->name, nctrl->ncmd.s.param1);
-		break;
-
-	case OCTNET_CMD_DEL_VLAN_FILTER:
-		dev_info(&oct->pci_dev->dev, "%s VLAN filter %d removed\n",
-			 netdev->name, nctrl->ncmd.s.param1);
-		break;
-
-	case OCTNET_CMD_SET_SETTINGS:
-		dev_info(&oct->pci_dev->dev, "%s settings changed\n",
-			 netdev->name);
-
-		break;
-		/* Case to handle "OCTNET_CMD_TNL_RX_CSUM_CTL"
-		 * Command passed by NIC driver
-		 */
-	case OCTNET_CMD_TNL_RX_CSUM_CTL:
-		if (nctrl->ncmd.s.param1 == OCTNET_CMD_RXCSUM_ENABLE) {
-			netif_info(lio, probe, lio->netdev,
-				   "%s RX Checksum Offload Enabled\n",
-				   netdev->name);
-		} else if (nctrl->ncmd.s.param1 ==
-			   OCTNET_CMD_RXCSUM_DISABLE) {
-			netif_info(lio, probe, lio->netdev,
-				   "%s RX Checksum Offload Disabled\n",
-				   netdev->name);
-		}
-		break;
-
-		/* Case to handle "OCTNET_CMD_TNL_TX_CSUM_CTL"
-		 * Command passed by NIC driver
-		 */
-	case OCTNET_CMD_TNL_TX_CSUM_CTL:
-		if (nctrl->ncmd.s.param1 == OCTNET_CMD_TXCSUM_ENABLE) {
-			netif_info(lio, probe, lio->netdev,
-				   "%s TX Checksum Offload Enabled\n",
-				   netdev->name);
-		} else if (nctrl->ncmd.s.param1 ==
-			   OCTNET_CMD_TXCSUM_DISABLE) {
-			netif_info(lio, probe, lio->netdev,
-				   "%s TX Checksum Offload Disabled\n",
-				   netdev->name);
-		}
-		break;
-
-		/* Case to handle "OCTNET_CMD_VXLAN_PORT_CONFIG"
-		 * Command passed by NIC driver
-		 */
-	case OCTNET_CMD_VXLAN_PORT_CONFIG:
-		if (nctrl->ncmd.s.more == OCTNET_CMD_VXLAN_PORT_ADD) {
-			netif_info(lio, probe, lio->netdev,
-				   "%s VxLAN Destination UDP PORT:%d ADDED\n",
-				   netdev->name,
-				   nctrl->ncmd.s.param1);
-		} else if (nctrl->ncmd.s.more ==
-			   OCTNET_CMD_VXLAN_PORT_DEL) {
-			netif_info(lio, probe, lio->netdev,
-				   "%s VxLAN Destination UDP PORT:%d DELETED\n",
-				   netdev->name,
-				   nctrl->ncmd.s.param1);
-		}
-		break;
-
-	case OCTNET_CMD_SET_FLOW_CTL:
-		netif_info(lio, probe, lio->netdev, "Set RX/TX flow control parameters\n");
-		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
@@ -2817,8 +3024,7 @@ static void handle_timestamp(struct octeon_device *oct,
  */
 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)
+					 struct octnet_buf_free_info *finfo)
 {
 	int retval;
 	struct octeon_soft_command *sc;
@@ -2846,9 +3052,15 @@ static inline int send_nic_timestamp_pkt(struct octeon_device *oct,
 	sc->callback_arg = finfo->skb;
 	sc->iq_no = ndata->q_no;
 
-	len = (u32)((struct octeon_instr_ih2 *)(&sc->cmd.cmd2.ih2))->dlengsz;
+	if (OCTEON_CN23XX_PF(oct))
+		len = (u32)((struct octeon_instr_ih3 *)
+			    (&sc->cmd.cmd3.ih3))->dlengsz;
+	else
+		len = (u32)((struct octeon_instr_ih2 *)
+			    (&sc->cmd.cmd2.ih2))->dlengsz;
+
+	ring_doorbell = 1;
 
-	ring_doorbell = !xmit_more;
 	retval = octeon_send_command(oct, sc->iq_no, ring_doorbell, &sc->cmd,
 				     sc, len, ndata->reqtype);
 
@@ -2881,7 +3093,7 @@ static int liquidio_xmit(struct sk_buff *skb, struct net_device *netdev)
 	union tx_info *tx_info;
 	int status = 0;
 	int q_idx = 0, iq_no = 0;
-	int xmit_more, j;
+	int j;
 	u64 dptr = 0;
 	u32 tag = 0;
 
@@ -2980,7 +3192,10 @@ static int liquidio_xmit(struct sk_buff *skb, struct net_device *netdev)
 			return NETDEV_TX_BUSY;
 		}
 
-		ndata.cmd.cmd2.dptr = dptr;
+		if (OCTEON_CN23XX_PF(oct))
+			ndata.cmd.cmd3.dptr = dptr;
+		else
+			ndata.cmd.cmd2.dptr = dptr;
 		finfo->dptr = dptr;
 		ndata.reqtype = REQTYPE_NORESP_NET;
 
@@ -3055,15 +3270,23 @@ static int liquidio_xmit(struct sk_buff *skb, struct net_device *netdev)
 					   g->sg_size, DMA_TO_DEVICE);
 		dptr = g->sg_dma_ptr;
 
-		ndata.cmd.cmd2.dptr = dptr;
+		if (OCTEON_CN23XX_PF(oct))
+			ndata.cmd.cmd3.dptr = dptr;
+		else
+			ndata.cmd.cmd2.dptr = dptr;
 		finfo->dptr = dptr;
 		finfo->g = g;
 
 		ndata.reqtype = REQTYPE_NORESP_NET_SG;
 	}
 
-	irh = (struct octeon_instr_irh *)&ndata.cmd.cmd2.irh;
-	tx_info = (union tx_info *)&ndata.cmd.cmd2.ossp[0];
+	if (OCTEON_CN23XX_PF(oct)) {
+		irh = (struct octeon_instr_irh *)&ndata.cmd.cmd3.irh;
+		tx_info = (union tx_info *)&ndata.cmd.cmd3.ossp[0];
+	} else {
+		irh = (struct octeon_instr_irh *)&ndata.cmd.cmd2.irh;
+		tx_info = (union tx_info *)&ndata.cmd.cmd2.ossp[0];
+	}
 
 	if (skb_shinfo(skb)->gso_size) {
 		tx_info->s.gso_size = skb_shinfo(skb)->gso_size;
@@ -3077,12 +3300,10 @@ static int liquidio_xmit(struct sk_buff *skb, struct net_device *netdev)
 		irh->vlan = skb_vlan_tag_get(skb) & 0xfff;
 	}
 
-	xmit_more = skb->xmit_more;
-
 	if (unlikely(cmdsetup.s.timestamp))
-		status = send_nic_timestamp_pkt(oct, &ndata, finfo, xmit_more);
+		status = send_nic_timestamp_pkt(oct, &ndata, finfo);
 	else
-		status = octnet_send_nic_data_pkt(oct, &ndata, xmit_more);
+		status = octnet_send_nic_data_pkt(oct, &ndata);
 	if (status == IQ_SEND_FAILED)
 		goto lio_xmit_failed;
 
@@ -3190,8 +3411,8 @@ static int liquidio_vlan_rx_kill_vid(struct net_device *netdev,
  *                              OCTNET_CMD_RXCSUM_DISABLE
  * @returns                     SUCCESS or FAILURE
  */
-int liquidio_set_rxcsum_command(struct net_device *netdev, int command,
-				u8 rx_cmd)
+static int liquidio_set_rxcsum_command(struct net_device *netdev, int command,
+				       u8 rx_cmd)
 {
 	struct lio *lio = GET_LIO(netdev);
 	struct octeon_device *oct = lio->oct_dev;
@@ -3249,31 +3470,6 @@ static int liquidio_vxlan_port_command(struct net_device *netdev, int command,
 	return ret;
 }
 
-int liquidio_set_feature(struct net_device *netdev, int cmd, u16 param1)
-{
-	struct lio *lio = GET_LIO(netdev);
-	struct octeon_device *oct = lio->oct_dev;
-	struct octnic_ctrl_pkt nctrl;
-	int ret = 0;
-
-	memset(&nctrl, 0, sizeof(struct octnic_ctrl_pkt));
-
-	nctrl.ncmd.u64 = 0;
-	nctrl.ncmd.s.cmd = cmd;
-	nctrl.ncmd.s.param1 = param1;
-	nctrl.iq_no = lio->linfo.txpciq[0].s.q_no;
-	nctrl.wait_time = 100;
-	nctrl.netpndev = (u64)netdev;
-	nctrl.cb_fn = liquidio_link_ctrl_cmd_completion;
-
-	ret = octnet_send_nic_ctrl_pkt(lio->oct_dev, &nctrl);
-	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
@@ -3492,8 +3688,9 @@ static int setup_nic_devices(struct octeon_device *octeon_dev)
 	union oct_nic_if_cfg if_cfg;
 	unsigned int base_queue;
 	unsigned int gmx_port_id;
-	u32 resp_size, ctx_size;
+	u32 resp_size, ctx_size, data_size;
 	u32 ifidx_or_pfnum;
+	struct lio_version *vdata;
 
 	/* This is to handle link status changes */
 	octeon_register_dispatch_fn(octeon_dev, OPCODE_NIC,
@@ -3515,21 +3712,37 @@ static int setup_nic_devices(struct octeon_device *octeon_dev)
 	for (i = 0; i < octeon_dev->ifcount; i++) {
 		resp_size = sizeof(struct liquidio_if_cfg_resp);
 		ctx_size = sizeof(struct liquidio_if_cfg_context);
+		data_size = sizeof(struct lio_version);
 		sc = (struct octeon_soft_command *)
-			octeon_alloc_soft_command(octeon_dev, 0,
+			octeon_alloc_soft_command(octeon_dev, data_size,
 						  resp_size, ctx_size);
 		resp = (struct liquidio_if_cfg_resp *)sc->virtrptr;
 		ctx  = (struct liquidio_if_cfg_context *)sc->ctxptr;
+		vdata = (struct lio_version *)sc->virtdptr;
+
+		*((u64 *)vdata) = 0;
+		vdata->major = cpu_to_be16(LIQUIDIO_BASE_MAJOR_VERSION);
+		vdata->minor = cpu_to_be16(LIQUIDIO_BASE_MINOR_VERSION);
+		vdata->micro = cpu_to_be16(LIQUIDIO_BASE_MICRO_VERSION);
 
-		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);
-		ifidx_or_pfnum = i;
+		if (OCTEON_CN23XX_PF(octeon_dev)) {
+			num_iqueues = octeon_dev->sriov_info.num_pf_rings;
+			num_oqueues = octeon_dev->sriov_info.num_pf_rings;
+			base_queue = octeon_dev->sriov_info.pf_srn;
+
+			gmx_port_id = octeon_dev->pf_num;
+			ifidx_or_pfnum = octeon_dev->pf_num;
+		} else {
+			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);
+			ifidx_or_pfnum = i;
+		}
 
 		dev_dbg(&octeon_dev->pci_dev->dev,
 			"requesting config for interface %d, iqs %d, oqs %d\n",
@@ -3566,7 +3779,11 @@ static int setup_nic_devices(struct octeon_device *octeon_dev)
 		/* Sleep on a wait queue till the cond flag indicates that the
 		 * response arrived or timed-out.
 		 */
-		sleep_cond(&ctx->wc, &ctx->cond);
+		if (sleep_cond(&ctx->wc, &ctx->cond) == -EINTR) {
+			dev_err(&octeon_dev->pci_dev->dev, "Wait interrupted\n");
+			goto setup_nic_wait_intr;
+		}
+
 		retval = resp->status;
 		if (retval) {
 			dev_err(&octeon_dev->pci_dev->dev, "iq/oq config failed\n");
@@ -3633,12 +3850,16 @@ static int setup_nic_devices(struct octeon_device *octeon_dev)
 
 		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_GRO
-				| NETIF_F_TSO | NETIF_F_TSO6
-				| NETIF_F_LRO;
+		if (OCTEON_CN23XX_PF(octeon_dev) ||
+		    OCTEON_CN6XXX(octeon_dev)) {
+			lio->dev_capability = NETIF_F_HIGHDMA
+					      | NETIF_F_IP_CSUM
+					      | NETIF_F_IPV6_CSUM
+					      | NETIF_F_SG | NETIF_F_RXCSUM
+					      | NETIF_F_GRO
+					      | NETIF_F_TSO | NETIF_F_TSO6
+					      | NETIF_F_LRO;
+		}
 		netif_set_gso_max_size(netdev, OCTNIC_GSO_MAX_SIZE);
 
 		/*  Copy of transmit encapsulation capabilities:
@@ -3713,7 +3934,10 @@ static int setup_nic_devices(struct octeon_device *octeon_dev)
 
 		/* Register ethtool support */
 		liquidio_set_ethtool_ops(netdev);
-		octeon_dev->priv_flags = 0x0;
+		if (lio->oct_dev->chip_id == OCTEON_CN23XX_PF_VID)
+			octeon_dev->priv_flags = OCT_PRIV_FLAG_DEFAULT;
+		else
+			octeon_dev->priv_flags = 0x0;
 
 		if (netdev->features & NETIF_F_LRO)
 			liquidio_set_feature(netdev, OCTNET_CMD_LRO_ENABLE,
@@ -3725,6 +3949,9 @@ static int setup_nic_devices(struct octeon_device *octeon_dev)
 			liquidio_set_feature(netdev,
 					     OCTNET_CMD_VERBOSE_ENABLE, 0);
 
+		if (setup_link_status_change_wq(netdev))
+			goto setup_nic_dev_fail;
+
 		/* Register the network device with the OS */
 		if (register_netdev(netdev)) {
 			dev_err(&octeon_dev->pci_dev->dev, "Device registration failed\n");
@@ -3760,6 +3987,8 @@ setup_nic_dev_fail:
 
 	octeon_free_soft_command(octeon_dev, sc);
 
+setup_nic_wait_intr:
+
 	while (i--) {
 		dev_err(&octeon_dev->pci_dev->dev,
 			"NIC ifidx:%d Setup failed\n", i);
@@ -3789,8 +4018,7 @@ static int liquidio_init_nic_module(struct octeon_device *oct)
 	/* run port_config command for each port */
 	oct->ifcount = num_nic_ports;
 
-	memset(oct->props, 0,
-	       sizeof(struct octdev_props) * num_nic_ports);
+	memset(oct->props, 0, sizeof(struct octdev_props) * num_nic_ports);
 
 	for (i = 0; i < MAX_OCTEON_LINKS; i++)
 		oct->props[i].gmxport = -1;
@@ -3806,7 +4034,7 @@ static int liquidio_init_nic_module(struct octeon_device *oct)
 	/* Initialize interrupt moderation params */
 	intrmod_cfg = &((struct octeon_device *)oct)->intrmod;
 	intrmod_cfg->rx_enable = 1;
-	intrmod_cfg->check_intrvl =   LIO_INTRMOD_CHECK_INTERVAL;
+	intrmod_cfg->check_intrvl = LIO_INTRMOD_CHECK_INTERVAL;
 	intrmod_cfg->maxpkt_ratethr = LIO_INTRMOD_MAXPKT_RATETHR;
 	intrmod_cfg->minpkt_ratethr = LIO_INTRMOD_MINPKT_RATETHR;
 	intrmod_cfg->rx_maxcnt_trigger = LIO_INTRMOD_RXMAXCNT_TRIGGER;
@@ -3818,6 +4046,7 @@ static int liquidio_init_nic_module(struct octeon_device *oct)
 	intrmod_cfg->tx_mincnt_trigger = LIO_INTRMOD_TXMINCNT_TRIGGER;
 	intrmod_cfg->rx_frames = CFG_GET_OQ_INTR_PKT(octeon_get_conf(oct));
 	intrmod_cfg->rx_usecs = CFG_GET_OQ_INTR_TIME(octeon_get_conf(oct));
+	intrmod_cfg->tx_frames = CFG_GET_IQ_INTR_PKT(octeon_get_conf(oct));
 	dev_dbg(&oct->pci_dev->dev, "Network interfaces ready\n");
 
 	return retval;
@@ -3880,6 +4109,7 @@ static void nic_starter(struct work_struct *work)
 static int octeon_device_init(struct octeon_device *octeon_dev)
 {
 	int j, ret;
+	int fw_loaded = 0;
 	char bootcmd[] = "\n";
 	struct octeon_device_priv *oct_priv =
 		(struct octeon_device_priv *)octeon_dev->priv;
@@ -3901,9 +4131,23 @@ static int octeon_device_init(struct octeon_device *octeon_dev)
 
 	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))
+	if (OCTEON_CN23XX_PF(octeon_dev)) {
+		if (!cn23xx_fw_loaded(octeon_dev)) {
+			fw_loaded = 0;
+			/* Do a soft reset of the Octeon device. */
+			if (octeon_dev->fn_list.soft_reset(octeon_dev))
+				return 1;
+			/* things might have changed */
+			if (!cn23xx_fw_loaded(octeon_dev))
+				fw_loaded = 0;
+			else
+				fw_loaded = 1;
+		} else {
+			fw_loaded = 1;
+		}
+	} else if (octeon_dev->fn_list.soft_reset(octeon_dev)) {
 		return 1;
+	}
 
 	/* Initialize the dispatch mechanism used to push packets arriving on
 	 * Octeon Output queues.
@@ -3925,6 +4169,22 @@ static int octeon_device_init(struct octeon_device *octeon_dev)
 
 	octeon_set_io_queues_off(octeon_dev);
 
+	if (OCTEON_CN23XX_PF(octeon_dev)) {
+		ret = octeon_dev->fn_list.setup_device_regs(octeon_dev);
+		if (ret) {
+			dev_err(&octeon_dev->pci_dev->dev, "OCTEON: Failed to configure device registers\n");
+			return ret;
+		}
+	}
+
+	/* 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);
+
 	/*  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,
@@ -3936,14 +4196,6 @@ static int octeon_device_init(struct octeon_device *octeon_dev)
 	}
 	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.
 	 */
@@ -3963,15 +4215,23 @@ static int octeon_device_init(struct octeon_device *octeon_dev)
 
 	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;
+	if (OCTEON_CN23XX_PF(octeon_dev)) {
+		if (octeon_allocate_ioq_vector(octeon_dev)) {
+			dev_err(&octeon_dev->pci_dev->dev, "OCTEON: ioq vector allocation failed\n");
+			return 1;
+		}
+
+	} else {
+		/* 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.*/
@@ -3985,63 +4245,76 @@ static int octeon_device_init(struct octeon_device *octeon_dev)
 		return 1;
 
 	/* Enable Octeon device interrupts */
-	octeon_dev->fn_list.enable_interrupt(octeon_dev->chip);
+	octeon_dev->fn_list.enable_interrupt(octeon_dev, OCTEON_ALL_INTR);
 
 	/* Enable the input and output queues for this Octeon device */
-	octeon_dev->fn_list.enable_io_queues(octeon_dev);
+	ret = octeon_dev->fn_list.enable_io_queues(octeon_dev);
+	if (ret) {
+		dev_err(&octeon_dev->pci_dev->dev, "Failed to enable input/output queues");
+		return ret;
+	}
 
 	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");
+	if ((!OCTEON_CN23XX_PF(octeon_dev)) || !fw_loaded) {
+		dev_dbg(&octeon_dev->pci_dev->dev, "Waiting for DDR initialization...\n");
+		if (!ddr_timeout) {
+			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);
+		schedule_timeout_uninterruptible(HZ * LIO_RESET_SECS);
 
-	/* Wait for the octeon to initialize DDR after the soft-reset. */
-	while (ddr_timeout == 0) {
-		set_current_state(TASK_INTERRUPTIBLE);
-		if (schedule_timeout(HZ / 10)) {
-			/* user probably pressed Control-C */
+		/* Wait for the octeon to initialize DDR after the soft-reset.*/
+		while (!ddr_timeout) {
+			set_current_state(TASK_INTERRUPTIBLE);
+			if (schedule_timeout(HZ / 10)) {
+				/* user probably pressed Control-C */
+				return 1;
+			}
+		}
+		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;
 		}
-	}
-	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;
-	}
+		if (octeon_wait_for_bootloader(octeon_dev, 1000)) {
+			dev_err(&octeon_dev->pci_dev->dev, "Board not responding\n");
+			return 1;
+		}
 
-	/* Divert uboot to take commands from host instead. */
-	ret = octeon_console_send_cmd(octeon_dev, bootcmd, 50);
+		/* Divert uboot to take commands from host instead. */
+		ret = octeon_console_send_cmd(octeon_dev, bootcmd, 50);
 
-	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;
-	}
+		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);
+		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;
+		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;
+		}
+		/* set bit 1 of SLI_SCRATCH_1 to indicate that firmware is
+		 * loaded
+		 */
+		if (OCTEON_CN23XX_PF(octeon_dev))
+			octeon_write_csr64(octeon_dev, CN23XX_SLI_SCRATCH1,
+					   2ULL);
 	}
 
 	handshake[octeon_dev->octeon_id].init_ok = 1;
@@ -4057,7 +4330,6 @@ static int octeon_device_init(struct octeon_device *octeon_dev)
 		       octeon_dev->droq[j]->pkts_credit_reg);
 
 	/* Packets can start arriving on the output queues from this point. */
-
 	return 0;
 }
 
diff --git a/drivers/net/ethernet/cavium/liquidio/liquidio_common.h b/drivers/net/ethernet/cavium/liquidio/liquidio_common.h
index 199a8b9c7dc5..0d990accb65e 100644
--- a/drivers/net/ethernet/cavium/liquidio/liquidio_common.h
+++ b/drivers/net/ethernet/cavium/liquidio/liquidio_common.h
@@ -30,10 +30,24 @@
 
 #include "octeon_config.h"
 
-#define LIQUIDIO_BASE_VERSION   "1.4"
-#define LIQUIDIO_MICRO_VERSION  ".1"
 #define LIQUIDIO_PACKAGE ""
-#define LIQUIDIO_VERSION  "1.4.1"
+#define LIQUIDIO_BASE_MAJOR_VERSION 1
+#define LIQUIDIO_BASE_MINOR_VERSION 4
+#define LIQUIDIO_BASE_MICRO_VERSION 1
+#define LIQUIDIO_BASE_VERSION   __stringify(LIQUIDIO_BASE_MAJOR_VERSION) "." \
+				__stringify(LIQUIDIO_BASE_MINOR_VERSION)
+#define LIQUIDIO_MICRO_VERSION  "." __stringify(LIQUIDIO_BASE_MICRO_VERSION)
+#define LIQUIDIO_VERSION        LIQUIDIO_PACKAGE \
+				__stringify(LIQUIDIO_BASE_MAJOR_VERSION) "." \
+				__stringify(LIQUIDIO_BASE_MINOR_VERSION) \
+				"." __stringify(LIQUIDIO_BASE_MICRO_VERSION)
+
+struct lio_version {
+	u16  major;
+	u16  minor;
+	u16  micro;
+	u16  reserved;
+};
 
 #define CONTROL_IQ 0
 /** Tag types used by Octeon cores in its work. */
@@ -218,6 +232,9 @@ static inline void add_sg_size(struct octeon_sg_entry *sg_entry,
 #define   OCTNET_CMD_ADD_VLAN_FILTER  0x17
 #define   OCTNET_CMD_DEL_VLAN_FILTER  0x18
 #define   OCTNET_CMD_VXLAN_PORT_CONFIG 0x19
+
+#define   OCTNET_CMD_ID_ACTIVE         0x1a
+
 #define   OCTNET_CMD_VXLAN_PORT_ADD    0x0
 #define   OCTNET_CMD_VXLAN_PORT_DEL    0x1
 #define   OCTNET_CMD_RXCSUM_ENABLE     0x0
@@ -296,6 +313,13 @@ union octnet_cmd {
 
 #define   OCTNET_CMD_SIZE     (sizeof(union octnet_cmd))
 
+/*pkiih3 + irh + ossp[0] + ossp[1] + rdp + rptr = 40 bytes */
+#define LIO_SOFTCMDRESP_IH2       40
+#define LIO_SOFTCMDRESP_IH3       (40 + 8)
+
+#define LIO_PCICMD_O2             24
+#define LIO_PCICMD_O3             (24 + 8)
+
 /* Instruction Header(DPI) - for OCTEON-III models */
 struct  octeon_instr_ih3 {
 #ifdef __BIG_ENDIAN_BITFIELD
@@ -814,6 +838,8 @@ struct oct_link_stats {
 #define VITESSE_PHY_GPIO_DRIVEOFF 0x4
 #define VITESSE_PHY_GPIO_HIGH     0x2
 #define VITESSE_PHY_GPIO_LOW      0x3
+#define LED_IDENTIFICATION_ON     0x1
+#define LED_IDENTIFICATION_OFF    0x0
 
 struct oct_mdio_cmd {
 	u64 op;
@@ -832,7 +858,7 @@ struct oct_mdio_cmd {
 /* intrmod: max. packets to trigger interrupt */
 #define LIO_INTRMOD_RXMAXCNT_TRIGGER	384
 /* intrmod: min. packets to trigger interrupt */
-#define LIO_INTRMOD_RXMINCNT_TRIGGER	1
+#define LIO_INTRMOD_RXMINCNT_TRIGGER	0
 /* intrmod: max. time to trigger interrupt */
 #define LIO_INTRMOD_RXMAXTMR_TRIGGER	128
 /* 66xx:intrmod: min. time to trigger interrupt
diff --git a/drivers/net/ethernet/cavium/liquidio/octeon_config.h b/drivers/net/ethernet/cavium/liquidio/octeon_config.h
index b3396e3a8bab..c76556809ed1 100644
--- a/drivers/net/ethernet/cavium/liquidio/octeon_config.h
+++ b/drivers/net/ethernet/cavium/liquidio/octeon_config.h
@@ -64,6 +64,34 @@
 #define   DEFAULT_NUM_NIC_PORTS_68XX   4
 #define   DEFAULT_NUM_NIC_PORTS_68XX_210NV  2
 
+/* CN23xx  IQ configuration macros */
+#define   CN23XX_MAX_RINGS_PER_PF_PASS_1_0 12
+#define   CN23XX_MAX_RINGS_PER_PF_PASS_1_1 32
+#define   CN23XX_MAX_RINGS_PER_PF          64
+
+#define   CN23XX_MAX_INPUT_QUEUES	CN23XX_MAX_RINGS_PER_PF
+#define   CN23XX_MAX_IQ_DESCRIPTORS	2048
+#define   CN23XX_DB_MIN                 1
+#define   CN23XX_DB_MAX                 8
+#define   CN23XX_DB_TIMEOUT             1
+
+#define   CN23XX_MAX_OUTPUT_QUEUES	CN23XX_MAX_RINGS_PER_PF
+#define   CN23XX_MAX_OQ_DESCRIPTORS	2048
+#define   CN23XX_OQ_BUF_SIZE		1536
+#define   CN23XX_OQ_PKTSPER_INTR	128
+/*#define CAVIUM_ONLY_CN23XX_RX_PERF*/
+#define   CN23XX_OQ_REFIL_THRESHOLD	128
+
+#define   CN23XX_OQ_INTR_PKT		64
+#define   CN23XX_OQ_INTR_TIME		100
+#define   DEFAULT_NUM_NIC_PORTS_23XX	1
+
+#define   CN23XX_CFG_IO_QUEUES		CN23XX_MAX_RINGS_PER_PF
+/* PEMs count */
+#define   CN23XX_MAX_MACS		4
+
+#define   CN23XX_DEF_IQ_INTR_THRESHOLD	32
+#define   CN23XX_DEF_IQ_INTR_BYTE_THRESHOLD   (64 * 1024)
 /* common OCTEON configuration macros */
 #define   CN6XXX_CFG_IO_QUEUES         32
 #define   OCTEON_32BYTE_INSTR          32
@@ -92,6 +120,9 @@
 #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_IQ_INTR_PKT(cfg)                 ((cfg)->iq.iq_intr_pkt)
+#define CFG_SET_IQ_INTR_PKT(cfg, val)            (cfg)->iq.iq_intr_pkt = val
+
 #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)
@@ -140,19 +171,24 @@
 enum lio_card_type {
 	LIO_210SV = 0, /* Two port, 66xx */
 	LIO_210NV,     /* Two port, 68xx */
-	LIO_410NV      /* Four port, 68xx */
+	LIO_410NV,     /* Four port, 68xx */
+	LIO_23XX       /* 23xx */
 };
 
 #define LIO_210SV_NAME "210sv"
 #define LIO_210NV_NAME "210nv"
 #define LIO_410NV_NAME "410nv"
+#define LIO_23XX_NAME  "23xx"
 
 /** 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;
+	u64 reserved:16;
+
+	/** Tx interrupt packets. Applicable to 23xx only */
+	u64 iq_intr_pkt:16;
 
 	/** Minimum ticks to wait before checking for pending instructions. */
 	u64 db_timeout:16;
@@ -192,7 +228,10 @@ struct octeon_iq_config {
 	/** Minimum ticks to wait before checking for pending instructions. */
 	u64 db_timeout:16;
 
-	u64 reserved:32;
+	/** Tx interrupt packets. Applicable to 23xx only */
+	u64 iq_intr_pkt:16;
+
+	u64 reserved:16;
 #endif
 };
 
@@ -416,11 +455,15 @@ struct octeon_config {
 #define DISPATCH_LIST_SIZE                      BIT(OPCODE_MASK_BITS)
 
 /* Maximum number of Octeon Instruction (command) queues */
-#define MAX_OCTEON_INSTR_QUEUES(oct)         CN6XXX_MAX_INPUT_QUEUES
-/* Maximum number of Octeon Output queues */
-#define MAX_OCTEON_OUTPUT_QUEUES(oct)         CN6XXX_MAX_OUTPUT_QUEUES
+#define MAX_OCTEON_INSTR_QUEUES(oct)		\
+		(OCTEON_CN23XX_PF(oct) ? CN23XX_MAX_INPUT_QUEUES : \
+					CN6XXX_MAX_INPUT_QUEUES)
 
-#define MAX_POSSIBLE_OCTEON_INSTR_QUEUES       CN6XXX_MAX_INPUT_QUEUES
-#define MAX_POSSIBLE_OCTEON_OUTPUT_QUEUES      CN6XXX_MAX_OUTPUT_QUEUES
+/* Maximum number of Octeon Instruction (command) queues */
+#define MAX_OCTEON_OUTPUT_QUEUES(oct)		\
+		(OCTEON_CN23XX_PF(oct) ? CN23XX_MAX_OUTPUT_QUEUES : \
+					CN6XXX_MAX_OUTPUT_QUEUES)
 
+#define MAX_POSSIBLE_OCTEON_INSTR_QUEUES	CN23XX_MAX_INPUT_QUEUES
+#define MAX_POSSIBLE_OCTEON_OUTPUT_QUEUES	CN23XX_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
index bbb50ea66f16..01a50f3b0c8e 100644
--- a/drivers/net/ethernet/cavium/liquidio/octeon_console.c
+++ b/drivers/net/ethernet/cavium/liquidio/octeon_console.c
@@ -25,12 +25,13 @@
  */
 #include <linux/pci.h>
 #include <linux/netdevice.h>
+#include <linux/crc32.h>
 #include "liquidio_common.h"
 #include "octeon_droq.h"
 #include "octeon_iq.h"
 #include "response_manager.h"
 #include "octeon_device.h"
-#include "octeon_main.h"
+#include "liquidio_image.h"
 #include "octeon_mem_ops.h"
 
 static void octeon_remote_lock(void);
@@ -40,6 +41,10 @@ static u64 cvmx_bootmem_phy_named_block_find(struct octeon_device *oct,
 					     u32 flags);
 static int octeon_console_read(struct octeon_device *oct, u32 console_num,
 			       char *buffer, u32 buf_size);
+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 MIN(a, b) min((a), (b))
 #define CAST_ULL(v) ((u64)(v))
@@ -177,6 +182,15 @@ struct octeon_pci_console_desc {
 	__cvmx_bootmem_desc_get(oct, addr,                               \
 		offsetof(struct cvmx_bootmem_named_block_desc, field),   \
 		SIZEOF_FIELD(struct cvmx_bootmem_named_block_desc, field))
+/**
+ * \brief determines if a given console has debug enabled.
+ * @param console console to check
+ * @returns  1 = enabled. 0 otherwise
+ */
+static int octeon_console_debug_enabled(u32 console)
+{
+	return (console_bitmask >> (console)) & 0x1;
+}
 
 /**
  * This function is the implementation of the get macros defined
@@ -709,3 +723,104 @@ static int octeon_console_read(struct octeon_device *oct, u32 console_num,
 
 	return bytes_to_read;
 }
+
+#define FBUF_SIZE	(4 * 1024 * 1024)
+u8 fbuf[FBUF_SIZE];
+
+int octeon_download_firmware(struct octeon_device *oct, const u8 *data,
+			     size_t size)
+{
+	int ret = 0;
+	u8 *p = fbuf;
+	u32 crc32_result;
+	u64 load_addr;
+	u32 image_len;
+	struct octeon_firmware_file_header *h;
+	u32 i, rem;
+
+	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((unsigned int)~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 (strncmp(LIQUIDIO_PACKAGE, h->version, strlen(LIQUIDIO_PACKAGE))) {
+		dev_err(&oct->pci_dev->dev, "Unmatched firmware package type. Expected %s, got %s.\n",
+			LIQUIDIO_PACKAGE, h->version);
+		return -EINVAL;
+	}
+
+	if (memcmp(LIQUIDIO_BASE_VERSION, h->version + strlen(LIQUIDIO_PACKAGE),
+		   strlen(LIQUIDIO_BASE_VERSION))) {
+		dev_err(&oct->pci_dev->dev, "Unmatched firmware version. Expected %s.x, got %s.\n",
+			LIQUIDIO_BASE_VERSION,
+			h->version + strlen(LIQUIDIO_PACKAGE));
+		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);
+
+	data += sizeof(struct octeon_firmware_file_header);
+
+	dev_info(&oct->pci_dev->dev, "%s: Loading %d images\n", __func__,
+		 be32_to_cpu(h->num_images));
+	/* 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);
+
+		dev_info(&oct->pci_dev->dev, "Loading firmware %d at %llx\n",
+			 image_len, load_addr);
+
+		/* Write in 4MB chunks*/
+		rem = image_len;
+
+		while (rem) {
+			if (rem < FBUF_SIZE)
+				size = rem;
+			else
+				size = FBUF_SIZE;
+
+			memcpy(p, data, size);
+
+			/* download the image */
+			octeon_pci_write_core_mem(oct, load_addr, p, (u32)size);
+
+			data += size;
+			rem -= (u32)size;
+			load_addr += size;
+		}
+	}
+	dev_info(&oct->pci_dev->dev, "Writing boot command: %s\n",
+		 h->bootcmd);
+
+	/* Invoke the bootcmd */
+	ret = octeon_console_send_cmd(oct, h->bootcmd, 50);
+
+	return 0;
+}
diff --git a/drivers/net/ethernet/cavium/liquidio/octeon_device.c b/drivers/net/ethernet/cavium/liquidio/octeon_device.c
index 0eb504a4379a..586b68899b06 100644
--- a/drivers/net/ethernet/cavium/liquidio/octeon_device.c
+++ b/drivers/net/ethernet/cavium/liquidio/octeon_device.c
@@ -20,7 +20,6 @@
 * Contact Cavium, Inc. for more information
 **********************************************************************/
 #include <linux/pci.h>
-#include <linux/crc32.h>
 #include <linux/netdevice.h>
 #include <linux/vmalloc.h>
 #include "liquidio_common.h"
@@ -32,8 +31,7 @@
 #include "octeon_network.h"
 #include "cn66xx_regs.h"
 #include "cn66xx_device.h"
-#include "liquidio_image.h"
-#include "octeon_mem_ops.h"
+#include "cn23xx_pf_device.h"
 
 /** Default configuration
  *  for CN66XX OCTEON Models.
@@ -420,6 +418,108 @@ static struct octeon_config default_cn68xx_210nv_conf = {
 	,
 };
 
+static struct octeon_config default_cn23xx_conf = {
+	.card_type                              = LIO_23XX,
+	.card_name                              = LIO_23XX_NAME,
+	/** IQ attributes */
+	.iq = {
+		.max_iqs		= CN23XX_CFG_IO_QUEUES,
+		.pending_list_size	= (CN23XX_MAX_IQ_DESCRIPTORS *
+					   CN23XX_CFG_IO_QUEUES),
+		.instr_type		= OCTEON_64BYTE_INSTR,
+		.db_min			= CN23XX_DB_MIN,
+		.db_timeout		= CN23XX_DB_TIMEOUT,
+		.iq_intr_pkt		= CN23XX_DEF_IQ_INTR_THRESHOLD,
+	},
+
+	/** OQ attributes */
+	.oq = {
+		.max_oqs		= CN23XX_CFG_IO_QUEUES,
+		.info_ptr		= OCTEON_OQ_INFOPTR_MODE,
+		.pkts_per_intr	= CN23XX_OQ_PKTSPER_INTR,
+		.refill_threshold	= CN23XX_OQ_REFIL_THRESHOLD,
+		.oq_intr_pkt	= CN23XX_OQ_INTR_PKT,
+		.oq_intr_time	= CN23XX_OQ_INTR_TIME,
+	},
+
+	.num_nic_ports				= DEFAULT_NUM_NIC_PORTS_23XX,
+	.num_def_rx_descs			= CN23XX_MAX_OQ_DESCRIPTORS,
+	.num_def_tx_descs			= CN23XX_MAX_IQ_DESCRIPTORS,
+	.def_rx_buf_size			= CN23XX_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			= CN23XX_MAX_OQ_DESCRIPTORS,
+
+		/* Num of desc for tx rings */
+		.num_tx_descs			= CN23XX_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			= CN23XX_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			= CN23XX_MAX_OQ_DESCRIPTORS,
+
+		/* Num of desc for tx rings */
+		.num_tx_descs			= CN23XX_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			= CN23XX_OQ_BUF_SIZE,
+
+		.base_queue			= BASE_QUEUE_NOT_REQUESTED,
+
+		.gmx_port_id			= 1,
+	},
+
+	.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,
@@ -487,6 +587,8 @@ static void *__retrieve_octeon_config_info(struct octeon_device *oct,
 		} else if ((oct->chip_id == OCTEON_CN68XX) &&
 			   (card_type == LIO_410NV)) {
 			ret =  (void *)&default_cn68xx_conf;
+		} else if (oct->chip_id == OCTEON_CN23XX_PF_VID) {
+			ret =  (void *)&default_cn23xx_conf;
 		}
 		break;
 	default:
@@ -501,7 +603,8 @@ static int __verify_octeon_config_info(struct octeon_device *oct, void *conf)
 	case OCTEON_CN66XX:
 	case OCTEON_CN68XX:
 		return lio_validate_cn6xxx_config_info(oct, conf);
-
+	case OCTEON_CN23XX_PF_VID:
+		return 0;
 	default:
 		break;
 	}
@@ -541,107 +644,6 @@ static char *get_oct_app_string(u32 app_mode)
 	return oct_dev_app_str[CVM_DRV_INVALID_APP - CVM_DRV_APP_START];
 }
 
-u8 fbuf[4 * 1024 * 1024];
-
-int octeon_download_firmware(struct octeon_device *oct, const u8 *data,
-			     size_t size)
-{
-	int ret = 0;
-	u8 *p = fbuf;
-	u32 crc32_result;
-	u64 load_addr;
-	u32 image_len;
-	struct octeon_firmware_file_header *h;
-	u32 i, rem, base_len = strlen(LIQUIDIO_BASE_VERSION);
-	char *base;
-
-	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((unsigned int)~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 (strncmp(LIQUIDIO_PACKAGE, h->version, strlen(LIQUIDIO_PACKAGE))) {
-		dev_err(&oct->pci_dev->dev, "Unmatched firmware package type. Expected %s, got %s.\n",
-			LIQUIDIO_PACKAGE, h->version);
-		return -EINVAL;
-	}
-
-	base = h->version + strlen(LIQUIDIO_PACKAGE);
-	ret = memcmp(LIQUIDIO_BASE_VERSION, base, base_len);
-	if (ret) {
-		dev_err(&oct->pci_dev->dev, "Unmatched firmware version. Expected %s.x, got %s.\n",
-			LIQUIDIO_BASE_VERSION, base);
-		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);
-
-	data += sizeof(struct octeon_firmware_file_header);
-
-	dev_info(&oct->pci_dev->dev, "%s: Loading %d images\n", __func__,
-		 be32_to_cpu(h->num_images));
-	/* 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);
-
-		dev_info(&oct->pci_dev->dev, "Loading firmware %d at %llx\n",
-			 image_len, load_addr);
-
-		/* Write in 4MB chunks*/
-		rem = image_len;
-
-		while (rem) {
-			if (rem < (4 * 1024 * 1024))
-				size = rem;
-			else
-				size = 4 * 1024 * 1024;
-
-			memcpy(p, data, size);
-
-			/* download the image */
-			octeon_pci_write_core_mem(oct, load_addr, p, (u32)size);
-
-			data += size;
-			rem -= (u32)size;
-			load_addr += size;
-		}
-	}
-	dev_info(&oct->pci_dev->dev, "Writing boot command: %s\n",
-		 h->bootcmd);
-
-	/* Invoke the bootcmd */
-	ret = octeon_console_send_cmd(oct, h->bootcmd, 50);
-
-	return 0;
-}
-
 void octeon_free_device_mem(struct octeon_device *oct)
 {
 	int i;
@@ -676,6 +678,9 @@ static struct octeon_device *octeon_allocate_device_mem(u32 pci_id,
 		configsize = sizeof(struct octeon_cn6xxx);
 		break;
 
+	case OCTEON_CN23XX_PF_VID:
+		configsize = sizeof(struct octeon_cn23xx_pf);
+		break;
 	default:
 		pr_err("%s: Unknown PCI Device: 0x%x\n",
 		       __func__,
@@ -741,6 +746,45 @@ struct octeon_device *octeon_allocate_device(u32 pci_id,
 	return oct;
 }
 
+int
+octeon_allocate_ioq_vector(struct octeon_device  *oct)
+{
+	int i, num_ioqs = 0;
+	struct octeon_ioq_vector *ioq_vector;
+	int cpu_num;
+	int size;
+
+	if (OCTEON_CN23XX_PF(oct))
+		num_ioqs = oct->sriov_info.num_pf_rings;
+	size = sizeof(struct octeon_ioq_vector) * num_ioqs;
+
+	oct->ioq_vector = vmalloc(size);
+	if (!oct->ioq_vector)
+		return 1;
+	memset(oct->ioq_vector, 0, size);
+	for (i = 0; i < num_ioqs; i++) {
+		ioq_vector		= &oct->ioq_vector[i];
+		ioq_vector->oct_dev	= oct;
+		ioq_vector->iq_index	= i;
+		ioq_vector->droq_index	= i;
+
+		cpu_num = i % num_online_cpus();
+		cpumask_set_cpu(cpu_num, &ioq_vector->affinity_mask);
+
+		if (oct->chip_id == OCTEON_CN23XX_PF_VID)
+			ioq_vector->ioq_num	= i + oct->sriov_info.pf_srn;
+		else
+			ioq_vector->ioq_num	= i;
+	}
+	return 0;
+}
+
+void
+octeon_free_ioq_vector(struct octeon_device *oct)
+{
+	vfree(oct->ioq_vector);
+}
+
 /* this function is only for setting up the first queue */
 int octeon_setup_instr_queues(struct octeon_device *oct)
 {
@@ -749,10 +793,12 @@ int octeon_setup_instr_queues(struct octeon_device *oct)
 	union oct_txpciq txpciq;
 	int numa_node = cpu_to_node(iq_no % num_online_cpus());
 
-	/* this causes queue 0 to be default queue */
 	if (OCTEON_CN6XXX(oct))
 		num_descs =
 			CFG_GET_NUM_DEF_TX_DESCS(CHIP_FIELD(oct, cn6xxx, conf));
+	else if (OCTEON_CN23XX_PF(oct))
+		num_descs = CFG_GET_NUM_DEF_TX_DESCS(CHIP_FIELD(oct, cn23xx_pf,
+								conf));
 
 	oct->num_iqs = 0;
 
@@ -769,6 +815,7 @@ int octeon_setup_instr_queues(struct octeon_device *oct)
 	oct->instr_queue[0]->ifidx = 0;
 	txpciq.u64 = 0;
 	txpciq.s.q_no = iq_no;
+	txpciq.s.pkind = oct->pfvf_hsword.pkind;
 	txpciq.s.use_qpg = 0;
 	txpciq.s.qpg = 0;
 	if (octeon_init_instr_queue(oct, txpciq, num_descs)) {
@@ -788,14 +835,17 @@ int octeon_setup_output_queues(struct octeon_device *oct)
 	u32 oq_no = 0;
 	int numa_node = cpu_to_node(oq_no % num_online_cpus());
 
-	/* this causes queue 0 to be default queue */
 	if (OCTEON_CN6XXX(oct)) {
 		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));
+	} else if (OCTEON_CN23XX_PF(oct)) {
+		num_descs = CFG_GET_NUM_DEF_RX_DESCS(CHIP_FIELD(oct, cn23xx_pf,
+								conf));
+		desc_size = CFG_GET_DEF_RX_BUF_SIZE(CHIP_FIELD(oct, cn23xx_pf,
+							       conf));
 	}
-
 	oct->num_oqs = 0;
 	oct->droq[0] = vmalloc_node(sizeof(*oct->droq[0]), numa_node);
 	if (!oct->droq[0])
@@ -812,10 +862,10 @@ int octeon_setup_output_queues(struct octeon_device *oct)
 
 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);
+	if (OCTEON_CN6XXX(oct)) {
+		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,
@@ -825,14 +875,16 @@ void octeon_set_droq_pkt_op(struct octeon_device *oct,
 	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 (OCTEON_CN6XXX(oct)) {
+		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));
+		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);
+		octeon_write_csr(oct, CN6XXX_SLI_PKT_OUT_ENB, reg_val);
+	}
 }
 
 int octeon_init_dispatch_list(struct octeon_device *oct)
@@ -1019,6 +1071,9 @@ int octeon_core_drv_init(struct octeon_recv_info *recv_info, void *buf)
 	if (OCTEON_CN6XXX(oct))
 		num_nic_ports =
 			CFG_GET_NUM_NIC_PORTS(CHIP_FIELD(oct, cn6xxx, conf));
+	else if (OCTEON_CN23XX_PF(oct))
+		num_nic_ports =
+			CFG_GET_NUM_NIC_PORTS(CHIP_FIELD(oct, cn23xx_pf, 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",
@@ -1046,6 +1101,12 @@ int octeon_core_drv_init(struct octeon_recv_info *recv_info, void *buf)
 	}
 	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;
+	oct->pfvf_hsword.app_mode = (u32)recv_pkt->rh.r_core_drv_init.app_mode;
+
+	oct->pfvf_hsword.pkind = recv_pkt->rh.r_core_drv_init.pkind;
+
+	for (i = 0; i < oct->num_iqs; i++)
+		oct->instr_queue[i]->txpciq.s.pkind = oct->pfvf_hsword.pkind;
 
 	atomic_set(&oct->status, OCT_DEV_CORE_OK);
 
@@ -1108,8 +1169,10 @@ struct octeon_config *octeon_get_conf(struct octeon_device *oct)
 	if (OCTEON_CN6XXX(oct)) {
 		default_oct_conf =
 			(struct octeon_config *)(CHIP_FIELD(oct, cn6xxx, conf));
+	} else if (OCTEON_CN23XX_PF(oct)) {
+		default_oct_conf = (struct octeon_config *)
+			(CHIP_FIELD(oct, cn23xx_pf, conf));
 	}
-
 	return default_oct_conf;
 }
 
@@ -1141,7 +1204,9 @@ u64 lio_pci_readq(struct octeon_device *oct, u64 addr)
 	 * So write MSB first
 	 */
 	addrhi = (addr >> 32);
-	if ((oct->chip_id == OCTEON_CN66XX) || (oct->chip_id == OCTEON_CN68XX))
+	if ((oct->chip_id == OCTEON_CN66XX) ||
+	    (oct->chip_id == OCTEON_CN68XX) ||
+	    (oct->chip_id == OCTEON_CN23XX_PF_VID))
 		addrhi |= 0x00060000;
 	writel(addrhi, oct->reg_list.pci_win_rd_addr_hi);
 
@@ -1185,8 +1250,15 @@ int octeon_mem_access_ok(struct octeon_device *oct)
 	u64 lmc0_reset_ctl;
 
 	/* Check to make sure a DDR interface is enabled */
-	lmc0_reset_ctl = lio_pci_readq(oct, CN6XXX_LMC0_RESET_CTL);
-	access_okay = (lmc0_reset_ctl & CN6XXX_LMC0_RESET_CTL_DDR3RST_MASK);
+	if (OCTEON_CN23XX_PF(oct)) {
+		lmc0_reset_ctl = lio_pci_readq(oct, CN23XX_LMC0_RESET_CTL);
+		access_okay =
+			(lmc0_reset_ctl & CN23XX_LMC0_RESET_CTL_DDR3RST_MASK);
+	} else {
+		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;
 }
@@ -1226,3 +1298,39 @@ int lio_get_device_id(void *dev)
 			return octeon_dev->octeon_id;
 	return -1;
 }
+
+void lio_enable_irq(struct octeon_droq *droq, struct octeon_instr_queue *iq)
+{
+	u64 instr_cnt;
+	struct octeon_device *oct = NULL;
+
+	/* the whole thing needs to be atomic, ideally */
+	if (droq) {
+		spin_lock_bh(&droq->lock);
+		writel(droq->pkt_count, droq->pkts_sent_reg);
+		droq->pkt_count = 0;
+		spin_unlock_bh(&droq->lock);
+		oct = droq->oct_dev;
+	}
+	if (iq) {
+		spin_lock_bh(&iq->lock);
+		writel(iq->pkt_in_done, iq->inst_cnt_reg);
+		iq->pkt_in_done = 0;
+		spin_unlock_bh(&iq->lock);
+		oct = iq->oct_dev;
+	}
+	/*write resend. Writing RESEND in SLI_PKTX_CNTS should be enough
+	 *to trigger tx interrupts as well, if they are pending.
+	 */
+	if (oct && OCTEON_CN23XX_PF(oct)) {
+		if (droq)
+			writeq(CN23XX_INTR_RESEND, droq->pkts_sent_reg);
+		/*we race with firmrware here. read and write the IN_DONE_CNTS*/
+		else if (iq) {
+			instr_cnt =  readq(iq->inst_cnt_reg);
+			writeq(((instr_cnt & 0xFFFFFFFF00000000ULL) |
+				CN23XX_INTR_RESEND),
+			       iq->inst_cnt_reg);
+		}
+	}
+}
diff --git a/drivers/net/ethernet/cavium/liquidio/octeon_device.h b/drivers/net/ethernet/cavium/liquidio/octeon_device.h
index 01edfb404346..da15c2ae9330 100644
--- a/drivers/net/ethernet/cavium/liquidio/octeon_device.h
+++ b/drivers/net/ethernet/cavium/liquidio/octeon_device.h
@@ -30,13 +30,19 @@
 /** PCI VendorId Device Id */
 #define  OCTEON_CN68XX_PCIID          0x91177d
 #define  OCTEON_CN66XX_PCIID          0x92177d
-
+#define  OCTEON_CN23XX_PCIID_PF       0x9702177d
 /** 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
+#define  OCTEON_CN23XX_PF_VID         0x9702
+
+/**RevisionId for the chips */
+#define  OCTEON_CN23XX_REV_1_0        0x00
+#define  OCTEON_CN23XX_REV_1_1        0x01
+#define  OCTEON_CN23XX_REV_2_0        0x80
 
 /** Endian-swap modes supported by Octeon. */
 enum octeon_pci_swap_mode {
@@ -46,6 +52,9 @@ enum octeon_pci_swap_mode {
 	OCTEON_PCI_32BIT_LW_SWAP = 3
 };
 
+#define  OCTEON_OUTPUT_INTR   (2)
+#define  OCTEON_ALL_INTR      0xff
+
 /*---------------   PCI BAR1 index registers -------------*/
 
 /* BAR1 Mask */
@@ -198,9 +207,9 @@ struct octeon_fn_list {
 	void (*setup_oq_regs)(struct octeon_device *, u32);
 
 	irqreturn_t (*process_interrupt_regs)(void *);
+	u64 (*msix_interrupt_handler)(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);
@@ -209,10 +218,10 @@ struct octeon_fn_list {
 	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_interrupt)(struct octeon_device *, u8);
+	void (*disable_interrupt)(struct octeon_device *, u8);
 
-	void (*enable_io_queues)(struct octeon_device *);
+	int (*enable_io_queues)(struct octeon_device *);
 	void (*disable_io_queues)(struct octeon_device *);
 };
 
@@ -266,11 +275,72 @@ struct octdev_props {
 	/* Each interface in the Octeon device has a network
 	 * device pointer (used for OS specific calls).
 	 */
+	int    rx_on;
 	int    napi_enabled;
 	int    gmxport;
 	struct net_device *netdev;
 };
 
+#define LIO_FLAG_MSIX_ENABLED	0x1
+#define MSIX_PO_INT		0x1
+#define MSIX_PI_INT		0x2
+
+struct octeon_pf_vf_hs_word {
+#ifdef __LITTLE_ENDIAN_BITFIELD
+	/** PKIND value assigned for the DPI interface */
+	u64        pkind : 8;
+
+	/** OCTEON core clock multiplier   */
+	u64        core_tics_per_us : 16;
+
+	/** OCTEON coprocessor clock multiplier  */
+	u64        coproc_tics_per_us : 16;
+
+	/** app that currently running on OCTEON  */
+	u64        app_mode : 8;
+
+	/** RESERVED */
+	u64 reserved : 16;
+
+#else
+
+	/** RESERVED */
+	u64 reserved : 16;
+
+	/** app that currently running on OCTEON  */
+	u64        app_mode : 8;
+
+	/** OCTEON coprocessor clock multiplier  */
+	u64        coproc_tics_per_us : 16;
+
+	/** OCTEON core clock multiplier   */
+	u64        core_tics_per_us : 16;
+
+	/** PKIND value assigned for the DPI interface */
+	u64        pkind : 8;
+#endif
+};
+
+struct octeon_sriov_info {
+	/* Actual rings left for PF device */
+	u32	num_pf_rings;
+
+	/* SRN of PF usable IO queues   */
+	u32	pf_srn;
+	/* total pf rings */
+	u32	trs;
+
+};
+
+struct octeon_ioq_vector {
+	struct octeon_device   *oct_dev;
+	int		        iq_index;
+	int		        droq_index;
+	int			vector;
+	struct cpumask		affinity_mask;
+	u32			ioq_num;
+};
+
 /** The Octeon device.
  *  Each Octeon device has this structure to represent all its
  *  components.
@@ -296,7 +366,7 @@ struct octeon_device {
 	/** Octeon Chip type. */
 	u16 chip_id;
 	u16 rev_id;
-
+	u16 pf_num;
 	/** This device's id - set by the driver. */
 	u32 octeon_id;
 
@@ -305,7 +375,6 @@ struct octeon_device {
 
 	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;
@@ -395,6 +464,19 @@ struct octeon_device {
 
 	void *priv;
 
+	int num_msix_irqs;
+
+	void *msix_entries;
+
+	struct octeon_sriov_info sriov_info;
+
+	struct octeon_pf_vf_hs_word pfvf_hsword;
+
+	int msix_on;
+
+	/** IOq information of it's corresponding MSI-X interrupt. */
+	struct octeon_ioq_vector    *ioq_vector;
+
 	int rx_pause;
 	int tx_pause;
 
@@ -402,12 +484,15 @@ struct octeon_device {
 
 	/* private flags to control driver-specific features through ethtool */
 	u32 priv_flags;
+
+	void *watchdog_task;
 };
 
 #define  OCT_DRV_ONLINE 1
 #define  OCT_DRV_OFFLINE 2
 #define  OCTEON_CN6XXX(oct)           ((oct->chip_id == OCTEON_CN66XX) || \
 				       (oct->chip_id == OCTEON_CN68XX))
+#define  OCTEON_CN23XX_PF(oct)        (oct->chip_id == OCTEON_CN23XX_PF_VID)
 #define CHIP_FIELD(oct, TYPE, field)             \
 	(((struct octeon_ ## TYPE  *)(oct->chip))->field)
 
@@ -661,13 +746,24 @@ void *oct_get_config_info(struct octeon_device *oct, u16 card_type);
  */
 struct octeon_config *octeon_get_conf(struct octeon_device *oct);
 
+void octeon_free_ioq_vector(struct octeon_device *oct);
+int octeon_allocate_ioq_vector(struct octeon_device  *oct);
+void lio_enable_irq(struct octeon_droq *droq, struct octeon_instr_queue *iq);
+
 /* LiquidIO driver pivate flags */
 enum {
 	OCT_PRIV_FLAG_TX_BYTES = 0, /* Tx interrupts by pending byte count */
 };
 
-static inline void lio_set_priv_flag(struct octeon_device *octdev, u32 flag,
-				     u32 val)
+#define OCT_PRIV_FLAG_DEFAULT 0x0
+
+static inline u32 lio_get_priv_flag(struct octeon_device *octdev, u32 flag)
+{
+	return !!(octdev->priv_flags & (0x1 << flag));
+}
+
+static inline void lio_set_priv_flag(struct octeon_device *octdev,
+				     u32 flag, u32 val)
 {
 	if (val)
 		octdev->priv_flags |= (0x1 << flag);
diff --git a/drivers/net/ethernet/cavium/liquidio/octeon_droq.c b/drivers/net/ethernet/cavium/liquidio/octeon_droq.c
index e0afe4c1fd01..f60e5320daf4 100644
--- a/drivers/net/ethernet/cavium/liquidio/octeon_droq.c
+++ b/drivers/net/ethernet/cavium/liquidio/octeon_droq.c
@@ -31,6 +31,7 @@
 #include "octeon_network.h"
 #include "cn66xx_regs.h"
 #include "cn66xx_device.h"
+#include "cn23xx_pf_device.h"
 
 #define     CVM_MIN(d1, d2)           (((d1) < (d2)) ? (d1) : (d2))
 #define     CVM_MAX(d1, d2)           (((d1) > (d2)) ? (d1) : (d2))
@@ -92,22 +93,25 @@ static inline void *octeon_get_dispatch_arg(struct octeon_device *octeon_dev,
 	return fn_arg;
 }
 
-/** Check for packets on Droq. This function should be called with
- * lock held.
+/** Check for packets on Droq. This function should be called with lock held.
  *  @param  droq - Droq on which count is checked.
  *  @return Returns packet count.
  */
 u32 octeon_droq_check_hw_for_pkts(struct octeon_droq *droq)
 {
 	u32 pkt_count = 0;
+	u32 last_count;
 
 	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;
+	last_count = pkt_count - droq->pkt_count;
+	droq->pkt_count = pkt_count;
+
+	/* we shall write to cnts  at napi irq enable or end of droq tasklet */
+	if (last_count)
+		atomic_add(last_count, &droq->pkts_pending);
+
+	return last_count;
 }
 
 static void octeon_droq_compute_max_packet_bufs(struct octeon_droq *droq)
@@ -259,6 +263,11 @@ int octeon_init_droq(struct octeon_device *oct,
 		c_pkts_per_intr = (u32)CFG_GET_OQ_PKTS_PER_INTR(conf6x);
 		c_refill_threshold =
 			(u32)CFG_GET_OQ_REFILL_THRESHOLD(conf6x);
+	} else if (OCTEON_CN23XX_PF(oct)) {
+		struct octeon_config *conf23 = CHIP_FIELD(oct, cn23xx_pf, conf);
+
+		c_pkts_per_intr = (u32)CFG_GET_OQ_PKTS_PER_INTR(conf23);
+		c_refill_threshold = (u32)CFG_GET_OQ_REFILL_THRESHOLD(conf23);
 	} else {
 		return 1;
 	}
@@ -564,7 +573,7 @@ octeon_droq_dispatch_pkt(struct octeon_device *oct,
 			(unsigned int)rh->r.opcode,
 			(unsigned int)rh->r.subcode);
 		droq->stats.dropped_nodispatch++;
-	}                       /* else (dispatch_fn ... */
+	}
 
 	return cnt;
 }
@@ -735,16 +744,20 @@ octeon_droq_process_packets(struct octeon_device *oct,
 	u32 pkt_count = 0, pkts_processed = 0;
 	struct list_head *tmp, *tmp2;
 
+	/* Grab the droq lock */
+	spin_lock(&droq->lock);
+
+	octeon_droq_check_hw_for_pkts(droq);
 	pkt_count = atomic_read(&droq->pkts_pending);
-	if (!pkt_count)
+
+	if (!pkt_count) {
+		spin_unlock(&droq->lock);
 		return 0;
+	}
 
 	if (pkt_count > budget)
 		pkt_count = budget;
 
-	/* Grab the droq lock */
-	spin_lock(&droq->lock);
-
 	pkts_processed = octeon_droq_fast_process_packets(oct, droq, pkt_count);
 
 	atomic_sub(pkts_processed, &droq->pkts_pending);
@@ -789,6 +802,8 @@ octeon_droq_process_poll_pkts(struct octeon_device *oct,
 	spin_lock(&droq->lock);
 
 	while (total_pkts_processed < budget) {
+		octeon_droq_check_hw_for_pkts(droq);
+
 		pkts_available =
 			CVM_MIN((budget - total_pkts_processed),
 				(u32)(atomic_read(&droq->pkts_pending)));
@@ -803,8 +818,6 @@ octeon_droq_process_poll_pkts(struct octeon_device *oct,
 		atomic_sub(pkts_processed, &droq->pkts_pending);
 
 		total_pkts_processed += pkts_processed;
-
-		octeon_droq_check_hw_for_pkts(droq);
 	}
 
 	spin_unlock(&droq->lock);
@@ -874,8 +887,11 @@ octeon_process_droq_poll_cmd(struct octeon_device *oct, u32 q_no, int cmd,
 			return 0;
 		}
 		break;
+		case OCTEON_CN23XX_PF_VID: {
+			lio_enable_irq(oct->droq[q_no], oct->instr_queue[q_no]);
+		}
+		break;
 		}
-
 		return 0;
 	}
 
diff --git a/drivers/net/ethernet/cavium/liquidio/octeon_droq.h b/drivers/net/ethernet/cavium/liquidio/octeon_droq.h
index 5a6fb9113bbd..5be002d5dba4 100644
--- a/drivers/net/ethernet/cavium/liquidio/octeon_droq.h
+++ b/drivers/net/ethernet/cavium/liquidio/octeon_droq.h
@@ -261,6 +261,8 @@ struct octeon_droq {
 
 	u32 q_no;
 
+	u32 pkt_count;
+
 	struct octeon_droq_ops ops;
 
 	struct octeon_device *oct_dev;
diff --git a/drivers/net/ethernet/cavium/liquidio/octeon_iq.h b/drivers/net/ethernet/cavium/liquidio/octeon_iq.h
index ff4b1d6f007b..e4d426ba18dc 100644
--- a/drivers/net/ethernet/cavium/liquidio/octeon_iq.h
+++ b/drivers/net/ethernet/cavium/liquidio/octeon_iq.h
@@ -88,6 +88,8 @@ struct octeon_instr_queue {
 	/** A spinlock to protect while posting on the ring.  */
 	spinlock_t post_lock;
 
+	u32 pkt_in_done;
+
 	/** A spinlock to protect access to the input ring.*/
 	spinlock_t iq_flush_running_lock;
 
diff --git a/drivers/net/ethernet/cavium/liquidio/octeon_main.h b/drivers/net/ethernet/cavium/liquidio/octeon_main.h
index bc14e4c27332..366298f7bcb2 100644
--- a/drivers/net/ethernet/cavium/liquidio/octeon_main.h
+++ b/drivers/net/ethernet/cavium/liquidio/octeon_main.h
@@ -38,12 +38,26 @@
 
 #define DRV_NAME "LiquidIO"
 
-/**
- * \brief determines if a given console has debug enabled.
- * @param console console to check
- * @returns  1 = enabled. 0 otherwise
+/** 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.
  */
-int octeon_console_debug_enabled(u32 console);
+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;
+};
 
 /* BQL-related functions */
 void octeon_report_sent_bytes_to_bql(void *buf, int reqtype);
@@ -167,22 +181,26 @@ cnnic_numa_alloc_aligned_dma(u32 size,
 #define cnnic_free_aligned_dma(pci_dev, ptr, size, orig_ptr, dma_addr) \
 		free_pages(orig_ptr, get_order(size))
 
-static inline void
+static inline int
 sleep_cond(wait_queue_head_t *wait_queue, int *condition)
 {
+	int errno = 0;
 	wait_queue_t we;
 
 	init_waitqueue_entry(&we, current);
 	add_wait_queue(wait_queue, &we);
 	while (!(READ_ONCE(*condition))) {
 		set_current_state(TASK_INTERRUPTIBLE);
-		if (signal_pending(current))
+		if (signal_pending(current)) {
+			errno = -EINTR;
 			goto out;
+		}
 		schedule();
 	}
 out:
 	set_current_state(TASK_RUNNING);
 	remove_wait_queue(wait_queue, &we);
+	return errno;
 }
 
 static inline void
diff --git a/drivers/net/ethernet/cavium/liquidio/octeon_mem_ops.c b/drivers/net/ethernet/cavium/liquidio/octeon_mem_ops.c
index 95a4bbedf557..0dc081a99b30 100644
--- a/drivers/net/ethernet/cavium/liquidio/octeon_mem_ops.c
+++ b/drivers/net/ethernet/cavium/liquidio/octeon_mem_ops.c
@@ -19,7 +19,6 @@
  * This file may also be available under a different license from Cavium.
  * Contact Cavium, Inc. for more information
  **********************************************************************/
-#include <linux/pci.h>
 #include <linux/netdevice.h>
 #include "liquidio_common.h"
 #include "octeon_droq.h"
diff --git a/drivers/net/ethernet/cavium/liquidio/octeon_network.h b/drivers/net/ethernet/cavium/liquidio/octeon_network.h
index fb820dc7fcb7..e5d1debd05ad 100644
--- a/drivers/net/ethernet/cavium/liquidio/octeon_network.h
+++ b/drivers/net/ethernet/cavium/liquidio/octeon_network.h
@@ -26,8 +26,6 @@
 
 #ifndef __OCTEON_NETWORK_H__
 #define __OCTEON_NETWORK_H__
-#include <linux/version.h>
-#include <linux/dma-mapping.h>
 #include <linux/ptp_clock_kernel.h>
 
 #define LIO_MAX_MTU_SIZE (OCTNET_MAX_FRM_SIZE - OCTNET_FRM_HEADER_SIZE)
@@ -124,11 +122,21 @@ struct lio {
 
 	/* work queue for  txq status */
 	struct cavium_wq	txq_status_wq;
+
+	/* work queue for  link status */
+	struct cavium_wq	link_status_wq;
+
 };
 
 #define LIO_SIZE         (sizeof(struct lio))
 #define GET_LIO(netdev)  ((struct lio *)netdev_priv(netdev))
 
+#define CIU3_WDOG(c)                 (0x1010000020000ULL + (c << 3))
+#define CIU3_WDOG_MASK               12ULL
+#define LIO_MONITOR_WDOG_EXPIRE      1
+#define LIO_MONITOR_CORE_STUCK_MSGD  2
+#define LIO_MAX_CORES                12
+
 /**
  * \brief Enable or disable feature
  * @param netdev    pointer to network device
diff --git a/drivers/net/ethernet/cavium/liquidio/octeon_nic.c b/drivers/net/ethernet/cavium/liquidio/octeon_nic.c
index 166727be928f..40ac1fe88956 100644
--- a/drivers/net/ethernet/cavium/liquidio/octeon_nic.c
+++ b/drivers/net/ethernet/cavium/liquidio/octeon_nic.c
@@ -19,7 +19,6 @@
  * This file may also be available under a different license from Cavium.
  * Contact Cavium, Inc. for more information
  **********************************************************************/
-#include <linux/interrupt.h>
 #include <linux/pci.h>
 #include <linux/netdevice.h>
 #include "liquidio_common.h"
@@ -36,6 +35,7 @@ octeon_alloc_soft_command_resp(struct octeon_device    *oct,
 			       u32		       rdatasize)
 {
 	struct octeon_soft_command *sc;
+	struct octeon_instr_ih3  *ih3;
 	struct octeon_instr_ih2  *ih2;
 	struct octeon_instr_irh *irh;
 	struct octeon_instr_rdp *rdp;
@@ -52,10 +52,19 @@ octeon_alloc_soft_command_resp(struct octeon_device    *oct,
 	/* Add in the response related fields. Opcode and Param are already
 	 * there.
 	 */
-	ih2      = (struct octeon_instr_ih2 *)&sc->cmd.cmd2.ih2;
-	rdp     = (struct octeon_instr_rdp *)&sc->cmd.cmd2.rdp;
-	irh     = (struct octeon_instr_irh *)&sc->cmd.cmd2.irh;
-	ih2->fsz = 40; /* irh + ossp[0] + ossp[1] + rdp + rptr = 40 bytes */
+	if (OCTEON_CN23XX_PF(oct)) {
+		ih3      = (struct octeon_instr_ih3 *)&sc->cmd.cmd3.ih3;
+		rdp     = (struct octeon_instr_rdp *)&sc->cmd.cmd3.rdp;
+		irh     = (struct octeon_instr_irh *)&sc->cmd.cmd3.irh;
+		/*pkiih3 + irh + ossp[0] + ossp[1] + rdp + rptr = 40 bytes */
+		ih3->fsz = LIO_SOFTCMDRESP_IH3;
+	} else {
+		ih2      = (struct octeon_instr_ih2 *)&sc->cmd.cmd2.ih2;
+		rdp     = (struct octeon_instr_rdp *)&sc->cmd.cmd2.rdp;
+		irh     = (struct octeon_instr_irh *)&sc->cmd.cmd2.irh;
+		/* irh + ossp[0] + ossp[1] + rdp + rptr = 40 bytes */
+		ih2->fsz = LIO_SOFTCMDRESP_IH2;
+	}
 
 	irh->rflag = 1; /* a response is required */
 
@@ -64,7 +73,10 @@ octeon_alloc_soft_command_resp(struct octeon_device    *oct,
 
 	*sc->status_word = COMPLETION_WORD_INIT;
 
-	sc->cmd.cmd2.rptr =  sc->dmarptr;
+	if (OCTEON_CN23XX_PF(oct))
+		sc->cmd.cmd3.rptr =  sc->dmarptr;
+	else
+		sc->cmd.cmd2.rptr =  sc->dmarptr;
 
 	sc->wait_time = 1000;
 	sc->timeout = jiffies + sc->wait_time;
@@ -73,12 +85,9 @@ octeon_alloc_soft_command_resp(struct octeon_device    *oct,
 }
 
 int octnet_send_nic_data_pkt(struct octeon_device *oct,
-			     struct octnic_data_pkt *ndata,
-			     u32 xmit_more)
+			     struct octnic_data_pkt *ndata)
 {
-	int ring_doorbell;
-
-	ring_doorbell = !xmit_more;
+	int ring_doorbell = 1;
 
 	return octeon_send_command(oct, ndata->q_no, ring_doorbell, &ndata->cmd,
 				   ndata->buf, ndata->datasize,
@@ -183,8 +192,8 @@ octnet_send_nic_ctrl_pkt(struct octeon_device *oct,
 	retval = octeon_send_soft_command(oct, sc);
 	if (retval == IQ_SEND_FAILED) {
 		octeon_free_soft_command(oct, sc);
-		dev_err(&oct->pci_dev->dev, "%s soft command:%d send failed status: %x\n",
-			__func__, nctrl->ncmd.s.cmd, retval);
+		dev_err(&oct->pci_dev->dev, "%s pf_num:%d soft command:%d send failed status: %x\n",
+			__func__, oct->pf_num, nctrl->ncmd.s.cmd, retval);
 		spin_unlock_bh(&oct->cmd_resp_wqlock);
 		return -1;
 	}
diff --git a/drivers/net/ethernet/cavium/liquidio/octeon_nic.h b/drivers/net/ethernet/cavium/liquidio/octeon_nic.h
index b71a2bbe4bee..4b8da67b995f 100644
--- a/drivers/net/ethernet/cavium/liquidio/octeon_nic.h
+++ b/drivers/net/ethernet/cavium/liquidio/octeon_nic.h
@@ -138,7 +138,7 @@ octnet_prepare_pci_cmd_o2(struct octeon_device *oct,
 	/* assume that rflag is cleared so therefore front data will only have
 	 * irh and ossp[0], ossp[1] for a total of 32 bytes
 	 */
-	ih2->fsz = 24;
+	ih2->fsz = LIO_PCICMD_O2;
 
 	ih2->tagtype = ORDERED_TAG;
 	ih2->grp = DEFAULT_POW_GRP;
@@ -196,7 +196,7 @@ octnet_prepare_pci_cmd_o3(struct octeon_device *oct,
 	 */
 	ih3->pkind       = oct->instr_queue[setup->s.iq_no]->txpciq.s.pkind;
 	/*PKI IH*/
-	ih3->fsz = 24 + 8;
+	ih3->fsz = LIO_PCICMD_O3;
 
 	if (!setup->s.gather) {
 		ih3->dlengsz = setup->s.u.datasize;
@@ -278,7 +278,7 @@ octeon_alloc_soft_command_resp(struct octeon_device    *oct,
  * 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);
+			     struct octnic_data_pkt *ndata);
 
 /** Send a NIC control packet to the device
  * @param oct - octeon device pointer
diff --git a/drivers/net/ethernet/cavium/liquidio/request_manager.c b/drivers/net/ethernet/cavium/liquidio/request_manager.c
index d32492f185ff..90866bb50033 100644
--- a/drivers/net/ethernet/cavium/liquidio/request_manager.c
+++ b/drivers/net/ethernet/cavium/liquidio/request_manager.c
@@ -30,6 +30,7 @@
 #include "octeon_main.h"
 #include "octeon_network.h"
 #include "cn66xx_device.h"
+#include "cn23xx_pf_device.h"
 
 #define INCR_INSTRQUEUE_PKT_COUNT(octeon_dev_ptr, iq_no, field, count)  \
 	(octeon_dev_ptr->instr_queue[iq_no]->stats.field += count)
@@ -71,7 +72,8 @@ int octeon_init_instr_queue(struct octeon_device *oct,
 
 	if (OCTEON_CN6XXX(oct))
 		conf = &(CFG_GET_IQ_CFG(CHIP_FIELD(oct, cn6xxx, conf)));
-
+	else if (OCTEON_CN23XX_PF(oct))
+		conf = &(CFG_GET_IQ_CFG(CHIP_FIELD(oct, cn23xx_pf, conf)));
 	if (!conf) {
 		dev_err(&oct->pci_dev->dev, "Unsupported Chip %x\n",
 			oct->chip_id);
@@ -88,6 +90,7 @@ int octeon_init_instr_queue(struct octeon_device *oct,
 	q_size = (u32)conf->instr_type * num_descs;
 
 	iq = oct->instr_queue[iq_no];
+
 	iq->oct_dev = oct;
 
 	set_dev_node(&oct->pci_dev->dev, numa_node);
@@ -181,6 +184,9 @@ int octeon_delete_instr_queue(struct octeon_device *oct, u32 iq_no)
 	if (OCTEON_CN6XXX(oct))
 		desc_size =
 		    CFG_GET_IQ_INSTR_TYPE(CHIP_FIELD(oct, cn6xxx, conf));
+	else if (OCTEON_CN23XX_PF(oct))
+		desc_size =
+		    CFG_GET_IQ_INSTR_TYPE(CHIP_FIELD(oct, cn23xx_pf, conf));
 
 	vfree(iq->request_list);
 
@@ -383,7 +389,12 @@ lio_process_iq_request_list(struct octeon_device *oct,
 		case REQTYPE_SOFT_COMMAND:
 			sc = buf;
 
-			irh = (struct octeon_instr_irh *)&sc->cmd.cmd2.irh;
+			if (OCTEON_CN23XX_PF(oct))
+				irh = (struct octeon_instr_irh *)
+					&sc->cmd.cmd3.irh;
+			else
+				irh = (struct octeon_instr_irh *)
+					&sc->cmd.cmd2.irh;
 			if (irh->rflag) {
 				/* We're expecting a response from Octeon.
 				 * It's up to lio_process_ordered_list() to
@@ -499,6 +510,7 @@ static void __check_db_timeout(struct octeon_device *oct, u64 iq_no)
 
 	if (!oct)
 		return;
+
 	iq = oct->instr_queue[iq_no];
 	if (!iq)
 		return;
@@ -514,6 +526,8 @@ static void __check_db_timeout(struct octeon_device *oct, u64 iq_no)
 
 	/* Flush the instruction queue */
 	octeon_flush_iq(oct, iq, 1, 0);
+
+	lio_enable_irq(NULL, iq);
 }
 
 /* Called by the Poll thread at regular intervals to check the instruction
@@ -580,6 +594,8 @@ octeon_prepare_soft_command(struct octeon_device *oct,
 {
 	struct octeon_config *oct_cfg;
 	struct octeon_instr_ih2 *ih2;
+	struct octeon_instr_ih3 *ih3;
+	struct octeon_instr_pki_ih3 *pki_ih3;
 	struct octeon_instr_irh *irh;
 	struct octeon_instr_rdp *rdp;
 
@@ -588,36 +604,88 @@ octeon_prepare_soft_command(struct octeon_device *oct,
 
 	oct_cfg = octeon_get_conf(oct);
 
-	ih2          = (struct octeon_instr_ih2 *)&sc->cmd.cmd2.ih2;
-	ih2->tagtype = ATOMIC_TAG;
-	ih2->tag     = LIO_CONTROL;
-	ih2->raw     = 1;
-	ih2->grp     = CFG_GET_CTRL_Q_GRP(oct_cfg);
-
-	if (sc->datasize) {
-		ih2->dlengsz = sc->datasize;
-		ih2->rs = 1;
-	}
-
-	irh            = (struct octeon_instr_irh *)&sc->cmd.cmd2.irh;
-	irh->opcode    = opcode;
-	irh->subcode   = subcode;
-
-	/* opcode/subcode specific parameters (ossp) */
-	irh->ossp       = irh_ossp;
-	sc->cmd.cmd2.ossp[0] = ossp0;
-	sc->cmd.cmd2.ossp[1] = ossp1;
-
-	if (sc->rdatasize) {
-		rdp = (struct octeon_instr_rdp *)&sc->cmd.cmd2.rdp;
-		rdp->pcie_port = oct->pcie_port;
-		rdp->rlen      = sc->rdatasize;
+	if (OCTEON_CN23XX_PF(oct)) {
+		ih3 = (struct octeon_instr_ih3 *)&sc->cmd.cmd3.ih3;
+
+		ih3->pkind = oct->instr_queue[sc->iq_no]->txpciq.s.pkind;
+
+		pki_ih3 = (struct octeon_instr_pki_ih3 *)&sc->cmd.cmd3.pki_ih3;
+
+		pki_ih3->w           = 1;
+		pki_ih3->raw         = 1;
+		pki_ih3->utag        = 1;
+		pki_ih3->uqpg        =
+			oct->instr_queue[sc->iq_no]->txpciq.s.use_qpg;
+		pki_ih3->utt         = 1;
+		pki_ih3->tag     = LIO_CONTROL;
+		pki_ih3->tagtype = ATOMIC_TAG;
+		pki_ih3->qpg         =
+			oct->instr_queue[sc->iq_no]->txpciq.s.qpg;
+		pki_ih3->pm          = 0x7;
+		pki_ih3->sl          = 8;
+
+		if (sc->datasize)
+			ih3->dlengsz = sc->datasize;
+
+		irh            = (struct octeon_instr_irh *)&sc->cmd.cmd3.irh;
+		irh->opcode    = opcode;
+		irh->subcode   = subcode;
+
+		/* opcode/subcode specific parameters (ossp) */
+		irh->ossp       = irh_ossp;
+		sc->cmd.cmd3.ossp[0] = ossp0;
+		sc->cmd.cmd3.ossp[1] = ossp1;
+
+		if (sc->rdatasize) {
+			rdp = (struct octeon_instr_rdp *)&sc->cmd.cmd3.rdp;
+			rdp->pcie_port = oct->pcie_port;
+			rdp->rlen      = sc->rdatasize;
+
+			irh->rflag =  1;
+			/*PKI IH3*/
+			/* pki_ih3 irh+ossp[0]+ossp[1]+rdp+rptr = 48 bytes */
+			ih3->fsz    = LIO_SOFTCMDRESP_IH3;
+		} else {
+			irh->rflag =  0;
+			/*PKI IH3*/
+			/* pki_h3 + irh + ossp[0] + ossp[1] = 32 bytes */
+			ih3->fsz    = LIO_PCICMD_O3;
+		}
 
-		irh->rflag =  1;
-		ih2->fsz   = 40; /* irh+ossp[0]+ossp[1]+rdp+rptr = 40 bytes */
 	} else {
-		irh->rflag =  0;
-		ih2->fsz   = 24; /* irh + ossp[0] + ossp[1] = 24 bytes */
+		ih2          = (struct octeon_instr_ih2 *)&sc->cmd.cmd2.ih2;
+		ih2->tagtype = ATOMIC_TAG;
+		ih2->tag     = LIO_CONTROL;
+		ih2->raw     = 1;
+		ih2->grp     = CFG_GET_CTRL_Q_GRP(oct_cfg);
+
+		if (sc->datasize) {
+			ih2->dlengsz = sc->datasize;
+			ih2->rs = 1;
+		}
+
+		irh            = (struct octeon_instr_irh *)&sc->cmd.cmd2.irh;
+		irh->opcode    = opcode;
+		irh->subcode   = subcode;
+
+		/* opcode/subcode specific parameters (ossp) */
+		irh->ossp       = irh_ossp;
+		sc->cmd.cmd2.ossp[0] = ossp0;
+		sc->cmd.cmd2.ossp[1] = ossp1;
+
+		if (sc->rdatasize) {
+			rdp = (struct octeon_instr_rdp *)&sc->cmd.cmd2.rdp;
+			rdp->pcie_port = oct->pcie_port;
+			rdp->rlen      = sc->rdatasize;
+
+			irh->rflag =  1;
+			/* irh+ossp[0]+ossp[1]+rdp+rptr = 40 bytes */
+			ih2->fsz   = LIO_SOFTCMDRESP_IH2;
+		} else {
+			irh->rflag =  0;
+			/* irh + ossp[0] + ossp[1] = 24 bytes */
+			ih2->fsz   = LIO_PCICMD_O2;
+		}
 	}
 }
 
@@ -625,23 +693,39 @@ int octeon_send_soft_command(struct octeon_device *oct,
 			     struct octeon_soft_command *sc)
 {
 	struct octeon_instr_ih2 *ih2;
+	struct octeon_instr_ih3 *ih3;
 	struct octeon_instr_irh *irh;
 	u32 len;
 
-	ih2 = (struct octeon_instr_ih2 *)&sc->cmd.cmd2.ih2;
-	if (ih2->dlengsz) {
-		WARN_ON(!sc->dmadptr);
-		sc->cmd.cmd2.dptr = sc->dmadptr;
-	}
-	irh = (struct octeon_instr_irh *)&sc->cmd.cmd2.irh;
-	if (irh->rflag) {
-		WARN_ON(!sc->dmarptr);
-		WARN_ON(!sc->status_word);
-		*sc->status_word = COMPLETION_WORD_INIT;
-
-		sc->cmd.cmd2.rptr = sc->dmarptr;
+	if (OCTEON_CN23XX_PF(oct)) {
+		ih3 =  (struct octeon_instr_ih3 *)&sc->cmd.cmd3.ih3;
+		if (ih3->dlengsz) {
+			WARN_ON(!sc->dmadptr);
+			sc->cmd.cmd3.dptr = sc->dmadptr;
+		}
+		irh = (struct octeon_instr_irh *)&sc->cmd.cmd3.irh;
+		if (irh->rflag) {
+			WARN_ON(!sc->dmarptr);
+			WARN_ON(!sc->status_word);
+			*sc->status_word = COMPLETION_WORD_INIT;
+			sc->cmd.cmd3.rptr = sc->dmarptr;
+		}
+		len = (u32)ih3->dlengsz;
+	} else {
+		ih2 = (struct octeon_instr_ih2 *)&sc->cmd.cmd2.ih2;
+		if (ih2->dlengsz) {
+			WARN_ON(!sc->dmadptr);
+			sc->cmd.cmd2.dptr = sc->dmadptr;
+		}
+		irh = (struct octeon_instr_irh *)&sc->cmd.cmd2.irh;
+		if (irh->rflag) {
+			WARN_ON(!sc->dmarptr);
+			WARN_ON(!sc->status_word);
+			*sc->status_word = COMPLETION_WORD_INIT;
+			sc->cmd.cmd2.rptr = sc->dmarptr;
+		}
+		len = (u32)ih2->dlengsz;
 	}
-	len = (u32)ih2->dlengsz;
 
 	if (sc->wait_time)
 		sc->timeout = jiffies + sc->wait_time;
diff --git a/drivers/net/ethernet/cavium/liquidio/response_manager.c b/drivers/net/ethernet/cavium/liquidio/response_manager.c
index 709049e36627..be52178d8cb6 100644
--- a/drivers/net/ethernet/cavium/liquidio/response_manager.c
+++ b/drivers/net/ethernet/cavium/liquidio/response_manager.c
@@ -91,8 +91,13 @@ int lio_process_ordered_list(struct octeon_device *octeon_dev,
 
 		sc = (struct octeon_soft_command *)ordered_sc_list->
 		    head.next;
-		rdp = (struct octeon_instr_rdp *)&sc->cmd.cmd2.rdp;
-		rptr = sc->cmd.cmd2.rptr;
+		if (OCTEON_CN23XX_PF(octeon_dev)) {
+			rdp = (struct octeon_instr_rdp *)&sc->cmd.cmd3.rdp;
+			rptr = sc->cmd.cmd3.rptr;
+		} else {
+			rdp = (struct octeon_instr_rdp *)&sc->cmd.cmd2.rdp;
+			rptr = sc->cmd.cmd2.rptr;
+		}
 
 		status = OCTEON_REQUEST_PENDING;
 
diff --git a/drivers/net/ethernet/cavium/thunder/Makefile b/drivers/net/ethernet/cavium/thunder/Makefile
index 5c4615ccaa14..6b4d4add7353 100644
--- a/drivers/net/ethernet/cavium/thunder/Makefile
+++ b/drivers/net/ethernet/cavium/thunder/Makefile
@@ -2,6 +2,7 @@
 # Makefile for Cavium's Thunder ethernet device
 #
 
+obj-$(CONFIG_THUNDER_NIC_RGX) += thunder_xcv.o
 obj-$(CONFIG_THUNDER_NIC_BGX) += thunder_bgx.o
 obj-$(CONFIG_THUNDER_NIC_PF) += nicpf.o
 obj-$(CONFIG_THUNDER_NIC_VF) += nicvf.o
diff --git a/drivers/net/ethernet/cavium/thunder/nic.h b/drivers/net/ethernet/cavium/thunder/nic.h
index e29815d9e6f4..30426109711c 100644
--- a/drivers/net/ethernet/cavium/thunder/nic.h
+++ b/drivers/net/ethernet/cavium/thunder/nic.h
@@ -20,6 +20,17 @@
 #define	PCI_DEVICE_ID_THUNDER_NIC_VF		0xA034
 #define	PCI_DEVICE_ID_THUNDER_BGX		0xA026
 
+/* Subsystem device IDs */
+#define PCI_SUBSYS_DEVID_88XX_NIC_PF		0xA11E
+#define PCI_SUBSYS_DEVID_81XX_NIC_PF		0xA21E
+#define PCI_SUBSYS_DEVID_83XX_NIC_PF		0xA31E
+
+#define PCI_SUBSYS_DEVID_88XX_PASS1_NIC_VF	0xA11E
+#define PCI_SUBSYS_DEVID_88XX_NIC_VF		0xA134
+#define PCI_SUBSYS_DEVID_81XX_NIC_VF		0xA234
+#define PCI_SUBSYS_DEVID_83XX_NIC_VF		0xA334
+
+
 /* PCI BAR nos */
 #define	PCI_CFG_REG_BAR_NUM		0
 #define	PCI_MSIX_REG_BAR_NUM		4
@@ -41,40 +52,8 @@
 /* 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)
+/* Max when CPI_ALG is IP diffserv */
+#define	NIC_MAX_CPI_PER_LMAC		64
 
 /* NIC VF Interrupts */
 #define	NICVF_INTR_CQ			0
@@ -148,7 +127,6 @@ struct nicvf_cq_poll {
 	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 */
@@ -273,6 +251,7 @@ struct nicvf {
 	struct net_device	*netdev;
 	struct pci_dev		*pdev;
 	void __iomem		*reg_base;
+#define	MAX_QUEUES_PER_QSET			8
 	struct queue_set	*qs;
 	struct nicvf_cq_poll	*napi[8];
 	u8			vf_id;
@@ -326,7 +305,7 @@ struct nicvf {
 	bool			msix_enabled;
 	u8			num_vec;
 	struct msix_entry	msix_entries[NIC_VF_MSIX_VECTORS];
-	char			irq_name[NIC_VF_MSIX_VECTORS][20];
+	char			irq_name[NIC_VF_MSIX_VECTORS][IFNAMSIZ + 15];
 	bool			irq_allocated[NIC_VF_MSIX_VECTORS];
 	cpumask_var_t		affinity_mask[NIC_VF_MSIX_VECTORS];
 
@@ -369,6 +348,7 @@ struct nicvf {
 #define	NIC_MBOX_MSG_PNICVF_PTR		0x14	/* Get primary qset nicvf ptr */
 #define	NIC_MBOX_MSG_SNICVF_PTR		0x15	/* Send sqet nicvf ptr to PVF */
 #define	NIC_MBOX_MSG_LOOPBACK		0x16	/* Set interface in loopback */
+#define	NIC_MBOX_MSG_RESET_STAT_COUNTER 0x17	/* Reset statistics counters */
 #define	NIC_MBOX_MSG_CFG_DONE		0xF0	/* VF configuration done */
 #define	NIC_MBOX_MSG_SHUTDOWN		0xF1	/* VF is being shutdown */
 
@@ -485,6 +465,31 @@ struct set_loopback {
 	bool  enable;
 };
 
+/* Reset statistics counters */
+struct reset_stat_cfg {
+	u8    msg;
+	/* Bitmap to select NIC_PF_VNIC(vf_id)_RX_STAT(0..13) */
+	u16   rx_stat_mask;
+	/* Bitmap to select NIC_PF_VNIC(vf_id)_TX_STAT(0..4) */
+	u8    tx_stat_mask;
+	/* Bitmap to select NIC_PF_QS(0..127)_RQ(0..7)_STAT(0..1)
+	 * bit14, bit15 NIC_PF_QS(vf_id)_RQ7_STAT(0..1)
+	 * bit12, bit13 NIC_PF_QS(vf_id)_RQ6_STAT(0..1)
+	 * ..
+	 * bit2, bit3 NIC_PF_QS(vf_id)_RQ1_STAT(0..1)
+	 * bit0, bit1 NIC_PF_QS(vf_id)_RQ0_STAT(0..1)
+	 */
+	u16   rq_stat_mask;
+	/* Bitmap to select NIC_PF_QS(0..127)_SQ(0..7)_STAT(0..1)
+	 * bit14, bit15 NIC_PF_QS(vf_id)_SQ7_STAT(0..1)
+	 * bit12, bit13 NIC_PF_QS(vf_id)_SQ6_STAT(0..1)
+	 * ..
+	 * bit2, bit3 NIC_PF_QS(vf_id)_SQ1_STAT(0..1)
+	 * bit0, bit1 NIC_PF_QS(vf_id)_SQ0_STAT(0..1)
+	 */
+	u16   sq_stat_mask;
+};
+
 /* 128 bit shared memory between PF and each VF */
 union nic_mbx {
 	struct { u8 msg; }	msg;
@@ -502,6 +507,7 @@ union nic_mbx {
 	struct sqs_alloc        sqs_alloc;
 	struct nicvf_ptr	nicvf;
 	struct set_loopback	lbk;
+	struct reset_stat_cfg	reset_stat;
 };
 
 #define NIC_NODE_ID_MASK	0x03
@@ -515,7 +521,14 @@ static inline int nic_get_node_id(struct pci_dev *pdev)
 
 static inline bool pass1_silicon(struct pci_dev *pdev)
 {
-	return pdev->revision < 8;
+	return (pdev->revision < 8) &&
+		(pdev->subsystem_device == PCI_SUBSYS_DEVID_88XX_NIC_PF);
+}
+
+static inline bool pass2_silicon(struct pci_dev *pdev)
+{
+	return (pdev->revision >= 8) &&
+		(pdev->subsystem_device == PCI_SUBSYS_DEVID_88XX_NIC_PF);
 }
 
 int nicvf_set_real_num_queues(struct net_device *netdev,
diff --git a/drivers/net/ethernet/cavium/thunder/nic_main.c b/drivers/net/ethernet/cavium/thunder/nic_main.c
index 85cc782b9060..2bbf4cbf08b2 100644
--- a/drivers/net/ethernet/cavium/thunder/nic_main.c
+++ b/drivers/net/ethernet/cavium/thunder/nic_main.c
@@ -20,8 +20,25 @@
 #define DRV_NAME	"thunder-nic"
 #define DRV_VERSION	"1.0"
 
+struct hw_info {
+	u8		bgx_cnt;
+	u8		chans_per_lmac;
+	u8		chans_per_bgx; /* Rx/Tx chans */
+	u8		chans_per_rgx;
+	u8		chans_per_lbk;
+	u16		cpi_cnt;
+	u16		rssi_cnt;
+	u16		rss_ind_tbl_size;
+	u16		tl4_cnt;
+	u16		tl3_cnt;
+	u8		tl2_cnt;
+	u8		tl1_cnt;
+	bool		tl1_per_bgx; /* TL1 per BGX or per LMAC */
+};
+
 struct nicpf {
 	struct pci_dev		*pdev;
+	struct hw_info          *hw;
 	u8			node;
 	unsigned int		flags;
 	u8			num_vf_en;      /* No of VF enabled */
@@ -36,22 +53,22 @@ struct nicpf {
 #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];
+	u8			*vf_lmac_map;
 	struct delayed_work     dwork;
 	struct workqueue_struct *check_link;
-	u8			link[MAX_LMAC];
-	u8			duplex[MAX_LMAC];
-	u32			speed[MAX_LMAC];
+	u8			*link;
+	u8			*duplex;
+	u32			*speed;
 	u16			cpi_base[MAX_NUM_VFS_SUPPORTED];
 	u16			rssi_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];
+	struct msix_entry	*msix_entries;
 	bool			irq_allocated[NIC_PF_MSIX_VECTORS];
+	char			irq_name[NIC_PF_MSIX_VECTORS][20];
 };
 
 /* Supported devices */
@@ -89,9 +106,22 @@ static u64 nic_reg_read(struct nicpf *nic, u64 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);
+	int vf_cnt = pci_sriov_get_totalvfs(nic->pdev);
+
+#define INTR_MASK(vfs) ((vfs < 64) ? (BIT_ULL(vfs) - 1) : (~0ull))
+
+	/* Clear it, to avoid spurious interrupts (if any) */
+	nic_reg_write(nic, NIC_PF_MAILBOX_INT, INTR_MASK(vf_cnt));
+
+	/* Enable mailbox interrupt for all VFs */
+	nic_reg_write(nic, NIC_PF_MAILBOX_ENA_W1S, INTR_MASK(vf_cnt));
+	/* One mailbox intr enable reg per 64 VFs */
+	if (vf_cnt > 64) {
+		nic_reg_write(nic, NIC_PF_MAILBOX_INT + sizeof(u64),
+			      INTR_MASK(vf_cnt - 64));
+		nic_reg_write(nic, NIC_PF_MAILBOX_ENA_W1S + sizeof(u64),
+			      INTR_MASK(vf_cnt - 64));
+	}
 }
 
 static void nic_clear_mbx_intr(struct nicpf *nic, int vf, int mbx_reg)
@@ -144,7 +174,7 @@ static void nic_mbx_send_ready(struct nicpf *nic, int vf)
 
 	mbx.nic_cfg.tns_mode = NIC_TNS_BYPASS_MODE;
 
-	if (vf < MAX_LMAC) {
+	if (vf < nic->num_vf_en) {
 		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]);
 
@@ -155,7 +185,7 @@ static void nic_mbx_send_ready(struct nicpf *nic, int vf)
 	mbx.nic_cfg.sqs_mode = (vf >= nic->num_vf_en) ? true : false;
 	mbx.nic_cfg.node_id = nic->node;
 
-	mbx.nic_cfg.loopback_supported = vf < MAX_LMAC;
+	mbx.nic_cfg.loopback_supported = vf < nic->num_vf_en;
 
 	nic_send_msg_to_vf(nic, vf, &mbx);
 }
@@ -248,7 +278,8 @@ static int nic_update_hw_frs(struct nicpf *nic, int new_frs, int vf)
 /* Set minimum transmit packet size */
 static void nic_set_tx_pkt_pad(struct nicpf *nic, int size)
 {
-	int lmac;
+	int lmac, max_lmac;
+	u16 sdevid;
 	u64 lmac_cfg;
 
 	/* There is a issue in HW where-in while sending GSO sized
@@ -260,7 +291,14 @@ static void nic_set_tx_pkt_pad(struct nicpf *nic, int size)
 	if (size > 52)
 		size = 52;
 
-	for (lmac = 0; lmac < (MAX_BGX_PER_CN88XX * MAX_LMAC_PER_BGX); lmac++) {
+	pci_read_config_word(nic->pdev, PCI_SUBSYSTEM_ID, &sdevid);
+	/* 81xx's RGX has only one LMAC */
+	if (sdevid == PCI_SUBSYS_DEVID_81XX_NIC_PF)
+		max_lmac = ((nic->hw->bgx_cnt - 1) * MAX_LMAC_PER_BGX) + 1;
+	else
+		max_lmac = nic->hw->bgx_cnt * MAX_LMAC_PER_BGX;
+
+	for (lmac = 0; lmac < max_lmac; lmac++) {
 		lmac_cfg = nic_reg_read(nic, NIC_PF_LMAC_0_7_CFG | (lmac << 3));
 		lmac_cfg &= ~(0xF << 2);
 		lmac_cfg |= ((size / 4) << 2);
@@ -280,7 +318,7 @@ static void nic_set_lmac_vf_mapping(struct nicpf *nic)
 
 	nic->num_vf_en = 0;
 
-	for (bgx = 0; bgx < NIC_MAX_BGX; bgx++) {
+	for (bgx = 0; bgx < nic->hw->bgx_cnt; bgx++) {
 		if (!(bgx_map & (1 << bgx)))
 			continue;
 		lmac_cnt = bgx_get_lmac_count(nic->node, bgx);
@@ -300,28 +338,125 @@ static void nic_set_lmac_vf_mapping(struct nicpf *nic)
 			nic_reg_write(nic,
 				      NIC_PF_LMAC_0_7_CREDIT + (lmac * 8),
 				      lmac_credit);
+
+		/* On CN81XX there are only 8 VFs but max possible no of
+		 * interfaces are 9.
+		 */
+		if (nic->num_vf_en >= pci_sriov_get_totalvfs(nic->pdev)) {
+			nic->num_vf_en = pci_sriov_get_totalvfs(nic->pdev);
+			break;
+		}
 	}
 }
 
+static void nic_free_lmacmem(struct nicpf *nic)
+{
+	kfree(nic->vf_lmac_map);
+	kfree(nic->link);
+	kfree(nic->duplex);
+	kfree(nic->speed);
+}
+
+static int nic_get_hw_info(struct nicpf *nic)
+{
+	u8 max_lmac;
+	u16 sdevid;
+	struct hw_info *hw = nic->hw;
+
+	pci_read_config_word(nic->pdev, PCI_SUBSYSTEM_ID, &sdevid);
+
+	switch (sdevid) {
+	case PCI_SUBSYS_DEVID_88XX_NIC_PF:
+		hw->bgx_cnt = MAX_BGX_PER_CN88XX;
+		hw->chans_per_lmac = 16;
+		hw->chans_per_bgx = 128;
+		hw->cpi_cnt = 2048;
+		hw->rssi_cnt = 4096;
+		hw->rss_ind_tbl_size = NIC_MAX_RSS_IDR_TBL_SIZE;
+		hw->tl3_cnt = 256;
+		hw->tl2_cnt = 64;
+		hw->tl1_cnt = 2;
+		hw->tl1_per_bgx = true;
+		break;
+	case PCI_SUBSYS_DEVID_81XX_NIC_PF:
+		hw->bgx_cnt = MAX_BGX_PER_CN81XX;
+		hw->chans_per_lmac = 8;
+		hw->chans_per_bgx = 32;
+		hw->chans_per_rgx = 8;
+		hw->chans_per_lbk = 24;
+		hw->cpi_cnt = 512;
+		hw->rssi_cnt = 256;
+		hw->rss_ind_tbl_size = 32; /* Max RSSI / Max interfaces */
+		hw->tl3_cnt = 64;
+		hw->tl2_cnt = 16;
+		hw->tl1_cnt = 10;
+		hw->tl1_per_bgx = false;
+		break;
+	case PCI_SUBSYS_DEVID_83XX_NIC_PF:
+		hw->bgx_cnt = MAX_BGX_PER_CN83XX;
+		hw->chans_per_lmac = 8;
+		hw->chans_per_bgx = 32;
+		hw->chans_per_lbk = 64;
+		hw->cpi_cnt = 2048;
+		hw->rssi_cnt = 1024;
+		hw->rss_ind_tbl_size = 64; /* Max RSSI / Max interfaces */
+		hw->tl3_cnt = 256;
+		hw->tl2_cnt = 64;
+		hw->tl1_cnt = 18;
+		hw->tl1_per_bgx = false;
+		break;
+	}
+	hw->tl4_cnt = MAX_QUEUES_PER_QSET * pci_sriov_get_totalvfs(nic->pdev);
+
+	/* Allocate memory for LMAC tracking elements */
+	max_lmac = hw->bgx_cnt * MAX_LMAC_PER_BGX;
+	nic->vf_lmac_map = kmalloc_array(max_lmac, sizeof(u8), GFP_KERNEL);
+	if (!nic->vf_lmac_map)
+		goto error;
+	nic->link = kmalloc_array(max_lmac, sizeof(u8), GFP_KERNEL);
+	if (!nic->link)
+		goto error;
+	nic->duplex = kmalloc_array(max_lmac, sizeof(u8), GFP_KERNEL);
+	if (!nic->duplex)
+		goto error;
+	nic->speed = kmalloc_array(max_lmac, sizeof(u32), GFP_KERNEL);
+	if (!nic->speed)
+		goto error;
+	return 0;
+
+error:
+	nic_free_lmacmem(nic);
+	return -ENOMEM;
+}
+
 #define BGX0_BLOCK 8
 #define BGX1_BLOCK 9
 
-static void nic_init_hw(struct nicpf *nic)
+static int nic_init_hw(struct nicpf *nic)
 {
-	int i;
+	int i, err;
 	u64 cqm_cfg;
 
+	/* Get HW capability info */
+	err = nic_get_hw_info(nic);
+	if (err)
+		return err;
+
 	/* 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);
+	/* TNS and TNS bypass modes are present only on 88xx */
+	if (nic->pdev->subsystem_device == PCI_SUBSYS_DEVID_88XX_NIC_PF) {
+		/* 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),
@@ -351,11 +486,14 @@ static void nic_init_hw(struct nicpf *nic)
 	cqm_cfg = nic_reg_read(nic, NIC_PF_CQM_CFG);
 	if (cqm_cfg < NICPF_CQM_MIN_DROP_LEVEL)
 		nic_reg_write(nic, NIC_PF_CQM_CFG, NICPF_CQM_MIN_DROP_LEVEL);
+
+	return 0;
 }
 
 /* Channel parse index configuration */
 static void nic_config_cpi(struct nicpf *nic, struct cpi_cfg_msg *cfg)
 {
+	struct hw_info *hw = nic->hw;
 	u32 vnic, bgx, lmac, chan;
 	u32 padd, cpi_count = 0;
 	u64 cpi_base, cpi, rssi_base, rssi;
@@ -365,9 +503,9 @@ static void nic_config_cpi(struct nicpf *nic, struct cpi_cfg_msg *cfg)
 	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);
+	chan = (lmac * hw->chans_per_lmac) + (bgx * hw->chans_per_bgx);
+	cpi_base = vnic * NIC_MAX_CPI_PER_LMAC;
+	rssi_base = vnic * hw->rss_ind_tbl_size;
 
 	/* Rx channel configuration */
 	nic_reg_write(nic, NIC_PF_CHAN_0_255_RX_BP_CFG | (chan << 3),
@@ -439,7 +577,7 @@ static void nic_send_rss_size(struct nicpf *nic, int vf)
 	msg = (u64 *)&mbx;
 
 	mbx.rss_size.msg = NIC_MBOX_MSG_RSS_SIZE;
-	mbx.rss_size.ind_tbl_size = nic->rss_ind_tbl_size;
+	mbx.rss_size.ind_tbl_size = nic->hw->rss_ind_tbl_size;
 	nic_send_msg_to_vf(nic, vf, &mbx);
 }
 
@@ -486,7 +624,7 @@ static void nic_config_rss(struct nicpf *nic, struct rss_cfg_msg *cfg)
 /* 4 level transmit side scheduler configutation
  * for TNS bypass mode
  *
- * Sample configuration for SQ0
+ * Sample configuration for SQ0 on 88xx
  * 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
@@ -499,6 +637,7 @@ static void nic_config_rss(struct nicpf *nic, struct rss_cfg_msg *cfg)
 static void nic_tx_channel_cfg(struct nicpf *nic, u8 vnic,
 			       struct sq_cfg_msg *sq)
 {
+	struct hw_info *hw = nic->hw;
 	u32 bgx, lmac, chan;
 	u32 tl2, tl3, tl4;
 	u32 rr_quantum;
@@ -517,21 +656,28 @@ static void nic_tx_channel_cfg(struct nicpf *nic, u8 vnic,
 	/* 24 bytes for FCS, IPG and preamble */
 	rr_quantum = ((NIC_HW_MAX_FRS + 24) / 4);
 
-	if (!sq->sqs_mode) {
-		tl4 = (lmac * NIC_TL4_PER_LMAC) + (bgx * NIC_TL4_PER_BGX);
-	} else {
-		for (svf = 0; svf < MAX_SQS_PER_VF; svf++) {
-			if (nic->vf_sqs[pqs_vnic][svf] == vnic)
-				break;
+	/* For 88xx 0-511 TL4 transmits via BGX0 and
+	 * 512-1023 TL4s transmit via BGX1.
+	 */
+	if (hw->tl1_per_bgx) {
+		tl4 = bgx * (hw->tl4_cnt / hw->bgx_cnt);
+		if (!sq->sqs_mode) {
+			tl4 += (lmac * MAX_QUEUES_PER_QSET);
+		} else {
+			for (svf = 0; svf < MAX_SQS_PER_VF; svf++) {
+				if (nic->vf_sqs[pqs_vnic][svf] == vnic)
+					break;
+			}
+			tl4 += (MAX_LMAC_PER_BGX * MAX_QUEUES_PER_QSET);
+			tl4 += (lmac * MAX_QUEUES_PER_QSET * MAX_SQS_PER_VF);
+			tl4 += (svf * MAX_QUEUES_PER_QSET);
 		}
-		tl4 = (MAX_LMAC_PER_BGX * NIC_TL4_PER_LMAC);
-		tl4 += (lmac * NIC_TL4_PER_LMAC * MAX_SQS_PER_VF);
-		tl4 += (svf * NIC_TL4_PER_LMAC);
-		tl4 += (bgx * NIC_TL4_PER_BGX);
+	} else {
+		tl4 = (vnic * MAX_QUEUES_PER_QSET);
 	}
 	tl4 += sq_idx;
 
-	tl3 = tl4 / (NIC_MAX_TL4 / NIC_MAX_TL3);
+	tl3 = tl4 / (hw->tl4_cnt / hw->tl3_cnt);
 	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);
@@ -539,8 +685,19 @@ static void nic_tx_channel_cfg(struct nicpf *nic, u8 vnic,
 		      ((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);
+
+	/* On 88xx 0-127 channels are for BGX0 and
+	 * 127-255 channels for BGX1.
+	 *
+	 * On 81xx/83xx TL3_CHAN reg should be configured with channel
+	 * within LMAC i.e 0-7 and not the actual channel number like on 88xx
+	 */
+	chan = (lmac * hw->chans_per_lmac) + (bgx * hw->chans_per_bgx);
+	if (hw->tl1_per_bgx)
+		nic_reg_write(nic, NIC_PF_TL3_0_255_CHAN | (tl3 << 3), chan);
+	else
+		nic_reg_write(nic, NIC_PF_TL3_0_255_CHAN | (tl3 << 3), 0);
+
 	/* Enable backpressure on the channel */
 	nic_reg_write(nic, NIC_PF_CHAN_0_255_TX_CFG | (chan << 3), 1);
 
@@ -549,6 +706,16 @@ static void nic_tx_channel_cfg(struct nicpf *nic, u8 vnic,
 	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);
+
+	/* Unlike 88xx where TL2s 0-31 transmits to TL1 '0' and rest to TL1 '1'
+	 * on 81xx/83xx TL2 needs to be configured to transmit to one of the
+	 * possible LMACs.
+	 *
+	 * This register doesn't exist on 88xx.
+	 */
+	if (!hw->tl1_per_bgx)
+		nic_reg_write(nic, NIC_PF_TL2_LMAC | (tl2 << 3),
+			      lmac + (bgx * MAX_LMAC_PER_BGX));
 }
 
 /* Send primary nicvf pointer to secondary QS's VF */
@@ -620,7 +787,7 @@ static int nic_config_loopback(struct nicpf *nic, struct set_loopback *lbk)
 {
 	int bgx_idx, lmac_idx;
 
-	if (lbk->vf_id > MAX_LMAC)
+	if (lbk->vf_id >= nic->num_vf_en)
 		return -1;
 
 	bgx_idx = NIC_GET_BGX_FROM_VF_LMAC_MAP(nic->vf_lmac_map[lbk->vf_id]);
@@ -631,6 +798,67 @@ static int nic_config_loopback(struct nicpf *nic, struct set_loopback *lbk)
 	return 0;
 }
 
+/* Reset statistics counters */
+static int nic_reset_stat_counters(struct nicpf *nic,
+				   int vf, struct reset_stat_cfg *cfg)
+{
+	int i, stat, qnum;
+	u64 reg_addr;
+
+	for (i = 0; i < RX_STATS_ENUM_LAST; i++) {
+		if (cfg->rx_stat_mask & BIT(i)) {
+			reg_addr = NIC_PF_VNIC_0_127_RX_STAT_0_13 |
+				   (vf << NIC_QS_ID_SHIFT) |
+				   (i << 3);
+			nic_reg_write(nic, reg_addr, 0);
+		}
+	}
+
+	for (i = 0; i < TX_STATS_ENUM_LAST; i++) {
+		if (cfg->tx_stat_mask & BIT(i)) {
+			reg_addr = NIC_PF_VNIC_0_127_TX_STAT_0_4 |
+				   (vf << NIC_QS_ID_SHIFT) |
+				   (i << 3);
+			nic_reg_write(nic, reg_addr, 0);
+		}
+	}
+
+	for (i = 0; i <= 15; i++) {
+		qnum = i >> 1;
+		stat = i & 1 ? 1 : 0;
+		reg_addr = (vf << NIC_QS_ID_SHIFT) |
+			   (qnum << NIC_Q_NUM_SHIFT) | (stat << 3);
+		if (cfg->rq_stat_mask & BIT(i)) {
+			reg_addr |= NIC_PF_QSET_0_127_RQ_0_7_STAT_0_1;
+			nic_reg_write(nic, reg_addr, 0);
+		}
+		if (cfg->sq_stat_mask & BIT(i)) {
+			reg_addr |= NIC_PF_QSET_0_127_SQ_0_7_STAT_0_1;
+			nic_reg_write(nic, reg_addr, 0);
+		}
+	}
+	return 0;
+}
+
+static void nic_enable_tunnel_parsing(struct nicpf *nic, int vf)
+{
+	u64 prot_def = (IPV6_PROT << 32) | (IPV4_PROT << 16) | ET_PROT;
+	u64 vxlan_prot_def = (IPV6_PROT_DEF << 32) |
+			      (IPV4_PROT_DEF) << 16 | ET_PROT_DEF;
+
+	/* Configure tunnel parsing parameters */
+	nic_reg_write(nic, NIC_PF_RX_GENEVE_DEF,
+		      (1ULL << 63 | UDP_GENEVE_PORT_NUM));
+	nic_reg_write(nic, NIC_PF_RX_GENEVE_PROT_DEF,
+		      ((7ULL << 61) | prot_def));
+	nic_reg_write(nic, NIC_PF_RX_NVGRE_PROT_DEF,
+		      ((7ULL << 61) | prot_def));
+	nic_reg_write(nic, NIC_PF_RX_VXLAN_DEF_0_1,
+		      ((1ULL << 63) | UDP_VXLAN_PORT_NUM));
+	nic_reg_write(nic, NIC_PF_RX_VXLAN_PROT_DEF,
+		      ((0xfULL << 60) | vxlan_prot_def));
+}
+
 static void nic_enable_vf(struct nicpf *nic, int vf, bool enable)
 {
 	int bgx, lmac;
@@ -669,18 +897,17 @@ static void nic_handle_mbx_intr(struct nicpf *nic, int vf)
 		mbx_addr += sizeof(u64);
 	}
 
-	dev_dbg(&nic->pdev->dev, "%s: Mailbox msg %d from VF%d\n",
+	dev_dbg(&nic->pdev->dev, "%s: Mailbox msg 0x%02x from VF%d\n",
 		__func__, mbx.msg.msg, vf);
 	switch (mbx.msg.msg) {
 	case NIC_MBOX_MSG_READY:
 		nic_mbx_send_ready(nic, vf);
-		if (vf < MAX_LMAC) {
+		if (vf < nic->num_vf_en) {
 			nic->link[vf] = 0;
 			nic->duplex[vf] = 0;
 			nic->speed[vf] = 0;
 		}
-		ret = 1;
-		break;
+		goto unlock;
 	case NIC_MBOX_MSG_QS_CFG:
 		reg_addr = NIC_PF_QSET_0_127_CFG |
 			   (mbx.qs.num << NIC_QS_ID_SHIFT);
@@ -698,6 +925,15 @@ static void nic_handle_mbx_intr(struct nicpf *nic, int vf)
 			   (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);
+		/* Enable CQE_RX2_S extension in CQE_RX descriptor.
+		 * This gets appended by default on 81xx/83xx chips,
+		 * for consistency enabling the same on 88xx pass2
+		 * where this is introduced.
+		 */
+		if (pass2_silicon(nic->pdev))
+			nic_reg_write(nic, NIC_PF_RX_CFG, 0x01);
+		if (!pass1_silicon(nic->pdev))
+			nic_enable_tunnel_parsing(nic, vf);
 		break;
 	case NIC_MBOX_MSG_RQ_BP_CFG:
 		reg_addr = NIC_PF_QSET_0_127_RQ_0_7_BP_CFG |
@@ -722,8 +958,10 @@ static void nic_handle_mbx_intr(struct nicpf *nic, int vf)
 		nic_tx_channel_cfg(nic, mbx.qs.num, &mbx.sq);
 		break;
 	case NIC_MBOX_MSG_SET_MAC:
-		if (vf >= nic->num_vf_en)
+		if (vf >= nic->num_vf_en) {
+			ret = -1; /* NACK */
 			break;
+		}
 		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]);
@@ -772,25 +1010,38 @@ static void nic_handle_mbx_intr(struct nicpf *nic, int vf)
 	case NIC_MBOX_MSG_LOOPBACK:
 		ret = nic_config_loopback(nic, &mbx.lbk);
 		break;
+	case NIC_MBOX_MSG_RESET_STAT_COUNTER:
+		ret = nic_reset_stat_counters(nic, vf, &mbx.reset_stat);
+		break;
 	default:
 		dev_err(&nic->pdev->dev,
 			"Invalid msg from VF%d, msg 0x%x\n", vf, mbx.msg.msg);
 		break;
 	}
 
-	if (!ret)
+	if (!ret) {
 		nic_mbx_send_ack(nic, vf);
-	else if (mbx.msg.msg != NIC_MBOX_MSG_READY)
+	} else if (mbx.msg.msg != NIC_MBOX_MSG_READY) {
+		dev_err(&nic->pdev->dev, "NACK for MBOX 0x%02x from VF %d\n",
+			mbx.msg.msg, vf);
 		nic_mbx_send_nack(nic, vf);
+	}
 unlock:
 	nic->mbx_lock[vf] = false;
 }
 
-static void nic_mbx_intr_handler (struct nicpf *nic, int mbx)
+static irqreturn_t nic_mbx_intr_handler(int irq, void *nic_irq)
 {
+	struct nicpf *nic = (struct nicpf *)nic_irq;
+	int mbx;
 	u64 intr;
 	u8  vf, vf_per_mbx_reg = 64;
 
+	if (irq == nic->msix_entries[NIC_PF_INTR_ID_MBOX0].vector)
+		mbx = 0;
+	else
+		mbx = 1;
+
 	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++) {
@@ -802,23 +1053,6 @@ static void nic_mbx_intr_handler (struct nicpf *nic, int mbx)
 			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;
 }
 
@@ -826,7 +1060,13 @@ static int nic_enable_msix(struct nicpf *nic)
 {
 	int i, ret;
 
-	nic->num_vec = NIC_PF_MSIX_VECTORS;
+	nic->num_vec = pci_msix_vec_count(nic->pdev);
+
+	nic->msix_entries = kmalloc_array(nic->num_vec,
+					  sizeof(struct msix_entry),
+					  GFP_KERNEL);
+	if (!nic->msix_entries)
+		return -ENOMEM;
 
 	for (i = 0; i < nic->num_vec; i++)
 		nic->msix_entries[i].entry = i;
@@ -834,8 +1074,9 @@ static int nic_enable_msix(struct nicpf *nic)
 	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);
+			"Request for #%d msix vectors failed, returned %d\n",
+			   nic->num_vec, ret);
+		kfree(nic->msix_entries);
 		return ret;
 	}
 
@@ -847,6 +1088,7 @@ static void nic_disable_msix(struct nicpf *nic)
 {
 	if (nic->msix_enabled) {
 		pci_disable_msix(nic->pdev);
+		kfree(nic->msix_entries);
 		nic->msix_enabled = 0;
 		nic->num_vec = 0;
 	}
@@ -865,27 +1107,26 @@ static void nic_free_all_interrupts(struct nicpf *nic)
 
 static int nic_register_interrupts(struct nicpf *nic)
 {
-	int ret;
+	int i, 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;
+	/* Register mailbox interrupt handler */
+	for (i = NIC_PF_INTR_ID_MBOX0; i < nic->num_vec; i++) {
+		sprintf(nic->irq_name[i],
+			"NICPF Mbox%d", (i - NIC_PF_INTR_ID_MBOX0));
 
-	ret = request_irq(nic->msix_entries[NIC_PF_INTR_ID_MBOX1].vector,
-			  nic_mbx1_intr_handler, 0, "NIC Mbox1", nic);
-	if (ret)
-		goto fail;
+		ret = request_irq(nic->msix_entries[i].vector,
+				  nic_mbx_intr_handler, 0,
+				  nic->irq_name[i], nic);
+		if (ret)
+			goto fail;
 
-	nic->irq_allocated[NIC_PF_INTR_ID_MBOX1] = true;
+		nic->irq_allocated[i] = true;
+	}
 
 	/* Enable mailbox interrupt */
 	nic_enable_mbx_intr(nic);
@@ -894,6 +1135,7 @@ static int nic_register_interrupts(struct nicpf *nic)
 fail:
 	dev_err(&nic->pdev->dev, "Request irq failed\n");
 	nic_free_all_interrupts(nic);
+	nic_disable_msix(nic);
 	return ret;
 }
 
@@ -908,6 +1150,12 @@ static int nic_num_sqs_en(struct nicpf *nic, int vf_en)
 	int pos, sqs_per_vf = MAX_SQS_PER_VF_SINGLE_NODE;
 	u16 total_vf;
 
+	/* Secondary Qsets are needed only if CPU count is
+	 * morethan MAX_QUEUES_PER_QSET.
+	 */
+	if (num_online_cpus() <= MAX_QUEUES_PER_QSET)
+		return 0;
+
 	/* Check if its a multi-node environment */
 	if (nr_node_ids > 1)
 		sqs_per_vf = MAX_SQS_PER_VF;
@@ -1013,6 +1261,12 @@ static int nic_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	if (!nic)
 		return -ENOMEM;
 
+	nic->hw = devm_kzalloc(dev, sizeof(struct hw_info), GFP_KERNEL);
+	if (!nic->hw) {
+		devm_kfree(dev, nic);
+		return -ENOMEM;
+	}
+
 	pci_set_drvdata(pdev, nic);
 
 	nic->pdev = pdev;
@@ -1052,13 +1306,12 @@ static int nic_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 
 	nic->node = nic_get_node_id(pdev);
 
-	nic_set_lmac_vf_mapping(nic);
-
 	/* Initialize hardware */
-	nic_init_hw(nic);
+	err = nic_init_hw(nic);
+	if (err)
+		goto err_release_regions;
 
-	/* Set RSS TBL size for each VF */
-	nic->rss_ind_tbl_size = NIC_MAX_RSS_IDR_TBL_SIZE;
+	nic_set_lmac_vf_mapping(nic);
 
 	/* Register interrupts */
 	err = nic_register_interrupts(nic);
@@ -1091,6 +1344,9 @@ err_unregister_interrupts:
 err_release_regions:
 	pci_release_regions(pdev);
 err_disable_device:
+	nic_free_lmacmem(nic);
+	devm_kfree(dev, nic->hw);
+	devm_kfree(dev, nic);
 	pci_disable_device(pdev);
 	pci_set_drvdata(pdev, NULL);
 	return err;
@@ -1111,6 +1367,11 @@ static void nic_remove(struct pci_dev *pdev)
 
 	nic_unregister_interrupts(nic);
 	pci_release_regions(pdev);
+
+	nic_free_lmacmem(nic);
+	devm_kfree(&pdev->dev, nic->hw);
+	devm_kfree(&pdev->dev, nic);
+
 	pci_disable_device(pdev);
 	pci_set_drvdata(pdev, NULL);
 }
diff --git a/drivers/net/ethernet/cavium/thunder/nic_reg.h b/drivers/net/ethernet/cavium/thunder/nic_reg.h
index fab35a593898..edf779f5a227 100644
--- a/drivers/net/ethernet/cavium/thunder/nic_reg.h
+++ b/drivers/net/ethernet/cavium/thunder/nic_reg.h
@@ -36,6 +36,20 @@
 #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_RX_GENEVE_DEF			(0x0580)
+#define    UDP_GENEVE_PORT_NUM				0x17C1ULL
+#define   NIC_PF_RX_GENEVE_PROT_DEF		(0x0588)
+#define    IPV6_PROT					0x86DDULL
+#define    IPV4_PROT					0x800ULL
+#define    ET_PROT					0x6558ULL
+#define   NIC_PF_RX_NVGRE_PROT_DEF		(0x0598)
+#define   NIC_PF_RX_VXLAN_DEF_0_1		(0x05A0)
+#define    UDP_VXLAN_PORT_NUM				0x12B5
+#define   NIC_PF_RX_VXLAN_PROT_DEF		(0x05B0)
+#define    IPV6_PROT_DEF				0x2ULL
+#define    IPV4_PROT_DEF				0x1ULL
+#define    ET_PROT_DEF					0x3ULL
+#define   NIC_PF_RX_CFG				(0x05D0)
 #define   NIC_PF_PKIND_0_15_CFG			(0x0600)
 #define   NIC_PF_ECC0_FLIP0			(0x1000)
 #define   NIC_PF_ECC1_FLIP0			(0x1008)
@@ -103,6 +117,7 @@
 #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_LMAC			(0x540000)
 #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)
diff --git a/drivers/net/ethernet/cavium/thunder/nicvf_main.c b/drivers/net/ethernet/cavium/thunder/nicvf_main.c
index 3240349615bd..45a13f718863 100644
--- a/drivers/net/ethernet/cavium/thunder/nicvf_main.c
+++ b/drivers/net/ethernet/cavium/thunder/nicvf_main.c
@@ -29,10 +29,20 @@
 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, 0xA134) },
+			 PCI_VENDOR_ID_CAVIUM,
+			 PCI_SUBSYS_DEVID_88XX_NIC_VF) },
 	{ PCI_DEVICE_SUB(PCI_VENDOR_ID_CAVIUM,
 			 PCI_DEVICE_ID_THUNDER_PASS1_NIC_VF,
-			 PCI_VENDOR_ID_CAVIUM, 0xA11E) },
+			 PCI_VENDOR_ID_CAVIUM,
+			 PCI_SUBSYS_DEVID_88XX_PASS1_NIC_VF) },
+	{ PCI_DEVICE_SUB(PCI_VENDOR_ID_CAVIUM,
+			 PCI_DEVICE_ID_THUNDER_NIC_VF,
+			 PCI_VENDOR_ID_CAVIUM,
+			 PCI_SUBSYS_DEVID_81XX_NIC_VF) },
+	{ PCI_DEVICE_SUB(PCI_VENDOR_ID_CAVIUM,
+			 PCI_DEVICE_ID_THUNDER_NIC_VF,
+			 PCI_VENDOR_ID_CAVIUM,
+			 PCI_SUBSYS_DEVID_83XX_NIC_VF) },
 	{ 0, }  /* end of table */
 };
 
@@ -134,15 +144,19 @@ int nicvf_send_msg_to_pf(struct nicvf *nic, union nic_mbx *mbx)
 
 	/* Wait for previous message to be acked, timeout 2sec */
 	while (!nic->pf_acked) {
-		if (nic->pf_nacked)
+		if (nic->pf_nacked) {
+			netdev_err(nic->netdev,
+				   "PF NACK to mbox msg 0x%02x from VF%d\n",
+				   (mbx->msg.msg & 0xFF), nic->vf_id);
 			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",
+				   "PF didn't ACK to mbox msg 0x%02x from VF%d\n",
 				   (mbx->msg.msg & 0xFF), nic->vf_id);
 			return -EBUSY;
 		}
@@ -352,13 +366,7 @@ static int nicvf_rss_init(struct nicvf *nic)
 
 	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;
-
+	netdev_rss_key_fill(rss->key, RSS_HASH_KEY_SIZE * sizeof(u64));
 	nicvf_set_rss_key(nic);
 
 	rss->cfg = RSS_IP_HASH_ENA | RSS_TCP_HASH_ENA | RSS_UDP_HASH_ENA;
@@ -507,7 +515,9 @@ static int nicvf_init_resources(struct nicvf *nic)
 
 static void nicvf_snd_pkt_handler(struct net_device *netdev,
 				  struct cmp_queue *cq,
-				  struct cqe_send_t *cqe_tx, int cqe_type)
+				  struct cqe_send_t *cqe_tx,
+				  int cqe_type, int budget,
+				  unsigned int *tx_pkts, unsigned int *tx_bytes)
 {
 	struct sk_buff *skb = NULL;
 	struct nicvf *nic = netdev_priv(netdev);
@@ -538,7 +548,9 @@ static void nicvf_snd_pkt_handler(struct net_device *netdev,
 		}
 		nicvf_put_sq_desc(sq, hdr->subdesc_cnt + 1);
 		prefetch(skb);
-		dev_consume_skb_any(skb);
+		(*tx_pkts)++;
+		*tx_bytes += skb->len;
+		napi_consume_skb(skb, budget);
 		sq->skbuff[cqe_tx->sqe_ptr] = (u64)NULL;
 	} else {
 		/* In case of SW TSO on 88xx, only last segment will have
@@ -653,6 +665,7 @@ static int nicvf_cq_intr_handler(struct net_device *netdev, u8 cq_idx,
 	struct cmp_queue *cq = &qs->cq[cq_idx];
 	struct cqe_rx_t *cq_desc;
 	struct netdev_queue *txq;
+	unsigned int tx_pkts = 0, tx_bytes = 0;
 
 	spin_lock_bh(&cq->lock);
 loop:
@@ -691,7 +704,8 @@ loop:
 		break;
 		case CQE_TYPE_SEND:
 			nicvf_snd_pkt_handler(netdev, cq,
-					      (void *)cq_desc, CQE_TYPE_SEND);
+					      (void *)cq_desc, CQE_TYPE_SEND,
+					      budget, &tx_pkts, &tx_bytes);
 			tx_done++;
 		break;
 		case CQE_TYPE_INVALID:
@@ -720,6 +734,9 @@ done:
 		netdev = nic->pnicvf->netdev;
 		txq = netdev_get_tx_queue(netdev,
 					  nicvf_netdev_qidx(nic, cq_idx));
+		if (tx_pkts)
+			netdev_tx_completed_queue(txq, tx_pkts, tx_bytes);
+
 		nic = nic->pnicvf;
 		if (netif_tx_queue_stopped(txq) && netif_carrier_ok(netdev)) {
 			netif_tx_start_queue(txq);
@@ -933,16 +950,19 @@ static int nicvf_register_interrupts(struct nicvf *nic)
 	int vector;
 
 	for_each_cq_irq(irq)
-		sprintf(nic->irq_name[irq], "NICVF%d CQ%d",
-			nic->vf_id, irq);
+		sprintf(nic->irq_name[irq], "%s-rxtx-%d",
+			nic->pnicvf->netdev->name,
+			nicvf_netdev_qidx(nic, irq));
 
 	for_each_sq_irq(irq)
-		sprintf(nic->irq_name[irq], "NICVF%d SQ%d",
-			nic->vf_id, irq - NICVF_INTR_ID_SQ);
+		sprintf(nic->irq_name[irq], "%s-sq-%d",
+			nic->pnicvf->netdev->name,
+			nicvf_netdev_qidx(nic, 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);
+		sprintf(nic->irq_name[irq], "%s-rbdr-%d",
+			nic->pnicvf->netdev->name,
+			nic->sqs_mode ? (nic->sqs_id + 1) : 0);
 
 	/* Register CQ interrupts */
 	for (irq = 0; irq < nic->qs->cq_cnt; irq++) {
@@ -966,8 +986,9 @@ static int nicvf_register_interrupts(struct nicvf *nic)
 	}
 
 	/* Register QS error interrupt */
-	sprintf(nic->irq_name[NICVF_INTR_ID_QS_ERR],
-		"NICVF%d Qset error", nic->vf_id);
+	sprintf(nic->irq_name[NICVF_INTR_ID_QS_ERR], "%s-qset-err-%d",
+		nic->pnicvf->netdev->name,
+		nic->sqs_mode ? (nic->sqs_id + 1) : 0);
 	irq = NICVF_INTR_ID_QS_ERR;
 	ret = request_irq(nic->msix_entries[irq].vector,
 			  nicvf_qs_err_intr_handler,
@@ -1146,6 +1167,9 @@ int nicvf_stop(struct net_device *netdev)
 
 	netif_tx_disable(netdev);
 
+	for (qidx = 0; qidx < netdev->num_tx_queues; qidx++)
+		netdev_tx_reset_queue(netdev_get_tx_queue(netdev, qidx));
+
 	/* Free resources */
 	nicvf_config_data_transfer(nic, false);
 
@@ -1196,7 +1220,7 @@ int nicvf_open(struct net_device *netdev)
 	}
 
 	/* Check if we got MAC address from PF or else generate a radom MAC */
-	if (is_zero_ether_addr(netdev->dev_addr)) {
+	if (!nic->sqs_mode && is_zero_ether_addr(netdev->dev_addr)) {
 		eth_hw_addr_random(netdev);
 		nicvf_hw_set_mac_addr(nic, netdev);
 	}
@@ -1533,14 +1557,13 @@ static int nicvf_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 		goto err_release_regions;
 	}
 
-	qcount = MAX_CMP_QUEUES_PER_QS;
+	qcount = netif_get_num_default_rss_queues();
 
 	/* Restrict multiqset support only for host bound VFs */
 	if (pdev->is_virtfn) {
 		/* Set max number of queues per VF */
-		qcount = roundup(num_online_cpus(), MAX_CMP_QUEUES_PER_QS);
-		qcount = min(qcount,
-			     (MAX_SQS_PER_VF + 1) * MAX_CMP_QUEUES_PER_QS);
+		qcount = min_t(int, num_online_cpus(),
+			       (MAX_SQS_PER_VF + 1) * MAX_CMP_QUEUES_PER_QS);
 	}
 
 	netdev = alloc_etherdev_mqs(sizeof(struct nicvf), qcount, qcount);
diff --git a/drivers/net/ethernet/cavium/thunder/nicvf_queues.c b/drivers/net/ethernet/cavium/thunder/nicvf_queues.c
index dda3ea3f3bb6..a4fc50155881 100644
--- a/drivers/net/ethernet/cavium/thunder/nicvf_queues.c
+++ b/drivers/net/ethernet/cavium/thunder/nicvf_queues.c
@@ -479,6 +479,16 @@ void nicvf_config_vlan_stripping(struct nicvf *nic, netdev_features_t features)
 					      NIC_QSET_RQ_GEN_CFG, 0, rq_cfg);
 }
 
+static void nicvf_reset_rcv_queue_stats(struct nicvf *nic)
+{
+	union nic_mbx mbx = {};
+
+	/* Reset all RXQ's stats */
+	mbx.reset_stat.msg = NIC_MBOX_MSG_RESET_STAT_COUNTER;
+	mbx.reset_stat.rq_stat_mask = 0xFFFF;
+	nicvf_send_msg_to_pf(nic, &mbx);
+}
+
 /* Configures receive queue */
 static void nicvf_rcv_queue_config(struct nicvf *nic, struct queue_set *qs,
 				   int qidx, bool enable)
@@ -762,10 +772,10 @@ int nicvf_set_qset_resources(struct nicvf *nic)
 	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;
+	qs->rbdr_cnt = DEFAULT_RBDR_CNT;
+	qs->rq_cnt = min_t(u8, MAX_RCV_QUEUES_PER_QS, num_online_cpus());
+	qs->sq_cnt = min_t(u8, MAX_SND_QUEUES_PER_QS, num_online_cpus());
+	qs->cq_cnt = max_t(u8, qs->rq_cnt, qs->sq_cnt);
 
 	/* Set queue lengths */
 	qs->rbdr_len = RCV_BUF_COUNT;
@@ -812,6 +822,11 @@ int nicvf_config_data_transfer(struct nicvf *nic, bool enable)
 		nicvf_free_resources(nic);
 	}
 
+	/* Reset RXQ's stats.
+	 * SQ's stats will get reset automatically once SQ is reset.
+	 */
+	nicvf_reset_rcv_queue_stats(nic);
+
 	return 0;
 }
 
@@ -1067,6 +1082,24 @@ static inline void nicvf_sq_add_cqe_subdesc(struct snd_queue *sq, int qentry,
 	imm->len = 1;
 }
 
+static inline void nicvf_sq_doorbell(struct nicvf *nic, struct sk_buff *skb,
+				     int sq_num, int desc_cnt)
+{
+	struct netdev_queue *txq;
+
+	txq = netdev_get_tx_queue(nic->pnicvf->netdev,
+				  skb_get_queue_mapping(skb));
+
+	netdev_tx_sent_queue(txq, skb->len);
+
+	/* 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,
+			      sq_num, desc_cnt);
+}
+
 /* Segment a TSO packet into 'gso_size' segments and append
  * them to SQ for transfer
  */
@@ -1126,12 +1159,8 @@ static int nicvf_sq_append_tso(struct nicvf *nic, struct snd_queue *sq,
 	/* 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();
+	nicvf_sq_doorbell(nic, skb, sq_num, desc_cnt);
 
-	/* Inform HW to xmit all TSO segments */
-	nicvf_queue_reg_write(nic, NIC_QSET_SQ_0_7_DOOR,
-			      sq_num, desc_cnt);
 	nic->drv_stats.tx_tso++;
 	return 1;
 }
@@ -1204,12 +1233,8 @@ doorbell:
 		nicvf_sq_add_cqe_subdesc(sq, qentry, tso_sqe, skb);
 	}
 
-	/* make sure all memory stores are done before ringing doorbell */
-	smp_wmb();
+	nicvf_sq_doorbell(nic, skb, sq_num, subdesc_cnt);
 
-	/* 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:
@@ -1234,13 +1259,23 @@ 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;
+	struct page *page;
+	int offset;
 	u16 *rb_lens = NULL;
 	u64 *rb_ptrs = NULL;
 
 	rb_lens = (void *)cqe_rx + (3 * sizeof(u64));
-	rb_ptrs = (void *)cqe_rx + (6 * sizeof(u64));
+	/* Except 88xx pass1 on all other chips CQE_RX2_S is added to
+	 * CQE_RX at word6, hence buffer pointers move by word
+	 *
+	 * Use existing 'hw_tso' flag which will be set for all chips
+	 * except 88xx pass1 instead of a additional cache line
+	 * access (or miss) by using pci dev's revision.
+	 */
+	if (!nic->hw_tso)
+		rb_ptrs = (void *)cqe_rx + (6 * sizeof(u64));
+	else
+		rb_ptrs = (void *)cqe_rx + (7 * 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);
@@ -1258,22 +1293,10 @@ struct sk_buff *nicvf_get_rcv_skb(struct nicvf *nic, struct cqe_rx_t *cqe_rx)
 			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;
+			page = virt_to_page(phys_to_virt(*rb_ptrs));
+			offset = phys_to_virt(*rb_ptrs) - page_address(page);
+			skb_add_rx_frag(skb, skb_shinfo(skb)->nr_frags, page,
+					offset, payload_len, RCV_FRAG_LEN);
 		}
 		/* Next buffer pointer */
 		rb_ptrs++;
diff --git a/drivers/net/ethernet/cavium/thunder/nicvf_queues.h b/drivers/net/ethernet/cavium/thunder/nicvf_queues.h
index 6673e1133523..869f3386028b 100644
--- a/drivers/net/ethernet/cavium/thunder/nicvf_queues.h
+++ b/drivers/net/ethernet/cavium/thunder/nicvf_queues.h
@@ -57,10 +57,7 @@
 #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 DEFAULT_RBDR_CNT	1
 
 #define SND_QSIZE		SND_QUEUE_SIZE2
 #define SND_QUEUE_LEN		(1ULL << (SND_QSIZE + 10))
diff --git a/drivers/net/ethernet/cavium/thunder/thunder_bgx.c b/drivers/net/ethernet/cavium/thunder/thunder_bgx.c
index 63a39ac97d53..8bbaedbb7b94 100644
--- a/drivers/net/ethernet/cavium/thunder/thunder_bgx.c
+++ b/drivers/net/ethernet/cavium/thunder/thunder_bgx.c
@@ -28,6 +28,9 @@ struct lmac {
 	struct bgx		*bgx;
 	int			dmac;
 	u8			mac[ETH_ALEN];
+	u8                      lmac_type;
+	u8                      lane_to_sds;
+	bool                    use_training;
 	bool			link_up;
 	int			lmacid; /* ID within BGX */
 	int			lmacid_bd; /* ID on board */
@@ -43,14 +46,13 @@ struct lmac {
 
 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;
+	u8			max_lmac;
 	void __iomem		*reg_base;
 	struct pci_dev		*pdev;
+	bool                    is_dlm;
+	bool                    is_rgx;
 };
 
 static struct bgx *bgx_vnic[MAX_BGX_THUNDER];
@@ -61,6 +63,7 @@ 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) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_CAVIUM, PCI_DEVICE_ID_THUNDER_RGX) },
 	{ 0, }  /* end of table */
 };
 
@@ -124,8 +127,8 @@ 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])
+	for (i = 0; i < MAX_BGX_PER_NODE; i++) {
+		if (bgx_vnic[(node * MAX_BGX_PER_NODE) + i])
 			map |= (1 << i);
 	}
 
@@ -138,7 +141,7 @@ int bgx_get_lmac_count(int node, int bgx_idx)
 {
 	struct bgx *bgx;
 
-	bgx = bgx_vnic[(node * MAX_BGX_PER_CN88XX) + bgx_idx];
+	bgx = bgx_vnic[(node * MAX_BGX_PER_NODE) + bgx_idx];
 	if (bgx)
 		return bgx->lmac_count;
 
@@ -153,7 +156,7 @@ void bgx_get_lmac_link_state(int node, int bgx_idx, int lmacid, void *status)
 	struct bgx *bgx;
 	struct lmac *lmac;
 
-	bgx = bgx_vnic[(node * MAX_BGX_PER_CN88XX) + bgx_idx];
+	bgx = bgx_vnic[(node * MAX_BGX_PER_NODE) + bgx_idx];
 	if (!bgx)
 		return;
 
@@ -166,7 +169,7 @@ 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];
+	struct bgx *bgx = bgx_vnic[(node * MAX_BGX_PER_NODE) + bgx_idx];
 
 	if (bgx)
 		return bgx->lmac[lmacid].mac;
@@ -177,7 +180,7 @@ 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];
+	struct bgx *bgx = bgx_vnic[(node * MAX_BGX_PER_NODE) + bgx_idx];
 
 	if (!bgx)
 		return;
@@ -188,11 +191,13 @@ EXPORT_SYMBOL(bgx_set_lmac_mac);
 
 void bgx_lmac_rx_tx_enable(int node, int bgx_idx, int lmacid, bool enable)
 {
-	struct bgx *bgx = bgx_vnic[(node * MAX_BGX_PER_CN88XX) + bgx_idx];
+	struct bgx *bgx = bgx_vnic[(node * MAX_BGX_PER_NODE) + bgx_idx];
+	struct lmac *lmac;
 	u64 cfg;
 
 	if (!bgx)
 		return;
+	lmac = &bgx->lmac[lmacid];
 
 	cfg = bgx_reg_read(bgx, lmacid, BGX_CMRX_CFG);
 	if (enable)
@@ -200,6 +205,9 @@ void bgx_lmac_rx_tx_enable(int node, int bgx_idx, int lmacid, bool enable)
 	else
 		cfg &= ~(CMR_PKT_RX_EN | CMR_PKT_TX_EN);
 	bgx_reg_write(bgx, lmacid, BGX_CMRX_CFG, cfg);
+
+	if (bgx->is_rgx)
+		xcv_setup_link(enable ? lmac->link_up : 0, lmac->last_speed);
 }
 EXPORT_SYMBOL(bgx_lmac_rx_tx_enable);
 
@@ -266,9 +274,12 @@ static void bgx_sgmii_change_link_state(struct lmac *lmac)
 
 	port_cfg = bgx_reg_read(bgx, lmac->lmacid, BGX_GMP_GMI_PRTX_CFG);
 
-	/* renable lmac */
+	/* Re-enable lmac */
 	cmr_cfg |= CMR_EN;
 	bgx_reg_write(bgx, lmac->lmacid, BGX_CMRX_CFG, cmr_cfg);
+
+	if (bgx->is_rgx && (cmr_cfg & (CMR_PKT_RX_EN | CMR_PKT_TX_EN)))
+		xcv_setup_link(lmac->link_up, lmac->last_speed);
 }
 
 static void bgx_lmac_handler(struct net_device *netdev)
@@ -314,7 +325,7 @@ 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];
+	bgx = bgx_vnic[(node * MAX_BGX_PER_NODE) + bgx_idx];
 	if (!bgx)
 		return 0;
 
@@ -328,7 +339,7 @@ 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];
+	bgx = bgx_vnic[(node * MAX_BGX_PER_NODE) + bgx_idx];
 	if (!bgx)
 		return 0;
 
@@ -356,7 +367,7 @@ void bgx_lmac_internal_loopback(int node, int bgx_idx,
 	struct lmac *lmac;
 	u64    cfg;
 
-	bgx = bgx_vnic[(node * MAX_BGX_PER_CN88XX) + bgx_idx];
+	bgx = bgx_vnic[(node * MAX_BGX_PER_NODE) + bgx_idx];
 	if (!bgx)
 		return;
 
@@ -379,8 +390,9 @@ void bgx_lmac_internal_loopback(int node, int bgx_idx,
 }
 EXPORT_SYMBOL(bgx_lmac_internal_loopback);
 
-static int bgx_lmac_sgmii_init(struct bgx *bgx, int lmacid)
+static int bgx_lmac_sgmii_init(struct bgx *bgx, struct lmac *lmac)
 {
+	int lmacid = lmac->lmacid;
 	u64 cfg;
 
 	bgx_reg_modify(bgx, lmacid, BGX_GMP_GMI_TXX_THRESH, 0x30);
@@ -409,18 +421,29 @@ static int bgx_lmac_sgmii_init(struct bgx *bgx, int lmacid)
 	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;
+	if (lmac->lmac_type == BGX_MODE_QSGMII) {
+		/* Disable disparity check for QSGMII */
+		cfg = bgx_reg_read(bgx, lmacid, BGX_GMP_PCS_MISCX_CTL);
+		cfg &= ~PCS_MISC_CTL_DISP_EN;
+		bgx_reg_write(bgx, lmacid, BGX_GMP_PCS_MISCX_CTL, cfg);
+		return 0;
+	}
+
+	if (lmac->lmac_type == BGX_MODE_SGMII) {
+		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)
+static int bgx_lmac_xaui_init(struct bgx *bgx, struct lmac *lmac)
 {
 	u64 cfg;
+	int lmacid = lmac->lmacid;
 
 	/* Reset SPU */
 	bgx_reg_modify(bgx, lmacid, BGX_SPUX_CONTROL1, SPU_CTL_RESET);
@@ -436,12 +459,14 @@ static int bgx_lmac_xaui_init(struct bgx *bgx, int lmacid, int lmac_type)
 
 	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
+	if (lmac->lmac_type == BGX_MODE_RXAUI)
 		bgx_reg_modify(bgx, lmacid, BGX_SPUX_MISC_CONTROL,
-			       SPU_MISC_CTL_RX_DIS | SPU_MISC_CTL_INTLV_RDISP);
+			       SPU_MISC_CTL_INTLV_RDISP);
+
+	/* Clear receive packet disable */
+	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);
 
 	/* clear all interrupts */
 	cfg = bgx_reg_read(bgx, lmacid, BGX_SMUX_RX_INT);
@@ -451,7 +476,7 @@ static int bgx_lmac_xaui_init(struct bgx *bgx, int lmacid, int lmac_type)
 	cfg = bgx_reg_read(bgx, lmacid, BGX_SPUX_INT);
 	bgx_reg_write(bgx, lmacid, BGX_SPUX_INT, cfg);
 
-	if (bgx->use_training) {
+	if (lmac->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);
@@ -474,9 +499,9 @@ static int bgx_lmac_xaui_init(struct bgx *bgx, int lmacid, int lmac_type)
 	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)
+	if (lmac->lmac_type == BGX_MODE_10G_KR)
 		cfg |= (1 << 23);
-	else if (bgx->lmac_type == BGX_MODE_40G_KR)
+	else if (lmac->lmac_type == BGX_MODE_40G_KR)
 		cfg |= (1 << 24);
 	else
 		cfg &= ~((1 << 23) | (1 << 24));
@@ -511,11 +536,10 @@ static int bgx_xaui_check_link(struct lmac *lmac)
 {
 	struct bgx *bgx = lmac->bgx;
 	int lmacid = lmac->lmacid;
-	int lmac_type = bgx->lmac_type;
+	int lmac_type = lmac->lmac_type;
 	u64 cfg;
 
-	bgx_reg_modify(bgx, lmacid, BGX_SPUX_MISC_CONTROL, SPU_MISC_CTL_RX_DIS);
-	if (bgx->use_training) {
+	if (lmac->use_training) {
 		cfg = bgx_reg_read(bgx, lmacid, BGX_SPUX_INT);
 		if (!(cfg & (1ull << 13))) {
 			cfg = (1ull << 13) | (1ull << 14);
@@ -556,7 +580,7 @@ static int bgx_xaui_check_link(struct lmac *lmac)
 			       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) {
+		if (lmac->use_training) {
 			cfg = bgx_reg_read(bgx, lmacid, BGX_SPUX_INT);
 			if (!(cfg & (1ull << 13))) {
 				cfg = (1ull << 13) | (1ull << 14);
@@ -584,11 +608,6 @@ static int bgx_xaui_check_link(struct lmac *lmac)
 		return -1;
 	}
 
-	/* Clear receive packet disable */
-	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);
-
 	/* Check for MAC RX faults */
 	cfg = bgx_reg_read(bgx, lmacid, BGX_SMUX_RX_CTL);
 	/* 0 - Link is okay, 1 - Local fault, 2 - Remote fault */
@@ -599,7 +618,7 @@ static int bgx_xaui_check_link(struct lmac *lmac)
 	/* Rx local/remote fault seen.
 	 * Do lmac reinit to see if condition recovers
 	 */
-	bgx_lmac_xaui_init(bgx, lmacid, bgx->lmac_type);
+	bgx_lmac_xaui_init(bgx, lmac);
 
 	return -1;
 }
@@ -623,7 +642,7 @@ static void bgx_poll_for_link(struct work_struct *work)
 	if ((spu_link & SPU_STATUS1_RCV_LNK) &&
 	    !(smu_link & SMU_RX_CTL_STATUS)) {
 		lmac->link_up = 1;
-		if (lmac->bgx->lmac_type == BGX_MODE_XLAUI)
+		if (lmac->lmac_type == BGX_MODE_XLAUI)
 			lmac->last_speed = 40000;
 		else
 			lmac->last_speed = 10000;
@@ -649,6 +668,16 @@ static void bgx_poll_for_link(struct work_struct *work)
 	queue_delayed_work(lmac->check_link, &lmac->dwork, HZ * 2);
 }
 
+static int phy_interface_mode(u8 lmac_type)
+{
+	if (lmac_type == BGX_MODE_QSGMII)
+		return PHY_INTERFACE_MODE_QSGMII;
+	if (lmac_type == BGX_MODE_RGMII)
+		return PHY_INTERFACE_MODE_RGMII;
+
+	return PHY_INTERFACE_MODE_SGMII;
+}
+
 static int bgx_lmac_enable(struct bgx *bgx, u8 lmacid)
 {
 	struct lmac *lmac;
@@ -657,13 +686,15 @@ static int bgx_lmac_enable(struct bgx *bgx, u8 lmacid)
 	lmac = &bgx->lmac[lmacid];
 	lmac->bgx = bgx;
 
-	if (bgx->lmac_type == BGX_MODE_SGMII) {
+	if ((lmac->lmac_type == BGX_MODE_SGMII) ||
+	    (lmac->lmac_type == BGX_MODE_QSGMII) ||
+	    (lmac->lmac_type == BGX_MODE_RGMII)) {
 		lmac->is_sgmii = 1;
-		if (bgx_lmac_sgmii_init(bgx, lmacid))
+		if (bgx_lmac_sgmii_init(bgx, lmac))
 			return -1;
 	} else {
 		lmac->is_sgmii = 0;
-		if (bgx_lmac_xaui_init(bgx, lmacid, bgx->lmac_type))
+		if (bgx_lmac_xaui_init(bgx, lmac))
 			return -1;
 	}
 
@@ -685,10 +716,10 @@ static int bgx_lmac_enable(struct bgx *bgx, u8 lmacid)
 	/* 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->lmac_type != BGX_MODE_XFI) &&
+	    (lmac->lmac_type != BGX_MODE_XLAUI) &&
+	    (lmac->lmac_type != BGX_MODE_40G_KR) &&
+	    (lmac->lmac_type != BGX_MODE_10G_KR)) {
 		if (!lmac->phydev)
 			return -ENODEV;
 
@@ -696,7 +727,7 @@ static int bgx_lmac_enable(struct bgx *bgx, u8 lmacid)
 
 		if (phy_connect_direct(&lmac->netdev, lmac->phydev,
 				       bgx_lmac_handler,
-				       PHY_INTERFACE_MODE_SGMII))
+				       phy_interface_mode(lmac->lmac_type)))
 			return -ENODEV;
 
 		phy_start_aneg(lmac->phydev);
@@ -753,76 +784,19 @@ static void bgx_lmac_disable(struct bgx *bgx, u8 lmacid)
 
 	bgx_flush_dmac_addrs(bgx, lmacid);
 
-	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) && lmac->phydev)
+	if ((lmac->lmac_type != BGX_MODE_XFI) &&
+	    (lmac->lmac_type != BGX_MODE_XLAUI) &&
+	    (lmac->lmac_type != BGX_MODE_40G_KR) &&
+	    (lmac->lmac_type != BGX_MODE_10G_KR) && 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);
+	struct lmac *lmac;
 
 	bgx_reg_modify(bgx, 0, BGX_CMR_GLOBAL_CFG, CMR_GLOBAL_CFG_FCS_STRIP);
 	if (bgx_reg_read(bgx, 0, BGX_CMR_BIST_STATUS))
@@ -830,17 +804,9 @@ static void bgx_init_hw(struct bgx *bgx)
 
 	/* 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;
-		}
+		lmac = &bgx->lmac[i];
 		bgx_reg_write(bgx, i, BGX_CMRX_CFG,
-			      (bgx->lmac_type << 8) | (bgx->lane_to_sds + i));
+			      (lmac->lmac_type << 8) | lmac->lane_to_sds);
 		bgx->lmac[i].lmacid_bd = lmac_count;
 		lmac_count++;
 	}
@@ -863,55 +829,212 @@ static void bgx_init_hw(struct bgx *bgx)
 		bgx_reg_write(bgx, 0, BGX_CMR_RX_STREERING + (i * 8), 0x00);
 }
 
-static void bgx_get_qlm_mode(struct bgx *bgx)
+static u8 bgx_get_lane2sds_cfg(struct bgx *bgx, struct lmac *lmac)
+{
+	return (u8)(bgx_reg_read(bgx, lmac->lmacid, BGX_CMRX_CFG) & 0xFF);
+}
+
+static void bgx_print_qlm_mode(struct bgx *bgx, u8 lmacid)
 {
 	struct device *dev = &bgx->pdev->dev;
-	int lmac_type;
-	int train_en;
+	struct lmac *lmac;
+	char str[20];
+	u8 dlm;
 
-	/* 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;
+	if (lmacid > bgx->max_lmac)
+		return;
 
-	train_en = bgx_reg_read(bgx, 0, BGX_SPUX_BR_PMD_CRTL) &
-				SPU_PMD_CRTL_TRAIN_EN;
+	lmac = &bgx->lmac[lmacid];
+	dlm = (lmacid / 2) + (bgx->bgx_id * 2);
+	if (!bgx->is_dlm)
+		sprintf(str, "BGX%d QLM mode", bgx->bgx_id);
+	else
+		sprintf(str, "BGX%d DLM%d mode", bgx->bgx_id, dlm);
 
-	switch (lmac_type) {
+	switch (lmac->lmac_type) {
 	case BGX_MODE_SGMII:
-		bgx->qlm_mode = QLM_MODE_SGMII;
-		dev_info(dev, "BGX%d QLM mode: SGMII\n", bgx->bgx_id);
+		dev_info(dev, "%s: SGMII\n", (char *)str);
 		break;
 	case BGX_MODE_XAUI:
-		bgx->qlm_mode = QLM_MODE_XAUI_1X4;
-		dev_info(dev, "BGX%d QLM mode: XAUI\n", bgx->bgx_id);
+		dev_info(dev, "%s: XAUI\n", (char *)str);
 		break;
 	case BGX_MODE_RXAUI:
-		bgx->qlm_mode = QLM_MODE_RXAUI_2X2;
-		dev_info(dev, "BGX%d QLM mode: RXAUI\n", bgx->bgx_id);
+		dev_info(dev, "%s: RXAUI\n", (char *)str);
 		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);
-		}
+		if (!lmac->use_training)
+			dev_info(dev, "%s: XFI\n", (char *)str);
+		else
+			dev_info(dev, "%s: 10G_KR\n", (char *)str);
 		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);
-		}
+		if (!lmac->use_training)
+			dev_info(dev, "%s: XLAUI\n", (char *)str);
+		else
+			dev_info(dev, "%s: 40G_KR4\n", (char *)str);
+		break;
+	case BGX_MODE_QSGMII:
+		if ((lmacid == 0) &&
+		    (bgx_get_lane2sds_cfg(bgx, lmac) != lmacid))
+			return;
+		if ((lmacid == 2) &&
+		    (bgx_get_lane2sds_cfg(bgx, lmac) == lmacid))
+			return;
+		dev_info(dev, "%s: QSGMII\n", (char *)str);
+		break;
+	case BGX_MODE_RGMII:
+		dev_info(dev, "%s: RGMII\n", (char *)str);
+		break;
+	case BGX_MODE_INVALID:
+		/* Nothing to do */
+		break;
+	}
+}
+
+static void lmac_set_lane2sds(struct bgx *bgx, struct lmac *lmac)
+{
+	switch (lmac->lmac_type) {
+	case BGX_MODE_SGMII:
+	case BGX_MODE_XFI:
+		lmac->lane_to_sds = lmac->lmacid;
+		break;
+	case BGX_MODE_XAUI:
+	case BGX_MODE_XLAUI:
+	case BGX_MODE_RGMII:
+		lmac->lane_to_sds = 0xE4;
+		break;
+	case BGX_MODE_RXAUI:
+		lmac->lane_to_sds = (lmac->lmacid) ? 0xE : 0x4;
+		break;
+	case BGX_MODE_QSGMII:
+		/* There is no way to determine if DLM0/2 is QSGMII or
+		 * DLM1/3 is configured to QSGMII as bootloader will
+		 * configure all LMACs, so take whatever is configured
+		 * by low level firmware.
+		 */
+		lmac->lane_to_sds = bgx_get_lane2sds_cfg(bgx, lmac);
 		break;
 	default:
-		bgx->qlm_mode = QLM_MODE_SGMII;
-		dev_info(dev, "BGX%d QLM default mode: SGMII\n", bgx->bgx_id);
+		lmac->lane_to_sds = 0;
+		break;
+	}
+}
+
+static void lmac_set_training(struct bgx *bgx, struct lmac *lmac, int lmacid)
+{
+	if ((lmac->lmac_type != BGX_MODE_10G_KR) &&
+	    (lmac->lmac_type != BGX_MODE_40G_KR)) {
+		lmac->use_training = 0;
+		return;
+	}
+
+	lmac->use_training = bgx_reg_read(bgx, lmacid, BGX_SPUX_BR_PMD_CRTL) &
+							SPU_PMD_CRTL_TRAIN_EN;
+}
+
+static void bgx_set_lmac_config(struct bgx *bgx, u8 idx)
+{
+	struct lmac *lmac;
+	struct lmac *olmac;
+	u64 cmr_cfg;
+	u8 lmac_type;
+	u8 lane_to_sds;
+
+	lmac = &bgx->lmac[idx];
+
+	if (!bgx->is_dlm || bgx->is_rgx) {
+		/* Read LMAC0 type to figure out QLM mode
+		 * This is configured by low level firmware
+		 */
+		cmr_cfg = bgx_reg_read(bgx, 0, BGX_CMRX_CFG);
+		lmac->lmac_type = (cmr_cfg >> 8) & 0x07;
+		if (bgx->is_rgx)
+			lmac->lmac_type = BGX_MODE_RGMII;
+		lmac_set_training(bgx, lmac, 0);
+		lmac_set_lane2sds(bgx, lmac);
+		return;
+	}
+
+	/* On 81xx BGX can be split across 2 DLMs
+	 * firmware programs lmac_type of LMAC0 and LMAC2
+	 */
+	if ((idx == 0) || (idx == 2)) {
+		cmr_cfg = bgx_reg_read(bgx, idx, BGX_CMRX_CFG);
+		lmac_type = (u8)((cmr_cfg >> 8) & 0x07);
+		lane_to_sds = (u8)(cmr_cfg & 0xFF);
+		/* Check if config is not reset value */
+		if ((lmac_type == 0) && (lane_to_sds == 0xE4))
+			lmac->lmac_type = BGX_MODE_INVALID;
+		else
+			lmac->lmac_type = lmac_type;
+		lmac_set_training(bgx, lmac, lmac->lmacid);
+		lmac_set_lane2sds(bgx, lmac);
+
+		/* Set LMAC type of other lmac on same DLM i.e LMAC 1/3 */
+		olmac = &bgx->lmac[idx + 1];
+		olmac->lmac_type = lmac->lmac_type;
+		lmac_set_training(bgx, olmac, olmac->lmacid);
+		lmac_set_lane2sds(bgx, olmac);
+	}
+}
+
+static bool is_dlm0_in_bgx_mode(struct bgx *bgx)
+{
+	struct lmac *lmac;
+
+	if (!bgx->is_dlm)
+		return true;
+
+	lmac = &bgx->lmac[0];
+	if (lmac->lmac_type == BGX_MODE_INVALID)
+		return false;
+
+	return true;
+}
+
+static void bgx_get_qlm_mode(struct bgx *bgx)
+{
+	struct lmac *lmac;
+	struct lmac *lmac01;
+	struct lmac *lmac23;
+	u8  idx;
+
+	/* Init all LMAC's type to invalid */
+	for (idx = 0; idx < bgx->max_lmac; idx++) {
+		lmac = &bgx->lmac[idx];
+		lmac->lmacid = idx;
+		lmac->lmac_type = BGX_MODE_INVALID;
+		lmac->use_training = false;
+	}
+
+	/* It is assumed that low level firmware sets this value */
+	bgx->lmac_count = bgx_reg_read(bgx, 0, BGX_CMR_RX_LMACS) & 0x7;
+	if (bgx->lmac_count > bgx->max_lmac)
+		bgx->lmac_count = bgx->max_lmac;
+
+	for (idx = 0; idx < bgx->max_lmac; idx++)
+		bgx_set_lmac_config(bgx, idx);
+
+	if (!bgx->is_dlm || bgx->is_rgx) {
+		bgx_print_qlm_mode(bgx, 0);
+		return;
+	}
+
+	if (bgx->lmac_count) {
+		bgx_print_qlm_mode(bgx, 0);
+		bgx_print_qlm_mode(bgx, 2);
+	}
+
+	/* If DLM0 is not in BGX mode then LMAC0/1 have
+	 * to be configured with serdes lanes of DLM1
+	 */
+	if (is_dlm0_in_bgx_mode(bgx) || (bgx->lmac_count > 2))
+		return;
+	for (idx = 0; idx < bgx->lmac_count; idx++) {
+		lmac01 = &bgx->lmac[idx];
+		lmac23 = &bgx->lmac[idx + 2];
+		lmac01->lmac_type = lmac23->lmac_type;
+		lmac01->lane_to_sds = lmac23->lane_to_sds;
 	}
 }
 
@@ -1042,7 +1165,7 @@ static int bgx_init_of_phy(struct bgx *bgx)
 		}
 
 		lmac++;
-		if (lmac == MAX_LMAC_PER_BGX) {
+		if (lmac == bgx->max_lmac) {
 			of_node_put(node);
 			break;
 		}
@@ -1087,6 +1210,7 @@ static int bgx_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	struct device *dev = &pdev->dev;
 	struct bgx *bgx = NULL;
 	u8 lmac;
+	u16 sdevid;
 
 	bgx = devm_kzalloc(dev, sizeof(*bgx), GFP_KERNEL);
 	if (!bgx)
@@ -1115,10 +1239,30 @@ static int bgx_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 		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;
+	pci_read_config_word(pdev, PCI_DEVICE_ID, &sdevid);
+	if (sdevid != PCI_DEVICE_ID_THUNDER_RGX) {
+		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_NODE;
+		bgx->max_lmac = MAX_LMAC_PER_BGX;
+		bgx_vnic[bgx->bgx_id] = bgx;
+	} else {
+		bgx->is_rgx = true;
+		bgx->max_lmac = 1;
+		bgx->bgx_id = MAX_BGX_PER_CN81XX - 1;
+		bgx_vnic[bgx->bgx_id] = bgx;
+		xcv_init_hw();
+	}
+
+	/* On 81xx all are DLMs and on 83xx there are 3 BGX QLMs and one
+	 * BGX i.e BGX2 can be split across 2 DLMs.
+	 */
+	pci_read_config_word(pdev, PCI_SUBSYSTEM_ID, &sdevid);
+	if ((sdevid == PCI_SUBSYS_DEVID_81XX_BGX) ||
+	    ((sdevid == PCI_SUBSYS_DEVID_83XX_BGX) && (bgx->bgx_id == 2)))
+		bgx->is_dlm = true;
+
 	bgx_get_qlm_mode(bgx);
 
 	err = bgx_init_phy(bgx);
@@ -1133,6 +1277,8 @@ static int bgx_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 		if (err) {
 			dev_err(dev, "BGX%d failed to enable lmac%d\n",
 				bgx->bgx_id, lmac);
+			while (lmac)
+				bgx_lmac_disable(bgx, --lmac);
 			goto err_enable;
 		}
 	}
diff --git a/drivers/net/ethernet/cavium/thunder/thunder_bgx.h b/drivers/net/ethernet/cavium/thunder/thunder_bgx.h
index 42010d2e5ddf..d59c71e4a000 100644
--- a/drivers/net/ethernet/cavium/thunder/thunder_bgx.h
+++ b/drivers/net/ethernet/cavium/thunder/thunder_bgx.h
@@ -9,8 +9,20 @@
 #ifndef THUNDER_BGX_H
 #define THUNDER_BGX_H
 
-#define    MAX_BGX_THUNDER			8 /* Max 4 nodes, 2 per node */
+/* PCI device ID */
+#define	PCI_DEVICE_ID_THUNDER_BGX		0xA026
+#define	PCI_DEVICE_ID_THUNDER_RGX		0xA054
+
+/* Subsystem device IDs */
+#define PCI_SUBSYS_DEVID_88XX_BGX		0xA126
+#define PCI_SUBSYS_DEVID_81XX_BGX		0xA226
+#define PCI_SUBSYS_DEVID_83XX_BGX		0xA326
+
+#define    MAX_BGX_THUNDER			8 /* Max 2 nodes, 4 per node */
 #define    MAX_BGX_PER_CN88XX			2
+#define    MAX_BGX_PER_CN81XX			3 /* 2 BGXs + 1 RGX */
+#define    MAX_BGX_PER_CN83XX			4
+#define    MAX_BGX_PER_NODE			4
 #define    MAX_LMAC_PER_BGX			4
 #define    MAX_BGX_CHANS_PER_LMAC		16
 #define    MAX_DMAC_PER_LMAC			8
@@ -18,8 +30,6 @@
 
 #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)
@@ -136,6 +146,7 @@
 #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_DISP_EN			BIT_ULL(13)
 #define  PCS_MISC_CTL_GMX_ENO			BIT_ULL(11)
 #define  PCS_MISC_CTL_SAMP_PT_MASK	0x7Full
 #define BGX_GMP_GMI_PRTX_CFG		0x38020
@@ -194,6 +205,9 @@ 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);
 void bgx_lmac_internal_loopback(int node, int bgx_idx,
 				int lmac_idx, bool enable);
+void xcv_init_hw(void);
+void xcv_setup_link(bool link_up, int link_speed);
+
 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
@@ -213,16 +227,9 @@ enum LMAC_TYPE {
 	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 */
+	BGX_MODE_RGMII = 5,
+	BGX_MODE_QSGMII = 6,
+	BGX_MODE_INVALID = 7,
 };
 
 #endif /* THUNDER_BGX_H */
diff --git a/drivers/net/ethernet/cavium/thunder/thunder_xcv.c b/drivers/net/ethernet/cavium/thunder/thunder_xcv.c
new file mode 100644
index 000000000000..67befedef709
--- /dev/null
+++ b/drivers/net/ethernet/cavium/thunder/thunder_xcv.c
@@ -0,0 +1,235 @@
+/*
+ * Copyright (C) 2016 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/acpi.h>
+#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.h"
+#include "thunder_bgx.h"
+
+#define DRV_NAME	"thunder-xcv"
+#define DRV_VERSION	"1.0"
+
+/* Register offsets */
+#define XCV_RESET		0x00
+#define   PORT_EN		BIT_ULL(63)
+#define   CLK_RESET		BIT_ULL(15)
+#define   DLL_RESET		BIT_ULL(11)
+#define   COMP_EN		BIT_ULL(7)
+#define   TX_PKT_RESET		BIT_ULL(3)
+#define   TX_DATA_RESET		BIT_ULL(2)
+#define   RX_PKT_RESET		BIT_ULL(1)
+#define   RX_DATA_RESET		BIT_ULL(0)
+#define XCV_DLL_CTL		0x10
+#define   CLKRX_BYP		BIT_ULL(23)
+#define   CLKTX_BYP		BIT_ULL(15)
+#define XCV_COMP_CTL		0x20
+#define   DRV_BYP		BIT_ULL(63)
+#define XCV_CTL			0x30
+#define XCV_INT			0x40
+#define XCV_INT_W1S		0x48
+#define XCV_INT_ENA_W1C		0x50
+#define XCV_INT_ENA_W1S		0x58
+#define XCV_INBND_STATUS	0x80
+#define XCV_BATCH_CRD_RET	0x100
+
+struct xcv {
+	void __iomem		*reg_base;
+	struct pci_dev		*pdev;
+};
+
+static struct xcv *xcv;
+
+/* Supported devices */
+static const struct pci_device_id xcv_id_table[] = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_CAVIUM, 0xA056) },
+	{ 0, }  /* end of table */
+};
+
+MODULE_AUTHOR("Cavium Inc");
+MODULE_DESCRIPTION("Cavium Thunder RGX/XCV Driver");
+MODULE_LICENSE("GPL v2");
+MODULE_VERSION(DRV_VERSION);
+MODULE_DEVICE_TABLE(pci, xcv_id_table);
+
+void xcv_init_hw(void)
+{
+	u64  cfg;
+
+	/* Take DLL out of reset */
+	cfg = readq_relaxed(xcv->reg_base + XCV_RESET);
+	cfg &= ~DLL_RESET;
+	writeq_relaxed(cfg, xcv->reg_base + XCV_RESET);
+
+	/* Take clock tree out of reset */
+	cfg = readq_relaxed(xcv->reg_base + XCV_RESET);
+	cfg &= ~CLK_RESET;
+	writeq_relaxed(cfg, xcv->reg_base + XCV_RESET);
+	/* Wait for DLL to lock */
+	msleep(1);
+
+	/* Configure DLL - enable or bypass
+	 * TX no bypass, RX bypass
+	 */
+	cfg = readq_relaxed(xcv->reg_base + XCV_DLL_CTL);
+	cfg &= ~0xFF03;
+	cfg |= CLKRX_BYP;
+	writeq_relaxed(cfg, xcv->reg_base + XCV_DLL_CTL);
+
+	/* Enable compensation controller and force the
+	 * write to be visible to HW by readig back.
+	 */
+	cfg = readq_relaxed(xcv->reg_base + XCV_RESET);
+	cfg |= COMP_EN;
+	writeq_relaxed(cfg, xcv->reg_base + XCV_RESET);
+	readq_relaxed(xcv->reg_base + XCV_RESET);
+	/* Wait for compensation state machine to lock */
+	msleep(10);
+
+	/* enable the XCV block */
+	cfg = readq_relaxed(xcv->reg_base + XCV_RESET);
+	cfg |= PORT_EN;
+	writeq_relaxed(cfg, xcv->reg_base + XCV_RESET);
+
+	cfg = readq_relaxed(xcv->reg_base + XCV_RESET);
+	cfg |= CLK_RESET;
+	writeq_relaxed(cfg, xcv->reg_base + XCV_RESET);
+}
+EXPORT_SYMBOL(xcv_init_hw);
+
+void xcv_setup_link(bool link_up, int link_speed)
+{
+	u64  cfg;
+	int speed = 2;
+
+	if (!xcv) {
+		dev_err(&xcv->pdev->dev,
+			"XCV init not done, probe may have failed\n");
+		return;
+	}
+
+	if (link_speed == 100)
+		speed = 1;
+	else if (link_speed == 10)
+		speed = 0;
+
+	if (link_up) {
+		/* set operating speed */
+		cfg = readq_relaxed(xcv->reg_base + XCV_CTL);
+		cfg &= ~0x03;
+		cfg |= speed;
+		writeq_relaxed(cfg, xcv->reg_base + XCV_CTL);
+
+		/* Reset datapaths */
+		cfg = readq_relaxed(xcv->reg_base + XCV_RESET);
+		cfg |= TX_DATA_RESET | RX_DATA_RESET;
+		writeq_relaxed(cfg, xcv->reg_base + XCV_RESET);
+
+		/* Enable the packet flow */
+		cfg = readq_relaxed(xcv->reg_base + XCV_RESET);
+		cfg |= TX_PKT_RESET | RX_PKT_RESET;
+		writeq_relaxed(cfg, xcv->reg_base + XCV_RESET);
+
+		/* Return credits to RGX */
+		writeq_relaxed(0x01, xcv->reg_base + XCV_BATCH_CRD_RET);
+	} else {
+		/* Disable packet flow */
+		cfg = readq_relaxed(xcv->reg_base + XCV_RESET);
+		cfg &= ~(TX_PKT_RESET | RX_PKT_RESET);
+		writeq_relaxed(cfg, xcv->reg_base + XCV_RESET);
+		readq_relaxed(xcv->reg_base + XCV_RESET);
+	}
+}
+EXPORT_SYMBOL(xcv_setup_link);
+
+static int xcv_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
+{
+	int err;
+	struct device *dev = &pdev->dev;
+
+	xcv = devm_kzalloc(dev, sizeof(struct xcv), GFP_KERNEL);
+	if (!xcv)
+		return -ENOMEM;
+	xcv->pdev = pdev;
+
+	pci_set_drvdata(pdev, xcv);
+
+	err = pci_enable_device(pdev);
+	if (err) {
+		dev_err(dev, "Failed to enable PCI device\n");
+		goto err_kfree;
+	}
+
+	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 */
+	xcv->reg_base = pcim_iomap(pdev, PCI_CFG_REG_BAR_NUM, 0);
+	if (!xcv->reg_base) {
+		dev_err(dev, "XCV: Cannot map CSR memory space, aborting\n");
+		err = -ENOMEM;
+		goto err_release_regions;
+	}
+
+	return 0;
+
+err_release_regions:
+	pci_release_regions(pdev);
+err_disable_device:
+	pci_disable_device(pdev);
+err_kfree:
+	devm_kfree(dev, xcv);
+	xcv = NULL;
+	return err;
+}
+
+static void xcv_remove(struct pci_dev *pdev)
+{
+	struct device *dev = &pdev->dev;
+
+	if (xcv) {
+		devm_kfree(dev, xcv);
+		xcv = NULL;
+	}
+
+	pci_release_regions(pdev);
+	pci_disable_device(pdev);
+}
+
+static struct pci_driver xcv_driver = {
+	.name = DRV_NAME,
+	.id_table = xcv_id_table,
+	.probe = xcv_probe,
+	.remove = xcv_remove,
+};
+
+static int __init xcv_init_module(void)
+{
+	pr_info("%s, ver %s\n", DRV_NAME, DRV_VERSION);
+
+	return pci_register_driver(&xcv_driver);
+}
+
+static void __exit xcv_cleanup_module(void)
+{
+	pci_unregister_driver(&xcv_driver);
+}
+
+module_init(xcv_init_module);
+module_exit(xcv_cleanup_module);
diff --git a/drivers/net/ethernet/chelsio/cxgb4/Makefile b/drivers/net/ethernet/chelsio/cxgb4/Makefile
index ace0ab98d0f1..c6b71f656992 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/Makefile
+++ b/drivers/net/ethernet/chelsio/cxgb4/Makefile
@@ -4,7 +4,7 @@
 
 obj-$(CONFIG_CHELSIO_T4) += cxgb4.o
 
-cxgb4-objs := cxgb4_main.o l2t.o t4_hw.o sge.o clip_tbl.o cxgb4_ethtool.o
+cxgb4-objs := cxgb4_main.o l2t.o t4_hw.o sge.o clip_tbl.o cxgb4_ethtool.o cxgb4_uld.o sched.o cxgb4_filter.o cxgb4_tc_u32.o
 cxgb4-$(CONFIG_CHELSIO_T4_DCB) +=  cxgb4_dcb.o
 cxgb4-$(CONFIG_CHELSIO_T4_FCOE) +=  cxgb4_fcoe.o
 cxgb4-$(CONFIG_DEBUG_FS) += cxgb4_debugfs.o
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h b/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h
index edd23386b47d..2125903043fb 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h
@@ -1,7 +1,7 @@
 /*
  * This file is part of the Chelsio T4 Ethernet driver for Linux.
  *
- * Copyright (c) 2003-2014 Chelsio Communications, Inc. All rights reserved.
+ * Copyright (c) 2003-2016 Chelsio Communications, Inc. All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
@@ -53,6 +53,8 @@
 #include "cxgb4_uld.h"
 
 #define CH_WARN(adap, fmt, ...) dev_warn(adap->pdev_dev, fmt, ## __VA_ARGS__)
+extern struct list_head adapter_list;
+extern struct mutex uld_mutex;
 
 enum {
 	MAX_NPORTS	= 4,     /* max # of ports */
@@ -338,14 +340,17 @@ struct adapter_params {
 	enum chip_type chip;               /* chip code */
 	struct arch_specific_params arch;  /* chip specific params */
 	unsigned char offload;
+	unsigned char crypto;		/* HW capability for crypto */
 
 	unsigned char bypass;
 
 	unsigned int ofldq_wr_cred;
 	bool ulptx_memwrite_dsgl;          /* use of T5 DSGL allowed */
 
+	unsigned int nsched_cls;          /* number of traffic classes */
 	unsigned int max_ordird_qp;       /* Max read depth per RDMA QP */
 	unsigned int max_ird_adapter;     /* Max read depth per adapter */
+	bool fr_nsmr_tpte_wr_support;	  /* FW support for FR_NSMR_TPTE_WR */
 };
 
 /* State needed to monitor the forward progress of SGE Ingress DMA activities
@@ -403,7 +408,6 @@ struct fw_info {
 	struct fw_hdr fw_hdr;
 };
 
-
 struct trace_params {
 	u32 data[TRACE_LEN / 4];
 	u32 mask[TRACE_LEN / 4];
@@ -434,11 +438,6 @@ enum {
 	MAX_ETH_QSETS = 32,           /* # of Ethernet Tx/Rx queue sets */
 	MAX_OFLD_QSETS = 16,          /* # of offload Tx, iscsi Rx queue sets */
 	MAX_CTRL_QUEUES = NCHAN,      /* # of control Tx queues */
-	MAX_RDMA_QUEUES = NCHAN,      /* # of streaming RDMA Rx queues */
-	MAX_RDMA_CIQS = 32,        /* # of  RDMA concentrator IQs */
-
-	/* # of streaming iSCSIT Rx queues */
-	MAX_ISCSIT_QUEUES = MAX_OFLD_QSETS,
 };
 
 enum {
@@ -455,8 +454,7 @@ enum {
 enum {
 	INGQ_EXTRAS = 2,        /* firmware event queue and */
 				/*   forwarded interrupts */
-	MAX_INGQ = MAX_ETH_QSETS + MAX_OFLD_QSETS + MAX_RDMA_QUEUES +
-		   MAX_RDMA_CIQS + MAX_ISCSIT_QUEUES + INGQ_EXTRAS,
+	MAX_INGQ = MAX_ETH_QSETS + INGQ_EXTRAS,
 };
 
 struct adapter;
@@ -493,6 +491,7 @@ struct port_info {
 #endif /* CONFIG_CHELSIO_T4_FCOE */
 	bool rxtstamp;  /* Enable TS */
 	struct hwtstamp_config tstamp_config;
+	struct sched_table *sched_tbl;
 };
 
 struct dentry;
@@ -510,6 +509,10 @@ enum {                                 /* adapter flags */
 	FW_OFLD_CONN       = (1 << 9),
 };
 
+enum {
+	ULP_CRYPTO_LOOKASIDE = 1 << 0,
+};
+
 struct rx_sw_desc;
 
 struct sge_fl {                     /* SGE free-buffer queue state */
@@ -680,17 +683,24 @@ struct sge_ctrl_txq {               /* state for an SGE control Tx queue */
 	u8 full;                    /* the Tx ring is full */
 } ____cacheline_aligned_in_smp;
 
+struct sge_uld_rxq_info {
+	char name[IFNAMSIZ];	/* name of ULD driver */
+	struct sge_ofld_rxq *uldrxq; /* Rxq's for ULD */
+	u16 *msix_tbl;		/* msix_tbl for uld */
+	u16 *rspq_id;		/* response queue id's of rxq */
+	u16 nrxq;		/* # of ingress uld queues */
+	u16 nciq;		/* # of completion queues */
+	u8 uld;			/* uld type */
+};
+
 struct sge {
 	struct sge_eth_txq ethtxq[MAX_ETH_QSETS];
 	struct sge_ofld_txq ofldtxq[MAX_OFLD_QSETS];
 	struct sge_ctrl_txq ctrlq[MAX_CTRL_QUEUES];
 
 	struct sge_eth_rxq ethrxq[MAX_ETH_QSETS];
-	struct sge_ofld_rxq iscsirxq[MAX_OFLD_QSETS];
-	struct sge_ofld_rxq iscsitrxq[MAX_ISCSIT_QUEUES];
-	struct sge_ofld_rxq rdmarxq[MAX_RDMA_QUEUES];
-	struct sge_ofld_rxq rdmaciq[MAX_RDMA_CIQS];
 	struct sge_rspq fw_evtq ____cacheline_aligned_in_smp;
+	struct sge_uld_rxq_info **uld_rxq_info;
 
 	struct sge_rspq intrq ____cacheline_aligned_in_smp;
 	spinlock_t intrq_lock;
@@ -698,14 +708,8 @@ struct sge {
 	u16 max_ethqsets;           /* # of available Ethernet queue sets */
 	u16 ethqsets;               /* # of active Ethernet queue sets */
 	u16 ethtxq_rover;           /* Tx queue to clean up next */
-	u16 iscsiqsets;              /* # of active iSCSI queue sets */
-	u16 niscsitq;               /* # of available iSCST Rx queues */
-	u16 rdmaqs;                 /* # of available RDMA Rx queues */
-	u16 rdmaciqs;               /* # of available RDMA concentrator IQs */
-	u16 iscsi_rxq[MAX_OFLD_QSETS];
-	u16 iscsit_rxq[MAX_ISCSIT_QUEUES];
-	u16 rdma_rxq[MAX_RDMA_QUEUES];
-	u16 rdma_ciq[MAX_RDMA_CIQS];
+	u16 ofldqsets;              /* # of active ofld queue sets */
+	u16 nqs_per_uld;	    /* # of Rx queues per ULD */
 	u16 timer_val[SGE_NTIMERS];
 	u8 counter_val[SGE_NCOUNTERS];
 	u32 fl_pg_order;            /* large page allocation size */
@@ -729,10 +733,7 @@ struct sge {
 };
 
 #define for_each_ethrxq(sge, i) for (i = 0; i < (sge)->ethqsets; i++)
-#define for_each_iscsirxq(sge, i) for (i = 0; i < (sge)->iscsiqsets; i++)
-#define for_each_iscsitrxq(sge, i) for (i = 0; i < (sge)->niscsitq; i++)
-#define for_each_rdmarxq(sge, i) for (i = 0; i < (sge)->rdmaqs; i++)
-#define for_each_rdmaciq(sge, i) for (i = 0; i < (sge)->rdmaciqs; i++)
+#define for_each_ofldtxq(sge, i) for (i = 0; i < (sge)->ofldqsets; i++)
 
 struct l2t_data;
 
@@ -757,6 +758,23 @@ struct hash_mac_addr {
 	u8 addr[ETH_ALEN];
 };
 
+struct uld_msix_bmap {
+	unsigned long *msix_bmap;
+	unsigned int mapsize;
+	spinlock_t lock; /* lock for acquiring bitmap */
+};
+
+struct uld_msix_info {
+	unsigned short vec;
+	char desc[IFNAMSIZ + 10];
+	unsigned int idx;
+};
+
+struct vf_info {
+	unsigned char vf_mac_addr[ETH_ALEN];
+	bool pf_set_mac;
+};
+
 struct adapter {
 	void __iomem *regs;
 	void __iomem *bar2;
@@ -767,6 +785,7 @@ struct adapter {
 	unsigned int mbox;
 	unsigned int pf;
 	unsigned int flags;
+	unsigned int adap_idx;
 	enum chip_type chip;
 
 	int msg_enable;
@@ -779,6 +798,9 @@ struct adapter {
 		unsigned short vec;
 		char desc[IFNAMSIZ + 10];
 	} msix_info[MAX_INGQ + 1];
+	struct uld_msix_info *msix_info_ulds; /* msix info for uld's */
+	struct uld_msix_bmap msix_bmap_ulds; /* msix bitmap for all uld */
+	int msi_idx;
 
 	struct doorbell_stats db_stats;
 	struct sge sge;
@@ -786,6 +808,9 @@ struct adapter {
 	struct net_device *port[MAX_NPORTS];
 	u8 chan_map[NCHAN];                   /* channel -> port map */
 
+	struct vf_info *vfinfo;
+	u8 num_vfs;
+
 	u32 filter_mode;
 	unsigned int l2t_start;
 	unsigned int l2t_end;
@@ -793,7 +818,10 @@ struct adapter {
 	unsigned int clipt_start;
 	unsigned int clipt_end;
 	struct clip_tbl *clipt;
+	struct cxgb4_uld_info *uld;
 	void *uld_handle[CXGB4_ULD_MAX];
+	unsigned int num_uld;
+	unsigned int num_ofld_uld;
 	struct list_head list_node;
 	struct list_head rcu_node;
 	struct list_head mac_hlist; /* list of MAC addresses in MPS Hash */
@@ -813,6 +841,8 @@ struct adapter {
 #define T4_OS_LOG_MBOX_CMDS 256
 	struct mbox_cmd_log *mbox_log;
 
+	struct mutex uld_mutex;
+
 	struct dentry *debugfs_root;
 	bool use_bd;     /* Use SGE Back Door intfc for reading SGE Contexts */
 	bool trace_rss;	/* 1 implies that different RSS flit per filter is
@@ -822,6 +852,58 @@ struct adapter {
 
 	spinlock_t stats_lock;
 	spinlock_t win0_lock ____cacheline_aligned_in_smp;
+
+	/* TC u32 offload */
+	struct cxgb4_tc_u32_table *tc_u32;
+};
+
+/* Support for "sched-class" command to allow a TX Scheduling Class to be
+ * programmed with various parameters.
+ */
+struct ch_sched_params {
+	s8   type;                     /* packet or flow */
+	union {
+		struct {
+			s8   level;    /* scheduler hierarchy level */
+			s8   mode;     /* per-class or per-flow */
+			s8   rateunit; /* bit or packet rate */
+			s8   ratemode; /* %port relative or kbps absolute */
+			s8   channel;  /* scheduler channel [0..N] */
+			s8   class;    /* scheduler class [0..N] */
+			s32  minrate;  /* minimum rate */
+			s32  maxrate;  /* maximum rate */
+			s16  weight;   /* percent weight */
+			s16  pktsize;  /* average packet size */
+		} params;
+	} u;
+};
+
+enum {
+	SCHED_CLASS_TYPE_PACKET = 0,    /* class type */
+};
+
+enum {
+	SCHED_CLASS_LEVEL_CL_RL = 0,    /* class rate limiter */
+};
+
+enum {
+	SCHED_CLASS_MODE_CLASS = 0,     /* per-class scheduling */
+};
+
+enum {
+	SCHED_CLASS_RATEUNIT_BITS = 0,  /* bit rate scheduling */
+};
+
+enum {
+	SCHED_CLASS_RATEMODE_ABS = 1,   /* Kb/s */
+};
+
+/* Support for "sched_queue" command to allow one or more NIC TX Queues
+ * to be bound to a TX Scheduling Class.
+ */
+struct ch_sched_queue {
+	s8   queue;    /* queue index */
+	s8   class;    /* class index */
 };
 
 /* Defined bit width of user definable filter tuples
@@ -947,11 +1029,47 @@ enum {
 	VLAN_REWRITE
 };
 
+/* Host shadow copy of ingress filter entry.  This is in host native format
+ * and doesn't match the ordering or bit order, etc. of the hardware of the
+ * firmware command.  The use of bit-field structure elements is purely to
+ * remind ourselves of the field size limitations and save memory in the case
+ * where the filter table is large.
+ */
+struct filter_entry {
+	/* Administrative fields for filter. */
+	u32 valid:1;            /* filter allocated and valid */
+	u32 locked:1;           /* filter is administratively locked */
+
+	u32 pending:1;          /* filter action is pending firmware reply */
+	u32 smtidx:8;           /* Source MAC Table index for smac */
+	struct filter_ctx *ctx; /* Caller's completion hook */
+	struct l2t_entry *l2t;  /* Layer Two Table entry for dmac */
+	struct net_device *dev; /* Associated net device */
+	u32 tid;                /* This will store the actual tid */
+
+	/* The filter itself.  Most of this is a straight copy of information
+	 * provided by the extended ioctl().  Some fields are translated to
+	 * internal forms -- for instance the Ingress Queue ID passed in from
+	 * the ioctl() is translated into the Absolute Ingress Queue ID.
+	 */
+	struct ch_filter_specification fs;
+};
+
 static inline int is_offload(const struct adapter *adap)
 {
 	return adap->params.offload;
 }
 
+static inline int is_pci_uld(const struct adapter *adap)
+{
+	return adap->params.crypto;
+}
+
+static inline int is_uld(const struct adapter *adap)
+{
+	return (adap->params.offload || adap->params.crypto);
+}
+
 static inline u32 t4_read_reg(struct adapter *adap, u32 reg_addr)
 {
 	return readl(adap->regs + reg_addr);
@@ -1178,6 +1296,8 @@ int t4_sge_alloc_eth_txq(struct adapter *adap, struct sge_eth_txq *txq,
 int t4_sge_alloc_ctrl_txq(struct adapter *adap, struct sge_ctrl_txq *txq,
 			  struct net_device *dev, unsigned int iqid,
 			  unsigned int cmplqid);
+int t4_sge_mod_ctrl_txq(struct adapter *adap, unsigned int eqid,
+			unsigned int cmplqid);
 int t4_sge_alloc_ofld_txq(struct adapter *adap, struct sge_ofld_txq *txq,
 			  struct net_device *dev, unsigned int iqid);
 irqreturn_t t4_sge_intr_msix(int irq, void *cookie);
@@ -1185,8 +1305,6 @@ int t4_sge_init(struct adapter *adap);
 void t4_sge_start(struct adapter *adap);
 void t4_sge_stop(struct adapter *adap);
 int cxgb_busy_poll(struct napi_struct *napi);
-int cxgb4_set_rspq_intr_params(struct sge_rspq *q, unsigned int us,
-			       unsigned int cnt);
 void cxgb4_set_ethtool_ops(struct net_device *netdev);
 int cxgb4_write_rss(const struct port_info *pi, const u16 *queues);
 extern int dbfifo_int_thresh;
@@ -1289,6 +1407,18 @@ static inline int hash_mac_addr(const u8 *addr)
 	return a & 0x3f;
 }
 
+int cxgb4_set_rspq_intr_params(struct sge_rspq *q, unsigned int us,
+			       unsigned int cnt);
+static inline void init_rspq(struct adapter *adap, struct sge_rspq *q,
+			     unsigned int us, unsigned int cnt,
+			     unsigned int size, unsigned int iqe_size)
+{
+	q->adap = adap;
+	cxgb4_set_rspq_intr_params(q, us, cnt);
+	q->iqe_len = iqe_size;
+	q->size = size;
+}
+
 void t4_write_indirect(struct adapter *adap, unsigned int addr_reg,
 		       unsigned int data_reg, const u32 *vals,
 		       unsigned int nregs, unsigned int start_idx);
@@ -1514,6 +1644,9 @@ void t4_get_trace_filter(struct adapter *adapter, struct trace_params *tp,
 			 int filter_index, int *enabled);
 int t4_fwaddrspace_write(struct adapter *adap, unsigned int mbox,
 			 u32 addr, u32 val);
+int t4_sched_params(struct adapter *adapter, int type, int level, int mode,
+		    int rateunit, int ratemode, int channel, int class,
+		    int minrate, int maxrate, int weight, int pktsize);
 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,
@@ -1521,4 +1654,11 @@ void t4_idma_monitor_init(struct adapter *adapter,
 void t4_idma_monitor(struct adapter *adapter,
 		     struct sge_idma_monitor_state *idma,
 		     int hz, int ticks);
+int t4_set_vf_mac_acl(struct adapter *adapter, unsigned int vf,
+		      unsigned int naddr, u8 *addr);
+void t4_uld_mem_free(struct adapter *adap);
+int t4_uld_mem_alloc(struct adapter *adap);
+void t4_uld_clean_up(struct adapter *adap);
+void t4_register_netevent_notifier(void);
+void free_rspq_fl(struct adapter *adap, struct sge_rspq *rq, struct sge_fl *fl);
 #endif /* __CXGB4_H__ */
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_debugfs.c b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_debugfs.c
index 91fb50850fff..20455d082cb8 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_debugfs.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_debugfs.c
@@ -2432,17 +2432,11 @@ static int sge_qinfo_show(struct seq_file *seq, void *v)
 {
 	struct adapter *adap = seq->private;
 	int eth_entries = DIV_ROUND_UP(adap->sge.ethqsets, 4);
-	int iscsi_entries = DIV_ROUND_UP(adap->sge.iscsiqsets, 4);
-	int iscsit_entries = DIV_ROUND_UP(adap->sge.niscsitq, 4);
-	int rdma_entries = DIV_ROUND_UP(adap->sge.rdmaqs, 4);
-	int ciq_entries = DIV_ROUND_UP(adap->sge.rdmaciqs, 4);
+	int ofld_entries = DIV_ROUND_UP(adap->sge.ofldqsets, 4);
 	int ctrl_entries = DIV_ROUND_UP(MAX_CTRL_QUEUES, 4);
 	int i, r = (uintptr_t)v - 1;
-	int iscsi_idx = r - eth_entries;
-	int iscsit_idx = iscsi_idx - iscsi_entries;
-	int rdma_idx = iscsit_idx - iscsit_entries;
-	int ciq_idx = rdma_idx - rdma_entries;
-	int ctrl_idx =  ciq_idx - ciq_entries;
+	int ofld_idx = r - eth_entries;
+	int ctrl_idx =  ofld_idx - ofld_entries;
 	int fq_idx =  ctrl_idx - ctrl_entries;
 
 	if (r)
@@ -2518,119 +2512,17 @@ do { \
 		RL("FLLow:", fl.low);
 		RL("FLStarving:", fl.starving);
 
-	} else if (iscsi_idx < iscsi_entries) {
-		const struct sge_ofld_rxq *rx =
-			&adap->sge.iscsirxq[iscsi_idx * 4];
+	} else if (ofld_idx < ofld_entries) {
 		const struct sge_ofld_txq *tx =
-			&adap->sge.ofldtxq[iscsi_idx * 4];
-		int n = min(4, adap->sge.iscsiqsets - 4 * iscsi_idx);
+			&adap->sge.ofldtxq[ofld_idx * 4];
+		int n = min(4, adap->sge.ofldqsets - 4 * ofld_idx);
 
-		S("QType:", "iSCSI");
+		S("QType:", "OFLD-Txq");
 		T("TxQ ID:", q.cntxt_id);
 		T("TxQ size:", q.size);
 		T("TxQ inuse:", q.in_use);
 		T("TxQ CIDX:", q.cidx);
 		T("TxQ PIDX:", q.pidx);
-		R("RspQ ID:", rspq.abs_id);
-		R("RspQ size:", rspq.size);
-		R("RspQE size:", rspq.iqe_len);
-		R("RspQ CIDX:", rspq.cidx);
-		R("RspQ Gen:", rspq.gen);
-		S3("u", "Intr delay:", qtimer_val(adap, &rx[i].rspq));
-		S3("u", "Intr pktcnt:",
-		   adap->sge.counter_val[rx[i].rspq.pktcnt_idx]);
-		R("FL ID:", fl.cntxt_id);
-		R("FL size:", fl.size - 8);
-		R("FL pend:", fl.pend_cred);
-		R("FL avail:", fl.avail);
-		R("FL PIDX:", fl.pidx);
-		R("FL CIDX:", fl.cidx);
-		RL("RxPackets:", stats.pkts);
-		RL("RxImmPkts:", stats.imm);
-		RL("RxNoMem:", stats.nomem);
-		RL("FLAllocErr:", fl.alloc_failed);
-		RL("FLLrgAlcErr:", fl.large_alloc_failed);
-		RL("FLMapErr:", fl.mapping_err);
-		RL("FLLow:", fl.low);
-		RL("FLStarving:", fl.starving);
-
-	} else if (iscsit_idx < iscsit_entries) {
-		const struct sge_ofld_rxq *rx =
-			&adap->sge.iscsitrxq[iscsit_idx * 4];
-		int n = min(4, adap->sge.niscsitq - 4 * iscsit_idx);
-
-		S("QType:", "iSCSIT");
-		R("RspQ ID:", rspq.abs_id);
-		R("RspQ size:", rspq.size);
-		R("RspQE size:", rspq.iqe_len);
-		R("RspQ CIDX:", rspq.cidx);
-		R("RspQ Gen:", rspq.gen);
-		S3("u", "Intr delay:", qtimer_val(adap, &rx[i].rspq));
-		S3("u", "Intr pktcnt:",
-		   adap->sge.counter_val[rx[i].rspq.pktcnt_idx]);
-		R("FL ID:", fl.cntxt_id);
-		R("FL size:", fl.size - 8);
-		R("FL pend:", fl.pend_cred);
-		R("FL avail:", fl.avail);
-		R("FL PIDX:", fl.pidx);
-		R("FL CIDX:", fl.cidx);
-		RL("RxPackets:", stats.pkts);
-		RL("RxImmPkts:", stats.imm);
-		RL("RxNoMem:", stats.nomem);
-		RL("FLAllocErr:", fl.alloc_failed);
-		RL("FLLrgAlcErr:", fl.large_alloc_failed);
-		RL("FLMapErr:", fl.mapping_err);
-		RL("FLLow:", fl.low);
-		RL("FLStarving:", fl.starving);
-
-	} else if (rdma_idx < rdma_entries) {
-		const struct sge_ofld_rxq *rx =
-				&adap->sge.rdmarxq[rdma_idx * 4];
-		int n = min(4, adap->sge.rdmaqs - 4 * rdma_idx);
-
-		S("QType:", "RDMA-CPL");
-		S("Interface:",
-		  rx[i].rspq.netdev ? rx[i].rspq.netdev->name : "N/A");
-		R("RspQ ID:", rspq.abs_id);
-		R("RspQ size:", rspq.size);
-		R("RspQE size:", rspq.iqe_len);
-		R("RspQ CIDX:", rspq.cidx);
-		R("RspQ Gen:", rspq.gen);
-		S3("u", "Intr delay:", qtimer_val(adap, &rx[i].rspq));
-		S3("u", "Intr pktcnt:",
-		   adap->sge.counter_val[rx[i].rspq.pktcnt_idx]);
-		R("FL ID:", fl.cntxt_id);
-		R("FL size:", fl.size - 8);
-		R("FL pend:", fl.pend_cred);
-		R("FL avail:", fl.avail);
-		R("FL PIDX:", fl.pidx);
-		R("FL CIDX:", fl.cidx);
-		RL("RxPackets:", stats.pkts);
-		RL("RxImmPkts:", stats.imm);
-		RL("RxNoMem:", stats.nomem);
-		RL("FLAllocErr:", fl.alloc_failed);
-		RL("FLLrgAlcErr:", fl.large_alloc_failed);
-		RL("FLMapErr:", fl.mapping_err);
-		RL("FLLow:", fl.low);
-		RL("FLStarving:", fl.starving);
-
-	} else if (ciq_idx < ciq_entries) {
-		const struct sge_ofld_rxq *rx = &adap->sge.rdmaciq[ciq_idx * 4];
-		int n = min(4, adap->sge.rdmaciqs - 4 * ciq_idx);
-
-		S("QType:", "RDMA-CIQ");
-		S("Interface:",
-		  rx[i].rspq.netdev ? rx[i].rspq.netdev->name : "N/A");
-		R("RspQ ID:", rspq.abs_id);
-		R("RspQ size:", rspq.size);
-		R("RspQE size:", rspq.iqe_len);
-		R("RspQ CIDX:", rspq.cidx);
-		R("RspQ Gen:", rspq.gen);
-		S3("u", "Intr delay:", qtimer_val(adap, &rx[i].rspq));
-		S3("u", "Intr pktcnt:",
-		   adap->sge.counter_val[rx[i].rspq.pktcnt_idx]);
-		RL("RxAN:", stats.an);
-		RL("RxNoMem:", stats.nomem);
 
 	} else if (ctrl_idx < ctrl_entries) {
 		const struct sge_ctrl_txq *tx = &adap->sge.ctrlq[ctrl_idx * 4];
@@ -2672,10 +2564,7 @@ do { \
 static int sge_queue_entries(const struct adapter *adap)
 {
 	return DIV_ROUND_UP(adap->sge.ethqsets, 4) +
-	       DIV_ROUND_UP(adap->sge.iscsiqsets, 4) +
-	       DIV_ROUND_UP(adap->sge.niscsitq, 4) +
-	       DIV_ROUND_UP(adap->sge.rdmaqs, 4) +
-	       DIV_ROUND_UP(adap->sge.rdmaciqs, 4) +
+	       DIV_ROUND_UP(adap->sge.ofldqsets, 4) +
 	       DIV_ROUND_UP(MAX_CTRL_QUEUES, 4) + 1;
 }
 
@@ -2859,12 +2748,6 @@ 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)
 {
@@ -2908,7 +2791,7 @@ static ssize_t blocked_fl_write(struct file *filp, const char __user *ubuf,
 
 static const struct file_operations blocked_fl_fops = {
 	.owner   = THIS_MODULE,
-	.open    = blocked_fl_open,
+	.open    = simple_open,
 	.read    = blocked_fl_read,
 	.write   = blocked_fl_write,
 	.llseek  = generic_file_llseek,
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_filter.c b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_filter.c
new file mode 100644
index 000000000000..10736738ff30
--- /dev/null
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_filter.c
@@ -0,0 +1,721 @@
+/*
+ * This file is part of the Chelsio T4 Ethernet driver for Linux.
+ *
+ * Copyright (c) 2003-2016 Chelsio Communications, Inc. All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * 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 "cxgb4.h"
+#include "t4_regs.h"
+#include "l2t.h"
+#include "t4fw_api.h"
+#include "cxgb4_filter.h"
+
+static inline bool is_field_set(u32 val, u32 mask)
+{
+	return val || mask;
+}
+
+static inline bool unsupported(u32 conf, u32 conf_mask, u32 val, u32 mask)
+{
+	return !(conf & conf_mask) && is_field_set(val, mask);
+}
+
+/* Validate filter spec against configuration done on the card. */
+static int validate_filter(struct net_device *dev,
+			   struct ch_filter_specification *fs)
+{
+	struct adapter *adapter = netdev2adap(dev);
+	u32 fconf, iconf;
+
+	/* Check for unconfigured fields being used. */
+	fconf = adapter->params.tp.vlan_pri_map;
+	iconf = adapter->params.tp.ingress_config;
+
+	if (unsupported(fconf, FCOE_F, fs->val.fcoe, fs->mask.fcoe) ||
+	    unsupported(fconf, PORT_F, fs->val.iport, fs->mask.iport) ||
+	    unsupported(fconf, TOS_F, fs->val.tos, fs->mask.tos) ||
+	    unsupported(fconf, ETHERTYPE_F, fs->val.ethtype,
+			fs->mask.ethtype) ||
+	    unsupported(fconf, MACMATCH_F, fs->val.macidx, fs->mask.macidx) ||
+	    unsupported(fconf, MPSHITTYPE_F, fs->val.matchtype,
+			fs->mask.matchtype) ||
+	    unsupported(fconf, FRAGMENTATION_F, fs->val.frag, fs->mask.frag) ||
+	    unsupported(fconf, PROTOCOL_F, fs->val.proto, fs->mask.proto) ||
+	    unsupported(fconf, VNIC_ID_F, fs->val.pfvf_vld,
+			fs->mask.pfvf_vld) ||
+	    unsupported(fconf, VNIC_ID_F, fs->val.ovlan_vld,
+			fs->mask.ovlan_vld) ||
+	    unsupported(fconf, VLAN_F, fs->val.ivlan_vld, fs->mask.ivlan_vld))
+		return -EOPNOTSUPP;
+
+	/* T4 inconveniently uses the same FT_VNIC_ID_W bits for both the Outer
+	 * VLAN Tag and PF/VF/VFvld fields based on VNIC_F being set
+	 * in TP_INGRESS_CONFIG.  Hense the somewhat crazy checks
+	 * below.  Additionally, since the T4 firmware interface also
+	 * carries that overlap, we need to translate any PF/VF
+	 * specification into that internal format below.
+	 */
+	if (is_field_set(fs->val.pfvf_vld, fs->mask.pfvf_vld) &&
+	    is_field_set(fs->val.ovlan_vld, fs->mask.ovlan_vld))
+		return -EOPNOTSUPP;
+	if (unsupported(iconf, VNIC_F, fs->val.pfvf_vld, fs->mask.pfvf_vld) ||
+	    (is_field_set(fs->val.ovlan_vld, fs->mask.ovlan_vld) &&
+	     (iconf & VNIC_F)))
+		return -EOPNOTSUPP;
+	if (fs->val.pf > 0x7 || fs->val.vf > 0x7f)
+		return -ERANGE;
+	fs->mask.pf &= 0x7;
+	fs->mask.vf &= 0x7f;
+
+	/* If the user is requesting that the filter action loop
+	 * matching packets back out one of our ports, make sure that
+	 * the egress port is in range.
+	 */
+	if (fs->action == FILTER_SWITCH &&
+	    fs->eport >= adapter->params.nports)
+		return -ERANGE;
+
+	/* Don't allow various trivially obvious bogus out-of-range values... */
+	if (fs->val.iport >= adapter->params.nports)
+		return -ERANGE;
+
+	/* T4 doesn't support removing VLAN Tags for loop back filters. */
+	if (is_t4(adapter->params.chip) &&
+	    fs->action == FILTER_SWITCH &&
+	    (fs->newvlan == VLAN_REMOVE ||
+	     fs->newvlan == VLAN_REWRITE))
+		return -EOPNOTSUPP;
+
+	return 0;
+}
+
+static int get_filter_steerq(struct net_device *dev,
+			     struct ch_filter_specification *fs)
+{
+	struct adapter *adapter = netdev2adap(dev);
+	int iq;
+
+	/* If the user has requested steering matching Ingress Packets
+	 * to a specific Queue Set, we need to make sure it's in range
+	 * for the port and map that into the Absolute Queue ID of the
+	 * Queue Set's Response Queue.
+	 */
+	if (!fs->dirsteer) {
+		if (fs->iq)
+			return -EINVAL;
+		iq = 0;
+	} else {
+		struct port_info *pi = netdev_priv(dev);
+
+		/* If the iq id is greater than the number of qsets,
+		 * then assume it is an absolute qid.
+		 */
+		if (fs->iq < pi->nqsets)
+			iq = adapter->sge.ethrxq[pi->first_qset +
+						 fs->iq].rspq.abs_id;
+		else
+			iq = fs->iq;
+	}
+
+	return iq;
+}
+
+static int cxgb4_set_ftid(struct tid_info *t, int fidx, int family)
+{
+	spin_lock_bh(&t->ftid_lock);
+
+	if (test_bit(fidx, t->ftid_bmap)) {
+		spin_unlock_bh(&t->ftid_lock);
+		return -EBUSY;
+	}
+
+	if (family == PF_INET)
+		__set_bit(fidx, t->ftid_bmap);
+	else
+		bitmap_allocate_region(t->ftid_bmap, fidx, 2);
+
+	spin_unlock_bh(&t->ftid_lock);
+	return 0;
+}
+
+static void cxgb4_clear_ftid(struct tid_info *t, int fidx, int family)
+{
+	spin_lock_bh(&t->ftid_lock);
+	if (family == PF_INET)
+		__clear_bit(fidx, t->ftid_bmap);
+	else
+		bitmap_release_region(t->ftid_bmap, fidx, 2);
+	spin_unlock_bh(&t->ftid_lock);
+}
+
+/* Delete the filter at a specified index. */
+static int del_filter_wr(struct adapter *adapter, int fidx)
+{
+	struct filter_entry *f = &adapter->tids.ftid_tab[fidx];
+	struct fw_filter_wr *fwr;
+	struct sk_buff *skb;
+	unsigned int len;
+
+	len = sizeof(*fwr);
+
+	skb = alloc_skb(len, GFP_KERNEL);
+	if (!skb)
+		return -ENOMEM;
+
+	fwr = (struct fw_filter_wr *)__skb_put(skb, len);
+	t4_mk_filtdelwr(f->tid, fwr, adapter->sge.fw_evtq.abs_id);
+
+	/* Mark the filter as "pending" and ship off the Filter Work Request.
+	 * When we get the Work Request Reply we'll clear the pending status.
+	 */
+	f->pending = 1;
+	t4_mgmt_tx(adapter, skb);
+	return 0;
+}
+
+/* Send a Work Request to write the filter at a specified index.  We construct
+ * a Firmware Filter Work Request to have the work done and put the indicated
+ * filter into "pending" mode which will prevent any further actions against
+ * it till we get a reply from the firmware on the completion status of the
+ * request.
+ */
+int set_filter_wr(struct adapter *adapter, int fidx)
+{
+	struct filter_entry *f = &adapter->tids.ftid_tab[fidx];
+	struct fw_filter_wr *fwr;
+	struct sk_buff *skb;
+
+	skb = alloc_skb(sizeof(*fwr), GFP_KERNEL);
+	if (!skb)
+		return -ENOMEM;
+
+	/* If the new filter requires loopback Destination MAC and/or VLAN
+	 * rewriting then we need to allocate a Layer 2 Table (L2T) entry for
+	 * the filter.
+	 */
+	if (f->fs.newdmac || f->fs.newvlan) {
+		/* allocate L2T entry for new filter */
+		f->l2t = t4_l2t_alloc_switching(adapter, f->fs.vlan,
+						f->fs.eport, f->fs.dmac);
+		if (!f->l2t) {
+			kfree_skb(skb);
+			return -ENOMEM;
+		}
+	}
+
+	fwr = (struct fw_filter_wr *)__skb_put(skb, sizeof(*fwr));
+	memset(fwr, 0, sizeof(*fwr));
+
+	/* It would be nice to put most of the following in t4_hw.c but most
+	 * of the work is translating the cxgbtool ch_filter_specification
+	 * into the Work Request and the definition of that structure is
+	 * currently in cxgbtool.h which isn't appropriate to pull into the
+	 * common code.  We may eventually try to come up with a more neutral
+	 * filter specification structure but for now it's easiest to simply
+	 * put this fairly direct code in line ...
+	 */
+	fwr->op_pkd = htonl(FW_WR_OP_V(FW_FILTER_WR));
+	fwr->len16_pkd = htonl(FW_WR_LEN16_V(sizeof(*fwr) / 16));
+	fwr->tid_to_iq =
+		htonl(FW_FILTER_WR_TID_V(f->tid) |
+		      FW_FILTER_WR_RQTYPE_V(f->fs.type) |
+		      FW_FILTER_WR_NOREPLY_V(0) |
+		      FW_FILTER_WR_IQ_V(f->fs.iq));
+	fwr->del_filter_to_l2tix =
+		htonl(FW_FILTER_WR_RPTTID_V(f->fs.rpttid) |
+		      FW_FILTER_WR_DROP_V(f->fs.action == FILTER_DROP) |
+		      FW_FILTER_WR_DIRSTEER_V(f->fs.dirsteer) |
+		      FW_FILTER_WR_MASKHASH_V(f->fs.maskhash) |
+		      FW_FILTER_WR_DIRSTEERHASH_V(f->fs.dirsteerhash) |
+		      FW_FILTER_WR_LPBK_V(f->fs.action == FILTER_SWITCH) |
+		      FW_FILTER_WR_DMAC_V(f->fs.newdmac) |
+		      FW_FILTER_WR_SMAC_V(f->fs.newsmac) |
+		      FW_FILTER_WR_INSVLAN_V(f->fs.newvlan == VLAN_INSERT ||
+					     f->fs.newvlan == VLAN_REWRITE) |
+		      FW_FILTER_WR_RMVLAN_V(f->fs.newvlan == VLAN_REMOVE ||
+					    f->fs.newvlan == VLAN_REWRITE) |
+		      FW_FILTER_WR_HITCNTS_V(f->fs.hitcnts) |
+		      FW_FILTER_WR_TXCHAN_V(f->fs.eport) |
+		      FW_FILTER_WR_PRIO_V(f->fs.prio) |
+		      FW_FILTER_WR_L2TIX_V(f->l2t ? f->l2t->idx : 0));
+	fwr->ethtype = htons(f->fs.val.ethtype);
+	fwr->ethtypem = htons(f->fs.mask.ethtype);
+	fwr->frag_to_ovlan_vldm =
+		(FW_FILTER_WR_FRAG_V(f->fs.val.frag) |
+		 FW_FILTER_WR_FRAGM_V(f->fs.mask.frag) |
+		 FW_FILTER_WR_IVLAN_VLD_V(f->fs.val.ivlan_vld) |
+		 FW_FILTER_WR_OVLAN_VLD_V(f->fs.val.ovlan_vld) |
+		 FW_FILTER_WR_IVLAN_VLDM_V(f->fs.mask.ivlan_vld) |
+		 FW_FILTER_WR_OVLAN_VLDM_V(f->fs.mask.ovlan_vld));
+	fwr->smac_sel = 0;
+	fwr->rx_chan_rx_rpl_iq =
+		htons(FW_FILTER_WR_RX_CHAN_V(0) |
+		      FW_FILTER_WR_RX_RPL_IQ_V(adapter->sge.fw_evtq.abs_id));
+	fwr->maci_to_matchtypem =
+		htonl(FW_FILTER_WR_MACI_V(f->fs.val.macidx) |
+		      FW_FILTER_WR_MACIM_V(f->fs.mask.macidx) |
+		      FW_FILTER_WR_FCOE_V(f->fs.val.fcoe) |
+		      FW_FILTER_WR_FCOEM_V(f->fs.mask.fcoe) |
+		      FW_FILTER_WR_PORT_V(f->fs.val.iport) |
+		      FW_FILTER_WR_PORTM_V(f->fs.mask.iport) |
+		      FW_FILTER_WR_MATCHTYPE_V(f->fs.val.matchtype) |
+		      FW_FILTER_WR_MATCHTYPEM_V(f->fs.mask.matchtype));
+	fwr->ptcl = f->fs.val.proto;
+	fwr->ptclm = f->fs.mask.proto;
+	fwr->ttyp = f->fs.val.tos;
+	fwr->ttypm = f->fs.mask.tos;
+	fwr->ivlan = htons(f->fs.val.ivlan);
+	fwr->ivlanm = htons(f->fs.mask.ivlan);
+	fwr->ovlan = htons(f->fs.val.ovlan);
+	fwr->ovlanm = htons(f->fs.mask.ovlan);
+	memcpy(fwr->lip, f->fs.val.lip, sizeof(fwr->lip));
+	memcpy(fwr->lipm, f->fs.mask.lip, sizeof(fwr->lipm));
+	memcpy(fwr->fip, f->fs.val.fip, sizeof(fwr->fip));
+	memcpy(fwr->fipm, f->fs.mask.fip, sizeof(fwr->fipm));
+	fwr->lp = htons(f->fs.val.lport);
+	fwr->lpm = htons(f->fs.mask.lport);
+	fwr->fp = htons(f->fs.val.fport);
+	fwr->fpm = htons(f->fs.mask.fport);
+	if (f->fs.newsmac)
+		memcpy(fwr->sma, f->fs.smac, sizeof(fwr->sma));
+
+	/* Mark the filter as "pending" and ship off the Filter Work Request.
+	 * When we get the Work Request Reply we'll clear the pending status.
+	 */
+	f->pending = 1;
+	set_wr_txq(skb, CPL_PRIORITY_CONTROL, f->fs.val.iport & 0x3);
+	t4_ofld_send(adapter, skb);
+	return 0;
+}
+
+/* Return an error number if the indicated filter isn't writable ... */
+int writable_filter(struct filter_entry *f)
+{
+	if (f->locked)
+		return -EPERM;
+	if (f->pending)
+		return -EBUSY;
+
+	return 0;
+}
+
+/* Delete the filter at the specified index (if valid).  The checks for all
+ * the common problems with doing this like the filter being locked, currently
+ * pending in another operation, etc.
+ */
+int delete_filter(struct adapter *adapter, unsigned int fidx)
+{
+	struct filter_entry *f;
+	int ret;
+
+	if (fidx >= adapter->tids.nftids + adapter->tids.nsftids)
+		return -EINVAL;
+
+	f = &adapter->tids.ftid_tab[fidx];
+	ret = writable_filter(f);
+	if (ret)
+		return ret;
+	if (f->valid)
+		return del_filter_wr(adapter, fidx);
+
+	return 0;
+}
+
+/* Clear a filter and release any of its resources that we own.  This also
+ * clears the filter's "pending" status.
+ */
+void clear_filter(struct adapter *adap, struct filter_entry *f)
+{
+	/* If the new or old filter have loopback rewriteing rules then we'll
+	 * need to free any existing Layer Two Table (L2T) entries of the old
+	 * filter rule.  The firmware will handle freeing up any Source MAC
+	 * Table (SMT) entries used for rewriting Source MAC Addresses in
+	 * loopback rules.
+	 */
+	if (f->l2t)
+		cxgb4_l2t_release(f->l2t);
+
+	/* The zeroing of the filter rule below clears the filter valid,
+	 * pending, locked flags, l2t pointer, etc. so it's all we need for
+	 * this operation.
+	 */
+	memset(f, 0, sizeof(*f));
+}
+
+void clear_all_filters(struct adapter *adapter)
+{
+	unsigned int i;
+
+	if (adapter->tids.ftid_tab) {
+		struct filter_entry *f = &adapter->tids.ftid_tab[0];
+		unsigned int max_ftid = adapter->tids.nftids +
+					adapter->tids.nsftids;
+
+		for (i = 0; i < max_ftid; i++, f++)
+			if (f->valid || f->pending)
+				clear_filter(adapter, f);
+	}
+}
+
+/* Fill up default masks for set match fields. */
+static void fill_default_mask(struct ch_filter_specification *fs)
+{
+	unsigned int lip = 0, lip_mask = 0;
+	unsigned int fip = 0, fip_mask = 0;
+	unsigned int i;
+
+	if (fs->val.iport && !fs->mask.iport)
+		fs->mask.iport |= ~0;
+	if (fs->val.fcoe && !fs->mask.fcoe)
+		fs->mask.fcoe |= ~0;
+	if (fs->val.matchtype && !fs->mask.matchtype)
+		fs->mask.matchtype |= ~0;
+	if (fs->val.macidx && !fs->mask.macidx)
+		fs->mask.macidx |= ~0;
+	if (fs->val.ethtype && !fs->mask.ethtype)
+		fs->mask.ethtype |= ~0;
+	if (fs->val.ivlan && !fs->mask.ivlan)
+		fs->mask.ivlan |= ~0;
+	if (fs->val.ovlan && !fs->mask.ovlan)
+		fs->mask.ovlan |= ~0;
+	if (fs->val.frag && !fs->mask.frag)
+		fs->mask.frag |= ~0;
+	if (fs->val.tos && !fs->mask.tos)
+		fs->mask.tos |= ~0;
+	if (fs->val.proto && !fs->mask.proto)
+		fs->mask.proto |= ~0;
+
+	for (i = 0; i < ARRAY_SIZE(fs->val.lip); i++) {
+		lip |= fs->val.lip[i];
+		lip_mask |= fs->mask.lip[i];
+		fip |= fs->val.fip[i];
+		fip_mask |= fs->mask.fip[i];
+	}
+
+	if (lip && !lip_mask)
+		memset(fs->mask.lip, ~0, sizeof(fs->mask.lip));
+
+	if (fip && !fip_mask)
+		memset(fs->mask.fip, ~0, sizeof(fs->mask.lip));
+
+	if (fs->val.lport && !fs->mask.lport)
+		fs->mask.lport = ~0;
+	if (fs->val.fport && !fs->mask.fport)
+		fs->mask.fport = ~0;
+}
+
+/* Check a Chelsio Filter Request for validity, convert it into our internal
+ * format and send it to the hardware.  Return 0 on success, an error number
+ * otherwise.  We attach any provided filter operation context to the internal
+ * filter specification in order to facilitate signaling completion of the
+ * operation.
+ */
+int __cxgb4_set_filter(struct net_device *dev, int filter_id,
+		       struct ch_filter_specification *fs,
+		       struct filter_ctx *ctx)
+{
+	struct adapter *adapter = netdev2adap(dev);
+	unsigned int max_fidx, fidx;
+	struct filter_entry *f;
+	u32 iconf;
+	int iq, ret;
+
+	max_fidx = adapter->tids.nftids;
+	if (filter_id != (max_fidx + adapter->tids.nsftids - 1) &&
+	    filter_id >= max_fidx)
+		return -E2BIG;
+
+	fill_default_mask(fs);
+
+	ret = validate_filter(dev, fs);
+	if (ret)
+		return ret;
+
+	iq = get_filter_steerq(dev, fs);
+	if (iq < 0)
+		return iq;
+
+	/* IPv6 filters occupy four slots and must be aligned on
+	 * four-slot boundaries.  IPv4 filters only occupy a single
+	 * slot and have no alignment requirements but writing a new
+	 * IPv4 filter into the middle of an existing IPv6 filter
+	 * requires clearing the old IPv6 filter and hence we prevent
+	 * insertion.
+	 */
+	if (fs->type == 0) { /* IPv4 */
+		/* If our IPv4 filter isn't being written to a
+		 * multiple of four filter index and there's an IPv6
+		 * filter at the multiple of 4 base slot, then we
+		 * prevent insertion.
+		 */
+		fidx = filter_id & ~0x3;
+		if (fidx != filter_id &&
+		    adapter->tids.ftid_tab[fidx].fs.type) {
+			f = &adapter->tids.ftid_tab[fidx];
+			if (f->valid) {
+				dev_err(adapter->pdev_dev,
+					"Invalid location. IPv6 requires 4 slots and is occupying slots %u to %u\n",
+					fidx, fidx + 3);
+				return -EINVAL;
+			}
+		}
+	} else { /* IPv6 */
+		/* Ensure that the IPv6 filter is aligned on a
+		 * multiple of 4 boundary.
+		 */
+		if (filter_id & 0x3) {
+			dev_err(adapter->pdev_dev,
+				"Invalid location. IPv6 must be aligned on a 4-slot boundary\n");
+			return -EINVAL;
+		}
+
+		/* Check all except the base overlapping IPv4 filter slots. */
+		for (fidx = filter_id + 1; fidx < filter_id + 4; fidx++) {
+			f = &adapter->tids.ftid_tab[fidx];
+			if (f->valid) {
+				dev_err(adapter->pdev_dev,
+					"Invalid location.  IPv6 requires 4 slots and an IPv4 filter exists at %u\n",
+					fidx);
+				return -EINVAL;
+			}
+		}
+	}
+
+	/* Check to make sure that provided filter index is not
+	 * already in use by someone else
+	 */
+	f = &adapter->tids.ftid_tab[filter_id];
+	if (f->valid)
+		return -EBUSY;
+
+	fidx = filter_id + adapter->tids.ftid_base;
+	ret = cxgb4_set_ftid(&adapter->tids, filter_id,
+			     fs->type ? PF_INET6 : PF_INET);
+	if (ret)
+		return ret;
+
+	/* Check to make sure the filter requested is writable ... */
+	ret = writable_filter(f);
+	if (ret) {
+		/* Clear the bits we have set above */
+		cxgb4_clear_ftid(&adapter->tids, filter_id,
+				 fs->type ? PF_INET6 : PF_INET);
+		return ret;
+	}
+
+	/* Clear out any old resources being used by the filter before
+	 * we start constructing the new filter.
+	 */
+	if (f->valid)
+		clear_filter(adapter, f);
+
+	/* Convert the filter specification into our internal format.
+	 * We copy the PF/VF specification into the Outer VLAN field
+	 * here so the rest of the code -- including the interface to
+	 * the firmware -- doesn't have to constantly do these checks.
+	 */
+	f->fs = *fs;
+	f->fs.iq = iq;
+	f->dev = dev;
+
+	iconf = adapter->params.tp.ingress_config;
+	if (iconf & VNIC_F) {
+		f->fs.val.ovlan = (fs->val.pf << 13) | fs->val.vf;
+		f->fs.mask.ovlan = (fs->mask.pf << 13) | fs->mask.vf;
+		f->fs.val.ovlan_vld = fs->val.pfvf_vld;
+		f->fs.mask.ovlan_vld = fs->mask.pfvf_vld;
+	}
+
+	/* Attempt to set the filter.  If we don't succeed, we clear
+	 * it and return the failure.
+	 */
+	f->ctx = ctx;
+	f->tid = fidx; /* Save the actual tid */
+	ret = set_filter_wr(adapter, filter_id);
+	if (ret) {
+		cxgb4_clear_ftid(&adapter->tids, filter_id,
+				 fs->type ? PF_INET6 : PF_INET);
+		clear_filter(adapter, f);
+	}
+
+	return ret;
+}
+
+/* Check a delete filter request for validity and send it to the hardware.
+ * Return 0 on success, an error number otherwise.  We attach any provided
+ * filter operation context to the internal filter specification in order to
+ * facilitate signaling completion of the operation.
+ */
+int __cxgb4_del_filter(struct net_device *dev, int filter_id,
+		       struct filter_ctx *ctx)
+{
+	struct adapter *adapter = netdev2adap(dev);
+	struct filter_entry *f;
+	unsigned int max_fidx;
+	int ret;
+
+	max_fidx = adapter->tids.nftids;
+	if (filter_id != (max_fidx + adapter->tids.nsftids - 1) &&
+	    filter_id >= max_fidx)
+		return -E2BIG;
+
+	f = &adapter->tids.ftid_tab[filter_id];
+	ret = writable_filter(f);
+	if (ret)
+		return ret;
+
+	if (f->valid) {
+		f->ctx = ctx;
+		cxgb4_clear_ftid(&adapter->tids, filter_id,
+				 f->fs.type ? PF_INET6 : PF_INET);
+		return del_filter_wr(adapter, filter_id);
+	}
+
+	/* If the caller has passed in a Completion Context then we need to
+	 * mark it as a successful completion so they don't stall waiting
+	 * for it.
+	 */
+	if (ctx) {
+		ctx->result = 0;
+		complete(&ctx->completion);
+	}
+	return ret;
+}
+
+int cxgb4_set_filter(struct net_device *dev, int filter_id,
+		     struct ch_filter_specification *fs)
+{
+	struct filter_ctx ctx;
+	int ret;
+
+	init_completion(&ctx.completion);
+
+	ret = __cxgb4_set_filter(dev, filter_id, fs, &ctx);
+	if (ret)
+		goto out;
+
+	/* Wait for reply */
+	ret = wait_for_completion_timeout(&ctx.completion, 10 * HZ);
+	if (!ret)
+		return -ETIMEDOUT;
+
+	ret = ctx.result;
+out:
+	return ret;
+}
+
+int cxgb4_del_filter(struct net_device *dev, int filter_id)
+{
+	struct filter_ctx ctx;
+	int ret;
+
+	init_completion(&ctx.completion);
+
+	ret = __cxgb4_del_filter(dev, filter_id, &ctx);
+	if (ret)
+		goto out;
+
+	/* Wait for reply */
+	ret = wait_for_completion_timeout(&ctx.completion, 10 * HZ);
+	if (!ret)
+		return -ETIMEDOUT;
+
+	ret = ctx.result;
+out:
+	return ret;
+}
+
+/* Handle a filter write/deletion reply. */
+void filter_rpl(struct adapter *adap, const struct cpl_set_tcb_rpl *rpl)
+{
+	unsigned int tid = GET_TID(rpl);
+	struct filter_entry *f = NULL;
+	unsigned int max_fidx;
+	int idx;
+
+	max_fidx = adap->tids.nftids + adap->tids.nsftids;
+	/* Get the corresponding filter entry for this tid */
+	if (adap->tids.ftid_tab) {
+		/* Check this in normal filter region */
+		idx = tid - adap->tids.ftid_base;
+		if (idx >= max_fidx)
+			return;
+		f = &adap->tids.ftid_tab[idx];
+		if (f->tid != tid)
+			return;
+	}
+
+	/* We found the filter entry for this tid */
+	if (f) {
+		unsigned int ret = TCB_COOKIE_G(rpl->cookie);
+		struct filter_ctx *ctx;
+
+		/* Pull off any filter operation context attached to the
+		 * filter.
+		 */
+		ctx = f->ctx;
+		f->ctx = NULL;
+
+		if (ret == FW_FILTER_WR_FLT_DELETED) {
+			/* Clear the filter when we get confirmation from the
+			 * hardware that the filter has been deleted.
+			 */
+			clear_filter(adap, f);
+			if (ctx)
+				ctx->result = 0;
+		} else if (ret == FW_FILTER_WR_SMT_TBL_FULL) {
+			dev_err(adap->pdev_dev, "filter %u setup failed due to full SMT\n",
+				idx);
+			clear_filter(adap, f);
+			if (ctx)
+				ctx->result = -ENOMEM;
+		} else if (ret == FW_FILTER_WR_FLT_ADDED) {
+			f->smtidx = (be64_to_cpu(rpl->oldval) >> 24) & 0xff;
+			f->pending = 0;  /* asynchronous setup completed */
+			f->valid = 1;
+			if (ctx) {
+				ctx->result = 0;
+				ctx->tid = idx;
+			}
+		} else {
+			/* Something went wrong.  Issue a warning about the
+			 * problem and clear everything out.
+			 */
+			dev_err(adap->pdev_dev, "filter %u setup failed with error %u\n",
+				idx, ret);
+			clear_filter(adap, f);
+			if (ctx)
+				ctx->result = -EINVAL;
+		}
+		if (ctx)
+			complete(&ctx->completion);
+	}
+}
diff --git a/drivers/infiniband/hw/cxgb4/user.h b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_filter.h
index 295f422b9a3a..23742cb1c69f 100644
--- a/drivers/infiniband/hw/cxgb4/user.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_filter.h
@@ -1,5 +1,7 @@
 /*
- * Copyright (c) 2009-2010 Chelsio, Inc. All rights reserved.
+ * This file is part of the Chelsio T4 Ethernet driver for Linux.
+ *
+ * Copyright (c) 2003-2016 Chelsio Communications, Inc. All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
@@ -29,52 +31,18 @@
  * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
  * SOFTWARE.
  */
-#ifndef __C4IW_USER_H__
-#define __C4IW_USER_H__
-
-#define C4IW_UVERBS_ABI_VERSION	3
 
-/*
- * Make sure that all structs defined in this file remain laid out so
- * that they pack the same way on 32-bit and 64-bit architectures (to
- * avoid incompatibility between 32-bit userspace and 64-bit kernels).
- * In particular do not use pointer types -- pass pointers in __u64
- * instead.
- */
-struct c4iw_create_cq_resp {
-	__u64 key;
-	__u64 gts_key;
-	__u64 memsize;
-	__u32 cqid;
-	__u32 size;
-	__u32 qid_mask;
-	__u32 reserved; /* explicit padding (optional for i386) */
-};
+#ifndef __CXGB4_FILTER_H
+#define __CXGB4_FILTER_H
 
+#include "t4_msg.h"
 
-enum {
-	C4IW_QPF_ONCHIP = (1<<0)
-};
+void filter_rpl(struct adapter *adap, const struct cpl_set_tcb_rpl *rpl);
+void clear_filter(struct adapter *adap, struct filter_entry *f);
 
-struct c4iw_create_qp_resp {
-	__u64 ma_sync_key;
-	__u64 sq_key;
-	__u64 rq_key;
-	__u64 sq_db_gts_key;
-	__u64 rq_db_gts_key;
-	__u64 sq_memsize;
-	__u64 rq_memsize;
-	__u32 sqid;
-	__u32 rqid;
-	__u32 sq_size;
-	__u32 rq_size;
-	__u32 qid_mask;
-	__u32 flags;
-};
+int set_filter_wr(struct adapter *adapter, int fidx);
+int delete_filter(struct adapter *adapter, unsigned int fidx);
 
-struct c4iw_alloc_ucontext_resp {
-	__u64 status_page_key;
-	__u32 status_page_size;
-	__u32 reserved; /* explicit padding (optional for i386) */
-};
-#endif
+int writable_filter(struct filter_entry *f);
+void clear_all_filters(struct adapter *adapter);
+#endif /* __CXGB4_FILTER_H */
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c
index 3ceafb55d6da..f320497368f4 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c
@@ -1,7 +1,7 @@
 /*
  * This file is part of the Chelsio T4 Ethernet driver for Linux.
  *
- * Copyright (c) 2003-2014 Chelsio Communications, Inc. All rights reserved.
+ * Copyright (c) 2003-2016 Chelsio Communications, Inc. All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
@@ -67,6 +67,7 @@
 #include <linux/crash_dump.h>
 
 #include "cxgb4.h"
+#include "cxgb4_filter.h"
 #include "t4_regs.h"
 #include "t4_values.h"
 #include "t4_msg.h"
@@ -76,6 +77,8 @@
 #include "cxgb4_debugfs.h"
 #include "clip_tbl.h"
 #include "l2t.h"
+#include "sched.h"
+#include "cxgb4_tc_u32.h"
 
 char cxgb4_driver_name[] = KBUILD_MODNAME;
 
@@ -86,30 +89,6 @@ char cxgb4_driver_name[] = KBUILD_MODNAME;
 const char cxgb4_driver_version[] = DRV_VERSION;
 #define DRV_DESC "Chelsio T4/T5/T6 Network Driver"
 
-/* Host shadow copy of ingress filter entry.  This is in host native format
- * and doesn't match the ordering or bit order, etc. of the hardware of the
- * firmware command.  The use of bit-field structure elements is purely to
- * remind ourselves of the field size limitations and save memory in the case
- * where the filter table is large.
- */
-struct filter_entry {
-	/* Administrative fields for filter.
-	 */
-	u32 valid:1;            /* filter allocated and valid */
-	u32 locked:1;           /* filter is administratively locked */
-
-	u32 pending:1;          /* filter action is pending firmware reply */
-	u32 smtidx:8;           /* Source MAC Table index for smac */
-	struct l2t_entry *l2t;  /* Layer Two Table entry for dmac */
-
-	/* The filter itself.  Most of this is a straight copy of information
-	 * provided by the extended ioctl().  Some fields are translated to
-	 * internal forms -- for instance the Ingress Queue ID passed in from
-	 * the ioctl() is translated into the Absolute Ingress Queue ID.
-	 */
-	struct ch_filter_specification fs;
-};
-
 #define DFLT_MSG_ENABLE (NETIF_MSG_DRV | NETIF_MSG_PROBE | NETIF_MSG_LINK | \
 			 NETIF_MSG_TIMER | NETIF_MSG_IFDOWN | NETIF_MSG_IFUP |\
 			 NETIF_MSG_RX_ERR | NETIF_MSG_TX_ERR)
@@ -223,13 +202,8 @@ MODULE_PARM_DESC(select_queue,
 
 static struct dentry *cxgb4_debugfs_root;
 
-static LIST_HEAD(adapter_list);
-static DEFINE_MUTEX(uld_mutex);
-/* Adapter list to be accessed from atomic context */
-static LIST_HEAD(adap_rcu_list);
-static DEFINE_SPINLOCK(adap_rcu_lock);
-static struct cxgb4_uld_info ulds[CXGB4_ULD_MAX];
-static const char *const uld_str[] = { "RDMA", "iSCSI", "iSCSIT" };
+LIST_HEAD(adapter_list);
+DEFINE_MUTEX(uld_mutex);
 
 static void link_report(struct net_device *dev)
 {
@@ -303,11 +277,9 @@ static void dcb_tx_queue_prio_enable(struct net_device *dev, int enable)
 			txq->dcb_prio = value;
 	}
 }
-#endif /* CONFIG_CHELSIO_T4_DCB */
 
-int cxgb4_dcb_enabled(const struct net_device *dev)
+static int cxgb4_dcb_enabled(const struct net_device *dev)
 {
-#ifdef CONFIG_CHELSIO_T4_DCB
 	struct port_info *pi = netdev_priv(dev);
 
 	if (!pi->dcb.enabled)
@@ -315,11 +287,8 @@ int cxgb4_dcb_enabled(const struct net_device *dev)
 
 	return ((pi->dcb.state == CXGB4_DCB_STATE_FW_ALLSYNCED) ||
 		(pi->dcb.state == CXGB4_DCB_STATE_HOST));
-#else
-	return 0;
-#endif
 }
-EXPORT_SYMBOL(cxgb4_dcb_enabled);
+#endif /* CONFIG_CHELSIO_T4_DCB */
 
 void t4_os_link_changed(struct adapter *adapter, int port_id, int link_stat)
 {
@@ -531,66 +500,6 @@ static void dcb_rpl(struct adapter *adap, const struct fw_port_cmd *pcmd)
 }
 #endif /* CONFIG_CHELSIO_T4_DCB */
 
-/* Clear a filter and release any of its resources that we own.  This also
- * clears the filter's "pending" status.
- */
-static void clear_filter(struct adapter *adap, struct filter_entry *f)
-{
-	/* If the new or old filter have loopback rewriteing rules then we'll
-	 * need to free any existing Layer Two Table (L2T) entries of the old
-	 * filter rule.  The firmware will handle freeing up any Source MAC
-	 * Table (SMT) entries used for rewriting Source MAC Addresses in
-	 * loopback rules.
-	 */
-	if (f->l2t)
-		cxgb4_l2t_release(f->l2t);
-
-	/* The zeroing of the filter rule below clears the filter valid,
-	 * pending, locked flags, l2t pointer, etc. so it's all we need for
-	 * this operation.
-	 */
-	memset(f, 0, sizeof(*f));
-}
-
-/* Handle a filter write/deletion reply.
- */
-static void filter_rpl(struct adapter *adap, const struct cpl_set_tcb_rpl *rpl)
-{
-	unsigned int idx = GET_TID(rpl);
-	unsigned int nidx = idx - adap->tids.ftid_base;
-	unsigned int ret;
-	struct filter_entry *f;
-
-	if (idx >= adap->tids.ftid_base && nidx <
-	   (adap->tids.nftids + adap->tids.nsftids)) {
-		idx = nidx;
-		ret = TCB_COOKIE_G(rpl->cookie);
-		f = &adap->tids.ftid_tab[idx];
-
-		if (ret == FW_FILTER_WR_FLT_DELETED) {
-			/* Clear the filter when we get confirmation from the
-			 * hardware that the filter has been deleted.
-			 */
-			clear_filter(adap, f);
-		} else if (ret == FW_FILTER_WR_SMT_TBL_FULL) {
-			dev_err(adap->pdev_dev, "filter %u setup failed due to full SMT\n",
-				idx);
-			clear_filter(adap, f);
-		} else if (ret == FW_FILTER_WR_FLT_ADDED) {
-			f->smtidx = (be64_to_cpu(rpl->oldval) >> 24) & 0xff;
-			f->pending = 0;  /* asynchronous setup completed */
-			f->valid = 1;
-		} else {
-			/* Something went wrong.  Issue a warning about the
-			 * problem and clear everything out.
-			 */
-			dev_err(adap->pdev_dev, "filter %u setup failed with error %u\n",
-				idx, ret);
-			clear_filter(adap, f);
-		}
-	}
-}
-
 /* Response queue handler for the FW event queue.
  */
 static int fwevtq_handler(struct sge_rspq *q, const __be64 *rsp,
@@ -677,56 +586,6 @@ out:
 	return 0;
 }
 
-/* Flush the aggregated lro sessions */
-static void uldrx_flush_handler(struct sge_rspq *q)
-{
-	if (ulds[q->uld].lro_flush)
-		ulds[q->uld].lro_flush(&q->lro_mgr);
-}
-
-/**
- *	uldrx_handler - response queue handler for ULD queues
- *	@q: the response queue that received the packet
- *	@rsp: the response queue descriptor holding the offload message
- *	@gl: the gather list of packet fragments
- *
- *	Deliver an ingress offload packet to a ULD.  All processing is done by
- *	the ULD, we just maintain statistics.
- */
-static int uldrx_handler(struct sge_rspq *q, const __be64 *rsp,
-			 const struct pkt_gl *gl)
-{
-	struct sge_ofld_rxq *rxq = container_of(q, struct sge_ofld_rxq, rspq);
-	int ret;
-
-	/* FW can send CPLs encapsulated in a CPL_FW4_MSG.
-	 */
-	if (((const struct rss_header *)rsp)->opcode == CPL_FW4_MSG &&
-	    ((const struct cpl_fw4_msg *)(rsp + 1))->type == FW_TYPE_RSSCPL)
-		rsp += 2;
-
-	if (q->flush_handler)
-		ret = ulds[q->uld].lro_rx_handler(q->adap->uld_handle[q->uld],
-						  rsp, gl, &q->lro_mgr,
-						  &q->napi);
-	else
-		ret = ulds[q->uld].rx_handler(q->adap->uld_handle[q->uld],
-					      rsp, gl);
-
-	if (ret) {
-		rxq->stats.nomem++;
-		return -1;
-	}
-
-	if (gl == NULL)
-		rxq->stats.imm++;
-	else if (gl == CXGB4_MSG_AN)
-		rxq->stats.an++;
-	else
-		rxq->stats.pkts++;
-	return 0;
-}
-
 static void disable_msi(struct adapter *adapter)
 {
 	if (adapter->flags & USING_MSIX) {
@@ -778,30 +637,12 @@ static void name_msix_vecs(struct adapter *adap)
 			snprintf(adap->msix_info[msi_idx].desc, n, "%s-Rx%d",
 				 d->name, i);
 	}
-
-	/* offload queues */
-	for_each_iscsirxq(&adap->sge, i)
-		snprintf(adap->msix_info[msi_idx++].desc, n, "%s-iscsi%d",
-			 adap->port[0]->name, i);
-
-	for_each_iscsitrxq(&adap->sge, i)
-		snprintf(adap->msix_info[msi_idx++].desc, n, "%s-iSCSIT%d",
-			 adap->port[0]->name, i);
-
-	for_each_rdmarxq(&adap->sge, i)
-		snprintf(adap->msix_info[msi_idx++].desc, n, "%s-rdma%d",
-			 adap->port[0]->name, i);
-
-	for_each_rdmaciq(&adap->sge, i)
-		snprintf(adap->msix_info[msi_idx++].desc, n, "%s-rdma-ciq%d",
-			 adap->port[0]->name, i);
 }
 
 static int request_msix_queue_irqs(struct adapter *adap)
 {
 	struct sge *s = &adap->sge;
-	int err, ethqidx, iscsiqidx = 0, rdmaqidx = 0, rdmaciqqidx = 0;
-	int iscsitqidx = 0;
+	int err, ethqidx;
 	int msi_index = 2;
 
 	err = request_irq(adap->msix_info[1].vec, t4_sge_intr_msix, 0,
@@ -818,57 +659,9 @@ static int request_msix_queue_irqs(struct adapter *adap)
 			goto unwind;
 		msi_index++;
 	}
-	for_each_iscsirxq(s, iscsiqidx) {
-		err = request_irq(adap->msix_info[msi_index].vec,
-				  t4_sge_intr_msix, 0,
-				  adap->msix_info[msi_index].desc,
-				  &s->iscsirxq[iscsiqidx].rspq);
-		if (err)
-			goto unwind;
-		msi_index++;
-	}
-	for_each_iscsitrxq(s, iscsitqidx) {
-		err = request_irq(adap->msix_info[msi_index].vec,
-				  t4_sge_intr_msix, 0,
-				  adap->msix_info[msi_index].desc,
-				  &s->iscsitrxq[iscsitqidx].rspq);
-		if (err)
-			goto unwind;
-		msi_index++;
-	}
-	for_each_rdmarxq(s, rdmaqidx) {
-		err = request_irq(adap->msix_info[msi_index].vec,
-				  t4_sge_intr_msix, 0,
-				  adap->msix_info[msi_index].desc,
-				  &s->rdmarxq[rdmaqidx].rspq);
-		if (err)
-			goto unwind;
-		msi_index++;
-	}
-	for_each_rdmaciq(s, rdmaciqqidx) {
-		err = request_irq(adap->msix_info[msi_index].vec,
-				  t4_sge_intr_msix, 0,
-				  adap->msix_info[msi_index].desc,
-				  &s->rdmaciq[rdmaciqqidx].rspq);
-		if (err)
-			goto unwind;
-		msi_index++;
-	}
 	return 0;
 
 unwind:
-	while (--rdmaciqqidx >= 0)
-		free_irq(adap->msix_info[--msi_index].vec,
-			 &s->rdmaciq[rdmaciqqidx].rspq);
-	while (--rdmaqidx >= 0)
-		free_irq(adap->msix_info[--msi_index].vec,
-			 &s->rdmarxq[rdmaqidx].rspq);
-	while (--iscsitqidx >= 0)
-		free_irq(adap->msix_info[--msi_index].vec,
-			 &s->iscsitrxq[iscsitqidx].rspq);
-	while (--iscsiqidx >= 0)
-		free_irq(adap->msix_info[--msi_index].vec,
-			 &s->iscsirxq[iscsiqidx].rspq);
 	while (--ethqidx >= 0)
 		free_irq(adap->msix_info[--msi_index].vec,
 			 &s->ethrxq[ethqidx].rspq);
@@ -884,16 +677,6 @@ static void free_msix_queue_irqs(struct adapter *adap)
 	free_irq(adap->msix_info[1].vec, &s->fw_evtq);
 	for_each_ethrxq(s, i)
 		free_irq(adap->msix_info[msi_index++].vec, &s->ethrxq[i].rspq);
-	for_each_iscsirxq(s, i)
-		free_irq(adap->msix_info[msi_index++].vec,
-			 &s->iscsirxq[i].rspq);
-	for_each_iscsitrxq(s, i)
-		free_irq(adap->msix_info[msi_index++].vec,
-			 &s->iscsitrxq[i].rspq);
-	for_each_rdmarxq(s, i)
-		free_irq(adap->msix_info[msi_index++].vec, &s->rdmarxq[i].rspq);
-	for_each_rdmaciq(s, i)
-		free_irq(adap->msix_info[msi_index++].vec, &s->rdmaciq[i].rspq);
 }
 
 /**
@@ -1032,28 +815,30 @@ static void enable_rx(struct adapter *adap)
 	}
 }
 
-static int alloc_ofld_rxqs(struct adapter *adap, struct sge_ofld_rxq *q,
-			   unsigned int nq, unsigned int per_chan, int msi_idx,
-			   u16 *ids, bool lro)
+
+static int setup_fw_sge_queues(struct adapter *adap)
 {
-	int i, err;
+	struct sge *s = &adap->sge;
+	int err = 0;
+
+	bitmap_zero(s->starving_fl, s->egr_sz);
+	bitmap_zero(s->txq_maperr, s->egr_sz);
 
-	for (i = 0; i < nq; i++, q++) {
-		if (msi_idx > 0)
-			msi_idx++;
-		err = t4_sge_alloc_rxq(adap, &q->rspq, false,
-				       adap->port[i / per_chan],
-				       msi_idx, q->fl.size ? &q->fl : NULL,
-				       uldrx_handler,
-				       lro ? uldrx_flush_handler : NULL,
-				       0);
+	if (adap->flags & USING_MSIX)
+		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, -1);
 		if (err)
 			return err;
-		memset(&q->stats, 0, sizeof(q->stats));
-		if (ids)
-			ids[i] = q->rspq.abs_id;
+		adap->msi_idx = -((int)s->intrq.abs_id + 1);
 	}
-	return 0;
+
+	err = t4_sge_alloc_rxq(adap, &s->fw_evtq, true, adap->port[0],
+			       adap->msi_idx, NULL, fwevtq_handler, NULL, -1);
+	if (err)
+		t4_free_sge_resources(adap);
+	return err;
 }
 
 /**
@@ -1066,41 +851,10 @@ static int alloc_ofld_rxqs(struct adapter *adap, struct sge_ofld_rxq *q,
  */
 static int setup_sge_queues(struct adapter *adap)
 {
-	int err, msi_idx, i, j;
+	int err, i, j;
 	struct sge *s = &adap->sge;
-
-	bitmap_zero(s->starving_fl, s->egr_sz);
-	bitmap_zero(s->txq_maperr, s->egr_sz);
-
-	if (adap->flags & USING_MSIX)
-		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, -1);
-		if (err)
-			return err;
-		msi_idx = -((int)s->intrq.abs_id + 1);
-	}
-
-	/* NOTE: If you add/delete any Ingress/Egress Queue allocations in here,
-	 * don't forget to update the following which need to be
-	 * synchronized to and changes here.
-	 *
-	 * 1. The calculations of MAX_INGQ in cxgb4.h.
-	 *
-	 * 2. Update enable_msix/name_msix_vecs/request_msix_queue_irqs
-	 *    to accommodate any new/deleted Ingress Queues
-	 *    which need MSI-X Vectors.
-	 *
-	 * 3. Update sge_qinfo_show() to include information on the
-	 *    new/deleted queues.
-	 */
-	err = t4_sge_alloc_rxq(adap, &s->fw_evtq, true, adap->port[0],
-			       msi_idx, NULL, fwevtq_handler, NULL, -1);
-	if (err) {
-freeout:	t4_free_sge_resources(adap);
-		return err;
-	}
+	struct sge_uld_rxq_info *rxq_info = s->uld_rxq_info[CXGB4_ULD_RDMA];
+	unsigned int cmplqid = 0;
 
 	for_each_port(adap, i) {
 		struct net_device *dev = adap->port[i];
@@ -1109,10 +863,10 @@ freeout:	t4_free_sge_resources(adap);
 		struct sge_eth_txq *t = &s->ethtxq[pi->first_qset];
 
 		for (j = 0; j < pi->nqsets; j++, q++) {
-			if (msi_idx > 0)
-				msi_idx++;
+			if (adap->msi_idx > 0)
+				adap->msi_idx++;
 			err = t4_sge_alloc_rxq(adap, &q->rspq, false, dev,
-					       msi_idx, &q->fl,
+					       adap->msi_idx, &q->fl,
 					       t4_ethrx_handler,
 					       NULL,
 					       t4_get_mps_bg_map(adap,
@@ -1131,8 +885,8 @@ freeout:	t4_free_sge_resources(adap);
 		}
 	}
 
-	j = s->iscsiqsets / adap->params.nports; /* iscsi queues per channel */
-	for_each_iscsirxq(s, i) {
+	j = s->ofldqsets / adap->params.nports; /* iscsi queues per channel */
+	for_each_ofldtxq(s, i) {
 		err = t4_sge_alloc_ofld_txq(adap, &s->ofldtxq[i],
 					    adap->port[i / j],
 					    s->fw_evtq.cntxt_id);
@@ -1140,30 +894,15 @@ freeout:	t4_free_sge_resources(adap);
 			goto freeout;
 	}
 
-#define ALLOC_OFLD_RXQS(firstq, nq, per_chan, ids, lro) do { \
-	err = alloc_ofld_rxqs(adap, firstq, nq, per_chan, msi_idx, ids, lro); \
-	if (err) \
-		goto freeout; \
-	if (msi_idx > 0) \
-		msi_idx += nq; \
-} while (0)
-
-	ALLOC_OFLD_RXQS(s->iscsirxq, s->iscsiqsets, j, s->iscsi_rxq, false);
-	ALLOC_OFLD_RXQS(s->iscsitrxq, s->niscsitq, j, s->iscsit_rxq, true);
-	ALLOC_OFLD_RXQS(s->rdmarxq, s->rdmaqs, 1, s->rdma_rxq, false);
-	j = s->rdmaciqs / adap->params.nports; /* rdmaq queues per channel */
-	ALLOC_OFLD_RXQS(s->rdmaciq, s->rdmaciqs, j, s->rdma_ciq, false);
-
-#undef ALLOC_OFLD_RXQS
-
 	for_each_port(adap, i) {
-		/*
-		 * Note that ->rdmarxq[i].rspq.cntxt_id below is 0 if we don't
+		/* Note that cmplqid below is 0 if we don't
 		 * have RDMA queues, and that's the right value.
 		 */
+		if (rxq_info)
+			cmplqid	= rxq_info->uldrxq[i].rspq.cntxt_id;
+
 		err = t4_sge_alloc_ctrl_txq(adap, &s->ctrlq[i], adap->port[i],
-					    s->fw_evtq.cntxt_id,
-					    s->rdmarxq[i].rspq.cntxt_id);
+					    s->fw_evtq.cntxt_id, cmplqid);
 		if (err)
 			goto freeout;
 	}
@@ -1174,6 +913,9 @@ freeout:	t4_free_sge_resources(adap);
 		     RSSCONTROL_V(netdev2pinfo(adap->port[0])->tx_chan) |
 		     QUEUENUMBER_V(s->ethrxq[0].rspq.abs_id));
 	return 0;
+freeout:
+	t4_free_sge_resources(adap);
+	return err;
 }
 
 /*
@@ -1197,151 +939,6 @@ void t4_free_mem(void *addr)
 	kvfree(addr);
 }
 
-/* Send a Work Request to write the filter at a specified index.  We construct
- * a Firmware Filter Work Request to have the work done and put the indicated
- * filter into "pending" mode which will prevent any further actions against
- * it till we get a reply from the firmware on the completion status of the
- * request.
- */
-static int set_filter_wr(struct adapter *adapter, int fidx)
-{
-	struct filter_entry *f = &adapter->tids.ftid_tab[fidx];
-	struct sk_buff *skb;
-	struct fw_filter_wr *fwr;
-	unsigned int ftid;
-
-	skb = alloc_skb(sizeof(*fwr), GFP_KERNEL);
-	if (!skb)
-		return -ENOMEM;
-
-	/* If the new filter requires loopback Destination MAC and/or VLAN
-	 * rewriting then we need to allocate a Layer 2 Table (L2T) entry for
-	 * the filter.
-	 */
-	if (f->fs.newdmac || f->fs.newvlan) {
-		/* allocate L2T entry for new filter */
-		f->l2t = t4_l2t_alloc_switching(adapter, f->fs.vlan,
-						f->fs.eport, f->fs.dmac);
-		if (f->l2t == NULL) {
-			kfree_skb(skb);
-			return -ENOMEM;
-		}
-	}
-
-	ftid = adapter->tids.ftid_base + fidx;
-
-	fwr = (struct fw_filter_wr *)__skb_put(skb, sizeof(*fwr));
-	memset(fwr, 0, sizeof(*fwr));
-
-	/* It would be nice to put most of the following in t4_hw.c but most
-	 * of the work is translating the cxgbtool ch_filter_specification
-	 * into the Work Request and the definition of that structure is
-	 * currently in cxgbtool.h which isn't appropriate to pull into the
-	 * common code.  We may eventually try to come up with a more neutral
-	 * filter specification structure but for now it's easiest to simply
-	 * put this fairly direct code in line ...
-	 */
-	fwr->op_pkd = htonl(FW_WR_OP_V(FW_FILTER_WR));
-	fwr->len16_pkd = htonl(FW_WR_LEN16_V(sizeof(*fwr)/16));
-	fwr->tid_to_iq =
-		htonl(FW_FILTER_WR_TID_V(ftid) |
-		      FW_FILTER_WR_RQTYPE_V(f->fs.type) |
-		      FW_FILTER_WR_NOREPLY_V(0) |
-		      FW_FILTER_WR_IQ_V(f->fs.iq));
-	fwr->del_filter_to_l2tix =
-		htonl(FW_FILTER_WR_RPTTID_V(f->fs.rpttid) |
-		      FW_FILTER_WR_DROP_V(f->fs.action == FILTER_DROP) |
-		      FW_FILTER_WR_DIRSTEER_V(f->fs.dirsteer) |
-		      FW_FILTER_WR_MASKHASH_V(f->fs.maskhash) |
-		      FW_FILTER_WR_DIRSTEERHASH_V(f->fs.dirsteerhash) |
-		      FW_FILTER_WR_LPBK_V(f->fs.action == FILTER_SWITCH) |
-		      FW_FILTER_WR_DMAC_V(f->fs.newdmac) |
-		      FW_FILTER_WR_SMAC_V(f->fs.newsmac) |
-		      FW_FILTER_WR_INSVLAN_V(f->fs.newvlan == VLAN_INSERT ||
-					     f->fs.newvlan == VLAN_REWRITE) |
-		      FW_FILTER_WR_RMVLAN_V(f->fs.newvlan == VLAN_REMOVE ||
-					    f->fs.newvlan == VLAN_REWRITE) |
-		      FW_FILTER_WR_HITCNTS_V(f->fs.hitcnts) |
-		      FW_FILTER_WR_TXCHAN_V(f->fs.eport) |
-		      FW_FILTER_WR_PRIO_V(f->fs.prio) |
-		      FW_FILTER_WR_L2TIX_V(f->l2t ? f->l2t->idx : 0));
-	fwr->ethtype = htons(f->fs.val.ethtype);
-	fwr->ethtypem = htons(f->fs.mask.ethtype);
-	fwr->frag_to_ovlan_vldm =
-		(FW_FILTER_WR_FRAG_V(f->fs.val.frag) |
-		 FW_FILTER_WR_FRAGM_V(f->fs.mask.frag) |
-		 FW_FILTER_WR_IVLAN_VLD_V(f->fs.val.ivlan_vld) |
-		 FW_FILTER_WR_OVLAN_VLD_V(f->fs.val.ovlan_vld) |
-		 FW_FILTER_WR_IVLAN_VLDM_V(f->fs.mask.ivlan_vld) |
-		 FW_FILTER_WR_OVLAN_VLDM_V(f->fs.mask.ovlan_vld));
-	fwr->smac_sel = 0;
-	fwr->rx_chan_rx_rpl_iq =
-		htons(FW_FILTER_WR_RX_CHAN_V(0) |
-		      FW_FILTER_WR_RX_RPL_IQ_V(adapter->sge.fw_evtq.abs_id));
-	fwr->maci_to_matchtypem =
-		htonl(FW_FILTER_WR_MACI_V(f->fs.val.macidx) |
-		      FW_FILTER_WR_MACIM_V(f->fs.mask.macidx) |
-		      FW_FILTER_WR_FCOE_V(f->fs.val.fcoe) |
-		      FW_FILTER_WR_FCOEM_V(f->fs.mask.fcoe) |
-		      FW_FILTER_WR_PORT_V(f->fs.val.iport) |
-		      FW_FILTER_WR_PORTM_V(f->fs.mask.iport) |
-		      FW_FILTER_WR_MATCHTYPE_V(f->fs.val.matchtype) |
-		      FW_FILTER_WR_MATCHTYPEM_V(f->fs.mask.matchtype));
-	fwr->ptcl = f->fs.val.proto;
-	fwr->ptclm = f->fs.mask.proto;
-	fwr->ttyp = f->fs.val.tos;
-	fwr->ttypm = f->fs.mask.tos;
-	fwr->ivlan = htons(f->fs.val.ivlan);
-	fwr->ivlanm = htons(f->fs.mask.ivlan);
-	fwr->ovlan = htons(f->fs.val.ovlan);
-	fwr->ovlanm = htons(f->fs.mask.ovlan);
-	memcpy(fwr->lip, f->fs.val.lip, sizeof(fwr->lip));
-	memcpy(fwr->lipm, f->fs.mask.lip, sizeof(fwr->lipm));
-	memcpy(fwr->fip, f->fs.val.fip, sizeof(fwr->fip));
-	memcpy(fwr->fipm, f->fs.mask.fip, sizeof(fwr->fipm));
-	fwr->lp = htons(f->fs.val.lport);
-	fwr->lpm = htons(f->fs.mask.lport);
-	fwr->fp = htons(f->fs.val.fport);
-	fwr->fpm = htons(f->fs.mask.fport);
-	if (f->fs.newsmac)
-		memcpy(fwr->sma, f->fs.smac, sizeof(fwr->sma));
-
-	/* Mark the filter as "pending" and ship off the Filter Work Request.
-	 * When we get the Work Request Reply we'll clear the pending status.
-	 */
-	f->pending = 1;
-	set_wr_txq(skb, CPL_PRIORITY_CONTROL, f->fs.val.iport & 0x3);
-	t4_ofld_send(adapter, skb);
-	return 0;
-}
-
-/* Delete the filter at a specified index.
- */
-static int del_filter_wr(struct adapter *adapter, int fidx)
-{
-	struct filter_entry *f = &adapter->tids.ftid_tab[fidx];
-	struct sk_buff *skb;
-	struct fw_filter_wr *fwr;
-	unsigned int len, ftid;
-
-	len = sizeof(*fwr);
-	ftid = adapter->tids.ftid_base + fidx;
-
-	skb = alloc_skb(len, GFP_KERNEL);
-	if (!skb)
-		return -ENOMEM;
-
-	fwr = (struct fw_filter_wr *)__skb_put(skb, len);
-	t4_mk_filtdelwr(ftid, fwr, adapter->sge.fw_evtq.abs_id);
-
-	/* Mark the filter as "pending" and ship off the Filter Work Request.
-	 * When we get the Work Request Reply we'll clear the pending status.
-	 */
-	f->pending = 1;
-	t4_mgmt_tx(adapter, skb);
-	return 0;
-}
-
 static u16 cxgb_select_queue(struct net_device *dev, struct sk_buff *skb,
 			     void *accel_priv, select_queue_fallback_t fallback)
 {
@@ -1723,19 +1320,22 @@ EXPORT_SYMBOL(cxgb4_remove_tid);
  */
 static int tid_init(struct tid_info *t)
 {
-	size_t size;
-	unsigned int stid_bmap_size;
-	unsigned int natids = t->natids;
 	struct adapter *adap = container_of(t, struct adapter, tids);
+	unsigned int max_ftids = t->nftids + t->nsftids;
+	unsigned int natids = t->natids;
+	unsigned int stid_bmap_size;
+	unsigned int ftid_bmap_size;
+	size_t size;
 
 	stid_bmap_size = BITS_TO_LONGS(t->nstids + t->nsftids);
+	ftid_bmap_size = BITS_TO_LONGS(t->nftids);
 	size = t->ntids * sizeof(*t->tid_tab) +
 	       natids * sizeof(*t->atid_tab) +
 	       t->nstids * sizeof(*t->stid_tab) +
 	       t->nsftids * sizeof(*t->stid_tab) +
 	       stid_bmap_size * sizeof(long) +
-	       t->nftids * sizeof(*t->ftid_tab) +
-	       t->nsftids * sizeof(*t->ftid_tab);
+	       max_ftids * sizeof(*t->ftid_tab) +
+	       ftid_bmap_size * sizeof(long);
 
 	t->tid_tab = t4_alloc_mem(size);
 	if (!t->tid_tab)
@@ -1745,8 +1345,10 @@ static int tid_init(struct tid_info *t)
 	t->stid_tab = (struct serv_entry *)&t->atid_tab[natids];
 	t->stid_bmap = (unsigned long *)&t->stid_tab[t->nstids + t->nsftids];
 	t->ftid_tab = (struct filter_entry *)&t->stid_bmap[stid_bmap_size];
+	t->ftid_bmap = (unsigned long *)&t->ftid_tab[max_ftids];
 	spin_lock_init(&t->stid_lock);
 	spin_lock_init(&t->atid_lock);
+	spin_lock_init(&t->ftid_lock);
 
 	t->stids_in_use = 0;
 	t->sftids_in_use = 0;
@@ -1761,12 +1363,16 @@ static int tid_init(struct tid_info *t)
 			t->atid_tab[natids - 1].next = &t->atid_tab[natids];
 		t->afree = t->atid_tab;
 	}
-	bitmap_zero(t->stid_bmap, t->nstids + t->nsftids);
-	/* Reserve stid 0 for T4/T5 adapters */
-	if (!t->stid_base &&
-	    (CHELSIO_CHIP_VERSION(adap->params.chip) <= CHELSIO_T5))
-		__set_bit(0, t->stid_bmap);
 
+	if (is_offload(adap)) {
+		bitmap_zero(t->stid_bmap, t->nstids + t->nsftids);
+		/* Reserve stid 0 for T4/T5 adapters */
+		if (!t->stid_base &&
+		    CHELSIO_CHIP_VERSION(adap->params.chip) <= CHELSIO_T5)
+			__set_bit(0, t->stid_bmap);
+	}
+
+	bitmap_zero(t->ftid_bmap, t->nftids);
 	return 0;
 }
 
@@ -2316,7 +1922,7 @@ static void disable_dbs(struct adapter *adap)
 
 	for_each_ethrxq(&adap->sge, i)
 		disable_txq_db(&adap->sge.ethtxq[i].q);
-	for_each_iscsirxq(&adap->sge, i)
+	for_each_ofldtxq(&adap->sge, i)
 		disable_txq_db(&adap->sge.ofldtxq[i].q);
 	for_each_port(adap, i)
 		disable_txq_db(&adap->sge.ctrlq[i].q);
@@ -2328,7 +1934,7 @@ static void enable_dbs(struct adapter *adap)
 
 	for_each_ethrxq(&adap->sge, i)
 		enable_txq_db(adap, &adap->sge.ethtxq[i].q);
-	for_each_iscsirxq(&adap->sge, i)
+	for_each_ofldtxq(&adap->sge, i)
 		enable_txq_db(adap, &adap->sge.ofldtxq[i].q);
 	for_each_port(adap, i)
 		enable_txq_db(adap, &adap->sge.ctrlq[i].q);
@@ -2336,9 +1942,10 @@ static void enable_dbs(struct adapter *adap)
 
 static void notify_rdma_uld(struct adapter *adap, enum cxgb4_control cmd)
 {
-	if (adap->uld_handle[CXGB4_ULD_RDMA])
-		ulds[CXGB4_ULD_RDMA].control(adap->uld_handle[CXGB4_ULD_RDMA],
-				cmd);
+	enum cxgb4_uld type = CXGB4_ULD_RDMA;
+
+	if (adap->uld && adap->uld[type].handle)
+		adap->uld[type].control(adap->uld[type].handle, cmd);
 }
 
 static void process_db_full(struct work_struct *work)
@@ -2392,13 +1999,14 @@ out:
 	if (ret)
 		CH_WARN(adap, "DB drop recovery failed.\n");
 }
+
 static void recover_all_queues(struct adapter *adap)
 {
 	int i;
 
 	for_each_ethrxq(&adap->sge, i)
 		sync_txq_pidx(adap, &adap->sge.ethtxq[i].q);
-	for_each_iscsirxq(&adap->sge, i)
+	for_each_ofldtxq(&adap->sge, i)
 		sync_txq_pidx(adap, &adap->sge.ofldtxq[i].q);
 	for_each_port(adap, i)
 		sync_txq_pidx(adap, &adap->sge.ctrlq[i].q);
@@ -2463,94 +2071,12 @@ void t4_db_dropped(struct adapter *adap)
 	queue_work(adap->workq, &adap->db_drop_task);
 }
 
-static void uld_attach(struct adapter *adap, unsigned int uld)
-{
-	void *handle;
-	struct cxgb4_lld_info lli;
-	unsigned short i;
-
-	lli.pdev = adap->pdev;
-	lli.pf = adap->pf;
-	lli.l2t = adap->l2t;
-	lli.tids = &adap->tids;
-	lli.ports = adap->port;
-	lli.vr = &adap->vres;
-	lli.mtus = adap->params.mtus;
-	if (uld == CXGB4_ULD_RDMA) {
-		lli.rxq_ids = adap->sge.rdma_rxq;
-		lli.ciq_ids = adap->sge.rdma_ciq;
-		lli.nrxq = adap->sge.rdmaqs;
-		lli.nciq = adap->sge.rdmaciqs;
-	} else if (uld == CXGB4_ULD_ISCSI) {
-		lli.rxq_ids = adap->sge.iscsi_rxq;
-		lli.nrxq = adap->sge.iscsiqsets;
-	} else if (uld == CXGB4_ULD_ISCSIT) {
-		lli.rxq_ids = adap->sge.iscsit_rxq;
-		lli.nrxq = adap->sge.niscsitq;
-	}
-	lli.ntxq = adap->sge.iscsiqsets;
-	lli.nchan = adap->params.nports;
-	lli.nports = adap->params.nports;
-	lli.wr_cred = adap->params.ofldq_wr_cred;
-	lli.adapter_type = adap->params.chip;
-	lli.iscsi_iolen = MAXRXDATA_G(t4_read_reg(adap, TP_PARA_REG2_A));
-	lli.iscsi_tagmask = t4_read_reg(adap, ULP_RX_ISCSI_TAGMASK_A);
-	lli.iscsi_pgsz_order = t4_read_reg(adap, ULP_RX_ISCSI_PSZ_A);
-	lli.iscsi_llimit = t4_read_reg(adap, ULP_RX_ISCSI_LLIMIT_A);
-	lli.iscsi_ppm = &adap->iscsi_ppm;
-	lli.cclk_ps = 1000000000 / adap->params.vpd.cclk;
-	lli.udb_density = 1 << adap->params.sge.eq_qpp;
-	lli.ucq_density = 1 << adap->params.sge.iq_qpp;
-	lli.filt_mode = adap->params.tp.vlan_pri_map;
-	/* MODQ_REQ_MAP sets queues 0-3 to chan 0-3 */
-	for (i = 0; i < NCHAN; i++)
-		lli.tx_modq[i] = i;
-	lli.gts_reg = adap->regs + MYPF_REG(SGE_PF_GTS_A);
-	lli.db_reg = adap->regs + MYPF_REG(SGE_PF_KDOORBELL_A);
-	lli.fw_vers = adap->params.fw_vers;
-	lli.dbfifo_int_thresh = dbfifo_int_thresh;
-	lli.sge_ingpadboundary = adap->sge.fl_align;
-	lli.sge_egrstatuspagesize = adap->sge.stat_len;
-	lli.sge_pktshift = adap->sge.pktshift;
-	lli.enable_fw_ofld_conn = adap->flags & FW_OFLD_CONN;
-	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)) {
-		dev_warn(adap->pdev_dev,
-			 "could not attach to the %s driver, error %ld\n",
-			 uld_str[uld], PTR_ERR(handle));
-		return;
-	}
-
-	adap->uld_handle[uld] = handle;
-
+void t4_register_netevent_notifier(void)
+{
 	if (!netevent_registered) {
 		register_netevent_notifier(&cxgb4_netevent_nb);
 		netevent_registered = true;
 	}
-
-	if (adap->flags & FULL_INIT_DONE)
-		ulds[uld].state_change(handle, CXGB4_STATE_UP);
-}
-
-static void attach_ulds(struct adapter *adap)
-{
-	unsigned int i;
-
-	spin_lock(&adap_rcu_lock);
-	list_add_tail_rcu(&adap->rcu_node, &adap_rcu_list);
-	spin_unlock(&adap_rcu_lock);
-
-	mutex_lock(&uld_mutex);
-	list_add_tail(&adap->list_node, &adapter_list);
-	for (i = 0; i < CXGB4_ULD_MAX; i++)
-		if (ulds[i].add)
-			uld_attach(adap, i);
-	mutex_unlock(&uld_mutex);
 }
 
 static void detach_ulds(struct adapter *adap)
@@ -2560,20 +2086,16 @@ static void detach_ulds(struct adapter *adap)
 	mutex_lock(&uld_mutex);
 	list_del(&adap->list_node);
 	for (i = 0; i < CXGB4_ULD_MAX; i++)
-		if (adap->uld_handle[i]) {
-			ulds[i].state_change(adap->uld_handle[i],
+		if (adap->uld && adap->uld[i].handle) {
+			adap->uld[i].state_change(adap->uld[i].handle,
 					     CXGB4_STATE_DETACH);
-			adap->uld_handle[i] = NULL;
+			adap->uld[i].handle = NULL;
 		}
 	if (netevent_registered && list_empty(&adapter_list)) {
 		unregister_netevent_notifier(&cxgb4_netevent_nb);
 		netevent_registered = false;
 	}
 	mutex_unlock(&uld_mutex);
-
-	spin_lock(&adap_rcu_lock);
-	list_del_rcu(&adap->rcu_node);
-	spin_unlock(&adap_rcu_lock);
 }
 
 static void notify_ulds(struct adapter *adap, enum cxgb4_state new_state)
@@ -2582,60 +2104,11 @@ static void notify_ulds(struct adapter *adap, enum cxgb4_state new_state)
 
 	mutex_lock(&uld_mutex);
 	for (i = 0; i < CXGB4_ULD_MAX; i++)
-		if (adap->uld_handle[i])
-			ulds[i].state_change(adap->uld_handle[i], new_state);
-	mutex_unlock(&uld_mutex);
-}
-
-/**
- *	cxgb4_register_uld - register an upper-layer driver
- *	@type: the ULD type
- *	@p: the ULD methods
- *
- *	Registers an upper-layer driver with this driver and notifies the ULD
- *	about any presently available devices that support its type.  Returns
- *	%-EBUSY if a ULD of the same type is already registered.
- */
-int cxgb4_register_uld(enum cxgb4_uld type, const struct cxgb4_uld_info *p)
-{
-	int ret = 0;
-	struct adapter *adap;
-
-	if (type >= CXGB4_ULD_MAX)
-		return -EINVAL;
-	mutex_lock(&uld_mutex);
-	if (ulds[type].add) {
-		ret = -EBUSY;
-		goto out;
-	}
-	ulds[type] = *p;
-	list_for_each_entry(adap, &adapter_list, list_node)
-		uld_attach(adap, type);
-out:	mutex_unlock(&uld_mutex);
-	return ret;
-}
-EXPORT_SYMBOL(cxgb4_register_uld);
-
-/**
- *	cxgb4_unregister_uld - unregister an upper-layer driver
- *	@type: the ULD type
- *
- *	Unregisters an existing upper-layer driver.
- */
-int cxgb4_unregister_uld(enum cxgb4_uld type)
-{
-	struct adapter *adap;
-
-	if (type >= CXGB4_ULD_MAX)
-		return -EINVAL;
-	mutex_lock(&uld_mutex);
-	list_for_each_entry(adap, &adapter_list, list_node)
-		adap->uld_handle[type] = NULL;
-	ulds[type].add = NULL;
+		if (adap->uld && adap->uld[i].handle)
+			adap->uld[i].state_change(adap->uld[i].handle,
+						  new_state);
 	mutex_unlock(&uld_mutex);
-	return 0;
 }
-EXPORT_SYMBOL(cxgb4_unregister_uld);
 
 #if IS_ENABLED(CONFIG_IPV6)
 static int cxgb4_inet6addr_handler(struct notifier_block *this,
@@ -2741,7 +2214,6 @@ static int cxgb_up(struct adapter *adap)
 				  adap->msix_info[0].desc, adap);
 		if (err)
 			goto irq_err;
-
 		err = request_msix_queue_irqs(adap);
 		if (err) {
 			free_irq(adap->msix_info[0].vec, adap);
@@ -2819,40 +2291,6 @@ static int cxgb_close(struct net_device *dev)
 	return t4_enable_vi(adapter, adapter->pf, pi->viid, false, false);
 }
 
-/* Return an error number if the indicated filter isn't writable ...
- */
-static int writable_filter(struct filter_entry *f)
-{
-	if (f->locked)
-		return -EPERM;
-	if (f->pending)
-		return -EBUSY;
-
-	return 0;
-}
-
-/* Delete the filter at the specified index (if valid).  The checks for all
- * the common problems with doing this like the filter being locked, currently
- * pending in another operation, etc.
- */
-static int delete_filter(struct adapter *adapter, unsigned int fidx)
-{
-	struct filter_entry *f;
-	int ret;
-
-	if (fidx >= adapter->tids.nftids + adapter->tids.nsftids)
-		return -EINVAL;
-
-	f = &adapter->tids.ftid_tab[fidx];
-	ret = writable_filter(f);
-	if (ret)
-		return ret;
-	if (f->valid)
-		return del_filter_wr(adapter, fidx);
-
-	return 0;
-}
-
 int cxgb4_create_server_filter(const struct net_device *dev, unsigned int stid,
 		__be32 sip, __be16 sport, __be16 vlan,
 		unsigned int queue, unsigned char port, unsigned char mask)
@@ -2922,7 +2360,6 @@ EXPORT_SYMBOL(cxgb4_create_server_filter);
 int cxgb4_remove_server_filter(const struct net_device *dev, unsigned int stid,
 		unsigned int queue, bool ipv6)
 {
-	int ret;
 	struct filter_entry *f;
 	struct adapter *adap;
 
@@ -2936,11 +2373,7 @@ int cxgb4_remove_server_filter(const struct net_device *dev, unsigned int stid,
 	/* Unlock the filter */
 	f->locked = 0;
 
-	ret = delete_filter(adap, stid);
-	if (ret)
-		return ret;
-
-	return 0;
+	return delete_filter(adap, stid);
 }
 EXPORT_SYMBOL(cxgb4_remove_server_filter);
 
@@ -3078,6 +2511,85 @@ static int cxgb_change_mtu(struct net_device *dev, int new_mtu)
 	return ret;
 }
 
+#ifdef CONFIG_PCI_IOV
+static int dummy_open(struct net_device *dev)
+{
+	/* Turn carrier off since we don't have to transmit anything on this
+	 * interface.
+	 */
+	netif_carrier_off(dev);
+	return 0;
+}
+
+/* Fill MAC address that will be assigned by the FW */
+static void fill_vf_station_mac_addr(struct adapter *adap)
+{
+	unsigned int i;
+	u8 hw_addr[ETH_ALEN], macaddr[ETH_ALEN];
+	int err;
+	u8 *na;
+	u16 a, b;
+
+	err = t4_get_raw_vpd_params(adap, &adap->params.vpd);
+	if (!err) {
+		na = adap->params.vpd.na;
+		for (i = 0; i < ETH_ALEN; i++)
+			hw_addr[i] = (hex2val(na[2 * i + 0]) * 16 +
+				      hex2val(na[2 * i + 1]));
+		a = (hw_addr[0] << 8) | hw_addr[1];
+		b = (hw_addr[1] << 8) | hw_addr[2];
+		a ^= b;
+		a |= 0x0200;    /* locally assigned Ethernet MAC address */
+		a &= ~0x0100;   /* not a multicast Ethernet MAC address */
+		macaddr[0] = a >> 8;
+		macaddr[1] = a & 0xff;
+
+		for (i = 2; i < 5; i++)
+			macaddr[i] = hw_addr[i + 1];
+
+		for (i = 0; i < adap->num_vfs; i++) {
+			macaddr[5] = adap->pf * 16 + i;
+			ether_addr_copy(adap->vfinfo[i].vf_mac_addr, macaddr);
+		}
+	}
+}
+
+static int cxgb_set_vf_mac(struct net_device *dev, int vf, u8 *mac)
+{
+	struct port_info *pi = netdev_priv(dev);
+	struct adapter *adap = pi->adapter;
+	int ret;
+
+	/* verify MAC addr is valid */
+	if (!is_valid_ether_addr(mac)) {
+		dev_err(pi->adapter->pdev_dev,
+			"Invalid Ethernet address %pM for VF %d\n",
+			mac, vf);
+		return -EINVAL;
+	}
+
+	dev_info(pi->adapter->pdev_dev,
+		 "Setting MAC %pM on VF %d\n", mac, vf);
+	ret = t4_set_vf_mac_acl(adap, vf + 1, 1, mac);
+	if (!ret)
+		ether_addr_copy(adap->vfinfo[vf].vf_mac_addr, mac);
+	return ret;
+}
+
+static int cxgb_get_vf_config(struct net_device *dev,
+			      int vf, struct ifla_vf_info *ivi)
+{
+	struct port_info *pi = netdev_priv(dev);
+	struct adapter *adap = pi->adapter;
+
+	if (vf >= adap->num_vfs)
+		return -EINVAL;
+	ivi->vf = vf;
+	ether_addr_copy(ivi->mac, adap->vfinfo[vf].vf_mac_addr);
+	return 0;
+}
+#endif
+
 static int cxgb_set_mac_addr(struct net_device *dev, void *p)
 {
 	int ret;
@@ -3114,6 +2626,116 @@ static void cxgb_netpoll(struct net_device *dev)
 }
 #endif
 
+static int cxgb_set_tx_maxrate(struct net_device *dev, int index, u32 rate)
+{
+	struct port_info *pi = netdev_priv(dev);
+	struct adapter *adap = pi->adapter;
+	struct sched_class *e;
+	struct ch_sched_params p;
+	struct ch_sched_queue qe;
+	u32 req_rate;
+	int err = 0;
+
+	if (!can_sched(dev))
+		return -ENOTSUPP;
+
+	if (index < 0 || index > pi->nqsets - 1)
+		return -EINVAL;
+
+	if (!(adap->flags & FULL_INIT_DONE)) {
+		dev_err(adap->pdev_dev,
+			"Failed to rate limit on queue %d. Link Down?\n",
+			index);
+		return -EINVAL;
+	}
+
+	/* Convert from Mbps to Kbps */
+	req_rate = rate << 10;
+
+	/* Max rate is 10 Gbps */
+	if (req_rate >= SCHED_MAX_RATE_KBPS) {
+		dev_err(adap->pdev_dev,
+			"Invalid rate %u Mbps, Max rate is %u Gbps\n",
+			rate, SCHED_MAX_RATE_KBPS);
+		return -ERANGE;
+	}
+
+	/* First unbind the queue from any existing class */
+	memset(&qe, 0, sizeof(qe));
+	qe.queue = index;
+	qe.class = SCHED_CLS_NONE;
+
+	err = cxgb4_sched_class_unbind(dev, (void *)(&qe), SCHED_QUEUE);
+	if (err) {
+		dev_err(adap->pdev_dev,
+			"Unbinding Queue %d on port %d fail. Err: %d\n",
+			index, pi->port_id, err);
+		return err;
+	}
+
+	/* Queue already unbound */
+	if (!req_rate)
+		return 0;
+
+	/* Fetch any available unused or matching scheduling class */
+	memset(&p, 0, sizeof(p));
+	p.type = SCHED_CLASS_TYPE_PACKET;
+	p.u.params.level    = SCHED_CLASS_LEVEL_CL_RL;
+	p.u.params.mode     = SCHED_CLASS_MODE_CLASS;
+	p.u.params.rateunit = SCHED_CLASS_RATEUNIT_BITS;
+	p.u.params.ratemode = SCHED_CLASS_RATEMODE_ABS;
+	p.u.params.channel  = pi->tx_chan;
+	p.u.params.class    = SCHED_CLS_NONE;
+	p.u.params.minrate  = 0;
+	p.u.params.maxrate  = req_rate;
+	p.u.params.weight   = 0;
+	p.u.params.pktsize  = dev->mtu;
+
+	e = cxgb4_sched_class_alloc(dev, &p);
+	if (!e)
+		return -ENOMEM;
+
+	/* Bind the queue to a scheduling class */
+	memset(&qe, 0, sizeof(qe));
+	qe.queue = index;
+	qe.class = e->idx;
+
+	err = cxgb4_sched_class_bind(dev, (void *)(&qe), SCHED_QUEUE);
+	if (err)
+		dev_err(adap->pdev_dev,
+			"Queue rate limiting failed. Err: %d\n", err);
+	return err;
+}
+
+static int cxgb_setup_tc(struct net_device *dev, u32 handle, __be16 proto,
+			 struct tc_to_netdev *tc)
+{
+	struct port_info *pi = netdev2pinfo(dev);
+	struct adapter *adap = netdev2adap(dev);
+
+	if (!(adap->flags & FULL_INIT_DONE)) {
+		dev_err(adap->pdev_dev,
+			"Failed to setup tc on port %d. Link Down?\n",
+			pi->port_id);
+		return -EINVAL;
+	}
+
+	if (TC_H_MAJ(handle) == TC_H_MAJ(TC_H_INGRESS) &&
+	    tc->type == TC_SETUP_CLSU32) {
+		switch (tc->cls_u32->command) {
+		case TC_CLSU32_NEW_KNODE:
+		case TC_CLSU32_REPLACE_KNODE:
+			return cxgb4_config_knode(dev, proto, tc->cls_u32);
+		case TC_CLSU32_DELETE_KNODE:
+			return cxgb4_delete_knode(dev, proto, tc->cls_u32);
+		default:
+			return -EOPNOTSUPP;
+		}
+	}
+
+	return -EOPNOTSUPP;
+}
+
 static const struct net_device_ops cxgb4_netdev_ops = {
 	.ndo_open             = cxgb_open,
 	.ndo_stop             = cxgb_close,
@@ -3136,7 +2758,31 @@ static const struct net_device_ops cxgb4_netdev_ops = {
 #ifdef CONFIG_NET_RX_BUSY_POLL
 	.ndo_busy_poll        = cxgb_busy_poll,
 #endif
+	.ndo_set_tx_maxrate   = cxgb_set_tx_maxrate,
+	.ndo_setup_tc         = cxgb_setup_tc,
+};
 
+#ifdef CONFIG_PCI_IOV
+static const struct net_device_ops cxgb4_mgmt_netdev_ops = {
+	.ndo_open             = dummy_open,
+	.ndo_set_vf_mac       = cxgb_set_vf_mac,
+	.ndo_get_vf_config    = cxgb_get_vf_config,
+};
+#endif
+
+static void get_drvinfo(struct net_device *dev, struct ethtool_drvinfo *info)
+{
+	struct adapter *adapter = netdev2adap(dev);
+
+	strlcpy(info->driver, cxgb4_driver_name, sizeof(info->driver));
+	strlcpy(info->version, cxgb4_driver_version,
+		sizeof(info->version));
+	strlcpy(info->bus_info, pci_name(adapter->pdev),
+		sizeof(info->bus_info));
+}
+
+static const struct ethtool_ops cxgb4_mgmt_ethtool_ops = {
+	.get_drvinfo       = get_drvinfo,
 };
 
 void t4_fatal_err(struct adapter *adap)
@@ -3979,6 +3625,12 @@ static int adap_init0(struct adapter *adap)
 	adap->clipt_start = val[0];
 	adap->clipt_end = val[1];
 
+	/* We don't yet have a PARAMs calls to retrieve the number of Traffic
+	 * Classes supported by the hardware/firmware so we hard code it here
+	 * for now.
+	 */
+	adap->params.nsched_cls = is_t4(adap->params.chip) ? 15 : 16;
+
 	/* query params related to active filter region */
 	params[0] = FW_PARAM_PFVF(ACTIVE_FILTER_START);
 	params[1] = FW_PARAM_PFVF(ACTIVE_FILTER_END);
@@ -4016,6 +3668,12 @@ static int adap_init0(struct adapter *adap)
 		adap->params.ulptx_memwrite_dsgl = (ret == 0 && val[0] != 0);
 	}
 
+	/* See if FW supports FW_RI_FR_NSMR_TPTE_WR work request */
+	params[0] = FW_PARAM_DEV(RI_FR_NSMR_TPTE_WR);
+	ret = t4_query_params(adap, adap->mbox, adap->pf, 0,
+			      1, params, val);
+	adap->params.fr_nsmr_tpte_wr_support = (ret == 0 && val[0] != 0);
+
 	/*
 	 * Get device capabilities so we can determine what resources we need
 	 * to manage.
@@ -4067,6 +3725,7 @@ static int adap_init0(struct adapter *adap)
 		adap->params.ofldq_wr_cred = val[5];
 
 		adap->params.offload = 1;
+		adap->num_ofld_uld += 1;
 	}
 	if (caps_cmd.rdmacaps) {
 		params[0] = FW_PARAM_PFVF(STAG_START);
@@ -4119,6 +3778,7 @@ static int adap_init0(struct adapter *adap)
 			 "max_ordird_qp %d max_ird_adapter %d\n",
 			 adap->params.max_ordird_qp,
 			 adap->params.max_ird_adapter);
+		adap->num_ofld_uld += 2;
 	}
 	if (caps_cmd.iscsicaps) {
 		params[0] = FW_PARAM_PFVF(ISCSI_START);
@@ -4129,6 +3789,13 @@ static int adap_init0(struct adapter *adap)
 			goto bye;
 		adap->vres.iscsi.start = val[0];
 		adap->vres.iscsi.size = val[1] - val[0] + 1;
+		/* LIO target and cxgb4i initiaitor */
+		adap->num_ofld_uld += 2;
+	}
+	if (caps_cmd.cryptocaps) {
+		/* Should query params here...TODO */
+		adap->params.crypto |= ULP_CRYPTO_LOOKASIDE;
+		adap->num_uld += 1;
 	}
 #undef FW_PARAM_PFVF
 #undef FW_PARAM_DEV
@@ -4318,16 +3985,6 @@ static inline bool is_x_10g_port(const struct link_config *lc)
 	return high_speeds != 0;
 }
 
-static inline void init_rspq(struct adapter *adap, struct sge_rspq *q,
-			     unsigned int us, unsigned int cnt,
-			     unsigned int size, unsigned int iqe_size)
-{
-	q->adap = adap;
-	cxgb4_set_rspq_intr_params(q, us, cnt);
-	q->iqe_len = iqe_size;
-	q->size = size;
-}
-
 /*
  * Perform default configuration of DMA queues depending on the number and type
  * of ports we found and the number of available CPUs.  Most settings can be
@@ -4340,12 +3997,16 @@ static void cfg_queues(struct adapter *adap)
 #ifndef CONFIG_CHELSIO_T4_DCB
 	int q10g = 0;
 #endif
-	int ciq_size;
 
 	/* Reduce memory usage in kdump environment, disable all offload.
 	 */
-	if (is_kdump_kernel())
+	if (is_kdump_kernel()) {
 		adap->params.offload = 0;
+		adap->params.crypto = 0;
+	} else if (is_uld(adap) && t4_uld_mem_alloc(adap)) {
+		adap->params.offload = 0;
+		adap->params.crypto = 0;
+	}
 
 	for_each_port(adap, i)
 		n10g += is_x_10g_port(&adap2pinfo(adap, i)->link_cfg);
@@ -4389,33 +4050,18 @@ static void cfg_queues(struct adapter *adap)
 	s->ethqsets = qidx;
 	s->max_ethqsets = qidx;   /* MSI-X may lower it later */
 
-	if (is_offload(adap)) {
+	if (is_uld(adap)) {
 		/*
 		 * For offload we use 1 queue/channel if all ports are up to 1G,
 		 * otherwise we divide all available queues amongst the channels
 		 * capped by the number of available cores.
 		 */
 		if (n10g) {
-			i = min_t(int, ARRAY_SIZE(s->iscsirxq),
-				  num_online_cpus());
-			s->iscsiqsets = roundup(i, adap->params.nports);
-		} else
-			s->iscsiqsets = adap->params.nports;
-		/* For RDMA one Rx queue per channel suffices */
-		s->rdmaqs = adap->params.nports;
-		/* Try and allow at least 1 CIQ per cpu rounding down
-		 * to the number of ports, with a minimum of 1 per port.
-		 * A 2 port card in a 6 cpu system: 6 CIQs, 3 / port.
-		 * A 4 port card in a 6 cpu system: 4 CIQs, 1 / port.
-		 * A 4 port card in a 2 cpu system: 4 CIQs, 1 / port.
-		 */
-		s->rdmaciqs = min_t(int, MAX_RDMA_CIQS, num_online_cpus());
-		s->rdmaciqs = (s->rdmaciqs / adap->params.nports) *
-				adap->params.nports;
-		s->rdmaciqs = max_t(int, s->rdmaciqs, adap->params.nports);
-
-		if (!is_t4(adap->params.chip))
-			s->niscsitq = s->iscsiqsets;
+			i = num_online_cpus();
+			s->ofldqsets = roundup(i, adap->params.nports);
+		} else {
+			s->ofldqsets = adap->params.nports;
+		}
 	}
 
 	for (i = 0; i < ARRAY_SIZE(s->ethrxq); i++) {
@@ -4434,47 +4080,8 @@ static void cfg_queues(struct adapter *adap)
 	for (i = 0; i < ARRAY_SIZE(s->ofldtxq); i++)
 		s->ofldtxq[i].q.size = 1024;
 
-	for (i = 0; i < ARRAY_SIZE(s->iscsirxq); i++) {
-		struct sge_ofld_rxq *r = &s->iscsirxq[i];
-
-		init_rspq(adap, &r->rspq, 5, 1, 1024, 64);
-		r->rspq.uld = CXGB4_ULD_ISCSI;
-		r->fl.size = 72;
-	}
-
-	if (!is_t4(adap->params.chip)) {
-		for (i = 0; i < ARRAY_SIZE(s->iscsitrxq); i++) {
-			struct sge_ofld_rxq *r = &s->iscsitrxq[i];
-
-			init_rspq(adap, &r->rspq, 5, 1, 1024, 64);
-			r->rspq.uld = CXGB4_ULD_ISCSIT;
-			r->fl.size = 72;
-		}
-	}
-
-	for (i = 0; i < ARRAY_SIZE(s->rdmarxq); i++) {
-		struct sge_ofld_rxq *r = &s->rdmarxq[i];
-
-		init_rspq(adap, &r->rspq, 5, 1, 511, 64);
-		r->rspq.uld = CXGB4_ULD_RDMA;
-		r->fl.size = 72;
-	}
-
-	ciq_size = 64 + adap->vres.cq.size + adap->tids.nftids;
-	if (ciq_size > SGE_MAX_IQ_SIZE) {
-		CH_WARN(adap, "CIQ size too small for available IQs\n");
-		ciq_size = SGE_MAX_IQ_SIZE;
-	}
-
-	for (i = 0; i < ARRAY_SIZE(s->rdmaciq); i++) {
-		struct sge_ofld_rxq *r = &s->rdmaciq[i];
-
-		init_rspq(adap, &r->rspq, 5, 1, ciq_size, 64);
-		r->rspq.uld = CXGB4_ULD_RDMA;
-	}
-
 	init_rspq(adap, &s->fw_evtq, 0, 1, 1024, 64);
-	init_rspq(adap, &s->intrq, 0, 1, 2 * MAX_INGQ, 64);
+	init_rspq(adap, &s->intrq, 0, 1, 512, 64);
 }
 
 /*
@@ -4505,42 +4112,90 @@ static void reduce_ethqs(struct adapter *adap, int n)
 	}
 }
 
+static int get_msix_info(struct adapter *adap)
+{
+	struct uld_msix_info *msix_info;
+	unsigned int max_ingq = 0;
+
+	if (is_offload(adap))
+		max_ingq += MAX_OFLD_QSETS * adap->num_ofld_uld;
+	if (is_pci_uld(adap))
+		max_ingq += MAX_OFLD_QSETS * adap->num_uld;
+
+	if (!max_ingq)
+		goto out;
+
+	msix_info = kcalloc(max_ingq, sizeof(*msix_info), GFP_KERNEL);
+	if (!msix_info)
+		return -ENOMEM;
+
+	adap->msix_bmap_ulds.msix_bmap = kcalloc(BITS_TO_LONGS(max_ingq),
+						 sizeof(long), GFP_KERNEL);
+	if (!adap->msix_bmap_ulds.msix_bmap) {
+		kfree(msix_info);
+		return -ENOMEM;
+	}
+	spin_lock_init(&adap->msix_bmap_ulds.lock);
+	adap->msix_info_ulds = msix_info;
+out:
+	return 0;
+}
+
+static void free_msix_info(struct adapter *adap)
+{
+	if (!(adap->num_uld && adap->num_ofld_uld))
+		return;
+
+	kfree(adap->msix_info_ulds);
+	kfree(adap->msix_bmap_ulds.msix_bmap);
+}
+
 /* 2 MSI-X vectors needed for the FW queue and non-data interrupts */
 #define EXTRA_VECS 2
 
 static int enable_msix(struct adapter *adap)
 {
-	int ofld_need = 0;
-	int i, want, need, allocated;
+	int ofld_need = 0, uld_need = 0;
+	int i, j, want, need, allocated;
 	struct sge *s = &adap->sge;
 	unsigned int nchan = adap->params.nports;
 	struct msix_entry *entries;
+	int max_ingq = MAX_INGQ;
 
-	entries = kmalloc(sizeof(*entries) * (MAX_INGQ + 1),
+	if (is_pci_uld(adap))
+		max_ingq += (MAX_OFLD_QSETS * adap->num_uld);
+	if (is_offload(adap))
+		max_ingq += (MAX_OFLD_QSETS * adap->num_ofld_uld);
+	entries = kmalloc(sizeof(*entries) * (max_ingq + 1),
 			  GFP_KERNEL);
 	if (!entries)
 		return -ENOMEM;
 
-	for (i = 0; i < MAX_INGQ + 1; ++i)
+	/* map for msix */
+	if (get_msix_info(adap)) {
+		adap->params.offload = 0;
+		adap->params.crypto = 0;
+	}
+
+	for (i = 0; i < max_ingq + 1; ++i)
 		entries[i].entry = i;
 
 	want = s->max_ethqsets + EXTRA_VECS;
 	if (is_offload(adap)) {
-		want += s->rdmaqs + s->rdmaciqs + s->iscsiqsets	+
-			s->niscsitq;
-		/* need nchan for each possible ULD */
-		if (is_t4(adap->params.chip))
-			ofld_need = 3 * nchan;
-		else
-			ofld_need = 4 * nchan;
+		want += adap->num_ofld_uld * s->ofldqsets;
+		ofld_need = adap->num_ofld_uld * nchan;
+	}
+	if (is_pci_uld(adap)) {
+		want += adap->num_uld * s->ofldqsets;
+		uld_need = adap->num_uld * nchan;
 	}
 #ifdef CONFIG_CHELSIO_T4_DCB
 	/* For Data Center Bridging we need 8 Ethernet TX Priority Queues for
 	 * each port.
 	 */
-	need = 8 * adap->params.nports + EXTRA_VECS + ofld_need;
+	need = 8 * adap->params.nports + EXTRA_VECS + ofld_need + uld_need;
 #else
-	need = adap->params.nports + EXTRA_VECS + ofld_need;
+	need = adap->params.nports + EXTRA_VECS + ofld_need + uld_need;
 #endif
 	allocated = pci_enable_msix_range(adap->pdev, entries, need, want);
 	if (allocated < 0) {
@@ -4554,33 +4209,31 @@ static int enable_msix(struct adapter *adap)
 	 * Every group gets its minimum requirement and NIC gets top
 	 * priority for leftovers.
 	 */
-	i = allocated - EXTRA_VECS - ofld_need;
+	i = allocated - EXTRA_VECS - ofld_need - uld_need;
 	if (i < s->max_ethqsets) {
 		s->max_ethqsets = i;
 		if (i < s->ethqsets)
 			reduce_ethqs(adap, i);
 	}
-	if (is_offload(adap)) {
-		if (allocated < want) {
-			s->rdmaqs = nchan;
-			s->rdmaciqs = nchan;
+	if (is_uld(adap)) {
+		if (allocated < want)
+			s->nqs_per_uld = nchan;
+		else
+			s->nqs_per_uld = s->ofldqsets;
+	}
 
-			if (!is_t4(adap->params.chip))
-				s->niscsitq = nchan;
+	for (i = 0; i < (s->max_ethqsets + EXTRA_VECS); ++i)
+		adap->msix_info[i].vec = entries[i].vector;
+	if (is_uld(adap)) {
+		for (j = 0 ; i < allocated; ++i, j++) {
+			adap->msix_info_ulds[j].vec = entries[i].vector;
+			adap->msix_info_ulds[j].idx = i;
 		}
-
-		/* leftovers go to OFLD */
-		i = allocated - EXTRA_VECS - s->max_ethqsets -
-		    s->rdmaqs - s->rdmaciqs - s->niscsitq;
-		s->iscsiqsets = (i / nchan) * nchan;  /* round down */
-
+		adap->msix_bmap_ulds.mapsize = j;
 	}
-	for (i = 0; i < allocated; ++i)
-		adap->msix_info[i].vec = entries[i].vector;
 	dev_info(adap->pdev_dev, "%d MSI-X vectors allocated, "
-		 "nic %d iscsi %d rdma cpl %d rdma ciq %d\n",
-		 allocated, s->max_ethqsets, s->iscsiqsets, s->rdmaqs,
-		 s->rdmaciqs);
+		 "nic %d per uld %d\n",
+		 allocated, s->max_ethqsets, s->nqs_per_uld);
 
 	kfree(entries);
 	return 0;
@@ -4794,7 +4447,9 @@ static void free_some_resources(struct adapter *adapter)
 	unsigned int i;
 
 	t4_free_mem(adapter->l2t);
+	t4_cleanup_sched(adapter);
 	t4_free_mem(adapter->tids.tid_tab);
+	cxgb4_cleanup_tc_u32(adapter);
 	kfree(adapter->sge.egr_map);
 	kfree(adapter->sge.ingr_map);
 	kfree(adapter->sge.starving_fl);
@@ -4845,21 +4500,59 @@ static int get_chip_type(struct pci_dev *pdev, u32 pl_rev)
 }
 
 #ifdef CONFIG_PCI_IOV
+static void dummy_setup(struct net_device *dev)
+{
+	dev->type = ARPHRD_NONE;
+	dev->mtu = 0;
+	dev->hard_header_len = 0;
+	dev->addr_len = 0;
+	dev->tx_queue_len = 0;
+	dev->flags |= IFF_NOARP;
+	dev->priv_flags |= IFF_NO_QUEUE;
+
+	/* Initialize the device structure. */
+	dev->netdev_ops = &cxgb4_mgmt_netdev_ops;
+	dev->ethtool_ops = &cxgb4_mgmt_ethtool_ops;
+	dev->destructor = free_netdev;
+}
+
+static int config_mgmt_dev(struct pci_dev *pdev)
+{
+	struct adapter *adap = pci_get_drvdata(pdev);
+	struct net_device *netdev;
+	struct port_info *pi;
+	char name[IFNAMSIZ];
+	int err;
+
+	snprintf(name, IFNAMSIZ, "mgmtpf%d%d", adap->adap_idx, adap->pf);
+	netdev = alloc_netdev(0, name, NET_NAME_UNKNOWN, dummy_setup);
+	if (!netdev)
+		return -ENOMEM;
+
+	pi = netdev_priv(netdev);
+	pi->adapter = adap;
+	SET_NETDEV_DEV(netdev, &pdev->dev);
+
+	adap->port[0] = netdev;
+
+	err = register_netdev(adap->port[0]);
+	if (err) {
+		pr_info("Unable to register VF mgmt netdev %s\n", name);
+		free_netdev(adap->port[0]);
+		adap->port[0] = NULL;
+		return err;
+	}
+	return 0;
+}
+
 static int cxgb4_iov_configure(struct pci_dev *pdev, int num_vfs)
 {
+	struct adapter *adap = pci_get_drvdata(pdev);
 	int err = 0;
 	int current_vfs = pci_num_vf(pdev);
 	u32 pcie_fw;
-	void __iomem *regs;
-
-	regs = pci_ioremap_bar(pdev, 0);
-	if (!regs) {
-		dev_err(&pdev->dev, "cannot map device registers\n");
-		return -ENOMEM;
-	}
 
-	pcie_fw = readl(regs + PCIE_FW_A);
-	iounmap(regs);
+	pcie_fw = readl(adap->regs + PCIE_FW_A);
 	/* Check if cxgb4 is the MASTER and fw is initialized */
 	if (!(pcie_fw & PCIE_FW_INIT_F) ||
 	    !(pcie_fw & PCIE_FW_MASTER_VLD_F) ||
@@ -4886,6 +4579,14 @@ static int cxgb4_iov_configure(struct pci_dev *pdev, int num_vfs)
 	 */
 	if (!num_vfs) {
 		pci_disable_sriov(pdev);
+		if (adap->port[0]) {
+			unregister_netdev(adap->port[0]);
+			adap->port[0] = NULL;
+		}
+		/* free VF resources */
+		kfree(adap->vfinfo);
+		adap->vfinfo = NULL;
+		adap->num_vfs = 0;
 		return num_vfs;
 	}
 
@@ -4893,7 +4594,17 @@ static int cxgb4_iov_configure(struct pci_dev *pdev, int num_vfs)
 		err = pci_enable_sriov(pdev, num_vfs);
 		if (err)
 			return err;
+
+		adap->num_vfs = num_vfs;
+		err = config_mgmt_dev(pdev);
+		if (err)
+			return err;
 	}
+
+	adap->vfinfo = kcalloc(adap->num_vfs,
+			       sizeof(struct vf_info), GFP_KERNEL);
+	if (adap->vfinfo)
+		fill_vf_station_mac_addr(adap);
 	return num_vfs;
 }
 #endif
@@ -4904,9 +4615,11 @@ static int init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	struct port_info *pi;
 	bool highdma = false;
 	struct adapter *adapter = NULL;
+	struct net_device *netdev;
 	void __iomem *regs;
 	u32 whoami, pl_rev;
 	enum chip_type chip;
+	static int adap_idx = 1;
 
 	printk_once(KERN_INFO "%s - version %s\n", DRV_DESC, DRV_VERSION);
 
@@ -4941,7 +4654,9 @@ static int init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	func = CHELSIO_CHIP_VERSION(chip) <= CHELSIO_T5 ?
 		SOURCEPF_G(whoami) : T6_SOURCEPF_G(whoami);
 	if (func != ent->driver_data) {
+#ifndef CONFIG_PCI_IOV
 		iounmap(regs);
+#endif
 		pci_disable_device(pdev);
 		pci_save_state(pdev);        /* to restore SR-IOV later */
 		goto sriov;
@@ -4973,6 +4688,7 @@ static int init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 		err = -ENOMEM;
 		goto out_unmap_bar0;
 	}
+	adap_idx++;
 
 	adapter->workq = create_singlethread_workqueue("cxgb4");
 	if (!adapter->workq) {
@@ -5059,8 +4775,6 @@ static int init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 			      T6_STATMODE_V(0)));
 
 	for_each_port(adapter, i) {
-		struct net_device *netdev;
-
 		netdev = alloc_etherdev_mq(sizeof(struct port_info),
 					   MAX_ETH_QSETS);
 		if (!netdev) {
@@ -5080,7 +4794,8 @@ static int init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 		netdev->hw_features = NETIF_F_SG | TSO_FLAGS |
 			NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM |
 			NETIF_F_RXCSUM | NETIF_F_RXHASH |
-			NETIF_F_HW_VLAN_CTAG_TX | NETIF_F_HW_VLAN_CTAG_RX;
+			NETIF_F_HW_VLAN_CTAG_TX | NETIF_F_HW_VLAN_CTAG_RX |
+			NETIF_F_HW_TC;
 		if (highdma)
 			netdev->hw_features |= NETIF_F_HIGHDMA;
 		netdev->features |= netdev->hw_features;
@@ -5154,10 +4869,26 @@ static int init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 		}
 	}
 #endif
-	if (is_offload(adapter) && tid_init(&adapter->tids) < 0) {
+
+	for_each_port(adapter, i) {
+		pi = adap2pinfo(adapter, i);
+		pi->sched_tbl = t4_init_sched(adapter->params.nsched_cls);
+		if (!pi->sched_tbl)
+			dev_warn(&pdev->dev,
+				 "could not activate scheduling on port %d\n",
+				 i);
+	}
+
+	if (tid_init(&adapter->tids) < 0) {
 		dev_warn(&pdev->dev, "could not allocate TID table, "
 			 "continuing\n");
 		adapter->params.offload = 0;
+	} else {
+		adapter->tc_u32 = cxgb4_init_tc_u32(adapter,
+						    CXGB4_MAX_LINK_HANDLE);
+		if (!adapter->tc_u32)
+			dev_warn(&pdev->dev,
+				 "could not offload tc u32, continuing\n");
 	}
 
 	if (is_offload(adapter)) {
@@ -5179,8 +4910,11 @@ static int init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	/* See what interrupts we'll be using */
 	if (msi > 1 && enable_msix(adapter) == 0)
 		adapter->flags |= USING_MSIX;
-	else if (msi > 0 && pci_enable_msi(pdev) == 0)
+	else if (msi > 0 && pci_enable_msi(pdev) == 0) {
 		adapter->flags |= USING_MSI;
+		if (msi > 1)
+			free_msix_info(adapter);
+	}
 
 	/* check for PCI Express bandwidth capabiltites */
 	cxgb4_check_pcie_caps(adapter);
@@ -5224,10 +4958,15 @@ static int init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	/* PCIe EEH recovery on powerpc platforms needs fundamental reset */
 	pdev->needs_freset = 1;
 
-	if (is_offload(adapter))
-		attach_ulds(adapter);
+	if (is_uld(adapter)) {
+		mutex_lock(&uld_mutex);
+		list_add_tail(&adapter->list_node, &adapter_list);
+		mutex_unlock(&uld_mutex);
+	}
 
 	print_adapter_info(adapter);
+	setup_fw_sge_queues(adapter);
+	return 0;
 
 sriov:
 #ifdef CONFIG_PCI_IOV
@@ -5241,11 +4980,48 @@ sriov:
 				 "instantiated %u virtual functions\n",
 				 num_vf[func]);
 	}
-#endif
+
+	adapter = kzalloc(sizeof(*adapter), GFP_KERNEL);
+	if (!adapter) {
+		err = -ENOMEM;
+		goto free_pci_region;
+	}
+
+	adapter->pdev = pdev;
+	adapter->pdev_dev = &pdev->dev;
+	adapter->name = pci_name(pdev);
+	adapter->mbox = func;
+	adapter->pf = func;
+	adapter->regs = regs;
+	adapter->adap_idx = adap_idx;
+	adapter->mbox_log = kzalloc(sizeof(*adapter->mbox_log) +
+				    (sizeof(struct mbox_cmd) *
+				     T4_OS_LOG_MBOX_CMDS),
+				    GFP_KERNEL);
+	if (!adapter->mbox_log) {
+		err = -ENOMEM;
+		goto free_adapter;
+	}
+	pci_set_drvdata(pdev, adapter);
+	return 0;
+
+ free_adapter:
+	kfree(adapter);
+ free_pci_region:
+	iounmap(regs);
+	pci_disable_sriov(pdev);
+	pci_release_regions(pdev);
+	return err;
+#else
 	return 0;
+#endif
 
  out_free_dev:
 	free_some_resources(adapter);
+	if (adapter->flags & USING_MSIX)
+		free_msix_info(adapter);
+	if (adapter->num_uld || adapter->num_ofld_uld)
+		t4_uld_mem_free(adapter);
  out_unmap_bar:
 	if (!is_t4(adapter->params.chip))
 		iounmap(adapter->bar2);
@@ -5269,12 +5045,12 @@ static void remove_one(struct pci_dev *pdev)
 {
 	struct adapter *adapter = pci_get_drvdata(pdev);
 
-#ifdef CONFIG_PCI_IOV
-	pci_disable_sriov(pdev);
-
-#endif
+	if (!adapter) {
+		pci_release_regions(pdev);
+		return;
+	}
 
-	if (adapter) {
+	if (adapter->pf == 4) {
 		int i;
 
 		/* Tear down per-adapter Work Queue first since it can contain
@@ -5282,7 +5058,7 @@ static void remove_one(struct pci_dev *pdev)
 		 */
 		destroy_workqueue(adapter->workq);
 
-		if (is_offload(adapter))
+		if (is_uld(adapter))
 			detach_ulds(adapter);
 
 		disable_interrupts(adapter);
@@ -5296,17 +5072,15 @@ static void remove_one(struct pci_dev *pdev)
 		/* If we allocated filters, free up state associated with any
 		 * valid filters ...
 		 */
-		if (adapter->tids.ftid_tab) {
-			struct filter_entry *f = &adapter->tids.ftid_tab[0];
-			for (i = 0; i < (adapter->tids.nftids +
-					adapter->tids.nsftids); i++, f++)
-				if (f->valid)
-					clear_filter(adapter, f);
-		}
+		clear_all_filters(adapter);
 
 		if (adapter->flags & FULL_INIT_DONE)
 			cxgb_down(adapter);
 
+		if (adapter->flags & USING_MSIX)
+			free_msix_info(adapter);
+		if (adapter->num_uld || adapter->num_ofld_uld)
+			t4_uld_mem_free(adapter);
 		free_some_resources(adapter);
 #if IS_ENABLED(CONFIG_IPV6)
 		t4_cleanup_clip_tbl(adapter);
@@ -5323,8 +5097,64 @@ static void remove_one(struct pci_dev *pdev)
 		kfree(adapter->mbox_log);
 		synchronize_rcu();
 		kfree(adapter);
-	} else
+	}
+#ifdef CONFIG_PCI_IOV
+	else {
+		if (adapter->port[0])
+			unregister_netdev(adapter->port[0]);
+		iounmap(adapter->regs);
+		kfree(adapter->vfinfo);
+		kfree(adapter);
+		pci_disable_sriov(pdev);
+		pci_release_regions(pdev);
+	}
+#endif
+}
+
+/* "Shutdown" quiesces the device, stopping Ingress Packet and Interrupt
+ * delivery.  This is essentially a stripped down version of the PCI remove()
+ * function where we do the minimal amount of work necessary to shutdown any
+ * further activity.
+ */
+static void shutdown_one(struct pci_dev *pdev)
+{
+	struct adapter *adapter = pci_get_drvdata(pdev);
+
+	/* As with remove_one() above (see extended comment), we only want do
+	 * do cleanup on PCI Devices which went all the way through init_one()
+	 * ...
+	 */
+	if (!adapter) {
+		pci_release_regions(pdev);
+		return;
+	}
+
+	if (adapter->pf == 4) {
+		int i;
+
+		for_each_port(adapter, i)
+			if (adapter->port[i]->reg_state == NETREG_REGISTERED)
+				cxgb_close(adapter->port[i]);
+
+		t4_uld_clean_up(adapter);
+		disable_interrupts(adapter);
+		disable_msi(adapter);
+
+		t4_sge_stop(adapter);
+		if (adapter->flags & FW_OK)
+			t4_fw_bye(adapter, adapter->mbox);
+	}
+#ifdef CONFIG_PCI_IOV
+	else {
+		if (adapter->port[0])
+			unregister_netdev(adapter->port[0]);
+		iounmap(adapter->regs);
+		kfree(adapter->vfinfo);
+		kfree(adapter);
+		pci_disable_sriov(pdev);
 		pci_release_regions(pdev);
+	}
+#endif
 }
 
 static struct pci_driver cxgb4_driver = {
@@ -5332,7 +5162,7 @@ static struct pci_driver cxgb4_driver = {
 	.id_table = cxgb4_pci_tbl,
 	.probe    = init_one,
 	.remove   = remove_one,
-	.shutdown = remove_one,
+	.shutdown = shutdown_one,
 #ifdef CONFIG_PCI_IOV
 	.sriov_configure = cxgb4_iov_configure,
 #endif
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_tc_u32.c b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_tc_u32.c
new file mode 100644
index 000000000000..49d2debb334e
--- /dev/null
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_tc_u32.c
@@ -0,0 +1,483 @@
+/*
+ * This file is part of the Chelsio T4 Ethernet driver for Linux.
+ *
+ * Copyright (c) 2016 Chelsio Communications, Inc. All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * 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 <net/tc_act/tc_gact.h>
+#include <net/tc_act/tc_mirred.h>
+
+#include "cxgb4.h"
+#include "cxgb4_tc_u32_parse.h"
+#include "cxgb4_tc_u32.h"
+
+/* Fill ch_filter_specification with parsed match value/mask pair. */
+static int fill_match_fields(struct adapter *adap,
+			     struct ch_filter_specification *fs,
+			     struct tc_cls_u32_offload *cls,
+			     const struct cxgb4_match_field *entry,
+			     bool next_header)
+{
+	unsigned int i, j;
+	u32 val, mask;
+	int off, err;
+	bool found;
+
+	for (i = 0; i < cls->knode.sel->nkeys; i++) {
+		off = cls->knode.sel->keys[i].off;
+		val = cls->knode.sel->keys[i].val;
+		mask = cls->knode.sel->keys[i].mask;
+
+		if (next_header) {
+			/* For next headers, parse only keys with offmask */
+			if (!cls->knode.sel->keys[i].offmask)
+				continue;
+		} else {
+			/* For the remaining, parse only keys without offmask */
+			if (cls->knode.sel->keys[i].offmask)
+				continue;
+		}
+
+		found = false;
+
+		for (j = 0; entry[j].val; j++) {
+			if (off == entry[j].off) {
+				found = true;
+				err = entry[j].val(fs, val, mask);
+				if (err)
+					return err;
+				break;
+			}
+		}
+
+		if (!found)
+			return -EINVAL;
+	}
+
+	return 0;
+}
+
+/* Fill ch_filter_specification with parsed action. */
+static int fill_action_fields(struct adapter *adap,
+			      struct ch_filter_specification *fs,
+			      struct tc_cls_u32_offload *cls)
+{
+	unsigned int num_actions = 0;
+	const struct tc_action *a;
+	struct tcf_exts *exts;
+	LIST_HEAD(actions);
+
+	exts = cls->knode.exts;
+	if (tc_no_actions(exts))
+		return -EINVAL;
+
+	tcf_exts_to_list(exts, &actions);
+	list_for_each_entry(a, &actions, list) {
+		/* Don't allow more than one action per rule. */
+		if (num_actions)
+			return -EINVAL;
+
+		/* Drop in hardware. */
+		if (is_tcf_gact_shot(a)) {
+			fs->action = FILTER_DROP;
+			num_actions++;
+			continue;
+		}
+
+		/* Re-direct to specified port in hardware. */
+		if (is_tcf_mirred_redirect(a)) {
+			struct net_device *n_dev;
+			unsigned int i, index;
+			bool found = false;
+
+			index = tcf_mirred_ifindex(a);
+			for_each_port(adap, i) {
+				n_dev = adap->port[i];
+				if (index == n_dev->ifindex) {
+					fs->action = FILTER_SWITCH;
+					fs->eport = i;
+					found = true;
+					break;
+				}
+			}
+
+			/* Interface doesn't belong to any port of
+			 * the underlying hardware.
+			 */
+			if (!found)
+				return -EINVAL;
+
+			num_actions++;
+			continue;
+		}
+
+		/* Un-supported action. */
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+int cxgb4_config_knode(struct net_device *dev, __be16 protocol,
+		       struct tc_cls_u32_offload *cls)
+{
+	const struct cxgb4_match_field *start, *link_start = NULL;
+	struct adapter *adapter = netdev2adap(dev);
+	struct ch_filter_specification fs;
+	struct cxgb4_tc_u32_table *t;
+	struct cxgb4_link *link;
+	unsigned int filter_id;
+	u32 uhtid, link_uhtid;
+	bool is_ipv6 = false;
+	int ret;
+
+	if (!can_tc_u32_offload(dev))
+		return -EOPNOTSUPP;
+
+	if (protocol != htons(ETH_P_IP) && protocol != htons(ETH_P_IPV6))
+		return -EOPNOTSUPP;
+
+	/* Fetch the location to insert the filter. */
+	filter_id = cls->knode.handle & 0xFFFFF;
+
+	if (filter_id > adapter->tids.nftids) {
+		dev_err(adapter->pdev_dev,
+			"Location %d out of range for insertion. Max: %d\n",
+			filter_id, adapter->tids.nftids);
+		return -ERANGE;
+	}
+
+	t = adapter->tc_u32;
+	uhtid = TC_U32_USERHTID(cls->knode.handle);
+	link_uhtid = TC_U32_USERHTID(cls->knode.link_handle);
+
+	/* Ensure that uhtid is either root u32 (i.e. 0x800)
+	 * or a a valid linked bucket.
+	 */
+	if (uhtid != 0x800 && uhtid >= t->size)
+		return -EINVAL;
+
+	/* Ensure link handle uhtid is sane, if specified. */
+	if (link_uhtid >= t->size)
+		return -EINVAL;
+
+	memset(&fs, 0, sizeof(fs));
+
+	if (protocol == htons(ETH_P_IPV6)) {
+		start = cxgb4_ipv6_fields;
+		is_ipv6 = true;
+	} else {
+		start = cxgb4_ipv4_fields;
+		is_ipv6 = false;
+	}
+
+	if (uhtid != 0x800) {
+		/* Link must exist from root node before insertion. */
+		if (!t->table[uhtid - 1].link_handle)
+			return -EINVAL;
+
+		/* Link must have a valid supported next header. */
+		link_start = t->table[uhtid - 1].match_field;
+		if (!link_start)
+			return -EINVAL;
+	}
+
+	/* Parse links and record them for subsequent jumps to valid
+	 * next headers.
+	 */
+	if (link_uhtid) {
+		const struct cxgb4_next_header *next;
+		bool found = false;
+		unsigned int i, j;
+		u32 val, mask;
+		int off;
+
+		if (t->table[link_uhtid - 1].link_handle) {
+			dev_err(adapter->pdev_dev,
+				"Link handle exists for: 0x%x\n",
+				link_uhtid);
+			return -EINVAL;
+		}
+
+		next = is_ipv6 ? cxgb4_ipv6_jumps : cxgb4_ipv4_jumps;
+
+		/* Try to find matches that allow jumps to next header. */
+		for (i = 0; next[i].jump; i++) {
+			if (next[i].offoff != cls->knode.sel->offoff ||
+			    next[i].shift != cls->knode.sel->offshift ||
+			    next[i].mask != cls->knode.sel->offmask ||
+			    next[i].offset != cls->knode.sel->off)
+				continue;
+
+			/* Found a possible candidate.  Find a key that
+			 * matches the corresponding offset, value, and
+			 * mask to jump to next header.
+			 */
+			for (j = 0; j < cls->knode.sel->nkeys; j++) {
+				off = cls->knode.sel->keys[j].off;
+				val = cls->knode.sel->keys[j].val;
+				mask = cls->knode.sel->keys[j].mask;
+
+				if (next[i].match_off == off &&
+				    next[i].match_val == val &&
+				    next[i].match_mask == mask) {
+					found = true;
+					break;
+				}
+			}
+
+			if (!found)
+				continue; /* Try next candidate. */
+
+			/* Candidate to jump to next header found.
+			 * Translate all keys to internal specification
+			 * and store them in jump table. This spec is copied
+			 * later to set the actual filters.
+			 */
+			ret = fill_match_fields(adapter, &fs, cls,
+						start, false);
+			if (ret)
+				goto out;
+
+			link = &t->table[link_uhtid - 1];
+			link->match_field = next[i].jump;
+			link->link_handle = cls->knode.handle;
+			memcpy(&link->fs, &fs, sizeof(fs));
+			break;
+		}
+
+		/* No candidate found to jump to next header. */
+		if (!found)
+			return -EINVAL;
+
+		return 0;
+	}
+
+	/* Fill ch_filter_specification match fields to be shipped to hardware.
+	 * Copy the linked spec (if any) first.  And then update the spec as
+	 * needed.
+	 */
+	if (uhtid != 0x800 && t->table[uhtid - 1].link_handle) {
+		/* Copy linked ch_filter_specification */
+		memcpy(&fs, &t->table[uhtid - 1].fs, sizeof(fs));
+		ret = fill_match_fields(adapter, &fs, cls,
+					link_start, true);
+		if (ret)
+			goto out;
+	}
+
+	ret = fill_match_fields(adapter, &fs, cls, start, false);
+	if (ret)
+		goto out;
+
+	/* Fill ch_filter_specification action fields to be shipped to
+	 * hardware.
+	 */
+	ret = fill_action_fields(adapter, &fs, cls);
+	if (ret)
+		goto out;
+
+	/* The filter spec has been completely built from the info
+	 * provided from u32.  We now set some default fields in the
+	 * spec for sanity.
+	 */
+
+	/* Match only packets coming from the ingress port where this
+	 * filter will be created.
+	 */
+	fs.val.iport = netdev2pinfo(dev)->port_id;
+	fs.mask.iport = ~0;
+
+	/* Enable filter hit counts. */
+	fs.hitcnts = 1;
+
+	/* Set type of filter - IPv6 or IPv4 */
+	fs.type = is_ipv6 ? 1 : 0;
+
+	/* Set the filter */
+	ret = cxgb4_set_filter(dev, filter_id, &fs);
+	if (ret)
+		goto out;
+
+	/* If this is a linked bucket, then set the corresponding
+	 * entry in the bitmap to mark it as belonging to this linked
+	 * bucket.
+	 */
+	if (uhtid != 0x800 && t->table[uhtid - 1].link_handle)
+		set_bit(filter_id, t->table[uhtid - 1].tid_map);
+
+out:
+	return ret;
+}
+
+int cxgb4_delete_knode(struct net_device *dev, __be16 protocol,
+		       struct tc_cls_u32_offload *cls)
+{
+	struct adapter *adapter = netdev2adap(dev);
+	unsigned int filter_id, max_tids, i, j;
+	struct cxgb4_link *link = NULL;
+	struct cxgb4_tc_u32_table *t;
+	u32 handle, uhtid;
+	int ret;
+
+	if (!can_tc_u32_offload(dev))
+		return -EOPNOTSUPP;
+
+	/* Fetch the location to delete the filter. */
+	filter_id = cls->knode.handle & 0xFFFFF;
+
+	if (filter_id > adapter->tids.nftids) {
+		dev_err(adapter->pdev_dev,
+			"Location %d out of range for deletion. Max: %d\n",
+			filter_id, adapter->tids.nftids);
+		return -ERANGE;
+	}
+
+	t = adapter->tc_u32;
+	handle = cls->knode.handle;
+	uhtid = TC_U32_USERHTID(cls->knode.handle);
+
+	/* Ensure that uhtid is either root u32 (i.e. 0x800)
+	 * or a a valid linked bucket.
+	 */
+	if (uhtid != 0x800 && uhtid >= t->size)
+		return -EINVAL;
+
+	/* Delete the specified filter */
+	if (uhtid != 0x800) {
+		link = &t->table[uhtid - 1];
+		if (!link->link_handle)
+			return -EINVAL;
+
+		if (!test_bit(filter_id, link->tid_map))
+			return -EINVAL;
+	}
+
+	ret = cxgb4_del_filter(dev, filter_id);
+	if (ret)
+		goto out;
+
+	if (link)
+		clear_bit(filter_id, link->tid_map);
+
+	/* If a link is being deleted, then delete all filters
+	 * associated with the link.
+	 */
+	max_tids = adapter->tids.nftids;
+	for (i = 0; i < t->size; i++) {
+		link = &t->table[i];
+
+		if (link->link_handle == handle) {
+			for (j = 0; j < max_tids; j++) {
+				if (!test_bit(j, link->tid_map))
+					continue;
+
+				ret = __cxgb4_del_filter(dev, j, NULL);
+				if (ret)
+					goto out;
+
+				clear_bit(j, link->tid_map);
+			}
+
+			/* Clear the link state */
+			link->match_field = NULL;
+			link->link_handle = 0;
+			memset(&link->fs, 0, sizeof(link->fs));
+			break;
+		}
+	}
+
+out:
+	return ret;
+}
+
+void cxgb4_cleanup_tc_u32(struct adapter *adap)
+{
+	struct cxgb4_tc_u32_table *t;
+	unsigned int i;
+
+	if (!adap->tc_u32)
+		return;
+
+	/* Free up all allocated memory. */
+	t = adap->tc_u32;
+	for (i = 0; i < t->size; i++) {
+		struct cxgb4_link *link = &t->table[i];
+
+		t4_free_mem(link->tid_map);
+	}
+	t4_free_mem(adap->tc_u32);
+}
+
+struct cxgb4_tc_u32_table *cxgb4_init_tc_u32(struct adapter *adap,
+					     unsigned int size)
+{
+	struct cxgb4_tc_u32_table *t;
+	unsigned int i;
+
+	if (!size)
+		return NULL;
+
+	t = t4_alloc_mem(sizeof(*t) +
+			 (size * sizeof(struct cxgb4_link)));
+	if (!t)
+		return NULL;
+
+	t->size = size;
+
+	for (i = 0; i < t->size; i++) {
+		struct cxgb4_link *link = &t->table[i];
+		unsigned int bmap_size;
+		unsigned int max_tids;
+
+		max_tids = adap->tids.nftids;
+		bmap_size = BITS_TO_LONGS(max_tids);
+		link->tid_map = t4_alloc_mem(sizeof(unsigned long) * bmap_size);
+		if (!link->tid_map)
+			goto out_no_mem;
+		bitmap_zero(link->tid_map, max_tids);
+	}
+
+	return t;
+
+out_no_mem:
+	for (i = 0; i < t->size; i++) {
+		struct cxgb4_link *link = &t->table[i];
+
+		if (link->tid_map)
+			t4_free_mem(link->tid_map);
+	}
+
+	if (t)
+		t4_free_mem(t);
+
+	return NULL;
+}
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_tc_u32.h b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_tc_u32.h
new file mode 100644
index 000000000000..6bdc885eff22
--- /dev/null
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_tc_u32.h
@@ -0,0 +1,57 @@
+/*
+ * This file is part of the Chelsio T4 Ethernet driver for Linux.
+ *
+ * Copyright (c) 2016 Chelsio Communications, Inc. All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * 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 __CXGB4_TC_U32_H
+#define __CXGB4_TC_U32_H
+
+#include <net/pkt_cls.h>
+
+#define CXGB4_MAX_LINK_HANDLE 32
+
+static inline bool can_tc_u32_offload(struct net_device *dev)
+{
+	struct adapter *adap = netdev2adap(dev);
+
+	return (dev->features & NETIF_F_HW_TC) && adap->tc_u32 ? true : false;
+}
+
+int cxgb4_config_knode(struct net_device *dev, __be16 protocol,
+		       struct tc_cls_u32_offload *cls);
+int cxgb4_delete_knode(struct net_device *dev, __be16 protocol,
+		       struct tc_cls_u32_offload *cls);
+
+void cxgb4_cleanup_tc_u32(struct adapter *adapter);
+struct cxgb4_tc_u32_table *cxgb4_init_tc_u32(struct adapter *adap,
+					     unsigned int size);
+#endif /* __CXGB4_TC_U32_H */
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_tc_u32_parse.h b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_tc_u32_parse.h
new file mode 100644
index 000000000000..a4b99edcc339
--- /dev/null
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_tc_u32_parse.h
@@ -0,0 +1,294 @@
+/*
+ * This file is part of the Chelsio T4 Ethernet driver for Linux.
+ *
+ * Copyright (c) 2016 Chelsio Communications, Inc. All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * 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 __CXGB4_TC_U32_PARSE_H
+#define __CXGB4_TC_U32_PARSE_H
+
+struct cxgb4_match_field {
+	int off; /* Offset from the beginning of the header to match */
+	/* Fill the value/mask pair in the spec if matched */
+	int (*val)(struct ch_filter_specification *f, u32 val, u32 mask);
+};
+
+/* IPv4 match fields */
+static inline int cxgb4_fill_ipv4_tos(struct ch_filter_specification *f,
+				      u32 val, u32 mask)
+{
+	f->val.tos  = (ntohl(val)  >> 16) & 0x000000FF;
+	f->mask.tos = (ntohl(mask) >> 16) & 0x000000FF;
+
+	return 0;
+}
+
+static inline int cxgb4_fill_ipv4_frag(struct ch_filter_specification *f,
+				       u32 val, u32 mask)
+{
+	u32 mask_val;
+	u8 frag_val;
+
+	frag_val = (ntohl(val) >> 13) & 0x00000007;
+	mask_val = ntohl(mask) & 0x0000FFFF;
+
+	if (frag_val == 0x1 && mask_val != 0x3FFF) { /* MF set */
+		f->val.frag = 1;
+		f->mask.frag = 1;
+	} else if (frag_val == 0x2 && mask_val != 0x3FFF) { /* DF set */
+		f->val.frag = 0;
+		f->mask.frag = 1;
+	} else {
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static inline int cxgb4_fill_ipv4_proto(struct ch_filter_specification *f,
+					u32 val, u32 mask)
+{
+	f->val.proto  = (ntohl(val)  >> 16) & 0x000000FF;
+	f->mask.proto = (ntohl(mask) >> 16) & 0x000000FF;
+
+	return 0;
+}
+
+static inline int cxgb4_fill_ipv4_src_ip(struct ch_filter_specification *f,
+					 u32 val, u32 mask)
+{
+	memcpy(&f->val.fip[0],  &val,  sizeof(u32));
+	memcpy(&f->mask.fip[0], &mask, sizeof(u32));
+
+	return 0;
+}
+
+static inline int cxgb4_fill_ipv4_dst_ip(struct ch_filter_specification *f,
+					 u32 val, u32 mask)
+{
+	memcpy(&f->val.lip[0],  &val,  sizeof(u32));
+	memcpy(&f->mask.lip[0], &mask, sizeof(u32));
+
+	return 0;
+}
+
+static const struct cxgb4_match_field cxgb4_ipv4_fields[] = {
+	{ .off = 0,  .val = cxgb4_fill_ipv4_tos },
+	{ .off = 4,  .val = cxgb4_fill_ipv4_frag },
+	{ .off = 8,  .val = cxgb4_fill_ipv4_proto },
+	{ .off = 12, .val = cxgb4_fill_ipv4_src_ip },
+	{ .off = 16, .val = cxgb4_fill_ipv4_dst_ip },
+	{ .val = NULL }
+};
+
+/* IPv6 match fields */
+static inline int cxgb4_fill_ipv6_tos(struct ch_filter_specification *f,
+				      u32 val, u32 mask)
+{
+	f->val.tos  = (ntohl(val)  >> 20) & 0x000000FF;
+	f->mask.tos = (ntohl(mask) >> 20) & 0x000000FF;
+
+	return 0;
+}
+
+static inline int cxgb4_fill_ipv6_proto(struct ch_filter_specification *f,
+					u32 val, u32 mask)
+{
+	f->val.proto  = (ntohl(val)  >> 8) & 0x000000FF;
+	f->mask.proto = (ntohl(mask) >> 8) & 0x000000FF;
+
+	return 0;
+}
+
+static inline int cxgb4_fill_ipv6_src_ip0(struct ch_filter_specification *f,
+					  u32 val, u32 mask)
+{
+	memcpy(&f->val.fip[0],  &val,  sizeof(u32));
+	memcpy(&f->mask.fip[0], &mask, sizeof(u32));
+
+	return 0;
+}
+
+static inline int cxgb4_fill_ipv6_src_ip1(struct ch_filter_specification *f,
+					  u32 val, u32 mask)
+{
+	memcpy(&f->val.fip[4],  &val,  sizeof(u32));
+	memcpy(&f->mask.fip[4], &mask, sizeof(u32));
+
+	return 0;
+}
+
+static inline int cxgb4_fill_ipv6_src_ip2(struct ch_filter_specification *f,
+					  u32 val, u32 mask)
+{
+	memcpy(&f->val.fip[8],  &val,  sizeof(u32));
+	memcpy(&f->mask.fip[8], &mask, sizeof(u32));
+
+	return 0;
+}
+
+static inline int cxgb4_fill_ipv6_src_ip3(struct ch_filter_specification *f,
+					  u32 val, u32 mask)
+{
+	memcpy(&f->val.fip[12],  &val,  sizeof(u32));
+	memcpy(&f->mask.fip[12], &mask, sizeof(u32));
+
+	return 0;
+}
+
+static inline int cxgb4_fill_ipv6_dst_ip0(struct ch_filter_specification *f,
+					  u32 val, u32 mask)
+{
+	memcpy(&f->val.lip[0],  &val,  sizeof(u32));
+	memcpy(&f->mask.lip[0], &mask, sizeof(u32));
+
+	return 0;
+}
+
+static inline int cxgb4_fill_ipv6_dst_ip1(struct ch_filter_specification *f,
+					  u32 val, u32 mask)
+{
+	memcpy(&f->val.lip[4],  &val,  sizeof(u32));
+	memcpy(&f->mask.lip[4], &mask, sizeof(u32));
+
+	return 0;
+}
+
+static inline int cxgb4_fill_ipv6_dst_ip2(struct ch_filter_specification *f,
+					  u32 val, u32 mask)
+{
+	memcpy(&f->val.lip[8],  &val,  sizeof(u32));
+	memcpy(&f->mask.lip[8], &mask, sizeof(u32));
+
+	return 0;
+}
+
+static inline int cxgb4_fill_ipv6_dst_ip3(struct ch_filter_specification *f,
+					  u32 val, u32 mask)
+{
+	memcpy(&f->val.lip[12],  &val,  sizeof(u32));
+	memcpy(&f->mask.lip[12], &mask, sizeof(u32));
+
+	return 0;
+}
+
+static const struct cxgb4_match_field cxgb4_ipv6_fields[] = {
+	{ .off = 0,  .val = cxgb4_fill_ipv6_tos },
+	{ .off = 4,  .val = cxgb4_fill_ipv6_proto },
+	{ .off = 8,  .val = cxgb4_fill_ipv6_src_ip0 },
+	{ .off = 12, .val = cxgb4_fill_ipv6_src_ip1 },
+	{ .off = 16, .val = cxgb4_fill_ipv6_src_ip2 },
+	{ .off = 20, .val = cxgb4_fill_ipv6_src_ip3 },
+	{ .off = 24, .val = cxgb4_fill_ipv6_dst_ip0 },
+	{ .off = 28, .val = cxgb4_fill_ipv6_dst_ip1 },
+	{ .off = 32, .val = cxgb4_fill_ipv6_dst_ip2 },
+	{ .off = 36, .val = cxgb4_fill_ipv6_dst_ip3 },
+	{ .val = NULL }
+};
+
+/* TCP/UDP match */
+static inline int cxgb4_fill_l4_ports(struct ch_filter_specification *f,
+				      u32 val, u32 mask)
+{
+	f->val.fport  = ntohl(val)  >> 16;
+	f->mask.fport = ntohl(mask) >> 16;
+	f->val.lport  = ntohl(val)  & 0x0000FFFF;
+	f->mask.lport = ntohl(mask) & 0x0000FFFF;
+
+	return 0;
+};
+
+static const struct cxgb4_match_field cxgb4_tcp_fields[] = {
+	{ .off = 0, .val = cxgb4_fill_l4_ports },
+	{ .val = NULL }
+};
+
+static const struct cxgb4_match_field cxgb4_udp_fields[] = {
+	{ .off = 0, .val = cxgb4_fill_l4_ports },
+	{ .val = NULL }
+};
+
+struct cxgb4_next_header {
+	unsigned int offset; /* Offset to next header */
+	/* offset, shift, and mask added to offset above
+	 * to get to next header.  Useful when using a header
+	 * field's value to jump to next header such as IHL field
+	 * in IPv4 header.
+	 */
+	unsigned int offoff;
+	u32 shift;
+	u32 mask;
+	/* match criteria to make this jump */
+	unsigned int match_off;
+	u32 match_val;
+	u32 match_mask;
+	/* location of jump to make */
+	const struct cxgb4_match_field *jump;
+};
+
+/* Accept a rule with a jump to transport layer header based on IHL field in
+ * IPv4 header.
+ */
+static const struct cxgb4_next_header cxgb4_ipv4_jumps[] = {
+	{ .offset = 0, .offoff = 0, .shift = 6, .mask = 0xF,
+	  .match_off = 8, .match_val = 0x600, .match_mask = 0xFF00,
+	  .jump = cxgb4_tcp_fields },
+	{ .offset = 0, .offoff = 0, .shift = 6, .mask = 0xF,
+	  .match_off = 8, .match_val = 0x1100, .match_mask = 0xFF00,
+	  .jump = cxgb4_udp_fields },
+	{ .jump = NULL }
+};
+
+/* Accept a rule with a jump directly past the 40 Bytes of IPv6 fixed header
+ * to get to transport layer header.
+ */
+static const struct cxgb4_next_header cxgb4_ipv6_jumps[] = {
+	{ .offset = 0x28, .offoff = 0, .shift = 0, .mask = 0,
+	  .match_off = 4, .match_val = 0x60000, .match_mask = 0xFF0000,
+	  .jump = cxgb4_tcp_fields },
+	{ .offset = 0x28, .offoff = 0, .shift = 0, .mask = 0,
+	  .match_off = 4, .match_val = 0x110000, .match_mask = 0xFF0000,
+	  .jump = cxgb4_udp_fields },
+	{ .jump = NULL }
+};
+
+struct cxgb4_link {
+	const struct cxgb4_match_field *match_field;  /* Next header */
+	struct ch_filter_specification fs; /* Match spec associated with link */
+	u32 link_handle;         /* Knode handle associated with the link */
+	unsigned long *tid_map;  /* Bitmap for filter tids */
+};
+
+struct cxgb4_tc_u32_table {
+	unsigned int size;          /* number of entries in table */
+	struct cxgb4_link table[0]; /* Jump table */
+};
+#endif /* __CXGB4_TC_U32_PARSE_H */
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_uld.c b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_uld.c
new file mode 100644
index 000000000000..0945fa49a5dd
--- /dev/null
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_uld.c
@@ -0,0 +1,697 @@
+/*
+ * cxgb4_uld.c:Chelsio Upper Layer Driver Interface for T4/T5/T6 SGE management
+ *
+ * Copyright (c) 2016 Chelsio Communications, Inc. All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * 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.
+ *
+ *  Written by: Atul Gupta (atul.gupta@chelsio.com)
+ *  Written by: Hariprasad Shenai (hariprasad@chelsio.com)
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/errno.h>
+#include <linux/types.h>
+#include <linux/debugfs.h>
+#include <linux/export.h>
+#include <linux/list.h>
+#include <linux/skbuff.h>
+#include <linux/pci.h>
+
+#include "cxgb4.h"
+#include "cxgb4_uld.h"
+#include "t4_regs.h"
+#include "t4fw_api.h"
+#include "t4_msg.h"
+
+#define for_each_uldrxq(m, i) for (i = 0; i < ((m)->nrxq + (m)->nciq); i++)
+
+static int get_msix_idx_from_bmap(struct adapter *adap)
+{
+	struct uld_msix_bmap *bmap = &adap->msix_bmap_ulds;
+	unsigned long flags;
+	unsigned int msix_idx;
+
+	spin_lock_irqsave(&bmap->lock, flags);
+	msix_idx = find_first_zero_bit(bmap->msix_bmap, bmap->mapsize);
+	if (msix_idx < bmap->mapsize) {
+		__set_bit(msix_idx, bmap->msix_bmap);
+	} else {
+		spin_unlock_irqrestore(&bmap->lock, flags);
+		return -ENOSPC;
+	}
+
+	spin_unlock_irqrestore(&bmap->lock, flags);
+	return msix_idx;
+}
+
+static void free_msix_idx_in_bmap(struct adapter *adap, unsigned int msix_idx)
+{
+	struct uld_msix_bmap *bmap = &adap->msix_bmap_ulds;
+	unsigned long flags;
+
+	spin_lock_irqsave(&bmap->lock, flags);
+	 __clear_bit(msix_idx, bmap->msix_bmap);
+	spin_unlock_irqrestore(&bmap->lock, flags);
+}
+
+/* Flush the aggregated lro sessions */
+static void uldrx_flush_handler(struct sge_rspq *q)
+{
+	struct adapter *adap = q->adap;
+
+	if (adap->uld[q->uld].lro_flush)
+		adap->uld[q->uld].lro_flush(&q->lro_mgr);
+}
+
+/**
+ *	uldrx_handler - response queue handler for ULD queues
+ *	@q: the response queue that received the packet
+ *	@rsp: the response queue descriptor holding the offload message
+ *	@gl: the gather list of packet fragments
+ *
+ *	Deliver an ingress offload packet to a ULD.  All processing is done by
+ *	the ULD, we just maintain statistics.
+ */
+static int uldrx_handler(struct sge_rspq *q, const __be64 *rsp,
+			 const struct pkt_gl *gl)
+{
+	struct adapter *adap = q->adap;
+	struct sge_ofld_rxq *rxq = container_of(q, struct sge_ofld_rxq, rspq);
+	int ret;
+
+	/* FW can send CPLs encapsulated in a CPL_FW4_MSG */
+	if (((const struct rss_header *)rsp)->opcode == CPL_FW4_MSG &&
+	    ((const struct cpl_fw4_msg *)(rsp + 1))->type == FW_TYPE_RSSCPL)
+		rsp += 2;
+
+	if (q->flush_handler)
+		ret = adap->uld[q->uld].lro_rx_handler(adap->uld[q->uld].handle,
+				rsp, gl, &q->lro_mgr,
+				&q->napi);
+	else
+		ret = adap->uld[q->uld].rx_handler(adap->uld[q->uld].handle,
+				rsp, gl);
+
+	if (ret) {
+		rxq->stats.nomem++;
+		return -1;
+	}
+
+	if (!gl)
+		rxq->stats.imm++;
+	else if (gl == CXGB4_MSG_AN)
+		rxq->stats.an++;
+	else
+		rxq->stats.pkts++;
+	return 0;
+}
+
+static int alloc_uld_rxqs(struct adapter *adap,
+			  struct sge_uld_rxq_info *rxq_info,
+			  unsigned int nq, unsigned int offset, bool lro)
+{
+	struct sge *s = &adap->sge;
+	struct sge_ofld_rxq *q = rxq_info->uldrxq + offset;
+	unsigned short *ids = rxq_info->rspq_id + offset;
+	unsigned int per_chan = nq / adap->params.nports;
+	unsigned int bmap_idx = 0;
+	int i, err, msi_idx;
+
+	if (adap->flags & USING_MSIX)
+		msi_idx = 1;
+	else
+		msi_idx = -((int)s->intrq.abs_id + 1);
+
+	for (i = 0; i < nq; i++, q++) {
+		if (msi_idx >= 0) {
+			bmap_idx = get_msix_idx_from_bmap(adap);
+			msi_idx = adap->msix_info_ulds[bmap_idx].idx;
+		}
+		err = t4_sge_alloc_rxq(adap, &q->rspq, false,
+				       adap->port[i / per_chan],
+				       msi_idx,
+				       q->fl.size ? &q->fl : NULL,
+				       uldrx_handler,
+				       lro ? uldrx_flush_handler : NULL,
+				       0);
+		if (err)
+			goto freeout;
+		if (msi_idx >= 0)
+			rxq_info->msix_tbl[i + offset] = bmap_idx;
+		memset(&q->stats, 0, sizeof(q->stats));
+		if (ids)
+			ids[i] = q->rspq.abs_id;
+	}
+	return 0;
+freeout:
+	q = rxq_info->uldrxq + offset;
+	for ( ; i; i--, q++) {
+		if (q->rspq.desc)
+			free_rspq_fl(adap, &q->rspq,
+				     q->fl.size ? &q->fl : NULL);
+	}
+
+	/* We need to free rxq also in case of ciq allocation failure */
+	if (offset) {
+		q = rxq_info->uldrxq + offset;
+		for ( ; i; i--, q++) {
+			if (q->rspq.desc)
+				free_rspq_fl(adap, &q->rspq,
+					     q->fl.size ? &q->fl : NULL);
+		}
+	}
+	return err;
+}
+
+static int
+setup_sge_queues_uld(struct adapter *adap, unsigned int uld_type, bool lro)
+{
+	struct sge_uld_rxq_info *rxq_info = adap->sge.uld_rxq_info[uld_type];
+	int i, ret = 0;
+
+	if (adap->flags & USING_MSIX) {
+		rxq_info->msix_tbl = kcalloc((rxq_info->nrxq + rxq_info->nciq),
+					     sizeof(unsigned short),
+					     GFP_KERNEL);
+		if (!rxq_info->msix_tbl)
+			return -ENOMEM;
+	}
+
+	ret = !(!alloc_uld_rxqs(adap, rxq_info, rxq_info->nrxq, 0, lro) &&
+		 !alloc_uld_rxqs(adap, rxq_info, rxq_info->nciq,
+				 rxq_info->nrxq, lro));
+
+	/* Tell uP to route control queue completions to rdma rspq */
+	if (adap->flags & FULL_INIT_DONE &&
+	    !ret && uld_type == CXGB4_ULD_RDMA) {
+		struct sge *s = &adap->sge;
+		unsigned int cmplqid;
+		u32 param, cmdop;
+
+		cmdop = FW_PARAMS_PARAM_DMAQ_EQ_CMPLIQID_CTRL;
+		for_each_port(adap, i) {
+			cmplqid = rxq_info->uldrxq[i].rspq.cntxt_id;
+			param = (FW_PARAMS_MNEM_V(FW_PARAMS_MNEM_DMAQ) |
+				 FW_PARAMS_PARAM_X_V(cmdop) |
+				 FW_PARAMS_PARAM_YZ_V(s->ctrlq[i].q.cntxt_id));
+			ret = t4_set_params(adap, adap->mbox, adap->pf,
+					    0, 1, &param, &cmplqid);
+		}
+	}
+	return ret;
+}
+
+static void t4_free_uld_rxqs(struct adapter *adap, int n,
+			     struct sge_ofld_rxq *q)
+{
+	for ( ; n; n--, q++) {
+		if (q->rspq.desc)
+			free_rspq_fl(adap, &q->rspq,
+				     q->fl.size ? &q->fl : NULL);
+	}
+}
+
+static void free_sge_queues_uld(struct adapter *adap, unsigned int uld_type)
+{
+	struct sge_uld_rxq_info *rxq_info = adap->sge.uld_rxq_info[uld_type];
+
+	if (adap->flags & FULL_INIT_DONE && uld_type == CXGB4_ULD_RDMA) {
+		struct sge *s = &adap->sge;
+		u32 param, cmdop, cmplqid = 0;
+		int i;
+
+		cmdop = FW_PARAMS_PARAM_DMAQ_EQ_CMPLIQID_CTRL;
+		for_each_port(adap, i) {
+			param = (FW_PARAMS_MNEM_V(FW_PARAMS_MNEM_DMAQ) |
+				 FW_PARAMS_PARAM_X_V(cmdop) |
+				 FW_PARAMS_PARAM_YZ_V(s->ctrlq[i].q.cntxt_id));
+			t4_set_params(adap, adap->mbox, adap->pf,
+				      0, 1, &param, &cmplqid);
+		}
+	}
+
+	if (rxq_info->nciq)
+		t4_free_uld_rxqs(adap, rxq_info->nciq,
+				 rxq_info->uldrxq + rxq_info->nrxq);
+	t4_free_uld_rxqs(adap, rxq_info->nrxq, rxq_info->uldrxq);
+	if (adap->flags & USING_MSIX)
+		kfree(rxq_info->msix_tbl);
+}
+
+static int cfg_queues_uld(struct adapter *adap, unsigned int uld_type,
+			  const struct cxgb4_uld_info *uld_info)
+{
+	struct sge *s = &adap->sge;
+	struct sge_uld_rxq_info *rxq_info;
+	int i, nrxq, ciq_size;
+
+	rxq_info = kzalloc(sizeof(*rxq_info), GFP_KERNEL);
+	if (!rxq_info)
+		return -ENOMEM;
+
+	if (adap->flags & USING_MSIX && uld_info->nrxq > s->nqs_per_uld) {
+		i = s->nqs_per_uld;
+		rxq_info->nrxq = roundup(i, adap->params.nports);
+	} else {
+		i = min_t(int, uld_info->nrxq,
+			  num_online_cpus());
+		rxq_info->nrxq = roundup(i, adap->params.nports);
+	}
+	if (!uld_info->ciq) {
+		rxq_info->nciq = 0;
+	} else  {
+		if (adap->flags & USING_MSIX)
+			rxq_info->nciq = min_t(int, s->nqs_per_uld,
+					       num_online_cpus());
+		else
+			rxq_info->nciq = min_t(int, MAX_OFLD_QSETS,
+					       num_online_cpus());
+		rxq_info->nciq = ((rxq_info->nciq / adap->params.nports) *
+				  adap->params.nports);
+		rxq_info->nciq = max_t(int, rxq_info->nciq,
+				       adap->params.nports);
+	}
+
+	nrxq = rxq_info->nrxq + rxq_info->nciq; /* total rxq's */
+	rxq_info->uldrxq = kcalloc(nrxq, sizeof(struct sge_ofld_rxq),
+				   GFP_KERNEL);
+	if (!rxq_info->uldrxq) {
+		kfree(rxq_info);
+		return -ENOMEM;
+	}
+
+	rxq_info->rspq_id = kcalloc(nrxq, sizeof(unsigned short), GFP_KERNEL);
+	if (!rxq_info->rspq_id) {
+		kfree(rxq_info->uldrxq);
+		kfree(rxq_info);
+		return -ENOMEM;
+	}
+
+	for (i = 0; i < rxq_info->nrxq; i++) {
+		struct sge_ofld_rxq *r = &rxq_info->uldrxq[i];
+
+		init_rspq(adap, &r->rspq, 5, 1, uld_info->rxq_size, 64);
+		r->rspq.uld = uld_type;
+		r->fl.size = 72;
+	}
+
+	ciq_size = 64 + adap->vres.cq.size + adap->tids.nftids;
+	if (ciq_size > SGE_MAX_IQ_SIZE) {
+		dev_warn(adap->pdev_dev, "CIQ size too small for available IQs\n");
+		ciq_size = SGE_MAX_IQ_SIZE;
+	}
+
+	for (i = rxq_info->nrxq; i < nrxq; i++) {
+		struct sge_ofld_rxq *r = &rxq_info->uldrxq[i];
+
+		init_rspq(adap, &r->rspq, 5, 1, ciq_size, 64);
+		r->rspq.uld = uld_type;
+	}
+
+	memcpy(rxq_info->name, uld_info->name, IFNAMSIZ);
+	adap->sge.uld_rxq_info[uld_type] = rxq_info;
+
+	return 0;
+}
+
+static void free_queues_uld(struct adapter *adap, unsigned int uld_type)
+{
+	struct sge_uld_rxq_info *rxq_info = adap->sge.uld_rxq_info[uld_type];
+
+	kfree(rxq_info->rspq_id);
+	kfree(rxq_info->uldrxq);
+	kfree(rxq_info);
+}
+
+static int
+request_msix_queue_irqs_uld(struct adapter *adap, unsigned int uld_type)
+{
+	struct sge_uld_rxq_info *rxq_info = adap->sge.uld_rxq_info[uld_type];
+	int err = 0;
+	unsigned int idx, bmap_idx;
+
+	for_each_uldrxq(rxq_info, idx) {
+		bmap_idx = rxq_info->msix_tbl[idx];
+		err = request_irq(adap->msix_info_ulds[bmap_idx].vec,
+				  t4_sge_intr_msix, 0,
+				  adap->msix_info_ulds[bmap_idx].desc,
+				  &rxq_info->uldrxq[idx].rspq);
+		if (err)
+			goto unwind;
+	}
+	return 0;
+unwind:
+	while (idx-- > 0) {
+		bmap_idx = rxq_info->msix_tbl[idx];
+		free_msix_idx_in_bmap(adap, bmap_idx);
+		free_irq(adap->msix_info_ulds[bmap_idx].vec,
+			 &rxq_info->uldrxq[idx].rspq);
+	}
+	return err;
+}
+
+static void
+free_msix_queue_irqs_uld(struct adapter *adap, unsigned int uld_type)
+{
+	struct sge_uld_rxq_info *rxq_info = adap->sge.uld_rxq_info[uld_type];
+	unsigned int idx, bmap_idx;
+
+	for_each_uldrxq(rxq_info, idx) {
+		bmap_idx = rxq_info->msix_tbl[idx];
+
+		free_msix_idx_in_bmap(adap, bmap_idx);
+		free_irq(adap->msix_info_ulds[bmap_idx].vec,
+			 &rxq_info->uldrxq[idx].rspq);
+	}
+}
+
+static void name_msix_vecs_uld(struct adapter *adap, unsigned int uld_type)
+{
+	struct sge_uld_rxq_info *rxq_info = adap->sge.uld_rxq_info[uld_type];
+	int n = sizeof(adap->msix_info_ulds[0].desc);
+	unsigned int idx, bmap_idx;
+
+	for_each_uldrxq(rxq_info, idx) {
+		bmap_idx = rxq_info->msix_tbl[idx];
+
+		snprintf(adap->msix_info_ulds[bmap_idx].desc, n, "%s-%s%d",
+			 adap->port[0]->name, rxq_info->name, idx);
+	}
+}
+
+static void enable_rx(struct adapter *adap, struct sge_rspq *q)
+{
+	if (!q)
+		return;
+
+	if (q->handler) {
+		cxgb_busy_poll_init_lock(q);
+		napi_enable(&q->napi);
+	}
+	/* 0-increment GTS to start the timer and enable interrupts */
+	t4_write_reg(adap, MYPF_REG(SGE_PF_GTS_A),
+		     SEINTARM_V(q->intr_params) |
+		     INGRESSQID_V(q->cntxt_id));
+}
+
+static void quiesce_rx(struct adapter *adap, struct sge_rspq *q)
+{
+	if (q && q->handler) {
+		napi_disable(&q->napi);
+		local_bh_disable();
+		while (!cxgb_poll_lock_napi(q))
+			mdelay(1);
+		local_bh_enable();
+	}
+}
+
+static void enable_rx_uld(struct adapter *adap, unsigned int uld_type)
+{
+	struct sge_uld_rxq_info *rxq_info = adap->sge.uld_rxq_info[uld_type];
+	int idx;
+
+	for_each_uldrxq(rxq_info, idx)
+		enable_rx(adap, &rxq_info->uldrxq[idx].rspq);
+}
+
+static void quiesce_rx_uld(struct adapter *adap, unsigned int uld_type)
+{
+	struct sge_uld_rxq_info *rxq_info = adap->sge.uld_rxq_info[uld_type];
+	int idx;
+
+	for_each_uldrxq(rxq_info, idx)
+		quiesce_rx(adap, &rxq_info->uldrxq[idx].rspq);
+}
+
+static void uld_queue_init(struct adapter *adap, unsigned int uld_type,
+			   struct cxgb4_lld_info *lli)
+{
+	struct sge_uld_rxq_info *rxq_info = adap->sge.uld_rxq_info[uld_type];
+
+	lli->rxq_ids = rxq_info->rspq_id;
+	lli->nrxq = rxq_info->nrxq;
+	lli->ciq_ids = rxq_info->rspq_id + rxq_info->nrxq;
+	lli->nciq = rxq_info->nciq;
+}
+
+int t4_uld_mem_alloc(struct adapter *adap)
+{
+	struct sge *s = &adap->sge;
+
+	adap->uld = kcalloc(CXGB4_ULD_MAX, sizeof(*adap->uld), GFP_KERNEL);
+	if (!adap->uld)
+		return -ENOMEM;
+
+	s->uld_rxq_info = kzalloc(CXGB4_ULD_MAX *
+				  sizeof(struct sge_uld_rxq_info *),
+				  GFP_KERNEL);
+	if (!s->uld_rxq_info)
+		goto err_uld;
+
+	return 0;
+err_uld:
+	kfree(adap->uld);
+	return -ENOMEM;
+}
+
+void t4_uld_mem_free(struct adapter *adap)
+{
+	struct sge *s = &adap->sge;
+
+	kfree(s->uld_rxq_info);
+	kfree(adap->uld);
+}
+
+void t4_uld_clean_up(struct adapter *adap)
+{
+	struct sge_uld_rxq_info *rxq_info;
+	unsigned int i;
+
+	if (!adap->uld)
+		return;
+	for (i = 0; i < CXGB4_ULD_MAX; i++) {
+		if (!adap->uld[i].handle)
+			continue;
+		rxq_info = adap->sge.uld_rxq_info[i];
+		if (adap->flags & FULL_INIT_DONE)
+			quiesce_rx_uld(adap, i);
+		if (adap->flags & USING_MSIX)
+			free_msix_queue_irqs_uld(adap, i);
+		free_sge_queues_uld(adap, i);
+		free_queues_uld(adap, i);
+	}
+}
+
+static void uld_init(struct adapter *adap, struct cxgb4_lld_info *lld)
+{
+	int i;
+
+	lld->pdev = adap->pdev;
+	lld->pf = adap->pf;
+	lld->l2t = adap->l2t;
+	lld->tids = &adap->tids;
+	lld->ports = adap->port;
+	lld->vr = &adap->vres;
+	lld->mtus = adap->params.mtus;
+	lld->ntxq = adap->sge.ofldqsets;
+	lld->nchan = adap->params.nports;
+	lld->nports = adap->params.nports;
+	lld->wr_cred = adap->params.ofldq_wr_cred;
+	lld->iscsi_iolen = MAXRXDATA_G(t4_read_reg(adap, TP_PARA_REG2_A));
+	lld->iscsi_tagmask = t4_read_reg(adap, ULP_RX_ISCSI_TAGMASK_A);
+	lld->iscsi_pgsz_order = t4_read_reg(adap, ULP_RX_ISCSI_PSZ_A);
+	lld->iscsi_llimit = t4_read_reg(adap, ULP_RX_ISCSI_LLIMIT_A);
+	lld->iscsi_ppm = &adap->iscsi_ppm;
+	lld->adapter_type = adap->params.chip;
+	lld->cclk_ps = 1000000000 / adap->params.vpd.cclk;
+	lld->udb_density = 1 << adap->params.sge.eq_qpp;
+	lld->ucq_density = 1 << adap->params.sge.iq_qpp;
+	lld->filt_mode = adap->params.tp.vlan_pri_map;
+	/* MODQ_REQ_MAP sets queues 0-3 to chan 0-3 */
+	for (i = 0; i < NCHAN; i++)
+		lld->tx_modq[i] = i;
+	lld->gts_reg = adap->regs + MYPF_REG(SGE_PF_GTS_A);
+	lld->db_reg = adap->regs + MYPF_REG(SGE_PF_KDOORBELL_A);
+	lld->fw_vers = adap->params.fw_vers;
+	lld->dbfifo_int_thresh = dbfifo_int_thresh;
+	lld->sge_ingpadboundary = adap->sge.fl_align;
+	lld->sge_egrstatuspagesize = adap->sge.stat_len;
+	lld->sge_pktshift = adap->sge.pktshift;
+	lld->enable_fw_ofld_conn = adap->flags & FW_OFLD_CONN;
+	lld->max_ordird_qp = adap->params.max_ordird_qp;
+	lld->max_ird_adapter = adap->params.max_ird_adapter;
+	lld->ulptx_memwrite_dsgl = adap->params.ulptx_memwrite_dsgl;
+	lld->nodeid = dev_to_node(adap->pdev_dev);
+	lld->fr_nsmr_tpte_wr_support = adap->params.fr_nsmr_tpte_wr_support;
+}
+
+static void uld_attach(struct adapter *adap, unsigned int uld)
+{
+	void *handle;
+	struct cxgb4_lld_info lli;
+
+	uld_init(adap, &lli);
+	uld_queue_init(adap, uld, &lli);
+
+	handle = adap->uld[uld].add(&lli);
+	if (IS_ERR(handle)) {
+		dev_warn(adap->pdev_dev,
+			 "could not attach to the %s driver, error %ld\n",
+			 adap->uld[uld].name, PTR_ERR(handle));
+		return;
+	}
+
+	adap->uld[uld].handle = handle;
+	t4_register_netevent_notifier();
+
+	if (adap->flags & FULL_INIT_DONE)
+		adap->uld[uld].state_change(handle, CXGB4_STATE_UP);
+}
+
+/**
+ *	cxgb4_register_uld - register an upper-layer driver
+ *	@type: the ULD type
+ *	@p: the ULD methods
+ *
+ *	Registers an upper-layer driver with this driver and notifies the ULD
+ *	about any presently available devices that support its type.  Returns
+ *	%-EBUSY if a ULD of the same type is already registered.
+ */
+int cxgb4_register_uld(enum cxgb4_uld type,
+		       const struct cxgb4_uld_info *p)
+{
+	int ret = 0;
+	unsigned int adap_idx = 0;
+	struct adapter *adap;
+
+	if (type >= CXGB4_ULD_MAX)
+		return -EINVAL;
+
+	mutex_lock(&uld_mutex);
+	list_for_each_entry(adap, &adapter_list, list_node) {
+		if ((type == CXGB4_ULD_CRYPTO && !is_pci_uld(adap)) ||
+		    (type != CXGB4_ULD_CRYPTO && !is_offload(adap)))
+			continue;
+		if (type == CXGB4_ULD_ISCSIT && is_t4(adap->params.chip))
+			continue;
+		ret = cfg_queues_uld(adap, type, p);
+		if (ret)
+			goto out;
+		ret = setup_sge_queues_uld(adap, type, p->lro);
+		if (ret)
+			goto free_queues;
+		if (adap->flags & USING_MSIX) {
+			name_msix_vecs_uld(adap, type);
+			ret = request_msix_queue_irqs_uld(adap, type);
+			if (ret)
+				goto free_rxq;
+		}
+		if (adap->flags & FULL_INIT_DONE)
+			enable_rx_uld(adap, type);
+		if (adap->uld[type].add) {
+			ret = -EBUSY;
+			goto free_irq;
+		}
+		adap->uld[type] = *p;
+		uld_attach(adap, type);
+		adap_idx++;
+	}
+	mutex_unlock(&uld_mutex);
+	return 0;
+
+free_irq:
+	if (adap->flags & FULL_INIT_DONE)
+		quiesce_rx_uld(adap, type);
+	if (adap->flags & USING_MSIX)
+		free_msix_queue_irqs_uld(adap, type);
+free_rxq:
+	free_sge_queues_uld(adap, type);
+free_queues:
+	free_queues_uld(adap, type);
+out:
+
+	list_for_each_entry(adap, &adapter_list, list_node) {
+		if ((type == CXGB4_ULD_CRYPTO && !is_pci_uld(adap)) ||
+		    (type != CXGB4_ULD_CRYPTO && !is_offload(adap)))
+			continue;
+		if (type == CXGB4_ULD_ISCSIT && is_t4(adap->params.chip))
+			continue;
+		if (!adap_idx)
+			break;
+		adap->uld[type].handle = NULL;
+		adap->uld[type].add = NULL;
+		if (adap->flags & FULL_INIT_DONE)
+			quiesce_rx_uld(adap, type);
+		if (adap->flags & USING_MSIX)
+			free_msix_queue_irqs_uld(adap, type);
+		free_sge_queues_uld(adap, type);
+		free_queues_uld(adap, type);
+		adap_idx--;
+	}
+	mutex_unlock(&uld_mutex);
+	return ret;
+}
+EXPORT_SYMBOL(cxgb4_register_uld);
+
+/**
+ *	cxgb4_unregister_uld - unregister an upper-layer driver
+ *	@type: the ULD type
+ *
+ *	Unregisters an existing upper-layer driver.
+ */
+int cxgb4_unregister_uld(enum cxgb4_uld type)
+{
+	struct adapter *adap;
+
+	if (type >= CXGB4_ULD_MAX)
+		return -EINVAL;
+
+	mutex_lock(&uld_mutex);
+	list_for_each_entry(adap, &adapter_list, list_node) {
+		if ((type == CXGB4_ULD_CRYPTO && !is_pci_uld(adap)) ||
+		    (type != CXGB4_ULD_CRYPTO && !is_offload(adap)))
+			continue;
+		if (type == CXGB4_ULD_ISCSIT && is_t4(adap->params.chip))
+			continue;
+		adap->uld[type].handle = NULL;
+		adap->uld[type].add = NULL;
+		if (adap->flags & FULL_INIT_DONE)
+			quiesce_rx_uld(adap, type);
+		if (adap->flags & USING_MSIX)
+			free_msix_queue_irqs_uld(adap, type);
+		free_sge_queues_uld(adap, type);
+		free_queues_uld(adap, type);
+	}
+	mutex_unlock(&uld_mutex);
+
+	return 0;
+}
+EXPORT_SYMBOL(cxgb4_unregister_uld);
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_uld.h b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_uld.h
index f3c58aaa932d..2996793b1aaa 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_uld.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_uld.h
@@ -1,7 +1,7 @@
 /*
  * This file is part of the Chelsio T4 Ethernet driver for Linux.
  *
- * Copyright (c) 2003-2014 Chelsio Communications, Inc. All rights reserved.
+ * Copyright (c) 2003-2016 Chelsio Communications, Inc. All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
@@ -32,8 +32,8 @@
  * SOFTWARE.
  */
 
-#ifndef __CXGB4_OFLD_H
-#define __CXGB4_OFLD_H
+#ifndef __CXGB4_ULD_H
+#define __CXGB4_ULD_H
 
 #include <linux/cache.h>
 #include <linux/spinlock.h>
@@ -42,6 +42,8 @@
 #include <linux/atomic.h>
 #include "cxgb4.h"
 
+#define MAX_ULD_QSETS 16
+
 /* CPL message priority levels */
 enum {
 	CPL_PRIORITY_DATA     = 0,  /* data messages */
@@ -104,6 +106,7 @@ struct tid_info {
 	unsigned int atid_base;
 
 	struct filter_entry *ftid_tab;
+	unsigned long *ftid_bmap;
 	unsigned int nftids;
 	unsigned int ftid_base;
 	unsigned int aftid_base;
@@ -124,6 +127,8 @@ struct tid_info {
 	atomic_t tids_in_use;
 	/* TIDs in the HASH */
 	atomic_t hash_tids_in_use;
+	/* lock for setting/clearing filter bitmap */
+	spinlock_t ftid_lock;
 };
 
 static inline void *lookup_tid(const struct tid_info *t, unsigned int tid)
@@ -183,15 +188,38 @@ int cxgb4_create_server_filter(const struct net_device *dev, unsigned int stid,
 int cxgb4_remove_server_filter(const struct net_device *dev, unsigned int stid,
 			       unsigned int queue, bool ipv6);
 
+/* Filter operation context to allow callers of cxgb4_set_filter() and
+ * cxgb4_del_filter() to wait for an asynchronous completion.
+ */
+struct filter_ctx {
+	struct completion completion;	/* completion rendezvous */
+	void *closure;			/* caller's opaque information */
+	int result;			/* result of operation */
+	u32 tid;			/* to store tid */
+};
+
+struct ch_filter_specification;
+
+int __cxgb4_set_filter(struct net_device *dev, int filter_id,
+		       struct ch_filter_specification *fs,
+		       struct filter_ctx *ctx);
+int __cxgb4_del_filter(struct net_device *dev, int filter_id,
+		       struct filter_ctx *ctx);
+int cxgb4_set_filter(struct net_device *dev, int filter_id,
+		     struct ch_filter_specification *fs);
+int cxgb4_del_filter(struct net_device *dev, int filter_id);
+
 static inline void set_wr_txq(struct sk_buff *skb, int prio, int queue)
 {
 	skb_set_queue_mapping(skb, (queue << 1) | prio);
 }
 
 enum cxgb4_uld {
+	CXGB4_ULD_INIT,
 	CXGB4_ULD_RDMA,
 	CXGB4_ULD_ISCSI,
 	CXGB4_ULD_ISCSIT,
+	CXGB4_ULD_CRYPTO,
 	CXGB4_ULD_MAX
 };
 
@@ -280,10 +308,16 @@ struct cxgb4_lld_info {
 	unsigned int iscsi_llimit;	     /* chip's iscsi region llimit */
 	void **iscsi_ppm;		     /* iscsi page pod manager */
 	int nodeid;			     /* device numa node id */
+	bool fr_nsmr_tpte_wr_support;	     /* FW supports FR_NSMR_TPTE_WR */
 };
 
 struct cxgb4_uld_info {
 	const char *name;
+	void *handle;
+	unsigned int nrxq;
+	unsigned int rxq_size;
+	bool ciq;
+	bool lro;
 	void *(*add)(const struct cxgb4_lld_info *p);
 	int (*rx_handler)(void *handle, const __be64 *rsp,
 			  const struct pkt_gl *gl);
@@ -330,4 +364,4 @@ int cxgb4_bar2_sge_qregs(struct net_device *dev,
 			 u64 *pbar2_qoffset,
 			 unsigned int *pbar2_qid);
 
-#endif  /* !__CXGB4_OFLD_H */
+#endif  /* !__CXGB4_ULD_H */
diff --git a/drivers/net/ethernet/chelsio/cxgb4/sched.c b/drivers/net/ethernet/chelsio/cxgb4/sched.c
new file mode 100644
index 000000000000..539de764bbd3
--- /dev/null
+++ b/drivers/net/ethernet/chelsio/cxgb4/sched.c
@@ -0,0 +1,556 @@
+/*
+ * This file is part of the Chelsio T4 Ethernet driver for Linux.
+ *
+ * Copyright (c) 2016 Chelsio Communications, Inc. All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * 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/module.h>
+#include <linux/netdevice.h>
+
+#include "cxgb4.h"
+#include "sched.h"
+
+/* Spinlock must be held by caller */
+static int t4_sched_class_fw_cmd(struct port_info *pi,
+				 struct ch_sched_params *p,
+				 enum sched_fw_ops op)
+{
+	struct adapter *adap = pi->adapter;
+	struct sched_table *s = pi->sched_tbl;
+	struct sched_class *e;
+	int err = 0;
+
+	e = &s->tab[p->u.params.class];
+	switch (op) {
+	case SCHED_FW_OP_ADD:
+		err = t4_sched_params(adap, p->type,
+				      p->u.params.level, p->u.params.mode,
+				      p->u.params.rateunit,
+				      p->u.params.ratemode,
+				      p->u.params.channel, e->idx,
+				      p->u.params.minrate, p->u.params.maxrate,
+				      p->u.params.weight, p->u.params.pktsize);
+		break;
+	default:
+		err = -ENOTSUPP;
+		break;
+	}
+
+	return err;
+}
+
+/* Spinlock must be held by caller */
+static int t4_sched_bind_unbind_op(struct port_info *pi, void *arg,
+				   enum sched_bind_type type, bool bind)
+{
+	struct adapter *adap = pi->adapter;
+	u32 fw_mnem, fw_class, fw_param;
+	unsigned int pf = adap->pf;
+	unsigned int vf = 0;
+	int err = 0;
+
+	switch (type) {
+	case SCHED_QUEUE: {
+		struct sched_queue_entry *qe;
+
+		qe = (struct sched_queue_entry *)arg;
+
+		/* Create a template for the FW_PARAMS_CMD mnemonic and
+		 * value (TX Scheduling Class in this case).
+		 */
+		fw_mnem = (FW_PARAMS_MNEM_V(FW_PARAMS_MNEM_DMAQ) |
+			   FW_PARAMS_PARAM_X_V(
+				   FW_PARAMS_PARAM_DMAQ_EQ_SCHEDCLASS_ETH));
+		fw_class = bind ? qe->param.class : FW_SCHED_CLS_NONE;
+		fw_param = (fw_mnem | FW_PARAMS_PARAM_YZ_V(qe->cntxt_id));
+
+		pf = adap->pf;
+		vf = 0;
+		break;
+	}
+	default:
+		err = -ENOTSUPP;
+		goto out;
+	}
+
+	err = t4_set_params(adap, adap->mbox, pf, vf, 1, &fw_param, &fw_class);
+
+out:
+	return err;
+}
+
+static struct sched_class *t4_sched_queue_lookup(struct port_info *pi,
+						 const unsigned int qid,
+						 int *index)
+{
+	struct sched_table *s = pi->sched_tbl;
+	struct sched_class *e, *end;
+	struct sched_class *found = NULL;
+	int i;
+
+	/* Look for a class with matching bound queue parameters */
+	end = &s->tab[s->sched_size];
+	for (e = &s->tab[0]; e != end; ++e) {
+		struct sched_queue_entry *qe;
+
+		i = 0;
+		if (e->state == SCHED_STATE_UNUSED)
+			continue;
+
+		list_for_each_entry(qe, &e->queue_list, list) {
+			if (qe->cntxt_id == qid) {
+				found = e;
+				if (index)
+					*index = i;
+				break;
+			}
+			i++;
+		}
+
+		if (found)
+			break;
+	}
+
+	return found;
+}
+
+static int t4_sched_queue_unbind(struct port_info *pi, struct ch_sched_queue *p)
+{
+	struct adapter *adap = pi->adapter;
+	struct sched_class *e;
+	struct sched_queue_entry *qe = NULL;
+	struct sge_eth_txq *txq;
+	unsigned int qid;
+	int index = -1;
+	int err = 0;
+
+	if (p->queue < 0 || p->queue >= pi->nqsets)
+		return -ERANGE;
+
+	txq = &adap->sge.ethtxq[pi->first_qset + p->queue];
+	qid = txq->q.cntxt_id;
+
+	/* Find the existing class that the queue is bound to */
+	e = t4_sched_queue_lookup(pi, qid, &index);
+	if (e && index >= 0) {
+		int i = 0;
+
+		spin_lock(&e->lock);
+		list_for_each_entry(qe, &e->queue_list, list) {
+			if (i == index)
+				break;
+			i++;
+		}
+		err = t4_sched_bind_unbind_op(pi, (void *)qe, SCHED_QUEUE,
+					      false);
+		if (err) {
+			spin_unlock(&e->lock);
+			goto out;
+		}
+
+		list_del(&qe->list);
+		t4_free_mem(qe);
+		if (atomic_dec_and_test(&e->refcnt)) {
+			e->state = SCHED_STATE_UNUSED;
+			memset(&e->info, 0, sizeof(e->info));
+		}
+		spin_unlock(&e->lock);
+	}
+out:
+	return err;
+}
+
+static int t4_sched_queue_bind(struct port_info *pi, struct ch_sched_queue *p)
+{
+	struct adapter *adap = pi->adapter;
+	struct sched_table *s = pi->sched_tbl;
+	struct sched_class *e;
+	struct sched_queue_entry *qe = NULL;
+	struct sge_eth_txq *txq;
+	unsigned int qid;
+	int err = 0;
+
+	if (p->queue < 0 || p->queue >= pi->nqsets)
+		return -ERANGE;
+
+	qe = t4_alloc_mem(sizeof(struct sched_queue_entry));
+	if (!qe)
+		return -ENOMEM;
+
+	txq = &adap->sge.ethtxq[pi->first_qset + p->queue];
+	qid = txq->q.cntxt_id;
+
+	/* Unbind queue from any existing class */
+	err = t4_sched_queue_unbind(pi, p);
+	if (err)
+		goto out;
+
+	/* Bind queue to specified class */
+	memset(qe, 0, sizeof(*qe));
+	qe->cntxt_id = qid;
+	memcpy(&qe->param, p, sizeof(qe->param));
+
+	e = &s->tab[qe->param.class];
+	spin_lock(&e->lock);
+	err = t4_sched_bind_unbind_op(pi, (void *)qe, SCHED_QUEUE, true);
+	if (err) {
+		t4_free_mem(qe);
+		spin_unlock(&e->lock);
+		goto out;
+	}
+
+	list_add_tail(&qe->list, &e->queue_list);
+	atomic_inc(&e->refcnt);
+	spin_unlock(&e->lock);
+out:
+	return err;
+}
+
+static void t4_sched_class_unbind_all(struct port_info *pi,
+				      struct sched_class *e,
+				      enum sched_bind_type type)
+{
+	if (!e)
+		return;
+
+	switch (type) {
+	case SCHED_QUEUE: {
+		struct sched_queue_entry *qe;
+
+		list_for_each_entry(qe, &e->queue_list, list)
+			t4_sched_queue_unbind(pi, &qe->param);
+		break;
+	}
+	default:
+		break;
+	}
+}
+
+static int t4_sched_class_bind_unbind_op(struct port_info *pi, void *arg,
+					 enum sched_bind_type type, bool bind)
+{
+	int err = 0;
+
+	if (!arg)
+		return -EINVAL;
+
+	switch (type) {
+	case SCHED_QUEUE: {
+		struct ch_sched_queue *qe = (struct ch_sched_queue *)arg;
+
+		if (bind)
+			err = t4_sched_queue_bind(pi, qe);
+		else
+			err = t4_sched_queue_unbind(pi, qe);
+		break;
+	}
+	default:
+		err = -ENOTSUPP;
+		break;
+	}
+
+	return err;
+}
+
+/**
+ * cxgb4_sched_class_bind - Bind an entity to a scheduling class
+ * @dev: net_device pointer
+ * @arg: Entity opaque data
+ * @type: Entity type (Queue)
+ *
+ * Binds an entity (queue) to a scheduling class.  If the entity
+ * is bound to another class, it will be unbound from the other class
+ * and bound to the class specified in @arg.
+ */
+int cxgb4_sched_class_bind(struct net_device *dev, void *arg,
+			   enum sched_bind_type type)
+{
+	struct port_info *pi = netdev2pinfo(dev);
+	struct sched_table *s;
+	int err = 0;
+	u8 class_id;
+
+	if (!can_sched(dev))
+		return -ENOTSUPP;
+
+	if (!arg)
+		return -EINVAL;
+
+	switch (type) {
+	case SCHED_QUEUE: {
+		struct ch_sched_queue *qe = (struct ch_sched_queue *)arg;
+
+		class_id = qe->class;
+		break;
+	}
+	default:
+		return -ENOTSUPP;
+	}
+
+	if (!valid_class_id(dev, class_id))
+		return -EINVAL;
+
+	if (class_id == SCHED_CLS_NONE)
+		return -ENOTSUPP;
+
+	s = pi->sched_tbl;
+	write_lock(&s->rw_lock);
+	err = t4_sched_class_bind_unbind_op(pi, arg, type, true);
+	write_unlock(&s->rw_lock);
+
+	return err;
+}
+
+/**
+ * cxgb4_sched_class_unbind - Unbind an entity from a scheduling class
+ * @dev: net_device pointer
+ * @arg: Entity opaque data
+ * @type: Entity type (Queue)
+ *
+ * Unbinds an entity (queue) from a scheduling class.
+ */
+int cxgb4_sched_class_unbind(struct net_device *dev, void *arg,
+			     enum sched_bind_type type)
+{
+	struct port_info *pi = netdev2pinfo(dev);
+	struct sched_table *s;
+	int err = 0;
+	u8 class_id;
+
+	if (!can_sched(dev))
+		return -ENOTSUPP;
+
+	if (!arg)
+		return -EINVAL;
+
+	switch (type) {
+	case SCHED_QUEUE: {
+		struct ch_sched_queue *qe = (struct ch_sched_queue *)arg;
+
+		class_id = qe->class;
+		break;
+	}
+	default:
+		return -ENOTSUPP;
+	}
+
+	if (!valid_class_id(dev, class_id))
+		return -EINVAL;
+
+	s = pi->sched_tbl;
+	write_lock(&s->rw_lock);
+	err = t4_sched_class_bind_unbind_op(pi, arg, type, false);
+	write_unlock(&s->rw_lock);
+
+	return err;
+}
+
+/* If @p is NULL, fetch any available unused class */
+static struct sched_class *t4_sched_class_lookup(struct port_info *pi,
+						const struct ch_sched_params *p)
+{
+	struct sched_table *s = pi->sched_tbl;
+	struct sched_class *e, *end;
+	struct sched_class *found = NULL;
+
+	if (!p) {
+		/* Get any available unused class */
+		end = &s->tab[s->sched_size];
+		for (e = &s->tab[0]; e != end; ++e) {
+			if (e->state == SCHED_STATE_UNUSED) {
+				found = e;
+				break;
+			}
+		}
+	} else {
+		/* Look for a class with matching scheduling parameters */
+		struct ch_sched_params info;
+		struct ch_sched_params tp;
+
+		memset(&info, 0, sizeof(info));
+		memset(&tp, 0, sizeof(tp));
+
+		memcpy(&tp, p, sizeof(tp));
+		/* Don't try to match class parameter */
+		tp.u.params.class = SCHED_CLS_NONE;
+
+		end = &s->tab[s->sched_size];
+		for (e = &s->tab[0]; e != end; ++e) {
+			if (e->state == SCHED_STATE_UNUSED)
+				continue;
+
+			memset(&info, 0, sizeof(info));
+			memcpy(&info, &e->info, sizeof(info));
+			/* Don't try to match class parameter */
+			info.u.params.class = SCHED_CLS_NONE;
+
+			if ((info.type == tp.type) &&
+			    (!memcmp(&info.u.params, &tp.u.params,
+				     sizeof(info.u.params)))) {
+				found = e;
+				break;
+			}
+		}
+	}
+
+	return found;
+}
+
+static struct sched_class *t4_sched_class_alloc(struct port_info *pi,
+						struct ch_sched_params *p)
+{
+	struct sched_table *s = pi->sched_tbl;
+	struct sched_class *e;
+	u8 class_id;
+	int err;
+
+	if (!p)
+		return NULL;
+
+	class_id = p->u.params.class;
+
+	/* Only accept search for existing class with matching params
+	 * or allocation of new class with specified params
+	 */
+	if (class_id != SCHED_CLS_NONE)
+		return NULL;
+
+	write_lock(&s->rw_lock);
+	/* See if there's an exisiting class with same
+	 * requested sched params
+	 */
+	e = t4_sched_class_lookup(pi, p);
+	if (!e) {
+		struct ch_sched_params np;
+
+		/* Fetch any available unused class */
+		e = t4_sched_class_lookup(pi, NULL);
+		if (!e)
+			goto out;
+
+		memset(&np, 0, sizeof(np));
+		memcpy(&np, p, sizeof(np));
+		np.u.params.class = e->idx;
+
+		spin_lock(&e->lock);
+		/* New class */
+		err = t4_sched_class_fw_cmd(pi, &np, SCHED_FW_OP_ADD);
+		if (err) {
+			spin_unlock(&e->lock);
+			e = NULL;
+			goto out;
+		}
+		memcpy(&e->info, &np, sizeof(e->info));
+		atomic_set(&e->refcnt, 0);
+		e->state = SCHED_STATE_ACTIVE;
+		spin_unlock(&e->lock);
+	}
+
+out:
+	write_unlock(&s->rw_lock);
+	return e;
+}
+
+/**
+ * cxgb4_sched_class_alloc - allocate a scheduling class
+ * @dev: net_device pointer
+ * @p: new scheduling class to create.
+ *
+ * Returns pointer to the scheduling class created.  If @p is NULL, then
+ * it allocates and returns any available unused scheduling class. If a
+ * scheduling class with matching @p is found, then the matching class is
+ * returned.
+ */
+struct sched_class *cxgb4_sched_class_alloc(struct net_device *dev,
+					    struct ch_sched_params *p)
+{
+	struct port_info *pi = netdev2pinfo(dev);
+	u8 class_id;
+
+	if (!can_sched(dev))
+		return NULL;
+
+	class_id = p->u.params.class;
+	if (!valid_class_id(dev, class_id))
+		return NULL;
+
+	return t4_sched_class_alloc(pi, p);
+}
+
+static void t4_sched_class_free(struct port_info *pi, struct sched_class *e)
+{
+	t4_sched_class_unbind_all(pi, e, SCHED_QUEUE);
+}
+
+struct sched_table *t4_init_sched(unsigned int sched_size)
+{
+	struct sched_table *s;
+	unsigned int i;
+
+	s = t4_alloc_mem(sizeof(*s) + sched_size * sizeof(struct sched_class));
+	if (!s)
+		return NULL;
+
+	s->sched_size = sched_size;
+	rwlock_init(&s->rw_lock);
+
+	for (i = 0; i < s->sched_size; i++) {
+		memset(&s->tab[i], 0, sizeof(struct sched_class));
+		s->tab[i].idx = i;
+		s->tab[i].state = SCHED_STATE_UNUSED;
+		INIT_LIST_HEAD(&s->tab[i].queue_list);
+		spin_lock_init(&s->tab[i].lock);
+		atomic_set(&s->tab[i].refcnt, 0);
+	}
+	return s;
+}
+
+void t4_cleanup_sched(struct adapter *adap)
+{
+	struct sched_table *s;
+	unsigned int i;
+
+	for_each_port(adap, i) {
+		struct port_info *pi = netdev2pinfo(adap->port[i]);
+
+		s = pi->sched_tbl;
+		for (i = 0; i < s->sched_size; i++) {
+			struct sched_class *e;
+
+			write_lock(&s->rw_lock);
+			e = &s->tab[i];
+			if (e->state == SCHED_STATE_ACTIVE)
+				t4_sched_class_free(pi, e);
+			write_unlock(&s->rw_lock);
+		}
+		t4_free_mem(s);
+	}
+}
diff --git a/drivers/net/ethernet/chelsio/cxgb4/sched.h b/drivers/net/ethernet/chelsio/cxgb4/sched.h
new file mode 100644
index 000000000000..77b2b3fd9021
--- /dev/null
+++ b/drivers/net/ethernet/chelsio/cxgb4/sched.h
@@ -0,0 +1,110 @@
+/*
+ * This file is part of the Chelsio T4 Ethernet driver for Linux.
+ *
+ * Copyright (c) 2016 Chelsio Communications, Inc. All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * 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 __CXGB4_SCHED_H
+#define __CXGB4_SCHED_H
+
+#include <linux/spinlock.h>
+#include <linux/atomic.h>
+
+#define SCHED_CLS_NONE 0xff
+
+#define FW_SCHED_CLS_NONE 0xffffffff
+
+/* Max rate that can be set to a scheduling class is 10 Gbps */
+#define SCHED_MAX_RATE_KBPS 10000000U
+
+enum {
+	SCHED_STATE_ACTIVE,
+	SCHED_STATE_UNUSED,
+};
+
+enum sched_fw_ops {
+	SCHED_FW_OP_ADD,
+};
+
+enum sched_bind_type {
+	SCHED_QUEUE,
+};
+
+struct sched_queue_entry {
+	struct list_head list;
+	unsigned int cntxt_id;
+	struct ch_sched_queue param;
+};
+
+struct sched_class {
+	u8 state;
+	u8 idx;
+	struct ch_sched_params info;
+	struct list_head queue_list;
+	spinlock_t lock; /* Per class lock */
+	atomic_t refcnt;
+};
+
+struct sched_table {      /* per port scheduling table */
+	u8 sched_size;
+	rwlock_t rw_lock; /* Table lock */
+	struct sched_class tab[0];
+};
+
+static inline bool can_sched(struct net_device *dev)
+{
+	struct port_info *pi = netdev2pinfo(dev);
+
+	return !pi->sched_tbl ? false : true;
+}
+
+static inline bool valid_class_id(struct net_device *dev, u8 class_id)
+{
+	struct port_info *pi = netdev2pinfo(dev);
+
+	if ((class_id > pi->sched_tbl->sched_size - 1) &&
+	    (class_id != SCHED_CLS_NONE))
+		return false;
+
+	return true;
+}
+
+int cxgb4_sched_class_bind(struct net_device *dev, void *arg,
+			   enum sched_bind_type type);
+int cxgb4_sched_class_unbind(struct net_device *dev, void *arg,
+			     enum sched_bind_type type);
+
+struct sched_class *cxgb4_sched_class_alloc(struct net_device *dev,
+					    struct ch_sched_params *p);
+
+struct sched_table *t4_init_sched(unsigned int size);
+void t4_cleanup_sched(struct adapter *adap);
+#endif  /* __CXGB4_SCHED_H */
diff --git a/drivers/net/ethernet/chelsio/cxgb4/sge.c b/drivers/net/ethernet/chelsio/cxgb4/sge.c
index ad3552df0545..1e74fd6085df 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/sge.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/sge.c
@@ -2860,6 +2860,18 @@ int t4_sge_alloc_ctrl_txq(struct adapter *adap, struct sge_ctrl_txq *txq,
 	return 0;
 }
 
+int t4_sge_mod_ctrl_txq(struct adapter *adap, unsigned int eqid,
+			unsigned int cmplqid)
+{
+	u32 param, val;
+
+	param = (FW_PARAMS_MNEM_V(FW_PARAMS_MNEM_DMAQ) |
+		 FW_PARAMS_PARAM_X_V(FW_PARAMS_PARAM_DMAQ_EQ_CMPLIQID_CTRL) |
+		 FW_PARAMS_PARAM_YZ_V(eqid));
+	val = cmplqid;
+	return t4_set_params(adap, adap->mbox, adap->pf, 0, 1, &param, &val);
+}
+
 int t4_sge_alloc_ofld_txq(struct adapter *adap, struct sge_ofld_txq *txq,
 			  struct net_device *dev, unsigned int iqid)
 {
@@ -2928,8 +2940,8 @@ static void free_txq(struct adapter *adap, struct sge_txq *q)
 	q->desc = NULL;
 }
 
-static void free_rspq_fl(struct adapter *adap, struct sge_rspq *rq,
-			 struct sge_fl *fl)
+void free_rspq_fl(struct adapter *adap, struct sge_rspq *rq,
+		  struct sge_fl *fl)
 {
 	struct sge *s = &adap->sge;
 	unsigned int fl_id = fl ? fl->cntxt_id : 0xffff;
@@ -3014,12 +3026,6 @@ void t4_free_sge_resources(struct adapter *adap)
 		}
 	}
 
-	/* clean up RDMA and iSCSI Rx queues */
-	t4_free_ofld_rxqs(adap, adap->sge.iscsiqsets, adap->sge.iscsirxq);
-	t4_free_ofld_rxqs(adap, adap->sge.niscsitq, adap->sge.iscsitrxq);
-	t4_free_ofld_rxqs(adap, adap->sge.rdmaqs, adap->sge.rdmarxq);
-	t4_free_ofld_rxqs(adap, adap->sge.rdmaciqs, adap->sge.rdmaciq);
-
 	/* clean up offload Tx queues */
 	for (i = 0; i < ARRAY_SIZE(adap->sge.ofldtxq); i++) {
 		struct sge_ofld_txq *q = &adap->sge.ofldtxq[i];
diff --git a/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c b/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c
index 660204bff726..20dec85da63d 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c
@@ -1,7 +1,7 @@
 /*
  * This file is part of the Chelsio T4 Ethernet driver for Linux.
  *
- * Copyright (c) 2003-2014 Chelsio Communications, Inc. All rights reserved.
+ * Copyright (c) 2003-2016 Chelsio Communications, Inc. All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
@@ -2729,7 +2729,7 @@ int t4_get_raw_vpd_params(struct adapter *adapter, struct vpd_params *p)
 
 out:
 	vfree(vpd);
-	return ret;
+	return ret < 0 ? ret : 0;
 }
 
 /**
@@ -8269,3 +8269,73 @@ void t4_idma_monitor(struct adapter *adapter,
 		t4_sge_decode_idma_state(adapter, idma->idma_state[i]);
 	}
 }
+
+/**
+ *	t4_set_vf_mac - Set MAC address for the specified VF
+ *	@adapter: The adapter
+ *	@vf: one of the VFs instantiated by the specified PF
+ *	@naddr: the number of MAC addresses
+ *	@addr: the MAC address(es) to be set to the specified VF
+ */
+int t4_set_vf_mac_acl(struct adapter *adapter, unsigned int vf,
+		      unsigned int naddr, u8 *addr)
+{
+	struct fw_acl_mac_cmd cmd;
+
+	memset(&cmd, 0, sizeof(cmd));
+	cmd.op_to_vfn = cpu_to_be32(FW_CMD_OP_V(FW_ACL_MAC_CMD) |
+				    FW_CMD_REQUEST_F |
+				    FW_CMD_WRITE_F |
+				    FW_ACL_MAC_CMD_PFN_V(adapter->pf) |
+				    FW_ACL_MAC_CMD_VFN_V(vf));
+
+	/* Note: Do not enable the ACL */
+	cmd.en_to_len16 = cpu_to_be32((unsigned int)FW_LEN16(cmd));
+	cmd.nmac = naddr;
+
+	switch (adapter->pf) {
+	case 3:
+		memcpy(cmd.macaddr3, addr, sizeof(cmd.macaddr3));
+		break;
+	case 2:
+		memcpy(cmd.macaddr2, addr, sizeof(cmd.macaddr2));
+		break;
+	case 1:
+		memcpy(cmd.macaddr1, addr, sizeof(cmd.macaddr1));
+		break;
+	case 0:
+		memcpy(cmd.macaddr0, addr, sizeof(cmd.macaddr0));
+		break;
+	}
+
+	return t4_wr_mbox(adapter, adapter->mbox, &cmd, sizeof(cmd), &cmd);
+}
+
+int t4_sched_params(struct adapter *adapter, int type, int level, int mode,
+		    int rateunit, int ratemode, int channel, int class,
+		    int minrate, int maxrate, int weight, int pktsize)
+{
+	struct fw_sched_cmd cmd;
+
+	memset(&cmd, 0, sizeof(cmd));
+	cmd.op_to_write = cpu_to_be32(FW_CMD_OP_V(FW_SCHED_CMD) |
+				      FW_CMD_REQUEST_F |
+				      FW_CMD_WRITE_F);
+	cmd.retval_len16 = cpu_to_be32(FW_LEN16(cmd));
+
+	cmd.u.params.sc = FW_SCHED_SC_PARAMS;
+	cmd.u.params.type = type;
+	cmd.u.params.level = level;
+	cmd.u.params.mode = mode;
+	cmd.u.params.ch = channel;
+	cmd.u.params.cl = class;
+	cmd.u.params.unit = rateunit;
+	cmd.u.params.rate = ratemode;
+	cmd.u.params.min = cpu_to_be32(minrate);
+	cmd.u.params.max = cpu_to_be32(maxrate);
+	cmd.u.params.weight = cpu_to_be16(weight);
+	cmd.u.params.pktsize = cpu_to_be16(pktsize);
+
+	return t4_wr_mbox_meat(adapter, adapter->mbox, &cmd, sizeof(cmd),
+			       NULL, 1);
+}
diff --git a/drivers/net/ethernet/chelsio/cxgb4/t4_msg.h b/drivers/net/ethernet/chelsio/cxgb4/t4_msg.h
index e0ebe1378cb2..fba3b2ad382d 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/t4_msg.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/t4_msg.h
@@ -61,6 +61,7 @@ enum {
 	CPL_ABORT_REQ_RSS     = 0x2B,
 	CPL_ABORT_RPL_RSS     = 0x2D,
 
+	CPL_RX_PHYS_ADDR      = 0x30,
 	CPL_CLOSE_CON_RPL     = 0x32,
 	CPL_ISCSI_HDR         = 0x33,
 	CPL_RDMA_CQE          = 0x35,
@@ -83,6 +84,10 @@ enum {
 	CPL_PASS_OPEN_REQ6    = 0x81,
 	CPL_ACT_OPEN_REQ6     = 0x83,
 
+	CPL_TX_TLS_PDU     =    0x88,
+	CPL_TX_SEC_PDU        = 0x8A,
+	CPL_TX_TLS_ACK        = 0x8B,
+
 	CPL_RDMA_TERMINATE    = 0xA2,
 	CPL_RDMA_WRITE        = 0xA4,
 	CPL_SGE_EGR_UPDATE    = 0xA5,
@@ -94,6 +99,8 @@ enum {
 	CPL_FW4_PLD           = 0xC1,
 	CPL_FW4_ACK           = 0xC3,
 
+	CPL_RX_PHYS_DSGL      = 0xD0,
+
 	CPL_FW6_MSG           = 0xE0,
 	CPL_FW6_PLD           = 0xE1,
 	CPL_TX_PKT_LSO        = 0xED,
@@ -1362,6 +1369,15 @@ struct ulptx_idata {
 	__be32 len;
 };
 
+struct ulp_txpkt {
+	__be32 cmd_dest;
+	__be32 len;
+};
+
+#define ULPTX_CMD_S    24
+#define ULPTX_CMD_M    0xFF
+#define ULPTX_CMD_V(x) ((x) << ULPTX_CMD_S)
+
 #define ULPTX_NSGE_S    0
 #define ULPTX_NSGE_V(x) ((x) << ULPTX_NSGE_S)
 
@@ -1369,6 +1385,22 @@ struct ulptx_idata {
 #define ULPTX_MORE_V(x)	((x) << ULPTX_MORE_S)
 #define ULPTX_MORE_F	ULPTX_MORE_V(1U)
 
+#define ULP_TXPKT_DEST_S    16
+#define ULP_TXPKT_DEST_M    0x3
+#define ULP_TXPKT_DEST_V(x) ((x) << ULP_TXPKT_DEST_S)
+
+#define ULP_TXPKT_FID_S     4
+#define ULP_TXPKT_FID_M     0x7ff
+#define ULP_TXPKT_FID_V(x)  ((x) << ULP_TXPKT_FID_S)
+
+#define ULP_TXPKT_RO_S      3
+#define ULP_TXPKT_RO_V(x) ((x) << ULP_TXPKT_RO_S)
+#define ULP_TXPKT_RO_F ULP_TXPKT_RO_V(1U)
+
+#define ULP_TX_SC_MORE_S 23
+#define ULP_TX_SC_MORE_V(x) ((x) << ULP_TX_SC_MORE_S)
+#define ULP_TX_SC_MORE_F  ULP_TX_SC_MORE_V(1U)
+
 struct ulp_mem_io {
 	WR_HDR;
 	__be32 cmd;
@@ -1406,4 +1438,409 @@ struct ulp_mem_io {
 #define ULP_MEMIO_DATA_LEN_S    0
 #define ULP_MEMIO_DATA_LEN_V(x) ((x) << ULP_MEMIO_DATA_LEN_S)
 
+#define ULPTX_NSGE_S    0
+#define ULPTX_NSGE_M    0xFFFF
+#define ULPTX_NSGE_V(x) ((x) << ULPTX_NSGE_S)
+#define ULPTX_NSGE_G(x) (((x) >> ULPTX_NSGE_S) & ULPTX_NSGE_M)
+
+struct ulptx_sc_memrd {
+	__be32 cmd_to_len;
+	__be32 addr;
+};
+
+#define ULP_TXPKT_DATAMODIFY_S       23
+#define ULP_TXPKT_DATAMODIFY_M       0x1
+#define ULP_TXPKT_DATAMODIFY_V(x)    ((x) << ULP_TXPKT_DATAMODIFY_S)
+#define ULP_TXPKT_DATAMODIFY_G(x)    \
+	(((x) >> ULP_TXPKT_DATAMODIFY_S) & ULP_TXPKT_DATAMODIFY__M)
+#define ULP_TXPKT_DATAMODIFY_F       ULP_TXPKT_DATAMODIFY_V(1U)
+
+#define ULP_TXPKT_CHANNELID_S        22
+#define ULP_TXPKT_CHANNELID_M        0x1
+#define ULP_TXPKT_CHANNELID_V(x)     ((x) << ULP_TXPKT_CHANNELID_S)
+#define ULP_TXPKT_CHANNELID_G(x)     \
+	(((x) >> ULP_TXPKT_CHANNELID_S) & ULP_TXPKT_CHANNELID_M)
+#define ULP_TXPKT_CHANNELID_F        ULP_TXPKT_CHANNELID_V(1U)
+
+#define SCMD_SEQ_NO_CTRL_S      29
+#define SCMD_SEQ_NO_CTRL_M      0x3
+#define SCMD_SEQ_NO_CTRL_V(x)   ((x) << SCMD_SEQ_NO_CTRL_S)
+#define SCMD_SEQ_NO_CTRL_G(x)   \
+	(((x) >> SCMD_SEQ_NO_CTRL_S) & SCMD_SEQ_NO_CTRL_M)
+
+/* StsFieldPrsnt- Status field at the end of the TLS PDU */
+#define SCMD_STATUS_PRESENT_S   28
+#define SCMD_STATUS_PRESENT_M   0x1
+#define SCMD_STATUS_PRESENT_V(x)    ((x) << SCMD_STATUS_PRESENT_S)
+#define SCMD_STATUS_PRESENT_G(x)    \
+	(((x) >> SCMD_STATUS_PRESENT_S) & SCMD_STATUS_PRESENT_M)
+#define SCMD_STATUS_PRESENT_F   SCMD_STATUS_PRESENT_V(1U)
+
+/* ProtoVersion - Protocol Version 0: 1.2, 1:1.1, 2:DTLS, 3:Generic,
+ * 3-15: Reserved.
+ */
+#define SCMD_PROTO_VERSION_S    24
+#define SCMD_PROTO_VERSION_M    0xf
+#define SCMD_PROTO_VERSION_V(x) ((x) << SCMD_PROTO_VERSION_S)
+#define SCMD_PROTO_VERSION_G(x) \
+	(((x) >> SCMD_PROTO_VERSION_S) & SCMD_PROTO_VERSION_M)
+
+/* EncDecCtrl - Encryption/Decryption Control. 0: Encrypt, 1: Decrypt */
+#define SCMD_ENC_DEC_CTRL_S     23
+#define SCMD_ENC_DEC_CTRL_M     0x1
+#define SCMD_ENC_DEC_CTRL_V(x)  ((x) << SCMD_ENC_DEC_CTRL_S)
+#define SCMD_ENC_DEC_CTRL_G(x)  \
+	(((x) >> SCMD_ENC_DEC_CTRL_S) & SCMD_ENC_DEC_CTRL_M)
+#define SCMD_ENC_DEC_CTRL_F SCMD_ENC_DEC_CTRL_V(1U)
+
+/* CipherAuthSeqCtrl - Cipher Authentication Sequence Control. */
+#define SCMD_CIPH_AUTH_SEQ_CTRL_S       22
+#define SCMD_CIPH_AUTH_SEQ_CTRL_M       0x1
+#define SCMD_CIPH_AUTH_SEQ_CTRL_V(x)    \
+	((x) << SCMD_CIPH_AUTH_SEQ_CTRL_S)
+#define SCMD_CIPH_AUTH_SEQ_CTRL_G(x)    \
+	(((x) >> SCMD_CIPH_AUTH_SEQ_CTRL_S) & SCMD_CIPH_AUTH_SEQ_CTRL_M)
+#define SCMD_CIPH_AUTH_SEQ_CTRL_F   SCMD_CIPH_AUTH_SEQ_CTRL_V(1U)
+
+/* CiphMode -  Cipher Mode. 0: NOP, 1:AES-CBC, 2:AES-GCM, 3:AES-CTR,
+ * 4:Generic-AES, 5-15: Reserved.
+ */
+#define SCMD_CIPH_MODE_S    18
+#define SCMD_CIPH_MODE_M    0xf
+#define SCMD_CIPH_MODE_V(x) ((x) << SCMD_CIPH_MODE_S)
+#define SCMD_CIPH_MODE_G(x) \
+	(((x) >> SCMD_CIPH_MODE_S) & SCMD_CIPH_MODE_M)
+
+/* AuthMode - Auth Mode. 0: NOP, 1:SHA1, 2:SHA2-224, 3:SHA2-256
+ * 4-15: Reserved
+ */
+#define SCMD_AUTH_MODE_S    14
+#define SCMD_AUTH_MODE_M    0xf
+#define SCMD_AUTH_MODE_V(x) ((x) << SCMD_AUTH_MODE_S)
+#define SCMD_AUTH_MODE_G(x) \
+	(((x) >> SCMD_AUTH_MODE_S) & SCMD_AUTH_MODE_M)
+
+/* HmacCtrl - HMAC Control. 0:NOP, 1:No truncation, 2:Support HMAC Truncation
+ * per RFC 4366, 3:IPSec 96 bits, 4-7:Reserved
+ */
+#define SCMD_HMAC_CTRL_S    11
+#define SCMD_HMAC_CTRL_M    0x7
+#define SCMD_HMAC_CTRL_V(x) ((x) << SCMD_HMAC_CTRL_S)
+#define SCMD_HMAC_CTRL_G(x) \
+	(((x) >> SCMD_HMAC_CTRL_S) & SCMD_HMAC_CTRL_M)
+
+/* IvSize - IV size in units of 2 bytes */
+#define SCMD_IV_SIZE_S  7
+#define SCMD_IV_SIZE_M  0xf
+#define SCMD_IV_SIZE_V(x)   ((x) << SCMD_IV_SIZE_S)
+#define SCMD_IV_SIZE_G(x)   \
+	(((x) >> SCMD_IV_SIZE_S) & SCMD_IV_SIZE_M)
+
+/* NumIVs - Number of IVs */
+#define SCMD_NUM_IVS_S  0
+#define SCMD_NUM_IVS_M  0x7f
+#define SCMD_NUM_IVS_V(x)   ((x) << SCMD_NUM_IVS_S)
+#define SCMD_NUM_IVS_G(x)   \
+	(((x) >> SCMD_NUM_IVS_S) & SCMD_NUM_IVS_M)
+
+/* EnbDbgId - If this is enabled upper 20 (63:44) bits if SeqNumber
+ * (below) are used as Cid (connection id for debug status), these
+ * bits are padded to zero for forming the 64 bit
+ * sequence number for TLS
+ */
+#define SCMD_ENB_DBGID_S  31
+#define SCMD_ENB_DBGID_M  0x1
+#define SCMD_ENB_DBGID_V(x)   ((x) << SCMD_ENB_DBGID_S)
+#define SCMD_ENB_DBGID_G(x)   \
+	(((x) >> SCMD_ENB_DBGID_S) & SCMD_ENB_DBGID_M)
+
+/* IV generation in SW. */
+#define SCMD_IV_GEN_CTRL_S      30
+#define SCMD_IV_GEN_CTRL_M      0x1
+#define SCMD_IV_GEN_CTRL_V(x)   ((x) << SCMD_IV_GEN_CTRL_S)
+#define SCMD_IV_GEN_CTRL_G(x)   \
+	(((x) >> SCMD_IV_GEN_CTRL_S) & SCMD_IV_GEN_CTRL_M)
+#define SCMD_IV_GEN_CTRL_F  SCMD_IV_GEN_CTRL_V(1U)
+
+/* More frags */
+#define SCMD_MORE_FRAGS_S   20
+#define SCMD_MORE_FRAGS_M   0x1
+#define SCMD_MORE_FRAGS_V(x)    ((x) << SCMD_MORE_FRAGS_S)
+#define SCMD_MORE_FRAGS_G(x)    (((x) >> SCMD_MORE_FRAGS_S) & SCMD_MORE_FRAGS_M)
+
+/*last frag */
+#define SCMD_LAST_FRAG_S    19
+#define SCMD_LAST_FRAG_M    0x1
+#define SCMD_LAST_FRAG_V(x) ((x) << SCMD_LAST_FRAG_S)
+#define SCMD_LAST_FRAG_G(x) (((x) >> SCMD_LAST_FRAG_S) & SCMD_LAST_FRAG_M)
+
+/* TlsCompPdu */
+#define SCMD_TLS_COMPPDU_S    18
+#define SCMD_TLS_COMPPDU_M    0x1
+#define SCMD_TLS_COMPPDU_V(x) ((x) << SCMD_TLS_COMPPDU_S)
+#define SCMD_TLS_COMPPDU_G(x) (((x) >> SCMD_TLS_COMPPDU_S) & SCMD_TLS_COMPPDU_M)
+
+/* KeyCntxtInline - Key context inline after the scmd  OR PayloadOnly*/
+#define SCMD_KEY_CTX_INLINE_S   17
+#define SCMD_KEY_CTX_INLINE_M   0x1
+#define SCMD_KEY_CTX_INLINE_V(x)    ((x) << SCMD_KEY_CTX_INLINE_S)
+#define SCMD_KEY_CTX_INLINE_G(x)    \
+	(((x) >> SCMD_KEY_CTX_INLINE_S) & SCMD_KEY_CTX_INLINE_M)
+#define SCMD_KEY_CTX_INLINE_F   SCMD_KEY_CTX_INLINE_V(1U)
+
+/* TLSFragEnable - 0: Host created TLS PDUs, 1: TLS Framgmentation in ASIC */
+#define SCMD_TLS_FRAG_ENABLE_S  16
+#define SCMD_TLS_FRAG_ENABLE_M  0x1
+#define SCMD_TLS_FRAG_ENABLE_V(x)   ((x) << SCMD_TLS_FRAG_ENABLE_S)
+#define SCMD_TLS_FRAG_ENABLE_G(x)   \
+	(((x) >> SCMD_TLS_FRAG_ENABLE_S) & SCMD_TLS_FRAG_ENABLE_M)
+#define SCMD_TLS_FRAG_ENABLE_F  SCMD_TLS_FRAG_ENABLE_V(1U)
+
+/* MacOnly - Only send the MAC and discard PDU. This is valid for hash only
+ * modes, in this case TLS_TX  will drop the PDU and only
+ * send back the MAC bytes.
+ */
+#define SCMD_MAC_ONLY_S 15
+#define SCMD_MAC_ONLY_M 0x1
+#define SCMD_MAC_ONLY_V(x)  ((x) << SCMD_MAC_ONLY_S)
+#define SCMD_MAC_ONLY_G(x)  \
+	(((x) >> SCMD_MAC_ONLY_S) & SCMD_MAC_ONLY_M)
+#define SCMD_MAC_ONLY_F SCMD_MAC_ONLY_V(1U)
+
+/* AadIVDrop - Drop the AAD and IV fields. Useful in protocols
+ * which have complex AAD and IV formations Eg:AES-CCM
+ */
+#define SCMD_AADIVDROP_S 14
+#define SCMD_AADIVDROP_M 0x1
+#define SCMD_AADIVDROP_V(x)  ((x) << SCMD_AADIVDROP_S)
+#define SCMD_AADIVDROP_G(x)  \
+	(((x) >> SCMD_AADIVDROP_S) & SCMD_AADIVDROP_M)
+#define SCMD_AADIVDROP_F SCMD_AADIVDROP_V(1U)
+
+/* HdrLength - Length of all headers excluding TLS header
+ * present before start of crypto PDU/payload.
+ */
+#define SCMD_HDR_LEN_S  0
+#define SCMD_HDR_LEN_M  0x3fff
+#define SCMD_HDR_LEN_V(x)   ((x) << SCMD_HDR_LEN_S)
+#define SCMD_HDR_LEN_G(x)   \
+	(((x) >> SCMD_HDR_LEN_S) & SCMD_HDR_LEN_M)
+
+struct cpl_tx_sec_pdu {
+	__be32 op_ivinsrtofst;
+	__be32 pldlen;
+	__be32 aadstart_cipherstop_hi;
+	__be32 cipherstop_lo_authinsert;
+	__be32 seqno_numivs;
+	__be32 ivgen_hdrlen;
+	__be64 scmd1;
+};
+
+#define CPL_TX_SEC_PDU_OPCODE_S     24
+#define CPL_TX_SEC_PDU_OPCODE_M     0xff
+#define CPL_TX_SEC_PDU_OPCODE_V(x)  ((x) << CPL_TX_SEC_PDU_OPCODE_S)
+#define CPL_TX_SEC_PDU_OPCODE_G(x)  \
+	(((x) >> CPL_TX_SEC_PDU_OPCODE_S) & CPL_TX_SEC_PDU_OPCODE_M)
+
+/* RX Channel Id */
+#define CPL_TX_SEC_PDU_RXCHID_S  22
+#define CPL_TX_SEC_PDU_RXCHID_M  0x1
+#define CPL_TX_SEC_PDU_RXCHID_V(x)   ((x) << CPL_TX_SEC_PDU_RXCHID_S)
+#define CPL_TX_SEC_PDU_RXCHID_G(x)   \
+	(((x) >> CPL_TX_SEC_PDU_RXCHID_S) & CPL_TX_SEC_PDU_RXCHID_M)
+#define CPL_TX_SEC_PDU_RXCHID_F  CPL_TX_SEC_PDU_RXCHID_V(1U)
+
+/* Ack Follows */
+#define CPL_TX_SEC_PDU_ACKFOLLOWS_S  21
+#define CPL_TX_SEC_PDU_ACKFOLLOWS_M  0x1
+#define CPL_TX_SEC_PDU_ACKFOLLOWS_V(x)   ((x) << CPL_TX_SEC_PDU_ACKFOLLOWS_S)
+#define CPL_TX_SEC_PDU_ACKFOLLOWS_G(x)   \
+	(((x) >> CPL_TX_SEC_PDU_ACKFOLLOWS_S) & CPL_TX_SEC_PDU_ACKFOLLOWS_M)
+#define CPL_TX_SEC_PDU_ACKFOLLOWS_F  CPL_TX_SEC_PDU_ACKFOLLOWS_V(1U)
+
+/* Loopback bit in cpl_tx_sec_pdu */
+#define CPL_TX_SEC_PDU_ULPTXLPBK_S  20
+#define CPL_TX_SEC_PDU_ULPTXLPBK_M  0x1
+#define CPL_TX_SEC_PDU_ULPTXLPBK_V(x)   ((x) << CPL_TX_SEC_PDU_ULPTXLPBK_S)
+#define CPL_TX_SEC_PDU_ULPTXLPBK_G(x)   \
+	(((x) >> CPL_TX_SEC_PDU_ULPTXLPBK_S) & CPL_TX_SEC_PDU_ULPTXLPBK_M)
+#define CPL_TX_SEC_PDU_ULPTXLPBK_F  CPL_TX_SEC_PDU_ULPTXLPBK_V(1U)
+
+/* Length of cpl header encapsulated */
+#define CPL_TX_SEC_PDU_CPLLEN_S     16
+#define CPL_TX_SEC_PDU_CPLLEN_M     0xf
+#define CPL_TX_SEC_PDU_CPLLEN_V(x)  ((x) << CPL_TX_SEC_PDU_CPLLEN_S)
+#define CPL_TX_SEC_PDU_CPLLEN_G(x)  \
+	(((x) >> CPL_TX_SEC_PDU_CPLLEN_S) & CPL_TX_SEC_PDU_CPLLEN_M)
+
+/* PlaceHolder */
+#define CPL_TX_SEC_PDU_PLACEHOLDER_S    10
+#define CPL_TX_SEC_PDU_PLACEHOLDER_M    0x1
+#define CPL_TX_SEC_PDU_PLACEHOLDER_V(x) ((x) << CPL_TX_SEC_PDU_PLACEHOLDER_S)
+#define CPL_TX_SEC_PDU_PLACEHOLDER_G(x) \
+	(((x) >> CPL_TX_SEC_PDU_PLACEHOLDER_S) & \
+	 CPL_TX_SEC_PDU_PLACEHOLDER_M)
+
+/* IvInsrtOffset: Insertion location for IV */
+#define CPL_TX_SEC_PDU_IVINSRTOFST_S    0
+#define CPL_TX_SEC_PDU_IVINSRTOFST_M    0x3ff
+#define CPL_TX_SEC_PDU_IVINSRTOFST_V(x) ((x) << CPL_TX_SEC_PDU_IVINSRTOFST_S)
+#define CPL_TX_SEC_PDU_IVINSRTOFST_G(x) \
+	(((x) >> CPL_TX_SEC_PDU_IVINSRTOFST_S) & \
+	 CPL_TX_SEC_PDU_IVINSRTOFST_M)
+
+/* AadStartOffset: Offset in bytes for AAD start from
+ * the first byte following the pkt headers (0-255 bytes)
+ */
+#define CPL_TX_SEC_PDU_AADSTART_S   24
+#define CPL_TX_SEC_PDU_AADSTART_M   0xff
+#define CPL_TX_SEC_PDU_AADSTART_V(x)    ((x) << CPL_TX_SEC_PDU_AADSTART_S)
+#define CPL_TX_SEC_PDU_AADSTART_G(x)    \
+	(((x) >> CPL_TX_SEC_PDU_AADSTART_S) & \
+	 CPL_TX_SEC_PDU_AADSTART_M)
+
+/* AadStopOffset: offset in bytes for AAD stop/end from the first byte following
+ * the pkt headers (0-511 bytes)
+ */
+#define CPL_TX_SEC_PDU_AADSTOP_S    15
+#define CPL_TX_SEC_PDU_AADSTOP_M    0x1ff
+#define CPL_TX_SEC_PDU_AADSTOP_V(x) ((x) << CPL_TX_SEC_PDU_AADSTOP_S)
+#define CPL_TX_SEC_PDU_AADSTOP_G(x) \
+	(((x) >> CPL_TX_SEC_PDU_AADSTOP_S) & CPL_TX_SEC_PDU_AADSTOP_M)
+
+/* CipherStartOffset: offset in bytes for encryption/decryption start from the
+ * first byte following the pkt headers (0-1023 bytes)
+ */
+#define CPL_TX_SEC_PDU_CIPHERSTART_S    5
+#define CPL_TX_SEC_PDU_CIPHERSTART_M    0x3ff
+#define CPL_TX_SEC_PDU_CIPHERSTART_V(x) ((x) << CPL_TX_SEC_PDU_CIPHERSTART_S)
+#define CPL_TX_SEC_PDU_CIPHERSTART_G(x) \
+	(((x) >> CPL_TX_SEC_PDU_CIPHERSTART_S) & \
+	 CPL_TX_SEC_PDU_CIPHERSTART_M)
+
+/* CipherStopOffset: offset in bytes for encryption/decryption end
+ * from end of the payload of this command (0-511 bytes)
+ */
+#define CPL_TX_SEC_PDU_CIPHERSTOP_HI_S      0
+#define CPL_TX_SEC_PDU_CIPHERSTOP_HI_M      0x1f
+#define CPL_TX_SEC_PDU_CIPHERSTOP_HI_V(x)   \
+	((x) << CPL_TX_SEC_PDU_CIPHERSTOP_HI_S)
+#define CPL_TX_SEC_PDU_CIPHERSTOP_HI_G(x)   \
+	(((x) >> CPL_TX_SEC_PDU_CIPHERSTOP_HI_S) & \
+	 CPL_TX_SEC_PDU_CIPHERSTOP_HI_M)
+
+#define CPL_TX_SEC_PDU_CIPHERSTOP_LO_S      28
+#define CPL_TX_SEC_PDU_CIPHERSTOP_LO_M      0xf
+#define CPL_TX_SEC_PDU_CIPHERSTOP_LO_V(x)   \
+	((x) << CPL_TX_SEC_PDU_CIPHERSTOP_LO_S)
+#define CPL_TX_SEC_PDU_CIPHERSTOP_LO_G(x)   \
+	(((x) >> CPL_TX_SEC_PDU_CIPHERSTOP_LO_S) & \
+	 CPL_TX_SEC_PDU_CIPHERSTOP_LO_M)
+
+/* AuthStartOffset: offset in bytes for authentication start from
+ * the first byte following the pkt headers (0-1023)
+ */
+#define CPL_TX_SEC_PDU_AUTHSTART_S  18
+#define CPL_TX_SEC_PDU_AUTHSTART_M  0x3ff
+#define CPL_TX_SEC_PDU_AUTHSTART_V(x)   ((x) << CPL_TX_SEC_PDU_AUTHSTART_S)
+#define CPL_TX_SEC_PDU_AUTHSTART_G(x)   \
+	(((x) >> CPL_TX_SEC_PDU_AUTHSTART_S) & \
+	 CPL_TX_SEC_PDU_AUTHSTART_M)
+
+/* AuthStopOffset: offset in bytes for authentication
+ * end from end of the payload of this command (0-511 Bytes)
+ */
+#define CPL_TX_SEC_PDU_AUTHSTOP_S   9
+#define CPL_TX_SEC_PDU_AUTHSTOP_M   0x1ff
+#define CPL_TX_SEC_PDU_AUTHSTOP_V(x)    ((x) << CPL_TX_SEC_PDU_AUTHSTOP_S)
+#define CPL_TX_SEC_PDU_AUTHSTOP_G(x)    \
+	(((x) >> CPL_TX_SEC_PDU_AUTHSTOP_S) & \
+	 CPL_TX_SEC_PDU_AUTHSTOP_M)
+
+/* AuthInsrtOffset: offset in bytes for authentication insertion
+ * from end of the payload of this command (0-511 bytes)
+ */
+#define CPL_TX_SEC_PDU_AUTHINSERT_S 0
+#define CPL_TX_SEC_PDU_AUTHINSERT_M 0x1ff
+#define CPL_TX_SEC_PDU_AUTHINSERT_V(x)  ((x) << CPL_TX_SEC_PDU_AUTHINSERT_S)
+#define CPL_TX_SEC_PDU_AUTHINSERT_G(x)  \
+	(((x) >> CPL_TX_SEC_PDU_AUTHINSERT_S) & \
+	 CPL_TX_SEC_PDU_AUTHINSERT_M)
+
+struct cpl_rx_phys_dsgl {
+	__be32 op_to_tid;
+	__be32 pcirlxorder_to_noofsgentr;
+	struct rss_header rss_hdr_int;
+};
+
+#define CPL_RX_PHYS_DSGL_OPCODE_S       24
+#define CPL_RX_PHYS_DSGL_OPCODE_M       0xff
+#define CPL_RX_PHYS_DSGL_OPCODE_V(x)    ((x) << CPL_RX_PHYS_DSGL_OPCODE_S)
+#define CPL_RX_PHYS_DSGL_OPCODE_G(x)    \
+	(((x) >> CPL_RX_PHYS_DSGL_OPCODE_S) & CPL_RX_PHYS_DSGL_OPCODE_M)
+
+#define CPL_RX_PHYS_DSGL_ISRDMA_S       23
+#define CPL_RX_PHYS_DSGL_ISRDMA_M       0x1
+#define CPL_RX_PHYS_DSGL_ISRDMA_V(x)    ((x) << CPL_RX_PHYS_DSGL_ISRDMA_S)
+#define CPL_RX_PHYS_DSGL_ISRDMA_G(x)    \
+	(((x) >> CPL_RX_PHYS_DSGL_ISRDMA_S) & CPL_RX_PHYS_DSGL_ISRDMA_M)
+#define CPL_RX_PHYS_DSGL_ISRDMA_F       CPL_RX_PHYS_DSGL_ISRDMA_V(1U)
+
+#define CPL_RX_PHYS_DSGL_RSVD1_S        20
+#define CPL_RX_PHYS_DSGL_RSVD1_M        0x7
+#define CPL_RX_PHYS_DSGL_RSVD1_V(x)     ((x) << CPL_RX_PHYS_DSGL_RSVD1_S)
+#define CPL_RX_PHYS_DSGL_RSVD1_G(x)     \
+	(((x) >> CPL_RX_PHYS_DSGL_RSVD1_S) & \
+	 CPL_RX_PHYS_DSGL_RSVD1_M)
+
+#define CPL_RX_PHYS_DSGL_PCIRLXORDER_S          31
+#define CPL_RX_PHYS_DSGL_PCIRLXORDER_M          0x1
+#define CPL_RX_PHYS_DSGL_PCIRLXORDER_V(x)       \
+	((x) << CPL_RX_PHYS_DSGL_PCIRLXORDER_S)
+#define CPL_RX_PHYS_DSGL_PCIRLXORDER_G(x)       \
+	(((x) >> CPL_RX_PHYS_DSGL_PCIRLXORDER_S) & \
+	 CPL_RX_PHYS_DSGL_PCIRLXORDER_M)
+#define CPL_RX_PHYS_DSGL_PCIRLXORDER_F  CPL_RX_PHYS_DSGL_PCIRLXORDER_V(1U)
+
+#define CPL_RX_PHYS_DSGL_PCINOSNOOP_S           30
+#define CPL_RX_PHYS_DSGL_PCINOSNOOP_M           0x1
+#define CPL_RX_PHYS_DSGL_PCINOSNOOP_V(x)        \
+	((x) << CPL_RX_PHYS_DSGL_PCINOSNOOP_S)
+#define CPL_RX_PHYS_DSGL_PCINOSNOOP_G(x)        \
+	(((x) >> CPL_RX_PHYS_DSGL_PCINOSNOOP_S) & \
+	 CPL_RX_PHYS_DSGL_PCINOSNOOP_M)
+
+#define CPL_RX_PHYS_DSGL_PCINOSNOOP_F   CPL_RX_PHYS_DSGL_PCINOSNOOP_V(1U)
+
+#define CPL_RX_PHYS_DSGL_PCITPHNTENB_S          29
+#define CPL_RX_PHYS_DSGL_PCITPHNTENB_M          0x1
+#define CPL_RX_PHYS_DSGL_PCITPHNTENB_V(x)       \
+	((x) << CPL_RX_PHYS_DSGL_PCITPHNTENB_S)
+#define CPL_RX_PHYS_DSGL_PCITPHNTENB_G(x)       \
+	(((x) >> CPL_RX_PHYS_DSGL_PCITPHNTENB_S) & \
+	 CPL_RX_PHYS_DSGL_PCITPHNTENB_M)
+#define CPL_RX_PHYS_DSGL_PCITPHNTENB_F  CPL_RX_PHYS_DSGL_PCITPHNTENB_V(1U)
+
+#define CPL_RX_PHYS_DSGL_PCITPHNT_S     27
+#define CPL_RX_PHYS_DSGL_PCITPHNT_M     0x3
+#define CPL_RX_PHYS_DSGL_PCITPHNT_V(x)  ((x) << CPL_RX_PHYS_DSGL_PCITPHNT_S)
+#define CPL_RX_PHYS_DSGL_PCITPHNT_G(x)  \
+	(((x) >> CPL_RX_PHYS_DSGL_PCITPHNT_S) & \
+	 CPL_RX_PHYS_DSGL_PCITPHNT_M)
+
+#define CPL_RX_PHYS_DSGL_DCAID_S        16
+#define CPL_RX_PHYS_DSGL_DCAID_M        0x7ff
+#define CPL_RX_PHYS_DSGL_DCAID_V(x)     ((x) << CPL_RX_PHYS_DSGL_DCAID_S)
+#define CPL_RX_PHYS_DSGL_DCAID_G(x)     \
+	(((x) >> CPL_RX_PHYS_DSGL_DCAID_S) & \
+	 CPL_RX_PHYS_DSGL_DCAID_M)
+
+#define CPL_RX_PHYS_DSGL_NOOFSGENTR_S           0
+#define CPL_RX_PHYS_DSGL_NOOFSGENTR_M           0xffff
+#define CPL_RX_PHYS_DSGL_NOOFSGENTR_V(x)        \
+	((x) << CPL_RX_PHYS_DSGL_NOOFSGENTR_S)
+#define CPL_RX_PHYS_DSGL_NOOFSGENTR_G(x)        \
+	(((x) >> CPL_RX_PHYS_DSGL_NOOFSGENTR_S) & \
+	 CPL_RX_PHYS_DSGL_NOOFSGENTR_M)
+
 #endif  /* __T4_MSG_H */
diff --git a/drivers/net/ethernet/chelsio/cxgb4/t4fw_api.h b/drivers/net/ethernet/chelsio/cxgb4/t4fw_api.h
index 30507d44422c..8d9e4b7a8e84 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/t4fw_api.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/t4fw_api.h
@@ -1,7 +1,7 @@
 /*
  * This file is part of the Chelsio T4 Ethernet driver for Linux.
  *
- * Copyright (c) 2009-2014 Chelsio Communications, Inc. All rights reserved.
+ * Copyright (c) 2009-2016 Chelsio Communications, Inc. All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
@@ -100,8 +100,10 @@ enum fw_wr_opcodes {
 	FW_RI_RECV_WR                  = 0x17,
 	FW_RI_BIND_MW_WR               = 0x18,
 	FW_RI_FR_NSMR_WR               = 0x19,
+	FW_RI_FR_NSMR_TPTE_WR	       = 0x20,
 	FW_RI_INV_LSTAG_WR             = 0x1a,
 	FW_ISCSI_TX_DATA_WR	       = 0x45,
+	FW_CRYPTO_LOOKASIDE_WR         = 0X6d,
 	FW_LASTC2E_WR                  = 0x70
 };
 
@@ -680,6 +682,7 @@ enum fw_cmd_opcodes {
 	FW_RSS_IND_TBL_CMD             = 0x20,
 	FW_RSS_GLB_CONFIG_CMD          = 0x22,
 	FW_RSS_VI_CONFIG_CMD           = 0x23,
+	FW_SCHED_CMD                   = 0x24,
 	FW_DEVLOG_CMD                  = 0x25,
 	FW_CLIP_CMD                    = 0x28,
 	FW_LASTC2E_CMD                 = 0x40,
@@ -1060,7 +1063,7 @@ struct fw_caps_config_cmd {
 	__be16 niccaps;
 	__be16 ofldcaps;
 	__be16 rdmacaps;
-	__be16 r4;
+	__be16 cryptocaps;
 	__be16 iscsicaps;
 	__be16 fcoecaps;
 	__be32 cfcsum;
@@ -1119,6 +1122,7 @@ enum fw_params_param_dev {
 	FW_PARAMS_PARAM_DEV_MAXIRD_ADAPTER = 0x14, /* max supported adap IRD */
 	FW_PARAMS_PARAM_DEV_ULPTX_MEMWRITE_DSGL = 0x17,
 	FW_PARAMS_PARAM_DEV_FWCACHE = 0x18,
+	FW_PARAMS_PARAM_DEV_RI_FR_NSMR_TPTE_WR	= 0x1C,
 };
 
 /*
@@ -2967,6 +2971,41 @@ struct fw_rss_vi_config_cmd {
 #define FW_RSS_VI_CONFIG_CMD_UDPEN_V(x)	((x) << FW_RSS_VI_CONFIG_CMD_UDPEN_S)
 #define FW_RSS_VI_CONFIG_CMD_UDPEN_F	FW_RSS_VI_CONFIG_CMD_UDPEN_V(1U)
 
+enum fw_sched_sc {
+	FW_SCHED_SC_PARAMS		= 1,
+};
+
+struct fw_sched_cmd {
+	__be32 op_to_write;
+	__be32 retval_len16;
+	union fw_sched {
+		struct fw_sched_config {
+			__u8   sc;
+			__u8   type;
+			__u8   minmaxen;
+			__u8   r3[5];
+			__u8   nclasses[4];
+			__be32 r4;
+		} config;
+		struct fw_sched_params {
+			__u8   sc;
+			__u8   type;
+			__u8   level;
+			__u8   mode;
+			__u8   unit;
+			__u8   rate;
+			__u8   ch;
+			__u8   cl;
+			__be32 min;
+			__be32 max;
+			__be16 weight;
+			__be16 pktsize;
+			__be16 burstsize;
+			__be16 r4;
+		} params;
+	} u;
+};
+
 struct fw_clip_cmd {
 	__be32 op_to_write;
 	__be32 alloc_to_len16;
@@ -3255,4 +3294,127 @@ struct fw_devlog_cmd {
 #define PCIE_FW_PF_DEVLOG_MEMTYPE_G(x) \
 	(((x) >> PCIE_FW_PF_DEVLOG_MEMTYPE_S) & PCIE_FW_PF_DEVLOG_MEMTYPE_M)
 
+#define MAX_IMM_OFLD_TX_DATA_WR_LEN (0xff + sizeof(struct fw_ofld_tx_data_wr))
+
+struct fw_crypto_lookaside_wr {
+	__be32 op_to_cctx_size;
+	__be32 len16_pkd;
+	__be32 session_id;
+	__be32 rx_chid_to_rx_q_id;
+	__be32 key_addr;
+	__be32 pld_size_hash_size;
+	__be64 cookie;
+};
+
+#define FW_CRYPTO_LOOKASIDE_WR_OPCODE_S 24
+#define FW_CRYPTO_LOOKASIDE_WR_OPCODE_M 0xff
+#define FW_CRYPTO_LOOKASIDE_WR_OPCODE_V(x) \
+	((x) << FW_CRYPTO_LOOKASIDE_WR_OPCODE_S)
+#define FW_CRYPTO_LOOKASIDE_WR_OPCODE_G(x) \
+	(((x) >> FW_CRYPTO_LOOKASIDE_WR_OPCODE_S) & \
+	 FW_CRYPTO_LOOKASIDE_WR_OPCODE_M)
+
+#define FW_CRYPTO_LOOKASIDE_WR_COMPL_S 23
+#define FW_CRYPTO_LOOKASIDE_WR_COMPL_M 0x1
+#define FW_CRYPTO_LOOKASIDE_WR_COMPL_V(x) \
+	((x) << FW_CRYPTO_LOOKASIDE_WR_COMPL_S)
+#define FW_CRYPTO_LOOKASIDE_WR_COMPL_G(x) \
+	(((x) >> FW_CRYPTO_LOOKASIDE_WR_COMPL_S) & \
+	 FW_CRYPTO_LOOKASIDE_WR_COMPL_M)
+#define FW_CRYPTO_LOOKASIDE_WR_COMPL_F FW_CRYPTO_LOOKASIDE_WR_COMPL_V(1U)
+
+#define FW_CRYPTO_LOOKASIDE_WR_IMM_LEN_S 15
+#define FW_CRYPTO_LOOKASIDE_WR_IMM_LEN_M 0xff
+#define FW_CRYPTO_LOOKASIDE_WR_IMM_LEN_V(x) \
+	((x) << FW_CRYPTO_LOOKASIDE_WR_IMM_LEN_S)
+#define FW_CRYPTO_LOOKASIDE_WR_IMM_LEN_G(x) \
+	(((x) >> FW_CRYPTO_LOOKASIDE_WR_IMM_LEN_S) & \
+	 FW_CRYPTO_LOOKASIDE_WR_IMM_LEN_M)
+
+#define FW_CRYPTO_LOOKASIDE_WR_CCTX_LOC_S 5
+#define FW_CRYPTO_LOOKASIDE_WR_CCTX_LOC_M 0x3
+#define FW_CRYPTO_LOOKASIDE_WR_CCTX_LOC_V(x) \
+	((x) << FW_CRYPTO_LOOKASIDE_WR_CCTX_LOC_S)
+#define FW_CRYPTO_LOOKASIDE_WR_CCTX_LOC_G(x) \
+	(((x) >> FW_CRYPTO_LOOKASIDE_WR_CCTX_LOC_S) & \
+	 FW_CRYPTO_LOOKASIDE_WR_CCTX_LOC_M)
+
+#define FW_CRYPTO_LOOKASIDE_WR_CCTX_SIZE_S 0
+#define FW_CRYPTO_LOOKASIDE_WR_CCTX_SIZE_M 0x1f
+#define FW_CRYPTO_LOOKASIDE_WR_CCTX_SIZE_V(x) \
+	((x) << FW_CRYPTO_LOOKASIDE_WR_CCTX_SIZE_S)
+#define FW_CRYPTO_LOOKASIDE_WR_CCTX_SIZE_G(x) \
+	(((x) >> FW_CRYPTO_LOOKASIDE_WR_CCTX_SIZE_S) & \
+	 FW_CRYPTO_LOOKASIDE_WR_CCTX_SIZE_M)
+
+#define FW_CRYPTO_LOOKASIDE_WR_LEN16_S 0
+#define FW_CRYPTO_LOOKASIDE_WR_LEN16_M 0xff
+#define FW_CRYPTO_LOOKASIDE_WR_LEN16_V(x) \
+	((x) << FW_CRYPTO_LOOKASIDE_WR_LEN16_S)
+#define FW_CRYPTO_LOOKASIDE_WR_LEN16_G(x) \
+	(((x) >> FW_CRYPTO_LOOKASIDE_WR_LEN16_S) & \
+	 FW_CRYPTO_LOOKASIDE_WR_LEN16_M)
+
+#define FW_CRYPTO_LOOKASIDE_WR_RX_CHID_S 29
+#define FW_CRYPTO_LOOKASIDE_WR_RX_CHID_M 0x3
+#define FW_CRYPTO_LOOKASIDE_WR_RX_CHID_V(x) \
+	((x) << FW_CRYPTO_LOOKASIDE_WR_RX_CHID_S)
+#define FW_CRYPTO_LOOKASIDE_WR_RX_CHID_G(x) \
+	(((x) >> FW_CRYPTO_LOOKASIDE_WR_RX_CHID_S) & \
+	 FW_CRYPTO_LOOKASIDE_WR_RX_CHID_M)
+
+#define FW_CRYPTO_LOOKASIDE_WR_LCB_S  27
+#define FW_CRYPTO_LOOKASIDE_WR_LCB_M  0x3
+#define FW_CRYPTO_LOOKASIDE_WR_LCB_V(x) \
+	((x) << FW_CRYPTO_LOOKASIDE_WR_LCB_S)
+#define FW_CRYPTO_LOOKASIDE_WR_LCB_G(x) \
+	(((x) >> FW_CRYPTO_LOOKASIDE_WR_LCB_S) & FW_CRYPTO_LOOKASIDE_WR_LCB_M)
+
+#define FW_CRYPTO_LOOKASIDE_WR_PHASH_S 25
+#define FW_CRYPTO_LOOKASIDE_WR_PHASH_M 0x3
+#define FW_CRYPTO_LOOKASIDE_WR_PHASH_V(x) \
+	((x) << FW_CRYPTO_LOOKASIDE_WR_PHASH_S)
+#define FW_CRYPTO_LOOKASIDE_WR_PHASH_G(x) \
+	(((x) >> FW_CRYPTO_LOOKASIDE_WR_PHASH_S) & \
+	 FW_CRYPTO_LOOKASIDE_WR_PHASH_M)
+
+#define FW_CRYPTO_LOOKASIDE_WR_IV_S   23
+#define FW_CRYPTO_LOOKASIDE_WR_IV_M   0x3
+#define FW_CRYPTO_LOOKASIDE_WR_IV_V(x) \
+	((x) << FW_CRYPTO_LOOKASIDE_WR_IV_S)
+#define FW_CRYPTO_LOOKASIDE_WR_IV_G(x) \
+	(((x) >> FW_CRYPTO_LOOKASIDE_WR_IV_S) & FW_CRYPTO_LOOKASIDE_WR_IV_M)
+
+#define FW_CRYPTO_LOOKASIDE_WR_TX_CH_S 10
+#define FW_CRYPTO_LOOKASIDE_WR_TX_CH_M 0x3
+#define FW_CRYPTO_LOOKASIDE_WR_TX_CH_V(x) \
+	((x) << FW_CRYPTO_LOOKASIDE_WR_TX_CH_S)
+#define FW_CRYPTO_LOOKASIDE_WR_TX_CH_G(x) \
+	(((x) >> FW_CRYPTO_LOOKASIDE_WR_TX_CH_S) & \
+	 FW_CRYPTO_LOOKASIDE_WR_TX_CH_M)
+
+#define FW_CRYPTO_LOOKASIDE_WR_RX_Q_ID_S 0
+#define FW_CRYPTO_LOOKASIDE_WR_RX_Q_ID_M 0x3ff
+#define FW_CRYPTO_LOOKASIDE_WR_RX_Q_ID_V(x) \
+	((x) << FW_CRYPTO_LOOKASIDE_WR_RX_Q_ID_S)
+#define FW_CRYPTO_LOOKASIDE_WR_RX_Q_ID_G(x) \
+	(((x) >> FW_CRYPTO_LOOKASIDE_WR_RX_Q_ID_S) & \
+	 FW_CRYPTO_LOOKASIDE_WR_RX_Q_ID_M)
+
+#define FW_CRYPTO_LOOKASIDE_WR_PLD_SIZE_S 24
+#define FW_CRYPTO_LOOKASIDE_WR_PLD_SIZE_M 0xff
+#define FW_CRYPTO_LOOKASIDE_WR_PLD_SIZE_V(x) \
+	((x) << FW_CRYPTO_LOOKASIDE_WR_PLD_SIZE_S)
+#define FW_CRYPTO_LOOKASIDE_WR_PLD_SIZE_G(x) \
+	(((x) >> FW_CRYPTO_LOOKASIDE_WR_PLD_SIZE_S) & \
+	 FW_CRYPTO_LOOKASIDE_WR_PLD_SIZE_M)
+
+#define FW_CRYPTO_LOOKASIDE_WR_HASH_SIZE_S 17
+#define FW_CRYPTO_LOOKASIDE_WR_HASH_SIZE_M 0x7f
+#define FW_CRYPTO_LOOKASIDE_WR_HASH_SIZE_V(x) \
+	((x) << FW_CRYPTO_LOOKASIDE_WR_HASH_SIZE_S)
+#define FW_CRYPTO_LOOKASIDE_WR_HASH_SIZE_G(x) \
+	(((x) >> FW_CRYPTO_LOOKASIDE_WR_HASH_SIZE_S) & \
+	 FW_CRYPTO_LOOKASIDE_WR_HASH_SIZE_M)
+
 #endif /* _T4FW_INTERFACE_H_ */
diff --git a/drivers/net/ethernet/chelsio/cxgb4vf/cxgb4vf_main.c b/drivers/net/ethernet/chelsio/cxgb4vf/cxgb4vf_main.c
index e116bb8d1729..100b2cc064a3 100644
--- a/drivers/net/ethernet/chelsio/cxgb4vf/cxgb4vf_main.c
+++ b/drivers/net/ethernet/chelsio/cxgb4vf/cxgb4vf_main.c
@@ -2378,7 +2378,7 @@ static void size_nports_qsets(struct adapter *adapter)
 	 */
 	pmask_nports = hweight32(adapter->params.vfres.pmask);
 	if (pmask_nports < adapter->params.nports) {
-		dev_warn(adapter->pdev_dev, "only using %d of %d provissioned"
+		dev_warn(adapter->pdev_dev, "only using %d of %d provisioned"
 			 " virtual interfaces; limited by Port Access Rights"
 			 " mask %#x\n", pmask_nports, adapter->params.nports,
 			 adapter->params.vfres.pmask);
@@ -2777,6 +2777,7 @@ static int cxgb4vf_pci_probe(struct pci_dev *pdev,
 	struct adapter *adapter;
 	struct port_info *pi;
 	struct net_device *netdev;
+	unsigned int pf;
 
 	/*
 	 * Print our driver banner the first time we're called to initialize a
@@ -2903,8 +2904,11 @@ static int cxgb4vf_pci_probe(struct pci_dev *pdev,
 	 * Allocate our "adapter ports" and stitch everything together.
 	 */
 	pmask = adapter->params.vfres.pmask;
+	pf = t4vf_get_pf_from_vf(adapter);
 	for_each_port(adapter, pidx) {
 		int port_id, viid;
+		u8 mac[ETH_ALEN];
+		unsigned int naddr = 1;
 
 		/*
 		 * We simplistically allocate our virtual interfaces
@@ -2975,6 +2979,26 @@ static int cxgb4vf_pci_probe(struct pci_dev *pdev,
 				pidx);
 			goto err_free_dev;
 		}
+
+		err = t4vf_get_vf_mac_acl(adapter, pf, &naddr, mac);
+		if (err) {
+			dev_err(&pdev->dev,
+				"unable to determine MAC ACL address, "
+				"continuing anyway.. (status %d)\n", err);
+		} else if (naddr && adapter->params.vfres.nvi == 1) {
+			struct sockaddr addr;
+
+			ether_addr_copy(addr.sa_data, mac);
+			err = cxgb4vf_set_mac_addr(netdev, &addr);
+			if (err) {
+				dev_err(&pdev->dev,
+					"unable to set MAC address %pM\n",
+					mac);
+				goto err_free_dev;
+			}
+			dev_info(&pdev->dev,
+				 "Using assigned MAC ACL: %pM\n", mac);
+		}
 	}
 
 	/* See what interrupts we'll be using.  If we've been configured to
diff --git a/drivers/net/ethernet/chelsio/cxgb4vf/sge.c b/drivers/net/ethernet/chelsio/cxgb4vf/sge.c
index c8fd4f8fe1fa..f3ed9ce99e5e 100644
--- a/drivers/net/ethernet/chelsio/cxgb4vf/sge.c
+++ b/drivers/net/ethernet/chelsio/cxgb4vf/sge.c
@@ -1648,14 +1648,15 @@ int t4vf_ethrx_handler(struct sge_rspq *rspq, const __be64 *rsp,
 
 	if (csum_ok && !pkt->err_vec &&
 	    (be32_to_cpu(pkt->l2info) & (RXF_UDP_F | RXF_TCP_F))) {
-		if (!pkt->ip_frag)
+		if (!pkt->ip_frag) {
 			skb->ip_summed = CHECKSUM_UNNECESSARY;
-		else {
+			rxq->stats.rx_cso++;
+		} else if (pkt->l2info & htonl(RXF_IP_F)) {
 			__sum16 c = (__force __sum16)pkt->csum;
 			skb->csum = csum_unfold(c);
 			skb->ip_summed = CHECKSUM_COMPLETE;
+			rxq->stats.rx_cso++;
 		}
-		rxq->stats.rx_cso++;
 	} else
 		skb_checksum_none_assert(skb);
 
diff --git a/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_common.h b/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_common.h
index 17a2bbcf93f0..b3903fe411aa 100644
--- a/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_common.h
+++ b/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_common.h
@@ -354,6 +354,7 @@ int t4vf_bar2_sge_qregs(struct adapter *adapter,
 			u64 *pbar2_qoffset,
 			unsigned int *pbar2_qid);
 
+unsigned int t4vf_get_pf_from_vf(struct adapter *);
 int t4vf_get_sge_params(struct adapter *);
 int t4vf_get_vpd_params(struct adapter *);
 int t4vf_get_dev_params(struct adapter *);
@@ -388,5 +389,7 @@ int t4vf_eth_eq_free(struct adapter *, unsigned int);
 
 int t4vf_handle_fw_rpl(struct adapter *, const __be64 *);
 int t4vf_prep_adapter(struct adapter *);
+int t4vf_get_vf_mac_acl(struct adapter *adapter, unsigned int pf,
+			unsigned int *naddr, u8 *addr);
 
 #endif /* __T4VF_COMMON_H__ */
diff --git a/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_hw.c b/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_hw.c
index b5622b1689e9..e98248f00fef 100644
--- a/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_hw.c
+++ b/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_hw.c
@@ -640,6 +640,15 @@ int t4vf_bar2_sge_qregs(struct adapter *adapter,
 	return 0;
 }
 
+unsigned int t4vf_get_pf_from_vf(struct adapter *adapter)
+{
+	u32 whoami;
+
+	whoami = t4_read_reg(adapter, T4VF_PL_BASE_ADDR + PL_VF_WHOAMI_A);
+	return (CHELSIO_CHIP_VERSION(adapter->params.chip) <= CHELSIO_T5 ?
+			SOURCEPF_G(whoami) : T6_SOURCEPF_G(whoami));
+}
+
 /**
  *	t4vf_get_sge_params - retrieve adapter Scatter gather Engine parameters
  *	@adapter: the adapter
@@ -717,7 +726,6 @@ int t4vf_get_sge_params(struct adapter *adapter)
 	 * read.
 	 */
 	if (!is_t4(adapter->params.chip)) {
-		u32 whoami;
 		unsigned int pf, s_hps, s_qpp;
 
 		params[0] = (FW_PARAMS_MNEM_V(FW_PARAMS_MNEM_REG) |
@@ -741,11 +749,7 @@ int t4vf_get_sge_params(struct adapter *adapter)
 		 * register we just read. Do it once here so other code in
 		 * the driver can just use it.
 		 */
-		whoami = t4_read_reg(adapter,
-				     T4VF_PL_BASE_ADDR + PL_VF_WHOAMI_A);
-		pf = CHELSIO_CHIP_VERSION(adapter->params.chip) <= CHELSIO_T5 ?
-			SOURCEPF_G(whoami) : T6_SOURCEPF_G(whoami);
-
+		pf = t4vf_get_pf_from_vf(adapter);
 		s_hps = (HOSTPAGESIZEPF0_S +
 			 (HOSTPAGESIZEPF1_S - HOSTPAGESIZEPF0_S) * pf);
 		sge_params->sge_vf_hps =
@@ -1812,3 +1816,50 @@ int t4vf_prep_adapter(struct adapter *adapter)
 
 	return 0;
 }
+
+/**
+ *	t4vf_get_vf_mac_acl - Get the MAC address to be set to
+ *			      the VI of this VF.
+ *	@adapter: The adapter
+ *	@pf: The pf associated with vf
+ *	@naddr: the number of ACL MAC addresses returned in addr
+ *	@addr: Placeholder for MAC addresses
+ *
+ *	Find the MAC address to be set to the VF's VI. The requested MAC address
+ *	is from the host OS via callback in the PF driver.
+ */
+int t4vf_get_vf_mac_acl(struct adapter *adapter, unsigned int pf,
+			unsigned int *naddr, u8 *addr)
+{
+	struct fw_acl_mac_cmd cmd;
+	int ret;
+
+	memset(&cmd, 0, sizeof(cmd));
+	cmd.op_to_vfn = cpu_to_be32(FW_CMD_OP_V(FW_ACL_MAC_CMD) |
+				    FW_CMD_REQUEST_F |
+				    FW_CMD_READ_F);
+	cmd.en_to_len16 = cpu_to_be32((unsigned int)FW_LEN16(cmd));
+	ret = t4vf_wr_mbox(adapter, &cmd, sizeof(cmd), &cmd);
+	if (ret)
+		return ret;
+
+	if (cmd.nmac < *naddr)
+		*naddr = cmd.nmac;
+
+	switch (pf) {
+	case 3:
+		memcpy(addr, cmd.macaddr3, sizeof(cmd.macaddr3));
+		break;
+	case 2:
+		memcpy(addr, cmd.macaddr2, sizeof(cmd.macaddr2));
+		break;
+	case 1:
+		memcpy(addr, cmd.macaddr1, sizeof(cmd.macaddr1));
+		break;
+	case 0:
+		memcpy(addr, cmd.macaddr0, sizeof(cmd.macaddr0));
+		break;
+	}
+
+	return ret;
+}
diff --git a/drivers/net/ethernet/chelsio/libcxgb/Makefile b/drivers/net/ethernet/chelsio/libcxgb/Makefile
index 2362230ef4fe..2534e30a1560 100644
--- a/drivers/net/ethernet/chelsio/libcxgb/Makefile
+++ b/drivers/net/ethernet/chelsio/libcxgb/Makefile
@@ -1,3 +1,5 @@
+ccflags-y := -Idrivers/net/ethernet/chelsio/cxgb4
+
 obj-$(CONFIG_CHELSIO_LIB) += libcxgb.o
 
-libcxgb-y := libcxgb_ppm.o
+libcxgb-y := libcxgb_ppm.o libcxgb_cm.o
diff --git a/drivers/net/ethernet/chelsio/libcxgb/libcxgb_cm.c b/drivers/net/ethernet/chelsio/libcxgb/libcxgb_cm.c
new file mode 100644
index 000000000000..0f0de5b63622
--- /dev/null
+++ b/drivers/net/ethernet/chelsio/libcxgb/libcxgb_cm.c
@@ -0,0 +1,149 @@
+/*
+ * Copyright (c) 2016 Chelsio Communications, Inc. All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * 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/ipv6.h>
+#include <net/route.h>
+#include <net/ip6_route.h>
+
+#include "libcxgb_cm.h"
+
+void
+cxgb_get_4tuple(struct cpl_pass_accept_req *req, enum chip_type type,
+		int *iptype, __u8 *local_ip, __u8 *peer_ip,
+		__be16 *local_port, __be16 *peer_port)
+{
+	int eth_len = (CHELSIO_CHIP_VERSION(type) <= CHELSIO_T5) ?
+		      ETH_HDR_LEN_G(be32_to_cpu(req->hdr_len)) :
+		      T6_ETH_HDR_LEN_G(be32_to_cpu(req->hdr_len));
+	int ip_len = (CHELSIO_CHIP_VERSION(type) <= CHELSIO_T5) ?
+		     IP_HDR_LEN_G(be32_to_cpu(req->hdr_len)) :
+		     T6_IP_HDR_LEN_G(be32_to_cpu(req->hdr_len));
+	struct iphdr *ip = (struct iphdr *)((u8 *)(req + 1) + eth_len);
+	struct ipv6hdr *ip6 = (struct ipv6hdr *)((u8 *)(req + 1) + eth_len);
+	struct tcphdr *tcp = (struct tcphdr *)
+			     ((u8 *)(req + 1) + eth_len + ip_len);
+
+	if (ip->version == 4) {
+		pr_debug("%s saddr 0x%x daddr 0x%x sport %u dport %u\n",
+			 __func__, ntohl(ip->saddr), ntohl(ip->daddr),
+			 ntohs(tcp->source), ntohs(tcp->dest));
+		*iptype = 4;
+		memcpy(peer_ip, &ip->saddr, 4);
+		memcpy(local_ip, &ip->daddr, 4);
+	} else {
+		pr_debug("%s saddr %pI6 daddr %pI6 sport %u dport %u\n",
+			 __func__, ip6->saddr.s6_addr, ip6->daddr.s6_addr,
+			 ntohs(tcp->source), ntohs(tcp->dest));
+		*iptype = 6;
+		memcpy(peer_ip, ip6->saddr.s6_addr, 16);
+		memcpy(local_ip, ip6->daddr.s6_addr, 16);
+	}
+	*peer_port = tcp->source;
+	*local_port = tcp->dest;
+}
+EXPORT_SYMBOL(cxgb_get_4tuple);
+
+static bool
+cxgb_our_interface(struct cxgb4_lld_info *lldi,
+		   struct net_device *(*get_real_dev)(struct net_device *),
+		   struct net_device *egress_dev)
+{
+	int i;
+
+	egress_dev = get_real_dev(egress_dev);
+	for (i = 0; i < lldi->nports; i++)
+		if (lldi->ports[i] == egress_dev)
+			return true;
+	return false;
+}
+
+struct dst_entry *
+cxgb_find_route(struct cxgb4_lld_info *lldi,
+		struct net_device *(*get_real_dev)(struct net_device *),
+		__be32 local_ip, __be32 peer_ip, __be16 local_port,
+		__be16 peer_port, u8 tos)
+{
+	struct rtable *rt;
+	struct flowi4 fl4;
+	struct neighbour *n;
+
+	rt = ip_route_output_ports(&init_net, &fl4, NULL, peer_ip, local_ip,
+				   peer_port, local_port, IPPROTO_TCP,
+				   tos, 0);
+	if (IS_ERR(rt))
+		return NULL;
+	n = dst_neigh_lookup(&rt->dst, &peer_ip);
+	if (!n)
+		return NULL;
+	if (!cxgb_our_interface(lldi, get_real_dev, n->dev) &&
+	    !(n->dev->flags & IFF_LOOPBACK)) {
+		neigh_release(n);
+		dst_release(&rt->dst);
+		return NULL;
+	}
+	neigh_release(n);
+	return &rt->dst;
+}
+EXPORT_SYMBOL(cxgb_find_route);
+
+struct dst_entry *
+cxgb_find_route6(struct cxgb4_lld_info *lldi,
+		 struct net_device *(*get_real_dev)(struct net_device *),
+		 __u8 *local_ip, __u8 *peer_ip, __be16 local_port,
+		 __be16 peer_port, u8 tos, __u32 sin6_scope_id)
+{
+	struct dst_entry *dst = NULL;
+
+	if (IS_ENABLED(CONFIG_IPV6)) {
+		struct flowi6 fl6;
+
+		memset(&fl6, 0, sizeof(fl6));
+		memcpy(&fl6.daddr, peer_ip, 16);
+		memcpy(&fl6.saddr, local_ip, 16);
+		if (ipv6_addr_type(&fl6.daddr) & IPV6_ADDR_LINKLOCAL)
+			fl6.flowi6_oif = sin6_scope_id;
+		dst = ip6_route_output(&init_net, NULL, &fl6);
+		if (!dst)
+			goto out;
+		if (!cxgb_our_interface(lldi, get_real_dev,
+					ip6_dst_idev(dst)->dev) &&
+		    !(ip6_dst_idev(dst)->dev->flags & IFF_LOOPBACK)) {
+			dst_release(dst);
+			dst = NULL;
+		}
+	}
+
+out:
+	return dst;
+}
+EXPORT_SYMBOL(cxgb_find_route6);
diff --git a/drivers/net/ethernet/chelsio/libcxgb/libcxgb_cm.h b/drivers/net/ethernet/chelsio/libcxgb/libcxgb_cm.h
new file mode 100644
index 000000000000..515b94ff9080
--- /dev/null
+++ b/drivers/net/ethernet/chelsio/libcxgb/libcxgb_cm.h
@@ -0,0 +1,160 @@
+/*
+ * Copyright (c) 2016 Chelsio Communications, Inc. All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * 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 __LIBCXGB_CM_H__
+#define __LIBCXGB_CM_H__
+
+
+#include <net/tcp.h>
+
+#include <cxgb4.h>
+#include <t4_msg.h>
+#include <l2t.h>
+
+void
+cxgb_get_4tuple(struct cpl_pass_accept_req *, enum chip_type,
+		int *, __u8 *, __u8 *, __be16 *, __be16 *);
+struct dst_entry *
+cxgb_find_route(struct cxgb4_lld_info *,
+		struct net_device *(*)(struct net_device *),
+		__be32, __be32, __be16,	__be16, u8);
+struct dst_entry *
+cxgb_find_route6(struct cxgb4_lld_info *,
+		 struct net_device *(*)(struct net_device *),
+		 __u8 *, __u8 *, __be16, __be16, u8, __u32);
+
+/* Returns whether a CPL status conveys negative advice.
+ */
+static inline bool cxgb_is_neg_adv(unsigned int status)
+{
+	return status == CPL_ERR_RTX_NEG_ADVICE ||
+	       status == CPL_ERR_PERSIST_NEG_ADVICE ||
+	       status == CPL_ERR_KEEPALV_NEG_ADVICE;
+}
+
+static inline void
+cxgb_best_mtu(const unsigned short *mtus, unsigned short mtu,
+	      unsigned int *idx, int use_ts, int ipv6)
+{
+	unsigned short hdr_size = (ipv6 ?
+				   sizeof(struct ipv6hdr) :
+				   sizeof(struct iphdr)) +
+				  sizeof(struct tcphdr) +
+				  (use_ts ?
+				   round_up(TCPOLEN_TIMESTAMP, 4) : 0);
+	unsigned short data_size = mtu - hdr_size;
+
+	cxgb4_best_aligned_mtu(mtus, hdr_size, data_size, 8, idx);
+}
+
+static inline u32 cxgb_compute_wscale(u32 win)
+{
+	u32 wscale = 0;
+
+	while (wscale < 14 && (65535 << wscale) < win)
+		wscale++;
+	return wscale;
+}
+
+static inline void
+cxgb_mk_tid_release(struct sk_buff *skb, u32 len, u32 tid, u16 chan)
+{
+	struct cpl_tid_release *req;
+
+	req = (struct cpl_tid_release *)__skb_put(skb, len);
+	memset(req, 0, len);
+
+	INIT_TP_WR(req, tid);
+	OPCODE_TID(req) = cpu_to_be32(MK_OPCODE_TID(CPL_TID_RELEASE, tid));
+	set_wr_txq(skb, CPL_PRIORITY_SETUP, chan);
+}
+
+static inline void
+cxgb_mk_close_con_req(struct sk_buff *skb, u32 len, u32 tid, u16 chan,
+		      void *handle, arp_err_handler_t handler)
+{
+	struct cpl_close_con_req *req;
+
+	req = (struct cpl_close_con_req *)__skb_put(skb, len);
+	memset(req, 0, len);
+
+	INIT_TP_WR(req, tid);
+	OPCODE_TID(req) = cpu_to_be32(MK_OPCODE_TID(CPL_CLOSE_CON_REQ, tid));
+	set_wr_txq(skb, CPL_PRIORITY_DATA, chan);
+	t4_set_arp_err_handler(skb, handle, handler);
+}
+
+static inline void
+cxgb_mk_abort_req(struct sk_buff *skb, u32 len, u32 tid, u16 chan,
+		  void *handle, arp_err_handler_t handler)
+{
+	struct cpl_abort_req *req;
+
+	req = (struct cpl_abort_req *)__skb_put(skb, len);
+	memset(req, 0, len);
+
+	INIT_TP_WR(req, tid);
+	OPCODE_TID(req) = cpu_to_be32(MK_OPCODE_TID(CPL_ABORT_REQ, tid));
+	req->cmd = CPL_ABORT_SEND_RST;
+	set_wr_txq(skb, CPL_PRIORITY_DATA, chan);
+	t4_set_arp_err_handler(skb, handle, handler);
+}
+
+static inline void
+cxgb_mk_abort_rpl(struct sk_buff *skb, u32 len, u32 tid, u16 chan)
+{
+	struct cpl_abort_rpl *rpl;
+
+	rpl = (struct cpl_abort_rpl *)__skb_put(skb, len);
+	memset(rpl, 0, len);
+
+	INIT_TP_WR(rpl, tid);
+	OPCODE_TID(rpl) = cpu_to_be32(MK_OPCODE_TID(CPL_ABORT_RPL, tid));
+	rpl->cmd = CPL_ABORT_NO_RST;
+	set_wr_txq(skb, CPL_PRIORITY_DATA, chan);
+}
+
+static inline void
+cxgb_mk_rx_data_ack(struct sk_buff *skb, u32 len, u32 tid, u16 chan,
+		    u32 credit_dack)
+{
+	struct cpl_rx_data_ack *req;
+
+	req = (struct cpl_rx_data_ack *)__skb_put(skb, len);
+	memset(req, 0, len);
+
+	INIT_TP_WR(req, tid);
+	OPCODE_TID(req) = cpu_to_be32(MK_OPCODE_TID(CPL_RX_DATA_ACK, tid));
+	req->credit_dack = cpu_to_be32(credit_dack);
+	set_wr_txq(skb, CPL_PRIORITY_ACK, chan);
+}
+#endif
diff --git a/drivers/net/ethernet/dec/tulip/de4x5.c b/drivers/net/ethernet/dec/tulip/de4x5.c
index f0e9e2ef62a0..6620fc861c47 100644
--- a/drivers/net/ethernet/dec/tulip/de4x5.c
+++ b/drivers/net/ethernet/dec/tulip/de4x5.c
@@ -1966,7 +1966,7 @@ SetMulticastFilter(struct net_device *dev)
     } else if (lp->setup_f == HASH_PERF) {   /* Hash Filtering */
 	netdev_for_each_mc_addr(ha, dev) {
 		crc = ether_crc_le(ETH_ALEN, ha->addr);
-		hashcode = crc & HASH_BITS;  /* hashcode is 9 LSb of CRC */
+		hashcode = crc & DE4X5_HASH_BITS;  /* hashcode is 9 LSb of CRC */
 
 		byte = hashcode >> 3;        /* bit[3-8] -> byte in filter */
 		bit = 1 << (hashcode & 0x07);/* bit[0-2] -> bit in byte */
@@ -5043,7 +5043,7 @@ build_setup_frame(struct net_device *dev, int mode)
 	    *(pa + i) = dev->dev_addr[i];                 /* Host address */
 	    if (i & 0x01) pa += 2;
 	}
-	*(lp->setup_frame + (HASH_TABLE_LEN >> 3) - 3) = 0x80;
+	*(lp->setup_frame + (DE4X5_HASH_TABLE_LEN >> 3) - 3) = 0x80;
     } else {
 	for (i=0; i<ETH_ALEN; i++) { /* Host address */
 	    *(pa + (i&1)) = dev->dev_addr[i];
diff --git a/drivers/net/ethernet/dec/tulip/de4x5.h b/drivers/net/ethernet/dec/tulip/de4x5.h
index ec756eba397b..1bfdc9b117f6 100644
--- a/drivers/net/ethernet/dec/tulip/de4x5.h
+++ b/drivers/net/ethernet/dec/tulip/de4x5.h
@@ -860,8 +860,8 @@
 #define PCI  0
 #define EISA 1
 
-#define HASH_TABLE_LEN   512       /* Bits */
-#define HASH_BITS        0x01ff    /* 9 LS bits */
+#define DE4X5_HASH_TABLE_LEN   512       /* Bits */
+#define DE4X5_HASH_BITS        0x01ff    /* 9 LS bits */
 
 #define SETUP_FRAME_LEN  192       /* Bytes */
 #define IMPERF_PA_OFFSET 156       /* Bytes */
diff --git a/drivers/net/ethernet/dlink/sundance.c b/drivers/net/ethernet/dlink/sundance.c
index 58c6338a839e..79d80090eac8 100644
--- a/drivers/net/ethernet/dlink/sundance.c
+++ b/drivers/net/ethernet/dlink/sundance.c
@@ -867,7 +867,7 @@ static int netdev_open(struct net_device *dev)
 
 	/* Initialize other registers. */
 	__set_mac_addr(dev);
-#if defined(CONFIG_VLAN_8021Q) || defined(CONFIG_VLAN_8021Q_MODULE)
+#if IS_ENABLED(CONFIG_VLAN_8021Q)
 	iowrite16(dev->mtu + 18, ioaddr + MaxFrameSize);
 #else
 	iowrite16(dev->mtu + 14, ioaddr + MaxFrameSize);
diff --git a/drivers/net/ethernet/emulex/benet/be.h b/drivers/net/ethernet/emulex/benet/be.h
index 4555e041ef69..6cfa63a5e9b4 100644
--- a/drivers/net/ethernet/emulex/benet/be.h
+++ b/drivers/net/ethernet/emulex/benet/be.h
@@ -37,7 +37,7 @@
 #include "be_hw.h"
 #include "be_roce.h"
 
-#define DRV_VER			"11.0.0.0"
+#define DRV_VER			"11.1.0.0"
 #define DRV_NAME		"be2net"
 #define BE_NAME			"Emulex BladeEngine2"
 #define BE3_NAME		"Emulex BladeEngine3"
@@ -399,13 +399,13 @@ enum vf_state {
 #define BE_FLAGS_PHY_MISCONFIGURED		BIT(10)
 #define BE_FLAGS_ERR_DETECTION_SCHEDULED	BIT(11)
 #define BE_FLAGS_OS2BMC				BIT(12)
+#define BE_FLAGS_TRY_RECOVERY			BIT(13)
 
 #define BE_UC_PMAC_COUNT			30
 #define BE_VF_UC_PMAC_COUNT			2
 
 #define MAX_ERR_RECOVERY_RETRY_COUNT		3
 #define ERR_DETECTION_DELAY			1000
-#define ERR_RECOVERY_RETRY_DELAY		30000
 
 /* Ethtool set_dump flags */
 #define LANCER_INITIATE_FW_DUMP			0x1
@@ -508,6 +508,70 @@ struct be_wrb_params {
 	u16 lso_mss;	/* MSS for LSO */
 };
 
+struct be_eth_addr {
+	unsigned char mac[ETH_ALEN];
+};
+
+#define BE_SEC	1000			/* in msec */
+#define BE_MIN	(60 * BE_SEC)		/* in msec */
+#define BE_HOUR	(60 * BE_MIN)		/* in msec */
+
+#define ERR_RECOVERY_MAX_RETRY_COUNT		3
+#define ERR_RECOVERY_DETECTION_DELAY		BE_SEC
+#define ERR_RECOVERY_RETRY_DELAY		(30 * BE_SEC)
+
+/* UE-detection-duration in BEx/Skyhawk:
+ * All PFs must wait for this duration after they detect UE before reading
+ * SLIPORT_SEMAPHORE register. At the end of this duration, the Firmware
+ * guarantees that the SLIPORT_SEMAPHORE register is updated to indicate
+ * if the UE is recoverable.
+ */
+#define ERR_RECOVERY_UE_DETECT_DURATION			BE_SEC
+
+/* Initial idle time (in msec) to elapse after driver load,
+ * before UE recovery is allowed.
+ */
+#define ERR_IDLE_HR			24
+#define ERR_RECOVERY_IDLE_TIME		(ERR_IDLE_HR * BE_HOUR)
+
+/* Time interval (in msec) after which UE recovery can be repeated */
+#define ERR_INTERVAL_HR			72
+#define ERR_RECOVERY_INTERVAL		(ERR_INTERVAL_HR * BE_HOUR)
+
+/* BEx/SH UE recovery state machine */
+enum {
+	ERR_RECOVERY_ST_NONE = 0,		/* No Recovery */
+	ERR_RECOVERY_ST_DETECT = 1,		/* UE detection duration */
+	ERR_RECOVERY_ST_RESET = 2,		/* Reset Phase (PF0 only) */
+	ERR_RECOVERY_ST_PRE_POLL = 3,		/* Pre-Poll Phase (all PFs) */
+	ERR_RECOVERY_ST_REINIT = 4		/* Re-initialize Phase */
+};
+
+struct be_error_recovery {
+	/* Lancer error recovery variables */
+	u8 recovery_retries;
+
+	/* BEx/Skyhawk error recovery variables */
+	u8 recovery_state;
+	u16 ue_to_reset_time;		/* Time after UE, to soft reset
+					 * the chip - PF0 only
+					 */
+	u16 ue_to_poll_time;		/* Time after UE, to Restart Polling
+					 * of SLIPORT_SEMAPHORE reg
+					 */
+	u16 last_err_code;
+	bool recovery_supported;
+	unsigned long probe_time;
+	unsigned long last_recovery_time;
+
+	/* Common to both Lancer & BEx/SH error recovery */
+	u32 resched_delay;
+	struct delayed_work err_detection_work;
+};
+
+/* Ethtool priv_flags */
+#define	BE_DISABLE_TPE_RECOVERY	0x1
+
 struct be_adapter {
 	struct pci_dev *pdev;
 	struct net_device *netdev;
@@ -523,7 +587,7 @@ struct be_adapter {
 	struct be_dma_mem mbox_mem_alloced;
 
 	struct be_mcc_obj mcc_obj;
-	spinlock_t mcc_lock;	/* For serializing mcc cmds to BE card */
+	struct mutex mcc_lock;	/* For serializing mcc cmds to BE card */
 	spinlock_t mcc_cq_lock;
 
 	u16 cfg_num_rx_irqs;		/* configured via set-channels */
@@ -556,7 +620,6 @@ struct be_adapter {
 	struct delayed_work work;
 	u16 work_counter;
 
-	struct delayed_work be_err_detection_work;
 	u8 recovery_retries;
 	u8 err_flags;
 	bool pcicfg_mapped;	/* pcicfg obtained via pci_iomap() */
@@ -570,9 +633,15 @@ struct be_adapter {
 	int if_handle;		/* Used to configure filtering */
 	u32 if_flags;		/* Interface filtering flags */
 	u32 *pmac_id;		/* MAC addr handle used by BE card */
+	struct be_eth_addr *uc_list;/* list of uc-addrs programmed (not perm) */
 	u32 uc_macs;		/* Count of secondary UC MAC programmed */
+	struct be_eth_addr *mc_list;/* list of mcast addrs programmed */
+	u32 mc_count;
 	unsigned long vids[BITS_TO_LONGS(VLAN_N_VID)];
 	u16 vlans_added;
+	bool update_uc_list;
+	bool update_mc_list;
+	struct mutex rx_filter_lock;/* For protecting vids[] & mc/uc_list[] */
 
 	u32 beacon_state;	/* for set_phys_id */
 
@@ -624,6 +693,18 @@ struct be_adapter {
 	u32 fat_dump_len;
 	u16 serial_num[CNTL_SERIAL_NUM_WORDS];
 	u8 phy_state; /* state of sfp optics (functional, faulted, etc.,) */
+	u8 dev_mac[ETH_ALEN];
+	u32 priv_flags; /* ethtool get/set_priv_flags() */
+	struct be_error_recovery error_recovery;
+};
+
+/* Used for defered FW config cmds. Add fields to this struct as reqd */
+struct be_cmd_work {
+	struct work_struct work;
+	struct be_adapter *adapter;
+	union {
+		__be16 vxlan_port;
+	} info;
 };
 
 #define be_physfn(adapter)		(!adapter->virtfn)
@@ -848,6 +929,9 @@ static inline bool is_ipv4_pkt(struct sk_buff *skb)
 	return skb->protocol == htons(ETH_P_IP) && ip_hdr(skb)->version == 4;
 }
 
+#define be_error_recovering(adapter)	\
+		(adapter->flags & BE_FLAGS_TRY_RECOVERY)
+
 #define BE_ERROR_EEH		1
 #define BE_ERROR_UE		BIT(1)
 #define BE_ERROR_FW		BIT(2)
diff --git a/drivers/net/ethernet/emulex/benet/be_cmds.c b/drivers/net/ethernet/emulex/benet/be_cmds.c
index 2cc11756859f..1fb5d7239254 100644
--- a/drivers/net/ethernet/emulex/benet/be_cmds.c
+++ b/drivers/net/ethernet/emulex/benet/be_cmds.c
@@ -92,6 +92,11 @@ static struct be_cmd_priv_map cmd_priv_map[] = {
 		CMD_SUBSYSTEM_COMMON,
 		BE_PRIV_DEVCFG | BE_PRIV_VHADM
 	},
+	{
+		OPCODE_COMMON_GET_EXT_FAT_CAPABILITIES,
+		CMD_SUBSYSTEM_COMMON,
+		BE_PRIV_DEVCFG
+	}
 };
 
 static bool be_cmd_allowed(struct be_adapter *adapter, u8 opcode, u8 subsystem)
@@ -571,7 +576,7 @@ int be_process_mcc(struct be_adapter *adapter)
 /* Wait till no more pending mcc requests are present */
 static int be_mcc_wait_compl(struct be_adapter *adapter)
 {
-#define mcc_timeout		120000 /* 12s timeout */
+#define mcc_timeout		12000 /* 12s timeout */
 	int i, status = 0;
 	struct be_mcc_obj *mcc_obj = &adapter->mcc_obj;
 
@@ -585,7 +590,7 @@ static int be_mcc_wait_compl(struct be_adapter *adapter)
 
 		if (atomic_read(&mcc_obj->q.used) == 0)
 			break;
-		udelay(100);
+		usleep_range(500, 1000);
 	}
 	if (i == mcc_timeout) {
 		dev_err(&adapter->pdev->dev, "FW not responding\n");
@@ -705,7 +710,7 @@ static int be_mbox_notify_wait(struct be_adapter *adapter)
 	return 0;
 }
 
-static u16 be_POST_stage_get(struct be_adapter *adapter)
+u16 be_POST_stage_get(struct be_adapter *adapter)
 {
 	u32 sem;
 
@@ -863,7 +868,7 @@ static bool use_mcc(struct be_adapter *adapter)
 static int be_cmd_lock(struct be_adapter *adapter)
 {
 	if (use_mcc(adapter)) {
-		spin_lock_bh(&adapter->mcc_lock);
+		mutex_lock(&adapter->mcc_lock);
 		return 0;
 	} else {
 		return mutex_lock_interruptible(&adapter->mbox_lock);
@@ -874,7 +879,7 @@ static int be_cmd_lock(struct be_adapter *adapter)
 static void be_cmd_unlock(struct be_adapter *adapter)
 {
 	if (use_mcc(adapter))
-		spin_unlock_bh(&adapter->mcc_lock);
+		return mutex_unlock(&adapter->mcc_lock);
 	else
 		return mutex_unlock(&adapter->mbox_lock);
 }
@@ -1044,7 +1049,7 @@ int be_cmd_mac_addr_query(struct be_adapter *adapter, u8 *mac_addr,
 	struct be_cmd_req_mac_query *req;
 	int status;
 
-	spin_lock_bh(&adapter->mcc_lock);
+	mutex_lock(&adapter->mcc_lock);
 
 	wrb = wrb_from_mccq(adapter);
 	if (!wrb) {
@@ -1073,7 +1078,7 @@ int be_cmd_mac_addr_query(struct be_adapter *adapter, u8 *mac_addr,
 	}
 
 err:
-	spin_unlock_bh(&adapter->mcc_lock);
+	mutex_unlock(&adapter->mcc_lock);
 	return status;
 }
 
@@ -1085,7 +1090,7 @@ int be_cmd_pmac_add(struct be_adapter *adapter, u8 *mac_addr,
 	struct be_cmd_req_pmac_add *req;
 	int status;
 
-	spin_lock_bh(&adapter->mcc_lock);
+	mutex_lock(&adapter->mcc_lock);
 
 	wrb = wrb_from_mccq(adapter);
 	if (!wrb) {
@@ -1110,7 +1115,7 @@ int be_cmd_pmac_add(struct be_adapter *adapter, u8 *mac_addr,
 	}
 
 err:
-	spin_unlock_bh(&adapter->mcc_lock);
+	mutex_unlock(&adapter->mcc_lock);
 
 	 if (status == MCC_STATUS_UNAUTHORIZED_REQUEST)
 		status = -EPERM;
@@ -1128,7 +1133,7 @@ int be_cmd_pmac_del(struct be_adapter *adapter, u32 if_id, int pmac_id, u32 dom)
 	if (pmac_id == -1)
 		return 0;
 
-	spin_lock_bh(&adapter->mcc_lock);
+	mutex_lock(&adapter->mcc_lock);
 
 	wrb = wrb_from_mccq(adapter);
 	if (!wrb) {
@@ -1148,7 +1153,7 @@ int be_cmd_pmac_del(struct be_adapter *adapter, u32 if_id, int pmac_id, u32 dom)
 	status = be_mcc_notify_wait(adapter);
 
 err:
-	spin_unlock_bh(&adapter->mcc_lock);
+	mutex_unlock(&adapter->mcc_lock);
 	return status;
 }
 
@@ -1411,7 +1416,7 @@ int be_cmd_rxq_create(struct be_adapter *adapter,
 	struct be_dma_mem *q_mem = &rxq->dma_mem;
 	int status;
 
-	spin_lock_bh(&adapter->mcc_lock);
+	mutex_lock(&adapter->mcc_lock);
 
 	wrb = wrb_from_mccq(adapter);
 	if (!wrb) {
@@ -1441,7 +1446,7 @@ int be_cmd_rxq_create(struct be_adapter *adapter,
 	}
 
 err:
-	spin_unlock_bh(&adapter->mcc_lock);
+	mutex_unlock(&adapter->mcc_lock);
 	return status;
 }
 
@@ -1505,7 +1510,7 @@ int be_cmd_rxq_destroy(struct be_adapter *adapter, struct be_queue_info *q)
 	struct be_cmd_req_q_destroy *req;
 	int status;
 
-	spin_lock_bh(&adapter->mcc_lock);
+	mutex_lock(&adapter->mcc_lock);
 
 	wrb = wrb_from_mccq(adapter);
 	if (!wrb) {
@@ -1522,7 +1527,7 @@ int be_cmd_rxq_destroy(struct be_adapter *adapter, struct be_queue_info *q)
 	q->created = false;
 
 err:
-	spin_unlock_bh(&adapter->mcc_lock);
+	mutex_unlock(&adapter->mcc_lock);
 	return status;
 }
 
@@ -1590,7 +1595,7 @@ int be_cmd_get_stats(struct be_adapter *adapter, struct be_dma_mem *nonemb_cmd)
 	struct be_cmd_req_hdr *hdr;
 	int status = 0;
 
-	spin_lock_bh(&adapter->mcc_lock);
+	mutex_lock(&adapter->mcc_lock);
 
 	wrb = wrb_from_mccq(adapter);
 	if (!wrb) {
@@ -1618,7 +1623,7 @@ int be_cmd_get_stats(struct be_adapter *adapter, struct be_dma_mem *nonemb_cmd)
 	adapter->stats_cmd_sent = true;
 
 err:
-	spin_unlock_bh(&adapter->mcc_lock);
+	mutex_unlock(&adapter->mcc_lock);
 	return status;
 }
 
@@ -1634,7 +1639,7 @@ int lancer_cmd_get_pport_stats(struct be_adapter *adapter,
 			    CMD_SUBSYSTEM_ETH))
 		return -EPERM;
 
-	spin_lock_bh(&adapter->mcc_lock);
+	mutex_lock(&adapter->mcc_lock);
 
 	wrb = wrb_from_mccq(adapter);
 	if (!wrb) {
@@ -1657,7 +1662,7 @@ int lancer_cmd_get_pport_stats(struct be_adapter *adapter,
 	adapter->stats_cmd_sent = true;
 
 err:
-	spin_unlock_bh(&adapter->mcc_lock);
+	mutex_unlock(&adapter->mcc_lock);
 	return status;
 }
 
@@ -1694,7 +1699,7 @@ int be_cmd_link_status_query(struct be_adapter *adapter, u16 *link_speed,
 	struct be_cmd_req_link_status *req;
 	int status;
 
-	spin_lock_bh(&adapter->mcc_lock);
+	mutex_lock(&adapter->mcc_lock);
 
 	if (link_status)
 		*link_status = LINK_DOWN;
@@ -1733,7 +1738,7 @@ int be_cmd_link_status_query(struct be_adapter *adapter, u16 *link_speed,
 	}
 
 err:
-	spin_unlock_bh(&adapter->mcc_lock);
+	mutex_unlock(&adapter->mcc_lock);
 	return status;
 }
 
@@ -1744,7 +1749,7 @@ int be_cmd_get_die_temperature(struct be_adapter *adapter)
 	struct be_cmd_req_get_cntl_addnl_attribs *req;
 	int status = 0;
 
-	spin_lock_bh(&adapter->mcc_lock);
+	mutex_lock(&adapter->mcc_lock);
 
 	wrb = wrb_from_mccq(adapter);
 	if (!wrb) {
@@ -1759,7 +1764,7 @@ int be_cmd_get_die_temperature(struct be_adapter *adapter)
 
 	status = be_mcc_notify(adapter);
 err:
-	spin_unlock_bh(&adapter->mcc_lock);
+	mutex_unlock(&adapter->mcc_lock);
 	return status;
 }
 
@@ -1808,7 +1813,7 @@ int be_cmd_get_fat_dump(struct be_adapter *adapter, u32 buf_len, void *buf)
 	if (!get_fat_cmd.va)
 		return -ENOMEM;
 
-	spin_lock_bh(&adapter->mcc_lock);
+	mutex_lock(&adapter->mcc_lock);
 
 	while (total_size) {
 		buf_size = min(total_size, (u32)60*1024);
@@ -1848,7 +1853,7 @@ int be_cmd_get_fat_dump(struct be_adapter *adapter, u32 buf_len, void *buf)
 err:
 	dma_free_coherent(&adapter->pdev->dev, get_fat_cmd.size,
 			  get_fat_cmd.va, get_fat_cmd.dma);
-	spin_unlock_bh(&adapter->mcc_lock);
+	mutex_unlock(&adapter->mcc_lock);
 	return status;
 }
 
@@ -1859,7 +1864,7 @@ int be_cmd_get_fw_ver(struct be_adapter *adapter)
 	struct be_cmd_req_get_fw_version *req;
 	int status;
 
-	spin_lock_bh(&adapter->mcc_lock);
+	mutex_lock(&adapter->mcc_lock);
 
 	wrb = wrb_from_mccq(adapter);
 	if (!wrb) {
@@ -1882,7 +1887,7 @@ int be_cmd_get_fw_ver(struct be_adapter *adapter)
 			sizeof(adapter->fw_on_flash));
 	}
 err:
-	spin_unlock_bh(&adapter->mcc_lock);
+	mutex_unlock(&adapter->mcc_lock);
 	return status;
 }
 
@@ -1896,7 +1901,7 @@ static int __be_cmd_modify_eqd(struct be_adapter *adapter,
 	struct be_cmd_req_modify_eq_delay *req;
 	int status = 0, i;
 
-	spin_lock_bh(&adapter->mcc_lock);
+	mutex_lock(&adapter->mcc_lock);
 
 	wrb = wrb_from_mccq(adapter);
 	if (!wrb) {
@@ -1919,7 +1924,7 @@ static int __be_cmd_modify_eqd(struct be_adapter *adapter,
 
 	status = be_mcc_notify(adapter);
 err:
-	spin_unlock_bh(&adapter->mcc_lock);
+	mutex_unlock(&adapter->mcc_lock);
 	return status;
 }
 
@@ -1946,7 +1951,7 @@ int be_cmd_vlan_config(struct be_adapter *adapter, u32 if_id, u16 *vtag_array,
 	struct be_cmd_req_vlan_config *req;
 	int status;
 
-	spin_lock_bh(&adapter->mcc_lock);
+	mutex_lock(&adapter->mcc_lock);
 
 	wrb = wrb_from_mccq(adapter);
 	if (!wrb) {
@@ -1968,7 +1973,7 @@ int be_cmd_vlan_config(struct be_adapter *adapter, u32 if_id, u16 *vtag_array,
 
 	status = be_mcc_notify_wait(adapter);
 err:
-	spin_unlock_bh(&adapter->mcc_lock);
+	mutex_unlock(&adapter->mcc_lock);
 	return status;
 }
 
@@ -1979,7 +1984,7 @@ static int __be_cmd_rx_filter(struct be_adapter *adapter, u32 flags, u32 value)
 	struct be_cmd_req_rx_filter *req = mem->va;
 	int status;
 
-	spin_lock_bh(&adapter->mcc_lock);
+	mutex_lock(&adapter->mcc_lock);
 
 	wrb = wrb_from_mccq(adapter);
 	if (!wrb) {
@@ -1996,8 +2001,7 @@ static int __be_cmd_rx_filter(struct be_adapter *adapter, u32 flags, u32 value)
 	req->if_flags = (value == ON) ? req->if_flags_mask : 0;
 
 	if (flags & BE_IF_FLAGS_MULTICAST) {
-		struct netdev_hw_addr *ha;
-		int i = 0;
+		int i;
 
 		/* Reset mcast promisc mode if already set by setting mask
 		 * and not setting flags field
@@ -2005,14 +2009,15 @@ static int __be_cmd_rx_filter(struct be_adapter *adapter, u32 flags, u32 value)
 		req->if_flags_mask |=
 			cpu_to_le32(BE_IF_FLAGS_MCAST_PROMISCUOUS &
 				    be_if_cap_flags(adapter));
-		req->mcast_num = cpu_to_le32(netdev_mc_count(adapter->netdev));
-		netdev_for_each_mc_addr(ha, adapter->netdev)
-			memcpy(req->mcast_mac[i++].byte, ha->addr, ETH_ALEN);
+		req->mcast_num = cpu_to_le32(adapter->mc_count);
+		for (i = 0; i < adapter->mc_count; i++)
+			ether_addr_copy(req->mcast_mac[i].byte,
+					adapter->mc_list[i].mac);
 	}
 
 	status = be_mcc_notify_wait(adapter);
 err:
-	spin_unlock_bh(&adapter->mcc_lock);
+	mutex_unlock(&adapter->mcc_lock);
 	return status;
 }
 
@@ -2043,7 +2048,7 @@ int be_cmd_set_flow_control(struct be_adapter *adapter, u32 tx_fc, u32 rx_fc)
 			    CMD_SUBSYSTEM_COMMON))
 		return -EPERM;
 
-	spin_lock_bh(&adapter->mcc_lock);
+	mutex_lock(&adapter->mcc_lock);
 
 	wrb = wrb_from_mccq(adapter);
 	if (!wrb) {
@@ -2063,7 +2068,7 @@ int be_cmd_set_flow_control(struct be_adapter *adapter, u32 tx_fc, u32 rx_fc)
 	status = be_mcc_notify_wait(adapter);
 
 err:
-	spin_unlock_bh(&adapter->mcc_lock);
+	mutex_unlock(&adapter->mcc_lock);
 
 	if (base_status(status) == MCC_STATUS_FEATURE_NOT_SUPPORTED)
 		return  -EOPNOTSUPP;
@@ -2082,7 +2087,7 @@ int be_cmd_get_flow_control(struct be_adapter *adapter, u32 *tx_fc, u32 *rx_fc)
 			    CMD_SUBSYSTEM_COMMON))
 		return -EPERM;
 
-	spin_lock_bh(&adapter->mcc_lock);
+	mutex_lock(&adapter->mcc_lock);
 
 	wrb = wrb_from_mccq(adapter);
 	if (!wrb) {
@@ -2105,7 +2110,7 @@ int be_cmd_get_flow_control(struct be_adapter *adapter, u32 *tx_fc, u32 *rx_fc)
 	}
 
 err:
-	spin_unlock_bh(&adapter->mcc_lock);
+	mutex_unlock(&adapter->mcc_lock);
 	return status;
 }
 
@@ -2186,7 +2191,7 @@ int be_cmd_rss_config(struct be_adapter *adapter, u8 *rsstable,
 	if (!(be_if_cap_flags(adapter) & BE_IF_FLAGS_RSS))
 		return 0;
 
-	spin_lock_bh(&adapter->mcc_lock);
+	mutex_lock(&adapter->mcc_lock);
 
 	wrb = wrb_from_mccq(adapter);
 	if (!wrb) {
@@ -2211,7 +2216,7 @@ int be_cmd_rss_config(struct be_adapter *adapter, u8 *rsstable,
 
 	status = be_mcc_notify_wait(adapter);
 err:
-	spin_unlock_bh(&adapter->mcc_lock);
+	mutex_unlock(&adapter->mcc_lock);
 	return status;
 }
 
@@ -2223,7 +2228,7 @@ int be_cmd_set_beacon_state(struct be_adapter *adapter, u8 port_num,
 	struct be_cmd_req_enable_disable_beacon *req;
 	int status;
 
-	spin_lock_bh(&adapter->mcc_lock);
+	mutex_lock(&adapter->mcc_lock);
 
 	wrb = wrb_from_mccq(adapter);
 	if (!wrb) {
@@ -2244,7 +2249,7 @@ int be_cmd_set_beacon_state(struct be_adapter *adapter, u8 port_num,
 	status = be_mcc_notify_wait(adapter);
 
 err:
-	spin_unlock_bh(&adapter->mcc_lock);
+	mutex_unlock(&adapter->mcc_lock);
 	return status;
 }
 
@@ -2255,7 +2260,7 @@ int be_cmd_get_beacon_state(struct be_adapter *adapter, u8 port_num, u32 *state)
 	struct be_cmd_req_get_beacon_state *req;
 	int status;
 
-	spin_lock_bh(&adapter->mcc_lock);
+	mutex_lock(&adapter->mcc_lock);
 
 	wrb = wrb_from_mccq(adapter);
 	if (!wrb) {
@@ -2279,7 +2284,7 @@ int be_cmd_get_beacon_state(struct be_adapter *adapter, u8 port_num, u32 *state)
 	}
 
 err:
-	spin_unlock_bh(&adapter->mcc_lock);
+	mutex_unlock(&adapter->mcc_lock);
 	return status;
 }
 
@@ -2303,7 +2308,7 @@ int be_cmd_read_port_transceiver_data(struct be_adapter *adapter,
 		return -ENOMEM;
 	}
 
-	spin_lock_bh(&adapter->mcc_lock);
+	mutex_lock(&adapter->mcc_lock);
 
 	wrb = wrb_from_mccq(adapter);
 	if (!wrb) {
@@ -2325,7 +2330,7 @@ int be_cmd_read_port_transceiver_data(struct be_adapter *adapter,
 		memcpy(data, resp->page_data, PAGE_DATA_LEN);
 	}
 err:
-	spin_unlock_bh(&adapter->mcc_lock);
+	mutex_unlock(&adapter->mcc_lock);
 	dma_free_coherent(&adapter->pdev->dev, cmd.size, cmd.va, cmd.dma);
 	return status;
 }
@@ -2342,7 +2347,7 @@ static int lancer_cmd_write_object(struct be_adapter *adapter,
 	void *ctxt = NULL;
 	int status;
 
-	spin_lock_bh(&adapter->mcc_lock);
+	mutex_lock(&adapter->mcc_lock);
 	adapter->flash_status = 0;
 
 	wrb = wrb_from_mccq(adapter);
@@ -2384,7 +2389,7 @@ static int lancer_cmd_write_object(struct be_adapter *adapter,
 	if (status)
 		goto err_unlock;
 
-	spin_unlock_bh(&adapter->mcc_lock);
+	mutex_unlock(&adapter->mcc_lock);
 
 	if (!wait_for_completion_timeout(&adapter->et_cmd_compl,
 					 msecs_to_jiffies(60000)))
@@ -2403,7 +2408,7 @@ static int lancer_cmd_write_object(struct be_adapter *adapter,
 	return status;
 
 err_unlock:
-	spin_unlock_bh(&adapter->mcc_lock);
+	mutex_unlock(&adapter->mcc_lock);
 	return status;
 }
 
@@ -2457,7 +2462,7 @@ static int lancer_cmd_delete_object(struct be_adapter *adapter,
 	struct be_mcc_wrb *wrb;
 	int status;
 
-	spin_lock_bh(&adapter->mcc_lock);
+	mutex_lock(&adapter->mcc_lock);
 
 	wrb = wrb_from_mccq(adapter);
 	if (!wrb) {
@@ -2475,7 +2480,7 @@ static int lancer_cmd_delete_object(struct be_adapter *adapter,
 
 	status = be_mcc_notify_wait(adapter);
 err:
-	spin_unlock_bh(&adapter->mcc_lock);
+	mutex_unlock(&adapter->mcc_lock);
 	return status;
 }
 
@@ -2488,7 +2493,7 @@ int lancer_cmd_read_object(struct be_adapter *adapter, struct be_dma_mem *cmd,
 	struct lancer_cmd_resp_read_object *resp;
 	int status;
 
-	spin_lock_bh(&adapter->mcc_lock);
+	mutex_lock(&adapter->mcc_lock);
 
 	wrb = wrb_from_mccq(adapter);
 	if (!wrb) {
@@ -2522,7 +2527,7 @@ int lancer_cmd_read_object(struct be_adapter *adapter, struct be_dma_mem *cmd,
 	}
 
 err_unlock:
-	spin_unlock_bh(&adapter->mcc_lock);
+	mutex_unlock(&adapter->mcc_lock);
 	return status;
 }
 
@@ -2534,7 +2539,7 @@ static int be_cmd_write_flashrom(struct be_adapter *adapter,
 	struct be_cmd_write_flashrom *req;
 	int status;
 
-	spin_lock_bh(&adapter->mcc_lock);
+	mutex_lock(&adapter->mcc_lock);
 	adapter->flash_status = 0;
 
 	wrb = wrb_from_mccq(adapter);
@@ -2559,7 +2564,7 @@ static int be_cmd_write_flashrom(struct be_adapter *adapter,
 	if (status)
 		goto err_unlock;
 
-	spin_unlock_bh(&adapter->mcc_lock);
+	mutex_unlock(&adapter->mcc_lock);
 
 	if (!wait_for_completion_timeout(&adapter->et_cmd_compl,
 					 msecs_to_jiffies(40000)))
@@ -2570,7 +2575,7 @@ static int be_cmd_write_flashrom(struct be_adapter *adapter,
 	return status;
 
 err_unlock:
-	spin_unlock_bh(&adapter->mcc_lock);
+	mutex_unlock(&adapter->mcc_lock);
 	return status;
 }
 
@@ -2581,7 +2586,7 @@ static int be_cmd_get_flash_crc(struct be_adapter *adapter, u8 *flashed_crc,
 	struct be_mcc_wrb *wrb;
 	int status;
 
-	spin_lock_bh(&adapter->mcc_lock);
+	mutex_lock(&adapter->mcc_lock);
 
 	wrb = wrb_from_mccq(adapter);
 	if (!wrb) {
@@ -2608,7 +2613,7 @@ static int be_cmd_get_flash_crc(struct be_adapter *adapter, u8 *flashed_crc,
 		memcpy(flashed_crc, req->crc, 4);
 
 err:
-	spin_unlock_bh(&adapter->mcc_lock);
+	mutex_unlock(&adapter->mcc_lock);
 	return status;
 }
 
@@ -2723,6 +2728,26 @@ static int be_flash(struct be_adapter *adapter, const u8 *img,
 	return 0;
 }
 
+#define NCSI_UPDATE_LOG	"NCSI section update is not supported in FW ver %s\n"
+static bool be_fw_ncsi_supported(char *ver)
+{
+	int v1[4] = {3, 102, 148, 0}; /* Min ver that supports NCSI FW */
+	int v2[4];
+	int i;
+
+	if (sscanf(ver, "%d.%d.%d.%d", &v2[0], &v2[1], &v2[2], &v2[3]) != 4)
+		return false;
+
+	for (i = 0; i < 4; i++) {
+		if (v1[i] < v2[i])
+			return true;
+		else if (v1[i] > v2[i])
+			return false;
+	}
+
+	return true;
+}
+
 /* For BE2, BE3 and BE3-R */
 static int be_flash_BEx(struct be_adapter *adapter,
 			const struct firmware *fw,
@@ -2800,8 +2825,10 @@ static int be_flash_BEx(struct be_adapter *adapter,
 			continue;
 
 		if ((pflashcomp[i].optype == OPTYPE_NCSI_FW) &&
-		    memcmp(adapter->fw_ver, "3.102.148.0", 11) < 0)
+		    !be_fw_ncsi_supported(adapter->fw_ver)) {
+			dev_info(dev, NCSI_UPDATE_LOG, adapter->fw_ver);
 			continue;
+		}
 
 		if (pflashcomp[i].optype == OPTYPE_PHY_FW  &&
 		    !phy_flashing_required(adapter))
@@ -3192,7 +3219,7 @@ int be_cmd_enable_magic_wol(struct be_adapter *adapter, u8 *mac,
 	struct be_cmd_req_acpi_wol_magic_config *req;
 	int status;
 
-	spin_lock_bh(&adapter->mcc_lock);
+	mutex_lock(&adapter->mcc_lock);
 
 	wrb = wrb_from_mccq(adapter);
 	if (!wrb) {
@@ -3209,7 +3236,7 @@ int be_cmd_enable_magic_wol(struct be_adapter *adapter, u8 *mac,
 	status = be_mcc_notify_wait(adapter);
 
 err:
-	spin_unlock_bh(&adapter->mcc_lock);
+	mutex_unlock(&adapter->mcc_lock);
 	return status;
 }
 
@@ -3224,7 +3251,7 @@ int be_cmd_set_loopback(struct be_adapter *adapter, u8 port_num,
 			    CMD_SUBSYSTEM_LOWLEVEL))
 		return -EPERM;
 
-	spin_lock_bh(&adapter->mcc_lock);
+	mutex_lock(&adapter->mcc_lock);
 
 	wrb = wrb_from_mccq(adapter);
 	if (!wrb) {
@@ -3247,7 +3274,7 @@ int be_cmd_set_loopback(struct be_adapter *adapter, u8 port_num,
 	if (status)
 		goto err_unlock;
 
-	spin_unlock_bh(&adapter->mcc_lock);
+	mutex_unlock(&adapter->mcc_lock);
 
 	if (!wait_for_completion_timeout(&adapter->et_cmd_compl,
 					 msecs_to_jiffies(SET_LB_MODE_TIMEOUT)))
@@ -3256,7 +3283,7 @@ int be_cmd_set_loopback(struct be_adapter *adapter, u8 port_num,
 	return status;
 
 err_unlock:
-	spin_unlock_bh(&adapter->mcc_lock);
+	mutex_unlock(&adapter->mcc_lock);
 	return status;
 }
 
@@ -3273,7 +3300,7 @@ int be_cmd_loopback_test(struct be_adapter *adapter, u32 port_num,
 			    CMD_SUBSYSTEM_LOWLEVEL))
 		return -EPERM;
 
-	spin_lock_bh(&adapter->mcc_lock);
+	mutex_lock(&adapter->mcc_lock);
 
 	wrb = wrb_from_mccq(adapter);
 	if (!wrb) {
@@ -3299,7 +3326,7 @@ int be_cmd_loopback_test(struct be_adapter *adapter, u32 port_num,
 	if (status)
 		goto err;
 
-	spin_unlock_bh(&adapter->mcc_lock);
+	mutex_unlock(&adapter->mcc_lock);
 
 	wait_for_completion(&adapter->et_cmd_compl);
 	resp = embedded_payload(wrb);
@@ -3307,7 +3334,7 @@ int be_cmd_loopback_test(struct be_adapter *adapter, u32 port_num,
 
 	return status;
 err:
-	spin_unlock_bh(&adapter->mcc_lock);
+	mutex_unlock(&adapter->mcc_lock);
 	return status;
 }
 
@@ -3323,7 +3350,7 @@ int be_cmd_ddr_dma_test(struct be_adapter *adapter, u64 pattern,
 			    CMD_SUBSYSTEM_LOWLEVEL))
 		return -EPERM;
 
-	spin_lock_bh(&adapter->mcc_lock);
+	mutex_lock(&adapter->mcc_lock);
 
 	wrb = wrb_from_mccq(adapter);
 	if (!wrb) {
@@ -3357,7 +3384,7 @@ int be_cmd_ddr_dma_test(struct be_adapter *adapter, u64 pattern,
 	}
 
 err:
-	spin_unlock_bh(&adapter->mcc_lock);
+	mutex_unlock(&adapter->mcc_lock);
 	return status;
 }
 
@@ -3368,7 +3395,7 @@ int be_cmd_get_seeprom_data(struct be_adapter *adapter,
 	struct be_cmd_req_seeprom_read *req;
 	int status;
 
-	spin_lock_bh(&adapter->mcc_lock);
+	mutex_lock(&adapter->mcc_lock);
 
 	wrb = wrb_from_mccq(adapter);
 	if (!wrb) {
@@ -3384,7 +3411,7 @@ int be_cmd_get_seeprom_data(struct be_adapter *adapter,
 	status = be_mcc_notify_wait(adapter);
 
 err:
-	spin_unlock_bh(&adapter->mcc_lock);
+	mutex_unlock(&adapter->mcc_lock);
 	return status;
 }
 
@@ -3399,7 +3426,7 @@ int be_cmd_get_phy_info(struct be_adapter *adapter)
 			    CMD_SUBSYSTEM_COMMON))
 		return -EPERM;
 
-	spin_lock_bh(&adapter->mcc_lock);
+	mutex_lock(&adapter->mcc_lock);
 
 	wrb = wrb_from_mccq(adapter);
 	if (!wrb) {
@@ -3444,7 +3471,7 @@ int be_cmd_get_phy_info(struct be_adapter *adapter)
 	}
 	dma_free_coherent(&adapter->pdev->dev, cmd.size, cmd.va, cmd.dma);
 err:
-	spin_unlock_bh(&adapter->mcc_lock);
+	mutex_unlock(&adapter->mcc_lock);
 	return status;
 }
 
@@ -3454,7 +3481,7 @@ static int be_cmd_set_qos(struct be_adapter *adapter, u32 bps, u32 domain)
 	struct be_cmd_req_set_qos *req;
 	int status;
 
-	spin_lock_bh(&adapter->mcc_lock);
+	mutex_lock(&adapter->mcc_lock);
 
 	wrb = wrb_from_mccq(adapter);
 	if (!wrb) {
@@ -3474,7 +3501,7 @@ static int be_cmd_set_qos(struct be_adapter *adapter, u32 bps, u32 domain)
 	status = be_mcc_notify_wait(adapter);
 
 err:
-	spin_unlock_bh(&adapter->mcc_lock);
+	mutex_unlock(&adapter->mcc_lock);
 	return status;
 }
 
@@ -3522,6 +3549,11 @@ int be_cmd_get_cntl_attributes(struct be_adapter *adapter)
 		for (i = 0; i < CNTL_SERIAL_NUM_WORDS; i++)
 			adapter->serial_num[i] = le32_to_cpu(serial_num[i]) &
 				(BIT_MASK(16) - 1);
+		/* For BEx, since GET_FUNC_CONFIG command is not
+		 * supported, we read funcnum here as a workaround.
+		 */
+		if (BEx_chip(adapter))
+			adapter->pf_num = attribs->hba_attribs.pci_funcnum;
 	}
 
 err:
@@ -3581,7 +3613,7 @@ int be_cmd_get_fn_privileges(struct be_adapter *adapter, u32 *privilege,
 	struct be_cmd_req_get_fn_privileges *req;
 	int status;
 
-	spin_lock_bh(&adapter->mcc_lock);
+	mutex_lock(&adapter->mcc_lock);
 
 	wrb = wrb_from_mccq(adapter);
 	if (!wrb) {
@@ -3613,7 +3645,7 @@ int be_cmd_get_fn_privileges(struct be_adapter *adapter, u32 *privilege,
 	}
 
 err:
-	spin_unlock_bh(&adapter->mcc_lock);
+	mutex_unlock(&adapter->mcc_lock);
 	return status;
 }
 
@@ -3625,7 +3657,7 @@ int be_cmd_set_fn_privileges(struct be_adapter *adapter, u32 privileges,
 	struct be_cmd_req_set_fn_privileges *req;
 	int status;
 
-	spin_lock_bh(&adapter->mcc_lock);
+	mutex_lock(&adapter->mcc_lock);
 
 	wrb = wrb_from_mccq(adapter);
 	if (!wrb) {
@@ -3645,7 +3677,7 @@ int be_cmd_set_fn_privileges(struct be_adapter *adapter, u32 privileges,
 
 	status = be_mcc_notify_wait(adapter);
 err:
-	spin_unlock_bh(&adapter->mcc_lock);
+	mutex_unlock(&adapter->mcc_lock);
 	return status;
 }
 
@@ -3677,7 +3709,7 @@ int be_cmd_get_mac_from_list(struct be_adapter *adapter, u8 *mac,
 		return -ENOMEM;
 	}
 
-	spin_lock_bh(&adapter->mcc_lock);
+	mutex_lock(&adapter->mcc_lock);
 
 	wrb = wrb_from_mccq(adapter);
 	if (!wrb) {
@@ -3741,7 +3773,7 @@ int be_cmd_get_mac_from_list(struct be_adapter *adapter, u8 *mac,
 	}
 
 out:
-	spin_unlock_bh(&adapter->mcc_lock);
+	mutex_unlock(&adapter->mcc_lock);
 	dma_free_coherent(&adapter->pdev->dev, get_mac_list_cmd.size,
 			  get_mac_list_cmd.va, get_mac_list_cmd.dma);
 	return status;
@@ -3801,7 +3833,7 @@ int be_cmd_set_mac_list(struct be_adapter *adapter, u8 *mac_array,
 	if (!cmd.va)
 		return -ENOMEM;
 
-	spin_lock_bh(&adapter->mcc_lock);
+	mutex_lock(&adapter->mcc_lock);
 
 	wrb = wrb_from_mccq(adapter);
 	if (!wrb) {
@@ -3823,7 +3855,7 @@ int be_cmd_set_mac_list(struct be_adapter *adapter, u8 *mac_array,
 
 err:
 	dma_free_coherent(&adapter->pdev->dev, cmd.size, cmd.va, cmd.dma);
-	spin_unlock_bh(&adapter->mcc_lock);
+	mutex_unlock(&adapter->mcc_lock);
 	return status;
 }
 
@@ -3859,7 +3891,7 @@ int be_cmd_set_hsw_config(struct be_adapter *adapter, u16 pvid,
 			    CMD_SUBSYSTEM_COMMON))
 		return -EPERM;
 
-	spin_lock_bh(&adapter->mcc_lock);
+	mutex_lock(&adapter->mcc_lock);
 
 	wrb = wrb_from_mccq(adapter);
 	if (!wrb) {
@@ -3900,7 +3932,7 @@ int be_cmd_set_hsw_config(struct be_adapter *adapter, u16 pvid,
 	status = be_mcc_notify_wait(adapter);
 
 err:
-	spin_unlock_bh(&adapter->mcc_lock);
+	mutex_unlock(&adapter->mcc_lock);
 	return status;
 }
 
@@ -3914,7 +3946,7 @@ int be_cmd_get_hsw_config(struct be_adapter *adapter, u16 *pvid,
 	int status;
 	u16 vid;
 
-	spin_lock_bh(&adapter->mcc_lock);
+	mutex_lock(&adapter->mcc_lock);
 
 	wrb = wrb_from_mccq(adapter);
 	if (!wrb) {
@@ -3961,7 +3993,7 @@ int be_cmd_get_hsw_config(struct be_adapter *adapter, u16 *pvid,
 	}
 
 err:
-	spin_unlock_bh(&adapter->mcc_lock);
+	mutex_unlock(&adapter->mcc_lock);
 	return status;
 }
 
@@ -4127,6 +4159,10 @@ int be_cmd_get_ext_fat_capabilites(struct be_adapter *adapter,
 	struct be_cmd_req_get_ext_fat_caps *req;
 	int status;
 
+	if (!be_cmd_allowed(adapter, OPCODE_COMMON_GET_EXT_FAT_CAPABILITIES,
+			    CMD_SUBSYSTEM_COMMON))
+		return -EPERM;
+
 	if (mutex_lock_interruptible(&adapter->mbox_lock))
 		return -1;
 
@@ -4138,7 +4174,7 @@ int be_cmd_get_ext_fat_capabilites(struct be_adapter *adapter,
 
 	req = cmd->va;
 	be_wrb_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_COMMON,
-			       OPCODE_COMMON_GET_EXT_FAT_CAPABILITES,
+			       OPCODE_COMMON_GET_EXT_FAT_CAPABILITIES,
 			       cmd->size, wrb, cmd);
 	req->parameter_type = cpu_to_le32(1);
 
@@ -4156,7 +4192,7 @@ int be_cmd_set_ext_fat_capabilites(struct be_adapter *adapter,
 	struct be_cmd_req_set_ext_fat_caps *req;
 	int status;
 
-	spin_lock_bh(&adapter->mcc_lock);
+	mutex_lock(&adapter->mcc_lock);
 
 	wrb = wrb_from_mccq(adapter);
 	if (!wrb) {
@@ -4167,12 +4203,12 @@ int be_cmd_set_ext_fat_capabilites(struct be_adapter *adapter,
 	req = cmd->va;
 	memcpy(&req->set_params, configs, sizeof(struct be_fat_conf_params));
 	be_wrb_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_COMMON,
-			       OPCODE_COMMON_SET_EXT_FAT_CAPABILITES,
+			       OPCODE_COMMON_SET_EXT_FAT_CAPABILITIES,
 			       cmd->size, wrb, cmd);
 
 	status = be_mcc_notify_wait(adapter);
 err:
-	spin_unlock_bh(&adapter->mcc_lock);
+	mutex_unlock(&adapter->mcc_lock);
 	return status;
 }
 
@@ -4373,7 +4409,7 @@ err:
 }
 
 /* This routine returns a list of all the NIC PF_nums in the adapter */
-u16 be_get_nic_pf_num_list(u8 *buf, u32 desc_count, u16 *nic_pf_nums)
+static u16 be_get_nic_pf_num_list(u8 *buf, u32 desc_count, u16 *nic_pf_nums)
 {
 	struct be_res_desc_hdr *hdr = (struct be_res_desc_hdr *)buf;
 	struct be_pcie_res_desc *pcie = NULL;
@@ -4525,7 +4561,7 @@ static int be_cmd_set_profile_config(struct be_adapter *adapter, void *desc,
 }
 
 /* Mark all fields invalid */
-void be_reset_nic_desc(struct be_nic_res_desc *nic)
+static void be_reset_nic_desc(struct be_nic_res_desc *nic)
 {
 	memset(nic, 0, sizeof(*nic));
 	nic->unicast_mac_count = 0xFFFF;
@@ -4650,7 +4686,7 @@ int be_cmd_manage_iface(struct be_adapter *adapter, u32 iface, u8 op)
 	if (iface == 0xFFFFFFFF)
 		return -1;
 
-	spin_lock_bh(&adapter->mcc_lock);
+	mutex_lock(&adapter->mcc_lock);
 
 	wrb = wrb_from_mccq(adapter);
 	if (!wrb) {
@@ -4667,7 +4703,7 @@ int be_cmd_manage_iface(struct be_adapter *adapter, u32 iface, u8 op)
 
 	status = be_mcc_notify_wait(adapter);
 err:
-	spin_unlock_bh(&adapter->mcc_lock);
+	mutex_unlock(&adapter->mcc_lock);
 	return status;
 }
 
@@ -4701,7 +4737,7 @@ int be_cmd_get_if_id(struct be_adapter *adapter, struct be_vf_cfg *vf_cfg,
 	struct be_cmd_resp_get_iface_list *resp;
 	int status;
 
-	spin_lock_bh(&adapter->mcc_lock);
+	mutex_lock(&adapter->mcc_lock);
 
 	wrb = wrb_from_mccq(adapter);
 	if (!wrb) {
@@ -4722,7 +4758,7 @@ int be_cmd_get_if_id(struct be_adapter *adapter, struct be_vf_cfg *vf_cfg,
 	}
 
 err:
-	spin_unlock_bh(&adapter->mcc_lock);
+	mutex_unlock(&adapter->mcc_lock);
 	return status;
 }
 
@@ -4816,7 +4852,7 @@ int be_cmd_enable_vf(struct be_adapter *adapter, u8 domain)
 	if (BEx_chip(adapter))
 		return 0;
 
-	spin_lock_bh(&adapter->mcc_lock);
+	mutex_lock(&adapter->mcc_lock);
 
 	wrb = wrb_from_mccq(adapter);
 	if (!wrb) {
@@ -4834,7 +4870,7 @@ int be_cmd_enable_vf(struct be_adapter *adapter, u8 domain)
 	req->enable = 1;
 	status = be_mcc_notify_wait(adapter);
 err:
-	spin_unlock_bh(&adapter->mcc_lock);
+	mutex_unlock(&adapter->mcc_lock);
 	return status;
 }
 
@@ -4898,14 +4934,15 @@ err:
 	return status;
 }
 
-int __be_cmd_set_logical_link_config(struct be_adapter *adapter,
-				     int link_state, int version, u8 domain)
+static int
+__be_cmd_set_logical_link_config(struct be_adapter *adapter,
+				 int link_state, int version, u8 domain)
 {
 	struct be_mcc_wrb *wrb;
 	struct be_cmd_req_set_ll_link *req;
 	int status;
 
-	spin_lock_bh(&adapter->mcc_lock);
+	mutex_lock(&adapter->mcc_lock);
 
 	wrb = wrb_from_mccq(adapter);
 	if (!wrb) {
@@ -4931,7 +4968,7 @@ int __be_cmd_set_logical_link_config(struct be_adapter *adapter,
 
 	status = be_mcc_notify_wait(adapter);
 err:
-	spin_unlock_bh(&adapter->mcc_lock);
+	mutex_unlock(&adapter->mcc_lock);
 	return status;
 }
 
@@ -4940,7 +4977,7 @@ int be_cmd_set_logical_link_config(struct be_adapter *adapter,
 {
 	int status;
 
-	if (BEx_chip(adapter))
+	if (BE2_chip(adapter))
 		return -EOPNOTSUPP;
 
 	status = __be_cmd_set_logical_link_config(adapter, link_state,
@@ -4954,6 +4991,57 @@ int be_cmd_set_logical_link_config(struct be_adapter *adapter,
 							  1, domain);
 	return status;
 }
+
+int be_cmd_set_features(struct be_adapter *adapter)
+{
+	struct be_cmd_resp_set_features *resp;
+	struct be_cmd_req_set_features *req;
+	struct be_mcc_wrb *wrb;
+	int status;
+
+	if (mutex_lock_interruptible(&adapter->mcc_lock))
+		return -1;
+
+	wrb = wrb_from_mccq(adapter);
+	if (!wrb) {
+		status = -EBUSY;
+		goto err;
+	}
+
+	req = embedded_payload(wrb);
+
+	be_wrb_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_COMMON,
+			       OPCODE_COMMON_SET_FEATURES,
+			       sizeof(*req), wrb, NULL);
+
+	req->features = cpu_to_le32(BE_FEATURE_UE_RECOVERY);
+	req->parameter_len = cpu_to_le32(sizeof(struct be_req_ue_recovery));
+	req->parameter.req.uer = cpu_to_le32(BE_UE_RECOVERY_UER_MASK);
+
+	status = be_mcc_notify_wait(adapter);
+	if (status)
+		goto err;
+
+	resp = embedded_payload(wrb);
+
+	adapter->error_recovery.ue_to_poll_time =
+		le16_to_cpu(resp->parameter.resp.ue2rp);
+	adapter->error_recovery.ue_to_reset_time =
+		le16_to_cpu(resp->parameter.resp.ue2sr);
+	adapter->error_recovery.recovery_supported = true;
+err:
+	/* Checking "MCC_STATUS_INVALID_LENGTH" for SKH as FW
+	 * returns this error in older firmware versions
+	 */
+	if (base_status(status) == MCC_STATUS_ILLEGAL_REQUEST ||
+	    base_status(status) == MCC_STATUS_INVALID_LENGTH)
+		dev_info(&adapter->pdev->dev,
+			 "Adapter does not support HW error recovery\n");
+
+	mutex_unlock(&adapter->mcc_lock);
+	return status;
+}
+
 int be_roce_mcc_cmd(void *netdev_handle, void *wrb_payload,
 		    int wrb_payload_size, u16 *cmd_status, u16 *ext_status)
 {
@@ -4964,7 +5052,7 @@ int be_roce_mcc_cmd(void *netdev_handle, void *wrb_payload,
 	struct be_cmd_resp_hdr *resp;
 	int status;
 
-	spin_lock_bh(&adapter->mcc_lock);
+	mutex_lock(&adapter->mcc_lock);
 
 	wrb = wrb_from_mccq(adapter);
 	if (!wrb) {
@@ -4987,7 +5075,7 @@ int be_roce_mcc_cmd(void *netdev_handle, void *wrb_payload,
 	memcpy(wrb_payload, resp, sizeof(*resp) + resp->response_length);
 	be_dws_le_to_cpu(wrb_payload, sizeof(*resp) + resp->response_length);
 err:
-	spin_unlock_bh(&adapter->mcc_lock);
+	mutex_unlock(&adapter->mcc_lock);
 	return status;
 }
 EXPORT_SYMBOL(be_roce_mcc_cmd);
diff --git a/drivers/net/ethernet/emulex/benet/be_cmds.h b/drivers/net/ethernet/emulex/benet/be_cmds.h
index 0d6be224a787..09da2d82c2f0 100644
--- a/drivers/net/ethernet/emulex/benet/be_cmds.h
+++ b/drivers/net/ethernet/emulex/benet/be_cmds.h
@@ -58,7 +58,8 @@ enum mcc_base_status {
 	MCC_STATUS_INSUFFICIENT_BUFFER = 4,
 	MCC_STATUS_UNAUTHORIZED_REQUEST = 5,
 	MCC_STATUS_NOT_SUPPORTED = 66,
-	MCC_STATUS_FEATURE_NOT_SUPPORTED = 68
+	MCC_STATUS_FEATURE_NOT_SUPPORTED = 68,
+	MCC_STATUS_INVALID_LENGTH = 116
 };
 
 /* Additional status */
@@ -294,8 +295,8 @@ struct be_mcc_mailbox {
 #define OPCODE_COMMON_GET_PHY_DETAILS			102
 #define OPCODE_COMMON_SET_DRIVER_FUNCTION_CAP		103
 #define OPCODE_COMMON_GET_CNTL_ADDITIONAL_ATTRIBUTES	121
-#define OPCODE_COMMON_GET_EXT_FAT_CAPABILITES		125
-#define OPCODE_COMMON_SET_EXT_FAT_CAPABILITES		126
+#define OPCODE_COMMON_GET_EXT_FAT_CAPABILITIES		125
+#define OPCODE_COMMON_SET_EXT_FAT_CAPABILITIES		126
 #define OPCODE_COMMON_GET_MAC_LIST			147
 #define OPCODE_COMMON_SET_MAC_LIST			148
 #define OPCODE_COMMON_GET_HSW_CONFIG			152
@@ -308,6 +309,7 @@ struct be_mcc_mailbox {
 #define OPCODE_COMMON_READ_OBJECT			171
 #define OPCODE_COMMON_WRITE_OBJECT			172
 #define OPCODE_COMMON_DELETE_OBJECT			174
+#define OPCODE_COMMON_SET_FEATURES			191
 #define OPCODE_COMMON_MANAGE_IFACE_FILTERS		193
 #define OPCODE_COMMON_GET_IFACE_LIST			194
 #define OPCODE_COMMON_ENABLE_DISABLE_VF			196
@@ -1718,7 +1720,11 @@ struct mgmt_hba_attribs {
 	u32 rsvd2[55];
 	u8 rsvd3[3];
 	u8 phy_port;
-	u32 rsvd4[13];
+	u32 rsvd4[15];
+	u8 rsvd5[2];
+	u8 pci_funcnum;
+	u8 rsvd6;
+	u32 rsvd7[6];
 } __packed;
 
 struct mgmt_controller_attrib {
@@ -2315,6 +2321,41 @@ struct be_cmd_resp_get_iface_list {
 	struct be_if_desc if_desc;
 };
 
+/************** Set Features *******************/
+#define	BE_FEATURE_UE_RECOVERY		0x10
+#define	BE_UE_RECOVERY_UER_MASK		0x1
+
+struct be_req_ue_recovery {
+	u32	uer;
+	u32	rsvd;
+};
+
+struct be_cmd_req_set_features {
+	struct be_cmd_req_hdr hdr;
+	u32 features;
+	u32 parameter_len;
+	union {
+		struct be_req_ue_recovery req;
+		u32 rsvd[2];
+	} parameter;
+};
+
+struct be_resp_ue_recovery {
+	u32 uer;
+	u16 ue2rp;
+	u16 ue2sr;
+};
+
+struct be_cmd_resp_set_features {
+	struct be_cmd_resp_hdr hdr;
+	u32 features;
+	u32 parameter_len;
+	union {
+		struct be_resp_ue_recovery resp;
+		u32 rsvd[2];
+	} parameter;
+};
+
 /*************** Set logical link ********************/
 #define PLINK_ENABLE            BIT(0)
 #define PLINK_TRACK             BIT(8)
@@ -2343,6 +2384,7 @@ struct be_cmd_req_manage_iface_filters {
 	u32 cap_control_flags;
 } __packed;
 
+u16 be_POST_stage_get(struct be_adapter *adapter);
 int be_pci_fnum_get(struct be_adapter *adapter);
 int be_fw_wait_ready(struct be_adapter *adapter);
 int be_cmd_mac_addr_query(struct be_adapter *adapter, u8 *mac_addr,
@@ -2470,3 +2512,4 @@ int be_cmd_manage_iface(struct be_adapter *adapter, u32 iface, u8 op);
 int be_cmd_set_sriov_config(struct be_adapter *adapter,
 			    struct be_resources res, u16 num_vfs,
 			    struct be_resources *vft_res);
+int be_cmd_set_features(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 50e7be5da50c..0a48a31225e6 100644
--- a/drivers/net/ethernet/emulex/benet/be_ethtool.c
+++ b/drivers/net/ethernet/emulex/benet/be_ethtool.c
@@ -421,6 +421,10 @@ static void be_get_ethtool_stats(struct net_device *netdev,
 	}
 }
 
+static const char be_priv_flags[][ETH_GSTRING_LEN] = {
+	"disable-tpe-recovery"
+};
+
 static void be_get_stat_strings(struct net_device *netdev, uint32_t stringset,
 				uint8_t *data)
 {
@@ -454,6 +458,10 @@ static void be_get_stat_strings(struct net_device *netdev, uint32_t stringset,
 			data += ETH_GSTRING_LEN;
 		}
 		break;
+	case ETH_SS_PRIV_FLAGS:
+		for (i = 0; i < ARRAY_SIZE(be_priv_flags); i++)
+			strcpy(data + i * ETH_GSTRING_LEN, be_priv_flags[i]);
+		break;
 	}
 }
 
@@ -468,6 +476,8 @@ static int be_get_sset_count(struct net_device *netdev, int stringset)
 		return ETHTOOL_STATS_NUM +
 			adapter->num_rx_qs * ETHTOOL_RXSTATS_NUM +
 			adapter->num_tx_qs * ETHTOOL_TXSTATS_NUM;
+	case ETH_SS_PRIV_FLAGS:
+		return ARRAY_SIZE(be_priv_flags);
 	default:
 		return -EINVAL;
 	}
@@ -1360,6 +1370,34 @@ err:
 	return be_cmd_status(status);
 }
 
+static u32 be_get_priv_flags(struct net_device *netdev)
+{
+	struct be_adapter *adapter = netdev_priv(netdev);
+
+	return adapter->priv_flags;
+}
+
+static int be_set_priv_flags(struct net_device *netdev, u32 flags)
+{
+	struct be_adapter *adapter = netdev_priv(netdev);
+	bool tpe_old = !!(adapter->priv_flags & BE_DISABLE_TPE_RECOVERY);
+	bool tpe_new = !!(flags & BE_DISABLE_TPE_RECOVERY);
+
+	if (tpe_old != tpe_new) {
+		if (tpe_new) {
+			adapter->priv_flags |= BE_DISABLE_TPE_RECOVERY;
+			dev_info(&adapter->pdev->dev,
+				 "HW error recovery is disabled\n");
+		} else {
+			adapter->priv_flags &= ~BE_DISABLE_TPE_RECOVERY;
+			dev_info(&adapter->pdev->dev,
+				 "HW error recovery is enabled\n");
+		}
+	}
+
+	return 0;
+}
+
 const struct ethtool_ops be_ethtool_ops = {
 	.get_settings = be_get_settings,
 	.get_drvinfo = be_get_drvinfo,
@@ -1373,6 +1411,8 @@ const struct ethtool_ops be_ethtool_ops = {
 	.get_ringparam = be_get_ringparam,
 	.get_pauseparam = be_get_pauseparam,
 	.set_pauseparam = be_set_pauseparam,
+	.set_priv_flags = be_set_priv_flags,
+	.get_priv_flags = be_get_priv_flags,
 	.get_strings = be_get_stat_strings,
 	.set_phys_id = be_set_phys_id,
 	.set_dump = be_set_dump,
diff --git a/drivers/net/ethernet/emulex/benet/be_hw.h b/drivers/net/ethernet/emulex/benet/be_hw.h
index c684bb32b487..36e4232ed6b8 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 - 2015 Emulex
+ * Copyright (C) 2005-2016 Broadcom.
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or
@@ -32,18 +32,23 @@
 #define MPU_EP_CONTROL 		0
 
 /********** MPU semphore: used for SH & BE  *************/
+#define SLIPORT_SOFTRESET_OFFSET		0x5c	/* CSR BAR offset */
 #define SLIPORT_SEMAPHORE_OFFSET_BEx		0xac  /* CSR BAR offset */
 #define SLIPORT_SEMAPHORE_OFFSET_SH		0x94  /* PCI-CFG offset */
 #define POST_STAGE_MASK				0x0000FFFF
 #define POST_ERR_MASK				0x1
 #define POST_ERR_SHIFT				31
+#define POST_ERR_RECOVERY_CODE_MASK		0xFFF
+
+/* Soft Reset register masks */
+#define SLIPORT_SOFTRESET_SR_MASK		0x00000080	/* SR bit */
 
 /* MPU semphore POST stage values */
 #define POST_STAGE_AWAITING_HOST_RDY 	0x1 /* FW awaiting goahead from host */
 #define POST_STAGE_HOST_RDY 		0x2 /* Host has given go-ahed to FW */
 #define POST_STAGE_BE_RESET		0x3 /* Host wants to reset chip */
 #define POST_STAGE_ARMFW_RDY		0xc000	/* FW is done with POST */
-
+#define POST_STAGE_RECOVERABLE_ERR	0xE000	/* Recoverable err detected */
 
 /* Lancer SLIPORT registers */
 #define SLIPORT_STATUS_OFFSET		0x404
diff --git a/drivers/net/ethernet/emulex/benet/be_main.c b/drivers/net/ethernet/emulex/benet/be_main.c
index 874c7539a79d..cece8a08edca 100644
--- a/drivers/net/ethernet/emulex/benet/be_main.c
+++ b/drivers/net/ethernet/emulex/benet/be_main.c
@@ -41,6 +41,11 @@ static ushort rx_frag_size = 2048;
 module_param(rx_frag_size, ushort, S_IRUGO);
 MODULE_PARM_DESC(rx_frag_size, "Size of a fragment that holds rcvd data.");
 
+/* Per-module error detection/recovery workq shared across all functions.
+ * Each function schedules its own work request on this shared workq.
+ */
+static struct workqueue_struct *be_err_recovery_workq;
+
 static const struct pci_device_id be_dev_ids[] = {
 	{ PCI_DEVICE(BE_VENDOR_ID, BE_DEVICE_ID1) },
 	{ PCI_DEVICE(BE_VENDOR_ID, BE_DEVICE_ID2) },
@@ -53,6 +58,10 @@ static const struct pci_device_id be_dev_ids[] = {
 	{ 0 }
 };
 MODULE_DEVICE_TABLE(pci, be_dev_ids);
+
+/* Workqueue used by all functions for defering cmd calls to the adapter */
+static struct workqueue_struct *be_wq;
+
 /* UE Status Low CSR */
 static const char * const ue_status_low_desc[] = {
 	"CEV",
@@ -260,6 +269,38 @@ void be_cq_notify(struct be_adapter *adapter, u16 qid, bool arm, u16 num_popped)
 	iowrite32(val, adapter->db + DB_CQ_OFFSET);
 }
 
+static int be_dev_mac_add(struct be_adapter *adapter, u8 *mac)
+{
+	int i;
+
+	/* Check if mac has already been added as part of uc-list */
+	for (i = 0; i < adapter->uc_macs; i++) {
+		if (ether_addr_equal((u8 *)&adapter->uc_list[i * ETH_ALEN],
+				     mac)) {
+			/* mac already added, skip addition */
+			adapter->pmac_id[0] = adapter->pmac_id[i + 1];
+			return 0;
+		}
+	}
+
+	return be_cmd_pmac_add(adapter, mac, adapter->if_handle,
+			       &adapter->pmac_id[0], 0);
+}
+
+static void be_dev_mac_del(struct be_adapter *adapter, int pmac_id)
+{
+	int i;
+
+	/* Skip deletion if the programmed mac is
+	 * being used in uc-list
+	 */
+	for (i = 0; i < adapter->uc_macs; i++) {
+		if (adapter->pmac_id[i + 1] == pmac_id)
+			return;
+	}
+	be_cmd_pmac_del(adapter, adapter->if_handle, pmac_id, 0);
+}
+
 static int be_mac_addr_set(struct net_device *netdev, void *p)
 {
 	struct be_adapter *adapter = netdev_priv(netdev);
@@ -267,7 +308,7 @@ static int be_mac_addr_set(struct net_device *netdev, void *p)
 	struct sockaddr *addr = p;
 	int status;
 	u8 mac[ETH_ALEN];
-	u32 old_pmac_id = adapter->pmac_id[0], curr_pmac_id = 0;
+	u32 old_pmac_id = adapter->pmac_id[0];
 
 	if (!is_valid_ether_addr(addr->sa_data))
 		return -EADDRNOTAVAIL;
@@ -275,7 +316,7 @@ static int be_mac_addr_set(struct net_device *netdev, void *p)
 	/* Proceed further only if, User provided MAC is different
 	 * from active MAC
 	 */
-	if (ether_addr_equal(addr->sa_data, netdev->dev_addr))
+	if (ether_addr_equal(addr->sa_data, adapter->dev_mac))
 		return 0;
 
 	/* if device is not running, copy MAC to netdev->dev_addr */
@@ -288,23 +329,22 @@ static int be_mac_addr_set(struct net_device *netdev, void *p)
 	 * FILTMGMT privilege. This failure is OK, only if the PF programmed
 	 * the MAC for the VF.
 	 */
-	status = be_cmd_pmac_add(adapter, (u8 *)addr->sa_data,
-				 adapter->if_handle, &adapter->pmac_id[0], 0);
+	mutex_lock(&adapter->rx_filter_lock);
+	status = be_dev_mac_add(adapter, (u8 *)addr->sa_data);
 	if (!status) {
-		curr_pmac_id = adapter->pmac_id[0];
 
 		/* Delete the old programmed MAC. This call may fail if the
 		 * old MAC was already deleted by the PF driver.
 		 */
 		if (adapter->pmac_id[0] != old_pmac_id)
-			be_cmd_pmac_del(adapter, adapter->if_handle,
-					old_pmac_id, 0);
+			be_dev_mac_del(adapter, old_pmac_id);
 	}
 
+	mutex_unlock(&adapter->rx_filter_lock);
 	/* Decide if the new MAC is successfully activated only after
 	 * querying the FW
 	 */
-	status = be_cmd_get_active_mac(adapter, curr_pmac_id, mac,
+	status = be_cmd_get_active_mac(adapter, adapter->pmac_id[0], mac,
 				       adapter->if_handle, true, 0);
 	if (status)
 		goto err;
@@ -317,6 +357,7 @@ static int be_mac_addr_set(struct net_device *netdev, void *p)
 		goto err;
 	}
 done:
+	ether_addr_copy(adapter->dev_mac, addr->sa_data);
 	ether_addr_copy(netdev->dev_addr, addr->sa_data);
 	dev_info(dev, "MAC address changed to %pM\n", addr->sa_data);
 	return 0;
@@ -683,14 +724,24 @@ void be_link_status_update(struct be_adapter *adapter, u8 link_status)
 	netdev_info(netdev, "Link is %s\n", link_status ? "Up" : "Down");
 }
 
+static int be_gso_hdr_len(struct sk_buff *skb)
+{
+	if (skb->encapsulation)
+		return skb_inner_transport_offset(skb) +
+		       inner_tcp_hdrlen(skb);
+	return skb_transport_offset(skb) + tcp_hdrlen(skb);
+}
+
 static void be_tx_stats_update(struct be_tx_obj *txo, struct sk_buff *skb)
 {
 	struct be_tx_stats *stats = tx_stats(txo);
-	u64 tx_pkts = skb_shinfo(skb)->gso_segs ? : 1;
+	u32 tx_pkts = skb_shinfo(skb)->gso_segs ? : 1;
+	/* Account for headers which get duplicated in TSO pkt */
+	u32 dup_hdr_len = tx_pkts > 1 ? be_gso_hdr_len(skb) * (tx_pkts - 1) : 0;
 
 	u64_stats_update_begin(&stats->sync);
 	stats->tx_reqs++;
-	stats->tx_bytes += skb->len;
+	stats->tx_bytes += skb->len + dup_hdr_len;
 	stats->tx_pkts += tx_pkts;
 	if (skb->encapsulation && skb->ip_summed == CHECKSUM_PARTIAL)
 		stats->tx_vxlan_offload_pkts += tx_pkts;
@@ -1420,13 +1471,18 @@ static int be_vid_config(struct be_adapter *adapter)
 	u16 num = 0, i = 0;
 	int status = 0;
 
-	/* No need to further configure vids if in promiscuous mode */
-	if (be_in_all_promisc(adapter))
+	/* No need to change the VLAN state if the I/F is in promiscuous */
+	if (adapter->netdev->flags & IFF_PROMISC)
 		return 0;
 
 	if (adapter->vlans_added > be_max_vlans(adapter))
 		return be_set_vlan_promisc(adapter);
 
+	if (adapter->if_flags & BE_IF_FLAGS_VLAN_PROMISCUOUS) {
+		status = be_clear_vlan_promisc(adapter);
+		if (status)
+			return status;
+	}
 	/* Construct VLAN Table to give to HW */
 	for_each_set_bit(i, adapter->vids, VLAN_N_VID)
 		vids[num++] = cpu_to_le16(i);
@@ -1439,8 +1495,6 @@ static int be_vid_config(struct be_adapter *adapter)
 		    addl_status(status) ==
 				MCC_ADDL_STATUS_INSUFFICIENT_RESOURCES)
 			return be_set_vlan_promisc(adapter);
-	} else if (adapter->if_flags & BE_IF_FLAGS_VLAN_PROMISCUOUS) {
-		status = be_clear_vlan_promisc(adapter);
 	}
 	return status;
 }
@@ -1450,46 +1504,45 @@ static int be_vlan_add_vid(struct net_device *netdev, __be16 proto, u16 vid)
 	struct be_adapter *adapter = netdev_priv(netdev);
 	int status = 0;
 
+	mutex_lock(&adapter->rx_filter_lock);
+
 	/* Packets with VID 0 are always received by Lancer by default */
 	if (lancer_chip(adapter) && vid == 0)
-		return status;
+		goto done;
 
 	if (test_bit(vid, adapter->vids))
-		return status;
+		goto done;
 
 	set_bit(vid, adapter->vids);
 	adapter->vlans_added++;
 
 	status = be_vid_config(adapter);
-	if (status) {
-		adapter->vlans_added--;
-		clear_bit(vid, adapter->vids);
-	}
-
+done:
+	mutex_unlock(&adapter->rx_filter_lock);
 	return status;
 }
 
 static int be_vlan_rem_vid(struct net_device *netdev, __be16 proto, u16 vid)
 {
 	struct be_adapter *adapter = netdev_priv(netdev);
+	int status = 0;
+
+	mutex_lock(&adapter->rx_filter_lock);
 
 	/* Packets with VID 0 are always received by Lancer by default */
 	if (lancer_chip(adapter) && vid == 0)
-		return 0;
+		goto done;
 
 	if (!test_bit(vid, adapter->vids))
-		return 0;
+		goto done;
 
 	clear_bit(vid, adapter->vids);
 	adapter->vlans_added--;
 
-	return be_vid_config(adapter);
-}
-
-static void be_clear_all_promisc(struct be_adapter *adapter)
-{
-	be_cmd_rx_filter(adapter, BE_IF_FLAGS_ALL_PROMISCUOUS, OFF);
-	adapter->if_flags &= ~BE_IF_FLAGS_ALL_PROMISCUOUS;
+	status = be_vid_config(adapter);
+done:
+	mutex_unlock(&adapter->rx_filter_lock);
+	return status;
 }
 
 static void be_set_all_promisc(struct be_adapter *adapter)
@@ -1510,75 +1563,226 @@ static void be_set_mc_promisc(struct be_adapter *adapter)
 		adapter->if_flags |= BE_IF_FLAGS_MCAST_PROMISCUOUS;
 }
 
-static void be_set_mc_list(struct be_adapter *adapter)
+static void be_set_uc_promisc(struct be_adapter *adapter)
 {
 	int status;
 
-	status = be_cmd_rx_filter(adapter, BE_IF_FLAGS_MULTICAST, ON);
+	if (adapter->if_flags & BE_IF_FLAGS_PROMISCUOUS)
+		return;
+
+	status = be_cmd_rx_filter(adapter, BE_IF_FLAGS_PROMISCUOUS, ON);
 	if (!status)
-		adapter->if_flags &= ~BE_IF_FLAGS_MCAST_PROMISCUOUS;
-	else
+		adapter->if_flags |= BE_IF_FLAGS_PROMISCUOUS;
+}
+
+static void be_clear_uc_promisc(struct be_adapter *adapter)
+{
+	int status;
+
+	if (!(adapter->if_flags & BE_IF_FLAGS_PROMISCUOUS))
+		return;
+
+	status = be_cmd_rx_filter(adapter, BE_IF_FLAGS_PROMISCUOUS, OFF);
+	if (!status)
+		adapter->if_flags &= ~BE_IF_FLAGS_PROMISCUOUS;
+}
+
+/* The below 2 functions are the callback args for __dev_mc_sync/dev_uc_sync().
+ * We use a single callback function for both sync and unsync. We really don't
+ * add/remove addresses through this callback. But, we use it to detect changes
+ * to the uc/mc lists. The entire uc/mc list is programmed in be_set_rx_mode().
+ */
+static int be_uc_list_update(struct net_device *netdev,
+			     const unsigned char *addr)
+{
+	struct be_adapter *adapter = netdev_priv(netdev);
+
+	adapter->update_uc_list = true;
+	return 0;
+}
+
+static int be_mc_list_update(struct net_device *netdev,
+			     const unsigned char *addr)
+{
+	struct be_adapter *adapter = netdev_priv(netdev);
+
+	adapter->update_mc_list = true;
+	return 0;
+}
+
+static void be_set_mc_list(struct be_adapter *adapter)
+{
+	struct net_device *netdev = adapter->netdev;
+	struct netdev_hw_addr *ha;
+	bool mc_promisc = false;
+	int status;
+
+	netif_addr_lock_bh(netdev);
+	__dev_mc_sync(netdev, be_mc_list_update, be_mc_list_update);
+
+	if (netdev->flags & IFF_PROMISC) {
+		adapter->update_mc_list = false;
+	} else if (netdev->flags & IFF_ALLMULTI ||
+		   netdev_mc_count(netdev) > be_max_mc(adapter)) {
+		/* Enable multicast promisc if num configured exceeds
+		 * what we support
+		 */
+		mc_promisc = true;
+		adapter->update_mc_list = false;
+	} else if (adapter->if_flags & BE_IF_FLAGS_MCAST_PROMISCUOUS) {
+		/* Update mc-list unconditionally if the iface was previously
+		 * in mc-promisc mode and now is out of that mode.
+		 */
+		adapter->update_mc_list = true;
+	}
+
+	if (adapter->update_mc_list) {
+		int i = 0;
+
+		/* cache the mc-list in adapter */
+		netdev_for_each_mc_addr(ha, netdev) {
+			ether_addr_copy(adapter->mc_list[i].mac, ha->addr);
+			i++;
+		}
+		adapter->mc_count = netdev_mc_count(netdev);
+	}
+	netif_addr_unlock_bh(netdev);
+
+	if (mc_promisc) {
 		be_set_mc_promisc(adapter);
+	} else if (adapter->update_mc_list) {
+		status = be_cmd_rx_filter(adapter, BE_IF_FLAGS_MULTICAST, ON);
+		if (!status)
+			adapter->if_flags &= ~BE_IF_FLAGS_MCAST_PROMISCUOUS;
+		else
+			be_set_mc_promisc(adapter);
+
+		adapter->update_mc_list = false;
+	}
+}
+
+static void be_clear_mc_list(struct be_adapter *adapter)
+{
+	struct net_device *netdev = adapter->netdev;
+
+	__dev_mc_unsync(netdev, NULL);
+	be_cmd_rx_filter(adapter, BE_IF_FLAGS_MULTICAST, OFF);
+	adapter->mc_count = 0;
+}
+
+static int be_uc_mac_add(struct be_adapter *adapter, int uc_idx)
+{
+	if (ether_addr_equal((u8 *)&adapter->uc_list[uc_idx * ETH_ALEN],
+			     adapter->dev_mac)) {
+		adapter->pmac_id[uc_idx + 1] = adapter->pmac_id[0];
+		return 0;
+	}
+
+	return be_cmd_pmac_add(adapter,
+			       (u8 *)&adapter->uc_list[uc_idx * ETH_ALEN],
+			       adapter->if_handle,
+			       &adapter->pmac_id[uc_idx + 1], 0);
+}
+
+static void be_uc_mac_del(struct be_adapter *adapter, int pmac_id)
+{
+	if (pmac_id == adapter->pmac_id[0])
+		return;
+
+	be_cmd_pmac_del(adapter, adapter->if_handle, pmac_id, 0);
 }
 
 static void be_set_uc_list(struct be_adapter *adapter)
 {
+	struct net_device *netdev = adapter->netdev;
 	struct netdev_hw_addr *ha;
-	int i = 1; /* First slot is claimed by the Primary MAC */
+	bool uc_promisc = false;
+	int curr_uc_macs = 0, i;
 
-	for (; adapter->uc_macs > 0; adapter->uc_macs--, i++)
-		be_cmd_pmac_del(adapter, adapter->if_handle,
-				adapter->pmac_id[i], 0);
+	netif_addr_lock_bh(netdev);
+	__dev_uc_sync(netdev, be_uc_list_update, be_uc_list_update);
 
-	if (netdev_uc_count(adapter->netdev) > be_max_uc(adapter)) {
-		be_set_all_promisc(adapter);
-		return;
+	if (netdev->flags & IFF_PROMISC) {
+		adapter->update_uc_list = false;
+	} else if (netdev_uc_count(netdev) > (be_max_uc(adapter) - 1)) {
+		uc_promisc = true;
+		adapter->update_uc_list = false;
+	}  else if (adapter->if_flags & BE_IF_FLAGS_PROMISCUOUS) {
+		/* Update uc-list unconditionally if the iface was previously
+		 * in uc-promisc mode and now is out of that mode.
+		 */
+		adapter->update_uc_list = true;
+	}
+
+	if (adapter->update_uc_list) {
+		i = 1; /* First slot is claimed by the Primary MAC */
+
+		/* cache the uc-list in adapter array */
+		netdev_for_each_uc_addr(ha, netdev) {
+			ether_addr_copy(adapter->uc_list[i].mac, ha->addr);
+			i++;
+		}
+		curr_uc_macs = netdev_uc_count(netdev);
 	}
+	netif_addr_unlock_bh(netdev);
+
+	if (uc_promisc) {
+		be_set_uc_promisc(adapter);
+	} else if (adapter->update_uc_list) {
+		be_clear_uc_promisc(adapter);
+
+		for (i = 0; i < adapter->uc_macs; i++)
+			be_uc_mac_del(adapter, adapter->pmac_id[i + 1]);
 
-	netdev_for_each_uc_addr(ha, adapter->netdev) {
-		adapter->uc_macs++; /* First slot is for Primary MAC */
-		be_cmd_pmac_add(adapter, (u8 *)ha->addr, adapter->if_handle,
-				&adapter->pmac_id[adapter->uc_macs], 0);
+		for (i = 0; i < curr_uc_macs; i++)
+			be_uc_mac_add(adapter, i);
+		adapter->uc_macs = curr_uc_macs;
+		adapter->update_uc_list = false;
 	}
 }
 
 static void be_clear_uc_list(struct be_adapter *adapter)
 {
+	struct net_device *netdev = adapter->netdev;
 	int i;
 
-	for (i = 1; i < (adapter->uc_macs + 1); i++)
-		be_cmd_pmac_del(adapter, adapter->if_handle,
-				adapter->pmac_id[i], 0);
+	__dev_uc_unsync(netdev, NULL);
+	for (i = 0; i < adapter->uc_macs; i++)
+		be_uc_mac_del(adapter, adapter->pmac_id[i + 1]);
+
 	adapter->uc_macs = 0;
 }
 
-static void be_set_rx_mode(struct net_device *netdev)
+static void __be_set_rx_mode(struct be_adapter *adapter)
 {
-	struct be_adapter *adapter = netdev_priv(netdev);
+	struct net_device *netdev = adapter->netdev;
+
+	mutex_lock(&adapter->rx_filter_lock);
 
 	if (netdev->flags & IFF_PROMISC) {
-		be_set_all_promisc(adapter);
-		return;
+		if (!be_in_all_promisc(adapter))
+			be_set_all_promisc(adapter);
+	} else if (be_in_all_promisc(adapter)) {
+		/* We need to re-program the vlan-list or clear
+		 * vlan-promisc mode (if needed) when the interface
+		 * comes out of promisc mode.
+		 */
+		be_vid_config(adapter);
 	}
 
-	/* Interface was previously in promiscuous mode; disable it */
-	if (be_in_all_promisc(adapter)) {
-		be_clear_all_promisc(adapter);
-		if (adapter->vlans_added)
-			be_vid_config(adapter);
-	}
+	be_set_uc_list(adapter);
+	be_set_mc_list(adapter);
 
-	/* Enable multicast promisc if num configured exceeds what we support */
-	if (netdev->flags & IFF_ALLMULTI ||
-	    netdev_mc_count(netdev) > be_max_mc(adapter)) {
-		be_set_mc_promisc(adapter);
-		return;
-	}
+	mutex_unlock(&adapter->rx_filter_lock);
+}
 
-	if (netdev_uc_count(netdev) != adapter->uc_macs)
-		be_set_uc_list(adapter);
+static void be_work_set_rx_mode(struct work_struct *work)
+{
+	struct be_cmd_work *cmd_work =
+				container_of(work, struct be_cmd_work, work);
 
-	be_set_mc_list(adapter);
+	__be_set_rx_mode(cmd_work->adapter);
+	kfree(cmd_work);
 }
 
 static int be_set_vf_mac(struct net_device *netdev, int vf, u8 *mac)
@@ -1701,7 +1905,8 @@ static int be_clear_vf_tvt(struct be_adapter *adapter, int vf)
 	return 0;
 }
 
-static int be_set_vf_vlan(struct net_device *netdev, int vf, u16 vlan, u8 qos)
+static int be_set_vf_vlan(struct net_device *netdev, int vf, u16 vlan, u8 qos,
+			  __be16 vlan_proto)
 {
 	struct be_adapter *adapter = netdev_priv(netdev);
 	struct be_vf_cfg *vf_cfg = &adapter->vf_cfg[vf];
@@ -1713,6 +1918,9 @@ static int be_set_vf_vlan(struct net_device *netdev, int vf, u16 vlan, u8 qos)
 	if (vf >= adapter->num_vfs || vlan > 4095 || qos > 7)
 		return -EINVAL;
 
+	if (vlan_proto != htons(ETH_P_8021Q))
+		return -EPROTONOSUPPORT;
+
 	if (vlan || qos) {
 		vlan |= qos << VLAN_PRIO_SHIFT;
 		status = be_set_vf_tvt(adapter, vf, vlan);
@@ -3220,9 +3428,7 @@ void be_detect_error(struct be_adapter *adapter)
 		 */
 
 		if (ue_lo || ue_hi) {
-			dev_err(dev,
-				"Unrecoverable Error detected in the adapter");
-			dev_err(dev, "Please reboot server to recover");
+			dev_err(dev, "Error detected in the adapter");
 			if (skyhawk_chip(adapter))
 				be_set_error(adapter, BE_ERROR_UE);
 
@@ -3425,10 +3631,9 @@ static void be_rx_qs_destroy(struct be_adapter *adapter)
 
 static void be_disable_if_filters(struct be_adapter *adapter)
 {
-	be_cmd_pmac_del(adapter, adapter->if_handle,
-			adapter->pmac_id[0], 0);
-
+	be_dev_mac_del(adapter, adapter->pmac_id[0]);
 	be_clear_uc_list(adapter);
+	be_clear_mc_list(adapter);
 
 	/* The IFACE flags are enabled in the open path and cleared
 	 * in the close path. When a VF gets detached from the host and
@@ -3462,6 +3667,11 @@ static int be_close(struct net_device *netdev)
 	if (!(adapter->flags & BE_FLAGS_SETUP_DONE))
 		return 0;
 
+	/* Before attempting cleanup ensure all the pending cmds in the
+	 * config_wq have finished execution
+	 */
+	flush_workqueue(be_wq);
+
 	be_disable_if_filters(adapter);
 
 	if (adapter->flags & BE_FLAGS_NAPI_ENABLED) {
@@ -3576,17 +3786,16 @@ static int be_enable_if_filters(struct be_adapter *adapter)
 
 	/* For BE3 VFs, the PF programs the initial MAC address */
 	if (!(BEx_chip(adapter) && be_virtfn(adapter))) {
-		status = be_cmd_pmac_add(adapter, adapter->netdev->dev_addr,
-					 adapter->if_handle,
-					 &adapter->pmac_id[0], 0);
+		status = be_dev_mac_add(adapter, adapter->netdev->dev_addr);
 		if (status)
 			return status;
+		ether_addr_copy(adapter->dev_mac, adapter->netdev->dev_addr);
 	}
 
 	if (adapter->vlans_added)
 		be_vid_config(adapter);
 
-	be_set_rx_mode(adapter->netdev);
+	__be_set_rx_mode(adapter);
 
 	return 0;
 }
@@ -3759,8 +3968,13 @@ static void be_cancel_worker(struct be_adapter *adapter)
 
 static void be_cancel_err_detection(struct be_adapter *adapter)
 {
+	struct be_error_recovery *err_rec = &adapter->error_recovery;
+
+	if (!be_err_recovery_workq)
+		return;
+
 	if (adapter->flags & BE_FLAGS_ERR_DETECTION_SCHEDULED) {
-		cancel_delayed_work_sync(&adapter->be_err_detection_work);
+		cancel_delayed_work_sync(&err_rec->err_detection_work);
 		adapter->flags &= ~BE_FLAGS_ERR_DETECTION_SCHEDULED;
 	}
 }
@@ -3860,6 +4074,20 @@ static void be_calculate_vf_res(struct be_adapter *adapter, u16 num_vfs,
 		vft_res->max_mcc_count = res.max_mcc_count / (num_vfs + 1);
 }
 
+static void be_if_destroy(struct be_adapter *adapter)
+{
+	be_cmd_if_destroy(adapter, adapter->if_handle,  0);
+
+	kfree(adapter->pmac_id);
+	adapter->pmac_id = NULL;
+
+	kfree(adapter->mc_list);
+	adapter->mc_list = NULL;
+
+	kfree(adapter->uc_list);
+	adapter->uc_list = NULL;
+}
+
 static int be_clear(struct be_adapter *adapter)
 {
 	struct pci_dev *pdev = adapter->pdev;
@@ -3867,6 +4095,8 @@ static int be_clear(struct be_adapter *adapter)
 
 	be_cancel_worker(adapter);
 
+	flush_workqueue(be_wq);
+
 	if (sriov_enabled(adapter))
 		be_vf_clear(adapter);
 
@@ -3884,10 +4114,8 @@ static int be_clear(struct be_adapter *adapter)
 	}
 
 	be_disable_vxlan_offloads(adapter);
-	kfree(adapter->pmac_id);
-	adapter->pmac_id = NULL;
 
-	be_cmd_if_destroy(adapter, adapter->if_handle,  0);
+	be_if_destroy(adapter);
 
 	be_clear_queues(adapter);
 
@@ -4151,7 +4379,7 @@ static void be_setup_init(struct be_adapter *adapter)
  * for distribution between the VFs. This self-imposed limit will determine the
  * no: of VFs for which RSS can be enabled.
  */
-void be_calculate_pf_pool_rss_tables(struct be_adapter *adapter)
+static void be_calculate_pf_pool_rss_tables(struct be_adapter *adapter)
 {
 	struct be_port_resources port_res = {0};
 	u8 rss_tables_on_port;
@@ -4341,14 +4569,29 @@ static int be_mac_setup(struct be_adapter *adapter)
 
 static void be_schedule_worker(struct be_adapter *adapter)
 {
-	schedule_delayed_work(&adapter->work, msecs_to_jiffies(1000));
+	queue_delayed_work(be_wq, &adapter->work, msecs_to_jiffies(1000));
 	adapter->flags |= BE_FLAGS_WORKER_SCHEDULED;
 }
 
+static void be_destroy_err_recovery_workq(void)
+{
+	if (!be_err_recovery_workq)
+		return;
+
+	flush_workqueue(be_err_recovery_workq);
+	destroy_workqueue(be_err_recovery_workq);
+	be_err_recovery_workq = NULL;
+}
+
 static void be_schedule_err_detection(struct be_adapter *adapter, u32 delay)
 {
-	schedule_delayed_work(&adapter->be_err_detection_work,
-			      msecs_to_jiffies(delay));
+	struct be_error_recovery *err_rec = &adapter->error_recovery;
+
+	if (!be_err_recovery_workq)
+		return;
+
+	queue_delayed_work(be_err_recovery_workq, &err_rec->err_detection_work,
+			   msecs_to_jiffies(delay));
 	adapter->flags |= BE_FLAGS_ERR_DETECTION_SCHEDULED;
 }
 
@@ -4393,6 +4636,22 @@ static int be_if_create(struct be_adapter *adapter)
 	u32 cap_flags = be_if_cap_flags(adapter);
 	int status;
 
+	/* alloc required memory for other filtering fields */
+	adapter->pmac_id = kcalloc(be_max_uc(adapter),
+				   sizeof(*adapter->pmac_id), GFP_KERNEL);
+	if (!adapter->pmac_id)
+		return -ENOMEM;
+
+	adapter->mc_list = kcalloc(be_max_mc(adapter),
+				   sizeof(*adapter->mc_list), GFP_KERNEL);
+	if (!adapter->mc_list)
+		return -ENOMEM;
+
+	adapter->uc_list = kcalloc(be_max_uc(adapter),
+				   sizeof(*adapter->uc_list), GFP_KERNEL);
+	if (!adapter->uc_list)
+		return -ENOMEM;
+
 	if (adapter->cfg_num_rx_irqs == 1)
 		cap_flags &= ~(BE_IF_FLAGS_DEFQ_RSS | BE_IF_FLAGS_RSS);
 
@@ -4401,7 +4660,10 @@ static int be_if_create(struct be_adapter *adapter)
 	status = be_cmd_if_create(adapter, be_if_cap_flags(adapter), en_flags,
 				  &adapter->if_handle, 0);
 
-	return status;
+	if (status)
+		return status;
+
+	return 0;
 }
 
 int be_update_queues(struct be_adapter *adapter)
@@ -4458,10 +4720,15 @@ static inline int fw_major_num(const char *fw_ver)
 	return fw_major;
 }
 
-/* If any VFs are already enabled don't FLR the PF */
+/* If it is error recovery, FLR the PF
+ * Else if any VFs are already enabled don't FLR the PF
+ */
 static bool be_reset_required(struct be_adapter *adapter)
 {
-	return pci_num_vf(adapter->pdev) ? false : true;
+	if (be_error_recovering(adapter))
+		return true;
+	else
+		return pci_num_vf(adapter->pdev) == 0;
 }
 
 /* Wait for the FW to be ready and perform the required initialization */
@@ -4473,6 +4740,9 @@ static int be_func_init(struct be_adapter *adapter)
 	if (status)
 		return status;
 
+	/* FW is now ready; clear errors to allow cmds/doorbell */
+	be_clear_error(adapter, BE_CLEAR_ALL);
+
 	if (be_reset_required(adapter)) {
 		status = be_cmd_reset_function(adapter);
 		if (status)
@@ -4480,9 +4750,6 @@ static int be_func_init(struct be_adapter *adapter)
 
 		/* Wait for interrupts to quiesce after an FLR */
 		msleep(100);
-
-		/* We can clear all errors when function reset succeeds */
-		be_clear_error(adapter, BE_CLEAR_ALL);
 	}
 
 	/* Tell FW we're ready to fire cmds */
@@ -4530,11 +4797,6 @@ static int be_setup(struct be_adapter *adapter)
 	if (status)
 		goto err;
 
-	adapter->pmac_id = kcalloc(be_max_uc(adapter),
-				   sizeof(*adapter->pmac_id), GFP_KERNEL);
-	if (!adapter->pmac_id)
-		return -ENOMEM;
-
 	status = be_msix_enable(adapter);
 	if (status)
 		goto err;
@@ -4595,6 +4857,9 @@ static int be_setup(struct be_adapter *adapter)
 	if (!status && be_pause_supported(adapter))
 		adapter->phy.fc_autoneg = 1;
 
+	if (be_physfn(adapter) && !lancer_chip(adapter))
+		be_cmd_set_features(adapter);
+
 	be_schedule_worker(adapter);
 	adapter->flags |= BE_FLAGS_SETUP_DONE;
 	return 0;
@@ -4728,6 +4993,23 @@ static int be_ndo_bridge_getlink(struct sk_buff *skb, u32 pid, u32 seq,
 				       0, 0, nlflags, filter_mask, NULL);
 }
 
+static struct be_cmd_work *be_alloc_work(struct be_adapter *adapter,
+					 void (*func)(struct work_struct *))
+{
+	struct be_cmd_work *work;
+
+	work = kzalloc(sizeof(*work), GFP_ATOMIC);
+	if (!work) {
+		dev_err(&adapter->pdev->dev,
+			"be_work memory allocation failed\n");
+		return NULL;
+	}
+
+	INIT_WORK(&work->work, func);
+	work->adapter = adapter;
+	return work;
+}
+
 /* VxLAN offload Notes:
  *
  * The stack defines tunnel offload flags (hw_enc_features) for IP and doesn't
@@ -4742,23 +5024,19 @@ static int be_ndo_bridge_getlink(struct sk_buff *skb, u32 pid, u32 seq,
  * adds more than one port, disable offloads and don't re-enable them again
  * until after all the tunnels are removed.
  */
-static void be_add_vxlan_port(struct net_device *netdev,
-			      struct udp_tunnel_info *ti)
+static void be_work_add_vxlan_port(struct work_struct *work)
 {
-	struct be_adapter *adapter = netdev_priv(netdev);
+	struct be_cmd_work *cmd_work =
+				container_of(work, struct be_cmd_work, work);
+	struct be_adapter *adapter = cmd_work->adapter;
+	struct net_device *netdev = adapter->netdev;
 	struct device *dev = &adapter->pdev->dev;
-	__be16 port = ti->port;
+	__be16 port = cmd_work->info.vxlan_port;
 	int status;
 
-	if (ti->type != UDP_TUNNEL_TYPE_VXLAN)
-		return;
-
-	if (lancer_chip(adapter) || BEx_chip(adapter) || be_is_mc(adapter))
-		return;
-
 	if (adapter->vxlan_port == port && adapter->vxlan_port_count) {
 		adapter->vxlan_port_aliases++;
-		return;
+		goto done;
 	}
 
 	if (adapter->flags & BE_FLAGS_VXLAN_OFFLOADS) {
@@ -4770,7 +5048,7 @@ static void be_add_vxlan_port(struct net_device *netdev,
 	}
 
 	if (adapter->vxlan_port_count++ >= 1)
-		return;
+		goto done;
 
 	status = be_cmd_manage_iface(adapter, adapter->if_handle,
 				     OP_CONVERT_NORMAL_TO_TUNNEL);
@@ -4795,29 +5073,26 @@ static void be_add_vxlan_port(struct net_device *netdev,
 
 	dev_info(dev, "Enabled VxLAN offloads for UDP port %d\n",
 		 be16_to_cpu(port));
-	return;
+	goto done;
 err:
 	be_disable_vxlan_offloads(adapter);
+done:
+	kfree(cmd_work);
 }
 
-static void be_del_vxlan_port(struct net_device *netdev,
-			      struct udp_tunnel_info *ti)
+static void be_work_del_vxlan_port(struct work_struct *work)
 {
-	struct be_adapter *adapter = netdev_priv(netdev);
-	__be16 port = ti->port;
-
-	if (ti->type != UDP_TUNNEL_TYPE_VXLAN)
-		return;
-
-	if (lancer_chip(adapter) || BEx_chip(adapter) || be_is_mc(adapter))
-		return;
+	struct be_cmd_work *cmd_work =
+				container_of(work, struct be_cmd_work, work);
+	struct be_adapter *adapter = cmd_work->adapter;
+	__be16 port = cmd_work->info.vxlan_port;
 
 	if (adapter->vxlan_port != port)
 		goto done;
 
 	if (adapter->vxlan_port_aliases) {
 		adapter->vxlan_port_aliases--;
-		return;
+		goto out;
 	}
 
 	be_disable_vxlan_offloads(adapter);
@@ -4827,6 +5102,40 @@ static void be_del_vxlan_port(struct net_device *netdev,
 		 be16_to_cpu(port));
 done:
 	adapter->vxlan_port_count--;
+out:
+	kfree(cmd_work);
+}
+
+static void be_cfg_vxlan_port(struct net_device *netdev,
+			      struct udp_tunnel_info *ti,
+			      void (*func)(struct work_struct *))
+{
+	struct be_adapter *adapter = netdev_priv(netdev);
+	struct be_cmd_work *cmd_work;
+
+	if (ti->type != UDP_TUNNEL_TYPE_VXLAN)
+		return;
+
+	if (lancer_chip(adapter) || BEx_chip(adapter) || be_is_mc(adapter))
+		return;
+
+	cmd_work = be_alloc_work(adapter, func);
+	if (cmd_work) {
+		cmd_work->info.vxlan_port = ti->port;
+		queue_work(be_wq, &cmd_work->work);
+	}
+}
+
+static void be_del_vxlan_port(struct net_device *netdev,
+			      struct udp_tunnel_info *ti)
+{
+	be_cfg_vxlan_port(netdev, ti, be_work_del_vxlan_port);
+}
+
+static void be_add_vxlan_port(struct net_device *netdev,
+			      struct udp_tunnel_info *ti)
+{
+	be_cfg_vxlan_port(netdev, ti, be_work_add_vxlan_port);
 }
 
 static netdev_features_t be_features_check(struct sk_buff *skb,
@@ -4891,6 +5200,16 @@ static int be_get_phys_port_id(struct net_device *dev,
 	return 0;
 }
 
+static void be_set_rx_mode(struct net_device *dev)
+{
+	struct be_adapter *adapter = netdev_priv(dev);
+	struct be_cmd_work *work;
+
+	work = be_alloc_work(adapter, be_work_set_rx_mode);
+	if (work)
+		queue_work(be_wq, &work->work);
+}
+
 static const struct net_device_ops be_netdev_ops = {
 	.ndo_open		= be_open,
 	.ndo_stop		= be_close,
@@ -4984,13 +5303,145 @@ static int be_resume(struct be_adapter *adapter)
 	return 0;
 }
 
+static void be_soft_reset(struct be_adapter *adapter)
+{
+	u32 val;
+
+	dev_info(&adapter->pdev->dev, "Initiating chip soft reset\n");
+	val = ioread32(adapter->pcicfg + SLIPORT_SOFTRESET_OFFSET);
+	val |= SLIPORT_SOFTRESET_SR_MASK;
+	iowrite32(val, adapter->pcicfg + SLIPORT_SOFTRESET_OFFSET);
+}
+
+static bool be_err_is_recoverable(struct be_adapter *adapter)
+{
+	struct be_error_recovery *err_rec = &adapter->error_recovery;
+	unsigned long initial_idle_time =
+		msecs_to_jiffies(ERR_RECOVERY_IDLE_TIME);
+	unsigned long recovery_interval =
+		msecs_to_jiffies(ERR_RECOVERY_INTERVAL);
+	u16 ue_err_code;
+	u32 val;
+
+	val = be_POST_stage_get(adapter);
+	if ((val & POST_STAGE_RECOVERABLE_ERR) != POST_STAGE_RECOVERABLE_ERR)
+		return false;
+	ue_err_code = val & POST_ERR_RECOVERY_CODE_MASK;
+	if (ue_err_code == 0)
+		return false;
+
+	dev_err(&adapter->pdev->dev, "Recoverable HW error code: 0x%x\n",
+		ue_err_code);
+
+	if (jiffies - err_rec->probe_time <= initial_idle_time) {
+		dev_err(&adapter->pdev->dev,
+			"Cannot recover within %lu sec from driver load\n",
+			jiffies_to_msecs(initial_idle_time) / MSEC_PER_SEC);
+		return false;
+	}
+
+	if (err_rec->last_recovery_time &&
+	    (jiffies - err_rec->last_recovery_time <= recovery_interval)) {
+		dev_err(&adapter->pdev->dev,
+			"Cannot recover within %lu sec from last recovery\n",
+			jiffies_to_msecs(recovery_interval) / MSEC_PER_SEC);
+		return false;
+	}
+
+	if (ue_err_code == err_rec->last_err_code) {
+		dev_err(&adapter->pdev->dev,
+			"Cannot recover from a consecutive TPE error\n");
+		return false;
+	}
+
+	err_rec->last_recovery_time = jiffies;
+	err_rec->last_err_code = ue_err_code;
+	return true;
+}
+
+static int be_tpe_recover(struct be_adapter *adapter)
+{
+	struct be_error_recovery *err_rec = &adapter->error_recovery;
+	int status = -EAGAIN;
+	u32 val;
+
+	switch (err_rec->recovery_state) {
+	case ERR_RECOVERY_ST_NONE:
+		err_rec->recovery_state = ERR_RECOVERY_ST_DETECT;
+		err_rec->resched_delay = ERR_RECOVERY_UE_DETECT_DURATION;
+		break;
+
+	case ERR_RECOVERY_ST_DETECT:
+		val = be_POST_stage_get(adapter);
+		if ((val & POST_STAGE_RECOVERABLE_ERR) !=
+		    POST_STAGE_RECOVERABLE_ERR) {
+			dev_err(&adapter->pdev->dev,
+				"Unrecoverable HW error detected: 0x%x\n", val);
+			status = -EINVAL;
+			err_rec->resched_delay = 0;
+			break;
+		}
+
+		dev_err(&adapter->pdev->dev, "Recoverable HW error detected\n");
+
+		/* Only PF0 initiates Chip Soft Reset. But PF0 must wait UE2SR
+		 * milliseconds before it checks for final error status in
+		 * SLIPORT_SEMAPHORE to determine if recovery criteria is met.
+		 * If it does, then PF0 initiates a Soft Reset.
+		 */
+		if (adapter->pf_num == 0) {
+			err_rec->recovery_state = ERR_RECOVERY_ST_RESET;
+			err_rec->resched_delay = err_rec->ue_to_reset_time -
+					ERR_RECOVERY_UE_DETECT_DURATION;
+			break;
+		}
+
+		err_rec->recovery_state = ERR_RECOVERY_ST_PRE_POLL;
+		err_rec->resched_delay = err_rec->ue_to_poll_time -
+					ERR_RECOVERY_UE_DETECT_DURATION;
+		break;
+
+	case ERR_RECOVERY_ST_RESET:
+		if (!be_err_is_recoverable(adapter)) {
+			dev_err(&adapter->pdev->dev,
+				"Failed to meet recovery criteria\n");
+			status = -EIO;
+			err_rec->resched_delay = 0;
+			break;
+		}
+		be_soft_reset(adapter);
+		err_rec->recovery_state = ERR_RECOVERY_ST_PRE_POLL;
+		err_rec->resched_delay = err_rec->ue_to_poll_time -
+					err_rec->ue_to_reset_time;
+		break;
+
+	case ERR_RECOVERY_ST_PRE_POLL:
+		err_rec->recovery_state = ERR_RECOVERY_ST_REINIT;
+		err_rec->resched_delay = 0;
+		status = 0;			/* done */
+		break;
+
+	default:
+		status = -EINVAL;
+		err_rec->resched_delay = 0;
+		break;
+	}
+
+	return status;
+}
+
 static int be_err_recover(struct be_adapter *adapter)
 {
 	int status;
 
-	/* Error recovery is supported only Lancer as of now */
-	if (!lancer_chip(adapter))
-		return -EIO;
+	if (!lancer_chip(adapter)) {
+		if (!adapter->error_recovery.recovery_supported ||
+		    adapter->priv_flags & BE_DISABLE_TPE_RECOVERY)
+			return -EIO;
+		status = be_tpe_recover(adapter);
+		if (status)
+			goto err;
+	}
 
 	/* Wait for adapter to reach quiescent state before
 	 * destroying queues
@@ -4999,59 +5450,74 @@ static int be_err_recover(struct be_adapter *adapter)
 	if (status)
 		goto err;
 
+	adapter->flags |= BE_FLAGS_TRY_RECOVERY;
+
 	be_cleanup(adapter);
 
 	status = be_resume(adapter);
 	if (status)
 		goto err;
 
-	return 0;
+	adapter->flags &= ~BE_FLAGS_TRY_RECOVERY;
+
 err:
 	return status;
 }
 
 static void be_err_detection_task(struct work_struct *work)
 {
+	struct be_error_recovery *err_rec =
+			container_of(work, struct be_error_recovery,
+				     err_detection_work.work);
 	struct be_adapter *adapter =
-				container_of(work, struct be_adapter,
-					     be_err_detection_work.work);
+			container_of(err_rec, struct be_adapter,
+				     error_recovery);
+	u32 resched_delay = ERR_RECOVERY_DETECTION_DELAY;
 	struct device *dev = &adapter->pdev->dev;
 	int recovery_status;
-	int delay = ERR_DETECTION_DELAY;
 
 	be_detect_error(adapter);
-
-	if (be_check_error(adapter, BE_ERROR_HW))
-		recovery_status = be_err_recover(adapter);
-	else
+	if (!be_check_error(adapter, BE_ERROR_HW))
 		goto reschedule_task;
 
+	recovery_status = be_err_recover(adapter);
 	if (!recovery_status) {
-		adapter->recovery_retries = 0;
+		err_rec->recovery_retries = 0;
+		err_rec->recovery_state = ERR_RECOVERY_ST_NONE;
 		dev_info(dev, "Adapter recovery successful\n");
 		goto reschedule_task;
-	} else if (be_virtfn(adapter)) {
+	} else if (!lancer_chip(adapter) && err_rec->resched_delay) {
+		/* BEx/SH recovery state machine */
+		if (adapter->pf_num == 0 &&
+		    err_rec->recovery_state > ERR_RECOVERY_ST_DETECT)
+			dev_err(&adapter->pdev->dev,
+				"Adapter recovery in progress\n");
+		resched_delay = err_rec->resched_delay;
+		goto reschedule_task;
+	} else if (lancer_chip(adapter) && be_virtfn(adapter)) {
 		/* For VFs, check if PF have allocated resources
 		 * every second.
 		 */
 		dev_err(dev, "Re-trying adapter recovery\n");
 		goto reschedule_task;
-	} else if (adapter->recovery_retries++ <
-		   MAX_ERR_RECOVERY_RETRY_COUNT) {
+	} else if (lancer_chip(adapter) && err_rec->recovery_retries++ <
+		   ERR_RECOVERY_MAX_RETRY_COUNT) {
 		/* In case of another error during recovery, it takes 30 sec
 		 * for adapter to come out of error. Retry error recovery after
 		 * this time interval.
 		 */
 		dev_err(&adapter->pdev->dev, "Re-trying adapter recovery\n");
-		delay = ERR_RECOVERY_RETRY_DELAY;
+		resched_delay = ERR_RECOVERY_RETRY_DELAY;
 		goto reschedule_task;
 	} else {
 		dev_err(dev, "Adapter recovery failed\n");
+		dev_err(dev, "Please reboot server to recover\n");
 	}
 
 	return;
+
 reschedule_task:
-	be_schedule_err_detection(adapter, delay);
+	be_schedule_err_detection(adapter, resched_delay);
 }
 
 static void be_log_sfp_info(struct be_adapter *adapter)
@@ -5116,7 +5582,7 @@ static void be_worker(struct work_struct *work)
 
 reschedule:
 	adapter->work_counter++;
-	schedule_delayed_work(&adapter->work, msecs_to_jiffies(1000));
+	queue_delayed_work(be_wq, &adapter->work, msecs_to_jiffies(1000));
 }
 
 static void be_unmap_pci_bars(struct be_adapter *adapter)
@@ -5256,14 +5722,18 @@ static int be_drv_init(struct be_adapter *adapter)
 	}
 
 	mutex_init(&adapter->mbox_lock);
-	spin_lock_init(&adapter->mcc_lock);
+	mutex_init(&adapter->mcc_lock);
+	mutex_init(&adapter->rx_filter_lock);
 	spin_lock_init(&adapter->mcc_cq_lock);
 	init_completion(&adapter->et_cmd_compl);
 
 	pci_save_state(adapter->pdev);
 
 	INIT_DELAYED_WORK(&adapter->work, be_worker);
-	INIT_DELAYED_WORK(&adapter->be_err_detection_work,
+
+	adapter->error_recovery.recovery_state = ERR_RECOVERY_ST_NONE;
+	adapter->error_recovery.resched_delay = 0;
+	INIT_DELAYED_WORK(&adapter->error_recovery.err_detection_work,
 			  be_err_detection_task);
 
 	adapter->rx_fc = true;
@@ -5298,6 +5768,9 @@ static void be_remove(struct pci_dev *pdev)
 
 	be_clear(adapter);
 
+	if (!pci_vfs_assigned(adapter->pdev))
+		be_cmd_reset_function(adapter);
+
 	/* tell fw we're done with firing cmds */
 	be_cmd_fw_clean(adapter);
 
@@ -5454,6 +5927,7 @@ static int be_probe(struct pci_dev *pdev, const struct pci_device_id *pdev_id)
 	be_roce_dev_add(adapter);
 
 	be_schedule_err_detection(adapter, ERR_DETECTION_DELAY);
+	adapter->error_recovery.probe_time = jiffies;
 
 	/* On Die temperature not supported for VF. */
 	if (be_physfn(adapter) && IS_ENABLED(CONFIG_BE2NET_HWMON)) {
@@ -5699,6 +6173,8 @@ static struct pci_driver be_driver = {
 
 static int __init be_init_module(void)
 {
+	int status;
+
 	if (rx_frag_size != 8192 && rx_frag_size != 4096 &&
 	    rx_frag_size != 2048) {
 		printk(KERN_WARNING DRV_NAME
@@ -5712,12 +6188,33 @@ static int __init be_init_module(void)
 		pr_info(DRV_NAME " : Use sysfs method to enable VFs\n");
 	}
 
-	return pci_register_driver(&be_driver);
+	be_wq = create_singlethread_workqueue("be_wq");
+	if (!be_wq) {
+		pr_warn(DRV_NAME "workqueue creation failed\n");
+		return -1;
+	}
+
+	be_err_recovery_workq =
+		create_singlethread_workqueue("be_err_recover");
+	if (!be_err_recovery_workq)
+		pr_warn(DRV_NAME "Could not create error recovery workqueue\n");
+
+	status = pci_register_driver(&be_driver);
+	if (status) {
+		destroy_workqueue(be_wq);
+		be_destroy_err_recovery_workq();
+	}
+	return status;
 }
 module_init(be_init_module);
 
 static void __exit be_exit_module(void)
 {
 	pci_unregister_driver(&be_driver);
+
+	be_destroy_err_recovery_workq();
+
+	if (be_wq)
+		destroy_workqueue(be_wq);
 }
 module_exit(be_exit_module);
diff --git a/drivers/net/ethernet/faraday/ftgmac100.c b/drivers/net/ethernet/faraday/ftgmac100.c
index 36361f8bf894..262587240c86 100644
--- a/drivers/net/ethernet/faraday/ftgmac100.c
+++ b/drivers/net/ethernet/faraday/ftgmac100.c
@@ -60,6 +60,8 @@ struct ftgmac100 {
 	struct ftgmac100_descs *descs;
 	dma_addr_t descs_dma_addr;
 
+	struct page *rx_pages[RX_QUEUE_ENTRIES];
+
 	unsigned int rx_pointer;
 	unsigned int tx_clean_pointer;
 	unsigned int tx_pointer;
@@ -77,6 +79,9 @@ struct ftgmac100 {
 	int int_mask_all;
 	bool use_ncsi;
 	bool enabled;
+
+	u32 rxdes0_edorr_mask;
+	u32 txdes0_edotr_mask;
 };
 
 static int ftgmac100_alloc_rx_page(struct ftgmac100 *priv,
@@ -257,10 +262,11 @@ static bool ftgmac100_rxdes_packet_ready(struct ftgmac100_rxdes *rxdes)
 	return rxdes->rxdes0 & cpu_to_le32(FTGMAC100_RXDES0_RXPKT_RDY);
 }
 
-static void ftgmac100_rxdes_set_dma_own(struct ftgmac100_rxdes *rxdes)
+static void ftgmac100_rxdes_set_dma_own(const struct ftgmac100 *priv,
+					struct ftgmac100_rxdes *rxdes)
 {
 	/* clear status bits */
-	rxdes->rxdes0 &= cpu_to_le32(FTGMAC100_RXDES0_EDORR);
+	rxdes->rxdes0 &= cpu_to_le32(priv->rxdes0_edorr_mask);
 }
 
 static bool ftgmac100_rxdes_rx_error(struct ftgmac100_rxdes *rxdes)
@@ -298,9 +304,10 @@ static bool ftgmac100_rxdes_multicast(struct ftgmac100_rxdes *rxdes)
 	return rxdes->rxdes0 & cpu_to_le32(FTGMAC100_RXDES0_MULTICAST);
 }
 
-static void ftgmac100_rxdes_set_end_of_ring(struct ftgmac100_rxdes *rxdes)
+static void ftgmac100_rxdes_set_end_of_ring(const struct ftgmac100 *priv,
+					    struct ftgmac100_rxdes *rxdes)
 {
-	rxdes->rxdes0 |= cpu_to_le32(FTGMAC100_RXDES0_EDORR);
+	rxdes->rxdes0 |= cpu_to_le32(priv->rxdes0_edorr_mask);
 }
 
 static void ftgmac100_rxdes_set_dma_addr(struct ftgmac100_rxdes *rxdes,
@@ -341,18 +348,27 @@ static bool ftgmac100_rxdes_ipcs_err(struct ftgmac100_rxdes *rxdes)
 	return rxdes->rxdes1 & cpu_to_le32(FTGMAC100_RXDES1_IP_CHKSUM_ERR);
 }
 
+static inline struct page **ftgmac100_rxdes_page_slot(struct ftgmac100 *priv,
+						      struct ftgmac100_rxdes *rxdes)
+{
+	return &priv->rx_pages[rxdes - priv->descs->rxdes];
+}
+
 /*
  * rxdes2 is not used by hardware. We use it to keep track of page.
  * Since hardware does not touch it, we can skip cpu_to_le32()/le32_to_cpu().
  */
-static void ftgmac100_rxdes_set_page(struct ftgmac100_rxdes *rxdes, struct page *page)
+static void ftgmac100_rxdes_set_page(struct ftgmac100 *priv,
+				     struct ftgmac100_rxdes *rxdes,
+				     struct page *page)
 {
-	rxdes->rxdes2 = (unsigned int)page;
+	*ftgmac100_rxdes_page_slot(priv, rxdes) = page;
 }
 
-static struct page *ftgmac100_rxdes_get_page(struct ftgmac100_rxdes *rxdes)
+static struct page *ftgmac100_rxdes_get_page(struct ftgmac100 *priv,
+					     struct ftgmac100_rxdes *rxdes)
 {
-	return (struct page *)rxdes->rxdes2;
+	return *ftgmac100_rxdes_page_slot(priv, rxdes);
 }
 
 /******************************************************************************
@@ -382,7 +398,7 @@ ftgmac100_rx_locate_first_segment(struct ftgmac100 *priv)
 		if (ftgmac100_rxdes_first_segment(rxdes))
 			return rxdes;
 
-		ftgmac100_rxdes_set_dma_own(rxdes);
+		ftgmac100_rxdes_set_dma_own(priv, rxdes);
 		ftgmac100_rx_pointer_advance(priv);
 		rxdes = ftgmac100_current_rxdes(priv);
 	}
@@ -453,7 +469,7 @@ static void ftgmac100_rx_drop_packet(struct ftgmac100 *priv)
 		if (ftgmac100_rxdes_last_segment(rxdes))
 			done = true;
 
-		ftgmac100_rxdes_set_dma_own(rxdes);
+		ftgmac100_rxdes_set_dma_own(priv, rxdes);
 		ftgmac100_rx_pointer_advance(priv);
 		rxdes = ftgmac100_current_rxdes(priv);
 	} while (!done && ftgmac100_rxdes_packet_ready(rxdes));
@@ -501,7 +517,7 @@ static bool ftgmac100_rx_packet(struct ftgmac100 *priv, int *processed)
 
 	do {
 		dma_addr_t map = ftgmac100_rxdes_get_dma_addr(rxdes);
-		struct page *page = ftgmac100_rxdes_get_page(rxdes);
+		struct page *page = ftgmac100_rxdes_get_page(priv, rxdes);
 		unsigned int size;
 
 		dma_unmap_page(priv->dev, map, RX_BUF_SIZE, DMA_FROM_DEVICE);
@@ -545,10 +561,11 @@ static bool ftgmac100_rx_packet(struct ftgmac100 *priv, int *processed)
 /******************************************************************************
  * internal functions (transmit descriptor)
  *****************************************************************************/
-static void ftgmac100_txdes_reset(struct ftgmac100_txdes *txdes)
+static void ftgmac100_txdes_reset(const struct ftgmac100 *priv,
+				  struct ftgmac100_txdes *txdes)
 {
 	/* clear all except end of ring bit */
-	txdes->txdes0 &= cpu_to_le32(FTGMAC100_TXDES0_EDOTR);
+	txdes->txdes0 &= cpu_to_le32(priv->txdes0_edotr_mask);
 	txdes->txdes1 = 0;
 	txdes->txdes2 = 0;
 	txdes->txdes3 = 0;
@@ -569,9 +586,10 @@ static void ftgmac100_txdes_set_dma_own(struct ftgmac100_txdes *txdes)
 	txdes->txdes0 |= cpu_to_le32(FTGMAC100_TXDES0_TXDMA_OWN);
 }
 
-static void ftgmac100_txdes_set_end_of_ring(struct ftgmac100_txdes *txdes)
+static void ftgmac100_txdes_set_end_of_ring(const struct ftgmac100 *priv,
+					    struct ftgmac100_txdes *txdes)
 {
-	txdes->txdes0 |= cpu_to_le32(FTGMAC100_TXDES0_EDOTR);
+	txdes->txdes0 |= cpu_to_le32(priv->txdes0_edotr_mask);
 }
 
 static void ftgmac100_txdes_set_first_segment(struct ftgmac100_txdes *txdes)
@@ -690,7 +708,7 @@ static bool ftgmac100_tx_complete_packet(struct ftgmac100 *priv)
 
 	dev_kfree_skb(skb);
 
-	ftgmac100_txdes_reset(txdes);
+	ftgmac100_txdes_reset(priv, txdes);
 
 	ftgmac100_tx_clean_pointer_advance(priv);
 
@@ -779,9 +797,9 @@ static int ftgmac100_alloc_rx_page(struct ftgmac100 *priv,
 		return -ENOMEM;
 	}
 
-	ftgmac100_rxdes_set_page(rxdes, page);
+	ftgmac100_rxdes_set_page(priv, rxdes, page);
 	ftgmac100_rxdes_set_dma_addr(rxdes, map);
-	ftgmac100_rxdes_set_dma_own(rxdes);
+	ftgmac100_rxdes_set_dma_own(priv, rxdes);
 	return 0;
 }
 
@@ -791,7 +809,7 @@ static void ftgmac100_free_buffers(struct ftgmac100 *priv)
 
 	for (i = 0; i < RX_QUEUE_ENTRIES; i++) {
 		struct ftgmac100_rxdes *rxdes = &priv->descs->rxdes[i];
-		struct page *page = ftgmac100_rxdes_get_page(rxdes);
+		struct page *page = ftgmac100_rxdes_get_page(priv, rxdes);
 		dma_addr_t map = ftgmac100_rxdes_get_dma_addr(rxdes);
 
 		if (!page)
@@ -828,7 +846,8 @@ static int ftgmac100_alloc_buffers(struct ftgmac100 *priv)
 		return -ENOMEM;
 
 	/* initialize RX ring */
-	ftgmac100_rxdes_set_end_of_ring(&priv->descs->rxdes[RX_QUEUE_ENTRIES - 1]);
+	ftgmac100_rxdes_set_end_of_ring(priv,
+					&priv->descs->rxdes[RX_QUEUE_ENTRIES - 1]);
 
 	for (i = 0; i < RX_QUEUE_ENTRIES; i++) {
 		struct ftgmac100_rxdes *rxdes = &priv->descs->rxdes[i];
@@ -838,7 +857,8 @@ static int ftgmac100_alloc_buffers(struct ftgmac100 *priv)
 	}
 
 	/* initialize TX ring */
-	ftgmac100_txdes_set_end_of_ring(&priv->descs->txdes[TX_QUEUE_ENTRIES - 1]);
+	ftgmac100_txdes_set_end_of_ring(priv,
+					&priv->descs->txdes[TX_QUEUE_ENTRIES - 1]);
 	return 0;
 
 err:
@@ -1055,14 +1075,12 @@ static int ftgmac100_poll(struct napi_struct *napi, int budget)
 	}
 
 	if (status & priv->int_mask_all & (FTGMAC100_INT_NO_RXBUF |
-			FTGMAC100_INT_RPKT_LOST | FTGMAC100_INT_AHB_ERR |
-			FTGMAC100_INT_PHYSTS_CHG)) {
+			FTGMAC100_INT_RPKT_LOST | FTGMAC100_INT_AHB_ERR)) {
 		if (net_ratelimit())
-			netdev_info(netdev, "[ISR] = 0x%x: %s%s%s%s\n", status,
+			netdev_info(netdev, "[ISR] = 0x%x: %s%s%s\n", status,
 				    status & FTGMAC100_INT_NO_RXBUF ? "NO_RXBUF " : "",
 				    status & FTGMAC100_INT_RPKT_LOST ? "RPKT_LOST " : "",
-				    status & FTGMAC100_INT_AHB_ERR ? "AHB_ERR " : "",
-				    status & FTGMAC100_INT_PHYSTS_CHG ? "PHYSTS_CHG" : "");
+				    status & FTGMAC100_INT_AHB_ERR ? "AHB_ERR " : "");
 
 		if (status & FTGMAC100_INT_NO_RXBUF) {
 			/* RX buffer unavailable */
@@ -1092,6 +1110,7 @@ static int ftgmac100_poll(struct napi_struct *napi, int budget)
 static int ftgmac100_open(struct net_device *netdev)
 {
 	struct ftgmac100 *priv = netdev_priv(netdev);
+	unsigned int status;
 	int err;
 
 	err = ftgmac100_alloc_buffers(priv);
@@ -1117,6 +1136,11 @@ static int ftgmac100_open(struct net_device *netdev)
 
 	ftgmac100_init_hw(priv);
 	ftgmac100_start_hw(priv, priv->use_ncsi ? 100 : 10);
+
+	/* Clear stale interrupts */
+	status = ioread32(priv->base + FTGMAC100_OFFSET_ISR);
+	iowrite32(status, priv->base + FTGMAC100_OFFSET_ISR);
+
 	if (netdev->phydev)
 		phy_start(netdev->phydev);
 	else if (priv->use_ncsi)
@@ -1166,6 +1190,8 @@ static int ftgmac100_stop(struct net_device *netdev)
 	napi_disable(&priv->napi);
 	if (netdev->phydev)
 		phy_stop(netdev->phydev);
+	else if (priv->use_ncsi)
+		ncsi_stop_dev(priv->ndev);
 
 	ftgmac100_stop_hw(priv);
 	free_irq(priv->irq, netdev);
@@ -1226,12 +1252,21 @@ static int ftgmac100_setup_mdio(struct net_device *netdev)
 	struct ftgmac100 *priv = netdev_priv(netdev);
 	struct platform_device *pdev = to_platform_device(priv->dev);
 	int i, err = 0;
+	u32 reg;
 
 	/* initialize mdio bus */
 	priv->mii_bus = mdiobus_alloc();
 	if (!priv->mii_bus)
 		return -EIO;
 
+	if (of_machine_is_compatible("aspeed,ast2400") ||
+	    of_machine_is_compatible("aspeed,ast2500")) {
+		/* This driver supports the old MDIO interface */
+		reg = ioread32(priv->base + FTGMAC100_OFFSET_REVR);
+		reg &= ~FTGMAC100_REVR_NEW_MDIO_INTERFACE;
+		iowrite32(reg, priv->base + FTGMAC100_OFFSET_REVR);
+	};
+
 	priv->mii_bus->name = "ftgmac100_mdio";
 	snprintf(priv->mii_bus->id, MII_BUS_ID_SIZE, "%s-%d",
 		 pdev->name, pdev->id);
@@ -1355,9 +1390,18 @@ static int ftgmac100_probe(struct platform_device *pdev)
 			      FTGMAC100_INT_XPKT_ETH |
 			      FTGMAC100_INT_XPKT_LOST |
 			      FTGMAC100_INT_AHB_ERR |
-			      FTGMAC100_INT_PHYSTS_CHG |
 			      FTGMAC100_INT_RPKT_BUF |
 			      FTGMAC100_INT_NO_RXBUF);
+
+	if (of_machine_is_compatible("aspeed,ast2400") ||
+	    of_machine_is_compatible("aspeed,ast2500")) {
+		priv->rxdes0_edorr_mask = BIT(30);
+		priv->txdes0_edotr_mask = BIT(30);
+	} else {
+		priv->rxdes0_edorr_mask = BIT(15);
+		priv->txdes0_edotr_mask = BIT(15);
+	}
+
 	if (pdev->dev.of_node &&
 	    of_get_property(pdev->dev.of_node, "use-ncsi", NULL)) {
 		if (!IS_ENABLED(CONFIG_NET_NCSI)) {
@@ -1367,7 +1411,6 @@ static int ftgmac100_probe(struct platform_device *pdev)
 
 		dev_info(&pdev->dev, "Using NCSI interface\n");
 		priv->use_ncsi = true;
-		priv->int_mask_all &= ~FTGMAC100_INT_PHYSTS_CHG;
 		priv->ndev = ncsi_register_dev(netdev, ftgmac100_ncsi_handler);
 		if (!priv->ndev)
 			goto err_ncsi_dev;
diff --git a/drivers/net/ethernet/faraday/ftgmac100.h b/drivers/net/ethernet/faraday/ftgmac100.h
index 13408d448b05..a7ce0ac8858a 100644
--- a/drivers/net/ethernet/faraday/ftgmac100.h
+++ b/drivers/net/ethernet/faraday/ftgmac100.h
@@ -134,6 +134,11 @@
 #define FTGMAC100_DMAFIFOS_TXDMA_REQ		(1 << 31)
 
 /*
+ * Feature Register
+ */
+#define FTGMAC100_REVR_NEW_MDIO_INTERFACE	BIT(31)
+
+/*
  * Receive buffer size register
  */
 #define FTGMAC100_RBSR_SIZE(x)		((x) & 0x3fff)
@@ -152,6 +157,7 @@
 #define FTGMAC100_MACCR_FULLDUP		(1 << 8)
 #define FTGMAC100_MACCR_GIGA_MODE	(1 << 9)
 #define FTGMAC100_MACCR_CRC_APD		(1 << 10)
+#define FTGMAC100_MACCR_PHY_LINK_LEVEL	(1 << 11)
 #define FTGMAC100_MACCR_RX_RUNT		(1 << 12)
 #define FTGMAC100_MACCR_JUMBO_LF	(1 << 13)
 #define FTGMAC100_MACCR_RX_ALL		(1 << 14)
@@ -189,7 +195,6 @@ struct ftgmac100_txdes {
 } __attribute__ ((aligned(16)));
 
 #define FTGMAC100_TXDES0_TXBUF_SIZE(x)	((x) & 0x3fff)
-#define FTGMAC100_TXDES0_EDOTR		(1 << 15)
 #define FTGMAC100_TXDES0_CRC_ERR	(1 << 19)
 #define FTGMAC100_TXDES0_LTS		(1 << 28)
 #define FTGMAC100_TXDES0_FTS		(1 << 29)
@@ -215,7 +220,6 @@ struct ftgmac100_rxdes {
 } __attribute__ ((aligned(16)));
 
 #define FTGMAC100_RXDES0_VDBC		0x3fff
-#define FTGMAC100_RXDES0_EDORR		(1 << 15)
 #define FTGMAC100_RXDES0_MULTICAST	(1 << 16)
 #define FTGMAC100_RXDES0_BROADCAST	(1 << 17)
 #define FTGMAC100_RXDES0_RX_ERR		(1 << 18)
diff --git a/drivers/net/ethernet/freescale/fec_main.c b/drivers/net/ethernet/freescale/fec_main.c
index 01f7e811739b..48a033e64423 100644
--- a/drivers/net/ethernet/freescale/fec_main.c
+++ b/drivers/net/ethernet/freescale/fec_main.c
@@ -89,10 +89,10 @@ static struct platform_device_id fec_devtype[] = {
 		.driver_data = 0,
 	}, {
 		.name = "imx25-fec",
-		.driver_data = FEC_QUIRK_USE_GASKET | FEC_QUIRK_HAS_RACC,
+		.driver_data = FEC_QUIRK_USE_GASKET,
 	}, {
 		.name = "imx27-fec",
-		.driver_data = FEC_QUIRK_HAS_RACC,
+		.driver_data = 0,
 	}, {
 		.name = "imx28-fec",
 		.driver_data = FEC_QUIRK_ENET_MAC | FEC_QUIRK_SWAP_FRAME |
@@ -180,6 +180,7 @@ MODULE_PARM_DESC(macaddr, "FEC Ethernet MAC address");
 /* FEC receive acceleration */
 #define FEC_RACC_IPDIS		(1 << 1)
 #define FEC_RACC_PRODIS		(1 << 2)
+#define FEC_RACC_SHIFT16	BIT(7)
 #define FEC_RACC_OPTIONS	(FEC_RACC_IPDIS | FEC_RACC_PRODIS)
 
 /*
@@ -912,13 +913,11 @@ fec_restart(struct net_device *ndev)
 	 * enet-mac reset will reset mac address registers too,
 	 * so need to reconfigure it.
 	 */
-	if (fep->quirks & FEC_QUIRK_ENET_MAC) {
-		memcpy(&temp_mac, ndev->dev_addr, ETH_ALEN);
-		writel((__force u32)cpu_to_be32(temp_mac[0]),
-		       fep->hwp + FEC_ADDR_LOW);
-		writel((__force u32)cpu_to_be32(temp_mac[1]),
-		       fep->hwp + FEC_ADDR_HIGH);
-	}
+	memcpy(&temp_mac, ndev->dev_addr, ETH_ALEN);
+	writel((__force u32)cpu_to_be32(temp_mac[0]),
+	       fep->hwp + FEC_ADDR_LOW);
+	writel((__force u32)cpu_to_be32(temp_mac[1]),
+	       fep->hwp + FEC_ADDR_HIGH);
 
 	/* Clear any outstanding interrupt. */
 	writel(0xffffffff, fep->hwp + FEC_IEVENT);
@@ -945,9 +944,11 @@ fec_restart(struct net_device *ndev)
 
 #if !defined(CONFIG_M5272)
 	if (fep->quirks & FEC_QUIRK_HAS_RACC) {
-		/* set RX checksum */
 		val = readl(fep->hwp + FEC_RACC);
+		/* align IP header */
+		val |= FEC_RACC_SHIFT16;
 		if (fep->csum_flags & FLAG_RX_CSUM_ENABLED)
+			/* set RX checksum */
 			val |= FEC_RACC_OPTIONS;
 		else
 			val &= ~FEC_RACC_OPTIONS;
@@ -1428,6 +1429,12 @@ fec_enet_rx_queue(struct net_device *ndev, int budget, u16 queue_id)
 		prefetch(skb->data - NET_IP_ALIGN);
 		skb_put(skb, pkt_len - 4);
 		data = skb->data;
+
+#if !defined(CONFIG_M5272)
+		if (fep->quirks & FEC_QUIRK_HAS_RACC)
+			data = skb_pull_inline(skb, 2);
+#endif
+
 		if (!is_copybreak && need_swap)
 			swap_buffer(data, pkt_len);
 
@@ -2887,7 +2894,7 @@ fec_enet_close(struct net_device *ndev)
  * this kind of feature?).
  */
 
-#define HASH_BITS	6		/* #bits in hash */
+#define FEC_HASH_BITS	6		/* #bits in hash */
 #define CRC32_POLY	0xEDB88320
 
 static void set_multicast_list(struct net_device *ndev)
@@ -2935,10 +2942,10 @@ static void set_multicast_list(struct net_device *ndev)
 			}
 		}
 
-		/* only upper 6 bits (HASH_BITS) are used
+		/* only upper 6 bits (FEC_HASH_BITS) are used
 		 * which point to specific bit in he hash registers
 		 */
-		hash = (crc >> (32 - HASH_BITS)) & 0x3f;
+		hash = (crc >> (32 - FEC_HASH_BITS)) & 0x3f;
 
 		if (hash > 31) {
 			tmp = readl(fep->hwp + FEC_GRP_HASH_TABLE_HIGH);
diff --git a/drivers/net/ethernet/freescale/fman/Makefile b/drivers/net/ethernet/freescale/fman/Makefile
index 51fd2e6c1b84..60491779e49f 100644
--- a/drivers/net/ethernet/freescale/fman/Makefile
+++ b/drivers/net/ethernet/freescale/fman/Makefile
@@ -1,7 +1,9 @@
 subdir-ccflags-y +=  -I$(srctree)/drivers/net/ethernet/freescale/fman
 
-obj-y		+= fsl_fman.o fsl_fman_mac.o fsl_mac.o
+obj-$(CONFIG_FSL_FMAN) += fsl_fman.o
+obj-$(CONFIG_FSL_FMAN) += fsl_fman_port.o
+obj-$(CONFIG_FSL_FMAN) += fsl_mac.o
 
-fsl_fman-objs	:= fman_muram.o fman.o fman_sp.o fman_port.o
-fsl_fman_mac-objs := fman_dtsec.o fman_memac.o fman_tgec.o
-fsl_mac-objs += mac.o
+fsl_fman-objs	:= fman_muram.o fman.o fman_sp.o
+fsl_fman_port-objs := fman_port.o
+fsl_mac-objs:= mac.o fman_dtsec.o fman_memac.o fman_tgec.o
diff --git a/drivers/net/ethernet/freescale/fman/fman.c b/drivers/net/ethernet/freescale/fman/fman.c
index 1de2e1e51c2b..dafd9e1baba2 100644
--- a/drivers/net/ethernet/freescale/fman/fman.c
+++ b/drivers/net/ethernet/freescale/fman/fman.c
@@ -618,7 +618,7 @@ struct fman {
 	unsigned long cam_offset;
 	size_t cam_size;
 	/* Fifo in MURAM */
-	int fifo_offset;
+	unsigned long fifo_offset;
 	size_t fifo_size;
 
 	u32 liodn_base[64];
@@ -2036,7 +2036,7 @@ static int fman_init(struct fman *fman)
 	/* allocate MURAM for FIFO according to total size */
 	fman->fifo_offset = fman_muram_alloc(fman->muram,
 					     fman->state->total_fifo_size);
-	if (IS_ERR_VALUE(fman->cam_offset)) {
+	if (IS_ERR_VALUE(fman->fifo_offset)) {
 		free_init_resources(fman);
 		dev_err(fman->dev, "%s: MURAM alloc for BMI FIFO failed\n",
 			__func__);
@@ -2115,6 +2115,7 @@ void fman_register_intr(struct fman *fman, enum fman_event_modules module,
 	fman->intr_mng[event].isr_cb = isr_cb;
 	fman->intr_mng[event].src_handle = src_arg;
 }
+EXPORT_SYMBOL(fman_register_intr);
 
 /**
  * fman_unregister_intr
@@ -2138,6 +2139,7 @@ void fman_unregister_intr(struct fman *fman, enum fman_event_modules module,
 	fman->intr_mng[event].isr_cb = NULL;
 	fman->intr_mng[event].src_handle = NULL;
 }
+EXPORT_SYMBOL(fman_unregister_intr);
 
 /**
  * fman_set_port_params
@@ -2241,6 +2243,7 @@ return_err:
 	spin_unlock_irqrestore(&fman->spinlock, flags);
 	return err;
 }
+EXPORT_SYMBOL(fman_set_port_params);
 
 /**
  * fman_reset_mac
@@ -2310,6 +2313,7 @@ int fman_reset_mac(struct fman *fman, u8 mac_id)
 
 	return 0;
 }
+EXPORT_SYMBOL(fman_reset_mac);
 
 /**
  * fman_set_mac_max_frame
@@ -2327,8 +2331,7 @@ int fman_set_mac_max_frame(struct fman *fman, u8 mac_id, u16 mfl)
 	 * or equal to the port's max
 	 */
 	if ((!fman->state->port_mfl[mac_id]) ||
-	    (fman->state->port_mfl[mac_id] &&
-	    (mfl <= fman->state->port_mfl[mac_id]))) {
+	    (mfl <= fman->state->port_mfl[mac_id])) {
 		fman->state->mac_mfl[mac_id] = mfl;
 	} else {
 		dev_warn(fman->dev, "%s: MAC max_frame_length is larger than Port max_frame_length\n",
@@ -2337,6 +2340,7 @@ int fman_set_mac_max_frame(struct fman *fman, u8 mac_id, u16 mfl)
 	}
 	return 0;
 }
+EXPORT_SYMBOL(fman_set_mac_max_frame);
 
 /**
  * fman_get_clock_freq
@@ -2363,6 +2367,7 @@ u32 fman_get_bmi_max_fifo_size(struct fman *fman)
 {
 	return fman->state->bmi_max_fifo_size;
 }
+EXPORT_SYMBOL(fman_get_bmi_max_fifo_size);
 
 /**
  * fman_get_revision
@@ -2384,6 +2389,7 @@ void fman_get_revision(struct fman *fman, struct fman_rev_info *rev_info)
 				FPM_REV1_MAJOR_SHIFT);
 	rev_info->minor = tmp & FPM_REV1_MINOR_MASK;
 }
+EXPORT_SYMBOL(fman_get_revision);
 
 /**
  * fman_get_qman_channel_id
@@ -2419,6 +2425,7 @@ u32 fman_get_qman_channel_id(struct fman *fman, u32 port_id)
 
 	return fman->state->qman_channel_base + i;
 }
+EXPORT_SYMBOL(fman_get_qman_channel_id);
 
 /**
  * fman_get_mem_region
@@ -2432,6 +2439,7 @@ struct resource *fman_get_mem_region(struct fman *fman)
 {
 	return fman->state->res;
 }
+EXPORT_SYMBOL(fman_get_mem_region);
 
 /* Bootargs defines */
 /* Extra headroom for RX buffers - Default, min and max */
@@ -2453,7 +2461,7 @@ struct resource *fman_get_mem_region(struct fman *fman)
  * particular forwarding scenarios that add extra headers to the
  * forwarded frame.
  */
-int fsl_fm_rx_extra_headroom = FSL_FM_RX_EXTRA_HEADROOM;
+static int fsl_fm_rx_extra_headroom = FSL_FM_RX_EXTRA_HEADROOM;
 module_param(fsl_fm_rx_extra_headroom, int, 0);
 MODULE_PARM_DESC(fsl_fm_rx_extra_headroom, "Extra headroom for Rx buffers");
 
@@ -2466,7 +2474,7 @@ MODULE_PARM_DESC(fsl_fm_rx_extra_headroom, "Extra headroom for Rx buffers");
  * Could be overridden once, at boot-time, via the
  * fm_set_max_frm() callback.
  */
-int fsl_fm_max_frm = FSL_FM_MAX_FRAME_SIZE;
+static int fsl_fm_max_frm = FSL_FM_MAX_FRAME_SIZE;
 module_param(fsl_fm_max_frm, int, 0);
 MODULE_PARM_DESC(fsl_fm_max_frm, "Maximum frame size, across all interfaces");
 
@@ -2538,6 +2546,7 @@ struct fman *fman_bind(struct device *fm_dev)
 {
 	return (struct fman *)(dev_get_drvdata(get_device(fm_dev)));
 }
+EXPORT_SYMBOL(fman_bind);
 
 static irqreturn_t fman_err_irq(int irq, void *handle)
 {
@@ -2727,8 +2736,8 @@ static struct fman *read_dts_node(struct platform_device *of_dev)
 	struct fman *fman;
 	struct device_node *fm_node, *muram_node;
 	struct resource *res;
-	const u32 *u32_prop;
-	int lenp, err, irq;
+	u32 val, range[2];
+	int err, irq;
 	struct clk *clk;
 	u32 clk_rate;
 	phys_addr_t phys_base_addr;
@@ -2740,16 +2749,13 @@ static struct fman *read_dts_node(struct platform_device *of_dev)
 
 	fm_node = of_node_get(of_dev->dev.of_node);
 
-	u32_prop = (const u32 *)of_get_property(fm_node, "cell-index", &lenp);
-	if (!u32_prop) {
-		dev_err(&of_dev->dev, "%s: of_get_property(%s, cell-index) failed\n",
+	err = of_property_read_u32(fm_node, "cell-index", &val);
+	if (err) {
+		dev_err(&of_dev->dev, "%s: failed to read cell-index for %s\n",
 			__func__, fm_node->full_name);
 		goto fman_node_put;
 	}
-	if (WARN_ON(lenp != sizeof(u32)))
-		goto fman_node_put;
-
-	fman->dts_params.id = (u8)fdt32_to_cpu(u32_prop[0]);
+	fman->dts_params.id = (u8)val;
 
 	/* Get the FM interrupt */
 	res = platform_get_resource(of_dev, IORESOURCE_IRQ, 0);
@@ -2796,18 +2802,15 @@ static struct fman *read_dts_node(struct platform_device *of_dev)
 	/* Rounding to MHz */
 	fman->dts_params.clk_freq = DIV_ROUND_UP(clk_rate, 1000000);
 
-	u32_prop = (const u32 *)of_get_property(fm_node,
-						"fsl,qman-channel-range",
-						&lenp);
-	if (!u32_prop) {
-		dev_err(&of_dev->dev, "%s: of_get_property(%s, fsl,qman-channel-range) failed\n",
+	err = of_property_read_u32_array(fm_node, "fsl,qman-channel-range",
+					 &range[0], 2);
+	if (err) {
+		dev_err(&of_dev->dev, "%s: failed to read fsl,qman-channel-range for %s\n",
 			__func__, fm_node->full_name);
 		goto fman_node_put;
 	}
-	if (WARN_ON(lenp != sizeof(u32) * 2))
-		goto fman_node_put;
-	fman->dts_params.qman_channel_base = fdt32_to_cpu(u32_prop[0]);
-	fman->dts_params.num_of_qman_channels = fdt32_to_cpu(u32_prop[1]);
+	fman->dts_params.qman_channel_base = range[0];
+	fman->dts_params.num_of_qman_channels = range[1];
 
 	/* Get the MURAM base address and size */
 	muram_node = of_find_matching_node(fm_node, fman_muram_match);
@@ -2858,7 +2861,7 @@ static struct fman *read_dts_node(struct platform_device *of_dev)
 
 	fman->dts_params.base_addr =
 		devm_ioremap(&of_dev->dev, phys_base_addr, mem_size);
-	if (fman->dts_params.base_addr == 0) {
+	if (!fman->dts_params.base_addr) {
 		dev_err(&of_dev->dev, "%s: devm_ioremap() failed\n", __func__);
 		goto fman_free;
 	}
@@ -2930,7 +2933,7 @@ static const struct of_device_id fman_match[] = {
 	{}
 };
 
-MODULE_DEVICE_TABLE(of, fm_match);
+MODULE_DEVICE_TABLE(of, fman_match);
 
 static struct platform_driver fman_driver = {
 	.driver = {
@@ -2940,4 +2943,25 @@ static struct platform_driver fman_driver = {
 	.probe = fman_probe,
 };
 
-builtin_platform_driver(fman_driver);
+static int __init fman_load(void)
+{
+	int err;
+
+	pr_debug("FSL DPAA FMan driver\n");
+
+	err = platform_driver_register(&fman_driver);
+	if (err < 0)
+		pr_err("Error, platform_driver_register() = %d\n", err);
+
+	return err;
+}
+module_init(fman_load);
+
+static void __exit fman_unload(void)
+{
+	platform_driver_unregister(&fman_driver);
+}
+module_exit(fman_unload);
+
+MODULE_LICENSE("Dual BSD/GPL");
+MODULE_DESCRIPTION("Freescale DPAA Frame Manager driver");
diff --git a/drivers/net/ethernet/freescale/fman/fman_mac.h b/drivers/net/ethernet/freescale/fman/fman_mac.h
index 8ddeedbcef9c..dd6d0526f6c1 100644
--- a/drivers/net/ethernet/freescale/fman/fman_mac.h
+++ b/drivers/net/ethernet/freescale/fman/fman_mac.h
@@ -191,10 +191,6 @@ struct fman_mac_params {
 	u16 max_speed;
 	/* A handle to the FM object this port related to */
 	void *fm;
-	/* MDIO exceptions interrupt source - not valid for all
-	 * MACs; MUST be set to 'NO_IRQ' for MACs that don't have
-	 * mdio-irq, or for polling
-	 */
 	void *dev_id; /* device cookie used by the exception cbs */
 	fman_mac_exception_cb *event_cb;    /* MDIO Events Callback Routine */
 	fman_mac_exception_cb *exception_cb;/* Exception Callback Routine */
diff --git a/drivers/net/ethernet/freescale/fman/fman_memac.c b/drivers/net/ethernet/freescale/fman/fman_memac.c
index 45e98fd8b79e..53ef51e3bd9e 100644
--- a/drivers/net/ethernet/freescale/fman/fman_memac.c
+++ b/drivers/net/ethernet/freescale/fman/fman_memac.c
@@ -507,6 +507,9 @@ static void setup_sgmii_internal_phy(struct fman_mac *memac,
 {
 	u16 tmp_reg16;
 
+	if (WARN_ON(!memac->pcsphy))
+		return;
+
 	/* SGMII mode */
 	tmp_reg16 = IF_MODE_SGMII_EN;
 	if (!fixed_link)
@@ -1151,7 +1154,8 @@ struct fman_mac *memac_config(struct fman_mac_params *params)
 	/* Save FMan revision */
 	fman_get_revision(memac->fm, &memac->fm_rev_info);
 
-	if (memac->phy_if == PHY_INTERFACE_MODE_SGMII) {
+	if (memac->phy_if == PHY_INTERFACE_MODE_SGMII ||
+	    memac->phy_if == PHY_INTERFACE_MODE_QSGMII) {
 		if (!params->internal_phy_node) {
 			pr_err("PCS PHY node is not available\n");
 			memac_free(memac);
diff --git a/drivers/net/ethernet/freescale/fman/fman_muram.c b/drivers/net/ethernet/freescale/fman/fman_muram.c
index 47394c45b6e8..5ec94d243da0 100644
--- a/drivers/net/ethernet/freescale/fman/fman_muram.c
+++ b/drivers/net/ethernet/freescale/fman/fman_muram.c
@@ -150,7 +150,8 @@ unsigned long fman_muram_alloc(struct muram_info *muram, size_t size)
  *
  * Free an allocated memory from FM-MURAM partition.
  */
-void fman_muram_free_mem(struct muram_info *muram, unsigned long offset, size_t size)
+void fman_muram_free_mem(struct muram_info *muram, unsigned long offset,
+			 size_t size)
 {
 	unsigned long addr = fman_muram_offset_to_vbase(muram, offset);
 
diff --git a/drivers/net/ethernet/freescale/fman/fman_muram.h b/drivers/net/ethernet/freescale/fman/fman_muram.h
index 889649ad8931..453bf849eee1 100644
--- a/drivers/net/ethernet/freescale/fman/fman_muram.h
+++ b/drivers/net/ethernet/freescale/fman/fman_muram.h
@@ -46,6 +46,7 @@ unsigned long fman_muram_offset_to_vbase(struct muram_info *muram,
 
 unsigned long fman_muram_alloc(struct muram_info *muram, size_t size);
 
-void fman_muram_free_mem(struct muram_info *muram, unsigned long offset, size_t size);
+void fman_muram_free_mem(struct muram_info *muram, unsigned long offset,
+			 size_t size);
 
 #endif /* __FM_MURAM_EXT */
diff --git a/drivers/net/ethernet/freescale/fman/fman_port.c b/drivers/net/ethernet/freescale/fman/fman_port.c
index 70c198d072dc..9f3bb50a2365 100644
--- a/drivers/net/ethernet/freescale/fman/fman_port.c
+++ b/drivers/net/ethernet/freescale/fman/fman_port.c
@@ -1477,7 +1477,8 @@ EXPORT_SYMBOL(fman_port_cfg_buf_prefix_content);
  */
 int fman_port_disable(struct fman_port *port)
 {
-	u32 __iomem *bmi_cfg_reg, *bmi_status_reg, tmp;
+	u32 __iomem *bmi_cfg_reg, *bmi_status_reg;
+	u32 tmp;
 	bool rx_port, failure = false;
 	int count;
 
@@ -1553,7 +1554,8 @@ EXPORT_SYMBOL(fman_port_disable);
  */
 int fman_port_enable(struct fman_port *port)
 {
-	u32 __iomem *bmi_cfg_reg, tmp;
+	u32 __iomem *bmi_cfg_reg;
+	u32 tmp;
 	bool rx_port;
 
 	if (!is_init_done(port->cfg))
@@ -1623,7 +1625,7 @@ static int fman_port_probe(struct platform_device *of_dev)
 	struct device_node *fm_node, *port_node;
 	struct resource res;
 	struct resource *dev_res;
-	const u32 *u32_prop;
+	u32 val;
 	int err = 0, lenp;
 	enum fman_port_type port_type;
 	u16 port_speed;
@@ -1652,28 +1654,20 @@ static int fman_port_probe(struct platform_device *of_dev)
 		goto return_err;
 	}
 
-	u32_prop = (const u32 *)of_get_property(port_node, "cell-index", &lenp);
-	if (!u32_prop) {
-		dev_err(port->dev, "%s: of_get_property(%s, cell-index) failed\n",
+	err = of_property_read_u32(port_node, "cell-index", &val);
+	if (err) {
+		dev_err(port->dev, "%s: reading cell-index for %s failed\n",
 			__func__, port_node->full_name);
 		err = -EINVAL;
 		goto return_err;
 	}
-	if (WARN_ON(lenp != sizeof(u32))) {
-		err = -EINVAL;
-		goto return_err;
-	}
-	port_id = (u8)fdt32_to_cpu(u32_prop[0]);
-
+	port_id = (u8)val;
 	port->dts_params.id = port_id;
 
 	if (of_device_is_compatible(port_node, "fsl,fman-v3-port-tx")) {
 		port_type = FMAN_PORT_TYPE_TX;
 		port_speed = 1000;
-		u32_prop = (const u32 *)of_get_property(port_node,
-							"fsl,fman-10g-port",
-							&lenp);
-		if (u32_prop)
+		if (of_find_property(port_node, "fsl,fman-10g-port", &lenp))
 			port_speed = 10000;
 
 	} else if (of_device_is_compatible(port_node, "fsl,fman-v2-port-tx")) {
@@ -1686,9 +1680,7 @@ static int fman_port_probe(struct platform_device *of_dev)
 	} else if (of_device_is_compatible(port_node, "fsl,fman-v3-port-rx")) {
 		port_type = FMAN_PORT_TYPE_RX;
 		port_speed = 1000;
-		u32_prop = (const u32 *)of_get_property(port_node,
-						  "fsl,fman-10g-port", &lenp);
-		if (u32_prop)
+		if (of_find_property(port_node, "fsl,fman-10g-port", &lenp))
 			port_speed = 10000;
 
 	} else if (of_device_is_compatible(port_node, "fsl,fman-v2-port-rx")) {
@@ -1743,7 +1735,7 @@ static int fman_port_probe(struct platform_device *of_dev)
 
 	port->dts_params.base_addr = devm_ioremap(port->dev, res.start,
 						  resource_size(&res));
-	if (port->dts_params.base_addr == 0)
+	if (!port->dts_params.base_addr)
 		dev_err(port->dev, "%s: devm_ioremap() failed\n", __func__);
 
 	dev_set_drvdata(&of_dev->dev, port);
@@ -1775,4 +1767,25 @@ static struct platform_driver fman_port_driver = {
 	.probe = fman_port_probe,
 };
 
-builtin_platform_driver(fman_port_driver);
+static int __init fman_port_load(void)
+{
+	int err;
+
+	pr_debug("FSL DPAA FMan driver\n");
+
+	err = platform_driver_register(&fman_port_driver);
+	if (err < 0)
+		pr_err("Error, platform_driver_register() = %d\n", err);
+
+	return err;
+}
+module_init(fman_port_load);
+
+static void __exit fman_port_unload(void)
+{
+	platform_driver_unregister(&fman_port_driver);
+}
+module_exit(fman_port_unload);
+
+MODULE_LICENSE("Dual BSD/GPL");
+MODULE_DESCRIPTION("Freescale DPAA Frame Manager Port driver");
diff --git a/drivers/net/ethernet/freescale/fman/fman_sp.c b/drivers/net/ethernet/freescale/fman/fman_sp.c
index f9e7aa385cba..248f5bcca468 100644
--- a/drivers/net/ethernet/freescale/fman/fman_sp.c
+++ b/drivers/net/ethernet/freescale/fman/fman_sp.c
@@ -80,6 +80,7 @@ void fman_sp_set_buf_pools_in_asc_order_of_buf_sizes(struct fman_ext_pools
 		}
 	}
 }
+EXPORT_SYMBOL(fman_sp_set_buf_pools_in_asc_order_of_buf_sizes);
 
 int fman_sp_build_buffer_struct(struct fman_sp_int_context_data_copy *
 				int_context_data_copy,
@@ -164,3 +165,5 @@ int fman_sp_build_buffer_struct(struct fman_sp_int_context_data_copy *
 
 	return 0;
 }
+EXPORT_SYMBOL(fman_sp_build_buffer_struct);
+
diff --git a/drivers/net/ethernet/freescale/fman/mac.c b/drivers/net/ethernet/freescale/fman/mac.c
index e33d9d24c1db..8fe6b3e253fa 100644
--- a/drivers/net/ethernet/freescale/fman/mac.c
+++ b/drivers/net/ethernet/freescale/fman/mac.c
@@ -469,9 +469,9 @@ static void adjust_link_memac(struct net_device *net_dev)
 /* Initializes driver's PHY state, and attaches to the PHY.
  * Returns 0 on success.
  */
-static int init_phy(struct net_device *net_dev,
-		    struct mac_device *mac_dev,
-		    void (*adj_lnk)(struct net_device *))
+static struct phy_device *init_phy(struct net_device *net_dev,
+				   struct mac_device *mac_dev,
+				   void (*adj_lnk)(struct net_device *))
 {
 	struct phy_device	*phy_dev;
 	struct mac_priv_s	*priv = mac_dev->priv;
@@ -480,7 +480,7 @@ static int init_phy(struct net_device *net_dev,
 				 priv->phy_if);
 	if (!phy_dev) {
 		netdev_err(net_dev, "Could not connect to PHY\n");
-		return -ENODEV;
+		return NULL;
 	}
 
 	/* Remove any features not supported by the controller */
@@ -493,23 +493,23 @@ static int init_phy(struct net_device *net_dev,
 
 	mac_dev->phy_dev = phy_dev;
 
-	return 0;
+	return phy_dev;
 }
 
-static int dtsec_init_phy(struct net_device *net_dev,
-			  struct mac_device *mac_dev)
+static struct phy_device *dtsec_init_phy(struct net_device *net_dev,
+					 struct mac_device *mac_dev)
 {
 	return init_phy(net_dev, mac_dev, &adjust_link_dtsec);
 }
 
-static int tgec_init_phy(struct net_device *net_dev,
-			 struct mac_device *mac_dev)
+static struct phy_device *tgec_init_phy(struct net_device *net_dev,
+					struct mac_device *mac_dev)
 {
 	return init_phy(net_dev, mac_dev, adjust_link_void);
 }
 
-static int memac_init_phy(struct net_device *net_dev,
-			  struct mac_device *mac_dev)
+static struct phy_device *memac_init_phy(struct net_device *net_dev,
+					 struct mac_device *mac_dev)
 {
 	return init_phy(net_dev, mac_dev, &adjust_link_memac);
 }
@@ -583,31 +583,6 @@ static void setup_memac(struct mac_device *mac_dev)
 
 static DEFINE_MUTEX(eth_lock);
 
-static const char phy_str[][11] = {
-	[PHY_INTERFACE_MODE_MII]		= "mii",
-	[PHY_INTERFACE_MODE_GMII]		= "gmii",
-	[PHY_INTERFACE_MODE_SGMII]		= "sgmii",
-	[PHY_INTERFACE_MODE_TBI]		= "tbi",
-	[PHY_INTERFACE_MODE_RMII]		= "rmii",
-	[PHY_INTERFACE_MODE_RGMII]		= "rgmii",
-	[PHY_INTERFACE_MODE_RGMII_ID]		= "rgmii-id",
-	[PHY_INTERFACE_MODE_RGMII_RXID]	= "rgmii-rxid",
-	[PHY_INTERFACE_MODE_RGMII_TXID]	= "rgmii-txid",
-	[PHY_INTERFACE_MODE_RTBI]		= "rtbi",
-	[PHY_INTERFACE_MODE_XGMII]		= "xgmii"
-};
-
-static phy_interface_t __pure __attribute__((nonnull)) str2phy(const char *str)
-{
-	int i;
-
-	for (i = 0; i < ARRAY_SIZE(phy_str); i++)
-		if (strcmp(str, phy_str[i]) == 0)
-			return (phy_interface_t)i;
-
-	return PHY_INTERFACE_MODE_MII;
-}
-
 static const u16 phy2speed[] = {
 	[PHY_INTERFACE_MODE_MII]		= SPEED_100,
 	[PHY_INTERFACE_MODE_GMII]		= SPEED_1000,
@@ -678,7 +653,7 @@ MODULE_DEVICE_TABLE(of, mac_match);
 
 static int mac_probe(struct platform_device *_of_dev)
 {
-	int			 err, i, lenp, nph;
+	int			 err, i, nph;
 	struct device		*dev;
 	struct device_node	*mac_node, *dev_node;
 	struct mac_device	*mac_dev;
@@ -686,9 +661,9 @@ static int mac_probe(struct platform_device *_of_dev)
 	struct resource		 res;
 	struct mac_priv_s	*priv;
 	const u8		*mac_addr;
-	const char		*char_prop;
-	const u32		*u32_prop;
+	u32			 val;
 	u8			fman_id;
+	int			phy_if;
 
 	dev = &_of_dev->dev;
 	mac_node = dev->of_node;
@@ -749,16 +724,15 @@ static int mac_probe(struct platform_device *_of_dev)
 	}
 
 	/* Get the FMan cell-index */
-	u32_prop = of_get_property(dev_node, "cell-index", &lenp);
-	if (!u32_prop) {
-		dev_err(dev, "of_get_property(%s, cell-index) failed\n",
+	err = of_property_read_u32(dev_node, "cell-index", &val);
+	if (err) {
+		dev_err(dev, "failed to read cell-index for %s\n",
 			dev_node->full_name);
 		err = -EINVAL;
 		goto _return_of_node_put;
 	}
-	WARN_ON(lenp != sizeof(u32));
 	/* cell-index 0 => FMan id 1 */
-	fman_id = (u8)(fdt32_to_cpu(u32_prop[0]) + 1);
+	fman_id = (u8)(val + 1);
 
 	priv->fman = fman_bind(&of_dev->dev);
 	if (!priv->fman) {
@@ -805,15 +779,14 @@ static int mac_probe(struct platform_device *_of_dev)
 	}
 
 	/* Get the cell-index */
-	u32_prop = of_get_property(mac_node, "cell-index", &lenp);
-	if (!u32_prop) {
-		dev_err(dev, "of_get_property(%s, cell-index) failed\n",
+	err = of_property_read_u32(mac_node, "cell-index", &val);
+	if (err) {
+		dev_err(dev, "failed to read cell-index for %s\n",
 			mac_node->full_name);
 		err = -EINVAL;
 		goto _return_dev_set_drvdata;
 	}
-	WARN_ON(lenp != sizeof(u32));
-	priv->cell_index = (u8)fdt32_to_cpu(u32_prop[0]);
+	priv->cell_index = (u8)val;
 
 	/* Get the MAC address */
 	mac_addr = of_get_mac_address(mac_node);
@@ -870,16 +843,14 @@ static int mac_probe(struct platform_device *_of_dev)
 	}
 
 	/* Get the PHY connection type */
-	char_prop = (const char *)of_get_property(mac_node,
-						  "phy-connection-type", NULL);
-	if (!char_prop) {
+	phy_if = of_get_phy_mode(mac_node);
+	if (phy_if < 0) {
 		dev_warn(dev,
-			 "of_get_property(%s, phy-connection-type) failed. Defaulting to MII\n",
+			 "of_get_phy_mode() for %s failed. Defaulting to SGMII\n",
 			 mac_node->full_name);
-		priv->phy_if = PHY_INTERFACE_MODE_MII;
-	} else {
-		priv->phy_if = str2phy(char_prop);
+		phy_if = PHY_INTERFACE_MODE_SGMII;
 	}
+	priv->phy_if = phy_if;
 
 	priv->speed		= phy2speed[priv->phy_if];
 	priv->max_speed		= priv->speed;
diff --git a/drivers/net/ethernet/freescale/fman/mac.h b/drivers/net/ethernet/freescale/fman/mac.h
index 0211cc9a46d6..d7313f0c5135 100644
--- a/drivers/net/ethernet/freescale/fman/mac.h
+++ b/drivers/net/ethernet/freescale/fman/mac.h
@@ -58,7 +58,8 @@ struct mac_device {
 	bool tx_pause_active;
 	bool promisc;
 
-	int (*init_phy)(struct net_device *net_dev, struct mac_device *mac_dev);
+	struct phy_device *(*init_phy)(struct net_device *net_dev,
+				       struct mac_device *mac_dev);
 	int (*init)(struct mac_device *mac_dev);
 	int (*start)(struct mac_device *mac_dev);
 	int (*stop)(struct mac_device *mac_dev);
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 61fd486c50bb..dc120c148d97 100644
--- a/drivers/net/ethernet/freescale/fs_enet/fs_enet-main.c
+++ b/drivers/net/ethernet/freescale/fs_enet/fs_enet-main.c
@@ -60,6 +60,9 @@ module_param(fs_enet_debug, int, 0);
 MODULE_PARM_DESC(fs_enet_debug,
 		 "Freescale bitmapped debugging message enable value");
 
+#define RX_RING_SIZE	32
+#define TX_RING_SIZE	64
+
 #ifdef CONFIG_NET_POLL_CONTROLLER
 static void fs_enet_netpoll(struct net_device *dev);
 #endif
@@ -79,8 +82,8 @@ static void skb_align(struct sk_buff *skb, int align)
 		skb_reserve(skb, align - off);
 }
 
-/* NAPI receive function */
-static int fs_enet_rx_napi(struct napi_struct *napi, int budget)
+/* NAPI function */
+static int fs_enet_napi(struct napi_struct *napi, int budget)
 {
 	struct fs_enet_private *fep = container_of(napi, struct fs_enet_private, napi);
 	struct net_device *dev = fep->ndev;
@@ -90,9 +93,102 @@ static int fs_enet_rx_napi(struct napi_struct *napi, int budget)
 	int received = 0;
 	u16 pkt_len, sc;
 	int curidx;
+	int dirtyidx, do_wake, do_restart;
+	int tx_left = TX_RING_SIZE;
 
-	if (budget <= 0)
-		return received;
+	spin_lock(&fep->tx_lock);
+	bdp = fep->dirty_tx;
+
+	/* clear status bits for napi*/
+	(*fep->ops->napi_clear_event)(dev);
+
+	do_wake = do_restart = 0;
+	while (((sc = CBDR_SC(bdp)) & BD_ENET_TX_READY) == 0 && tx_left) {
+		dirtyidx = bdp - fep->tx_bd_base;
+
+		if (fep->tx_free == fep->tx_ring)
+			break;
+
+		skb = fep->tx_skbuff[dirtyidx];
+
+		/*
+		 * Check for errors.
+		 */
+		if (sc & (BD_ENET_TX_HB | BD_ENET_TX_LC |
+			  BD_ENET_TX_RL | BD_ENET_TX_UN | BD_ENET_TX_CSL)) {
+
+			if (sc & BD_ENET_TX_HB)	/* No heartbeat */
+				fep->stats.tx_heartbeat_errors++;
+			if (sc & BD_ENET_TX_LC)	/* Late collision */
+				fep->stats.tx_window_errors++;
+			if (sc & BD_ENET_TX_RL)	/* Retrans limit */
+				fep->stats.tx_aborted_errors++;
+			if (sc & BD_ENET_TX_UN)	/* Underrun */
+				fep->stats.tx_fifo_errors++;
+			if (sc & BD_ENET_TX_CSL)	/* Carrier lost */
+				fep->stats.tx_carrier_errors++;
+
+			if (sc & (BD_ENET_TX_LC | BD_ENET_TX_RL | BD_ENET_TX_UN)) {
+				fep->stats.tx_errors++;
+				do_restart = 1;
+			}
+		} else
+			fep->stats.tx_packets++;
+
+		if (sc & BD_ENET_TX_READY) {
+			dev_warn(fep->dev,
+				 "HEY! Enet xmit interrupt and TX_READY.\n");
+		}
+
+		/*
+		 * Deferred means some collisions occurred during transmit,
+		 * but we eventually sent the packet OK.
+		 */
+		if (sc & BD_ENET_TX_DEF)
+			fep->stats.collisions++;
+
+		/* unmap */
+		if (fep->mapped_as_page[dirtyidx])
+			dma_unmap_page(fep->dev, CBDR_BUFADDR(bdp),
+				       CBDR_DATLEN(bdp), DMA_TO_DEVICE);
+		else
+			dma_unmap_single(fep->dev, CBDR_BUFADDR(bdp),
+					 CBDR_DATLEN(bdp), DMA_TO_DEVICE);
+
+		/*
+		 * Free the sk buffer associated with this last transmit.
+		 */
+		if (skb) {
+			dev_kfree_skb(skb);
+			fep->tx_skbuff[dirtyidx] = NULL;
+		}
+
+		/*
+		 * Update pointer to next buffer descriptor to be transmitted.
+		 */
+		if ((sc & BD_ENET_TX_WRAP) == 0)
+			bdp++;
+		else
+			bdp = fep->tx_bd_base;
+
+		/*
+		 * Since we have freed up a buffer, the ring is no longer
+		 * full.
+		 */
+		if (++fep->tx_free == MAX_SKB_FRAGS)
+			do_wake = 1;
+		tx_left--;
+	}
+
+	fep->dirty_tx = bdp;
+
+	if (do_restart)
+		(*fep->ops->tx_restart)(dev);
+
+	spin_unlock(&fep->tx_lock);
+
+	if (do_wake)
+		netif_wake_queue(dev);
 
 	/*
 	 * First, grab all of the stats for the incoming packet.
@@ -100,10 +196,8 @@ static int fs_enet_rx_napi(struct napi_struct *napi, int budget)
 	 */
 	bdp = fep->cur_rx;
 
-	/* clear RX status bits for napi*/
-	(*fep->ops->napi_clear_rx_event)(dev);
-
-	while (((sc = CBDR_SC(bdp)) & BD_ENET_RX_EMPTY) == 0) {
+	while (((sc = CBDR_SC(bdp)) & BD_ENET_RX_EMPTY) == 0 &&
+	       received < budget) {
 		curidx = bdp - fep->rx_bd_base;
 
 		/*
@@ -132,21 +226,10 @@ static int fs_enet_rx_napi(struct napi_struct *napi, int budget)
 			if (sc & BD_ENET_RX_OV)
 				fep->stats.rx_crc_errors++;
 
-			skb = fep->rx_skbuff[curidx];
-
-			dma_unmap_single(fep->dev, CBDR_BUFADDR(bdp),
-				L1_CACHE_ALIGN(PKT_MAXBUF_SIZE),
-				DMA_FROM_DEVICE);
-
-			skbn = skb;
-
+			skbn = fep->rx_skbuff[curidx];
 		} else {
 			skb = fep->rx_skbuff[curidx];
 
-			dma_unmap_single(fep->dev, CBDR_BUFADDR(bdp),
-				L1_CACHE_ALIGN(PKT_MAXBUF_SIZE),
-				DMA_FROM_DEVICE);
-
 			/*
 			 * Process the incoming frame.
 			 */
@@ -162,12 +245,30 @@ static int fs_enet_rx_napi(struct napi_struct *napi, int budget)
 					skb_copy_from_linear_data(skb,
 						      skbn->data, pkt_len);
 					swap(skb, skbn);
+					dma_sync_single_for_cpu(fep->dev,
+						CBDR_BUFADDR(bdp),
+						L1_CACHE_ALIGN(pkt_len),
+						DMA_FROM_DEVICE);
 				}
 			} else {
 				skbn = netdev_alloc_skb(dev, ENET_RX_FRSIZE);
 
-				if (skbn)
+				if (skbn) {
+					dma_addr_t dma;
+
 					skb_align(skbn, ENET_RX_ALIGN);
+
+					dma_unmap_single(fep->dev,
+						CBDR_BUFADDR(bdp),
+						L1_CACHE_ALIGN(PKT_MAXBUF_SIZE),
+						DMA_FROM_DEVICE);
+
+					dma = dma_map_single(fep->dev,
+						skbn->data,
+						L1_CACHE_ALIGN(PKT_MAXBUF_SIZE),
+						DMA_FROM_DEVICE);
+					CBDW_BUFADDR(bdp, dma);
+				}
 			}
 
 			if (skbn != NULL) {
@@ -182,9 +283,6 @@ static int fs_enet_rx_napi(struct napi_struct *napi, int budget)
 		}
 
 		fep->rx_skbuff[curidx] = skbn;
-		CBDW_BUFADDR(bdp, dma_map_single(fep->dev, skbn->data,
-			     L1_CACHE_ALIGN(PKT_MAXBUF_SIZE),
-			     DMA_FROM_DEVICE));
 		CBDW_DATLEN(bdp, 0);
 		CBDW_SC(bdp, (sc & ~BD_ENET_RX_STATS) | BD_ENET_RX_EMPTY);
 
@@ -197,134 +295,19 @@ static int fs_enet_rx_napi(struct napi_struct *napi, int budget)
 			bdp = fep->rx_bd_base;
 
 		(*fep->ops->rx_bd_done)(dev);
-
-		if (received >= budget)
-			break;
 	}
 
 	fep->cur_rx = bdp;
 
-	if (received < budget) {
+	if (received < budget && tx_left) {
 		/* done */
 		napi_complete(napi);
-		(*fep->ops->napi_enable_rx)(dev);
-	}
-	return received;
-}
-
-static int fs_enet_tx_napi(struct napi_struct *napi, int budget)
-{
-	struct fs_enet_private *fep = container_of(napi, struct fs_enet_private,
-						   napi_tx);
-	struct net_device *dev = fep->ndev;
-	cbd_t __iomem *bdp;
-	struct sk_buff *skb;
-	int dirtyidx, do_wake, do_restart;
-	u16 sc;
-	int has_tx_work = 0;
-
-	spin_lock(&fep->tx_lock);
-	bdp = fep->dirty_tx;
-
-	/* clear TX status bits for napi*/
-	(*fep->ops->napi_clear_tx_event)(dev);
-
-	do_wake = do_restart = 0;
-	while (((sc = CBDR_SC(bdp)) & BD_ENET_TX_READY) == 0) {
-		dirtyidx = bdp - fep->tx_bd_base;
-
-		if (fep->tx_free == fep->tx_ring)
-			break;
-
-		skb = fep->tx_skbuff[dirtyidx];
-
-		/*
-		 * Check for errors.
-		 */
-		if (sc & (BD_ENET_TX_HB | BD_ENET_TX_LC |
-			  BD_ENET_TX_RL | BD_ENET_TX_UN | BD_ENET_TX_CSL)) {
-
-			if (sc & BD_ENET_TX_HB)	/* No heartbeat */
-				fep->stats.tx_heartbeat_errors++;
-			if (sc & BD_ENET_TX_LC)	/* Late collision */
-				fep->stats.tx_window_errors++;
-			if (sc & BD_ENET_TX_RL)	/* Retrans limit */
-				fep->stats.tx_aborted_errors++;
-			if (sc & BD_ENET_TX_UN)	/* Underrun */
-				fep->stats.tx_fifo_errors++;
-			if (sc & BD_ENET_TX_CSL)	/* Carrier lost */
-				fep->stats.tx_carrier_errors++;
-
-			if (sc & (BD_ENET_TX_LC | BD_ENET_TX_RL | BD_ENET_TX_UN)) {
-				fep->stats.tx_errors++;
-				do_restart = 1;
-			}
-		} else
-			fep->stats.tx_packets++;
-
-		if (sc & BD_ENET_TX_READY) {
-			dev_warn(fep->dev,
-				 "HEY! Enet xmit interrupt and TX_READY.\n");
-		}
-
-		/*
-		 * Deferred means some collisions occurred during transmit,
-		 * but we eventually sent the packet OK.
-		 */
-		if (sc & BD_ENET_TX_DEF)
-			fep->stats.collisions++;
-
-		/* unmap */
-		if (fep->mapped_as_page[dirtyidx])
-			dma_unmap_page(fep->dev, CBDR_BUFADDR(bdp),
-				       CBDR_DATLEN(bdp), DMA_TO_DEVICE);
-		else
-			dma_unmap_single(fep->dev, CBDR_BUFADDR(bdp),
-					 CBDR_DATLEN(bdp), DMA_TO_DEVICE);
-
-		/*
-		 * Free the sk buffer associated with this last transmit.
-		 */
-		if (skb) {
-			dev_kfree_skb(skb);
-			fep->tx_skbuff[dirtyidx] = NULL;
-		}
-
-		/*
-		 * Update pointer to next buffer descriptor to be transmitted.
-		 */
-		if ((sc & BD_ENET_TX_WRAP) == 0)
-			bdp++;
-		else
-			bdp = fep->tx_bd_base;
-
-		/*
-		 * Since we have freed up a buffer, the ring is no longer
-		 * full.
-		 */
-		if (++fep->tx_free >= MAX_SKB_FRAGS)
-			do_wake = 1;
-		has_tx_work = 1;
-	}
-
-	fep->dirty_tx = bdp;
-
-	if (do_restart)
-		(*fep->ops->tx_restart)(dev);
+		(*fep->ops->napi_enable)(dev);
 
-	if (!has_tx_work) {
-		napi_complete(napi);
-		(*fep->ops->napi_enable_tx)(dev);
+		return received;
 	}
 
-	spin_unlock(&fep->tx_lock);
-
-	if (do_wake)
-		netif_wake_queue(dev);
-
-	if (has_tx_work)
-		return budget;
-	return 0;
+	return budget;
 }
 
 /*
@@ -350,18 +333,18 @@ fs_enet_interrupt(int irq, void *dev_id)
 		nr++;
 
 		int_clr_events = int_events;
-		int_clr_events &= ~fep->ev_napi_rx;
+		int_clr_events &= ~fep->ev_napi;
 
 		(*fep->ops->clear_int_events)(dev, int_clr_events);
 
 		if (int_events & fep->ev_err)
 			(*fep->ops->ev_error)(dev, int_events);
 
-		if (int_events & fep->ev_rx) {
+		if (int_events & fep->ev) {
 			napi_ok = napi_schedule_prep(&fep->napi);
 
-			(*fep->ops->napi_disable_rx)(dev);
-			(*fep->ops->clear_int_events)(dev, fep->ev_napi_rx);
+			(*fep->ops->napi_disable)(dev);
+			(*fep->ops->clear_int_events)(dev, fep->ev_napi);
 
 			/* NOTE: it is possible for FCCs in NAPI mode    */
 			/* to submit a spurious interrupt while in poll  */
@@ -369,17 +352,6 @@ fs_enet_interrupt(int irq, void *dev_id)
 				__napi_schedule(&fep->napi);
 		}
 
-		if (int_events & fep->ev_tx) {
-			napi_ok = napi_schedule_prep(&fep->napi_tx);
-
-			(*fep->ops->napi_disable_tx)(dev);
-			(*fep->ops->clear_int_events)(dev, fep->ev_napi_tx);
-
-			/* NOTE: it is possible for FCCs in NAPI mode    */
-			/* to submit a spurious interrupt while in poll  */
-			if (napi_ok)
-				__napi_schedule(&fep->napi_tx);
-		}
 	}
 
 	handled = nr > 0;
@@ -659,7 +631,8 @@ static void fs_timeout(struct net_device *dev)
 	}
 
 	phy_start(dev->phydev);
-	wake = fep->tx_free && !(CBDR_SC(fep->cur_tx) & BD_ENET_TX_READY);
+	wake = fep->tx_free >= MAX_SKB_FRAGS &&
+	       !(CBDR_SC(fep->cur_tx) & BD_ENET_TX_READY);
 	spin_unlock_irqrestore(&fep->lock, flags);
 
 	if (wake)
@@ -751,11 +724,10 @@ static int fs_enet_open(struct net_device *dev)
 	int err;
 
 	/* to initialize the fep->cur_rx,... */
-	/* not doing this, will cause a crash in fs_enet_rx_napi */
+	/* not doing this, will cause a crash in fs_enet_napi */
 	fs_init_bds(fep->ndev);
 
 	napi_enable(&fep->napi);
-	napi_enable(&fep->napi_tx);
 
 	/* Install our interrupt handler. */
 	r = request_irq(fep->interrupt, fs_enet_interrupt, IRQF_SHARED,
@@ -763,7 +735,6 @@ static int fs_enet_open(struct net_device *dev)
 	if (r != 0) {
 		dev_err(fep->dev, "Could not allocate FS_ENET IRQ!");
 		napi_disable(&fep->napi);
-		napi_disable(&fep->napi_tx);
 		return -EINVAL;
 	}
 
@@ -771,7 +742,6 @@ static int fs_enet_open(struct net_device *dev)
 	if (err) {
 		free_irq(fep->interrupt, dev);
 		napi_disable(&fep->napi);
-		napi_disable(&fep->napi_tx);
 		return err;
 	}
 	phy_start(dev->phydev);
@@ -789,7 +759,6 @@ static int fs_enet_close(struct net_device *dev)
 	netif_stop_queue(dev);
 	netif_carrier_off(dev);
 	napi_disable(&fep->napi);
-	napi_disable(&fep->napi_tx);
 	phy_stop(dev->phydev);
 
 	spin_lock_irqsave(&fep->lock, flags);
@@ -861,6 +830,44 @@ static void fs_set_msglevel(struct net_device *dev, u32 value)
 	fep->msg_enable = value;
 }
 
+static int fs_get_tunable(struct net_device *dev,
+			  const struct ethtool_tunable *tuna, void *data)
+{
+	struct fs_enet_private *fep = netdev_priv(dev);
+	struct fs_platform_info *fpi = fep->fpi;
+	int ret = 0;
+
+	switch (tuna->id) {
+	case ETHTOOL_RX_COPYBREAK:
+		*(u32 *)data = fpi->rx_copybreak;
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
+static int fs_set_tunable(struct net_device *dev,
+			  const struct ethtool_tunable *tuna, const void *data)
+{
+	struct fs_enet_private *fep = netdev_priv(dev);
+	struct fs_platform_info *fpi = fep->fpi;
+	int ret = 0;
+
+	switch (tuna->id) {
+	case ETHTOOL_RX_COPYBREAK:
+		fpi->rx_copybreak = *(u32 *)data;
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
 static const struct ethtool_ops fs_ethtool_ops = {
 	.get_drvinfo = fs_get_drvinfo,
 	.get_regs_len = fs_get_regs_len,
@@ -872,6 +879,8 @@ static const struct ethtool_ops fs_ethtool_ops = {
 	.get_ts_info = ethtool_op_get_ts_info,
 	.get_link_ksettings = phy_ethtool_get_link_ksettings,
 	.set_link_ksettings = phy_ethtool_set_link_ksettings,
+	.get_tunable = fs_get_tunable,
+	.set_tunable = fs_set_tunable,
 };
 
 static int fs_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
@@ -939,8 +948,8 @@ static int fs_enet_probe(struct platform_device *ofdev)
 		fpi->cp_command = *data;
 	}
 
-	fpi->rx_ring = 32;
-	fpi->tx_ring = 64;
+	fpi->rx_ring = RX_RING_SIZE;
+	fpi->tx_ring = TX_RING_SIZE;
 	fpi->rx_copybreak = 240;
 	fpi->napi_weight = 17;
 	fpi->phy_node = of_parse_phandle(ofdev->dev.of_node, "phy-handle", 0);
@@ -1024,8 +1033,7 @@ static int fs_enet_probe(struct platform_device *ofdev)
 
 	ndev->netdev_ops = &fs_enet_netdev_ops;
 	ndev->watchdog_timeo = 2 * HZ;
-	netif_napi_add(ndev, &fep->napi, fs_enet_rx_napi, fpi->napi_weight);
-	netif_tx_napi_add(ndev, &fep->napi_tx, fs_enet_tx_napi, 2);
+	netif_napi_add(ndev, &fep->napi, fs_enet_napi, fpi->napi_weight);
 
 	ndev->ethtool_ops = &fs_ethtool_ops;
 
diff --git a/drivers/net/ethernet/freescale/fs_enet/fs_enet.h b/drivers/net/ethernet/freescale/fs_enet/fs_enet.h
index e29f54a35210..fee24c822fad 100644
--- a/drivers/net/ethernet/freescale/fs_enet/fs_enet.h
+++ b/drivers/net/ethernet/freescale/fs_enet/fs_enet.h
@@ -81,12 +81,9 @@ struct fs_ops {
 	void (*adjust_link)(struct net_device *dev);
 	void (*restart)(struct net_device *dev);
 	void (*stop)(struct net_device *dev);
-	void (*napi_clear_rx_event)(struct net_device *dev);
-	void (*napi_enable_rx)(struct net_device *dev);
-	void (*napi_disable_rx)(struct net_device *dev);
-	void (*napi_clear_tx_event)(struct net_device *dev);
-	void (*napi_enable_tx)(struct net_device *dev);
-	void (*napi_disable_tx)(struct net_device *dev);
+	void (*napi_clear_event)(struct net_device *dev);
+	void (*napi_enable)(struct net_device *dev);
+	void (*napi_disable)(struct net_device *dev);
 	void (*rx_bd_done)(struct net_device *dev);
 	void (*tx_kickstart)(struct net_device *dev);
 	u32 (*get_int_events)(struct net_device *dev);
@@ -122,7 +119,6 @@ struct phy_info {
 
 struct fs_enet_private {
 	struct napi_struct napi;
-	struct napi_struct napi_tx;
 	struct device *dev;	/* pointer back to the device (must be initialized first) */
 	struct net_device *ndev;
 	spinlock_t lock;	/* during all ops except TX pckt processing */
@@ -152,10 +148,8 @@ struct fs_enet_private {
 	int oldduplex, oldspeed, oldlink;	/* current settings */
 
 	/* event masks */
-	u32 ev_napi_rx;		/* mask of NAPI rx events */
-	u32 ev_napi_tx;		/* mask of NAPI rx events */
-	u32 ev_rx;		/* rx event mask          */
-	u32 ev_tx;		/* tx event mask          */
+	u32 ev_napi;		/* mask of NAPI events */
+	u32 ev;			/* event mask          */
 	u32 ev_err;		/* error event mask       */
 
 	u16 bd_rx_empty;	/* mask of BD rx empty	  */
diff --git a/drivers/net/ethernet/freescale/fs_enet/mac-fcc.c b/drivers/net/ethernet/freescale/fs_enet/mac-fcc.c
index d71761a34022..120c758f5d01 100644
--- a/drivers/net/ethernet/freescale/fs_enet/mac-fcc.c
+++ b/drivers/net/ethernet/freescale/fs_enet/mac-fcc.c
@@ -90,7 +90,7 @@ static int do_pd_setup(struct fs_enet_private *fep)
 	int ret = -EINVAL;
 
 	fep->interrupt = irq_of_parse_and_map(ofdev->dev.of_node, 0);
-	if (fep->interrupt == NO_IRQ)
+	if (!fep->interrupt)
 		goto out;
 
 	fep->fcc.fccp = of_iomap(ofdev->dev.of_node, 0);
@@ -124,10 +124,8 @@ out:
 	return ret;
 }
 
-#define FCC_NAPI_RX_EVENT_MSK	(FCC_ENET_RXF | FCC_ENET_RXB)
-#define FCC_NAPI_TX_EVENT_MSK	(FCC_ENET_TXB)
-#define FCC_RX_EVENT		(FCC_ENET_RXF)
-#define FCC_TX_EVENT		(FCC_ENET_TXB)
+#define FCC_NAPI_EVENT_MSK	(FCC_ENET_RXF | FCC_ENET_RXB | FCC_ENET_TXB)
+#define FCC_EVENT		(FCC_ENET_RXF | FCC_ENET_TXB)
 #define FCC_ERR_EVENT_MSK	(FCC_ENET_TXE)
 
 static int setup_data(struct net_device *dev)
@@ -137,10 +135,8 @@ static int setup_data(struct net_device *dev)
 	if (do_pd_setup(fep) != 0)
 		return -EINVAL;
 
-	fep->ev_napi_rx = FCC_NAPI_RX_EVENT_MSK;
-	fep->ev_napi_tx = FCC_NAPI_TX_EVENT_MSK;
-	fep->ev_rx = FCC_RX_EVENT;
-	fep->ev_tx = FCC_TX_EVENT;
+	fep->ev_napi = FCC_NAPI_EVENT_MSK;
+	fep->ev = FCC_EVENT;
 	fep->ev_err = FCC_ERR_EVENT_MSK;
 
 	return 0;
@@ -424,52 +420,28 @@ static void stop(struct net_device *dev)
 	fs_cleanup_bds(dev);
 }
 
-static void napi_clear_rx_event(struct net_device *dev)
+static void napi_clear_event_fs(struct net_device *dev)
 {
 	struct fs_enet_private *fep = netdev_priv(dev);
 	fcc_t __iomem *fccp = fep->fcc.fccp;
 
-	W16(fccp, fcc_fcce, FCC_NAPI_RX_EVENT_MSK);
+	W16(fccp, fcc_fcce, FCC_NAPI_EVENT_MSK);
 }
 
-static void napi_enable_rx(struct net_device *dev)
+static void napi_enable_fs(struct net_device *dev)
 {
 	struct fs_enet_private *fep = netdev_priv(dev);
 	fcc_t __iomem *fccp = fep->fcc.fccp;
 
-	S16(fccp, fcc_fccm, FCC_NAPI_RX_EVENT_MSK);
+	S16(fccp, fcc_fccm, FCC_NAPI_EVENT_MSK);
 }
 
-static void napi_disable_rx(struct net_device *dev)
+static void napi_disable_fs(struct net_device *dev)
 {
 	struct fs_enet_private *fep = netdev_priv(dev);
 	fcc_t __iomem *fccp = fep->fcc.fccp;
 
-	C16(fccp, fcc_fccm, FCC_NAPI_RX_EVENT_MSK);
-}
-
-static void napi_clear_tx_event(struct net_device *dev)
-{
-	struct fs_enet_private *fep = netdev_priv(dev);
-	fcc_t __iomem *fccp = fep->fcc.fccp;
-
-	W16(fccp, fcc_fcce, FCC_NAPI_TX_EVENT_MSK);
-}
-
-static void napi_enable_tx(struct net_device *dev)
-{
-	struct fs_enet_private *fep = netdev_priv(dev);
-	fcc_t __iomem *fccp = fep->fcc.fccp;
-
-	S16(fccp, fcc_fccm, FCC_NAPI_TX_EVENT_MSK);
-}
-
-static void napi_disable_tx(struct net_device *dev)
-{
-	struct fs_enet_private *fep = netdev_priv(dev);
-	fcc_t __iomem *fccp = fep->fcc.fccp;
-
-	C16(fccp, fcc_fccm, FCC_NAPI_TX_EVENT_MSK);
+	C16(fccp, fcc_fccm, FCC_NAPI_EVENT_MSK);
 }
 
 static void rx_bd_done(struct net_device *dev)
@@ -595,12 +567,9 @@ const struct fs_ops fs_fcc_ops = {
 	.set_multicast_list	= set_multicast_list,
 	.restart		= restart,
 	.stop			= stop,
-	.napi_clear_rx_event	= napi_clear_rx_event,
-	.napi_enable_rx		= napi_enable_rx,
-	.napi_disable_rx	= napi_disable_rx,
-	.napi_clear_tx_event	= napi_clear_tx_event,
-	.napi_enable_tx		= napi_enable_tx,
-	.napi_disable_tx	= napi_disable_tx,
+	.napi_clear_event	= napi_clear_event_fs,
+	.napi_enable		= napi_enable_fs,
+	.napi_disable		= napi_disable_fs,
 	.rx_bd_done		= rx_bd_done,
 	.tx_kickstart		= tx_kickstart,
 	.get_int_events		= get_int_events,
diff --git a/drivers/net/ethernet/freescale/fs_enet/mac-fec.c b/drivers/net/ethernet/freescale/fs_enet/mac-fec.c
index 35a318ed3a62..777beffa1e1e 100644
--- a/drivers/net/ethernet/freescale/fs_enet/mac-fec.c
+++ b/drivers/net/ethernet/freescale/fs_enet/mac-fec.c
@@ -99,7 +99,7 @@ static int do_pd_setup(struct fs_enet_private *fep)
 	struct platform_device *ofdev = to_platform_device(fep->dev);
 
 	fep->interrupt = irq_of_parse_and_map(ofdev->dev.of_node, 0);
-	if (fep->interrupt == NO_IRQ)
+	if (!fep->interrupt)
 		return -EINVAL;
 
 	fep->fec.fecp = of_iomap(ofdev->dev.of_node, 0);
@@ -109,10 +109,8 @@ static int do_pd_setup(struct fs_enet_private *fep)
 	return 0;
 }
 
-#define FEC_NAPI_RX_EVENT_MSK	(FEC_ENET_RXF | FEC_ENET_RXB)
-#define FEC_NAPI_TX_EVENT_MSK	(FEC_ENET_TXF)
-#define FEC_RX_EVENT		(FEC_ENET_RXF)
-#define FEC_TX_EVENT		(FEC_ENET_TXF)
+#define FEC_NAPI_EVENT_MSK	(FEC_ENET_RXF | FEC_ENET_RXB | FEC_ENET_TXF)
+#define FEC_EVENT		(FEC_ENET_RXF | FEC_ENET_TXF)
 #define FEC_ERR_EVENT_MSK	(FEC_ENET_HBERR | FEC_ENET_BABR | \
 				 FEC_ENET_BABT | FEC_ENET_EBERR)
 
@@ -126,10 +124,8 @@ static int setup_data(struct net_device *dev)
 	fep->fec.hthi = 0;
 	fep->fec.htlo = 0;
 
-	fep->ev_napi_rx = FEC_NAPI_RX_EVENT_MSK;
-	fep->ev_napi_tx = FEC_NAPI_TX_EVENT_MSK;
-	fep->ev_rx = FEC_RX_EVENT;
-	fep->ev_tx = FEC_TX_EVENT;
+	fep->ev_napi = FEC_NAPI_EVENT_MSK;
+	fep->ev = FEC_EVENT;
 	fep->ev_err = FEC_ERR_EVENT_MSK;
 
 	return 0;
@@ -396,52 +392,28 @@ static void stop(struct net_device *dev)
 	}
 }
 
-static void napi_clear_rx_event(struct net_device *dev)
+static void napi_clear_event_fs(struct net_device *dev)
 {
 	struct fs_enet_private *fep = netdev_priv(dev);
 	struct fec __iomem *fecp = fep->fec.fecp;
 
-	FW(fecp, ievent, FEC_NAPI_RX_EVENT_MSK);
+	FW(fecp, ievent, FEC_NAPI_EVENT_MSK);
 }
 
-static void napi_enable_rx(struct net_device *dev)
+static void napi_enable_fs(struct net_device *dev)
 {
 	struct fs_enet_private *fep = netdev_priv(dev);
 	struct fec __iomem *fecp = fep->fec.fecp;
 
-	FS(fecp, imask, FEC_NAPI_RX_EVENT_MSK);
+	FS(fecp, imask, FEC_NAPI_EVENT_MSK);
 }
 
-static void napi_disable_rx(struct net_device *dev)
+static void napi_disable_fs(struct net_device *dev)
 {
 	struct fs_enet_private *fep = netdev_priv(dev);
 	struct fec __iomem *fecp = fep->fec.fecp;
 
-	FC(fecp, imask, FEC_NAPI_RX_EVENT_MSK);
-}
-
-static void napi_clear_tx_event(struct net_device *dev)
-{
-	struct fs_enet_private *fep = netdev_priv(dev);
-	struct fec __iomem *fecp = fep->fec.fecp;
-
-	FW(fecp, ievent, FEC_NAPI_TX_EVENT_MSK);
-}
-
-static void napi_enable_tx(struct net_device *dev)
-{
-	struct fs_enet_private *fep = netdev_priv(dev);
-	struct fec __iomem *fecp = fep->fec.fecp;
-
-	FS(fecp, imask, FEC_NAPI_TX_EVENT_MSK);
-}
-
-static void napi_disable_tx(struct net_device *dev)
-{
-	struct fs_enet_private *fep = netdev_priv(dev);
-	struct fec __iomem *fecp = fep->fec.fecp;
-
-	FC(fecp, imask, FEC_NAPI_TX_EVENT_MSK);
+	FC(fecp, imask, FEC_NAPI_EVENT_MSK);
 }
 
 static void rx_bd_done(struct net_device *dev)
@@ -513,12 +485,9 @@ const struct fs_ops fs_fec_ops = {
 	.set_multicast_list	= set_multicast_list,
 	.restart		= restart,
 	.stop			= stop,
-	.napi_clear_rx_event	= napi_clear_rx_event,
-	.napi_enable_rx		= napi_enable_rx,
-	.napi_disable_rx	= napi_disable_rx,
-	.napi_clear_tx_event	= napi_clear_tx_event,
-	.napi_enable_tx		= napi_enable_tx,
-	.napi_disable_tx	= napi_disable_tx,
+	.napi_clear_event	= napi_clear_event_fs,
+	.napi_enable		= napi_enable_fs,
+	.napi_disable		= napi_disable_fs,
 	.rx_bd_done		= rx_bd_done,
 	.tx_kickstart		= tx_kickstart,
 	.get_int_events		= get_int_events,
diff --git a/drivers/net/ethernet/freescale/fs_enet/mac-scc.c b/drivers/net/ethernet/freescale/fs_enet/mac-scc.c
index e8b9c33d35b4..15abd37d70e3 100644
--- a/drivers/net/ethernet/freescale/fs_enet/mac-scc.c
+++ b/drivers/net/ethernet/freescale/fs_enet/mac-scc.c
@@ -99,7 +99,7 @@ static int do_pd_setup(struct fs_enet_private *fep)
 	struct platform_device *ofdev = to_platform_device(fep->dev);
 
 	fep->interrupt = irq_of_parse_and_map(ofdev->dev.of_node, 0);
-	if (fep->interrupt == NO_IRQ)
+	if (!fep->interrupt)
 		return -EINVAL;
 
 	fep->scc.sccp = of_iomap(ofdev->dev.of_node, 0);
@@ -115,10 +115,8 @@ static int do_pd_setup(struct fs_enet_private *fep)
 	return 0;
 }
 
-#define SCC_NAPI_RX_EVENT_MSK	(SCCE_ENET_RXF | SCCE_ENET_RXB)
-#define SCC_NAPI_TX_EVENT_MSK	(SCCE_ENET_TXB)
-#define SCC_RX_EVENT		(SCCE_ENET_RXF)
-#define SCC_TX_EVENT		(SCCE_ENET_TXB)
+#define SCC_NAPI_EVENT_MSK	(SCCE_ENET_RXF | SCCE_ENET_RXB | SCCE_ENET_TXB)
+#define SCC_EVENT		(SCCE_ENET_RXF | SCCE_ENET_TXB)
 #define SCC_ERR_EVENT_MSK	(SCCE_ENET_TXE | SCCE_ENET_BSY)
 
 static int setup_data(struct net_device *dev)
@@ -130,10 +128,8 @@ static int setup_data(struct net_device *dev)
 	fep->scc.hthi = 0;
 	fep->scc.htlo = 0;
 
-	fep->ev_napi_rx = SCC_NAPI_RX_EVENT_MSK;
-	fep->ev_napi_tx = SCC_NAPI_TX_EVENT_MSK;
-	fep->ev_rx = SCC_RX_EVENT;
-	fep->ev_tx = SCC_TX_EVENT | SCCE_ENET_TXE;
+	fep->ev_napi = SCC_NAPI_EVENT_MSK;
+	fep->ev = SCC_EVENT | SCCE_ENET_TXE;
 	fep->ev_err = SCC_ERR_EVENT_MSK;
 
 	return 0;
@@ -379,52 +375,28 @@ static void stop(struct net_device *dev)
 	fs_cleanup_bds(dev);
 }
 
-static void napi_clear_rx_event(struct net_device *dev)
+static void napi_clear_event_fs(struct net_device *dev)
 {
 	struct fs_enet_private *fep = netdev_priv(dev);
 	scc_t __iomem *sccp = fep->scc.sccp;
 
-	W16(sccp, scc_scce, SCC_NAPI_RX_EVENT_MSK);
+	W16(sccp, scc_scce, SCC_NAPI_EVENT_MSK);
 }
 
-static void napi_enable_rx(struct net_device *dev)
+static void napi_enable_fs(struct net_device *dev)
 {
 	struct fs_enet_private *fep = netdev_priv(dev);
 	scc_t __iomem *sccp = fep->scc.sccp;
 
-	S16(sccp, scc_sccm, SCC_NAPI_RX_EVENT_MSK);
+	S16(sccp, scc_sccm, SCC_NAPI_EVENT_MSK);
 }
 
-static void napi_disable_rx(struct net_device *dev)
+static void napi_disable_fs(struct net_device *dev)
 {
 	struct fs_enet_private *fep = netdev_priv(dev);
 	scc_t __iomem *sccp = fep->scc.sccp;
 
-	C16(sccp, scc_sccm, SCC_NAPI_RX_EVENT_MSK);
-}
-
-static void napi_clear_tx_event(struct net_device *dev)
-{
-	struct fs_enet_private *fep = netdev_priv(dev);
-	scc_t __iomem *sccp = fep->scc.sccp;
-
-	W16(sccp, scc_scce, SCC_NAPI_TX_EVENT_MSK);
-}
-
-static void napi_enable_tx(struct net_device *dev)
-{
-	struct fs_enet_private *fep = netdev_priv(dev);
-	scc_t __iomem *sccp = fep->scc.sccp;
-
-	S16(sccp, scc_sccm, SCC_NAPI_TX_EVENT_MSK);
-}
-
-static void napi_disable_tx(struct net_device *dev)
-{
-	struct fs_enet_private *fep = netdev_priv(dev);
-	scc_t __iomem *sccp = fep->scc.sccp;
-
-	C16(sccp, scc_sccm, SCC_NAPI_TX_EVENT_MSK);
+	C16(sccp, scc_sccm, SCC_NAPI_EVENT_MSK);
 }
 
 static void rx_bd_done(struct net_device *dev)
@@ -497,12 +469,9 @@ const struct fs_ops fs_scc_ops = {
 	.set_multicast_list	= set_multicast_list,
 	.restart		= restart,
 	.stop			= stop,
-	.napi_clear_rx_event	= napi_clear_rx_event,
-	.napi_enable_rx		= napi_enable_rx,
-	.napi_disable_rx	= napi_disable_rx,
-	.napi_clear_tx_event	= napi_clear_tx_event,
-	.napi_enable_tx		= napi_enable_tx,
-	.napi_disable_tx	= napi_disable_tx,
+	.napi_clear_event	= napi_clear_event_fs,
+	.napi_enable		= napi_enable_fs,
+	.napi_disable		= napi_disable_fs,
 	.rx_bd_done		= rx_bd_done,
 	.tx_kickstart		= tx_kickstart,
 	.get_int_events		= get_int_events,
diff --git a/drivers/net/ethernet/freescale/fsl_pq_mdio.c b/drivers/net/ethernet/freescale/fsl_pq_mdio.c
index f3c63dce1e30..446c7b374ff5 100644
--- a/drivers/net/ethernet/freescale/fsl_pq_mdio.c
+++ b/drivers/net/ethernet/freescale/fsl_pq_mdio.c
@@ -195,7 +195,7 @@ static int fsl_pq_mdio_reset(struct mii_bus *bus)
 	return 0;
 }
 
-#if defined(CONFIG_GIANFAR) || defined(CONFIG_GIANFAR_MODULE)
+#if IS_ENABLED(CONFIG_GIANFAR)
 /*
  * Return the TBIPA address, starting from the address
  * of the mapped GFAR MDIO registers (struct gfar)
@@ -228,7 +228,7 @@ static uint32_t __iomem *get_etsec_tbipa(void __iomem *p)
 }
 #endif
 
-#if defined(CONFIG_UCC_GETH) || defined(CONFIG_UCC_GETH_MODULE)
+#if IS_ENABLED(CONFIG_UCC_GETH)
 /*
  * Return the TBIPAR address for a QE MDIO node, starting from the address
  * of the mapped MII registers (struct fsl_pq_mii)
@@ -306,7 +306,7 @@ static void ucc_configure(phys_addr_t start, phys_addr_t end)
 #endif
 
 static const struct of_device_id fsl_pq_mdio_match[] = {
-#if defined(CONFIG_GIANFAR) || defined(CONFIG_GIANFAR_MODULE)
+#if IS_ENABLED(CONFIG_GIANFAR)
 	{
 		.compatible = "fsl,gianfar-tbi",
 		.data = &(struct fsl_pq_mdio_data) {
@@ -344,7 +344,7 @@ static const struct of_device_id fsl_pq_mdio_match[] = {
 		},
 	},
 #endif
-#if defined(CONFIG_UCC_GETH) || defined(CONFIG_UCC_GETH_MODULE)
+#if IS_ENABLED(CONFIG_UCC_GETH)
 	{
 		.compatible = "fsl,ucc-mdio",
 		.data = &(struct fsl_pq_mdio_data) {
diff --git a/drivers/net/ethernet/freescale/ucc_geth.c b/drivers/net/ethernet/freescale/ucc_geth.c
index 5bf1ade28315..186ef8f16c80 100644
--- a/drivers/net/ethernet/freescale/ucc_geth.c
+++ b/drivers/net/ethernet/freescale/ucc_geth.c
@@ -3756,7 +3756,7 @@ static int ucc_geth_probe(struct platform_device* ofdev)
 			return -EINVAL;
 		}
 		if ((*prop < QE_CLK_NONE) || (*prop > QE_CLK24)) {
-			pr_err("invalid rx-clock propperty\n");
+			pr_err("invalid rx-clock property\n");
 			return -EINVAL;
 		}
 		ug_info->uf_info.rx_clock = *prop;
diff --git a/drivers/net/ethernet/freescale/xgmac_mdio.c b/drivers/net/ethernet/freescale/xgmac_mdio.c
index 7b8fe866f603..e03b30c60dcf 100644
--- a/drivers/net/ethernet/freescale/xgmac_mdio.c
+++ b/drivers/net/ethernet/freescale/xgmac_mdio.c
@@ -271,11 +271,8 @@ static int xgmac_mdio_probe(struct platform_device *pdev)
 		goto err_ioremap;
 	}
 
-	if (of_get_property(pdev->dev.of_node,
-			    "little-endian", NULL))
-		priv->is_little_endian = true;
-	else
-		priv->is_little_endian = false;
+	priv->is_little_endian = of_property_read_bool(pdev->dev.of_node,
+						       "little-endian");
 
 	ret = of_mdiobus_register(bus, np);
 	if (ret) {
diff --git a/drivers/net/ethernet/hisilicon/hip04_eth.c b/drivers/net/ethernet/hisilicon/hip04_eth.c
index 0c4afe95ef54..39778892b3b3 100644
--- a/drivers/net/ethernet/hisilicon/hip04_eth.c
+++ b/drivers/net/ethernet/hisilicon/hip04_eth.c
@@ -600,7 +600,7 @@ static irqreturn_t hip04_mac_interrupt(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
-enum hrtimer_restart tx_done(struct hrtimer *hrtimer)
+static enum hrtimer_restart tx_done(struct hrtimer *hrtimer)
 {
 	struct hip04_priv *priv;
 
@@ -755,13 +755,13 @@ static void hip04_get_drvinfo(struct net_device *netdev,
 	strlcpy(drvinfo->version, DRV_VERSION, sizeof(drvinfo->version));
 }
 
-static struct ethtool_ops hip04_ethtool_ops = {
+static const struct ethtool_ops hip04_ethtool_ops = {
 	.get_coalesce		= hip04_get_coalesce,
 	.set_coalesce		= hip04_set_coalesce,
 	.get_drvinfo		= hip04_get_drvinfo,
 };
 
-static struct net_device_ops hip04_netdev_ops = {
+static const struct net_device_ops hip04_netdev_ops = {
 	.ndo_open		= hip04_mac_open,
 	.ndo_stop		= hip04_mac_stop,
 	.ndo_get_stats		= hip04_get_stats,
diff --git a/drivers/net/ethernet/hisilicon/hisi_femac.c b/drivers/net/ethernet/hisilicon/hisi_femac.c
index b5d7ad0252a0..ced185962ef8 100644
--- a/drivers/net/ethernet/hisilicon/hisi_femac.c
+++ b/drivers/net/ethernet/hisilicon/hisi_femac.c
@@ -699,7 +699,7 @@ static int hisi_femac_net_ioctl(struct net_device *dev,
 	return phy_mii_ioctl(dev->phydev, ifreq, cmd);
 }
 
-static struct ethtool_ops hisi_femac_ethtools_ops = {
+static const struct ethtool_ops hisi_femac_ethtools_ops = {
 	.get_link		= ethtool_op_get_link,
 	.get_link_ksettings	= phy_ethtool_get_link_ksettings,
 	.set_link_ksettings	= phy_ethtool_set_link_ksettings,
@@ -940,8 +940,8 @@ static int hisi_femac_drv_remove(struct platform_device *pdev)
 }
 
 #ifdef CONFIG_PM
-int hisi_femac_drv_suspend(struct platform_device *pdev,
-			   pm_message_t state)
+static int hisi_femac_drv_suspend(struct platform_device *pdev,
+				  pm_message_t state)
 {
 	struct net_device *ndev = platform_get_drvdata(pdev);
 	struct hisi_femac_priv *priv = netdev_priv(ndev);
@@ -957,7 +957,7 @@ int hisi_femac_drv_suspend(struct platform_device *pdev,
 	return 0;
 }
 
-int hisi_femac_drv_resume(struct platform_device *pdev)
+static int hisi_femac_drv_resume(struct platform_device *pdev)
 {
 	struct net_device *ndev = platform_get_drvdata(pdev);
 	struct hisi_femac_priv *priv = netdev_priv(ndev);
diff --git a/drivers/net/ethernet/hisilicon/hix5hd2_gmac.c b/drivers/net/ethernet/hisilicon/hix5hd2_gmac.c
index 275618bb4646..e69a6bed31a9 100644
--- a/drivers/net/ethernet/hisilicon/hix5hd2_gmac.c
+++ b/drivers/net/ethernet/hisilicon/hix5hd2_gmac.c
@@ -750,7 +750,7 @@ static const struct net_device_ops hix5hd2_netdev_ops = {
 	.ndo_set_mac_address	= hix5hd2_net_set_mac_address,
 };
 
-static struct ethtool_ops hix5hd2_ethtools_ops = {
+static const struct ethtool_ops hix5hd2_ethtools_ops = {
 	.get_link		= ethtool_op_get_link,
 	.get_link_ksettings     = phy_ethtool_get_link_ksettings,
 	.set_link_ksettings     = phy_ethtool_set_link_ksettings,
diff --git a/drivers/net/ethernet/hisilicon/hns/hns_ae_adapt.c b/drivers/net/ethernet/hisilicon/hns/hns_ae_adapt.c
index e28d960997af..2d0cb609adc3 100644
--- a/drivers/net/ethernet/hisilicon/hns/hns_ae_adapt.c
+++ b/drivers/net/ethernet/hisilicon/hns/hns_ae_adapt.c
@@ -207,6 +207,7 @@ static int hns_ae_set_multicast_one(struct hnae_handle *handle, void *addr)
 	int ret;
 	char *mac_addr = (char *)addr;
 	struct hns_mac_cb *mac_cb = hns_get_mac_cb(handle);
+	u8 port_num;
 
 	assert(mac_cb);
 
@@ -221,8 +222,11 @@ static int hns_ae_set_multicast_one(struct hnae_handle *handle, void *addr)
 		return ret;
 	}
 
-	ret = hns_mac_set_multi(mac_cb, DSAF_BASE_INNER_PORT_NUM,
-				mac_addr, true);
+	ret = hns_mac_get_inner_port_num(mac_cb, handle->vf_id, &port_num);
+	if (ret)
+		return ret;
+
+	ret = hns_mac_set_multi(mac_cb, port_num, mac_addr, true);
 	if (ret)
 		dev_err(handle->owner_dev,
 			"mac add mul_mac:%pM port%d  fail, ret = %#x!\n",
@@ -678,9 +682,6 @@ static int hns_ae_config_loopback(struct hnae_handle *handle,
 		ret = -EINVAL;
 	}
 
-	if (!ret)
-		hns_dsaf_set_inner_lb(mac_cb->dsaf_dev, mac_cb->mac_id, en);
-
 	return ret;
 }
 
diff --git a/drivers/net/ethernet/hisilicon/hns/hns_dsaf_mac.c b/drivers/net/ethernet/hisilicon/hns/hns_dsaf_mac.c
index 5c8afe1a5ccb..ec8c738af726 100644
--- a/drivers/net/ethernet/hisilicon/hns/hns_dsaf_mac.c
+++ b/drivers/net/ethernet/hisilicon/hns/hns_dsaf_mac.c
@@ -126,7 +126,7 @@ void hns_mac_adjust_link(struct hns_mac_cb *mac_cb, int speed, int duplex)
 			(enum mac_speed)speed, duplex);
 		if (ret) {
 			dev_err(mac_cb->dev,
-				"adjust_link failed,%s mac%d ret = %#x!\n",
+				"adjust_link failed, %s mac%d ret = %#x!\n",
 				mac_cb->dsaf_dev->ae_dev.name,
 				mac_cb->mac_id, ret);
 			return;
@@ -141,15 +141,16 @@ void hns_mac_adjust_link(struct hns_mac_cb *mac_cb, int speed, int duplex)
  *@port_num:port number
  *
  */
-static int hns_mac_get_inner_port_num(struct hns_mac_cb *mac_cb,
-				      u8 vmid, u8 *port_num)
+int hns_mac_get_inner_port_num(struct hns_mac_cb *mac_cb, u8 vmid, u8 *port_num)
 {
+	int q_num_per_vf, vf_num_per_port;
+	int vm_queue_id;
 	u8 tmp_port;
 
 	if (mac_cb->dsaf_dev->dsaf_mode <= DSAF_MODE_ENABLE) {
 		if (mac_cb->mac_id != DSAF_MAX_PORT_NUM) {
 			dev_err(mac_cb->dev,
-				"input invalid,%s mac%d vmid%d !\n",
+				"input invalid, %s mac%d vmid%d !\n",
 				mac_cb->dsaf_dev->ae_dev.name,
 				mac_cb->mac_id, vmid);
 			return -EINVAL;
@@ -157,23 +158,29 @@ static int hns_mac_get_inner_port_num(struct hns_mac_cb *mac_cb,
 	} else if (mac_cb->dsaf_dev->dsaf_mode < DSAF_MODE_MAX) {
 		if (mac_cb->mac_id >= DSAF_MAX_PORT_NUM) {
 			dev_err(mac_cb->dev,
-				"input invalid,%s mac%d vmid%d!\n",
+				"input invalid, %s mac%d vmid%d!\n",
 				mac_cb->dsaf_dev->ae_dev.name,
 				mac_cb->mac_id, vmid);
 			return -EINVAL;
 		}
 	} else {
-		dev_err(mac_cb->dev, "dsaf mode invalid,%s mac%d!\n",
+		dev_err(mac_cb->dev, "dsaf mode invalid, %s mac%d!\n",
 			mac_cb->dsaf_dev->ae_dev.name, mac_cb->mac_id);
 		return -EINVAL;
 	}
 
 	if (vmid >= mac_cb->dsaf_dev->rcb_common[0]->max_vfn) {
-		dev_err(mac_cb->dev, "input invalid,%s mac%d vmid%d !\n",
+		dev_err(mac_cb->dev, "input invalid, %s mac%d vmid%d !\n",
 			mac_cb->dsaf_dev->ae_dev.name, mac_cb->mac_id, vmid);
 		return -EINVAL;
 	}
 
+	q_num_per_vf = mac_cb->dsaf_dev->rcb_common[0]->max_q_per_vf;
+	vf_num_per_port = mac_cb->dsaf_dev->rcb_common[0]->max_vfn;
+
+	vm_queue_id = vmid * q_num_per_vf +
+			vf_num_per_port * q_num_per_vf * mac_cb->mac_id;
+
 	switch (mac_cb->dsaf_dev->dsaf_mode) {
 	case DSAF_MODE_ENABLE_FIX:
 		tmp_port = 0;
@@ -193,10 +200,10 @@ static int hns_mac_get_inner_port_num(struct hns_mac_cb *mac_cb,
 	case DSAF_MODE_DISABLE_6PORT_2VM:
 	case DSAF_MODE_DISABLE_6PORT_4VM:
 	case DSAF_MODE_DISABLE_6PORT_16VM:
-		tmp_port = vmid;
+		tmp_port = vm_queue_id;
 		break;
 	default:
-		dev_err(mac_cb->dev, "dsaf mode invalid,%s mac%d!\n",
+		dev_err(mac_cb->dev, "dsaf mode invalid, %s mac%d!\n",
 			mac_cb->dsaf_dev->ae_dev.name, mac_cb->mac_id);
 		return -EINVAL;
 	}
@@ -275,7 +282,7 @@ int hns_mac_set_multi(struct hns_mac_cb *mac_cb,
 			ret = hns_dsaf_add_mac_mc_port(dsaf_dev, &mac_entry);
 		if (ret) {
 			dev_err(dsaf_dev->dev,
-				"set mac mc port failed,%s mac%d ret = %#x!\n",
+				"set mac mc port failed, %s mac%d ret = %#x!\n",
 				mac_cb->dsaf_dev->ae_dev.name,
 				mac_cb->mac_id, ret);
 			return ret;
@@ -305,7 +312,7 @@ int hns_mac_del_mac(struct hns_mac_cb *mac_cb, u32 vfn, char *mac)
 		old_mac = &mac_cb->addr_entry_idx[vfn];
 	} else {
 		dev_err(mac_cb->dev,
-			"vf queue is too large,%s mac%d queue = %#x!\n",
+			"vf queue is too large, %s mac%d queue = %#x!\n",
 			mac_cb->dsaf_dev->ae_dev.name, mac_cb->mac_id, vfn);
 		return -EINVAL;
 	}
@@ -547,7 +554,7 @@ int hns_mac_set_autoneg(struct hns_mac_cb *mac_cb, u8 enable)
 	struct mac_driver *mac_ctrl_drv = hns_mac_get_drv(mac_cb);
 
 	if (mac_cb->phy_if == PHY_INTERFACE_MODE_XGMII && enable) {
-		dev_err(mac_cb->dev, "enable autoneg is not allowed!");
+		dev_err(mac_cb->dev, "enabling autoneg is not allowed!\n");
 		return -ENOTSUPP;
 	}
 
@@ -571,7 +578,7 @@ int hns_mac_set_pauseparam(struct hns_mac_cb *mac_cb, u32 rx_en, u32 tx_en)
 
 	if (mac_cb->mac_type == HNAE_PORT_DEBUG) {
 		if (is_ver1 && (tx_en || rx_en)) {
-			dev_err(mac_cb->dev, "macv1 cann't enable tx/rx_pause!");
+			dev_err(mac_cb->dev, "macv1 can't enable tx/rx_pause!\n");
 			return -EINVAL;
 		}
 	}
@@ -644,21 +651,6 @@ hns_mac_phy_parse_addr(struct device *dev, struct fwnode_handle *fwnode)
 	return addr;
 }
 
-static int hns_mac_phydev_match(struct device *dev, void *fwnode)
-{
-	return dev->fwnode == fwnode;
-}
-
-static struct
-platform_device *hns_mac_find_platform_device(struct fwnode_handle *fwnode)
-{
-	struct device *dev;
-
-	dev = bus_find_device(&platform_bus_type, NULL,
-			      fwnode, hns_mac_phydev_match);
-	return dev ? to_platform_device(dev) : NULL;
-}
-
 static int
 hns_mac_register_phydev(struct mii_bus *mdio, struct hns_mac_cb *mac_cb,
 			u32 addr)
@@ -684,8 +676,7 @@ hns_mac_register_phydev(struct mii_bus *mdio, struct hns_mac_cb *mac_cb,
 	if (!phy || IS_ERR(phy))
 		return -EIO;
 
-	if (mdio->irq)
-		phy->irq = mdio->irq[addr];
+	phy->irq = mdio->irq[addr];
 
 	/* All data is now stored in the phy struct;
 	 * register it
@@ -725,7 +716,7 @@ static void hns_mac_register_phy(struct hns_mac_cb *mac_cb)
 		return;
 
 	/* dev address in adev */
-	pdev = hns_mac_find_platform_device(acpi_fwnode_handle(args.adev));
+	pdev = hns_dsaf_find_platform_device(acpi_fwnode_handle(args.adev));
 	mii_bus = platform_get_drvdata(pdev);
 	rc = hns_mac_register_phydev(mii_bus, mac_cb, addr);
 	if (!rc)
@@ -942,7 +933,7 @@ int hns_mac_get_cfg(struct dsaf_device *dsaf_dev, struct hns_mac_cb *mac_cb)
 	ret = hns_mac_get_mode(mac_cb->phy_if);
 	if (ret < 0) {
 		dev_err(dsaf_dev->dev,
-			"hns_mac_get_mode failed,mac%d ret = %#x!\n",
+			"hns_mac_get_mode failed, mac%d ret = %#x!\n",
 			mac_cb->mac_id, ret);
 		return ret;
 	}
diff --git a/drivers/net/ethernet/hisilicon/hns/hns_dsaf_mac.h b/drivers/net/ethernet/hisilicon/hns/hns_dsaf_mac.h
index 4cbdf14f5c16..d3a1f72ece0e 100644
--- a/drivers/net/ethernet/hisilicon/hns/hns_dsaf_mac.h
+++ b/drivers/net/ethernet/hisilicon/hns/hns_dsaf_mac.h
@@ -461,5 +461,7 @@ void hns_set_led_opt(struct hns_mac_cb *mac_cb);
 int hns_cpld_led_set_id(struct hns_mac_cb *mac_cb,
 			enum hnae_led_state status);
 void hns_mac_set_promisc(struct hns_mac_cb *mac_cb, u8 en);
+int hns_mac_get_inner_port_num(struct hns_mac_cb *mac_cb,
+			       u8 vmid, u8 *port_num);
 
 #endif /* _HNS_DSAF_MAC_H */
diff --git a/drivers/net/ethernet/hisilicon/hns/hns_dsaf_main.c b/drivers/net/ethernet/hisilicon/hns/hns_dsaf_main.c
index afb5daa3721d..8d70377f6624 100644
--- a/drivers/net/ethernet/hisilicon/hns/hns_dsaf_main.c
+++ b/drivers/net/ethernet/hisilicon/hns/hns_dsaf_main.c
@@ -18,6 +18,7 @@
 #include <linux/of.h>
 #include <linux/of_address.h>
 #include <linux/of_irq.h>
+#include <linux/of_platform.h>
 #include <linux/platform_device.h>
 #include <linux/vmalloc.h>
 
@@ -115,10 +116,8 @@ int hns_dsaf_get_cfg(struct dsaf_device *dsaf_dev)
 
 			dsaf_dev->sc_base = devm_ioremap_resource(&pdev->dev,
 								  res);
-			if (IS_ERR(dsaf_dev->sc_base)) {
-				dev_err(dsaf_dev->dev, "subctrl can not map!\n");
+			if (IS_ERR(dsaf_dev->sc_base))
 				return PTR_ERR(dsaf_dev->sc_base);
-			}
 
 			res = platform_get_resource(pdev, IORESOURCE_MEM,
 						    res_idx++);
@@ -129,10 +128,8 @@ int hns_dsaf_get_cfg(struct dsaf_device *dsaf_dev)
 
 			dsaf_dev->sds_base = devm_ioremap_resource(&pdev->dev,
 								   res);
-			if (IS_ERR(dsaf_dev->sds_base)) {
-				dev_err(dsaf_dev->dev, "serdes-ctrl can not map!\n");
+			if (IS_ERR(dsaf_dev->sds_base))
 				return PTR_ERR(dsaf_dev->sds_base);
-			}
 		} else {
 			dsaf_dev->sub_ctrl = syscon;
 		}
@@ -147,10 +144,8 @@ int hns_dsaf_get_cfg(struct dsaf_device *dsaf_dev)
 		}
 	}
 	dsaf_dev->ppe_base = devm_ioremap_resource(&pdev->dev, res);
-	if (IS_ERR(dsaf_dev->ppe_base)) {
-		dev_err(dsaf_dev->dev, "ppe-base resource can not map!\n");
+	if (IS_ERR(dsaf_dev->ppe_base))
 		return PTR_ERR(dsaf_dev->ppe_base);
-	}
 	dsaf_dev->ppe_paddr = res->start;
 
 	if (!HNS_DSAF_IS_DEBUG(dsaf_dev)) {
@@ -166,10 +161,8 @@ int hns_dsaf_get_cfg(struct dsaf_device *dsaf_dev)
 			}
 		}
 		dsaf_dev->io_base = devm_ioremap_resource(&pdev->dev, res);
-		if (IS_ERR(dsaf_dev->io_base)) {
-			dev_err(dsaf_dev->dev, "dsaf-base resource can not map!\n");
+		if (IS_ERR(dsaf_dev->io_base))
 			return PTR_ERR(dsaf_dev->io_base);
-		}
 	}
 
 	ret = device_property_read_u32(dsaf_dev->dev, "desc-num", &desc_num);
@@ -767,16 +760,6 @@ void hns_dsaf_set_promisc_mode(struct dsaf_device *dsaf_dev, u32 en)
 				 DSAF_CFG_MIX_MODE_S, !!en);
 }
 
-void hns_dsaf_set_inner_lb(struct dsaf_device *dsaf_dev, u32 mac_id, u32 en)
-{
-	if (AE_IS_VER1(dsaf_dev->dsaf_ver) ||
-	    dsaf_dev->mac_cb[mac_id]->mac_type == HNAE_PORT_DEBUG)
-		return;
-
-	dsaf_set_dev_bit(dsaf_dev, DSAFV2_SERDES_LBK_0_REG + 4 * mac_id,
-			 DSAFV2_SERDES_LBK_EN_B, !!en);
-}
-
 /**
  * hns_dsaf_tbl_stat_en - tbl
  * @dsaf_id: dsa fabric id
@@ -2781,6 +2764,110 @@ static struct platform_driver g_dsaf_driver = {
 
 module_platform_driver(g_dsaf_driver);
 
+/**
+ * hns_dsaf_roce_reset - reset dsaf and roce
+ * @dsaf_fwnode: Pointer to framework node for the dasf
+ * @enable: false - request reset , true - drop reset
+ * retuen 0 - success , negative -fail
+ */
+int hns_dsaf_roce_reset(struct fwnode_handle *dsaf_fwnode, bool dereset)
+{
+	struct dsaf_device *dsaf_dev;
+	struct platform_device *pdev;
+	u32 mp;
+	u32 sl;
+	u32 credit;
+	int i;
+	const u32 port_map[DSAF_ROCE_CREDIT_CHN][DSAF_ROCE_CHAN_MODE_NUM] = {
+		{DSAF_ROCE_PORT_0, DSAF_ROCE_PORT_0, DSAF_ROCE_PORT_0},
+		{DSAF_ROCE_PORT_1, DSAF_ROCE_PORT_0, DSAF_ROCE_PORT_0},
+		{DSAF_ROCE_PORT_2, DSAF_ROCE_PORT_1, DSAF_ROCE_PORT_0},
+		{DSAF_ROCE_PORT_3, DSAF_ROCE_PORT_1, DSAF_ROCE_PORT_0},
+		{DSAF_ROCE_PORT_4, DSAF_ROCE_PORT_2, DSAF_ROCE_PORT_1},
+		{DSAF_ROCE_PORT_4, DSAF_ROCE_PORT_2, DSAF_ROCE_PORT_1},
+		{DSAF_ROCE_PORT_5, DSAF_ROCE_PORT_3, DSAF_ROCE_PORT_1},
+		{DSAF_ROCE_PORT_5, DSAF_ROCE_PORT_3, DSAF_ROCE_PORT_1},
+	};
+	const u32 sl_map[DSAF_ROCE_CREDIT_CHN][DSAF_ROCE_CHAN_MODE_NUM] = {
+		{DSAF_ROCE_SL_0, DSAF_ROCE_SL_0, DSAF_ROCE_SL_0},
+		{DSAF_ROCE_SL_0, DSAF_ROCE_SL_1, DSAF_ROCE_SL_1},
+		{DSAF_ROCE_SL_0, DSAF_ROCE_SL_0, DSAF_ROCE_SL_2},
+		{DSAF_ROCE_SL_0, DSAF_ROCE_SL_1, DSAF_ROCE_SL_3},
+		{DSAF_ROCE_SL_0, DSAF_ROCE_SL_0, DSAF_ROCE_SL_0},
+		{DSAF_ROCE_SL_1, DSAF_ROCE_SL_1, DSAF_ROCE_SL_1},
+		{DSAF_ROCE_SL_0, DSAF_ROCE_SL_0, DSAF_ROCE_SL_2},
+		{DSAF_ROCE_SL_1, DSAF_ROCE_SL_1, DSAF_ROCE_SL_3},
+	};
+
+	/* find the platform device corresponding to fwnode */
+	if (is_of_node(dsaf_fwnode)) {
+		pdev = of_find_device_by_node(to_of_node(dsaf_fwnode));
+	} else if (is_acpi_device_node(dsaf_fwnode)) {
+		pdev = hns_dsaf_find_platform_device(dsaf_fwnode);
+	} else {
+		pr_err("fwnode is neither OF or ACPI type\n");
+		return -EINVAL;
+	}
+
+	/* check if we were a success in fetching pdev */
+	if (!pdev) {
+		pr_err("couldn't find platform device for node\n");
+		return -ENODEV;
+	}
+
+	/* retrieve the dsaf_device from the driver data */
+	dsaf_dev = dev_get_drvdata(&pdev->dev);
+	if (!dsaf_dev) {
+		dev_err(&pdev->dev, "dsaf_dev is NULL\n");
+		return -ENODEV;
+	}
+
+	/* now, make sure we are running on compatible SoC */
+	if (AE_IS_VER1(dsaf_dev->dsaf_ver)) {
+		dev_err(dsaf_dev->dev, "%s v1 chip doesn't support RoCE!\n",
+			dsaf_dev->ae_dev.name);
+		return -ENODEV;
+	}
+
+	/* do reset or de-reset according to the flag */
+	if (!dereset) {
+		/* reset rocee-channels in dsaf and rocee */
+		dsaf_dev->misc_op->hns_dsaf_srst_chns(dsaf_dev, DSAF_CHNS_MASK,
+						      false);
+		dsaf_dev->misc_op->hns_dsaf_roce_srst(dsaf_dev, false);
+	} else {
+		/* configure dsaf tx roce correspond to port map and sl map */
+		mp = dsaf_read_dev(dsaf_dev, DSAF_ROCE_PORT_MAP_REG);
+		for (i = 0; i < DSAF_ROCE_CREDIT_CHN; i++)
+			dsaf_set_field(mp, 7 << i * 3, i * 3,
+				       port_map[i][DSAF_ROCE_6PORT_MODE]);
+		dsaf_set_field(mp, 3 << i * 3, i * 3, 0);
+		dsaf_write_dev(dsaf_dev, DSAF_ROCE_PORT_MAP_REG, mp);
+
+		sl = dsaf_read_dev(dsaf_dev, DSAF_ROCE_SL_MAP_REG);
+		for (i = 0; i < DSAF_ROCE_CREDIT_CHN; i++)
+			dsaf_set_field(sl, 3 << i * 2, i * 2,
+				       sl_map[i][DSAF_ROCE_6PORT_MODE]);
+		dsaf_write_dev(dsaf_dev, DSAF_ROCE_SL_MAP_REG, sl);
+
+		/* de-reset rocee-channels in dsaf and rocee */
+		dsaf_dev->misc_op->hns_dsaf_srst_chns(dsaf_dev, DSAF_CHNS_MASK,
+						      true);
+		msleep(SRST_TIME_INTERVAL);
+		dsaf_dev->misc_op->hns_dsaf_roce_srst(dsaf_dev, true);
+
+		/* enable dsaf channel rocee credit */
+		credit = dsaf_read_dev(dsaf_dev, DSAF_SBM_ROCEE_CFG_REG_REG);
+		dsaf_set_bit(credit, DSAF_SBM_ROCEE_CFG_CRD_EN_B, 0);
+		dsaf_write_dev(dsaf_dev, DSAF_SBM_ROCEE_CFG_REG_REG, credit);
+
+		dsaf_set_bit(credit, DSAF_SBM_ROCEE_CFG_CRD_EN_B, 1);
+		dsaf_write_dev(dsaf_dev, DSAF_SBM_ROCEE_CFG_REG_REG, credit);
+	}
+	return 0;
+}
+EXPORT_SYMBOL(hns_dsaf_roce_reset);
+
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Huawei Tech. Co., Ltd.");
 MODULE_DESCRIPTION("HNS DSAF driver");
diff --git a/drivers/net/ethernet/hisilicon/hns/hns_dsaf_main.h b/drivers/net/ethernet/hisilicon/hns/hns_dsaf_main.h
index 1daf018d9071..c494fc52be74 100644
--- a/drivers/net/ethernet/hisilicon/hns/hns_dsaf_main.h
+++ b/drivers/net/ethernet/hisilicon/hns/hns_dsaf_main.h
@@ -43,6 +43,32 @@ struct hns_mac_cb;
 #define DSAF_PRIO_NR	8
 #define DSAF_REG_PER_ZONE	3
 
+#define DSAF_ROCE_CREDIT_CHN	8
+#define DSAF_ROCE_CHAN_MODE	3
+
+enum dsaf_roce_port_mode {
+	DSAF_ROCE_6PORT_MODE,
+	DSAF_ROCE_4PORT_MODE,
+	DSAF_ROCE_2PORT_MODE,
+	DSAF_ROCE_CHAN_MODE_NUM,
+};
+
+enum dsaf_roce_port_num {
+	DSAF_ROCE_PORT_0,
+	DSAF_ROCE_PORT_1,
+	DSAF_ROCE_PORT_2,
+	DSAF_ROCE_PORT_3,
+	DSAF_ROCE_PORT_4,
+	DSAF_ROCE_PORT_5,
+};
+
+enum dsaf_roce_qos_sl {
+	DSAF_ROCE_SL_0,
+	DSAF_ROCE_SL_1,
+	DSAF_ROCE_SL_2,
+	DSAF_ROCE_SL_3,
+};
+
 #define DSAF_STATS_READ(p, offset) (*((u64 *)((u8 *)(p) + (offset))))
 #define HNS_DSAF_IS_DEBUG(dev) (dev->dsaf_mode == DSAF_MODE_DISABLE_SP)
 
@@ -279,7 +305,7 @@ struct dsaf_misc_op {
 	void (*cpld_reset_led)(struct hns_mac_cb *mac_cb);
 	int (*cpld_set_led_id)(struct hns_mac_cb *mac_cb,
 			       enum hnae_led_state status);
-	/* reset seris function, it will be reset if the dereseet is 0 */
+	/* reset series function, it will be reset if the dereset is 0 */
 	void (*dsaf_reset)(struct dsaf_device *dsaf_dev, bool dereset);
 	void (*xge_srst)(struct dsaf_device *dsaf_dev, u32 port, bool dereset);
 	void (*xge_core_srst)(struct dsaf_device *dsaf_dev, u32 port,
@@ -287,6 +313,9 @@ struct dsaf_misc_op {
 	void (*ge_srst)(struct dsaf_device *dsaf_dev, u32 port, bool dereset);
 	void (*ppe_srst)(struct dsaf_device *dsaf_dev, u32 port, bool dereset);
 	void (*ppe_comm_srst)(struct dsaf_device *dsaf_dev, bool dereset);
+	void (*hns_dsaf_srst_chns)(struct dsaf_device *dsaf_dev, u32 msk,
+				   bool dereset);
+	void (*hns_dsaf_roce_srst)(struct dsaf_device *dsaf_dev, bool dereset);
 
 	phy_interface_t (*get_phy_if)(struct hns_mac_cb *mac_cb);
 	int (*get_sfp_prsnt)(struct hns_mac_cb *mac_cb, int *sfp_prsnt);
@@ -437,6 +466,5 @@ void hns_dsaf_get_rx_mac_pause_en(struct dsaf_device *dsaf_dev, int mac_id,
 				  u32 *en);
 int hns_dsaf_set_rx_mac_pause_en(struct dsaf_device *dsaf_dev, int mac_id,
 				 u32 en);
-void hns_dsaf_set_inner_lb(struct dsaf_device *dsaf_dev, u32 mac_id, u32 en);
 
 #endif /* __HNS_DSAF_MAIN_H__ */
diff --git a/drivers/net/ethernet/hisilicon/hns/hns_dsaf_misc.c b/drivers/net/ethernet/hisilicon/hns/hns_dsaf_misc.c
index 611b67b6f450..67accce1d33d 100644
--- a/drivers/net/ethernet/hisilicon/hns/hns_dsaf_misc.c
+++ b/drivers/net/ethernet/hisilicon/hns/hns_dsaf_misc.c
@@ -26,6 +26,8 @@ enum _dsm_rst_type {
 	HNS_XGE_CORE_RESET_FUNC = 0x3,
 	HNS_XGE_RESET_FUNC      = 0x4,
 	HNS_GE_RESET_FUNC       = 0x5,
+	HNS_DSAF_CHN_RESET_FUNC = 0x6,
+	HNS_ROCE_RESET_FUNC     = 0x7,
 };
 
 const u8 hns_dsaf_acpi_dsm_uuid[] = {
@@ -231,6 +233,66 @@ static void hns_dsaf_xge_core_srst_by_port(struct dsaf_device *dsaf_dev,
 	dsaf_write_sub(dsaf_dev, reg_addr, reg_val);
 }
 
+/**
+ * hns_dsaf_srst_chns - reset dsaf channels
+ * @dsaf_dev: dsaf device struct pointer
+ * @msk: xbar channels mask value:
+ * bit0-5 for xge0-5
+ * bit6-11 for ppe0-5
+ * bit12-17 for roce0-5
+ * bit18-19 for com/dfx
+ * @enable: false - request reset , true - drop reset
+ */
+void hns_dsaf_srst_chns(struct dsaf_device *dsaf_dev, u32 msk, bool dereset)
+{
+	u32 reg_addr;
+
+	if (!dereset)
+		reg_addr = DSAF_SUB_SC_DSAF_RESET_REQ_REG;
+	else
+		reg_addr = DSAF_SUB_SC_DSAF_RESET_DREQ_REG;
+
+	dsaf_write_sub(dsaf_dev, reg_addr, msk);
+}
+
+/**
+ * hns_dsaf_srst_chns - reset dsaf channels
+ * @dsaf_dev: dsaf device struct pointer
+ * @msk: xbar channels mask value:
+ * bit0-5 for xge0-5
+ * bit6-11 for ppe0-5
+ * bit12-17 for roce0-5
+ * bit18-19 for com/dfx
+ * @enable: false - request reset , true - drop reset
+ */
+void
+hns_dsaf_srst_chns_acpi(struct dsaf_device *dsaf_dev, u32 msk, bool dereset)
+{
+	hns_dsaf_acpi_srst_by_port(dsaf_dev, HNS_OP_RESET_FUNC,
+				   HNS_DSAF_CHN_RESET_FUNC,
+				   msk, dereset);
+}
+
+void hns_dsaf_roce_srst(struct dsaf_device *dsaf_dev, bool dereset)
+{
+	if (!dereset) {
+		dsaf_write_sub(dsaf_dev, DSAF_SUB_SC_ROCEE_RESET_REQ_REG, 1);
+	} else {
+		dsaf_write_sub(dsaf_dev,
+			       DSAF_SUB_SC_ROCEE_CLK_DIS_REG, 1);
+		dsaf_write_sub(dsaf_dev,
+			       DSAF_SUB_SC_ROCEE_RESET_DREQ_REG, 1);
+		msleep(20);
+		dsaf_write_sub(dsaf_dev, DSAF_SUB_SC_ROCEE_CLK_EN_REG, 1);
+	}
+}
+
+void hns_dsaf_roce_srst_acpi(struct dsaf_device *dsaf_dev, bool dereset)
+{
+	hns_dsaf_acpi_srst_by_port(dsaf_dev, HNS_OP_RESET_FUNC,
+				   HNS_ROCE_RESET_FUNC, 0, dereset);
+}
+
 static void
 hns_dsaf_xge_core_srst_by_port_acpi(struct dsaf_device *dsaf_dev,
 				    u32 port, bool dereset)
@@ -539,6 +601,8 @@ struct dsaf_misc_op *hns_misc_op_get(struct dsaf_device *dsaf_dev)
 		misc_op->ge_srst = hns_dsaf_ge_srst_by_port;
 		misc_op->ppe_srst = hns_ppe_srst_by_port;
 		misc_op->ppe_comm_srst = hns_ppe_com_srst;
+		misc_op->hns_dsaf_srst_chns = hns_dsaf_srst_chns;
+		misc_op->hns_dsaf_roce_srst = hns_dsaf_roce_srst;
 
 		misc_op->get_phy_if = hns_mac_get_phy_if;
 		misc_op->get_sfp_prsnt = hns_mac_get_sfp_prsnt;
@@ -555,6 +619,8 @@ struct dsaf_misc_op *hns_misc_op_get(struct dsaf_device *dsaf_dev)
 		misc_op->ge_srst = hns_dsaf_ge_srst_by_port_acpi;
 		misc_op->ppe_srst = hns_ppe_srst_by_port_acpi;
 		misc_op->ppe_comm_srst = hns_ppe_com_srst;
+		misc_op->hns_dsaf_srst_chns = hns_dsaf_srst_chns_acpi;
+		misc_op->hns_dsaf_roce_srst = hns_dsaf_roce_srst_acpi;
 
 		misc_op->get_phy_if = hns_mac_get_phy_if_acpi;
 		misc_op->get_sfp_prsnt = hns_mac_get_sfp_prsnt;
@@ -567,3 +633,18 @@ struct dsaf_misc_op *hns_misc_op_get(struct dsaf_device *dsaf_dev)
 
 	return (void *)misc_op;
 }
+
+static int hns_dsaf_dev_match(struct device *dev, void *fwnode)
+{
+	return dev->fwnode == fwnode;
+}
+
+struct
+platform_device *hns_dsaf_find_platform_device(struct fwnode_handle *fwnode)
+{
+	struct device *dev;
+
+	dev = bus_find_device(&platform_bus_type, NULL,
+			      fwnode, hns_dsaf_dev_match);
+	return dev ? to_platform_device(dev) : NULL;
+}
diff --git a/drivers/net/ethernet/hisilicon/hns/hns_dsaf_misc.h b/drivers/net/ethernet/hisilicon/hns/hns_dsaf_misc.h
index f06bb03d47a6..310e80261366 100644
--- a/drivers/net/ethernet/hisilicon/hns/hns_dsaf_misc.h
+++ b/drivers/net/ethernet/hisilicon/hns/hns_dsaf_misc.h
@@ -34,5 +34,6 @@
 #define DSAF_LED_ANCHOR_B 5
 
 struct dsaf_misc_op *hns_misc_op_get(struct dsaf_device *dsaf_dev);
-
+struct
+platform_device *hns_dsaf_find_platform_device(struct fwnode_handle *fwnode);
 #endif
diff --git a/drivers/net/ethernet/hisilicon/hns/hns_dsaf_rcb.c b/drivers/net/ethernet/hisilicon/hns/hns_dsaf_rcb.c
index ef1107777c08..f0ed80d6ef9c 100644
--- a/drivers/net/ethernet/hisilicon/hns/hns_dsaf_rcb.c
+++ b/drivers/net/ethernet/hisilicon/hns/hns_dsaf_rcb.c
@@ -543,6 +543,22 @@ int hns_rcb_set_coalesce_usecs(
 			"error: coalesce_usecs setting supports 0~1023us\n");
 		return -EINVAL;
 	}
+
+	if (!AE_IS_VER1(rcb_common->dsaf_dev->dsaf_ver)) {
+		if (timeout == 0)
+			/* set timeout to 0, Disable gap time */
+			dsaf_set_reg_field(rcb_common->io_base,
+					   RCB_INT_GAP_TIME_REG + port_idx * 4,
+					   PPE_INT_GAPTIME_M, PPE_INT_GAPTIME_B,
+					   0);
+		else
+			/* set timeout non 0, restore gap time to 1 */
+			dsaf_set_reg_field(rcb_common->io_base,
+					   RCB_INT_GAP_TIME_REG + port_idx * 4,
+					   PPE_INT_GAPTIME_M, PPE_INT_GAPTIME_B,
+					   1);
+	}
+
 	hns_rcb_set_port_timeout(rcb_common, port_idx, timeout);
 	return 0;
 }
diff --git a/drivers/net/ethernet/hisilicon/hns/hns_dsaf_reg.h b/drivers/net/ethernet/hisilicon/hns/hns_dsaf_reg.h
index 235f74444b1d..878950a42e6c 100644
--- a/drivers/net/ethernet/hisilicon/hns/hns_dsaf_reg.h
+++ b/drivers/net/ethernet/hisilicon/hns/hns_dsaf_reg.h
@@ -77,6 +77,12 @@
 #define DSAF_SUB_SC_PPE_RESET_DREQ_REG			0xA4C
 #define DSAF_SUB_SC_RCB_PPE_COM_RESET_REQ_REG		0xA88
 #define DSAF_SUB_SC_RCB_PPE_COM_RESET_DREQ_REG		0xA8C
+#define DSAF_SUB_SC_DSAF_RESET_REQ_REG			0xAA8
+#define DSAF_SUB_SC_DSAF_RESET_DREQ_REG			0xAAC
+#define DSAF_SUB_SC_ROCEE_RESET_REQ_REG			0xA50
+#define DSAF_SUB_SC_ROCEE_RESET_DREQ_REG		0xA54
+#define DSAF_SUB_SC_ROCEE_CLK_DIS_REG			0x32C
+#define DSAF_SUB_SC_ROCEE_CLK_EN_REG			0x328
 #define DSAF_SUB_SC_LIGHT_MODULE_DETECT_EN_REG		0x2060
 #define DSAF_SUB_SC_TCAM_MBIST_EN_REG			0x2300
 #define DSAF_SUB_SC_DSAF_CLK_ST_REG			0x5300
@@ -133,6 +139,8 @@
 #define DSAF_ROCEE_INT_STS_0_REG	0x200
 #define DSAFV2_SERDES_LBK_0_REG         0x220
 #define DSAF_PAUSE_CFG_REG		0x240
+#define DSAF_ROCE_PORT_MAP_REG		0x2A0
+#define DSAF_ROCE_SL_MAP_REG		0x2A4
 #define DSAF_PPE_QID_CFG_0_REG		0x300
 #define DSAF_SW_PORT_TYPE_0_REG		0x320
 #define DSAF_STP_PORT_TYPE_0_REG	0x340
@@ -178,6 +186,7 @@
 #define DSAF_SBM_BP_CFG_2_XGE_REG_0_REG		0x200C
 #define DSAF_SBM_BP_CFG_2_PPE_REG_0_REG		0x230C
 #define DSAF_SBM_BP_CFG_2_ROCEE_REG_0_REG	0x260C
+#define DSAF_SBM_ROCEE_CFG_REG_REG		0x2380
 #define DSAFV2_SBM_BP_CFG_2_ROCEE_REG_0_REG	0x238C
 #define DSAF_SBM_FREE_CNT_0_0_REG		0x2010
 #define DSAF_SBM_FREE_CNT_1_0_REG		0x2014
@@ -408,6 +417,7 @@
 #define RCB_CFG_OVERTIME_REG			0x9300
 #define RCB_CFG_PKTLINE_INT_NUM_REG		0x9304
 #define RCB_CFG_OVERTIME_INT_NUM_REG		0x9308
+#define RCB_INT_GAP_TIME_REG			0x9400
 #define RCB_PORT_CFG_OVERTIME_REG		0x9430
 
 #define RCB_RING_RX_RING_BASEADDR_L_REG		0x00000
@@ -796,6 +806,9 @@
 #define DSAFV2_SBM_CFG4_RESET_BUF_NUM_NO_PFC_S 9
 #define DSAFV2_SBM_CFG4_RESET_BUF_NUM_NO_PFC_M (((1ULL << 9) - 1) << 9)
 
+#define DSAF_CHNS_MASK			0x3f000
+#define DSAF_SBM_ROCEE_CFG_CRD_EN_B	2
+#define SRST_TIME_INTERVAL		20
 #define DSAFV2_SBM_CFG2_ROCEE_SET_BUF_NUM_S 0
 #define DSAFV2_SBM_CFG2_ROCEE_SET_BUF_NUM_M (((1ULL << 8) - 1) << 0)
 #define DSAFV2_SBM_CFG2_ROCEE_RESET_BUF_NUM_S 8
@@ -886,6 +899,9 @@
 #define PPE_CNT_CLR_CE_B	0
 #define PPE_CNT_CLR_SNAP_EN_B	1
 
+#define PPE_INT_GAPTIME_B	0
+#define PPE_INT_GAPTIME_M	0x3ff
+
 #define PPE_COMMON_CNT_CLR_CE_B	0
 #define PPE_COMMON_CNT_CLR_SNAP_EN_B	1
 #define RCB_COM_TSO_MODE_B	0
diff --git a/drivers/net/ethernet/hisilicon/hns/hns_enet.c b/drivers/net/ethernet/hisilicon/hns/hns_enet.c
index d7e1f8c7ae92..dff7b60345d8 100644
--- a/drivers/net/ethernet/hisilicon/hns/hns_enet.c
+++ b/drivers/net/ethernet/hisilicon/hns/hns_enet.c
@@ -574,7 +574,6 @@ static int hns_nic_poll_rx_skb(struct hns_nic_ring_data *ring_data,
 	struct sk_buff *skb;
 	struct hnae_desc *desc;
 	struct hnae_desc_cb *desc_cb;
-	struct ethhdr *eh;
 	unsigned char *va;
 	int bnum, length, i;
 	int pull_len;
@@ -600,7 +599,6 @@ static int hns_nic_poll_rx_skb(struct hns_nic_ring_data *ring_data,
 		ring->stats.sw_err_cnt++;
 		return -ENOMEM;
 	}
-	skb_reset_mac_header(skb);
 
 	prefetchw(skb->data);
 	length = le16_to_cpu(desc->rx.pkt_len);
@@ -682,14 +680,6 @@ out_bnum_err:
 		return -EFAULT;
 	}
 
-	/* filter out multicast pkt with the same src mac as this port */
-	eh = eth_hdr(skb);
-	if (unlikely(is_multicast_ether_addr(eh->h_dest) &&
-		     ether_addr_equal(ndev->dev_addr, eh->h_source))) {
-		dev_kfree_skb_any(skb);
-		return -EFAULT;
-	}
-
 	ring->stats.rx_pkts++;
 	ring->stats.rx_bytes += skb->len;
 
@@ -747,25 +737,37 @@ static void hns_nic_rx_up_pro(struct hns_nic_ring_data *ring_data,
 	ndev->last_rx = jiffies;
 }
 
+static int hns_desc_unused(struct hnae_ring *ring)
+{
+	int ntc = ring->next_to_clean;
+	int ntu = ring->next_to_use;
+
+	return ((ntc >= ntu) ? 0 : ring->desc_num) + ntc - ntu;
+}
+
 static int hns_nic_rx_poll_one(struct hns_nic_ring_data *ring_data,
 			       int budget, void *v)
 {
 	struct hnae_ring *ring = ring_data->ring;
 	struct sk_buff *skb;
-	int num, bnum, ex_num;
+	int num, bnum;
 #define RCB_NOF_ALLOC_RX_BUFF_ONCE 16
 	int recv_pkts, recv_bds, clean_count, err;
+	int unused_count = hns_desc_unused(ring);
 
 	num = readl_relaxed(ring->io_base + RCB_REG_FBDNUM);
 	rmb(); /* make sure num taken effect before the other data is touched */
 
 	recv_pkts = 0, recv_bds = 0, clean_count = 0;
-recv:
+	num -= unused_count;
+
 	while (recv_pkts < budget && recv_bds < num) {
 		/* reuse or realloc buffers */
-		if (clean_count >= RCB_NOF_ALLOC_RX_BUFF_ONCE) {
-			hns_nic_alloc_rx_buffers(ring_data, clean_count);
+		if (clean_count + unused_count >= RCB_NOF_ALLOC_RX_BUFF_ONCE) {
+			hns_nic_alloc_rx_buffers(ring_data,
+						 clean_count + unused_count);
 			clean_count = 0;
+			unused_count = hns_desc_unused(ring);
 		}
 
 		/* poll one pkt */
@@ -786,21 +788,11 @@ recv:
 		recv_pkts++;
 	}
 
-	/* make all data has been write before submit */
-	if (recv_pkts < budget) {
-		ex_num = readl_relaxed(ring->io_base + RCB_REG_FBDNUM);
-
-		if (ex_num > clean_count) {
-			num += ex_num - clean_count;
-			rmb(); /*complete read rx ring bd number*/
-			goto recv;
-		}
-	}
-
 out:
 	/* make all data has been write before submit */
-	if (clean_count > 0)
-		hns_nic_alloc_rx_buffers(ring_data, clean_count);
+	if (clean_count + unused_count > 0)
+		hns_nic_alloc_rx_buffers(ring_data,
+					 clean_count + unused_count);
 
 	return recv_pkts;
 }
@@ -810,6 +802,8 @@ static void hns_nic_rx_fini_pro(struct hns_nic_ring_data *ring_data)
 	struct hnae_ring *ring = ring_data->ring;
 	int num = 0;
 
+	ring_data->ring->q->handle->dev->ops->toggle_ring_irq(ring, 0);
+
 	/* for hardware bug fixed */
 	num = readl_relaxed(ring->io_base + RCB_REG_FBDNUM);
 
@@ -821,6 +815,20 @@ static void hns_nic_rx_fini_pro(struct hns_nic_ring_data *ring_data)
 	}
 }
 
+static void hns_nic_rx_fini_pro_v2(struct hns_nic_ring_data *ring_data)
+{
+	struct hnae_ring *ring = ring_data->ring;
+	int num = 0;
+
+	num = readl_relaxed(ring->io_base + RCB_REG_FBDNUM);
+
+	if (num == 0)
+		ring_data->ring->q->handle->dev->ops->toggle_ring_irq(
+			ring, 0);
+	else
+		napi_schedule(&ring_data->napi);
+}
+
 static inline void hns_nic_reclaim_one_desc(struct hnae_ring *ring,
 					    int *bytes, int *pkts)
 {
@@ -922,7 +930,11 @@ static int hns_nic_tx_poll_one(struct hns_nic_ring_data *ring_data,
 static void hns_nic_tx_fini_pro(struct hns_nic_ring_data *ring_data)
 {
 	struct hnae_ring *ring = ring_data->ring;
-	int head = readl_relaxed(ring->io_base + RCB_REG_HEAD);
+	int head;
+
+	ring_data->ring->q->handle->dev->ops->toggle_ring_irq(ring, 0);
+
+	head = readl_relaxed(ring->io_base + RCB_REG_HEAD);
 
 	if (head != ring->next_to_clean) {
 		ring_data->ring->q->handle->dev->ops->toggle_ring_irq(
@@ -932,6 +944,18 @@ static void hns_nic_tx_fini_pro(struct hns_nic_ring_data *ring_data)
 	}
 }
 
+static void hns_nic_tx_fini_pro_v2(struct hns_nic_ring_data *ring_data)
+{
+	struct hnae_ring *ring = ring_data->ring;
+	int head = readl_relaxed(ring->io_base + RCB_REG_HEAD);
+
+	if (head == ring->next_to_clean)
+		ring_data->ring->q->handle->dev->ops->toggle_ring_irq(
+			ring, 0);
+	else
+		napi_schedule(&ring_data->napi);
+}
+
 static void hns_nic_tx_clr_all_bufs(struct hns_nic_ring_data *ring_data)
 {
 	struct hnae_ring *ring = ring_data->ring;
@@ -963,10 +987,7 @@ static int hns_nic_common_poll(struct napi_struct *napi, int budget)
 
 	if (clean_complete >= 0 && clean_complete < budget) {
 		napi_complete(napi);
-		ring_data->ring->q->handle->dev->ops->toggle_ring_irq(
-			ring_data->ring, 0);
-		if (ring_data->fini_process)
-			ring_data->fini_process(ring_data);
+		ring_data->fini_process(ring_data);
 		return 0;
 	}
 
@@ -994,10 +1015,10 @@ static void hns_nic_adjust_link(struct net_device *ndev)
 	struct hnae_handle *h = priv->ae_handle;
 	int state = 1;
 
-	if (priv->phy) {
+	if (ndev->phydev) {
 		h->dev->ops->adjust_link(h, ndev->phydev->speed,
 					 ndev->phydev->duplex);
-		state = priv->phy->link;
+		state = ndev->phydev->link;
 	}
 	state = state && h->dev->ops->get_status(h);
 
@@ -1022,7 +1043,6 @@ static void hns_nic_adjust_link(struct net_device *ndev)
  */
 int hns_nic_init_phy(struct net_device *ndev, struct hnae_handle *h)
 {
-	struct hns_nic_priv *priv = netdev_priv(ndev);
 	struct phy_device *phy_dev = h->phy_dev;
 	int ret;
 
@@ -1046,8 +1066,6 @@ int hns_nic_init_phy(struct net_device *ndev, struct hnae_handle *h)
 	if (h->phy_if == PHY_INTERFACE_MODE_XGMII)
 		phy_dev->autoneg = false;
 
-	priv->phy = phy_dev;
-
 	return 0;
 }
 
@@ -1224,8 +1242,8 @@ static int hns_nic_net_up(struct net_device *ndev)
 	if (ret)
 		goto out_start_err;
 
-	if (priv->phy)
-		phy_start(priv->phy);
+	if (ndev->phydev)
+		phy_start(ndev->phydev);
 
 	clear_bit(NIC_STATE_DOWN, &priv->state);
 	(void)mod_timer(&priv->service_timer, jiffies + SERVICE_TIMER_HZ);
@@ -1259,8 +1277,8 @@ static void hns_nic_net_down(struct net_device *ndev)
 	netif_tx_disable(ndev);
 	priv->link = 0;
 
-	if (priv->phy)
-		phy_stop(priv->phy);
+	if (ndev->phydev)
+		phy_stop(ndev->phydev);
 
 	ops = priv->ae_handle->dev->ops;
 
@@ -1359,8 +1377,7 @@ static void hns_nic_net_timeout(struct net_device *ndev)
 static int hns_nic_do_ioctl(struct net_device *netdev, struct ifreq *ifr,
 			    int cmd)
 {
-	struct hns_nic_priv *priv = netdev_priv(netdev);
-	struct phy_device *phy_dev = priv->phy;
+	struct phy_device *phy_dev = netdev->phydev;
 
 	if (!netif_running(netdev))
 		return -EINVAL;
@@ -1566,6 +1583,21 @@ struct rtnl_link_stats64 *hns_nic_get_stats64(struct net_device *ndev,
 	return stats;
 }
 
+static u16
+hns_nic_select_queue(struct net_device *ndev, struct sk_buff *skb,
+		     void *accel_priv, select_queue_fallback_t fallback)
+{
+	struct ethhdr *eth_hdr = (struct ethhdr *)skb->data;
+	struct hns_nic_priv *priv = netdev_priv(ndev);
+
+	/* fix hardware broadcast/multicast packets queue loopback */
+	if (!AE_IS_VER1(priv->enet_ver) &&
+	    is_multicast_ether_addr(eth_hdr->h_dest))
+		return 0;
+	else
+		return fallback(ndev, skb);
+}
+
 static const struct net_device_ops hns_nic_netdev_ops = {
 	.ndo_open = hns_nic_net_open,
 	.ndo_stop = hns_nic_net_stop,
@@ -1581,6 +1613,7 @@ static const struct net_device_ops hns_nic_netdev_ops = {
 	.ndo_poll_controller = hns_nic_poll_controller,
 #endif
 	.ndo_set_rx_mode = hns_nic_set_rx_mode,
+	.ndo_select_queue = hns_nic_select_queue,
 };
 
 static void hns_nic_update_link_status(struct net_device *netdev)
@@ -1742,7 +1775,8 @@ static int hns_nic_init_ring_data(struct hns_nic_priv *priv)
 		rd->queue_index = i;
 		rd->ring = &h->qs[i]->tx_ring;
 		rd->poll_one = hns_nic_tx_poll_one;
-		rd->fini_process = is_ver1 ? hns_nic_tx_fini_pro : NULL;
+		rd->fini_process = is_ver1 ? hns_nic_tx_fini_pro :
+			hns_nic_tx_fini_pro_v2;
 
 		netif_napi_add(priv->netdev, &rd->napi,
 			       hns_nic_common_poll, NIC_TX_CLEAN_MAX_NUM);
@@ -1754,7 +1788,8 @@ static int hns_nic_init_ring_data(struct hns_nic_priv *priv)
 		rd->ring = &h->qs[i - h->q_num]->rx_ring;
 		rd->poll_one = hns_nic_rx_poll_one;
 		rd->ex_process = hns_nic_rx_up_pro;
-		rd->fini_process = is_ver1 ? hns_nic_rx_fini_pro : NULL;
+		rd->fini_process = is_ver1 ? hns_nic_rx_fini_pro :
+			hns_nic_rx_fini_pro_v2;
 
 		netif_napi_add(priv->netdev, &rd->napi,
 			       hns_nic_common_poll, NIC_RX_CLEAN_MAX_NUM);
@@ -2017,9 +2052,8 @@ static int hns_nic_dev_remove(struct platform_device *pdev)
 		hns_nic_uninit_ring_data(priv);
 	priv->ring_data = NULL;
 
-	if (priv->phy)
-		phy_disconnect(priv->phy);
-	priv->phy = NULL;
+	if (ndev->phydev)
+		phy_disconnect(ndev->phydev);
 
 	if (!IS_ERR_OR_NULL(priv->ae_handle))
 		hnae_put_handle(priv->ae_handle);
diff --git a/drivers/net/ethernet/hisilicon/hns/hns_enet.h b/drivers/net/ethernet/hisilicon/hns/hns_enet.h
index 44bb3015eed3..5b412de350aa 100644
--- a/drivers/net/ethernet/hisilicon/hns/hns_enet.h
+++ b/drivers/net/ethernet/hisilicon/hns/hns_enet.h
@@ -59,7 +59,6 @@ struct hns_nic_priv {
 	u32 port_id;
 	int phy_mode;
 	int phy_led_val;
-	struct phy_device *phy;
 	struct net_device *netdev;
 	struct device *dev;
 	struct hnae_handle *ae_handle;
diff --git a/drivers/net/ethernet/hisilicon/hns/hns_ethtool.c b/drivers/net/ethernet/hisilicon/hns/hns_ethtool.c
index ab33487a5321..87d5c94b2810 100644
--- a/drivers/net/ethernet/hisilicon/hns/hns_ethtool.c
+++ b/drivers/net/ethernet/hisilicon/hns/hns_ethtool.c
@@ -48,9 +48,9 @@ static u32 hns_nic_get_link(struct net_device *net_dev)
 
 	h = priv->ae_handle;
 
-	if (priv->phy) {
-		if (!genphy_read_status(priv->phy))
-			link_stat = priv->phy->link;
+	if (net_dev->phydev) {
+		if (!genphy_read_status(net_dev->phydev))
+			link_stat = net_dev->phydev->link;
 		else
 			link_stat = 0;
 	}
@@ -64,15 +64,14 @@ static u32 hns_nic_get_link(struct net_device *net_dev)
 }
 
 static void hns_get_mdix_mode(struct net_device *net_dev,
-			      struct ethtool_cmd *cmd)
+			      struct ethtool_link_ksettings *cmd)
 {
 	int mdix_ctrl, mdix, retval, is_resolved;
-	struct hns_nic_priv *priv = netdev_priv(net_dev);
-	struct phy_device *phy_dev = priv->phy;
+	struct phy_device *phy_dev = net_dev->phydev;
 
 	if (!phy_dev || !phy_dev->mdio.bus) {
-		cmd->eth_tp_mdix_ctrl = ETH_TP_MDI_INVALID;
-		cmd->eth_tp_mdix = ETH_TP_MDI_INVALID;
+		cmd->base.eth_tp_mdix_ctrl = ETH_TP_MDI_INVALID;
+		cmd->base.eth_tp_mdix = ETH_TP_MDI_INVALID;
 		return;
 	}
 
@@ -89,35 +88,35 @@ static void hns_get_mdix_mode(struct net_device *net_dev,
 
 	switch (mdix_ctrl) {
 	case 0x0:
-		cmd->eth_tp_mdix_ctrl = ETH_TP_MDI;
+		cmd->base.eth_tp_mdix_ctrl = ETH_TP_MDI;
 		break;
 	case 0x1:
-		cmd->eth_tp_mdix_ctrl = ETH_TP_MDI_X;
+		cmd->base.eth_tp_mdix_ctrl = ETH_TP_MDI_X;
 		break;
 	case 0x3:
-		cmd->eth_tp_mdix_ctrl = ETH_TP_MDI_AUTO;
+		cmd->base.eth_tp_mdix_ctrl = ETH_TP_MDI_AUTO;
 		break;
 	default:
-		cmd->eth_tp_mdix_ctrl = ETH_TP_MDI_INVALID;
+		cmd->base.eth_tp_mdix_ctrl = ETH_TP_MDI_INVALID;
 		break;
 	}
 
 	if (!is_resolved)
-		cmd->eth_tp_mdix = ETH_TP_MDI_INVALID;
+		cmd->base.eth_tp_mdix = ETH_TP_MDI_INVALID;
 	else if (mdix)
-		cmd->eth_tp_mdix = ETH_TP_MDI_X;
+		cmd->base.eth_tp_mdix = ETH_TP_MDI_X;
 	else
-		cmd->eth_tp_mdix = ETH_TP_MDI;
+		cmd->base.eth_tp_mdix = ETH_TP_MDI;
 }
 
 /**
- *hns_nic_get_settings - implement ethtool get settings
+ *hns_nic_get_link_ksettings - implement ethtool get link ksettings
  *@net_dev: net_device
- *@cmd: ethtool_cmd
+ *@cmd: ethtool_link_ksettings
  *retuen 0 - success , negative --fail
  */
-static int hns_nic_get_settings(struct net_device *net_dev,
-				struct ethtool_cmd *cmd)
+static int hns_nic_get_link_ksettings(struct net_device *net_dev,
+				      struct ethtool_link_ksettings *cmd)
 {
 	struct hns_nic_priv *priv = netdev_priv(net_dev);
 	struct hnae_handle *h;
@@ -125,6 +124,7 @@ static int hns_nic_get_settings(struct net_device *net_dev,
 	int ret;
 	u8 duplex;
 	u16 speed;
+	u32 supported, advertising;
 
 	if (!priv || !priv->ae_handle)
 		return -ESRCH;
@@ -139,38 +139,43 @@ static int hns_nic_get_settings(struct net_device *net_dev,
 		return -EINVAL;
 	}
 
+	ethtool_convert_link_mode_to_legacy_u32(&supported,
+						cmd->link_modes.supported);
+	ethtool_convert_link_mode_to_legacy_u32(&advertising,
+						cmd->link_modes.advertising);
+
 	/* When there is no phy, autoneg is off. */
-	cmd->autoneg = false;
-	ethtool_cmd_speed_set(cmd, speed);
-	cmd->duplex = duplex;
+	cmd->base.autoneg = false;
+	cmd->base.cmd = speed;
+	cmd->base.duplex = duplex;
 
-	if (priv->phy)
-		(void)phy_ethtool_gset(priv->phy, cmd);
+	if (net_dev->phydev)
+		(void)phy_ethtool_ksettings_get(net_dev->phydev, cmd);
 
 	link_stat = hns_nic_get_link(net_dev);
 	if (!link_stat) {
-		ethtool_cmd_speed_set(cmd, (u32)SPEED_UNKNOWN);
-		cmd->duplex = DUPLEX_UNKNOWN;
+		cmd->base.speed = (u32)SPEED_UNKNOWN;
+		cmd->base.duplex = DUPLEX_UNKNOWN;
 	}
 
-	if (cmd->autoneg)
-		cmd->advertising |= ADVERTISED_Autoneg;
+	if (cmd->base.autoneg)
+		advertising |= ADVERTISED_Autoneg;
 
-	cmd->supported |= h->if_support;
+	supported |= h->if_support;
 	if (h->phy_if == PHY_INTERFACE_MODE_SGMII) {
-		cmd->supported |= SUPPORTED_TP;
-		cmd->advertising |= ADVERTISED_1000baseT_Full;
+		supported |= SUPPORTED_TP;
+		advertising |= ADVERTISED_1000baseT_Full;
 	} else if (h->phy_if == PHY_INTERFACE_MODE_XGMII) {
-		cmd->supported |= SUPPORTED_FIBRE;
-		cmd->advertising |= ADVERTISED_10000baseKR_Full;
+		supported |= SUPPORTED_FIBRE;
+		advertising |= ADVERTISED_10000baseKR_Full;
 	}
 
 	switch (h->media_type) {
 	case HNAE_MEDIA_TYPE_FIBER:
-		cmd->port = PORT_FIBRE;
+		cmd->base.port = PORT_FIBRE;
 		break;
 	case HNAE_MEDIA_TYPE_COPPER:
-		cmd->port = PORT_TP;
+		cmd->base.port = PORT_TP;
 		break;
 	case HNAE_MEDIA_TYPE_UNKNOWN:
 	default:
@@ -178,23 +183,27 @@ static int hns_nic_get_settings(struct net_device *net_dev,
 	}
 
 	if (!(AE_IS_VER1(priv->enet_ver) && h->port_type == HNAE_PORT_DEBUG))
-		cmd->supported |= SUPPORTED_Pause;
+		supported |= SUPPORTED_Pause;
 
-	cmd->transceiver = XCVR_EXTERNAL;
-	cmd->mdio_support = (ETH_MDIO_SUPPORTS_C45 | ETH_MDIO_SUPPORTS_C22);
+	ethtool_convert_legacy_u32_to_link_mode(cmd->link_modes.supported,
+						supported);
+	ethtool_convert_legacy_u32_to_link_mode(cmd->link_modes.advertising,
+						advertising);
+
+	cmd->base.mdio_support = ETH_MDIO_SUPPORTS_C45 | ETH_MDIO_SUPPORTS_C22;
 	hns_get_mdix_mode(net_dev, cmd);
 
 	return 0;
 }
 
 /**
- *hns_nic_set_settings - implement ethtool set settings
+ *hns_nic_set_link_settings - implement ethtool set link ksettings
  *@net_dev: net_device
- *@cmd: ethtool_cmd
+ *@cmd: ethtool_link_ksettings
  *retuen 0 - success , negative --fail
  */
-static int hns_nic_set_settings(struct net_device *net_dev,
-				struct ethtool_cmd *cmd)
+static int hns_nic_set_link_ksettings(struct net_device *net_dev,
+				      const struct ethtool_link_ksettings *cmd)
 {
 	struct hns_nic_priv *priv = netdev_priv(net_dev);
 	struct hnae_handle *h;
@@ -208,24 +217,25 @@ static int hns_nic_set_settings(struct net_device *net_dev,
 		return -ENODEV;
 
 	h = priv->ae_handle;
-	speed = ethtool_cmd_speed(cmd);
+	speed = cmd->base.speed;
 
 	if (h->phy_if == PHY_INTERFACE_MODE_XGMII) {
-		if (cmd->autoneg == AUTONEG_ENABLE || speed != SPEED_10000 ||
-		    cmd->duplex != DUPLEX_FULL)
+		if (cmd->base.autoneg == AUTONEG_ENABLE ||
+		    speed != SPEED_10000 ||
+		    cmd->base.duplex != DUPLEX_FULL)
 			return -EINVAL;
 	} else if (h->phy_if == PHY_INTERFACE_MODE_SGMII) {
-		if (!priv->phy && cmd->autoneg == AUTONEG_ENABLE)
+		if (!net_dev->phydev && cmd->base.autoneg == AUTONEG_ENABLE)
 			return -EINVAL;
 
-		if (speed == SPEED_1000 && cmd->duplex == DUPLEX_HALF)
+		if (speed == SPEED_1000 && cmd->base.duplex == DUPLEX_HALF)
 			return -EINVAL;
-		if (priv->phy)
-			return phy_ethtool_sset(priv->phy, cmd);
+		if (net_dev->phydev)
+			return phy_ethtool_ksettings_set(net_dev->phydev, cmd);
 
 		if ((speed != SPEED_10 && speed != SPEED_100 &&
-		     speed != SPEED_1000) || (cmd->duplex != DUPLEX_HALF &&
-		     cmd->duplex != DUPLEX_FULL))
+		     speed != SPEED_1000) || (cmd->base.duplex != DUPLEX_HALF &&
+		     cmd->base.duplex != DUPLEX_FULL))
 			return -EINVAL;
 	} else {
 		netdev_err(net_dev, "Not supported!");
@@ -233,7 +243,7 @@ static int hns_nic_set_settings(struct net_device *net_dev,
 	}
 
 	if (h->dev->ops->adjust_link) {
-		h->dev->ops->adjust_link(h, (int)speed, cmd->duplex);
+		h->dev->ops->adjust_link(h, (int)speed, cmd->base.duplex);
 		return 0;
 	}
 
@@ -305,7 +315,7 @@ static int __lb_setup(struct net_device *ndev,
 {
 	int ret = 0;
 	struct hns_nic_priv *priv = netdev_priv(ndev);
-	struct phy_device *phy_dev = priv->phy;
+	struct phy_device *phy_dev = ndev->phydev;
 	struct hnae_handle *h = priv->ae_handle;
 
 	switch (loop) {
@@ -342,6 +352,13 @@ static int __lb_setup(struct net_device *ndev,
 		break;
 	}
 
+	if (!ret) {
+		if (loop == MAC_LOOP_NONE)
+			h->dev->ops->set_promisc_mode(
+				h, ndev->flags & IFF_PROMISC);
+		else
+			h->dev->ops->set_promisc_mode(h, 1);
+	}
 	return ret;
 }
 
@@ -910,7 +927,7 @@ void hns_get_strings(struct net_device *netdev, u32 stringset, u8 *data)
 		memcpy(buff, hns_nic_test_strs[MAC_INTERNALLOOP_SERDES],
 		       ETH_GSTRING_LEN);
 		buff += ETH_GSTRING_LEN;
-		if ((priv->phy) && (!priv->phy->is_c45))
+		if ((netdev->phydev) && (!netdev->phydev->is_c45))
 			memcpy(buff, hns_nic_test_strs[MAC_INTERNALLOOP_PHY],
 			       ETH_GSTRING_LEN);
 
@@ -996,7 +1013,7 @@ int hns_get_sset_count(struct net_device *netdev, int stringset)
 		if (priv->ae_handle->phy_if == PHY_INTERFACE_MODE_XGMII)
 			cnt--;
 
-		if ((!priv->phy) || (priv->phy->is_c45))
+		if ((!netdev->phydev) || (netdev->phydev->is_c45))
 			cnt--;
 
 		return cnt;
@@ -1015,8 +1032,7 @@ int hns_get_sset_count(struct net_device *netdev, int stringset)
 int hns_phy_led_set(struct net_device *netdev, int value)
 {
 	int retval;
-	struct hns_nic_priv *priv = netdev_priv(netdev);
-	struct phy_device *phy_dev = priv->phy;
+	struct phy_device *phy_dev = netdev->phydev;
 
 	retval = phy_write(phy_dev, HNS_PHY_PAGE_REG, HNS_PHY_PAGE_LED);
 	retval |= phy_write(phy_dev, HNS_LED_FC_REG, value);
@@ -1039,7 +1055,7 @@ int hns_set_phys_id(struct net_device *netdev, enum ethtool_phys_id_state state)
 {
 	struct hns_nic_priv *priv = netdev_priv(netdev);
 	struct hnae_handle *h = priv->ae_handle;
-	struct phy_device *phy_dev = priv->phy;
+	struct phy_device *phy_dev = netdev->phydev;
 	int ret;
 
 	if (phy_dev)
@@ -1159,8 +1175,7 @@ static int hns_get_regs_len(struct net_device *net_dev)
 static int hns_nic_nway_reset(struct net_device *netdev)
 {
 	int ret = 0;
-	struct hns_nic_priv *priv = netdev_priv(netdev);
-	struct phy_device *phy = priv->phy;
+	struct phy_device *phy = netdev->phydev;
 
 	if (netif_running(netdev)) {
 		if (phy)
@@ -1264,11 +1279,9 @@ static int hns_get_rxnfc(struct net_device *netdev,
 	return 0;
 }
 
-static struct ethtool_ops hns_ethtool_ops = {
+static const struct ethtool_ops hns_ethtool_ops = {
 	.get_drvinfo = hns_nic_get_drvinfo,
 	.get_link  = hns_nic_get_link,
-	.get_settings  = hns_nic_get_settings,
-	.set_settings  = hns_nic_set_settings,
 	.get_ringparam = hns_get_ringparam,
 	.get_pauseparam = hns_get_pauseparam,
 	.set_pauseparam = hns_set_pauseparam,
@@ -1288,6 +1301,8 @@ static struct ethtool_ops hns_ethtool_ops = {
 	.get_rxfh = hns_get_rss,
 	.set_rxfh = hns_set_rss,
 	.get_rxnfc = hns_get_rxnfc,
+	.get_link_ksettings  = hns_nic_get_link_ksettings,
+	.set_link_ksettings  = hns_nic_set_link_ksettings,
 };
 
 void hns_ethtool_set_ops(struct net_device *ndev)
diff --git a/drivers/net/ethernet/i825xx/82596.c b/drivers/net/ethernet/i825xx/82596.c
index befb4ac3e2b0..ce235b776793 100644
--- a/drivers/net/ethernet/i825xx/82596.c
+++ b/drivers/net/ethernet/i825xx/82596.c
@@ -89,10 +89,10 @@ static char version[] __initdata =
 #define DEB(x,y)	if (i596_debug & (x)) y
 
 
-#if defined(CONFIG_MVME16x_NET) || defined(CONFIG_MVME16x_NET_MODULE)
+#if IS_ENABLED(CONFIG_MVME16x_NET)
 #define ENABLE_MVME16x_NET
 #endif
-#if defined(CONFIG_BVME6000_NET) || defined(CONFIG_BVME6000_NET_MODULE)
+#if IS_ENABLED(CONFIG_BVME6000_NET)
 #define ENABLE_BVME6000_NET
 #endif
 
diff --git a/drivers/net/ethernet/ibm/emac/core.c b/drivers/net/ethernet/ibm/emac/core.c
index 7af09cbc53f0..8f139197f1aa 100644
--- a/drivers/net/ethernet/ibm/emac/core.c
+++ b/drivers/net/ethernet/ibm/emac/core.c
@@ -2780,7 +2780,7 @@ static int emac_probe(struct platform_device *ofdev)
 	/* Get interrupts. EMAC irq is mandatory, WOL irq is optional */
 	dev->emac_irq = irq_of_parse_and_map(np, 0);
 	dev->wol_irq = irq_of_parse_and_map(np, 1);
-	if (dev->emac_irq == NO_IRQ) {
+	if (!dev->emac_irq) {
 		printk(KERN_ERR "%s: Can't map main interrupt\n", np->full_name);
 		goto err_free;
 	}
@@ -2943,9 +2943,9 @@ static int emac_probe(struct platform_device *ofdev)
  err_reg_unmap:
 	iounmap(dev->emacp);
  err_irq_unmap:
-	if (dev->wol_irq != NO_IRQ)
+	if (dev->wol_irq)
 		irq_dispose_mapping(dev->wol_irq);
-	if (dev->emac_irq != NO_IRQ)
+	if (dev->emac_irq)
 		irq_dispose_mapping(dev->emac_irq);
  err_free:
 	free_netdev(ndev);
@@ -2987,9 +2987,9 @@ static int emac_remove(struct platform_device *ofdev)
 	emac_dbg_unregister(dev);
 	iounmap(dev->emacp);
 
-	if (dev->wol_irq != NO_IRQ)
+	if (dev->wol_irq)
 		irq_dispose_mapping(dev->wol_irq);
-	if (dev->emac_irq != NO_IRQ)
+	if (dev->emac_irq)
 		irq_dispose_mapping(dev->emac_irq);
 
 	free_netdev(dev->ndev);
diff --git a/drivers/net/ethernet/ibm/emac/mal.c b/drivers/net/ethernet/ibm/emac/mal.c
index fdb5cdb3cd15..aaf6fec566b5 100644
--- a/drivers/net/ethernet/ibm/emac/mal.c
+++ b/drivers/net/ethernet/ibm/emac/mal.c
@@ -597,9 +597,8 @@ static int mal_probe(struct platform_device *ofdev)
 		mal->rxde_irq = irq_of_parse_and_map(ofdev->dev.of_node, 4);
 	}
 
-	if (mal->txeob_irq == NO_IRQ || mal->rxeob_irq == NO_IRQ ||
-	    mal->serr_irq == NO_IRQ || mal->txde_irq == NO_IRQ ||
-	    mal->rxde_irq == NO_IRQ) {
+	if (!mal->txeob_irq || !mal->rxeob_irq || !mal->serr_irq ||
+	    !mal->txde_irq  || !mal->rxde_irq) {
 		printk(KERN_ERR
 		       "mal%d: failed to map interrupts !\n", index);
 		err = -ENODEV;
diff --git a/drivers/net/ethernet/ibm/ibmvnic.c b/drivers/net/ethernet/ibm/ibmvnic.c
index 88f3c85fb04a..bfe17d9c022d 100644
--- a/drivers/net/ethernet/ibm/ibmvnic.c
+++ b/drivers/net/ethernet/ibm/ibmvnic.c
@@ -203,7 +203,8 @@ static void free_long_term_buff(struct ibmvnic_adapter *adapter,
 	struct device *dev = &adapter->vdev->dev;
 
 	dma_free_coherent(dev, ltb->size, ltb->buff, ltb->addr);
-	send_request_unmap(adapter, ltb->map_id);
+	if (!adapter->failover)
+		send_request_unmap(adapter, ltb->map_id);
 }
 
 static int alloc_rx_pool(struct ibmvnic_adapter *adapter,
@@ -522,7 +523,8 @@ static int ibmvnic_close(struct net_device *netdev)
 	for (i = 0; i < adapter->req_rx_queues; i++)
 		napi_disable(&adapter->napi[i]);
 
-	netif_tx_stop_all_queues(netdev);
+	if (!adapter->failover)
+		netif_tx_stop_all_queues(netdev);
 
 	if (adapter->bounce_buffer) {
 		if (!dma_mapping_error(dev, adapter->bounce_buffer_dma)) {
@@ -1422,7 +1424,7 @@ static int init_sub_crq_irqs(struct ibmvnic_adapter *adapter)
 		scrq = adapter->tx_scrq[i];
 		scrq->irq = irq_create_mapping(NULL, scrq->hw_irq);
 
-		if (scrq->irq == NO_IRQ) {
+		if (!scrq->irq) {
 			rc = -EINVAL;
 			dev_err(dev, "Error mapping irq\n");
 			goto req_tx_irq_failed;
@@ -1442,7 +1444,7 @@ static int init_sub_crq_irqs(struct ibmvnic_adapter *adapter)
 	for (i = 0; i < adapter->req_rx_queues; i++) {
 		scrq = adapter->rx_scrq[i];
 		scrq->irq = irq_create_mapping(NULL, scrq->hw_irq);
-		if (scrq->irq == NO_IRQ) {
+		if (!scrq->irq) {
 			rc = -EINVAL;
 			dev_err(dev, "Error mapping irq\n");
 			goto req_rx_irq_failed;
@@ -2777,12 +2779,6 @@ static void handle_control_ras_rsp(union ibmvnic_crq *crq,
 	}
 }
 
-static int ibmvnic_fw_comp_open(struct inode *inode, struct file *file)
-{
-	file->private_data = inode->i_private;
-	return 0;
-}
-
 static ssize_t trace_read(struct file *file, char __user *user_buf, size_t len,
 			  loff_t *ppos)
 {
@@ -2834,7 +2830,7 @@ static ssize_t trace_read(struct file *file, char __user *user_buf, size_t len,
 
 static const struct file_operations trace_ops = {
 	.owner		= THIS_MODULE,
-	.open		= ibmvnic_fw_comp_open,
+	.open		= simple_open,
 	.read		= trace_read,
 };
 
@@ -2884,7 +2880,7 @@ static ssize_t paused_write(struct file *file, const char __user *user_buf,
 
 static const struct file_operations paused_ops = {
 	.owner		= THIS_MODULE,
-	.open		= ibmvnic_fw_comp_open,
+	.open		= simple_open,
 	.read		= paused_read,
 	.write		= paused_write,
 };
@@ -2932,7 +2928,7 @@ static ssize_t tracing_write(struct file *file, const char __user *user_buf,
 
 static const struct file_operations tracing_ops = {
 	.owner		= THIS_MODULE,
-	.open		= ibmvnic_fw_comp_open,
+	.open		= simple_open,
 	.read		= tracing_read,
 	.write		= tracing_write,
 };
@@ -2985,7 +2981,7 @@ static ssize_t error_level_write(struct file *file, const char __user *user_buf,
 
 static const struct file_operations error_level_ops = {
 	.owner		= THIS_MODULE,
-	.open		= ibmvnic_fw_comp_open,
+	.open		= simple_open,
 	.read		= error_level_read,
 	.write		= error_level_write,
 };
@@ -3036,7 +3032,7 @@ static ssize_t trace_level_write(struct file *file, const char __user *user_buf,
 
 static const struct file_operations trace_level_ops = {
 	.owner		= THIS_MODULE,
-	.open		= ibmvnic_fw_comp_open,
+	.open		= simple_open,
 	.read		= trace_level_read,
 	.write		= trace_level_write,
 };
@@ -3089,7 +3085,7 @@ static ssize_t trace_buff_size_write(struct file *file,
 
 static const struct file_operations trace_size_ops = {
 	.owner		= THIS_MODULE,
-	.open		= ibmvnic_fw_comp_open,
+	.open		= simple_open,
 	.read		= trace_buff_size_read,
 	.write		= trace_buff_size_write,
 };
@@ -3280,6 +3276,10 @@ static void ibmvnic_handle_crq(union ibmvnic_crq *crq,
 			rc = ibmvnic_send_crq_init(adapter);
 			if (rc)
 				dev_err(dev, "Error sending init rc=%ld\n", rc);
+		} else if (gen_crq->cmd == IBMVNIC_DEVICE_FAILOVER) {
+			dev_info(dev, "Backing device failover detected\n");
+			netif_carrier_off(netdev);
+			adapter->failover = true;
 		} else {
 			/* The adapter lost the connection */
 			dev_err(dev, "Virtual Adapter failed (rc=%d)\n",
@@ -3615,8 +3615,18 @@ static void handle_crq_init_rsp(struct work_struct *work)
 	struct device *dev = &adapter->vdev->dev;
 	struct net_device *netdev = adapter->netdev;
 	unsigned long timeout = msecs_to_jiffies(30000);
+	bool restart = false;
 	int rc;
 
+	if (adapter->failover) {
+		release_sub_crqs(adapter);
+		if (netif_running(netdev)) {
+			netif_tx_disable(netdev);
+			ibmvnic_close(netdev);
+			restart = true;
+		}
+	}
+
 	send_version_xchg(adapter);
 	reinit_completion(&adapter->init_done);
 	if (!wait_for_completion_timeout(&adapter->init_done, timeout)) {
@@ -3645,6 +3655,17 @@ static void handle_crq_init_rsp(struct work_struct *work)
 
 	netdev->real_num_tx_queues = adapter->req_tx_queues;
 
+	if (adapter->failover) {
+		adapter->failover = false;
+		if (restart) {
+			rc = ibmvnic_open(netdev);
+			if (rc)
+				goto restart_failed;
+		}
+		netif_carrier_on(netdev);
+		return;
+	}
+
 	rc = register_netdev(netdev);
 	if (rc) {
 		dev_err(dev,
@@ -3655,6 +3676,8 @@ static void handle_crq_init_rsp(struct work_struct *work)
 
 	return;
 
+restart_failed:
+	dev_err(dev, "Failed to restart ibmvnic, rc=%d\n", rc);
 register_failed:
 	release_sub_crqs(adapter);
 task_failed:
@@ -3692,6 +3715,7 @@ static int ibmvnic_probe(struct vio_dev *dev, const struct vio_device_id *id)
 	dev_set_drvdata(&dev->dev, netdev);
 	adapter->vdev = dev;
 	adapter->netdev = netdev;
+	adapter->failover = false;
 
 	ether_addr_copy(adapter->mac_addr, mac_addr_p);
 	ether_addr_copy(netdev->dev_addr, adapter->mac_addr);
@@ -3721,6 +3745,7 @@ static int ibmvnic_probe(struct vio_dev *dev, const struct vio_device_id *id)
 	if (dma_mapping_error(&dev->dev, adapter->stats_token)) {
 		if (!firmware_has_feature(FW_FEATURE_CMO))
 			dev_err(&dev->dev, "Couldn't map stats buffer\n");
+		rc = -ENOMEM;
 		goto free_crq;
 	}
 
diff --git a/drivers/net/ethernet/ibm/ibmvnic.h b/drivers/net/ethernet/ibm/ibmvnic.h
index e82898fd518e..bfc84c7d0e11 100644
--- a/drivers/net/ethernet/ibm/ibmvnic.h
+++ b/drivers/net/ethernet/ibm/ibmvnic.h
@@ -830,6 +830,7 @@ enum ibmvfc_crq_format {
 	IBMVNIC_CRQ_INIT                 = 0x01,
 	IBMVNIC_CRQ_INIT_COMPLETE        = 0x02,
 	IBMVNIC_PARTITION_MIGRATED       = 0x06,
+	IBMVNIC_DEVICE_FAILOVER          = 0x08,
 };
 
 struct ibmvnic_crq_queue {
@@ -1047,4 +1048,5 @@ struct ibmvnic_adapter {
 	u8 map_id;
 
 	struct work_struct vnic_crq_init;
+	bool failover;
 };
diff --git a/drivers/net/ethernet/intel/e1000e/ptp.c b/drivers/net/ethernet/intel/e1000e/ptp.c
index 2e1b17ad52a3..ad03763e009a 100644
--- a/drivers/net/ethernet/intel/e1000e/ptp.c
+++ b/drivers/net/ethernet/intel/e1000e/ptp.c
@@ -334,7 +334,7 @@ void e1000e_ptp_init(struct e1000_adapter *adapter)
 	if (IS_ERR(adapter->ptp_clock)) {
 		adapter->ptp_clock = NULL;
 		e_err("ptp_clock_register failed\n");
-	} else {
+	} else if (adapter->ptp_clock) {
 		e_info("registered PHC clock\n");
 	}
 }
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k.h b/drivers/net/ethernet/intel/fm10k/fm10k.h
index c4cf08dcf5af..4d19e46f7c55 100644
--- a/drivers/net/ethernet/intel/fm10k/fm10k.h
+++ b/drivers/net/ethernet/intel/fm10k/fm10k.h
@@ -240,9 +240,7 @@ struct fm10k_iov_data {
 	struct fm10k_vf_info	vf_info[0];
 };
 
-#define fm10k_vxlan_port_for_each(vp, intfc) \
-	list_for_each_entry(vp, &(intfc)->vxlan_port, list)
-struct fm10k_vxlan_port {
+struct fm10k_udp_port {
 	struct list_head	list;
 	sa_family_t		sa_family;
 	__be16			port;
@@ -335,8 +333,9 @@ struct fm10k_intfc {
 	u32 reta[FM10K_RETA_SIZE];
 	u32 rssrk[FM10K_RSSRK_SIZE];
 
-	/* VXLAN port tracking information */
+	/* UDP encapsulation port tracking information */
 	struct list_head vxlan_port;
+	struct list_head geneve_port;
 
 #ifdef CONFIG_DEBUG_FS
 	struct dentry *dbg_intfc;
@@ -458,7 +457,7 @@ __be16 fm10k_tx_encap_offload(struct sk_buff *skb);
 netdev_tx_t fm10k_xmit_frame_ring(struct sk_buff *skb,
 				  struct fm10k_ring *tx_ring);
 void fm10k_tx_timeout_reset(struct fm10k_intfc *interface);
-u64 fm10k_get_tx_pending(struct fm10k_ring *ring);
+u64 fm10k_get_tx_pending(struct fm10k_ring *ring, bool in_sw);
 bool fm10k_check_tx_hang(struct fm10k_ring *tx_ring);
 void fm10k_alloc_rx_buffers(struct fm10k_ring *rx_ring, u16 cleaned_count);
 
@@ -496,7 +495,6 @@ int fm10k_close(struct net_device *netdev);
 
 /* Ethtool */
 void fm10k_set_ethtool_ops(struct net_device *dev);
-u32 fm10k_get_reta_size(struct net_device *netdev);
 void fm10k_write_reta(struct fm10k_intfc *interface, const u32 *indir);
 
 /* IOV */
@@ -509,7 +507,7 @@ int fm10k_iov_configure(struct pci_dev *pdev, int num_vfs);
 s32 fm10k_iov_update_pvid(struct fm10k_intfc *interface, u16 glort, u16 pvid);
 int fm10k_ndo_set_vf_mac(struct net_device *netdev, int vf_idx, u8 *mac);
 int fm10k_ndo_set_vf_vlan(struct net_device *netdev,
-			  int vf_idx, u16 vid, u8 qos);
+			  int vf_idx, u16 vid, u8 qos, __be16 vlan_proto);
 int fm10k_ndo_set_vf_bw(struct net_device *netdev, int vf_idx, int rate,
 			int unused);
 int fm10k_ndo_get_vf_config(struct net_device *netdev,
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_common.c b/drivers/net/ethernet/intel/fm10k/fm10k_common.c
index d6baaea8bc7c..dd95ac4f4c64 100644
--- a/drivers/net/ethernet/intel/fm10k/fm10k_common.c
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_common.c
@@ -207,6 +207,9 @@ s32 fm10k_disable_queues_generic(struct fm10k_hw *hw, u16 q_cnt)
 	/* clear tx_ready to prevent any false hits for reset */
 	hw->mac.tx_ready = false;
 
+	if (FM10K_REMOVED(hw->hw_addr))
+		return 0;
+
 	/* clear the enable bit for all rings */
 	for (i = 0; i < q_cnt; i++) {
 		reg = fm10k_read_reg(hw, FM10K_TXDCTL(i));
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_common.h b/drivers/net/ethernet/intel/fm10k/fm10k_common.h
index 50f71e997448..d51f9c7a47ff 100644
--- a/drivers/net/ethernet/intel/fm10k/fm10k_common.h
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_common.h
@@ -34,7 +34,7 @@ u32 fm10k_read_reg(struct fm10k_hw *hw, int reg);
 /* write operations, indexed using DWORDS */
 #define fm10k_write_reg(hw, reg, val) \
 do { \
-	u32 __iomem *hw_addr = ACCESS_ONCE((hw)->hw_addr); \
+	u32 __iomem *hw_addr = READ_ONCE((hw)->hw_addr); \
 	if (!FM10K_REMOVED(hw_addr)) \
 		writel((val), &hw_addr[(reg)]); \
 } while (0)
@@ -42,7 +42,7 @@ do { \
 /* Switch register write operations, index using DWORDS */
 #define fm10k_write_sw_reg(hw, reg, val) \
 do { \
-	u32 __iomem *sw_addr = ACCESS_ONCE((hw)->sw_addr); \
+	u32 __iomem *sw_addr = READ_ONCE((hw)->sw_addr); \
 	if (!FM10K_REMOVED(sw_addr)) \
 		writel((val), &sw_addr[(reg)]); \
 } while (0)
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_ethtool.c b/drivers/net/ethernet/intel/fm10k/fm10k_ethtool.c
index c04cbe9c9f7c..5241e0873397 100644
--- a/drivers/net/ethernet/intel/fm10k/fm10k_ethtool.c
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_ethtool.c
@@ -966,7 +966,7 @@ static int fm10k_set_priv_flags(struct net_device *netdev, u32 priv_flags)
 	return 0;
 }
 
-u32 fm10k_get_reta_size(struct net_device __always_unused *netdev)
+static u32 fm10k_get_reta_size(struct net_device __always_unused *netdev)
 {
 	return FM10K_RETA_SIZE * FM10K_RETA_ENTRIES_PER_REG;
 }
@@ -1182,6 +1182,7 @@ static const struct ethtool_ops fm10k_ethtool_ops = {
 	.set_rxfh		= fm10k_set_rssh,
 	.get_channels		= fm10k_get_channels,
 	.set_channels		= fm10k_set_channels,
+	.get_ts_info		= ethtool_op_get_ts_info,
 };
 
 void fm10k_set_ethtool_ops(struct net_device *dev)
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_iov.c b/drivers/net/ethernet/intel/fm10k/fm10k_iov.c
index 47f0743ec03b..5f4dac0d36ef 100644
--- a/drivers/net/ethernet/intel/fm10k/fm10k_iov.c
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_iov.c
@@ -51,7 +51,7 @@ s32 fm10k_iov_event(struct fm10k_intfc *interface)
 	int i;
 
 	/* if there is no iov_data then there is no mailbox to process */
-	if (!ACCESS_ONCE(interface->iov_data))
+	if (!READ_ONCE(interface->iov_data))
 		return 0;
 
 	rcu_read_lock();
@@ -99,7 +99,7 @@ s32 fm10k_iov_mbx(struct fm10k_intfc *interface)
 	int i;
 
 	/* if there is no iov_data then there is no mailbox to process */
-	if (!ACCESS_ONCE(interface->iov_data))
+	if (!READ_ONCE(interface->iov_data))
 		return 0;
 
 	rcu_read_lock();
@@ -445,7 +445,7 @@ int fm10k_ndo_set_vf_mac(struct net_device *netdev, int vf_idx, u8 *mac)
 }
 
 int fm10k_ndo_set_vf_vlan(struct net_device *netdev, int vf_idx, u16 vid,
-			  u8 qos)
+			  u8 qos, __be16 vlan_proto)
 {
 	struct fm10k_intfc *interface = netdev_priv(netdev);
 	struct fm10k_iov_data *iov_data = interface->iov_data;
@@ -460,6 +460,10 @@ int fm10k_ndo_set_vf_vlan(struct net_device *netdev, int vf_idx, u16 vid,
 	if (qos || (vid > (VLAN_VID_MASK - 1)))
 		return -EINVAL;
 
+	/* VF VLAN Protocol part to default is unsupported */
+	if (vlan_proto != htons(ETH_P_8021Q))
+		return -EPROTONOSUPPORT;
+
 	vf_info = &iov_data->vf_info[vf_idx];
 
 	/* exit if there is nothing to do */
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_main.c b/drivers/net/ethernet/intel/fm10k/fm10k_main.c
index e9767b6366a8..5de937852436 100644
--- a/drivers/net/ethernet/intel/fm10k/fm10k_main.c
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_main.c
@@ -56,7 +56,8 @@ static int __init fm10k_init_module(void)
 	pr_info("%s\n", fm10k_copyright);
 
 	/* create driver workqueue */
-	fm10k_workqueue = alloc_workqueue("fm10k", WQ_MEM_RECLAIM, 0);
+	fm10k_workqueue = alloc_workqueue("%s", WQ_MEM_RECLAIM, 0,
+					  fm10k_driver_name);
 
 	fm10k_dbg_init();
 
@@ -651,11 +652,11 @@ static int fm10k_clean_rx_irq(struct fm10k_q_vector *q_vector,
 static struct ethhdr *fm10k_port_is_vxlan(struct sk_buff *skb)
 {
 	struct fm10k_intfc *interface = netdev_priv(skb->dev);
-	struct fm10k_vxlan_port *vxlan_port;
+	struct fm10k_udp_port *vxlan_port;
 
 	/* we can only offload a vxlan if we recognize it as such */
 	vxlan_port = list_first_entry_or_null(&interface->vxlan_port,
-					      struct fm10k_vxlan_port, list);
+					      struct fm10k_udp_port, list);
 
 	if (!vxlan_port)
 		return NULL;
@@ -1128,13 +1129,24 @@ static u64 fm10k_get_tx_completed(struct fm10k_ring *ring)
 	return ring->stats.packets;
 }
 
-u64 fm10k_get_tx_pending(struct fm10k_ring *ring)
+/**
+ * fm10k_get_tx_pending - how many Tx descriptors not processed
+ * @ring: the ring structure
+ * @in_sw: is tx_pending being checked in SW or in HW?
+ */
+u64 fm10k_get_tx_pending(struct fm10k_ring *ring, bool in_sw)
 {
 	struct fm10k_intfc *interface = ring->q_vector->interface;
 	struct fm10k_hw *hw = &interface->hw;
+	u32 head, tail;
 
-	u32 head = fm10k_read_reg(hw, FM10K_TDH(ring->reg_idx));
-	u32 tail = fm10k_read_reg(hw, FM10K_TDT(ring->reg_idx));
+	if (likely(in_sw)) {
+		head = ring->next_to_clean;
+		tail = ring->next_to_use;
+	} else {
+		head = fm10k_read_reg(hw, FM10K_TDH(ring->reg_idx));
+		tail = fm10k_read_reg(hw, FM10K_TDT(ring->reg_idx));
+	}
 
 	return ((head <= tail) ? tail : tail + ring->count) - head;
 }
@@ -1143,7 +1155,7 @@ bool fm10k_check_tx_hang(struct fm10k_ring *tx_ring)
 {
 	u32 tx_done = fm10k_get_tx_completed(tx_ring);
 	u32 tx_done_old = tx_ring->tx_stats.tx_done_old;
-	u32 tx_pending = fm10k_get_tx_pending(tx_ring);
+	u32 tx_pending = fm10k_get_tx_pending(tx_ring, true);
 
 	clear_check_for_tx_hang(tx_ring);
 
@@ -1397,7 +1409,7 @@ static void fm10k_update_itr(struct fm10k_ring_container *ring_container)
 	 * that the calculation will never get below a 1. The bit shift
 	 * accounts for changes in the ITR due to PCIe link speed.
 	 */
-	itr_round = ACCESS_ONCE(ring_container->itr_scale) + 8;
+	itr_round = READ_ONCE(ring_container->itr_scale) + 8;
 	avg_wire_size += BIT(itr_round) - 1;
 	avg_wire_size >>= itr_round;
 
@@ -1473,7 +1485,7 @@ static int fm10k_poll(struct napi_struct *napi, int budget)
 	/* re-enable the q_vector */
 	fm10k_qv_enable(q_vector);
 
-	return 0;
+	return min(work_done, budget - 1);
 }
 
 /**
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_netdev.c b/drivers/net/ethernet/intel/fm10k/fm10k_netdev.c
index 20a5bbe3f536..05629381be6b 100644
--- a/drivers/net/ethernet/intel/fm10k/fm10k_netdev.c
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_netdev.c
@@ -384,129 +384,171 @@ static void fm10k_request_glort_range(struct fm10k_intfc *interface)
 }
 
 /**
- * fm10k_del_vxlan_port_all
+ * fm10k_free_udp_port_info
  * @interface: board private structure
  *
- * This function frees the entire vxlan_port list
+ * This function frees both geneve_port and vxlan_port structures
  **/
-static void fm10k_del_vxlan_port_all(struct fm10k_intfc *interface)
+static void fm10k_free_udp_port_info(struct fm10k_intfc *interface)
 {
-	struct fm10k_vxlan_port *vxlan_port;
-
-	/* flush all entries from list */
-	vxlan_port = list_first_entry_or_null(&interface->vxlan_port,
-					      struct fm10k_vxlan_port, list);
-	while (vxlan_port) {
-		list_del(&vxlan_port->list);
-		kfree(vxlan_port);
-		vxlan_port = list_first_entry_or_null(&interface->vxlan_port,
-						      struct fm10k_vxlan_port,
-						      list);
+	struct fm10k_udp_port *port;
+
+	/* flush all entries from vxlan list */
+	port = list_first_entry_or_null(&interface->vxlan_port,
+					struct fm10k_udp_port, list);
+	while (port) {
+		list_del(&port->list);
+		kfree(port);
+		port = list_first_entry_or_null(&interface->vxlan_port,
+						struct fm10k_udp_port,
+						list);
+	}
+
+	/* flush all entries from geneve list */
+	port = list_first_entry_or_null(&interface->geneve_port,
+					struct fm10k_udp_port, list);
+	while (port) {
+		list_del(&port->list);
+		kfree(port);
+		port = list_first_entry_or_null(&interface->vxlan_port,
+						struct fm10k_udp_port,
+						list);
 	}
 }
 
 /**
- * fm10k_restore_vxlan_port
+ * fm10k_restore_udp_port_info
  * @interface: board private structure
  *
- * This function restores the value in the tunnel_cfg register after reset
+ * This function restores the value in the tunnel_cfg register(s) after reset
  **/
-static void fm10k_restore_vxlan_port(struct fm10k_intfc *interface)
+static void fm10k_restore_udp_port_info(struct fm10k_intfc *interface)
 {
 	struct fm10k_hw *hw = &interface->hw;
-	struct fm10k_vxlan_port *vxlan_port;
+	struct fm10k_udp_port *port;
 
 	/* only the PF supports configuring tunnels */
 	if (hw->mac.type != fm10k_mac_pf)
 		return;
 
-	vxlan_port = list_first_entry_or_null(&interface->vxlan_port,
-					      struct fm10k_vxlan_port, list);
+	port = list_first_entry_or_null(&interface->vxlan_port,
+					struct fm10k_udp_port, list);
 
 	/* restore tunnel configuration register */
 	fm10k_write_reg(hw, FM10K_TUNNEL_CFG,
-			(vxlan_port ? ntohs(vxlan_port->port) : 0) |
+			(port ? ntohs(port->port) : 0) |
 			(ETH_P_TEB << FM10K_TUNNEL_CFG_NVGRE_SHIFT));
+
+	port = list_first_entry_or_null(&interface->geneve_port,
+					struct fm10k_udp_port, list);
+
+	/* restore Geneve tunnel configuration register */
+	fm10k_write_reg(hw, FM10K_TUNNEL_CFG_GENEVE,
+			(port ? ntohs(port->port) : 0));
+}
+
+static struct fm10k_udp_port *
+fm10k_remove_tunnel_port(struct list_head *ports,
+			 struct udp_tunnel_info *ti)
+{
+	struct fm10k_udp_port *port;
+
+	list_for_each_entry(port, ports, list) {
+		if ((port->port == ti->port) &&
+		    (port->sa_family == ti->sa_family)) {
+			list_del(&port->list);
+			return port;
+		}
+	}
+
+	return NULL;
+}
+
+static void fm10k_insert_tunnel_port(struct list_head *ports,
+				     struct udp_tunnel_info *ti)
+{
+	struct fm10k_udp_port *port;
+
+	/* remove existing port entry from the list so that the newest items
+	 * are always at the tail of the list.
+	 */
+	port = fm10k_remove_tunnel_port(ports, ti);
+	if (!port) {
+		port = kmalloc(sizeof(*port), GFP_ATOMIC);
+		if  (!port)
+			return;
+		port->port = ti->port;
+		port->sa_family = ti->sa_family;
+	}
+
+	list_add_tail(&port->list, ports);
 }
 
 /**
- * fm10k_add_vxlan_port
+ * fm10k_udp_tunnel_add
  * @netdev: network interface device structure
  * @ti: Tunnel endpoint information
  *
- * This function is called when a new VXLAN interface has added a new port
- * number to the range that is currently in use for VXLAN.  The new port
- * number is always added to the tail so that the port number list should
- * match the order in which the ports were allocated.  The head of the list
- * is always used as the VXLAN port number for offloads.
+ * This function is called when a new UDP tunnel port has been added.
+ * Due to hardware restrictions, only one port per type can be offloaded at
+ * once.
  **/
-static void fm10k_add_vxlan_port(struct net_device *dev,
+static void fm10k_udp_tunnel_add(struct net_device *dev,
 				 struct udp_tunnel_info *ti)
 {
 	struct fm10k_intfc *interface = netdev_priv(dev);
-	struct fm10k_vxlan_port *vxlan_port;
 
-	if (ti->type != UDP_TUNNEL_TYPE_VXLAN)
-		return;
 	/* only the PF supports configuring tunnels */
 	if (interface->hw.mac.type != fm10k_mac_pf)
 		return;
 
-	/* existing ports are pulled out so our new entry is always last */
-	fm10k_vxlan_port_for_each(vxlan_port, interface) {
-		if ((vxlan_port->port == ti->port) &&
-		    (vxlan_port->sa_family == ti->sa_family)) {
-			list_del(&vxlan_port->list);
-			goto insert_tail;
-		}
-	}
-
-	/* allocate memory to track ports */
-	vxlan_port = kmalloc(sizeof(*vxlan_port), GFP_ATOMIC);
-	if (!vxlan_port)
+	switch (ti->type) {
+	case UDP_TUNNEL_TYPE_VXLAN:
+		fm10k_insert_tunnel_port(&interface->vxlan_port, ti);
+		break;
+	case UDP_TUNNEL_TYPE_GENEVE:
+		fm10k_insert_tunnel_port(&interface->geneve_port, ti);
+		break;
+	default:
 		return;
-	vxlan_port->port = ti->port;
-	vxlan_port->sa_family = ti->sa_family;
-
-insert_tail:
-	/* add new port value to list */
-	list_add_tail(&vxlan_port->list, &interface->vxlan_port);
+	}
 
-	fm10k_restore_vxlan_port(interface);
+	fm10k_restore_udp_port_info(interface);
 }
 
 /**
- * fm10k_del_vxlan_port
+ * fm10k_udp_tunnel_del
  * @netdev: network interface device structure
  * @ti: Tunnel endpoint information
  *
- * This function is called when a new VXLAN interface has freed a port
- * number from the range that is currently in use for VXLAN.  The freed
- * port is removed from the list and the new head is used to determine
- * the port number for offloads.
+ * This function is called when a new UDP tunnel port is deleted. The freed
+ * port will be removed from the list, then we reprogram the offloaded port
+ * based on the head of the list.
  **/
-static void fm10k_del_vxlan_port(struct net_device *dev,
+static void fm10k_udp_tunnel_del(struct net_device *dev,
 				 struct udp_tunnel_info *ti)
 {
 	struct fm10k_intfc *interface = netdev_priv(dev);
-	struct fm10k_vxlan_port *vxlan_port;
+	struct fm10k_udp_port *port = NULL;
 
-	if (ti->type != UDP_TUNNEL_TYPE_VXLAN)
-		return;
 	if (interface->hw.mac.type != fm10k_mac_pf)
 		return;
 
-	/* find the port in the list and free it */
-	fm10k_vxlan_port_for_each(vxlan_port, interface) {
-		if ((vxlan_port->port == ti->port) &&
-		    (vxlan_port->sa_family == ti->sa_family)) {
-			list_del(&vxlan_port->list);
-			kfree(vxlan_port);
-			break;
-		}
+	switch (ti->type) {
+	case UDP_TUNNEL_TYPE_VXLAN:
+		port = fm10k_remove_tunnel_port(&interface->vxlan_port, ti);
+		break;
+	case UDP_TUNNEL_TYPE_GENEVE:
+		port = fm10k_remove_tunnel_port(&interface->geneve_port, ti);
+		break;
+	default:
+		return;
 	}
 
-	fm10k_restore_vxlan_port(interface);
+	/* if we did remove a port we need to free its memory */
+	kfree(port);
+
+	fm10k_restore_udp_port_info(interface);
 }
 
 /**
@@ -555,7 +597,6 @@ int fm10k_open(struct net_device *netdev)
 	if (err)
 		goto err_set_queues;
 
-	/* update VXLAN port configuration */
 	udp_tunnel_get_rx_info(netdev);
 
 	fm10k_up(interface);
@@ -591,7 +632,7 @@ int fm10k_close(struct net_device *netdev)
 
 	fm10k_qv_free_irq(interface);
 
-	fm10k_del_vxlan_port_all(interface);
+	fm10k_free_udp_port_info(interface);
 
 	fm10k_free_all_tx_resources(interface);
 	fm10k_free_all_rx_resources(interface);
@@ -1055,7 +1096,7 @@ void fm10k_restore_rx_state(struct fm10k_intfc *interface)
 	interface->xcast_mode = xcast_mode;
 
 	/* Restore tunnel configuration */
-	fm10k_restore_vxlan_port(interface);
+	fm10k_restore_udp_port_info(interface);
 }
 
 void fm10k_reset_rx_state(struct fm10k_intfc *interface)
@@ -1098,7 +1139,7 @@ static struct rtnl_link_stats64 *fm10k_get_stats64(struct net_device *netdev,
 	rcu_read_lock();
 
 	for (i = 0; i < interface->num_rx_queues; i++) {
-		ring = ACCESS_ONCE(interface->rx_ring[i]);
+		ring = READ_ONCE(interface->rx_ring[i]);
 
 		if (!ring)
 			continue;
@@ -1114,7 +1155,7 @@ static struct rtnl_link_stats64 *fm10k_get_stats64(struct net_device *netdev,
 	}
 
 	for (i = 0; i < interface->num_tx_queues; i++) {
-		ring = ACCESS_ONCE(interface->tx_ring[i]);
+		ring = READ_ONCE(interface->tx_ring[i]);
 
 		if (!ring)
 			continue;
@@ -1299,7 +1340,7 @@ static void *fm10k_dfwd_add_station(struct net_device *dev,
 static void fm10k_dfwd_del_station(struct net_device *dev, void *priv)
 {
 	struct fm10k_intfc *interface = netdev_priv(dev);
-	struct fm10k_l2_accel *l2_accel = ACCESS_ONCE(interface->l2_accel);
+	struct fm10k_l2_accel *l2_accel = READ_ONCE(interface->l2_accel);
 	struct fm10k_dglort_cfg dglort = { 0 };
 	struct fm10k_hw *hw = &interface->hw;
 	struct net_device *sdev = priv;
@@ -1375,8 +1416,8 @@ static const struct net_device_ops fm10k_netdev_ops = {
 	.ndo_set_vf_vlan	= fm10k_ndo_set_vf_vlan,
 	.ndo_set_vf_rate	= fm10k_ndo_set_vf_bw,
 	.ndo_get_vf_config	= fm10k_ndo_get_vf_config,
-	.ndo_udp_tunnel_add	= fm10k_add_vxlan_port,
-	.ndo_udp_tunnel_del	= fm10k_del_vxlan_port,
+	.ndo_udp_tunnel_add	= fm10k_udp_tunnel_add,
+	.ndo_udp_tunnel_del	= fm10k_udp_tunnel_del,
 	.ndo_dfwd_add_station	= fm10k_dfwd_add_station,
 	.ndo_dfwd_del_station	= fm10k_dfwd_del_station,
 #ifdef CONFIG_NET_POLL_CONTROLLER
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_pci.c b/drivers/net/ethernet/intel/fm10k/fm10k_pci.c
index 774a5654bf42..b1a2f8437d59 100644
--- a/drivers/net/ethernet/intel/fm10k/fm10k_pci.c
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_pci.c
@@ -62,7 +62,7 @@ u16 fm10k_read_pci_cfg_word(struct fm10k_hw *hw, u32 reg)
 
 u32 fm10k_read_reg(struct fm10k_hw *hw, int reg)
 {
-	u32 __iomem *hw_addr = ACCESS_ONCE(hw->hw_addr);
+	u32 __iomem *hw_addr = READ_ONCE(hw->hw_addr);
 	u32 value = 0;
 
 	if (FM10K_REMOVED(hw_addr))
@@ -133,7 +133,7 @@ static void fm10k_detach_subtask(struct fm10k_intfc *interface)
 	/* check the real address space to see if we've recovered */
 	hw_addr = READ_ONCE(interface->uc_addr);
 	value = readl(hw_addr);
-	if ((~value)) {
+	if (~value) {
 		interface->hw.hw_addr = interface->uc_addr;
 		netif_device_attach(netdev);
 		interface->flags |= FM10K_FLAG_RESET_REQUESTED;
@@ -734,15 +734,15 @@ static void fm10k_configure_rx_ring(struct fm10k_intfc *interface,
 	u64 rdba = ring->dma;
 	struct fm10k_hw *hw = &interface->hw;
 	u32 size = ring->count * sizeof(union fm10k_rx_desc);
-	u32 rxqctl = FM10K_RXQCTL_ENABLE | FM10K_RXQCTL_PF;
-	u32 rxdctl = FM10K_RXDCTL_WRITE_BACK_MIN_DELAY;
+	u32 rxqctl, rxdctl = FM10K_RXDCTL_WRITE_BACK_MIN_DELAY;
 	u32 srrctl = FM10K_SRRCTL_BUFFER_CHAINING_EN;
 	u32 rxint = FM10K_INT_MAP_DISABLE;
 	u8 rx_pause = interface->rx_pause;
 	u8 reg_idx = ring->reg_idx;
 
 	/* disable queue to avoid issues while updating state */
-	fm10k_write_reg(hw, FM10K_RXQCTL(reg_idx), 0);
+	rxqctl = fm10k_read_reg(hw, FM10K_RXQCTL(reg_idx));
+	rxqctl &= ~FM10K_RXQCTL_ENABLE;
 	fm10k_write_flush(hw);
 
 	/* possible poll here to verify ring resources have been cleaned */
@@ -797,6 +797,8 @@ static void fm10k_configure_rx_ring(struct fm10k_intfc *interface,
 	fm10k_write_reg(hw, FM10K_RXINT(reg_idx), rxint);
 
 	/* enable queue */
+	rxqctl = fm10k_read_reg(hw, FM10K_RXQCTL(reg_idx));
+	rxqctl |= FM10K_RXQCTL_ENABLE;
 	fm10k_write_reg(hw, FM10K_RXQCTL(reg_idx), rxqctl);
 
 	/* place buffers on ring for receive data */
@@ -1699,7 +1701,7 @@ void fm10k_down(struct fm10k_intfc *interface)
 
 		/* start checking at the last ring to have pending Tx */
 		for (; i < interface->num_tx_queues; i++)
-			if (fm10k_get_tx_pending(interface->tx_ring[i]))
+			if (fm10k_get_tx_pending(interface->tx_ring[i], false))
 				break;
 
 		/* if all the queues are drained, we can break now */
@@ -1835,8 +1837,9 @@ static int fm10k_sw_init(struct fm10k_intfc *interface,
 	interface->tx_itr = FM10K_TX_ITR_DEFAULT;
 	interface->rx_itr = FM10K_ITR_ADAPTIVE | FM10K_RX_ITR_DEFAULT;
 
-	/* initialize vxlan_port list */
+	/* initialize udp port lists */
 	INIT_LIST_HEAD(&interface->vxlan_port);
+	INIT_LIST_HEAD(&interface->geneve_port);
 
 	netdev_rss_key_fill(rss_key, sizeof(rss_key));
 	memcpy(interface->rssrk, rss_key, sizeof(rss_key));
@@ -1950,9 +1953,18 @@ static int fm10k_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	struct fm10k_intfc *interface;
 	int err;
 
+	if (pdev->error_state != pci_channel_io_normal) {
+		dev_err(&pdev->dev,
+			"PCI device still in an error state. Unable to load...\n");
+		return -EIO;
+	}
+
 	err = pci_enable_device_mem(pdev);
-	if (err)
+	if (err) {
+		dev_err(&pdev->dev,
+			"PCI enable device failed: %d\n", err);
 		return err;
+	}
 
 	err = dma_set_mask_and_coherent(&pdev->dev, DMA_BIT_MASK(48));
 	if (err)
@@ -2275,7 +2287,7 @@ static pci_ers_result_t fm10k_io_slot_reset(struct pci_dev *pdev)
 {
 	pci_ers_result_t result;
 
-	if (pci_enable_device_mem(pdev)) {
+	if (pci_reenable_device(pdev)) {
 		dev_err(&pdev->dev,
 			"Cannot re-enable PCI device after reset.\n");
 		result = PCI_ERS_RESULT_DISCONNECT;
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_pf.c b/drivers/net/ethernet/intel/fm10k/fm10k_pf.c
index 682299dd0ce4..23fb319fd2a0 100644
--- a/drivers/net/ethernet/intel/fm10k/fm10k_pf.c
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_pf.c
@@ -867,10 +867,6 @@ static s32 fm10k_iov_assign_default_mac_vlan_pf(struct fm10k_hw *hw,
 	vf_q_idx = fm10k_vf_queue_index(hw, vf_idx);
 	qmap_idx = qmap_stride * vf_idx;
 
-	/* MAP Tx queue back to 0 temporarily, and disable it */
-	fm10k_write_reg(hw, FM10K_TQMAP(qmap_idx), 0);
-	fm10k_write_reg(hw, FM10K_TXDCTL(vf_q_idx), 0);
-
 	/* Determine correct default VLAN ID. The FM10K_VLAN_OVERRIDE bit is
 	 * used here to indicate to the VF that it will not have privilege to
 	 * write VLAN_TABLE. All policy is enforced on the PF but this allows
@@ -886,9 +882,35 @@ static s32 fm10k_iov_assign_default_mac_vlan_pf(struct fm10k_hw *hw,
 	fm10k_tlv_attr_put_mac_vlan(msg, FM10K_MAC_VLAN_MSG_DEFAULT_MAC,
 				    vf_info->mac, vf_vid);
 
-	/* load onto outgoing mailbox, ignore any errors on enqueue */
-	if (vf_info->mbx.ops.enqueue_tx)
-		vf_info->mbx.ops.enqueue_tx(hw, &vf_info->mbx, msg);
+	/* Configure Queue control register with new VLAN ID. The TXQCTL
+	 * register is RO from the VF, so the PF must do this even in the
+	 * case of notifying the VF of a new VID via the mailbox.
+	 */
+	txqctl = ((u32)vf_vid << FM10K_TXQCTL_VID_SHIFT) &
+		 FM10K_TXQCTL_VID_MASK;
+	txqctl |= (vf_idx << FM10K_TXQCTL_TC_SHIFT) |
+		  FM10K_TXQCTL_VF | vf_idx;
+
+	for (i = 0; i < queues_per_pool; i++)
+		fm10k_write_reg(hw, FM10K_TXQCTL(vf_q_idx + i), txqctl);
+
+	/* try loading a message onto outgoing mailbox first */
+	if (vf_info->mbx.ops.enqueue_tx) {
+		err = vf_info->mbx.ops.enqueue_tx(hw, &vf_info->mbx, msg);
+		if (err != FM10K_MBX_ERR_NO_MBX)
+			return err;
+		err = 0;
+	}
+
+	/* If we aren't connected to a mailbox, this is most likely because
+	 * the VF driver is not running. It should thus be safe to re-map
+	 * queues and use the registers to pass the MAC address so that the VF
+	 * driver gets correct information during its initialization.
+	 */
+
+	/* MAP Tx queue back to 0 temporarily, and disable it */
+	fm10k_write_reg(hw, FM10K_TQMAP(qmap_idx), 0);
+	fm10k_write_reg(hw, FM10K_TXDCTL(vf_q_idx), 0);
 
 	/* verify ring has disabled before modifying base address registers */
 	txdctl = fm10k_read_reg(hw, FM10K_TXDCTL(vf_q_idx));
@@ -927,16 +949,6 @@ static s32 fm10k_iov_assign_default_mac_vlan_pf(struct fm10k_hw *hw,
 						   FM10K_TDLEN_ITR_SCALE_SHIFT);
 
 err_out:
-	/* configure Queue control register */
-	txqctl = ((u32)vf_vid << FM10K_TXQCTL_VID_SHIFT) &
-		 FM10K_TXQCTL_VID_MASK;
-	txqctl |= (vf_idx << FM10K_TXQCTL_TC_SHIFT) |
-		  FM10K_TXQCTL_VF | vf_idx;
-
-	/* assign VLAN ID */
-	for (i = 0; i < queues_per_pool; i++)
-		fm10k_write_reg(hw, FM10K_TXQCTL(vf_q_idx + i), txqctl);
-
 	/* restore the queue back to VF ownership */
 	fm10k_write_reg(hw, FM10K_TQMAP(qmap_idx), vf_q_idx);
 	return err;
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_type.h b/drivers/net/ethernet/intel/fm10k/fm10k_type.h
index f4e75c498287..6bb16c13d9d6 100644
--- a/drivers/net/ethernet/intel/fm10k/fm10k_type.h
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_type.h
@@ -154,6 +154,7 @@ struct fm10k_hw;
 #define FM10K_DGLORTDEC_INNERRSS_ENABLE		0x08000000
 #define FM10K_TUNNEL_CFG	0x0040
 #define FM10K_TUNNEL_CFG_NVGRE_SHIFT		16
+#define FM10K_TUNNEL_CFG_GENEVE	0x0041
 #define FM10K_SWPRI_MAP(_n)	((_n) + 0x0050)
 #define FM10K_SWPRI_MAX		16
 #define FM10K_RSSRK(_n, _m)	(((_n) * 0x10) + (_m) + 0x0800)
diff --git a/drivers/net/ethernet/intel/i40e/i40e.h b/drivers/net/ethernet/intel/i40e/i40e.h
index 2a882916b4f6..2030d7c1dc94 100644
--- a/drivers/net/ethernet/intel/i40e/i40e.h
+++ b/drivers/net/ethernet/intel/i40e/i40e.h
@@ -65,76 +65,72 @@
 #include "i40e_dcb.h"
 
 /* Useful i40e defaults */
-#define I40E_MAX_VEB          16
-
-#define I40E_MAX_NUM_DESCRIPTORS      4096
-#define I40E_MAX_CSR_SPACE (4 * 1024 * 1024 - 64 * 1024)
-#define I40E_DEFAULT_NUM_DESCRIPTORS  512
-#define I40E_REQ_DESCRIPTOR_MULTIPLE  32
-#define I40E_MIN_NUM_DESCRIPTORS      64
-#define I40E_MIN_MSIX                 2
-#define I40E_DEFAULT_NUM_VMDQ_VSI     8 /* max 256 VSIs */
-#define I40E_MIN_VSI_ALLOC            51 /* LAN, ATR, FCOE, 32 VF, 16 VMDQ */
+#define I40E_MAX_VEB			16
+
+#define I40E_MAX_NUM_DESCRIPTORS	4096
+#define I40E_MAX_CSR_SPACE		(4 * 1024 * 1024 - 64 * 1024)
+#define I40E_DEFAULT_NUM_DESCRIPTORS	512
+#define I40E_REQ_DESCRIPTOR_MULTIPLE	32
+#define I40E_MIN_NUM_DESCRIPTORS	64
+#define I40E_MIN_MSIX			2
+#define I40E_DEFAULT_NUM_VMDQ_VSI	8 /* max 256 VSIs */
+#define I40E_MIN_VSI_ALLOC		83 /* LAN, ATR, FCOE, 64 VF */
 /* max 16 qps */
 #define i40e_default_queues_per_vmdq(pf) \
 		(((pf)->flags & I40E_FLAG_RSS_AQ_CAPABLE) ? 4 : 1)
-#define I40E_DEFAULT_QUEUES_PER_VF    4
-#define I40E_DEFAULT_QUEUES_PER_TC    1 /* should be a power of 2 */
+#define I40E_DEFAULT_QUEUES_PER_VF	4
+#define I40E_DEFAULT_QUEUES_PER_TC	1 /* should be a power of 2 */
 #define i40e_pf_get_max_q_per_tc(pf) \
 		(((pf)->flags & I40E_FLAG_128_QP_RSS_CAPABLE) ? 128 : 64)
-#define I40E_FDIR_RING                0
-#define I40E_FDIR_RING_COUNT          32
+#define I40E_FDIR_RING			0
+#define I40E_FDIR_RING_COUNT		32
 #ifdef I40E_FCOE
-#define I40E_DEFAULT_FCOE             8 /* default number of QPs for FCoE */
-#define I40E_MINIMUM_FCOE             1 /* minimum number of QPs for FCoE */
+#define I40E_DEFAULT_FCOE		8 /* default number of QPs for FCoE */
+#define I40E_MINIMUM_FCOE		1 /* minimum number of QPs for FCoE */
 #endif /* I40E_FCOE */
-#define I40E_MAX_AQ_BUF_SIZE          4096
-#define I40E_AQ_LEN                   256
-#define I40E_AQ_WORK_LIMIT            66 /* max number of VFs + a little */
-#define I40E_MAX_USER_PRIORITY        8
-#define I40E_DEFAULT_MSG_ENABLE       4
-#define I40E_QUEUE_WAIT_RETRY_LIMIT   10
-#define I40E_INT_NAME_STR_LEN        (IFNAMSIZ + 16)
+#define I40E_MAX_AQ_BUF_SIZE		4096
+#define I40E_AQ_LEN			256
+#define I40E_AQ_WORK_LIMIT		66 /* max number of VFs + a little */
+#define I40E_MAX_USER_PRIORITY		8
+#define I40E_DEFAULT_MSG_ENABLE		4
+#define I40E_QUEUE_WAIT_RETRY_LIMIT	10
+#define I40E_INT_NAME_STR_LEN		(IFNAMSIZ + 16)
 
 /* Ethtool Private Flags */
-#define	I40E_PRIV_FLAGS_MFP_FLAG		BIT(0)
-#define	I40E_PRIV_FLAGS_LINKPOLL_FLAG		BIT(1)
+#define I40E_PRIV_FLAGS_MFP_FLAG		BIT(0)
+#define I40E_PRIV_FLAGS_LINKPOLL_FLAG		BIT(1)
 #define I40E_PRIV_FLAGS_FD_ATR			BIT(2)
 #define I40E_PRIV_FLAGS_VEB_STATS		BIT(3)
 #define I40E_PRIV_FLAGS_HW_ATR_EVICT		BIT(4)
 #define I40E_PRIV_FLAGS_TRUE_PROMISC_SUPPORT	BIT(5)
 
-#define I40E_NVM_VERSION_LO_SHIFT  0
-#define I40E_NVM_VERSION_LO_MASK   (0xff << I40E_NVM_VERSION_LO_SHIFT)
-#define I40E_NVM_VERSION_HI_SHIFT  12
-#define I40E_NVM_VERSION_HI_MASK   (0xf << I40E_NVM_VERSION_HI_SHIFT)
-#define I40E_OEM_VER_BUILD_MASK    0xffff
-#define I40E_OEM_VER_PATCH_MASK    0xff
-#define I40E_OEM_VER_BUILD_SHIFT   8
-#define I40E_OEM_VER_SHIFT         24
+#define I40E_NVM_VERSION_LO_SHIFT	0
+#define I40E_NVM_VERSION_LO_MASK	(0xff << I40E_NVM_VERSION_LO_SHIFT)
+#define I40E_NVM_VERSION_HI_SHIFT	12
+#define I40E_NVM_VERSION_HI_MASK	(0xf << I40E_NVM_VERSION_HI_SHIFT)
+#define I40E_OEM_VER_BUILD_MASK		0xffff
+#define I40E_OEM_VER_PATCH_MASK		0xff
+#define I40E_OEM_VER_BUILD_SHIFT	8
+#define I40E_OEM_VER_SHIFT		24
 #define I40E_PHY_DEBUG_ALL \
 	(I40E_AQ_PHY_DEBUG_DISABLE_LINK_FW | \
 	I40E_AQ_PHY_DEBUG_DISABLE_ALL_LINK_FW)
 
 /* The values in here are decimal coded as hex as is the case in the NVM map*/
-#define I40E_CURRENT_NVM_VERSION_HI 0x2
-#define I40E_CURRENT_NVM_VERSION_LO 0x40
+#define I40E_CURRENT_NVM_VERSION_HI	0x2
+#define I40E_CURRENT_NVM_VERSION_LO	0x40
 
-/* magic for getting defines into strings */
-#define STRINGIFY(foo)  #foo
-#define XSTRINGIFY(bar) STRINGIFY(bar)
-
-#define I40E_RX_DESC(R, i)			\
+#define I40E_RX_DESC(R, i)	\
 	(&(((union i40e_32byte_rx_desc *)((R)->desc))[i]))
-#define I40E_TX_DESC(R, i)			\
+#define I40E_TX_DESC(R, i)	\
 	(&(((struct i40e_tx_desc *)((R)->desc))[i]))
-#define I40E_TX_CTXTDESC(R, i)			\
+#define I40E_TX_CTXTDESC(R, i)	\
 	(&(((struct i40e_tx_context_desc *)((R)->desc))[i]))
-#define I40E_TX_FDIRDESC(R, i)			\
+#define I40E_TX_FDIRDESC(R, i)	\
 	(&(((struct i40e_filter_program_desc *)((R)->desc))[i]))
 
 /* default to trying for four seconds */
-#define I40E_TRY_LINK_TIMEOUT (4 * HZ)
+#define I40E_TRY_LINK_TIMEOUT	(4 * HZ)
 
 /**
  * i40e_is_mac_710 - Return true if MAC is X710/XL710
@@ -199,9 +195,9 @@ struct i40e_lump_tracking {
 #define I40E_FDIR_BUFFER_HEAD_ROOM	32
 #define I40E_FDIR_BUFFER_HEAD_ROOM_FOR_ATR (I40E_FDIR_BUFFER_HEAD_ROOM * 4)
 
-#define I40E_HKEY_ARRAY_SIZE ((I40E_PFQF_HKEY_MAX_INDEX + 1) * 4)
-#define I40E_HLUT_ARRAY_SIZE ((I40E_PFQF_HLUT_MAX_INDEX + 1) * 4)
-#define I40E_VF_HLUT_ARRAY_SIZE ((I40E_VFQF_HLUT1_MAX_INDEX + 1) * 4)
+#define I40E_HKEY_ARRAY_SIZE	((I40E_PFQF_HKEY_MAX_INDEX + 1) * 4)
+#define I40E_HLUT_ARRAY_SIZE	((I40E_PFQF_HLUT_MAX_INDEX + 1) * 4)
+#define I40E_VF_HLUT_ARRAY_SIZE	((I40E_VFQF_HLUT1_MAX_INDEX + 1) * 4)
 
 enum i40e_fd_stat_idx {
 	I40E_FD_STAT_ATR,
@@ -387,8 +383,8 @@ struct i40e_pf {
 	struct mutex switch_mutex;
 	u16 lan_vsi;       /* our default LAN VSI */
 	u16 lan_veb;       /* initial relay, if exists */
-#define I40E_NO_VEB   0xffff
-#define I40E_NO_VSI   0xffff
+#define I40E_NO_VEB	0xffff
+#define I40E_NO_VSI	0xffff
 	u16 next_vsi;      /* Next unallocated VSI - 0-based! */
 	struct i40e_vsi **vsi;
 	struct i40e_veb *veb[I40E_MAX_VEB];
@@ -423,8 +419,8 @@ struct i40e_pf {
 	 */
 	u16 dcbx_cap;
 
-	u32	fcoe_hmc_filt_num;
-	u32	fcoe_hmc_cntx_num;
+	u32 fcoe_hmc_filt_num;
+	u32 fcoe_hmc_cntx_num;
 	struct i40e_filter_control_settings filter_settings;
 
 	struct ptp_clock *ptp_clock;
@@ -470,10 +466,10 @@ struct i40e_mac_filter {
 struct i40e_veb {
 	struct i40e_pf *pf;
 	u16 idx;
-	u16 veb_idx;           /* index of VEB parent */
+	u16 veb_idx;		/* index of VEB parent */
 	u16 seid;
 	u16 uplink_seid;
-	u16 stats_idx;           /* index of VEB parent */
+	u16 stats_idx;		/* index of VEB parent */
 	u8  enabled_tc;
 	u16 bridge_mode;	/* Bridge Mode (VEB/VEPA) */
 	u16 flags;
@@ -534,12 +530,13 @@ struct i40e_vsi {
 	u32  promisc_threshold;
 
 	u16 work_limit;
-	u16 int_rate_limit;  /* value in usecs */
+	u16 int_rate_limit;	/* value in usecs */
+
+	u16 rss_table_size;	/* HW RSS table size */
+	u16 rss_size;		/* Allocated RSS queues */
+	u8  *rss_hkey_user;	/* User configured hash keys */
+	u8  *rss_lut_user;	/* User configured lookup table entries */
 
-	u16 rss_table_size; /* HW RSS table size */
-	u16 rss_size;       /* Allocated RSS queues */
-	u8  *rss_hkey_user; /* User configured hash keys */
-	u8  *rss_lut_user;  /* User configured lookup table entries */
 
 	u16 max_frame;
 	u16 rx_buf_len;
@@ -550,14 +547,14 @@ struct i40e_vsi {
 	int base_vector;
 	bool irqs_ready;
 
-	u16 seid;            /* HW index of this VSI (absolute index) */
-	u16 id;              /* VSI number */
+	u16 seid;		/* HW index of this VSI (absolute index) */
+	u16 id;			/* VSI number */
 	u16 uplink_seid;
 
-	u16 base_queue;      /* vsi's first queue in hw array */
-	u16 alloc_queue_pairs; /* Allocated Tx/Rx queues */
-	u16 req_queue_pairs; /* User requested queue pairs */
-	u16 num_queue_pairs; /* Used tx and rx pairs */
+	u16 base_queue;		/* vsi's first queue in hw array */
+	u16 alloc_queue_pairs;	/* Allocated Tx/Rx queues */
+	u16 req_queue_pairs;	/* User requested queue pairs */
+	u16 num_queue_pairs;	/* Used tx and rx pairs */
 	u16 num_desc;
 	enum i40e_vsi_type type;  /* VSI type, e.g., LAN, FCoE, etc */
 	s16 vf_id;		/* Virtual function ID for SRIOV VSIs */
@@ -576,19 +573,16 @@ struct i40e_vsi {
 	/* TC BW limit max quanta within VSI */
 	u8  bw_ets_max_quanta[I40E_MAX_TRAFFIC_CLASS];
 
-	struct i40e_pf *back;  /* Backreference to associated PF */
-	u16 idx;               /* index in pf->vsi[] */
-	u16 veb_idx;           /* index of VEB parent */
-	struct kobject *kobj;  /* sysfs object */
-	bool current_isup;     /* Sync 'link up' logging */
+	struct i40e_pf *back;	/* Backreference to associated PF */
+	u16 idx;		/* index in pf->vsi[] */
+	u16 veb_idx;		/* index of VEB parent */
+	struct kobject *kobj;	/* sysfs object */
+	bool current_isup;	/* Sync 'link up' logging */
 
 	void *priv;	/* client driver data reference. */
 
 	/* VSI specific handlers */
 	irqreturn_t (*irq_handler)(int irq, void *data);
-
-	/* current rxnfc data */
-	struct ethtool_rxnfc rxnfc; /* current rss hash opts */
 } ____cacheline_internodealigned_in_smp;
 
 struct i40e_netdev_priv {
@@ -707,6 +701,8 @@ void i40e_do_reset_safe(struct i40e_pf *pf, u32 reset_flags);
 void i40e_do_reset(struct i40e_pf *pf, u32 reset_flags);
 int i40e_config_rss(struct i40e_vsi *vsi, u8 *seed, u8 *lut, u16 lut_size);
 int i40e_get_rss(struct i40e_vsi *vsi, u8 *seed, u8 *lut, u16 lut_size);
+void i40e_fill_rss_lut(struct i40e_pf *pf, u8 *lut,
+		       u16 rss_table_size, u16 rss_size);
 struct i40e_vsi *i40e_find_vsi_from_id(struct i40e_pf *pf, u16 id);
 void i40e_update_stats(struct i40e_vsi *vsi);
 void i40e_update_eth_stats(struct i40e_vsi *vsi);
@@ -714,8 +710,6 @@ struct rtnl_link_stats64 *i40e_get_vsi_stats_struct(struct i40e_vsi *vsi);
 int i40e_fetch_switch_configuration(struct i40e_pf *pf,
 				    bool printconfig);
 
-int i40e_program_fdir_filter(struct i40e_fdir_filter *fdir_data, u8 *raw_packet,
-			     struct i40e_pf *pf, bool add);
 int i40e_add_del_fdir(struct i40e_vsi *vsi,
 		      struct i40e_fdir_filter *input, bool add);
 void i40e_fdir_check_and_reenable(struct i40e_pf *pf);
diff --git a/drivers/net/ethernet/intel/i40e/i40e_adminq_cmd.h b/drivers/net/ethernet/intel/i40e/i40e_adminq_cmd.h
index 11cf1a5ebccf..67e396b2b347 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_adminq_cmd.h
+++ b/drivers/net/ethernet/intel/i40e/i40e_adminq_cmd.h
@@ -204,6 +204,9 @@ enum i40e_admin_queue_opc {
 	i40e_aqc_opc_suspend_port_tx				= 0x041B,
 	i40e_aqc_opc_resume_port_tx				= 0x041C,
 	i40e_aqc_opc_configure_partition_bw			= 0x041D,
+	/* hmc */
+	i40e_aqc_opc_query_hmc_resource_profile	= 0x0500,
+	i40e_aqc_opc_set_hmc_resource_profile	= 0x0501,
 
 	/* phy commands*/
 	i40e_aqc_opc_get_phy_abilities		= 0x0600,
@@ -450,13 +453,15 @@ I40E_CHECK_CMD_LENGTH(i40e_aqc_cppm_configuration);
 /* Set ARP Proxy command / response (indirect 0x0104) */
 struct i40e_aqc_arp_proxy_data {
 	__le16	command_flags;
-#define I40E_AQ_ARP_INIT_IPV4	0x0008
-#define I40E_AQ_ARP_UNSUP_CTL	0x0010
-#define I40E_AQ_ARP_ENA		0x0020
-#define I40E_AQ_ARP_ADD_IPV4	0x0040
-#define I40E_AQ_ARP_DEL_IPV4	0x0080
+#define I40E_AQ_ARP_INIT_IPV4	0x0800
+#define I40E_AQ_ARP_UNSUP_CTL	0x1000
+#define I40E_AQ_ARP_ENA		0x2000
+#define I40E_AQ_ARP_ADD_IPV4	0x4000
+#define I40E_AQ_ARP_DEL_IPV4	0x8000
 	__le16	table_id;
-	__le32	pfpm_proxyfc;
+	__le32	enabled_offloads;
+#define I40E_AQ_ARP_DIRECTED_OFFLOAD_ENABLE	0x00000020
+#define I40E_AQ_ARP_OFFLOAD_ENABLE		0x00000800
 	__le32	ip_addr;
 	u8	mac_addr[6];
 	u8	reserved[2];
@@ -471,17 +476,19 @@ struct i40e_aqc_ns_proxy_data {
 	__le16	table_idx_ipv6_0;
 	__le16	table_idx_ipv6_1;
 	__le16	control;
-#define I40E_AQ_NS_PROXY_ADD_0		0x0100
-#define I40E_AQ_NS_PROXY_DEL_0		0x0200
-#define I40E_AQ_NS_PROXY_ADD_1		0x0400
-#define I40E_AQ_NS_PROXY_DEL_1		0x0800
-#define I40E_AQ_NS_PROXY_ADD_IPV6_0	0x1000
-#define I40E_AQ_NS_PROXY_DEL_IPV6_0	0x2000
-#define I40E_AQ_NS_PROXY_ADD_IPV6_1	0x4000
-#define I40E_AQ_NS_PROXY_DEL_IPV6_1	0x8000
-#define I40E_AQ_NS_PROXY_COMMAND_SEQ	0x0001
-#define I40E_AQ_NS_PROXY_INIT_IPV6_TBL	0x0002
-#define I40E_AQ_NS_PROXY_INIT_MAC_TBL	0x0004
+#define I40E_AQ_NS_PROXY_ADD_0		0x0001
+#define I40E_AQ_NS_PROXY_DEL_0		0x0002
+#define I40E_AQ_NS_PROXY_ADD_1		0x0004
+#define I40E_AQ_NS_PROXY_DEL_1		0x0008
+#define I40E_AQ_NS_PROXY_ADD_IPV6_0	0x0010
+#define I40E_AQ_NS_PROXY_DEL_IPV6_0	0x0020
+#define I40E_AQ_NS_PROXY_ADD_IPV6_1	0x0040
+#define I40E_AQ_NS_PROXY_DEL_IPV6_1	0x0080
+#define I40E_AQ_NS_PROXY_COMMAND_SEQ	0x0100
+#define I40E_AQ_NS_PROXY_INIT_IPV6_TBL	0x0200
+#define I40E_AQ_NS_PROXY_INIT_MAC_TBL	0x0400
+#define I40E_AQ_NS_PROXY_OFFLOAD_ENABLE	0x0800
+#define I40E_AQ_NS_PROXY_DIRECTED_OFFLOAD_ENABLE	0x1000
 	u8	mac_addr_0[6];
 	u8	mac_addr_1[6];
 	u8	local_mac_addr[6];
@@ -1582,6 +1589,24 @@ struct i40e_aqc_configure_partition_bw_data {
 
 I40E_CHECK_STRUCT_LEN(0x22, i40e_aqc_configure_partition_bw_data);
 
+/* Get and set the active HMC resource profile and status.
+ * (direct 0x0500) and (direct 0x0501)
+ */
+struct i40e_aq_get_set_hmc_resource_profile {
+	u8	pm_profile;
+	u8	pe_vf_enabled;
+	u8	reserved[14];
+};
+
+I40E_CHECK_CMD_LENGTH(i40e_aq_get_set_hmc_resource_profile);
+
+enum i40e_aq_hmc_profile {
+	/* I40E_HMC_PROFILE_NO_CHANGE	= 0, reserved */
+	I40E_HMC_PROFILE_DEFAULT	= 1,
+	I40E_HMC_PROFILE_FAVOR_VF	= 2,
+	I40E_HMC_PROFILE_EQUAL		= 3,
+};
+
 /* Get PHY Abilities (indirect 0x0600) uses the generic indirect struct */
 
 /* set in param0 for get phy abilities to report qualified modules */
diff --git a/drivers/net/ethernet/intel/i40e/i40e_client.c b/drivers/net/ethernet/intel/i40e/i40e_client.c
index 618f18436618..250db0b244b7 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_client.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_client.c
@@ -148,6 +148,11 @@ i40e_notify_client_of_vf_msg(struct i40e_vsi *vsi, u32 vf_id, u8 *msg, u16 len)
 					"Cannot locate client instance virtual channel receive routine\n");
 				continue;
 			}
+			if (!test_bit(__I40E_CLIENT_INSTANCE_OPENED,
+				      &cdev->state)) {
+				dev_dbg(&vsi->back->pdev->dev, "Client is not open, abort virtchnl_receive\n");
+				continue;
+			}
 			cdev->client->ops->virtchnl_receive(&cdev->lan_info,
 							    cdev->client,
 							    vf_id, msg, len);
@@ -181,6 +186,11 @@ void i40e_notify_client_of_l2_param_changes(struct i40e_vsi *vsi)
 					"Cannot locate client instance l2_param_change routine\n");
 				continue;
 			}
+			if (!test_bit(__I40E_CLIENT_INSTANCE_OPENED,
+				      &cdev->state)) {
+				dev_dbg(&vsi->back->pdev->dev, "Client is not open, abort l2 param change\n");
+				continue;
+			}
 			cdev->lan_info.params = params;
 			cdev->client->ops->l2_param_change(&cdev->lan_info,
 							   cdev->client,
@@ -306,6 +316,11 @@ void i40e_notify_client_of_vf_reset(struct i40e_pf *pf, u32 vf_id)
 					"Cannot locate client instance VF reset routine\n");
 				continue;
 			}
+			if (!test_bit(__I40E_CLIENT_INSTANCE_OPENED,
+				      &cdev->state)) {
+				dev_dbg(&pf->pdev->dev, "Client is not open, abort vf-reset\n");
+				continue;
+			}
 			cdev->client->ops->vf_reset(&cdev->lan_info,
 						    cdev->client, vf_id);
 		}
@@ -336,6 +351,11 @@ void i40e_notify_client_of_vf_enable(struct i40e_pf *pf, u32 num_vfs)
 					"Cannot locate client instance VF enable routine\n");
 				continue;
 			}
+			if (!test_bit(__I40E_CLIENT_INSTANCE_OPENED,
+				      &cdev->state)) {
+				dev_dbg(&pf->pdev->dev, "Client is not open, abort vf-enable\n");
+				continue;
+			}
 			cdev->client->ops->vf_enable(&cdev->lan_info,
 						     cdev->client, num_vfs);
 		}
@@ -370,6 +390,11 @@ int i40e_vf_client_capable(struct i40e_pf *pf, u32 vf_id,
 					"Cannot locate client instance VF capability routine\n");
 				continue;
 			}
+			if (!test_bit(__I40E_CLIENT_INSTANCE_OPENED,
+				      &cdev->state)) {
+				dev_dbg(&pf->pdev->dev, "Client is not open, abort vf-capable\n");
+				continue;
+			}
 			capable = cdev->client->ops->vf_capable(&cdev->lan_info,
 								cdev->client,
 								vf_id);
@@ -559,6 +584,7 @@ void i40e_client_subtask(struct i40e_pf *pf)
 				 pf->hw.bus.device, pf->hw.bus.func);
 		}
 
+		mutex_lock(&i40e_client_instance_mutex);
 		/* Send an Open request to the client */
 		atomic_inc(&cdev->ref_cnt);
 		if (client->ops && client->ops->open)
@@ -568,10 +594,12 @@ void i40e_client_subtask(struct i40e_pf *pf)
 			set_bit(__I40E_CLIENT_INSTANCE_OPENED, &cdev->state);
 		} else {
 			/* remove client instance */
+			mutex_unlock(&i40e_client_instance_mutex);
 			i40e_client_del_instance(pf, client);
 			atomic_dec(&client->ref_cnt);
 			continue;
 		}
+		mutex_unlock(&i40e_client_instance_mutex);
 	}
 	mutex_unlock(&i40e_client_mutex);
 }
@@ -654,7 +682,7 @@ int i40e_lan_del_device(struct i40e_pf *pf)
 static int i40e_client_release(struct i40e_client *client)
 {
 	struct i40e_client_instance *cdev, *tmp;
-	struct i40e_pf *pf = NULL;
+	struct i40e_pf *pf;
 	int ret = 0;
 
 	LIST_HEAD(cdevs_tmp);
@@ -664,12 +692,12 @@ static int i40e_client_release(struct i40e_client *client)
 		if (strncmp(cdev->client->name, client->name,
 			    I40E_CLIENT_STR_LENGTH))
 			continue;
+		pf = (struct i40e_pf *)cdev->lan_info.pf;
 		if (test_bit(__I40E_CLIENT_INSTANCE_OPENED, &cdev->state)) {
 			if (atomic_read(&cdev->ref_cnt) > 0) {
 				ret = I40E_ERR_NOT_READY;
 				goto out;
 			}
-			pf = (struct i40e_pf *)cdev->lan_info.pf;
 			if (client->ops && client->ops->close)
 				client->ops->close(&cdev->lan_info, client,
 						   false);
@@ -681,8 +709,7 @@ static int i40e_client_release(struct i40e_client *client)
 				 client->name, pf->hw.pf_id);
 		}
 		/* delete the client instance from the list */
-		list_del(&cdev->list);
-		list_add(&cdev->list, &cdevs_tmp);
+		list_move(&cdev->list, &cdevs_tmp);
 		atomic_dec(&client->ref_cnt);
 		dev_info(&pf->pdev->dev, "Deleted client instance of Client %s\n",
 			 client->name);
@@ -811,7 +838,8 @@ static int i40e_client_setup_qvlist(struct i40e_info *ldev,
 			wr32(hw, I40E_PFINT_AEQCTL, reg);
 		}
 	}
-
+	/* Mitigate sync problems with iwarp VF driver */
+	i40e_flush(hw);
 	return 0;
 err:
 	kfree(ldev->qvlist_info);
@@ -1009,7 +1037,6 @@ int i40e_unregister_client(struct i40e_client *client)
 	if (!i40e_client_is_registered(client)) {
 		pr_info("i40e: Client %s has not been registered\n",
 			client->name);
-		mutex_unlock(&i40e_client_mutex);
 		ret = -ENODEV;
 		goto out;
 	}
diff --git a/drivers/net/ethernet/intel/i40e/i40e_client.h b/drivers/net/ethernet/intel/i40e/i40e_client.h
index a4601d97fb24..38a6c36a6a0e 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_client.h
+++ b/drivers/net/ethernet/intel/i40e/i40e_client.h
@@ -36,9 +36,9 @@
 #define I40E_CLIENT_VERSION_MINOR 01
 #define I40E_CLIENT_VERSION_BUILD 00
 #define I40E_CLIENT_VERSION_STR     \
-	XSTRINGIFY(I40E_CLIENT_VERSION_MAJOR) "." \
-	XSTRINGIFY(I40E_CLIENT_VERSION_MINOR) "." \
-	XSTRINGIFY(I40E_CLIENT_VERSION_BUILD)
+	__stringify(I40E_CLIENT_VERSION_MAJOR) "." \
+	__stringify(I40E_CLIENT_VERSION_MINOR) "." \
+	__stringify(I40E_CLIENT_VERSION_BUILD)
 
 struct i40e_client_version {
 	u8 major;
diff --git a/drivers/net/ethernet/intel/i40e/i40e_debugfs.c b/drivers/net/ethernet/intel/i40e/i40e_debugfs.c
index 05cf9a719bab..0c1875b5b16d 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_debugfs.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_debugfs.c
@@ -1054,6 +1054,7 @@ static ssize_t i40e_dbg_command_write(struct file *filp,
 			struct i40e_dcbx_config *r_cfg =
 						&pf->hw.remote_dcbx_config;
 			int i, ret;
+			u32 switch_id;
 
 			bw_data = kzalloc(sizeof(
 				    struct i40e_aqc_query_port_ets_config_resp),
@@ -1063,8 +1064,12 @@ static ssize_t i40e_dbg_command_write(struct file *filp,
 				goto command_write_done;
 			}
 
+			vsi = pf->vsi[pf->lan_vsi];
+			switch_id =
+				vsi->info.switch_id & I40E_AQ_VSI_SW_ID_MASK;
+
 			ret = i40e_aq_query_port_ets_config(&pf->hw,
-							    pf->mac_seid,
+							    switch_id,
 							    bw_data, NULL);
 			if (ret) {
 				dev_info(&pf->pdev->dev,
@@ -1425,84 +1430,6 @@ static ssize_t i40e_dbg_command_write(struct file *filp,
 		buff = NULL;
 		kfree(desc);
 		desc = NULL;
-	} else if ((strncmp(cmd_buf, "add fd_filter", 13) == 0) ||
-		   (strncmp(cmd_buf, "rem fd_filter", 13) == 0)) {
-		struct i40e_fdir_filter fd_data;
-		u16 packet_len, i, j = 0;
-		char *asc_packet;
-		u8 *raw_packet;
-		bool add = false;
-		int ret;
-
-		if (!(pf->flags & I40E_FLAG_FD_SB_ENABLED))
-			goto command_write_done;
-
-		if (strncmp(cmd_buf, "add", 3) == 0)
-			add = true;
-
-		if (add && (pf->auto_disable_flags & I40E_FLAG_FD_SB_ENABLED))
-			goto command_write_done;
-
-		asc_packet = kzalloc(I40E_FDIR_MAX_RAW_PACKET_SIZE,
-				     GFP_KERNEL);
-		if (!asc_packet)
-			goto command_write_done;
-
-		raw_packet = kzalloc(I40E_FDIR_MAX_RAW_PACKET_SIZE,
-				     GFP_KERNEL);
-
-		if (!raw_packet) {
-			kfree(asc_packet);
-			asc_packet = NULL;
-			goto command_write_done;
-		}
-
-		cnt = sscanf(&cmd_buf[13],
-			     "%hx %2hhx %2hhx %hx %2hhx %2hhx %hx %x %hd %511s",
-			     &fd_data.q_index,
-			     &fd_data.flex_off, &fd_data.pctype,
-			     &fd_data.dest_vsi, &fd_data.dest_ctl,
-			     &fd_data.fd_status, &fd_data.cnt_index,
-			     &fd_data.fd_id, &packet_len, asc_packet);
-		if (cnt != 10) {
-			dev_info(&pf->pdev->dev,
-				 "program fd_filter: bad command string, cnt=%d\n",
-				 cnt);
-			kfree(asc_packet);
-			asc_packet = NULL;
-			kfree(raw_packet);
-			goto command_write_done;
-		}
-
-		/* fix packet length if user entered 0 */
-		if (packet_len == 0)
-			packet_len = I40E_FDIR_MAX_RAW_PACKET_SIZE;
-
-		/* make sure to check the max as well */
-		packet_len = min_t(u16,
-				   packet_len, I40E_FDIR_MAX_RAW_PACKET_SIZE);
-
-		for (i = 0; i < packet_len; i++) {
-			cnt = sscanf(&asc_packet[j], "%2hhx ", &raw_packet[i]);
-			if (!cnt)
-				break;
-			j += 3;
-		}
-		dev_info(&pf->pdev->dev, "FD raw packet dump\n");
-		print_hex_dump(KERN_INFO, "FD raw packet: ",
-			       DUMP_PREFIX_OFFSET, 16, 1,
-			       raw_packet, packet_len, true);
-		ret = i40e_program_fdir_filter(&fd_data, raw_packet, pf, add);
-		if (!ret) {
-			dev_info(&pf->pdev->dev, "Filter command send Status : Success\n");
-		} else {
-			dev_info(&pf->pdev->dev,
-				 "Filter command send failed %d\n", ret);
-		}
-		kfree(raw_packet);
-		raw_packet = NULL;
-		kfree(asc_packet);
-		asc_packet = NULL;
 	} else if (strncmp(cmd_buf, "fd current cnt", 14) == 0) {
 		dev_info(&pf->pdev->dev, "FD current total filter count for this interface: %d\n",
 			 i40e_get_current_fd_count(pf));
@@ -1727,8 +1654,6 @@ static ssize_t i40e_dbg_command_write(struct file *filp,
 		dev_info(&pf->pdev->dev, "  globr\n");
 		dev_info(&pf->pdev->dev, "  send aq_cmd <flags> <opcode> <datalen> <retval> <cookie_h> <cookie_l> <param0> <param1> <param2> <param3>\n");
 		dev_info(&pf->pdev->dev, "  send indirect aq_cmd <flags> <opcode> <datalen> <retval> <cookie_h> <cookie_l> <param0> <param1> <param2> <param3> <buffer_len>\n");
-		dev_info(&pf->pdev->dev, "  add fd_filter <dest q_index> <flex_off> <pctype> <dest_vsi> <dest_ctl> <fd_status> <cnt_index> <fd_id> <packet_len> <packet>\n");
-		dev_info(&pf->pdev->dev, "  rem fd_filter <dest q_index> <flex_off> <pctype> <dest_vsi> <dest_ctl> <fd_status> <cnt_index> <fd_id> <packet_len> <packet>\n");
 		dev_info(&pf->pdev->dev, "  fd current cnt");
 		dev_info(&pf->pdev->dev, "  lldp start\n");
 		dev_info(&pf->pdev->dev, "  lldp stop\n");
diff --git a/drivers/net/ethernet/intel/i40e/i40e_ethtool.c b/drivers/net/ethernet/intel/i40e/i40e_ethtool.c
index c912e041d102..92bc8846f1ba 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_ethtool.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_ethtool.c
@@ -1560,13 +1560,13 @@ static void i40e_get_strings(struct net_device *netdev, u32 stringset,
 		}
 #endif
 		for (i = 0; i < vsi->num_queue_pairs; i++) {
-			snprintf(p, ETH_GSTRING_LEN, "tx-%u.tx_packets", i);
+			snprintf(p, ETH_GSTRING_LEN, "tx-%d.tx_packets", i);
 			p += ETH_GSTRING_LEN;
-			snprintf(p, ETH_GSTRING_LEN, "tx-%u.tx_bytes", i);
+			snprintf(p, ETH_GSTRING_LEN, "tx-%d.tx_bytes", i);
 			p += ETH_GSTRING_LEN;
-			snprintf(p, ETH_GSTRING_LEN, "rx-%u.rx_packets", i);
+			snprintf(p, ETH_GSTRING_LEN, "rx-%d.rx_packets", i);
 			p += ETH_GSTRING_LEN;
-			snprintf(p, ETH_GSTRING_LEN, "rx-%u.rx_bytes", i);
+			snprintf(p, ETH_GSTRING_LEN, "rx-%d.rx_bytes", i);
 			p += ETH_GSTRING_LEN;
 		}
 		if (vsi != pf->vsi[pf->lan_vsi] || pf->hw.partition_id != 1)
@@ -1581,16 +1581,16 @@ static void i40e_get_strings(struct net_device *netdev, u32 stringset,
 			}
 			for (i = 0; i < I40E_MAX_TRAFFIC_CLASS; i++) {
 				snprintf(p, ETH_GSTRING_LEN,
-					 "veb.tc_%u_tx_packets", i);
+					 "veb.tc_%d_tx_packets", i);
 				p += ETH_GSTRING_LEN;
 				snprintf(p, ETH_GSTRING_LEN,
-					 "veb.tc_%u_tx_bytes", i);
+					 "veb.tc_%d_tx_bytes", i);
 				p += ETH_GSTRING_LEN;
 				snprintf(p, ETH_GSTRING_LEN,
-					 "veb.tc_%u_rx_packets", i);
+					 "veb.tc_%d_rx_packets", i);
 				p += ETH_GSTRING_LEN;
 				snprintf(p, ETH_GSTRING_LEN,
-					 "veb.tc_%u_rx_bytes", i);
+					 "veb.tc_%d_rx_bytes", i);
 				p += ETH_GSTRING_LEN;
 			}
 		}
@@ -1601,23 +1601,23 @@ static void i40e_get_strings(struct net_device *netdev, u32 stringset,
 		}
 		for (i = 0; i < I40E_MAX_USER_PRIORITY; i++) {
 			snprintf(p, ETH_GSTRING_LEN,
-				 "port.tx_priority_%u_xon", i);
+				 "port.tx_priority_%d_xon", i);
 			p += ETH_GSTRING_LEN;
 			snprintf(p, ETH_GSTRING_LEN,
-				 "port.tx_priority_%u_xoff", i);
+				 "port.tx_priority_%d_xoff", i);
 			p += ETH_GSTRING_LEN;
 		}
 		for (i = 0; i < I40E_MAX_USER_PRIORITY; i++) {
 			snprintf(p, ETH_GSTRING_LEN,
-				 "port.rx_priority_%u_xon", i);
+				 "port.rx_priority_%d_xon", i);
 			p += ETH_GSTRING_LEN;
 			snprintf(p, ETH_GSTRING_LEN,
-				 "port.rx_priority_%u_xoff", i);
+				 "port.rx_priority_%d_xoff", i);
 			p += ETH_GSTRING_LEN;
 		}
 		for (i = 0; i < I40E_MAX_USER_PRIORITY; i++) {
 			snprintf(p, ETH_GSTRING_LEN,
-				 "port.rx_priority_%u_xon_2_xoff", i);
+				 "port.rx_priority_%d_xon_2_xoff", i);
 			p += ETH_GSTRING_LEN;
 		}
 		/* BUG_ON(p - data != I40E_STATS_LEN * ETH_GSTRING_LEN); */
@@ -1970,11 +1970,22 @@ static int i40e_set_phys_id(struct net_device *netdev,
  * 125us (8000 interrupts per second) == ITR(62)
  */
 
+/**
+ * __i40e_get_coalesce - get per-queue coalesce settings
+ * @netdev: the netdev to check
+ * @ec: ethtool coalesce data structure
+ * @queue: which queue to pick
+ *
+ * Gets the per-queue settings for coalescence. Specifically Rx and Tx usecs
+ * are per queue. If queue is <0 then we default to queue 0 as the
+ * representative value.
+ **/
 static int __i40e_get_coalesce(struct net_device *netdev,
 			       struct ethtool_coalesce *ec,
 			       int queue)
 {
 	struct i40e_netdev_priv *np = netdev_priv(netdev);
+	struct i40e_ring *rx_ring, *tx_ring;
 	struct i40e_vsi *vsi = np->vsi;
 
 	ec->tx_max_coalesced_frames_irq = vsi->work_limit;
@@ -1989,14 +2000,18 @@ static int __i40e_get_coalesce(struct net_device *netdev,
 		return -EINVAL;
 	}
 
-	if (ITR_IS_DYNAMIC(vsi->rx_rings[queue]->rx_itr_setting))
+	rx_ring = vsi->rx_rings[queue];
+	tx_ring = vsi->tx_rings[queue];
+
+	if (ITR_IS_DYNAMIC(rx_ring->rx_itr_setting))
 		ec->use_adaptive_rx_coalesce = 1;
 
-	if (ITR_IS_DYNAMIC(vsi->tx_rings[queue]->tx_itr_setting))
+	if (ITR_IS_DYNAMIC(tx_ring->tx_itr_setting))
 		ec->use_adaptive_tx_coalesce = 1;
 
-	ec->rx_coalesce_usecs = vsi->rx_rings[queue]->rx_itr_setting & ~I40E_ITR_DYNAMIC;
-	ec->tx_coalesce_usecs = vsi->tx_rings[queue]->tx_itr_setting & ~I40E_ITR_DYNAMIC;
+	ec->rx_coalesce_usecs = rx_ring->rx_itr_setting & ~I40E_ITR_DYNAMIC;
+	ec->tx_coalesce_usecs = tx_ring->tx_itr_setting & ~I40E_ITR_DYNAMIC;
+
 
 	/* we use the _usecs_high to store/set the interrupt rate limit
 	 * that the hardware supports, that almost but not quite
@@ -2010,18 +2025,44 @@ static int __i40e_get_coalesce(struct net_device *netdev,
 	return 0;
 }
 
+/**
+ * i40e_get_coalesce - get a netdev's coalesce settings
+ * @netdev: the netdev to check
+ * @ec: ethtool coalesce data structure
+ *
+ * Gets the coalesce settings for a particular netdev. Note that if user has
+ * modified per-queue settings, this only guarantees to represent queue 0. See
+ * __i40e_get_coalesce for more details.
+ **/
 static int i40e_get_coalesce(struct net_device *netdev,
 			     struct ethtool_coalesce *ec)
 {
 	return __i40e_get_coalesce(netdev, ec, -1);
 }
 
+/**
+ * i40e_get_per_queue_coalesce - gets coalesce settings for particular queue
+ * @netdev: netdev structure
+ * @ec: ethtool's coalesce settings
+ * @queue: the particular queue to read
+ *
+ * Will read a specific queue's coalesce settings
+ **/
 static int i40e_get_per_queue_coalesce(struct net_device *netdev, u32 queue,
 				       struct ethtool_coalesce *ec)
 {
 	return __i40e_get_coalesce(netdev, ec, queue);
 }
 
+/**
+ * i40e_set_itr_per_queue - set ITR values for specific queue
+ * @vsi: the VSI to set values for
+ * @ec: coalesce settings from ethtool
+ * @queue: the queue to modify
+ *
+ * Change the ITR settings for a specific queue.
+ **/
+
 static void i40e_set_itr_per_queue(struct i40e_vsi *vsi,
 				   struct ethtool_coalesce *ec,
 				   int queue)
@@ -2060,6 +2101,14 @@ static void i40e_set_itr_per_queue(struct i40e_vsi *vsi,
 	i40e_flush(hw);
 }
 
+/**
+ * __i40e_set_coalesce - set coalesce settings for particular queue
+ * @netdev: the netdev to change
+ * @ec: ethtool coalesce settings
+ * @queue: the queue to change
+ *
+ * Sets the coalesce settings for a particular queue.
+ **/
 static int __i40e_set_coalesce(struct net_device *netdev,
 			       struct ethtool_coalesce *ec,
 			       int queue)
@@ -2120,12 +2169,27 @@ static int __i40e_set_coalesce(struct net_device *netdev,
 	return 0;
 }
 
+/**
+ * i40e_set_coalesce - set coalesce settings for every queue on the netdev
+ * @netdev: the netdev to change
+ * @ec: ethtool coalesce settings
+ *
+ * This will set each queue to the same coalesce settings.
+ **/
 static int i40e_set_coalesce(struct net_device *netdev,
 			     struct ethtool_coalesce *ec)
 {
 	return __i40e_set_coalesce(netdev, ec, -1);
 }
 
+/**
+ * i40e_set_per_queue_coalesce - set specific queue's coalesce settings
+ * @netdev: the netdev to change
+ * @ec: ethtool's coalesce settings
+ * @queue: the queue to change
+ *
+ * Sets the specified queue's coalesce settings.
+ **/
 static int i40e_set_per_queue_coalesce(struct net_device *netdev, u32 queue,
 				       struct ethtool_coalesce *ec)
 {
@@ -2141,41 +2205,72 @@ static int i40e_set_per_queue_coalesce(struct net_device *netdev, u32 queue,
  **/
 static int i40e_get_rss_hash_opts(struct i40e_pf *pf, struct ethtool_rxnfc *cmd)
 {
+	struct i40e_hw *hw = &pf->hw;
+	u8 flow_pctype = 0;
+	u64 i_set = 0;
+
 	cmd->data = 0;
 
-	if (pf->vsi[pf->lan_vsi]->rxnfc.data != 0) {
-		cmd->data = pf->vsi[pf->lan_vsi]->rxnfc.data;
-		cmd->flow_type = pf->vsi[pf->lan_vsi]->rxnfc.flow_type;
-		return 0;
-	}
-	/* Report default options for RSS on i40e */
 	switch (cmd->flow_type) {
 	case TCP_V4_FLOW:
+		flow_pctype = I40E_FILTER_PCTYPE_NONF_IPV4_TCP;
+		break;
 	case UDP_V4_FLOW:
-		cmd->data |= RXH_L4_B_0_1 | RXH_L4_B_2_3;
-	/* fall through to add IP fields */
+		flow_pctype = I40E_FILTER_PCTYPE_NONF_IPV4_UDP;
+		break;
+	case TCP_V6_FLOW:
+		flow_pctype = I40E_FILTER_PCTYPE_NONF_IPV6_TCP;
+		break;
+	case UDP_V6_FLOW:
+		flow_pctype = I40E_FILTER_PCTYPE_NONF_IPV6_UDP;
+		break;
 	case SCTP_V4_FLOW:
 	case AH_ESP_V4_FLOW:
 	case AH_V4_FLOW:
 	case ESP_V4_FLOW:
 	case IPV4_FLOW:
-		cmd->data |= RXH_IP_SRC | RXH_IP_DST;
-		break;
-	case TCP_V6_FLOW:
-	case UDP_V6_FLOW:
-		cmd->data |= RXH_L4_B_0_1 | RXH_L4_B_2_3;
-	/* fall through to add IP fields */
 	case SCTP_V6_FLOW:
 	case AH_ESP_V6_FLOW:
 	case AH_V6_FLOW:
 	case ESP_V6_FLOW:
 	case IPV6_FLOW:
+		/* Default is src/dest for IP, no matter the L4 hashing */
 		cmd->data |= RXH_IP_SRC | RXH_IP_DST;
 		break;
 	default:
 		return -EINVAL;
 	}
 
+	/* Read flow based hash input set register */
+	if (flow_pctype) {
+		i_set = (u64)i40e_read_rx_ctl(hw, I40E_GLQF_HASH_INSET(0,
+					      flow_pctype)) |
+			((u64)i40e_read_rx_ctl(hw, I40E_GLQF_HASH_INSET(1,
+					       flow_pctype)) << 32);
+	}
+
+	/* Process bits of hash input set */
+	if (i_set) {
+		if (i_set & I40E_L4_SRC_MASK)
+			cmd->data |= RXH_L4_B_0_1;
+		if (i_set & I40E_L4_DST_MASK)
+			cmd->data |= RXH_L4_B_2_3;
+
+		if (cmd->flow_type == TCP_V4_FLOW ||
+		    cmd->flow_type == UDP_V4_FLOW) {
+			if (i_set & I40E_L3_SRC_MASK)
+				cmd->data |= RXH_IP_SRC;
+			if (i_set & I40E_L3_DST_MASK)
+				cmd->data |= RXH_IP_DST;
+		} else if (cmd->flow_type == TCP_V6_FLOW ||
+			  cmd->flow_type == UDP_V6_FLOW) {
+			if (i_set & I40E_L3_V6_SRC_MASK)
+				cmd->data |= RXH_IP_SRC;
+			if (i_set & I40E_L3_V6_DST_MASK)
+				cmd->data |= RXH_IP_DST;
+		}
+	}
+
 	return 0;
 }
 
@@ -2318,6 +2413,51 @@ static int i40e_get_rxnfc(struct net_device *netdev, struct ethtool_rxnfc *cmd,
 }
 
 /**
+ * i40e_get_rss_hash_bits - Read RSS Hash bits from register
+ * @nfc: pointer to user request
+ * @i_setc bits currently set
+ *
+ * Returns value of bits to be set per user request
+ **/
+static u64 i40e_get_rss_hash_bits(struct ethtool_rxnfc *nfc, u64 i_setc)
+{
+	u64 i_set = i_setc;
+	u64 src_l3 = 0, dst_l3 = 0;
+
+	if (nfc->data & RXH_L4_B_0_1)
+		i_set |= I40E_L4_SRC_MASK;
+	else
+		i_set &= ~I40E_L4_SRC_MASK;
+	if (nfc->data & RXH_L4_B_2_3)
+		i_set |= I40E_L4_DST_MASK;
+	else
+		i_set &= ~I40E_L4_DST_MASK;
+
+	if (nfc->flow_type == TCP_V6_FLOW || nfc->flow_type == UDP_V6_FLOW) {
+		src_l3 = I40E_L3_V6_SRC_MASK;
+		dst_l3 = I40E_L3_V6_DST_MASK;
+	} else if (nfc->flow_type == TCP_V4_FLOW ||
+		  nfc->flow_type == UDP_V4_FLOW) {
+		src_l3 = I40E_L3_SRC_MASK;
+		dst_l3 = I40E_L3_DST_MASK;
+	} else {
+		/* Any other flow type are not supported here */
+		return i_set;
+	}
+
+	if (nfc->data & RXH_IP_SRC)
+		i_set |= src_l3;
+	else
+		i_set &= ~src_l3;
+	if (nfc->data & RXH_IP_DST)
+		i_set |= dst_l3;
+	else
+		i_set &= ~dst_l3;
+
+	return i_set;
+}
+
+/**
  * i40e_set_rss_hash_opt - Enable/Disable flow types for RSS hash
  * @pf: pointer to the physical function struct
  * @cmd: ethtool rxnfc command
@@ -2329,6 +2469,8 @@ static int i40e_set_rss_hash_opt(struct i40e_pf *pf, struct ethtool_rxnfc *nfc)
 	struct i40e_hw *hw = &pf->hw;
 	u64 hena = (u64)i40e_read_rx_ctl(hw, I40E_PFQF_HENA(0)) |
 		   ((u64)i40e_read_rx_ctl(hw, I40E_PFQF_HENA(1)) << 32);
+	u8 flow_pctype = 0;
+	u64 i_set, i_setc;
 
 	/* RSS does not support anything other than hashing
 	 * to queues on src and dst IPs and ports
@@ -2337,75 +2479,39 @@ static int i40e_set_rss_hash_opt(struct i40e_pf *pf, struct ethtool_rxnfc *nfc)
 			  RXH_L4_B_0_1 | RXH_L4_B_2_3))
 		return -EINVAL;
 
-	/* We need at least the IP SRC and DEST fields for hashing */
-	if (!(nfc->data & RXH_IP_SRC) ||
-	    !(nfc->data & RXH_IP_DST))
-		return -EINVAL;
-
 	switch (nfc->flow_type) {
 	case TCP_V4_FLOW:
-		switch (nfc->data & (RXH_L4_B_0_1 | RXH_L4_B_2_3)) {
-		case 0:
-			return -EINVAL;
-		case (RXH_L4_B_0_1 | RXH_L4_B_2_3):
-			if (pf->flags & I40E_FLAG_MULTIPLE_TCP_UDP_RSS_PCTYPE)
-				hena |=
-			   BIT_ULL(I40E_FILTER_PCTYPE_NONF_IPV4_TCP_SYN_NO_ACK);
-
-			hena |= BIT_ULL(I40E_FILTER_PCTYPE_NONF_IPV4_TCP);
-			break;
-		default:
-			return -EINVAL;
-		}
+		flow_pctype = I40E_FILTER_PCTYPE_NONF_IPV4_TCP;
+		if (pf->flags & I40E_FLAG_MULTIPLE_TCP_UDP_RSS_PCTYPE)
+			hena |=
+			  BIT_ULL(I40E_FILTER_PCTYPE_NONF_IPV4_TCP_SYN_NO_ACK);
 		break;
 	case TCP_V6_FLOW:
-		switch (nfc->data & (RXH_L4_B_0_1 | RXH_L4_B_2_3)) {
-		case 0:
-			return -EINVAL;
-		case (RXH_L4_B_0_1 | RXH_L4_B_2_3):
-			if (pf->flags & I40E_FLAG_MULTIPLE_TCP_UDP_RSS_PCTYPE)
-				hena |=
-			   BIT_ULL(I40E_FILTER_PCTYPE_NONF_IPV6_TCP_SYN_NO_ACK);
-
-			hena |= BIT_ULL(I40E_FILTER_PCTYPE_NONF_IPV6_TCP);
-			break;
-		default:
-			return -EINVAL;
-		}
+		flow_pctype = I40E_FILTER_PCTYPE_NONF_IPV6_TCP;
+		if (pf->flags & I40E_FLAG_MULTIPLE_TCP_UDP_RSS_PCTYPE)
+			hena |=
+			  BIT_ULL(I40E_FILTER_PCTYPE_NONF_IPV4_TCP_SYN_NO_ACK);
+		if (pf->flags & I40E_FLAG_MULTIPLE_TCP_UDP_RSS_PCTYPE)
+			hena |=
+			  BIT_ULL(I40E_FILTER_PCTYPE_NONF_IPV6_TCP_SYN_NO_ACK);
 		break;
 	case UDP_V4_FLOW:
-		switch (nfc->data & (RXH_L4_B_0_1 | RXH_L4_B_2_3)) {
-		case 0:
-			return -EINVAL;
-		case (RXH_L4_B_0_1 | RXH_L4_B_2_3):
-			if (pf->flags & I40E_FLAG_MULTIPLE_TCP_UDP_RSS_PCTYPE)
-				hena |=
-			    BIT_ULL(I40E_FILTER_PCTYPE_NONF_UNICAST_IPV4_UDP) |
-			    BIT_ULL(I40E_FILTER_PCTYPE_NONF_MULTICAST_IPV4_UDP);
-
-			hena |= (BIT_ULL(I40E_FILTER_PCTYPE_NONF_IPV4_UDP) |
-				 BIT_ULL(I40E_FILTER_PCTYPE_FRAG_IPV4));
-			break;
-		default:
-			return -EINVAL;
-		}
+		flow_pctype = I40E_FILTER_PCTYPE_NONF_IPV4_UDP;
+		if (pf->flags & I40E_FLAG_MULTIPLE_TCP_UDP_RSS_PCTYPE)
+			hena |=
+			  BIT_ULL(I40E_FILTER_PCTYPE_NONF_UNICAST_IPV4_UDP) |
+			  BIT_ULL(I40E_FILTER_PCTYPE_NONF_MULTICAST_IPV4_UDP);
+
+		hena |= BIT_ULL(I40E_FILTER_PCTYPE_FRAG_IPV4);
 		break;
 	case UDP_V6_FLOW:
-		switch (nfc->data & (RXH_L4_B_0_1 | RXH_L4_B_2_3)) {
-		case 0:
-			return -EINVAL;
-		case (RXH_L4_B_0_1 | RXH_L4_B_2_3):
-			if (pf->flags & I40E_FLAG_MULTIPLE_TCP_UDP_RSS_PCTYPE)
-				hena |=
-			    BIT_ULL(I40E_FILTER_PCTYPE_NONF_UNICAST_IPV6_UDP) |
-			    BIT_ULL(I40E_FILTER_PCTYPE_NONF_MULTICAST_IPV6_UDP);
-
-			hena |= (BIT_ULL(I40E_FILTER_PCTYPE_NONF_IPV6_UDP) |
-				 BIT_ULL(I40E_FILTER_PCTYPE_FRAG_IPV6));
-			break;
-		default:
-			return -EINVAL;
-		}
+		flow_pctype = I40E_FILTER_PCTYPE_NONF_IPV6_UDP;
+		if (pf->flags & I40E_FLAG_MULTIPLE_TCP_UDP_RSS_PCTYPE)
+			hena |=
+			  BIT_ULL(I40E_FILTER_PCTYPE_NONF_UNICAST_IPV6_UDP) |
+			  BIT_ULL(I40E_FILTER_PCTYPE_NONF_MULTICAST_IPV6_UDP);
+
+		hena |= BIT_ULL(I40E_FILTER_PCTYPE_FRAG_IPV6);
 		break;
 	case AH_ESP_V4_FLOW:
 	case AH_V4_FLOW:
@@ -2437,13 +2543,23 @@ static int i40e_set_rss_hash_opt(struct i40e_pf *pf, struct ethtool_rxnfc *nfc)
 		return -EINVAL;
 	}
 
+	if (flow_pctype) {
+		i_setc = (u64)i40e_read_rx_ctl(hw, I40E_GLQF_HASH_INSET(0,
+					       flow_pctype)) |
+			((u64)i40e_read_rx_ctl(hw, I40E_GLQF_HASH_INSET(1,
+					       flow_pctype)) << 32);
+		i_set = i40e_get_rss_hash_bits(nfc, i_setc);
+		i40e_write_rx_ctl(hw, I40E_GLQF_HASH_INSET(0, flow_pctype),
+				  (u32)i_set);
+		i40e_write_rx_ctl(hw, I40E_GLQF_HASH_INSET(1, flow_pctype),
+				  (u32)(i_set >> 32));
+		hena |= BIT_ULL(flow_pctype);
+	}
+
 	i40e_write_rx_ctl(hw, I40E_PFQF_HENA(0), (u32)hena);
 	i40e_write_rx_ctl(hw, I40E_PFQF_HENA(1), (u32)(hena >> 32));
 	i40e_flush(hw);
 
-	/* Save setting for future output/update */
-	pf->vsi[pf->lan_vsi]->rxnfc = *nfc;
-
 	return 0;
 }
 
@@ -2744,11 +2860,15 @@ static void i40e_get_channels(struct net_device *dev,
 static int i40e_set_channels(struct net_device *dev,
 			      struct ethtool_channels *ch)
 {
+	const u8 drop = I40E_FILTER_PROGRAM_DESC_DEST_DROP_PACKET;
 	struct i40e_netdev_priv *np = netdev_priv(dev);
 	unsigned int count = ch->combined_count;
 	struct i40e_vsi *vsi = np->vsi;
 	struct i40e_pf *pf = vsi->back;
+	struct i40e_fdir_filter *rule;
+	struct hlist_node *node2;
 	int new_count;
+	int err = 0;
 
 	/* We do not support setting channels for any other VSI at present */
 	if (vsi->type != I40E_VSI_MAIN)
@@ -2766,6 +2886,26 @@ static int i40e_set_channels(struct net_device *dev,
 	if (count > i40e_max_channels(vsi))
 		return -EINVAL;
 
+	/* verify that the number of channels does not invalidate any current
+	 * flow director rules
+	 */
+	hlist_for_each_entry_safe(rule, node2,
+				  &pf->fdir_filter_list, fdir_node) {
+		if (rule->dest_ctl != drop && count <= rule->q_index) {
+			dev_warn(&pf->pdev->dev,
+				 "Existing user defined filter %d assigns flow to queue %d\n",
+				 rule->fd_id, rule->q_index);
+			err = -EINVAL;
+		}
+	}
+
+	if (err) {
+		dev_err(&pf->pdev->dev,
+			"Existing filter rules must be deleted to reduce combined channel count to %d\n",
+			count);
+		return err;
+	}
+
 	/* update feature limits from largest to smallest supported values */
 	/* TODO: Flow director limit, DCB etc */
 
@@ -2846,15 +2986,13 @@ static int i40e_set_rxfh(struct net_device *netdev, const u32 *indir,
 {
 	struct i40e_netdev_priv *np = netdev_priv(netdev);
 	struct i40e_vsi *vsi = np->vsi;
+	struct i40e_pf *pf = vsi->back;
 	u8 *seed = NULL;
 	u16 i;
 
 	if (hfunc != ETH_RSS_HASH_NO_CHANGE && hfunc != ETH_RSS_HASH_TOP)
 		return -EOPNOTSUPP;
 
-	if (!indir)
-		return 0;
-
 	if (key) {
 		if (!vsi->rss_hkey_user) {
 			vsi->rss_hkey_user = kzalloc(I40E_HKEY_ARRAY_SIZE,
@@ -2872,8 +3010,12 @@ static int i40e_set_rxfh(struct net_device *netdev, const u32 *indir,
 	}
 
 	/* Each 32 bits pointed by 'indir' is stored with a lut entry */
-	for (i = 0; i < I40E_HLUT_ARRAY_SIZE; i++)
-		vsi->rss_lut_user[i] = (u8)(indir[i]);
+	if (indir)
+		for (i = 0; i < I40E_HLUT_ARRAY_SIZE; i++)
+			vsi->rss_lut_user[i] = (u8)(indir[i]);
+	else
+		i40e_fill_rss_lut(pf, vsi->rss_lut_user, I40E_HLUT_ARRAY_SIZE,
+				  vsi->rss_size);
 
 	return i40e_config_rss(vsi, seed, vsi->rss_lut_user,
 			       I40E_HLUT_ARRAY_SIZE);
@@ -2943,6 +3085,9 @@ static int i40e_set_priv_flags(struct net_device *dev, u32 flags)
 	} else {
 		pf->flags &= ~I40E_FLAG_FD_ATR_ENABLED;
 		pf->auto_disable_flags |= I40E_FLAG_FD_ATR_ENABLED;
+
+		/* flush current ATR settings */
+		set_bit(__I40E_FD_FLUSH_REQUESTED, &pf->state);
 	}
 
 	if ((flags & I40E_PRIV_FLAGS_VEB_STATS) &&
diff --git a/drivers/net/ethernet/intel/i40e/i40e_main.c b/drivers/net/ethernet/intel/i40e/i40e_main.c
index d0b3a1bb82ca..ac1faee2a5b8 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_main.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_main.c
@@ -41,7 +41,7 @@ static const char i40e_driver_string[] =
 
 #define DRV_VERSION_MAJOR 1
 #define DRV_VERSION_MINOR 6
-#define DRV_VERSION_BUILD 11
+#define DRV_VERSION_BUILD 16
 #define DRV_VERSION __stringify(DRV_VERSION_MAJOR) "." \
 	     __stringify(DRV_VERSION_MINOR) "." \
 	     __stringify(DRV_VERSION_BUILD)    DRV_KERN
@@ -57,8 +57,6 @@ static int i40e_setup_pf_switch(struct i40e_pf *pf, bool reinit);
 static int i40e_setup_misc_vector(struct i40e_pf *pf);
 static void i40e_determine_queue_usage(struct i40e_pf *pf);
 static int i40e_setup_pf_filter_control(struct i40e_pf *pf);
-static void i40e_fill_rss_lut(struct i40e_pf *pf, u8 *lut,
-			      u16 rss_table_size, u16 rss_size);
 static void i40e_fdir_sb_setup(struct i40e_pf *pf);
 static int i40e_veb_get_bw_info(struct i40e_veb *veb);
 
@@ -527,6 +525,7 @@ void i40e_pf_reset_stats(struct i40e_pf *pf)
 			pf->veb[i]->stat_offsets_loaded = false;
 		}
 	}
+	pf->hw_csum_rx_error = 0;
 }
 
 /**
@@ -1316,7 +1315,7 @@ static void i40e_rm_default_mac_filter(struct i40e_vsi *vsi, u8 *macaddr)
 	element.vlan_tag = 0;
 	/* ...and some firmware does it this way. */
 	element.flags = I40E_AQC_MACVLAN_DEL_PERFECT_MATCH |
-			I40E_AQC_MACVLAN_ADD_IGNORE_VLAN;
+			I40E_AQC_MACVLAN_DEL_IGNORE_VLAN;
 	i40e_aq_remove_macvlan(&pf->hw, vsi->seid, &element, 1, NULL);
 }
 
@@ -1909,7 +1908,7 @@ int i40e_sync_vsi_filters(struct i40e_vsi *vsi)
 			ether_addr_copy(del_list[num_del].mac_addr, f->macaddr);
 			if (f->vlan == I40E_VLAN_ANY) {
 				del_list[num_del].vlan_tag = 0;
-				cmd_flags |= I40E_AQC_MACVLAN_ADD_IGNORE_VLAN;
+				cmd_flags |= I40E_AQC_MACVLAN_DEL_IGNORE_VLAN;
 			} else {
 				del_list[num_del].vlan_tag =
 					cpu_to_le16((u16)(f->vlan));
@@ -4616,7 +4615,7 @@ static u8 i40e_dcb_get_enabled_tc(struct i40e_dcbx_config *dcbcfg)
 static u8 i40e_pf_get_num_tc(struct i40e_pf *pf)
 {
 	struct i40e_hw *hw = &pf->hw;
-	u8 i, enabled_tc;
+	u8 i, enabled_tc = 1;
 	u8 num_tc = 0;
 	struct i40e_dcbx_config *dcbcfg = &hw->local_dcbx_config;
 
@@ -4634,8 +4633,6 @@ static u8 i40e_pf_get_num_tc(struct i40e_pf *pf)
 	else
 		return 1; /* Only TC0 */
 
-	/* At least have TC0 */
-	enabled_tc = (enabled_tc ? enabled_tc : 0x1);
 	for (i = 0; i < I40E_MAX_TRAFFIC_CLASS; i++) {
 		if (enabled_tc & BIT(i))
 			num_tc++;
@@ -5245,7 +5242,7 @@ static int i40e_up_complete(struct i40e_vsi *vsi)
 		/* reset fd counters */
 		pf->fd_add_err = pf->fd_atr_cnt = 0;
 		if (pf->fd_tcp_rule > 0) {
-			pf->flags &= ~I40E_FLAG_FD_ATR_ENABLED;
+			pf->auto_disable_flags |= I40E_FLAG_FD_ATR_ENABLED;
 			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;
@@ -5942,13 +5939,17 @@ void i40e_fdir_check_and_reenable(struct i40e_pf *pf)
 				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 */
+
+	/* Wait for some more space to be available to turn on ATR. We also
+	 * must check that no existing ntuple rules for TCP are in effect
+	 */
 	if (fcnt_prog < (fcnt_avail - I40E_FDIR_BUFFER_HEAD_ROOM * 2)) {
 		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) &&
+		    (pf->fd_tcp_rule == 0)) {
 			pf->auto_disable_flags &= ~I40E_FLAG_FD_ATR_ENABLED;
 			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");
+				dev_info(&pf->pdev->dev, "ATR is being enabled since we have space in the table and there are no conflicting ntuple rules\n");
 		}
 	}
 
@@ -5979,9 +5980,6 @@ static void i40e_fdir_flush_and_replay(struct i40e_pf *pf)
 	int fd_room;
 	int reg;
 
-	if (!(pf->flags & (I40E_FLAG_FD_SB_ENABLED | I40E_FLAG_FD_ATR_ENABLED)))
-		return;
-
 	if (!time_after(jiffies, pf->fd_flush_timestamp +
 				 (I40E_MIN_FD_FLUSH_INTERVAL * HZ)))
 		return;
@@ -6001,7 +5999,7 @@ static void i40e_fdir_flush_and_replay(struct i40e_pf *pf)
 	}
 
 	pf->fd_flush_timestamp = jiffies;
-	pf->flags &= ~I40E_FLAG_FD_ATR_ENABLED;
+	pf->auto_disable_flags |= I40E_FLAG_FD_ATR_ENABLED;
 	/* flush all filters */
 	wr32(&pf->hw, I40E_PFQF_CTL_1,
 	     I40E_PFQF_CTL_1_CLEARFDTABLE_MASK);
@@ -6021,7 +6019,7 @@ static void i40e_fdir_flush_and_replay(struct i40e_pf *pf)
 		/* replay sideband filters */
 		i40e_fdir_filter_restore(pf->vsi[pf->lan_vsi]);
 		if (!disable_atr)
-			pf->flags |= I40E_FLAG_FD_ATR_ENABLED;
+			pf->auto_disable_flags &= ~I40E_FLAG_FD_ATR_ENABLED;
 		clear_bit(__I40E_FD_FLUSH_REQUESTED, &pf->state);
 		if (I40E_DEBUG_FD & pf->hw.debug_mask)
 			dev_info(&pf->pdev->dev, "FD Filter table flushed and FD-SB replayed.\n");
@@ -6055,9 +6053,6 @@ static void i40e_fdir_reinit_subtask(struct i40e_pf *pf)
 	if (test_bit(__I40E_DOWN, &pf->state))
 		return;
 
-	if (!(pf->flags & (I40E_FLAG_FD_SB_ENABLED | I40E_FLAG_FD_ATR_ENABLED)))
-		return;
-
 	if (test_bit(__I40E_FD_FLUSH_REQUESTED, &pf->state))
 		i40e_fdir_flush_and_replay(pf);
 
@@ -7157,9 +7152,9 @@ static void i40e_sync_udp_filters_subtask(struct i40e_pf *pf)
 			pf->pending_udp_bitmap &= ~BIT_ULL(i);
 			port = pf->udp_ports[i].index;
 			if (port)
-				ret = i40e_aq_add_udp_tunnel(hw, ntohs(port),
-						     pf->udp_ports[i].type,
-						     NULL, NULL);
+				ret = i40e_aq_add_udp_tunnel(hw, port,
+							pf->udp_ports[i].type,
+							NULL, NULL);
 			else
 				ret = i40e_aq_del_udp_tunnel(hw, i, NULL);
 
@@ -7646,7 +7641,6 @@ static int i40e_init_msix(struct i40e_pf *pf)
 			vectors_left--;
 		} else {
 			pf->num_fdsb_msix = 0;
-			pf->flags &= ~I40E_FLAG_FD_SB_ENABLED;
 		}
 	}
 
@@ -7666,6 +7660,8 @@ static int i40e_init_msix(struct i40e_pf *pf)
 #endif
 	/* can we reserve enough for iWARP? */
 	if (pf->flags & I40E_FLAG_IWARP_ENABLED) {
+		iwarp_requested = pf->num_iwarp_msix;
+
 		if (!vectors_left)
 			pf->num_iwarp_msix = 0;
 		else if (vectors_left < pf->num_iwarp_msix)
@@ -7679,18 +7675,23 @@ static int i40e_init_msix(struct i40e_pf *pf)
 		int vmdq_vecs_wanted = pf->num_vmdq_vsis * pf->num_vmdq_qps;
 		int vmdq_vecs = min_t(int, vectors_left, vmdq_vecs_wanted);
 
-		/* if we're short on vectors for what's desired, we limit
-		 * the queues per vmdq.  If this is still more than are
-		 * available, the user will need to change the number of
-		 * queues/vectors used by the PF later with the ethtool
-		 * channels command
-		 */
-		if (vmdq_vecs < vmdq_vecs_wanted)
-			pf->num_vmdq_qps = 1;
-		pf->num_vmdq_msix = pf->num_vmdq_qps;
+		if (!vectors_left) {
+			pf->num_vmdq_msix = 0;
+			pf->num_vmdq_qps = 0;
+		} else {
+			/* if we're short on vectors for what's desired, we limit
+			 * the queues per vmdq.  If this is still more than are
+			 * available, the user will need to change the number of
+			 * queues/vectors used by the PF later with the ethtool
+			 * channels command
+			 */
+			if (vmdq_vecs < vmdq_vecs_wanted)
+				pf->num_vmdq_qps = 1;
+			pf->num_vmdq_msix = pf->num_vmdq_qps;
 
-		v_budget += vmdq_vecs;
-		vectors_left -= vmdq_vecs;
+			v_budget += vmdq_vecs;
+			vectors_left -= vmdq_vecs;
+		}
 	}
 
 	pf->msix_entries = kcalloc(v_budget, sizeof(struct msix_entry),
@@ -7702,21 +7703,6 @@ static int i40e_init_msix(struct i40e_pf *pf)
 		pf->msix_entries[i].entry = i;
 	v_actual = i40e_reserve_msix_vectors(pf, v_budget);
 
-	if (v_actual != v_budget) {
-		/* If we have limited resources, we will start with no vectors
-		 * for the special features and then allocate vectors to some
-		 * of these features based on the policy and at the end disable
-		 * the features that did not get any vectors.
-		 */
-		iwarp_requested = pf->num_iwarp_msix;
-		pf->num_iwarp_msix = 0;
-#ifdef I40E_FCOE
-		pf->num_fcoe_qps = 0;
-		pf->num_fcoe_msix = 0;
-#endif
-		pf->num_vmdq_msix = 0;
-	}
-
 	if (v_actual < I40E_MIN_MSIX) {
 		pf->flags &= ~I40E_FLAG_MSIX_ENABLED;
 		kfree(pf->msix_entries);
@@ -7730,9 +7716,16 @@ static int i40e_init_msix(struct i40e_pf *pf)
 		pf->num_lan_qps = 1;
 		pf->num_lan_msix = 1;
 
-	} else if (v_actual != v_budget) {
+	} else if (!vectors_left) {
+		/* If we have limited resources, we will start with no vectors
+		 * for the special features and then allocate vectors to some
+		 * of these features based on the policy and at the end disable
+		 * the features that did not get any vectors.
+		 */
 		int vec;
 
+		dev_info(&pf->pdev->dev,
+			 "MSI-X vector limit reached, attempting to redistribute vectors\n");
 		/* reserve the misc vector */
 		vec = v_actual - 1;
 
@@ -7740,7 +7733,10 @@ static int i40e_init_msix(struct i40e_pf *pf)
 		pf->num_vmdq_msix = 1;    /* force VMDqs to only one vector */
 		pf->num_vmdq_vsis = 1;
 		pf->num_vmdq_qps = 1;
-		pf->flags &= ~I40E_FLAG_FD_SB_ENABLED;
+#ifdef I40E_FCOE
+		pf->num_fcoe_qps = 0;
+		pf->num_fcoe_msix = 0;
+#endif
 
 		/* partition out the remaining vectors */
 		switch (vec) {
@@ -7772,9 +7768,14 @@ static int i40e_init_msix(struct i40e_pf *pf)
 				pf->num_vmdq_vsis = min_t(int, (vec / 2),
 						  I40E_DEFAULT_NUM_VMDQ_VSI);
 			}
+			if (pf->flags & I40E_FLAG_FD_SB_ENABLED) {
+				pf->num_fdsb_msix = 1;
+				vec--;
+			}
 			pf->num_lan_msix = min_t(int,
 			       (vec - (pf->num_iwarp_msix + pf->num_vmdq_vsis)),
 							      pf->num_lan_msix);
+			pf->num_lan_qps = pf->num_lan_msix;
 #ifdef I40E_FCOE
 			/* give one vector to FCoE */
 			if (pf->flags & I40E_FLAG_FCOE_ENABLED) {
@@ -7786,6 +7787,11 @@ static int i40e_init_msix(struct i40e_pf *pf)
 		}
 	}
 
+	if ((pf->flags & I40E_FLAG_FD_SB_ENABLED) &&
+	    (pf->num_fdsb_msix == 0)) {
+		dev_info(&pf->pdev->dev, "Sideband Flowdir disabled, not enough MSI-X vectors\n");
+		pf->flags &= ~I40E_FLAG_FD_SB_ENABLED;
+	}
 	if ((pf->flags & I40E_FLAG_VMDQ_ENABLED) &&
 	    (pf->num_vmdq_msix == 0)) {
 		dev_info(&pf->pdev->dev, "VMDq disabled, not enough MSI-X vectors\n");
@@ -7804,6 +7810,13 @@ static int i40e_init_msix(struct i40e_pf *pf)
 		pf->flags &= ~I40E_FLAG_FCOE_ENABLED;
 	}
 #endif
+	i40e_debug(&pf->hw, I40E_DEBUG_INIT,
+		   "MSI-X vector distribution: PF %d, VMDq %d, FDSB %d, iWARP %d\n",
+		   pf->num_lan_msix,
+		   pf->num_vmdq_msix * pf->num_vmdq_vsis,
+		   pf->num_fdsb_msix,
+		   pf->num_iwarp_msix);
+
 	return v_actual;
 }
 
@@ -7990,72 +8003,34 @@ static int i40e_setup_misc_vector(struct i40e_pf *pf)
 static int i40e_config_rss_aq(struct i40e_vsi *vsi, const u8 *seed,
 			      u8 *lut, u16 lut_size)
 {
-	struct i40e_aqc_get_set_rss_key_data rss_key;
 	struct i40e_pf *pf = vsi->back;
 	struct i40e_hw *hw = &pf->hw;
-	bool pf_lut = false;
-	u8 *rss_lut;
-	int ret, i;
-
-	memcpy(&rss_key, seed, sizeof(rss_key));
-
-	rss_lut = kzalloc(pf->rss_table_size, GFP_KERNEL);
-	if (!rss_lut)
-		return -ENOMEM;
-
-	/* Populate the LUT with max no. of queues in round robin fashion */
-	for (i = 0; i < vsi->rss_table_size; i++)
-		rss_lut[i] = i % vsi->rss_size;
+	int ret = 0;
 
-	ret = i40e_aq_set_rss_key(hw, vsi->id, &rss_key);
-	if (ret) {
-		dev_info(&pf->pdev->dev,
-			 "Cannot set RSS key, err %s aq_err %s\n",
-			 i40e_stat_str(&pf->hw, ret),
-			 i40e_aq_str(&pf->hw, pf->hw.aq.asq_last_status));
-		goto config_rss_aq_out;
+	if (seed) {
+		struct i40e_aqc_get_set_rss_key_data *seed_dw =
+			(struct i40e_aqc_get_set_rss_key_data *)seed;
+		ret = i40e_aq_set_rss_key(hw, vsi->id, seed_dw);
+		if (ret) {
+			dev_info(&pf->pdev->dev,
+				 "Cannot set RSS key, err %s aq_err %s\n",
+				 i40e_stat_str(hw, ret),
+				 i40e_aq_str(hw, hw->aq.asq_last_status));
+			return ret;
+		}
 	}
+	if (lut) {
+		bool pf_lut = vsi->type == I40E_VSI_MAIN ? true : false;
 
-	if (vsi->type == I40E_VSI_MAIN)
-		pf_lut = true;
-
-	ret = i40e_aq_set_rss_lut(hw, vsi->id, pf_lut, rss_lut,
-				  vsi->rss_table_size);
-	if (ret)
-		dev_info(&pf->pdev->dev,
-			 "Cannot set RSS lut, err %s aq_err %s\n",
-			 i40e_stat_str(&pf->hw, ret),
-			 i40e_aq_str(&pf->hw, pf->hw.aq.asq_last_status));
-
-config_rss_aq_out:
-	kfree(rss_lut);
-	return ret;
-}
-
-/**
- * i40e_vsi_config_rss - Prepare for VSI(VMDq) RSS if used
- * @vsi: VSI structure
- **/
-static int i40e_vsi_config_rss(struct i40e_vsi *vsi)
-{
-	u8 seed[I40E_HKEY_ARRAY_SIZE];
-	struct i40e_pf *pf = vsi->back;
-	u8 *lut;
-	int ret;
-
-	if (!(pf->flags & I40E_FLAG_RSS_AQ_CAPABLE))
-		return 0;
-
-	lut = kzalloc(vsi->rss_table_size, GFP_KERNEL);
-	if (!lut)
-		return -ENOMEM;
-
-	i40e_fill_rss_lut(pf, lut, vsi->rss_table_size, vsi->rss_size);
-	netdev_rss_key_fill((void *)seed, I40E_HKEY_ARRAY_SIZE);
-	vsi->rss_size = min_t(int, pf->alloc_rss_size, vsi->num_queue_pairs);
-	ret = i40e_config_rss_aq(vsi, seed, lut, vsi->rss_table_size);
-	kfree(lut);
-
+		ret = i40e_aq_set_rss_lut(hw, vsi->id, pf_lut, lut, lut_size);
+		if (ret) {
+			dev_info(&pf->pdev->dev,
+				 "Cannot set RSS lut, err %s aq_err %s\n",
+				 i40e_stat_str(hw, ret),
+				 i40e_aq_str(hw, hw->aq.asq_last_status));
+			return ret;
+		}
+	}
 	return ret;
 }
 
@@ -8106,6 +8081,46 @@ static int i40e_get_rss_aq(struct i40e_vsi *vsi, const u8 *seed,
 }
 
 /**
+ * i40e_vsi_config_rss - Prepare for VSI(VMDq) RSS if used
+ * @vsi: VSI structure
+ **/
+static int i40e_vsi_config_rss(struct i40e_vsi *vsi)
+{
+	u8 seed[I40E_HKEY_ARRAY_SIZE];
+	struct i40e_pf *pf = vsi->back;
+	u8 *lut;
+	int ret;
+
+	if (!(pf->flags & I40E_FLAG_RSS_AQ_CAPABLE))
+		return 0;
+
+	if (!vsi->rss_size)
+		vsi->rss_size = min_t(int, pf->alloc_rss_size,
+				      vsi->num_queue_pairs);
+	if (!vsi->rss_size)
+		return -EINVAL;
+
+	lut = kzalloc(vsi->rss_table_size, GFP_KERNEL);
+	if (!lut)
+		return -ENOMEM;
+	/* Use the user configured hash keys and lookup table if there is one,
+	 * otherwise use default
+	 */
+	if (vsi->rss_lut_user)
+		memcpy(lut, vsi->rss_lut_user, vsi->rss_table_size);
+	else
+		i40e_fill_rss_lut(pf, lut, vsi->rss_table_size, vsi->rss_size);
+	if (vsi->rss_hkey_user)
+		memcpy(seed, vsi->rss_hkey_user, I40E_HKEY_ARRAY_SIZE);
+	else
+		netdev_rss_key_fill((void *)seed, I40E_HKEY_ARRAY_SIZE);
+	ret = i40e_config_rss_aq(vsi, seed, lut, vsi->rss_table_size);
+	kfree(lut);
+
+	return ret;
+}
+
+/**
  * i40e_config_rss_reg - Configure RSS keys and lut by writing registers
  * @vsi: Pointer to vsi structure
  * @seed: RSS hash seed
@@ -8243,8 +8258,8 @@ int i40e_get_rss(struct i40e_vsi *vsi, u8 *seed, u8 *lut, u16 lut_size)
  * @rss_table_size: Lookup table size
  * @rss_size: Range of queue number for hashing
  */
-static void i40e_fill_rss_lut(struct i40e_pf *pf, u8 *lut,
-			      u16 rss_table_size, u16 rss_size)
+void i40e_fill_rss_lut(struct i40e_pf *pf, u8 *lut,
+		       u16 rss_table_size, u16 rss_size)
 {
 	u16 i;
 
@@ -8285,6 +8300,8 @@ static int i40e_pf_config_rss(struct i40e_pf *pf)
 	if (!vsi->rss_size)
 		vsi->rss_size = min_t(int, pf->alloc_rss_size,
 				      vsi->num_queue_pairs);
+	if (!vsi->rss_size)
+		return -EINVAL;
 
 	lut = kzalloc(vsi->rss_table_size, GFP_KERNEL);
 	if (!lut)
@@ -8609,7 +8626,6 @@ static int i40e_sw_init(struct i40e_pf *pf)
 			     I40E_FLAG_WB_ON_ITR_CAPABLE |
 			     I40E_FLAG_MULTIPLE_TCP_UDP_RSS_PCTYPE |
 			     I40E_FLAG_NO_PCI_LINK_CHECK |
-			     I40E_FLAG_100M_SGMII_CAPABLE |
 			     I40E_FLAG_USE_SET_LLDP_MIB |
 			     I40E_FLAG_GENEVE_OFFLOAD_CAPABLE;
 	} else if ((pf->hw.aq.api_maj_ver > 1) ||
@@ -8684,18 +8700,40 @@ bool i40e_set_ntuple(struct i40e_pf *pf, netdev_features_t features)
 		/* reset fd counters */
 		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;
-		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))
+		    (pf->auto_disable_flags & I40E_FLAG_FD_ATR_ENABLED)) {
 			pf->auto_disable_flags &= ~I40E_FLAG_FD_ATR_ENABLED;
+			if (I40E_DEBUG_FD & pf->hw.debug_mask)
+				dev_info(&pf->pdev->dev, "ATR re-enabled.\n");
+		}
 	}
 	return need_reset;
 }
 
 /**
+ * i40e_clear_rss_lut - clear the rx hash lookup table
+ * @vsi: the VSI being configured
+ **/
+static void i40e_clear_rss_lut(struct i40e_vsi *vsi)
+{
+	struct i40e_pf *pf = vsi->back;
+	struct i40e_hw *hw = &pf->hw;
+	u16 vf_id = vsi->vf_id;
+	u8 i;
+
+	if (vsi->type == I40E_VSI_MAIN) {
+		for (i = 0; i <= I40E_PFQF_HLUT_MAX_INDEX; i++)
+			wr32(hw, I40E_PFQF_HLUT(i), 0);
+	} else if (vsi->type == I40E_VSI_SRIOV) {
+		for (i = 0; i <= I40E_VFQF_HLUT_MAX_INDEX; i++)
+			i40e_write_rx_ctl(hw, I40E_VFQF_HLUT1(i, vf_id), 0);
+	} else {
+		dev_err(&pf->pdev->dev, "Cannot set RSS LUT - invalid VSI type\n");
+	}
+}
+
+/**
  * i40e_set_features - set the netdev feature flags
  * @netdev: ptr to the netdev being adjusted
  * @features: the feature set that the stack is suggesting
@@ -8708,6 +8746,12 @@ static int i40e_set_features(struct net_device *netdev,
 	struct i40e_pf *pf = vsi->back;
 	bool need_reset;
 
+	if (features & NETIF_F_RXHASH && !(netdev->features & NETIF_F_RXHASH))
+		i40e_pf_config_rss(pf);
+	else if (!(features & NETIF_F_RXHASH) &&
+		 netdev->features & NETIF_F_RXHASH)
+		i40e_clear_rss_lut(vsi);
+
 	if (features & NETIF_F_HW_VLAN_CTAG_RX)
 		i40e_vlan_stripping_enable(vsi);
 	else
@@ -11309,11 +11353,7 @@ static void i40e_remove(struct pci_dev *pdev)
 	}
 
 	/* shutdown the adminq */
-	ret_code = i40e_shutdown_adminq(hw);
-	if (ret_code)
-		dev_warn(&pdev->dev,
-			 "Failed to destroy the Admin Queue resources: %d\n",
-			 ret_code);
+	i40e_shutdown_adminq(hw);
 
 	/* destroy the locks only once, here */
 	mutex_destroy(&hw->aq.arq_mutex);
@@ -11360,6 +11400,12 @@ static pci_ers_result_t i40e_pci_error_detected(struct pci_dev *pdev,
 
 	dev_info(&pdev->dev, "%s: error %d\n", __func__, error);
 
+	if (!pf) {
+		dev_info(&pdev->dev,
+			 "Cannot recover - error happened during device probe\n");
+		return PCI_ERS_RESULT_DISCONNECT;
+	}
+
 	/* shutdown all operations */
 	if (!test_bit(__I40E_SUSPENDED, &pf->state)) {
 		rtnl_lock();
@@ -11582,7 +11628,8 @@ static int __init i40e_init_module(void)
 	 * it can't be any worse than using the system workqueue which
 	 * was already single threaded
 	 */
-	i40e_wq = create_singlethread_workqueue(i40e_driver_name);
+	i40e_wq = alloc_workqueue("%s", WQ_UNBOUND | WQ_MEM_RECLAIM, 1,
+				  i40e_driver_name);
 	if (!i40e_wq) {
 		pr_err("%s: Failed to create workqueue\n", i40e_driver_name);
 		return -ENOMEM;
diff --git a/drivers/net/ethernet/intel/i40e/i40e_ptp.c b/drivers/net/ethernet/intel/i40e/i40e_ptp.c
index ed39cbad24bd..f1feceab758a 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_ptp.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_ptp.c
@@ -669,7 +669,7 @@ void i40e_ptp_init(struct i40e_pf *pf)
 		pf->ptp_clock = NULL;
 		dev_err(&pf->pdev->dev, "%s: ptp_clock_register failed\n",
 			__func__);
-	} else {
+	} else if (pf->ptp_clock) {
 		struct timespec64 ts;
 		u32 regval;
 
diff --git a/drivers/net/ethernet/intel/i40e/i40e_txrx.c b/drivers/net/ethernet/intel/i40e/i40e_txrx.c
index df7ecc9578c9..6287bf63c43c 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_txrx.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_txrx.c
@@ -40,6 +40,69 @@ static inline __le64 build_ctob(u32 td_cmd, u32 td_offset, unsigned int size,
 }
 
 #define I40E_TXD_CMD (I40E_TX_DESC_CMD_EOP | I40E_TX_DESC_CMD_RS)
+/**
+ * i40e_fdir - Generate a Flow Director descriptor based on fdata
+ * @tx_ring: Tx ring to send buffer on
+ * @fdata: Flow director filter data
+ * @add: Indicate if we are adding a rule or deleting one
+ *
+ **/
+static void i40e_fdir(struct i40e_ring *tx_ring,
+		      struct i40e_fdir_filter *fdata, bool add)
+{
+	struct i40e_filter_program_desc *fdir_desc;
+	struct i40e_pf *pf = tx_ring->vsi->back;
+	u32 flex_ptype, dtype_cmd;
+	u16 i;
+
+	/* grab the next descriptor */
+	i = tx_ring->next_to_use;
+	fdir_desc = I40E_TX_FDIRDESC(tx_ring, i);
+
+	i++;
+	tx_ring->next_to_use = (i < tx_ring->count) ? i : 0;
+
+	flex_ptype = I40E_TXD_FLTR_QW0_QINDEX_MASK &
+		     (fdata->q_index << I40E_TXD_FLTR_QW0_QINDEX_SHIFT);
+
+	flex_ptype |= I40E_TXD_FLTR_QW0_FLEXOFF_MASK &
+		      (fdata->flex_off << I40E_TXD_FLTR_QW0_FLEXOFF_SHIFT);
+
+	flex_ptype |= I40E_TXD_FLTR_QW0_PCTYPE_MASK &
+		      (fdata->pctype << I40E_TXD_FLTR_QW0_PCTYPE_SHIFT);
+
+	/* Use LAN VSI Id if not programmed by user */
+	flex_ptype |= I40E_TXD_FLTR_QW0_DEST_VSI_MASK &
+		      ((u32)(fdata->dest_vsi ? : pf->vsi[pf->lan_vsi]->id) <<
+		       I40E_TXD_FLTR_QW0_DEST_VSI_SHIFT);
+
+	dtype_cmd = I40E_TX_DESC_DTYPE_FILTER_PROG;
+
+	dtype_cmd |= add ?
+		     I40E_FILTER_PROGRAM_DESC_PCMD_ADD_UPDATE <<
+		     I40E_TXD_FLTR_QW1_PCMD_SHIFT :
+		     I40E_FILTER_PROGRAM_DESC_PCMD_REMOVE <<
+		     I40E_TXD_FLTR_QW1_PCMD_SHIFT;
+
+	dtype_cmd |= I40E_TXD_FLTR_QW1_DEST_MASK &
+		     (fdata->dest_ctl << I40E_TXD_FLTR_QW1_DEST_SHIFT);
+
+	dtype_cmd |= I40E_TXD_FLTR_QW1_FD_STATUS_MASK &
+		     (fdata->fd_status << I40E_TXD_FLTR_QW1_FD_STATUS_SHIFT);
+
+	if (fdata->cnt_index) {
+		dtype_cmd |= I40E_TXD_FLTR_QW1_CNT_ENA_MASK;
+		dtype_cmd |= I40E_TXD_FLTR_QW1_CNTINDEX_MASK &
+			     ((u32)fdata->cnt_index <<
+			      I40E_TXD_FLTR_QW1_CNTINDEX_SHIFT);
+	}
+
+	fdir_desc->qindex_flex_ptype_vsi = cpu_to_le32(flex_ptype);
+	fdir_desc->rsvd = cpu_to_le32(0);
+	fdir_desc->dtype_cmd_cntindex = cpu_to_le32(dtype_cmd);
+	fdir_desc->fd_id = cpu_to_le32(fdata->fd_id);
+}
+
 #define I40E_FD_CLEAN_DELAY 10
 /**
  * i40e_program_fdir_filter - Program a Flow Director filter
@@ -48,14 +111,13 @@ static inline __le64 build_ctob(u32 td_cmd, u32 td_offset, unsigned int size,
  * @pf: The PF pointer
  * @add: True for add/update, False for remove
  **/
-int i40e_program_fdir_filter(struct i40e_fdir_filter *fdir_data, u8 *raw_packet,
-			     struct i40e_pf *pf, bool add)
+static int i40e_program_fdir_filter(struct i40e_fdir_filter *fdir_data,
+				    u8 *raw_packet, struct i40e_pf *pf,
+				    bool add)
 {
-	struct i40e_filter_program_desc *fdir_desc;
 	struct i40e_tx_buffer *tx_buf, *first;
 	struct i40e_tx_desc *tx_desc;
 	struct i40e_ring *tx_ring;
-	unsigned int fpt, dcc;
 	struct i40e_vsi *vsi;
 	struct device *dev;
 	dma_addr_t dma;
@@ -92,56 +154,8 @@ int i40e_program_fdir_filter(struct i40e_fdir_filter *fdir_data, u8 *raw_packet,
 
 	/* grab the next descriptor */
 	i = tx_ring->next_to_use;
-	fdir_desc = I40E_TX_FDIRDESC(tx_ring, i);
 	first = &tx_ring->tx_bi[i];
-	memset(first, 0, sizeof(struct i40e_tx_buffer));
-
-	tx_ring->next_to_use = ((i + 1) < tx_ring->count) ? i + 1 : 0;
-
-	fpt = (fdir_data->q_index << I40E_TXD_FLTR_QW0_QINDEX_SHIFT) &
-	      I40E_TXD_FLTR_QW0_QINDEX_MASK;
-
-	fpt |= (fdir_data->flex_off << I40E_TXD_FLTR_QW0_FLEXOFF_SHIFT) &
-	       I40E_TXD_FLTR_QW0_FLEXOFF_MASK;
-
-	fpt |= (fdir_data->pctype << I40E_TXD_FLTR_QW0_PCTYPE_SHIFT) &
-	       I40E_TXD_FLTR_QW0_PCTYPE_MASK;
-
-	/* Use LAN VSI Id if not programmed by user */
-	if (fdir_data->dest_vsi == 0)
-		fpt |= (pf->vsi[pf->lan_vsi]->id) <<
-		       I40E_TXD_FLTR_QW0_DEST_VSI_SHIFT;
-	else
-		fpt |= ((u32)fdir_data->dest_vsi <<
-			I40E_TXD_FLTR_QW0_DEST_VSI_SHIFT) &
-		       I40E_TXD_FLTR_QW0_DEST_VSI_MASK;
-
-	dcc = I40E_TX_DESC_DTYPE_FILTER_PROG;
-
-	if (add)
-		dcc |= I40E_FILTER_PROGRAM_DESC_PCMD_ADD_UPDATE <<
-		       I40E_TXD_FLTR_QW1_PCMD_SHIFT;
-	else
-		dcc |= I40E_FILTER_PROGRAM_DESC_PCMD_REMOVE <<
-		       I40E_TXD_FLTR_QW1_PCMD_SHIFT;
-
-	dcc |= (fdir_data->dest_ctl << I40E_TXD_FLTR_QW1_DEST_SHIFT) &
-	       I40E_TXD_FLTR_QW1_DEST_MASK;
-
-	dcc |= (fdir_data->fd_status << I40E_TXD_FLTR_QW1_FD_STATUS_SHIFT) &
-	       I40E_TXD_FLTR_QW1_FD_STATUS_MASK;
-
-	if (fdir_data->cnt_index != 0) {
-		dcc |= I40E_TXD_FLTR_QW1_CNT_ENA_MASK;
-		dcc |= ((u32)fdir_data->cnt_index <<
-			I40E_TXD_FLTR_QW1_CNTINDEX_SHIFT) &
-			I40E_TXD_FLTR_QW1_CNTINDEX_MASK;
-	}
-
-	fdir_desc->qindex_flex_ptype_vsi = cpu_to_le32(fpt);
-	fdir_desc->rsvd = cpu_to_le32(0);
-	fdir_desc->dtype_cmd_cntindex = cpu_to_le32(dcc);
-	fdir_desc->fd_id = cpu_to_le32(fdir_data->fd_id);
+	i40e_fdir(tx_ring, fdir_data, add);
 
 	/* Now program a dummy descriptor */
 	i = tx_ring->next_to_use;
@@ -282,18 +296,18 @@ 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) {
-			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;
-		}
+		if ((pf->flags & I40E_FLAG_FD_ATR_ENABLED) &&
+		    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->auto_disable_flags |= I40E_FLAG_FD_ATR_ENABLED;
 	} else {
 		pf->fd_tcp_rule = (pf->fd_tcp_rule > 0) ?
 				  (pf->fd_tcp_rule - 1) : 0;
 		if (pf->fd_tcp_rule == 0) {
-			pf->flags |= I40E_FLAG_FD_ATR_ENABLED;
-			if (I40E_DEBUG_FD & pf->hw.debug_mask)
+			if ((pf->flags & I40E_FLAG_FD_ATR_ENABLED) &&
+			    I40E_DEBUG_FD & pf->hw.debug_mask)
 				dev_info(&pf->pdev->dev, "ATR re-enabled due to no sideband TCP/IPv4 rules\n");
+			pf->auto_disable_flags &= ~I40E_FLAG_FD_ATR_ENABLED;
 		}
 	}
 
@@ -532,7 +546,10 @@ static void i40e_unmap_and_free_tx_resource(struct i40e_ring *ring,
 					    struct i40e_tx_buffer *tx_buffer)
 {
 	if (tx_buffer->skb) {
-		dev_kfree_skb_any(tx_buffer->skb);
+		if (tx_buffer->tx_flags & I40E_TX_FLAGS_FD_SB)
+			kfree(tx_buffer->raw_buf);
+		else
+			dev_kfree_skb_any(tx_buffer->skb);
 		if (dma_unmap_len(tx_buffer, len))
 			dma_unmap_single(ring->dev,
 					 dma_unmap_addr(tx_buffer, dma),
@@ -545,9 +562,6 @@ static void i40e_unmap_and_free_tx_resource(struct i40e_ring *ring,
 			       DMA_TO_DEVICE);
 	}
 
-	if (tx_buffer->tx_flags & I40E_TX_FLAGS_FD_SB)
-		kfree(tx_buffer->raw_buf);
-
 	tx_buffer->next_to_watch = NULL;
 	tx_buffer->skb = NULL;
 	dma_unmap_len_set(tx_buffer, len, 0);
@@ -584,8 +598,7 @@ void i40e_clean_tx_ring(struct i40e_ring *tx_ring)
 		return;
 
 	/* cleanup Tx queue statistics */
-	netdev_tx_reset_queue(netdev_get_tx_queue(tx_ring->netdev,
-						  tx_ring->queue_index));
+	netdev_tx_reset_queue(txring_txq(tx_ring));
 }
 
 /**
@@ -754,8 +767,8 @@ static bool i40e_clean_tx_irq(struct i40e_vsi *vsi,
 			tx_ring->arm_wb = true;
 	}
 
-	netdev_tx_completed_queue(netdev_get_tx_queue(tx_ring->netdev,
-						      tx_ring->queue_index),
+	/* notify netdev of completed buffers */
+	netdev_tx_completed_queue(txring_txq(tx_ring),
 				  total_packets, total_bytes);
 
 #define TX_WAKE_THRESHOLD (DESC_NEEDED * 2)
@@ -1864,6 +1877,15 @@ static u32 i40e_buildreg_itr(const int type, const u16 itr)
 
 /* a small macro to shorten up some long lines */
 #define INTREG I40E_PFINT_DYN_CTLN
+static inline int get_rx_itr_enabled(struct i40e_vsi *vsi, int idx)
+{
+	return !!(vsi->rx_rings[idx]->rx_itr_setting);
+}
+
+static inline int get_tx_itr_enabled(struct i40e_vsi *vsi, int idx)
+{
+	return !!(vsi->tx_rings[idx]->tx_itr_setting);
+}
 
 /**
  * i40e_update_enable_itr - Update itr and re-enable MSIX interrupt
@@ -1879,6 +1901,7 @@ static inline void i40e_update_enable_itr(struct i40e_vsi *vsi,
 	u32 rxval, txval;
 	int vector;
 	int idx = q_vector->v_idx;
+	int rx_itr_setting, tx_itr_setting;
 
 	vector = (q_vector->v_idx + vsi->base_vector);
 
@@ -1887,18 +1910,21 @@ static inline void i40e_update_enable_itr(struct i40e_vsi *vsi,
 	 */
 	rxval = txval = i40e_buildreg_itr(I40E_ITR_NONE, 0);
 
+	rx_itr_setting = get_rx_itr_enabled(vsi, idx);
+	tx_itr_setting = get_tx_itr_enabled(vsi, idx);
+
 	if (q_vector->itr_countdown > 0 ||
-	    (!ITR_IS_DYNAMIC(vsi->rx_rings[idx]->rx_itr_setting) &&
-	     !ITR_IS_DYNAMIC(vsi->tx_rings[idx]->tx_itr_setting))) {
+	    (!ITR_IS_DYNAMIC(rx_itr_setting) &&
+	     !ITR_IS_DYNAMIC(tx_itr_setting))) {
 		goto enable_int;
 	}
 
-	if (ITR_IS_DYNAMIC(vsi->rx_rings[idx]->rx_itr_setting)) {
+	if (ITR_IS_DYNAMIC(tx_itr_setting)) {
 		rx = i40e_set_new_dynamic_itr(&q_vector->rx);
 		rxval = i40e_buildreg_itr(I40E_RX_ITR, q_vector->rx.itr);
 	}
 
-	if (ITR_IS_DYNAMIC(vsi->tx_rings[idx]->tx_itr_setting)) {
+	if (ITR_IS_DYNAMIC(tx_itr_setting)) {
 		tx = i40e_set_new_dynamic_itr(&q_vector->tx);
 		txval = i40e_buildreg_itr(I40E_TX_ITR, q_vector->tx.itr);
 	}
@@ -2621,9 +2647,7 @@ bool __i40e_chk_linearize(struct sk_buff *skb)
 		return false;
 
 	/* We need to walk through the list and validate that each group
-	 * of 6 fragments totals at least gso_size.  However we don't need
-	 * to perform such validation on the last 6 since the last 6 cannot
-	 * inherit any data from a descriptor after them.
+	 * of 6 fragments totals at least gso_size.
 	 */
 	nr_frags -= I40E_MAX_BUFFER_TXD - 2;
 	frag = &skb_shinfo(skb)->frags[0];
@@ -2654,8 +2678,7 @@ bool __i40e_chk_linearize(struct sk_buff *skb)
 		if (sum < 0)
 			return true;
 
-		/* use pre-decrement to avoid processing last fragment */
-		if (!--nr_frags)
+		if (!nr_frags--)
 			break;
 
 		sum -= skb_frag_size(stale++);
@@ -2787,9 +2810,7 @@ static inline void i40e_tx_map(struct i40e_ring *tx_ring, struct sk_buff *skb,
 
 	tx_ring->next_to_use = i;
 
-	netdev_tx_sent_queue(netdev_get_tx_queue(tx_ring->netdev,
-						 tx_ring->queue_index),
-						 first->bytecount);
+	netdev_tx_sent_queue(txring_txq(tx_ring), first->bytecount);
 	i40e_maybe_stop_tx(tx_ring, DESC_NEEDED);
 
 	/* Algorithm to optimize tail and RS bit setting:
@@ -2814,13 +2835,11 @@ static inline void i40e_tx_map(struct i40e_ring *tx_ring, struct sk_buff *skb,
 	 * trigger a force WB.
 	 */
 	if (skb->xmit_more  &&
-	    !netif_xmit_stopped(netdev_get_tx_queue(tx_ring->netdev,
-						    tx_ring->queue_index))) {
+	    !netif_xmit_stopped(txring_txq(tx_ring))) {
 		tx_ring->flags |= I40E_TXR_FLAGS_LAST_XMIT_MORE_SET;
 		tail_bump = false;
 	} else if (!skb->xmit_more &&
-		   !netif_xmit_stopped(netdev_get_tx_queue(tx_ring->netdev,
-						       tx_ring->queue_index)) &&
+		   !netif_xmit_stopped(txring_txq(tx_ring)) &&
 		   (!(tx_ring->flags & I40E_TXR_FLAGS_LAST_XMIT_MORE_SET)) &&
 		   (tx_ring->packet_stride < WB_STRIDE) &&
 		   (desc_count < WB_STRIDE)) {
@@ -2840,10 +2859,9 @@ static inline void i40e_tx_map(struct i40e_ring *tx_ring, struct sk_buff *skb,
 						  I40E_TXD_QW1_CMD_SHIFT);
 
 	/* notify HW of packet */
-	if (!tail_bump)
+	if (!tail_bump) {
 		prefetchw(tx_desc + 1);
-
-	if (tail_bump) {
+	} else {
 		/* Force memory writes to complete before letting h/w
 		 * know there are new descriptors to fetch.  (Only
 		 * applicable for weak-ordered memory model archs,
@@ -2852,7 +2870,6 @@ static inline void i40e_tx_map(struct i40e_ring *tx_ring, struct sk_buff *skb,
 		wmb();
 		writel(i, tx_ring->tail);
 	}
-
 	return;
 
 dma_error:
diff --git a/drivers/net/ethernet/intel/i40e/i40e_txrx.h b/drivers/net/ethernet/intel/i40e/i40e_txrx.h
index b78c810d1835..508840585645 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_txrx.h
+++ b/drivers/net/ethernet/intel/i40e/i40e_txrx.h
@@ -463,4 +463,13 @@ static inline bool i40e_rx_is_fcoe(u16 ptype)
 	return (ptype >= I40E_RX_PTYPE_L2_FCOE_PAY3) &&
 	       (ptype <= I40E_RX_PTYPE_L2_FCOE_VFT_FCOTHER);
 }
+
+/**
+ * txring_txq - Find the netdev Tx ring based on the i40e Tx ring
+ * @ring: Tx ring to find the netdev equivalent of
+ **/
+static inline struct netdev_queue *txring_txq(const struct i40e_ring *ring)
+{
+	return netdev_get_tx_queue(ring->netdev, ring->queue_index);
+}
 #endif /* _I40E_TXRX_H_ */
diff --git a/drivers/net/ethernet/intel/i40e/i40e_virtchnl.h b/drivers/net/ethernet/intel/i40e/i40e_virtchnl.h
index c92a3bdee229..f861d3109d1a 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_virtchnl.h
+++ b/drivers/net/ethernet/intel/i40e/i40e_virtchnl.h
@@ -163,6 +163,7 @@ struct i40e_virtchnl_vsi_resource {
 #define I40E_VIRTCHNL_VF_OFFLOAD_RX_POLLING	0x00020000
 #define I40E_VIRTCHNL_VF_OFFLOAD_RSS_PCTYPE_V2	0x00040000
 #define I40E_VIRTCHNL_VF_OFFLOAD_RSS_PF		0X00080000
+#define I40E_VIRTCHNL_VF_OFFLOAD_ENCAP_CSUM	0X00100000
 
 struct i40e_virtchnl_vf_resource {
 	u16 num_vsis;
diff --git a/drivers/net/ethernet/intel/i40e/i40e_virtchnl_pf.c b/drivers/net/ethernet/intel/i40e/i40e_virtchnl_pf.c
index 6fcbf764f32b..54b8ee2583f1 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_virtchnl_pf.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_virtchnl_pf.c
@@ -502,8 +502,16 @@ static int i40e_config_vsi_tx_queue(struct i40e_vf *vf, u16 vsi_id,
 	u32 qtx_ctl;
 	int ret = 0;
 
+	if (!i40e_vc_isvalid_vsi_id(vf, info->vsi_id)) {
+		ret = -ENOENT;
+		goto error_context;
+	}
 	pf_queue_id = i40e_vc_get_pf_queue_id(vf, vsi_id, vsi_queue_id);
 	vsi = i40e_find_vsi_from_id(pf, vsi_id);
+	if (!vsi) {
+		ret = -ENOENT;
+		goto error_context;
+	}
 
 	/* clear the context structure first */
 	memset(&tx_ctx, 0, sizeof(struct i40e_hmc_obj_txq));
@@ -991,7 +999,10 @@ complete_reset:
 		i40e_enable_vf_mappings(vf);
 		set_bit(I40E_VF_STAT_ACTIVE, &vf->vf_states);
 		clear_bit(I40E_VF_STAT_DISABLED, &vf->vf_states);
-		i40e_notify_client_of_vf_reset(pf, abs_vf_id);
+		/* Do not notify the client during VF init */
+		if (vf->pf->num_alloc_vfs)
+			i40e_notify_client_of_vf_reset(pf, abs_vf_id);
+		vf->num_vlan = 0;
 	}
 	/* tell the VF the reset is done */
 	wr32(hw, I40E_VFGEN_RSTAT1(vf->vf_id), I40E_VFR_VFACTIVE);
@@ -1089,7 +1100,6 @@ int i40e_alloc_vfs(struct i40e_pf *pf, u16 num_alloc_vfs)
 			goto err_iov;
 		}
 	}
-	i40e_notify_client_of_vf_enable(pf, num_alloc_vfs);
 	/* allocate memory */
 	vfs = kcalloc(num_alloc_vfs, sizeof(struct i40e_vf), GFP_KERNEL);
 	if (!vfs) {
@@ -1113,6 +1123,8 @@ int i40e_alloc_vfs(struct i40e_pf *pf, u16 num_alloc_vfs)
 	}
 	pf->num_alloc_vfs = num_alloc_vfs;
 
+	i40e_notify_client_of_vf_enable(pf, num_alloc_vfs);
+
 err_alloc:
 	if (ret)
 		i40e_free_vfs(pf);
@@ -1472,7 +1484,8 @@ static int i40e_vc_config_promiscuous_mode_msg(struct i40e_vf *vf,
 
 	vsi = i40e_find_vsi_from_id(pf, info->vsi_id);
 	if (!test_bit(I40E_VF_STAT_ACTIVE, &vf->vf_states) ||
-	    !i40e_vc_isvalid_vsi_id(vf, info->vsi_id)) {
+	    !i40e_vc_isvalid_vsi_id(vf, info->vsi_id) ||
+	    !vsi) {
 		aq_ret = I40E_ERR_PARAM;
 		goto error_param;
 	}
@@ -2213,8 +2226,8 @@ static int i40e_vc_iwarp_qvmap_msg(struct i40e_vf *vf, u8 *msg, u16 msglen,
 error_param:
 	/* send the response to the VF */
 	return i40e_vc_send_resp_to_vf(vf,
-			       config ? I40E_VIRTCHNL_OP_RELEASE_IWARP_IRQ_MAP :
-			       I40E_VIRTCHNL_OP_CONFIG_IWARP_IRQ_MAP,
+			       config ? I40E_VIRTCHNL_OP_CONFIG_IWARP_IRQ_MAP :
+			       I40E_VIRTCHNL_OP_RELEASE_IWARP_IRQ_MAP,
 			       aq_ret);
 }
 
@@ -2314,6 +2327,7 @@ err:
 	/* send the response back to the VF */
 	aq_ret = i40e_vc_send_msg_to_vf(vf, I40E_VIRTCHNL_OP_GET_RSS_HENA_CAPS,
 					aq_ret, (u8 *)vrh, len);
+	kfree(vrh);
 	return aq_ret;
 }
 
@@ -2742,11 +2756,12 @@ error_param:
  * @vf_id: VF identifier
  * @vlan_id: mac address
  * @qos: priority setting
+ * @vlan_proto: vlan protocol
  *
  * program VF vlan id and/or qos
  **/
-int i40e_ndo_set_vf_port_vlan(struct net_device *netdev,
-			      int vf_id, u16 vlan_id, u8 qos)
+int i40e_ndo_set_vf_port_vlan(struct net_device *netdev, int vf_id,
+			      u16 vlan_id, u8 qos, __be16 vlan_proto)
 {
 	u16 vlanprio = vlan_id | (qos << I40E_VLAN_PRIORITY_SHIFT);
 	struct i40e_netdev_priv *np = netdev_priv(netdev);
@@ -2769,6 +2784,12 @@ int i40e_ndo_set_vf_port_vlan(struct net_device *netdev,
 		goto error_pvid;
 	}
 
+	if (vlan_proto != htons(ETH_P_8021Q)) {
+		dev_err(&pf->pdev->dev, "VF VLAN protocol is not supported\n");
+		ret = -EPROTONOSUPPORT;
+		goto error_pvid;
+	}
+
 	vf = &(pf->vf[vf_id]);
 	vsi = pf->vsi[vf->lan_vsi_idx];
 	if (!test_bit(I40E_VF_STAT_INIT, &vf->vf_states)) {
@@ -2995,6 +3016,7 @@ int i40e_ndo_get_vf_config(struct net_device *netdev,
 	else
 		ivi->linkstate = IFLA_VF_LINK_STATE_DISABLE;
 	ivi->spoofchk = vf->spoofchk;
+	ivi->trusted = vf->trusted;
 	ret = 0;
 
 error_param:
diff --git a/drivers/net/ethernet/intel/i40e/i40e_virtchnl_pf.h b/drivers/net/ethernet/intel/i40e/i40e_virtchnl_pf.h
index 875174141451..4012d069939a 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_virtchnl_pf.h
+++ b/drivers/net/ethernet/intel/i40e/i40e_virtchnl_pf.h
@@ -129,8 +129,8 @@ void i40e_vc_notify_vf_reset(struct i40e_vf *vf);
 
 /* VF configuration related iplink handlers */
 int i40e_ndo_set_vf_mac(struct net_device *netdev, int vf_id, u8 *mac);
-int i40e_ndo_set_vf_port_vlan(struct net_device *netdev,
-			      int vf_id, u16 vlan_id, u8 qos);
+int i40e_ndo_set_vf_port_vlan(struct net_device *netdev, int vf_id,
+			      u16 vlan_id, u8 qos, __be16 vlan_proto);
 int i40e_ndo_set_vf_bw(struct net_device *netdev, int vf_id, int min_tx_rate,
 		       int max_tx_rate);
 int i40e_ndo_set_vf_trust(struct net_device *netdev, int vf_id, bool setting);
diff --git a/drivers/net/ethernet/intel/i40evf/i40e_adminq_cmd.h b/drivers/net/ethernet/intel/i40evf/i40e_adminq_cmd.h
index 3114dcfa1724..40b0eafd0c71 100644
--- a/drivers/net/ethernet/intel/i40evf/i40e_adminq_cmd.h
+++ b/drivers/net/ethernet/intel/i40evf/i40e_adminq_cmd.h
@@ -204,6 +204,9 @@ enum i40e_admin_queue_opc {
 	i40e_aqc_opc_suspend_port_tx				= 0x041B,
 	i40e_aqc_opc_resume_port_tx				= 0x041C,
 	i40e_aqc_opc_configure_partition_bw			= 0x041D,
+	/* hmc */
+	i40e_aqc_opc_query_hmc_resource_profile	= 0x0500,
+	i40e_aqc_opc_set_hmc_resource_profile	= 0x0501,
 
 	/* phy commands*/
 	i40e_aqc_opc_get_phy_abilities		= 0x0600,
@@ -447,13 +450,15 @@ I40E_CHECK_CMD_LENGTH(i40e_aqc_cppm_configuration);
 /* Set ARP Proxy command / response (indirect 0x0104) */
 struct i40e_aqc_arp_proxy_data {
 	__le16	command_flags;
-#define I40E_AQ_ARP_INIT_IPV4	0x0008
-#define I40E_AQ_ARP_UNSUP_CTL	0x0010
-#define I40E_AQ_ARP_ENA		0x0020
-#define I40E_AQ_ARP_ADD_IPV4	0x0040
-#define I40E_AQ_ARP_DEL_IPV4	0x0080
+#define I40E_AQ_ARP_INIT_IPV4	0x0800
+#define I40E_AQ_ARP_UNSUP_CTL	0x1000
+#define I40E_AQ_ARP_ENA		0x2000
+#define I40E_AQ_ARP_ADD_IPV4	0x4000
+#define I40E_AQ_ARP_DEL_IPV4	0x8000
 	__le16	table_id;
-	__le32	pfpm_proxyfc;
+	__le32	enabled_offloads;
+#define I40E_AQ_ARP_DIRECTED_OFFLOAD_ENABLE	0x00000020
+#define I40E_AQ_ARP_OFFLOAD_ENABLE		0x00000800
 	__le32	ip_addr;
 	u8	mac_addr[6];
 	u8	reserved[2];
@@ -468,17 +473,19 @@ struct i40e_aqc_ns_proxy_data {
 	__le16	table_idx_ipv6_0;
 	__le16	table_idx_ipv6_1;
 	__le16	control;
-#define I40E_AQ_NS_PROXY_ADD_0		0x0100
-#define I40E_AQ_NS_PROXY_DEL_0		0x0200
-#define I40E_AQ_NS_PROXY_ADD_1		0x0400
-#define I40E_AQ_NS_PROXY_DEL_1		0x0800
-#define I40E_AQ_NS_PROXY_ADD_IPV6_0	0x1000
-#define I40E_AQ_NS_PROXY_DEL_IPV6_0	0x2000
-#define I40E_AQ_NS_PROXY_ADD_IPV6_1	0x4000
-#define I40E_AQ_NS_PROXY_DEL_IPV6_1	0x8000
-#define I40E_AQ_NS_PROXY_COMMAND_SEQ	0x0001
-#define I40E_AQ_NS_PROXY_INIT_IPV6_TBL	0x0002
-#define I40E_AQ_NS_PROXY_INIT_MAC_TBL	0x0004
+#define I40E_AQ_NS_PROXY_ADD_0		0x0001
+#define I40E_AQ_NS_PROXY_DEL_0		0x0002
+#define I40E_AQ_NS_PROXY_ADD_1		0x0004
+#define I40E_AQ_NS_PROXY_DEL_1		0x0008
+#define I40E_AQ_NS_PROXY_ADD_IPV6_0	0x0010
+#define I40E_AQ_NS_PROXY_DEL_IPV6_0	0x0020
+#define I40E_AQ_NS_PROXY_ADD_IPV6_1	0x0040
+#define I40E_AQ_NS_PROXY_DEL_IPV6_1	0x0080
+#define I40E_AQ_NS_PROXY_COMMAND_SEQ	0x0100
+#define I40E_AQ_NS_PROXY_INIT_IPV6_TBL	0x0200
+#define I40E_AQ_NS_PROXY_INIT_MAC_TBL	0x0400
+#define I40E_AQ_NS_PROXY_OFFLOAD_ENABLE	0x0800
+#define I40E_AQ_NS_PROXY_DIRECTED_OFFLOAD_ENABLE	0x1000
 	u8	mac_addr_0[6];
 	u8	mac_addr_1[6];
 	u8	local_mac_addr[6];
@@ -1579,6 +1586,24 @@ struct i40e_aqc_configure_partition_bw_data {
 
 I40E_CHECK_STRUCT_LEN(0x22, i40e_aqc_configure_partition_bw_data);
 
+/* Get and set the active HMC resource profile and status.
+ * (direct 0x0500) and (direct 0x0501)
+ */
+struct i40e_aq_get_set_hmc_resource_profile {
+	u8	pm_profile;
+	u8	pe_vf_enabled;
+	u8	reserved[14];
+};
+
+I40E_CHECK_CMD_LENGTH(i40e_aq_get_set_hmc_resource_profile);
+
+enum i40e_aq_hmc_profile {
+	/* I40E_HMC_PROFILE_NO_CHANGE	= 0, reserved */
+	I40E_HMC_PROFILE_DEFAULT	= 1,
+	I40E_HMC_PROFILE_FAVOR_VF	= 2,
+	I40E_HMC_PROFILE_EQUAL		= 3,
+};
+
 /* Get PHY Abilities (indirect 0x0600) uses the generic indirect struct */
 
 /* set in param0 for get phy abilities to report qualified modules */
diff --git a/drivers/net/ethernet/intel/i40evf/i40e_common.c b/drivers/net/ethernet/intel/i40evf/i40e_common.c
index 4db0c0326185..7953c13451b9 100644
--- a/drivers/net/ethernet/intel/i40evf/i40e_common.c
+++ b/drivers/net/ethernet/intel/i40evf/i40e_common.c
@@ -302,7 +302,6 @@ void i40evf_debug_aq(struct i40e_hw *hw, enum i40e_debug_mask mask, void *desc,
 		   void *buffer, u16 buf_len)
 {
 	struct i40e_aq_desc *aq_desc = (struct i40e_aq_desc *)desc;
-	u16 len = le16_to_cpu(aq_desc->datalen);
 	u8 *buf = (u8 *)buffer;
 	u16 i = 0;
 
@@ -326,6 +325,8 @@ void i40evf_debug_aq(struct i40e_hw *hw, enum i40e_debug_mask mask, void *desc,
 		   le32_to_cpu(aq_desc->params.external.addr_low));
 
 	if ((buffer != NULL) && (aq_desc->datalen != 0)) {
+		u16 len = le16_to_cpu(aq_desc->datalen);
+
 		i40e_debug(hw, mask, "AQ CMD Buffer:\n");
 		if (buf_len < len)
 			len = buf_len;
diff --git a/drivers/net/ethernet/intel/i40evf/i40e_txrx.c b/drivers/net/ethernet/intel/i40evf/i40e_txrx.c
index a579193b2c21..75f2a2cdd738 100644
--- a/drivers/net/ethernet/intel/i40evf/i40e_txrx.c
+++ b/drivers/net/ethernet/intel/i40evf/i40e_txrx.c
@@ -51,7 +51,10 @@ static void i40e_unmap_and_free_tx_resource(struct i40e_ring *ring,
 					    struct i40e_tx_buffer *tx_buffer)
 {
 	if (tx_buffer->skb) {
-		dev_kfree_skb_any(tx_buffer->skb);
+		if (tx_buffer->tx_flags & I40E_TX_FLAGS_FD_SB)
+			kfree(tx_buffer->raw_buf);
+		else
+			dev_kfree_skb_any(tx_buffer->skb);
 		if (dma_unmap_len(tx_buffer, len))
 			dma_unmap_single(ring->dev,
 					 dma_unmap_addr(tx_buffer, dma),
@@ -64,9 +67,6 @@ static void i40e_unmap_and_free_tx_resource(struct i40e_ring *ring,
 			       DMA_TO_DEVICE);
 	}
 
-	if (tx_buffer->tx_flags & I40E_TX_FLAGS_FD_SB)
-		kfree(tx_buffer->raw_buf);
-
 	tx_buffer->next_to_watch = NULL;
 	tx_buffer->skb = NULL;
 	dma_unmap_len_set(tx_buffer, len, 0);
@@ -103,8 +103,7 @@ void i40evf_clean_tx_ring(struct i40e_ring *tx_ring)
 		return;
 
 	/* cleanup Tx queue statistics */
-	netdev_tx_reset_queue(netdev_get_tx_queue(tx_ring->netdev,
-						  tx_ring->queue_index));
+	netdev_tx_reset_queue(txring_txq(tx_ring));
 }
 
 /**
@@ -273,8 +272,8 @@ static bool i40e_clean_tx_irq(struct i40e_vsi *vsi,
 			tx_ring->arm_wb = true;
 	}
 
-	netdev_tx_completed_queue(netdev_get_tx_queue(tx_ring->netdev,
-						      tx_ring->queue_index),
+	/* notify netdev of completed buffers */
+	netdev_tx_completed_queue(txring_txq(tx_ring),
 				  total_packets, total_bytes);
 
 #define TX_WAKE_THRESHOLD (DESC_NEEDED * 2)
@@ -1312,6 +1311,19 @@ static u32 i40e_buildreg_itr(const int type, const u16 itr)
 
 /* a small macro to shorten up some long lines */
 #define INTREG I40E_VFINT_DYN_CTLN1
+static inline int get_rx_itr_enabled(struct i40e_vsi *vsi, int idx)
+{
+	struct i40evf_adapter *adapter = vsi->back;
+
+	return !!(adapter->rx_rings[idx].rx_itr_setting);
+}
+
+static inline int get_tx_itr_enabled(struct i40e_vsi *vsi, int idx)
+{
+	struct i40evf_adapter *adapter = vsi->back;
+
+	return !!(adapter->tx_rings[idx].tx_itr_setting);
+}
 
 /**
  * i40e_update_enable_itr - Update itr and re-enable MSIX interrupt
@@ -1326,6 +1338,8 @@ static inline void i40e_update_enable_itr(struct i40e_vsi *vsi,
 	bool rx = false, tx = false;
 	u32 rxval, txval;
 	int vector;
+	int idx = q_vector->v_idx;
+	int rx_itr_setting, tx_itr_setting;
 
 	vector = (q_vector->v_idx + vsi->base_vector);
 
@@ -1334,18 +1348,21 @@ static inline void i40e_update_enable_itr(struct i40e_vsi *vsi,
 	 */
 	rxval = txval = i40e_buildreg_itr(I40E_ITR_NONE, 0);
 
+	rx_itr_setting = get_rx_itr_enabled(vsi, idx);
+	tx_itr_setting = get_tx_itr_enabled(vsi, idx);
+
 	if (q_vector->itr_countdown > 0 ||
-	    (!ITR_IS_DYNAMIC(vsi->rx_itr_setting) &&
-	     !ITR_IS_DYNAMIC(vsi->tx_itr_setting))) {
+	    (!ITR_IS_DYNAMIC(rx_itr_setting) &&
+	     !ITR_IS_DYNAMIC(tx_itr_setting))) {
 		goto enable_int;
 	}
 
-	if (ITR_IS_DYNAMIC(vsi->rx_itr_setting)) {
+	if (ITR_IS_DYNAMIC(rx_itr_setting)) {
 		rx = i40e_set_new_dynamic_itr(&q_vector->rx);
 		rxval = i40e_buildreg_itr(I40E_RX_ITR, q_vector->rx.itr);
 	}
 
-	if (ITR_IS_DYNAMIC(vsi->tx_itr_setting)) {
+	if (ITR_IS_DYNAMIC(tx_itr_setting)) {
 		tx = i40e_set_new_dynamic_itr(&q_vector->tx);
 		txval = i40e_buildreg_itr(I40E_TX_ITR, q_vector->tx.itr);
 	}
@@ -1832,9 +1849,7 @@ bool __i40evf_chk_linearize(struct sk_buff *skb)
 		return false;
 
 	/* We need to walk through the list and validate that each group
-	 * of 6 fragments totals at least gso_size.  However we don't need
-	 * to perform such validation on the last 6 since the last 6 cannot
-	 * inherit any data from a descriptor after them.
+	 * of 6 fragments totals at least gso_size.
 	 */
 	nr_frags -= I40E_MAX_BUFFER_TXD - 2;
 	frag = &skb_shinfo(skb)->frags[0];
@@ -1865,8 +1880,7 @@ bool __i40evf_chk_linearize(struct sk_buff *skb)
 		if (sum < 0)
 			return true;
 
-		/* use pre-decrement to avoid processing last fragment */
-		if (!--nr_frags)
+		if (!nr_frags--)
 			break;
 
 		sum -= skb_frag_size(stale++);
@@ -2015,9 +2029,7 @@ static inline void i40evf_tx_map(struct i40e_ring *tx_ring, struct sk_buff *skb,
 
 	tx_ring->next_to_use = i;
 
-	netdev_tx_sent_queue(netdev_get_tx_queue(tx_ring->netdev,
-						 tx_ring->queue_index),
-						 first->bytecount);
+	netdev_tx_sent_queue(txring_txq(tx_ring), first->bytecount);
 	i40e_maybe_stop_tx(tx_ring, DESC_NEEDED);
 
 	/* Algorithm to optimize tail and RS bit setting:
@@ -2042,13 +2054,11 @@ static inline void i40evf_tx_map(struct i40e_ring *tx_ring, struct sk_buff *skb,
 	 * trigger a force WB.
 	 */
 	if (skb->xmit_more  &&
-	    !netif_xmit_stopped(netdev_get_tx_queue(tx_ring->netdev,
-						    tx_ring->queue_index))) {
+	    !netif_xmit_stopped(txring_txq(tx_ring))) {
 		tx_ring->flags |= I40E_TXR_FLAGS_LAST_XMIT_MORE_SET;
 		tail_bump = false;
 	} else if (!skb->xmit_more &&
-		   !netif_xmit_stopped(netdev_get_tx_queue(tx_ring->netdev,
-						       tx_ring->queue_index)) &&
+		   !netif_xmit_stopped(txring_txq(tx_ring)) &&
 		   (!(tx_ring->flags & I40E_TXR_FLAGS_LAST_XMIT_MORE_SET)) &&
 		   (tx_ring->packet_stride < WB_STRIDE) &&
 		   (desc_count < WB_STRIDE)) {
@@ -2068,10 +2078,9 @@ static inline void i40evf_tx_map(struct i40e_ring *tx_ring, struct sk_buff *skb,
 						  I40E_TXD_QW1_CMD_SHIFT);
 
 	/* notify HW of packet */
-	if (!tail_bump)
+	if (!tail_bump) {
 		prefetchw(tx_desc + 1);
-
-	if (tail_bump) {
+	} else {
 		/* Force memory writes to complete before letting h/w
 		 * know there are new descriptors to fetch.  (Only
 		 * applicable for weak-ordered memory model archs,
@@ -2080,7 +2089,6 @@ static inline void i40evf_tx_map(struct i40e_ring *tx_ring, struct sk_buff *skb,
 		wmb();
 		writel(i, tx_ring->tail);
 	}
-
 	return;
 
 dma_error:
diff --git a/drivers/net/ethernet/intel/i40evf/i40e_txrx.h b/drivers/net/ethernet/intel/i40evf/i40e_txrx.h
index 0112277e5882..abcdecabbc56 100644
--- a/drivers/net/ethernet/intel/i40evf/i40e_txrx.h
+++ b/drivers/net/ethernet/intel/i40evf/i40e_txrx.h
@@ -287,6 +287,14 @@ struct i40e_ring {
 	u8 dcb_tc;			/* Traffic class of ring */
 	u8 __iomem *tail;
 
+	/* high bit set means dynamic, use accessors routines to read/write.
+	 * hardware only supports 2us resolution for the ITR registers.
+	 * these values always store the USER setting, and must be converted
+	 * before programming to a register.
+	 */
+	u16 rx_itr_setting;
+	u16 tx_itr_setting;
+
 	u16 count;			/* Number of descriptors */
 	u16 reg_idx;			/* HW register index of the ring */
 	u16 rx_buf_len;
@@ -445,4 +453,13 @@ static inline bool i40e_rx_is_fcoe(u16 ptype)
 	return (ptype >= I40E_RX_PTYPE_L2_FCOE_PAY3) &&
 	       (ptype <= I40E_RX_PTYPE_L2_FCOE_VFT_FCOTHER);
 }
+
+/**
+ * txring_txq - Find the netdev Tx ring based on the i40e Tx ring
+ * @ring: Tx ring to find the netdev equivalent of
+ **/
+static inline struct netdev_queue *txring_txq(const struct i40e_ring *ring)
+{
+	return netdev_get_tx_queue(ring->netdev, ring->queue_index);
+}
 #endif /* _I40E_TXRX_H_ */
diff --git a/drivers/net/ethernet/intel/i40evf/i40e_virtchnl.h b/drivers/net/ethernet/intel/i40evf/i40e_virtchnl.h
index f04ce6cb70dc..bd691ad86673 100644
--- a/drivers/net/ethernet/intel/i40evf/i40e_virtchnl.h
+++ b/drivers/net/ethernet/intel/i40evf/i40e_virtchnl.h
@@ -160,6 +160,7 @@ struct i40e_virtchnl_vsi_resource {
 #define I40E_VIRTCHNL_VF_OFFLOAD_RX_POLLING	0x00020000
 #define I40E_VIRTCHNL_VF_OFFLOAD_RSS_PCTYPE_V2	0x00040000
 #define I40E_VIRTCHNL_VF_OFFLOAD_RSS_PF		0X00080000
+#define I40E_VIRTCHNL_VF_OFFLOAD_ENCAP_CSUM	0X00100000
 
 struct i40e_virtchnl_vf_resource {
 	u16 num_vsis;
diff --git a/drivers/net/ethernet/intel/i40evf/i40evf.h b/drivers/net/ethernet/intel/i40evf/i40evf.h
index 76ed97db28e2..c5fd724313c7 100644
--- a/drivers/net/ethernet/intel/i40evf/i40evf.h
+++ b/drivers/net/ethernet/intel/i40evf/i40evf.h
@@ -59,32 +59,25 @@ struct i40e_vsi {
 	unsigned long state;
 	int base_vector;
 	u16 work_limit;
-	/* high bit set means dynamic, use accessor routines to read/write.
-	 * hardware only supports 2us resolution for the ITR registers.
-	 * these values always store the USER setting, and must be converted
-	 * before programming to a register.
-	 */
-	u16 rx_itr_setting;
-	u16 tx_itr_setting;
 	u16 qs_handle;
 };
 
 /* How many Rx Buffers do we bundle into one write to the hardware ? */
 #define I40EVF_RX_BUFFER_WRITE	16	/* Must be power of 2 */
-#define I40EVF_DEFAULT_TXD   512
-#define I40EVF_DEFAULT_RXD   512
-#define I40EVF_MAX_TXD       4096
-#define I40EVF_MIN_TXD       64
-#define I40EVF_MAX_RXD       4096
-#define I40EVF_MIN_RXD       64
-#define I40EVF_REQ_DESCRIPTOR_MULTIPLE  32
+#define I40EVF_DEFAULT_TXD	512
+#define I40EVF_DEFAULT_RXD	512
+#define I40EVF_MAX_TXD		4096
+#define I40EVF_MIN_TXD		64
+#define I40EVF_MAX_RXD		4096
+#define I40EVF_MIN_RXD		64
+#define I40EVF_REQ_DESCRIPTOR_MULTIPLE	32
 
 /* Supported Rx Buffer Sizes */
-#define I40EVF_RXBUFFER_2048  2048
-#define I40EVF_MAX_RXBUFFER   16384  /* largest size for single descriptor */
-#define I40EVF_MAX_AQ_BUF_SIZE    4096
-#define I40EVF_AQ_LEN             32
-#define I40EVF_AQ_MAX_ERR         20 /* times to try before resetting AQ */
+#define I40EVF_RXBUFFER_2048	2048
+#define I40EVF_MAX_RXBUFFER	16384  /* largest size for single descriptor */
+#define I40EVF_MAX_AQ_BUF_SIZE	4096
+#define I40EVF_AQ_LEN		32
+#define I40EVF_AQ_MAX_ERR	20 /* times to try before resetting AQ */
 
 #define MAXIMUM_ETHERNET_VLAN_SIZE (VLAN_ETH_FRAME_LEN + ETH_FCS_LEN)
 
@@ -111,7 +104,7 @@ struct i40e_q_vector {
 	u8 num_ringpairs;	/* total number of ring pairs in vector */
 #define ITR_COUNTDOWN_START 100
 	u8 itr_countdown;	/* when 0 or 1 update ITR */
-	int v_idx;	  /* vector index in list */
+	int v_idx;	/* vector index in list */
 	char name[IFNAMSIZ + 9];
 	bool arm_wb_state;
 	cpumask_var_t affinity_mask;
@@ -129,11 +122,11 @@ struct i40e_q_vector {
 	((((R)->next_to_clean > (R)->next_to_use) ? 0 : (R)->count) + \
 	(R)->next_to_clean - (R)->next_to_use - 1)
 
-#define I40EVF_RX_DESC_ADV(R, i)	    \
+#define I40EVF_RX_DESC_ADV(R, i)	\
 	(&(((union i40e_adv_rx_desc *)((R).desc))[i]))
-#define I40EVF_TX_DESC_ADV(R, i)	    \
+#define I40EVF_TX_DESC_ADV(R, i)	\
 	(&(((union i40e_adv_tx_desc *)((R).desc))[i]))
-#define I40EVF_TX_CTXTDESC_ADV(R, i)	    \
+#define I40EVF_TX_CTXTDESC_ADV(R, i)	\
 	(&(((struct i40e_adv_tx_context_desc *)((R).desc))[i]))
 
 #define OTHER_VECTOR 1
@@ -204,22 +197,25 @@ struct i40evf_adapter {
 	struct msix_entry *msix_entries;
 
 	u32 flags;
-#define I40EVF_FLAG_RX_CSUM_ENABLED              BIT(0)
-#define I40EVF_FLAG_IMIR_ENABLED                 BIT(5)
-#define I40EVF_FLAG_MQ_CAPABLE                   BIT(6)
-#define I40EVF_FLAG_NEED_LINK_UPDATE             BIT(7)
-#define I40EVF_FLAG_PF_COMMS_FAILED              BIT(8)
-#define I40EVF_FLAG_RESET_PENDING                BIT(9)
-#define I40EVF_FLAG_RESET_NEEDED                 BIT(10)
+#define I40EVF_FLAG_RX_CSUM_ENABLED		BIT(0)
+#define I40EVF_FLAG_IN_NETPOLL			BIT(4)
+#define I40EVF_FLAG_IMIR_ENABLED		BIT(5)
+#define I40EVF_FLAG_MQ_CAPABLE			BIT(6)
+#define I40EVF_FLAG_NEED_LINK_UPDATE		BIT(7)
+#define I40EVF_FLAG_PF_COMMS_FAILED		BIT(8)
+#define I40EVF_FLAG_RESET_PENDING		BIT(9)
+#define I40EVF_FLAG_RESET_NEEDED		BIT(10)
 #define I40EVF_FLAG_WB_ON_ITR_CAPABLE		BIT(11)
 #define I40EVF_FLAG_OUTER_UDP_CSUM_CAPABLE	BIT(12)
 #define I40EVF_FLAG_ADDR_SET_BY_PF		BIT(13)
+#define I40EVF_FLAG_SERVICE_CLIENT_REQUESTED	BIT(14)
 #define I40EVF_FLAG_PROMISC_ON			BIT(15)
 #define I40EVF_FLAG_ALLMULTI_ON			BIT(16)
 /* duplicates for common code */
-#define I40E_FLAG_FDIR_ATR_ENABLED		 0
-#define I40E_FLAG_DCB_ENABLED			 0
-#define I40E_FLAG_RX_CSUM_ENABLED                I40EVF_FLAG_RX_CSUM_ENABLED
+#define I40E_FLAG_FDIR_ATR_ENABLED		0
+#define I40E_FLAG_DCB_ENABLED			0
+#define I40E_FLAG_IN_NETPOLL			I40EVF_FLAG_IN_NETPOLL
+#define I40E_FLAG_RX_CSUM_ENABLED		I40EVF_FLAG_RX_CSUM_ENABLED
 #define I40E_FLAG_WB_ON_ITR_CAPABLE		I40EVF_FLAG_WB_ON_ITR_CAPABLE
 #define I40E_FLAG_OUTER_UDP_CSUM_CAPABLE	I40EVF_FLAG_OUTER_UDP_CSUM_CAPABLE
 	/* flags for admin queue service task */
@@ -233,7 +229,7 @@ struct i40evf_adapter {
 #define I40EVF_FLAG_AQ_CONFIGURE_QUEUES		BIT(6)
 #define I40EVF_FLAG_AQ_MAP_VECTORS		BIT(7)
 #define I40EVF_FLAG_AQ_HANDLE_RESET		BIT(8)
-#define I40EVF_FLAG_AQ_CONFIGURE_RSS		BIT(9)	/* direct AQ config */
+#define I40EVF_FLAG_AQ_CONFIGURE_RSS		BIT(9) /* direct AQ config */
 #define I40EVF_FLAG_AQ_GET_CONFIG		BIT(10)
 /* Newer style, RSS done by the PF so we can ignore hardware vagaries. */
 #define I40EVF_FLAG_AQ_GET_HENA			BIT(11)
@@ -258,6 +254,7 @@ struct i40evf_adapter {
 	struct work_struct watchdog_task;
 	bool netdev_registered;
 	bool link_up;
+	enum i40e_aq_link_speed link_speed;
 	enum i40e_virtchnl_ops current_op;
 #define CLIENT_ENABLED(_a) ((_a)->vf_res ? \
 			    (_a)->vf_res->vf_offload_flags & \
diff --git a/drivers/net/ethernet/intel/i40evf/i40evf_ethtool.c b/drivers/net/ethernet/intel/i40evf/i40evf_ethtool.c
index c9c202f6c521..a9940154eead 100644
--- a/drivers/net/ethernet/intel/i40evf/i40evf_ethtool.c
+++ b/drivers/net/ethernet/intel/i40evf/i40evf_ethtool.c
@@ -74,13 +74,33 @@ static const struct i40evf_stats i40evf_gstrings_stats[] = {
 static int i40evf_get_settings(struct net_device *netdev,
 			       struct ethtool_cmd *ecmd)
 {
-	/* In the future the VF will be able to query the PF for
-	 * some information - for now use a dummy value
-	 */
+	struct i40evf_adapter *adapter = netdev_priv(netdev);
+
 	ecmd->supported = 0;
 	ecmd->autoneg = AUTONEG_DISABLE;
 	ecmd->transceiver = XCVR_DUMMY1;
 	ecmd->port = PORT_NONE;
+	/* Set speed and duplex */
+	switch (adapter->link_speed) {
+	case I40E_LINK_SPEED_40GB:
+		ethtool_cmd_speed_set(ecmd, SPEED_40000);
+		break;
+	case I40E_LINK_SPEED_20GB:
+		ethtool_cmd_speed_set(ecmd, SPEED_20000);
+		break;
+	case I40E_LINK_SPEED_10GB:
+		ethtool_cmd_speed_set(ecmd, SPEED_10000);
+		break;
+	case I40E_LINK_SPEED_1GB:
+		ethtool_cmd_speed_set(ecmd, SPEED_1000);
+		break;
+	case I40E_LINK_SPEED_100MB:
+		ethtool_cmd_speed_set(ecmd, SPEED_100);
+		break;
+	default:
+		break;
+	}
+	ecmd->duplex = DUPLEX_FULL;
 
 	return 0;
 }
@@ -276,93 +296,207 @@ static int i40evf_set_ringparam(struct net_device *netdev,
 }
 
 /**
- * i40evf_get_coalesce - Get interrupt coalescing settings
- * @netdev: network interface device structure
- * @ec: ethtool coalesce structure
+ * __i40evf_get_coalesce - get per-queue coalesce settings
+ * @netdev: the netdev to check
+ * @ec: ethtool coalesce data structure
+ * @queue: which queue to pick
  *
- * Returns current coalescing settings. This is referred to elsewhere in the
- * driver as Interrupt Throttle Rate, as this is how the hardware describes
- * this functionality.
+ * Gets the per-queue settings for coalescence. Specifically Rx and Tx usecs
+ * are per queue. If queue is <0 then we default to queue 0 as the
+ * representative value.
  **/
-static int i40evf_get_coalesce(struct net_device *netdev,
-			       struct ethtool_coalesce *ec)
+static int __i40evf_get_coalesce(struct net_device *netdev,
+				 struct ethtool_coalesce *ec,
+				 int queue)
 {
 	struct i40evf_adapter *adapter = netdev_priv(netdev);
 	struct i40e_vsi *vsi = &adapter->vsi;
+	struct i40e_ring *rx_ring, *tx_ring;
 
 	ec->tx_max_coalesced_frames = vsi->work_limit;
 	ec->rx_max_coalesced_frames = vsi->work_limit;
 
-	if (ITR_IS_DYNAMIC(vsi->rx_itr_setting))
+	/* Rx and Tx usecs per queue value. If user doesn't specify the
+	 * queue, return queue 0's value to represent.
+	 */
+	if (queue < 0)
+		queue = 0;
+	else if (queue >= adapter->num_active_queues)
+		return -EINVAL;
+
+	rx_ring = &adapter->rx_rings[queue];
+	tx_ring = &adapter->tx_rings[queue];
+
+	if (ITR_IS_DYNAMIC(rx_ring->rx_itr_setting))
 		ec->use_adaptive_rx_coalesce = 1;
 
-	if (ITR_IS_DYNAMIC(vsi->tx_itr_setting))
+	if (ITR_IS_DYNAMIC(tx_ring->tx_itr_setting))
 		ec->use_adaptive_tx_coalesce = 1;
 
-	ec->rx_coalesce_usecs = vsi->rx_itr_setting & ~I40E_ITR_DYNAMIC;
-	ec->tx_coalesce_usecs = vsi->tx_itr_setting & ~I40E_ITR_DYNAMIC;
+	ec->rx_coalesce_usecs = rx_ring->rx_itr_setting & ~I40E_ITR_DYNAMIC;
+	ec->tx_coalesce_usecs = tx_ring->tx_itr_setting & ~I40E_ITR_DYNAMIC;
 
 	return 0;
 }
 
 /**
- * i40evf_set_coalesce - Set interrupt coalescing settings
+ * i40evf_get_coalesce - Get interrupt coalescing settings
  * @netdev: network interface device structure
  * @ec: ethtool coalesce structure
  *
- * Change current coalescing settings.
+ * Returns current coalescing settings. This is referred to elsewhere in the
+ * driver as Interrupt Throttle Rate, as this is how the hardware describes
+ * this functionality. Note that if per-queue settings have been modified this
+ * only represents the settings of queue 0.
  **/
-static int i40evf_set_coalesce(struct net_device *netdev,
+static int i40evf_get_coalesce(struct net_device *netdev,
 			       struct ethtool_coalesce *ec)
 {
-	struct i40evf_adapter *adapter = netdev_priv(netdev);
-	struct i40e_hw *hw = &adapter->hw;
+	return __i40evf_get_coalesce(netdev, ec, -1);
+}
+
+/**
+ * i40evf_get_per_queue_coalesce - get coalesce values for specific queue
+ * @netdev: netdev to read
+ * @ec: coalesce settings from ethtool
+ * @queue: the queue to read
+ *
+ * Read specific queue's coalesce settings.
+ **/
+static int i40evf_get_per_queue_coalesce(struct net_device *netdev,
+					 u32 queue,
+					 struct ethtool_coalesce *ec)
+{
+	return __i40evf_get_coalesce(netdev, ec, queue);
+}
+
+/**
+ * i40evf_set_itr_per_queue - set ITR values for specific queue
+ * @vsi: the VSI to set values for
+ * @ec: coalesce settings from ethtool
+ * @queue: the queue to modify
+ *
+ * Change the ITR settings for a specific queue.
+ **/
+static void i40evf_set_itr_per_queue(struct i40evf_adapter *adapter,
+				     struct ethtool_coalesce *ec,
+				     int queue)
+{
 	struct i40e_vsi *vsi = &adapter->vsi;
+	struct i40e_hw *hw = &adapter->hw;
 	struct i40e_q_vector *q_vector;
+	u16 vector;
+
+	adapter->rx_rings[queue].rx_itr_setting = ec->rx_coalesce_usecs;
+	adapter->tx_rings[queue].tx_itr_setting = ec->tx_coalesce_usecs;
+
+	if (ec->use_adaptive_rx_coalesce)
+		adapter->rx_rings[queue].rx_itr_setting |= I40E_ITR_DYNAMIC;
+	else
+		adapter->rx_rings[queue].rx_itr_setting &= ~I40E_ITR_DYNAMIC;
+
+	if (ec->use_adaptive_tx_coalesce)
+		adapter->tx_rings[queue].tx_itr_setting |= I40E_ITR_DYNAMIC;
+	else
+		adapter->tx_rings[queue].tx_itr_setting &= ~I40E_ITR_DYNAMIC;
+
+	q_vector = adapter->rx_rings[queue].q_vector;
+	q_vector->rx.itr = ITR_TO_REG(adapter->rx_rings[queue].rx_itr_setting);
+	vector = vsi->base_vector + q_vector->v_idx;
+	wr32(hw, I40E_VFINT_ITRN1(I40E_RX_ITR, vector - 1), q_vector->rx.itr);
+
+	q_vector = adapter->tx_rings[queue].q_vector;
+	q_vector->tx.itr = ITR_TO_REG(adapter->tx_rings[queue].tx_itr_setting);
+	vector = vsi->base_vector + q_vector->v_idx;
+	wr32(hw, I40E_VFINT_ITRN1(I40E_TX_ITR, vector - 1), q_vector->tx.itr);
+
+	i40e_flush(hw);
+}
+
+/**
+ * __i40evf_set_coalesce - set coalesce settings for particular queue
+ * @netdev: the netdev to change
+ * @ec: ethtool coalesce settings
+ * @queue: the queue to change
+ *
+ * Sets the coalesce settings for a particular queue.
+ **/
+static int __i40evf_set_coalesce(struct net_device *netdev,
+				 struct ethtool_coalesce *ec,
+				 int queue)
+{
+	struct i40evf_adapter *adapter = netdev_priv(netdev);
+	struct i40e_vsi *vsi = &adapter->vsi;
 	int i;
 
 	if (ec->tx_max_coalesced_frames_irq || ec->rx_max_coalesced_frames_irq)
 		vsi->work_limit = ec->tx_max_coalesced_frames_irq;
 
-	if ((ec->rx_coalesce_usecs >= (I40E_MIN_ITR << 1)) &&
-	    (ec->rx_coalesce_usecs <= (I40E_MAX_ITR << 1)))
-		vsi->rx_itr_setting = ec->rx_coalesce_usecs;
-
-	else
+	if (ec->rx_coalesce_usecs == 0) {
+		if (ec->use_adaptive_rx_coalesce)
+			netif_info(adapter, drv, netdev, "rx-usecs=0, need to disable adaptive-rx for a complete disable\n");
+	} else if ((ec->rx_coalesce_usecs < (I40E_MIN_ITR << 1)) ||
+		   (ec->rx_coalesce_usecs > (I40E_MAX_ITR << 1))) {
+		netif_info(adapter, drv, netdev, "Invalid value, rx-usecs range is 0-8160\n");
 		return -EINVAL;
+	}
 
-	if ((ec->tx_coalesce_usecs >= (I40E_MIN_ITR << 1)) &&
-	    (ec->tx_coalesce_usecs <= (I40E_MAX_ITR << 1)))
-		vsi->tx_itr_setting = ec->tx_coalesce_usecs;
-	else if (ec->use_adaptive_tx_coalesce)
-		vsi->tx_itr_setting = (I40E_ITR_DYNAMIC |
-				       ITR_REG_TO_USEC(I40E_ITR_RX_DEF));
 	else
+	if (ec->tx_coalesce_usecs == 0) {
+		if (ec->use_adaptive_tx_coalesce)
+			netif_info(adapter, drv, netdev, "tx-usecs=0, need to disable adaptive-tx for a complete disable\n");
+	} else if ((ec->tx_coalesce_usecs < (I40E_MIN_ITR << 1)) ||
+		   (ec->tx_coalesce_usecs > (I40E_MAX_ITR << 1))) {
+		netif_info(adapter, drv, netdev, "Invalid value, tx-usecs range is 0-8160\n");
 		return -EINVAL;
+	}
 
-	if (ec->use_adaptive_rx_coalesce)
-		vsi->rx_itr_setting |= I40E_ITR_DYNAMIC;
-	else
-		vsi->rx_itr_setting &= ~I40E_ITR_DYNAMIC;
-
-	if (ec->use_adaptive_tx_coalesce)
-		vsi->tx_itr_setting |= I40E_ITR_DYNAMIC;
-	else
-		vsi->tx_itr_setting &= ~I40E_ITR_DYNAMIC;
-
-	for (i = 0; i < adapter->num_msix_vectors - NONQ_VECS; i++) {
-		q_vector = &adapter->q_vectors[i];
-		q_vector->rx.itr = ITR_TO_REG(vsi->rx_itr_setting);
-		wr32(hw, I40E_VFINT_ITRN1(0, i), q_vector->rx.itr);
-		q_vector->tx.itr = ITR_TO_REG(vsi->tx_itr_setting);
-		wr32(hw, I40E_VFINT_ITRN1(1, i), q_vector->tx.itr);
-		i40e_flush(hw);
+	/* Rx and Tx usecs has per queue value. If user doesn't specify the
+	 * queue, apply to all queues.
+	 */
+	if (queue < 0) {
+		for (i = 0; i < adapter->num_active_queues; i++)
+			i40evf_set_itr_per_queue(adapter, ec, i);
+	} else if (queue < adapter->num_active_queues) {
+		i40evf_set_itr_per_queue(adapter, ec, queue);
+	} else {
+		netif_info(adapter, drv, netdev, "Invalid queue value, queue range is 0 - %d\n",
+			   adapter->num_active_queues - 1);
+		return -EINVAL;
 	}
 
 	return 0;
 }
 
 /**
+ * i40evf_set_coalesce - Set interrupt coalescing settings
+ * @netdev: network interface device structure
+ * @ec: ethtool coalesce structure
+ *
+ * Change current coalescing settings for every queue.
+ **/
+static int i40evf_set_coalesce(struct net_device *netdev,
+			       struct ethtool_coalesce *ec)
+{
+	return __i40evf_set_coalesce(netdev, ec, -1);
+}
+
+/**
+ * i40evf_set_per_queue_coalesce - set specific queue's coalesce settings
+ * @netdev: the netdev to change
+ * @ec: ethtool's coalesce settings
+ * @queue: the queue to modify
+ *
+ * Modifies a specific queue's coalesce settings.
+ */
+static int i40evf_set_per_queue_coalesce(struct net_device *netdev,
+					 u32 queue,
+					 struct ethtool_coalesce *ec)
+{
+	return __i40evf_set_coalesce(netdev, ec, queue);
+}
+
+/**
  * i40evf_get_rxnfc - command to get RX flow classification rules
  * @netdev: network interface device structure
  * @cmd: ethtool rxnfc command
@@ -513,6 +647,8 @@ static const struct ethtool_ops i40evf_ethtool_ops = {
 	.set_msglevel		= i40evf_set_msglevel,
 	.get_coalesce		= i40evf_get_coalesce,
 	.set_coalesce		= i40evf_set_coalesce,
+	.get_per_queue_coalesce = i40evf_get_per_queue_coalesce,
+	.set_per_queue_coalesce = i40evf_set_per_queue_coalesce,
 	.get_rxnfc		= i40evf_get_rxnfc,
 	.get_rxfh_indir_size	= i40evf_get_rxfh_indir_size,
 	.get_rxfh		= i40evf_get_rxfh,
diff --git a/drivers/net/ethernet/intel/i40evf/i40evf_main.c b/drivers/net/ethernet/intel/i40evf/i40evf_main.c
index 600fb9c4a7f0..14372810fc27 100644
--- a/drivers/net/ethernet/intel/i40evf/i40evf_main.c
+++ b/drivers/net/ethernet/intel/i40evf/i40evf_main.c
@@ -38,7 +38,7 @@ static const char i40evf_driver_string[] =
 
 #define DRV_VERSION_MAJOR 1
 #define DRV_VERSION_MINOR 6
-#define DRV_VERSION_BUILD 11
+#define DRV_VERSION_BUILD 16
 #define DRV_VERSION __stringify(DRV_VERSION_MAJOR) "." \
 	     __stringify(DRV_VERSION_MINOR) "." \
 	     __stringify(DRV_VERSION_BUILD) \
@@ -370,6 +370,7 @@ i40evf_map_vector_to_rxq(struct i40evf_adapter *adapter, int v_idx, int r_idx)
 {
 	struct i40e_q_vector *q_vector = &adapter->q_vectors[v_idx];
 	struct i40e_ring *rx_ring = &adapter->rx_rings[r_idx];
+	struct i40e_hw *hw = &adapter->hw;
 
 	rx_ring->q_vector = q_vector;
 	rx_ring->next = q_vector->rx.ring;
@@ -377,7 +378,10 @@ i40evf_map_vector_to_rxq(struct i40evf_adapter *adapter, int v_idx, int r_idx)
 	q_vector->rx.ring = rx_ring;
 	q_vector->rx.count++;
 	q_vector->rx.latency_range = I40E_LOW_LATENCY;
+	q_vector->rx.itr = ITR_TO_REG(rx_ring->rx_itr_setting);
+	q_vector->ring_mask |= BIT(r_idx);
 	q_vector->itr_countdown = ITR_COUNTDOWN_START;
+	wr32(hw, I40E_VFINT_ITRN1(I40E_RX_ITR, v_idx - 1), q_vector->rx.itr);
 }
 
 /**
@@ -391,6 +395,7 @@ i40evf_map_vector_to_txq(struct i40evf_adapter *adapter, int v_idx, int t_idx)
 {
 	struct i40e_q_vector *q_vector = &adapter->q_vectors[v_idx];
 	struct i40e_ring *tx_ring = &adapter->tx_rings[t_idx];
+	struct i40e_hw *hw = &adapter->hw;
 
 	tx_ring->q_vector = q_vector;
 	tx_ring->next = q_vector->tx.ring;
@@ -398,9 +403,10 @@ i40evf_map_vector_to_txq(struct i40evf_adapter *adapter, int v_idx, int t_idx)
 	q_vector->tx.ring = tx_ring;
 	q_vector->tx.count++;
 	q_vector->tx.latency_range = I40E_LOW_LATENCY;
+	q_vector->tx.itr = ITR_TO_REG(tx_ring->tx_itr_setting);
 	q_vector->itr_countdown = ITR_COUNTDOWN_START;
 	q_vector->num_ringpairs++;
-	q_vector->ring_mask |= BIT(t_idx);
+	wr32(hw, I40E_VFINT_ITRN1(I40E_TX_ITR, v_idx - 1), q_vector->tx.itr);
 }
 
 /**
@@ -1007,7 +1013,7 @@ static void i40evf_configure(struct i40evf_adapter *adapter)
  * i40evf_up_complete - Finish the last steps of bringing up a connection
  * @adapter: board private structure
  **/
-static int i40evf_up_complete(struct i40evf_adapter *adapter)
+static void i40evf_up_complete(struct i40evf_adapter *adapter)
 {
 	adapter->state = __I40EVF_RUNNING;
 	clear_bit(__I40E_DOWN, &adapter->vsi.state);
@@ -1016,7 +1022,6 @@ static int i40evf_up_complete(struct i40evf_adapter *adapter)
 
 	adapter->aq_required |= I40EVF_FLAG_AQ_ENABLE_QUEUES;
 	mod_timer_pending(&adapter->watchdog_timer, jiffies + 1);
-	return 0;
 }
 
 /**
@@ -1037,6 +1042,7 @@ void i40evf_down(struct i40evf_adapter *adapter)
 
 	netif_carrier_off(netdev);
 	netif_tx_disable(netdev);
+	adapter->link_up = false;
 	i40evf_napi_disable_all(adapter);
 	i40evf_irq_disable(adapter);
 
@@ -1154,6 +1160,7 @@ static int i40evf_alloc_queues(struct i40evf_adapter *adapter)
 		tx_ring->netdev = adapter->netdev;
 		tx_ring->dev = &adapter->pdev->dev;
 		tx_ring->count = adapter->tx_desc_count;
+		tx_ring->tx_itr_setting = (I40E_ITR_DYNAMIC | I40E_ITR_TX_DEF);
 		if (adapter->flags & I40E_FLAG_WB_ON_ITR_CAPABLE)
 			tx_ring->flags |= I40E_TXR_FLAGS_WB_ON_ITR;
 
@@ -1162,6 +1169,7 @@ static int i40evf_alloc_queues(struct i40evf_adapter *adapter)
 		rx_ring->netdev = adapter->netdev;
 		rx_ring->dev = &adapter->pdev->dev;
 		rx_ring->count = adapter->rx_desc_count;
+		rx_ring->rx_itr_setting = (I40E_ITR_DYNAMIC | I40E_ITR_RX_DEF);
 	}
 
 	return 0;
@@ -1420,7 +1428,9 @@ int i40evf_init_interrupt_scheme(struct i40evf_adapter *adapter)
 {
 	int err;
 
+	rtnl_lock();
 	err = i40evf_set_interrupt_capability(adapter);
+	rtnl_unlock();
 	if (err) {
 		dev_err(&adapter->pdev->dev,
 			"Unable to setup interrupt capabilities\n");
@@ -1729,6 +1739,7 @@ static void i40evf_reset_task(struct work_struct *work)
 			set_bit(__I40E_DOWN, &adapter->vsi.state);
 			netif_carrier_off(netdev);
 			netif_tx_disable(netdev);
+			adapter->link_up = false;
 			i40evf_napi_disable_all(adapter);
 			i40evf_irq_disable(adapter);
 			i40evf_free_traffic_irqs(adapter);
@@ -1767,6 +1778,7 @@ continue_reset:
 	if (netif_running(adapter->netdev)) {
 		netif_carrier_off(netdev);
 		netif_tx_stop_all_queues(netdev);
+		adapter->link_up = false;
 		i40evf_napi_disable_all(adapter);
 	}
 	i40evf_irq_disable(adapter);
@@ -1781,8 +1793,7 @@ continue_reset:
 	i40evf_free_all_tx_resources(adapter);
 
 	/* kill and reinit the admin queue */
-	if (i40evf_shutdown_adminq(hw))
-		dev_warn(&adapter->pdev->dev, "Failed to shut down adminq\n");
+	i40evf_shutdown_adminq(hw);
 	adapter->current_op = I40E_VIRTCHNL_OP_UNKNOWN;
 	err = i40evf_init_adminq(hw);
 	if (err)
@@ -1802,6 +1813,8 @@ continue_reset:
 	}
 	adapter->aq_required |= I40EVF_FLAG_AQ_ADD_MAC_FILTER;
 	adapter->aq_required |= I40EVF_FLAG_AQ_ADD_VLAN_FILTER;
+	/* Open RDMA Client again */
+	adapter->aq_required |= I40EVF_FLAG_SERVICE_CLIENT_REQUESTED;
 	clear_bit(__I40EVF_IN_CRITICAL_TASK, &adapter->crit_section);
 	i40evf_misc_irq_enable(adapter);
 
@@ -1820,9 +1833,7 @@ continue_reset:
 
 		i40evf_configure(adapter);
 
-		err = i40evf_up_complete(adapter);
-		if (err)
-			goto reset_err;
+		i40evf_up_complete(adapter);
 
 		i40evf_irq_enable(adapter, true);
 	} else {
@@ -2052,9 +2063,7 @@ static int i40evf_open(struct net_device *netdev)
 	i40evf_add_filter(adapter, adapter->hw.mac.addr);
 	i40evf_configure(adapter);
 
-	err = i40evf_up_complete(adapter);
-	if (err)
-		goto err_req_irq;
+	i40evf_up_complete(adapter);
 
 	i40evf_irq_enable(adapter, true);
 
@@ -2268,10 +2277,6 @@ int i40evf_process_config(struct i40evf_adapter *adapter)
 	adapter->vsi.back = adapter;
 	adapter->vsi.base_vector = 1;
 	adapter->vsi.work_limit = I40E_DEFAULT_IRQ_WORK;
-	adapter->vsi.rx_itr_setting = (I40E_ITR_DYNAMIC |
-				       ITR_REG_TO_USEC(I40E_ITR_RX_DEF));
-	adapter->vsi.tx_itr_setting = (I40E_ITR_DYNAMIC |
-				       ITR_REG_TO_USEC(I40E_ITR_TX_DEF));
 	vsi->netdev = adapter->netdev;
 	vsi->qs_handle = adapter->vsi_res->qset_handle;
 	if (vfres->vf_offload_flags & I40E_VIRTCHNL_VF_OFFLOAD_RSS_PF) {
@@ -2453,6 +2458,7 @@ static void i40evf_init_task(struct work_struct *work)
 		goto err_sw_init;
 
 	netif_carrier_off(netdev);
+	adapter->link_up = false;
 
 	if (!adapter->netdev_registered) {
 		err = register_netdev(netdev);
@@ -2831,7 +2837,8 @@ static int __init i40evf_init_module(void)
 
 	pr_info("%s\n", i40evf_copyright);
 
-	i40evf_wq = create_singlethread_workqueue(i40evf_driver_name);
+	i40evf_wq = alloc_workqueue("%s", WQ_UNBOUND | WQ_MEM_RECLAIM, 1,
+				    i40evf_driver_name);
 	if (!i40evf_wq) {
 		pr_err("%s: Failed to create workqueue\n", i40evf_driver_name);
 		return -ENOMEM;
diff --git a/drivers/net/ethernet/intel/i40evf/i40evf_virtchnl.c b/drivers/net/ethernet/intel/i40evf/i40evf_virtchnl.c
index d76c221d4c8a..ddf478d6322b 100644
--- a/drivers/net/ethernet/intel/i40evf/i40evf_virtchnl.c
+++ b/drivers/net/ethernet/intel/i40evf/i40evf_virtchnl.c
@@ -817,6 +817,45 @@ void i40evf_set_rss_lut(struct i40evf_adapter *adapter)
 }
 
 /**
+ * i40evf_print_link_message - print link up or down
+ * @adapter: adapter structure
+ *
+ * Log a message telling the world of our wonderous link status
+ */
+static void i40evf_print_link_message(struct i40evf_adapter *adapter)
+{
+	struct net_device *netdev = adapter->netdev;
+	char *speed = "Unknown ";
+
+	if (!adapter->link_up) {
+		netdev_info(netdev, "NIC Link is Down\n");
+		return;
+	}
+
+	switch (adapter->link_speed) {
+	case I40E_LINK_SPEED_40GB:
+		speed = "40 G";
+		break;
+	case I40E_LINK_SPEED_20GB:
+		speed = "20 G";
+		break;
+	case I40E_LINK_SPEED_10GB:
+		speed = "10 G";
+		break;
+	case I40E_LINK_SPEED_1GB:
+		speed = "1000 M";
+		break;
+	case I40E_LINK_SPEED_100MB:
+		speed = "100 M";
+		break;
+	default:
+		break;
+	}
+
+	netdev_info(netdev, "NIC Link is Up %sbps Full Duplex\n", speed);
+}
+
+/**
  * i40evf_request_reset
  * @adapter: adapter structure
  *
@@ -853,16 +892,20 @@ void i40evf_virtchnl_completion(struct i40evf_adapter *adapter,
 			(struct i40e_virtchnl_pf_event *)msg;
 		switch (vpe->event) {
 		case I40E_VIRTCHNL_EVENT_LINK_CHANGE:
-			adapter->link_up =
-				vpe->event_data.link_event.link_status;
-			if (adapter->link_up && !netif_carrier_ok(netdev)) {
-				dev_info(&adapter->pdev->dev, "NIC Link is Up\n");
-				netif_carrier_on(netdev);
-				netif_tx_wake_all_queues(netdev);
-			} else if (!adapter->link_up) {
-				dev_info(&adapter->pdev->dev, "NIC Link is Down\n");
-				netif_carrier_off(netdev);
-				netif_tx_stop_all_queues(netdev);
+			adapter->link_speed =
+				vpe->event_data.link_event.link_speed;
+			if (adapter->link_up !=
+			    vpe->event_data.link_event.link_status) {
+				adapter->link_up =
+					vpe->event_data.link_event.link_status;
+				if (adapter->link_up) {
+					netif_tx_start_all_queues(netdev);
+					netif_carrier_on(netdev);
+				} else {
+					netif_tx_stop_all_queues(netdev);
+					netif_carrier_off(netdev);
+				}
+				i40evf_print_link_message(adapter);
 			}
 			break;
 		case I40E_VIRTCHNL_EVENT_RESET_IMPENDING:
@@ -937,8 +980,6 @@ void i40evf_virtchnl_completion(struct i40evf_adapter *adapter,
 	case I40E_VIRTCHNL_OP_ENABLE_QUEUES:
 		/* enable transmits */
 		i40evf_irq_enable(adapter, true);
-		netif_tx_start_all_queues(adapter->netdev);
-		netif_carrier_on(adapter->netdev);
 		break;
 	case I40E_VIRTCHNL_OP_DISABLE_QUEUES:
 		i40evf_free_all_tx_resources(adapter);
diff --git a/drivers/net/ethernet/intel/igb/e1000_82575.h b/drivers/net/ethernet/intel/igb/e1000_82575.h
index 199ff98209cf..acf06051e111 100644
--- a/drivers/net/ethernet/intel/igb/e1000_82575.h
+++ b/drivers/net/ethernet/intel/igb/e1000_82575.h
@@ -188,6 +188,11 @@ struct e1000_adv_tx_context_desc {
 /* ETQF register bit definitions */
 #define E1000_ETQF_FILTER_ENABLE   BIT(26)
 #define E1000_ETQF_1588            BIT(30)
+#define E1000_ETQF_IMM_INT         BIT(29)
+#define E1000_ETQF_QUEUE_ENABLE    BIT(31)
+#define E1000_ETQF_QUEUE_SHIFT     16
+#define E1000_ETQF_QUEUE_MASK      0x00070000
+#define E1000_ETQF_ETYPE_MASK      0x0000FFFF
 
 /* FTQF register bit definitions */
 #define E1000_FTQF_VF_BP               0x00008000
diff --git a/drivers/net/ethernet/intel/igb/e1000_defines.h b/drivers/net/ethernet/intel/igb/e1000_defines.h
index 2997c443c5dc..2688180a7acd 100644
--- a/drivers/net/ethernet/intel/igb/e1000_defines.h
+++ b/drivers/net/ethernet/intel/igb/e1000_defines.h
@@ -1024,4 +1024,8 @@
 #define E1000_RTTBCNRC_RF_INT_MASK	\
 	(E1000_RTTBCNRC_RF_DEC_MASK << E1000_RTTBCNRC_RF_INT_SHIFT)
 
+#define E1000_VLAPQF_QUEUE_SEL(_n, q_idx) (q_idx << ((_n) * 4))
+#define E1000_VLAPQF_P_VALID(_n)	(0x1 << (3 + (_n) * 4))
+#define E1000_VLAPQF_QUEUE_MASK	0x03
+
 #endif
diff --git a/drivers/net/ethernet/intel/igb/e1000_regs.h b/drivers/net/ethernet/intel/igb/e1000_regs.h
index 21d9d02885cb..d84afdd83e53 100644
--- a/drivers/net/ethernet/intel/igb/e1000_regs.h
+++ b/drivers/net/ethernet/intel/igb/e1000_regs.h
@@ -309,6 +309,7 @@
 					(0x054E0 + ((_i - 16) * 8)))
 #define E1000_RAH(_i)  (((_i) <= 15) ? (0x05404 + ((_i) * 8)) : \
 					(0x054E4 + ((_i - 16) * 8)))
+#define E1000_VLAPQF	0x055B0  /* VLAN Priority Queue Filter VLAPQF */
 #define E1000_IP4AT_REG(_i)     (0x05840 + ((_i) * 8))
 #define E1000_IP6AT_REG(_i)     (0x05880 + ((_i) * 4))
 #define E1000_WUPM_REG(_i)      (0x05A00 + ((_i) * 4))
diff --git a/drivers/net/ethernet/intel/igb/igb.h b/drivers/net/ethernet/intel/igb/igb.h
index 5387b3a96489..d11093dce1b9 100644
--- a/drivers/net/ethernet/intel/igb/igb.h
+++ b/drivers/net/ethernet/intel/igb/igb.h
@@ -350,11 +350,49 @@ struct hwmon_buff {
 	};
 #endif
 
+/* The number of L2 ether-type filter registers, Index 3 is reserved
+ * for PTP 1588 timestamp
+ */
+#define MAX_ETYPE_FILTER	(4 - 1)
+/* ETQF filter list: one static filter per filter consumer. This is
+ * to avoid filter collisions later. Add new filters here!!
+ *
+ * Current filters:		Filter 3
+ */
+#define IGB_ETQF_FILTER_1588	3
+
 #define IGB_N_EXTTS	2
 #define IGB_N_PEROUT	2
 #define IGB_N_SDP	4
 #define IGB_RETA_SIZE	128
 
+enum igb_filter_match_flags {
+	IGB_FILTER_FLAG_ETHER_TYPE = 0x1,
+	IGB_FILTER_FLAG_VLAN_TCI   = 0x2,
+};
+
+#define IGB_MAX_RXNFC_FILTERS 16
+
+/* RX network flow classification data structure */
+struct igb_nfc_input {
+	/* Byte layout in order, all values with MSB first:
+	 * match_flags - 1 byte
+	 * etype - 2 bytes
+	 * vlan_tci - 2 bytes
+	 */
+	u8 match_flags;
+	__be16 etype;
+	__be16 vlan_tci;
+};
+
+struct igb_nfc_filter {
+	struct hlist_node nfc_node;
+	struct igb_nfc_input filter;
+	u16 etype_reg_index;
+	u16 sw_idx;
+	u16 action;
+};
+
 /* board specific private data structure */
 struct igb_adapter {
 	unsigned long active_vlans[BITS_TO_LONGS(VLAN_N_VID)];
@@ -451,6 +489,7 @@ struct igb_adapter {
 	struct timecounter tc;
 	u32 tx_hwtstamp_timeouts;
 	u32 rx_hwtstamp_cleared;
+	bool pps_sys_wrap_on;
 
 	struct ptp_pin_desc sdp_config[IGB_N_SDP];
 	struct {
@@ -473,6 +512,13 @@ struct igb_adapter {
 	int copper_tries;
 	struct e1000_info ei;
 	u16 eee_advert;
+
+	/* RX network flow classification support */
+	struct hlist_head nfc_filter_list;
+	unsigned int nfc_filter_count;
+	/* lock for RX network flow classification filter */
+	spinlock_t nfc_lock;
+	bool etype_bitmap[MAX_ETYPE_FILTER];
 };
 
 /* flags controlling PTP/1588 function */
@@ -599,4 +645,9 @@ static inline struct netdev_queue *txring_txq(const struct igb_ring *tx_ring)
 	return netdev_get_tx_queue(tx_ring->netdev, tx_ring->queue_index);
 }
 
+int igb_add_filter(struct igb_adapter *adapter,
+		   struct igb_nfc_filter *input);
+int igb_erase_filter(struct igb_adapter *adapter,
+		     struct igb_nfc_filter *input);
+
 #endif /* _IGB_H_ */
diff --git a/drivers/net/ethernet/intel/igb/igb_ethtool.c b/drivers/net/ethernet/intel/igb/igb_ethtool.c
index 64e91c575a39..737b664d004c 100644
--- a/drivers/net/ethernet/intel/igb/igb_ethtool.c
+++ b/drivers/net/ethernet/intel/igb/igb_ethtool.c
@@ -2431,6 +2431,63 @@ static int igb_get_ts_info(struct net_device *dev,
 	}
 }
 
+#define ETHER_TYPE_FULL_MASK ((__force __be16)~0)
+static int igb_get_ethtool_nfc_entry(struct igb_adapter *adapter,
+				     struct ethtool_rxnfc *cmd)
+{
+	struct ethtool_rx_flow_spec *fsp = &cmd->fs;
+	struct igb_nfc_filter *rule = NULL;
+
+	/* report total rule count */
+	cmd->data = IGB_MAX_RXNFC_FILTERS;
+
+	hlist_for_each_entry(rule, &adapter->nfc_filter_list, nfc_node) {
+		if (fsp->location <= rule->sw_idx)
+			break;
+	}
+
+	if (!rule || fsp->location != rule->sw_idx)
+		return -EINVAL;
+
+	if (rule->filter.match_flags) {
+		fsp->flow_type = ETHER_FLOW;
+		fsp->ring_cookie = rule->action;
+		if (rule->filter.match_flags & IGB_FILTER_FLAG_ETHER_TYPE) {
+			fsp->h_u.ether_spec.h_proto = rule->filter.etype;
+			fsp->m_u.ether_spec.h_proto = ETHER_TYPE_FULL_MASK;
+		}
+		if (rule->filter.match_flags & IGB_FILTER_FLAG_VLAN_TCI) {
+			fsp->flow_type |= FLOW_EXT;
+			fsp->h_ext.vlan_tci = rule->filter.vlan_tci;
+			fsp->m_ext.vlan_tci = htons(VLAN_PRIO_MASK);
+		}
+		return 0;
+	}
+	return -EINVAL;
+}
+
+static int igb_get_ethtool_nfc_all(struct igb_adapter *adapter,
+				   struct ethtool_rxnfc *cmd,
+				   u32 *rule_locs)
+{
+	struct igb_nfc_filter *rule;
+	int cnt = 0;
+
+	/* report total rule count */
+	cmd->data = IGB_MAX_RXNFC_FILTERS;
+
+	hlist_for_each_entry(rule, &adapter->nfc_filter_list, nfc_node) {
+		if (cnt == cmd->rule_cnt)
+			return -EMSGSIZE;
+		rule_locs[cnt] = rule->sw_idx;
+		cnt++;
+	}
+
+	cmd->rule_cnt = cnt;
+
+	return 0;
+}
+
 static int igb_get_rss_hash_opts(struct igb_adapter *adapter,
 				 struct ethtool_rxnfc *cmd)
 {
@@ -2484,6 +2541,16 @@ static int igb_get_rxnfc(struct net_device *dev, struct ethtool_rxnfc *cmd,
 		cmd->data = adapter->num_rx_queues;
 		ret = 0;
 		break;
+	case ETHTOOL_GRXCLSRLCNT:
+		cmd->rule_cnt = adapter->nfc_filter_count;
+		ret = 0;
+		break;
+	case ETHTOOL_GRXCLSRULE:
+		ret = igb_get_ethtool_nfc_entry(adapter, cmd);
+		break;
+	case ETHTOOL_GRXCLSRLALL:
+		ret = igb_get_ethtool_nfc_all(adapter, cmd, rule_locs);
+		break;
 	case ETHTOOL_GRXFH:
 		ret = igb_get_rss_hash_opts(adapter, cmd);
 		break;
@@ -2598,6 +2665,279 @@ static int igb_set_rss_hash_opt(struct igb_adapter *adapter,
 	return 0;
 }
 
+static int igb_rxnfc_write_etype_filter(struct igb_adapter *adapter,
+					struct igb_nfc_filter *input)
+{
+	struct e1000_hw *hw = &adapter->hw;
+	u8 i;
+	u32 etqf;
+	u16 etype;
+
+	/* find an empty etype filter register */
+	for (i = 0; i < MAX_ETYPE_FILTER; ++i) {
+		if (!adapter->etype_bitmap[i])
+			break;
+	}
+	if (i == MAX_ETYPE_FILTER) {
+		dev_err(&adapter->pdev->dev, "ethtool -N: etype filters are all used.\n");
+		return -EINVAL;
+	}
+
+	adapter->etype_bitmap[i] = true;
+
+	etqf = rd32(E1000_ETQF(i));
+	etype = ntohs(input->filter.etype & ETHER_TYPE_FULL_MASK);
+
+	etqf |= E1000_ETQF_FILTER_ENABLE;
+	etqf &= ~E1000_ETQF_ETYPE_MASK;
+	etqf |= (etype & E1000_ETQF_ETYPE_MASK);
+
+	etqf &= ~E1000_ETQF_QUEUE_MASK;
+	etqf |= ((input->action << E1000_ETQF_QUEUE_SHIFT)
+		& E1000_ETQF_QUEUE_MASK);
+	etqf |= E1000_ETQF_QUEUE_ENABLE;
+
+	wr32(E1000_ETQF(i), etqf);
+
+	input->etype_reg_index = i;
+
+	return 0;
+}
+
+static int igb_rxnfc_write_vlan_prio_filter(struct igb_adapter *adapter,
+					    struct igb_nfc_filter *input)
+{
+	struct e1000_hw *hw = &adapter->hw;
+	u8 vlan_priority;
+	u16 queue_index;
+	u32 vlapqf;
+
+	vlapqf = rd32(E1000_VLAPQF);
+	vlan_priority = (ntohs(input->filter.vlan_tci) & VLAN_PRIO_MASK)
+				>> VLAN_PRIO_SHIFT;
+	queue_index = (vlapqf >> (vlan_priority * 4)) & E1000_VLAPQF_QUEUE_MASK;
+
+	/* check whether this vlan prio is already set */
+	if ((vlapqf & E1000_VLAPQF_P_VALID(vlan_priority)) &&
+	    (queue_index != input->action)) {
+		dev_err(&adapter->pdev->dev, "ethtool rxnfc set vlan prio filter failed.\n");
+		return -EEXIST;
+	}
+
+	vlapqf |= E1000_VLAPQF_P_VALID(vlan_priority);
+	vlapqf |= E1000_VLAPQF_QUEUE_SEL(vlan_priority, input->action);
+
+	wr32(E1000_VLAPQF, vlapqf);
+
+	return 0;
+}
+
+int igb_add_filter(struct igb_adapter *adapter, struct igb_nfc_filter *input)
+{
+	int err = -EINVAL;
+
+	if (input->filter.match_flags & IGB_FILTER_FLAG_ETHER_TYPE) {
+		err = igb_rxnfc_write_etype_filter(adapter, input);
+		if (err)
+			return err;
+	}
+
+	if (input->filter.match_flags & IGB_FILTER_FLAG_VLAN_TCI)
+		err = igb_rxnfc_write_vlan_prio_filter(adapter, input);
+
+	return err;
+}
+
+static void igb_clear_etype_filter_regs(struct igb_adapter *adapter,
+					u16 reg_index)
+{
+	struct e1000_hw *hw = &adapter->hw;
+	u32 etqf = rd32(E1000_ETQF(reg_index));
+
+	etqf &= ~E1000_ETQF_QUEUE_ENABLE;
+	etqf &= ~E1000_ETQF_QUEUE_MASK;
+	etqf &= ~E1000_ETQF_FILTER_ENABLE;
+
+	wr32(E1000_ETQF(reg_index), etqf);
+
+	adapter->etype_bitmap[reg_index] = false;
+}
+
+static void igb_clear_vlan_prio_filter(struct igb_adapter *adapter,
+				       u16 vlan_tci)
+{
+	struct e1000_hw *hw = &adapter->hw;
+	u8 vlan_priority;
+	u32 vlapqf;
+
+	vlan_priority = (vlan_tci & VLAN_PRIO_MASK) >> VLAN_PRIO_SHIFT;
+
+	vlapqf = rd32(E1000_VLAPQF);
+	vlapqf &= ~E1000_VLAPQF_P_VALID(vlan_priority);
+	vlapqf &= ~E1000_VLAPQF_QUEUE_SEL(vlan_priority,
+						E1000_VLAPQF_QUEUE_MASK);
+
+	wr32(E1000_VLAPQF, vlapqf);
+}
+
+int igb_erase_filter(struct igb_adapter *adapter, struct igb_nfc_filter *input)
+{
+	if (input->filter.match_flags & IGB_FILTER_FLAG_ETHER_TYPE)
+		igb_clear_etype_filter_regs(adapter,
+					    input->etype_reg_index);
+
+	if (input->filter.match_flags & IGB_FILTER_FLAG_VLAN_TCI)
+		igb_clear_vlan_prio_filter(adapter,
+					   ntohs(input->filter.vlan_tci));
+
+	return 0;
+}
+
+static int igb_update_ethtool_nfc_entry(struct igb_adapter *adapter,
+					struct igb_nfc_filter *input,
+					u16 sw_idx)
+{
+	struct igb_nfc_filter *rule, *parent;
+	int err = -EINVAL;
+
+	parent = NULL;
+	rule = NULL;
+
+	hlist_for_each_entry(rule, &adapter->nfc_filter_list, nfc_node) {
+		/* hash found, or no matching entry */
+		if (rule->sw_idx >= sw_idx)
+			break;
+		parent = rule;
+	}
+
+	/* if there is an old rule occupying our place remove it */
+	if (rule && (rule->sw_idx == sw_idx)) {
+		if (!input)
+			err = igb_erase_filter(adapter, rule);
+
+		hlist_del(&rule->nfc_node);
+		kfree(rule);
+		adapter->nfc_filter_count--;
+	}
+
+	/* If no input this was a delete, err should be 0 if a rule was
+	 * successfully found and removed from the list else -EINVAL
+	 */
+	if (!input)
+		return err;
+
+	/* initialize node */
+	INIT_HLIST_NODE(&input->nfc_node);
+
+	/* add filter to the list */
+	if (parent)
+		hlist_add_behind(&parent->nfc_node, &input->nfc_node);
+	else
+		hlist_add_head(&input->nfc_node, &adapter->nfc_filter_list);
+
+	/* update counts */
+	adapter->nfc_filter_count++;
+
+	return 0;
+}
+
+static int igb_add_ethtool_nfc_entry(struct igb_adapter *adapter,
+				     struct ethtool_rxnfc *cmd)
+{
+	struct net_device *netdev = adapter->netdev;
+	struct ethtool_rx_flow_spec *fsp =
+		(struct ethtool_rx_flow_spec *)&cmd->fs;
+	struct igb_nfc_filter *input, *rule;
+	int err = 0;
+
+	if (!(netdev->hw_features & NETIF_F_NTUPLE))
+		return -EOPNOTSUPP;
+
+	/* Don't allow programming if the action is a queue greater than
+	 * the number of online Rx queues.
+	 */
+	if ((fsp->ring_cookie == RX_CLS_FLOW_DISC) ||
+	    (fsp->ring_cookie >= adapter->num_rx_queues)) {
+		dev_err(&adapter->pdev->dev, "ethtool -N: The specified action is invalid\n");
+		return -EINVAL;
+	}
+
+	/* Don't allow indexes to exist outside of available space */
+	if (fsp->location >= IGB_MAX_RXNFC_FILTERS) {
+		dev_err(&adapter->pdev->dev, "Location out of range\n");
+		return -EINVAL;
+	}
+
+	if ((fsp->flow_type & ~FLOW_EXT) != ETHER_FLOW)
+		return -EINVAL;
+
+	if (fsp->m_u.ether_spec.h_proto != ETHER_TYPE_FULL_MASK &&
+	    fsp->m_ext.vlan_tci != htons(VLAN_PRIO_MASK))
+		return -EINVAL;
+
+	input = kzalloc(sizeof(*input), GFP_KERNEL);
+	if (!input)
+		return -ENOMEM;
+
+	if (fsp->m_u.ether_spec.h_proto == ETHER_TYPE_FULL_MASK) {
+		input->filter.etype = fsp->h_u.ether_spec.h_proto;
+		input->filter.match_flags = IGB_FILTER_FLAG_ETHER_TYPE;
+	}
+
+	if ((fsp->flow_type & FLOW_EXT) && fsp->m_ext.vlan_tci) {
+		if (fsp->m_ext.vlan_tci != htons(VLAN_PRIO_MASK)) {
+			err = -EINVAL;
+			goto err_out;
+		}
+		input->filter.vlan_tci = fsp->h_ext.vlan_tci;
+		input->filter.match_flags |= IGB_FILTER_FLAG_VLAN_TCI;
+	}
+
+	input->action = fsp->ring_cookie;
+	input->sw_idx = fsp->location;
+
+	spin_lock(&adapter->nfc_lock);
+
+	hlist_for_each_entry(rule, &adapter->nfc_filter_list, nfc_node) {
+		if (!memcmp(&input->filter, &rule->filter,
+			    sizeof(input->filter))) {
+			err = -EEXIST;
+			dev_err(&adapter->pdev->dev,
+				"ethtool: this filter is already set\n");
+			goto err_out_w_lock;
+		}
+	}
+
+	err = igb_add_filter(adapter, input);
+	if (err)
+		goto err_out_w_lock;
+
+	igb_update_ethtool_nfc_entry(adapter, input, input->sw_idx);
+
+	spin_unlock(&adapter->nfc_lock);
+	return 0;
+
+err_out_w_lock:
+	spin_unlock(&adapter->nfc_lock);
+err_out:
+	kfree(input);
+	return err;
+}
+
+static int igb_del_ethtool_nfc_entry(struct igb_adapter *adapter,
+				     struct ethtool_rxnfc *cmd)
+{
+	struct ethtool_rx_flow_spec *fsp =
+		(struct ethtool_rx_flow_spec *)&cmd->fs;
+	int err;
+
+	spin_lock(&adapter->nfc_lock);
+	err = igb_update_ethtool_nfc_entry(adapter, NULL, fsp->location);
+	spin_unlock(&adapter->nfc_lock);
+
+	return err;
+}
+
 static int igb_set_rxnfc(struct net_device *dev, struct ethtool_rxnfc *cmd)
 {
 	struct igb_adapter *adapter = netdev_priv(dev);
@@ -2607,6 +2947,11 @@ static int igb_set_rxnfc(struct net_device *dev, struct ethtool_rxnfc *cmd)
 	case ETHTOOL_SRXFH:
 		ret = igb_set_rss_hash_opt(adapter, cmd);
 		break;
+	case ETHTOOL_SRXCLSRLINS:
+		ret = igb_add_ethtool_nfc_entry(adapter, cmd);
+		break;
+	case ETHTOOL_SRXCLSRLDEL:
+		ret = igb_del_ethtool_nfc_entry(adapter, cmd);
 	default:
 		break;
 	}
diff --git a/drivers/net/ethernet/intel/igb/igb_main.c b/drivers/net/ethernet/intel/igb/igb_main.c
index 942a89fb0090..edc9a6ac5169 100644
--- a/drivers/net/ethernet/intel/igb/igb_main.c
+++ b/drivers/net/ethernet/intel/igb/igb_main.c
@@ -58,7 +58,7 @@
 #include "igb.h"
 
 #define MAJ 5
-#define MIN 3
+#define MIN 4
 #define BUILD 0
 #define DRV_VERSION __stringify(MAJ) "." __stringify(MIN) "." \
 __stringify(BUILD) "-k"
@@ -169,13 +169,15 @@ static int igb_set_vf_mac(struct igb_adapter *, int, unsigned char *);
 static void igb_restore_vf_multicasts(struct igb_adapter *adapter);
 static int igb_ndo_set_vf_mac(struct net_device *netdev, int vf, u8 *mac);
 static int igb_ndo_set_vf_vlan(struct net_device *netdev,
-			       int vf, u16 vlan, u8 qos);
+			       int vf, u16 vlan, u8 qos, __be16 vlan_proto);
 static int igb_ndo_set_vf_bw(struct net_device *, int, int, int);
 static int igb_ndo_set_vf_spoofchk(struct net_device *netdev, int vf,
 				   bool setting);
 static int igb_ndo_get_vf_config(struct net_device *netdev, int vf,
 				 struct ifla_vf_info *ivi);
 static void igb_check_vf_rate_limit(struct igb_adapter *);
+static void igb_nfc_filter_exit(struct igb_adapter *adapter);
+static void igb_nfc_filter_restore(struct igb_adapter *adapter);
 
 #ifdef CONFIG_PCI_IOV
 static int igb_vf_configure(struct igb_adapter *adapter, int vf);
@@ -1611,6 +1613,7 @@ static void igb_configure(struct igb_adapter *adapter)
 	igb_setup_mrqc(adapter);
 	igb_setup_rctl(adapter);
 
+	igb_nfc_filter_restore(adapter);
 	igb_configure_tx(adapter);
 	igb_configure_rx(adapter);
 
@@ -2059,6 +2062,21 @@ static int igb_set_features(struct net_device *netdev,
 	if (!(changed & (NETIF_F_RXALL | NETIF_F_NTUPLE)))
 		return 0;
 
+	if (!(features & NETIF_F_NTUPLE)) {
+		struct hlist_node *node2;
+		struct igb_nfc_filter *rule;
+
+		spin_lock(&adapter->nfc_lock);
+		hlist_for_each_entry_safe(rule, node2,
+					  &adapter->nfc_filter_list, nfc_node) {
+			igb_erase_filter(adapter, rule);
+			hlist_del(&rule->nfc_node);
+			kfree(rule);
+		}
+		spin_unlock(&adapter->nfc_lock);
+		adapter->nfc_filter_count = 0;
+	}
+
 	netdev->features = features;
 
 	if (netif_running(netdev))
@@ -3053,6 +3071,7 @@ static int igb_sw_init(struct igb_adapter *adapter)
 				  VLAN_HLEN;
 	adapter->min_frame_size = ETH_ZLEN + ETH_FCS_LEN;
 
+	spin_lock_init(&adapter->nfc_lock);
 	spin_lock_init(&adapter->stats64_lock);
 #ifdef CONFIG_PCI_IOV
 	switch (hw->mac.type) {
@@ -3240,6 +3259,8 @@ static int __igb_close(struct net_device *netdev, bool suspending)
 	igb_down(adapter);
 	igb_free_irq(adapter);
 
+	igb_nfc_filter_exit(adapter);
+
 	igb_free_all_tx_resources(adapter);
 	igb_free_all_rx_resources(adapter);
 
@@ -6201,14 +6222,17 @@ static int igb_disable_port_vlan(struct igb_adapter *adapter, int vf)
 	return 0;
 }
 
-static int igb_ndo_set_vf_vlan(struct net_device *netdev,
-			       int vf, u16 vlan, u8 qos)
+static int igb_ndo_set_vf_vlan(struct net_device *netdev, int vf,
+			       u16 vlan, u8 qos, __be16 vlan_proto)
 {
 	struct igb_adapter *adapter = netdev_priv(netdev);
 
 	if ((vf >= adapter->vfs_allocated_count) || (vlan > 4095) || (qos > 7))
 		return -EINVAL;
 
+	if (vlan_proto != htons(ETH_P_8021Q))
+		return -EPROTONOSUPPORT;
+
 	return (vlan || qos) ? igb_enable_port_vlan(adapter, vf, vlan, qos) :
 			       igb_disable_port_vlan(adapter, vf);
 }
@@ -8306,4 +8330,28 @@ int igb_reinit_queues(struct igb_adapter *adapter)
 
 	return err;
 }
+
+static void igb_nfc_filter_exit(struct igb_adapter *adapter)
+{
+	struct igb_nfc_filter *rule;
+
+	spin_lock(&adapter->nfc_lock);
+
+	hlist_for_each_entry(rule, &adapter->nfc_filter_list, nfc_node)
+		igb_erase_filter(adapter, rule);
+
+	spin_unlock(&adapter->nfc_lock);
+}
+
+static void igb_nfc_filter_restore(struct igb_adapter *adapter)
+{
+	struct igb_nfc_filter *rule;
+
+	spin_lock(&adapter->nfc_lock);
+
+	hlist_for_each_entry(rule, &adapter->nfc_filter_list, nfc_node)
+		igb_add_filter(adapter, rule);
+
+	spin_unlock(&adapter->nfc_lock);
+}
 /* igb_main.c */
diff --git a/drivers/net/ethernet/intel/igb/igb_ptp.c b/drivers/net/ethernet/intel/igb/igb_ptp.c
index 336c103ae374..a7895c4cbcc3 100644
--- a/drivers/net/ethernet/intel/igb/igb_ptp.c
+++ b/drivers/net/ethernet/intel/igb/igb_ptp.c
@@ -591,6 +591,7 @@ static int igb_ptp_feature_enable_i210(struct ptp_clock_info *ptp,
 			tsim |= TSINTR_SYS_WRAP;
 		else
 			tsim &= ~TSINTR_SYS_WRAP;
+		igb->pps_sys_wrap_on = !!on;
 		wr32(E1000_TSIM, tsim);
 		spin_unlock_irqrestore(&igb->tmreg_lock, flags);
 		return 0;
@@ -998,12 +999,12 @@ static int igb_ptp_set_timestamp_mode(struct igb_adapter *adapter,
 
 	/* define ethertype filter for timestamped packets */
 	if (is_l2)
-		wr32(E1000_ETQF(3),
+		wr32(E1000_ETQF(IGB_ETQF_FILTER_1588),
 		     (E1000_ETQF_FILTER_ENABLE | /* enable filter */
 		      E1000_ETQF_1588 | /* enable timestamping */
 		      ETH_P_1588));     /* 1588 eth protocol type */
 	else
-		wr32(E1000_ETQF(3), 0);
+		wr32(E1000_ETQF(IGB_ETQF_FILTER_1588), 0);
 
 	/* L4 Queue Filter[3]: filter by destination port and protocol */
 	if (is_l4) {
@@ -1159,7 +1160,7 @@ void igb_ptp_init(struct igb_adapter *adapter)
 	if (IS_ERR(adapter->ptp_clock)) {
 		adapter->ptp_clock = NULL;
 		dev_err(&adapter->pdev->dev, "ptp_clock_register failed\n");
-	} else {
+	} else if (adapter->ptp_clock) {
 		dev_info(&adapter->pdev->dev, "added PHC on %s\n",
 			 adapter->netdev->name);
 		adapter->ptp_flags |= IGB_PTP_ENABLED;
@@ -1235,7 +1236,9 @@ void igb_ptp_reset(struct igb_adapter *adapter)
 	case e1000_i211:
 		wr32(E1000_TSAUXC, 0x0);
 		wr32(E1000_TSSDP, 0x0);
-		wr32(E1000_TSIM, TSYNC_INTERRUPTS);
+		wr32(E1000_TSIM,
+		     TSYNC_INTERRUPTS |
+		     (adapter->pps_sys_wrap_on ? TSINTR_SYS_WRAP : 0));
 		wr32(E1000_IMS, E1000_IMS_TS);
 		break;
 	default:
diff --git a/drivers/net/ethernet/intel/igbvf/netdev.c b/drivers/net/ethernet/intel/igbvf/netdev.c
index b0778ba65083..12bb877df860 100644
--- a/drivers/net/ethernet/intel/igbvf/netdev.c
+++ b/drivers/net/ethernet/intel/igbvf/netdev.c
@@ -47,7 +47,7 @@
 
 #include "igbvf.h"
 
-#define DRV_VERSION "2.0.2-k"
+#define DRV_VERSION "2.4.0-k"
 char igbvf_driver_name[] = "igbvf";
 const char igbvf_driver_version[] = DRV_VERSION;
 static const char igbvf_driver_string[] =
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe.h b/drivers/net/ethernet/intel/ixgbe/ixgbe.h
index 9475ff9055aa..b06e32d0d22a 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe.h
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe.h
@@ -45,10 +45,10 @@
 #include "ixgbe_type.h"
 #include "ixgbe_common.h"
 #include "ixgbe_dcb.h"
-#if defined(CONFIG_FCOE) || defined(CONFIG_FCOE_MODULE)
+#if IS_ENABLED(CONFIG_FCOE)
 #define IXGBE_FCOE
 #include "ixgbe_fcoe.h"
-#endif /* CONFIG_FCOE or CONFIG_FCOE_MODULE */
+#endif /* IS_ENABLED(CONFIG_FCOE) */
 #ifdef CONFIG_IXGBE_DCA
 #include <linux/dca.h>
 #endif
@@ -645,6 +645,7 @@ struct ixgbe_adapter {
 #define IXGBE_FLAG_RX_HWTSTAMP_ENABLED		BIT(25)
 #define IXGBE_FLAG_RX_HWTSTAMP_IN_REGISTER	BIT(26)
 #define IXGBE_FLAG_DCB_CAPABLE			BIT(27)
+#define IXGBE_FLAG_GENEVE_OFFLOAD_CAPABLE	BIT(28)
 
 	u32 flags2;
 #define IXGBE_FLAG2_RSC_CAPABLE			BIT(0)
@@ -653,13 +654,12 @@ struct ixgbe_adapter {
 #define IXGBE_FLAG2_TEMP_SENSOR_EVENT		BIT(3)
 #define IXGBE_FLAG2_SEARCH_FOR_SFP		BIT(4)
 #define IXGBE_FLAG2_SFP_NEEDS_RESET		BIT(5)
-#define IXGBE_FLAG2_RESET_REQUESTED		BIT(6)
 #define IXGBE_FLAG2_FDIR_REQUIRES_REINIT	BIT(7)
 #define IXGBE_FLAG2_RSS_FIELD_IPV4_UDP		BIT(8)
 #define IXGBE_FLAG2_RSS_FIELD_IPV6_UDP		BIT(9)
 #define IXGBE_FLAG2_PTP_PPS_ENABLED		BIT(10)
 #define IXGBE_FLAG2_PHY_INTERRUPT		BIT(11)
-#define IXGBE_FLAG2_VXLAN_REREG_NEEDED		BIT(12)
+#define IXGBE_FLAG2_UDP_TUN_REREG_NEEDED	BIT(12)
 #define IXGBE_FLAG2_VLAN_PROMISC		BIT(13)
 
 	/* Tx fast path data */
@@ -673,6 +673,7 @@ struct ixgbe_adapter {
 
 	/* Port number used to identify VXLAN traffic */
 	__be16 vxlan_port;
+	__be16 geneve_port;
 
 	/* TX */
 	struct ixgbe_ring *tx_ring[MAX_TX_QUEUES] ____cacheline_aligned_in_smp;
@@ -840,6 +841,7 @@ enum ixgbe_state_t {
 	__IXGBE_IN_SFP_INIT,
 	__IXGBE_PTP_RUNNING,
 	__IXGBE_PTP_TX_IN_PROGRESS,
+	__IXGBE_RESET_REQUESTED,
 };
 
 struct ixgbe_cb {
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_common.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_common.c
index c47b605e8651..77d3039283f6 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_common.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_common.c
@@ -99,6 +99,7 @@ bool ixgbe_device_supports_autoneg_fc(struct ixgbe_hw *hw)
 		case IXGBE_DEV_ID_X550T:
 		case IXGBE_DEV_ID_X550T1:
 		case IXGBE_DEV_ID_X550EM_X_10G_T:
+		case IXGBE_DEV_ID_X550EM_A_10G_T:
 			supported = true;
 			break;
 		default:
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_ethtool.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_ethtool.c
index 0d7209eb5abf..f49f80380aa5 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_ethtool.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_ethtool.c
@@ -193,7 +193,9 @@ static int ixgbe_get_settings(struct net_device *netdev,
 	if (supported_link & IXGBE_LINK_SPEED_10GB_FULL)
 		ecmd->supported |= ixgbe_get_supported_10gtypes(hw);
 	if (supported_link & IXGBE_LINK_SPEED_1GB_FULL)
-		ecmd->supported |= SUPPORTED_1000baseT_Full;
+		ecmd->supported |= (ixgbe_isbackplane(hw->phy.media_type)) ?
+				   SUPPORTED_1000baseKX_Full :
+				   SUPPORTED_1000baseT_Full;
 	if (supported_link & IXGBE_LINK_SPEED_100_FULL)
 		ecmd->supported |= ixgbe_isbackplane(hw->phy.media_type) ?
 				   SUPPORTED_1000baseKX_Full :
@@ -311,6 +313,25 @@ static int ixgbe_get_settings(struct net_device *netdev,
 		break;
 	}
 
+	/* Indicate pause support */
+	ecmd->supported |= SUPPORTED_Pause;
+
+	switch (hw->fc.requested_mode) {
+	case ixgbe_fc_full:
+		ecmd->advertising |= ADVERTISED_Pause;
+		break;
+	case ixgbe_fc_rx_pause:
+		ecmd->advertising |= ADVERTISED_Pause |
+				     ADVERTISED_Asym_Pause;
+		break;
+	case ixgbe_fc_tx_pause:
+		ecmd->advertising |= ADVERTISED_Asym_Pause;
+		break;
+	default:
+		ecmd->advertising &= ~(ADVERTISED_Pause |
+				       ADVERTISED_Asym_Pause);
+	}
+
 	if (netif_carrier_ok(netdev)) {
 		switch (adapter->link_speed) {
 		case IXGBE_LINK_SPEED_10GB_FULL:
@@ -2926,9 +2947,13 @@ static u32 ixgbe_rss_indir_size(struct net_device *netdev)
 static void ixgbe_get_reta(struct ixgbe_adapter *adapter, u32 *indir)
 {
 	int i, reta_size = ixgbe_rss_indir_tbl_entries(adapter);
+	u16 rss_m = adapter->ring_feature[RING_F_RSS].mask;
+
+	if (adapter->flags & IXGBE_FLAG_SRIOV_ENABLED)
+		rss_m = adapter->ring_feature[RING_F_RSS].indices - 1;
 
 	for (i = 0; i < reta_size; i++)
-		indir[i] = adapter->rss_indir_tbl[i];
+		indir[i] = adapter->rss_indir_tbl[i] & rss_m;
 }
 
 static int ixgbe_get_rxfh(struct net_device *netdev, u32 *indir, u8 *key,
@@ -3039,8 +3064,8 @@ static unsigned int ixgbe_max_channels(struct ixgbe_adapter *adapter)
 		/* We only support one q_vector without MSI-X */
 		max_combined = 1;
 	} else if (adapter->flags & IXGBE_FLAG_SRIOV_ENABLED) {
-		/* SR-IOV currently only allows one queue on the PF */
-		max_combined = 1;
+		/* Limit value based on the queue mask */
+		max_combined = adapter->ring_feature[RING_F_RSS].mask + 1;
 	} else if (tcs > 1) {
 		/* For DCB report channels per traffic class */
 		if (adapter->hw.mac.type == ixgbe_mac_82598EB) {
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_lib.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_lib.c
index bcdc88444ceb..15ab337fd7ad 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_lib.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_lib.c
@@ -515,15 +515,16 @@ static bool ixgbe_set_sriov_queues(struct ixgbe_adapter *adapter)
 	vmdq_i = min_t(u16, IXGBE_MAX_VMDQ_INDICES, vmdq_i);
 
 	/* 64 pool mode with 2 queues per pool */
-	if ((vmdq_i > 32) || (rss_i < 4) || (vmdq_i > 16 && pools)) {
+	if ((vmdq_i > 32) || (vmdq_i > 16 && pools)) {
 		vmdq_m = IXGBE_82599_VMDQ_2Q_MASK;
 		rss_m = IXGBE_RSS_2Q_MASK;
 		rss_i = min_t(u16, rss_i, 2);
-	/* 32 pool mode with 4 queues per pool */
+	/* 32 pool mode with up to 4 queues per pool */
 	} else {
 		vmdq_m = IXGBE_82599_VMDQ_4Q_MASK;
 		rss_m = IXGBE_RSS_4Q_MASK;
-		rss_i = 4;
+		/* We can support 4, 2, or 1 queues */
+		rss_i = (rss_i > 3) ? 4 : (rss_i > 1) ? 2 : 1;
 	}
 
 #ifdef IXGBE_FCOE
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_main.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_main.c
index b4f03748adc0..a244d9a67264 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_main.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_main.c
@@ -137,6 +137,7 @@ static const struct pci_device_id ixgbe_pci_tbl[] = {
 	{PCI_VDEVICE(INTEL, IXGBE_DEV_ID_X550EM_A_SFP_N), board_x550em_a },
 	{PCI_VDEVICE(INTEL, IXGBE_DEV_ID_X550EM_A_SGMII), board_x550em_a },
 	{PCI_VDEVICE(INTEL, IXGBE_DEV_ID_X550EM_A_SGMII_L), board_x550em_a },
+	{PCI_VDEVICE(INTEL, IXGBE_DEV_ID_X550EM_A_10G_T), board_x550em_a},
 	{PCI_VDEVICE(INTEL, IXGBE_DEV_ID_X550EM_A_SFP), board_x550em_a },
 	/* required last entry */
 	{0, }
@@ -1103,7 +1104,7 @@ static void ixgbe_tx_timeout_reset(struct ixgbe_adapter *adapter)
 
 	/* Do the reset outside of interrupt context */
 	if (!test_bit(__IXGBE_DOWN, &adapter->state)) {
-		adapter->flags2 |= IXGBE_FLAG2_RESET_REQUESTED;
+		set_bit(__IXGBE_RESET_REQUESTED, &adapter->state);
 		e_warn(drv, "initiating reset due to tx timeout\n");
 		ixgbe_service_event_schedule(adapter);
 	}
@@ -1495,7 +1496,6 @@ static inline void ixgbe_rx_checksum(struct ixgbe_ring *ring,
 				     struct sk_buff *skb)
 {
 	__le16 pkt_info = rx_desc->wb.lower.lo_dword.hs_rss.pkt_info;
-	__le16 hdr_info = rx_desc->wb.lower.lo_dword.hs_rss.hdr_info;
 	bool encap_pkt = false;
 
 	skb_checksum_none_assert(skb);
@@ -1504,8 +1504,8 @@ static inline void ixgbe_rx_checksum(struct ixgbe_ring *ring,
 	if (!(ring->netdev->features & NETIF_F_RXCSUM))
 		return;
 
-	if ((pkt_info & cpu_to_le16(IXGBE_RXDADV_PKTTYPE_VXLAN)) &&
-	    (hdr_info & cpu_to_le16(IXGBE_RXDADV_PKTTYPE_TUNNEL >> 16))) {
+	/* check for VXLAN and Geneve packets */
+	if (pkt_info & cpu_to_le16(IXGBE_RXDADV_PKTTYPE_VXLAN)) {
 		encap_pkt = true;
 		skb->encapsulation = 1;
 	}
@@ -2777,7 +2777,7 @@ static irqreturn_t ixgbe_msix_other(int irq, void *data)
 		}
 		if (eicr & IXGBE_EICR_ECC) {
 			e_info(link, "Received ECC Err, initiating reset\n");
-			adapter->flags2 |= IXGBE_FLAG2_RESET_REQUESTED;
+			set_bit(__IXGBE_RESET_REQUESTED, &adapter->state);
 			ixgbe_service_event_schedule(adapter);
 			IXGBE_WRITE_REG(hw, IXGBE_EICR, IXGBE_EICR_ECC);
 		}
@@ -3007,7 +3007,7 @@ static irqreturn_t ixgbe_intr(int irq, void *data)
 	case ixgbe_mac_x550em_a:
 		if (eicr & IXGBE_EICR_ECC) {
 			e_info(link, "Received ECC Err, initiating reset\n");
-			adapter->flags2 |= IXGBE_FLAG2_RESET_REQUESTED;
+			set_bit(__IXGBE_RESET_REQUESTED, &adapter->state);
 			ixgbe_service_event_schedule(adapter);
 			IXGBE_WRITE_REG(hw, IXGBE_EICR, IXGBE_EICR_ECC);
 		}
@@ -3224,7 +3224,7 @@ void ixgbe_configure_tx_ring(struct ixgbe_adapter *adapter,
 		txdctl = IXGBE_READ_REG(hw, IXGBE_TXDCTL(reg_idx));
 	} while (--wait_loop && !(txdctl & IXGBE_TXDCTL_ENABLE));
 	if (!wait_loop)
-		e_err(drv, "Could not enable Tx Queue %d\n", reg_idx);
+		hw_dbg(hw, "Could not enable Tx Queue %d\n", reg_idx);
 }
 
 static void ixgbe_setup_mtqc(struct ixgbe_adapter *adapter)
@@ -3248,7 +3248,8 @@ static void ixgbe_setup_mtqc(struct ixgbe_adapter *adapter)
 			mtqc |= IXGBE_MTQC_RT_ENA | IXGBE_MTQC_8TC_8TQ;
 		else if (tcs > 1)
 			mtqc |= IXGBE_MTQC_RT_ENA | IXGBE_MTQC_4TC_4TQ;
-		else if (adapter->ring_feature[RING_F_RSS].indices == 4)
+		else if (adapter->ring_feature[RING_F_VMDQ].mask ==
+			 IXGBE_82599_VMDQ_4Q_MASK)
 			mtqc |= IXGBE_MTQC_32VF;
 		else
 			mtqc |= IXGBE_MTQC_64VF;
@@ -3475,12 +3476,12 @@ static void ixgbe_setup_reta(struct ixgbe_adapter *adapter)
 	u32 reta_entries = ixgbe_rss_indir_tbl_entries(adapter);
 	u16 rss_i = adapter->ring_feature[RING_F_RSS].indices;
 
-	/* Program table for at least 2 queues w/ SR-IOV so that VFs can
+	/* Program table for at least 4 queues w/ SR-IOV so that VFs can
 	 * make full use of any rings they may have.  We will use the
 	 * PSRTYPE register to control how many rings we use within the PF.
 	 */
-	if ((adapter->flags & IXGBE_FLAG_SRIOV_ENABLED) && (rss_i < 2))
-		rss_i = 2;
+	if ((adapter->flags & IXGBE_FLAG_SRIOV_ENABLED) && (rss_i < 4))
+		rss_i = 4;
 
 	/* Fill out hash function seeds */
 	for (i = 0; i < 10; i++)
@@ -3544,7 +3545,8 @@ static void ixgbe_setup_mrqc(struct ixgbe_adapter *adapter)
 				mrqc = IXGBE_MRQC_VMDQRT8TCEN;	/* 8 TCs */
 			else if (tcs > 1)
 				mrqc = IXGBE_MRQC_VMDQRT4TCEN;	/* 4 TCs */
-			else if (adapter->ring_feature[RING_F_RSS].indices == 4)
+			else if (adapter->ring_feature[RING_F_VMDQ].mask ==
+				 IXGBE_82599_VMDQ_4Q_MASK)
 				mrqc = IXGBE_MRQC_VMDQRSS32EN;
 			else
 				mrqc = IXGBE_MRQC_VMDQRSS64EN;
@@ -3922,6 +3924,9 @@ static void ixgbe_configure_rx(struct ixgbe_adapter *adapter)
 	rfctl &= ~IXGBE_RFCTL_RSC_DIS;
 	if (!(adapter->flags2 & IXGBE_FLAG2_RSC_ENABLED))
 		rfctl |= IXGBE_RFCTL_RSC_DIS;
+
+	/* disable NFS filtering */
+	rfctl |= (IXGBE_RFCTL_NFSW_DIS | IXGBE_RFCTL_NFSR_DIS);
 	IXGBE_WRITE_REG(hw, IXGBE_RFCTL, rfctl);
 
 	/* Program registers for the distribution of queues */
@@ -4102,23 +4107,20 @@ static void ixgbe_vlan_promisc_enable(struct ixgbe_adapter *adapter)
 
 	vlnctrl = IXGBE_READ_REG(hw, IXGBE_VLNCTRL);
 
-	switch (hw->mac.type) {
-	case ixgbe_mac_82599EB:
-	case ixgbe_mac_X540:
-	case ixgbe_mac_X550:
-	case ixgbe_mac_X550EM_x:
-	case ixgbe_mac_x550em_a:
-	default:
-		if (adapter->flags & IXGBE_FLAG_VMDQ_ENABLED)
-			break;
-		/* fall through */
-	case ixgbe_mac_82598EB:
-		/* legacy case, we can just disable VLAN filtering */
+	if (adapter->flags & IXGBE_FLAG_VMDQ_ENABLED) {
+	/* For VMDq and SR-IOV we must leave VLAN filtering enabled */
+		vlnctrl |= IXGBE_VLNCTRL_VFE;
+		IXGBE_WRITE_REG(hw, IXGBE_VLNCTRL, vlnctrl);
+	} else {
 		vlnctrl &= ~IXGBE_VLNCTRL_VFE;
 		IXGBE_WRITE_REG(hw, IXGBE_VLNCTRL, vlnctrl);
 		return;
 	}
 
+	/* Nothing to do for 82598 */
+	if (hw->mac.type == ixgbe_mac_82598EB)
+		return;
+
 	/* We are already in VLAN promisc, nothing to do */
 	if (adapter->flags2 & IXGBE_FLAG2_VLAN_PROMISC)
 		return;
@@ -4126,10 +4128,6 @@ static void ixgbe_vlan_promisc_enable(struct ixgbe_adapter *adapter)
 	/* Set flag so we don't redo unnecessary work */
 	adapter->flags2 |= IXGBE_FLAG2_VLAN_PROMISC;
 
-	/* For VMDq and SR-IOV we must leave VLAN filtering enabled */
-	vlnctrl |= IXGBE_VLNCTRL_VFE;
-	IXGBE_WRITE_REG(hw, IXGBE_VLNCTRL, vlnctrl);
-
 	/* Add PF to all active pools */
 	for (i = IXGBE_VLVF_ENTRIES; --i;) {
 		u32 reg_offset = IXGBE_VLVFB(i * 2 + VMDQ_P(0) / 32);
@@ -4201,19 +4199,9 @@ static void ixgbe_vlan_promisc_disable(struct ixgbe_adapter *adapter)
 	vlnctrl |= IXGBE_VLNCTRL_VFE;
 	IXGBE_WRITE_REG(hw, IXGBE_VLNCTRL, vlnctrl);
 
-	switch (hw->mac.type) {
-	case ixgbe_mac_82599EB:
-	case ixgbe_mac_X540:
-	case ixgbe_mac_X550:
-	case ixgbe_mac_X550EM_x:
-	case ixgbe_mac_x550em_a:
-	default:
-		if (adapter->flags & IXGBE_FLAG_VMDQ_ENABLED)
-			break;
-		/* fall through */
-	case ixgbe_mac_82598EB:
+	if (!(adapter->flags & IXGBE_FLAG_VMDQ_ENABLED) ||
+	    hw->mac.type == ixgbe_mac_82598EB)
 		return;
-	}
 
 	/* We are not in VLAN promisc, nothing to do */
 	if (!(adapter->flags2 & IXGBE_FLAG2_VLAN_PROMISC))
@@ -4586,18 +4574,23 @@ static void ixgbe_napi_disable_all(struct ixgbe_adapter *adapter)
 	}
 }
 
-static void ixgbe_clear_vxlan_port(struct ixgbe_adapter *adapter)
+static void ixgbe_clear_udp_tunnel_port(struct ixgbe_adapter *adapter, u32 mask)
 {
-	switch (adapter->hw.mac.type) {
-	case ixgbe_mac_X550:
-	case ixgbe_mac_X550EM_x:
-	case ixgbe_mac_x550em_a:
-		IXGBE_WRITE_REG(&adapter->hw, IXGBE_VXLANCTRL, 0);
+	struct ixgbe_hw *hw = &adapter->hw;
+	u32 vxlanctrl;
+
+	if (!(adapter->flags & (IXGBE_FLAG_VXLAN_OFFLOAD_CAPABLE |
+				IXGBE_FLAG_GENEVE_OFFLOAD_CAPABLE)))
+		return;
+
+	vxlanctrl = IXGBE_READ_REG(hw, IXGBE_VXLANCTRL) && ~mask;
+	IXGBE_WRITE_REG(hw, IXGBE_VXLANCTRL, vxlanctrl);
+
+	if (mask & IXGBE_VXLANCTRL_VXLAN_UDPPORT_MASK)
 		adapter->vxlan_port = 0;
-		break;
-	default:
-		break;
-	}
+
+	if (mask & IXGBE_VXLANCTRL_GENEVE_UDPPORT_MASK)
+		adapter->geneve_port = 0;
 }
 
 #ifdef CONFIG_IXGBE_DCB
@@ -5500,8 +5493,8 @@ void ixgbe_down(struct ixgbe_adapter *adapter)
 
 	ixgbe_napi_disable_all(adapter);
 
-	adapter->flags2 &= ~(IXGBE_FLAG2_FDIR_REQUIRES_REINIT |
-			     IXGBE_FLAG2_RESET_REQUESTED);
+	clear_bit(__IXGBE_RESET_REQUESTED, &adapter->state);
+	adapter->flags2 &= ~IXGBE_FLAG2_FDIR_REQUIRES_REINIT;
 	adapter->flags &= ~IXGBE_FLAG_NEED_LINK_UPDATE;
 
 	del_timer_sync(&adapter->service_timer);
@@ -5711,8 +5704,10 @@ static int ixgbe_sw_init(struct ixgbe_adapter *adapter)
 		if (fwsm & IXGBE_FWSM_TS_ENABLED)
 			adapter->flags2 |= IXGBE_FLAG2_TEMP_SENSOR_CAPABLE;
 		break;
-	case ixgbe_mac_X550EM_x:
 	case ixgbe_mac_x550em_a:
+		adapter->flags |= IXGBE_FLAG_GENEVE_OFFLOAD_CAPABLE;
+	/* fall through */
+	case ixgbe_mac_X550EM_x:
 #ifdef CONFIG_IXGBE_DCB
 		adapter->flags &= ~IXGBE_FLAG_DCB_CAPABLE;
 #endif
@@ -6144,7 +6139,7 @@ int ixgbe_open(struct net_device *netdev)
 
 	ixgbe_up_complete(adapter);
 
-	ixgbe_clear_vxlan_port(adapter);
+	ixgbe_clear_udp_tunnel_port(adapter, IXGBE_VXLANCTRL_ALL_UDPPORT_MASK);
 	udp_tunnel_get_rx_info(netdev);
 
 	return 0;
@@ -6921,7 +6916,7 @@ static void ixgbe_watchdog_flush_tx(struct ixgbe_adapter *adapter)
 			 * (Do the reset outside of interrupt context).
 			 */
 			e_warn(drv, "initiating reset to clear Tx work after link loss\n");
-			adapter->flags2 |= IXGBE_FLAG2_RESET_REQUESTED;
+			set_bit(__IXGBE_RESET_REQUESTED, &adapter->state);
 		}
 	}
 }
@@ -7187,11 +7182,9 @@ static void ixgbe_phy_interrupt_subtask(struct ixgbe_adapter *adapter)
 
 static void ixgbe_reset_subtask(struct ixgbe_adapter *adapter)
 {
-	if (!(adapter->flags2 & IXGBE_FLAG2_RESET_REQUESTED))
+	if (!test_and_clear_bit(__IXGBE_RESET_REQUESTED, &adapter->state))
 		return;
 
-	adapter->flags2 &= ~IXGBE_FLAG2_RESET_REQUESTED;
-
 	/* If we're already down, removing or resetting, just bail */
 	if (test_bit(__IXGBE_DOWN, &adapter->state) ||
 	    test_bit(__IXGBE_REMOVING, &adapter->state) ||
@@ -7225,9 +7218,9 @@ static void ixgbe_service_task(struct work_struct *work)
 		ixgbe_service_event_complete(adapter);
 		return;
 	}
-	if (adapter->flags2 & IXGBE_FLAG2_VXLAN_REREG_NEEDED) {
+	if (adapter->flags2 & IXGBE_FLAG2_UDP_TUN_REREG_NEEDED) {
 		rtnl_lock();
-		adapter->flags2 &= ~IXGBE_FLAG2_VXLAN_REREG_NEEDED;
+		adapter->flags2 &= ~IXGBE_FLAG2_UDP_TUN_REREG_NEEDED;
 		udp_tunnel_get_rx_info(adapter->netdev);
 		rtnl_unlock();
 	}
@@ -7667,6 +7660,10 @@ static void ixgbe_atr(struct ixgbe_ring *ring,
 		if (adapter->vxlan_port &&
 		    udp_hdr(skb)->dest == adapter->vxlan_port)
 			hdr.network = skb_inner_network_header(skb);
+
+		if (adapter->geneve_port &&
+		    udp_hdr(skb)->dest == adapter->geneve_port)
+			hdr.network = skb_inner_network_header(skb);
 	}
 
 	/* Currently only IPv4/IPv6 with TCP is supported */
@@ -8802,10 +8799,23 @@ static int ixgbe_set_features(struct net_device *netdev,
 	netdev->features = features;
 
 	if ((adapter->flags & IXGBE_FLAG_VXLAN_OFFLOAD_CAPABLE)) {
-		if (features & NETIF_F_RXCSUM)
-			adapter->flags2 |= IXGBE_FLAG2_VXLAN_REREG_NEEDED;
-		else
-			ixgbe_clear_vxlan_port(adapter);
+		if (features & NETIF_F_RXCSUM) {
+			adapter->flags2 |= IXGBE_FLAG2_UDP_TUN_REREG_NEEDED;
+		} else {
+			u32 port_mask = IXGBE_VXLANCTRL_VXLAN_UDPPORT_MASK;
+
+			ixgbe_clear_udp_tunnel_port(adapter, port_mask);
+		}
+	}
+
+	if ((adapter->flags & IXGBE_FLAG_GENEVE_OFFLOAD_CAPABLE)) {
+		if (features & NETIF_F_RXCSUM) {
+			adapter->flags2 |= IXGBE_FLAG2_UDP_TUN_REREG_NEEDED;
+		} else {
+			u32 port_mask = IXGBE_VXLANCTRL_GENEVE_UDPPORT_MASK;
+
+			ixgbe_clear_udp_tunnel_port(adapter, port_mask);
+		}
 	}
 
 	if (need_reset)
@@ -8818,67 +8828,115 @@ static int ixgbe_set_features(struct net_device *netdev,
 }
 
 /**
- * ixgbe_add_vxlan_port - Get notifications about VXLAN ports that come up
+ * ixgbe_add_udp_tunnel_port - Get notifications about adding UDP tunnel ports
  * @dev: The port's netdev
  * @ti: Tunnel endpoint information
  **/
-static void ixgbe_add_vxlan_port(struct net_device *dev,
-				 struct udp_tunnel_info *ti)
+static void ixgbe_add_udp_tunnel_port(struct net_device *dev,
+				      struct udp_tunnel_info *ti)
 {
 	struct ixgbe_adapter *adapter = netdev_priv(dev);
 	struct ixgbe_hw *hw = &adapter->hw;
 	__be16 port = ti->port;
-
-	if (ti->type != UDP_TUNNEL_TYPE_VXLAN)
-		return;
+	u32 port_shift = 0;
+	u32 reg;
 
 	if (ti->sa_family != AF_INET)
 		return;
 
-	if (!(adapter->flags & IXGBE_FLAG_VXLAN_OFFLOAD_CAPABLE))
-		return;
+	switch (ti->type) {
+	case UDP_TUNNEL_TYPE_VXLAN:
+		if (!(adapter->flags & IXGBE_FLAG_VXLAN_OFFLOAD_CAPABLE))
+			return;
 
-	if (adapter->vxlan_port == port)
-		return;
+		if (adapter->vxlan_port == port)
+			return;
+
+		if (adapter->vxlan_port) {
+			netdev_info(dev,
+				    "VXLAN port %d set, not adding port %d\n",
+				    ntohs(adapter->vxlan_port),
+				    ntohs(port));
+			return;
+		}
 
-	if (adapter->vxlan_port) {
-		netdev_info(dev,
-			    "Hit Max num of VXLAN ports, not adding port %d\n",
-			    ntohs(port));
+		adapter->vxlan_port = port;
+		break;
+	case UDP_TUNNEL_TYPE_GENEVE:
+		if (!(adapter->flags & IXGBE_FLAG_GENEVE_OFFLOAD_CAPABLE))
+			return;
+
+		if (adapter->geneve_port == port)
+			return;
+
+		if (adapter->geneve_port) {
+			netdev_info(dev,
+				    "GENEVE port %d set, not adding port %d\n",
+				    ntohs(adapter->geneve_port),
+				    ntohs(port));
+			return;
+		}
+
+		port_shift = IXGBE_VXLANCTRL_GENEVE_UDPPORT_SHIFT;
+		adapter->geneve_port = port;
+		break;
+	default:
 		return;
 	}
 
-	adapter->vxlan_port = port;
-	IXGBE_WRITE_REG(hw, IXGBE_VXLANCTRL, ntohs(port));
+	reg = IXGBE_READ_REG(hw, IXGBE_VXLANCTRL) | ntohs(port) << port_shift;
+	IXGBE_WRITE_REG(hw, IXGBE_VXLANCTRL, reg);
 }
 
 /**
- * ixgbe_del_vxlan_port - Get notifications about VXLAN ports that go away
+ * ixgbe_del_udp_tunnel_port - Get notifications about removing UDP tunnel ports
  * @dev: The port's netdev
  * @ti: Tunnel endpoint information
  **/
-static void ixgbe_del_vxlan_port(struct net_device *dev,
-				 struct udp_tunnel_info *ti)
+static void ixgbe_del_udp_tunnel_port(struct net_device *dev,
+				      struct udp_tunnel_info *ti)
 {
 	struct ixgbe_adapter *adapter = netdev_priv(dev);
+	u32 port_mask;
 
-	if (ti->type != UDP_TUNNEL_TYPE_VXLAN)
+	if (ti->type != UDP_TUNNEL_TYPE_VXLAN &&
+	    ti->type != UDP_TUNNEL_TYPE_GENEVE)
 		return;
 
 	if (ti->sa_family != AF_INET)
 		return;
 
-	if (!(adapter->flags & IXGBE_FLAG_VXLAN_OFFLOAD_CAPABLE))
-		return;
+	switch (ti->type) {
+	case UDP_TUNNEL_TYPE_VXLAN:
+		if (!(adapter->flags & IXGBE_FLAG_VXLAN_OFFLOAD_CAPABLE))
+			return;
 
-	if (adapter->vxlan_port != ti->port) {
-		netdev_info(dev, "Port %d was not found, not deleting\n",
-			    ntohs(ti->port));
+		if (adapter->vxlan_port != ti->port) {
+			netdev_info(dev, "VXLAN port %d not found\n",
+				    ntohs(ti->port));
+			return;
+		}
+
+		port_mask = IXGBE_VXLANCTRL_VXLAN_UDPPORT_MASK;
+		break;
+	case UDP_TUNNEL_TYPE_GENEVE:
+		if (!(adapter->flags & IXGBE_FLAG_GENEVE_OFFLOAD_CAPABLE))
+			return;
+
+		if (adapter->geneve_port != ti->port) {
+			netdev_info(dev, "GENEVE port %d not found\n",
+				    ntohs(ti->port));
+			return;
+		}
+
+		port_mask = IXGBE_VXLANCTRL_GENEVE_UDPPORT_MASK;
+		break;
+	default:
 		return;
 	}
 
-	ixgbe_clear_vxlan_port(adapter);
-	adapter->flags2 |= IXGBE_FLAG2_VXLAN_REREG_NEEDED;
+	ixgbe_clear_udp_tunnel_port(adapter, port_mask);
+	adapter->flags2 |= IXGBE_FLAG2_UDP_TUN_REREG_NEEDED;
 }
 
 static int ixgbe_ndo_fdb_add(struct ndmsg *ndm, struct nlattr *tb[],
@@ -9192,8 +9250,8 @@ static const struct net_device_ops ixgbe_netdev_ops = {
 	.ndo_bridge_getlink	= ixgbe_ndo_bridge_getlink,
 	.ndo_dfwd_add_station	= ixgbe_fwd_add,
 	.ndo_dfwd_del_station	= ixgbe_fwd_del,
-	.ndo_udp_tunnel_add	= ixgbe_add_vxlan_port,
-	.ndo_udp_tunnel_del	= ixgbe_del_vxlan_port,
+	.ndo_udp_tunnel_add	= ixgbe_add_udp_tunnel_port,
+	.ndo_udp_tunnel_del	= ixgbe_del_udp_tunnel_port,
 	.ndo_features_check	= ixgbe_features_check,
 };
 
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_phy.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_phy.c
index db0731e05401..021ab9b89c71 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_phy.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_phy.c
@@ -346,8 +346,8 @@ s32 ixgbe_identify_phy_generic(struct ixgbe_hw *hw)
 				return 0;
 			}
 		}
-		/* clear value if nothing found */
-		hw->phy.mdio.prtad = 0;
+		/* indicate no PHY found */
+		hw->phy.mdio.prtad = MDIO_PRTAD_NONE;
 		return IXGBE_ERR_PHY_ADDR_INVALID;
 	}
 	return 0;
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_ptp.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_ptp.c
index e5431bfe3339..a92277683a64 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_ptp.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_ptp.c
@@ -1254,7 +1254,7 @@ static long ixgbe_ptp_create_clock(struct ixgbe_adapter *adapter)
 		adapter->ptp_clock = NULL;
 		e_dev_err("ptp_clock_register failed\n");
 		return err;
-	} else
+	} else if (adapter->ptp_clock)
 		e_dev_info("registered PHC device on %s\n", netdev->name);
 
 	/* set default timestamp mode to disabled here. We do this in
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_sriov.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_sriov.c
index 8618599dfd6f..7e5d9850e4b2 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_sriov.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_sriov.c
@@ -329,13 +329,15 @@ static int ixgbe_pci_sriov_enable(struct pci_dev *dev, int num_vfs)
 	for (i = 0; i < adapter->num_vfs; i++)
 		ixgbe_vf_configuration(dev, (i | 0x10000000));
 
+	/* reset before enabling SRIOV to avoid mailbox issues */
+	ixgbe_sriov_reinit(adapter);
+
 	err = pci_enable_sriov(dev, num_vfs);
 	if (err) {
 		e_dev_warn("Failed to enable PCI sriov: %d\n", err);
 		return err;
 	}
 	ixgbe_get_vfs(adapter);
-	ixgbe_sriov_reinit(adapter);
 
 	return num_vfs;
 #else
@@ -1354,13 +1356,16 @@ static int ixgbe_disable_port_vlan(struct ixgbe_adapter *adapter, int vf)
 	return err;
 }
 
-int ixgbe_ndo_set_vf_vlan(struct net_device *netdev, int vf, u16 vlan, u8 qos)
+int ixgbe_ndo_set_vf_vlan(struct net_device *netdev, int vf, u16 vlan,
+			  u8 qos, __be16 vlan_proto)
 {
 	int err = 0;
 	struct ixgbe_adapter *adapter = netdev_priv(netdev);
 
 	if ((vf >= adapter->num_vfs) || (vlan > 4095) || (qos > 7))
 		return -EINVAL;
+	if (vlan_proto != htons(ETH_P_8021Q))
+		return -EPROTONOSUPPORT;
 	if (vlan || qos) {
 		/* Check if there is already a port VLAN set, if so
 		 * we have to delete the old one first before we
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_sriov.h b/drivers/net/ethernet/intel/ixgbe/ixgbe_sriov.h
index 47e65e2f886a..0c7977d27b71 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_sriov.h
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_sriov.h
@@ -43,7 +43,7 @@ void ixgbe_disable_tx_rx(struct ixgbe_adapter *adapter);
 void ixgbe_ping_all_vfs(struct ixgbe_adapter *adapter);
 int ixgbe_ndo_set_vf_mac(struct net_device *netdev, int queue, u8 *mac);
 int ixgbe_ndo_set_vf_vlan(struct net_device *netdev, int queue, u16 vlan,
-			   u8 qos);
+			   u8 qos, __be16 vlan_proto);
 int ixgbe_link_mbps(struct ixgbe_adapter *adapter);
 int ixgbe_ndo_set_vf_bw(struct net_device *netdev, int vf, int min_tx_rate,
 			int max_tx_rate);
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_type.h b/drivers/net/ethernet/intel/ixgbe/ixgbe_type.h
index 1248a9936f7a..31d82e3abac8 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_type.h
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_type.h
@@ -90,6 +90,7 @@
 #define IXGBE_DEV_ID_X550EM_A_SFP_N	0x15C4
 #define IXGBE_DEV_ID_X550EM_A_SGMII	0x15C6
 #define IXGBE_DEV_ID_X550EM_A_SGMII_L	0x15C7
+#define IXGBE_DEV_ID_X550EM_A_10G_T	0x15C8
 #define IXGBE_DEV_ID_X550EM_A_SFP	0x15CE
 
 /* VF Device IDs */
@@ -487,6 +488,13 @@ struct ixgbe_thermal_sensor_data {
 #define IXGBE_FHFT_EXT(_n)	(0x09800 + ((_n) * 0x100)) /* Ext Flexible Host
 							    * Filter Table */
 
+/* masks for accessing VXLAN and GENEVE UDP ports */
+#define IXGBE_VXLANCTRL_VXLAN_UDPPORT_MASK     0x0000ffff /* VXLAN port */
+#define IXGBE_VXLANCTRL_GENEVE_UDPPORT_MASK    0xffff0000 /* GENEVE port */
+#define IXGBE_VXLANCTRL_ALL_UDPPORT_MASK       0xffffffff /* GENEVE/VXLAN */
+
+#define IXGBE_VXLANCTRL_GENEVE_UDPPORT_SHIFT   16
+
 #define IXGBE_FLEXIBLE_FILTER_COUNT_MAX         4
 #define IXGBE_EXT_FLEXIBLE_FILTER_COUNT_MAX     2
 
@@ -1823,6 +1831,9 @@ enum {
 #define IXGBE_LED_IVRT(_i)       IXGBE_LED_OFFSET(IXGBE_LED_IVRT_BASE, _i)
 #define IXGBE_LED_BLINK(_i)      IXGBE_LED_OFFSET(IXGBE_LED_BLINK_BASE, _i)
 #define IXGBE_LED_MODE_MASK(_i)  IXGBE_LED_OFFSET(IXGBE_LED_MODE_MASK_BASE, _i)
+#define IXGBE_X557_LED_MANUAL_SET_MASK	BIT(8)
+#define IXGBE_X557_MAX_LED_INDEX	3
+#define IXGBE_X557_LED_PROVISIONING	0xC430
 
 /* LED modes */
 #define IXGBE_LED_LINK_UP       0x0
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_x550.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_x550.c
index 4716ca499e67..7e6b9267ca9d 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_x550.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_x550.c
@@ -295,6 +295,12 @@ static s32 ixgbe_identify_phy_x550em(struct ixgbe_hw *hw)
 	case IXGBE_DEV_ID_X550EM_A_KR_L:
 		hw->phy.type = ixgbe_phy_x550em_kr;
 		break;
+	case IXGBE_DEV_ID_X550EM_A_10G_T:
+		if (hw->bus.lan_id)
+			hw->phy.phy_semaphore_mask = IXGBE_GSSR_PHY1_SM;
+		else
+			hw->phy.phy_semaphore_mask = IXGBE_GSSR_PHY0_SM;
+		/* Fallthrough */
 	case IXGBE_DEV_ID_X550EM_X_1G_T:
 	case IXGBE_DEV_ID_X550EM_X_10G_T:
 		return ixgbe_identify_phy_generic(hw);
@@ -1453,7 +1459,7 @@ ixgbe_setup_mac_link_sfp_x550a(struct ixgbe_hw *hw, ixgbe_link_speed speed,
 	/* Configure internal PHY for KR/KX. */
 	ixgbe_setup_kr_speed_x550em(hw, speed);
 
-	if (!hw->phy.mdio.prtad || hw->phy.mdio.prtad == 0xFFFF)
+	if (hw->phy.mdio.prtad == MDIO_PRTAD_NONE)
 		return IXGBE_ERR_PHY_ADDR_INVALID;
 
 	/* Get external PHY device id */
@@ -2114,6 +2120,50 @@ static s32 ixgbe_reset_phy_t_X550em(struct ixgbe_hw *hw)
 	return ixgbe_enable_lasi_ext_t_x550em(hw);
 }
 
+/**
+ *  ixgbe_led_on_t_x550em - Turns on the software controllable LEDs.
+ *  @hw: pointer to hardware structure
+ *  @led_idx: led number to turn on
+ **/
+static s32 ixgbe_led_on_t_x550em(struct ixgbe_hw *hw, u32 led_idx)
+{
+	u16 phy_data;
+
+	if (led_idx >= IXGBE_X557_MAX_LED_INDEX)
+		return IXGBE_ERR_PARAM;
+
+	/* To turn on the LED, set mode to ON. */
+	hw->phy.ops.read_reg(hw, IXGBE_X557_LED_PROVISIONING + led_idx,
+			     IXGBE_MDIO_VENDOR_SPECIFIC_1_DEV_TYPE, &phy_data);
+	phy_data |= IXGBE_X557_LED_MANUAL_SET_MASK;
+	hw->phy.ops.write_reg(hw, IXGBE_X557_LED_PROVISIONING + led_idx,
+			      IXGBE_MDIO_VENDOR_SPECIFIC_1_DEV_TYPE, phy_data);
+
+	return 0;
+}
+
+/**
+ *  ixgbe_led_off_t_x550em - Turns off the software controllable LEDs.
+ *  @hw: pointer to hardware structure
+ *  @led_idx: led number to turn off
+ **/
+static s32 ixgbe_led_off_t_x550em(struct ixgbe_hw *hw, u32 led_idx)
+{
+	u16 phy_data;
+
+	if (led_idx >= IXGBE_X557_MAX_LED_INDEX)
+		return IXGBE_ERR_PARAM;
+
+	/* To turn on the LED, set mode to ON. */
+	hw->phy.ops.read_reg(hw, IXGBE_X557_LED_PROVISIONING + led_idx,
+			     IXGBE_MDIO_VENDOR_SPECIFIC_1_DEV_TYPE, &phy_data);
+	phy_data &= ~IXGBE_X557_LED_MANUAL_SET_MASK;
+	hw->phy.ops.write_reg(hw, IXGBE_X557_LED_PROVISIONING + led_idx,
+			      IXGBE_MDIO_VENDOR_SPECIFIC_1_DEV_TYPE, phy_data);
+
+	return 0;
+}
+
 /** ixgbe_get_lcd_x550em - Determine lowest common denominator
  *  @hw: pointer to hardware structure
  *  @lcd_speed: pointer to lowest common link speed
@@ -2344,18 +2394,12 @@ static void ixgbe_read_mng_if_sel_x550em(struct ixgbe_hw *hw)
 	/* If X552 (X550EM_a) and MDIO is connected to external PHY, then set
 	 * PHY address. This register field was has only been used for X552.
 	 */
-	if (!hw->phy.nw_mng_if_sel) {
-		if (hw->mac.type == ixgbe_mac_x550em_a) {
-			struct ixgbe_adapter *adapter = hw->back;
-
-			e_warn(drv, "nw_mng_if_sel not set\n");
-		}
-		return;
+	if (hw->mac.type == ixgbe_mac_x550em_a &&
+	    hw->phy.nw_mng_if_sel & IXGBE_NW_MNG_IF_SEL_MDIO_ACT) {
+		hw->phy.mdio.prtad = (hw->phy.nw_mng_if_sel &
+				      IXGBE_NW_MNG_IF_SEL_MDIO_PHY_ADD) >>
+				     IXGBE_NW_MNG_IF_SEL_MDIO_PHY_ADD_SHIFT;
 	}
-
-	hw->phy.mdio.prtad = (hw->phy.nw_mng_if_sel &
-			      IXGBE_NW_MNG_IF_SEL_MDIO_PHY_ADD) >>
-			     IXGBE_NW_MNG_IF_SEL_MDIO_PHY_ADD_SHIFT;
 }
 
 /** ixgbe_init_phy_ops_X550em - PHY/SFP specific init
@@ -2456,6 +2500,7 @@ static enum ixgbe_media_type ixgbe_get_media_type_X550em(struct ixgbe_hw *hw)
 		break;
 	case IXGBE_DEV_ID_X550EM_X_1G_T:
 	case IXGBE_DEV_ID_X550EM_X_10G_T:
+	case IXGBE_DEV_ID_X550EM_A_10G_T:
 		media_type = ixgbe_media_type_copper;
 		break;
 	default:
@@ -2514,6 +2559,9 @@ static void ixgbe_set_mdio_speed(struct ixgbe_hw *hw)
 
 	switch (hw->device_id) {
 	case IXGBE_DEV_ID_X550EM_X_10G_T:
+	case IXGBE_DEV_ID_X550EM_A_SGMII:
+	case IXGBE_DEV_ID_X550EM_A_SGMII_L:
+	case IXGBE_DEV_ID_X550EM_A_10G_T:
 	case IXGBE_DEV_ID_X550EM_A_SFP:
 		/* Config MDIO clock speed before the first MDIO PHY access */
 		hlreg0 = IXGBE_READ_REG(hw, IXGBE_HLREG0);
@@ -2853,8 +2901,6 @@ static s32 ixgbe_write_phy_reg_x550a(struct ixgbe_hw *hw, u32 reg_addr,
 	.write_analog_reg8		= NULL, \
 	.set_rxpba			= &ixgbe_set_rxpba_generic, \
 	.check_link			= &ixgbe_check_mac_link_generic, \
-	.led_on				= &ixgbe_led_on_generic, \
-	.led_off			= &ixgbe_led_off_generic, \
 	.blink_led_start		= &ixgbe_blink_led_start_X540, \
 	.blink_led_stop			= &ixgbe_blink_led_stop_X540, \
 	.set_rar			= &ixgbe_set_rar_generic, \
@@ -2886,6 +2932,8 @@ static s32 ixgbe_write_phy_reg_x550a(struct ixgbe_hw *hw, u32 reg_addr,
 
 static const struct ixgbe_mac_operations mac_ops_X550 = {
 	X550_COMMON_MAC
+	.led_on			= ixgbe_led_on_generic,
+	.led_off		= ixgbe_led_off_generic,
 	.reset_hw		= &ixgbe_reset_hw_X540,
 	.get_media_type		= &ixgbe_get_media_type_X540,
 	.get_san_mac_addr	= &ixgbe_get_san_mac_addr_generic,
@@ -2904,6 +2952,8 @@ static const struct ixgbe_mac_operations mac_ops_X550 = {
 
 static const struct ixgbe_mac_operations mac_ops_X550EM_x = {
 	X550_COMMON_MAC
+	.led_on			= ixgbe_led_on_t_x550em,
+	.led_off		= ixgbe_led_off_t_x550em,
 	.reset_hw		= &ixgbe_reset_hw_X550em,
 	.get_media_type		= &ixgbe_get_media_type_X550em,
 	.get_san_mac_addr	= NULL,
@@ -2922,6 +2972,8 @@ static const struct ixgbe_mac_operations mac_ops_X550EM_x = {
 
 static struct ixgbe_mac_operations mac_ops_x550em_a = {
 	X550_COMMON_MAC
+	.led_on			= ixgbe_led_on_t_x550em,
+	.led_off		= ixgbe_led_off_t_x550em,
 	.reset_hw		= ixgbe_reset_hw_X550em,
 	.get_media_type		= ixgbe_get_media_type_X550em,
 	.get_san_mac_addr	= NULL,
@@ -2997,6 +3049,8 @@ static const struct ixgbe_phy_operations phy_ops_x550em_a = {
 	.identify		= &ixgbe_identify_phy_x550em,
 	.read_reg		= &ixgbe_read_phy_reg_x550a,
 	.write_reg		= &ixgbe_write_phy_reg_x550a,
+	.read_reg_mdi		= &ixgbe_read_phy_reg_mdi,
+	.write_reg_mdi		= &ixgbe_write_phy_reg_mdi,
 };
 
 static const u32 ixgbe_mvals_X550[IXGBE_MVALS_IDX_LIMIT] = {
diff --git a/drivers/net/ethernet/intel/ixgbevf/ixgbevf.h b/drivers/net/ethernet/intel/ixgbevf/ixgbevf.h
index be52f597688b..5639fbe294d0 100644
--- a/drivers/net/ethernet/intel/ixgbevf/ixgbevf.h
+++ b/drivers/net/ethernet/intel/ixgbevf/ixgbevf.h
@@ -502,12 +502,9 @@ extern void ixgbevf_write_eitr(struct ixgbevf_q_vector *q_vector);
 void ixgbe_napi_add_all(struct ixgbevf_adapter *adapter);
 void ixgbe_napi_del_all(struct ixgbevf_adapter *adapter);
 
-#ifdef DEBUG
-char *ixgbevf_get_hw_dev_name(struct ixgbe_hw *hw);
-#define hw_dbg(hw, format, arg...) \
-	printk(KERN_DEBUG "%s: " format, ixgbevf_get_hw_dev_name(hw), ##arg)
-#else
-#define hw_dbg(hw, format, arg...) do {} while (0)
-#endif
+#define ixgbevf_hw_to_netdev(hw) \
+	(((struct ixgbevf_adapter *)(hw)->back)->netdev)
 
+#define hw_dbg(hw, format, arg...) \
+	netdev_dbg(ixgbevf_hw_to_netdev(hw), format, ## arg)
 #endif /* _IXGBEVF_H_ */
diff --git a/drivers/net/ethernet/intel/ixgbevf/ixgbevf_main.c b/drivers/net/ethernet/intel/ixgbevf/ixgbevf_main.c
index d9d6616f02a4..7eaac3234049 100644
--- a/drivers/net/ethernet/intel/ixgbevf/ixgbevf_main.c
+++ b/drivers/net/ethernet/intel/ixgbevf/ixgbevf_main.c
@@ -1612,7 +1612,7 @@ static void ixgbevf_configure_tx_ring(struct ixgbevf_adapter *adapter,
 		txdctl = IXGBE_READ_REG(hw, IXGBE_VFTXDCTL(reg_idx));
 	}  while (--wait_loop && !(txdctl & IXGBE_TXDCTL_ENABLE));
 	if (!wait_loop)
-		pr_err("Could not enable Tx Queue %d\n", reg_idx);
+		hw_dbg(hw, "Could not enable Tx Queue %d\n", reg_idx);
 }
 
 /**
@@ -1810,8 +1810,10 @@ static void ixgbevf_configure_rx(struct ixgbevf_adapter *adapter)
 	if (hw->mac.type >= ixgbe_mac_X550_vf)
 		ixgbevf_setup_vfmrqc(adapter);
 
+	spin_lock_bh(&adapter->mbx_lock);
 	/* notify the PF of our intent to use this size of frame */
 	ret = hw->mac.ops.set_rlpml(hw, netdev->mtu + ETH_HLEN + ETH_FCS_LEN);
+	spin_unlock_bh(&adapter->mbx_lock);
 	if (ret)
 		dev_err(&adapter->pdev->dev,
 			"Failed to set MTU at %d\n", netdev->mtu);
@@ -2993,6 +2995,7 @@ static void ixgbevf_free_all_tx_resources(struct ixgbevf_adapter *adapter)
  **/
 int ixgbevf_setup_tx_resources(struct ixgbevf_ring *tx_ring)
 {
+	struct ixgbevf_adapter *adapter = netdev_priv(tx_ring->netdev);
 	int size;
 
 	size = sizeof(struct ixgbevf_tx_buffer) * tx_ring->count;
@@ -3757,8 +3760,10 @@ static int ixgbevf_change_mtu(struct net_device *netdev, int new_mtu)
 	if ((new_mtu < 68) || (max_frame > max_possible_frame))
 		return -EINVAL;
 
+	spin_lock_bh(&adapter->mbx_lock);
 	/* notify the PF of our intent to use this size of frame */
 	ret = hw->mac.ops.set_rlpml(hw, max_frame);
+	spin_unlock_bh(&adapter->mbx_lock);
 	if (ret)
 		return -EINVAL;
 
diff --git a/drivers/net/ethernet/intel/ixgbevf/vf.c b/drivers/net/ethernet/intel/ixgbevf/vf.c
index a52f70ec42b6..d46ba1dabcb7 100644
--- a/drivers/net/ethernet/intel/ixgbevf/vf.c
+++ b/drivers/net/ethernet/intel/ixgbevf/vf.c
@@ -284,7 +284,8 @@ static s32 ixgbevf_set_uc_addr_vf(struct ixgbe_hw *hw, u32 index, u8 *addr)
 	if (addr)
 		ether_addr_copy(msg_addr, addr);
 
-	ret_val = ixgbevf_write_msg_read_ack(hw, msgbuf, msgbuf, 3);
+	ret_val = ixgbevf_write_msg_read_ack(hw, msgbuf, msgbuf,
+					     sizeof(msgbuf) / sizeof(u32));
 	if (!ret_val) {
 		msgbuf[0] &= ~IXGBE_VT_MSGTYPE_CTS;
 
@@ -441,7 +442,8 @@ static s32 ixgbevf_set_rar_vf(struct ixgbe_hw *hw, u32 index, u8 *addr,
 	msgbuf[0] = IXGBE_VF_SET_MAC_ADDR;
 	ether_addr_copy(msg_addr, addr);
 
-	ret_val = ixgbevf_write_msg_read_ack(hw, msgbuf, msgbuf, 2);
+	ret_val = ixgbevf_write_msg_read_ack(hw, msgbuf, msgbuf,
+					     sizeof(msgbuf) / sizeof(u32));
 
 	msgbuf[0] &= ~IXGBE_VT_MSGTYPE_CTS;
 
@@ -551,7 +553,8 @@ static s32 ixgbevf_update_xcast_mode(struct ixgbe_hw *hw, int xcast_mode)
 	msgbuf[0] = IXGBE_VF_UPDATE_XCAST_MODE;
 	msgbuf[1] = xcast_mode;
 
-	err = ixgbevf_write_msg_read_ack(hw, msgbuf, msgbuf, 2);
+	err = ixgbevf_write_msg_read_ack(hw, msgbuf, msgbuf,
+					 sizeof(msgbuf) / sizeof(u32));
 	if (err)
 		return err;
 
@@ -588,7 +591,8 @@ static s32 ixgbevf_set_vfta_vf(struct ixgbe_hw *hw, u32 vlan, u32 vind,
 	/* Setting the 8 bit field MSG INFO to TRUE indicates "add" */
 	msgbuf[0] |= vlan_on << IXGBE_VT_MSGINFO_SHIFT;
 
-	err = ixgbevf_write_msg_read_ack(hw, msgbuf, msgbuf, 2);
+	err = ixgbevf_write_msg_read_ack(hw, msgbuf, msgbuf,
+					 sizeof(msgbuf) / sizeof(u32));
 	if (err)
 		goto mbx_err;
 
@@ -791,7 +795,8 @@ static s32 ixgbevf_set_rlpml_vf(struct ixgbe_hw *hw, u16 max_size)
 	msgbuf[0] = IXGBE_VF_SET_LPE;
 	msgbuf[1] = max_size;
 
-	ret_val = ixgbevf_write_msg_read_ack(hw, msgbuf, msgbuf, 2);
+	ret_val = ixgbevf_write_msg_read_ack(hw, msgbuf, msgbuf,
+					     sizeof(msgbuf) / sizeof(u32));
 	if (ret_val)
 		return ret_val;
 	if ((msgbuf[0] & IXGBE_VF_SET_LPE) &&
@@ -837,7 +842,8 @@ static int ixgbevf_negotiate_api_version_vf(struct ixgbe_hw *hw, int api)
 	msg[1] = api;
 	msg[2] = 0;
 
-	err = ixgbevf_write_msg_read_ack(hw, msg, msg, 3);
+	err = ixgbevf_write_msg_read_ack(hw, msg, msg,
+					 sizeof(msg) / sizeof(u32));
 	if (!err) {
 		msg[0] &= ~IXGBE_VT_MSGTYPE_CTS;
 
@@ -887,7 +893,8 @@ int ixgbevf_get_queues(struct ixgbe_hw *hw, unsigned int *num_tcs,
 	msg[0] = IXGBE_VF_GET_QUEUE;
 	msg[1] = msg[2] = msg[3] = msg[4] = 0;
 
-	err = ixgbevf_write_msg_read_ack(hw, msg, msg, 5);
+	err = ixgbevf_write_msg_read_ack(hw, msg, msg,
+					 sizeof(msg) / sizeof(u32));
 	if (!err) {
 		msg[0] &= ~IXGBE_VT_MSGTYPE_CTS;
 
diff --git a/drivers/net/ethernet/marvell/mvmdio.c b/drivers/net/ethernet/marvell/mvmdio.c
index 8982c882af1b..a0d1b084ecec 100644
--- a/drivers/net/ethernet/marvell/mvmdio.c
+++ b/drivers/net/ethernet/marvell/mvmdio.c
@@ -211,8 +211,7 @@ static int orion_mdio_probe(struct platform_device *pdev)
 	dev->regs = devm_ioremap(&pdev->dev, r->start, resource_size(r));
 	if (!dev->regs) {
 		dev_err(&pdev->dev, "Unable to remap SMI register\n");
-		ret = -ENODEV;
-		goto out_mdio;
+		return -ENODEV;
 	}
 
 	init_waitqueue_head(&dev->smi_busy_wait);
diff --git a/drivers/net/ethernet/marvell/mvneta.c b/drivers/net/ethernet/marvell/mvneta.c
index d41c28d00b57..5cb07c2017bf 100644
--- a/drivers/net/ethernet/marvell/mvneta.c
+++ b/drivers/net/ethernet/marvell/mvneta.c
@@ -382,7 +382,8 @@ struct mvneta_port {
 	struct mvneta_rx_queue *rxqs;
 	struct mvneta_tx_queue *txqs;
 	struct net_device *dev;
-	struct notifier_block cpu_notifier;
+	struct hlist_node node_online;
+	struct hlist_node node_dead;
 	int rxq_def;
 	/* Protect the access to the percpu interrupt registers,
 	 * ensuring that the configuration remains coherent.
@@ -399,7 +400,6 @@ struct mvneta_port {
 	u16 rx_ring_size;
 
 	struct mii_bus *mii_bus;
-	struct phy_device *phy_dev;
 	phy_interface_t phy_interface;
 	struct device_node *phy_node;
 	unsigned int link;
@@ -574,6 +574,7 @@ struct mvneta_rx_queue {
 	int next_desc_to_proc;
 };
 
+static enum cpuhp_state online_hpstate;
 /* The hardware supports eight (8) rx queues, but we are only allowing
  * the first one to be used. Therefore, let's just allocate one queue.
  */
@@ -635,8 +636,9 @@ static void mvneta_mib_counters_clear(struct mvneta_port *pp)
 }
 
 /* Get System Network Statistics */
-struct rtnl_link_stats64 *mvneta_get_stats64(struct net_device *dev,
-					     struct rtnl_link_stats64 *stats)
+static struct rtnl_link_stats64 *
+mvneta_get_stats64(struct net_device *dev,
+		   struct rtnl_link_stats64 *stats)
 {
 	struct mvneta_port *pp = netdev_priv(dev);
 	unsigned int start;
@@ -2651,6 +2653,7 @@ static int mvneta_poll(struct napi_struct *napi, int budget)
 	u32 cause_rx_tx;
 	int rx_queue;
 	struct mvneta_port *pp = netdev_priv(napi->dev);
+	struct net_device *ndev = pp->dev;
 	struct mvneta_pcpu_port *port = this_cpu_ptr(pp->ports);
 
 	if (!netif_running(pp->dev)) {
@@ -2668,7 +2671,7 @@ static int mvneta_poll(struct napi_struct *napi, int budget)
 				(MVNETA_CAUSE_PHY_STATUS_CHANGE |
 				 MVNETA_CAUSE_LINK_CHANGE |
 				 MVNETA_CAUSE_PSC_SYNC_CHANGE))) {
-			mvneta_fixed_link_update(pp, pp->phy_dev);
+			mvneta_fixed_link_update(pp, ndev->phydev);
 		}
 	}
 
@@ -2963,6 +2966,7 @@ static int mvneta_setup_txqs(struct mvneta_port *pp)
 static void mvneta_start_dev(struct mvneta_port *pp)
 {
 	int cpu;
+	struct net_device *ndev = pp->dev;
 
 	mvneta_max_rx_size_set(pp, pp->pkt_size);
 	mvneta_txq_max_tx_size_set(pp, pp->pkt_size);
@@ -2985,15 +2989,16 @@ static void mvneta_start_dev(struct mvneta_port *pp)
 		    MVNETA_CAUSE_LINK_CHANGE |
 		    MVNETA_CAUSE_PSC_SYNC_CHANGE);
 
-	phy_start(pp->phy_dev);
+	phy_start(ndev->phydev);
 	netif_tx_start_all_queues(pp->dev);
 }
 
 static void mvneta_stop_dev(struct mvneta_port *pp)
 {
 	unsigned int cpu;
+	struct net_device *ndev = pp->dev;
 
-	phy_stop(pp->phy_dev);
+	phy_stop(ndev->phydev);
 
 	for_each_online_cpu(cpu) {
 		struct mvneta_pcpu_port *port = per_cpu_ptr(pp->ports, cpu);
@@ -3166,7 +3171,7 @@ static int mvneta_set_mac_addr(struct net_device *dev, void *addr)
 static void mvneta_adjust_link(struct net_device *ndev)
 {
 	struct mvneta_port *pp = netdev_priv(ndev);
-	struct phy_device *phydev = pp->phy_dev;
+	struct phy_device *phydev = ndev->phydev;
 	int status_change = 0;
 
 	if (phydev->link) {
@@ -3244,7 +3249,6 @@ static int mvneta_mdio_probe(struct mvneta_port *pp)
 	phy_dev->supported &= PHY_GBIT_FEATURES;
 	phy_dev->advertising = phy_dev->supported;
 
-	pp->phy_dev = phy_dev;
 	pp->link    = 0;
 	pp->duplex  = 0;
 	pp->speed   = 0;
@@ -3254,8 +3258,9 @@ static int mvneta_mdio_probe(struct mvneta_port *pp)
 
 static void mvneta_mdio_remove(struct mvneta_port *pp)
 {
-	phy_disconnect(pp->phy_dev);
-	pp->phy_dev = NULL;
+	struct net_device *ndev = pp->dev;
+
+	phy_disconnect(ndev->phydev);
 }
 
 /* Electing a CPU must be done in an atomic way: it should be done
@@ -3311,101 +3316,104 @@ static void mvneta_percpu_elect(struct mvneta_port *pp)
 	}
 };
 
-static int mvneta_percpu_notifier(struct notifier_block *nfb,
-				  unsigned long action, void *hcpu)
+static int mvneta_cpu_online(unsigned int cpu, struct hlist_node *node)
 {
-	struct mvneta_port *pp = container_of(nfb, struct mvneta_port,
-					      cpu_notifier);
-	int cpu = (unsigned long)hcpu, other_cpu;
+	int other_cpu;
+	struct mvneta_port *pp = hlist_entry_safe(node, struct mvneta_port,
+						  node_online);
 	struct mvneta_pcpu_port *port = per_cpu_ptr(pp->ports, cpu);
 
-	switch (action) {
-	case CPU_ONLINE:
-	case CPU_ONLINE_FROZEN:
-	case CPU_DOWN_FAILED:
-	case CPU_DOWN_FAILED_FROZEN:
-		spin_lock(&pp->lock);
-		/* Configuring the driver for a new CPU while the
-		 * driver is stopping is racy, so just avoid it.
-		 */
-		if (pp->is_stopped) {
-			spin_unlock(&pp->lock);
-			break;
-		}
-		netif_tx_stop_all_queues(pp->dev);
 
-		/* We have to synchronise on tha napi of each CPU
-		 * except the one just being waked up
-		 */
-		for_each_online_cpu(other_cpu) {
-			if (other_cpu != cpu) {
-				struct mvneta_pcpu_port *other_port =
-					per_cpu_ptr(pp->ports, other_cpu);
+	spin_lock(&pp->lock);
+	/*
+	 * Configuring the driver for a new CPU while the driver is
+	 * stopping is racy, so just avoid it.
+	 */
+	if (pp->is_stopped) {
+		spin_unlock(&pp->lock);
+		return 0;
+	}
+	netif_tx_stop_all_queues(pp->dev);
 
-				napi_synchronize(&other_port->napi);
-			}
+	/*
+	 * We have to synchronise on tha napi of each CPU except the one
+	 * just being woken up
+	 */
+	for_each_online_cpu(other_cpu) {
+		if (other_cpu != cpu) {
+			struct mvneta_pcpu_port *other_port =
+				per_cpu_ptr(pp->ports, other_cpu);
+
+			napi_synchronize(&other_port->napi);
 		}
+	}
 
-		/* Mask all ethernet port interrupts */
-		on_each_cpu(mvneta_percpu_mask_interrupt, pp, true);
-		napi_enable(&port->napi);
+	/* Mask all ethernet port interrupts */
+	on_each_cpu(mvneta_percpu_mask_interrupt, pp, true);
+	napi_enable(&port->napi);
 
+	/*
+	 * Enable per-CPU interrupts on the CPU that is
+	 * brought up.
+	 */
+	mvneta_percpu_enable(pp);
 
-		/* Enable per-CPU interrupts on the CPU that is
-		 * brought up.
-		 */
-		mvneta_percpu_enable(pp);
+	/*
+	 * Enable per-CPU interrupt on the one CPU we care
+	 * about.
+	 */
+	mvneta_percpu_elect(pp);
 
-		/* Enable per-CPU interrupt on the one CPU we care
-		 * about.
-		 */
-		mvneta_percpu_elect(pp);
-
-		/* Unmask all ethernet port interrupts */
-		on_each_cpu(mvneta_percpu_unmask_interrupt, pp, true);
-		mvreg_write(pp, MVNETA_INTR_MISC_MASK,
-			MVNETA_CAUSE_PHY_STATUS_CHANGE |
-			MVNETA_CAUSE_LINK_CHANGE |
-			MVNETA_CAUSE_PSC_SYNC_CHANGE);
-		netif_tx_start_all_queues(pp->dev);
-		spin_unlock(&pp->lock);
-		break;
-	case CPU_DOWN_PREPARE:
-	case CPU_DOWN_PREPARE_FROZEN:
-		netif_tx_stop_all_queues(pp->dev);
-		/* Thanks to this lock we are sure that any pending
-		 * cpu election is done
-		 */
-		spin_lock(&pp->lock);
-		/* Mask all ethernet port interrupts */
-		on_each_cpu(mvneta_percpu_mask_interrupt, pp, true);
-		spin_unlock(&pp->lock);
+	/* Unmask all ethernet port interrupts */
+	on_each_cpu(mvneta_percpu_unmask_interrupt, pp, true);
+	mvreg_write(pp, MVNETA_INTR_MISC_MASK,
+		    MVNETA_CAUSE_PHY_STATUS_CHANGE |
+		    MVNETA_CAUSE_LINK_CHANGE |
+		    MVNETA_CAUSE_PSC_SYNC_CHANGE);
+	netif_tx_start_all_queues(pp->dev);
+	spin_unlock(&pp->lock);
+	return 0;
+}
 
-		napi_synchronize(&port->napi);
-		napi_disable(&port->napi);
-		/* Disable per-CPU interrupts on the CPU that is
-		 * brought down.
-		 */
-		mvneta_percpu_disable(pp);
+static int mvneta_cpu_down_prepare(unsigned int cpu, struct hlist_node *node)
+{
+	struct mvneta_port *pp = hlist_entry_safe(node, struct mvneta_port,
+						  node_online);
+	struct mvneta_pcpu_port *port = per_cpu_ptr(pp->ports, cpu);
 
-		break;
-	case CPU_DEAD:
-	case CPU_DEAD_FROZEN:
-		/* Check if a new CPU must be elected now this on is down */
-		spin_lock(&pp->lock);
-		mvneta_percpu_elect(pp);
-		spin_unlock(&pp->lock);
-		/* Unmask all ethernet port interrupts */
-		on_each_cpu(mvneta_percpu_unmask_interrupt, pp, true);
-		mvreg_write(pp, MVNETA_INTR_MISC_MASK,
-			MVNETA_CAUSE_PHY_STATUS_CHANGE |
-			MVNETA_CAUSE_LINK_CHANGE |
-			MVNETA_CAUSE_PSC_SYNC_CHANGE);
-		netif_tx_start_all_queues(pp->dev);
-		break;
-	}
+	/*
+	 * Thanks to this lock we are sure that any pending cpu election is
+	 * done.
+	 */
+	spin_lock(&pp->lock);
+	/* Mask all ethernet port interrupts */
+	on_each_cpu(mvneta_percpu_mask_interrupt, pp, true);
+	spin_unlock(&pp->lock);
 
-	return NOTIFY_OK;
+	napi_synchronize(&port->napi);
+	napi_disable(&port->napi);
+	/* Disable per-CPU interrupts on the CPU that is brought down. */
+	mvneta_percpu_disable(pp);
+	return 0;
+}
+
+static int mvneta_cpu_dead(unsigned int cpu, struct hlist_node *node)
+{
+	struct mvneta_port *pp = hlist_entry_safe(node, struct mvneta_port,
+						  node_dead);
+
+	/* Check if a new CPU must be elected now this on is down */
+	spin_lock(&pp->lock);
+	mvneta_percpu_elect(pp);
+	spin_unlock(&pp->lock);
+	/* Unmask all ethernet port interrupts */
+	on_each_cpu(mvneta_percpu_unmask_interrupt, pp, true);
+	mvreg_write(pp, MVNETA_INTR_MISC_MASK,
+		    MVNETA_CAUSE_PHY_STATUS_CHANGE |
+		    MVNETA_CAUSE_LINK_CHANGE |
+		    MVNETA_CAUSE_PSC_SYNC_CHANGE);
+	netif_tx_start_all_queues(pp->dev);
+	return 0;
 }
 
 static int mvneta_open(struct net_device *dev)
@@ -3442,7 +3450,15 @@ static int mvneta_open(struct net_device *dev)
 	/* Register a CPU notifier to handle the case where our CPU
 	 * might be taken offline.
 	 */
-	register_cpu_notifier(&pp->cpu_notifier);
+	ret = cpuhp_state_add_instance_nocalls(online_hpstate,
+					       &pp->node_online);
+	if (ret)
+		goto err_free_irq;
+
+	ret = cpuhp_state_add_instance_nocalls(CPUHP_NET_MVNETA_DEAD,
+					       &pp->node_dead);
+	if (ret)
+		goto err_free_online_hp;
 
 	/* In default link is down */
 	netif_carrier_off(pp->dev);
@@ -3450,15 +3466,19 @@ static int mvneta_open(struct net_device *dev)
 	ret = mvneta_mdio_probe(pp);
 	if (ret < 0) {
 		netdev_err(dev, "cannot probe MDIO bus\n");
-		goto err_free_irq;
+		goto err_free_dead_hp;
 	}
 
 	mvneta_start_dev(pp);
 
 	return 0;
 
+err_free_dead_hp:
+	cpuhp_state_remove_instance_nocalls(CPUHP_NET_MVNETA_DEAD,
+					    &pp->node_dead);
+err_free_online_hp:
+	cpuhp_state_remove_instance_nocalls(online_hpstate, &pp->node_online);
 err_free_irq:
-	unregister_cpu_notifier(&pp->cpu_notifier);
 	on_each_cpu(mvneta_percpu_disable, pp, true);
 	free_percpu_irq(pp->dev->irq, pp->ports);
 err_cleanup_txqs:
@@ -3484,7 +3504,10 @@ static int mvneta_stop(struct net_device *dev)
 
 	mvneta_stop_dev(pp);
 	mvneta_mdio_remove(pp);
-	unregister_cpu_notifier(&pp->cpu_notifier);
+
+	cpuhp_state_remove_instance_nocalls(online_hpstate, &pp->node_online);
+	cpuhp_state_remove_instance_nocalls(CPUHP_NET_MVNETA_DEAD,
+					    &pp->node_dead);
 	on_each_cpu(mvneta_percpu_disable, pp, true);
 	free_percpu_irq(dev->irq, pp->ports);
 	mvneta_cleanup_rxqs(pp);
@@ -3495,42 +3518,31 @@ static int mvneta_stop(struct net_device *dev)
 
 static int mvneta_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
 {
-	struct mvneta_port *pp = netdev_priv(dev);
-
-	if (!pp->phy_dev)
+	if (!dev->phydev)
 		return -ENOTSUPP;
 
-	return phy_mii_ioctl(pp->phy_dev, ifr, cmd);
+	return phy_mii_ioctl(dev->phydev, ifr, cmd);
 }
 
 /* Ethtool methods */
 
-/* Get settings (phy address, speed) for ethtools */
-int mvneta_ethtool_get_settings(struct net_device *dev, struct ethtool_cmd *cmd)
+/* Set link ksettings (phy address, speed) for ethtools */
+static int
+mvneta_ethtool_set_link_ksettings(struct net_device *ndev,
+				  const struct ethtool_link_ksettings *cmd)
 {
-	struct mvneta_port *pp = netdev_priv(dev);
-
-	if (!pp->phy_dev)
-		return -ENODEV;
-
-	return phy_ethtool_gset(pp->phy_dev, cmd);
-}
-
-/* Set settings (phy address, speed) for ethtools */
-int mvneta_ethtool_set_settings(struct net_device *dev, struct ethtool_cmd *cmd)
-{
-	struct mvneta_port *pp = netdev_priv(dev);
-	struct phy_device *phydev = pp->phy_dev;
+	struct mvneta_port *pp = netdev_priv(ndev);
+	struct phy_device *phydev = ndev->phydev;
 
 	if (!phydev)
 		return -ENODEV;
 
-	if ((cmd->autoneg == AUTONEG_ENABLE) != pp->use_inband_status) {
+	if ((cmd->base.autoneg == AUTONEG_ENABLE) != pp->use_inband_status) {
 		u32 val;
 
-		mvneta_set_autoneg(pp, cmd->autoneg == AUTONEG_ENABLE);
+		mvneta_set_autoneg(pp, cmd->base.autoneg == AUTONEG_ENABLE);
 
-		if (cmd->autoneg == AUTONEG_DISABLE) {
+		if (cmd->base.autoneg == AUTONEG_DISABLE) {
 			val = mvreg_read(pp, MVNETA_GMAC_AUTONEG_CONFIG);
 			val &= ~(MVNETA_GMAC_CONFIG_MII_SPEED |
 				 MVNETA_GMAC_CONFIG_GMII_SPEED |
@@ -3547,17 +3559,17 @@ int mvneta_ethtool_set_settings(struct net_device *dev, struct ethtool_cmd *cmd)
 			mvreg_write(pp, MVNETA_GMAC_AUTONEG_CONFIG, val);
 		}
 
-		pp->use_inband_status = (cmd->autoneg == AUTONEG_ENABLE);
+		pp->use_inband_status = (cmd->base.autoneg == AUTONEG_ENABLE);
 		netdev_info(pp->dev, "autoneg status set to %i\n",
 			    pp->use_inband_status);
 
-		if (netif_running(dev)) {
+		if (netif_running(ndev)) {
 			mvneta_port_down(pp);
 			mvneta_port_up(pp);
 		}
 	}
 
-	return phy_ethtool_sset(pp->phy_dev, cmd);
+	return phy_ethtool_ksettings_set(ndev->phydev, cmd);
 }
 
 /* Set interrupt coalescing for ethtools */
@@ -3821,8 +3833,6 @@ static const struct net_device_ops mvneta_netdev_ops = {
 
 const struct ethtool_ops mvneta_eth_tool_ops = {
 	.get_link       = ethtool_op_get_link,
-	.get_settings   = mvneta_ethtool_get_settings,
-	.set_settings   = mvneta_ethtool_set_settings,
 	.set_coalesce   = mvneta_ethtool_set_coalesce,
 	.get_coalesce   = mvneta_ethtool_get_coalesce,
 	.get_drvinfo    = mvneta_ethtool_get_drvinfo,
@@ -3835,6 +3845,8 @@ const struct ethtool_ops mvneta_eth_tool_ops = {
 	.get_rxnfc	= mvneta_ethtool_get_rxnfc,
 	.get_rxfh	= mvneta_ethtool_get_rxfh,
 	.set_rxfh	= mvneta_ethtool_set_rxfh,
+	.get_link_ksettings = phy_ethtool_get_link_ksettings,
+	.set_link_ksettings = mvneta_ethtool_set_link_ksettings,
 };
 
 /* Initialize hw */
@@ -4024,7 +4036,6 @@ static int mvneta_probe(struct platform_device *pdev)
 	err = of_property_read_string(dn, "managed", &managed);
 	pp->use_inband_status = (err == 0 &&
 				 strcmp(managed, "in-band-status") == 0);
-	pp->cpu_notifier.notifier_call = mvneta_percpu_notifier;
 
 	pp->rxq_def = rxq_def;
 
@@ -4227,7 +4238,42 @@ static struct platform_driver mvneta_driver = {
 	},
 };
 
-module_platform_driver(mvneta_driver);
+static int __init mvneta_driver_init(void)
+{
+	int ret;
+
+	ret = cpuhp_setup_state_multi(CPUHP_AP_ONLINE_DYN, "net/mvmeta:online",
+				      mvneta_cpu_online,
+				      mvneta_cpu_down_prepare);
+	if (ret < 0)
+		goto out;
+	online_hpstate = ret;
+	ret = cpuhp_setup_state_multi(CPUHP_NET_MVNETA_DEAD, "net/mvneta:dead",
+				      NULL, mvneta_cpu_dead);
+	if (ret)
+		goto err_dead;
+
+	ret = platform_driver_register(&mvneta_driver);
+	if (ret)
+		goto err;
+	return 0;
+
+err:
+	cpuhp_remove_multi_state(CPUHP_NET_MVNETA_DEAD);
+err_dead:
+	cpuhp_remove_multi_state(online_hpstate);
+out:
+	return ret;
+}
+module_init(mvneta_driver_init);
+
+static void __exit mvneta_driver_exit(void)
+{
+	platform_driver_unregister(&mvneta_driver);
+	cpuhp_remove_multi_state(CPUHP_NET_MVNETA_DEAD);
+	cpuhp_remove_multi_state(online_hpstate);
+}
+module_exit(mvneta_driver_exit);
 
 MODULE_DESCRIPTION("Marvell NETA Ethernet Driver - www.marvell.com");
 MODULE_AUTHOR("Rami Rosen <rosenr@marvell.com>, Thomas Petazzoni <thomas.petazzoni@free-electrons.com>");
diff --git a/drivers/net/ethernet/marvell/mvneta_bm.h b/drivers/net/ethernet/marvell/mvneta_bm.h
index e74fd44a92f7..a32de432800c 100644
--- a/drivers/net/ethernet/marvell/mvneta_bm.h
+++ b/drivers/net/ethernet/marvell/mvneta_bm.h
@@ -133,7 +133,7 @@ struct mvneta_bm_pool {
 void *mvneta_frag_alloc(unsigned int frag_size);
 void mvneta_frag_free(unsigned int frag_size, void *data);
 
-#if defined(CONFIG_MVNETA_BM) || defined(CONFIG_MVNETA_BM_MODULE)
+#if IS_ENABLED(CONFIG_MVNETA_BM)
 void mvneta_bm_pool_destroy(struct mvneta_bm *priv,
 			    struct mvneta_bm_pool *bm_pool, u8 port_map);
 void mvneta_bm_bufs_free(struct mvneta_bm *priv, struct mvneta_bm_pool *bm_pool,
diff --git a/drivers/net/ethernet/marvell/sky2.c b/drivers/net/ethernet/marvell/sky2.c
index 467138b423d3..f05ea56dcff2 100644
--- a/drivers/net/ethernet/marvell/sky2.c
+++ b/drivers/net/ethernet/marvell/sky2.c
@@ -3070,7 +3070,7 @@ static int sky2_poll(struct napi_struct *napi, int work_limit)
 			goto done;
 	}
 
-	napi_complete(napi);
+	napi_complete_done(napi, work_done);
 	sky2_read32(hw, B0_Y2_SP_LISR);
 done:
 
diff --git a/drivers/net/ethernet/mediatek/mtk_eth_soc.c b/drivers/net/ethernet/mediatek/mtk_eth_soc.c
index 3743af8f1ded..4a62ffd7729d 100644
--- a/drivers/net/ethernet/mediatek/mtk_eth_soc.c
+++ b/drivers/net/ethernet/mediatek/mtk_eth_soc.c
@@ -18,6 +18,7 @@
 #include <linux/mfd/syscon.h>
 #include <linux/regmap.h>
 #include <linux/clk.h>
+#include <linux/pm_runtime.h>
 #include <linux/if_vlan.h>
 #include <linux/reset.h>
 #include <linux/tcp.h>
@@ -51,7 +52,7 @@ static const struct mtk_ethtool_stats {
 };
 
 static const char * const mtk_clks_source_name[] = {
-	"ethif", "esw", "gp1", "gp2"
+	"ethif", "esw", "gp1", "gp2", "trgpll"
 };
 
 void mtk_w32(struct mtk_eth *eth, u32 val, unsigned reg)
@@ -134,6 +135,33 @@ static int mtk_mdio_read(struct mii_bus *bus, int phy_addr, int phy_reg)
 	return _mtk_mdio_read(eth, phy_addr, phy_reg);
 }
 
+static void mtk_gmac0_rgmii_adjust(struct mtk_eth *eth, int speed)
+{
+	u32 val;
+	int ret;
+
+	val = (speed == SPEED_1000) ?
+		INTF_MODE_RGMII_1000 : INTF_MODE_RGMII_10_100;
+	mtk_w32(eth, val, INTF_MODE);
+
+	regmap_update_bits(eth->ethsys, ETHSYS_CLKCFG0,
+			   ETHSYS_TRGMII_CLK_SEL362_5,
+			   ETHSYS_TRGMII_CLK_SEL362_5);
+
+	val = (speed == SPEED_1000) ? 250000000 : 500000000;
+	ret = clk_set_rate(eth->clks[MTK_CLK_TRGPLL], val);
+	if (ret)
+		dev_err(eth->dev, "Failed to set trgmii pll: %d\n", ret);
+
+	val = (speed == SPEED_1000) ?
+		RCK_CTRL_RGMII_1000 : RCK_CTRL_RGMII_10_100;
+	mtk_w32(eth, val, TRGMII_RCK_CTRL);
+
+	val = (speed == SPEED_1000) ?
+		TCK_CTRL_RGMII_1000 : TCK_CTRL_RGMII_10_100;
+	mtk_w32(eth, val, TRGMII_TCK_CTRL);
+}
+
 static void mtk_phy_link_adjust(struct net_device *dev)
 {
 	struct mtk_mac *mac = netdev_priv(dev);
@@ -144,7 +172,10 @@ static void mtk_phy_link_adjust(struct net_device *dev)
 		  MAC_MCR_RX_EN | MAC_MCR_BACKOFF_EN |
 		  MAC_MCR_BACKPR_EN;
 
-	switch (mac->phy_dev->speed) {
+	if (unlikely(test_bit(MTK_RESETTING, &mac->hw->state)))
+		return;
+
+	switch (dev->phydev->speed) {
 	case SPEED_1000:
 		mcr |= MAC_MCR_SPEED_1000;
 		break;
@@ -153,20 +184,23 @@ static void mtk_phy_link_adjust(struct net_device *dev)
 		break;
 	};
 
-	if (mac->phy_dev->link)
+	if (mac->id == 0 && !mac->trgmii)
+		mtk_gmac0_rgmii_adjust(mac->hw, dev->phydev->speed);
+
+	if (dev->phydev->link)
 		mcr |= MAC_MCR_FORCE_LINK;
 
-	if (mac->phy_dev->duplex) {
+	if (dev->phydev->duplex) {
 		mcr |= MAC_MCR_FORCE_DPX;
 
-		if (mac->phy_dev->pause)
+		if (dev->phydev->pause)
 			rmt_adv = LPA_PAUSE_CAP;
-		if (mac->phy_dev->asym_pause)
+		if (dev->phydev->asym_pause)
 			rmt_adv |= LPA_PAUSE_ASYM;
 
-		if (mac->phy_dev->advertising & ADVERTISED_Pause)
+		if (dev->phydev->advertising & ADVERTISED_Pause)
 			lcl_adv |= ADVERTISE_PAUSE_CAP;
-		if (mac->phy_dev->advertising & ADVERTISED_Asym_Pause)
+		if (dev->phydev->advertising & ADVERTISED_Asym_Pause)
 			lcl_adv |= ADVERTISE_PAUSE_ASYM;
 
 		flowctrl = mii_resolve_flowctrl_fdx(lcl_adv, rmt_adv);
@@ -183,7 +217,7 @@ static void mtk_phy_link_adjust(struct net_device *dev)
 
 	mtk_w32(mac->hw, mcr, MTK_MAC_MCR(mac->id));
 
-	if (mac->phy_dev->link)
+	if (dev->phydev->link)
 		netif_carrier_on(dev);
 	else
 		netif_carrier_off(dev);
@@ -192,17 +226,9 @@ static void mtk_phy_link_adjust(struct net_device *dev)
 static int mtk_phy_connect_node(struct mtk_eth *eth, struct mtk_mac *mac,
 				struct device_node *phy_node)
 {
-	const __be32 *_addr = NULL;
 	struct phy_device *phydev;
-	int phy_mode, addr;
-
-	_addr = of_get_property(phy_node, "reg", NULL);
+	int phy_mode;
 
-	if (!_addr || (be32_to_cpu(*_addr) >= 0x20)) {
-		pr_err("%s: invalid phy address\n", phy_node->name);
-		return -EINVAL;
-	}
-	addr = be32_to_cpu(*_addr);
 	phy_mode = of_get_phy_mode(phy_node);
 	if (phy_mode < 0) {
 		dev_err(eth->dev, "incorrect phy-mode %d\n", phy_mode);
@@ -221,17 +247,17 @@ static int mtk_phy_connect_node(struct mtk_eth *eth, struct mtk_mac *mac,
 		 mac->id, phydev_name(phydev), phydev->phy_id,
 		 phydev->drv->name);
 
-	mac->phy_dev = phydev;
-
 	return 0;
 }
 
-static int mtk_phy_connect(struct mtk_mac *mac)
+static int mtk_phy_connect(struct net_device *dev)
 {
-	struct mtk_eth *eth = mac->hw;
+	struct mtk_mac *mac = netdev_priv(dev);
+	struct mtk_eth *eth;
 	struct device_node *np;
-	u32 val, ge_mode;
+	u32 val;
 
+	eth = mac->hw;
 	np = of_parse_phandle(mac->of_node, "phy-handle", 0);
 	if (!np && of_phy_is_fixed_link(mac->of_node))
 		if (!of_phy_register_fixed_link(mac->of_node))
@@ -240,22 +266,24 @@ static int mtk_phy_connect(struct mtk_mac *mac)
 		return -ENODEV;
 
 	switch (of_get_phy_mode(np)) {
+	case PHY_INTERFACE_MODE_TRGMII:
+		mac->trgmii = true;
 	case PHY_INTERFACE_MODE_RGMII_TXID:
 	case PHY_INTERFACE_MODE_RGMII_RXID:
 	case PHY_INTERFACE_MODE_RGMII_ID:
 	case PHY_INTERFACE_MODE_RGMII:
-		ge_mode = 0;
+		mac->ge_mode = 0;
 		break;
 	case PHY_INTERFACE_MODE_MII:
-		ge_mode = 1;
+		mac->ge_mode = 1;
 		break;
 	case PHY_INTERFACE_MODE_REVMII:
-		ge_mode = 2;
+		mac->ge_mode = 2;
 		break;
 	case PHY_INTERFACE_MODE_RMII:
 		if (!mac->id)
 			goto err_phy;
-		ge_mode = 3;
+		mac->ge_mode = 3;
 		break;
 	default:
 		goto err_phy;
@@ -264,23 +292,26 @@ static int mtk_phy_connect(struct mtk_mac *mac)
 	/* put the gmac into the right mode */
 	regmap_read(eth->ethsys, ETHSYS_SYSCFG0, &val);
 	val &= ~SYSCFG0_GE_MODE(SYSCFG0_GE_MASK, mac->id);
-	val |= SYSCFG0_GE_MODE(ge_mode, mac->id);
+	val |= SYSCFG0_GE_MODE(mac->ge_mode, mac->id);
 	regmap_write(eth->ethsys, ETHSYS_SYSCFG0, val);
 
-	mtk_phy_connect_node(eth, mac, np);
-	mac->phy_dev->autoneg = AUTONEG_ENABLE;
-	mac->phy_dev->speed = 0;
-	mac->phy_dev->duplex = 0;
+	/* couple phydev to net_device */
+	if (mtk_phy_connect_node(eth, mac, np))
+		goto err_phy;
+
+	dev->phydev->autoneg = AUTONEG_ENABLE;
+	dev->phydev->speed = 0;
+	dev->phydev->duplex = 0;
 
 	if (of_phy_is_fixed_link(mac->of_node))
-		mac->phy_dev->supported |=
+		dev->phydev->supported |=
 		SUPPORTED_Pause | SUPPORTED_Asym_Pause;
 
-	mac->phy_dev->supported &= PHY_GBIT_FEATURES | SUPPORTED_Pause |
+	dev->phydev->supported &= PHY_GBIT_FEATURES | SUPPORTED_Pause |
 				   SUPPORTED_Asym_Pause;
-	mac->phy_dev->advertising = mac->phy_dev->supported |
+	dev->phydev->advertising = dev->phydev->supported |
 				    ADVERTISED_Autoneg;
-	phy_start_aneg(mac->phy_dev);
+	phy_start_aneg(dev->phydev);
 
 	of_node_put(np);
 
@@ -288,7 +319,7 @@ static int mtk_phy_connect(struct mtk_mac *mac)
 
 err_phy:
 	of_node_put(np);
-	dev_err(eth->dev, "invalid phy_mode\n");
+	dev_err(eth->dev, "%s: invalid phy\n", __func__);
 	return -EINVAL;
 }
 
@@ -336,25 +367,27 @@ static void mtk_mdio_cleanup(struct mtk_eth *eth)
 	mdiobus_unregister(eth->mii_bus);
 }
 
-static inline void mtk_irq_disable(struct mtk_eth *eth, u32 mask)
+static inline void mtk_irq_disable(struct mtk_eth *eth,
+				   unsigned reg, u32 mask)
 {
 	unsigned long flags;
 	u32 val;
 
 	spin_lock_irqsave(&eth->irq_lock, flags);
-	val = mtk_r32(eth, MTK_QDMA_INT_MASK);
-	mtk_w32(eth, val & ~mask, MTK_QDMA_INT_MASK);
+	val = mtk_r32(eth, reg);
+	mtk_w32(eth, val & ~mask, reg);
 	spin_unlock_irqrestore(&eth->irq_lock, flags);
 }
 
-static inline void mtk_irq_enable(struct mtk_eth *eth, u32 mask)
+static inline void mtk_irq_enable(struct mtk_eth *eth,
+				  unsigned reg, u32 mask)
 {
 	unsigned long flags;
 	u32 val;
 
 	spin_lock_irqsave(&eth->irq_lock, flags);
-	val = mtk_r32(eth, MTK_QDMA_INT_MASK);
-	mtk_w32(eth, val | mask, MTK_QDMA_INT_MASK);
+	val = mtk_r32(eth, reg);
+	mtk_w32(eth, val | mask, reg);
 	spin_unlock_irqrestore(&eth->irq_lock, flags);
 }
 
@@ -363,18 +396,20 @@ static int mtk_set_mac_address(struct net_device *dev, void *p)
 	int ret = eth_mac_addr(dev, p);
 	struct mtk_mac *mac = netdev_priv(dev);
 	const char *macaddr = dev->dev_addr;
-	unsigned long flags;
 
 	if (ret)
 		return ret;
 
-	spin_lock_irqsave(&mac->hw->page_lock, flags);
+	if (unlikely(test_bit(MTK_RESETTING, &mac->hw->state)))
+		return -EBUSY;
+
+	spin_lock_bh(&mac->hw->page_lock);
 	mtk_w32(mac->hw, (macaddr[0] << 8) | macaddr[1],
 		MTK_GDMA_MAC_ADRH(mac->id));
 	mtk_w32(mac->hw, (macaddr[2] << 24) | (macaddr[3] << 16) |
 		(macaddr[4] << 8) | macaddr[5],
 		MTK_GDMA_MAC_ADRL(mac->id));
-	spin_unlock_irqrestore(&mac->hw->page_lock, flags);
+	spin_unlock_bh(&mac->hw->page_lock);
 
 	return 0;
 }
@@ -759,7 +794,6 @@ static int mtk_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	struct mtk_eth *eth = mac->hw;
 	struct mtk_tx_ring *ring = &eth->tx_ring;
 	struct net_device_stats *stats = &dev->stats;
-	unsigned long flags;
 	bool gso = false;
 	int tx_num;
 
@@ -767,14 +801,17 @@ static int mtk_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	 * however we have 2 queues running on the same ring so we need to lock
 	 * the ring access
 	 */
-	spin_lock_irqsave(&eth->page_lock, flags);
+	spin_lock(&eth->page_lock);
+
+	if (unlikely(test_bit(MTK_RESETTING, &eth->state)))
+		goto drop;
 
 	tx_num = mtk_cal_txd_req(skb);
 	if (unlikely(atomic_read(&ring->free_count) <= tx_num)) {
 		mtk_stop_queue(eth);
 		netif_err(eth, tx_queued, dev,
 			  "Tx Ring full when queue awake!\n");
-		spin_unlock_irqrestore(&eth->page_lock, flags);
+		spin_unlock(&eth->page_lock);
 		return NETDEV_TX_BUSY;
 	}
 
@@ -799,22 +836,62 @@ static int mtk_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	if (unlikely(atomic_read(&ring->free_count) <= ring->thresh))
 		mtk_stop_queue(eth);
 
-	spin_unlock_irqrestore(&eth->page_lock, flags);
+	spin_unlock(&eth->page_lock);
 
 	return NETDEV_TX_OK;
 
 drop:
-	spin_unlock_irqrestore(&eth->page_lock, flags);
+	spin_unlock(&eth->page_lock);
 	stats->tx_dropped++;
 	dev_kfree_skb(skb);
 	return NETDEV_TX_OK;
 }
 
+static struct mtk_rx_ring *mtk_get_rx_ring(struct mtk_eth *eth)
+{
+	int i;
+	struct mtk_rx_ring *ring;
+	int idx;
+
+	if (!eth->hwlro)
+		return &eth->rx_ring[0];
+
+	for (i = 0; i < MTK_MAX_RX_RING_NUM; i++) {
+		ring = &eth->rx_ring[i];
+		idx = NEXT_RX_DESP_IDX(ring->calc_idx, ring->dma_size);
+		if (ring->dma[idx].rxd2 & RX_DMA_DONE) {
+			ring->calc_idx_update = true;
+			return ring;
+		}
+	}
+
+	return NULL;
+}
+
+static void mtk_update_rx_cpu_idx(struct mtk_eth *eth)
+{
+	struct mtk_rx_ring *ring;
+	int i;
+
+	if (!eth->hwlro) {
+		ring = &eth->rx_ring[0];
+		mtk_w32(eth, ring->calc_idx, ring->crx_idx_reg);
+	} else {
+		for (i = 0; i < MTK_MAX_RX_RING_NUM; i++) {
+			ring = &eth->rx_ring[i];
+			if (ring->calc_idx_update) {
+				ring->calc_idx_update = false;
+				mtk_w32(eth, ring->calc_idx, ring->crx_idx_reg);
+			}
+		}
+	}
+}
+
 static int mtk_poll_rx(struct napi_struct *napi, int budget,
 		       struct mtk_eth *eth)
 {
-	struct mtk_rx_ring *ring = &eth->rx_ring;
-	int idx = ring->calc_idx;
+	struct mtk_rx_ring *ring;
+	int idx;
 	struct sk_buff *skb;
 	u8 *data, *new_data;
 	struct mtk_rx_dma *rxd, trxd;
@@ -826,7 +903,11 @@ static int mtk_poll_rx(struct napi_struct *napi, int budget,
 		dma_addr_t dma_addr;
 		int mac = 0;
 
-		idx = NEXT_RX_DESP_IDX(idx);
+		ring = mtk_get_rx_ring(eth);
+		if (unlikely(!ring))
+			goto rx_done;
+
+		idx = NEXT_RX_DESP_IDX(ring->calc_idx, ring->dma_size);
 		rxd = &ring->dma[idx];
 		data = ring->data[idx];
 
@@ -841,6 +922,9 @@ static int mtk_poll_rx(struct napi_struct *napi, int budget,
 
 		netdev = eth->netdev[mac];
 
+		if (unlikely(test_bit(MTK_RESETTING, &eth->state)))
+			goto release_desc;
+
 		/* alloc new buffer */
 		new_data = napi_alloc_frag(ring->frag_size);
 		if (unlikely(!new_data)) {
@@ -890,17 +974,19 @@ release_desc:
 		rxd->rxd2 = RX_DMA_PLEN0(ring->buf_size);
 
 		ring->calc_idx = idx;
+
+		done++;
+	}
+
+rx_done:
+	if (done) {
 		/* make sure that all changes to the dma ring are flushed before
 		 * we continue
 		 */
 		wmb();
-		mtk_w32(eth, ring->calc_idx, MTK_QRX_CRX_IDX0);
-		done++;
+		mtk_update_rx_cpu_idx(eth);
 	}
 
-	if (done < budget)
-		mtk_w32(eth, MTK_RX_DONE_INT, MTK_QMTK_INT_STATUS);
-
 	return done;
 }
 
@@ -1009,7 +1095,7 @@ static int mtk_napi_tx(struct napi_struct *napi, int budget)
 		return budget;
 
 	napi_complete(napi);
-	mtk_irq_enable(eth, MTK_TX_DONE_INT);
+	mtk_irq_enable(eth, MTK_QDMA_INT_MASK, MTK_TX_DONE_INT);
 
 	return tx_done;
 }
@@ -1019,30 +1105,33 @@ static int mtk_napi_rx(struct napi_struct *napi, int budget)
 	struct mtk_eth *eth = container_of(napi, struct mtk_eth, rx_napi);
 	u32 status, mask;
 	int rx_done = 0;
+	int remain_budget = budget;
 
 	mtk_handle_status_irq(eth);
-	mtk_w32(eth, MTK_RX_DONE_INT, MTK_QMTK_INT_STATUS);
-	rx_done = mtk_poll_rx(napi, budget, eth);
+
+poll_again:
+	mtk_w32(eth, MTK_RX_DONE_INT, MTK_PDMA_INT_STATUS);
+	rx_done = mtk_poll_rx(napi, remain_budget, eth);
 
 	if (unlikely(netif_msg_intr(eth))) {
-		status = mtk_r32(eth, MTK_QMTK_INT_STATUS);
-		mask = mtk_r32(eth, MTK_QDMA_INT_MASK);
+		status = mtk_r32(eth, MTK_PDMA_INT_STATUS);
+		mask = mtk_r32(eth, MTK_PDMA_INT_MASK);
 		dev_info(eth->dev,
 			 "done rx %d, intr 0x%08x/0x%x\n",
 			 rx_done, status, mask);
 	}
-
-	if (rx_done == budget)
-		return budget;
-
-	status = mtk_r32(eth, MTK_QMTK_INT_STATUS);
-	if (status & MTK_RX_DONE_INT)
+	if (rx_done == remain_budget)
 		return budget;
 
+	status = mtk_r32(eth, MTK_PDMA_INT_STATUS);
+	if (status & MTK_RX_DONE_INT) {
+		remain_budget -= rx_done;
+		goto poll_again;
+	}
 	napi_complete(napi);
-	mtk_irq_enable(eth, MTK_RX_DONE_INT);
+	mtk_irq_enable(eth, MTK_PDMA_INT_MASK, MTK_RX_DONE_INT);
 
-	return rx_done;
+	return rx_done + budget - remain_budget;
 }
 
 static int mtk_tx_alloc(struct mtk_eth *eth)
@@ -1089,6 +1178,7 @@ static int mtk_tx_alloc(struct mtk_eth *eth)
 	mtk_w32(eth,
 		ring->phys + ((MTK_DMA_SIZE - 1) * sz),
 		MTK_QTX_DRX_PTR);
+	mtk_w32(eth, (QDMA_RES_THRES << 8) | QDMA_RES_THRES, MTK_QTX_CFG(0));
 
 	return 0;
 
@@ -1117,32 +1207,41 @@ static void mtk_tx_clean(struct mtk_eth *eth)
 	}
 }
 
-static int mtk_rx_alloc(struct mtk_eth *eth)
+static int mtk_rx_alloc(struct mtk_eth *eth, int ring_no, int rx_flag)
 {
-	struct mtk_rx_ring *ring = &eth->rx_ring;
+	struct mtk_rx_ring *ring = &eth->rx_ring[ring_no];
+	int rx_data_len, rx_dma_size;
 	int i;
 
-	ring->frag_size = mtk_max_frag_size(ETH_DATA_LEN);
+	if (rx_flag == MTK_RX_FLAGS_HWLRO) {
+		rx_data_len = MTK_MAX_LRO_RX_LENGTH;
+		rx_dma_size = MTK_HW_LRO_DMA_SIZE;
+	} else {
+		rx_data_len = ETH_DATA_LEN;
+		rx_dma_size = MTK_DMA_SIZE;
+	}
+
+	ring->frag_size = mtk_max_frag_size(rx_data_len);
 	ring->buf_size = mtk_max_buf_size(ring->frag_size);
-	ring->data = kcalloc(MTK_DMA_SIZE, sizeof(*ring->data),
+	ring->data = kcalloc(rx_dma_size, sizeof(*ring->data),
 			     GFP_KERNEL);
 	if (!ring->data)
 		return -ENOMEM;
 
-	for (i = 0; i < MTK_DMA_SIZE; i++) {
+	for (i = 0; i < rx_dma_size; i++) {
 		ring->data[i] = netdev_alloc_frag(ring->frag_size);
 		if (!ring->data[i])
 			return -ENOMEM;
 	}
 
 	ring->dma = dma_alloc_coherent(eth->dev,
-				       MTK_DMA_SIZE * sizeof(*ring->dma),
+				       rx_dma_size * sizeof(*ring->dma),
 				       &ring->phys,
 				       GFP_ATOMIC | __GFP_ZERO);
 	if (!ring->dma)
 		return -ENOMEM;
 
-	for (i = 0; i < MTK_DMA_SIZE; i++) {
+	for (i = 0; i < rx_dma_size; i++) {
 		dma_addr_t dma_addr = dma_map_single(eth->dev,
 				ring->data[i] + NET_SKB_PAD,
 				ring->buf_size,
@@ -1153,28 +1252,30 @@ static int mtk_rx_alloc(struct mtk_eth *eth)
 
 		ring->dma[i].rxd2 = RX_DMA_PLEN0(ring->buf_size);
 	}
-	ring->calc_idx = MTK_DMA_SIZE - 1;
+	ring->dma_size = rx_dma_size;
+	ring->calc_idx_update = false;
+	ring->calc_idx = rx_dma_size - 1;
+	ring->crx_idx_reg = MTK_PRX_CRX_IDX_CFG(ring_no);
 	/* make sure that all changes to the dma ring are flushed before we
 	 * continue
 	 */
 	wmb();
 
-	mtk_w32(eth, eth->rx_ring.phys, MTK_QRX_BASE_PTR0);
-	mtk_w32(eth, MTK_DMA_SIZE, MTK_QRX_MAX_CNT0);
-	mtk_w32(eth, eth->rx_ring.calc_idx, MTK_QRX_CRX_IDX0);
-	mtk_w32(eth, MTK_PST_DRX_IDX0, MTK_QDMA_RST_IDX);
-	mtk_w32(eth, (QDMA_RES_THRES << 8) | QDMA_RES_THRES, MTK_QTX_CFG(0));
+	mtk_w32(eth, ring->phys, MTK_PRX_BASE_PTR_CFG(ring_no));
+	mtk_w32(eth, rx_dma_size, MTK_PRX_MAX_CNT_CFG(ring_no));
+	mtk_w32(eth, ring->calc_idx, ring->crx_idx_reg);
+	mtk_w32(eth, MTK_PST_DRX_IDX_CFG(ring_no), MTK_PDMA_RST_IDX);
 
 	return 0;
 }
 
-static void mtk_rx_clean(struct mtk_eth *eth)
+static void mtk_rx_clean(struct mtk_eth *eth, int ring_no)
 {
-	struct mtk_rx_ring *ring = &eth->rx_ring;
+	struct mtk_rx_ring *ring = &eth->rx_ring[ring_no];
 	int i;
 
 	if (ring->data && ring->dma) {
-		for (i = 0; i < MTK_DMA_SIZE; i++) {
+		for (i = 0; i < ring->dma_size; i++) {
 			if (!ring->data[i])
 				continue;
 			if (!ring->dma[i].rxd1)
@@ -1191,13 +1292,275 @@ static void mtk_rx_clean(struct mtk_eth *eth)
 
 	if (ring->dma) {
 		dma_free_coherent(eth->dev,
-				  MTK_DMA_SIZE * sizeof(*ring->dma),
+				  ring->dma_size * sizeof(*ring->dma),
 				  ring->dma,
 				  ring->phys);
 		ring->dma = NULL;
 	}
 }
 
+static int mtk_hwlro_rx_init(struct mtk_eth *eth)
+{
+	int i;
+	u32 ring_ctrl_dw1 = 0, ring_ctrl_dw2 = 0, ring_ctrl_dw3 = 0;
+	u32 lro_ctrl_dw0 = 0, lro_ctrl_dw3 = 0;
+
+	/* set LRO rings to auto-learn modes */
+	ring_ctrl_dw2 |= MTK_RING_AUTO_LERAN_MODE;
+
+	/* validate LRO ring */
+	ring_ctrl_dw2 |= MTK_RING_VLD;
+
+	/* set AGE timer (unit: 20us) */
+	ring_ctrl_dw2 |= MTK_RING_AGE_TIME_H;
+	ring_ctrl_dw1 |= MTK_RING_AGE_TIME_L;
+
+	/* set max AGG timer (unit: 20us) */
+	ring_ctrl_dw2 |= MTK_RING_MAX_AGG_TIME;
+
+	/* set max LRO AGG count */
+	ring_ctrl_dw2 |= MTK_RING_MAX_AGG_CNT_L;
+	ring_ctrl_dw3 |= MTK_RING_MAX_AGG_CNT_H;
+
+	for (i = 1; i < MTK_MAX_RX_RING_NUM; i++) {
+		mtk_w32(eth, ring_ctrl_dw1, MTK_LRO_CTRL_DW1_CFG(i));
+		mtk_w32(eth, ring_ctrl_dw2, MTK_LRO_CTRL_DW2_CFG(i));
+		mtk_w32(eth, ring_ctrl_dw3, MTK_LRO_CTRL_DW3_CFG(i));
+	}
+
+	/* IPv4 checksum update enable */
+	lro_ctrl_dw0 |= MTK_L3_CKS_UPD_EN;
+
+	/* switch priority comparison to packet count mode */
+	lro_ctrl_dw0 |= MTK_LRO_ALT_PKT_CNT_MODE;
+
+	/* bandwidth threshold setting */
+	mtk_w32(eth, MTK_HW_LRO_BW_THRE, MTK_PDMA_LRO_CTRL_DW2);
+
+	/* auto-learn score delta setting */
+	mtk_w32(eth, MTK_HW_LRO_REPLACE_DELTA, MTK_PDMA_LRO_ALT_SCORE_DELTA);
+
+	/* set refresh timer for altering flows to 1 sec. (unit: 20us) */
+	mtk_w32(eth, (MTK_HW_LRO_TIMER_UNIT << 16) | MTK_HW_LRO_REFRESH_TIME,
+		MTK_PDMA_LRO_ALT_REFRESH_TIMER);
+
+	/* set HW LRO mode & the max aggregation count for rx packets */
+	lro_ctrl_dw3 |= MTK_ADMA_MODE | (MTK_HW_LRO_MAX_AGG_CNT & 0xff);
+
+	/* the minimal remaining room of SDL0 in RXD for lro aggregation */
+	lro_ctrl_dw3 |= MTK_LRO_MIN_RXD_SDL;
+
+	/* enable HW LRO */
+	lro_ctrl_dw0 |= MTK_LRO_EN;
+
+	mtk_w32(eth, lro_ctrl_dw3, MTK_PDMA_LRO_CTRL_DW3);
+	mtk_w32(eth, lro_ctrl_dw0, MTK_PDMA_LRO_CTRL_DW0);
+
+	return 0;
+}
+
+static void mtk_hwlro_rx_uninit(struct mtk_eth *eth)
+{
+	int i;
+	u32 val;
+
+	/* relinquish lro rings, flush aggregated packets */
+	mtk_w32(eth, MTK_LRO_RING_RELINQUISH_REQ, MTK_PDMA_LRO_CTRL_DW0);
+
+	/* wait for relinquishments done */
+	for (i = 0; i < 10; i++) {
+		val = mtk_r32(eth, MTK_PDMA_LRO_CTRL_DW0);
+		if (val & MTK_LRO_RING_RELINQUISH_DONE) {
+			msleep(20);
+			continue;
+		}
+		break;
+	}
+
+	/* invalidate lro rings */
+	for (i = 1; i < MTK_MAX_RX_RING_NUM; i++)
+		mtk_w32(eth, 0, MTK_LRO_CTRL_DW2_CFG(i));
+
+	/* disable HW LRO */
+	mtk_w32(eth, 0, MTK_PDMA_LRO_CTRL_DW0);
+}
+
+static void mtk_hwlro_val_ipaddr(struct mtk_eth *eth, int idx, __be32 ip)
+{
+	u32 reg_val;
+
+	reg_val = mtk_r32(eth, MTK_LRO_CTRL_DW2_CFG(idx));
+
+	/* invalidate the IP setting */
+	mtk_w32(eth, (reg_val & ~MTK_RING_MYIP_VLD), MTK_LRO_CTRL_DW2_CFG(idx));
+
+	mtk_w32(eth, ip, MTK_LRO_DIP_DW0_CFG(idx));
+
+	/* validate the IP setting */
+	mtk_w32(eth, (reg_val | MTK_RING_MYIP_VLD), MTK_LRO_CTRL_DW2_CFG(idx));
+}
+
+static void mtk_hwlro_inval_ipaddr(struct mtk_eth *eth, int idx)
+{
+	u32 reg_val;
+
+	reg_val = mtk_r32(eth, MTK_LRO_CTRL_DW2_CFG(idx));
+
+	/* invalidate the IP setting */
+	mtk_w32(eth, (reg_val & ~MTK_RING_MYIP_VLD), MTK_LRO_CTRL_DW2_CFG(idx));
+
+	mtk_w32(eth, 0, MTK_LRO_DIP_DW0_CFG(idx));
+}
+
+static int mtk_hwlro_get_ip_cnt(struct mtk_mac *mac)
+{
+	int cnt = 0;
+	int i;
+
+	for (i = 0; i < MTK_MAX_LRO_IP_CNT; i++) {
+		if (mac->hwlro_ip[i])
+			cnt++;
+	}
+
+	return cnt;
+}
+
+static int mtk_hwlro_add_ipaddr(struct net_device *dev,
+				struct ethtool_rxnfc *cmd)
+{
+	struct ethtool_rx_flow_spec *fsp =
+		(struct ethtool_rx_flow_spec *)&cmd->fs;
+	struct mtk_mac *mac = netdev_priv(dev);
+	struct mtk_eth *eth = mac->hw;
+	int hwlro_idx;
+
+	if ((fsp->flow_type != TCP_V4_FLOW) ||
+	    (!fsp->h_u.tcp_ip4_spec.ip4dst) ||
+	    (fsp->location > 1))
+		return -EINVAL;
+
+	mac->hwlro_ip[fsp->location] = htonl(fsp->h_u.tcp_ip4_spec.ip4dst);
+	hwlro_idx = (mac->id * MTK_MAX_LRO_IP_CNT) + fsp->location;
+
+	mac->hwlro_ip_cnt = mtk_hwlro_get_ip_cnt(mac);
+
+	mtk_hwlro_val_ipaddr(eth, hwlro_idx, mac->hwlro_ip[fsp->location]);
+
+	return 0;
+}
+
+static int mtk_hwlro_del_ipaddr(struct net_device *dev,
+				struct ethtool_rxnfc *cmd)
+{
+	struct ethtool_rx_flow_spec *fsp =
+		(struct ethtool_rx_flow_spec *)&cmd->fs;
+	struct mtk_mac *mac = netdev_priv(dev);
+	struct mtk_eth *eth = mac->hw;
+	int hwlro_idx;
+
+	if (fsp->location > 1)
+		return -EINVAL;
+
+	mac->hwlro_ip[fsp->location] = 0;
+	hwlro_idx = (mac->id * MTK_MAX_LRO_IP_CNT) + fsp->location;
+
+	mac->hwlro_ip_cnt = mtk_hwlro_get_ip_cnt(mac);
+
+	mtk_hwlro_inval_ipaddr(eth, hwlro_idx);
+
+	return 0;
+}
+
+static void mtk_hwlro_netdev_disable(struct net_device *dev)
+{
+	struct mtk_mac *mac = netdev_priv(dev);
+	struct mtk_eth *eth = mac->hw;
+	int i, hwlro_idx;
+
+	for (i = 0; i < MTK_MAX_LRO_IP_CNT; i++) {
+		mac->hwlro_ip[i] = 0;
+		hwlro_idx = (mac->id * MTK_MAX_LRO_IP_CNT) + i;
+
+		mtk_hwlro_inval_ipaddr(eth, hwlro_idx);
+	}
+
+	mac->hwlro_ip_cnt = 0;
+}
+
+static int mtk_hwlro_get_fdir_entry(struct net_device *dev,
+				    struct ethtool_rxnfc *cmd)
+{
+	struct mtk_mac *mac = netdev_priv(dev);
+	struct ethtool_rx_flow_spec *fsp =
+		(struct ethtool_rx_flow_spec *)&cmd->fs;
+
+	/* only tcp dst ipv4 is meaningful, others are meaningless */
+	fsp->flow_type = TCP_V4_FLOW;
+	fsp->h_u.tcp_ip4_spec.ip4dst = ntohl(mac->hwlro_ip[fsp->location]);
+	fsp->m_u.tcp_ip4_spec.ip4dst = 0;
+
+	fsp->h_u.tcp_ip4_spec.ip4src = 0;
+	fsp->m_u.tcp_ip4_spec.ip4src = 0xffffffff;
+	fsp->h_u.tcp_ip4_spec.psrc = 0;
+	fsp->m_u.tcp_ip4_spec.psrc = 0xffff;
+	fsp->h_u.tcp_ip4_spec.pdst = 0;
+	fsp->m_u.tcp_ip4_spec.pdst = 0xffff;
+	fsp->h_u.tcp_ip4_spec.tos = 0;
+	fsp->m_u.tcp_ip4_spec.tos = 0xff;
+
+	return 0;
+}
+
+static int mtk_hwlro_get_fdir_all(struct net_device *dev,
+				  struct ethtool_rxnfc *cmd,
+				  u32 *rule_locs)
+{
+	struct mtk_mac *mac = netdev_priv(dev);
+	int cnt = 0;
+	int i;
+
+	for (i = 0; i < MTK_MAX_LRO_IP_CNT; i++) {
+		if (mac->hwlro_ip[i]) {
+			rule_locs[cnt] = i;
+			cnt++;
+		}
+	}
+
+	cmd->rule_cnt = cnt;
+
+	return 0;
+}
+
+static netdev_features_t mtk_fix_features(struct net_device *dev,
+					  netdev_features_t features)
+{
+	if (!(features & NETIF_F_LRO)) {
+		struct mtk_mac *mac = netdev_priv(dev);
+		int ip_cnt = mtk_hwlro_get_ip_cnt(mac);
+
+		if (ip_cnt) {
+			netdev_info(dev, "RX flow is programmed, LRO should keep on\n");
+
+			features |= NETIF_F_LRO;
+		}
+	}
+
+	return features;
+}
+
+static int mtk_set_features(struct net_device *dev, netdev_features_t features)
+{
+	int err = 0;
+
+	if (!((dev->features ^ features) & NETIF_F_LRO))
+		return 0;
+
+	if (!(features & NETIF_F_LRO))
+		mtk_hwlro_netdev_disable(dev);
+
+	return err;
+}
+
 /* wait for DMA to finish whatever it is doing before we start using it again */
 static int mtk_dma_busy_wait(struct mtk_eth *eth)
 {
@@ -1218,6 +1581,7 @@ static int mtk_dma_busy_wait(struct mtk_eth *eth)
 static int mtk_dma_init(struct mtk_eth *eth)
 {
 	int err;
+	u32 i;
 
 	if (mtk_dma_busy_wait(eth))
 		return -EBUSY;
@@ -1233,10 +1597,21 @@ static int mtk_dma_init(struct mtk_eth *eth)
 	if (err)
 		return err;
 
-	err = mtk_rx_alloc(eth);
+	err = mtk_rx_alloc(eth, 0, MTK_RX_FLAGS_NORMAL);
 	if (err)
 		return err;
 
+	if (eth->hwlro) {
+		for (i = 1; i < MTK_MAX_RX_RING_NUM; i++) {
+			err = mtk_rx_alloc(eth, i, MTK_RX_FLAGS_HWLRO);
+			if (err)
+				return err;
+		}
+		err = mtk_hwlro_rx_init(eth);
+		if (err)
+			return err;
+	}
+
 	/* Enable random early drop and set drop threshold automatically */
 	mtk_w32(eth, FC_THRES_DROP_MODE | FC_THRES_DROP_EN | FC_THRES_MIN,
 		MTK_QDMA_FC_THRES);
@@ -1261,7 +1636,14 @@ static void mtk_dma_free(struct mtk_eth *eth)
 		eth->phy_scratch_ring = 0;
 	}
 	mtk_tx_clean(eth);
-	mtk_rx_clean(eth);
+	mtk_rx_clean(eth, 0);
+
+	if (eth->hwlro) {
+		mtk_hwlro_rx_uninit(eth);
+		for (i = 1; i < MTK_MAX_RX_RING_NUM; i++)
+			mtk_rx_clean(eth, i);
+	}
+
 	kfree(eth->scratch_head);
 }
 
@@ -1282,7 +1664,7 @@ static irqreturn_t mtk_handle_irq_rx(int irq, void *_eth)
 
 	if (likely(napi_schedule_prep(&eth->rx_napi))) {
 		__napi_schedule(&eth->rx_napi);
-		mtk_irq_disable(eth, MTK_RX_DONE_INT);
+		mtk_irq_disable(eth, MTK_PDMA_INT_MASK, MTK_RX_DONE_INT);
 	}
 
 	return IRQ_HANDLED;
@@ -1294,7 +1676,7 @@ static irqreturn_t mtk_handle_irq_tx(int irq, void *_eth)
 
 	if (likely(napi_schedule_prep(&eth->tx_napi))) {
 		__napi_schedule(&eth->tx_napi);
-		mtk_irq_disable(eth, MTK_TX_DONE_INT);
+		mtk_irq_disable(eth, MTK_QDMA_INT_MASK, MTK_TX_DONE_INT);
 	}
 
 	return IRQ_HANDLED;
@@ -1305,11 +1687,12 @@ static void mtk_poll_controller(struct net_device *dev)
 {
 	struct mtk_mac *mac = netdev_priv(dev);
 	struct mtk_eth *eth = mac->hw;
-	u32 int_mask = MTK_TX_DONE_INT | MTK_RX_DONE_INT;
 
-	mtk_irq_disable(eth, int_mask);
+	mtk_irq_disable(eth, MTK_QDMA_INT_MASK, MTK_TX_DONE_INT);
+	mtk_irq_disable(eth, MTK_PDMA_INT_MASK, MTK_RX_DONE_INT);
 	mtk_handle_irq_rx(eth->irq[2], dev);
-	mtk_irq_enable(eth, int_mask);
+	mtk_irq_enable(eth, MTK_QDMA_INT_MASK, MTK_TX_DONE_INT);
+	mtk_irq_enable(eth, MTK_PDMA_INT_MASK, MTK_RX_DONE_INT);
 }
 #endif
 
@@ -1324,11 +1707,15 @@ static int mtk_start_dma(struct mtk_eth *eth)
 	}
 
 	mtk_w32(eth,
-		MTK_TX_WB_DDONE | MTK_RX_DMA_EN | MTK_TX_DMA_EN |
-		MTK_RX_2B_OFFSET | MTK_DMA_SIZE_16DWORDS |
-		MTK_RX_BT_32DWORDS | MTK_NDP_CO_PRO,
+		MTK_TX_WB_DDONE | MTK_TX_DMA_EN |
+		MTK_DMA_SIZE_16DWORDS | MTK_NDP_CO_PRO,
 		MTK_QDMA_GLO_CFG);
 
+	mtk_w32(eth,
+		MTK_RX_DMA_EN | MTK_RX_2B_OFFSET |
+		MTK_RX_BT_32DWORDS | MTK_MULTI_EN,
+		MTK_PDMA_GLO_CFG);
+
 	return 0;
 }
 
@@ -1346,11 +1733,12 @@ static int mtk_open(struct net_device *dev)
 
 		napi_enable(&eth->tx_napi);
 		napi_enable(&eth->rx_napi);
-		mtk_irq_enable(eth, MTK_TX_DONE_INT | MTK_RX_DONE_INT);
+		mtk_irq_enable(eth, MTK_QDMA_INT_MASK, MTK_TX_DONE_INT);
+		mtk_irq_enable(eth, MTK_PDMA_INT_MASK, MTK_RX_DONE_INT);
 	}
 	atomic_inc(&eth->dma_refcnt);
 
-	phy_start(mac->phy_dev);
+	phy_start(dev->phydev);
 	netif_start_queue(dev);
 
 	return 0;
@@ -1358,16 +1746,15 @@ static int mtk_open(struct net_device *dev)
 
 static void mtk_stop_dma(struct mtk_eth *eth, u32 glo_cfg)
 {
-	unsigned long flags;
 	u32 val;
 	int i;
 
 	/* stop the dma engine */
-	spin_lock_irqsave(&eth->page_lock, flags);
+	spin_lock_bh(&eth->page_lock);
 	val = mtk_r32(eth, glo_cfg);
 	mtk_w32(eth, val & ~(MTK_TX_WB_DDONE | MTK_RX_DMA_EN | MTK_TX_DMA_EN),
 		glo_cfg);
-	spin_unlock_irqrestore(&eth->page_lock, flags);
+	spin_unlock_bh(&eth->page_lock);
 
 	/* wait for dma stop */
 	for (i = 0; i < 10; i++) {
@@ -1386,32 +1773,63 @@ static int mtk_stop(struct net_device *dev)
 	struct mtk_eth *eth = mac->hw;
 
 	netif_tx_disable(dev);
-	phy_stop(mac->phy_dev);
+	phy_stop(dev->phydev);
 
 	/* only shutdown DMA if this is the last user */
 	if (!atomic_dec_and_test(&eth->dma_refcnt))
 		return 0;
 
-	mtk_irq_disable(eth, MTK_TX_DONE_INT | MTK_RX_DONE_INT);
+	mtk_irq_disable(eth, MTK_QDMA_INT_MASK, MTK_TX_DONE_INT);
+	mtk_irq_disable(eth, MTK_PDMA_INT_MASK, MTK_RX_DONE_INT);
 	napi_disable(&eth->tx_napi);
 	napi_disable(&eth->rx_napi);
 
 	mtk_stop_dma(eth, MTK_QDMA_GLO_CFG);
+	mtk_stop_dma(eth, MTK_PDMA_GLO_CFG);
 
 	mtk_dma_free(eth);
 
 	return 0;
 }
 
-static int __init mtk_hw_init(struct mtk_eth *eth)
+static void ethsys_reset(struct mtk_eth *eth, u32 reset_bits)
 {
-	int err, i;
+	regmap_update_bits(eth->ethsys, ETHSYS_RSTCTRL,
+			   reset_bits,
+			   reset_bits);
+
+	usleep_range(1000, 1100);
+	regmap_update_bits(eth->ethsys, ETHSYS_RSTCTRL,
+			   reset_bits,
+			   ~reset_bits);
+	mdelay(10);
+}
+
+static int mtk_hw_init(struct mtk_eth *eth)
+{
+	int i, val;
+
+	if (test_and_set_bit(MTK_HW_INIT, &eth->state))
+		return 0;
+
+	pm_runtime_enable(eth->dev);
+	pm_runtime_get_sync(eth->dev);
+
+	clk_prepare_enable(eth->clks[MTK_CLK_ETHIF]);
+	clk_prepare_enable(eth->clks[MTK_CLK_ESW]);
+	clk_prepare_enable(eth->clks[MTK_CLK_GP1]);
+	clk_prepare_enable(eth->clks[MTK_CLK_GP2]);
+	ethsys_reset(eth, RSTCTRL_FE);
+	ethsys_reset(eth, RSTCTRL_PPE);
 
-	/* reset the frame engine */
-	reset_control_assert(eth->rstc);
-	usleep_range(10, 20);
-	reset_control_deassert(eth->rstc);
-	usleep_range(10, 20);
+	regmap_read(eth->ethsys, ETHSYS_SYSCFG0, &val);
+	for (i = 0; i < MTK_MAC_COUNT; i++) {
+		if (!eth->mac[i])
+			continue;
+		val &= ~SYSCFG0_GE_MODE(SYSCFG0_GE_MASK, eth->mac[i]->id);
+		val |= SYSCFG0_GE_MODE(eth->mac[i]->ge_mode, eth->mac[i]->id);
+	}
+	regmap_write(eth->ethsys, ETHSYS_SYSCFG0, val);
 
 	/* Set GE2 driving and slew rate */
 	regmap_write(eth->pctl, GPIO_DRV_SEL10, 0xa00);
@@ -1431,22 +1849,11 @@ static int __init mtk_hw_init(struct mtk_eth *eth)
 	/* Enable RX VLan Offloading */
 	mtk_w32(eth, 1, MTK_CDMP_EG_CTRL);
 
-	err = devm_request_irq(eth->dev, eth->irq[1], mtk_handle_irq_tx, 0,
-			       dev_name(eth->dev), eth);
-	if (err)
-		return err;
-	err = devm_request_irq(eth->dev, eth->irq[2], mtk_handle_irq_rx, 0,
-			       dev_name(eth->dev), eth);
-	if (err)
-		return err;
-
-	err = mtk_mdio_init(eth);
-	if (err)
-		return err;
-
 	/* disable delay and normal interrupt */
 	mtk_w32(eth, 0, MTK_QDMA_DELAY_INT);
-	mtk_irq_disable(eth, ~0);
+	mtk_w32(eth, 0, MTK_PDMA_DELAY_INT);
+	mtk_irq_disable(eth, MTK_QDMA_INT_MASK, ~0);
+	mtk_irq_disable(eth, MTK_PDMA_INT_MASK, ~0);
 	mtk_w32(eth, RST_GL_PSE, MTK_RST_GL);
 	mtk_w32(eth, 0, MTK_RST_GL);
 
@@ -1460,9 +1867,8 @@ static int __init mtk_hw_init(struct mtk_eth *eth)
 	for (i = 0; i < 2; i++) {
 		u32 val = mtk_r32(eth, MTK_GDMA_FWD_CFG(i));
 
-		/* setup the forward port to send frame to QDMA */
+		/* setup the forward port to send frame to PDMA */
 		val &= ~0xffff;
-		val |= 0x5555;
 
 		/* Enable RX checksum */
 		val |= MTK_GDMA_ICS_EN | MTK_GDMA_TCS_EN | MTK_GDMA_UCS_EN;
@@ -1474,6 +1880,22 @@ static int __init mtk_hw_init(struct mtk_eth *eth)
 	return 0;
 }
 
+static int mtk_hw_deinit(struct mtk_eth *eth)
+{
+	if (!test_and_clear_bit(MTK_HW_INIT, &eth->state))
+		return 0;
+
+	clk_disable_unprepare(eth->clks[MTK_CLK_GP2]);
+	clk_disable_unprepare(eth->clks[MTK_CLK_GP1]);
+	clk_disable_unprepare(eth->clks[MTK_CLK_ESW]);
+	clk_disable_unprepare(eth->clks[MTK_CLK_ETHIF]);
+
+	pm_runtime_put_sync(eth->dev);
+	pm_runtime_disable(eth->dev);
+
+	return 0;
+}
+
 static int __init mtk_init(struct net_device *dev)
 {
 	struct mtk_mac *mac = netdev_priv(dev);
@@ -1492,7 +1914,7 @@ static int __init mtk_init(struct net_device *dev)
 		dev->addr_assign_type = NET_ADDR_RANDOM;
 	}
 
-	return mtk_phy_connect(mac);
+	return mtk_phy_connect(dev);
 }
 
 static void mtk_uninit(struct net_device *dev)
@@ -1500,19 +1922,18 @@ static void mtk_uninit(struct net_device *dev)
 	struct mtk_mac *mac = netdev_priv(dev);
 	struct mtk_eth *eth = mac->hw;
 
-	phy_disconnect(mac->phy_dev);
-	mtk_irq_disable(eth, ~0);
+	phy_disconnect(dev->phydev);
+	mtk_irq_disable(eth, MTK_QDMA_INT_MASK, ~0);
+	mtk_irq_disable(eth, MTK_PDMA_INT_MASK, ~0);
 }
 
 static int mtk_do_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
 {
-	struct mtk_mac *mac = netdev_priv(dev);
-
 	switch (cmd) {
 	case SIOCGMIIPHY:
 	case SIOCGMIIREG:
 	case SIOCSMIIREG:
-		return phy_mii_ioctl(mac->phy_dev, ifr, cmd);
+		return phy_mii_ioctl(dev->phydev, ifr, cmd);
 	default:
 		break;
 	}
@@ -1528,6 +1949,12 @@ static void mtk_pending_work(struct work_struct *work)
 
 	rtnl_lock();
 
+	dev_dbg(eth->dev, "[%s][%d] reset\n", __func__, __LINE__);
+
+	while (test_and_set_bit_lock(MTK_RESETTING, &eth->state))
+		cpu_relax();
+
+	dev_dbg(eth->dev, "[%s][%d] mtk_stop starts\n", __func__, __LINE__);
 	/* stop all devices to make sure that dma is properly shut down */
 	for (i = 0; i < MTK_MAC_COUNT; i++) {
 		if (!eth->netdev[i])
@@ -1535,6 +1962,27 @@ static void mtk_pending_work(struct work_struct *work)
 		mtk_stop(eth->netdev[i]);
 		__set_bit(i, &restart);
 	}
+	dev_dbg(eth->dev, "[%s][%d] mtk_stop ends\n", __func__, __LINE__);
+
+	/* restart underlying hardware such as power, clock, pin mux
+	 * and the connected phy
+	 */
+	mtk_hw_deinit(eth);
+
+	if (eth->dev->pins)
+		pinctrl_select_state(eth->dev->pins->p,
+				     eth->dev->pins->default_state);
+	mtk_hw_init(eth);
+
+	for (i = 0; i < MTK_MAC_COUNT; i++) {
+		if (!eth->mac[i] ||
+		    of_phy_is_fixed_link(eth->mac[i]->of_node))
+			continue;
+		err = phy_init_hw(eth->netdev[i]->phydev);
+		if (err)
+			dev_err(eth->dev, "%s: PHY init failed.\n",
+				eth->netdev[i]->name);
+	}
 
 	/* restart DMA and enable IRQs */
 	for (i = 0; i < MTK_MAC_COUNT; i++) {
@@ -1547,51 +1995,69 @@ static void mtk_pending_work(struct work_struct *work)
 			dev_close(eth->netdev[i]);
 		}
 	}
+
+	dev_dbg(eth->dev, "[%s][%d] reset done\n", __func__, __LINE__);
+
+	clear_bit_unlock(MTK_RESETTING, &eth->state);
+
 	rtnl_unlock();
 }
 
-static int mtk_cleanup(struct mtk_eth *eth)
+static int mtk_free_dev(struct mtk_eth *eth)
 {
 	int i;
 
 	for (i = 0; i < MTK_MAC_COUNT; i++) {
 		if (!eth->netdev[i])
 			continue;
+		free_netdev(eth->netdev[i]);
+	}
+
+	return 0;
+}
+
+static int mtk_unreg_dev(struct mtk_eth *eth)
+{
+	int i;
 
+	for (i = 0; i < MTK_MAC_COUNT; i++) {
+		if (!eth->netdev[i])
+			continue;
 		unregister_netdev(eth->netdev[i]);
-		free_netdev(eth->netdev[i]);
 	}
+
+	return 0;
+}
+
+static int mtk_cleanup(struct mtk_eth *eth)
+{
+	mtk_unreg_dev(eth);
+	mtk_free_dev(eth);
 	cancel_work_sync(&eth->pending_work);
 
 	return 0;
 }
 
-static int mtk_get_settings(struct net_device *dev,
-			    struct ethtool_cmd *cmd)
+static int mtk_get_link_ksettings(struct net_device *ndev,
+				  struct ethtool_link_ksettings *cmd)
 {
-	struct mtk_mac *mac = netdev_priv(dev);
-	int err;
+	struct mtk_mac *mac = netdev_priv(ndev);
 
-	err = phy_read_status(mac->phy_dev);
-	if (err)
-		return -ENODEV;
+	if (unlikely(test_bit(MTK_RESETTING, &mac->hw->state)))
+		return -EBUSY;
 
-	return phy_ethtool_gset(mac->phy_dev, cmd);
+	return phy_ethtool_ksettings_get(ndev->phydev, cmd);
 }
 
-static int mtk_set_settings(struct net_device *dev,
-			    struct ethtool_cmd *cmd)
+static int mtk_set_link_ksettings(struct net_device *ndev,
+				  const struct ethtool_link_ksettings *cmd)
 {
-	struct mtk_mac *mac = netdev_priv(dev);
+	struct mtk_mac *mac = netdev_priv(ndev);
 
-	if (cmd->phy_address != mac->phy_dev->mdio.addr) {
-		mac->phy_dev = mdiobus_get_phy(mac->hw->mii_bus,
-					       cmd->phy_address);
-		if (!mac->phy_dev)
-			return -ENODEV;
-	}
+	if (unlikely(test_bit(MTK_RESETTING, &mac->hw->state)))
+		return -EBUSY;
 
-	return phy_ethtool_sset(mac->phy_dev, cmd);
+	return phy_ethtool_ksettings_set(ndev->phydev, cmd);
 }
 
 static void mtk_get_drvinfo(struct net_device *dev,
@@ -1622,7 +2088,10 @@ static int mtk_nway_reset(struct net_device *dev)
 {
 	struct mtk_mac *mac = netdev_priv(dev);
 
-	return genphy_restart_aneg(mac->phy_dev);
+	if (unlikely(test_bit(MTK_RESETTING, &mac->hw->state)))
+		return -EBUSY;
+
+	return genphy_restart_aneg(dev->phydev);
 }
 
 static u32 mtk_get_link(struct net_device *dev)
@@ -1630,11 +2099,14 @@ static u32 mtk_get_link(struct net_device *dev)
 	struct mtk_mac *mac = netdev_priv(dev);
 	int err;
 
-	err = genphy_update_link(mac->phy_dev);
+	if (unlikely(test_bit(MTK_RESETTING, &mac->hw->state)))
+		return -EBUSY;
+
+	err = genphy_update_link(dev->phydev);
 	if (err)
 		return ethtool_op_get_link(dev);
 
-	return mac->phy_dev->link;
+	return dev->phydev->link;
 }
 
 static void mtk_get_strings(struct net_device *dev, u32 stringset, u8 *data)
@@ -1670,6 +2142,9 @@ static void mtk_get_ethtool_stats(struct net_device *dev,
 	unsigned int start;
 	int i;
 
+	if (unlikely(test_bit(MTK_RESETTING, &mac->hw->state)))
+		return;
+
 	if (netif_running(dev) && netif_device_present(dev)) {
 		if (spin_trylock(&hwstats->stats_lock)) {
 			mtk_stats_update_mac(mac);
@@ -1677,8 +2152,9 @@ static void mtk_get_ethtool_stats(struct net_device *dev,
 		}
 	}
 
+	data_src = (u64 *)hwstats;
+
 	do {
-		data_src = (u64*)hwstats;
 		data_dst = data;
 		start = u64_stats_fetch_begin_irq(&hwstats->syncp);
 
@@ -1687,9 +2163,65 @@ static void mtk_get_ethtool_stats(struct net_device *dev,
 	} while (u64_stats_fetch_retry_irq(&hwstats->syncp, start));
 }
 
-static struct ethtool_ops mtk_ethtool_ops = {
-	.get_settings		= mtk_get_settings,
-	.set_settings		= mtk_set_settings,
+static int mtk_get_rxnfc(struct net_device *dev, struct ethtool_rxnfc *cmd,
+			 u32 *rule_locs)
+{
+	int ret = -EOPNOTSUPP;
+
+	switch (cmd->cmd) {
+	case ETHTOOL_GRXRINGS:
+		if (dev->features & NETIF_F_LRO) {
+			cmd->data = MTK_MAX_RX_RING_NUM;
+			ret = 0;
+		}
+		break;
+	case ETHTOOL_GRXCLSRLCNT:
+		if (dev->features & NETIF_F_LRO) {
+			struct mtk_mac *mac = netdev_priv(dev);
+
+			cmd->rule_cnt = mac->hwlro_ip_cnt;
+			ret = 0;
+		}
+		break;
+	case ETHTOOL_GRXCLSRULE:
+		if (dev->features & NETIF_F_LRO)
+			ret = mtk_hwlro_get_fdir_entry(dev, cmd);
+		break;
+	case ETHTOOL_GRXCLSRLALL:
+		if (dev->features & NETIF_F_LRO)
+			ret = mtk_hwlro_get_fdir_all(dev, cmd,
+						     rule_locs);
+		break;
+	default:
+		break;
+	}
+
+	return ret;
+}
+
+static int mtk_set_rxnfc(struct net_device *dev, struct ethtool_rxnfc *cmd)
+{
+	int ret = -EOPNOTSUPP;
+
+	switch (cmd->cmd) {
+	case ETHTOOL_SRXCLSRLINS:
+		if (dev->features & NETIF_F_LRO)
+			ret = mtk_hwlro_add_ipaddr(dev, cmd);
+		break;
+	case ETHTOOL_SRXCLSRLDEL:
+		if (dev->features & NETIF_F_LRO)
+			ret = mtk_hwlro_del_ipaddr(dev, cmd);
+		break;
+	default:
+		break;
+	}
+
+	return ret;
+}
+
+static const struct ethtool_ops mtk_ethtool_ops = {
+	.get_link_ksettings	= mtk_get_link_ksettings,
+	.set_link_ksettings	= mtk_set_link_ksettings,
 	.get_drvinfo		= mtk_get_drvinfo,
 	.get_msglevel		= mtk_get_msglevel,
 	.set_msglevel		= mtk_set_msglevel,
@@ -1698,6 +2230,8 @@ static struct ethtool_ops mtk_ethtool_ops = {
 	.get_strings		= mtk_get_strings,
 	.get_sset_count		= mtk_get_sset_count,
 	.get_ethtool_stats	= mtk_get_ethtool_stats,
+	.get_rxnfc		= mtk_get_rxnfc,
+	.set_rxnfc              = mtk_set_rxnfc,
 };
 
 static const struct net_device_ops mtk_netdev_ops = {
@@ -1712,6 +2246,8 @@ static const struct net_device_ops mtk_netdev_ops = {
 	.ndo_change_mtu		= eth_change_mtu,
 	.ndo_tx_timeout		= mtk_tx_timeout,
 	.ndo_get_stats64        = mtk_get_stats64,
+	.ndo_fix_features	= mtk_fix_features,
+	.ndo_set_features	= mtk_set_features,
 #ifdef CONFIG_NET_POLL_CONTROLLER
 	.ndo_poll_controller	= mtk_poll_controller,
 #endif
@@ -1750,6 +2286,9 @@ static int mtk_add_mac(struct mtk_eth *eth, struct device_node *np)
 	mac->hw = eth;
 	mac->of_node = np;
 
+	memset(mac->hwlro_ip, 0, sizeof(mac->hwlro_ip));
+	mac->hwlro_ip_cnt = 0;
+
 	mac->hw_stats = devm_kzalloc(eth->dev,
 				     sizeof(*mac->hw_stats),
 				     GFP_KERNEL);
@@ -1766,21 +2305,17 @@ static int mtk_add_mac(struct mtk_eth *eth, struct device_node *np)
 	eth->netdev[id]->watchdog_timeo = 5 * HZ;
 	eth->netdev[id]->netdev_ops = &mtk_netdev_ops;
 	eth->netdev[id]->base_addr = (unsigned long)eth->base;
+
+	eth->netdev[id]->hw_features = MTK_HW_FEATURES;
+	if (eth->hwlro)
+		eth->netdev[id]->hw_features |= NETIF_F_LRO;
+
 	eth->netdev[id]->vlan_features = MTK_HW_FEATURES &
 		~(NETIF_F_HW_VLAN_CTAG_TX | NETIF_F_HW_VLAN_CTAG_RX);
 	eth->netdev[id]->features |= MTK_HW_FEATURES;
 	eth->netdev[id]->ethtool_ops = &mtk_ethtool_ops;
 
-	err = register_netdev(eth->netdev[id]);
-	if (err) {
-		dev_err(eth->dev, "error bringing up device\n");
-		goto free_netdev;
-	}
 	eth->netdev[id]->irq = eth->irq[0];
-	netif_info(eth, probe, eth->netdev[id],
-		   "mediatek frame engine at 0x%08lx, irq %d\n",
-		   eth->netdev[id]->base_addr, eth->irq[0]);
-
 	return 0;
 
 free_netdev:
@@ -1788,6 +2323,41 @@ free_netdev:
 	return err;
 }
 
+static int mtk_get_chip_id(struct mtk_eth *eth, u32 *chip_id)
+{
+	u32 val[2], id[4];
+
+	regmap_read(eth->ethsys, ETHSYS_CHIPID0_3, &val[0]);
+	regmap_read(eth->ethsys, ETHSYS_CHIPID4_7, &val[1]);
+
+	id[3] = ((val[0] >> 16) & 0xff) - '0';
+	id[2] = ((val[0] >> 24) & 0xff) - '0';
+	id[1] = (val[1] & 0xff) - '0';
+	id[0] = ((val[1] >> 8) & 0xff) - '0';
+
+	*chip_id = (id[3] * 1000) + (id[2] * 100) +
+		   (id[1] * 10) + id[0];
+
+	if (!(*chip_id)) {
+		dev_err(eth->dev, "failed to get chip id\n");
+		return -ENODEV;
+	}
+
+	dev_info(eth->dev, "chip id = %d\n", *chip_id);
+
+	return 0;
+}
+
+static bool mtk_is_hwlro_supported(struct mtk_eth *eth)
+{
+	switch (eth->chip_id) {
+	case MT7623_ETH:
+		return true;
+	}
+
+	return false;
+}
+
 static int mtk_probe(struct platform_device *pdev)
 {
 	struct resource *res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
@@ -1827,12 +2397,6 @@ static int mtk_probe(struct platform_device *pdev)
 		return PTR_ERR(eth->pctl);
 	}
 
-	eth->rstc = devm_reset_control_get(&pdev->dev, "eth");
-	if (IS_ERR(eth->rstc)) {
-		dev_err(&pdev->dev, "no eth reset found\n");
-		return PTR_ERR(eth->rstc);
-	}
-
 	for (i = 0; i < 3; i++) {
 		eth->irq[i] = platform_get_irq(pdev, i);
 		if (eth->irq[i] < 0) {
@@ -1850,11 +2414,6 @@ static int mtk_probe(struct platform_device *pdev)
 		}
 	}
 
-	clk_prepare_enable(eth->clks[MTK_CLK_ETHIF]);
-	clk_prepare_enable(eth->clks[MTK_CLK_ESW]);
-	clk_prepare_enable(eth->clks[MTK_CLK_GP1]);
-	clk_prepare_enable(eth->clks[MTK_CLK_GP2]);
-
 	eth->msg_enable = netif_msg_init(mtk_msg_level, MTK_DEFAULT_MSG_ENABLE);
 	INIT_WORK(&eth->pending_work, mtk_pending_work);
 
@@ -1862,6 +2421,12 @@ static int mtk_probe(struct platform_device *pdev)
 	if (err)
 		return err;
 
+	err = mtk_get_chip_id(eth, &eth->chip_id);
+	if (err)
+		return err;
+
+	eth->hwlro = mtk_is_hwlro_supported(eth);
+
 	for_each_child_of_node(pdev->dev.of_node, mac_np) {
 		if (!of_device_is_compatible(mac_np,
 					     "mediatek,eth-mac"))
@@ -1872,7 +2437,35 @@ static int mtk_probe(struct platform_device *pdev)
 
 		err = mtk_add_mac(eth, mac_np);
 		if (err)
-			goto err_free_dev;
+			goto err_deinit_hw;
+	}
+
+	err = devm_request_irq(eth->dev, eth->irq[1], mtk_handle_irq_tx, 0,
+			       dev_name(eth->dev), eth);
+	if (err)
+		goto err_free_dev;
+
+	err = devm_request_irq(eth->dev, eth->irq[2], mtk_handle_irq_rx, 0,
+			       dev_name(eth->dev), eth);
+	if (err)
+		goto err_free_dev;
+
+	err = mtk_mdio_init(eth);
+	if (err)
+		goto err_free_dev;
+
+	for (i = 0; i < MTK_MAX_DEVS; i++) {
+		if (!eth->netdev[i])
+			continue;
+
+		err = register_netdev(eth->netdev[i]);
+		if (err) {
+			dev_err(eth->dev, "error bringing up device\n");
+			goto err_deinit_mdio;
+		} else
+			netif_info(eth, probe, eth->netdev[i],
+				   "mediatek frame engine at 0x%08lx, irq %d\n",
+				   eth->netdev[i]->base_addr, eth->irq[0]);
 	}
 
 	/* we run 2 devices on the same DMA ring so we need a dummy device
@@ -1888,8 +2481,13 @@ static int mtk_probe(struct platform_device *pdev)
 
 	return 0;
 
+err_deinit_mdio:
+	mtk_mdio_cleanup(eth);
 err_free_dev:
-	mtk_cleanup(eth);
+	mtk_free_dev(eth);
+err_deinit_hw:
+	mtk_hw_deinit(eth);
+
 	return err;
 }
 
@@ -1905,16 +2503,12 @@ static int mtk_remove(struct platform_device *pdev)
 		mtk_stop(eth->netdev[i]);
 	}
 
-	clk_disable_unprepare(eth->clks[MTK_CLK_ETHIF]);
-	clk_disable_unprepare(eth->clks[MTK_CLK_ESW]);
-	clk_disable_unprepare(eth->clks[MTK_CLK_GP1]);
-	clk_disable_unprepare(eth->clks[MTK_CLK_GP2]);
+	mtk_hw_deinit(eth);
 
 	netif_napi_del(&eth->tx_napi);
 	netif_napi_del(&eth->rx_napi);
 	mtk_cleanup(eth);
 	mtk_mdio_cleanup(eth);
-	platform_set_drvdata(pdev, NULL);
 
 	return 0;
 }
diff --git a/drivers/net/ethernet/mediatek/mtk_eth_soc.h b/drivers/net/ethernet/mediatek/mtk_eth_soc.h
index 6e1ade7a25c5..99b1c8e9f16f 100644
--- a/drivers/net/ethernet/mediatek/mtk_eth_soc.h
+++ b/drivers/net/ethernet/mediatek/mtk_eth_soc.h
@@ -39,7 +39,21 @@
 				 NETIF_F_SG | NETIF_F_TSO | \
 				 NETIF_F_TSO6 | \
 				 NETIF_F_IPV6_CSUM)
-#define NEXT_RX_DESP_IDX(X)	(((X) + 1) & (MTK_DMA_SIZE - 1))
+#define NEXT_RX_DESP_IDX(X, Y)	(((X) + 1) & ((Y) - 1))
+
+#define MTK_MAX_RX_RING_NUM	4
+#define MTK_HW_LRO_DMA_SIZE	8
+
+#define	MTK_MAX_LRO_RX_LENGTH		(4096 * 3)
+#define	MTK_MAX_LRO_IP_CNT		2
+#define	MTK_HW_LRO_TIMER_UNIT		1	/* 20 us */
+#define	MTK_HW_LRO_REFRESH_TIME		50000	/* 1 sec. */
+#define	MTK_HW_LRO_AGG_TIME		10	/* 200us */
+#define	MTK_HW_LRO_AGE_TIME		50	/* 1ms */
+#define	MTK_HW_LRO_MAX_AGG_CNT		64
+#define	MTK_HW_LRO_BW_THRE		3000
+#define	MTK_HW_LRO_REPLACE_DELTA	1000
+#define	MTK_HW_LRO_SDL_REMAIN_ROOM	1522
 
 /* Frame Engine Global Reset Register */
 #define MTK_RST_GL		0x04
@@ -50,6 +64,9 @@
 #define MTK_GDM1_AF		BIT(28)
 #define MTK_GDM2_AF		BIT(29)
 
+/* PDMA HW LRO Alter Flow Timer Register */
+#define MTK_PDMA_LRO_ALT_REFRESH_TIMER	0x1c
+
 /* Frame Engine Interrupt Grouping Register */
 #define MTK_FE_INT_GRP		0x20
 
@@ -68,10 +85,77 @@
 /* Unicast Filter MAC Address Register - High */
 #define MTK_GDMA_MAC_ADRH(x)	(0x50C + (x * 0x1000))
 
+/* PDMA RX Base Pointer Register */
+#define MTK_PRX_BASE_PTR0	0x900
+#define MTK_PRX_BASE_PTR_CFG(x)	(MTK_PRX_BASE_PTR0 + (x * 0x10))
+
+/* PDMA RX Maximum Count Register */
+#define MTK_PRX_MAX_CNT0	0x904
+#define MTK_PRX_MAX_CNT_CFG(x)	(MTK_PRX_MAX_CNT0 + (x * 0x10))
+
+/* PDMA RX CPU Pointer Register */
+#define MTK_PRX_CRX_IDX0	0x908
+#define MTK_PRX_CRX_IDX_CFG(x)	(MTK_PRX_CRX_IDX0 + (x * 0x10))
+
+/* PDMA HW LRO Control Registers */
+#define MTK_PDMA_LRO_CTRL_DW0	0x980
+#define MTK_LRO_EN			BIT(0)
+#define MTK_L3_CKS_UPD_EN		BIT(7)
+#define MTK_LRO_ALT_PKT_CNT_MODE	BIT(21)
+#define MTK_LRO_RING_RELINQUISH_REQ	(0x7 << 26)
+#define MTK_LRO_RING_RELINQUISH_DONE	(0x7 << 29)
+
+#define MTK_PDMA_LRO_CTRL_DW1	0x984
+#define MTK_PDMA_LRO_CTRL_DW2	0x988
+#define MTK_PDMA_LRO_CTRL_DW3	0x98c
+#define MTK_ADMA_MODE		BIT(15)
+#define MTK_LRO_MIN_RXD_SDL	(MTK_HW_LRO_SDL_REMAIN_ROOM << 16)
+
+/* PDMA Global Configuration Register */
+#define MTK_PDMA_GLO_CFG	0xa04
+#define MTK_MULTI_EN		BIT(10)
+
+/* PDMA Reset Index Register */
+#define MTK_PDMA_RST_IDX	0xa08
+#define MTK_PST_DRX_IDX0	BIT(16)
+#define MTK_PST_DRX_IDX_CFG(x)	(MTK_PST_DRX_IDX0 << (x))
+
+/* PDMA Delay Interrupt Register */
+#define MTK_PDMA_DELAY_INT	0xa0c
+
+/* PDMA Interrupt Status Register */
+#define MTK_PDMA_INT_STATUS	0xa20
+
+/* PDMA Interrupt Mask Register */
+#define MTK_PDMA_INT_MASK	0xa28
+
+/* PDMA HW LRO Alter Flow Delta Register */
+#define MTK_PDMA_LRO_ALT_SCORE_DELTA	0xa4c
+
 /* PDMA Interrupt grouping registers */
 #define MTK_PDMA_INT_GRP1	0xa50
 #define MTK_PDMA_INT_GRP2	0xa54
 
+/* PDMA HW LRO IP Setting Registers */
+#define MTK_LRO_RX_RING0_DIP_DW0	0xb04
+#define MTK_LRO_DIP_DW0_CFG(x)		(MTK_LRO_RX_RING0_DIP_DW0 + (x * 0x40))
+#define MTK_RING_MYIP_VLD		BIT(9)
+
+/* PDMA HW LRO Ring Control Registers */
+#define MTK_LRO_RX_RING0_CTRL_DW1	0xb28
+#define MTK_LRO_RX_RING0_CTRL_DW2	0xb2c
+#define MTK_LRO_RX_RING0_CTRL_DW3	0xb30
+#define MTK_LRO_CTRL_DW1_CFG(x)		(MTK_LRO_RX_RING0_CTRL_DW1 + (x * 0x40))
+#define MTK_LRO_CTRL_DW2_CFG(x)		(MTK_LRO_RX_RING0_CTRL_DW2 + (x * 0x40))
+#define MTK_LRO_CTRL_DW3_CFG(x)		(MTK_LRO_RX_RING0_CTRL_DW3 + (x * 0x40))
+#define MTK_RING_AGE_TIME_L		((MTK_HW_LRO_AGE_TIME & 0x3ff) << 22)
+#define MTK_RING_AGE_TIME_H		((MTK_HW_LRO_AGE_TIME >> 10) & 0x3f)
+#define MTK_RING_AUTO_LERAN_MODE	(3 << 6)
+#define MTK_RING_VLD			BIT(8)
+#define MTK_RING_MAX_AGG_TIME		((MTK_HW_LRO_AGG_TIME & 0xffff) << 10)
+#define MTK_RING_MAX_AGG_CNT_L		((MTK_HW_LRO_MAX_AGG_CNT & 0x3f) << 26)
+#define MTK_RING_MAX_AGG_CNT_H		((MTK_HW_LRO_MAX_AGG_CNT >> 6) & 0x3)
+
 /* QDMA TX Queue Configuration Registers */
 #define MTK_QTX_CFG(x)		(0x1800 + (x * 0x10))
 #define QDMA_RES_THRES		4
@@ -106,7 +190,6 @@
 
 /* QDMA Reset Index Register */
 #define MTK_QDMA_RST_IDX	0x1A08
-#define MTK_PST_DRX_IDX0	BIT(16)
 
 /* QDMA Delay Interrupt Register */
 #define MTK_QDMA_DELAY_INT	0x1A0C
@@ -119,13 +202,16 @@
 
 /* QDMA Interrupt Status Register */
 #define MTK_QMTK_INT_STATUS	0x1A18
+#define MTK_RX_DONE_INT3	BIT(19)
+#define MTK_RX_DONE_INT2	BIT(18)
 #define MTK_RX_DONE_INT1	BIT(17)
 #define MTK_RX_DONE_INT0	BIT(16)
 #define MTK_TX_DONE_INT3	BIT(3)
 #define MTK_TX_DONE_INT2	BIT(2)
 #define MTK_TX_DONE_INT1	BIT(1)
 #define MTK_TX_DONE_INT0	BIT(0)
-#define MTK_RX_DONE_INT		(MTK_RX_DONE_INT0 | MTK_RX_DONE_INT1)
+#define MTK_RX_DONE_INT		(MTK_RX_DONE_INT0 | MTK_RX_DONE_INT1 | \
+				 MTK_RX_DONE_INT2 | MTK_RX_DONE_INT3)
 #define MTK_TX_DONE_INT		(MTK_TX_DONE_INT0 | MTK_TX_DONE_INT1 | \
 				 MTK_TX_DONE_INT2 | MTK_TX_DONE_INT3)
 
@@ -227,16 +313,54 @@
 				 MAC_MCR_FORCE_TX_FC | MAC_MCR_SPEED_1000 | \
 				 MAC_MCR_FORCE_DPX | MAC_MCR_FORCE_LINK)
 
+/* TRGMII RXC control register */
+#define TRGMII_RCK_CTRL		0x10300
+#define DQSI0(x)		((x << 0) & GENMASK(6, 0))
+#define DQSI1(x)		((x << 8) & GENMASK(14, 8))
+#define RXCTL_DMWTLAT(x)	((x << 16) & GENMASK(18, 16))
+#define RXC_DQSISEL		BIT(30)
+#define RCK_CTRL_RGMII_1000	(RXC_DQSISEL | RXCTL_DMWTLAT(2) | DQSI1(16))
+#define RCK_CTRL_RGMII_10_100	RXCTL_DMWTLAT(2)
+
+/* TRGMII RXC control register */
+#define TRGMII_TCK_CTRL		0x10340
+#define TXCTL_DMWTLAT(x)	((x << 16) & GENMASK(18, 16))
+#define TXC_INV			BIT(30)
+#define TCK_CTRL_RGMII_1000	TXCTL_DMWTLAT(2)
+#define TCK_CTRL_RGMII_10_100	(TXC_INV | TXCTL_DMWTLAT(2))
+
+/* TRGMII Interface mode register */
+#define INTF_MODE		0x10390
+#define TRGMII_INTF_DIS		BIT(0)
+#define TRGMII_MODE		BIT(1)
+#define TRGMII_CENTRAL_ALIGNED	BIT(2)
+#define INTF_MODE_RGMII_1000    (TRGMII_MODE | TRGMII_CENTRAL_ALIGNED)
+#define INTF_MODE_RGMII_10_100  0
+
 /* GPIO port control registers for GMAC 2*/
 #define GPIO_OD33_CTRL8		0x4c0
 #define GPIO_BIAS_CTRL		0xed0
 #define GPIO_DRV_SEL10		0xf00
 
+/* ethernet subsystem chip id register */
+#define ETHSYS_CHIPID0_3	0x0
+#define ETHSYS_CHIPID4_7	0x4
+#define MT7623_ETH		7623
+
 /* ethernet subsystem config register */
 #define ETHSYS_SYSCFG0		0x14
 #define SYSCFG0_GE_MASK		0x3
 #define SYSCFG0_GE_MODE(x, y)	(x << (12 + (y * 2)))
 
+/* ethernet subsystem clock register */
+#define ETHSYS_CLKCFG0		0x2c
+#define ETHSYS_TRGMII_CLK_SEL362_5	BIT(11)
+
+/* ethernet reset control register */
+#define ETHSYS_RSTCTRL		0x34
+#define RSTCTRL_FE		BIT(6)
+#define RSTCTRL_PPE		BIT(31)
+
 struct mtk_rx_dma {
 	unsigned int rxd1;
 	unsigned int rxd2;
@@ -298,9 +422,15 @@ enum mtk_clks_map {
 	MTK_CLK_ESW,
 	MTK_CLK_GP1,
 	MTK_CLK_GP2,
+	MTK_CLK_TRGPLL,
 	MTK_CLK_MAX
 };
 
+enum mtk_dev_state {
+	MTK_HW_INIT,
+	MTK_RESETTING
+};
+
 /* struct mtk_tx_buf -	This struct holds the pointers to the memory pointed at
  *			by the TX descriptor	s
  * @skb:		The SKB pointer of the packet being sent
@@ -338,6 +468,12 @@ struct mtk_tx_ring {
 	atomic_t free_count;
 };
 
+/* PDMA rx ring mode */
+enum mtk_rx_flags {
+	MTK_RX_FLAGS_NORMAL = 0,
+	MTK_RX_FLAGS_HWLRO,
+};
+
 /* struct mtk_rx_ring -	This struct holds info describing a RX ring
  * @dma:		The descriptor ring
  * @data:		The memory pointed at by the ring
@@ -352,7 +488,10 @@ struct mtk_rx_ring {
 	dma_addr_t phys;
 	u16 frag_size;
 	u16 buf_size;
+	u16 dma_size;
+	bool calc_idx_update;
 	u16 calc_idx;
+	u32 crx_idx_reg;
 };
 
 /* currently no SoC has more than 2 macs */
@@ -384,12 +523,12 @@ struct mtk_rx_ring {
  * @clks:		clock array for all clocks required
  * @mii_bus:		If there is a bus we need to create an instance for it
  * @pending_work:	The workqueue used to reset the dma ring
+ * @state               Initialization and runtime state of the device.
  */
 
 struct mtk_eth {
 	struct device			*dev;
 	void __iomem			*base;
-	struct reset_control		*rstc;
 	spinlock_t			page_lock;
 	spinlock_t			irq_lock;
 	struct net_device		dummy_dev;
@@ -400,9 +539,11 @@ struct mtk_eth {
 	unsigned long			sysclk;
 	struct regmap			*ethsys;
 	struct regmap			*pctl;
+	u32				chip_id;
+	bool				hwlro;
 	atomic_t			dma_refcnt;
 	struct mtk_tx_ring		tx_ring;
-	struct mtk_rx_ring		rx_ring;
+	struct mtk_rx_ring		rx_ring[MTK_MAX_RX_RING_NUM];
 	struct napi_struct		tx_napi;
 	struct napi_struct		rx_napi;
 	struct mtk_tx_dma		*scratch_ring;
@@ -412,22 +553,28 @@ struct mtk_eth {
 
 	struct mii_bus			*mii_bus;
 	struct work_struct		pending_work;
+	unsigned long			state;
 };
 
 /* struct mtk_mac -	the structure that holds the info about the MACs of the
  *			SoC
  * @id:			The number of the MAC
+ * @ge_mode:            Interface mode kept for setup restoring
  * @of_node:		Our devicetree node
  * @hw:			Backpointer to our main datastruture
  * @hw_stats:		Packet statistics counter
- * @phy_dev:		The attached PHY if available
+ * @trgmii		Indicate if the MAC uses TRGMII connected to internal
+			switch
  */
 struct mtk_mac {
 	int				id;
+	int				ge_mode;
 	struct device_node		*of_node;
 	struct mtk_eth			*hw;
 	struct mtk_hw_stats		*hw_stats;
-	struct phy_device		*phy_dev;
+	__be32				hwlro_ip[MTK_MAX_LRO_IP_CNT];
+	int				hwlro_ip_cnt;
+	bool				trgmii;
 };
 
 /* the struct describing the SoC. these are declared in the soc_xyz.c files */
diff --git a/drivers/net/ethernet/mellanox/mlx4/cmd.c b/drivers/net/ethernet/mellanox/mlx4/cmd.c
index f04a423ff79d..b1cef7a0f7ca 100644
--- a/drivers/net/ethernet/mellanox/mlx4/cmd.c
+++ b/drivers/net/ethernet/mellanox/mlx4/cmd.c
@@ -785,17 +785,23 @@ int __mlx4_cmd(struct mlx4_dev *dev, u64 in_param, u64 *out_param,
 		return mlx4_cmd_reset_flow(dev, op, op_modifier, -EIO);
 
 	if (!mlx4_is_mfunc(dev) || (native && mlx4_is_master(dev))) {
+		int ret;
+
 		if (dev->persist->state & MLX4_DEVICE_STATE_INTERNAL_ERROR)
 			return mlx4_internal_err_ret_value(dev, op,
 							  op_modifier);
+		down_read(&mlx4_priv(dev)->cmd.switch_sem);
 		if (mlx4_priv(dev)->cmd.use_events)
-			return mlx4_cmd_wait(dev, in_param, out_param,
-					     out_is_imm, in_modifier,
-					     op_modifier, op, timeout);
+			ret = mlx4_cmd_wait(dev, in_param, out_param,
+					    out_is_imm, in_modifier,
+					    op_modifier, op, timeout);
 		else
-			return mlx4_cmd_poll(dev, in_param, out_param,
-					     out_is_imm, in_modifier,
-					     op_modifier, op, timeout);
+			ret = mlx4_cmd_poll(dev, in_param, out_param,
+					    out_is_imm, in_modifier,
+					    op_modifier, op, timeout);
+
+		up_read(&mlx4_priv(dev)->cmd.switch_sem);
+		return ret;
 	}
 	return mlx4_slave_cmd(dev, in_param, out_param, out_is_imm,
 			      in_modifier, op_modifier, op, timeout);
@@ -1845,6 +1851,7 @@ static int mlx4_master_immediate_activate_vlan_qos(struct mlx4_priv *priv,
 
 	if (vp_oper->state.default_vlan == vp_admin->default_vlan &&
 	    vp_oper->state.default_qos == vp_admin->default_qos &&
+	    vp_oper->state.vlan_proto == vp_admin->vlan_proto &&
 	    vp_oper->state.link_state == vp_admin->link_state &&
 	    vp_oper->state.qos_vport == vp_admin->qos_vport)
 		return 0;
@@ -1903,6 +1910,7 @@ static int mlx4_master_immediate_activate_vlan_qos(struct mlx4_priv *priv,
 
 	vp_oper->state.default_vlan = vp_admin->default_vlan;
 	vp_oper->state.default_qos = vp_admin->default_qos;
+	vp_oper->state.vlan_proto = vp_admin->vlan_proto;
 	vp_oper->state.link_state = vp_admin->link_state;
 	vp_oper->state.qos_vport = vp_admin->qos_vport;
 
@@ -1916,6 +1924,7 @@ static int mlx4_master_immediate_activate_vlan_qos(struct mlx4_priv *priv,
 	work->qos_vport = vp_oper->state.qos_vport;
 	work->vlan_id = vp_oper->state.default_vlan;
 	work->vlan_ix = vp_oper->vlan_idx;
+	work->vlan_proto = vp_oper->state.vlan_proto;
 	work->priv = priv;
 	INIT_WORK(&work->work, mlx4_vf_immed_vlan_work_handler);
 	queue_work(priv->mfunc.master.comm_wq, &work->work);
@@ -1986,6 +1995,8 @@ static int mlx4_master_activate_admin_state(struct mlx4_priv *priv, int slave)
 	int port, err;
 	struct mlx4_vport_state *vp_admin;
 	struct mlx4_vport_oper_state *vp_oper;
+	struct mlx4_slave_state *slave_state =
+		&priv->mfunc.master.slave_state[slave];
 	struct mlx4_active_ports actv_ports = mlx4_get_active_ports(
 			&priv->dev, slave);
 	int min_port = find_first_bit(actv_ports.ports,
@@ -2000,12 +2011,26 @@ static int mlx4_master_activate_admin_state(struct mlx4_priv *priv, int slave)
 			priv->mfunc.master.vf_admin[slave].enable_smi[port];
 		vp_oper = &priv->mfunc.master.vf_oper[slave].vport[port];
 		vp_admin = &priv->mfunc.master.vf_admin[slave].vport[port];
-		vp_oper->state = *vp_admin;
+		if (vp_admin->vlan_proto != htons(ETH_P_8021AD) ||
+		    slave_state->vst_qinq_supported) {
+			vp_oper->state.vlan_proto   = vp_admin->vlan_proto;
+			vp_oper->state.default_vlan = vp_admin->default_vlan;
+			vp_oper->state.default_qos  = vp_admin->default_qos;
+		}
+		vp_oper->state.link_state = vp_admin->link_state;
+		vp_oper->state.mac        = vp_admin->mac;
+		vp_oper->state.spoofchk   = vp_admin->spoofchk;
+		vp_oper->state.tx_rate    = vp_admin->tx_rate;
+		vp_oper->state.qos_vport  = vp_admin->qos_vport;
+		vp_oper->state.guid       = vp_admin->guid;
+
 		if (MLX4_VGT != vp_admin->default_vlan) {
 			err = __mlx4_register_vlan(&priv->dev, port,
 						   vp_admin->default_vlan, &(vp_oper->vlan_idx));
 			if (err) {
 				vp_oper->vlan_idx = NO_INDX;
+				vp_oper->state.default_vlan = MLX4_VGT;
+				vp_oper->state.vlan_proto = htons(ETH_P_8021Q);
 				mlx4_warn(&priv->dev,
 					  "No vlan resources slave %d, port %d\n",
 					  slave, port);
@@ -2086,6 +2111,7 @@ static void mlx4_master_do_cmd(struct mlx4_dev *dev, int slave, u8 cmd,
 		mlx4_warn(dev, "Received reset from slave:%d\n", slave);
 		slave_state[slave].active = false;
 		slave_state[slave].old_vlan_api = false;
+		slave_state[slave].vst_qinq_supported = false;
 		mlx4_master_deactivate_admin_state(priv, slave);
 		for (i = 0; i < MLX4_EVENT_TYPES_NUM; ++i) {
 				slave_state[slave].event_eq[i].eqn = -1;
@@ -2353,6 +2379,7 @@ int mlx4_multi_func_init(struct mlx4_dev *dev)
 			vf_oper = &priv->mfunc.master.vf_oper[i];
 			s_state = &priv->mfunc.master.slave_state[i];
 			s_state->last_cmd = MLX4_COMM_CMD_RESET;
+			s_state->vst_qinq_supported = false;
 			mutex_init(&priv->mfunc.master.gen_eqe_mutex[i]);
 			for (j = 0; j < MLX4_EVENT_TYPES_NUM; ++j)
 				s_state->event_eq[j].eqn = -1;
@@ -2382,6 +2409,8 @@ int mlx4_multi_func_init(struct mlx4_dev *dev)
 				admin_vport->qos_vport =
 						MLX4_VPP_DEFAULT_VPORT;
 				oper_vport->qos_vport = MLX4_VPP_DEFAULT_VPORT;
+				admin_vport->vlan_proto = htons(ETH_P_8021Q);
+				oper_vport->vlan_proto = htons(ETH_P_8021Q);
 				vf_oper->vport[port].vlan_idx = NO_INDX;
 				vf_oper->vport[port].mac_idx = NO_INDX;
 				mlx4_set_random_admin_guid(dev, i, port);
@@ -2454,6 +2483,7 @@ int mlx4_cmd_init(struct mlx4_dev *dev)
 	int flags = 0;
 
 	if (!priv->cmd.initialized) {
+		init_rwsem(&priv->cmd.switch_sem);
 		mutex_init(&priv->cmd.slave_cmd_mutex);
 		sema_init(&priv->cmd.poll_sem, 1);
 		priv->cmd.use_events = 0;
@@ -2583,6 +2613,7 @@ int mlx4_cmd_use_events(struct mlx4_dev *dev)
 	if (!priv->cmd.context)
 		return -ENOMEM;
 
+	down_write(&priv->cmd.switch_sem);
 	for (i = 0; i < priv->cmd.max_cmds; ++i) {
 		priv->cmd.context[i].token = i;
 		priv->cmd.context[i].next  = i + 1;
@@ -2606,6 +2637,7 @@ int mlx4_cmd_use_events(struct mlx4_dev *dev)
 
 	down(&priv->cmd.poll_sem);
 	priv->cmd.use_events = 1;
+	up_write(&priv->cmd.switch_sem);
 
 	return err;
 }
@@ -2618,6 +2650,7 @@ void mlx4_cmd_use_polling(struct mlx4_dev *dev)
 	struct mlx4_priv *priv = mlx4_priv(dev);
 	int i;
 
+	down_write(&priv->cmd.switch_sem);
 	priv->cmd.use_events = 0;
 
 	for (i = 0; i < priv->cmd.max_cmds; ++i)
@@ -2626,6 +2659,7 @@ void mlx4_cmd_use_polling(struct mlx4_dev *dev)
 	kfree(priv->cmd.context);
 
 	up(&priv->cmd.poll_sem);
+	up_write(&priv->cmd.switch_sem);
 }
 
 struct mlx4_cmd_mailbox *mlx4_alloc_cmd_mailbox(struct mlx4_dev *dev)
@@ -2937,10 +2971,13 @@ int mlx4_set_vf_mac(struct mlx4_dev *dev, int port, int vf, u64 mac)
 EXPORT_SYMBOL_GPL(mlx4_set_vf_mac);
 
 
-int mlx4_set_vf_vlan(struct mlx4_dev *dev, int port, int vf, u16 vlan, u8 qos)
+int mlx4_set_vf_vlan(struct mlx4_dev *dev, int port, int vf, u16 vlan, u8 qos,
+		     __be16 proto)
 {
 	struct mlx4_priv *priv = mlx4_priv(dev);
 	struct mlx4_vport_state *vf_admin;
+	struct mlx4_slave_state *slave_state;
+	struct mlx4_vport_oper_state *vf_oper;
 	int slave;
 
 	if ((!mlx4_is_master(dev)) ||
@@ -2950,12 +2987,31 @@ int mlx4_set_vf_vlan(struct mlx4_dev *dev, int port, int vf, u16 vlan, u8 qos)
 	if ((vlan > 4095) || (qos > 7))
 		return -EINVAL;
 
+	if (proto == htons(ETH_P_8021AD) &&
+	    !(dev->caps.flags2 & MLX4_DEV_CAP_FLAG2_SVLAN_BY_QP))
+		return -EPROTONOSUPPORT;
+
+	if (proto != htons(ETH_P_8021Q) &&
+	    proto != htons(ETH_P_8021AD))
+		return -EINVAL;
+
+	if ((proto == htons(ETH_P_8021AD)) &&
+	    ((vlan == 0) || (vlan == MLX4_VGT)))
+		return -EINVAL;
+
 	slave = mlx4_get_slave_indx(dev, vf);
 	if (slave < 0)
 		return -EINVAL;
 
+	slave_state = &priv->mfunc.master.slave_state[slave];
+	if ((proto == htons(ETH_P_8021AD)) && (slave_state->active) &&
+	    (!slave_state->vst_qinq_supported)) {
+		mlx4_err(dev, "vf %d does not support VST QinQ mode\n", vf);
+		return -EPROTONOSUPPORT;
+	}
 	port = mlx4_slaves_closest_port(dev, slave, port);
 	vf_admin = &priv->mfunc.master.vf_admin[slave].vport[port];
+	vf_oper = &priv->mfunc.master.vf_oper[slave].vport[port];
 
 	if (!mlx4_valid_vf_state_change(dev, port, vf_admin, vlan, qos))
 		return -EPERM;
@@ -2965,6 +3021,7 @@ int mlx4_set_vf_vlan(struct mlx4_dev *dev, int port, int vf, u16 vlan, u8 qos)
 	else
 		vf_admin->default_vlan = vlan;
 	vf_admin->default_qos = qos;
+	vf_admin->vlan_proto = proto;
 
 	/* If rate was configured prior to VST, we saved the configured rate
 	 * in vf_admin->rate and now, if priority supported we enforce the QoS
@@ -2973,7 +3030,12 @@ int mlx4_set_vf_vlan(struct mlx4_dev *dev, int port, int vf, u16 vlan, u8 qos)
 	    vf_admin->tx_rate)
 		vf_admin->qos_vport = slave;
 
-	if (mlx4_master_immediate_activate_vlan_qos(priv, slave, port))
+	/* Try to activate new vf state without restart,
+	 * this option is not supported while moving to VST QinQ mode.
+	 */
+	if ((proto == htons(ETH_P_8021AD) &&
+	     vf_oper->state.vlan_proto != proto) ||
+	    mlx4_master_immediate_activate_vlan_qos(priv, slave, port))
 		mlx4_info(dev,
 			  "updating vf %d port %d config will take effect on next VF restart\n",
 			  vf, port);
@@ -3117,6 +3179,7 @@ int mlx4_get_vf_config(struct mlx4_dev *dev, int port, int vf, struct ifla_vf_in
 
 	ivf->vlan		= s_info->default_vlan;
 	ivf->qos		= s_info->default_qos;
+	ivf->vlan_proto		= s_info->vlan_proto;
 
 	if (mlx4_is_vf_vst_and_prio_qos(dev, port, s_info))
 		ivf->max_tx_rate = s_info->tx_rate;
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_clock.c b/drivers/net/ethernet/mellanox/mlx4/en_clock.c
index 1494997c4f7e..08fc5fc56d43 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_clock.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_clock.c
@@ -298,7 +298,7 @@ void mlx4_en_init_timestamp(struct mlx4_en_dev *mdev)
 	if (IS_ERR(mdev->ptp_clock)) {
 		mdev->ptp_clock = NULL;
 		mlx4_err(mdev, "ptp_clock_register failed\n");
-	} else {
+	} else if (mdev->ptp_clock) {
 		mlx4_info(mdev, "registered PHC clock\n");
 	}
 
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_netdev.c b/drivers/net/ethernet/mellanox/mlx4/en_netdev.c
index fedb829276f4..7e703bed7b82 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_netdev.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_netdev.c
@@ -2400,12 +2400,14 @@ static int mlx4_en_set_vf_mac(struct net_device *dev, int queue, u8 *mac)
 	return mlx4_set_vf_mac(mdev->dev, en_priv->port, queue, mac_u64);
 }
 
-static int mlx4_en_set_vf_vlan(struct net_device *dev, int vf, u16 vlan, u8 qos)
+static int mlx4_en_set_vf_vlan(struct net_device *dev, int vf, u16 vlan, u8 qos,
+			       __be16 vlan_proto)
 {
 	struct mlx4_en_priv *en_priv = netdev_priv(dev);
 	struct mlx4_en_dev *mdev = en_priv->mdev;
 
-	return mlx4_set_vf_vlan(mdev->dev, en_priv->port, vf, vlan, qos);
+	return mlx4_set_vf_vlan(mdev->dev, en_priv->port, vf, vlan, qos,
+				vlan_proto);
 }
 
 static int mlx4_en_set_vf_rate(struct net_device *dev, int vf, int min_tx_rate,
@@ -2643,12 +2645,16 @@ static int mlx4_xdp_set(struct net_device *dev, struct bpf_prog *prog)
 			if (IS_ERR(prog))
 				return PTR_ERR(prog);
 		}
+		mutex_lock(&mdev->state_lock);
 		for (i = 0; i < priv->rx_ring_num; i++) {
-			/* This xchg is paired with READ_ONCE in the fastpath */
-			old_prog = xchg(&priv->rx_ring[i]->xdp_prog, prog);
+			old_prog = rcu_dereference_protected(
+					priv->rx_ring[i]->xdp_prog,
+					lockdep_is_held(&mdev->state_lock));
+			rcu_assign_pointer(priv->rx_ring[i]->xdp_prog, prog);
 			if (old_prog)
 				bpf_prog_put(old_prog);
 		}
+		mutex_unlock(&mdev->state_lock);
 		return 0;
 	}
 
@@ -2681,7 +2687,10 @@ static int mlx4_xdp_set(struct net_device *dev, struct bpf_prog *prog)
 							priv->xdp_ring_num);
 
 	for (i = 0; i < priv->rx_ring_num; i++) {
-		old_prog = xchg(&priv->rx_ring[i]->xdp_prog, prog);
+		old_prog = rcu_dereference_protected(
+					priv->rx_ring[i]->xdp_prog,
+					lockdep_is_held(&mdev->state_lock));
+		rcu_assign_pointer(priv->rx_ring[i]->xdp_prog, prog);
 		if (old_prog)
 			bpf_prog_put(old_prog);
 	}
@@ -3217,6 +3226,7 @@ int mlx4_en_init_netdev(struct mlx4_en_dev *mdev, int port,
 	}
 
 	if (mlx4_is_slave(mdev->dev)) {
+		bool vlan_offload_disabled;
 		int phv;
 
 		err = get_phv_bit(mdev->dev, port, &phv);
@@ -3224,6 +3234,18 @@ int mlx4_en_init_netdev(struct mlx4_en_dev *mdev, int port,
 			dev->hw_features |= NETIF_F_HW_VLAN_STAG_TX;
 			priv->pflags |= MLX4_EN_PRIV_FLAGS_PHV;
 		}
+		err = mlx4_get_is_vlan_offload_disabled(mdev->dev, port,
+							&vlan_offload_disabled);
+		if (!err && vlan_offload_disabled) {
+			dev->hw_features &= ~(NETIF_F_HW_VLAN_CTAG_TX |
+					      NETIF_F_HW_VLAN_CTAG_RX |
+					      NETIF_F_HW_VLAN_STAG_TX |
+					      NETIF_F_HW_VLAN_STAG_RX);
+			dev->features &= ~(NETIF_F_HW_VLAN_CTAG_TX |
+					   NETIF_F_HW_VLAN_CTAG_RX |
+					   NETIF_F_HW_VLAN_STAG_TX |
+					   NETIF_F_HW_VLAN_STAG_RX);
+		}
 	} else {
 		if (mdev->dev->caps.flags2 & MLX4_DEV_CAP_FLAG2_PHV_EN &&
 		    !(mdev->dev->caps.flags2 &
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_rx.c b/drivers/net/ethernet/mellanox/mlx4/en_rx.c
index 2040dad8611d..f2e8beddcf44 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_rx.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_rx.c
@@ -72,7 +72,7 @@ static int mlx4_alloc_pages(struct mlx4_en_priv *priv,
 	}
 	dma = dma_map_page(priv->ddev, page, 0, PAGE_SIZE << order,
 			   frag_info->dma_dir);
-	if (dma_mapping_error(priv->ddev, dma)) {
+	if (unlikely(dma_mapping_error(priv->ddev, dma))) {
 		put_page(page);
 		return -ENOMEM;
 	}
@@ -108,7 +108,8 @@ static int mlx4_en_alloc_frags(struct mlx4_en_priv *priv,
 		    ring_alloc[i].page_size)
 			continue;
 
-		if (mlx4_alloc_pages(priv, &page_alloc[i], frag_info, gfp))
+		if (unlikely(mlx4_alloc_pages(priv, &page_alloc[i],
+					      frag_info, gfp)))
 			goto out;
 	}
 
@@ -537,7 +538,9 @@ void mlx4_en_destroy_rx_ring(struct mlx4_en_priv *priv,
 	struct mlx4_en_rx_ring *ring = *pring;
 	struct bpf_prog *old_prog;
 
-	old_prog = READ_ONCE(ring->xdp_prog);
+	old_prog = rcu_dereference_protected(
+					ring->xdp_prog,
+					lockdep_is_held(&mdev->state_lock));
 	if (old_prog)
 		bpf_prog_put(old_prog);
 	mlx4_free_hwq_res(mdev->dev, &ring->wqres, size * stride + TXBB_SIZE);
@@ -583,7 +586,7 @@ static int mlx4_en_complete_rx_desc(struct mlx4_en_priv *priv,
 		frag_info = &priv->frag_info[nr];
 		if (length <= frag_info->frag_prefix_size)
 			break;
-		if (!frags[nr].page)
+		if (unlikely(!frags[nr].page))
 			goto fail;
 
 		dma = be64_to_cpu(rx_desc->data[nr].addr);
@@ -623,7 +626,7 @@ static struct sk_buff *mlx4_en_rx_skb(struct mlx4_en_priv *priv,
 	dma_addr_t dma;
 
 	skb = netdev_alloc_skb(priv->dev, SMALL_PACKET_SIZE + NET_IP_ALIGN);
-	if (!skb) {
+	if (unlikely(!skb)) {
 		en_dbg(RX_ERR, priv, "Failed allocating skb\n");
 		return NULL;
 	}
@@ -734,7 +737,8 @@ static int get_fixed_ipv6_csum(__wsum hw_checksum, struct sk_buff *skb,
 {
 	__wsum csum_pseudo_hdr = 0;
 
-	if (ipv6h->nexthdr == IPPROTO_FRAGMENT || ipv6h->nexthdr == IPPROTO_HOPOPTS)
+	if (unlikely(ipv6h->nexthdr == IPPROTO_FRAGMENT ||
+		     ipv6h->nexthdr == IPPROTO_HOPOPTS))
 		return -1;
 	hw_checksum = csum_add(hw_checksum, (__force __wsum)htons(ipv6h->nexthdr));
 
@@ -767,7 +771,7 @@ static int check_csum(struct mlx4_cqe *cqe, struct sk_buff *skb, void *va,
 		get_fixed_ipv4_csum(hw_checksum, skb, hdr);
 #if IS_ENABLED(CONFIG_IPV6)
 	else if (cqe->status & cpu_to_be16(MLX4_CQE_STATUS_IPV6))
-		if (get_fixed_ipv6_csum(hw_checksum, skb, hdr))
+		if (unlikely(get_fixed_ipv6_csum(hw_checksum, skb, hdr)))
 			return -1;
 #endif
 	return 0;
@@ -794,13 +798,15 @@ int mlx4_en_process_rx_cq(struct net_device *dev, struct mlx4_en_cq *cq, int bud
 	u64 timestamp;
 	bool l2_tunnel;
 
-	if (!priv->port_up)
+	if (unlikely(!priv->port_up))
 		return 0;
 
-	if (budget <= 0)
+	if (unlikely(budget <= 0))
 		return polled;
 
-	xdp_prog = READ_ONCE(ring->xdp_prog);
+	/* Protect accesses to: ring->xdp_prog, priv->mac_hash list */
+	rcu_read_lock();
+	xdp_prog = rcu_dereference(ring->xdp_prog);
 	doorbell_pending = 0;
 	tx_index = (priv->tx_ring_num - priv->xdp_ring_num) + cq->ring;
 
@@ -858,15 +864,11 @@ int mlx4_en_process_rx_cq(struct net_device *dev, struct mlx4_en_cq *cq, int bud
 				/* Drop the packet, since HW loopback-ed it */
 				mac_hash = ethh->h_source[MLX4_EN_MAC_HASH_IDX];
 				bucket = &priv->mac_hash[mac_hash];
-				rcu_read_lock();
 				hlist_for_each_entry_rcu(entry, bucket, hlist) {
 					if (ether_addr_equal_64bits(entry->mac,
-								    ethh->h_source)) {
-						rcu_read_unlock();
+								    ethh->h_source))
 						goto next;
-					}
 				}
-				rcu_read_unlock();
 			}
 		}
 
@@ -902,16 +904,17 @@ int mlx4_en_process_rx_cq(struct net_device *dev, struct mlx4_en_cq *cq, int bud
 			case XDP_PASS:
 				break;
 			case XDP_TX:
-				if (!mlx4_en_xmit_frame(frags, dev,
+				if (likely(!mlx4_en_xmit_frame(frags, dev,
 							length, tx_index,
-							&doorbell_pending))
+							&doorbell_pending)))
 					goto consumed;
-				break;
+				goto xdp_drop; /* Drop on xmit failure */
 			default:
 				bpf_warn_invalid_xdp_action(act);
 			case XDP_ABORTED:
 			case XDP_DROP:
-				if (mlx4_en_rx_recycle(ring, frags))
+xdp_drop:
+				if (likely(mlx4_en_rx_recycle(ring, frags)))
 					goto consumed;
 				goto next;
 			}
@@ -1015,12 +1018,12 @@ int mlx4_en_process_rx_cq(struct net_device *dev, struct mlx4_en_cq *cq, int bud
 
 		/* GRO not possible, complete processing here */
 		skb = mlx4_en_rx_skb(priv, rx_desc, frags, length);
-		if (!skb) {
+		if (unlikely(!skb)) {
 			ring->dropped++;
 			goto next;
 		}
 
-                if (unlikely(priv->validate_loopback)) {
+		if (unlikely(priv->validate_loopback)) {
 			validate_loopback(priv, skb);
 			goto next;
 		}
@@ -1077,6 +1080,7 @@ consumed:
 	}
 
 out:
+	rcu_read_unlock();
 	if (doorbell_pending)
 		mlx4_en_xmit_doorbell(priv->tx_ring[tx_index]);
 
diff --git a/drivers/net/ethernet/mellanox/mlx4/fw.c b/drivers/net/ethernet/mellanox/mlx4/fw.c
index d728704d0c7b..c41ab31a39f8 100644
--- a/drivers/net/ethernet/mellanox/mlx4/fw.c
+++ b/drivers/net/ethernet/mellanox/mlx4/fw.c
@@ -158,7 +158,9 @@ static void dump_dev_cap_flags2(struct mlx4_dev *dev, u64 flags)
 		[31] = "Modifying loopback source checks using UPDATE_QP support",
 		[32] = "Loopback source checks support",
 		[33] = "RoCEv2 support",
-		[34] = "DMFS Sniffer support (UC & MC)"
+		[34] = "DMFS Sniffer support (UC & MC)",
+		[35] = "QinQ VST mode support",
+		[36] = "sl to vl mapping table change event support"
 	};
 	int i;
 
@@ -248,6 +250,72 @@ out:
 	return err;
 }
 
+static int mlx4_activate_vst_qinq(struct mlx4_priv *priv, int slave, int port)
+{
+	struct mlx4_vport_oper_state *vp_oper;
+	struct mlx4_vport_state *vp_admin;
+	int err;
+
+	vp_oper = &priv->mfunc.master.vf_oper[slave].vport[port];
+	vp_admin = &priv->mfunc.master.vf_admin[slave].vport[port];
+
+	if (vp_admin->default_vlan != vp_oper->state.default_vlan) {
+		err = __mlx4_register_vlan(&priv->dev, port,
+					   vp_admin->default_vlan,
+					   &vp_oper->vlan_idx);
+		if (err) {
+			vp_oper->vlan_idx = NO_INDX;
+			mlx4_warn(&priv->dev,
+				  "No vlan resources slave %d, port %d\n",
+				  slave, port);
+			return err;
+		}
+		mlx4_dbg(&priv->dev, "alloc vlan %d idx  %d slave %d port %d\n",
+			 (int)(vp_oper->state.default_vlan),
+			 vp_oper->vlan_idx, slave, port);
+	}
+	vp_oper->state.vlan_proto   = vp_admin->vlan_proto;
+	vp_oper->state.default_vlan = vp_admin->default_vlan;
+	vp_oper->state.default_qos  = vp_admin->default_qos;
+
+	return 0;
+}
+
+static int mlx4_handle_vst_qinq(struct mlx4_priv *priv, int slave, int port)
+{
+	struct mlx4_vport_oper_state *vp_oper;
+	struct mlx4_slave_state *slave_state;
+	struct mlx4_vport_state *vp_admin;
+	int err;
+
+	vp_oper = &priv->mfunc.master.vf_oper[slave].vport[port];
+	vp_admin = &priv->mfunc.master.vf_admin[slave].vport[port];
+	slave_state = &priv->mfunc.master.slave_state[slave];
+
+	if ((vp_admin->vlan_proto != htons(ETH_P_8021AD)) ||
+	    (!slave_state->active))
+		return 0;
+
+	if (vp_oper->state.vlan_proto == vp_admin->vlan_proto &&
+	    vp_oper->state.default_vlan == vp_admin->default_vlan &&
+	    vp_oper->state.default_qos == vp_admin->default_qos)
+		return 0;
+
+	if (!slave_state->vst_qinq_supported) {
+		/* Warn and revert the request to set vst QinQ mode */
+		vp_admin->vlan_proto   = vp_oper->state.vlan_proto;
+		vp_admin->default_vlan = vp_oper->state.default_vlan;
+		vp_admin->default_qos  = vp_oper->state.default_qos;
+
+		mlx4_warn(&priv->dev,
+			  "Slave %d does not support VST QinQ mode\n", slave);
+		return 0;
+	}
+
+	err = mlx4_activate_vst_qinq(priv, slave, port);
+	return err;
+}
+
 int mlx4_QUERY_FUNC_CAP_wrapper(struct mlx4_dev *dev, int slave,
 				struct mlx4_vhcr *vhcr,
 				struct mlx4_cmd_mailbox *inbox,
@@ -311,14 +379,18 @@ int mlx4_QUERY_FUNC_CAP_wrapper(struct mlx4_dev *dev, int slave,
 #define QUERY_FUNC_CAP_VF_ENABLE_QP0		0x08
 
 #define QUERY_FUNC_CAP_FLAGS0_FORCE_PHY_WQE_GID 0x80
-#define QUERY_FUNC_CAP_SUPPORTS_NON_POWER_OF_2_NUM_EQS (1 << 31)
 #define QUERY_FUNC_CAP_PHV_BIT			0x40
+#define QUERY_FUNC_CAP_VLAN_OFFLOAD_DISABLE	0x20
+
+#define QUERY_FUNC_CAP_SUPPORTS_VST_QINQ	BIT(30)
+#define QUERY_FUNC_CAP_SUPPORTS_NON_POWER_OF_2_NUM_EQS BIT(31)
 
 	if (vhcr->op_modifier == 1) {
 		struct mlx4_active_ports actv_ports =
 			mlx4_get_active_ports(dev, slave);
 		int converted_port = mlx4_slave_convert_port(
 				dev, slave, vhcr->in_modifier);
+		struct mlx4_vport_oper_state *vp_oper;
 
 		if (converted_port < 0)
 			return -EINVAL;
@@ -357,15 +429,24 @@ int mlx4_QUERY_FUNC_CAP_wrapper(struct mlx4_dev *dev, int slave,
 		MLX4_PUT(outbox->buf, dev->caps.phys_port_id[vhcr->in_modifier],
 			 QUERY_FUNC_CAP_PHYS_PORT_ID);
 
-		if (dev->caps.phv_bit[port]) {
-			field = QUERY_FUNC_CAP_PHV_BIT;
-			MLX4_PUT(outbox->buf, field,
-				 QUERY_FUNC_CAP_FLAGS0_OFFSET);
-		}
+		vp_oper = &priv->mfunc.master.vf_oper[slave].vport[port];
+		err = mlx4_handle_vst_qinq(priv, slave, port);
+		if (err)
+			return err;
+
+		field = 0;
+		if (dev->caps.phv_bit[port])
+			field |= QUERY_FUNC_CAP_PHV_BIT;
+		if (vp_oper->state.vlan_proto == htons(ETH_P_8021AD))
+			field |= QUERY_FUNC_CAP_VLAN_OFFLOAD_DISABLE;
+		MLX4_PUT(outbox->buf, field, QUERY_FUNC_CAP_FLAGS0_OFFSET);
 
 	} else if (vhcr->op_modifier == 0) {
 		struct mlx4_active_ports actv_ports =
 			mlx4_get_active_ports(dev, slave);
+		struct mlx4_slave_state *slave_state =
+			&priv->mfunc.master.slave_state[slave];
+
 		/* enable rdma and ethernet interfaces, new quota locations,
 		 * and reserved lkey
 		 */
@@ -439,6 +520,10 @@ int mlx4_QUERY_FUNC_CAP_wrapper(struct mlx4_dev *dev, int slave,
 
 		size = dev->caps.reserved_lkey + ((slave << 8) & 0xFF00);
 		MLX4_PUT(outbox->buf, size, QUERY_FUNC_CAP_QP_RESD_LKEY_OFFSET);
+
+		if (vhcr->in_modifier & QUERY_FUNC_CAP_SUPPORTS_VST_QINQ)
+			slave_state->vst_qinq_supported = true;
+
 	} else
 		err = -EINVAL;
 
@@ -454,10 +539,12 @@ int mlx4_QUERY_FUNC_CAP(struct mlx4_dev *dev, u8 gen_or_port,
 	u32			size, qkey;
 	int			err = 0, quotas = 0;
 	u32                     in_modifier;
+	u32			slave_caps;
 
 	op_modifier = !!gen_or_port; /* 0 = general, 1 = logical port */
-	in_modifier = op_modifier ? gen_or_port :
+	slave_caps = QUERY_FUNC_CAP_SUPPORTS_VST_QINQ |
 		QUERY_FUNC_CAP_SUPPORTS_NON_POWER_OF_2_NUM_EQS;
+	in_modifier = op_modifier ? gen_or_port : slave_caps;
 
 	mailbox = mlx4_alloc_cmd_mailbox(dev);
 	if (IS_ERR(mailbox))
@@ -612,8 +699,7 @@ int mlx4_QUERY_FUNC_CAP(struct mlx4_dev *dev, u8 gen_or_port,
 		MLX4_GET(func_cap->phys_port_id, outbox,
 			 QUERY_FUNC_CAP_PHYS_PORT_ID);
 
-	MLX4_GET(field, outbox, QUERY_FUNC_CAP_FLAGS0_OFFSET);
-	func_cap->flags |= (field & QUERY_FUNC_CAP_PHV_BIT);
+	MLX4_GET(func_cap->flags0, outbox, QUERY_FUNC_CAP_FLAGS0_OFFSET);
 
 	/* All other resources are allocated by the master, but we still report
 	 * 'num' and 'reserved' capabilities as follows:
@@ -690,6 +776,7 @@ int mlx4_QUERY_DEV_CAP(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap)
 #define QUERY_DEV_CAP_MAX_DESC_SZ_SQ_OFFSET	0x52
 #define QUERY_DEV_CAP_MAX_SG_RQ_OFFSET		0x55
 #define QUERY_DEV_CAP_MAX_DESC_SZ_RQ_OFFSET	0x56
+#define QUERY_DEV_CAP_SVLAN_BY_QP_OFFSET	0x5D
 #define QUERY_DEV_CAP_MAX_QP_MCG_OFFSET		0x61
 #define QUERY_DEV_CAP_RSVD_MCG_OFFSET		0x62
 #define QUERY_DEV_CAP_MAX_MCG_OFFSET		0x63
@@ -703,6 +790,7 @@ int mlx4_QUERY_DEV_CAP(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap)
 #define QUERY_DEV_CAP_FLOW_STEERING_IPOIB_OFFSET	0x74
 #define QUERY_DEV_CAP_FLOW_STEERING_RANGE_EN_OFFSET	0x76
 #define QUERY_DEV_CAP_FLOW_STEERING_MAX_QP_OFFSET	0x77
+#define QUERY_DEV_CAP_SL2VL_EVENT_OFFSET	0x78
 #define QUERY_DEV_CAP_CQ_EQ_CACHE_LINE_STRIDE	0x7a
 #define QUERY_DEV_CAP_ECN_QCN_VER_OFFSET	0x7b
 #define QUERY_DEV_CAP_RDMARC_ENTRY_SZ_OFFSET	0x80
@@ -767,12 +855,8 @@ int mlx4_QUERY_DEV_CAP(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap)
 	dev_cap->max_eqs = 1 << (field & 0xf);
 	MLX4_GET(field, outbox, QUERY_DEV_CAP_RSVD_MTT_OFFSET);
 	dev_cap->reserved_mtts = 1 << (field >> 4);
-	MLX4_GET(field, outbox, QUERY_DEV_CAP_MAX_MRW_SZ_OFFSET);
-	dev_cap->max_mrw_sz = 1 << field;
 	MLX4_GET(field, outbox, QUERY_DEV_CAP_RSVD_MRW_OFFSET);
 	dev_cap->reserved_mrws = 1 << (field & 0xf);
-	MLX4_GET(field, outbox, QUERY_DEV_CAP_MAX_MTT_SEG_OFFSET);
-	dev_cap->max_mtt_seg = 1 << (field & 0x3f);
 	MLX4_GET(size, outbox, QUERY_DEV_CAP_NUM_SYS_EQ_OFFSET);
 	dev_cap->num_sys_eqs = size & 0xfff;
 	MLX4_GET(field, outbox, QUERY_DEV_CAP_MAX_REQ_QP_OFFSET);
@@ -822,6 +906,9 @@ int mlx4_QUERY_DEV_CAP(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap)
 		dev_cap->flags2 |= MLX4_DEV_CAP_FLAG2_DMFS_IPOIB;
 	MLX4_GET(field, outbox, QUERY_DEV_CAP_FLOW_STEERING_MAX_QP_OFFSET);
 	dev_cap->fs_max_num_qp_per_entry = field;
+	MLX4_GET(field, outbox, QUERY_DEV_CAP_SL2VL_EVENT_OFFSET);
+	if (field & (1 << 5))
+		dev_cap->flags2 |= MLX4_DEV_CAP_FLAG2_SL_TO_VL_CHANGE_EVENT;
 	MLX4_GET(field, outbox, QUERY_DEV_CAP_ECN_QCN_VER_OFFSET);
 	if (field & 0x1)
 		dev_cap->flags2 |= MLX4_DEV_CAP_FLAG2_QCN;
@@ -857,6 +944,9 @@ int mlx4_QUERY_DEV_CAP(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap)
 	MLX4_GET(size, outbox, QUERY_DEV_CAP_MAX_DESC_SZ_SQ_OFFSET);
 	dev_cap->max_sq_desc_sz = size;
 
+	MLX4_GET(field, outbox, QUERY_DEV_CAP_SVLAN_BY_QP_OFFSET);
+	if (field & 0x1)
+		dev_cap->flags2 |= MLX4_DEV_CAP_FLAG2_SVLAN_BY_QP;
 	MLX4_GET(field, outbox, QUERY_DEV_CAP_MAX_QP_MCG_OFFSET);
 	dev_cap->max_qp_per_mcg = 1 << field;
 	MLX4_GET(field, outbox, QUERY_DEV_CAP_RSVD_MCG_OFFSET);
@@ -2698,7 +2788,6 @@ static int mlx4_check_smp_firewall_active(struct mlx4_dev *dev,
 int mlx4_config_mad_demux(struct mlx4_dev *dev)
 {
 	struct mlx4_cmd_mailbox *mailbox;
-	int secure_host_active;
 	int err;
 
 	/* Check if mad_demux is supported */
@@ -2721,7 +2810,8 @@ int mlx4_config_mad_demux(struct mlx4_dev *dev)
 		goto out;
 	}
 
-	secure_host_active = mlx4_check_smp_firewall_active(dev, mailbox);
+	if (mlx4_check_smp_firewall_active(dev, mailbox))
+		dev->flags |= MLX4_FLAG_SECURE_HOST;
 
 	/* Config mad_demux to handle all MADs returned by the query above */
 	err = mlx4_cmd(dev, mailbox->dma, 0x01 /* subn mgmt class */,
@@ -2732,7 +2822,7 @@ int mlx4_config_mad_demux(struct mlx4_dev *dev)
 		goto out;
 	}
 
-	if (secure_host_active)
+	if (dev->flags & MLX4_FLAG_SECURE_HOST)
 		mlx4_warn(dev, "HCA operating in secure-host mode. SMP firewall activated.\n");
 out:
 	mlx4_free_cmd_mailbox(dev, mailbox);
@@ -2914,7 +3004,7 @@ int get_phv_bit(struct mlx4_dev *dev, u8 port, int *phv)
 	memset(&func_cap, 0, sizeof(func_cap));
 	err = mlx4_QUERY_FUNC_CAP(dev, port, &func_cap);
 	if (!err)
-		*phv = func_cap.flags & QUERY_FUNC_CAP_PHV_BIT;
+		*phv = func_cap.flags0 & QUERY_FUNC_CAP_PHV_BIT;
 	return err;
 }
 EXPORT_SYMBOL(get_phv_bit);
@@ -2938,6 +3028,22 @@ int set_phv_bit(struct mlx4_dev *dev, u8 port, int new_val)
 }
 EXPORT_SYMBOL(set_phv_bit);
 
+int mlx4_get_is_vlan_offload_disabled(struct mlx4_dev *dev, u8 port,
+				      bool *vlan_offload_disabled)
+{
+	struct mlx4_func_cap func_cap;
+	int err;
+
+	memset(&func_cap, 0, sizeof(func_cap));
+	err = mlx4_QUERY_FUNC_CAP(dev, port, &func_cap);
+	if (!err)
+		*vlan_offload_disabled =
+			!!(func_cap.flags0 &
+			   QUERY_FUNC_CAP_VLAN_OFFLOAD_DISABLE);
+	return err;
+}
+EXPORT_SYMBOL(mlx4_get_is_vlan_offload_disabled);
+
 void mlx4_replace_zero_macs(struct mlx4_dev *dev)
 {
 	int i;
diff --git a/drivers/net/ethernet/mellanox/mlx4/fw.h b/drivers/net/ethernet/mellanox/mlx4/fw.h
index cdbd76f10ced..5343a0599253 100644
--- a/drivers/net/ethernet/mellanox/mlx4/fw.h
+++ b/drivers/net/ethernet/mellanox/mlx4/fw.h
@@ -80,9 +80,7 @@ struct mlx4_dev_cap {
 	int max_eqs;
 	int num_sys_eqs;
 	int reserved_mtts;
-	int max_mrw_sz;
 	int reserved_mrws;
-	int max_mtt_seg;
 	int max_requester_per_qp;
 	int max_responder_per_qp;
 	int max_rdma_global;
@@ -152,7 +150,7 @@ struct mlx4_func_cap {
 	u32	qp1_proxy_qpn;
 	u32	reserved_lkey;
 	u8	physical_port;
-	u8	port_flags;
+	u8	flags0;
 	u8	flags1;
 	u64	phys_port_id;
 	u32	extra_flags;
diff --git a/drivers/net/ethernet/mellanox/mlx4/mlx4.h b/drivers/net/ethernet/mellanox/mlx4/mlx4.h
index c9d7fc5159f2..e4878f31e45d 100644
--- a/drivers/net/ethernet/mellanox/mlx4/mlx4.h
+++ b/drivers/net/ethernet/mellanox/mlx4/mlx4.h
@@ -46,6 +46,7 @@
 #include <linux/interrupt.h>
 #include <linux/spinlock.h>
 #include <net/devlink.h>
+#include <linux/rwsem.h>
 
 #include <linux/mlx4/device.h>
 #include <linux/mlx4/driver.h>
@@ -482,6 +483,7 @@ struct mlx4_slave_state {
 	u8 init_port_mask;
 	bool active;
 	bool old_vlan_api;
+	bool vst_qinq_supported;
 	u8 function;
 	dma_addr_t vhcr_dma;
 	u16 mtu[MLX4_MAX_PORTS + 1];
@@ -507,6 +509,7 @@ struct mlx4_vport_state {
 	u64 mac;
 	u16 default_vlan;
 	u8  default_qos;
+	__be16 vlan_proto;
 	u32 tx_rate;
 	bool spoofchk;
 	u32 link_state;
@@ -627,6 +630,7 @@ struct mlx4_cmd {
 	struct mutex		slave_cmd_mutex;
 	struct semaphore	poll_sem;
 	struct semaphore	event_sem;
+	struct rw_semaphore	switch_sem;
 	int			max_cmds;
 	spinlock_t		context_lock;
 	int			free_head;
@@ -655,6 +659,7 @@ struct mlx4_vf_immed_vlan_work {
 	u8                      qos_vport;
 	u16			vlan_id;
 	u16			orig_vlan_id;
+	__be16			vlan_proto;
 };
 
 
diff --git a/drivers/net/ethernet/mellanox/mlx4/mlx4_en.h b/drivers/net/ethernet/mellanox/mlx4/mlx4_en.h
index 9099dbd04951..a3528dd1e72e 100644
--- a/drivers/net/ethernet/mellanox/mlx4/mlx4_en.h
+++ b/drivers/net/ethernet/mellanox/mlx4/mlx4_en.h
@@ -340,7 +340,7 @@ struct mlx4_en_rx_ring {
 	u8  fcs_del;
 	void *buf;
 	void *rx_info;
-	struct bpf_prog *xdp_prog;
+	struct bpf_prog __rcu *xdp_prog;
 	struct mlx4_en_page_cache page_cache;
 	unsigned long bytes;
 	unsigned long packets;
diff --git a/drivers/net/ethernet/mellanox/mlx4/resource_tracker.c b/drivers/net/ethernet/mellanox/mlx4/resource_tracker.c
index 8b81114bdc72..84d7857ccc27 100644
--- a/drivers/net/ethernet/mellanox/mlx4/resource_tracker.c
+++ b/drivers/net/ethernet/mellanox/mlx4/resource_tracker.c
@@ -790,10 +790,22 @@ static int update_vport_qp_param(struct mlx4_dev *dev,
 				MLX4_VLAN_CTRL_ETH_RX_BLOCK_UNTAGGED |
 				MLX4_VLAN_CTRL_ETH_RX_BLOCK_TAGGED;
 		} else if (0 != vp_oper->state.default_vlan) {
-			qpc->pri_path.vlan_control |=
-				MLX4_VLAN_CTRL_ETH_TX_BLOCK_TAGGED |
-				MLX4_VLAN_CTRL_ETH_RX_BLOCK_PRIO_TAGGED |
-				MLX4_VLAN_CTRL_ETH_RX_BLOCK_UNTAGGED;
+			if (vp_oper->state.vlan_proto == htons(ETH_P_8021AD)) {
+				/* vst QinQ should block untagged on TX,
+				 * but cvlan is in payload and phv is set so
+				 * hw see it as untagged. Block tagged instead.
+				 */
+				qpc->pri_path.vlan_control |=
+					MLX4_VLAN_CTRL_ETH_TX_BLOCK_PRIO_TAGGED |
+					MLX4_VLAN_CTRL_ETH_TX_BLOCK_TAGGED |
+					MLX4_VLAN_CTRL_ETH_RX_BLOCK_PRIO_TAGGED |
+					MLX4_VLAN_CTRL_ETH_RX_BLOCK_UNTAGGED;
+			} else { /* vst 802.1Q */
+				qpc->pri_path.vlan_control |=
+					MLX4_VLAN_CTRL_ETH_TX_BLOCK_TAGGED |
+					MLX4_VLAN_CTRL_ETH_RX_BLOCK_PRIO_TAGGED |
+					MLX4_VLAN_CTRL_ETH_RX_BLOCK_UNTAGGED;
+			}
 		} else { /* priority tagged */
 			qpc->pri_path.vlan_control |=
 				MLX4_VLAN_CTRL_ETH_TX_BLOCK_TAGGED |
@@ -802,7 +814,11 @@ static int update_vport_qp_param(struct mlx4_dev *dev,
 
 		qpc->pri_path.fvl_rx |= MLX4_FVL_RX_FORCE_ETH_VLAN;
 		qpc->pri_path.vlan_index = vp_oper->vlan_idx;
-		qpc->pri_path.fl |= MLX4_FL_CV | MLX4_FL_ETH_HIDE_CQE_VLAN;
+		qpc->pri_path.fl |= MLX4_FL_ETH_HIDE_CQE_VLAN;
+		if (vp_oper->state.vlan_proto == htons(ETH_P_8021AD))
+			qpc->pri_path.fl |= MLX4_FL_SV;
+		else
+			qpc->pri_path.fl |= MLX4_FL_CV;
 		qpc->pri_path.feup |= MLX4_FEUP_FORCE_ETH_UP | MLX4_FVL_FORCE_ETH_VLAN;
 		qpc->pri_path.sched_queue &= 0xC7;
 		qpc->pri_path.sched_queue |= (vp_oper->state.default_qos) << 3;
@@ -5238,6 +5254,7 @@ void mlx4_vf_immed_vlan_work_handler(struct work_struct *_work)
 	u64 qp_path_mask = ((1ULL << MLX4_UPD_QP_PATH_MASK_VLAN_INDEX) |
 		       (1ULL << MLX4_UPD_QP_PATH_MASK_FVL) |
 		       (1ULL << MLX4_UPD_QP_PATH_MASK_CV) |
+		       (1ULL << MLX4_UPD_QP_PATH_MASK_SV) |
 		       (1ULL << MLX4_UPD_QP_PATH_MASK_ETH_HIDE_CQE_VLAN) |
 		       (1ULL << MLX4_UPD_QP_PATH_MASK_FEUP) |
 		       (1ULL << MLX4_UPD_QP_PATH_MASK_FVL_RX) |
@@ -5266,7 +5283,12 @@ void mlx4_vf_immed_vlan_work_handler(struct work_struct *_work)
 	else if (!work->vlan_id)
 		vlan_control = MLX4_VLAN_CTRL_ETH_TX_BLOCK_TAGGED |
 			MLX4_VLAN_CTRL_ETH_RX_BLOCK_TAGGED;
-	else
+	else if (work->vlan_proto == htons(ETH_P_8021AD))
+		vlan_control = MLX4_VLAN_CTRL_ETH_TX_BLOCK_PRIO_TAGGED |
+			MLX4_VLAN_CTRL_ETH_TX_BLOCK_TAGGED |
+			MLX4_VLAN_CTRL_ETH_RX_BLOCK_PRIO_TAGGED |
+			MLX4_VLAN_CTRL_ETH_RX_BLOCK_UNTAGGED;
+	else  /* vst 802.1Q */
 		vlan_control = MLX4_VLAN_CTRL_ETH_TX_BLOCK_TAGGED |
 			MLX4_VLAN_CTRL_ETH_RX_BLOCK_PRIO_TAGGED |
 			MLX4_VLAN_CTRL_ETH_RX_BLOCK_UNTAGGED;
@@ -5311,7 +5333,11 @@ void mlx4_vf_immed_vlan_work_handler(struct work_struct *_work)
 				upd_context->qp_context.pri_path.fvl_rx =
 					qp->fvl_rx | MLX4_FVL_RX_FORCE_ETH_VLAN;
 				upd_context->qp_context.pri_path.fl =
-					qp->pri_path_fl | MLX4_FL_CV | MLX4_FL_ETH_HIDE_CQE_VLAN;
+					qp->pri_path_fl | MLX4_FL_ETH_HIDE_CQE_VLAN;
+				if (work->vlan_proto == htons(ETH_P_8021AD))
+					upd_context->qp_context.pri_path.fl |= MLX4_FL_SV;
+				else
+					upd_context->qp_context.pri_path.fl |= MLX4_FL_CV;
 				upd_context->qp_context.pri_path.feup =
 					qp->feup | MLX4_FEUP_FORCE_ETH_UP | MLX4_FVL_FORCE_ETH_VLAN;
 				upd_context->qp_context.pri_path.sched_queue =
diff --git a/drivers/net/ethernet/mellanox/mlx4/srq.c b/drivers/net/ethernet/mellanox/mlx4/srq.c
index 67146624eb58..f44d089e2ca6 100644
--- a/drivers/net/ethernet/mellanox/mlx4/srq.c
+++ b/drivers/net/ethernet/mellanox/mlx4/srq.c
@@ -45,15 +45,12 @@ void mlx4_srq_event(struct mlx4_dev *dev, u32 srqn, int event_type)
 	struct mlx4_srq_table *srq_table = &mlx4_priv(dev)->srq_table;
 	struct mlx4_srq *srq;
 
-	spin_lock(&srq_table->lock);
-
+	rcu_read_lock();
 	srq = radix_tree_lookup(&srq_table->tree, srqn & (dev->caps.num_srqs - 1));
+	rcu_read_unlock();
 	if (srq)
 		atomic_inc(&srq->refcount);
-
-	spin_unlock(&srq_table->lock);
-
-	if (!srq) {
+	else {
 		mlx4_warn(dev, "Async event for bogus SRQ %08x\n", srqn);
 		return;
 	}
@@ -301,12 +298,11 @@ struct mlx4_srq *mlx4_srq_lookup(struct mlx4_dev *dev, u32 srqn)
 {
 	struct mlx4_srq_table *srq_table = &mlx4_priv(dev)->srq_table;
 	struct mlx4_srq *srq;
-	unsigned long flags;
 
-	spin_lock_irqsave(&srq_table->lock, flags);
+	rcu_read_lock();
 	srq = radix_tree_lookup(&srq_table->tree,
 				srqn & (dev->caps.num_srqs - 1));
-	spin_unlock_irqrestore(&srq_table->lock, flags);
+	rcu_read_unlock();
 
 	return srq;
 }
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/Makefile b/drivers/net/ethernet/mellanox/mlx5/core/Makefile
index 05cc1effc13c..0343725d7f44 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/Makefile
+++ b/drivers/net/ethernet/mellanox/mlx5/core/Makefile
@@ -3,7 +3,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 transobj.o vport.o sriov.o fs_cmd.o fs_core.o \
-		fs_counters.o rl.o
+		fs_counters.o rl.o lag.o dev.o
 
 mlx5_core-$(CONFIG_MLX5_CORE_EN) += wq.o eswitch.o eswitch_offloads.o \
 		en_main.o en_common.o en_fs.o en_ethtool.o en_tx.o \
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/cmd.c b/drivers/net/ethernet/mellanox/mlx5/core/cmd.c
index c2ec01a22d55..1e639f886021 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/cmd.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/cmd.c
@@ -294,11 +294,13 @@ static int mlx5_internal_err_ret_value(struct mlx5_core_dev *dev, u16 op,
 	case MLX5_CMD_OP_DEALLOC_Q_COUNTER:
 	case MLX5_CMD_OP_DEALLOC_PD:
 	case MLX5_CMD_OP_DEALLOC_UAR:
-	case MLX5_CMD_OP_DETTACH_FROM_MCG:
+	case MLX5_CMD_OP_DETACH_FROM_MCG:
 	case MLX5_CMD_OP_DEALLOC_XRCD:
 	case MLX5_CMD_OP_DEALLOC_TRANSPORT_DOMAIN:
 	case MLX5_CMD_OP_DELETE_VXLAN_UDP_DPORT:
 	case MLX5_CMD_OP_DELETE_L2_TABLE_ENTRY:
+	case MLX5_CMD_OP_DESTROY_LAG:
+	case MLX5_CMD_OP_DESTROY_VPORT_LAG:
 	case MLX5_CMD_OP_DESTROY_TIR:
 	case MLX5_CMD_OP_DESTROY_SQ:
 	case MLX5_CMD_OP_DESTROY_RQ:
@@ -315,6 +317,7 @@ static int mlx5_internal_err_ret_value(struct mlx5_core_dev *dev, u16 op,
 	case MLX5_CMD_OP_MODIFY_FLOW_TABLE:
 	case MLX5_CMD_OP_SET_FLOW_TABLE_ENTRY:
 	case MLX5_CMD_OP_SET_FLOW_TABLE_ROOT:
+	case MLX5_CMD_OP_DEALLOC_ENCAP_HEADER:
 		return MLX5_CMD_STAT_OK;
 
 	case MLX5_CMD_OP_QUERY_HCA_CAP:
@@ -389,6 +392,10 @@ static int mlx5_internal_err_ret_value(struct mlx5_core_dev *dev, u16 op,
 	case MLX5_CMD_OP_ADD_VXLAN_UDP_DPORT:
 	case MLX5_CMD_OP_SET_L2_TABLE_ENTRY:
 	case MLX5_CMD_OP_QUERY_L2_TABLE_ENTRY:
+	case MLX5_CMD_OP_CREATE_LAG:
+	case MLX5_CMD_OP_MODIFY_LAG:
+	case MLX5_CMD_OP_QUERY_LAG:
+	case MLX5_CMD_OP_CREATE_VPORT_LAG:
 	case MLX5_CMD_OP_CREATE_TIR:
 	case MLX5_CMD_OP_MODIFY_TIR:
 	case MLX5_CMD_OP_QUERY_TIR:
@@ -416,6 +423,7 @@ static int mlx5_internal_err_ret_value(struct mlx5_core_dev *dev, u16 op,
 	case MLX5_CMD_OP_QUERY_FLOW_TABLE_ENTRY:
 	case MLX5_CMD_OP_ALLOC_FLOW_COUNTER:
 	case MLX5_CMD_OP_QUERY_FLOW_COUNTER:
+	case MLX5_CMD_OP_ALLOC_ENCAP_HEADER:
 		*status = MLX5_DRIVER_STATUS_ABORTED;
 		*synd = MLX5_DRIVER_SYND;
 		return -EIO;
@@ -504,7 +512,7 @@ const char *mlx5_command_str(int command)
 	MLX5_COMMAND_STR_CASE(CONFIG_INT_MODERATION);
 	MLX5_COMMAND_STR_CASE(ACCESS_REG);
 	MLX5_COMMAND_STR_CASE(ATTACH_TO_MCG);
-	MLX5_COMMAND_STR_CASE(DETTACH_FROM_MCG);
+	MLX5_COMMAND_STR_CASE(DETACH_FROM_MCG);
 	MLX5_COMMAND_STR_CASE(GET_DROPPED_PACKET_LOG);
 	MLX5_COMMAND_STR_CASE(MAD_IFC);
 	MLX5_COMMAND_STR_CASE(QUERY_MAD_DEMUX);
@@ -526,6 +534,12 @@ const char *mlx5_command_str(int command)
 	MLX5_COMMAND_STR_CASE(DELETE_L2_TABLE_ENTRY);
 	MLX5_COMMAND_STR_CASE(SET_WOL_ROL);
 	MLX5_COMMAND_STR_CASE(QUERY_WOL_ROL);
+	MLX5_COMMAND_STR_CASE(CREATE_LAG);
+	MLX5_COMMAND_STR_CASE(MODIFY_LAG);
+	MLX5_COMMAND_STR_CASE(QUERY_LAG);
+	MLX5_COMMAND_STR_CASE(DESTROY_LAG);
+	MLX5_COMMAND_STR_CASE(CREATE_VPORT_LAG);
+	MLX5_COMMAND_STR_CASE(DESTROY_VPORT_LAG);
 	MLX5_COMMAND_STR_CASE(CREATE_TIR);
 	MLX5_COMMAND_STR_CASE(MODIFY_TIR);
 	MLX5_COMMAND_STR_CASE(DESTROY_TIR);
@@ -564,15 +578,130 @@ const char *mlx5_command_str(int command)
 	MLX5_COMMAND_STR_CASE(DEALLOC_FLOW_COUNTER);
 	MLX5_COMMAND_STR_CASE(QUERY_FLOW_COUNTER);
 	MLX5_COMMAND_STR_CASE(MODIFY_FLOW_TABLE);
+	MLX5_COMMAND_STR_CASE(ALLOC_ENCAP_HEADER);
+	MLX5_COMMAND_STR_CASE(DEALLOC_ENCAP_HEADER);
 	default: return "unknown command opcode";
 	}
 }
 
+static const char *cmd_status_str(u8 status)
+{
+	switch (status) {
+	case MLX5_CMD_STAT_OK:
+		return "OK";
+	case MLX5_CMD_STAT_INT_ERR:
+		return "internal error";
+	case MLX5_CMD_STAT_BAD_OP_ERR:
+		return "bad operation";
+	case MLX5_CMD_STAT_BAD_PARAM_ERR:
+		return "bad parameter";
+	case MLX5_CMD_STAT_BAD_SYS_STATE_ERR:
+		return "bad system state";
+	case MLX5_CMD_STAT_BAD_RES_ERR:
+		return "bad resource";
+	case MLX5_CMD_STAT_RES_BUSY:
+		return "resource busy";
+	case MLX5_CMD_STAT_LIM_ERR:
+		return "limits exceeded";
+	case MLX5_CMD_STAT_BAD_RES_STATE_ERR:
+		return "bad resource state";
+	case MLX5_CMD_STAT_IX_ERR:
+		return "bad index";
+	case MLX5_CMD_STAT_NO_RES_ERR:
+		return "no resources";
+	case MLX5_CMD_STAT_BAD_INP_LEN_ERR:
+		return "bad input length";
+	case MLX5_CMD_STAT_BAD_OUTP_LEN_ERR:
+		return "bad output length";
+	case MLX5_CMD_STAT_BAD_QP_STATE_ERR:
+		return "bad QP state";
+	case MLX5_CMD_STAT_BAD_PKT_ERR:
+		return "bad packet (discarded)";
+	case MLX5_CMD_STAT_BAD_SIZE_OUTS_CQES_ERR:
+		return "bad size too many outstanding CQEs";
+	default:
+		return "unknown status";
+	}
+}
+
+static int cmd_status_to_err(u8 status)
+{
+	switch (status) {
+	case MLX5_CMD_STAT_OK:				return 0;
+	case MLX5_CMD_STAT_INT_ERR:			return -EIO;
+	case MLX5_CMD_STAT_BAD_OP_ERR:			return -EINVAL;
+	case MLX5_CMD_STAT_BAD_PARAM_ERR:		return -EINVAL;
+	case MLX5_CMD_STAT_BAD_SYS_STATE_ERR:		return -EIO;
+	case MLX5_CMD_STAT_BAD_RES_ERR:			return -EINVAL;
+	case MLX5_CMD_STAT_RES_BUSY:			return -EBUSY;
+	case MLX5_CMD_STAT_LIM_ERR:			return -ENOMEM;
+	case MLX5_CMD_STAT_BAD_RES_STATE_ERR:		return -EINVAL;
+	case MLX5_CMD_STAT_IX_ERR:			return -EINVAL;
+	case MLX5_CMD_STAT_NO_RES_ERR:			return -EAGAIN;
+	case MLX5_CMD_STAT_BAD_INP_LEN_ERR:		return -EIO;
+	case MLX5_CMD_STAT_BAD_OUTP_LEN_ERR:		return -EIO;
+	case MLX5_CMD_STAT_BAD_QP_STATE_ERR:		return -EINVAL;
+	case MLX5_CMD_STAT_BAD_PKT_ERR:			return -EINVAL;
+	case MLX5_CMD_STAT_BAD_SIZE_OUTS_CQES_ERR:	return -EINVAL;
+	default:					return -EIO;
+	}
+}
+
+struct mlx5_ifc_mbox_out_bits {
+	u8         status[0x8];
+	u8         reserved_at_8[0x18];
+
+	u8         syndrome[0x20];
+
+	u8         reserved_at_40[0x40];
+};
+
+struct mlx5_ifc_mbox_in_bits {
+	u8         opcode[0x10];
+	u8         reserved_at_10[0x10];
+
+	u8         reserved_at_20[0x10];
+	u8         op_mod[0x10];
+
+	u8         reserved_at_40[0x40];
+};
+
+void mlx5_cmd_mbox_status(void *out, u8 *status, u32 *syndrome)
+{
+	*status = MLX5_GET(mbox_out, out, status);
+	*syndrome = MLX5_GET(mbox_out, out, syndrome);
+}
+
+static int mlx5_cmd_check(struct mlx5_core_dev *dev, void *in, void *out)
+{
+	u32 syndrome;
+	u8  status;
+	u16 opcode;
+	u16 op_mod;
+
+	mlx5_cmd_mbox_status(out, &status, &syndrome);
+	if (!status)
+		return 0;
+
+	opcode = MLX5_GET(mbox_in, in, opcode);
+	op_mod = MLX5_GET(mbox_in, in, op_mod);
+
+	mlx5_core_err(dev,
+		      "%s(0x%x) op_mod(0x%x) failed, status %s(0x%x), syndrome (0x%x)\n",
+		      mlx5_command_str(opcode),
+		      opcode, op_mod,
+		      cmd_status_str(status),
+		      status,
+		      syndrome);
+
+	return cmd_status_to_err(status);
+}
+
 static void dump_command(struct mlx5_core_dev *dev,
 			 struct mlx5_cmd_work_ent *ent, int input)
 {
-	u16 op = be16_to_cpu(((struct mlx5_inbox_hdr *)(ent->lay->in))->opcode);
 	struct mlx5_cmd_msg *msg = input ? ent->in : ent->out;
+	u16 op = MLX5_GET(mbox_in, ent->lay->in, opcode);
 	struct mlx5_cmd_mailbox *next = msg->next;
 	int data_only;
 	u32 offset = 0;
@@ -622,9 +751,7 @@ static void dump_command(struct mlx5_core_dev *dev,
 
 static u16 msg_to_opcode(struct mlx5_cmd_msg *in)
 {
-	struct mlx5_inbox_hdr *hdr = (struct mlx5_inbox_hdr *)(in->first.data);
-
-	return be16_to_cpu(hdr->opcode);
+	return MLX5_GET(mbox_in, in->first.data, opcode);
 }
 
 static void cb_timeout_handler(struct work_struct *work)
@@ -762,16 +889,6 @@ static int wait_func(struct mlx5_core_dev *dev, struct mlx5_cmd_work_ent *ent)
 	return err;
 }
 
-static __be32 *get_synd_ptr(struct mlx5_outbox_hdr *out)
-{
-	return &out->syndrome;
-}
-
-static u8 *get_status_ptr(struct mlx5_outbox_hdr *out)
-{
-	return &out->status;
-}
-
 /*  Notes:
  *    1. Callback functions may not sleep
  *    2. page queue commands do not support asynchrous completion
@@ -820,7 +937,7 @@ static int mlx5_cmd_invoke(struct mlx5_core_dev *dev, struct mlx5_cmd_msg *in,
 		goto out_free;
 
 	ds = ent->ts2 - ent->ts1;
-	op = be16_to_cpu(((struct mlx5_inbox_hdr *)in->first.data)->opcode);
+	op = MLX5_GET(mbox_in, in->first.data, opcode);
 	if (op < ARRAY_SIZE(cmd->stats)) {
 		stats = &cmd->stats[op];
 		spin_lock_irq(&stats->lock);
@@ -1035,7 +1152,6 @@ static ssize_t data_write(struct file *filp, const char __user *buf,
 	struct mlx5_core_dev *dev = filp->private_data;
 	struct mlx5_cmd_debug *dbg = &dev->cmd.dbg;
 	void *ptr;
-	int err;
 
 	if (*pos != 0)
 		return -EINVAL;
@@ -1043,25 +1159,15 @@ static ssize_t data_write(struct file *filp, const char __user *buf,
 	kfree(dbg->in_msg);
 	dbg->in_msg = NULL;
 	dbg->inlen = 0;
-
-	ptr = kzalloc(count, GFP_KERNEL);
-	if (!ptr)
-		return -ENOMEM;
-
-	if (copy_from_user(ptr, buf, count)) {
-		err = -EFAULT;
-		goto out;
-	}
+	ptr = memdup_user(buf, count);
+	if (IS_ERR(ptr))
+		return PTR_ERR(ptr);
 	dbg->in_msg = ptr;
 	dbg->inlen = count;
 
 	*pos = count;
 
 	return count;
-
-out:
-	kfree(ptr);
-	return err;
 }
 
 static ssize_t data_read(struct file *filp, char __user *buf, size_t count,
@@ -1321,11 +1427,16 @@ void mlx5_cmd_comp_handler(struct mlx5_core_dev *dev, u64 vec)
 				callback = ent->callback;
 				context = ent->context;
 				err = ent->ret;
-				if (!err)
+				if (!err) {
 					err = mlx5_copy_from_msg(ent->uout,
 								 ent->out,
 								 ent->uout_size);
 
+					err = err ? err : mlx5_cmd_check(dev,
+									ent->in->first.data,
+									ent->uout);
+				}
+
 				mlx5_free_cmd_msg(dev, ent->out);
 				free_msg(dev, ent->in);
 
@@ -1377,14 +1488,9 @@ static struct mlx5_cmd_msg *alloc_msg(struct mlx5_core_dev *dev, int in_size,
 	return msg;
 }
 
-static u16 opcode_from_in(struct mlx5_inbox_hdr *in)
-{
-	return be16_to_cpu(in->opcode);
-}
-
-static int is_manage_pages(struct mlx5_inbox_hdr *in)
+static int is_manage_pages(void *in)
 {
-	return be16_to_cpu(in->opcode) == MLX5_CMD_OP_MANAGE_PAGES;
+	return MLX5_GET(mbox_in, in, opcode) == MLX5_CMD_OP_MANAGE_PAGES;
 }
 
 static int cmd_exec(struct mlx5_core_dev *dev, void *in, int in_size, void *out,
@@ -1401,9 +1507,11 @@ static int cmd_exec(struct mlx5_core_dev *dev, void *in, int in_size, void *out,
 
 	if (pci_channel_offline(dev->pdev) ||
 	    dev->state == MLX5_DEVICE_STATE_INTERNAL_ERROR) {
-		err = mlx5_internal_err_ret_value(dev, opcode_from_in(in), &drv_synd, &status);
-		*get_synd_ptr(out) = cpu_to_be32(drv_synd);
-		*get_status_ptr(out) = status;
+		u16 opcode = MLX5_GET(mbox_in, in, opcode);
+
+		err = mlx5_internal_err_ret_value(dev, opcode, &drv_synd, &status);
+		MLX5_SET(mbox_out, out, status, status);
+		MLX5_SET(mbox_out, out, syndrome, drv_synd);
 		return err;
 	}
 
@@ -1457,7 +1565,10 @@ out_in:
 int mlx5_cmd_exec(struct mlx5_core_dev *dev, void *in, int in_size, void *out,
 		  int out_size)
 {
-	return cmd_exec(dev, in, in_size, out, out_size, NULL, NULL);
+	int err;
+
+	err = cmd_exec(dev, in, in_size, out, out_size, NULL, NULL);
+	return err ? : mlx5_cmd_check(dev, in, out);
 }
 EXPORT_SYMBOL(mlx5_cmd_exec);
 
@@ -1694,96 +1805,3 @@ void mlx5_cmd_cleanup(struct mlx5_core_dev *dev)
 	pci_pool_destroy(cmd->pool);
 }
 EXPORT_SYMBOL(mlx5_cmd_cleanup);
-
-static const char *cmd_status_str(u8 status)
-{
-	switch (status) {
-	case MLX5_CMD_STAT_OK:
-		return "OK";
-	case MLX5_CMD_STAT_INT_ERR:
-		return "internal error";
-	case MLX5_CMD_STAT_BAD_OP_ERR:
-		return "bad operation";
-	case MLX5_CMD_STAT_BAD_PARAM_ERR:
-		return "bad parameter";
-	case MLX5_CMD_STAT_BAD_SYS_STATE_ERR:
-		return "bad system state";
-	case MLX5_CMD_STAT_BAD_RES_ERR:
-		return "bad resource";
-	case MLX5_CMD_STAT_RES_BUSY:
-		return "resource busy";
-	case MLX5_CMD_STAT_LIM_ERR:
-		return "limits exceeded";
-	case MLX5_CMD_STAT_BAD_RES_STATE_ERR:
-		return "bad resource state";
-	case MLX5_CMD_STAT_IX_ERR:
-		return "bad index";
-	case MLX5_CMD_STAT_NO_RES_ERR:
-		return "no resources";
-	case MLX5_CMD_STAT_BAD_INP_LEN_ERR:
-		return "bad input length";
-	case MLX5_CMD_STAT_BAD_OUTP_LEN_ERR:
-		return "bad output length";
-	case MLX5_CMD_STAT_BAD_QP_STATE_ERR:
-		return "bad QP state";
-	case MLX5_CMD_STAT_BAD_PKT_ERR:
-		return "bad packet (discarded)";
-	case MLX5_CMD_STAT_BAD_SIZE_OUTS_CQES_ERR:
-		return "bad size too many outstanding CQEs";
-	default:
-		return "unknown status";
-	}
-}
-
-static int cmd_status_to_err(u8 status)
-{
-	switch (status) {
-	case MLX5_CMD_STAT_OK:				return 0;
-	case MLX5_CMD_STAT_INT_ERR:			return -EIO;
-	case MLX5_CMD_STAT_BAD_OP_ERR:			return -EINVAL;
-	case MLX5_CMD_STAT_BAD_PARAM_ERR:		return -EINVAL;
-	case MLX5_CMD_STAT_BAD_SYS_STATE_ERR:		return -EIO;
-	case MLX5_CMD_STAT_BAD_RES_ERR:			return -EINVAL;
-	case MLX5_CMD_STAT_RES_BUSY:			return -EBUSY;
-	case MLX5_CMD_STAT_LIM_ERR:			return -ENOMEM;
-	case MLX5_CMD_STAT_BAD_RES_STATE_ERR:		return -EINVAL;
-	case MLX5_CMD_STAT_IX_ERR:			return -EINVAL;
-	case MLX5_CMD_STAT_NO_RES_ERR:			return -EAGAIN;
-	case MLX5_CMD_STAT_BAD_INP_LEN_ERR:		return -EIO;
-	case MLX5_CMD_STAT_BAD_OUTP_LEN_ERR:		return -EIO;
-	case MLX5_CMD_STAT_BAD_QP_STATE_ERR:		return -EINVAL;
-	case MLX5_CMD_STAT_BAD_PKT_ERR:			return -EINVAL;
-	case MLX5_CMD_STAT_BAD_SIZE_OUTS_CQES_ERR:	return -EINVAL;
-	default:					return -EIO;
-	}
-}
-
-/* this will be available till all the commands use set/get macros */
-int mlx5_cmd_status_to_err(struct mlx5_outbox_hdr *hdr)
-{
-	if (!hdr->status)
-		return 0;
-
-	pr_warn("command failed, status %s(0x%x), syndrome 0x%x\n",
-		cmd_status_str(hdr->status), hdr->status,
-		be32_to_cpu(hdr->syndrome));
-
-	return cmd_status_to_err(hdr->status);
-}
-
-int mlx5_cmd_status_to_err_v2(void *ptr)
-{
-	u32	syndrome;
-	u8	status;
-
-	status = be32_to_cpu(*(__be32 *)ptr) >> 24;
-	if (!status)
-		return 0;
-
-	syndrome = be32_to_cpu(*(__be32 *)(ptr + 4));
-
-	pr_warn("command failed, status %s(0x%x), syndrome 0x%x\n",
-		cmd_status_str(status), status, syndrome);
-
-	return cmd_status_to_err(status);
-}
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/cq.c b/drivers/net/ethernet/mellanox/mlx5/core/cq.c
index 873a631ad155..32d4af9b594d 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/cq.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/cq.c
@@ -134,33 +134,29 @@ void mlx5_cq_event(struct mlx5_core_dev *dev, u32 cqn, int event_type)
 		complete(&cq->free);
 }
 
-
 int mlx5_core_create_cq(struct mlx5_core_dev *dev, struct mlx5_core_cq *cq,
-			struct mlx5_create_cq_mbox_in *in, int inlen)
+			u32 *in, int inlen)
 {
-	int err;
 	struct mlx5_cq_table *table = &dev->priv.cq_table;
-	struct mlx5_create_cq_mbox_out out;
-	struct mlx5_destroy_cq_mbox_in din;
-	struct mlx5_destroy_cq_mbox_out dout;
+	u32 out[MLX5_ST_SZ_DW(create_cq_out)];
+	u32 din[MLX5_ST_SZ_DW(destroy_cq_in)];
+	u32 dout[MLX5_ST_SZ_DW(destroy_cq_out)];
 	int eqn = MLX5_GET(cqc, MLX5_ADDR_OF(create_cq_in, in, cq_context),
 			   c_eqn);
 	struct mlx5_eq *eq;
+	int err;
 
 	eq = mlx5_eqn2eq(dev, eqn);
 	if (IS_ERR(eq))
 		return PTR_ERR(eq);
 
-	in->hdr.opcode = cpu_to_be16(MLX5_CMD_OP_CREATE_CQ);
-	memset(&out, 0, sizeof(out));
-	err = mlx5_cmd_exec(dev, in, inlen, &out, sizeof(out));
+	memset(out, 0, sizeof(out));
+	MLX5_SET(create_cq_in, in, opcode, MLX5_CMD_OP_CREATE_CQ);
+	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);
-
-	cq->cqn = be32_to_cpu(out.cqn) & 0xffffff;
+	cq->cqn = MLX5_GET(create_cq_out, out, cqn);
 	cq->cons_index = 0;
 	cq->arm_sn     = 0;
 	atomic_set(&cq->refcount, 1);
@@ -186,10 +182,11 @@ int mlx5_core_create_cq(struct mlx5_core_dev *dev, struct mlx5_core_cq *cq,
 	return 0;
 
 err_cmd:
-	memset(&din, 0, sizeof(din));
-	memset(&dout, 0, sizeof(dout));
-	din.hdr.opcode = cpu_to_be16(MLX5_CMD_OP_DESTROY_CQ);
-	mlx5_cmd_exec(dev, &din, sizeof(din), &dout, sizeof(dout));
+	memset(din, 0, sizeof(din));
+	memset(dout, 0, sizeof(dout));
+	MLX5_SET(destroy_cq_in, din, opcode, MLX5_CMD_OP_DESTROY_CQ);
+	MLX5_SET(destroy_cq_in, din, cqn, cq->cqn);
+	mlx5_cmd_exec(dev, din, sizeof(din), dout, sizeof(dout));
 	return err;
 }
 EXPORT_SYMBOL(mlx5_core_create_cq);
@@ -197,8 +194,8 @@ EXPORT_SYMBOL(mlx5_core_create_cq);
 int mlx5_core_destroy_cq(struct mlx5_core_dev *dev, struct mlx5_core_cq *cq)
 {
 	struct mlx5_cq_table *table = &dev->priv.cq_table;
-	struct mlx5_destroy_cq_mbox_in in;
-	struct mlx5_destroy_cq_mbox_out out;
+	u32 out[MLX5_ST_SZ_DW(destroy_cq_out)] = {0};
+	u32 in[MLX5_ST_SZ_DW(destroy_cq_in)] = {0};
 	struct mlx5_core_cq *tmp;
 	int err;
 
@@ -214,17 +211,12 @@ int mlx5_core_destroy_cq(struct mlx5_core_dev *dev, struct mlx5_core_cq *cq)
 		return -EINVAL;
 	}
 
-	memset(&in, 0, sizeof(in));
-	memset(&out, 0, sizeof(out));
-	in.hdr.opcode = cpu_to_be16(MLX5_CMD_OP_DESTROY_CQ);
-	in.cqn = cpu_to_be32(cq->cqn);
-	err = mlx5_cmd_exec(dev, &in, sizeof(in), &out, sizeof(out));
+	MLX5_SET(destroy_cq_in, in, opcode, MLX5_CMD_OP_DESTROY_CQ);
+	MLX5_SET(destroy_cq_in, in, cqn, cq->cqn);
+	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);
-
 	synchronize_irq(cq->irqn);
 
 	mlx5_debug_cq_remove(dev, cq);
@@ -237,44 +229,23 @@ int mlx5_core_destroy_cq(struct mlx5_core_dev *dev, struct mlx5_core_cq *cq)
 EXPORT_SYMBOL(mlx5_core_destroy_cq);
 
 int mlx5_core_query_cq(struct mlx5_core_dev *dev, struct mlx5_core_cq *cq,
-		       struct mlx5_query_cq_mbox_out *out)
+		       u32 *out, int outlen)
 {
-	struct mlx5_query_cq_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_CQ);
-	in.cqn = cpu_to_be32(cq->cqn);
-	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);
+	u32 in[MLX5_ST_SZ_DW(query_cq_in)] = {0};
 
-	return err;
+	MLX5_SET(query_cq_in, in, opcode, MLX5_CMD_OP_QUERY_CQ);
+	MLX5_SET(query_cq_in, in, cqn, cq->cqn);
+	return mlx5_cmd_exec(dev, in, sizeof(in), out, outlen);
 }
 EXPORT_SYMBOL(mlx5_core_query_cq);
 
-
 int mlx5_core_modify_cq(struct mlx5_core_dev *dev, struct mlx5_core_cq *cq,
-			struct mlx5_modify_cq_mbox_in *in, int in_sz)
+			u32 *in, int inlen)
 {
-	struct mlx5_modify_cq_mbox_out out;
-	int err;
-
-	memset(&out, 0, sizeof(out));
-	in->hdr.opcode = cpu_to_be16(MLX5_CMD_OP_MODIFY_CQ);
-	err = mlx5_cmd_exec(dev, in, in_sz, &out, sizeof(out));
-	if (err)
-		return err;
+	u32 out[MLX5_ST_SZ_DW(modify_cq_out)] = {0};
 
-	if (out.hdr.status)
-		return mlx5_cmd_status_to_err(&out.hdr);
-
-	return 0;
+	MLX5_SET(modify_cq_in, in, opcode, MLX5_CMD_OP_MODIFY_CQ);
+	return mlx5_cmd_exec(dev, in, inlen, out, sizeof(out));
 }
 EXPORT_SYMBOL(mlx5_core_modify_cq);
 
@@ -283,18 +254,20 @@ int mlx5_core_modify_cq_moderation(struct mlx5_core_dev *dev,
 				   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));
+	u32 in[MLX5_ST_SZ_DW(modify_cq_in)] = {0};
+	void *cqc;
+
+	MLX5_SET(modify_cq_in, in, cqn, cq->cqn);
+	cqc = MLX5_ADDR_OF(modify_cq_in, in, cq_context);
+	MLX5_SET(cqc, cqc, cq_period, cq_period);
+	MLX5_SET(cqc, cqc, cq_max_count, cq_max_count);
+	MLX5_SET(modify_cq_in, in,
+		 modify_field_select_resize_field_select.modify_field_select.modify_field_select,
+		 MLX5_CQ_MODIFY_PERIOD | MLX5_CQ_MODIFY_COUNT);
+
+	return mlx5_core_modify_cq(dev, cq, in, sizeof(in));
 }
+EXPORT_SYMBOL(mlx5_core_modify_cq_moderation);
 
 int mlx5_init_cq_table(struct mlx5_core_dev *dev)
 {
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/debugfs.c b/drivers/net/ethernet/mellanox/mlx5/core/debugfs.c
index 5210d92e6bc7..e94a9532e218 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/debugfs.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/debugfs.c
@@ -277,24 +277,28 @@ void mlx5_cq_debugfs_cleanup(struct mlx5_core_dev *dev)
 static u64 qp_read_field(struct mlx5_core_dev *dev, struct mlx5_core_qp *qp,
 			 int index, int *is_str)
 {
-	struct mlx5_query_qp_mbox_out *out;
+	int outlen = MLX5_ST_SZ_BYTES(query_qp_out);
 	struct mlx5_qp_context *ctx;
 	u64 param = 0;
+	u32 *out;
 	int err;
 	int no_sq;
 
-	out = kzalloc(sizeof(*out), GFP_KERNEL);
+	out = kzalloc(outlen, GFP_KERNEL);
 	if (!out)
 		return param;
 
-	err = mlx5_core_qp_query(dev, qp, out, sizeof(*out));
+	err = mlx5_core_qp_query(dev, qp, out, outlen);
 	if (err) {
-		mlx5_core_warn(dev, "failed to query qp\n");
+		mlx5_core_warn(dev, "failed to query qp err=%d\n", err);
 		goto out;
 	}
 
 	*is_str = 0;
-	ctx = &out->ctx;
+
+	/* FIXME: use MLX5_GET rather than mlx5_qp_context manual struct */
+	ctx = (struct mlx5_qp_context *)MLX5_ADDR_OF(query_qp_out, out, qpc);
+
 	switch (index) {
 	case QP_PID:
 		param = qp->pid;
@@ -358,32 +362,32 @@ out:
 static u64 eq_read_field(struct mlx5_core_dev *dev, struct mlx5_eq *eq,
 			 int index)
 {
-	struct mlx5_query_eq_mbox_out *out;
-	struct mlx5_eq_context *ctx;
+	int outlen = MLX5_ST_SZ_BYTES(query_eq_out);
 	u64 param = 0;
+	void *ctx;
+	u32 *out;
 	int err;
 
-	out = kzalloc(sizeof(*out), GFP_KERNEL);
+	out = kzalloc(outlen, GFP_KERNEL);
 	if (!out)
 		return param;
 
-	ctx = &out->ctx;
-
-	err = mlx5_core_eq_query(dev, eq, out, sizeof(*out));
+	err = mlx5_core_eq_query(dev, eq, out, outlen);
 	if (err) {
 		mlx5_core_warn(dev, "failed to query eq\n");
 		goto out;
 	}
+	ctx = MLX5_ADDR_OF(query_eq_out, out, eq_context_entry);
 
 	switch (index) {
 	case EQ_NUM_EQES:
-		param = 1 << ((be32_to_cpu(ctx->log_sz_usr_page) >> 24) & 0x1f);
+		param = 1 << MLX5_GET(eqc, ctx, log_eq_size);
 		break;
 	case EQ_INTR:
-		param = ctx->intr;
+		param = MLX5_GET(eqc, ctx, intr);
 		break;
 	case EQ_LOG_PG_SZ:
-		param = (ctx->log_page_size & 0x1f) + 12;
+		param = MLX5_GET(eqc, ctx, log_page_size) + 12;
 		break;
 	}
 
@@ -395,37 +399,37 @@ out:
 static u64 cq_read_field(struct mlx5_core_dev *dev, struct mlx5_core_cq *cq,
 			 int index)
 {
-	struct mlx5_query_cq_mbox_out *out;
-	struct mlx5_cq_context *ctx;
+	int outlen = MLX5_ST_SZ_BYTES(query_cq_out);
 	u64 param = 0;
+	void *ctx;
+	u32 *out;
 	int err;
 
-	out = kzalloc(sizeof(*out), GFP_KERNEL);
+	out = mlx5_vzalloc(outlen);
 	if (!out)
 		return param;
 
-	ctx = &out->ctx;
-
-	err = mlx5_core_query_cq(dev, cq, out);
+	err = mlx5_core_query_cq(dev, cq, out, outlen);
 	if (err) {
 		mlx5_core_warn(dev, "failed to query cq\n");
 		goto out;
 	}
+	ctx = MLX5_ADDR_OF(query_cq_out, out, cq_context);
 
 	switch (index) {
 	case CQ_PID:
 		param = cq->pid;
 		break;
 	case CQ_NUM_CQES:
-		param = 1 << ((be32_to_cpu(ctx->log_sz_usr_page) >> 24) & 0x1f);
+		param = 1 << MLX5_GET(cqc, ctx, log_cq_size);
 		break;
 	case CQ_LOG_PG_SZ:
-		param = (ctx->log_pg_sz & 0x1f) + 12;
+		param = MLX5_GET(cqc, ctx, log_page_size);
 		break;
 	}
 
 out:
-	kfree(out);
+	kvfree(out);
 	return param;
 }
 
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/dev.c b/drivers/net/ethernet/mellanox/mlx5/core/dev.c
new file mode 100644
index 000000000000..a9dbc28f6b97
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/dev.c
@@ -0,0 +1,345 @@
+/*
+ * Copyright (c) 2013-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/driver.h>
+#include "mlx5_core.h"
+
+static LIST_HEAD(intf_list);
+static LIST_HEAD(mlx5_dev_list);
+/* intf dev list mutex */
+static DEFINE_MUTEX(mlx5_intf_mutex);
+
+struct mlx5_device_context {
+	struct list_head	list;
+	struct mlx5_interface  *intf;
+	void		       *context;
+	unsigned long		state;
+};
+
+enum {
+	MLX5_INTERFACE_ADDED,
+	MLX5_INTERFACE_ATTACHED,
+};
+
+void mlx5_add_device(struct mlx5_interface *intf, struct mlx5_priv *priv)
+{
+	struct mlx5_device_context *dev_ctx;
+	struct mlx5_core_dev *dev = container_of(priv, struct mlx5_core_dev, priv);
+
+	if (!mlx5_lag_intf_add(intf, priv))
+		return;
+
+	dev_ctx = kzalloc(sizeof(*dev_ctx), GFP_KERNEL);
+	if (!dev_ctx)
+		return;
+
+	dev_ctx->intf = intf;
+	dev_ctx->context = intf->add(dev);
+	set_bit(MLX5_INTERFACE_ADDED, &dev_ctx->state);
+	if (intf->attach)
+		set_bit(MLX5_INTERFACE_ATTACHED, &dev_ctx->state);
+
+	if (dev_ctx->context) {
+		spin_lock_irq(&priv->ctx_lock);
+		list_add_tail(&dev_ctx->list, &priv->ctx_list);
+		spin_unlock_irq(&priv->ctx_lock);
+	} else {
+		kfree(dev_ctx);
+	}
+}
+
+static struct mlx5_device_context *mlx5_get_device(struct mlx5_interface *intf,
+						   struct mlx5_priv *priv)
+{
+	struct mlx5_device_context *dev_ctx;
+
+	list_for_each_entry(dev_ctx, &priv->ctx_list, list)
+		if (dev_ctx->intf == intf)
+			return dev_ctx;
+	return NULL;
+}
+
+void mlx5_remove_device(struct mlx5_interface *intf, struct mlx5_priv *priv)
+{
+	struct mlx5_device_context *dev_ctx;
+	struct mlx5_core_dev *dev = container_of(priv, struct mlx5_core_dev, priv);
+
+	dev_ctx = mlx5_get_device(intf, priv);
+	if (!dev_ctx)
+		return;
+
+	spin_lock_irq(&priv->ctx_lock);
+	list_del(&dev_ctx->list);
+	spin_unlock_irq(&priv->ctx_lock);
+
+	if (test_bit(MLX5_INTERFACE_ADDED, &dev_ctx->state))
+		intf->remove(dev, dev_ctx->context);
+
+	kfree(dev_ctx);
+}
+
+static void mlx5_attach_interface(struct mlx5_interface *intf, struct mlx5_priv *priv)
+{
+	struct mlx5_device_context *dev_ctx;
+	struct mlx5_core_dev *dev = container_of(priv, struct mlx5_core_dev, priv);
+
+	dev_ctx = mlx5_get_device(intf, priv);
+	if (!dev_ctx)
+		return;
+
+	if (intf->attach) {
+		if (test_bit(MLX5_INTERFACE_ATTACHED, &dev_ctx->state))
+			return;
+		intf->attach(dev, dev_ctx->context);
+		set_bit(MLX5_INTERFACE_ATTACHED, &dev_ctx->state);
+	} else {
+		if (test_bit(MLX5_INTERFACE_ADDED, &dev_ctx->state))
+			return;
+		dev_ctx->context = intf->add(dev);
+		set_bit(MLX5_INTERFACE_ADDED, &dev_ctx->state);
+	}
+}
+
+void mlx5_attach_device(struct mlx5_core_dev *dev)
+{
+	struct mlx5_priv *priv = &dev->priv;
+	struct mlx5_interface *intf;
+
+	mutex_lock(&mlx5_intf_mutex);
+	list_for_each_entry(intf, &intf_list, list)
+		mlx5_attach_interface(intf, priv);
+	mutex_unlock(&mlx5_intf_mutex);
+}
+
+static void mlx5_detach_interface(struct mlx5_interface *intf, struct mlx5_priv *priv)
+{
+	struct mlx5_device_context *dev_ctx;
+	struct mlx5_core_dev *dev = container_of(priv, struct mlx5_core_dev, priv);
+
+	dev_ctx = mlx5_get_device(intf, priv);
+	if (!dev_ctx)
+		return;
+
+	if (intf->detach) {
+		if (!test_bit(MLX5_INTERFACE_ATTACHED, &dev_ctx->state))
+			return;
+		intf->detach(dev, dev_ctx->context);
+		clear_bit(MLX5_INTERFACE_ATTACHED, &dev_ctx->state);
+	} else {
+		if (!test_bit(MLX5_INTERFACE_ADDED, &dev_ctx->state))
+			return;
+		intf->remove(dev, dev_ctx->context);
+		clear_bit(MLX5_INTERFACE_ADDED, &dev_ctx->state);
+	}
+}
+
+void mlx5_detach_device(struct mlx5_core_dev *dev)
+{
+	struct mlx5_priv *priv = &dev->priv;
+	struct mlx5_interface *intf;
+
+	mutex_lock(&mlx5_intf_mutex);
+	list_for_each_entry(intf, &intf_list, list)
+		mlx5_detach_interface(intf, priv);
+	mutex_unlock(&mlx5_intf_mutex);
+}
+
+bool mlx5_device_registered(struct mlx5_core_dev *dev)
+{
+	struct mlx5_priv *priv;
+	bool found = false;
+
+	mutex_lock(&mlx5_intf_mutex);
+	list_for_each_entry(priv, &mlx5_dev_list, dev_list)
+		if (priv == &dev->priv)
+			found = true;
+	mutex_unlock(&mlx5_intf_mutex);
+
+	return found;
+}
+
+int mlx5_register_device(struct mlx5_core_dev *dev)
+{
+	struct mlx5_priv *priv = &dev->priv;
+	struct mlx5_interface *intf;
+
+	mutex_lock(&mlx5_intf_mutex);
+	list_add_tail(&priv->dev_list, &mlx5_dev_list);
+	list_for_each_entry(intf, &intf_list, list)
+		mlx5_add_device(intf, priv);
+	mutex_unlock(&mlx5_intf_mutex);
+
+	return 0;
+}
+
+void mlx5_unregister_device(struct mlx5_core_dev *dev)
+{
+	struct mlx5_priv *priv = &dev->priv;
+	struct mlx5_interface *intf;
+
+	mutex_lock(&mlx5_intf_mutex);
+	list_for_each_entry(intf, &intf_list, list)
+		mlx5_remove_device(intf, priv);
+	list_del(&priv->dev_list);
+	mutex_unlock(&mlx5_intf_mutex);
+}
+
+int mlx5_register_interface(struct mlx5_interface *intf)
+{
+	struct mlx5_priv *priv;
+
+	if (!intf->add || !intf->remove)
+		return -EINVAL;
+
+	mutex_lock(&mlx5_intf_mutex);
+	list_add_tail(&intf->list, &intf_list);
+	list_for_each_entry(priv, &mlx5_dev_list, dev_list)
+		mlx5_add_device(intf, priv);
+	mutex_unlock(&mlx5_intf_mutex);
+
+	return 0;
+}
+EXPORT_SYMBOL(mlx5_register_interface);
+
+void mlx5_unregister_interface(struct mlx5_interface *intf)
+{
+	struct mlx5_priv *priv;
+
+	mutex_lock(&mlx5_intf_mutex);
+	list_for_each_entry(priv, &mlx5_dev_list, dev_list)
+		mlx5_remove_device(intf, priv);
+	list_del(&intf->list);
+	mutex_unlock(&mlx5_intf_mutex);
+}
+EXPORT_SYMBOL(mlx5_unregister_interface);
+
+void *mlx5_get_protocol_dev(struct mlx5_core_dev *mdev, int protocol)
+{
+	struct mlx5_priv *priv = &mdev->priv;
+	struct mlx5_device_context *dev_ctx;
+	unsigned long flags;
+	void *result = NULL;
+
+	spin_lock_irqsave(&priv->ctx_lock, flags);
+
+	list_for_each_entry(dev_ctx, &mdev->priv.ctx_list, list)
+		if ((dev_ctx->intf->protocol == protocol) &&
+		    dev_ctx->intf->get_dev) {
+			result = dev_ctx->intf->get_dev(dev_ctx->context);
+			break;
+		}
+
+	spin_unlock_irqrestore(&priv->ctx_lock, flags);
+
+	return result;
+}
+EXPORT_SYMBOL(mlx5_get_protocol_dev);
+
+/* Must be called with intf_mutex held */
+void mlx5_add_dev_by_protocol(struct mlx5_core_dev *dev, int protocol)
+{
+	struct mlx5_interface *intf;
+
+	list_for_each_entry(intf, &intf_list, list)
+		if (intf->protocol == protocol) {
+			mlx5_add_device(intf, &dev->priv);
+			break;
+		}
+}
+
+/* Must be called with intf_mutex held */
+void mlx5_remove_dev_by_protocol(struct mlx5_core_dev *dev, int protocol)
+{
+	struct mlx5_interface *intf;
+
+	list_for_each_entry(intf, &intf_list, list)
+		if (intf->protocol == protocol) {
+			mlx5_remove_device(intf, &dev->priv);
+			break;
+		}
+}
+
+static u16 mlx5_gen_pci_id(struct mlx5_core_dev *dev)
+{
+	return (u16)((dev->pdev->bus->number << 8) |
+		     PCI_SLOT(dev->pdev->devfn));
+}
+
+/* Must be called with intf_mutex held */
+struct mlx5_core_dev *mlx5_get_next_phys_dev(struct mlx5_core_dev *dev)
+{
+	u16 pci_id = mlx5_gen_pci_id(dev);
+	struct mlx5_core_dev *res = NULL;
+	struct mlx5_core_dev *tmp_dev;
+	struct mlx5_priv *priv;
+
+	list_for_each_entry(priv, &mlx5_dev_list, dev_list) {
+		tmp_dev = container_of(priv, struct mlx5_core_dev, priv);
+		if ((dev != tmp_dev) && (mlx5_gen_pci_id(tmp_dev) == pci_id)) {
+			res = tmp_dev;
+			break;
+		}
+	}
+
+	return res;
+}
+
+void mlx5_core_event(struct mlx5_core_dev *dev, enum mlx5_dev_event event,
+		     unsigned long param)
+{
+	struct mlx5_priv *priv = &dev->priv;
+	struct mlx5_device_context *dev_ctx;
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->ctx_lock, flags);
+
+	list_for_each_entry(dev_ctx, &priv->ctx_list, list)
+		if (dev_ctx->intf->event)
+			dev_ctx->intf->event(dev, dev_ctx->context, event, param);
+
+	spin_unlock_irqrestore(&priv->ctx_lock, flags);
+}
+
+void mlx5_dev_list_lock(void)
+{
+	mutex_lock(&mlx5_intf_mutex);
+}
+
+void mlx5_dev_list_unlock(void)
+{
+	mutex_unlock(&mlx5_intf_mutex);
+}
+
+int mlx5_dev_list_trylock(void)
+{
+	return mutex_trylock(&mlx5_intf_mutex);
+}
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en.h b/drivers/net/ethernet/mellanox/mlx5/core/en.h
index bf722aa88cf0..460363b66cb1 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en.h
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en.h
@@ -62,12 +62,14 @@
 #define MLX5E_PARAMS_MAXIMUM_LOG_RQ_SIZE                0xd
 
 #define MLX5E_PARAMS_MINIMUM_LOG_RQ_SIZE_MPW            0x1
-#define MLX5E_PARAMS_DEFAULT_LOG_RQ_SIZE_MPW            0x4
+#define MLX5E_PARAMS_DEFAULT_LOG_RQ_SIZE_MPW            0x3
 #define MLX5E_PARAMS_MAXIMUM_LOG_RQ_SIZE_MPW            0x6
 
+#define MLX5_RX_HEADROOM NET_SKB_PAD
+
 #define MLX5_MPWRQ_LOG_STRIDE_SIZE		6  /* >= 6, HW restriction */
 #define MLX5_MPWRQ_LOG_STRIDE_SIZE_CQE_COMPRESS	8  /* >= 6, HW restriction */
-#define MLX5_MPWRQ_LOG_WQE_SZ			17
+#define MLX5_MPWRQ_LOG_WQE_SZ			18
 #define MLX5_MPWRQ_WQE_PAGE_ORDER  (MLX5_MPWRQ_LOG_WQE_SZ - PAGE_SHIFT > 0 ? \
 				    MLX5_MPWRQ_LOG_WQE_SZ - PAGE_SHIFT : 0)
 #define MLX5_MPWRQ_PAGES_PER_WQE		BIT(MLX5_MPWRQ_WQE_PAGE_ORDER)
@@ -99,6 +101,18 @@
 #define MLX5E_UPDATE_STATS_INTERVAL    200 /* msecs */
 #define MLX5E_SQ_BF_BUDGET             16
 
+#define MLX5E_ICOSQ_MAX_WQEBBS \
+	(DIV_ROUND_UP(sizeof(struct mlx5e_umr_wqe), MLX5_SEND_WQE_BB))
+
+#define MLX5E_XDP_MIN_INLINE (ETH_HLEN + VLAN_HLEN)
+#define MLX5E_XDP_IHS_DS_COUNT \
+	DIV_ROUND_UP(MLX5E_XDP_MIN_INLINE - 2, MLX5_SEND_WQE_DS)
+#define MLX5E_XDP_TX_DS_COUNT \
+	(MLX5E_XDP_IHS_DS_COUNT + \
+	 (sizeof(struct mlx5e_tx_wqe) / MLX5_SEND_WQE_DS) + 1 /* SG DS */)
+#define MLX5E_XDP_TX_WQEBBS \
+	DIV_ROUND_UP(MLX5E_XDP_TX_DS_COUNT, MLX5_SEND_WQEBB_NUM_DS)
+
 #define MLX5E_NUM_MAIN_GROUPS 9
 
 static inline u16 mlx5_min_rx_wqes(int wq_type, u32 wq_size)
@@ -287,29 +301,53 @@ struct mlx5e_rx_am { /* Adaptive Moderation */
 	u8					tired;
 };
 
+/* a single cache unit is capable to serve one napi call (for non-striding rq)
+ * or a MPWQE (for striding rq).
+ */
+#define MLX5E_CACHE_UNIT	(MLX5_MPWRQ_PAGES_PER_WQE > NAPI_POLL_WEIGHT ? \
+				 MLX5_MPWRQ_PAGES_PER_WQE : NAPI_POLL_WEIGHT)
+#define MLX5E_CACHE_SIZE	(2 * roundup_pow_of_two(MLX5E_CACHE_UNIT))
+struct mlx5e_page_cache {
+	u32 head;
+	u32 tail;
+	struct mlx5e_dma_info page_cache[MLX5E_CACHE_SIZE];
+};
+
 struct mlx5e_rq {
 	/* data path */
 	struct mlx5_wq_ll      wq;
-	u32                    wqe_sz;
-	struct sk_buff       **skb;
-	struct mlx5e_mpw_info *wqe_info;
+
+	union {
+		struct mlx5e_dma_info *dma_info;
+		struct {
+			struct mlx5e_mpw_info *info;
+			void                  *mtt_no_align;
+			u32                    mtt_offset;
+		} mpwqe;
+	};
+	struct {
+		u8             page_order;
+		u32            wqe_sz;    /* wqe data buffer size */
+		u8             map_dir;   /* dma map direction */
+	} buff;
 	__be32                 mkey_be;
-	__be32                 umr_mkey_be;
 
 	struct device         *pdev;
 	struct net_device     *netdev;
 	struct mlx5e_tstamp   *tstamp;
 	struct mlx5e_rq_stats  stats;
 	struct mlx5e_cq        cq;
+	struct mlx5e_page_cache page_cache;
+
 	mlx5e_fp_handle_rx_cqe handle_rx_cqe;
 	mlx5e_fp_alloc_wqe     alloc_wqe;
 	mlx5e_fp_dealloc_wqe   dealloc_wqe;
 
 	unsigned long          state;
 	int                    ix;
-	u32                    mpwqe_mtt_offset;
 
 	struct mlx5e_rx_am     am; /* Adaptive Moderation */
+	struct bpf_prog       *xdp_prog;
 
 	/* control */
 	struct mlx5_wq_ctrl    wq_ctrl;
@@ -323,32 +361,15 @@ struct mlx5e_rq {
 
 struct mlx5e_umr_dma_info {
 	__be64                *mtt;
-	__be64                *mtt_no_align;
 	dma_addr_t             mtt_addr;
-	struct mlx5e_dma_info *dma_info;
+	struct mlx5e_dma_info  dma_info[MLX5_MPWRQ_PAGES_PER_WQE];
+	struct mlx5e_umr_wqe   wqe;
 };
 
 struct mlx5e_mpw_info {
-	union {
-		struct mlx5e_dma_info     dma_info;
-		struct mlx5e_umr_dma_info umr;
-	};
+	struct mlx5e_umr_dma_info umr;
 	u16 consumed_strides;
 	u16 skbs_frags[MLX5_MPWRQ_PAGES_PER_WQE];
-
-	void (*dma_pre_sync)(struct device *pdev,
-			     struct mlx5e_mpw_info *wi,
-			     u32 wqe_offset, u32 len);
-	void (*add_skb_frag)(struct mlx5e_rq *rq,
-			     struct sk_buff *skb,
-			     struct mlx5e_mpw_info *wi,
-			     u32 page_idx, u32 frag_offset, u32 len);
-	void (*copy_skb_header)(struct device *pdev,
-				struct sk_buff *skb,
-				struct mlx5e_mpw_info *wi,
-				u32 page_idx, u32 offset,
-				u32 headlen);
-	void (*free_wqe)(struct mlx5e_rq *rq, struct mlx5e_mpw_info *wi);
 };
 
 struct mlx5e_tx_wqe_info {
@@ -373,11 +394,17 @@ enum {
 	MLX5E_SQ_STATE_BF_ENABLE,
 };
 
-struct mlx5e_ico_wqe_info {
+struct mlx5e_sq_wqe_info {
 	u8  opcode;
 	u8  num_wqebbs;
 };
 
+enum mlx5e_sq_type {
+	MLX5E_SQ_TXQ,
+	MLX5E_SQ_ICO,
+	MLX5E_SQ_XDP
+};
+
 struct mlx5e_sq {
 	/* data path */
 
@@ -395,10 +422,20 @@ struct mlx5e_sq {
 
 	struct mlx5e_cq            cq;
 
-	/* pointers to per packet info: write@xmit, read@completion */
-	struct sk_buff           **skb;
-	struct mlx5e_sq_dma       *dma_fifo;
-	struct mlx5e_tx_wqe_info  *wqe_info;
+	/* pointers to per tx element info: write@xmit, read@completion */
+	union {
+		struct {
+			struct sk_buff           **skb;
+			struct mlx5e_sq_dma       *dma_fifo;
+			struct mlx5e_tx_wqe_info  *wqe_info;
+		} txq;
+		struct mlx5e_sq_wqe_info *ico_wqe;
+		struct {
+			struct mlx5e_sq_wqe_info  *wqe_info;
+			struct mlx5e_dma_info     *di;
+			bool                       doorbell;
+		} xdp;
+	} db;
 
 	/* read only */
 	struct mlx5_wq_cyc         wq;
@@ -420,8 +457,8 @@ struct mlx5e_sq {
 	struct mlx5_uar            uar;
 	struct mlx5e_channel      *channel;
 	int                        tc;
-	struct mlx5e_ico_wqe_info *ico_wqe_info;
 	u32                        rate_limit;
+	u8                         type;
 } ____cacheline_aligned_in_smp;
 
 static inline bool mlx5e_sq_has_room_for(struct mlx5e_sq *sq, u16 n)
@@ -437,8 +474,10 @@ enum channel_flags {
 struct mlx5e_channel {
 	/* data path */
 	struct mlx5e_rq            rq;
+	struct mlx5e_sq            xdp_sq;
 	struct mlx5e_sq            sq[MLX5E_MAX_NUM_TC];
 	struct mlx5e_sq            icosq;   /* internal control operations */
+	bool                       xdp;
 	struct napi_struct         napi;
 	struct device             *pdev;
 	struct net_device         *netdev;
@@ -620,6 +659,7 @@ struct mlx5e_priv {
 	/* priv data path fields - start */
 	struct mlx5e_sq            **txq_to_sq_map;
 	int channeltc_to_txq_map[MLX5E_MAX_NUM_CHANNELS][MLX5E_MAX_NUM_TC];
+	struct bpf_prog *xdp_prog;
 	/* priv data path fields - end */
 
 	unsigned long              state;
@@ -654,40 +694,6 @@ struct mlx5e_priv {
 	void                      *ppriv;
 };
 
-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_50GBASE_SR2	 = 18,
-	MLX5E_100GBASE_CR4	 = 20,
-	MLX5E_100GBASE_SR4	 = 21,
-	MLX5E_100GBASE_KR4	 = 22,
-	MLX5E_100GBASE_LR4	 = 23,
-	MLX5E_100BASE_TX	 = 24,
-	MLX5E_1000BASE_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_build_ptys2ethtool_map(void);
 
 void mlx5e_send_nop(struct mlx5e_sq *sq, bool notify_hw);
@@ -700,30 +706,19 @@ 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, int napi_budget);
 int mlx5e_poll_rx_cq(struct mlx5e_cq *cq, int budget);
-void mlx5e_free_tx_descs(struct mlx5e_sq *sq);
+void mlx5e_free_sq_descs(struct mlx5e_sq *sq);
 
+void mlx5e_page_release(struct mlx5e_rq *rq, struct mlx5e_dma_info *dma_info,
+			bool recycle);
 void mlx5e_handle_rx_cqe(struct mlx5e_rq *rq, struct mlx5_cqe64 *cqe);
 void mlx5e_handle_rx_cqe_mpwrq(struct mlx5e_rq *rq, struct mlx5_cqe64 *cqe);
 bool mlx5e_post_rx_wqes(struct mlx5e_rq *rq);
 int mlx5e_alloc_rx_wqe(struct mlx5e_rq *rq, struct mlx5e_rx_wqe *wqe, u16 ix);
-int mlx5e_alloc_rx_mpwqe(struct mlx5e_rq *rq, struct mlx5e_rx_wqe *wqe, u16 ix);
+int mlx5e_alloc_rx_mpwqe(struct mlx5e_rq *rq, struct mlx5e_rx_wqe *wqe,	u16 ix);
 void mlx5e_dealloc_rx_wqe(struct mlx5e_rq *rq, u16 ix);
 void mlx5e_dealloc_rx_mpwqe(struct mlx5e_rq *rq, u16 ix);
-void mlx5e_post_rx_fragmented_mpwqe(struct mlx5e_rq *rq);
-void mlx5e_complete_rx_linear_mpwqe(struct mlx5e_rq *rq,
-				    struct mlx5_cqe64 *cqe,
-				    u16 byte_cnt,
-				    struct mlx5e_mpw_info *wi,
-				    struct sk_buff *skb);
-void mlx5e_complete_rx_fragmented_mpwqe(struct mlx5e_rq *rq,
-					struct mlx5_cqe64 *cqe,
-					u16 byte_cnt,
-					struct mlx5e_mpw_info *wi,
-					struct sk_buff *skb);
-void mlx5e_free_rx_linear_mpwqe(struct mlx5e_rq *rq,
-				struct mlx5e_mpw_info *wi);
-void mlx5e_free_rx_fragmented_mpwqe(struct mlx5e_rq *rq,
-				    struct mlx5e_mpw_info *wi);
+void mlx5e_post_rx_mpwqe(struct mlx5e_rq *rq);
+void mlx5e_free_rx_mpwqe(struct mlx5e_rq *rq, struct mlx5e_mpw_info *wi);
 struct mlx5_cqe64 *mlx5e_get_cqe(struct mlx5e_cq *cq);
 
 void mlx5e_rx_am(struct mlx5e_rq *rq);
@@ -810,6 +805,12 @@ static inline void mlx5e_cq_arm(struct mlx5e_cq *cq)
 	mlx5_cq_arm(mcq, MLX5_CQ_DB_REQ_NOT, mcq->uar->map, NULL, cq->wq.cc);
 }
 
+static inline u32 mlx5e_get_wqe_mtt_offset(struct mlx5e_rq *rq, u16 wqe_ix)
+{
+	return rq->mpwqe.mtt_offset +
+		wqe_ix * ALIGN(MLX5_MPWRQ_PAGES_PER_WQE, 8);
+}
+
 static inline int mlx5e_get_max_num_channels(struct mlx5_core_dev *mdev)
 {
 	return min_t(int, mdev->priv.eq_table.num_comp_vectors,
@@ -868,6 +869,7 @@ void mlx5e_nic_rep_unload(struct mlx5_eswitch *esw,
 int mlx5e_add_sqs_fwd_rules(struct mlx5e_priv *priv);
 void mlx5e_remove_sqs_fwd_rules(struct mlx5e_priv *priv);
 int mlx5e_attr_get(struct net_device *dev, struct switchdev_attr *attr);
+void mlx5e_handle_rx_cqe_rep(struct mlx5e_rq *rq, struct mlx5_cqe64 *cqe);
 
 int mlx5e_create_direct_rqts(struct mlx5e_priv *priv);
 void mlx5e_destroy_rqt(struct mlx5e_priv *priv, struct mlx5e_rqt *rqt);
@@ -878,9 +880,12 @@ void mlx5e_cleanup_nic_tx(struct mlx5e_priv *priv);
 int mlx5e_close(struct net_device *netdev);
 int mlx5e_open(struct net_device *netdev);
 void mlx5e_update_stats_work(struct work_struct *work);
-void *mlx5e_create_netdev(struct mlx5_core_dev *mdev,
-			  const struct mlx5e_profile *profile, void *ppriv);
+struct net_device *mlx5e_create_netdev(struct mlx5_core_dev *mdev,
+				       const struct mlx5e_profile *profile,
+				       void *ppriv);
 void mlx5e_destroy_netdev(struct mlx5_core_dev *mdev, struct mlx5e_priv *priv);
+int mlx5e_attach_netdev(struct mlx5_core_dev *mdev, struct net_device *netdev);
+void mlx5e_detach_netdev(struct mlx5_core_dev *mdev, struct net_device *netdev);
 struct rtnl_link_stats64 *
 mlx5e_get_stats(struct net_device *dev, struct rtnl_link_stats64 *stats);
 
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_clock.c b/drivers/net/ethernet/mellanox/mlx5/core/en_clock.c
index 847a8f3ac2b2..13dc388667b6 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_clock.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_clock.c
@@ -273,7 +273,7 @@ void mlx5e_timestamp_init(struct mlx5e_priv *priv)
 
 	tstamp->ptp = ptp_clock_register(&tstamp->ptp_info,
 					 &priv->mdev->pdev->dev);
-	if (IS_ERR_OR_NULL(tstamp->ptp)) {
+	if (IS_ERR(tstamp->ptp)) {
 		mlx5_core_warn(priv->mdev, "ptp_clock_register failed %ld\n",
 			       PTR_ERR(tstamp->ptp));
 		tstamp->ptp = NULL;
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_common.c b/drivers/net/ethernet/mellanox/mlx5/core/en_common.c
index 9cce153e1035..029e856f72a0 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_common.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_common.c
@@ -60,24 +60,27 @@ void mlx5e_destroy_tir(struct mlx5_core_dev *mdev,
 static int mlx5e_create_mkey(struct mlx5_core_dev *mdev, u32 pdn,
 			     struct mlx5_core_mkey *mkey)
 {
-	struct mlx5_create_mkey_mbox_in *in;
+	int inlen = MLX5_ST_SZ_BYTES(create_mkey_in);
+	void *mkc;
+	u32 *in;
 	int err;
 
-	in = mlx5_vzalloc(sizeof(*in));
+	in = mlx5_vzalloc(inlen);
 	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);
+	mkc = MLX5_ADDR_OF(create_mkey_in, in, memory_key_mkey_entry);
+	MLX5_SET(mkc, mkc, access_mode, MLX5_MKC_ACCESS_MODE_PA);
+	MLX5_SET(mkc, mkc, lw, 1);
+	MLX5_SET(mkc, mkc, lr, 1);
 
-	err = mlx5_core_create_mkey(mdev, mkey, in, sizeof(*in), NULL, NULL,
-				    NULL);
+	MLX5_SET(mkc, mkc, pd, pdn);
+	MLX5_SET(mkc, mkc, length64, 1);
+	MLX5_SET(mkc, mkc, qpn, 0xffffff);
 
-	kvfree(in);
+	err = mlx5_core_create_mkey(mdev, mkey, in, inlen);
 
+	kvfree(in);
 	return err;
 }
 
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_ethtool.c b/drivers/net/ethernet/mellanox/mlx5/core/en_ethtool.c
index 7a346bb2ed00..27ff401cec20 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_ethtool.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_ethtool.c
@@ -805,7 +805,7 @@ static int mlx5e_get_link_ksettings(struct net_device *netdev,
 {
 	struct mlx5e_priv *priv    = netdev_priv(netdev);
 	struct mlx5_core_dev *mdev = priv->mdev;
-	u32 out[MLX5_ST_SZ_DW(ptys_reg)];
+	u32 out[MLX5_ST_SZ_DW(ptys_reg)] = {0};
 	u32 eth_proto_cap;
 	u32 eth_proto_admin;
 	u32 eth_proto_lp;
@@ -815,7 +815,6 @@ static int mlx5e_get_link_ksettings(struct net_device *netdev,
 	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);
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_fs.c b/drivers/net/ethernet/mellanox/mlx5/core/en_fs.c
index 1587a9fd5724..36fbc6b21a33 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_fs.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_fs.c
@@ -294,6 +294,36 @@ int mlx5e_vlan_rx_kill_vid(struct net_device *dev, __always_unused __be16 proto,
 	return 0;
 }
 
+static void mlx5e_add_vlan_rules(struct mlx5e_priv *priv)
+{
+	int i;
+
+	mlx5e_add_vlan_rule(priv, MLX5E_VLAN_RULE_TYPE_UNTAGGED, 0);
+
+	for_each_set_bit(i, priv->fs.vlan.active_vlans, VLAN_N_VID) {
+		mlx5e_add_vlan_rule(priv, MLX5E_VLAN_RULE_TYPE_MATCH_VID, i);
+	}
+
+	if (priv->fs.vlan.filter_disabled &&
+	    !(priv->netdev->flags & IFF_PROMISC))
+		mlx5e_add_vlan_rule(priv, MLX5E_VLAN_RULE_TYPE_ANY_VID, 0);
+}
+
+static void mlx5e_del_vlan_rules(struct mlx5e_priv *priv)
+{
+	int i;
+
+	mlx5e_del_vlan_rule(priv, MLX5E_VLAN_RULE_TYPE_UNTAGGED, 0);
+
+	for_each_set_bit(i, priv->fs.vlan.active_vlans, VLAN_N_VID) {
+		mlx5e_del_vlan_rule(priv, MLX5E_VLAN_RULE_TYPE_MATCH_VID, i);
+	}
+
+	if (priv->fs.vlan.filter_disabled &&
+	    !(priv->netdev->flags & IFF_PROMISC))
+		mlx5e_del_vlan_rule(priv, MLX5E_VLAN_RULE_TYPE_ANY_VID, 0);
+}
+
 #define mlx5e_for_each_hash_node(hn, tmp, hash, i) \
 	for (i = 0; i < MLX5E_L2_ADDR_HASH_SIZE; i++) \
 		hlist_for_each_entry_safe(hn, tmp, &hash[i], hlist)
@@ -1024,14 +1054,10 @@ static int mlx5e_create_vlan_table(struct mlx5e_priv *priv)
 	if (err)
 		goto err_free_g;
 
-	err = mlx5e_add_vlan_rule(priv, MLX5E_VLAN_RULE_TYPE_UNTAGGED, 0);
-	if (err)
-		goto err_destroy_vlan_flow_groups;
+	mlx5e_add_vlan_rules(priv);
 
 	return 0;
 
-err_destroy_vlan_flow_groups:
-	mlx5e_destroy_groups(ft);
 err_free_g:
 	kfree(ft->g);
 err_destroy_vlan_table:
@@ -1043,6 +1069,7 @@ err_destroy_vlan_table:
 
 static void mlx5e_destroy_vlan_table(struct mlx5e_priv *priv)
 {
+	mlx5e_del_vlan_rules(priv);
 	mlx5e_destroy_flow_table(&priv->fs.vlan.ft);
 }
 
@@ -1100,7 +1127,6 @@ err_destroy_arfs_tables:
 
 void mlx5e_destroy_flow_steering(struct mlx5e_priv *priv)
 {
-	mlx5e_del_vlan_rule(priv, MLX5E_VLAN_RULE_TYPE_UNTAGGED, 0);
 	mlx5e_destroy_vlan_table(priv);
 	mlx5e_destroy_l2_table(priv);
 	mlx5e_destroy_ttc_table(priv);
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_main.c b/drivers/net/ethernet/mellanox/mlx5/core/en_main.c
index 2459c7f3db8d..7eaf38020a8f 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_main.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_main.c
@@ -34,6 +34,7 @@
 #include <net/pkt_cls.h>
 #include <linux/mlx5/fs.h>
 #include <net/vxlan.h>
+#include <linux/bpf.h>
 #include "en.h"
 #include "en_tc.h"
 #include "eswitch.h"
@@ -50,7 +51,7 @@ struct mlx5e_sq_param {
 	struct mlx5_wq_param       wq;
 	u16                        max_inline;
 	u8                         min_inline_mode;
-	bool                       icosq;
+	enum mlx5e_sq_type         type;
 };
 
 struct mlx5e_cq_param {
@@ -63,12 +64,55 @@ struct mlx5e_cq_param {
 struct mlx5e_channel_param {
 	struct mlx5e_rq_param      rq;
 	struct mlx5e_sq_param      sq;
+	struct mlx5e_sq_param      xdp_sq;
 	struct mlx5e_sq_param      icosq;
 	struct mlx5e_cq_param      rx_cq;
 	struct mlx5e_cq_param      tx_cq;
 	struct mlx5e_cq_param      icosq_cq;
 };
 
+static bool mlx5e_check_fragmented_striding_rq_cap(struct mlx5_core_dev *mdev)
+{
+	return MLX5_CAP_GEN(mdev, striding_rq) &&
+		MLX5_CAP_GEN(mdev, umr_ptr_rlky) &&
+		MLX5_CAP_ETH(mdev, reg_umr_sq);
+}
+
+static void mlx5e_set_rq_type_params(struct mlx5e_priv *priv, u8 rq_type)
+{
+	priv->params.rq_wq_type = rq_type;
+	switch (priv->params.rq_wq_type) {
+	case MLX5_WQ_TYPE_LINKED_LIST_STRIDING_RQ:
+		priv->params.log_rq_size = MLX5E_PARAMS_DEFAULT_LOG_RQ_SIZE_MPW;
+		priv->params.mpwqe_log_stride_sz = priv->params.rx_cqe_compress ?
+			MLX5_MPWRQ_LOG_STRIDE_SIZE_CQE_COMPRESS :
+			MLX5_MPWRQ_LOG_STRIDE_SIZE;
+		priv->params.mpwqe_log_num_strides = MLX5_MPWRQ_LOG_WQE_SZ -
+			priv->params.mpwqe_log_stride_sz;
+		break;
+	default: /* MLX5_WQ_TYPE_LINKED_LIST */
+		priv->params.log_rq_size = MLX5E_PARAMS_DEFAULT_LOG_RQ_SIZE;
+	}
+	priv->params.min_rx_wqes = mlx5_min_rx_wqes(priv->params.rq_wq_type,
+					       BIT(priv->params.log_rq_size));
+
+	mlx5_core_info(priv->mdev,
+		       "MLX5E: StrdRq(%d) RqSz(%ld) StrdSz(%ld) RxCqeCmprss(%d)\n",
+		       priv->params.rq_wq_type == MLX5_WQ_TYPE_LINKED_LIST_STRIDING_RQ,
+		       BIT(priv->params.log_rq_size),
+		       BIT(priv->params.mpwqe_log_stride_sz),
+		       priv->params.rx_cqe_compress_admin);
+}
+
+static void mlx5e_set_rq_priv_params(struct mlx5e_priv *priv)
+{
+	u8 rq_type = mlx5e_check_fragmented_striding_rq_cap(priv->mdev) &&
+		    !priv->xdp_prog ?
+		    MLX5_WQ_TYPE_LINKED_LIST_STRIDING_RQ :
+		    MLX5_WQ_TYPE_LINKED_LIST;
+	mlx5e_set_rq_type_params(priv, rq_type);
+}
+
 static void mlx5e_update_carrier(struct mlx5e_priv *priv)
 {
 	struct mlx5_core_dev *mdev = priv->mdev;
@@ -136,12 +180,18 @@ static void mlx5e_update_sw_counters(struct mlx5e_priv *priv)
 		s->rx_csum_none	+= rq_stats->csum_none;
 		s->rx_csum_complete += rq_stats->csum_complete;
 		s->rx_csum_unnecessary_inner += rq_stats->csum_unnecessary_inner;
+		s->rx_xdp_drop += rq_stats->xdp_drop;
+		s->rx_xdp_tx += rq_stats->xdp_tx;
+		s->rx_xdp_tx_full += rq_stats->xdp_tx_full;
 		s->rx_wqe_err   += rq_stats->wqe_err;
 		s->rx_mpwqe_filler += rq_stats->mpwqe_filler;
-		s->rx_mpwqe_frag   += rq_stats->mpwqe_frag;
 		s->rx_buff_alloc_err += rq_stats->buff_alloc_err;
 		s->rx_cqe_compress_blks += rq_stats->cqe_compress_blks;
 		s->rx_cqe_compress_pkts += rq_stats->cqe_compress_pkts;
+		s->rx_cache_reuse += rq_stats->cache_reuse;
+		s->rx_cache_full  += rq_stats->cache_full;
+		s->rx_cache_empty += rq_stats->cache_empty;
+		s->rx_cache_busy  += rq_stats->cache_busy;
 
 		for (j = 0; j < priv->params.num_tc; j++) {
 			sq_stats = &priv->channel[i]->sq[j].stats;
@@ -174,18 +224,15 @@ static void mlx5e_update_vport_counters(struct mlx5e_priv *priv)
 {
 	int outlen = MLX5_ST_SZ_BYTES(query_vport_counter_out);
 	u32 *out = (u32 *)priv->stats.vport.query_vport_out;
-	u32 in[MLX5_ST_SZ_DW(query_vport_counter_in)];
+	u32 in[MLX5_ST_SZ_DW(query_vport_counter_in)] = {0};
 	struct mlx5_core_dev *mdev = priv->mdev;
 
-	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);
-
 	mlx5_cmd_exec(mdev, in, sizeof(in), out, outlen);
 }
 
@@ -298,6 +345,117 @@ static void mlx5e_disable_async_events(struct mlx5e_priv *priv)
 #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 inline int mlx5e_get_wqe_mtt_sz(void)
+{
+	/* UMR copies MTTs in units of MLX5_UMR_MTT_ALIGNMENT bytes.
+	 * To avoid copying garbage after the mtt array, we allocate
+	 * a little more.
+	 */
+	return ALIGN(MLX5_MPWRQ_PAGES_PER_WQE * sizeof(__be64),
+		     MLX5_UMR_MTT_ALIGNMENT);
+}
+
+static inline void mlx5e_build_umr_wqe(struct mlx5e_rq *rq, struct mlx5e_sq *sq,
+				       struct mlx5e_umr_wqe *wqe, u16 ix)
+{
+	struct mlx5_wqe_ctrl_seg      *cseg = &wqe->ctrl;
+	struct mlx5_wqe_umr_ctrl_seg *ucseg = &wqe->uctrl;
+	struct mlx5_wqe_data_seg      *dseg = &wqe->data;
+	struct mlx5e_mpw_info *wi = &rq->mpwqe.info[ix];
+	u8 ds_cnt = DIV_ROUND_UP(sizeof(*wqe), MLX5_SEND_WQE_DS);
+	u32 umr_wqe_mtt_offset = mlx5e_get_wqe_mtt_offset(rq, ix);
+
+	cseg->qpn_ds    = cpu_to_be32((sq->sqn << MLX5_WQE_CTRL_QPN_SHIFT) |
+				      ds_cnt);
+	cseg->fm_ce_se  = MLX5_WQE_CTRL_CQ_UPDATE;
+	cseg->imm       = rq->mkey_be;
+
+	ucseg->flags = MLX5_UMR_TRANSLATION_OFFSET_EN;
+	ucseg->klm_octowords =
+		cpu_to_be16(MLX5_MTT_OCTW(MLX5_MPWRQ_PAGES_PER_WQE));
+	ucseg->bsf_octowords =
+		cpu_to_be16(MLX5_MTT_OCTW(umr_wqe_mtt_offset));
+	ucseg->mkey_mask     = cpu_to_be64(MLX5_MKEY_MASK_FREE);
+
+	dseg->lkey = sq->mkey_be;
+	dseg->addr = cpu_to_be64(wi->umr.mtt_addr);
+}
+
+static int mlx5e_rq_alloc_mpwqe_info(struct mlx5e_rq *rq,
+				     struct mlx5e_channel *c)
+{
+	int wq_sz = mlx5_wq_ll_get_size(&rq->wq);
+	int mtt_sz = mlx5e_get_wqe_mtt_sz();
+	int mtt_alloc = mtt_sz + MLX5_UMR_ALIGN - 1;
+	int i;
+
+	rq->mpwqe.info = kzalloc_node(wq_sz * sizeof(*rq->mpwqe.info),
+				      GFP_KERNEL, cpu_to_node(c->cpu));
+	if (!rq->mpwqe.info)
+		goto err_out;
+
+	/* We allocate more than mtt_sz as we will align the pointer */
+	rq->mpwqe.mtt_no_align = kzalloc_node(mtt_alloc * wq_sz, GFP_KERNEL,
+					cpu_to_node(c->cpu));
+	if (unlikely(!rq->mpwqe.mtt_no_align))
+		goto err_free_wqe_info;
+
+	for (i = 0; i < wq_sz; i++) {
+		struct mlx5e_mpw_info *wi = &rq->mpwqe.info[i];
+
+		wi->umr.mtt = PTR_ALIGN(rq->mpwqe.mtt_no_align + i * mtt_alloc,
+					MLX5_UMR_ALIGN);
+		wi->umr.mtt_addr = dma_map_single(c->pdev, wi->umr.mtt, mtt_sz,
+						  PCI_DMA_TODEVICE);
+		if (unlikely(dma_mapping_error(c->pdev, wi->umr.mtt_addr)))
+			goto err_unmap_mtts;
+
+		mlx5e_build_umr_wqe(rq, &c->icosq, &wi->umr.wqe, i);
+	}
+
+	return 0;
+
+err_unmap_mtts:
+	while (--i >= 0) {
+		struct mlx5e_mpw_info *wi = &rq->mpwqe.info[i];
+
+		dma_unmap_single(c->pdev, wi->umr.mtt_addr, mtt_sz,
+				 PCI_DMA_TODEVICE);
+	}
+	kfree(rq->mpwqe.mtt_no_align);
+err_free_wqe_info:
+	kfree(rq->mpwqe.info);
+
+err_out:
+	return -ENOMEM;
+}
+
+static void mlx5e_rq_free_mpwqe_info(struct mlx5e_rq *rq)
+{
+	int wq_sz = mlx5_wq_ll_get_size(&rq->wq);
+	int mtt_sz = mlx5e_get_wqe_mtt_sz();
+	int i;
+
+	for (i = 0; i < wq_sz; i++) {
+		struct mlx5e_mpw_info *wi = &rq->mpwqe.info[i];
+
+		dma_unmap_single(rq->pdev, wi->umr.mtt_addr, mtt_sz,
+				 PCI_DMA_TODEVICE);
+	}
+	kfree(rq->mpwqe.mtt_no_align);
+	kfree(rq->mpwqe.info);
+}
+
+static bool mlx5e_is_vf_vport_rep(struct mlx5e_priv *priv)
+{
+	struct mlx5_eswitch_rep *rep = (struct mlx5_eswitch_rep *)priv->ppriv;
+
+	if (rep && rep->vport != FDB_UPLINK_VPORT)
+		return true;
+
+	return false;
+}
+
 static int mlx5e_create_rq(struct mlx5e_channel *c,
 			   struct mlx5e_rq_param *param,
 			   struct mlx5e_rq *rq)
@@ -307,6 +465,8 @@ static int mlx5e_create_rq(struct mlx5e_channel *c,
 	void *rqc = param->rqc;
 	void *rqc_wq = MLX5_ADDR_OF(rqc, rqc, wq);
 	u32 byte_count;
+	u32 frag_sz;
+	int npages;
 	int wq_sz;
 	int err;
 	int i;
@@ -322,63 +482,92 @@ static int mlx5e_create_rq(struct mlx5e_channel *c,
 
 	wq_sz = mlx5_wq_ll_get_size(&rq->wq);
 
+	rq->wq_type = priv->params.rq_wq_type;
+	rq->pdev    = c->pdev;
+	rq->netdev  = c->netdev;
+	rq->tstamp  = &priv->tstamp;
+	rq->channel = c;
+	rq->ix      = c->ix;
+	rq->priv    = c->priv;
+	rq->xdp_prog = priv->xdp_prog;
+
+	rq->buff.map_dir = DMA_FROM_DEVICE;
+	if (rq->xdp_prog)
+		rq->buff.map_dir = DMA_BIDIRECTIONAL;
+
 	switch (priv->params.rq_wq_type) {
 	case MLX5_WQ_TYPE_LINKED_LIST_STRIDING_RQ:
-		rq->wqe_info = kzalloc_node(wq_sz * sizeof(*rq->wqe_info),
-					    GFP_KERNEL, cpu_to_node(c->cpu));
-		if (!rq->wqe_info) {
-			err = -ENOMEM;
+		if (mlx5e_is_vf_vport_rep(priv)) {
+			err = -EINVAL;
 			goto err_rq_wq_destroy;
 		}
+
 		rq->handle_rx_cqe = mlx5e_handle_rx_cqe_mpwrq;
 		rq->alloc_wqe = mlx5e_alloc_rx_mpwqe;
 		rq->dealloc_wqe = mlx5e_dealloc_rx_mpwqe;
 
-		rq->mpwqe_mtt_offset = c->ix *
+		rq->mpwqe.mtt_offset = c->ix *
 			MLX5E_REQUIRED_MTTS(1, BIT(priv->params.log_rq_size));
 
 		rq->mpwqe_stride_sz = BIT(priv->params.mpwqe_log_stride_sz);
 		rq->mpwqe_num_strides = BIT(priv->params.mpwqe_log_num_strides);
-		rq->wqe_sz = rq->mpwqe_stride_sz * rq->mpwqe_num_strides;
-		byte_count = rq->wqe_sz;
+
+		rq->buff.wqe_sz = rq->mpwqe_stride_sz * rq->mpwqe_num_strides;
+		byte_count = rq->buff.wqe_sz;
+		rq->mkey_be = cpu_to_be32(c->priv->umr_mkey.key);
+		err = mlx5e_rq_alloc_mpwqe_info(rq, c);
+		if (err)
+			goto err_rq_wq_destroy;
 		break;
 	default: /* MLX5_WQ_TYPE_LINKED_LIST */
-		rq->skb = kzalloc_node(wq_sz * sizeof(*rq->skb), GFP_KERNEL,
-				       cpu_to_node(c->cpu));
-		if (!rq->skb) {
+		rq->dma_info = kzalloc_node(wq_sz * sizeof(*rq->dma_info),
+					    GFP_KERNEL, cpu_to_node(c->cpu));
+		if (!rq->dma_info) {
 			err = -ENOMEM;
 			goto err_rq_wq_destroy;
 		}
-		rq->handle_rx_cqe = mlx5e_handle_rx_cqe;
+
+		if (mlx5e_is_vf_vport_rep(priv))
+			rq->handle_rx_cqe = mlx5e_handle_rx_cqe_rep;
+		else
+			rq->handle_rx_cqe = mlx5e_handle_rx_cqe;
+
 		rq->alloc_wqe = mlx5e_alloc_rx_wqe;
 		rq->dealloc_wqe = mlx5e_dealloc_rx_wqe;
 
-		rq->wqe_sz = (priv->params.lro_en) ?
+		rq->buff.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);
-		byte_count = rq->wqe_sz;
+		byte_count = rq->buff.wqe_sz;
+
+		/* calc the required page order */
+		frag_sz = MLX5_RX_HEADROOM +
+			  byte_count /* packet data */ +
+			  SKB_DATA_ALIGN(sizeof(struct skb_shared_info));
+		frag_sz = SKB_DATA_ALIGN(frag_sz);
+
+		npages = DIV_ROUND_UP(frag_sz, PAGE_SIZE);
+		rq->buff.page_order = order_base_2(npages);
+
 		byte_count |= MLX5_HW_START_PADDING;
+		rq->mkey_be = c->mkey_be;
 	}
 
 	for (i = 0; i < wq_sz; i++) {
 		struct mlx5e_rx_wqe *wqe = mlx5_wq_ll_get_wqe(&rq->wq, i);
 
 		wqe->data.byte_count = cpu_to_be32(byte_count);
+		wqe->data.lkey = rq->mkey_be;
 	}
 
 	INIT_WORK(&rq->am.work, mlx5e_rx_am_work);
 	rq->am.mode = priv->params.rx_cq_period_mode;
 
-	rq->wq_type = priv->params.rq_wq_type;
-	rq->pdev    = c->pdev;
-	rq->netdev  = c->netdev;
-	rq->tstamp  = &priv->tstamp;
-	rq->channel = c;
-	rq->ix      = c->ix;
-	rq->priv    = c->priv;
-	rq->mkey_be = c->mkey_be;
-	rq->umr_mkey_be = cpu_to_be32(c->priv->umr_mkey.key);
+	rq->page_cache.head = 0;
+	rq->page_cache.tail = 0;
+
+	if (rq->xdp_prog)
+		bpf_prog_add(rq->xdp_prog, 1);
 
 	return 0;
 
@@ -390,14 +579,25 @@ err_rq_wq_destroy:
 
 static void mlx5e_destroy_rq(struct mlx5e_rq *rq)
 {
+	int i;
+
+	if (rq->xdp_prog)
+		bpf_prog_put(rq->xdp_prog);
+
 	switch (rq->wq_type) {
 	case MLX5_WQ_TYPE_LINKED_LIST_STRIDING_RQ:
-		kfree(rq->wqe_info);
+		mlx5e_rq_free_mpwqe_info(rq);
 		break;
 	default: /* MLX5_WQ_TYPE_LINKED_LIST */
-		kfree(rq->skb);
+		kfree(rq->dma_info);
 	}
 
+	for (i = rq->page_cache.head; i != rq->page_cache.tail;
+	     i = (i + 1) & (MLX5E_CACHE_SIZE - 1)) {
+		struct mlx5e_dma_info *dma_info = &rq->page_cache.page_cache[i];
+
+		mlx5e_page_release(rq, dma_info, false);
+	}
 	mlx5_wq_destroy(&rq->wq_ctrl);
 }
 
@@ -488,7 +688,8 @@ static int mlx5e_modify_rq_vsd(struct mlx5e_rq *rq, bool vsd)
 	rqc = MLX5_ADDR_OF(modify_rq_in, in, ctx);
 
 	MLX5_SET(modify_rq_in, in, rq_state, MLX5_RQC_STATE_RDY);
-	MLX5_SET64(modify_rq_in, in, modify_bitmask, MLX5_RQ_BITMASK_VSD);
+	MLX5_SET64(modify_rq_in, in, modify_bitmask,
+		   MLX5_MODIFY_RQ_IN_MODIFY_BITMASK_VSD);
 	MLX5_SET(rqc, rqc, vsd, vsd);
 	MLX5_SET(rqc, rqc, state, MLX5_RQC_STATE_RDY);
 
@@ -530,7 +731,7 @@ static void mlx5e_free_rx_descs(struct mlx5e_rq *rq)
 
 	/* UMR WQE (if in progress) is always at wq->head */
 	if (test_bit(MLX5E_RQ_STATE_UMR_WQE_IN_PROGRESS, &rq->state))
-		mlx5e_free_rx_fragmented_mpwqe(rq, &rq->wqe_info[wq->head]);
+		mlx5e_free_rx_mpwqe(rq, &rq->mpwqe.info[wq->head]);
 
 	while (!mlx5_wq_ll_is_empty(wq)) {
 		wqe_ix_be = *wq->tail_next;
@@ -565,8 +766,8 @@ static int mlx5e_open_rq(struct mlx5e_channel *c,
 	if (param->am_enabled)
 		set_bit(MLX5E_RQ_STATE_AM, &c->rq.state);
 
-	sq->ico_wqe_info[pi].opcode     = MLX5_OPCODE_NOP;
-	sq->ico_wqe_info[pi].num_wqebbs = 1;
+	sq->db.ico_wqe[pi].opcode     = MLX5_OPCODE_NOP;
+	sq->db.ico_wqe[pi].num_wqebbs = 1;
 	mlx5e_send_nop(sq, true); /* trigger mlx5e_post_rx_wqes() */
 
 	return 0;
@@ -590,26 +791,65 @@ static void mlx5e_close_rq(struct mlx5e_rq *rq)
 	mlx5e_destroy_rq(rq);
 }
 
-static void mlx5e_free_sq_db(struct mlx5e_sq *sq)
+static void mlx5e_free_sq_xdp_db(struct mlx5e_sq *sq)
 {
-	kfree(sq->wqe_info);
-	kfree(sq->dma_fifo);
-	kfree(sq->skb);
+	kfree(sq->db.xdp.di);
+	kfree(sq->db.xdp.wqe_info);
 }
 
-static int mlx5e_alloc_sq_db(struct mlx5e_sq *sq, int numa)
+static int mlx5e_alloc_sq_xdp_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);
-	sq->wqe_info = kzalloc_node(wq_sz * sizeof(*sq->wqe_info), GFP_KERNEL,
-				    numa);
+	sq->db.xdp.di = kzalloc_node(sizeof(*sq->db.xdp.di) * wq_sz,
+				     GFP_KERNEL, numa);
+	sq->db.xdp.wqe_info = kzalloc_node(sizeof(*sq->db.xdp.wqe_info) * wq_sz,
+					   GFP_KERNEL, numa);
+	if (!sq->db.xdp.di || !sq->db.xdp.wqe_info) {
+		mlx5e_free_sq_xdp_db(sq);
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+
+static void mlx5e_free_sq_ico_db(struct mlx5e_sq *sq)
+{
+	kfree(sq->db.ico_wqe);
+}
 
-	if (!sq->skb || !sq->dma_fifo || !sq->wqe_info) {
-		mlx5e_free_sq_db(sq);
+static int mlx5e_alloc_sq_ico_db(struct mlx5e_sq *sq, int numa)
+{
+	u8 wq_sz = mlx5_wq_cyc_get_size(&sq->wq);
+
+	sq->db.ico_wqe = kzalloc_node(sizeof(*sq->db.ico_wqe) * wq_sz,
+				      GFP_KERNEL, numa);
+	if (!sq->db.ico_wqe)
+		return -ENOMEM;
+
+	return 0;
+}
+
+static void mlx5e_free_sq_txq_db(struct mlx5e_sq *sq)
+{
+	kfree(sq->db.txq.wqe_info);
+	kfree(sq->db.txq.dma_fifo);
+	kfree(sq->db.txq.skb);
+}
+
+static int mlx5e_alloc_sq_txq_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->db.txq.skb = kzalloc_node(wq_sz * sizeof(*sq->db.txq.skb),
+				      GFP_KERNEL, numa);
+	sq->db.txq.dma_fifo = kzalloc_node(df_sz * sizeof(*sq->db.txq.dma_fifo),
+					   GFP_KERNEL, numa);
+	sq->db.txq.wqe_info = kzalloc_node(wq_sz * sizeof(*sq->db.txq.wqe_info),
+					   GFP_KERNEL, numa);
+	if (!sq->db.txq.skb || !sq->db.txq.dma_fifo || !sq->db.txq.wqe_info) {
+		mlx5e_free_sq_txq_db(sq);
 		return -ENOMEM;
 	}
 
@@ -618,6 +858,46 @@ static int mlx5e_alloc_sq_db(struct mlx5e_sq *sq, int numa)
 	return 0;
 }
 
+static void mlx5e_free_sq_db(struct mlx5e_sq *sq)
+{
+	switch (sq->type) {
+	case MLX5E_SQ_TXQ:
+		mlx5e_free_sq_txq_db(sq);
+		break;
+	case MLX5E_SQ_ICO:
+		mlx5e_free_sq_ico_db(sq);
+		break;
+	case MLX5E_SQ_XDP:
+		mlx5e_free_sq_xdp_db(sq);
+		break;
+	}
+}
+
+static int mlx5e_alloc_sq_db(struct mlx5e_sq *sq, int numa)
+{
+	switch (sq->type) {
+	case MLX5E_SQ_TXQ:
+		return mlx5e_alloc_sq_txq_db(sq, numa);
+	case MLX5E_SQ_ICO:
+		return mlx5e_alloc_sq_ico_db(sq, numa);
+	case MLX5E_SQ_XDP:
+		return mlx5e_alloc_sq_xdp_db(sq, numa);
+	}
+
+	return 0;
+}
+
+static int mlx5e_sq_get_max_wqebbs(u8 sq_type)
+{
+	switch (sq_type) {
+	case MLX5E_SQ_ICO:
+		return MLX5E_ICOSQ_MAX_WQEBBS;
+	case MLX5E_SQ_XDP:
+		return MLX5E_XDP_TX_WQEBBS;
+	}
+	return MLX5_SEND_WQE_MAX_WQEBBS;
+}
+
 static int mlx5e_create_sq(struct mlx5e_channel *c,
 			   int tc,
 			   struct mlx5e_sq_param *param,
@@ -630,6 +910,13 @@ static int mlx5e_create_sq(struct mlx5e_channel *c,
 	void *sqc_wq = MLX5_ADDR_OF(sqc, sqc, wq);
 	int err;
 
+	sq->type      = param->type;
+	sq->pdev      = c->pdev;
+	sq->tstamp    = &priv->tstamp;
+	sq->mkey_be   = c->mkey_be;
+	sq->channel   = c;
+	sq->tc        = tc;
+
 	err = mlx5_alloc_map_uar(mdev, &sq->uar, !!MLX5_CAP_GEN(mdev, bf));
 	if (err)
 		return err;
@@ -658,18 +945,7 @@ static int mlx5e_create_sq(struct mlx5e_channel *c,
 	if (err)
 		goto err_sq_wq_destroy;
 
-	if (param->icosq) {
-		u8 wq_sz = mlx5_wq_cyc_get_size(&sq->wq);
-
-		sq->ico_wqe_info = kzalloc_node(sizeof(*sq->ico_wqe_info) *
-						wq_sz,
-						GFP_KERNEL,
-						cpu_to_node(c->cpu));
-		if (!sq->ico_wqe_info) {
-			err = -ENOMEM;
-			goto err_free_sq_db;
-		}
-	} else {
+	if (sq->type == MLX5E_SQ_TXQ) {
 		int txq_ix;
 
 		txq_ix = c->ix + tc * priv->params.num_channels;
@@ -677,19 +953,11 @@ static int mlx5e_create_sq(struct mlx5e_channel *c,
 		priv->txq_to_sq_map[txq_ix] = sq;
 	}
 
-	sq->pdev      = c->pdev;
-	sq->tstamp    = &priv->tstamp;
-	sq->mkey_be   = c->mkey_be;
-	sq->channel   = c;
-	sq->tc        = tc;
-	sq->edge      = (sq->wq.sz_m1 + 1) - MLX5_SEND_WQE_MAX_WQEBBS;
+	sq->edge = (sq->wq.sz_m1 + 1) - mlx5e_sq_get_max_wqebbs(sq->type);
 	sq->bf_budget = MLX5E_SQ_BF_BUDGET;
 
 	return 0;
 
-err_free_sq_db:
-	mlx5e_free_sq_db(sq);
-
 err_sq_wq_destroy:
 	mlx5_wq_destroy(&sq->wq_ctrl);
 
@@ -704,7 +972,6 @@ static void mlx5e_destroy_sq(struct mlx5e_sq *sq)
 	struct mlx5e_channel *c = sq->channel;
 	struct mlx5e_priv *priv = c->priv;
 
-	kfree(sq->ico_wqe_info);
 	mlx5e_free_sq_db(sq);
 	mlx5_wq_destroy(&sq->wq_ctrl);
 	mlx5_unmap_free_uar(priv->mdev, &sq->uar);
@@ -733,11 +1000,12 @@ static int mlx5e_enable_sq(struct mlx5e_sq *sq, struct mlx5e_sq_param *param)
 
 	memcpy(sqc, param->sqc, sizeof(param->sqc));
 
-	MLX5_SET(sqc,  sqc, tis_num_0, param->icosq ? 0 : priv->tisn[sq->tc]);
+	MLX5_SET(sqc,  sqc, tis_num_0, param->type == MLX5E_SQ_ICO ?
+				       0 : priv->tisn[sq->tc]);
 	MLX5_SET(sqc,  sqc, cqn,		sq->cq.mcq.cqn);
 	MLX5_SET(sqc,  sqc, min_wqe_inline_mode, sq->min_inline_mode);
 	MLX5_SET(sqc,  sqc, state,		MLX5_SQC_STATE_RST);
-	MLX5_SET(sqc,  sqc, tis_lst_sz,		param->icosq ? 0 : 1);
+	MLX5_SET(sqc,  sqc, tis_lst_sz, param->type == MLX5E_SQ_ICO ? 0 : 1);
 	MLX5_SET(sqc,  sqc, flush_in_error_en,	1);
 
 	MLX5_SET(wq,   wq, wq_type,       MLX5_WQ_TYPE_CYCLIC);
@@ -852,12 +1120,14 @@ static void mlx5e_close_sq(struct mlx5e_sq *sq)
 		netif_tx_disable_queue(sq->txq);
 
 		/* last doorbell out, godspeed .. */
-		if (mlx5e_sq_has_room_for(sq, 1))
+		if (mlx5e_sq_has_room_for(sq, 1)) {
+			sq->db.txq.skb[(sq->pc & sq->wq.sz_m1)] = NULL;
 			mlx5e_send_nop(sq, true);
+		}
 	}
 
 	mlx5e_disable_sq(sq);
-	mlx5e_free_tx_descs(sq);
+	mlx5e_free_sq_descs(sq);
 	mlx5e_destroy_sq(sq);
 }
 
@@ -1218,14 +1488,31 @@ static int mlx5e_open_channel(struct mlx5e_priv *priv, int ix,
 		}
 	}
 
+	if (priv->xdp_prog) {
+		/* XDP SQ CQ params are same as normal TXQ sq CQ params */
+		err = mlx5e_open_cq(c, &cparam->tx_cq, &c->xdp_sq.cq,
+				    priv->params.tx_cq_moderation);
+		if (err)
+			goto err_close_sqs;
+
+		err = mlx5e_open_sq(c, 0, &cparam->xdp_sq, &c->xdp_sq);
+		if (err) {
+			mlx5e_close_cq(&c->xdp_sq.cq);
+			goto err_close_sqs;
+		}
+	}
+
+	c->xdp = !!priv->xdp_prog;
 	err = mlx5e_open_rq(c, &cparam->rq, &c->rq);
 	if (err)
-		goto err_close_sqs;
+		goto err_close_xdp_sq;
 
 	netif_set_xps_queue(netdev, get_cpu_mask(c->cpu), ix);
 	*cp = c;
 
 	return 0;
+err_close_xdp_sq:
+	mlx5e_close_sq(&c->xdp_sq);
 
 err_close_sqs:
 	mlx5e_close_sqs(c);
@@ -1254,9 +1541,13 @@ err_napi_del:
 static void mlx5e_close_channel(struct mlx5e_channel *c)
 {
 	mlx5e_close_rq(&c->rq);
+	if (c->xdp)
+		mlx5e_close_sq(&c->xdp_sq);
 	mlx5e_close_sqs(c);
 	mlx5e_close_sq(&c->icosq);
 	napi_disable(&c->napi);
+	if (c->xdp)
+		mlx5e_close_cq(&c->xdp_sq.cq);
 	mlx5e_close_cq(&c->rq.cq);
 	mlx5e_close_tx_cqs(c);
 	mlx5e_close_cq(&c->icosq.cq);
@@ -1330,6 +1621,7 @@ static void mlx5e_build_sq_param(struct mlx5e_priv *priv,
 
 	param->max_inline = priv->params.tx_max_inline;
 	param->min_inline_mode = priv->params.tx_min_inline_mode;
+	param->type = MLX5E_SQ_TXQ;
 }
 
 static void mlx5e_build_common_cq_param(struct mlx5e_priv *priv,
@@ -1403,7 +1695,22 @@ static void mlx5e_build_icosq_param(struct mlx5e_priv *priv,
 	MLX5_SET(wq, wq, log_wq_sz, log_wq_size);
 	MLX5_SET(sqc, sqc, reg_umr, MLX5_CAP_ETH(priv->mdev, reg_umr_sq));
 
-	param->icosq = true;
+	param->type = MLX5E_SQ_ICO;
+}
+
+static void mlx5e_build_xdpsq_param(struct mlx5e_priv *priv,
+				    struct mlx5e_sq_param *param)
+{
+	void *sqc = param->sqc;
+	void *wq = MLX5_ADDR_OF(sqc, sqc, wq);
+
+	mlx5e_build_sq_param_common(priv, param);
+	MLX5_SET(wq, wq, log_wq_sz,     priv->params.log_sq_size);
+
+	param->max_inline = priv->params.tx_max_inline;
+	/* FOR XDP SQs will support only L2 inline mode */
+	param->min_inline_mode = MLX5_INLINE_MODE_NONE;
+	param->type = MLX5E_SQ_XDP;
 }
 
 static void mlx5e_build_channel_param(struct mlx5e_priv *priv, struct mlx5e_channel_param *cparam)
@@ -1412,6 +1719,7 @@ static void mlx5e_build_channel_param(struct mlx5e_priv *priv, struct mlx5e_chan
 
 	mlx5e_build_rq_param(priv, &cparam->rq);
 	mlx5e_build_sq_param(priv, &cparam->sq);
+	mlx5e_build_xdpsq_param(priv, &cparam->xdp_sq);
 	mlx5e_build_icosq_param(priv, &cparam->icosq, icosq_log_wq_sz);
 	mlx5e_build_rx_cq_param(priv, &cparam->rx_cq);
 	mlx5e_build_tx_cq_param(priv, &cparam->tx_cq);
@@ -1885,6 +2193,9 @@ int mlx5e_close(struct net_device *netdev)
 	struct mlx5e_priv *priv = netdev_priv(netdev);
 	int err;
 
+	if (!netif_device_present(netdev))
+		return -ENODEV;
+
 	mutex_lock(&priv->state_lock);
 	err = mlx5e_close_locked(netdev);
 	mutex_unlock(&priv->state_lock);
@@ -1999,14 +2310,15 @@ static void mlx5e_close_drop_rq(struct mlx5e_priv *priv)
 static int mlx5e_create_tis(struct mlx5e_priv *priv, int tc)
 {
 	struct mlx5_core_dev *mdev = priv->mdev;
-	u32 in[MLX5_ST_SZ_DW(create_tis_in)];
+	u32 in[MLX5_ST_SZ_DW(create_tis_in)] = {0};
 	void *tisc = MLX5_ADDR_OF(create_tis_in, in, ctx);
 
-	memset(in, 0, sizeof(in));
-
 	MLX5_SET(tisc, tisc, prio, tc << 1);
 	MLX5_SET(tisc, tisc, transport_domain, mdev->mlx5e_res.td.tdn);
 
+	if (mlx5_lag_is_lacp_owner(mdev))
+		MLX5_SET(tisc, tisc, strict_lag_tx_port_affinity, 1);
+
 	return mlx5_core_create_tis(mdev, in, sizeof(in), &priv->tisn[tc]);
 }
 
@@ -2605,11 +2917,15 @@ static int mlx5e_set_vf_mac(struct net_device *dev, int vf, u8 *mac)
 	return mlx5_eswitch_set_vport_mac(mdev->priv.eswitch, vf + 1, mac);
 }
 
-static int mlx5e_set_vf_vlan(struct net_device *dev, int vf, u16 vlan, u8 qos)
+static int mlx5e_set_vf_vlan(struct net_device *dev, int vf, u16 vlan, u8 qos,
+			     __be16 vlan_proto)
 {
 	struct mlx5e_priv *priv = netdev_priv(dev);
 	struct mlx5_core_dev *mdev = priv->mdev;
 
+	if (vlan_proto != htons(ETH_P_8021Q))
+		return -EPROTONOSUPPORT;
+
 	return mlx5_eswitch_set_vport_vlan(mdev->priv.eswitch, vf + 1,
 					   vlan, qos);
 }
@@ -2786,6 +3102,106 @@ static void mlx5e_tx_timeout(struct net_device *dev)
 		schedule_work(&priv->tx_timeout_work);
 }
 
+static int mlx5e_xdp_set(struct net_device *netdev, struct bpf_prog *prog)
+{
+	struct mlx5e_priv *priv = netdev_priv(netdev);
+	struct bpf_prog *old_prog;
+	int err = 0;
+	bool reset, was_opened;
+	int i;
+
+	mutex_lock(&priv->state_lock);
+
+	if ((netdev->features & NETIF_F_LRO) && prog) {
+		netdev_warn(netdev, "can't set XDP while LRO is on, disable LRO first\n");
+		err = -EINVAL;
+		goto unlock;
+	}
+
+	was_opened = test_bit(MLX5E_STATE_OPENED, &priv->state);
+	/* no need for full reset when exchanging programs */
+	reset = (!priv->xdp_prog || !prog);
+
+	if (was_opened && reset)
+		mlx5e_close_locked(netdev);
+
+	/* exchange programs */
+	old_prog = xchg(&priv->xdp_prog, prog);
+	if (prog)
+		bpf_prog_add(prog, 1);
+	if (old_prog)
+		bpf_prog_put(old_prog);
+
+	if (reset) /* change RQ type according to priv->xdp_prog */
+		mlx5e_set_rq_priv_params(priv);
+
+	if (was_opened && reset)
+		mlx5e_open_locked(netdev);
+
+	if (!test_bit(MLX5E_STATE_OPENED, &priv->state) || reset)
+		goto unlock;
+
+	/* exchanging programs w/o reset, we update ref counts on behalf
+	 * of the channels RQs here.
+	 */
+	bpf_prog_add(prog, priv->params.num_channels);
+	for (i = 0; i < priv->params.num_channels; i++) {
+		struct mlx5e_channel *c = priv->channel[i];
+
+		set_bit(MLX5E_RQ_STATE_FLUSH, &c->rq.state);
+		napi_synchronize(&c->napi);
+		/* prevent mlx5e_poll_rx_cq from accessing rq->xdp_prog */
+
+		old_prog = xchg(&c->rq.xdp_prog, prog);
+
+		clear_bit(MLX5E_RQ_STATE_FLUSH, &c->rq.state);
+		/* napi_schedule in case we have missed anything */
+		set_bit(MLX5E_CHANNEL_NAPI_SCHED, &c->flags);
+		napi_schedule(&c->napi);
+
+		if (old_prog)
+			bpf_prog_put(old_prog);
+	}
+
+unlock:
+	mutex_unlock(&priv->state_lock);
+	return err;
+}
+
+static bool mlx5e_xdp_attached(struct net_device *dev)
+{
+	struct mlx5e_priv *priv = netdev_priv(dev);
+
+	return !!priv->xdp_prog;
+}
+
+static int mlx5e_xdp(struct net_device *dev, struct netdev_xdp *xdp)
+{
+	switch (xdp->command) {
+	case XDP_SETUP_PROG:
+		return mlx5e_xdp_set(dev, xdp->prog);
+	case XDP_QUERY_PROG:
+		xdp->prog_attached = mlx5e_xdp_attached(dev);
+		return 0;
+	default:
+		return -EINVAL;
+	}
+}
+
+#ifdef CONFIG_NET_POLL_CONTROLLER
+/* Fake "interrupt" called by netpoll (eg netconsole) to send skbs without
+ * reenabling interrupts.
+ */
+static void mlx5e_netpoll(struct net_device *dev)
+{
+	struct mlx5e_priv *priv = netdev_priv(dev);
+	int i;
+
+	for (i = 0; i < priv->params.num_channels; i++)
+		napi_schedule(&priv->channel[i]->napi);
+}
+#endif
+
 static const struct net_device_ops mlx5e_netdev_ops_basic = {
 	.ndo_open                = mlx5e_open,
 	.ndo_stop                = mlx5e_close,
@@ -2805,6 +3221,10 @@ static const struct net_device_ops mlx5e_netdev_ops_basic = {
 	.ndo_rx_flow_steer	 = mlx5e_rx_flow_steer,
 #endif
 	.ndo_tx_timeout          = mlx5e_tx_timeout,
+	.ndo_xdp		 = mlx5e_xdp,
+#ifdef CONFIG_NET_POLL_CONTROLLER
+	.ndo_poll_controller     = mlx5e_netpoll,
+#endif
 };
 
 static const struct net_device_ops mlx5e_netdev_ops_sriov = {
@@ -2836,6 +3256,10 @@ static const struct net_device_ops mlx5e_netdev_ops_sriov = {
 	.ndo_set_vf_link_state   = mlx5e_set_vf_link_state,
 	.ndo_get_vf_stats        = mlx5e_get_vf_stats,
 	.ndo_tx_timeout          = mlx5e_tx_timeout,
+	.ndo_xdp		 = mlx5e_xdp,
+#ifdef CONFIG_NET_POLL_CONTROLLER
+	.ndo_poll_controller     = mlx5e_netpoll,
+#endif
 };
 
 static int mlx5e_check_required_hca_cap(struct mlx5_core_dev *mdev)
@@ -2910,13 +3334,6 @@ void mlx5e_build_default_indir_rqt(struct mlx5_core_dev *mdev,
 		indirection_rqt[i] = i % num_channels;
 }
 
-static bool mlx5e_check_fragmented_striding_rq_cap(struct mlx5_core_dev *mdev)
-{
-	return MLX5_CAP_GEN(mdev, striding_rq) &&
-		MLX5_CAP_GEN(mdev, umr_ptr_rlky) &&
-		MLX5_CAP_ETH(mdev, reg_umr_sq);
-}
-
 static int mlx5e_get_pci_bw(struct mlx5_core_dev *mdev, u32 *pci_bw)
 {
 	enum pcie_link_width width;
@@ -2996,11 +3413,13 @@ static void mlx5e_build_nic_netdev_priv(struct mlx5_core_dev *mdev,
 					 MLX5_CQ_PERIOD_MODE_START_FROM_CQE :
 					 MLX5_CQ_PERIOD_MODE_START_FROM_EQE;
 
-	priv->params.log_sq_size           =
-		MLX5E_PARAMS_DEFAULT_LOG_SQ_SIZE;
-	priv->params.rq_wq_type = mlx5e_check_fragmented_striding_rq_cap(mdev) ?
-		MLX5_WQ_TYPE_LINKED_LIST_STRIDING_RQ :
-		MLX5_WQ_TYPE_LINKED_LIST;
+	priv->mdev                         = mdev;
+	priv->netdev                       = netdev;
+	priv->params.num_channels          = profile->max_nch(mdev);
+	priv->profile                      = profile;
+	priv->ppriv                        = ppriv;
+
+	priv->params.log_sq_size = MLX5E_PARAMS_DEFAULT_LOG_SQ_SIZE;
 
 	/* set CQE compression */
 	priv->params.rx_cqe_compress_admin = false;
@@ -3013,33 +3432,11 @@ static void mlx5e_build_nic_netdev_priv(struct mlx5_core_dev *mdev,
 		priv->params.rx_cqe_compress_admin =
 			cqe_compress_heuristic(link_speed, pci_bw);
 	}
-
 	priv->params.rx_cqe_compress = priv->params.rx_cqe_compress_admin;
 
-	switch (priv->params.rq_wq_type) {
-	case MLX5_WQ_TYPE_LINKED_LIST_STRIDING_RQ:
-		priv->params.log_rq_size = MLX5E_PARAMS_DEFAULT_LOG_RQ_SIZE_MPW;
-		priv->params.mpwqe_log_stride_sz =
-			priv->params.rx_cqe_compress ?
-			MLX5_MPWRQ_LOG_STRIDE_SIZE_CQE_COMPRESS :
-			MLX5_MPWRQ_LOG_STRIDE_SIZE;
-		priv->params.mpwqe_log_num_strides = MLX5_MPWRQ_LOG_WQE_SZ -
-			priv->params.mpwqe_log_stride_sz;
+	mlx5e_set_rq_priv_params(priv);
+	if (priv->params.rq_wq_type == MLX5_WQ_TYPE_LINKED_LIST_STRIDING_RQ)
 		priv->params.lro_en = true;
-		break;
-	default: /* MLX5_WQ_TYPE_LINKED_LIST */
-		priv->params.log_rq_size = MLX5E_PARAMS_DEFAULT_LOG_RQ_SIZE;
-	}
-
-	mlx5_core_info(mdev,
-		       "MLX5E: StrdRq(%d) RqSz(%ld) StrdSz(%ld) RxCqeCmprss(%d)\n",
-		       priv->params.rq_wq_type == MLX5_WQ_TYPE_LINKED_LIST_STRIDING_RQ,
-		       BIT(priv->params.log_rq_size),
-		       BIT(priv->params.mpwqe_log_stride_sz),
-		       priv->params.rx_cqe_compress_admin);
-
-	priv->params.min_rx_wqes = mlx5_min_rx_wqes(priv->params.rq_wq_type,
-					    BIT(priv->params.log_rq_size));
 
 	priv->params.rx_am_enabled = MLX5_CAP_GEN(mdev, cq_moderation);
 	mlx5e_set_rx_cq_mode_params(&priv->params, cq_period_mode);
@@ -3059,19 +3456,16 @@ static void mlx5e_build_nic_netdev_priv(struct mlx5_core_dev *mdev,
 	mlx5e_build_default_indir_rqt(mdev, priv->params.indirection_rqt,
 				      MLX5E_INDIR_RQT_SIZE, profile->max_nch(mdev));
 
-	priv->params.lro_wqe_sz            =
-		MLX5E_PARAMS_DEFAULT_LRO_WQE_SZ;
+	priv->params.lro_wqe_sz =
+		MLX5E_PARAMS_DEFAULT_LRO_WQE_SZ -
+		/* Extra room needed for build_skb */
+		MLX5_RX_HEADROOM -
+		SKB_DATA_ALIGN(sizeof(struct skb_shared_info));
 
 	/* Initialize pflags */
 	MLX5E_SET_PRIV_FLAG(priv, MLX5E_PFLAG_RX_CQE_BASED_MODER,
 			    priv->params.rx_cq_period_mode == MLX5_CQ_PERIOD_MODE_START_FROM_CQE);
 
-	priv->mdev                         = mdev;
-	priv->netdev                       = netdev;
-	priv->params.num_channels          = profile->max_nch(mdev);
-	priv->profile                      = profile;
-	priv->ppriv                        = ppriv;
-
 #ifdef CONFIG_MLX5_CORE_EN_DCB
 	mlx5e_ets_init(priv);
 #endif
@@ -3211,37 +3605,37 @@ static void mlx5e_destroy_q_counter(struct mlx5e_priv *priv)
 static int mlx5e_create_umr_mkey(struct mlx5e_priv *priv)
 {
 	struct mlx5_core_dev *mdev = priv->mdev;
-	struct mlx5_create_mkey_mbox_in *in;
-	struct mlx5_mkey_seg *mkc;
-	int inlen = sizeof(*in);
 	u64 npages = MLX5E_REQUIRED_MTTS(priv->profile->max_nch(mdev),
 					 BIT(MLX5E_PARAMS_MAXIMUM_LOG_RQ_SIZE_MPW));
+	int inlen = MLX5_ST_SZ_BYTES(create_mkey_in);
+	void *mkc;
+	u32 *in;
 	int err;
 
 	in = mlx5_vzalloc(inlen);
 	if (!in)
 		return -ENOMEM;
 
-	mkc = &in->seg;
-	mkc->status = MLX5_MKEY_STATUS_FREE;
-	mkc->flags = MLX5_PERM_UMR_EN |
-		     MLX5_PERM_LOCAL_READ |
-		     MLX5_PERM_LOCAL_WRITE |
-		     MLX5_ACCESS_MODE_MTT;
+	mkc = MLX5_ADDR_OF(create_mkey_in, in, memory_key_mkey_entry);
 
 	npages = min_t(u32, ALIGN(U16_MAX, 4) * 2, npages);
 
-	mkc->qpn_mkey7_0 = cpu_to_be32(0xffffff << 8);
-	mkc->flags_pd = cpu_to_be32(mdev->mlx5e_res.pdn);
-	mkc->len = cpu_to_be64(npages << PAGE_SHIFT);
-	mkc->xlt_oct_size = cpu_to_be32(MLX5_MTT_OCTW(npages));
-	mkc->log2_page_size = PAGE_SHIFT;
+	MLX5_SET(mkc, mkc, free, 1);
+	MLX5_SET(mkc, mkc, umr_en, 1);
+	MLX5_SET(mkc, mkc, lw, 1);
+	MLX5_SET(mkc, mkc, lr, 1);
+	MLX5_SET(mkc, mkc, access_mode, MLX5_MKC_ACCESS_MODE_MTT);
 
-	err = mlx5_core_create_mkey(mdev, &priv->umr_mkey, in, inlen, NULL,
-				    NULL, NULL);
+	MLX5_SET(mkc, mkc, qpn, 0xffffff);
+	MLX5_SET(mkc, mkc, pd, mdev->mlx5e_res.pdn);
+	MLX5_SET64(mkc, mkc, len, npages << PAGE_SHIFT);
+	MLX5_SET(mkc, mkc, translations_octword_size,
+		 MLX5_MTT_OCTW(npages));
+	MLX5_SET(mkc, mkc, log_page_size, PAGE_SHIFT);
 
-	kvfree(in);
+	err = mlx5_core_create_mkey(mdev, &priv->umr_mkey, in, inlen);
 
+	kvfree(in);
 	return err;
 }
 
@@ -3360,6 +3754,8 @@ static void mlx5e_nic_enable(struct mlx5e_priv *priv)
 	struct mlx5_eswitch *esw = mdev->priv.eswitch;
 	struct mlx5_eswitch_rep rep;
 
+	mlx5_lag_add(mdev, netdev);
+
 	if (mlx5e_vxlan_allowed(mdev)) {
 		rtnl_lock();
 		udp_tunnel_get_rx_info(netdev);
@@ -3373,9 +3769,9 @@ static void mlx5e_nic_enable(struct mlx5e_priv *priv)
 		mlx5_query_nic_vport_mac_address(mdev, 0, rep.hw_id);
 		rep.load = mlx5e_nic_rep_load;
 		rep.unload = mlx5e_nic_rep_unload;
-		rep.vport = 0;
+		rep.vport = FDB_UPLINK_VPORT;
 		rep.priv_data = priv;
-		mlx5_eswitch_register_vport_rep(esw, &rep);
+		mlx5_eswitch_register_vport_rep(esw, 0, &rep);
 	}
 }
 
@@ -3383,6 +3779,7 @@ static void mlx5e_nic_disable(struct mlx5e_priv *priv)
 {
 	queue_work(priv->wq, &priv->set_rx_mode_work);
 	mlx5e_disable_async_events(priv);
+	mlx5_lag_remove(priv->mdev);
 }
 
 static const struct mlx5e_profile mlx5e_nic_profile = {
@@ -3399,13 +3796,13 @@ static const struct mlx5e_profile mlx5e_nic_profile = {
 	.max_tc		   = MLX5E_MAX_NUM_TC,
 };
 
-void *mlx5e_create_netdev(struct mlx5_core_dev *mdev,
-			  const struct mlx5e_profile *profile, void *ppriv)
+struct net_device *mlx5e_create_netdev(struct mlx5_core_dev *mdev,
+				       const struct mlx5e_profile *profile,
+				       void *ppriv)
 {
+	int nch = profile->max_nch(mdev);
 	struct net_device *netdev;
 	struct mlx5e_priv *priv;
-	int nch = profile->max_nch(mdev);
-	int err;
 
 	netdev = alloc_etherdev_mqs(sizeof(struct mlx5e_priv),
 				    nch * profile->max_tc,
@@ -3423,12 +3820,31 @@ void *mlx5e_create_netdev(struct mlx5_core_dev *mdev,
 
 	priv->wq = create_singlethread_workqueue("mlx5e");
 	if (!priv->wq)
-		goto err_free_netdev;
+		goto err_cleanup_nic;
+
+	return netdev;
+
+err_cleanup_nic:
+	profile->cleanup(priv);
+	free_netdev(netdev);
+
+	return NULL;
+}
+
+int mlx5e_attach_netdev(struct mlx5_core_dev *mdev, struct net_device *netdev)
+{
+	const struct mlx5e_profile *profile;
+	struct mlx5e_priv *priv;
+	int err;
+
+	priv = netdev_priv(netdev);
+	profile = priv->profile;
+	clear_bit(MLX5E_STATE_DESTROYING, &priv->state);
 
 	err = mlx5e_create_umr_mkey(priv);
 	if (err) {
 		mlx5_core_err(mdev, "create umr mkey failed, %d\n", err);
-		goto err_destroy_wq;
+		goto out;
 	}
 
 	err = profile->init_tx(priv);
@@ -3451,20 +3867,16 @@ void *mlx5e_create_netdev(struct mlx5_core_dev *mdev,
 
 	mlx5e_set_dev_port_mtu(netdev);
 
-	err = register_netdev(netdev);
-	if (err) {
-		mlx5_core_err(mdev, "register_netdev failed, %d\n", err);
-		goto err_dealloc_q_counters;
-	}
-
 	if (profile->enable)
 		profile->enable(priv);
 
-	return priv;
+	rtnl_lock();
+	if (netif_running(netdev))
+		mlx5e_open(netdev);
+	netif_device_attach(netdev);
+	rtnl_unlock();
 
-err_dealloc_q_counters:
-	mlx5e_destroy_q_counter(priv);
-	profile->cleanup_rx(priv);
+	return 0;
 
 err_close_drop_rq:
 	mlx5e_close_drop_rq(priv);
@@ -3475,13 +3887,8 @@ err_cleanup_tx:
 err_destroy_umr_mkey:
 	mlx5_core_destroy_mkey(mdev, &priv->umr_mkey);
 
-err_destroy_wq:
-	destroy_workqueue(priv->wq);
-
-err_free_netdev:
-	free_netdev(netdev);
-
-	return NULL;
+out:
+	return err;
 }
 
 static void mlx5e_register_vport_rep(struct mlx5_core_dev *mdev)
@@ -3503,20 +3910,84 @@ static void mlx5e_register_vport_rep(struct mlx5_core_dev *mdev)
 		rep.unload = mlx5e_vport_rep_unload;
 		rep.vport = vport;
 		ether_addr_copy(rep.hw_id, mac);
-		mlx5_eswitch_register_vport_rep(esw, &rep);
+		mlx5_eswitch_register_vport_rep(esw, vport, &rep);
 	}
 }
 
+void mlx5e_detach_netdev(struct mlx5_core_dev *mdev, struct net_device *netdev)
+{
+	struct mlx5e_priv *priv = netdev_priv(netdev);
+	const struct mlx5e_profile *profile = priv->profile;
+
+	set_bit(MLX5E_STATE_DESTROYING, &priv->state);
+	if (profile->disable)
+		profile->disable(priv);
+
+	flush_workqueue(priv->wq);
+
+	rtnl_lock();
+	if (netif_running(netdev))
+		mlx5e_close(netdev);
+	netif_device_detach(netdev);
+	rtnl_unlock();
+
+	mlx5e_destroy_q_counter(priv);
+	profile->cleanup_rx(priv);
+	mlx5e_close_drop_rq(priv);
+	profile->cleanup_tx(priv);
+	mlx5_core_destroy_mkey(priv->mdev, &priv->umr_mkey);
+	cancel_delayed_work_sync(&priv->update_stats_work);
+}
+
+/* mlx5e_attach and mlx5e_detach scope should be only creating/destroying
+ * hardware contexts and to connect it to the current netdev.
+ */
+static int mlx5e_attach(struct mlx5_core_dev *mdev, void *vpriv)
+{
+	struct mlx5e_priv *priv = vpriv;
+	struct net_device *netdev = priv->netdev;
+	int err;
+
+	if (netif_device_present(netdev))
+		return 0;
+
+	err = mlx5e_create_mdev_resources(mdev);
+	if (err)
+		return err;
+
+	err = mlx5e_attach_netdev(mdev, netdev);
+	if (err) {
+		mlx5e_destroy_mdev_resources(mdev);
+		return err;
+	}
+
+	return 0;
+}
+
+static void mlx5e_detach(struct mlx5_core_dev *mdev, void *vpriv)
+{
+	struct mlx5e_priv *priv = vpriv;
+	struct net_device *netdev = priv->netdev;
+
+	if (!netif_device_present(netdev))
+		return;
+
+	mlx5e_detach_netdev(mdev, netdev);
+	mlx5e_destroy_mdev_resources(mdev);
+}
+
 static void *mlx5e_add(struct mlx5_core_dev *mdev)
 {
 	struct mlx5_eswitch *esw = mdev->priv.eswitch;
+	int total_vfs = MLX5_TOTAL_VPORTS(mdev);
 	void *ppriv = NULL;
-	void *ret;
-
-	if (mlx5e_check_required_hca_cap(mdev))
-		return NULL;
+	void *priv;
+	int vport;
+	int err;
+	struct net_device *netdev;
 
-	if (mlx5e_create_mdev_resources(mdev))
+	err = mlx5e_check_required_hca_cap(mdev);
+	if (err)
 		return NULL;
 
 	mlx5e_register_vport_rep(mdev);
@@ -3524,12 +3995,39 @@ static void *mlx5e_add(struct mlx5_core_dev *mdev)
 	if (MLX5_CAP_GEN(mdev, vport_group_manager))
 		ppriv = &esw->offloads.vport_reps[0];
 
-	ret = mlx5e_create_netdev(mdev, &mlx5e_nic_profile, ppriv);
-	if (!ret) {
-		mlx5e_destroy_mdev_resources(mdev);
-		return NULL;
+	netdev = mlx5e_create_netdev(mdev, &mlx5e_nic_profile, ppriv);
+	if (!netdev) {
+		mlx5_core_err(mdev, "mlx5e_create_netdev failed\n");
+		goto err_unregister_reps;
 	}
-	return ret;
+
+	priv = netdev_priv(netdev);
+
+	err = mlx5e_attach(mdev, priv);
+	if (err) {
+		mlx5_core_err(mdev, "mlx5e_attach failed, %d\n", err);
+		goto err_destroy_netdev;
+	}
+
+	err = register_netdev(netdev);
+	if (err) {
+		mlx5_core_err(mdev, "register_netdev failed, %d\n", err);
+		goto err_detach;
+	}
+
+	return priv;
+
+err_detach:
+	mlx5e_detach(mdev, priv);
+
+err_destroy_netdev:
+	mlx5e_destroy_netdev(mdev, priv);
+
+err_unregister_reps:
+	for (vport = 1; vport < total_vfs; vport++)
+		mlx5_eswitch_unregister_vport_rep(esw, vport);
+
+	return NULL;
 }
 
 void mlx5e_destroy_netdev(struct mlx5_core_dev *mdev, struct mlx5e_priv *priv)
@@ -3537,30 +4035,11 @@ void mlx5e_destroy_netdev(struct mlx5_core_dev *mdev, struct mlx5e_priv *priv)
 	const struct mlx5e_profile *profile = priv->profile;
 	struct net_device *netdev = priv->netdev;
 
-	set_bit(MLX5E_STATE_DESTROYING, &priv->state);
-	if (profile->disable)
-		profile->disable(priv);
-
-	flush_workqueue(priv->wq);
-	if (test_bit(MLX5_INTERFACE_STATE_SHUTDOWN, &mdev->intf_state)) {
-		netif_device_detach(netdev);
-		mlx5e_close(netdev);
-	} else {
-		unregister_netdev(netdev);
-	}
-
-	mlx5e_destroy_q_counter(priv);
-	profile->cleanup_rx(priv);
-	mlx5e_close_drop_rq(priv);
-	profile->cleanup_tx(priv);
-	mlx5_core_destroy_mkey(priv->mdev, &priv->umr_mkey);
-	cancel_delayed_work_sync(&priv->update_stats_work);
+	unregister_netdev(netdev);
 	destroy_workqueue(priv->wq);
 	if (profile->cleanup)
 		profile->cleanup(priv);
-
-	if (!test_bit(MLX5_INTERFACE_STATE_SHUTDOWN, &mdev->intf_state))
-		free_netdev(netdev);
+	free_netdev(netdev);
 }
 
 static void mlx5e_remove(struct mlx5_core_dev *mdev, void *vpriv)
@@ -3570,12 +4049,11 @@ static void mlx5e_remove(struct mlx5_core_dev *mdev, void *vpriv)
 	struct mlx5e_priv *priv = vpriv;
 	int vport;
 
-	mlx5e_destroy_netdev(mdev, priv);
-
 	for (vport = 1; vport < total_vfs; vport++)
 		mlx5_eswitch_unregister_vport_rep(esw, vport);
 
-	mlx5e_destroy_mdev_resources(mdev);
+	mlx5e_detach(mdev, vpriv);
+	mlx5e_destroy_netdev(mdev, priv);
 }
 
 static void *mlx5e_get_netdev(void *vpriv)
@@ -3588,6 +4066,8 @@ static void *mlx5e_get_netdev(void *vpriv)
 static struct mlx5_interface mlx5e_interface = {
 	.add       = mlx5e_add,
 	.remove    = mlx5e_remove,
+	.attach    = mlx5e_attach,
+	.detach    = mlx5e_detach,
 	.event     = mlx5e_async_event,
 	.protocol  = MLX5_INTERFACE_PROTOCOL_ETH,
 	.get_dev   = mlx5e_get_netdev,
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_rep.c b/drivers/net/ethernet/mellanox/mlx5/core/en_rep.c
index 134de4a11f1d..3c97da103d30 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_rep.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_rep.c
@@ -413,19 +413,50 @@ static struct mlx5e_profile mlx5e_rep_profile = {
 int mlx5e_vport_rep_load(struct mlx5_eswitch *esw,
 			 struct mlx5_eswitch_rep *rep)
 {
-	rep->priv_data = mlx5e_create_netdev(esw->dev, &mlx5e_rep_profile, rep);
-	if (!rep->priv_data) {
-		pr_warn("Failed to create representor for vport %d\n",
+	struct net_device *netdev;
+	int err;
+
+	netdev = mlx5e_create_netdev(esw->dev, &mlx5e_rep_profile, rep);
+	if (!netdev) {
+		pr_warn("Failed to create representor netdev for vport %d\n",
 			rep->vport);
 		return -EINVAL;
 	}
+
+	rep->priv_data = netdev_priv(netdev);
+
+	err = mlx5e_attach_netdev(esw->dev, netdev);
+	if (err) {
+		pr_warn("Failed to attach representor netdev for vport %d\n",
+			rep->vport);
+		goto err_destroy_netdev;
+	}
+
+	err = register_netdev(netdev);
+	if (err) {
+		pr_warn("Failed to register representor netdev for vport %d\n",
+			rep->vport);
+		goto err_detach_netdev;
+	}
+
 	return 0;
+
+err_detach_netdev:
+	mlx5e_detach_netdev(esw->dev, netdev);
+
+err_destroy_netdev:
+	mlx5e_destroy_netdev(esw->dev, rep->priv_data);
+
+	return err;
+
 }
 
 void mlx5e_vport_rep_unload(struct mlx5_eswitch *esw,
 			    struct mlx5_eswitch_rep *rep)
 {
 	struct mlx5e_priv *priv = rep->priv_data;
+	struct net_device *netdev = priv->netdev;
 
+	mlx5e_detach_netdev(esw->dev, netdev);
 	mlx5e_destroy_netdev(esw->dev, priv);
 }
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_rx.c b/drivers/net/ethernet/mellanox/mlx5/core/en_rx.c
index e7c969df3dad..c6de6fba5843 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_rx.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_rx.c
@@ -36,6 +36,7 @@
 #include <net/busy_poll.h>
 #include "en.h"
 #include "en_tc.h"
+#include "eswitch.h"
 
 static inline bool mlx5e_rx_hw_stamp(struct mlx5e_tstamp *tstamp)
 {
@@ -179,96 +180,111 @@ unlock:
 	mutex_unlock(&priv->state_lock);
 }
 
-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;
+#define RQ_PAGE_SIZE(rq) ((1 << rq->buff.page_order) << PAGE_SHIFT)
 
-	skb = napi_alloc_skb(rq->cq.napi, rq->wqe_sz);
-	if (unlikely(!skb))
-		return -ENOMEM;
+static inline bool mlx5e_rx_cache_put(struct mlx5e_rq *rq,
+				      struct mlx5e_dma_info *dma_info)
+{
+	struct mlx5e_page_cache *cache = &rq->page_cache;
+	u32 tail_next = (cache->tail + 1) & (MLX5E_CACHE_SIZE - 1);
 
-	dma_addr = dma_map_single(rq->pdev,
-				  /* hw start padding */
-				  skb->data,
-				  /* hw end padding */
-				  rq->wqe_sz,
-				  DMA_FROM_DEVICE);
+	if (tail_next == cache->head) {
+		rq->stats.cache_full++;
+		return false;
+	}
 
-	if (unlikely(dma_mapping_error(rq->pdev, dma_addr)))
-		goto err_free_skb;
+	cache->page_cache[cache->tail] = *dma_info;
+	cache->tail = tail_next;
+	return true;
+}
 
-	*((dma_addr_t *)skb->cb) = dma_addr;
-	wqe->data.addr = cpu_to_be64(dma_addr);
-	wqe->data.lkey = rq->mkey_be;
+static inline bool mlx5e_rx_cache_get(struct mlx5e_rq *rq,
+				      struct mlx5e_dma_info *dma_info)
+{
+	struct mlx5e_page_cache *cache = &rq->page_cache;
 
-	rq->skb[ix] = skb;
+	if (unlikely(cache->head == cache->tail)) {
+		rq->stats.cache_empty++;
+		return false;
+	}
 
-	return 0;
+	if (page_ref_count(cache->page_cache[cache->head].page) != 1) {
+		rq->stats.cache_busy++;
+		return false;
+	}
 
-err_free_skb:
-	dev_kfree_skb(skb);
+	*dma_info = cache->page_cache[cache->head];
+	cache->head = (cache->head + 1) & (MLX5E_CACHE_SIZE - 1);
+	rq->stats.cache_reuse++;
 
-	return -ENOMEM;
+	dma_sync_single_for_device(rq->pdev, dma_info->addr,
+				   RQ_PAGE_SIZE(rq),
+				   DMA_FROM_DEVICE);
+	return true;
 }
 
-void mlx5e_dealloc_rx_wqe(struct mlx5e_rq *rq, u16 ix)
+static inline int mlx5e_page_alloc_mapped(struct mlx5e_rq *rq,
+					  struct mlx5e_dma_info *dma_info)
 {
-	struct sk_buff *skb = rq->skb[ix];
+	struct page *page;
+
+	if (mlx5e_rx_cache_get(rq, dma_info))
+		return 0;
+
+	page = dev_alloc_pages(rq->buff.page_order);
+	if (unlikely(!page))
+		return -ENOMEM;
 
-	if (skb) {
-		rq->skb[ix] = NULL;
-		dma_unmap_single(rq->pdev,
-				 *((dma_addr_t *)skb->cb),
-				 rq->wqe_sz,
-				 DMA_FROM_DEVICE);
-		dev_kfree_skb(skb);
+	dma_info->page = page;
+	dma_info->addr = dma_map_page(rq->pdev, page, 0,
+				      RQ_PAGE_SIZE(rq), rq->buff.map_dir);
+	if (unlikely(dma_mapping_error(rq->pdev, dma_info->addr))) {
+		put_page(page);
+		return -ENOMEM;
 	}
+
+	return 0;
 }
 
-static inline int mlx5e_mpwqe_strides_per_page(struct mlx5e_rq *rq)
+void mlx5e_page_release(struct mlx5e_rq *rq, struct mlx5e_dma_info *dma_info,
+			bool recycle)
 {
-	return rq->mpwqe_num_strides >> MLX5_MPWRQ_WQE_PAGE_ORDER;
+	if (likely(recycle) && mlx5e_rx_cache_put(rq, dma_info))
+		return;
+
+	dma_unmap_page(rq->pdev, dma_info->addr, RQ_PAGE_SIZE(rq),
+		       rq->buff.map_dir);
+	put_page(dma_info->page);
 }
 
-static inline void
-mlx5e_dma_pre_sync_linear_mpwqe(struct device *pdev,
-				struct mlx5e_mpw_info *wi,
-				u32 wqe_offset, u32 len)
+int mlx5e_alloc_rx_wqe(struct mlx5e_rq *rq, struct mlx5e_rx_wqe *wqe, u16 ix)
 {
-	dma_sync_single_for_cpu(pdev, wi->dma_info.addr + wqe_offset,
-				len, DMA_FROM_DEVICE);
+	struct mlx5e_dma_info *di = &rq->dma_info[ix];
+
+	if (unlikely(mlx5e_page_alloc_mapped(rq, di)))
+		return -ENOMEM;
+
+	wqe->data.addr = cpu_to_be64(di->addr + MLX5_RX_HEADROOM);
+	return 0;
 }
 
-static inline void
-mlx5e_dma_pre_sync_fragmented_mpwqe(struct device *pdev,
-				    struct mlx5e_mpw_info *wi,
-				    u32 wqe_offset, u32 len)
+void mlx5e_dealloc_rx_wqe(struct mlx5e_rq *rq, u16 ix)
 {
-	/* No dma pre sync for fragmented MPWQE */
+	struct mlx5e_dma_info *di = &rq->dma_info[ix];
+
+	mlx5e_page_release(rq, di, true);
 }
 
-static inline void
-mlx5e_add_skb_frag_linear_mpwqe(struct mlx5e_rq *rq,
-				struct sk_buff *skb,
-				struct mlx5e_mpw_info *wi,
-				u32 page_idx, u32 frag_offset,
-				u32 len)
+static inline int mlx5e_mpwqe_strides_per_page(struct mlx5e_rq *rq)
 {
-	unsigned int truesize =	ALIGN(len, rq->mpwqe_stride_sz);
-
-	wi->skbs_frags[page_idx]++;
-	skb_add_rx_frag(skb, skb_shinfo(skb)->nr_frags,
-			&wi->dma_info.page[page_idx], frag_offset,
-			len, truesize);
+	return rq->mpwqe_num_strides >> MLX5_MPWRQ_WQE_PAGE_ORDER;
 }
 
-static inline void
-mlx5e_add_skb_frag_fragmented_mpwqe(struct mlx5e_rq *rq,
-				    struct sk_buff *skb,
-				    struct mlx5e_mpw_info *wi,
-				    u32 page_idx, u32 frag_offset,
-				    u32 len)
+static inline void mlx5e_add_skb_frag_mpwqe(struct mlx5e_rq *rq,
+					    struct sk_buff *skb,
+					    struct mlx5e_mpw_info *wi,
+					    u32 page_idx, u32 frag_offset,
+					    u32 len)
 {
 	unsigned int truesize =	ALIGN(len, rq->mpwqe_stride_sz);
 
@@ -282,24 +298,11 @@ mlx5e_add_skb_frag_fragmented_mpwqe(struct mlx5e_rq *rq,
 }
 
 static inline void
-mlx5e_copy_skb_header_linear_mpwqe(struct device *pdev,
-				   struct sk_buff *skb,
-				   struct mlx5e_mpw_info *wi,
-				   u32 page_idx, u32 offset,
-				   u32 headlen)
-{
-	struct page *page = &wi->dma_info.page[page_idx];
-
-	skb_copy_to_linear_data(skb, page_address(page) + offset,
-				ALIGN(headlen, sizeof(long)));
-}
-
-static inline void
-mlx5e_copy_skb_header_fragmented_mpwqe(struct device *pdev,
-				       struct sk_buff *skb,
-				       struct mlx5e_mpw_info *wi,
-				       u32 page_idx, u32 offset,
-				       u32 headlen)
+mlx5e_copy_skb_header_mpwqe(struct device *pdev,
+			    struct sk_buff *skb,
+			    struct mlx5e_mpw_info *wi,
+			    u32 page_idx, u32 offset,
+			    u32 headlen)
 {
 	u16 headlen_pg = min_t(u32, headlen, PAGE_SIZE - offset);
 	struct mlx5e_dma_info *dma_info = &wi->umr.dma_info[page_idx];
@@ -324,46 +327,9 @@ mlx5e_copy_skb_header_fragmented_mpwqe(struct device *pdev,
 	}
 }
 
-static u32 mlx5e_get_wqe_mtt_offset(struct mlx5e_rq *rq, u16 wqe_ix)
-{
-	return rq->mpwqe_mtt_offset +
-		wqe_ix * ALIGN(MLX5_MPWRQ_PAGES_PER_WQE, 8);
-}
-
-static void mlx5e_build_umr_wqe(struct mlx5e_rq *rq,
-				struct mlx5e_sq *sq,
-				struct mlx5e_umr_wqe *wqe,
-				u16 ix)
-{
-	struct mlx5_wqe_ctrl_seg      *cseg = &wqe->ctrl;
-	struct mlx5_wqe_umr_ctrl_seg *ucseg = &wqe->uctrl;
-	struct mlx5_wqe_data_seg      *dseg = &wqe->data;
-	struct mlx5e_mpw_info *wi = &rq->wqe_info[ix];
-	u8 ds_cnt = DIV_ROUND_UP(sizeof(*wqe), MLX5_SEND_WQE_DS);
-	u32 umr_wqe_mtt_offset = mlx5e_get_wqe_mtt_offset(rq, ix);
-
-	memset(wqe, 0, sizeof(*wqe));
-	cseg->opmod_idx_opcode =
-		cpu_to_be32((sq->pc << MLX5_WQE_CTRL_WQE_INDEX_SHIFT) |
-			    MLX5_OPCODE_UMR);
-	cseg->qpn_ds    = cpu_to_be32((sq->sqn << MLX5_WQE_CTRL_QPN_SHIFT) |
-				      ds_cnt);
-	cseg->fm_ce_se  = MLX5_WQE_CTRL_CQ_UPDATE;
-	cseg->imm       = rq->umr_mkey_be;
-
-	ucseg->flags = MLX5_UMR_TRANSLATION_OFFSET_EN;
-	ucseg->klm_octowords =
-		cpu_to_be16(MLX5_MTT_OCTW(MLX5_MPWRQ_PAGES_PER_WQE));
-	ucseg->bsf_octowords =
-		cpu_to_be16(MLX5_MTT_OCTW(umr_wqe_mtt_offset));
-	ucseg->mkey_mask     = cpu_to_be64(MLX5_MKEY_MASK_FREE);
-
-	dseg->lkey = sq->mkey_be;
-	dseg->addr = cpu_to_be64(wi->umr.mtt_addr);
-}
-
-static void mlx5e_post_umr_wqe(struct mlx5e_rq *rq, u16 ix)
+static inline void mlx5e_post_umr_wqe(struct mlx5e_rq *rq, u16 ix)
 {
+	struct mlx5e_mpw_info *wi = &rq->mpwqe.info[ix];
 	struct mlx5e_sq *sq = &rq->channel->icosq;
 	struct mlx5_wq_cyc *wq = &sq->wq;
 	struct mlx5e_umr_wqe *wqe;
@@ -372,135 +338,74 @@ static void mlx5e_post_umr_wqe(struct mlx5e_rq *rq, u16 ix)
 
 	/* fill sq edge with nops to avoid wqe wrap around */
 	while ((pi = (sq->pc & wq->sz_m1)) > sq->edge) {
-		sq->ico_wqe_info[pi].opcode = MLX5_OPCODE_NOP;
-		sq->ico_wqe_info[pi].num_wqebbs = 1;
+		sq->db.ico_wqe[pi].opcode = MLX5_OPCODE_NOP;
+		sq->db.ico_wqe[pi].num_wqebbs = 1;
 		mlx5e_send_nop(sq, true);
 	}
 
 	wqe = mlx5_wq_cyc_get_wqe(wq, pi);
-	mlx5e_build_umr_wqe(rq, sq, wqe, ix);
-	sq->ico_wqe_info[pi].opcode = MLX5_OPCODE_UMR;
-	sq->ico_wqe_info[pi].num_wqebbs = num_wqebbs;
+	memcpy(wqe, &wi->umr.wqe, sizeof(*wqe));
+	wqe->ctrl.opmod_idx_opcode =
+		cpu_to_be32((sq->pc << MLX5_WQE_CTRL_WQE_INDEX_SHIFT) |
+			    MLX5_OPCODE_UMR);
+
+	sq->db.ico_wqe[pi].opcode = MLX5_OPCODE_UMR;
+	sq->db.ico_wqe[pi].num_wqebbs = num_wqebbs;
 	sq->pc += num_wqebbs;
 	mlx5e_tx_notify_hw(sq, &wqe->ctrl, 0);
 }
 
-static inline int mlx5e_get_wqe_mtt_sz(void)
-{
-	/* UMR copies MTTs in units of MLX5_UMR_MTT_ALIGNMENT bytes.
-	 * To avoid copying garbage after the mtt array, we allocate
-	 * a little more.
-	 */
-	return ALIGN(MLX5_MPWRQ_PAGES_PER_WQE * sizeof(__be64),
-		     MLX5_UMR_MTT_ALIGNMENT);
-}
-
-static int mlx5e_alloc_and_map_page(struct mlx5e_rq *rq,
-				    struct mlx5e_mpw_info *wi,
-				    int i)
-{
-	struct page *page;
-
-	page = dev_alloc_page();
-	if (unlikely(!page))
-		return -ENOMEM;
-
-	wi->umr.dma_info[i].page = page;
-	wi->umr.dma_info[i].addr = dma_map_page(rq->pdev, page, 0, PAGE_SIZE,
-						PCI_DMA_FROMDEVICE);
-	if (unlikely(dma_mapping_error(rq->pdev, wi->umr.dma_info[i].addr))) {
-		put_page(page);
-		return -ENOMEM;
-	}
-	wi->umr.mtt[i] = cpu_to_be64(wi->umr.dma_info[i].addr | MLX5_EN_WR);
-
-	return 0;
-}
-
-static int mlx5e_alloc_rx_fragmented_mpwqe(struct mlx5e_rq *rq,
-					   struct mlx5e_rx_wqe *wqe,
-					   u16 ix)
+static int mlx5e_alloc_rx_umr_mpwqe(struct mlx5e_rq *rq,
+				    struct mlx5e_rx_wqe *wqe,
+				    u16 ix)
 {
-	struct mlx5e_mpw_info *wi = &rq->wqe_info[ix];
-	int mtt_sz = mlx5e_get_wqe_mtt_sz();
+	struct mlx5e_mpw_info *wi = &rq->mpwqe.info[ix];
 	u64 dma_offset = (u64)mlx5e_get_wqe_mtt_offset(rq, ix) << PAGE_SHIFT;
+	int pg_strides = mlx5e_mpwqe_strides_per_page(rq);
+	int err;
 	int i;
 
-	wi->umr.dma_info = kmalloc(sizeof(*wi->umr.dma_info) *
-				   MLX5_MPWRQ_PAGES_PER_WQE,
-				   GFP_ATOMIC);
-	if (unlikely(!wi->umr.dma_info))
-		goto err_out;
-
-	/* We allocate more than mtt_sz as we will align the pointer */
-	wi->umr.mtt_no_align = kzalloc(mtt_sz + MLX5_UMR_ALIGN - 1,
-				       GFP_ATOMIC);
-	if (unlikely(!wi->umr.mtt_no_align))
-		goto err_free_umr;
-
-	wi->umr.mtt = PTR_ALIGN(wi->umr.mtt_no_align, MLX5_UMR_ALIGN);
-	wi->umr.mtt_addr = dma_map_single(rq->pdev, wi->umr.mtt, mtt_sz,
-					  PCI_DMA_TODEVICE);
-	if (unlikely(dma_mapping_error(rq->pdev, wi->umr.mtt_addr)))
-		goto err_free_mtt;
-
 	for (i = 0; i < MLX5_MPWRQ_PAGES_PER_WQE; i++) {
-		if (unlikely(mlx5e_alloc_and_map_page(rq, wi, i)))
+		struct mlx5e_dma_info *dma_info = &wi->umr.dma_info[i];
+
+		err = mlx5e_page_alloc_mapped(rq, dma_info);
+		if (unlikely(err))
 			goto err_unmap;
-		page_ref_add(wi->umr.dma_info[i].page,
-			     mlx5e_mpwqe_strides_per_page(rq));
+		wi->umr.mtt[i] = cpu_to_be64(dma_info->addr | MLX5_EN_WR);
+		page_ref_add(dma_info->page, pg_strides);
 		wi->skbs_frags[i] = 0;
 	}
 
 	wi->consumed_strides = 0;
-	wi->dma_pre_sync = mlx5e_dma_pre_sync_fragmented_mpwqe;
-	wi->add_skb_frag = mlx5e_add_skb_frag_fragmented_mpwqe;
-	wi->copy_skb_header = mlx5e_copy_skb_header_fragmented_mpwqe;
-	wi->free_wqe     = mlx5e_free_rx_fragmented_mpwqe;
-	wqe->data.lkey = rq->umr_mkey_be;
 	wqe->data.addr = cpu_to_be64(dma_offset);
 
 	return 0;
 
 err_unmap:
 	while (--i >= 0) {
-		dma_unmap_page(rq->pdev, wi->umr.dma_info[i].addr, PAGE_SIZE,
-			       PCI_DMA_FROMDEVICE);
-		page_ref_sub(wi->umr.dma_info[i].page,
-			     mlx5e_mpwqe_strides_per_page(rq));
-		put_page(wi->umr.dma_info[i].page);
-	}
-	dma_unmap_single(rq->pdev, wi->umr.mtt_addr, mtt_sz, PCI_DMA_TODEVICE);
-
-err_free_mtt:
-	kfree(wi->umr.mtt_no_align);
+		struct mlx5e_dma_info *dma_info = &wi->umr.dma_info[i];
 
-err_free_umr:
-	kfree(wi->umr.dma_info);
+		page_ref_sub(dma_info->page, pg_strides);
+		mlx5e_page_release(rq, dma_info, true);
+	}
 
-err_out:
-	return -ENOMEM;
+	return err;
 }
 
-void mlx5e_free_rx_fragmented_mpwqe(struct mlx5e_rq *rq,
-				    struct mlx5e_mpw_info *wi)
+void mlx5e_free_rx_mpwqe(struct mlx5e_rq *rq, struct mlx5e_mpw_info *wi)
 {
-	int mtt_sz = mlx5e_get_wqe_mtt_sz();
+	int pg_strides = mlx5e_mpwqe_strides_per_page(rq);
 	int i;
 
 	for (i = 0; i < MLX5_MPWRQ_PAGES_PER_WQE; i++) {
-		dma_unmap_page(rq->pdev, wi->umr.dma_info[i].addr, PAGE_SIZE,
-			       PCI_DMA_FROMDEVICE);
-		page_ref_sub(wi->umr.dma_info[i].page,
-			mlx5e_mpwqe_strides_per_page(rq) - wi->skbs_frags[i]);
-		put_page(wi->umr.dma_info[i].page);
+		struct mlx5e_dma_info *dma_info = &wi->umr.dma_info[i];
+
+		page_ref_sub(dma_info->page, pg_strides - wi->skbs_frags[i]);
+		mlx5e_page_release(rq, dma_info, true);
 	}
-	dma_unmap_single(rq->pdev, wi->umr.mtt_addr, mtt_sz, PCI_DMA_TODEVICE);
-	kfree(wi->umr.mtt_no_align);
-	kfree(wi->umr.dma_info);
 }
 
-void mlx5e_post_rx_fragmented_mpwqe(struct mlx5e_rq *rq)
+void mlx5e_post_rx_mpwqe(struct mlx5e_rq *rq)
 {
 	struct mlx5_wq_ll *wq = &rq->wq;
 	struct mlx5e_rx_wqe *wqe = mlx5_wq_ll_get_wqe(wq, wq->head);
@@ -508,12 +413,11 @@ void mlx5e_post_rx_fragmented_mpwqe(struct mlx5e_rq *rq)
 	clear_bit(MLX5E_RQ_STATE_UMR_WQE_IN_PROGRESS, &rq->state);
 
 	if (unlikely(test_bit(MLX5E_RQ_STATE_FLUSH, &rq->state))) {
-		mlx5e_free_rx_fragmented_mpwqe(rq, &rq->wqe_info[wq->head]);
+		mlx5e_free_rx_mpwqe(rq, &rq->mpwqe.info[wq->head]);
 		return;
 	}
 
 	mlx5_wq_ll_push(wq, be16_to_cpu(wqe->next.next_wqe_index));
-	rq->stats.mpwqe_frag++;
 
 	/* ensure wqes are visible to device before updating doorbell record */
 	dma_wmb();
@@ -521,84 +425,23 @@ void mlx5e_post_rx_fragmented_mpwqe(struct mlx5e_rq *rq)
 	mlx5_wq_ll_update_db_record(wq);
 }
 
-static int mlx5e_alloc_rx_linear_mpwqe(struct mlx5e_rq *rq,
-				       struct mlx5e_rx_wqe *wqe,
-				       u16 ix)
-{
-	struct mlx5e_mpw_info *wi = &rq->wqe_info[ix];
-	gfp_t gfp_mask;
-	int i;
-
-	gfp_mask = GFP_ATOMIC | __GFP_COLD | __GFP_MEMALLOC;
-	wi->dma_info.page = alloc_pages_node(NUMA_NO_NODE, gfp_mask,
-					     MLX5_MPWRQ_WQE_PAGE_ORDER);
-	if (unlikely(!wi->dma_info.page))
-		return -ENOMEM;
-
-	wi->dma_info.addr = dma_map_page(rq->pdev, wi->dma_info.page, 0,
-					 rq->wqe_sz, PCI_DMA_FROMDEVICE);
-	if (unlikely(dma_mapping_error(rq->pdev, wi->dma_info.addr))) {
-		put_page(wi->dma_info.page);
-		return -ENOMEM;
-	}
-
-	/* We split the high-order page into order-0 ones and manage their
-	 * reference counter to minimize the memory held by small skb fragments
-	 */
-	split_page(wi->dma_info.page, MLX5_MPWRQ_WQE_PAGE_ORDER);
-	for (i = 0; i < MLX5_MPWRQ_PAGES_PER_WQE; i++) {
-		page_ref_add(&wi->dma_info.page[i],
-			     mlx5e_mpwqe_strides_per_page(rq));
-		wi->skbs_frags[i] = 0;
-	}
-
-	wi->consumed_strides = 0;
-	wi->dma_pre_sync = mlx5e_dma_pre_sync_linear_mpwqe;
-	wi->add_skb_frag = mlx5e_add_skb_frag_linear_mpwqe;
-	wi->copy_skb_header = mlx5e_copy_skb_header_linear_mpwqe;
-	wi->free_wqe     = mlx5e_free_rx_linear_mpwqe;
-	wqe->data.lkey = rq->mkey_be;
-	wqe->data.addr = cpu_to_be64(wi->dma_info.addr);
-
-	return 0;
-}
-
-void mlx5e_free_rx_linear_mpwqe(struct mlx5e_rq *rq,
-				struct mlx5e_mpw_info *wi)
-{
-	int i;
-
-	dma_unmap_page(rq->pdev, wi->dma_info.addr, rq->wqe_sz,
-		       PCI_DMA_FROMDEVICE);
-	for (i = 0; i < MLX5_MPWRQ_PAGES_PER_WQE; i++) {
-		page_ref_sub(&wi->dma_info.page[i],
-			mlx5e_mpwqe_strides_per_page(rq) - wi->skbs_frags[i]);
-		put_page(&wi->dma_info.page[i]);
-	}
-}
-
 int mlx5e_alloc_rx_mpwqe(struct mlx5e_rq *rq, struct mlx5e_rx_wqe *wqe, u16 ix)
 {
 	int err;
 
-	err = mlx5e_alloc_rx_linear_mpwqe(rq, wqe, ix);
-	if (unlikely(err)) {
-		err = mlx5e_alloc_rx_fragmented_mpwqe(rq, wqe, ix);
-		if (unlikely(err))
-			return err;
-		set_bit(MLX5E_RQ_STATE_UMR_WQE_IN_PROGRESS, &rq->state);
-		mlx5e_post_umr_wqe(rq, ix);
-		return -EBUSY;
-	}
-
-	return 0;
+	err = mlx5e_alloc_rx_umr_mpwqe(rq, wqe, ix);
+	if (unlikely(err))
+		return err;
+	set_bit(MLX5E_RQ_STATE_UMR_WQE_IN_PROGRESS, &rq->state);
+	mlx5e_post_umr_wqe(rq, ix);
+	return -EBUSY;
 }
 
 void mlx5e_dealloc_rx_mpwqe(struct mlx5e_rq *rq, u16 ix)
 {
-	struct mlx5e_mpw_info *wi = &rq->wqe_info[ix];
+	struct mlx5e_mpw_info *wi = &rq->mpwqe.info[ix];
 
-	wi->free_wqe(rq, wi);
+	mlx5e_free_rx_mpwqe(rq, wi);
 }
 
 #define RQ_CANNOT_POST(rq) \
@@ -617,9 +460,10 @@ bool mlx5e_post_rx_wqes(struct mlx5e_rq *rq)
 		int err;
 
 		err = rq->alloc_wqe(rq, wqe, wq->head);
+		if (err == -EBUSY)
+			return true;
 		if (unlikely(err)) {
-			if (err != -EBUSY)
-				rq->stats.buff_alloc_err++;
+			rq->stats.buff_alloc_err++;
 			break;
 		}
 
@@ -786,40 +630,207 @@ static inline void mlx5e_complete_rx_cqe(struct mlx5e_rq *rq,
 	rq->stats.packets++;
 	rq->stats.bytes += cqe_bcnt;
 	mlx5e_build_rx_skb(cqe, cqe_bcnt, rq, skb);
-	napi_gro_receive(rq->cq.napi, skb);
+}
+
+static inline void mlx5e_xmit_xdp_doorbell(struct mlx5e_sq *sq)
+{
+	struct mlx5_wq_cyc *wq = &sq->wq;
+	struct mlx5e_tx_wqe *wqe;
+	u16 pi = (sq->pc - MLX5E_XDP_TX_WQEBBS) & wq->sz_m1; /* last pi */
+
+	wqe  = mlx5_wq_cyc_get_wqe(wq, pi);
+
+	wqe->ctrl.fm_ce_se = MLX5_WQE_CTRL_CQ_UPDATE;
+	mlx5e_tx_notify_hw(sq, &wqe->ctrl, 0);
+}
+
+static inline void mlx5e_xmit_xdp_frame(struct mlx5e_rq *rq,
+					struct mlx5e_dma_info *di,
+					unsigned int data_offset,
+					int len)
+{
+	struct mlx5e_sq          *sq   = &rq->channel->xdp_sq;
+	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 mlx5e_sq_wqe_info *wi   = &sq->db.xdp.wqe_info[pi];
+
+	struct mlx5_wqe_ctrl_seg *cseg = &wqe->ctrl;
+	struct mlx5_wqe_eth_seg  *eseg = &wqe->eth;
+	struct mlx5_wqe_data_seg *dseg;
+
+	dma_addr_t dma_addr  = di->addr + data_offset + MLX5E_XDP_MIN_INLINE;
+	unsigned int dma_len = len - MLX5E_XDP_MIN_INLINE;
+	void *data           = page_address(di->page) + data_offset;
+
+	if (unlikely(!mlx5e_sq_has_room_for(sq, MLX5E_XDP_TX_WQEBBS))) {
+		if (sq->db.xdp.doorbell) {
+			/* SQ is full, ring doorbell */
+			mlx5e_xmit_xdp_doorbell(sq);
+			sq->db.xdp.doorbell = false;
+		}
+		rq->stats.xdp_tx_full++;
+		mlx5e_page_release(rq, di, true);
+		return;
+	}
+
+	dma_sync_single_for_device(sq->pdev, dma_addr, dma_len,
+				   PCI_DMA_TODEVICE);
+
+	memset(wqe, 0, sizeof(*wqe));
+
+	/* copy the inline part */
+	memcpy(eseg->inline_hdr_start, data, MLX5E_XDP_MIN_INLINE);
+	eseg->inline_hdr_sz = cpu_to_be16(MLX5E_XDP_MIN_INLINE);
+
+	dseg = (struct mlx5_wqe_data_seg *)cseg + (MLX5E_XDP_TX_DS_COUNT - 1);
+
+	/* write the dma part */
+	dseg->addr       = cpu_to_be64(dma_addr);
+	dseg->byte_count = cpu_to_be32(dma_len);
+	dseg->lkey       = sq->mkey_be;
+
+	cseg->opmod_idx_opcode = cpu_to_be32((sq->pc << 8) | MLX5_OPCODE_SEND);
+	cseg->qpn_ds = cpu_to_be32((sq->sqn << 8) | MLX5E_XDP_TX_DS_COUNT);
+
+	sq->db.xdp.di[pi] = *di;
+	wi->opcode     = MLX5_OPCODE_SEND;
+	wi->num_wqebbs = MLX5E_XDP_TX_WQEBBS;
+	sq->pc += MLX5E_XDP_TX_WQEBBS;
+
+	sq->db.xdp.doorbell = true;
+	rq->stats.xdp_tx++;
+}
+
+/* returns true if packet was consumed by xdp */
+static inline bool mlx5e_xdp_handle(struct mlx5e_rq *rq,
+				    const struct bpf_prog *prog,
+				    struct mlx5e_dma_info *di,
+				    void *data, u16 len)
+{
+	struct xdp_buff xdp;
+	u32 act;
+
+	if (!prog)
+		return false;
+
+	xdp.data = data;
+	xdp.data_end = xdp.data + len;
+	act = bpf_prog_run_xdp(prog, &xdp);
+	switch (act) {
+	case XDP_PASS:
+		return false;
+	case XDP_TX:
+		mlx5e_xmit_xdp_frame(rq, di, MLX5_RX_HEADROOM, len);
+		return true;
+	default:
+		bpf_warn_invalid_xdp_action(act);
+	case XDP_ABORTED:
+	case XDP_DROP:
+		rq->stats.xdp_drop++;
+		mlx5e_page_release(rq, di, true);
+		return true;
+	}
+}
+
+static inline
+struct sk_buff *skb_from_cqe(struct mlx5e_rq *rq, struct mlx5_cqe64 *cqe,
+			     u16 wqe_counter, u32 cqe_bcnt)
+{
+	struct bpf_prog *xdp_prog = READ_ONCE(rq->xdp_prog);
+	struct mlx5e_dma_info *di;
+	struct sk_buff *skb;
+	void *va, *data;
+
+	di             = &rq->dma_info[wqe_counter];
+	va             = page_address(di->page);
+	data           = va + MLX5_RX_HEADROOM;
+
+	dma_sync_single_range_for_cpu(rq->pdev,
+				      di->addr,
+				      MLX5_RX_HEADROOM,
+				      rq->buff.wqe_sz,
+				      DMA_FROM_DEVICE);
+	prefetch(data);
+
+	if (unlikely((cqe->op_own >> 4) != MLX5_CQE_RESP_SEND)) {
+		rq->stats.wqe_err++;
+		mlx5e_page_release(rq, di, true);
+		return NULL;
+	}
+
+	if (mlx5e_xdp_handle(rq, xdp_prog, di, data, cqe_bcnt))
+		return NULL; /* page/packet was consumed by XDP */
+
+	skb = build_skb(va, RQ_PAGE_SIZE(rq));
+	if (unlikely(!skb)) {
+		rq->stats.buff_alloc_err++;
+		mlx5e_page_release(rq, di, true);
+		return NULL;
+	}
+
+	/* queue up for recycling ..*/
+	page_ref_inc(di->page);
+	mlx5e_page_release(rq, di, true);
+
+	skb_reserve(skb, MLX5_RX_HEADROOM);
+	skb_put(skb, cqe_bcnt);
+
+	return skb;
 }
 
 void mlx5e_handle_rx_cqe(struct mlx5e_rq *rq, struct mlx5_cqe64 *cqe)
 {
 	struct mlx5e_rx_wqe *wqe;
-	struct sk_buff *skb;
 	__be16 wqe_counter_be;
+	struct sk_buff *skb;
 	u16 wqe_counter;
 	u32 cqe_bcnt;
 
 	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;
+	cqe_bcnt       = be32_to_cpu(cqe->byte_cnt);
 
-	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);
+	skb = skb_from_cqe(rq, cqe, wqe_counter, cqe_bcnt);
+	if (!skb)
 		goto wq_ll_pop;
-	}
 
-	cqe_bcnt = be32_to_cpu(cqe->byte_cnt);
-	skb_put(skb, cqe_bcnt);
+	mlx5e_complete_rx_cqe(rq, cqe, cqe_bcnt, skb);
+	napi_gro_receive(rq->cq.napi, skb);
+
+wq_ll_pop:
+	mlx5_wq_ll_pop(&rq->wq, wqe_counter_be,
+		       &wqe->next.next_wqe_index);
+}
+
+void mlx5e_handle_rx_cqe_rep(struct mlx5e_rq *rq, struct mlx5_cqe64 *cqe)
+{
+	struct net_device *netdev = rq->netdev;
+	struct mlx5e_priv *priv = netdev_priv(netdev);
+	struct mlx5_eswitch_rep *rep = priv->ppriv;
+	struct mlx5e_rx_wqe *wqe;
+	struct sk_buff *skb;
+	__be16 wqe_counter_be;
+	u16 wqe_counter;
+	u32 cqe_bcnt;
+
+	wqe_counter_be = cqe->wqe_counter;
+	wqe_counter    = be16_to_cpu(wqe_counter_be);
+	wqe            = mlx5_wq_ll_get_wqe(&rq->wq, wqe_counter);
+	cqe_bcnt       = be32_to_cpu(cqe->byte_cnt);
+
+	skb = skb_from_cqe(rq, cqe, wqe_counter, cqe_bcnt);
+	if (!skb)
+		goto wq_ll_pop;
 
 	mlx5e_complete_rx_cqe(rq, cqe, cqe_bcnt, skb);
 
+	if (rep->vlan && skb_vlan_tag_present(skb))
+		skb_vlan_pop(skb);
+
+	napi_gro_receive(rq->cq.napi, skb);
+
 wq_ll_pop:
 	mlx5_wq_ll_pop(&rq->wq, wqe_counter_be,
 		       &wqe->next.next_wqe_index);
@@ -831,7 +842,6 @@ static inline void mlx5e_mpwqe_fill_rx_skb(struct mlx5e_rq *rq,
 					   u32 cqe_bcnt,
 					   struct sk_buff *skb)
 {
-	u32 consumed_bytes = ALIGN(cqe_bcnt, rq->mpwqe_stride_sz);
 	u16 stride_ix      = mpwrq_get_cqe_stride_index(cqe);
 	u32 wqe_offset     = stride_ix * rq->mpwqe_stride_sz;
 	u32 head_offset    = wqe_offset & (PAGE_SIZE - 1);
@@ -845,21 +855,20 @@ static inline void mlx5e_mpwqe_fill_rx_skb(struct mlx5e_rq *rq,
 		page_idx++;
 		frag_offset -= PAGE_SIZE;
 	}
-	wi->dma_pre_sync(rq->pdev, wi, wqe_offset, consumed_bytes);
 
 	while (byte_cnt) {
 		u32 pg_consumed_bytes =
 			min_t(u32, PAGE_SIZE - frag_offset, byte_cnt);
 
-		wi->add_skb_frag(rq, skb, wi, page_idx, frag_offset,
-				 pg_consumed_bytes);
+		mlx5e_add_skb_frag_mpwqe(rq, skb, wi, page_idx, frag_offset,
+					 pg_consumed_bytes);
 		byte_cnt -= pg_consumed_bytes;
 		frag_offset = 0;
 		page_idx++;
 	}
 	/* copy header */
-	wi->copy_skb_header(rq->pdev, skb, wi, head_page_idx, head_offset,
-			    headlen);
+	mlx5e_copy_skb_header_mpwqe(rq->pdev, skb, wi, head_page_idx,
+				    head_offset, headlen);
 	/* skb linear part was allocated with headlen and aligned to long */
 	skb->tail += headlen;
 	skb->len  += headlen;
@@ -869,7 +878,7 @@ void mlx5e_handle_rx_cqe_mpwrq(struct mlx5e_rq *rq, struct mlx5_cqe64 *cqe)
 {
 	u16 cstrides       = mpwrq_get_cqe_consumed_strides(cqe);
 	u16 wqe_id         = be16_to_cpu(cqe->wqe_id);
-	struct mlx5e_mpw_info *wi = &rq->wqe_info[wqe_id];
+	struct mlx5e_mpw_info *wi = &rq->mpwqe.info[wqe_id];
 	struct mlx5e_rx_wqe  *wqe = mlx5_wq_ll_get_wqe(&rq->wq, wqe_id);
 	struct sk_buff *skb;
 	u16 cqe_bcnt;
@@ -899,18 +908,20 @@ void mlx5e_handle_rx_cqe_mpwrq(struct mlx5e_rq *rq, struct mlx5_cqe64 *cqe)
 
 	mlx5e_mpwqe_fill_rx_skb(rq, cqe, wi, cqe_bcnt, skb);
 	mlx5e_complete_rx_cqe(rq, cqe, cqe_bcnt, skb);
+	napi_gro_receive(rq->cq.napi, skb);
 
 mpwrq_cqe_out:
 	if (likely(wi->consumed_strides < rq->mpwqe_num_strides))
 		return;
 
-	wi->free_wqe(rq, wi);
+	mlx5e_free_rx_mpwqe(rq, wi);
 	mlx5_wq_ll_pop(&rq->wq, cqe->wqe_id, &wqe->next.next_wqe_index);
 }
 
 int mlx5e_poll_rx_cq(struct mlx5e_cq *cq, int budget)
 {
 	struct mlx5e_rq *rq = container_of(cq, struct mlx5e_rq, cq);
+	struct mlx5e_sq *xdp_sq = &rq->channel->xdp_sq;
 	int work_done = 0;
 
 	if (unlikely(test_bit(MLX5E_RQ_STATE_FLUSH, &rq->state)))
@@ -937,6 +948,11 @@ int mlx5e_poll_rx_cq(struct mlx5e_cq *cq, int budget)
 		rq->handle_rx_cqe(rq, cqe);
 	}
 
+	if (xdp_sq->db.xdp.doorbell) {
+		mlx5e_xmit_xdp_doorbell(xdp_sq);
+		xdp_sq->db.xdp.doorbell = false;
+	}
+
 	mlx5_cqwq_update_db_record(&cq->wq);
 
 	/* ensure cq space is freed before enabling more cqes */
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_stats.h b/drivers/net/ethernet/mellanox/mlx5/core/en_stats.h
index 499487ce3b53..57452fdc5154 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_stats.h
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_stats.h
@@ -65,6 +65,9 @@ struct mlx5e_sw_stats {
 	u64 rx_csum_none;
 	u64 rx_csum_complete;
 	u64 rx_csum_unnecessary_inner;
+	u64 rx_xdp_drop;
+	u64 rx_xdp_tx;
+	u64 rx_xdp_tx_full;
 	u64 tx_csum_partial;
 	u64 tx_csum_partial_inner;
 	u64 tx_queue_stopped;
@@ -73,10 +76,13 @@ struct mlx5e_sw_stats {
 	u64 tx_xmit_more;
 	u64 rx_wqe_err;
 	u64 rx_mpwqe_filler;
-	u64 rx_mpwqe_frag;
 	u64 rx_buff_alloc_err;
 	u64 rx_cqe_compress_blks;
 	u64 rx_cqe_compress_pkts;
+	u64 rx_cache_reuse;
+	u64 rx_cache_full;
+	u64 rx_cache_empty;
+	u64 rx_cache_busy;
 
 	/* Special handling counters */
 	u64 link_down_events_phy;
@@ -97,6 +103,9 @@ static const struct counter_desc sw_stats_desc[] = {
 	{ MLX5E_DECLARE_STAT(struct mlx5e_sw_stats, rx_csum_none) },
 	{ MLX5E_DECLARE_STAT(struct mlx5e_sw_stats, rx_csum_complete) },
 	{ MLX5E_DECLARE_STAT(struct mlx5e_sw_stats, rx_csum_unnecessary_inner) },
+	{ MLX5E_DECLARE_STAT(struct mlx5e_sw_stats, rx_xdp_drop) },
+	{ MLX5E_DECLARE_STAT(struct mlx5e_sw_stats, rx_xdp_tx) },
+	{ MLX5E_DECLARE_STAT(struct mlx5e_sw_stats, rx_xdp_tx_full) },
 	{ MLX5E_DECLARE_STAT(struct mlx5e_sw_stats, tx_csum_partial) },
 	{ MLX5E_DECLARE_STAT(struct mlx5e_sw_stats, tx_csum_partial_inner) },
 	{ MLX5E_DECLARE_STAT(struct mlx5e_sw_stats, tx_queue_stopped) },
@@ -105,10 +114,13 @@ static const struct counter_desc sw_stats_desc[] = {
 	{ MLX5E_DECLARE_STAT(struct mlx5e_sw_stats, tx_xmit_more) },
 	{ MLX5E_DECLARE_STAT(struct mlx5e_sw_stats, rx_wqe_err) },
 	{ MLX5E_DECLARE_STAT(struct mlx5e_sw_stats, rx_mpwqe_filler) },
-	{ MLX5E_DECLARE_STAT(struct mlx5e_sw_stats, rx_mpwqe_frag) },
 	{ MLX5E_DECLARE_STAT(struct mlx5e_sw_stats, rx_buff_alloc_err) },
 	{ MLX5E_DECLARE_STAT(struct mlx5e_sw_stats, rx_cqe_compress_blks) },
 	{ MLX5E_DECLARE_STAT(struct mlx5e_sw_stats, rx_cqe_compress_pkts) },
+	{ MLX5E_DECLARE_STAT(struct mlx5e_sw_stats, rx_cache_reuse) },
+	{ MLX5E_DECLARE_STAT(struct mlx5e_sw_stats, rx_cache_full) },
+	{ MLX5E_DECLARE_STAT(struct mlx5e_sw_stats, rx_cache_empty) },
+	{ MLX5E_DECLARE_STAT(struct mlx5e_sw_stats, rx_cache_busy) },
 	{ MLX5E_DECLARE_STAT(struct mlx5e_sw_stats, link_down_events_phy) },
 };
 
@@ -272,12 +284,18 @@ struct mlx5e_rq_stats {
 	u64 csum_none;
 	u64 lro_packets;
 	u64 lro_bytes;
+	u64 xdp_drop;
+	u64 xdp_tx;
+	u64 xdp_tx_full;
 	u64 wqe_err;
 	u64 mpwqe_filler;
-	u64 mpwqe_frag;
 	u64 buff_alloc_err;
 	u64 cqe_compress_blks;
 	u64 cqe_compress_pkts;
+	u64 cache_reuse;
+	u64 cache_full;
+	u64 cache_empty;
+	u64 cache_busy;
 };
 
 static const struct counter_desc rq_stats_desc[] = {
@@ -286,14 +304,20 @@ static const struct counter_desc rq_stats_desc[] = {
 	{ MLX5E_DECLARE_RX_STAT(struct mlx5e_rq_stats, csum_complete) },
 	{ MLX5E_DECLARE_RX_STAT(struct mlx5e_rq_stats, csum_unnecessary_inner) },
 	{ MLX5E_DECLARE_RX_STAT(struct mlx5e_rq_stats, csum_none) },
+	{ MLX5E_DECLARE_RX_STAT(struct mlx5e_rq_stats, xdp_drop) },
+	{ MLX5E_DECLARE_RX_STAT(struct mlx5e_rq_stats, xdp_tx) },
+	{ MLX5E_DECLARE_RX_STAT(struct mlx5e_rq_stats, xdp_tx_full) },
 	{ MLX5E_DECLARE_RX_STAT(struct mlx5e_rq_stats, lro_packets) },
 	{ MLX5E_DECLARE_RX_STAT(struct mlx5e_rq_stats, lro_bytes) },
 	{ MLX5E_DECLARE_RX_STAT(struct mlx5e_rq_stats, wqe_err) },
 	{ MLX5E_DECLARE_RX_STAT(struct mlx5e_rq_stats, mpwqe_filler) },
-	{ MLX5E_DECLARE_RX_STAT(struct mlx5e_rq_stats, mpwqe_frag) },
 	{ MLX5E_DECLARE_RX_STAT(struct mlx5e_rq_stats, buff_alloc_err) },
 	{ MLX5E_DECLARE_RX_STAT(struct mlx5e_rq_stats, cqe_compress_blks) },
 	{ MLX5E_DECLARE_RX_STAT(struct mlx5e_rq_stats, cqe_compress_pkts) },
+	{ MLX5E_DECLARE_RX_STAT(struct mlx5e_rq_stats, cache_reuse) },
+	{ MLX5E_DECLARE_RX_STAT(struct mlx5e_rq_stats, cache_full) },
+	{ MLX5E_DECLARE_RX_STAT(struct mlx5e_rq_stats, cache_empty) },
+	{ MLX5E_DECLARE_RX_STAT(struct mlx5e_rq_stats, cache_busy) },
 };
 
 struct mlx5e_sq_stats {
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_tc.c b/drivers/net/ethernet/mellanox/mlx5/core/en_tc.c
index 22cfc4ac1837..ce8c54d18906 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_tc.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_tc.c
@@ -39,6 +39,7 @@
 #include <linux/rhashtable.h>
 #include <net/switchdev.h>
 #include <net/tc_act/tc_mirred.h>
+#include <net/tc_act/tc_vlan.h>
 #include "en.h"
 #include "en_tc.h"
 #include "eswitch.h"
@@ -47,6 +48,7 @@ struct mlx5e_tc_flow {
 	struct rhash_head	node;
 	u64			cookie;
 	struct mlx5_flow_rule	*rule;
+	struct mlx5_esw_flow_attr *attr;
 };
 
 #define MLX5E_TC_TABLE_NUM_ENTRIES 1024
@@ -114,27 +116,30 @@ err_create_ft:
 
 static struct mlx5_flow_rule *mlx5e_tc_add_fdb_flow(struct mlx5e_priv *priv,
 						    struct mlx5_flow_spec *spec,
-						    u32 action, u32 dst_vport)
+						    struct mlx5_esw_flow_attr *attr)
 {
 	struct mlx5_eswitch *esw = priv->mdev->priv.eswitch;
-	struct mlx5_eswitch_rep *rep = priv->ppriv;
-	u32 src_vport;
+	int err;
 
-	if (rep->vport) /* set source vport for the flow */
-		src_vport = rep->vport;
-	else
-		src_vport = FDB_UPLINK_VPORT;
+	err = mlx5_eswitch_add_vlan_action(esw, attr);
+	if (err)
+		return ERR_PTR(err);
 
-	return mlx5_eswitch_add_offloaded_rule(esw, spec, action, src_vport, dst_vport);
+	return mlx5_eswitch_add_offloaded_rule(esw, spec, attr);
 }
 
 static void mlx5e_tc_del_flow(struct mlx5e_priv *priv,
-			      struct mlx5_flow_rule *rule)
+			      struct mlx5_flow_rule *rule,
+			      struct mlx5_esw_flow_attr *attr)
 {
+	struct mlx5_eswitch *esw = priv->mdev->priv.eswitch;
 	struct mlx5_fc *counter = NULL;
 
 	counter = mlx5_flow_rule_counter(rule);
 
+	if (esw && esw->mode == SRIOV_OFFLOADS)
+		mlx5_eswitch_del_vlan_action(esw, attr);
+
 	mlx5_del_flow_rule(rule);
 
 	mlx5_fc_destroy(priv->mdev, counter);
@@ -159,6 +164,7 @@ static int parse_cls_flower(struct mlx5e_priv *priv, struct mlx5_flow_spec *spec
 	    ~(BIT(FLOW_DISSECTOR_KEY_CONTROL) |
 	      BIT(FLOW_DISSECTOR_KEY_BASIC) |
 	      BIT(FLOW_DISSECTOR_KEY_ETH_ADDRS) |
+	      BIT(FLOW_DISSECTOR_KEY_VLAN) |
 	      BIT(FLOW_DISSECTOR_KEY_IPV4_ADDRS) |
 	      BIT(FLOW_DISSECTOR_KEY_IPV6_ADDRS) |
 	      BIT(FLOW_DISSECTOR_KEY_PORTS))) {
@@ -222,6 +228,24 @@ static int parse_cls_flower(struct mlx5e_priv *priv, struct mlx5_flow_spec *spec
 				key->src);
 	}
 
+	if (dissector_uses_key(f->dissector, FLOW_DISSECTOR_KEY_VLAN)) {
+		struct flow_dissector_key_vlan *key =
+			skb_flow_dissector_target(f->dissector,
+						  FLOW_DISSECTOR_KEY_VLAN,
+						  f->key);
+		struct flow_dissector_key_vlan *mask =
+			skb_flow_dissector_target(f->dissector,
+						  FLOW_DISSECTOR_KEY_VLAN,
+						  f->mask);
+		if (mask->vlan_id) {
+			MLX5_SET(fte_match_set_lyr_2_4, headers_c, vlan_tag, 1);
+			MLX5_SET(fte_match_set_lyr_2_4, headers_v, vlan_tag, 1);
+
+			MLX5_SET(fte_match_set_lyr_2_4, headers_c, first_vid, mask->vlan_id);
+			MLX5_SET(fte_match_set_lyr_2_4, headers_v, first_vid, key->vlan_id);
+		}
+	}
+
 	if (addr_type == FLOW_DISSECTOR_KEY_IPV4_ADDRS) {
 		struct flow_dissector_key_ipv4_addrs *key =
 			skb_flow_dissector_target(f->dissector,
@@ -361,7 +385,7 @@ static int parse_tc_nic_actions(struct mlx5e_priv *priv, struct tcf_exts *exts,
 }
 
 static int parse_tc_fdb_actions(struct mlx5e_priv *priv, struct tcf_exts *exts,
-				u32 *action, u32 *dest_vport)
+				struct mlx5_esw_flow_attr *attr)
 {
 	const struct tc_action *a;
 	LIST_HEAD(actions);
@@ -369,17 +393,14 @@ static int parse_tc_fdb_actions(struct mlx5e_priv *priv, struct tcf_exts *exts,
 	if (tc_no_actions(exts))
 		return -EINVAL;
 
-	*action = 0;
+	memset(attr, 0, sizeof(*attr));
+	attr->in_rep = priv->ppriv;
 
 	tcf_exts_to_list(exts, &actions);
 	list_for_each_entry(a, &actions, list) {
-		/* Only support a single action per rule */
-		if (*action)
-			return -EINVAL;
-
 		if (is_tcf_gact_shot(a)) {
-			*action = MLX5_FLOW_CONTEXT_ACTION_DROP |
-				  MLX5_FLOW_CONTEXT_ACTION_COUNT;
+			attr->action |= MLX5_FLOW_CONTEXT_ACTION_DROP |
+					MLX5_FLOW_CONTEXT_ACTION_COUNT;
 			continue;
 		}
 
@@ -387,7 +408,6 @@ static int parse_tc_fdb_actions(struct mlx5e_priv *priv, struct tcf_exts *exts,
 			int ifindex = tcf_mirred_ifindex(a);
 			struct net_device *out_dev;
 			struct mlx5e_priv *out_priv;
-			struct mlx5_eswitch_rep *out_rep;
 
 			out_dev = __dev_get_by_index(dev_net(priv->netdev), ifindex);
 
@@ -397,13 +417,22 @@ static int parse_tc_fdb_actions(struct mlx5e_priv *priv, struct tcf_exts *exts,
 				return -EINVAL;
 			}
 
+			attr->action |= MLX5_FLOW_CONTEXT_ACTION_FWD_DEST;
 			out_priv = netdev_priv(out_dev);
-			out_rep  = out_priv->ppriv;
-			if (out_rep->vport == 0)
-				*dest_vport = FDB_UPLINK_VPORT;
-			else
-				*dest_vport = out_rep->vport;
-			*action = MLX5_FLOW_CONTEXT_ACTION_FWD_DEST;
+			attr->out_rep = out_priv->ppriv;
+			continue;
+		}
+
+		if (is_tcf_vlan(a)) {
+			if (tcf_vlan_action(a) == VLAN_F_POP) {
+				attr->action |= MLX5_FLOW_CONTEXT_ACTION_VLAN_POP;
+			} else if (tcf_vlan_action(a) == VLAN_F_PUSH) {
+				if (tcf_vlan_push_proto(a) != htons(ETH_P_8021Q))
+					return -EOPNOTSUPP;
+
+				attr->action |= MLX5_FLOW_CONTEXT_ACTION_VLAN_PUSH;
+				attr->vlan = tcf_vlan_push_vid(a);
+			}
 			continue;
 		}
 
@@ -417,18 +446,29 @@ int mlx5e_configure_flower(struct mlx5e_priv *priv, __be16 protocol,
 {
 	struct mlx5e_tc_table *tc = &priv->fs.tc;
 	int err = 0;
-	u32 flow_tag, action, dest_vport = 0;
+	bool fdb_flow = false;
+	u32 flow_tag, action;
 	struct mlx5e_tc_flow *flow;
 	struct mlx5_flow_spec *spec;
 	struct mlx5_flow_rule *old = NULL;
+	struct mlx5_esw_flow_attr *old_attr = NULL;
 	struct mlx5_eswitch *esw = priv->mdev->priv.eswitch;
 
+	if (esw && esw->mode == SRIOV_OFFLOADS)
+		fdb_flow = true;
+
 	flow = rhashtable_lookup_fast(&tc->ht, &f->cookie,
 				      tc->ht_params);
-	if (flow)
+	if (flow) {
 		old = flow->rule;
-	else
-		flow = kzalloc(sizeof(*flow), GFP_KERNEL);
+		old_attr = flow->attr;
+	} else {
+		if (fdb_flow)
+			flow = kzalloc(sizeof(*flow) + sizeof(struct mlx5_esw_flow_attr),
+				       GFP_KERNEL);
+		else
+			flow = kzalloc(sizeof(*flow), GFP_KERNEL);
+	}
 
 	spec = mlx5_vzalloc(sizeof(*spec));
 	if (!spec || !flow) {
@@ -442,11 +482,12 @@ int mlx5e_configure_flower(struct mlx5e_priv *priv, __be16 protocol,
 	if (err < 0)
 		goto err_free;
 
-	if (esw && esw->mode == SRIOV_OFFLOADS) {
-		err = parse_tc_fdb_actions(priv, f->exts, &action, &dest_vport);
+	if (fdb_flow) {
+		flow->attr  = (struct mlx5_esw_flow_attr *)(flow + 1);
+		err = parse_tc_fdb_actions(priv, f->exts, flow->attr);
 		if (err < 0)
 			goto err_free;
-		flow->rule = mlx5e_tc_add_fdb_flow(priv, spec, action, dest_vport);
+		flow->rule = mlx5e_tc_add_fdb_flow(priv, spec, flow->attr);
 	} else {
 		err = parse_tc_nic_actions(priv, f->exts, &action, &flow_tag);
 		if (err < 0)
@@ -465,7 +506,7 @@ int mlx5e_configure_flower(struct mlx5e_priv *priv, __be16 protocol,
 		goto err_del_rule;
 
 	if (old)
-		mlx5e_tc_del_flow(priv, old);
+		mlx5e_tc_del_flow(priv, old, old_attr);
 
 	goto out;
 
@@ -493,7 +534,7 @@ int mlx5e_delete_flower(struct mlx5e_priv *priv,
 
 	rhashtable_remove_fast(&tc->ht, &flow->node, tc->ht_params);
 
-	mlx5e_tc_del_flow(priv, flow->rule);
+	mlx5e_tc_del_flow(priv, flow->rule, flow->attr);
 
 	kfree(flow);
 
@@ -550,7 +591,7 @@ static void _mlx5e_tc_del_flow(void *ptr, void *arg)
 	struct mlx5e_tc_flow *flow = ptr;
 	struct mlx5e_priv *priv = arg;
 
-	mlx5e_tc_del_flow(priv, flow->rule);
+	mlx5e_tc_del_flow(priv, flow->rule, flow->attr);
 	kfree(flow);
 }
 
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_tx.c b/drivers/net/ethernet/mellanox/mlx5/core/en_tx.c
index eb0e72537f10..70a717382357 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_tx.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_tx.c
@@ -52,7 +52,6 @@ void mlx5e_send_nop(struct mlx5e_sq *sq, bool notify_hw)
 	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++;
 	sq->stats.nop++;
 
@@ -82,15 +81,17 @@ static inline void mlx5e_dma_push(struct mlx5e_sq *sq,
 				  u32 size,
 				  enum mlx5e_dma_map_type map_type)
 {
-	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[sq->dma_fifo_pc & sq->dma_fifo_mask].type = map_type;
+	u32 i = sq->dma_fifo_pc & sq->dma_fifo_mask;
+
+	sq->db.txq.dma_fifo[i].addr = addr;
+	sq->db.txq.dma_fifo[i].size = size;
+	sq->db.txq.dma_fifo[i].type = map_type;
 	sq->dma_fifo_pc++;
 }
 
 static inline struct mlx5e_sq_dma *mlx5e_dma_get(struct mlx5e_sq *sq, u32 i)
 {
-	return &sq->dma_fifo[i & sq->dma_fifo_mask];
+	return &sq->db.txq.dma_fifo[i & sq->dma_fifo_mask];
 }
 
 static void mlx5e_dma_unmap_wqe_err(struct mlx5e_sq *sq, u8 num_dma)
@@ -221,7 +222,7 @@ static netdev_tx_t mlx5e_sq_xmit(struct mlx5e_sq *sq, struct sk_buff *skb)
 
 	u16 pi = sq->pc & wq->sz_m1;
 	struct mlx5e_tx_wqe      *wqe  = mlx5_wq_cyc_get_wqe(wq, pi);
-	struct mlx5e_tx_wqe_info *wi   = &sq->wqe_info[pi];
+	struct mlx5e_tx_wqe_info *wi   = &sq->db.txq.wqe_info[pi];
 
 	struct mlx5_wqe_ctrl_seg *cseg = &wqe->ctrl;
 	struct mlx5_wqe_eth_seg  *eseg = &wqe->eth;
@@ -341,7 +342,7 @@ static netdev_tx_t mlx5e_sq_xmit(struct mlx5e_sq *sq, struct sk_buff *skb)
 	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;
+	sq->db.txq.skb[pi] = skb;
 
 	wi->num_wqebbs = DIV_ROUND_UP(ds_cnt, MLX5_SEND_WQEBB_NUM_DS);
 	sq->pc += wi->num_wqebbs;
@@ -368,8 +369,10 @@ static netdev_tx_t mlx5e_sq_xmit(struct mlx5e_sq *sq, struct sk_buff *skb)
 	}
 
 	/* fill sq edge with nops to avoid wqe wrap around */
-	while ((sq->pc & wq->sz_m1) > sq->edge)
+	while ((pi = (sq->pc & wq->sz_m1)) > sq->edge) {
+		sq->db.txq.skb[pi] = NULL;
 		mlx5e_send_nop(sq, false);
+	}
 
 	if (bf)
 		sq->bf_budget--;
@@ -442,8 +445,8 @@ bool mlx5e_poll_tx_cq(struct mlx5e_cq *cq, int napi_budget)
 			last_wqe = (sqcc == wqe_counter);
 
 			ci = sqcc & sq->wq.sz_m1;
-			skb = sq->skb[ci];
-			wi = &sq->wqe_info[ci];
+			skb = sq->db.txq.skb[ci];
+			wi = &sq->db.txq.wqe_info[ci];
 
 			if (unlikely(!skb)) { /* nop */
 				sqcc++;
@@ -492,7 +495,7 @@ bool mlx5e_poll_tx_cq(struct mlx5e_cq *cq, int napi_budget)
 	return (i == MLX5E_TX_CQ_POLL_BUDGET);
 }
 
-void mlx5e_free_tx_descs(struct mlx5e_sq *sq)
+static void mlx5e_free_txq_sq_descs(struct mlx5e_sq *sq)
 {
 	struct mlx5e_tx_wqe_info *wi;
 	struct sk_buff *skb;
@@ -501,8 +504,8 @@ void mlx5e_free_tx_descs(struct mlx5e_sq *sq)
 
 	while (sq->cc != sq->pc) {
 		ci = sq->cc & sq->wq.sz_m1;
-		skb = sq->skb[ci];
-		wi = &sq->wqe_info[ci];
+		skb = sq->db.txq.skb[ci];
+		wi = &sq->db.txq.wqe_info[ci];
 
 		if (!skb) { /* nop */
 			sq->cc++;
@@ -520,3 +523,37 @@ void mlx5e_free_tx_descs(struct mlx5e_sq *sq)
 		sq->cc += wi->num_wqebbs;
 	}
 }
+
+static void mlx5e_free_xdp_sq_descs(struct mlx5e_sq *sq)
+{
+	struct mlx5e_sq_wqe_info *wi;
+	struct mlx5e_dma_info *di;
+	u16 ci;
+
+	while (sq->cc != sq->pc) {
+		ci = sq->cc & sq->wq.sz_m1;
+		di = &sq->db.xdp.di[ci];
+		wi = &sq->db.xdp.wqe_info[ci];
+
+		if (wi->opcode == MLX5_OPCODE_NOP) {
+			sq->cc++;
+			continue;
+		}
+
+		sq->cc += wi->num_wqebbs;
+
+		mlx5e_page_release(&sq->channel->rq, di, false);
+	}
+}
+
+void mlx5e_free_sq_descs(struct mlx5e_sq *sq)
+{
+	switch (sq->type) {
+	case MLX5E_SQ_TXQ:
+		mlx5e_free_txq_sq_descs(sq);
+		break;
+	case MLX5E_SQ_XDP:
+		mlx5e_free_xdp_sq_descs(sq);
+		break;
+	}
+}
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_txrx.c b/drivers/net/ethernet/mellanox/mlx5/core/en_txrx.c
index 9bf33bb69210..5703f19a6a24 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_txrx.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_txrx.c
@@ -72,7 +72,7 @@ static void mlx5e_poll_ico_cq(struct mlx5e_cq *cq)
 
 	do {
 		u16 ci = be16_to_cpu(cqe->wqe_counter) & wq->sz_m1;
-		struct mlx5e_ico_wqe_info *icowi = &sq->ico_wqe_info[ci];
+		struct mlx5e_sq_wqe_info *icowi = &sq->db.ico_wqe[ci];
 
 		mlx5_cqwq_pop(&cq->wq);
 		sqcc += icowi->num_wqebbs;
@@ -87,7 +87,7 @@ static void mlx5e_poll_ico_cq(struct mlx5e_cq *cq)
 		case MLX5_OPCODE_NOP:
 			break;
 		case MLX5_OPCODE_UMR:
-			mlx5e_post_rx_fragmented_mpwqe(&sq->channel->rq);
+			mlx5e_post_rx_mpwqe(&sq->channel->rq);
 			break;
 		default:
 			WARN_ONCE(true,
@@ -105,6 +105,66 @@ static void mlx5e_poll_ico_cq(struct mlx5e_cq *cq)
 	sq->cc = sqcc;
 }
 
+static inline bool mlx5e_poll_xdp_tx_cq(struct mlx5e_cq *cq)
+{
+	struct mlx5e_sq *sq;
+	u16 sqcc;
+	int i;
+
+	sq = container_of(cq, struct mlx5e_sq, cq);
+
+	if (unlikely(test_bit(MLX5E_SQ_STATE_FLUSH, &sq->state)))
+		return false;
+
+	/* sq->cc must be updated only after mlx5_cqwq_update_db_record(),
+	 * otherwise a cq overrun may occur
+	 */
+	sqcc = sq->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 mlx5e_sq_wqe_info *wi;
+			struct mlx5e_dma_info *di;
+			u16 ci;
+
+			last_wqe = (sqcc == wqe_counter);
+
+			ci = sqcc & sq->wq.sz_m1;
+			di = &sq->db.xdp.di[ci];
+			wi = &sq->db.xdp.wqe_info[ci];
+
+			if (unlikely(wi->opcode == MLX5_OPCODE_NOP)) {
+				sqcc++;
+				continue;
+			}
+
+			sqcc += wi->num_wqebbs;
+			/* Recycle RX page */
+			mlx5e_page_release(&sq->channel->rq, di, true);
+		} while (!last_wqe);
+	}
+
+	mlx5_cqwq_update_db_record(&cq->wq);
+
+	/* ensure cq space is freed before enabling more cqes */
+	wmb();
+
+	sq->cc = sqcc;
+	return (i == MLX5E_TX_CQ_POLL_BUDGET);
+}
+
 int mlx5e_napi_poll(struct napi_struct *napi, int budget)
 {
 	struct mlx5e_channel *c = container_of(napi, struct mlx5e_channel,
@@ -121,6 +181,9 @@ int mlx5e_napi_poll(struct napi_struct *napi, int budget)
 	work_done = mlx5e_poll_rx_cq(&c->rq.cq, budget);
 	busy |= work_done == budget;
 
+	if (c->xdp)
+		busy |= mlx5e_poll_xdp_tx_cq(&c->xdp_sq.cq);
+
 	mlx5e_poll_ico_cq(&c->icosq.cq);
 
 	busy |= mlx5e_post_rx_wqes(&c->rq);
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/eq.c b/drivers/net/ethernet/mellanox/mlx5/core/eq.c
index 0e30602ef76d..aaca09002ca6 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/eq.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/eq.c
@@ -86,23 +86,12 @@ struct cre_des_eq {
 
 static int mlx5_cmd_destroy_eq(struct mlx5_core_dev *dev, u8 eqn)
 {
-	struct mlx5_destroy_eq_mbox_in in;
-	struct mlx5_destroy_eq_mbox_out out;
-	int err;
-
-	memset(&in, 0, sizeof(in));
-	memset(&out, 0, sizeof(out));
-	in.hdr.opcode = cpu_to_be16(MLX5_CMD_OP_DESTROY_EQ);
-	in.eqn = eqn;
-	err = mlx5_cmd_exec(dev, &in, sizeof(in), &out, sizeof(out));
-	if (!err)
-		goto ex;
-
-	if (out.hdr.status)
-		err = mlx5_cmd_status_to_err(&out.hdr);
+	u32 out[MLX5_ST_SZ_DW(destroy_eq_out)] = {0};
+	u32 in[MLX5_ST_SZ_DW(destroy_eq_in)]   = {0};
 
-ex:
-	return err;
+	MLX5_SET(destroy_eq_in, in, opcode, MLX5_CMD_OP_DESTROY_EQ);
+	MLX5_SET(destroy_eq_in, in, eq_number, eqn);
+	return mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
 }
 
 static struct mlx5_eqe *get_eqe(struct mlx5_eq *eq, u32 entry)
@@ -351,11 +340,13 @@ 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)
 {
+	u32 out[MLX5_ST_SZ_DW(create_eq_out)] = {0};
 	struct mlx5_priv *priv = &dev->priv;
-	struct mlx5_create_eq_mbox_in *in;
-	struct mlx5_create_eq_mbox_out out;
-	int err;
+	__be64 *pas;
+	void *eqc;
 	int inlen;
+	u32 *in;
+	int err;
 
 	eq->nent = roundup_pow_of_two(nent + MLX5_NUM_SPARE_EQE);
 	eq->cons_index = 0;
@@ -365,35 +356,36 @@ int mlx5_create_map_eq(struct mlx5_core_dev *dev, struct mlx5_eq *eq, u8 vecidx,
 
 	init_eq_buf(eq);
 
-	inlen = sizeof(*in) + sizeof(in->pas[0]) * eq->buf.npages;
+	inlen = MLX5_ST_SZ_BYTES(create_eq_in) +
+		MLX5_FLD_SZ_BYTES(create_eq_in, pas[0]) * eq->buf.npages;
+
 	in = mlx5_vzalloc(inlen);
 	if (!in) {
 		err = -ENOMEM;
 		goto err_buf;
 	}
-	memset(&out, 0, sizeof(out));
 
-	mlx5_fill_page_array(&eq->buf, in->pas);
+	pas = (__be64 *)MLX5_ADDR_OF(create_eq_in, in, pas);
+	mlx5_fill_page_array(&eq->buf, pas);
 
-	in->hdr.opcode = cpu_to_be16(MLX5_CMD_OP_CREATE_EQ);
-	in->ctx.log_sz_usr_page = cpu_to_be32(ilog2(eq->nent) << 24 | uar->index);
-	in->ctx.intr = vecidx;
-	in->ctx.log_page_size = eq->buf.page_shift - MLX5_ADAPTER_PAGE_SHIFT;
-	in->events_mask = cpu_to_be64(mask);
+	MLX5_SET(create_eq_in, in, opcode, MLX5_CMD_OP_CREATE_EQ);
+	MLX5_SET64(create_eq_in, in, event_bitmask, mask);
 
-	err = mlx5_cmd_exec(dev, in, inlen, &out, sizeof(out));
-	if (err)
-		goto err_in;
+	eqc = MLX5_ADDR_OF(create_eq_in, in, eq_context_entry);
+	MLX5_SET(eqc, eqc, log_eq_size, ilog2(eq->nent));
+	MLX5_SET(eqc, eqc, uar_page, uar->index);
+	MLX5_SET(eqc, eqc, intr, vecidx);
+	MLX5_SET(eqc, eqc, log_page_size,
+		 eq->buf.page_shift - MLX5_ADAPTER_PAGE_SHIFT);
 
-	if (out.hdr.status) {
-		err = mlx5_cmd_status_to_err(&out.hdr);
+	err = mlx5_cmd_exec(dev, in, inlen, out, sizeof(out));
+	if (err)
 		goto err_in;
-	}
 
 	snprintf(priv->irq_info[vecidx].name, MLX5_MAX_IRQ_NAME, "%s@pci:%s",
 		 name, pci_name(dev->pdev));
 
-	eq->eqn = out.eq_number;
+	eq->eqn = MLX5_GET(create_eq_out, out, eq_number);
 	eq->irqn = priv->msix_arr[vecidx].vector;
 	eq->dev = dev;
 	eq->doorbell = uar->map + MLX5_EQ_DOORBEL_OFFSET;
@@ -547,22 +539,12 @@ int mlx5_stop_eqs(struct mlx5_core_dev *dev)
 }
 
 int mlx5_core_eq_query(struct mlx5_core_dev *dev, struct mlx5_eq *eq,
-		       struct mlx5_query_eq_mbox_out *out, int outlen)
+		       u32 *out, int outlen)
 {
-	struct mlx5_query_eq_mbox_in in;
-	int err;
-
-	memset(&in, 0, sizeof(in));
-	memset(out, 0, outlen);
-	in.hdr.opcode = cpu_to_be16(MLX5_CMD_OP_QUERY_EQ);
-	in.eqn = eq->eqn;
-	err = mlx5_cmd_exec(dev, &in, sizeof(in), out, outlen);
-	if (err)
-		return err;
+	u32 in[MLX5_ST_SZ_DW(query_eq_in)] = {0};
 
-	if (out->hdr.status)
-		err = mlx5_cmd_status_to_err(&out->hdr);
-
-	return err;
+	MLX5_SET(query_eq_in, in, opcode, MLX5_CMD_OP_QUERY_EQ);
+	MLX5_SET(query_eq_in, in, eq_number, eq->eqn);
+	return mlx5_cmd_exec(dev, in, sizeof(in), out, outlen);
 }
 EXPORT_SYMBOL_GPL(mlx5_core_eq_query);
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/eswitch.c b/drivers/net/ethernet/mellanox/mlx5/core/eswitch.c
index b247949df135..abbf2c369923 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/eswitch.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/eswitch.c
@@ -81,19 +81,12 @@ enum {
 			    MC_ADDR_CHANGE | \
 			    PROMISC_CHANGE)
 
-int esw_offloads_init(struct mlx5_eswitch *esw, int nvports);
-void esw_offloads_cleanup(struct mlx5_eswitch *esw, int nvports);
-
 static int arm_vport_context_events_cmd(struct mlx5_core_dev *dev, u16 vport,
 					u32 events_mask)
 {
-	int in[MLX5_ST_SZ_DW(modify_nic_vport_context_in)];
-	int out[MLX5_ST_SZ_DW(modify_nic_vport_context_out)];
+	int in[MLX5_ST_SZ_DW(modify_nic_vport_context_in)]   = {0};
+	int out[MLX5_ST_SZ_DW(modify_nic_vport_context_out)] = {0};
 	void *nic_vport_ctx;
-	int err;
-
-	memset(out, 0, sizeof(out));
-	memset(in, 0, sizeof(in));
 
 	MLX5_SET(modify_nic_vport_context_in, in,
 		 opcode, MLX5_CMD_OP_MODIFY_NIC_VPORT_CONTEXT);
@@ -116,113 +109,44 @@ static int arm_vport_context_events_cmd(struct mlx5_core_dev *dev, u16 vport,
 		MLX5_SET(nic_vport_context, nic_vport_ctx,
 			 event_on_promisc_change, 1);
 
-	err = mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
-	if (err)
-		goto ex;
-	err = mlx5_cmd_status_to_err_v2(out);
-	if (err)
-		goto ex;
-	return 0;
-ex:
-	return err;
+	return mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
 }
 
 /* E-Switch vport context HW commands */
-static int query_esw_vport_context_cmd(struct mlx5_core_dev *mdev, u32 vport,
-				       u32 *out, int outlen)
-{
-	u32 in[MLX5_ST_SZ_DW(query_esw_vport_context_in)];
-
-	memset(in, 0, sizeof(in));
-
-	MLX5_SET(query_nic_vport_context_in, in, opcode,
-		 MLX5_CMD_OP_QUERY_ESW_VPORT_CONTEXT);
-
-	MLX5_SET(query_esw_vport_context_in, in, vport_number, vport);
-	if (vport)
-		MLX5_SET(query_esw_vport_context_in, in, other_vport, 1);
-
-	return mlx5_cmd_exec_check_status(mdev, in, sizeof(in), out, outlen);
-}
-
-static int query_esw_vport_cvlan(struct mlx5_core_dev *dev, u32 vport,
-				 u16 *vlan, u8 *qos)
-{
-	u32 out[MLX5_ST_SZ_DW(query_esw_vport_context_out)];
-	int err;
-	bool cvlan_strip;
-	bool cvlan_insert;
-
-	memset(out, 0, sizeof(out));
-
-	*vlan = 0;
-	*qos = 0;
-
-	if (!MLX5_CAP_ESW(dev, vport_cvlan_strip) ||
-	    !MLX5_CAP_ESW(dev, vport_cvlan_insert_if_not_exist))
-		return -ENOTSUPP;
-
-	err = query_esw_vport_context_cmd(dev, vport, out, sizeof(out));
-	if (err)
-		goto out;
-
-	cvlan_strip = MLX5_GET(query_esw_vport_context_out, out,
-			       esw_vport_context.vport_cvlan_strip);
-
-	cvlan_insert = MLX5_GET(query_esw_vport_context_out, out,
-				esw_vport_context.vport_cvlan_insert);
-
-	if (cvlan_strip || cvlan_insert) {
-		*vlan = MLX5_GET(query_esw_vport_context_out, out,
-				 esw_vport_context.cvlan_id);
-		*qos = MLX5_GET(query_esw_vport_context_out, out,
-				esw_vport_context.cvlan_pcp);
-	}
-
-	esw_debug(dev, "Query Vport[%d] cvlan: VLAN %d qos=%d\n",
-		  vport, *vlan, *qos);
-out:
-	return err;
-}
-
 static int modify_esw_vport_context_cmd(struct mlx5_core_dev *dev, u16 vport,
 					void *in, int inlen)
 {
-	u32 out[MLX5_ST_SZ_DW(modify_esw_vport_context_out)];
-
-	memset(out, 0, sizeof(out));
+	u32 out[MLX5_ST_SZ_DW(modify_esw_vport_context_out)] = {0};
 
+	MLX5_SET(modify_esw_vport_context_in, in, opcode,
+		 MLX5_CMD_OP_MODIFY_ESW_VPORT_CONTEXT);
 	MLX5_SET(modify_esw_vport_context_in, in, vport_number, vport);
 	if (vport)
 		MLX5_SET(modify_esw_vport_context_in, in, other_vport, 1);
-
-	MLX5_SET(modify_esw_vport_context_in, in, opcode,
-		 MLX5_CMD_OP_MODIFY_ESW_VPORT_CONTEXT);
-
-	return mlx5_cmd_exec_check_status(dev, in, inlen,
-					  out, sizeof(out));
+	return mlx5_cmd_exec(dev, in, inlen, out, sizeof(out));
 }
 
 static int modify_esw_vport_cvlan(struct mlx5_core_dev *dev, u32 vport,
-				  u16 vlan, u8 qos, bool set)
+				  u16 vlan, u8 qos, u8 set_flags)
 {
-	u32 in[MLX5_ST_SZ_DW(modify_esw_vport_context_in)];
-
-	memset(in, 0, sizeof(in));
+	u32 in[MLX5_ST_SZ_DW(modify_esw_vport_context_in)] = {0};
 
 	if (!MLX5_CAP_ESW(dev, vport_cvlan_strip) ||
 	    !MLX5_CAP_ESW(dev, vport_cvlan_insert_if_not_exist))
 		return -ENOTSUPP;
 
-	esw_debug(dev, "Set Vport[%d] VLAN %d qos %d set=%d\n",
-		  vport, vlan, qos, set);
+	esw_debug(dev, "Set Vport[%d] VLAN %d qos %d set=%x\n",
+		  vport, vlan, qos, set_flags);
 
-	if (set) {
+	if (set_flags & SET_VLAN_STRIP)
 		MLX5_SET(modify_esw_vport_context_in, in,
 			 esw_vport_context.vport_cvlan_strip, 1);
+
+	if (set_flags & SET_VLAN_INSERT) {
 		/* insert only if no vlan in packet */
 		MLX5_SET(modify_esw_vport_context_in, in,
 			 esw_vport_context.vport_cvlan_insert, 1);
+
 		MLX5_SET(modify_esw_vport_context_in, in,
 			 esw_vport_context.cvlan_pcp, qos);
 		MLX5_SET(modify_esw_vport_context_in, in,
@@ -241,13 +165,10 @@ static int modify_esw_vport_cvlan(struct mlx5_core_dev *dev, u32 vport,
 static int set_l2_table_entry_cmd(struct mlx5_core_dev *dev, u32 index,
 				  u8 *mac, u8 vlan_valid, u16 vlan)
 {
-	u32 in[MLX5_ST_SZ_DW(set_l2_table_entry_in)];
-	u32 out[MLX5_ST_SZ_DW(set_l2_table_entry_out)];
+	u32 in[MLX5_ST_SZ_DW(set_l2_table_entry_in)]   = {0};
+	u32 out[MLX5_ST_SZ_DW(set_l2_table_entry_out)] = {0};
 	u8 *in_mac_addr;
 
-	memset(in, 0, sizeof(in));
-	memset(out, 0, sizeof(out));
-
 	MLX5_SET(set_l2_table_entry_in, in, opcode,
 		 MLX5_CMD_OP_SET_L2_TABLE_ENTRY);
 	MLX5_SET(set_l2_table_entry_in, in, table_index, index);
@@ -257,23 +178,18 @@ static int set_l2_table_entry_cmd(struct mlx5_core_dev *dev, u32 index,
 	in_mac_addr = MLX5_ADDR_OF(set_l2_table_entry_in, in, mac_address);
 	ether_addr_copy(&in_mac_addr[2], mac);
 
-	return mlx5_cmd_exec_check_status(dev, in, sizeof(in),
-					  out, sizeof(out));
+	return mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
 }
 
 static int del_l2_table_entry_cmd(struct mlx5_core_dev *dev, u32 index)
 {
-	u32 in[MLX5_ST_SZ_DW(delete_l2_table_entry_in)];
-	u32 out[MLX5_ST_SZ_DW(delete_l2_table_entry_out)];
-
-	memset(in, 0, sizeof(in));
-	memset(out, 0, sizeof(out));
+	u32 in[MLX5_ST_SZ_DW(delete_l2_table_entry_in)]   = {0};
+	u32 out[MLX5_ST_SZ_DW(delete_l2_table_entry_out)] = {0};
 
 	MLX5_SET(delete_l2_table_entry_in, in, opcode,
 		 MLX5_CMD_OP_DELETE_L2_TABLE_ENTRY);
 	MLX5_SET(delete_l2_table_entry_in, in, table_index, index);
-	return mlx5_cmd_exec_check_status(dev, in, sizeof(in),
-					  out, sizeof(out));
+	return mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
 }
 
 static int alloc_l2_table_index(struct mlx5_l2_table *l2_table, u32 *ix)
@@ -340,7 +256,7 @@ __esw_fdb_set_vport_rule(struct mlx5_eswitch *esw, u32 vport, bool rx_rule,
 
 	spec = mlx5_vzalloc(sizeof(*spec));
 	if (!spec) {
-		pr_warn("FDB: Failed to alloc match parameters\n");
+		esw_warn(esw->dev, "FDB: Failed to alloc match parameters\n");
 		return NULL;
 	}
 	dmac_v = MLX5_ADDR_OF(fte_match_param, spec->match_value,
@@ -374,8 +290,8 @@ __esw_fdb_set_vport_rule(struct mlx5_eswitch *esw, u32 vport, bool rx_rule,
 				   MLX5_FLOW_CONTEXT_ACTION_FWD_DEST,
 				   0, &dest);
 	if (IS_ERR(flow_rule)) {
-		pr_warn(
-			"FDB: Failed to add flow rule: dmac_v(%pM) dmac_c(%pM) -> vport(%d), err(%ld)\n",
+		esw_warn(esw->dev,
+			 "FDB: Failed to add flow rule: dmac_v(%pM) dmac_c(%pM) -> vport(%d), err(%ld)\n",
 			 dmac_v, dmac_c, vport, PTR_ERR(flow_rule));
 		flow_rule = NULL;
 	}
@@ -955,7 +871,7 @@ static void esw_update_vport_rx_mode(struct mlx5_eswitch *esw, u32 vport_num)
 	esw_debug(esw->dev, "vport[%d] context update rx mode promisc_all=%d, all_multi=%d\n",
 		  vport_num, promisc_all, promisc_mc);
 
-	if (!vport->trusted || !vport->enabled) {
+	if (!vport->info.trusted || !vport->enabled) {
 		promisc_uc = 0;
 		promisc_mc = 0;
 		promisc_all = 0;
@@ -1291,30 +1207,20 @@ static int esw_vport_ingress_config(struct mlx5_eswitch *esw,
 				    struct mlx5_vport *vport)
 {
 	struct mlx5_flow_spec *spec;
-	u8 smac[ETH_ALEN];
 	int err = 0;
 	u8 *smac_v;
 
-	if (vport->spoofchk) {
-		err = mlx5_query_nic_vport_mac_address(esw->dev, vport->vport, smac);
-		if (err) {
-			esw_warn(esw->dev,
-				 "vport[%d] configure ingress rules failed, query smac failed, err(%d)\n",
-				 vport->vport, err);
-			return err;
-		}
+	if (vport->info.spoofchk && !is_valid_ether_addr(vport->info.mac)) {
+		mlx5_core_warn(esw->dev,
+			       "vport[%d] configure ingress rules failed, illegal mac with spoofchk\n",
+			       vport->vport);
+		return -EPERM;
 
-		if (!is_valid_ether_addr(smac)) {
-			mlx5_core_warn(esw->dev,
-				       "vport[%d] configure ingress rules failed, illegal mac with spoofchk\n",
-				       vport->vport);
-			return -EPERM;
-		}
 	}
 
 	esw_vport_cleanup_ingress_rules(esw, vport);
 
-	if (!vport->vlan && !vport->qos && !vport->spoofchk) {
+	if (!vport->info.vlan && !vport->info.qos && !vport->info.spoofchk) {
 		esw_vport_disable_ingress_acl(esw, vport);
 		return 0;
 	}
@@ -1323,7 +1229,7 @@ static int esw_vport_ingress_config(struct mlx5_eswitch *esw,
 
 	esw_debug(esw->dev,
 		  "vport[%d] configure ingress rules, vlan(%d) qos(%d)\n",
-		  vport->vport, vport->vlan, vport->qos);
+		  vport->vport, vport->info.vlan, vport->info.qos);
 
 	spec = mlx5_vzalloc(sizeof(*spec));
 	if (!spec) {
@@ -1333,16 +1239,16 @@ static int esw_vport_ingress_config(struct mlx5_eswitch *esw,
 		goto out;
 	}
 
-	if (vport->vlan || vport->qos)
+	if (vport->info.vlan || vport->info.qos)
 		MLX5_SET_TO_ONES(fte_match_param, spec->match_criteria, outer_headers.vlan_tag);
 
-	if (vport->spoofchk) {
+	if (vport->info.spoofchk) {
 		MLX5_SET_TO_ONES(fte_match_param, spec->match_criteria, outer_headers.smac_47_16);
 		MLX5_SET_TO_ONES(fte_match_param, spec->match_criteria, outer_headers.smac_15_0);
 		smac_v = MLX5_ADDR_OF(fte_match_param,
 				      spec->match_value,
 				      outer_headers.smac_47_16);
-		ether_addr_copy(smac_v, smac);
+		ether_addr_copy(smac_v, vport->info.mac);
 	}
 
 	spec->match_criteria_enable = MLX5_MATCH_OUTER_HEADERS;
@@ -1352,8 +1258,9 @@ static int esw_vport_ingress_config(struct mlx5_eswitch *esw,
 				   0, NULL);
 	if (IS_ERR(vport->ingress.allow_rule)) {
 		err = PTR_ERR(vport->ingress.allow_rule);
-		pr_warn("vport[%d] configure ingress allow rule, err(%d)\n",
-			vport->vport, err);
+		esw_warn(esw->dev,
+			 "vport[%d] configure ingress allow rule, err(%d)\n",
+			 vport->vport, err);
 		vport->ingress.allow_rule = NULL;
 		goto out;
 	}
@@ -1365,8 +1272,9 @@ static int esw_vport_ingress_config(struct mlx5_eswitch *esw,
 				   0, NULL);
 	if (IS_ERR(vport->ingress.drop_rule)) {
 		err = PTR_ERR(vport->ingress.drop_rule);
-		pr_warn("vport[%d] configure ingress drop rule, err(%d)\n",
-			vport->vport, err);
+		esw_warn(esw->dev,
+			 "vport[%d] configure ingress drop rule, err(%d)\n",
+			 vport->vport, err);
 		vport->ingress.drop_rule = NULL;
 		goto out;
 	}
@@ -1386,7 +1294,7 @@ static int esw_vport_egress_config(struct mlx5_eswitch *esw,
 
 	esw_vport_cleanup_egress_rules(esw, vport);
 
-	if (!vport->vlan && !vport->qos) {
+	if (!vport->info.vlan && !vport->info.qos) {
 		esw_vport_disable_egress_acl(esw, vport);
 		return 0;
 	}
@@ -1395,7 +1303,7 @@ static int esw_vport_egress_config(struct mlx5_eswitch *esw,
 
 	esw_debug(esw->dev,
 		  "vport[%d] configure egress rules, vlan(%d) qos(%d)\n",
-		  vport->vport, vport->vlan, vport->qos);
+		  vport->vport, vport->info.vlan, vport->info.qos);
 
 	spec = mlx5_vzalloc(sizeof(*spec));
 	if (!spec) {
@@ -1409,7 +1317,7 @@ static int esw_vport_egress_config(struct mlx5_eswitch *esw,
 	MLX5_SET_TO_ONES(fte_match_param, spec->match_criteria, outer_headers.vlan_tag);
 	MLX5_SET_TO_ONES(fte_match_param, spec->match_value, outer_headers.vlan_tag);
 	MLX5_SET_TO_ONES(fte_match_param, spec->match_criteria, outer_headers.first_vid);
-	MLX5_SET(fte_match_param, spec->match_value, outer_headers.first_vid, vport->vlan);
+	MLX5_SET(fte_match_param, spec->match_value, outer_headers.first_vid, vport->info.vlan);
 
 	spec->match_criteria_enable = MLX5_MATCH_OUTER_HEADERS;
 	vport->egress.allowed_vlan =
@@ -1418,8 +1326,9 @@ static int esw_vport_egress_config(struct mlx5_eswitch *esw,
 				   0, NULL);
 	if (IS_ERR(vport->egress.allowed_vlan)) {
 		err = PTR_ERR(vport->egress.allowed_vlan);
-		pr_warn("vport[%d] configure egress allowed vlan rule failed, err(%d)\n",
-			vport->vport, err);
+		esw_warn(esw->dev,
+			 "vport[%d] configure egress allowed vlan rule failed, err(%d)\n",
+			 vport->vport, err);
 		vport->egress.allowed_vlan = NULL;
 		goto out;
 	}
@@ -1432,8 +1341,9 @@ static int esw_vport_egress_config(struct mlx5_eswitch *esw,
 				   0, NULL);
 	if (IS_ERR(vport->egress.drop_rule)) {
 		err = PTR_ERR(vport->egress.drop_rule);
-		pr_warn("vport[%d] configure egress drop rule failed, err(%d)\n",
-			vport->vport, err);
+		esw_warn(esw->dev,
+			 "vport[%d] configure egress drop rule failed, err(%d)\n",
+			 vport->vport, err);
 		vport->egress.drop_rule = NULL;
 	}
 out:
@@ -1441,6 +1351,41 @@ out:
 	return err;
 }
 
+static void node_guid_gen_from_mac(u64 *node_guid, u8 mac[ETH_ALEN])
+{
+	((u8 *)node_guid)[7] = mac[0];
+	((u8 *)node_guid)[6] = mac[1];
+	((u8 *)node_guid)[5] = mac[2];
+	((u8 *)node_guid)[4] = 0xff;
+	((u8 *)node_guid)[3] = 0xfe;
+	((u8 *)node_guid)[2] = mac[3];
+	((u8 *)node_guid)[1] = mac[4];
+	((u8 *)node_guid)[0] = mac[5];
+}
+
+static void esw_apply_vport_conf(struct mlx5_eswitch *esw,
+				 struct mlx5_vport *vport)
+{
+	int vport_num = vport->vport;
+
+	if (!vport_num)
+		return;
+
+	mlx5_modify_vport_admin_state(esw->dev,
+				      MLX5_QUERY_VPORT_STATE_IN_OP_MOD_ESW_VPORT,
+				      vport_num,
+				      vport->info.link_state);
+	mlx5_modify_nic_vport_mac_address(esw->dev, vport_num, vport->info.mac);
+	mlx5_modify_nic_vport_node_guid(esw->dev, vport_num, vport->info.node_guid);
+	modify_esw_vport_cvlan(esw->dev, vport_num, vport->info.vlan, vport->info.qos,
+			       (vport->info.vlan || vport->info.qos));
+
+	/* Only legacy mode needs ACLs */
+	if (esw->mode == SRIOV_LEGACY) {
+		esw_vport_ingress_config(esw, vport);
+		esw_vport_egress_config(esw, vport);
+	}
+}
 static void esw_enable_vport(struct mlx5_eswitch *esw, int vport_num,
 			     int enable_events)
 {
@@ -1451,23 +1396,17 @@ static void esw_enable_vport(struct mlx5_eswitch *esw, int vport_num,
 
 	esw_debug(esw->dev, "Enabling VPORT(%d)\n", vport_num);
 
-	/* Only VFs need ACLs for VST and spoofchk filtering */
-	if (vport_num && esw->mode == SRIOV_LEGACY) {
-		esw_vport_ingress_config(esw, vport);
-		esw_vport_egress_config(esw, vport);
-	}
-
-	mlx5_modify_vport_admin_state(esw->dev,
-				      MLX5_QUERY_VPORT_STATE_IN_OP_MOD_ESW_VPORT,
-				      vport_num,
-				      MLX5_ESW_VPORT_ADMIN_STATE_AUTO);
+	/* Restore old vport configuration */
+	esw_apply_vport_conf(esw, vport);
 
 	/* Sync with current vport context */
 	vport->enabled_events = enable_events;
 	vport->enabled = true;
 
 	/* only PF is trusted by default */
-	vport->trusted = (vport_num) ? false : true;
+	if (!vport_num)
+		vport->info.trusted = true;
+
 	esw_vport_change_handle_locked(vport);
 
 	esw->enabled_vports++;
@@ -1487,11 +1426,6 @@ static void esw_disable_vport(struct mlx5_eswitch *esw, int vport_num)
 	vport->enabled = false;
 
 	synchronize_irq(mlx5_get_msix_vec(esw->dev, MLX5_EQ_VEC_ASYNC));
-
-	mlx5_modify_vport_admin_state(esw->dev,
-				      MLX5_QUERY_VPORT_STATE_IN_OP_MOD_ESW_VPORT,
-				      vport_num,
-				      MLX5_ESW_VPORT_ADMIN_STATE_DOWN);
 	/* Wait for current already scheduled events to complete */
 	flush_workqueue(esw->work_queue);
 	/* Disable events from this vport */
@@ -1503,7 +1437,12 @@ static void esw_disable_vport(struct mlx5_eswitch *esw, int vport_num)
 	 */
 	esw_vport_change_handle_locked(vport);
 	vport->enabled_events = 0;
+
 	if (vport_num && esw->mode == SRIOV_LEGACY) {
+		mlx5_modify_vport_admin_state(esw->dev,
+					      MLX5_QUERY_VPORT_STATE_IN_OP_MOD_ESW_VPORT,
+					      vport_num,
+					      MLX5_ESW_VPORT_ADMIN_STATE_DOWN);
 		esw_vport_disable_egress_acl(esw, vport);
 		esw_vport_disable_ingress_acl(esw, vport);
 	}
@@ -1590,6 +1529,25 @@ void mlx5_eswitch_disable_sriov(struct mlx5_eswitch *esw)
 	esw_enable_vport(esw, 0, UC_ADDR_CHANGE);
 }
 
+void mlx5_eswitch_attach(struct mlx5_eswitch *esw)
+{
+	if (!esw || !MLX5_CAP_GEN(esw->dev, vport_group_manager) ||
+	    MLX5_CAP_GEN(esw->dev, port_type) != MLX5_CAP_PORT_TYPE_ETH)
+		return;
+
+	esw_enable_vport(esw, 0, UC_ADDR_CHANGE);
+	/* VF Vports will be enabled when SRIOV is enabled */
+}
+
+void mlx5_eswitch_detach(struct mlx5_eswitch *esw)
+{
+	if (!esw || !MLX5_CAP_GEN(esw->dev, vport_group_manager) ||
+	    MLX5_CAP_GEN(esw->dev, port_type) != MLX5_CAP_PORT_TYPE_ETH)
+		return;
+
+	esw_disable_vport(esw, 0);
+}
+
 int mlx5_eswitch_init(struct mlx5_core_dev *dev)
 {
 	int l2_table_size = 1 << MLX5_CAP_GEN(dev, log_max_l2_table);
@@ -1657,6 +1615,7 @@ int mlx5_eswitch_init(struct mlx5_core_dev *dev)
 		struct mlx5_vport *vport = &esw->vports[vport_num];
 
 		vport->vport = vport_num;
+		vport->info.link_state = MLX5_ESW_VPORT_ADMIN_STATE_AUTO;
 		vport->dev = dev;
 		INIT_WORK(&vport->vport_change_handler,
 			  esw_vport_change_handler);
@@ -1667,8 +1626,6 @@ int mlx5_eswitch_init(struct mlx5_core_dev *dev)
 	esw->mode = SRIOV_NONE;
 
 	dev->priv.eswitch = esw;
-	esw_enable_vport(esw, 0, UC_ADDR_CHANGE);
-	/* VF Vports will be enabled when SRIOV is enabled */
 	return 0;
 abort:
 	if (esw->work_queue)
@@ -1687,7 +1644,6 @@ void mlx5_eswitch_cleanup(struct mlx5_eswitch *esw)
 		return;
 
 	esw_info(esw->dev, "cleanup\n");
-	esw_disable_vport(esw, 0);
 
 	esw->dev->priv.eswitch = NULL;
 	destroy_workqueue(esw->work_queue);
@@ -1720,18 +1676,6 @@ void mlx5_eswitch_vport_event(struct mlx5_eswitch *esw, struct mlx5_eqe *eqe)
 	(esw && MLX5_CAP_GEN(esw->dev, vport_group_manager) && mlx5_core_is_pf(esw->dev))
 #define LEGAL_VPORT(esw, vport) (vport >= 0 && vport < esw->total_vports)
 
-static void node_guid_gen_from_mac(u64 *node_guid, u8 mac[ETH_ALEN])
-{
-	((u8 *)node_guid)[7] = mac[0];
-	((u8 *)node_guid)[6] = mac[1];
-	((u8 *)node_guid)[5] = mac[2];
-	((u8 *)node_guid)[4] = 0xff;
-	((u8 *)node_guid)[3] = 0xfe;
-	((u8 *)node_guid)[2] = mac[3];
-	((u8 *)node_guid)[1] = mac[4];
-	((u8 *)node_guid)[0] = mac[5];
-}
-
 int mlx5_eswitch_set_vport_mac(struct mlx5_eswitch *esw,
 			       int vport, u8 mac[ETH_ALEN])
 {
@@ -1744,13 +1688,15 @@ int mlx5_eswitch_set_vport_mac(struct mlx5_eswitch *esw,
 	if (!LEGAL_VPORT(esw, vport))
 		return -EINVAL;
 
+	mutex_lock(&esw->state_lock);
 	evport = &esw->vports[vport];
 
-	if (evport->spoofchk && !is_valid_ether_addr(mac)) {
+	if (evport->info.spoofchk && !is_valid_ether_addr(mac)) {
 		mlx5_core_warn(esw->dev,
 			       "MAC invalidation is not allowed when spoofchk is on, vport(%d)\n",
 			       vport);
-		return -EPERM;
+		err = -EPERM;
+		goto unlock;
 	}
 
 	err = mlx5_modify_nic_vport_mac_address(esw->dev, vport, mac);
@@ -1758,7 +1704,7 @@ int mlx5_eswitch_set_vport_mac(struct mlx5_eswitch *esw,
 		mlx5_core_warn(esw->dev,
 			       "Failed to mlx5_modify_nic_vport_mac vport(%d) err=(%d)\n",
 			       vport, err);
-		return err;
+		goto unlock;
 	}
 
 	node_guid_gen_from_mac(&node_guid, mac);
@@ -1768,9 +1714,12 @@ int mlx5_eswitch_set_vport_mac(struct mlx5_eswitch *esw,
 			       "Failed to set vport %d node guid, err = %d. RDMA_CM will not function properly for this VF.\n",
 			       vport, err);
 
-	mutex_lock(&esw->state_lock);
+	ether_addr_copy(evport->info.mac, mac);
+	evport->info.node_guid = node_guid;
 	if (evport->enabled && esw->mode == SRIOV_LEGACY)
 		err = esw_vport_ingress_config(esw, evport);
+
+unlock:
 	mutex_unlock(&esw->state_lock);
 	return err;
 }
@@ -1778,22 +1727,38 @@ int mlx5_eswitch_set_vport_mac(struct mlx5_eswitch *esw,
 int mlx5_eswitch_set_vport_state(struct mlx5_eswitch *esw,
 				 int vport, int link_state)
 {
+	struct mlx5_vport *evport;
+	int err = 0;
+
 	if (!ESW_ALLOWED(esw))
 		return -EPERM;
 	if (!LEGAL_VPORT(esw, vport))
 		return -EINVAL;
 
-	return mlx5_modify_vport_admin_state(esw->dev,
-					     MLX5_QUERY_VPORT_STATE_IN_OP_MOD_ESW_VPORT,
-					     vport, link_state);
+	mutex_lock(&esw->state_lock);
+	evport = &esw->vports[vport];
+
+	err = mlx5_modify_vport_admin_state(esw->dev,
+					    MLX5_QUERY_VPORT_STATE_IN_OP_MOD_ESW_VPORT,
+					    vport, link_state);
+	if (err) {
+		mlx5_core_warn(esw->dev,
+			       "Failed to set vport %d link state, err = %d",
+			       vport, err);
+		goto unlock;
+	}
+
+	evport->info.link_state = link_state;
+
+unlock:
+	mutex_unlock(&esw->state_lock);
+	return 0;
 }
 
 int mlx5_eswitch_get_vport_config(struct mlx5_eswitch *esw,
 				  int vport, struct ifla_vf_info *ivi)
 {
 	struct mlx5_vport *evport;
-	u16 vlan;
-	u8 qos;
 
 	if (!ESW_ALLOWED(esw))
 		return -EPERM;
@@ -1805,54 +1770,61 @@ int mlx5_eswitch_get_vport_config(struct mlx5_eswitch *esw,
 	memset(ivi, 0, sizeof(*ivi));
 	ivi->vf = vport - 1;
 
-	mlx5_query_nic_vport_mac_address(esw->dev, vport, ivi->mac);
-	ivi->linkstate = mlx5_query_vport_admin_state(esw->dev,
-						      MLX5_QUERY_VPORT_STATE_IN_OP_MOD_ESW_VPORT,
-						      vport);
-	query_esw_vport_cvlan(esw->dev, vport, &vlan, &qos);
-	ivi->vlan = vlan;
-	ivi->qos = qos;
-	ivi->spoofchk = evport->spoofchk;
+	mutex_lock(&esw->state_lock);
+	ether_addr_copy(ivi->mac, evport->info.mac);
+	ivi->linkstate = evport->info.link_state;
+	ivi->vlan = evport->info.vlan;
+	ivi->qos = evport->info.qos;
+	ivi->spoofchk = evport->info.spoofchk;
+	ivi->trusted = evport->info.trusted;
+	mutex_unlock(&esw->state_lock);
 
 	return 0;
 }
 
-int mlx5_eswitch_set_vport_vlan(struct mlx5_eswitch *esw,
-				int vport, u16 vlan, u8 qos)
+int __mlx5_eswitch_set_vport_vlan(struct mlx5_eswitch *esw,
+				  int vport, u16 vlan, u8 qos, u8 set_flags)
 {
 	struct mlx5_vport *evport;
 	int err = 0;
-	int set = 0;
 
 	if (!ESW_ALLOWED(esw))
 		return -EPERM;
 	if (!LEGAL_VPORT(esw, vport) || (vlan > 4095) || (qos > 7))
 		return -EINVAL;
 
-	if (vlan || qos)
-		set = 1;
-
+	mutex_lock(&esw->state_lock);
 	evport = &esw->vports[vport];
 
-	err = modify_esw_vport_cvlan(esw->dev, vport, vlan, qos, set);
+	err = modify_esw_vport_cvlan(esw->dev, vport, vlan, qos, set_flags);
 	if (err)
-		return err;
+		goto unlock;
 
-	mutex_lock(&esw->state_lock);
-	evport->vlan = vlan;
-	evport->qos = qos;
+	evport->info.vlan = vlan;
+	evport->info.qos = qos;
 	if (evport->enabled && esw->mode == SRIOV_LEGACY) {
 		err = esw_vport_ingress_config(esw, evport);
 		if (err)
-			goto out;
+			goto unlock;
 		err = esw_vport_egress_config(esw, evport);
 	}
 
-out:
+unlock:
 	mutex_unlock(&esw->state_lock);
 	return err;
 }
 
+int mlx5_eswitch_set_vport_vlan(struct mlx5_eswitch *esw,
+				int vport, u16 vlan, u8 qos)
+{
+	u8 set_flags = 0;
+
+	if (vlan || qos)
+		set_flags = SET_VLAN_STRIP | SET_VLAN_INSERT;
+
+	return __mlx5_eswitch_set_vport_vlan(esw, vport, vlan, qos, set_flags);
+}
+
 int mlx5_eswitch_set_vport_spoofchk(struct mlx5_eswitch *esw,
 				    int vport, bool spoofchk)
 {
@@ -1865,16 +1837,14 @@ int mlx5_eswitch_set_vport_spoofchk(struct mlx5_eswitch *esw,
 	if (!LEGAL_VPORT(esw, vport))
 		return -EINVAL;
 
-	evport = &esw->vports[vport];
-
 	mutex_lock(&esw->state_lock);
-	pschk = evport->spoofchk;
-	evport->spoofchk = spoofchk;
-	if (evport->enabled && esw->mode == SRIOV_LEGACY) {
+	evport = &esw->vports[vport];
+	pschk = evport->info.spoofchk;
+	evport->info.spoofchk = spoofchk;
+	if (evport->enabled && esw->mode == SRIOV_LEGACY)
 		err = esw_vport_ingress_config(esw, evport);
-		if (err)
-			evport->spoofchk = pschk;
-	}
+	if (err)
+		evport->info.spoofchk = pschk;
 	mutex_unlock(&esw->state_lock);
 
 	return err;
@@ -1890,10 +1860,9 @@ int mlx5_eswitch_set_vport_trust(struct mlx5_eswitch *esw,
 	if (!LEGAL_VPORT(esw, vport))
 		return -EINVAL;
 
-	evport = &esw->vports[vport];
-
 	mutex_lock(&esw->state_lock);
-	evport->trusted = setting;
+	evport = &esw->vports[vport];
+	evport->info.trusted = setting;
 	if (evport->enabled)
 		esw_vport_change_handle_locked(evport);
 	mutex_unlock(&esw->state_lock);
@@ -1906,7 +1875,7 @@ int mlx5_eswitch_get_vport_stats(struct mlx5_eswitch *esw,
 				 struct ifla_vf_stats *vf_stats)
 {
 	int outlen = MLX5_ST_SZ_BYTES(query_vport_counter_out);
-	u32 in[MLX5_ST_SZ_DW(query_vport_counter_in)];
+	u32 in[MLX5_ST_SZ_DW(query_vport_counter_in)] = {0};
 	int err = 0;
 	u32 *out;
 
@@ -1919,8 +1888,6 @@ int mlx5_eswitch_get_vport_stats(struct mlx5_eswitch *esw,
 	if (!out)
 		return -ENOMEM;
 
-	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);
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/eswitch.h b/drivers/net/ethernet/mellanox/mlx5/core/eswitch.h
index a96140971d77..2e2938e08cda 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/eswitch.h
+++ b/drivers/net/ethernet/mellanox/mlx5/core/eswitch.h
@@ -109,6 +109,16 @@ struct vport_egress {
 	struct mlx5_flow_rule  *drop_rule;
 };
 
+struct mlx5_vport_info {
+	u8                      mac[ETH_ALEN];
+	u16                     vlan;
+	u8                      qos;
+	u64                     node_guid;
+	int                     link_state;
+	bool                    spoofchk;
+	bool                    trusted;
+};
+
 struct mlx5_vport {
 	struct mlx5_core_dev    *dev;
 	int                     vport;
@@ -121,10 +131,8 @@ struct mlx5_vport {
 	struct vport_ingress    ingress;
 	struct vport_egress     egress;
 
-	u16                     vlan;
-	u8                      qos;
-	bool                    spoofchk;
-	bool                    trusted;
+	struct mlx5_vport_info  info;
+
 	bool                    enabled;
 	u16                     enabled_events;
 };
@@ -149,6 +157,7 @@ struct mlx5_eswitch_fdb {
 			struct mlx5_flow_group *send_to_vport_grp;
 			struct mlx5_flow_group *miss_grp;
 			struct mlx5_flow_rule  *miss_rule;
+			int vlan_push_pop_refcount;
 		} offloads;
 	};
 };
@@ -170,11 +179,14 @@ struct mlx5_eswitch_rep {
 	void		       (*unload)(struct mlx5_eswitch *esw,
 					 struct mlx5_eswitch_rep *rep);
 	u16		       vport;
-	struct mlx5_flow_rule *vport_rx_rule;
+	u8		       hw_id[ETH_ALEN];
 	void		      *priv_data;
+
+	struct mlx5_flow_rule *vport_rx_rule;
 	struct list_head       vport_sqs_list;
+	u16		       vlan;
+	u32		       vlan_refcount;
 	bool		       valid;
-	u8		       hw_id[ETH_ALEN];
 };
 
 struct mlx5_esw_offload {
@@ -201,9 +213,14 @@ struct mlx5_eswitch {
 	int                     mode;
 };
 
+void esw_offloads_cleanup(struct mlx5_eswitch *esw, int nvports);
+int esw_offloads_init(struct mlx5_eswitch *esw, int nvports);
+
 /* E-Switch API */
 int mlx5_eswitch_init(struct mlx5_core_dev *dev);
 void mlx5_eswitch_cleanup(struct mlx5_eswitch *esw);
+void mlx5_eswitch_attach(struct mlx5_eswitch *esw);
+void mlx5_eswitch_detach(struct mlx5_eswitch *esw);
 void mlx5_eswitch_vport_event(struct mlx5_eswitch *esw, struct mlx5_eqe *eqe);
 int mlx5_eswitch_enable_sriov(struct mlx5_eswitch *esw, int nvfs, int mode);
 void mlx5_eswitch_disable_sriov(struct mlx5_eswitch *esw);
@@ -224,14 +241,32 @@ int mlx5_eswitch_get_vport_stats(struct mlx5_eswitch *esw,
 				 struct ifla_vf_stats *vf_stats);
 
 struct mlx5_flow_spec;
+struct mlx5_esw_flow_attr;
 
 struct mlx5_flow_rule *
 mlx5_eswitch_add_offloaded_rule(struct mlx5_eswitch *esw,
 				struct mlx5_flow_spec *spec,
-				u32 action, u32 src_vport, u32 dst_vport);
+				struct mlx5_esw_flow_attr *attr);
 struct mlx5_flow_rule *
 mlx5_eswitch_create_vport_rx_rule(struct mlx5_eswitch *esw, int vport, u32 tirn);
 
+enum {
+	SET_VLAN_STRIP	= BIT(0),
+	SET_VLAN_INSERT	= BIT(1)
+};
+
+#define MLX5_FLOW_CONTEXT_ACTION_VLAN_POP  0x40
+#define MLX5_FLOW_CONTEXT_ACTION_VLAN_PUSH 0x80
+
+struct mlx5_esw_flow_attr {
+	struct mlx5_eswitch_rep *in_rep;
+	struct mlx5_eswitch_rep *out_rep;
+
+	int	action;
+	u16	vlan;
+	bool	vlan_handled;
+};
+
 int mlx5_eswitch_sqs2vport_start(struct mlx5_eswitch *esw,
 				 struct mlx5_eswitch_rep *rep,
 				 u16 *sqns_array, int sqns_num);
@@ -241,9 +276,17 @@ void mlx5_eswitch_sqs2vport_stop(struct mlx5_eswitch *esw,
 int mlx5_devlink_eswitch_mode_set(struct devlink *devlink, u16 mode);
 int mlx5_devlink_eswitch_mode_get(struct devlink *devlink, u16 *mode);
 void mlx5_eswitch_register_vport_rep(struct mlx5_eswitch *esw,
+				     int vport_index,
 				     struct mlx5_eswitch_rep *rep);
 void mlx5_eswitch_unregister_vport_rep(struct mlx5_eswitch *esw,
-				       int vport);
+				       int vport_index);
+
+int mlx5_eswitch_add_vlan_action(struct mlx5_eswitch *esw,
+				 struct mlx5_esw_flow_attr *attr);
+int mlx5_eswitch_del_vlan_action(struct mlx5_eswitch *esw,
+				 struct mlx5_esw_flow_attr *attr);
+int __mlx5_eswitch_set_vport_vlan(struct mlx5_eswitch *esw,
+				  int vport, u16 vlan, u8 qos, u8 set_flags);
 
 #define MLX5_DEBUG_ESWITCH_MASK BIT(3)
 
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/eswitch_offloads.c b/drivers/net/ethernet/mellanox/mlx5/core/eswitch_offloads.c
index 7de40e6b0c25..c55ad8d00c05 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/eswitch_offloads.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/eswitch_offloads.c
@@ -46,19 +46,22 @@ enum {
 struct mlx5_flow_rule *
 mlx5_eswitch_add_offloaded_rule(struct mlx5_eswitch *esw,
 				struct mlx5_flow_spec *spec,
-				u32 action, u32 src_vport, u32 dst_vport)
+				struct mlx5_esw_flow_attr *attr)
 {
 	struct mlx5_flow_destination dest = { 0 };
 	struct mlx5_fc *counter = NULL;
 	struct mlx5_flow_rule *rule;
 	void *misc;
+	int action;
 
 	if (esw->mode != SRIOV_OFFLOADS)
 		return ERR_PTR(-EOPNOTSUPP);
 
+	action = attr->action;
+
 	if (action & MLX5_FLOW_CONTEXT_ACTION_FWD_DEST) {
 		dest.type = MLX5_FLOW_DESTINATION_TYPE_VPORT;
-		dest.vport_num = dst_vport;
+		dest.vport_num = attr->out_rep->vport;
 		action = MLX5_FLOW_CONTEXT_ACTION_FWD_DEST;
 	} else if (action & MLX5_FLOW_CONTEXT_ACTION_COUNT) {
 		counter = mlx5_fc_create(esw->dev, true);
@@ -69,7 +72,7 @@ mlx5_eswitch_add_offloaded_rule(struct mlx5_eswitch *esw,
 	}
 
 	misc = MLX5_ADDR_OF(fte_match_param, spec->match_value, misc_parameters);
-	MLX5_SET(fte_match_set_misc, misc, source_port, src_vport);
+	MLX5_SET(fte_match_set_misc, misc, source_port, attr->in_rep->vport);
 
 	misc = MLX5_ADDR_OF(fte_match_param, spec->match_criteria, misc_parameters);
 	MLX5_SET_TO_ONES(fte_match_set_misc, misc, source_port);
@@ -86,6 +89,186 @@ mlx5_eswitch_add_offloaded_rule(struct mlx5_eswitch *esw,
 	return rule;
 }
 
+static int esw_set_global_vlan_pop(struct mlx5_eswitch *esw, u8 val)
+{
+	struct mlx5_eswitch_rep *rep;
+	int vf_vport, err = 0;
+
+	esw_debug(esw->dev, "%s applying global %s policy\n", __func__, val ? "pop" : "none");
+	for (vf_vport = 1; vf_vport < esw->enabled_vports; vf_vport++) {
+		rep = &esw->offloads.vport_reps[vf_vport];
+		if (!rep->valid)
+			continue;
+
+		err = __mlx5_eswitch_set_vport_vlan(esw, rep->vport, 0, 0, val);
+		if (err)
+			goto out;
+	}
+
+out:
+	return err;
+}
+
+static struct mlx5_eswitch_rep *
+esw_vlan_action_get_vport(struct mlx5_esw_flow_attr *attr, bool push, bool pop)
+{
+	struct mlx5_eswitch_rep *in_rep, *out_rep, *vport = NULL;
+
+	in_rep  = attr->in_rep;
+	out_rep = attr->out_rep;
+
+	if (push)
+		vport = in_rep;
+	else if (pop)
+		vport = out_rep;
+	else
+		vport = in_rep;
+
+	return vport;
+}
+
+static int esw_add_vlan_action_check(struct mlx5_esw_flow_attr *attr,
+				     bool push, bool pop, bool fwd)
+{
+	struct mlx5_eswitch_rep *in_rep, *out_rep;
+
+	if ((push || pop) && !fwd)
+		goto out_notsupp;
+
+	in_rep  = attr->in_rep;
+	out_rep = attr->out_rep;
+
+	if (push && in_rep->vport == FDB_UPLINK_VPORT)
+		goto out_notsupp;
+
+	if (pop && out_rep->vport == FDB_UPLINK_VPORT)
+		goto out_notsupp;
+
+	/* vport has vlan push configured, can't offload VF --> wire rules w.o it */
+	if (!push && !pop && fwd)
+		if (in_rep->vlan && out_rep->vport == FDB_UPLINK_VPORT)
+			goto out_notsupp;
+
+	/* protects against (1) setting rules with different vlans to push and
+	 * (2) setting rules w.o vlans (attr->vlan = 0) && w. vlans to push (!= 0)
+	 */
+	if (push && in_rep->vlan_refcount && (in_rep->vlan != attr->vlan))
+		goto out_notsupp;
+
+	return 0;
+
+out_notsupp:
+	return -ENOTSUPP;
+}
+
+int mlx5_eswitch_add_vlan_action(struct mlx5_eswitch *esw,
+				 struct mlx5_esw_flow_attr *attr)
+{
+	struct offloads_fdb *offloads = &esw->fdb_table.offloads;
+	struct mlx5_eswitch_rep *vport = NULL;
+	bool push, pop, fwd;
+	int err = 0;
+
+	push = !!(attr->action & MLX5_FLOW_CONTEXT_ACTION_VLAN_PUSH);
+	pop  = !!(attr->action & MLX5_FLOW_CONTEXT_ACTION_VLAN_POP);
+	fwd  = !!(attr->action & MLX5_FLOW_CONTEXT_ACTION_FWD_DEST);
+
+	err = esw_add_vlan_action_check(attr, push, pop, fwd);
+	if (err)
+		return err;
+
+	attr->vlan_handled = false;
+
+	vport = esw_vlan_action_get_vport(attr, push, pop);
+
+	if (!push && !pop && fwd) {
+		/* tracks VF --> wire rules without vlan push action */
+		if (attr->out_rep->vport == FDB_UPLINK_VPORT) {
+			vport->vlan_refcount++;
+			attr->vlan_handled = true;
+		}
+
+		return 0;
+	}
+
+	if (!push && !pop)
+		return 0;
+
+	if (!(offloads->vlan_push_pop_refcount)) {
+		/* it's the 1st vlan rule, apply global vlan pop policy */
+		err = esw_set_global_vlan_pop(esw, SET_VLAN_STRIP);
+		if (err)
+			goto out;
+	}
+	offloads->vlan_push_pop_refcount++;
+
+	if (push) {
+		if (vport->vlan_refcount)
+			goto skip_set_push;
+
+		err = __mlx5_eswitch_set_vport_vlan(esw, vport->vport, attr->vlan, 0,
+						    SET_VLAN_INSERT | SET_VLAN_STRIP);
+		if (err)
+			goto out;
+		vport->vlan = attr->vlan;
+skip_set_push:
+		vport->vlan_refcount++;
+	}
+out:
+	if (!err)
+		attr->vlan_handled = true;
+	return err;
+}
+
+int mlx5_eswitch_del_vlan_action(struct mlx5_eswitch *esw,
+				 struct mlx5_esw_flow_attr *attr)
+{
+	struct offloads_fdb *offloads = &esw->fdb_table.offloads;
+	struct mlx5_eswitch_rep *vport = NULL;
+	bool push, pop, fwd;
+	int err = 0;
+
+	if (!attr->vlan_handled)
+		return 0;
+
+	push = !!(attr->action & MLX5_FLOW_CONTEXT_ACTION_VLAN_PUSH);
+	pop  = !!(attr->action & MLX5_FLOW_CONTEXT_ACTION_VLAN_POP);
+	fwd  = !!(attr->action & MLX5_FLOW_CONTEXT_ACTION_FWD_DEST);
+
+	vport = esw_vlan_action_get_vport(attr, push, pop);
+
+	if (!push && !pop && fwd) {
+		/* tracks VF --> wire rules without vlan push action */
+		if (attr->out_rep->vport == FDB_UPLINK_VPORT)
+			vport->vlan_refcount--;
+
+		return 0;
+	}
+
+	if (push) {
+		vport->vlan_refcount--;
+		if (vport->vlan_refcount)
+			goto skip_unset_push;
+
+		vport->vlan = 0;
+		err = __mlx5_eswitch_set_vport_vlan(esw, vport->vport,
+						    0, 0, SET_VLAN_STRIP);
+		if (err)
+			goto out;
+	}
+
+skip_unset_push:
+	offloads->vlan_push_pop_refcount--;
+	if (offloads->vlan_push_pop_refcount)
+		return 0;
+
+	/* no more vlan rules, stop global vlan pop policy */
+	err = esw_set_global_vlan_pop(esw, 0);
+
+out:
+	return err;
+}
+
 static struct mlx5_flow_rule *
 mlx5_eswitch_add_send_to_vport_rule(struct mlx5_eswitch *esw, int vport, u32 sqn)
 {
@@ -144,16 +327,12 @@ int mlx5_eswitch_sqs2vport_start(struct mlx5_eswitch *esw,
 {
 	struct mlx5_flow_rule *flow_rule;
 	struct mlx5_esw_sq *esw_sq;
-	int vport;
 	int err;
 	int i;
 
 	if (esw->mode != SRIOV_OFFLOADS)
 		return 0;
 
-	vport = rep->vport == 0 ?
-		FDB_UPLINK_VPORT : rep->vport;
-
 	for (i = 0; i < sqns_num; i++) {
 		esw_sq = kzalloc(sizeof(*esw_sq), GFP_KERNEL);
 		if (!esw_sq) {
@@ -163,7 +342,7 @@ int mlx5_eswitch_sqs2vport_start(struct mlx5_eswitch *esw,
 
 		/* Add re-inject rule to the PF/representor sqs */
 		flow_rule = mlx5_eswitch_add_send_to_vport_rule(esw,
-								vport,
+								rep->vport,
 								sqns_array[i]);
 		if (IS_ERR(flow_rule)) {
 			err = PTR_ERR(flow_rule);
@@ -620,27 +799,36 @@ int mlx5_devlink_eswitch_mode_get(struct devlink *devlink, u16 *mode)
 }
 
 void mlx5_eswitch_register_vport_rep(struct mlx5_eswitch *esw,
-				     struct mlx5_eswitch_rep *rep)
+				     int vport_index,
+				     struct mlx5_eswitch_rep *__rep)
 {
 	struct mlx5_esw_offload *offloads = &esw->offloads;
+	struct mlx5_eswitch_rep *rep;
+
+	rep = &offloads->vport_reps[vport_index];
+
+	memset(rep, 0, sizeof(*rep));
 
-	memcpy(&offloads->vport_reps[rep->vport], rep,
-	       sizeof(struct mlx5_eswitch_rep));
+	rep->load   = __rep->load;
+	rep->unload = __rep->unload;
+	rep->vport  = __rep->vport;
+	rep->priv_data = __rep->priv_data;
+	ether_addr_copy(rep->hw_id, __rep->hw_id);
 
-	INIT_LIST_HEAD(&offloads->vport_reps[rep->vport].vport_sqs_list);
-	offloads->vport_reps[rep->vport].valid = true;
+	INIT_LIST_HEAD(&rep->vport_sqs_list);
+	rep->valid = true;
 }
 
 void mlx5_eswitch_unregister_vport_rep(struct mlx5_eswitch *esw,
-				       int vport)
+				       int vport_index)
 {
 	struct mlx5_esw_offload *offloads = &esw->offloads;
 	struct mlx5_eswitch_rep *rep;
 
-	rep = &offloads->vport_reps[vport];
+	rep = &offloads->vport_reps[vport_index];
 
-	if (esw->mode == SRIOV_OFFLOADS && esw->vports[vport].enabled)
+	if (esw->mode == SRIOV_OFFLOADS && esw->vports[vport_index].enabled)
 		rep->unload(esw, rep);
 
-	offloads->vport_reps[vport].valid = false;
+	rep->valid = false;
 }
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/fs_cmd.c b/drivers/net/ethernet/mellanox/mlx5/core/fs_cmd.c
index 287ade151ec8..113c32326333 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/fs_cmd.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/fs_cmd.c
@@ -41,10 +41,8 @@
 int mlx5_cmd_update_root_ft(struct mlx5_core_dev *dev,
 			    struct mlx5_flow_table *ft)
 {
-	u32 in[MLX5_ST_SZ_DW(set_flow_table_root_in)];
-	u32 out[MLX5_ST_SZ_DW(set_flow_table_root_out)];
-
-	memset(in, 0, sizeof(in));
+	u32 in[MLX5_ST_SZ_DW(set_flow_table_root_in)]   = {0};
+	u32 out[MLX5_ST_SZ_DW(set_flow_table_root_out)] = {0};
 
 	MLX5_SET(set_flow_table_root_in, in, opcode,
 		 MLX5_CMD_OP_SET_FLOW_TABLE_ROOT);
@@ -55,30 +53,23 @@ int mlx5_cmd_update_root_ft(struct mlx5_core_dev *dev,
 		MLX5_SET(set_flow_table_root_in, in, other_vport, 1);
 	}
 
-	memset(out, 0, sizeof(out));
-	return mlx5_cmd_exec_check_status(dev, in, sizeof(in), out,
-					  sizeof(out));
+	return mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
 }
 
 int mlx5_cmd_create_flow_table(struct mlx5_core_dev *dev,
 			       u16 vport,
+			       enum fs_flow_table_op_mod op_mod,
 			       enum fs_flow_table_type type, unsigned int level,
 			       unsigned int log_size, struct mlx5_flow_table
 			       *next_ft, unsigned int *table_id)
 {
-	u32 out[MLX5_ST_SZ_DW(create_flow_table_out)];
-	u32 in[MLX5_ST_SZ_DW(create_flow_table_in)];
+	u32 out[MLX5_ST_SZ_DW(create_flow_table_out)] = {0};
+	u32 in[MLX5_ST_SZ_DW(create_flow_table_in)]   = {0};
 	int err;
 
-	memset(in, 0, sizeof(in));
-
 	MLX5_SET(create_flow_table_in, in, opcode,
 		 MLX5_CMD_OP_CREATE_FLOW_TABLE);
 
-	if (next_ft) {
-		MLX5_SET(create_flow_table_in, in, table_miss_mode, 1);
-		MLX5_SET(create_flow_table_in, in, table_miss_id, next_ft->id);
-	}
 	MLX5_SET(create_flow_table_in, in, table_type, type);
 	MLX5_SET(create_flow_table_in, in, level, level);
 	MLX5_SET(create_flow_table_in, in, log_size, log_size);
@@ -87,10 +78,23 @@ int mlx5_cmd_create_flow_table(struct mlx5_core_dev *dev,
 		MLX5_SET(create_flow_table_in, in, other_vport, 1);
 	}
 
-	memset(out, 0, sizeof(out));
-	err = mlx5_cmd_exec_check_status(dev, in, sizeof(in), out,
-					 sizeof(out));
+	switch (op_mod) {
+	case FS_FT_OP_MOD_NORMAL:
+		if (next_ft) {
+			MLX5_SET(create_flow_table_in, in, table_miss_mode, 1);
+			MLX5_SET(create_flow_table_in, in, table_miss_id, next_ft->id);
+		}
+		break;
+
+	case FS_FT_OP_MOD_LAG_DEMUX:
+		MLX5_SET(create_flow_table_in, in, op_mod, 0x1);
+		if (next_ft)
+			MLX5_SET(create_flow_table_in, in, lag_master_next_table_id,
+				 next_ft->id);
+		break;
+	}
 
+	err = mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
 	if (!err)
 		*table_id = MLX5_GET(create_flow_table_out, out,
 				     table_id);
@@ -100,11 +104,8 @@ int mlx5_cmd_create_flow_table(struct mlx5_core_dev *dev,
 int mlx5_cmd_destroy_flow_table(struct mlx5_core_dev *dev,
 				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));
+	u32 in[MLX5_ST_SZ_DW(destroy_flow_table_in)]   = {0};
+	u32 out[MLX5_ST_SZ_DW(destroy_flow_table_out)] = {0};
 
 	MLX5_SET(destroy_flow_table_in, in, opcode,
 		 MLX5_CMD_OP_DESTROY_FLOW_TABLE);
@@ -115,39 +116,49 @@ int mlx5_cmd_destroy_flow_table(struct mlx5_core_dev *dev,
 		MLX5_SET(destroy_flow_table_in, in, other_vport, 1);
 	}
 
-	return mlx5_cmd_exec_check_status(dev, in, sizeof(in), out,
-					  sizeof(out));
+	return mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
 }
 
 int mlx5_cmd_modify_flow_table(struct mlx5_core_dev *dev,
 			       struct mlx5_flow_table *ft,
 			       struct mlx5_flow_table *next_ft)
 {
-	u32 in[MLX5_ST_SZ_DW(modify_flow_table_in)];
-	u32 out[MLX5_ST_SZ_DW(modify_flow_table_out)];
-
-	memset(in, 0, sizeof(in));
-	memset(out, 0, sizeof(out));
+	u32 in[MLX5_ST_SZ_DW(modify_flow_table_in)]   = {0};
+	u32 out[MLX5_ST_SZ_DW(modify_flow_table_out)] = {0};
 
 	MLX5_SET(modify_flow_table_in, in, opcode,
 		 MLX5_CMD_OP_MODIFY_FLOW_TABLE);
 	MLX5_SET(modify_flow_table_in, in, table_type, ft->type);
 	MLX5_SET(modify_flow_table_in, in, table_id, ft->id);
-	if (ft->vport) {
-		MLX5_SET(modify_flow_table_in, in, vport_number, ft->vport);
-		MLX5_SET(modify_flow_table_in, in, other_vport, 1);
-	}
-	MLX5_SET(modify_flow_table_in, in, modify_field_select,
-		 MLX5_MODIFY_FLOW_TABLE_MISS_TABLE_ID);
-	if (next_ft) {
-		MLX5_SET(modify_flow_table_in, in, table_miss_mode, 1);
-		MLX5_SET(modify_flow_table_in, in, table_miss_id, next_ft->id);
+
+	if (ft->op_mod == FS_FT_OP_MOD_LAG_DEMUX) {
+		MLX5_SET(modify_flow_table_in, in, modify_field_select,
+			 MLX5_MODIFY_FLOW_TABLE_LAG_NEXT_TABLE_ID);
+		if (next_ft) {
+			MLX5_SET(modify_flow_table_in, in,
+				 lag_master_next_table_id, next_ft->id);
+		} else {
+			MLX5_SET(modify_flow_table_in, in,
+				 lag_master_next_table_id, 0);
+		}
 	} else {
-		MLX5_SET(modify_flow_table_in, in, table_miss_mode, 0);
+		if (ft->vport) {
+			MLX5_SET(modify_flow_table_in, in, vport_number,
+				 ft->vport);
+			MLX5_SET(modify_flow_table_in, in, other_vport, 1);
+		}
+		MLX5_SET(modify_flow_table_in, in, modify_field_select,
+			 MLX5_MODIFY_FLOW_TABLE_MISS_TABLE_ID);
+		if (next_ft) {
+			MLX5_SET(modify_flow_table_in, in, table_miss_mode, 1);
+			MLX5_SET(modify_flow_table_in, in, table_miss_id,
+				 next_ft->id);
+		} else {
+			MLX5_SET(modify_flow_table_in, in, table_miss_mode, 0);
+		}
 	}
 
-	return mlx5_cmd_exec_check_status(dev, in, sizeof(in), out,
-					  sizeof(out));
+	return mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
 }
 
 int mlx5_cmd_create_flow_group(struct mlx5_core_dev *dev,
@@ -155,12 +166,10 @@ int mlx5_cmd_create_flow_group(struct mlx5_core_dev *dev,
 			       u32 *in,
 			       unsigned int *group_id)
 {
+	u32 out[MLX5_ST_SZ_DW(create_flow_group_out)] = {0};
 	int inlen = MLX5_ST_SZ_BYTES(create_flow_group_in);
-	u32 out[MLX5_ST_SZ_DW(create_flow_group_out)];
 	int err;
 
-	memset(out, 0, sizeof(out));
-
 	MLX5_SET(create_flow_group_in, in, opcode,
 		 MLX5_CMD_OP_CREATE_FLOW_GROUP);
 	MLX5_SET(create_flow_group_in, in, table_type, ft->type);
@@ -170,13 +179,10 @@ int mlx5_cmd_create_flow_group(struct mlx5_core_dev *dev,
 		MLX5_SET(create_flow_group_in, in, other_vport, 1);
 	}
 
-	err = mlx5_cmd_exec_check_status(dev, in,
-					 inlen, out,
-					 sizeof(out));
+	err = mlx5_cmd_exec(dev, in, inlen, out, sizeof(out));
 	if (!err)
 		*group_id = MLX5_GET(create_flow_group_out, out,
 				     group_id);
-
 	return err;
 }
 
@@ -184,11 +190,8 @@ int mlx5_cmd_destroy_flow_group(struct mlx5_core_dev *dev,
 				struct mlx5_flow_table *ft,
 				unsigned int group_id)
 {
-	u32 out[MLX5_ST_SZ_DW(destroy_flow_group_out)];
-	u32 in[MLX5_ST_SZ_DW(destroy_flow_group_in)];
-
-	memset(in, 0, sizeof(in));
-	memset(out, 0, sizeof(out));
+	u32 out[MLX5_ST_SZ_DW(destroy_flow_group_out)] = {0};
+	u32 in[MLX5_ST_SZ_DW(destroy_flow_group_in)]   = {0};
 
 	MLX5_SET(destroy_flow_group_in, in, opcode,
 		 MLX5_CMD_OP_DESTROY_FLOW_GROUP);
@@ -200,8 +203,7 @@ int mlx5_cmd_destroy_flow_group(struct mlx5_core_dev *dev,
 		MLX5_SET(destroy_flow_group_in, in, other_vport, 1);
 	}
 
-	return mlx5_cmd_exec_check_status(dev, in, sizeof(in), out,
-					  sizeof(out));
+	return mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
 }
 
 static int mlx5_cmd_set_fte(struct mlx5_core_dev *dev,
@@ -212,7 +214,7 @@ static int mlx5_cmd_set_fte(struct mlx5_core_dev *dev,
 {
 	unsigned int inlen = MLX5_ST_SZ_BYTES(set_fte_in) +
 		fte->dests_size * MLX5_ST_SZ_BYTES(dest_format_struct);
-	u32 out[MLX5_ST_SZ_DW(set_fte_out)];
+	u32 out[MLX5_ST_SZ_DW(set_fte_out)] = {0};
 	struct mlx5_flow_rule *dst;
 	void *in_flow_context;
 	void *in_match_value;
@@ -290,11 +292,8 @@ static int mlx5_cmd_set_fte(struct mlx5_core_dev *dev,
 			 list_size);
 	}
 
-	memset(out, 0, sizeof(out));
-	err = mlx5_cmd_exec_check_status(dev, in, inlen, out,
-					 sizeof(out));
+	err = mlx5_cmd_exec(dev, in, inlen, out, sizeof(out));
 	kvfree(in);
-
 	return err;
 }
 
@@ -303,7 +302,7 @@ int mlx5_cmd_create_fte(struct mlx5_core_dev *dev,
 			unsigned group_id,
 			struct fs_fte *fte)
 {
-	return	mlx5_cmd_set_fte(dev, 0, 0, ft, group_id, fte);
+	return mlx5_cmd_set_fte(dev, 0, 0, ft, group_id, fte);
 }
 
 int mlx5_cmd_update_fte(struct mlx5_core_dev *dev,
@@ -327,12 +326,8 @@ int mlx5_cmd_delete_fte(struct mlx5_core_dev *dev,
 			struct mlx5_flow_table *ft,
 			unsigned int index)
 {
-	u32 out[MLX5_ST_SZ_DW(delete_fte_out)];
-	u32 in[MLX5_ST_SZ_DW(delete_fte_in)];
-	int err;
-
-	memset(in, 0, sizeof(in));
-	memset(out, 0, sizeof(out));
+	u32 out[MLX5_ST_SZ_DW(delete_fte_out)] = {0};
+	u32 in[MLX5_ST_SZ_DW(delete_fte_in)]   = {0};
 
 	MLX5_SET(delete_fte_in, in, opcode, MLX5_CMD_OP_DELETE_FLOW_TABLE_ENTRY);
 	MLX5_SET(delete_fte_in, in, table_type, ft->type);
@@ -343,74 +338,55 @@ int mlx5_cmd_delete_fte(struct mlx5_core_dev *dev,
 		MLX5_SET(delete_fte_in, in, other_vport, 1);
 	}
 
-	err =  mlx5_cmd_exec_check_status(dev, in, sizeof(in), out, sizeof(out));
-
-	return err;
+	return mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
 }
 
 int mlx5_cmd_fc_alloc(struct mlx5_core_dev *dev, u16 *id)
 {
-	u32 in[MLX5_ST_SZ_DW(alloc_flow_counter_in)];
-	u32 out[MLX5_ST_SZ_DW(alloc_flow_counter_out)];
+	u32 in[MLX5_ST_SZ_DW(alloc_flow_counter_in)]   = {0};
+	u32 out[MLX5_ST_SZ_DW(alloc_flow_counter_out)] = {0};
 	int err;
 
-	memset(in, 0, sizeof(in));
-	memset(out, 0, sizeof(out));
-
 	MLX5_SET(alloc_flow_counter_in, in, opcode,
 		 MLX5_CMD_OP_ALLOC_FLOW_COUNTER);
 
-	err = mlx5_cmd_exec_check_status(dev, in, sizeof(in), out,
-					 sizeof(out));
-	if (err)
-		return err;
-
-	*id = MLX5_GET(alloc_flow_counter_out, out, flow_counter_id);
-
-	return 0;
+	err = mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
+	if (!err)
+		*id = MLX5_GET(alloc_flow_counter_out, out, flow_counter_id);
+	return err;
 }
 
 int mlx5_cmd_fc_free(struct mlx5_core_dev *dev, u16 id)
 {
-	u32 in[MLX5_ST_SZ_DW(dealloc_flow_counter_in)];
-	u32 out[MLX5_ST_SZ_DW(dealloc_flow_counter_out)];
-
-	memset(in, 0, sizeof(in));
-	memset(out, 0, sizeof(out));
+	u32 in[MLX5_ST_SZ_DW(dealloc_flow_counter_in)]   = {0};
+	u32 out[MLX5_ST_SZ_DW(dealloc_flow_counter_out)] = {0};
 
 	MLX5_SET(dealloc_flow_counter_in, in, opcode,
 		 MLX5_CMD_OP_DEALLOC_FLOW_COUNTER);
 	MLX5_SET(dealloc_flow_counter_in, in, flow_counter_id, id);
-
-	return mlx5_cmd_exec_check_status(dev, in, sizeof(in), out,
-					  sizeof(out));
+	return mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
 }
 
 int mlx5_cmd_fc_query(struct mlx5_core_dev *dev, u16 id,
 		      u64 *packets, u64 *bytes)
 {
 	u32 out[MLX5_ST_SZ_BYTES(query_flow_counter_out) +
-		MLX5_ST_SZ_BYTES(traffic_counter)];
-	u32 in[MLX5_ST_SZ_DW(query_flow_counter_in)];
+		MLX5_ST_SZ_BYTES(traffic_counter)]   = {0};
+	u32 in[MLX5_ST_SZ_DW(query_flow_counter_in)] = {0};
 	void *stats;
 	int err = 0;
 
-	memset(in, 0, sizeof(in));
-	memset(out, 0, sizeof(out));
-
 	MLX5_SET(query_flow_counter_in, in, opcode,
 		 MLX5_CMD_OP_QUERY_FLOW_COUNTER);
 	MLX5_SET(query_flow_counter_in, in, op_mod, 0);
 	MLX5_SET(query_flow_counter_in, in, flow_counter_id, id);
-
-	err = mlx5_cmd_exec_check_status(dev, in, sizeof(in), out, sizeof(out));
+	err = mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
 	if (err)
 		return err;
 
 	stats = MLX5_ADDR_OF(query_flow_counter_out, out, flow_statistics);
 	*packets = MLX5_GET64(traffic_counter, stats, packets);
 	*bytes = MLX5_GET64(traffic_counter, stats, octets);
-
 	return 0;
 }
 
@@ -448,18 +424,14 @@ void mlx5_cmd_fc_bulk_free(struct mlx5_cmd_fc_bulk *b)
 int
 mlx5_cmd_fc_bulk_query(struct mlx5_core_dev *dev, struct mlx5_cmd_fc_bulk *b)
 {
-	u32 in[MLX5_ST_SZ_DW(query_flow_counter_in)];
-
-	memset(in, 0, sizeof(in));
+	u32 in[MLX5_ST_SZ_DW(query_flow_counter_in)] = {0};
 
 	MLX5_SET(query_flow_counter_in, in, opcode,
 		 MLX5_CMD_OP_QUERY_FLOW_COUNTER);
 	MLX5_SET(query_flow_counter_in, in, op_mod, 0);
 	MLX5_SET(query_flow_counter_in, in, flow_counter_id, b->id);
 	MLX5_SET(query_flow_counter_in, in, num_of_counters, b->num);
-
-	return mlx5_cmd_exec_check_status(dev, in, sizeof(in),
-					  b->out, b->outlen);
+	return mlx5_cmd_exec(dev, in, sizeof(in), b->out, b->outlen);
 }
 
 void mlx5_cmd_fc_bulk_get(struct mlx5_core_dev *dev,
@@ -480,3 +452,51 @@ void mlx5_cmd_fc_bulk_get(struct mlx5_core_dev *dev,
 	*packets = MLX5_GET64(traffic_counter, stats, packets);
 	*bytes = MLX5_GET64(traffic_counter, stats, octets);
 }
+
+#define MAX_ENCAP_SIZE (128)
+
+int mlx5_cmd_alloc_encap(struct mlx5_core_dev *dev,
+			 int header_type,
+			 size_t size,
+			 void *encap_header,
+			 u32 *encap_id)
+{
+	u32 out[MLX5_ST_SZ_DW(alloc_encap_header_out)];
+	u32 in[MLX5_ST_SZ_DW(alloc_encap_header_in) +
+	      (MAX_ENCAP_SIZE / sizeof(u32))];
+	void *encap_header_in = MLX5_ADDR_OF(alloc_encap_header_in, in,
+					     encap_header);
+	void *header = MLX5_ADDR_OF(encap_header_in, encap_header_in,
+				    encap_header);
+	int inlen = header - (void *)in  + size;
+	int err;
+
+	if (size > MAX_ENCAP_SIZE)
+		return -EINVAL;
+
+	memset(in, 0, inlen);
+	MLX5_SET(alloc_encap_header_in, in, opcode,
+		 MLX5_CMD_OP_ALLOC_ENCAP_HEADER);
+	MLX5_SET(encap_header_in, encap_header_in, encap_header_size, size);
+	MLX5_SET(encap_header_in, encap_header_in, header_type, header_type);
+	memcpy(header, encap_header, size);
+
+	memset(out, 0, sizeof(out));
+	err = mlx5_cmd_exec(dev, in, inlen, out, sizeof(out));
+
+	*encap_id = MLX5_GET(alloc_encap_header_out, out, encap_id);
+	return err;
+}
+
+void mlx5_cmd_dealloc_encap(struct mlx5_core_dev *dev, u32 encap_id)
+{
+	u32 in[MLX5_ST_SZ_DW(dealloc_encap_header_in)];
+	u32 out[MLX5_ST_SZ_DW(dealloc_encap_header_out)];
+
+	memset(in, 0, sizeof(in));
+	MLX5_SET(dealloc_encap_header_in, in, opcode,
+		 MLX5_CMD_OP_DEALLOC_ENCAP_HEADER);
+	MLX5_SET(dealloc_encap_header_in, in, encap_id, encap_id);
+
+	mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
+}
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/fs_cmd.h b/drivers/net/ethernet/mellanox/mlx5/core/fs_cmd.h
index 158844cef82b..c5bc4686c832 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/fs_cmd.h
+++ b/drivers/net/ethernet/mellanox/mlx5/core/fs_cmd.h
@@ -35,6 +35,7 @@
 
 int mlx5_cmd_create_flow_table(struct mlx5_core_dev *dev,
 			       u16 vport,
+			       enum fs_flow_table_op_mod op_mod,
 			       enum fs_flow_table_type type, unsigned int level,
 			       unsigned int log_size, struct mlx5_flow_table
 			       *next_ft, unsigned int *table_id);
@@ -88,4 +89,11 @@ void mlx5_cmd_fc_bulk_get(struct mlx5_core_dev *dev,
 			  struct mlx5_cmd_fc_bulk *b, u16 id,
 			  u64 *packets, u64 *bytes);
 
+int mlx5_cmd_alloc_encap(struct mlx5_core_dev *dev,
+			 int header_type,
+			 size_t size,
+			 void *encap_header,
+			 u32 *encap_id);
+void mlx5_cmd_dealloc_encap(struct mlx5_core_dev *dev, u32 encap_id);
+
 #endif
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/fs_core.c b/drivers/net/ethernet/mellanox/mlx5/core/fs_core.c
index 3d6c1f65e586..5da2cc878582 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/fs_core.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/fs_core.c
@@ -96,6 +96,10 @@
 #define OFFLOADS_NUM_PRIOS 1
 #define OFFLOADS_MIN_LEVEL (ANCHOR_MIN_LEVEL + 1)
 
+#define LAG_PRIO_NUM_LEVELS 1
+#define LAG_NUM_PRIOS 1
+#define LAG_MIN_LEVEL (OFFLOADS_MIN_LEVEL + 1)
+
 struct node_caps {
 	size_t	arr_sz;
 	long	*caps;
@@ -111,12 +115,16 @@ static struct init_tree_node {
 	int num_levels;
 } root_fs = {
 	.type = FS_TYPE_NAMESPACE,
-	.ar_size = 6,
+	.ar_size = 7,
 	.children = (struct init_tree_node[]) {
 		ADD_PRIO(0, BY_PASS_MIN_LEVEL, 0,
 			 FS_CHAINING_CAPS,
 			 ADD_NS(ADD_MULTIPLE_PRIO(MLX5_BY_PASS_NUM_PRIOS,
 						  BY_PASS_PRIO_NUM_LEVELS))),
+		ADD_PRIO(0, LAG_MIN_LEVEL, 0,
+			 FS_CHAINING_CAPS,
+			 ADD_NS(ADD_MULTIPLE_PRIO(LAG_NUM_PRIOS,
+						  LAG_PRIO_NUM_LEVELS))),
 		ADD_PRIO(0, OFFLOADS_MIN_LEVEL, 0, {},
 			 ADD_NS(ADD_MULTIPLE_PRIO(OFFLOADS_NUM_PRIOS, OFFLOADS_MAX_FT))),
 		ADD_PRIO(0, ETHTOOL_MIN_LEVEL, 0,
@@ -345,7 +353,7 @@ static void del_flow_table(struct fs_node *node)
 
 	err = mlx5_cmd_destroy_flow_table(dev, ft);
 	if (err)
-		pr_warn("flow steering can't destroy ft\n");
+		mlx5_core_warn(dev, "flow steering can't destroy ft\n");
 	fs_get_obj(prio, ft->node.parent);
 	prio->num_ft--;
 }
@@ -364,7 +372,7 @@ static void del_rule(struct fs_node *node)
 
 	match_value = mlx5_vzalloc(match_len);
 	if (!match_value) {
-		pr_warn("failed to allocate inbox\n");
+		mlx5_core_warn(dev, "failed to allocate inbox\n");
 		return;
 	}
 
@@ -387,8 +395,9 @@ static void del_rule(struct fs_node *node)
 					  modify_mask,
 					  fte);
 		if (err)
-			pr_warn("%s can't del rule fg id=%d fte_index=%d\n",
-				__func__, fg->id, fte->index);
+			mlx5_core_warn(dev,
+				       "%s can't del rule fg id=%d fte_index=%d\n",
+				       __func__, fg->id, fte->index);
 	}
 	kvfree(match_value);
 }
@@ -409,8 +418,9 @@ static void del_fte(struct fs_node *node)
 	err = mlx5_cmd_delete_fte(dev, ft,
 				  fte->index);
 	if (err)
-		pr_warn("flow steering can't delete fte in index %d of flow group id %d\n",
-			fte->index, fg->id);
+		mlx5_core_warn(dev,
+			       "flow steering can't delete fte in index %d of flow group id %d\n",
+			       fte->index, fg->id);
 
 	fte->status = 0;
 	fg->num_ftes--;
@@ -427,8 +437,8 @@ static void del_flow_group(struct fs_node *node)
 	dev = get_dev(&ft->node);
 
 	if (mlx5_cmd_destroy_flow_group(dev, ft, fg->id))
-		pr_warn("flow steering can't destroy fg %d of ft %d\n",
-			fg->id, ft->id);
+		mlx5_core_warn(dev, "flow steering can't destroy fg %d of ft %d\n",
+			       fg->id, ft->id);
 }
 
 static struct fs_fte *alloc_fte(u8 action,
@@ -475,7 +485,8 @@ static struct mlx5_flow_group *alloc_flow_group(u32 *create_fg_in)
 }
 
 static struct mlx5_flow_table *alloc_flow_table(int level, u16 vport, int max_fte,
-						enum fs_flow_table_type table_type)
+						enum fs_flow_table_type table_type,
+						enum fs_flow_table_op_mod op_mod)
 {
 	struct mlx5_flow_table *ft;
 
@@ -485,6 +496,7 @@ static struct mlx5_flow_table *alloc_flow_table(int level, u16 vport, int max_ft
 
 	ft->level = level;
 	ft->node.type = FS_TYPE_FLOW_TABLE;
+	ft->op_mod = op_mod;
 	ft->type = table_type;
 	ft->vport = vport;
 	ft->max_fte = max_fte;
@@ -722,6 +734,7 @@ static void list_add_flow_table(struct mlx5_flow_table *ft,
 }
 
 static struct mlx5_flow_table *__mlx5_create_flow_table(struct mlx5_flow_namespace *ns,
+							enum fs_flow_table_op_mod op_mod,
 							u16 vport, int prio,
 							int max_fte, u32 level)
 {
@@ -754,18 +767,19 @@ static struct mlx5_flow_table *__mlx5_create_flow_table(struct mlx5_flow_namespa
 	level += fs_prio->start_level;
 	ft = alloc_flow_table(level,
 			      vport,
-			      roundup_pow_of_two(max_fte),
-			      root->table_type);
+			      max_fte ? roundup_pow_of_two(max_fte) : 0,
+			      root->table_type,
+			      op_mod);
 	if (!ft) {
 		err = -ENOMEM;
 		goto unlock_root;
 	}
 
 	tree_init_node(&ft->node, 1, del_flow_table);
-	log_table_sz = ilog2(ft->max_fte);
+	log_table_sz = ft->max_fte ? ilog2(ft->max_fte) : 0;
 	next_ft = find_next_chained_ft(fs_prio);
-	err = mlx5_cmd_create_flow_table(root->dev, ft->vport, ft->type, ft->level,
-					 log_table_sz, next_ft, &ft->id);
+	err = mlx5_cmd_create_flow_table(root->dev, ft->vport, ft->op_mod, ft->type,
+					 ft->level, log_table_sz, next_ft, &ft->id);
 	if (err)
 		goto free_ft;
 
@@ -792,15 +806,26 @@ struct mlx5_flow_table *mlx5_create_flow_table(struct mlx5_flow_namespace *ns,
 					       int prio, int max_fte,
 					       u32 level)
 {
-	return __mlx5_create_flow_table(ns, 0, prio, max_fte, level);
+	return __mlx5_create_flow_table(ns, FS_FT_OP_MOD_NORMAL, 0, prio,
+					max_fte, level);
 }
 
 struct mlx5_flow_table *mlx5_create_vport_flow_table(struct mlx5_flow_namespace *ns,
 						     int prio, int max_fte,
 						     u32 level, u16 vport)
 {
-	return __mlx5_create_flow_table(ns, vport, prio, max_fte, level);
+	return __mlx5_create_flow_table(ns, FS_FT_OP_MOD_NORMAL, vport, prio,
+					max_fte, level);
+}
+
+struct mlx5_flow_table *mlx5_create_lag_demux_flow_table(
+					       struct mlx5_flow_namespace *ns,
+					       int prio, u32 level)
+{
+	return __mlx5_create_flow_table(ns, FS_FT_OP_MOD_LAG_DEMUX, 0, prio, 0,
+					level);
 }
+EXPORT_SYMBOL(mlx5_create_lag_demux_flow_table);
 
 struct mlx5_flow_table *mlx5_create_auto_grouped_flow_table(struct mlx5_flow_namespace *ns,
 							    int prio,
@@ -1379,6 +1404,7 @@ struct mlx5_flow_namespace *mlx5_get_flow_namespace(struct mlx5_core_dev *dev,
 
 	switch (type) {
 	case MLX5_FLOW_NAMESPACE_BYPASS:
+	case MLX5_FLOW_NAMESPACE_LAG:
 	case MLX5_FLOW_NAMESPACE_OFFLOADS:
 	case MLX5_FLOW_NAMESPACE_ETHTOOL:
 	case MLX5_FLOW_NAMESPACE_KERNEL:
@@ -1401,6 +1427,16 @@ struct mlx5_flow_namespace *mlx5_get_flow_namespace(struct mlx5_core_dev *dev,
 			return &steering->esw_ingress_root_ns->ns;
 		else
 			return NULL;
+	case MLX5_FLOW_NAMESPACE_SNIFFER_RX:
+		if (steering->sniffer_rx_root_ns)
+			return &steering->sniffer_rx_root_ns->ns;
+		else
+			return NULL;
+	case MLX5_FLOW_NAMESPACE_SNIFFER_TX:
+		if (steering->sniffer_tx_root_ns)
+			return &steering->sniffer_tx_root_ns->ns;
+		else
+			return NULL;
 	default:
 		return NULL;
 	}
@@ -1700,10 +1736,46 @@ void mlx5_cleanup_fs(struct mlx5_core_dev *dev)
 	cleanup_root_ns(steering->esw_egress_root_ns);
 	cleanup_root_ns(steering->esw_ingress_root_ns);
 	cleanup_root_ns(steering->fdb_root_ns);
+	cleanup_root_ns(steering->sniffer_rx_root_ns);
+	cleanup_root_ns(steering->sniffer_tx_root_ns);
 	mlx5_cleanup_fc_stats(dev);
 	kfree(steering);
 }
 
+static int init_sniffer_tx_root_ns(struct mlx5_flow_steering *steering)
+{
+	struct fs_prio *prio;
+
+	steering->sniffer_tx_root_ns = create_root_ns(steering, FS_FT_SNIFFER_TX);
+	if (!steering->sniffer_tx_root_ns)
+		return -ENOMEM;
+
+	/* Create single prio */
+	prio = fs_create_prio(&steering->sniffer_tx_root_ns->ns, 0, 1);
+	if (IS_ERR(prio)) {
+		cleanup_root_ns(steering->sniffer_tx_root_ns);
+		return PTR_ERR(prio);
+	}
+	return 0;
+}
+
+static int init_sniffer_rx_root_ns(struct mlx5_flow_steering *steering)
+{
+	struct fs_prio *prio;
+
+	steering->sniffer_rx_root_ns = create_root_ns(steering, FS_FT_SNIFFER_RX);
+	if (!steering->sniffer_rx_root_ns)
+		return -ENOMEM;
+
+	/* Create single prio */
+	prio = fs_create_prio(&steering->sniffer_rx_root_ns->ns, 0, 1);
+	if (IS_ERR(prio)) {
+		cleanup_root_ns(steering->sniffer_rx_root_ns);
+		return PTR_ERR(prio);
+	}
+	return 0;
+}
+
 static int init_fdb_root_ns(struct mlx5_flow_steering *steering)
 {
 	struct fs_prio *prio;
@@ -1800,6 +1872,18 @@ int mlx5_init_fs(struct mlx5_core_dev *dev)
 		}
 	}
 
+	if (MLX5_CAP_FLOWTABLE_SNIFFER_RX(dev, ft_support)) {
+		err = init_sniffer_rx_root_ns(steering);
+		if (err)
+			goto err;
+	}
+
+	if (MLX5_CAP_FLOWTABLE_SNIFFER_TX(dev, ft_support)) {
+		err = init_sniffer_tx_root_ns(steering);
+		if (err)
+			goto err;
+	}
+
 	return 0;
 err:
 	mlx5_cleanup_fs(dev);
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/fs_core.h b/drivers/net/ethernet/mellanox/mlx5/core/fs_core.h
index 9cffb6aeb4e9..71ff03bceabb 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/fs_core.h
+++ b/drivers/net/ethernet/mellanox/mlx5/core/fs_core.h
@@ -49,6 +49,13 @@ enum fs_flow_table_type {
 	FS_FT_ESW_EGRESS_ACL  = 0x2,
 	FS_FT_ESW_INGRESS_ACL = 0x3,
 	FS_FT_FDB             = 0X4,
+	FS_FT_SNIFFER_RX	= 0X5,
+	FS_FT_SNIFFER_TX	= 0X6,
+};
+
+enum fs_flow_table_op_mod {
+	FS_FT_OP_MOD_NORMAL,
+	FS_FT_OP_MOD_LAG_DEMUX,
 };
 
 enum fs_fte_status {
@@ -61,6 +68,8 @@ struct mlx5_flow_steering {
 	struct mlx5_flow_root_namespace *fdb_root_ns;
 	struct mlx5_flow_root_namespace *esw_egress_root_ns;
 	struct mlx5_flow_root_namespace *esw_ingress_root_ns;
+	struct mlx5_flow_root_namespace	*sniffer_tx_root_ns;
+	struct mlx5_flow_root_namespace	*sniffer_rx_root_ns;
 };
 
 struct fs_node {
@@ -93,6 +102,7 @@ struct mlx5_flow_table {
 	unsigned int			max_fte;
 	unsigned int			level;
 	enum fs_flow_table_type		type;
+	enum fs_flow_table_op_mod	op_mod;
 	struct {
 		bool			active;
 		unsigned int		required_groups;
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/fw.c b/drivers/net/ethernet/mellanox/mlx5/core/fw.c
index 77fc1aa26114..5718aada6605 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/fw.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/fw.c
@@ -38,13 +38,10 @@
 static int mlx5_cmd_query_adapter(struct mlx5_core_dev *dev, u32 *out,
 				  int outlen)
 {
-	u32 in[MLX5_ST_SZ_DW(query_adapter_in)];
-
-	memset(in, 0, sizeof(in));
+	u32 in[MLX5_ST_SZ_DW(query_adapter_in)] = {0};
 
 	MLX5_SET(query_adapter_in, in, opcode, MLX5_CMD_OP_QUERY_ADAPTER);
-
-	return mlx5_cmd_exec_check_status(dev, in, sizeof(in), out, outlen);
+	return mlx5_cmd_exec(dev, in, sizeof(in), out, outlen);
 }
 
 int mlx5_query_board_id(struct mlx5_core_dev *dev)
@@ -162,38 +159,18 @@ int mlx5_query_hca_caps(struct mlx5_core_dev *dev)
 
 int mlx5_cmd_init_hca(struct mlx5_core_dev *dev)
 {
-	struct mlx5_cmd_init_hca_mbox_in in;
-	struct mlx5_cmd_init_hca_mbox_out out;
-	int err;
-
-	memset(&in, 0, sizeof(in));
-	memset(&out, 0, sizeof(out));
-	in.hdr.opcode = cpu_to_be16(MLX5_CMD_OP_INIT_HCA);
-	err = mlx5_cmd_exec(dev, &in, sizeof(in), &out, sizeof(out));
-	if (err)
-		return err;
-
-	if (out.hdr.status)
-		err = mlx5_cmd_status_to_err(&out.hdr);
+	u32 out[MLX5_ST_SZ_DW(init_hca_out)] = {0};
+	u32 in[MLX5_ST_SZ_DW(init_hca_in)]   = {0};
 
-	return err;
+	MLX5_SET(init_hca_in, in, opcode, MLX5_CMD_OP_INIT_HCA);
+	return mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
 }
 
 int mlx5_cmd_teardown_hca(struct mlx5_core_dev *dev)
 {
-	struct mlx5_cmd_teardown_hca_mbox_in in;
-	struct mlx5_cmd_teardown_hca_mbox_out out;
-	int err;
+	u32 out[MLX5_ST_SZ_DW(teardown_hca_out)] = {0};
+	u32 in[MLX5_ST_SZ_DW(teardown_hca_in)]   = {0};
 
-	memset(&in, 0, sizeof(in));
-	memset(&out, 0, sizeof(out));
-	in.hdr.opcode = cpu_to_be16(MLX5_CMD_OP_TEARDOWN_HCA);
-	err = mlx5_cmd_exec(dev, &in, sizeof(in), &out, sizeof(out));
-	if (err)
-		return err;
-
-	if (out.hdr.status)
-		err = mlx5_cmd_status_to_err(&out.hdr);
-
-	return err;
+	MLX5_SET(teardown_hca_in, in, opcode, MLX5_CMD_OP_TEARDOWN_HCA);
+	return mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
 }
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/lag.c b/drivers/net/ethernet/mellanox/mlx5/core/lag.c
new file mode 100644
index 000000000000..55957246c0e8
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/lag.c
@@ -0,0 +1,588 @@
+/*
+ * Copyright (c) 2016, 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/netdevice.h>
+#include <linux/mlx5/driver.h>
+#include <linux/mlx5/vport.h>
+#include "mlx5_core.h"
+
+enum {
+	MLX5_LAG_FLAG_BONDED = 1 << 0,
+};
+
+struct lag_func {
+	struct mlx5_core_dev *dev;
+	struct net_device    *netdev;
+};
+
+/* Used for collection of netdev event info. */
+struct lag_tracker {
+	enum   netdev_lag_tx_type           tx_type;
+	struct netdev_lag_lower_state_info  netdev_state[MLX5_MAX_PORTS];
+	bool is_bonded;
+};
+
+/* LAG data of a ConnectX card.
+ * It serves both its phys functions.
+ */
+struct mlx5_lag {
+	u8                        flags;
+	u8                        v2p_map[MLX5_MAX_PORTS];
+	struct lag_func           pf[MLX5_MAX_PORTS];
+	struct lag_tracker        tracker;
+	struct delayed_work       bond_work;
+	struct notifier_block     nb;
+};
+
+/* General purpose, use for short periods of time.
+ * Beware of lock dependencies (preferably, no locks should be acquired
+ * under it).
+ */
+static DEFINE_MUTEX(lag_mutex);
+
+static int mlx5_cmd_create_lag(struct mlx5_core_dev *dev, u8 remap_port1,
+			       u8 remap_port2)
+{
+	u32   in[MLX5_ST_SZ_DW(create_lag_in)]   = {0};
+	u32   out[MLX5_ST_SZ_DW(create_lag_out)] = {0};
+	void *lag_ctx = MLX5_ADDR_OF(create_lag_in, in, ctx);
+
+	MLX5_SET(create_lag_in, in, opcode, MLX5_CMD_OP_CREATE_LAG);
+
+	MLX5_SET(lagc, lag_ctx, tx_remap_affinity_1, remap_port1);
+	MLX5_SET(lagc, lag_ctx, tx_remap_affinity_2, remap_port2);
+
+	return mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
+}
+
+static int mlx5_cmd_modify_lag(struct mlx5_core_dev *dev, u8 remap_port1,
+			       u8 remap_port2)
+{
+	u32   in[MLX5_ST_SZ_DW(modify_lag_in)]   = {0};
+	u32   out[MLX5_ST_SZ_DW(modify_lag_out)] = {0};
+	void *lag_ctx = MLX5_ADDR_OF(modify_lag_in, in, ctx);
+
+	MLX5_SET(modify_lag_in, in, opcode, MLX5_CMD_OP_MODIFY_LAG);
+	MLX5_SET(modify_lag_in, in, field_select, 0x1);
+
+	MLX5_SET(lagc, lag_ctx, tx_remap_affinity_1, remap_port1);
+	MLX5_SET(lagc, lag_ctx, tx_remap_affinity_2, remap_port2);
+
+	return mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
+}
+
+static int mlx5_cmd_destroy_lag(struct mlx5_core_dev *dev)
+{
+	u32  in[MLX5_ST_SZ_DW(destroy_lag_in)]  = {0};
+	u32 out[MLX5_ST_SZ_DW(destroy_lag_out)] = {0};
+
+	MLX5_SET(destroy_lag_in, in, opcode, MLX5_CMD_OP_DESTROY_LAG);
+
+	return mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
+}
+
+int mlx5_cmd_create_vport_lag(struct mlx5_core_dev *dev)
+{
+	u32  in[MLX5_ST_SZ_DW(create_vport_lag_in)]  = {0};
+	u32 out[MLX5_ST_SZ_DW(create_vport_lag_out)] = {0};
+
+	MLX5_SET(create_vport_lag_in, in, opcode, MLX5_CMD_OP_CREATE_VPORT_LAG);
+
+	return mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
+}
+EXPORT_SYMBOL(mlx5_cmd_create_vport_lag);
+
+int mlx5_cmd_destroy_vport_lag(struct mlx5_core_dev *dev)
+{
+	u32  in[MLX5_ST_SZ_DW(destroy_vport_lag_in)]  = {0};
+	u32 out[MLX5_ST_SZ_DW(destroy_vport_lag_out)] = {0};
+
+	MLX5_SET(destroy_vport_lag_in, in, opcode, MLX5_CMD_OP_DESTROY_VPORT_LAG);
+
+	return mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
+}
+EXPORT_SYMBOL(mlx5_cmd_destroy_vport_lag);
+
+static struct mlx5_lag *mlx5_lag_dev_get(struct mlx5_core_dev *dev)
+{
+	return dev->priv.lag;
+}
+
+static int mlx5_lag_dev_get_netdev_idx(struct mlx5_lag *ldev,
+				       struct net_device *ndev)
+{
+	int i;
+
+	for (i = 0; i < MLX5_MAX_PORTS; i++)
+		if (ldev->pf[i].netdev == ndev)
+			return i;
+
+	return -1;
+}
+
+static bool mlx5_lag_is_bonded(struct mlx5_lag *ldev)
+{
+	return !!(ldev->flags & MLX5_LAG_FLAG_BONDED);
+}
+
+static void mlx5_infer_tx_affinity_mapping(struct lag_tracker *tracker,
+					   u8 *port1, u8 *port2)
+{
+	if (tracker->tx_type == NETDEV_LAG_TX_TYPE_ACTIVEBACKUP) {
+		if (tracker->netdev_state[0].tx_enabled) {
+			*port1 = 1;
+			*port2 = 1;
+		} else {
+			*port1 = 2;
+			*port2 = 2;
+		}
+	} else {
+		*port1 = 1;
+		*port2 = 2;
+		if (!tracker->netdev_state[0].link_up)
+			*port1 = 2;
+		else if (!tracker->netdev_state[1].link_up)
+			*port2 = 1;
+	}
+}
+
+static void mlx5_activate_lag(struct mlx5_lag *ldev,
+			      struct lag_tracker *tracker)
+{
+	struct mlx5_core_dev *dev0 = ldev->pf[0].dev;
+	int err;
+
+	ldev->flags |= MLX5_LAG_FLAG_BONDED;
+
+	mlx5_infer_tx_affinity_mapping(tracker, &ldev->v2p_map[0],
+				       &ldev->v2p_map[1]);
+
+	err = mlx5_cmd_create_lag(dev0, ldev->v2p_map[0], ldev->v2p_map[1]);
+	if (err)
+		mlx5_core_err(dev0,
+			      "Failed to create LAG (%d)\n",
+			      err);
+}
+
+static void mlx5_deactivate_lag(struct mlx5_lag *ldev)
+{
+	struct mlx5_core_dev *dev0 = ldev->pf[0].dev;
+	int err;
+
+	ldev->flags &= ~MLX5_LAG_FLAG_BONDED;
+
+	err = mlx5_cmd_destroy_lag(dev0);
+	if (err)
+		mlx5_core_err(dev0,
+			      "Failed to destroy LAG (%d)\n",
+			      err);
+}
+
+static void mlx5_do_bond(struct mlx5_lag *ldev)
+{
+	struct mlx5_core_dev *dev0 = ldev->pf[0].dev;
+	struct mlx5_core_dev *dev1 = ldev->pf[1].dev;
+	struct lag_tracker tracker;
+	u8 v2p_port1, v2p_port2;
+	int i, err;
+
+	if (!dev0 || !dev1)
+		return;
+
+	mutex_lock(&lag_mutex);
+	tracker = ldev->tracker;
+	mutex_unlock(&lag_mutex);
+
+	if (tracker.is_bonded && !mlx5_lag_is_bonded(ldev)) {
+		if (mlx5_sriov_is_enabled(dev0) ||
+		    mlx5_sriov_is_enabled(dev1)) {
+			mlx5_core_warn(dev0, "LAG is not supported with SRIOV");
+			return;
+		}
+
+		for (i = 0; i < MLX5_MAX_PORTS; i++)
+			mlx5_remove_dev_by_protocol(ldev->pf[i].dev,
+						    MLX5_INTERFACE_PROTOCOL_IB);
+
+		mlx5_activate_lag(ldev, &tracker);
+
+		mlx5_add_dev_by_protocol(dev0, MLX5_INTERFACE_PROTOCOL_IB);
+		mlx5_nic_vport_enable_roce(dev1);
+	} else if (tracker.is_bonded && mlx5_lag_is_bonded(ldev)) {
+		mlx5_infer_tx_affinity_mapping(&tracker, &v2p_port1,
+					       &v2p_port2);
+
+		if ((v2p_port1 != ldev->v2p_map[0]) ||
+		    (v2p_port2 != ldev->v2p_map[1])) {
+			ldev->v2p_map[0] = v2p_port1;
+			ldev->v2p_map[1] = v2p_port2;
+
+			err = mlx5_cmd_modify_lag(dev0, v2p_port1, v2p_port2);
+			if (err)
+				mlx5_core_err(dev0,
+					      "Failed to modify LAG (%d)\n",
+					      err);
+		}
+	} else if (!tracker.is_bonded && mlx5_lag_is_bonded(ldev)) {
+		mlx5_remove_dev_by_protocol(dev0, MLX5_INTERFACE_PROTOCOL_IB);
+		mlx5_nic_vport_disable_roce(dev1);
+
+		mlx5_deactivate_lag(ldev);
+
+		for (i = 0; i < MLX5_MAX_PORTS; i++)
+			if (ldev->pf[i].dev)
+				mlx5_add_dev_by_protocol(ldev->pf[i].dev,
+							 MLX5_INTERFACE_PROTOCOL_IB);
+	}
+}
+
+static void mlx5_queue_bond_work(struct mlx5_lag *ldev, unsigned long delay)
+{
+	schedule_delayed_work(&ldev->bond_work, delay);
+}
+
+static void mlx5_do_bond_work(struct work_struct *work)
+{
+	struct delayed_work *delayed_work = to_delayed_work(work);
+	struct mlx5_lag *ldev = container_of(delayed_work, struct mlx5_lag,
+					     bond_work);
+	int status;
+
+	status = mlx5_dev_list_trylock();
+	if (!status) {
+		/* 1 sec delay. */
+		mlx5_queue_bond_work(ldev, HZ);
+		return;
+	}
+
+	mlx5_do_bond(ldev);
+	mlx5_dev_list_unlock();
+}
+
+static int mlx5_handle_changeupper_event(struct mlx5_lag *ldev,
+					 struct lag_tracker *tracker,
+					 struct net_device *ndev,
+					 struct netdev_notifier_changeupper_info *info)
+{
+	struct net_device *upper = info->upper_dev, *ndev_tmp;
+	struct netdev_lag_upper_info *lag_upper_info;
+	bool is_bonded;
+	int bond_status = 0;
+	int num_slaves = 0;
+	int idx;
+
+	if (!netif_is_lag_master(upper))
+		return 0;
+
+	lag_upper_info = info->upper_info;
+
+	/* The event may still be of interest if the slave does not belong to
+	 * us, but is enslaved to a master which has one or more of our netdevs
+	 * as slaves (e.g., if a new slave is added to a master that bonds two
+	 * of our netdevs, we should unbond).
+	 */
+	rcu_read_lock();
+	for_each_netdev_in_bond_rcu(upper, ndev_tmp) {
+		idx = mlx5_lag_dev_get_netdev_idx(ldev, ndev_tmp);
+		if (idx > -1)
+			bond_status |= (1 << idx);
+
+		num_slaves++;
+	}
+	rcu_read_unlock();
+
+	/* None of this lagdev's netdevs are slaves of this master. */
+	if (!(bond_status & 0x3))
+		return 0;
+
+	if (lag_upper_info)
+		tracker->tx_type = lag_upper_info->tx_type;
+
+	/* Determine bonding status:
+	 * A device is considered bonded if both its physical ports are slaves
+	 * of the same lag master, and only them.
+	 * Lag mode must be activebackup or hash.
+	 */
+	is_bonded = (num_slaves == MLX5_MAX_PORTS) &&
+		    (bond_status == 0x3) &&
+		    ((tracker->tx_type == NETDEV_LAG_TX_TYPE_ACTIVEBACKUP) ||
+		     (tracker->tx_type == NETDEV_LAG_TX_TYPE_HASH));
+
+	if (tracker->is_bonded != is_bonded) {
+		tracker->is_bonded = is_bonded;
+		return 1;
+	}
+
+	return 0;
+}
+
+static int mlx5_handle_changelowerstate_event(struct mlx5_lag *ldev,
+					      struct lag_tracker *tracker,
+					      struct net_device *ndev,
+					      struct netdev_notifier_changelowerstate_info *info)
+{
+	struct netdev_lag_lower_state_info *lag_lower_info;
+	int idx;
+
+	if (!netif_is_lag_port(ndev))
+		return 0;
+
+	idx = mlx5_lag_dev_get_netdev_idx(ldev, ndev);
+	if (idx == -1)
+		return 0;
+
+	/* This information is used to determine virtual to physical
+	 * port mapping.
+	 */
+	lag_lower_info = info->lower_state_info;
+	if (!lag_lower_info)
+		return 0;
+
+	tracker->netdev_state[idx] = *lag_lower_info;
+
+	return 1;
+}
+
+static int mlx5_lag_netdev_event(struct notifier_block *this,
+				 unsigned long event, void *ptr)
+{
+	struct net_device *ndev = netdev_notifier_info_to_dev(ptr);
+	struct lag_tracker tracker;
+	struct mlx5_lag *ldev;
+	int changed = 0;
+
+	if (!net_eq(dev_net(ndev), &init_net))
+		return NOTIFY_DONE;
+
+	if ((event != NETDEV_CHANGEUPPER) && (event != NETDEV_CHANGELOWERSTATE))
+		return NOTIFY_DONE;
+
+	ldev    = container_of(this, struct mlx5_lag, nb);
+	tracker = ldev->tracker;
+
+	switch (event) {
+	case NETDEV_CHANGEUPPER:
+		changed = mlx5_handle_changeupper_event(ldev, &tracker, ndev,
+							ptr);
+		break;
+	case NETDEV_CHANGELOWERSTATE:
+		changed = mlx5_handle_changelowerstate_event(ldev, &tracker,
+							     ndev, ptr);
+		break;
+	}
+
+	mutex_lock(&lag_mutex);
+	ldev->tracker = tracker;
+	mutex_unlock(&lag_mutex);
+
+	if (changed)
+		mlx5_queue_bond_work(ldev, 0);
+
+	return NOTIFY_DONE;
+}
+
+static struct mlx5_lag *mlx5_lag_dev_alloc(void)
+{
+	struct mlx5_lag *ldev;
+
+	ldev = kzalloc(sizeof(*ldev), GFP_KERNEL);
+	if (!ldev)
+		return NULL;
+
+	INIT_DELAYED_WORK(&ldev->bond_work, mlx5_do_bond_work);
+
+	return ldev;
+}
+
+static void mlx5_lag_dev_free(struct mlx5_lag *ldev)
+{
+	kfree(ldev);
+}
+
+static void mlx5_lag_dev_add_pf(struct mlx5_lag *ldev,
+				struct mlx5_core_dev *dev,
+				struct net_device *netdev)
+{
+	unsigned int fn = PCI_FUNC(dev->pdev->devfn);
+
+	if (fn >= MLX5_MAX_PORTS)
+		return;
+
+	mutex_lock(&lag_mutex);
+	ldev->pf[fn].dev    = dev;
+	ldev->pf[fn].netdev = netdev;
+	ldev->tracker.netdev_state[fn].link_up = 0;
+	ldev->tracker.netdev_state[fn].tx_enabled = 0;
+
+	dev->priv.lag = ldev;
+	mutex_unlock(&lag_mutex);
+}
+
+static void mlx5_lag_dev_remove_pf(struct mlx5_lag *ldev,
+				   struct mlx5_core_dev *dev)
+{
+	int i;
+
+	for (i = 0; i < MLX5_MAX_PORTS; i++)
+		if (ldev->pf[i].dev == dev)
+			break;
+
+	if (i == MLX5_MAX_PORTS)
+		return;
+
+	mutex_lock(&lag_mutex);
+	memset(&ldev->pf[i], 0, sizeof(*ldev->pf));
+
+	dev->priv.lag = NULL;
+	mutex_unlock(&lag_mutex);
+}
+
+
+/* Must be called with intf_mutex held */
+void mlx5_lag_add(struct mlx5_core_dev *dev, struct net_device *netdev)
+{
+	struct mlx5_lag *ldev = NULL;
+	struct mlx5_core_dev *tmp_dev;
+
+	if (!MLX5_CAP_GEN(dev, vport_group_manager) ||
+	    !MLX5_CAP_GEN(dev, lag_master) ||
+	    (MLX5_CAP_GEN(dev, num_lag_ports) != MLX5_MAX_PORTS))
+		return;
+
+	tmp_dev = mlx5_get_next_phys_dev(dev);
+	if (tmp_dev)
+		ldev = tmp_dev->priv.lag;
+
+	if (!ldev) {
+		ldev = mlx5_lag_dev_alloc();
+		if (!ldev) {
+			mlx5_core_err(dev, "Failed to alloc lag dev\n");
+			return;
+		}
+	}
+
+	mlx5_lag_dev_add_pf(ldev, dev, netdev);
+
+	if (!ldev->nb.notifier_call) {
+		ldev->nb.notifier_call = mlx5_lag_netdev_event;
+		if (register_netdevice_notifier(&ldev->nb)) {
+			ldev->nb.notifier_call = NULL;
+			mlx5_core_err(dev, "Failed to register LAG netdev notifier\n");
+		}
+	}
+}
+
+/* Must be called with intf_mutex held */
+void mlx5_lag_remove(struct mlx5_core_dev *dev)
+{
+	struct mlx5_lag *ldev;
+	int i;
+
+	ldev = mlx5_lag_dev_get(dev);
+	if (!ldev)
+		return;
+
+	if (mlx5_lag_is_bonded(ldev))
+		mlx5_deactivate_lag(ldev);
+
+	mlx5_lag_dev_remove_pf(ldev, dev);
+
+	for (i = 0; i < MLX5_MAX_PORTS; i++)
+		if (ldev->pf[i].dev)
+			break;
+
+	if (i == MLX5_MAX_PORTS) {
+		if (ldev->nb.notifier_call)
+			unregister_netdevice_notifier(&ldev->nb);
+		cancel_delayed_work_sync(&ldev->bond_work);
+		mlx5_lag_dev_free(ldev);
+	}
+}
+
+bool mlx5_lag_is_active(struct mlx5_core_dev *dev)
+{
+	struct mlx5_lag *ldev;
+	bool res;
+
+	mutex_lock(&lag_mutex);
+	ldev = mlx5_lag_dev_get(dev);
+	res  = ldev && mlx5_lag_is_bonded(ldev);
+	mutex_unlock(&lag_mutex);
+
+	return res;
+}
+EXPORT_SYMBOL(mlx5_lag_is_active);
+
+struct net_device *mlx5_lag_get_roce_netdev(struct mlx5_core_dev *dev)
+{
+	struct net_device *ndev = NULL;
+	struct mlx5_lag *ldev;
+
+	mutex_lock(&lag_mutex);
+	ldev = mlx5_lag_dev_get(dev);
+
+	if (!(ldev && mlx5_lag_is_bonded(ldev)))
+		goto unlock;
+
+	if (ldev->tracker.tx_type == NETDEV_LAG_TX_TYPE_ACTIVEBACKUP) {
+		ndev = ldev->tracker.netdev_state[0].tx_enabled ?
+		       ldev->pf[0].netdev : ldev->pf[1].netdev;
+	} else {
+		ndev = ldev->pf[0].netdev;
+	}
+	if (ndev)
+		dev_hold(ndev);
+
+unlock:
+	mutex_unlock(&lag_mutex);
+
+	return ndev;
+}
+EXPORT_SYMBOL(mlx5_lag_get_roce_netdev);
+
+bool mlx5_lag_intf_add(struct mlx5_interface *intf, struct mlx5_priv *priv)
+{
+	struct mlx5_core_dev *dev = container_of(priv, struct mlx5_core_dev,
+						 priv);
+	struct mlx5_lag *ldev;
+
+	if (intf->protocol != MLX5_INTERFACE_PROTOCOL_IB)
+		return true;
+
+	ldev = mlx5_lag_dev_get(dev);
+	if (!ldev || !mlx5_lag_is_bonded(ldev) || ldev->pf[0].dev == dev)
+		return true;
+
+	/* If bonded, we do not add an IB device for PF1. */
+	return false;
+}
+
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/mad.c b/drivers/net/ethernet/mellanox/mlx5/core/mad.c
index 1368dac00da0..3a3b0005fd2b 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/mad.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/mad.c
@@ -39,36 +39,33 @@
 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;
-	struct mlx5_mad_ifc_mbox_out *out = NULL;
-	int err;
+	int outlen = MLX5_ST_SZ_BYTES(mad_ifc_out);
+	int inlen = MLX5_ST_SZ_BYTES(mad_ifc_in);
+	int err = -ENOMEM;
+	void *data;
+	void *resp;
+	u32 *out;
+	u32 *in;
 
-	in = kzalloc(sizeof(*in), GFP_KERNEL);
-	if (!in)
-		return -ENOMEM;
-
-	out = kzalloc(sizeof(*out), GFP_KERNEL);
-	if (!out) {
-		err = -ENOMEM;
+	in = kzalloc(inlen, GFP_KERNEL);
+	out = kzalloc(outlen, GFP_KERNEL);
+	if (!in || !out)
 		goto out;
-	}
 
-	in->hdr.opcode = cpu_to_be16(MLX5_CMD_OP_MAD_IFC);
-	in->hdr.opmod = cpu_to_be16(opmod);
-	in->port = port;
+	MLX5_SET(mad_ifc_in, in, opcode, MLX5_CMD_OP_MAD_IFC);
+	MLX5_SET(mad_ifc_in, in, op_mod, opmod);
+	MLX5_SET(mad_ifc_in, in, port, port);
 
-	memcpy(in->data, inb, sizeof(in->data));
+	data = MLX5_ADDR_OF(mad_ifc_in, in, mad);
+	memcpy(data, inb, MLX5_FLD_SZ_BYTES(mad_ifc_in, mad));
 
-	err = mlx5_cmd_exec(dev, in, sizeof(*in), out, sizeof(*out));
+	err = mlx5_cmd_exec(dev, in, inlen, out, outlen);
 	if (err)
 		goto out;
 
-	if (out->hdr.status) {
-		err = mlx5_cmd_status_to_err(&out->hdr);
-		goto out;
-	}
-
-	memcpy(outb, out->data, sizeof(out->data));
+	resp = MLX5_ADDR_OF(mad_ifc_out, out, response_mad_packet);
+	memcpy(outb, resp,
+	       MLX5_FLD_SZ_BYTES(mad_ifc_out, response_mad_packet));
 
 out:
 	kfree(out);
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/main.c b/drivers/net/ethernet/mellanox/mlx5/core/main.c
index 2385bae92672..d9c3c70b29e4 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/main.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/main.c
@@ -72,16 +72,6 @@ static int prof_sel = MLX5_DEFAULT_PROF;
 module_param_named(prof_sel, prof_sel, int, 0444);
 MODULE_PARM_DESC(prof_sel, "profile selector. Valid range 0 - 2");
 
-static LIST_HEAD(intf_list);
-static LIST_HEAD(dev_list);
-static DEFINE_MUTEX(intf_mutex);
-
-struct mlx5_device_context {
-	struct list_head	list;
-	struct mlx5_interface  *intf;
-	void		       *context;
-};
-
 enum {
 	MLX5_ATOMIC_REQ_MODE_BE = 0x0,
 	MLX5_ATOMIC_REQ_MODE_HOST_ENDIANNESS = 0x1,
@@ -324,7 +314,7 @@ enum {
 				MLX5_DEV_CAP_FLAG_DCT,
 };
 
-static u16 to_fw_pkey_sz(u32 size)
+static u16 to_fw_pkey_sz(struct mlx5_core_dev *dev, u32 size)
 {
 	switch (size) {
 	case 128:
@@ -340,7 +330,7 @@ static u16 to_fw_pkey_sz(u32 size)
 	case 4096:
 		return 5;
 	default:
-		pr_warn("invalid pkey table size %d\n", size);
+		mlx5_core_warn(dev, "invalid pkey table size %d\n", size);
 		return 0;
 	}
 }
@@ -363,10 +353,6 @@ static int mlx5_core_get_caps_mode(struct mlx5_core_dev *dev,
 	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);
-	if (err)
-		goto query_ex;
-
-	err = mlx5_cmd_status_to_err_v2(out);
 	if (err) {
 		mlx5_core_warn(dev,
 			       "QUERY_HCA_CAP : type(%x) opmode(%x) Failed(%d)\n",
@@ -409,20 +395,11 @@ int mlx5_core_get_caps(struct mlx5_core_dev *dev, enum mlx5_cap_type cap_type)
 
 static int set_caps(struct mlx5_core_dev *dev, void *in, int in_sz, int opmod)
 {
-	u32 out[MLX5_ST_SZ_DW(set_hca_cap_out)];
-	int err;
-
-	memset(out, 0, sizeof(out));
+	u32 out[MLX5_ST_SZ_DW(set_hca_cap_out)] = {0};
 
 	MLX5_SET(set_hca_cap_in, in, opcode, MLX5_CMD_OP_SET_HCA_CAP);
 	MLX5_SET(set_hca_cap_in, in, op_mod, opmod << 1);
-	err = mlx5_cmd_exec(dev, in, in_sz, out, sizeof(out));
-	if (err)
-		return err;
-
-	err = mlx5_cmd_status_to_err_v2(out);
-
-	return err;
+	return mlx5_cmd_exec(dev, in, in_sz, out, sizeof(out));
 }
 
 static int handle_hca_cap_atomic(struct mlx5_core_dev *dev)
@@ -490,7 +467,7 @@ static int handle_hca_cap(struct mlx5_core_dev *dev)
 		      128);
 	/* we limit the size of the pkey table to 128 entries for now */
 	MLX5_SET(cmd_hca_cap, set_hca_cap, pkey_table_size,
-		 to_fw_pkey_sz(128));
+		 to_fw_pkey_sz(dev, 128));
 
 	if (prof->mask & MLX5_PROF_MASK_QP_SIZE)
 		MLX5_SET(cmd_hca_cap, set_hca_cap, log_max_qp,
@@ -528,37 +505,22 @@ static int set_hca_ctrl(struct mlx5_core_dev *dev)
 
 int mlx5_core_enable_hca(struct mlx5_core_dev *dev, u16 func_id)
 {
-	u32 out[MLX5_ST_SZ_DW(enable_hca_out)];
-	u32 in[MLX5_ST_SZ_DW(enable_hca_in)];
-	int err;
+	u32 out[MLX5_ST_SZ_DW(enable_hca_out)] = {0};
+	u32 in[MLX5_ST_SZ_DW(enable_hca_in)]   = {0};
 
-	memset(in, 0, sizeof(in));
 	MLX5_SET(enable_hca_in, in, opcode, MLX5_CMD_OP_ENABLE_HCA);
 	MLX5_SET(enable_hca_in, in, function_id, func_id);
-	memset(out, 0, sizeof(out));
-
-	err = mlx5_cmd_exec(dev, &in, sizeof(in), &out, sizeof(out));
-	if (err)
-		return err;
-
-	return mlx5_cmd_status_to_err_v2(out);
+	return mlx5_cmd_exec(dev, &in, sizeof(in), &out, sizeof(out));
 }
 
 int mlx5_core_disable_hca(struct mlx5_core_dev *dev, u16 func_id)
 {
-	u32 out[MLX5_ST_SZ_DW(disable_hca_out)];
-	u32 in[MLX5_ST_SZ_DW(disable_hca_in)];
-	int err;
+	u32 out[MLX5_ST_SZ_DW(disable_hca_out)] = {0};
+	u32 in[MLX5_ST_SZ_DW(disable_hca_in)]   = {0};
 
-	memset(in, 0, sizeof(in));
 	MLX5_SET(disable_hca_in, in, opcode, MLX5_CMD_OP_DISABLE_HCA);
 	MLX5_SET(disable_hca_in, in, function_id, func_id);
-	memset(out, 0, sizeof(out));
-	err = mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
-	if (err)
-		return err;
-
-	return mlx5_cmd_status_to_err_v2(out);
+	return mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
 }
 
 cycle_t mlx5_read_internal_timer(struct mlx5_core_dev *dev)
@@ -758,44 +720,40 @@ clean:
 
 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 query_in[MLX5_ST_SZ_DW(query_issi_in)]   = {0};
+	u32 query_out[MLX5_ST_SZ_DW(query_issi_out)] = {0};
 	u32 sup_issi;
-
-	memset(query_in, 0, sizeof(query_in));
-	memset(query_out, 0, sizeof(query_out));
+	int err;
 
 	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));
+	err = mlx5_cmd_exec(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) {
+		u32 syndrome;
+		u8 status;
+
+		mlx5_cmd_mbox_status(query_out, &status, &syndrome);
+		if (status == MLX5_CMD_STAT_BAD_OP_ERR) {
 			pr_debug("Only ISSI 0 is supported\n");
 			return 0;
 		}
 
-		pr_err("failed to query ISSI\n");
+		pr_err("failed to query ISSI err(%d)\n", err);
 		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));
+		u32 set_in[MLX5_ST_SZ_DW(set_issi_in)]   = {0};
+		u32 set_out[MLX5_ST_SZ_DW(set_issi_out)] = {0};
 
 		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));
+		err = mlx5_cmd_exec(dev, set_in, sizeof(set_in),
+				    set_out, sizeof(set_out));
 		if (err) {
-			pr_err("failed to set ISSI=1\n");
+			pr_err("failed to set ISSI=1 err(%d)\n", err);
 			return err;
 		}
 
@@ -809,120 +767,6 @@ static int mlx5_core_set_issi(struct mlx5_core_dev *dev)
 	return -ENOTSUPP;
 }
 
-static void mlx5_add_device(struct mlx5_interface *intf, struct mlx5_priv *priv)
-{
-	struct mlx5_device_context *dev_ctx;
-	struct mlx5_core_dev *dev = container_of(priv, struct mlx5_core_dev, priv);
-
-	dev_ctx = kmalloc(sizeof(*dev_ctx), GFP_KERNEL);
-	if (!dev_ctx)
-		return;
-
-	dev_ctx->intf    = intf;
-	dev_ctx->context = intf->add(dev);
-
-	if (dev_ctx->context) {
-		spin_lock_irq(&priv->ctx_lock);
-		list_add_tail(&dev_ctx->list, &priv->ctx_list);
-		spin_unlock_irq(&priv->ctx_lock);
-	} else {
-		kfree(dev_ctx);
-	}
-}
-
-static void mlx5_remove_device(struct mlx5_interface *intf, struct mlx5_priv *priv)
-{
-	struct mlx5_device_context *dev_ctx;
-	struct mlx5_core_dev *dev = container_of(priv, struct mlx5_core_dev, priv);
-
-	list_for_each_entry(dev_ctx, &priv->ctx_list, list)
-		if (dev_ctx->intf == intf) {
-			spin_lock_irq(&priv->ctx_lock);
-			list_del(&dev_ctx->list);
-			spin_unlock_irq(&priv->ctx_lock);
-
-			intf->remove(dev, dev_ctx->context);
-			kfree(dev_ctx);
-			return;
-		}
-}
-
-static int mlx5_register_device(struct mlx5_core_dev *dev)
-{
-	struct mlx5_priv *priv = &dev->priv;
-	struct mlx5_interface *intf;
-
-	mutex_lock(&intf_mutex);
-	list_add_tail(&priv->dev_list, &dev_list);
-	list_for_each_entry(intf, &intf_list, list)
-		mlx5_add_device(intf, priv);
-	mutex_unlock(&intf_mutex);
-
-	return 0;
-}
-
-static void mlx5_unregister_device(struct mlx5_core_dev *dev)
-{
-	struct mlx5_priv *priv = &dev->priv;
-	struct mlx5_interface *intf;
-
-	mutex_lock(&intf_mutex);
-	list_for_each_entry(intf, &intf_list, list)
-		mlx5_remove_device(intf, priv);
-	list_del(&priv->dev_list);
-	mutex_unlock(&intf_mutex);
-}
-
-int mlx5_register_interface(struct mlx5_interface *intf)
-{
-	struct mlx5_priv *priv;
-
-	if (!intf->add || !intf->remove)
-		return -EINVAL;
-
-	mutex_lock(&intf_mutex);
-	list_add_tail(&intf->list, &intf_list);
-	list_for_each_entry(priv, &dev_list, dev_list)
-		mlx5_add_device(intf, priv);
-	mutex_unlock(&intf_mutex);
-
-	return 0;
-}
-EXPORT_SYMBOL(mlx5_register_interface);
-
-void mlx5_unregister_interface(struct mlx5_interface *intf)
-{
-	struct mlx5_priv *priv;
-
-	mutex_lock(&intf_mutex);
-	list_for_each_entry(priv, &dev_list, dev_list)
-		mlx5_remove_device(intf, priv);
-	list_del(&intf->list);
-	mutex_unlock(&intf_mutex);
-}
-EXPORT_SYMBOL(mlx5_unregister_interface);
-
-void *mlx5_get_protocol_dev(struct mlx5_core_dev *mdev, int protocol)
-{
-	struct mlx5_priv *priv = &mdev->priv;
-	struct mlx5_device_context *dev_ctx;
-	unsigned long flags;
-	void *result = NULL;
-
-	spin_lock_irqsave(&priv->ctx_lock, flags);
-
-	list_for_each_entry(dev_ctx, &mdev->priv.ctx_list, list)
-		if ((dev_ctx->intf->protocol == protocol) &&
-		    dev_ctx->intf->get_dev) {
-			result = dev_ctx->intf->get_dev(dev_ctx->context);
-			break;
-		}
-
-	spin_unlock_irqrestore(&priv->ctx_lock, flags);
-
-	return result;
-}
-EXPORT_SYMBOL(mlx5_get_protocol_dev);
 
 static int mlx5_pci_init(struct mlx5_core_dev *dev, struct mlx5_priv *priv)
 {
@@ -995,8 +839,102 @@ static void mlx5_pci_close(struct mlx5_core_dev *dev, struct mlx5_priv *priv)
 	debugfs_remove(priv->dbg_root);
 }
 
-#define MLX5_IB_MOD "mlx5_ib"
-static int mlx5_load_one(struct mlx5_core_dev *dev, struct mlx5_priv *priv)
+static int mlx5_init_once(struct mlx5_core_dev *dev, struct mlx5_priv *priv)
+{
+	struct pci_dev *pdev = dev->pdev;
+	int err;
+
+	err = mlx5_query_hca_caps(dev);
+	if (err) {
+		dev_err(&pdev->dev, "query hca failed\n");
+		goto out;
+	}
+
+	err = mlx5_query_board_id(dev);
+	if (err) {
+		dev_err(&pdev->dev, "query board id failed\n");
+		goto out;
+	}
+
+	err = mlx5_eq_init(dev);
+	if (err) {
+		dev_err(&pdev->dev, "failed to initialize eq\n");
+		goto out;
+	}
+
+	MLX5_INIT_DOORBELL_LOCK(&priv->cq_uar_lock);
+
+	err = mlx5_init_cq_table(dev);
+	if (err) {
+		dev_err(&pdev->dev, "failed to initialize cq table\n");
+		goto err_eq_cleanup;
+	}
+
+	mlx5_init_qp_table(dev);
+
+	mlx5_init_srq_table(dev);
+
+	mlx5_init_mkey_table(dev);
+
+	err = mlx5_init_rl_table(dev);
+	if (err) {
+		dev_err(&pdev->dev, "Failed to init rate limiting\n");
+		goto err_tables_cleanup;
+	}
+
+#ifdef CONFIG_MLX5_CORE_EN
+	err = mlx5_eswitch_init(dev);
+	if (err) {
+		dev_err(&pdev->dev, "Failed to init eswitch %d\n", err);
+		goto err_rl_cleanup;
+	}
+#endif
+
+	err = mlx5_sriov_init(dev);
+	if (err) {
+		dev_err(&pdev->dev, "Failed to init sriov %d\n", err);
+		goto err_eswitch_cleanup;
+	}
+
+	return 0;
+
+err_eswitch_cleanup:
+#ifdef CONFIG_MLX5_CORE_EN
+	mlx5_eswitch_cleanup(dev->priv.eswitch);
+
+err_rl_cleanup:
+#endif
+	mlx5_cleanup_rl_table(dev);
+
+err_tables_cleanup:
+	mlx5_cleanup_mkey_table(dev);
+	mlx5_cleanup_srq_table(dev);
+	mlx5_cleanup_qp_table(dev);
+	mlx5_cleanup_cq_table(dev);
+
+err_eq_cleanup:
+	mlx5_eq_cleanup(dev);
+
+out:
+	return err;
+}
+
+static void mlx5_cleanup_once(struct mlx5_core_dev *dev)
+{
+	mlx5_sriov_cleanup(dev);
+#ifdef CONFIG_MLX5_CORE_EN
+	mlx5_eswitch_cleanup(dev->priv.eswitch);
+#endif
+	mlx5_cleanup_rl_table(dev);
+	mlx5_cleanup_mkey_table(dev);
+	mlx5_cleanup_srq_table(dev);
+	mlx5_cleanup_qp_table(dev);
+	mlx5_cleanup_cq_table(dev);
+	mlx5_eq_cleanup(dev);
+}
+
+static int mlx5_load_one(struct mlx5_core_dev *dev, struct mlx5_priv *priv,
+			 bool boot)
 {
 	struct pci_dev *pdev = dev->pdev;
 	int err;
@@ -1029,12 +967,10 @@ static int mlx5_load_one(struct mlx5_core_dev *dev, struct mlx5_priv *priv)
 		goto out_err;
 	}
 
-	mlx5_pagealloc_init(dev);
-
 	err = mlx5_core_enable_hca(dev, 0);
 	if (err) {
 		dev_err(&pdev->dev, "enable hca failed\n");
-		goto err_pagealloc_cleanup;
+		goto err_cmd_cleanup;
 	}
 
 	err = mlx5_core_set_issi(dev);
@@ -1087,34 +1023,21 @@ static int mlx5_load_one(struct mlx5_core_dev *dev, struct mlx5_priv *priv)
 
 	mlx5_start_health_poll(dev);
 
-	err = mlx5_query_hca_caps(dev);
-	if (err) {
-		dev_err(&pdev->dev, "query hca failed\n");
-		goto err_stop_poll;
-	}
-
-	err = mlx5_query_board_id(dev);
-	if (err) {
-		dev_err(&pdev->dev, "query board id failed\n");
+	if (boot && mlx5_init_once(dev, priv)) {
+		dev_err(&pdev->dev, "sw objs init failed\n");
 		goto err_stop_poll;
 	}
 
 	err = mlx5_enable_msix(dev);
 	if (err) {
 		dev_err(&pdev->dev, "enable msix failed\n");
-		goto err_stop_poll;
-	}
-
-	err = mlx5_eq_init(dev);
-	if (err) {
-		dev_err(&pdev->dev, "failed to initialize eq\n");
-		goto disable_msix;
+		goto err_cleanup_once;
 	}
 
 	err = mlx5_alloc_uuars(dev, &priv->uuari);
 	if (err) {
 		dev_err(&pdev->dev, "Failed allocating uar, aborting\n");
-		goto err_eq_cleanup;
+		goto err_disable_msix;
 	}
 
 	err = mlx5_start_eqs(dev);
@@ -1130,15 +1053,10 @@ static int mlx5_load_one(struct mlx5_core_dev *dev, struct mlx5_priv *priv)
 	}
 
 	err = mlx5_irq_set_affinity_hints(dev);
-	if (err)
+	if (err) {
 		dev_err(&pdev->dev, "Failed to alloc affinity hint cpumask\n");
-
-	MLX5_INIT_DOORBELL_LOCK(&priv->cq_uar_lock);
-
-	mlx5_init_cq_table(dev);
-	mlx5_init_qp_table(dev);
-	mlx5_init_srq_table(dev);
-	mlx5_init_mkey_table(dev);
+		goto err_affinity_hints;
+	}
 
 	err = mlx5_init_fs(dev);
 	if (err) {
@@ -1146,36 +1064,26 @@ static int mlx5_load_one(struct mlx5_core_dev *dev, struct mlx5_priv *priv)
 		goto err_fs;
 	}
 
-	err = mlx5_init_rl_table(dev);
-	if (err) {
-		dev_err(&pdev->dev, "Failed to init rate limiting\n");
-		goto err_rl;
-	}
-
 #ifdef CONFIG_MLX5_CORE_EN
-	err = mlx5_eswitch_init(dev);
-	if (err) {
-		dev_err(&pdev->dev, "eswitch init failed %d\n", err);
-		goto err_reg_dev;
-	}
+	mlx5_eswitch_attach(dev->priv.eswitch);
 #endif
 
-	err = mlx5_sriov_init(dev);
+	err = mlx5_sriov_attach(dev);
 	if (err) {
 		dev_err(&pdev->dev, "sriov init failed %d\n", err);
 		goto err_sriov;
 	}
 
-	err = mlx5_register_device(dev);
-	if (err) {
-		dev_err(&pdev->dev, "mlx5_register_device failed %d\n", err);
-		goto err_reg_dev;
+	if (mlx5_device_registered(dev)) {
+		mlx5_attach_device(dev);
+	} else {
+		err = mlx5_register_device(dev);
+		if (err) {
+			dev_err(&pdev->dev, "mlx5_register_device failed %d\n", err);
+			goto err_reg_dev;
+		}
 	}
 
-	err = request_module_nowait(MLX5_IB_MOD);
-	if (err)
-		pr_info("failed request module on %s\n", MLX5_IB_MOD);
-
 	clear_bit(MLX5_INTERFACE_STATE_DOWN, &dev->intf_state);
 	set_bit(MLX5_INTERFACE_STATE_UP, &dev->intf_state);
 out:
@@ -1183,23 +1091,19 @@ out:
 
 	return 0;
 
-err_sriov:
-	if (mlx5_sriov_cleanup(dev))
-		dev_err(&dev->pdev->dev, "sriov cleanup failed\n");
+err_reg_dev:
+	mlx5_sriov_detach(dev);
 
+err_sriov:
 #ifdef CONFIG_MLX5_CORE_EN
-	mlx5_eswitch_cleanup(dev->priv.eswitch);
+	mlx5_eswitch_detach(dev->priv.eswitch);
 #endif
-err_reg_dev:
-	mlx5_cleanup_rl_table(dev);
-err_rl:
 	mlx5_cleanup_fs(dev);
+
 err_fs:
-	mlx5_cleanup_mkey_table(dev);
-	mlx5_cleanup_srq_table(dev);
-	mlx5_cleanup_qp_table(dev);
-	mlx5_cleanup_cq_table(dev);
 	mlx5_irq_clear_affinity_hints(dev);
+
+err_affinity_hints:
 	free_comp_eqs(dev);
 
 err_stop_eqs:
@@ -1208,12 +1112,13 @@ err_stop_eqs:
 err_free_uar:
 	mlx5_free_uuars(dev, &priv->uuari);
 
-err_eq_cleanup:
-	mlx5_eq_cleanup(dev);
-
-disable_msix:
+err_disable_msix:
 	mlx5_disable_msix(dev);
 
+err_cleanup_once:
+	if (boot)
+		mlx5_cleanup_once(dev);
+
 err_stop_poll:
 	mlx5_stop_health_poll(dev);
 	if (mlx5_cmd_teardown_hca(dev)) {
@@ -1230,8 +1135,7 @@ reclaim_boot_pages:
 err_disable_hca:
 	mlx5_core_disable_hca(dev, 0);
 
-err_pagealloc_cleanup:
-	mlx5_pagealloc_cleanup(dev);
+err_cmd_cleanup:
 	mlx5_cmd_cleanup(dev);
 
 out_err:
@@ -1241,40 +1145,35 @@ out_err:
 	return err;
 }
 
-static int mlx5_unload_one(struct mlx5_core_dev *dev, struct mlx5_priv *priv)
+static int mlx5_unload_one(struct mlx5_core_dev *dev, struct mlx5_priv *priv,
+			   bool cleanup)
 {
 	int err = 0;
 
-	err = mlx5_sriov_cleanup(dev);
-	if (err) {
-		dev_warn(&dev->pdev->dev, "%s: sriov cleanup failed - abort\n",
-			 __func__);
-		return err;
-	}
-
 	mutex_lock(&dev->intf_state_mutex);
 	if (test_bit(MLX5_INTERFACE_STATE_DOWN, &dev->intf_state)) {
 		dev_warn(&dev->pdev->dev, "%s: interface is down, NOP\n",
 			 __func__);
+		if (cleanup)
+			mlx5_cleanup_once(dev);
 		goto out;
 	}
-	mlx5_unregister_device(dev);
+
+	if (mlx5_device_registered(dev))
+		mlx5_detach_device(dev);
+
+	mlx5_sriov_detach(dev);
 #ifdef CONFIG_MLX5_CORE_EN
-	mlx5_eswitch_cleanup(dev->priv.eswitch);
+	mlx5_eswitch_detach(dev->priv.eswitch);
 #endif
-
-	mlx5_cleanup_rl_table(dev);
 	mlx5_cleanup_fs(dev);
-	mlx5_cleanup_mkey_table(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);
-	mlx5_eq_cleanup(dev);
 	mlx5_disable_msix(dev);
+	if (cleanup)
+		mlx5_cleanup_once(dev);
 	mlx5_stop_health_poll(dev);
 	err = mlx5_cmd_teardown_hca(dev);
 	if (err) {
@@ -1284,7 +1183,6 @@ static int mlx5_unload_one(struct mlx5_core_dev *dev, struct mlx5_priv *priv)
 	mlx5_pagealloc_stop(dev);
 	mlx5_reclaim_startup_pages(dev);
 	mlx5_core_disable_hca(dev, 0);
-	mlx5_pagealloc_cleanup(dev);
 	mlx5_cmd_cleanup(dev);
 
 out:
@@ -1294,22 +1192,6 @@ out:
 	return err;
 }
 
-void mlx5_core_event(struct mlx5_core_dev *dev, enum mlx5_dev_event event,
-		     unsigned long param)
-{
-	struct mlx5_priv *priv = &dev->priv;
-	struct mlx5_device_context *dev_ctx;
-	unsigned long flags;
-
-	spin_lock_irqsave(&priv->ctx_lock, flags);
-
-	list_for_each_entry(dev_ctx, &priv->ctx_list, list)
-		if (dev_ctx->intf->event)
-			dev_ctx->intf->event(dev, dev_ctx->context, event, param);
-
-	spin_unlock_irqrestore(&priv->ctx_lock, flags);
-}
-
 struct mlx5_core_event_handler {
 	void (*event)(struct mlx5_core_dev *dev,
 		      enum mlx5_dev_event event,
@@ -1323,6 +1205,7 @@ static const struct devlink_ops mlx5_devlink_ops = {
 #endif
 };
 
+#define MLX5_IB_MOD "mlx5_ib"
 static int init_one(struct pci_dev *pdev,
 		    const struct pci_device_id *id)
 {
@@ -1344,8 +1227,9 @@ static int init_one(struct pci_dev *pdev,
 	pci_set_drvdata(pdev, dev);
 
 	if (prof_sel < 0 || prof_sel >= ARRAY_SIZE(profile)) {
-		pr_warn("selected profile out of range, selecting default (%d)\n",
-			MLX5_DEFAULT_PROF);
+		mlx5_core_warn(dev,
+			       "selected profile out of range, selecting default (%d)\n",
+			       MLX5_DEFAULT_PROF);
 		prof_sel = MLX5_DEFAULT_PROF;
 	}
 	dev->profile = &profile[prof_sel];
@@ -1368,12 +1252,18 @@ static int init_one(struct pci_dev *pdev,
 		goto close_pci;
 	}
 
-	err = mlx5_load_one(dev, priv);
+	mlx5_pagealloc_init(dev);
+
+	err = mlx5_load_one(dev, priv, true);
 	if (err) {
 		dev_err(&pdev->dev, "mlx5_load_one failed with error code %d\n", err);
 		goto clean_health;
 	}
 
+	err = request_module_nowait(MLX5_IB_MOD);
+	if (err)
+		pr_info("failed request module on %s\n", MLX5_IB_MOD);
+
 	err = devlink_register(devlink, &pdev->dev);
 	if (err)
 		goto clean_load;
@@ -1381,8 +1271,9 @@ static int init_one(struct pci_dev *pdev,
 	return 0;
 
 clean_load:
-	mlx5_unload_one(dev, priv);
+	mlx5_unload_one(dev, priv, true);
 clean_health:
+	mlx5_pagealloc_cleanup(dev);
 	mlx5_health_cleanup(dev);
 close_pci:
 	mlx5_pci_close(dev, priv);
@@ -1400,11 +1291,15 @@ static void remove_one(struct pci_dev *pdev)
 	struct mlx5_priv *priv = &dev->priv;
 
 	devlink_unregister(devlink);
-	if (mlx5_unload_one(dev, priv)) {
+	mlx5_unregister_device(dev);
+
+	if (mlx5_unload_one(dev, priv, true)) {
 		dev_err(&dev->pdev->dev, "mlx5_unload_one failed\n");
 		mlx5_health_cleanup(dev);
 		return;
 	}
+
+	mlx5_pagealloc_cleanup(dev);
 	mlx5_health_cleanup(dev);
 	mlx5_pci_close(dev, priv);
 	pci_set_drvdata(pdev, NULL);
@@ -1419,7 +1314,7 @@ static pci_ers_result_t mlx5_pci_err_detected(struct pci_dev *pdev,
 
 	dev_info(&pdev->dev, "%s was called\n", __func__);
 	mlx5_enter_error_state(dev);
-	mlx5_unload_one(dev, priv);
+	mlx5_unload_one(dev, priv, false);
 	pci_save_state(pdev);
 	mlx5_pci_disable_device(dev);
 	return state == pci_channel_io_perm_failure ?
@@ -1491,7 +1386,7 @@ static void mlx5_pci_resume(struct pci_dev *pdev)
 
 	dev_info(&pdev->dev, "%s was called\n", __func__);
 
-	err = mlx5_load_one(dev, priv);
+	err = mlx5_load_one(dev, priv, false);
 	if (err)
 		dev_err(&pdev->dev, "%s: mlx5_load_one failed with error code: %d\n"
 			, __func__, err);
@@ -1513,7 +1408,7 @@ static void shutdown(struct pci_dev *pdev)
 	dev_info(&pdev->dev, "Shutdown was called\n");
 	/* Notify mlx5 clients that the kernel is being shut down */
 	set_bit(MLX5_INTERFACE_STATE_SHUTDOWN, &dev->intf_state);
-	mlx5_unload_one(dev, priv);
+	mlx5_unload_one(dev, priv, false);
 	mlx5_pci_disable_device(dev);
 }
 
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/mcg.c b/drivers/net/ethernet/mellanox/mlx5/core/mcg.c
index d5a0c2d61a18..ba2b09cc192f 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/mcg.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/mcg.c
@@ -37,70 +37,30 @@
 #include <rdma/ib_verbs.h>
 #include "mlx5_core.h"
 
-struct mlx5_attach_mcg_mbox_in {
-	struct mlx5_inbox_hdr	hdr;
-	__be32			qpn;
-	__be32			rsvd;
-	u8			gid[16];
-};
-
-struct mlx5_attach_mcg_mbox_out {
-	struct mlx5_outbox_hdr	hdr;
-	u8			rsvf[8];
-};
-
-struct mlx5_detach_mcg_mbox_in {
-	struct mlx5_inbox_hdr	hdr;
-	__be32			qpn;
-	__be32			rsvd;
-	u8			gid[16];
-};
-
-struct mlx5_detach_mcg_mbox_out {
-	struct mlx5_outbox_hdr	hdr;
-	u8			rsvf[8];
-};
-
 int mlx5_core_attach_mcg(struct mlx5_core_dev *dev, union ib_gid *mgid, u32 qpn)
 {
-	struct mlx5_attach_mcg_mbox_in in;
-	struct mlx5_attach_mcg_mbox_out out;
-	int err;
+	u32 out[MLX5_ST_SZ_DW(attach_to_mcg_out)] = {0};
+	u32 in[MLX5_ST_SZ_DW(attach_to_mcg_in)]   = {0};
+	void *gid;
 
-	memset(&in, 0, sizeof(in));
-	memset(&out, 0, sizeof(out));
-	in.hdr.opcode = cpu_to_be16(MLX5_CMD_OP_ATTACH_TO_MCG);
-	memcpy(in.gid, mgid, sizeof(*mgid));
-	in.qpn = cpu_to_be32(qpn);
-	err = mlx5_cmd_exec(dev, &in, sizeof(in), &out, sizeof(out));
-	if (err)
-		return err;
-
-	if (out.hdr.status)
-		err = mlx5_cmd_status_to_err(&out.hdr);
-
-	return err;
+	MLX5_SET(attach_to_mcg_in, in, opcode, MLX5_CMD_OP_ATTACH_TO_MCG);
+	MLX5_SET(attach_to_mcg_in, in, qpn, qpn);
+	gid = MLX5_ADDR_OF(attach_to_mcg_in, in, multicast_gid);
+	memcpy(gid, mgid, sizeof(*mgid));
+	return mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
 }
 EXPORT_SYMBOL(mlx5_core_attach_mcg);
 
 int mlx5_core_detach_mcg(struct mlx5_core_dev *dev, union ib_gid *mgid, u32 qpn)
 {
-	struct mlx5_detach_mcg_mbox_in in;
-	struct mlx5_detach_mcg_mbox_out out;
-	int err;
-
-	memset(&in, 0, sizeof(in));
-	memset(&out, 0, sizeof(out));
-	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));
-	if (err)
-		return err;
-
-	if (out.hdr.status)
-		err = mlx5_cmd_status_to_err(&out.hdr);
+	u32 out[MLX5_ST_SZ_DW(detach_from_mcg_out)] = {0};
+	u32 in[MLX5_ST_SZ_DW(detach_from_mcg_in)]   = {0};
+	void *gid;
 
-	return err;
+	MLX5_SET(detach_from_mcg_in, in, opcode, MLX5_CMD_OP_DETACH_FROM_MCG);
+	MLX5_SET(detach_from_mcg_in, in, qpn, qpn);
+	gid = MLX5_ADDR_OF(detach_from_mcg_in, in, multicast_gid);
+	memcpy(gid, mgid, sizeof(*mgid));
+	return mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
 }
 EXPORT_SYMBOL(mlx5_core_detach_mcg);
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/mlx5_core.h b/drivers/net/ethernet/mellanox/mlx5/core/mlx5_core.h
index 2f86ec6fcf25..3d0cfb9f18f9 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/mlx5_core.h
+++ b/drivers/net/ethernet/mellanox/mlx5/core/mlx5_core.h
@@ -58,8 +58,8 @@ do {									\
 } while (0)
 
 #define mlx5_core_err(__dev, format, ...)				\
-	dev_err(&(__dev)->pdev->dev, "%s:%s:%d:(pid %d): " format,	\
-	       (__dev)->priv.name, __func__, __LINE__, current->pid,	\
+	dev_err(&(__dev)->pdev->dev, "%s:%d:(pid %d): " format,	\
+		__func__, __LINE__, current->pid,	\
 	       ##__VA_ARGS__)
 
 #define mlx5_core_warn(__dev, format, ...)				\
@@ -75,19 +75,6 @@ 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)
-{
-	int err;
-
-	err = mlx5_cmd_exec(dev, in, in_size, out, out_size);
-	if (err)
-		return err;
-
-	return mlx5_cmd_status_to_err((struct mlx5_outbox_hdr *)out);
-}
-
 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);
@@ -96,7 +83,12 @@ void mlx5_core_event(struct mlx5_core_dev *dev, enum mlx5_dev_event event,
 		     unsigned long param);
 void mlx5_enter_error_state(struct mlx5_core_dev *dev);
 void mlx5_disable_device(struct mlx5_core_dev *dev);
+int mlx5_sriov_init(struct mlx5_core_dev *dev);
+void mlx5_sriov_cleanup(struct mlx5_core_dev *dev);
+int mlx5_sriov_attach(struct mlx5_core_dev *dev);
+void mlx5_sriov_detach(struct mlx5_core_dev *dev);
 int mlx5_core_sriov_configure(struct pci_dev *dev, int num_vfs);
+bool mlx5_sriov_is_enabled(struct mlx5_core_dev *dev);
 int mlx5_core_enable_hca(struct mlx5_core_dev *dev, u16 func_id);
 int mlx5_core_disable_hca(struct mlx5_core_dev *dev, u16 func_id);
 int mlx5_wait_for_vf_pages(struct mlx5_core_dev *dev);
@@ -105,7 +97,38 @@ u32 mlx5_get_msix_vec(struct mlx5_core_dev *dev, int vecidx);
 struct mlx5_eq *mlx5_eqn2eq(struct mlx5_core_dev *dev, int eqn);
 void mlx5_cq_tasklet_cb(unsigned long data);
 
+void mlx5_lag_add(struct mlx5_core_dev *dev, struct net_device *netdev);
+void mlx5_lag_remove(struct mlx5_core_dev *dev);
+
+void mlx5_add_device(struct mlx5_interface *intf, struct mlx5_priv *priv);
+void mlx5_remove_device(struct mlx5_interface *intf, struct mlx5_priv *priv);
+void mlx5_attach_device(struct mlx5_core_dev *dev);
+void mlx5_detach_device(struct mlx5_core_dev *dev);
+bool mlx5_device_registered(struct mlx5_core_dev *dev);
+int mlx5_register_device(struct mlx5_core_dev *dev);
+void mlx5_unregister_device(struct mlx5_core_dev *dev);
+void mlx5_add_dev_by_protocol(struct mlx5_core_dev *dev, int protocol);
+void mlx5_remove_dev_by_protocol(struct mlx5_core_dev *dev, int protocol);
+struct mlx5_core_dev *mlx5_get_next_phys_dev(struct mlx5_core_dev *dev);
+void mlx5_dev_list_lock(void);
+void mlx5_dev_list_unlock(void);
+int mlx5_dev_list_trylock(void);
+
+bool mlx5_lag_intf_add(struct mlx5_interface *intf, struct mlx5_priv *priv);
+
 void mlx5e_init(void);
 void mlx5e_cleanup(void);
 
+static inline int mlx5_lag_is_lacp_owner(struct mlx5_core_dev *dev)
+{
+	/* LACP owner conditions:
+	 * 1) Function is physical.
+	 * 2) LAG is supported by FW.
+	 * 3) LAG is managed by driver (currently the only option).
+	 */
+	return  MLX5_CAP_GEN(dev, vport_group_manager) &&
+		   (MLX5_CAP_GEN(dev, num_lag_ports) > 1) &&
+		    MLX5_CAP_GEN(dev, lag_master);
+}
+
 #endif /* __MLX5_CORE_H__ */
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/mr.c b/drivers/net/ethernet/mellanox/mlx5/core/mr.c
index 77a7293921d5..b9736f505bdf 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/mr.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/mr.c
@@ -49,48 +49,43 @@ void mlx5_cleanup_mkey_table(struct mlx5_core_dev *dev)
 {
 }
 
-int mlx5_core_create_mkey(struct mlx5_core_dev *dev,
-			  struct mlx5_core_mkey *mkey,
-			  struct mlx5_create_mkey_mbox_in *in, int inlen,
-			  mlx5_cmd_cbk_t callback, void *context,
-			  struct mlx5_create_mkey_mbox_out *out)
+int mlx5_core_create_mkey_cb(struct mlx5_core_dev *dev,
+			     struct mlx5_core_mkey *mkey,
+			     u32 *in, int inlen,
+			     u32 *out, int outlen,
+			     mlx5_cmd_cbk_t callback, void *context)
 {
 	struct mlx5_mkey_table *table = &dev->priv.mkey_table;
-	struct mlx5_create_mkey_mbox_out lout;
+	u32 lout[MLX5_ST_SZ_DW(create_mkey_out)] = {0};
+	u32 mkey_index;
+	void *mkc;
 	int err;
 	u8 key;
 
-	memset(&lout, 0, sizeof(lout));
 	spin_lock_irq(&dev->priv.mkey_lock);
 	key = dev->priv.mkey_key++;
 	spin_unlock_irq(&dev->priv.mkey_lock);
-	in->seg.qpn_mkey7_0 |= cpu_to_be32(key);
-	in->hdr.opcode = cpu_to_be16(MLX5_CMD_OP_CREATE_MKEY);
-	if (callback) {
-		err = mlx5_cmd_exec_cb(dev, in, inlen, out, sizeof(*out),
-				       callback, context);
-		return err;
-	} else {
-		err = mlx5_cmd_exec(dev, in, inlen, &lout, sizeof(lout));
-	}
+	mkc = MLX5_ADDR_OF(create_mkey_in, in, memory_key_mkey_entry);
 
-	if (err) {
-		mlx5_core_dbg(dev, "cmd exec failed %d\n", err);
-		return err;
-	}
+	MLX5_SET(create_mkey_in, in, opcode, MLX5_CMD_OP_CREATE_MKEY);
+	MLX5_SET(mkc, mkc, mkey_7_0, key);
 
-	if (lout.hdr.status) {
-		mlx5_core_dbg(dev, "status %d\n", lout.hdr.status);
-		return mlx5_cmd_status_to_err(&lout.hdr);
-	}
+	if (callback)
+		return mlx5_cmd_exec_cb(dev, in, inlen, out, outlen,
+					callback, context);
+
+	err = mlx5_cmd_exec(dev, in, inlen, lout, sizeof(lout));
+	if (err)
+		return err;
 
-	mkey->iova = be64_to_cpu(in->seg.start_addr);
-	mkey->size = be64_to_cpu(in->seg.len);
-	mkey->key = mlx5_idx_to_mkey(be32_to_cpu(lout.mkey) & 0xffffff) | key;
-	mkey->pd = be32_to_cpu(in->seg.flags_pd) & 0xffffff;
+	mkey_index = MLX5_GET(create_mkey_out, lout, mkey_index);
+	mkey->iova = MLX5_GET64(mkc, mkc, start_addr);
+	mkey->size = MLX5_GET64(mkc, mkc, len);
+	mkey->key = mlx5_idx_to_mkey(mkey_index) | key;
+	mkey->pd = MLX5_GET(mkc, mkc, pd);
 
 	mlx5_core_dbg(dev, "out 0x%x, key 0x%x, mkey 0x%x\n",
-		      be32_to_cpu(lout.mkey), key, mkey->key);
+		      mkey_index, key, mkey->key);
 
 	/* connect to mkey tree */
 	write_lock_irq(&table->lock);
@@ -104,20 +99,25 @@ int mlx5_core_create_mkey(struct mlx5_core_dev *dev,
 
 	return err;
 }
+EXPORT_SYMBOL(mlx5_core_create_mkey_cb);
+
+int mlx5_core_create_mkey(struct mlx5_core_dev *dev,
+			  struct mlx5_core_mkey *mkey,
+			  u32 *in, int inlen)
+{
+	return mlx5_core_create_mkey_cb(dev, mkey, in, inlen,
+					NULL, 0, NULL, NULL);
+}
 EXPORT_SYMBOL(mlx5_core_create_mkey);
 
 int mlx5_core_destroy_mkey(struct mlx5_core_dev *dev,
 			   struct mlx5_core_mkey *mkey)
 {
 	struct mlx5_mkey_table *table = &dev->priv.mkey_table;
-	struct mlx5_destroy_mkey_mbox_in in;
-	struct mlx5_destroy_mkey_mbox_out out;
+	u32 out[MLX5_ST_SZ_DW(destroy_mkey_out)] = {0};
+	u32 in[MLX5_ST_SZ_DW(destroy_mkey_in)]   = {0};
 	struct mlx5_core_mkey *deleted_mkey;
 	unsigned long flags;
-	int err;
-
-	memset(&in, 0, sizeof(in));
-	memset(&out, 0, sizeof(out));
 
 	write_lock_irqsave(&table->lock, flags);
 	deleted_mkey = radix_tree_delete(&table->tree, mlx5_base_mkey(mkey->key));
@@ -128,94 +128,71 @@ int mlx5_core_destroy_mkey(struct mlx5_core_dev *dev,
 		return -ENOENT;
 	}
 
-	in.hdr.opcode = cpu_to_be16(MLX5_CMD_OP_DESTROY_MKEY);
-	in.mkey = cpu_to_be32(mlx5_mkey_to_idx(mkey->key));
-	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;
+	MLX5_SET(destroy_mkey_in, in, opcode, MLX5_CMD_OP_DESTROY_MKEY);
+	MLX5_SET(destroy_mkey_in, in, mkey_index, mlx5_mkey_to_idx(mkey->key));
+	return mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
 }
 EXPORT_SYMBOL(mlx5_core_destroy_mkey);
 
 int mlx5_core_query_mkey(struct mlx5_core_dev *dev, struct mlx5_core_mkey *mkey,
-			 struct mlx5_query_mkey_mbox_out *out, int outlen)
+			 u32 *out, int outlen)
 {
-	struct mlx5_query_mkey_mbox_in in;
-	int err;
+	u32 in[MLX5_ST_SZ_DW(query_mkey_in)] = {0};
 
-	memset(&in, 0, sizeof(in));
 	memset(out, 0, outlen);
-
-	in.hdr.opcode = cpu_to_be16(MLX5_CMD_OP_QUERY_MKEY);
-	in.mkey = cpu_to_be32(mlx5_mkey_to_idx(mkey->key));
-	err = mlx5_cmd_exec(dev, &in, sizeof(in), out, outlen);
-	if (err)
-		return err;
-
-	if (out->hdr.status)
-		return mlx5_cmd_status_to_err(&out->hdr);
-
-	return err;
+	MLX5_SET(query_mkey_in, in, opcode, MLX5_CMD_OP_QUERY_MKEY);
+	MLX5_SET(query_mkey_in, in, mkey_index, mlx5_mkey_to_idx(mkey->key));
+	return mlx5_cmd_exec(dev, in, sizeof(in), out, outlen);
 }
 EXPORT_SYMBOL(mlx5_core_query_mkey);
 
 int mlx5_core_dump_fill_mkey(struct mlx5_core_dev *dev, struct mlx5_core_mkey *_mkey,
 			     u32 *mkey)
 {
-	struct mlx5_query_special_ctxs_mbox_in in;
-	struct mlx5_query_special_ctxs_mbox_out out;
+	u32 out[MLX5_ST_SZ_DW(query_special_contexts_out)] = {0};
+	u32 in[MLX5_ST_SZ_DW(query_special_contexts_in)]   = {0};
 	int err;
 
-	memset(&in, 0, sizeof(in));
-	memset(&out, 0, sizeof(out));
-
-	in.hdr.opcode = cpu_to_be16(MLX5_CMD_OP_QUERY_SPECIAL_CONTEXTS);
-	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);
-
-	*mkey = be32_to_cpu(out.dump_fill_mkey);
-
+	MLX5_SET(query_special_contexts_in, in, opcode,
+		 MLX5_CMD_OP_QUERY_SPECIAL_CONTEXTS);
+	err = mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
+	if (!err)
+		*mkey = MLX5_GET(query_special_contexts_out, out,
+				 dump_fill_mkey);
 	return err;
 }
 EXPORT_SYMBOL(mlx5_core_dump_fill_mkey);
 
+static inline u32 mlx5_get_psv(u32 *out, int psv_index)
+{
+	switch (psv_index) {
+	case 1: return MLX5_GET(create_psv_out, out, psv1_index);
+	case 2: return MLX5_GET(create_psv_out, out, psv2_index);
+	case 3: return MLX5_GET(create_psv_out, out, psv3_index);
+	default: return MLX5_GET(create_psv_out, out, psv0_index);
+	}
+}
+
 int mlx5_core_create_psv(struct mlx5_core_dev *dev, u32 pdn,
 			 int npsvs, u32 *sig_index)
 {
-	struct mlx5_allocate_psv_in in;
-	struct mlx5_allocate_psv_out out;
+	u32 out[MLX5_ST_SZ_DW(create_psv_out)] = {0};
+	u32 in[MLX5_ST_SZ_DW(create_psv_in)]   = {0};
 	int i, err;
 
 	if (npsvs > MLX5_MAX_PSVS)
 		return -EINVAL;
 
-	memset(&in, 0, sizeof(in));
-	memset(&out, 0, sizeof(out));
+	MLX5_SET(create_psv_in, in, opcode, MLX5_CMD_OP_CREATE_PSV);
+	MLX5_SET(create_psv_in, in, pd, pdn);
+	MLX5_SET(create_psv_in, in, num_psv, npsvs);
 
-	in.hdr.opcode = cpu_to_be16(MLX5_CMD_OP_CREATE_PSV);
-	in.npsv_pd = cpu_to_be32((npsvs << 28) | pdn);
-	err = mlx5_cmd_exec(dev, &in, sizeof(in), &out, sizeof(out));
-	if (err) {
-		mlx5_core_err(dev, "cmd exec failed %d\n", err);
+	err = mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
+	if (err)
 		return err;
-	}
-
-	if (out.hdr.status) {
-		mlx5_core_err(dev, "create_psv bad status %d\n",
-			      out.hdr.status);
-		return mlx5_cmd_status_to_err(&out.hdr);
-	}
 
 	for (i = 0; i < npsvs; i++)
-		sig_index[i] = be32_to_cpu(out.psv_idx[i]) & 0xffffff;
+		sig_index[i] = mlx5_get_psv(out, i);
 
 	return err;
 }
@@ -223,29 +200,11 @@ EXPORT_SYMBOL(mlx5_core_create_psv);
 
 int mlx5_core_destroy_psv(struct mlx5_core_dev *dev, int psv_num)
 {
-	struct mlx5_destroy_psv_in in;
-	struct mlx5_destroy_psv_out out;
-	int err;
-
-	memset(&in, 0, sizeof(in));
-	memset(&out, 0, sizeof(out));
+	u32 out[MLX5_ST_SZ_DW(destroy_psv_out)] = {0};
+	u32 in[MLX5_ST_SZ_DW(destroy_psv_in)]   = {0};
 
-	in.psv_number = cpu_to_be32(psv_num);
-	in.hdr.opcode = cpu_to_be16(MLX5_CMD_OP_DESTROY_PSV);
-	err = mlx5_cmd_exec(dev, &in, sizeof(in), &out, sizeof(out));
-	if (err) {
-		mlx5_core_err(dev, "destroy_psv cmd exec failed %d\n", err);
-		goto out;
-	}
-
-	if (out.hdr.status) {
-		mlx5_core_err(dev, "destroy_psv bad status %d\n",
-			      out.hdr.status);
-		err = mlx5_cmd_status_to_err(&out.hdr);
-		goto out;
-	}
-
-out:
-	return err;
+	MLX5_SET(destroy_psv_in, in, opcode, MLX5_CMD_OP_DESTROY_PSV);
+	MLX5_SET(destroy_psv_in, in, psvn, psv_num);
+	return mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
 }
 EXPORT_SYMBOL(mlx5_core_destroy_psv);
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/pagealloc.c b/drivers/net/ethernet/mellanox/mlx5/core/pagealloc.c
index 32dea3524cee..cc4fd61914d3 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/pagealloc.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/pagealloc.c
@@ -44,12 +44,6 @@ enum {
 	MLX5_PAGES_TAKE		= 2
 };
 
-enum {
-	MLX5_BOOT_PAGES		= 1,
-	MLX5_INIT_PAGES		= 2,
-	MLX5_POST_INIT_PAGES	= 3
-};
-
 struct mlx5_pages_req {
 	struct mlx5_core_dev *dev;
 	u16	func_id;
@@ -67,33 +61,6 @@ struct fw_page {
 	unsigned		free_count;
 };
 
-struct mlx5_query_pages_inbox {
-	struct mlx5_inbox_hdr	hdr;
-	u8			rsvd[8];
-};
-
-struct mlx5_query_pages_outbox {
-	struct mlx5_outbox_hdr	hdr;
-	__be16			rsvd;
-	__be16			func_id;
-	__be32			num_pages;
-};
-
-struct mlx5_manage_pages_inbox {
-	struct mlx5_inbox_hdr	hdr;
-	__be16			rsvd;
-	__be16			func_id;
-	__be32			num_entries;
-	__be64			pas[0];
-};
-
-struct mlx5_manage_pages_outbox {
-	struct mlx5_outbox_hdr	hdr;
-	__be32			num_entries;
-	u8			rsvd[4];
-	__be64			pas[0];
-};
-
 enum {
 	MAX_RECLAIM_TIME_MSECS	= 5000,
 	MAX_RECLAIM_VFS_PAGES_TIME_MSECS = 2 * 1000 * 60,
@@ -167,24 +134,21 @@ static struct fw_page *find_fw_page(struct mlx5_core_dev *dev, u64 addr)
 static int mlx5_cmd_query_pages(struct mlx5_core_dev *dev, u16 *func_id,
 				s32 *npages, int boot)
 {
-	struct mlx5_query_pages_inbox	in;
-	struct mlx5_query_pages_outbox	out;
+	u32 out[MLX5_ST_SZ_DW(query_pages_out)] = {0};
+	u32 in[MLX5_ST_SZ_DW(query_pages_in)]   = {0};
 	int err;
 
-	memset(&in, 0, sizeof(in));
-	memset(&out, 0, sizeof(out));
-	in.hdr.opcode = cpu_to_be16(MLX5_CMD_OP_QUERY_PAGES);
-	in.hdr.opmod = boot ? cpu_to_be16(MLX5_BOOT_PAGES) : cpu_to_be16(MLX5_INIT_PAGES);
+	MLX5_SET(query_pages_in, in, opcode, MLX5_CMD_OP_QUERY_PAGES);
+	MLX5_SET(query_pages_in, in, op_mod, boot ?
+		 MLX5_QUERY_PAGES_IN_OP_MOD_BOOT_PAGES :
+		 MLX5_QUERY_PAGES_IN_OP_MOD_INIT_PAGES);
 
-	err = mlx5_cmd_exec(dev, &in, sizeof(in), &out, sizeof(out));
+	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);
-
-	*npages = be32_to_cpu(out.num_pages);
-	*func_id = be16_to_cpu(out.func_id);
+	*npages = MLX5_GET(query_pages_out, out, num_pages);
+	*func_id = MLX5_GET(query_pages_out, out, function_id);
 
 	return err;
 }
@@ -280,46 +244,37 @@ out_alloc:
 
 static void page_notify_fail(struct mlx5_core_dev *dev, u16 func_id)
 {
-	struct mlx5_manage_pages_inbox *in;
-	struct mlx5_manage_pages_outbox out;
+	u32 out[MLX5_ST_SZ_DW(manage_pages_out)] = {0};
+	u32 in[MLX5_ST_SZ_DW(manage_pages_in)]   = {0};
 	int err;
 
-	in = kzalloc(sizeof(*in), GFP_KERNEL);
-	if (!in)
-		return;
-
-	memset(&out, 0, sizeof(out));
-	in->hdr.opcode = cpu_to_be16(MLX5_CMD_OP_MANAGE_PAGES);
-	in->hdr.opmod = cpu_to_be16(MLX5_PAGES_CANT_GIVE);
-	in->func_id = cpu_to_be16(func_id);
-	err = mlx5_cmd_exec(dev, in, sizeof(*in), &out, sizeof(out));
-	if (!err)
-		err = mlx5_cmd_status_to_err(&out.hdr);
+	MLX5_SET(manage_pages_in, in, opcode, MLX5_CMD_OP_MANAGE_PAGES);
+	MLX5_SET(manage_pages_in, in, op_mod, MLX5_PAGES_CANT_GIVE);
+	MLX5_SET(manage_pages_in, in, function_id, func_id);
 
+	err = mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
 	if (err)
-		mlx5_core_warn(dev, "page notify failed\n");
-
-	kfree(in);
+		mlx5_core_warn(dev, "page notify failed func_id(%d) err(%d)\n",
+			       func_id, err);
 }
 
 static int give_pages(struct mlx5_core_dev *dev, u16 func_id, int npages,
 		      int notify_fail)
 {
-	struct mlx5_manage_pages_inbox *in;
-	struct mlx5_manage_pages_outbox out;
-	int inlen;
+	u32 out[MLX5_ST_SZ_DW(manage_pages_out)] = {0};
+	int inlen = MLX5_ST_SZ_BYTES(manage_pages_in);
 	u64 addr;
 	int err;
+	u32 *in;
 	int i;
 
-	inlen = sizeof(*in) + npages * sizeof(in->pas[0]);
+	inlen += npages * MLX5_FLD_SZ_BYTES(manage_pages_in, pas[0]);
 	in = mlx5_vzalloc(inlen);
 	if (!in) {
 		err = -ENOMEM;
 		mlx5_core_warn(dev, "vzalloc failed %d\n", inlen);
 		goto out_free;
 	}
-	memset(&out, 0, sizeof(out));
 
 	for (i = 0; i < npages; i++) {
 retry:
@@ -332,27 +287,21 @@ retry:
 
 			goto retry;
 		}
-		in->pas[i] = cpu_to_be64(addr);
+		MLX5_ARRAY_SET64(manage_pages_in, in, pas, i, addr);
 	}
 
-	in->hdr.opcode = cpu_to_be16(MLX5_CMD_OP_MANAGE_PAGES);
-	in->hdr.opmod = cpu_to_be16(MLX5_PAGES_GIVE);
-	in->func_id = cpu_to_be16(func_id);
-	in->num_entries = cpu_to_be32(npages);
-	err = mlx5_cmd_exec(dev, in, inlen, &out, sizeof(out));
+	MLX5_SET(manage_pages_in, in, opcode, MLX5_CMD_OP_MANAGE_PAGES);
+	MLX5_SET(manage_pages_in, in, op_mod, MLX5_PAGES_GIVE);
+	MLX5_SET(manage_pages_in, in, function_id, func_id);
+	MLX5_SET(manage_pages_in, in, input_num_entries, npages);
+
+	err = mlx5_cmd_exec(dev, in, inlen, out, sizeof(out));
 	if (err) {
 		mlx5_core_warn(dev, "func_id 0x%x, npages %d, err %d\n",
 			       func_id, npages, err);
 		goto out_4k;
 	}
 
-	err = mlx5_cmd_status_to_err(&out.hdr);
-	if (err) {
-		mlx5_core_warn(dev, "func_id 0x%x, npages %d, status %d\n",
-			       func_id, npages, out.hdr.status);
-		goto out_4k;
-	}
-
 	dev->priv.fw_pages += npages;
 	if (func_id)
 		dev->priv.vfs_pages += npages;
@@ -364,7 +313,7 @@ retry:
 
 out_4k:
 	for (i--; i >= 0; i--)
-		free_4k(dev, be64_to_cpu(in->pas[i]));
+		free_4k(dev, MLX5_GET64(manage_pages_in, in, pas[i]));
 out_free:
 	kvfree(in);
 	if (notify_fail)
@@ -373,64 +322,67 @@ out_free:
 }
 
 static int reclaim_pages_cmd(struct mlx5_core_dev *dev,
-			     struct mlx5_manage_pages_inbox *in, int in_size,
-			     struct mlx5_manage_pages_outbox *out, int out_size)
+			     u32 *in, int in_size, u32 *out, int out_size)
 {
 	struct fw_page *fwp;
 	struct rb_node *p;
+	u32 func_id;
 	u32 npages;
 	u32 i = 0;
 
 	if (dev->state != MLX5_DEVICE_STATE_INTERNAL_ERROR)
-		return mlx5_cmd_exec_check_status(dev, (u32 *)in, in_size,
-						  (u32 *)out, out_size);
+		return mlx5_cmd_exec(dev, in, in_size, out, out_size);
 
-	npages = be32_to_cpu(in->num_entries);
+	/* No hard feelings, we want our pages back! */
+	npages = MLX5_GET(manage_pages_in, in, input_num_entries);
+	func_id = MLX5_GET(manage_pages_in, in, function_id);
 
 	p = rb_first(&dev->priv.page_root);
 	while (p && i < npages) {
 		fwp = rb_entry(p, struct fw_page, rb_node);
-		out->pas[i] = cpu_to_be64(fwp->addr);
 		p = rb_next(p);
+		if (fwp->func_id != func_id)
+			continue;
+
+		MLX5_ARRAY_SET64(manage_pages_out, out, pas, i, fwp->addr);
 		i++;
 	}
 
-	out->num_entries = cpu_to_be32(i);
+	MLX5_SET(manage_pages_out, out, output_num_entries, i);
 	return 0;
 }
 
 static int reclaim_pages(struct mlx5_core_dev *dev, u32 func_id, int npages,
 			 int *nclaimed)
 {
-	struct mlx5_manage_pages_inbox   in;
-	struct mlx5_manage_pages_outbox *out;
+	int outlen = MLX5_ST_SZ_BYTES(manage_pages_out);
+	u32 in[MLX5_ST_SZ_DW(manage_pages_in)] = {0};
 	int num_claimed;
-	int outlen;
-	u64 addr;
+	u32 *out;
 	int err;
 	int i;
 
 	if (nclaimed)
 		*nclaimed = 0;
 
-	memset(&in, 0, sizeof(in));
-	outlen = sizeof(*out) + npages * sizeof(out->pas[0]);
+	outlen += npages * MLX5_FLD_SZ_BYTES(manage_pages_out, pas[0]);
 	out = mlx5_vzalloc(outlen);
 	if (!out)
 		return -ENOMEM;
 
-	in.hdr.opcode = cpu_to_be16(MLX5_CMD_OP_MANAGE_PAGES);
-	in.hdr.opmod = cpu_to_be16(MLX5_PAGES_TAKE);
-	in.func_id = cpu_to_be16(func_id);
-	in.num_entries = cpu_to_be32(npages);
+	MLX5_SET(manage_pages_in, in, opcode, MLX5_CMD_OP_MANAGE_PAGES);
+	MLX5_SET(manage_pages_in, in, op_mod, MLX5_PAGES_TAKE);
+	MLX5_SET(manage_pages_in, in, function_id, func_id);
+	MLX5_SET(manage_pages_in, in, input_num_entries, npages);
+
 	mlx5_core_dbg(dev, "npages %d, outlen %d\n", npages, outlen);
-	err = reclaim_pages_cmd(dev, &in, sizeof(in), out, outlen);
+	err = reclaim_pages_cmd(dev, in, sizeof(in), out, outlen);
 	if (err) {
 		mlx5_core_err(dev, "failed reclaiming pages: err %d\n", err);
 		goto out_free;
 	}
 
-	num_claimed = be32_to_cpu(out->num_entries);
+	num_claimed = MLX5_GET(manage_pages_out, out, output_num_entries);
 	if (num_claimed > npages) {
 		mlx5_core_warn(dev, "fw returned %d, driver asked %d => corruption\n",
 			       num_claimed, npages);
@@ -438,10 +390,9 @@ static int reclaim_pages(struct mlx5_core_dev *dev, u32 func_id, int npages,
 		goto out_free;
 	}
 
-	for (i = 0; i < num_claimed; i++) {
-		addr = be64_to_cpu(out->pas[i]);
-		free_4k(dev, addr);
-	}
+	for (i = 0; i < num_claimed; i++)
+		free_4k(dev, MLX5_GET64(manage_pages_out, out, pas[i]));
+
 
 	if (nclaimed)
 		*nclaimed = num_claimed;
@@ -518,8 +469,8 @@ static int optimal_reclaimed_pages(void)
 	int ret;
 
 	ret = (sizeof(lay->out) + MLX5_BLKS_FOR_RECLAIM_PAGES * sizeof(block->data) -
-	       sizeof(struct mlx5_manage_pages_outbox)) /
-	       FIELD_SIZEOF(struct mlx5_manage_pages_outbox, pas[0]);
+	       MLX5_ST_SZ_BYTES(manage_pages_out)) /
+	       MLX5_FLD_SZ_BYTES(manage_pages_out, pas[0]);
 
 	return ret;
 }
@@ -594,6 +545,12 @@ int mlx5_wait_for_vf_pages(struct mlx5_core_dev *dev)
 	unsigned long end = jiffies + msecs_to_jiffies(MAX_RECLAIM_VFS_PAGES_TIME_MSECS);
 	int prev_vfs_pages = dev->priv.vfs_pages;
 
+	/* In case of internal error we will free the pages manually later */
+	if (dev->state == MLX5_DEVICE_STATE_INTERNAL_ERROR) {
+		mlx5_core_warn(dev, "Skipping wait for vf pages stage");
+		return 0;
+	}
+
 	mlx5_core_dbg(dev, "Waiting for %d pages from %s\n", prev_vfs_pages,
 		      dev->priv.name);
 	while (dev->priv.vfs_pages) {
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/pd.c b/drivers/net/ethernet/mellanox/mlx5/core/pd.c
index f2d3aee909e8..bd830d8d6c5f 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/pd.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/pd.c
@@ -36,66 +36,27 @@
 #include <linux/mlx5/cmd.h>
 #include "mlx5_core.h"
 
-struct mlx5_alloc_pd_mbox_in {
-	struct mlx5_inbox_hdr	hdr;
-	u8			rsvd[8];
-};
-
-struct mlx5_alloc_pd_mbox_out {
-	struct mlx5_outbox_hdr	hdr;
-	__be32			pdn;
-	u8			rsvd[4];
-};
-
-struct mlx5_dealloc_pd_mbox_in {
-	struct mlx5_inbox_hdr	hdr;
-	__be32			pdn;
-	u8			rsvd[4];
-};
-
-struct mlx5_dealloc_pd_mbox_out {
-	struct mlx5_outbox_hdr	hdr;
-	u8			rsvd[8];
-};
-
 int mlx5_core_alloc_pd(struct mlx5_core_dev *dev, u32 *pdn)
 {
-	struct mlx5_alloc_pd_mbox_in	in;
-	struct mlx5_alloc_pd_mbox_out	out;
+	u32 out[MLX5_ST_SZ_DW(alloc_pd_out)] = {0};
+	u32 in[MLX5_ST_SZ_DW(alloc_pd_in)]   = {0};
 	int err;
 
-	memset(&in, 0, sizeof(in));
-	memset(&out, 0, sizeof(out));
-	in.hdr.opcode = cpu_to_be16(MLX5_CMD_OP_ALLOC_PD);
-	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);
-
-	*pdn = be32_to_cpu(out.pdn) & 0xffffff;
+	MLX5_SET(alloc_pd_in, in, opcode, MLX5_CMD_OP_ALLOC_PD);
+	err = mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
+	if (!err)
+		*pdn = MLX5_GET(alloc_pd_out, out, pd);
 	return err;
 }
 EXPORT_SYMBOL(mlx5_core_alloc_pd);
 
 int mlx5_core_dealloc_pd(struct mlx5_core_dev *dev, u32 pdn)
 {
-	struct mlx5_dealloc_pd_mbox_in	in;
-	struct mlx5_dealloc_pd_mbox_out	out;
-	int err;
-
-	memset(&in, 0, sizeof(in));
-	memset(&out, 0, sizeof(out));
-	in.hdr.opcode = cpu_to_be16(MLX5_CMD_OP_DEALLOC_PD);
-	in.pdn = cpu_to_be32(pdn);
-	err = mlx5_cmd_exec(dev, &in, sizeof(in), &out, sizeof(out));
-	if (err)
-		return err;
+	u32 out[MLX5_ST_SZ_DW(dealloc_pd_out)] = {0};
+	u32 in[MLX5_ST_SZ_DW(dealloc_pd_in)]   = {0};
 
-	if (out.hdr.status)
-		return mlx5_cmd_status_to_err(&out.hdr);
-
-	return err;
+	MLX5_SET(dealloc_pd_in, in, opcode, MLX5_CMD_OP_DEALLOC_PD);
+	MLX5_SET(dealloc_pd_in, in, pd, pdn);
+	return mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
 }
 EXPORT_SYMBOL(mlx5_core_dealloc_pd);
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/port.c b/drivers/net/ethernet/mellanox/mlx5/core/port.c
index 752c08127138..34e7184e23c9 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/port.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/port.c
@@ -38,45 +38,42 @@
 
 int mlx5_core_access_reg(struct mlx5_core_dev *dev, void *data_in,
 			 int size_in, void *data_out, int size_out,
-			 u16 reg_num, int arg, int write)
+			 u16 reg_id, int arg, int write)
 {
-	struct mlx5_access_reg_mbox_in *in = NULL;
-	struct mlx5_access_reg_mbox_out *out = NULL;
+	int outlen = MLX5_ST_SZ_BYTES(access_register_out) + size_out;
+	int inlen = MLX5_ST_SZ_BYTES(access_register_in) + size_in;
 	int err = -ENOMEM;
+	u32 *out = NULL;
+	u32 *in = NULL;
+	void *data;
 
-	in = mlx5_vzalloc(sizeof(*in) + size_in);
-	if (!in)
-		return -ENOMEM;
-
-	out = mlx5_vzalloc(sizeof(*out) + size_out);
-	if (!out)
-		goto ex1;
-
-	memcpy(in->data, data_in, size_in);
-	in->hdr.opcode = cpu_to_be16(MLX5_CMD_OP_ACCESS_REG);
-	in->hdr.opmod = cpu_to_be16(!write);
-	in->arg = cpu_to_be32(arg);
-	in->register_id = cpu_to_be16(reg_num);
-	err = mlx5_cmd_exec(dev, in, sizeof(*in) + size_in, out,
-			    sizeof(*out) + size_out);
-	if (err)
-		goto ex2;
+	in = mlx5_vzalloc(inlen);
+	out = mlx5_vzalloc(outlen);
+	if (!in || !out)
+		goto out;
 
-	if (out->hdr.status)
-		err = mlx5_cmd_status_to_err(&out->hdr);
+	data = MLX5_ADDR_OF(access_register_in, in, register_data);
+	memcpy(data, data_in, size_in);
 
-	if (!err)
-		memcpy(data_out, out->data, size_out);
+	MLX5_SET(access_register_in, in, opcode, MLX5_CMD_OP_ACCESS_REG);
+	MLX5_SET(access_register_in, in, op_mod, !write);
+	MLX5_SET(access_register_in, in, argument, arg);
+	MLX5_SET(access_register_in, in, register_id, reg_id);
+
+	err = mlx5_cmd_exec(dev, in, inlen, out, outlen);
+	if (err)
+		goto out;
+
+	data = MLX5_ADDR_OF(access_register_out, out, register_data);
+	memcpy(data_out, data, size_out);
 
-ex2:
+out:
 	kvfree(out);
-ex1:
 	kvfree(in);
 	return err;
 }
 EXPORT_SYMBOL_GPL(mlx5_core_access_reg);
 
-
 struct mlx5_reg_pcap {
 	u8			rsvd0;
 	u8			port_num;
@@ -104,12 +101,10 @@ 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)];
+	u32 in[MLX5_ST_SZ_DW(ptys_reg)] = {0};
 
-	memset(in, 0, sizeof(in));
 	MLX5_SET(ptys_reg, in, local_port, local_port);
 	MLX5_SET(ptys_reg, in, proto_mask, proto_mask);
-
 	return mlx5_core_access_reg(dev, in, sizeof(in), ptys,
 				    ptys_size, MLX5_REG_PTYS, 0, 0);
 }
@@ -117,13 +112,11 @@ EXPORT_SYMBOL_GPL(mlx5_query_port_ptys);
 
 int mlx5_set_port_beacon(struct mlx5_core_dev *dev, u16 beacon_duration)
 {
+	u32 in[MLX5_ST_SZ_DW(mlcr_reg)]  = {0};
 	u32 out[MLX5_ST_SZ_DW(mlcr_reg)];
-	u32 in[MLX5_ST_SZ_DW(mlcr_reg)];
 
-	memset(in, 0, sizeof(in));
 	MLX5_SET(mlcr_reg, in, local_port, 1);
 	MLX5_SET(mlcr_reg, in, beacon_duration, beacon_duration);
-
 	return mlx5_core_access_reg(dev, in, sizeof(in), out,
 				    sizeof(out), MLX5_REG_MLCR, 0, 1);
 }
@@ -182,25 +175,39 @@ int mlx5_query_port_link_width_oper(struct mlx5_core_dev *dev,
 }
 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)
+int mlx5_query_port_eth_proto_oper(struct mlx5_core_dev *dev,
+				   u32 *proto_oper, 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);
+	err = mlx5_query_port_ptys(dev, out, sizeof(out), MLX5_PTYS_EN,
+				   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);
+	*proto_oper = MLX5_GET(ptys_reg, out, eth_proto_oper);
+
+	return 0;
+}
+EXPORT_SYMBOL(mlx5_query_port_eth_proto_oper);
+
+int mlx5_query_port_ib_proto_oper(struct mlx5_core_dev *dev,
+				  u8 *proto_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;
+
+	*proto_oper = MLX5_GET(ptys_reg, out, ib_proto_oper);
 
 	return 0;
 }
-EXPORT_SYMBOL_GPL(mlx5_query_port_proto_oper);
+EXPORT_SYMBOL(mlx5_query_port_ib_proto_oper);
 
 int mlx5_set_port_ptys(struct mlx5_core_dev *dev, bool an_disable,
 		       u32 proto_admin, int proto_mask)
@@ -246,15 +253,12 @@ EXPORT_SYMBOL_GPL(mlx5_toggle_port_link);
 int mlx5_set_port_admin_status(struct mlx5_core_dev *dev,
 			       enum mlx5_port_status status)
 {
-	u32 in[MLX5_ST_SZ_DW(paos_reg)];
+	u32 in[MLX5_ST_SZ_DW(paos_reg)] = {0};
 	u32 out[MLX5_ST_SZ_DW(paos_reg)];
 
-	memset(in, 0, sizeof(in));
-
 	MLX5_SET(paos_reg, in, local_port, 1);
 	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);
 }
@@ -263,19 +267,15 @@ EXPORT_SYMBOL_GPL(mlx5_set_port_admin_status);
 int mlx5_query_port_admin_status(struct mlx5_core_dev *dev,
 				 enum mlx5_port_status *status)
 {
-	u32 in[MLX5_ST_SZ_DW(paos_reg)];
+	u32 in[MLX5_ST_SZ_DW(paos_reg)] = {0};
 	u32 out[MLX5_ST_SZ_DW(paos_reg)];
 	int err;
 
-	memset(in, 0, sizeof(in));
-
 	MLX5_SET(paos_reg, in, local_port, 1);
-
 	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, admin_status);
 	return 0;
 }
@@ -284,13 +284,10 @@ EXPORT_SYMBOL_GPL(mlx5_query_port_admin_status);
 static void mlx5_query_port_mtu(struct mlx5_core_dev *dev, u16 *admin_mtu,
 				u16 *max_mtu, u16 *oper_mtu, u8 port)
 {
-	u32 in[MLX5_ST_SZ_DW(pmtu_reg)];
+	u32 in[MLX5_ST_SZ_DW(pmtu_reg)] = {0};
 	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);
 
@@ -304,14 +301,11 @@ static void mlx5_query_port_mtu(struct mlx5_core_dev *dev, u16 *admin_mtu,
 
 int mlx5_set_port_mtu(struct mlx5_core_dev *dev, u16 mtu, u8 port)
 {
-	u32 in[MLX5_ST_SZ_DW(pmtu_reg)];
+	u32 in[MLX5_ST_SZ_DW(pmtu_reg)] = {0};
 	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);
 }
@@ -333,15 +327,12 @@ EXPORT_SYMBOL_GPL(mlx5_query_port_oper_mtu);
 
 static int mlx5_query_module_num(struct mlx5_core_dev *dev, int *module_num)
 {
+	u32 in[MLX5_ST_SZ_DW(pmlp_reg)] = {0};
 	u32 out[MLX5_ST_SZ_DW(pmlp_reg)];
-	u32 in[MLX5_ST_SZ_DW(pmlp_reg)];
 	int module_mapping;
 	int err;
 
-	memset(in, 0, sizeof(in));
-
 	MLX5_SET(pmlp_reg, in, local_port, 1);
-
 	err = mlx5_core_access_reg(dev, in, sizeof(in), out, sizeof(out),
 				   MLX5_REG_PMLP, 0, 0);
 	if (err)
@@ -410,11 +401,9 @@ EXPORT_SYMBOL_GPL(mlx5_query_module_eeprom);
 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)];
+	u32 in[MLX5_ST_SZ_DW(pvlc_reg)] = {0};
 
-	memset(in, 0, sizeof(in));
 	MLX5_SET(pvlc_reg, in, local_port, local_port);
-
 	return mlx5_core_access_reg(dev, in, sizeof(in), pvlc,
 				    pvlc_size, MLX5_REG_PVLC, 0, 0);
 }
@@ -460,10 +449,9 @@ EXPORT_SYMBOL_GPL(mlx5_core_query_ib_ppcnt);
 
 int mlx5_set_port_pause(struct mlx5_core_dev *dev, u32 rx_pause, u32 tx_pause)
 {
-	u32 in[MLX5_ST_SZ_DW(pfcc_reg)];
+	u32 in[MLX5_ST_SZ_DW(pfcc_reg)] = {0};
 	u32 out[MLX5_ST_SZ_DW(pfcc_reg)];
 
-	memset(in, 0, sizeof(in));
 	MLX5_SET(pfcc_reg, in, local_port, 1);
 	MLX5_SET(pfcc_reg, in, pptx, tx_pause);
 	MLX5_SET(pfcc_reg, in, pprx, rx_pause);
@@ -476,13 +464,11 @@ EXPORT_SYMBOL_GPL(mlx5_set_port_pause);
 int mlx5_query_port_pause(struct mlx5_core_dev *dev,
 			  u32 *rx_pause, u32 *tx_pause)
 {
-	u32 in[MLX5_ST_SZ_DW(pfcc_reg)];
+	u32 in[MLX5_ST_SZ_DW(pfcc_reg)] = {0};
 	u32 out[MLX5_ST_SZ_DW(pfcc_reg)];
 	int err;
 
-	memset(in, 0, sizeof(in));
 	MLX5_SET(pfcc_reg, in, local_port, 1);
-
 	err = mlx5_core_access_reg(dev, in, sizeof(in), out,
 				   sizeof(out), MLX5_REG_PFCC, 0, 0);
 	if (err)
@@ -500,10 +486,9 @@ EXPORT_SYMBOL_GPL(mlx5_query_port_pause);
 
 int mlx5_set_port_pfc(struct mlx5_core_dev *dev, u8 pfc_en_tx, u8 pfc_en_rx)
 {
-	u32 in[MLX5_ST_SZ_DW(pfcc_reg)];
+	u32 in[MLX5_ST_SZ_DW(pfcc_reg)] = {0};
 	u32 out[MLX5_ST_SZ_DW(pfcc_reg)];
 
-	memset(in, 0, sizeof(in));
 	MLX5_SET(pfcc_reg, in, local_port, 1);
 	MLX5_SET(pfcc_reg, in, pfctx, pfc_en_tx);
 	MLX5_SET(pfcc_reg, in, pfcrx, pfc_en_rx);
@@ -517,13 +502,11 @@ EXPORT_SYMBOL_GPL(mlx5_set_port_pfc);
 
 int mlx5_query_port_pfc(struct mlx5_core_dev *dev, u8 *pfc_en_tx, u8 *pfc_en_rx)
 {
-	u32 in[MLX5_ST_SZ_DW(pfcc_reg)];
+	u32 in[MLX5_ST_SZ_DW(pfcc_reg)] = {0};
 	u32 out[MLX5_ST_SZ_DW(pfcc_reg)];
 	int err;
 
-	memset(in, 0, sizeof(in));
 	MLX5_SET(pfcc_reg, in, local_port, 1);
-
 	err = mlx5_core_access_reg(dev, in, sizeof(in), out,
 				   sizeof(out), MLX5_REG_PFCC, 0, 0);
 	if (err)
@@ -567,12 +550,11 @@ int mlx5_max_tc(struct mlx5_core_dev *mdev)
 
 int mlx5_set_port_prio_tc(struct mlx5_core_dev *mdev, u8 *prio_tc)
 {
-	u32 in[MLX5_ST_SZ_DW(qtct_reg)];
+	u32 in[MLX5_ST_SZ_DW(qtct_reg)] = {0};
 	u32 out[MLX5_ST_SZ_DW(qtct_reg)];
 	int err;
 	int i;
 
-	memset(in, 0, sizeof(in));
 	for (i = 0; i < 8; i++) {
 		if (prio_tc[i] > mlx5_max_tc(mdev))
 			return -EINVAL;
@@ -617,11 +599,9 @@ static int mlx5_query_port_qetcr_reg(struct mlx5_core_dev *mdev, u32 *out,
 
 int mlx5_set_port_tc_group(struct mlx5_core_dev *mdev, u8 *tc_group)
 {
-	u32 in[MLX5_ST_SZ_DW(qetc_reg)];
+	u32 in[MLX5_ST_SZ_DW(qetc_reg)] = {0};
 	int i;
 
-	memset(in, 0, sizeof(in));
-
 	for (i = 0; i <= mlx5_max_tc(mdev); i++) {
 		MLX5_SET(qetc_reg, in, tc_configuration[i].g, 1);
 		MLX5_SET(qetc_reg, in, tc_configuration[i].group, tc_group[i]);
@@ -633,11 +613,9 @@ EXPORT_SYMBOL_GPL(mlx5_set_port_tc_group);
 
 int mlx5_set_port_tc_bw_alloc(struct mlx5_core_dev *mdev, u8 *tc_bw)
 {
-	u32 in[MLX5_ST_SZ_DW(qetc_reg)];
+	u32 in[MLX5_ST_SZ_DW(qetc_reg)] = {0};
 	int i;
 
-	memset(in, 0, sizeof(in));
-
 	for (i = 0; i <= mlx5_max_tc(mdev); i++) {
 		MLX5_SET(qetc_reg, in, tc_configuration[i].b, 1);
 		MLX5_SET(qetc_reg, in, tc_configuration[i].bw_allocation, tc_bw[i]);
@@ -651,12 +629,10 @@ int mlx5_modify_port_ets_rate_limit(struct mlx5_core_dev *mdev,
 				    u8 *max_bw_value,
 				    u8 *max_bw_units)
 {
-	u32 in[MLX5_ST_SZ_DW(qetc_reg)];
+	u32 in[MLX5_ST_SZ_DW(qetc_reg)] = {0};
 	void *ets_tcn_conf;
 	int i;
 
-	memset(in, 0, sizeof(in));
-
 	MLX5_SET(qetc_reg, in, port_number, 1);
 
 	for (i = 0; i <= mlx5_max_tc(mdev); i++) {
@@ -701,35 +677,24 @@ EXPORT_SYMBOL_GPL(mlx5_query_port_ets_rate_limit);
 
 int mlx5_set_port_wol(struct mlx5_core_dev *mdev, u8 wol_mode)
 {
-	u32 in[MLX5_ST_SZ_DW(set_wol_rol_in)];
-	u32 out[MLX5_ST_SZ_DW(set_wol_rol_out)];
-
-	memset(in, 0, sizeof(in));
-	memset(out, 0, sizeof(out));
+	u32 in[MLX5_ST_SZ_DW(set_wol_rol_in)]   = {0};
+	u32 out[MLX5_ST_SZ_DW(set_wol_rol_out)] = {0};
 
 	MLX5_SET(set_wol_rol_in, in, opcode, MLX5_CMD_OP_SET_WOL_ROL);
 	MLX5_SET(set_wol_rol_in, in, wol_mode_valid, 1);
 	MLX5_SET(set_wol_rol_in, in, wol_mode, wol_mode);
-
-	return mlx5_cmd_exec_check_status(mdev, in, sizeof(in),
-					  out, sizeof(out));
+	return mlx5_cmd_exec(mdev, in, sizeof(in), out, sizeof(out));
 }
 EXPORT_SYMBOL_GPL(mlx5_set_port_wol);
 
 int mlx5_query_port_wol(struct mlx5_core_dev *mdev, u8 *wol_mode)
 {
-	u32 in[MLX5_ST_SZ_DW(query_wol_rol_in)];
-	u32 out[MLX5_ST_SZ_DW(query_wol_rol_out)];
+	u32 in[MLX5_ST_SZ_DW(query_wol_rol_in)]   = {0};
+	u32 out[MLX5_ST_SZ_DW(query_wol_rol_out)] = {0};
 	int err;
 
-	memset(in, 0, sizeof(in));
-	memset(out, 0, sizeof(out));
-
 	MLX5_SET(query_wol_rol_in, in, opcode, MLX5_CMD_OP_QUERY_WOL_ROL);
-
-	err = mlx5_cmd_exec_check_status(mdev, in, sizeof(in),
-					 out, sizeof(out));
-
+	err = mlx5_cmd_exec(mdev, in, sizeof(in), out, sizeof(out));
 	if (!err)
 		*wol_mode = MLX5_GET(query_wol_rol_out, out, wol_mode);
 
@@ -740,11 +705,9 @@ EXPORT_SYMBOL_GPL(mlx5_query_port_wol);
 static int mlx5_query_ports_check(struct mlx5_core_dev *mdev, u32 *out,
 				  int outlen)
 {
-	u32 in[MLX5_ST_SZ_DW(pcmr_reg)];
+	u32 in[MLX5_ST_SZ_DW(pcmr_reg)] = {0};
 
-	memset(in, 0, sizeof(in));
 	MLX5_SET(pcmr_reg, in, local_port, 1);
-
 	return mlx5_core_access_reg(mdev, in, sizeof(in), out,
 				    outlen, MLX5_REG_PCMR, 0, 0);
 }
@@ -759,12 +722,10 @@ static int mlx5_set_ports_check(struct mlx5_core_dev *mdev, u32 *in, int inlen)
 
 int mlx5_set_port_fcs(struct mlx5_core_dev *mdev, u8 enable)
 {
-	u32 in[MLX5_ST_SZ_DW(pcmr_reg)];
+	u32 in[MLX5_ST_SZ_DW(pcmr_reg)] = {0};
 
-	memset(in, 0, sizeof(in));
 	MLX5_SET(pcmr_reg, in, local_port, 1);
 	MLX5_SET(pcmr_reg, in, fcs_chk, enable);
-
 	return mlx5_set_ports_check(mdev, in, sizeof(in));
 }
 
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/qp.c b/drivers/net/ethernet/mellanox/mlx5/core/qp.c
index b82d65802d96..d0a4005fe63a 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/qp.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/qp.c
@@ -271,30 +271,20 @@ static void destroy_qprqsq_common(struct mlx5_core_dev *dev,
 
 int mlx5_core_create_qp(struct mlx5_core_dev *dev,
 			struct mlx5_core_qp *qp,
-			struct mlx5_create_qp_mbox_in *in,
-			int inlen)
+			u32 *in, int inlen)
 {
-	struct mlx5_create_qp_mbox_out out;
-	struct mlx5_destroy_qp_mbox_in din;
-	struct mlx5_destroy_qp_mbox_out dout;
+	u32 out[MLX5_ST_SZ_DW(create_qp_out)] = {0};
+	u32 dout[MLX5_ST_SZ_DW(destroy_qp_out)];
+	u32 din[MLX5_ST_SZ_DW(destroy_qp_in)];
 	int err;
 
-	memset(&out, 0, sizeof(out));
-	in->hdr.opcode = cpu_to_be16(MLX5_CMD_OP_CREATE_QP);
+	MLX5_SET(create_qp_in, in, opcode, MLX5_CMD_OP_CREATE_QP);
 
-	err = mlx5_cmd_exec(dev, in, inlen, &out, sizeof(out));
-	if (err) {
-		mlx5_core_warn(dev, "ret %d\n", err);
+	err = mlx5_cmd_exec(dev, in, inlen, out, sizeof(out));
+	if (err)
 		return err;
-	}
-
-	if (out.hdr.status) {
-		mlx5_core_warn(dev, "current num of QPs 0x%x\n",
-			       atomic_read(&dev->num_qps));
-		return mlx5_cmd_status_to_err(&out.hdr);
-	}
 
-	qp->qpn = be32_to_cpu(out.qpn) & 0xffffff;
+	qp->qpn = MLX5_GET(create_qp_out, out, qpn);
 	mlx5_core_dbg(dev, "qpn = 0x%x\n", qp->qpn);
 
 	err = create_qprqsq_common(dev, qp, MLX5_RES_QP);
@@ -311,12 +301,11 @@ int mlx5_core_create_qp(struct mlx5_core_dev *dev,
 	return 0;
 
 err_cmd:
-	memset(&din, 0, sizeof(din));
-	memset(&dout, 0, sizeof(dout));
-	din.hdr.opcode = cpu_to_be16(MLX5_CMD_OP_DESTROY_QP);
-	din.qpn = cpu_to_be32(qp->qpn);
-	mlx5_cmd_exec(dev, &din, sizeof(din), &out, sizeof(dout));
-
+	memset(din, 0, sizeof(din));
+	memset(dout, 0, sizeof(dout));
+	MLX5_SET(destroy_qp_in, in, opcode, MLX5_CMD_OP_DESTROY_QP);
+	MLX5_SET(destroy_qp_in, in, qpn, qp->qpn);
+	mlx5_cmd_exec(dev, din, sizeof(din), dout, sizeof(dout));
 	return err;
 }
 EXPORT_SYMBOL_GPL(mlx5_core_create_qp);
@@ -324,45 +313,145 @@ EXPORT_SYMBOL_GPL(mlx5_core_create_qp);
 int mlx5_core_destroy_qp(struct mlx5_core_dev *dev,
 			 struct mlx5_core_qp *qp)
 {
-	struct mlx5_destroy_qp_mbox_in in;
-	struct mlx5_destroy_qp_mbox_out out;
+	u32 out[MLX5_ST_SZ_DW(destroy_qp_out)] = {0};
+	u32 in[MLX5_ST_SZ_DW(destroy_qp_in)]   = {0};
 	int err;
 
 	mlx5_debug_qp_remove(dev, qp);
 
 	destroy_qprqsq_common(dev, qp);
 
-	memset(&in, 0, sizeof(in));
-	memset(&out, 0, sizeof(out));
-	in.hdr.opcode = cpu_to_be16(MLX5_CMD_OP_DESTROY_QP);
-	in.qpn = cpu_to_be32(qp->qpn);
-	err = mlx5_cmd_exec(dev, &in, sizeof(in), &out, sizeof(out));
+	MLX5_SET(destroy_qp_in, in, opcode, MLX5_CMD_OP_DESTROY_QP);
+	MLX5_SET(destroy_qp_in, in, qpn, qp->qpn);
+	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);
-
 	atomic_dec(&dev->num_qps);
 	return 0;
 }
 EXPORT_SYMBOL_GPL(mlx5_core_destroy_qp);
 
-int mlx5_core_qp_modify(struct mlx5_core_dev *dev, u16 operation,
-			struct mlx5_modify_qp_mbox_in *in, int sqd_event,
+struct mbox_info {
+	u32 *in;
+	u32 *out;
+	int inlen;
+	int outlen;
+};
+
+static int mbox_alloc(struct mbox_info *mbox, int inlen, int outlen)
+{
+	mbox->inlen  = inlen;
+	mbox->outlen = outlen;
+	mbox->in = kzalloc(mbox->inlen, GFP_KERNEL);
+	mbox->out = kzalloc(mbox->outlen, GFP_KERNEL);
+	if (!mbox->in || !mbox->out) {
+		kfree(mbox->in);
+		kfree(mbox->out);
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+
+static void mbox_free(struct mbox_info *mbox)
+{
+	kfree(mbox->in);
+	kfree(mbox->out);
+}
+
+static int modify_qp_mbox_alloc(struct mlx5_core_dev *dev, u16 opcode, int qpn,
+				u32 opt_param_mask, void *qpc,
+				struct mbox_info *mbox)
+{
+	mbox->out = NULL;
+	mbox->in = NULL;
+
+#define MBOX_ALLOC(mbox, typ)  \
+	mbox_alloc(mbox, MLX5_ST_SZ_BYTES(typ##_in), MLX5_ST_SZ_BYTES(typ##_out))
+
+#define MOD_QP_IN_SET(typ, in, _opcode, _qpn) \
+	MLX5_SET(typ##_in, in, opcode, _opcode); \
+	MLX5_SET(typ##_in, in, qpn, _qpn)
+
+#define MOD_QP_IN_SET_QPC(typ, in, _opcode, _qpn, _opt_p, _qpc) \
+	MOD_QP_IN_SET(typ, in, _opcode, _qpn); \
+	MLX5_SET(typ##_in, in, opt_param_mask, _opt_p); \
+	memcpy(MLX5_ADDR_OF(typ##_in, in, qpc), _qpc, MLX5_ST_SZ_BYTES(qpc))
+
+	switch (opcode) {
+	/* 2RST & 2ERR */
+	case MLX5_CMD_OP_2RST_QP:
+		if (MBOX_ALLOC(mbox, qp_2rst))
+			return -ENOMEM;
+		MOD_QP_IN_SET(qp_2rst, mbox->in, opcode, qpn);
+		break;
+	case MLX5_CMD_OP_2ERR_QP:
+		if (MBOX_ALLOC(mbox, qp_2err))
+			return -ENOMEM;
+		MOD_QP_IN_SET(qp_2err, mbox->in, opcode, qpn);
+		break;
+
+	/* MODIFY with QPC */
+	case MLX5_CMD_OP_RST2INIT_QP:
+		if (MBOX_ALLOC(mbox, rst2init_qp))
+			return -ENOMEM;
+		 MOD_QP_IN_SET_QPC(rst2init_qp, mbox->in, opcode, qpn,
+				   opt_param_mask, qpc);
+		 break;
+	case MLX5_CMD_OP_INIT2RTR_QP:
+		if (MBOX_ALLOC(mbox, init2rtr_qp))
+			return -ENOMEM;
+		 MOD_QP_IN_SET_QPC(init2rtr_qp, mbox->in, opcode, qpn,
+				   opt_param_mask, qpc);
+		 break;
+	case MLX5_CMD_OP_RTR2RTS_QP:
+		if (MBOX_ALLOC(mbox, rtr2rts_qp))
+			return -ENOMEM;
+		 MOD_QP_IN_SET_QPC(rtr2rts_qp, mbox->in, opcode, qpn,
+				   opt_param_mask, qpc);
+		 break;
+	case MLX5_CMD_OP_RTS2RTS_QP:
+		if (MBOX_ALLOC(mbox, rts2rts_qp))
+			return -ENOMEM;
+		MOD_QP_IN_SET_QPC(rts2rts_qp, mbox->in, opcode, qpn,
+				  opt_param_mask, qpc);
+		break;
+	case MLX5_CMD_OP_SQERR2RTS_QP:
+		if (MBOX_ALLOC(mbox, sqerr2rts_qp))
+			return -ENOMEM;
+		MOD_QP_IN_SET_QPC(sqerr2rts_qp, mbox->in, opcode, qpn,
+				  opt_param_mask, qpc);
+		break;
+	case MLX5_CMD_OP_INIT2INIT_QP:
+		if (MBOX_ALLOC(mbox, init2init_qp))
+			return -ENOMEM;
+		MOD_QP_IN_SET_QPC(init2init_qp, mbox->in, opcode, qpn,
+				  opt_param_mask, qpc);
+		break;
+	default:
+		mlx5_core_err(dev, "Unknown transition for modify QP: OP(0x%x) QPN(0x%x)\n",
+			      opcode, qpn);
+		return -EINVAL;
+	}
+	return 0;
+}
+
+int mlx5_core_qp_modify(struct mlx5_core_dev *dev, u16 opcode,
+			u32 opt_param_mask, void *qpc,
 			struct mlx5_core_qp *qp)
 {
-	struct mlx5_modify_qp_mbox_out out;
-	int err = 0;
+	struct mbox_info mbox;
+	int err;
 
-	memset(&out, 0, sizeof(out));
-	in->hdr.opcode = cpu_to_be16(operation);
-	in->qpn = cpu_to_be32(qp->qpn);
-	err = mlx5_cmd_exec(dev, in, sizeof(*in), &out, sizeof(out));
+	err = modify_qp_mbox_alloc(dev, opcode, qp->qpn,
+				   opt_param_mask, qpc, &mbox);
 	if (err)
 		return err;
 
-	return mlx5_cmd_status_to_err(&out.hdr);
+	err = mlx5_cmd_exec(dev, mbox.in, mbox.inlen, mbox.out, mbox.outlen);
+	mbox_free(&mbox);
+	return err;
 }
 EXPORT_SYMBOL_GPL(mlx5_core_qp_modify);
 
@@ -382,66 +471,38 @@ void mlx5_cleanup_qp_table(struct mlx5_core_dev *dev)
 }
 
 int mlx5_core_qp_query(struct mlx5_core_dev *dev, struct mlx5_core_qp *qp,
-		       struct mlx5_query_qp_mbox_out *out, int outlen)
+		       u32 *out, int outlen)
 {
-	struct mlx5_query_qp_mbox_in in;
-	int err;
-
-	memset(&in, 0, sizeof(in));
-	memset(out, 0, outlen);
-	in.hdr.opcode = cpu_to_be16(MLX5_CMD_OP_QUERY_QP);
-	in.qpn = cpu_to_be32(qp->qpn);
-	err = mlx5_cmd_exec(dev, &in, sizeof(in), out, outlen);
-	if (err)
-		return err;
+	u32 in[MLX5_ST_SZ_DW(query_qp_in)] = {0};
 
-	if (out->hdr.status)
-		return mlx5_cmd_status_to_err(&out->hdr);
-
-	return err;
+	MLX5_SET(query_qp_in, in, opcode, MLX5_CMD_OP_QUERY_QP);
+	MLX5_SET(query_qp_in, in, qpn, qp->qpn);
+	return mlx5_cmd_exec(dev, in, sizeof(in), out, outlen);
 }
 EXPORT_SYMBOL_GPL(mlx5_core_qp_query);
 
 int mlx5_core_xrcd_alloc(struct mlx5_core_dev *dev, u32 *xrcdn)
 {
-	struct mlx5_alloc_xrcd_mbox_in in;
-	struct mlx5_alloc_xrcd_mbox_out out;
+	u32 out[MLX5_ST_SZ_DW(alloc_xrcd_out)] = {0};
+	u32 in[MLX5_ST_SZ_DW(alloc_xrcd_in)]   = {0};
 	int err;
 
-	memset(&in, 0, sizeof(in));
-	memset(&out, 0, sizeof(out));
-	in.hdr.opcode = cpu_to_be16(MLX5_CMD_OP_ALLOC_XRCD);
-	err = mlx5_cmd_exec(dev, &in, sizeof(in), &out, sizeof(out));
-	if (err)
-		return err;
-
-	if (out.hdr.status)
-		err = mlx5_cmd_status_to_err(&out.hdr);
-	else
-		*xrcdn = be32_to_cpu(out.xrcdn) & 0xffffff;
-
+	MLX5_SET(alloc_xrcd_in, in, opcode, MLX5_CMD_OP_ALLOC_XRCD);
+	err = mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
+	if (!err)
+		*xrcdn = MLX5_GET(alloc_xrcd_out, out, xrcd);
 	return err;
 }
 EXPORT_SYMBOL_GPL(mlx5_core_xrcd_alloc);
 
 int mlx5_core_xrcd_dealloc(struct mlx5_core_dev *dev, u32 xrcdn)
 {
-	struct mlx5_dealloc_xrcd_mbox_in in;
-	struct mlx5_dealloc_xrcd_mbox_out out;
-	int err;
+	u32 out[MLX5_ST_SZ_DW(dealloc_xrcd_out)] = {0};
+	u32 in[MLX5_ST_SZ_DW(dealloc_xrcd_in)]   = {0};
 
-	memset(&in, 0, sizeof(in));
-	memset(&out, 0, sizeof(out));
-	in.hdr.opcode = cpu_to_be16(MLX5_CMD_OP_DEALLOC_XRCD);
-	in.xrcdn = cpu_to_be32(xrcdn);
-	err = mlx5_cmd_exec(dev, &in, sizeof(in), &out, sizeof(out));
-	if (err)
-		return err;
-
-	if (out.hdr.status)
-		err = mlx5_cmd_status_to_err(&out.hdr);
-
-	return err;
+	MLX5_SET(dealloc_xrcd_in, in, opcode, MLX5_CMD_OP_DEALLOC_XRCD);
+	MLX5_SET(dealloc_xrcd_in, in, xrcd, xrcdn);
+	return mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
 }
 EXPORT_SYMBOL_GPL(mlx5_core_xrcd_dealloc);
 
@@ -449,28 +510,23 @@ EXPORT_SYMBOL_GPL(mlx5_core_xrcd_dealloc);
 int mlx5_core_page_fault_resume(struct mlx5_core_dev *dev, u32 qpn,
 				u8 flags, int error)
 {
-	struct mlx5_page_fault_resume_mbox_in in;
-	struct mlx5_page_fault_resume_mbox_out out;
-	int err;
-
-	memset(&in, 0, sizeof(in));
-	memset(&out, 0, sizeof(out));
-	in.hdr.opcode = cpu_to_be16(MLX5_CMD_OP_PAGE_FAULT_RESUME);
-	in.hdr.opmod = 0;
-	flags &= (MLX5_PAGE_FAULT_RESUME_REQUESTOR |
-		  MLX5_PAGE_FAULT_RESUME_WRITE	   |
-		  MLX5_PAGE_FAULT_RESUME_RDMA);
-	flags |= (error ? MLX5_PAGE_FAULT_RESUME_ERROR : 0);
-	in.flags_qpn = cpu_to_be32((qpn & MLX5_QPN_MASK) |
-				   (flags << MLX5_QPN_BITS));
-	err = mlx5_cmd_exec(dev, &in, sizeof(in), &out, sizeof(out));
-	if (err)
-		return err;
-
-	if (out.hdr.status)
-		err = mlx5_cmd_status_to_err(&out.hdr);
-
-	return err;
+	u32 out[MLX5_ST_SZ_DW(page_fault_resume_out)] = {0};
+	u32 in[MLX5_ST_SZ_DW(page_fault_resume_in)]   = {0};
+
+	MLX5_SET(page_fault_resume_in, in, opcode,
+		 MLX5_CMD_OP_PAGE_FAULT_RESUME);
+	MLX5_SET(page_fault_resume_in, in, qpn, qpn);
+
+	if (flags & MLX5_PAGE_FAULT_RESUME_REQUESTOR)
+		MLX5_SET(page_fault_resume_in, in, req_res, 1);
+	if (flags & MLX5_PAGE_FAULT_RESUME_WRITE)
+		MLX5_SET(page_fault_resume_in, in, read_write, 1);
+	if (flags & MLX5_PAGE_FAULT_RESUME_RDMA)
+		MLX5_SET(page_fault_resume_in, in, rdma, 1);
+	if (error)
+		MLX5_SET(page_fault_resume_in, in, error, 1);
+
+	return mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
 }
 EXPORT_SYMBOL_GPL(mlx5_core_page_fault_resume);
 #endif
@@ -541,15 +597,12 @@ EXPORT_SYMBOL(mlx5_core_destroy_sq_tracked);
 
 int mlx5_core_alloc_q_counter(struct mlx5_core_dev *dev, u16 *counter_id)
 {
-	u32 in[MLX5_ST_SZ_DW(alloc_q_counter_in)];
-	u32 out[MLX5_ST_SZ_DW(alloc_q_counter_out)];
+	u32 in[MLX5_ST_SZ_DW(alloc_q_counter_in)]   = {0};
+	u32 out[MLX5_ST_SZ_DW(alloc_q_counter_out)] = {0};
 	int err;
 
-	memset(in, 0, sizeof(in));
-	memset(out, 0, sizeof(out));
-
 	MLX5_SET(alloc_q_counter_in, in, opcode, MLX5_CMD_OP_ALLOC_Q_COUNTER);
-	err = mlx5_cmd_exec_check_status(dev, in, sizeof(in), out, sizeof(out));
+	err = mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
 	if (!err)
 		*counter_id = MLX5_GET(alloc_q_counter_out, out,
 				       counter_set_id);
@@ -559,31 +612,25 @@ EXPORT_SYMBOL_GPL(mlx5_core_alloc_q_counter);
 
 int mlx5_core_dealloc_q_counter(struct mlx5_core_dev *dev, u16 counter_id)
 {
-	u32 in[MLX5_ST_SZ_DW(dealloc_q_counter_in)];
-	u32 out[MLX5_ST_SZ_DW(dealloc_q_counter_out)];
-
-	memset(in, 0, sizeof(in));
-	memset(out, 0, sizeof(out));
+	u32 in[MLX5_ST_SZ_DW(dealloc_q_counter_in)]   = {0};
+	u32 out[MLX5_ST_SZ_DW(dealloc_q_counter_out)] = {0};
 
 	MLX5_SET(dealloc_q_counter_in, in, opcode,
 		 MLX5_CMD_OP_DEALLOC_Q_COUNTER);
 	MLX5_SET(dealloc_q_counter_in, in, counter_set_id, counter_id);
-	return mlx5_cmd_exec_check_status(dev, in, sizeof(in), out,
-					  sizeof(out));
+	return mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
 }
 EXPORT_SYMBOL_GPL(mlx5_core_dealloc_q_counter);
 
 int mlx5_core_query_q_counter(struct mlx5_core_dev *dev, u16 counter_id,
 			      int reset, void *out, int out_size)
 {
-	u32 in[MLX5_ST_SZ_DW(query_q_counter_in)];
-
-	memset(in, 0, sizeof(in));
+	u32 in[MLX5_ST_SZ_DW(query_q_counter_in)] = {0};
 
 	MLX5_SET(query_q_counter_in, in, opcode, MLX5_CMD_OP_QUERY_Q_COUNTER);
 	MLX5_SET(query_q_counter_in, in, clear, reset);
 	MLX5_SET(query_q_counter_in, in, counter_set_id, counter_id);
-	return mlx5_cmd_exec_check_status(dev, in, sizeof(in), out, out_size);
+	return mlx5_cmd_exec(dev, in, sizeof(in), out, out_size);
 }
 EXPORT_SYMBOL_GPL(mlx5_core_query_q_counter);
 
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/rl.c b/drivers/net/ethernet/mellanox/mlx5/core/rl.c
index c07c28bd3d55..104902a93a0b 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/rl.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/rl.c
@@ -63,19 +63,14 @@ static struct mlx5_rl_entry *find_rl_entry(struct mlx5_rl_table *table,
 static int mlx5_set_rate_limit_cmd(struct mlx5_core_dev *dev,
 				   u32 rate, u16 index)
 {
-	u32 in[MLX5_ST_SZ_DW(set_rate_limit_in)];
-	u32 out[MLX5_ST_SZ_DW(set_rate_limit_out)];
-
-	memset(in, 0, sizeof(in));
-	memset(out, 0, sizeof(out));
+	u32 in[MLX5_ST_SZ_DW(set_rate_limit_in)]   = {0};
+	u32 out[MLX5_ST_SZ_DW(set_rate_limit_out)] = {0};
 
 	MLX5_SET(set_rate_limit_in, in, opcode,
 		 MLX5_CMD_OP_SET_RATE_LIMIT);
 	MLX5_SET(set_rate_limit_in, in, rate_limit_index, index);
 	MLX5_SET(set_rate_limit_in, in, rate_limit, rate);
-
-	return mlx5_cmd_exec_check_status(dev, in, sizeof(in),
-					  out, sizeof(out));
+	return mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
 }
 
 bool mlx5_rl_is_in_range(struct mlx5_core_dev *dev, u32 rate)
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/sriov.c b/drivers/net/ethernet/mellanox/mlx5/core/sriov.c
index b380a6bc1f85..e08627785590 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/sriov.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/sriov.c
@@ -37,198 +37,200 @@
 #include "eswitch.h"
 #endif
 
-static void enable_vfs(struct mlx5_core_dev *dev, int num_vfs)
+bool mlx5_sriov_is_enabled(struct mlx5_core_dev *dev)
+{
+	struct mlx5_core_sriov *sriov = &dev->priv.sriov;
+
+	return !!sriov->num_vfs;
+}
+
+static int mlx5_device_enable_sriov(struct mlx5_core_dev *dev, int num_vfs)
 {
 	struct mlx5_core_sriov *sriov = &dev->priv.sriov;
 	int err;
 	int vf;
 
-	for (vf = 1; vf <= num_vfs; vf++) {
-		err = mlx5_core_enable_hca(dev, vf);
+	if (sriov->enabled_vfs) {
+		mlx5_core_warn(dev,
+			       "failed to enable SRIOV on device, already enabled with %d vfs\n",
+			       sriov->enabled_vfs);
+		return -EBUSY;
+	}
+
+#ifdef CONFIG_MLX5_CORE_EN
+	err = mlx5_eswitch_enable_sriov(dev->priv.eswitch, num_vfs, SRIOV_LEGACY);
+	if (err) {
+		mlx5_core_warn(dev,
+			       "failed to enable eswitch SRIOV (%d)\n", err);
+		return err;
+	}
+#endif
+
+	for (vf = 0; vf < num_vfs; vf++) {
+		err = mlx5_core_enable_hca(dev, vf + 1);
 		if (err) {
-			mlx5_core_warn(dev, "failed to enable VF %d\n", vf - 1);
-		} else {
-			sriov->vfs_ctx[vf - 1].enabled = 1;
-			mlx5_core_dbg(dev, "successfully enabled VF %d\n", vf - 1);
+			mlx5_core_warn(dev, "failed to enable VF %d (%d)\n", vf, err);
+			continue;
 		}
+		sriov->vfs_ctx[vf].enabled = 1;
+		sriov->enabled_vfs++;
+		mlx5_core_dbg(dev, "successfully enabled VF* %d\n", vf);
+
 	}
+
+	return 0;
 }
 
-static void disable_vfs(struct mlx5_core_dev *dev, int num_vfs)
+static void mlx5_device_disable_sriov(struct mlx5_core_dev *dev)
 {
 	struct mlx5_core_sriov *sriov = &dev->priv.sriov;
+	int err;
 	int vf;
 
-	for (vf = 1; vf <= num_vfs; vf++) {
-		if (sriov->vfs_ctx[vf - 1].enabled) {
-			if (mlx5_core_disable_hca(dev, vf))
-				mlx5_core_warn(dev, "failed to disable VF %d\n", vf - 1);
-			else
-				sriov->vfs_ctx[vf - 1].enabled = 0;
+	if (!sriov->enabled_vfs)
+		return;
+
+	for (vf = 0; vf < sriov->num_vfs; vf++) {
+		if (!sriov->vfs_ctx[vf].enabled)
+			continue;
+		err = mlx5_core_disable_hca(dev, vf + 1);
+		if (err) {
+			mlx5_core_warn(dev, "failed to disable VF %d\n", vf);
+			continue;
 		}
+		sriov->vfs_ctx[vf].enabled = 0;
+		sriov->enabled_vfs--;
 	}
+
+#ifdef CONFIG_MLX5_CORE_EN
+	mlx5_eswitch_disable_sriov(dev->priv.eswitch);
+#endif
+
+	if (mlx5_wait_for_vf_pages(dev))
+		mlx5_core_warn(dev, "timeout reclaiming VFs pages\n");
 }
 
-static int mlx5_core_create_vfs(struct pci_dev *pdev, int num_vfs)
+static int mlx5_pci_enable_sriov(struct pci_dev *pdev, int num_vfs)
 {
 	struct mlx5_core_dev *dev  = pci_get_drvdata(pdev);
-	int err;
-
-	if (pci_num_vf(pdev))
-		pci_disable_sriov(pdev);
+	int err = 0;
 
-	enable_vfs(dev, num_vfs);
-
-	err = pci_enable_sriov(pdev, num_vfs);
-	if (err) {
-		dev_warn(&pdev->dev, "enable sriov failed %d\n", err);
-		goto ex;
+	if (pci_num_vf(pdev)) {
+		mlx5_core_warn(dev, "Unable to enable pci sriov, already enabled\n");
+		return -EBUSY;
 	}
 
-	return 0;
+	err = pci_enable_sriov(pdev, num_vfs);
+	if (err)
+		mlx5_core_warn(dev, "pci_enable_sriov failed : %d\n", err);
 
-ex:
-	disable_vfs(dev, num_vfs);
 	return err;
 }
 
-static int mlx5_core_sriov_enable(struct pci_dev *pdev, int num_vfs)
+static void mlx5_pci_disable_sriov(struct pci_dev *pdev)
+{
+	pci_disable_sriov(pdev);
+}
+
+static int mlx5_sriov_enable(struct pci_dev *pdev, int num_vfs)
 {
 	struct mlx5_core_dev *dev  = pci_get_drvdata(pdev);
 	struct mlx5_core_sriov *sriov = &dev->priv.sriov;
-	int err;
+	int err = 0;
 
-	kfree(sriov->vfs_ctx);
-	sriov->vfs_ctx = kcalloc(num_vfs, sizeof(*sriov->vfs_ctx), GFP_ATOMIC);
-	if (!sriov->vfs_ctx)
-		return -ENOMEM;
+	err = mlx5_device_enable_sriov(dev, num_vfs);
+	if (err) {
+		mlx5_core_warn(dev, "mlx5_device_enable_sriov failed : %d\n", err);
+		return err;
+	}
 
-	sriov->enabled_vfs = num_vfs;
-	err = mlx5_core_create_vfs(pdev, num_vfs);
+	err = mlx5_pci_enable_sriov(pdev, num_vfs);
 	if (err) {
-		kfree(sriov->vfs_ctx);
-		sriov->vfs_ctx = NULL;
+		mlx5_core_warn(dev, "mlx5_pci_enable_sriov failed : %d\n", err);
+		mlx5_device_disable_sriov(dev);
 		return err;
 	}
 
+	sriov->num_vfs = num_vfs;
+
 	return 0;
 }
 
-static void mlx5_core_init_vfs(struct mlx5_core_dev *dev, int num_vfs)
+static void mlx5_sriov_disable(struct pci_dev *pdev)
 {
+	struct mlx5_core_dev *dev  = pci_get_drvdata(pdev);
 	struct mlx5_core_sriov *sriov = &dev->priv.sriov;
 
-	sriov->num_vfs = num_vfs;
-}
-
-static void mlx5_core_cleanup_vfs(struct mlx5_core_dev *dev)
-{
-	struct mlx5_core_sriov *sriov;
-
-	sriov = &dev->priv.sriov;
-	disable_vfs(dev, sriov->num_vfs);
-
-	if (mlx5_wait_for_vf_pages(dev))
-		mlx5_core_warn(dev, "timeout claiming VFs pages\n");
-
+	mlx5_pci_disable_sriov(pdev);
+	mlx5_device_disable_sriov(dev);
 	sriov->num_vfs = 0;
 }
 
 int mlx5_core_sriov_configure(struct pci_dev *pdev, int num_vfs)
 {
 	struct mlx5_core_dev *dev  = pci_get_drvdata(pdev);
-	struct mlx5_core_sriov *sriov = &dev->priv.sriov;
-	int err;
+	int err = 0;
 
 	mlx5_core_dbg(dev, "requested num_vfs %d\n", num_vfs);
 	if (!mlx5_core_is_pf(dev))
 		return -EPERM;
 
-	mlx5_core_cleanup_vfs(dev);
-
-	if (!num_vfs) {
-#ifdef CONFIG_MLX5_CORE_EN
-		mlx5_eswitch_disable_sriov(dev->priv.eswitch);
-#endif
-		kfree(sriov->vfs_ctx);
-		sriov->vfs_ctx = NULL;
-		if (!pci_vfs_assigned(pdev))
-			pci_disable_sriov(pdev);
-		else
-			pr_info("unloading PF driver while leaving orphan VFs\n");
-		return 0;
+	if (num_vfs && mlx5_lag_is_active(dev)) {
+		mlx5_core_warn(dev, "can't turn sriov on while LAG is active");
+		return -EINVAL;
 	}
 
-	err = mlx5_core_sriov_enable(pdev, num_vfs);
-	if (err) {
-		dev_warn(&pdev->dev, "mlx5_core_sriov_enable failed %d\n", err);
-		return err;
-	}
+	if (num_vfs)
+		err = mlx5_sriov_enable(pdev, num_vfs);
+	else
+		mlx5_sriov_disable(pdev);
 
-	mlx5_core_init_vfs(dev, num_vfs);
-#ifdef CONFIG_MLX5_CORE_EN
-	mlx5_eswitch_enable_sriov(dev->priv.eswitch, num_vfs, SRIOV_LEGACY);
-#endif
-
-	return num_vfs;
+	return err ? err : num_vfs;
 }
 
-static int sync_required(struct pci_dev *pdev)
+int mlx5_sriov_attach(struct mlx5_core_dev *dev)
 {
-	struct mlx5_core_dev *dev  = pci_get_drvdata(pdev);
 	struct mlx5_core_sriov *sriov = &dev->priv.sriov;
-	int cur_vfs = pci_num_vf(pdev);
 
-	if (cur_vfs != sriov->num_vfs) {
-		pr_info("current VFs %d, registered %d - sync needed\n", cur_vfs, sriov->num_vfs);
-		return 1;
-	}
+	if (!mlx5_core_is_pf(dev) || !sriov->num_vfs)
+		return 0;
 
-	return 0;
+	/* If sriov VFs exist in PCI level, enable them in device level */
+	return mlx5_device_enable_sriov(dev, sriov->num_vfs);
+}
+
+void mlx5_sriov_detach(struct mlx5_core_dev *dev)
+{
+	if (!mlx5_core_is_pf(dev))
+		return;
+
+	mlx5_device_disable_sriov(dev);
 }
 
 int mlx5_sriov_init(struct mlx5_core_dev *dev)
 {
 	struct mlx5_core_sriov *sriov = &dev->priv.sriov;
 	struct pci_dev *pdev = dev->pdev;
-	int cur_vfs;
+	int total_vfs;
 
 	if (!mlx5_core_is_pf(dev))
 		return 0;
 
-	if (!sync_required(dev->pdev))
-		return 0;
-
-	cur_vfs = pci_num_vf(pdev);
-	sriov->vfs_ctx = kcalloc(cur_vfs, sizeof(*sriov->vfs_ctx), GFP_KERNEL);
+	total_vfs = pci_sriov_get_totalvfs(pdev);
+	sriov->num_vfs = pci_num_vf(pdev);
+	sriov->vfs_ctx = kcalloc(total_vfs, sizeof(*sriov->vfs_ctx), GFP_KERNEL);
 	if (!sriov->vfs_ctx)
 		return -ENOMEM;
 
-	sriov->enabled_vfs = cur_vfs;
-
-	mlx5_core_init_vfs(dev, cur_vfs);
-#ifdef CONFIG_MLX5_CORE_EN
-	if (cur_vfs)
-		mlx5_eswitch_enable_sriov(dev->priv.eswitch, cur_vfs,
-					  SRIOV_LEGACY);
-#endif
-
-	enable_vfs(dev, cur_vfs);
-
 	return 0;
 }
 
-int mlx5_sriov_cleanup(struct mlx5_core_dev *dev)
+void mlx5_sriov_cleanup(struct mlx5_core_dev *dev)
 {
-	struct pci_dev *pdev = dev->pdev;
-	int err;
+	struct mlx5_core_sriov *sriov = &dev->priv.sriov;
 
 	if (!mlx5_core_is_pf(dev))
-		return 0;
+		return;
 
-	err = mlx5_core_sriov_configure(pdev, 0);
-	if (err)
-		return err;
-
-	return 0;
+	kfree(sriov->vfs_ctx);
 }
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/srq.c b/drivers/net/ethernet/mellanox/mlx5/core/srq.c
index c07f4d01b70e..3099630015d7 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/srq.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/srq.c
@@ -175,8 +175,8 @@ static int create_srq_cmd(struct mlx5_core_dev *dev, struct mlx5_core_srq *srq,
 	MLX5_SET(create_srq_in, create_in, opcode,
 		 MLX5_CMD_OP_CREATE_SRQ);
 
-	err = mlx5_cmd_exec_check_status(dev, create_in, inlen, create_out,
-					 sizeof(create_out));
+	err = mlx5_cmd_exec(dev, create_in, inlen, create_out,
+			    sizeof(create_out));
 	kvfree(create_in);
 	if (!err)
 		srq->srqn = MLX5_GET(create_srq_out, create_out, srqn);
@@ -194,8 +194,8 @@ static int destroy_srq_cmd(struct mlx5_core_dev *dev,
 		 MLX5_CMD_OP_DESTROY_SRQ);
 	MLX5_SET(destroy_srq_in, srq_in, srqn, srq->srqn);
 
-	return mlx5_cmd_exec_check_status(dev, srq_in, sizeof(srq_in),
-					  srq_out, sizeof(srq_out));
+	return mlx5_cmd_exec(dev, srq_in, sizeof(srq_in),
+			     srq_out, sizeof(srq_out));
 }
 
 static int arm_srq_cmd(struct mlx5_core_dev *dev, struct mlx5_core_srq *srq,
@@ -209,8 +209,8 @@ static int arm_srq_cmd(struct mlx5_core_dev *dev, struct mlx5_core_srq *srq,
 	MLX5_SET(arm_xrc_srq_in, srq_in, xrc_srqn, srq->srqn);
 	MLX5_SET(arm_xrc_srq_in, srq_in, lwm,      lwm);
 
-	return  mlx5_cmd_exec_check_status(dev, srq_in, sizeof(srq_in),
-					   srq_out, sizeof(srq_out));
+	return  mlx5_cmd_exec(dev, srq_in, sizeof(srq_in),
+			      srq_out, sizeof(srq_out));
 }
 
 static int query_srq_cmd(struct mlx5_core_dev *dev, struct mlx5_core_srq *srq,
@@ -228,9 +228,8 @@ static int query_srq_cmd(struct mlx5_core_dev *dev, struct mlx5_core_srq *srq,
 	MLX5_SET(query_srq_in, srq_in, opcode,
 		 MLX5_CMD_OP_QUERY_SRQ);
 	MLX5_SET(query_srq_in, srq_in, srqn, srq->srqn);
-	err =  mlx5_cmd_exec_check_status(dev, srq_in, sizeof(srq_in),
-					  srq_out,
-					  MLX5_ST_SZ_BYTES(query_srq_out));
+	err =  mlx5_cmd_exec(dev, srq_in, sizeof(srq_in),
+			     srq_out, MLX5_ST_SZ_BYTES(query_srq_out));
 	if (err)
 		goto out;
 
@@ -272,8 +271,8 @@ static int create_xrc_srq_cmd(struct mlx5_core_dev *dev,
 		 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));
+	err = mlx5_cmd_exec(dev, create_in, inlen, create_out,
+			    sizeof(create_out));
 	if (err)
 		goto out;
 
@@ -286,36 +285,30 @@ out:
 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));
+	u32 xrcsrq_in[MLX5_ST_SZ_DW(destroy_xrc_srq_in)]   = {0};
+	u32 xrcsrq_out[MLX5_ST_SZ_DW(destroy_xrc_srq_out)] = {0};
 
 	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));
+	return mlx5_cmd_exec(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));
+	u32 xrcsrq_in[MLX5_ST_SZ_DW(arm_xrc_srq_in)]   = {0};
+	u32 xrcsrq_out[MLX5_ST_SZ_DW(arm_xrc_srq_out)] = {0};
 
 	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));
+	return  mlx5_cmd_exec(dev, xrcsrq_in, sizeof(xrcsrq_in),
+			      xrcsrq_out, sizeof(xrcsrq_out));
 }
 
 static int query_xrc_srq_cmd(struct mlx5_core_dev *dev,
@@ -335,9 +328,9 @@ static int query_xrc_srq_cmd(struct mlx5_core_dev *dev,
 	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));
+
+	err =  mlx5_cmd_exec(dev, xrcsrq_in, sizeof(xrcsrq_in), xrcsrq_out,
+			     MLX5_ST_SZ_BYTES(query_xrc_srq_out));
 	if (err)
 		goto out;
 
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/transobj.c b/drivers/net/ethernet/mellanox/mlx5/core/transobj.c
index 28274a6fbafe..a00ff49eec18 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/transobj.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/transobj.c
@@ -36,17 +36,14 @@
 
 int mlx5_core_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)];
+	u32 in[MLX5_ST_SZ_DW(alloc_transport_domain_in)]   = {0};
+	u32 out[MLX5_ST_SZ_DW(alloc_transport_domain_out)] = {0};
 	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));
+	err = mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
 	if (!err)
 		*tdn = MLX5_GET(alloc_transport_domain_out, out,
 				transport_domain);
@@ -57,29 +54,23 @@ EXPORT_SYMBOL(mlx5_core_alloc_transport_domain);
 
 void mlx5_core_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));
+	u32 in[MLX5_ST_SZ_DW(dealloc_transport_domain_in)]   = {0};
+	u32 out[MLX5_ST_SZ_DW(dealloc_transport_domain_out)] = {0};
 
 	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));
+	mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
 }
 EXPORT_SYMBOL(mlx5_core_dealloc_transport_domain);
 
 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)];
+	u32 out[MLX5_ST_SZ_DW(create_rq_out)] = {0};
 	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));
+	err = mlx5_cmd_exec(dev, in, inlen, out, sizeof(out));
 	if (!err)
 		*rqn = MLX5_GET(create_rq_out, out, rqn);
 
@@ -95,21 +86,18 @@ int mlx5_core_modify_rq(struct mlx5_core_dev *dev, u32 rqn, u32 *in, int inlen)
 	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));
+	return mlx5_cmd_exec(dev, in, inlen, out, sizeof(out));
 }
 EXPORT_SYMBOL(mlx5_core_modify_rq);
 
 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));
+	u32 in[MLX5_ST_SZ_DW(destroy_rq_in)]   = {0};
+	u32 out[MLX5_ST_SZ_DW(destroy_rq_out)] = {0};
 
 	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));
+	mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
 }
 EXPORT_SYMBOL(mlx5_core_destroy_rq);
 
@@ -121,19 +109,17 @@ int mlx5_core_query_rq(struct mlx5_core_dev *dev, u32 rqn, u32 *out)
 	MLX5_SET(query_rq_in, in, opcode, MLX5_CMD_OP_QUERY_RQ);
 	MLX5_SET(query_rq_in, in, rqn, rqn);
 
-	return mlx5_cmd_exec_check_status(dev, in, sizeof(in), out, outlen);
+	return mlx5_cmd_exec(dev, in, sizeof(in), out, outlen);
 }
 EXPORT_SYMBOL(mlx5_core_query_rq);
 
 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)];
+	u32 out[MLX5_ST_SZ_DW(create_sq_out)] = {0};
 	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));
+	err = mlx5_cmd_exec(dev, in, inlen, out, sizeof(out));
 	if (!err)
 		*sqn = MLX5_GET(create_sq_out, out, sqn);
 
@@ -142,27 +128,22 @@ 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)
 {
-	u32 out[MLX5_ST_SZ_DW(modify_sq_out)];
+	u32 out[MLX5_ST_SZ_DW(modify_sq_out)] = {0};
 
 	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));
+	return mlx5_cmd_exec(dev, in, inlen, out, sizeof(out));
 }
 EXPORT_SYMBOL(mlx5_core_modify_sq);
 
 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));
+	u32 in[MLX5_ST_SZ_DW(destroy_sq_in)]   = {0};
+	u32 out[MLX5_ST_SZ_DW(destroy_sq_out)] = {0};
 
 	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));
+	mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
 }
 
 int mlx5_core_query_sq(struct mlx5_core_dev *dev, u32 sqn, u32 *out)
@@ -172,21 +153,20 @@ int mlx5_core_query_sq(struct mlx5_core_dev *dev, u32 sqn, u32 *out)
 
 	MLX5_SET(query_sq_in, in, opcode, MLX5_CMD_OP_QUERY_SQ);
 	MLX5_SET(query_sq_in, in, sqn, sqn);
-
-	return mlx5_cmd_exec_check_status(dev, in, sizeof(in), out, outlen);
+	return mlx5_cmd_exec(dev, in, sizeof(in), out, outlen);
 }
 EXPORT_SYMBOL(mlx5_core_query_sq);
 
 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)];
+	u32 out[MLX5_ST_SZ_DW(create_tir_out)] = {0};
 	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));
+	err = mlx5_cmd_exec(dev, in, inlen, out, sizeof(out));
 	if (!err)
 		*tirn = MLX5_GET(create_tir_out, out, tirn);
 
@@ -197,39 +177,32 @@ EXPORT_SYMBOL(mlx5_core_create_tir);
 int mlx5_core_modify_tir(struct mlx5_core_dev *dev, u32 tirn, u32 *in,
 			 int inlen)
 {
-	u32 out[MLX5_ST_SZ_DW(modify_tir_out)];
+	u32 out[MLX5_ST_SZ_DW(modify_tir_out)] = {0};
 
 	MLX5_SET(modify_tir_in, in, tirn, tirn);
 	MLX5_SET(modify_tir_in, in, opcode, MLX5_CMD_OP_MODIFY_TIR);
-
-	memset(out, 0, sizeof(out));
-	return mlx5_cmd_exec_check_status(dev, in, inlen, out, sizeof(out));
+	return mlx5_cmd_exec(dev, in, inlen, out, sizeof(out));
 }
 
 void mlx5_core_destroy_tir(struct mlx5_core_dev *dev, u32 tirn)
 {
-	u32 in[MLX5_ST_SZ_DW(destroy_tir_in)];
-	u32 out[MLX5_ST_SZ_DW(destroy_tir_out)];
-
-	memset(in, 0, sizeof(in));
+	u32 in[MLX5_ST_SZ_DW(destroy_tir_in)]   = {0};
+	u32 out[MLX5_ST_SZ_DW(destroy_tir_out)] = {0};
 
 	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));
+	mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
 }
 EXPORT_SYMBOL(mlx5_core_destroy_tir);
 
 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)];
+	u32 out[MLX5_ST_SZ_DW(create_tis_out)] = {0};
 	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));
+	err = mlx5_cmd_exec(dev, in, inlen, out, sizeof(out));
 	if (!err)
 		*tisn = MLX5_GET(create_tis_out, out, tisn);
 
@@ -245,34 +218,29 @@ int mlx5_core_modify_tis(struct mlx5_core_dev *dev, u32 tisn, u32 *in,
 	MLX5_SET(modify_tis_in, in, tisn, tisn);
 	MLX5_SET(modify_tis_in, in, opcode, MLX5_CMD_OP_MODIFY_TIS);
 
-	return mlx5_cmd_exec_check_status(dev, in, inlen, out, sizeof(out));
+	return mlx5_cmd_exec(dev, in, inlen, out, sizeof(out));
 }
 EXPORT_SYMBOL(mlx5_core_modify_tis);
 
 void mlx5_core_destroy_tis(struct mlx5_core_dev *dev, u32 tisn)
 {
-	u32 in[MLX5_ST_SZ_DW(destroy_tis_in)];
-	u32 out[MLX5_ST_SZ_DW(destroy_tis_out)];
-
-	memset(in, 0, sizeof(in));
+	u32 in[MLX5_ST_SZ_DW(destroy_tis_in)]   = {0};
+	u32 out[MLX5_ST_SZ_DW(destroy_tis_out)] = {0};
 
 	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));
+	mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
 }
 EXPORT_SYMBOL(mlx5_core_destroy_tis);
 
 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)];
+	u32 out[MLX5_ST_SZ_DW(create_rmp_out)] = {0};
 	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));
+	err = mlx5_cmd_exec(dev, in, inlen, out, sizeof(out));
 	if (!err)
 		*rmpn = MLX5_GET(create_rmp_out, out, rmpn);
 
@@ -281,38 +249,31 @@ int mlx5_core_create_rmp(struct mlx5_core_dev *dev, u32 *in, int inlen,
 
 int mlx5_core_modify_rmp(struct mlx5_core_dev *dev, u32 *in, int inlen)
 {
-	u32 out[MLX5_ST_SZ_DW(modify_rmp_out)];
+	u32 out[MLX5_ST_SZ_DW(modify_rmp_out)] = {0};
 
 	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));
+	return mlx5_cmd_exec(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));
+	u32 in[MLX5_ST_SZ_DW(destroy_rmp_in)]   = {0};
+	u32 out[MLX5_ST_SZ_DW(destroy_rmp_out)] = {0};
 
 	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,
+	return mlx5_cmd_exec(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)];
+	u32 in[MLX5_ST_SZ_DW(query_rmp_in)] = {0};
 	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);
+	return mlx5_cmd_exec(dev, in, sizeof(in), out, outlen);
 }
 
 int mlx5_core_arm_rmp(struct mlx5_core_dev *dev, u32 rmpn, u16 lwm)
@@ -347,13 +308,11 @@ 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 *xsrqn)
 {
-	u32 out[MLX5_ST_SZ_DW(create_xrc_srq_out)];
+	u32 out[MLX5_ST_SZ_DW(create_xrc_srq_out)] = {0};
 	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));
+	err = mlx5_cmd_exec(dev, in, inlen, out, sizeof(out));
 	if (!err)
 		*xsrqn = MLX5_GET(create_xrc_srq_out, out, xrc_srqn);
 
@@ -362,33 +321,25 @@ int mlx5_core_create_xsrq(struct mlx5_core_dev *dev, u32 *in, int inlen,
 
 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));
+	u32 in[MLX5_ST_SZ_DW(destroy_xrc_srq_in)]   = {0};
+	u32 out[MLX5_ST_SZ_DW(destroy_xrc_srq_out)] = {0};
 
 	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));
+	return mlx5_cmd_exec(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)];
+	u32 in[MLX5_ST_SZ_DW(query_xrc_srq_in)] = {0};
 	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));
+	err = mlx5_cmd_exec(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);
@@ -401,32 +352,25 @@ int mlx5_core_query_xsrq(struct mlx5_core_dev *dev, u32 xsrqn, u32 *out)
 
 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));
+	u32 in[MLX5_ST_SZ_DW(arm_xrc_srq_in)]   = {0};
+	u32 out[MLX5_ST_SZ_DW(arm_xrc_srq_out)] = {0};
 
 	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));
+	return mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
 }
 
 int mlx5_core_create_rqt(struct mlx5_core_dev *dev, u32 *in, int inlen,
 			 u32 *rqtn)
 {
-	u32 out[MLX5_ST_SZ_DW(create_rqt_out)];
+	u32 out[MLX5_ST_SZ_DW(create_rqt_out)] = {0};
 	int err;
 
 	MLX5_SET(create_rqt_in, in, opcode, MLX5_CMD_OP_CREATE_RQT);
-
-	memset(out, 0, sizeof(out));
-	err = mlx5_cmd_exec_check_status(dev, in, inlen, out, sizeof(out));
+	err = mlx5_cmd_exec(dev, in, inlen, out, sizeof(out));
 	if (!err)
 		*rqtn = MLX5_GET(create_rqt_out, out, rqtn);
 
@@ -437,25 +381,20 @@ EXPORT_SYMBOL(mlx5_core_create_rqt);
 int mlx5_core_modify_rqt(struct mlx5_core_dev *dev, u32 rqtn, u32 *in,
 			 int inlen)
 {
-	u32 out[MLX5_ST_SZ_DW(modify_rqt_out)];
+	u32 out[MLX5_ST_SZ_DW(modify_rqt_out)] = {0};
 
 	MLX5_SET(modify_rqt_in, in, rqtn, rqtn);
 	MLX5_SET(modify_rqt_in, in, opcode, MLX5_CMD_OP_MODIFY_RQT);
-
-	memset(out, 0, sizeof(out));
-	return mlx5_cmd_exec_check_status(dev, in, inlen, out, sizeof(out));
+	return mlx5_cmd_exec(dev, in, inlen, out, sizeof(out));
 }
 
 void mlx5_core_destroy_rqt(struct mlx5_core_dev *dev, u32 rqtn)
 {
-	u32 in[MLX5_ST_SZ_DW(destroy_rqt_in)];
-	u32 out[MLX5_ST_SZ_DW(destroy_rqt_out)];
-
-	memset(in, 0, sizeof(in));
+	u32 in[MLX5_ST_SZ_DW(destroy_rqt_in)]   = {0};
+	u32 out[MLX5_ST_SZ_DW(destroy_rqt_out)] = {0};
 
 	MLX5_SET(destroy_rqt_in, in, opcode, MLX5_CMD_OP_DESTROY_RQT);
 	MLX5_SET(destroy_rqt_in, in, rqtn, rqtn);
-
-	mlx5_cmd_exec_check_status(dev, in, sizeof(in), out, sizeof(out));
+	mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
 }
 EXPORT_SYMBOL(mlx5_core_destroy_rqt);
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/uar.c b/drivers/net/ethernet/mellanox/mlx5/core/uar.c
index 5ff8af472bf5..ab0b896621a0 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/uar.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/uar.c
@@ -42,73 +42,28 @@ enum {
 	NUM_LOW_LAT_UUARS	= 4,
 };
 
-
-struct mlx5_alloc_uar_mbox_in {
-	struct mlx5_inbox_hdr	hdr;
-	u8			rsvd[8];
-};
-
-struct mlx5_alloc_uar_mbox_out {
-	struct mlx5_outbox_hdr	hdr;
-	__be32			uarn;
-	u8			rsvd[4];
-};
-
-struct mlx5_free_uar_mbox_in {
-	struct mlx5_inbox_hdr	hdr;
-	__be32			uarn;
-	u8			rsvd[4];
-};
-
-struct mlx5_free_uar_mbox_out {
-	struct mlx5_outbox_hdr	hdr;
-	u8			rsvd[8];
-};
-
 int mlx5_cmd_alloc_uar(struct mlx5_core_dev *dev, u32 *uarn)
 {
-	struct mlx5_alloc_uar_mbox_in	in;
-	struct mlx5_alloc_uar_mbox_out	out;
+	u32 out[MLX5_ST_SZ_DW(alloc_uar_out)] = {0};
+	u32 in[MLX5_ST_SZ_DW(alloc_uar_in)]   = {0};
 	int err;
 
-	memset(&in, 0, sizeof(in));
-	memset(&out, 0, sizeof(out));
-	in.hdr.opcode = cpu_to_be16(MLX5_CMD_OP_ALLOC_UAR);
-	err = mlx5_cmd_exec(dev, &in, sizeof(in), &out, sizeof(out));
-	if (err)
-		goto ex;
-
-	if (out.hdr.status) {
-		err = mlx5_cmd_status_to_err(&out.hdr);
-		goto ex;
-	}
-
-	*uarn = be32_to_cpu(out.uarn) & 0xffffff;
-
-ex:
+	MLX5_SET(alloc_uar_in, in, opcode, MLX5_CMD_OP_ALLOC_UAR);
+	err = mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
+	if (!err)
+		*uarn = MLX5_GET(alloc_uar_out, out, uar);
 	return err;
 }
 EXPORT_SYMBOL(mlx5_cmd_alloc_uar);
 
 int mlx5_cmd_free_uar(struct mlx5_core_dev *dev, u32 uarn)
 {
-	struct mlx5_free_uar_mbox_in	in;
-	struct mlx5_free_uar_mbox_out	out;
-	int err;
-
-	memset(&in, 0, sizeof(in));
-	memset(&out, 0, sizeof(out));
-	in.hdr.opcode = cpu_to_be16(MLX5_CMD_OP_DEALLOC_UAR);
-	in.uarn = cpu_to_be32(uarn);
-	err = mlx5_cmd_exec(dev, &in, sizeof(in), &out, sizeof(out));
-	if (err)
-		goto ex;
+	u32 out[MLX5_ST_SZ_DW(dealloc_uar_out)] = {0};
+	u32 in[MLX5_ST_SZ_DW(dealloc_uar_in)]   = {0};
 
-	if (out.hdr.status)
-		err = mlx5_cmd_status_to_err(&out.hdr);
-
-ex:
-	return err;
+	MLX5_SET(dealloc_uar_in, in, opcode, MLX5_CMD_OP_DEALLOC_UAR);
+	MLX5_SET(dealloc_uar_in, in, uar, uarn);
+	return mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
 }
 EXPORT_SYMBOL(mlx5_cmd_free_uar);
 
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/vport.c b/drivers/net/ethernet/mellanox/mlx5/core/vport.c
index 21365d06982b..525f17af108e 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/vport.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/vport.c
@@ -39,10 +39,7 @@
 static int _mlx5_query_vport_state(struct mlx5_core_dev *mdev, u8 opmod,
 				   u16 vport, u32 *out, int outlen)
 {
-	int err;
-	u32 in[MLX5_ST_SZ_DW(query_vport_state_in)];
-
-	memset(in, 0, sizeof(in));
+	u32 in[MLX5_ST_SZ_DW(query_vport_state_in)] = {0};
 
 	MLX5_SET(query_vport_state_in, in, opcode,
 		 MLX5_CMD_OP_QUERY_VPORT_STATE);
@@ -51,11 +48,7 @@ static int _mlx5_query_vport_state(struct mlx5_core_dev *mdev, u8 opmod,
 	if (vport)
 		MLX5_SET(query_vport_state_in, in, other_vport, 1);
 
-	err = mlx5_cmd_exec_check_status(mdev, in, sizeof(in), out, outlen);
-	if (err)
-		mlx5_core_warn(mdev, "MLX5_CMD_OP_QUERY_VPORT_STATE failed\n");
-
-	return err;
+	return mlx5_cmd_exec(mdev, in, sizeof(in), out, outlen);
 }
 
 u8 mlx5_query_vport_state(struct mlx5_core_dev *mdev, u8 opmod, u16 vport)
@@ -81,58 +74,43 @@ EXPORT_SYMBOL_GPL(mlx5_query_vport_admin_state);
 int mlx5_modify_vport_admin_state(struct mlx5_core_dev *mdev, u8 opmod,
 				  u16 vport, u8 state)
 {
-	u32 in[MLX5_ST_SZ_DW(modify_vport_state_in)];
-	u32 out[MLX5_ST_SZ_DW(modify_vport_state_out)];
-	int err;
-
-	memset(in, 0, sizeof(in));
+	u32 in[MLX5_ST_SZ_DW(modify_vport_state_in)]   = {0};
+	u32 out[MLX5_ST_SZ_DW(modify_vport_state_out)] = {0};
 
 	MLX5_SET(modify_vport_state_in, in, opcode,
 		 MLX5_CMD_OP_MODIFY_VPORT_STATE);
 	MLX5_SET(modify_vport_state_in, in, op_mod, opmod);
 	MLX5_SET(modify_vport_state_in, in, vport_number, vport);
-
 	if (vport)
 		MLX5_SET(modify_vport_state_in, in, other_vport, 1);
-
 	MLX5_SET(modify_vport_state_in, in, admin_state, state);
 
-	err = mlx5_cmd_exec_check_status(mdev, in, sizeof(in), out,
-					 sizeof(out));
-	if (err)
-		mlx5_core_warn(mdev, "MLX5_CMD_OP_MODIFY_VPORT_STATE failed\n");
-
-	return err;
+	return mlx5_cmd_exec(mdev, in, sizeof(in), out, sizeof(out));
 }
 EXPORT_SYMBOL_GPL(mlx5_modify_vport_admin_state);
 
 static int mlx5_query_nic_vport_context(struct mlx5_core_dev *mdev, u16 vport,
 					u32 *out, int outlen)
 {
-	u32 in[MLX5_ST_SZ_DW(query_nic_vport_context_in)];
-
-	memset(in, 0, sizeof(in));
+	u32 in[MLX5_ST_SZ_DW(query_nic_vport_context_in)] = {0};
 
 	MLX5_SET(query_nic_vport_context_in, in, opcode,
 		 MLX5_CMD_OP_QUERY_NIC_VPORT_CONTEXT);
-
 	MLX5_SET(query_nic_vport_context_in, in, vport_number, vport);
 	if (vport)
 		MLX5_SET(query_nic_vport_context_in, in, other_vport, 1);
 
-	return mlx5_cmd_exec_check_status(mdev, in, sizeof(in), out, outlen);
+	return mlx5_cmd_exec(mdev, in, sizeof(in), out, outlen);
 }
 
 static int mlx5_modify_nic_vport_context(struct mlx5_core_dev *mdev, void *in,
 					 int inlen)
 {
-	u32 out[MLX5_ST_SZ_DW(modify_nic_vport_context_out)];
+	u32 out[MLX5_ST_SZ_DW(modify_nic_vport_context_out)] = {0};
 
 	MLX5_SET(modify_nic_vport_context_in, in, opcode,
 		 MLX5_CMD_OP_MODIFY_NIC_VPORT_CONTEXT);
-
-	memset(out, 0, sizeof(out));
-	return mlx5_cmd_exec_check_status(mdev, in, inlen, out, sizeof(out));
+	return mlx5_cmd_exec(mdev, in, inlen, out, sizeof(out));
 }
 
 void mlx5_query_nic_vport_min_inline(struct mlx5_core_dev *mdev,
@@ -147,6 +125,26 @@ void mlx5_query_nic_vport_min_inline(struct mlx5_core_dev *mdev,
 }
 EXPORT_SYMBOL_GPL(mlx5_query_nic_vport_min_inline);
 
+int mlx5_modify_nic_vport_min_inline(struct mlx5_core_dev *mdev,
+				     u16 vport, u8 min_inline)
+{
+	u32 in[MLX5_ST_SZ_DW(modify_nic_vport_context_in)] = {0};
+	int inlen = MLX5_ST_SZ_BYTES(modify_nic_vport_context_in);
+	void *nic_vport_ctx;
+
+	MLX5_SET(modify_nic_vport_context_in, in,
+		 field_select.min_inline, 1);
+	MLX5_SET(modify_nic_vport_context_in, in, vport_number, vport);
+	MLX5_SET(modify_nic_vport_context_in, in, other_vport, 1);
+
+	nic_vport_ctx = MLX5_ADDR_OF(modify_nic_vport_context_in,
+				     in, nic_vport_context);
+	MLX5_SET(nic_vport_context, nic_vport_ctx,
+		 min_wqe_inline_mode, min_inline);
+
+	return mlx5_modify_nic_vport_context(mdev, in, inlen);
+}
+
 int mlx5_query_nic_vport_mac_address(struct mlx5_core_dev *mdev,
 				     u16 vport, u8 *addr)
 {
@@ -254,7 +252,7 @@ int mlx5_query_nic_vport_mac_list(struct mlx5_core_dev *dev,
 				  u8 addr_list[][ETH_ALEN],
 				  int *list_size)
 {
-	u32 in[MLX5_ST_SZ_DW(query_nic_vport_context_in)];
+	u32 in[MLX5_ST_SZ_DW(query_nic_vport_context_in)] = {0};
 	void *nic_vport_ctx;
 	int max_list_size;
 	int req_list_size;
@@ -278,7 +276,6 @@ int mlx5_query_nic_vport_mac_list(struct mlx5_core_dev *dev,
 	out_sz = MLX5_ST_SZ_BYTES(modify_nic_vport_context_in) +
 			req_list_size * MLX5_ST_SZ_BYTES(mac_address_layout);
 
-	memset(in, 0, sizeof(in));
 	out = kzalloc(out_sz, GFP_KERNEL);
 	if (!out)
 		return -ENOMEM;
@@ -291,7 +288,7 @@ int mlx5_query_nic_vport_mac_list(struct mlx5_core_dev *dev,
 	if (vport)
 		MLX5_SET(query_nic_vport_context_in, in, other_vport, 1);
 
-	err = mlx5_cmd_exec_check_status(dev, in, sizeof(in), out, out_sz);
+	err = mlx5_cmd_exec(dev, in, sizeof(in), out, out_sz);
 	if (err)
 		goto out;
 
@@ -361,7 +358,7 @@ int mlx5_modify_nic_vport_mac_list(struct mlx5_core_dev *dev,
 		ether_addr_copy(curr_mac, addr_list[i]);
 	}
 
-	err = mlx5_cmd_exec_check_status(dev, in, in_sz, out, sizeof(out));
+	err = mlx5_cmd_exec(dev, in, in_sz, out, sizeof(out));
 	kfree(in);
 	return err;
 }
@@ -406,7 +403,7 @@ int mlx5_query_nic_vport_vlans(struct mlx5_core_dev *dev,
 	if (vport)
 		MLX5_SET(query_nic_vport_context_in, in, other_vport, 1);
 
-	err = mlx5_cmd_exec_check_status(dev, in, sizeof(in), out, out_sz);
+	err = mlx5_cmd_exec(dev, in, sizeof(in), out, out_sz);
 	if (err)
 		goto out;
 
@@ -473,7 +470,7 @@ int mlx5_modify_nic_vport_vlans(struct mlx5_core_dev *dev,
 		MLX5_SET(vlan_layout, vlan_addr, vlan, vlans[i]);
 	}
 
-	err = mlx5_cmd_exec_check_status(dev, in, in_sz, out, sizeof(out));
+	err = mlx5_cmd_exec(dev, in, in_sz, out, sizeof(out));
 	kfree(in);
 	return err;
 }
@@ -631,10 +628,6 @@ int mlx5_query_hca_vport_gid(struct mlx5_core_dev *dev, u8 other_vport,
 	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;
@@ -700,10 +693,6 @@ int mlx5_query_hca_vport_pkey(struct mlx5_core_dev *dev, u8 other_vport,
 	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);
@@ -721,7 +710,7 @@ int mlx5_query_hca_vport_context(struct mlx5_core_dev *dev,
 				 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 in[MLX5_ST_SZ_DW(query_hca_vport_context_in)] = {0};
 	int is_group_manager;
 	void *out;
 	void *ctx;
@@ -729,7 +718,6 @@ int mlx5_query_hca_vport_context(struct mlx5_core_dev *dev,
 
 	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;
@@ -752,9 +740,6 @@ int mlx5_query_hca_vport_context(struct mlx5_core_dev *dev,
 	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);
@@ -969,10 +954,6 @@ int mlx5_core_query_vport_counter(struct mlx5_core_dev *dev, u8 other_vport,
 		MLX5_SET(query_vport_counter_in, in, port_num, port_num);
 
 	err = mlx5_cmd_exec(dev, in, in_sz, out,  out_sz);
-	if (err)
-		goto free;
-	err = mlx5_cmd_status_to_err_v2(out);
-
 free:
 	kvfree(in);
 	return err;
@@ -1035,11 +1016,6 @@ int mlx5_core_modify_hca_vport_context(struct mlx5_core_dev *dev,
 	MLX5_SET(hca_vport_context, ctx, qkey_violation_counter, req->qkey_violation_counter);
 	MLX5_SET(hca_vport_context, ctx, pkey_violation_counter, req->pkey_violation_counter);
 	err = mlx5_cmd_exec(dev, in, in_sz, out, sizeof(out));
-	if (err)
-		goto ex;
-
-	err = mlx5_cmd_status_to_err_v2(out);
-
 ex:
 	kfree(in);
 	return err;
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/vxlan.c b/drivers/net/ethernet/mellanox/mlx5/core/vxlan.c
index e25a73ed2981..07a9ba6cfc70 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/vxlan.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/vxlan.c
@@ -46,41 +46,24 @@ void mlx5e_vxlan_init(struct mlx5e_priv *priv)
 
 static int mlx5e_vxlan_core_add_port_cmd(struct mlx5_core_dev *mdev, u16 port)
 {
-	struct mlx5_outbox_hdr *hdr;
-	int err;
-
-	u32 in[MLX5_ST_SZ_DW(add_vxlan_udp_dport_in)];
-	u32 out[MLX5_ST_SZ_DW(add_vxlan_udp_dport_out)];
-
-	memset(in, 0, sizeof(in));
-	memset(out, 0, sizeof(out));
+	u32 in[MLX5_ST_SZ_DW(add_vxlan_udp_dport_in)]   = {0};
+	u32 out[MLX5_ST_SZ_DW(add_vxlan_udp_dport_out)] = {0};
 
 	MLX5_SET(add_vxlan_udp_dport_in, in, opcode,
 		 MLX5_CMD_OP_ADD_VXLAN_UDP_DPORT);
 	MLX5_SET(add_vxlan_udp_dport_in, in, vxlan_udp_port, port);
-
-	err = mlx5_cmd_exec(mdev, in, sizeof(in), out, sizeof(out));
-	if (err)
-		return err;
-
-	hdr = (struct mlx5_outbox_hdr *)out;
-	return hdr->status ? -ENOMEM : 0;
+	return mlx5_cmd_exec(mdev, in, sizeof(in), out, sizeof(out));
 }
 
 static int mlx5e_vxlan_core_del_port_cmd(struct mlx5_core_dev *mdev, u16 port)
 {
-	u32 in[MLX5_ST_SZ_DW(delete_vxlan_udp_dport_in)];
-	u32 out[MLX5_ST_SZ_DW(delete_vxlan_udp_dport_out)];
-
-	memset(in, 0, sizeof(in));
-	memset(out, 0, sizeof(out));
+	u32 in[MLX5_ST_SZ_DW(delete_vxlan_udp_dport_in)]   = {0};
+	u32 out[MLX5_ST_SZ_DW(delete_vxlan_udp_dport_out)] = {0};
 
 	MLX5_SET(delete_vxlan_udp_dport_in, in, opcode,
 		 MLX5_CMD_OP_DELETE_VXLAN_UDP_DPORT);
 	MLX5_SET(delete_vxlan_udp_dport_in, in, vxlan_udp_port, port);
-
-	return mlx5_cmd_exec_check_status(mdev, in, sizeof(in), out,
-					  sizeof(out));
+	return mlx5_cmd_exec(mdev, in, sizeof(in), out, sizeof(out));
 }
 
 struct mlx5e_vxlan *mlx5e_vxlan_lookup_port(struct mlx5e_priv *priv, u16 port)
diff --git a/drivers/net/ethernet/mellanox/mlxsw/core.c b/drivers/net/ethernet/mellanox/mlxsw/core.c
index 068ee65a960b..aa33d58b9f81 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/core.c
+++ b/drivers/net/ethernet/mellanox/mlxsw/core.c
@@ -1100,10 +1100,15 @@ int mlxsw_core_bus_device_register(const struct mlxsw_bus_info *mlxsw_bus_info,
 		goto err_alloc_stats;
 	}
 
-	if (mlxsw_driver->profile->used_max_lag &&
-	    mlxsw_driver->profile->used_max_port_per_lag) {
-		alloc_size = sizeof(u8) * mlxsw_driver->profile->max_lag *
-			     mlxsw_driver->profile->max_port_per_lag;
+	err = mlxsw_bus->init(bus_priv, mlxsw_core, mlxsw_driver->profile,
+			      &mlxsw_core->resources);
+	if (err)
+		goto err_bus_init;
+
+	if (mlxsw_core->resources.max_lag_valid &&
+	    mlxsw_core->resources.max_ports_in_lag_valid) {
+		alloc_size = sizeof(u8) * mlxsw_core->resources.max_lag *
+			mlxsw_core->resources.max_ports_in_lag;
 		mlxsw_core->lag.mapping = kzalloc(alloc_size, GFP_KERNEL);
 		if (!mlxsw_core->lag.mapping) {
 			err = -ENOMEM;
@@ -1111,11 +1116,6 @@ int mlxsw_core_bus_device_register(const struct mlxsw_bus_info *mlxsw_bus_info,
 		}
 	}
 
-	err = mlxsw_bus->init(bus_priv, mlxsw_core, mlxsw_driver->profile,
-			      &mlxsw_core->resources);
-	if (err)
-		goto err_bus_init;
-
 	err = mlxsw_emad_init(mlxsw_core);
 	if (err)
 		goto err_emad_init;
@@ -1146,10 +1146,10 @@ err_hwmon_init:
 err_devlink_register:
 	mlxsw_emad_fini(mlxsw_core);
 err_emad_init:
-	mlxsw_bus->fini(bus_priv);
-err_bus_init:
 	kfree(mlxsw_core->lag.mapping);
 err_alloc_lag_mapping:
+	mlxsw_bus->fini(bus_priv);
+err_bus_init:
 	free_percpu(mlxsw_core->pcpu_stats);
 err_alloc_stats:
 	devlink_free(devlink);
@@ -1615,7 +1615,7 @@ EXPORT_SYMBOL(mlxsw_core_skb_receive);
 static int mlxsw_core_lag_mapping_index(struct mlxsw_core *mlxsw_core,
 					u16 lag_id, u8 port_index)
 {
-	return mlxsw_core->driver->profile->max_port_per_lag * lag_id +
+	return mlxsw_core->resources.max_ports_in_lag * lag_id +
 	       port_index;
 }
 
@@ -1644,7 +1644,7 @@ void mlxsw_core_lag_mapping_clear(struct mlxsw_core *mlxsw_core,
 {
 	int i;
 
-	for (i = 0; i < mlxsw_core->driver->profile->max_port_per_lag; i++) {
+	for (i = 0; i < mlxsw_core->resources.max_ports_in_lag; i++) {
 		int index = mlxsw_core_lag_mapping_index(mlxsw_core,
 							 lag_id, i);
 
diff --git a/drivers/net/ethernet/mellanox/mlxsw/core.h b/drivers/net/ethernet/mellanox/mlxsw/core.h
index d3476ead9982..c4f550b6f783 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/core.h
+++ b/drivers/net/ethernet/mellanox/mlxsw/core.h
@@ -87,6 +87,7 @@ struct mlxsw_rx_listener {
 	void (*func)(struct sk_buff *skb, u8 local_port, void *priv);
 	u8 local_port;
 	u16 trap_id;
+	enum mlxsw_reg_hpkt_action action;
 };
 
 struct mlxsw_event_listener {
@@ -178,8 +179,6 @@ struct mlxsw_swid_config {
 
 struct mlxsw_config_profile {
 	u16	used_max_vepa_channels:1,
-		used_max_lag:1,
-		used_max_port_per_lag:1,
 		used_max_mid:1,
 		used_max_pgt:1,
 		used_max_system_port:1,
@@ -191,10 +190,9 @@ struct mlxsw_config_profile {
 		used_max_pkey:1,
 		used_ar_sec:1,
 		used_adaptive_routing_group_cap:1,
-		used_kvd_sizes:1;
+		used_kvd_split_data:1; /* indicate for the kvd's values */
+
 	u8	max_vepa_channels;
-	u16	max_lag;
-	u16	max_port_per_lag;
 	u16	max_mid;
 	u16	max_pgt;
 	u16	max_system_port;
@@ -213,8 +211,9 @@ struct mlxsw_config_profile {
 	u16	adaptive_routing_group_cap;
 	u8	arn;
 	u32	kvd_linear_size;
-	u32	kvd_hash_single_size;
-	u32	kvd_hash_double_size;
+	u16	kvd_hash_granularity;
+	u8	kvd_hash_single_parts;
+	u8	kvd_hash_double_parts;
 	u8	resource_query_enable;
 	struct mlxsw_swid_config swid_config[MLXSW_CONFIG_PROFILE_SWID_COUNT];
 };
@@ -268,8 +267,35 @@ struct mlxsw_driver {
 };
 
 struct mlxsw_resources {
-	u8	max_span_valid:1;
+	u32	max_span_valid:1,
+		max_lag_valid:1,
+		max_ports_in_lag_valid:1,
+		kvd_size_valid:1,
+		kvd_single_min_size_valid:1,
+		kvd_double_min_size_valid:1,
+		max_virtual_routers_valid:1,
+		max_system_ports_valid:1,
+		max_vlan_groups_valid:1,
+		max_regions_valid:1,
+		max_rif_valid:1;
 	u8      max_span;
+	u8	max_lag;
+	u8	max_ports_in_lag;
+	u32	kvd_size;
+	u32	kvd_single_min_size;
+	u32	kvd_double_min_size;
+	u16     max_virtual_routers;
+	u16	max_system_ports;
+	u16	max_vlan_groups;
+	u16	max_regions;
+	u16	max_rif;
+
+	/* Internal resources.
+	 * Determined by the SW, not queried from the HW.
+	 */
+	u32	kvd_single_size;
+	u32	kvd_double_size;
+	u32	kvd_linear_size;
 };
 
 struct mlxsw_resources *mlxsw_core_resources_get(struct mlxsw_core *mlxsw_core);
diff --git a/drivers/net/ethernet/mellanox/mlxsw/pci.c b/drivers/net/ethernet/mellanox/mlxsw/pci.c
index 1d1360c178bb..e742bd4e8894 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/pci.c
+++ b/drivers/net/ethernet/mellanox/mlxsw/pci.c
@@ -1156,6 +1156,16 @@ mlxsw_pci_config_profile_swid_config(struct mlxsw_pci *mlxsw_pci,
 
 #define MLXSW_RESOURCES_TABLE_END_ID 0xffff
 #define MLXSW_MAX_SPAN_ID 0x2420
+#define MLXSW_MAX_LAG_ID 0x2520
+#define MLXSW_MAX_PORTS_IN_LAG_ID 0x2521
+#define MLXSW_KVD_SIZE_ID 0x1001
+#define MLXSW_KVD_SINGLE_MIN_SIZE_ID 0x1002
+#define MLXSW_KVD_DOUBLE_MIN_SIZE_ID 0x1003
+#define MLXSW_MAX_VIRTUAL_ROUTERS_ID 0x2C01
+#define MLXSW_MAX_SYSTEM_PORT_ID 0x2502
+#define MLXSW_MAX_VLAN_GROUPS_ID 0x2906
+#define MLXSW_MAX_REGIONS_ID 0x2901
+#define MLXSW_MAX_RIF_ID 0x2C02
 #define MLXSW_RESOURCES_QUERY_MAX_QUERIES 100
 #define MLXSW_RESOURCES_PER_QUERY 32
 
@@ -1167,6 +1177,46 @@ static void mlxsw_pci_resources_query_parse(int id, u64 val,
 		resources->max_span = val;
 		resources->max_span_valid = 1;
 		break;
+	case MLXSW_MAX_LAG_ID:
+		resources->max_lag = val;
+		resources->max_lag_valid = 1;
+		break;
+	case MLXSW_MAX_PORTS_IN_LAG_ID:
+		resources->max_ports_in_lag = val;
+		resources->max_ports_in_lag_valid = 1;
+		break;
+	case MLXSW_KVD_SIZE_ID:
+		resources->kvd_size = val;
+		resources->kvd_size_valid = 1;
+		break;
+	case MLXSW_KVD_SINGLE_MIN_SIZE_ID:
+		resources->kvd_single_min_size = val;
+		resources->kvd_single_min_size_valid = 1;
+		break;
+	case MLXSW_KVD_DOUBLE_MIN_SIZE_ID:
+		resources->kvd_double_min_size = val;
+		resources->kvd_double_min_size_valid = 1;
+		break;
+	case MLXSW_MAX_VIRTUAL_ROUTERS_ID:
+		resources->max_virtual_routers = val;
+		resources->max_virtual_routers_valid = 1;
+		break;
+	case MLXSW_MAX_SYSTEM_PORT_ID:
+		resources->max_system_ports = val;
+		resources->max_system_ports_valid = 1;
+		break;
+	case MLXSW_MAX_VLAN_GROUPS_ID:
+		resources->max_vlan_groups = val;
+		resources->max_vlan_groups_valid = 1;
+		break;
+	case MLXSW_MAX_REGIONS_ID:
+		resources->max_regions = val;
+		resources->max_regions_valid = 1;
+		break;
+	case MLXSW_MAX_RIF_ID:
+		resources->max_rif = val;
+		resources->max_rif_valid = 1;
+		break;
 	default:
 		break;
 	}
@@ -1209,10 +1259,52 @@ static int mlxsw_pci_resources_query(struct mlxsw_pci *mlxsw_pci, char *mbox,
 	return -EIO;
 }
 
+static int mlxsw_pci_profile_get_kvd_sizes(const struct mlxsw_config_profile *profile,
+					   struct mlxsw_resources *resources)
+{
+	u32 singles_size, doubles_size, linear_size;
+
+	if (!resources->kvd_single_min_size_valid ||
+	    !resources->kvd_double_min_size_valid ||
+	    !profile->used_kvd_split_data)
+		return -EIO;
+
+	linear_size = profile->kvd_linear_size;
+
+	/* The hash part is what left of the kvd without the
+	 * linear part. It is split to the single size and
+	 * double size by the parts ratio from the profile.
+	 * Both sizes must be a multiplications of the
+	 * granularity from the profile.
+	 */
+	doubles_size = (resources->kvd_size - linear_size);
+	doubles_size *= profile->kvd_hash_double_parts;
+	doubles_size /= (profile->kvd_hash_double_parts +
+			 profile->kvd_hash_single_parts);
+	doubles_size /= profile->kvd_hash_granularity;
+	doubles_size *= profile->kvd_hash_granularity;
+	singles_size = resources->kvd_size - doubles_size -
+		       linear_size;
+
+	/* Check results are legal. */
+	if (singles_size < resources->kvd_single_min_size ||
+	    doubles_size < resources->kvd_double_min_size ||
+	    resources->kvd_size < linear_size)
+		return -EIO;
+
+	resources->kvd_single_size = singles_size;
+	resources->kvd_double_size = doubles_size;
+	resources->kvd_linear_size = linear_size;
+
+	return 0;
+}
+
 static int mlxsw_pci_config_profile(struct mlxsw_pci *mlxsw_pci, char *mbox,
-				    const struct mlxsw_config_profile *profile)
+				    const struct mlxsw_config_profile *profile,
+				    struct mlxsw_resources *resources)
 {
 	int i;
+	int err;
 
 	mlxsw_cmd_mbox_zero(mbox);
 
@@ -1222,18 +1314,6 @@ static int mlxsw_pci_config_profile(struct mlxsw_pci *mlxsw_pci, char *mbox,
 		mlxsw_cmd_mbox_config_profile_max_vepa_channels_set(
 			mbox, profile->max_vepa_channels);
 	}
-	if (profile->used_max_lag) {
-		mlxsw_cmd_mbox_config_profile_set_max_lag_set(
-			mbox, 1);
-		mlxsw_cmd_mbox_config_profile_max_lag_set(
-			mbox, profile->max_lag);
-	}
-	if (profile->used_max_port_per_lag) {
-		mlxsw_cmd_mbox_config_profile_set_max_port_per_lag_set(
-			mbox, 1);
-		mlxsw_cmd_mbox_config_profile_max_port_per_lag_set(
-			mbox, profile->max_port_per_lag);
-	}
 	if (profile->used_max_mid) {
 		mlxsw_cmd_mbox_config_profile_set_max_mid_set(
 			mbox, 1);
@@ -1310,19 +1390,22 @@ static int mlxsw_pci_config_profile(struct mlxsw_pci *mlxsw_pci, char *mbox,
 		mlxsw_cmd_mbox_config_profile_adaptive_routing_group_cap_set(
 			mbox, profile->adaptive_routing_group_cap);
 	}
-	if (profile->used_kvd_sizes) {
-		mlxsw_cmd_mbox_config_profile_set_kvd_linear_size_set(
-			mbox, 1);
-		mlxsw_cmd_mbox_config_profile_kvd_linear_size_set(
-			mbox, profile->kvd_linear_size);
-		mlxsw_cmd_mbox_config_profile_set_kvd_hash_single_size_set(
-			mbox, 1);
-		mlxsw_cmd_mbox_config_profile_kvd_hash_single_size_set(
-			mbox, profile->kvd_hash_single_size);
+	if (resources->kvd_size_valid) {
+		err = mlxsw_pci_profile_get_kvd_sizes(profile, resources);
+		if (err)
+			return err;
+
+		mlxsw_cmd_mbox_config_profile_set_kvd_linear_size_set(mbox, 1);
+		mlxsw_cmd_mbox_config_profile_kvd_linear_size_set(mbox,
+						resources->kvd_linear_size);
+		mlxsw_cmd_mbox_config_profile_set_kvd_hash_single_size_set(mbox,
+									   1);
+		mlxsw_cmd_mbox_config_profile_kvd_hash_single_size_set(mbox,
+						resources->kvd_single_size);
 		mlxsw_cmd_mbox_config_profile_set_kvd_hash_double_size_set(
-			mbox, 1);
-		mlxsw_cmd_mbox_config_profile_kvd_hash_double_size_set(
-			mbox, profile->kvd_hash_double_size);
+								mbox, 1);
+		mlxsw_cmd_mbox_config_profile_kvd_hash_double_size_set(mbox,
+						resources->kvd_double_size);
 	}
 
 	for (i = 0; i < MLXSW_CONFIG_PROFILE_SWID_COUNT; i++)
@@ -1524,7 +1607,7 @@ static int mlxsw_pci_init(void *bus_priv, struct mlxsw_core *mlxsw_core,
 	if (err)
 		goto err_query_resources;
 
-	err = mlxsw_pci_config_profile(mlxsw_pci, mbox, profile);
+	err = mlxsw_pci_config_profile(mlxsw_pci, mbox, profile, resources);
 	if (err)
 		goto err_config_profile;
 
diff --git a/drivers/net/ethernet/mellanox/mlxsw/reg.h b/drivers/net/ethernet/mellanox/mlxsw/reg.h
index 1721098eef13..6460c7256f2b 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/reg.h
+++ b/drivers/net/ethernet/mellanox/mlxsw/reg.h
@@ -591,6 +591,12 @@ static const struct mlxsw_reg_info mlxsw_reg_sfn = {
  */
 MLXSW_ITEM32(reg, sfn, swid, 0x00, 24, 8);
 
+/* reg_sfn_end
+ * Forces the current session to end.
+ * Access: OP
+ */
+MLXSW_ITEM32(reg, sfn, end, 0x04, 20, 1);
+
 /* reg_sfn_num_rec
  * Request: Number of learned notifications and aged-out notification
  * records requested.
@@ -605,6 +611,7 @@ static inline void mlxsw_reg_sfn_pack(char *payload)
 {
 	MLXSW_REG_ZERO(sfn, payload);
 	mlxsw_reg_sfn_swid_set(payload, 0);
+	mlxsw_reg_sfn_end_set(payload, 1);
 	mlxsw_reg_sfn_num_rec_set(payload, MLXSW_REG_SFN_REC_MAX_COUNT);
 }
 
@@ -1385,7 +1392,7 @@ static inline void mlxsw_reg_slcr_pack(char *payload, u16 lag_hash)
 {
 	MLXSW_REG_ZERO(slcr, payload);
 	mlxsw_reg_slcr_pp_set(payload, MLXSW_REG_SLCR_PP_GLOBAL);
-	mlxsw_reg_slcr_type_set(payload, MLXSW_REG_SLCR_TYPE_XOR);
+	mlxsw_reg_slcr_type_set(payload, MLXSW_REG_SLCR_TYPE_CRC);
 	mlxsw_reg_slcr_lag_hash_set(payload, lag_hash);
 }
 
@@ -2131,6 +2138,18 @@ MLXSW_ITEM32(reg, ptys, local_port, 0x00, 16, 8);
  */
 MLXSW_ITEM32(reg, ptys, proto_mask, 0x00, 0, 3);
 
+enum {
+	MLXSW_REG_PTYS_AN_STATUS_NA,
+	MLXSW_REG_PTYS_AN_STATUS_OK,
+	MLXSW_REG_PTYS_AN_STATUS_FAIL,
+};
+
+/* reg_ptys_an_status
+ * Autonegotiation status.
+ * Access: RO
+ */
+MLXSW_ITEM32(reg, ptys, an_status, 0x04, 28, 4);
+
 #define MLXSW_REG_PTYS_ETH_SPEED_SGMII			BIT(0)
 #define MLXSW_REG_PTYS_ETH_SPEED_1000BASE_KX		BIT(1)
 #define MLXSW_REG_PTYS_ETH_SPEED_10GBASE_CX4		BIT(2)
@@ -2145,6 +2164,7 @@ MLXSW_ITEM32(reg, ptys, proto_mask, 0x00, 0, 3);
 #define MLXSW_REG_PTYS_ETH_SPEED_10GBASE_ER_LR		BIT(14)
 #define MLXSW_REG_PTYS_ETH_SPEED_40GBASE_SR4		BIT(15)
 #define MLXSW_REG_PTYS_ETH_SPEED_40GBASE_LR4_ER4	BIT(16)
+#define MLXSW_REG_PTYS_ETH_SPEED_50GBASE_SR2		BIT(18)
 #define MLXSW_REG_PTYS_ETH_SPEED_50GBASE_KR4		BIT(19)
 #define MLXSW_REG_PTYS_ETH_SPEED_100GBASE_CR4		BIT(20)
 #define MLXSW_REG_PTYS_ETH_SPEED_100GBASE_SR4		BIT(21)
@@ -2177,6 +2197,13 @@ MLXSW_ITEM32(reg, ptys, eth_proto_admin, 0x18, 0, 32);
  */
 MLXSW_ITEM32(reg, ptys, eth_proto_oper, 0x24, 0, 32);
 
+/* reg_ptys_eth_proto_lp_advertise
+ * The protocols that were advertised by the link partner during
+ * autonegotiation.
+ * Access: RO
+ */
+MLXSW_ITEM32(reg, ptys, eth_proto_lp_advertise, 0x30, 0, 32);
+
 static inline void mlxsw_reg_ptys_pack(char *payload, u8 local_port,
 				       u32 proto_admin)
 {
diff --git a/drivers/net/ethernet/mellanox/mlxsw/spectrum.c b/drivers/net/ethernet/mellanox/mlxsw/spectrum.c
index d48873bcbddf..1ec0a4ce3c46 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/spectrum.c
+++ b/drivers/net/ethernet/mellanox/mlxsw/spectrum.c
@@ -248,7 +248,8 @@ static void mlxsw_sp_span_entry_destroy(struct mlxsw_sp *mlxsw_sp,
 	span_entry->used = false;
 }
 
-struct mlxsw_sp_span_entry *mlxsw_sp_span_entry_find(struct mlxsw_sp_port *port)
+static struct mlxsw_sp_span_entry *
+mlxsw_sp_span_entry_find(struct mlxsw_sp_port *port)
 {
 	struct mlxsw_sp *mlxsw_sp = port->mlxsw_sp;
 	int i;
@@ -262,7 +263,8 @@ struct mlxsw_sp_span_entry *mlxsw_sp_span_entry_find(struct mlxsw_sp_port *port)
 	return NULL;
 }
 
-struct mlxsw_sp_span_entry *mlxsw_sp_span_entry_get(struct mlxsw_sp_port *port)
+static struct mlxsw_sp_span_entry
+*mlxsw_sp_span_entry_get(struct mlxsw_sp_port *port)
 {
 	struct mlxsw_sp_span_entry *span_entry;
 
@@ -364,7 +366,8 @@ mlxsw_sp_span_inspected_port_bind(struct mlxsw_sp_port *port,
 	}
 
 	/* bind the port to the SPAN entry */
-	mlxsw_reg_mpar_pack(mpar_pl, port->local_port, type, true, pa_id);
+	mlxsw_reg_mpar_pack(mpar_pl, port->local_port,
+			    (enum mlxsw_reg_mpar_i_e) type, true, pa_id);
 	err = mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(mpar), mpar_pl);
 	if (err)
 		goto err_mpar_reg_write;
@@ -405,7 +408,8 @@ mlxsw_sp_span_inspected_port_unbind(struct mlxsw_sp_port *port,
 		return;
 
 	/* remove the inspected port */
-	mlxsw_reg_mpar_pack(mpar_pl, port->local_port, type, false, pa_id);
+	mlxsw_reg_mpar_pack(mpar_pl, port->local_port,
+			    (enum mlxsw_reg_mpar_i_e) type, false, pa_id);
 	mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(mpar), mpar_pl);
 
 	/* remove the SBIB buffer if it was egress SPAN */
@@ -556,8 +560,9 @@ int mlxsw_sp_port_vid_to_fid_set(struct mlxsw_sp_port *mlxsw_sp_port,
 	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(svfa), svfa_pl);
 }
 
-static int mlxsw_sp_port_vid_learning_set(struct mlxsw_sp_port *mlxsw_sp_port,
-					  u16 vid, bool learn_enable)
+int __mlxsw_sp_port_vid_learning_set(struct mlxsw_sp_port *mlxsw_sp_port,
+				     u16 vid_begin, u16 vid_end,
+				     bool learn_enable)
 {
 	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
 	char *spvmlr_pl;
@@ -566,13 +571,20 @@ static int mlxsw_sp_port_vid_learning_set(struct mlxsw_sp_port *mlxsw_sp_port,
 	spvmlr_pl = kmalloc(MLXSW_REG_SPVMLR_LEN, GFP_KERNEL);
 	if (!spvmlr_pl)
 		return -ENOMEM;
-	mlxsw_reg_spvmlr_pack(spvmlr_pl, mlxsw_sp_port->local_port, vid, vid,
-			      learn_enable);
+	mlxsw_reg_spvmlr_pack(spvmlr_pl, mlxsw_sp_port->local_port, vid_begin,
+			      vid_end, learn_enable);
 	err = mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(spvmlr), spvmlr_pl);
 	kfree(spvmlr_pl);
 	return err;
 }
 
+static int mlxsw_sp_port_vid_learning_set(struct mlxsw_sp_port *mlxsw_sp_port,
+					  u16 vid, bool learn_enable)
+{
+	return __mlxsw_sp_port_vid_learning_set(mlxsw_sp_port, vid, vid,
+						learn_enable);
+}
+
 static int
 mlxsw_sp_port_system_port_mapping_set(struct mlxsw_sp_port *mlxsw_sp_port)
 {
@@ -811,9 +823,9 @@ err_span_port_mtu_update:
 	return err;
 }
 
-static struct rtnl_link_stats64 *
-mlxsw_sp_port_get_stats64(struct net_device *dev,
-			  struct rtnl_link_stats64 *stats)
+static int
+mlxsw_sp_port_get_sw_stats64(const struct net_device *dev,
+			     struct rtnl_link_stats64 *stats)
 {
 	struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
 	struct mlxsw_sp_port_pcpu_stats *p;
@@ -840,6 +852,107 @@ mlxsw_sp_port_get_stats64(struct net_device *dev,
 		tx_dropped	+= p->tx_dropped;
 	}
 	stats->tx_dropped	= tx_dropped;
+	return 0;
+}
+
+static bool mlxsw_sp_port_has_offload_stats(int attr_id)
+{
+	switch (attr_id) {
+	case IFLA_OFFLOAD_XSTATS_CPU_HIT:
+		return true;
+	}
+
+	return false;
+}
+
+static int mlxsw_sp_port_get_offload_stats(int attr_id, const struct net_device *dev,
+					   void *sp)
+{
+	switch (attr_id) {
+	case IFLA_OFFLOAD_XSTATS_CPU_HIT:
+		return mlxsw_sp_port_get_sw_stats64(dev, sp);
+	}
+
+	return -EINVAL;
+}
+
+static int mlxsw_sp_port_get_stats_raw(struct net_device *dev, int grp,
+				       int prio, char *ppcnt_pl)
+{
+	struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
+
+	mlxsw_reg_ppcnt_pack(ppcnt_pl, mlxsw_sp_port->local_port, grp, prio);
+	return mlxsw_reg_query(mlxsw_sp->core, MLXSW_REG(ppcnt), ppcnt_pl);
+}
+
+static int mlxsw_sp_port_get_hw_stats(struct net_device *dev,
+				      struct rtnl_link_stats64 *stats)
+{
+	char ppcnt_pl[MLXSW_REG_PPCNT_LEN];
+	int err;
+
+	err = mlxsw_sp_port_get_stats_raw(dev, MLXSW_REG_PPCNT_IEEE_8023_CNT,
+					  0, ppcnt_pl);
+	if (err)
+		goto out;
+
+	stats->tx_packets =
+		mlxsw_reg_ppcnt_a_frames_transmitted_ok_get(ppcnt_pl);
+	stats->rx_packets =
+		mlxsw_reg_ppcnt_a_frames_received_ok_get(ppcnt_pl);
+	stats->tx_bytes =
+		mlxsw_reg_ppcnt_a_octets_transmitted_ok_get(ppcnt_pl);
+	stats->rx_bytes =
+		mlxsw_reg_ppcnt_a_octets_received_ok_get(ppcnt_pl);
+	stats->multicast =
+		mlxsw_reg_ppcnt_a_multicast_frames_received_ok_get(ppcnt_pl);
+
+	stats->rx_crc_errors =
+		mlxsw_reg_ppcnt_a_frame_check_sequence_errors_get(ppcnt_pl);
+	stats->rx_frame_errors =
+		mlxsw_reg_ppcnt_a_alignment_errors_get(ppcnt_pl);
+
+	stats->rx_length_errors = (
+		mlxsw_reg_ppcnt_a_in_range_length_errors_get(ppcnt_pl) +
+		mlxsw_reg_ppcnt_a_out_of_range_length_field_get(ppcnt_pl) +
+		mlxsw_reg_ppcnt_a_frame_too_long_errors_get(ppcnt_pl));
+
+	stats->rx_errors = (stats->rx_crc_errors +
+		stats->rx_frame_errors + stats->rx_length_errors);
+
+out:
+	return err;
+}
+
+static void update_stats_cache(struct work_struct *work)
+{
+	struct mlxsw_sp_port *mlxsw_sp_port =
+		container_of(work, struct mlxsw_sp_port,
+			     hw_stats.update_dw.work);
+
+	if (!netif_carrier_ok(mlxsw_sp_port->dev))
+		goto out;
+
+	mlxsw_sp_port_get_hw_stats(mlxsw_sp_port->dev,
+				   mlxsw_sp_port->hw_stats.cache);
+
+out:
+	mlxsw_core_schedule_dw(&mlxsw_sp_port->hw_stats.update_dw,
+			       MLXSW_HW_STATS_UPDATE_TIME);
+}
+
+/* Return the stats from a cache that is updated periodically,
+ * as this function might get called in an atomic context.
+ */
+static struct rtnl_link_stats64 *
+mlxsw_sp_port_get_stats64(struct net_device *dev,
+			  struct rtnl_link_stats64 *stats)
+{
+	struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
+
+	memcpy(stats, mlxsw_sp_port->hw_stats.cache, sizeof(*stats));
+
 	return stats;
 }
 
@@ -974,10 +1087,6 @@ static int mlxsw_sp_port_add_vid(struct net_device *dev,
 			goto err_port_vp_mode_trans;
 	}
 
-	err = mlxsw_sp_port_vid_learning_set(mlxsw_sp_vport, vid, false);
-	if (err)
-		goto err_port_vid_learning_set;
-
 	err = mlxsw_sp_port_vlan_set(mlxsw_sp_vport, vid, vid, true, untagged);
 	if (err)
 		goto err_port_add_vid;
@@ -985,8 +1094,6 @@ static int mlxsw_sp_port_add_vid(struct net_device *dev,
 	return 0;
 
 err_port_add_vid:
-	mlxsw_sp_port_vid_learning_set(mlxsw_sp_vport, vid, true);
-err_port_vid_learning_set:
 	if (list_is_singular(&mlxsw_sp_port->vports_list))
 		mlxsw_sp_port_vlan_mode_trans(mlxsw_sp_port);
 err_port_vp_mode_trans:
@@ -1013,8 +1120,6 @@ static int mlxsw_sp_port_kill_vid(struct net_device *dev,
 
 	mlxsw_sp_port_vlan_set(mlxsw_sp_vport, vid, vid, false, false);
 
-	mlxsw_sp_port_vid_learning_set(mlxsw_sp_vport, vid, true);
-
 	/* Drop FID reference. If this was the last reference the
 	 * resources will be freed.
 	 */
@@ -1209,6 +1314,8 @@ static const struct net_device_ops mlxsw_sp_port_netdev_ops = {
 	.ndo_set_mac_address	= mlxsw_sp_port_set_mac_address,
 	.ndo_change_mtu		= mlxsw_sp_port_change_mtu,
 	.ndo_get_stats64	= mlxsw_sp_port_get_stats64,
+	.ndo_has_offload_stats	= mlxsw_sp_port_has_offload_stats,
+	.ndo_get_offload_stats	= mlxsw_sp_port_get_offload_stats,
 	.ndo_vlan_rx_add_vid	= mlxsw_sp_port_add_vid,
 	.ndo_vlan_rx_kill_vid	= mlxsw_sp_port_kill_vid,
 	.ndo_neigh_construct	= mlxsw_sp_router_neigh_construct,
@@ -1547,8 +1654,6 @@ static void __mlxsw_sp_port_get_stats(struct net_device *dev,
 				      enum mlxsw_reg_ppcnt_grp grp, int prio,
 				      u64 *data, int data_index)
 {
-	struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
-	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
 	struct mlxsw_sp_port_hw_stats *hw_stats;
 	char ppcnt_pl[MLXSW_REG_PPCNT_LEN];
 	int i, len;
@@ -1557,10 +1662,9 @@ static void __mlxsw_sp_port_get_stats(struct net_device *dev,
 	err = mlxsw_sp_get_hw_stats_by_group(&hw_stats, &len, grp);
 	if (err)
 		return;
-	mlxsw_reg_ppcnt_pack(ppcnt_pl, mlxsw_sp_port->local_port, grp, prio);
-	err = mlxsw_reg_query(mlxsw_sp->core, MLXSW_REG(ppcnt), ppcnt_pl);
+	mlxsw_sp_port_get_stats_raw(dev, grp, prio, ppcnt_pl);
 	for (i = 0; i < len; i++)
-		data[data_index + i] = !err ? hw_stats[i].getter(ppcnt_pl) : 0;
+		data[data_index + i] = hw_stats[i].getter(ppcnt_pl);
 }
 
 static void mlxsw_sp_port_get_stats(struct net_device *dev,
@@ -1599,112 +1703,149 @@ static int mlxsw_sp_port_get_sset_count(struct net_device *dev, int sset)
 }
 
 struct mlxsw_sp_port_link_mode {
+	enum ethtool_link_mode_bit_indices mask_ethtool;
 	u32 mask;
-	u32 supported;
-	u32 advertised;
 	u32 speed;
 };
 
 static const struct mlxsw_sp_port_link_mode mlxsw_sp_port_link_mode[] = {
 	{
 		.mask		= MLXSW_REG_PTYS_ETH_SPEED_100BASE_T,
-		.supported	= SUPPORTED_100baseT_Full,
-		.advertised	= ADVERTISED_100baseT_Full,
-		.speed		= 100,
-	},
-	{
-		.mask		= MLXSW_REG_PTYS_ETH_SPEED_100BASE_TX,
-		.speed		= 100,
+		.mask_ethtool	= ETHTOOL_LINK_MODE_100baseT_Full_BIT,
+		.speed		= SPEED_100,
 	},
 	{
 		.mask		= MLXSW_REG_PTYS_ETH_SPEED_SGMII |
 				  MLXSW_REG_PTYS_ETH_SPEED_1000BASE_KX,
-		.supported	= SUPPORTED_1000baseKX_Full,
-		.advertised	= ADVERTISED_1000baseKX_Full,
-		.speed		= 1000,
+		.mask_ethtool	= ETHTOOL_LINK_MODE_1000baseKX_Full_BIT,
+		.speed		= SPEED_1000,
 	},
 	{
 		.mask		= MLXSW_REG_PTYS_ETH_SPEED_10GBASE_T,
-		.supported	= SUPPORTED_10000baseT_Full,
-		.advertised	= ADVERTISED_10000baseT_Full,
-		.speed		= 10000,
+		.mask_ethtool	= ETHTOOL_LINK_MODE_10000baseT_Full_BIT,
+		.speed		= SPEED_10000,
 	},
 	{
 		.mask		= MLXSW_REG_PTYS_ETH_SPEED_10GBASE_CX4 |
 				  MLXSW_REG_PTYS_ETH_SPEED_10GBASE_KX4,
-		.supported	= SUPPORTED_10000baseKX4_Full,
-		.advertised	= ADVERTISED_10000baseKX4_Full,
-		.speed		= 10000,
+		.mask_ethtool	= ETHTOOL_LINK_MODE_10000baseKX4_Full_BIT,
+		.speed		= SPEED_10000,
 	},
 	{
 		.mask		= MLXSW_REG_PTYS_ETH_SPEED_10GBASE_KR |
 				  MLXSW_REG_PTYS_ETH_SPEED_10GBASE_CR |
 				  MLXSW_REG_PTYS_ETH_SPEED_10GBASE_SR |
 				  MLXSW_REG_PTYS_ETH_SPEED_10GBASE_ER_LR,
-		.supported	= SUPPORTED_10000baseKR_Full,
-		.advertised	= ADVERTISED_10000baseKR_Full,
-		.speed		= 10000,
+		.mask_ethtool	= ETHTOOL_LINK_MODE_10000baseKR_Full_BIT,
+		.speed		= SPEED_10000,
 	},
 	{
 		.mask		= MLXSW_REG_PTYS_ETH_SPEED_20GBASE_KR2,
-		.supported	= SUPPORTED_20000baseKR2_Full,
-		.advertised	= ADVERTISED_20000baseKR2_Full,
-		.speed		= 20000,
+		.mask_ethtool	= ETHTOOL_LINK_MODE_20000baseKR2_Full_BIT,
+		.speed		= SPEED_20000,
 	},
 	{
 		.mask		= MLXSW_REG_PTYS_ETH_SPEED_40GBASE_CR4,
-		.supported	= SUPPORTED_40000baseCR4_Full,
-		.advertised	= ADVERTISED_40000baseCR4_Full,
-		.speed		= 40000,
+		.mask_ethtool	= ETHTOOL_LINK_MODE_40000baseCR4_Full_BIT,
+		.speed		= SPEED_40000,
 	},
 	{
 		.mask		= MLXSW_REG_PTYS_ETH_SPEED_40GBASE_KR4,
-		.supported	= SUPPORTED_40000baseKR4_Full,
-		.advertised	= ADVERTISED_40000baseKR4_Full,
-		.speed		= 40000,
+		.mask_ethtool	= ETHTOOL_LINK_MODE_40000baseKR4_Full_BIT,
+		.speed		= SPEED_40000,
 	},
 	{
 		.mask		= MLXSW_REG_PTYS_ETH_SPEED_40GBASE_SR4,
-		.supported	= SUPPORTED_40000baseSR4_Full,
-		.advertised	= ADVERTISED_40000baseSR4_Full,
-		.speed		= 40000,
+		.mask_ethtool	= ETHTOOL_LINK_MODE_40000baseSR4_Full_BIT,
+		.speed		= SPEED_40000,
 	},
 	{
 		.mask		= MLXSW_REG_PTYS_ETH_SPEED_40GBASE_LR4_ER4,
-		.supported	= SUPPORTED_40000baseLR4_Full,
-		.advertised	= ADVERTISED_40000baseLR4_Full,
-		.speed		= 40000,
+		.mask_ethtool	= ETHTOOL_LINK_MODE_40000baseLR4_Full_BIT,
+		.speed		= SPEED_40000,
+	},
+	{
+		.mask		= MLXSW_REG_PTYS_ETH_SPEED_25GBASE_CR,
+		.mask_ethtool	= ETHTOOL_LINK_MODE_25000baseCR_Full_BIT,
+		.speed		= SPEED_25000,
+	},
+	{
+		.mask		= MLXSW_REG_PTYS_ETH_SPEED_25GBASE_KR,
+		.mask_ethtool	= ETHTOOL_LINK_MODE_25000baseKR_Full_BIT,
+		.speed		= SPEED_25000,
+	},
+	{
+		.mask		= MLXSW_REG_PTYS_ETH_SPEED_25GBASE_SR,
+		.mask_ethtool	= ETHTOOL_LINK_MODE_25000baseSR_Full_BIT,
+		.speed		= SPEED_25000,
+	},
+	{
+		.mask		= MLXSW_REG_PTYS_ETH_SPEED_25GBASE_SR,
+		.mask_ethtool	= ETHTOOL_LINK_MODE_25000baseSR_Full_BIT,
+		.speed		= SPEED_25000,
 	},
 	{
-		.mask		= MLXSW_REG_PTYS_ETH_SPEED_25GBASE_CR |
-				  MLXSW_REG_PTYS_ETH_SPEED_25GBASE_KR |
-				  MLXSW_REG_PTYS_ETH_SPEED_25GBASE_SR,
-		.speed		= 25000,
+		.mask		= MLXSW_REG_PTYS_ETH_SPEED_50GBASE_CR2,
+		.mask_ethtool	= ETHTOOL_LINK_MODE_50000baseCR2_Full_BIT,
+		.speed		= SPEED_50000,
 	},
 	{
-		.mask		= MLXSW_REG_PTYS_ETH_SPEED_50GBASE_KR4 |
-				  MLXSW_REG_PTYS_ETH_SPEED_50GBASE_CR2 |
-				  MLXSW_REG_PTYS_ETH_SPEED_50GBASE_KR2,
-		.speed		= 50000,
+		.mask		= MLXSW_REG_PTYS_ETH_SPEED_50GBASE_KR2,
+		.mask_ethtool	= ETHTOOL_LINK_MODE_50000baseKR2_Full_BIT,
+		.speed		= SPEED_50000,
+	},
+	{
+		.mask		= MLXSW_REG_PTYS_ETH_SPEED_50GBASE_SR2,
+		.mask_ethtool	= ETHTOOL_LINK_MODE_50000baseSR2_Full_BIT,
+		.speed		= SPEED_50000,
+	},
+	{
+		.mask		= MLXSW_REG_PTYS_ETH_SPEED_56GBASE_R4,
+		.mask_ethtool	= ETHTOOL_LINK_MODE_56000baseKR4_Full_BIT,
+		.speed		= SPEED_56000,
+	},
+	{
+		.mask		= MLXSW_REG_PTYS_ETH_SPEED_56GBASE_R4,
+		.mask_ethtool	= ETHTOOL_LINK_MODE_56000baseCR4_Full_BIT,
+		.speed		= SPEED_56000,
 	},
 	{
 		.mask		= MLXSW_REG_PTYS_ETH_SPEED_56GBASE_R4,
-		.supported	= SUPPORTED_56000baseKR4_Full,
-		.advertised	= ADVERTISED_56000baseKR4_Full,
-		.speed		= 56000,
+		.mask_ethtool	= ETHTOOL_LINK_MODE_56000baseSR4_Full_BIT,
+		.speed		= SPEED_56000,
 	},
 	{
-		.mask		= MLXSW_REG_PTYS_ETH_SPEED_100GBASE_CR4 |
-				  MLXSW_REG_PTYS_ETH_SPEED_100GBASE_SR4 |
-				  MLXSW_REG_PTYS_ETH_SPEED_100GBASE_KR4 |
-				  MLXSW_REG_PTYS_ETH_SPEED_100GBASE_LR4_ER4,
-		.speed		= 100000,
+		.mask		= MLXSW_REG_PTYS_ETH_SPEED_56GBASE_R4,
+		.mask_ethtool	= ETHTOOL_LINK_MODE_56000baseLR4_Full_BIT,
+		.speed		= SPEED_56000,
+	},
+	{
+		.mask		= MLXSW_REG_PTYS_ETH_SPEED_100GBASE_CR4,
+		.mask_ethtool	= ETHTOOL_LINK_MODE_100000baseCR4_Full_BIT,
+		.speed		= SPEED_100000,
+	},
+	{
+		.mask		= MLXSW_REG_PTYS_ETH_SPEED_100GBASE_SR4,
+		.mask_ethtool	= ETHTOOL_LINK_MODE_100000baseSR4_Full_BIT,
+		.speed		= SPEED_100000,
+	},
+	{
+		.mask		= MLXSW_REG_PTYS_ETH_SPEED_100GBASE_KR4,
+		.mask_ethtool	= ETHTOOL_LINK_MODE_100000baseKR4_Full_BIT,
+		.speed		= SPEED_100000,
+	},
+	{
+		.mask		= MLXSW_REG_PTYS_ETH_SPEED_100GBASE_LR4_ER4,
+		.mask_ethtool	= ETHTOOL_LINK_MODE_100000baseLR4_ER4_Full_BIT,
+		.speed		= SPEED_100000,
 	},
 };
 
 #define MLXSW_SP_PORT_LINK_MODE_LEN ARRAY_SIZE(mlxsw_sp_port_link_mode)
 
-static u32 mlxsw_sp_from_ptys_supported_port(u32 ptys_eth_proto)
+static void
+mlxsw_sp_from_ptys_supported_port(u32 ptys_eth_proto,
+				  struct ethtool_link_ksettings *cmd)
 {
 	if (ptys_eth_proto & (MLXSW_REG_PTYS_ETH_SPEED_10GBASE_CR |
 			      MLXSW_REG_PTYS_ETH_SPEED_10GBASE_SR |
@@ -1712,43 +1853,29 @@ static u32 mlxsw_sp_from_ptys_supported_port(u32 ptys_eth_proto)
 			      MLXSW_REG_PTYS_ETH_SPEED_40GBASE_SR4 |
 			      MLXSW_REG_PTYS_ETH_SPEED_100GBASE_SR4 |
 			      MLXSW_REG_PTYS_ETH_SPEED_SGMII))
-		return SUPPORTED_FIBRE;
+		ethtool_link_ksettings_add_link_mode(cmd, supported, FIBRE);
 
 	if (ptys_eth_proto & (MLXSW_REG_PTYS_ETH_SPEED_10GBASE_KR |
 			      MLXSW_REG_PTYS_ETH_SPEED_10GBASE_KX4 |
 			      MLXSW_REG_PTYS_ETH_SPEED_40GBASE_KR4 |
 			      MLXSW_REG_PTYS_ETH_SPEED_100GBASE_KR4 |
 			      MLXSW_REG_PTYS_ETH_SPEED_1000BASE_KX))
-		return SUPPORTED_Backplane;
-	return 0;
-}
-
-static u32 mlxsw_sp_from_ptys_supported_link(u32 ptys_eth_proto)
-{
-	u32 modes = 0;
-	int i;
-
-	for (i = 0; i < MLXSW_SP_PORT_LINK_MODE_LEN; i++) {
-		if (ptys_eth_proto & mlxsw_sp_port_link_mode[i].mask)
-			modes |= mlxsw_sp_port_link_mode[i].supported;
-	}
-	return modes;
+		ethtool_link_ksettings_add_link_mode(cmd, supported, Backplane);
 }
 
-static u32 mlxsw_sp_from_ptys_advert_link(u32 ptys_eth_proto)
+static void mlxsw_sp_from_ptys_link(u32 ptys_eth_proto, unsigned long *mode)
 {
-	u32 modes = 0;
 	int i;
 
 	for (i = 0; i < MLXSW_SP_PORT_LINK_MODE_LEN; i++) {
 		if (ptys_eth_proto & mlxsw_sp_port_link_mode[i].mask)
-			modes |= mlxsw_sp_port_link_mode[i].advertised;
+			__set_bit(mlxsw_sp_port_link_mode[i].mask_ethtool,
+				  mode);
 	}
-	return modes;
 }
 
 static void mlxsw_sp_from_ptys_speed_duplex(bool carrier_ok, u32 ptys_eth_proto,
-					    struct ethtool_cmd *cmd)
+					    struct ethtool_link_ksettings *cmd)
 {
 	u32 speed = SPEED_UNKNOWN;
 	u8 duplex = DUPLEX_UNKNOWN;
@@ -1765,8 +1892,8 @@ static void mlxsw_sp_from_ptys_speed_duplex(bool carrier_ok, u32 ptys_eth_proto,
 		}
 	}
 out:
-	ethtool_cmd_speed_set(cmd, speed);
-	cmd->duplex = duplex;
+	cmd->base.speed = speed;
+	cmd->base.duplex = duplex;
 }
 
 static u8 mlxsw_sp_port_connector_port(u32 ptys_eth_proto)
@@ -1791,49 +1918,15 @@ static u8 mlxsw_sp_port_connector_port(u32 ptys_eth_proto)
 	return PORT_OTHER;
 }
 
-static int mlxsw_sp_port_get_settings(struct net_device *dev,
-				      struct ethtool_cmd *cmd)
-{
-	struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
-	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
-	char ptys_pl[MLXSW_REG_PTYS_LEN];
-	u32 eth_proto_cap;
-	u32 eth_proto_admin;
-	u32 eth_proto_oper;
-	int err;
-
-	mlxsw_reg_ptys_pack(ptys_pl, mlxsw_sp_port->local_port, 0);
-	err = mlxsw_reg_query(mlxsw_sp->core, MLXSW_REG(ptys), ptys_pl);
-	if (err) {
-		netdev_err(dev, "Failed to get proto");
-		return err;
-	}
-	mlxsw_reg_ptys_unpack(ptys_pl, &eth_proto_cap,
-			      &eth_proto_admin, &eth_proto_oper);
-
-	cmd->supported = mlxsw_sp_from_ptys_supported_port(eth_proto_cap) |
-			 mlxsw_sp_from_ptys_supported_link(eth_proto_cap) |
-			 SUPPORTED_Pause | SUPPORTED_Asym_Pause |
-			 SUPPORTED_Autoneg;
-	cmd->advertising = mlxsw_sp_from_ptys_advert_link(eth_proto_admin);
-	mlxsw_sp_from_ptys_speed_duplex(netif_carrier_ok(dev),
-					eth_proto_oper, cmd);
-
-	eth_proto_oper = eth_proto_oper ? eth_proto_oper : eth_proto_cap;
-	cmd->port = mlxsw_sp_port_connector_port(eth_proto_oper);
-	cmd->lp_advertising = mlxsw_sp_from_ptys_advert_link(eth_proto_oper);
-
-	cmd->transceiver = XCVR_INTERNAL;
-	return 0;
-}
-
-static u32 mlxsw_sp_to_ptys_advert_link(u32 advertising)
+static u32
+mlxsw_sp_to_ptys_advert_link(const struct ethtool_link_ksettings *cmd)
 {
 	u32 ptys_proto = 0;
 	int i;
 
 	for (i = 0; i < MLXSW_SP_PORT_LINK_MODE_LEN; i++) {
-		if (advertising & mlxsw_sp_port_link_mode[i].advertised)
+		if (test_bit(mlxsw_sp_port_link_mode[i].mask_ethtool,
+			     cmd->link_modes.advertising))
 			ptys_proto |= mlxsw_sp_port_link_mode[i].mask;
 	}
 	return ptys_proto;
@@ -1863,61 +1956,113 @@ static u32 mlxsw_sp_to_ptys_upper_speed(u32 upper_speed)
 	return ptys_proto;
 }
 
-static int mlxsw_sp_port_set_settings(struct net_device *dev,
-				      struct ethtool_cmd *cmd)
+static void mlxsw_sp_port_get_link_supported(u32 eth_proto_cap,
+					     struct ethtool_link_ksettings *cmd)
 {
+	ethtool_link_ksettings_add_link_mode(cmd, supported, Asym_Pause);
+	ethtool_link_ksettings_add_link_mode(cmd, supported, Autoneg);
+	ethtool_link_ksettings_add_link_mode(cmd, supported, Pause);
+
+	mlxsw_sp_from_ptys_supported_port(eth_proto_cap, cmd);
+	mlxsw_sp_from_ptys_link(eth_proto_cap, cmd->link_modes.supported);
+}
+
+static void mlxsw_sp_port_get_link_advertise(u32 eth_proto_admin, bool autoneg,
+					     struct ethtool_link_ksettings *cmd)
+{
+	if (!autoneg)
+		return;
+
+	ethtool_link_ksettings_add_link_mode(cmd, advertising, Autoneg);
+	mlxsw_sp_from_ptys_link(eth_proto_admin, cmd->link_modes.advertising);
+}
+
+static void
+mlxsw_sp_port_get_link_lp_advertise(u32 eth_proto_lp, u8 autoneg_status,
+				    struct ethtool_link_ksettings *cmd)
+{
+	if (autoneg_status != MLXSW_REG_PTYS_AN_STATUS_OK || !eth_proto_lp)
+		return;
+
+	ethtool_link_ksettings_add_link_mode(cmd, lp_advertising, Autoneg);
+	mlxsw_sp_from_ptys_link(eth_proto_lp, cmd->link_modes.lp_advertising);
+}
+
+static int mlxsw_sp_port_get_link_ksettings(struct net_device *dev,
+					    struct ethtool_link_ksettings *cmd)
+{
+	u32 eth_proto_cap, eth_proto_admin, eth_proto_oper, eth_proto_lp;
 	struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
 	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
 	char ptys_pl[MLXSW_REG_PTYS_LEN];
-	u32 speed;
-	u32 eth_proto_new;
-	u32 eth_proto_cap;
-	u32 eth_proto_admin;
+	u8 autoneg_status;
+	bool autoneg;
 	int err;
 
-	speed = ethtool_cmd_speed(cmd);
+	autoneg = mlxsw_sp_port->link.autoneg;
+	mlxsw_reg_ptys_pack(ptys_pl, mlxsw_sp_port->local_port, 0);
+	err = mlxsw_reg_query(mlxsw_sp->core, MLXSW_REG(ptys), ptys_pl);
+	if (err)
+		return err;
+	mlxsw_reg_ptys_unpack(ptys_pl, &eth_proto_cap, &eth_proto_admin,
+			      &eth_proto_oper);
+
+	mlxsw_sp_port_get_link_supported(eth_proto_cap, cmd);
+
+	mlxsw_sp_port_get_link_advertise(eth_proto_admin, autoneg, cmd);
+
+	eth_proto_lp = mlxsw_reg_ptys_eth_proto_lp_advertise_get(ptys_pl);
+	autoneg_status = mlxsw_reg_ptys_an_status_get(ptys_pl);
+	mlxsw_sp_port_get_link_lp_advertise(eth_proto_lp, autoneg_status, cmd);
+
+	cmd->base.autoneg = autoneg ? AUTONEG_ENABLE : AUTONEG_DISABLE;
+	cmd->base.port = mlxsw_sp_port_connector_port(eth_proto_oper);
+	mlxsw_sp_from_ptys_speed_duplex(netif_carrier_ok(dev), eth_proto_oper,
+					cmd);
+
+	return 0;
+}
 
-	eth_proto_new = cmd->autoneg == AUTONEG_ENABLE ?
-		mlxsw_sp_to_ptys_advert_link(cmd->advertising) :
-		mlxsw_sp_to_ptys_speed(speed);
+static int
+mlxsw_sp_port_set_link_ksettings(struct net_device *dev,
+				 const struct ethtool_link_ksettings *cmd)
+{
+	struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
+	char ptys_pl[MLXSW_REG_PTYS_LEN];
+	u32 eth_proto_cap, eth_proto_new;
+	bool autoneg;
+	int err;
 
 	mlxsw_reg_ptys_pack(ptys_pl, mlxsw_sp_port->local_port, 0);
 	err = mlxsw_reg_query(mlxsw_sp->core, MLXSW_REG(ptys), ptys_pl);
-	if (err) {
-		netdev_err(dev, "Failed to get proto");
+	if (err)
 		return err;
-	}
-	mlxsw_reg_ptys_unpack(ptys_pl, &eth_proto_cap, &eth_proto_admin, NULL);
+	mlxsw_reg_ptys_unpack(ptys_pl, &eth_proto_cap, NULL, NULL);
+
+	autoneg = cmd->base.autoneg == AUTONEG_ENABLE;
+	eth_proto_new = autoneg ?
+		mlxsw_sp_to_ptys_advert_link(cmd) :
+		mlxsw_sp_to_ptys_speed(cmd->base.speed);
 
 	eth_proto_new = eth_proto_new & eth_proto_cap;
 	if (!eth_proto_new) {
-		netdev_err(dev, "Not supported proto admin requested");
+		netdev_err(dev, "No supported speed requested\n");
 		return -EINVAL;
 	}
-	if (eth_proto_new == eth_proto_admin)
-		return 0;
 
 	mlxsw_reg_ptys_pack(ptys_pl, mlxsw_sp_port->local_port, eth_proto_new);
 	err = mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(ptys), ptys_pl);
-	if (err) {
-		netdev_err(dev, "Failed to set proto admin");
+	if (err)
 		return err;
-	}
 
 	if (!netif_running(dev))
 		return 0;
 
-	err = mlxsw_sp_port_admin_status_set(mlxsw_sp_port, false);
-	if (err) {
-		netdev_err(dev, "Failed to set admin status");
-		return err;
-	}
+	mlxsw_sp_port->link.autoneg = autoneg;
 
-	err = mlxsw_sp_port_admin_status_set(mlxsw_sp_port, true);
-	if (err) {
-		netdev_err(dev, "Failed to set admin status");
-		return err;
-	}
+	mlxsw_sp_port_admin_status_set(mlxsw_sp_port, false);
+	mlxsw_sp_port_admin_status_set(mlxsw_sp_port, true);
 
 	return 0;
 }
@@ -1931,8 +2076,8 @@ static const struct ethtool_ops mlxsw_sp_port_ethtool_ops = {
 	.set_phys_id		= mlxsw_sp_port_set_phys_id,
 	.get_ethtool_stats	= mlxsw_sp_port_get_stats,
 	.get_sset_count		= mlxsw_sp_port_get_sset_count,
-	.get_settings		= mlxsw_sp_port_get_settings,
-	.set_settings		= mlxsw_sp_port_set_settings,
+	.get_link_ksettings	= mlxsw_sp_port_get_link_ksettings,
+	.set_link_ksettings	= mlxsw_sp_port_set_link_ksettings,
 };
 
 static int
@@ -2082,6 +2227,7 @@ static int mlxsw_sp_port_create(struct mlxsw_sp *mlxsw_sp, u8 local_port,
 	mlxsw_sp_port->mapping.module = module;
 	mlxsw_sp_port->mapping.width = width;
 	mlxsw_sp_port->mapping.lane = lane;
+	mlxsw_sp_port->link.autoneg = 1;
 	bytes = DIV_ROUND_UP(VLAN_N_VID, BITS_PER_BYTE);
 	mlxsw_sp_port->active_vlans = kzalloc(bytes, GFP_KERNEL);
 	if (!mlxsw_sp_port->active_vlans) {
@@ -2103,6 +2249,16 @@ static int mlxsw_sp_port_create(struct mlxsw_sp *mlxsw_sp, u8 local_port,
 		goto err_alloc_stats;
 	}
 
+	mlxsw_sp_port->hw_stats.cache =
+		kzalloc(sizeof(*mlxsw_sp_port->hw_stats.cache), GFP_KERNEL);
+
+	if (!mlxsw_sp_port->hw_stats.cache) {
+		err = -ENOMEM;
+		goto err_alloc_hw_stats;
+	}
+	INIT_DELAYED_WORK(&mlxsw_sp_port->hw_stats.update_dw,
+			  &update_stats_cache);
+
 	dev->netdev_ops = &mlxsw_sp_port_netdev_ops;
 	dev->ethtool_ops = &mlxsw_sp_port_ethtool_ops;
 
@@ -2129,7 +2285,7 @@ static int mlxsw_sp_port_create(struct mlxsw_sp *mlxsw_sp, u8 local_port,
 	/* Each packet needs to have a Tx header (metadata) on top all other
 	 * headers.
 	 */
-	dev->hard_header_len += MLXSW_TXHDR_LEN;
+	dev->needed_headroom = MLXSW_TXHDR_LEN;
 
 	err = mlxsw_sp_port_system_port_mapping_set(mlxsw_sp_port);
 	if (err) {
@@ -2203,6 +2359,7 @@ static int mlxsw_sp_port_create(struct mlxsw_sp *mlxsw_sp, u8 local_port,
 		goto err_core_port_init;
 	}
 
+	mlxsw_core_schedule_dw(&mlxsw_sp_port->hw_stats.update_dw, 0);
 	return 0;
 
 err_core_port_init:
@@ -2223,6 +2380,8 @@ err_port_system_port_mapping_set:
 err_dev_addr_init:
 	mlxsw_sp_port_swid_set(mlxsw_sp_port, MLXSW_PORT_SWID_DISABLED_PORT);
 err_port_swid_set:
+	kfree(mlxsw_sp_port->hw_stats.cache);
+err_alloc_hw_stats:
 	free_percpu(mlxsw_sp_port->pcpu_stats);
 err_alloc_stats:
 	kfree(mlxsw_sp_port->untagged_vlans);
@@ -2239,6 +2398,7 @@ static void mlxsw_sp_port_remove(struct mlxsw_sp *mlxsw_sp, u8 local_port)
 
 	if (!mlxsw_sp_port)
 		return;
+	cancel_delayed_work_sync(&mlxsw_sp_port->hw_stats.update_dw);
 	mlxsw_core_port_fini(&mlxsw_sp_port->core_port);
 	unregister_netdev(mlxsw_sp_port->dev); /* This calls ndo_stop */
 	mlxsw_sp->ports[local_port] = NULL;
@@ -2248,6 +2408,7 @@ static void mlxsw_sp_port_remove(struct mlxsw_sp *mlxsw_sp, u8 local_port)
 	mlxsw_sp_port_swid_set(mlxsw_sp_port, MLXSW_PORT_SWID_DISABLED_PORT);
 	mlxsw_sp_port_module_unmap(mlxsw_sp, mlxsw_sp_port->local_port);
 	free_percpu(mlxsw_sp_port->pcpu_stats);
+	kfree(mlxsw_sp_port->hw_stats.cache);
 	kfree(mlxsw_sp_port->untagged_vlans);
 	kfree(mlxsw_sp_port->active_vlans);
 	WARN_ON_ONCE(!list_empty(&mlxsw_sp_port->vports_list));
@@ -2571,123 +2732,47 @@ static void mlxsw_sp_rx_listener_func(struct sk_buff *skb, u8 local_port,
 	netif_receive_skb(skb);
 }
 
+static void mlxsw_sp_rx_listener_mark_func(struct sk_buff *skb, u8 local_port,
+					   void *priv)
+{
+	skb->offload_fwd_mark = 1;
+	return mlxsw_sp_rx_listener_func(skb, local_port, priv);
+}
+
+#define MLXSW_SP_RXL(_func, _trap_id, _action)			\
+	{							\
+		.func = _func,					\
+		.local_port = MLXSW_PORT_DONT_CARE,		\
+		.trap_id = MLXSW_TRAP_ID_##_trap_id,		\
+		.action = MLXSW_REG_HPKT_ACTION_##_action,	\
+	}
+
 static const struct mlxsw_rx_listener mlxsw_sp_rx_listener[] = {
-	{
-		.func = mlxsw_sp_rx_listener_func,
-		.local_port = MLXSW_PORT_DONT_CARE,
-		.trap_id = MLXSW_TRAP_ID_FDB_MC,
-	},
+	MLXSW_SP_RXL(mlxsw_sp_rx_listener_func, FDB_MC, TRAP_TO_CPU),
 	/* Traps for specific L2 packet types, not trapped as FDB MC */
-	{
-		.func = mlxsw_sp_rx_listener_func,
-		.local_port = MLXSW_PORT_DONT_CARE,
-		.trap_id = MLXSW_TRAP_ID_STP,
-	},
-	{
-		.func = mlxsw_sp_rx_listener_func,
-		.local_port = MLXSW_PORT_DONT_CARE,
-		.trap_id = MLXSW_TRAP_ID_LACP,
-	},
-	{
-		.func = mlxsw_sp_rx_listener_func,
-		.local_port = MLXSW_PORT_DONT_CARE,
-		.trap_id = MLXSW_TRAP_ID_EAPOL,
-	},
-	{
-		.func = mlxsw_sp_rx_listener_func,
-		.local_port = MLXSW_PORT_DONT_CARE,
-		.trap_id = MLXSW_TRAP_ID_LLDP,
-	},
-	{
-		.func = mlxsw_sp_rx_listener_func,
-		.local_port = MLXSW_PORT_DONT_CARE,
-		.trap_id = MLXSW_TRAP_ID_MMRP,
-	},
-	{
-		.func = mlxsw_sp_rx_listener_func,
-		.local_port = MLXSW_PORT_DONT_CARE,
-		.trap_id = MLXSW_TRAP_ID_MVRP,
-	},
-	{
-		.func = mlxsw_sp_rx_listener_func,
-		.local_port = MLXSW_PORT_DONT_CARE,
-		.trap_id = MLXSW_TRAP_ID_RPVST,
-	},
-	{
-		.func = mlxsw_sp_rx_listener_func,
-		.local_port = MLXSW_PORT_DONT_CARE,
-		.trap_id = MLXSW_TRAP_ID_DHCP,
-	},
-	{
-		.func = mlxsw_sp_rx_listener_func,
-		.local_port = MLXSW_PORT_DONT_CARE,
-		.trap_id = MLXSW_TRAP_ID_IGMP_QUERY,
-	},
-	{
-		.func = mlxsw_sp_rx_listener_func,
-		.local_port = MLXSW_PORT_DONT_CARE,
-		.trap_id = MLXSW_TRAP_ID_IGMP_V1_REPORT,
-	},
-	{
-		.func = mlxsw_sp_rx_listener_func,
-		.local_port = MLXSW_PORT_DONT_CARE,
-		.trap_id = MLXSW_TRAP_ID_IGMP_V2_REPORT,
-	},
-	{
-		.func = mlxsw_sp_rx_listener_func,
-		.local_port = MLXSW_PORT_DONT_CARE,
-		.trap_id = MLXSW_TRAP_ID_IGMP_V2_LEAVE,
-	},
-	{
-		.func = mlxsw_sp_rx_listener_func,
-		.local_port = MLXSW_PORT_DONT_CARE,
-		.trap_id = MLXSW_TRAP_ID_IGMP_V3_REPORT,
-	},
-	{
-		.func = mlxsw_sp_rx_listener_func,
-		.local_port = MLXSW_PORT_DONT_CARE,
-		.trap_id = MLXSW_TRAP_ID_ARPBC,
-	},
-	{
-		.func = mlxsw_sp_rx_listener_func,
-		.local_port = MLXSW_PORT_DONT_CARE,
-		.trap_id = MLXSW_TRAP_ID_ARPUC,
-	},
-	{
-		.func = mlxsw_sp_rx_listener_func,
-		.local_port = MLXSW_PORT_DONT_CARE,
-		.trap_id = MLXSW_TRAP_ID_MTUERROR,
-	},
-	{
-		.func = mlxsw_sp_rx_listener_func,
-		.local_port = MLXSW_PORT_DONT_CARE,
-		.trap_id = MLXSW_TRAP_ID_TTLERROR,
-	},
-	{
-		.func = mlxsw_sp_rx_listener_func,
-		.local_port = MLXSW_PORT_DONT_CARE,
-		.trap_id = MLXSW_TRAP_ID_LBERROR,
-	},
-	{
-		.func = mlxsw_sp_rx_listener_func,
-		.local_port = MLXSW_PORT_DONT_CARE,
-		.trap_id = MLXSW_TRAP_ID_OSPF,
-	},
-	{
-		.func = mlxsw_sp_rx_listener_func,
-		.local_port = MLXSW_PORT_DONT_CARE,
-		.trap_id = MLXSW_TRAP_ID_IP2ME,
-	},
-	{
-		.func = mlxsw_sp_rx_listener_func,
-		.local_port = MLXSW_PORT_DONT_CARE,
-		.trap_id = MLXSW_TRAP_ID_RTR_INGRESS0,
-	},
-	{
-		.func = mlxsw_sp_rx_listener_func,
-		.local_port = MLXSW_PORT_DONT_CARE,
-		.trap_id = MLXSW_TRAP_ID_HOST_MISS_IPV4,
-	},
+	MLXSW_SP_RXL(mlxsw_sp_rx_listener_func, STP, TRAP_TO_CPU),
+	MLXSW_SP_RXL(mlxsw_sp_rx_listener_func, LACP, TRAP_TO_CPU),
+	MLXSW_SP_RXL(mlxsw_sp_rx_listener_func, EAPOL, TRAP_TO_CPU),
+	MLXSW_SP_RXL(mlxsw_sp_rx_listener_func, LLDP, TRAP_TO_CPU),
+	MLXSW_SP_RXL(mlxsw_sp_rx_listener_func, MMRP, TRAP_TO_CPU),
+	MLXSW_SP_RXL(mlxsw_sp_rx_listener_func, MVRP, TRAP_TO_CPU),
+	MLXSW_SP_RXL(mlxsw_sp_rx_listener_func, RPVST, TRAP_TO_CPU),
+	MLXSW_SP_RXL(mlxsw_sp_rx_listener_mark_func, DHCP, MIRROR_TO_CPU),
+	MLXSW_SP_RXL(mlxsw_sp_rx_listener_mark_func, IGMP_QUERY, MIRROR_TO_CPU),
+	MLXSW_SP_RXL(mlxsw_sp_rx_listener_func, IGMP_V1_REPORT, TRAP_TO_CPU),
+	MLXSW_SP_RXL(mlxsw_sp_rx_listener_func, IGMP_V2_REPORT, TRAP_TO_CPU),
+	MLXSW_SP_RXL(mlxsw_sp_rx_listener_func, IGMP_V2_LEAVE, TRAP_TO_CPU),
+	MLXSW_SP_RXL(mlxsw_sp_rx_listener_func, IGMP_V3_REPORT, TRAP_TO_CPU),
+	MLXSW_SP_RXL(mlxsw_sp_rx_listener_mark_func, ARPBC, MIRROR_TO_CPU),
+	MLXSW_SP_RXL(mlxsw_sp_rx_listener_mark_func, ARPUC, MIRROR_TO_CPU),
+	/* L3 traps */
+	MLXSW_SP_RXL(mlxsw_sp_rx_listener_func, MTUERROR, TRAP_TO_CPU),
+	MLXSW_SP_RXL(mlxsw_sp_rx_listener_func, TTLERROR, TRAP_TO_CPU),
+	MLXSW_SP_RXL(mlxsw_sp_rx_listener_func, LBERROR, TRAP_TO_CPU),
+	MLXSW_SP_RXL(mlxsw_sp_rx_listener_mark_func, OSPF, TRAP_TO_CPU),
+	MLXSW_SP_RXL(mlxsw_sp_rx_listener_func, IP2ME, TRAP_TO_CPU),
+	MLXSW_SP_RXL(mlxsw_sp_rx_listener_func, RTR_INGRESS0, TRAP_TO_CPU),
+	MLXSW_SP_RXL(mlxsw_sp_rx_listener_func, HOST_MISS_IPV4, TRAP_TO_CPU),
 };
 
 static int mlxsw_sp_traps_init(struct mlxsw_sp *mlxsw_sp)
@@ -2714,7 +2799,7 @@ static int mlxsw_sp_traps_init(struct mlxsw_sp *mlxsw_sp)
 		if (err)
 			goto err_rx_listener_register;
 
-		mlxsw_reg_hpkt_pack(hpkt_pl, MLXSW_REG_HPKT_ACTION_TRAP_TO_CPU,
+		mlxsw_reg_hpkt_pack(hpkt_pl, mlxsw_sp_rx_listener[i].action,
 				    mlxsw_sp_rx_listener[i].trap_id);
 		err = mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(hpkt), hpkt_pl);
 		if (err)
@@ -2802,7 +2887,9 @@ static int mlxsw_sp_flood_init(struct mlxsw_sp *mlxsw_sp)
 
 static int mlxsw_sp_lag_init(struct mlxsw_sp *mlxsw_sp)
 {
+	struct mlxsw_resources *resources;
 	char slcr_pl[MLXSW_REG_SLCR_LEN];
+	int err;
 
 	mlxsw_reg_slcr_pack(slcr_pl, MLXSW_REG_SLCR_LAG_HASH_SMAC |
 				     MLXSW_REG_SLCR_LAG_HASH_DMAC |
@@ -2813,7 +2900,26 @@ static int mlxsw_sp_lag_init(struct mlxsw_sp *mlxsw_sp)
 				     MLXSW_REG_SLCR_LAG_HASH_SPORT |
 				     MLXSW_REG_SLCR_LAG_HASH_DPORT |
 				     MLXSW_REG_SLCR_LAG_HASH_IPPROTO);
-	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(slcr), slcr_pl);
+	err = mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(slcr), slcr_pl);
+	if (err)
+		return err;
+
+	resources = mlxsw_core_resources_get(mlxsw_sp->core);
+	if (!(resources->max_lag_valid && resources->max_ports_in_lag_valid))
+		return -EIO;
+
+	mlxsw_sp->lags = kcalloc(resources->max_lag,
+				 sizeof(struct mlxsw_sp_upper),
+				 GFP_KERNEL);
+	if (!mlxsw_sp->lags)
+		return -ENOMEM;
+
+	return 0;
+}
+
+static void mlxsw_sp_lag_fini(struct mlxsw_sp *mlxsw_sp)
+{
+	kfree(mlxsw_sp->lags);
 }
 
 static int mlxsw_sp_init(struct mlxsw_core *mlxsw_core,
@@ -2897,6 +3003,7 @@ err_span_init:
 err_router_init:
 	mlxsw_sp_switchdev_fini(mlxsw_sp);
 err_switchdev_init:
+	mlxsw_sp_lag_fini(mlxsw_sp);
 err_lag_init:
 	mlxsw_sp_buffers_fini(mlxsw_sp);
 err_buffers_init:
@@ -2910,38 +3017,26 @@ err_rx_listener_register:
 static void mlxsw_sp_fini(struct mlxsw_core *mlxsw_core)
 {
 	struct mlxsw_sp *mlxsw_sp = mlxsw_core_driver_priv(mlxsw_core);
-	int i;
 
 	mlxsw_sp_ports_remove(mlxsw_sp);
 	mlxsw_sp_span_fini(mlxsw_sp);
 	mlxsw_sp_router_fini(mlxsw_sp);
 	mlxsw_sp_switchdev_fini(mlxsw_sp);
+	mlxsw_sp_lag_fini(mlxsw_sp);
 	mlxsw_sp_buffers_fini(mlxsw_sp);
 	mlxsw_sp_traps_fini(mlxsw_sp);
 	mlxsw_sp_event_unregister(mlxsw_sp, MLXSW_TRAP_ID_PUDE);
 	WARN_ON(!list_empty(&mlxsw_sp->vfids.list));
 	WARN_ON(!list_empty(&mlxsw_sp->fids));
-	for (i = 0; i < MLXSW_SP_RIF_MAX; i++)
-		WARN_ON_ONCE(mlxsw_sp->rifs[i]);
 }
 
 static struct mlxsw_config_profile mlxsw_sp_config_profile = {
 	.used_max_vepa_channels		= 1,
 	.max_vepa_channels		= 0,
-	.used_max_lag			= 1,
-	.max_lag			= MLXSW_SP_LAG_MAX,
-	.used_max_port_per_lag		= 1,
-	.max_port_per_lag		= MLXSW_SP_PORT_PER_LAG_MAX,
 	.used_max_mid			= 1,
 	.max_mid			= MLXSW_SP_MID_MAX,
 	.used_max_pgt			= 1,
 	.max_pgt			= 0,
-	.used_max_system_port		= 1,
-	.max_system_port		= 64,
-	.used_max_vlan_groups		= 1,
-	.max_vlan_groups		= 127,
-	.used_max_regions		= 1,
-	.max_regions			= 400,
 	.used_flood_tables		= 1,
 	.used_flood_mode		= 1,
 	.flood_mode			= 3,
@@ -2953,10 +3048,11 @@ static struct mlxsw_config_profile mlxsw_sp_config_profile = {
 	.max_ib_mc			= 0,
 	.used_max_pkey			= 1,
 	.max_pkey			= 0,
-	.used_kvd_sizes			= 1,
+	.used_kvd_split_data		= 1,
+	.kvd_hash_granularity		= MLXSW_SP_KVD_GRANULARITY,
+	.kvd_hash_single_parts		= 2,
+	.kvd_hash_double_parts		= 1,
 	.kvd_linear_size		= MLXSW_SP_KVD_LINEAR_SIZE,
-	.kvd_hash_single_size		= MLXSW_SP_KVD_HASH_SINGLE_SIZE,
-	.kvd_hash_double_size		= MLXSW_SP_KVD_HASH_DOUBLE_SIZE,
 	.swid_config			= {
 		{
 			.used_type	= 1,
@@ -3073,13 +3169,15 @@ static bool mlxsw_sp_rif_should_config(struct mlxsw_sp_rif *r,
 
 static int mlxsw_sp_avail_rif_get(struct mlxsw_sp *mlxsw_sp)
 {
+	struct mlxsw_resources *resources;
 	int i;
 
-	for (i = 0; i < MLXSW_SP_RIF_MAX; i++)
+	resources = mlxsw_core_resources_get(mlxsw_sp->core);
+	for (i = 0; i < resources->max_rif; i++)
 		if (!mlxsw_sp->rifs[i])
 			return i;
 
-	return MLXSW_SP_RIF_MAX;
+	return MLXSW_SP_INVALID_RIF;
 }
 
 static void mlxsw_sp_vport_rif_sp_attr_get(struct mlxsw_sp_port *mlxsw_sp_vport,
@@ -3159,7 +3257,7 @@ mlxsw_sp_vport_rif_sp_create(struct mlxsw_sp_port *mlxsw_sp_vport,
 	int err;
 
 	rif = mlxsw_sp_avail_rif_get(mlxsw_sp);
-	if (rif == MLXSW_SP_RIF_MAX)
+	if (rif == MLXSW_SP_INVALID_RIF)
 		return ERR_PTR(-ERANGE);
 
 	err = mlxsw_sp_vport_rif_sp_op(mlxsw_sp_vport, l3_dev, rif, true);
@@ -3391,7 +3489,7 @@ static int mlxsw_sp_rif_bridge_create(struct mlxsw_sp *mlxsw_sp,
 	int err;
 
 	rif = mlxsw_sp_avail_rif_get(mlxsw_sp);
-	if (rif == MLXSW_SP_RIF_MAX)
+	if (rif == MLXSW_SP_INVALID_RIF)
 		return -ERANGE;
 
 	err = mlxsw_sp_router_port_flood_set(mlxsw_sp, f->fid, true);
@@ -3598,12 +3696,14 @@ static bool mlxsw_sp_port_fdb_should_flush(struct mlxsw_sp_port *mlxsw_sp_port,
 	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
 	u8 local_port = mlxsw_sp_port->local_port;
 	u16 lag_id = mlxsw_sp_port->lag_id;
+	struct mlxsw_resources *resources;
 	int i, count = 0;
 
 	if (!mlxsw_sp_port->lagged)
 		return true;
 
-	for (i = 0; i < MLXSW_SP_PORT_PER_LAG_MAX; i++) {
+	resources = mlxsw_core_resources_get(mlxsw_sp->core);
+	for (i = 0; i < resources->max_ports_in_lag; i++) {
 		struct mlxsw_sp_port *lag_port;
 
 		lag_port = mlxsw_sp_port_lagged_get(mlxsw_sp, lag_id, i);
@@ -3809,11 +3909,13 @@ static int mlxsw_sp_lag_index_get(struct mlxsw_sp *mlxsw_sp,
 				  struct net_device *lag_dev,
 				  u16 *p_lag_id)
 {
+	struct mlxsw_resources *resources;
 	struct mlxsw_sp_upper *lag;
 	int free_lag_id = -1;
 	int i;
 
-	for (i = 0; i < MLXSW_SP_LAG_MAX; i++) {
+	resources = mlxsw_core_resources_get(mlxsw_sp->core);
+	for (i = 0; i < resources->max_lag; i++) {
 		lag = mlxsw_sp_lag_get(mlxsw_sp, i);
 		if (lag->ref_count) {
 			if (lag->dev == lag_dev) {
@@ -3847,9 +3949,11 @@ mlxsw_sp_master_lag_check(struct mlxsw_sp *mlxsw_sp,
 static int mlxsw_sp_port_lag_index_get(struct mlxsw_sp *mlxsw_sp,
 				       u16 lag_id, u8 *p_port_index)
 {
+	struct mlxsw_resources *resources;
 	int i;
 
-	for (i = 0; i < MLXSW_SP_PORT_PER_LAG_MAX; i++) {
+	resources = mlxsw_core_resources_get(mlxsw_sp->core);
+	for (i = 0; i < resources->max_ports_in_lag; i++) {
 		if (!mlxsw_sp_port_lagged_get(mlxsw_sp, lag_id, i)) {
 			*p_port_index = i;
 			return 0;
diff --git a/drivers/net/ethernet/mellanox/mlxsw/spectrum.h b/drivers/net/ethernet/mellanox/mlxsw/spectrum.h
index ac48abebe904..9b22863a924b 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/spectrum.h
+++ b/drivers/net/ethernet/mellanox/mlxsw/spectrum.h
@@ -45,7 +45,7 @@
 #include <linux/list.h>
 #include <linux/dcbnl.h>
 #include <linux/in6.h>
-#include <net/switchdev.h>
+#include <linux/notifier.h>
 
 #include "port.h"
 #include "core.h"
@@ -54,10 +54,7 @@
 #define MLXSW_SP_VFID_MAX 6656	/* Bridged VLAN interfaces */
 
 #define MLXSW_SP_RFID_BASE 15360
-#define MLXSW_SP_RIF_MAX 800
-
-#define MLXSW_SP_LAG_MAX 64
-#define MLXSW_SP_PORT_PER_LAG_MAX 16
+#define MLXSW_SP_INVALID_RIF 0xffff
 
 #define MLXSW_SP_MID_MAX 7000
 
@@ -67,8 +64,6 @@
 #define MLXSW_SP_LPM_TREE_MAX 22
 #define MLXSW_SP_LPM_TREE_COUNT (MLXSW_SP_LPM_TREE_MAX - MLXSW_SP_LPM_TREE_MIN)
 
-#define MLXSW_SP_VIRTUAL_ROUTER_MAX 256
-
 #define MLXSW_SP_PORT_BASE_SPEED 25000	/* Mb/s */
 
 #define MLXSW_SP_BYTES_PER_CELL 96
@@ -77,8 +72,7 @@
 #define MLXSW_SP_CELLS_TO_BYTES(c) (c * MLXSW_SP_BYTES_PER_CELL)
 
 #define MLXSW_SP_KVD_LINEAR_SIZE 65536 /* entries */
-#define MLXSW_SP_KVD_HASH_SINGLE_SIZE 163840 /* entries */
-#define MLXSW_SP_KVD_HASH_DOUBLE_SIZE 32768 /* entries */
+#define MLXSW_SP_KVD_GRANULARITY 128
 
 /* Maximum delay buffer needed in case of PAUSE frames, in cells.
  * Assumes 100m cable and maximum MTU.
@@ -253,7 +247,7 @@ struct mlxsw_sp_port_mall_tc_entry {
 
 struct mlxsw_sp_router {
 	struct mlxsw_sp_lpm_tree lpm_trees[MLXSW_SP_LPM_TREE_COUNT];
-	struct mlxsw_sp_vr vrs[MLXSW_SP_VIRTUAL_ROUTER_MAX];
+	struct mlxsw_sp_vr *vrs;
 	struct rhashtable neigh_ht;
 	struct {
 		struct delayed_work dw;
@@ -263,6 +257,7 @@ struct mlxsw_sp_router {
 #define MLXSW_SP_UNRESOLVED_NH_PROBE_INTERVAL 5000 /* ms */
 	struct list_head nexthop_group_list;
 	struct list_head nexthop_neighs_list;
+	bool aborted;
 };
 
 struct mlxsw_sp {
@@ -275,7 +270,7 @@ struct mlxsw_sp {
 		DECLARE_BITMAP(mapped, MLXSW_SP_MID_MAX);
 	} br_mids;
 	struct list_head fids;	/* VLAN-aware bridge FIDs */
-	struct mlxsw_sp_rif *rifs[MLXSW_SP_RIF_MAX];
+	struct mlxsw_sp_rif **rifs;
 	struct mlxsw_sp_port **ports;
 	struct mlxsw_core *core;
 	const struct mlxsw_bus_info *bus_info;
@@ -290,7 +285,7 @@ struct mlxsw_sp {
 #define MLXSW_SP_DEFAULT_AGEING_TIME 300
 	u32 ageing_time;
 	struct mlxsw_sp_upper master_bridge;
-	struct mlxsw_sp_upper lags[MLXSW_SP_LAG_MAX];
+	struct mlxsw_sp_upper *lags;
 	u8 port_to_module[MLXSW_PORT_MAX_PORTS];
 	struct mlxsw_sp_sb sb;
 	struct mlxsw_sp_router router;
@@ -302,6 +297,7 @@ struct mlxsw_sp {
 		struct mlxsw_sp_span_entry *entries;
 		int entries_count;
 	} span;
+	struct notifier_block fib_nb;
 };
 
 static inline struct mlxsw_sp_upper *
@@ -341,7 +337,8 @@ struct mlxsw_sp_port {
 	} vport;
 	struct {
 		u8 tx_pause:1,
-		   rx_pause:1;
+		   rx_pause:1,
+		   autoneg:1;
 	} link;
 	struct {
 		struct ieee_ets *ets;
@@ -360,6 +357,11 @@ struct mlxsw_sp_port {
 	struct list_head vports_list;
 	/* TC handles */
 	struct list_head mall_tc_list;
+	struct {
+		#define MLXSW_HW_STATS_UPDATE_TIME HZ
+		struct rtnl_link_stats64 *cache;
+		struct delayed_work update_dw;
+	} hw_stats;
 };
 
 struct mlxsw_sp_port *mlxsw_sp_port_lower_dev_hold(struct net_device *dev);
@@ -477,9 +479,12 @@ static inline struct mlxsw_sp_rif *
 mlxsw_sp_rif_find_by_dev(const struct mlxsw_sp *mlxsw_sp,
 			 const struct net_device *dev)
 {
+	struct mlxsw_resources *resources;
 	int i;
 
-	for (i = 0; i < MLXSW_SP_RIF_MAX; i++)
+	resources = mlxsw_core_resources_get(mlxsw_sp->core);
+
+	for (i = 0; i < resources->max_rif; i++)
 		if (mlxsw_sp->rifs[i] && mlxsw_sp->rifs[i]->dev == dev)
 			return mlxsw_sp->rifs[i];
 
@@ -558,6 +563,9 @@ int __mlxsw_sp_port_headroom_set(struct mlxsw_sp_port *mlxsw_sp_port, int mtu,
 int mlxsw_sp_port_ets_maxrate_set(struct mlxsw_sp_port *mlxsw_sp_port,
 				  enum mlxsw_reg_qeec_hr hr, u8 index,
 				  u8 next_index, u32 maxrate);
+int __mlxsw_sp_port_vid_learning_set(struct mlxsw_sp_port *mlxsw_sp_port,
+				     u16 vid_begin, u16 vid_end,
+				     bool learn_enable);
 
 #ifdef CONFIG_MLXSW_SPECTRUM_DCB
 
@@ -578,11 +586,6 @@ static inline void mlxsw_sp_port_dcb_fini(struct mlxsw_sp_port *mlxsw_sp_port)
 
 int mlxsw_sp_router_init(struct mlxsw_sp *mlxsw_sp);
 void mlxsw_sp_router_fini(struct mlxsw_sp *mlxsw_sp);
-int mlxsw_sp_router_fib4_add(struct mlxsw_sp_port *mlxsw_sp_port,
-			     const struct switchdev_obj_ipv4_fib *fib4,
-			     struct switchdev_trans *trans);
-int mlxsw_sp_router_fib4_del(struct mlxsw_sp_port *mlxsw_sp_port,
-			     const struct switchdev_obj_ipv4_fib *fib4);
 int mlxsw_sp_router_neigh_construct(struct net_device *dev,
 				    struct neighbour *n);
 void mlxsw_sp_router_neigh_destroy(struct net_device *dev,
diff --git a/drivers/net/ethernet/mellanox/mlxsw/spectrum_buffers.c b/drivers/net/ethernet/mellanox/mlxsw/spectrum_buffers.c
index 953b214f38d0..bcaed8a38037 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/spectrum_buffers.c
+++ b/drivers/net/ethernet/mellanox/mlxsw/spectrum_buffers.c
@@ -595,9 +595,9 @@ int mlxsw_sp_sb_pool_get(struct mlxsw_core *mlxsw_core,
 	enum mlxsw_reg_sbxx_dir dir = dir_get(pool_index);
 	struct mlxsw_sp_sb_pr *pr = mlxsw_sp_sb_pr_get(mlxsw_sp, pool, dir);
 
-	pool_info->pool_type = dir;
+	pool_info->pool_type = (enum devlink_sb_pool_type) dir;
 	pool_info->size = MLXSW_SP_CELLS_TO_BYTES(pr->size);
-	pool_info->threshold_type = pr->mode;
+	pool_info->threshold_type = (enum devlink_sb_threshold_type) pr->mode;
 	return 0;
 }
 
@@ -608,9 +608,10 @@ int mlxsw_sp_sb_pool_set(struct mlxsw_core *mlxsw_core,
 	struct mlxsw_sp *mlxsw_sp = mlxsw_core_driver_priv(mlxsw_core);
 	u8 pool = pool_get(pool_index);
 	enum mlxsw_reg_sbxx_dir dir = dir_get(pool_index);
-	enum mlxsw_reg_sbpr_mode mode = threshold_type;
 	u32 pool_size = MLXSW_SP_BYTES_TO_CELLS(size);
+	enum mlxsw_reg_sbpr_mode mode;
 
+	mode = (enum mlxsw_reg_sbpr_mode) threshold_type;
 	return mlxsw_sp_sb_pr_write(mlxsw_sp, pool, dir, mode, pool_size);
 }
 
@@ -696,13 +697,13 @@ int mlxsw_sp_sb_tc_pool_bind_get(struct mlxsw_core_port *mlxsw_core_port,
 	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
 	u8 local_port = mlxsw_sp_port->local_port;
 	u8 pg_buff = tc_index;
-	enum mlxsw_reg_sbxx_dir dir = pool_type;
+	enum mlxsw_reg_sbxx_dir dir = (enum mlxsw_reg_sbxx_dir) pool_type;
 	struct mlxsw_sp_sb_cm *cm = mlxsw_sp_sb_cm_get(mlxsw_sp, local_port,
 						       pg_buff, dir);
 
 	*p_threshold = mlxsw_sp_sb_threshold_out(mlxsw_sp, cm->pool, dir,
 						 cm->max_buff);
-	*p_pool_index = pool_index_get(cm->pool, pool_type);
+	*p_pool_index = pool_index_get(cm->pool, dir);
 	return 0;
 }
 
@@ -716,7 +717,7 @@ int mlxsw_sp_sb_tc_pool_bind_set(struct mlxsw_core_port *mlxsw_core_port,
 	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
 	u8 local_port = mlxsw_sp_port->local_port;
 	u8 pg_buff = tc_index;
-	enum mlxsw_reg_sbxx_dir dir = pool_type;
+	enum mlxsw_reg_sbxx_dir dir = (enum mlxsw_reg_sbxx_dir) pool_type;
 	u8 pool = pool_get(pool_index);
 	u32 max_buff;
 	int err;
@@ -943,7 +944,7 @@ int mlxsw_sp_sb_occ_tc_port_bind_get(struct mlxsw_core_port *mlxsw_core_port,
 	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
 	u8 local_port = mlxsw_sp_port->local_port;
 	u8 pg_buff = tc_index;
-	enum mlxsw_reg_sbxx_dir dir = pool_type;
+	enum mlxsw_reg_sbxx_dir dir = (enum mlxsw_reg_sbxx_dir) pool_type;
 	struct mlxsw_sp_sb_cm *cm = mlxsw_sp_sb_cm_get(mlxsw_sp, local_port,
 						       pg_buff, dir);
 
diff --git a/drivers/net/ethernet/mellanox/mlxsw/spectrum_router.c b/drivers/net/ethernet/mellanox/mlxsw/spectrum_router.c
index 3f5c51da6d3e..78fc557d6dd7 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/spectrum_router.c
+++ b/drivers/net/ethernet/mellanox/mlxsw/spectrum_router.c
@@ -43,6 +43,7 @@
 #include <net/netevent.h>
 #include <net/neighbour.h>
 #include <net/arp.h>
+#include <net/ip_fib.h>
 
 #include "spectrum.h"
 #include "core.h"
@@ -122,17 +123,20 @@ struct mlxsw_sp_nexthop_group;
 
 struct mlxsw_sp_fib_entry {
 	struct rhash_head ht_node;
+	struct list_head list;
 	struct mlxsw_sp_fib_key key;
 	enum mlxsw_sp_fib_entry_type type;
 	unsigned int ref_count;
 	u16 rif; /* used for action local */
 	struct mlxsw_sp_vr *vr;
+	struct fib_info *fi;
 	struct list_head nexthop_group_node;
 	struct mlxsw_sp_nexthop_group *nh_group;
 };
 
 struct mlxsw_sp_fib {
 	struct rhashtable ht;
+	struct list_head entry_list;
 	unsigned long prefix_ref_count[MLXSW_SP_PREFIX_COUNT];
 	struct mlxsw_sp_prefix_usage prefix_usage;
 };
@@ -154,6 +158,7 @@ static int mlxsw_sp_fib_entry_insert(struct mlxsw_sp_fib *fib,
 				     mlxsw_sp_fib_ht_params);
 	if (err)
 		return err;
+	list_add_tail(&fib_entry->list, &fib->entry_list);
 	if (fib->prefix_ref_count[prefix_len]++ == 0)
 		mlxsw_sp_prefix_usage_set(&fib->prefix_usage, prefix_len);
 	return 0;
@@ -166,6 +171,7 @@ static void mlxsw_sp_fib_entry_remove(struct mlxsw_sp_fib *fib,
 
 	if (--fib->prefix_ref_count[prefix_len] == 0)
 		mlxsw_sp_prefix_usage_clear(&fib->prefix_usage, prefix_len);
+	list_del(&fib_entry->list);
 	rhashtable_remove_fast(&fib->ht, &fib_entry->ht_node,
 			       mlxsw_sp_fib_ht_params);
 }
@@ -216,6 +222,7 @@ static struct mlxsw_sp_fib *mlxsw_sp_fib_create(void)
 	err = rhashtable_init(&fib->ht, &mlxsw_sp_fib_ht_params);
 	if (err)
 		goto err_rhashtable_init;
+	INIT_LIST_HEAD(&fib->entry_list);
 	return fib;
 
 err_rhashtable_init:
@@ -252,7 +259,9 @@ static int mlxsw_sp_lpm_tree_alloc(struct mlxsw_sp *mlxsw_sp,
 {
 	char ralta_pl[MLXSW_REG_RALTA_LEN];
 
-	mlxsw_reg_ralta_pack(ralta_pl, true, lpm_tree->proto, lpm_tree->id);
+	mlxsw_reg_ralta_pack(ralta_pl, true,
+			     (enum mlxsw_reg_ralxx_protocol) lpm_tree->proto,
+			     lpm_tree->id);
 	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(ralta), ralta_pl);
 }
 
@@ -261,7 +270,9 @@ static int mlxsw_sp_lpm_tree_free(struct mlxsw_sp *mlxsw_sp,
 {
 	char ralta_pl[MLXSW_REG_RALTA_LEN];
 
-	mlxsw_reg_ralta_pack(ralta_pl, false, lpm_tree->proto, lpm_tree->id);
+	mlxsw_reg_ralta_pack(ralta_pl, false,
+			     (enum mlxsw_reg_ralxx_protocol) lpm_tree->proto,
+			     lpm_tree->id);
 	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(ralta), ralta_pl);
 }
 
@@ -368,10 +379,12 @@ static void mlxsw_sp_lpm_init(struct mlxsw_sp *mlxsw_sp)
 
 static struct mlxsw_sp_vr *mlxsw_sp_vr_find_unused(struct mlxsw_sp *mlxsw_sp)
 {
+	struct mlxsw_resources *resources;
 	struct mlxsw_sp_vr *vr;
 	int i;
 
-	for (i = 0; i < MLXSW_SP_VIRTUAL_ROUTER_MAX; i++) {
+	resources = mlxsw_core_resources_get(mlxsw_sp->core);
+	for (i = 0; i < resources->max_virtual_routers; i++) {
 		vr = &mlxsw_sp->router.vrs[i];
 		if (!vr->used)
 			return vr;
@@ -384,7 +397,9 @@ static int mlxsw_sp_vr_lpm_tree_bind(struct mlxsw_sp *mlxsw_sp,
 {
 	char raltb_pl[MLXSW_REG_RALTB_LEN];
 
-	mlxsw_reg_raltb_pack(raltb_pl, vr->id, vr->proto, vr->lpm_tree->id);
+	mlxsw_reg_raltb_pack(raltb_pl, vr->id,
+			     (enum mlxsw_reg_ralxx_protocol) vr->proto,
+			     vr->lpm_tree->id);
 	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(raltb), raltb_pl);
 }
 
@@ -394,7 +409,8 @@ static int mlxsw_sp_vr_lpm_tree_unbind(struct mlxsw_sp *mlxsw_sp,
 	char raltb_pl[MLXSW_REG_RALTB_LEN];
 
 	/* Bind to tree 0 which is default */
-	mlxsw_reg_raltb_pack(raltb_pl, vr->id, vr->proto, 0);
+	mlxsw_reg_raltb_pack(raltb_pl, vr->id,
+			     (enum mlxsw_reg_ralxx_protocol) vr->proto, 0);
 	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(raltb), raltb_pl);
 }
 
@@ -410,11 +426,14 @@ static struct mlxsw_sp_vr *mlxsw_sp_vr_find(struct mlxsw_sp *mlxsw_sp,
 					    u32 tb_id,
 					    enum mlxsw_sp_l3proto proto)
 {
+	struct mlxsw_resources *resources;
 	struct mlxsw_sp_vr *vr;
 	int i;
 
 	tb_id = mlxsw_sp_fix_tb_id(tb_id);
-	for (i = 0; i < MLXSW_SP_VIRTUAL_ROUTER_MAX; i++) {
+
+	resources = mlxsw_core_resources_get(mlxsw_sp->core);
+	for (i = 0; i < resources->max_virtual_routers; i++) {
 		vr = &mlxsw_sp->router.vrs[i];
 		if (vr->used && vr->proto == proto && vr->tb_id == tb_id)
 			return vr;
@@ -548,15 +567,33 @@ static void mlxsw_sp_vr_put(struct mlxsw_sp *mlxsw_sp, struct mlxsw_sp_vr *vr)
 					   &vr->fib->prefix_usage);
 }
 
-static void mlxsw_sp_vrs_init(struct mlxsw_sp *mlxsw_sp)
+static int mlxsw_sp_vrs_init(struct mlxsw_sp *mlxsw_sp)
 {
+	struct mlxsw_resources *resources;
 	struct mlxsw_sp_vr *vr;
 	int i;
 
-	for (i = 0; i < MLXSW_SP_VIRTUAL_ROUTER_MAX; i++) {
+	resources = mlxsw_core_resources_get(mlxsw_sp->core);
+	if (!resources->max_virtual_routers_valid)
+		return -EIO;
+
+	mlxsw_sp->router.vrs = kcalloc(resources->max_virtual_routers,
+				       sizeof(struct mlxsw_sp_vr),
+				       GFP_KERNEL);
+	if (!mlxsw_sp->router.vrs)
+		return -ENOMEM;
+
+	for (i = 0; i < resources->max_virtual_routers; i++) {
 		vr = &mlxsw_sp->router.vrs[i];
 		vr->id = i;
 	}
+
+	return 0;
+}
+
+static void mlxsw_sp_vrs_fini(struct mlxsw_sp *mlxsw_sp)
+{
+	kfree(mlxsw_sp->router.vrs);
 }
 
 struct mlxsw_sp_neigh_key {
@@ -1081,9 +1118,10 @@ static int mlxsw_sp_adj_index_mass_update_vr(struct mlxsw_sp *mlxsw_sp,
 {
 	char raleu_pl[MLXSW_REG_RALEU_LEN];
 
-	mlxsw_reg_raleu_pack(raleu_pl, vr->proto, vr->id,
-			     adj_index, ecmp_size,
-			     new_adj_index, new_ecmp_size);
+	mlxsw_reg_raleu_pack(raleu_pl,
+			     (enum mlxsw_reg_ralxx_protocol) vr->proto, vr->id,
+			     adj_index, ecmp_size, new_adj_index,
+			     new_ecmp_size);
 	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(raleu), raleu_pl);
 }
 
@@ -1489,50 +1527,6 @@ static void mlxsw_sp_nexthop_group_put(struct mlxsw_sp *mlxsw_sp,
 	mlxsw_sp_nexthop_group_destroy(mlxsw_sp, nh_grp);
 }
 
-static int __mlxsw_sp_router_init(struct mlxsw_sp *mlxsw_sp)
-{
-	char rgcr_pl[MLXSW_REG_RGCR_LEN];
-
-	mlxsw_reg_rgcr_pack(rgcr_pl, true);
-	mlxsw_reg_rgcr_max_router_interfaces_set(rgcr_pl, MLXSW_SP_RIF_MAX);
-	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(rgcr), rgcr_pl);
-}
-
-static void __mlxsw_sp_router_fini(struct mlxsw_sp *mlxsw_sp)
-{
-	char rgcr_pl[MLXSW_REG_RGCR_LEN];
-
-	mlxsw_reg_rgcr_pack(rgcr_pl, false);
-	mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(rgcr), rgcr_pl);
-}
-
-int mlxsw_sp_router_init(struct mlxsw_sp *mlxsw_sp)
-{
-	int err;
-
-	INIT_LIST_HEAD(&mlxsw_sp->router.nexthop_neighs_list);
-	INIT_LIST_HEAD(&mlxsw_sp->router.nexthop_group_list);
-	err = __mlxsw_sp_router_init(mlxsw_sp);
-	if (err)
-		return err;
-	mlxsw_sp_lpm_init(mlxsw_sp);
-	mlxsw_sp_vrs_init(mlxsw_sp);
-	err = mlxsw_sp_neigh_init(mlxsw_sp);
-	if (err)
-		goto err_neigh_init;
-	return 0;
-
-err_neigh_init:
-	__mlxsw_sp_router_fini(mlxsw_sp);
-	return err;
-}
-
-void mlxsw_sp_router_fini(struct mlxsw_sp *mlxsw_sp)
-{
-	mlxsw_sp_neigh_fini(mlxsw_sp);
-	__mlxsw_sp_router_fini(mlxsw_sp);
-}
-
 static int mlxsw_sp_fib_entry_op4_remote(struct mlxsw_sp *mlxsw_sp,
 					 struct mlxsw_sp_fib_entry *fib_entry,
 					 enum mlxsw_reg_ralue_op op)
@@ -1558,8 +1552,9 @@ static int mlxsw_sp_fib_entry_op4_remote(struct mlxsw_sp *mlxsw_sp,
 		trap_id = MLXSW_TRAP_ID_RTR_INGRESS0;
 	}
 
-	mlxsw_reg_ralue_pack4(ralue_pl, vr->proto, op, vr->id,
-			      fib_entry->key.prefix_len, *p_dip);
+	mlxsw_reg_ralue_pack4(ralue_pl,
+			      (enum mlxsw_reg_ralxx_protocol) vr->proto, op,
+			      vr->id, fib_entry->key.prefix_len, *p_dip);
 	mlxsw_reg_ralue_act_remote_pack(ralue_pl, trap_action, trap_id,
 					adjacency_index, ecmp_size);
 	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(ralue), ralue_pl);
@@ -1573,8 +1568,9 @@ static int mlxsw_sp_fib_entry_op4_local(struct mlxsw_sp *mlxsw_sp,
 	u32 *p_dip = (u32 *) fib_entry->key.addr;
 	struct mlxsw_sp_vr *vr = fib_entry->vr;
 
-	mlxsw_reg_ralue_pack4(ralue_pl, vr->proto, op, vr->id,
-			      fib_entry->key.prefix_len, *p_dip);
+	mlxsw_reg_ralue_pack4(ralue_pl,
+			      (enum mlxsw_reg_ralxx_protocol) vr->proto, op,
+			      vr->id, fib_entry->key.prefix_len, *p_dip);
 	mlxsw_reg_ralue_act_local_pack(ralue_pl,
 				       MLXSW_REG_RALUE_TRAP_ACTION_NOP, 0,
 				       fib_entry->rif);
@@ -1589,8 +1585,9 @@ static int mlxsw_sp_fib_entry_op4_trap(struct mlxsw_sp *mlxsw_sp,
 	u32 *p_dip = (u32 *) fib_entry->key.addr;
 	struct mlxsw_sp_vr *vr = fib_entry->vr;
 
-	mlxsw_reg_ralue_pack4(ralue_pl, vr->proto, op, vr->id,
-			      fib_entry->key.prefix_len, *p_dip);
+	mlxsw_reg_ralue_pack4(ralue_pl,
+			      (enum mlxsw_reg_ralxx_protocol) vr->proto, op,
+			      vr->id, fib_entry->key.prefix_len, *p_dip);
 	mlxsw_reg_ralue_act_ip2me_pack(ralue_pl);
 	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(ralue), ralue_pl);
 }
@@ -1637,94 +1634,102 @@ static int mlxsw_sp_fib_entry_del(struct mlxsw_sp *mlxsw_sp,
 				     MLXSW_REG_RALUE_OP_WRITE_DELETE);
 }
 
-struct mlxsw_sp_router_fib4_add_info {
-	struct switchdev_trans_item tritem;
-	struct mlxsw_sp *mlxsw_sp;
-	struct mlxsw_sp_fib_entry *fib_entry;
-};
-
-static void mlxsw_sp_router_fib4_add_info_destroy(void const *data)
-{
-	const struct mlxsw_sp_router_fib4_add_info *info = data;
-	struct mlxsw_sp_fib_entry *fib_entry = info->fib_entry;
-	struct mlxsw_sp *mlxsw_sp = info->mlxsw_sp;
-	struct mlxsw_sp_vr *vr = fib_entry->vr;
-
-	mlxsw_sp_fib_entry_destroy(fib_entry);
-	mlxsw_sp_vr_put(mlxsw_sp, vr);
-	kfree(info);
-}
-
 static int
 mlxsw_sp_router_fib4_entry_init(struct mlxsw_sp *mlxsw_sp,
-				const struct switchdev_obj_ipv4_fib *fib4,
+				const struct fib_entry_notifier_info *fen_info,
 				struct mlxsw_sp_fib_entry *fib_entry)
 {
-	struct fib_info *fi = fib4->fi;
+	struct fib_info *fi = fen_info->fi;
+	struct mlxsw_sp_rif *r = NULL;
+	int nhsel;
+	int err;
 
-	if (fib4->type == RTN_LOCAL || fib4->type == RTN_BROADCAST) {
+	if (fen_info->type == RTN_LOCAL || fen_info->type == RTN_BROADCAST) {
 		fib_entry->type = MLXSW_SP_FIB_ENTRY_TYPE_TRAP;
 		return 0;
 	}
-	if (fib4->type != RTN_UNICAST)
+	if (fen_info->type != RTN_UNICAST)
 		return -EINVAL;
 
-	if (fi->fib_scope != RT_SCOPE_UNIVERSE) {
-		struct mlxsw_sp_rif *r;
+	for (nhsel = 0; nhsel < fi->fib_nhs; nhsel++) {
+		const struct fib_nh *nh = &fi->fib_nh[nhsel];
+
+		if (!nh->nh_dev)
+			continue;
+		r = mlxsw_sp_rif_find_by_dev(mlxsw_sp, nh->nh_dev);
+		if (!r) {
+			/* In case router interface is not found for
+			 * at least one of the nexthops, that means
+			 * the nexthop points to some device unrelated
+			 * to us. Set trap and pass the packets for
+			 * this prefix to kernel.
+			 */
+			break;
+		}
+	}
 
+	if (!r) {
+		fib_entry->type = MLXSW_SP_FIB_ENTRY_TYPE_TRAP;
+		return 0;
+	}
+
+	if (fi->fib_scope != RT_SCOPE_UNIVERSE) {
 		fib_entry->type = MLXSW_SP_FIB_ENTRY_TYPE_LOCAL;
-		r = mlxsw_sp_rif_find_by_dev(mlxsw_sp, fi->fib_dev);
-		if (!r)
-			return -EINVAL;
 		fib_entry->rif = r->rif;
-		return 0;
+	} else {
+		fib_entry->type = MLXSW_SP_FIB_ENTRY_TYPE_REMOTE;
+		err = mlxsw_sp_nexthop_group_get(mlxsw_sp, fib_entry, fi);
+		if (err)
+			return err;
 	}
-	fib_entry->type = MLXSW_SP_FIB_ENTRY_TYPE_REMOTE;
-	return mlxsw_sp_nexthop_group_get(mlxsw_sp, fib_entry, fi);
+	fib_info_offload_inc(fen_info->fi);
+	return 0;
 }
 
 static void
 mlxsw_sp_router_fib4_entry_fini(struct mlxsw_sp *mlxsw_sp,
 				struct mlxsw_sp_fib_entry *fib_entry)
 {
-	if (fib_entry->type != MLXSW_SP_FIB_ENTRY_TYPE_REMOTE)
-		return;
-	mlxsw_sp_nexthop_group_put(mlxsw_sp, fib_entry);
+	if (fib_entry->type != MLXSW_SP_FIB_ENTRY_TYPE_TRAP)
+		fib_info_offload_dec(fib_entry->fi);
+	if (fib_entry->type == MLXSW_SP_FIB_ENTRY_TYPE_REMOTE)
+		mlxsw_sp_nexthop_group_put(mlxsw_sp, fib_entry);
 }
 
 static struct mlxsw_sp_fib_entry *
 mlxsw_sp_fib_entry_get(struct mlxsw_sp *mlxsw_sp,
-		       const struct switchdev_obj_ipv4_fib *fib4)
+		       const struct fib_entry_notifier_info *fen_info)
 {
 	struct mlxsw_sp_fib_entry *fib_entry;
-	struct fib_info *fi = fib4->fi;
+	struct fib_info *fi = fen_info->fi;
 	struct mlxsw_sp_vr *vr;
 	int err;
 
-	vr = mlxsw_sp_vr_get(mlxsw_sp, fib4->dst_len, fib4->tb_id,
+	vr = mlxsw_sp_vr_get(mlxsw_sp, fen_info->dst_len, fen_info->tb_id,
 			     MLXSW_SP_L3_PROTO_IPV4);
 	if (IS_ERR(vr))
 		return ERR_CAST(vr);
 
-	fib_entry = mlxsw_sp_fib_entry_lookup(vr->fib, &fib4->dst,
-					      sizeof(fib4->dst),
-					      fib4->dst_len, fi->fib_dev);
+	fib_entry = mlxsw_sp_fib_entry_lookup(vr->fib, &fen_info->dst,
+					      sizeof(fen_info->dst),
+					      fen_info->dst_len, fi->fib_dev);
 	if (fib_entry) {
 		/* Already exists, just take a reference */
 		fib_entry->ref_count++;
 		return fib_entry;
 	}
-	fib_entry = mlxsw_sp_fib_entry_create(vr->fib, &fib4->dst,
-					      sizeof(fib4->dst),
-					      fib4->dst_len, fi->fib_dev);
+	fib_entry = mlxsw_sp_fib_entry_create(vr->fib, &fen_info->dst,
+					      sizeof(fen_info->dst),
+					      fen_info->dst_len, fi->fib_dev);
 	if (!fib_entry) {
 		err = -ENOMEM;
 		goto err_fib_entry_create;
 	}
 	fib_entry->vr = vr;
+	fib_entry->fi = fi;
 	fib_entry->ref_count = 1;
 
-	err = mlxsw_sp_router_fib4_entry_init(mlxsw_sp, fib4, fib_entry);
+	err = mlxsw_sp_router_fib4_entry_init(mlxsw_sp, fen_info, fib_entry);
 	if (err)
 		goto err_fib4_entry_init;
 
@@ -1740,21 +1745,23 @@ err_fib_entry_create:
 
 static struct mlxsw_sp_fib_entry *
 mlxsw_sp_fib_entry_find(struct mlxsw_sp *mlxsw_sp,
-			const struct switchdev_obj_ipv4_fib *fib4)
+			const struct fib_entry_notifier_info *fen_info)
 {
 	struct mlxsw_sp_vr *vr;
 
-	vr = mlxsw_sp_vr_find(mlxsw_sp, fib4->tb_id, MLXSW_SP_L3_PROTO_IPV4);
+	vr = mlxsw_sp_vr_find(mlxsw_sp, fen_info->tb_id,
+			      MLXSW_SP_L3_PROTO_IPV4);
 	if (!vr)
 		return NULL;
 
-	return mlxsw_sp_fib_entry_lookup(vr->fib, &fib4->dst,
-					 sizeof(fib4->dst), fib4->dst_len,
-					 fib4->fi->fib_dev);
+	return mlxsw_sp_fib_entry_lookup(vr->fib, &fen_info->dst,
+					 sizeof(fen_info->dst),
+					 fen_info->dst_len,
+					 fen_info->fi->fib_dev);
 }
 
-void mlxsw_sp_fib_entry_put(struct mlxsw_sp *mlxsw_sp,
-			    struct mlxsw_sp_fib_entry *fib_entry)
+static void mlxsw_sp_fib_entry_put(struct mlxsw_sp *mlxsw_sp,
+				   struct mlxsw_sp_fib_entry *fib_entry)
 {
 	struct mlxsw_sp_vr *vr = fib_entry->vr;
 
@@ -1765,60 +1772,43 @@ void mlxsw_sp_fib_entry_put(struct mlxsw_sp *mlxsw_sp,
 	mlxsw_sp_vr_put(mlxsw_sp, vr);
 }
 
-static int
-mlxsw_sp_router_fib4_add_prepare(struct mlxsw_sp_port *mlxsw_sp_port,
-				 const struct switchdev_obj_ipv4_fib *fib4,
-				 struct switchdev_trans *trans)
+static void mlxsw_sp_fib_entry_put_all(struct mlxsw_sp *mlxsw_sp,
+				       struct mlxsw_sp_fib_entry *fib_entry)
 {
-	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
-	struct mlxsw_sp_router_fib4_add_info *info;
-	struct mlxsw_sp_fib_entry *fib_entry;
-	int err;
+	unsigned int last_ref_count;
 
-	fib_entry = mlxsw_sp_fib_entry_get(mlxsw_sp, fib4);
-	if (IS_ERR(fib_entry))
-		return PTR_ERR(fib_entry);
-
-	info = kmalloc(sizeof(*info), GFP_KERNEL);
-	if (!info) {
-		err = -ENOMEM;
-		goto err_alloc_info;
-	}
-	info->mlxsw_sp = mlxsw_sp;
-	info->fib_entry = fib_entry;
-	switchdev_trans_item_enqueue(trans, info,
-				     mlxsw_sp_router_fib4_add_info_destroy,
-				     &info->tritem);
-	return 0;
-
-err_alloc_info:
-	mlxsw_sp_fib_entry_put(mlxsw_sp, fib_entry);
-	return err;
+	do {
+		last_ref_count = fib_entry->ref_count;
+		mlxsw_sp_fib_entry_put(mlxsw_sp, fib_entry);
+	} while (last_ref_count != 1);
 }
 
-static int
-mlxsw_sp_router_fib4_add_commit(struct mlxsw_sp_port *mlxsw_sp_port,
-				const struct switchdev_obj_ipv4_fib *fib4,
-				struct switchdev_trans *trans)
+static int mlxsw_sp_router_fib4_add(struct mlxsw_sp *mlxsw_sp,
+				    struct fib_entry_notifier_info *fen_info)
 {
-	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
-	struct mlxsw_sp_router_fib4_add_info *info;
 	struct mlxsw_sp_fib_entry *fib_entry;
 	struct mlxsw_sp_vr *vr;
 	int err;
 
-	info = switchdev_trans_item_dequeue(trans);
-	fib_entry = info->fib_entry;
-	kfree(info);
+	if (mlxsw_sp->router.aborted)
+		return 0;
+
+	fib_entry = mlxsw_sp_fib_entry_get(mlxsw_sp, fen_info);
+	if (IS_ERR(fib_entry)) {
+		dev_warn(mlxsw_sp->bus_info->dev, "Failed to get FIB4 entry being added.\n");
+		return PTR_ERR(fib_entry);
+	}
 
 	if (fib_entry->ref_count != 1)
 		return 0;
 
 	vr = fib_entry->vr;
 	err = mlxsw_sp_fib_entry_insert(vr->fib, fib_entry);
-	if (err)
+	if (err) {
+		dev_warn(mlxsw_sp->bus_info->dev, "Failed to insert FIB4 entry being added.\n");
 		goto err_fib_entry_insert;
-	err = mlxsw_sp_fib_entry_update(mlxsw_sp_port->mlxsw_sp, fib_entry);
+	}
+	err = mlxsw_sp_fib_entry_update(mlxsw_sp, fib_entry);
 	if (err)
 		goto err_fib_entry_add;
 	return 0;
@@ -1830,24 +1820,15 @@ err_fib_entry_insert:
 	return err;
 }
 
-int mlxsw_sp_router_fib4_add(struct mlxsw_sp_port *mlxsw_sp_port,
-			     const struct switchdev_obj_ipv4_fib *fib4,
-			     struct switchdev_trans *trans)
-{
-	if (switchdev_trans_ph_prepare(trans))
-		return mlxsw_sp_router_fib4_add_prepare(mlxsw_sp_port,
-							fib4, trans);
-	return mlxsw_sp_router_fib4_add_commit(mlxsw_sp_port,
-					       fib4, trans);
-}
-
-int mlxsw_sp_router_fib4_del(struct mlxsw_sp_port *mlxsw_sp_port,
-			     const struct switchdev_obj_ipv4_fib *fib4)
+static int mlxsw_sp_router_fib4_del(struct mlxsw_sp *mlxsw_sp,
+				    struct fib_entry_notifier_info *fen_info)
 {
-	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
 	struct mlxsw_sp_fib_entry *fib_entry;
 
-	fib_entry = mlxsw_sp_fib_entry_find(mlxsw_sp, fib4);
+	if (mlxsw_sp->router.aborted)
+		return 0;
+
+	fib_entry = mlxsw_sp_fib_entry_find(mlxsw_sp, fen_info);
 	if (!fib_entry) {
 		dev_warn(mlxsw_sp->bus_info->dev, "Failed to find FIB4 entry being removed.\n");
 		return -ENOENT;
@@ -1861,3 +1842,172 @@ int mlxsw_sp_router_fib4_del(struct mlxsw_sp_port *mlxsw_sp_port,
 	mlxsw_sp_fib_entry_put(mlxsw_sp, fib_entry);
 	return 0;
 }
+
+static int mlxsw_sp_router_set_abort_trap(struct mlxsw_sp *mlxsw_sp)
+{
+	char ralta_pl[MLXSW_REG_RALTA_LEN];
+	char ralst_pl[MLXSW_REG_RALST_LEN];
+	char raltb_pl[MLXSW_REG_RALTB_LEN];
+	char ralue_pl[MLXSW_REG_RALUE_LEN];
+	int err;
+
+	mlxsw_reg_ralta_pack(ralta_pl, true, MLXSW_REG_RALXX_PROTOCOL_IPV4,
+			     MLXSW_SP_LPM_TREE_MIN);
+	err = mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(ralta), ralta_pl);
+	if (err)
+		return err;
+
+	mlxsw_reg_ralst_pack(ralst_pl, 0xff, MLXSW_SP_LPM_TREE_MIN);
+	err = mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(ralst), ralst_pl);
+	if (err)
+		return err;
+
+	mlxsw_reg_raltb_pack(raltb_pl, 0, MLXSW_REG_RALXX_PROTOCOL_IPV4, 0);
+	err = mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(raltb), raltb_pl);
+	if (err)
+		return err;
+
+	mlxsw_reg_ralue_pack4(ralue_pl, MLXSW_SP_L3_PROTO_IPV4,
+			      MLXSW_REG_RALUE_OP_WRITE_WRITE, 0, 0, 0);
+	mlxsw_reg_ralue_act_ip2me_pack(ralue_pl);
+	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(ralue), ralue_pl);
+}
+
+static void mlxsw_sp_router_fib4_abort(struct mlxsw_sp *mlxsw_sp)
+{
+	struct mlxsw_resources *resources;
+	struct mlxsw_sp_fib_entry *fib_entry;
+	struct mlxsw_sp_fib_entry *tmp;
+	struct mlxsw_sp_vr *vr;
+	int i;
+	int err;
+
+	resources = mlxsw_core_resources_get(mlxsw_sp->core);
+	for (i = 0; i < resources->max_virtual_routers; i++) {
+		vr = &mlxsw_sp->router.vrs[i];
+		if (!vr->used)
+			continue;
+
+		list_for_each_entry_safe(fib_entry, tmp,
+					 &vr->fib->entry_list, list) {
+			bool do_break = &tmp->list == &vr->fib->entry_list;
+
+			mlxsw_sp_fib_entry_del(mlxsw_sp, fib_entry);
+			mlxsw_sp_fib_entry_remove(fib_entry->vr->fib,
+						  fib_entry);
+			mlxsw_sp_fib_entry_put_all(mlxsw_sp, fib_entry);
+			if (do_break)
+				break;
+		}
+	}
+	mlxsw_sp->router.aborted = true;
+	err = mlxsw_sp_router_set_abort_trap(mlxsw_sp);
+	if (err)
+		dev_warn(mlxsw_sp->bus_info->dev, "Failed to set abort trap.\n");
+}
+
+static int __mlxsw_sp_router_init(struct mlxsw_sp *mlxsw_sp)
+{
+	struct mlxsw_resources *resources;
+	char rgcr_pl[MLXSW_REG_RGCR_LEN];
+	int err;
+
+	resources = mlxsw_core_resources_get(mlxsw_sp->core);
+	if (!resources->max_rif_valid)
+		return -EIO;
+
+	mlxsw_sp->rifs = kcalloc(resources->max_rif,
+				 sizeof(struct mlxsw_sp_rif *), GFP_KERNEL);
+	if (!mlxsw_sp->rifs)
+		return -ENOMEM;
+
+	mlxsw_reg_rgcr_pack(rgcr_pl, true);
+	mlxsw_reg_rgcr_max_router_interfaces_set(rgcr_pl, resources->max_rif);
+	err = mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(rgcr), rgcr_pl);
+	if (err)
+		goto err_rgcr_fail;
+
+	return 0;
+
+err_rgcr_fail:
+	kfree(mlxsw_sp->rifs);
+	return err;
+}
+
+static void __mlxsw_sp_router_fini(struct mlxsw_sp *mlxsw_sp)
+{
+	struct mlxsw_resources *resources;
+	char rgcr_pl[MLXSW_REG_RGCR_LEN];
+	int i;
+
+	mlxsw_reg_rgcr_pack(rgcr_pl, false);
+	mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(rgcr), rgcr_pl);
+
+	resources = mlxsw_core_resources_get(mlxsw_sp->core);
+	for (i = 0; i < resources->max_rif; i++)
+		WARN_ON_ONCE(mlxsw_sp->rifs[i]);
+
+	kfree(mlxsw_sp->rifs);
+}
+
+static int mlxsw_sp_router_fib_event(struct notifier_block *nb,
+				     unsigned long event, void *ptr)
+{
+	struct mlxsw_sp *mlxsw_sp = container_of(nb, struct mlxsw_sp, fib_nb);
+	struct fib_entry_notifier_info *fen_info = ptr;
+	int err;
+
+	switch (event) {
+	case FIB_EVENT_ENTRY_ADD:
+		err = mlxsw_sp_router_fib4_add(mlxsw_sp, fen_info);
+		if (err)
+			mlxsw_sp_router_fib4_abort(mlxsw_sp);
+		break;
+	case FIB_EVENT_ENTRY_DEL:
+		mlxsw_sp_router_fib4_del(mlxsw_sp, fen_info);
+		break;
+	case FIB_EVENT_RULE_ADD: /* fall through */
+	case FIB_EVENT_RULE_DEL:
+		mlxsw_sp_router_fib4_abort(mlxsw_sp);
+		break;
+	}
+	return NOTIFY_DONE;
+}
+
+int mlxsw_sp_router_init(struct mlxsw_sp *mlxsw_sp)
+{
+	int err;
+
+	INIT_LIST_HEAD(&mlxsw_sp->router.nexthop_neighs_list);
+	INIT_LIST_HEAD(&mlxsw_sp->router.nexthop_group_list);
+	err = __mlxsw_sp_router_init(mlxsw_sp);
+	if (err)
+		return err;
+
+	mlxsw_sp_lpm_init(mlxsw_sp);
+	err = mlxsw_sp_vrs_init(mlxsw_sp);
+	if (err)
+		goto err_vrs_init;
+
+	err =  mlxsw_sp_neigh_init(mlxsw_sp);
+	if (err)
+		goto err_neigh_init;
+
+	mlxsw_sp->fib_nb.notifier_call = mlxsw_sp_router_fib_event;
+	register_fib_notifier(&mlxsw_sp->fib_nb);
+	return 0;
+
+err_neigh_init:
+	mlxsw_sp_vrs_fini(mlxsw_sp);
+err_vrs_init:
+	__mlxsw_sp_router_fini(mlxsw_sp);
+	return err;
+}
+
+void mlxsw_sp_router_fini(struct mlxsw_sp *mlxsw_sp)
+{
+	unregister_fib_notifier(&mlxsw_sp->fib_nb);
+	mlxsw_sp_neigh_fini(mlxsw_sp);
+	mlxsw_sp_vrs_fini(mlxsw_sp);
+	__mlxsw_sp_router_fini(mlxsw_sp);
+}
diff --git a/drivers/net/ethernet/mellanox/mlxsw/spectrum_switchdev.c b/drivers/net/ethernet/mellanox/mlxsw/spectrum_switchdev.c
index 7b654c517b91..5e00c79e8133 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/spectrum_switchdev.c
+++ b/drivers/net/ethernet/mellanox/mlxsw/spectrum_switchdev.c
@@ -254,12 +254,40 @@ int mlxsw_sp_vport_flood_set(struct mlxsw_sp_port *mlxsw_sp_vport, u16 fid,
 	return __mlxsw_sp_port_flood_set(mlxsw_sp_vport, vfid, vfid, set, set);
 }
 
+static int mlxsw_sp_port_learning_set(struct mlxsw_sp_port *mlxsw_sp_port,
+				      bool set)
+{
+	u16 vid;
+	int err;
+
+	if (mlxsw_sp_port_is_vport(mlxsw_sp_port)) {
+		vid = mlxsw_sp_vport_vid_get(mlxsw_sp_port);
+
+		return __mlxsw_sp_port_vid_learning_set(mlxsw_sp_port, vid, vid,
+							set);
+	}
+
+	for_each_set_bit(vid, mlxsw_sp_port->active_vlans, VLAN_N_VID) {
+		err = __mlxsw_sp_port_vid_learning_set(mlxsw_sp_port, vid, vid,
+						       set);
+		if (err)
+			goto err_port_vid_learning_set;
+	}
+
+	return 0;
+
+err_port_vid_learning_set:
+	for_each_set_bit(vid, mlxsw_sp_port->active_vlans, VLAN_N_VID)
+		__mlxsw_sp_port_vid_learning_set(mlxsw_sp_port, vid, vid, !set);
+	return err;
+}
+
 static int mlxsw_sp_port_attr_br_flags_set(struct mlxsw_sp_port *mlxsw_sp_port,
 					   struct switchdev_trans *trans,
 					   unsigned long brport_flags)
 {
+	unsigned long learning = mlxsw_sp_port->learning ? BR_LEARNING : 0;
 	unsigned long uc_flood = mlxsw_sp_port->uc_flood ? BR_FLOOD : 0;
-	bool set;
 	int err;
 
 	if (!mlxsw_sp_port->bridged)
@@ -269,17 +297,30 @@ static int mlxsw_sp_port_attr_br_flags_set(struct mlxsw_sp_port *mlxsw_sp_port,
 		return 0;
 
 	if ((uc_flood ^ brport_flags) & BR_FLOOD) {
-		set = mlxsw_sp_port->uc_flood ? false : true;
-		err = mlxsw_sp_port_uc_flood_set(mlxsw_sp_port, set);
+		err = mlxsw_sp_port_uc_flood_set(mlxsw_sp_port,
+						 !mlxsw_sp_port->uc_flood);
 		if (err)
 			return err;
 	}
 
+	if ((learning ^ brport_flags) & BR_LEARNING) {
+		err = mlxsw_sp_port_learning_set(mlxsw_sp_port,
+						 !mlxsw_sp_port->learning);
+		if (err)
+			goto err_port_learning_set;
+	}
+
 	mlxsw_sp_port->uc_flood = brport_flags & BR_FLOOD ? 1 : 0;
 	mlxsw_sp_port->learning = brport_flags & BR_LEARNING ? 1 : 0;
 	mlxsw_sp_port->learning_sync = brport_flags & BR_LEARNING_SYNC ? 1 : 0;
 
 	return 0;
+
+err_port_learning_set:
+	if ((uc_flood ^ brport_flags) & BR_FLOOD)
+		mlxsw_sp_port_uc_flood_set(mlxsw_sp_port,
+					   mlxsw_sp_port->uc_flood);
+	return err;
 }
 
 static int mlxsw_sp_ageing_set(struct mlxsw_sp *mlxsw_sp, u32 ageing_time)
@@ -631,6 +672,27 @@ static int __mlxsw_sp_port_vlans_set(struct mlxsw_sp_port *mlxsw_sp_port,
 	return 0;
 }
 
+static int mlxsw_sp_port_vid_learning_set(struct mlxsw_sp_port *mlxsw_sp_port,
+					  u16 vid_begin, u16 vid_end,
+					  bool learn_enable)
+{
+	u16 vid, vid_e;
+	int err;
+
+	for (vid = vid_begin; vid <= vid_end;
+	     vid += MLXSW_REG_SPVMLR_REC_MAX_COUNT) {
+		vid_e = min((u16) (vid + MLXSW_REG_SPVMLR_REC_MAX_COUNT - 1),
+			    vid_end);
+
+		err = __mlxsw_sp_port_vid_learning_set(mlxsw_sp_port, vid,
+						       vid_e, learn_enable);
+		if (err)
+			return err;
+	}
+
+	return 0;
+}
+
 static int __mlxsw_sp_port_vlans_add(struct mlxsw_sp_port *mlxsw_sp_port,
 				     u16 vid_begin, u16 vid_end,
 				     bool flag_untagged, bool flag_pvid)
@@ -671,6 +733,14 @@ static int __mlxsw_sp_port_vlans_add(struct mlxsw_sp_port *mlxsw_sp_port,
 		}
 	}
 
+	err = mlxsw_sp_port_vid_learning_set(mlxsw_sp_port, vid_begin, vid_end,
+					     mlxsw_sp_port->learning);
+	if (err) {
+		netdev_err(dev, "Failed to set learning for VIDs %d-%d\n",
+			   vid_begin, vid_end);
+		goto err_port_vid_learning_set;
+	}
+
 	/* Changing activity bits only if HW operation succeded */
 	for (vid = vid_begin; vid <= vid_end; vid++) {
 		set_bit(vid, mlxsw_sp_port->active_vlans);
@@ -693,6 +763,9 @@ static int __mlxsw_sp_port_vlans_add(struct mlxsw_sp_port *mlxsw_sp_port,
 err_port_stp_state_set:
 	for (vid = vid_begin; vid <= vid_end; vid++)
 		clear_bit(vid, mlxsw_sp_port->active_vlans);
+	mlxsw_sp_port_vid_learning_set(mlxsw_sp_port, vid_begin, vid_end,
+				       false);
+err_port_vid_learning_set:
 	if (old_pvid != mlxsw_sp_port->pvid)
 		mlxsw_sp_port_pvid_set(mlxsw_sp_port, old_pvid);
 err_port_pvid_set:
@@ -971,11 +1044,6 @@ static int mlxsw_sp_port_obj_add(struct net_device *dev,
 					      SWITCHDEV_OBJ_PORT_VLAN(obj),
 					      trans);
 		break;
-	case SWITCHDEV_OBJ_ID_IPV4_FIB:
-		err = mlxsw_sp_router_fib4_add(mlxsw_sp_port,
-					       SWITCHDEV_OBJ_IPV4_FIB(obj),
-					       trans);
-		break;
 	case SWITCHDEV_OBJ_ID_PORT_FDB:
 		err = mlxsw_sp_port_fdb_static_add(mlxsw_sp_port,
 						   SWITCHDEV_OBJ_PORT_FDB(obj),
@@ -997,29 +1065,20 @@ static int mlxsw_sp_port_obj_add(struct net_device *dev,
 static int __mlxsw_sp_port_vlans_del(struct mlxsw_sp_port *mlxsw_sp_port,
 				     u16 vid_begin, u16 vid_end)
 {
-	struct net_device *dev = mlxsw_sp_port->dev;
 	u16 vid, pvid;
-	int err;
 
 	if (!mlxsw_sp_port->bridged)
 		return -EINVAL;
 
-	err = __mlxsw_sp_port_vlans_set(mlxsw_sp_port, vid_begin, vid_end,
-					false, false);
-	if (err) {
-		netdev_err(dev, "Unable to del VIDs %d-%d\n", vid_begin,
-			   vid_end);
-		return err;
-	}
+	mlxsw_sp_port_vid_learning_set(mlxsw_sp_port, vid_begin, vid_end,
+				       false);
 
 	pvid = mlxsw_sp_port->pvid;
-	if (pvid >= vid_begin && pvid <= vid_end) {
-		err = mlxsw_sp_port_pvid_set(mlxsw_sp_port, 0);
-		if (err) {
-			netdev_err(dev, "Unable to del PVID %d\n", pvid);
-			return err;
-		}
-	}
+	if (pvid >= vid_begin && pvid <= vid_end)
+		mlxsw_sp_port_pvid_set(mlxsw_sp_port, 0);
+
+	__mlxsw_sp_port_vlans_set(mlxsw_sp_port, vid_begin, vid_end, false,
+				  false);
 
 	mlxsw_sp_port_fid_leave(mlxsw_sp_port, vid_begin, vid_end);
 
@@ -1117,10 +1176,6 @@ static int mlxsw_sp_port_obj_del(struct net_device *dev,
 		err = mlxsw_sp_port_vlans_del(mlxsw_sp_port,
 					      SWITCHDEV_OBJ_PORT_VLAN(obj));
 		break;
-	case SWITCHDEV_OBJ_ID_IPV4_FIB:
-		err = mlxsw_sp_router_fib4_del(mlxsw_sp_port,
-					       SWITCHDEV_OBJ_IPV4_FIB(obj));
-		break;
 	case SWITCHDEV_OBJ_ID_PORT_FDB:
 		err = mlxsw_sp_port_fdb_static_del(mlxsw_sp_port,
 						   SWITCHDEV_OBJ_PORT_FDB(obj));
@@ -1141,9 +1196,11 @@ static struct mlxsw_sp_port *mlxsw_sp_lag_rep_port(struct mlxsw_sp *mlxsw_sp,
 						   u16 lag_id)
 {
 	struct mlxsw_sp_port *mlxsw_sp_port;
+	struct mlxsw_resources *resources;
 	int i;
 
-	for (i = 0; i < MLXSW_SP_PORT_PER_LAG_MAX; i++) {
+	resources = mlxsw_core_resources_get(mlxsw_sp->core);
+	for (i = 0; i < resources->max_ports_in_lag; i++) {
 		mlxsw_sp_port = mlxsw_sp_port_lagged_get(mlxsw_sp, lag_id, i);
 		if (mlxsw_sp_port)
 			return mlxsw_sp_port;
@@ -1362,8 +1419,6 @@ static void mlxsw_sp_fdb_notify_mac_process(struct mlxsw_sp *mlxsw_sp,
 		vid = fid;
 	}
 
-	adding = adding && mlxsw_sp_port->learning;
-
 do_fdb_op:
 	err = mlxsw_sp_port_fdb_uc_op(mlxsw_sp, local_port, mac, fid,
 				      adding, true);
@@ -1425,8 +1480,6 @@ static void mlxsw_sp_fdb_notify_mac_lag_process(struct mlxsw_sp *mlxsw_sp,
 		vid = fid;
 	}
 
-	adding = adding && mlxsw_sp_port->learning;
-
 do_fdb_op:
 	err = mlxsw_sp_port_fdb_uc_lag_op(mlxsw_sp, lag_id, mac, fid, lag_vid,
 					  adding, true);
@@ -1492,20 +1545,18 @@ static void mlxsw_sp_fdb_notify_work(struct work_struct *work)
 	mlxsw_sp = container_of(work, struct mlxsw_sp, fdb_notify.dw.work);
 
 	rtnl_lock();
-	do {
-		mlxsw_reg_sfn_pack(sfn_pl);
-		err = mlxsw_reg_query(mlxsw_sp->core, MLXSW_REG(sfn), sfn_pl);
-		if (err) {
-			dev_err_ratelimited(mlxsw_sp->bus_info->dev, "Failed to get FDB notifications\n");
-			break;
-		}
-		num_rec = mlxsw_reg_sfn_num_rec_get(sfn_pl);
-		for (i = 0; i < num_rec; i++)
-			mlxsw_sp_fdb_notify_rec_process(mlxsw_sp, sfn_pl, i);
+	mlxsw_reg_sfn_pack(sfn_pl);
+	err = mlxsw_reg_query(mlxsw_sp->core, MLXSW_REG(sfn), sfn_pl);
+	if (err) {
+		dev_err_ratelimited(mlxsw_sp->bus_info->dev, "Failed to get FDB notifications\n");
+		goto out;
+	}
+	num_rec = mlxsw_reg_sfn_num_rec_get(sfn_pl);
+	for (i = 0; i < num_rec; i++)
+		mlxsw_sp_fdb_notify_rec_process(mlxsw_sp, sfn_pl, i);
 
-	} while (num_rec);
+out:
 	rtnl_unlock();
-
 	kfree(sfn_pl);
 	mlxsw_sp_fdb_notify_work_schedule(mlxsw_sp);
 }
diff --git a/drivers/net/ethernet/mellanox/mlxsw/switchx2.c b/drivers/net/ethernet/mellanox/mlxsw/switchx2.c
index 377daa4d509c..c0c23e2f3275 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/switchx2.c
+++ b/drivers/net/ethernet/mellanox/mlxsw/switchx2.c
@@ -997,7 +997,7 @@ static int mlxsw_sx_port_create(struct mlxsw_sx *mlxsw_sx, u8 local_port)
 	/* Each packet needs to have a Tx header (metadata) on top all other
 	 * headers.
 	 */
-	dev->hard_header_len += MLXSW_TXHDR_LEN;
+	dev->needed_headroom = MLXSW_TXHDR_LEN;
 
 	err = mlxsw_sx_port_module_check(mlxsw_sx_port, &usable);
 	if (err) {
@@ -1512,10 +1512,6 @@ static void mlxsw_sx_fini(struct mlxsw_core *mlxsw_core)
 static struct mlxsw_config_profile mlxsw_sx_config_profile = {
 	.used_max_vepa_channels		= 1,
 	.max_vepa_channels		= 0,
-	.used_max_lag			= 1,
-	.max_lag			= 64,
-	.used_max_port_per_lag		= 1,
-	.max_port_per_lag		= 16,
 	.used_max_mid			= 1,
 	.max_mid			= 7000,
 	.used_max_pgt			= 1,
diff --git a/drivers/net/ethernet/microchip/encx24j600.c b/drivers/net/ethernet/microchip/encx24j600.c
index 42e34076d2de..b14f0305aa31 100644
--- a/drivers/net/ethernet/microchip/encx24j600.c
+++ b/drivers/net/ethernet/microchip/encx24j600.c
@@ -821,7 +821,7 @@ static void encx24j600_set_multicast_list(struct net_device *dev)
 	}
 
 	if (oldfilter != priv->rxfilter)
-		queue_kthread_work(&priv->kworker, &priv->setrx_work);
+		kthread_queue_work(&priv->kworker, &priv->setrx_work);
 }
 
 static void encx24j600_hw_tx(struct encx24j600_priv *priv)
@@ -879,7 +879,7 @@ static netdev_tx_t encx24j600_tx(struct sk_buff *skb, struct net_device *dev)
 	/* Remember the skb for deferred processing */
 	priv->tx_skb = skb;
 
-	queue_kthread_work(&priv->kworker, &priv->tx_work);
+	kthread_queue_work(&priv->kworker, &priv->tx_work);
 
 	return NETDEV_TX_OK;
 }
@@ -1037,9 +1037,9 @@ static int encx24j600_spi_probe(struct spi_device *spi)
 		goto out_free;
 	}
 
-	init_kthread_worker(&priv->kworker);
-	init_kthread_work(&priv->tx_work, encx24j600_tx_proc);
-	init_kthread_work(&priv->setrx_work, encx24j600_setrx_proc);
+	kthread_init_worker(&priv->kworker);
+	kthread_init_work(&priv->tx_work, encx24j600_tx_proc);
+	kthread_init_work(&priv->setrx_work, encx24j600_setrx_proc);
 
 	priv->kworker_task = kthread_run(kthread_worker_fn, &priv->kworker,
 					 "encx24j600");
diff --git a/drivers/net/ethernet/natsemi/ns83820.c b/drivers/net/ethernet/natsemi/ns83820.c
index eb807b0dc72a..569ade6cf85c 100644
--- a/drivers/net/ethernet/natsemi/ns83820.c
+++ b/drivers/net/ethernet/natsemi/ns83820.c
@@ -134,7 +134,7 @@ static int lnksts = 0;		/* CFG_LNKSTS bit polarity */
 
 /* tunables */
 #define RX_BUF_SIZE	1500	/* 8192 */
-#if defined(CONFIG_VLAN_8021Q) || defined(CONFIG_VLAN_8021Q_MODULE)
+#if IS_ENABLED(CONFIG_VLAN_8021Q)
 #define NS83820_VLAN_ACCEL_SUPPORT
 #endif
 
diff --git a/drivers/net/ethernet/netronome/nfp/Makefile b/drivers/net/ethernet/netronome/nfp/Makefile
index 68178819ff12..0efb2ba9a558 100644
--- a/drivers/net/ethernet/netronome/nfp/Makefile
+++ b/drivers/net/ethernet/netronome/nfp/Makefile
@@ -3,6 +3,13 @@ obj-$(CONFIG_NFP_NETVF)	+= nfp_netvf.o
 nfp_netvf-objs := \
 	    nfp_net_common.o \
 	    nfp_net_ethtool.o \
+	    nfp_net_offload.o \
 	    nfp_netvf_main.o
 
+ifeq ($(CONFIG_BPF_SYSCALL),y)
+nfp_netvf-objs += \
+	    nfp_bpf_verifier.o \
+	    nfp_bpf_jit.o
+endif
+
 nfp_netvf-$(CONFIG_NFP_NET_DEBUG) += nfp_net_debugfs.o
diff --git a/drivers/net/ethernet/netronome/nfp/nfp_asm.h b/drivers/net/ethernet/netronome/nfp/nfp_asm.h
new file mode 100644
index 000000000000..22484b6fd3e8
--- /dev/null
+++ b/drivers/net/ethernet/netronome/nfp/nfp_asm.h
@@ -0,0 +1,233 @@
+/*
+ * Copyright (C) 2016 Netronome Systems, Inc.
+ *
+ * This software is dual licensed under the GNU General License Version 2,
+ * June 1991 as shown in the file COPYING in the top-level directory of this
+ * source tree or the BSD 2-Clause License provided below.  You have the
+ * option to license this software under the complete terms of either license.
+ *
+ * The BSD 2-Clause License:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      1. Redistributions of source code must retain the above
+ *         copyright notice, this list of conditions and the following
+ *         disclaimer.
+ *
+ *      2. 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 __NFP_ASM_H__
+#define __NFP_ASM_H__ 1
+
+#include "nfp_bpf.h"
+
+#define REG_NONE	0
+
+#define RE_REG_NO_DST	0x020
+#define RE_REG_IMM	0x020
+#define RE_REG_IMM_encode(x)					\
+	(RE_REG_IMM | ((x) & 0x1f) | (((x) & 0x60) << 1))
+#define RE_REG_IMM_MAX	 0x07fULL
+#define RE_REG_XFR	0x080
+
+#define UR_REG_XFR	0x180
+#define UR_REG_NN	0x280
+#define UR_REG_NO_DST	0x300
+#define UR_REG_IMM	UR_REG_NO_DST
+#define UR_REG_IMM_encode(x) (UR_REG_IMM | (x))
+#define UR_REG_IMM_MAX	 0x0ffULL
+
+#define OP_BR_BASE	0x0d800000020ULL
+#define OP_BR_BASE_MASK	0x0f8000c3ce0ULL
+#define OP_BR_MASK	0x0000000001fULL
+#define OP_BR_EV_PIP	0x00000000300ULL
+#define OP_BR_CSS	0x0000003c000ULL
+#define OP_BR_DEFBR	0x00000300000ULL
+#define OP_BR_ADDR_LO	0x007ffc00000ULL
+#define OP_BR_ADDR_HI	0x10000000000ULL
+
+#define nfp_is_br(_insn)				\
+	(((_insn) & OP_BR_BASE_MASK) == OP_BR_BASE)
+
+enum br_mask {
+	BR_BEQ = 0x00,
+	BR_BNE = 0x01,
+	BR_BHS = 0x04,
+	BR_BLO = 0x05,
+	BR_BGE = 0x08,
+	BR_UNC = 0x18,
+};
+
+enum br_ev_pip {
+	BR_EV_PIP_UNCOND = 0,
+	BR_EV_PIP_COND = 1,
+};
+
+enum br_ctx_signal_state {
+	BR_CSS_NONE = 2,
+};
+
+#define OP_BBYTE_BASE	0x0c800000000ULL
+#define OP_BB_A_SRC	0x000000000ffULL
+#define OP_BB_BYTE	0x00000000300ULL
+#define OP_BB_B_SRC	0x0000003fc00ULL
+#define OP_BB_I8	0x00000040000ULL
+#define OP_BB_EQ	0x00000080000ULL
+#define OP_BB_DEFBR	0x00000300000ULL
+#define OP_BB_ADDR_LO	0x007ffc00000ULL
+#define OP_BB_ADDR_HI	0x10000000000ULL
+
+#define OP_BALU_BASE	0x0e800000000ULL
+#define OP_BA_A_SRC	0x000000003ffULL
+#define OP_BA_B_SRC	0x000000ffc00ULL
+#define OP_BA_DEFBR	0x00000300000ULL
+#define OP_BA_ADDR_HI	0x0007fc00000ULL
+
+#define OP_IMMED_A_SRC	0x000000003ffULL
+#define OP_IMMED_B_SRC	0x000000ffc00ULL
+#define OP_IMMED_IMM	0x0000ff00000ULL
+#define OP_IMMED_WIDTH	0x00060000000ULL
+#define OP_IMMED_INV	0x00080000000ULL
+#define OP_IMMED_SHIFT	0x00600000000ULL
+#define OP_IMMED_BASE	0x0f000000000ULL
+#define OP_IMMED_WR_AB	0x20000000000ULL
+
+enum immed_width {
+	IMMED_WIDTH_ALL = 0,
+	IMMED_WIDTH_BYTE = 1,
+	IMMED_WIDTH_WORD = 2,
+};
+
+enum immed_shift {
+	IMMED_SHIFT_0B = 0,
+	IMMED_SHIFT_1B = 1,
+	IMMED_SHIFT_2B = 2,
+};
+
+#define OP_SHF_BASE	0x08000000000ULL
+#define OP_SHF_A_SRC	0x000000000ffULL
+#define OP_SHF_SC	0x00000000300ULL
+#define OP_SHF_B_SRC	0x0000003fc00ULL
+#define OP_SHF_I8	0x00000040000ULL
+#define OP_SHF_SW	0x00000080000ULL
+#define OP_SHF_DST	0x0000ff00000ULL
+#define OP_SHF_SHIFT	0x001f0000000ULL
+#define OP_SHF_OP	0x00e00000000ULL
+#define OP_SHF_DST_AB	0x01000000000ULL
+#define OP_SHF_WR_AB	0x20000000000ULL
+
+enum shf_op {
+	SHF_OP_NONE = 0,
+	SHF_OP_AND = 2,
+	SHF_OP_OR = 5,
+};
+
+enum shf_sc {
+	SHF_SC_R_ROT = 0,
+	SHF_SC_R_SHF = 1,
+	SHF_SC_L_SHF = 2,
+	SHF_SC_R_DSHF = 3,
+};
+
+#define OP_ALU_A_SRC	0x000000003ffULL
+#define OP_ALU_B_SRC	0x000000ffc00ULL
+#define OP_ALU_DST	0x0003ff00000ULL
+#define OP_ALU_SW	0x00040000000ULL
+#define OP_ALU_OP	0x00f80000000ULL
+#define OP_ALU_DST_AB	0x01000000000ULL
+#define OP_ALU_BASE	0x0a000000000ULL
+#define OP_ALU_WR_AB	0x20000000000ULL
+
+enum alu_op {
+	ALU_OP_NONE	= 0x00,
+	ALU_OP_ADD	= 0x01,
+	ALU_OP_NEG	= 0x04,
+	ALU_OP_AND	= 0x08,
+	ALU_OP_SUB_C	= 0x0d,
+	ALU_OP_ADD_C	= 0x11,
+	ALU_OP_OR	= 0x14,
+	ALU_OP_SUB	= 0x15,
+	ALU_OP_XOR	= 0x18,
+};
+
+enum alu_dst_ab {
+	ALU_DST_A = 0,
+	ALU_DST_B = 1,
+};
+
+#define OP_LDF_BASE	0x0c000000000ULL
+#define OP_LDF_A_SRC	0x000000000ffULL
+#define OP_LDF_SC	0x00000000300ULL
+#define OP_LDF_B_SRC	0x0000003fc00ULL
+#define OP_LDF_I8	0x00000040000ULL
+#define OP_LDF_SW	0x00000080000ULL
+#define OP_LDF_ZF	0x00000100000ULL
+#define OP_LDF_BMASK	0x0000f000000ULL
+#define OP_LDF_SHF	0x001f0000000ULL
+#define OP_LDF_WR_AB	0x20000000000ULL
+
+#define OP_CMD_A_SRC	 0x000000000ffULL
+#define OP_CMD_CTX	 0x00000000300ULL
+#define OP_CMD_B_SRC	 0x0000003fc00ULL
+#define OP_CMD_TOKEN	 0x000000c0000ULL
+#define OP_CMD_XFER	 0x00001f00000ULL
+#define OP_CMD_CNT	 0x0000e000000ULL
+#define OP_CMD_SIG	 0x000f0000000ULL
+#define OP_CMD_TGT_CMD	 0x07f00000000ULL
+#define OP_CMD_MODE	0x1c0000000000ULL
+
+struct cmd_tgt_act {
+	u8 token;
+	u8 tgt_cmd;
+};
+
+enum cmd_tgt_map {
+	CMD_TGT_READ8,
+	CMD_TGT_WRITE8,
+	CMD_TGT_READ_LE,
+	CMD_TGT_READ_SWAP_LE,
+	__CMD_TGT_MAP_SIZE,
+};
+
+enum cmd_mode {
+	CMD_MODE_40b_AB	= 0,
+	CMD_MODE_40b_BA	= 1,
+	CMD_MODE_32b	= 4,
+};
+
+enum cmd_ctx_swap {
+	CMD_CTX_SWAP = 0,
+	CMD_CTX_NO_SWAP = 3,
+};
+
+#define OP_LCSR_BASE	0x0fc00000000ULL
+#define OP_LCSR_A_SRC	0x000000003ffULL
+#define OP_LCSR_B_SRC	0x000000ffc00ULL
+#define OP_LCSR_WRITE	0x00000200000ULL
+#define OP_LCSR_ADDR	0x001ffc00000ULL
+
+enum lcsr_wr_src {
+	LCSR_WR_AREG,
+	LCSR_WR_BREG,
+	LCSR_WR_IMM,
+};
+
+#define OP_CARB_BASE	0x0e000000000ULL
+#define OP_CARB_OR	0x00000010000ULL
+
+#endif
diff --git a/drivers/net/ethernet/netronome/nfp/nfp_bpf.h b/drivers/net/ethernet/netronome/nfp/nfp_bpf.h
new file mode 100644
index 000000000000..87aa8a3e9112
--- /dev/null
+++ b/drivers/net/ethernet/netronome/nfp/nfp_bpf.h
@@ -0,0 +1,202 @@
+/*
+ * Copyright (C) 2016 Netronome Systems, Inc.
+ *
+ * This software is dual licensed under the GNU General License Version 2,
+ * June 1991 as shown in the file COPYING in the top-level directory of this
+ * source tree or the BSD 2-Clause License provided below.  You have the
+ * option to license this software under the complete terms of either license.
+ *
+ * The BSD 2-Clause License:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      1. Redistributions of source code must retain the above
+ *         copyright notice, this list of conditions and the following
+ *         disclaimer.
+ *
+ *      2. 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 __NFP_BPF_H__
+#define __NFP_BPF_H__ 1
+
+#include <linux/bitfield.h>
+#include <linux/bpf.h>
+#include <linux/list.h>
+#include <linux/types.h>
+
+#define FIELD_FIT(mask, val)  (!((((u64)val) << __bf_shf(mask)) & ~(mask)))
+
+/* For branch fixup logic use up-most byte of branch instruction as scratch
+ * area.  Remember to clear this before sending instructions to HW!
+ */
+#define OP_BR_SPECIAL	0xff00000000000000ULL
+
+enum br_special {
+	OP_BR_NORMAL = 0,
+	OP_BR_GO_OUT,
+	OP_BR_GO_ABORT,
+};
+
+enum static_regs {
+	STATIC_REG_PKT		= 1,
+#define REG_PKT_BANK	ALU_DST_A
+	STATIC_REG_IMM		= 2, /* Bank AB */
+};
+
+enum nfp_bpf_action_type {
+	NN_ACT_TC_DROP,
+	NN_ACT_TC_REDIR,
+	NN_ACT_DIRECT,
+};
+
+/* Software register representation, hardware encoding in asm.h */
+#define NN_REG_TYPE	GENMASK(31, 24)
+#define NN_REG_VAL	GENMASK(7, 0)
+
+enum nfp_bpf_reg_type {
+	NN_REG_GPR_A =	BIT(0),
+	NN_REG_GPR_B =	BIT(1),
+	NN_REG_NNR =	BIT(2),
+	NN_REG_XFER =	BIT(3),
+	NN_REG_IMM =	BIT(4),
+	NN_REG_NONE =	BIT(5),
+};
+
+#define NN_REG_GPR_BOTH	(NN_REG_GPR_A | NN_REG_GPR_B)
+
+#define reg_both(x)	((x) | FIELD_PREP(NN_REG_TYPE, NN_REG_GPR_BOTH))
+#define reg_a(x)	((x) | FIELD_PREP(NN_REG_TYPE, NN_REG_GPR_A))
+#define reg_b(x)	((x) | FIELD_PREP(NN_REG_TYPE, NN_REG_GPR_B))
+#define reg_nnr(x)	((x) | FIELD_PREP(NN_REG_TYPE, NN_REG_NNR))
+#define reg_xfer(x)	((x) | FIELD_PREP(NN_REG_TYPE, NN_REG_XFER))
+#define reg_imm(x)	((x) | FIELD_PREP(NN_REG_TYPE, NN_REG_IMM))
+#define reg_none()	(FIELD_PREP(NN_REG_TYPE, NN_REG_NONE))
+
+#define pkt_reg(np)	reg_a((np)->regs_per_thread - STATIC_REG_PKT)
+#define imm_a(np)	reg_a((np)->regs_per_thread - STATIC_REG_IMM)
+#define imm_b(np)	reg_b((np)->regs_per_thread - STATIC_REG_IMM)
+#define imm_both(np)	reg_both((np)->regs_per_thread - STATIC_REG_IMM)
+
+#define NFP_BPF_ABI_FLAGS	reg_nnr(0)
+#define   NFP_BPF_ABI_FLAG_MARK	1
+#define NFP_BPF_ABI_MARK	reg_nnr(1)
+#define NFP_BPF_ABI_PKT		reg_nnr(2)
+#define NFP_BPF_ABI_LEN		reg_nnr(3)
+
+struct nfp_prog;
+struct nfp_insn_meta;
+typedef int (*instr_cb_t)(struct nfp_prog *, struct nfp_insn_meta *);
+
+#define nfp_prog_first_meta(nfp_prog)					\
+	list_first_entry(&(nfp_prog)->insns, struct nfp_insn_meta, l)
+#define nfp_prog_last_meta(nfp_prog)					\
+	list_last_entry(&(nfp_prog)->insns, struct nfp_insn_meta, l)
+#define nfp_meta_next(meta)	list_next_entry(meta, l)
+#define nfp_meta_prev(meta)	list_prev_entry(meta, l)
+
+/**
+ * struct nfp_insn_meta - BPF instruction wrapper
+ * @insn: BPF instruction
+ * @off: index of first generated machine instruction (in nfp_prog.prog)
+ * @n: eBPF instruction number
+ * @skip: skip this instruction (optimized out)
+ * @double_cb: callback for second part of the instruction
+ * @l: link on nfp_prog->insns list
+ */
+struct nfp_insn_meta {
+	struct bpf_insn insn;
+	unsigned int off;
+	unsigned short n;
+	bool skip;
+	instr_cb_t double_cb;
+
+	struct list_head l;
+};
+
+#define BPF_SIZE_MASK	0x18
+
+static inline u8 mbpf_class(const struct nfp_insn_meta *meta)
+{
+	return BPF_CLASS(meta->insn.code);
+}
+
+static inline u8 mbpf_src(const struct nfp_insn_meta *meta)
+{
+	return BPF_SRC(meta->insn.code);
+}
+
+static inline u8 mbpf_op(const struct nfp_insn_meta *meta)
+{
+	return BPF_OP(meta->insn.code);
+}
+
+static inline u8 mbpf_mode(const struct nfp_insn_meta *meta)
+{
+	return BPF_MODE(meta->insn.code);
+}
+
+/**
+ * struct nfp_prog - nfp BPF program
+ * @prog: machine code
+ * @prog_len: number of valid instructions in @prog array
+ * @__prog_alloc_len: alloc size of @prog array
+ * @act: BPF program/action type (TC DA, TC with action, XDP etc.)
+ * @num_regs: number of registers used by this program
+ * @regs_per_thread: number of basic registers allocated per thread
+ * @start_off: address of the first instruction in the memory
+ * @tgt_out: jump target for normal exit
+ * @tgt_abort: jump target for abort (e.g. access outside of packet buffer)
+ * @tgt_done: jump target to get the next packet
+ * @n_translated: number of successfully translated instructions (for errors)
+ * @error: error code if something went wrong
+ * @insns: list of BPF instruction wrappers (struct nfp_insn_meta)
+ */
+struct nfp_prog {
+	u64 *prog;
+	unsigned int prog_len;
+	unsigned int __prog_alloc_len;
+
+	enum nfp_bpf_action_type act;
+
+	unsigned int num_regs;
+	unsigned int regs_per_thread;
+
+	unsigned int start_off;
+	unsigned int tgt_out;
+	unsigned int tgt_abort;
+	unsigned int tgt_done;
+
+	unsigned int n_translated;
+	int error;
+
+	struct list_head insns;
+};
+
+struct nfp_bpf_result {
+	unsigned int n_instr;
+	bool dense_mode;
+};
+
+int
+nfp_bpf_jit(struct bpf_prog *filter, void *prog, enum nfp_bpf_action_type act,
+	    unsigned int prog_start, unsigned int prog_done,
+	    unsigned int prog_sz, struct nfp_bpf_result *res);
+
+int nfp_prog_verify(struct nfp_prog *nfp_prog, struct bpf_prog *prog);
+
+#endif
diff --git a/drivers/net/ethernet/netronome/nfp/nfp_bpf_jit.c b/drivers/net/ethernet/netronome/nfp/nfp_bpf_jit.c
new file mode 100644
index 000000000000..f8df5300f49c
--- /dev/null
+++ b/drivers/net/ethernet/netronome/nfp/nfp_bpf_jit.c
@@ -0,0 +1,1813 @@
+/*
+ * Copyright (C) 2016 Netronome Systems, Inc.
+ *
+ * This software is dual licensed under the GNU General License Version 2,
+ * June 1991 as shown in the file COPYING in the top-level directory of this
+ * source tree or the BSD 2-Clause License provided below.  You have the
+ * option to license this software under the complete terms of either license.
+ *
+ * The BSD 2-Clause License:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      1. Redistributions of source code must retain the above
+ *         copyright notice, this list of conditions and the following
+ *         disclaimer.
+ *
+ *      2. 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.
+ */
+
+#define pr_fmt(fmt)	"NFP net bpf: " fmt
+
+#include <linux/kernel.h>
+#include <linux/bpf.h>
+#include <linux/filter.h>
+#include <linux/pkt_cls.h>
+#include <linux/unistd.h>
+
+#include "nfp_asm.h"
+#include "nfp_bpf.h"
+
+/* --- NFP prog --- */
+/* Foreach "multiple" entries macros provide pos and next<n> pointers.
+ * It's safe to modify the next pointers (but not pos).
+ */
+#define nfp_for_each_insn_walk2(nfp_prog, pos, next)			\
+	for (pos = list_first_entry(&(nfp_prog)->insns, typeof(*pos), l), \
+	     next = list_next_entry(pos, l);			\
+	     &(nfp_prog)->insns != &pos->l &&			\
+	     &(nfp_prog)->insns != &next->l;			\
+	     pos = nfp_meta_next(pos),				\
+	     next = nfp_meta_next(pos))
+
+#define nfp_for_each_insn_walk3(nfp_prog, pos, next, next2)		\
+	for (pos = list_first_entry(&(nfp_prog)->insns, typeof(*pos), l), \
+	     next = list_next_entry(pos, l),			\
+	     next2 = list_next_entry(next, l);			\
+	     &(nfp_prog)->insns != &pos->l &&			\
+	     &(nfp_prog)->insns != &next->l &&			\
+	     &(nfp_prog)->insns != &next2->l;			\
+	     pos = nfp_meta_next(pos),				\
+	     next = nfp_meta_next(pos),				\
+	     next2 = nfp_meta_next(next))
+
+static bool
+nfp_meta_has_next(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta)
+{
+	return meta->l.next != &nfp_prog->insns;
+}
+
+static bool
+nfp_meta_has_prev(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta)
+{
+	return meta->l.prev != &nfp_prog->insns;
+}
+
+static void nfp_prog_free(struct nfp_prog *nfp_prog)
+{
+	struct nfp_insn_meta *meta, *tmp;
+
+	list_for_each_entry_safe(meta, tmp, &nfp_prog->insns, l) {
+		list_del(&meta->l);
+		kfree(meta);
+	}
+	kfree(nfp_prog);
+}
+
+static void nfp_prog_push(struct nfp_prog *nfp_prog, u64 insn)
+{
+	if (nfp_prog->__prog_alloc_len == nfp_prog->prog_len) {
+		nfp_prog->error = -ENOSPC;
+		return;
+	}
+
+	nfp_prog->prog[nfp_prog->prog_len] = insn;
+	nfp_prog->prog_len++;
+}
+
+static unsigned int nfp_prog_current_offset(struct nfp_prog *nfp_prog)
+{
+	return nfp_prog->start_off + nfp_prog->prog_len;
+}
+
+static unsigned int
+nfp_prog_offset_to_index(struct nfp_prog *nfp_prog, unsigned int offset)
+{
+	return offset - nfp_prog->start_off;
+}
+
+/* --- SW reg --- */
+struct nfp_insn_ur_regs {
+	enum alu_dst_ab dst_ab;
+	u16 dst;
+	u16 areg, breg;
+	bool swap;
+	bool wr_both;
+};
+
+struct nfp_insn_re_regs {
+	enum alu_dst_ab dst_ab;
+	u8 dst;
+	u8 areg, breg;
+	bool swap;
+	bool wr_both;
+	bool i8;
+};
+
+static u16 nfp_swreg_to_unreg(u32 swreg, bool is_dst)
+{
+	u16 val = FIELD_GET(NN_REG_VAL, swreg);
+
+	switch (FIELD_GET(NN_REG_TYPE, swreg)) {
+	case NN_REG_GPR_A:
+	case NN_REG_GPR_B:
+	case NN_REG_GPR_BOTH:
+		return val;
+	case NN_REG_NNR:
+		return UR_REG_NN | val;
+	case NN_REG_XFER:
+		return UR_REG_XFR | val;
+	case NN_REG_IMM:
+		if (val & ~0xff) {
+			pr_err("immediate too large\n");
+			return 0;
+		}
+		return UR_REG_IMM_encode(val);
+	case NN_REG_NONE:
+		return is_dst ? UR_REG_NO_DST : REG_NONE;
+	default:
+		pr_err("unrecognized reg encoding %08x\n", swreg);
+		return 0;
+	}
+}
+
+static int
+swreg_to_unrestricted(u32 dst, u32 lreg, u32 rreg, struct nfp_insn_ur_regs *reg)
+{
+	memset(reg, 0, sizeof(*reg));
+
+	/* Decode destination */
+	if (FIELD_GET(NN_REG_TYPE, dst) == NN_REG_IMM)
+		return -EFAULT;
+
+	if (FIELD_GET(NN_REG_TYPE, dst) == NN_REG_GPR_B)
+		reg->dst_ab = ALU_DST_B;
+	if (FIELD_GET(NN_REG_TYPE, dst) == NN_REG_GPR_BOTH)
+		reg->wr_both = true;
+	reg->dst = nfp_swreg_to_unreg(dst, true);
+
+	/* Decode source operands */
+	if (FIELD_GET(NN_REG_TYPE, lreg) == FIELD_GET(NN_REG_TYPE, rreg))
+		return -EFAULT;
+
+	if (FIELD_GET(NN_REG_TYPE, lreg) == NN_REG_GPR_B ||
+	    FIELD_GET(NN_REG_TYPE, rreg) == NN_REG_GPR_A) {
+		reg->areg = nfp_swreg_to_unreg(rreg, false);
+		reg->breg = nfp_swreg_to_unreg(lreg, false);
+		reg->swap = true;
+	} else {
+		reg->areg = nfp_swreg_to_unreg(lreg, false);
+		reg->breg = nfp_swreg_to_unreg(rreg, false);
+	}
+
+	return 0;
+}
+
+static u16 nfp_swreg_to_rereg(u32 swreg, bool is_dst, bool has_imm8, bool *i8)
+{
+	u16 val = FIELD_GET(NN_REG_VAL, swreg);
+
+	switch (FIELD_GET(NN_REG_TYPE, swreg)) {
+	case NN_REG_GPR_A:
+	case NN_REG_GPR_B:
+	case NN_REG_GPR_BOTH:
+		return val;
+	case NN_REG_XFER:
+		return RE_REG_XFR | val;
+	case NN_REG_IMM:
+		if (val & ~(0x7f | has_imm8 << 7)) {
+			pr_err("immediate too large\n");
+			return 0;
+		}
+		*i8 = val & 0x80;
+		return RE_REG_IMM_encode(val & 0x7f);
+	case NN_REG_NONE:
+		return is_dst ? RE_REG_NO_DST : REG_NONE;
+	default:
+		pr_err("unrecognized reg encoding\n");
+		return 0;
+	}
+}
+
+static int
+swreg_to_restricted(u32 dst, u32 lreg, u32 rreg, struct nfp_insn_re_regs *reg,
+		    bool has_imm8)
+{
+	memset(reg, 0, sizeof(*reg));
+
+	/* Decode destination */
+	if (FIELD_GET(NN_REG_TYPE, dst) == NN_REG_IMM)
+		return -EFAULT;
+
+	if (FIELD_GET(NN_REG_TYPE, dst) == NN_REG_GPR_B)
+		reg->dst_ab = ALU_DST_B;
+	if (FIELD_GET(NN_REG_TYPE, dst) == NN_REG_GPR_BOTH)
+		reg->wr_both = true;
+	reg->dst = nfp_swreg_to_rereg(dst, true, false, NULL);
+
+	/* Decode source operands */
+	if (FIELD_GET(NN_REG_TYPE, lreg) == FIELD_GET(NN_REG_TYPE, rreg))
+		return -EFAULT;
+
+	if (FIELD_GET(NN_REG_TYPE, lreg) == NN_REG_GPR_B ||
+	    FIELD_GET(NN_REG_TYPE, rreg) == NN_REG_GPR_A) {
+		reg->areg = nfp_swreg_to_rereg(rreg, false, has_imm8, &reg->i8);
+		reg->breg = nfp_swreg_to_rereg(lreg, false, has_imm8, &reg->i8);
+		reg->swap = true;
+	} else {
+		reg->areg = nfp_swreg_to_rereg(lreg, false, has_imm8, &reg->i8);
+		reg->breg = nfp_swreg_to_rereg(rreg, false, has_imm8, &reg->i8);
+	}
+
+	return 0;
+}
+
+/* --- Emitters --- */
+static const struct cmd_tgt_act cmd_tgt_act[__CMD_TGT_MAP_SIZE] = {
+	[CMD_TGT_WRITE8] =		{ 0x00, 0x42 },
+	[CMD_TGT_READ8] =		{ 0x01, 0x43 },
+	[CMD_TGT_READ_LE] =		{ 0x01, 0x40 },
+	[CMD_TGT_READ_SWAP_LE] =	{ 0x03, 0x40 },
+};
+
+static void
+__emit_cmd(struct nfp_prog *nfp_prog, enum cmd_tgt_map op,
+	   u8 mode, u8 xfer, u8 areg, u8 breg, u8 size, bool sync)
+{
+	enum cmd_ctx_swap ctx;
+	u64 insn;
+
+	if (sync)
+		ctx = CMD_CTX_SWAP;
+	else
+		ctx = CMD_CTX_NO_SWAP;
+
+	insn =	FIELD_PREP(OP_CMD_A_SRC, areg) |
+		FIELD_PREP(OP_CMD_CTX, ctx) |
+		FIELD_PREP(OP_CMD_B_SRC, breg) |
+		FIELD_PREP(OP_CMD_TOKEN, cmd_tgt_act[op].token) |
+		FIELD_PREP(OP_CMD_XFER, xfer) |
+		FIELD_PREP(OP_CMD_CNT, size) |
+		FIELD_PREP(OP_CMD_SIG, sync) |
+		FIELD_PREP(OP_CMD_TGT_CMD, cmd_tgt_act[op].tgt_cmd) |
+		FIELD_PREP(OP_CMD_MODE, mode);
+
+	nfp_prog_push(nfp_prog, insn);
+}
+
+static void
+emit_cmd(struct nfp_prog *nfp_prog, enum cmd_tgt_map op,
+	 u8 mode, u8 xfer, u32 lreg, u32 rreg, u8 size, bool sync)
+{
+	struct nfp_insn_re_regs reg;
+	int err;
+
+	err = swreg_to_restricted(reg_none(), lreg, rreg, &reg, false);
+	if (err) {
+		nfp_prog->error = err;
+		return;
+	}
+	if (reg.swap) {
+		pr_err("cmd can't swap arguments\n");
+		nfp_prog->error = -EFAULT;
+		return;
+	}
+
+	__emit_cmd(nfp_prog, op, mode, xfer, reg.areg, reg.breg, size, sync);
+}
+
+static void
+__emit_br(struct nfp_prog *nfp_prog, enum br_mask mask, enum br_ev_pip ev_pip,
+	  enum br_ctx_signal_state css, u16 addr, u8 defer)
+{
+	u16 addr_lo, addr_hi;
+	u64 insn;
+
+	addr_lo = addr & (OP_BR_ADDR_LO >> __bf_shf(OP_BR_ADDR_LO));
+	addr_hi = addr != addr_lo;
+
+	insn = OP_BR_BASE |
+		FIELD_PREP(OP_BR_MASK, mask) |
+		FIELD_PREP(OP_BR_EV_PIP, ev_pip) |
+		FIELD_PREP(OP_BR_CSS, css) |
+		FIELD_PREP(OP_BR_DEFBR, defer) |
+		FIELD_PREP(OP_BR_ADDR_LO, addr_lo) |
+		FIELD_PREP(OP_BR_ADDR_HI, addr_hi);
+
+	nfp_prog_push(nfp_prog, insn);
+}
+
+static void emit_br_def(struct nfp_prog *nfp_prog, u16 addr, u8 defer)
+{
+	if (defer > 2) {
+		pr_err("BUG: branch defer out of bounds %d\n", defer);
+		nfp_prog->error = -EFAULT;
+		return;
+	}
+	__emit_br(nfp_prog, BR_UNC, BR_EV_PIP_UNCOND, BR_CSS_NONE, addr, defer);
+}
+
+static void
+emit_br(struct nfp_prog *nfp_prog, enum br_mask mask, u16 addr, u8 defer)
+{
+	__emit_br(nfp_prog, mask,
+		  mask != BR_UNC ? BR_EV_PIP_COND : BR_EV_PIP_UNCOND,
+		  BR_CSS_NONE, addr, defer);
+}
+
+static void
+__emit_br_byte(struct nfp_prog *nfp_prog, u8 areg, u8 breg, bool imm8,
+	       u8 byte, bool equal, u16 addr, u8 defer)
+{
+	u16 addr_lo, addr_hi;
+	u64 insn;
+
+	addr_lo = addr & (OP_BB_ADDR_LO >> __bf_shf(OP_BB_ADDR_LO));
+	addr_hi = addr != addr_lo;
+
+	insn = OP_BBYTE_BASE |
+		FIELD_PREP(OP_BB_A_SRC, areg) |
+		FIELD_PREP(OP_BB_BYTE, byte) |
+		FIELD_PREP(OP_BB_B_SRC, breg) |
+		FIELD_PREP(OP_BB_I8, imm8) |
+		FIELD_PREP(OP_BB_EQ, equal) |
+		FIELD_PREP(OP_BB_DEFBR, defer) |
+		FIELD_PREP(OP_BB_ADDR_LO, addr_lo) |
+		FIELD_PREP(OP_BB_ADDR_HI, addr_hi);
+
+	nfp_prog_push(nfp_prog, insn);
+}
+
+static void
+emit_br_byte_neq(struct nfp_prog *nfp_prog,
+		 u32 dst, u8 imm, u8 byte, u16 addr, u8 defer)
+{
+	struct nfp_insn_re_regs reg;
+	int err;
+
+	err = swreg_to_restricted(reg_none(), dst, reg_imm(imm), &reg, true);
+	if (err) {
+		nfp_prog->error = err;
+		return;
+	}
+
+	__emit_br_byte(nfp_prog, reg.areg, reg.breg, reg.i8, byte, false, addr,
+		       defer);
+}
+
+static void
+__emit_immed(struct nfp_prog *nfp_prog, u16 areg, u16 breg, u16 imm_hi,
+	     enum immed_width width, bool invert,
+	     enum immed_shift shift, bool wr_both)
+{
+	u64 insn;
+
+	insn = OP_IMMED_BASE |
+		FIELD_PREP(OP_IMMED_A_SRC, areg) |
+		FIELD_PREP(OP_IMMED_B_SRC, breg) |
+		FIELD_PREP(OP_IMMED_IMM, imm_hi) |
+		FIELD_PREP(OP_IMMED_WIDTH, width) |
+		FIELD_PREP(OP_IMMED_INV, invert) |
+		FIELD_PREP(OP_IMMED_SHIFT, shift) |
+		FIELD_PREP(OP_IMMED_WR_AB, wr_both);
+
+	nfp_prog_push(nfp_prog, insn);
+}
+
+static void
+emit_immed(struct nfp_prog *nfp_prog, u32 dst, u16 imm,
+	   enum immed_width width, bool invert, enum immed_shift shift)
+{
+	struct nfp_insn_ur_regs reg;
+	int err;
+
+	if (FIELD_GET(NN_REG_TYPE, dst) == NN_REG_IMM) {
+		nfp_prog->error = -EFAULT;
+		return;
+	}
+
+	err = swreg_to_unrestricted(dst, dst, reg_imm(imm & 0xff), &reg);
+	if (err) {
+		nfp_prog->error = err;
+		return;
+	}
+
+	__emit_immed(nfp_prog, reg.areg, reg.breg, imm >> 8, width,
+		     invert, shift, reg.wr_both);
+}
+
+static void
+__emit_shf(struct nfp_prog *nfp_prog, u16 dst, enum alu_dst_ab dst_ab,
+	   enum shf_sc sc, u8 shift,
+	   u16 areg, enum shf_op op, u16 breg, bool i8, bool sw, bool wr_both)
+{
+	u64 insn;
+
+	if (!FIELD_FIT(OP_SHF_SHIFT, shift)) {
+		nfp_prog->error = -EFAULT;
+		return;
+	}
+
+	if (sc == SHF_SC_L_SHF)
+		shift = 32 - shift;
+
+	insn = OP_SHF_BASE |
+		FIELD_PREP(OP_SHF_A_SRC, areg) |
+		FIELD_PREP(OP_SHF_SC, sc) |
+		FIELD_PREP(OP_SHF_B_SRC, breg) |
+		FIELD_PREP(OP_SHF_I8, i8) |
+		FIELD_PREP(OP_SHF_SW, sw) |
+		FIELD_PREP(OP_SHF_DST, dst) |
+		FIELD_PREP(OP_SHF_SHIFT, shift) |
+		FIELD_PREP(OP_SHF_OP, op) |
+		FIELD_PREP(OP_SHF_DST_AB, dst_ab) |
+		FIELD_PREP(OP_SHF_WR_AB, wr_both);
+
+	nfp_prog_push(nfp_prog, insn);
+}
+
+static void
+emit_shf(struct nfp_prog *nfp_prog, u32 dst, u32 lreg, enum shf_op op, u32 rreg,
+	 enum shf_sc sc, u8 shift)
+{
+	struct nfp_insn_re_regs reg;
+	int err;
+
+	err = swreg_to_restricted(dst, lreg, rreg, &reg, true);
+	if (err) {
+		nfp_prog->error = err;
+		return;
+	}
+
+	__emit_shf(nfp_prog, reg.dst, reg.dst_ab, sc, shift,
+		   reg.areg, op, reg.breg, reg.i8, reg.swap, reg.wr_both);
+}
+
+static void
+__emit_alu(struct nfp_prog *nfp_prog, u16 dst, enum alu_dst_ab dst_ab,
+	   u16 areg, enum alu_op op, u16 breg, bool swap, bool wr_both)
+{
+	u64 insn;
+
+	insn = OP_ALU_BASE |
+		FIELD_PREP(OP_ALU_A_SRC, areg) |
+		FIELD_PREP(OP_ALU_B_SRC, breg) |
+		FIELD_PREP(OP_ALU_DST, dst) |
+		FIELD_PREP(OP_ALU_SW, swap) |
+		FIELD_PREP(OP_ALU_OP, op) |
+		FIELD_PREP(OP_ALU_DST_AB, dst_ab) |
+		FIELD_PREP(OP_ALU_WR_AB, wr_both);
+
+	nfp_prog_push(nfp_prog, insn);
+}
+
+static void
+emit_alu(struct nfp_prog *nfp_prog, u32 dst, u32 lreg, enum alu_op op, u32 rreg)
+{
+	struct nfp_insn_ur_regs reg;
+	int err;
+
+	err = swreg_to_unrestricted(dst, lreg, rreg, &reg);
+	if (err) {
+		nfp_prog->error = err;
+		return;
+	}
+
+	__emit_alu(nfp_prog, reg.dst, reg.dst_ab,
+		   reg.areg, op, reg.breg, reg.swap, reg.wr_both);
+}
+
+static void
+__emit_ld_field(struct nfp_prog *nfp_prog, enum shf_sc sc,
+		u8 areg, u8 bmask, u8 breg, u8 shift, bool imm8,
+		bool zero, bool swap, bool wr_both)
+{
+	u64 insn;
+
+	insn = OP_LDF_BASE |
+		FIELD_PREP(OP_LDF_A_SRC, areg) |
+		FIELD_PREP(OP_LDF_SC, sc) |
+		FIELD_PREP(OP_LDF_B_SRC, breg) |
+		FIELD_PREP(OP_LDF_I8, imm8) |
+		FIELD_PREP(OP_LDF_SW, swap) |
+		FIELD_PREP(OP_LDF_ZF, zero) |
+		FIELD_PREP(OP_LDF_BMASK, bmask) |
+		FIELD_PREP(OP_LDF_SHF, shift) |
+		FIELD_PREP(OP_LDF_WR_AB, wr_both);
+
+	nfp_prog_push(nfp_prog, insn);
+}
+
+static void
+emit_ld_field_any(struct nfp_prog *nfp_prog, enum shf_sc sc, u8 shift,
+		  u32 dst, u8 bmask, u32 src, bool zero)
+{
+	struct nfp_insn_re_regs reg;
+	int err;
+
+	err = swreg_to_restricted(reg_none(), dst, src, &reg, true);
+	if (err) {
+		nfp_prog->error = err;
+		return;
+	}
+
+	__emit_ld_field(nfp_prog, sc, reg.areg, bmask, reg.breg, shift,
+			reg.i8, zero, reg.swap, reg.wr_both);
+}
+
+static void
+emit_ld_field(struct nfp_prog *nfp_prog, u32 dst, u8 bmask, u32 src,
+	      enum shf_sc sc, u8 shift)
+{
+	emit_ld_field_any(nfp_prog, sc, shift, dst, bmask, src, false);
+}
+
+/* --- Wrappers --- */
+static bool pack_immed(u32 imm, u16 *val, enum immed_shift *shift)
+{
+	if (!(imm & 0xffff0000)) {
+		*val = imm;
+		*shift = IMMED_SHIFT_0B;
+	} else if (!(imm & 0xff0000ff)) {
+		*val = imm >> 8;
+		*shift = IMMED_SHIFT_1B;
+	} else if (!(imm & 0x0000ffff)) {
+		*val = imm >> 16;
+		*shift = IMMED_SHIFT_2B;
+	} else {
+		return false;
+	}
+
+	return true;
+}
+
+static void wrp_immed(struct nfp_prog *nfp_prog, u32 dst, u32 imm)
+{
+	enum immed_shift shift;
+	u16 val;
+
+	if (pack_immed(imm, &val, &shift)) {
+		emit_immed(nfp_prog, dst, val, IMMED_WIDTH_ALL, false, shift);
+	} else if (pack_immed(~imm, &val, &shift)) {
+		emit_immed(nfp_prog, dst, val, IMMED_WIDTH_ALL, true, shift);
+	} else {
+		emit_immed(nfp_prog, dst, imm & 0xffff, IMMED_WIDTH_ALL,
+			   false, IMMED_SHIFT_0B);
+		emit_immed(nfp_prog, dst, imm >> 16, IMMED_WIDTH_WORD,
+			   false, IMMED_SHIFT_2B);
+	}
+}
+
+/* ur_load_imm_any() - encode immediate or use tmp register (unrestricted)
+ * If the @imm is small enough encode it directly in operand and return
+ * otherwise load @imm to a spare register and return its encoding.
+ */
+static u32 ur_load_imm_any(struct nfp_prog *nfp_prog, u32 imm, u32 tmp_reg)
+{
+	if (FIELD_FIT(UR_REG_IMM_MAX, imm))
+		return reg_imm(imm);
+
+	wrp_immed(nfp_prog, tmp_reg, imm);
+	return tmp_reg;
+}
+
+/* re_load_imm_any() - encode immediate or use tmp register (restricted)
+ * If the @imm is small enough encode it directly in operand and return
+ * otherwise load @imm to a spare register and return its encoding.
+ */
+static u32 re_load_imm_any(struct nfp_prog *nfp_prog, u32 imm, u32 tmp_reg)
+{
+	if (FIELD_FIT(RE_REG_IMM_MAX, imm))
+		return reg_imm(imm);
+
+	wrp_immed(nfp_prog, tmp_reg, imm);
+	return tmp_reg;
+}
+
+static void
+wrp_br_special(struct nfp_prog *nfp_prog, enum br_mask mask,
+	       enum br_special special)
+{
+	emit_br(nfp_prog, mask, 0, 0);
+
+	nfp_prog->prog[nfp_prog->prog_len - 1] |=
+		FIELD_PREP(OP_BR_SPECIAL, special);
+}
+
+static void wrp_reg_mov(struct nfp_prog *nfp_prog, u16 dst, u16 src)
+{
+	emit_alu(nfp_prog, reg_both(dst), reg_none(), ALU_OP_NONE, reg_b(src));
+}
+
+static int
+construct_data_ind_ld(struct nfp_prog *nfp_prog, u16 offset,
+		      u16 src, bool src_valid, u8 size)
+{
+	unsigned int i;
+	u16 shift, sz;
+	u32 tmp_reg;
+
+	/* We load the value from the address indicated in @offset and then
+	 * shift out the data we don't need.  Note: this is big endian!
+	 */
+	sz = size < 4 ? 4 : size;
+	shift = size < 4 ? 4 - size : 0;
+
+	if (src_valid) {
+		/* Calculate the true offset (src_reg + imm) */
+		tmp_reg = ur_load_imm_any(nfp_prog, offset, imm_b(nfp_prog));
+		emit_alu(nfp_prog, imm_both(nfp_prog),
+			 reg_a(src), ALU_OP_ADD, tmp_reg);
+		/* Check packet length (size guaranteed to fit b/c it's u8) */
+		emit_alu(nfp_prog, imm_a(nfp_prog),
+			 imm_a(nfp_prog), ALU_OP_ADD, reg_imm(size));
+		emit_alu(nfp_prog, reg_none(),
+			 NFP_BPF_ABI_LEN, ALU_OP_SUB, imm_a(nfp_prog));
+		wrp_br_special(nfp_prog, BR_BLO, OP_BR_GO_ABORT);
+		/* Load data */
+		emit_cmd(nfp_prog, CMD_TGT_READ8, CMD_MODE_32b, 0,
+			 pkt_reg(nfp_prog), imm_b(nfp_prog), sz - 1, true);
+	} else {
+		/* Check packet length */
+		tmp_reg = ur_load_imm_any(nfp_prog, offset + size,
+					  imm_a(nfp_prog));
+		emit_alu(nfp_prog, reg_none(),
+			 NFP_BPF_ABI_LEN, ALU_OP_SUB, tmp_reg);
+		wrp_br_special(nfp_prog, BR_BLO, OP_BR_GO_ABORT);
+		/* Load data */
+		tmp_reg = re_load_imm_any(nfp_prog, offset, imm_b(nfp_prog));
+		emit_cmd(nfp_prog, CMD_TGT_READ8, CMD_MODE_32b, 0,
+			 pkt_reg(nfp_prog), tmp_reg, sz - 1, true);
+	}
+
+	i = 0;
+	if (shift)
+		emit_shf(nfp_prog, reg_both(0), reg_none(), SHF_OP_NONE,
+			 reg_xfer(0), SHF_SC_R_SHF, shift * 8);
+	else
+		for (; i * 4 < size; i++)
+			emit_alu(nfp_prog, reg_both(i),
+				 reg_none(), ALU_OP_NONE, reg_xfer(i));
+
+	if (i < 2)
+		wrp_immed(nfp_prog, reg_both(1), 0);
+
+	return 0;
+}
+
+static int construct_data_ld(struct nfp_prog *nfp_prog, u16 offset, u8 size)
+{
+	return construct_data_ind_ld(nfp_prog, offset, 0, false, size);
+}
+
+static int wrp_set_mark(struct nfp_prog *nfp_prog, u8 src)
+{
+	emit_alu(nfp_prog, NFP_BPF_ABI_MARK,
+		 reg_none(), ALU_OP_NONE, reg_b(src));
+	emit_alu(nfp_prog, NFP_BPF_ABI_FLAGS,
+		 NFP_BPF_ABI_FLAGS, ALU_OP_OR, reg_imm(NFP_BPF_ABI_FLAG_MARK));
+
+	return 0;
+}
+
+static void
+wrp_alu_imm(struct nfp_prog *nfp_prog, u8 dst, enum alu_op alu_op, u32 imm)
+{
+	u32 tmp_reg;
+
+	if (alu_op == ALU_OP_AND) {
+		if (!imm)
+			wrp_immed(nfp_prog, reg_both(dst), 0);
+		if (!imm || !~imm)
+			return;
+	}
+	if (alu_op == ALU_OP_OR) {
+		if (!~imm)
+			wrp_immed(nfp_prog, reg_both(dst), ~0U);
+		if (!imm || !~imm)
+			return;
+	}
+	if (alu_op == ALU_OP_XOR) {
+		if (!~imm)
+			emit_alu(nfp_prog, reg_both(dst), reg_none(),
+				 ALU_OP_NEG, reg_b(dst));
+		if (!imm || !~imm)
+			return;
+	}
+
+	tmp_reg = ur_load_imm_any(nfp_prog, imm, imm_b(nfp_prog));
+	emit_alu(nfp_prog, reg_both(dst), reg_a(dst), alu_op, tmp_reg);
+}
+
+static int
+wrp_alu64_imm(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta,
+	      enum alu_op alu_op, bool skip)
+{
+	const struct bpf_insn *insn = &meta->insn;
+	u64 imm = insn->imm; /* sign extend */
+
+	if (skip) {
+		meta->skip = true;
+		return 0;
+	}
+
+	wrp_alu_imm(nfp_prog, insn->dst_reg * 2, alu_op, imm & ~0U);
+	wrp_alu_imm(nfp_prog, insn->dst_reg * 2 + 1, alu_op, imm >> 32);
+
+	return 0;
+}
+
+static int
+wrp_alu64_reg(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta,
+	      enum alu_op alu_op)
+{
+	u8 dst = meta->insn.dst_reg * 2, src = meta->insn.src_reg * 2;
+
+	emit_alu(nfp_prog, reg_both(dst), reg_a(dst), alu_op, reg_b(src));
+	emit_alu(nfp_prog, reg_both(dst + 1),
+		 reg_a(dst + 1), alu_op, reg_b(src + 1));
+
+	return 0;
+}
+
+static int
+wrp_alu32_imm(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta,
+	      enum alu_op alu_op, bool skip)
+{
+	const struct bpf_insn *insn = &meta->insn;
+
+	if (skip) {
+		meta->skip = true;
+		return 0;
+	}
+
+	wrp_alu_imm(nfp_prog, insn->dst_reg * 2, alu_op, insn->imm);
+	wrp_immed(nfp_prog, reg_both(insn->dst_reg * 2 + 1), 0);
+
+	return 0;
+}
+
+static int
+wrp_alu32_reg(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta,
+	      enum alu_op alu_op)
+{
+	u8 dst = meta->insn.dst_reg * 2, src = meta->insn.src_reg * 2;
+
+	emit_alu(nfp_prog, reg_both(dst), reg_a(dst), alu_op, reg_b(src));
+	wrp_immed(nfp_prog, reg_both(meta->insn.dst_reg * 2 + 1), 0);
+
+	return 0;
+}
+
+static void
+wrp_test_reg_one(struct nfp_prog *nfp_prog, u8 dst, enum alu_op alu_op, u8 src,
+		 enum br_mask br_mask, u16 off)
+{
+	emit_alu(nfp_prog, reg_none(), reg_a(dst), alu_op, reg_b(src));
+	emit_br(nfp_prog, br_mask, off, 0);
+}
+
+static int
+wrp_test_reg(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta,
+	     enum alu_op alu_op, enum br_mask br_mask)
+{
+	const struct bpf_insn *insn = &meta->insn;
+
+	if (insn->off < 0) /* TODO */
+		return -ENOTSUPP;
+
+	wrp_test_reg_one(nfp_prog, insn->dst_reg * 2, alu_op,
+			 insn->src_reg * 2, br_mask, insn->off);
+	wrp_test_reg_one(nfp_prog, insn->dst_reg * 2 + 1, alu_op,
+			 insn->src_reg * 2 + 1, br_mask, insn->off);
+
+	return 0;
+}
+
+static int
+wrp_cmp_imm(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta,
+	    enum br_mask br_mask, bool swap)
+{
+	const struct bpf_insn *insn = &meta->insn;
+	u64 imm = insn->imm; /* sign extend */
+	u8 reg = insn->dst_reg * 2;
+	u32 tmp_reg;
+
+	if (insn->off < 0) /* TODO */
+		return -ENOTSUPP;
+
+	tmp_reg = ur_load_imm_any(nfp_prog, imm & ~0U, imm_b(nfp_prog));
+	if (!swap)
+		emit_alu(nfp_prog, reg_none(), reg_a(reg), ALU_OP_SUB, tmp_reg);
+	else
+		emit_alu(nfp_prog, reg_none(), tmp_reg, ALU_OP_SUB, reg_a(reg));
+
+	tmp_reg = ur_load_imm_any(nfp_prog, imm >> 32, imm_b(nfp_prog));
+	if (!swap)
+		emit_alu(nfp_prog, reg_none(),
+			 reg_a(reg + 1), ALU_OP_SUB_C, tmp_reg);
+	else
+		emit_alu(nfp_prog, reg_none(),
+			 tmp_reg, ALU_OP_SUB_C, reg_a(reg + 1));
+
+	emit_br(nfp_prog, br_mask, insn->off, 0);
+
+	return 0;
+}
+
+static int
+wrp_cmp_reg(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta,
+	    enum br_mask br_mask, bool swap)
+{
+	const struct bpf_insn *insn = &meta->insn;
+	u8 areg = insn->src_reg * 2, breg = insn->dst_reg * 2;
+
+	if (insn->off < 0) /* TODO */
+		return -ENOTSUPP;
+
+	if (swap) {
+		areg ^= breg;
+		breg ^= areg;
+		areg ^= breg;
+	}
+
+	emit_alu(nfp_prog, reg_none(), reg_a(areg), ALU_OP_SUB, reg_b(breg));
+	emit_alu(nfp_prog, reg_none(),
+		 reg_a(areg + 1), ALU_OP_SUB_C, reg_b(breg + 1));
+	emit_br(nfp_prog, br_mask, insn->off, 0);
+
+	return 0;
+}
+
+/* --- Callbacks --- */
+static int mov_reg64(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta)
+{
+	const struct bpf_insn *insn = &meta->insn;
+
+	wrp_reg_mov(nfp_prog, insn->dst_reg * 2, insn->src_reg * 2);
+	wrp_reg_mov(nfp_prog, insn->dst_reg * 2 + 1, insn->src_reg * 2 + 1);
+
+	return 0;
+}
+
+static int mov_imm64(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta)
+{
+	u64 imm = meta->insn.imm; /* sign extend */
+
+	wrp_immed(nfp_prog, reg_both(meta->insn.dst_reg * 2), imm & ~0U);
+	wrp_immed(nfp_prog, reg_both(meta->insn.dst_reg * 2 + 1), imm >> 32);
+
+	return 0;
+}
+
+static int xor_reg64(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta)
+{
+	return wrp_alu64_reg(nfp_prog, meta, ALU_OP_XOR);
+}
+
+static int xor_imm64(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta)
+{
+	return wrp_alu64_imm(nfp_prog, meta, ALU_OP_XOR, !meta->insn.imm);
+}
+
+static int and_reg64(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta)
+{
+	return wrp_alu64_reg(nfp_prog, meta, ALU_OP_AND);
+}
+
+static int and_imm64(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta)
+{
+	return wrp_alu64_imm(nfp_prog, meta, ALU_OP_AND, !~meta->insn.imm);
+}
+
+static int or_reg64(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta)
+{
+	return wrp_alu64_reg(nfp_prog, meta, ALU_OP_OR);
+}
+
+static int or_imm64(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta)
+{
+	return wrp_alu64_imm(nfp_prog, meta, ALU_OP_OR, !meta->insn.imm);
+}
+
+static int add_reg64(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta)
+{
+	const struct bpf_insn *insn = &meta->insn;
+
+	emit_alu(nfp_prog, reg_both(insn->dst_reg * 2),
+		 reg_a(insn->dst_reg * 2), ALU_OP_ADD,
+		 reg_b(insn->src_reg * 2));
+	emit_alu(nfp_prog, reg_both(insn->dst_reg * 2 + 1),
+		 reg_a(insn->dst_reg * 2 + 1), ALU_OP_ADD_C,
+		 reg_b(insn->src_reg * 2 + 1));
+
+	return 0;
+}
+
+static int add_imm64(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta)
+{
+	const struct bpf_insn *insn = &meta->insn;
+	u64 imm = insn->imm; /* sign extend */
+
+	wrp_alu_imm(nfp_prog, insn->dst_reg * 2, ALU_OP_ADD, imm & ~0U);
+	wrp_alu_imm(nfp_prog, insn->dst_reg * 2 + 1, ALU_OP_ADD_C, imm >> 32);
+
+	return 0;
+}
+
+static int sub_reg64(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta)
+{
+	const struct bpf_insn *insn = &meta->insn;
+
+	emit_alu(nfp_prog, reg_both(insn->dst_reg * 2),
+		 reg_a(insn->dst_reg * 2), ALU_OP_SUB,
+		 reg_b(insn->src_reg * 2));
+	emit_alu(nfp_prog, reg_both(insn->dst_reg * 2 + 1),
+		 reg_a(insn->dst_reg * 2 + 1), ALU_OP_SUB_C,
+		 reg_b(insn->src_reg * 2 + 1));
+
+	return 0;
+}
+
+static int sub_imm64(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta)
+{
+	const struct bpf_insn *insn = &meta->insn;
+	u64 imm = insn->imm; /* sign extend */
+
+	wrp_alu_imm(nfp_prog, insn->dst_reg * 2, ALU_OP_SUB, imm & ~0U);
+	wrp_alu_imm(nfp_prog, insn->dst_reg * 2 + 1, ALU_OP_SUB_C, imm >> 32);
+
+	return 0;
+}
+
+static int shl_imm64(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta)
+{
+	const struct bpf_insn *insn = &meta->insn;
+
+	if (insn->imm != 32)
+		return 1; /* TODO */
+
+	wrp_reg_mov(nfp_prog, insn->dst_reg * 2 + 1, insn->dst_reg * 2);
+	wrp_immed(nfp_prog, reg_both(insn->dst_reg * 2), 0);
+
+	return 0;
+}
+
+static int shr_imm64(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta)
+{
+	const struct bpf_insn *insn = &meta->insn;
+
+	if (insn->imm != 32)
+		return 1; /* TODO */
+
+	wrp_reg_mov(nfp_prog, insn->dst_reg * 2, insn->dst_reg * 2 + 1);
+	wrp_immed(nfp_prog, reg_both(insn->dst_reg * 2 + 1), 0);
+
+	return 0;
+}
+
+static int mov_reg(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta)
+{
+	const struct bpf_insn *insn = &meta->insn;
+
+	wrp_reg_mov(nfp_prog, insn->dst_reg * 2,  insn->src_reg * 2);
+	wrp_immed(nfp_prog, reg_both(insn->dst_reg * 2 + 1), 0);
+
+	return 0;
+}
+
+static int mov_imm(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta)
+{
+	const struct bpf_insn *insn = &meta->insn;
+
+	wrp_immed(nfp_prog, reg_both(insn->dst_reg * 2), insn->imm);
+	wrp_immed(nfp_prog, reg_both(insn->dst_reg * 2 + 1), 0);
+
+	return 0;
+}
+
+static int xor_reg(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta)
+{
+	return wrp_alu32_reg(nfp_prog, meta, ALU_OP_XOR);
+}
+
+static int xor_imm(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta)
+{
+	return wrp_alu32_imm(nfp_prog, meta, ALU_OP_XOR, !~meta->insn.imm);
+}
+
+static int and_reg(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta)
+{
+	return wrp_alu32_reg(nfp_prog, meta, ALU_OP_AND);
+}
+
+static int and_imm(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta)
+{
+	return wrp_alu32_imm(nfp_prog, meta, ALU_OP_AND, !~meta->insn.imm);
+}
+
+static int or_reg(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta)
+{
+	return wrp_alu32_reg(nfp_prog, meta, ALU_OP_OR);
+}
+
+static int or_imm(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta)
+{
+	return wrp_alu32_imm(nfp_prog, meta, ALU_OP_OR, !meta->insn.imm);
+}
+
+static int add_reg(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta)
+{
+	return wrp_alu32_reg(nfp_prog, meta, ALU_OP_ADD);
+}
+
+static int add_imm(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta)
+{
+	return wrp_alu32_imm(nfp_prog, meta, ALU_OP_ADD, !meta->insn.imm);
+}
+
+static int sub_reg(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta)
+{
+	return wrp_alu32_reg(nfp_prog, meta, ALU_OP_SUB);
+}
+
+static int sub_imm(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta)
+{
+	return wrp_alu32_imm(nfp_prog, meta, ALU_OP_SUB, !meta->insn.imm);
+}
+
+static int shl_imm(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta)
+{
+	const struct bpf_insn *insn = &meta->insn;
+
+	if (!insn->imm)
+		return 1; /* TODO: zero shift means indirect */
+
+	emit_shf(nfp_prog, reg_both(insn->dst_reg * 2),
+		 reg_none(), SHF_OP_NONE, reg_b(insn->dst_reg * 2),
+		 SHF_SC_L_SHF, insn->imm);
+	wrp_immed(nfp_prog, reg_both(insn->dst_reg * 2 + 1), 0);
+
+	return 0;
+}
+
+static int imm_ld8_part2(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta)
+{
+	wrp_immed(nfp_prog, reg_both(nfp_meta_prev(meta)->insn.dst_reg * 2 + 1),
+		  meta->insn.imm);
+
+	return 0;
+}
+
+static int imm_ld8(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta)
+{
+	const struct bpf_insn *insn = &meta->insn;
+
+	meta->double_cb = imm_ld8_part2;
+	wrp_immed(nfp_prog, reg_both(insn->dst_reg * 2), insn->imm);
+
+	return 0;
+}
+
+static int data_ld1(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta)
+{
+	return construct_data_ld(nfp_prog, meta->insn.imm, 1);
+}
+
+static int data_ld2(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta)
+{
+	return construct_data_ld(nfp_prog, meta->insn.imm, 2);
+}
+
+static int data_ld4(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta)
+{
+	return construct_data_ld(nfp_prog, meta->insn.imm, 4);
+}
+
+static int data_ind_ld1(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta)
+{
+	return construct_data_ind_ld(nfp_prog, meta->insn.imm,
+				     meta->insn.src_reg * 2, true, 1);
+}
+
+static int data_ind_ld2(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta)
+{
+	return construct_data_ind_ld(nfp_prog, meta->insn.imm,
+				     meta->insn.src_reg * 2, true, 2);
+}
+
+static int data_ind_ld4(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta)
+{
+	return construct_data_ind_ld(nfp_prog, meta->insn.imm,
+				     meta->insn.src_reg * 2, true, 4);
+}
+
+static int mem_ldx4(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta)
+{
+	if (meta->insn.off == offsetof(struct sk_buff, len))
+		emit_alu(nfp_prog, reg_both(meta->insn.dst_reg * 2),
+			 reg_none(), ALU_OP_NONE, NFP_BPF_ABI_LEN);
+	else
+		return -ENOTSUPP;
+
+	wrp_immed(nfp_prog, reg_both(meta->insn.dst_reg * 2 + 1), 0);
+
+	return 0;
+}
+
+static int mem_stx4(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta)
+{
+	if (meta->insn.off == offsetof(struct sk_buff, mark))
+		return wrp_set_mark(nfp_prog, meta->insn.src_reg * 2);
+
+	return -ENOTSUPP;
+}
+
+static int jump(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta)
+{
+	if (meta->insn.off < 0) /* TODO */
+		return -ENOTSUPP;
+	emit_br(nfp_prog, BR_UNC, meta->insn.off, 0);
+
+	return 0;
+}
+
+static int jeq_imm(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta)
+{
+	const struct bpf_insn *insn = &meta->insn;
+	u64 imm = insn->imm; /* sign extend */
+	u32 or1 = reg_a(insn->dst_reg * 2), or2 = reg_b(insn->dst_reg * 2 + 1);
+	u32 tmp_reg;
+
+	if (insn->off < 0) /* TODO */
+		return -ENOTSUPP;
+
+	if (imm & ~0U) {
+		tmp_reg = ur_load_imm_any(nfp_prog, imm & ~0U, imm_b(nfp_prog));
+		emit_alu(nfp_prog, imm_a(nfp_prog),
+			 reg_a(insn->dst_reg * 2), ALU_OP_XOR, tmp_reg);
+		or1 = imm_a(nfp_prog);
+	}
+
+	if (imm >> 32) {
+		tmp_reg = ur_load_imm_any(nfp_prog, imm >> 32, imm_b(nfp_prog));
+		emit_alu(nfp_prog, imm_b(nfp_prog),
+			 reg_a(insn->dst_reg * 2 + 1), ALU_OP_XOR, tmp_reg);
+		or2 = imm_b(nfp_prog);
+	}
+
+	emit_alu(nfp_prog, reg_none(), or1, ALU_OP_OR, or2);
+	emit_br(nfp_prog, BR_BEQ, insn->off, 0);
+
+	return 0;
+}
+
+static int jgt_imm(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta)
+{
+	return wrp_cmp_imm(nfp_prog, meta, BR_BLO, false);
+}
+
+static int jge_imm(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta)
+{
+	return wrp_cmp_imm(nfp_prog, meta, BR_BHS, true);
+}
+
+static int jset_imm(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta)
+{
+	const struct bpf_insn *insn = &meta->insn;
+	u64 imm = insn->imm; /* sign extend */
+	u32 tmp_reg;
+
+	if (insn->off < 0) /* TODO */
+		return -ENOTSUPP;
+
+	if (!imm) {
+		meta->skip = true;
+		return 0;
+	}
+
+	if (imm & ~0U) {
+		tmp_reg = ur_load_imm_any(nfp_prog, imm & ~0U, imm_b(nfp_prog));
+		emit_alu(nfp_prog, reg_none(),
+			 reg_a(insn->dst_reg * 2), ALU_OP_AND, tmp_reg);
+		emit_br(nfp_prog, BR_BNE, insn->off, 0);
+	}
+
+	if (imm >> 32) {
+		tmp_reg = ur_load_imm_any(nfp_prog, imm >> 32, imm_b(nfp_prog));
+		emit_alu(nfp_prog, reg_none(),
+			 reg_a(insn->dst_reg * 2 + 1), ALU_OP_AND, tmp_reg);
+		emit_br(nfp_prog, BR_BNE, insn->off, 0);
+	}
+
+	return 0;
+}
+
+static int jne_imm(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta)
+{
+	const struct bpf_insn *insn = &meta->insn;
+	u64 imm = insn->imm; /* sign extend */
+	u32 tmp_reg;
+
+	if (insn->off < 0) /* TODO */
+		return -ENOTSUPP;
+
+	if (!imm) {
+		emit_alu(nfp_prog, reg_none(), reg_a(insn->dst_reg * 2),
+			 ALU_OP_OR, reg_b(insn->dst_reg * 2 + 1));
+		emit_br(nfp_prog, BR_BNE, insn->off, 0);
+	}
+
+	tmp_reg = ur_load_imm_any(nfp_prog, imm & ~0U, imm_b(nfp_prog));
+	emit_alu(nfp_prog, reg_none(),
+		 reg_a(insn->dst_reg * 2), ALU_OP_XOR, tmp_reg);
+	emit_br(nfp_prog, BR_BNE, insn->off, 0);
+
+	tmp_reg = ur_load_imm_any(nfp_prog, imm >> 32, imm_b(nfp_prog));
+	emit_alu(nfp_prog, reg_none(),
+		 reg_a(insn->dst_reg * 2 + 1), ALU_OP_XOR, tmp_reg);
+	emit_br(nfp_prog, BR_BNE, insn->off, 0);
+
+	return 0;
+}
+
+static int jeq_reg(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta)
+{
+	const struct bpf_insn *insn = &meta->insn;
+
+	if (insn->off < 0) /* TODO */
+		return -ENOTSUPP;
+
+	emit_alu(nfp_prog, imm_a(nfp_prog), reg_a(insn->dst_reg * 2),
+		 ALU_OP_XOR, reg_b(insn->src_reg * 2));
+	emit_alu(nfp_prog, imm_b(nfp_prog), reg_a(insn->dst_reg * 2 + 1),
+		 ALU_OP_XOR, reg_b(insn->src_reg * 2 + 1));
+	emit_alu(nfp_prog, reg_none(),
+		 imm_a(nfp_prog), ALU_OP_OR, imm_b(nfp_prog));
+	emit_br(nfp_prog, BR_BEQ, insn->off, 0);
+
+	return 0;
+}
+
+static int jgt_reg(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta)
+{
+	return wrp_cmp_reg(nfp_prog, meta, BR_BLO, false);
+}
+
+static int jge_reg(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta)
+{
+	return wrp_cmp_reg(nfp_prog, meta, BR_BHS, true);
+}
+
+static int jset_reg(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta)
+{
+	return wrp_test_reg(nfp_prog, meta, ALU_OP_AND, BR_BNE);
+}
+
+static int jne_reg(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta)
+{
+	return wrp_test_reg(nfp_prog, meta, ALU_OP_XOR, BR_BNE);
+}
+
+static int goto_out(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta)
+{
+	wrp_br_special(nfp_prog, BR_UNC, OP_BR_GO_OUT);
+
+	return 0;
+}
+
+static const instr_cb_t instr_cb[256] = {
+	[BPF_ALU64 | BPF_MOV | BPF_X] =	mov_reg64,
+	[BPF_ALU64 | BPF_MOV | BPF_K] =	mov_imm64,
+	[BPF_ALU64 | BPF_XOR | BPF_X] =	xor_reg64,
+	[BPF_ALU64 | BPF_XOR | BPF_K] =	xor_imm64,
+	[BPF_ALU64 | BPF_AND | BPF_X] =	and_reg64,
+	[BPF_ALU64 | BPF_AND | BPF_K] =	and_imm64,
+	[BPF_ALU64 | BPF_OR | BPF_X] =	or_reg64,
+	[BPF_ALU64 | BPF_OR | BPF_K] =	or_imm64,
+	[BPF_ALU64 | BPF_ADD | BPF_X] =	add_reg64,
+	[BPF_ALU64 | BPF_ADD | BPF_K] =	add_imm64,
+	[BPF_ALU64 | BPF_SUB | BPF_X] =	sub_reg64,
+	[BPF_ALU64 | BPF_SUB | BPF_K] =	sub_imm64,
+	[BPF_ALU64 | BPF_LSH | BPF_K] =	shl_imm64,
+	[BPF_ALU64 | BPF_RSH | BPF_K] =	shr_imm64,
+	[BPF_ALU | BPF_MOV | BPF_X] =	mov_reg,
+	[BPF_ALU | BPF_MOV | BPF_K] =	mov_imm,
+	[BPF_ALU | BPF_XOR | BPF_X] =	xor_reg,
+	[BPF_ALU | BPF_XOR | BPF_K] =	xor_imm,
+	[BPF_ALU | BPF_AND | BPF_X] =	and_reg,
+	[BPF_ALU | BPF_AND | BPF_K] =	and_imm,
+	[BPF_ALU | BPF_OR | BPF_X] =	or_reg,
+	[BPF_ALU | BPF_OR | BPF_K] =	or_imm,
+	[BPF_ALU | BPF_ADD | BPF_X] =	add_reg,
+	[BPF_ALU | BPF_ADD | BPF_K] =	add_imm,
+	[BPF_ALU | BPF_SUB | BPF_X] =	sub_reg,
+	[BPF_ALU | BPF_SUB | BPF_K] =	sub_imm,
+	[BPF_ALU | BPF_LSH | BPF_K] =	shl_imm,
+	[BPF_LD | BPF_IMM | BPF_DW] =	imm_ld8,
+	[BPF_LD | BPF_ABS | BPF_B] =	data_ld1,
+	[BPF_LD | BPF_ABS | BPF_H] =	data_ld2,
+	[BPF_LD | BPF_ABS | BPF_W] =	data_ld4,
+	[BPF_LD | BPF_IND | BPF_B] =	data_ind_ld1,
+	[BPF_LD | BPF_IND | BPF_H] =	data_ind_ld2,
+	[BPF_LD | BPF_IND | BPF_W] =	data_ind_ld4,
+	[BPF_LDX | BPF_MEM | BPF_W] =	mem_ldx4,
+	[BPF_STX | BPF_MEM | BPF_W] =	mem_stx4,
+	[BPF_JMP | BPF_JA | BPF_K] =	jump,
+	[BPF_JMP | BPF_JEQ | BPF_K] =	jeq_imm,
+	[BPF_JMP | BPF_JGT | BPF_K] =	jgt_imm,
+	[BPF_JMP | BPF_JGE | BPF_K] =	jge_imm,
+	[BPF_JMP | BPF_JSET | BPF_K] =	jset_imm,
+	[BPF_JMP | BPF_JNE | BPF_K] =	jne_imm,
+	[BPF_JMP | BPF_JEQ | BPF_X] =	jeq_reg,
+	[BPF_JMP | BPF_JGT | BPF_X] =	jgt_reg,
+	[BPF_JMP | BPF_JGE | BPF_X] =	jge_reg,
+	[BPF_JMP | BPF_JSET | BPF_X] =	jset_reg,
+	[BPF_JMP | BPF_JNE | BPF_X] =	jne_reg,
+	[BPF_JMP | BPF_EXIT] =		goto_out,
+};
+
+/* --- Misc code --- */
+static void br_set_offset(u64 *instr, u16 offset)
+{
+	u16 addr_lo, addr_hi;
+
+	addr_lo = offset & (OP_BR_ADDR_LO >> __bf_shf(OP_BR_ADDR_LO));
+	addr_hi = offset != addr_lo;
+	*instr &= ~(OP_BR_ADDR_HI | OP_BR_ADDR_LO);
+	*instr |= FIELD_PREP(OP_BR_ADDR_HI, addr_hi);
+	*instr |= FIELD_PREP(OP_BR_ADDR_LO, addr_lo);
+}
+
+/* --- Assembler logic --- */
+static int nfp_fixup_branches(struct nfp_prog *nfp_prog)
+{
+	struct nfp_insn_meta *meta, *next;
+	u32 off, br_idx;
+	u32 idx;
+
+	nfp_for_each_insn_walk2(nfp_prog, meta, next) {
+		if (meta->skip)
+			continue;
+		if (BPF_CLASS(meta->insn.code) != BPF_JMP)
+			continue;
+
+		br_idx = nfp_prog_offset_to_index(nfp_prog, next->off) - 1;
+		if (!nfp_is_br(nfp_prog->prog[br_idx])) {
+			pr_err("Fixup found block not ending in branch %d %02x %016llx!!\n",
+			       br_idx, meta->insn.code, nfp_prog->prog[br_idx]);
+			return -ELOOP;
+		}
+		/* Leave special branches for later */
+		if (FIELD_GET(OP_BR_SPECIAL, nfp_prog->prog[br_idx]))
+			continue;
+
+		/* Find the target offset in assembler realm */
+		off = meta->insn.off;
+		if (!off) {
+			pr_err("Fixup found zero offset!!\n");
+			return -ELOOP;
+		}
+
+		while (off && nfp_meta_has_next(nfp_prog, next)) {
+			next = nfp_meta_next(next);
+			off--;
+		}
+		if (off) {
+			pr_err("Fixup found too large jump!! %d\n", off);
+			return -ELOOP;
+		}
+
+		if (next->skip) {
+			pr_err("Branch landing on removed instruction!!\n");
+			return -ELOOP;
+		}
+
+		for (idx = nfp_prog_offset_to_index(nfp_prog, meta->off);
+		     idx <= br_idx; idx++) {
+			if (!nfp_is_br(nfp_prog->prog[idx]))
+				continue;
+			br_set_offset(&nfp_prog->prog[idx], next->off);
+		}
+	}
+
+	/* Fixup 'goto out's separately, they can be scattered around */
+	for (br_idx = 0; br_idx < nfp_prog->prog_len; br_idx++) {
+		enum br_special special;
+
+		if ((nfp_prog->prog[br_idx] & OP_BR_BASE_MASK) != OP_BR_BASE)
+			continue;
+
+		special = FIELD_GET(OP_BR_SPECIAL, nfp_prog->prog[br_idx]);
+		switch (special) {
+		case OP_BR_NORMAL:
+			break;
+		case OP_BR_GO_OUT:
+			br_set_offset(&nfp_prog->prog[br_idx],
+				      nfp_prog->tgt_out);
+			break;
+		case OP_BR_GO_ABORT:
+			br_set_offset(&nfp_prog->prog[br_idx],
+				      nfp_prog->tgt_abort);
+			break;
+		}
+
+		nfp_prog->prog[br_idx] &= ~OP_BR_SPECIAL;
+	}
+
+	return 0;
+}
+
+static void nfp_intro(struct nfp_prog *nfp_prog)
+{
+	emit_alu(nfp_prog, pkt_reg(nfp_prog),
+		 reg_none(), ALU_OP_NONE, NFP_BPF_ABI_PKT);
+}
+
+static void nfp_outro_tc_legacy(struct nfp_prog *nfp_prog)
+{
+	const u8 act2code[] = {
+		[NN_ACT_TC_DROP]  = 0x22,
+		[NN_ACT_TC_REDIR] = 0x24
+	};
+	/* Target for aborts */
+	nfp_prog->tgt_abort = nfp_prog_current_offset(nfp_prog);
+	wrp_immed(nfp_prog, reg_both(0), 0);
+
+	/* Target for normal exits */
+	nfp_prog->tgt_out = nfp_prog_current_offset(nfp_prog);
+	/* Legacy TC mode:
+	 *   0        0x11 -> pass,  count as stat0
+	 *  -1  drop  0x22 -> drop,  count as stat1
+	 *     redir  0x24 -> redir, count as stat1
+	 *  ife mark  0x21 -> pass,  count as stat1
+	 *  ife + tx  0x24 -> redir, count as stat1
+	 */
+	emit_br_byte_neq(nfp_prog, reg_b(0), 0xff, 0, nfp_prog->tgt_done, 2);
+	emit_alu(nfp_prog, reg_a(0),
+		 reg_none(), ALU_OP_NONE, NFP_BPF_ABI_FLAGS);
+	emit_ld_field(nfp_prog, reg_a(0), 0xc, reg_imm(0x11), SHF_SC_L_SHF, 16);
+
+	emit_br(nfp_prog, BR_UNC, nfp_prog->tgt_done, 1);
+	emit_ld_field(nfp_prog, reg_a(0), 0xc, reg_imm(act2code[nfp_prog->act]),
+		      SHF_SC_L_SHF, 16);
+}
+
+static void nfp_outro_tc_da(struct nfp_prog *nfp_prog)
+{
+	/* TC direct-action mode:
+	 *   0,1   ok        NOT SUPPORTED[1]
+	 *   2   drop  0x22 -> drop,  count as stat1
+	 *   4,5 nuke  0x02 -> drop
+	 *   7  redir  0x44 -> redir, count as stat2
+	 *   * unspec  0x11 -> pass,  count as stat0
+	 *
+	 * [1] We can't support OK and RECLASSIFY because we can't tell TC
+	 *     the exact decision made.  We are forced to support UNSPEC
+	 *     to handle aborts so that's the only one we handle for passing
+	 *     packets up the stack.
+	 */
+	/* Target for aborts */
+	nfp_prog->tgt_abort = nfp_prog_current_offset(nfp_prog);
+
+	emit_br_def(nfp_prog, nfp_prog->tgt_done, 2);
+
+	emit_alu(nfp_prog, reg_a(0),
+		 reg_none(), ALU_OP_NONE, NFP_BPF_ABI_FLAGS);
+	emit_ld_field(nfp_prog, reg_a(0), 0xc, reg_imm(0x11), SHF_SC_L_SHF, 16);
+
+	/* Target for normal exits */
+	nfp_prog->tgt_out = nfp_prog_current_offset(nfp_prog);
+
+	/* if R0 > 7 jump to abort */
+	emit_alu(nfp_prog, reg_none(), reg_imm(7), ALU_OP_SUB, reg_b(0));
+	emit_br(nfp_prog, BR_BLO, nfp_prog->tgt_abort, 0);
+	emit_alu(nfp_prog, reg_a(0),
+		 reg_none(), ALU_OP_NONE, NFP_BPF_ABI_FLAGS);
+
+	wrp_immed(nfp_prog, reg_b(2), 0x41221211);
+	wrp_immed(nfp_prog, reg_b(3), 0x41001211);
+
+	emit_shf(nfp_prog, reg_a(1),
+		 reg_none(), SHF_OP_NONE, reg_b(0), SHF_SC_L_SHF, 2);
+
+	emit_alu(nfp_prog, reg_none(), reg_a(1), ALU_OP_OR, reg_imm(0));
+	emit_shf(nfp_prog, reg_a(2),
+		 reg_imm(0xf), SHF_OP_AND, reg_b(2), SHF_SC_R_SHF, 0);
+
+	emit_alu(nfp_prog, reg_none(), reg_a(1), ALU_OP_OR, reg_imm(0));
+	emit_shf(nfp_prog, reg_b(2),
+		 reg_imm(0xf), SHF_OP_AND, reg_b(3), SHF_SC_R_SHF, 0);
+
+	emit_br_def(nfp_prog, nfp_prog->tgt_done, 2);
+
+	emit_shf(nfp_prog, reg_b(2),
+		 reg_a(2), SHF_OP_OR, reg_b(2), SHF_SC_L_SHF, 4);
+	emit_ld_field(nfp_prog, reg_a(0), 0xc, reg_b(2), SHF_SC_L_SHF, 16);
+}
+
+static void nfp_outro(struct nfp_prog *nfp_prog)
+{
+	switch (nfp_prog->act) {
+	case NN_ACT_DIRECT:
+		nfp_outro_tc_da(nfp_prog);
+		break;
+	case NN_ACT_TC_DROP:
+	case NN_ACT_TC_REDIR:
+		nfp_outro_tc_legacy(nfp_prog);
+		break;
+	}
+}
+
+static int nfp_translate(struct nfp_prog *nfp_prog)
+{
+	struct nfp_insn_meta *meta;
+	int err;
+
+	nfp_intro(nfp_prog);
+	if (nfp_prog->error)
+		return nfp_prog->error;
+
+	list_for_each_entry(meta, &nfp_prog->insns, l) {
+		instr_cb_t cb = instr_cb[meta->insn.code];
+
+		meta->off = nfp_prog_current_offset(nfp_prog);
+
+		if (meta->skip) {
+			nfp_prog->n_translated++;
+			continue;
+		}
+
+		if (nfp_meta_has_prev(nfp_prog, meta) &&
+		    nfp_meta_prev(meta)->double_cb)
+			cb = nfp_meta_prev(meta)->double_cb;
+		if (!cb)
+			return -ENOENT;
+		err = cb(nfp_prog, meta);
+		if (err)
+			return err;
+
+		nfp_prog->n_translated++;
+	}
+
+	nfp_outro(nfp_prog);
+	if (nfp_prog->error)
+		return nfp_prog->error;
+
+	return nfp_fixup_branches(nfp_prog);
+}
+
+static int
+nfp_prog_prepare(struct nfp_prog *nfp_prog, const struct bpf_insn *prog,
+		 unsigned int cnt)
+{
+	unsigned int i;
+
+	for (i = 0; i < cnt; i++) {
+		struct nfp_insn_meta *meta;
+
+		meta = kzalloc(sizeof(*meta), GFP_KERNEL);
+		if (!meta)
+			return -ENOMEM;
+
+		meta->insn = prog[i];
+		meta->n = i;
+
+		list_add_tail(&meta->l, &nfp_prog->insns);
+	}
+
+	return 0;
+}
+
+/* --- Optimizations --- */
+static void nfp_bpf_opt_reg_init(struct nfp_prog *nfp_prog)
+{
+	struct nfp_insn_meta *meta;
+
+	list_for_each_entry(meta, &nfp_prog->insns, l) {
+		struct bpf_insn insn = meta->insn;
+
+		/* Programs converted from cBPF start with register xoring */
+		if (insn.code == (BPF_ALU64 | BPF_XOR | BPF_X) &&
+		    insn.src_reg == insn.dst_reg)
+			continue;
+
+		/* Programs start with R6 = R1 but we ignore the skb pointer */
+		if (insn.code == (BPF_ALU64 | BPF_MOV | BPF_X) &&
+		    insn.src_reg == 1 && insn.dst_reg == 6)
+			meta->skip = true;
+
+		/* Return as soon as something doesn't match */
+		if (!meta->skip)
+			return;
+	}
+}
+
+/* Try to rename registers so that program uses only low ones */
+static int nfp_bpf_opt_reg_rename(struct nfp_prog *nfp_prog)
+{
+	bool reg_used[MAX_BPF_REG] = {};
+	u8 tgt_reg[MAX_BPF_REG] = {};
+	struct nfp_insn_meta *meta;
+	unsigned int i, j;
+
+	list_for_each_entry(meta, &nfp_prog->insns, l) {
+		if (meta->skip)
+			continue;
+
+		reg_used[meta->insn.src_reg] = true;
+		reg_used[meta->insn.dst_reg] = true;
+	}
+
+	for (i = 0, j = 0; i < ARRAY_SIZE(tgt_reg); i++) {
+		if (!reg_used[i])
+			continue;
+
+		tgt_reg[i] = j++;
+	}
+	nfp_prog->num_regs = j;
+
+	list_for_each_entry(meta, &nfp_prog->insns, l) {
+		meta->insn.src_reg = tgt_reg[meta->insn.src_reg];
+		meta->insn.dst_reg = tgt_reg[meta->insn.dst_reg];
+	}
+
+	return 0;
+}
+
+/* Remove masking after load since our load guarantees this is not needed */
+static void nfp_bpf_opt_ld_mask(struct nfp_prog *nfp_prog)
+{
+	struct nfp_insn_meta *meta1, *meta2;
+	const s32 exp_mask[] = {
+		[BPF_B] = 0x000000ffU,
+		[BPF_H] = 0x0000ffffU,
+		[BPF_W] = 0xffffffffU,
+	};
+
+	nfp_for_each_insn_walk2(nfp_prog, meta1, meta2) {
+		struct bpf_insn insn, next;
+
+		insn = meta1->insn;
+		next = meta2->insn;
+
+		if (BPF_CLASS(insn.code) != BPF_LD)
+			continue;
+		if (BPF_MODE(insn.code) != BPF_ABS &&
+		    BPF_MODE(insn.code) != BPF_IND)
+			continue;
+
+		if (next.code != (BPF_ALU64 | BPF_AND | BPF_K))
+			continue;
+
+		if (!exp_mask[BPF_SIZE(insn.code)])
+			continue;
+		if (exp_mask[BPF_SIZE(insn.code)] != next.imm)
+			continue;
+
+		if (next.src_reg || next.dst_reg)
+			continue;
+
+		meta2->skip = true;
+	}
+}
+
+static void nfp_bpf_opt_ld_shift(struct nfp_prog *nfp_prog)
+{
+	struct nfp_insn_meta *meta1, *meta2, *meta3;
+
+	nfp_for_each_insn_walk3(nfp_prog, meta1, meta2, meta3) {
+		struct bpf_insn insn, next1, next2;
+
+		insn = meta1->insn;
+		next1 = meta2->insn;
+		next2 = meta3->insn;
+
+		if (BPF_CLASS(insn.code) != BPF_LD)
+			continue;
+		if (BPF_MODE(insn.code) != BPF_ABS &&
+		    BPF_MODE(insn.code) != BPF_IND)
+			continue;
+		if (BPF_SIZE(insn.code) != BPF_W)
+			continue;
+
+		if (!(next1.code == (BPF_LSH | BPF_K | BPF_ALU64) &&
+		      next2.code == (BPF_RSH | BPF_K | BPF_ALU64)) &&
+		    !(next1.code == (BPF_RSH | BPF_K | BPF_ALU64) &&
+		      next2.code == (BPF_LSH | BPF_K | BPF_ALU64)))
+			continue;
+
+		if (next1.src_reg || next1.dst_reg ||
+		    next2.src_reg || next2.dst_reg)
+			continue;
+
+		if (next1.imm != 0x20 || next2.imm != 0x20)
+			continue;
+
+		meta2->skip = true;
+		meta3->skip = true;
+	}
+}
+
+static int nfp_bpf_optimize(struct nfp_prog *nfp_prog)
+{
+	int ret;
+
+	nfp_bpf_opt_reg_init(nfp_prog);
+
+	ret = nfp_bpf_opt_reg_rename(nfp_prog);
+	if (ret)
+		return ret;
+
+	nfp_bpf_opt_ld_mask(nfp_prog);
+	nfp_bpf_opt_ld_shift(nfp_prog);
+
+	return 0;
+}
+
+/**
+ * nfp_bpf_jit() - translate BPF code into NFP assembly
+ * @filter:	kernel BPF filter struct
+ * @prog_mem:	memory to store assembler instructions
+ * @act:	action attached to this eBPF program
+ * @prog_start:	offset of the first instruction when loaded
+ * @prog_done:	where to jump on exit
+ * @prog_sz:	size of @prog_mem in instructions
+ * @res:	achieved parameters of translation results
+ */
+int
+nfp_bpf_jit(struct bpf_prog *filter, void *prog_mem,
+	    enum nfp_bpf_action_type act,
+	    unsigned int prog_start, unsigned int prog_done,
+	    unsigned int prog_sz, struct nfp_bpf_result *res)
+{
+	struct nfp_prog *nfp_prog;
+	int ret;
+
+	nfp_prog = kzalloc(sizeof(*nfp_prog), GFP_KERNEL);
+	if (!nfp_prog)
+		return -ENOMEM;
+
+	INIT_LIST_HEAD(&nfp_prog->insns);
+	nfp_prog->act = act;
+	nfp_prog->start_off = prog_start;
+	nfp_prog->tgt_done = prog_done;
+
+	ret = nfp_prog_prepare(nfp_prog, filter->insnsi, filter->len);
+	if (ret)
+		goto out;
+
+	ret = nfp_prog_verify(nfp_prog, filter);
+	if (ret)
+		goto out;
+
+	ret = nfp_bpf_optimize(nfp_prog);
+	if (ret)
+		goto out;
+
+	if (nfp_prog->num_regs <= 7)
+		nfp_prog->regs_per_thread = 16;
+	else
+		nfp_prog->regs_per_thread = 32;
+
+	nfp_prog->prog = prog_mem;
+	nfp_prog->__prog_alloc_len = prog_sz;
+
+	ret = nfp_translate(nfp_prog);
+	if (ret) {
+		pr_err("Translation failed with error %d (translated: %u)\n",
+		       ret, nfp_prog->n_translated);
+		ret = -EINVAL;
+	}
+
+	res->n_instr = nfp_prog->prog_len;
+	res->dense_mode = nfp_prog->num_regs <= 7;
+out:
+	nfp_prog_free(nfp_prog);
+
+	return ret;
+}
diff --git a/drivers/net/ethernet/netronome/nfp/nfp_bpf_verifier.c b/drivers/net/ethernet/netronome/nfp/nfp_bpf_verifier.c
new file mode 100644
index 000000000000..144cae87f63a
--- /dev/null
+++ b/drivers/net/ethernet/netronome/nfp/nfp_bpf_verifier.c
@@ -0,0 +1,171 @@
+/*
+ * Copyright (C) 2016 Netronome Systems, Inc.
+ *
+ * This software is dual licensed under the GNU General License Version 2,
+ * June 1991 as shown in the file COPYING in the top-level directory of this
+ * source tree or the BSD 2-Clause License provided below.  You have the
+ * option to license this software under the complete terms of either license.
+ *
+ * The BSD 2-Clause License:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      1. Redistributions of source code must retain the above
+ *         copyright notice, this list of conditions and the following
+ *         disclaimer.
+ *
+ *      2. 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.
+ */
+
+#define pr_fmt(fmt)	"NFP net bpf: " fmt
+
+#include <linux/bpf.h>
+#include <linux/bpf_verifier.h>
+#include <linux/kernel.h>
+#include <linux/pkt_cls.h>
+
+#include "nfp_bpf.h"
+
+/* Analyzer/verifier definitions */
+struct nfp_bpf_analyzer_priv {
+	struct nfp_prog *prog;
+	struct nfp_insn_meta *meta;
+};
+
+static struct nfp_insn_meta *
+nfp_bpf_goto_meta(struct nfp_prog *nfp_prog, struct nfp_insn_meta *meta,
+		  unsigned int insn_idx, unsigned int n_insns)
+{
+	unsigned int forward, backward, i;
+
+	backward = meta->n - insn_idx;
+	forward = insn_idx - meta->n;
+
+	if (min(forward, backward) > n_insns - insn_idx - 1) {
+		backward = n_insns - insn_idx - 1;
+		meta = nfp_prog_last_meta(nfp_prog);
+	}
+	if (min(forward, backward) > insn_idx && backward > insn_idx) {
+		forward = insn_idx;
+		meta = nfp_prog_first_meta(nfp_prog);
+	}
+
+	if (forward < backward)
+		for (i = 0; i < forward; i++)
+			meta = nfp_meta_next(meta);
+	else
+		for (i = 0; i < backward; i++)
+			meta = nfp_meta_prev(meta);
+
+	return meta;
+}
+
+static int
+nfp_bpf_check_exit(struct nfp_prog *nfp_prog,
+		   const struct bpf_verifier_env *env)
+{
+	const struct bpf_reg_state *reg0 = &env->cur_state.regs[0];
+
+	if (reg0->type != CONST_IMM) {
+		pr_info("unsupported exit state: %d, imm: %llx\n",
+			reg0->type, reg0->imm);
+		return -EINVAL;
+	}
+
+	if (nfp_prog->act != NN_ACT_DIRECT &&
+	    reg0->imm != 0 && (reg0->imm & ~0U) != ~0U) {
+		pr_info("unsupported exit state: %d, imm: %llx\n",
+			reg0->type, reg0->imm);
+		return -EINVAL;
+	}
+
+	if (nfp_prog->act == NN_ACT_DIRECT && reg0->imm <= TC_ACT_REDIRECT &&
+	    reg0->imm != TC_ACT_SHOT && reg0->imm != TC_ACT_STOLEN &&
+	    reg0->imm != TC_ACT_QUEUED) {
+		pr_info("unsupported exit state: %d, imm: %llx\n",
+			reg0->type, reg0->imm);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int
+nfp_bpf_check_ctx_ptr(struct nfp_prog *nfp_prog,
+		      const struct bpf_verifier_env *env, u8 reg)
+{
+	if (env->cur_state.regs[reg].type != PTR_TO_CTX)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int
+nfp_verify_insn(struct bpf_verifier_env *env, int insn_idx, int prev_insn_idx)
+{
+	struct nfp_bpf_analyzer_priv *priv = env->analyzer_priv;
+	struct nfp_insn_meta *meta = priv->meta;
+
+	meta = nfp_bpf_goto_meta(priv->prog, meta, insn_idx, env->prog->len);
+	priv->meta = meta;
+
+	if (meta->insn.src_reg == BPF_REG_10 ||
+	    meta->insn.dst_reg == BPF_REG_10) {
+		pr_err("stack not yet supported\n");
+		return -EINVAL;
+	}
+	if (meta->insn.src_reg >= MAX_BPF_REG ||
+	    meta->insn.dst_reg >= MAX_BPF_REG) {
+		pr_err("program uses extended registers - jit hardening?\n");
+		return -EINVAL;
+	}
+
+	if (meta->insn.code == (BPF_JMP | BPF_EXIT))
+		return nfp_bpf_check_exit(priv->prog, env);
+
+	if ((meta->insn.code & ~BPF_SIZE_MASK) == (BPF_LDX | BPF_MEM))
+		return nfp_bpf_check_ctx_ptr(priv->prog, env,
+					     meta->insn.src_reg);
+	if ((meta->insn.code & ~BPF_SIZE_MASK) == (BPF_STX | BPF_MEM))
+		return nfp_bpf_check_ctx_ptr(priv->prog, env,
+					     meta->insn.dst_reg);
+
+	return 0;
+}
+
+static const struct bpf_ext_analyzer_ops nfp_bpf_analyzer_ops = {
+	.insn_hook = nfp_verify_insn,
+};
+
+int nfp_prog_verify(struct nfp_prog *nfp_prog, struct bpf_prog *prog)
+{
+	struct nfp_bpf_analyzer_priv *priv;
+	int ret;
+
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->prog = nfp_prog;
+	priv->meta = nfp_prog_first_meta(nfp_prog);
+
+	ret = bpf_analyzer(prog, &nfp_bpf_analyzer_ops, priv);
+
+	kfree(priv);
+
+	return ret;
+}
diff --git a/drivers/net/ethernet/netronome/nfp/nfp_net.h b/drivers/net/ethernet/netronome/nfp/nfp_net.h
index 690635660195..ed824e11a1e3 100644
--- a/drivers/net/ethernet/netronome/nfp/nfp_net.h
+++ b/drivers/net/ethernet/netronome/nfp/nfp_net.h
@@ -62,6 +62,9 @@
 /* Max time to wait for NFP to respond on updates (in seconds) */
 #define NFP_NET_POLL_TIMEOUT	5
 
+/* Interval for reading offloaded filter stats */
+#define NFP_NET_STAT_POLL_IVL	msecs_to_jiffies(100)
+
 /* Bar allocation */
 #define NFP_NET_CTRL_BAR	0
 #define NFP_NET_Q0_BAR		2
@@ -220,7 +223,7 @@ struct nfp_net_tx_ring {
 #define PCIE_DESC_RX_I_TCP_CSUM_OK	cpu_to_le16(BIT(11))
 #define PCIE_DESC_RX_I_UDP_CSUM		cpu_to_le16(BIT(10))
 #define PCIE_DESC_RX_I_UDP_CSUM_OK	cpu_to_le16(BIT(9))
-#define PCIE_DESC_RX_SPARE		cpu_to_le16(BIT(8))
+#define PCIE_DESC_RX_BPF		cpu_to_le16(BIT(8))
 #define PCIE_DESC_RX_EOP		cpu_to_le16(BIT(7))
 #define PCIE_DESC_RX_IP4_CSUM		cpu_to_le16(BIT(6))
 #define PCIE_DESC_RX_IP4_CSUM_OK	cpu_to_le16(BIT(5))
@@ -266,6 +269,8 @@ struct nfp_net_rx_desc {
 	};
 };
 
+#define NFP_NET_META_FIELD_MASK GENMASK(NFP_NET_META_FIELD_SIZE - 1, 0)
+
 struct nfp_net_rx_hash {
 	__be32 hash_type;
 	__be32 hash;
@@ -405,6 +410,11 @@ static inline bool nfp_net_fw_ver_eq(struct nfp_net_fw_version *fw_ver,
 	       fw_ver->minor == minor;
 }
 
+struct nfp_stat_pair {
+	u64 pkts;
+	u64 bytes;
+};
+
 /**
  * struct nfp_net - NFP network device structure
  * @pdev:               Backpointer to PCI device
@@ -413,6 +423,7 @@ static inline bool nfp_net_fw_ver_eq(struct nfp_net_fw_version *fw_ver,
  * @is_vf:              Is the driver attached to a VF?
  * @is_nfp3200:         Is the driver for a NFP-3200 card?
  * @fw_loaded:          Is the firmware loaded?
+ * @bpf_offload_skip_sw:  Offloaded BPF program will not be rerun by cls_bpf
  * @ctrl:               Local copy of the control register/word.
  * @fl_bufsz:           Currently configured size of the freelist buffers
  * @rx_offset:		Offset in the RX buffers where packet data starts
@@ -427,6 +438,11 @@ static inline bool nfp_net_fw_ver_eq(struct nfp_net_fw_version *fw_ver,
  * @rss_cfg:            RSS configuration
  * @rss_key:            RSS secret key
  * @rss_itbl:           RSS indirection table
+ * @rx_filter:		Filter offload statistics - dropped packets/bytes
+ * @rx_filter_prev:	Filter offload statistics - values from previous update
+ * @rx_filter_change:	Jiffies when statistics last changed
+ * @rx_filter_stats_timer:  Timer for polling filter offload statistics
+ * @rx_filter_lock:	Lock protecting timer state changes (teardown)
  * @max_tx_rings:       Maximum number of TX rings supported by the Firmware
  * @max_rx_rings:       Maximum number of RX rings supported by the Firmware
  * @num_tx_rings:       Currently configured number of TX rings
@@ -473,6 +489,7 @@ struct nfp_net {
 	unsigned is_vf:1;
 	unsigned is_nfp3200:1;
 	unsigned fw_loaded:1;
+	unsigned bpf_offload_skip_sw:1;
 
 	u32 ctrl;
 	u32 fl_bufsz;
@@ -502,6 +519,11 @@ struct nfp_net {
 	u8 rss_key[NFP_NET_CFG_RSS_KEY_SZ];
 	u8 rss_itbl[NFP_NET_CFG_RSS_ITBL_SZ];
 
+	struct nfp_stat_pair rx_filter, rx_filter_prev;
+	unsigned long rx_filter_change;
+	struct timer_list rx_filter_stats_timer;
+	spinlock_t rx_filter_lock;
+
 	int max_tx_rings;
 	int max_rx_rings;
 
@@ -561,12 +583,28 @@ struct nfp_net {
 /* Functions to read/write from/to a BAR
  * Performs any endian conversion necessary.
  */
+static inline u16 nn_readb(struct nfp_net *nn, int off)
+{
+	return readb(nn->ctrl_bar + off);
+}
+
 static inline void nn_writeb(struct nfp_net *nn, int off, u8 val)
 {
 	writeb(val, nn->ctrl_bar + off);
 }
 
-/* NFP-3200 can't handle 16-bit accesses too well - hence no readw/writew */
+/* NFP-3200 can't handle 16-bit accesses too well */
+static inline u16 nn_readw(struct nfp_net *nn, int off)
+{
+	WARN_ON_ONCE(nn->is_nfp3200);
+	return readw(nn->ctrl_bar + off);
+}
+
+static inline void nn_writew(struct nfp_net *nn, int off, u16 val)
+{
+	WARN_ON_ONCE(nn->is_nfp3200);
+	writew(val, nn->ctrl_bar + off);
+}
 
 static inline u32 nn_readl(struct nfp_net *nn, int off)
 {
@@ -757,4 +795,9 @@ static inline void nfp_net_debugfs_adapter_del(struct nfp_net *nn)
 }
 #endif /* CONFIG_NFP_NET_DEBUG */
 
+void nfp_net_filter_stats_timer(unsigned long data);
+int
+nfp_net_bpf_offload(struct nfp_net *nn, u32 handle, __be16 proto,
+		    struct tc_cls_bpf_offload *cls_bpf);
+
 #endif /* _NFP_NET_H_ */
diff --git a/drivers/net/ethernet/netronome/nfp/nfp_net_common.c b/drivers/net/ethernet/netronome/nfp/nfp_net_common.c
index 39dadfca84ef..aee3fd2b6538 100644
--- a/drivers/net/ethernet/netronome/nfp/nfp_net_common.c
+++ b/drivers/net/ethernet/netronome/nfp/nfp_net_common.c
@@ -60,6 +60,7 @@
 
 #include <linux/ktime.h>
 
+#include <net/pkt_cls.h>
 #include <net/vxlan.h>
 
 #include "nfp_net_ctrl.h"
@@ -1292,38 +1293,72 @@ static void nfp_net_rx_csum(struct nfp_net *nn, struct nfp_net_r_vector *r_vec,
 	}
 }
 
-/**
- * nfp_net_set_hash() - Set SKB hash data
- * @netdev: adapter's net_device structure
- * @skb:   SKB to set the hash data on
- * @rxd:   RX descriptor
- *
- * The RSS hash and hash-type are pre-pended to the packet data.
- * Extract and decode it and set the skb fields.
- */
 static void nfp_net_set_hash(struct net_device *netdev, struct sk_buff *skb,
-			     struct nfp_net_rx_desc *rxd)
+			     unsigned int type, __be32 *hash)
 {
-	struct nfp_net_rx_hash *rx_hash;
-
-	if (!(rxd->rxd.flags & PCIE_DESC_RX_RSS) ||
-	    !(netdev->features & NETIF_F_RXHASH))
+	if (!(netdev->features & NETIF_F_RXHASH))
 		return;
 
-	rx_hash = (struct nfp_net_rx_hash *)(skb->data - sizeof(*rx_hash));
-
-	switch (be32_to_cpu(rx_hash->hash_type)) {
+	switch (type) {
 	case NFP_NET_RSS_IPV4:
 	case NFP_NET_RSS_IPV6:
 	case NFP_NET_RSS_IPV6_EX:
-		skb_set_hash(skb, be32_to_cpu(rx_hash->hash), PKT_HASH_TYPE_L3);
+		skb_set_hash(skb, get_unaligned_be32(hash), PKT_HASH_TYPE_L3);
 		break;
 	default:
-		skb_set_hash(skb, be32_to_cpu(rx_hash->hash), PKT_HASH_TYPE_L4);
+		skb_set_hash(skb, get_unaligned_be32(hash), PKT_HASH_TYPE_L4);
 		break;
 	}
 }
 
+static void
+nfp_net_set_hash_desc(struct net_device *netdev, struct sk_buff *skb,
+		      struct nfp_net_rx_desc *rxd)
+{
+	struct nfp_net_rx_hash *rx_hash;
+
+	if (!(rxd->rxd.flags & PCIE_DESC_RX_RSS))
+		return;
+
+	rx_hash = (struct nfp_net_rx_hash *)(skb->data - sizeof(*rx_hash));
+
+	nfp_net_set_hash(netdev, skb, get_unaligned_be32(&rx_hash->hash_type),
+			 &rx_hash->hash);
+}
+
+static void *
+nfp_net_parse_meta(struct net_device *netdev, struct sk_buff *skb,
+		   int meta_len)
+{
+	u8 *data = skb->data - meta_len;
+	u32 meta_info;
+
+	meta_info = get_unaligned_be32(data);
+	data += 4;
+
+	while (meta_info) {
+		switch (meta_info & NFP_NET_META_FIELD_MASK) {
+		case NFP_NET_META_HASH:
+			meta_info >>= NFP_NET_META_FIELD_SIZE;
+			nfp_net_set_hash(netdev, skb,
+					 meta_info & NFP_NET_META_FIELD_MASK,
+					 (__be32 *)data);
+			data += 4;
+			break;
+		case NFP_NET_META_MARK:
+			skb->mark = get_unaligned_be32(data);
+			data += 4;
+			break;
+		default:
+			return NULL;
+		}
+
+		meta_info >>= NFP_NET_META_FIELD_SIZE;
+	}
+
+	return data;
+}
+
 /**
  * nfp_net_rx() - receive up to @budget packets on @rx_ring
  * @rx_ring:   RX ring to receive from
@@ -1438,14 +1473,29 @@ static int nfp_net_rx(struct nfp_net_rx_ring *rx_ring, int budget)
 			skb_reserve(skb, nn->rx_offset);
 		skb_put(skb, data_len - meta_len);
 
-		nfp_net_set_hash(nn->netdev, skb, rxd);
-
 		/* Stats update */
 		u64_stats_update_begin(&r_vec->rx_sync);
 		r_vec->rx_pkts++;
 		r_vec->rx_bytes += skb->len;
 		u64_stats_update_end(&r_vec->rx_sync);
 
+		if (nn->fw_ver.major <= 3) {
+			nfp_net_set_hash_desc(nn->netdev, skb, rxd);
+		} else if (meta_len) {
+			void *end;
+
+			end = nfp_net_parse_meta(nn->netdev, skb, meta_len);
+			if (unlikely(end != skb->data)) {
+				u64_stats_update_begin(&r_vec->rx_sync);
+				r_vec->rx_drops++;
+				u64_stats_update_end(&r_vec->rx_sync);
+
+				dev_kfree_skb_any(skb);
+				nn_warn_ratelimit(nn, "invalid RX packet metadata\n");
+				continue;
+			}
+		}
+
 		skb_record_rx_queue(skb, rx_ring->idx);
 		skb->protocol = eth_type_trans(skb, nn->netdev);
 
@@ -2386,6 +2436,31 @@ static struct rtnl_link_stats64 *nfp_net_stat64(struct net_device *netdev,
 	return stats;
 }
 
+static bool nfp_net_ebpf_capable(struct nfp_net *nn)
+{
+	if (nn->cap & NFP_NET_CFG_CTRL_BPF &&
+	    nn_readb(nn, NFP_NET_CFG_BPF_ABI) == NFP_NET_BPF_ABI)
+		return true;
+	return false;
+}
+
+static int
+nfp_net_setup_tc(struct net_device *netdev, u32 handle, __be16 proto,
+		 struct tc_to_netdev *tc)
+{
+	struct nfp_net *nn = netdev_priv(netdev);
+
+	if (TC_H_MAJ(handle) != TC_H_MAJ(TC_H_INGRESS))
+		return -ENOTSUPP;
+	if (proto != htons(ETH_P_ALL))
+		return -ENOTSUPP;
+
+	if (tc->type == TC_SETUP_CLSBPF && nfp_net_ebpf_capable(nn))
+		return nfp_net_bpf_offload(nn, handle, proto, tc->cls_bpf);
+
+	return -EINVAL;
+}
+
 static int nfp_net_set_features(struct net_device *netdev,
 				netdev_features_t features)
 {
@@ -2440,6 +2515,11 @@ static int nfp_net_set_features(struct net_device *netdev,
 			new_ctrl &= ~NFP_NET_CFG_CTRL_GATHER;
 	}
 
+	if (changed & NETIF_F_HW_TC && nn->ctrl & NFP_NET_CFG_CTRL_BPF) {
+		nn_err(nn, "Cannot disable HW TC offload while in use\n");
+		return -EBUSY;
+	}
+
 	nn_dbg(nn, "Feature change 0x%llx -> 0x%llx (changed=0x%llx)\n",
 	       netdev->features, features, changed);
 
@@ -2589,6 +2669,7 @@ static const struct net_device_ops nfp_net_netdev_ops = {
 	.ndo_stop		= nfp_net_netdev_close,
 	.ndo_start_xmit		= nfp_net_tx,
 	.ndo_get_stats64	= nfp_net_stat64,
+	.ndo_setup_tc		= nfp_net_setup_tc,
 	.ndo_tx_timeout		= nfp_net_tx_timeout,
 	.ndo_set_rx_mode	= nfp_net_set_rx_mode,
 	.ndo_change_mtu		= nfp_net_change_mtu,
@@ -2614,7 +2695,7 @@ void nfp_net_info(struct nfp_net *nn)
 		nn->fw_ver.resv, nn->fw_ver.class,
 		nn->fw_ver.major, nn->fw_ver.minor,
 		nn->max_mtu);
-	nn_info(nn, "CAP: %#x %s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s\n",
+	nn_info(nn, "CAP: %#x %s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s\n",
 		nn->cap,
 		nn->cap & NFP_NET_CFG_CTRL_PROMISC  ? "PROMISC "  : "",
 		nn->cap & NFP_NET_CFG_CTRL_L2BC     ? "L2BCFILT " : "",
@@ -2631,7 +2712,8 @@ void nfp_net_info(struct nfp_net *nn)
 		nn->cap & NFP_NET_CFG_CTRL_MSIXAUTO ? "AUTOMASK " : "",
 		nn->cap & NFP_NET_CFG_CTRL_IRQMOD   ? "IRQMOD "   : "",
 		nn->cap & NFP_NET_CFG_CTRL_VXLAN    ? "VXLAN "    : "",
-		nn->cap & NFP_NET_CFG_CTRL_NVGRE    ? "NVGRE "	  : "");
+		nn->cap & NFP_NET_CFG_CTRL_NVGRE    ? "NVGRE "	  : "",
+		nfp_net_ebpf_capable(nn)            ? "BPF "	  : "");
 }
 
 /**
@@ -2674,10 +2756,13 @@ struct nfp_net *nfp_net_netdev_alloc(struct pci_dev *pdev,
 	nn->rxd_cnt = NFP_NET_RX_DESCS_DEFAULT;
 
 	spin_lock_init(&nn->reconfig_lock);
+	spin_lock_init(&nn->rx_filter_lock);
 	spin_lock_init(&nn->link_status_lock);
 
 	setup_timer(&nn->reconfig_timer,
 		    nfp_net_reconfig_timer, (unsigned long)nn);
+	setup_timer(&nn->rx_filter_stats_timer,
+		    nfp_net_filter_stats_timer, (unsigned long)nn);
 
 	return nn;
 }
@@ -2799,6 +2884,9 @@ int nfp_net_netdev_init(struct net_device *netdev)
 
 	netdev->features = netdev->hw_features;
 
+	if (nfp_net_ebpf_capable(nn))
+		netdev->hw_features |= NETIF_F_HW_TC;
+
 	/* Advertise but disable TSO by default. */
 	netdev->features &= ~(NETIF_F_TSO | NETIF_F_TSO6);
 
diff --git a/drivers/net/ethernet/netronome/nfp/nfp_net_ctrl.h b/drivers/net/ethernet/netronome/nfp/nfp_net_ctrl.h
index ad6c4e31cedd..93b10b441acb 100644
--- a/drivers/net/ethernet/netronome/nfp/nfp_net_ctrl.h
+++ b/drivers/net/ethernet/netronome/nfp/nfp_net_ctrl.h
@@ -66,6 +66,13 @@
 #define NFP_NET_LSO_MAX_HDR_SZ		255
 
 /**
+ * Prepend field types
+ */
+#define NFP_NET_META_FIELD_SIZE		4
+#define NFP_NET_META_HASH		1 /* next field carries hash type */
+#define NFP_NET_META_MARK		2
+
+/**
  * Hash type pre-pended when a RSS hash was computed
  */
 #define NFP_NET_RSS_NONE                0
@@ -123,6 +130,7 @@
 #define   NFP_NET_CFG_CTRL_L2SWITCH_LOCAL (0x1 << 23) /* Switch to local */
 #define   NFP_NET_CFG_CTRL_VXLAN	  (0x1 << 24) /* VXLAN tunnel support */
 #define   NFP_NET_CFG_CTRL_NVGRE	  (0x1 << 25) /* NVGRE tunnel support */
+#define   NFP_NET_CFG_CTRL_BPF		  (0x1 << 27) /* BPF offload capable */
 #define NFP_NET_CFG_UPDATE              0x0004
 #define   NFP_NET_CFG_UPDATE_GEN          (0x1 <<  0) /* General update */
 #define   NFP_NET_CFG_UPDATE_RING         (0x1 <<  1) /* Ring config change */
@@ -134,6 +142,7 @@
 #define   NFP_NET_CFG_UPDATE_RESET        (0x1 <<  7) /* Update due to FLR */
 #define   NFP_NET_CFG_UPDATE_IRQMOD       (0x1 <<  8) /* IRQ mod change */
 #define   NFP_NET_CFG_UPDATE_VXLAN	  (0x1 <<  9) /* VXLAN port change */
+#define   NFP_NET_CFG_UPDATE_BPF	  (0x1 << 10) /* BPF program load */
 #define   NFP_NET_CFG_UPDATE_ERR          (0x1 << 31) /* A error occurred */
 #define NFP_NET_CFG_TXRS_ENABLE         0x0008
 #define NFP_NET_CFG_RXRS_ENABLE         0x0010
@@ -196,10 +205,37 @@
 #define NFP_NET_CFG_VXLAN_SZ		  0x0008
 
 /**
- * 64B reserved for future use (0x0080 - 0x00c0)
+ * NFP6000 - BPF section
+ * @NFP_NET_CFG_BPF_ABI:	BPF ABI version
+ * @NFP_NET_CFG_BPF_CAP:	BPF capabilities
+ * @NFP_NET_CFG_BPF_MAX_LEN:	Maximum size of JITed BPF code in bytes
+ * @NFP_NET_CFG_BPF_START:	Offset at which BPF will be loaded
+ * @NFP_NET_CFG_BPF_DONE:	Offset to jump to on exit
+ * @NFP_NET_CFG_BPF_STACK_SZ:	Total size of stack area in 64B chunks
+ * @NFP_NET_CFG_BPF_INL_MTU:	Packet data split offset in 64B chunks
+ * @NFP_NET_CFG_BPF_SIZE:	Size of the JITed BPF code in instructions
+ * @NFP_NET_CFG_BPF_ADDR:	DMA address of the buffer with JITed BPF code
  */
-#define NFP_NET_CFG_RESERVED            0x0080
-#define NFP_NET_CFG_RESERVED_SZ         0x0040
+#define NFP_NET_CFG_BPF_ABI		0x0080
+#define   NFP_NET_BPF_ABI		1
+#define NFP_NET_CFG_BPF_CAP		0x0081
+#define   NFP_NET_BPF_CAP_RELO		(1 << 0) /* seamless reload */
+#define NFP_NET_CFG_BPF_MAX_LEN		0x0082
+#define NFP_NET_CFG_BPF_START		0x0084
+#define NFP_NET_CFG_BPF_DONE		0x0086
+#define NFP_NET_CFG_BPF_STACK_SZ	0x0088
+#define NFP_NET_CFG_BPF_INL_MTU		0x0089
+#define NFP_NET_CFG_BPF_SIZE		0x008e
+#define NFP_NET_CFG_BPF_ADDR		0x0090
+#define   NFP_NET_CFG_BPF_CFG_8CTX	(1 << 0) /* 8ctx mode */
+#define   NFP_NET_CFG_BPF_CFG_MASK	7ULL
+#define   NFP_NET_CFG_BPF_ADDR_MASK	(~NFP_NET_CFG_BPF_CFG_MASK)
+
+/**
+ * 40B reserved for future use (0x0098 - 0x00c0)
+ */
+#define NFP_NET_CFG_RESERVED            0x0098
+#define NFP_NET_CFG_RESERVED_SZ         0x0028
 
 /**
  * RSS configuration (0x0100 - 0x01ac):
@@ -303,6 +339,15 @@
 #define NFP_NET_CFG_STATS_TX_MC_FRAMES  (NFP_NET_CFG_STATS_BASE + 0x80)
 #define NFP_NET_CFG_STATS_TX_BC_FRAMES  (NFP_NET_CFG_STATS_BASE + 0x88)
 
+#define NFP_NET_CFG_STATS_APP0_FRAMES	(NFP_NET_CFG_STATS_BASE + 0x90)
+#define NFP_NET_CFG_STATS_APP0_BYTES	(NFP_NET_CFG_STATS_BASE + 0x98)
+#define NFP_NET_CFG_STATS_APP1_FRAMES	(NFP_NET_CFG_STATS_BASE + 0xa0)
+#define NFP_NET_CFG_STATS_APP1_BYTES	(NFP_NET_CFG_STATS_BASE + 0xa8)
+#define NFP_NET_CFG_STATS_APP2_FRAMES	(NFP_NET_CFG_STATS_BASE + 0xb0)
+#define NFP_NET_CFG_STATS_APP2_BYTES	(NFP_NET_CFG_STATS_BASE + 0xb8)
+#define NFP_NET_CFG_STATS_APP3_FRAMES	(NFP_NET_CFG_STATS_BASE + 0xc0)
+#define NFP_NET_CFG_STATS_APP3_BYTES	(NFP_NET_CFG_STATS_BASE + 0xc8)
+
 /**
  * Per ring stats (0x1000 - 0x1800)
  * options, 64bit per entry
diff --git a/drivers/net/ethernet/netronome/nfp/nfp_net_ethtool.c b/drivers/net/ethernet/netronome/nfp/nfp_net_ethtool.c
index 4c9897220969..3418f2277e9d 100644
--- a/drivers/net/ethernet/netronome/nfp/nfp_net_ethtool.c
+++ b/drivers/net/ethernet/netronome/nfp/nfp_net_ethtool.c
@@ -106,6 +106,18 @@ static const struct _nfp_net_et_stats nfp_net_et_stats[] = {
 	{"dev_tx_pkts", NN_ET_DEV_STAT(NFP_NET_CFG_STATS_TX_FRAMES)},
 	{"dev_tx_mc_pkts", NN_ET_DEV_STAT(NFP_NET_CFG_STATS_TX_MC_FRAMES)},
 	{"dev_tx_bc_pkts", NN_ET_DEV_STAT(NFP_NET_CFG_STATS_TX_BC_FRAMES)},
+
+	{"bpf_pass_pkts", NN_ET_DEV_STAT(NFP_NET_CFG_STATS_APP0_FRAMES)},
+	{"bpf_pass_bytes", NN_ET_DEV_STAT(NFP_NET_CFG_STATS_APP0_BYTES)},
+	/* see comments in outro functions in nfp_bpf_jit.c to find out
+	 * how different BPF modes use app-specific counters
+	 */
+	{"bpf_app1_pkts", NN_ET_DEV_STAT(NFP_NET_CFG_STATS_APP1_FRAMES)},
+	{"bpf_app1_bytes", NN_ET_DEV_STAT(NFP_NET_CFG_STATS_APP1_BYTES)},
+	{"bpf_app2_pkts", NN_ET_DEV_STAT(NFP_NET_CFG_STATS_APP2_FRAMES)},
+	{"bpf_app2_bytes", NN_ET_DEV_STAT(NFP_NET_CFG_STATS_APP2_BYTES)},
+	{"bpf_app3_pkts", NN_ET_DEV_STAT(NFP_NET_CFG_STATS_APP3_FRAMES)},
+	{"bpf_app3_bytes", NN_ET_DEV_STAT(NFP_NET_CFG_STATS_APP3_BYTES)},
 };
 
 #define NN_ET_GLOBAL_STATS_LEN ARRAY_SIZE(nfp_net_et_stats)
diff --git a/drivers/net/ethernet/netronome/nfp/nfp_net_offload.c b/drivers/net/ethernet/netronome/nfp/nfp_net_offload.c
new file mode 100644
index 000000000000..8acfb631a0ea
--- /dev/null
+++ b/drivers/net/ethernet/netronome/nfp/nfp_net_offload.c
@@ -0,0 +1,294 @@
+/*
+ * Copyright (C) 2016 Netronome Systems, Inc.
+ *
+ * This software is dual licensed under the GNU General License Version 2,
+ * June 1991 as shown in the file COPYING in the top-level directory of this
+ * source tree or the BSD 2-Clause License provided below.  You have the
+ * option to license this software under the complete terms of either license.
+ *
+ * The BSD 2-Clause License:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      1. Redistributions of source code must retain the above
+ *         copyright notice, this list of conditions and the following
+ *         disclaimer.
+ *
+ *      2. 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.
+ */
+
+/*
+ * nfp_net_offload.c
+ * Netronome network device driver: TC offload functions for PF and VF
+ */
+
+#include <linux/kernel.h>
+#include <linux/netdevice.h>
+#include <linux/pci.h>
+#include <linux/jiffies.h>
+#include <linux/timer.h>
+#include <linux/list.h>
+
+#include <net/pkt_cls.h>
+#include <net/tc_act/tc_gact.h>
+#include <net/tc_act/tc_mirred.h>
+
+#include "nfp_bpf.h"
+#include "nfp_net_ctrl.h"
+#include "nfp_net.h"
+
+void nfp_net_filter_stats_timer(unsigned long data)
+{
+	struct nfp_net *nn = (void *)data;
+	struct nfp_stat_pair latest;
+
+	spin_lock_bh(&nn->rx_filter_lock);
+
+	if (nn->ctrl & NFP_NET_CFG_CTRL_BPF)
+		mod_timer(&nn->rx_filter_stats_timer,
+			  jiffies + NFP_NET_STAT_POLL_IVL);
+
+	spin_unlock_bh(&nn->rx_filter_lock);
+
+	latest.pkts = nn_readq(nn, NFP_NET_CFG_STATS_APP1_FRAMES);
+	latest.bytes = nn_readq(nn, NFP_NET_CFG_STATS_APP1_BYTES);
+
+	if (latest.pkts != nn->rx_filter.pkts)
+		nn->rx_filter_change = jiffies;
+
+	nn->rx_filter = latest;
+}
+
+static void nfp_net_bpf_stats_reset(struct nfp_net *nn)
+{
+	nn->rx_filter.pkts = nn_readq(nn, NFP_NET_CFG_STATS_APP1_FRAMES);
+	nn->rx_filter.bytes = nn_readq(nn, NFP_NET_CFG_STATS_APP1_BYTES);
+	nn->rx_filter_prev = nn->rx_filter;
+	nn->rx_filter_change = jiffies;
+}
+
+static int
+nfp_net_bpf_stats_update(struct nfp_net *nn, struct tc_cls_bpf_offload *cls_bpf)
+{
+	struct tc_action *a;
+	LIST_HEAD(actions);
+	u64 bytes, pkts;
+
+	pkts = nn->rx_filter.pkts - nn->rx_filter_prev.pkts;
+	bytes = nn->rx_filter.bytes - nn->rx_filter_prev.bytes;
+	bytes -= pkts * ETH_HLEN;
+
+	nn->rx_filter_prev = nn->rx_filter;
+
+	preempt_disable();
+
+	tcf_exts_to_list(cls_bpf->exts, &actions);
+	list_for_each_entry(a, &actions, list)
+		tcf_action_stats_update(a, bytes, pkts, nn->rx_filter_change);
+
+	preempt_enable();
+
+	return 0;
+}
+
+static int
+nfp_net_bpf_get_act(struct nfp_net *nn, struct tc_cls_bpf_offload *cls_bpf)
+{
+	const struct tc_action *a;
+	LIST_HEAD(actions);
+
+	/* TC direct action */
+	if (cls_bpf->exts_integrated) {
+		if (tc_no_actions(cls_bpf->exts))
+			return NN_ACT_DIRECT;
+
+		return -ENOTSUPP;
+	}
+
+	/* TC legacy mode */
+	if (!tc_single_action(cls_bpf->exts))
+		return -ENOTSUPP;
+
+	tcf_exts_to_list(cls_bpf->exts, &actions);
+	list_for_each_entry(a, &actions, list) {
+		if (is_tcf_gact_shot(a))
+			return NN_ACT_TC_DROP;
+
+		if (is_tcf_mirred_redirect(a) &&
+		    tcf_mirred_ifindex(a) == nn->netdev->ifindex)
+			return NN_ACT_TC_REDIR;
+	}
+
+	return -ENOTSUPP;
+}
+
+static int
+nfp_net_bpf_offload_prepare(struct nfp_net *nn,
+			    struct tc_cls_bpf_offload *cls_bpf,
+			    struct nfp_bpf_result *res,
+			    void **code, dma_addr_t *dma_addr, u16 max_instr)
+{
+	unsigned int code_sz = max_instr * sizeof(u64);
+	enum nfp_bpf_action_type act;
+	u16 start_off, done_off;
+	unsigned int max_mtu;
+	int ret;
+
+	if (!IS_ENABLED(CONFIG_BPF_SYSCALL))
+		return -ENOTSUPP;
+
+	ret = nfp_net_bpf_get_act(nn, cls_bpf);
+	if (ret < 0)
+		return ret;
+	act = ret;
+
+	max_mtu = nn_readb(nn, NFP_NET_CFG_BPF_INL_MTU) * 64 - 32;
+	if (max_mtu < nn->netdev->mtu) {
+		nn_info(nn, "BPF offload not supported with MTU larger than HW packet split boundary\n");
+		return -ENOTSUPP;
+	}
+
+	start_off = nn_readw(nn, NFP_NET_CFG_BPF_START);
+	done_off = nn_readw(nn, NFP_NET_CFG_BPF_DONE);
+
+	*code = dma_zalloc_coherent(&nn->pdev->dev, code_sz, dma_addr,
+				    GFP_KERNEL);
+	if (!*code)
+		return -ENOMEM;
+
+	ret = nfp_bpf_jit(cls_bpf->prog, *code, act, start_off, done_off,
+			  max_instr, res);
+	if (ret)
+		goto out;
+
+	return 0;
+
+out:
+	dma_free_coherent(&nn->pdev->dev, code_sz, *code, *dma_addr);
+	return ret;
+}
+
+static void
+nfp_net_bpf_load_and_start(struct nfp_net *nn, u32 tc_flags,
+			   void *code, dma_addr_t dma_addr,
+			   unsigned int code_sz, unsigned int n_instr,
+			   bool dense_mode)
+{
+	u64 bpf_addr = dma_addr;
+	int err;
+
+	nn->bpf_offload_skip_sw = !!(tc_flags & TCA_CLS_FLAGS_SKIP_SW);
+
+	if (dense_mode)
+		bpf_addr |= NFP_NET_CFG_BPF_CFG_8CTX;
+
+	nn_writew(nn, NFP_NET_CFG_BPF_SIZE, n_instr);
+	nn_writeq(nn, NFP_NET_CFG_BPF_ADDR, bpf_addr);
+
+	/* Load up the JITed code */
+	err = nfp_net_reconfig(nn, NFP_NET_CFG_UPDATE_BPF);
+	if (err)
+		nn_err(nn, "FW command error while loading BPF: %d\n", err);
+
+	/* Enable passing packets through BPF function */
+	nn->ctrl |= NFP_NET_CFG_CTRL_BPF;
+	nn_writel(nn, NFP_NET_CFG_CTRL, nn->ctrl);
+	err = nfp_net_reconfig(nn, NFP_NET_CFG_UPDATE_GEN);
+	if (err)
+		nn_err(nn, "FW command error while enabling BPF: %d\n", err);
+
+	dma_free_coherent(&nn->pdev->dev, code_sz, code, dma_addr);
+
+	nfp_net_bpf_stats_reset(nn);
+	mod_timer(&nn->rx_filter_stats_timer, jiffies + NFP_NET_STAT_POLL_IVL);
+}
+
+static int nfp_net_bpf_stop(struct nfp_net *nn)
+{
+	if (!(nn->ctrl & NFP_NET_CFG_CTRL_BPF))
+		return 0;
+
+	spin_lock_bh(&nn->rx_filter_lock);
+	nn->ctrl &= ~NFP_NET_CFG_CTRL_BPF;
+	spin_unlock_bh(&nn->rx_filter_lock);
+	nn_writel(nn, NFP_NET_CFG_CTRL, nn->ctrl);
+
+	del_timer_sync(&nn->rx_filter_stats_timer);
+	nn->bpf_offload_skip_sw = 0;
+
+	return nfp_net_reconfig(nn, NFP_NET_CFG_UPDATE_GEN);
+}
+
+int
+nfp_net_bpf_offload(struct nfp_net *nn, u32 handle, __be16 proto,
+		    struct tc_cls_bpf_offload *cls_bpf)
+{
+	struct nfp_bpf_result res;
+	dma_addr_t dma_addr;
+	u16 max_instr;
+	void *code;
+	int err;
+
+	max_instr = nn_readw(nn, NFP_NET_CFG_BPF_MAX_LEN);
+
+	switch (cls_bpf->command) {
+	case TC_CLSBPF_REPLACE:
+		/* There is nothing stopping us from implementing seamless
+		 * replace but the simple method of loading I adopted in
+		 * the firmware does not handle atomic replace (i.e. we have to
+		 * stop the BPF offload and re-enable it).  Leaking-in a few
+		 * frames which didn't have BPF applied in the hardware should
+		 * be fine if software fallback is available, though.
+		 */
+		if (nn->bpf_offload_skip_sw)
+			return -EBUSY;
+
+		err = nfp_net_bpf_offload_prepare(nn, cls_bpf, &res, &code,
+						  &dma_addr, max_instr);
+		if (err)
+			return err;
+
+		nfp_net_bpf_stop(nn);
+		nfp_net_bpf_load_and_start(nn, cls_bpf->gen_flags, code,
+					   dma_addr, max_instr * sizeof(u64),
+					   res.n_instr, res.dense_mode);
+		return 0;
+
+	case TC_CLSBPF_ADD:
+		if (nn->ctrl & NFP_NET_CFG_CTRL_BPF)
+			return -EBUSY;
+
+		err = nfp_net_bpf_offload_prepare(nn, cls_bpf, &res, &code,
+						  &dma_addr, max_instr);
+		if (err)
+			return err;
+
+		nfp_net_bpf_load_and_start(nn, cls_bpf->gen_flags, code,
+					   dma_addr, max_instr * sizeof(u64),
+					   res.n_instr, res.dense_mode);
+		return 0;
+
+	case TC_CLSBPF_DESTROY:
+		return nfp_net_bpf_stop(nn);
+
+	case TC_CLSBPF_STATS:
+		return nfp_net_bpf_stats_update(nn, cls_bpf);
+
+	default:
+		return -ENOTSUPP;
+	}
+}
diff --git a/drivers/net/ethernet/netronome/nfp/nfp_netvf_main.c b/drivers/net/ethernet/netronome/nfp/nfp_netvf_main.c
index f7062cb648e1..2800bbf65a89 100644
--- a/drivers/net/ethernet/netronome/nfp/nfp_netvf_main.c
+++ b/drivers/net/ethernet/netronome/nfp/nfp_netvf_main.c
@@ -148,7 +148,7 @@ static int nfp_netvf_pci_probe(struct pci_dev *pdev,
 		dev_warn(&pdev->dev, "OBSOLETE Firmware detected - VF isolation not available\n");
 	} else {
 		switch (fw_ver.major) {
-		case 1 ... 3:
+		case 1 ... 4:
 			if (is_nfp3200) {
 				stride = 2;
 				tx_bar_no = NFP_NET_Q0_BAR;
diff --git a/drivers/net/ethernet/nuvoton/w90p910_ether.c b/drivers/net/ethernet/nuvoton/w90p910_ether.c
index 87b7b814778b..712d8bcb7d8c 100644
--- a/drivers/net/ethernet/nuvoton/w90p910_ether.c
+++ b/drivers/net/ethernet/nuvoton/w90p910_ether.c
@@ -751,7 +751,7 @@ static void netdev_rx(struct net_device *dev)
 				dev_err(&pdev->dev, "rx crc err\n");
 				ether->stats.rx_crc_errors++;
 			} else if (status & RXDS_ALIE) {
-				dev_err(&pdev->dev, "rx aligment err\n");
+				dev_err(&pdev->dev, "rx alignment err\n");
 				ether->stats.rx_frame_errors++;
 			} else if (status & RXDS_PTLE) {
 				dev_err(&pdev->dev, "rx longer err\n");
diff --git a/drivers/net/ethernet/qlogic/Kconfig b/drivers/net/ethernet/qlogic/Kconfig
index 6ba48406899e..1e8339a67f6e 100644
--- a/drivers/net/ethernet/qlogic/Kconfig
+++ b/drivers/net/ethernet/qlogic/Kconfig
@@ -88,6 +88,9 @@ config QED
 	---help---
 	  This enables the support for ...
 
+config QED_LL2
+	bool
+
 config QED_SRIOV
 	bool "QLogic QED 25/40/100Gb SR-IOV support"
 	depends on QED && PCI_IOV
diff --git a/drivers/net/ethernet/qlogic/qed/Makefile b/drivers/net/ethernet/qlogic/qed/Makefile
index d1f157e439cf..cda0af7fbc20 100644
--- a/drivers/net/ethernet/qlogic/qed/Makefile
+++ b/drivers/net/ethernet/qlogic/qed/Makefile
@@ -2,5 +2,7 @@ obj-$(CONFIG_QED) := qed.o
 
 qed-y := qed_cxt.o qed_dev.o qed_hw.o qed_init_fw_funcs.o qed_init_ops.o \
 	 qed_int.o qed_main.o qed_mcp.o qed_sp_commands.o qed_spq.o qed_l2.o \
-	 qed_selftest.o qed_dcbx.o
+	 qed_selftest.o qed_dcbx.o qed_debug.o
 qed-$(CONFIG_QED_SRIOV) += qed_sriov.o qed_vf.o
+qed-$(CONFIG_QED_LL2) += qed_ll2.o
+qed-$(CONFIG_INFINIBAND_QEDR) += qed_roce.o
diff --git a/drivers/net/ethernet/qlogic/qed/qed.h b/drivers/net/ethernet/qlogic/qed/qed.h
index 45ab74676573..653bb5735f0c 100644
--- a/drivers/net/ethernet/qlogic/qed/qed.h
+++ b/drivers/net/ethernet/qlogic/qed/qed.h
@@ -23,10 +23,11 @@
 #include <linux/zlib.h>
 #include <linux/hashtable.h>
 #include <linux/qed/qed_if.h>
+#include "qed_debug.h"
 #include "qed_hsi.h"
 
 extern const struct qed_common_ops qed_common_ops_pass;
-#define DRV_MODULE_VERSION "8.7.1.20"
+#define DRV_MODULE_VERSION "8.10.9.20"
 
 #define MAX_HWFNS_PER_DEVICE    (4)
 #define NAME_SIZE 16
@@ -34,6 +35,9 @@ extern const struct qed_common_ops qed_common_ops_pass;
 
 #define QED_WFQ_UNIT	100
 
+#define QED_WID_SIZE            (1024)
+#define QED_PF_DEMS_SIZE        (4)
+
 /* cau states */
 enum qed_coalescing_mode {
 	QED_COAL_MODE_DISABLE,
@@ -42,11 +46,21 @@ enum qed_coalescing_mode {
 
 struct qed_eth_cb_ops;
 struct qed_dev_info;
+union qed_mcp_protocol_stats;
+enum qed_mcp_protocol_type;
 
 /* helpers */
 static inline u32 qed_db_addr(u32 cid, u32 DEMS)
 {
 	u32 db_addr = FIELD_VALUE(DB_LEGACY_ADDR_DEMS, DEMS) |
+		      (cid * QED_PF_DEMS_SIZE);
+
+	return db_addr;
+}
+
+static inline u32 qed_db_addr_vf(u32 cid, u32 DEMS)
+{
+	u32 db_addr = FIELD_VALUE(DB_LEGACY_ADDR_DEMS, DEMS) |
 		      FIELD_VALUE(DB_LEGACY_ADDR_ICID, cid);
 
 	return db_addr;
@@ -69,6 +83,7 @@ struct qed_sb_info;
 struct qed_sb_attn_info;
 struct qed_cxt_mngr;
 struct qed_sb_sp_info;
+struct qed_ll2_info;
 struct qed_mcp_info;
 
 struct qed_rt_data {
@@ -148,13 +163,17 @@ enum QED_RESOURCES {
 	QED_RL,
 	QED_MAC,
 	QED_VLAN,
+	QED_RDMA_CNQ_RAM,
 	QED_ILT,
+	QED_LL2_QUEUE,
+	QED_RDMA_STATS_QUEUE,
 	QED_MAX_RESC,
 };
 
 enum QED_FEATURE {
 	QED_PF_L2_QUE,
 	QED_VF,
+	QED_RDMA_CNQ,
 	QED_MAX_FEATURES,
 };
 
@@ -357,6 +376,9 @@ struct qed_hwfn {
 	struct qed_sb_attn_info		*p_sb_attn;
 
 	/* Protocol related */
+	bool				using_ll2;
+	struct qed_ll2_info		*p_ll2_info;
+	struct qed_rdma_info		*p_rdma_info;
 	struct qed_pf_params		pf_params;
 
 	bool b_rdma_enabled_in_prs;
@@ -393,6 +415,19 @@ struct qed_hwfn {
 	/* Buffer for unzipping firmware data */
 	void				*unzip_buf;
 
+	struct dbg_tools_data		dbg_info;
+
+	/* PWM region specific data */
+	u32				dpi_size;
+	u32				dpi_count;
+
+	/* This is used to calculate the doorbell address */
+	u32 dpi_start_offset;
+
+	/* If one of the following is set then EDPM shouldn't be used */
+	u8 dcbx_no_edpm;
+	u8 db_bar_no_edpm;
+
 	struct qed_simd_fp_handler	simd_proto_handler[64];
 
 #ifdef CONFIG_QED_SRIOV
@@ -402,6 +437,7 @@ struct qed_hwfn {
 #endif
 
 	struct z_stream_s		*stream;
+	struct qed_roce_ll2_info	*ll2;
 };
 
 struct pci_params {
@@ -426,6 +462,21 @@ struct qed_int_params {
 	bool			fp_initialized;
 	u8			fp_msix_base;
 	u8			fp_msix_cnt;
+	u8			rdma_msix_base;
+	u8			rdma_msix_cnt;
+};
+
+struct qed_dbg_feature {
+	struct dentry *dentry;
+	u8 *dump_buf;
+	u32 buf_size;
+	u32 dumped_dwords;
+};
+
+struct qed_dbg_params {
+	struct qed_dbg_feature features[DBG_FEATURE_NUM];
+	u8 engine_for_debug;
+	bool print_data;
 };
 
 struct qed_dev {
@@ -442,6 +493,8 @@ struct qed_dev {
 				 CHIP_REV_IS_A0(dev))
 #define QED_IS_BB_B0(dev)       (QED_IS_BB(dev) && \
 				 CHIP_REV_IS_B0(dev))
+#define QED_IS_AH(dev)  ((dev)->type == QED_DEV_TYPE_AH)
+#define QED_IS_K2(dev)  QED_IS_AH(dev)
 
 #define QED_GET_TYPE(dev)       (QED_IS_BB_A0(dev) ? CHIP_BB_A0 : \
 				 QED_IS_BB_B0(dev) ? CHIP_BB_B0 : CHIP_K2)
@@ -517,7 +570,6 @@ struct qed_dev {
 
 	bool				b_is_vf;
 	u32				drv_type;
-
 	struct qed_eth_stats		*reset_stats;
 	struct qed_fw_data		*fw_data;
 
@@ -542,7 +594,18 @@ struct qed_dev {
 	} protocol_ops;
 	void				*ops_cookie;
 
+	struct qed_dbg_params		dbg_params;
+
+#ifdef CONFIG_QED_LL2
+	struct qed_cb_ll2_info		*ll2;
+	u8				ll2_mac_address[ETH_ALEN];
+#endif
+
 	const struct firmware		*firmware;
+
+	u32 rdma_max_sge;
+	u32 rdma_max_inline;
+	u32 rdma_max_srq_sge;
 };
 
 #define NUM_OF_VFS(dev)         MAX_NUM_VFS_BB
@@ -606,7 +669,9 @@ void qed_link_update(struct qed_hwfn *hwfn);
 u32 qed_unzip_data(struct qed_hwfn *p_hwfn,
 		   u32 input_len, u8 *input_buf,
 		   u32 max_size, u8 *unzip_buf);
-
+void qed_get_protocol_stats(struct qed_dev *cdev,
+			    enum qed_mcp_protocol_type type,
+			    union qed_mcp_protocol_stats *stats);
 int qed_slowpath_irq_req(struct qed_hwfn *hwfn);
 
 #endif /* _QED_H */
diff --git a/drivers/net/ethernet/qlogic/qed/qed_cxt.c b/drivers/net/ethernet/qlogic/qed/qed_cxt.c
index 1c35f376143e..82370a1a59ad 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_cxt.c
+++ b/drivers/net/ethernet/qlogic/qed/qed_cxt.c
@@ -48,7 +48,13 @@
 #define TM_ELEM_SIZE    4
 
 /* ILT constants */
+#if IS_ENABLED(CONFIG_INFINIBAND_QEDR)
+/* For RoCE we configure to 64K to cover for RoCE max tasks 256K purpose. */
+#define ILT_DEFAULT_HW_P_SIZE		4
+#else
 #define ILT_DEFAULT_HW_P_SIZE		3
+#endif
+
 #define ILT_PAGE_IN_BYTES(hw_p_size)	(1U << ((hw_p_size) + 12))
 #define ILT_CFG_REG(cli, reg)	PSWRQ2_REG_ ## cli ## _ ## reg ## _RT_OFFSET
 
@@ -377,9 +383,8 @@ static void qed_cxt_set_proto_cid_count(struct qed_hwfn *p_hwfn,
 	}
 }
 
-u32 qed_cxt_get_proto_cid_count(struct qed_hwfn		*p_hwfn,
-				enum protocol_type	type,
-				u32			*vf_cid)
+u32 qed_cxt_get_proto_cid_count(struct qed_hwfn *p_hwfn,
+				enum protocol_type type, u32 *vf_cid)
 {
 	if (vf_cid)
 		*vf_cid = p_hwfn->p_cxt_mngr->conn_cfg[type].cids_per_vf;
@@ -405,10 +410,10 @@ u32 qed_cxt_get_proto_tid_count(struct qed_hwfn *p_hwfn,
 	return cnt;
 }
 
-static void
-qed_cxt_set_proto_tid_count(struct qed_hwfn *p_hwfn,
-			    enum protocol_type proto,
-			    u8 seg, u8 seg_type, u32 count, bool has_fl)
+static void qed_cxt_set_proto_tid_count(struct qed_hwfn *p_hwfn,
+					enum protocol_type proto,
+					u8 seg,
+					u8 seg_type, u32 count, bool has_fl)
 {
 	struct qed_cxt_mngr *p_mngr = p_hwfn->p_cxt_mngr;
 	struct qed_tid_seg *p_seg = &p_mngr->conn_cfg[proto].tid_seg[seg];
@@ -420,8 +425,7 @@ qed_cxt_set_proto_tid_count(struct qed_hwfn *p_hwfn,
 
 static void qed_ilt_cli_blk_fill(struct qed_ilt_client_cfg *p_cli,
 				 struct qed_ilt_cli_blk *p_blk,
-				 u32 start_line, u32 total_size,
-				 u32 elem_size)
+				 u32 start_line, u32 total_size, u32 elem_size)
 {
 	u32 ilt_size = ILT_PAGE_IN_BYTES(p_cli->p_size.val);
 
@@ -448,8 +452,7 @@ static void qed_ilt_cli_adv_line(struct qed_hwfn *p_hwfn,
 		p_cli->first.val = *p_line;
 
 	p_cli->active = true;
-	*p_line += DIV_ROUND_UP(p_blk->total_size,
-				p_blk->real_size_in_page);
+	*p_line += DIV_ROUND_UP(p_blk->total_size, p_blk->real_size_in_page);
 	p_cli->last.val = *p_line - 1;
 
 	DP_VERBOSE(p_hwfn, QED_MSG_ILT,
@@ -795,10 +798,9 @@ static int qed_cxt_src_t2_alloc(struct qed_hwfn *p_hwfn)
 	p_mngr->t2_num_pages = DIV_ROUND_UP(total_size, psz);
 
 	/* allocate t2 */
-	p_mngr->t2 = kzalloc(p_mngr->t2_num_pages * sizeof(struct qed_dma_mem),
+	p_mngr->t2 = kcalloc(p_mngr->t2_num_pages, sizeof(struct qed_dma_mem),
 			     GFP_KERNEL);
 	if (!p_mngr->t2) {
-		DP_NOTICE(p_hwfn, "Failed to allocate t2 table\n");
 		rc = -ENOMEM;
 		goto t2_fail;
 	}
@@ -926,12 +928,9 @@ static int qed_ilt_blk_alloc(struct qed_hwfn *p_hwfn,
 		void *p_virt;
 		u32 size;
 
-		size = min_t(u32, sz_left,
-			     p_blk->real_size_in_page);
+		size = min_t(u32, sz_left, p_blk->real_size_in_page);
 		p_virt = dma_alloc_coherent(&p_hwfn->cdev->pdev->dev,
-					    size,
-					    &p_phys,
-					    GFP_KERNEL);
+					    size, &p_phys, GFP_KERNEL);
 		if (!p_virt)
 			return -ENOMEM;
 		memset(p_virt, 0, size);
@@ -963,7 +962,6 @@ static int qed_ilt_shadow_alloc(struct qed_hwfn *p_hwfn)
 	p_mngr->ilt_shadow = kcalloc(size, sizeof(struct qed_dma_mem),
 				     GFP_KERNEL);
 	if (!p_mngr->ilt_shadow) {
-		DP_NOTICE(p_hwfn, "Failed to allocate ilt shadow table\n");
 		rc = -ENOMEM;
 		goto ilt_shadow_fail;
 	}
@@ -976,7 +974,7 @@ static int qed_ilt_shadow_alloc(struct qed_hwfn *p_hwfn)
 		for (j = 0; j < ILT_CLI_PF_BLOCKS; j++) {
 			p_blk = &clients[i].pf_blks[j];
 			rc = qed_ilt_blk_alloc(p_hwfn, p_blk, i, 0);
-			if (rc != 0)
+			if (rc)
 				goto ilt_shadow_fail;
 		}
 		for (k = 0; k < p_mngr->vf_count; k++) {
@@ -985,7 +983,7 @@ static int qed_ilt_shadow_alloc(struct qed_hwfn *p_hwfn)
 
 				p_blk = &clients[i].vf_blks[j];
 				rc = qed_ilt_blk_alloc(p_hwfn, p_blk, i, lines);
-				if (rc != 0)
+				if (rc)
 					goto ilt_shadow_fail;
 			}
 		}
@@ -1056,10 +1054,8 @@ int qed_cxt_mngr_alloc(struct qed_hwfn *p_hwfn)
 	u32 i;
 
 	p_mngr = kzalloc(sizeof(*p_mngr), GFP_KERNEL);
-	if (!p_mngr) {
-		DP_NOTICE(p_hwfn, "Failed to allocate `struct qed_cxt_mngr'\n");
+	if (!p_mngr)
 		return -ENOMEM;
-	}
 
 	/* Initialize ILT client registers */
 	clients = p_mngr->clients;
@@ -1111,24 +1107,18 @@ int qed_cxt_tables_alloc(struct qed_hwfn *p_hwfn)
 
 	/* Allocate the ILT shadow table */
 	rc = qed_ilt_shadow_alloc(p_hwfn);
-	if (rc) {
-		DP_NOTICE(p_hwfn, "Failed to allocate ilt memory\n");
+	if (rc)
 		goto tables_alloc_fail;
-	}
 
 	/* Allocate the T2  table */
 	rc = qed_cxt_src_t2_alloc(p_hwfn);
-	if (rc) {
-		DP_NOTICE(p_hwfn, "Failed to allocate T2 memory\n");
+	if (rc)
 		goto tables_alloc_fail;
-	}
 
 	/* Allocate and initialize the acquired cids bitmaps */
 	rc = qed_cid_map_alloc(p_hwfn);
-	if (rc) {
-		DP_NOTICE(p_hwfn, "Failed to allocate cid maps\n");
+	if (rc)
 		goto tables_alloc_fail;
-	}
 
 	return 0;
 
@@ -1672,7 +1662,7 @@ static void qed_tm_init_pf(struct qed_hwfn *p_hwfn)
 		     p_hwfn->rel_pf_id * NUM_TASK_PF_SEGMENTS + i);
 
 		STORE_RT_REG_AGG(p_hwfn, rt_reg, cfg_word);
-		active_seg_mask |= (tm_iids.pf_tids[i] ? (1 << i) : 0);
+		active_seg_mask |= (tm_iids.pf_tids[i] ? BIT(i) : 0);
 
 		tm_offset += tm_iids.pf_tids[i];
 	}
@@ -1702,8 +1692,7 @@ void qed_cxt_hw_init_pf(struct qed_hwfn *p_hwfn)
 }
 
 int qed_cxt_acquire_cid(struct qed_hwfn *p_hwfn,
-			enum protocol_type type,
-			u32 *p_cid)
+			enum protocol_type type, u32 *p_cid)
 {
 	struct qed_cxt_mngr *p_mngr = p_hwfn->p_cxt_mngr;
 	u32 rel_cid;
@@ -1717,8 +1706,7 @@ int qed_cxt_acquire_cid(struct qed_hwfn *p_hwfn,
 				      p_mngr->acquired[type].max_count);
 
 	if (rel_cid >= p_mngr->acquired[type].max_count) {
-		DP_NOTICE(p_hwfn, "no CID available for protocol %d\n",
-			  type);
+		DP_NOTICE(p_hwfn, "no CID available for protocol %d\n", type);
 		return -EINVAL;
 	}
 
@@ -1730,8 +1718,7 @@ int qed_cxt_acquire_cid(struct qed_hwfn *p_hwfn,
 }
 
 static bool qed_cxt_test_cid_acquired(struct qed_hwfn *p_hwfn,
-				      u32 cid,
-				      enum protocol_type *p_type)
+				      u32 cid, enum protocol_type *p_type)
 {
 	struct qed_cxt_mngr *p_mngr = p_hwfn->p_cxt_mngr;
 	struct qed_cid_acquired_map *p_map;
@@ -1763,8 +1750,7 @@ static bool qed_cxt_test_cid_acquired(struct qed_hwfn *p_hwfn,
 	return true;
 }
 
-void qed_cxt_release_cid(struct qed_hwfn *p_hwfn,
-			 u32 cid)
+void qed_cxt_release_cid(struct qed_hwfn *p_hwfn, u32 cid)
 {
 	struct qed_cxt_mngr *p_mngr = p_hwfn->p_cxt_mngr;
 	enum protocol_type type;
@@ -1781,8 +1767,7 @@ void qed_cxt_release_cid(struct qed_hwfn *p_hwfn,
 	__clear_bit(rel_cid, p_mngr->acquired[type].cid_map);
 }
 
-int qed_cxt_get_cid_info(struct qed_hwfn *p_hwfn,
-			 struct qed_cxt_info *p_info)
+int qed_cxt_get_cid_info(struct qed_hwfn *p_hwfn, struct qed_cxt_info *p_info)
 {
 	struct qed_cxt_mngr *p_mngr = p_hwfn->p_cxt_mngr;
 	u32 conn_cxt_size, hw_p_size, cxts_per_p, line;
@@ -1860,6 +1845,8 @@ int qed_cxt_set_pf_params(struct qed_hwfn *p_hwfn)
 	/* Set the number of required CORE connections */
 	u32 core_cids = 1; /* SPQ */
 
+	if (p_hwfn->using_ll2)
+		core_cids += 4;
 	qed_cxt_set_proto_cid_count(p_hwfn, PROTOCOLID_CORE, core_cids, 0);
 
 	switch (p_hwfn->hw_info.personality) {
diff --git a/drivers/net/ethernet/qlogic/qed/qed_cxt.h b/drivers/net/ethernet/qlogic/qed/qed_cxt.h
index c6f6f2e8192d..2b8bdaa77800 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_cxt.h
+++ b/drivers/net/ethernet/qlogic/qed/qed_cxt.h
@@ -170,6 +170,13 @@ int qed_qm_reconf(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt);
  */
 void qed_cxt_release_cid(struct qed_hwfn *p_hwfn,
 			 u32 cid);
+int qed_cxt_dynamic_ilt_alloc(struct qed_hwfn *p_hwfn,
+			      enum qed_cxt_elem_type elem_type, u32 iid);
+u32 qed_cxt_get_proto_tid_count(struct qed_hwfn *p_hwfn,
+				enum protocol_type type);
+u32 qed_cxt_get_proto_cid_start(struct qed_hwfn *p_hwfn,
+				enum protocol_type type);
+int qed_cxt_free_proto_ilt(struct qed_hwfn *p_hwfn, enum protocol_type proto);
 
 #define QED_CTX_WORKING_MEM 0
 #define QED_CTX_FL_MEM 1
diff --git a/drivers/net/ethernet/qlogic/qed/qed_dcbx.c b/drivers/net/ethernet/qlogic/qed/qed_dcbx.c
index 3656d2fd673d..130da1c0490b 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_dcbx.c
+++ b/drivers/net/ethernet/qlogic/qed/qed_dcbx.c
@@ -875,11 +875,8 @@ int qed_dcbx_info_alloc(struct qed_hwfn *p_hwfn)
 	int rc = 0;
 
 	p_hwfn->p_dcbx_info = kzalloc(sizeof(*p_hwfn->p_dcbx_info), GFP_KERNEL);
-	if (!p_hwfn->p_dcbx_info) {
-		DP_NOTICE(p_hwfn,
-			  "Failed to allocate 'struct qed_dcbx_info'\n");
+	if (!p_hwfn->p_dcbx_info)
 		rc = -ENOMEM;
-	}
 
 	return rc;
 }
@@ -1190,10 +1187,8 @@ int qed_dcbx_get_config_params(struct qed_hwfn *p_hwfn,
 	}
 
 	dcbx_info = kzalloc(sizeof(*dcbx_info), GFP_KERNEL);
-	if (!dcbx_info) {
-		DP_ERR(p_hwfn, "Failed to allocate struct qed_dcbx_info\n");
+	if (!dcbx_info)
 		return -ENOMEM;
-	}
 
 	rc = qed_dcbx_query_params(p_hwfn, dcbx_info, QED_DCBX_OPERATIONAL_MIB);
 	if (rc) {
@@ -1227,10 +1222,8 @@ static struct qed_dcbx_get *qed_dcbnl_get_dcbx(struct qed_hwfn *hwfn,
 	struct qed_dcbx_get *dcbx_info;
 
 	dcbx_info = kzalloc(sizeof(*dcbx_info), GFP_KERNEL);
-	if (!dcbx_info) {
-		DP_ERR(hwfn->cdev, "Failed to allocate memory for dcbx_info\n");
+	if (!dcbx_info)
 		return NULL;
-	}
 
 	if (qed_dcbx_query_params(hwfn, dcbx_info, type)) {
 		kfree(dcbx_info);
@@ -1982,6 +1975,7 @@ static int qed_dcbnl_get_ieee_pfc(struct qed_dev *cdev,
 
 	if (!dcbx_info->operational.ieee) {
 		DP_INFO(hwfn, "DCBX is not enabled/operational in IEEE mode\n");
+		kfree(dcbx_info);
 		return -EINVAL;
 	}
 
@@ -2150,17 +2144,19 @@ static int qed_dcbnl_ieee_setets(struct qed_dev *cdev, struct ieee_ets *ets)
 	return rc;
 }
 
-int qed_dcbnl_ieee_peer_getets(struct qed_dev *cdev, struct ieee_ets *ets)
+static int
+qed_dcbnl_ieee_peer_getets(struct qed_dev *cdev, struct ieee_ets *ets)
 {
 	return qed_dcbnl_get_ieee_ets(cdev, ets, true);
 }
 
-int qed_dcbnl_ieee_peer_getpfc(struct qed_dev *cdev, struct ieee_pfc *pfc)
+static int
+qed_dcbnl_ieee_peer_getpfc(struct qed_dev *cdev, struct ieee_pfc *pfc)
 {
 	return qed_dcbnl_get_ieee_pfc(cdev, pfc, true);
 }
 
-int qed_dcbnl_ieee_getapp(struct qed_dev *cdev, struct dcb_app *app)
+static int qed_dcbnl_ieee_getapp(struct qed_dev *cdev, struct dcb_app *app)
 {
 	struct qed_hwfn *hwfn = QED_LEADING_HWFN(cdev);
 	struct qed_dcbx_get *dcbx_info;
@@ -2204,7 +2200,7 @@ int qed_dcbnl_ieee_getapp(struct qed_dev *cdev, struct dcb_app *app)
 	return 0;
 }
 
-int qed_dcbnl_ieee_setapp(struct qed_dev *cdev, struct dcb_app *app)
+static int qed_dcbnl_ieee_setapp(struct qed_dev *cdev, struct dcb_app *app)
 {
 	struct qed_hwfn *hwfn = QED_LEADING_HWFN(cdev);
 	struct qed_dcbx_get *dcbx_info;
diff --git a/drivers/net/ethernet/qlogic/qed/qed_debug.c b/drivers/net/ethernet/qlogic/qed/qed_debug.c
new file mode 100644
index 000000000000..88e7d5bef909
--- /dev/null
+++ b/drivers/net/ethernet/qlogic/qed/qed_debug.c
@@ -0,0 +1,6898 @@
+/* QLogic qed NIC Driver
+ * Copyright (c) 2015 QLogic Corporation
+ *
+ * This software is available 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.
+ */
+
+#include <linux/module.h>
+#include <linux/vmalloc.h>
+#include <linux/crc32.h>
+#include "qed.h"
+#include "qed_hsi.h"
+#include "qed_hw.h"
+#include "qed_mcp.h"
+#include "qed_reg_addr.h"
+
+/* Chip IDs enum */
+enum chip_ids {
+	CHIP_RESERVED,
+	CHIP_BB_B0,
+	CHIP_K2,
+	MAX_CHIP_IDS
+};
+
+/* Memory groups enum */
+enum mem_groups {
+	MEM_GROUP_PXP_MEM,
+	MEM_GROUP_DMAE_MEM,
+	MEM_GROUP_CM_MEM,
+	MEM_GROUP_QM_MEM,
+	MEM_GROUP_TM_MEM,
+	MEM_GROUP_BRB_RAM,
+	MEM_GROUP_BRB_MEM,
+	MEM_GROUP_PRS_MEM,
+	MEM_GROUP_SDM_MEM,
+	MEM_GROUP_PBUF,
+	MEM_GROUP_IOR,
+	MEM_GROUP_RAM,
+	MEM_GROUP_BTB_RAM,
+	MEM_GROUP_RDIF_CTX,
+	MEM_GROUP_TDIF_CTX,
+	MEM_GROUP_CONN_CFC_MEM,
+	MEM_GROUP_TASK_CFC_MEM,
+	MEM_GROUP_CAU_PI,
+	MEM_GROUP_CAU_MEM,
+	MEM_GROUP_PXP_ILT,
+	MEM_GROUP_MULD_MEM,
+	MEM_GROUP_BTB_MEM,
+	MEM_GROUP_IGU_MEM,
+	MEM_GROUP_IGU_MSIX,
+	MEM_GROUP_CAU_SB,
+	MEM_GROUP_BMB_RAM,
+	MEM_GROUP_BMB_MEM,
+	MEM_GROUPS_NUM
+};
+
+/* Memory groups names */
+static const char * const s_mem_group_names[] = {
+	"PXP_MEM",
+	"DMAE_MEM",
+	"CM_MEM",
+	"QM_MEM",
+	"TM_MEM",
+	"BRB_RAM",
+	"BRB_MEM",
+	"PRS_MEM",
+	"SDM_MEM",
+	"PBUF",
+	"IOR",
+	"RAM",
+	"BTB_RAM",
+	"RDIF_CTX",
+	"TDIF_CTX",
+	"CONN_CFC_MEM",
+	"TASK_CFC_MEM",
+	"CAU_PI",
+	"CAU_MEM",
+	"PXP_ILT",
+	"MULD_MEM",
+	"BTB_MEM",
+	"IGU_MEM",
+	"IGU_MSIX",
+	"CAU_SB",
+	"BMB_RAM",
+	"BMB_MEM",
+};
+
+/* Idle check conditions */
+static u32 cond4(const u32 *r, const u32 *imm)
+{
+	return ((r[0] & imm[0]) != imm[1]) && ((r[1] & imm[2]) != imm[3]);
+}
+
+static u32 cond6(const u32 *r, const u32 *imm)
+{
+	return ((r[0] >> imm[0]) & imm[1]) != imm[2];
+}
+
+static u32 cond5(const u32 *r, const u32 *imm)
+{
+	return (r[0] & imm[0]) != imm[1];
+}
+
+static u32 cond8(const u32 *r, const u32 *imm)
+{
+	return ((r[0] & imm[0]) >> imm[1]) !=
+	    (((r[0] & imm[2]) >> imm[3]) | ((r[1] & imm[4]) << imm[5]));
+}
+
+static u32 cond9(const u32 *r, const u32 *imm)
+{
+	return ((r[0] & imm[0]) >> imm[1]) != (r[0] & imm[2]);
+}
+
+static u32 cond1(const u32 *r, const u32 *imm)
+{
+	return (r[0] & ~imm[0]) != imm[1];
+}
+
+static u32 cond0(const u32 *r, const u32 *imm)
+{
+	return r[0] != imm[0];
+}
+
+static u32 cond10(const u32 *r, const u32 *imm)
+{
+	return r[0] != r[1] && r[2] == imm[0];
+}
+
+static u32 cond11(const u32 *r, const u32 *imm)
+{
+	return r[0] != r[1] && r[2] > imm[0];
+}
+
+static u32 cond3(const u32 *r, const u32 *imm)
+{
+	return r[0] != r[1];
+}
+
+static u32 cond12(const u32 *r, const u32 *imm)
+{
+	return r[0] & imm[0];
+}
+
+static u32 cond7(const u32 *r, const u32 *imm)
+{
+	return r[0] < (r[1] - imm[0]);
+}
+
+static u32 cond2(const u32 *r, const u32 *imm)
+{
+	return r[0] > imm[0];
+}
+
+/* Array of Idle Check conditions */
+static u32(*cond_arr[]) (const u32 *r, const u32 *imm) = {
+	cond0,
+	cond1,
+	cond2,
+	cond3,
+	cond4,
+	cond5,
+	cond6,
+	cond7,
+	cond8,
+	cond9,
+	cond10,
+	cond11,
+	cond12,
+};
+
+/******************************* Data Types **********************************/
+
+enum platform_ids {
+	PLATFORM_ASIC,
+	PLATFORM_RESERVED,
+	PLATFORM_RESERVED2,
+	PLATFORM_RESERVED3,
+	MAX_PLATFORM_IDS
+};
+
+struct dbg_array {
+	const u32 *ptr;
+	u32 size_in_dwords;
+};
+
+/* Chip constant definitions */
+struct chip_defs {
+	const char *name;
+	struct {
+		u8 num_ports;
+		u8 num_pfs;
+	} per_platform[MAX_PLATFORM_IDS];
+};
+
+/* Platform constant definitions */
+struct platform_defs {
+	const char *name;
+	u32 delay_factor;
+};
+
+/* Storm constant definitions */
+struct storm_defs {
+	char letter;
+	enum block_id block_id;
+	enum dbg_bus_clients dbg_client_id[MAX_CHIP_IDS];
+	bool has_vfc;
+	u32 sem_fast_mem_addr;
+	u32 sem_frame_mode_addr;
+	u32 sem_slow_enable_addr;
+	u32 sem_slow_mode_addr;
+	u32 sem_slow_mode1_conf_addr;
+	u32 sem_sync_dbg_empty_addr;
+	u32 sem_slow_dbg_empty_addr;
+	u32 cm_ctx_wr_addr;
+	u32 cm_conn_ag_ctx_lid_size; /* In quad-regs */
+	u32 cm_conn_ag_ctx_rd_addr;
+	u32 cm_conn_st_ctx_lid_size; /* In quad-regs */
+	u32 cm_conn_st_ctx_rd_addr;
+	u32 cm_task_ag_ctx_lid_size; /* In quad-regs */
+	u32 cm_task_ag_ctx_rd_addr;
+	u32 cm_task_st_ctx_lid_size; /* In quad-regs */
+	u32 cm_task_st_ctx_rd_addr;
+};
+
+/* Block constant definitions */
+struct block_defs {
+	const char *name;
+	bool has_dbg_bus[MAX_CHIP_IDS];
+	bool associated_to_storm;
+	u32 storm_id; /* Valid only if associated_to_storm is true */
+	enum dbg_bus_clients dbg_client_id[MAX_CHIP_IDS];
+	u32 dbg_select_addr;
+	u32 dbg_cycle_enable_addr;
+	u32 dbg_shift_addr;
+	u32 dbg_force_valid_addr;
+	u32 dbg_force_frame_addr;
+	bool has_reset_bit;
+	bool unreset; /* If true, the block is taken out of reset before dump */
+	enum dbg_reset_regs reset_reg;
+	u8 reset_bit_offset; /* Bit offset in reset register */
+};
+
+/* Reset register definitions */
+struct reset_reg_defs {
+	u32 addr;
+	u32 unreset_val;
+	bool exists[MAX_CHIP_IDS];
+};
+
+struct grc_param_defs {
+	u32 default_val[MAX_CHIP_IDS];
+	u32 min;
+	u32 max;
+	bool is_preset;
+	u32 exclude_all_preset_val;
+	u32 crash_preset_val;
+};
+
+struct rss_mem_defs {
+	const char *mem_name;
+	const char *type_name;
+	u32 addr; /* In 128b units */
+	u32 num_entries[MAX_CHIP_IDS];
+	u32 entry_width[MAX_CHIP_IDS]; /* In bits */
+};
+
+struct vfc_ram_defs {
+	const char *mem_name;
+	const char *type_name;
+	u32 base_row;
+	u32 num_rows;
+};
+
+struct big_ram_defs {
+	const char *instance_name;
+	enum mem_groups mem_group_id;
+	enum mem_groups ram_mem_group_id;
+	enum dbg_grc_params grc_param;
+	u32 addr_reg_addr;
+	u32 data_reg_addr;
+	u32 num_of_blocks[MAX_CHIP_IDS];
+};
+
+struct phy_defs {
+	const char *phy_name;
+	u32 base_addr;
+	u32 tbus_addr_lo_addr;
+	u32 tbus_addr_hi_addr;
+	u32 tbus_data_lo_addr;
+	u32 tbus_data_hi_addr;
+};
+
+/******************************** Constants **********************************/
+
+#define MAX_LCIDS			320
+#define MAX_LTIDS			320
+#define NUM_IOR_SETS			2
+#define IORS_PER_SET			176
+#define IOR_SET_OFFSET(set_id)		((set_id) * 256)
+#define BYTES_IN_DWORD			sizeof(u32)
+
+/* In the macros below, size and offset are specified in bits */
+#define CEIL_DWORDS(size)		DIV_ROUND_UP(size, 32)
+#define FIELD_BIT_OFFSET(type, field)	type ## _ ## field ## _ ## OFFSET
+#define FIELD_BIT_SIZE(type, field)	type ## _ ## field ## _ ## SIZE
+#define FIELD_DWORD_OFFSET(type, field) \
+	 (int)(FIELD_BIT_OFFSET(type, field) / 32)
+#define FIELD_DWORD_SHIFT(type, field)	(FIELD_BIT_OFFSET(type, field) % 32)
+#define FIELD_BIT_MASK(type, field) \
+	(((1 << FIELD_BIT_SIZE(type, field)) - 1) << \
+	 FIELD_DWORD_SHIFT(type, field))
+#define SET_VAR_FIELD(var, type, field, val) \
+	do { \
+		var[FIELD_DWORD_OFFSET(type, field)] &=	\
+		(~FIELD_BIT_MASK(type, field));	\
+		var[FIELD_DWORD_OFFSET(type, field)] |= \
+		(val) << FIELD_DWORD_SHIFT(type, field); \
+	} while (0)
+#define ARR_REG_WR(dev, ptt, addr, arr, arr_size) \
+	do { \
+		for (i = 0; i < (arr_size); i++) \
+			qed_wr(dev, ptt, addr,	(arr)[i]); \
+	} while (0)
+#define ARR_REG_RD(dev, ptt, addr, arr, arr_size) \
+	do { \
+		for (i = 0; i < (arr_size); i++) \
+			(arr)[i] = qed_rd(dev, ptt, addr); \
+	} while (0)
+
+#define DWORDS_TO_BYTES(dwords)		((dwords) * BYTES_IN_DWORD)
+#define BYTES_TO_DWORDS(bytes)		((bytes) / BYTES_IN_DWORD)
+#define RAM_LINES_TO_DWORDS(lines)	((lines) * 2)
+#define RAM_LINES_TO_BYTES(lines) \
+	DWORDS_TO_BYTES(RAM_LINES_TO_DWORDS(lines))
+#define REG_DUMP_LEN_SHIFT		24
+#define MEM_DUMP_ENTRY_SIZE_DWORDS \
+	BYTES_TO_DWORDS(sizeof(struct dbg_dump_mem))
+#define IDLE_CHK_RULE_SIZE_DWORDS \
+	BYTES_TO_DWORDS(sizeof(struct dbg_idle_chk_rule))
+#define IDLE_CHK_RESULT_HDR_DWORDS \
+	BYTES_TO_DWORDS(sizeof(struct dbg_idle_chk_result_hdr))
+#define IDLE_CHK_RESULT_REG_HDR_DWORDS \
+	BYTES_TO_DWORDS(sizeof(struct dbg_idle_chk_result_reg_hdr))
+#define IDLE_CHK_MAX_ENTRIES_SIZE	32
+
+/* The sizes and offsets below are specified in bits */
+#define VFC_CAM_CMD_STRUCT_SIZE		64
+#define VFC_CAM_CMD_ROW_OFFSET		48
+#define VFC_CAM_CMD_ROW_SIZE		9
+#define VFC_CAM_ADDR_STRUCT_SIZE	16
+#define VFC_CAM_ADDR_OP_OFFSET		0
+#define VFC_CAM_ADDR_OP_SIZE		4
+#define VFC_CAM_RESP_STRUCT_SIZE	256
+#define VFC_RAM_ADDR_STRUCT_SIZE	16
+#define VFC_RAM_ADDR_OP_OFFSET		0
+#define VFC_RAM_ADDR_OP_SIZE		2
+#define VFC_RAM_ADDR_ROW_OFFSET		2
+#define VFC_RAM_ADDR_ROW_SIZE		10
+#define VFC_RAM_RESP_STRUCT_SIZE	256
+#define VFC_CAM_CMD_DWORDS		CEIL_DWORDS(VFC_CAM_CMD_STRUCT_SIZE)
+#define VFC_CAM_ADDR_DWORDS		CEIL_DWORDS(VFC_CAM_ADDR_STRUCT_SIZE)
+#define VFC_CAM_RESP_DWORDS		CEIL_DWORDS(VFC_CAM_RESP_STRUCT_SIZE)
+#define VFC_RAM_CMD_DWORDS		VFC_CAM_CMD_DWORDS
+#define VFC_RAM_ADDR_DWORDS		CEIL_DWORDS(VFC_RAM_ADDR_STRUCT_SIZE)
+#define VFC_RAM_RESP_DWORDS		CEIL_DWORDS(VFC_RAM_RESP_STRUCT_SIZE)
+#define NUM_VFC_RAM_TYPES		4
+#define VFC_CAM_NUM_ROWS		512
+#define VFC_OPCODE_CAM_RD		14
+#define VFC_OPCODE_RAM_RD		0
+#define NUM_RSS_MEM_TYPES		5
+#define NUM_BIG_RAM_TYPES		3
+#define BIG_RAM_BLOCK_SIZE_BYTES	128
+#define BIG_RAM_BLOCK_SIZE_DWORDS \
+	BYTES_TO_DWORDS(BIG_RAM_BLOCK_SIZE_BYTES)
+#define NUM_PHY_TBUS_ADDRESSES		2048
+#define PHY_DUMP_SIZE_DWORDS		(NUM_PHY_TBUS_ADDRESSES / 2)
+#define RESET_REG_UNRESET_OFFSET	4
+#define STALL_DELAY_MS			500
+#define STATIC_DEBUG_LINE_DWORDS	9
+#define NUM_DBG_BUS_LINES		256
+#define NUM_COMMON_GLOBAL_PARAMS	8
+#define FW_IMG_MAIN			1
+#define REG_FIFO_DEPTH_ELEMENTS		32
+#define REG_FIFO_ELEMENT_DWORDS		2
+#define REG_FIFO_DEPTH_DWORDS \
+	(REG_FIFO_ELEMENT_DWORDS * REG_FIFO_DEPTH_ELEMENTS)
+#define IGU_FIFO_DEPTH_ELEMENTS		64
+#define IGU_FIFO_ELEMENT_DWORDS		4
+#define IGU_FIFO_DEPTH_DWORDS \
+	(IGU_FIFO_ELEMENT_DWORDS * IGU_FIFO_DEPTH_ELEMENTS)
+#define PROTECTION_OVERRIDE_DEPTH_ELEMENTS	20
+#define PROTECTION_OVERRIDE_ELEMENT_DWORDS	2
+#define PROTECTION_OVERRIDE_DEPTH_DWORDS \
+	(PROTECTION_OVERRIDE_DEPTH_ELEMENTS * \
+	 PROTECTION_OVERRIDE_ELEMENT_DWORDS)
+#define MCP_SPAD_TRACE_OFFSIZE_ADDR \
+	(MCP_REG_SCRATCH + \
+	 offsetof(struct static_init, sections[SPAD_SECTION_TRACE]))
+#define MCP_TRACE_META_IMAGE_SIGNATURE  0x669955aa
+#define EMPTY_FW_VERSION_STR		"???_???_???_???"
+#define EMPTY_FW_IMAGE_STR		"???????????????"
+
+/***************************** Constant Arrays *******************************/
+
+/* Debug arrays */
+static struct dbg_array s_dbg_arrays[MAX_BIN_DBG_BUFFER_TYPE] = { {0} };
+
+/* Chip constant definitions array */
+static struct chip_defs s_chip_defs[MAX_CHIP_IDS] = {
+	{ "reserved", { {0, 0}, {0, 0}, {0, 0}, {0, 0} } },
+	{ "bb_b0",
+	  { {MAX_NUM_PORTS_BB, MAX_NUM_PFS_BB}, {0, 0}, {0, 0}, {0, 0} } },
+	{ "k2", { {MAX_NUM_PORTS_K2, MAX_NUM_PFS_K2}, {0, 0}, {0, 0}, {0, 0} } }
+};
+
+/* Storm constant definitions array */
+static struct storm_defs s_storm_defs[] = {
+	/* Tstorm */
+	{'T', BLOCK_TSEM,
+	 {DBG_BUS_CLIENT_RBCT, DBG_BUS_CLIENT_RBCT,
+	  DBG_BUS_CLIENT_RBCT}, true,
+	 TSEM_REG_FAST_MEMORY,
+	 TSEM_REG_DBG_FRAME_MODE, TSEM_REG_SLOW_DBG_ACTIVE,
+	 TSEM_REG_SLOW_DBG_MODE, TSEM_REG_DBG_MODE1_CFG,
+	 TSEM_REG_SYNC_DBG_EMPTY, TSEM_REG_SLOW_DBG_EMPTY,
+	 TCM_REG_CTX_RBC_ACCS,
+	 4, TCM_REG_AGG_CON_CTX,
+	 16, TCM_REG_SM_CON_CTX,
+	 2, TCM_REG_AGG_TASK_CTX,
+	 4, TCM_REG_SM_TASK_CTX},
+	/* Mstorm */
+	{'M', BLOCK_MSEM,
+	 {DBG_BUS_CLIENT_RBCT, DBG_BUS_CLIENT_RBCT,
+	  DBG_BUS_CLIENT_RBCM}, false,
+	 MSEM_REG_FAST_MEMORY,
+	 MSEM_REG_DBG_FRAME_MODE, MSEM_REG_SLOW_DBG_ACTIVE,
+	 MSEM_REG_SLOW_DBG_MODE, MSEM_REG_DBG_MODE1_CFG,
+	 MSEM_REG_SYNC_DBG_EMPTY, MSEM_REG_SLOW_DBG_EMPTY,
+	 MCM_REG_CTX_RBC_ACCS,
+	 1, MCM_REG_AGG_CON_CTX,
+	 10, MCM_REG_SM_CON_CTX,
+	 2, MCM_REG_AGG_TASK_CTX,
+	 7, MCM_REG_SM_TASK_CTX},
+	/* Ustorm */
+	{'U', BLOCK_USEM,
+	 {DBG_BUS_CLIENT_RBCU, DBG_BUS_CLIENT_RBCU,
+	  DBG_BUS_CLIENT_RBCU}, false,
+	 USEM_REG_FAST_MEMORY,
+	 USEM_REG_DBG_FRAME_MODE, USEM_REG_SLOW_DBG_ACTIVE,
+	 USEM_REG_SLOW_DBG_MODE, USEM_REG_DBG_MODE1_CFG,
+	 USEM_REG_SYNC_DBG_EMPTY, USEM_REG_SLOW_DBG_EMPTY,
+	 UCM_REG_CTX_RBC_ACCS,
+	 2, UCM_REG_AGG_CON_CTX,
+	 13, UCM_REG_SM_CON_CTX,
+	 3, UCM_REG_AGG_TASK_CTX,
+	 3, UCM_REG_SM_TASK_CTX},
+	/* Xstorm */
+	{'X', BLOCK_XSEM,
+	 {DBG_BUS_CLIENT_RBCX, DBG_BUS_CLIENT_RBCX,
+	  DBG_BUS_CLIENT_RBCX}, false,
+	 XSEM_REG_FAST_MEMORY,
+	 XSEM_REG_DBG_FRAME_MODE, XSEM_REG_SLOW_DBG_ACTIVE,
+	 XSEM_REG_SLOW_DBG_MODE, XSEM_REG_DBG_MODE1_CFG,
+	 XSEM_REG_SYNC_DBG_EMPTY, XSEM_REG_SLOW_DBG_EMPTY,
+	 XCM_REG_CTX_RBC_ACCS,
+	 9, XCM_REG_AGG_CON_CTX,
+	 15, XCM_REG_SM_CON_CTX,
+	 0, 0,
+	 0, 0},
+	/* Ystorm */
+	{'Y', BLOCK_YSEM,
+	 {DBG_BUS_CLIENT_RBCX, DBG_BUS_CLIENT_RBCX,
+	  DBG_BUS_CLIENT_RBCY}, false,
+	 YSEM_REG_FAST_MEMORY,
+	 YSEM_REG_DBG_FRAME_MODE, YSEM_REG_SLOW_DBG_ACTIVE,
+	 YSEM_REG_SLOW_DBG_MODE, YSEM_REG_DBG_MODE1_CFG,
+	 YSEM_REG_SYNC_DBG_EMPTY, TSEM_REG_SLOW_DBG_EMPTY,
+	 YCM_REG_CTX_RBC_ACCS,
+	 2, YCM_REG_AGG_CON_CTX,
+	 3, YCM_REG_SM_CON_CTX,
+	 2, YCM_REG_AGG_TASK_CTX,
+	 12, YCM_REG_SM_TASK_CTX},
+	/* Pstorm */
+	{'P', BLOCK_PSEM,
+	 {DBG_BUS_CLIENT_RBCS, DBG_BUS_CLIENT_RBCS,
+	  DBG_BUS_CLIENT_RBCS}, true,
+	 PSEM_REG_FAST_MEMORY,
+	 PSEM_REG_DBG_FRAME_MODE, PSEM_REG_SLOW_DBG_ACTIVE,
+	 PSEM_REG_SLOW_DBG_MODE, PSEM_REG_DBG_MODE1_CFG,
+	 PSEM_REG_SYNC_DBG_EMPTY, PSEM_REG_SLOW_DBG_EMPTY,
+	 PCM_REG_CTX_RBC_ACCS,
+	 0, 0,
+	 10, PCM_REG_SM_CON_CTX,
+	 0, 0,
+	 0, 0}
+};
+
+/* Block definitions array */
+static struct block_defs block_grc_defs = {
+	"grc", {true, true, true}, false, 0,
+	{DBG_BUS_CLIENT_RBCN, DBG_BUS_CLIENT_RBCN, DBG_BUS_CLIENT_RBCN},
+	GRC_REG_DBG_SELECT, GRC_REG_DBG_DWORD_ENABLE,
+	GRC_REG_DBG_SHIFT, GRC_REG_DBG_FORCE_VALID,
+	GRC_REG_DBG_FORCE_FRAME,
+	true, false, DBG_RESET_REG_MISC_PL_UA, 1
+};
+
+static struct block_defs block_miscs_defs = {
+	"miscs", {false, false, false}, false, 0,
+	{MAX_DBG_BUS_CLIENTS, MAX_DBG_BUS_CLIENTS, MAX_DBG_BUS_CLIENTS},
+	0, 0, 0, 0, 0,
+	false, false, MAX_DBG_RESET_REGS, 0
+};
+
+static struct block_defs block_misc_defs = {
+	"misc", {false, false, false}, false, 0,
+	{MAX_DBG_BUS_CLIENTS, MAX_DBG_BUS_CLIENTS, MAX_DBG_BUS_CLIENTS},
+	0, 0, 0, 0, 0,
+	false, false, MAX_DBG_RESET_REGS, 0
+};
+
+static struct block_defs block_dbu_defs = {
+	"dbu", {false, false, false}, false, 0,
+	{MAX_DBG_BUS_CLIENTS, MAX_DBG_BUS_CLIENTS, MAX_DBG_BUS_CLIENTS},
+	0, 0, 0, 0, 0,
+	false, false, MAX_DBG_RESET_REGS, 0
+};
+
+static struct block_defs block_pglue_b_defs = {
+	"pglue_b", {true, true, true}, false, 0,
+	{DBG_BUS_CLIENT_RBCH, DBG_BUS_CLIENT_RBCH, DBG_BUS_CLIENT_RBCH},
+	PGLUE_B_REG_DBG_SELECT, PGLUE_B_REG_DBG_DWORD_ENABLE,
+	PGLUE_B_REG_DBG_SHIFT, PGLUE_B_REG_DBG_FORCE_VALID,
+	PGLUE_B_REG_DBG_FORCE_FRAME,
+	true, false, DBG_RESET_REG_MISCS_PL_HV, 1
+};
+
+static struct block_defs block_cnig_defs = {
+	"cnig", {false, false, true}, false, 0,
+	{MAX_DBG_BUS_CLIENTS, MAX_DBG_BUS_CLIENTS, DBG_BUS_CLIENT_RBCW},
+	CNIG_REG_DBG_SELECT_K2, CNIG_REG_DBG_DWORD_ENABLE_K2,
+	CNIG_REG_DBG_SHIFT_K2, CNIG_REG_DBG_FORCE_VALID_K2,
+	CNIG_REG_DBG_FORCE_FRAME_K2,
+	true, false, DBG_RESET_REG_MISCS_PL_HV, 0
+};
+
+static struct block_defs block_cpmu_defs = {
+	"cpmu", {false, false, false}, false, 0,
+	{MAX_DBG_BUS_CLIENTS, MAX_DBG_BUS_CLIENTS, MAX_DBG_BUS_CLIENTS},
+	0, 0, 0, 0, 0,
+	true, false, DBG_RESET_REG_MISCS_PL_HV, 8
+};
+
+static struct block_defs block_ncsi_defs = {
+	"ncsi", {true, true, true}, false, 0,
+	{DBG_BUS_CLIENT_RBCZ, DBG_BUS_CLIENT_RBCZ, DBG_BUS_CLIENT_RBCZ},
+	NCSI_REG_DBG_SELECT, NCSI_REG_DBG_DWORD_ENABLE,
+	NCSI_REG_DBG_SHIFT, NCSI_REG_DBG_FORCE_VALID,
+	NCSI_REG_DBG_FORCE_FRAME,
+	true, false, DBG_RESET_REG_MISCS_PL_HV, 5
+};
+
+static struct block_defs block_opte_defs = {
+	"opte", {false, false, false}, false, 0,
+	{MAX_DBG_BUS_CLIENTS, MAX_DBG_BUS_CLIENTS, MAX_DBG_BUS_CLIENTS},
+	0, 0, 0, 0, 0,
+	true, false, DBG_RESET_REG_MISCS_PL_HV, 4
+};
+
+static struct block_defs block_bmb_defs = {
+	"bmb", {true, true, true}, false, 0,
+	{DBG_BUS_CLIENT_RBCZ, DBG_BUS_CLIENT_RBCZ, DBG_BUS_CLIENT_RBCB},
+	BMB_REG_DBG_SELECT, BMB_REG_DBG_DWORD_ENABLE,
+	BMB_REG_DBG_SHIFT, BMB_REG_DBG_FORCE_VALID,
+	BMB_REG_DBG_FORCE_FRAME,
+	true, false, DBG_RESET_REG_MISCS_PL_UA, 7
+};
+
+static struct block_defs block_pcie_defs = {
+	"pcie", {false, false, true}, false, 0,
+	{MAX_DBG_BUS_CLIENTS, MAX_DBG_BUS_CLIENTS, DBG_BUS_CLIENT_RBCH},
+	PCIE_REG_DBG_COMMON_SELECT, PCIE_REG_DBG_COMMON_DWORD_ENABLE,
+	PCIE_REG_DBG_COMMON_SHIFT, PCIE_REG_DBG_COMMON_FORCE_VALID,
+	PCIE_REG_DBG_COMMON_FORCE_FRAME,
+	false, false, MAX_DBG_RESET_REGS, 0
+};
+
+static struct block_defs block_mcp_defs = {
+	"mcp", {false, false, false}, false, 0,
+	{MAX_DBG_BUS_CLIENTS, MAX_DBG_BUS_CLIENTS, MAX_DBG_BUS_CLIENTS},
+	0, 0, 0, 0, 0,
+	false, false, MAX_DBG_RESET_REGS, 0
+};
+
+static struct block_defs block_mcp2_defs = {
+	"mcp2", {true, true, true}, false, 0,
+	{DBG_BUS_CLIENT_RBCZ, DBG_BUS_CLIENT_RBCZ, DBG_BUS_CLIENT_RBCZ},
+	MCP2_REG_DBG_SELECT, MCP2_REG_DBG_DWORD_ENABLE,
+	MCP2_REG_DBG_SHIFT, MCP2_REG_DBG_FORCE_VALID,
+	MCP2_REG_DBG_FORCE_FRAME,
+	false, false, MAX_DBG_RESET_REGS, 0
+};
+
+static struct block_defs block_pswhst_defs = {
+	"pswhst", {true, true, true}, false, 0,
+	{DBG_BUS_CLIENT_RBCP, DBG_BUS_CLIENT_RBCP, DBG_BUS_CLIENT_RBCP},
+	PSWHST_REG_DBG_SELECT, PSWHST_REG_DBG_DWORD_ENABLE,
+	PSWHST_REG_DBG_SHIFT, PSWHST_REG_DBG_FORCE_VALID,
+	PSWHST_REG_DBG_FORCE_FRAME,
+	true, false, DBG_RESET_REG_MISC_PL_HV, 0
+};
+
+static struct block_defs block_pswhst2_defs = {
+	"pswhst2", {true, true, true}, false, 0,
+	{DBG_BUS_CLIENT_RBCP, DBG_BUS_CLIENT_RBCP, DBG_BUS_CLIENT_RBCP},
+	PSWHST2_REG_DBG_SELECT, PSWHST2_REG_DBG_DWORD_ENABLE,
+	PSWHST2_REG_DBG_SHIFT, PSWHST2_REG_DBG_FORCE_VALID,
+	PSWHST2_REG_DBG_FORCE_FRAME,
+	true, false, DBG_RESET_REG_MISC_PL_HV, 0
+};
+
+static struct block_defs block_pswrd_defs = {
+	"pswrd", {true, true, true}, false, 0,
+	{DBG_BUS_CLIENT_RBCP, DBG_BUS_CLIENT_RBCP, DBG_BUS_CLIENT_RBCP},
+	PSWRD_REG_DBG_SELECT, PSWRD_REG_DBG_DWORD_ENABLE,
+	PSWRD_REG_DBG_SHIFT, PSWRD_REG_DBG_FORCE_VALID,
+	PSWRD_REG_DBG_FORCE_FRAME,
+	true, false, DBG_RESET_REG_MISC_PL_HV, 2
+};
+
+static struct block_defs block_pswrd2_defs = {
+	"pswrd2", {true, true, true}, false, 0,
+	{DBG_BUS_CLIENT_RBCP, DBG_BUS_CLIENT_RBCP, DBG_BUS_CLIENT_RBCP},
+	PSWRD2_REG_DBG_SELECT, PSWRD2_REG_DBG_DWORD_ENABLE,
+	PSWRD2_REG_DBG_SHIFT, PSWRD2_REG_DBG_FORCE_VALID,
+	PSWRD2_REG_DBG_FORCE_FRAME,
+	true, false, DBG_RESET_REG_MISC_PL_HV, 2
+};
+
+static struct block_defs block_pswwr_defs = {
+	"pswwr", {true, true, true}, false, 0,
+	{DBG_BUS_CLIENT_RBCP, DBG_BUS_CLIENT_RBCP, DBG_BUS_CLIENT_RBCP},
+	PSWWR_REG_DBG_SELECT, PSWWR_REG_DBG_DWORD_ENABLE,
+	PSWWR_REG_DBG_SHIFT, PSWWR_REG_DBG_FORCE_VALID,
+	PSWWR_REG_DBG_FORCE_FRAME,
+	true, false, DBG_RESET_REG_MISC_PL_HV, 3
+};
+
+static struct block_defs block_pswwr2_defs = {
+	"pswwr2", {false, false, false}, false, 0,
+	{MAX_DBG_BUS_CLIENTS, MAX_DBG_BUS_CLIENTS, MAX_DBG_BUS_CLIENTS},
+	0, 0, 0, 0, 0,
+	true, false, DBG_RESET_REG_MISC_PL_HV, 3
+};
+
+static struct block_defs block_pswrq_defs = {
+	"pswrq", {true, true, true}, false, 0,
+	{DBG_BUS_CLIENT_RBCP, DBG_BUS_CLIENT_RBCP, DBG_BUS_CLIENT_RBCP},
+	PSWRQ_REG_DBG_SELECT, PSWRQ_REG_DBG_DWORD_ENABLE,
+	PSWRQ_REG_DBG_SHIFT, PSWRQ_REG_DBG_FORCE_VALID,
+	PSWRQ_REG_DBG_FORCE_FRAME,
+	true, false, DBG_RESET_REG_MISC_PL_HV, 1
+};
+
+static struct block_defs block_pswrq2_defs = {
+	"pswrq2", {true, true, true}, false, 0,
+	{DBG_BUS_CLIENT_RBCP, DBG_BUS_CLIENT_RBCP, DBG_BUS_CLIENT_RBCP},
+	PSWRQ2_REG_DBG_SELECT, PSWRQ2_REG_DBG_DWORD_ENABLE,
+	PSWRQ2_REG_DBG_SHIFT, PSWRQ2_REG_DBG_FORCE_VALID,
+	PSWRQ2_REG_DBG_FORCE_FRAME,
+	true, false, DBG_RESET_REG_MISC_PL_HV, 1
+};
+
+static struct block_defs block_pglcs_defs = {
+	"pglcs", {false, false, true}, false, 0,
+	{MAX_DBG_BUS_CLIENTS, MAX_DBG_BUS_CLIENTS, DBG_BUS_CLIENT_RBCH},
+	PGLCS_REG_DBG_SELECT, PGLCS_REG_DBG_DWORD_ENABLE,
+	PGLCS_REG_DBG_SHIFT, PGLCS_REG_DBG_FORCE_VALID,
+	PGLCS_REG_DBG_FORCE_FRAME,
+	true, false, DBG_RESET_REG_MISCS_PL_HV, 2
+};
+
+static struct block_defs block_ptu_defs = {
+	"ptu", {true, true, true}, false, 0,
+	{DBG_BUS_CLIENT_RBCP, DBG_BUS_CLIENT_RBCP, DBG_BUS_CLIENT_RBCP},
+	PTU_REG_DBG_SELECT, PTU_REG_DBG_DWORD_ENABLE,
+	PTU_REG_DBG_SHIFT, PTU_REG_DBG_FORCE_VALID,
+	PTU_REG_DBG_FORCE_FRAME,
+	true, true, DBG_RESET_REG_MISC_PL_PDA_VMAIN_2, 20
+};
+
+static struct block_defs block_dmae_defs = {
+	"dmae", {true, true, true}, false, 0,
+	{DBG_BUS_CLIENT_RBCP, DBG_BUS_CLIENT_RBCP, DBG_BUS_CLIENT_RBCP},
+	DMAE_REG_DBG_SELECT, DMAE_REG_DBG_DWORD_ENABLE,
+	DMAE_REG_DBG_SHIFT, DMAE_REG_DBG_FORCE_VALID,
+	DMAE_REG_DBG_FORCE_FRAME,
+	true, true, DBG_RESET_REG_MISC_PL_PDA_VMAIN_1, 28
+};
+
+static struct block_defs block_tcm_defs = {
+	"tcm", {true, true, true}, true, DBG_TSTORM_ID,
+	{DBG_BUS_CLIENT_RBCT, DBG_BUS_CLIENT_RBCT, DBG_BUS_CLIENT_RBCT},
+	TCM_REG_DBG_SELECT, TCM_REG_DBG_DWORD_ENABLE,
+	TCM_REG_DBG_SHIFT, TCM_REG_DBG_FORCE_VALID,
+	TCM_REG_DBG_FORCE_FRAME,
+	true, true, DBG_RESET_REG_MISC_PL_PDA_VMAIN_1, 5
+};
+
+static struct block_defs block_mcm_defs = {
+	"mcm", {true, true, true}, true, DBG_MSTORM_ID,
+	{DBG_BUS_CLIENT_RBCT, DBG_BUS_CLIENT_RBCT, DBG_BUS_CLIENT_RBCM},
+	MCM_REG_DBG_SELECT, MCM_REG_DBG_DWORD_ENABLE,
+	MCM_REG_DBG_SHIFT, MCM_REG_DBG_FORCE_VALID,
+	MCM_REG_DBG_FORCE_FRAME,
+	true, true, DBG_RESET_REG_MISC_PL_PDA_VMAIN_2, 3
+};
+
+static struct block_defs block_ucm_defs = {
+	"ucm", {true, true, true}, true, DBG_USTORM_ID,
+	{DBG_BUS_CLIENT_RBCU, DBG_BUS_CLIENT_RBCU, DBG_BUS_CLIENT_RBCU},
+	UCM_REG_DBG_SELECT, UCM_REG_DBG_DWORD_ENABLE,
+	UCM_REG_DBG_SHIFT, UCM_REG_DBG_FORCE_VALID,
+	UCM_REG_DBG_FORCE_FRAME,
+	true, true, DBG_RESET_REG_MISC_PL_PDA_VMAIN_1, 8
+};
+
+static struct block_defs block_xcm_defs = {
+	"xcm", {true, true, true}, true, DBG_XSTORM_ID,
+	{DBG_BUS_CLIENT_RBCX, DBG_BUS_CLIENT_RBCX, DBG_BUS_CLIENT_RBCX},
+	XCM_REG_DBG_SELECT, XCM_REG_DBG_DWORD_ENABLE,
+	XCM_REG_DBG_SHIFT, XCM_REG_DBG_FORCE_VALID,
+	XCM_REG_DBG_FORCE_FRAME,
+	true, true, DBG_RESET_REG_MISC_PL_PDA_VMAIN_1, 19
+};
+
+static struct block_defs block_ycm_defs = {
+	"ycm", {true, true, true}, true, DBG_YSTORM_ID,
+	{DBG_BUS_CLIENT_RBCX, DBG_BUS_CLIENT_RBCX, DBG_BUS_CLIENT_RBCY},
+	YCM_REG_DBG_SELECT, YCM_REG_DBG_DWORD_ENABLE,
+	YCM_REG_DBG_SHIFT, YCM_REG_DBG_FORCE_VALID,
+	YCM_REG_DBG_FORCE_FRAME,
+	true, true, DBG_RESET_REG_MISC_PL_PDA_VMAIN_2, 5
+};
+
+static struct block_defs block_pcm_defs = {
+	"pcm", {true, true, true}, true, DBG_PSTORM_ID,
+	{DBG_BUS_CLIENT_RBCS, DBG_BUS_CLIENT_RBCS, DBG_BUS_CLIENT_RBCS},
+	PCM_REG_DBG_SELECT, PCM_REG_DBG_DWORD_ENABLE,
+	PCM_REG_DBG_SHIFT, PCM_REG_DBG_FORCE_VALID,
+	PCM_REG_DBG_FORCE_FRAME,
+	true, true, DBG_RESET_REG_MISC_PL_PDA_VMAIN_2, 4
+};
+
+static struct block_defs block_qm_defs = {
+	"qm", {true, true, true}, false, 0,
+	{DBG_BUS_CLIENT_RBCP, DBG_BUS_CLIENT_RBCP, DBG_BUS_CLIENT_RBCQ},
+	QM_REG_DBG_SELECT, QM_REG_DBG_DWORD_ENABLE,
+	QM_REG_DBG_SHIFT, QM_REG_DBG_FORCE_VALID,
+	QM_REG_DBG_FORCE_FRAME,
+	true, true, DBG_RESET_REG_MISC_PL_PDA_VMAIN_1, 16
+};
+
+static struct block_defs block_tm_defs = {
+	"tm", {true, true, true}, false, 0,
+	{DBG_BUS_CLIENT_RBCS, DBG_BUS_CLIENT_RBCS, DBG_BUS_CLIENT_RBCS},
+	TM_REG_DBG_SELECT, TM_REG_DBG_DWORD_ENABLE,
+	TM_REG_DBG_SHIFT, TM_REG_DBG_FORCE_VALID,
+	TM_REG_DBG_FORCE_FRAME,
+	true, true, DBG_RESET_REG_MISC_PL_PDA_VMAIN_1, 17
+};
+
+static struct block_defs block_dorq_defs = {
+	"dorq", {true, true, true}, false, 0,
+	{DBG_BUS_CLIENT_RBCX, DBG_BUS_CLIENT_RBCX, DBG_BUS_CLIENT_RBCY},
+	DORQ_REG_DBG_SELECT, DORQ_REG_DBG_DWORD_ENABLE,
+	DORQ_REG_DBG_SHIFT, DORQ_REG_DBG_FORCE_VALID,
+	DORQ_REG_DBG_FORCE_FRAME,
+	true, true, DBG_RESET_REG_MISC_PL_PDA_VMAIN_1, 18
+};
+
+static struct block_defs block_brb_defs = {
+	"brb", {true, true, true}, false, 0,
+	{DBG_BUS_CLIENT_RBCR, DBG_BUS_CLIENT_RBCR, DBG_BUS_CLIENT_RBCR},
+	BRB_REG_DBG_SELECT, BRB_REG_DBG_DWORD_ENABLE,
+	BRB_REG_DBG_SHIFT, BRB_REG_DBG_FORCE_VALID,
+	BRB_REG_DBG_FORCE_FRAME,
+	true, true, DBG_RESET_REG_MISC_PL_PDA_VMAIN_1, 0
+};
+
+static struct block_defs block_src_defs = {
+	"src", {true, true, true}, false, 0,
+	{DBG_BUS_CLIENT_RBCF, DBG_BUS_CLIENT_RBCF, DBG_BUS_CLIENT_RBCF},
+	SRC_REG_DBG_SELECT, SRC_REG_DBG_DWORD_ENABLE,
+	SRC_REG_DBG_SHIFT, SRC_REG_DBG_FORCE_VALID,
+	SRC_REG_DBG_FORCE_FRAME,
+	true, true, DBG_RESET_REG_MISC_PL_PDA_VMAIN_1, 2
+};
+
+static struct block_defs block_prs_defs = {
+	"prs", {true, true, true}, false, 0,
+	{DBG_BUS_CLIENT_RBCR, DBG_BUS_CLIENT_RBCR, DBG_BUS_CLIENT_RBCR},
+	PRS_REG_DBG_SELECT, PRS_REG_DBG_DWORD_ENABLE,
+	PRS_REG_DBG_SHIFT, PRS_REG_DBG_FORCE_VALID,
+	PRS_REG_DBG_FORCE_FRAME,
+	true, true, DBG_RESET_REG_MISC_PL_PDA_VMAIN_1, 1
+};
+
+static struct block_defs block_tsdm_defs = {
+	"tsdm", {true, true, true}, true, DBG_TSTORM_ID,
+	{DBG_BUS_CLIENT_RBCT, DBG_BUS_CLIENT_RBCT, DBG_BUS_CLIENT_RBCT},
+	TSDM_REG_DBG_SELECT, TSDM_REG_DBG_DWORD_ENABLE,
+	TSDM_REG_DBG_SHIFT, TSDM_REG_DBG_FORCE_VALID,
+	TSDM_REG_DBG_FORCE_FRAME,
+	true, true, DBG_RESET_REG_MISC_PL_PDA_VMAIN_1, 3
+};
+
+static struct block_defs block_msdm_defs = {
+	"msdm", {true, true, true}, true, DBG_MSTORM_ID,
+	{DBG_BUS_CLIENT_RBCT, DBG_BUS_CLIENT_RBCT, DBG_BUS_CLIENT_RBCM},
+	MSDM_REG_DBG_SELECT, MSDM_REG_DBG_DWORD_ENABLE,
+	MSDM_REG_DBG_SHIFT, MSDM_REG_DBG_FORCE_VALID,
+	MSDM_REG_DBG_FORCE_FRAME,
+	true, true, DBG_RESET_REG_MISC_PL_PDA_VMAIN_2, 6
+};
+
+static struct block_defs block_usdm_defs = {
+	"usdm", {true, true, true}, true, DBG_USTORM_ID,
+	{DBG_BUS_CLIENT_RBCU, DBG_BUS_CLIENT_RBCU, DBG_BUS_CLIENT_RBCU},
+	USDM_REG_DBG_SELECT, USDM_REG_DBG_DWORD_ENABLE,
+	USDM_REG_DBG_SHIFT, USDM_REG_DBG_FORCE_VALID,
+	USDM_REG_DBG_FORCE_FRAME,
+	true, true, DBG_RESET_REG_MISC_PL_PDA_VMAIN_1, 7
+};
+
+static struct block_defs block_xsdm_defs = {
+	"xsdm", {true, true, true}, true, DBG_XSTORM_ID,
+	{DBG_BUS_CLIENT_RBCX, DBG_BUS_CLIENT_RBCX, DBG_BUS_CLIENT_RBCX},
+	XSDM_REG_DBG_SELECT, XSDM_REG_DBG_DWORD_ENABLE,
+	XSDM_REG_DBG_SHIFT, XSDM_REG_DBG_FORCE_VALID,
+	XSDM_REG_DBG_FORCE_FRAME,
+	true, true, DBG_RESET_REG_MISC_PL_PDA_VMAIN_1, 20
+};
+
+static struct block_defs block_ysdm_defs = {
+	"ysdm", {true, true, true}, true, DBG_YSTORM_ID,
+	{DBG_BUS_CLIENT_RBCX, DBG_BUS_CLIENT_RBCX, DBG_BUS_CLIENT_RBCY},
+	YSDM_REG_DBG_SELECT, YSDM_REG_DBG_DWORD_ENABLE,
+	YSDM_REG_DBG_SHIFT, YSDM_REG_DBG_FORCE_VALID,
+	YSDM_REG_DBG_FORCE_FRAME,
+	true, true, DBG_RESET_REG_MISC_PL_PDA_VMAIN_2, 8
+};
+
+static struct block_defs block_psdm_defs = {
+	"psdm", {true, true, true}, true, DBG_PSTORM_ID,
+	{DBG_BUS_CLIENT_RBCS, DBG_BUS_CLIENT_RBCS, DBG_BUS_CLIENT_RBCS},
+	PSDM_REG_DBG_SELECT, PSDM_REG_DBG_DWORD_ENABLE,
+	PSDM_REG_DBG_SHIFT, PSDM_REG_DBG_FORCE_VALID,
+	PSDM_REG_DBG_FORCE_FRAME,
+	true, true, DBG_RESET_REG_MISC_PL_PDA_VMAIN_2, 7
+};
+
+static struct block_defs block_tsem_defs = {
+	"tsem", {true, true, true}, true, DBG_TSTORM_ID,
+	{DBG_BUS_CLIENT_RBCT, DBG_BUS_CLIENT_RBCT, DBG_BUS_CLIENT_RBCT},
+	TSEM_REG_DBG_SELECT, TSEM_REG_DBG_DWORD_ENABLE,
+	TSEM_REG_DBG_SHIFT, TSEM_REG_DBG_FORCE_VALID,
+	TSEM_REG_DBG_FORCE_FRAME,
+	true, true, DBG_RESET_REG_MISC_PL_PDA_VMAIN_1, 4
+};
+
+static struct block_defs block_msem_defs = {
+	"msem", {true, true, true}, true, DBG_MSTORM_ID,
+	{DBG_BUS_CLIENT_RBCT, DBG_BUS_CLIENT_RBCT, DBG_BUS_CLIENT_RBCM},
+	MSEM_REG_DBG_SELECT, MSEM_REG_DBG_DWORD_ENABLE,
+	MSEM_REG_DBG_SHIFT, MSEM_REG_DBG_FORCE_VALID,
+	MSEM_REG_DBG_FORCE_FRAME,
+	true, true, DBG_RESET_REG_MISC_PL_PDA_VMAIN_2, 9
+};
+
+static struct block_defs block_usem_defs = {
+	"usem", {true, true, true}, true, DBG_USTORM_ID,
+	{DBG_BUS_CLIENT_RBCU, DBG_BUS_CLIENT_RBCU, DBG_BUS_CLIENT_RBCU},
+	USEM_REG_DBG_SELECT, USEM_REG_DBG_DWORD_ENABLE,
+	USEM_REG_DBG_SHIFT, USEM_REG_DBG_FORCE_VALID,
+	USEM_REG_DBG_FORCE_FRAME,
+	true, true, DBG_RESET_REG_MISC_PL_PDA_VMAIN_1, 9
+};
+
+static struct block_defs block_xsem_defs = {
+	"xsem", {true, true, true}, true, DBG_XSTORM_ID,
+	{DBG_BUS_CLIENT_RBCX, DBG_BUS_CLIENT_RBCX, DBG_BUS_CLIENT_RBCX},
+	XSEM_REG_DBG_SELECT, XSEM_REG_DBG_DWORD_ENABLE,
+	XSEM_REG_DBG_SHIFT, XSEM_REG_DBG_FORCE_VALID,
+	XSEM_REG_DBG_FORCE_FRAME,
+	true, true, DBG_RESET_REG_MISC_PL_PDA_VMAIN_1, 21
+};
+
+static struct block_defs block_ysem_defs = {
+	"ysem", {true, true, true}, true, DBG_YSTORM_ID,
+	{DBG_BUS_CLIENT_RBCX, DBG_BUS_CLIENT_RBCX, DBG_BUS_CLIENT_RBCY},
+	YSEM_REG_DBG_SELECT, YSEM_REG_DBG_DWORD_ENABLE,
+	YSEM_REG_DBG_SHIFT, YSEM_REG_DBG_FORCE_VALID,
+	YSEM_REG_DBG_FORCE_FRAME,
+	true, true, DBG_RESET_REG_MISC_PL_PDA_VMAIN_2, 11
+};
+
+static struct block_defs block_psem_defs = {
+	"psem", {true, true, true}, true, DBG_PSTORM_ID,
+	{DBG_BUS_CLIENT_RBCS, DBG_BUS_CLIENT_RBCS, DBG_BUS_CLIENT_RBCS},
+	PSEM_REG_DBG_SELECT, PSEM_REG_DBG_DWORD_ENABLE,
+	PSEM_REG_DBG_SHIFT, PSEM_REG_DBG_FORCE_VALID,
+	PSEM_REG_DBG_FORCE_FRAME,
+	true, true, DBG_RESET_REG_MISC_PL_PDA_VMAIN_2, 10
+};
+
+static struct block_defs block_rss_defs = {
+	"rss", {true, true, true}, false, 0,
+	{DBG_BUS_CLIENT_RBCT, DBG_BUS_CLIENT_RBCT, DBG_BUS_CLIENT_RBCT},
+	RSS_REG_DBG_SELECT, RSS_REG_DBG_DWORD_ENABLE,
+	RSS_REG_DBG_SHIFT, RSS_REG_DBG_FORCE_VALID,
+	RSS_REG_DBG_FORCE_FRAME,
+	true, true, DBG_RESET_REG_MISC_PL_PDA_VMAIN_2, 18
+};
+
+static struct block_defs block_tmld_defs = {
+	"tmld", {true, true, true}, false, 0,
+	{DBG_BUS_CLIENT_RBCT, DBG_BUS_CLIENT_RBCT, DBG_BUS_CLIENT_RBCM},
+	TMLD_REG_DBG_SELECT, TMLD_REG_DBG_DWORD_ENABLE,
+	TMLD_REG_DBG_SHIFT, TMLD_REG_DBG_FORCE_VALID,
+	TMLD_REG_DBG_FORCE_FRAME,
+	true, true, DBG_RESET_REG_MISC_PL_PDA_VMAIN_2, 13
+};
+
+static struct block_defs block_muld_defs = {
+	"muld", {true, true, true}, false, 0,
+	{DBG_BUS_CLIENT_RBCU, DBG_BUS_CLIENT_RBCU, DBG_BUS_CLIENT_RBCU},
+	MULD_REG_DBG_SELECT, MULD_REG_DBG_DWORD_ENABLE,
+	MULD_REG_DBG_SHIFT, MULD_REG_DBG_FORCE_VALID,
+	MULD_REG_DBG_FORCE_FRAME,
+	true, true, DBG_RESET_REG_MISC_PL_PDA_VMAIN_2, 14
+};
+
+static struct block_defs block_yuld_defs = {
+	"yuld", {true, true, true}, false, 0,
+	{DBG_BUS_CLIENT_RBCU, DBG_BUS_CLIENT_RBCU, DBG_BUS_CLIENT_RBCU},
+	YULD_REG_DBG_SELECT, YULD_REG_DBG_DWORD_ENABLE,
+	YULD_REG_DBG_SHIFT, YULD_REG_DBG_FORCE_VALID,
+	YULD_REG_DBG_FORCE_FRAME,
+	true, true, DBG_RESET_REG_MISC_PL_PDA_VMAIN_2, 15
+};
+
+static struct block_defs block_xyld_defs = {
+	"xyld", {true, true, true}, false, 0,
+	{DBG_BUS_CLIENT_RBCX, DBG_BUS_CLIENT_RBCX, DBG_BUS_CLIENT_RBCX},
+	XYLD_REG_DBG_SELECT, XYLD_REG_DBG_DWORD_ENABLE,
+	XYLD_REG_DBG_SHIFT, XYLD_REG_DBG_FORCE_VALID,
+	XYLD_REG_DBG_FORCE_FRAME,
+	true, true, DBG_RESET_REG_MISC_PL_PDA_VMAIN_2, 12
+};
+
+static struct block_defs block_prm_defs = {
+	"prm", {true, true, true}, false, 0,
+	{DBG_BUS_CLIENT_RBCT, DBG_BUS_CLIENT_RBCT, DBG_BUS_CLIENT_RBCM},
+	PRM_REG_DBG_SELECT, PRM_REG_DBG_DWORD_ENABLE,
+	PRM_REG_DBG_SHIFT, PRM_REG_DBG_FORCE_VALID,
+	PRM_REG_DBG_FORCE_FRAME,
+	true, true, DBG_RESET_REG_MISC_PL_PDA_VMAIN_2, 21
+};
+
+static struct block_defs block_pbf_pb1_defs = {
+	"pbf_pb1", {true, true, true}, false, 0,
+	{DBG_BUS_CLIENT_RBCS, DBG_BUS_CLIENT_RBCS, DBG_BUS_CLIENT_RBCV},
+	PBF_PB1_REG_DBG_SELECT, PBF_PB1_REG_DBG_DWORD_ENABLE,
+	PBF_PB1_REG_DBG_SHIFT, PBF_PB1_REG_DBG_FORCE_VALID,
+	PBF_PB1_REG_DBG_FORCE_FRAME,
+	true, true, DBG_RESET_REG_MISC_PL_PDA_VMAIN_1,
+	11
+};
+
+static struct block_defs block_pbf_pb2_defs = {
+	"pbf_pb2", {true, true, true}, false, 0,
+	{DBG_BUS_CLIENT_RBCS, DBG_BUS_CLIENT_RBCS, DBG_BUS_CLIENT_RBCV},
+	PBF_PB2_REG_DBG_SELECT, PBF_PB2_REG_DBG_DWORD_ENABLE,
+	PBF_PB2_REG_DBG_SHIFT, PBF_PB2_REG_DBG_FORCE_VALID,
+	PBF_PB2_REG_DBG_FORCE_FRAME,
+	true, true, DBG_RESET_REG_MISC_PL_PDA_VMAIN_1,
+	12
+};
+
+static struct block_defs block_rpb_defs = {
+	"rpb", {true, true, true}, false, 0,
+	{DBG_BUS_CLIENT_RBCT, DBG_BUS_CLIENT_RBCT, DBG_BUS_CLIENT_RBCM},
+	RPB_REG_DBG_SELECT, RPB_REG_DBG_DWORD_ENABLE,
+	RPB_REG_DBG_SHIFT, RPB_REG_DBG_FORCE_VALID,
+	RPB_REG_DBG_FORCE_FRAME,
+	true, true, DBG_RESET_REG_MISC_PL_PDA_VMAIN_1, 13
+};
+
+static struct block_defs block_btb_defs = {
+	"btb", {true, true, true}, false, 0,
+	{DBG_BUS_CLIENT_RBCR, DBG_BUS_CLIENT_RBCR, DBG_BUS_CLIENT_RBCV},
+	BTB_REG_DBG_SELECT, BTB_REG_DBG_DWORD_ENABLE,
+	BTB_REG_DBG_SHIFT, BTB_REG_DBG_FORCE_VALID,
+	BTB_REG_DBG_FORCE_FRAME,
+	true, true, DBG_RESET_REG_MISC_PL_PDA_VMAIN_1, 10
+};
+
+static struct block_defs block_pbf_defs = {
+	"pbf", {true, true, true}, false, 0,
+	{DBG_BUS_CLIENT_RBCS, DBG_BUS_CLIENT_RBCS, DBG_BUS_CLIENT_RBCV},
+	PBF_REG_DBG_SELECT, PBF_REG_DBG_DWORD_ENABLE,
+	PBF_REG_DBG_SHIFT, PBF_REG_DBG_FORCE_VALID,
+	PBF_REG_DBG_FORCE_FRAME,
+	true, true, DBG_RESET_REG_MISC_PL_PDA_VMAIN_1, 15
+};
+
+static struct block_defs block_rdif_defs = {
+	"rdif", {true, true, true}, false, 0,
+	{DBG_BUS_CLIENT_RBCT, DBG_BUS_CLIENT_RBCT, DBG_BUS_CLIENT_RBCM},
+	RDIF_REG_DBG_SELECT, RDIF_REG_DBG_DWORD_ENABLE,
+	RDIF_REG_DBG_SHIFT, RDIF_REG_DBG_FORCE_VALID,
+	RDIF_REG_DBG_FORCE_FRAME,
+	true, true, DBG_RESET_REG_MISC_PL_PDA_VMAIN_2, 16
+};
+
+static struct block_defs block_tdif_defs = {
+	"tdif", {true, true, true}, false, 0,
+	{DBG_BUS_CLIENT_RBCS, DBG_BUS_CLIENT_RBCS, DBG_BUS_CLIENT_RBCS},
+	TDIF_REG_DBG_SELECT, TDIF_REG_DBG_DWORD_ENABLE,
+	TDIF_REG_DBG_SHIFT, TDIF_REG_DBG_FORCE_VALID,
+	TDIF_REG_DBG_FORCE_FRAME,
+	true, true, DBG_RESET_REG_MISC_PL_PDA_VMAIN_2, 17
+};
+
+static struct block_defs block_cdu_defs = {
+	"cdu", {true, true, true}, false, 0,
+	{DBG_BUS_CLIENT_RBCF, DBG_BUS_CLIENT_RBCF, DBG_BUS_CLIENT_RBCF},
+	CDU_REG_DBG_SELECT, CDU_REG_DBG_DWORD_ENABLE,
+	CDU_REG_DBG_SHIFT, CDU_REG_DBG_FORCE_VALID,
+	CDU_REG_DBG_FORCE_FRAME,
+	true, true, DBG_RESET_REG_MISC_PL_PDA_VMAIN_1, 23
+};
+
+static struct block_defs block_ccfc_defs = {
+	"ccfc", {true, true, true}, false, 0,
+	{DBG_BUS_CLIENT_RBCF, DBG_BUS_CLIENT_RBCF, DBG_BUS_CLIENT_RBCF},
+	CCFC_REG_DBG_SELECT, CCFC_REG_DBG_DWORD_ENABLE,
+	CCFC_REG_DBG_SHIFT, CCFC_REG_DBG_FORCE_VALID,
+	CCFC_REG_DBG_FORCE_FRAME,
+	true, true, DBG_RESET_REG_MISC_PL_PDA_VMAIN_1, 24
+};
+
+static struct block_defs block_tcfc_defs = {
+	"tcfc", {true, true, true}, false, 0,
+	{DBG_BUS_CLIENT_RBCF, DBG_BUS_CLIENT_RBCF, DBG_BUS_CLIENT_RBCF},
+	TCFC_REG_DBG_SELECT, TCFC_REG_DBG_DWORD_ENABLE,
+	TCFC_REG_DBG_SHIFT, TCFC_REG_DBG_FORCE_VALID,
+	TCFC_REG_DBG_FORCE_FRAME,
+	true, true, DBG_RESET_REG_MISC_PL_PDA_VMAIN_1, 25
+};
+
+static struct block_defs block_igu_defs = {
+	"igu", {true, true, true}, false, 0,
+	{DBG_BUS_CLIENT_RBCP, DBG_BUS_CLIENT_RBCP, DBG_BUS_CLIENT_RBCP},
+	IGU_REG_DBG_SELECT, IGU_REG_DBG_DWORD_ENABLE,
+	IGU_REG_DBG_SHIFT, IGU_REG_DBG_FORCE_VALID,
+	IGU_REG_DBG_FORCE_FRAME,
+	true, true, DBG_RESET_REG_MISC_PL_PDA_VMAIN_1, 27
+};
+
+static struct block_defs block_cau_defs = {
+	"cau", {true, true, true}, false, 0,
+	{DBG_BUS_CLIENT_RBCP, DBG_BUS_CLIENT_RBCP, DBG_BUS_CLIENT_RBCP},
+	CAU_REG_DBG_SELECT, CAU_REG_DBG_DWORD_ENABLE,
+	CAU_REG_DBG_SHIFT, CAU_REG_DBG_FORCE_VALID,
+	CAU_REG_DBG_FORCE_FRAME,
+	true, true, DBG_RESET_REG_MISC_PL_PDA_VMAIN_2, 19
+};
+
+static struct block_defs block_umac_defs = {
+	"umac", {false, false, true}, false, 0,
+	{MAX_DBG_BUS_CLIENTS, MAX_DBG_BUS_CLIENTS, DBG_BUS_CLIENT_RBCZ},
+	UMAC_REG_DBG_SELECT, UMAC_REG_DBG_DWORD_ENABLE,
+	UMAC_REG_DBG_SHIFT, UMAC_REG_DBG_FORCE_VALID,
+	UMAC_REG_DBG_FORCE_FRAME,
+	true, false, DBG_RESET_REG_MISCS_PL_HV, 6
+};
+
+static struct block_defs block_xmac_defs = {
+	"xmac", {false, false, false}, false, 0,
+	{MAX_DBG_BUS_CLIENTS, MAX_DBG_BUS_CLIENTS, MAX_DBG_BUS_CLIENTS},
+	0, 0, 0, 0, 0,
+	false, false, MAX_DBG_RESET_REGS, 0
+};
+
+static struct block_defs block_dbg_defs = {
+	"dbg", {false, false, false}, false, 0,
+	{MAX_DBG_BUS_CLIENTS, MAX_DBG_BUS_CLIENTS, MAX_DBG_BUS_CLIENTS},
+	0, 0, 0, 0, 0,
+	true, true, DBG_RESET_REG_MISC_PL_PDA_VAUX, 3
+};
+
+static struct block_defs block_nig_defs = {
+	"nig", {true, true, true}, false, 0,
+	{DBG_BUS_CLIENT_RBCN, DBG_BUS_CLIENT_RBCN, DBG_BUS_CLIENT_RBCN},
+	NIG_REG_DBG_SELECT, NIG_REG_DBG_DWORD_ENABLE,
+	NIG_REG_DBG_SHIFT, NIG_REG_DBG_FORCE_VALID,
+	NIG_REG_DBG_FORCE_FRAME,
+	true, true, DBG_RESET_REG_MISC_PL_PDA_VAUX, 0
+};
+
+static struct block_defs block_wol_defs = {
+	"wol", {false, false, true}, false, 0,
+	{MAX_DBG_BUS_CLIENTS, MAX_DBG_BUS_CLIENTS, DBG_BUS_CLIENT_RBCZ},
+	WOL_REG_DBG_SELECT, WOL_REG_DBG_DWORD_ENABLE,
+	WOL_REG_DBG_SHIFT, WOL_REG_DBG_FORCE_VALID,
+	WOL_REG_DBG_FORCE_FRAME,
+	true, true, DBG_RESET_REG_MISC_PL_PDA_VAUX, 7
+};
+
+static struct block_defs block_bmbn_defs = {
+	"bmbn", {false, false, true}, false, 0,
+	{MAX_DBG_BUS_CLIENTS, MAX_DBG_BUS_CLIENTS, DBG_BUS_CLIENT_RBCB},
+	BMBN_REG_DBG_SELECT, BMBN_REG_DBG_DWORD_ENABLE,
+	BMBN_REG_DBG_SHIFT, BMBN_REG_DBG_FORCE_VALID,
+	BMBN_REG_DBG_FORCE_FRAME,
+	false, false, MAX_DBG_RESET_REGS, 0
+};
+
+static struct block_defs block_ipc_defs = {
+	"ipc", {false, false, false}, false, 0,
+	{MAX_DBG_BUS_CLIENTS, MAX_DBG_BUS_CLIENTS, MAX_DBG_BUS_CLIENTS},
+	0, 0, 0, 0, 0,
+	true, false, DBG_RESET_REG_MISCS_PL_UA, 8
+};
+
+static struct block_defs block_nwm_defs = {
+	"nwm", {false, false, true}, false, 0,
+	{MAX_DBG_BUS_CLIENTS, MAX_DBG_BUS_CLIENTS, DBG_BUS_CLIENT_RBCW},
+	NWM_REG_DBG_SELECT, NWM_REG_DBG_DWORD_ENABLE,
+	NWM_REG_DBG_SHIFT, NWM_REG_DBG_FORCE_VALID,
+	NWM_REG_DBG_FORCE_FRAME,
+	true, false, DBG_RESET_REG_MISCS_PL_HV_2, 0
+};
+
+static struct block_defs block_nws_defs = {
+	"nws", {false, false, false}, false, 0,
+	{MAX_DBG_BUS_CLIENTS, MAX_DBG_BUS_CLIENTS, MAX_DBG_BUS_CLIENTS},
+	0, 0, 0, 0, 0,
+	true, false, DBG_RESET_REG_MISCS_PL_HV, 12
+};
+
+static struct block_defs block_ms_defs = {
+	"ms", {false, false, false}, false, 0,
+	{MAX_DBG_BUS_CLIENTS, MAX_DBG_BUS_CLIENTS, MAX_DBG_BUS_CLIENTS},
+	0, 0, 0, 0, 0,
+	true, false, DBG_RESET_REG_MISCS_PL_HV, 13
+};
+
+static struct block_defs block_phy_pcie_defs = {
+	"phy_pcie", {false, false, true}, false, 0,
+	{MAX_DBG_BUS_CLIENTS, MAX_DBG_BUS_CLIENTS, DBG_BUS_CLIENT_RBCH},
+	PCIE_REG_DBG_COMMON_SELECT, PCIE_REG_DBG_COMMON_DWORD_ENABLE,
+	PCIE_REG_DBG_COMMON_SHIFT, PCIE_REG_DBG_COMMON_FORCE_VALID,
+	PCIE_REG_DBG_COMMON_FORCE_FRAME,
+	false, false, MAX_DBG_RESET_REGS, 0
+};
+
+static struct block_defs block_led_defs = {
+	"led", {false, false, false}, false, 0,
+	{MAX_DBG_BUS_CLIENTS, MAX_DBG_BUS_CLIENTS, MAX_DBG_BUS_CLIENTS},
+	0, 0, 0, 0, 0,
+	true, true, DBG_RESET_REG_MISCS_PL_HV, 14
+};
+
+static struct block_defs block_misc_aeu_defs = {
+	"misc_aeu", {false, false, false}, false, 0,
+	{MAX_DBG_BUS_CLIENTS, MAX_DBG_BUS_CLIENTS, MAX_DBG_BUS_CLIENTS},
+	0, 0, 0, 0, 0,
+	false, false, MAX_DBG_RESET_REGS, 0
+};
+
+static struct block_defs block_bar0_map_defs = {
+	"bar0_map", {false, false, false}, false, 0,
+	{MAX_DBG_BUS_CLIENTS, MAX_DBG_BUS_CLIENTS, MAX_DBG_BUS_CLIENTS},
+	0, 0, 0, 0, 0,
+	false, false, MAX_DBG_RESET_REGS, 0
+};
+
+static struct block_defs *s_block_defs[MAX_BLOCK_ID] = {
+	&block_grc_defs,
+	&block_miscs_defs,
+	&block_misc_defs,
+	&block_dbu_defs,
+	&block_pglue_b_defs,
+	&block_cnig_defs,
+	&block_cpmu_defs,
+	&block_ncsi_defs,
+	&block_opte_defs,
+	&block_bmb_defs,
+	&block_pcie_defs,
+	&block_mcp_defs,
+	&block_mcp2_defs,
+	&block_pswhst_defs,
+	&block_pswhst2_defs,
+	&block_pswrd_defs,
+	&block_pswrd2_defs,
+	&block_pswwr_defs,
+	&block_pswwr2_defs,
+	&block_pswrq_defs,
+	&block_pswrq2_defs,
+	&block_pglcs_defs,
+	&block_dmae_defs,
+	&block_ptu_defs,
+	&block_tcm_defs,
+	&block_mcm_defs,
+	&block_ucm_defs,
+	&block_xcm_defs,
+	&block_ycm_defs,
+	&block_pcm_defs,
+	&block_qm_defs,
+	&block_tm_defs,
+	&block_dorq_defs,
+	&block_brb_defs,
+	&block_src_defs,
+	&block_prs_defs,
+	&block_tsdm_defs,
+	&block_msdm_defs,
+	&block_usdm_defs,
+	&block_xsdm_defs,
+	&block_ysdm_defs,
+	&block_psdm_defs,
+	&block_tsem_defs,
+	&block_msem_defs,
+	&block_usem_defs,
+	&block_xsem_defs,
+	&block_ysem_defs,
+	&block_psem_defs,
+	&block_rss_defs,
+	&block_tmld_defs,
+	&block_muld_defs,
+	&block_yuld_defs,
+	&block_xyld_defs,
+	&block_prm_defs,
+	&block_pbf_pb1_defs,
+	&block_pbf_pb2_defs,
+	&block_rpb_defs,
+	&block_btb_defs,
+	&block_pbf_defs,
+	&block_rdif_defs,
+	&block_tdif_defs,
+	&block_cdu_defs,
+	&block_ccfc_defs,
+	&block_tcfc_defs,
+	&block_igu_defs,
+	&block_cau_defs,
+	&block_umac_defs,
+	&block_xmac_defs,
+	&block_dbg_defs,
+	&block_nig_defs,
+	&block_wol_defs,
+	&block_bmbn_defs,
+	&block_ipc_defs,
+	&block_nwm_defs,
+	&block_nws_defs,
+	&block_ms_defs,
+	&block_phy_pcie_defs,
+	&block_led_defs,
+	&block_misc_aeu_defs,
+	&block_bar0_map_defs,
+};
+
+static struct platform_defs s_platform_defs[] = {
+	{"asic", 1},
+	{"reserved", 0},
+	{"reserved2", 0},
+	{"reserved3", 0}
+};
+
+static struct grc_param_defs s_grc_param_defs[] = {
+	{{1, 1, 1}, 0, 1, false, 1, 1},	/* DBG_GRC_PARAM_DUMP_TSTORM */
+	{{1, 1, 1}, 0, 1, false, 1, 1},	/* DBG_GRC_PARAM_DUMP_MSTORM */
+	{{1, 1, 1}, 0, 1, false, 1, 1},	/* DBG_GRC_PARAM_DUMP_USTORM */
+	{{1, 1, 1}, 0, 1, false, 1, 1},	/* DBG_GRC_PARAM_DUMP_XSTORM */
+	{{1, 1, 1}, 0, 1, false, 1, 1},	/* DBG_GRC_PARAM_DUMP_YSTORM */
+	{{1, 1, 1}, 0, 1, false, 1, 1},	/* DBG_GRC_PARAM_DUMP_PSTORM */
+	{{1, 1, 1}, 0, 1, false, 0, 1},	/* DBG_GRC_PARAM_DUMP_REGS */
+	{{1, 1, 1}, 0, 1, false, 0, 1},	/* DBG_GRC_PARAM_DUMP_RAM */
+	{{1, 1, 1}, 0, 1, false, 0, 1},	/* DBG_GRC_PARAM_DUMP_PBUF */
+	{{0, 0, 0}, 0, 1, false, 0, 1},	/* DBG_GRC_PARAM_DUMP_IOR */
+	{{0, 0, 0}, 0, 1, false, 0, 1},	/* DBG_GRC_PARAM_DUMP_VFC */
+	{{1, 1, 1}, 0, 1, false, 0, 1},	/* DBG_GRC_PARAM_DUMP_CM_CTX */
+	{{1, 1, 1}, 0, 1, false, 0, 1},	/* DBG_GRC_PARAM_DUMP_ILT */
+	{{1, 1, 1}, 0, 1, false, 0, 1},	/* DBG_GRC_PARAM_DUMP_RSS */
+	{{1, 1, 1}, 0, 1, false, 0, 1},	/* DBG_GRC_PARAM_DUMP_CAU */
+	{{1, 1, 1}, 0, 1, false, 0, 1},	/* DBG_GRC_PARAM_DUMP_QM */
+	{{1, 1, 1}, 0, 1, false, 0, 1},	/* DBG_GRC_PARAM_DUMP_MCP */
+	{{1, 1, 1}, 0, 1, false, 0, 1},	/* DBG_GRC_PARAM_RESERVED */
+	{{1, 1, 1}, 0, 1, false, 0, 1},	/* DBG_GRC_PARAM_DUMP_CFC */
+	{{1, 1, 1}, 0, 1, false, 0, 1},	/* DBG_GRC_PARAM_DUMP_IGU */
+	{{0, 0, 0}, 0, 1, false, 0, 1},	/* DBG_GRC_PARAM_DUMP_BRB */
+	{{0, 0, 0}, 0, 1, false, 0, 1},	/* DBG_GRC_PARAM_DUMP_BTB */
+	{{0, 0, 0}, 0, 1, false, 0, 1},	/* DBG_GRC_PARAM_DUMP_BMB */
+	{{1, 1, 1}, 0, 1, false, 0, 1},	/* DBG_GRC_PARAM_DUMP_NIG */
+	{{1, 1, 1}, 0, 1, false, 0, 1},	/* DBG_GRC_PARAM_DUMP_MULD */
+	{{1, 1, 1}, 0, 1, false, 0, 1},	/* DBG_GRC_PARAM_DUMP_PRS */
+	{{1, 1, 1}, 0, 1, false, 0, 1},	/* DBG_GRC_PARAM_DUMP_DMAE */
+	{{1, 1, 1}, 0, 1, false, 0, 1},	/* DBG_GRC_PARAM_DUMP_TM */
+	{{1, 1, 1}, 0, 1, false, 0, 1},	/* DBG_GRC_PARAM_DUMP_SDM */
+	{{1, 1, 1}, 0, 1, false, 0, 1},	/* DBG_GRC_PARAM_DUMP_DIF */
+	{{1, 1, 1}, 0, 1, false, 0, 1},	/* DBG_GRC_PARAM_DUMP_STATIC */
+	{{0, 0, 0}, 0, 1, false, 0, 0},	/* DBG_GRC_PARAM_UNSTALL */
+	{{MAX_LCIDS, MAX_LCIDS, MAX_LCIDS}, 1, MAX_LCIDS, false, MAX_LCIDS,
+	 MAX_LCIDS},			/* DBG_GRC_PARAM_NUM_LCIDS */
+	{{MAX_LTIDS, MAX_LTIDS, MAX_LTIDS}, 1, MAX_LTIDS, false, MAX_LTIDS,
+	 MAX_LTIDS},			/* DBG_GRC_PARAM_NUM_LTIDS */
+	{{0, 0, 0}, 0, 1, true, 0, 0},	/* DBG_GRC_PARAM_EXCLUDE_ALL */
+	{{0, 0, 0}, 0, 1, true, 0, 0},	/* DBG_GRC_PARAM_CRASH */
+	{{0, 0, 0}, 0, 1, false, 1, 0},	/* DBG_GRC_PARAM_PARITY_SAFE */
+	{{1, 1, 1}, 0, 1, false, 0, 1},	/* DBG_GRC_PARAM_DUMP_CM */
+	{{1, 1, 1}, 0, 1, false, 0, 1}	/* DBG_GRC_PARAM_DUMP_PHY */
+};
+
+static struct rss_mem_defs s_rss_mem_defs[] = {
+	{ "rss_mem_cid", "rss_cid", 0,
+	  {256, 256, 320},
+	  {32, 32, 32} },
+	{ "rss_mem_key_msb", "rss_key", 1024,
+	  {128, 128, 208},
+	  {256, 256, 256} },
+	{ "rss_mem_key_lsb", "rss_key", 2048,
+	  {128, 128, 208},
+	  {64, 64, 64} },
+	{ "rss_mem_info", "rss_info", 3072,
+	  {128, 128, 208},
+	  {16, 16, 16} },
+	{ "rss_mem_ind", "rss_ind", 4096,
+	  {(128 * 128), (128 * 128), (128 * 208)},
+	  {16, 16, 16} }
+};
+
+static struct vfc_ram_defs s_vfc_ram_defs[] = {
+	{"vfc_ram_tt1", "vfc_ram", 0, 512},
+	{"vfc_ram_mtt2", "vfc_ram", 512, 128},
+	{"vfc_ram_stt2", "vfc_ram", 640, 32},
+	{"vfc_ram_ro_vect", "vfc_ram", 672, 32}
+};
+
+static struct big_ram_defs s_big_ram_defs[] = {
+	{ "BRB", MEM_GROUP_BRB_MEM, MEM_GROUP_BRB_RAM, DBG_GRC_PARAM_DUMP_BRB,
+	  BRB_REG_BIG_RAM_ADDRESS, BRB_REG_BIG_RAM_DATA,
+	  {4800, 4800, 5632} },
+	{ "BTB", MEM_GROUP_BTB_MEM, MEM_GROUP_BTB_RAM, DBG_GRC_PARAM_DUMP_BTB,
+	  BTB_REG_BIG_RAM_ADDRESS, BTB_REG_BIG_RAM_DATA,
+	  {2880, 2880, 3680} },
+	{ "BMB", MEM_GROUP_BMB_MEM, MEM_GROUP_BMB_RAM, DBG_GRC_PARAM_DUMP_BMB,
+	  BMB_REG_BIG_RAM_ADDRESS, BMB_REG_BIG_RAM_DATA,
+	  {1152, 1152, 1152} }
+};
+
+static struct reset_reg_defs s_reset_regs_defs[] = {
+	{ MISCS_REG_RESET_PL_UA, 0x0,
+	  {true, true, true} },		/* DBG_RESET_REG_MISCS_PL_UA */
+	{ MISCS_REG_RESET_PL_HV, 0x0,
+	  {true, true, true} },		/* DBG_RESET_REG_MISCS_PL_HV */
+	{ MISCS_REG_RESET_PL_HV_2, 0x0,
+	  {false, false, true} },	/* DBG_RESET_REG_MISCS_PL_HV_2 */
+	{ MISC_REG_RESET_PL_UA, 0x0,
+	  {true, true, true} },		/* DBG_RESET_REG_MISC_PL_UA */
+	{ MISC_REG_RESET_PL_HV, 0x0,
+	  {true, true, true} },		/* DBG_RESET_REG_MISC_PL_HV */
+	{ MISC_REG_RESET_PL_PDA_VMAIN_1, 0x4404040,
+	  {true, true, true} },		/* DBG_RESET_REG_MISC_PL_PDA_VMAIN_1 */
+	{ MISC_REG_RESET_PL_PDA_VMAIN_2, 0x7c00007,
+	  {true, true, true} },		/* DBG_RESET_REG_MISC_PL_PDA_VMAIN_2 */
+	{ MISC_REG_RESET_PL_PDA_VAUX, 0x2,
+	  {true, true, true} },		/* DBG_RESET_REG_MISC_PL_PDA_VAUX */
+};
+
+static struct phy_defs s_phy_defs[] = {
+	{"nw_phy", NWS_REG_NWS_CMU, PHY_NW_IP_REG_PHY0_TOP_TBUS_ADDR_7_0,
+	 PHY_NW_IP_REG_PHY0_TOP_TBUS_ADDR_15_8,
+	 PHY_NW_IP_REG_PHY0_TOP_TBUS_DATA_7_0,
+	 PHY_NW_IP_REG_PHY0_TOP_TBUS_DATA_11_8},
+	{"sgmii_phy", MS_REG_MS_CMU, PHY_SGMII_IP_REG_AHB_CMU_CSR_0_X132,
+	 PHY_SGMII_IP_REG_AHB_CMU_CSR_0_X133,
+	 PHY_SGMII_IP_REG_AHB_CMU_CSR_0_X130,
+	 PHY_SGMII_IP_REG_AHB_CMU_CSR_0_X131},
+	{"pcie_phy0", PHY_PCIE_REG_PHY0, PHY_PCIE_IP_REG_AHB_CMU_CSR_0_X132,
+	 PHY_PCIE_IP_REG_AHB_CMU_CSR_0_X133,
+	 PHY_PCIE_IP_REG_AHB_CMU_CSR_0_X130,
+	 PHY_PCIE_IP_REG_AHB_CMU_CSR_0_X131},
+	{"pcie_phy1", PHY_PCIE_REG_PHY1, PHY_PCIE_IP_REG_AHB_CMU_CSR_0_X132,
+	 PHY_PCIE_IP_REG_AHB_CMU_CSR_0_X133,
+	 PHY_PCIE_IP_REG_AHB_CMU_CSR_0_X130,
+	 PHY_PCIE_IP_REG_AHB_CMU_CSR_0_X131},
+};
+
+/**************************** Private Functions ******************************/
+
+/* Reads and returns a single dword from the specified unaligned buffer */
+static u32 qed_read_unaligned_dword(u8 *buf)
+{
+	u32 dword;
+
+	memcpy((u8 *)&dword, buf, sizeof(dword));
+	return dword;
+}
+
+/* Initializes debug data for the specified device */
+static enum dbg_status qed_dbg_dev_init(struct qed_hwfn *p_hwfn,
+					struct qed_ptt *p_ptt)
+{
+	struct dbg_tools_data *dev_data = &p_hwfn->dbg_info;
+
+	if (dev_data->initialized)
+		return DBG_STATUS_OK;
+
+	if (QED_IS_K2(p_hwfn->cdev)) {
+		dev_data->chip_id = CHIP_K2;
+		dev_data->mode_enable[MODE_K2] = 1;
+	} else if (QED_IS_BB_B0(p_hwfn->cdev)) {
+		dev_data->chip_id = CHIP_BB_B0;
+		dev_data->mode_enable[MODE_BB_B0] = 1;
+	} else {
+		return DBG_STATUS_UNKNOWN_CHIP;
+	}
+
+	dev_data->platform_id = PLATFORM_ASIC;
+	dev_data->mode_enable[MODE_ASIC] = 1;
+	dev_data->initialized = true;
+	return DBG_STATUS_OK;
+}
+
+/* Reads the FW info structure for the specified Storm from the chip,
+ * and writes it to the specified fw_info pointer.
+ */
+static void qed_read_fw_info(struct qed_hwfn *p_hwfn,
+			     struct qed_ptt *p_ptt,
+			     u8 storm_id, struct fw_info *fw_info)
+{
+	/* Read first the address that points to fw_info location.
+	 * The address is located in the last line of the Storm RAM.
+	 */
+	u32 addr = s_storm_defs[storm_id].sem_fast_mem_addr +
+		   SEM_FAST_REG_INT_RAM +
+		   DWORDS_TO_BYTES(SEM_FAST_REG_INT_RAM_SIZE) -
+		   sizeof(struct fw_info_location);
+	struct fw_info_location fw_info_location;
+	u32 *dest = (u32 *)&fw_info_location;
+	u32 i;
+
+	memset(&fw_info_location, 0, sizeof(fw_info_location));
+	memset(fw_info, 0, sizeof(*fw_info));
+	for (i = 0; i < BYTES_TO_DWORDS(sizeof(fw_info_location));
+	     i++, addr += BYTES_IN_DWORD)
+		dest[i] = qed_rd(p_hwfn, p_ptt, addr);
+	if (fw_info_location.size > 0 && fw_info_location.size <=
+	    sizeof(*fw_info)) {
+		/* Read FW version info from Storm RAM */
+		addr = fw_info_location.grc_addr;
+		dest = (u32 *)fw_info;
+		for (i = 0; i < BYTES_TO_DWORDS(fw_info_location.size);
+		     i++, addr += BYTES_IN_DWORD)
+			dest[i] = qed_rd(p_hwfn, p_ptt, addr);
+	}
+}
+
+/* Dumps the specified string to the specified buffer. Returns the dumped size
+ * in bytes (actual length + 1 for the null character termination).
+ */
+static u32 qed_dump_str(char *dump_buf, bool dump, const char *str)
+{
+	if (dump)
+		strcpy(dump_buf, str);
+	return (u32)strlen(str) + 1;
+}
+
+/* Dumps zeros to align the specified buffer to dwords. Returns the dumped size
+ * in bytes.
+ */
+static u32 qed_dump_align(char *dump_buf, bool dump, u32 byte_offset)
+{
+	u8 offset_in_dword = (u8)(byte_offset & 0x3), align_size;
+
+	align_size = offset_in_dword ? BYTES_IN_DWORD - offset_in_dword : 0;
+
+	if (dump && align_size)
+		memset(dump_buf, 0, align_size);
+	return align_size;
+}
+
+/* Writes the specified string param to the specified buffer.
+ * Returns the dumped size in dwords.
+ */
+static u32 qed_dump_str_param(u32 *dump_buf,
+			      bool dump,
+			      const char *param_name, const char *param_val)
+{
+	char *char_buf = (char *)dump_buf;
+	u32 offset = 0;
+
+	/* Dump param name */
+	offset += qed_dump_str(char_buf + offset, dump, param_name);
+
+	/* Indicate a string param value */
+	if (dump)
+		*(char_buf + offset) = 1;
+	offset++;
+
+	/* Dump param value */
+	offset += qed_dump_str(char_buf + offset, dump, param_val);
+
+	/* Align buffer to next dword */
+	offset += qed_dump_align(char_buf + offset, dump, offset);
+	return BYTES_TO_DWORDS(offset);
+}
+
+/* Writes the specified numeric param to the specified buffer.
+ * Returns the dumped size in dwords.
+ */
+static u32 qed_dump_num_param(u32 *dump_buf,
+			      bool dump, const char *param_name, u32 param_val)
+{
+	char *char_buf = (char *)dump_buf;
+	u32 offset = 0;
+
+	/* Dump param name */
+	offset += qed_dump_str(char_buf + offset, dump, param_name);
+
+	/* Indicate a numeric param value */
+	if (dump)
+		*(char_buf + offset) = 0;
+	offset++;
+
+	/* Align buffer to next dword */
+	offset += qed_dump_align(char_buf + offset, dump, offset);
+
+	/* Dump param value (and change offset from bytes to dwords) */
+	offset = BYTES_TO_DWORDS(offset);
+	if (dump)
+		*(dump_buf + offset) = param_val;
+	offset++;
+	return offset;
+}
+
+/* Reads the FW version and writes it as a param to the specified buffer.
+ * Returns the dumped size in dwords.
+ */
+static u32 qed_dump_fw_ver_param(struct qed_hwfn *p_hwfn,
+				 struct qed_ptt *p_ptt,
+				 u32 *dump_buf, bool dump)
+{
+	struct dbg_tools_data *dev_data = &p_hwfn->dbg_info;
+	char fw_ver_str[16] = EMPTY_FW_VERSION_STR;
+	char fw_img_str[16] = EMPTY_FW_IMAGE_STR;
+	struct fw_info fw_info = { {0}, {0} };
+	int printed_chars;
+	u32 offset = 0;
+
+	if (dump) {
+		/* Read FW image/version from PRAM in a non-reset SEMI */
+		bool found = false;
+		u8 storm_id;
+
+		for (storm_id = 0; storm_id < MAX_DBG_STORMS && !found;
+		     storm_id++) {
+			/* Read FW version/image  */
+			if (!dev_data->block_in_reset
+			    [s_storm_defs[storm_id].block_id]) {
+				/* read FW info for the current Storm */
+				qed_read_fw_info(p_hwfn,
+						 p_ptt, storm_id, &fw_info);
+
+				/* Create FW version/image strings */
+				printed_chars =
+				    snprintf(fw_ver_str,
+					     sizeof(fw_ver_str),
+					     "%d_%d_%d_%d",
+					     fw_info.ver.num.major,
+					     fw_info.ver.num.minor,
+					     fw_info.ver.num.rev,
+					     fw_info.ver.num.eng);
+				if (printed_chars < 0 || printed_chars >=
+				    sizeof(fw_ver_str))
+					DP_NOTICE(p_hwfn,
+						  "Unexpected debug error: invalid FW version string\n");
+				switch (fw_info.ver.image_id) {
+				case FW_IMG_MAIN:
+					strcpy(fw_img_str, "main");
+					break;
+				default:
+					strcpy(fw_img_str, "unknown");
+					break;
+				}
+
+				found = true;
+			}
+		}
+	}
+
+	/* Dump FW version, image and timestamp */
+	offset += qed_dump_str_param(dump_buf + offset,
+				     dump, "fw-version", fw_ver_str);
+	offset += qed_dump_str_param(dump_buf + offset,
+				     dump, "fw-image", fw_img_str);
+	offset += qed_dump_num_param(dump_buf + offset,
+				     dump,
+				     "fw-timestamp", fw_info.ver.timestamp);
+	return offset;
+}
+
+/* Reads the MFW version and writes it as a param to the specified buffer.
+ * Returns the dumped size in dwords.
+ */
+static u32 qed_dump_mfw_ver_param(struct qed_hwfn *p_hwfn,
+				  struct qed_ptt *p_ptt,
+				  u32 *dump_buf, bool dump)
+{
+	char mfw_ver_str[16] = EMPTY_FW_VERSION_STR;
+
+	if (dump) {
+		u32 global_section_offsize, global_section_addr, mfw_ver;
+		u32 public_data_addr, global_section_offsize_addr;
+		int printed_chars;
+
+		/* Find MCP public data GRC address.
+		 * Needs to be ORed with MCP_REG_SCRATCH due to a HW bug.
+		 */
+		public_data_addr = qed_rd(p_hwfn, p_ptt,
+					  MISC_REG_SHARED_MEM_ADDR) |
+					  MCP_REG_SCRATCH;
+
+		/* Find MCP public global section offset */
+		global_section_offsize_addr = public_data_addr +
+					      offsetof(struct mcp_public_data,
+						       sections) +
+					      sizeof(offsize_t) * PUBLIC_GLOBAL;
+		global_section_offsize = qed_rd(p_hwfn, p_ptt,
+						global_section_offsize_addr);
+		global_section_addr = MCP_REG_SCRATCH +
+				      (global_section_offsize &
+				       OFFSIZE_OFFSET_MASK) * 4;
+
+		/* Read MFW version from MCP public global section */
+		mfw_ver = qed_rd(p_hwfn, p_ptt,
+				 global_section_addr +
+				 offsetof(struct public_global, mfw_ver));
+
+		/* Dump MFW version param */
+		printed_chars = snprintf(mfw_ver_str, sizeof(mfw_ver_str),
+					 "%d_%d_%d_%d",
+					 (u8) (mfw_ver >> 24),
+					 (u8) (mfw_ver >> 16),
+					 (u8) (mfw_ver >> 8),
+					 (u8) mfw_ver);
+		if (printed_chars < 0 || printed_chars >= sizeof(mfw_ver_str))
+			DP_NOTICE(p_hwfn,
+				  "Unexpected debug error: invalid MFW version string\n");
+	}
+
+	return qed_dump_str_param(dump_buf, dump, "mfw-version", mfw_ver_str);
+}
+
+/* Writes a section header to the specified buffer.
+ * Returns the dumped size in dwords.
+ */
+static u32 qed_dump_section_hdr(u32 *dump_buf,
+				bool dump, const char *name, u32 num_params)
+{
+	return qed_dump_num_param(dump_buf, dump, name, num_params);
+}
+
+/* Writes the common global params to the specified buffer.
+ * Returns the dumped size in dwords.
+ */
+static u32 qed_dump_common_global_params(struct qed_hwfn *p_hwfn,
+					 struct qed_ptt *p_ptt,
+					 u32 *dump_buf,
+					 bool dump,
+					 u8 num_specific_global_params)
+{
+	struct dbg_tools_data *dev_data = &p_hwfn->dbg_info;
+	u32 offset = 0;
+
+	/* Find platform string and dump global params section header */
+	offset += qed_dump_section_hdr(dump_buf + offset,
+				       dump,
+				       "global_params",
+				       NUM_COMMON_GLOBAL_PARAMS +
+				       num_specific_global_params);
+
+	/* Store params */
+	offset += qed_dump_fw_ver_param(p_hwfn, p_ptt, dump_buf + offset, dump);
+	offset += qed_dump_mfw_ver_param(p_hwfn,
+					 p_ptt, dump_buf + offset, dump);
+	offset += qed_dump_num_param(dump_buf + offset,
+				     dump, "tools-version", TOOLS_VERSION);
+	offset += qed_dump_str_param(dump_buf + offset,
+				     dump,
+				     "chip",
+				     s_chip_defs[dev_data->chip_id].name);
+	offset += qed_dump_str_param(dump_buf + offset,
+				     dump,
+				     "platform",
+				     s_platform_defs[dev_data->platform_id].
+				     name);
+	offset +=
+	    qed_dump_num_param(dump_buf + offset, dump, "pci-func",
+			       p_hwfn->abs_pf_id);
+	return offset;
+}
+
+/* Writes the last section to the specified buffer at the given offset.
+ * Returns the dumped size in dwords.
+ */
+static u32 qed_dump_last_section(u32 *dump_buf, u32 offset, bool dump)
+{
+	u32 start_offset = offset, crc = ~0;
+
+	/* Dump CRC section header */
+	offset += qed_dump_section_hdr(dump_buf + offset, dump, "last", 0);
+
+	/* Calculate CRC32 and add it to the dword following the "last" section.
+	 */
+	if (dump)
+		*(dump_buf + offset) = ~crc32(crc, (u8 *)dump_buf,
+					      DWORDS_TO_BYTES(offset));
+	offset++;
+	return offset - start_offset;
+}
+
+/* Update blocks reset state  */
+static void qed_update_blocks_reset_state(struct qed_hwfn *p_hwfn,
+					  struct qed_ptt *p_ptt)
+{
+	struct dbg_tools_data *dev_data = &p_hwfn->dbg_info;
+	u32 reg_val[MAX_DBG_RESET_REGS] = { 0 };
+	u32 i;
+
+	/* Read reset registers */
+	for (i = 0; i < MAX_DBG_RESET_REGS; i++)
+		if (s_reset_regs_defs[i].exists[dev_data->chip_id])
+			reg_val[i] = qed_rd(p_hwfn,
+					    p_ptt, s_reset_regs_defs[i].addr);
+
+	/* Check if blocks are in reset */
+	for (i = 0; i < MAX_BLOCK_ID; i++)
+		dev_data->block_in_reset[i] =
+		    s_block_defs[i]->has_reset_bit &&
+		    !(reg_val[s_block_defs[i]->reset_reg] &
+		      BIT(s_block_defs[i]->reset_bit_offset));
+}
+
+/* Enable / disable the Debug block */
+static void qed_bus_enable_dbg_block(struct qed_hwfn *p_hwfn,
+				     struct qed_ptt *p_ptt, bool enable)
+{
+	qed_wr(p_hwfn, p_ptt, DBG_REG_DBG_BLOCK_ON, enable ? 1 : 0);
+}
+
+/* Resets the Debug block */
+static void qed_bus_reset_dbg_block(struct qed_hwfn *p_hwfn,
+				    struct qed_ptt *p_ptt)
+{
+	u32 dbg_reset_reg_addr, old_reset_reg_val, new_reset_reg_val;
+
+	dbg_reset_reg_addr =
+		s_reset_regs_defs[s_block_defs[BLOCK_DBG]->reset_reg].addr;
+	old_reset_reg_val = qed_rd(p_hwfn, p_ptt, dbg_reset_reg_addr);
+	new_reset_reg_val = old_reset_reg_val &
+			    ~BIT(s_block_defs[BLOCK_DBG]->reset_bit_offset);
+
+	qed_wr(p_hwfn, p_ptt, dbg_reset_reg_addr, new_reset_reg_val);
+	qed_wr(p_hwfn, p_ptt, dbg_reset_reg_addr, old_reset_reg_val);
+}
+
+static void qed_bus_set_framing_mode(struct qed_hwfn *p_hwfn,
+				     struct qed_ptt *p_ptt,
+				     enum dbg_bus_frame_modes mode)
+{
+	qed_wr(p_hwfn, p_ptt, DBG_REG_FRAMING_MODE, (u8)mode);
+}
+
+/* Enable / disable Debug Bus clients according to the specified mask.
+ * (1 = enable, 0 = disable)
+ */
+static void qed_bus_enable_clients(struct qed_hwfn *p_hwfn,
+				   struct qed_ptt *p_ptt, u32 client_mask)
+{
+	qed_wr(p_hwfn, p_ptt, DBG_REG_CLIENT_ENABLE, client_mask);
+}
+
+static bool qed_is_mode_match(struct qed_hwfn *p_hwfn, u16 *modes_buf_offset)
+{
+	const u32 *ptr = s_dbg_arrays[BIN_BUF_DBG_MODE_TREE].ptr;
+	struct dbg_tools_data *dev_data = &p_hwfn->dbg_info;
+	u8 tree_val = ((u8 *)ptr)[(*modes_buf_offset)++];
+	bool arg1, arg2;
+
+	switch (tree_val) {
+	case INIT_MODE_OP_NOT:
+		return !qed_is_mode_match(p_hwfn, modes_buf_offset);
+	case INIT_MODE_OP_OR:
+	case INIT_MODE_OP_AND:
+		arg1 = qed_is_mode_match(p_hwfn, modes_buf_offset);
+		arg2 = qed_is_mode_match(p_hwfn, modes_buf_offset);
+		return (tree_val == INIT_MODE_OP_OR) ? (arg1 ||
+							arg2) : (arg1 && arg2);
+	default:
+		return dev_data->mode_enable[tree_val - MAX_INIT_MODE_OPS] > 0;
+	}
+}
+
+/* Returns the value of the specified GRC param */
+static u32 qed_grc_get_param(struct qed_hwfn *p_hwfn,
+			     enum dbg_grc_params grc_param)
+{
+	struct dbg_tools_data *dev_data = &p_hwfn->dbg_info;
+
+	return dev_data->grc.param_val[grc_param];
+}
+
+/* Clear all GRC params */
+static void qed_dbg_grc_clear_params(struct qed_hwfn *p_hwfn)
+{
+	struct dbg_tools_data *dev_data = &p_hwfn->dbg_info;
+	u32 i;
+
+	for (i = 0; i < MAX_DBG_GRC_PARAMS; i++)
+		dev_data->grc.param_set_by_user[i] = 0;
+}
+
+/* Assign default GRC param values */
+static void qed_dbg_grc_set_params_default(struct qed_hwfn *p_hwfn)
+{
+	struct dbg_tools_data *dev_data = &p_hwfn->dbg_info;
+	u32 i;
+
+	for (i = 0; i < MAX_DBG_GRC_PARAMS; i++)
+		if (!dev_data->grc.param_set_by_user[i])
+			dev_data->grc.param_val[i] =
+			    s_grc_param_defs[i].default_val[dev_data->chip_id];
+}
+
+/* Returns true if the specified entity (indicated by GRC param) should be
+ * included in the dump, false otherwise.
+ */
+static bool qed_grc_is_included(struct qed_hwfn *p_hwfn,
+				enum dbg_grc_params grc_param)
+{
+	return qed_grc_get_param(p_hwfn, grc_param) > 0;
+}
+
+/* Returns true of the specified Storm should be included in the dump, false
+ * otherwise.
+ */
+static bool qed_grc_is_storm_included(struct qed_hwfn *p_hwfn,
+				      enum dbg_storms storm)
+{
+	return qed_grc_get_param(p_hwfn, (enum dbg_grc_params)storm) > 0;
+}
+
+/* Returns true if the specified memory should be included in the dump, false
+ * otherwise.
+ */
+static bool qed_grc_is_mem_included(struct qed_hwfn *p_hwfn,
+				    enum block_id block_id, u8 mem_group_id)
+{
+	u8 i;
+
+	/* Check Storm match */
+	if (s_block_defs[block_id]->associated_to_storm &&
+	    !qed_grc_is_storm_included(p_hwfn,
+			(enum dbg_storms)s_block_defs[block_id]->storm_id))
+		return false;
+
+	for (i = 0; i < NUM_BIG_RAM_TYPES; i++)
+		if (mem_group_id == s_big_ram_defs[i].mem_group_id ||
+		    mem_group_id == s_big_ram_defs[i].ram_mem_group_id)
+			return qed_grc_is_included(p_hwfn,
+						   s_big_ram_defs[i].grc_param);
+	if (mem_group_id == MEM_GROUP_PXP_ILT || mem_group_id ==
+	    MEM_GROUP_PXP_MEM)
+		return qed_grc_is_included(p_hwfn, DBG_GRC_PARAM_DUMP_PXP);
+	if (mem_group_id == MEM_GROUP_RAM)
+		return qed_grc_is_included(p_hwfn, DBG_GRC_PARAM_DUMP_RAM);
+	if (mem_group_id == MEM_GROUP_PBUF)
+		return qed_grc_is_included(p_hwfn, DBG_GRC_PARAM_DUMP_PBUF);
+	if (mem_group_id == MEM_GROUP_CAU_MEM ||
+	    mem_group_id == MEM_GROUP_CAU_SB ||
+	    mem_group_id == MEM_GROUP_CAU_PI)
+		return qed_grc_is_included(p_hwfn, DBG_GRC_PARAM_DUMP_CAU);
+	if (mem_group_id == MEM_GROUP_QM_MEM)
+		return qed_grc_is_included(p_hwfn, DBG_GRC_PARAM_DUMP_QM);
+	if (mem_group_id == MEM_GROUP_CONN_CFC_MEM ||
+	    mem_group_id == MEM_GROUP_TASK_CFC_MEM)
+		return qed_grc_is_included(p_hwfn, DBG_GRC_PARAM_DUMP_CFC);
+	if (mem_group_id == MEM_GROUP_IGU_MEM || mem_group_id ==
+	    MEM_GROUP_IGU_MSIX)
+		return qed_grc_is_included(p_hwfn, DBG_GRC_PARAM_DUMP_IGU);
+	if (mem_group_id == MEM_GROUP_MULD_MEM)
+		return qed_grc_is_included(p_hwfn, DBG_GRC_PARAM_DUMP_MULD);
+	if (mem_group_id == MEM_GROUP_PRS_MEM)
+		return qed_grc_is_included(p_hwfn, DBG_GRC_PARAM_DUMP_PRS);
+	if (mem_group_id == MEM_GROUP_DMAE_MEM)
+		return qed_grc_is_included(p_hwfn, DBG_GRC_PARAM_DUMP_DMAE);
+	if (mem_group_id == MEM_GROUP_TM_MEM)
+		return qed_grc_is_included(p_hwfn, DBG_GRC_PARAM_DUMP_TM);
+	if (mem_group_id == MEM_GROUP_SDM_MEM)
+		return qed_grc_is_included(p_hwfn, DBG_GRC_PARAM_DUMP_SDM);
+	if (mem_group_id == MEM_GROUP_TDIF_CTX || mem_group_id ==
+	    MEM_GROUP_RDIF_CTX)
+		return qed_grc_is_included(p_hwfn, DBG_GRC_PARAM_DUMP_DIF);
+	if (mem_group_id == MEM_GROUP_CM_MEM)
+		return qed_grc_is_included(p_hwfn, DBG_GRC_PARAM_DUMP_CM);
+	if (mem_group_id == MEM_GROUP_IOR)
+		return qed_grc_is_included(p_hwfn, DBG_GRC_PARAM_DUMP_IOR);
+
+	return true;
+}
+
+/* Stalls all Storms */
+static void qed_grc_stall_storms(struct qed_hwfn *p_hwfn,
+				 struct qed_ptt *p_ptt, bool stall)
+{
+	u8 reg_val = stall ? 1 : 0;
+	u8 storm_id;
+
+	for (storm_id = 0; storm_id < MAX_DBG_STORMS; storm_id++) {
+		if (qed_grc_is_storm_included(p_hwfn,
+					      (enum dbg_storms)storm_id)) {
+			u32 reg_addr =
+			    s_storm_defs[storm_id].sem_fast_mem_addr +
+			    SEM_FAST_REG_STALL_0;
+
+			qed_wr(p_hwfn, p_ptt, reg_addr, reg_val);
+		}
+	}
+
+	msleep(STALL_DELAY_MS);
+}
+
+/* Takes all blocks out of reset */
+static void qed_grc_unreset_blocks(struct qed_hwfn *p_hwfn,
+				   struct qed_ptt *p_ptt)
+{
+	struct dbg_tools_data *dev_data = &p_hwfn->dbg_info;
+	u32 reg_val[MAX_DBG_RESET_REGS] = { 0 };
+	u32 i;
+
+	/* Fill reset regs values */
+	for (i = 0; i < MAX_BLOCK_ID; i++)
+		if (s_block_defs[i]->has_reset_bit && s_block_defs[i]->unreset)
+			reg_val[s_block_defs[i]->reset_reg] |=
+			    BIT(s_block_defs[i]->reset_bit_offset);
+
+	/* Write reset registers */
+	for (i = 0; i < MAX_DBG_RESET_REGS; i++) {
+		if (s_reset_regs_defs[i].exists[dev_data->chip_id]) {
+			reg_val[i] |= s_reset_regs_defs[i].unreset_val;
+			if (reg_val[i])
+				qed_wr(p_hwfn,
+				       p_ptt,
+				       s_reset_regs_defs[i].addr +
+				       RESET_REG_UNRESET_OFFSET, reg_val[i]);
+		}
+	}
+}
+
+/* Returns the attention name offsets of the specified block */
+static const struct dbg_attn_block_type_data *
+qed_get_block_attn_data(enum block_id block_id, enum dbg_attn_type attn_type)
+{
+	const struct dbg_attn_block *base_attn_block_arr =
+		(const struct dbg_attn_block *)
+		s_dbg_arrays[BIN_BUF_DBG_ATTN_BLOCKS].ptr;
+
+	return &base_attn_block_arr[block_id].per_type_data[attn_type];
+}
+
+/* Returns the attention registers of the specified block */
+static const struct dbg_attn_reg *
+qed_get_block_attn_regs(enum block_id block_id, enum dbg_attn_type attn_type,
+			u8 *num_attn_regs)
+{
+	const struct dbg_attn_block_type_data *block_type_data =
+		qed_get_block_attn_data(block_id, attn_type);
+
+	*num_attn_regs = block_type_data->num_regs;
+	return &((const struct dbg_attn_reg *)
+		 s_dbg_arrays[BIN_BUF_DBG_ATTN_REGS].ptr)[block_type_data->
+							  regs_offset];
+}
+
+/* For each block, clear the status of all parities */
+static void qed_grc_clear_all_prty(struct qed_hwfn *p_hwfn,
+				   struct qed_ptt *p_ptt)
+{
+	struct dbg_tools_data *dev_data = &p_hwfn->dbg_info;
+	u8 reg_idx, num_attn_regs;
+	u32 block_id;
+
+	for (block_id = 0; block_id < MAX_BLOCK_ID; block_id++) {
+		const struct dbg_attn_reg *attn_reg_arr;
+
+		if (dev_data->block_in_reset[block_id])
+			continue;
+
+		attn_reg_arr = qed_get_block_attn_regs((enum block_id)block_id,
+						       ATTN_TYPE_PARITY,
+						       &num_attn_regs);
+		for (reg_idx = 0; reg_idx < num_attn_regs; reg_idx++) {
+			const struct dbg_attn_reg *reg_data =
+				&attn_reg_arr[reg_idx];
+
+			/* Check mode */
+			bool eval_mode = GET_FIELD(reg_data->mode.data,
+						   DBG_MODE_HDR_EVAL_MODE) > 0;
+			u16 modes_buf_offset =
+				GET_FIELD(reg_data->mode.data,
+					  DBG_MODE_HDR_MODES_BUF_OFFSET);
+
+			if (!eval_mode ||
+			    qed_is_mode_match(p_hwfn, &modes_buf_offset))
+				/* Mode match - read parity status read-clear
+				 * register.
+				 */
+				qed_rd(p_hwfn, p_ptt,
+				       DWORDS_TO_BYTES(reg_data->
+						       sts_clr_address));
+		}
+	}
+}
+
+/* Dumps GRC registers section header. Returns the dumped size in dwords.
+ * The following parameters are dumped:
+ * - 'count' = num_dumped_entries
+ * - 'split' = split_type
+ * - 'id'i = split_id (dumped only if split_id >= 0)
+ * - 'param_name' = param_val (user param, dumped only if param_name != NULL and
+ *	param_val != NULL)
+ */
+static u32 qed_grc_dump_regs_hdr(u32 *dump_buf,
+				 bool dump,
+				 u32 num_reg_entries,
+				 const char *split_type,
+				 int split_id,
+				 const char *param_name, const char *param_val)
+{
+	u8 num_params = 2 + (split_id >= 0 ? 1 : 0) + (param_name ? 1 : 0);
+	u32 offset = 0;
+
+	offset += qed_dump_section_hdr(dump_buf + offset,
+				       dump, "grc_regs", num_params);
+	offset += qed_dump_num_param(dump_buf + offset,
+				     dump, "count", num_reg_entries);
+	offset += qed_dump_str_param(dump_buf + offset,
+				     dump, "split", split_type);
+	if (split_id >= 0)
+		offset += qed_dump_num_param(dump_buf + offset,
+					     dump, "id", split_id);
+	if (param_name && param_val)
+		offset += qed_dump_str_param(dump_buf + offset,
+					     dump, param_name, param_val);
+	return offset;
+}
+
+/* Dumps GRC register/memory. Returns the dumped size in dwords. */
+static u32 qed_grc_dump_reg_entry(struct qed_hwfn *p_hwfn,
+				  struct qed_ptt *p_ptt, u32 *dump_buf,
+				  bool dump, u32 addr, u32 len)
+{
+	u32 offset = 0, i;
+
+	if (dump) {
+		*(dump_buf + offset++) = addr | (len << REG_DUMP_LEN_SHIFT);
+		for (i = 0; i < len; i++, addr++, offset++)
+			*(dump_buf + offset) = qed_rd(p_hwfn,
+						      p_ptt,
+						      DWORDS_TO_BYTES(addr));
+	} else {
+		offset += len + 1;
+	}
+
+	return offset;
+}
+
+/* Dumps GRC registers entries. Returns the dumped size in dwords. */
+static u32 qed_grc_dump_regs_entries(struct qed_hwfn *p_hwfn,
+				     struct qed_ptt *p_ptt,
+				     struct dbg_array input_regs_arr,
+				     u32 *dump_buf,
+				     bool dump,
+				     bool block_enable[MAX_BLOCK_ID],
+				     u32 *num_dumped_reg_entries)
+{
+	u32 i, offset = 0, input_offset = 0;
+	bool mode_match = true;
+
+	*num_dumped_reg_entries = 0;
+	while (input_offset < input_regs_arr.size_in_dwords) {
+		const struct dbg_dump_cond_hdr *cond_hdr =
+		    (const struct dbg_dump_cond_hdr *)
+		    &input_regs_arr.ptr[input_offset++];
+		bool eval_mode = GET_FIELD(cond_hdr->mode.data,
+					   DBG_MODE_HDR_EVAL_MODE) > 0;
+
+		/* Check mode/block */
+		if (eval_mode) {
+			u16 modes_buf_offset =
+				GET_FIELD(cond_hdr->mode.data,
+					  DBG_MODE_HDR_MODES_BUF_OFFSET);
+			mode_match = qed_is_mode_match(p_hwfn,
+						       &modes_buf_offset);
+		}
+
+		if (mode_match && block_enable[cond_hdr->block_id]) {
+			for (i = 0; i < cond_hdr->data_size;
+			     i++, input_offset++) {
+				const struct dbg_dump_reg *reg =
+				    (const struct dbg_dump_reg *)
+				    &input_regs_arr.ptr[input_offset];
+
+				offset +=
+					qed_grc_dump_reg_entry(p_hwfn, p_ptt,
+						    dump_buf + offset, dump,
+						    GET_FIELD(reg->data,
+							DBG_DUMP_REG_ADDRESS),
+						    GET_FIELD(reg->data,
+							DBG_DUMP_REG_LENGTH));
+				(*num_dumped_reg_entries)++;
+			}
+		} else {
+			input_offset += cond_hdr->data_size;
+		}
+	}
+
+	return offset;
+}
+
+/* Dumps GRC registers entries. Returns the dumped size in dwords. */
+static u32 qed_grc_dump_split_data(struct qed_hwfn *p_hwfn,
+				   struct qed_ptt *p_ptt,
+				   struct dbg_array input_regs_arr,
+				   u32 *dump_buf,
+				   bool dump,
+				   bool block_enable[MAX_BLOCK_ID],
+				   const char *split_type_name,
+				   u32 split_id,
+				   const char *param_name,
+				   const char *param_val)
+{
+	u32 num_dumped_reg_entries, offset;
+
+	/* Calculate register dump header size (and skip it for now) */
+	offset = qed_grc_dump_regs_hdr(dump_buf,
+				       false,
+				       0,
+				       split_type_name,
+				       split_id, param_name, param_val);
+
+	/* Dump registers */
+	offset += qed_grc_dump_regs_entries(p_hwfn,
+					    p_ptt,
+					    input_regs_arr,
+					    dump_buf + offset,
+					    dump,
+					    block_enable,
+					    &num_dumped_reg_entries);
+
+	/* Write register dump header */
+	if (dump && num_dumped_reg_entries > 0)
+		qed_grc_dump_regs_hdr(dump_buf,
+				      dump,
+				      num_dumped_reg_entries,
+				      split_type_name,
+				      split_id, param_name, param_val);
+
+	return num_dumped_reg_entries > 0 ? offset : 0;
+}
+
+/* Dumps registers according to the input registers array.
+ * Returns the dumped size in dwords.
+ */
+static u32 qed_grc_dump_registers(struct qed_hwfn *p_hwfn,
+				  struct qed_ptt *p_ptt,
+				  u32 *dump_buf,
+				  bool dump,
+				  bool block_enable[MAX_BLOCK_ID],
+				  const char *param_name, const char *param_val)
+{
+	struct dbg_tools_data *dev_data = &p_hwfn->dbg_info;
+	u32 offset = 0, input_offset = 0;
+	u8 port_id, pf_id;
+
+	if (dump)
+		DP_VERBOSE(p_hwfn, QED_MSG_DEBUG, "Dumping registers...\n");
+	while (input_offset <
+	       s_dbg_arrays[BIN_BUF_DBG_DUMP_REG].size_in_dwords) {
+		const struct dbg_dump_split_hdr *split_hdr =
+			(const struct dbg_dump_split_hdr *)
+			&s_dbg_arrays[BIN_BUF_DBG_DUMP_REG].ptr[input_offset++];
+		u8 split_type_id = GET_FIELD(split_hdr->hdr,
+					     DBG_DUMP_SPLIT_HDR_SPLIT_TYPE_ID);
+		u32 split_data_size = GET_FIELD(split_hdr->hdr,
+						DBG_DUMP_SPLIT_HDR_DATA_SIZE);
+		struct dbg_array curr_input_regs_arr = {
+			&s_dbg_arrays[BIN_BUF_DBG_DUMP_REG].ptr[input_offset],
+			split_data_size};
+
+		switch (split_type_id) {
+		case SPLIT_TYPE_NONE:
+		case SPLIT_TYPE_VF:
+			offset += qed_grc_dump_split_data(p_hwfn,
+							  p_ptt,
+							  curr_input_regs_arr,
+							  dump_buf + offset,
+							  dump,
+							  block_enable,
+							  "eng",
+							  (u32)(-1),
+							  param_name,
+							  param_val);
+			break;
+		case SPLIT_TYPE_PORT:
+			for (port_id = 0;
+			     port_id <
+			     s_chip_defs[dev_data->chip_id].
+			     per_platform[dev_data->platform_id].num_ports;
+			     port_id++) {
+				if (dump)
+					qed_port_pretend(p_hwfn, p_ptt,
+							 port_id);
+				offset +=
+				    qed_grc_dump_split_data(p_hwfn, p_ptt,
+							    curr_input_regs_arr,
+							    dump_buf + offset,
+							    dump, block_enable,
+							    "port", port_id,
+							    param_name,
+							    param_val);
+			}
+			break;
+		case SPLIT_TYPE_PF:
+		case SPLIT_TYPE_PORT_PF:
+			for (pf_id = 0;
+			     pf_id <
+			     s_chip_defs[dev_data->chip_id].
+			     per_platform[dev_data->platform_id].num_pfs;
+			     pf_id++) {
+				if (dump)
+					qed_fid_pretend(p_hwfn, p_ptt, pf_id);
+				offset += qed_grc_dump_split_data(p_hwfn,
+							p_ptt,
+							curr_input_regs_arr,
+							dump_buf + offset,
+							dump, block_enable,
+							"pf", pf_id, param_name,
+							param_val);
+			}
+			break;
+		default:
+			break;
+		}
+
+		input_offset += split_data_size;
+	}
+
+	/* Pretend to original PF */
+	if (dump)
+		qed_fid_pretend(p_hwfn, p_ptt, p_hwfn->rel_pf_id);
+	return offset;
+}
+
+/* Dump reset registers. Returns the dumped size in dwords. */
+static u32 qed_grc_dump_reset_regs(struct qed_hwfn *p_hwfn,
+				   struct qed_ptt *p_ptt,
+				   u32 *dump_buf, bool dump)
+{
+	struct dbg_tools_data *dev_data = &p_hwfn->dbg_info;
+	u32 i, offset = 0, num_regs = 0;
+
+	/* Calculate header size */
+	offset += qed_grc_dump_regs_hdr(dump_buf,
+					false, 0, "eng", -1, NULL, NULL);
+
+	/* Write reset registers */
+	for (i = 0; i < MAX_DBG_RESET_REGS; i++) {
+		if (s_reset_regs_defs[i].exists[dev_data->chip_id]) {
+			offset += qed_grc_dump_reg_entry(p_hwfn,
+							 p_ptt,
+							 dump_buf + offset,
+							 dump,
+							 BYTES_TO_DWORDS
+							 (s_reset_regs_defs
+							  [i].addr), 1);
+			num_regs++;
+		}
+	}
+
+	/* Write header */
+	if (dump)
+		qed_grc_dump_regs_hdr(dump_buf,
+				      true, num_regs, "eng", -1, NULL, NULL);
+	return offset;
+}
+
+/* Dump registers that are modified during GRC Dump and therefore must be dumped
+ * first. Returns the dumped size in dwords.
+ */
+static u32 qed_grc_dump_modified_regs(struct qed_hwfn *p_hwfn,
+				      struct qed_ptt *p_ptt,
+				      u32 *dump_buf, bool dump)
+{
+	struct dbg_tools_data *dev_data = &p_hwfn->dbg_info;
+	u32 offset = 0, num_reg_entries = 0, block_id;
+	u8 storm_id, reg_idx, num_attn_regs;
+
+	/* Calculate header size */
+	offset += qed_grc_dump_regs_hdr(dump_buf,
+					false, 0, "eng", -1, NULL, NULL);
+
+	/* Write parity registers */
+	for (block_id = 0; block_id < MAX_BLOCK_ID; block_id++) {
+		const struct dbg_attn_reg *attn_reg_arr;
+
+		if (dev_data->block_in_reset[block_id] && dump)
+			continue;
+
+		attn_reg_arr = qed_get_block_attn_regs((enum block_id)block_id,
+						       ATTN_TYPE_PARITY,
+						       &num_attn_regs);
+		for (reg_idx = 0; reg_idx < num_attn_regs; reg_idx++) {
+			const struct dbg_attn_reg *reg_data =
+				&attn_reg_arr[reg_idx];
+			u16 modes_buf_offset;
+			bool eval_mode;
+
+			/* Check mode */
+			eval_mode = GET_FIELD(reg_data->mode.data,
+					      DBG_MODE_HDR_EVAL_MODE) > 0;
+			modes_buf_offset =
+				GET_FIELD(reg_data->mode.data,
+					  DBG_MODE_HDR_MODES_BUF_OFFSET);
+			if (!eval_mode ||
+			    qed_is_mode_match(p_hwfn, &modes_buf_offset)) {
+				/* Mode match - read and dump registers */
+				offset += qed_grc_dump_reg_entry(p_hwfn,
+							p_ptt,
+							dump_buf + offset,
+							dump,
+							reg_data->mask_address,
+							1);
+				offset += qed_grc_dump_reg_entry(p_hwfn,
+						p_ptt,
+						dump_buf + offset,
+						dump,
+						GET_FIELD(reg_data->data,
+						    DBG_ATTN_REG_STS_ADDRESS),
+						1);
+				num_reg_entries += 2;
+			}
+		}
+	}
+
+	/* Write storm stall status registers */
+	for (storm_id = 0; storm_id < MAX_DBG_STORMS; storm_id++) {
+		if (dev_data->block_in_reset[s_storm_defs[storm_id].block_id] &&
+		    dump)
+			continue;
+
+		offset += qed_grc_dump_reg_entry(p_hwfn,
+					p_ptt,
+					dump_buf + offset,
+					dump,
+					BYTES_TO_DWORDS(s_storm_defs[storm_id].
+							sem_fast_mem_addr +
+							SEM_FAST_REG_STALLED),
+					1);
+		num_reg_entries++;
+	}
+
+	/* Write header */
+	if (dump)
+		qed_grc_dump_regs_hdr(dump_buf,
+				      true,
+				      num_reg_entries, "eng", -1, NULL, NULL);
+	return offset;
+}
+
+/* Dumps a GRC memory header (section and params).
+ * The following parameters are dumped:
+ * name - name is dumped only if it's not NULL.
+ * addr - byte_addr is dumped only if name is NULL.
+ * len - dword_len is always dumped.
+ * width - bit_width is dumped if it's not zero.
+ * packed - packed=1 is dumped if it's not false.
+ * mem_group - mem_group is always dumped.
+ * is_storm - true only if the memory is related to a Storm.
+ * storm_letter - storm letter (valid only if is_storm is true).
+ * Returns the dumped size in dwords.
+ */
+static u32 qed_grc_dump_mem_hdr(struct qed_hwfn *p_hwfn,
+				u32 *dump_buf,
+				bool dump,
+				const char *name,
+				u32 byte_addr,
+				u32 dword_len,
+				u32 bit_width,
+				bool packed,
+				const char *mem_group,
+				bool is_storm, char storm_letter)
+{
+	u8 num_params = 3;
+	u32 offset = 0;
+	char buf[64];
+
+	if (!dword_len)
+		DP_NOTICE(p_hwfn,
+			  "Unexpected GRC Dump error: dumped memory size must be non-zero\n");
+	if (bit_width)
+		num_params++;
+	if (packed)
+		num_params++;
+
+	/* Dump section header */
+	offset += qed_dump_section_hdr(dump_buf + offset,
+				       dump, "grc_mem", num_params);
+	if (name) {
+		/* Dump name */
+		if (is_storm) {
+			strcpy(buf, "?STORM_");
+			buf[0] = storm_letter;
+			strcpy(buf + strlen(buf), name);
+		} else {
+			strcpy(buf, name);
+		}
+
+		offset += qed_dump_str_param(dump_buf + offset,
+					     dump, "name", buf);
+		if (dump)
+			DP_VERBOSE(p_hwfn,
+				   QED_MSG_DEBUG,
+				   "Dumping %d registers from %s...\n",
+				   dword_len, buf);
+	} else {
+		/* Dump address */
+		offset += qed_dump_num_param(dump_buf + offset,
+					     dump, "addr", byte_addr);
+		if (dump && dword_len > 64)
+			DP_VERBOSE(p_hwfn,
+				   QED_MSG_DEBUG,
+				   "Dumping %d registers from address 0x%x...\n",
+				   dword_len, byte_addr);
+	}
+
+	/* Dump len */
+	offset += qed_dump_num_param(dump_buf + offset, dump, "len", dword_len);
+
+	/* Dump bit width */
+	if (bit_width)
+		offset += qed_dump_num_param(dump_buf + offset,
+					     dump, "width", bit_width);
+
+	/* Dump packed */
+	if (packed)
+		offset += qed_dump_num_param(dump_buf + offset,
+					     dump, "packed", 1);
+
+	/* Dump reg type */
+	if (is_storm) {
+		strcpy(buf, "?STORM_");
+		buf[0] = storm_letter;
+		strcpy(buf + strlen(buf), mem_group);
+	} else {
+		strcpy(buf, mem_group);
+	}
+
+	offset += qed_dump_str_param(dump_buf + offset, dump, "type", buf);
+	return offset;
+}
+
+/* Dumps a single GRC memory. If name is NULL, the memory is stored by address.
+ * Returns the dumped size in dwords.
+ */
+static u32 qed_grc_dump_mem(struct qed_hwfn *p_hwfn,
+			    struct qed_ptt *p_ptt,
+			    u32 *dump_buf,
+			    bool dump,
+			    const char *name,
+			    u32 byte_addr,
+			    u32 dword_len,
+			    u32 bit_width,
+			    bool packed,
+			    const char *mem_group,
+			    bool is_storm, char storm_letter)
+{
+	u32 offset = 0;
+
+	offset += qed_grc_dump_mem_hdr(p_hwfn,
+				       dump_buf + offset,
+				       dump,
+				       name,
+				       byte_addr,
+				       dword_len,
+				       bit_width,
+				       packed,
+				       mem_group, is_storm, storm_letter);
+	if (dump) {
+		u32 i;
+
+		for (i = 0; i < dword_len;
+		     i++, byte_addr += BYTES_IN_DWORD, offset++)
+			*(dump_buf + offset) = qed_rd(p_hwfn, p_ptt, byte_addr);
+	} else {
+		offset += dword_len;
+	}
+
+	return offset;
+}
+
+/* Dumps GRC memories entries. Returns the dumped size in dwords. */
+static u32 qed_grc_dump_mem_entries(struct qed_hwfn *p_hwfn,
+				    struct qed_ptt *p_ptt,
+				    struct dbg_array input_mems_arr,
+				    u32 *dump_buf, bool dump)
+{
+	u32 i, offset = 0, input_offset = 0;
+	bool mode_match = true;
+
+	while (input_offset < input_mems_arr.size_in_dwords) {
+		const struct dbg_dump_cond_hdr *cond_hdr;
+		u32 num_entries;
+		bool eval_mode;
+
+		cond_hdr = (const struct dbg_dump_cond_hdr *)
+			   &input_mems_arr.ptr[input_offset++];
+		eval_mode = GET_FIELD(cond_hdr->mode.data,
+				      DBG_MODE_HDR_EVAL_MODE) > 0;
+
+		/* Check required mode */
+		if (eval_mode) {
+			u16 modes_buf_offset =
+				GET_FIELD(cond_hdr->mode.data,
+					  DBG_MODE_HDR_MODES_BUF_OFFSET);
+
+			mode_match = qed_is_mode_match(p_hwfn,
+						       &modes_buf_offset);
+		}
+
+		if (!mode_match) {
+			input_offset += cond_hdr->data_size;
+			continue;
+		}
+
+		num_entries = cond_hdr->data_size / MEM_DUMP_ENTRY_SIZE_DWORDS;
+		for (i = 0; i < num_entries;
+		     i++, input_offset += MEM_DUMP_ENTRY_SIZE_DWORDS) {
+			const struct dbg_dump_mem *mem =
+				(const struct dbg_dump_mem *)
+				&input_mems_arr.ptr[input_offset];
+			u8 mem_group_id;
+
+			mem_group_id = GET_FIELD(mem->dword0,
+						 DBG_DUMP_MEM_MEM_GROUP_ID);
+			if (mem_group_id >= MEM_GROUPS_NUM) {
+				DP_NOTICE(p_hwfn, "Invalid mem_group_id\n");
+				return 0;
+			}
+
+			if (qed_grc_is_mem_included(p_hwfn,
+					(enum block_id)cond_hdr->block_id,
+					mem_group_id)) {
+				u32 mem_byte_addr =
+					DWORDS_TO_BYTES(GET_FIELD(mem->dword0,
+							DBG_DUMP_MEM_ADDRESS));
+				u32 mem_len = GET_FIELD(mem->dword1,
+							DBG_DUMP_MEM_LENGTH);
+				char storm_letter = 'a';
+				bool is_storm = false;
+
+				/* Update memory length for CCFC/TCFC memories
+				 * according to number of LCIDs/LTIDs.
+				 */
+				if (mem_group_id == MEM_GROUP_CONN_CFC_MEM)
+					mem_len = qed_grc_get_param(p_hwfn,
+							DBG_GRC_PARAM_NUM_LCIDS)
+							* (mem_len / MAX_LCIDS);
+				else if (mem_group_id == MEM_GROUP_TASK_CFC_MEM)
+					mem_len = qed_grc_get_param(p_hwfn,
+							DBG_GRC_PARAM_NUM_LTIDS)
+							* (mem_len / MAX_LTIDS);
+
+				/* If memory is associated with Storm, update
+				 * Storm details.
+				 */
+				if (s_block_defs[cond_hdr->block_id]->
+							associated_to_storm) {
+					is_storm = true;
+					storm_letter =
+						s_storm_defs[s_block_defs[
+						cond_hdr->block_id]->
+						storm_id].letter;
+				}
+
+				/* Dump memory */
+				offset += qed_grc_dump_mem(p_hwfn, p_ptt,
+						dump_buf + offset, dump, NULL,
+						mem_byte_addr, mem_len, 0,
+						false,
+						s_mem_group_names[mem_group_id],
+						is_storm, storm_letter);
+				}
+			}
+	}
+
+	return offset;
+}
+
+/* Dumps GRC memories according to the input array dump_mem.
+ * Returns the dumped size in dwords.
+ */
+static u32 qed_grc_dump_memories(struct qed_hwfn *p_hwfn,
+				 struct qed_ptt *p_ptt,
+				 u32 *dump_buf, bool dump)
+{
+	u32 offset = 0, input_offset = 0;
+
+	while (input_offset <
+	       s_dbg_arrays[BIN_BUF_DBG_DUMP_MEM].size_in_dwords) {
+		const struct dbg_dump_split_hdr *split_hdr =
+			(const struct dbg_dump_split_hdr *)
+			&s_dbg_arrays[BIN_BUF_DBG_DUMP_MEM].ptr[input_offset++];
+		u8 split_type_id = GET_FIELD(split_hdr->hdr,
+					     DBG_DUMP_SPLIT_HDR_SPLIT_TYPE_ID);
+		u32 split_data_size = GET_FIELD(split_hdr->hdr,
+						DBG_DUMP_SPLIT_HDR_DATA_SIZE);
+		struct dbg_array curr_input_mems_arr = {
+			&s_dbg_arrays[BIN_BUF_DBG_DUMP_MEM].ptr[input_offset],
+			split_data_size};
+
+		switch (split_type_id) {
+		case SPLIT_TYPE_NONE:
+			offset += qed_grc_dump_mem_entries(p_hwfn,
+							   p_ptt,
+							   curr_input_mems_arr,
+							   dump_buf + offset,
+							   dump);
+			break;
+		default:
+			DP_NOTICE(p_hwfn,
+				  "Dumping split memories is currently not supported\n");
+			break;
+		}
+
+		input_offset += split_data_size;
+	}
+
+	return offset;
+}
+
+/* Dumps GRC context data for the specified Storm.
+ * Returns the dumped size in dwords.
+ */
+static u32 qed_grc_dump_ctx_data(struct qed_hwfn *p_hwfn,
+				 struct qed_ptt *p_ptt,
+				 u32 *dump_buf,
+				 bool dump,
+				 const char *name,
+				 u32 num_lids,
+				 u32 lid_size,
+				 u32 rd_reg_addr,
+				 u8 storm_id)
+{
+	u32 i, lid, total_size;
+	u32 offset = 0;
+
+	if (!lid_size)
+		return 0;
+	lid_size *= BYTES_IN_DWORD;
+	total_size = num_lids * lid_size;
+	offset += qed_grc_dump_mem_hdr(p_hwfn,
+				       dump_buf + offset,
+				       dump,
+				       name,
+				       0,
+				       total_size,
+				       lid_size * 32,
+				       false,
+				       name,
+				       true, s_storm_defs[storm_id].letter);
+
+	/* Dump context data */
+	if (dump) {
+		for (lid = 0; lid < num_lids; lid++) {
+			for (i = 0; i < lid_size; i++, offset++) {
+				qed_wr(p_hwfn,
+				       p_ptt,
+				       s_storm_defs[storm_id].cm_ctx_wr_addr,
+				       BIT(9) | lid);
+				*(dump_buf + offset) = qed_rd(p_hwfn,
+							      p_ptt,
+							      rd_reg_addr);
+			}
+		}
+	} else {
+		offset += total_size;
+	}
+
+	return offset;
+}
+
+/* Dumps GRC contexts. Returns the dumped size in dwords. */
+static u32 qed_grc_dump_ctx(struct qed_hwfn *p_hwfn,
+			    struct qed_ptt *p_ptt, u32 *dump_buf, bool dump)
+{
+	u32 offset = 0;
+	u8 storm_id;
+
+	for (storm_id = 0; storm_id < MAX_DBG_STORMS; storm_id++) {
+		if (!qed_grc_is_storm_included(p_hwfn,
+					       (enum dbg_storms)storm_id))
+			continue;
+
+		/* Dump Conn AG context size */
+		offset +=
+			qed_grc_dump_ctx_data(p_hwfn,
+					      p_ptt,
+					      dump_buf + offset,
+					      dump,
+					      "CONN_AG_CTX",
+					      qed_grc_get_param(p_hwfn,
+						    DBG_GRC_PARAM_NUM_LCIDS),
+					      s_storm_defs[storm_id].
+						    cm_conn_ag_ctx_lid_size,
+					      s_storm_defs[storm_id].
+						    cm_conn_ag_ctx_rd_addr,
+					      storm_id);
+
+		/* Dump Conn ST context size */
+		offset +=
+			qed_grc_dump_ctx_data(p_hwfn,
+					      p_ptt,
+					      dump_buf + offset,
+					      dump,
+					      "CONN_ST_CTX",
+					      qed_grc_get_param(p_hwfn,
+						    DBG_GRC_PARAM_NUM_LCIDS),
+					      s_storm_defs[storm_id].
+						    cm_conn_st_ctx_lid_size,
+					      s_storm_defs[storm_id].
+						    cm_conn_st_ctx_rd_addr,
+					      storm_id);
+
+		/* Dump Task AG context size */
+		offset +=
+			qed_grc_dump_ctx_data(p_hwfn,
+					      p_ptt,
+					      dump_buf + offset,
+					      dump,
+					      "TASK_AG_CTX",
+					      qed_grc_get_param(p_hwfn,
+						    DBG_GRC_PARAM_NUM_LTIDS),
+					      s_storm_defs[storm_id].
+						    cm_task_ag_ctx_lid_size,
+					      s_storm_defs[storm_id].
+						    cm_task_ag_ctx_rd_addr,
+					      storm_id);
+
+		/* Dump Task ST context size */
+		offset +=
+			qed_grc_dump_ctx_data(p_hwfn,
+					      p_ptt,
+					      dump_buf + offset,
+					      dump,
+					      "TASK_ST_CTX",
+					      qed_grc_get_param(p_hwfn,
+						    DBG_GRC_PARAM_NUM_LTIDS),
+					      s_storm_defs[storm_id].
+						    cm_task_st_ctx_lid_size,
+					      s_storm_defs[storm_id].
+						    cm_task_st_ctx_rd_addr,
+					      storm_id);
+	}
+
+	return offset;
+}
+
+/* Dumps GRC IORs data. Returns the dumped size in dwords. */
+static u32 qed_grc_dump_iors(struct qed_hwfn *p_hwfn,
+			     struct qed_ptt *p_ptt, u32 *dump_buf, bool dump)
+{
+	char buf[10] = "IOR_SET_?";
+	u8 storm_id, set_id;
+	u32 offset = 0;
+
+	for (storm_id = 0; storm_id < MAX_DBG_STORMS; storm_id++) {
+		if (qed_grc_is_storm_included(p_hwfn,
+					      (enum dbg_storms)storm_id)) {
+			for (set_id = 0; set_id < NUM_IOR_SETS; set_id++) {
+				u32 addr =
+				    s_storm_defs[storm_id].sem_fast_mem_addr +
+				    SEM_FAST_REG_STORM_REG_FILE +
+				    DWORDS_TO_BYTES(IOR_SET_OFFSET(set_id));
+
+				buf[strlen(buf) - 1] = '0' + set_id;
+				offset += qed_grc_dump_mem(p_hwfn,
+							   p_ptt,
+							   dump_buf + offset,
+							   dump,
+							   buf,
+							   addr,
+							   IORS_PER_SET,
+							   32,
+							   false,
+							   "ior",
+							   true,
+							   s_storm_defs
+							   [storm_id].letter);
+			}
+		}
+	}
+
+	return offset;
+}
+
+/* Dump VFC CAM. Returns the dumped size in dwords. */
+static u32 qed_grc_dump_vfc_cam(struct qed_hwfn *p_hwfn,
+				struct qed_ptt *p_ptt,
+				u32 *dump_buf, bool dump, u8 storm_id)
+{
+	u32 total_size = VFC_CAM_NUM_ROWS * VFC_CAM_RESP_DWORDS;
+	u32 cam_addr[VFC_CAM_ADDR_DWORDS] = { 0 };
+	u32 cam_cmd[VFC_CAM_CMD_DWORDS] = { 0 };
+	u32 offset = 0;
+	u32 row, i;
+
+	offset += qed_grc_dump_mem_hdr(p_hwfn,
+				       dump_buf + offset,
+				       dump,
+				       "vfc_cam",
+				       0,
+				       total_size,
+				       256,
+				       false,
+				       "vfc_cam",
+				       true, s_storm_defs[storm_id].letter);
+	if (dump) {
+		/* Prepare CAM address */
+		SET_VAR_FIELD(cam_addr, VFC_CAM_ADDR, OP, VFC_OPCODE_CAM_RD);
+		for (row = 0; row < VFC_CAM_NUM_ROWS;
+		     row++, offset += VFC_CAM_RESP_DWORDS) {
+			/* Write VFC CAM command */
+			SET_VAR_FIELD(cam_cmd, VFC_CAM_CMD, ROW, row);
+			ARR_REG_WR(p_hwfn,
+				   p_ptt,
+				   s_storm_defs[storm_id].sem_fast_mem_addr +
+				   SEM_FAST_REG_VFC_DATA_WR,
+				   cam_cmd, VFC_CAM_CMD_DWORDS);
+
+			/* Write VFC CAM address */
+			ARR_REG_WR(p_hwfn,
+				   p_ptt,
+				   s_storm_defs[storm_id].sem_fast_mem_addr +
+				   SEM_FAST_REG_VFC_ADDR,
+				   cam_addr, VFC_CAM_ADDR_DWORDS);
+
+			/* Read VFC CAM read response */
+			ARR_REG_RD(p_hwfn,
+				   p_ptt,
+				   s_storm_defs[storm_id].sem_fast_mem_addr +
+				   SEM_FAST_REG_VFC_DATA_RD,
+				   dump_buf + offset, VFC_CAM_RESP_DWORDS);
+		}
+	} else {
+		offset += total_size;
+	}
+
+	return offset;
+}
+
+/* Dump VFC RAM. Returns the dumped size in dwords. */
+static u32 qed_grc_dump_vfc_ram(struct qed_hwfn *p_hwfn,
+				struct qed_ptt *p_ptt,
+				u32 *dump_buf,
+				bool dump,
+				u8 storm_id, struct vfc_ram_defs *ram_defs)
+{
+	u32 total_size = ram_defs->num_rows * VFC_RAM_RESP_DWORDS;
+	u32 ram_addr[VFC_RAM_ADDR_DWORDS] = { 0 };
+	u32 ram_cmd[VFC_RAM_CMD_DWORDS] = { 0 };
+	u32 offset = 0;
+	u32 row, i;
+
+	offset += qed_grc_dump_mem_hdr(p_hwfn,
+				       dump_buf + offset,
+				       dump,
+				       ram_defs->mem_name,
+				       0,
+				       total_size,
+				       256,
+				       false,
+				       ram_defs->type_name,
+				       true, s_storm_defs[storm_id].letter);
+
+	/* Prepare RAM address */
+	SET_VAR_FIELD(ram_addr, VFC_RAM_ADDR, OP, VFC_OPCODE_RAM_RD);
+
+	if (!dump)
+		return offset + total_size;
+
+	for (row = ram_defs->base_row;
+	     row < ram_defs->base_row + ram_defs->num_rows;
+	     row++, offset += VFC_RAM_RESP_DWORDS) {
+		/* Write VFC RAM command */
+		ARR_REG_WR(p_hwfn,
+			   p_ptt,
+			   s_storm_defs[storm_id].sem_fast_mem_addr +
+			   SEM_FAST_REG_VFC_DATA_WR,
+			   ram_cmd, VFC_RAM_CMD_DWORDS);
+
+		/* Write VFC RAM address */
+		SET_VAR_FIELD(ram_addr, VFC_RAM_ADDR, ROW, row);
+		ARR_REG_WR(p_hwfn,
+			   p_ptt,
+			   s_storm_defs[storm_id].sem_fast_mem_addr +
+			   SEM_FAST_REG_VFC_ADDR,
+			   ram_addr, VFC_RAM_ADDR_DWORDS);
+
+		/* Read VFC RAM read response */
+		ARR_REG_RD(p_hwfn,
+			   p_ptt,
+			   s_storm_defs[storm_id].sem_fast_mem_addr +
+			   SEM_FAST_REG_VFC_DATA_RD,
+			   dump_buf + offset, VFC_RAM_RESP_DWORDS);
+	}
+
+	return offset;
+}
+
+/* Dumps GRC VFC data. Returns the dumped size in dwords. */
+static u32 qed_grc_dump_vfc(struct qed_hwfn *p_hwfn,
+			    struct qed_ptt *p_ptt, u32 *dump_buf, bool dump)
+{
+	struct dbg_tools_data *dev_data = &p_hwfn->dbg_info;
+	u8 storm_id, i;
+	u32 offset = 0;
+
+	for (storm_id = 0; storm_id < MAX_DBG_STORMS; storm_id++) {
+		if (qed_grc_is_storm_included(p_hwfn,
+					      (enum dbg_storms)storm_id) &&
+		    s_storm_defs[storm_id].has_vfc &&
+		    (storm_id != DBG_PSTORM_ID ||
+		     dev_data->platform_id == PLATFORM_ASIC)) {
+			/* Read CAM */
+			offset += qed_grc_dump_vfc_cam(p_hwfn,
+						       p_ptt,
+						       dump_buf + offset,
+						       dump, storm_id);
+
+			/* Read RAM */
+			for (i = 0; i < NUM_VFC_RAM_TYPES; i++)
+				offset += qed_grc_dump_vfc_ram(p_hwfn,
+							       p_ptt,
+							       dump_buf +
+							       offset,
+							       dump,
+							       storm_id,
+							       &s_vfc_ram_defs
+							       [i]);
+		}
+	}
+
+	return offset;
+}
+
+/* Dumps GRC RSS data. Returns the dumped size in dwords. */
+static u32 qed_grc_dump_rss(struct qed_hwfn *p_hwfn,
+			    struct qed_ptt *p_ptt, u32 *dump_buf, bool dump)
+{
+	struct dbg_tools_data *dev_data = &p_hwfn->dbg_info;
+	u32 offset = 0;
+	u8 rss_mem_id;
+
+	for (rss_mem_id = 0; rss_mem_id < NUM_RSS_MEM_TYPES; rss_mem_id++) {
+		struct rss_mem_defs *rss_defs = &s_rss_mem_defs[rss_mem_id];
+		u32 num_entries = rss_defs->num_entries[dev_data->chip_id];
+		u32 entry_width = rss_defs->entry_width[dev_data->chip_id];
+		u32 total_size = (num_entries * entry_width) / 32;
+		bool packed = (entry_width == 16);
+		u32 addr = rss_defs->addr;
+		u32 i, j;
+
+		offset += qed_grc_dump_mem_hdr(p_hwfn,
+					       dump_buf + offset,
+					       dump,
+					       rss_defs->mem_name,
+					       addr,
+					       total_size,
+					       entry_width,
+					       packed,
+					       rss_defs->type_name, false, 0);
+
+		if (!dump) {
+			offset += total_size;
+			continue;
+		}
+
+		/* Dump RSS data */
+		for (i = 0; i < BYTES_TO_DWORDS(total_size); i++, addr++) {
+			qed_wr(p_hwfn, p_ptt, RSS_REG_RSS_RAM_ADDR, addr);
+			for (j = 0; j < BYTES_IN_DWORD; j++, offset++)
+				*(dump_buf + offset) =
+					qed_rd(p_hwfn, p_ptt,
+					       RSS_REG_RSS_RAM_DATA +
+					       DWORDS_TO_BYTES(j));
+		}
+	}
+
+	return offset;
+}
+
+/* Dumps GRC Big RAM. Returns the dumped size in dwords. */
+static u32 qed_grc_dump_big_ram(struct qed_hwfn *p_hwfn,
+				struct qed_ptt *p_ptt,
+				u32 *dump_buf, bool dump, u8 big_ram_id)
+{
+	struct dbg_tools_data *dev_data = &p_hwfn->dbg_info;
+	char mem_name[12] = "???_BIG_RAM";
+	char type_name[8] = "???_RAM";
+	u32 ram_size, total_blocks;
+	u32 offset = 0, i, j;
+
+	total_blocks =
+		s_big_ram_defs[big_ram_id].num_of_blocks[dev_data->chip_id];
+	ram_size = total_blocks * BIG_RAM_BLOCK_SIZE_DWORDS;
+
+	strncpy(type_name, s_big_ram_defs[big_ram_id].instance_name,
+		strlen(s_big_ram_defs[big_ram_id].instance_name));
+	strncpy(mem_name, s_big_ram_defs[big_ram_id].instance_name,
+		strlen(s_big_ram_defs[big_ram_id].instance_name));
+
+	/* Dump memory header */
+	offset += qed_grc_dump_mem_hdr(p_hwfn,
+				       dump_buf + offset,
+				       dump,
+				       mem_name,
+				       0,
+				       ram_size,
+				       BIG_RAM_BLOCK_SIZE_BYTES * 8,
+				       false, type_name, false, 0);
+
+	if (!dump)
+		return offset + ram_size;
+
+	/* Read and dump Big RAM data */
+	for (i = 0; i < total_blocks / 2; i++) {
+		qed_wr(p_hwfn, p_ptt, s_big_ram_defs[big_ram_id].addr_reg_addr,
+		       i);
+		for (j = 0; j < 2 * BIG_RAM_BLOCK_SIZE_DWORDS; j++, offset++)
+			*(dump_buf + offset) = qed_rd(p_hwfn, p_ptt,
+						s_big_ram_defs[big_ram_id].
+							data_reg_addr +
+						DWORDS_TO_BYTES(j));
+	}
+
+	return offset;
+}
+
+static u32 qed_grc_dump_mcp(struct qed_hwfn *p_hwfn,
+			    struct qed_ptt *p_ptt, u32 *dump_buf, bool dump)
+{
+	bool block_enable[MAX_BLOCK_ID] = { 0 };
+	bool halted = false;
+	u32 offset = 0;
+
+	/* Halt MCP */
+	if (dump) {
+		halted = !qed_mcp_halt(p_hwfn, p_ptt);
+		if (!halted)
+			DP_NOTICE(p_hwfn, "MCP halt failed!\n");
+	}
+
+	/* Dump MCP scratchpad */
+	offset += qed_grc_dump_mem(p_hwfn,
+				   p_ptt,
+				   dump_buf + offset,
+				   dump,
+				   NULL,
+				   MCP_REG_SCRATCH,
+				   MCP_REG_SCRATCH_SIZE,
+				   0, false, "MCP", false, 0);
+
+	/* Dump MCP cpu_reg_file */
+	offset += qed_grc_dump_mem(p_hwfn,
+				   p_ptt,
+				   dump_buf + offset,
+				   dump,
+				   NULL,
+				   MCP_REG_CPU_REG_FILE,
+				   MCP_REG_CPU_REG_FILE_SIZE,
+				   0, false, "MCP", false, 0);
+
+	/* Dump MCP registers */
+	block_enable[BLOCK_MCP] = true;
+	offset += qed_grc_dump_registers(p_hwfn,
+					 p_ptt,
+					 dump_buf + offset,
+					 dump, block_enable, "block", "MCP");
+
+	/* Dump required non-MCP registers */
+	offset += qed_grc_dump_regs_hdr(dump_buf + offset,
+					dump, 1, "eng", -1, "block", "MCP");
+	offset += qed_grc_dump_reg_entry(p_hwfn,
+					 p_ptt,
+					 dump_buf + offset,
+					 dump,
+					 BYTES_TO_DWORDS
+					 (MISC_REG_SHARED_MEM_ADDR), 1);
+
+	/* Release MCP */
+	if (halted && qed_mcp_resume(p_hwfn, p_ptt))
+		DP_NOTICE(p_hwfn, "Failed to resume MCP after halt!\n");
+	return offset;
+}
+
+/* Dumps the tbus indirect memory for all PHYs. */
+static u32 qed_grc_dump_phy(struct qed_hwfn *p_hwfn,
+			    struct qed_ptt *p_ptt, u32 *dump_buf, bool dump)
+{
+	u32 offset = 0, tbus_lo_offset, tbus_hi_offset;
+	char mem_name[32];
+	u8 phy_id;
+
+	for (phy_id = 0; phy_id < ARRAY_SIZE(s_phy_defs); phy_id++) {
+		struct phy_defs *phy_defs = &s_phy_defs[phy_id];
+		int printed_chars;
+
+		printed_chars = snprintf(mem_name, sizeof(mem_name), "tbus_%s",
+					 phy_defs->phy_name);
+		if (printed_chars < 0 || printed_chars >= sizeof(mem_name))
+			DP_NOTICE(p_hwfn,
+				  "Unexpected debug error: invalid PHY memory name\n");
+		offset += qed_grc_dump_mem_hdr(p_hwfn,
+					       dump_buf + offset,
+					       dump,
+					       mem_name,
+					       0,
+					       PHY_DUMP_SIZE_DWORDS,
+					       16, true, mem_name, false, 0);
+		if (dump) {
+			u32 addr_lo_addr = phy_defs->base_addr +
+					   phy_defs->tbus_addr_lo_addr;
+			u32 addr_hi_addr = phy_defs->base_addr +
+					   phy_defs->tbus_addr_hi_addr;
+			u32 data_lo_addr = phy_defs->base_addr +
+					   phy_defs->tbus_data_lo_addr;
+			u32 data_hi_addr = phy_defs->base_addr +
+					   phy_defs->tbus_data_hi_addr;
+			u8 *bytes_buf = (u8 *)(dump_buf + offset);
+
+			for (tbus_hi_offset = 0;
+			     tbus_hi_offset < (NUM_PHY_TBUS_ADDRESSES >> 8);
+			     tbus_hi_offset++) {
+				qed_wr(p_hwfn,
+				       p_ptt, addr_hi_addr, tbus_hi_offset);
+				for (tbus_lo_offset = 0; tbus_lo_offset < 256;
+				     tbus_lo_offset++) {
+					qed_wr(p_hwfn,
+					       p_ptt,
+					       addr_lo_addr, tbus_lo_offset);
+					*(bytes_buf++) =
+						(u8)qed_rd(p_hwfn, p_ptt,
+							   data_lo_addr);
+					*(bytes_buf++) =
+						(u8)qed_rd(p_hwfn, p_ptt,
+							   data_hi_addr);
+				}
+			}
+		}
+
+		offset += PHY_DUMP_SIZE_DWORDS;
+	}
+
+	return offset;
+}
+
+static void qed_config_dbg_line(struct qed_hwfn *p_hwfn,
+				struct qed_ptt *p_ptt,
+				enum block_id block_id,
+				u8 line_id,
+				u8 cycle_en,
+				u8 right_shift, u8 force_valid, u8 force_frame)
+{
+	struct block_defs *p_block_defs = s_block_defs[block_id];
+
+	qed_wr(p_hwfn, p_ptt, p_block_defs->dbg_select_addr, line_id);
+	qed_wr(p_hwfn, p_ptt, p_block_defs->dbg_cycle_enable_addr, cycle_en);
+	qed_wr(p_hwfn, p_ptt, p_block_defs->dbg_shift_addr, right_shift);
+	qed_wr(p_hwfn, p_ptt, p_block_defs->dbg_force_valid_addr, force_valid);
+	qed_wr(p_hwfn, p_ptt, p_block_defs->dbg_force_frame_addr, force_frame);
+}
+
+/* Dumps Static Debug data. Returns the dumped size in dwords. */
+static u32 qed_grc_dump_static_debug(struct qed_hwfn *p_hwfn,
+				     struct qed_ptt *p_ptt,
+				     u32 *dump_buf, bool dump)
+{
+	u32 block_dwords = NUM_DBG_BUS_LINES * STATIC_DEBUG_LINE_DWORDS;
+	struct dbg_tools_data *dev_data = &p_hwfn->dbg_info;
+	u32 offset = 0, block_id, line_id, addr, i;
+	struct block_defs *p_block_defs;
+
+	if (dump) {
+		DP_VERBOSE(p_hwfn,
+			   QED_MSG_DEBUG, "Dumping static debug data...\n");
+
+		/* Disable all blocks debug output */
+		for (block_id = 0; block_id < MAX_BLOCK_ID; block_id++) {
+			p_block_defs = s_block_defs[block_id];
+
+			if (p_block_defs->has_dbg_bus[dev_data->chip_id])
+				qed_wr(p_hwfn, p_ptt,
+				       p_block_defs->dbg_cycle_enable_addr, 0);
+		}
+
+		qed_bus_reset_dbg_block(p_hwfn, p_ptt);
+		qed_bus_set_framing_mode(p_hwfn,
+					 p_ptt, DBG_BUS_FRAME_MODE_8HW_0ST);
+		qed_wr(p_hwfn,
+		       p_ptt, DBG_REG_DEBUG_TARGET, DBG_BUS_TARGET_ID_INT_BUF);
+		qed_wr(p_hwfn, p_ptt, DBG_REG_FULL_MODE, 1);
+		qed_bus_enable_dbg_block(p_hwfn, p_ptt, true);
+	}
+
+	/* Dump all static debug lines for each relevant block */
+	for (block_id = 0; block_id < MAX_BLOCK_ID; block_id++) {
+		p_block_defs = s_block_defs[block_id];
+
+		if (!p_block_defs->has_dbg_bus[dev_data->chip_id])
+			continue;
+
+		/* Dump static section params */
+		offset += qed_grc_dump_mem_hdr(p_hwfn,
+					       dump_buf + offset,
+					       dump,
+					       p_block_defs->name, 0,
+					       block_dwords, 32, false,
+					       "STATIC", false, 0);
+
+		if (dump && !dev_data->block_in_reset[block_id]) {
+			u8 dbg_client_id =
+				p_block_defs->dbg_client_id[dev_data->chip_id];
+
+			/* Enable block's client */
+			qed_bus_enable_clients(p_hwfn, p_ptt,
+					       BIT(dbg_client_id));
+
+			for (line_id = 0; line_id < NUM_DBG_BUS_LINES;
+			     line_id++) {
+				/* Configure debug line ID */
+				qed_config_dbg_line(p_hwfn,
+						    p_ptt,
+						    (enum block_id)block_id,
+						    (u8)line_id,
+						    0xf, 0, 0, 0);
+
+				/* Read debug line info */
+				for (i = 0, addr = DBG_REG_CALENDAR_OUT_DATA;
+				     i < STATIC_DEBUG_LINE_DWORDS;
+				     i++, offset++, addr += BYTES_IN_DWORD)
+					dump_buf[offset] = qed_rd(p_hwfn, p_ptt,
+								  addr);
+			}
+
+			/* Disable block's client and debug output */
+			qed_bus_enable_clients(p_hwfn, p_ptt, 0);
+			qed_wr(p_hwfn, p_ptt,
+			       p_block_defs->dbg_cycle_enable_addr, 0);
+		} else {
+			/* All lines are invalid - dump zeros */
+			if (dump)
+				memset(dump_buf + offset, 0,
+				       DWORDS_TO_BYTES(block_dwords));
+			offset += block_dwords;
+		}
+	}
+
+	if (dump) {
+		qed_bus_enable_dbg_block(p_hwfn, p_ptt, false);
+		qed_bus_enable_clients(p_hwfn, p_ptt, 0);
+	}
+
+	return offset;
+}
+
+/* Performs GRC Dump to the specified buffer.
+ * Returns the dumped size in dwords.
+ */
+static enum dbg_status qed_grc_dump(struct qed_hwfn *p_hwfn,
+				    struct qed_ptt *p_ptt,
+				    u32 *dump_buf,
+				    bool dump, u32 *num_dumped_dwords)
+{
+	struct dbg_tools_data *dev_data = &p_hwfn->dbg_info;
+	bool parities_masked = false;
+	u8 i, port_mode = 0;
+	u32 offset = 0;
+
+	/* Check if emulation platform */
+	*num_dumped_dwords = 0;
+
+	/* Fill GRC parameters that were not set by the user with their default
+	 * value.
+	 */
+	qed_dbg_grc_set_params_default(p_hwfn);
+
+	/* Find port mode */
+	if (dump) {
+		switch (qed_rd(p_hwfn, p_ptt, MISC_REG_PORT_MODE)) {
+		case 0:
+			port_mode = 1;
+			break;
+		case 1:
+			port_mode = 2;
+			break;
+		case 2:
+			port_mode = 4;
+			break;
+		}
+	}
+
+	/* Update reset state */
+	if (dump)
+		qed_update_blocks_reset_state(p_hwfn, p_ptt);
+
+	/* Dump global params */
+	offset += qed_dump_common_global_params(p_hwfn,
+						p_ptt,
+						dump_buf + offset, dump, 4);
+	offset += qed_dump_str_param(dump_buf + offset,
+				     dump, "dump-type", "grc-dump");
+	offset += qed_dump_num_param(dump_buf + offset,
+				     dump,
+				     "num-lcids",
+				     qed_grc_get_param(p_hwfn,
+						DBG_GRC_PARAM_NUM_LCIDS));
+	offset += qed_dump_num_param(dump_buf + offset,
+				     dump,
+				     "num-ltids",
+				     qed_grc_get_param(p_hwfn,
+						DBG_GRC_PARAM_NUM_LTIDS));
+	offset += qed_dump_num_param(dump_buf + offset,
+				     dump, "num-ports", port_mode);
+
+	/* Dump reset registers (dumped before taking blocks out of reset ) */
+	if (qed_grc_is_included(p_hwfn, DBG_GRC_PARAM_DUMP_REGS))
+		offset += qed_grc_dump_reset_regs(p_hwfn,
+						  p_ptt,
+						  dump_buf + offset, dump);
+
+	/* Take all blocks out of reset (using reset registers) */
+	if (dump) {
+		qed_grc_unreset_blocks(p_hwfn, p_ptt);
+		qed_update_blocks_reset_state(p_hwfn, p_ptt);
+	}
+
+	/* Disable all parities using MFW command */
+	if (dump) {
+		parities_masked = !qed_mcp_mask_parities(p_hwfn, p_ptt, 1);
+		if (!parities_masked) {
+			if (qed_grc_get_param
+			    (p_hwfn, DBG_GRC_PARAM_PARITY_SAFE))
+				return DBG_STATUS_MCP_COULD_NOT_MASK_PRTY;
+			else
+				DP_NOTICE(p_hwfn,
+					  "Failed to mask parities using MFW\n");
+		}
+	}
+
+	/* Dump modified registers (dumped before modifying them) */
+	if (qed_grc_is_included(p_hwfn, DBG_GRC_PARAM_DUMP_REGS))
+		offset += qed_grc_dump_modified_regs(p_hwfn,
+						     p_ptt,
+						     dump_buf + offset, dump);
+
+	/* Stall storms */
+	if (dump &&
+	    (qed_grc_is_included(p_hwfn,
+				 DBG_GRC_PARAM_DUMP_IOR) ||
+	     qed_grc_is_included(p_hwfn, DBG_GRC_PARAM_DUMP_VFC)))
+		qed_grc_stall_storms(p_hwfn, p_ptt, true);
+
+	/* Dump all regs  */
+	if (qed_grc_is_included(p_hwfn, DBG_GRC_PARAM_DUMP_REGS)) {
+		/* Dump all blocks except MCP */
+		bool block_enable[MAX_BLOCK_ID];
+
+		for (i = 0; i < MAX_BLOCK_ID; i++)
+			block_enable[i] = true;
+		block_enable[BLOCK_MCP] = false;
+		offset += qed_grc_dump_registers(p_hwfn,
+						 p_ptt,
+						 dump_buf +
+						 offset,
+						 dump,
+						 block_enable, NULL, NULL);
+	}
+
+	/* Dump memories */
+	offset += qed_grc_dump_memories(p_hwfn, p_ptt, dump_buf + offset, dump);
+
+	/* Dump MCP */
+	if (qed_grc_is_included(p_hwfn, DBG_GRC_PARAM_DUMP_MCP))
+		offset += qed_grc_dump_mcp(p_hwfn,
+					   p_ptt, dump_buf + offset, dump);
+
+	/* Dump context */
+	if (qed_grc_is_included(p_hwfn, DBG_GRC_PARAM_DUMP_CM_CTX))
+		offset += qed_grc_dump_ctx(p_hwfn,
+					   p_ptt, dump_buf + offset, dump);
+
+	/* Dump RSS memories */
+	if (qed_grc_is_included(p_hwfn, DBG_GRC_PARAM_DUMP_RSS))
+		offset += qed_grc_dump_rss(p_hwfn,
+					   p_ptt, dump_buf + offset, dump);
+
+	/* Dump Big RAM */
+	for (i = 0; i < NUM_BIG_RAM_TYPES; i++)
+		if (qed_grc_is_included(p_hwfn, s_big_ram_defs[i].grc_param))
+			offset += qed_grc_dump_big_ram(p_hwfn,
+						       p_ptt,
+						       dump_buf + offset,
+						       dump, i);
+
+	/* Dump IORs */
+	if (qed_grc_is_included(p_hwfn, DBG_GRC_PARAM_DUMP_IOR))
+		offset += qed_grc_dump_iors(p_hwfn,
+					    p_ptt, dump_buf + offset, dump);
+
+	/* Dump VFC */
+	if (qed_grc_is_included(p_hwfn, DBG_GRC_PARAM_DUMP_VFC))
+		offset += qed_grc_dump_vfc(p_hwfn,
+					   p_ptt, dump_buf + offset, dump);
+
+	/* Dump PHY tbus */
+	if (qed_grc_is_included(p_hwfn,
+				DBG_GRC_PARAM_DUMP_PHY) && dev_data->chip_id ==
+	    CHIP_K2 && dev_data->platform_id == PLATFORM_ASIC)
+		offset += qed_grc_dump_phy(p_hwfn,
+					   p_ptt, dump_buf + offset, dump);
+
+	/* Dump static debug data  */
+	if (qed_grc_is_included(p_hwfn,
+				DBG_GRC_PARAM_DUMP_STATIC) &&
+	    dev_data->bus.state == DBG_BUS_STATE_IDLE)
+		offset += qed_grc_dump_static_debug(p_hwfn,
+						    p_ptt,
+						    dump_buf + offset, dump);
+
+	/* Dump last section */
+	offset += qed_dump_last_section(dump_buf, offset, dump);
+	if (dump) {
+		/* Unstall storms */
+		if (qed_grc_get_param(p_hwfn, DBG_GRC_PARAM_UNSTALL))
+			qed_grc_stall_storms(p_hwfn, p_ptt, false);
+
+		/* Clear parity status */
+		qed_grc_clear_all_prty(p_hwfn, p_ptt);
+
+		/* Enable all parities using MFW command */
+		if (parities_masked)
+			qed_mcp_mask_parities(p_hwfn, p_ptt, 0);
+	}
+
+	*num_dumped_dwords = offset;
+
+	return DBG_STATUS_OK;
+}
+
+/* Writes the specified failing Idle Check rule to the specified buffer.
+ * Returns the dumped size in dwords.
+ */
+static u32 qed_idle_chk_dump_failure(struct qed_hwfn *p_hwfn,
+				     struct qed_ptt *p_ptt,
+				     u32 *
+				     dump_buf,
+				     bool dump,
+				     u16 rule_id,
+				     const struct dbg_idle_chk_rule *rule,
+				     u16 fail_entry_id, u32 *cond_reg_values)
+{
+	const union dbg_idle_chk_reg *regs = &((const union dbg_idle_chk_reg *)
+					       s_dbg_arrays
+					       [BIN_BUF_DBG_IDLE_CHK_REGS].
+					       ptr)[rule->reg_offset];
+	const struct dbg_idle_chk_cond_reg *cond_regs = &regs[0].cond_reg;
+	struct dbg_tools_data *dev_data = &p_hwfn->dbg_info;
+	struct dbg_idle_chk_result_hdr *hdr =
+		(struct dbg_idle_chk_result_hdr *)dump_buf;
+	const struct dbg_idle_chk_info_reg *info_regs =
+		&regs[rule->num_cond_regs].info_reg;
+	u32 next_reg_offset = 0, i, offset = 0;
+	u8 reg_id;
+
+	/* Dump rule data */
+	if (dump) {
+		memset(hdr, 0, sizeof(*hdr));
+		hdr->rule_id = rule_id;
+		hdr->mem_entry_id = fail_entry_id;
+		hdr->severity = rule->severity;
+		hdr->num_dumped_cond_regs = rule->num_cond_regs;
+	}
+
+	offset += IDLE_CHK_RESULT_HDR_DWORDS;
+
+	/* Dump condition register values */
+	for (reg_id = 0; reg_id < rule->num_cond_regs; reg_id++) {
+		const struct dbg_idle_chk_cond_reg *reg = &cond_regs[reg_id];
+
+		/* Write register header */
+		if (dump) {
+			struct dbg_idle_chk_result_reg_hdr *reg_hdr =
+			    (struct dbg_idle_chk_result_reg_hdr *)(dump_buf
+								   + offset);
+			offset += IDLE_CHK_RESULT_REG_HDR_DWORDS;
+			memset(reg_hdr, 0,
+			       sizeof(struct dbg_idle_chk_result_reg_hdr));
+			reg_hdr->start_entry = reg->start_entry;
+			reg_hdr->size = reg->entry_size;
+			SET_FIELD(reg_hdr->data,
+				  DBG_IDLE_CHK_RESULT_REG_HDR_IS_MEM,
+				  reg->num_entries > 1 || reg->start_entry > 0
+				  ? 1 : 0);
+			SET_FIELD(reg_hdr->data,
+				  DBG_IDLE_CHK_RESULT_REG_HDR_REG_ID, reg_id);
+
+			/* Write register values */
+			for (i = 0; i < reg_hdr->size;
+			     i++, next_reg_offset++, offset++)
+				dump_buf[offset] =
+				    cond_reg_values[next_reg_offset];
+		} else {
+			offset += IDLE_CHK_RESULT_REG_HDR_DWORDS +
+			    reg->entry_size;
+		}
+	}
+
+	/* Dump info register values */
+	for (reg_id = 0; reg_id < rule->num_info_regs; reg_id++) {
+		const struct dbg_idle_chk_info_reg *reg = &info_regs[reg_id];
+		u32 block_id;
+
+		if (!dump) {
+			offset += IDLE_CHK_RESULT_REG_HDR_DWORDS + reg->size;
+			continue;
+		}
+
+		/* Check if register's block is in reset */
+		block_id = GET_FIELD(reg->data, DBG_IDLE_CHK_INFO_REG_BLOCK_ID);
+		if (block_id >= MAX_BLOCK_ID) {
+			DP_NOTICE(p_hwfn, "Invalid block_id\n");
+			return 0;
+		}
+
+		if (!dev_data->block_in_reset[block_id]) {
+			bool eval_mode = GET_FIELD(reg->mode.data,
+						   DBG_MODE_HDR_EVAL_MODE) > 0;
+			bool mode_match = true;
+
+			/* Check mode */
+			if (eval_mode) {
+				u16 modes_buf_offset =
+					GET_FIELD(reg->mode.data,
+						DBG_MODE_HDR_MODES_BUF_OFFSET);
+				mode_match =
+					qed_is_mode_match(p_hwfn,
+							  &modes_buf_offset);
+			}
+
+			if (mode_match) {
+				u32 grc_addr =
+					DWORDS_TO_BYTES(GET_FIELD(reg->data,
+						DBG_IDLE_CHK_INFO_REG_ADDRESS));
+
+				/* Write register header */
+				struct dbg_idle_chk_result_reg_hdr *reg_hdr =
+					(struct dbg_idle_chk_result_reg_hdr *)
+					(dump_buf + offset);
+
+				offset += IDLE_CHK_RESULT_REG_HDR_DWORDS;
+				hdr->num_dumped_info_regs++;
+				memset(reg_hdr, 0, sizeof(*reg_hdr));
+				reg_hdr->size = reg->size;
+				SET_FIELD(reg_hdr->data,
+					DBG_IDLE_CHK_RESULT_REG_HDR_REG_ID,
+					rule->num_cond_regs + reg_id);
+
+				/* Write register values */
+				for (i = 0; i < reg->size;
+				     i++, offset++, grc_addr += 4)
+					dump_buf[offset] =
+						qed_rd(p_hwfn, p_ptt, grc_addr);
+				}
+			}
+	}
+
+	return offset;
+}
+
+/* Dumps idle check rule entries. Returns the dumped size in dwords. */
+static u32
+qed_idle_chk_dump_rule_entries(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt,
+			       u32 *dump_buf, bool dump,
+			       const struct dbg_idle_chk_rule *input_rules,
+			       u32 num_input_rules, u32 *num_failing_rules)
+{
+	struct dbg_tools_data *dev_data = &p_hwfn->dbg_info;
+	u32 cond_reg_values[IDLE_CHK_MAX_ENTRIES_SIZE];
+	u32 i, j, offset = 0;
+	u16 entry_id;
+	u8 reg_id;
+
+	*num_failing_rules = 0;
+	for (i = 0; i < num_input_rules; i++) {
+		const struct dbg_idle_chk_cond_reg *cond_regs;
+		const struct dbg_idle_chk_rule *rule;
+		const union dbg_idle_chk_reg *regs;
+		u16 num_reg_entries = 1;
+		bool check_rule = true;
+		const u32 *imm_values;
+
+		rule = &input_rules[i];
+		regs = &((const union dbg_idle_chk_reg *)
+			 s_dbg_arrays[BIN_BUF_DBG_IDLE_CHK_REGS].ptr)
+			[rule->reg_offset];
+		cond_regs = &regs[0].cond_reg;
+		imm_values = &s_dbg_arrays[BIN_BUF_DBG_IDLE_CHK_IMMS].ptr
+			     [rule->imm_offset];
+
+		/* Check if all condition register blocks are out of reset, and
+		 * find maximal number of entries (all condition registers that
+		 * are memories must have the same size, which is > 1).
+		 */
+		for (reg_id = 0; reg_id < rule->num_cond_regs && check_rule;
+		     reg_id++) {
+			u32 block_id = GET_FIELD(cond_regs[reg_id].data,
+						DBG_IDLE_CHK_COND_REG_BLOCK_ID);
+
+			if (block_id >= MAX_BLOCK_ID) {
+				DP_NOTICE(p_hwfn, "Invalid block_id\n");
+				return 0;
+			}
+
+			check_rule = !dev_data->block_in_reset[block_id];
+			if (cond_regs[reg_id].num_entries > num_reg_entries)
+				num_reg_entries = cond_regs[reg_id].num_entries;
+		}
+
+		if (!check_rule && dump)
+			continue;
+
+		/* Go over all register entries (number of entries is the same
+		 * for all condition registers).
+		 */
+		for (entry_id = 0; entry_id < num_reg_entries; entry_id++) {
+			/* Read current entry of all condition registers */
+			if (dump) {
+				u32 next_reg_offset = 0;
+
+				for (reg_id = 0;
+				     reg_id < rule->num_cond_regs;
+				     reg_id++) {
+					const struct dbg_idle_chk_cond_reg
+						*reg = &cond_regs[reg_id];
+
+					/* Find GRC address (if it's a memory,
+					 * the address of the specific entry is
+					 * calculated).
+					 */
+					u32 grc_addr =
+					   DWORDS_TO_BYTES(
+						GET_FIELD(reg->data,
+						    DBG_IDLE_CHK_COND_REG_ADDRESS));
+
+					if (reg->num_entries > 1 ||
+					    reg->start_entry > 0) {
+						u32 padded_entry_size =
+							reg->entry_size > 1 ?
+							roundup_pow_of_two
+							(reg->entry_size) : 1;
+
+						grc_addr +=
+							DWORDS_TO_BYTES(
+								(reg->start_entry +
+								entry_id)
+								* padded_entry_size);
+					}
+
+					/* Read registers */
+					if (next_reg_offset + reg->entry_size >=
+					    IDLE_CHK_MAX_ENTRIES_SIZE) {
+						DP_NOTICE(p_hwfn,
+							  "idle check registers entry is too large\n");
+						return 0;
+					}
+
+					for (j = 0; j < reg->entry_size;
+					     j++, next_reg_offset++,
+					     grc_addr += 4)
+					     cond_reg_values[next_reg_offset] =
+						qed_rd(p_hwfn, p_ptt, grc_addr);
+				}
+			}
+
+			/* Call rule's condition function - a return value of
+			 * true indicates failure.
+			 */
+			if ((*cond_arr[rule->cond_id])(cond_reg_values,
+						       imm_values) || !dump) {
+				offset +=
+					qed_idle_chk_dump_failure(p_hwfn,
+							p_ptt,
+							dump_buf + offset,
+							dump,
+							rule->rule_id,
+							rule,
+							entry_id,
+							cond_reg_values);
+				(*num_failing_rules)++;
+				break;
+			}
+		}
+	}
+
+	return offset;
+}
+
+/* Performs Idle Check Dump to the specified buffer.
+ * Returns the dumped size in dwords.
+ */
+static u32 qed_idle_chk_dump(struct qed_hwfn *p_hwfn,
+			     struct qed_ptt *p_ptt, u32 *dump_buf, bool dump)
+{
+	u32 offset = 0, input_offset = 0, num_failing_rules = 0;
+	u32 num_failing_rules_offset;
+
+	/* Dump global params */
+	offset += qed_dump_common_global_params(p_hwfn,
+						p_ptt,
+						dump_buf + offset, dump, 1);
+	offset += qed_dump_str_param(dump_buf + offset,
+				     dump, "dump-type", "idle-chk");
+
+	/* Dump idle check section header with a single parameter */
+	offset += qed_dump_section_hdr(dump_buf + offset, dump, "idle_chk", 1);
+	num_failing_rules_offset = offset;
+	offset += qed_dump_num_param(dump_buf + offset, dump, "num_rules", 0);
+	while (input_offset <
+	       s_dbg_arrays[BIN_BUF_DBG_IDLE_CHK_RULES].size_in_dwords) {
+		const struct dbg_idle_chk_cond_hdr *cond_hdr =
+			(const struct dbg_idle_chk_cond_hdr *)
+			&s_dbg_arrays[BIN_BUF_DBG_IDLE_CHK_RULES].ptr
+			[input_offset++];
+		bool eval_mode = GET_FIELD(cond_hdr->mode.data,
+					   DBG_MODE_HDR_EVAL_MODE) > 0;
+		bool mode_match = true;
+
+		/* Check mode */
+		if (eval_mode) {
+			u16 modes_buf_offset =
+				GET_FIELD(cond_hdr->mode.data,
+					  DBG_MODE_HDR_MODES_BUF_OFFSET);
+
+			mode_match = qed_is_mode_match(p_hwfn,
+						       &modes_buf_offset);
+		}
+
+		if (mode_match) {
+			u32 curr_failing_rules;
+
+			offset +=
+			    qed_idle_chk_dump_rule_entries(p_hwfn,
+				p_ptt,
+				dump_buf + offset,
+				dump,
+				(const struct dbg_idle_chk_rule *)
+				&s_dbg_arrays[BIN_BUF_DBG_IDLE_CHK_RULES].
+				ptr[input_offset],
+				cond_hdr->data_size / IDLE_CHK_RULE_SIZE_DWORDS,
+				&curr_failing_rules);
+			num_failing_rules += curr_failing_rules;
+		}
+
+		input_offset += cond_hdr->data_size;
+	}
+
+	/* Overwrite num_rules parameter */
+	if (dump)
+		qed_dump_num_param(dump_buf + num_failing_rules_offset,
+				   dump, "num_rules", num_failing_rules);
+
+	return offset;
+}
+
+/* Finds the meta data image in NVRAM. */
+static enum dbg_status qed_find_nvram_image(struct qed_hwfn *p_hwfn,
+					    struct qed_ptt *p_ptt,
+					    u32 image_type,
+					    u32 *nvram_offset_bytes,
+					    u32 *nvram_size_bytes)
+{
+	u32 ret_mcp_resp, ret_mcp_param, ret_txn_size;
+	struct mcp_file_att file_att;
+
+	/* Call NVRAM get file command */
+	if (qed_mcp_nvm_rd_cmd(p_hwfn, p_ptt, DRV_MSG_CODE_NVM_GET_FILE_ATT,
+			       image_type, &ret_mcp_resp, &ret_mcp_param,
+			       &ret_txn_size, (u32 *)&file_att) != 0)
+		return DBG_STATUS_NVRAM_GET_IMAGE_FAILED;
+
+	/* Check response */
+	if ((ret_mcp_resp & FW_MSG_CODE_MASK) != FW_MSG_CODE_NVM_OK)
+		return DBG_STATUS_NVRAM_GET_IMAGE_FAILED;
+
+	/* Update return values */
+	*nvram_offset_bytes = file_att.nvm_start_addr;
+	*nvram_size_bytes = file_att.len;
+	DP_VERBOSE(p_hwfn,
+		   QED_MSG_DEBUG,
+		   "find_nvram_image: found NVRAM image of type %d in NVRAM offset %d bytes with size %d bytes\n",
+		   image_type, *nvram_offset_bytes, *nvram_size_bytes);
+
+	/* Check alignment */
+	if (*nvram_size_bytes & 0x3)
+		return DBG_STATUS_NON_ALIGNED_NVRAM_IMAGE;
+	return DBG_STATUS_OK;
+}
+
+static enum dbg_status qed_nvram_read(struct qed_hwfn *p_hwfn,
+				      struct qed_ptt *p_ptt,
+				      u32 nvram_offset_bytes,
+				      u32 nvram_size_bytes, u32 *ret_buf)
+{
+	u32 ret_mcp_resp, ret_mcp_param, ret_read_size;
+	u32 bytes_to_copy, read_offset = 0;
+	s32 bytes_left = nvram_size_bytes;
+
+	DP_VERBOSE(p_hwfn,
+		   QED_MSG_DEBUG,
+		   "nvram_read: reading image of size %d bytes from NVRAM\n",
+		   nvram_size_bytes);
+	do {
+		bytes_to_copy =
+		    (bytes_left >
+		     MCP_DRV_NVM_BUF_LEN) ? MCP_DRV_NVM_BUF_LEN : bytes_left;
+
+		/* Call NVRAM read command */
+		if (qed_mcp_nvm_rd_cmd(p_hwfn, p_ptt,
+				       DRV_MSG_CODE_NVM_READ_NVRAM,
+				       (nvram_offset_bytes +
+					read_offset) |
+				       (bytes_to_copy <<
+					DRV_MB_PARAM_NVM_LEN_SHIFT),
+				       &ret_mcp_resp, &ret_mcp_param,
+				       &ret_read_size,
+				       (u32 *)((u8 *)ret_buf +
+					       read_offset)) != 0)
+			return DBG_STATUS_NVRAM_READ_FAILED;
+
+		/* Check response */
+		if ((ret_mcp_resp & FW_MSG_CODE_MASK) != FW_MSG_CODE_NVM_OK)
+			return DBG_STATUS_NVRAM_READ_FAILED;
+
+		/* Update read offset */
+		read_offset += ret_read_size;
+		bytes_left -= ret_read_size;
+	} while (bytes_left > 0);
+
+	return DBG_STATUS_OK;
+}
+
+/* Get info on the MCP Trace data in the scratchpad:
+ * - trace_data_grc_addr - the GRC address of the trace data
+ * - trace_data_size_bytes - the size in bytes of the MCP Trace data (without
+ *	the header)
+ */
+static enum dbg_status qed_mcp_trace_get_data_info(struct qed_hwfn *p_hwfn,
+						   struct qed_ptt *p_ptt,
+						   u32 *trace_data_grc_addr,
+						   u32 *trace_data_size_bytes)
+{
+	/* Read MCP trace section offsize structure from MCP scratchpad */
+	u32 spad_trace_offsize = qed_rd(p_hwfn,
+					p_ptt,
+					MCP_SPAD_TRACE_OFFSIZE_ADDR);
+	u32 signature;
+
+	/* Extract MCP trace section GRC address from offsize structure (within
+	 * scratchpad).
+	 */
+	*trace_data_grc_addr =
+		MCP_REG_SCRATCH + SECTION_OFFSET(spad_trace_offsize);
+
+	/* Read signature from MCP trace section */
+	signature = qed_rd(p_hwfn, p_ptt,
+			   *trace_data_grc_addr +
+			   offsetof(struct mcp_trace, signature));
+	if (signature != MFW_TRACE_SIGNATURE)
+		return DBG_STATUS_INVALID_TRACE_SIGNATURE;
+
+	/* Read trace size from MCP trace section */
+	*trace_data_size_bytes = qed_rd(p_hwfn,
+					p_ptt,
+					*trace_data_grc_addr +
+					offsetof(struct mcp_trace, size));
+	return DBG_STATUS_OK;
+}
+
+/* Reads MCP trace meta data image from NVRAM.
+ * - running_bundle_id (OUT) - the running bundle ID (invalid when loaded from
+ *	file)
+ * - trace_meta_offset_bytes (OUT) - the NVRAM offset in bytes in which the MCP
+ *	Trace meta data starts (invalid when loaded from file)
+ * - trace_meta_size_bytes (OUT) - the size in bytes of the MCP Trace meta data
+ */
+static enum dbg_status qed_mcp_trace_get_meta_info(struct qed_hwfn *p_hwfn,
+						   struct qed_ptt *p_ptt,
+						   u32 trace_data_size_bytes,
+						   u32 *running_bundle_id,
+						   u32 *trace_meta_offset_bytes,
+						   u32 *trace_meta_size_bytes)
+{
+	/* Read MCP trace section offsize structure from MCP scratchpad */
+	u32 spad_trace_offsize = qed_rd(p_hwfn,
+					p_ptt,
+					MCP_SPAD_TRACE_OFFSIZE_ADDR);
+
+	/* Find running bundle ID */
+	u32 running_mfw_addr =
+		MCP_REG_SCRATCH + SECTION_OFFSET(spad_trace_offsize) +
+		QED_SECTION_SIZE(spad_trace_offsize) + trace_data_size_bytes;
+	enum dbg_status status;
+	u32 nvram_image_type;
+
+	*running_bundle_id = qed_rd(p_hwfn, p_ptt, running_mfw_addr);
+	if (*running_bundle_id > 1)
+		return DBG_STATUS_INVALID_NVRAM_BUNDLE;
+
+	/* Find image in NVRAM */
+	nvram_image_type =
+	    (*running_bundle_id ==
+	     DIR_ID_1) ? NVM_TYPE_MFW_TRACE1 : NVM_TYPE_MFW_TRACE2;
+	status = qed_find_nvram_image(p_hwfn,
+				      p_ptt,
+				      nvram_image_type,
+				      trace_meta_offset_bytes,
+				      trace_meta_size_bytes);
+
+	return status;
+}
+
+/* Reads the MCP Trace data from the specified GRC address into the specified
+ * buffer.
+ */
+static void qed_mcp_trace_read_data(struct qed_hwfn *p_hwfn,
+				    struct qed_ptt *p_ptt,
+				    u32 grc_addr, u32 size_in_dwords, u32 *buf)
+{
+	u32 i;
+
+	DP_VERBOSE(p_hwfn,
+		   QED_MSG_DEBUG,
+		   "mcp_trace_read_data: reading trace data of size %d dwords from GRC address 0x%x\n",
+		   size_in_dwords, grc_addr);
+	for (i = 0; i < size_in_dwords; i++, grc_addr += BYTES_IN_DWORD)
+		buf[i] = qed_rd(p_hwfn, p_ptt, grc_addr);
+}
+
+/* Reads the MCP Trace meta data (from NVRAM or buffer) into the specified
+ * buffer.
+ */
+static enum dbg_status qed_mcp_trace_read_meta(struct qed_hwfn *p_hwfn,
+					       struct qed_ptt *p_ptt,
+					       u32 nvram_offset_in_bytes,
+					       u32 size_in_bytes, u32 *buf)
+{
+	u8 *byte_buf = (u8 *)buf;
+	u8 modules_num, i;
+	u32 signature;
+
+	/* Read meta data from NVRAM */
+	enum dbg_status status = qed_nvram_read(p_hwfn,
+						p_ptt,
+						nvram_offset_in_bytes,
+						size_in_bytes,
+						buf);
+
+	if (status != DBG_STATUS_OK)
+		return status;
+
+	/* Extract and check first signature */
+	signature = qed_read_unaligned_dword(byte_buf);
+	byte_buf += sizeof(u32);
+	if (signature != MCP_TRACE_META_IMAGE_SIGNATURE)
+		return DBG_STATUS_INVALID_TRACE_SIGNATURE;
+
+	/* Extract number of modules */
+	modules_num = *(byte_buf++);
+
+	/* Skip all modules */
+	for (i = 0; i < modules_num; i++) {
+		u8 module_len = *(byte_buf++);
+
+		byte_buf += module_len;
+	}
+
+	/* Extract and check second signature */
+	signature = qed_read_unaligned_dword(byte_buf);
+	byte_buf += sizeof(u32);
+	if (signature != MCP_TRACE_META_IMAGE_SIGNATURE)
+		return DBG_STATUS_INVALID_TRACE_SIGNATURE;
+	return DBG_STATUS_OK;
+}
+
+/* Dump MCP Trace */
+enum dbg_status qed_mcp_trace_dump(struct qed_hwfn *p_hwfn,
+				   struct qed_ptt *p_ptt,
+				   u32 *dump_buf,
+				   bool dump, u32 *num_dumped_dwords)
+{
+	u32 trace_data_grc_addr, trace_data_size_bytes, trace_data_size_dwords;
+	u32 trace_meta_size_dwords, running_bundle_id, offset = 0;
+	u32 trace_meta_offset_bytes, trace_meta_size_bytes;
+	enum dbg_status status;
+	int halted = 0;
+
+	*num_dumped_dwords = 0;
+
+	/* Get trace data info */
+	status = qed_mcp_trace_get_data_info(p_hwfn,
+					     p_ptt,
+					     &trace_data_grc_addr,
+					     &trace_data_size_bytes);
+	if (status != DBG_STATUS_OK)
+		return status;
+
+	/* Dump global params */
+	offset += qed_dump_common_global_params(p_hwfn,
+						p_ptt,
+						dump_buf + offset, dump, 1);
+	offset += qed_dump_str_param(dump_buf + offset,
+				     dump, "dump-type", "mcp-trace");
+
+	/* Halt MCP while reading from scratchpad so the read data will be
+	 * consistent if halt fails, MCP trace is taken anyway, with a small
+	 * risk that it may be corrupt.
+	 */
+	if (dump) {
+		halted = !qed_mcp_halt(p_hwfn, p_ptt);
+		if (!halted)
+			DP_NOTICE(p_hwfn, "MCP halt failed!\n");
+	}
+
+	/* Find trace data size */
+	trace_data_size_dwords =
+		DIV_ROUND_UP(trace_data_size_bytes + sizeof(struct mcp_trace),
+			     BYTES_IN_DWORD);
+
+	/* Dump trace data section header and param */
+	offset += qed_dump_section_hdr(dump_buf + offset,
+				       dump, "mcp_trace_data", 1);
+	offset += qed_dump_num_param(dump_buf + offset,
+				     dump, "size", trace_data_size_dwords);
+
+	/* Read trace data from scratchpad into dump buffer */
+	if (dump)
+		qed_mcp_trace_read_data(p_hwfn,
+					p_ptt,
+					trace_data_grc_addr,
+					trace_data_size_dwords,
+					dump_buf + offset);
+	offset += trace_data_size_dwords;
+
+	/* Resume MCP (only if halt succeeded) */
+	if (halted && qed_mcp_resume(p_hwfn, p_ptt) != 0)
+		DP_NOTICE(p_hwfn, "Failed to resume MCP after halt!\n");
+
+	/* Dump trace meta section header */
+	offset += qed_dump_section_hdr(dump_buf + offset,
+				       dump, "mcp_trace_meta", 1);
+
+	/* Read trace meta info */
+	status = qed_mcp_trace_get_meta_info(p_hwfn,
+					     p_ptt,
+					     trace_data_size_bytes,
+					     &running_bundle_id,
+					     &trace_meta_offset_bytes,
+					     &trace_meta_size_bytes);
+	if (status != DBG_STATUS_OK)
+		return status;
+
+	/* Dump trace meta size param (trace_meta_size_bytes is always
+	 * dword-aligned).
+	 */
+	trace_meta_size_dwords = BYTES_TO_DWORDS(trace_meta_size_bytes);
+	offset += qed_dump_num_param(dump_buf + offset,	dump, "size",
+				     trace_meta_size_dwords);
+
+	/* Read trace meta image into dump buffer */
+	if (dump) {
+		status = qed_mcp_trace_read_meta(p_hwfn,
+						p_ptt,
+						trace_meta_offset_bytes,
+						trace_meta_size_bytes,
+						dump_buf + offset);
+		if (status != DBG_STATUS_OK)
+			return status;
+	}
+
+	offset += trace_meta_size_dwords;
+
+	*num_dumped_dwords = offset;
+
+	return DBG_STATUS_OK;
+}
+
+/* Dump GRC FIFO */
+enum dbg_status qed_reg_fifo_dump(struct qed_hwfn *p_hwfn,
+				  struct qed_ptt *p_ptt,
+				  u32 *dump_buf,
+				  bool dump, u32 *num_dumped_dwords)
+{
+	u32 offset = 0, dwords_read, size_param_offset;
+	bool fifo_has_data;
+
+	*num_dumped_dwords = 0;
+
+	/* Dump global params */
+	offset += qed_dump_common_global_params(p_hwfn,
+						p_ptt,
+						dump_buf + offset, dump, 1);
+	offset += qed_dump_str_param(dump_buf + offset,
+				     dump, "dump-type", "reg-fifo");
+
+	/* Dump fifo data section header and param. The size param is 0 for now,
+	 * and is overwritten after reading the FIFO.
+	 */
+	offset += qed_dump_section_hdr(dump_buf + offset,
+				       dump, "reg_fifo_data", 1);
+	size_param_offset = offset;
+	offset += qed_dump_num_param(dump_buf + offset, dump, "size", 0);
+
+	if (!dump) {
+		/* FIFO max size is REG_FIFO_DEPTH_DWORDS. There is no way to
+		 * test how much data is available, except for reading it.
+		 */
+		offset += REG_FIFO_DEPTH_DWORDS;
+		*num_dumped_dwords = offset;
+		return DBG_STATUS_OK;
+	}
+
+	fifo_has_data = qed_rd(p_hwfn, p_ptt,
+			       GRC_REG_TRACE_FIFO_VALID_DATA) > 0;
+
+	/* Pull available data from fifo. Use DMAE since this is widebus memory
+	 * and must be accessed atomically. Test for dwords_read not passing
+	 * buffer size since more entries could be added to the buffer as we are
+	 * emptying it.
+	 */
+	for (dwords_read = 0;
+	     fifo_has_data && dwords_read < REG_FIFO_DEPTH_DWORDS;
+	     dwords_read += REG_FIFO_ELEMENT_DWORDS, offset +=
+	     REG_FIFO_ELEMENT_DWORDS) {
+		if (qed_dmae_grc2host(p_hwfn, p_ptt, GRC_REG_TRACE_FIFO,
+				      (u64)(uintptr_t)(&dump_buf[offset]),
+				      REG_FIFO_ELEMENT_DWORDS, 0))
+			return DBG_STATUS_DMAE_FAILED;
+		fifo_has_data = qed_rd(p_hwfn, p_ptt,
+				       GRC_REG_TRACE_FIFO_VALID_DATA) > 0;
+	}
+
+	qed_dump_num_param(dump_buf + size_param_offset, dump, "size",
+			   dwords_read);
+
+	*num_dumped_dwords = offset;
+	return DBG_STATUS_OK;
+}
+
+/* Dump IGU FIFO */
+enum dbg_status qed_igu_fifo_dump(struct qed_hwfn *p_hwfn,
+				  struct qed_ptt *p_ptt,
+				  u32 *dump_buf,
+				  bool dump, u32 *num_dumped_dwords)
+{
+	u32 offset = 0, dwords_read, size_param_offset;
+	bool fifo_has_data;
+
+	*num_dumped_dwords = 0;
+
+	/* Dump global params */
+	offset += qed_dump_common_global_params(p_hwfn,
+						p_ptt,
+						dump_buf + offset, dump, 1);
+	offset += qed_dump_str_param(dump_buf + offset,
+				     dump, "dump-type", "igu-fifo");
+
+	/* Dump fifo data section header and param. The size param is 0 for now,
+	 * and is overwritten after reading the FIFO.
+	 */
+	offset += qed_dump_section_hdr(dump_buf + offset,
+				       dump, "igu_fifo_data", 1);
+	size_param_offset = offset;
+	offset += qed_dump_num_param(dump_buf + offset, dump, "size", 0);
+
+	if (!dump) {
+		/* FIFO max size is IGU_FIFO_DEPTH_DWORDS. There is no way to
+		 * test how much data is available, except for reading it.
+		 */
+		offset += IGU_FIFO_DEPTH_DWORDS;
+		*num_dumped_dwords = offset;
+		return DBG_STATUS_OK;
+	}
+
+	fifo_has_data = qed_rd(p_hwfn, p_ptt,
+			       IGU_REG_ERROR_HANDLING_DATA_VALID) > 0;
+
+	/* Pull available data from fifo. Use DMAE since this is widebus memory
+	 * and must be accessed atomically. Test for dwords_read not passing
+	 * buffer size since more entries could be added to the buffer as we are
+	 * emptying it.
+	 */
+	for (dwords_read = 0;
+	     fifo_has_data && dwords_read < IGU_FIFO_DEPTH_DWORDS;
+	     dwords_read += IGU_FIFO_ELEMENT_DWORDS, offset +=
+	     IGU_FIFO_ELEMENT_DWORDS) {
+		if (qed_dmae_grc2host(p_hwfn, p_ptt,
+				      IGU_REG_ERROR_HANDLING_MEMORY,
+				      (u64)(uintptr_t)(&dump_buf[offset]),
+				      IGU_FIFO_ELEMENT_DWORDS, 0))
+			return DBG_STATUS_DMAE_FAILED;
+		fifo_has_data =	qed_rd(p_hwfn, p_ptt,
+				       IGU_REG_ERROR_HANDLING_DATA_VALID) > 0;
+	}
+
+	qed_dump_num_param(dump_buf + size_param_offset, dump, "size",
+			   dwords_read);
+
+	*num_dumped_dwords = offset;
+	return DBG_STATUS_OK;
+}
+
+/* Protection Override dump */
+enum dbg_status qed_protection_override_dump(struct qed_hwfn *p_hwfn,
+					     struct qed_ptt *p_ptt,
+					     u32 *dump_buf,
+					     bool dump, u32 *num_dumped_dwords)
+{
+	u32 offset = 0, size_param_offset, override_window_dwords;
+
+	*num_dumped_dwords = 0;
+
+	/* Dump global params */
+	offset += qed_dump_common_global_params(p_hwfn,
+						p_ptt,
+						dump_buf + offset, dump, 1);
+	offset += qed_dump_str_param(dump_buf + offset,
+				     dump, "dump-type", "protection-override");
+
+	/* Dump data section header and param. The size param is 0 for now, and
+	 * is overwritten after reading the data.
+	 */
+	offset += qed_dump_section_hdr(dump_buf + offset,
+				       dump, "protection_override_data", 1);
+	size_param_offset = offset;
+	offset += qed_dump_num_param(dump_buf + offset, dump, "size", 0);
+
+	if (!dump) {
+		offset += PROTECTION_OVERRIDE_DEPTH_DWORDS;
+		*num_dumped_dwords = offset;
+		return DBG_STATUS_OK;
+	}
+
+	/* Add override window info to buffer */
+	override_window_dwords =
+		qed_rd(p_hwfn, p_ptt,
+		       GRC_REG_NUMBER_VALID_OVERRIDE_WINDOW) *
+		       PROTECTION_OVERRIDE_ELEMENT_DWORDS;
+	if (qed_dmae_grc2host(p_hwfn, p_ptt,
+			      GRC_REG_PROTECTION_OVERRIDE_WINDOW,
+			      (u64)(uintptr_t)(dump_buf + offset),
+			      override_window_dwords, 0))
+		return DBG_STATUS_DMAE_FAILED;
+	offset += override_window_dwords;
+	qed_dump_num_param(dump_buf + size_param_offset, dump, "size",
+			   override_window_dwords);
+
+	*num_dumped_dwords = offset;
+	return DBG_STATUS_OK;
+}
+
+/* Performs FW Asserts Dump to the specified buffer.
+ * Returns the dumped size in dwords.
+ */
+static u32 qed_fw_asserts_dump(struct qed_hwfn *p_hwfn,
+			       struct qed_ptt *p_ptt, u32 *dump_buf, bool dump)
+{
+	struct dbg_tools_data *dev_data = &p_hwfn->dbg_info;
+	char storm_letter_str[2] = "?";
+	struct fw_info fw_info;
+	u32 offset = 0, i;
+	u8 storm_id;
+
+	/* Dump global params */
+	offset += qed_dump_common_global_params(p_hwfn,
+						p_ptt,
+						dump_buf + offset, dump, 1);
+	offset += qed_dump_str_param(dump_buf + offset,
+				     dump, "dump-type", "fw-asserts");
+	for (storm_id = 0; storm_id < MAX_DBG_STORMS; storm_id++) {
+		u32 fw_asserts_section_addr, next_list_idx_addr, next_list_idx,
+			last_list_idx, element_addr;
+
+		if (dev_data->block_in_reset[s_storm_defs[storm_id].block_id])
+			continue;
+
+		/* Read FW info for the current Storm */
+		qed_read_fw_info(p_hwfn, p_ptt, storm_id, &fw_info);
+
+		/* Dump FW Asserts section header and params */
+		storm_letter_str[0] = s_storm_defs[storm_id].letter;
+		offset += qed_dump_section_hdr(dump_buf + offset, dump,
+					       "fw_asserts", 2);
+		offset += qed_dump_str_param(dump_buf + offset, dump, "storm",
+					     storm_letter_str);
+		offset += qed_dump_num_param(dump_buf + offset, dump, "size",
+					     fw_info.fw_asserts_section.
+					     list_element_dword_size);
+
+		if (!dump) {
+			offset += fw_info.fw_asserts_section.
+				  list_element_dword_size;
+			continue;
+		}
+
+		/* Read and dump FW Asserts data */
+		fw_asserts_section_addr =
+			s_storm_defs[storm_id].sem_fast_mem_addr +
+			SEM_FAST_REG_INT_RAM +
+			RAM_LINES_TO_BYTES(fw_info.fw_asserts_section.
+					   section_ram_line_offset);
+		next_list_idx_addr =
+			fw_asserts_section_addr +
+			DWORDS_TO_BYTES(fw_info.fw_asserts_section.
+					list_next_index_dword_offset);
+		next_list_idx = qed_rd(p_hwfn, p_ptt, next_list_idx_addr);
+		last_list_idx = (next_list_idx > 0
+				 ? next_list_idx
+				 : fw_info.fw_asserts_section.list_num_elements)
+				- 1;
+		element_addr =
+			fw_asserts_section_addr +
+			DWORDS_TO_BYTES(fw_info.fw_asserts_section.
+					list_dword_offset) +
+			last_list_idx *
+			DWORDS_TO_BYTES(fw_info.fw_asserts_section.
+					list_element_dword_size);
+		for (i = 0;
+		     i < fw_info.fw_asserts_section.list_element_dword_size;
+		     i++, offset++, element_addr += BYTES_IN_DWORD)
+			dump_buf[offset] = qed_rd(p_hwfn, p_ptt, element_addr);
+	}
+
+	/* Dump last section */
+	offset += qed_dump_section_hdr(dump_buf + offset, dump, "last", 0);
+	return offset;
+}
+
+/***************************** Public Functions *******************************/
+
+enum dbg_status qed_dbg_set_bin_ptr(const u8 * const bin_ptr)
+{
+	/* Convert binary data to debug arrays */
+	u32 num_of_buffers = *(u32 *)bin_ptr;
+	struct bin_buffer_hdr *buf_array;
+	u8 buf_id;
+
+	buf_array = (struct bin_buffer_hdr *)((u32 *)bin_ptr + 1);
+
+	for (buf_id = 0; buf_id < num_of_buffers; buf_id++) {
+		s_dbg_arrays[buf_id].ptr =
+		    (u32 *)(bin_ptr + buf_array[buf_id].offset);
+		s_dbg_arrays[buf_id].size_in_dwords =
+		    BYTES_TO_DWORDS(buf_array[buf_id].length);
+	}
+
+	return DBG_STATUS_OK;
+}
+
+enum dbg_status qed_dbg_grc_get_dump_buf_size(struct qed_hwfn *p_hwfn,
+					      struct qed_ptt *p_ptt,
+					      u32 *buf_size)
+{
+	enum dbg_status status = qed_dbg_dev_init(p_hwfn, p_ptt);
+
+	*buf_size = 0;
+	if (status != DBG_STATUS_OK)
+		return status;
+	if (!s_dbg_arrays[BIN_BUF_DBG_MODE_TREE].ptr ||
+	    !s_dbg_arrays[BIN_BUF_DBG_DUMP_REG].ptr ||
+	    !s_dbg_arrays[BIN_BUF_DBG_DUMP_MEM].ptr ||
+	    !s_dbg_arrays[BIN_BUF_DBG_ATTN_BLOCKS].ptr ||
+	    !s_dbg_arrays[BIN_BUF_DBG_ATTN_REGS].ptr)
+		return DBG_STATUS_DBG_ARRAY_NOT_SET;
+	return qed_grc_dump(p_hwfn, p_ptt, NULL, false, buf_size);
+}
+
+enum dbg_status qed_dbg_grc_dump(struct qed_hwfn *p_hwfn,
+				 struct qed_ptt *p_ptt,
+				 u32 *dump_buf,
+				 u32 buf_size_in_dwords,
+				 u32 *num_dumped_dwords)
+{
+	u32 needed_buf_size_in_dwords;
+	enum dbg_status status;
+
+	status = qed_dbg_grc_get_dump_buf_size(p_hwfn, p_ptt,
+					       &needed_buf_size_in_dwords);
+
+	*num_dumped_dwords = 0;
+	if (status != DBG_STATUS_OK)
+		return status;
+	if (buf_size_in_dwords < needed_buf_size_in_dwords)
+		return DBG_STATUS_DUMP_BUF_TOO_SMALL;
+
+	/* GRC Dump */
+	status = qed_grc_dump(p_hwfn, p_ptt, dump_buf, true, num_dumped_dwords);
+
+	/* Clear all GRC params */
+	qed_dbg_grc_clear_params(p_hwfn);
+	return status;
+}
+
+enum dbg_status qed_dbg_idle_chk_get_dump_buf_size(struct qed_hwfn *p_hwfn,
+						   struct qed_ptt *p_ptt,
+						   u32 *buf_size)
+{
+	enum dbg_status status = qed_dbg_dev_init(p_hwfn, p_ptt);
+	struct dbg_tools_data *dev_data = &p_hwfn->dbg_info;
+
+	*buf_size = 0;
+	if (status != DBG_STATUS_OK)
+		return status;
+	if (!s_dbg_arrays[BIN_BUF_DBG_MODE_TREE].ptr ||
+	    !s_dbg_arrays[BIN_BUF_DBG_IDLE_CHK_REGS].ptr ||
+	    !s_dbg_arrays[BIN_BUF_DBG_IDLE_CHK_IMMS].ptr ||
+	    !s_dbg_arrays[BIN_BUF_DBG_IDLE_CHK_RULES].ptr)
+		return DBG_STATUS_DBG_ARRAY_NOT_SET;
+	if (!dev_data->idle_chk.buf_size_set) {
+		dev_data->idle_chk.buf_size = qed_idle_chk_dump(p_hwfn,
+								p_ptt,
+								NULL, false);
+		dev_data->idle_chk.buf_size_set = true;
+	}
+
+	*buf_size = dev_data->idle_chk.buf_size;
+	return DBG_STATUS_OK;
+}
+
+enum dbg_status qed_dbg_idle_chk_dump(struct qed_hwfn *p_hwfn,
+				      struct qed_ptt *p_ptt,
+				      u32 *dump_buf,
+				      u32 buf_size_in_dwords,
+				      u32 *num_dumped_dwords)
+{
+	u32 needed_buf_size_in_dwords;
+	enum dbg_status status;
+
+	status = qed_dbg_idle_chk_get_dump_buf_size(p_hwfn, p_ptt,
+						    &needed_buf_size_in_dwords);
+
+	*num_dumped_dwords = 0;
+	if (status != DBG_STATUS_OK)
+		return status;
+	if (buf_size_in_dwords < needed_buf_size_in_dwords)
+		return DBG_STATUS_DUMP_BUF_TOO_SMALL;
+
+	/* Update reset state */
+	qed_update_blocks_reset_state(p_hwfn, p_ptt);
+
+	/* Idle Check Dump */
+	*num_dumped_dwords = qed_idle_chk_dump(p_hwfn, p_ptt, dump_buf, true);
+	return DBG_STATUS_OK;
+}
+
+enum dbg_status qed_dbg_mcp_trace_get_dump_buf_size(struct qed_hwfn *p_hwfn,
+						    struct qed_ptt *p_ptt,
+						    u32 *buf_size)
+{
+	enum dbg_status status = qed_dbg_dev_init(p_hwfn, p_ptt);
+
+	*buf_size = 0;
+	if (status != DBG_STATUS_OK)
+		return status;
+	return qed_mcp_trace_dump(p_hwfn, p_ptt, NULL, false, buf_size);
+}
+
+enum dbg_status qed_dbg_mcp_trace_dump(struct qed_hwfn *p_hwfn,
+				       struct qed_ptt *p_ptt,
+				       u32 *dump_buf,
+				       u32 buf_size_in_dwords,
+				       u32 *num_dumped_dwords)
+{
+	u32 needed_buf_size_in_dwords;
+	enum dbg_status status;
+
+	status = qed_dbg_mcp_trace_get_dump_buf_size(p_hwfn, p_ptt,
+						&needed_buf_size_in_dwords);
+
+	if (status != DBG_STATUS_OK)
+		return status;
+	if (buf_size_in_dwords < needed_buf_size_in_dwords)
+		return DBG_STATUS_DUMP_BUF_TOO_SMALL;
+
+	/* Update reset state */
+	qed_update_blocks_reset_state(p_hwfn, p_ptt);
+
+	/* Perform dump */
+	return qed_mcp_trace_dump(p_hwfn,
+				  p_ptt, dump_buf, true, num_dumped_dwords);
+}
+
+enum dbg_status qed_dbg_reg_fifo_get_dump_buf_size(struct qed_hwfn *p_hwfn,
+						   struct qed_ptt *p_ptt,
+						   u32 *buf_size)
+{
+	enum dbg_status status = qed_dbg_dev_init(p_hwfn, p_ptt);
+
+	*buf_size = 0;
+	if (status != DBG_STATUS_OK)
+		return status;
+	return qed_reg_fifo_dump(p_hwfn, p_ptt, NULL, false, buf_size);
+}
+
+enum dbg_status qed_dbg_reg_fifo_dump(struct qed_hwfn *p_hwfn,
+				      struct qed_ptt *p_ptt,
+				      u32 *dump_buf,
+				      u32 buf_size_in_dwords,
+				      u32 *num_dumped_dwords)
+{
+	u32 needed_buf_size_in_dwords;
+	enum dbg_status status;
+
+	status = qed_dbg_reg_fifo_get_dump_buf_size(p_hwfn, p_ptt,
+						    &needed_buf_size_in_dwords);
+
+	*num_dumped_dwords = 0;
+	if (status != DBG_STATUS_OK)
+		return status;
+	if (buf_size_in_dwords < needed_buf_size_in_dwords)
+		return DBG_STATUS_DUMP_BUF_TOO_SMALL;
+
+	/* Update reset state */
+	qed_update_blocks_reset_state(p_hwfn, p_ptt);
+	return qed_reg_fifo_dump(p_hwfn,
+				 p_ptt, dump_buf, true, num_dumped_dwords);
+}
+
+enum dbg_status qed_dbg_igu_fifo_get_dump_buf_size(struct qed_hwfn *p_hwfn,
+						   struct qed_ptt *p_ptt,
+						   u32 *buf_size)
+{
+	enum dbg_status status = qed_dbg_dev_init(p_hwfn, p_ptt);
+
+	*buf_size = 0;
+	if (status != DBG_STATUS_OK)
+		return status;
+	return qed_igu_fifo_dump(p_hwfn, p_ptt, NULL, false, buf_size);
+}
+
+enum dbg_status qed_dbg_igu_fifo_dump(struct qed_hwfn *p_hwfn,
+				      struct qed_ptt *p_ptt,
+				      u32 *dump_buf,
+				      u32 buf_size_in_dwords,
+				      u32 *num_dumped_dwords)
+{
+	u32 needed_buf_size_in_dwords;
+	enum dbg_status status;
+
+	status = qed_dbg_igu_fifo_get_dump_buf_size(p_hwfn, p_ptt,
+						    &needed_buf_size_in_dwords);
+
+	*num_dumped_dwords = 0;
+	if (status != DBG_STATUS_OK)
+		return status;
+	if (buf_size_in_dwords < needed_buf_size_in_dwords)
+		return DBG_STATUS_DUMP_BUF_TOO_SMALL;
+
+	/* Update reset state */
+	qed_update_blocks_reset_state(p_hwfn, p_ptt);
+	return qed_igu_fifo_dump(p_hwfn,
+				 p_ptt, dump_buf, true, num_dumped_dwords);
+}
+
+enum dbg_status
+qed_dbg_protection_override_get_dump_buf_size(struct qed_hwfn *p_hwfn,
+					      struct qed_ptt *p_ptt,
+					      u32 *buf_size)
+{
+	enum dbg_status status = qed_dbg_dev_init(p_hwfn, p_ptt);
+
+	*buf_size = 0;
+	if (status != DBG_STATUS_OK)
+		return status;
+	return qed_protection_override_dump(p_hwfn,
+					    p_ptt, NULL, false, buf_size);
+}
+
+enum dbg_status qed_dbg_protection_override_dump(struct qed_hwfn *p_hwfn,
+						 struct qed_ptt *p_ptt,
+						 u32 *dump_buf,
+						 u32 buf_size_in_dwords,
+						 u32 *num_dumped_dwords)
+{
+	u32 needed_buf_size_in_dwords;
+	enum dbg_status status;
+
+	status = qed_dbg_protection_override_get_dump_buf_size(p_hwfn, p_ptt,
+						&needed_buf_size_in_dwords);
+
+	*num_dumped_dwords = 0;
+	if (status != DBG_STATUS_OK)
+		return status;
+	if (buf_size_in_dwords < needed_buf_size_in_dwords)
+		return DBG_STATUS_DUMP_BUF_TOO_SMALL;
+
+	/* Update reset state */
+	qed_update_blocks_reset_state(p_hwfn, p_ptt);
+	return qed_protection_override_dump(p_hwfn,
+					    p_ptt,
+					    dump_buf, true, num_dumped_dwords);
+}
+
+enum dbg_status qed_dbg_fw_asserts_get_dump_buf_size(struct qed_hwfn *p_hwfn,
+						     struct qed_ptt *p_ptt,
+						     u32 *buf_size)
+{
+	enum dbg_status status = qed_dbg_dev_init(p_hwfn, p_ptt);
+
+	*buf_size = 0;
+	if (status != DBG_STATUS_OK)
+		return status;
+
+	/* Update reset state */
+	qed_update_blocks_reset_state(p_hwfn, p_ptt);
+	*buf_size = qed_fw_asserts_dump(p_hwfn, p_ptt, NULL, false);
+	return DBG_STATUS_OK;
+}
+
+enum dbg_status qed_dbg_fw_asserts_dump(struct qed_hwfn *p_hwfn,
+					struct qed_ptt *p_ptt,
+					u32 *dump_buf,
+					u32 buf_size_in_dwords,
+					u32 *num_dumped_dwords)
+{
+	u32 needed_buf_size_in_dwords;
+	enum dbg_status status;
+
+	status = qed_dbg_fw_asserts_get_dump_buf_size(p_hwfn, p_ptt,
+						&needed_buf_size_in_dwords);
+
+	*num_dumped_dwords = 0;
+	if (status != DBG_STATUS_OK)
+		return status;
+	if (buf_size_in_dwords < needed_buf_size_in_dwords)
+		return DBG_STATUS_DUMP_BUF_TOO_SMALL;
+
+	*num_dumped_dwords = qed_fw_asserts_dump(p_hwfn, p_ptt, dump_buf, true);
+	return DBG_STATUS_OK;
+}
+
+/******************************* Data Types **********************************/
+
+struct mcp_trace_format {
+	u32 data;
+#define MCP_TRACE_FORMAT_MODULE_MASK	0x0000ffff
+#define MCP_TRACE_FORMAT_MODULE_SHIFT	0
+#define MCP_TRACE_FORMAT_LEVEL_MASK	0x00030000
+#define MCP_TRACE_FORMAT_LEVEL_SHIFT	16
+#define MCP_TRACE_FORMAT_P1_SIZE_MASK	0x000c0000
+#define MCP_TRACE_FORMAT_P1_SIZE_SHIFT	18
+#define MCP_TRACE_FORMAT_P2_SIZE_MASK	0x00300000
+#define MCP_TRACE_FORMAT_P2_SIZE_SHIFT	20
+#define MCP_TRACE_FORMAT_P3_SIZE_MASK	0x00c00000
+#define MCP_TRACE_FORMAT_P3_SIZE_SHIFT	22
+#define MCP_TRACE_FORMAT_LEN_MASK	0xff000000
+#define MCP_TRACE_FORMAT_LEN_SHIFT	24
+	char *format_str;
+};
+
+struct mcp_trace_meta {
+	u32 modules_num;
+	char **modules;
+	u32 formats_num;
+	struct mcp_trace_format *formats;
+};
+
+/* Reg fifo element */
+struct reg_fifo_element {
+	u64 data;
+#define REG_FIFO_ELEMENT_ADDRESS_SHIFT		0
+#define REG_FIFO_ELEMENT_ADDRESS_MASK		0x7fffff
+#define REG_FIFO_ELEMENT_ACCESS_SHIFT		23
+#define REG_FIFO_ELEMENT_ACCESS_MASK		0x1
+#define REG_FIFO_ELEMENT_PF_SHIFT		24
+#define REG_FIFO_ELEMENT_PF_MASK		0xf
+#define REG_FIFO_ELEMENT_VF_SHIFT		28
+#define REG_FIFO_ELEMENT_VF_MASK		0xff
+#define REG_FIFO_ELEMENT_PORT_SHIFT		36
+#define REG_FIFO_ELEMENT_PORT_MASK		0x3
+#define REG_FIFO_ELEMENT_PRIVILEGE_SHIFT	38
+#define REG_FIFO_ELEMENT_PRIVILEGE_MASK		0x3
+#define REG_FIFO_ELEMENT_PROTECTION_SHIFT	40
+#define REG_FIFO_ELEMENT_PROTECTION_MASK	0x7
+#define REG_FIFO_ELEMENT_MASTER_SHIFT		43
+#define REG_FIFO_ELEMENT_MASTER_MASK		0xf
+#define REG_FIFO_ELEMENT_ERROR_SHIFT		47
+#define REG_FIFO_ELEMENT_ERROR_MASK		0x1f
+};
+
+/* IGU fifo element */
+struct igu_fifo_element {
+	u32 dword0;
+#define IGU_FIFO_ELEMENT_DWORD0_FID_SHIFT		0
+#define IGU_FIFO_ELEMENT_DWORD0_FID_MASK		0xff
+#define IGU_FIFO_ELEMENT_DWORD0_IS_PF_SHIFT		8
+#define IGU_FIFO_ELEMENT_DWORD0_IS_PF_MASK		0x1
+#define IGU_FIFO_ELEMENT_DWORD0_SOURCE_SHIFT		9
+#define IGU_FIFO_ELEMENT_DWORD0_SOURCE_MASK		0xf
+#define IGU_FIFO_ELEMENT_DWORD0_ERR_TYPE_SHIFT		13
+#define IGU_FIFO_ELEMENT_DWORD0_ERR_TYPE_MASK		0xf
+#define IGU_FIFO_ELEMENT_DWORD0_CMD_ADDR_SHIFT		17
+#define IGU_FIFO_ELEMENT_DWORD0_CMD_ADDR_MASK		0x7fff
+	u32 dword1;
+	u32 dword2;
+#define IGU_FIFO_ELEMENT_DWORD12_IS_WR_CMD_SHIFT	0
+#define IGU_FIFO_ELEMENT_DWORD12_IS_WR_CMD_MASK		0x1
+#define IGU_FIFO_ELEMENT_DWORD12_WR_DATA_SHIFT		1
+#define IGU_FIFO_ELEMENT_DWORD12_WR_DATA_MASK		0xffffffff
+	u32 reserved;
+};
+
+struct igu_fifo_wr_data {
+	u32 data;
+#define IGU_FIFO_WR_DATA_PROD_CONS_SHIFT		0
+#define IGU_FIFO_WR_DATA_PROD_CONS_MASK			0xffffff
+#define IGU_FIFO_WR_DATA_UPDATE_FLAG_SHIFT		24
+#define IGU_FIFO_WR_DATA_UPDATE_FLAG_MASK		0x1
+#define IGU_FIFO_WR_DATA_EN_DIS_INT_FOR_SB_SHIFT	25
+#define IGU_FIFO_WR_DATA_EN_DIS_INT_FOR_SB_MASK		0x3
+#define IGU_FIFO_WR_DATA_SEGMENT_SHIFT			27
+#define IGU_FIFO_WR_DATA_SEGMENT_MASK			0x1
+#define IGU_FIFO_WR_DATA_TIMER_MASK_SHIFT		28
+#define IGU_FIFO_WR_DATA_TIMER_MASK_MASK		0x1
+#define IGU_FIFO_WR_DATA_CMD_TYPE_SHIFT			31
+#define IGU_FIFO_WR_DATA_CMD_TYPE_MASK			0x1
+};
+
+struct igu_fifo_cleanup_wr_data {
+	u32 data;
+#define IGU_FIFO_CLEANUP_WR_DATA_RESERVED_SHIFT		0
+#define IGU_FIFO_CLEANUP_WR_DATA_RESERVED_MASK		0x7ffffff
+#define IGU_FIFO_CLEANUP_WR_DATA_CLEANUP_VAL_SHIFT	27
+#define IGU_FIFO_CLEANUP_WR_DATA_CLEANUP_VAL_MASK	0x1
+#define IGU_FIFO_CLEANUP_WR_DATA_CLEANUP_TYPE_SHIFT	28
+#define IGU_FIFO_CLEANUP_WR_DATA_CLEANUP_TYPE_MASK	0x7
+#define IGU_FIFO_CLEANUP_WR_DATA_CMD_TYPE_SHIFT		31
+#define IGU_FIFO_CLEANUP_WR_DATA_CMD_TYPE_MASK		0x1
+};
+
+/* Protection override element */
+struct protection_override_element {
+	u64 data;
+#define PROTECTION_OVERRIDE_ELEMENT_ADDRESS_SHIFT		0
+#define PROTECTION_OVERRIDE_ELEMENT_ADDRESS_MASK		0x7fffff
+#define PROTECTION_OVERRIDE_ELEMENT_WINDOW_SIZE_SHIFT		23
+#define PROTECTION_OVERRIDE_ELEMENT_WINDOW_SIZE_MASK		0xffffff
+#define PROTECTION_OVERRIDE_ELEMENT_READ_SHIFT			47
+#define PROTECTION_OVERRIDE_ELEMENT_READ_MASK			0x1
+#define PROTECTION_OVERRIDE_ELEMENT_WRITE_SHIFT			48
+#define PROTECTION_OVERRIDE_ELEMENT_WRITE_MASK			0x1
+#define PROTECTION_OVERRIDE_ELEMENT_READ_PROTECTION_SHIFT	49
+#define PROTECTION_OVERRIDE_ELEMENT_READ_PROTECTION_MASK	0x7
+#define PROTECTION_OVERRIDE_ELEMENT_WRITE_PROTECTION_SHIFT	52
+#define PROTECTION_OVERRIDE_ELEMENT_WRITE_PROTECTION_MASK	0x7
+};
+
+enum igu_fifo_sources {
+	IGU_SRC_PXP0,
+	IGU_SRC_PXP1,
+	IGU_SRC_PXP2,
+	IGU_SRC_PXP3,
+	IGU_SRC_PXP4,
+	IGU_SRC_PXP5,
+	IGU_SRC_PXP6,
+	IGU_SRC_PXP7,
+	IGU_SRC_CAU,
+	IGU_SRC_ATTN,
+	IGU_SRC_GRC
+};
+
+enum igu_fifo_addr_types {
+	IGU_ADDR_TYPE_MSIX_MEM,
+	IGU_ADDR_TYPE_WRITE_PBA,
+	IGU_ADDR_TYPE_WRITE_INT_ACK,
+	IGU_ADDR_TYPE_WRITE_ATTN_BITS,
+	IGU_ADDR_TYPE_READ_INT,
+	IGU_ADDR_TYPE_WRITE_PROD_UPDATE,
+	IGU_ADDR_TYPE_RESERVED
+};
+
+struct igu_fifo_addr_data {
+	u16 start_addr;
+	u16 end_addr;
+	char *desc;
+	char *vf_desc;
+	enum igu_fifo_addr_types type;
+};
+
+/******************************** Constants **********************************/
+
+#define MAX_MSG_LEN				1024
+#define MCP_TRACE_MAX_MODULE_LEN		8
+#define MCP_TRACE_FORMAT_MAX_PARAMS		3
+#define MCP_TRACE_FORMAT_PARAM_WIDTH \
+	(MCP_TRACE_FORMAT_P2_SIZE_SHIFT - MCP_TRACE_FORMAT_P1_SIZE_SHIFT)
+#define REG_FIFO_ELEMENT_ADDR_FACTOR		4
+#define REG_FIFO_ELEMENT_IS_PF_VF_VAL		127
+#define PROTECTION_OVERRIDE_ELEMENT_ADDR_FACTOR	4
+
+/********************************* Macros ************************************/
+
+#define BYTES_TO_DWORDS(bytes)			((bytes) / BYTES_IN_DWORD)
+
+/***************************** Constant Arrays *******************************/
+
+/* Status string array */
+static const char * const s_status_str[] = {
+	"Operation completed successfully",
+	"Debug application version wasn't set",
+	"Unsupported debug application version",
+	"The debug block wasn't reset since the last recording",
+	"Invalid arguments",
+	"The debug output was already set",
+	"Invalid PCI buffer size",
+	"PCI buffer allocation failed",
+	"A PCI buffer wasn't allocated",
+	"Too many inputs were enabled. Enabled less inputs, or set 'unifyInputs' to true",
+	"GRC/Timestamp input overlap in cycle dword 0",
+	"Cannot record Storm data since the entire recording cycle is used by HW",
+	"The Storm was already enabled",
+	"The specified Storm wasn't enabled",
+	"The block was already enabled",
+	"The specified block wasn't enabled",
+	"No input was enabled for recording",
+	"Filters and triggers are not allowed when recording in 64b units",
+	"The filter was already enabled",
+	"The trigger was already enabled",
+	"The trigger wasn't enabled",
+	"A constraint can be added only after a filter was enabled or a trigger state was added",
+	"Cannot add more than 3 trigger states",
+	"Cannot add more than 4 constraints per filter or trigger state",
+	"The recording wasn't started",
+	"A trigger was configured, but it didn't trigger",
+	"No data was recorded",
+	"Dump buffer is too small",
+	"Dumped data is not aligned to chunks",
+	"Unknown chip",
+	"Failed allocating virtual memory",
+	"The input block is in reset",
+	"Invalid MCP trace signature found in NVRAM",
+	"Invalid bundle ID found in NVRAM",
+	"Failed getting NVRAM image",
+	"NVRAM image is not dword-aligned",
+	"Failed reading from NVRAM",
+	"Idle check parsing failed",
+	"MCP Trace data is corrupt",
+	"Dump doesn't contain meta data - it must be provided in an image file",
+	"Failed to halt MCP",
+	"Failed to resume MCP after halt",
+	"DMAE transaction failed",
+	"Failed to empty SEMI sync FIFO",
+	"IGU FIFO data is corrupt",
+	"MCP failed to mask parities",
+	"FW Asserts parsing failed",
+	"GRC FIFO data is corrupt",
+	"Protection Override data is corrupt",
+	"Debug arrays were not set (when using binary files, dbg_set_bin_ptr must be called)",
+	"When a block is filtered, no other blocks can be recorded unless inputs are unified (due to a HW bug)"
+};
+
+/* Idle check severity names array */
+static const char * const s_idle_chk_severity_str[] = {
+	"Error",
+	"Error if no traffic",
+	"Warning"
+};
+
+/* MCP Trace level names array */
+static const char * const s_mcp_trace_level_str[] = {
+	"ERROR",
+	"TRACE",
+	"DEBUG"
+};
+
+/* Parsing strings */
+static const char * const s_access_strs[] = {
+	"read",
+	"write"
+};
+
+static const char * const s_privilege_strs[] = {
+	"VF",
+	"PDA",
+	"HV",
+	"UA"
+};
+
+static const char * const s_protection_strs[] = {
+	"(default)",
+	"(default)",
+	"(default)",
+	"(default)",
+	"override VF",
+	"override PDA",
+	"override HV",
+	"override UA"
+};
+
+static const char * const s_master_strs[] = {
+	"???",
+	"pxp",
+	"mcp",
+	"msdm",
+	"psdm",
+	"ysdm",
+	"usdm",
+	"tsdm",
+	"xsdm",
+	"dbu",
+	"dmae",
+	"???",
+	"???",
+	"???",
+	"???",
+	"???"
+};
+
+static const char * const s_reg_fifo_error_strs[] = {
+	"grc timeout",
+	"address doesn't belong to any block",
+	"reserved address in block or write to read-only address",
+	"privilege/protection mismatch",
+	"path isolation error"
+};
+
+static const char * const s_igu_fifo_source_strs[] = {
+	"TSTORM",
+	"MSTORM",
+	"USTORM",
+	"XSTORM",
+	"YSTORM",
+	"PSTORM",
+	"PCIE",
+	"NIG_QM_PBF",
+	"CAU",
+	"ATTN",
+	"GRC",
+};
+
+static const char * const s_igu_fifo_error_strs[] = {
+	"no error",
+	"length error",
+	"function disabled",
+	"VF sent command to attnetion address",
+	"host sent prod update command",
+	"read of during interrupt register while in MIMD mode",
+	"access to PXP BAR reserved address",
+	"producer update command to attention index",
+	"unknown error",
+	"SB index not valid",
+	"SB relative index and FID not found",
+	"FID not match",
+	"command with error flag asserted (PCI error or CAU discard)",
+	"VF sent cleanup and RF cleanup is disabled",
+	"cleanup command on type bigger than 4"
+};
+
+/* IGU FIFO address data */
+static const struct igu_fifo_addr_data s_igu_fifo_addr_data[] = {
+	{0x0, 0x101, "MSI-X Memory", NULL, IGU_ADDR_TYPE_MSIX_MEM},
+	{0x102, 0x1ff, "reserved", NULL, IGU_ADDR_TYPE_RESERVED},
+	{0x200, 0x200, "Write PBA[0:63]", NULL, IGU_ADDR_TYPE_WRITE_PBA},
+	{0x201, 0x201, "Write PBA[64:127]", "reserved",
+	 IGU_ADDR_TYPE_WRITE_PBA},
+	{0x202, 0x202, "Write PBA[128]", "reserved", IGU_ADDR_TYPE_WRITE_PBA},
+	{0x203, 0x3ff, "reserved", NULL, IGU_ADDR_TYPE_RESERVED},
+	{0x400, 0x5ef, "Write interrupt acknowledgment", NULL,
+	 IGU_ADDR_TYPE_WRITE_INT_ACK},
+	{0x5f0, 0x5f0, "Attention bits update", NULL,
+	 IGU_ADDR_TYPE_WRITE_ATTN_BITS},
+	{0x5f1, 0x5f1, "Attention bits set", NULL,
+	 IGU_ADDR_TYPE_WRITE_ATTN_BITS},
+	{0x5f2, 0x5f2, "Attention bits clear", NULL,
+	 IGU_ADDR_TYPE_WRITE_ATTN_BITS},
+	{0x5f3, 0x5f3, "Read interrupt 0:63 with mask", NULL,
+	 IGU_ADDR_TYPE_READ_INT},
+	{0x5f4, 0x5f4, "Read interrupt 0:31 with mask", NULL,
+	 IGU_ADDR_TYPE_READ_INT},
+	{0x5f5, 0x5f5, "Read interrupt 32:63 with mask", NULL,
+	 IGU_ADDR_TYPE_READ_INT},
+	{0x5f6, 0x5f6, "Read interrupt 0:63 without mask", NULL,
+	 IGU_ADDR_TYPE_READ_INT},
+	{0x5f7, 0x5ff, "reserved", NULL, IGU_ADDR_TYPE_RESERVED},
+	{0x600, 0x7ff, "Producer update", NULL, IGU_ADDR_TYPE_WRITE_PROD_UPDATE}
+};
+
+/******************************** Variables **********************************/
+
+/* MCP Trace meta data - used in case the dump doesn't contain the meta data
+ * (e.g. due to no NVRAM access).
+ */
+static struct dbg_array s_mcp_trace_meta = { NULL, 0 };
+
+/* Temporary buffer, used for print size calculations */
+static char s_temp_buf[MAX_MSG_LEN];
+
+/***************************** Public Functions *******************************/
+
+enum dbg_status qed_dbg_user_set_bin_ptr(const u8 * const bin_ptr)
+{
+	/* Convert binary data to debug arrays */
+	u32 num_of_buffers = *(u32 *)bin_ptr;
+	struct bin_buffer_hdr *buf_array;
+	u8 buf_id;
+
+	buf_array = (struct bin_buffer_hdr *)((u32 *)bin_ptr + 1);
+
+	for (buf_id = 0; buf_id < num_of_buffers; buf_id++) {
+		s_dbg_arrays[buf_id].ptr =
+		    (u32 *)(bin_ptr + buf_array[buf_id].offset);
+		s_dbg_arrays[buf_id].size_in_dwords =
+		    BYTES_TO_DWORDS(buf_array[buf_id].length);
+	}
+
+	return DBG_STATUS_OK;
+}
+
+static u32 qed_cyclic_add(u32 a, u32 b, u32 size)
+{
+	return (a + b) % size;
+}
+
+static u32 qed_cyclic_sub(u32 a, u32 b, u32 size)
+{
+	return (size + a - b) % size;
+}
+
+/* Reads the specified number of bytes from the specified cyclic buffer (up to 4
+ * bytes) and returns them as a dword value. the specified buffer offset is
+ * updated.
+ */
+static u32 qed_read_from_cyclic_buf(void *buf,
+				    u32 *offset,
+				    u32 buf_size, u8 num_bytes_to_read)
+{
+	u8 *bytes_buf = (u8 *)buf;
+	u8 *val_ptr;
+	u32 val = 0;
+	u8 i;
+
+	val_ptr = (u8 *)&val;
+
+	for (i = 0; i < num_bytes_to_read; i++) {
+		val_ptr[i] = bytes_buf[*offset];
+		*offset = qed_cyclic_add(*offset, 1, buf_size);
+	}
+
+	return val;
+}
+
+/* Reads and returns the next byte from the specified buffer.
+ * The specified buffer offset is updated.
+ */
+static u8 qed_read_byte_from_buf(void *buf, u32 *offset)
+{
+	return ((u8 *)buf)[(*offset)++];
+}
+
+/* Reads and returns the next dword from the specified buffer.
+ * The specified buffer offset is updated.
+ */
+static u32 qed_read_dword_from_buf(void *buf, u32 *offset)
+{
+	u32 dword_val = *(u32 *)&((u8 *)buf)[*offset];
+
+	*offset += 4;
+	return dword_val;
+}
+
+/* Reads the next string from the specified buffer, and copies it to the
+ * specified pointer. The specified buffer offset is updated.
+ */
+static void qed_read_str_from_buf(void *buf, u32 *offset, u32 size, char *dest)
+{
+	const char *source_str = &((const char *)buf)[*offset];
+
+	strncpy(dest, source_str, size);
+	dest[size - 1] = '\0';
+	*offset += size;
+}
+
+/* Returns a pointer to the specified offset (in bytes) of the specified buffer.
+ * If the specified buffer in NULL, a temporary buffer pointer is returned.
+ */
+static char *qed_get_buf_ptr(void *buf, u32 offset)
+{
+	return buf ? (char *)buf + offset : s_temp_buf;
+}
+
+/* Reads a param from the specified buffer. Returns the number of dwords read.
+ * If the returned str_param is NULL, the param is numeric and its value is
+ * returned in num_param.
+ * Otheriwise, the param is a string and its pointer is returned in str_param.
+ */
+static u32 qed_read_param(u32 *dump_buf,
+			  const char **param_name,
+			  const char **param_str_val, u32 *param_num_val)
+{
+	char *char_buf = (char *)dump_buf;
+	u32 offset = 0; /* In bytes */
+
+	/* Extract param name */
+	*param_name = char_buf;
+	offset += strlen(*param_name) + 1;
+
+	/* Check param type */
+	if (*(char_buf + offset++)) {
+		/* String param */
+		*param_str_val = char_buf + offset;
+		offset += strlen(*param_str_val) + 1;
+		if (offset & 0x3)
+			offset += (4 - (offset & 0x3));
+	} else {
+		/* Numeric param */
+		*param_str_val = NULL;
+		if (offset & 0x3)
+			offset += (4 - (offset & 0x3));
+		*param_num_val = *(u32 *)(char_buf + offset);
+		offset += 4;
+	}
+
+	return offset / 4;
+}
+
+/* Reads a section header from the specified buffer.
+ * Returns the number of dwords read.
+ */
+static u32 qed_read_section_hdr(u32 *dump_buf,
+				const char **section_name,
+				u32 *num_section_params)
+{
+	const char *param_str_val;
+
+	return qed_read_param(dump_buf,
+			      section_name, &param_str_val, num_section_params);
+}
+
+/* Reads section params from the specified buffer and prints them to the results
+ * buffer. Returns the number of dwords read.
+ */
+static u32 qed_print_section_params(u32 *dump_buf,
+				    u32 num_section_params,
+				    char *results_buf, u32 *num_chars_printed)
+{
+	u32 i, dump_offset = 0, results_offset = 0;
+
+	for (i = 0; i < num_section_params; i++) {
+		const char *param_name;
+		const char *param_str_val;
+		u32 param_num_val = 0;
+
+		dump_offset += qed_read_param(dump_buf + dump_offset,
+					      &param_name,
+					      &param_str_val, &param_num_val);
+		if (param_str_val)
+			/* String param */
+			results_offset +=
+				sprintf(qed_get_buf_ptr(results_buf,
+							results_offset),
+					"%s: %s\n", param_name, param_str_val);
+		else if (strcmp(param_name, "fw-timestamp"))
+			/* Numeric param */
+			results_offset +=
+				sprintf(qed_get_buf_ptr(results_buf,
+							results_offset),
+					"%s: %d\n", param_name, param_num_val);
+	}
+
+	results_offset +=
+	    sprintf(qed_get_buf_ptr(results_buf, results_offset), "\n");
+	*num_chars_printed = results_offset;
+	return dump_offset;
+}
+
+const char *qed_dbg_get_status_str(enum dbg_status status)
+{
+	return (status <
+		MAX_DBG_STATUS) ? s_status_str[status] : "Invalid debug status";
+}
+
+/* Parses the idle check rules and returns the number of characters printed.
+ * In case of parsing error, returns 0.
+ */
+static u32 qed_parse_idle_chk_dump_rules(struct qed_hwfn *p_hwfn,
+					 u32 *dump_buf,
+					 u32 *dump_buf_end,
+					 u32 num_rules,
+					 bool print_fw_idle_chk,
+					 char *results_buf,
+					 u32 *num_errors, u32 *num_warnings)
+{
+	u32 rule_idx, results_offset = 0; /* Offset in results_buf in bytes */
+	u16 i, j;
+
+	*num_errors = 0;
+	*num_warnings = 0;
+
+	/* Go over dumped results */
+	for (rule_idx = 0; rule_idx < num_rules && dump_buf < dump_buf_end;
+	     rule_idx++) {
+		const struct dbg_idle_chk_rule_parsing_data *rule_parsing_data;
+		struct dbg_idle_chk_result_hdr *hdr;
+		const char *parsing_str;
+		u32 parsing_str_offset;
+		const char *lsi_msg;
+		u8 curr_reg_id = 0;
+		bool has_fw_msg;
+
+		hdr = (struct dbg_idle_chk_result_hdr *)dump_buf;
+		rule_parsing_data =
+			(const struct dbg_idle_chk_rule_parsing_data *)
+			&s_dbg_arrays[BIN_BUF_DBG_IDLE_CHK_PARSING_DATA].
+			ptr[hdr->rule_id];
+		parsing_str_offset =
+			GET_FIELD(rule_parsing_data->data,
+				  DBG_IDLE_CHK_RULE_PARSING_DATA_STR_OFFSET);
+		has_fw_msg =
+			GET_FIELD(rule_parsing_data->data,
+				DBG_IDLE_CHK_RULE_PARSING_DATA_HAS_FW_MSG) > 0;
+		parsing_str = &((const char *)
+				s_dbg_arrays[BIN_BUF_DBG_PARSING_STRINGS].ptr)
+				[parsing_str_offset];
+		lsi_msg = parsing_str;
+
+		if (hdr->severity >= MAX_DBG_IDLE_CHK_SEVERITY_TYPES)
+			return 0;
+
+		/* Skip rule header */
+		dump_buf += (sizeof(struct dbg_idle_chk_result_hdr) / 4);
+
+		/* Update errors/warnings count */
+		if (hdr->severity == IDLE_CHK_SEVERITY_ERROR ||
+		    hdr->severity == IDLE_CHK_SEVERITY_ERROR_NO_TRAFFIC)
+			(*num_errors)++;
+		else
+			(*num_warnings)++;
+
+		/* Print rule severity */
+		results_offset +=
+		    sprintf(qed_get_buf_ptr(results_buf,
+					    results_offset), "%s: ",
+			    s_idle_chk_severity_str[hdr->severity]);
+
+		/* Print rule message */
+		if (has_fw_msg)
+			parsing_str += strlen(parsing_str) + 1;
+		results_offset +=
+		    sprintf(qed_get_buf_ptr(results_buf,
+					    results_offset), "%s.",
+			    has_fw_msg &&
+			    print_fw_idle_chk ? parsing_str : lsi_msg);
+		parsing_str += strlen(parsing_str) + 1;
+
+		/* Print register values */
+		results_offset +=
+		    sprintf(qed_get_buf_ptr(results_buf,
+					    results_offset), " Registers:");
+		for (i = 0;
+		     i < hdr->num_dumped_cond_regs + hdr->num_dumped_info_regs;
+		     i++) {
+			struct dbg_idle_chk_result_reg_hdr *reg_hdr
+			    = (struct dbg_idle_chk_result_reg_hdr *)
+			    dump_buf;
+			bool is_mem =
+				GET_FIELD(reg_hdr->data,
+					  DBG_IDLE_CHK_RESULT_REG_HDR_IS_MEM);
+			u8 reg_id =
+				GET_FIELD(reg_hdr->data,
+					  DBG_IDLE_CHK_RESULT_REG_HDR_REG_ID);
+
+			/* Skip reg header */
+			dump_buf +=
+			    (sizeof(struct dbg_idle_chk_result_reg_hdr) / 4);
+
+			/* Skip register names until the required reg_id is
+			 * reached.
+			 */
+			for (; reg_id > curr_reg_id;
+			     curr_reg_id++,
+			     parsing_str += strlen(parsing_str) + 1);
+
+			results_offset +=
+			    sprintf(qed_get_buf_ptr(results_buf,
+						    results_offset), " %s",
+				    parsing_str);
+			if (i < hdr->num_dumped_cond_regs && is_mem)
+				results_offset +=
+				    sprintf(qed_get_buf_ptr(results_buf,
+							    results_offset),
+					    "[%d]", hdr->mem_entry_id +
+					    reg_hdr->start_entry);
+			results_offset +=
+			    sprintf(qed_get_buf_ptr(results_buf,
+						    results_offset), "=");
+			for (j = 0; j < reg_hdr->size; j++, dump_buf++) {
+				results_offset +=
+				    sprintf(qed_get_buf_ptr(results_buf,
+							    results_offset),
+					    "0x%x", *dump_buf);
+				if (j < reg_hdr->size - 1)
+					results_offset +=
+					    sprintf(qed_get_buf_ptr
+						    (results_buf,
+						     results_offset), ",");
+			}
+		}
+
+		results_offset +=
+		    sprintf(qed_get_buf_ptr(results_buf, results_offset), "\n");
+	}
+
+	/* Check if end of dump buffer was exceeded */
+	if (dump_buf > dump_buf_end)
+		return 0;
+	return results_offset;
+}
+
+/* Parses an idle check dump buffer.
+ * If result_buf is not NULL, the idle check results are printed to it.
+ * In any case, the required results buffer size is assigned to
+ * parsed_results_bytes.
+ * The parsing status is returned.
+ */
+static enum dbg_status qed_parse_idle_chk_dump(struct qed_hwfn *p_hwfn,
+					       u32 *dump_buf,
+					       u32 num_dumped_dwords,
+					       char *results_buf,
+					       u32 *parsed_results_bytes,
+					       u32 *num_errors,
+					       u32 *num_warnings)
+{
+	const char *section_name, *param_name, *param_str_val;
+	u32 *dump_buf_end = dump_buf + num_dumped_dwords;
+	u32 num_section_params = 0, num_rules;
+	u32 results_offset = 0;	/* Offset in results_buf in bytes */
+
+	*parsed_results_bytes = 0;
+	*num_errors = 0;
+	*num_warnings = 0;
+	if (!s_dbg_arrays[BIN_BUF_DBG_PARSING_STRINGS].ptr ||
+	    !s_dbg_arrays[BIN_BUF_DBG_IDLE_CHK_PARSING_DATA].ptr)
+		return DBG_STATUS_DBG_ARRAY_NOT_SET;
+
+	/* Read global_params section */
+	dump_buf += qed_read_section_hdr(dump_buf,
+					 &section_name, &num_section_params);
+	if (strcmp(section_name, "global_params"))
+		return DBG_STATUS_IDLE_CHK_PARSE_FAILED;
+
+	/* Print global params */
+	dump_buf += qed_print_section_params(dump_buf,
+					     num_section_params,
+					     results_buf, &results_offset);
+
+	/* Read idle_chk section */
+	dump_buf += qed_read_section_hdr(dump_buf,
+					 &section_name, &num_section_params);
+	if (strcmp(section_name, "idle_chk") || num_section_params != 1)
+		return DBG_STATUS_IDLE_CHK_PARSE_FAILED;
+
+	dump_buf += qed_read_param(dump_buf,
+				   &param_name, &param_str_val, &num_rules);
+	if (strcmp(param_name, "num_rules") != 0)
+		return DBG_STATUS_IDLE_CHK_PARSE_FAILED;
+
+	if (num_rules) {
+		u32 rules_print_size;
+
+		/* Print FW output */
+		results_offset +=
+		    sprintf(qed_get_buf_ptr(results_buf,
+					    results_offset),
+			    "FW_IDLE_CHECK:\n");
+		rules_print_size =
+			qed_parse_idle_chk_dump_rules(p_hwfn, dump_buf,
+						      dump_buf_end, num_rules,
+						      true,
+						      results_buf ?
+						      results_buf +
+						      results_offset : NULL,
+						      num_errors, num_warnings);
+		results_offset += rules_print_size;
+		if (rules_print_size == 0)
+			return DBG_STATUS_IDLE_CHK_PARSE_FAILED;
+
+		/* Print LSI output */
+		results_offset +=
+		    sprintf(qed_get_buf_ptr(results_buf,
+					    results_offset),
+			    "\nLSI_IDLE_CHECK:\n");
+		rules_print_size =
+			qed_parse_idle_chk_dump_rules(p_hwfn, dump_buf,
+						      dump_buf_end, num_rules,
+						      false,
+						      results_buf ?
+						      results_buf +
+						      results_offset : NULL,
+						      num_errors, num_warnings);
+		results_offset += rules_print_size;
+		if (rules_print_size == 0)
+			return DBG_STATUS_IDLE_CHK_PARSE_FAILED;
+	}
+
+	/* Print errors/warnings count */
+	if (*num_errors) {
+		results_offset +=
+		    sprintf(qed_get_buf_ptr(results_buf,
+					    results_offset),
+			    "\nIdle Check failed!!! (with %d errors and %d warnings)\n",
+			    *num_errors, *num_warnings);
+	} else if (*num_warnings) {
+		results_offset +=
+		    sprintf(qed_get_buf_ptr(results_buf,
+					    results_offset),
+			    "\nIdle Check completed successfuly (with %d warnings)\n",
+			    *num_warnings);
+	} else {
+		results_offset +=
+		    sprintf(qed_get_buf_ptr(results_buf,
+					    results_offset),
+			    "\nIdle Check completed successfuly\n");
+	}
+
+	/* Add 1 for string NULL termination */
+	*parsed_results_bytes = results_offset + 1;
+	return DBG_STATUS_OK;
+}
+
+enum dbg_status qed_get_idle_chk_results_buf_size(struct qed_hwfn *p_hwfn,
+						  u32 *dump_buf,
+						  u32 num_dumped_dwords,
+						  u32 *results_buf_size)
+{
+	u32 num_errors, num_warnings;
+
+	return qed_parse_idle_chk_dump(p_hwfn,
+				       dump_buf,
+				       num_dumped_dwords,
+				       NULL,
+				       results_buf_size,
+				       &num_errors, &num_warnings);
+}
+
+enum dbg_status qed_print_idle_chk_results(struct qed_hwfn *p_hwfn,
+					   u32 *dump_buf,
+					   u32 num_dumped_dwords,
+					   char *results_buf,
+					   u32 *num_errors, u32 *num_warnings)
+{
+	u32 parsed_buf_size;
+
+	return qed_parse_idle_chk_dump(p_hwfn,
+				       dump_buf,
+				       num_dumped_dwords,
+				       results_buf,
+				       &parsed_buf_size,
+				       num_errors, num_warnings);
+}
+
+/* Frees the specified MCP Trace meta data */
+static void qed_mcp_trace_free_meta(struct qed_hwfn *p_hwfn,
+				    struct mcp_trace_meta *meta)
+{
+	u32 i;
+
+	/* Release modules */
+	if (meta->modules) {
+		for (i = 0; i < meta->modules_num; i++)
+			kfree(meta->modules[i]);
+		kfree(meta->modules);
+	}
+
+	/* Release formats */
+	if (meta->formats) {
+		for (i = 0; i < meta->formats_num; i++)
+			kfree(meta->formats[i].format_str);
+		kfree(meta->formats);
+	}
+}
+
+/* Allocates and fills MCP Trace meta data based on the specified meta data
+ * dump buffer.
+ * Returns debug status code.
+ */
+static enum dbg_status qed_mcp_trace_alloc_meta(struct qed_hwfn *p_hwfn,
+						const u32 *meta_buf,
+						struct mcp_trace_meta *meta)
+{
+	u8 *meta_buf_bytes = (u8 *)meta_buf;
+	u32 offset = 0, signature, i;
+
+	memset(meta, 0, sizeof(*meta));
+
+	/* Read first signature */
+	signature = qed_read_dword_from_buf(meta_buf_bytes, &offset);
+	if (signature != MCP_TRACE_META_IMAGE_SIGNATURE)
+		return DBG_STATUS_INVALID_TRACE_SIGNATURE;
+
+	/* Read number of modules and allocate memory for all the modules
+	 * pointers.
+	 */
+	meta->modules_num = qed_read_byte_from_buf(meta_buf_bytes, &offset);
+	meta->modules = kzalloc(meta->modules_num * sizeof(char *), GFP_KERNEL);
+	if (!meta->modules)
+		return DBG_STATUS_VIRT_MEM_ALLOC_FAILED;
+
+	/* Allocate and read all module strings */
+	for (i = 0; i < meta->modules_num; i++) {
+		u8 module_len = qed_read_byte_from_buf(meta_buf_bytes, &offset);
+
+		*(meta->modules + i) = kzalloc(module_len, GFP_KERNEL);
+		if (!(*(meta->modules + i))) {
+			/* Update number of modules to be released */
+			meta->modules_num = i ? i - 1 : 0;
+			return DBG_STATUS_VIRT_MEM_ALLOC_FAILED;
+		}
+
+		qed_read_str_from_buf(meta_buf_bytes, &offset, module_len,
+				      *(meta->modules + i));
+		if (module_len > MCP_TRACE_MAX_MODULE_LEN)
+			(*(meta->modules + i))[MCP_TRACE_MAX_MODULE_LEN] = '\0';
+	}
+
+	/* Read second signature */
+	signature = qed_read_dword_from_buf(meta_buf_bytes, &offset);
+	if (signature != MCP_TRACE_META_IMAGE_SIGNATURE)
+		return DBG_STATUS_INVALID_TRACE_SIGNATURE;
+
+	/* Read number of formats and allocate memory for all formats */
+	meta->formats_num = qed_read_dword_from_buf(meta_buf_bytes, &offset);
+	meta->formats = kzalloc(meta->formats_num *
+				sizeof(struct mcp_trace_format),
+				GFP_KERNEL);
+	if (!meta->formats)
+		return DBG_STATUS_VIRT_MEM_ALLOC_FAILED;
+
+	/* Allocate and read all strings */
+	for (i = 0; i < meta->formats_num; i++) {
+		struct mcp_trace_format *format_ptr = &meta->formats[i];
+		u8 format_len;
+
+		format_ptr->data = qed_read_dword_from_buf(meta_buf_bytes,
+							   &offset);
+		format_len =
+		    (format_ptr->data &
+		     MCP_TRACE_FORMAT_LEN_MASK) >> MCP_TRACE_FORMAT_LEN_SHIFT;
+		format_ptr->format_str = kzalloc(format_len, GFP_KERNEL);
+		if (!format_ptr->format_str) {
+			/* Update number of modules to be released */
+			meta->formats_num = i ? i - 1 : 0;
+			return DBG_STATUS_VIRT_MEM_ALLOC_FAILED;
+		}
+
+		qed_read_str_from_buf(meta_buf_bytes,
+				      &offset,
+				      format_len, format_ptr->format_str);
+	}
+
+	return DBG_STATUS_OK;
+}
+
+/* Parses an MCP Trace dump buffer.
+ * If result_buf is not NULL, the MCP Trace results are printed to it.
+ * In any case, the required results buffer size is assigned to
+ * parsed_results_bytes.
+ * The parsing status is returned.
+ */
+static enum dbg_status qed_parse_mcp_trace_dump(struct qed_hwfn *p_hwfn,
+						u32 *dump_buf,
+						u32 num_dumped_dwords,
+						char *results_buf,
+						u32 *parsed_results_bytes)
+{
+	u32 results_offset = 0, param_mask, param_shift, param_num_val;
+	u32 num_section_params, offset, end_offset, bytes_left;
+	const char *section_name, *param_name, *param_str_val;
+	u32 trace_data_dwords, trace_meta_dwords;
+	struct mcp_trace_meta meta;
+	struct mcp_trace *trace;
+	enum dbg_status status;
+	const u32 *meta_buf;
+	u8 *trace_buf;
+
+	*parsed_results_bytes = 0;
+
+	/* Read global_params section */
+	dump_buf += qed_read_section_hdr(dump_buf,
+					 &section_name, &num_section_params);
+	if (strcmp(section_name, "global_params"))
+		return DBG_STATUS_MCP_TRACE_BAD_DATA;
+
+	/* Print global params */
+	dump_buf += qed_print_section_params(dump_buf,
+					     num_section_params,
+					     results_buf, &results_offset);
+
+	/* Read trace_data section */
+	dump_buf += qed_read_section_hdr(dump_buf,
+					 &section_name, &num_section_params);
+	if (strcmp(section_name, "mcp_trace_data") || num_section_params != 1)
+		return DBG_STATUS_MCP_TRACE_BAD_DATA;
+	dump_buf += qed_read_param(dump_buf,
+				   &param_name, &param_str_val, &param_num_val);
+	if (strcmp(param_name, "size"))
+		return DBG_STATUS_MCP_TRACE_BAD_DATA;
+	trace_data_dwords = param_num_val;
+
+	/* Prepare trace info */
+	trace = (struct mcp_trace *)dump_buf;
+	trace_buf = (u8 *)dump_buf + sizeof(struct mcp_trace);
+	offset = trace->trace_oldest;
+	end_offset = trace->trace_prod;
+	bytes_left = qed_cyclic_sub(end_offset, offset, trace->size);
+	dump_buf += trace_data_dwords;
+
+	/* Read meta_data section */
+	dump_buf += qed_read_section_hdr(dump_buf,
+					 &section_name, &num_section_params);
+	if (strcmp(section_name, "mcp_trace_meta"))
+		return DBG_STATUS_MCP_TRACE_BAD_DATA;
+	dump_buf += qed_read_param(dump_buf,
+				   &param_name, &param_str_val, &param_num_val);
+	if (strcmp(param_name, "size") != 0)
+		return DBG_STATUS_MCP_TRACE_BAD_DATA;
+	trace_meta_dwords = param_num_val;
+
+	/* Choose meta data buffer */
+	if (!trace_meta_dwords) {
+		/* Dump doesn't include meta data */
+		if (!s_mcp_trace_meta.ptr)
+			return DBG_STATUS_MCP_TRACE_NO_META;
+		meta_buf = s_mcp_trace_meta.ptr;
+	} else {
+		/* Dump includes meta data */
+		meta_buf = dump_buf;
+	}
+
+	/* Allocate meta data memory */
+	status = qed_mcp_trace_alloc_meta(p_hwfn, meta_buf, &meta);
+	if (status != DBG_STATUS_OK)
+		goto free_mem;
+
+	/* Ignore the level and modules masks - just print everything that is
+	 * already in the buffer.
+	 */
+	while (bytes_left) {
+		struct mcp_trace_format *format_ptr;
+		u8 format_level, format_module;
+		u32 params[3] = { 0, 0, 0 };
+		u32 header, format_idx, i;
+
+		if (bytes_left < MFW_TRACE_ENTRY_SIZE) {
+			status = DBG_STATUS_MCP_TRACE_BAD_DATA;
+			goto free_mem;
+		}
+
+		header = qed_read_from_cyclic_buf(trace_buf,
+						  &offset,
+						  trace->size,
+						  MFW_TRACE_ENTRY_SIZE);
+		bytes_left -= MFW_TRACE_ENTRY_SIZE;
+		format_idx = header & MFW_TRACE_EVENTID_MASK;
+
+		/* Skip message if its  index doesn't exist in the meta data */
+		if (format_idx > meta.formats_num) {
+			u8 format_size =
+			    (u8)((header &
+				  MFW_TRACE_PRM_SIZE_MASK) >>
+				 MFW_TRACE_PRM_SIZE_SHIFT);
+
+			if (bytes_left < format_size) {
+				status = DBG_STATUS_MCP_TRACE_BAD_DATA;
+				goto free_mem;
+			}
+
+			offset = qed_cyclic_add(offset,
+						format_size, trace->size);
+			bytes_left -= format_size;
+			continue;
+		}
+
+		format_ptr = &meta.formats[format_idx];
+		for (i = 0,
+		     param_mask = MCP_TRACE_FORMAT_P1_SIZE_MASK, param_shift =
+		     MCP_TRACE_FORMAT_P1_SIZE_SHIFT;
+		     i < MCP_TRACE_FORMAT_MAX_PARAMS;
+		     i++, param_mask <<= MCP_TRACE_FORMAT_PARAM_WIDTH,
+		     param_shift += MCP_TRACE_FORMAT_PARAM_WIDTH) {
+			/* Extract param size (0..3) */
+			u8 param_size =
+			    (u8)((format_ptr->data &
+				  param_mask) >> param_shift);
+
+			/* If the param size is zero, there are no other
+			 * parameters.
+			 */
+			if (!param_size)
+				break;
+
+			/* Size is encoded using 2 bits, where 3 is used to
+			 * encode 4.
+			 */
+			if (param_size == 3)
+				param_size = 4;
+			if (bytes_left < param_size) {
+				status = DBG_STATUS_MCP_TRACE_BAD_DATA;
+				goto free_mem;
+			}
+
+			params[i] = qed_read_from_cyclic_buf(trace_buf,
+							     &offset,
+							     trace->size,
+							     param_size);
+			bytes_left -= param_size;
+		}
+
+		format_level =
+		    (u8)((format_ptr->data &
+			  MCP_TRACE_FORMAT_LEVEL_MASK) >>
+			  MCP_TRACE_FORMAT_LEVEL_SHIFT);
+		format_module =
+		    (u8)((format_ptr->data &
+			  MCP_TRACE_FORMAT_MODULE_MASK) >>
+			 MCP_TRACE_FORMAT_MODULE_SHIFT);
+		if (format_level >= ARRAY_SIZE(s_mcp_trace_level_str)) {
+			status = DBG_STATUS_MCP_TRACE_BAD_DATA;
+			goto free_mem;
+		}
+
+		/* Print current message to results buffer */
+		results_offset +=
+		    sprintf(qed_get_buf_ptr(results_buf,
+					    results_offset), "%s %-8s: ",
+			    s_mcp_trace_level_str[format_level],
+			    meta.modules[format_module]);
+		results_offset +=
+		    sprintf(qed_get_buf_ptr(results_buf,
+					    results_offset),
+			    format_ptr->format_str, params[0], params[1],
+			    params[2]);
+	}
+
+free_mem:
+	*parsed_results_bytes = results_offset + 1;
+	qed_mcp_trace_free_meta(p_hwfn, &meta);
+	return status;
+}
+
+enum dbg_status qed_get_mcp_trace_results_buf_size(struct qed_hwfn *p_hwfn,
+						   u32 *dump_buf,
+						   u32 num_dumped_dwords,
+						   u32 *results_buf_size)
+{
+	return qed_parse_mcp_trace_dump(p_hwfn,
+					dump_buf,
+					num_dumped_dwords,
+					NULL, results_buf_size);
+}
+
+enum dbg_status qed_print_mcp_trace_results(struct qed_hwfn *p_hwfn,
+					    u32 *dump_buf,
+					    u32 num_dumped_dwords,
+					    char *results_buf)
+{
+	u32 parsed_buf_size;
+
+	return qed_parse_mcp_trace_dump(p_hwfn,
+					dump_buf,
+					num_dumped_dwords,
+					results_buf, &parsed_buf_size);
+}
+
+/* Parses a Reg FIFO dump buffer.
+ * If result_buf is not NULL, the Reg FIFO results are printed to it.
+ * In any case, the required results buffer size is assigned to
+ * parsed_results_bytes.
+ * The parsing status is returned.
+ */
+static enum dbg_status qed_parse_reg_fifo_dump(struct qed_hwfn *p_hwfn,
+					       u32 *dump_buf,
+					       u32 num_dumped_dwords,
+					       char *results_buf,
+					       u32 *parsed_results_bytes)
+{
+	u32 results_offset = 0, param_num_val, num_section_params, num_elements;
+	const char *section_name, *param_name, *param_str_val;
+	struct reg_fifo_element *elements;
+	u8 i, j, err_val, vf_val;
+	char vf_str[4];
+
+	/* Read global_params section */
+	dump_buf += qed_read_section_hdr(dump_buf,
+					 &section_name, &num_section_params);
+	if (strcmp(section_name, "global_params"))
+		return DBG_STATUS_REG_FIFO_BAD_DATA;
+
+	/* Print global params */
+	dump_buf += qed_print_section_params(dump_buf,
+					     num_section_params,
+					     results_buf, &results_offset);
+
+	/* Read reg_fifo_data section */
+	dump_buf += qed_read_section_hdr(dump_buf,
+					 &section_name, &num_section_params);
+	if (strcmp(section_name, "reg_fifo_data"))
+		return DBG_STATUS_REG_FIFO_BAD_DATA;
+	dump_buf += qed_read_param(dump_buf,
+				   &param_name, &param_str_val, &param_num_val);
+	if (strcmp(param_name, "size"))
+		return DBG_STATUS_REG_FIFO_BAD_DATA;
+	if (param_num_val % REG_FIFO_ELEMENT_DWORDS)
+		return DBG_STATUS_REG_FIFO_BAD_DATA;
+	num_elements = param_num_val / REG_FIFO_ELEMENT_DWORDS;
+	elements = (struct reg_fifo_element *)dump_buf;
+
+	/* Decode elements */
+	for (i = 0; i < num_elements; i++) {
+		bool err_printed = false;
+
+		/* Discover if element belongs to a VF or a PF */
+		vf_val = GET_FIELD(elements[i].data, REG_FIFO_ELEMENT_VF);
+		if (vf_val == REG_FIFO_ELEMENT_IS_PF_VF_VAL)
+			sprintf(vf_str, "%s", "N/A");
+		else
+			sprintf(vf_str, "%d", vf_val);
+
+		/* Add parsed element to parsed buffer */
+		results_offset +=
+		    sprintf(qed_get_buf_ptr(results_buf,
+					    results_offset),
+			    "raw: 0x%016llx, address: 0x%07llx, access: %-5s, pf: %2lld, vf: %s, port: %lld, privilege: %-3s, protection: %-12s, master: %-4s, errors: ",
+			    elements[i].data,
+			    GET_FIELD(elements[i].data,
+				      REG_FIFO_ELEMENT_ADDRESS) *
+				      REG_FIFO_ELEMENT_ADDR_FACTOR,
+				      s_access_strs[GET_FIELD(elements[i].data,
+						    REG_FIFO_ELEMENT_ACCESS)],
+			    GET_FIELD(elements[i].data,
+				      REG_FIFO_ELEMENT_PF), vf_str,
+			    GET_FIELD(elements[i].data,
+				      REG_FIFO_ELEMENT_PORT),
+				      s_privilege_strs[GET_FIELD(elements[i].
+				      data,
+				      REG_FIFO_ELEMENT_PRIVILEGE)],
+			    s_protection_strs[GET_FIELD(elements[i].data,
+						REG_FIFO_ELEMENT_PROTECTION)],
+			    s_master_strs[GET_FIELD(elements[i].data,
+						REG_FIFO_ELEMENT_MASTER)]);
+
+		/* Print errors */
+		for (j = 0,
+		     err_val = GET_FIELD(elements[i].data,
+					 REG_FIFO_ELEMENT_ERROR);
+		     j < ARRAY_SIZE(s_reg_fifo_error_strs);
+		     j++, err_val >>= 1) {
+			if (!(err_val & 0x1))
+				continue;
+			if (err_printed)
+				results_offset +=
+					sprintf(qed_get_buf_ptr(results_buf,
+								results_offset),
+						", ");
+			results_offset +=
+				sprintf(qed_get_buf_ptr(results_buf,
+							results_offset), "%s",
+					s_reg_fifo_error_strs[j]);
+			err_printed = true;
+		}
+
+		results_offset +=
+		    sprintf(qed_get_buf_ptr(results_buf, results_offset), "\n");
+	}
+
+	results_offset += sprintf(qed_get_buf_ptr(results_buf,
+						  results_offset),
+				  "fifo contained %d elements", num_elements);
+
+	/* Add 1 for string NULL termination */
+	*parsed_results_bytes = results_offset + 1;
+	return DBG_STATUS_OK;
+}
+
+enum dbg_status qed_get_reg_fifo_results_buf_size(struct qed_hwfn *p_hwfn,
+						  u32 *dump_buf,
+						  u32 num_dumped_dwords,
+						  u32 *results_buf_size)
+{
+	return qed_parse_reg_fifo_dump(p_hwfn,
+				       dump_buf,
+				       num_dumped_dwords,
+				       NULL, results_buf_size);
+}
+
+enum dbg_status qed_print_reg_fifo_results(struct qed_hwfn *p_hwfn,
+					   u32 *dump_buf,
+					   u32 num_dumped_dwords,
+					   char *results_buf)
+{
+	u32 parsed_buf_size;
+
+	return qed_parse_reg_fifo_dump(p_hwfn,
+				       dump_buf,
+				       num_dumped_dwords,
+				       results_buf, &parsed_buf_size);
+}
+
+/* Parses an IGU FIFO dump buffer.
+ * If result_buf is not NULL, the IGU FIFO results are printed to it.
+ * In any case, the required results buffer size is assigned to
+ * parsed_results_bytes.
+ * The parsing status is returned.
+ */
+static enum dbg_status qed_parse_igu_fifo_dump(struct qed_hwfn *p_hwfn,
+					       u32 *dump_buf,
+					       u32 num_dumped_dwords,
+					       char *results_buf,
+					       u32 *parsed_results_bytes)
+{
+	u32 results_offset = 0, param_num_val, num_section_params, num_elements;
+	const char *section_name, *param_name, *param_str_val;
+	struct igu_fifo_element *elements;
+	char parsed_addr_data[32];
+	char parsed_wr_data[256];
+	u8 i, j;
+
+	/* Read global_params section */
+	dump_buf += qed_read_section_hdr(dump_buf,
+					 &section_name, &num_section_params);
+	if (strcmp(section_name, "global_params"))
+		return DBG_STATUS_IGU_FIFO_BAD_DATA;
+
+	/* Print global params */
+	dump_buf += qed_print_section_params(dump_buf,
+					     num_section_params,
+					     results_buf, &results_offset);
+
+	/* Read igu_fifo_data section */
+	dump_buf += qed_read_section_hdr(dump_buf,
+					 &section_name, &num_section_params);
+	if (strcmp(section_name, "igu_fifo_data"))
+		return DBG_STATUS_IGU_FIFO_BAD_DATA;
+	dump_buf += qed_read_param(dump_buf,
+				   &param_name, &param_str_val, &param_num_val);
+	if (strcmp(param_name, "size"))
+		return DBG_STATUS_IGU_FIFO_BAD_DATA;
+	if (param_num_val % IGU_FIFO_ELEMENT_DWORDS)
+		return DBG_STATUS_IGU_FIFO_BAD_DATA;
+	num_elements = param_num_val / IGU_FIFO_ELEMENT_DWORDS;
+	elements = (struct igu_fifo_element *)dump_buf;
+
+	/* Decode elements */
+	for (i = 0; i < num_elements; i++) {
+		/* dword12 (dword index 1 and 2) contains bits 32..95 of the
+		 * FIFO element.
+		 */
+		u64 dword12 =
+		    ((u64)elements[i].dword2 << 32) | elements[i].dword1;
+		bool is_wr_cmd = GET_FIELD(dword12,
+					   IGU_FIFO_ELEMENT_DWORD12_IS_WR_CMD);
+		bool is_pf = GET_FIELD(elements[i].dword0,
+				       IGU_FIFO_ELEMENT_DWORD0_IS_PF);
+		u16 cmd_addr = GET_FIELD(elements[i].dword0,
+					 IGU_FIFO_ELEMENT_DWORD0_CMD_ADDR);
+		u8 source = GET_FIELD(elements[i].dword0,
+				      IGU_FIFO_ELEMENT_DWORD0_SOURCE);
+		u8 err_type = GET_FIELD(elements[i].dword0,
+					IGU_FIFO_ELEMENT_DWORD0_ERR_TYPE);
+		const struct igu_fifo_addr_data *addr_data = NULL;
+
+		if (source >= ARRAY_SIZE(s_igu_fifo_source_strs))
+			return DBG_STATUS_IGU_FIFO_BAD_DATA;
+		if (err_type >= ARRAY_SIZE(s_igu_fifo_error_strs))
+			return DBG_STATUS_IGU_FIFO_BAD_DATA;
+
+		/* Find address data */
+		for (j = 0; j < ARRAY_SIZE(s_igu_fifo_addr_data) && !addr_data;
+		     j++)
+			if (cmd_addr >= s_igu_fifo_addr_data[j].start_addr &&
+			    cmd_addr <= s_igu_fifo_addr_data[j].end_addr)
+				addr_data = &s_igu_fifo_addr_data[j];
+		if (!addr_data)
+			return DBG_STATUS_IGU_FIFO_BAD_DATA;
+
+		/* Prepare parsed address data */
+		switch (addr_data->type) {
+		case IGU_ADDR_TYPE_MSIX_MEM:
+			sprintf(parsed_addr_data,
+				" vector_num=0x%x", cmd_addr / 2);
+			break;
+		case IGU_ADDR_TYPE_WRITE_INT_ACK:
+		case IGU_ADDR_TYPE_WRITE_PROD_UPDATE:
+			sprintf(parsed_addr_data,
+				" SB=0x%x", cmd_addr - addr_data->start_addr);
+			break;
+		default:
+			parsed_addr_data[0] = '\0';
+		}
+
+		/* Prepare parsed write data */
+		if (is_wr_cmd) {
+			u32 wr_data = GET_FIELD(dword12,
+					IGU_FIFO_ELEMENT_DWORD12_WR_DATA);
+			u32 prod_cons = GET_FIELD(wr_data,
+						  IGU_FIFO_WR_DATA_PROD_CONS);
+			u8 is_cleanup = GET_FIELD(wr_data,
+						  IGU_FIFO_WR_DATA_CMD_TYPE);
+
+			if (source == IGU_SRC_ATTN) {
+				sprintf(parsed_wr_data,
+					"prod: 0x%x, ", prod_cons);
+			} else {
+				if (is_cleanup) {
+					u8 cleanup_val = GET_FIELD(wr_data,
+								   IGU_FIFO_CLEANUP_WR_DATA_CLEANUP_VAL);
+					u8 cleanup_type = GET_FIELD(wr_data,
+								    IGU_FIFO_CLEANUP_WR_DATA_CLEANUP_TYPE);
+
+					sprintf(parsed_wr_data,
+						"cmd_type: cleanup, cleanup_val: %s, cleanup_type: %d, ",
+						cleanup_val ? "set" : "clear",
+						cleanup_type);
+				} else {
+					u8 update_flag = GET_FIELD(wr_data,
+								   IGU_FIFO_WR_DATA_UPDATE_FLAG);
+					u8 en_dis_int_for_sb =
+					    GET_FIELD(wr_data,
+						      IGU_FIFO_WR_DATA_EN_DIS_INT_FOR_SB);
+					u8 segment = GET_FIELD(wr_data,
+							       IGU_FIFO_WR_DATA_SEGMENT);
+					u8 timer_mask = GET_FIELD(wr_data,
+								  IGU_FIFO_WR_DATA_TIMER_MASK);
+
+					sprintf(parsed_wr_data,
+						"cmd_type: prod/cons update, prod/cons: 0x%x, update_flag: %s, en_dis_int_for_sb: %s, segment: %s, timer_mask=%d, ",
+						prod_cons,
+						update_flag ? "update" : "nop",
+						en_dis_int_for_sb
+						? (en_dis_int_for_sb ==
+						   1 ? "disable" : "nop") :
+						"enable",
+						segment ? "attn" : "regular",
+						timer_mask);
+				}
+			}
+		} else {
+			parsed_wr_data[0] = '\0';
+		}
+
+		/* Add parsed element to parsed buffer */
+		results_offset +=
+		    sprintf(qed_get_buf_ptr(results_buf,
+					    results_offset),
+			    "raw: 0x%01x%08x%08x, %s: %d, source: %s, type: %s, cmd_addr: 0x%x (%s%s), %serror: %s\n",
+			    elements[i].dword2, elements[i].dword1,
+			    elements[i].dword0,
+			    is_pf ? "pf" : "vf",
+			    GET_FIELD(elements[i].dword0,
+				      IGU_FIFO_ELEMENT_DWORD0_FID),
+			    s_igu_fifo_source_strs[source],
+			    is_wr_cmd ? "wr" : "rd", cmd_addr,
+			    (!is_pf && addr_data->vf_desc)
+			    ? addr_data->vf_desc : addr_data->desc,
+			    parsed_addr_data, parsed_wr_data,
+			    s_igu_fifo_error_strs[err_type]);
+	}
+
+	results_offset += sprintf(qed_get_buf_ptr(results_buf,
+						  results_offset),
+				  "fifo contained %d elements", num_elements);
+
+	/* Add 1 for string NULL termination */
+	*parsed_results_bytes = results_offset + 1;
+	return DBG_STATUS_OK;
+}
+
+enum dbg_status qed_get_igu_fifo_results_buf_size(struct qed_hwfn *p_hwfn,
+						  u32 *dump_buf,
+						  u32 num_dumped_dwords,
+						  u32 *results_buf_size)
+{
+	return qed_parse_igu_fifo_dump(p_hwfn,
+				       dump_buf,
+				       num_dumped_dwords,
+				       NULL, results_buf_size);
+}
+
+enum dbg_status qed_print_igu_fifo_results(struct qed_hwfn *p_hwfn,
+					   u32 *dump_buf,
+					   u32 num_dumped_dwords,
+					   char *results_buf)
+{
+	u32 parsed_buf_size;
+
+	return qed_parse_igu_fifo_dump(p_hwfn,
+				       dump_buf,
+				       num_dumped_dwords,
+				       results_buf, &parsed_buf_size);
+}
+
+static enum dbg_status
+qed_parse_protection_override_dump(struct qed_hwfn *p_hwfn,
+				   u32 *dump_buf,
+				   u32 num_dumped_dwords,
+				   char *results_buf,
+				   u32 *parsed_results_bytes)
+{
+	u32 results_offset = 0, param_num_val, num_section_params, num_elements;
+	const char *section_name, *param_name, *param_str_val;
+	struct protection_override_element *elements;
+	u8 i;
+
+	/* Read global_params section */
+	dump_buf += qed_read_section_hdr(dump_buf,
+					 &section_name, &num_section_params);
+	if (strcmp(section_name, "global_params"))
+		return DBG_STATUS_PROTECTION_OVERRIDE_BAD_DATA;
+
+	/* Print global params */
+	dump_buf += qed_print_section_params(dump_buf,
+					     num_section_params,
+					     results_buf, &results_offset);
+
+	/* Read protection_override_data section */
+	dump_buf += qed_read_section_hdr(dump_buf,
+					 &section_name, &num_section_params);
+	if (strcmp(section_name, "protection_override_data"))
+		return DBG_STATUS_PROTECTION_OVERRIDE_BAD_DATA;
+	dump_buf += qed_read_param(dump_buf,
+				   &param_name, &param_str_val, &param_num_val);
+	if (strcmp(param_name, "size"))
+		return DBG_STATUS_PROTECTION_OVERRIDE_BAD_DATA;
+	if (param_num_val % PROTECTION_OVERRIDE_ELEMENT_DWORDS != 0)
+		return DBG_STATUS_PROTECTION_OVERRIDE_BAD_DATA;
+	num_elements = param_num_val / PROTECTION_OVERRIDE_ELEMENT_DWORDS;
+	elements = (struct protection_override_element *)dump_buf;
+
+	/* Decode elements */
+	for (i = 0; i < num_elements; i++) {
+		u32 address = GET_FIELD(elements[i].data,
+					PROTECTION_OVERRIDE_ELEMENT_ADDRESS) *
+					PROTECTION_OVERRIDE_ELEMENT_ADDR_FACTOR;
+
+		results_offset +=
+		    sprintf(qed_get_buf_ptr(results_buf,
+					    results_offset),
+			    "window %2d, address: 0x%07x, size: %7lld regs, read: %lld, write: %lld, read protection: %-12s, write protection: %-12s\n",
+			    i, address,
+			    GET_FIELD(elements[i].data,
+				      PROTECTION_OVERRIDE_ELEMENT_WINDOW_SIZE),
+			    GET_FIELD(elements[i].data,
+				      PROTECTION_OVERRIDE_ELEMENT_READ),
+			    GET_FIELD(elements[i].data,
+				      PROTECTION_OVERRIDE_ELEMENT_WRITE),
+			    s_protection_strs[GET_FIELD(elements[i].data,
+				PROTECTION_OVERRIDE_ELEMENT_READ_PROTECTION)],
+			    s_protection_strs[GET_FIELD(elements[i].data,
+				PROTECTION_OVERRIDE_ELEMENT_WRITE_PROTECTION)]);
+	}
+
+	results_offset += sprintf(qed_get_buf_ptr(results_buf,
+						  results_offset),
+				  "protection override contained %d elements",
+				  num_elements);
+
+	/* Add 1 for string NULL termination */
+	*parsed_results_bytes = results_offset + 1;
+	return DBG_STATUS_OK;
+}
+
+enum dbg_status
+qed_get_protection_override_results_buf_size(struct qed_hwfn *p_hwfn,
+					     u32 *dump_buf,
+					     u32 num_dumped_dwords,
+					     u32 *results_buf_size)
+{
+	return qed_parse_protection_override_dump(p_hwfn,
+						  dump_buf,
+						  num_dumped_dwords,
+						  NULL, results_buf_size);
+}
+
+enum dbg_status qed_print_protection_override_results(struct qed_hwfn *p_hwfn,
+						      u32 *dump_buf,
+						      u32 num_dumped_dwords,
+						      char *results_buf)
+{
+	u32 parsed_buf_size;
+
+	return qed_parse_protection_override_dump(p_hwfn,
+						  dump_buf,
+						  num_dumped_dwords,
+						  results_buf,
+						  &parsed_buf_size);
+}
+
+/* Parses a FW Asserts dump buffer.
+ * If result_buf is not NULL, the FW Asserts results are printed to it.
+ * In any case, the required results buffer size is assigned to
+ * parsed_results_bytes.
+ * The parsing status is returned.
+ */
+static enum dbg_status qed_parse_fw_asserts_dump(struct qed_hwfn *p_hwfn,
+						 u32 *dump_buf,
+						 u32 num_dumped_dwords,
+						 char *results_buf,
+						 u32 *parsed_results_bytes)
+{
+	u32 results_offset = 0, num_section_params, param_num_val, i;
+	const char *param_name, *param_str_val, *section_name;
+	bool last_section_found = false;
+
+	*parsed_results_bytes = 0;
+
+	/* Read global_params section */
+	dump_buf += qed_read_section_hdr(dump_buf,
+					 &section_name, &num_section_params);
+	if (strcmp(section_name, "global_params"))
+		return DBG_STATUS_FW_ASSERTS_PARSE_FAILED;
+
+	/* Print global params */
+	dump_buf += qed_print_section_params(dump_buf,
+					     num_section_params,
+					     results_buf, &results_offset);
+	while (!last_section_found) {
+		const char *storm_letter = NULL;
+		u32 storm_dump_size = 0;
+
+		dump_buf += qed_read_section_hdr(dump_buf,
+						 &section_name,
+						 &num_section_params);
+		if (!strcmp(section_name, "last")) {
+			last_section_found = true;
+			continue;
+		} else if (strcmp(section_name, "fw_asserts")) {
+			return DBG_STATUS_FW_ASSERTS_PARSE_FAILED;
+		}
+
+		/* Extract params */
+		for (i = 0; i < num_section_params; i++) {
+			dump_buf += qed_read_param(dump_buf,
+						   &param_name,
+						   &param_str_val,
+						   &param_num_val);
+			if (!strcmp(param_name, "storm"))
+				storm_letter = param_str_val;
+			else if (!strcmp(param_name, "size"))
+				storm_dump_size = param_num_val;
+			else
+				return DBG_STATUS_FW_ASSERTS_PARSE_FAILED;
+		}
+
+		if (!storm_letter || !storm_dump_size)
+			return DBG_STATUS_FW_ASSERTS_PARSE_FAILED;
+
+		/* Print data */
+		results_offset += sprintf(qed_get_buf_ptr(results_buf,
+							  results_offset),
+					  "\n%sSTORM_ASSERT: size=%d\n",
+					  storm_letter, storm_dump_size);
+		for (i = 0; i < storm_dump_size; i++, dump_buf++)
+			results_offset +=
+			    sprintf(qed_get_buf_ptr(results_buf,
+						    results_offset),
+				    "%08x\n", *dump_buf);
+	}
+
+	/* Add 1 for string NULL termination */
+	*parsed_results_bytes = results_offset + 1;
+	return DBG_STATUS_OK;
+}
+
+enum dbg_status qed_get_fw_asserts_results_buf_size(struct qed_hwfn *p_hwfn,
+						    u32 *dump_buf,
+						    u32 num_dumped_dwords,
+						    u32 *results_buf_size)
+{
+	return qed_parse_fw_asserts_dump(p_hwfn,
+					 dump_buf,
+					 num_dumped_dwords,
+					 NULL, results_buf_size);
+}
+
+enum dbg_status qed_print_fw_asserts_results(struct qed_hwfn *p_hwfn,
+					     u32 *dump_buf,
+					     u32 num_dumped_dwords,
+					     char *results_buf)
+{
+	u32 parsed_buf_size;
+
+	return qed_parse_fw_asserts_dump(p_hwfn,
+					 dump_buf,
+					 num_dumped_dwords,
+					 results_buf, &parsed_buf_size);
+}
+
+/* Wrapper for unifying the idle_chk and mcp_trace api */
+enum dbg_status qed_print_idle_chk_results_wrapper(struct qed_hwfn *p_hwfn,
+						   u32 *dump_buf,
+						   u32 num_dumped_dwords,
+						   char *results_buf)
+{
+	u32 num_errors, num_warnnings;
+
+	return qed_print_idle_chk_results(p_hwfn, dump_buf, num_dumped_dwords,
+					  results_buf, &num_errors,
+					  &num_warnnings);
+}
+
+/* Feature meta data lookup table */
+static struct {
+	char *name;
+	enum dbg_status (*get_size)(struct qed_hwfn *p_hwfn,
+				    struct qed_ptt *p_ptt, u32 *size);
+	enum dbg_status (*perform_dump)(struct qed_hwfn *p_hwfn,
+					struct qed_ptt *p_ptt, u32 *dump_buf,
+					u32 buf_size, u32 *dumped_dwords);
+	enum dbg_status (*print_results)(struct qed_hwfn *p_hwfn,
+					 u32 *dump_buf, u32 num_dumped_dwords,
+					 char *results_buf);
+	enum dbg_status (*results_buf_size)(struct qed_hwfn *p_hwfn,
+					    u32 *dump_buf,
+					    u32 num_dumped_dwords,
+					    u32 *results_buf_size);
+} qed_features_lookup[] = {
+	{
+	"grc", qed_dbg_grc_get_dump_buf_size,
+		    qed_dbg_grc_dump, NULL, NULL}, {
+	"idle_chk",
+		    qed_dbg_idle_chk_get_dump_buf_size,
+		    qed_dbg_idle_chk_dump,
+		    qed_print_idle_chk_results_wrapper,
+		    qed_get_idle_chk_results_buf_size}, {
+	"mcp_trace",
+		    qed_dbg_mcp_trace_get_dump_buf_size,
+		    qed_dbg_mcp_trace_dump, qed_print_mcp_trace_results,
+		    qed_get_mcp_trace_results_buf_size}, {
+	"reg_fifo",
+		    qed_dbg_reg_fifo_get_dump_buf_size,
+		    qed_dbg_reg_fifo_dump, qed_print_reg_fifo_results,
+		    qed_get_reg_fifo_results_buf_size}, {
+	"igu_fifo",
+		    qed_dbg_igu_fifo_get_dump_buf_size,
+		    qed_dbg_igu_fifo_dump, qed_print_igu_fifo_results,
+		    qed_get_igu_fifo_results_buf_size}, {
+	"protection_override",
+		    qed_dbg_protection_override_get_dump_buf_size,
+		    qed_dbg_protection_override_dump,
+		    qed_print_protection_override_results,
+		    qed_get_protection_override_results_buf_size}, {
+	"fw_asserts",
+		    qed_dbg_fw_asserts_get_dump_buf_size,
+		    qed_dbg_fw_asserts_dump,
+		    qed_print_fw_asserts_results,
+		    qed_get_fw_asserts_results_buf_size},};
+
+static void qed_dbg_print_feature(u8 *p_text_buf, u32 text_size)
+{
+	u32 i, precision = 80;
+
+	if (!p_text_buf)
+		return;
+
+	pr_notice("\n%.*s", precision, p_text_buf);
+	for (i = precision; i < text_size; i += precision)
+		pr_cont("%.*s", precision, p_text_buf + i);
+	pr_cont("\n");
+}
+
+#define QED_RESULTS_BUF_MIN_SIZE 16
+/* Generic function for decoding debug feature info */
+enum dbg_status format_feature(struct qed_hwfn *p_hwfn,
+			       enum qed_dbg_features feature_idx)
+{
+	struct qed_dbg_feature *feature =
+	    &p_hwfn->cdev->dbg_params.features[feature_idx];
+	u32 text_size_bytes, null_char_pos, i;
+	enum dbg_status rc;
+	char *text_buf;
+
+	/* Check if feature supports formatting capability */
+	if (!qed_features_lookup[feature_idx].results_buf_size)
+		return DBG_STATUS_OK;
+
+	/* Obtain size of formatted output */
+	rc = qed_features_lookup[feature_idx].
+		results_buf_size(p_hwfn, (u32 *)feature->dump_buf,
+				 feature->dumped_dwords, &text_size_bytes);
+	if (rc != DBG_STATUS_OK)
+		return rc;
+
+	/* Make sure that the allocated size is a multiple of dword (4 bytes) */
+	null_char_pos = text_size_bytes - 1;
+	text_size_bytes = (text_size_bytes + 3) & ~0x3;
+
+	if (text_size_bytes < QED_RESULTS_BUF_MIN_SIZE) {
+		DP_NOTICE(p_hwfn->cdev,
+			  "formatted size of feature was too small %d. Aborting\n",
+			  text_size_bytes);
+		return DBG_STATUS_INVALID_ARGS;
+	}
+
+	/* Allocate temp text buf */
+	text_buf = vzalloc(text_size_bytes);
+	if (!text_buf)
+		return DBG_STATUS_VIRT_MEM_ALLOC_FAILED;
+
+	/* Decode feature opcodes to string on temp buf */
+	rc = qed_features_lookup[feature_idx].
+		print_results(p_hwfn, (u32 *)feature->dump_buf,
+			      feature->dumped_dwords, text_buf);
+	if (rc != DBG_STATUS_OK) {
+		vfree(text_buf);
+		return rc;
+	}
+
+	/* Replace the original null character with a '\n' character.
+	 * The bytes that were added as a result of the dword alignment are also
+	 * padded with '\n' characters.
+	 */
+	for (i = null_char_pos; i < text_size_bytes; i++)
+		text_buf[i] = '\n';
+
+	/* Dump printable feature to log */
+	if (p_hwfn->cdev->dbg_params.print_data)
+		qed_dbg_print_feature(text_buf, text_size_bytes);
+
+	/* Free the old dump_buf and point the dump_buf to the newly allocagted
+	 * and formatted text buffer.
+	 */
+	vfree(feature->dump_buf);
+	feature->dump_buf = text_buf;
+	feature->buf_size = text_size_bytes;
+	feature->dumped_dwords = text_size_bytes / 4;
+	return rc;
+}
+
+/* Generic function for performing the dump of a debug feature. */
+enum dbg_status qed_dbg_dump(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt,
+			     enum qed_dbg_features feature_idx)
+{
+	struct qed_dbg_feature *feature =
+	    &p_hwfn->cdev->dbg_params.features[feature_idx];
+	u32 buf_size_dwords;
+	enum dbg_status rc;
+
+	DP_NOTICE(p_hwfn->cdev, "Collecting a debug feature [\"%s\"]\n",
+		  qed_features_lookup[feature_idx].name);
+
+	/* Dump_buf was already allocated need to free (this can happen if dump
+	 * was called but file was never read).
+	 * We can't use the buffer as is since size may have changed.
+	 */
+	if (feature->dump_buf) {
+		vfree(feature->dump_buf);
+		feature->dump_buf = NULL;
+	}
+
+	/* Get buffer size from hsi, allocate accordingly, and perform the
+	 * dump.
+	 */
+	rc = qed_features_lookup[feature_idx].get_size(p_hwfn, p_ptt,
+						       &buf_size_dwords);
+	if (rc != DBG_STATUS_OK)
+		return rc;
+	feature->buf_size = buf_size_dwords * sizeof(u32);
+	feature->dump_buf = vmalloc(feature->buf_size);
+	if (!feature->dump_buf)
+		return DBG_STATUS_VIRT_MEM_ALLOC_FAILED;
+
+	rc = qed_features_lookup[feature_idx].
+		perform_dump(p_hwfn, p_ptt, (u32 *)feature->dump_buf,
+			     feature->buf_size / sizeof(u32),
+			     &feature->dumped_dwords);
+
+	/* If mcp is stuck we get DBG_STATUS_NVRAM_GET_IMAGE_FAILED error.
+	 * In this case the buffer holds valid binary data, but we wont able
+	 * to parse it (since parsing relies on data in NVRAM which is only
+	 * accessible when MFW is responsive). skip the formatting but return
+	 * success so that binary data is provided.
+	 */
+	if (rc == DBG_STATUS_NVRAM_GET_IMAGE_FAILED)
+		return DBG_STATUS_OK;
+
+	if (rc != DBG_STATUS_OK)
+		return rc;
+
+	/* Format output */
+	rc = format_feature(p_hwfn, feature_idx);
+	return rc;
+}
+
+int qed_dbg_grc(struct qed_dev *cdev, void *buffer, u32 *num_dumped_bytes)
+{
+	return qed_dbg_feature(cdev, buffer, DBG_FEATURE_GRC, num_dumped_bytes);
+}
+
+int qed_dbg_grc_size(struct qed_dev *cdev)
+{
+	return qed_dbg_feature_size(cdev, DBG_FEATURE_GRC);
+}
+
+int qed_dbg_idle_chk(struct qed_dev *cdev, void *buffer, u32 *num_dumped_bytes)
+{
+	return qed_dbg_feature(cdev, buffer, DBG_FEATURE_IDLE_CHK,
+			       num_dumped_bytes);
+}
+
+int qed_dbg_idle_chk_size(struct qed_dev *cdev)
+{
+	return qed_dbg_feature_size(cdev, DBG_FEATURE_IDLE_CHK);
+}
+
+int qed_dbg_reg_fifo(struct qed_dev *cdev, void *buffer, u32 *num_dumped_bytes)
+{
+	return qed_dbg_feature(cdev, buffer, DBG_FEATURE_REG_FIFO,
+			       num_dumped_bytes);
+}
+
+int qed_dbg_reg_fifo_size(struct qed_dev *cdev)
+{
+	return qed_dbg_feature_size(cdev, DBG_FEATURE_REG_FIFO);
+}
+
+int qed_dbg_igu_fifo(struct qed_dev *cdev, void *buffer, u32 *num_dumped_bytes)
+{
+	return qed_dbg_feature(cdev, buffer, DBG_FEATURE_IGU_FIFO,
+			       num_dumped_bytes);
+}
+
+int qed_dbg_igu_fifo_size(struct qed_dev *cdev)
+{
+	return qed_dbg_feature_size(cdev, DBG_FEATURE_IGU_FIFO);
+}
+
+int qed_dbg_protection_override(struct qed_dev *cdev, void *buffer,
+				u32 *num_dumped_bytes)
+{
+	return qed_dbg_feature(cdev, buffer, DBG_FEATURE_PROTECTION_OVERRIDE,
+			       num_dumped_bytes);
+}
+
+int qed_dbg_protection_override_size(struct qed_dev *cdev)
+{
+	return qed_dbg_feature_size(cdev, DBG_FEATURE_PROTECTION_OVERRIDE);
+}
+
+int qed_dbg_fw_asserts(struct qed_dev *cdev, void *buffer,
+		       u32 *num_dumped_bytes)
+{
+	return qed_dbg_feature(cdev, buffer, DBG_FEATURE_FW_ASSERTS,
+			       num_dumped_bytes);
+}
+
+int qed_dbg_fw_asserts_size(struct qed_dev *cdev)
+{
+	return qed_dbg_feature_size(cdev, DBG_FEATURE_FW_ASSERTS);
+}
+
+int qed_dbg_mcp_trace(struct qed_dev *cdev, void *buffer,
+		      u32 *num_dumped_bytes)
+{
+	return qed_dbg_feature(cdev, buffer, DBG_FEATURE_MCP_TRACE,
+			       num_dumped_bytes);
+}
+
+int qed_dbg_mcp_trace_size(struct qed_dev *cdev)
+{
+	return qed_dbg_feature_size(cdev, DBG_FEATURE_MCP_TRACE);
+}
+
+/* Defines the amount of bytes allocated for recording the length of debugfs
+ * feature buffer.
+ */
+#define REGDUMP_HEADER_SIZE			sizeof(u32)
+#define REGDUMP_HEADER_FEATURE_SHIFT		24
+#define REGDUMP_HEADER_ENGINE_SHIFT		31
+#define REGDUMP_HEADER_OMIT_ENGINE_SHIFT	30
+enum debug_print_features {
+	OLD_MODE = 0,
+	IDLE_CHK = 1,
+	GRC_DUMP = 2,
+	MCP_TRACE = 3,
+	REG_FIFO = 4,
+	PROTECTION_OVERRIDE = 5,
+	IGU_FIFO = 6,
+	PHY = 7,
+	FW_ASSERTS = 8,
+};
+
+static u32 qed_calc_regdump_header(enum debug_print_features feature,
+				   int engine, u32 feature_size, u8 omit_engine)
+{
+	/* Insert the engine, feature and mode inside the header and combine it
+	 * with feature size.
+	 */
+	return feature_size | (feature << REGDUMP_HEADER_FEATURE_SHIFT) |
+	       (omit_engine << REGDUMP_HEADER_OMIT_ENGINE_SHIFT) |
+	       (engine << REGDUMP_HEADER_ENGINE_SHIFT);
+}
+
+int qed_dbg_all_data(struct qed_dev *cdev, void *buffer)
+{
+	u8 cur_engine, omit_engine = 0, org_engine;
+	u32 offset = 0, feature_size;
+	int rc;
+
+	if (cdev->num_hwfns == 1)
+		omit_engine = 1;
+
+	org_engine = qed_get_debug_engine(cdev);
+	for (cur_engine = 0; cur_engine < cdev->num_hwfns; cur_engine++) {
+		/* Collect idle_chks and grcDump for each hw function */
+		DP_VERBOSE(cdev, QED_MSG_DEBUG,
+			   "obtaining idle_chk and grcdump for current engine\n");
+		qed_set_debug_engine(cdev, cur_engine);
+
+		/* First idle_chk */
+		rc = qed_dbg_idle_chk(cdev, (u8 *)buffer + offset +
+				      REGDUMP_HEADER_SIZE, &feature_size);
+		if (!rc) {
+			*(u32 *)((u8 *)buffer + offset) =
+			    qed_calc_regdump_header(IDLE_CHK, cur_engine,
+						    feature_size, omit_engine);
+			offset += (feature_size + REGDUMP_HEADER_SIZE);
+		} else {
+			DP_ERR(cdev, "qed_dbg_idle_chk failed. rc = %d\n", rc);
+		}
+
+		/* Second idle_chk */
+		rc = qed_dbg_idle_chk(cdev, (u8 *)buffer + offset +
+				      REGDUMP_HEADER_SIZE, &feature_size);
+		if (!rc) {
+			*(u32 *)((u8 *)buffer + offset) =
+			    qed_calc_regdump_header(IDLE_CHK, cur_engine,
+						    feature_size, omit_engine);
+			offset += (feature_size + REGDUMP_HEADER_SIZE);
+		} else {
+			DP_ERR(cdev, "qed_dbg_idle_chk failed. rc = %d\n", rc);
+		}
+
+		/* reg_fifo dump */
+		rc = qed_dbg_reg_fifo(cdev, (u8 *)buffer + offset +
+				      REGDUMP_HEADER_SIZE, &feature_size);
+		if (!rc) {
+			*(u32 *)((u8 *)buffer + offset) =
+			    qed_calc_regdump_header(REG_FIFO, cur_engine,
+						    feature_size, omit_engine);
+			offset += (feature_size + REGDUMP_HEADER_SIZE);
+		} else {
+			DP_ERR(cdev, "qed_dbg_reg_fifo failed. rc = %d\n", rc);
+		}
+
+		/* igu_fifo dump */
+		rc = qed_dbg_igu_fifo(cdev, (u8 *)buffer + offset +
+				      REGDUMP_HEADER_SIZE, &feature_size);
+		if (!rc) {
+			*(u32 *)((u8 *)buffer + offset) =
+			    qed_calc_regdump_header(IGU_FIFO, cur_engine,
+						    feature_size, omit_engine);
+			offset += (feature_size + REGDUMP_HEADER_SIZE);
+		} else {
+			DP_ERR(cdev, "qed_dbg_igu_fifo failed. rc = %d", rc);
+		}
+
+		/* protection_override dump */
+		rc = qed_dbg_protection_override(cdev, (u8 *)buffer + offset +
+						 REGDUMP_HEADER_SIZE,
+						 &feature_size);
+		if (!rc) {
+			*(u32 *)((u8 *)buffer + offset) =
+			    qed_calc_regdump_header(PROTECTION_OVERRIDE,
+						    cur_engine,
+						    feature_size, omit_engine);
+			offset += (feature_size + REGDUMP_HEADER_SIZE);
+		} else {
+			DP_ERR(cdev,
+			       "qed_dbg_protection_override failed. rc = %d\n",
+			       rc);
+		}
+
+		/* fw_asserts dump */
+		rc = qed_dbg_fw_asserts(cdev, (u8 *)buffer + offset +
+					REGDUMP_HEADER_SIZE, &feature_size);
+		if (!rc) {
+			*(u32 *)((u8 *)buffer + offset) =
+			    qed_calc_regdump_header(FW_ASSERTS, cur_engine,
+						    feature_size, omit_engine);
+			offset += (feature_size + REGDUMP_HEADER_SIZE);
+		} else {
+			DP_ERR(cdev, "qed_dbg_fw_asserts failed. rc = %d\n",
+			       rc);
+		}
+
+		/* GRC dump - must be last because when mcp stuck it will
+		 * clutter idle_chk, reg_fifo, ...
+		 */
+		rc = qed_dbg_grc(cdev, (u8 *)buffer + offset +
+				 REGDUMP_HEADER_SIZE, &feature_size);
+		if (!rc) {
+			*(u32 *)((u8 *)buffer + offset) =
+			    qed_calc_regdump_header(GRC_DUMP, cur_engine,
+						    feature_size, omit_engine);
+			offset += (feature_size + REGDUMP_HEADER_SIZE);
+		} else {
+			DP_ERR(cdev, "qed_dbg_grc failed. rc = %d", rc);
+		}
+	}
+
+	/* mcp_trace */
+	rc = qed_dbg_mcp_trace(cdev, (u8 *)buffer + offset +
+			       REGDUMP_HEADER_SIZE, &feature_size);
+	if (!rc) {
+		*(u32 *)((u8 *)buffer + offset) =
+		    qed_calc_regdump_header(MCP_TRACE, cur_engine,
+					    feature_size, omit_engine);
+		offset += (feature_size + REGDUMP_HEADER_SIZE);
+	} else {
+		DP_ERR(cdev, "qed_dbg_mcp_trace failed. rc = %d\n", rc);
+	}
+
+	qed_set_debug_engine(cdev, org_engine);
+
+	return 0;
+}
+
+int qed_dbg_all_data_size(struct qed_dev *cdev)
+{
+	u8 cur_engine, org_engine;
+	u32 regs_len = 0;
+
+	org_engine = qed_get_debug_engine(cdev);
+	for (cur_engine = 0; cur_engine < cdev->num_hwfns; cur_engine++) {
+		/* Engine specific */
+		DP_VERBOSE(cdev, QED_MSG_DEBUG,
+			   "calculating idle_chk and grcdump register length for current engine\n");
+		qed_set_debug_engine(cdev, cur_engine);
+		regs_len += REGDUMP_HEADER_SIZE + qed_dbg_idle_chk_size(cdev) +
+			    REGDUMP_HEADER_SIZE + qed_dbg_idle_chk_size(cdev) +
+			    REGDUMP_HEADER_SIZE + qed_dbg_grc_size(cdev) +
+			    REGDUMP_HEADER_SIZE + qed_dbg_reg_fifo_size(cdev) +
+			    REGDUMP_HEADER_SIZE + qed_dbg_igu_fifo_size(cdev) +
+			    REGDUMP_HEADER_SIZE +
+			    qed_dbg_protection_override_size(cdev) +
+			    REGDUMP_HEADER_SIZE + qed_dbg_fw_asserts_size(cdev);
+	}
+
+	/* Engine common */
+	regs_len += REGDUMP_HEADER_SIZE + qed_dbg_mcp_trace_size(cdev);
+	qed_set_debug_engine(cdev, org_engine);
+
+	return regs_len;
+}
+
+int qed_dbg_feature(struct qed_dev *cdev, void *buffer,
+		    enum qed_dbg_features feature, u32 *num_dumped_bytes)
+{
+	struct qed_hwfn *p_hwfn =
+		&cdev->hwfns[cdev->dbg_params.engine_for_debug];
+	struct qed_dbg_feature *qed_feature =
+		&cdev->dbg_params.features[feature];
+	enum dbg_status dbg_rc;
+	struct qed_ptt *p_ptt;
+	int rc = 0;
+
+	/* Acquire ptt */
+	p_ptt = qed_ptt_acquire(p_hwfn);
+	if (!p_ptt)
+		return -EINVAL;
+
+	/* Get dump */
+	dbg_rc = qed_dbg_dump(p_hwfn, p_ptt, feature);
+	if (dbg_rc != DBG_STATUS_OK) {
+		DP_VERBOSE(cdev, QED_MSG_DEBUG, "%s\n",
+			   qed_dbg_get_status_str(dbg_rc));
+		*num_dumped_bytes = 0;
+		rc = -EINVAL;
+		goto out;
+	}
+
+	DP_VERBOSE(cdev, QED_MSG_DEBUG,
+		   "copying debugfs feature to external buffer\n");
+	memcpy(buffer, qed_feature->dump_buf, qed_feature->buf_size);
+	*num_dumped_bytes = cdev->dbg_params.features[feature].dumped_dwords *
+			    4;
+
+out:
+	qed_ptt_release(p_hwfn, p_ptt);
+	return rc;
+}
+
+int qed_dbg_feature_size(struct qed_dev *cdev, enum qed_dbg_features feature)
+{
+	struct qed_hwfn *p_hwfn =
+		&cdev->hwfns[cdev->dbg_params.engine_for_debug];
+	struct qed_ptt *p_ptt = qed_ptt_acquire(p_hwfn);
+	struct qed_dbg_feature *qed_feature =
+		&cdev->dbg_params.features[feature];
+	u32 buf_size_dwords;
+	enum dbg_status rc;
+
+	if (!p_ptt)
+		return -EINVAL;
+
+	rc = qed_features_lookup[feature].get_size(p_hwfn, p_ptt,
+						   &buf_size_dwords);
+	if (rc != DBG_STATUS_OK)
+		buf_size_dwords = 0;
+
+	qed_ptt_release(p_hwfn, p_ptt);
+	qed_feature->buf_size = buf_size_dwords * sizeof(u32);
+	return qed_feature->buf_size;
+}
+
+u8 qed_get_debug_engine(struct qed_dev *cdev)
+{
+	return cdev->dbg_params.engine_for_debug;
+}
+
+void qed_set_debug_engine(struct qed_dev *cdev, int engine_number)
+{
+	DP_VERBOSE(cdev, QED_MSG_DEBUG, "set debug engine to %d\n",
+		   engine_number);
+	cdev->dbg_params.engine_for_debug = engine_number;
+}
+
+void qed_dbg_pf_init(struct qed_dev *cdev)
+{
+	const u8 *dbg_values;
+
+	/* Debug values are after init values.
+	 * The offset is the first dword of the file.
+	 */
+	dbg_values = cdev->firmware->data + *(u32 *)cdev->firmware->data;
+	qed_dbg_set_bin_ptr((u8 *)dbg_values);
+	qed_dbg_user_set_bin_ptr((u8 *)dbg_values);
+}
+
+void qed_dbg_pf_exit(struct qed_dev *cdev)
+{
+	struct qed_dbg_feature *feature = NULL;
+	enum qed_dbg_features feature_idx;
+
+	/* Debug features' buffers may be allocated if debug feature was used
+	 * but dump wasn't called.
+	 */
+	for (feature_idx = 0; feature_idx < DBG_FEATURE_NUM; feature_idx++) {
+		feature = &cdev->dbg_params.features[feature_idx];
+		if (feature->dump_buf) {
+			vfree(feature->dump_buf);
+			feature->dump_buf = NULL;
+		}
+	}
+}
diff --git a/drivers/net/ethernet/qlogic/qed/qed_debug.h b/drivers/net/ethernet/qlogic/qed/qed_debug.h
new file mode 100644
index 000000000000..f872d7324814
--- /dev/null
+++ b/drivers/net/ethernet/qlogic/qed/qed_debug.h
@@ -0,0 +1,54 @@
+/* QLogic qed NIC Driver
+ * Copyright (c) 2015 QLogic Corporation
+ *
+ * This software is available 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.
+ */
+
+#ifndef _QED_DEBUGFS_H
+#define _QED_DEBUGFS_H
+
+enum qed_dbg_features {
+	DBG_FEATURE_GRC,
+	DBG_FEATURE_IDLE_CHK,
+	DBG_FEATURE_MCP_TRACE,
+	DBG_FEATURE_REG_FIFO,
+	DBG_FEATURE_IGU_FIFO,
+	DBG_FEATURE_PROTECTION_OVERRIDE,
+	DBG_FEATURE_FW_ASSERTS,
+	DBG_FEATURE_NUM
+};
+
+int qed_dbg_grc(struct qed_dev *cdev, void *buffer, u32 *num_dumped_bytes);
+int qed_dbg_grc_size(struct qed_dev *cdev);
+int qed_dbg_idle_chk(struct qed_dev *cdev, void *buffer,
+		     u32 *num_dumped_bytes);
+int qed_dbg_idle_chk_size(struct qed_dev *cdev);
+int qed_dbg_reg_fifo(struct qed_dev *cdev, void *buffer,
+		     u32 *num_dumped_bytes);
+int qed_dbg_reg_fifo_size(struct qed_dev *cdev);
+int qed_dbg_igu_fifo(struct qed_dev *cdev, void *buffer,
+		     u32 *num_dumped_bytes);
+int qed_dbg_igu_fifo_size(struct qed_dev *cdev);
+int qed_dbg_protection_override(struct qed_dev *cdev, void *buffer,
+				u32 *num_dumped_bytes);
+int qed_dbg_protection_override_size(struct qed_dev *cdev);
+int qed_dbg_fw_asserts(struct qed_dev *cdev, void *buffer,
+		       u32 *num_dumped_bytes);
+int qed_dbg_fw_asserts_size(struct qed_dev *cdev);
+int qed_dbg_mcp_trace(struct qed_dev *cdev, void *buffer,
+		      u32 *num_dumped_bytes);
+int qed_dbg_mcp_trace_size(struct qed_dev *cdev);
+int qed_dbg_all_data(struct qed_dev *cdev, void *buffer);
+int qed_dbg_all_data_size(struct qed_dev *cdev);
+u8 qed_get_debug_engine(struct qed_dev *cdev);
+void qed_set_debug_engine(struct qed_dev *cdev, int engine_number);
+int qed_dbg_feature(struct qed_dev *cdev, void *buffer,
+		    enum qed_dbg_features feature, u32 *num_dumped_bytes);
+int qed_dbg_feature_size(struct qed_dev *cdev, enum qed_dbg_features feature);
+
+void qed_dbg_pf_init(struct qed_dev *cdev);
+void qed_dbg_pf_exit(struct qed_dev *cdev);
+
+#endif
diff --git a/drivers/net/ethernet/qlogic/qed/qed_dev.c b/drivers/net/ethernet/qlogic/qed/qed_dev.c
index 0e4f4a9306b5..754f6a908858 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_dev.c
+++ b/drivers/net/ethernet/qlogic/qed/qed_dev.c
@@ -29,14 +29,18 @@
 #include "qed_hw.h"
 #include "qed_init_ops.h"
 #include "qed_int.h"
+#include "qed_ll2.h"
 #include "qed_mcp.h"
 #include "qed_reg_addr.h"
 #include "qed_sp.h"
 #include "qed_sriov.h"
 #include "qed_vf.h"
+#include "qed_roce.h"
 
-static spinlock_t qm_lock;
-static bool qm_lock_init = false;
+static DEFINE_SPINLOCK(qm_lock);
+
+#define QED_MIN_DPIS            (4)
+#define QED_MIN_PWM_REGION      (QED_WID_SIZE * QED_MIN_DPIS)
 
 /* API common to all protocols */
 enum BAR_ID {
@@ -44,8 +48,7 @@ enum BAR_ID {
 	BAR_ID_1        /* Used for doorbells */
 };
 
-static u32 qed_hw_bar_size(struct qed_hwfn	*p_hwfn,
-			   enum BAR_ID		bar_id)
+static u32 qed_hw_bar_size(struct qed_hwfn *p_hwfn, enum BAR_ID bar_id)
 {
 	u32 bar_reg = (bar_id == BAR_ID_0 ?
 		       PGLUE_B_REG_PF_BAR0_SIZE : PGLUE_B_REG_PF_BAR1_SIZE);
@@ -70,8 +73,7 @@ static u32 qed_hw_bar_size(struct qed_hwfn	*p_hwfn,
 	}
 }
 
-void qed_init_dp(struct qed_dev *cdev,
-		 u32 dp_module, u8 dp_level)
+void qed_init_dp(struct qed_dev *cdev, u32 dp_module, u8 dp_level)
 {
 	u32 i;
 
@@ -150,6 +152,9 @@ void qed_resc_free(struct qed_dev *cdev)
 		qed_eq_free(p_hwfn, p_hwfn->p_eq);
 		qed_consq_free(p_hwfn, p_hwfn->p_consq);
 		qed_int_free(p_hwfn);
+#ifdef CONFIG_QED_LL2
+		qed_ll2_free(p_hwfn, p_hwfn->p_ll2_info);
+#endif
 		qed_iov_free(p_hwfn);
 		qed_dmae_info_free(p_hwfn);
 		qed_dcbx_info_free(p_hwfn, p_hwfn->p_dcbx_info);
@@ -343,7 +348,6 @@ static int qed_init_qm_info(struct qed_hwfn *p_hwfn, bool b_sleepable)
 	return 0;
 
 alloc_err:
-	DP_NOTICE(p_hwfn, "Failed to allocate memory for QM params\n");
 	qed_qm_info_free(p_hwfn);
 	return -ENOMEM;
 }
@@ -407,6 +411,9 @@ int qed_qm_reconf(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
 
 int qed_resc_alloc(struct qed_dev *cdev)
 {
+#ifdef CONFIG_QED_LL2
+	struct qed_ll2_info *p_ll2_info;
+#endif
 	struct qed_consq *p_consq;
 	struct qed_eq *p_eq;
 	int i, rc = 0;
@@ -427,18 +434,12 @@ int qed_resc_alloc(struct qed_dev *cdev)
 				     RESC_NUM(p_hwfn, QED_L2_QUEUE);
 
 		p_hwfn->p_tx_cids = kzalloc(tx_size, GFP_KERNEL);
-		if (!p_hwfn->p_tx_cids) {
-			DP_NOTICE(p_hwfn,
-				  "Failed to allocate memory for Tx Cids\n");
+		if (!p_hwfn->p_tx_cids)
 			goto alloc_no_mem;
-		}
 
 		p_hwfn->p_rx_cids = kzalloc(rx_size, GFP_KERNEL);
-		if (!p_hwfn->p_rx_cids) {
-			DP_NOTICE(p_hwfn,
-				  "Failed to allocate memory for Rx Cids\n");
+		if (!p_hwfn->p_rx_cids)
 			goto alloc_no_mem;
-		}
 	}
 
 	for_each_hwfn(cdev, i) {
@@ -523,29 +524,29 @@ int qed_resc_alloc(struct qed_dev *cdev)
 			goto alloc_no_mem;
 		p_hwfn->p_consq = p_consq;
 
+#ifdef CONFIG_QED_LL2
+		if (p_hwfn->using_ll2) {
+			p_ll2_info = qed_ll2_alloc(p_hwfn);
+			if (!p_ll2_info)
+				goto alloc_no_mem;
+			p_hwfn->p_ll2_info = p_ll2_info;
+		}
+#endif
+
 		/* DMA info initialization */
 		rc = qed_dmae_info_alloc(p_hwfn);
-		if (rc) {
-			DP_NOTICE(p_hwfn,
-				  "Failed to allocate memory for dmae_info structure\n");
+		if (rc)
 			goto alloc_err;
-		}
 
 		/* DCBX initialization */
 		rc = qed_dcbx_info_alloc(p_hwfn);
-		if (rc) {
-			DP_NOTICE(p_hwfn,
-				  "Failed to allocate memory for dcbx structure\n");
+		if (rc)
 			goto alloc_err;
-		}
 	}
 
 	cdev->reset_stats = kzalloc(sizeof(*cdev->reset_stats), GFP_KERNEL);
-	if (!cdev->reset_stats) {
-		DP_NOTICE(cdev, "Failed to allocate reset statistics\n");
-		rc = -ENOMEM;
-		goto alloc_err;
-	}
+	if (!cdev->reset_stats)
+		goto alloc_no_mem;
 
 	return 0;
 
@@ -580,6 +581,10 @@ void qed_resc_setup(struct qed_dev *cdev)
 		qed_int_setup(p_hwfn, p_hwfn->p_main_ptt);
 
 		qed_iov_setup(p_hwfn, p_hwfn->p_main_ptt);
+#ifdef CONFIG_QED_LL2
+		if (p_hwfn->using_ll2)
+			qed_ll2_setup(p_hwfn, p_hwfn->p_ll2_info);
+#endif
 	}
 }
 
@@ -605,9 +610,8 @@ int qed_final_cleanup(struct qed_hwfn *p_hwfn,
 
 	/* Make sure notification is not set before initiating final cleanup */
 	if (REG_RD(p_hwfn, addr)) {
-		DP_NOTICE(
-			p_hwfn,
-			"Unexpected; Found final cleanup notification before initiating final cleanup\n");
+		DP_NOTICE(p_hwfn,
+			  "Unexpected; Found final cleanup notification before initiating final cleanup\n");
 		REG_WR(p_hwfn, addr, 0);
 	}
 
@@ -701,17 +705,14 @@ static void qed_init_cau_rt_data(struct qed_dev *cdev)
 				continue;
 
 			qed_init_cau_sb_entry(p_hwfn, &sb_entry,
-					      p_block->function_id,
-					      0, 0);
-			STORE_RT_REG_AGG(p_hwfn, offset + sb_id * 2,
-					 sb_entry);
+					      p_block->function_id, 0, 0);
+			STORE_RT_REG_AGG(p_hwfn, offset + sb_id * 2, sb_entry);
 		}
 	}
 }
 
 static int qed_hw_init_common(struct qed_hwfn *p_hwfn,
-			      struct qed_ptt *p_ptt,
-			      int hw_mode)
+			      struct qed_ptt *p_ptt, int hw_mode)
 {
 	struct qed_qm_info *qm_info = &p_hwfn->qm_info;
 	struct qed_qm_common_rt_init_params params;
@@ -759,7 +760,7 @@ static int qed_hw_init_common(struct qed_hwfn *p_hwfn,
 	qed_port_unpretend(p_hwfn, p_ptt);
 
 	rc = qed_init_run(p_hwfn, p_ptt, PHASE_ENGINE, ANY_PHASE_ID, hw_mode);
-	if (rc != 0)
+	if (rc)
 		return rc;
 
 	qed_wr(p_hwfn, p_ptt, PSWRQ2_REG_L2P_VALIDATE_VFID, 0);
@@ -780,6 +781,9 @@ static int qed_hw_init_common(struct qed_hwfn *p_hwfn,
 		concrete_fid = qed_vfid_to_concrete(p_hwfn, vf_id);
 		qed_fid_pretend(p_hwfn, p_ptt, (u16) concrete_fid);
 		qed_wr(p_hwfn, p_ptt, CCFC_REG_STRONG_ENABLE_VF, 0x1);
+		qed_wr(p_hwfn, p_ptt, CCFC_REG_WEAK_ENABLE_VF, 0x0);
+		qed_wr(p_hwfn, p_ptt, TCFC_REG_STRONG_ENABLE_VF, 0x1);
+		qed_wr(p_hwfn, p_ptt, TCFC_REG_WEAK_ENABLE_VF, 0x0);
 	}
 	/* pretend to original PF */
 	qed_fid_pretend(p_hwfn, p_ptt, p_hwfn->rel_pf_id);
@@ -787,38 +791,141 @@ static int qed_hw_init_common(struct qed_hwfn *p_hwfn,
 	return rc;
 }
 
-static int qed_hw_init_port(struct qed_hwfn *p_hwfn,
-			    struct qed_ptt *p_ptt,
-			    int hw_mode)
+static int
+qed_hw_init_dpi_size(struct qed_hwfn *p_hwfn,
+		     struct qed_ptt *p_ptt, u32 pwm_region_size, u32 n_cpus)
 {
-	int rc = 0;
+	u32 dpi_page_size_1, dpi_page_size_2, dpi_page_size;
+	u32 dpi_bit_shift, dpi_count;
+	u32 min_dpis;
 
-	rc = qed_init_run(p_hwfn, p_ptt, PHASE_PORT, p_hwfn->port_id, hw_mode);
-	if (rc != 0)
-		return rc;
+	/* Calculate DPI size */
+	dpi_page_size_1 = QED_WID_SIZE * n_cpus;
+	dpi_page_size_2 = max_t(u32, QED_WID_SIZE, PAGE_SIZE);
+	dpi_page_size = max_t(u32, dpi_page_size_1, dpi_page_size_2);
+	dpi_page_size = roundup_pow_of_two(dpi_page_size);
+	dpi_bit_shift = ilog2(dpi_page_size / 4096);
 
-	if (hw_mode & (1 << MODE_MF_SI)) {
-		u8 pf_id = 0;
+	dpi_count = pwm_region_size / dpi_page_size;
 
-		if (!qed_hw_init_first_eth(p_hwfn, p_ptt, &pf_id)) {
-			DP_VERBOSE(p_hwfn, NETIF_MSG_IFUP,
-				   "PF[%08x] is first eth on engine\n", pf_id);
+	min_dpis = p_hwfn->pf_params.rdma_pf_params.min_dpis;
+	min_dpis = max_t(u32, QED_MIN_DPIS, min_dpis);
 
-			/* We should have configured BIT for ppfid, i.e., the
-			 * relative function number in the port. But there's a
-			 * bug in LLH in BB where the ppfid is actually engine
-			 * based, so we need to take this into account.
-			 */
-			qed_wr(p_hwfn, p_ptt,
-			       NIG_REG_LLH_TAGMAC_DEF_PF_VECTOR, 1 << pf_id);
-		}
+	p_hwfn->dpi_size = dpi_page_size;
+	p_hwfn->dpi_count = dpi_count;
+
+	qed_wr(p_hwfn, p_ptt, DORQ_REG_PF_DPI_BIT_SHIFT, dpi_bit_shift);
+
+	if (dpi_count < min_dpis)
+		return -EINVAL;
+
+	return 0;
+}
+
+enum QED_ROCE_EDPM_MODE {
+	QED_ROCE_EDPM_MODE_ENABLE = 0,
+	QED_ROCE_EDPM_MODE_FORCE_ON = 1,
+	QED_ROCE_EDPM_MODE_DISABLE = 2,
+};
 
-		/* Take the protocol-based hit vector if there is a hit,
-		 * otherwise take the other vector.
+static int
+qed_hw_init_pf_doorbell_bar(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
+{
+	u32 pwm_regsize, norm_regsize;
+	u32 non_pwm_conn, min_addr_reg1;
+	u32 db_bar_size, n_cpus;
+	u32 roce_edpm_mode;
+	u32 pf_dems_shift;
+	int rc = 0;
+	u8 cond;
+
+	db_bar_size = qed_hw_bar_size(p_hwfn, BAR_ID_1);
+	if (p_hwfn->cdev->num_hwfns > 1)
+		db_bar_size /= 2;
+
+	/* Calculate doorbell regions */
+	non_pwm_conn = qed_cxt_get_proto_cid_start(p_hwfn, PROTOCOLID_CORE) +
+		       qed_cxt_get_proto_cid_count(p_hwfn, PROTOCOLID_CORE,
+						   NULL) +
+		       qed_cxt_get_proto_cid_count(p_hwfn, PROTOCOLID_ETH,
+						   NULL);
+	norm_regsize = roundup(QED_PF_DEMS_SIZE * non_pwm_conn, 4096);
+	min_addr_reg1 = norm_regsize / 4096;
+	pwm_regsize = db_bar_size - norm_regsize;
+
+	/* Check that the normal and PWM sizes are valid */
+	if (db_bar_size < norm_regsize) {
+		DP_ERR(p_hwfn->cdev,
+		       "Doorbell BAR size 0x%x is too small (normal region is 0x%0x )\n",
+		       db_bar_size, norm_regsize);
+		return -EINVAL;
+	}
+
+	if (pwm_regsize < QED_MIN_PWM_REGION) {
+		DP_ERR(p_hwfn->cdev,
+		       "PWM region size 0x%0x is too small. Should be at least 0x%0x (Doorbell BAR size is 0x%x and normal region size is 0x%0x)\n",
+		       pwm_regsize,
+		       QED_MIN_PWM_REGION, db_bar_size, norm_regsize);
+		return -EINVAL;
+	}
+
+	/* Calculate number of DPIs */
+	roce_edpm_mode = p_hwfn->pf_params.rdma_pf_params.roce_edpm_mode;
+	if ((roce_edpm_mode == QED_ROCE_EDPM_MODE_ENABLE) ||
+	    ((roce_edpm_mode == QED_ROCE_EDPM_MODE_FORCE_ON))) {
+		/* Either EDPM is mandatory, or we are attempting to allocate a
+		 * WID per CPU.
 		 */
-		qed_wr(p_hwfn, p_ptt, NIG_REG_LLH_CLS_TYPE_DUALMODE, 0x2);
+		n_cpus = num_active_cpus();
+		rc = qed_hw_init_dpi_size(p_hwfn, p_ptt, pwm_regsize, n_cpus);
 	}
-	return rc;
+
+	cond = (rc && (roce_edpm_mode == QED_ROCE_EDPM_MODE_ENABLE)) ||
+	       (roce_edpm_mode == QED_ROCE_EDPM_MODE_DISABLE);
+	if (cond || p_hwfn->dcbx_no_edpm) {
+		/* Either EDPM is disabled from user configuration, or it is
+		 * disabled via DCBx, or it is not mandatory and we failed to
+		 * allocated a WID per CPU.
+		 */
+		n_cpus = 1;
+		rc = qed_hw_init_dpi_size(p_hwfn, p_ptt, pwm_regsize, n_cpus);
+
+		if (cond)
+			qed_rdma_dpm_bar(p_hwfn, p_ptt);
+	}
+
+	DP_INFO(p_hwfn,
+		"doorbell bar: normal_region_size=%d, pwm_region_size=%d, dpi_size=%d, dpi_count=%d, roce_edpm=%s\n",
+		norm_regsize,
+		pwm_regsize,
+		p_hwfn->dpi_size,
+		p_hwfn->dpi_count,
+		((p_hwfn->dcbx_no_edpm) || (p_hwfn->db_bar_no_edpm)) ?
+		"disabled" : "enabled");
+
+	if (rc) {
+		DP_ERR(p_hwfn,
+		       "Failed to allocate enough DPIs. Allocated %d but the current minimum is %d.\n",
+		       p_hwfn->dpi_count,
+		       p_hwfn->pf_params.rdma_pf_params.min_dpis);
+		return -EINVAL;
+	}
+
+	p_hwfn->dpi_start_offset = norm_regsize;
+
+	/* DEMS size is configured log2 of DWORDs, hence the division by 4 */
+	pf_dems_shift = ilog2(QED_PF_DEMS_SIZE / 4);
+	qed_wr(p_hwfn, p_ptt, DORQ_REG_PF_ICID_BIT_SHIFT_NORM, pf_dems_shift);
+	qed_wr(p_hwfn, p_ptt, DORQ_REG_PF_MIN_ADDR_REG1, min_addr_reg1);
+
+	return 0;
+}
+
+static int qed_hw_init_port(struct qed_hwfn *p_hwfn,
+			    struct qed_ptt *p_ptt, int hw_mode)
+{
+	return qed_init_run(p_hwfn, p_ptt, PHASE_PORT,
+			    p_hwfn->port_id, hw_mode);
 }
 
 static int qed_hw_init_pf(struct qed_hwfn *p_hwfn,
@@ -848,7 +955,7 @@ static int qed_hw_init_pf(struct qed_hwfn *p_hwfn,
 	qed_int_igu_init_rt(p_hwfn);
 
 	/* Set VLAN in NIG if needed */
-	if (hw_mode & (1 << MODE_MF_SD)) {
+	if (hw_mode & BIT(MODE_MF_SD)) {
 		DP_VERBOSE(p_hwfn, NETIF_MSG_HW, "Configuring LLH_FUNC_TAG\n");
 		STORE_RT_REG(p_hwfn, NIG_REG_LLH_FUNC_TAG_EN_RT_OFFSET, 1);
 		STORE_RT_REG(p_hwfn, NIG_REG_LLH_FUNC_TAG_VALUE_RT_OFFSET,
@@ -856,7 +963,7 @@ static int qed_hw_init_pf(struct qed_hwfn *p_hwfn,
 	}
 
 	/* Enable classification by MAC if needed */
-	if (hw_mode & (1 << MODE_MF_SI)) {
+	if (hw_mode & BIT(MODE_MF_SI)) {
 		DP_VERBOSE(p_hwfn, NETIF_MSG_HW,
 			   "Configuring TAGMAC_CLS_TYPE\n");
 		STORE_RT_REG(p_hwfn,
@@ -871,7 +978,7 @@ static int qed_hw_init_pf(struct qed_hwfn *p_hwfn,
 
 	/* Cleanup chip from previous driver if such remains exist */
 	rc = qed_final_cleanup(p_hwfn, p_ptt, rel_pf_id, false);
-	if (rc != 0)
+	if (rc)
 		return rc;
 
 	/* PF Init sequence */
@@ -887,20 +994,9 @@ static int qed_hw_init_pf(struct qed_hwfn *p_hwfn,
 	/* Pure runtime initializations - directly to the HW  */
 	qed_int_igu_init_pure_rt(p_hwfn, p_ptt, true, true);
 
-	if (hw_mode & (1 << MODE_MF_SI)) {
-		u8 pf_id = 0;
-		u32 val = 0;
-
-		if (!qed_hw_init_first_eth(p_hwfn, p_ptt, &pf_id)) {
-			if (p_hwfn->rel_pf_id == pf_id) {
-				DP_VERBOSE(p_hwfn, NETIF_MSG_IFUP,
-					   "PF[%d] is first ETH on engine\n",
-					   pf_id);
-				val = 1;
-			}
-			qed_wr(p_hwfn, p_ptt, PRS_REG_MSG_INFO, val);
-		}
-	}
+	rc = qed_hw_init_pf_doorbell_bar(p_hwfn, p_ptt);
+	if (rc)
+		return rc;
 
 	if (b_hw_start) {
 		/* enable interrupts */
@@ -950,8 +1046,7 @@ static void qed_reset_mb_shadow(struct qed_hwfn *p_hwfn,
 	/* Read shadow of current MFW mailbox */
 	qed_mcp_read_mb(p_hwfn, p_main_ptt);
 	memcpy(p_hwfn->mcp_info->mfw_mb_shadow,
-	       p_hwfn->mcp_info->mfw_mb_cur,
-	       p_hwfn->mcp_info->mfw_mb_length);
+	       p_hwfn->mcp_info->mfw_mb_cur, p_hwfn->mcp_info->mfw_mb_length);
 }
 
 int qed_hw_init(struct qed_dev *cdev,
@@ -971,7 +1066,7 @@ int qed_hw_init(struct qed_dev *cdev,
 
 	if (IS_PF(cdev)) {
 		rc = qed_init_fw_data(cdev, bin_fw_data);
-		if (rc != 0)
+		if (rc)
 			return rc;
 	}
 
@@ -988,8 +1083,7 @@ int qed_hw_init(struct qed_dev *cdev,
 
 		qed_calc_hw_mode(p_hwfn);
 
-		rc = qed_mcp_load_req(p_hwfn, p_hwfn->p_main_ptt,
-				      &load_code);
+		rc = qed_mcp_load_req(p_hwfn, p_hwfn->p_main_ptt, &load_code);
 		if (rc) {
 			DP_NOTICE(p_hwfn, "Failed sending LOAD_REQ command\n");
 			return rc;
@@ -1004,11 +1098,6 @@ int qed_hw_init(struct qed_dev *cdev,
 		p_hwfn->first_on_engine = (load_code ==
 					   FW_MSG_CODE_DRV_LOAD_ENGINE);
 
-		if (!qm_lock_init) {
-			spin_lock_init(&qm_lock);
-			qm_lock_init = true;
-		}
-
 		switch (load_code) {
 		case FW_MSG_CODE_DRV_LOAD_ENGINE:
 			rc = qed_hw_init_common(p_hwfn, p_hwfn->p_main_ptt,
@@ -1071,9 +1160,8 @@ int qed_hw_init(struct qed_dev *cdev,
 }
 
 #define QED_HW_STOP_RETRY_LIMIT (10)
-static inline void qed_hw_timers_stop(struct qed_dev *cdev,
-				      struct qed_hwfn *p_hwfn,
-				      struct qed_ptt *p_ptt)
+static void qed_hw_timers_stop(struct qed_dev *cdev,
+			       struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
 {
 	int i;
 
@@ -1084,8 +1172,7 @@ static inline void qed_hw_timers_stop(struct qed_dev *cdev,
 	for (i = 0; i < QED_HW_STOP_RETRY_LIMIT; i++) {
 		if ((!qed_rd(p_hwfn, p_ptt,
 			     TM_REG_PF_SCAN_ACTIVE_CONN)) &&
-		    (!qed_rd(p_hwfn, p_ptt,
-			     TM_REG_PF_SCAN_ACTIVE_TASK)))
+		    (!qed_rd(p_hwfn, p_ptt, TM_REG_PF_SCAN_ACTIVE_TASK)))
 			break;
 
 		/* Dependent on number of connection/tasks, possibly
@@ -1190,8 +1277,7 @@ void qed_hw_stop_fastpath(struct qed_dev *cdev)
 		}
 
 		DP_VERBOSE(p_hwfn,
-			   NETIF_MSG_IFDOWN,
-			   "Shutting down the fastpath\n");
+			   NETIF_MSG_IFDOWN, "Shutting down the fastpath\n");
 
 		qed_wr(p_hwfn, p_ptt,
 		       NIG_REG_RX_LLH_BRB_GATE_DNTFWD_PERPF, 0x1);
@@ -1219,14 +1305,13 @@ void qed_hw_start_fastpath(struct qed_hwfn *p_hwfn)
 	       NIG_REG_RX_LLH_BRB_GATE_DNTFWD_PERPF, 0x0);
 }
 
-static int qed_reg_assert(struct qed_hwfn *hwfn,
-			  struct qed_ptt *ptt, u32 reg,
-			  bool expected)
+static int qed_reg_assert(struct qed_hwfn *p_hwfn,
+			  struct qed_ptt *p_ptt, u32 reg, bool expected)
 {
-	u32 assert_val = qed_rd(hwfn, ptt, reg);
+	u32 assert_val = qed_rd(p_hwfn, p_ptt, reg);
 
 	if (assert_val != expected) {
-		DP_NOTICE(hwfn, "Value at address 0x%x != 0x%08x\n",
+		DP_NOTICE(p_hwfn, "Value at address 0x%08x != 0x%08x\n",
 			  reg, expected);
 		return -EINVAL;
 	}
@@ -1306,8 +1391,7 @@ static void qed_hw_hwfn_prepare(struct qed_hwfn *p_hwfn)
 
 	/* Clean Previous errors if such exist */
 	qed_wr(p_hwfn, p_hwfn->p_main_ptt,
-	       PGLUE_B_REG_WAS_ERROR_PF_31_0_CLR,
-	       1 << p_hwfn->abs_pf_id);
+	       PGLUE_B_REG_WAS_ERROR_PF_31_0_CLR, 1 << p_hwfn->abs_pf_id);
 
 	/* enable internal target-read */
 	qed_wr(p_hwfn, p_hwfn->p_main_ptt,
@@ -1317,7 +1401,8 @@ static void qed_hw_hwfn_prepare(struct qed_hwfn *p_hwfn)
 static void get_function_id(struct qed_hwfn *p_hwfn)
 {
 	/* ME Register */
-	p_hwfn->hw_info.opaque_fid = (u16)REG_RD(p_hwfn, PXP_PF_ME_OPAQUE_ADDR);
+	p_hwfn->hw_info.opaque_fid = (u16) REG_RD(p_hwfn,
+						  PXP_PF_ME_OPAQUE_ADDR);
 
 	p_hwfn->hw_info.concrete_fid = REG_RD(p_hwfn, PXP_PF_ME_CONCRETE_ADDR);
 
@@ -1326,6 +1411,10 @@ static void get_function_id(struct qed_hwfn *p_hwfn)
 				      PXP_CONCRETE_FID_PFID);
 	p_hwfn->port_id = GET_FIELD(p_hwfn->hw_info.concrete_fid,
 				    PXP_CONCRETE_FID_PORT);
+
+	DP_VERBOSE(p_hwfn, NETIF_MSG_PROBE,
+		   "Read ME register: Concrete 0x%08x Opaque 0x%04x\n",
+		   p_hwfn->hw_info.concrete_fid, p_hwfn->hw_info.opaque_fid);
 }
 
 static void qed_hw_set_feat(struct qed_hwfn *p_hwfn)
@@ -1333,6 +1422,19 @@ static void qed_hw_set_feat(struct qed_hwfn *p_hwfn)
 	u32 *feat_num = p_hwfn->hw_info.feat_num;
 	int num_features = 1;
 
+#if IS_ENABLED(CONFIG_INFINIBAND_QEDR)
+	/* Roce CNQ each requires: 1 status block + 1 CNQ. We divide the
+	 * status blocks equally between L2 / RoCE but with consideration as
+	 * to how many l2 queues / cnqs we have
+	 */
+	if (p_hwfn->hw_info.personality == QED_PCI_ETH_ROCE) {
+		num_features++;
+
+		feat_num[QED_RDMA_CNQ] =
+			min_t(u32, RESC_NUM(p_hwfn, QED_SB) / num_features,
+			      RESC_NUM(p_hwfn, QED_RDMA_CNQ_RAM));
+	}
+#endif
 	feat_num[QED_PF_L2_QUE] = min_t(u32, RESC_NUM(p_hwfn, QED_SB) /
 						num_features,
 					RESC_NUM(p_hwfn, QED_L2_QUEUE));
@@ -1373,6 +1475,10 @@ static int qed_hw_get_resc(struct qed_hwfn *p_hwfn)
 	resc_num[QED_VLAN] = (ETH_NUM_VLAN_FILTERS - 1 /*For vlan0*/) /
 			     num_funcs;
 	resc_num[QED_ILT] = PXP_NUM_ILT_RECORDS_BB / num_funcs;
+	resc_num[QED_LL2_QUEUE] = MAX_NUM_LL2_RX_QUEUES / num_funcs;
+	resc_num[QED_RDMA_CNQ_RAM] = NUM_OF_CMDQS_CQS / num_funcs;
+	resc_num[QED_RDMA_STATS_QUEUE] = RDMA_NUM_STATISTIC_COUNTERS_BB /
+					 num_funcs;
 
 	for (i = 0; i < QED_MAX_RESC; i++)
 		resc_start[i] = resc_num[i] * enabled_func_idx;
@@ -1396,7 +1502,8 @@ static int qed_hw_get_resc(struct qed_hwfn *p_hwfn)
 		   "RL = %d start = %d\n"
 		   "MAC = %d start = %d\n"
 		   "VLAN = %d start = %d\n"
-		   "ILT = %d start = %d\n",
+		   "ILT = %d start = %d\n"
+		   "LL2_QUEUE = %d start = %d\n",
 		   p_hwfn->hw_info.resc_num[QED_SB],
 		   p_hwfn->hw_info.resc_start[QED_SB],
 		   p_hwfn->hw_info.resc_num[QED_L2_QUEUE],
@@ -1412,13 +1519,14 @@ static int qed_hw_get_resc(struct qed_hwfn *p_hwfn)
 		   p_hwfn->hw_info.resc_num[QED_VLAN],
 		   p_hwfn->hw_info.resc_start[QED_VLAN],
 		   p_hwfn->hw_info.resc_num[QED_ILT],
-		   p_hwfn->hw_info.resc_start[QED_ILT]);
+		   p_hwfn->hw_info.resc_start[QED_ILT],
+		   RESC_NUM(p_hwfn, QED_LL2_QUEUE),
+		   RESC_START(p_hwfn, QED_LL2_QUEUE));
 
 	return 0;
 }
 
-static int qed_hw_get_nvm_info(struct qed_hwfn *p_hwfn,
-			       struct qed_ptt *p_ptt)
+static int qed_hw_get_nvm_info(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
 {
 	u32 nvm_cfg1_offset, mf_mode, addr, generic_cont0, core_cfg;
 	u32 port_cfg_addr, link_temp, nvm_cfg_addr, device_capabilities;
@@ -1472,8 +1580,7 @@ static int qed_hw_get_nvm_info(struct qed_hwfn *p_hwfn,
 		p_hwfn->hw_info.port_mode = QED_PORT_MODE_DE_1X25G;
 		break;
 	default:
-		DP_NOTICE(p_hwfn, "Unknown port mode in 0x%08x\n",
-			  core_cfg);
+		DP_NOTICE(p_hwfn, "Unknown port mode in 0x%08x\n", core_cfg);
 		break;
 	}
 
@@ -1484,11 +1591,11 @@ static int qed_hw_get_nvm_info(struct qed_hwfn *p_hwfn,
 	link_temp = qed_rd(p_hwfn, p_ptt,
 			   port_cfg_addr +
 			   offsetof(struct nvm_cfg1_port, speed_cap_mask));
-	link->speed.advertised_speeds =
-		link_temp & NVM_CFG1_PORT_DRV_SPEED_CAPABILITY_MASK_MASK;
+	link_temp &= NVM_CFG1_PORT_DRV_SPEED_CAPABILITY_MASK_MASK;
+	link->speed.advertised_speeds = link_temp;
 
-	p_hwfn->mcp_info->link_capabilities.speed_capabilities =
-						link->speed.advertised_speeds;
+	link_temp = link->speed.advertised_speeds;
+	p_hwfn->mcp_info->link_capabilities.speed_capabilities = link_temp;
 
 	link_temp = qed_rd(p_hwfn, p_ptt,
 			   port_cfg_addr +
@@ -1517,8 +1624,7 @@ static int qed_hw_get_nvm_info(struct qed_hwfn *p_hwfn,
 		link->speed.forced_speed = 100000;
 		break;
 	default:
-		DP_NOTICE(p_hwfn, "Unknown Speed in 0x%08x\n",
-			  link_temp);
+		DP_NOTICE(p_hwfn, "Unknown Speed in 0x%08x\n", link_temp);
 	}
 
 	link_temp &= NVM_CFG1_PORT_DRV_FLOW_CONTROL_MASK;
@@ -1628,10 +1734,10 @@ static void qed_get_num_funcs(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
 
 	DP_VERBOSE(p_hwfn,
 		   NETIF_MSG_PROBE,
-		   "PF [rel_id %d, abs_id %d] within the %d enabled functions on the engine\n",
+		   "PF [rel_id %d, abs_id %d] occupies index %d within the %d enabled functions on the engine\n",
 		   p_hwfn->rel_pf_id,
 		   p_hwfn->abs_pf_id,
-		   p_hwfn->num_funcs_on_engine);
+		   p_hwfn->enabled_func_idx, p_hwfn->num_funcs_on_engine);
 }
 
 static int
@@ -1703,10 +1809,9 @@ static int qed_get_dev_info(struct qed_dev *cdev)
 	u32 tmp;
 
 	/* Read Vendor Id / Device Id */
-	pci_read_config_word(cdev->pdev, PCI_VENDOR_ID,
-			     &cdev->vendor_id);
-	pci_read_config_word(cdev->pdev, PCI_DEVICE_ID,
-			     &cdev->device_id);
+	pci_read_config_word(cdev->pdev, PCI_VENDOR_ID, &cdev->vendor_id);
+	pci_read_config_word(cdev->pdev, PCI_DEVICE_ID, &cdev->device_id);
+
 	cdev->chip_num = (u16)qed_rd(p_hwfn, p_hwfn->p_main_ptt,
 				     MISCS_REG_CHIP_NUM);
 	cdev->chip_rev = (u16)qed_rd(p_hwfn, p_hwfn->p_main_ptt,
@@ -1771,10 +1876,8 @@ static int qed_hw_prepare_single(struct qed_hwfn *p_hwfn,
 
 	/* Allocate PTT pool */
 	rc = qed_ptt_pool_alloc(p_hwfn);
-	if (rc) {
-		DP_NOTICE(p_hwfn, "Failed to prepare hwfn's hw\n");
+	if (rc)
 		goto err0;
-	}
 
 	/* Allocate the main PTT */
 	p_hwfn->p_main_ptt = qed_get_reserved_ptt(p_hwfn, RESERVED_PTT_MAIN);
@@ -1782,7 +1885,7 @@ static int qed_hw_prepare_single(struct qed_hwfn *p_hwfn,
 	/* First hwfn learns basic information, e.g., number of hwfns */
 	if (!p_hwfn->my_id) {
 		rc = qed_get_dev_info(p_hwfn->cdev);
-		if (rc != 0)
+		if (rc)
 			goto err1;
 	}
 
@@ -1804,10 +1907,8 @@ static int qed_hw_prepare_single(struct qed_hwfn *p_hwfn,
 
 	/* Allocate the init RT array and initialize the init-ops engine */
 	rc = qed_init_alloc(p_hwfn);
-	if (rc) {
-		DP_NOTICE(p_hwfn, "Failed to allocate the init array\n");
+	if (rc)
 		goto err2;
-	}
 
 	return rc;
 err2:
@@ -2015,10 +2116,8 @@ qed_chain_alloc_next_ptr(struct qed_dev *cdev, struct qed_chain *p_chain)
 		p_virt = dma_alloc_coherent(&cdev->pdev->dev,
 					    QED_CHAIN_PAGE_SIZE,
 					    &p_phys, GFP_KERNEL);
-		if (!p_virt) {
-			DP_NOTICE(cdev, "Failed to allocate chain memory\n");
+		if (!p_virt)
 			return -ENOMEM;
-		}
 
 		if (i == 0) {
 			qed_chain_init_mem(p_chain, p_virt, p_phys);
@@ -2048,10 +2147,8 @@ qed_chain_alloc_single(struct qed_dev *cdev, struct qed_chain *p_chain)
 
 	p_virt = dma_alloc_coherent(&cdev->pdev->dev,
 				    QED_CHAIN_PAGE_SIZE, &p_phys, GFP_KERNEL);
-	if (!p_virt) {
-		DP_NOTICE(cdev, "Failed to allocate chain memory\n");
+	if (!p_virt)
 		return -ENOMEM;
-	}
 
 	qed_chain_init_mem(p_chain, p_virt, p_phys);
 	qed_chain_reset(p_chain);
@@ -2068,13 +2165,9 @@ static int qed_chain_alloc_pbl(struct qed_dev *cdev, struct qed_chain *p_chain)
 	void *p_virt = NULL;
 
 	size = page_cnt * sizeof(*pp_virt_addr_tbl);
-	pp_virt_addr_tbl = vmalloc(size);
-	if (!pp_virt_addr_tbl) {
-		DP_NOTICE(cdev,
-			  "Failed to allocate memory for the chain virtual addresses table\n");
+	pp_virt_addr_tbl = vzalloc(size);
+	if (!pp_virt_addr_tbl)
 		return -ENOMEM;
-	}
-	memset(pp_virt_addr_tbl, 0, size);
 
 	/* The allocation of the PBL table is done with its full size, since it
 	 * is expected to be successive.
@@ -2087,19 +2180,15 @@ static int qed_chain_alloc_pbl(struct qed_dev *cdev, struct qed_chain *p_chain)
 					size, &p_pbl_phys, GFP_KERNEL);
 	qed_chain_init_pbl_mem(p_chain, p_pbl_virt, p_pbl_phys,
 			       pp_virt_addr_tbl);
-	if (!p_pbl_virt) {
-		DP_NOTICE(cdev, "Failed to allocate chain pbl memory\n");
+	if (!p_pbl_virt)
 		return -ENOMEM;
-	}
 
 	for (i = 0; i < page_cnt; i++) {
 		p_virt = dma_alloc_coherent(&cdev->pdev->dev,
 					    QED_CHAIN_PAGE_SIZE,
 					    &p_phys, GFP_KERNEL);
-		if (!p_virt) {
-			DP_NOTICE(cdev, "Failed to allocate chain memory\n");
+		if (!p_virt)
 			return -ENOMEM;
-		}
 
 		if (i == 0) {
 			qed_chain_init_mem(p_chain, p_virt, p_phys);
@@ -2134,7 +2223,8 @@ int qed_chain_alloc(struct qed_dev *cdev,
 	rc = qed_chain_alloc_sanity_check(cdev, cnt_type, elem_size, page_cnt);
 	if (rc) {
 		DP_NOTICE(cdev,
-			  "Cannot allocate a chain with the given arguments:\n"
+			  "Cannot allocate a chain with the given arguments:\n");
+		DP_NOTICE(cdev,
 			  "[use_mode %d, mode %d, cnt_type %d, num_elems %d, elem_size %zu]\n",
 			  intended_use, mode, cnt_type, num_elems, elem_size);
 		return rc;
@@ -2183,8 +2273,7 @@ int qed_fw_l2_queue(struct qed_hwfn *p_hwfn, u16 src_id, u16 *dst_id)
 	return 0;
 }
 
-int qed_fw_vport(struct qed_hwfn *p_hwfn,
-		 u8 src_id, u8 *dst_id)
+int qed_fw_vport(struct qed_hwfn *p_hwfn, u8 src_id, u8 *dst_id)
 {
 	if (src_id >= RESC_NUM(p_hwfn, QED_VPORT)) {
 		u8 min, max;
@@ -2203,8 +2292,7 @@ int qed_fw_vport(struct qed_hwfn *p_hwfn,
 	return 0;
 }
 
-int qed_fw_rss_eng(struct qed_hwfn *p_hwfn,
-		   u8 src_id, u8 *dst_id)
+int qed_fw_rss_eng(struct qed_hwfn *p_hwfn, u8 src_id, u8 *dst_id)
 {
 	if (src_id >= RESC_NUM(p_hwfn, QED_RSS_ENG)) {
 		u8 min, max;
@@ -2223,6 +2311,98 @@ int qed_fw_rss_eng(struct qed_hwfn *p_hwfn,
 	return 0;
 }
 
+static void qed_llh_mac_to_filter(u32 *p_high, u32 *p_low,
+				  u8 *p_filter)
+{
+	*p_high = p_filter[1] | (p_filter[0] << 8);
+	*p_low = p_filter[5] | (p_filter[4] << 8) |
+		 (p_filter[3] << 16) | (p_filter[2] << 24);
+}
+
+int qed_llh_add_mac_filter(struct qed_hwfn *p_hwfn,
+			   struct qed_ptt *p_ptt, u8 *p_filter)
+{
+	u32 high = 0, low = 0, en;
+	int i;
+
+	if (!(IS_MF_SI(p_hwfn) || IS_MF_DEFAULT(p_hwfn)))
+		return 0;
+
+	qed_llh_mac_to_filter(&high, &low, p_filter);
+
+	/* Find a free entry and utilize it */
+	for (i = 0; i < NIG_REG_LLH_FUNC_FILTER_EN_SIZE; i++) {
+		en = qed_rd(p_hwfn, p_ptt,
+			    NIG_REG_LLH_FUNC_FILTER_EN + i * sizeof(u32));
+		if (en)
+			continue;
+		qed_wr(p_hwfn, p_ptt,
+		       NIG_REG_LLH_FUNC_FILTER_VALUE +
+		       2 * i * sizeof(u32), low);
+		qed_wr(p_hwfn, p_ptt,
+		       NIG_REG_LLH_FUNC_FILTER_VALUE +
+		       (2 * i + 1) * sizeof(u32), high);
+		qed_wr(p_hwfn, p_ptt,
+		       NIG_REG_LLH_FUNC_FILTER_MODE + i * sizeof(u32), 0);
+		qed_wr(p_hwfn, p_ptt,
+		       NIG_REG_LLH_FUNC_FILTER_PROTOCOL_TYPE +
+		       i * sizeof(u32), 0);
+		qed_wr(p_hwfn, p_ptt,
+		       NIG_REG_LLH_FUNC_FILTER_EN + i * sizeof(u32), 1);
+		break;
+	}
+	if (i >= NIG_REG_LLH_FUNC_FILTER_EN_SIZE) {
+		DP_NOTICE(p_hwfn,
+			  "Failed to find an empty LLH filter to utilize\n");
+		return -EINVAL;
+	}
+
+	DP_VERBOSE(p_hwfn, NETIF_MSG_HW,
+		   "mac: %pM is added at %d\n",
+		   p_filter, i);
+
+	return 0;
+}
+
+void qed_llh_remove_mac_filter(struct qed_hwfn *p_hwfn,
+			       struct qed_ptt *p_ptt, u8 *p_filter)
+{
+	u32 high = 0, low = 0;
+	int i;
+
+	if (!(IS_MF_SI(p_hwfn) || IS_MF_DEFAULT(p_hwfn)))
+		return;
+
+	qed_llh_mac_to_filter(&high, &low, p_filter);
+
+	/* Find the entry and clean it */
+	for (i = 0; i < NIG_REG_LLH_FUNC_FILTER_EN_SIZE; i++) {
+		if (qed_rd(p_hwfn, p_ptt,
+			   NIG_REG_LLH_FUNC_FILTER_VALUE +
+			   2 * i * sizeof(u32)) != low)
+			continue;
+		if (qed_rd(p_hwfn, p_ptt,
+			   NIG_REG_LLH_FUNC_FILTER_VALUE +
+			   (2 * i + 1) * sizeof(u32)) != high)
+			continue;
+
+		qed_wr(p_hwfn, p_ptt,
+		       NIG_REG_LLH_FUNC_FILTER_EN + i * sizeof(u32), 0);
+		qed_wr(p_hwfn, p_ptt,
+		       NIG_REG_LLH_FUNC_FILTER_VALUE + 2 * i * sizeof(u32), 0);
+		qed_wr(p_hwfn, p_ptt,
+		       NIG_REG_LLH_FUNC_FILTER_VALUE +
+		       (2 * i + 1) * sizeof(u32), 0);
+
+		DP_VERBOSE(p_hwfn, NETIF_MSG_HW,
+			   "mac: %pM is removed from %d\n",
+			   p_filter, i);
+		break;
+	}
+	if (i >= NIG_REG_LLH_FUNC_FILTER_EN_SIZE)
+		DP_NOTICE(p_hwfn, "Tried to remove a non-configured filter\n");
+}
+
 static int qed_set_coalesce(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt,
 			    u32 hw_addr, void *p_eth_qzone,
 			    size_t eth_qzone_size, u8 timeset)
@@ -2386,8 +2566,7 @@ static void qed_disable_wfq_for_all_vports(struct qed_hwfn *p_hwfn,
  * 3. total_req_min_rate [all vports min rate sum] shouldn't exceed min_pf_rate.
  */
 static int qed_init_wfq_param(struct qed_hwfn *p_hwfn,
-			      u16 vport_id, u32 req_rate,
-			      u32 min_pf_rate)
+			      u16 vport_id, u32 req_rate, u32 min_pf_rate)
 {
 	u32 total_req_min_rate = 0, total_left_rate = 0, left_rate_per_vp = 0;
 	int non_requested_count = 0, req_count = 0, i, num_vports;
@@ -2471,7 +2650,7 @@ static int __qed_configure_vport_wfq(struct qed_hwfn *p_hwfn,
 
 	rc = qed_init_wfq_param(p_hwfn, vp_id, rate, p_link->min_pf_rate);
 
-	if (rc == 0)
+	if (!rc)
 		qed_configure_wfq_for_all_vports(p_hwfn, p_ptt,
 						 p_link->min_pf_rate);
 	else
diff --git a/drivers/net/ethernet/qlogic/qed/qed_dev_api.h b/drivers/net/ethernet/qlogic/qed/qed_dev_api.h
index 343bb0344f62..b6711c106597 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_dev_api.h
+++ b/drivers/net/ethernet/qlogic/qed/qed_dev_api.h
@@ -310,6 +310,26 @@ int qed_fw_rss_eng(struct qed_hwfn *p_hwfn,
 		   u8 *dst_id);
 
 /**
+ * @brief qed_llh_add_mac_filter - configures a MAC filter in llh
+ *
+ * @param p_hwfn
+ * @param p_ptt
+ * @param p_filter - MAC to add
+ */
+int qed_llh_add_mac_filter(struct qed_hwfn *p_hwfn,
+			   struct qed_ptt *p_ptt, u8 *p_filter);
+
+/**
+ * @brief qed_llh_remove_mac_filter - removes a MAC filter from llh
+ *
+ * @param p_hwfn
+ * @param p_ptt
+ * @param p_filter - MAC to remove
+ */
+void qed_llh_remove_mac_filter(struct qed_hwfn *p_hwfn,
+			       struct qed_ptt *p_ptt, u8 *p_filter);
+
+/**
  * *@brief Cleanup of previous driver remains prior to load
  *
  * @param p_hwfn
diff --git a/drivers/net/ethernet/qlogic/qed/qed_hsi.h b/drivers/net/ethernet/qlogic/qed/qed_hsi.h
index 6f9d3b831a2a..72eee29c677f 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_hsi.h
+++ b/drivers/net/ethernet/qlogic/qed/qed_hsi.h
@@ -536,6 +536,247 @@ struct core_conn_context {
 	struct regpair ustorm_st_padding[2];
 };
 
+enum core_error_handle {
+	LL2_DROP_PACKET,
+	LL2_DO_NOTHING,
+	LL2_ASSERT,
+	MAX_CORE_ERROR_HANDLE
+};
+
+enum core_event_opcode {
+	CORE_EVENT_TX_QUEUE_START,
+	CORE_EVENT_TX_QUEUE_STOP,
+	CORE_EVENT_RX_QUEUE_START,
+	CORE_EVENT_RX_QUEUE_STOP,
+	MAX_CORE_EVENT_OPCODE
+};
+
+enum core_l4_pseudo_checksum_mode {
+	CORE_L4_PSEUDO_CSUM_CORRECT_LENGTH,
+	CORE_L4_PSEUDO_CSUM_ZERO_LENGTH,
+	MAX_CORE_L4_PSEUDO_CHECKSUM_MODE
+};
+
+struct core_ll2_port_stats {
+	struct regpair gsi_invalid_hdr;
+	struct regpair gsi_invalid_pkt_length;
+	struct regpair gsi_unsupported_pkt_typ;
+	struct regpair gsi_crcchksm_error;
+};
+
+struct core_ll2_pstorm_per_queue_stat {
+	struct regpair sent_ucast_bytes;
+	struct regpair sent_mcast_bytes;
+	struct regpair sent_bcast_bytes;
+	struct regpair sent_ucast_pkts;
+	struct regpair sent_mcast_pkts;
+	struct regpair sent_bcast_pkts;
+};
+
+struct core_ll2_rx_prod {
+	__le16 bd_prod;
+	__le16 cqe_prod;
+	__le32 reserved;
+};
+
+struct core_ll2_tstorm_per_queue_stat {
+	struct regpair packet_too_big_discard;
+	struct regpair no_buff_discard;
+};
+
+struct core_ll2_ustorm_per_queue_stat {
+	struct regpair rcv_ucast_bytes;
+	struct regpair rcv_mcast_bytes;
+	struct regpair rcv_bcast_bytes;
+	struct regpair rcv_ucast_pkts;
+	struct regpair rcv_mcast_pkts;
+	struct regpair rcv_bcast_pkts;
+};
+
+enum core_ramrod_cmd_id {
+	CORE_RAMROD_UNUSED,
+	CORE_RAMROD_RX_QUEUE_START,
+	CORE_RAMROD_TX_QUEUE_START,
+	CORE_RAMROD_RX_QUEUE_STOP,
+	CORE_RAMROD_TX_QUEUE_STOP,
+	MAX_CORE_RAMROD_CMD_ID
+};
+
+enum core_roce_flavor_type {
+	CORE_ROCE,
+	CORE_RROCE,
+	MAX_CORE_ROCE_FLAVOR_TYPE
+};
+
+struct core_rx_action_on_error {
+	u8 error_type;
+#define CORE_RX_ACTION_ON_ERROR_PACKET_TOO_BIG_MASK	0x3
+#define CORE_RX_ACTION_ON_ERROR_PACKET_TOO_BIG_SHIFT 0
+#define CORE_RX_ACTION_ON_ERROR_NO_BUFF_MASK	0x3
+#define CORE_RX_ACTION_ON_ERROR_NO_BUFF_SHIFT	2
+#define CORE_RX_ACTION_ON_ERROR_RESERVED_MASK	0xF
+#define CORE_RX_ACTION_ON_ERROR_RESERVED_SHIFT	4
+};
+
+struct core_rx_bd {
+	struct regpair addr;
+	__le16 reserved[4];
+};
+
+struct core_rx_bd_with_buff_len {
+	struct regpair addr;
+	__le16 buff_length;
+	__le16 reserved[3];
+};
+
+union core_rx_bd_union {
+	struct core_rx_bd rx_bd;
+	struct core_rx_bd_with_buff_len rx_bd_with_len;
+};
+
+struct core_rx_cqe_opaque_data {
+	__le32 data[2];
+};
+
+enum core_rx_cqe_type {
+	CORE_RX_CQE_ILLIGAL_TYPE,
+	CORE_RX_CQE_TYPE_REGULAR,
+	CORE_RX_CQE_TYPE_GSI_OFFLOAD,
+	CORE_RX_CQE_TYPE_SLOW_PATH,
+	MAX_CORE_RX_CQE_TYPE
+};
+
+struct core_rx_fast_path_cqe {
+	u8 type;
+	u8 placement_offset;
+	struct parsing_and_err_flags parse_flags;
+	__le16 packet_length;
+	__le16 vlan;
+	struct core_rx_cqe_opaque_data opaque_data;
+	__le32 reserved[4];
+};
+
+struct core_rx_gsi_offload_cqe {
+	u8 type;
+	u8 data_length_error;
+	struct parsing_and_err_flags parse_flags;
+	__le16 data_length;
+	__le16 vlan;
+	__le32 src_mac_addrhi;
+	__le16 src_mac_addrlo;
+	u8 reserved1[2];
+	__le32 gid_dst[4];
+};
+
+struct core_rx_slow_path_cqe {
+	u8 type;
+	u8 ramrod_cmd_id;
+	__le16 echo;
+	__le32 reserved1[7];
+};
+
+union core_rx_cqe_union {
+	struct core_rx_fast_path_cqe rx_cqe_fp;
+	struct core_rx_gsi_offload_cqe rx_cqe_gsi;
+	struct core_rx_slow_path_cqe rx_cqe_sp;
+};
+
+struct core_rx_start_ramrod_data {
+	struct regpair bd_base;
+	struct regpair cqe_pbl_addr;
+	__le16 mtu;
+	__le16 sb_id;
+	u8 sb_index;
+	u8 complete_cqe_flg;
+	u8 complete_event_flg;
+	u8 drop_ttl0_flg;
+	__le16 num_of_pbl_pages;
+	u8 inner_vlan_removal_en;
+	u8 queue_id;
+	u8 main_func_queue;
+	u8 mf_si_bcast_accept_all;
+	u8 mf_si_mcast_accept_all;
+	struct core_rx_action_on_error action_on_error;
+	u8 gsi_offload_flag;
+	u8 reserved[7];
+};
+
+struct core_rx_stop_ramrod_data {
+	u8 complete_cqe_flg;
+	u8 complete_event_flg;
+	u8 queue_id;
+	u8 reserved1;
+	__le16 reserved2[2];
+};
+
+struct core_tx_bd_flags {
+	u8 as_bitfield;
+#define CORE_TX_BD_FLAGS_FORCE_VLAN_MODE_MASK	0x1
+#define CORE_TX_BD_FLAGS_FORCE_VLAN_MODE_SHIFT	0
+#define CORE_TX_BD_FLAGS_VLAN_INSERTION_MASK	0x1
+#define CORE_TX_BD_FLAGS_VLAN_INSERTION_SHIFT	1
+#define CORE_TX_BD_FLAGS_START_BD_MASK	0x1
+#define CORE_TX_BD_FLAGS_START_BD_SHIFT	2
+#define CORE_TX_BD_FLAGS_IP_CSUM_MASK	0x1
+#define CORE_TX_BD_FLAGS_IP_CSUM_SHIFT	3
+#define CORE_TX_BD_FLAGS_L4_CSUM_MASK	0x1
+#define CORE_TX_BD_FLAGS_L4_CSUM_SHIFT	4
+#define CORE_TX_BD_FLAGS_IPV6_EXT_MASK	0x1
+#define CORE_TX_BD_FLAGS_IPV6_EXT_SHIFT	5
+#define CORE_TX_BD_FLAGS_L4_PROTOCOL_MASK	0x1
+#define CORE_TX_BD_FLAGS_L4_PROTOCOL_SHIFT	6
+#define CORE_TX_BD_FLAGS_L4_PSEUDO_CSUM_MODE_MASK	0x1
+#define CORE_TX_BD_FLAGS_L4_PSEUDO_CSUM_MODE_SHIFT 7
+#define CORE_TX_BD_FLAGS_ROCE_FLAV_MASK		0x1
+#define CORE_TX_BD_FLAGS_ROCE_FLAV_SHIFT	12
+
+};
+
+struct core_tx_bd {
+	struct regpair addr;
+	__le16 nbytes;
+	__le16 nw_vlan_or_lb_echo;
+	u8 bitfield0;
+#define CORE_TX_BD_NBDS_MASK	0xF
+#define CORE_TX_BD_NBDS_SHIFT	0
+#define CORE_TX_BD_ROCE_FLAV_MASK	0x1
+#define CORE_TX_BD_ROCE_FLAV_SHIFT	4
+#define CORE_TX_BD_RESERVED0_MASK	0x7
+#define CORE_TX_BD_RESERVED0_SHIFT	5
+	struct core_tx_bd_flags bd_flags;
+	__le16 bitfield1;
+#define CORE_TX_BD_L4_HDR_OFFSET_W_MASK	0x3FFF
+#define CORE_TX_BD_L4_HDR_OFFSET_W_SHIFT 0
+#define CORE_TX_BD_TX_DST_MASK	0x1
+#define CORE_TX_BD_TX_DST_SHIFT	14
+#define CORE_TX_BD_RESERVED1_MASK	0x1
+#define CORE_TX_BD_RESERVED1_SHIFT	15
+};
+
+enum core_tx_dest {
+	CORE_TX_DEST_NW,
+	CORE_TX_DEST_LB,
+	MAX_CORE_TX_DEST
+};
+
+struct core_tx_start_ramrod_data {
+	struct regpair pbl_base_addr;
+	__le16 mtu;
+	__le16 sb_id;
+	u8 sb_index;
+	u8 stats_en;
+	u8 stats_id;
+	u8 conn_type;
+	__le16 pbl_size;
+	__le16 qm_pq_id;
+	u8 gsi_offload_flag;
+	u8 resrved[3];
+};
+
+struct core_tx_stop_ramrod_data {
+	__le32 reserved0[2];
+};
+
 struct eth_mstorm_per_pf_stat {
 	struct regpair gre_discard_pkts;
 	struct regpair vxlan_discard_pkts;
@@ -636,9 +877,33 @@ struct hsi_fp_ver_struct {
 };
 
 /* Mstorm non-triggering VF zone */
+enum malicious_vf_error_id {
+	MALICIOUS_VF_NO_ERROR,
+	VF_PF_CHANNEL_NOT_READY,
+	VF_ZONE_MSG_NOT_VALID,
+	VF_ZONE_FUNC_NOT_ENABLED,
+	ETH_PACKET_TOO_SMALL,
+	ETH_ILLEGAL_VLAN_MODE,
+	ETH_MTU_VIOLATION,
+	ETH_ILLEGAL_INBAND_TAGS,
+	ETH_VLAN_INSERT_AND_INBAND_VLAN,
+	ETH_ILLEGAL_NBDS,
+	ETH_FIRST_BD_WO_SOP,
+	ETH_INSUFFICIENT_BDS,
+	ETH_ILLEGAL_LSO_HDR_NBDS,
+	ETH_ILLEGAL_LSO_MSS,
+	ETH_ZERO_SIZE_BD,
+	ETH_ILLEGAL_LSO_HDR_LEN,
+	ETH_INSUFFICIENT_PAYLOAD,
+	ETH_EDPM_OUT_OF_SYNC,
+	ETH_TUNN_IPV6_EXT_NBD_ERR,
+	ETH_CONTROL_PACKET_VIOLATION,
+	MAX_MALICIOUS_VF_ERROR_ID
+};
+
 struct mstorm_non_trigger_vf_zone {
 	struct eth_mstorm_per_queue_stat eth_queue_stat;
-	struct eth_rx_prod_data eth_rx_queue_producers[ETH_MAX_NUM_RX_QUEUES_PER_VF];
+	struct eth_rx_prod_data eth_rx_queue_producers[ETH_MAX_NUM_RX_QUEUES_PER_VF_QUAD];
 };
 
 /* Mstorm VF zone */
@@ -705,13 +970,17 @@ struct pf_start_ramrod_data {
 
 struct protocol_dcb_data {
 	u8 dcb_enable_flag;
+	u8 reserved_a;
 	u8 dcb_priority;
 	u8 dcb_tc;
-	u8 reserved;
+	u8 reserved_b;
+	u8 reserved0;
 };
 
 struct pf_update_tunnel_config {
 	u8 update_rx_pf_clss;
+	u8 update_rx_def_ucast_clss;
+	u8 update_rx_def_non_ucast_clss;
 	u8 update_tx_pf_clss;
 	u8 set_vxlan_udp_port_flg;
 	u8 set_geneve_udp_port_flg;
@@ -727,7 +996,7 @@ struct pf_update_tunnel_config {
 	u8 tunnel_clss_ipgre;
 	__le16 vxlan_udp_port;
 	__le16 geneve_udp_port;
-	__le16 reserved[3];
+	__le16 reserved[2];
 };
 
 struct pf_update_ramrod_data {
@@ -736,16 +1005,17 @@ struct pf_update_ramrod_data {
 	u8 update_fcoe_dcb_data_flag;
 	u8 update_iscsi_dcb_data_flag;
 	u8 update_roce_dcb_data_flag;
+	u8 update_rroce_dcb_data_flag;
 	u8 update_iwarp_dcb_data_flag;
 	u8 update_mf_vlan_flag;
-	u8 reserved;
 	struct protocol_dcb_data eth_dcb_data;
 	struct protocol_dcb_data fcoe_dcb_data;
 	struct protocol_dcb_data iscsi_dcb_data;
 	struct protocol_dcb_data roce_dcb_data;
+	struct protocol_dcb_data rroce_dcb_data;
 	struct protocol_dcb_data iwarp_dcb_data;
 	__le16 mf_vlan;
-	__le16 reserved2;
+	__le16 reserved;
 	struct pf_update_tunnel_config tunnel_config;
 };
 
@@ -766,10 +1036,14 @@ enum protocol_version_array_key {
 	MAX_PROTOCOL_VERSION_ARRAY_KEY
 };
 
-/* Pstorm non-triggering VF zone */
+struct rdma_sent_stats {
+	struct regpair sent_bytes;
+	struct regpair sent_pkts;
+};
+
 struct pstorm_non_trigger_vf_zone {
 	struct eth_pstorm_per_queue_stat eth_queue_stat;
-	struct regpair reserved[2];
+	struct rdma_sent_stats rdma_stats;
 };
 
 /* Pstorm VF zone */
@@ -786,7 +1060,11 @@ struct ramrod_header {
 	__le16 echo;
 };
 
-/* Slowpath Element (SPQE) */
+struct rdma_rcv_stats {
+	struct regpair rcv_bytes;
+	struct regpair rcv_pkts;
+};
+
 struct slow_path_element {
 	struct ramrod_header hdr;
 	struct regpair data_ptr;
@@ -794,7 +1072,7 @@ struct slow_path_element {
 
 /* Tstorm non-triggering VF zone */
 struct tstorm_non_trigger_vf_zone {
-	struct regpair reserved[2];
+	struct rdma_rcv_stats rdma_stats;
 };
 
 struct tstorm_per_port_stat {
@@ -802,9 +1080,14 @@ struct tstorm_per_port_stat {
 	struct regpair mac_error_discard;
 	struct regpair mftag_filter_discard;
 	struct regpair eth_mac_filter_discard;
-	struct regpair reserved[5];
+	struct regpair ll2_mac_filter_discard;
+	struct regpair ll2_conn_disabled_discard;
+	struct regpair iscsi_irregular_pkt;
+	struct regpair reserved;
+	struct regpair roce_irregular_pkt;
 	struct regpair eth_irregular_pkt;
-	struct regpair reserved1[2];
+	struct regpair reserved1;
+	struct regpair preroce_irregular_pkt;
 	struct regpair eth_gre_tunn_filter_discard;
 	struct regpair eth_vxlan_tunn_filter_discard;
 	struct regpair eth_geneve_tunn_filter_discard;
@@ -870,7 +1153,13 @@ struct vf_stop_ramrod_data {
 	__le32 reserved2;
 };
 
-/* Attentions status block */
+enum vf_zone_size_mode {
+	VF_ZONE_SIZE_MODE_DEFAULT,
+	VF_ZONE_SIZE_MODE_DOUBLE,
+	VF_ZONE_SIZE_MODE_QUAD,
+	MAX_VF_ZONE_SIZE_MODE
+};
+
 struct atten_status_block {
 	__le32 atten_bits;
 	__le32 atten_ack;
@@ -1442,13 +1731,6 @@ enum bin_dbg_buffer_type {
 	MAX_BIN_DBG_BUFFER_TYPE
 };
 
-/* Chip IDs */
-enum chip_ids {
-	CHIP_RESERVED,
-	CHIP_BB_B0,
-	CHIP_RESERVED2,
-	MAX_CHIP_IDS
-};
 
 /* Attention bit mapping */
 struct dbg_attn_bit_mapping {
@@ -1527,6 +1809,371 @@ enum dbg_attn_type {
 	MAX_DBG_ATTN_TYPE
 };
 
+/* condition header for registers dump */
+struct dbg_dump_cond_hdr {
+	struct dbg_mode_hdr mode; /* Mode header */
+	u8 block_id; /* block ID */
+	u8 data_size; /* size in dwords of the data following this header */
+};
+
+/* memory data for registers dump */
+struct dbg_dump_mem {
+	__le32 dword0;
+#define DBG_DUMP_MEM_ADDRESS_MASK       0xFFFFFF
+#define DBG_DUMP_MEM_ADDRESS_SHIFT      0
+#define DBG_DUMP_MEM_MEM_GROUP_ID_MASK  0xFF
+#define DBG_DUMP_MEM_MEM_GROUP_ID_SHIFT 24
+	__le32 dword1;
+#define DBG_DUMP_MEM_LENGTH_MASK        0xFFFFFF
+#define DBG_DUMP_MEM_LENGTH_SHIFT       0
+#define DBG_DUMP_MEM_RESERVED_MASK      0xFF
+#define DBG_DUMP_MEM_RESERVED_SHIFT     24
+};
+
+/* register data for registers dump */
+struct dbg_dump_reg {
+	__le32 data;
+#define DBG_DUMP_REG_ADDRESS_MASK  0xFFFFFF /* register address (in dwords) */
+#define DBG_DUMP_REG_ADDRESS_SHIFT 0
+#define DBG_DUMP_REG_LENGTH_MASK   0xFF /* register size (in dwords) */
+#define DBG_DUMP_REG_LENGTH_SHIFT  24
+};
+
+/* split header for registers dump */
+struct dbg_dump_split_hdr {
+	__le32 hdr;
+#define DBG_DUMP_SPLIT_HDR_DATA_SIZE_MASK      0xFFFFFF
+#define DBG_DUMP_SPLIT_HDR_DATA_SIZE_SHIFT     0
+#define DBG_DUMP_SPLIT_HDR_SPLIT_TYPE_ID_MASK  0xFF
+#define DBG_DUMP_SPLIT_HDR_SPLIT_TYPE_ID_SHIFT 24
+};
+
+/* condition header for idle check */
+struct dbg_idle_chk_cond_hdr {
+	struct dbg_mode_hdr mode; /* Mode header */
+	__le16 data_size; /* size in dwords of the data following this header */
+};
+
+/* Idle Check condition register */
+struct dbg_idle_chk_cond_reg {
+	__le32 data;
+#define DBG_IDLE_CHK_COND_REG_ADDRESS_MASK   0xFFFFFF
+#define DBG_IDLE_CHK_COND_REG_ADDRESS_SHIFT  0
+#define DBG_IDLE_CHK_COND_REG_BLOCK_ID_MASK  0xFF
+#define DBG_IDLE_CHK_COND_REG_BLOCK_ID_SHIFT 24
+	__le16 num_entries; /* number of registers entries to check */
+	u8 entry_size; /* size of registers entry (in dwords) */
+	u8 start_entry; /* index of the first entry to check */
+};
+
+/* Idle Check info register */
+struct dbg_idle_chk_info_reg {
+	__le32 data;
+#define DBG_IDLE_CHK_INFO_REG_ADDRESS_MASK   0xFFFFFF
+#define DBG_IDLE_CHK_INFO_REG_ADDRESS_SHIFT  0
+#define DBG_IDLE_CHK_INFO_REG_BLOCK_ID_MASK  0xFF
+#define DBG_IDLE_CHK_INFO_REG_BLOCK_ID_SHIFT 24
+	__le16 size; /* register size in dwords */
+	struct dbg_mode_hdr mode; /* Mode header */
+};
+
+/* Idle Check register */
+union dbg_idle_chk_reg {
+	struct dbg_idle_chk_cond_reg cond_reg; /* condition register */
+	struct dbg_idle_chk_info_reg info_reg; /* info register */
+};
+
+/* Idle Check result header */
+struct dbg_idle_chk_result_hdr {
+	__le16 rule_id; /* Failing rule index */
+	__le16 mem_entry_id; /* Failing memory entry index */
+	u8 num_dumped_cond_regs; /* number of dumped condition registers */
+	u8 num_dumped_info_regs; /* number of dumped condition registers */
+	u8 severity; /* from dbg_idle_chk_severity_types enum */
+	u8 reserved;
+};
+
+/* Idle Check result register header */
+struct dbg_idle_chk_result_reg_hdr {
+	u8 data;
+#define DBG_IDLE_CHK_RESULT_REG_HDR_IS_MEM_MASK  0x1
+#define DBG_IDLE_CHK_RESULT_REG_HDR_IS_MEM_SHIFT 0
+#define DBG_IDLE_CHK_RESULT_REG_HDR_REG_ID_MASK  0x7F
+#define DBG_IDLE_CHK_RESULT_REG_HDR_REG_ID_SHIFT 1
+	u8 start_entry; /* index of the first checked entry */
+	__le16 size; /* register size in dwords */
+};
+
+/* Idle Check rule */
+struct dbg_idle_chk_rule {
+	__le16 rule_id; /* Idle Check rule ID */
+	u8 severity; /* value from dbg_idle_chk_severity_types enum */
+	u8 cond_id; /* Condition ID */
+	u8 num_cond_regs; /* number of condition registers */
+	u8 num_info_regs; /* number of info registers */
+	u8 num_imms; /* number of immediates in the condition */
+	u8 reserved1;
+	__le16 reg_offset; /* offset of this rules registers in the idle check
+			    * register array (in dbg_idle_chk_reg units).
+			    */
+	__le16 imm_offset; /* offset of this rules immediate values in the
+			    * immediate values array (in dwords).
+			    */
+};
+
+/* Idle Check rule parsing data */
+struct dbg_idle_chk_rule_parsing_data {
+	__le32 data;
+#define DBG_IDLE_CHK_RULE_PARSING_DATA_HAS_FW_MSG_MASK  0x1
+#define DBG_IDLE_CHK_RULE_PARSING_DATA_HAS_FW_MSG_SHIFT 0
+#define DBG_IDLE_CHK_RULE_PARSING_DATA_STR_OFFSET_MASK  0x7FFFFFFF
+#define DBG_IDLE_CHK_RULE_PARSING_DATA_STR_OFFSET_SHIFT 1
+};
+
+/* idle check severity types */
+enum dbg_idle_chk_severity_types {
+	/* idle check failure should cause an error */
+	IDLE_CHK_SEVERITY_ERROR,
+	/* idle check failure should cause an error only if theres no traffic */
+	IDLE_CHK_SEVERITY_ERROR_NO_TRAFFIC,
+	/* idle check failure should cause a warning */
+	IDLE_CHK_SEVERITY_WARNING,
+	MAX_DBG_IDLE_CHK_SEVERITY_TYPES
+};
+
+/* Debug Bus block data */
+struct dbg_bus_block_data {
+	u8 enabled; /* Indicates if the block is enabled for recording (0/1) */
+	u8 hw_id; /* HW ID associated with the block */
+	u8 line_num; /* Debug line number to select */
+	u8 right_shift; /* Number of units to  right the debug data (0-3) */
+	u8 cycle_en; /* 4-bit value: bit i set -> unit i is enabled. */
+	u8 force_valid; /* 4-bit value: bit i set -> unit i is forced valid. */
+	u8 force_frame; /* 4-bit value: bit i set -> unit i frame bit is forced.
+			 */
+	u8 reserved;
+};
+
+/* Debug Bus Clients */
+enum dbg_bus_clients {
+	DBG_BUS_CLIENT_RBCN,
+	DBG_BUS_CLIENT_RBCP,
+	DBG_BUS_CLIENT_RBCR,
+	DBG_BUS_CLIENT_RBCT,
+	DBG_BUS_CLIENT_RBCU,
+	DBG_BUS_CLIENT_RBCF,
+	DBG_BUS_CLIENT_RBCX,
+	DBG_BUS_CLIENT_RBCS,
+	DBG_BUS_CLIENT_RBCH,
+	DBG_BUS_CLIENT_RBCZ,
+	DBG_BUS_CLIENT_OTHER_ENGINE,
+	DBG_BUS_CLIENT_TIMESTAMP,
+	DBG_BUS_CLIENT_CPU,
+	DBG_BUS_CLIENT_RBCY,
+	DBG_BUS_CLIENT_RBCQ,
+	DBG_BUS_CLIENT_RBCM,
+	DBG_BUS_CLIENT_RBCB,
+	DBG_BUS_CLIENT_RBCW,
+	DBG_BUS_CLIENT_RBCV,
+	MAX_DBG_BUS_CLIENTS
+};
+
+/* Debug Bus memory address */
+struct dbg_bus_mem_addr {
+	__le32 lo;
+	__le32 hi;
+};
+
+/* Debug Bus PCI buffer data */
+struct dbg_bus_pci_buf_data {
+	struct dbg_bus_mem_addr phys_addr; /* PCI buffer physical address */
+	struct dbg_bus_mem_addr virt_addr; /* PCI buffer virtual address */
+	__le32 size; /* PCI buffer size in bytes */
+};
+
+/* Debug Bus Storm EID range filter params */
+struct dbg_bus_storm_eid_range_params {
+	u8 min; /* Minimal event ID to filter on */
+	u8 max; /* Maximal event ID to filter on */
+};
+
+/* Debug Bus Storm EID mask filter params */
+struct dbg_bus_storm_eid_mask_params {
+	u8 val; /* Event ID value */
+	u8 mask; /* Event ID mask. 1s in the mask = dont care bits. */
+};
+
+/* Debug Bus Storm EID filter params */
+union dbg_bus_storm_eid_params {
+	struct dbg_bus_storm_eid_range_params range;
+	struct dbg_bus_storm_eid_mask_params mask;
+};
+
+/* Debug Bus Storm data */
+struct dbg_bus_storm_data {
+	u8 fast_enabled;
+	u8 fast_mode;
+	u8 slow_enabled;
+	u8 slow_mode;
+	u8 hw_id;
+	u8 eid_filter_en;
+	u8 eid_range_not_mask;
+	u8 cid_filter_en;
+	union dbg_bus_storm_eid_params eid_filter_params;
+	__le16 reserved;
+	__le32 cid;
+};
+
+/* Debug Bus data */
+struct dbg_bus_data {
+	__le32 app_version; /* The tools version number of the application */
+	u8 state; /* The current debug bus state */
+	u8 hw_dwords; /* HW dwords per cycle */
+	u8 next_hw_id; /* Next HW ID to be associated with an input */
+	u8 num_enabled_blocks; /* Number of blocks enabled for recording */
+	u8 num_enabled_storms; /* Number of Storms enabled for recording */
+	u8 target; /* Output target */
+	u8 next_trigger_state; /* ID of next trigger state to be added */
+	u8 next_constraint_id; /* ID of next filter/trigger constraint to be
+				* added.
+				*/
+	u8 one_shot_en; /* Indicates if one-shot mode is enabled (0/1) */
+	u8 grc_input_en; /* Indicates if GRC recording is enabled (0/1) */
+	u8 timestamp_input_en; /* Indicates if timestamp recording is enabled
+				* (0/1).
+				*/
+	u8 filter_en; /* Indicates if the recording filter is enabled (0/1) */
+	u8 trigger_en; /* Indicates if the recording trigger is enabled (0/1) */
+	u8 adding_filter; /* If true, the next added constraint belong to the
+			   * filter. Otherwise, it belongs to the last added
+			   * trigger state. Valid only if either filter or
+			   * triggers are enabled.
+			   */
+	u8 filter_pre_trigger; /* Indicates if the recording filter should be
+				* applied before the trigger. Valid only if both
+				* filter and trigger are enabled (0/1).
+				*/
+	u8 filter_post_trigger; /* Indicates if the recording filter should be
+				 * applied after the trigger. Valid only if both
+				 * filter and trigger are enabled (0/1).
+				 */
+	u8 unify_inputs; /* If true, all inputs are associated with HW ID 0.
+			  * Otherwise, each input is assigned a different HW ID
+			  * (0/1).
+			  */
+	u8 rcv_from_other_engine; /* Indicates if the other engine sends it NW
+				   * recording to this engine (0/1).
+				   */
+	struct dbg_bus_pci_buf_data pci_buf; /* Debug Bus PCI buffer data. Valid
+					      * only when the target is
+					      * DBG_BUS_TARGET_ID_PCI.
+					      */
+	__le16 reserved;
+	struct dbg_bus_block_data blocks[80];/* Debug Bus data for each block */
+	struct dbg_bus_storm_data storms[6]; /* Debug Bus data for each block */
+};
+
+/* Debug bus frame modes */
+enum dbg_bus_frame_modes {
+	DBG_BUS_FRAME_MODE_0HW_4ST = 0, /* 0 HW dwords, 4 Storm dwords */
+	DBG_BUS_FRAME_MODE_4HW_0ST = 3, /* 4 HW dwords, 0 Storm dwords */
+	DBG_BUS_FRAME_MODE_8HW_0ST = 4, /* 8 HW dwords, 0 Storm dwords */
+	MAX_DBG_BUS_FRAME_MODES
+};
+
+/* Debug bus states */
+enum dbg_bus_states {
+	DBG_BUS_STATE_IDLE, /* debug bus idle state (not recording) */
+	DBG_BUS_STATE_READY, /* debug bus is ready for configuration and
+			      * recording.
+			      */
+	DBG_BUS_STATE_RECORDING, /* debug bus is currently recording */
+	DBG_BUS_STATE_STOPPED, /* debug bus recording has stopped */
+	MAX_DBG_BUS_STATES
+};
+
+/* Debug bus target IDs */
+enum dbg_bus_targets {
+	/* records debug bus to DBG block internal buffer */
+	DBG_BUS_TARGET_ID_INT_BUF,
+	/* records debug bus to the NW */
+	DBG_BUS_TARGET_ID_NIG,
+	/* records debug bus to a PCI buffer */
+	DBG_BUS_TARGET_ID_PCI,
+	MAX_DBG_BUS_TARGETS
+};
+
+/* GRC Dump data */
+struct dbg_grc_data {
+	__le32 param_val[40]; /* Value of each GRC parameter. Array size must
+			       * match the enum dbg_grc_params.
+			       */
+	u8 param_set_by_user[40]; /* Indicates for each GRC parameter if it was
+				   * set by the user (0/1). Array size must
+				   * match the enum dbg_grc_params.
+				   */
+};
+
+/* Debug GRC params */
+enum dbg_grc_params {
+	DBG_GRC_PARAM_DUMP_TSTORM, /* dump Tstorm memories (0/1) */
+	DBG_GRC_PARAM_DUMP_MSTORM, /* dump Mstorm memories (0/1) */
+	DBG_GRC_PARAM_DUMP_USTORM, /* dump Ustorm memories (0/1) */
+	DBG_GRC_PARAM_DUMP_XSTORM, /* dump Xstorm memories (0/1) */
+	DBG_GRC_PARAM_DUMP_YSTORM, /* dump Ystorm memories (0/1) */
+	DBG_GRC_PARAM_DUMP_PSTORM, /* dump Pstorm memories (0/1) */
+	DBG_GRC_PARAM_DUMP_REGS, /* dump non-memory registers (0/1) */
+	DBG_GRC_PARAM_DUMP_RAM, /* dump Storm internal RAMs (0/1) */
+	DBG_GRC_PARAM_DUMP_PBUF, /* dump Storm passive buffer (0/1) */
+	DBG_GRC_PARAM_DUMP_IOR, /* dump Storm IORs (0/1) */
+	DBG_GRC_PARAM_DUMP_VFC, /* dump VFC memories (0/1) */
+	DBG_GRC_PARAM_DUMP_CM_CTX, /* dump CM contexts (0/1) */
+	DBG_GRC_PARAM_DUMP_PXP, /* dump PXP memories (0/1) */
+	DBG_GRC_PARAM_DUMP_RSS, /* dump RSS memories (0/1) */
+	DBG_GRC_PARAM_DUMP_CAU, /* dump CAU memories (0/1) */
+	DBG_GRC_PARAM_DUMP_QM, /* dump QM memories (0/1) */
+	DBG_GRC_PARAM_DUMP_MCP, /* dump MCP memories (0/1) */
+	DBG_GRC_PARAM_RESERVED, /* reserved */
+	DBG_GRC_PARAM_DUMP_CFC, /* dump CFC memories (0/1) */
+	DBG_GRC_PARAM_DUMP_IGU, /* dump IGU memories (0/1) */
+	DBG_GRC_PARAM_DUMP_BRB, /* dump BRB memories (0/1) */
+	DBG_GRC_PARAM_DUMP_BTB, /* dump BTB memories (0/1) */
+	DBG_GRC_PARAM_DUMP_BMB, /* dump BMB memories (0/1) */
+	DBG_GRC_PARAM_DUMP_NIG, /* dump NIG memories (0/1) */
+	DBG_GRC_PARAM_DUMP_MULD, /* dump MULD memories (0/1) */
+	DBG_GRC_PARAM_DUMP_PRS, /* dump PRS memories (0/1) */
+	DBG_GRC_PARAM_DUMP_DMAE, /* dump PRS memories (0/1) */
+	DBG_GRC_PARAM_DUMP_TM, /* dump TM (timers) memories (0/1) */
+	DBG_GRC_PARAM_DUMP_SDM, /* dump SDM memories (0/1) */
+	DBG_GRC_PARAM_DUMP_DIF, /* dump DIF memories (0/1) */
+	DBG_GRC_PARAM_DUMP_STATIC, /* dump static debug data (0/1) */
+	DBG_GRC_PARAM_UNSTALL, /* un-stall Storms after dump (0/1) */
+	DBG_GRC_PARAM_NUM_LCIDS, /* number of LCIDs (0..320) */
+	DBG_GRC_PARAM_NUM_LTIDS, /* number of LTIDs (0..320) */
+	/* preset: exclude all memories from dump (1 only) */
+	DBG_GRC_PARAM_EXCLUDE_ALL,
+	/* preset: include memories for crash dump (1 only) */
+	DBG_GRC_PARAM_CRASH,
+	/* perform dump only if MFW is responding (0/1) */
+	DBG_GRC_PARAM_PARITY_SAFE,
+	DBG_GRC_PARAM_DUMP_CM, /* dump CM memories (0/1) */
+	DBG_GRC_PARAM_DUMP_PHY, /* dump PHY memories (0/1) */
+	MAX_DBG_GRC_PARAMS
+};
+
+/* Debug reset registers */
+enum dbg_reset_regs {
+	DBG_RESET_REG_MISCS_PL_UA,
+	DBG_RESET_REG_MISCS_PL_HV,
+	DBG_RESET_REG_MISCS_PL_HV_2,
+	DBG_RESET_REG_MISC_PL_UA,
+	DBG_RESET_REG_MISC_PL_HV,
+	DBG_RESET_REG_MISC_PL_PDA_VMAIN_1,
+	DBG_RESET_REG_MISC_PL_PDA_VMAIN_2,
+	DBG_RESET_REG_MISC_PL_PDA_VAUX,
+	MAX_DBG_RESET_REGS
+};
+
 /* Debug status codes */
 enum dbg_status {
 	DBG_STATUS_OK,
@@ -1579,9 +2226,45 @@ enum dbg_status {
 	DBG_STATUS_REG_FIFO_BAD_DATA,
 	DBG_STATUS_PROTECTION_OVERRIDE_BAD_DATA,
 	DBG_STATUS_DBG_ARRAY_NOT_SET,
+	DBG_STATUS_MULTI_BLOCKS_WITH_FILTER,
 	MAX_DBG_STATUS
 };
 
+/* Debug Storms IDs */
+enum dbg_storms {
+	DBG_TSTORM_ID,
+	DBG_MSTORM_ID,
+	DBG_USTORM_ID,
+	DBG_XSTORM_ID,
+	DBG_YSTORM_ID,
+	DBG_PSTORM_ID,
+	MAX_DBG_STORMS
+};
+
+/* Idle Check data */
+struct idle_chk_data {
+	__le32 buf_size; /* Idle check buffer size in dwords */
+	u8 buf_size_set; /* Indicates if the idle check buffer size was set
+			  * (0/1).
+			  */
+	u8 reserved1;
+	__le16 reserved2;
+};
+
+/* Debug Tools data (per HW function) */
+struct dbg_tools_data {
+	struct dbg_grc_data grc; /* GRC Dump data */
+	struct dbg_bus_data bus; /* Debug Bus data */
+	struct idle_chk_data idle_chk; /* Idle Check data */
+	u8 mode_enable[40]; /* Indicates if a mode is enabled (0/1) */
+	u8 block_in_reset[80]; /* Indicates if a block is in reset state (0/1).
+				*/
+	u8 chip_id; /* Chip ID (from enum chip_ids) */
+	u8 platform_id; /* Platform ID (from enum platform_ids) */
+	u8 initialized; /* Indicates if the data was initialized */
+	u8 reserved;
+};
+
 /********************************/
 /* HSI Init Functions constants */
 /********************************/
@@ -1589,7 +2272,41 @@ enum dbg_status {
 /* Number of VLAN priorities */
 #define NUM_OF_VLAN_PRIORITIES	8
 
-/* QM per-port init parameters */
+struct init_brb_ram_req {
+	__le32 guranteed_per_tc;
+	__le32 headroom_per_tc;
+	__le32 min_pkt_size;
+	__le32 max_ports_per_engine;
+	u8 num_active_tcs[MAX_NUM_PORTS];
+};
+
+struct init_ets_tc_req {
+	u8 use_sp;
+	u8 use_wfq;
+	__le16 weight;
+};
+
+struct init_ets_req {
+	__le32 mtu;
+	struct init_ets_tc_req tc_req[NUM_OF_TCS];
+};
+
+struct init_nig_lb_rl_req {
+	__le16 lb_mac_rate;
+	__le16 lb_rate;
+	__le32 mtu;
+	__le16 tc_rate[NUM_OF_PHYS_TCS];
+};
+
+struct init_nig_pri_tc_map_entry {
+	u8 tc_id;
+	u8 valid;
+};
+
+struct init_nig_pri_tc_map_req {
+	struct init_nig_pri_tc_map_entry pri[NUM_OF_VLAN_PRIORITIES];
+};
+
 struct init_qm_port_params {
 	u8 active;
 	u8 active_phys_tcs;
@@ -1619,7 +2336,7 @@ struct init_qm_vport_params {
 
 /* Width of GRC address in bits (addresses are specified in dwords) */
 #define GRC_ADDR_BITS	23
-#define MAX_GRC_ADDR	((1 << GRC_ADDR_BITS) - 1)
+#define MAX_GRC_ADDR	(BIT(GRC_ADDR_BITS) - 1)
 
 /* indicates an init that should be applied to any phase ID */
 #define ANY_PHASE_ID	0xffff
@@ -1627,15 +2344,50 @@ struct init_qm_vport_params {
 /* Max size in dwords of a zipped array */
 #define MAX_ZIPPED_SIZE	8192
 
+struct fw_asserts_ram_section {
+	__le16 section_ram_line_offset;
+	__le16 section_ram_line_size;
+	u8 list_dword_offset;
+	u8 list_element_dword_size;
+	u8 list_num_elements;
+	u8 list_next_index_dword_offset;
+};
+
+struct fw_ver_num {
+	u8 major; /* Firmware major version number */
+	u8 minor; /* Firmware minor version number */
+	u8 rev; /* Firmware revision version number */
+	u8 eng; /* Firmware engineering version number (for bootleg versions) */
+};
+
+struct fw_ver_info {
+	__le16 tools_ver; /* Tools version number */
+	u8 image_id; /* FW image ID (e.g. main) */
+	u8 reserved1;
+	struct fw_ver_num num; /* FW version number */
+	__le32 timestamp; /* FW Timestamp in unix time  (sec. since 1970) */
+	__le32 reserved2;
+};
+
+struct fw_info {
+	struct fw_ver_info ver;
+	struct fw_asserts_ram_section fw_asserts_section;
+};
+
+struct fw_info_location {
+	__le32 grc_addr;
+	__le32 size;
+};
+
 enum init_modes {
 	MODE_RESERVED,
 	MODE_BB_B0,
-	MODE_RESERVED2,
+	MODE_K2,
 	MODE_ASIC,
+	MODE_RESERVED2,
 	MODE_RESERVED3,
 	MODE_RESERVED4,
 	MODE_RESERVED5,
-	MODE_RESERVED6,
 	MODE_SF,
 	MODE_MF_SD,
 	MODE_MF_SI,
@@ -1644,7 +2396,7 @@ enum init_modes {
 	MODE_PORTS_PER_ENG_4,
 	MODE_100G,
 	MODE_40G,
-	MODE_RESERVED7,
+	MODE_RESERVED6,
 	MAX_INIT_MODES
 };
 
@@ -1674,11 +2426,11 @@ struct bin_buffer_hdr {
 
 /* binary init buffer types */
 enum bin_init_buffer_type {
-	BIN_BUF_FW_VER_INFO,
+	BIN_BUF_INIT_FW_VER_INFO,
 	BIN_BUF_INIT_CMD,
 	BIN_BUF_INIT_VAL,
 	BIN_BUF_INIT_MODE_TREE,
-	BIN_BUF_IRO,
+	BIN_BUF_INIT_IRO,
 	MAX_BIN_INIT_BUFFER_TYPE
 };
 
@@ -1902,8 +2654,276 @@ struct iro {
 	__le16 size;
 };
 
+/***************************** Public Functions *******************************/
 /**
- * @brief qed_dbg_print_attn - Prints attention registers values in the specified results struct.
+ * @brief qed_dbg_set_bin_ptr - Sets a pointer to the binary data with debug
+ *	arrays.
+ *
+ * @param bin_ptr - a pointer to the binary data with debug arrays.
+ */
+enum dbg_status qed_dbg_set_bin_ptr(const u8 * const bin_ptr);
+/**
+ * @brief qed_dbg_grc_get_dump_buf_size - Returns the required buffer size for
+ *	GRC Dump.
+ *
+ * @param p_hwfn - HW device data
+ * @param p_ptt - Ptt window used for writing the registers.
+ * @param buf_size - OUT: required buffer size (in dwords) for the GRC Dump
+ *	data.
+ *
+ * @return error if one of the following holds:
+ *	- the version wasn't set
+ * Otherwise, returns ok.
+ */
+enum dbg_status qed_dbg_grc_get_dump_buf_size(struct qed_hwfn *p_hwfn,
+					      struct qed_ptt *p_ptt,
+					      u32 *buf_size);
+/**
+ * @brief qed_dbg_grc_dump - Dumps GRC data into the specified buffer.
+ *
+ * @param p_hwfn - HW device data
+ * @param p_ptt - Ptt window used for writing the registers.
+ * @param dump_buf - Pointer to write the collected GRC data into.
+ * @param buf_size_in_dwords - Size of the specified buffer in dwords.
+ * @param num_dumped_dwords - OUT: number of dumped dwords.
+ *
+ * @return error if one of the following holds:
+ *	- the version wasn't set
+ *	- the specified dump buffer is too small
+ * Otherwise, returns ok.
+ */
+enum dbg_status qed_dbg_grc_dump(struct qed_hwfn *p_hwfn,
+				 struct qed_ptt *p_ptt,
+				 u32 *dump_buf,
+				 u32 buf_size_in_dwords,
+				 u32 *num_dumped_dwords);
+/**
+ * @brief qed_dbg_idle_chk_get_dump_buf_size - Returns the required buffer size
+ *	for idle check results.
+ *
+ * @param p_hwfn - HW device data
+ * @param p_ptt - Ptt window used for writing the registers.
+ * @param buf_size - OUT: required buffer size (in dwords) for the idle check
+ *	data.
+ *
+ * @return error if one of the following holds:
+ *	- the version wasn't set
+ * Otherwise, returns ok.
+ */
+enum dbg_status qed_dbg_idle_chk_get_dump_buf_size(struct qed_hwfn *p_hwfn,
+						   struct qed_ptt *p_ptt,
+						   u32 *buf_size);
+/**
+ * @brief qed_dbg_idle_chk_dump - Performs idle check and writes the results
+ *	into the specified buffer.
+ *
+ * @param p_hwfn - HW device data
+ * @param p_ptt - Ptt window used for writing the registers.
+ * @param dump_buf - Pointer to write the idle check data into.
+ * @param buf_size_in_dwords - Size of the specified buffer in dwords.
+ * @param num_dumped_dwords - OUT: number of dumped dwords.
+ *
+ * @return error if one of the following holds:
+ *	- the version wasn't set
+ *	- the specified buffer is too small
+ * Otherwise, returns ok.
+ */
+enum dbg_status qed_dbg_idle_chk_dump(struct qed_hwfn *p_hwfn,
+				      struct qed_ptt *p_ptt,
+				      u32 *dump_buf,
+				      u32 buf_size_in_dwords,
+				      u32 *num_dumped_dwords);
+/**
+ * @brief qed_dbg_mcp_trace_get_dump_buf_size - Returns the required buffer size
+ *	for mcp trace results.
+ *
+ * @param p_hwfn - HW device data
+ * @param p_ptt - Ptt window used for writing the registers.
+ * @param buf_size - OUT: required buffer size (in dwords) for mcp trace data.
+ *
+ * @return error if one of the following holds:
+ *	- the version wasn't set
+ *	- the trace data in MCP scratchpad contain an invalid signature
+ *	- the bundle ID in NVRAM is invalid
+ *	- the trace meta data cannot be found (in NVRAM or image file)
+ * Otherwise, returns ok.
+ */
+enum dbg_status qed_dbg_mcp_trace_get_dump_buf_size(struct qed_hwfn *p_hwfn,
+						    struct qed_ptt *p_ptt,
+						    u32 *buf_size);
+/**
+ * @brief qed_dbg_mcp_trace_dump - Performs mcp trace and writes the results
+ *	into the specified buffer.
+ *
+ * @param p_hwfn - HW device data
+ * @param p_ptt - Ptt window used for writing the registers.
+ * @param dump_buf - Pointer to write the mcp trace data into.
+ * @param buf_size_in_dwords - Size of the specified buffer in dwords.
+ * @param num_dumped_dwords - OUT: number of dumped dwords.
+ *
+ * @return error if one of the following holds:
+ *	- the version wasn't set
+ *	- the specified buffer is too small
+ *	- the trace data in MCP scratchpad contain an invalid signature
+ *	- the bundle ID in NVRAM is invalid
+ *	- the trace meta data cannot be found (in NVRAM or image file)
+ *	- the trace meta data cannot be read (from NVRAM or image file)
+ * Otherwise, returns ok.
+ */
+enum dbg_status qed_dbg_mcp_trace_dump(struct qed_hwfn *p_hwfn,
+				       struct qed_ptt *p_ptt,
+				       u32 *dump_buf,
+				       u32 buf_size_in_dwords,
+				       u32 *num_dumped_dwords);
+/**
+ * @brief qed_dbg_reg_fifo_get_dump_buf_size - Returns the required buffer size
+ *	for grc trace fifo results.
+ *
+ * @param p_hwfn - HW device data
+ * @param p_ptt - Ptt window used for writing the registers.
+ * @param buf_size - OUT: required buffer size (in dwords) for reg fifo data.
+ *
+ * @return error if one of the following holds:
+ *	- the version wasn't set
+ * Otherwise, returns ok.
+ */
+enum dbg_status qed_dbg_reg_fifo_get_dump_buf_size(struct qed_hwfn *p_hwfn,
+						   struct qed_ptt *p_ptt,
+						   u32 *buf_size);
+/**
+ * @brief qed_dbg_reg_fifo_dump - Reads the reg fifo and writes the results into
+ *	the specified buffer.
+ *
+ * @param p_hwfn - HW device data
+ * @param p_ptt - Ptt window used for writing the registers.
+ * @param dump_buf - Pointer to write the reg fifo data into.
+ * @param buf_size_in_dwords - Size of the specified buffer in dwords.
+ * @param num_dumped_dwords - OUT: number of dumped dwords.
+ *
+ * @return error if one of the following holds:
+ *	- the version wasn't set
+ *	- the specified buffer is too small
+ *	- DMAE transaction failed
+ * Otherwise, returns ok.
+ */
+enum dbg_status qed_dbg_reg_fifo_dump(struct qed_hwfn *p_hwfn,
+				      struct qed_ptt *p_ptt,
+				      u32 *dump_buf,
+				      u32 buf_size_in_dwords,
+				      u32 *num_dumped_dwords);
+/**
+ * @brief qed_dbg_igu_fifo_get_dump_buf_size - Returns the required buffer size
+ *	for the IGU fifo results.
+ *
+ * @param p_hwfn - HW device data
+ * @param p_ptt - Ptt window used for writing the registers.
+ * @param buf_size - OUT: required buffer size (in dwords) for the IGU fifo
+ *	data.
+ *
+ * @return error if one of the following holds:
+ *	- the version wasn't set
+ * Otherwise, returns ok.
+ */
+enum dbg_status qed_dbg_igu_fifo_get_dump_buf_size(struct qed_hwfn *p_hwfn,
+						   struct qed_ptt *p_ptt,
+						   u32 *buf_size);
+/**
+ * @brief qed_dbg_igu_fifo_dump - Reads the IGU fifo and writes the results into
+ *	the specified buffer.
+ *
+ * @param p_hwfn - HW device data
+ * @param p_ptt - Ptt window used for writing the registers.
+ * @param dump_buf - Pointer to write the IGU fifo data into.
+ * @param buf_size_in_dwords - Size of the specified buffer in dwords.
+ * @param num_dumped_dwords - OUT: number of dumped dwords.
+ *
+ * @return error if one of the following holds:
+ *	- the version wasn't set
+ *	- the specified buffer is too small
+ *	- DMAE transaction failed
+ * Otherwise, returns ok.
+ */
+enum dbg_status qed_dbg_igu_fifo_dump(struct qed_hwfn *p_hwfn,
+				      struct qed_ptt *p_ptt,
+				      u32 *dump_buf,
+				      u32 buf_size_in_dwords,
+				      u32 *num_dumped_dwords);
+/**
+ * @brief qed_dbg_protection_override_get_dump_buf_size - Returns the required
+ *	buffer size for protection override window results.
+ *
+ * @param p_hwfn - HW device data
+ * @param p_ptt - Ptt window used for writing the registers.
+ * @param buf_size - OUT: required buffer size (in dwords) for protection
+ *	override data.
+ *
+ * @return error if one of the following holds:
+ *	- the version wasn't set
+ * Otherwise, returns ok.
+ */
+enum dbg_status
+qed_dbg_protection_override_get_dump_buf_size(struct qed_hwfn *p_hwfn,
+					      struct qed_ptt *p_ptt,
+					      u32 *buf_size);
+/**
+ * @brief qed_dbg_protection_override_dump - Reads protection override window
+ *	entries and writes the results into the specified buffer.
+ *
+ * @param p_hwfn - HW device data
+ * @param p_ptt - Ptt window used for writing the registers.
+ * @param dump_buf - Pointer to write the protection override data into.
+ * @param buf_size_in_dwords - Size of the specified buffer in dwords.
+ * @param num_dumped_dwords - OUT: number of dumped dwords.
+ *
+ * @return error if one of the following holds:
+ *	- the version wasn't set
+ *	- the specified buffer is too small
+ *	- DMAE transaction failed
+ * Otherwise, returns ok.
+ */
+enum dbg_status qed_dbg_protection_override_dump(struct qed_hwfn *p_hwfn,
+						 struct qed_ptt *p_ptt,
+						 u32 *dump_buf,
+						 u32 buf_size_in_dwords,
+						 u32 *num_dumped_dwords);
+/**
+ * @brief qed_dbg_fw_asserts_get_dump_buf_size - Returns the required buffer
+ *	size for FW Asserts results.
+ *
+ * @param p_hwfn - HW device data
+ * @param p_ptt - Ptt window used for writing the registers.
+ * @param buf_size - OUT: required buffer size (in dwords) for FW Asserts data.
+ *
+ * @return error if one of the following holds:
+ *	- the version wasn't set
+ * Otherwise, returns ok.
+ */
+enum dbg_status qed_dbg_fw_asserts_get_dump_buf_size(struct qed_hwfn *p_hwfn,
+						     struct qed_ptt *p_ptt,
+						     u32 *buf_size);
+/**
+ * @brief qed_dbg_fw_asserts_dump - Reads the FW Asserts and writes the results
+ *	into the specified buffer.
+ *
+ * @param p_hwfn - HW device data
+ * @param p_ptt - Ptt window used for writing the registers.
+ * @param dump_buf - Pointer to write the FW Asserts data into.
+ * @param buf_size_in_dwords - Size of the specified buffer in dwords.
+ * @param num_dumped_dwords - OUT: number of dumped dwords.
+ *
+ * @return error if one of the following holds:
+ *	- the version wasn't set
+ *	- the specified buffer is too small
+ * Otherwise, returns ok.
+ */
+enum dbg_status qed_dbg_fw_asserts_dump(struct qed_hwfn *p_hwfn,
+					struct qed_ptt *p_ptt,
+					u32 *dump_buf,
+					u32 buf_size_in_dwords,
+					u32 *num_dumped_dwords);
+/**
+ * @brief qed_dbg_print_attn - Prints attention registers values in the
+ *	specified results struct.
  *
  * @param p_hwfn
  * @param results - Pointer to the attention read results
@@ -1915,47 +2935,241 @@ struct iro {
 enum dbg_status qed_dbg_print_attn(struct qed_hwfn *p_hwfn,
 				   struct dbg_attn_block_result *results);
 
+/******************************** Constants **********************************/
+
 #define MAX_NAME_LEN	16
 
+/***************************** Public Functions *******************************/
+/**
+ * @brief qed_dbg_user_set_bin_ptr - Sets a pointer to the binary data with
+ *	debug arrays.
+ *
+ * @param bin_ptr - a pointer to the binary data with debug arrays.
+ */
+enum dbg_status qed_dbg_user_set_bin_ptr(const u8 * const bin_ptr);
+/**
+ * @brief qed_dbg_get_status_str - Returns a string for the specified status.
+ *
+ * @param status - a debug status code.
+ *
+ * @return a string for the specified status
+ */
+const char *qed_dbg_get_status_str(enum dbg_status status);
+/**
+ * @brief qed_get_idle_chk_results_buf_size - Returns the required buffer size
+ *	for idle check results (in bytes).
+ *
+ * @param p_hwfn - HW device data
+ * @param dump_buf - idle check dump buffer.
+ * @param num_dumped_dwords - number of dwords that were dumped.
+ * @param results_buf_size - OUT: required buffer size (in bytes) for the parsed
+ *	results.
+ *
+ * @return error if the parsing fails, ok otherwise.
+ */
+enum dbg_status qed_get_idle_chk_results_buf_size(struct qed_hwfn *p_hwfn,
+						  u32 *dump_buf,
+						  u32  num_dumped_dwords,
+						  u32 *results_buf_size);
+/**
+ * @brief qed_print_idle_chk_results - Prints idle check results
+ *
+ * @param p_hwfn - HW device data
+ * @param dump_buf - idle check dump buffer.
+ * @param num_dumped_dwords - number of dwords that were dumped.
+ * @param results_buf - buffer for printing the idle check results.
+ * @param num_errors - OUT: number of errors found in idle check.
+ * @param num_warnings - OUT: number of warnings found in idle check.
+ *
+ * @return error if the parsing fails, ok otherwise.
+ */
+enum dbg_status qed_print_idle_chk_results(struct qed_hwfn *p_hwfn,
+					   u32 *dump_buf,
+					   u32 num_dumped_dwords,
+					   char *results_buf,
+					   u32 *num_errors,
+					   u32 *num_warnings);
+/**
+ * @brief qed_get_mcp_trace_results_buf_size - Returns the required buffer size
+ *	for MCP Trace results (in bytes).
+ *
+ * @param p_hwfn - HW device data
+ * @param dump_buf - MCP Trace dump buffer.
+ * @param num_dumped_dwords - number of dwords that were dumped.
+ * @param results_buf_size - OUT: required buffer size (in bytes) for the parsed
+ *	results.
+ *
+ * @return error if the parsing fails, ok otherwise.
+ */
+enum dbg_status qed_get_mcp_trace_results_buf_size(struct qed_hwfn *p_hwfn,
+						   u32 *dump_buf,
+						   u32 num_dumped_dwords,
+						   u32 *results_buf_size);
+/**
+ * @brief qed_print_mcp_trace_results - Prints MCP Trace results
+ *
+ * @param p_hwfn - HW device data
+ * @param dump_buf - mcp trace dump buffer, starting from the header.
+ * @param num_dumped_dwords - number of dwords that were dumped.
+ * @param results_buf - buffer for printing the mcp trace results.
+ *
+ * @return error if the parsing fails, ok otherwise.
+ */
+enum dbg_status qed_print_mcp_trace_results(struct qed_hwfn *p_hwfn,
+					    u32 *dump_buf,
+					    u32 num_dumped_dwords,
+					    char *results_buf);
+/**
+ * @brief qed_get_reg_fifo_results_buf_size - Returns the required buffer size
+ *	for reg_fifo results (in bytes).
+ *
+ * @param p_hwfn - HW device data
+ * @param dump_buf - reg fifo dump buffer.
+ * @param num_dumped_dwords - number of dwords that were dumped.
+ * @param results_buf_size - OUT: required buffer size (in bytes) for the parsed
+ *	results.
+ *
+ * @return error if the parsing fails, ok otherwise.
+ */
+enum dbg_status qed_get_reg_fifo_results_buf_size(struct qed_hwfn *p_hwfn,
+						  u32 *dump_buf,
+						  u32 num_dumped_dwords,
+						  u32 *results_buf_size);
+/**
+ * @brief qed_print_reg_fifo_results - Prints reg fifo results
+ *
+ * @param p_hwfn - HW device data
+ * @param dump_buf - reg fifo dump buffer, starting from the header.
+ * @param num_dumped_dwords - number of dwords that were dumped.
+ * @param results_buf - buffer for printing the reg fifo results.
+ *
+ * @return error if the parsing fails, ok otherwise.
+ */
+enum dbg_status qed_print_reg_fifo_results(struct qed_hwfn *p_hwfn,
+					   u32 *dump_buf,
+					   u32 num_dumped_dwords,
+					   char *results_buf);
+/**
+ * @brief qed_get_igu_fifo_results_buf_size - Returns the required buffer size
+ *	for igu_fifo results (in bytes).
+ *
+ * @param p_hwfn - HW device data
+ * @param dump_buf - IGU fifo dump buffer.
+ * @param num_dumped_dwords - number of dwords that were dumped.
+ * @param results_buf_size - OUT: required buffer size (in bytes) for the parsed
+ *	results.
+ *
+ * @return error if the parsing fails, ok otherwise.
+ */
+enum dbg_status qed_get_igu_fifo_results_buf_size(struct qed_hwfn *p_hwfn,
+						  u32 *dump_buf,
+						  u32 num_dumped_dwords,
+						  u32 *results_buf_size);
+/**
+ * @brief qed_print_igu_fifo_results - Prints IGU fifo results
+ *
+ * @param p_hwfn - HW device data
+ * @param dump_buf - IGU fifo dump buffer, starting from the header.
+ * @param num_dumped_dwords - number of dwords that were dumped.
+ * @param results_buf - buffer for printing the IGU fifo results.
+ *
+ * @return error if the parsing fails, ok otherwise.
+ */
+enum dbg_status qed_print_igu_fifo_results(struct qed_hwfn *p_hwfn,
+					   u32 *dump_buf,
+					   u32 num_dumped_dwords,
+					   char *results_buf);
+/**
+ * @brief qed_get_protection_override_results_buf_size - Returns the required
+ *	buffer size for protection override results (in bytes).
+ *
+ * @param p_hwfn - HW device data
+ * @param dump_buf - protection override dump buffer.
+ * @param num_dumped_dwords - number of dwords that were dumped.
+ * @param results_buf_size - OUT: required buffer size (in bytes) for the parsed
+ *	results.
+ *
+ * @return error if the parsing fails, ok otherwise.
+ */
+enum dbg_status
+qed_get_protection_override_results_buf_size(struct qed_hwfn *p_hwfn,
+					     u32 *dump_buf,
+					     u32 num_dumped_dwords,
+					     u32 *results_buf_size);
+/**
+ * @brief qed_print_protection_override_results - Prints protection override
+ *	results.
+ *
+ * @param p_hwfn - HW device data
+ * @param dump_buf - protection override dump buffer, starting from the header.
+ * @param num_dumped_dwords - number of dwords that were dumped.
+ * @param results_buf - buffer for printing the reg fifo results.
+ *
+ * @return error if the parsing fails, ok otherwise.
+ */
+enum dbg_status qed_print_protection_override_results(struct qed_hwfn *p_hwfn,
+						      u32 *dump_buf,
+						      u32 num_dumped_dwords,
+						      char *results_buf);
+/**
+ * @brief qed_get_fw_asserts_results_buf_size - Returns the required buffer size
+ *	for FW Asserts results (in bytes).
+ *
+ * @param p_hwfn - HW device data
+ * @param dump_buf - FW Asserts dump buffer.
+ * @param num_dumped_dwords - number of dwords that were dumped.
+ * @param results_buf_size - OUT: required buffer size (in bytes) for the parsed
+ *	results.
+ *
+ * @return error if the parsing fails, ok otherwise.
+ */
+enum dbg_status qed_get_fw_asserts_results_buf_size(struct qed_hwfn *p_hwfn,
+						    u32 *dump_buf,
+						    u32 num_dumped_dwords,
+						    u32 *results_buf_size);
+/**
+ * @brief qed_print_fw_asserts_results - Prints FW Asserts results
+ *
+ * @param p_hwfn - HW device data
+ * @param dump_buf - FW Asserts dump buffer, starting from the header.
+ * @param num_dumped_dwords - number of dwords that were dumped.
+ * @param results_buf - buffer for printing the FW Asserts results.
+ *
+ * @return error if the parsing fails, ok otherwise.
+ */
+enum dbg_status qed_print_fw_asserts_results(struct qed_hwfn *p_hwfn,
+					     u32 *dump_buf,
+					     u32 num_dumped_dwords,
+					     char *results_buf);
 /* Win 2 */
-#define GTT_BAR0_MAP_REG_IGU_CMD \
-	0x00f000UL
+#define GTT_BAR0_MAP_REG_IGU_CMD	0x00f000UL
 
 /* Win 3 */
-#define GTT_BAR0_MAP_REG_TSDM_RAM \
-	0x010000UL
+#define GTT_BAR0_MAP_REG_TSDM_RAM	0x010000UL
 
 /* Win 4 */
-#define GTT_BAR0_MAP_REG_MSDM_RAM \
-	0x011000UL
+#define GTT_BAR0_MAP_REG_MSDM_RAM	0x011000UL
 
 /* Win 5 */
-#define GTT_BAR0_MAP_REG_MSDM_RAM_1024 \
-	0x012000UL
+#define GTT_BAR0_MAP_REG_MSDM_RAM_1024	0x012000UL
 
 /* Win 6 */
-#define GTT_BAR0_MAP_REG_USDM_RAM \
-	0x013000UL
+#define GTT_BAR0_MAP_REG_USDM_RAM	0x013000UL
 
 /* Win 7 */
-#define GTT_BAR0_MAP_REG_USDM_RAM_1024 \
-	0x014000UL
+#define GTT_BAR0_MAP_REG_USDM_RAM_1024	0x014000UL
 
 /* Win 8 */
-#define GTT_BAR0_MAP_REG_USDM_RAM_2048 \
-	0x015000UL
+#define GTT_BAR0_MAP_REG_USDM_RAM_2048	0x015000UL
 
 /* Win 9 */
-#define GTT_BAR0_MAP_REG_XSDM_RAM \
-	0x016000UL
+#define GTT_BAR0_MAP_REG_XSDM_RAM	0x016000UL
 
 /* Win 10 */
-#define GTT_BAR0_MAP_REG_YSDM_RAM \
-	0x017000UL
+#define GTT_BAR0_MAP_REG_YSDM_RAM	0x017000UL
 
 /* Win 11 */
-#define GTT_BAR0_MAP_REG_PSDM_RAM \
-	0x018000UL
+#define GTT_BAR0_MAP_REG_PSDM_RAM	0x018000UL
 
 /**
  * @brief qed_qm_pf_mem_size - prepare QM ILT sizes
@@ -2003,7 +3217,7 @@ struct qed_qm_pf_rt_init_params {
 	u16 num_vf_pqs;
 	u8 start_vport;
 	u8 num_vports;
-	u8 pf_wfq;
+	u16 pf_wfq;
 	u32 pf_rl;
 	struct init_qm_pq_params *pq_params;
 	struct init_qm_vport_params *vport_params;
@@ -2138,6 +3352,9 @@ void qed_set_geneve_enable(struct qed_hwfn *p_hwfn,
 #define	TSTORM_PORT_STAT_OFFSET(port_id) \
 	(IRO[1].base + ((port_id) * IRO[1].m1))
 #define	TSTORM_PORT_STAT_SIZE				(IRO[1].size)
+#define TSTORM_LL2_PORT_STAT_OFFSET(port_id) \
+	(IRO[2].base + ((port_id) * IRO[2].m1))
+#define TSTORM_LL2_PORT_STAT_SIZE			(IRO[2].size)
 #define	USTORM_VF_PF_CHANNEL_READY_OFFSET(vf_id) \
 	(IRO[3].base + ((vf_id) * IRO[3].m1))
 #define	USTORM_VF_PF_CHANNEL_READY_SIZE			(IRO[3].size)
@@ -2153,42 +3370,90 @@ void qed_set_geneve_enable(struct qed_hwfn *p_hwfn,
 #define	USTORM_COMMON_QUEUE_CONS_OFFSET(queue_zone_id) \
 	(IRO[7].base + ((queue_zone_id) * IRO[7].m1))
 #define	USTORM_COMMON_QUEUE_CONS_SIZE			(IRO[7].size)
+#define TSTORM_LL2_RX_PRODS_OFFSET(core_rx_queue_id) \
+	(IRO[14].base +	((core_rx_queue_id) * IRO[14].m1))
+#define TSTORM_LL2_RX_PRODS_SIZE			(IRO[14].size)
+#define CORE_LL2_TSTORM_PER_QUEUE_STAT_OFFSET(core_rx_queue_id) \
+	(IRO[15].base + ((core_rx_queue_id) * IRO[15].m1))
+#define CORE_LL2_TSTORM_PER_QUEUE_STAT_SIZE		(IRO[15].size)
+#define CORE_LL2_USTORM_PER_QUEUE_STAT_OFFSET(core_rx_queue_id) \
+	(IRO[16].base +	((core_rx_queue_id) * IRO[16].m1))
+#define CORE_LL2_USTORM_PER_QUEUE_STAT_SIZE		(IRO[16].size)
+#define CORE_LL2_PSTORM_PER_QUEUE_STAT_OFFSET(core_tx_stats_id) \
+	(IRO[17].base +	((core_tx_stats_id) * IRO[17].m1))
+#define CORE_LL2_PSTORM_PER_QUEUE_STAT_SIZE	(IRO[17].	size)
 #define	MSTORM_QUEUE_STAT_OFFSET(stat_counter_id) \
 	(IRO[18].base + ((stat_counter_id) * IRO[18].m1))
 #define	MSTORM_QUEUE_STAT_SIZE				(IRO[18].size)
 #define	MSTORM_ETH_PF_PRODS_OFFSET(queue_id) \
 	(IRO[19].base + ((queue_id) * IRO[19].m1))
 #define	MSTORM_ETH_PF_PRODS_SIZE			(IRO[19].size)
-#define	MSTORM_TPA_TIMEOUT_US_OFFSET			(IRO[20].base)
-#define	MSTORM_TPA_TIMEOUT_US_SIZE			(IRO[20].size)
+#define MSTORM_ETH_VF_PRODS_OFFSET(vf_id, vf_queue_id) \
+	(IRO[20].base +	((vf_id) * IRO[20].m1) + ((vf_queue_id) * IRO[20].m2))
+#define MSTORM_ETH_VF_PRODS_SIZE			(IRO[20].size)
+#define	MSTORM_TPA_TIMEOUT_US_OFFSET			(IRO[21].base)
+#define	MSTORM_TPA_TIMEOUT_US_SIZE			(IRO[21].size)
 #define	MSTORM_ETH_PF_STAT_OFFSET(pf_id) \
-	(IRO[21].base + ((pf_id) * IRO[21].m1))
+	(IRO[22].base + ((pf_id) * IRO[22].m1))
 #define	MSTORM_ETH_PF_STAT_SIZE				(IRO[21].size)
 #define	USTORM_QUEUE_STAT_OFFSET(stat_counter_id) \
-	(IRO[22].base + ((stat_counter_id) * IRO[22].m1))
-#define	USTORM_QUEUE_STAT_SIZE				(IRO[22].size)
+	(IRO[23].base + ((stat_counter_id) * IRO[23].m1))
+#define	USTORM_QUEUE_STAT_SIZE				(IRO[23].size)
 #define	USTORM_ETH_PF_STAT_OFFSET(pf_id) \
-	(IRO[23].base + ((pf_id) * IRO[23].m1))
-#define	USTORM_ETH_PF_STAT_SIZE				(IRO[23].size)
+	(IRO[24].base + ((pf_id) * IRO[24].m1))
+#define	USTORM_ETH_PF_STAT_SIZE				(IRO[24].size)
 #define	PSTORM_QUEUE_STAT_OFFSET(stat_counter_id) \
-	(IRO[24].base + ((stat_counter_id) * IRO[24].m1))
-#define	PSTORM_QUEUE_STAT_SIZE				(IRO[24].size)
+	(IRO[25].base + ((stat_counter_id) * IRO[25].m1))
+#define	PSTORM_QUEUE_STAT_SIZE				(IRO[25].size)
 #define	PSTORM_ETH_PF_STAT_OFFSET(pf_id) \
-	(IRO[25].base + ((pf_id) * IRO[25].m1))
-#define	PSTORM_ETH_PF_STAT_SIZE				(IRO[25].size)
+	(IRO[26].base + ((pf_id) * IRO[26].m1))
+#define	PSTORM_ETH_PF_STAT_SIZE				(IRO[26].size)
 #define	PSTORM_CTL_FRAME_ETHTYPE_OFFSET(ethtype) \
-	(IRO[26].base + ((ethtype) * IRO[26].m1))
-#define	PSTORM_CTL_FRAME_ETHTYPE_SIZE			(IRO[26].size)
-#define	TSTORM_ETH_PRS_INPUT_OFFSET			(IRO[27].base)
-#define	TSTORM_ETH_PRS_INPUT_SIZE			(IRO[27].size)
+	(IRO[27].base + ((ethtype) * IRO[27].m1))
+#define	PSTORM_CTL_FRAME_ETHTYPE_SIZE			(IRO[27].size)
+#define	TSTORM_ETH_PRS_INPUT_OFFSET			(IRO[28].base)
+#define	TSTORM_ETH_PRS_INPUT_SIZE			(IRO[28].size)
 #define	ETH_RX_RATE_LIMIT_OFFSET(pf_id) \
-	(IRO[28].base + ((pf_id) * IRO[28].m1))
-#define	ETH_RX_RATE_LIMIT_SIZE				(IRO[28].size)
+	(IRO[29].base + ((pf_id) * IRO[29].m1))
+#define	ETH_RX_RATE_LIMIT_SIZE				(IRO[29].size)
 #define	XSTORM_ETH_QUEUE_ZONE_OFFSET(queue_id) \
-	(IRO[29].base + ((queue_id) * IRO[29].m1))
-#define	XSTORM_ETH_QUEUE_ZONE_SIZE			(IRO[29].size)
-
-static const struct iro iro_arr[46] = {
+	(IRO[30].base + ((queue_id) * IRO[30].m1))
+#define	XSTORM_ETH_QUEUE_ZONE_SIZE			(IRO[30].size)
+#define TSTORM_SCSI_CMDQ_CONS_OFFSET(cmdq_queue_id) \
+	(IRO[34].base +	((cmdq_queue_id) * IRO[34].m1))
+#define TSTORM_SCSI_CMDQ_CONS_SIZE				(IRO[34].size)
+#define TSTORM_SCSI_BDQ_EXT_PROD_OFFSET(func_id, bdq_id) \
+	(IRO[35].base +	((func_id) * IRO[35].m1) + ((bdq_id) * IRO[35].m2))
+#define TSTORM_SCSI_BDQ_EXT_PROD_SIZE				(IRO[35].size)
+#define MSTORM_SCSI_BDQ_EXT_PROD_OFFSET(func_id, bdq_id) \
+	(IRO[36].base +	((func_id) * IRO[36].m1) + ((bdq_id) * IRO[36].m2))
+#define MSTORM_SCSI_BDQ_EXT_PROD_SIZE				(IRO[36].size)
+#define TSTORM_ISCSI_RX_STATS_OFFSET(pf_id) \
+	(IRO[37].base +	((pf_id) * IRO[37].m1))
+#define TSTORM_ISCSI_RX_STATS_SIZE				(IRO[37].size)
+#define MSTORM_ISCSI_RX_STATS_OFFSET(pf_id) \
+	(IRO[38].base +	((pf_id) * IRO[38].m1))
+#define MSTORM_ISCSI_RX_STATS_SIZE				(IRO[38].size)
+#define USTORM_ISCSI_RX_STATS_OFFSET(pf_id) \
+	(IRO[39].base +	((pf_id) * IRO[39].m1))
+#define USTORM_ISCSI_RX_STATS_SIZE				(IRO[39].size)
+#define XSTORM_ISCSI_TX_STATS_OFFSET(pf_id) \
+	(IRO[40].base +	((pf_id) * IRO[40].m1))
+#define XSTORM_ISCSI_TX_STATS_SIZE				(IRO[40].size)
+#define YSTORM_ISCSI_TX_STATS_OFFSET(pf_id) \
+	(IRO[41].base +	((pf_id) * IRO[41].m1))
+#define YSTORM_ISCSI_TX_STATS_SIZE				(IRO[41].size)
+#define PSTORM_ISCSI_TX_STATS_OFFSET(pf_id) \
+	(IRO[42].base +	((pf_id) * IRO[42].m1))
+#define PSTORM_ISCSI_TX_STATS_SIZE				(IRO[42].size)
+#define PSTORM_RDMA_QUEUE_STAT_OFFSET(rdma_stat_counter_id) \
+	(IRO[45].base +	((rdma_stat_counter_id) * IRO[45].m1))
+#define PSTORM_RDMA_QUEUE_STAT_SIZE				(IRO[45].size)
+#define TSTORM_RDMA_QUEUE_STAT_OFFSET(rdma_stat_counter_id) \
+	(IRO[46].base +	((rdma_stat_counter_id) * IRO[46].m1))
+#define TSTORM_RDMA_QUEUE_STAT_SIZE				(IRO[46].size)
+
+static const struct iro iro_arr[47] = {
 	{0x0, 0x0, 0x0, 0x0, 0x8},
 	{0x4cb0, 0x78, 0x0, 0x0, 0x78},
 	{0x6318, 0x20, 0x0, 0x0, 0x20},
@@ -2201,20 +3466,21 @@ static const struct iro iro_arr[46] = {
 	{0x3df0, 0x0, 0x0, 0x0, 0x78},
 	{0x29b0, 0x0, 0x0, 0x0, 0x78},
 	{0x4c38, 0x0, 0x0, 0x0, 0x78},
-	{0x4a48, 0x0, 0x0, 0x0, 0x78},
+	{0x4990, 0x0, 0x0, 0x0, 0x78},
 	{0x7e48, 0x0, 0x0, 0x0, 0x78},
 	{0xa28, 0x8, 0x0, 0x0, 0x8},
 	{0x60f8, 0x10, 0x0, 0x0, 0x10},
 	{0xb820, 0x30, 0x0, 0x0, 0x30},
 	{0x95b8, 0x30, 0x0, 0x0, 0x30},
-	{0x4c18, 0x80, 0x0, 0x0, 0x40},
+	{0x4b60, 0x80, 0x0, 0x0, 0x40},
 	{0x1f8, 0x4, 0x0, 0x0, 0x4},
-	{0xc9a8, 0x0, 0x0, 0x0, 0x4},
-	{0x4c58, 0x80, 0x0, 0x0, 0x20},
+	{0x53a0, 0x80, 0x4, 0x0, 0x4},
+	{0xc8f0, 0x0, 0x0, 0x0, 0x4},
+	{0x4ba0, 0x80, 0x0, 0x0, 0x20},
 	{0x8050, 0x40, 0x0, 0x0, 0x30},
 	{0xe770, 0x60, 0x0, 0x0, 0x60},
 	{0x2b48, 0x80, 0x0, 0x0, 0x38},
-	{0xdf88, 0x78, 0x0, 0x0, 0x78},
+	{0xf188, 0x78, 0x0, 0x0, 0x78},
 	{0x1f8, 0x4, 0x0, 0x0, 0x4},
 	{0xacf0, 0x0, 0x0, 0x0, 0xf0},
 	{0xade0, 0x8, 0x0, 0x0, 0x8},
@@ -2226,455 +3492,457 @@ static const struct iro iro_arr[46] = {
 	{0x200, 0x10, 0x8, 0x0, 0x8},
 	{0xb78, 0x10, 0x8, 0x0, 0x2},
 	{0xd888, 0x38, 0x0, 0x0, 0x24},
-	{0x12120, 0x10, 0x0, 0x0, 0x8},
-	{0x11b20, 0x38, 0x0, 0x0, 0x18},
+	{0x12c38, 0x10, 0x0, 0x0, 0x8},
+	{0x11aa0, 0x38, 0x0, 0x0, 0x18},
 	{0xa8c0, 0x30, 0x0, 0x0, 0x10},
 	{0x86f8, 0x28, 0x0, 0x0, 0x18},
-	{0xeff8, 0x10, 0x0, 0x0, 0x10},
+	{0x101f8, 0x10, 0x0, 0x0, 0x10},
 	{0xdd08, 0x48, 0x0, 0x0, 0x38},
-	{0xf460, 0x20, 0x0, 0x0, 0x20},
+	{0x10660, 0x20, 0x0, 0x0, 0x20},
 	{0x2b80, 0x80, 0x0, 0x0, 0x10},
 	{0x5000, 0x10, 0x0, 0x0, 0x10},
 };
 
 /* Runtime array offsets */
-#define DORQ_REG_PF_MAX_ICID_0_RT_OFFSET 0
-#define DORQ_REG_PF_MAX_ICID_1_RT_OFFSET 1
-#define DORQ_REG_PF_MAX_ICID_2_RT_OFFSET 2
-#define DORQ_REG_PF_MAX_ICID_3_RT_OFFSET 3
-#define DORQ_REG_PF_MAX_ICID_4_RT_OFFSET 4
-#define DORQ_REG_PF_MAX_ICID_5_RT_OFFSET 5
-#define DORQ_REG_PF_MAX_ICID_6_RT_OFFSET 6
-#define DORQ_REG_PF_MAX_ICID_7_RT_OFFSET 7
-#define DORQ_REG_VF_MAX_ICID_0_RT_OFFSET 8
-#define DORQ_REG_VF_MAX_ICID_1_RT_OFFSET 9
-#define DORQ_REG_VF_MAX_ICID_2_RT_OFFSET 10
-#define DORQ_REG_VF_MAX_ICID_3_RT_OFFSET 11
-#define DORQ_REG_VF_MAX_ICID_4_RT_OFFSET 12
-#define DORQ_REG_VF_MAX_ICID_5_RT_OFFSET 13
-#define DORQ_REG_VF_MAX_ICID_6_RT_OFFSET 14
-#define DORQ_REG_VF_MAX_ICID_7_RT_OFFSET 15
-#define DORQ_REG_PF_WAKE_ALL_RT_OFFSET 16
-#define DORQ_REG_TAG1_ETHERTYPE_RT_OFFSET 17
-#define IGU_REG_PF_CONFIGURATION_RT_OFFSET 18
-#define IGU_REG_VF_CONFIGURATION_RT_OFFSET 19
-#define IGU_REG_ATTN_MSG_ADDR_L_RT_OFFSET 20
-#define IGU_REG_ATTN_MSG_ADDR_H_RT_OFFSET 21
-#define IGU_REG_LEADING_EDGE_LATCH_RT_OFFSET 22
-#define IGU_REG_TRAILING_EDGE_LATCH_RT_OFFSET 23
-#define CAU_REG_CQE_AGG_UNIT_SIZE_RT_OFFSET 24
-#define CAU_REG_SB_VAR_MEMORY_RT_OFFSET 761
-#define CAU_REG_SB_VAR_MEMORY_RT_SIZE 736
-#define CAU_REG_SB_VAR_MEMORY_RT_OFFSET 761
-#define CAU_REG_SB_VAR_MEMORY_RT_SIZE 736
-#define CAU_REG_SB_ADDR_MEMORY_RT_OFFSET 1497
-#define CAU_REG_SB_ADDR_MEMORY_RT_SIZE 736
-#define CAU_REG_PI_MEMORY_RT_OFFSET 2233
-#define CAU_REG_PI_MEMORY_RT_SIZE 4416
-#define PRS_REG_SEARCH_RESP_INITIATOR_TYPE_RT_OFFSET 6649
-#define PRS_REG_TASK_ID_MAX_INITIATOR_PF_RT_OFFSET 6650
-#define PRS_REG_TASK_ID_MAX_INITIATOR_VF_RT_OFFSET 6651
-#define PRS_REG_TASK_ID_MAX_TARGET_PF_RT_OFFSET 6652
-#define PRS_REG_TASK_ID_MAX_TARGET_VF_RT_OFFSET 6653
-#define PRS_REG_SEARCH_TCP_RT_OFFSET 6654
-#define PRS_REG_SEARCH_FCOE_RT_OFFSET 6655
-#define PRS_REG_SEARCH_ROCE_RT_OFFSET 6656
-#define PRS_REG_ROCE_DEST_QP_MAX_VF_RT_OFFSET 6657
-#define PRS_REG_ROCE_DEST_QP_MAX_PF_RT_OFFSET 6658
-#define PRS_REG_SEARCH_OPENFLOW_RT_OFFSET 6659
-#define PRS_REG_SEARCH_NON_IP_AS_OPENFLOW_RT_OFFSET 6660
-#define PRS_REG_OPENFLOW_SUPPORT_ONLY_KNOWN_OVER_IP_RT_OFFSET 6661
-#define PRS_REG_OPENFLOW_SEARCH_KEY_MASK_RT_OFFSET 6662
-#define PRS_REG_TAG_ETHERTYPE_0_RT_OFFSET 6663
-#define PRS_REG_LIGHT_L2_ETHERTYPE_EN_RT_OFFSET 6664
-#define SRC_REG_FIRSTFREE_RT_OFFSET 6665
-#define SRC_REG_FIRSTFREE_RT_SIZE 2
-#define SRC_REG_LASTFREE_RT_OFFSET 6667
-#define SRC_REG_LASTFREE_RT_SIZE 2
-#define SRC_REG_COUNTFREE_RT_OFFSET 6669
-#define SRC_REG_NUMBER_HASH_BITS_RT_OFFSET 6670
-#define PSWRQ2_REG_CDUT_P_SIZE_RT_OFFSET 6671
-#define PSWRQ2_REG_CDUC_P_SIZE_RT_OFFSET 6672
-#define PSWRQ2_REG_TM_P_SIZE_RT_OFFSET 6673
-#define PSWRQ2_REG_QM_P_SIZE_RT_OFFSET 6674
-#define PSWRQ2_REG_SRC_P_SIZE_RT_OFFSET 6675
-#define PSWRQ2_REG_TSDM_P_SIZE_RT_OFFSET 6676
-#define PSWRQ2_REG_TM_FIRST_ILT_RT_OFFSET 6677
-#define PSWRQ2_REG_TM_LAST_ILT_RT_OFFSET 6678
-#define PSWRQ2_REG_QM_FIRST_ILT_RT_OFFSET 6679
-#define PSWRQ2_REG_QM_LAST_ILT_RT_OFFSET 6680
-#define PSWRQ2_REG_SRC_FIRST_ILT_RT_OFFSET 6681
-#define PSWRQ2_REG_SRC_LAST_ILT_RT_OFFSET 6682
-#define PSWRQ2_REG_CDUC_FIRST_ILT_RT_OFFSET 6683
-#define PSWRQ2_REG_CDUC_LAST_ILT_RT_OFFSET 6684
-#define PSWRQ2_REG_CDUT_FIRST_ILT_RT_OFFSET 6685
-#define PSWRQ2_REG_CDUT_LAST_ILT_RT_OFFSET 6686
-#define PSWRQ2_REG_TSDM_FIRST_ILT_RT_OFFSET 6687
-#define PSWRQ2_REG_TSDM_LAST_ILT_RT_OFFSET 6688
-#define PSWRQ2_REG_TM_NUMBER_OF_PF_BLOCKS_RT_OFFSET 6689
-#define PSWRQ2_REG_CDUT_NUMBER_OF_PF_BLOCKS_RT_OFFSET 6690
-#define PSWRQ2_REG_CDUC_NUMBER_OF_PF_BLOCKS_RT_OFFSET 6691
-#define PSWRQ2_REG_TM_VF_BLOCKS_RT_OFFSET 6692
-#define PSWRQ2_REG_CDUT_VF_BLOCKS_RT_OFFSET 6693
-#define PSWRQ2_REG_CDUC_VF_BLOCKS_RT_OFFSET 6694
-#define PSWRQ2_REG_TM_BLOCKS_FACTOR_RT_OFFSET 6695
-#define PSWRQ2_REG_CDUT_BLOCKS_FACTOR_RT_OFFSET 6696
-#define PSWRQ2_REG_CDUC_BLOCKS_FACTOR_RT_OFFSET 6697
-#define PSWRQ2_REG_VF_BASE_RT_OFFSET 6698
-#define PSWRQ2_REG_VF_LAST_ILT_RT_OFFSET 6699
-#define PSWRQ2_REG_WR_MBS0_RT_OFFSET 6700
-#define PSWRQ2_REG_RD_MBS0_RT_OFFSET 6701
-#define PSWRQ2_REG_DRAM_ALIGN_WR_RT_OFFSET 6702
-#define PSWRQ2_REG_DRAM_ALIGN_RD_RT_OFFSET 6703
-#define PSWRQ2_REG_ILT_MEMORY_RT_OFFSET 6704
-#define PSWRQ2_REG_ILT_MEMORY_RT_SIZE 22000
-#define PGLUE_REG_B_VF_BASE_RT_OFFSET 28704
-#define PGLUE_REG_B_CACHE_LINE_SIZE_RT_OFFSET 28705
-#define PGLUE_REG_B_PF_BAR0_SIZE_RT_OFFSET 28706
-#define PGLUE_REG_B_PF_BAR1_SIZE_RT_OFFSET 28707
-#define PGLUE_REG_B_VF_BAR1_SIZE_RT_OFFSET 28708
-#define TM_REG_VF_ENABLE_CONN_RT_OFFSET 28709
-#define TM_REG_PF_ENABLE_CONN_RT_OFFSET 28710
-#define TM_REG_PF_ENABLE_TASK_RT_OFFSET 28711
-#define TM_REG_GROUP_SIZE_RESOLUTION_CONN_RT_OFFSET 28712
-#define TM_REG_GROUP_SIZE_RESOLUTION_TASK_RT_OFFSET 28713
-#define TM_REG_CONFIG_CONN_MEM_RT_OFFSET 28714
-#define TM_REG_CONFIG_CONN_MEM_RT_SIZE 416
-#define TM_REG_CONFIG_TASK_MEM_RT_OFFSET 29130
-#define TM_REG_CONFIG_TASK_MEM_RT_SIZE 512
-#define QM_REG_MAXPQSIZE_0_RT_OFFSET 29642
-#define QM_REG_MAXPQSIZE_1_RT_OFFSET 29643
-#define QM_REG_MAXPQSIZE_2_RT_OFFSET 29644
-#define QM_REG_MAXPQSIZETXSEL_0_RT_OFFSET 29645
-#define QM_REG_MAXPQSIZETXSEL_1_RT_OFFSET 29646
-#define QM_REG_MAXPQSIZETXSEL_2_RT_OFFSET 29647
-#define QM_REG_MAXPQSIZETXSEL_3_RT_OFFSET 29648
-#define QM_REG_MAXPQSIZETXSEL_4_RT_OFFSET 29649
-#define QM_REG_MAXPQSIZETXSEL_5_RT_OFFSET 29650
-#define QM_REG_MAXPQSIZETXSEL_6_RT_OFFSET 29651
-#define QM_REG_MAXPQSIZETXSEL_7_RT_OFFSET 29652
-#define QM_REG_MAXPQSIZETXSEL_8_RT_OFFSET 29653
-#define QM_REG_MAXPQSIZETXSEL_9_RT_OFFSET 29654
-#define QM_REG_MAXPQSIZETXSEL_10_RT_OFFSET 29655
-#define QM_REG_MAXPQSIZETXSEL_11_RT_OFFSET 29656
-#define QM_REG_MAXPQSIZETXSEL_12_RT_OFFSET 29657
-#define QM_REG_MAXPQSIZETXSEL_13_RT_OFFSET 29658
-#define QM_REG_MAXPQSIZETXSEL_14_RT_OFFSET 29659
-#define QM_REG_MAXPQSIZETXSEL_15_RT_OFFSET 29660
-#define QM_REG_MAXPQSIZETXSEL_16_RT_OFFSET 29661
-#define QM_REG_MAXPQSIZETXSEL_17_RT_OFFSET 29662
-#define QM_REG_MAXPQSIZETXSEL_18_RT_OFFSET 29663
-#define QM_REG_MAXPQSIZETXSEL_19_RT_OFFSET 29664
-#define QM_REG_MAXPQSIZETXSEL_20_RT_OFFSET 29665
-#define QM_REG_MAXPQSIZETXSEL_21_RT_OFFSET 29666
-#define QM_REG_MAXPQSIZETXSEL_22_RT_OFFSET 29667
-#define QM_REG_MAXPQSIZETXSEL_23_RT_OFFSET 29668
-#define QM_REG_MAXPQSIZETXSEL_24_RT_OFFSET 29669
-#define QM_REG_MAXPQSIZETXSEL_25_RT_OFFSET 29670
-#define QM_REG_MAXPQSIZETXSEL_26_RT_OFFSET 29671
-#define QM_REG_MAXPQSIZETXSEL_27_RT_OFFSET 29672
-#define QM_REG_MAXPQSIZETXSEL_28_RT_OFFSET 29673
-#define QM_REG_MAXPQSIZETXSEL_29_RT_OFFSET 29674
-#define QM_REG_MAXPQSIZETXSEL_30_RT_OFFSET 29675
-#define QM_REG_MAXPQSIZETXSEL_31_RT_OFFSET 29676
-#define QM_REG_MAXPQSIZETXSEL_32_RT_OFFSET 29677
-#define QM_REG_MAXPQSIZETXSEL_33_RT_OFFSET 29678
-#define QM_REG_MAXPQSIZETXSEL_34_RT_OFFSET 29679
-#define QM_REG_MAXPQSIZETXSEL_35_RT_OFFSET 29680
-#define QM_REG_MAXPQSIZETXSEL_36_RT_OFFSET 29681
-#define QM_REG_MAXPQSIZETXSEL_37_RT_OFFSET 29682
-#define QM_REG_MAXPQSIZETXSEL_38_RT_OFFSET 29683
-#define QM_REG_MAXPQSIZETXSEL_39_RT_OFFSET 29684
-#define QM_REG_MAXPQSIZETXSEL_40_RT_OFFSET 29685
-#define QM_REG_MAXPQSIZETXSEL_41_RT_OFFSET 29686
-#define QM_REG_MAXPQSIZETXSEL_42_RT_OFFSET 29687
-#define QM_REG_MAXPQSIZETXSEL_43_RT_OFFSET 29688
-#define QM_REG_MAXPQSIZETXSEL_44_RT_OFFSET 29689
-#define QM_REG_MAXPQSIZETXSEL_45_RT_OFFSET 29690
-#define QM_REG_MAXPQSIZETXSEL_46_RT_OFFSET 29691
-#define QM_REG_MAXPQSIZETXSEL_47_RT_OFFSET 29692
-#define QM_REG_MAXPQSIZETXSEL_48_RT_OFFSET 29693
-#define QM_REG_MAXPQSIZETXSEL_49_RT_OFFSET 29694
-#define QM_REG_MAXPQSIZETXSEL_50_RT_OFFSET 29695
-#define QM_REG_MAXPQSIZETXSEL_51_RT_OFFSET 29696
-#define QM_REG_MAXPQSIZETXSEL_52_RT_OFFSET 29697
-#define QM_REG_MAXPQSIZETXSEL_53_RT_OFFSET 29698
-#define QM_REG_MAXPQSIZETXSEL_54_RT_OFFSET 29699
-#define QM_REG_MAXPQSIZETXSEL_55_RT_OFFSET 29700
-#define QM_REG_MAXPQSIZETXSEL_56_RT_OFFSET 29701
-#define QM_REG_MAXPQSIZETXSEL_57_RT_OFFSET 29702
-#define QM_REG_MAXPQSIZETXSEL_58_RT_OFFSET 29703
-#define QM_REG_MAXPQSIZETXSEL_59_RT_OFFSET 29704
-#define QM_REG_MAXPQSIZETXSEL_60_RT_OFFSET 29705
-#define QM_REG_MAXPQSIZETXSEL_61_RT_OFFSET 29706
-#define QM_REG_MAXPQSIZETXSEL_62_RT_OFFSET 29707
-#define QM_REG_MAXPQSIZETXSEL_63_RT_OFFSET 29708
-#define QM_REG_BASEADDROTHERPQ_RT_OFFSET 29709
-#define QM_REG_BASEADDROTHERPQ_RT_SIZE 128
-#define QM_REG_VOQCRDLINE_RT_OFFSET 29837
-#define QM_REG_VOQCRDLINE_RT_SIZE 20
-#define QM_REG_VOQINITCRDLINE_RT_OFFSET 29857
-#define QM_REG_VOQINITCRDLINE_RT_SIZE 20
-#define QM_REG_AFULLQMBYPTHRPFWFQ_RT_OFFSET 29877
-#define QM_REG_AFULLQMBYPTHRVPWFQ_RT_OFFSET 29878
-#define QM_REG_AFULLQMBYPTHRPFRL_RT_OFFSET 29879
-#define QM_REG_AFULLQMBYPTHRGLBLRL_RT_OFFSET 29880
-#define QM_REG_AFULLOPRTNSTCCRDMASK_RT_OFFSET 29881
-#define QM_REG_WRROTHERPQGRP_0_RT_OFFSET 29882
-#define QM_REG_WRROTHERPQGRP_1_RT_OFFSET 29883
-#define QM_REG_WRROTHERPQGRP_2_RT_OFFSET 29884
-#define QM_REG_WRROTHERPQGRP_3_RT_OFFSET 29885
-#define QM_REG_WRROTHERPQGRP_4_RT_OFFSET 29886
-#define QM_REG_WRROTHERPQGRP_5_RT_OFFSET 29887
-#define QM_REG_WRROTHERPQGRP_6_RT_OFFSET 29888
-#define QM_REG_WRROTHERPQGRP_7_RT_OFFSET 29889
-#define QM_REG_WRROTHERPQGRP_8_RT_OFFSET 29890
-#define QM_REG_WRROTHERPQGRP_9_RT_OFFSET 29891
-#define QM_REG_WRROTHERPQGRP_10_RT_OFFSET 29892
-#define QM_REG_WRROTHERPQGRP_11_RT_OFFSET 29893
-#define QM_REG_WRROTHERPQGRP_12_RT_OFFSET 29894
-#define QM_REG_WRROTHERPQGRP_13_RT_OFFSET 29895
-#define QM_REG_WRROTHERPQGRP_14_RT_OFFSET 29896
-#define QM_REG_WRROTHERPQGRP_15_RT_OFFSET 29897
-#define QM_REG_WRROTHERGRPWEIGHT_0_RT_OFFSET 29898
-#define QM_REG_WRROTHERGRPWEIGHT_1_RT_OFFSET 29899
-#define QM_REG_WRROTHERGRPWEIGHT_2_RT_OFFSET 29900
-#define QM_REG_WRROTHERGRPWEIGHT_3_RT_OFFSET 29901
-#define QM_REG_WRRTXGRPWEIGHT_0_RT_OFFSET 29902
-#define QM_REG_WRRTXGRPWEIGHT_1_RT_OFFSET 29903
-#define QM_REG_PQTX2PF_0_RT_OFFSET 29904
-#define QM_REG_PQTX2PF_1_RT_OFFSET 29905
-#define QM_REG_PQTX2PF_2_RT_OFFSET 29906
-#define QM_REG_PQTX2PF_3_RT_OFFSET 29907
-#define QM_REG_PQTX2PF_4_RT_OFFSET 29908
-#define QM_REG_PQTX2PF_5_RT_OFFSET 29909
-#define QM_REG_PQTX2PF_6_RT_OFFSET 29910
-#define QM_REG_PQTX2PF_7_RT_OFFSET 29911
-#define QM_REG_PQTX2PF_8_RT_OFFSET 29912
-#define QM_REG_PQTX2PF_9_RT_OFFSET 29913
-#define QM_REG_PQTX2PF_10_RT_OFFSET 29914
-#define QM_REG_PQTX2PF_11_RT_OFFSET 29915
-#define QM_REG_PQTX2PF_12_RT_OFFSET 29916
-#define QM_REG_PQTX2PF_13_RT_OFFSET 29917
-#define QM_REG_PQTX2PF_14_RT_OFFSET 29918
-#define QM_REG_PQTX2PF_15_RT_OFFSET 29919
-#define QM_REG_PQTX2PF_16_RT_OFFSET 29920
-#define QM_REG_PQTX2PF_17_RT_OFFSET 29921
-#define QM_REG_PQTX2PF_18_RT_OFFSET 29922
-#define QM_REG_PQTX2PF_19_RT_OFFSET 29923
-#define QM_REG_PQTX2PF_20_RT_OFFSET 29924
-#define QM_REG_PQTX2PF_21_RT_OFFSET 29925
-#define QM_REG_PQTX2PF_22_RT_OFFSET 29926
-#define QM_REG_PQTX2PF_23_RT_OFFSET 29927
-#define QM_REG_PQTX2PF_24_RT_OFFSET 29928
-#define QM_REG_PQTX2PF_25_RT_OFFSET 29929
-#define QM_REG_PQTX2PF_26_RT_OFFSET 29930
-#define QM_REG_PQTX2PF_27_RT_OFFSET 29931
-#define QM_REG_PQTX2PF_28_RT_OFFSET 29932
-#define QM_REG_PQTX2PF_29_RT_OFFSET 29933
-#define QM_REG_PQTX2PF_30_RT_OFFSET 29934
-#define QM_REG_PQTX2PF_31_RT_OFFSET 29935
-#define QM_REG_PQTX2PF_32_RT_OFFSET 29936
-#define QM_REG_PQTX2PF_33_RT_OFFSET 29937
-#define QM_REG_PQTX2PF_34_RT_OFFSET 29938
-#define QM_REG_PQTX2PF_35_RT_OFFSET 29939
-#define QM_REG_PQTX2PF_36_RT_OFFSET 29940
-#define QM_REG_PQTX2PF_37_RT_OFFSET 29941
-#define QM_REG_PQTX2PF_38_RT_OFFSET 29942
-#define QM_REG_PQTX2PF_39_RT_OFFSET 29943
-#define QM_REG_PQTX2PF_40_RT_OFFSET 29944
-#define QM_REG_PQTX2PF_41_RT_OFFSET 29945
-#define QM_REG_PQTX2PF_42_RT_OFFSET 29946
-#define QM_REG_PQTX2PF_43_RT_OFFSET 29947
-#define QM_REG_PQTX2PF_44_RT_OFFSET 29948
-#define QM_REG_PQTX2PF_45_RT_OFFSET 29949
-#define QM_REG_PQTX2PF_46_RT_OFFSET 29950
-#define QM_REG_PQTX2PF_47_RT_OFFSET 29951
-#define QM_REG_PQTX2PF_48_RT_OFFSET 29952
-#define QM_REG_PQTX2PF_49_RT_OFFSET 29953
-#define QM_REG_PQTX2PF_50_RT_OFFSET 29954
-#define QM_REG_PQTX2PF_51_RT_OFFSET 29955
-#define QM_REG_PQTX2PF_52_RT_OFFSET 29956
-#define QM_REG_PQTX2PF_53_RT_OFFSET 29957
-#define QM_REG_PQTX2PF_54_RT_OFFSET 29958
-#define QM_REG_PQTX2PF_55_RT_OFFSET 29959
-#define QM_REG_PQTX2PF_56_RT_OFFSET 29960
-#define QM_REG_PQTX2PF_57_RT_OFFSET 29961
-#define QM_REG_PQTX2PF_58_RT_OFFSET 29962
-#define QM_REG_PQTX2PF_59_RT_OFFSET 29963
-#define QM_REG_PQTX2PF_60_RT_OFFSET 29964
-#define QM_REG_PQTX2PF_61_RT_OFFSET 29965
-#define QM_REG_PQTX2PF_62_RT_OFFSET 29966
-#define QM_REG_PQTX2PF_63_RT_OFFSET 29967
-#define QM_REG_PQOTHER2PF_0_RT_OFFSET 29968
-#define QM_REG_PQOTHER2PF_1_RT_OFFSET 29969
-#define QM_REG_PQOTHER2PF_2_RT_OFFSET 29970
-#define QM_REG_PQOTHER2PF_3_RT_OFFSET 29971
-#define QM_REG_PQOTHER2PF_4_RT_OFFSET 29972
-#define QM_REG_PQOTHER2PF_5_RT_OFFSET 29973
-#define QM_REG_PQOTHER2PF_6_RT_OFFSET 29974
-#define QM_REG_PQOTHER2PF_7_RT_OFFSET 29975
-#define QM_REG_PQOTHER2PF_8_RT_OFFSET 29976
-#define QM_REG_PQOTHER2PF_9_RT_OFFSET 29977
-#define QM_REG_PQOTHER2PF_10_RT_OFFSET 29978
-#define QM_REG_PQOTHER2PF_11_RT_OFFSET 29979
-#define QM_REG_PQOTHER2PF_12_RT_OFFSET 29980
-#define QM_REG_PQOTHER2PF_13_RT_OFFSET 29981
-#define QM_REG_PQOTHER2PF_14_RT_OFFSET 29982
-#define QM_REG_PQOTHER2PF_15_RT_OFFSET 29983
-#define QM_REG_RLGLBLPERIOD_0_RT_OFFSET 29984
-#define QM_REG_RLGLBLPERIOD_1_RT_OFFSET 29985
-#define QM_REG_RLGLBLPERIODTIMER_0_RT_OFFSET 29986
-#define QM_REG_RLGLBLPERIODTIMER_1_RT_OFFSET 29987
-#define QM_REG_RLGLBLPERIODSEL_0_RT_OFFSET 29988
-#define QM_REG_RLGLBLPERIODSEL_1_RT_OFFSET 29989
-#define QM_REG_RLGLBLPERIODSEL_2_RT_OFFSET 29990
-#define QM_REG_RLGLBLPERIODSEL_3_RT_OFFSET 29991
-#define QM_REG_RLGLBLPERIODSEL_4_RT_OFFSET 29992
-#define QM_REG_RLGLBLPERIODSEL_5_RT_OFFSET 29993
-#define QM_REG_RLGLBLPERIODSEL_6_RT_OFFSET 29994
-#define QM_REG_RLGLBLPERIODSEL_7_RT_OFFSET 29995
-#define QM_REG_RLGLBLINCVAL_RT_OFFSET 29996
-#define QM_REG_RLGLBLINCVAL_RT_SIZE 256
-#define QM_REG_RLGLBLUPPERBOUND_RT_OFFSET 30252
-#define QM_REG_RLGLBLUPPERBOUND_RT_SIZE 256
-#define QM_REG_RLGLBLCRD_RT_OFFSET 30508
-#define QM_REG_RLGLBLCRD_RT_SIZE 256
-#define QM_REG_RLGLBLENABLE_RT_OFFSET 30764
-#define QM_REG_RLPFPERIOD_RT_OFFSET 30765
-#define QM_REG_RLPFPERIODTIMER_RT_OFFSET 30766
-#define QM_REG_RLPFINCVAL_RT_OFFSET 30767
-#define QM_REG_RLPFINCVAL_RT_SIZE 16
-#define QM_REG_RLPFUPPERBOUND_RT_OFFSET 30783
-#define QM_REG_RLPFUPPERBOUND_RT_SIZE 16
-#define QM_REG_RLPFCRD_RT_OFFSET 30799
-#define QM_REG_RLPFCRD_RT_SIZE 16
-#define QM_REG_RLPFENABLE_RT_OFFSET 30815
-#define QM_REG_RLPFVOQENABLE_RT_OFFSET 30816
-#define QM_REG_WFQPFWEIGHT_RT_OFFSET 30817
-#define QM_REG_WFQPFWEIGHT_RT_SIZE 16
-#define QM_REG_WFQPFUPPERBOUND_RT_OFFSET 30833
-#define QM_REG_WFQPFUPPERBOUND_RT_SIZE 16
-#define QM_REG_WFQPFCRD_RT_OFFSET 30849
-#define QM_REG_WFQPFCRD_RT_SIZE 160
-#define QM_REG_WFQPFENABLE_RT_OFFSET 31009
-#define QM_REG_WFQVPENABLE_RT_OFFSET 31010
-#define QM_REG_BASEADDRTXPQ_RT_OFFSET 31011
-#define QM_REG_BASEADDRTXPQ_RT_SIZE 512
-#define QM_REG_TXPQMAP_RT_OFFSET 31523
-#define QM_REG_TXPQMAP_RT_SIZE 512
-#define QM_REG_WFQVPWEIGHT_RT_OFFSET 32035
-#define QM_REG_WFQVPWEIGHT_RT_SIZE 512
-#define QM_REG_WFQVPCRD_RT_OFFSET 32547
-#define QM_REG_WFQVPCRD_RT_SIZE 512
-#define QM_REG_WFQVPMAP_RT_OFFSET 33059
-#define QM_REG_WFQVPMAP_RT_SIZE 512
-#define QM_REG_WFQPFCRD_MSB_RT_OFFSET 33571
-#define QM_REG_WFQPFCRD_MSB_RT_SIZE 160
-#define NIG_REG_TAG_ETHERTYPE_0_RT_OFFSET 33731
-#define NIG_REG_OUTER_TAG_VALUE_LIST0_RT_OFFSET 33732
-#define NIG_REG_OUTER_TAG_VALUE_LIST1_RT_OFFSET 33733
-#define NIG_REG_OUTER_TAG_VALUE_LIST2_RT_OFFSET 33734
-#define NIG_REG_OUTER_TAG_VALUE_LIST3_RT_OFFSET 33735
-#define NIG_REG_OUTER_TAG_VALUE_MASK_RT_OFFSET 33736
-#define NIG_REG_LLH_FUNC_TAGMAC_CLS_TYPE_RT_OFFSET 33737
-#define NIG_REG_LLH_FUNC_TAG_EN_RT_OFFSET 33738
-#define NIG_REG_LLH_FUNC_TAG_EN_RT_SIZE 4
-#define NIG_REG_LLH_FUNC_TAG_HDR_SEL_RT_OFFSET 33742
-#define NIG_REG_LLH_FUNC_TAG_HDR_SEL_RT_SIZE 4
-#define NIG_REG_LLH_FUNC_TAG_VALUE_RT_OFFSET 33746
-#define NIG_REG_LLH_FUNC_TAG_VALUE_RT_SIZE 4
-#define NIG_REG_LLH_FUNC_NO_TAG_RT_OFFSET 33750
-#define NIG_REG_LLH_FUNC_FILTER_VALUE_RT_OFFSET 33751
-#define NIG_REG_LLH_FUNC_FILTER_VALUE_RT_SIZE 32
-#define NIG_REG_LLH_FUNC_FILTER_EN_RT_OFFSET 33783
-#define NIG_REG_LLH_FUNC_FILTER_EN_RT_SIZE 16
-#define NIG_REG_LLH_FUNC_FILTER_MODE_RT_OFFSET 33799
-#define NIG_REG_LLH_FUNC_FILTER_MODE_RT_SIZE 16
-#define NIG_REG_LLH_FUNC_FILTER_PROTOCOL_TYPE_RT_OFFSET 33815
-#define NIG_REG_LLH_FUNC_FILTER_PROTOCOL_TYPE_RT_SIZE 16
-#define NIG_REG_LLH_FUNC_FILTER_HDR_SEL_RT_OFFSET 33831
-#define NIG_REG_LLH_FUNC_FILTER_HDR_SEL_RT_SIZE 16
-#define NIG_REG_TX_EDPM_CTRL_RT_OFFSET 33847
-#define NIG_REG_ROCE_DUPLICATE_TO_HOST_RT_OFFSET 33848
-#define CDU_REG_CID_ADDR_PARAMS_RT_OFFSET 33849
-#define CDU_REG_SEGMENT0_PARAMS_RT_OFFSET 33850
-#define CDU_REG_SEGMENT1_PARAMS_RT_OFFSET 33851
-#define CDU_REG_PF_SEG0_TYPE_OFFSET_RT_OFFSET 33852
-#define CDU_REG_PF_SEG1_TYPE_OFFSET_RT_OFFSET 33853
-#define CDU_REG_PF_SEG2_TYPE_OFFSET_RT_OFFSET 33854
-#define CDU_REG_PF_SEG3_TYPE_OFFSET_RT_OFFSET 33855
-#define CDU_REG_PF_FL_SEG0_TYPE_OFFSET_RT_OFFSET 33856
-#define CDU_REG_PF_FL_SEG1_TYPE_OFFSET_RT_OFFSET 33857
-#define CDU_REG_PF_FL_SEG2_TYPE_OFFSET_RT_OFFSET 33858
-#define CDU_REG_PF_FL_SEG3_TYPE_OFFSET_RT_OFFSET 33859
-#define CDU_REG_VF_SEG_TYPE_OFFSET_RT_OFFSET 33860
-#define CDU_REG_VF_FL_SEG_TYPE_OFFSET_RT_OFFSET 33861
-#define PBF_REG_TAG_ETHERTYPE_0_RT_OFFSET 33862
-#define PBF_REG_BTB_SHARED_AREA_SIZE_RT_OFFSET 33863
-#define PBF_REG_YCMD_QS_NUM_LINES_VOQ0_RT_OFFSET 33864
-#define PBF_REG_BTB_GUARANTEED_VOQ0_RT_OFFSET 33865
-#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ0_RT_OFFSET 33866
-#define PBF_REG_YCMD_QS_NUM_LINES_VOQ1_RT_OFFSET 33867
-#define PBF_REG_BTB_GUARANTEED_VOQ1_RT_OFFSET 33868
-#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ1_RT_OFFSET 33869
-#define PBF_REG_YCMD_QS_NUM_LINES_VOQ2_RT_OFFSET 33870
-#define PBF_REG_BTB_GUARANTEED_VOQ2_RT_OFFSET 33871
-#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ2_RT_OFFSET 33872
-#define PBF_REG_YCMD_QS_NUM_LINES_VOQ3_RT_OFFSET 33873
-#define PBF_REG_BTB_GUARANTEED_VOQ3_RT_OFFSET 33874
-#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ3_RT_OFFSET 33875
-#define PBF_REG_YCMD_QS_NUM_LINES_VOQ4_RT_OFFSET 33876
-#define PBF_REG_BTB_GUARANTEED_VOQ4_RT_OFFSET 33877
-#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ4_RT_OFFSET 33878
-#define PBF_REG_YCMD_QS_NUM_LINES_VOQ5_RT_OFFSET 33879
-#define PBF_REG_BTB_GUARANTEED_VOQ5_RT_OFFSET 33880
-#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ5_RT_OFFSET 33881
-#define PBF_REG_YCMD_QS_NUM_LINES_VOQ6_RT_OFFSET 33882
-#define PBF_REG_BTB_GUARANTEED_VOQ6_RT_OFFSET 33883
-#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ6_RT_OFFSET 33884
-#define PBF_REG_YCMD_QS_NUM_LINES_VOQ7_RT_OFFSET 33885
-#define PBF_REG_BTB_GUARANTEED_VOQ7_RT_OFFSET 33886
-#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ7_RT_OFFSET 33887
-#define PBF_REG_YCMD_QS_NUM_LINES_VOQ8_RT_OFFSET 33888
-#define PBF_REG_BTB_GUARANTEED_VOQ8_RT_OFFSET 33889
-#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ8_RT_OFFSET 33890
-#define PBF_REG_YCMD_QS_NUM_LINES_VOQ9_RT_OFFSET 33891
-#define PBF_REG_BTB_GUARANTEED_VOQ9_RT_OFFSET 33892
-#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ9_RT_OFFSET 33893
-#define PBF_REG_YCMD_QS_NUM_LINES_VOQ10_RT_OFFSET 33894
-#define PBF_REG_BTB_GUARANTEED_VOQ10_RT_OFFSET 33895
-#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ10_RT_OFFSET 33896
-#define PBF_REG_YCMD_QS_NUM_LINES_VOQ11_RT_OFFSET 33897
-#define PBF_REG_BTB_GUARANTEED_VOQ11_RT_OFFSET 33898
-#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ11_RT_OFFSET 33899
-#define PBF_REG_YCMD_QS_NUM_LINES_VOQ12_RT_OFFSET 33900
-#define PBF_REG_BTB_GUARANTEED_VOQ12_RT_OFFSET 33901
-#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ12_RT_OFFSET 33902
-#define PBF_REG_YCMD_QS_NUM_LINES_VOQ13_RT_OFFSET 33903
-#define PBF_REG_BTB_GUARANTEED_VOQ13_RT_OFFSET 33904
-#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ13_RT_OFFSET 33905
-#define PBF_REG_YCMD_QS_NUM_LINES_VOQ14_RT_OFFSET 33906
-#define PBF_REG_BTB_GUARANTEED_VOQ14_RT_OFFSET 33907
-#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ14_RT_OFFSET 33908
-#define PBF_REG_YCMD_QS_NUM_LINES_VOQ15_RT_OFFSET 33909
-#define PBF_REG_BTB_GUARANTEED_VOQ15_RT_OFFSET 33910
-#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ15_RT_OFFSET 33911
-#define PBF_REG_YCMD_QS_NUM_LINES_VOQ16_RT_OFFSET 33912
-#define PBF_REG_BTB_GUARANTEED_VOQ16_RT_OFFSET 33913
-#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ16_RT_OFFSET 33914
-#define PBF_REG_YCMD_QS_NUM_LINES_VOQ17_RT_OFFSET 33915
-#define PBF_REG_BTB_GUARANTEED_VOQ17_RT_OFFSET 33916
-#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ17_RT_OFFSET 33917
-#define PBF_REG_YCMD_QS_NUM_LINES_VOQ18_RT_OFFSET 33918
-#define PBF_REG_BTB_GUARANTEED_VOQ18_RT_OFFSET 33919
-#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ18_RT_OFFSET 33920
-#define PBF_REG_YCMD_QS_NUM_LINES_VOQ19_RT_OFFSET 33921
-#define PBF_REG_BTB_GUARANTEED_VOQ19_RT_OFFSET 33922
-#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ19_RT_OFFSET 33923
-#define XCM_REG_CON_PHY_Q3_RT_OFFSET 33924
-
-#define RUNTIME_ARRAY_SIZE 33925
+#define DORQ_REG_PF_MAX_ICID_0_RT_OFFSET	0
+#define DORQ_REG_PF_MAX_ICID_1_RT_OFFSET	1
+#define DORQ_REG_PF_MAX_ICID_2_RT_OFFSET	2
+#define DORQ_REG_PF_MAX_ICID_3_RT_OFFSET	3
+#define DORQ_REG_PF_MAX_ICID_4_RT_OFFSET	4
+#define DORQ_REG_PF_MAX_ICID_5_RT_OFFSET	5
+#define DORQ_REG_PF_MAX_ICID_6_RT_OFFSET	6
+#define DORQ_REG_PF_MAX_ICID_7_RT_OFFSET	7
+#define DORQ_REG_VF_MAX_ICID_0_RT_OFFSET	8
+#define DORQ_REG_VF_MAX_ICID_1_RT_OFFSET	9
+#define DORQ_REG_VF_MAX_ICID_2_RT_OFFSET	10
+#define DORQ_REG_VF_MAX_ICID_3_RT_OFFSET	11
+#define DORQ_REG_VF_MAX_ICID_4_RT_OFFSET	12
+#define DORQ_REG_VF_MAX_ICID_5_RT_OFFSET	13
+#define DORQ_REG_VF_MAX_ICID_6_RT_OFFSET	14
+#define DORQ_REG_VF_MAX_ICID_7_RT_OFFSET	15
+#define DORQ_REG_PF_WAKE_ALL_RT_OFFSET	16
+#define DORQ_REG_TAG1_ETHERTYPE_RT_OFFSET	17
+#define IGU_REG_PF_CONFIGURATION_RT_OFFSET	18
+#define IGU_REG_VF_CONFIGURATION_RT_OFFSET	19
+#define IGU_REG_ATTN_MSG_ADDR_L_RT_OFFSET	20
+#define IGU_REG_ATTN_MSG_ADDR_H_RT_OFFSET	21
+#define IGU_REG_LEADING_EDGE_LATCH_RT_OFFSET	22
+#define IGU_REG_TRAILING_EDGE_LATCH_RT_OFFSET	23
+#define CAU_REG_CQE_AGG_UNIT_SIZE_RT_OFFSET	24
+#define CAU_REG_SB_VAR_MEMORY_RT_OFFSET	761
+#define CAU_REG_SB_VAR_MEMORY_RT_SIZE	736
+#define CAU_REG_SB_VAR_MEMORY_RT_OFFSET	761
+#define CAU_REG_SB_VAR_MEMORY_RT_SIZE	736
+#define CAU_REG_SB_ADDR_MEMORY_RT_OFFSET	1497
+#define CAU_REG_SB_ADDR_MEMORY_RT_SIZE	736
+#define CAU_REG_PI_MEMORY_RT_OFFSET	2233
+#define CAU_REG_PI_MEMORY_RT_SIZE	4416
+#define PRS_REG_SEARCH_RESP_INITIATOR_TYPE_RT_OFFSET	6649
+#define PRS_REG_TASK_ID_MAX_INITIATOR_PF_RT_OFFSET	6650
+#define PRS_REG_TASK_ID_MAX_INITIATOR_VF_RT_OFFSET	6651
+#define PRS_REG_TASK_ID_MAX_TARGET_PF_RT_OFFSET	6652
+#define PRS_REG_TASK_ID_MAX_TARGET_VF_RT_OFFSET	6653
+#define PRS_REG_SEARCH_TCP_RT_OFFSET	6654
+#define PRS_REG_SEARCH_FCOE_RT_OFFSET	6655
+#define PRS_REG_SEARCH_ROCE_RT_OFFSET	6656
+#define PRS_REG_ROCE_DEST_QP_MAX_VF_RT_OFFSET	6657
+#define PRS_REG_ROCE_DEST_QP_MAX_PF_RT_OFFSET	6658
+#define PRS_REG_SEARCH_OPENFLOW_RT_OFFSET	6659
+#define PRS_REG_SEARCH_NON_IP_AS_OPENFLOW_RT_OFFSET	6660
+#define PRS_REG_OPENFLOW_SUPPORT_ONLY_KNOWN_OVER_IP_RT_OFFSET	6661
+#define PRS_REG_OPENFLOW_SEARCH_KEY_MASK_RT_OFFSET	6662
+#define PRS_REG_TAG_ETHERTYPE_0_RT_OFFSET	6663
+#define PRS_REG_LIGHT_L2_ETHERTYPE_EN_RT_OFFSET	6664
+#define SRC_REG_FIRSTFREE_RT_OFFSET	6665
+#define SRC_REG_FIRSTFREE_RT_SIZE	2
+#define SRC_REG_LASTFREE_RT_OFFSET	6667
+#define SRC_REG_LASTFREE_RT_SIZE	2
+#define SRC_REG_COUNTFREE_RT_OFFSET	6669
+#define SRC_REG_NUMBER_HASH_BITS_RT_OFFSET	6670
+#define PSWRQ2_REG_CDUT_P_SIZE_RT_OFFSET	6671
+#define PSWRQ2_REG_CDUC_P_SIZE_RT_OFFSET	6672
+#define PSWRQ2_REG_TM_P_SIZE_RT_OFFSET	6673
+#define PSWRQ2_REG_QM_P_SIZE_RT_OFFSET	6674
+#define PSWRQ2_REG_SRC_P_SIZE_RT_OFFSET	6675
+#define PSWRQ2_REG_TSDM_P_SIZE_RT_OFFSET	6676
+#define PSWRQ2_REG_TM_FIRST_ILT_RT_OFFSET	6677
+#define PSWRQ2_REG_TM_LAST_ILT_RT_OFFSET	6678
+#define PSWRQ2_REG_QM_FIRST_ILT_RT_OFFSET	6679
+#define PSWRQ2_REG_QM_LAST_ILT_RT_OFFSET	6680
+#define PSWRQ2_REG_SRC_FIRST_ILT_RT_OFFSET	6681
+#define PSWRQ2_REG_SRC_LAST_ILT_RT_OFFSET	6682
+#define PSWRQ2_REG_CDUC_FIRST_ILT_RT_OFFSET	6683
+#define PSWRQ2_REG_CDUC_LAST_ILT_RT_OFFSET	6684
+#define PSWRQ2_REG_CDUT_FIRST_ILT_RT_OFFSET	6685
+#define PSWRQ2_REG_CDUT_LAST_ILT_RT_OFFSET	6686
+#define PSWRQ2_REG_TSDM_FIRST_ILT_RT_OFFSET	6687
+#define PSWRQ2_REG_TSDM_LAST_ILT_RT_OFFSET	6688
+#define PSWRQ2_REG_TM_NUMBER_OF_PF_BLOCKS_RT_OFFSET	6689
+#define PSWRQ2_REG_CDUT_NUMBER_OF_PF_BLOCKS_RT_OFFSET	6690
+#define PSWRQ2_REG_CDUC_NUMBER_OF_PF_BLOCKS_RT_OFFSET	6691
+#define PSWRQ2_REG_TM_VF_BLOCKS_RT_OFFSET	6692
+#define PSWRQ2_REG_CDUT_VF_BLOCKS_RT_OFFSET	6693
+#define PSWRQ2_REG_CDUC_VF_BLOCKS_RT_OFFSET	6694
+#define PSWRQ2_REG_TM_BLOCKS_FACTOR_RT_OFFSET	6695
+#define PSWRQ2_REG_CDUT_BLOCKS_FACTOR_RT_OFFSET	6696
+#define PSWRQ2_REG_CDUC_BLOCKS_FACTOR_RT_OFFSET	6697
+#define PSWRQ2_REG_VF_BASE_RT_OFFSET	6698
+#define PSWRQ2_REG_VF_LAST_ILT_RT_OFFSET	6699
+#define PSWRQ2_REG_WR_MBS0_RT_OFFSET	6700
+#define PSWRQ2_REG_RD_MBS0_RT_OFFSET	6701
+#define PSWRQ2_REG_DRAM_ALIGN_WR_RT_OFFSET	6702
+#define PSWRQ2_REG_DRAM_ALIGN_RD_RT_OFFSET	6703
+#define PSWRQ2_REG_ILT_MEMORY_RT_OFFSET	6704
+#define PSWRQ2_REG_ILT_MEMORY_RT_SIZE	22000
+#define PGLUE_REG_B_VF_BASE_RT_OFFSET	28704
+#define PGLUE_REG_B_MSDM_OFFSET_MASK_B_RT_OFFSET	28705
+#define PGLUE_REG_B_MSDM_VF_SHIFT_B_RT_OFFSET	28706
+#define PGLUE_REG_B_CACHE_LINE_SIZE_RT_OFFSET	28707
+#define PGLUE_REG_B_PF_BAR0_SIZE_RT_OFFSET	28708
+#define PGLUE_REG_B_PF_BAR1_SIZE_RT_OFFSET	28709
+#define PGLUE_REG_B_VF_BAR1_SIZE_RT_OFFSET	28710
+#define TM_REG_VF_ENABLE_CONN_RT_OFFSET	28711
+#define TM_REG_PF_ENABLE_CONN_RT_OFFSET	28712
+#define TM_REG_PF_ENABLE_TASK_RT_OFFSET	28713
+#define TM_REG_GROUP_SIZE_RESOLUTION_CONN_RT_OFFSET	28714
+#define TM_REG_GROUP_SIZE_RESOLUTION_TASK_RT_OFFSET	28715
+#define TM_REG_CONFIG_CONN_MEM_RT_OFFSET	28716
+#define TM_REG_CONFIG_CONN_MEM_RT_SIZE	416
+#define TM_REG_CONFIG_TASK_MEM_RT_OFFSET	29132
+#define TM_REG_CONFIG_TASK_MEM_RT_SIZE	512
+#define QM_REG_MAXPQSIZE_0_RT_OFFSET	29644
+#define QM_REG_MAXPQSIZE_1_RT_OFFSET	29645
+#define QM_REG_MAXPQSIZE_2_RT_OFFSET	29646
+#define QM_REG_MAXPQSIZETXSEL_0_RT_OFFSET	29647
+#define QM_REG_MAXPQSIZETXSEL_1_RT_OFFSET	29648
+#define QM_REG_MAXPQSIZETXSEL_2_RT_OFFSET	29649
+#define QM_REG_MAXPQSIZETXSEL_3_RT_OFFSET	29650
+#define QM_REG_MAXPQSIZETXSEL_4_RT_OFFSET	29651
+#define QM_REG_MAXPQSIZETXSEL_5_RT_OFFSET	29652
+#define QM_REG_MAXPQSIZETXSEL_6_RT_OFFSET	29653
+#define QM_REG_MAXPQSIZETXSEL_7_RT_OFFSET	29654
+#define QM_REG_MAXPQSIZETXSEL_8_RT_OFFSET	29655
+#define QM_REG_MAXPQSIZETXSEL_9_RT_OFFSET	29656
+#define QM_REG_MAXPQSIZETXSEL_10_RT_OFFSET	29657
+#define QM_REG_MAXPQSIZETXSEL_11_RT_OFFSET	29658
+#define QM_REG_MAXPQSIZETXSEL_12_RT_OFFSET	29659
+#define QM_REG_MAXPQSIZETXSEL_13_RT_OFFSET	29660
+#define QM_REG_MAXPQSIZETXSEL_14_RT_OFFSET	29661
+#define QM_REG_MAXPQSIZETXSEL_15_RT_OFFSET	29662
+#define QM_REG_MAXPQSIZETXSEL_16_RT_OFFSET	29663
+#define QM_REG_MAXPQSIZETXSEL_17_RT_OFFSET	29664
+#define QM_REG_MAXPQSIZETXSEL_18_RT_OFFSET	29665
+#define QM_REG_MAXPQSIZETXSEL_19_RT_OFFSET	29666
+#define QM_REG_MAXPQSIZETXSEL_20_RT_OFFSET	29667
+#define QM_REG_MAXPQSIZETXSEL_21_RT_OFFSET	29668
+#define QM_REG_MAXPQSIZETXSEL_22_RT_OFFSET	29669
+#define QM_REG_MAXPQSIZETXSEL_23_RT_OFFSET	29670
+#define QM_REG_MAXPQSIZETXSEL_24_RT_OFFSET	29671
+#define QM_REG_MAXPQSIZETXSEL_25_RT_OFFSET	29672
+#define QM_REG_MAXPQSIZETXSEL_26_RT_OFFSET	29673
+#define QM_REG_MAXPQSIZETXSEL_27_RT_OFFSET	29674
+#define QM_REG_MAXPQSIZETXSEL_28_RT_OFFSET	29675
+#define QM_REG_MAXPQSIZETXSEL_29_RT_OFFSET	29676
+#define QM_REG_MAXPQSIZETXSEL_30_RT_OFFSET	29677
+#define QM_REG_MAXPQSIZETXSEL_31_RT_OFFSET	29678
+#define QM_REG_MAXPQSIZETXSEL_32_RT_OFFSET	29679
+#define QM_REG_MAXPQSIZETXSEL_33_RT_OFFSET	29680
+#define QM_REG_MAXPQSIZETXSEL_34_RT_OFFSET	29681
+#define QM_REG_MAXPQSIZETXSEL_35_RT_OFFSET	29682
+#define QM_REG_MAXPQSIZETXSEL_36_RT_OFFSET	29683
+#define QM_REG_MAXPQSIZETXSEL_37_RT_OFFSET	29684
+#define QM_REG_MAXPQSIZETXSEL_38_RT_OFFSET	29685
+#define QM_REG_MAXPQSIZETXSEL_39_RT_OFFSET	29686
+#define QM_REG_MAXPQSIZETXSEL_40_RT_OFFSET	29687
+#define QM_REG_MAXPQSIZETXSEL_41_RT_OFFSET	29688
+#define QM_REG_MAXPQSIZETXSEL_42_RT_OFFSET	29689
+#define QM_REG_MAXPQSIZETXSEL_43_RT_OFFSET	29690
+#define QM_REG_MAXPQSIZETXSEL_44_RT_OFFSET	29691
+#define QM_REG_MAXPQSIZETXSEL_45_RT_OFFSET	29692
+#define QM_REG_MAXPQSIZETXSEL_46_RT_OFFSET	29693
+#define QM_REG_MAXPQSIZETXSEL_47_RT_OFFSET	29694
+#define QM_REG_MAXPQSIZETXSEL_48_RT_OFFSET	29695
+#define QM_REG_MAXPQSIZETXSEL_49_RT_OFFSET	29696
+#define QM_REG_MAXPQSIZETXSEL_50_RT_OFFSET	29697
+#define QM_REG_MAXPQSIZETXSEL_51_RT_OFFSET	29698
+#define QM_REG_MAXPQSIZETXSEL_52_RT_OFFSET	29699
+#define QM_REG_MAXPQSIZETXSEL_53_RT_OFFSET	29700
+#define QM_REG_MAXPQSIZETXSEL_54_RT_OFFSET	29701
+#define QM_REG_MAXPQSIZETXSEL_55_RT_OFFSET	29702
+#define QM_REG_MAXPQSIZETXSEL_56_RT_OFFSET	29703
+#define QM_REG_MAXPQSIZETXSEL_57_RT_OFFSET	29704
+#define QM_REG_MAXPQSIZETXSEL_58_RT_OFFSET	29705
+#define QM_REG_MAXPQSIZETXSEL_59_RT_OFFSET	29706
+#define QM_REG_MAXPQSIZETXSEL_60_RT_OFFSET	29707
+#define QM_REG_MAXPQSIZETXSEL_61_RT_OFFSET	29708
+#define QM_REG_MAXPQSIZETXSEL_62_RT_OFFSET	29709
+#define QM_REG_MAXPQSIZETXSEL_63_RT_OFFSET	29710
+#define QM_REG_BASEADDROTHERPQ_RT_OFFSET	29711
+#define QM_REG_BASEADDROTHERPQ_RT_SIZE	128
+#define QM_REG_VOQCRDLINE_RT_OFFSET	29839
+#define QM_REG_VOQCRDLINE_RT_SIZE	20
+#define QM_REG_VOQINITCRDLINE_RT_OFFSET	29859
+#define QM_REG_VOQINITCRDLINE_RT_SIZE	20
+#define QM_REG_AFULLQMBYPTHRPFWFQ_RT_OFFSET	29879
+#define QM_REG_AFULLQMBYPTHRVPWFQ_RT_OFFSET	29880
+#define QM_REG_AFULLQMBYPTHRPFRL_RT_OFFSET	29881
+#define QM_REG_AFULLQMBYPTHRGLBLRL_RT_OFFSET	29882
+#define QM_REG_AFULLOPRTNSTCCRDMASK_RT_OFFSET	29883
+#define QM_REG_WRROTHERPQGRP_0_RT_OFFSET	29884
+#define QM_REG_WRROTHERPQGRP_1_RT_OFFSET	29885
+#define QM_REG_WRROTHERPQGRP_2_RT_OFFSET	29886
+#define QM_REG_WRROTHERPQGRP_3_RT_OFFSET	29887
+#define QM_REG_WRROTHERPQGRP_4_RT_OFFSET	29888
+#define QM_REG_WRROTHERPQGRP_5_RT_OFFSET	29889
+#define QM_REG_WRROTHERPQGRP_6_RT_OFFSET	29890
+#define QM_REG_WRROTHERPQGRP_7_RT_OFFSET	29891
+#define QM_REG_WRROTHERPQGRP_8_RT_OFFSET	29892
+#define QM_REG_WRROTHERPQGRP_9_RT_OFFSET	29893
+#define QM_REG_WRROTHERPQGRP_10_RT_OFFSET	29894
+#define QM_REG_WRROTHERPQGRP_11_RT_OFFSET	29895
+#define QM_REG_WRROTHERPQGRP_12_RT_OFFSET	29896
+#define QM_REG_WRROTHERPQGRP_13_RT_OFFSET	29897
+#define QM_REG_WRROTHERPQGRP_14_RT_OFFSET	29898
+#define QM_REG_WRROTHERPQGRP_15_RT_OFFSET	29899
+#define QM_REG_WRROTHERGRPWEIGHT_0_RT_OFFSET	29900
+#define QM_REG_WRROTHERGRPWEIGHT_1_RT_OFFSET	29901
+#define QM_REG_WRROTHERGRPWEIGHT_2_RT_OFFSET	29902
+#define QM_REG_WRROTHERGRPWEIGHT_3_RT_OFFSET	29903
+#define QM_REG_WRRTXGRPWEIGHT_0_RT_OFFSET	29904
+#define QM_REG_WRRTXGRPWEIGHT_1_RT_OFFSET	29905
+#define QM_REG_PQTX2PF_0_RT_OFFSET	29906
+#define QM_REG_PQTX2PF_1_RT_OFFSET	29907
+#define QM_REG_PQTX2PF_2_RT_OFFSET	29908
+#define QM_REG_PQTX2PF_3_RT_OFFSET	29909
+#define QM_REG_PQTX2PF_4_RT_OFFSET	29910
+#define QM_REG_PQTX2PF_5_RT_OFFSET	29911
+#define QM_REG_PQTX2PF_6_RT_OFFSET	29912
+#define QM_REG_PQTX2PF_7_RT_OFFSET	29913
+#define QM_REG_PQTX2PF_8_RT_OFFSET	29914
+#define QM_REG_PQTX2PF_9_RT_OFFSET	29915
+#define QM_REG_PQTX2PF_10_RT_OFFSET	29916
+#define QM_REG_PQTX2PF_11_RT_OFFSET	29917
+#define QM_REG_PQTX2PF_12_RT_OFFSET	29918
+#define QM_REG_PQTX2PF_13_RT_OFFSET	29919
+#define QM_REG_PQTX2PF_14_RT_OFFSET	29920
+#define QM_REG_PQTX2PF_15_RT_OFFSET	29921
+#define QM_REG_PQTX2PF_16_RT_OFFSET	29922
+#define QM_REG_PQTX2PF_17_RT_OFFSET	29923
+#define QM_REG_PQTX2PF_18_RT_OFFSET	29924
+#define QM_REG_PQTX2PF_19_RT_OFFSET	29925
+#define QM_REG_PQTX2PF_20_RT_OFFSET	29926
+#define QM_REG_PQTX2PF_21_RT_OFFSET	29927
+#define QM_REG_PQTX2PF_22_RT_OFFSET	29928
+#define QM_REG_PQTX2PF_23_RT_OFFSET	29929
+#define QM_REG_PQTX2PF_24_RT_OFFSET	29930
+#define QM_REG_PQTX2PF_25_RT_OFFSET	29931
+#define QM_REG_PQTX2PF_26_RT_OFFSET	29932
+#define QM_REG_PQTX2PF_27_RT_OFFSET	29933
+#define QM_REG_PQTX2PF_28_RT_OFFSET	29934
+#define QM_REG_PQTX2PF_29_RT_OFFSET	29935
+#define QM_REG_PQTX2PF_30_RT_OFFSET	29936
+#define QM_REG_PQTX2PF_31_RT_OFFSET	29937
+#define QM_REG_PQTX2PF_32_RT_OFFSET	29938
+#define QM_REG_PQTX2PF_33_RT_OFFSET	29939
+#define QM_REG_PQTX2PF_34_RT_OFFSET	29940
+#define QM_REG_PQTX2PF_35_RT_OFFSET	29941
+#define QM_REG_PQTX2PF_36_RT_OFFSET	29942
+#define QM_REG_PQTX2PF_37_RT_OFFSET	29943
+#define QM_REG_PQTX2PF_38_RT_OFFSET	29944
+#define QM_REG_PQTX2PF_39_RT_OFFSET	29945
+#define QM_REG_PQTX2PF_40_RT_OFFSET	29946
+#define QM_REG_PQTX2PF_41_RT_OFFSET	29947
+#define QM_REG_PQTX2PF_42_RT_OFFSET	29948
+#define QM_REG_PQTX2PF_43_RT_OFFSET	29949
+#define QM_REG_PQTX2PF_44_RT_OFFSET	29950
+#define QM_REG_PQTX2PF_45_RT_OFFSET	29951
+#define QM_REG_PQTX2PF_46_RT_OFFSET	29952
+#define QM_REG_PQTX2PF_47_RT_OFFSET	29953
+#define QM_REG_PQTX2PF_48_RT_OFFSET	29954
+#define QM_REG_PQTX2PF_49_RT_OFFSET	29955
+#define QM_REG_PQTX2PF_50_RT_OFFSET	29956
+#define QM_REG_PQTX2PF_51_RT_OFFSET	29957
+#define QM_REG_PQTX2PF_52_RT_OFFSET	29958
+#define QM_REG_PQTX2PF_53_RT_OFFSET	29959
+#define QM_REG_PQTX2PF_54_RT_OFFSET	29960
+#define QM_REG_PQTX2PF_55_RT_OFFSET	29961
+#define QM_REG_PQTX2PF_56_RT_OFFSET	29962
+#define QM_REG_PQTX2PF_57_RT_OFFSET	29963
+#define QM_REG_PQTX2PF_58_RT_OFFSET	29964
+#define QM_REG_PQTX2PF_59_RT_OFFSET	29965
+#define QM_REG_PQTX2PF_60_RT_OFFSET	29966
+#define QM_REG_PQTX2PF_61_RT_OFFSET	29967
+#define QM_REG_PQTX2PF_62_RT_OFFSET	29968
+#define QM_REG_PQTX2PF_63_RT_OFFSET	29969
+#define QM_REG_PQOTHER2PF_0_RT_OFFSET	29970
+#define QM_REG_PQOTHER2PF_1_RT_OFFSET	29971
+#define QM_REG_PQOTHER2PF_2_RT_OFFSET	29972
+#define QM_REG_PQOTHER2PF_3_RT_OFFSET	29973
+#define QM_REG_PQOTHER2PF_4_RT_OFFSET	29974
+#define QM_REG_PQOTHER2PF_5_RT_OFFSET	29975
+#define QM_REG_PQOTHER2PF_6_RT_OFFSET	29976
+#define QM_REG_PQOTHER2PF_7_RT_OFFSET	29977
+#define QM_REG_PQOTHER2PF_8_RT_OFFSET	29978
+#define QM_REG_PQOTHER2PF_9_RT_OFFSET	29979
+#define QM_REG_PQOTHER2PF_10_RT_OFFSET	29980
+#define QM_REG_PQOTHER2PF_11_RT_OFFSET	29981
+#define QM_REG_PQOTHER2PF_12_RT_OFFSET	29982
+#define QM_REG_PQOTHER2PF_13_RT_OFFSET	29983
+#define QM_REG_PQOTHER2PF_14_RT_OFFSET	29984
+#define QM_REG_PQOTHER2PF_15_RT_OFFSET	29985
+#define QM_REG_RLGLBLPERIOD_0_RT_OFFSET	29986
+#define QM_REG_RLGLBLPERIOD_1_RT_OFFSET	29987
+#define QM_REG_RLGLBLPERIODTIMER_0_RT_OFFSET	29988
+#define QM_REG_RLGLBLPERIODTIMER_1_RT_OFFSET	29989
+#define QM_REG_RLGLBLPERIODSEL_0_RT_OFFSET	29990
+#define QM_REG_RLGLBLPERIODSEL_1_RT_OFFSET	29991
+#define QM_REG_RLGLBLPERIODSEL_2_RT_OFFSET	29992
+#define QM_REG_RLGLBLPERIODSEL_3_RT_OFFSET	29993
+#define QM_REG_RLGLBLPERIODSEL_4_RT_OFFSET	29994
+#define QM_REG_RLGLBLPERIODSEL_5_RT_OFFSET	29995
+#define QM_REG_RLGLBLPERIODSEL_6_RT_OFFSET	29996
+#define QM_REG_RLGLBLPERIODSEL_7_RT_OFFSET	29997
+#define QM_REG_RLGLBLINCVAL_RT_OFFSET	29998
+#define QM_REG_RLGLBLINCVAL_RT_SIZE	256
+#define QM_REG_RLGLBLUPPERBOUND_RT_OFFSET	30254
+#define QM_REG_RLGLBLUPPERBOUND_RT_SIZE	256
+#define QM_REG_RLGLBLCRD_RT_OFFSET	30510
+#define QM_REG_RLGLBLCRD_RT_SIZE	256
+#define QM_REG_RLGLBLENABLE_RT_OFFSET	30766
+#define QM_REG_RLPFPERIOD_RT_OFFSET	30767
+#define QM_REG_RLPFPERIODTIMER_RT_OFFSET	30768
+#define QM_REG_RLPFINCVAL_RT_OFFSET	30769
+#define QM_REG_RLPFINCVAL_RT_SIZE	16
+#define QM_REG_RLPFUPPERBOUND_RT_OFFSET	30785
+#define QM_REG_RLPFUPPERBOUND_RT_SIZE	16
+#define QM_REG_RLPFCRD_RT_OFFSET	30801
+#define QM_REG_RLPFCRD_RT_SIZE	16
+#define QM_REG_RLPFENABLE_RT_OFFSET	30817
+#define QM_REG_RLPFVOQENABLE_RT_OFFSET	30818
+#define QM_REG_WFQPFWEIGHT_RT_OFFSET	30819
+#define QM_REG_WFQPFWEIGHT_RT_SIZE	16
+#define QM_REG_WFQPFUPPERBOUND_RT_OFFSET	30835
+#define QM_REG_WFQPFUPPERBOUND_RT_SIZE	16
+#define QM_REG_WFQPFCRD_RT_OFFSET	30851
+#define QM_REG_WFQPFCRD_RT_SIZE	160
+#define QM_REG_WFQPFENABLE_RT_OFFSET	31011
+#define QM_REG_WFQVPENABLE_RT_OFFSET	31012
+#define QM_REG_BASEADDRTXPQ_RT_OFFSET	31013
+#define QM_REG_BASEADDRTXPQ_RT_SIZE	512
+#define QM_REG_TXPQMAP_RT_OFFSET	31525
+#define QM_REG_TXPQMAP_RT_SIZE	512
+#define QM_REG_WFQVPWEIGHT_RT_OFFSET	32037
+#define QM_REG_WFQVPWEIGHT_RT_SIZE	512
+#define QM_REG_WFQVPCRD_RT_OFFSET	32549
+#define QM_REG_WFQVPCRD_RT_SIZE	512
+#define QM_REG_WFQVPMAP_RT_OFFSET	33061
+#define QM_REG_WFQVPMAP_RT_SIZE	512
+#define QM_REG_WFQPFCRD_MSB_RT_OFFSET	33573
+#define QM_REG_WFQPFCRD_MSB_RT_SIZE	160
+#define NIG_REG_TAG_ETHERTYPE_0_RT_OFFSET	33733
+#define NIG_REG_OUTER_TAG_VALUE_LIST0_RT_OFFSET	33734
+#define NIG_REG_OUTER_TAG_VALUE_LIST1_RT_OFFSET	33735
+#define NIG_REG_OUTER_TAG_VALUE_LIST2_RT_OFFSET	33736
+#define NIG_REG_OUTER_TAG_VALUE_LIST3_RT_OFFSET	33737
+#define NIG_REG_OUTER_TAG_VALUE_MASK_RT_OFFSET	33738
+#define NIG_REG_LLH_FUNC_TAGMAC_CLS_TYPE_RT_OFFSET	33739
+#define NIG_REG_LLH_FUNC_TAG_EN_RT_OFFSET	33740
+#define NIG_REG_LLH_FUNC_TAG_EN_RT_SIZE	4
+#define NIG_REG_LLH_FUNC_TAG_HDR_SEL_RT_OFFSET	33744
+#define NIG_REG_LLH_FUNC_TAG_HDR_SEL_RT_SIZE	4
+#define NIG_REG_LLH_FUNC_TAG_VALUE_RT_OFFSET	33748
+#define NIG_REG_LLH_FUNC_TAG_VALUE_RT_SIZE	4
+#define NIG_REG_LLH_FUNC_NO_TAG_RT_OFFSET	33752
+#define NIG_REG_LLH_FUNC_FILTER_VALUE_RT_OFFSET	33753
+#define NIG_REG_LLH_FUNC_FILTER_VALUE_RT_SIZE	32
+#define NIG_REG_LLH_FUNC_FILTER_EN_RT_OFFSET	33785
+#define NIG_REG_LLH_FUNC_FILTER_EN_RT_SIZE	16
+#define NIG_REG_LLH_FUNC_FILTER_MODE_RT_OFFSET	33801
+#define NIG_REG_LLH_FUNC_FILTER_MODE_RT_SIZE	16
+#define NIG_REG_LLH_FUNC_FILTER_PROTOCOL_TYPE_RT_OFFSET	33817
+#define NIG_REG_LLH_FUNC_FILTER_PROTOCOL_TYPE_RT_SIZE	16
+#define NIG_REG_LLH_FUNC_FILTER_HDR_SEL_RT_OFFSET	33833
+#define NIG_REG_LLH_FUNC_FILTER_HDR_SEL_RT_SIZE	16
+#define NIG_REG_TX_EDPM_CTRL_RT_OFFSET	33849
+#define NIG_REG_ROCE_DUPLICATE_TO_HOST_RT_OFFSET	33850
+#define CDU_REG_CID_ADDR_PARAMS_RT_OFFSET	33851
+#define CDU_REG_SEGMENT0_PARAMS_RT_OFFSET	33852
+#define CDU_REG_SEGMENT1_PARAMS_RT_OFFSET	33853
+#define CDU_REG_PF_SEG0_TYPE_OFFSET_RT_OFFSET	33854
+#define CDU_REG_PF_SEG1_TYPE_OFFSET_RT_OFFSET	33855
+#define CDU_REG_PF_SEG2_TYPE_OFFSET_RT_OFFSET	33856
+#define CDU_REG_PF_SEG3_TYPE_OFFSET_RT_OFFSET	33857
+#define CDU_REG_PF_FL_SEG0_TYPE_OFFSET_RT_OFFSET	33858
+#define CDU_REG_PF_FL_SEG1_TYPE_OFFSET_RT_OFFSET	33859
+#define CDU_REG_PF_FL_SEG2_TYPE_OFFSET_RT_OFFSET	33860
+#define CDU_REG_PF_FL_SEG3_TYPE_OFFSET_RT_OFFSET	33861
+#define CDU_REG_VF_SEG_TYPE_OFFSET_RT_OFFSET	33862
+#define CDU_REG_VF_FL_SEG_TYPE_OFFSET_RT_OFFSET	33863
+#define PBF_REG_TAG_ETHERTYPE_0_RT_OFFSET	33864
+#define PBF_REG_BTB_SHARED_AREA_SIZE_RT_OFFSET	33865
+#define PBF_REG_YCMD_QS_NUM_LINES_VOQ0_RT_OFFSET	33866
+#define PBF_REG_BTB_GUARANTEED_VOQ0_RT_OFFSET	33867
+#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ0_RT_OFFSET	33868
+#define PBF_REG_YCMD_QS_NUM_LINES_VOQ1_RT_OFFSET	33869
+#define PBF_REG_BTB_GUARANTEED_VOQ1_RT_OFFSET	33870
+#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ1_RT_OFFSET	33871
+#define PBF_REG_YCMD_QS_NUM_LINES_VOQ2_RT_OFFSET	33872
+#define PBF_REG_BTB_GUARANTEED_VOQ2_RT_OFFSET	33873
+#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ2_RT_OFFSET	33874
+#define PBF_REG_YCMD_QS_NUM_LINES_VOQ3_RT_OFFSET	33875
+#define PBF_REG_BTB_GUARANTEED_VOQ3_RT_OFFSET	33876
+#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ3_RT_OFFSET	33877
+#define PBF_REG_YCMD_QS_NUM_LINES_VOQ4_RT_OFFSET	33878
+#define PBF_REG_BTB_GUARANTEED_VOQ4_RT_OFFSET	33879
+#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ4_RT_OFFSET	33880
+#define PBF_REG_YCMD_QS_NUM_LINES_VOQ5_RT_OFFSET	33881
+#define PBF_REG_BTB_GUARANTEED_VOQ5_RT_OFFSET	33882
+#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ5_RT_OFFSET	33883
+#define PBF_REG_YCMD_QS_NUM_LINES_VOQ6_RT_OFFSET	33884
+#define PBF_REG_BTB_GUARANTEED_VOQ6_RT_OFFSET	33885
+#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ6_RT_OFFSET	33886
+#define PBF_REG_YCMD_QS_NUM_LINES_VOQ7_RT_OFFSET	33887
+#define PBF_REG_BTB_GUARANTEED_VOQ7_RT_OFFSET	33888
+#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ7_RT_OFFSET	33889
+#define PBF_REG_YCMD_QS_NUM_LINES_VOQ8_RT_OFFSET	33890
+#define PBF_REG_BTB_GUARANTEED_VOQ8_RT_OFFSET	33891
+#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ8_RT_OFFSET	33892
+#define PBF_REG_YCMD_QS_NUM_LINES_VOQ9_RT_OFFSET	33893
+#define PBF_REG_BTB_GUARANTEED_VOQ9_RT_OFFSET	33894
+#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ9_RT_OFFSET	33895
+#define PBF_REG_YCMD_QS_NUM_LINES_VOQ10_RT_OFFSET	33896
+#define PBF_REG_BTB_GUARANTEED_VOQ10_RT_OFFSET	33897
+#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ10_RT_OFFSET	33898
+#define PBF_REG_YCMD_QS_NUM_LINES_VOQ11_RT_OFFSET	33899
+#define PBF_REG_BTB_GUARANTEED_VOQ11_RT_OFFSET	33900
+#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ11_RT_OFFSET	33901
+#define PBF_REG_YCMD_QS_NUM_LINES_VOQ12_RT_OFFSET	33902
+#define PBF_REG_BTB_GUARANTEED_VOQ12_RT_OFFSET	33903
+#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ12_RT_OFFSET	33904
+#define PBF_REG_YCMD_QS_NUM_LINES_VOQ13_RT_OFFSET	33905
+#define PBF_REG_BTB_GUARANTEED_VOQ13_RT_OFFSET	33906
+#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ13_RT_OFFSET	33907
+#define PBF_REG_YCMD_QS_NUM_LINES_VOQ14_RT_OFFSET	33908
+#define PBF_REG_BTB_GUARANTEED_VOQ14_RT_OFFSET	33909
+#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ14_RT_OFFSET	33910
+#define PBF_REG_YCMD_QS_NUM_LINES_VOQ15_RT_OFFSET	33911
+#define PBF_REG_BTB_GUARANTEED_VOQ15_RT_OFFSET	33912
+#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ15_RT_OFFSET	33913
+#define PBF_REG_YCMD_QS_NUM_LINES_VOQ16_RT_OFFSET	33914
+#define PBF_REG_BTB_GUARANTEED_VOQ16_RT_OFFSET	33915
+#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ16_RT_OFFSET	33916
+#define PBF_REG_YCMD_QS_NUM_LINES_VOQ17_RT_OFFSET	33917
+#define PBF_REG_BTB_GUARANTEED_VOQ17_RT_OFFSET	33918
+#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ17_RT_OFFSET	33919
+#define PBF_REG_YCMD_QS_NUM_LINES_VOQ18_RT_OFFSET	33920
+#define PBF_REG_BTB_GUARANTEED_VOQ18_RT_OFFSET	33921
+#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ18_RT_OFFSET	33922
+#define PBF_REG_YCMD_QS_NUM_LINES_VOQ19_RT_OFFSET	33923
+#define PBF_REG_BTB_GUARANTEED_VOQ19_RT_OFFSET	33924
+#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ19_RT_OFFSET	33925
+#define XCM_REG_CON_PHY_Q3_RT_OFFSET	33926
+
+#define RUNTIME_ARRAY_SIZE 33927
 
 /* The eth storm context for the Tstorm */
 struct tstorm_eth_conn_st_ctx {
@@ -3201,7 +4469,31 @@ struct eth_conn_context {
 	struct mstorm_eth_conn_st_ctx mstorm_st_context;
 };
 
-/* opcodes for the event ring */
+enum eth_error_code {
+	ETH_OK = 0x00,
+	ETH_FILTERS_MAC_ADD_FAIL_FULL,
+	ETH_FILTERS_MAC_ADD_FAIL_FULL_MTT2,
+	ETH_FILTERS_MAC_ADD_FAIL_DUP_MTT2,
+	ETH_FILTERS_MAC_ADD_FAIL_DUP_STT2,
+	ETH_FILTERS_MAC_DEL_FAIL_NOF,
+	ETH_FILTERS_MAC_DEL_FAIL_NOF_MTT2,
+	ETH_FILTERS_MAC_DEL_FAIL_NOF_STT2,
+	ETH_FILTERS_MAC_ADD_FAIL_ZERO_MAC,
+	ETH_FILTERS_VLAN_ADD_FAIL_FULL,
+	ETH_FILTERS_VLAN_ADD_FAIL_DUP,
+	ETH_FILTERS_VLAN_DEL_FAIL_NOF,
+	ETH_FILTERS_VLAN_DEL_FAIL_NOF_TT1,
+	ETH_FILTERS_PAIR_ADD_FAIL_DUP,
+	ETH_FILTERS_PAIR_ADD_FAIL_FULL,
+	ETH_FILTERS_PAIR_ADD_FAIL_FULL_MAC,
+	ETH_FILTERS_PAIR_DEL_FAIL_NOF,
+	ETH_FILTERS_PAIR_DEL_FAIL_NOF_TT1,
+	ETH_FILTERS_PAIR_ADD_FAIL_ZERO_MAC,
+	ETH_FILTERS_VNI_ADD_FAIL_FULL,
+	ETH_FILTERS_VNI_ADD_FAIL_DUP,
+	MAX_ETH_ERROR_CODE
+};
+
 enum eth_event_opcode {
 	ETH_EVENT_UNUSED,
 	ETH_EVENT_VPORT_START,
@@ -3269,7 +4561,13 @@ enum eth_filter_type {
 	MAX_ETH_FILTER_TYPE
 };
 
-/* Ethernet Ramrod Command IDs */
+enum eth_ipv4_frag_type {
+	ETH_IPV4_NOT_FRAG,
+	ETH_IPV4_FIRST_FRAG,
+	ETH_IPV4_NON_FIRST_FRAG,
+	MAX_ETH_IPV4_FRAG_TYPE
+};
+
 enum eth_ramrod_cmd_id {
 	ETH_RAMROD_UNUSED,
 	ETH_RAMROD_VPORT_START,
@@ -3451,8 +4749,8 @@ struct rx_queue_start_ramrod_data {
 	u8 toggle_val;
 
 	u8 vf_rx_prod_index;
-
-	u8 reserved[6];
+	u8 vf_rx_prod_use_zone_a;
+	u8 reserved[5];
 	__le16 reserved1;
 	struct regpair cqe_pbl_addr;
 	struct regpair bd_base;
@@ -3526,10 +4824,11 @@ struct tx_queue_start_ramrod_data {
 	__le16 pxp_st_index;
 	__le16 comp_agg_size;
 	__le16 queue_zone_id;
-	__le16 test_dup_count;
+	__le16 reserved2;
 	__le16 pbl_size;
 	__le16 tx_queue_id;
-
+	__le16 same_as_last_id;
+	__le16 reserved[3];
 	struct regpair pbl_base_addr;
 	struct regpair bd_cons_address;
 };
@@ -4926,8 +6225,8 @@ struct roce_create_qp_resp_ramrod_data {
 #define ROCE_CREATE_QP_RESP_RAMROD_DATA_SRQ_FLG_SHIFT             5
 #define ROCE_CREATE_QP_RESP_RAMROD_DATA_E2E_FLOW_CONTROL_EN_MASK  0x1
 #define ROCE_CREATE_QP_RESP_RAMROD_DATA_E2E_FLOW_CONTROL_EN_SHIFT 6
-#define ROCE_CREATE_QP_RESP_RAMROD_DATA_RESERVED0_MASK            0x1
-#define ROCE_CREATE_QP_RESP_RAMROD_DATA_RESERVED0_SHIFT           7
+#define ROCE_CREATE_QP_RESP_RAMROD_DATA_RESERVED_KEY_EN_MASK	0x1
+#define ROCE_CREATE_QP_RESP_RAMROD_DATA_RESERVED_KEY_EN_SHIFT	7
 #define ROCE_CREATE_QP_RESP_RAMROD_DATA_PRI_MASK                  0x7
 #define ROCE_CREATE_QP_RESP_RAMROD_DATA_PRI_SHIFT                 8
 #define ROCE_CREATE_QP_RESP_RAMROD_DATA_MIN_RNR_NAK_TIMER_MASK    0x1F
@@ -4988,6 +6287,10 @@ enum roce_event_opcode {
 	MAX_ROCE_EVENT_OPCODE
 };
 
+struct roce_init_func_ramrod_data {
+	struct rdma_init_func_ramrod_data rdma;
+};
+
 struct roce_modify_qp_req_ramrod_data {
 	__le16 flags;
 #define ROCE_MODIFY_QP_REQ_RAMROD_DATA_MOVE_TO_ERR_FLG_MASK      0x1
@@ -6639,6 +7942,35 @@ struct ystorm_iscsi_conn_ag_ctx {
 	__le32 reg2;
 	__le32 reg3;
 };
+
+#define MFW_TRACE_SIGNATURE     0x25071946
+
+/* The trace in the buffer */
+#define MFW_TRACE_EVENTID_MASK          0x00ffff
+#define MFW_TRACE_PRM_SIZE_MASK         0x0f0000
+#define MFW_TRACE_PRM_SIZE_SHIFT        16
+#define MFW_TRACE_ENTRY_SIZE            3
+
+struct mcp_trace {
+	u32 signature;		/* Help to identify that the trace is valid */
+	u32 size;		/* the size of the trace buffer in bytes */
+	u32 curr_level;		/* 2 - all will be written to the buffer
+				 * 1 - debug trace will not be written
+				 * 0 - just errors will be written to the buffer
+				 */
+	u32 modules_mask[2];	/* a bit per module, 1 means write it, 0 means
+				 * mask it.
+				 */
+
+	/* Warning: the following pointers are assumed to be 32bits as they are
+	 * used only in the MFW.
+	 */
+	u32 trace_prod; /* The next trace will be written to this offset */
+	u32 trace_oldest; /* The oldest valid trace starts at this offset
+			   * (usually very close after the current producer).
+			   */
+};
+
 #define VF_MAX_STATIC 192
 
 #define MCP_GLOB_PATH_MAX	2
@@ -6646,6 +7978,7 @@ struct ystorm_iscsi_conn_ag_ctx {
 #define MCP_GLOB_PORT_MAX	4
 #define MCP_GLOB_FUNC_MAX	16
 
+typedef u32 offsize_t;		/* In DWORDS !!! */
 /* Offset from the beginning of the MCP scratchpad */
 #define OFFSIZE_OFFSET_SHIFT	0
 #define OFFSIZE_OFFSET_MASK	0x0000ffff
@@ -7236,8 +8569,19 @@ struct public_drv_mb {
 #define DRV_MSG_CODE_NIG_DRAIN			0x30000000
 #define DRV_MSG_CODE_VF_DISABLED_DONE		0xc0000000
 #define DRV_MSG_CODE_CFG_VF_MSIX		0xc0010000
+#define DRV_MSG_CODE_NVM_GET_FILE_ATT		0x00030000
+#define DRV_MSG_CODE_NVM_READ_NVRAM		0x00050000
 #define DRV_MSG_CODE_MCP_RESET			0x00090000
 #define DRV_MSG_CODE_SET_VERSION		0x000f0000
+#define DRV_MSG_CODE_MCP_HALT                   0x00100000
+
+#define DRV_MSG_CODE_GET_STATS                  0x00130000
+#define DRV_MSG_CODE_STATS_TYPE_LAN             1
+#define DRV_MSG_CODE_STATS_TYPE_FCOE            2
+#define DRV_MSG_CODE_STATS_TYPE_ISCSI           3
+#define DRV_MSG_CODE_STATS_TYPE_RDMA            4
+
+#define DRV_MSG_CODE_MASK_PARITIES              0x001a0000
 
 #define DRV_MSG_CODE_BIST_TEST			0x001e0000
 #define DRV_MSG_CODE_SET_LED_MODE		0x00200000
@@ -7248,6 +8592,9 @@ struct public_drv_mb {
 #define DRV_MB_PARAM_UNLOAD_WOL_MCP		0x00000001
 #define DRV_MB_PARAM_DCBX_NOTIFY_MASK		0x000000FF
 #define DRV_MB_PARAM_DCBX_NOTIFY_SHIFT		3
+
+#define DRV_MB_PARAM_NVM_LEN_SHIFT		24
+
 #define DRV_MB_PARAM_CFG_VF_MSIX_VF_ID_SHIFT	0
 #define DRV_MB_PARAM_CFG_VF_MSIX_VF_ID_MASK	0x000000FF
 #define DRV_MB_PARAM_CFG_VF_MSIX_SB_NUM_SHIFT	8
@@ -7285,6 +8632,8 @@ struct public_drv_mb {
 #define FW_MSG_CODE_DRV_UNLOAD_FUNCTION		0x20130000
 #define FW_MSG_CODE_DRV_UNLOAD_DONE		0x21100000
 #define FW_MSG_CODE_DRV_CFG_VF_MSIX_DONE	0xb0010000
+
+#define FW_MSG_CODE_NVM_OK			0x00010000
 #define FW_MSG_CODE_OK				0x00160000
 
 #define FW_MSG_SEQ_NUMBER_MASK			0x0000ffff
@@ -7315,10 +8664,10 @@ enum MFW_DRV_MSG_TYPE {
 	MFW_DRV_MSG_RESERVED4,
 	MFW_DRV_MSG_BW_UPDATE,
 	MFW_DRV_MSG_BW_UPDATE5,
-	MFW_DRV_MSG_BW_UPDATE6,
-	MFW_DRV_MSG_BW_UPDATE7,
-	MFW_DRV_MSG_BW_UPDATE8,
-	MFW_DRV_MSG_BW_UPDATE9,
+	MFW_DRV_MSG_GET_LAN_STATS,
+	MFW_DRV_MSG_GET_FCOE_STATS,
+	MFW_DRV_MSG_GET_ISCSI_STATS,
+	MFW_DRV_MSG_GET_RDMA_STATS,
 	MFW_DRV_MSG_BW_UPDATE10,
 	MFW_DRV_MSG_TRANSCEIVER_STATE_CHANGE,
 	MFW_DRV_MSG_BW_UPDATE11,
@@ -7521,4 +8870,101 @@ struct nvm_cfg1 {
 	struct nvm_cfg1_port port[MCP_GLOB_PORT_MAX];
 	struct nvm_cfg1_func func[MCP_GLOB_FUNC_MAX];
 };
+
+enum spad_sections {
+	SPAD_SECTION_TRACE,
+	SPAD_SECTION_NVM_CFG,
+	SPAD_SECTION_PUBLIC,
+	SPAD_SECTION_PRIVATE,
+	SPAD_SECTION_MAX
+};
+
+#define MCP_TRACE_SIZE          2048	/* 2kb */
+
+/* This section is located at a fixed location in the beginning of the
+ * scratchpad, to ensure that the MCP trace is not run over during MFW upgrade.
+ * All the rest of data has a floating location which differs from version to
+ * version, and is pointed by the mcp_meta_data below.
+ * Moreover, the spad_layout section is part of the MFW firmware, and is loaded
+ * with it from nvram in order to clear this portion.
+ */
+struct static_init {
+	u32 num_sections;
+	offsize_t sections[SPAD_SECTION_MAX];
+#define SECTION(_sec_) (*((offsize_t *)(STRUCT_OFFSET(sections[_sec_]))))
+
+	struct mcp_trace trace;
+#define MCP_TRACE_P ((struct mcp_trace *)(STRUCT_OFFSET(trace)))
+	u8 trace_buffer[MCP_TRACE_SIZE];
+#define MCP_TRACE_BUF ((u8 *)(STRUCT_OFFSET(trace_buffer)))
+	/* running_mfw has the same definition as in nvm_map.h.
+	 * This bit indicate both the running dir, and the running bundle.
+	 * It is set once when the LIM is loaded.
+	 */
+	u32 running_mfw;
+#define RUNNING_MFW (*((u32 *)(STRUCT_OFFSET(running_mfw))))
+	u32 build_time;
+#define MFW_BUILD_TIME (*((u32 *)(STRUCT_OFFSET(build_time))))
+	u32 reset_type;
+#define RESET_TYPE (*((u32 *)(STRUCT_OFFSET(reset_type))))
+	u32 mfw_secure_mode;
+#define MFW_SECURE_MODE (*((u32 *)(STRUCT_OFFSET(mfw_secure_mode))))
+	u16 pme_status_pf_bitmap;
+#define PME_STATUS_PF_BITMAP (*((u16 *)(STRUCT_OFFSET(pme_status_pf_bitmap))))
+	u16 pme_enable_pf_bitmap;
+#define PME_ENABLE_PF_BITMAP (*((u16 *)(STRUCT_OFFSET(pme_enable_pf_bitmap))))
+	u32 mim_nvm_addr;
+	u32 mim_start_addr;
+	u32 ah_pcie_link_params;
+#define AH_PCIE_LINK_PARAMS_LINK_SPEED_MASK     (0x000000ff)
+#define AH_PCIE_LINK_PARAMS_LINK_SPEED_SHIFT    (0)
+#define AH_PCIE_LINK_PARAMS_LINK_WIDTH_MASK     (0x0000ff00)
+#define AH_PCIE_LINK_PARAMS_LINK_WIDTH_SHIFT    (8)
+#define AH_PCIE_LINK_PARAMS_ASPM_MODE_MASK      (0x00ff0000)
+#define AH_PCIE_LINK_PARAMS_ASPM_MODE_SHIFT     (16)
+#define AH_PCIE_LINK_PARAMS_ASPM_CAP_MASK       (0xff000000)
+#define AH_PCIE_LINK_PARAMS_ASPM_CAP_SHIFT      (24)
+#define AH_PCIE_LINK_PARAMS (*((u32 *)(STRUCT_OFFSET(ah_pcie_link_params))))
+
+	u32 rsrv_persist[5];	/* Persist reserved for MFW upgrades */
+};
+
+enum nvm_image_type {
+	NVM_TYPE_TIM1 = 0x01,
+	NVM_TYPE_TIM2 = 0x02,
+	NVM_TYPE_MIM1 = 0x03,
+	NVM_TYPE_MIM2 = 0x04,
+	NVM_TYPE_MBA = 0x05,
+	NVM_TYPE_MODULES_PN = 0x06,
+	NVM_TYPE_VPD = 0x07,
+	NVM_TYPE_MFW_TRACE1 = 0x08,
+	NVM_TYPE_MFW_TRACE2 = 0x09,
+	NVM_TYPE_NVM_CFG1 = 0x0a,
+	NVM_TYPE_L2B = 0x0b,
+	NVM_TYPE_DIR1 = 0x0c,
+	NVM_TYPE_EAGLE_FW1 = 0x0d,
+	NVM_TYPE_FALCON_FW1 = 0x0e,
+	NVM_TYPE_PCIE_FW1 = 0x0f,
+	NVM_TYPE_HW_SET = 0x10,
+	NVM_TYPE_LIM = 0x11,
+	NVM_TYPE_AVS_FW1 = 0x12,
+	NVM_TYPE_DIR2 = 0x13,
+	NVM_TYPE_CCM = 0x14,
+	NVM_TYPE_EAGLE_FW2 = 0x15,
+	NVM_TYPE_FALCON_FW2 = 0x16,
+	NVM_TYPE_PCIE_FW2 = 0x17,
+	NVM_TYPE_AVS_FW2 = 0x18,
+	NVM_TYPE_INIT_HW = 0x19,
+	NVM_TYPE_DEFAULT_CFG = 0x1a,
+	NVM_TYPE_MDUMP = 0x1b,
+	NVM_TYPE_META = 0x1c,
+	NVM_TYPE_ISCSI_CFG = 0x1d,
+	NVM_TYPE_FCOE_CFG = 0x1f,
+	NVM_TYPE_ETH_PHY_FW1 = 0x20,
+	NVM_TYPE_ETH_PHY_FW2 = 0x21,
+	NVM_TYPE_MAX,
+};
+
+#define DIR_ID_1    (0)
+
 #endif
diff --git a/drivers/net/ethernet/qlogic/qed/qed_hw.c b/drivers/net/ethernet/qlogic/qed/qed_hw.c
index e17885321faf..6e4fae9b1430 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_hw.c
+++ b/drivers/net/ethernet/qlogic/qed/qed_hw.c
@@ -44,8 +44,7 @@ struct qed_ptt_pool {
 
 int qed_ptt_pool_alloc(struct qed_hwfn *p_hwfn)
 {
-	struct qed_ptt_pool *p_pool = kmalloc(sizeof(*p_pool),
-					      GFP_KERNEL);
+	struct qed_ptt_pool *p_pool = kmalloc(sizeof(*p_pool), GFP_KERNEL);
 	int i;
 
 	if (!p_pool)
@@ -113,16 +112,14 @@ struct qed_ptt *qed_ptt_acquire(struct qed_hwfn *p_hwfn)
 	return NULL;
 }
 
-void qed_ptt_release(struct qed_hwfn *p_hwfn,
-		     struct qed_ptt *p_ptt)
+void qed_ptt_release(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
 {
 	spin_lock_bh(&p_hwfn->p_ptt_pool->lock);
 	list_add(&p_ptt->list_entry, &p_hwfn->p_ptt_pool->free_list);
 	spin_unlock_bh(&p_hwfn->p_ptt_pool->lock);
 }
 
-u32 qed_ptt_get_hw_addr(struct qed_hwfn *p_hwfn,
-			struct qed_ptt *p_ptt)
+u32 qed_ptt_get_hw_addr(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
 {
 	/* The HW is using DWORDS and we need to translate it to Bytes */
 	return le32_to_cpu(p_ptt->pxp.offset) << 2;
@@ -141,8 +138,7 @@ u32 qed_ptt_get_bar_addr(struct qed_ptt *p_ptt)
 }
 
 void qed_ptt_set_win(struct qed_hwfn *p_hwfn,
-		     struct qed_ptt *p_ptt,
-		     u32 new_hw_addr)
+		     struct qed_ptt *p_ptt, u32 new_hw_addr)
 {
 	u32 prev_hw_addr;
 
@@ -166,8 +162,7 @@ void qed_ptt_set_win(struct qed_hwfn *p_hwfn,
 }
 
 static u32 qed_set_ptt(struct qed_hwfn *p_hwfn,
-		       struct qed_ptt *p_ptt,
-		       u32 hw_addr)
+		       struct qed_ptt *p_ptt, u32 hw_addr)
 {
 	u32 win_hw_addr = qed_ptt_get_hw_addr(p_hwfn, p_ptt);
 	u32 offset;
@@ -224,10 +219,7 @@ u32 qed_rd(struct qed_hwfn *p_hwfn,
 
 static void qed_memcpy_hw(struct qed_hwfn *p_hwfn,
 			  struct qed_ptt *p_ptt,
-			  void *addr,
-			  u32 hw_addr,
-			  size_t n,
-			  bool to_device)
+			  void *addr, u32 hw_addr, size_t n, bool to_device)
 {
 	u32 dw_count, *host_addr, hw_offset;
 	size_t quota, done = 0;
@@ -259,8 +251,7 @@ static void qed_memcpy_hw(struct qed_hwfn *p_hwfn,
 }
 
 void qed_memcpy_from(struct qed_hwfn *p_hwfn,
-		     struct qed_ptt *p_ptt,
-		     void *dest, u32 hw_addr, size_t n)
+		     struct qed_ptt *p_ptt, void *dest, u32 hw_addr, size_t n)
 {
 	DP_VERBOSE(p_hwfn, NETIF_MSG_HW,
 		   "hw_addr 0x%x, dest %p hw_addr 0x%x, size %lu\n",
@@ -270,8 +261,7 @@ void qed_memcpy_from(struct qed_hwfn *p_hwfn,
 }
 
 void qed_memcpy_to(struct qed_hwfn *p_hwfn,
-		   struct qed_ptt *p_ptt,
-		   u32 hw_addr, void *src, size_t n)
+		   struct qed_ptt *p_ptt, u32 hw_addr, void *src, size_t n)
 {
 	DP_VERBOSE(p_hwfn, NETIF_MSG_HW,
 		   "hw_addr 0x%x, hw_addr 0x%x, src %p size %lu\n",
@@ -280,9 +270,7 @@ void qed_memcpy_to(struct qed_hwfn *p_hwfn,
 	qed_memcpy_hw(p_hwfn, p_ptt, src, hw_addr, n, true);
 }
 
-void qed_fid_pretend(struct qed_hwfn *p_hwfn,
-		     struct qed_ptt *p_ptt,
-		     u16 fid)
+void qed_fid_pretend(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt, u16 fid)
 {
 	u16 control = 0;
 
@@ -309,8 +297,7 @@ void qed_fid_pretend(struct qed_hwfn *p_hwfn,
 }
 
 void qed_port_pretend(struct qed_hwfn *p_hwfn,
-		      struct qed_ptt *p_ptt,
-		      u8 port_id)
+		      struct qed_ptt *p_ptt, u8 port_id)
 {
 	u16 control = 0;
 
@@ -326,8 +313,7 @@ void qed_port_pretend(struct qed_hwfn *p_hwfn,
 	       *(u32 *)&p_ptt->pxp.pretend);
 }
 
-void qed_port_unpretend(struct qed_hwfn *p_hwfn,
-			struct qed_ptt *p_ptt)
+void qed_port_unpretend(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
 {
 	u16 control = 0;
 
@@ -429,28 +415,27 @@ u32 qed_dmae_idx_to_go_cmd(u8 idx)
 	return DMAE_REG_GO_C0 + (idx << 2);
 }
 
-static int
-qed_dmae_post_command(struct qed_hwfn *p_hwfn,
-		      struct qed_ptt *p_ptt)
+static int qed_dmae_post_command(struct qed_hwfn *p_hwfn,
+				 struct qed_ptt *p_ptt)
 {
-	struct dmae_cmd *command = p_hwfn->dmae_info.p_dmae_cmd;
+	struct dmae_cmd *p_command = p_hwfn->dmae_info.p_dmae_cmd;
 	u8 idx_cmd = p_hwfn->dmae_info.channel, i;
 	int qed_status = 0;
 
 	/* verify address is not NULL */
-	if ((((command->dst_addr_lo == 0) && (command->dst_addr_hi == 0)) ||
-	     ((command->src_addr_lo == 0) && (command->src_addr_hi == 0)))) {
+	if ((((!p_command->dst_addr_lo) && (!p_command->dst_addr_hi)) ||
+	     ((!p_command->src_addr_lo) && (!p_command->src_addr_hi)))) {
 		DP_NOTICE(p_hwfn,
 			  "source or destination address 0 idx_cmd=%d\n"
 			  "opcode = [0x%08x,0x%04x] len=0x%x src=0x%x:%x dst=0x%x:%x\n",
-			   idx_cmd,
-			   le32_to_cpu(command->opcode),
-			   le16_to_cpu(command->opcode_b),
-			   le16_to_cpu(command->length_dw),
-			   le32_to_cpu(command->src_addr_hi),
-			   le32_to_cpu(command->src_addr_lo),
-			   le32_to_cpu(command->dst_addr_hi),
-			   le32_to_cpu(command->dst_addr_lo));
+			  idx_cmd,
+			  le32_to_cpu(p_command->opcode),
+			  le16_to_cpu(p_command->opcode_b),
+			  le16_to_cpu(p_command->length_dw),
+			  le32_to_cpu(p_command->src_addr_hi),
+			  le32_to_cpu(p_command->src_addr_lo),
+			  le32_to_cpu(p_command->dst_addr_hi),
+			  le32_to_cpu(p_command->dst_addr_lo));
 
 		return -EINVAL;
 	}
@@ -459,13 +444,13 @@ qed_dmae_post_command(struct qed_hwfn *p_hwfn,
 		   NETIF_MSG_HW,
 		   "Posting DMAE command [idx %d]: opcode = [0x%08x,0x%04x] len=0x%x src=0x%x:%x dst=0x%x:%x\n",
 		   idx_cmd,
-		   le32_to_cpu(command->opcode),
-		   le16_to_cpu(command->opcode_b),
-		   le16_to_cpu(command->length_dw),
-		   le32_to_cpu(command->src_addr_hi),
-		   le32_to_cpu(command->src_addr_lo),
-		   le32_to_cpu(command->dst_addr_hi),
-		   le32_to_cpu(command->dst_addr_lo));
+		   le32_to_cpu(p_command->opcode),
+		   le16_to_cpu(p_command->opcode_b),
+		   le16_to_cpu(p_command->length_dw),
+		   le32_to_cpu(p_command->src_addr_hi),
+		   le32_to_cpu(p_command->src_addr_lo),
+		   le32_to_cpu(p_command->dst_addr_hi),
+		   le32_to_cpu(p_command->dst_addr_lo));
 
 	/* Copy the command to DMAE - need to do it before every call
 	 * for source/dest address no reset.
@@ -475,7 +460,7 @@ qed_dmae_post_command(struct qed_hwfn *p_hwfn,
 	 */
 	for (i = 0; i < DMAE_CMD_SIZE; i++) {
 		u32 data = (i < DMAE_CMD_SIZE_TO_FILL) ?
-			   *(((u32 *)command) + i) : 0;
+			   *(((u32 *)p_command) + i) : 0;
 
 		qed_wr(p_hwfn, p_ptt,
 		       DMAE_REG_CMD_MEM +
@@ -483,9 +468,7 @@ qed_dmae_post_command(struct qed_hwfn *p_hwfn,
 		       (i * sizeof(u32)), data);
 	}
 
-	qed_wr(p_hwfn, p_ptt,
-	       qed_dmae_idx_to_go_cmd(idx_cmd),
-	       DMAE_GO_VALUE);
+	qed_wr(p_hwfn, p_ptt, qed_dmae_idx_to_go_cmd(idx_cmd), DMAE_GO_VALUE);
 
 	return qed_status;
 }
@@ -498,31 +481,23 @@ int qed_dmae_info_alloc(struct qed_hwfn *p_hwfn)
 	u32 **p_comp = &p_hwfn->dmae_info.p_completion_word;
 
 	*p_comp = dma_alloc_coherent(&p_hwfn->cdev->pdev->dev,
-				     sizeof(u32),
-				     p_addr,
-				     GFP_KERNEL);
-	if (!*p_comp) {
-		DP_NOTICE(p_hwfn, "Failed to allocate `p_completion_word'\n");
+				     sizeof(u32), p_addr, GFP_KERNEL);
+	if (!*p_comp)
 		goto err;
-	}
 
 	p_addr = &p_hwfn->dmae_info.dmae_cmd_phys_addr;
 	*p_cmd = dma_alloc_coherent(&p_hwfn->cdev->pdev->dev,
 				    sizeof(struct dmae_cmd),
 				    p_addr, GFP_KERNEL);
-	if (!*p_cmd) {
-		DP_NOTICE(p_hwfn, "Failed to allocate `struct dmae_cmd'\n");
+	if (!*p_cmd)
 		goto err;
-	}
 
 	p_addr = &p_hwfn->dmae_info.intermediate_buffer_phys_addr;
 	*p_buff = dma_alloc_coherent(&p_hwfn->cdev->pdev->dev,
 				     sizeof(u32) * DMAE_MAX_RW_SIZE,
 				     p_addr, GFP_KERNEL);
-	if (!*p_buff) {
-		DP_NOTICE(p_hwfn, "Failed to allocate `intermediate_buffer'\n");
+	if (!*p_buff)
 		goto err;
-	}
 
 	p_hwfn->dmae_info.channel = p_hwfn->rel_pf_id;
 
@@ -543,8 +518,7 @@ void qed_dmae_info_free(struct qed_hwfn *p_hwfn)
 		p_phys = p_hwfn->dmae_info.completion_word_phys_addr;
 		dma_free_coherent(&p_hwfn->cdev->pdev->dev,
 				  sizeof(u32),
-				  p_hwfn->dmae_info.p_completion_word,
-				  p_phys);
+				  p_hwfn->dmae_info.p_completion_word, p_phys);
 		p_hwfn->dmae_info.p_completion_word = NULL;
 	}
 
@@ -552,8 +526,7 @@ void qed_dmae_info_free(struct qed_hwfn *p_hwfn)
 		p_phys = p_hwfn->dmae_info.dmae_cmd_phys_addr;
 		dma_free_coherent(&p_hwfn->cdev->pdev->dev,
 				  sizeof(struct dmae_cmd),
-				  p_hwfn->dmae_info.p_dmae_cmd,
-				  p_phys);
+				  p_hwfn->dmae_info.p_dmae_cmd, p_phys);
 		p_hwfn->dmae_info.p_dmae_cmd = NULL;
 	}
 
@@ -571,9 +544,7 @@ void qed_dmae_info_free(struct qed_hwfn *p_hwfn)
 
 static int qed_dmae_operation_wait(struct qed_hwfn *p_hwfn)
 {
-	u32 wait_cnt = 0;
-	u32 wait_cnt_limit = 10000;
-
+	u32 wait_cnt_limit = 10000, wait_cnt = 0;
 	int qed_status = 0;
 
 	barrier();
@@ -606,7 +577,7 @@ static int qed_dmae_execute_sub_operation(struct qed_hwfn *p_hwfn,
 					  u64 dst_addr,
 					  u8 src_type,
 					  u8 dst_type,
-					  u32 length)
+					  u32 length_dw)
 {
 	dma_addr_t phys = p_hwfn->dmae_info.intermediate_buffer_phys_addr;
 	struct dmae_cmd *cmd = p_hwfn->dmae_info.p_dmae_cmd;
@@ -624,7 +595,7 @@ static int qed_dmae_execute_sub_operation(struct qed_hwfn *p_hwfn,
 		cmd->src_addr_lo = cpu_to_le32(lower_32_bits(phys));
 		memcpy(&p_hwfn->dmae_info.p_intermediate_buffer[0],
 		       (void *)(uintptr_t)src_addr,
-		       length * sizeof(u32));
+		       length_dw * sizeof(u32));
 		break;
 	default:
 		return -EINVAL;
@@ -645,7 +616,7 @@ static int qed_dmae_execute_sub_operation(struct qed_hwfn *p_hwfn,
 		return -EINVAL;
 	}
 
-	cmd->length_dw = cpu_to_le16((u16)length);
+	cmd->length_dw = cpu_to_le16((u16)length_dw);
 
 	qed_dmae_post_command(p_hwfn, p_ptt);
 
@@ -654,16 +625,14 @@ static int qed_dmae_execute_sub_operation(struct qed_hwfn *p_hwfn,
 	if (qed_status) {
 		DP_NOTICE(p_hwfn,
 			  "qed_dmae_host2grc: Wait Failed. source_addr 0x%llx, grc_addr 0x%llx, size_in_dwords 0x%x\n",
-			  src_addr,
-			  dst_addr,
-			  length);
+			  src_addr, dst_addr, length_dw);
 		return qed_status;
 	}
 
 	if (dst_type == QED_DMAE_ADDRESS_HOST_VIRT)
 		memcpy((void *)(uintptr_t)(dst_addr),
 		       &p_hwfn->dmae_info.p_intermediate_buffer[0],
-		       length * sizeof(u32));
+		       length_dw * sizeof(u32));
 
 	return 0;
 }
@@ -730,10 +699,7 @@ static int qed_dmae_execute_command(struct qed_hwfn *p_hwfn,
 		if (qed_status) {
 			DP_NOTICE(p_hwfn,
 				  "qed_dmae_execute_sub_operation Failed with error 0x%x. source_addr 0x%llx, destination addr 0x%llx, size_in_dwords 0x%x\n",
-				  qed_status,
-				  src_addr,
-				  dst_addr,
-				  length_cur);
+				  qed_status, src_addr, dst_addr, length_cur);
 			break;
 		}
 	}
@@ -743,10 +709,7 @@ static int qed_dmae_execute_command(struct qed_hwfn *p_hwfn,
 
 int qed_dmae_host2grc(struct qed_hwfn *p_hwfn,
 		      struct qed_ptt *p_ptt,
-		      u64 source_addr,
-		      u32 grc_addr,
-		      u32 size_in_dwords,
-		      u32 flags)
+		  u64 source_addr, u32 grc_addr, u32 size_in_dwords, u32 flags)
 {
 	u32 grc_addr_in_dw = grc_addr / sizeof(u32);
 	struct qed_dmae_params params;
@@ -768,9 +731,10 @@ int qed_dmae_host2grc(struct qed_hwfn *p_hwfn,
 	return rc;
 }
 
-int
-qed_dmae_grc2host(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt, u32 grc_addr,
-		  dma_addr_t dest_addr, u32 size_in_dwords, u32 flags)
+int qed_dmae_grc2host(struct qed_hwfn *p_hwfn,
+		      struct qed_ptt *p_ptt,
+		      u32 grc_addr,
+		      dma_addr_t dest_addr, u32 size_in_dwords, u32 flags)
 {
 	u32 grc_addr_in_dw = grc_addr / sizeof(u32);
 	struct qed_dmae_params params;
@@ -791,12 +755,11 @@ qed_dmae_grc2host(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt, u32 grc_addr,
 	return rc;
 }
 
-int
-qed_dmae_host2host(struct qed_hwfn *p_hwfn,
-		   struct qed_ptt *p_ptt,
-		   dma_addr_t source_addr,
-		   dma_addr_t dest_addr,
-		   u32 size_in_dwords, struct qed_dmae_params *p_params)
+int qed_dmae_host2host(struct qed_hwfn *p_hwfn,
+		       struct qed_ptt *p_ptt,
+		       dma_addr_t source_addr,
+		       dma_addr_t dest_addr,
+		       u32 size_in_dwords, struct qed_dmae_params *p_params)
 {
 	int rc;
 
diff --git a/drivers/net/ethernet/qlogic/qed/qed_init_ops.c b/drivers/net/ethernet/qlogic/qed/qed_init_ops.c
index 9866a20d2128..d567ba94c8d1 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_init_ops.c
+++ b/drivers/net/ethernet/qlogic/qed/qed_init_ops.c
@@ -59,17 +59,14 @@ void qed_init_clear_rt_data(struct qed_hwfn *p_hwfn)
 		p_hwfn->rt_data.b_valid[i] = false;
 }
 
-void qed_init_store_rt_reg(struct qed_hwfn *p_hwfn,
-			   u32 rt_offset,
-			   u32 val)
+void qed_init_store_rt_reg(struct qed_hwfn *p_hwfn, u32 rt_offset, u32 val)
 {
 	p_hwfn->rt_data.init_val[rt_offset] = val;
 	p_hwfn->rt_data.b_valid[rt_offset] = true;
 }
 
 void qed_init_store_rt_agg(struct qed_hwfn *p_hwfn,
-			   u32 rt_offset, u32 *p_val,
-			   size_t size)
+			   u32 rt_offset, u32 *p_val, size_t size)
 {
 	size_t i;
 
@@ -81,10 +78,7 @@ void qed_init_store_rt_agg(struct qed_hwfn *p_hwfn,
 
 static int qed_init_rt(struct qed_hwfn	*p_hwfn,
 		       struct qed_ptt *p_ptt,
-		       u32 addr,
-		       u16 rt_offset,
-		       u16 size,
-		       bool b_must_dmae)
+		       u32 addr, u16 rt_offset, u16 size, bool b_must_dmae)
 {
 	u32 *p_init_val = &p_hwfn->rt_data.init_val[rt_offset];
 	bool *p_valid = &p_hwfn->rt_data.b_valid[rt_offset];
@@ -102,8 +96,7 @@ static int qed_init_rt(struct qed_hwfn	*p_hwfn,
 		 * simply write the data instead of using dmae.
 		 */
 		if (!b_must_dmae) {
-			qed_wr(p_hwfn, p_ptt, addr + (i << 2),
-			       p_init_val[i]);
+			qed_wr(p_hwfn, p_ptt, addr + (i << 2), p_init_val[i]);
 			continue;
 		}
 
@@ -115,7 +108,7 @@ static int qed_init_rt(struct qed_hwfn	*p_hwfn,
 		rc = qed_dmae_host2grc(p_hwfn, p_ptt,
 				       (uintptr_t)(p_init_val + i),
 				       addr + (i << 2), segment, 0);
-		if (rc != 0)
+		if (rc)
 			return rc;
 
 		/* Jump over the entire segment, including invalid entry */
@@ -182,9 +175,7 @@ static int qed_init_array_dmae(struct qed_hwfn *p_hwfn,
 
 static int qed_init_fill_dmae(struct qed_hwfn *p_hwfn,
 			      struct qed_ptt *p_ptt,
-			      u32 addr,
-			      u32 fill,
-			      u32 fill_count)
+			      u32 addr, u32 fill, u32 fill_count)
 {
 	static u32 zero_buffer[DMAE_MAX_RW_SIZE];
 
@@ -199,15 +190,12 @@ static int qed_init_fill_dmae(struct qed_hwfn *p_hwfn,
 
 	return qed_dmae_host2grc(p_hwfn, p_ptt,
 				 (uintptr_t)(&zero_buffer[0]),
-				 addr, fill_count,
-				 QED_DMAE_FLAG_RW_REPL_SRC);
+				 addr, fill_count, QED_DMAE_FLAG_RW_REPL_SRC);
 }
 
 static void qed_init_fill(struct qed_hwfn *p_hwfn,
 			  struct qed_ptt *p_ptt,
-			  u32 addr,
-			  u32 fill,
-			  u32 fill_count)
+			  u32 addr, u32 fill, u32 fill_count)
 {
 	u32 i;
 
@@ -218,12 +206,12 @@ static void qed_init_fill(struct qed_hwfn *p_hwfn,
 static int qed_init_cmd_array(struct qed_hwfn *p_hwfn,
 			      struct qed_ptt *p_ptt,
 			      struct init_write_op *cmd,
-			      bool b_must_dmae,
-			      bool b_can_dmae)
+			      bool b_must_dmae, bool b_can_dmae)
 {
+	u32 dmae_array_offset = le32_to_cpu(cmd->args.array_offset);
 	u32 data = le32_to_cpu(cmd->data);
 	u32 addr = GET_FIELD(data, INIT_WRITE_OP_ADDRESS) << 2;
-	u32 dmae_array_offset = le32_to_cpu(cmd->args.array_offset);
+
 	u32 offset, output_len, input_len, max_size;
 	struct qed_dev *cdev = p_hwfn->cdev;
 	union init_array_hdr *hdr;
@@ -233,8 +221,7 @@ static int qed_init_cmd_array(struct qed_hwfn *p_hwfn,
 
 	array_data = cdev->fw_data->arr_data;
 
-	hdr = (union init_array_hdr *)(array_data +
-				       dmae_array_offset);
+	hdr = (union init_array_hdr *)(array_data + dmae_array_offset);
 	data = le32_to_cpu(hdr->raw.data);
 	switch (GET_FIELD(data, INIT_ARRAY_RAW_HDR_TYPE)) {
 	case INIT_ARR_ZIPPED:
@@ -290,13 +277,12 @@ static int qed_init_cmd_array(struct qed_hwfn *p_hwfn,
 /* init_ops write command */
 static int qed_init_cmd_wr(struct qed_hwfn *p_hwfn,
 			   struct qed_ptt *p_ptt,
-			   struct init_write_op *cmd,
-			   bool b_can_dmae)
+			   struct init_write_op *p_cmd, bool b_can_dmae)
 {
-	u32 data = le32_to_cpu(cmd->data);
-	u32 addr = GET_FIELD(data, INIT_WRITE_OP_ADDRESS) << 2;
+	u32 data = le32_to_cpu(p_cmd->data);
 	bool b_must_dmae = GET_FIELD(data, INIT_WRITE_OP_WIDE_BUS);
-	union init_write_args *arg = &cmd->args;
+	u32 addr = GET_FIELD(data, INIT_WRITE_OP_ADDRESS) << 2;
+	union init_write_args *arg = &p_cmd->args;
 	int rc = 0;
 
 	/* Sanitize */
@@ -309,20 +295,18 @@ static int qed_init_cmd_wr(struct qed_hwfn *p_hwfn,
 
 	switch (GET_FIELD(data, INIT_WRITE_OP_SOURCE)) {
 	case INIT_SRC_INLINE:
-		qed_wr(p_hwfn, p_ptt, addr,
-		       le32_to_cpu(arg->inline_val));
+		data = le32_to_cpu(p_cmd->args.inline_val);
+		qed_wr(p_hwfn, p_ptt, addr, data);
 		break;
 	case INIT_SRC_ZEROS:
-		if (b_must_dmae ||
-		    (b_can_dmae && (le32_to_cpu(arg->zeros_count) >= 64)))
-			rc = qed_init_fill_dmae(p_hwfn, p_ptt, addr, 0,
-						le32_to_cpu(arg->zeros_count));
+		data = le32_to_cpu(p_cmd->args.zeros_count);
+		if (b_must_dmae || (b_can_dmae && (data >= 64)))
+			rc = qed_init_fill_dmae(p_hwfn, p_ptt, addr, 0, data);
 		else
-			qed_init_fill(p_hwfn, p_ptt, addr, 0,
-				      le32_to_cpu(arg->zeros_count));
+			qed_init_fill(p_hwfn, p_ptt, addr, 0, data);
 		break;
 	case INIT_SRC_ARRAY:
-		rc = qed_init_cmd_array(p_hwfn, p_ptt, cmd,
+		rc = qed_init_cmd_array(p_hwfn, p_ptt, p_cmd,
 					b_must_dmae, b_can_dmae);
 		break;
 	case INIT_SRC_RUNTIME:
@@ -353,8 +337,7 @@ static inline bool comp_or(u32 val, u32 expected_val)
 
 /* init_ops read/poll commands */
 static void qed_init_cmd_rd(struct qed_hwfn *p_hwfn,
-			    struct qed_ptt *p_ptt,
-			    struct init_read_op *cmd)
+			    struct qed_ptt *p_ptt, struct init_read_op *cmd)
 {
 	bool (*comp_check)(u32 val, u32 expected_val);
 	u32 delay = QED_INIT_POLL_PERIOD_US, val;
@@ -412,35 +395,33 @@ static void qed_init_cmd_cb(struct qed_hwfn *p_hwfn,
 }
 
 static u8 qed_init_cmd_mode_match(struct qed_hwfn *p_hwfn,
-				  u16 *offset,
-				  int modes)
+				  u16 *p_offset, int modes)
 {
 	struct qed_dev *cdev = p_hwfn->cdev;
 	const u8 *modes_tree_buf;
 	u8 arg1, arg2, tree_val;
 
 	modes_tree_buf = cdev->fw_data->modes_tree_buf;
-	tree_val = modes_tree_buf[(*offset)++];
+	tree_val = modes_tree_buf[(*p_offset)++];
 	switch (tree_val) {
 	case INIT_MODE_OP_NOT:
-		return qed_init_cmd_mode_match(p_hwfn, offset, modes) ^ 1;
+		return qed_init_cmd_mode_match(p_hwfn, p_offset, modes) ^ 1;
 	case INIT_MODE_OP_OR:
-		arg1	= qed_init_cmd_mode_match(p_hwfn, offset, modes);
-		arg2	= qed_init_cmd_mode_match(p_hwfn, offset, modes);
+		arg1 = qed_init_cmd_mode_match(p_hwfn, p_offset, modes);
+		arg2 = qed_init_cmd_mode_match(p_hwfn, p_offset, modes);
 		return arg1 | arg2;
 	case INIT_MODE_OP_AND:
-		arg1	= qed_init_cmd_mode_match(p_hwfn, offset, modes);
-		arg2	= qed_init_cmd_mode_match(p_hwfn, offset, modes);
+		arg1 = qed_init_cmd_mode_match(p_hwfn, p_offset, modes);
+		arg2 = qed_init_cmd_mode_match(p_hwfn, p_offset, modes);
 		return arg1 & arg2;
 	default:
 		tree_val -= MAX_INIT_MODE_OPS;
-		return (modes & (1 << tree_val)) ? 1 : 0;
+		return (modes & BIT(tree_val)) ? 1 : 0;
 	}
 }
 
 static u32 qed_init_cmd_mode(struct qed_hwfn *p_hwfn,
-			     struct init_if_mode_op *p_cmd,
-			     int modes)
+			     struct init_if_mode_op *p_cmd, int modes)
 {
 	u16 offset = le16_to_cpu(p_cmd->modes_buf_offset);
 
@@ -453,8 +434,7 @@ static u32 qed_init_cmd_mode(struct qed_hwfn *p_hwfn,
 
 static u32 qed_init_cmd_phase(struct qed_hwfn *p_hwfn,
 			      struct init_if_phase_op *p_cmd,
-			      u32 phase,
-			      u32 phase_id)
+			      u32 phase, u32 phase_id)
 {
 	u32 data = le32_to_cpu(p_cmd->phase_data);
 	u32 op_data = le32_to_cpu(p_cmd->op_data);
@@ -468,10 +448,7 @@ static u32 qed_init_cmd_phase(struct qed_hwfn *p_hwfn,
 }
 
 int qed_init_run(struct qed_hwfn *p_hwfn,
-		 struct qed_ptt *p_ptt,
-		 int phase,
-		 int phase_id,
-		 int modes)
+		 struct qed_ptt *p_ptt, int phase, int phase_id, int modes)
 {
 	struct qed_dev *cdev = p_hwfn->cdev;
 	u32 cmd_num, num_init_ops;
@@ -483,10 +460,8 @@ int qed_init_run(struct qed_hwfn *p_hwfn,
 	init_ops = cdev->fw_data->init_ops;
 
 	p_hwfn->unzip_buf = kzalloc(MAX_ZIPPED_SIZE * 4, GFP_ATOMIC);
-	if (!p_hwfn->unzip_buf) {
-		DP_NOTICE(p_hwfn, "Failed to allocate unzip buffer\n");
+	if (!p_hwfn->unzip_buf)
 		return -ENOMEM;
-	}
 
 	for (cmd_num = 0; cmd_num < num_init_ops; cmd_num++) {
 		union init_op *cmd = &init_ops[cmd_num];
@@ -557,7 +532,7 @@ int qed_init_fw_data(struct qed_dev *cdev, const u8 *data)
 	/* First Dword contains metadata and should be skipped */
 	buf_hdr = (struct bin_buffer_hdr *)(data + sizeof(u32));
 
-	offset = buf_hdr[BIN_BUF_FW_VER_INFO].offset;
+	offset = buf_hdr[BIN_BUF_INIT_FW_VER_INFO].offset;
 	fw->fw_ver_info = (struct fw_ver_info *)(data + offset);
 
 	offset = buf_hdr[BIN_BUF_INIT_CMD].offset;
diff --git a/drivers/net/ethernet/qlogic/qed/qed_int.c b/drivers/net/ethernet/qlogic/qed/qed_int.c
index 8fa50fa23c8d..2adedc6fb6cf 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_int.c
+++ b/drivers/net/ethernet/qlogic/qed/qed_int.c
@@ -1775,10 +1775,9 @@ struct qed_sb_attn_info {
 };
 
 static inline u16 qed_attn_update_idx(struct qed_hwfn *p_hwfn,
-				      struct qed_sb_attn_info   *p_sb_desc)
+				      struct qed_sb_attn_info *p_sb_desc)
 {
-	u16     rc = 0;
-	u16     index;
+	u16 rc = 0, index;
 
 	/* Make certain HW write took affect */
 	mmiowb();
@@ -1802,15 +1801,13 @@ static inline u16 qed_attn_update_idx(struct qed_hwfn *p_hwfn,
  *  @param asserted_bits newly asserted bits
  *  @return int
  */
-static int qed_int_assertion(struct qed_hwfn *p_hwfn,
-			     u16 asserted_bits)
+static int qed_int_assertion(struct qed_hwfn *p_hwfn, u16 asserted_bits)
 {
 	struct qed_sb_attn_info *sb_attn_sw = p_hwfn->p_sb_attn;
 	u32 igu_mask;
 
 	/* Mask the source of the attention in the IGU */
-	igu_mask = qed_rd(p_hwfn, p_hwfn->p_dpc_ptt,
-			  IGU_REG_ATTENTION_ENABLE);
+	igu_mask = qed_rd(p_hwfn, p_hwfn->p_dpc_ptt, IGU_REG_ATTENTION_ENABLE);
 	DP_VERBOSE(p_hwfn, NETIF_MSG_INTR, "IGU mask: 0x%08x --> 0x%08x\n",
 		   igu_mask, igu_mask & ~(asserted_bits & ATTN_BITS_MASKABLE));
 	igu_mask &= ~(asserted_bits & ATTN_BITS_MASKABLE);
@@ -2041,7 +2038,7 @@ static int qed_int_deassertion(struct qed_hwfn  *p_hwfn,
 			struct aeu_invert_reg_bit *p_bit = &p_aeu->bits[j];
 
 			if ((p_bit->flags & ATTENTION_PARITY) &&
-			    !!(parities & (1 << bit_idx)))
+			    !!(parities & BIT(bit_idx)))
 				qed_int_deassertion_parity(p_hwfn, p_bit,
 							   bit_idx);
 
@@ -2114,8 +2111,7 @@ static int qed_int_deassertion(struct qed_hwfn  *p_hwfn,
 				    ~((u32)deasserted_bits));
 
 	/* Unmask deasserted attentions in IGU */
-	aeu_mask = qed_rd(p_hwfn, p_hwfn->p_dpc_ptt,
-			  IGU_REG_ATTENTION_ENABLE);
+	aeu_mask = qed_rd(p_hwfn, p_hwfn->p_dpc_ptt, IGU_REG_ATTENTION_ENABLE);
 	aeu_mask |= (deasserted_bits & ATTN_BITS_MASKABLE);
 	qed_wr(p_hwfn, p_hwfn->p_dpc_ptt, IGU_REG_ATTENTION_ENABLE, aeu_mask);
 
@@ -2160,8 +2156,7 @@ static int qed_int_attentions(struct qed_hwfn *p_hwfn)
 			index, attn_bits, attn_acks, asserted_bits,
 			deasserted_bits, p_sb_attn_sw->known_attn);
 	} else if (asserted_bits == 0x100) {
-		DP_INFO(p_hwfn,
-			"MFW indication via attention\n");
+		DP_INFO(p_hwfn, "MFW indication via attention\n");
 	} else {
 		DP_VERBOSE(p_hwfn, NETIF_MSG_INTR,
 			   "MFW indication [deassertion]\n");
@@ -2173,18 +2168,14 @@ static int qed_int_attentions(struct qed_hwfn *p_hwfn)
 			return rc;
 	}
 
-	if (deasserted_bits) {
+	if (deasserted_bits)
 		rc = qed_int_deassertion(p_hwfn, deasserted_bits);
-		if (rc)
-			return rc;
-	}
 
 	return rc;
 }
 
 static void qed_sb_ack_attn(struct qed_hwfn *p_hwfn,
-			    void __iomem *igu_addr,
-			    u32 ack_cons)
+			    void __iomem *igu_addr, u32 ack_cons)
 {
 	struct igu_prod_cons_update igu_ack = { 0 };
 
@@ -2242,9 +2233,8 @@ void qed_int_sp_dpc(unsigned long hwfn_cookie)
 
 	/* Gather Interrupts/Attentions information */
 	if (!sb_info->sb_virt) {
-		DP_ERR(
-			p_hwfn->cdev,
-			"Interrupt Status block is NULL - cannot check for new interrupts!\n");
+		DP_ERR(p_hwfn->cdev,
+		       "Interrupt Status block is NULL - cannot check for new interrupts!\n");
 	} else {
 		u32 tmp_index = sb_info->sb_ack;
 
@@ -2255,9 +2245,8 @@ void qed_int_sp_dpc(unsigned long hwfn_cookie)
 	}
 
 	if (!sb_attn || !sb_attn->sb_attn) {
-		DP_ERR(
-			p_hwfn->cdev,
-			"Attentions Status block is NULL - cannot check for new attentions!\n");
+		DP_ERR(p_hwfn->cdev,
+		       "Attentions Status block is NULL - cannot check for new attentions!\n");
 	} else {
 		u16 tmp_index = sb_attn->index;
 
@@ -2313,8 +2302,7 @@ static void qed_int_sb_attn_free(struct qed_hwfn *p_hwfn)
 	if (p_sb->sb_attn)
 		dma_free_coherent(&p_hwfn->cdev->pdev->dev,
 				  SB_ATTN_ALIGNED_SIZE(p_hwfn),
-				  p_sb->sb_attn,
-				  p_sb->sb_phys);
+				  p_sb->sb_attn, p_sb->sb_phys);
 	kfree(p_sb);
 }
 
@@ -2337,8 +2325,7 @@ static void qed_int_sb_attn_setup(struct qed_hwfn *p_hwfn,
 
 static void qed_int_sb_attn_init(struct qed_hwfn *p_hwfn,
 				 struct qed_ptt *p_ptt,
-				 void *sb_virt_addr,
-				 dma_addr_t sb_phy_addr)
+				 void *sb_virt_addr, dma_addr_t sb_phy_addr)
 {
 	struct qed_sb_attn_info *sb_info = p_hwfn->p_sb_attn;
 	int i, j, k;
@@ -2378,15 +2365,13 @@ static int qed_int_sb_attn_alloc(struct qed_hwfn *p_hwfn,
 {
 	struct qed_dev *cdev = p_hwfn->cdev;
 	struct qed_sb_attn_info *p_sb;
-	void *p_virt;
 	dma_addr_t p_phys = 0;
+	void *p_virt;
 
 	/* SB struct */
 	p_sb = kmalloc(sizeof(*p_sb), GFP_KERNEL);
-	if (!p_sb) {
-		DP_NOTICE(cdev, "Failed to allocate `struct qed_sb_attn_info'\n");
+	if (!p_sb)
 		return -ENOMEM;
-	}
 
 	/* SB ring  */
 	p_virt = dma_alloc_coherent(&cdev->pdev->dev,
@@ -2394,7 +2379,6 @@ static int qed_int_sb_attn_alloc(struct qed_hwfn *p_hwfn,
 				    &p_phys, GFP_KERNEL);
 
 	if (!p_virt) {
-		DP_NOTICE(cdev, "Failed to allocate status block (attentions)\n");
 		kfree(p_sb);
 		return -ENOMEM;
 	}
@@ -2412,9 +2396,7 @@ static int qed_int_sb_attn_alloc(struct qed_hwfn *p_hwfn,
 
 void qed_init_cau_sb_entry(struct qed_hwfn *p_hwfn,
 			   struct cau_sb_entry *p_sb_entry,
-			   u8 pf_id,
-			   u16 vf_number,
-			   u8 vf_valid)
+			   u8 pf_id, u16 vf_number, u8 vf_valid)
 {
 	struct qed_dev *cdev = p_hwfn->cdev;
 	u32 cau_state;
@@ -2428,12 +2410,6 @@ void qed_init_cau_sb_entry(struct qed_hwfn *p_hwfn,
 	SET_FIELD(p_sb_entry->params, CAU_SB_ENTRY_SB_TIMESET0, 0x7F);
 	SET_FIELD(p_sb_entry->params, CAU_SB_ENTRY_SB_TIMESET1, 0x7F);
 
-	/* setting the time resultion to a fixed value ( = 1) */
-	SET_FIELD(p_sb_entry->params, CAU_SB_ENTRY_TIMER_RES0,
-		  QED_CAU_DEF_RX_TIMER_RES);
-	SET_FIELD(p_sb_entry->params, CAU_SB_ENTRY_TIMER_RES1,
-		  QED_CAU_DEF_TX_TIMER_RES);
-
 	cau_state = CAU_HC_DISABLE_STATE;
 
 	if (cdev->int_coalescing_mode == QED_COAL_MODE_ENABLE) {
@@ -2468,9 +2444,7 @@ void qed_init_cau_sb_entry(struct qed_hwfn *p_hwfn,
 void qed_int_cau_conf_sb(struct qed_hwfn *p_hwfn,
 			 struct qed_ptt *p_ptt,
 			 dma_addr_t sb_phys,
-			 u16 igu_sb_id,
-			 u16 vf_number,
-			 u8 vf_valid)
+			 u16 igu_sb_id, u16 vf_number, u8 vf_valid)
 {
 	struct cau_sb_entry sb_entry;
 
@@ -2514,8 +2488,7 @@ void qed_int_cau_conf_sb(struct qed_hwfn *p_hwfn,
 			timer_res = 2;
 		timeset = (u8)(p_hwfn->cdev->rx_coalesce_usecs >> timer_res);
 		qed_int_cau_conf_pi(p_hwfn, p_ptt, igu_sb_id, RX_PI,
-				    QED_COAL_RX_STATE_MACHINE,
-				    timeset);
+				    QED_COAL_RX_STATE_MACHINE, timeset);
 
 		if (p_hwfn->cdev->tx_coalesce_usecs <= 0x7F)
 			timer_res = 0;
@@ -2541,8 +2514,7 @@ void qed_int_cau_conf_pi(struct qed_hwfn *p_hwfn,
 			 u8 timeset)
 {
 	struct cau_pi_entry pi_entry;
-	u32 sb_offset;
-	u32 pi_offset;
+	u32 sb_offset, pi_offset;
 
 	if (IS_VF(p_hwfn->cdev))
 		return;
@@ -2569,8 +2541,7 @@ void qed_int_cau_conf_pi(struct qed_hwfn *p_hwfn,
 }
 
 void qed_int_sb_setup(struct qed_hwfn *p_hwfn,
-		      struct qed_ptt *p_ptt,
-		      struct qed_sb_info *sb_info)
+		      struct qed_ptt *p_ptt, struct qed_sb_info *sb_info)
 {
 	/* zero status block and ack counter */
 	sb_info->sb_ack = 0;
@@ -2590,8 +2561,7 @@ void qed_int_sb_setup(struct qed_hwfn *p_hwfn,
  *
  * @return u16
  */
-static u16 qed_get_igu_sb_id(struct qed_hwfn *p_hwfn,
-			     u16 sb_id)
+static u16 qed_get_igu_sb_id(struct qed_hwfn *p_hwfn, u16 sb_id)
 {
 	u16 igu_sb_id;
 
@@ -2603,8 +2573,12 @@ static u16 qed_get_igu_sb_id(struct qed_hwfn *p_hwfn,
 	else
 		igu_sb_id = qed_vf_get_igu_sb_id(p_hwfn, sb_id);
 
-	DP_VERBOSE(p_hwfn, NETIF_MSG_INTR, "SB [%s] index is 0x%04x\n",
-		   (sb_id == QED_SP_SB_ID) ? "DSB" : "non-DSB", igu_sb_id);
+	if (sb_id == QED_SP_SB_ID)
+		DP_VERBOSE(p_hwfn, NETIF_MSG_INTR,
+			   "Slowpath SB index in IGU is 0x%04x\n", igu_sb_id);
+	else
+		DP_VERBOSE(p_hwfn, NETIF_MSG_INTR,
+			   "SB [%04x] <--> IGU SB [%04x]\n", sb_id, igu_sb_id);
 
 	return igu_sb_id;
 }
@@ -2612,9 +2586,7 @@ static u16 qed_get_igu_sb_id(struct qed_hwfn *p_hwfn,
 int qed_int_sb_init(struct qed_hwfn *p_hwfn,
 		    struct qed_ptt *p_ptt,
 		    struct qed_sb_info *sb_info,
-		    void *sb_virt_addr,
-		    dma_addr_t sb_phy_addr,
-		    u16 sb_id)
+		    void *sb_virt_addr, dma_addr_t sb_phy_addr, u16 sb_id)
 {
 	sb_info->sb_virt = sb_virt_addr;
 	sb_info->sb_phys = sb_phy_addr;
@@ -2650,8 +2622,7 @@ int qed_int_sb_init(struct qed_hwfn *p_hwfn,
 }
 
 int qed_int_sb_release(struct qed_hwfn *p_hwfn,
-		       struct qed_sb_info *sb_info,
-		       u16 sb_id)
+		       struct qed_sb_info *sb_info, u16 sb_id)
 {
 	if (sb_id == QED_SP_SB_ID) {
 		DP_ERR(p_hwfn, "Do Not free sp sb using this function");
@@ -2685,8 +2656,7 @@ static void qed_int_sp_sb_free(struct qed_hwfn *p_hwfn)
 	kfree(p_sb);
 }
 
-static int qed_int_sp_sb_alloc(struct qed_hwfn *p_hwfn,
-			       struct qed_ptt *p_ptt)
+static int qed_int_sp_sb_alloc(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
 {
 	struct qed_sb_sp_info *p_sb;
 	dma_addr_t p_phys = 0;
@@ -2694,17 +2664,14 @@ static int qed_int_sp_sb_alloc(struct qed_hwfn *p_hwfn,
 
 	/* SB struct */
 	p_sb = kmalloc(sizeof(*p_sb), GFP_KERNEL);
-	if (!p_sb) {
-		DP_NOTICE(p_hwfn, "Failed to allocate `struct qed_sb_info'\n");
+	if (!p_sb)
 		return -ENOMEM;
-	}
 
 	/* SB ring  */
 	p_virt = dma_alloc_coherent(&p_hwfn->cdev->pdev->dev,
 				    SB_ALIGNED_SIZE(p_hwfn),
 				    &p_phys, GFP_KERNEL);
 	if (!p_virt) {
-		DP_NOTICE(p_hwfn, "Failed to allocate status block\n");
 		kfree(p_sb);
 		return -ENOMEM;
 	}
@@ -2721,9 +2688,7 @@ static int qed_int_sp_sb_alloc(struct qed_hwfn *p_hwfn,
 
 int qed_int_register_cb(struct qed_hwfn *p_hwfn,
 			qed_int_comp_cb_t comp_cb,
-			void *cookie,
-			u8 *sb_idx,
-			__le16 **p_fw_cons)
+			void *cookie, u8 *sb_idx, __le16 **p_fw_cons)
 {
 	struct qed_sb_sp_info *p_sp_sb = p_hwfn->p_sp_sb;
 	int rc = -ENOMEM;
@@ -2764,8 +2729,7 @@ u16 qed_int_get_sp_sb_id(struct qed_hwfn *p_hwfn)
 }
 
 void qed_int_igu_enable_int(struct qed_hwfn *p_hwfn,
-			    struct qed_ptt *p_ptt,
-			    enum qed_int_mode int_mode)
+			    struct qed_ptt *p_ptt, enum qed_int_mode int_mode)
 {
 	u32 igu_pf_conf = IGU_PF_CONF_FUNC_EN | IGU_PF_CONF_ATTN_BIT_EN;
 
@@ -2809,7 +2773,7 @@ int qed_int_igu_enable(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt,
 	qed_wr(p_hwfn, p_ptt, MISC_REG_AEU_MASK_ATTN_IGU, 0xff);
 	if ((int_mode != QED_INT_MODE_INTA) || IS_LEAD_HWFN(p_hwfn)) {
 		rc = qed_slowpath_irq_req(p_hwfn);
-		if (rc != 0) {
+		if (rc) {
 			DP_NOTICE(p_hwfn, "Slowpath IRQ request failed\n");
 			return -EINVAL;
 		}
@@ -2822,8 +2786,7 @@ int qed_int_igu_enable(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt,
 	return rc;
 }
 
-void qed_int_igu_disable_int(struct qed_hwfn *p_hwfn,
-			     struct qed_ptt *p_ptt)
+void qed_int_igu_disable_int(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
 {
 	p_hwfn->b_int_enabled = 0;
 
@@ -2950,13 +2913,11 @@ void qed_int_igu_init_pure_rt(struct qed_hwfn *p_hwfn,
 					p_hwfn->hw_info.opaque_fid, b_set);
 }
 
-static u32 qed_int_igu_read_cam_block(struct qed_hwfn	*p_hwfn,
-				      struct qed_ptt	*p_ptt,
-				      u16		sb_id)
+static u32 qed_int_igu_read_cam_block(struct qed_hwfn *p_hwfn,
+				      struct qed_ptt *p_ptt, u16 sb_id)
 {
 	u32 val = qed_rd(p_hwfn, p_ptt,
-			 IGU_REG_MAPPING_MEMORY +
-			 sizeof(u32) * sb_id);
+			 IGU_REG_MAPPING_MEMORY + sizeof(u32) * sb_id);
 	struct qed_igu_block *p_block;
 
 	p_block = &p_hwfn->hw_info.p_igu_info->igu_map.igu_blocks[sb_id];
@@ -2983,8 +2944,7 @@ out:
 	return val;
 }
 
-int qed_int_igu_read_cam(struct qed_hwfn *p_hwfn,
-			 struct qed_ptt *p_ptt)
+int qed_int_igu_read_cam(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
 {
 	struct qed_igu_info *p_igu_info;
 	u32 val, min_vf = 0, max_vf = 0;
@@ -2993,7 +2953,6 @@ int qed_int_igu_read_cam(struct qed_hwfn *p_hwfn,
 	u16 prev_sb_id = 0xFF;
 
 	p_hwfn->hw_info.p_igu_info = kzalloc(sizeof(*p_igu_info), GFP_KERNEL);
-
 	if (!p_hwfn->hw_info.p_igu_info)
 		return -ENOMEM;
 
@@ -3104,22 +3063,19 @@ int qed_int_igu_read_cam(struct qed_hwfn *p_hwfn,
  */
 void qed_int_igu_init_rt(struct qed_hwfn *p_hwfn)
 {
-	u32 igu_pf_conf = 0;
-
-	igu_pf_conf |= IGU_PF_CONF_FUNC_EN;
+	u32 igu_pf_conf = IGU_PF_CONF_FUNC_EN;
 
 	STORE_RT_REG(p_hwfn, IGU_REG_PF_CONFIGURATION_RT_OFFSET, igu_pf_conf);
 }
 
 u64 qed_int_igu_read_sisr_reg(struct qed_hwfn *p_hwfn)
 {
-	u64 intr_status = 0;
-	u32 intr_status_lo = 0;
-	u32 intr_status_hi = 0;
 	u32 lsb_igu_cmd_addr = IGU_REG_SISR_MDPC_WMASK_LSB_UPPER -
 			       IGU_CMD_INT_ACK_BASE;
 	u32 msb_igu_cmd_addr = IGU_REG_SISR_MDPC_WMASK_MSB_UPPER -
 			       IGU_CMD_INT_ACK_BASE;
+	u32 intr_status_hi = 0, intr_status_lo = 0;
+	u64 intr_status = 0;
 
 	intr_status_lo = REG_RD(p_hwfn,
 				GTT_BAR0_MAP_REG_IGU_CMD +
@@ -3153,26 +3109,20 @@ static void qed_int_sp_dpc_free(struct qed_hwfn *p_hwfn)
 	kfree(p_hwfn->sp_dpc);
 }
 
-int qed_int_alloc(struct qed_hwfn *p_hwfn,
-		  struct qed_ptt *p_ptt)
+int qed_int_alloc(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
 {
 	int rc = 0;
 
 	rc = qed_int_sp_dpc_alloc(p_hwfn);
-	if (rc) {
-		DP_ERR(p_hwfn->cdev, "Failed to allocate sp dpc mem\n");
+	if (rc)
 		return rc;
-	}
+
 	rc = qed_int_sp_sb_alloc(p_hwfn, p_ptt);
-	if (rc) {
-		DP_ERR(p_hwfn->cdev, "Failed to allocate sp sb mem\n");
+	if (rc)
 		return rc;
-	}
+
 	rc = qed_int_sb_attn_alloc(p_hwfn, p_ptt);
-	if (rc) {
-		DP_ERR(p_hwfn->cdev, "Failed to allocate sb attn mem\n");
-		return rc;
-	}
+
 	return rc;
 }
 
@@ -3183,8 +3133,7 @@ void qed_int_free(struct qed_hwfn *p_hwfn)
 	qed_int_sp_dpc_free(p_hwfn);
 }
 
-void qed_int_setup(struct qed_hwfn *p_hwfn,
-		   struct qed_ptt *p_ptt)
+void qed_int_setup(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
 {
 	qed_int_sb_setup(p_hwfn, p_ptt, &p_hwfn->p_sp_sb->sb_info);
 	qed_int_sb_attn_setup(p_hwfn, p_ptt);
diff --git a/drivers/net/ethernet/qlogic/qed/qed_l2.c b/drivers/net/ethernet/qlogic/qed/qed_l2.c
index 401e738543b5..ddd410a91e13 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_l2.c
+++ b/drivers/net/ethernet/qlogic/qed/qed_l2.c
@@ -52,7 +52,7 @@ int qed_sp_eth_vport_start(struct qed_hwfn *p_hwfn,
 	u16 rx_mode = 0;
 
 	rc = qed_fw_vport(p_hwfn, p_params->vport_id, &abs_vport_id);
-	if (rc != 0)
+	if (rc)
 		return rc;
 
 	memset(&init_data, 0, sizeof(init_data));
@@ -80,8 +80,7 @@ int qed_sp_eth_vport_start(struct qed_hwfn *p_hwfn,
 	p_ramrod->rx_mode.state = cpu_to_le16(rx_mode);
 
 	/* TPA related fields */
-	memset(&p_ramrod->tpa_param, 0,
-	       sizeof(struct eth_vport_tpa_param));
+	memset(&p_ramrod->tpa_param, 0, sizeof(struct eth_vport_tpa_param));
 
 	p_ramrod->tpa_param.max_buff_num = p_params->max_buffers_per_cqe;
 
@@ -102,6 +101,9 @@ int qed_sp_eth_vport_start(struct qed_hwfn *p_hwfn,
 
 	p_ramrod->tx_switching_en = p_params->tx_switching;
 
+	p_ramrod->ctl_frame_mac_check_en = !!p_params->check_mac;
+	p_ramrod->ctl_frame_ethtype_check_en = !!p_params->check_ethtype;
+
 	/* Software Function ID in hwfn (PFs are 0 - 15, VFs are 16 - 135) */
 	p_ramrod->sw_fid = qed_concrete_to_sw_fid(p_hwfn->cdev,
 						  p_params->concrete_fid);
@@ -109,8 +111,8 @@ int qed_sp_eth_vport_start(struct qed_hwfn *p_hwfn,
 	return qed_spq_post(p_hwfn, p_ent, NULL);
 }
 
-int qed_sp_vport_start(struct qed_hwfn *p_hwfn,
-		       struct qed_sp_vport_start_params *p_params)
+static int qed_sp_vport_start(struct qed_hwfn *p_hwfn,
+			      struct qed_sp_vport_start_params *p_params)
 {
 	if (IS_VF(p_hwfn->cdev)) {
 		return qed_vf_pf_vport_start(p_hwfn, p_params->vport_id,
@@ -306,14 +308,14 @@ qed_sp_update_mcast_bin(struct qed_hwfn *p_hwfn,
 	memset(&p_ramrod->approx_mcast.bins, 0,
 	       sizeof(p_ramrod->approx_mcast.bins));
 
-	if (p_params->update_approx_mcast_flg) {
-		p_ramrod->common.update_approx_mcast_flg = 1;
-		for (i = 0; i < ETH_MULTICAST_MAC_BINS_IN_REGS; i++) {
-			u32 *p_bins = (u32 *)p_params->bins;
-			__le32 val = cpu_to_le32(p_bins[i]);
+	if (!p_params->update_approx_mcast_flg)
+		return;
 
-			p_ramrod->approx_mcast.bins[i] = val;
-		}
+	p_ramrod->common.update_approx_mcast_flg = 1;
+	for (i = 0; i < ETH_MULTICAST_MAC_BINS_IN_REGS; i++) {
+		u32 *p_bins = (u32 *)p_params->bins;
+
+		p_ramrod->approx_mcast.bins[i] = cpu_to_le32(p_bins[i]);
 	}
 }
 
@@ -336,7 +338,7 @@ int qed_sp_vport_update(struct qed_hwfn *p_hwfn,
 	}
 
 	rc = qed_fw_vport(p_hwfn, p_params->vport_id, &abs_vport_id);
-	if (rc != 0)
+	if (rc)
 		return rc;
 
 	memset(&init_data, 0, sizeof(init_data));
@@ -361,8 +363,8 @@ int qed_sp_vport_update(struct qed_hwfn *p_hwfn,
 	p_cmn->tx_active_flg = p_params->vport_active_tx_flg;
 	p_cmn->update_tx_active_flg = p_params->update_vport_active_tx_flg;
 	p_cmn->accept_any_vlan = p_params->accept_any_vlan;
-	p_cmn->update_accept_any_vlan_flg =
-			p_params->update_accept_any_vlan_flg;
+	val = p_params->update_accept_any_vlan_flg;
+	p_cmn->update_accept_any_vlan_flg = val;
 
 	p_cmn->inner_vlan_removal_en = p_params->inner_vlan_removal_flg;
 	val = p_params->update_inner_vlan_removal_flg;
@@ -411,7 +413,7 @@ int qed_sp_vport_stop(struct qed_hwfn *p_hwfn, u16 opaque_fid, u8 vport_id)
 		return qed_vf_pf_vport_stop(p_hwfn);
 
 	rc = qed_fw_vport(p_hwfn, vport_id, &abs_vport_id);
-	if (rc != 0)
+	if (rc)
 		return rc;
 
 	memset(&init_data, 0, sizeof(init_data));
@@ -476,7 +478,7 @@ static int qed_filter_accept_cmd(struct qed_dev *cdev,
 
 		rc = qed_sp_vport_update(p_hwfn, &vport_update_params,
 					 comp_mode, p_comp_data);
-		if (rc != 0) {
+		if (rc) {
 			DP_ERR(cdev, "Update rx_mode failed %d\n", rc);
 			return rc;
 		}
@@ -511,11 +513,12 @@ static int qed_sp_release_queue_cid(
 int qed_sp_eth_rxq_start_ramrod(struct qed_hwfn *p_hwfn,
 				u16 opaque_fid,
 				u32 cid,
-				struct qed_queue_start_common_params *params,
+				struct qed_queue_start_common_params *p_params,
 				u8 stats_id,
 				u16 bd_max_bytes,
 				dma_addr_t bd_chain_phys_addr,
-				dma_addr_t cqe_pbl_addr, u16 cqe_pbl_size)
+				dma_addr_t cqe_pbl_addr,
+				u16 cqe_pbl_size, bool b_use_zone_a_prod)
 {
 	struct rx_queue_start_ramrod_data *p_ramrod = NULL;
 	struct qed_spq_entry *p_ent = NULL;
@@ -526,23 +529,23 @@ int qed_sp_eth_rxq_start_ramrod(struct qed_hwfn *p_hwfn,
 	int rc = -EINVAL;
 
 	/* Store information for the stop */
-	p_rx_cid		= &p_hwfn->p_rx_cids[params->queue_id];
-	p_rx_cid->cid		= cid;
-	p_rx_cid->opaque_fid	= opaque_fid;
-	p_rx_cid->vport_id	= params->vport_id;
+	p_rx_cid = &p_hwfn->p_rx_cids[p_params->queue_id];
+	p_rx_cid->cid = cid;
+	p_rx_cid->opaque_fid = opaque_fid;
+	p_rx_cid->vport_id = p_params->vport_id;
 
-	rc = qed_fw_vport(p_hwfn, params->vport_id, &abs_vport_id);
-	if (rc != 0)
+	rc = qed_fw_vport(p_hwfn, p_params->vport_id, &abs_vport_id);
+	if (rc)
 		return rc;
 
-	rc = qed_fw_l2_queue(p_hwfn, params->queue_id, &abs_rx_q_id);
-	if (rc != 0)
+	rc = qed_fw_l2_queue(p_hwfn, p_params->queue_id, &abs_rx_q_id);
+	if (rc)
 		return rc;
 
 	DP_VERBOSE(p_hwfn, QED_MSG_SP,
 		   "opaque_fid=0x%x, cid=0x%x, rx_qid=0x%x, vport_id=0x%x, sb_id=0x%x\n",
-		   opaque_fid, cid, params->queue_id, params->vport_id,
-		   params->sb);
+		   opaque_fid,
+		   cid, p_params->queue_id, p_params->vport_id, p_params->sb);
 
 	/* Get SPQ entry */
 	memset(&init_data, 0, sizeof(init_data));
@@ -558,24 +561,28 @@ int qed_sp_eth_rxq_start_ramrod(struct qed_hwfn *p_hwfn,
 
 	p_ramrod = &p_ent->ramrod.rx_queue_start;
 
-	p_ramrod->sb_id			= cpu_to_le16(params->sb);
-	p_ramrod->sb_index		= params->sb_idx;
-	p_ramrod->vport_id		= abs_vport_id;
-	p_ramrod->stats_counter_id	= stats_id;
-	p_ramrod->rx_queue_id		= cpu_to_le16(abs_rx_q_id);
-	p_ramrod->complete_cqe_flg	= 0;
-	p_ramrod->complete_event_flg	= 1;
+	p_ramrod->sb_id = cpu_to_le16(p_params->sb);
+	p_ramrod->sb_index = p_params->sb_idx;
+	p_ramrod->vport_id = abs_vport_id;
+	p_ramrod->stats_counter_id = stats_id;
+	p_ramrod->rx_queue_id = cpu_to_le16(abs_rx_q_id);
+	p_ramrod->complete_cqe_flg = 0;
+	p_ramrod->complete_event_flg = 1;
 
-	p_ramrod->bd_max_bytes	= cpu_to_le16(bd_max_bytes);
+	p_ramrod->bd_max_bytes = cpu_to_le16(bd_max_bytes);
 	DMA_REGPAIR_LE(p_ramrod->bd_base, bd_chain_phys_addr);
 
-	p_ramrod->num_of_pbl_pages	= cpu_to_le16(cqe_pbl_size);
+	p_ramrod->num_of_pbl_pages = cpu_to_le16(cqe_pbl_size);
 	DMA_REGPAIR_LE(p_ramrod->cqe_pbl_addr, cqe_pbl_addr);
 
-	p_ramrod->vf_rx_prod_index = params->vf_qid;
-	if (params->vf_qid)
+	if (p_params->vf_qid || b_use_zone_a_prod) {
+		p_ramrod->vf_rx_prod_index = p_params->vf_qid;
 		DP_VERBOSE(p_hwfn, QED_MSG_SP,
-			   "Queue is meant for VF rxq[%04x]\n", params->vf_qid);
+			   "Queue%s is meant for VF rxq[%02x]\n",
+			   b_use_zone_a_prod ? " [legacy]" : "",
+			   p_params->vf_qid);
+		p_ramrod->vf_rx_prod_use_zone_a = b_use_zone_a_prod;
+	}
 
 	return qed_spq_post(p_hwfn, p_ent, NULL);
 }
@@ -583,7 +590,7 @@ int qed_sp_eth_rxq_start_ramrod(struct qed_hwfn *p_hwfn,
 static int
 qed_sp_eth_rx_queue_start(struct qed_hwfn *p_hwfn,
 			  u16 opaque_fid,
-			  struct qed_queue_start_common_params *params,
+			  struct qed_queue_start_common_params *p_params,
 			  u16 bd_max_bytes,
 			  dma_addr_t bd_chain_phys_addr,
 			  dma_addr_t cqe_pbl_addr,
@@ -597,20 +604,20 @@ qed_sp_eth_rx_queue_start(struct qed_hwfn *p_hwfn,
 
 	if (IS_VF(p_hwfn->cdev)) {
 		return qed_vf_pf_rxq_start(p_hwfn,
-					   params->queue_id,
-					   params->sb,
-					   params->sb_idx,
+					   p_params->queue_id,
+					   p_params->sb,
+					   (u8)p_params->sb_idx,
 					   bd_max_bytes,
 					   bd_chain_phys_addr,
 					   cqe_pbl_addr, cqe_pbl_size, pp_prod);
 	}
 
-	rc = qed_fw_l2_queue(p_hwfn, params->queue_id, &abs_l2_queue);
-	if (rc != 0)
+	rc = qed_fw_l2_queue(p_hwfn, p_params->queue_id, &abs_l2_queue);
+	if (rc)
 		return rc;
 
-	rc = qed_fw_vport(p_hwfn, params->vport_id, &abs_stats_id);
-	if (rc != 0)
+	rc = qed_fw_vport(p_hwfn, p_params->vport_id, &abs_stats_id);
+	if (rc)
 		return rc;
 
 	*pp_prod = (u8 __iomem *)p_hwfn->regview +
@@ -622,9 +629,8 @@ qed_sp_eth_rx_queue_start(struct qed_hwfn *p_hwfn,
 			  (u32 *)(&init_prod_val));
 
 	/* Allocate a CID for the queue */
-	p_rx_cid = &p_hwfn->p_rx_cids[params->queue_id];
-	rc = qed_cxt_acquire_cid(p_hwfn, PROTOCOLID_ETH,
-				 &p_rx_cid->cid);
+	p_rx_cid = &p_hwfn->p_rx_cids[p_params->queue_id];
+	rc = qed_cxt_acquire_cid(p_hwfn, PROTOCOLID_ETH, &p_rx_cid->cid);
 	if (rc) {
 		DP_NOTICE(p_hwfn, "Failed to acquire cid\n");
 		return rc;
@@ -634,14 +640,13 @@ qed_sp_eth_rx_queue_start(struct qed_hwfn *p_hwfn,
 	rc = qed_sp_eth_rxq_start_ramrod(p_hwfn,
 					 opaque_fid,
 					 p_rx_cid->cid,
-					 params,
+					 p_params,
 					 abs_stats_id,
 					 bd_max_bytes,
 					 bd_chain_phys_addr,
-					 cqe_pbl_addr,
-					 cqe_pbl_size);
+					 cqe_pbl_addr, cqe_pbl_size, false);
 
-	if (rc != 0)
+	if (rc)
 		qed_sp_release_queue_cid(p_hwfn, p_rx_cid);
 
 	return rc;
@@ -788,21 +793,20 @@ int qed_sp_eth_txq_start_ramrod(struct qed_hwfn  *p_hwfn,
 	if (rc)
 		return rc;
 
-	p_ramrod		= &p_ent->ramrod.tx_queue_start;
-	p_ramrod->vport_id	= abs_vport_id;
+	p_ramrod = &p_ent->ramrod.tx_queue_start;
+	p_ramrod->vport_id = abs_vport_id;
 
-	p_ramrod->sb_id			= cpu_to_le16(p_params->sb);
-	p_ramrod->sb_index		= p_params->sb_idx;
-	p_ramrod->stats_counter_id	= stats_id;
+	p_ramrod->sb_id = cpu_to_le16(p_params->sb);
+	p_ramrod->sb_index = p_params->sb_idx;
+	p_ramrod->stats_counter_id = stats_id;
 
-	p_ramrod->queue_zone_id		= cpu_to_le16(abs_tx_q_id);
-	p_ramrod->pbl_size		= cpu_to_le16(pbl_size);
+	p_ramrod->queue_zone_id = cpu_to_le16(abs_tx_q_id);
+
+	p_ramrod->pbl_size = cpu_to_le16(pbl_size);
 	DMA_REGPAIR_LE(p_ramrod->pbl_base_addr, pbl_addr);
 
-	pq_id			= qed_get_qm_pq(p_hwfn,
-						PROTOCOLID_ETH,
-						p_pq_params);
-	p_ramrod->qm_pq_id	= cpu_to_le16(pq_id);
+	pq_id = qed_get_qm_pq(p_hwfn, PROTOCOLID_ETH, p_pq_params);
+	p_ramrod->qm_pq_id = cpu_to_le16(pq_id);
 
 	return qed_spq_post(p_hwfn, p_ent, NULL);
 }
@@ -836,8 +840,7 @@ qed_sp_eth_tx_queue_start(struct qed_hwfn *p_hwfn,
 	memset(&pq_params, 0, sizeof(pq_params));
 
 	/* Allocate a CID for the queue */
-	rc = qed_cxt_acquire_cid(p_hwfn, PROTOCOLID_ETH,
-				 &p_tx_cid->cid);
+	rc = qed_cxt_acquire_cid(p_hwfn, PROTOCOLID_ETH, &p_tx_cid->cid);
 	if (rc) {
 		DP_NOTICE(p_hwfn, "Failed to acquire cid\n");
 		return rc;
@@ -896,8 +899,7 @@ int qed_sp_eth_tx_queue_stop(struct qed_hwfn *p_hwfn, u16 tx_queue_id)
 	return qed_sp_release_queue_cid(p_hwfn, p_tx_cid);
 }
 
-static enum eth_filter_action
-qed_filter_action(enum qed_filter_opcode opcode)
+static enum eth_filter_action qed_filter_action(enum qed_filter_opcode opcode)
 {
 	enum eth_filter_action action = MAX_ETH_FILTER_ACTION;
 
@@ -1033,19 +1035,19 @@ qed_filter_ucast_common(struct qed_hwfn *p_hwfn,
 		p_first_filter->vni = cpu_to_le32(p_filter_cmd->vni);
 
 	if (p_filter_cmd->opcode == QED_FILTER_MOVE) {
-		p_second_filter->type		= p_first_filter->type;
-		p_second_filter->mac_msb	= p_first_filter->mac_msb;
-		p_second_filter->mac_mid	= p_first_filter->mac_mid;
-		p_second_filter->mac_lsb	= p_first_filter->mac_lsb;
-		p_second_filter->vlan_id	= p_first_filter->vlan_id;
-		p_second_filter->vni		= p_first_filter->vni;
+		p_second_filter->type = p_first_filter->type;
+		p_second_filter->mac_msb = p_first_filter->mac_msb;
+		p_second_filter->mac_mid = p_first_filter->mac_mid;
+		p_second_filter->mac_lsb = p_first_filter->mac_lsb;
+		p_second_filter->vlan_id = p_first_filter->vlan_id;
+		p_second_filter->vni = p_first_filter->vni;
 
 		p_first_filter->action = ETH_FILTER_ACTION_REMOVE;
 
 		p_first_filter->vport_id = vport_to_remove_from;
 
-		p_second_filter->action		= ETH_FILTER_ACTION_ADD;
-		p_second_filter->vport_id	= vport_to_add_to;
+		p_second_filter->action = ETH_FILTER_ACTION_ADD;
+		p_second_filter->vport_id = vport_to_add_to;
 	} else if (p_filter_cmd->opcode == QED_FILTER_REPLACE) {
 		p_first_filter->vport_id = vport_to_add_to;
 		memcpy(p_second_filter, p_first_filter,
@@ -1086,7 +1088,7 @@ int qed_sp_eth_filter_ucast(struct qed_hwfn *p_hwfn,
 	rc = qed_filter_ucast_common(p_hwfn, opaque_fid, p_filter_cmd,
 				     &p_ramrod, &p_ent,
 				     comp_mode, p_comp_data);
-	if (rc != 0) {
+	if (rc) {
 		DP_ERR(p_hwfn, "Uni. filter command failed %d\n", rc);
 		return rc;
 	}
@@ -1094,10 +1096,8 @@ int qed_sp_eth_filter_ucast(struct qed_hwfn *p_hwfn,
 	p_header->assert_on_error = p_filter_cmd->assert_on_error;
 
 	rc = qed_spq_post(p_hwfn, p_ent, NULL);
-	if (rc != 0) {
-		DP_ERR(p_hwfn,
-		       "Unicast filter ADD command failed %d\n",
-		       rc);
+	if (rc) {
+		DP_ERR(p_hwfn, "Unicast filter ADD command failed %d\n", rc);
 		return rc;
 	}
 
@@ -1136,15 +1136,10 @@ int qed_sp_eth_filter_ucast(struct qed_hwfn *p_hwfn,
  * Return:
  ******************************************************************************/
 static u32 qed_calc_crc32c(u8 *crc32_packet,
-			   u32 crc32_length,
-			   u32 crc32_seed,
-			   u8 complement)
+			   u32 crc32_length, u32 crc32_seed, u8 complement)
 {
-	u32 byte = 0;
-	u32 bit = 0;
-	u8 msb = 0;
-	u8 current_byte = 0;
-	u32 crc32_result = crc32_seed;
+	u32 byte = 0, bit = 0, crc32_result = crc32_seed;
+	u8 msb = 0, current_byte = 0;
 
 	if ((!crc32_packet) ||
 	    (crc32_length == 0) ||
@@ -1164,9 +1159,7 @@ static u32 qed_calc_crc32c(u8 *crc32_packet,
 	return crc32_result;
 }
 
-static inline u32 qed_crc32c_le(u32 seed,
-				u8 *mac,
-				u32 len)
+static u32 qed_crc32c_le(u32 seed, u8 *mac, u32 len)
 {
 	u32 packet_buf[2] = { 0 };
 
@@ -1196,17 +1189,14 @@ qed_sp_eth_filter_mcast(struct qed_hwfn *p_hwfn,
 	u8 abs_vport_id = 0;
 	int rc, i;
 
-	if (p_filter_cmd->opcode == QED_FILTER_ADD) {
+	if (p_filter_cmd->opcode == QED_FILTER_ADD)
 		rc = qed_fw_vport(p_hwfn, p_filter_cmd->vport_to_add_to,
 				  &abs_vport_id);
-		if (rc)
-			return rc;
-	} else {
+	else
 		rc = qed_fw_vport(p_hwfn, p_filter_cmd->vport_to_remove_from,
 				  &abs_vport_id);
-		if (rc)
-			return rc;
-	}
+	if (rc)
+		return rc;
 
 	/* Get SPQ entry */
 	memset(&init_data, 0, sizeof(init_data));
@@ -1244,11 +1234,11 @@ qed_sp_eth_filter_mcast(struct qed_hwfn *p_hwfn,
 
 		/* Convert to correct endianity */
 		for (i = 0; i < ETH_MULTICAST_MAC_BINS_IN_REGS; i++) {
+			struct vport_update_ramrod_mcast *p_ramrod_bins;
 			u32 *p_bins = (u32 *)bins;
-			struct vport_update_ramrod_mcast *approx_mcast;
 
-			approx_mcast = &p_ramrod->approx_mcast;
-			approx_mcast->bins[i] = cpu_to_le32(p_bins[i]);
+			p_ramrod_bins = &p_ramrod->approx_mcast;
+			p_ramrod_bins->bins[i] = cpu_to_le32(p_bins[i]);
 		}
 	}
 
@@ -1286,8 +1276,7 @@ static int qed_filter_mcast_cmd(struct qed_dev *cdev,
 		rc = qed_sp_eth_filter_mcast(p_hwfn,
 					     opaque_fid,
 					     p_filter_cmd,
-					     comp_mode,
-					     p_comp_data);
+					     comp_mode, p_comp_data);
 	}
 	return rc;
 }
@@ -1314,9 +1303,8 @@ static int qed_filter_ucast_cmd(struct qed_dev *cdev,
 		rc = qed_sp_eth_filter_ucast(p_hwfn,
 					     opaque_fid,
 					     p_filter_cmd,
-					     comp_mode,
-					     p_comp_data);
-		if (rc != 0)
+					     comp_mode, p_comp_data);
+		if (rc)
 			break;
 	}
 
@@ -1590,8 +1578,7 @@ out:
 	}
 }
 
-void qed_get_vport_stats(struct qed_dev *cdev,
-			 struct qed_eth_stats *stats)
+void qed_get_vport_stats(struct qed_dev *cdev, struct qed_eth_stats *stats)
 {
 	u32 i;
 
@@ -1698,6 +1685,8 @@ static int qed_fill_eth_dev_info(struct qed_dev *cdev,
 		qed_vf_get_num_vlan_filters(&cdev->hwfns[0],
 					    &info->num_vlan_filters);
 		qed_vf_get_port_mac(&cdev->hwfns[0], info->port_mac);
+
+		info->is_legacy = !!cdev->hwfns[0].vf_iov_info->b_pre_fp_hsi;
 	}
 
 	qed_fill_dev_info(cdev, &info->common);
@@ -1766,8 +1755,7 @@ static int qed_start_vport(struct qed_dev *cdev,
 	return 0;
 }
 
-static int qed_stop_vport(struct qed_dev *cdev,
-			  u8 vport_id)
+static int qed_stop_vport(struct qed_dev *cdev, u8 vport_id)
 {
 	int rc, i;
 
@@ -1775,8 +1763,7 @@ static int qed_stop_vport(struct qed_dev *cdev,
 		struct qed_hwfn *p_hwfn = &cdev->hwfns[i];
 
 		rc = qed_sp_vport_stop(p_hwfn,
-				       p_hwfn->hw_info.opaque_fid,
-				       vport_id);
+				       p_hwfn->hw_info.opaque_fid, vport_id);
 
 		if (rc) {
 			DP_ERR(cdev, "Failed to stop VPORT\n");
@@ -1801,10 +1788,8 @@ static int qed_update_vport(struct qed_dev *cdev,
 
 	/* Translate protocol params into sp params */
 	sp_params.vport_id = params->vport_id;
-	sp_params.update_vport_active_rx_flg =
-		params->update_vport_active_flg;
-	sp_params.update_vport_active_tx_flg =
-		params->update_vport_active_flg;
+	sp_params.update_vport_active_rx_flg = params->update_vport_active_flg;
+	sp_params.update_vport_active_tx_flg = params->update_vport_active_flg;
 	sp_params.vport_active_rx_flg = params->vport_active_flg;
 	sp_params.vport_active_tx_flg = params->vport_active_flg;
 	sp_params.update_tx_switching_flg = params->update_tx_switching_flg;
@@ -1817,8 +1802,7 @@ static int qed_update_vport(struct qed_dev *cdev,
 	 * We need to re-fix the rss values per engine for CMT.
 	 */
 	if (cdev->num_hwfns > 1 && params->update_rss_flg) {
-		struct qed_update_vport_rss_params *rss =
-			&params->rss_params;
+		struct qed_update_vport_rss_params *rss = &params->rss_params;
 		int k, max = 0;
 
 		/* Find largest entry, since it's possible RSS needs to
@@ -1861,8 +1845,8 @@ static int qed_update_vport(struct qed_dev *cdev,
 		       QED_RSS_IND_TABLE_SIZE * sizeof(u16));
 		memcpy(sp_rss_params.rss_key, params->rss_params.rss_key,
 		       QED_RSS_KEY_SIZE * sizeof(u32));
+		sp_params.rss_params = &sp_rss_params;
 	}
-	sp_params.rss_params = &sp_rss_params;
 
 	for_each_hwfn(cdev, i) {
 		struct qed_hwfn *p_hwfn = &cdev->hwfns[i];
@@ -1893,8 +1877,8 @@ static int qed_start_rxq(struct qed_dev *cdev,
 			 u16 cqe_pbl_size,
 			 void __iomem **pp_prod)
 {
-	int rc, hwfn_index;
 	struct qed_hwfn *p_hwfn;
+	int rc, hwfn_index;
 
 	hwfn_index = params->rss_id % cdev->num_hwfns;
 	p_hwfn = &cdev->hwfns[hwfn_index];
@@ -1935,8 +1919,7 @@ static int qed_stop_rxq(struct qed_dev *cdev,
 
 	rc = qed_sp_eth_rx_queue_stop(p_hwfn,
 				      params->rx_queue_id / cdev->num_hwfns,
-				      params->eq_completion_only,
-				      false);
+				      params->eq_completion_only, false);
 	if (rc) {
 		DP_ERR(cdev, "Failed to stop RXQ#%d\n", params->rx_queue_id);
 		return rc;
@@ -2047,11 +2030,11 @@ static int qed_configure_filter_rx_mode(struct qed_dev *cdev,
 
 	memset(&accept_flags, 0, sizeof(accept_flags));
 
-	accept_flags.update_rx_mode_config	= 1;
-	accept_flags.update_tx_mode_config	= 1;
-	accept_flags.rx_accept_filter		= QED_ACCEPT_UCAST_MATCHED |
-						  QED_ACCEPT_MCAST_MATCHED |
-						  QED_ACCEPT_BCAST;
+	accept_flags.update_rx_mode_config = 1;
+	accept_flags.update_tx_mode_config = 1;
+	accept_flags.rx_accept_filter = QED_ACCEPT_UCAST_MATCHED |
+					QED_ACCEPT_MCAST_MATCHED |
+					QED_ACCEPT_BCAST;
 	accept_flags.tx_accept_filter = QED_ACCEPT_UCAST_MATCHED |
 					QED_ACCEPT_MCAST_MATCHED |
 					QED_ACCEPT_BCAST;
@@ -2072,9 +2055,8 @@ static int qed_configure_filter_ucast(struct qed_dev *cdev,
 	struct qed_filter_ucast ucast;
 
 	if (!params->vlan_valid && !params->mac_valid) {
-		DP_NOTICE(
-			cdev,
-			"Tried configuring a unicast filter, but both MAC and VLAN are not set\n");
+		DP_NOTICE(cdev,
+			  "Tried configuring a unicast filter, but both MAC and VLAN are not set\n");
 		return -EINVAL;
 	}
 
@@ -2135,8 +2117,7 @@ static int qed_configure_filter_mcast(struct qed_dev *cdev,
 	for (i = 0; i < mcast.num_mc_addrs; i++)
 		ether_addr_copy(mcast.mac[i], params->mac[i]);
 
-	return qed_filter_mcast_cmd(cdev, &mcast,
-				    QED_SPQ_MODE_CB, NULL);
+	return qed_filter_mcast_cmd(cdev, &mcast, QED_SPQ_MODE_CB, NULL);
 }
 
 static int qed_configure_filter(struct qed_dev *cdev,
@@ -2153,15 +2134,13 @@ static int qed_configure_filter(struct qed_dev *cdev,
 		accept_flags = params->filter.accept_flags;
 		return qed_configure_filter_rx_mode(cdev, accept_flags);
 	default:
-		DP_NOTICE(cdev, "Unknown filter type %d\n",
-			  (int)params->type);
+		DP_NOTICE(cdev, "Unknown filter type %d\n", (int)params->type);
 		return -EINVAL;
 	}
 }
 
 static int qed_fp_cqe_completion(struct qed_dev *dev,
-				 u8 rss_id,
-				 struct eth_slow_path_rx_cqe *cqe)
+				 u8 rss_id, struct eth_slow_path_rx_cqe *cqe)
 {
 	return qed_eth_cqe_completion(&dev->hwfns[rss_id % dev->num_hwfns],
 				      cqe);
diff --git a/drivers/net/ethernet/qlogic/qed/qed_l2.h b/drivers/net/ethernet/qlogic/qed/qed_l2.h
index 002114543451..e495d62fcc03 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_l2.h
+++ b/drivers/net/ethernet/qlogic/qed/qed_l2.h
@@ -102,6 +102,8 @@ struct qed_sp_vport_start_params {
 	u16 opaque_fid;
 	u8 vport_id;
 	u16 mtu;
+	bool check_mac;
+	bool check_ethtype;
 };
 
 int qed_sp_eth_vport_start(struct qed_hwfn *p_hwfn,
@@ -213,6 +215,8 @@ qed_sp_eth_rx_queues_update(struct qed_hwfn *p_hwfn,
 			    enum spq_mode comp_mode,
 			    struct qed_spq_comp_cb *p_comp_data);
 
+void qed_get_vport_stats(struct qed_dev *cdev, struct qed_eth_stats *stats);
+
 int qed_sp_eth_vport_start(struct qed_hwfn *p_hwfn,
 			   struct qed_sp_vport_start_params *p_params);
 
@@ -223,7 +227,8 @@ int qed_sp_eth_rxq_start_ramrod(struct qed_hwfn *p_hwfn,
 				u8 stats_id,
 				u16 bd_max_bytes,
 				dma_addr_t bd_chain_phys_addr,
-				dma_addr_t cqe_pbl_addr, u16 cqe_pbl_size);
+				dma_addr_t cqe_pbl_addr,
+				u16 cqe_pbl_size, bool b_use_zone_a_prod);
 
 int qed_sp_eth_txq_start_ramrod(struct qed_hwfn  *p_hwfn,
 				u16  opaque_fid,
diff --git a/drivers/net/ethernet/qlogic/qed/qed_ll2.c b/drivers/net/ethernet/qlogic/qed/qed_ll2.c
new file mode 100644
index 000000000000..02a8be2faed7
--- /dev/null
+++ b/drivers/net/ethernet/qlogic/qed/qed_ll2.c
@@ -0,0 +1,1792 @@
+/* QLogic qed NIC Driver
+ *
+ * Copyright (c) 2015 QLogic Corporation
+ *
+ * This software is available 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.
+ */
+
+#include <linux/types.h>
+#include <asm/byteorder.h>
+#include <linux/dma-mapping.h>
+#include <linux/if_vlan.h>
+#include <linux/kernel.h>
+#include <linux/pci.h>
+#include <linux/slab.h>
+#include <linux/stddef.h>
+#include <linux/version.h>
+#include <linux/workqueue.h>
+#include <net/ipv6.h>
+#include <linux/bitops.h>
+#include <linux/delay.h>
+#include <linux/errno.h>
+#include <linux/etherdevice.h>
+#include <linux/io.h>
+#include <linux/list.h>
+#include <linux/mutex.h>
+#include <linux/spinlock.h>
+#include <linux/string.h>
+#include <linux/qed/qed_ll2_if.h>
+#include "qed.h"
+#include "qed_cxt.h"
+#include "qed_dev_api.h"
+#include "qed_hsi.h"
+#include "qed_hw.h"
+#include "qed_int.h"
+#include "qed_ll2.h"
+#include "qed_mcp.h"
+#include "qed_reg_addr.h"
+#include "qed_sp.h"
+
+#define QED_LL2_RX_REGISTERED(ll2)	((ll2)->rx_queue.b_cb_registred)
+#define QED_LL2_TX_REGISTERED(ll2)	((ll2)->tx_queue.b_cb_registred)
+
+#define QED_LL2_TX_SIZE (256)
+#define QED_LL2_RX_SIZE (4096)
+
+struct qed_cb_ll2_info {
+	int rx_cnt;
+	u32 rx_size;
+	u8 handle;
+	bool frags_mapped;
+
+	/* Lock protecting LL2 buffer lists in sleepless context */
+	spinlock_t lock;
+	struct list_head list;
+
+	const struct qed_ll2_cb_ops *cbs;
+	void *cb_cookie;
+};
+
+struct qed_ll2_buffer {
+	struct list_head list;
+	void *data;
+	dma_addr_t phys_addr;
+};
+
+static void qed_ll2b_complete_tx_packet(struct qed_hwfn *p_hwfn,
+					u8 connection_handle,
+					void *cookie,
+					dma_addr_t first_frag_addr,
+					bool b_last_fragment,
+					bool b_last_packet)
+{
+	struct qed_dev *cdev = p_hwfn->cdev;
+	struct sk_buff *skb = cookie;
+
+	/* All we need to do is release the mapping */
+	dma_unmap_single(&p_hwfn->cdev->pdev->dev, first_frag_addr,
+			 skb_headlen(skb), DMA_TO_DEVICE);
+
+	if (cdev->ll2->cbs && cdev->ll2->cbs->tx_cb)
+		cdev->ll2->cbs->tx_cb(cdev->ll2->cb_cookie, skb,
+				      b_last_fragment);
+
+	if (cdev->ll2->frags_mapped)
+		/* Case where mapped frags were received, need to
+		 * free skb with nr_frags marked as 0
+		 */
+		skb_shinfo(skb)->nr_frags = 0;
+
+	dev_kfree_skb_any(skb);
+}
+
+static int qed_ll2_alloc_buffer(struct qed_dev *cdev,
+				u8 **data, dma_addr_t *phys_addr)
+{
+	*data = kmalloc(cdev->ll2->rx_size, GFP_ATOMIC);
+	if (!(*data)) {
+		DP_INFO(cdev, "Failed to allocate LL2 buffer data\n");
+		return -ENOMEM;
+	}
+
+	*phys_addr = dma_map_single(&cdev->pdev->dev,
+				    ((*data) + NET_SKB_PAD),
+				    cdev->ll2->rx_size, DMA_FROM_DEVICE);
+	if (dma_mapping_error(&cdev->pdev->dev, *phys_addr)) {
+		DP_INFO(cdev, "Failed to map LL2 buffer data\n");
+		kfree((*data));
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+
+static int qed_ll2_dealloc_buffer(struct qed_dev *cdev,
+				 struct qed_ll2_buffer *buffer)
+{
+	spin_lock_bh(&cdev->ll2->lock);
+
+	dma_unmap_single(&cdev->pdev->dev, buffer->phys_addr,
+			 cdev->ll2->rx_size, DMA_FROM_DEVICE);
+	kfree(buffer->data);
+	list_del(&buffer->list);
+
+	cdev->ll2->rx_cnt--;
+	if (!cdev->ll2->rx_cnt)
+		DP_INFO(cdev, "All LL2 entries were removed\n");
+
+	spin_unlock_bh(&cdev->ll2->lock);
+
+	return 0;
+}
+
+static void qed_ll2_kill_buffers(struct qed_dev *cdev)
+{
+	struct qed_ll2_buffer *buffer, *tmp_buffer;
+
+	list_for_each_entry_safe(buffer, tmp_buffer, &cdev->ll2->list, list)
+		qed_ll2_dealloc_buffer(cdev, buffer);
+}
+
+void qed_ll2b_complete_rx_packet(struct qed_hwfn *p_hwfn,
+				 u8 connection_handle,
+				 struct qed_ll2_rx_packet *p_pkt,
+				 struct core_rx_fast_path_cqe *p_cqe,
+				 bool b_last_packet)
+{
+	u16 packet_length = le16_to_cpu(p_cqe->packet_length);
+	struct qed_ll2_buffer *buffer = p_pkt->cookie;
+	struct qed_dev *cdev = p_hwfn->cdev;
+	u16 vlan = le16_to_cpu(p_cqe->vlan);
+	u32 opaque_data_0, opaque_data_1;
+	u8 pad = p_cqe->placement_offset;
+	dma_addr_t new_phys_addr;
+	struct sk_buff *skb;
+	bool reuse = false;
+	int rc = -EINVAL;
+	u8 *new_data;
+
+	opaque_data_0 = le32_to_cpu(p_cqe->opaque_data.data[0]);
+	opaque_data_1 = le32_to_cpu(p_cqe->opaque_data.data[1]);
+
+	DP_VERBOSE(p_hwfn,
+		   (NETIF_MSG_RX_STATUS | QED_MSG_STORAGE | NETIF_MSG_PKTDATA),
+		   "Got an LL2 Rx completion: [Buffer at phys 0x%llx, offset 0x%02x] Length 0x%04x Parse_flags 0x%04x vlan 0x%04x Opaque data [0x%08x:0x%08x]\n",
+		   (u64)p_pkt->rx_buf_addr, pad, packet_length,
+		   le16_to_cpu(p_cqe->parse_flags.flags), vlan,
+		   opaque_data_0, opaque_data_1);
+
+	if ((cdev->dp_module & NETIF_MSG_PKTDATA) && buffer->data) {
+		print_hex_dump(KERN_INFO, "",
+			       DUMP_PREFIX_OFFSET, 16, 1,
+			       buffer->data, packet_length, false);
+	}
+
+	/* Determine if data is valid */
+	if (packet_length < ETH_HLEN)
+		reuse = true;
+
+	/* Allocate a replacement for buffer; Reuse upon failure */
+	if (!reuse)
+		rc = qed_ll2_alloc_buffer(p_hwfn->cdev, &new_data,
+					  &new_phys_addr);
+
+	/* If need to reuse or there's no replacement buffer, repost this */
+	if (rc)
+		goto out_post;
+
+	skb = build_skb(buffer->data, 0);
+	if (!skb) {
+		rc = -ENOMEM;
+		goto out_post;
+	}
+
+	pad += NET_SKB_PAD;
+	skb_reserve(skb, pad);
+	skb_put(skb, packet_length);
+	skb_checksum_none_assert(skb);
+
+	/* Get parital ethernet information instead of eth_type_trans(),
+	 * Since we don't have an associated net_device.
+	 */
+	skb_reset_mac_header(skb);
+	skb->protocol = eth_hdr(skb)->h_proto;
+
+	/* Pass SKB onward */
+	if (cdev->ll2->cbs && cdev->ll2->cbs->rx_cb) {
+		if (vlan)
+			__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), vlan);
+		cdev->ll2->cbs->rx_cb(cdev->ll2->cb_cookie, skb,
+				      opaque_data_0, opaque_data_1);
+	}
+
+	/* Update Buffer information and update FW producer */
+	buffer->data = new_data;
+	buffer->phys_addr = new_phys_addr;
+
+out_post:
+	rc = qed_ll2_post_rx_buffer(QED_LEADING_HWFN(cdev), cdev->ll2->handle,
+				    buffer->phys_addr, 0,  buffer, 1);
+
+	if (rc)
+		qed_ll2_dealloc_buffer(cdev, buffer);
+}
+
+static struct qed_ll2_info *__qed_ll2_handle_sanity(struct qed_hwfn *p_hwfn,
+						    u8 connection_handle,
+						    bool b_lock,
+						    bool b_only_active)
+{
+	struct qed_ll2_info *p_ll2_conn, *p_ret = NULL;
+
+	if (connection_handle >= QED_MAX_NUM_OF_LL2_CONNECTIONS)
+		return NULL;
+
+	if (!p_hwfn->p_ll2_info)
+		return NULL;
+
+	p_ll2_conn = &p_hwfn->p_ll2_info[connection_handle];
+
+	if (b_only_active) {
+		if (b_lock)
+			mutex_lock(&p_ll2_conn->mutex);
+		if (p_ll2_conn->b_active)
+			p_ret = p_ll2_conn;
+		if (b_lock)
+			mutex_unlock(&p_ll2_conn->mutex);
+	} else {
+		p_ret = p_ll2_conn;
+	}
+
+	return p_ret;
+}
+
+static struct qed_ll2_info *qed_ll2_handle_sanity(struct qed_hwfn *p_hwfn,
+						  u8 connection_handle)
+{
+	return __qed_ll2_handle_sanity(p_hwfn, connection_handle, false, true);
+}
+
+static struct qed_ll2_info *qed_ll2_handle_sanity_lock(struct qed_hwfn *p_hwfn,
+						       u8 connection_handle)
+{
+	return __qed_ll2_handle_sanity(p_hwfn, connection_handle, true, true);
+}
+
+static struct qed_ll2_info *qed_ll2_handle_sanity_inactive(struct qed_hwfn
+							   *p_hwfn,
+							   u8 connection_handle)
+{
+	return __qed_ll2_handle_sanity(p_hwfn, connection_handle, false, false);
+}
+
+static void qed_ll2_txq_flush(struct qed_hwfn *p_hwfn, u8 connection_handle)
+{
+	bool b_last_packet = false, b_last_frag = false;
+	struct qed_ll2_tx_packet *p_pkt = NULL;
+	struct qed_ll2_info *p_ll2_conn;
+	struct qed_ll2_tx_queue *p_tx;
+	dma_addr_t tx_frag;
+
+	p_ll2_conn = qed_ll2_handle_sanity_inactive(p_hwfn, connection_handle);
+	if (!p_ll2_conn)
+		return;
+
+	p_tx = &p_ll2_conn->tx_queue;
+
+	while (!list_empty(&p_tx->active_descq)) {
+		p_pkt = list_first_entry(&p_tx->active_descq,
+					 struct qed_ll2_tx_packet, list_entry);
+		if (!p_pkt)
+			break;
+
+		list_del(&p_pkt->list_entry);
+		b_last_packet = list_empty(&p_tx->active_descq);
+		list_add_tail(&p_pkt->list_entry, &p_tx->free_descq);
+		p_tx->cur_completing_packet = *p_pkt;
+		p_tx->cur_completing_bd_idx = 1;
+		b_last_frag = p_tx->cur_completing_bd_idx == p_pkt->bd_used;
+		tx_frag = p_pkt->bds_set[0].tx_frag;
+		if (p_ll2_conn->gsi_enable)
+			qed_ll2b_release_tx_gsi_packet(p_hwfn,
+						       p_ll2_conn->my_id,
+						       p_pkt->cookie,
+						       tx_frag,
+						       b_last_frag,
+						       b_last_packet);
+		else
+			qed_ll2b_complete_tx_packet(p_hwfn,
+						    p_ll2_conn->my_id,
+						    p_pkt->cookie,
+						    tx_frag,
+						    b_last_frag,
+						    b_last_packet);
+
+	}
+}
+
+static int qed_ll2_txq_completion(struct qed_hwfn *p_hwfn, void *p_cookie)
+{
+	struct qed_ll2_info *p_ll2_conn = p_cookie;
+	struct qed_ll2_tx_queue *p_tx = &p_ll2_conn->tx_queue;
+	u16 new_idx = 0, num_bds = 0, num_bds_in_packet = 0;
+	struct qed_ll2_tx_packet *p_pkt;
+	bool b_last_frag = false;
+	unsigned long flags;
+	dma_addr_t tx_frag;
+	int rc = -EINVAL;
+
+	spin_lock_irqsave(&p_tx->lock, flags);
+	if (p_tx->b_completing_packet) {
+		rc = -EBUSY;
+		goto out;
+	}
+
+	new_idx = le16_to_cpu(*p_tx->p_fw_cons);
+	num_bds = ((s16)new_idx - (s16)p_tx->bds_idx);
+	while (num_bds) {
+		if (list_empty(&p_tx->active_descq))
+			goto out;
+
+		p_pkt = list_first_entry(&p_tx->active_descq,
+					 struct qed_ll2_tx_packet, list_entry);
+		if (!p_pkt)
+			goto out;
+
+		p_tx->b_completing_packet = true;
+		p_tx->cur_completing_packet = *p_pkt;
+		num_bds_in_packet = p_pkt->bd_used;
+		list_del(&p_pkt->list_entry);
+
+		if (num_bds < num_bds_in_packet) {
+			DP_NOTICE(p_hwfn,
+				  "Rest of BDs does not cover whole packet\n");
+			goto out;
+		}
+
+		num_bds -= num_bds_in_packet;
+		p_tx->bds_idx += num_bds_in_packet;
+		while (num_bds_in_packet--)
+			qed_chain_consume(&p_tx->txq_chain);
+
+		p_tx->cur_completing_bd_idx = 1;
+		b_last_frag = p_tx->cur_completing_bd_idx == p_pkt->bd_used;
+		list_add_tail(&p_pkt->list_entry, &p_tx->free_descq);
+
+		spin_unlock_irqrestore(&p_tx->lock, flags);
+		tx_frag = p_pkt->bds_set[0].tx_frag;
+		if (p_ll2_conn->gsi_enable)
+			qed_ll2b_complete_tx_gsi_packet(p_hwfn,
+							p_ll2_conn->my_id,
+							p_pkt->cookie,
+							tx_frag,
+							b_last_frag, !num_bds);
+		else
+			qed_ll2b_complete_tx_packet(p_hwfn,
+						    p_ll2_conn->my_id,
+						    p_pkt->cookie,
+						    tx_frag,
+						    b_last_frag, !num_bds);
+		spin_lock_irqsave(&p_tx->lock, flags);
+	}
+
+	p_tx->b_completing_packet = false;
+	rc = 0;
+out:
+	spin_unlock_irqrestore(&p_tx->lock, flags);
+	return rc;
+}
+
+static int
+qed_ll2_rxq_completion_gsi(struct qed_hwfn *p_hwfn,
+			   struct qed_ll2_info *p_ll2_info,
+			   union core_rx_cqe_union *p_cqe,
+			   unsigned long lock_flags, bool b_last_cqe)
+{
+	struct qed_ll2_rx_queue *p_rx = &p_ll2_info->rx_queue;
+	struct qed_ll2_rx_packet *p_pkt = NULL;
+	u16 packet_length, parse_flags, vlan;
+	u32 src_mac_addrhi;
+	u16 src_mac_addrlo;
+
+	if (!list_empty(&p_rx->active_descq))
+		p_pkt = list_first_entry(&p_rx->active_descq,
+					 struct qed_ll2_rx_packet, list_entry);
+	if (!p_pkt) {
+		DP_NOTICE(p_hwfn,
+			  "GSI Rx completion but active_descq is empty\n");
+		return -EIO;
+	}
+
+	list_del(&p_pkt->list_entry);
+	parse_flags = le16_to_cpu(p_cqe->rx_cqe_gsi.parse_flags.flags);
+	packet_length = le16_to_cpu(p_cqe->rx_cqe_gsi.data_length);
+	vlan = le16_to_cpu(p_cqe->rx_cqe_gsi.vlan);
+	src_mac_addrhi = le32_to_cpu(p_cqe->rx_cqe_gsi.src_mac_addrhi);
+	src_mac_addrlo = le16_to_cpu(p_cqe->rx_cqe_gsi.src_mac_addrlo);
+	if (qed_chain_consume(&p_rx->rxq_chain) != p_pkt->rxq_bd)
+		DP_NOTICE(p_hwfn,
+			  "Mismatch between active_descq and the LL2 Rx chain\n");
+	list_add_tail(&p_pkt->list_entry, &p_rx->free_descq);
+
+	spin_unlock_irqrestore(&p_rx->lock, lock_flags);
+	qed_ll2b_complete_rx_gsi_packet(p_hwfn,
+					p_ll2_info->my_id,
+					p_pkt->cookie,
+					p_pkt->rx_buf_addr,
+					packet_length,
+					p_cqe->rx_cqe_gsi.data_length_error,
+					parse_flags,
+					vlan,
+					src_mac_addrhi,
+					src_mac_addrlo, b_last_cqe);
+	spin_lock_irqsave(&p_rx->lock, lock_flags);
+
+	return 0;
+}
+
+static int qed_ll2_rxq_completion_reg(struct qed_hwfn *p_hwfn,
+				      struct qed_ll2_info *p_ll2_conn,
+				      union core_rx_cqe_union *p_cqe,
+				      unsigned long lock_flags,
+				      bool b_last_cqe)
+{
+	struct qed_ll2_rx_queue *p_rx = &p_ll2_conn->rx_queue;
+	struct qed_ll2_rx_packet *p_pkt = NULL;
+
+	if (!list_empty(&p_rx->active_descq))
+		p_pkt = list_first_entry(&p_rx->active_descq,
+					 struct qed_ll2_rx_packet, list_entry);
+	if (!p_pkt) {
+		DP_NOTICE(p_hwfn,
+			  "LL2 Rx completion but active_descq is empty\n");
+		return -EIO;
+	}
+	list_del(&p_pkt->list_entry);
+
+	if (qed_chain_consume(&p_rx->rxq_chain) != p_pkt->rxq_bd)
+		DP_NOTICE(p_hwfn,
+			  "Mismatch between active_descq and the LL2 Rx chain\n");
+	list_add_tail(&p_pkt->list_entry, &p_rx->free_descq);
+
+	spin_unlock_irqrestore(&p_rx->lock, lock_flags);
+	qed_ll2b_complete_rx_packet(p_hwfn, p_ll2_conn->my_id,
+				    p_pkt, &p_cqe->rx_cqe_fp, b_last_cqe);
+	spin_lock_irqsave(&p_rx->lock, lock_flags);
+
+	return 0;
+}
+
+static int qed_ll2_rxq_completion(struct qed_hwfn *p_hwfn, void *cookie)
+{
+	struct qed_ll2_info *p_ll2_conn = cookie;
+	struct qed_ll2_rx_queue *p_rx = &p_ll2_conn->rx_queue;
+	union core_rx_cqe_union *cqe = NULL;
+	u16 cq_new_idx = 0, cq_old_idx = 0;
+	unsigned long flags = 0;
+	int rc = 0;
+
+	spin_lock_irqsave(&p_rx->lock, flags);
+	cq_new_idx = le16_to_cpu(*p_rx->p_fw_cons);
+	cq_old_idx = qed_chain_get_cons_idx(&p_rx->rcq_chain);
+
+	while (cq_new_idx != cq_old_idx) {
+		bool b_last_cqe = (cq_new_idx == cq_old_idx);
+
+		cqe = qed_chain_consume(&p_rx->rcq_chain);
+		cq_old_idx = qed_chain_get_cons_idx(&p_rx->rcq_chain);
+
+		DP_VERBOSE(p_hwfn,
+			   QED_MSG_LL2,
+			   "LL2 [sw. cons %04x, fw. at %04x] - Got Packet of type %02x\n",
+			   cq_old_idx, cq_new_idx, cqe->rx_cqe_sp.type);
+
+		switch (cqe->rx_cqe_sp.type) {
+		case CORE_RX_CQE_TYPE_SLOW_PATH:
+			DP_NOTICE(p_hwfn, "LL2 - unexpected Rx CQE slowpath\n");
+			rc = -EINVAL;
+			break;
+		case CORE_RX_CQE_TYPE_GSI_OFFLOAD:
+			rc = qed_ll2_rxq_completion_gsi(p_hwfn, p_ll2_conn,
+							cqe, flags, b_last_cqe);
+			break;
+		case CORE_RX_CQE_TYPE_REGULAR:
+			rc = qed_ll2_rxq_completion_reg(p_hwfn, p_ll2_conn,
+							cqe, flags, b_last_cqe);
+			break;
+		default:
+			rc = -EIO;
+		}
+	}
+
+	spin_unlock_irqrestore(&p_rx->lock, flags);
+	return rc;
+}
+
+void qed_ll2_rxq_flush(struct qed_hwfn *p_hwfn, u8 connection_handle)
+{
+	struct qed_ll2_info *p_ll2_conn = NULL;
+	struct qed_ll2_rx_packet *p_pkt = NULL;
+	struct qed_ll2_rx_queue *p_rx;
+
+	p_ll2_conn = qed_ll2_handle_sanity_inactive(p_hwfn, connection_handle);
+	if (!p_ll2_conn)
+		return;
+
+	p_rx = &p_ll2_conn->rx_queue;
+
+	while (!list_empty(&p_rx->active_descq)) {
+		dma_addr_t rx_buf_addr;
+		void *cookie;
+		bool b_last;
+
+		p_pkt = list_first_entry(&p_rx->active_descq,
+					 struct qed_ll2_rx_packet, list_entry);
+		if (!p_pkt)
+			break;
+
+		list_del(&p_pkt->list_entry);
+		list_add_tail(&p_pkt->list_entry, &p_rx->free_descq);
+
+		rx_buf_addr = p_pkt->rx_buf_addr;
+		cookie = p_pkt->cookie;
+
+		b_last = list_empty(&p_rx->active_descq);
+	}
+}
+
+static int qed_sp_ll2_rx_queue_start(struct qed_hwfn *p_hwfn,
+				     struct qed_ll2_info *p_ll2_conn,
+				     u8 action_on_error)
+{
+	enum qed_ll2_conn_type conn_type = p_ll2_conn->conn_type;
+	struct qed_ll2_rx_queue *p_rx = &p_ll2_conn->rx_queue;
+	struct core_rx_start_ramrod_data *p_ramrod = NULL;
+	struct qed_spq_entry *p_ent = NULL;
+	struct qed_sp_init_data init_data;
+	u16 cqe_pbl_size;
+	int rc = 0;
+
+	/* Get SPQ entry */
+	memset(&init_data, 0, sizeof(init_data));
+	init_data.cid = p_ll2_conn->cid;
+	init_data.opaque_fid = p_hwfn->hw_info.opaque_fid;
+	init_data.comp_mode = QED_SPQ_MODE_EBLOCK;
+
+	rc = qed_sp_init_request(p_hwfn, &p_ent,
+				 CORE_RAMROD_RX_QUEUE_START,
+				 PROTOCOLID_CORE, &init_data);
+	if (rc)
+		return rc;
+
+	p_ramrod = &p_ent->ramrod.core_rx_queue_start;
+
+	p_ramrod->sb_id = cpu_to_le16(qed_int_get_sp_sb_id(p_hwfn));
+	p_ramrod->sb_index = p_rx->rx_sb_index;
+	p_ramrod->complete_event_flg = 1;
+
+	p_ramrod->mtu = cpu_to_le16(p_ll2_conn->mtu);
+	DMA_REGPAIR_LE(p_ramrod->bd_base,
+		       p_rx->rxq_chain.p_phys_addr);
+	cqe_pbl_size = (u16)qed_chain_get_page_cnt(&p_rx->rcq_chain);
+	p_ramrod->num_of_pbl_pages = cpu_to_le16(cqe_pbl_size);
+	DMA_REGPAIR_LE(p_ramrod->cqe_pbl_addr,
+		       qed_chain_get_pbl_phys(&p_rx->rcq_chain));
+
+	p_ramrod->drop_ttl0_flg = p_ll2_conn->rx_drop_ttl0_flg;
+	p_ramrod->inner_vlan_removal_en = p_ll2_conn->rx_vlan_removal_en;
+	p_ramrod->queue_id = p_ll2_conn->queue_id;
+	p_ramrod->main_func_queue = 1;
+
+	if ((IS_MF_DEFAULT(p_hwfn) || IS_MF_SI(p_hwfn)) &&
+	    p_ramrod->main_func_queue && (conn_type != QED_LL2_TYPE_ROCE)) {
+		p_ramrod->mf_si_bcast_accept_all = 1;
+		p_ramrod->mf_si_mcast_accept_all = 1;
+	} else {
+		p_ramrod->mf_si_bcast_accept_all = 0;
+		p_ramrod->mf_si_mcast_accept_all = 0;
+	}
+
+	p_ramrod->action_on_error.error_type = action_on_error;
+	p_ramrod->gsi_offload_flag = p_ll2_conn->gsi_enable;
+	return qed_spq_post(p_hwfn, p_ent, NULL);
+}
+
+static int qed_sp_ll2_tx_queue_start(struct qed_hwfn *p_hwfn,
+				     struct qed_ll2_info *p_ll2_conn)
+{
+	enum qed_ll2_conn_type conn_type = p_ll2_conn->conn_type;
+	struct qed_ll2_tx_queue *p_tx = &p_ll2_conn->tx_queue;
+	struct core_tx_start_ramrod_data *p_ramrod = NULL;
+	struct qed_spq_entry *p_ent = NULL;
+	struct qed_sp_init_data init_data;
+	union qed_qm_pq_params pq_params;
+	u16 pq_id = 0, pbl_size;
+	int rc = -EINVAL;
+
+	if (!QED_LL2_TX_REGISTERED(p_ll2_conn))
+		return 0;
+
+	/* Get SPQ entry */
+	memset(&init_data, 0, sizeof(init_data));
+	init_data.cid = p_ll2_conn->cid;
+	init_data.opaque_fid = p_hwfn->hw_info.opaque_fid;
+	init_data.comp_mode = QED_SPQ_MODE_EBLOCK;
+
+	rc = qed_sp_init_request(p_hwfn, &p_ent,
+				 CORE_RAMROD_TX_QUEUE_START,
+				 PROTOCOLID_CORE, &init_data);
+	if (rc)
+		return rc;
+
+	p_ramrod = &p_ent->ramrod.core_tx_queue_start;
+
+	p_ramrod->sb_id = cpu_to_le16(qed_int_get_sp_sb_id(p_hwfn));
+	p_ramrod->sb_index = p_tx->tx_sb_index;
+	p_ramrod->mtu = cpu_to_le16(p_ll2_conn->mtu);
+	p_ll2_conn->tx_stats_en = 1;
+	p_ramrod->stats_en = p_ll2_conn->tx_stats_en;
+	p_ramrod->stats_id = p_ll2_conn->tx_stats_id;
+
+	DMA_REGPAIR_LE(p_ramrod->pbl_base_addr,
+		       qed_chain_get_pbl_phys(&p_tx->txq_chain));
+	pbl_size = qed_chain_get_page_cnt(&p_tx->txq_chain);
+	p_ramrod->pbl_size = cpu_to_le16(pbl_size);
+
+	memset(&pq_params, 0, sizeof(pq_params));
+	pq_params.core.tc = p_ll2_conn->tx_tc;
+	pq_id = qed_get_qm_pq(p_hwfn, PROTOCOLID_CORE, &pq_params);
+	p_ramrod->qm_pq_id = cpu_to_le16(pq_id);
+
+	switch (conn_type) {
+	case QED_LL2_TYPE_ISCSI:
+	case QED_LL2_TYPE_ISCSI_OOO:
+		p_ramrod->conn_type = PROTOCOLID_ISCSI;
+		break;
+	case QED_LL2_TYPE_ROCE:
+		p_ramrod->conn_type = PROTOCOLID_ROCE;
+		break;
+	default:
+		p_ramrod->conn_type = PROTOCOLID_ETH;
+		DP_NOTICE(p_hwfn, "Unknown connection type: %d\n", conn_type);
+	}
+
+	p_ramrod->gsi_offload_flag = p_ll2_conn->gsi_enable;
+	return qed_spq_post(p_hwfn, p_ent, NULL);
+}
+
+static int qed_sp_ll2_rx_queue_stop(struct qed_hwfn *p_hwfn,
+				    struct qed_ll2_info *p_ll2_conn)
+{
+	struct core_rx_stop_ramrod_data *p_ramrod = NULL;
+	struct qed_spq_entry *p_ent = NULL;
+	struct qed_sp_init_data init_data;
+	int rc = -EINVAL;
+
+	/* Get SPQ entry */
+	memset(&init_data, 0, sizeof(init_data));
+	init_data.cid = p_ll2_conn->cid;
+	init_data.opaque_fid = p_hwfn->hw_info.opaque_fid;
+	init_data.comp_mode = QED_SPQ_MODE_EBLOCK;
+
+	rc = qed_sp_init_request(p_hwfn, &p_ent,
+				 CORE_RAMROD_RX_QUEUE_STOP,
+				 PROTOCOLID_CORE, &init_data);
+	if (rc)
+		return rc;
+
+	p_ramrod = &p_ent->ramrod.core_rx_queue_stop;
+
+	p_ramrod->complete_event_flg = 1;
+	p_ramrod->queue_id = p_ll2_conn->queue_id;
+
+	return qed_spq_post(p_hwfn, p_ent, NULL);
+}
+
+static int qed_sp_ll2_tx_queue_stop(struct qed_hwfn *p_hwfn,
+				    struct qed_ll2_info *p_ll2_conn)
+{
+	struct qed_spq_entry *p_ent = NULL;
+	struct qed_sp_init_data init_data;
+	int rc = -EINVAL;
+
+	/* Get SPQ entry */
+	memset(&init_data, 0, sizeof(init_data));
+	init_data.cid = p_ll2_conn->cid;
+	init_data.opaque_fid = p_hwfn->hw_info.opaque_fid;
+	init_data.comp_mode = QED_SPQ_MODE_EBLOCK;
+
+	rc = qed_sp_init_request(p_hwfn, &p_ent,
+				 CORE_RAMROD_TX_QUEUE_STOP,
+				 PROTOCOLID_CORE, &init_data);
+	if (rc)
+		return rc;
+
+	return qed_spq_post(p_hwfn, p_ent, NULL);
+}
+
+static int
+qed_ll2_acquire_connection_rx(struct qed_hwfn *p_hwfn,
+			      struct qed_ll2_info *p_ll2_info, u16 rx_num_desc)
+{
+	struct qed_ll2_rx_packet *p_descq;
+	u32 capacity;
+	int rc = 0;
+
+	if (!rx_num_desc)
+		goto out;
+
+	rc = qed_chain_alloc(p_hwfn->cdev,
+			     QED_CHAIN_USE_TO_CONSUME_PRODUCE,
+			     QED_CHAIN_MODE_NEXT_PTR,
+			     QED_CHAIN_CNT_TYPE_U16,
+			     rx_num_desc,
+			     sizeof(struct core_rx_bd),
+			     &p_ll2_info->rx_queue.rxq_chain);
+	if (rc) {
+		DP_NOTICE(p_hwfn, "Failed to allocate ll2 rxq chain\n");
+		goto out;
+	}
+
+	capacity = qed_chain_get_capacity(&p_ll2_info->rx_queue.rxq_chain);
+	p_descq = kcalloc(capacity, sizeof(struct qed_ll2_rx_packet),
+			  GFP_KERNEL);
+	if (!p_descq) {
+		rc = -ENOMEM;
+		DP_NOTICE(p_hwfn, "Failed to allocate ll2 Rx desc\n");
+		goto out;
+	}
+	p_ll2_info->rx_queue.descq_array = p_descq;
+
+	rc = qed_chain_alloc(p_hwfn->cdev,
+			     QED_CHAIN_USE_TO_CONSUME_PRODUCE,
+			     QED_CHAIN_MODE_PBL,
+			     QED_CHAIN_CNT_TYPE_U16,
+			     rx_num_desc,
+			     sizeof(struct core_rx_fast_path_cqe),
+			     &p_ll2_info->rx_queue.rcq_chain);
+	if (rc) {
+		DP_NOTICE(p_hwfn, "Failed to allocate ll2 rcq chain\n");
+		goto out;
+	}
+
+	DP_VERBOSE(p_hwfn, QED_MSG_LL2,
+		   "Allocated LL2 Rxq [Type %08x] with 0x%08x buffers\n",
+		   p_ll2_info->conn_type, rx_num_desc);
+
+out:
+	return rc;
+}
+
+static int qed_ll2_acquire_connection_tx(struct qed_hwfn *p_hwfn,
+					 struct qed_ll2_info *p_ll2_info,
+					 u16 tx_num_desc)
+{
+	struct qed_ll2_tx_packet *p_descq;
+	u32 capacity;
+	int rc = 0;
+
+	if (!tx_num_desc)
+		goto out;
+
+	rc = qed_chain_alloc(p_hwfn->cdev,
+			     QED_CHAIN_USE_TO_CONSUME_PRODUCE,
+			     QED_CHAIN_MODE_PBL,
+			     QED_CHAIN_CNT_TYPE_U16,
+			     tx_num_desc,
+			     sizeof(struct core_tx_bd),
+			     &p_ll2_info->tx_queue.txq_chain);
+	if (rc)
+		goto out;
+
+	capacity = qed_chain_get_capacity(&p_ll2_info->tx_queue.txq_chain);
+	p_descq = kcalloc(capacity, sizeof(struct qed_ll2_tx_packet),
+			  GFP_KERNEL);
+	if (!p_descq) {
+		rc = -ENOMEM;
+		goto out;
+	}
+	p_ll2_info->tx_queue.descq_array = p_descq;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_LL2,
+		   "Allocated LL2 Txq [Type %08x] with 0x%08x buffers\n",
+		   p_ll2_info->conn_type, tx_num_desc);
+
+out:
+	if (rc)
+		DP_NOTICE(p_hwfn,
+			  "Can't allocate memory for Tx LL2 with 0x%08x buffers\n",
+			  tx_num_desc);
+	return rc;
+}
+
+int qed_ll2_acquire_connection(struct qed_hwfn *p_hwfn,
+			       struct qed_ll2_info *p_params,
+			       u16 rx_num_desc,
+			       u16 tx_num_desc,
+			       u8 *p_connection_handle)
+{
+	qed_int_comp_cb_t comp_rx_cb, comp_tx_cb;
+	struct qed_ll2_info *p_ll2_info = NULL;
+	int rc;
+	u8 i;
+
+	if (!p_connection_handle || !p_hwfn->p_ll2_info)
+		return -EINVAL;
+
+	/* Find a free connection to be used */
+	for (i = 0; (i < QED_MAX_NUM_OF_LL2_CONNECTIONS); i++) {
+		mutex_lock(&p_hwfn->p_ll2_info[i].mutex);
+		if (p_hwfn->p_ll2_info[i].b_active) {
+			mutex_unlock(&p_hwfn->p_ll2_info[i].mutex);
+			continue;
+		}
+
+		p_hwfn->p_ll2_info[i].b_active = true;
+		p_ll2_info = &p_hwfn->p_ll2_info[i];
+		mutex_unlock(&p_hwfn->p_ll2_info[i].mutex);
+		break;
+	}
+	if (!p_ll2_info)
+		return -EBUSY;
+
+	p_ll2_info->conn_type = p_params->conn_type;
+	p_ll2_info->mtu = p_params->mtu;
+	p_ll2_info->rx_drop_ttl0_flg = p_params->rx_drop_ttl0_flg;
+	p_ll2_info->rx_vlan_removal_en = p_params->rx_vlan_removal_en;
+	p_ll2_info->tx_tc = p_params->tx_tc;
+	p_ll2_info->tx_dest = p_params->tx_dest;
+	p_ll2_info->ai_err_packet_too_big = p_params->ai_err_packet_too_big;
+	p_ll2_info->ai_err_no_buf = p_params->ai_err_no_buf;
+	p_ll2_info->gsi_enable = p_params->gsi_enable;
+
+	rc = qed_ll2_acquire_connection_rx(p_hwfn, p_ll2_info, rx_num_desc);
+	if (rc)
+		goto q_allocate_fail;
+
+	rc = qed_ll2_acquire_connection_tx(p_hwfn, p_ll2_info, tx_num_desc);
+	if (rc)
+		goto q_allocate_fail;
+
+	/* Register callbacks for the Rx/Tx queues */
+	comp_rx_cb = qed_ll2_rxq_completion;
+	comp_tx_cb = qed_ll2_txq_completion;
+
+	if (rx_num_desc) {
+		qed_int_register_cb(p_hwfn, comp_rx_cb,
+				    &p_hwfn->p_ll2_info[i],
+				    &p_ll2_info->rx_queue.rx_sb_index,
+				    &p_ll2_info->rx_queue.p_fw_cons);
+		p_ll2_info->rx_queue.b_cb_registred = true;
+	}
+
+	if (tx_num_desc) {
+		qed_int_register_cb(p_hwfn,
+				    comp_tx_cb,
+				    &p_hwfn->p_ll2_info[i],
+				    &p_ll2_info->tx_queue.tx_sb_index,
+				    &p_ll2_info->tx_queue.p_fw_cons);
+		p_ll2_info->tx_queue.b_cb_registred = true;
+	}
+
+	*p_connection_handle = i;
+	return rc;
+
+q_allocate_fail:
+	qed_ll2_release_connection(p_hwfn, i);
+	return -ENOMEM;
+}
+
+static int qed_ll2_establish_connection_rx(struct qed_hwfn *p_hwfn,
+					   struct qed_ll2_info *p_ll2_conn)
+{
+	u8 action_on_error = 0;
+
+	if (!QED_LL2_RX_REGISTERED(p_ll2_conn))
+		return 0;
+
+	DIRECT_REG_WR(p_ll2_conn->rx_queue.set_prod_addr, 0x0);
+
+	SET_FIELD(action_on_error,
+		  CORE_RX_ACTION_ON_ERROR_PACKET_TOO_BIG,
+		  p_ll2_conn->ai_err_packet_too_big);
+	SET_FIELD(action_on_error,
+		  CORE_RX_ACTION_ON_ERROR_NO_BUFF, p_ll2_conn->ai_err_no_buf);
+
+	return qed_sp_ll2_rx_queue_start(p_hwfn, p_ll2_conn, action_on_error);
+}
+
+int qed_ll2_establish_connection(struct qed_hwfn *p_hwfn, u8 connection_handle)
+{
+	struct qed_ll2_info *p_ll2_conn;
+	struct qed_ll2_rx_queue *p_rx;
+	struct qed_ll2_tx_queue *p_tx;
+	int rc = -EINVAL;
+	u32 i, capacity;
+	u8 qid;
+
+	p_ll2_conn = qed_ll2_handle_sanity_lock(p_hwfn, connection_handle);
+	if (!p_ll2_conn)
+		return -EINVAL;
+	p_rx = &p_ll2_conn->rx_queue;
+	p_tx = &p_ll2_conn->tx_queue;
+
+	qed_chain_reset(&p_rx->rxq_chain);
+	qed_chain_reset(&p_rx->rcq_chain);
+	INIT_LIST_HEAD(&p_rx->active_descq);
+	INIT_LIST_HEAD(&p_rx->free_descq);
+	INIT_LIST_HEAD(&p_rx->posting_descq);
+	spin_lock_init(&p_rx->lock);
+	capacity = qed_chain_get_capacity(&p_rx->rxq_chain);
+	for (i = 0; i < capacity; i++)
+		list_add_tail(&p_rx->descq_array[i].list_entry,
+			      &p_rx->free_descq);
+	*p_rx->p_fw_cons = 0;
+
+	qed_chain_reset(&p_tx->txq_chain);
+	INIT_LIST_HEAD(&p_tx->active_descq);
+	INIT_LIST_HEAD(&p_tx->free_descq);
+	INIT_LIST_HEAD(&p_tx->sending_descq);
+	spin_lock_init(&p_tx->lock);
+	capacity = qed_chain_get_capacity(&p_tx->txq_chain);
+	for (i = 0; i < capacity; i++)
+		list_add_tail(&p_tx->descq_array[i].list_entry,
+			      &p_tx->free_descq);
+	p_tx->cur_completing_bd_idx = 0;
+	p_tx->bds_idx = 0;
+	p_tx->b_completing_packet = false;
+	p_tx->cur_send_packet = NULL;
+	p_tx->cur_send_frag_num = 0;
+	p_tx->cur_completing_frag_num = 0;
+	*p_tx->p_fw_cons = 0;
+
+	qed_cxt_acquire_cid(p_hwfn, PROTOCOLID_CORE, &p_ll2_conn->cid);
+
+	qid = p_hwfn->hw_info.resc_start[QED_LL2_QUEUE] + connection_handle;
+	p_ll2_conn->queue_id = qid;
+	p_ll2_conn->tx_stats_id = qid;
+	p_rx->set_prod_addr = (u8 __iomem *)p_hwfn->regview +
+					    GTT_BAR0_MAP_REG_TSDM_RAM +
+					    TSTORM_LL2_RX_PRODS_OFFSET(qid);
+	p_tx->doorbell_addr = (u8 __iomem *)p_hwfn->doorbells +
+					    qed_db_addr(p_ll2_conn->cid,
+							DQ_DEMS_LEGACY);
+
+	rc = qed_ll2_establish_connection_rx(p_hwfn, p_ll2_conn);
+	if (rc)
+		return rc;
+
+	rc = qed_sp_ll2_tx_queue_start(p_hwfn, p_ll2_conn);
+	if (rc)
+		return rc;
+
+	if (p_hwfn->hw_info.personality != QED_PCI_ETH_ROCE)
+		qed_wr(p_hwfn, p_hwfn->p_main_ptt, PRS_REG_USE_LIGHT_L2, 1);
+
+	return rc;
+}
+
+static void qed_ll2_post_rx_buffer_notify_fw(struct qed_hwfn *p_hwfn,
+					     struct qed_ll2_rx_queue *p_rx,
+					     struct qed_ll2_rx_packet *p_curp)
+{
+	struct qed_ll2_rx_packet *p_posting_packet = NULL;
+	struct core_ll2_rx_prod rx_prod = { 0, 0, 0 };
+	bool b_notify_fw = false;
+	u16 bd_prod, cq_prod;
+
+	/* This handles the flushing of already posted buffers */
+	while (!list_empty(&p_rx->posting_descq)) {
+		p_posting_packet = list_first_entry(&p_rx->posting_descq,
+						    struct qed_ll2_rx_packet,
+						    list_entry);
+		list_del(&p_posting_packet->list_entry);
+		list_add_tail(&p_posting_packet->list_entry,
+			      &p_rx->active_descq);
+		b_notify_fw = true;
+	}
+
+	/* This handles the supplied packet [if there is one] */
+	if (p_curp) {
+		list_add_tail(&p_curp->list_entry, &p_rx->active_descq);
+		b_notify_fw = true;
+	}
+
+	if (!b_notify_fw)
+		return;
+
+	bd_prod = qed_chain_get_prod_idx(&p_rx->rxq_chain);
+	cq_prod = qed_chain_get_prod_idx(&p_rx->rcq_chain);
+	rx_prod.bd_prod = cpu_to_le16(bd_prod);
+	rx_prod.cqe_prod = cpu_to_le16(cq_prod);
+	DIRECT_REG_WR(p_rx->set_prod_addr, *((u32 *)&rx_prod));
+}
+
+int qed_ll2_post_rx_buffer(struct qed_hwfn *p_hwfn,
+			   u8 connection_handle,
+			   dma_addr_t addr,
+			   u16 buf_len, void *cookie, u8 notify_fw)
+{
+	struct core_rx_bd_with_buff_len *p_curb = NULL;
+	struct qed_ll2_rx_packet *p_curp = NULL;
+	struct qed_ll2_info *p_ll2_conn;
+	struct qed_ll2_rx_queue *p_rx;
+	unsigned long flags;
+	void *p_data;
+	int rc = 0;
+
+	p_ll2_conn = qed_ll2_handle_sanity(p_hwfn, connection_handle);
+	if (!p_ll2_conn)
+		return -EINVAL;
+	p_rx = &p_ll2_conn->rx_queue;
+
+	spin_lock_irqsave(&p_rx->lock, flags);
+	if (!list_empty(&p_rx->free_descq))
+		p_curp = list_first_entry(&p_rx->free_descq,
+					  struct qed_ll2_rx_packet, list_entry);
+	if (p_curp) {
+		if (qed_chain_get_elem_left(&p_rx->rxq_chain) &&
+		    qed_chain_get_elem_left(&p_rx->rcq_chain)) {
+			p_data = qed_chain_produce(&p_rx->rxq_chain);
+			p_curb = (struct core_rx_bd_with_buff_len *)p_data;
+			qed_chain_produce(&p_rx->rcq_chain);
+		}
+	}
+
+	/* If we're lacking entires, let's try to flush buffers to FW */
+	if (!p_curp || !p_curb) {
+		rc = -EBUSY;
+		p_curp = NULL;
+		goto out_notify;
+	}
+
+	/* We have an Rx packet we can fill */
+	DMA_REGPAIR_LE(p_curb->addr, addr);
+	p_curb->buff_length = cpu_to_le16(buf_len);
+	p_curp->rx_buf_addr = addr;
+	p_curp->cookie = cookie;
+	p_curp->rxq_bd = p_curb;
+	p_curp->buf_length = buf_len;
+	list_del(&p_curp->list_entry);
+
+	/* Check if we only want to enqueue this packet without informing FW */
+	if (!notify_fw) {
+		list_add_tail(&p_curp->list_entry, &p_rx->posting_descq);
+		goto out;
+	}
+
+out_notify:
+	qed_ll2_post_rx_buffer_notify_fw(p_hwfn, p_rx, p_curp);
+out:
+	spin_unlock_irqrestore(&p_rx->lock, flags);
+	return rc;
+}
+
+static void qed_ll2_prepare_tx_packet_set(struct qed_hwfn *p_hwfn,
+					  struct qed_ll2_tx_queue *p_tx,
+					  struct qed_ll2_tx_packet *p_curp,
+					  u8 num_of_bds,
+					  dma_addr_t first_frag,
+					  u16 first_frag_len, void *p_cookie,
+					  u8 notify_fw)
+{
+	list_del(&p_curp->list_entry);
+	p_curp->cookie = p_cookie;
+	p_curp->bd_used = num_of_bds;
+	p_curp->notify_fw = notify_fw;
+	p_tx->cur_send_packet = p_curp;
+	p_tx->cur_send_frag_num = 0;
+
+	p_curp->bds_set[p_tx->cur_send_frag_num].tx_frag = first_frag;
+	p_curp->bds_set[p_tx->cur_send_frag_num].frag_len = first_frag_len;
+	p_tx->cur_send_frag_num++;
+}
+
+static void qed_ll2_prepare_tx_packet_set_bd(struct qed_hwfn *p_hwfn,
+					     struct qed_ll2_info *p_ll2,
+					     struct qed_ll2_tx_packet *p_curp,
+					     u8 num_of_bds,
+					     enum core_tx_dest tx_dest,
+					     u16 vlan,
+					     u8 bd_flags,
+					     u16 l4_hdr_offset_w,
+					     enum core_roce_flavor_type type,
+					     dma_addr_t first_frag,
+					     u16 first_frag_len)
+{
+	struct qed_chain *p_tx_chain = &p_ll2->tx_queue.txq_chain;
+	u16 prod_idx = qed_chain_get_prod_idx(p_tx_chain);
+	struct core_tx_bd *start_bd = NULL;
+	u16 frag_idx;
+
+	start_bd = (struct core_tx_bd *)qed_chain_produce(p_tx_chain);
+	start_bd->nw_vlan_or_lb_echo = cpu_to_le16(vlan);
+	SET_FIELD(start_bd->bitfield1, CORE_TX_BD_L4_HDR_OFFSET_W,
+		  cpu_to_le16(l4_hdr_offset_w));
+	SET_FIELD(start_bd->bitfield1, CORE_TX_BD_TX_DST, tx_dest);
+	start_bd->bd_flags.as_bitfield = bd_flags;
+	start_bd->bd_flags.as_bitfield |= CORE_TX_BD_FLAGS_START_BD_MASK <<
+	    CORE_TX_BD_FLAGS_START_BD_SHIFT;
+	SET_FIELD(start_bd->bitfield0, CORE_TX_BD_NBDS, num_of_bds);
+	DMA_REGPAIR_LE(start_bd->addr, first_frag);
+	start_bd->nbytes = cpu_to_le16(first_frag_len);
+
+	SET_FIELD(start_bd->bd_flags.as_bitfield, CORE_TX_BD_FLAGS_ROCE_FLAV,
+		  type);
+
+	DP_VERBOSE(p_hwfn,
+		   (NETIF_MSG_TX_QUEUED | QED_MSG_LL2),
+		   "LL2 [q 0x%02x cid 0x%08x type 0x%08x] Tx Producer at [0x%04x] - set with a %04x bytes %02x BDs buffer at %08x:%08x\n",
+		   p_ll2->queue_id,
+		   p_ll2->cid,
+		   p_ll2->conn_type,
+		   prod_idx,
+		   first_frag_len,
+		   num_of_bds,
+		   le32_to_cpu(start_bd->addr.hi),
+		   le32_to_cpu(start_bd->addr.lo));
+
+	if (p_ll2->tx_queue.cur_send_frag_num == num_of_bds)
+		return;
+
+	/* Need to provide the packet with additional BDs for frags */
+	for (frag_idx = p_ll2->tx_queue.cur_send_frag_num;
+	     frag_idx < num_of_bds; frag_idx++) {
+		struct core_tx_bd **p_bd = &p_curp->bds_set[frag_idx].txq_bd;
+
+		*p_bd = (struct core_tx_bd *)qed_chain_produce(p_tx_chain);
+		(*p_bd)->bd_flags.as_bitfield = 0;
+		(*p_bd)->bitfield1 = 0;
+		(*p_bd)->bitfield0 = 0;
+		p_curp->bds_set[frag_idx].tx_frag = 0;
+		p_curp->bds_set[frag_idx].frag_len = 0;
+	}
+}
+
+/* This should be called while the Txq spinlock is being held */
+static void qed_ll2_tx_packet_notify(struct qed_hwfn *p_hwfn,
+				     struct qed_ll2_info *p_ll2_conn)
+{
+	bool b_notify = p_ll2_conn->tx_queue.cur_send_packet->notify_fw;
+	struct qed_ll2_tx_queue *p_tx = &p_ll2_conn->tx_queue;
+	struct qed_ll2_tx_packet *p_pkt = NULL;
+	struct core_db_data db_msg = { 0, 0, 0 };
+	u16 bd_prod;
+
+	/* If there are missing BDs, don't do anything now */
+	if (p_ll2_conn->tx_queue.cur_send_frag_num !=
+	    p_ll2_conn->tx_queue.cur_send_packet->bd_used)
+		return;
+
+	/* Push the current packet to the list and clean after it */
+	list_add_tail(&p_ll2_conn->tx_queue.cur_send_packet->list_entry,
+		      &p_ll2_conn->tx_queue.sending_descq);
+	p_ll2_conn->tx_queue.cur_send_packet = NULL;
+	p_ll2_conn->tx_queue.cur_send_frag_num = 0;
+
+	/* Notify FW of packet only if requested to */
+	if (!b_notify)
+		return;
+
+	bd_prod = qed_chain_get_prod_idx(&p_ll2_conn->tx_queue.txq_chain);
+
+	while (!list_empty(&p_tx->sending_descq)) {
+		p_pkt = list_first_entry(&p_tx->sending_descq,
+					 struct qed_ll2_tx_packet, list_entry);
+		if (!p_pkt)
+			break;
+
+		list_del(&p_pkt->list_entry);
+		list_add_tail(&p_pkt->list_entry, &p_tx->active_descq);
+	}
+
+	SET_FIELD(db_msg.params, CORE_DB_DATA_DEST, DB_DEST_XCM);
+	SET_FIELD(db_msg.params, CORE_DB_DATA_AGG_CMD, DB_AGG_CMD_SET);
+	SET_FIELD(db_msg.params, CORE_DB_DATA_AGG_VAL_SEL,
+		  DQ_XCM_CORE_TX_BD_PROD_CMD);
+	db_msg.agg_flags = DQ_XCM_CORE_DQ_CF_CMD;
+	db_msg.spq_prod = cpu_to_le16(bd_prod);
+
+	/* Make sure the BDs data is updated before ringing the doorbell */
+	wmb();
+
+	DIRECT_REG_WR(p_tx->doorbell_addr, *((u32 *)&db_msg));
+
+	DP_VERBOSE(p_hwfn,
+		   (NETIF_MSG_TX_QUEUED | QED_MSG_LL2),
+		   "LL2 [q 0x%02x cid 0x%08x type 0x%08x] Doorbelled [producer 0x%04x]\n",
+		   p_ll2_conn->queue_id,
+		   p_ll2_conn->cid, p_ll2_conn->conn_type, db_msg.spq_prod);
+}
+
+int qed_ll2_prepare_tx_packet(struct qed_hwfn *p_hwfn,
+			      u8 connection_handle,
+			      u8 num_of_bds,
+			      u16 vlan,
+			      u8 bd_flags,
+			      u16 l4_hdr_offset_w,
+			      enum qed_ll2_roce_flavor_type qed_roce_flavor,
+			      dma_addr_t first_frag,
+			      u16 first_frag_len, void *cookie, u8 notify_fw)
+{
+	struct qed_ll2_tx_packet *p_curp = NULL;
+	struct qed_ll2_info *p_ll2_conn = NULL;
+	enum core_roce_flavor_type roce_flavor;
+	struct qed_ll2_tx_queue *p_tx;
+	struct qed_chain *p_tx_chain;
+	unsigned long flags;
+	int rc = 0;
+
+	p_ll2_conn = qed_ll2_handle_sanity(p_hwfn, connection_handle);
+	if (!p_ll2_conn)
+		return -EINVAL;
+	p_tx = &p_ll2_conn->tx_queue;
+	p_tx_chain = &p_tx->txq_chain;
+
+	if (num_of_bds > CORE_LL2_TX_MAX_BDS_PER_PACKET)
+		return -EIO;
+
+	spin_lock_irqsave(&p_tx->lock, flags);
+	if (p_tx->cur_send_packet) {
+		rc = -EEXIST;
+		goto out;
+	}
+
+	/* Get entry, but only if we have tx elements for it */
+	if (!list_empty(&p_tx->free_descq))
+		p_curp = list_first_entry(&p_tx->free_descq,
+					  struct qed_ll2_tx_packet, list_entry);
+	if (p_curp && qed_chain_get_elem_left(p_tx_chain) < num_of_bds)
+		p_curp = NULL;
+
+	if (!p_curp) {
+		rc = -EBUSY;
+		goto out;
+	}
+
+	if (qed_roce_flavor == QED_LL2_ROCE) {
+		roce_flavor = CORE_ROCE;
+	} else if (qed_roce_flavor == QED_LL2_RROCE) {
+		roce_flavor = CORE_RROCE;
+	} else {
+		rc = -EINVAL;
+		goto out;
+	}
+
+	/* Prepare packet and BD, and perhaps send a doorbell to FW */
+	qed_ll2_prepare_tx_packet_set(p_hwfn, p_tx, p_curp,
+				      num_of_bds, first_frag,
+				      first_frag_len, cookie, notify_fw);
+	qed_ll2_prepare_tx_packet_set_bd(p_hwfn, p_ll2_conn, p_curp,
+					 num_of_bds, CORE_TX_DEST_NW,
+					 vlan, bd_flags, l4_hdr_offset_w,
+					 roce_flavor,
+					 first_frag, first_frag_len);
+
+	qed_ll2_tx_packet_notify(p_hwfn, p_ll2_conn);
+
+out:
+	spin_unlock_irqrestore(&p_tx->lock, flags);
+	return rc;
+}
+
+int qed_ll2_set_fragment_of_tx_packet(struct qed_hwfn *p_hwfn,
+				      u8 connection_handle,
+				      dma_addr_t addr, u16 nbytes)
+{
+	struct qed_ll2_tx_packet *p_cur_send_packet = NULL;
+	struct qed_ll2_info *p_ll2_conn = NULL;
+	u16 cur_send_frag_num = 0;
+	struct core_tx_bd *p_bd;
+	unsigned long flags;
+
+	p_ll2_conn = qed_ll2_handle_sanity(p_hwfn, connection_handle);
+	if (!p_ll2_conn)
+		return -EINVAL;
+
+	if (!p_ll2_conn->tx_queue.cur_send_packet)
+		return -EINVAL;
+
+	p_cur_send_packet = p_ll2_conn->tx_queue.cur_send_packet;
+	cur_send_frag_num = p_ll2_conn->tx_queue.cur_send_frag_num;
+
+	if (cur_send_frag_num >= p_cur_send_packet->bd_used)
+		return -EINVAL;
+
+	/* Fill the BD information, and possibly notify FW */
+	p_bd = p_cur_send_packet->bds_set[cur_send_frag_num].txq_bd;
+	DMA_REGPAIR_LE(p_bd->addr, addr);
+	p_bd->nbytes = cpu_to_le16(nbytes);
+	p_cur_send_packet->bds_set[cur_send_frag_num].tx_frag = addr;
+	p_cur_send_packet->bds_set[cur_send_frag_num].frag_len = nbytes;
+
+	p_ll2_conn->tx_queue.cur_send_frag_num++;
+
+	spin_lock_irqsave(&p_ll2_conn->tx_queue.lock, flags);
+	qed_ll2_tx_packet_notify(p_hwfn, p_ll2_conn);
+	spin_unlock_irqrestore(&p_ll2_conn->tx_queue.lock, flags);
+
+	return 0;
+}
+
+int qed_ll2_terminate_connection(struct qed_hwfn *p_hwfn, u8 connection_handle)
+{
+	struct qed_ll2_info *p_ll2_conn = NULL;
+	int rc = -EINVAL;
+
+	p_ll2_conn = qed_ll2_handle_sanity_lock(p_hwfn, connection_handle);
+	if (!p_ll2_conn)
+		return -EINVAL;
+
+	/* Stop Tx & Rx of connection, if needed */
+	if (QED_LL2_TX_REGISTERED(p_ll2_conn)) {
+		rc = qed_sp_ll2_tx_queue_stop(p_hwfn, p_ll2_conn);
+		if (rc)
+			return rc;
+		qed_ll2_txq_flush(p_hwfn, connection_handle);
+	}
+
+	if (QED_LL2_RX_REGISTERED(p_ll2_conn)) {
+		rc = qed_sp_ll2_rx_queue_stop(p_hwfn, p_ll2_conn);
+		if (rc)
+			return rc;
+		qed_ll2_rxq_flush(p_hwfn, connection_handle);
+	}
+
+	return rc;
+}
+
+void qed_ll2_release_connection(struct qed_hwfn *p_hwfn, u8 connection_handle)
+{
+	struct qed_ll2_info *p_ll2_conn = NULL;
+
+	p_ll2_conn = qed_ll2_handle_sanity(p_hwfn, connection_handle);
+	if (!p_ll2_conn)
+		return;
+
+	if (QED_LL2_RX_REGISTERED(p_ll2_conn)) {
+		p_ll2_conn->rx_queue.b_cb_registred = false;
+		qed_int_unregister_cb(p_hwfn, p_ll2_conn->rx_queue.rx_sb_index);
+	}
+
+	if (QED_LL2_TX_REGISTERED(p_ll2_conn)) {
+		p_ll2_conn->tx_queue.b_cb_registred = false;
+		qed_int_unregister_cb(p_hwfn, p_ll2_conn->tx_queue.tx_sb_index);
+	}
+
+	kfree(p_ll2_conn->tx_queue.descq_array);
+	qed_chain_free(p_hwfn->cdev, &p_ll2_conn->tx_queue.txq_chain);
+
+	kfree(p_ll2_conn->rx_queue.descq_array);
+	qed_chain_free(p_hwfn->cdev, &p_ll2_conn->rx_queue.rxq_chain);
+	qed_chain_free(p_hwfn->cdev, &p_ll2_conn->rx_queue.rcq_chain);
+
+	qed_cxt_release_cid(p_hwfn, p_ll2_conn->cid);
+
+	mutex_lock(&p_ll2_conn->mutex);
+	p_ll2_conn->b_active = false;
+	mutex_unlock(&p_ll2_conn->mutex);
+}
+
+struct qed_ll2_info *qed_ll2_alloc(struct qed_hwfn *p_hwfn)
+{
+	struct qed_ll2_info *p_ll2_connections;
+	u8 i;
+
+	/* Allocate LL2's set struct */
+	p_ll2_connections = kcalloc(QED_MAX_NUM_OF_LL2_CONNECTIONS,
+				    sizeof(struct qed_ll2_info), GFP_KERNEL);
+	if (!p_ll2_connections) {
+		DP_NOTICE(p_hwfn, "Failed to allocate `struct qed_ll2'\n");
+		return NULL;
+	}
+
+	for (i = 0; i < QED_MAX_NUM_OF_LL2_CONNECTIONS; i++)
+		p_ll2_connections[i].my_id = i;
+
+	return p_ll2_connections;
+}
+
+void qed_ll2_setup(struct qed_hwfn *p_hwfn,
+		   struct qed_ll2_info *p_ll2_connections)
+{
+	int i;
+
+	for (i = 0; i < QED_MAX_NUM_OF_LL2_CONNECTIONS; i++)
+		mutex_init(&p_ll2_connections[i].mutex);
+}
+
+void qed_ll2_free(struct qed_hwfn *p_hwfn,
+		  struct qed_ll2_info *p_ll2_connections)
+{
+	kfree(p_ll2_connections);
+}
+
+static void _qed_ll2_get_tstats(struct qed_hwfn *p_hwfn,
+				struct qed_ptt *p_ptt,
+				struct qed_ll2_info *p_ll2_conn,
+				struct qed_ll2_stats *p_stats)
+{
+	struct core_ll2_tstorm_per_queue_stat tstats;
+	u8 qid = p_ll2_conn->queue_id;
+	u32 tstats_addr;
+
+	memset(&tstats, 0, sizeof(tstats));
+	tstats_addr = BAR0_MAP_REG_TSDM_RAM +
+		      CORE_LL2_TSTORM_PER_QUEUE_STAT_OFFSET(qid);
+	qed_memcpy_from(p_hwfn, p_ptt, &tstats, tstats_addr, sizeof(tstats));
+
+	p_stats->packet_too_big_discard =
+			HILO_64_REGPAIR(tstats.packet_too_big_discard);
+	p_stats->no_buff_discard = HILO_64_REGPAIR(tstats.no_buff_discard);
+}
+
+static void _qed_ll2_get_ustats(struct qed_hwfn *p_hwfn,
+				struct qed_ptt *p_ptt,
+				struct qed_ll2_info *p_ll2_conn,
+				struct qed_ll2_stats *p_stats)
+{
+	struct core_ll2_ustorm_per_queue_stat ustats;
+	u8 qid = p_ll2_conn->queue_id;
+	u32 ustats_addr;
+
+	memset(&ustats, 0, sizeof(ustats));
+	ustats_addr = BAR0_MAP_REG_USDM_RAM +
+		      CORE_LL2_USTORM_PER_QUEUE_STAT_OFFSET(qid);
+	qed_memcpy_from(p_hwfn, p_ptt, &ustats, ustats_addr, sizeof(ustats));
+
+	p_stats->rcv_ucast_bytes = HILO_64_REGPAIR(ustats.rcv_ucast_bytes);
+	p_stats->rcv_mcast_bytes = HILO_64_REGPAIR(ustats.rcv_mcast_bytes);
+	p_stats->rcv_bcast_bytes = HILO_64_REGPAIR(ustats.rcv_bcast_bytes);
+	p_stats->rcv_ucast_pkts = HILO_64_REGPAIR(ustats.rcv_ucast_pkts);
+	p_stats->rcv_mcast_pkts = HILO_64_REGPAIR(ustats.rcv_mcast_pkts);
+	p_stats->rcv_bcast_pkts = HILO_64_REGPAIR(ustats.rcv_bcast_pkts);
+}
+
+static void _qed_ll2_get_pstats(struct qed_hwfn *p_hwfn,
+				struct qed_ptt *p_ptt,
+				struct qed_ll2_info *p_ll2_conn,
+				struct qed_ll2_stats *p_stats)
+{
+	struct core_ll2_pstorm_per_queue_stat pstats;
+	u8 stats_id = p_ll2_conn->tx_stats_id;
+	u32 pstats_addr;
+
+	memset(&pstats, 0, sizeof(pstats));
+	pstats_addr = BAR0_MAP_REG_PSDM_RAM +
+		      CORE_LL2_PSTORM_PER_QUEUE_STAT_OFFSET(stats_id);
+	qed_memcpy_from(p_hwfn, p_ptt, &pstats, pstats_addr, sizeof(pstats));
+
+	p_stats->sent_ucast_bytes = HILO_64_REGPAIR(pstats.sent_ucast_bytes);
+	p_stats->sent_mcast_bytes = HILO_64_REGPAIR(pstats.sent_mcast_bytes);
+	p_stats->sent_bcast_bytes = HILO_64_REGPAIR(pstats.sent_bcast_bytes);
+	p_stats->sent_ucast_pkts = HILO_64_REGPAIR(pstats.sent_ucast_pkts);
+	p_stats->sent_mcast_pkts = HILO_64_REGPAIR(pstats.sent_mcast_pkts);
+	p_stats->sent_bcast_pkts = HILO_64_REGPAIR(pstats.sent_bcast_pkts);
+}
+
+int qed_ll2_get_stats(struct qed_hwfn *p_hwfn,
+		      u8 connection_handle, struct qed_ll2_stats *p_stats)
+{
+	struct qed_ll2_info *p_ll2_conn = NULL;
+	struct qed_ptt *p_ptt;
+
+	memset(p_stats, 0, sizeof(*p_stats));
+
+	if ((connection_handle >= QED_MAX_NUM_OF_LL2_CONNECTIONS) ||
+	    !p_hwfn->p_ll2_info)
+		return -EINVAL;
+
+	p_ll2_conn = &p_hwfn->p_ll2_info[connection_handle];
+
+	p_ptt = qed_ptt_acquire(p_hwfn);
+	if (!p_ptt) {
+		DP_ERR(p_hwfn, "Failed to acquire ptt\n");
+		return -EINVAL;
+	}
+
+	_qed_ll2_get_tstats(p_hwfn, p_ptt, p_ll2_conn, p_stats);
+	_qed_ll2_get_ustats(p_hwfn, p_ptt, p_ll2_conn, p_stats);
+	if (p_ll2_conn->tx_stats_en)
+		_qed_ll2_get_pstats(p_hwfn, p_ptt, p_ll2_conn, p_stats);
+
+	qed_ptt_release(p_hwfn, p_ptt);
+	return 0;
+}
+
+static void qed_ll2_register_cb_ops(struct qed_dev *cdev,
+				    const struct qed_ll2_cb_ops *ops,
+				    void *cookie)
+{
+	cdev->ll2->cbs = ops;
+	cdev->ll2->cb_cookie = cookie;
+}
+
+static int qed_ll2_start(struct qed_dev *cdev, struct qed_ll2_params *params)
+{
+	struct qed_ll2_info ll2_info;
+	struct qed_ll2_buffer *buffer, *tmp_buffer;
+	enum qed_ll2_conn_type conn_type;
+	struct qed_ptt *p_ptt;
+	int rc, i;
+
+	/* Initialize LL2 locks & lists */
+	INIT_LIST_HEAD(&cdev->ll2->list);
+	spin_lock_init(&cdev->ll2->lock);
+	cdev->ll2->rx_size = NET_SKB_PAD + ETH_HLEN +
+			     L1_CACHE_BYTES + params->mtu;
+	cdev->ll2->frags_mapped = params->frags_mapped;
+
+	/*Allocate memory for LL2 */
+	DP_INFO(cdev, "Allocating LL2 buffers of size %08x bytes\n",
+		cdev->ll2->rx_size);
+	for (i = 0; i < QED_LL2_RX_SIZE; i++) {
+		buffer = kzalloc(sizeof(*buffer), GFP_KERNEL);
+		if (!buffer) {
+			DP_INFO(cdev, "Failed to allocate LL2 buffers\n");
+			goto fail;
+		}
+
+		rc = qed_ll2_alloc_buffer(cdev, (u8 **)&buffer->data,
+					  &buffer->phys_addr);
+		if (rc) {
+			kfree(buffer);
+			goto fail;
+		}
+
+		list_add_tail(&buffer->list, &cdev->ll2->list);
+	}
+
+	switch (QED_LEADING_HWFN(cdev)->hw_info.personality) {
+	case QED_PCI_ISCSI:
+		conn_type = QED_LL2_TYPE_ISCSI;
+		break;
+	case QED_PCI_ETH_ROCE:
+		conn_type = QED_LL2_TYPE_ROCE;
+		break;
+	default:
+		conn_type = QED_LL2_TYPE_TEST;
+	}
+
+	/* Prepare the temporary ll2 information */
+	memset(&ll2_info, 0, sizeof(ll2_info));
+	ll2_info.conn_type = conn_type;
+	ll2_info.mtu = params->mtu;
+	ll2_info.rx_drop_ttl0_flg = params->drop_ttl0_packets;
+	ll2_info.rx_vlan_removal_en = params->rx_vlan_stripping;
+	ll2_info.tx_tc = 0;
+	ll2_info.tx_dest = CORE_TX_DEST_NW;
+	ll2_info.gsi_enable = 1;
+
+	rc = qed_ll2_acquire_connection(QED_LEADING_HWFN(cdev), &ll2_info,
+					QED_LL2_RX_SIZE, QED_LL2_TX_SIZE,
+					&cdev->ll2->handle);
+	if (rc) {
+		DP_INFO(cdev, "Failed to acquire LL2 connection\n");
+		goto fail;
+	}
+
+	rc = qed_ll2_establish_connection(QED_LEADING_HWFN(cdev),
+					  cdev->ll2->handle);
+	if (rc) {
+		DP_INFO(cdev, "Failed to establish LL2 connection\n");
+		goto release_fail;
+	}
+
+	/* Post all Rx buffers to FW */
+	spin_lock_bh(&cdev->ll2->lock);
+	list_for_each_entry_safe(buffer, tmp_buffer, &cdev->ll2->list, list) {
+		rc = qed_ll2_post_rx_buffer(QED_LEADING_HWFN(cdev),
+					    cdev->ll2->handle,
+					    buffer->phys_addr, 0, buffer, 1);
+		if (rc) {
+			DP_INFO(cdev,
+				"Failed to post an Rx buffer; Deleting it\n");
+			dma_unmap_single(&cdev->pdev->dev, buffer->phys_addr,
+					 cdev->ll2->rx_size, DMA_FROM_DEVICE);
+			kfree(buffer->data);
+			list_del(&buffer->list);
+			kfree(buffer);
+		} else {
+			cdev->ll2->rx_cnt++;
+		}
+	}
+	spin_unlock_bh(&cdev->ll2->lock);
+
+	if (!cdev->ll2->rx_cnt) {
+		DP_INFO(cdev, "Failed passing even a single Rx buffer\n");
+		goto release_terminate;
+	}
+
+	if (!is_valid_ether_addr(params->ll2_mac_address)) {
+		DP_INFO(cdev, "Invalid Ethernet address\n");
+		goto release_terminate;
+	}
+
+	p_ptt = qed_ptt_acquire(QED_LEADING_HWFN(cdev));
+	if (!p_ptt) {
+		DP_INFO(cdev, "Failed to acquire PTT\n");
+		goto release_terminate;
+	}
+
+	rc = qed_llh_add_mac_filter(QED_LEADING_HWFN(cdev), p_ptt,
+				    params->ll2_mac_address);
+	qed_ptt_release(QED_LEADING_HWFN(cdev), p_ptt);
+	if (rc) {
+		DP_ERR(cdev, "Failed to allocate LLH filter\n");
+		goto release_terminate_all;
+	}
+
+	ether_addr_copy(cdev->ll2_mac_address, params->ll2_mac_address);
+
+	return 0;
+
+release_terminate_all:
+
+release_terminate:
+	qed_ll2_terminate_connection(QED_LEADING_HWFN(cdev), cdev->ll2->handle);
+release_fail:
+	qed_ll2_release_connection(QED_LEADING_HWFN(cdev), cdev->ll2->handle);
+fail:
+	qed_ll2_kill_buffers(cdev);
+	cdev->ll2->handle = QED_LL2_UNUSED_HANDLE;
+	return -EINVAL;
+}
+
+static int qed_ll2_stop(struct qed_dev *cdev)
+{
+	struct qed_ptt *p_ptt;
+	int rc;
+
+	if (cdev->ll2->handle == QED_LL2_UNUSED_HANDLE)
+		return 0;
+
+	p_ptt = qed_ptt_acquire(QED_LEADING_HWFN(cdev));
+	if (!p_ptt) {
+		DP_INFO(cdev, "Failed to acquire PTT\n");
+		goto fail;
+	}
+
+	qed_llh_remove_mac_filter(QED_LEADING_HWFN(cdev), p_ptt,
+				  cdev->ll2_mac_address);
+	qed_ptt_release(QED_LEADING_HWFN(cdev), p_ptt);
+	eth_zero_addr(cdev->ll2_mac_address);
+
+	rc = qed_ll2_terminate_connection(QED_LEADING_HWFN(cdev),
+					  cdev->ll2->handle);
+	if (rc)
+		DP_INFO(cdev, "Failed to terminate LL2 connection\n");
+
+	qed_ll2_kill_buffers(cdev);
+
+	qed_ll2_release_connection(QED_LEADING_HWFN(cdev), cdev->ll2->handle);
+	cdev->ll2->handle = QED_LL2_UNUSED_HANDLE;
+
+	return rc;
+fail:
+	return -EINVAL;
+}
+
+static int qed_ll2_start_xmit(struct qed_dev *cdev, struct sk_buff *skb)
+{
+	const skb_frag_t *frag;
+	int rc = -EINVAL, i;
+	dma_addr_t mapping;
+	u16 vlan = 0;
+	u8 flags = 0;
+
+	if (unlikely(skb->ip_summed != CHECKSUM_NONE)) {
+		DP_INFO(cdev, "Cannot transmit a checksumed packet\n");
+		return -EINVAL;
+	}
+
+	if (1 + skb_shinfo(skb)->nr_frags > CORE_LL2_TX_MAX_BDS_PER_PACKET) {
+		DP_ERR(cdev, "Cannot transmit a packet with %d fragments\n",
+		       1 + skb_shinfo(skb)->nr_frags);
+		return -EINVAL;
+	}
+
+	mapping = dma_map_single(&cdev->pdev->dev, skb->data,
+				 skb->len, DMA_TO_DEVICE);
+	if (unlikely(dma_mapping_error(&cdev->pdev->dev, mapping))) {
+		DP_NOTICE(cdev, "SKB mapping failed\n");
+		return -EINVAL;
+	}
+
+	/* Request HW to calculate IP csum */
+	if (!((vlan_get_protocol(skb) == htons(ETH_P_IPV6)) &&
+	      ipv6_hdr(skb)->nexthdr == NEXTHDR_IPV6))
+		flags |= BIT(CORE_TX_BD_FLAGS_IP_CSUM_SHIFT);
+
+	if (skb_vlan_tag_present(skb)) {
+		vlan = skb_vlan_tag_get(skb);
+		flags |= BIT(CORE_TX_BD_FLAGS_VLAN_INSERTION_SHIFT);
+	}
+
+	rc = qed_ll2_prepare_tx_packet(QED_LEADING_HWFN(cdev),
+				       cdev->ll2->handle,
+				       1 + skb_shinfo(skb)->nr_frags,
+				       vlan, flags, 0, 0 /* RoCE FLAVOR */,
+				       mapping, skb->len, skb, 1);
+	if (rc)
+		goto err;
+
+	for (i = 0; i < skb_shinfo(skb)->nr_frags; i++) {
+		frag = &skb_shinfo(skb)->frags[i];
+		if (!cdev->ll2->frags_mapped) {
+			mapping = skb_frag_dma_map(&cdev->pdev->dev, frag, 0,
+						   skb_frag_size(frag),
+						   DMA_TO_DEVICE);
+
+			if (unlikely(dma_mapping_error(&cdev->pdev->dev,
+						       mapping))) {
+				DP_NOTICE(cdev,
+					  "Unable to map frag - dropping packet\n");
+				goto err;
+			}
+		} else {
+			mapping = page_to_phys(skb_frag_page(frag)) |
+			    frag->page_offset;
+		}
+
+		rc = qed_ll2_set_fragment_of_tx_packet(QED_LEADING_HWFN(cdev),
+						       cdev->ll2->handle,
+						       mapping,
+						       skb_frag_size(frag));
+
+		/* if failed not much to do here, partial packet has been posted
+		 * we can't free memory, will need to wait for completion.
+		 */
+		if (rc)
+			goto err2;
+	}
+
+	return 0;
+
+err:
+	dma_unmap_single(&cdev->pdev->dev, mapping, skb->len, DMA_TO_DEVICE);
+
+err2:
+	return rc;
+}
+
+static int qed_ll2_stats(struct qed_dev *cdev, struct qed_ll2_stats *stats)
+{
+	if (!cdev->ll2)
+		return -EINVAL;
+
+	return qed_ll2_get_stats(QED_LEADING_HWFN(cdev),
+				 cdev->ll2->handle, stats);
+}
+
+const struct qed_ll2_ops qed_ll2_ops_pass = {
+	.start = &qed_ll2_start,
+	.stop = &qed_ll2_stop,
+	.start_xmit = &qed_ll2_start_xmit,
+	.register_cb_ops = &qed_ll2_register_cb_ops,
+	.get_stats = &qed_ll2_stats,
+};
+
+int qed_ll2_alloc_if(struct qed_dev *cdev)
+{
+	cdev->ll2 = kzalloc(sizeof(*cdev->ll2), GFP_KERNEL);
+	return cdev->ll2 ? 0 : -ENOMEM;
+}
+
+void qed_ll2_dealloc_if(struct qed_dev *cdev)
+{
+	kfree(cdev->ll2);
+	cdev->ll2 = NULL;
+}
diff --git a/drivers/net/ethernet/qlogic/qed/qed_ll2.h b/drivers/net/ethernet/qlogic/qed/qed_ll2.h
new file mode 100644
index 000000000000..80a5dc2d652d
--- /dev/null
+++ b/drivers/net/ethernet/qlogic/qed/qed_ll2.h
@@ -0,0 +1,316 @@
+/* QLogic qed NIC Driver
+ *
+ * Copyright (c) 2015 QLogic Corporation
+ *
+ * This software is available 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.
+ */
+
+#ifndef _QED_LL2_H
+#define _QED_LL2_H
+
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/list.h>
+#include <linux/mutex.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/qed/qed_chain.h>
+#include <linux/qed/qed_ll2_if.h>
+#include "qed.h"
+#include "qed_hsi.h"
+#include "qed_sp.h"
+
+#define QED_MAX_NUM_OF_LL2_CONNECTIONS                    (4)
+
+enum qed_ll2_roce_flavor_type {
+	QED_LL2_ROCE,
+	QED_LL2_RROCE,
+	MAX_QED_LL2_ROCE_FLAVOR_TYPE
+};
+
+enum qed_ll2_conn_type {
+	QED_LL2_TYPE_RESERVED,
+	QED_LL2_TYPE_ISCSI,
+	QED_LL2_TYPE_TEST,
+	QED_LL2_TYPE_ISCSI_OOO,
+	QED_LL2_TYPE_RESERVED2,
+	QED_LL2_TYPE_ROCE,
+	QED_LL2_TYPE_RESERVED3,
+	MAX_QED_LL2_RX_CONN_TYPE
+};
+
+struct qed_ll2_rx_packet {
+	struct list_head list_entry;
+	struct core_rx_bd_with_buff_len *rxq_bd;
+	dma_addr_t rx_buf_addr;
+	u16 buf_length;
+	void *cookie;
+	u8 placement_offset;
+	u16 parse_flags;
+	u16 packet_length;
+	u16 vlan;
+	u32 opaque_data[2];
+};
+
+struct qed_ll2_tx_packet {
+	struct list_head list_entry;
+	u16 bd_used;
+	u16 vlan;
+	u16 l4_hdr_offset_w;
+	u8 bd_flags;
+	bool notify_fw;
+	void *cookie;
+
+	struct {
+		struct core_tx_bd *txq_bd;
+		dma_addr_t tx_frag;
+		u16 frag_len;
+	} bds_set[ETH_TX_MAX_BDS_PER_NON_LSO_PACKET];
+};
+
+struct qed_ll2_rx_queue {
+	/* Lock protecting the Rx queue manipulation */
+	spinlock_t lock;
+	struct qed_chain rxq_chain;
+	struct qed_chain rcq_chain;
+	u8 rx_sb_index;
+	bool b_cb_registred;
+	__le16 *p_fw_cons;
+	struct list_head active_descq;
+	struct list_head free_descq;
+	struct list_head posting_descq;
+	struct qed_ll2_rx_packet *descq_array;
+	void __iomem *set_prod_addr;
+};
+
+struct qed_ll2_tx_queue {
+	/* Lock protecting the Tx queue manipulation */
+	spinlock_t lock;
+	struct qed_chain txq_chain;
+	u8 tx_sb_index;
+	bool b_cb_registred;
+	__le16 *p_fw_cons;
+	struct list_head active_descq;
+	struct list_head free_descq;
+	struct list_head sending_descq;
+	struct qed_ll2_tx_packet *descq_array;
+	struct qed_ll2_tx_packet *cur_send_packet;
+	struct qed_ll2_tx_packet cur_completing_packet;
+	u16 cur_completing_bd_idx;
+	void __iomem *doorbell_addr;
+	u16 bds_idx;
+	u16 cur_send_frag_num;
+	u16 cur_completing_frag_num;
+	bool b_completing_packet;
+};
+
+struct qed_ll2_info {
+	/* Lock protecting the state of LL2 */
+	struct mutex mutex;
+	enum qed_ll2_conn_type conn_type;
+	u32 cid;
+	u8 my_id;
+	u8 queue_id;
+	u8 tx_stats_id;
+	bool b_active;
+	u16 mtu;
+	u8 rx_drop_ttl0_flg;
+	u8 rx_vlan_removal_en;
+	u8 tx_tc;
+	enum core_tx_dest tx_dest;
+	enum core_error_handle ai_err_packet_too_big;
+	enum core_error_handle ai_err_no_buf;
+	u8 tx_stats_en;
+	struct qed_ll2_rx_queue rx_queue;
+	struct qed_ll2_tx_queue tx_queue;
+	u8 gsi_enable;
+};
+
+/**
+ * @brief qed_ll2_acquire_connection - allocate resources,
+ *        starts rx & tx (if relevant) queues pair. Provides
+ *        connecion handler as output parameter.
+ *
+ * @param p_hwfn
+ * @param p_params		Contain various configuration properties
+ * @param rx_num_desc
+ * @param tx_num_desc
+ *
+ * @param p_connection_handle  Output container for LL2 connection's handle
+ *
+ * @return 0 on success, failure otherwise
+ */
+int qed_ll2_acquire_connection(struct qed_hwfn *p_hwfn,
+			       struct qed_ll2_info *p_params,
+			       u16 rx_num_desc,
+			       u16 tx_num_desc,
+			       u8 *p_connection_handle);
+
+/**
+ * @brief qed_ll2_establish_connection - start previously
+ *        allocated LL2 queues pair
+ *
+ * @param p_hwfn
+ * @param p_ptt
+ * @param connection_handle	LL2 connection's handle obtained from
+ *                              qed_ll2_require_connection
+ *
+ * @return 0 on success, failure otherwise
+ */
+int qed_ll2_establish_connection(struct qed_hwfn *p_hwfn, u8 connection_handle);
+
+/**
+ * @brief qed_ll2_post_rx_buffers - submit buffers to LL2 Rx queue.
+ *
+ * @param p_hwfn
+ * @param connection_handle	LL2 connection's handle obtained from
+ *				qed_ll2_require_connection
+ * @param addr			rx (physical address) buffers to submit
+ * @param cookie
+ * @param notify_fw		produce corresponding Rx BD immediately
+ *
+ * @return 0 on success, failure otherwise
+ */
+int qed_ll2_post_rx_buffer(struct qed_hwfn *p_hwfn,
+			   u8 connection_handle,
+			   dma_addr_t addr,
+			   u16 buf_len, void *cookie, u8 notify_fw);
+
+/**
+ * @brief qed_ll2_prepare_tx_packet - request for start Tx BD
+ *				      to prepare Tx packet submission to FW.
+ *
+ * @param p_hwfn
+ * @param connection_handle	LL2 connection's handle obtained from
+ *				qed_ll2_require_connection
+ * @param num_of_bds		a number of requested BD equals a number of
+ *				fragments in Tx packet
+ * @param vlan			VLAN to insert to packet (if insertion set)
+ * @param bd_flags
+ * @param l4_hdr_offset_w	L4 Header Offset from start of packet
+ *				(in words). This is needed if both l4_csum
+ *				and ipv6_ext are set
+ * @param first_frag
+ * @param first_frag_len
+ * @param cookie
+ *
+ * @param notify_fw
+ *
+ * @return 0 on success, failure otherwise
+ */
+int qed_ll2_prepare_tx_packet(struct qed_hwfn *p_hwfn,
+			      u8 connection_handle,
+			      u8 num_of_bds,
+			      u16 vlan,
+			      u8 bd_flags,
+			      u16 l4_hdr_offset_w,
+			      enum qed_ll2_roce_flavor_type qed_roce_flavor,
+			      dma_addr_t first_frag,
+			      u16 first_frag_len, void *cookie, u8 notify_fw);
+
+/**
+ * @brief qed_ll2_release_connection -	releases resources
+ *					allocated for LL2 connection
+ *
+ * @param p_hwfn
+ * @param connection_handle		LL2 connection's handle obtained from
+ *					qed_ll2_require_connection
+ */
+void qed_ll2_release_connection(struct qed_hwfn *p_hwfn, u8 connection_handle);
+
+/**
+ * @brief qed_ll2_set_fragment_of_tx_packet -	provides fragments to fill
+ *						Tx BD of BDs requested by
+ *						qed_ll2_prepare_tx_packet
+ *
+ * @param p_hwfn
+ * @param connection_handle			LL2 connection's handle
+ *						obtained from
+ *						qed_ll2_require_connection
+ * @param addr
+ * @param nbytes
+ *
+ * @return 0 on success, failure otherwise
+ */
+int qed_ll2_set_fragment_of_tx_packet(struct qed_hwfn *p_hwfn,
+				      u8 connection_handle,
+				      dma_addr_t addr, u16 nbytes);
+
+/**
+ * @brief qed_ll2_terminate_connection -	stops Tx/Rx queues
+ *
+ *
+ * @param p_hwfn
+ * @param connection_handle			LL2 connection's handle
+ *						obtained from
+ *						qed_ll2_require_connection
+ *
+ * @return 0 on success, failure otherwise
+ */
+int qed_ll2_terminate_connection(struct qed_hwfn *p_hwfn, u8 connection_handle);
+
+/**
+ * @brief qed_ll2_get_stats -	get LL2 queue's statistics
+ *
+ *
+ * @param p_hwfn
+ * @param connection_handle	LL2 connection's handle obtained from
+ *				qed_ll2_require_connection
+ * @param p_stats
+ *
+ * @return 0 on success, failure otherwise
+ */
+int qed_ll2_get_stats(struct qed_hwfn *p_hwfn,
+		      u8 connection_handle, struct qed_ll2_stats *p_stats);
+
+/**
+ * @brief qed_ll2_alloc - Allocates LL2 connections set
+ *
+ * @param p_hwfn
+ *
+ * @return pointer to alocated qed_ll2_info or NULL
+ */
+struct qed_ll2_info *qed_ll2_alloc(struct qed_hwfn *p_hwfn);
+
+/**
+ * @brief qed_ll2_setup - Inits LL2 connections set
+ *
+ * @param p_hwfn
+ * @param p_ll2_connections
+ *
+ */
+void qed_ll2_setup(struct qed_hwfn *p_hwfn,
+		   struct qed_ll2_info *p_ll2_connections);
+
+/**
+ * @brief qed_ll2_free - Releases LL2 connections set
+ *
+ * @param p_hwfn
+ * @param p_ll2_connections
+ *
+ */
+void qed_ll2_free(struct qed_hwfn *p_hwfn,
+		  struct qed_ll2_info *p_ll2_connections);
+void qed_ll2b_complete_rx_gsi_packet(struct qed_hwfn *p_hwfn,
+				     u8 connection_handle,
+				     void *cookie,
+				     dma_addr_t rx_buf_addr,
+				     u16 data_length,
+				     u8 data_length_error,
+				     u16 parse_flags,
+				     u16 vlan,
+				     u32 src_mac_addr_hi,
+				     u16 src_mac_addr_lo, bool b_last_packet);
+void qed_ll2b_complete_tx_gsi_packet(struct qed_hwfn *p_hwfn,
+				     u8 connection_handle,
+				     void *cookie,
+				     dma_addr_t first_frag_addr,
+				     bool b_last_fragment, bool b_last_packet);
+void qed_ll2b_release_tx_gsi_packet(struct qed_hwfn *p_hwfn,
+				    u8 connection_handle,
+				    void *cookie,
+				    dma_addr_t first_frag_addr,
+				    bool b_last_fragment, bool b_last_packet);
+#endif
diff --git a/drivers/net/ethernet/qlogic/qed/qed_main.c b/drivers/net/ethernet/qlogic/qed/qed_main.c
index c7dc34bfdd0a..4ee3151e80c2 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_main.c
+++ b/drivers/net/ethernet/qlogic/qed/qed_main.c
@@ -22,15 +22,22 @@
 #include <linux/etherdevice.h>
 #include <linux/vmalloc.h>
 #include <linux/qed/qed_if.h>
+#include <linux/qed/qed_ll2_if.h>
 
 #include "qed.h"
 #include "qed_sriov.h"
 #include "qed_sp.h"
 #include "qed_dev_api.h"
+#include "qed_ll2.h"
 #include "qed_mcp.h"
 #include "qed_hw.h"
 #include "qed_selftest.h"
 
+#if IS_ENABLED(CONFIG_INFINIBAND_QEDR)
+#define QED_ROCE_QPS			(8192)
+#define QED_ROCE_DPIS			(8)
+#endif
+
 static char version[] =
 	"QLogic FastLinQ 4xxxx Core Module qed " DRV_MODULE_VERSION "\n";
 
@@ -51,8 +58,6 @@ MODULE_FIRMWARE(QED_FW_FILE_NAME);
 
 static int __init qed_init(void)
 {
-	pr_notice("qed_init called\n");
-
 	pr_info("%s", version);
 
 	return 0;
@@ -106,8 +111,7 @@ static void qed_free_pci(struct qed_dev *cdev)
 /* Performs PCI initializations as well as initializing PCI-related parameters
  * in the device structrue. Returns 0 in case of success.
  */
-static int qed_init_pci(struct qed_dev *cdev,
-			struct pci_dev *pdev)
+static int qed_init_pci(struct qed_dev *cdev, struct pci_dev *pdev)
 {
 	u8 rev_id;
 	int rc;
@@ -207,8 +211,8 @@ int qed_fill_dev_info(struct qed_dev *cdev,
 	dev_info->pci_mem_start = cdev->pci_params.mem_start;
 	dev_info->pci_mem_end = cdev->pci_params.mem_end;
 	dev_info->pci_irq = cdev->pci_params.irq;
-	dev_info->rdma_supported =
-	    (cdev->hwfns[0].hw_info.personality == QED_PCI_ETH_ROCE);
+	dev_info->rdma_supported = (cdev->hwfns[0].hw_info.personality ==
+				    QED_PCI_ETH_ROCE);
 	dev_info->is_mf_default = IS_MF_DEFAULT(&cdev->hwfns[0]);
 	ether_addr_copy(dev_info->hw_mac, cdev->hwfns[0].hw_info.hw_mac_addr);
 
@@ -263,8 +267,7 @@ static struct qed_dev *qed_alloc_cdev(struct pci_dev *pdev)
 }
 
 /* Sets the requested power state */
-static int qed_set_power_state(struct qed_dev *cdev,
-			       pci_power_t state)
+static int qed_set_power_state(struct qed_dev *cdev, pci_power_t state)
 {
 	if (!cdev)
 		return -ENODEV;
@@ -366,8 +369,8 @@ static int qed_enable_msix(struct qed_dev *cdev,
 		DP_NOTICE(cdev,
 			  "Trying to enable MSI-X with less vectors (%d out of %d)\n",
 			  cnt, int_params->in.num_vectors);
-		rc = pci_enable_msix_exact(cdev->pdev,
-					   int_params->msix_table, cnt);
+		rc = pci_enable_msix_exact(cdev->pdev, int_params->msix_table,
+					   cnt);
 		if (!rc)
 			rc = cnt;
 	}
@@ -439,6 +442,11 @@ static int qed_set_int_mode(struct qed_dev *cdev, bool force_mode)
 	}
 
 out:
+	if (!rc)
+		DP_INFO(cdev, "Using %s interrupts\n",
+			int_params->out.int_mode == QED_INT_MODE_INTA ?
+			"INTa" : int_params->out.int_mode == QED_INT_MODE_MSI ?
+			"MSI" : "MSIX");
 	cdev->int_coalescing_mode = QED_COAL_MODE_ENABLE;
 
 	return rc;
@@ -514,19 +522,18 @@ static irqreturn_t qed_single_int(int irq, void *dev_instance)
 int qed_slowpath_irq_req(struct qed_hwfn *hwfn)
 {
 	struct qed_dev *cdev = hwfn->cdev;
+	u32 int_mode;
 	int rc = 0;
 	u8 id;
 
-	if (cdev->int_params.out.int_mode == QED_INT_MODE_MSIX) {
+	int_mode = cdev->int_params.out.int_mode;
+	if (int_mode == QED_INT_MODE_MSIX) {
 		id = hwfn->my_id;
 		snprintf(hwfn->name, NAME_SIZE, "sp-%d-%02x:%02x.%02x",
 			 id, cdev->pdev->bus->number,
 			 PCI_SLOT(cdev->pdev->devfn), hwfn->abs_pf_id);
 		rc = request_irq(cdev->int_params.msix_table[id].vector,
 				 qed_msix_sp_int, 0, hwfn->name, hwfn->sp_dpc);
-		if (!rc)
-			DP_VERBOSE(hwfn, (NETIF_MSG_INTR | QED_MSG_SP),
-				   "Requested slowpath MSI-X\n");
 	} else {
 		unsigned long flags = 0;
 
@@ -541,6 +548,13 @@ int qed_slowpath_irq_req(struct qed_hwfn *hwfn)
 				 flags, cdev->name, cdev);
 	}
 
+	if (rc)
+		DP_NOTICE(cdev, "request_irq failed, rc = %d\n", rc);
+	else
+		DP_VERBOSE(hwfn, (NETIF_MSG_INTR | QED_MSG_SP),
+			   "Requested slowpath %s\n",
+			   (int_mode == QED_INT_MODE_MSIX) ? "MSI-X" : "IRQ");
+
 	return rc;
 }
 
@@ -581,6 +595,8 @@ static int qed_nic_stop(struct qed_dev *cdev)
 		}
 	}
 
+	qed_dbg_pf_exit(cdev);
+
 	return rc;
 }
 
@@ -599,7 +615,16 @@ static int qed_nic_reset(struct qed_dev *cdev)
 
 static int qed_nic_setup(struct qed_dev *cdev)
 {
-	int rc;
+	int rc, i;
+
+	/* Determine if interface is going to require LL2 */
+	if (QED_LEADING_HWFN(cdev)->hw_info.personality != QED_PCI_ETH) {
+		for (i = 0; i < cdev->num_hwfns; i++) {
+			struct qed_hwfn *p_hwfn = &cdev->hwfns[i];
+
+			p_hwfn->using_ll2 = true;
+		}
+	}
 
 	rc = qed_resc_alloc(cdev);
 	if (rc)
@@ -657,6 +682,9 @@ static int qed_slowpath_setup_int(struct qed_dev *cdev,
 				  enum qed_int_mode int_mode)
 {
 	struct qed_sb_cnt_info sb_cnt_info;
+#if IS_ENABLED(CONFIG_INFINIBAND_QEDR)
+	int num_l2_queues;
+#endif
 	int rc;
 	int i;
 
@@ -687,6 +715,31 @@ static int qed_slowpath_setup_int(struct qed_dev *cdev,
 	cdev->int_params.fp_msix_cnt = cdev->int_params.out.num_vectors -
 				       cdev->num_hwfns;
 
+#if IS_ENABLED(CONFIG_INFINIBAND_QEDR)
+	num_l2_queues = 0;
+	for_each_hwfn(cdev, i)
+		num_l2_queues += FEAT_NUM(&cdev->hwfns[i], QED_PF_L2_QUE);
+
+	DP_VERBOSE(cdev, QED_MSG_RDMA,
+		   "cdev->int_params.fp_msix_cnt=%d num_l2_queues=%d\n",
+		   cdev->int_params.fp_msix_cnt, num_l2_queues);
+
+	if (cdev->int_params.fp_msix_cnt > num_l2_queues) {
+		cdev->int_params.rdma_msix_cnt =
+			(cdev->int_params.fp_msix_cnt - num_l2_queues)
+			/ cdev->num_hwfns;
+		cdev->int_params.rdma_msix_base =
+			cdev->int_params.fp_msix_base + num_l2_queues;
+		cdev->int_params.fp_msix_cnt = num_l2_queues;
+	} else {
+		cdev->int_params.rdma_msix_cnt = 0;
+	}
+
+	DP_VERBOSE(cdev, QED_MSG_RDMA, "roce_msix_cnt=%d roce_msix_base=%d\n",
+		   cdev->int_params.rdma_msix_cnt,
+		   cdev->int_params.rdma_msix_base);
+#endif
+
 	return 0;
 }
 
@@ -790,6 +843,13 @@ static void qed_update_pf_params(struct qed_dev *cdev,
 {
 	int i;
 
+#if IS_ENABLED(CONFIG_INFINIBAND_QEDR)
+	params->rdma_pf_params.num_qps = QED_ROCE_QPS;
+	params->rdma_pf_params.min_dpis = QED_ROCE_DPIS;
+	/* divide by 3 the MRs to avoid MF ILT overflow */
+	params->rdma_pf_params.num_mrs = RDMA_MAX_TIDS;
+	params->rdma_pf_params.gl_pi = QED_ROCE_PROTOCOL_INDEX;
+#endif
 	for (i = 0; i < cdev->num_hwfns; i++) {
 		struct qed_hwfn *p_hwfn = &cdev->hwfns[i];
 
@@ -834,13 +894,13 @@ static int qed_slowpath_start(struct qed_dev *cdev,
 	if (IS_PF(cdev)) {
 		/* Allocate stream for unzipping */
 		rc = qed_alloc_stream_mem(cdev);
-		if (rc) {
-			DP_NOTICE(cdev, "Failed to allocate stream memory\n");
+		if (rc)
 			goto err2;
-		}
 
 		/* First Dword used to diffrentiate between various sources */
 		data = cdev->firmware->data + sizeof(u32);
+
+		qed_dbg_pf_init(cdev);
 	}
 
 	memset(&tunn_info, 0, sizeof(tunn_info));
@@ -864,6 +924,12 @@ static int qed_slowpath_start(struct qed_dev *cdev,
 	DP_INFO(cdev,
 		"HW initialization and function start completed successfully\n");
 
+	/* Allocate LL2 interface if needed */
+	if (QED_LEADING_HWFN(cdev)->using_ll2) {
+		rc = qed_ll2_alloc_if(cdev);
+		if (rc)
+			goto err3;
+	}
 	if (IS_PF(cdev)) {
 		hwfn = QED_LEADING_HWFN(cdev);
 		drv_version.version = (params->drv_major << 24) |
@@ -884,6 +950,8 @@ static int qed_slowpath_start(struct qed_dev *cdev,
 
 	return 0;
 
+err3:
+	qed_hw_stop(cdev);
 err2:
 	qed_hw_timers_stop_all(cdev);
 	if (IS_PF(cdev))
@@ -906,6 +974,8 @@ static int qed_slowpath_stop(struct qed_dev *cdev)
 	if (!cdev)
 		return -ENODEV;
 
+	qed_ll2_dealloc_if(cdev);
+
 	if (IS_PF(cdev)) {
 		qed_free_stream_mem(cdev);
 		if (IS_QED_ETH_IF(cdev))
@@ -974,8 +1044,7 @@ static u32 qed_sb_init(struct qed_dev *cdev,
 }
 
 static u32 qed_sb_release(struct qed_dev *cdev,
-			  struct qed_sb_info *sb_info,
-			  u16 sb_id)
+			  struct qed_sb_info *sb_info, u16 sb_id)
 {
 	struct qed_hwfn *p_hwfn;
 	int hwfn_index;
@@ -1025,20 +1094,23 @@ static int qed_set_link(struct qed_dev *cdev, struct qed_link_params *params)
 		link_params->speed.autoneg = params->autoneg;
 	if (params->override_flags & QED_LINK_OVERRIDE_SPEED_ADV_SPEEDS) {
 		link_params->speed.advertised_speeds = 0;
-		if ((params->adv_speeds & SUPPORTED_1000baseT_Half) ||
-		    (params->adv_speeds & SUPPORTED_1000baseT_Full))
+		if ((params->adv_speeds & QED_LM_1000baseT_Half_BIT) ||
+		    (params->adv_speeds & QED_LM_1000baseT_Full_BIT))
+			link_params->speed.advertised_speeds |=
+			    NVM_CFG1_PORT_DRV_SPEED_CAPABILITY_MASK_1G;
+		if (params->adv_speeds & QED_LM_10000baseKR_Full_BIT)
 			link_params->speed.advertised_speeds |=
-				NVM_CFG1_PORT_DRV_SPEED_CAPABILITY_MASK_1G;
-		if (params->adv_speeds & SUPPORTED_10000baseKR_Full)
+			    NVM_CFG1_PORT_DRV_SPEED_CAPABILITY_MASK_10G;
+		if (params->adv_speeds & QED_LM_25000baseKR_Full_BIT)
 			link_params->speed.advertised_speeds |=
-				NVM_CFG1_PORT_DRV_SPEED_CAPABILITY_MASK_10G;
-		if (params->adv_speeds & SUPPORTED_40000baseLR4_Full)
+			    NVM_CFG1_PORT_DRV_SPEED_CAPABILITY_MASK_25G;
+		if (params->adv_speeds & QED_LM_40000baseLR4_Full_BIT)
 			link_params->speed.advertised_speeds |=
-				NVM_CFG1_PORT_DRV_SPEED_CAPABILITY_MASK_40G;
-		if (params->adv_speeds & 0)
+			    NVM_CFG1_PORT_DRV_SPEED_CAPABILITY_MASK_40G;
+		if (params->adv_speeds & QED_LM_50000baseKR2_Full_BIT)
 			link_params->speed.advertised_speeds |=
-				NVM_CFG1_PORT_DRV_SPEED_CAPABILITY_MASK_50G;
-		if (params->adv_speeds & 0)
+			    NVM_CFG1_PORT_DRV_SPEED_CAPABILITY_MASK_50G;
+		if (params->adv_speeds & QED_LM_100000baseKR4_Full_BIT)
 			link_params->speed.advertised_speeds |=
 			    NVM_CFG1_PORT_DRV_SPEED_CAPABILITY_MASK_BB_100G;
 	}
@@ -1168,50 +1240,56 @@ static void qed_fill_link(struct qed_hwfn *hwfn,
 		if_link->link_up = true;
 
 	/* TODO - at the moment assume supported and advertised speed equal */
-	if_link->supported_caps = SUPPORTED_FIBRE;
+	if_link->supported_caps = QED_LM_FIBRE_BIT;
 	if (params.speed.autoneg)
-		if_link->supported_caps |= SUPPORTED_Autoneg;
+		if_link->supported_caps |= QED_LM_Autoneg_BIT;
 	if (params.pause.autoneg ||
 	    (params.pause.forced_rx && params.pause.forced_tx))
-		if_link->supported_caps |= SUPPORTED_Asym_Pause;
+		if_link->supported_caps |= QED_LM_Asym_Pause_BIT;
 	if (params.pause.autoneg || params.pause.forced_rx ||
 	    params.pause.forced_tx)
-		if_link->supported_caps |= SUPPORTED_Pause;
+		if_link->supported_caps |= QED_LM_Pause_BIT;
 
 	if_link->advertised_caps = if_link->supported_caps;
 	if (params.speed.advertised_speeds &
 	    NVM_CFG1_PORT_DRV_SPEED_CAPABILITY_MASK_1G)
-		if_link->advertised_caps |= SUPPORTED_1000baseT_Half |
-					   SUPPORTED_1000baseT_Full;
+		if_link->advertised_caps |= QED_LM_1000baseT_Half_BIT |
+		    QED_LM_1000baseT_Full_BIT;
 	if (params.speed.advertised_speeds &
 	    NVM_CFG1_PORT_DRV_SPEED_CAPABILITY_MASK_10G)
-		if_link->advertised_caps |= SUPPORTED_10000baseKR_Full;
+		if_link->advertised_caps |= QED_LM_10000baseKR_Full_BIT;
 	if (params.speed.advertised_speeds &
-		NVM_CFG1_PORT_DRV_SPEED_CAPABILITY_MASK_40G)
-		if_link->advertised_caps |= SUPPORTED_40000baseLR4_Full;
+	    NVM_CFG1_PORT_DRV_SPEED_CAPABILITY_MASK_25G)
+		if_link->advertised_caps |= QED_LM_25000baseKR_Full_BIT;
 	if (params.speed.advertised_speeds &
-		NVM_CFG1_PORT_DRV_SPEED_CAPABILITY_MASK_50G)
-		if_link->advertised_caps |= 0;
+	    NVM_CFG1_PORT_DRV_SPEED_CAPABILITY_MASK_40G)
+		if_link->advertised_caps |= QED_LM_40000baseLR4_Full_BIT;
+	if (params.speed.advertised_speeds &
+	    NVM_CFG1_PORT_DRV_SPEED_CAPABILITY_MASK_50G)
+		if_link->advertised_caps |= QED_LM_50000baseKR2_Full_BIT;
 	if (params.speed.advertised_speeds &
 	    NVM_CFG1_PORT_DRV_SPEED_CAPABILITY_MASK_BB_100G)
-		if_link->advertised_caps |= 0;
+		if_link->advertised_caps |= QED_LM_100000baseKR4_Full_BIT;
 
 	if (link_caps.speed_capabilities &
 	    NVM_CFG1_PORT_DRV_SPEED_CAPABILITY_MASK_1G)
-		if_link->supported_caps |= SUPPORTED_1000baseT_Half |
-					   SUPPORTED_1000baseT_Full;
+		if_link->supported_caps |= QED_LM_1000baseT_Half_BIT |
+		    QED_LM_1000baseT_Full_BIT;
 	if (link_caps.speed_capabilities &
 	    NVM_CFG1_PORT_DRV_SPEED_CAPABILITY_MASK_10G)
-		if_link->supported_caps |= SUPPORTED_10000baseKR_Full;
+		if_link->supported_caps |= QED_LM_10000baseKR_Full_BIT;
+	if (link_caps.speed_capabilities &
+	    NVM_CFG1_PORT_DRV_SPEED_CAPABILITY_MASK_25G)
+		if_link->supported_caps |= QED_LM_25000baseKR_Full_BIT;
 	if (link_caps.speed_capabilities &
-		NVM_CFG1_PORT_DRV_SPEED_CAPABILITY_MASK_40G)
-		if_link->supported_caps |= SUPPORTED_40000baseLR4_Full;
+	    NVM_CFG1_PORT_DRV_SPEED_CAPABILITY_MASK_40G)
+		if_link->supported_caps |= QED_LM_40000baseLR4_Full_BIT;
 	if (link_caps.speed_capabilities &
-		NVM_CFG1_PORT_DRV_SPEED_CAPABILITY_MASK_50G)
-		if_link->supported_caps |= 0;
+	    NVM_CFG1_PORT_DRV_SPEED_CAPABILITY_MASK_50G)
+		if_link->supported_caps |= QED_LM_50000baseKR2_Full_BIT;
 	if (link_caps.speed_capabilities &
 	    NVM_CFG1_PORT_DRV_SPEED_CAPABILITY_MASK_BB_100G)
-		if_link->supported_caps |= 0;
+		if_link->supported_caps |= QED_LM_100000baseKR4_Full_BIT;
 
 	if (link.link_up)
 		if_link->speed = link.speed;
@@ -1231,33 +1309,29 @@ static void qed_fill_link(struct qed_hwfn *hwfn,
 		if_link->pause_config |= QED_LINK_PAUSE_TX_ENABLE;
 
 	/* Link partner capabilities */
-	if (link.partner_adv_speed &
-	    QED_LINK_PARTNER_SPEED_1G_HD)
-		if_link->lp_caps |= SUPPORTED_1000baseT_Half;
-	if (link.partner_adv_speed &
-	    QED_LINK_PARTNER_SPEED_1G_FD)
-		if_link->lp_caps |= SUPPORTED_1000baseT_Full;
-	if (link.partner_adv_speed &
-	    QED_LINK_PARTNER_SPEED_10G)
-		if_link->lp_caps |= SUPPORTED_10000baseKR_Full;
-	if (link.partner_adv_speed &
-	    QED_LINK_PARTNER_SPEED_40G)
-		if_link->lp_caps |= SUPPORTED_40000baseLR4_Full;
-	if (link.partner_adv_speed &
-	    QED_LINK_PARTNER_SPEED_50G)
-		if_link->lp_caps |= 0;
-	if (link.partner_adv_speed &
-	    QED_LINK_PARTNER_SPEED_100G)
-		if_link->lp_caps |= 0;
+	if (link.partner_adv_speed & QED_LINK_PARTNER_SPEED_1G_HD)
+		if_link->lp_caps |= QED_LM_1000baseT_Half_BIT;
+	if (link.partner_adv_speed & QED_LINK_PARTNER_SPEED_1G_FD)
+		if_link->lp_caps |= QED_LM_1000baseT_Full_BIT;
+	if (link.partner_adv_speed & QED_LINK_PARTNER_SPEED_10G)
+		if_link->lp_caps |= QED_LM_10000baseKR_Full_BIT;
+	if (link.partner_adv_speed & QED_LINK_PARTNER_SPEED_25G)
+		if_link->lp_caps |= QED_LM_25000baseKR_Full_BIT;
+	if (link.partner_adv_speed & QED_LINK_PARTNER_SPEED_40G)
+		if_link->lp_caps |= QED_LM_40000baseLR4_Full_BIT;
+	if (link.partner_adv_speed & QED_LINK_PARTNER_SPEED_50G)
+		if_link->lp_caps |= QED_LM_50000baseKR2_Full_BIT;
+	if (link.partner_adv_speed & QED_LINK_PARTNER_SPEED_100G)
+		if_link->lp_caps |= QED_LM_100000baseKR4_Full_BIT;
 
 	if (link.an_complete)
-		if_link->lp_caps |= SUPPORTED_Autoneg;
+		if_link->lp_caps |= QED_LM_Autoneg_BIT;
 
 	if (link.partner_adv_pause)
-		if_link->lp_caps |= SUPPORTED_Pause;
+		if_link->lp_caps |= QED_LM_Pause_BIT;
 	if (link.partner_adv_pause == QED_LINK_PARTNER_ASYMMETRIC_PAUSE ||
 	    link.partner_adv_pause == QED_LINK_PARTNER_BOTH_PAUSE)
-		if_link->lp_caps |= SUPPORTED_Asym_Pause;
+		if_link->lp_caps |= QED_LM_Asym_Pause_BIT;
 }
 
 static void qed_get_current_link(struct qed_dev *cdev,
@@ -1385,9 +1459,32 @@ const struct qed_common_ops qed_common_ops_pass = {
 	.get_link = &qed_get_current_link,
 	.drain = &qed_drain,
 	.update_msglvl = &qed_init_dp,
+	.dbg_all_data = &qed_dbg_all_data,
+	.dbg_all_data_size = &qed_dbg_all_data_size,
 	.chain_alloc = &qed_chain_alloc,
 	.chain_free = &qed_chain_free,
 	.get_coalesce = &qed_get_coalesce,
 	.set_coalesce = &qed_set_coalesce,
 	.set_led = &qed_set_led,
 };
+
+void qed_get_protocol_stats(struct qed_dev *cdev,
+			    enum qed_mcp_protocol_type type,
+			    union qed_mcp_protocol_stats *stats)
+{
+	struct qed_eth_stats eth_stats;
+
+	memset(stats, 0, sizeof(*stats));
+
+	switch (type) {
+	case QED_MCP_LAN_STATS:
+		qed_get_vport_stats(cdev, &eth_stats);
+		stats->lan_stats.ucast_rx_pkts = eth_stats.rx_ucast_pkts;
+		stats->lan_stats.ucast_tx_pkts = eth_stats.tx_ucast_pkts;
+		stats->lan_stats.fcs_err = -1;
+		break;
+	default:
+		DP_ERR(cdev, "Invalid protocol type = %d\n", type);
+		return;
+	}
+}
diff --git a/drivers/net/ethernet/qlogic/qed/qed_mcp.c b/drivers/net/ethernet/qlogic/qed/qed_mcp.c
index f776a77794c5..bdc9ba92f6d4 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_mcp.c
+++ b/drivers/net/ethernet/qlogic/qed/qed_mcp.c
@@ -54,8 +54,7 @@ bool qed_mcp_is_init(struct qed_hwfn *p_hwfn)
 	return true;
 }
 
-void qed_mcp_cmd_port_init(struct qed_hwfn *p_hwfn,
-			   struct qed_ptt *p_ptt)
+void qed_mcp_cmd_port_init(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
 {
 	u32 addr = SECTION_OFFSIZE_ADDR(p_hwfn->mcp_info->public_base,
 					PUBLIC_PORT);
@@ -68,8 +67,7 @@ void qed_mcp_cmd_port_init(struct qed_hwfn *p_hwfn,
 		   p_hwfn->mcp_info->port_addr, MFW_PORT(p_hwfn));
 }
 
-void qed_mcp_read_mb(struct qed_hwfn *p_hwfn,
-		     struct qed_ptt *p_ptt)
+void qed_mcp_read_mb(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
 {
 	u32 length = MFW_DRV_MSG_MAX_DWORDS(p_hwfn->mcp_info->mfw_mb_length);
 	u32 tmp, i;
@@ -99,8 +97,7 @@ int qed_mcp_free(struct qed_hwfn *p_hwfn)
 	return 0;
 }
 
-static int qed_load_mcp_offsets(struct qed_hwfn *p_hwfn,
-				struct qed_ptt *p_ptt)
+static int qed_load_mcp_offsets(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
 {
 	struct qed_mcp_info *p_info = p_hwfn->mcp_info;
 	u32 drv_mb_offsize, mfw_mb_offsize;
@@ -143,8 +140,7 @@ static int qed_load_mcp_offsets(struct qed_hwfn *p_hwfn,
 	return 0;
 }
 
-int qed_mcp_cmd_init(struct qed_hwfn *p_hwfn,
-		     struct qed_ptt *p_ptt)
+int qed_mcp_cmd_init(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
 {
 	struct qed_mcp_info *p_info;
 	u32 size;
@@ -165,9 +161,7 @@ int qed_mcp_cmd_init(struct qed_hwfn *p_hwfn,
 
 	size = MFW_DRV_MSG_MAX_DWORDS(p_info->mfw_mb_length) * sizeof(u32);
 	p_info->mfw_mb_cur = kzalloc(size, GFP_KERNEL);
-	p_info->mfw_mb_shadow =
-		kzalloc(sizeof(u32) * MFW_DRV_MSG_MAX_DWORDS(
-				p_info->mfw_mb_length), GFP_KERNEL);
+	p_info->mfw_mb_shadow = kzalloc(size, GFP_KERNEL);
 	if (!p_info->mfw_mb_shadow || !p_info->mfw_mb_addr)
 		goto err;
 
@@ -177,7 +171,6 @@ int qed_mcp_cmd_init(struct qed_hwfn *p_hwfn,
 	return 0;
 
 err:
-	DP_NOTICE(p_hwfn, "Failed to allocate mcp memory\n");
 	qed_mcp_free(p_hwfn);
 	return -ENOMEM;
 }
@@ -189,8 +182,7 @@ err:
  * access is achieved by setting a blocking flag, which will fail other
  * competing contexts to send their mailboxes.
  */
-static int qed_mcp_mb_lock(struct qed_hwfn *p_hwfn,
-			   u32 cmd)
+static int qed_mcp_mb_lock(struct qed_hwfn *p_hwfn, u32 cmd)
 {
 	spin_lock_bh(&p_hwfn->mcp_info->lock);
 
@@ -221,15 +213,13 @@ static int qed_mcp_mb_lock(struct qed_hwfn *p_hwfn,
 	return 0;
 }
 
-static void qed_mcp_mb_unlock(struct qed_hwfn	*p_hwfn,
-			      u32		cmd)
+static void qed_mcp_mb_unlock(struct qed_hwfn *p_hwfn, u32 cmd)
 {
 	if (cmd != DRV_MSG_CODE_LOAD_REQ && cmd != DRV_MSG_CODE_UNLOAD_REQ)
 		spin_unlock_bh(&p_hwfn->mcp_info->lock);
 }
 
-int qed_mcp_reset(struct qed_hwfn *p_hwfn,
-		  struct qed_ptt *p_ptt)
+int qed_mcp_reset(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
 {
 	u32 seq = ++p_hwfn->mcp_info->drv_mb_seq;
 	u8 delay = CHIP_MCP_RESP_ITER_US;
@@ -326,7 +316,8 @@ static int qed_do_mcp_cmd(struct qed_hwfn *p_hwfn,
 		*o_mcp_param = DRV_MB_RD(p_hwfn, p_ptt, fw_mb_param);
 	} else {
 		/* FW BUG! */
-		DP_ERR(p_hwfn, "MFW failed to respond!\n");
+		DP_ERR(p_hwfn, "MFW failed to respond [cmd 0x%x param 0x%x]\n",
+		       cmd, param);
 		*o_mcp_resp = 0;
 		rc = -EAGAIN;
 	}
@@ -342,7 +333,7 @@ static int qed_mcp_cmd_and_union(struct qed_hwfn *p_hwfn,
 
 	/* MCP not initialized */
 	if (!qed_mcp_is_init(p_hwfn)) {
-		DP_NOTICE(p_hwfn, "MFW is not initialized !\n");
+		DP_NOTICE(p_hwfn, "MFW is not initialized!\n");
 		return -EBUSY;
 	}
 
@@ -398,9 +389,36 @@ int qed_mcp_cmd(struct qed_hwfn *p_hwfn,
 	return 0;
 }
 
+int qed_mcp_nvm_rd_cmd(struct qed_hwfn *p_hwfn,
+		       struct qed_ptt *p_ptt,
+		       u32 cmd,
+		       u32 param,
+		       u32 *o_mcp_resp,
+		       u32 *o_mcp_param, u32 *o_txn_size, u32 *o_buf)
+{
+	struct qed_mcp_mb_params mb_params;
+	union drv_union_data union_data;
+	int rc;
+
+	memset(&mb_params, 0, sizeof(mb_params));
+	mb_params.cmd = cmd;
+	mb_params.param = param;
+	mb_params.p_data_dst = &union_data;
+	rc = qed_mcp_cmd_and_union(p_hwfn, p_ptt, &mb_params);
+	if (rc)
+		return rc;
+
+	*o_mcp_resp = mb_params.mcp_resp;
+	*o_mcp_param = mb_params.mcp_param;
+
+	*o_txn_size = *o_mcp_param;
+	memcpy(o_buf, &union_data.raw_data, *o_txn_size);
+
+	return 0;
+}
+
 int qed_mcp_load_req(struct qed_hwfn *p_hwfn,
-		     struct qed_ptt *p_ptt,
-		     u32 *p_load_code)
+		     struct qed_ptt *p_ptt, u32 *p_load_code)
 {
 	struct qed_dev *cdev = p_hwfn->cdev;
 	struct qed_mcp_mb_params mb_params;
@@ -527,8 +545,7 @@ static void qed_mcp_handle_transceiver_change(struct qed_hwfn *p_hwfn,
 		   "Received transceiver state update [0x%08x] from mfw [Addr 0x%x]\n",
 		   transceiver_state,
 		   (u32)(p_hwfn->mcp_info->port_addr +
-			 offsetof(struct public_port,
-				  transceiver_data)));
+			  offsetof(struct public_port, transceiver_data)));
 
 	transceiver_state = GET_FIELD(transceiver_state,
 				      ETH_TRANSCEIVER_STATE);
@@ -540,8 +557,7 @@ static void qed_mcp_handle_transceiver_change(struct qed_hwfn *p_hwfn,
 }
 
 static void qed_mcp_handle_link_change(struct qed_hwfn *p_hwfn,
-				       struct qed_ptt *p_ptt,
-				       bool b_reset)
+				       struct qed_ptt *p_ptt, bool b_reset)
 {
 	struct qed_mcp_link_state *p_link;
 	u8 max_bw, min_bw;
@@ -557,8 +573,7 @@ static void qed_mcp_handle_link_change(struct qed_hwfn *p_hwfn,
 			   "Received link update [0x%08x] from mfw [Addr 0x%x]\n",
 			   status,
 			   (u32)(p_hwfn->mcp_info->port_addr +
-				 offsetof(struct public_port,
-					  link_status)));
+				 offsetof(struct public_port, link_status)));
 	} else {
 		DP_VERBOSE(p_hwfn, NETIF_MSG_LINK,
 			   "Resetting link indications\n");
@@ -635,6 +650,9 @@ static void qed_mcp_handle_link_change(struct qed_hwfn *p_hwfn,
 		(status & LINK_STATUS_LINK_PARTNER_20G_CAPABLE) ?
 		QED_LINK_PARTNER_SPEED_20G : 0;
 	p_link->partner_adv_speed |=
+		(status & LINK_STATUS_LINK_PARTNER_25G_CAPABLE) ?
+		QED_LINK_PARTNER_SPEED_25G : 0;
+	p_link->partner_adv_speed |=
 		(status & LINK_STATUS_LINK_PARTNER_40G_CAPABLE) ?
 		QED_LINK_PARTNER_SPEED_40G : 0;
 	p_link->partner_adv_speed |=
@@ -722,6 +740,48 @@ int qed_mcp_set_link(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt, bool b_up)
 	return 0;
 }
 
+static void qed_mcp_send_protocol_stats(struct qed_hwfn *p_hwfn,
+					struct qed_ptt *p_ptt,
+					enum MFW_DRV_MSG_TYPE type)
+{
+	enum qed_mcp_protocol_type stats_type;
+	union qed_mcp_protocol_stats stats;
+	struct qed_mcp_mb_params mb_params;
+	union drv_union_data union_data;
+	u32 hsi_param;
+
+	switch (type) {
+	case MFW_DRV_MSG_GET_LAN_STATS:
+		stats_type = QED_MCP_LAN_STATS;
+		hsi_param = DRV_MSG_CODE_STATS_TYPE_LAN;
+		break;
+	case MFW_DRV_MSG_GET_FCOE_STATS:
+		stats_type = QED_MCP_FCOE_STATS;
+		hsi_param = DRV_MSG_CODE_STATS_TYPE_FCOE;
+		break;
+	case MFW_DRV_MSG_GET_ISCSI_STATS:
+		stats_type = QED_MCP_ISCSI_STATS;
+		hsi_param = DRV_MSG_CODE_STATS_TYPE_ISCSI;
+		break;
+	case MFW_DRV_MSG_GET_RDMA_STATS:
+		stats_type = QED_MCP_RDMA_STATS;
+		hsi_param = DRV_MSG_CODE_STATS_TYPE_RDMA;
+		break;
+	default:
+		DP_NOTICE(p_hwfn, "Invalid protocol type %d\n", type);
+		return;
+	}
+
+	qed_get_protocol_stats(p_hwfn->cdev, stats_type, &stats);
+
+	memset(&mb_params, 0, sizeof(mb_params));
+	mb_params.cmd = DRV_MSG_CODE_GET_STATS;
+	mb_params.param = hsi_param;
+	memcpy(&union_data, &stats, sizeof(stats));
+	mb_params.p_data_src = &union_data;
+	qed_mcp_cmd_and_union(p_hwfn, p_ptt, &mb_params);
+}
+
 static void qed_read_pf_bandwidth(struct qed_hwfn *p_hwfn,
 				  struct public_func *p_shmem_info)
 {
@@ -752,8 +812,7 @@ static void qed_read_pf_bandwidth(struct qed_hwfn *p_hwfn,
 
 static u32 qed_mcp_get_shmem_func(struct qed_hwfn *p_hwfn,
 				  struct qed_ptt *p_ptt,
-				  struct public_func *p_data,
-				  int pfid)
+				  struct public_func *p_data, int pfid)
 {
 	u32 addr = SECTION_OFFSIZE_ADDR(p_hwfn->mcp_info->public_base,
 					PUBLIC_FUNC);
@@ -763,51 +822,20 @@ static u32 qed_mcp_get_shmem_func(struct qed_hwfn *p_hwfn,
 
 	memset(p_data, 0, sizeof(*p_data));
 
-	size = min_t(u32, sizeof(*p_data),
-		     QED_SECTION_SIZE(mfw_path_offsize));
+	size = min_t(u32, sizeof(*p_data), QED_SECTION_SIZE(mfw_path_offsize));
 	for (i = 0; i < size / sizeof(u32); i++)
 		((u32 *)p_data)[i] = qed_rd(p_hwfn, p_ptt,
 					    func_addr + (i << 2));
 	return size;
 }
 
-int qed_hw_init_first_eth(struct qed_hwfn *p_hwfn,
-			  struct qed_ptt *p_ptt, u8 *p_pf)
-{
-	struct public_func shmem_info;
-	int i;
-
-	/* Find first Ethernet interface in port */
-	for (i = 0; i < NUM_OF_ENG_PFS(p_hwfn->cdev);
-	     i += p_hwfn->cdev->num_ports_in_engines) {
-		qed_mcp_get_shmem_func(p_hwfn, p_ptt, &shmem_info,
-				       MCP_PF_ID_BY_REL(p_hwfn, i));
-
-		if (shmem_info.config & FUNC_MF_CFG_FUNC_HIDE)
-			continue;
-
-		if ((shmem_info.config & FUNC_MF_CFG_PROTOCOL_MASK) ==
-		    FUNC_MF_CFG_PROTOCOL_ETHERNET) {
-			*p_pf = (u8)i;
-			return 0;
-		}
-	}
-
-	DP_NOTICE(p_hwfn,
-		  "Failed to find on port an ethernet interface in MF_SI mode\n");
-
-	return -EINVAL;
-}
-
-static void qed_mcp_update_bw(struct qed_hwfn *p_hwfn,
-			      struct qed_ptt *p_ptt)
+static void qed_mcp_update_bw(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
 {
 	struct qed_mcp_function_info *p_info;
 	struct public_func shmem_info;
 	u32 resp = 0, param = 0;
 
-	qed_mcp_get_shmem_func(p_hwfn, p_ptt, &shmem_info,
-			       MCP_PF_ID(p_hwfn));
+	qed_mcp_get_shmem_func(p_hwfn, p_ptt, &shmem_info, MCP_PF_ID(p_hwfn));
 
 	qed_read_pf_bandwidth(p_hwfn, &shmem_info);
 
@@ -867,6 +895,12 @@ int qed_mcp_handle_events(struct qed_hwfn *p_hwfn,
 		case MFW_DRV_MSG_TRANSCEIVER_STATE_CHANGE:
 			qed_mcp_handle_transceiver_change(p_hwfn, p_ptt);
 			break;
+		case MFW_DRV_MSG_GET_LAN_STATS:
+		case MFW_DRV_MSG_GET_FCOE_STATS:
+		case MFW_DRV_MSG_GET_ISCSI_STATS:
+		case MFW_DRV_MSG_GET_RDMA_STATS:
+			qed_mcp_send_protocol_stats(p_hwfn, p_ptt, i);
+			break;
 		case MFW_DRV_MSG_BW_UPDATE:
 			qed_mcp_update_bw(p_hwfn, p_ptt);
 			break;
@@ -940,8 +974,7 @@ int qed_mcp_get_mfw_ver(struct qed_hwfn *p_hwfn,
 	return 0;
 }
 
-int qed_mcp_get_media_type(struct qed_dev *cdev,
-			   u32 *p_media_type)
+int qed_mcp_get_media_type(struct qed_dev *cdev, u32 *p_media_type)
 {
 	struct qed_hwfn *p_hwfn = &cdev->hwfns[0];
 	struct qed_ptt  *p_ptt;
@@ -950,7 +983,7 @@ int qed_mcp_get_media_type(struct qed_dev *cdev,
 		return -EINVAL;
 
 	if (!qed_mcp_is_init(p_hwfn)) {
-		DP_NOTICE(p_hwfn, "MFW is not initialized !\n");
+		DP_NOTICE(p_hwfn, "MFW is not initialized!\n");
 		return -EBUSY;
 	}
 
@@ -1003,15 +1036,13 @@ int qed_mcp_fill_shmem_func_info(struct qed_hwfn *p_hwfn,
 	struct qed_mcp_function_info *info;
 	struct public_func shmem_info;
 
-	qed_mcp_get_shmem_func(p_hwfn, p_ptt, &shmem_info,
-			       MCP_PF_ID(p_hwfn));
+	qed_mcp_get_shmem_func(p_hwfn, p_ptt, &shmem_info, MCP_PF_ID(p_hwfn));
 	info = &p_hwfn->mcp_info->func_info;
 
 	info->pause_on_host = (shmem_info.config &
 			       FUNC_MF_CFG_PAUSE_ON_HOST_RING) ? 1 : 0;
 
-	if (qed_mcp_get_shmem_proto(p_hwfn, &shmem_info,
-				    &info->protocol)) {
+	if (qed_mcp_get_shmem_proto(p_hwfn, &shmem_info, &info->protocol)) {
 		DP_ERR(p_hwfn, "Unknown personality %08x\n",
 		       (u32)(shmem_info.config & FUNC_MF_CFG_PROTOCOL_MASK));
 		return -EINVAL;
@@ -1072,15 +1103,13 @@ struct qed_mcp_link_capabilities
 	return &p_hwfn->mcp_info->link_capabilities;
 }
 
-int qed_mcp_drain(struct qed_hwfn *p_hwfn,
-		  struct qed_ptt *p_ptt)
+int qed_mcp_drain(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
 {
 	u32 resp = 0, param = 0;
 	int rc;
 
 	rc = qed_mcp_cmd(p_hwfn, p_ptt,
-			 DRV_MSG_CODE_NIG_DRAIN, 1000,
-			 &resp, &param);
+			 DRV_MSG_CODE_NIG_DRAIN, 1000, &resp, &param);
 
 	/* Wait for the drain to complete before returning */
 	msleep(1020);
@@ -1089,8 +1118,7 @@ int qed_mcp_drain(struct qed_hwfn *p_hwfn,
 }
 
 int qed_mcp_get_flash_size(struct qed_hwfn *p_hwfn,
-			   struct qed_ptt *p_ptt,
-			   u32 *p_flash_size)
+			   struct qed_ptt *p_ptt, u32 *p_flash_size)
 {
 	u32 flash_size;
 
@@ -1168,8 +1196,35 @@ qed_mcp_send_drv_version(struct qed_hwfn *p_hwfn,
 	return rc;
 }
 
-int qed_mcp_set_led(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt,
-		    enum qed_led_mode mode)
+int qed_mcp_halt(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
+{
+	u32 resp = 0, param = 0;
+	int rc;
+
+	rc = qed_mcp_cmd(p_hwfn, p_ptt, DRV_MSG_CODE_MCP_HALT, 0, &resp,
+			 &param);
+	if (rc)
+		DP_ERR(p_hwfn, "MCP response failure, aborting\n");
+
+	return rc;
+}
+
+int qed_mcp_resume(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
+{
+	u32 value, cpu_mode;
+
+	qed_wr(p_hwfn, p_ptt, MCP_REG_CPU_STATE, 0xffffffff);
+
+	value = qed_rd(p_hwfn, p_ptt, MCP_REG_CPU_MODE);
+	value &= ~MCP_REG_CPU_MODE_SOFT_HALT;
+	qed_wr(p_hwfn, p_ptt, MCP_REG_CPU_MODE, value);
+	cpu_mode = qed_rd(p_hwfn, p_ptt, MCP_REG_CPU_MODE);
+
+	return (cpu_mode & MCP_REG_CPU_MODE_SOFT_HALT) ? -EAGAIN : 0;
+}
+
+int qed_mcp_set_led(struct qed_hwfn *p_hwfn,
+		    struct qed_ptt *p_ptt, enum qed_led_mode mode)
 {
 	u32 resp = 0, param = 0, drv_mb_param;
 	int rc;
@@ -1195,6 +1250,27 @@ int qed_mcp_set_led(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt,
 	return rc;
 }
 
+int qed_mcp_mask_parities(struct qed_hwfn *p_hwfn,
+			  struct qed_ptt *p_ptt, u32 mask_parities)
+{
+	u32 resp = 0, param = 0;
+	int rc;
+
+	rc = qed_mcp_cmd(p_hwfn, p_ptt, DRV_MSG_CODE_MASK_PARITIES,
+			 mask_parities, &resp, &param);
+
+	if (rc) {
+		DP_ERR(p_hwfn,
+		       "MCP response failure for mask parities, aborting\n");
+	} else if (resp != FW_MSG_CODE_OK) {
+		DP_ERR(p_hwfn,
+		       "MCP did not acknowledge mask parity request. Old MFW?\n");
+		rc = -EINVAL;
+	}
+
+	return rc;
+}
+
 int qed_mcp_bist_register_test(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
 {
 	u32 drv_mb_param = 0, rsp, param;
diff --git a/drivers/net/ethernet/qlogic/qed/qed_mcp.h b/drivers/net/ethernet/qlogic/qed/qed_mcp.h
index 7f319aa1b229..dff520ed069b 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_mcp.h
+++ b/drivers/net/ethernet/qlogic/qed/qed_mcp.h
@@ -60,9 +60,10 @@ struct qed_mcp_link_state {
 #define QED_LINK_PARTNER_SPEED_1G_FD    BIT(1)
 #define QED_LINK_PARTNER_SPEED_10G      BIT(2)
 #define QED_LINK_PARTNER_SPEED_20G      BIT(3)
-#define QED_LINK_PARTNER_SPEED_40G      BIT(4)
-#define QED_LINK_PARTNER_SPEED_50G      BIT(5)
-#define QED_LINK_PARTNER_SPEED_100G     BIT(6)
+#define QED_LINK_PARTNER_SPEED_25G      BIT(4)
+#define QED_LINK_PARTNER_SPEED_40G      BIT(5)
+#define QED_LINK_PARTNER_SPEED_50G      BIT(6)
+#define QED_LINK_PARTNER_SPEED_100G     BIT(7)
 	u32     partner_adv_speed;
 
 	bool    partner_tx_flow_ctrl_en;
@@ -105,6 +106,47 @@ struct qed_mcp_drv_version {
 	u8	name[MCP_DRV_VER_STR_SIZE - 4];
 };
 
+struct qed_mcp_lan_stats {
+	u64 ucast_rx_pkts;
+	u64 ucast_tx_pkts;
+	u32 fcs_err;
+};
+
+struct qed_mcp_fcoe_stats {
+	u64 rx_pkts;
+	u64 tx_pkts;
+	u32 fcs_err;
+	u32 login_failure;
+};
+
+struct qed_mcp_iscsi_stats {
+	u64 rx_pdus;
+	u64 tx_pdus;
+	u64 rx_bytes;
+	u64 tx_bytes;
+};
+
+struct qed_mcp_rdma_stats {
+	u64 rx_pkts;
+	u64 tx_pkts;
+	u64 rx_bytes;
+	u64 tx_byts;
+};
+
+enum qed_mcp_protocol_type {
+	QED_MCP_LAN_STATS,
+	QED_MCP_FCOE_STATS,
+	QED_MCP_ISCSI_STATS,
+	QED_MCP_RDMA_STATS
+};
+
+union qed_mcp_protocol_stats {
+	struct qed_mcp_lan_stats lan_stats;
+	struct qed_mcp_fcoe_stats fcoe_stats;
+	struct qed_mcp_iscsi_stats iscsi_stats;
+	struct qed_mcp_rdma_stats rdma_stats;
+};
+
 /**
  * @brief - returns the link params of the hw function
  *
@@ -426,6 +468,29 @@ int qed_mcp_reset(struct qed_hwfn *p_hwfn,
 		  struct qed_ptt *p_ptt);
 
 /**
+ * @brief - Sends an NVM read command request to the MFW to get
+ *        a buffer.
+ *
+ * @param p_hwfn
+ * @param p_ptt
+ * @param cmd - Command: DRV_MSG_CODE_NVM_GET_FILE_DATA or
+ *            DRV_MSG_CODE_NVM_READ_NVRAM commands
+ * @param param - [0:23] - Offset [24:31] - Size
+ * @param o_mcp_resp - MCP response
+ * @param o_mcp_param - MCP response param
+ * @param o_txn_size -  Buffer size output
+ * @param o_buf - Pointer to the buffer returned by the MFW.
+ *
+ * @param return 0 upon success.
+ */
+int qed_mcp_nvm_rd_cmd(struct qed_hwfn *p_hwfn,
+		       struct qed_ptt *p_ptt,
+		       u32 cmd,
+		       u32 param,
+		       u32 *o_mcp_resp,
+		       u32 *o_mcp_param, u32 *o_txn_size, u32 *o_buf);
+
+/**
  * @brief indicates whether the MFW objects [under mcp_info] are accessible
  *
  * @param p_hwfn
@@ -447,6 +512,26 @@ bool qed_mcp_is_init(struct qed_hwfn *p_hwfn);
 int qed_mcp_config_vf_msix(struct qed_hwfn *p_hwfn,
 			   struct qed_ptt *p_ptt, u8 vf_id, u8 num);
 
+/**
+ * @brief - Halt the MCP.
+ *
+ * @param p_hwfn
+ * @param p_ptt
+ *
+ * @param return 0 upon success.
+ */
+int qed_mcp_halt(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt);
+
+/**
+ * @brief - Wake up the MCP.
+ *
+ * @param p_hwfn
+ * @param p_ptt
+ *
+ * @param return 0 upon success.
+ */
+int qed_mcp_resume(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt);
+
 int qed_configure_pf_min_bandwidth(struct qed_dev *cdev, u8 min_bw);
 int qed_configure_pf_max_bandwidth(struct qed_dev *cdev, u8 max_bw);
 int __qed_configure_pf_max_bandwidth(struct qed_hwfn *p_hwfn,
@@ -458,6 +543,7 @@ int __qed_configure_pf_min_bandwidth(struct qed_hwfn *p_hwfn,
 				     struct qed_mcp_link_state *p_link,
 				     u8 min_bw);
 
-int qed_hw_init_first_eth(struct qed_hwfn *p_hwfn,
-			  struct qed_ptt *p_ptt, u8 *p_pf);
+int qed_mcp_mask_parities(struct qed_hwfn *p_hwfn,
+			  struct qed_ptt *p_ptt, u32 mask_parities);
+
 #endif
diff --git a/drivers/net/ethernet/qlogic/qed/qed_reg_addr.h b/drivers/net/ethernet/qlogic/qed/qed_reg_addr.h
index f6b86ca1ff79..b414a0542177 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_reg_addr.h
+++ b/drivers/net/ethernet/qlogic/qed/qed_reg_addr.h
@@ -116,8 +116,14 @@
 	0x1009c4UL
 #define  QM_REG_PF_EN \
 	0x2f2ea4UL
+#define TCFC_REG_WEAK_ENABLE_VF \
+	0x2d0704UL
 #define  TCFC_REG_STRONG_ENABLE_PF \
 	0x2d0708UL
+#define  TCFC_REG_STRONG_ENABLE_VF \
+	0x2d070cUL
+#define CCFC_REG_WEAK_ENABLE_VF \
+	0x2e0704UL
 #define  CCFC_REG_STRONG_ENABLE_PF \
 	0x2e0708UL
 #define  PGLUE_B_REG_PGL_ADDR_88_F0 \
@@ -202,6 +208,26 @@
 	0x50196cUL
 #define NIG_REG_LLH_CLS_TYPE_DUALMODE \
 	0x501964UL
+#define NIG_REG_LLH_FUNC_FILTER_VALUE \
+	0x501a00UL
+#define NIG_REG_LLH_FUNC_FILTER_VALUE_SIZE \
+	32
+#define NIG_REG_LLH_FUNC_FILTER_EN \
+	0x501a80UL
+#define NIG_REG_LLH_FUNC_FILTER_EN_SIZE	\
+	16
+#define NIG_REG_LLH_FUNC_FILTER_MODE \
+	0x501ac0UL
+#define NIG_REG_LLH_FUNC_FILTER_MODE_SIZE \
+	16
+#define NIG_REG_LLH_FUNC_FILTER_PROTOCOL_TYPE \
+	0x501b00UL
+#define NIG_REG_LLH_FUNC_FILTER_PROTOCOL_TYPE_SIZE \
+	16
+#define NIG_REG_LLH_FUNC_FILTER_HDR_SEL	\
+	0x501b40UL
+#define NIG_REG_LLH_FUNC_FILTER_HDR_SEL_SIZE \
+	16
 #define  NCSI_REG_CONFIG	\
 	0x040200UL
 #define  PBF_REG_INIT \
@@ -258,6 +284,8 @@
 	0x1f0a1cUL
 #define PRS_REG_ROCE_DEST_QP_MAX_PF \
 	0x1f0430UL
+#define PRS_REG_USE_LIGHT_L2 \
+	0x1f096cUL
 #define  PSDM_REG_ENABLE_IN1 \
 	0xfa0004UL
 #define  PSEM_REG_ENABLE_IN \
@@ -521,4 +549,910 @@
 
 #define QM_REG_WFQPFWEIGHT	0x2f4e80UL
 #define QM_REG_WFQVPWEIGHT	0x2fa000UL
+
+#define PGLCS_REG_DBG_SELECT \
+	0x001d14UL
+#define PGLCS_REG_DBG_DWORD_ENABLE \
+	0x001d18UL
+#define PGLCS_REG_DBG_SHIFT \
+	0x001d1cUL
+#define PGLCS_REG_DBG_FORCE_VALID \
+	0x001d20UL
+#define PGLCS_REG_DBG_FORCE_FRAME \
+	0x001d24UL
+#define MISC_REG_RESET_PL_PDA_VMAIN_1 \
+	0x008070UL
+#define MISC_REG_RESET_PL_PDA_VMAIN_2 \
+	0x008080UL
+#define MISC_REG_RESET_PL_PDA_VAUX \
+	0x008090UL
+#define MISCS_REG_RESET_PL_UA \
+	0x009050UL
+#define MISCS_REG_RESET_PL_HV \
+	0x009060UL
+#define MISCS_REG_RESET_PL_HV_2	\
+	0x009150UL
+#define DMAE_REG_DBG_SELECT \
+	0x00c510UL
+#define DMAE_REG_DBG_DWORD_ENABLE \
+	0x00c514UL
+#define DMAE_REG_DBG_SHIFT \
+	0x00c518UL
+#define DMAE_REG_DBG_FORCE_VALID \
+	0x00c51cUL
+#define DMAE_REG_DBG_FORCE_FRAME \
+	0x00c520UL
+#define NCSI_REG_DBG_SELECT \
+	0x040474UL
+#define NCSI_REG_DBG_DWORD_ENABLE \
+	0x040478UL
+#define NCSI_REG_DBG_SHIFT \
+	0x04047cUL
+#define NCSI_REG_DBG_FORCE_VALID \
+	0x040480UL
+#define NCSI_REG_DBG_FORCE_FRAME \
+	0x040484UL
+#define GRC_REG_DBG_SELECT \
+	0x0500a4UL
+#define GRC_REG_DBG_DWORD_ENABLE \
+	0x0500a8UL
+#define GRC_REG_DBG_SHIFT \
+	0x0500acUL
+#define GRC_REG_DBG_FORCE_VALID	\
+	0x0500b0UL
+#define GRC_REG_DBG_FORCE_FRAME	\
+	0x0500b4UL
+#define UMAC_REG_DBG_SELECT \
+	0x051094UL
+#define UMAC_REG_DBG_DWORD_ENABLE \
+	0x051098UL
+#define UMAC_REG_DBG_SHIFT \
+	0x05109cUL
+#define UMAC_REG_DBG_FORCE_VALID \
+	0x0510a0UL
+#define UMAC_REG_DBG_FORCE_FRAME \
+	0x0510a4UL
+#define MCP2_REG_DBG_SELECT \
+	0x052400UL
+#define MCP2_REG_DBG_DWORD_ENABLE \
+	0x052404UL
+#define MCP2_REG_DBG_SHIFT \
+	0x052408UL
+#define MCP2_REG_DBG_FORCE_VALID \
+	0x052440UL
+#define MCP2_REG_DBG_FORCE_FRAME \
+	0x052444UL
+#define PCIE_REG_DBG_SELECT \
+	0x0547e8UL
+#define PCIE_REG_DBG_DWORD_ENABLE \
+	0x0547ecUL
+#define PCIE_REG_DBG_SHIFT \
+	0x0547f0UL
+#define PCIE_REG_DBG_FORCE_VALID \
+	0x0547f4UL
+#define PCIE_REG_DBG_FORCE_FRAME \
+	0x0547f8UL
+#define DORQ_REG_DBG_SELECT \
+	0x100ad0UL
+#define DORQ_REG_DBG_DWORD_ENABLE \
+	0x100ad4UL
+#define DORQ_REG_DBG_SHIFT \
+	0x100ad8UL
+#define DORQ_REG_DBG_FORCE_VALID \
+	0x100adcUL
+#define DORQ_REG_DBG_FORCE_FRAME \
+	0x100ae0UL
+#define IGU_REG_DBG_SELECT \
+	0x181578UL
+#define IGU_REG_DBG_DWORD_ENABLE \
+	0x18157cUL
+#define IGU_REG_DBG_SHIFT \
+	0x181580UL
+#define IGU_REG_DBG_FORCE_VALID	\
+	0x181584UL
+#define IGU_REG_DBG_FORCE_FRAME	\
+	0x181588UL
+#define CAU_REG_DBG_SELECT \
+	0x1c0ea8UL
+#define CAU_REG_DBG_DWORD_ENABLE \
+	0x1c0eacUL
+#define CAU_REG_DBG_SHIFT \
+	0x1c0eb0UL
+#define CAU_REG_DBG_FORCE_VALID	\
+	0x1c0eb4UL
+#define CAU_REG_DBG_FORCE_FRAME	\
+	0x1c0eb8UL
+#define PRS_REG_DBG_SELECT \
+	0x1f0b6cUL
+#define PRS_REG_DBG_DWORD_ENABLE \
+	0x1f0b70UL
+#define PRS_REG_DBG_SHIFT \
+	0x1f0b74UL
+#define PRS_REG_DBG_FORCE_VALID	\
+	0x1f0ba0UL
+#define PRS_REG_DBG_FORCE_FRAME	\
+	0x1f0ba4UL
+#define CNIG_REG_DBG_SELECT_K2 \
+	0x218254UL
+#define CNIG_REG_DBG_DWORD_ENABLE_K2 \
+	0x218258UL
+#define CNIG_REG_DBG_SHIFT_K2 \
+	0x21825cUL
+#define CNIG_REG_DBG_FORCE_VALID_K2 \
+	0x218260UL
+#define CNIG_REG_DBG_FORCE_FRAME_K2 \
+	0x218264UL
+#define PRM_REG_DBG_SELECT \
+	0x2306a8UL
+#define PRM_REG_DBG_DWORD_ENABLE \
+	0x2306acUL
+#define PRM_REG_DBG_SHIFT \
+	0x2306b0UL
+#define PRM_REG_DBG_FORCE_VALID	\
+	0x2306b4UL
+#define PRM_REG_DBG_FORCE_FRAME	\
+	0x2306b8UL
+#define SRC_REG_DBG_SELECT \
+	0x238700UL
+#define SRC_REG_DBG_DWORD_ENABLE \
+	0x238704UL
+#define SRC_REG_DBG_SHIFT \
+	0x238708UL
+#define SRC_REG_DBG_FORCE_VALID	\
+	0x23870cUL
+#define SRC_REG_DBG_FORCE_FRAME	\
+	0x238710UL
+#define RSS_REG_DBG_SELECT \
+	0x238c4cUL
+#define RSS_REG_DBG_DWORD_ENABLE \
+	0x238c50UL
+#define RSS_REG_DBG_SHIFT \
+	0x238c54UL
+#define RSS_REG_DBG_FORCE_VALID	\
+	0x238c58UL
+#define RSS_REG_DBG_FORCE_FRAME	\
+	0x238c5cUL
+#define RPB_REG_DBG_SELECT \
+	0x23c728UL
+#define RPB_REG_DBG_DWORD_ENABLE \
+	0x23c72cUL
+#define RPB_REG_DBG_SHIFT \
+	0x23c730UL
+#define RPB_REG_DBG_FORCE_VALID	\
+	0x23c734UL
+#define RPB_REG_DBG_FORCE_FRAME	\
+	0x23c738UL
+#define PSWRQ2_REG_DBG_SELECT \
+	0x240100UL
+#define PSWRQ2_REG_DBG_DWORD_ENABLE \
+	0x240104UL
+#define PSWRQ2_REG_DBG_SHIFT \
+	0x240108UL
+#define PSWRQ2_REG_DBG_FORCE_VALID \
+	0x24010cUL
+#define PSWRQ2_REG_DBG_FORCE_FRAME \
+	0x240110UL
+#define PSWRQ_REG_DBG_SELECT \
+	0x280020UL
+#define PSWRQ_REG_DBG_DWORD_ENABLE \
+	0x280024UL
+#define PSWRQ_REG_DBG_SHIFT \
+	0x280028UL
+#define PSWRQ_REG_DBG_FORCE_VALID \
+	0x28002cUL
+#define PSWRQ_REG_DBG_FORCE_FRAME \
+	0x280030UL
+#define PSWWR_REG_DBG_SELECT \
+	0x29a084UL
+#define PSWWR_REG_DBG_DWORD_ENABLE \
+	0x29a088UL
+#define PSWWR_REG_DBG_SHIFT \
+	0x29a08cUL
+#define PSWWR_REG_DBG_FORCE_VALID \
+	0x29a090UL
+#define PSWWR_REG_DBG_FORCE_FRAME \
+	0x29a094UL
+#define PSWRD_REG_DBG_SELECT \
+	0x29c040UL
+#define PSWRD_REG_DBG_DWORD_ENABLE \
+	0x29c044UL
+#define PSWRD_REG_DBG_SHIFT \
+	0x29c048UL
+#define PSWRD_REG_DBG_FORCE_VALID \
+	0x29c04cUL
+#define PSWRD_REG_DBG_FORCE_FRAME \
+	0x29c050UL
+#define PSWRD2_REG_DBG_SELECT \
+	0x29d400UL
+#define PSWRD2_REG_DBG_DWORD_ENABLE \
+	0x29d404UL
+#define PSWRD2_REG_DBG_SHIFT \
+	0x29d408UL
+#define PSWRD2_REG_DBG_FORCE_VALID \
+	0x29d40cUL
+#define PSWRD2_REG_DBG_FORCE_FRAME \
+	0x29d410UL
+#define PSWHST2_REG_DBG_SELECT \
+	0x29e058UL
+#define PSWHST2_REG_DBG_DWORD_ENABLE \
+	0x29e05cUL
+#define PSWHST2_REG_DBG_SHIFT \
+	0x29e060UL
+#define PSWHST2_REG_DBG_FORCE_VALID \
+	0x29e064UL
+#define PSWHST2_REG_DBG_FORCE_FRAME \
+	0x29e068UL
+#define PSWHST_REG_DBG_SELECT \
+	0x2a0100UL
+#define PSWHST_REG_DBG_DWORD_ENABLE \
+	0x2a0104UL
+#define PSWHST_REG_DBG_SHIFT \
+	0x2a0108UL
+#define PSWHST_REG_DBG_FORCE_VALID \
+	0x2a010cUL
+#define PSWHST_REG_DBG_FORCE_FRAME \
+	0x2a0110UL
+#define PGLUE_B_REG_DBG_SELECT \
+	0x2a8400UL
+#define PGLUE_B_REG_DBG_DWORD_ENABLE \
+	0x2a8404UL
+#define PGLUE_B_REG_DBG_SHIFT \
+	0x2a8408UL
+#define PGLUE_B_REG_DBG_FORCE_VALID \
+	0x2a840cUL
+#define PGLUE_B_REG_DBG_FORCE_FRAME \
+	0x2a8410UL
+#define TM_REG_DBG_SELECT \
+	0x2c07a8UL
+#define TM_REG_DBG_DWORD_ENABLE	\
+	0x2c07acUL
+#define TM_REG_DBG_SHIFT \
+	0x2c07b0UL
+#define TM_REG_DBG_FORCE_VALID \
+	0x2c07b4UL
+#define TM_REG_DBG_FORCE_FRAME \
+	0x2c07b8UL
+#define TCFC_REG_DBG_SELECT \
+	0x2d0500UL
+#define TCFC_REG_DBG_DWORD_ENABLE \
+	0x2d0504UL
+#define TCFC_REG_DBG_SHIFT \
+	0x2d0508UL
+#define TCFC_REG_DBG_FORCE_VALID \
+	0x2d050cUL
+#define TCFC_REG_DBG_FORCE_FRAME \
+	0x2d0510UL
+#define CCFC_REG_DBG_SELECT \
+	0x2e0500UL
+#define CCFC_REG_DBG_DWORD_ENABLE \
+	0x2e0504UL
+#define CCFC_REG_DBG_SHIFT \
+	0x2e0508UL
+#define CCFC_REG_DBG_FORCE_VALID \
+	0x2e050cUL
+#define CCFC_REG_DBG_FORCE_FRAME \
+	0x2e0510UL
+#define QM_REG_DBG_SELECT \
+	0x2f2e74UL
+#define QM_REG_DBG_DWORD_ENABLE	\
+	0x2f2e78UL
+#define QM_REG_DBG_SHIFT \
+	0x2f2e7cUL
+#define QM_REG_DBG_FORCE_VALID \
+	0x2f2e80UL
+#define QM_REG_DBG_FORCE_FRAME \
+	0x2f2e84UL
+#define RDIF_REG_DBG_SELECT \
+	0x300500UL
+#define RDIF_REG_DBG_DWORD_ENABLE \
+	0x300504UL
+#define RDIF_REG_DBG_SHIFT \
+	0x300508UL
+#define RDIF_REG_DBG_FORCE_VALID \
+	0x30050cUL
+#define RDIF_REG_DBG_FORCE_FRAME \
+	0x300510UL
+#define TDIF_REG_DBG_SELECT \
+	0x310500UL
+#define TDIF_REG_DBG_DWORD_ENABLE \
+	0x310504UL
+#define TDIF_REG_DBG_SHIFT \
+	0x310508UL
+#define TDIF_REG_DBG_FORCE_VALID \
+	0x31050cUL
+#define TDIF_REG_DBG_FORCE_FRAME \
+	0x310510UL
+#define BRB_REG_DBG_SELECT \
+	0x340ed0UL
+#define BRB_REG_DBG_DWORD_ENABLE \
+	0x340ed4UL
+#define BRB_REG_DBG_SHIFT \
+	0x340ed8UL
+#define BRB_REG_DBG_FORCE_VALID	\
+	0x340edcUL
+#define BRB_REG_DBG_FORCE_FRAME	\
+	0x340ee0UL
+#define XYLD_REG_DBG_SELECT \
+	0x4c1600UL
+#define XYLD_REG_DBG_DWORD_ENABLE \
+	0x4c1604UL
+#define XYLD_REG_DBG_SHIFT \
+	0x4c1608UL
+#define XYLD_REG_DBG_FORCE_VALID \
+	0x4c160cUL
+#define XYLD_REG_DBG_FORCE_FRAME \
+	0x4c1610UL
+#define YULD_REG_DBG_SELECT \
+	0x4c9600UL
+#define YULD_REG_DBG_DWORD_ENABLE \
+	0x4c9604UL
+#define YULD_REG_DBG_SHIFT \
+	0x4c9608UL
+#define YULD_REG_DBG_FORCE_VALID \
+	0x4c960cUL
+#define YULD_REG_DBG_FORCE_FRAME \
+	0x4c9610UL
+#define TMLD_REG_DBG_SELECT \
+	0x4d1600UL
+#define TMLD_REG_DBG_DWORD_ENABLE \
+	0x4d1604UL
+#define TMLD_REG_DBG_SHIFT \
+	0x4d1608UL
+#define TMLD_REG_DBG_FORCE_VALID \
+	0x4d160cUL
+#define TMLD_REG_DBG_FORCE_FRAME \
+	0x4d1610UL
+#define MULD_REG_DBG_SELECT \
+	0x4e1600UL
+#define MULD_REG_DBG_DWORD_ENABLE \
+	0x4e1604UL
+#define MULD_REG_DBG_SHIFT \
+	0x4e1608UL
+#define MULD_REG_DBG_FORCE_VALID \
+	0x4e160cUL
+#define MULD_REG_DBG_FORCE_FRAME \
+	0x4e1610UL
+#define NIG_REG_DBG_SELECT \
+	0x502140UL
+#define NIG_REG_DBG_DWORD_ENABLE \
+	0x502144UL
+#define NIG_REG_DBG_SHIFT \
+	0x502148UL
+#define NIG_REG_DBG_FORCE_VALID	\
+	0x50214cUL
+#define NIG_REG_DBG_FORCE_FRAME	\
+	0x502150UL
+#define BMB_REG_DBG_SELECT \
+	0x540a7cUL
+#define BMB_REG_DBG_DWORD_ENABLE \
+	0x540a80UL
+#define BMB_REG_DBG_SHIFT \
+	0x540a84UL
+#define BMB_REG_DBG_FORCE_VALID	\
+	0x540a88UL
+#define BMB_REG_DBG_FORCE_FRAME	\
+	0x540a8cUL
+#define PTU_REG_DBG_SELECT \
+	0x560100UL
+#define PTU_REG_DBG_DWORD_ENABLE \
+	0x560104UL
+#define PTU_REG_DBG_SHIFT \
+	0x560108UL
+#define PTU_REG_DBG_FORCE_VALID	\
+	0x56010cUL
+#define PTU_REG_DBG_FORCE_FRAME	\
+	0x560110UL
+#define CDU_REG_DBG_SELECT \
+	0x580704UL
+#define CDU_REG_DBG_DWORD_ENABLE \
+	0x580708UL
+#define CDU_REG_DBG_SHIFT \
+	0x58070cUL
+#define CDU_REG_DBG_FORCE_VALID	\
+	0x580710UL
+#define CDU_REG_DBG_FORCE_FRAME	\
+	0x580714UL
+#define WOL_REG_DBG_SELECT \
+	0x600140UL
+#define WOL_REG_DBG_DWORD_ENABLE \
+	0x600144UL
+#define WOL_REG_DBG_SHIFT \
+	0x600148UL
+#define WOL_REG_DBG_FORCE_VALID	\
+	0x60014cUL
+#define WOL_REG_DBG_FORCE_FRAME	\
+	0x600150UL
+#define BMBN_REG_DBG_SELECT \
+	0x610140UL
+#define BMBN_REG_DBG_DWORD_ENABLE \
+	0x610144UL
+#define BMBN_REG_DBG_SHIFT \
+	0x610148UL
+#define BMBN_REG_DBG_FORCE_VALID \
+	0x61014cUL
+#define BMBN_REG_DBG_FORCE_FRAME \
+	0x610150UL
+#define NWM_REG_DBG_SELECT \
+	0x8000ecUL
+#define NWM_REG_DBG_DWORD_ENABLE \
+	0x8000f0UL
+#define NWM_REG_DBG_SHIFT \
+	0x8000f4UL
+#define NWM_REG_DBG_FORCE_VALID	\
+	0x8000f8UL
+#define NWM_REG_DBG_FORCE_FRAME	\
+	0x8000fcUL
+#define PBF_REG_DBG_SELECT \
+	0xd80060UL
+#define PBF_REG_DBG_DWORD_ENABLE \
+	0xd80064UL
+#define PBF_REG_DBG_SHIFT \
+	0xd80068UL
+#define PBF_REG_DBG_FORCE_VALID	\
+	0xd8006cUL
+#define PBF_REG_DBG_FORCE_FRAME	\
+	0xd80070UL
+#define PBF_PB1_REG_DBG_SELECT \
+	0xda0728UL
+#define PBF_PB1_REG_DBG_DWORD_ENABLE \
+	0xda072cUL
+#define PBF_PB1_REG_DBG_SHIFT \
+	0xda0730UL
+#define PBF_PB1_REG_DBG_FORCE_VALID \
+	0xda0734UL
+#define PBF_PB1_REG_DBG_FORCE_FRAME \
+	0xda0738UL
+#define PBF_PB2_REG_DBG_SELECT \
+	0xda4728UL
+#define PBF_PB2_REG_DBG_DWORD_ENABLE \
+	0xda472cUL
+#define PBF_PB2_REG_DBG_SHIFT \
+	0xda4730UL
+#define PBF_PB2_REG_DBG_FORCE_VALID \
+	0xda4734UL
+#define PBF_PB2_REG_DBG_FORCE_FRAME \
+	0xda4738UL
+#define BTB_REG_DBG_SELECT \
+	0xdb08c8UL
+#define BTB_REG_DBG_DWORD_ENABLE \
+	0xdb08ccUL
+#define BTB_REG_DBG_SHIFT \
+	0xdb08d0UL
+#define BTB_REG_DBG_FORCE_VALID	\
+	0xdb08d4UL
+#define BTB_REG_DBG_FORCE_FRAME	\
+	0xdb08d8UL
+#define XSDM_REG_DBG_SELECT \
+	0xf80e28UL
+#define XSDM_REG_DBG_DWORD_ENABLE \
+	0xf80e2cUL
+#define XSDM_REG_DBG_SHIFT \
+	0xf80e30UL
+#define XSDM_REG_DBG_FORCE_VALID \
+	0xf80e34UL
+#define XSDM_REG_DBG_FORCE_FRAME \
+	0xf80e38UL
+#define YSDM_REG_DBG_SELECT \
+	0xf90e28UL
+#define YSDM_REG_DBG_DWORD_ENABLE \
+	0xf90e2cUL
+#define YSDM_REG_DBG_SHIFT \
+	0xf90e30UL
+#define YSDM_REG_DBG_FORCE_VALID \
+	0xf90e34UL
+#define YSDM_REG_DBG_FORCE_FRAME \
+	0xf90e38UL
+#define PSDM_REG_DBG_SELECT \
+	0xfa0e28UL
+#define PSDM_REG_DBG_DWORD_ENABLE \
+	0xfa0e2cUL
+#define PSDM_REG_DBG_SHIFT \
+	0xfa0e30UL
+#define PSDM_REG_DBG_FORCE_VALID \
+	0xfa0e34UL
+#define PSDM_REG_DBG_FORCE_FRAME \
+	0xfa0e38UL
+#define TSDM_REG_DBG_SELECT \
+	0xfb0e28UL
+#define TSDM_REG_DBG_DWORD_ENABLE \
+	0xfb0e2cUL
+#define TSDM_REG_DBG_SHIFT \
+	0xfb0e30UL
+#define TSDM_REG_DBG_FORCE_VALID \
+	0xfb0e34UL
+#define TSDM_REG_DBG_FORCE_FRAME \
+	0xfb0e38UL
+#define MSDM_REG_DBG_SELECT \
+	0xfc0e28UL
+#define MSDM_REG_DBG_DWORD_ENABLE \
+	0xfc0e2cUL
+#define MSDM_REG_DBG_SHIFT \
+	0xfc0e30UL
+#define MSDM_REG_DBG_FORCE_VALID \
+	0xfc0e34UL
+#define MSDM_REG_DBG_FORCE_FRAME \
+	0xfc0e38UL
+#define USDM_REG_DBG_SELECT \
+	0xfd0e28UL
+#define USDM_REG_DBG_DWORD_ENABLE \
+	0xfd0e2cUL
+#define USDM_REG_DBG_SHIFT \
+	0xfd0e30UL
+#define USDM_REG_DBG_FORCE_VALID \
+	0xfd0e34UL
+#define USDM_REG_DBG_FORCE_FRAME \
+	0xfd0e38UL
+#define XCM_REG_DBG_SELECT \
+	0x1000040UL
+#define XCM_REG_DBG_DWORD_ENABLE \
+	0x1000044UL
+#define XCM_REG_DBG_SHIFT \
+	0x1000048UL
+#define XCM_REG_DBG_FORCE_VALID	\
+	0x100004cUL
+#define XCM_REG_DBG_FORCE_FRAME	\
+	0x1000050UL
+#define YCM_REG_DBG_SELECT \
+	0x1080040UL
+#define YCM_REG_DBG_DWORD_ENABLE \
+	0x1080044UL
+#define YCM_REG_DBG_SHIFT \
+	0x1080048UL
+#define YCM_REG_DBG_FORCE_VALID	\
+	0x108004cUL
+#define YCM_REG_DBG_FORCE_FRAME	\
+	0x1080050UL
+#define PCM_REG_DBG_SELECT \
+	0x1100040UL
+#define PCM_REG_DBG_DWORD_ENABLE \
+	0x1100044UL
+#define PCM_REG_DBG_SHIFT \
+	0x1100048UL
+#define PCM_REG_DBG_FORCE_VALID	\
+	0x110004cUL
+#define PCM_REG_DBG_FORCE_FRAME	\
+	0x1100050UL
+#define TCM_REG_DBG_SELECT \
+	0x1180040UL
+#define TCM_REG_DBG_DWORD_ENABLE \
+	0x1180044UL
+#define TCM_REG_DBG_SHIFT \
+	0x1180048UL
+#define TCM_REG_DBG_FORCE_VALID	\
+	0x118004cUL
+#define TCM_REG_DBG_FORCE_FRAME	\
+	0x1180050UL
+#define MCM_REG_DBG_SELECT \
+	0x1200040UL
+#define MCM_REG_DBG_DWORD_ENABLE \
+	0x1200044UL
+#define MCM_REG_DBG_SHIFT \
+	0x1200048UL
+#define MCM_REG_DBG_FORCE_VALID	\
+	0x120004cUL
+#define MCM_REG_DBG_FORCE_FRAME	\
+	0x1200050UL
+#define UCM_REG_DBG_SELECT \
+	0x1280050UL
+#define UCM_REG_DBG_DWORD_ENABLE \
+	0x1280054UL
+#define UCM_REG_DBG_SHIFT \
+	0x1280058UL
+#define UCM_REG_DBG_FORCE_VALID	\
+	0x128005cUL
+#define UCM_REG_DBG_FORCE_FRAME	\
+	0x1280060UL
+#define XSEM_REG_DBG_SELECT \
+	0x1401528UL
+#define XSEM_REG_DBG_DWORD_ENABLE \
+	0x140152cUL
+#define XSEM_REG_DBG_SHIFT \
+	0x1401530UL
+#define XSEM_REG_DBG_FORCE_VALID \
+	0x1401534UL
+#define XSEM_REG_DBG_FORCE_FRAME \
+	0x1401538UL
+#define YSEM_REG_DBG_SELECT \
+	0x1501528UL
+#define YSEM_REG_DBG_DWORD_ENABLE \
+	0x150152cUL
+#define YSEM_REG_DBG_SHIFT \
+	0x1501530UL
+#define YSEM_REG_DBG_FORCE_VALID \
+	0x1501534UL
+#define YSEM_REG_DBG_FORCE_FRAME \
+	0x1501538UL
+#define PSEM_REG_DBG_SELECT \
+	0x1601528UL
+#define PSEM_REG_DBG_DWORD_ENABLE \
+	0x160152cUL
+#define PSEM_REG_DBG_SHIFT \
+	0x1601530UL
+#define PSEM_REG_DBG_FORCE_VALID \
+	0x1601534UL
+#define PSEM_REG_DBG_FORCE_FRAME \
+	0x1601538UL
+#define TSEM_REG_DBG_SELECT \
+	0x1701528UL
+#define TSEM_REG_DBG_DWORD_ENABLE \
+	0x170152cUL
+#define TSEM_REG_DBG_SHIFT \
+	0x1701530UL
+#define TSEM_REG_DBG_FORCE_VALID \
+	0x1701534UL
+#define TSEM_REG_DBG_FORCE_FRAME \
+	0x1701538UL
+#define MSEM_REG_DBG_SELECT \
+	0x1801528UL
+#define MSEM_REG_DBG_DWORD_ENABLE \
+	0x180152cUL
+#define MSEM_REG_DBG_SHIFT \
+	0x1801530UL
+#define MSEM_REG_DBG_FORCE_VALID \
+	0x1801534UL
+#define MSEM_REG_DBG_FORCE_FRAME \
+	0x1801538UL
+#define USEM_REG_DBG_SELECT \
+	0x1901528UL
+#define USEM_REG_DBG_DWORD_ENABLE \
+	0x190152cUL
+#define USEM_REG_DBG_SHIFT \
+	0x1901530UL
+#define USEM_REG_DBG_FORCE_VALID \
+	0x1901534UL
+#define USEM_REG_DBG_FORCE_FRAME \
+	0x1901538UL
+#define PCIE_REG_DBG_COMMON_SELECT \
+	0x054398UL
+#define PCIE_REG_DBG_COMMON_DWORD_ENABLE \
+	0x05439cUL
+#define PCIE_REG_DBG_COMMON_SHIFT \
+	0x0543a0UL
+#define PCIE_REG_DBG_COMMON_FORCE_VALID	\
+	0x0543a4UL
+#define PCIE_REG_DBG_COMMON_FORCE_FRAME	\
+	0x0543a8UL
+#define MISC_REG_RESET_PL_UA \
+	0x008050UL
+#define MISC_REG_RESET_PL_HV \
+	0x008060UL
+#define XCM_REG_CTX_RBC_ACCS \
+	0x1001800UL
+#define XCM_REG_AGG_CON_CTX \
+	0x1001804UL
+#define XCM_REG_SM_CON_CTX \
+	0x1001808UL
+#define YCM_REG_CTX_RBC_ACCS \
+	0x1081800UL
+#define YCM_REG_AGG_CON_CTX \
+	0x1081804UL
+#define YCM_REG_AGG_TASK_CTX \
+	0x1081808UL
+#define YCM_REG_SM_CON_CTX \
+	0x108180cUL
+#define YCM_REG_SM_TASK_CTX \
+	0x1081810UL
+#define PCM_REG_CTX_RBC_ACCS \
+	0x1101440UL
+#define PCM_REG_SM_CON_CTX \
+	0x1101444UL
+#define TCM_REG_CTX_RBC_ACCS \
+	0x11814c0UL
+#define TCM_REG_AGG_CON_CTX \
+	0x11814c4UL
+#define TCM_REG_AGG_TASK_CTX \
+	0x11814c8UL
+#define TCM_REG_SM_CON_CTX \
+	0x11814ccUL
+#define TCM_REG_SM_TASK_CTX \
+	0x11814d0UL
+#define MCM_REG_CTX_RBC_ACCS \
+	0x1201800UL
+#define MCM_REG_AGG_CON_CTX \
+	0x1201804UL
+#define MCM_REG_AGG_TASK_CTX \
+	0x1201808UL
+#define MCM_REG_SM_CON_CTX \
+	0x120180cUL
+#define MCM_REG_SM_TASK_CTX \
+	0x1201810UL
+#define UCM_REG_CTX_RBC_ACCS \
+	0x1281700UL
+#define UCM_REG_AGG_CON_CTX \
+	0x1281704UL
+#define UCM_REG_AGG_TASK_CTX \
+	0x1281708UL
+#define UCM_REG_SM_CON_CTX \
+	0x128170cUL
+#define UCM_REG_SM_TASK_CTX \
+	0x1281710UL
+#define XSEM_REG_SLOW_DBG_EMPTY	\
+	0x1401140UL
+#define XSEM_REG_SYNC_DBG_EMPTY	\
+	0x1401160UL
+#define XSEM_REG_SLOW_DBG_ACTIVE \
+	0x1401400UL
+#define XSEM_REG_SLOW_DBG_MODE \
+	0x1401404UL
+#define XSEM_REG_DBG_FRAME_MODE	\
+	0x1401408UL
+#define XSEM_REG_DBG_MODE1_CFG \
+	0x1401420UL
+#define XSEM_REG_FAST_MEMORY \
+	0x1440000UL
+#define YSEM_REG_SYNC_DBG_EMPTY	\
+	0x1501160UL
+#define YSEM_REG_SLOW_DBG_ACTIVE \
+	0x1501400UL
+#define YSEM_REG_SLOW_DBG_MODE \
+	0x1501404UL
+#define YSEM_REG_DBG_FRAME_MODE	\
+	0x1501408UL
+#define YSEM_REG_DBG_MODE1_CFG \
+	0x1501420UL
+#define YSEM_REG_FAST_MEMORY \
+	0x1540000UL
+#define PSEM_REG_SLOW_DBG_EMPTY	\
+	0x1601140UL
+#define PSEM_REG_SYNC_DBG_EMPTY	\
+	0x1601160UL
+#define PSEM_REG_SLOW_DBG_ACTIVE \
+	0x1601400UL
+#define PSEM_REG_SLOW_DBG_MODE \
+	0x1601404UL
+#define PSEM_REG_DBG_FRAME_MODE	\
+	0x1601408UL
+#define PSEM_REG_DBG_MODE1_CFG \
+	0x1601420UL
+#define PSEM_REG_FAST_MEMORY \
+	0x1640000UL
+#define TSEM_REG_SLOW_DBG_EMPTY	\
+	0x1701140UL
+#define TSEM_REG_SYNC_DBG_EMPTY	\
+	0x1701160UL
+#define TSEM_REG_SLOW_DBG_ACTIVE \
+	0x1701400UL
+#define TSEM_REG_SLOW_DBG_MODE \
+	0x1701404UL
+#define TSEM_REG_DBG_FRAME_MODE	\
+	0x1701408UL
+#define TSEM_REG_DBG_MODE1_CFG \
+	0x1701420UL
+#define TSEM_REG_FAST_MEMORY \
+	0x1740000UL
+#define MSEM_REG_SLOW_DBG_EMPTY	\
+	0x1801140UL
+#define MSEM_REG_SYNC_DBG_EMPTY	\
+	0x1801160UL
+#define MSEM_REG_SLOW_DBG_ACTIVE \
+	0x1801400UL
+#define MSEM_REG_SLOW_DBG_MODE \
+	0x1801404UL
+#define MSEM_REG_DBG_FRAME_MODE	\
+	0x1801408UL
+#define MSEM_REG_DBG_MODE1_CFG \
+	0x1801420UL
+#define MSEM_REG_FAST_MEMORY \
+	0x1840000UL
+#define USEM_REG_SLOW_DBG_EMPTY	\
+	0x1901140UL
+#define USEM_REG_SYNC_DBG_EMPTY	\
+	0x1901160UL
+#define USEM_REG_SLOW_DBG_ACTIVE \
+	0x1901400UL
+#define USEM_REG_SLOW_DBG_MODE \
+	0x1901404UL
+#define USEM_REG_DBG_FRAME_MODE	\
+	0x1901408UL
+#define USEM_REG_DBG_MODE1_CFG \
+	0x1901420UL
+#define USEM_REG_FAST_MEMORY \
+	0x1940000UL
+#define SEM_FAST_REG_INT_RAM \
+	0x020000UL
+#define SEM_FAST_REG_INT_RAM_SIZE \
+	20480
+#define GRC_REG_TRACE_FIFO_VALID_DATA \
+	0x050064UL
+#define GRC_REG_NUMBER_VALID_OVERRIDE_WINDOW \
+	0x05040cUL
+#define GRC_REG_PROTECTION_OVERRIDE_WINDOW \
+	0x050500UL
+#define IGU_REG_ERROR_HANDLING_MEMORY \
+	0x181520UL
+#define MCP_REG_CPU_MODE \
+	0xe05000UL
+#define MCP_REG_CPU_MODE_SOFT_HALT \
+		(0x1 << 10)
+#define BRB_REG_BIG_RAM_ADDRESS \
+	0x340800UL
+#define BRB_REG_BIG_RAM_DATA \
+	0x341500UL
+#define SEM_FAST_REG_STALL_0 \
+	0x000488UL
+#define SEM_FAST_REG_STALLED \
+	0x000494UL
+#define BTB_REG_BIG_RAM_ADDRESS \
+	0xdb0800UL
+#define BTB_REG_BIG_RAM_DATA \
+	0xdb0c00UL
+#define BMB_REG_BIG_RAM_ADDRESS \
+	0x540800UL
+#define BMB_REG_BIG_RAM_DATA \
+	0x540f00UL
+#define SEM_FAST_REG_STORM_REG_FILE \
+	0x008000UL
+#define RSS_REG_RSS_RAM_ADDR \
+	0x238c30UL
+#define MISCS_REG_BLOCK_256B_EN \
+	0x009074UL
+#define MCP_REG_SCRATCH_SIZE \
+	57344
+#define MCP_REG_CPU_REG_FILE \
+	0xe05200UL
+#define MCP_REG_CPU_REG_FILE_SIZE \
+	32
+#define DBG_REG_DEBUG_TARGET \
+	0x01005cUL
+#define DBG_REG_FULL_MODE \
+	0x010060UL
+#define DBG_REG_CALENDAR_OUT_DATA \
+	0x010480UL
+#define GRC_REG_TRACE_FIFO \
+	0x050068UL
+#define IGU_REG_ERROR_HANDLING_DATA_VALID \
+	0x181530UL
+#define DBG_REG_DBG_BLOCK_ON \
+	0x010454UL
+#define DBG_REG_FRAMING_MODE \
+	0x010058UL
+#define SEM_FAST_REG_VFC_DATA_WR \
+	0x000b40UL
+#define SEM_FAST_REG_VFC_ADDR \
+	0x000b44UL
+#define SEM_FAST_REG_VFC_DATA_RD \
+	0x000b48UL
+#define RSS_REG_RSS_RAM_DATA \
+	0x238c20UL
+#define MISC_REG_BLOCK_256B_EN \
+	0x008c14UL
+#define NWS_REG_NWS_CMU	\
+	0x720000UL
+#define PHY_NW_IP_REG_PHY0_TOP_TBUS_ADDR_7_0 \
+	0x000680UL
+#define PHY_NW_IP_REG_PHY0_TOP_TBUS_ADDR_15_8 \
+	0x000684UL
+#define PHY_NW_IP_REG_PHY0_TOP_TBUS_DATA_7_0 \
+	0x0006c0UL
+#define PHY_NW_IP_REG_PHY0_TOP_TBUS_DATA_11_8 \
+	0x0006c4UL
+#define MS_REG_MS_CMU \
+	0x6a4000UL
+#define PHY_SGMII_IP_REG_AHB_CMU_CSR_0_X130 \
+	0x000208UL
+#define PHY_SGMII_IP_REG_AHB_CMU_CSR_0_X132 \
+	0x000210UL
+#define PHY_SGMII_IP_REG_AHB_CMU_CSR_0_X131 \
+	0x00020cUL
+#define PHY_SGMII_IP_REG_AHB_CMU_CSR_0_X133 \
+	0x000214UL
+#define PHY_PCIE_IP_REG_AHB_CMU_CSR_0_X130 \
+	0x000208UL
+#define PHY_PCIE_IP_REG_AHB_CMU_CSR_0_X131 \
+	0x00020cUL
+#define PHY_PCIE_IP_REG_AHB_CMU_CSR_0_X132 \
+	0x000210UL
+#define PHY_PCIE_IP_REG_AHB_CMU_CSR_0_X133 \
+	0x000214UL
+#define PHY_PCIE_REG_PHY0 \
+	0x620000UL
+#define PHY_PCIE_REG_PHY1 \
+	0x624000UL
+#define NIG_REG_ROCE_DUPLICATE_TO_HOST 0x5088f0UL
+#define PRS_REG_LIGHT_L2_ETHERTYPE_EN 0x1f0968UL
+#define NIG_REG_LLH_ENG_CLS_ENG_ID_TBL 0x501b90UL
+#define DORQ_REG_PF_DPM_ENABLE 0x100510UL
+#define DORQ_REG_PF_ICID_BIT_SHIFT_NORM	0x100448UL
+#define DORQ_REG_PF_MIN_ADDR_REG1 0x100400UL
+#define DORQ_REG_PF_DPI_BIT_SHIFT 0x100450UL
 #endif
diff --git a/drivers/net/ethernet/qlogic/qed/qed_roce.c b/drivers/net/ethernet/qlogic/qed/qed_roce.c
new file mode 100644
index 000000000000..76831a398bed
--- /dev/null
+++ b/drivers/net/ethernet/qlogic/qed/qed_roce.c
@@ -0,0 +1,2954 @@
+/* QLogic qed NIC Driver
+ * Copyright (c) 2015-2016  QLogic Corporation
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * COPYING in the main directory of this source tree, or the
+ * OpenIB.org BSD license below:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      - Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *
+ *      - Redistributions in binary form must reproduce the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer in the documentation and /or other materials
+ *        provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+#include <linux/types.h>
+#include <asm/byteorder.h>
+#include <linux/bitops.h>
+#include <linux/delay.h>
+#include <linux/dma-mapping.h>
+#include <linux/errno.h>
+#include <linux/etherdevice.h>
+#include <linux/if_ether.h>
+#include <linux/if_vlan.h>
+#include <linux/io.h>
+#include <linux/ip.h>
+#include <linux/ipv6.h>
+#include <linux/kernel.h>
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/pci.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/string.h>
+#include <linux/tcp.h>
+#include <linux/bitops.h>
+#include <linux/qed/qed_roce_if.h>
+#include <linux/qed/qed_roce_if.h>
+#include "qed.h"
+#include "qed_cxt.h"
+#include "qed_hsi.h"
+#include "qed_hw.h"
+#include "qed_init_ops.h"
+#include "qed_int.h"
+#include "qed_ll2.h"
+#include "qed_mcp.h"
+#include "qed_reg_addr.h"
+#include "qed_sp.h"
+#include "qed_roce.h"
+#include "qed_ll2.h"
+
+void qed_async_roce_event(struct qed_hwfn *p_hwfn,
+			  struct event_ring_entry *p_eqe)
+{
+	struct qed_rdma_info *p_rdma_info = p_hwfn->p_rdma_info;
+
+	p_rdma_info->events.affiliated_event(p_rdma_info->events.context,
+					     p_eqe->opcode, &p_eqe->data);
+}
+
+static int qed_rdma_bmap_alloc(struct qed_hwfn *p_hwfn,
+			       struct qed_bmap *bmap, u32 max_count)
+{
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "max_count = %08x\n", max_count);
+
+	bmap->max_count = max_count;
+
+	bmap->bitmap = kzalloc(BITS_TO_LONGS(max_count) * sizeof(long),
+			       GFP_KERNEL);
+	if (!bmap->bitmap) {
+		DP_NOTICE(p_hwfn,
+			  "qed bmap alloc failed: cannot allocate memory (bitmap)\n");
+		return -ENOMEM;
+	}
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Allocated bitmap %p\n",
+		   bmap->bitmap);
+	return 0;
+}
+
+static int qed_rdma_bmap_alloc_id(struct qed_hwfn *p_hwfn,
+				  struct qed_bmap *bmap, u32 *id_num)
+{
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "bmap = %p\n", bmap);
+
+	*id_num = find_first_zero_bit(bmap->bitmap, bmap->max_count);
+
+	if (*id_num >= bmap->max_count) {
+		DP_NOTICE(p_hwfn, "no id available max_count=%d\n",
+			  bmap->max_count);
+		return -EINVAL;
+	}
+
+	__set_bit(*id_num, bmap->bitmap);
+
+	return 0;
+}
+
+static void qed_bmap_release_id(struct qed_hwfn *p_hwfn,
+				struct qed_bmap *bmap, u32 id_num)
+{
+	bool b_acquired;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "id_num = %08x", id_num);
+	if (id_num >= bmap->max_count)
+		return;
+
+	b_acquired = test_and_clear_bit(id_num, bmap->bitmap);
+	if (!b_acquired) {
+		DP_NOTICE(p_hwfn, "ID %d already released\n", id_num);
+		return;
+	}
+}
+
+u32 qed_rdma_get_sb_id(void *p_hwfn, u32 rel_sb_id)
+{
+	/* First sb id for RoCE is after all the l2 sb */
+	return FEAT_NUM((struct qed_hwfn *)p_hwfn, QED_PF_L2_QUE) + rel_sb_id;
+}
+
+u32 qed_rdma_query_cau_timer_res(void *rdma_cxt)
+{
+	return QED_CAU_DEF_RX_TIMER_RES;
+}
+
+static int qed_rdma_alloc(struct qed_hwfn *p_hwfn,
+			  struct qed_ptt *p_ptt,
+			  struct qed_rdma_start_in_params *params)
+{
+	struct qed_rdma_info *p_rdma_info;
+	u32 num_cons, num_tasks;
+	int rc = -ENOMEM;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Allocating RDMA\n");
+
+	/* Allocate a struct with current pf rdma info */
+	p_rdma_info = kzalloc(sizeof(*p_rdma_info), GFP_KERNEL);
+	if (!p_rdma_info) {
+		DP_NOTICE(p_hwfn,
+			  "qed rdma alloc failed: cannot allocate memory (rdma info). rc = %d\n",
+			  rc);
+		return rc;
+	}
+
+	p_hwfn->p_rdma_info = p_rdma_info;
+	p_rdma_info->proto = PROTOCOLID_ROCE;
+
+	num_cons = qed_cxt_get_proto_cid_count(p_hwfn, p_rdma_info->proto, 0);
+
+	p_rdma_info->num_qps = num_cons / 2;
+
+	num_tasks = qed_cxt_get_proto_tid_count(p_hwfn, PROTOCOLID_ROCE);
+
+	/* Each MR uses a single task */
+	p_rdma_info->num_mrs = num_tasks;
+
+	/* Queue zone lines are shared between RoCE and L2 in such a way that
+	 * they can be used by each without obstructing the other.
+	 */
+	p_rdma_info->queue_zone_base = (u16)FEAT_NUM(p_hwfn, QED_L2_QUEUE);
+
+	/* Allocate a struct with device params and fill it */
+	p_rdma_info->dev = kzalloc(sizeof(*p_rdma_info->dev), GFP_KERNEL);
+	if (!p_rdma_info->dev) {
+		DP_NOTICE(p_hwfn,
+			  "qed rdma alloc failed: cannot allocate memory (rdma info dev). rc = %d\n",
+			  rc);
+		goto free_rdma_info;
+	}
+
+	/* Allocate a struct with port params and fill it */
+	p_rdma_info->port = kzalloc(sizeof(*p_rdma_info->port), GFP_KERNEL);
+	if (!p_rdma_info->port) {
+		DP_NOTICE(p_hwfn,
+			  "qed rdma alloc failed: cannot allocate memory (rdma info port). rc = %d\n",
+			  rc);
+		goto free_rdma_dev;
+	}
+
+	/* Allocate bit map for pd's */
+	rc = qed_rdma_bmap_alloc(p_hwfn, &p_rdma_info->pd_map, RDMA_MAX_PDS);
+	if (rc) {
+		DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
+			   "Failed to allocate pd_map, rc = %d\n",
+			   rc);
+		goto free_rdma_port;
+	}
+
+	/* Allocate DPI bitmap */
+	rc = qed_rdma_bmap_alloc(p_hwfn, &p_rdma_info->dpi_map,
+				 p_hwfn->dpi_count);
+	if (rc) {
+		DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
+			   "Failed to allocate DPI bitmap, rc = %d\n", rc);
+		goto free_pd_map;
+	}
+
+	/* Allocate bitmap for cq's. The maximum number of CQs is bounded to
+	 * twice the number of QPs.
+	 */
+	rc = qed_rdma_bmap_alloc(p_hwfn, &p_rdma_info->cq_map,
+				 p_rdma_info->num_qps * 2);
+	if (rc) {
+		DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
+			   "Failed to allocate cq bitmap, rc = %d\n", rc);
+		goto free_dpi_map;
+	}
+
+	/* Allocate bitmap for toggle bit for cq icids
+	 * We toggle the bit every time we create or resize cq for a given icid.
+	 * The maximum number of CQs is bounded to  twice the number of QPs.
+	 */
+	rc = qed_rdma_bmap_alloc(p_hwfn, &p_rdma_info->toggle_bits,
+				 p_rdma_info->num_qps * 2);
+	if (rc) {
+		DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
+			   "Failed to allocate toogle bits, rc = %d\n", rc);
+		goto free_cq_map;
+	}
+
+	/* Allocate bitmap for itids */
+	rc = qed_rdma_bmap_alloc(p_hwfn, &p_rdma_info->tid_map,
+				 p_rdma_info->num_mrs);
+	if (rc) {
+		DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
+			   "Failed to allocate itids bitmaps, rc = %d\n", rc);
+		goto free_toggle_map;
+	}
+
+	/* Allocate bitmap for cids used for qps. */
+	rc = qed_rdma_bmap_alloc(p_hwfn, &p_rdma_info->cid_map, num_cons);
+	if (rc) {
+		DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
+			   "Failed to allocate cid bitmap, rc = %d\n", rc);
+		goto free_tid_map;
+	}
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Allocation successful\n");
+	return 0;
+
+free_tid_map:
+	kfree(p_rdma_info->tid_map.bitmap);
+free_toggle_map:
+	kfree(p_rdma_info->toggle_bits.bitmap);
+free_cq_map:
+	kfree(p_rdma_info->cq_map.bitmap);
+free_dpi_map:
+	kfree(p_rdma_info->dpi_map.bitmap);
+free_pd_map:
+	kfree(p_rdma_info->pd_map.bitmap);
+free_rdma_port:
+	kfree(p_rdma_info->port);
+free_rdma_dev:
+	kfree(p_rdma_info->dev);
+free_rdma_info:
+	kfree(p_rdma_info);
+
+	return rc;
+}
+
+void qed_rdma_resc_free(struct qed_hwfn *p_hwfn)
+{
+	struct qed_rdma_info *p_rdma_info = p_hwfn->p_rdma_info;
+
+	kfree(p_rdma_info->cid_map.bitmap);
+	kfree(p_rdma_info->tid_map.bitmap);
+	kfree(p_rdma_info->toggle_bits.bitmap);
+	kfree(p_rdma_info->cq_map.bitmap);
+	kfree(p_rdma_info->dpi_map.bitmap);
+	kfree(p_rdma_info->pd_map.bitmap);
+
+	kfree(p_rdma_info->port);
+	kfree(p_rdma_info->dev);
+
+	kfree(p_rdma_info);
+}
+
+static void qed_rdma_free(struct qed_hwfn *p_hwfn)
+{
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Freeing RDMA\n");
+
+	qed_rdma_resc_free(p_hwfn);
+}
+
+static void qed_rdma_get_guid(struct qed_hwfn *p_hwfn, u8 *guid)
+{
+	guid[0] = p_hwfn->hw_info.hw_mac_addr[0] ^ 2;
+	guid[1] = p_hwfn->hw_info.hw_mac_addr[1];
+	guid[2] = p_hwfn->hw_info.hw_mac_addr[2];
+	guid[3] = 0xff;
+	guid[4] = 0xfe;
+	guid[5] = p_hwfn->hw_info.hw_mac_addr[3];
+	guid[6] = p_hwfn->hw_info.hw_mac_addr[4];
+	guid[7] = p_hwfn->hw_info.hw_mac_addr[5];
+}
+
+static void qed_rdma_init_events(struct qed_hwfn *p_hwfn,
+				 struct qed_rdma_start_in_params *params)
+{
+	struct qed_rdma_events *events;
+
+	events = &p_hwfn->p_rdma_info->events;
+
+	events->unaffiliated_event = params->events->unaffiliated_event;
+	events->affiliated_event = params->events->affiliated_event;
+	events->context = params->events->context;
+}
+
+static void qed_rdma_init_devinfo(struct qed_hwfn *p_hwfn,
+				  struct qed_rdma_start_in_params *params)
+{
+	struct qed_rdma_device *dev = p_hwfn->p_rdma_info->dev;
+	struct qed_dev *cdev = p_hwfn->cdev;
+	u32 pci_status_control;
+	u32 num_qps;
+
+	/* Vendor specific information */
+	dev->vendor_id = cdev->vendor_id;
+	dev->vendor_part_id = cdev->device_id;
+	dev->hw_ver = 0;
+	dev->fw_ver = (FW_MAJOR_VERSION << 24) | (FW_MINOR_VERSION << 16) |
+		      (FW_REVISION_VERSION << 8) | (FW_ENGINEERING_VERSION);
+
+	qed_rdma_get_guid(p_hwfn, (u8 *)&dev->sys_image_guid);
+	dev->node_guid = dev->sys_image_guid;
+
+	dev->max_sge = min_t(u32, RDMA_MAX_SGE_PER_SQ_WQE,
+			     RDMA_MAX_SGE_PER_RQ_WQE);
+
+	if (cdev->rdma_max_sge)
+		dev->max_sge = min_t(u32, cdev->rdma_max_sge, dev->max_sge);
+
+	dev->max_inline = ROCE_REQ_MAX_INLINE_DATA_SIZE;
+
+	dev->max_inline = (cdev->rdma_max_inline) ?
+			  min_t(u32, cdev->rdma_max_inline, dev->max_inline) :
+			  dev->max_inline;
+
+	dev->max_wqe = QED_RDMA_MAX_WQE;
+	dev->max_cnq = (u8)FEAT_NUM(p_hwfn, QED_RDMA_CNQ);
+
+	/* The number of QPs may be higher than QED_ROCE_MAX_QPS, because
+	 * it is up-aligned to 16 and then to ILT page size within qed cxt.
+	 * This is OK in terms of ILT but we don't want to configure the FW
+	 * above its abilities
+	 */
+	num_qps = ROCE_MAX_QPS;
+	num_qps = min_t(u64, num_qps, p_hwfn->p_rdma_info->num_qps);
+	dev->max_qp = num_qps;
+
+	/* CQs uses the same icids that QPs use hence they are limited by the
+	 * number of icids. There are two icids per QP.
+	 */
+	dev->max_cq = num_qps * 2;
+
+	/* The number of mrs is smaller by 1 since the first is reserved */
+	dev->max_mr = p_hwfn->p_rdma_info->num_mrs - 1;
+	dev->max_mr_size = QED_RDMA_MAX_MR_SIZE;
+
+	/* The maximum CQE capacity per CQ supported.
+	 * max number of cqes will be in two layer pbl,
+	 * 8 is the pointer size in bytes
+	 * 32 is the size of cq element in bytes
+	 */
+	if (params->cq_mode == QED_RDMA_CQ_MODE_32_BITS)
+		dev->max_cqe = QED_RDMA_MAX_CQE_32_BIT;
+	else
+		dev->max_cqe = QED_RDMA_MAX_CQE_16_BIT;
+
+	dev->max_mw = 0;
+	dev->max_fmr = QED_RDMA_MAX_FMR;
+	dev->max_mr_mw_fmr_pbl = (PAGE_SIZE / 8) * (PAGE_SIZE / 8);
+	dev->max_mr_mw_fmr_size = dev->max_mr_mw_fmr_pbl * PAGE_SIZE;
+	dev->max_pkey = QED_RDMA_MAX_P_KEY;
+
+	dev->max_qp_resp_rd_atomic_resc = RDMA_RING_PAGE_SIZE /
+					  (RDMA_RESP_RD_ATOMIC_ELM_SIZE * 2);
+	dev->max_qp_req_rd_atomic_resc = RDMA_RING_PAGE_SIZE /
+					 RDMA_REQ_RD_ATOMIC_ELM_SIZE;
+	dev->max_dev_resp_rd_atomic_resc = dev->max_qp_resp_rd_atomic_resc *
+					   p_hwfn->p_rdma_info->num_qps;
+	dev->page_size_caps = QED_RDMA_PAGE_SIZE_CAPS;
+	dev->dev_ack_delay = QED_RDMA_ACK_DELAY;
+	dev->max_pd = RDMA_MAX_PDS;
+	dev->max_ah = p_hwfn->p_rdma_info->num_qps;
+	dev->max_stats_queues = (u8)RESC_NUM(p_hwfn, QED_RDMA_STATS_QUEUE);
+
+	/* Set capablities */
+	dev->dev_caps = 0;
+	SET_FIELD(dev->dev_caps, QED_RDMA_DEV_CAP_RNR_NAK, 1);
+	SET_FIELD(dev->dev_caps, QED_RDMA_DEV_CAP_PORT_ACTIVE_EVENT, 1);
+	SET_FIELD(dev->dev_caps, QED_RDMA_DEV_CAP_PORT_CHANGE_EVENT, 1);
+	SET_FIELD(dev->dev_caps, QED_RDMA_DEV_CAP_RESIZE_CQ, 1);
+	SET_FIELD(dev->dev_caps, QED_RDMA_DEV_CAP_BASE_MEMORY_EXT, 1);
+	SET_FIELD(dev->dev_caps, QED_RDMA_DEV_CAP_BASE_QUEUE_EXT, 1);
+	SET_FIELD(dev->dev_caps, QED_RDMA_DEV_CAP_ZBVA, 1);
+	SET_FIELD(dev->dev_caps, QED_RDMA_DEV_CAP_LOCAL_INV_FENCE, 1);
+
+	/* Check atomic operations support in PCI configuration space. */
+	pci_read_config_dword(cdev->pdev,
+			      cdev->pdev->pcie_cap + PCI_EXP_DEVCTL2,
+			      &pci_status_control);
+
+	if (pci_status_control & PCI_EXP_DEVCTL2_LTR_EN)
+		SET_FIELD(dev->dev_caps, QED_RDMA_DEV_CAP_ATOMIC_OP, 1);
+}
+
+static void qed_rdma_init_port(struct qed_hwfn *p_hwfn)
+{
+	struct qed_rdma_port *port = p_hwfn->p_rdma_info->port;
+	struct qed_rdma_device *dev = p_hwfn->p_rdma_info->dev;
+
+	port->port_state = p_hwfn->mcp_info->link_output.link_up ?
+			   QED_RDMA_PORT_UP : QED_RDMA_PORT_DOWN;
+
+	port->max_msg_size = min_t(u64,
+				   (dev->max_mr_mw_fmr_size *
+				    p_hwfn->cdev->rdma_max_sge),
+				   BIT(31));
+
+	port->pkey_bad_counter = 0;
+}
+
+static int qed_rdma_init_hw(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
+{
+	u32 ll2_ethertype_en;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Initializing HW\n");
+	p_hwfn->b_rdma_enabled_in_prs = false;
+
+	qed_wr(p_hwfn, p_ptt, PRS_REG_ROCE_DEST_QP_MAX_PF, 0);
+
+	p_hwfn->rdma_prs_search_reg = PRS_REG_SEARCH_ROCE;
+
+	/* We delay writing to this reg until first cid is allocated. See
+	 * qed_cxt_dynamic_ilt_alloc function for more details
+	 */
+	ll2_ethertype_en = qed_rd(p_hwfn, p_ptt, PRS_REG_LIGHT_L2_ETHERTYPE_EN);
+	qed_wr(p_hwfn, p_ptt, PRS_REG_LIGHT_L2_ETHERTYPE_EN,
+	       (ll2_ethertype_en | 0x01));
+
+	if (qed_cxt_get_proto_cid_start(p_hwfn, PROTOCOLID_ROCE) % 2) {
+		DP_NOTICE(p_hwfn, "The first RoCE's cid should be even\n");
+		return -EINVAL;
+	}
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Initializing HW - Done\n");
+	return 0;
+}
+
+static int qed_rdma_start_fw(struct qed_hwfn *p_hwfn,
+			     struct qed_rdma_start_in_params *params,
+			     struct qed_ptt *p_ptt)
+{
+	struct rdma_init_func_ramrod_data *p_ramrod;
+	struct qed_rdma_cnq_params *p_cnq_pbl_list;
+	struct rdma_init_func_hdr *p_params_header;
+	struct rdma_cnq_params *p_cnq_params;
+	struct qed_sp_init_data init_data;
+	struct qed_spq_entry *p_ent;
+	u32 cnq_id, sb_id;
+	int rc;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Starting FW\n");
+
+	/* Save the number of cnqs for the function close ramrod */
+	p_hwfn->p_rdma_info->num_cnqs = params->desired_cnq;
+
+	/* Get SPQ entry */
+	memset(&init_data, 0, sizeof(init_data));
+	init_data.opaque_fid = p_hwfn->hw_info.opaque_fid;
+	init_data.comp_mode = QED_SPQ_MODE_EBLOCK;
+
+	rc = qed_sp_init_request(p_hwfn, &p_ent, RDMA_RAMROD_FUNC_INIT,
+				 p_hwfn->p_rdma_info->proto, &init_data);
+	if (rc)
+		return rc;
+
+	p_ramrod = &p_ent->ramrod.roce_init_func.rdma;
+
+	p_params_header = &p_ramrod->params_header;
+	p_params_header->cnq_start_offset = (u8)RESC_START(p_hwfn,
+							   QED_RDMA_CNQ_RAM);
+	p_params_header->num_cnqs = params->desired_cnq;
+
+	if (params->cq_mode == QED_RDMA_CQ_MODE_16_BITS)
+		p_params_header->cq_ring_mode = 1;
+	else
+		p_params_header->cq_ring_mode = 0;
+
+	for (cnq_id = 0; cnq_id < params->desired_cnq; cnq_id++) {
+		sb_id = qed_rdma_get_sb_id(p_hwfn, cnq_id);
+		p_cnq_params = &p_ramrod->cnq_params[cnq_id];
+		p_cnq_pbl_list = &params->cnq_pbl_list[cnq_id];
+		p_cnq_params->sb_num =
+			cpu_to_le16(p_hwfn->sbs_info[sb_id]->igu_sb_id);
+
+		p_cnq_params->sb_index = p_hwfn->pf_params.rdma_pf_params.gl_pi;
+		p_cnq_params->num_pbl_pages = p_cnq_pbl_list->num_pbl_pages;
+
+		DMA_REGPAIR_LE(p_cnq_params->pbl_base_addr,
+			       p_cnq_pbl_list->pbl_ptr);
+
+		/* we assume here that cnq_id and qz_offset are the same */
+		p_cnq_params->queue_zone_num =
+			cpu_to_le16(p_hwfn->p_rdma_info->queue_zone_base +
+				    cnq_id);
+	}
+
+	return qed_spq_post(p_hwfn, p_ent, NULL);
+}
+
+static int qed_rdma_reserve_lkey(struct qed_hwfn *p_hwfn)
+{
+	struct qed_rdma_device *dev = p_hwfn->p_rdma_info->dev;
+
+	/* The first DPI is reserved for the Kernel */
+	__set_bit(0, p_hwfn->p_rdma_info->dpi_map.bitmap);
+
+	/* Tid 0 will be used as the key for "reserved MR".
+	 * The driver should allocate memory for it so it can be loaded but no
+	 * ramrod should be passed on it.
+	 */
+	qed_rdma_alloc_tid(p_hwfn, &dev->reserved_lkey);
+	if (dev->reserved_lkey != RDMA_RESERVED_LKEY) {
+		DP_NOTICE(p_hwfn,
+			  "Reserved lkey should be equal to RDMA_RESERVED_LKEY\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int qed_rdma_setup(struct qed_hwfn *p_hwfn,
+			  struct qed_ptt *p_ptt,
+			  struct qed_rdma_start_in_params *params)
+{
+	int rc;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "RDMA setup\n");
+
+	spin_lock_init(&p_hwfn->p_rdma_info->lock);
+
+	qed_rdma_init_devinfo(p_hwfn, params);
+	qed_rdma_init_port(p_hwfn);
+	qed_rdma_init_events(p_hwfn, params);
+
+	rc = qed_rdma_reserve_lkey(p_hwfn);
+	if (rc)
+		return rc;
+
+	rc = qed_rdma_init_hw(p_hwfn, p_ptt);
+	if (rc)
+		return rc;
+
+	return qed_rdma_start_fw(p_hwfn, params, p_ptt);
+}
+
+int qed_rdma_stop(void *rdma_cxt)
+{
+	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
+	struct rdma_close_func_ramrod_data *p_ramrod;
+	struct qed_sp_init_data init_data;
+	struct qed_spq_entry *p_ent;
+	struct qed_ptt *p_ptt;
+	u32 ll2_ethertype_en;
+	int rc = -EBUSY;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "RDMA stop\n");
+
+	p_ptt = qed_ptt_acquire(p_hwfn);
+	if (!p_ptt) {
+		DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Failed to acquire PTT\n");
+		return rc;
+	}
+
+	/* Disable RoCE search */
+	qed_wr(p_hwfn, p_ptt, p_hwfn->rdma_prs_search_reg, 0);
+	p_hwfn->b_rdma_enabled_in_prs = false;
+
+	qed_wr(p_hwfn, p_ptt, PRS_REG_ROCE_DEST_QP_MAX_PF, 0);
+
+	ll2_ethertype_en = qed_rd(p_hwfn, p_ptt, PRS_REG_LIGHT_L2_ETHERTYPE_EN);
+
+	qed_wr(p_hwfn, p_ptt, PRS_REG_LIGHT_L2_ETHERTYPE_EN,
+	       (ll2_ethertype_en & 0xFFFE));
+
+	qed_ptt_release(p_hwfn, p_ptt);
+
+	/* Get SPQ entry */
+	memset(&init_data, 0, sizeof(init_data));
+	init_data.opaque_fid = p_hwfn->hw_info.opaque_fid;
+	init_data.comp_mode = QED_SPQ_MODE_EBLOCK;
+
+	/* Stop RoCE */
+	rc = qed_sp_init_request(p_hwfn, &p_ent, RDMA_RAMROD_FUNC_CLOSE,
+				 p_hwfn->p_rdma_info->proto, &init_data);
+	if (rc)
+		goto out;
+
+	p_ramrod = &p_ent->ramrod.rdma_close_func;
+
+	p_ramrod->num_cnqs = p_hwfn->p_rdma_info->num_cnqs;
+	p_ramrod->cnq_start_offset = (u8)RESC_START(p_hwfn, QED_RDMA_CNQ_RAM);
+
+	rc = qed_spq_post(p_hwfn, p_ent, NULL);
+
+out:
+	qed_rdma_free(p_hwfn);
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "RDMA stop done, rc = %d\n", rc);
+	return rc;
+}
+
+int qed_rdma_add_user(void *rdma_cxt,
+		      struct qed_rdma_add_user_out_params *out_params)
+{
+	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
+	u32 dpi_start_offset;
+	u32 returned_id = 0;
+	int rc;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Adding User\n");
+
+	/* Allocate DPI */
+	spin_lock_bh(&p_hwfn->p_rdma_info->lock);
+	rc = qed_rdma_bmap_alloc_id(p_hwfn, &p_hwfn->p_rdma_info->dpi_map,
+				    &returned_id);
+	spin_unlock_bh(&p_hwfn->p_rdma_info->lock);
+
+	out_params->dpi = (u16)returned_id;
+
+	/* Calculate the corresponding DPI address */
+	dpi_start_offset = p_hwfn->dpi_start_offset;
+
+	out_params->dpi_addr = (u64)((u8 __iomem *)p_hwfn->doorbells +
+				     dpi_start_offset +
+				     ((out_params->dpi) * p_hwfn->dpi_size));
+
+	out_params->dpi_phys_addr = p_hwfn->cdev->db_phys_addr +
+				    dpi_start_offset +
+				    ((out_params->dpi) * p_hwfn->dpi_size);
+
+	out_params->dpi_size = p_hwfn->dpi_size;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Adding user - done, rc = %d\n", rc);
+	return rc;
+}
+
+struct qed_rdma_port *qed_rdma_query_port(void *rdma_cxt)
+{
+	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
+	struct qed_rdma_port *p_port = p_hwfn->p_rdma_info->port;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "RDMA Query port\n");
+
+	/* Link may have changed */
+	p_port->port_state = p_hwfn->mcp_info->link_output.link_up ?
+			     QED_RDMA_PORT_UP : QED_RDMA_PORT_DOWN;
+
+	p_port->link_speed = p_hwfn->mcp_info->link_output.speed;
+
+	return p_port;
+}
+
+struct qed_rdma_device *qed_rdma_query_device(void *rdma_cxt)
+{
+	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Query device\n");
+
+	/* Return struct with device parameters */
+	return p_hwfn->p_rdma_info->dev;
+}
+
+void qed_rdma_free_tid(void *rdma_cxt, u32 itid)
+{
+	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "itid = %08x\n", itid);
+
+	spin_lock_bh(&p_hwfn->p_rdma_info->lock);
+	qed_bmap_release_id(p_hwfn, &p_hwfn->p_rdma_info->tid_map, itid);
+	spin_unlock_bh(&p_hwfn->p_rdma_info->lock);
+}
+
+int qed_rdma_alloc_tid(void *rdma_cxt, u32 *itid)
+{
+	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
+	int rc;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Allocate TID\n");
+
+	spin_lock_bh(&p_hwfn->p_rdma_info->lock);
+	rc = qed_rdma_bmap_alloc_id(p_hwfn,
+				    &p_hwfn->p_rdma_info->tid_map, itid);
+	spin_unlock_bh(&p_hwfn->p_rdma_info->lock);
+	if (rc)
+		goto out;
+
+	rc = qed_cxt_dynamic_ilt_alloc(p_hwfn, QED_ELEM_TASK, *itid);
+out:
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Allocate TID - done, rc = %d\n", rc);
+	return rc;
+}
+
+void qed_rdma_cnq_prod_update(void *rdma_cxt, u8 qz_offset, u16 prod)
+{
+	struct qed_hwfn *p_hwfn;
+	u16 qz_num;
+	u32 addr;
+
+	p_hwfn = (struct qed_hwfn *)rdma_cxt;
+	qz_num = p_hwfn->p_rdma_info->queue_zone_base + qz_offset;
+	addr = GTT_BAR0_MAP_REG_USDM_RAM +
+	       USTORM_COMMON_QUEUE_CONS_OFFSET(qz_num);
+
+	REG_WR16(p_hwfn, addr, prod);
+
+	/* keep prod updates ordered */
+	wmb();
+}
+
+static int qed_fill_rdma_dev_info(struct qed_dev *cdev,
+				  struct qed_dev_rdma_info *info)
+{
+	memset(info, 0, sizeof(*info));
+
+	info->rdma_type = QED_RDMA_TYPE_ROCE;
+
+	qed_fill_dev_info(cdev, &info->common);
+
+	return 0;
+}
+
+static int qed_rdma_get_sb_start(struct qed_dev *cdev)
+{
+	int feat_num;
+
+	if (cdev->num_hwfns > 1)
+		feat_num = FEAT_NUM(QED_LEADING_HWFN(cdev), QED_PF_L2_QUE);
+	else
+		feat_num = FEAT_NUM(QED_LEADING_HWFN(cdev), QED_PF_L2_QUE) *
+			   cdev->num_hwfns;
+
+	return feat_num;
+}
+
+static int qed_rdma_get_min_cnq_msix(struct qed_dev *cdev)
+{
+	int n_cnq = FEAT_NUM(QED_LEADING_HWFN(cdev), QED_RDMA_CNQ);
+	int n_msix = cdev->int_params.rdma_msix_cnt;
+
+	return min_t(int, n_cnq, n_msix);
+}
+
+static int qed_rdma_set_int(struct qed_dev *cdev, u16 cnt)
+{
+	int limit = 0;
+
+	/* Mark the fastpath as free/used */
+	cdev->int_params.fp_initialized = cnt ? true : false;
+
+	if (cdev->int_params.out.int_mode != QED_INT_MODE_MSIX) {
+		DP_ERR(cdev,
+		       "qed roce supports only MSI-X interrupts (detected %d).\n",
+		       cdev->int_params.out.int_mode);
+		return -EINVAL;
+	} else if (cdev->int_params.fp_msix_cnt) {
+		limit = cdev->int_params.rdma_msix_cnt;
+	}
+
+	if (!limit)
+		return -ENOMEM;
+
+	return min_t(int, cnt, limit);
+}
+
+static int qed_rdma_get_int(struct qed_dev *cdev, struct qed_int_info *info)
+{
+	memset(info, 0, sizeof(*info));
+
+	if (!cdev->int_params.fp_initialized) {
+		DP_INFO(cdev,
+			"Protocol driver requested interrupt information, but its support is not yet configured\n");
+		return -EINVAL;
+	}
+
+	if (cdev->int_params.out.int_mode == QED_INT_MODE_MSIX) {
+		int msix_base = cdev->int_params.rdma_msix_base;
+
+		info->msix_cnt = cdev->int_params.rdma_msix_cnt;
+		info->msix = &cdev->int_params.msix_table[msix_base];
+
+		DP_VERBOSE(cdev, QED_MSG_RDMA, "msix_cnt = %d msix_base=%d\n",
+			   info->msix_cnt, msix_base);
+	}
+
+	return 0;
+}
+
+int qed_rdma_alloc_pd(void *rdma_cxt, u16 *pd)
+{
+	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
+	u32 returned_id;
+	int rc;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Alloc PD\n");
+
+	/* Allocates an unused protection domain */
+	spin_lock_bh(&p_hwfn->p_rdma_info->lock);
+	rc = qed_rdma_bmap_alloc_id(p_hwfn,
+				    &p_hwfn->p_rdma_info->pd_map, &returned_id);
+	spin_unlock_bh(&p_hwfn->p_rdma_info->lock);
+
+	*pd = (u16)returned_id;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Alloc PD - done, rc = %d\n", rc);
+	return rc;
+}
+
+void qed_rdma_free_pd(void *rdma_cxt, u16 pd)
+{
+	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "pd = %08x\n", pd);
+
+	/* Returns a previously allocated protection domain for reuse */
+	spin_lock_bh(&p_hwfn->p_rdma_info->lock);
+	qed_bmap_release_id(p_hwfn, &p_hwfn->p_rdma_info->pd_map, pd);
+	spin_unlock_bh(&p_hwfn->p_rdma_info->lock);
+}
+
+static enum qed_rdma_toggle_bit
+qed_rdma_toggle_bit_create_resize_cq(struct qed_hwfn *p_hwfn, u16 icid)
+{
+	struct qed_rdma_info *p_info = p_hwfn->p_rdma_info;
+	enum qed_rdma_toggle_bit toggle_bit;
+	u32 bmap_id;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "icid = %08x\n", icid);
+
+	/* the function toggle the bit that is related to a given icid
+	 * and returns the new toggle bit's value
+	 */
+	bmap_id = icid - qed_cxt_get_proto_cid_start(p_hwfn, p_info->proto);
+
+	spin_lock_bh(&p_info->lock);
+	toggle_bit = !test_and_change_bit(bmap_id,
+					  p_info->toggle_bits.bitmap);
+	spin_unlock_bh(&p_info->lock);
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "QED_RDMA_TOGGLE_BIT_= %d\n",
+		   toggle_bit);
+
+	return toggle_bit;
+}
+
+int qed_rdma_create_cq(void *rdma_cxt,
+		       struct qed_rdma_create_cq_in_params *params, u16 *icid)
+{
+	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
+	struct qed_rdma_info *p_info = p_hwfn->p_rdma_info;
+	struct rdma_create_cq_ramrod_data *p_ramrod;
+	enum qed_rdma_toggle_bit toggle_bit;
+	struct qed_sp_init_data init_data;
+	struct qed_spq_entry *p_ent;
+	u32 returned_id, start_cid;
+	int rc;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "cq_handle = %08x%08x\n",
+		   params->cq_handle_hi, params->cq_handle_lo);
+
+	/* Allocate icid */
+	spin_lock_bh(&p_info->lock);
+	rc = qed_rdma_bmap_alloc_id(p_hwfn,
+				    &p_info->cq_map, &returned_id);
+	spin_unlock_bh(&p_info->lock);
+
+	if (rc) {
+		DP_NOTICE(p_hwfn, "Can't create CQ, rc = %d\n", rc);
+		return rc;
+	}
+
+	start_cid = qed_cxt_get_proto_cid_start(p_hwfn,
+						p_info->proto);
+	*icid = returned_id + start_cid;
+
+	/* Check if icid requires a page allocation */
+	rc = qed_cxt_dynamic_ilt_alloc(p_hwfn, QED_ELEM_CXT, *icid);
+	if (rc)
+		goto err;
+
+	/* Get SPQ entry */
+	memset(&init_data, 0, sizeof(init_data));
+	init_data.cid = *icid;
+	init_data.opaque_fid = p_hwfn->hw_info.opaque_fid;
+	init_data.comp_mode = QED_SPQ_MODE_EBLOCK;
+
+	/* Send create CQ ramrod */
+	rc = qed_sp_init_request(p_hwfn, &p_ent,
+				 RDMA_RAMROD_CREATE_CQ,
+				 p_info->proto, &init_data);
+	if (rc)
+		goto err;
+
+	p_ramrod = &p_ent->ramrod.rdma_create_cq;
+
+	p_ramrod->cq_handle.hi = cpu_to_le32(params->cq_handle_hi);
+	p_ramrod->cq_handle.lo = cpu_to_le32(params->cq_handle_lo);
+	p_ramrod->dpi = cpu_to_le16(params->dpi);
+	p_ramrod->is_two_level_pbl = params->pbl_two_level;
+	p_ramrod->max_cqes = cpu_to_le32(params->cq_size);
+	DMA_REGPAIR_LE(p_ramrod->pbl_addr, params->pbl_ptr);
+	p_ramrod->pbl_num_pages = cpu_to_le16(params->pbl_num_pages);
+	p_ramrod->cnq_id = (u8)RESC_START(p_hwfn, QED_RDMA_CNQ_RAM) +
+			   params->cnq_id;
+	p_ramrod->int_timeout = params->int_timeout;
+
+	/* toggle the bit for every resize or create cq for a given icid */
+	toggle_bit = qed_rdma_toggle_bit_create_resize_cq(p_hwfn, *icid);
+
+	p_ramrod->toggle_bit = toggle_bit;
+
+	rc = qed_spq_post(p_hwfn, p_ent, NULL);
+	if (rc) {
+		/* restore toggle bit */
+		qed_rdma_toggle_bit_create_resize_cq(p_hwfn, *icid);
+		goto err;
+	}
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Created CQ, rc = %d\n", rc);
+	return rc;
+
+err:
+	/* release allocated icid */
+	qed_bmap_release_id(p_hwfn, &p_info->cq_map, returned_id);
+	DP_NOTICE(p_hwfn, "Create CQ failed, rc = %d\n", rc);
+
+	return rc;
+}
+
+int qed_rdma_resize_cq(void *rdma_cxt,
+		       struct qed_rdma_resize_cq_in_params *in_params,
+		       struct qed_rdma_resize_cq_out_params *out_params)
+{
+	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
+	struct rdma_resize_cq_output_params *p_ramrod_res;
+	struct rdma_resize_cq_ramrod_data *p_ramrod;
+	enum qed_rdma_toggle_bit toggle_bit;
+	struct qed_sp_init_data init_data;
+	struct qed_spq_entry *p_ent;
+	dma_addr_t ramrod_res_phys;
+	u8 fw_return_code;
+	int rc = -ENOMEM;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "icid = %08x\n", in_params->icid);
+
+	p_ramrod_res =
+	    (struct rdma_resize_cq_output_params *)
+	    dma_alloc_coherent(&p_hwfn->cdev->pdev->dev,
+			       sizeof(struct rdma_resize_cq_output_params),
+			       &ramrod_res_phys, GFP_KERNEL);
+	if (!p_ramrod_res) {
+		DP_NOTICE(p_hwfn,
+			  "qed resize cq failed: cannot allocate memory (ramrod)\n");
+		return rc;
+	}
+
+	/* Get SPQ entry */
+	memset(&init_data, 0, sizeof(init_data));
+	init_data.cid = in_params->icid;
+	init_data.opaque_fid = p_hwfn->hw_info.opaque_fid;
+	init_data.comp_mode = QED_SPQ_MODE_EBLOCK;
+
+	rc = qed_sp_init_request(p_hwfn, &p_ent,
+				 RDMA_RAMROD_RESIZE_CQ,
+				 p_hwfn->p_rdma_info->proto, &init_data);
+	if (rc)
+		goto err;
+
+	p_ramrod = &p_ent->ramrod.rdma_resize_cq;
+
+	p_ramrod->flags = 0;
+
+	/* toggle the bit for every resize or create cq for a given icid */
+	toggle_bit = qed_rdma_toggle_bit_create_resize_cq(p_hwfn,
+							  in_params->icid);
+
+	SET_FIELD(p_ramrod->flags,
+		  RDMA_RESIZE_CQ_RAMROD_DATA_TOGGLE_BIT, toggle_bit);
+
+	SET_FIELD(p_ramrod->flags,
+		  RDMA_RESIZE_CQ_RAMROD_DATA_IS_TWO_LEVEL_PBL,
+		  in_params->pbl_two_level);
+
+	p_ramrod->pbl_log_page_size = in_params->pbl_page_size_log - 12;
+	p_ramrod->pbl_num_pages = cpu_to_le16(in_params->pbl_num_pages);
+	p_ramrod->max_cqes = cpu_to_le32(in_params->cq_size);
+	DMA_REGPAIR_LE(p_ramrod->pbl_addr, in_params->pbl_ptr);
+	DMA_REGPAIR_LE(p_ramrod->output_params_addr, ramrod_res_phys);
+
+	rc = qed_spq_post(p_hwfn, p_ent, &fw_return_code);
+	if (rc)
+		goto err;
+
+	if (fw_return_code != RDMA_RETURN_OK) {
+		DP_NOTICE(p_hwfn, "fw_return_code = %d\n", fw_return_code);
+		rc = -EINVAL;
+		goto err;
+	}
+
+	out_params->prod = le32_to_cpu(p_ramrod_res->old_cq_prod);
+	out_params->cons = le32_to_cpu(p_ramrod_res->old_cq_cons);
+
+	dma_free_coherent(&p_hwfn->cdev->pdev->dev,
+			  sizeof(struct rdma_resize_cq_output_params),
+			  p_ramrod_res, ramrod_res_phys);
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Resized CQ, rc = %d\n", rc);
+
+	return rc;
+
+err:	dma_free_coherent(&p_hwfn->cdev->pdev->dev,
+			  sizeof(struct rdma_resize_cq_output_params),
+			  p_ramrod_res, ramrod_res_phys);
+	DP_NOTICE(p_hwfn, "Resized CQ, Failed - rc = %d\n", rc);
+
+	return rc;
+}
+
+int qed_rdma_destroy_cq(void *rdma_cxt,
+			struct qed_rdma_destroy_cq_in_params *in_params,
+			struct qed_rdma_destroy_cq_out_params *out_params)
+{
+	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
+	struct rdma_destroy_cq_output_params *p_ramrod_res;
+	struct rdma_destroy_cq_ramrod_data *p_ramrod;
+	struct qed_sp_init_data init_data;
+	struct qed_spq_entry *p_ent;
+	dma_addr_t ramrod_res_phys;
+	int rc = -ENOMEM;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "icid = %08x\n", in_params->icid);
+
+	p_ramrod_res =
+	    (struct rdma_destroy_cq_output_params *)
+	    dma_alloc_coherent(&p_hwfn->cdev->pdev->dev,
+			       sizeof(struct rdma_destroy_cq_output_params),
+			       &ramrod_res_phys, GFP_KERNEL);
+	if (!p_ramrod_res) {
+		DP_NOTICE(p_hwfn,
+			  "qed destroy cq failed: cannot allocate memory (ramrod)\n");
+		return rc;
+	}
+
+	/* Get SPQ entry */
+	memset(&init_data, 0, sizeof(init_data));
+	init_data.cid = in_params->icid;
+	init_data.opaque_fid = p_hwfn->hw_info.opaque_fid;
+	init_data.comp_mode = QED_SPQ_MODE_EBLOCK;
+
+	/* Send destroy CQ ramrod */
+	rc = qed_sp_init_request(p_hwfn, &p_ent,
+				 RDMA_RAMROD_DESTROY_CQ,
+				 p_hwfn->p_rdma_info->proto, &init_data);
+	if (rc)
+		goto err;
+
+	p_ramrod = &p_ent->ramrod.rdma_destroy_cq;
+	DMA_REGPAIR_LE(p_ramrod->output_params_addr, ramrod_res_phys);
+
+	rc = qed_spq_post(p_hwfn, p_ent, NULL);
+	if (rc)
+		goto err;
+
+	out_params->num_cq_notif = le16_to_cpu(p_ramrod_res->cnq_num);
+
+	dma_free_coherent(&p_hwfn->cdev->pdev->dev,
+			  sizeof(struct rdma_destroy_cq_output_params),
+			  p_ramrod_res, ramrod_res_phys);
+
+	/* Free icid */
+	spin_lock_bh(&p_hwfn->p_rdma_info->lock);
+
+	qed_bmap_release_id(p_hwfn,
+			    &p_hwfn->p_rdma_info->cq_map,
+			    (in_params->icid -
+			     qed_cxt_get_proto_cid_start(p_hwfn,
+							 p_hwfn->
+							 p_rdma_info->proto)));
+
+	spin_unlock_bh(&p_hwfn->p_rdma_info->lock);
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Destroyed CQ, rc = %d\n", rc);
+	return rc;
+
+err:	dma_free_coherent(&p_hwfn->cdev->pdev->dev,
+			  sizeof(struct rdma_destroy_cq_output_params),
+			  p_ramrod_res, ramrod_res_phys);
+
+	return rc;
+}
+
+static void qed_rdma_set_fw_mac(u16 *p_fw_mac, u8 *p_qed_mac)
+{
+	p_fw_mac[0] = cpu_to_le16((p_qed_mac[0] << 8) + p_qed_mac[1]);
+	p_fw_mac[1] = cpu_to_le16((p_qed_mac[2] << 8) + p_qed_mac[3]);
+	p_fw_mac[2] = cpu_to_le16((p_qed_mac[4] << 8) + p_qed_mac[5]);
+}
+
+static void qed_rdma_copy_gids(struct qed_rdma_qp *qp, __le32 *src_gid,
+			       __le32 *dst_gid)
+{
+	u32 i;
+
+	if (qp->roce_mode == ROCE_V2_IPV4) {
+		/* The IPv4 addresses shall be aligned to the highest word.
+		 * The lower words must be zero.
+		 */
+		memset(src_gid, 0, sizeof(union qed_gid));
+		memset(dst_gid, 0, sizeof(union qed_gid));
+		src_gid[3] = cpu_to_le32(qp->sgid.ipv4_addr);
+		dst_gid[3] = cpu_to_le32(qp->dgid.ipv4_addr);
+	} else {
+		/* GIDs and IPv6 addresses coincide in location and size */
+		for (i = 0; i < ARRAY_SIZE(qp->sgid.dwords); i++) {
+			src_gid[i] = cpu_to_le32(qp->sgid.dwords[i]);
+			dst_gid[i] = cpu_to_le32(qp->dgid.dwords[i]);
+		}
+	}
+}
+
+static enum roce_flavor qed_roce_mode_to_flavor(enum roce_mode roce_mode)
+{
+	enum roce_flavor flavor;
+
+	switch (roce_mode) {
+	case ROCE_V1:
+		flavor = PLAIN_ROCE;
+		break;
+	case ROCE_V2_IPV4:
+		flavor = RROCE_IPV4;
+		break;
+	case ROCE_V2_IPV6:
+		flavor = ROCE_V2_IPV6;
+		break;
+	default:
+		flavor = MAX_ROCE_MODE;
+		break;
+	}
+	return flavor;
+}
+
+int qed_roce_alloc_cid(struct qed_hwfn *p_hwfn, u16 *cid)
+{
+	struct qed_rdma_info *p_rdma_info = p_hwfn->p_rdma_info;
+	u32 responder_icid;
+	u32 requester_icid;
+	int rc;
+
+	spin_lock_bh(&p_hwfn->p_rdma_info->lock);
+	rc = qed_rdma_bmap_alloc_id(p_hwfn, &p_rdma_info->cid_map,
+				    &responder_icid);
+	if (rc) {
+		spin_unlock_bh(&p_rdma_info->lock);
+		return rc;
+	}
+
+	rc = qed_rdma_bmap_alloc_id(p_hwfn, &p_rdma_info->cid_map,
+				    &requester_icid);
+
+	spin_unlock_bh(&p_rdma_info->lock);
+	if (rc)
+		goto err;
+
+	/* the two icid's should be adjacent */
+	if ((requester_icid - responder_icid) != 1) {
+		DP_NOTICE(p_hwfn, "Failed to allocate two adjacent qp's'\n");
+		rc = -EINVAL;
+		goto err;
+	}
+
+	responder_icid += qed_cxt_get_proto_cid_start(p_hwfn,
+						      p_rdma_info->proto);
+	requester_icid += qed_cxt_get_proto_cid_start(p_hwfn,
+						      p_rdma_info->proto);
+
+	/* If these icids require a new ILT line allocate DMA-able context for
+	 * an ILT page
+	 */
+	rc = qed_cxt_dynamic_ilt_alloc(p_hwfn, QED_ELEM_CXT, responder_icid);
+	if (rc)
+		goto err;
+
+	rc = qed_cxt_dynamic_ilt_alloc(p_hwfn, QED_ELEM_CXT, requester_icid);
+	if (rc)
+		goto err;
+
+	*cid = (u16)responder_icid;
+	return rc;
+
+err:
+	spin_lock_bh(&p_rdma_info->lock);
+	qed_bmap_release_id(p_hwfn, &p_rdma_info->cid_map, responder_icid);
+	qed_bmap_release_id(p_hwfn, &p_rdma_info->cid_map, requester_icid);
+
+	spin_unlock_bh(&p_rdma_info->lock);
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
+		   "Allocate CID - failed, rc = %d\n", rc);
+	return rc;
+}
+
+static int qed_roce_sp_create_responder(struct qed_hwfn *p_hwfn,
+					struct qed_rdma_qp *qp)
+{
+	struct roce_create_qp_resp_ramrod_data *p_ramrod;
+	struct qed_sp_init_data init_data;
+	union qed_qm_pq_params qm_params;
+	enum roce_flavor roce_flavor;
+	struct qed_spq_entry *p_ent;
+	u16 physical_queue0 = 0;
+	int rc;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "icid = %08x\n", qp->icid);
+
+	/* Allocate DMA-able memory for IRQ */
+	qp->irq_num_pages = 1;
+	qp->irq = dma_alloc_coherent(&p_hwfn->cdev->pdev->dev,
+				     RDMA_RING_PAGE_SIZE,
+				     &qp->irq_phys_addr, GFP_KERNEL);
+	if (!qp->irq) {
+		rc = -ENOMEM;
+		DP_NOTICE(p_hwfn,
+			  "qed create responder failed: cannot allocate memory (irq). rc = %d\n",
+			  rc);
+		return rc;
+	}
+
+	/* Get SPQ entry */
+	memset(&init_data, 0, sizeof(init_data));
+	init_data.cid = qp->icid;
+	init_data.opaque_fid = p_hwfn->hw_info.opaque_fid;
+	init_data.comp_mode = QED_SPQ_MODE_EBLOCK;
+
+	rc = qed_sp_init_request(p_hwfn, &p_ent, ROCE_RAMROD_CREATE_QP,
+				 PROTOCOLID_ROCE, &init_data);
+	if (rc)
+		goto err;
+
+	p_ramrod = &p_ent->ramrod.roce_create_qp_resp;
+
+	p_ramrod->flags = 0;
+
+	roce_flavor = qed_roce_mode_to_flavor(qp->roce_mode);
+	SET_FIELD(p_ramrod->flags,
+		  ROCE_CREATE_QP_RESP_RAMROD_DATA_ROCE_FLAVOR, roce_flavor);
+
+	SET_FIELD(p_ramrod->flags,
+		  ROCE_CREATE_QP_RESP_RAMROD_DATA_RDMA_RD_EN,
+		  qp->incoming_rdma_read_en);
+
+	SET_FIELD(p_ramrod->flags,
+		  ROCE_CREATE_QP_RESP_RAMROD_DATA_RDMA_WR_EN,
+		  qp->incoming_rdma_write_en);
+
+	SET_FIELD(p_ramrod->flags,
+		  ROCE_CREATE_QP_RESP_RAMROD_DATA_ATOMIC_EN,
+		  qp->incoming_atomic_en);
+
+	SET_FIELD(p_ramrod->flags,
+		  ROCE_CREATE_QP_RESP_RAMROD_DATA_E2E_FLOW_CONTROL_EN,
+		  qp->e2e_flow_control_en);
+
+	SET_FIELD(p_ramrod->flags,
+		  ROCE_CREATE_QP_RESP_RAMROD_DATA_SRQ_FLG, qp->use_srq);
+
+	SET_FIELD(p_ramrod->flags,
+		  ROCE_CREATE_QP_RESP_RAMROD_DATA_RESERVED_KEY_EN,
+		  qp->fmr_and_reserved_lkey);
+
+	SET_FIELD(p_ramrod->flags,
+		  ROCE_CREATE_QP_RESP_RAMROD_DATA_MIN_RNR_NAK_TIMER,
+		  qp->min_rnr_nak_timer);
+
+	p_ramrod->max_ird = qp->max_rd_atomic_resp;
+	p_ramrod->traffic_class = qp->traffic_class_tos;
+	p_ramrod->hop_limit = qp->hop_limit_ttl;
+	p_ramrod->irq_num_pages = qp->irq_num_pages;
+	p_ramrod->p_key = cpu_to_le16(qp->pkey);
+	p_ramrod->flow_label = cpu_to_le32(qp->flow_label);
+	p_ramrod->dst_qp_id = cpu_to_le32(qp->dest_qp);
+	p_ramrod->mtu = cpu_to_le16(qp->mtu);
+	p_ramrod->initial_psn = cpu_to_le32(qp->rq_psn);
+	p_ramrod->pd = cpu_to_le16(qp->pd);
+	p_ramrod->rq_num_pages = cpu_to_le16(qp->rq_num_pages);
+	DMA_REGPAIR_LE(p_ramrod->rq_pbl_addr, qp->rq_pbl_ptr);
+	DMA_REGPAIR_LE(p_ramrod->irq_pbl_addr, qp->irq_phys_addr);
+	qed_rdma_copy_gids(qp, p_ramrod->src_gid, p_ramrod->dst_gid);
+	p_ramrod->qp_handle_for_async.hi = cpu_to_le32(qp->qp_handle_async.hi);
+	p_ramrod->qp_handle_for_async.lo = cpu_to_le32(qp->qp_handle_async.lo);
+	p_ramrod->qp_handle_for_cqe.hi = cpu_to_le32(qp->qp_handle.hi);
+	p_ramrod->qp_handle_for_cqe.lo = cpu_to_le32(qp->qp_handle.lo);
+	p_ramrod->stats_counter_id = p_hwfn->rel_pf_id;
+	p_ramrod->cq_cid = cpu_to_le32((p_hwfn->hw_info.opaque_fid << 16) |
+				       qp->rq_cq_id);
+
+	memset(&qm_params, 0, sizeof(qm_params));
+	qm_params.roce.qpid = qp->icid >> 1;
+	physical_queue0 = qed_get_qm_pq(p_hwfn, PROTOCOLID_ROCE, &qm_params);
+
+	p_ramrod->physical_queue0 = cpu_to_le16(physical_queue0);
+	p_ramrod->dpi = cpu_to_le16(qp->dpi);
+
+	qed_rdma_set_fw_mac(p_ramrod->remote_mac_addr, qp->remote_mac_addr);
+	qed_rdma_set_fw_mac(p_ramrod->local_mac_addr, qp->local_mac_addr);
+
+	p_ramrod->udp_src_port = qp->udp_src_port;
+	p_ramrod->vlan_id = cpu_to_le16(qp->vlan_id);
+	p_ramrod->srq_id.srq_idx = cpu_to_le16(qp->srq_id);
+	p_ramrod->srq_id.opaque_fid = cpu_to_le16(p_hwfn->hw_info.opaque_fid);
+
+	p_ramrod->stats_counter_id = RESC_START(p_hwfn, QED_RDMA_STATS_QUEUE) +
+				     qp->stats_queue;
+
+	rc = qed_spq_post(p_hwfn, p_ent, NULL);
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "rc = %d physical_queue0 = 0x%x\n",
+		   rc, physical_queue0);
+
+	if (rc)
+		goto err;
+
+	qp->resp_offloaded = true;
+
+	return rc;
+
+err:
+	DP_NOTICE(p_hwfn, "create responder - failed, rc = %d\n", rc);
+	dma_free_coherent(&p_hwfn->cdev->pdev->dev,
+			  qp->irq_num_pages * RDMA_RING_PAGE_SIZE,
+			  qp->irq, qp->irq_phys_addr);
+
+	return rc;
+}
+
+static int qed_roce_sp_create_requester(struct qed_hwfn *p_hwfn,
+					struct qed_rdma_qp *qp)
+{
+	struct roce_create_qp_req_ramrod_data *p_ramrod;
+	struct qed_sp_init_data init_data;
+	union qed_qm_pq_params qm_params;
+	enum roce_flavor roce_flavor;
+	struct qed_spq_entry *p_ent;
+	u16 physical_queue0 = 0;
+	int rc;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "icid = %08x\n", qp->icid);
+
+	/* Allocate DMA-able memory for ORQ */
+	qp->orq_num_pages = 1;
+	qp->orq = dma_alloc_coherent(&p_hwfn->cdev->pdev->dev,
+				     RDMA_RING_PAGE_SIZE,
+				     &qp->orq_phys_addr, GFP_KERNEL);
+	if (!qp->orq) {
+		rc = -ENOMEM;
+		DP_NOTICE(p_hwfn,
+			  "qed create requester failed: cannot allocate memory (orq). rc = %d\n",
+			  rc);
+		return rc;
+	}
+
+	/* Get SPQ entry */
+	memset(&init_data, 0, sizeof(init_data));
+	init_data.cid = qp->icid + 1;
+	init_data.opaque_fid = p_hwfn->hw_info.opaque_fid;
+	init_data.comp_mode = QED_SPQ_MODE_EBLOCK;
+
+	rc = qed_sp_init_request(p_hwfn, &p_ent,
+				 ROCE_RAMROD_CREATE_QP,
+				 PROTOCOLID_ROCE, &init_data);
+	if (rc)
+		goto err;
+
+	p_ramrod = &p_ent->ramrod.roce_create_qp_req;
+
+	p_ramrod->flags = 0;
+
+	roce_flavor = qed_roce_mode_to_flavor(qp->roce_mode);
+	SET_FIELD(p_ramrod->flags,
+		  ROCE_CREATE_QP_REQ_RAMROD_DATA_ROCE_FLAVOR, roce_flavor);
+
+	SET_FIELD(p_ramrod->flags,
+		  ROCE_CREATE_QP_REQ_RAMROD_DATA_FMR_AND_RESERVED_EN,
+		  qp->fmr_and_reserved_lkey);
+
+	SET_FIELD(p_ramrod->flags,
+		  ROCE_CREATE_QP_REQ_RAMROD_DATA_SIGNALED_COMP, qp->signal_all);
+
+	SET_FIELD(p_ramrod->flags,
+		  ROCE_CREATE_QP_REQ_RAMROD_DATA_ERR_RETRY_CNT, qp->retry_cnt);
+
+	SET_FIELD(p_ramrod->flags,
+		  ROCE_CREATE_QP_REQ_RAMROD_DATA_RNR_NAK_CNT,
+		  qp->rnr_retry_cnt);
+
+	p_ramrod->max_ord = qp->max_rd_atomic_req;
+	p_ramrod->traffic_class = qp->traffic_class_tos;
+	p_ramrod->hop_limit = qp->hop_limit_ttl;
+	p_ramrod->orq_num_pages = qp->orq_num_pages;
+	p_ramrod->p_key = cpu_to_le16(qp->pkey);
+	p_ramrod->flow_label = cpu_to_le32(qp->flow_label);
+	p_ramrod->dst_qp_id = cpu_to_le32(qp->dest_qp);
+	p_ramrod->ack_timeout_val = cpu_to_le32(qp->ack_timeout);
+	p_ramrod->mtu = cpu_to_le16(qp->mtu);
+	p_ramrod->initial_psn = cpu_to_le32(qp->sq_psn);
+	p_ramrod->pd = cpu_to_le16(qp->pd);
+	p_ramrod->sq_num_pages = cpu_to_le16(qp->sq_num_pages);
+	DMA_REGPAIR_LE(p_ramrod->sq_pbl_addr, qp->sq_pbl_ptr);
+	DMA_REGPAIR_LE(p_ramrod->orq_pbl_addr, qp->orq_phys_addr);
+	qed_rdma_copy_gids(qp, p_ramrod->src_gid, p_ramrod->dst_gid);
+	p_ramrod->qp_handle_for_async.hi = cpu_to_le32(qp->qp_handle_async.hi);
+	p_ramrod->qp_handle_for_async.lo = cpu_to_le32(qp->qp_handle_async.lo);
+	p_ramrod->qp_handle_for_cqe.hi = cpu_to_le32(qp->qp_handle.hi);
+	p_ramrod->qp_handle_for_cqe.lo = cpu_to_le32(qp->qp_handle.lo);
+	p_ramrod->stats_counter_id = p_hwfn->rel_pf_id;
+	p_ramrod->cq_cid = cpu_to_le32((p_hwfn->hw_info.opaque_fid << 16) |
+				       qp->sq_cq_id);
+
+	memset(&qm_params, 0, sizeof(qm_params));
+	qm_params.roce.qpid = qp->icid >> 1;
+	physical_queue0 = qed_get_qm_pq(p_hwfn, PROTOCOLID_ROCE, &qm_params);
+
+	p_ramrod->physical_queue0 = cpu_to_le16(physical_queue0);
+	p_ramrod->dpi = cpu_to_le16(qp->dpi);
+
+	qed_rdma_set_fw_mac(p_ramrod->remote_mac_addr, qp->remote_mac_addr);
+	qed_rdma_set_fw_mac(p_ramrod->local_mac_addr, qp->local_mac_addr);
+
+	p_ramrod->udp_src_port = qp->udp_src_port;
+	p_ramrod->vlan_id = cpu_to_le16(qp->vlan_id);
+	p_ramrod->stats_counter_id = RESC_START(p_hwfn, QED_RDMA_STATS_QUEUE) +
+				     qp->stats_queue;
+
+	rc = qed_spq_post(p_hwfn, p_ent, NULL);
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "rc = %d\n", rc);
+
+	if (rc)
+		goto err;
+
+	qp->req_offloaded = true;
+
+	return rc;
+
+err:
+	DP_NOTICE(p_hwfn, "Create requested - failed, rc = %d\n", rc);
+	dma_free_coherent(&p_hwfn->cdev->pdev->dev,
+			  qp->orq_num_pages * RDMA_RING_PAGE_SIZE,
+			  qp->orq, qp->orq_phys_addr);
+	return rc;
+}
+
+static int qed_roce_sp_modify_responder(struct qed_hwfn *p_hwfn,
+					struct qed_rdma_qp *qp,
+					bool move_to_err, u32 modify_flags)
+{
+	struct roce_modify_qp_resp_ramrod_data *p_ramrod;
+	struct qed_sp_init_data init_data;
+	struct qed_spq_entry *p_ent;
+	int rc;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "icid = %08x\n", qp->icid);
+
+	if (move_to_err && !qp->resp_offloaded)
+		return 0;
+
+	/* Get SPQ entry */
+	memset(&init_data, 0, sizeof(init_data));
+	init_data.cid = qp->icid;
+	init_data.opaque_fid = p_hwfn->hw_info.opaque_fid;
+	init_data.comp_mode = QED_SPQ_MODE_EBLOCK;
+
+	rc = qed_sp_init_request(p_hwfn, &p_ent,
+				 ROCE_EVENT_MODIFY_QP,
+				 PROTOCOLID_ROCE, &init_data);
+	if (rc) {
+		DP_NOTICE(p_hwfn, "rc = %d\n", rc);
+		return rc;
+	}
+
+	p_ramrod = &p_ent->ramrod.roce_modify_qp_resp;
+
+	p_ramrod->flags = 0;
+
+	SET_FIELD(p_ramrod->flags,
+		  ROCE_MODIFY_QP_RESP_RAMROD_DATA_MOVE_TO_ERR_FLG, move_to_err);
+
+	SET_FIELD(p_ramrod->flags,
+		  ROCE_MODIFY_QP_RESP_RAMROD_DATA_RDMA_RD_EN,
+		  qp->incoming_rdma_read_en);
+
+	SET_FIELD(p_ramrod->flags,
+		  ROCE_MODIFY_QP_RESP_RAMROD_DATA_RDMA_WR_EN,
+		  qp->incoming_rdma_write_en);
+
+	SET_FIELD(p_ramrod->flags,
+		  ROCE_MODIFY_QP_RESP_RAMROD_DATA_ATOMIC_EN,
+		  qp->incoming_atomic_en);
+
+	SET_FIELD(p_ramrod->flags,
+		  ROCE_CREATE_QP_RESP_RAMROD_DATA_E2E_FLOW_CONTROL_EN,
+		  qp->e2e_flow_control_en);
+
+	SET_FIELD(p_ramrod->flags,
+		  ROCE_MODIFY_QP_RESP_RAMROD_DATA_RDMA_OPS_EN_FLG,
+		  GET_FIELD(modify_flags,
+			    QED_RDMA_MODIFY_QP_VALID_RDMA_OPS_EN));
+
+	SET_FIELD(p_ramrod->flags,
+		  ROCE_MODIFY_QP_RESP_RAMROD_DATA_P_KEY_FLG,
+		  GET_FIELD(modify_flags, QED_ROCE_MODIFY_QP_VALID_PKEY));
+
+	SET_FIELD(p_ramrod->flags,
+		  ROCE_MODIFY_QP_RESP_RAMROD_DATA_ADDRESS_VECTOR_FLG,
+		  GET_FIELD(modify_flags,
+			    QED_ROCE_MODIFY_QP_VALID_ADDRESS_VECTOR));
+
+	SET_FIELD(p_ramrod->flags,
+		  ROCE_MODIFY_QP_RESP_RAMROD_DATA_MAX_IRD_FLG,
+		  GET_FIELD(modify_flags,
+			    QED_RDMA_MODIFY_QP_VALID_MAX_RD_ATOMIC_RESP));
+
+	SET_FIELD(p_ramrod->flags,
+		  ROCE_MODIFY_QP_RESP_RAMROD_DATA_MIN_RNR_NAK_TIMER_FLG,
+		  GET_FIELD(modify_flags,
+			    QED_ROCE_MODIFY_QP_VALID_MIN_RNR_NAK_TIMER));
+
+	p_ramrod->fields = 0;
+	SET_FIELD(p_ramrod->fields,
+		  ROCE_MODIFY_QP_RESP_RAMROD_DATA_MIN_RNR_NAK_TIMER,
+		  qp->min_rnr_nak_timer);
+
+	p_ramrod->max_ird = qp->max_rd_atomic_resp;
+	p_ramrod->traffic_class = qp->traffic_class_tos;
+	p_ramrod->hop_limit = qp->hop_limit_ttl;
+	p_ramrod->p_key = cpu_to_le16(qp->pkey);
+	p_ramrod->flow_label = cpu_to_le32(qp->flow_label);
+	p_ramrod->mtu = cpu_to_le16(qp->mtu);
+	qed_rdma_copy_gids(qp, p_ramrod->src_gid, p_ramrod->dst_gid);
+	rc = qed_spq_post(p_hwfn, p_ent, NULL);
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Modify responder, rc = %d\n", rc);
+	return rc;
+}
+
+static int qed_roce_sp_modify_requester(struct qed_hwfn *p_hwfn,
+					struct qed_rdma_qp *qp,
+					bool move_to_sqd,
+					bool move_to_err, u32 modify_flags)
+{
+	struct roce_modify_qp_req_ramrod_data *p_ramrod;
+	struct qed_sp_init_data init_data;
+	struct qed_spq_entry *p_ent;
+	int rc;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "icid = %08x\n", qp->icid);
+
+	if (move_to_err && !(qp->req_offloaded))
+		return 0;
+
+	/* Get SPQ entry */
+	memset(&init_data, 0, sizeof(init_data));
+	init_data.cid = qp->icid + 1;
+	init_data.opaque_fid = p_hwfn->hw_info.opaque_fid;
+	init_data.comp_mode = QED_SPQ_MODE_EBLOCK;
+
+	rc = qed_sp_init_request(p_hwfn, &p_ent,
+				 ROCE_EVENT_MODIFY_QP,
+				 PROTOCOLID_ROCE, &init_data);
+	if (rc) {
+		DP_NOTICE(p_hwfn, "rc = %d\n", rc);
+		return rc;
+	}
+
+	p_ramrod = &p_ent->ramrod.roce_modify_qp_req;
+
+	p_ramrod->flags = 0;
+
+	SET_FIELD(p_ramrod->flags,
+		  ROCE_MODIFY_QP_REQ_RAMROD_DATA_MOVE_TO_ERR_FLG, move_to_err);
+
+	SET_FIELD(p_ramrod->flags,
+		  ROCE_MODIFY_QP_REQ_RAMROD_DATA_MOVE_TO_SQD_FLG, move_to_sqd);
+
+	SET_FIELD(p_ramrod->flags,
+		  ROCE_MODIFY_QP_REQ_RAMROD_DATA_EN_SQD_ASYNC_NOTIFY,
+		  qp->sqd_async);
+
+	SET_FIELD(p_ramrod->flags,
+		  ROCE_MODIFY_QP_REQ_RAMROD_DATA_P_KEY_FLG,
+		  GET_FIELD(modify_flags, QED_ROCE_MODIFY_QP_VALID_PKEY));
+
+	SET_FIELD(p_ramrod->flags,
+		  ROCE_MODIFY_QP_REQ_RAMROD_DATA_ADDRESS_VECTOR_FLG,
+		  GET_FIELD(modify_flags,
+			    QED_ROCE_MODIFY_QP_VALID_ADDRESS_VECTOR));
+
+	SET_FIELD(p_ramrod->flags,
+		  ROCE_MODIFY_QP_REQ_RAMROD_DATA_MAX_ORD_FLG,
+		  GET_FIELD(modify_flags,
+			    QED_RDMA_MODIFY_QP_VALID_MAX_RD_ATOMIC_REQ));
+
+	SET_FIELD(p_ramrod->flags,
+		  ROCE_MODIFY_QP_REQ_RAMROD_DATA_RNR_NAK_CNT_FLG,
+		  GET_FIELD(modify_flags,
+			    QED_ROCE_MODIFY_QP_VALID_RNR_RETRY_CNT));
+
+	SET_FIELD(p_ramrod->flags,
+		  ROCE_MODIFY_QP_REQ_RAMROD_DATA_ERR_RETRY_CNT_FLG,
+		  GET_FIELD(modify_flags, QED_ROCE_MODIFY_QP_VALID_RETRY_CNT));
+
+	SET_FIELD(p_ramrod->flags,
+		  ROCE_MODIFY_QP_REQ_RAMROD_DATA_ACK_TIMEOUT_FLG,
+		  GET_FIELD(modify_flags,
+			    QED_ROCE_MODIFY_QP_VALID_ACK_TIMEOUT));
+
+	p_ramrod->fields = 0;
+	SET_FIELD(p_ramrod->fields,
+		  ROCE_MODIFY_QP_REQ_RAMROD_DATA_ERR_RETRY_CNT, qp->retry_cnt);
+
+	SET_FIELD(p_ramrod->fields,
+		  ROCE_MODIFY_QP_REQ_RAMROD_DATA_RNR_NAK_CNT,
+		  qp->rnr_retry_cnt);
+
+	p_ramrod->max_ord = qp->max_rd_atomic_req;
+	p_ramrod->traffic_class = qp->traffic_class_tos;
+	p_ramrod->hop_limit = qp->hop_limit_ttl;
+	p_ramrod->p_key = cpu_to_le16(qp->pkey);
+	p_ramrod->flow_label = cpu_to_le32(qp->flow_label);
+	p_ramrod->ack_timeout_val = cpu_to_le32(qp->ack_timeout);
+	p_ramrod->mtu = cpu_to_le16(qp->mtu);
+	qed_rdma_copy_gids(qp, p_ramrod->src_gid, p_ramrod->dst_gid);
+	rc = qed_spq_post(p_hwfn, p_ent, NULL);
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Modify requester, rc = %d\n", rc);
+	return rc;
+}
+
+static int qed_roce_sp_destroy_qp_responder(struct qed_hwfn *p_hwfn,
+					    struct qed_rdma_qp *qp,
+					    u32 *num_invalidated_mw)
+{
+	struct roce_destroy_qp_resp_output_params *p_ramrod_res;
+	struct roce_destroy_qp_resp_ramrod_data *p_ramrod;
+	struct qed_sp_init_data init_data;
+	struct qed_spq_entry *p_ent;
+	dma_addr_t ramrod_res_phys;
+	int rc;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "icid = %08x\n", qp->icid);
+
+	if (!qp->resp_offloaded)
+		return 0;
+
+	/* Get SPQ entry */
+	memset(&init_data, 0, sizeof(init_data));
+	init_data.cid = qp->icid;
+	init_data.opaque_fid = p_hwfn->hw_info.opaque_fid;
+	init_data.comp_mode = QED_SPQ_MODE_EBLOCK;
+
+	rc = qed_sp_init_request(p_hwfn, &p_ent,
+				 ROCE_RAMROD_DESTROY_QP,
+				 PROTOCOLID_ROCE, &init_data);
+	if (rc)
+		return rc;
+
+	p_ramrod = &p_ent->ramrod.roce_destroy_qp_resp;
+
+	p_ramrod_res = (struct roce_destroy_qp_resp_output_params *)
+	    dma_alloc_coherent(&p_hwfn->cdev->pdev->dev, sizeof(*p_ramrod_res),
+			       &ramrod_res_phys, GFP_KERNEL);
+
+	if (!p_ramrod_res) {
+		rc = -ENOMEM;
+		DP_NOTICE(p_hwfn,
+			  "qed destroy responder failed: cannot allocate memory (ramrod). rc = %d\n",
+			  rc);
+		return rc;
+	}
+
+	DMA_REGPAIR_LE(p_ramrod->output_params_addr, ramrod_res_phys);
+
+	rc = qed_spq_post(p_hwfn, p_ent, NULL);
+	if (rc)
+		goto err;
+
+	*num_invalidated_mw = le32_to_cpu(p_ramrod_res->num_invalidated_mw);
+
+	/* Free IRQ - only if ramrod succeeded, in case FW is still using it */
+	dma_free_coherent(&p_hwfn->cdev->pdev->dev,
+			  qp->irq_num_pages * RDMA_RING_PAGE_SIZE,
+			  qp->irq, qp->irq_phys_addr);
+
+	qp->resp_offloaded = false;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Destroy responder, rc = %d\n", rc);
+
+err:
+	dma_free_coherent(&p_hwfn->cdev->pdev->dev,
+			  sizeof(struct roce_destroy_qp_resp_output_params),
+			  p_ramrod_res, ramrod_res_phys);
+
+	return rc;
+}
+
+static int qed_roce_sp_destroy_qp_requester(struct qed_hwfn *p_hwfn,
+					    struct qed_rdma_qp *qp,
+					    u32 *num_bound_mw)
+{
+	struct roce_destroy_qp_req_output_params *p_ramrod_res;
+	struct roce_destroy_qp_req_ramrod_data *p_ramrod;
+	struct qed_sp_init_data init_data;
+	struct qed_spq_entry *p_ent;
+	dma_addr_t ramrod_res_phys;
+	int rc = -ENOMEM;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "icid = %08x\n", qp->icid);
+
+	if (!qp->req_offloaded)
+		return 0;
+
+	p_ramrod_res = (struct roce_destroy_qp_req_output_params *)
+		       dma_alloc_coherent(&p_hwfn->cdev->pdev->dev,
+					  sizeof(*p_ramrod_res),
+					  &ramrod_res_phys, GFP_KERNEL);
+	if (!p_ramrod_res) {
+		DP_NOTICE(p_hwfn,
+			  "qed destroy requester failed: cannot allocate memory (ramrod)\n");
+		return rc;
+	}
+
+	/* Get SPQ entry */
+	memset(&init_data, 0, sizeof(init_data));
+	init_data.cid = qp->icid + 1;
+	init_data.opaque_fid = p_hwfn->hw_info.opaque_fid;
+	init_data.comp_mode = QED_SPQ_MODE_EBLOCK;
+
+	rc = qed_sp_init_request(p_hwfn, &p_ent, ROCE_RAMROD_DESTROY_QP,
+				 PROTOCOLID_ROCE, &init_data);
+	if (rc)
+		goto err;
+
+	p_ramrod = &p_ent->ramrod.roce_destroy_qp_req;
+	DMA_REGPAIR_LE(p_ramrod->output_params_addr, ramrod_res_phys);
+
+	rc = qed_spq_post(p_hwfn, p_ent, NULL);
+	if (rc)
+		goto err;
+
+	*num_bound_mw = le32_to_cpu(p_ramrod_res->num_bound_mw);
+
+	/* Free ORQ - only if ramrod succeeded, in case FW is still using it */
+	dma_free_coherent(&p_hwfn->cdev->pdev->dev,
+			  qp->orq_num_pages * RDMA_RING_PAGE_SIZE,
+			  qp->orq, qp->orq_phys_addr);
+
+	qp->req_offloaded = false;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Destroy requester, rc = %d\n", rc);
+
+err:
+	dma_free_coherent(&p_hwfn->cdev->pdev->dev, sizeof(*p_ramrod_res),
+			  p_ramrod_res, ramrod_res_phys);
+
+	return rc;
+}
+
+int qed_roce_query_qp(struct qed_hwfn *p_hwfn,
+		      struct qed_rdma_qp *qp,
+		      struct qed_rdma_query_qp_out_params *out_params)
+{
+	struct roce_query_qp_resp_output_params *p_resp_ramrod_res;
+	struct roce_query_qp_req_output_params *p_req_ramrod_res;
+	struct roce_query_qp_resp_ramrod_data *p_resp_ramrod;
+	struct roce_query_qp_req_ramrod_data *p_req_ramrod;
+	struct qed_sp_init_data init_data;
+	dma_addr_t resp_ramrod_res_phys;
+	dma_addr_t req_ramrod_res_phys;
+	struct qed_spq_entry *p_ent;
+	bool rq_err_state;
+	bool sq_err_state;
+	bool sq_draining;
+	int rc = -ENOMEM;
+
+	if ((!(qp->resp_offloaded)) && (!(qp->req_offloaded))) {
+		/* We can't send ramrod to the fw since this qp wasn't offloaded
+		 * to the fw yet
+		 */
+		out_params->draining = false;
+		out_params->rq_psn = qp->rq_psn;
+		out_params->sq_psn = qp->sq_psn;
+		out_params->state = qp->cur_state;
+
+		DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "No QPs as no offload\n");
+		return 0;
+	}
+
+	if (!(qp->resp_offloaded)) {
+		DP_NOTICE(p_hwfn,
+			  "The responder's qp should be offloded before requester's\n");
+		return -EINVAL;
+	}
+
+	/* Send a query responder ramrod to FW to get RQ-PSN and state */
+	p_resp_ramrod_res = (struct roce_query_qp_resp_output_params *)
+	    dma_alloc_coherent(&p_hwfn->cdev->pdev->dev,
+			       sizeof(*p_resp_ramrod_res),
+			       &resp_ramrod_res_phys, GFP_KERNEL);
+	if (!p_resp_ramrod_res) {
+		DP_NOTICE(p_hwfn,
+			  "qed query qp failed: cannot allocate memory (ramrod)\n");
+		return rc;
+	}
+
+	/* Get SPQ entry */
+	memset(&init_data, 0, sizeof(init_data));
+	init_data.cid = qp->icid;
+	init_data.opaque_fid = p_hwfn->hw_info.opaque_fid;
+	init_data.comp_mode = QED_SPQ_MODE_EBLOCK;
+	rc = qed_sp_init_request(p_hwfn, &p_ent, ROCE_RAMROD_QUERY_QP,
+				 PROTOCOLID_ROCE, &init_data);
+	if (rc)
+		goto err_resp;
+
+	p_resp_ramrod = &p_ent->ramrod.roce_query_qp_resp;
+	DMA_REGPAIR_LE(p_resp_ramrod->output_params_addr, resp_ramrod_res_phys);
+
+	rc = qed_spq_post(p_hwfn, p_ent, NULL);
+	if (rc)
+		goto err_resp;
+
+	dma_free_coherent(&p_hwfn->cdev->pdev->dev, sizeof(*p_resp_ramrod_res),
+			  p_resp_ramrod_res, resp_ramrod_res_phys);
+
+	out_params->rq_psn = le32_to_cpu(p_resp_ramrod_res->psn);
+	rq_err_state = GET_FIELD(le32_to_cpu(p_resp_ramrod_res->err_flag),
+				 ROCE_QUERY_QP_RESP_OUTPUT_PARAMS_ERROR_FLG);
+
+	if (!(qp->req_offloaded)) {
+		/* Don't send query qp for the requester */
+		out_params->sq_psn = qp->sq_psn;
+		out_params->draining = false;
+
+		if (rq_err_state)
+			qp->cur_state = QED_ROCE_QP_STATE_ERR;
+
+		out_params->state = qp->cur_state;
+
+		return 0;
+	}
+
+	/* Send a query requester ramrod to FW to get SQ-PSN and state */
+	p_req_ramrod_res = (struct roce_query_qp_req_output_params *)
+			   dma_alloc_coherent(&p_hwfn->cdev->pdev->dev,
+					      sizeof(*p_req_ramrod_res),
+					      &req_ramrod_res_phys,
+					      GFP_KERNEL);
+	if (!p_req_ramrod_res) {
+		rc = -ENOMEM;
+		DP_NOTICE(p_hwfn,
+			  "qed query qp failed: cannot allocate memory (ramrod)\n");
+		return rc;
+	}
+
+	/* Get SPQ entry */
+	init_data.cid = qp->icid + 1;
+	rc = qed_sp_init_request(p_hwfn, &p_ent, ROCE_RAMROD_QUERY_QP,
+				 PROTOCOLID_ROCE, &init_data);
+	if (rc)
+		goto err_req;
+
+	p_req_ramrod = &p_ent->ramrod.roce_query_qp_req;
+	DMA_REGPAIR_LE(p_req_ramrod->output_params_addr, req_ramrod_res_phys);
+
+	rc = qed_spq_post(p_hwfn, p_ent, NULL);
+	if (rc)
+		goto err_req;
+
+	dma_free_coherent(&p_hwfn->cdev->pdev->dev, sizeof(*p_req_ramrod_res),
+			  p_req_ramrod_res, req_ramrod_res_phys);
+
+	out_params->sq_psn = le32_to_cpu(p_req_ramrod_res->psn);
+	sq_err_state = GET_FIELD(le32_to_cpu(p_req_ramrod_res->flags),
+				 ROCE_QUERY_QP_REQ_OUTPUT_PARAMS_ERR_FLG);
+	sq_draining =
+		GET_FIELD(le32_to_cpu(p_req_ramrod_res->flags),
+			  ROCE_QUERY_QP_REQ_OUTPUT_PARAMS_SQ_DRAINING_FLG);
+
+	out_params->draining = false;
+
+	if (rq_err_state)
+		qp->cur_state = QED_ROCE_QP_STATE_ERR;
+	else if (sq_err_state)
+		qp->cur_state = QED_ROCE_QP_STATE_SQE;
+	else if (sq_draining)
+		out_params->draining = true;
+	out_params->state = qp->cur_state;
+
+	return 0;
+
+err_req:
+	dma_free_coherent(&p_hwfn->cdev->pdev->dev, sizeof(*p_req_ramrod_res),
+			  p_req_ramrod_res, req_ramrod_res_phys);
+	return rc;
+err_resp:
+	dma_free_coherent(&p_hwfn->cdev->pdev->dev, sizeof(*p_resp_ramrod_res),
+			  p_resp_ramrod_res, resp_ramrod_res_phys);
+	return rc;
+}
+
+int qed_roce_destroy_qp(struct qed_hwfn *p_hwfn, struct qed_rdma_qp *qp)
+{
+	u32 num_invalidated_mw = 0;
+	u32 num_bound_mw = 0;
+	u32 start_cid;
+	int rc;
+
+	/* Destroys the specified QP */
+	if ((qp->cur_state != QED_ROCE_QP_STATE_RESET) &&
+	    (qp->cur_state != QED_ROCE_QP_STATE_ERR) &&
+	    (qp->cur_state != QED_ROCE_QP_STATE_INIT)) {
+		DP_NOTICE(p_hwfn,
+			  "QP must be in error, reset or init state before destroying it\n");
+		return -EINVAL;
+	}
+
+	rc = qed_roce_sp_destroy_qp_responder(p_hwfn, qp, &num_invalidated_mw);
+	if (rc)
+		return rc;
+
+	/* Send destroy requester ramrod */
+	rc = qed_roce_sp_destroy_qp_requester(p_hwfn, qp, &num_bound_mw);
+	if (rc)
+		return rc;
+
+	if (num_invalidated_mw != num_bound_mw) {
+		DP_NOTICE(p_hwfn,
+			  "number of invalidate memory windows is different from bounded ones\n");
+		return -EINVAL;
+	}
+
+	spin_lock_bh(&p_hwfn->p_rdma_info->lock);
+
+	start_cid = qed_cxt_get_proto_cid_start(p_hwfn,
+						p_hwfn->p_rdma_info->proto);
+
+	/* Release responder's icid */
+	qed_bmap_release_id(p_hwfn, &p_hwfn->p_rdma_info->cid_map,
+			    qp->icid - start_cid);
+
+	/* Release requester's icid */
+	qed_bmap_release_id(p_hwfn, &p_hwfn->p_rdma_info->cid_map,
+			    qp->icid + 1 - start_cid);
+
+	spin_unlock_bh(&p_hwfn->p_rdma_info->lock);
+
+	return 0;
+}
+
+int qed_rdma_query_qp(void *rdma_cxt,
+		      struct qed_rdma_qp *qp,
+		      struct qed_rdma_query_qp_out_params *out_params)
+{
+	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
+	int rc;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "icid = %08x\n", qp->icid);
+
+	/* The following fields are filled in from qp and not FW as they can't
+	 * be modified by FW
+	 */
+	out_params->mtu = qp->mtu;
+	out_params->dest_qp = qp->dest_qp;
+	out_params->incoming_atomic_en = qp->incoming_atomic_en;
+	out_params->e2e_flow_control_en = qp->e2e_flow_control_en;
+	out_params->incoming_rdma_read_en = qp->incoming_rdma_read_en;
+	out_params->incoming_rdma_write_en = qp->incoming_rdma_write_en;
+	out_params->dgid = qp->dgid;
+	out_params->flow_label = qp->flow_label;
+	out_params->hop_limit_ttl = qp->hop_limit_ttl;
+	out_params->traffic_class_tos = qp->traffic_class_tos;
+	out_params->timeout = qp->ack_timeout;
+	out_params->rnr_retry = qp->rnr_retry_cnt;
+	out_params->retry_cnt = qp->retry_cnt;
+	out_params->min_rnr_nak_timer = qp->min_rnr_nak_timer;
+	out_params->pkey_index = 0;
+	out_params->max_rd_atomic = qp->max_rd_atomic_req;
+	out_params->max_dest_rd_atomic = qp->max_rd_atomic_resp;
+	out_params->sqd_async = qp->sqd_async;
+
+	rc = qed_roce_query_qp(p_hwfn, qp, out_params);
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Query QP, rc = %d\n", rc);
+	return rc;
+}
+
+int qed_rdma_destroy_qp(void *rdma_cxt, struct qed_rdma_qp *qp)
+{
+	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
+	int rc = 0;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "icid = %08x\n", qp->icid);
+
+	rc = qed_roce_destroy_qp(p_hwfn, qp);
+
+	/* free qp params struct */
+	kfree(qp);
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "QP destroyed\n");
+	return rc;
+}
+
+struct qed_rdma_qp *
+qed_rdma_create_qp(void *rdma_cxt,
+		   struct qed_rdma_create_qp_in_params *in_params,
+		   struct qed_rdma_create_qp_out_params *out_params)
+{
+	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
+	struct qed_rdma_qp *qp;
+	u8 max_stats_queues;
+	int rc;
+
+	if (!rdma_cxt || !in_params || !out_params || !p_hwfn->p_rdma_info) {
+		DP_ERR(p_hwfn->cdev,
+		       "qed roce create qp failed due to NULL entry (rdma_cxt=%p, in=%p, out=%p, roce_info=?\n",
+		       rdma_cxt, in_params, out_params);
+		return NULL;
+	}
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
+		   "qed rdma create qp called with qp_handle = %08x%08x\n",
+		   in_params->qp_handle_hi, in_params->qp_handle_lo);
+
+	/* Some sanity checks... */
+	max_stats_queues = p_hwfn->p_rdma_info->dev->max_stats_queues;
+	if (in_params->stats_queue >= max_stats_queues) {
+		DP_ERR(p_hwfn->cdev,
+		       "qed rdma create qp failed due to invalid statistics queue %d. maximum is %d\n",
+		       in_params->stats_queue, max_stats_queues);
+		return NULL;
+	}
+
+	qp = kzalloc(sizeof(*qp), GFP_KERNEL);
+	if (!qp) {
+		DP_NOTICE(p_hwfn, "Failed to allocate qed_rdma_qp\n");
+		return NULL;
+	}
+
+	rc = qed_roce_alloc_cid(p_hwfn, &qp->icid);
+	qp->qpid = ((0xFF << 16) | qp->icid);
+
+	DP_INFO(p_hwfn, "ROCE qpid=%x\n", qp->qpid);
+
+	if (rc) {
+		kfree(qp);
+		return NULL;
+	}
+
+	qp->cur_state = QED_ROCE_QP_STATE_RESET;
+	qp->qp_handle.hi = cpu_to_le32(in_params->qp_handle_hi);
+	qp->qp_handle.lo = cpu_to_le32(in_params->qp_handle_lo);
+	qp->qp_handle_async.hi = cpu_to_le32(in_params->qp_handle_async_hi);
+	qp->qp_handle_async.lo = cpu_to_le32(in_params->qp_handle_async_lo);
+	qp->use_srq = in_params->use_srq;
+	qp->signal_all = in_params->signal_all;
+	qp->fmr_and_reserved_lkey = in_params->fmr_and_reserved_lkey;
+	qp->pd = in_params->pd;
+	qp->dpi = in_params->dpi;
+	qp->sq_cq_id = in_params->sq_cq_id;
+	qp->sq_num_pages = in_params->sq_num_pages;
+	qp->sq_pbl_ptr = in_params->sq_pbl_ptr;
+	qp->rq_cq_id = in_params->rq_cq_id;
+	qp->rq_num_pages = in_params->rq_num_pages;
+	qp->rq_pbl_ptr = in_params->rq_pbl_ptr;
+	qp->srq_id = in_params->srq_id;
+	qp->req_offloaded = false;
+	qp->resp_offloaded = false;
+	qp->e2e_flow_control_en = qp->use_srq ? false : true;
+	qp->stats_queue = in_params->stats_queue;
+
+	out_params->icid = qp->icid;
+	out_params->qp_id = qp->qpid;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Create QP, rc = %d\n", rc);
+	return qp;
+}
+
+static int qed_roce_modify_qp(struct qed_hwfn *p_hwfn,
+			      struct qed_rdma_qp *qp,
+			      enum qed_roce_qp_state prev_state,
+			      struct qed_rdma_modify_qp_in_params *params)
+{
+	u32 num_invalidated_mw = 0, num_bound_mw = 0;
+	int rc = 0;
+
+	/* Perform additional operations according to the current state and the
+	 * next state
+	 */
+	if (((prev_state == QED_ROCE_QP_STATE_INIT) ||
+	     (prev_state == QED_ROCE_QP_STATE_RESET)) &&
+	    (qp->cur_state == QED_ROCE_QP_STATE_RTR)) {
+		/* Init->RTR or Reset->RTR */
+		rc = qed_roce_sp_create_responder(p_hwfn, qp);
+		return rc;
+	} else if ((prev_state == QED_ROCE_QP_STATE_RTR) &&
+		   (qp->cur_state == QED_ROCE_QP_STATE_RTS)) {
+		/* RTR-> RTS */
+		rc = qed_roce_sp_create_requester(p_hwfn, qp);
+		if (rc)
+			return rc;
+
+		/* Send modify responder ramrod */
+		rc = qed_roce_sp_modify_responder(p_hwfn, qp, false,
+						  params->modify_flags);
+		return rc;
+	} else if ((prev_state == QED_ROCE_QP_STATE_RTS) &&
+		   (qp->cur_state == QED_ROCE_QP_STATE_RTS)) {
+		/* RTS->RTS */
+		rc = qed_roce_sp_modify_responder(p_hwfn, qp, false,
+						  params->modify_flags);
+		if (rc)
+			return rc;
+
+		rc = qed_roce_sp_modify_requester(p_hwfn, qp, false, false,
+						  params->modify_flags);
+		return rc;
+	} else if ((prev_state == QED_ROCE_QP_STATE_RTS) &&
+		   (qp->cur_state == QED_ROCE_QP_STATE_SQD)) {
+		/* RTS->SQD */
+		rc = qed_roce_sp_modify_requester(p_hwfn, qp, true, false,
+						  params->modify_flags);
+		return rc;
+	} else if ((prev_state == QED_ROCE_QP_STATE_SQD) &&
+		   (qp->cur_state == QED_ROCE_QP_STATE_SQD)) {
+		/* SQD->SQD */
+		rc = qed_roce_sp_modify_responder(p_hwfn, qp, false,
+						  params->modify_flags);
+		if (rc)
+			return rc;
+
+		rc = qed_roce_sp_modify_requester(p_hwfn, qp, false, false,
+						  params->modify_flags);
+		return rc;
+	} else if ((prev_state == QED_ROCE_QP_STATE_SQD) &&
+		   (qp->cur_state == QED_ROCE_QP_STATE_RTS)) {
+		/* SQD->RTS */
+		rc = qed_roce_sp_modify_responder(p_hwfn, qp, false,
+						  params->modify_flags);
+		if (rc)
+			return rc;
+
+		rc = qed_roce_sp_modify_requester(p_hwfn, qp, false, false,
+						  params->modify_flags);
+
+		return rc;
+	} else if (qp->cur_state == QED_ROCE_QP_STATE_ERR ||
+		   qp->cur_state == QED_ROCE_QP_STATE_SQE) {
+		/* ->ERR */
+		rc = qed_roce_sp_modify_responder(p_hwfn, qp, true,
+						  params->modify_flags);
+		if (rc)
+			return rc;
+
+		rc = qed_roce_sp_modify_requester(p_hwfn, qp, false, true,
+						  params->modify_flags);
+		return rc;
+	} else if (qp->cur_state == QED_ROCE_QP_STATE_RESET) {
+		/* Any state -> RESET */
+
+		rc = qed_roce_sp_destroy_qp_responder(p_hwfn, qp,
+						      &num_invalidated_mw);
+		if (rc)
+			return rc;
+
+		rc = qed_roce_sp_destroy_qp_requester(p_hwfn, qp,
+						      &num_bound_mw);
+
+		if (num_invalidated_mw != num_bound_mw) {
+			DP_NOTICE(p_hwfn,
+				  "number of invalidate memory windows is different from bounded ones\n");
+			return -EINVAL;
+		}
+	} else {
+		DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "0\n");
+	}
+
+	return rc;
+}
+
+int qed_rdma_modify_qp(void *rdma_cxt,
+		       struct qed_rdma_qp *qp,
+		       struct qed_rdma_modify_qp_in_params *params)
+{
+	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
+	enum qed_roce_qp_state prev_state;
+	int rc = 0;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "icid = %08x params->new_state=%d\n",
+		   qp->icid, params->new_state);
+
+	if (rc) {
+		DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "rc = %d\n", rc);
+		return rc;
+	}
+
+	if (GET_FIELD(params->modify_flags,
+		      QED_RDMA_MODIFY_QP_VALID_RDMA_OPS_EN)) {
+		qp->incoming_rdma_read_en = params->incoming_rdma_read_en;
+		qp->incoming_rdma_write_en = params->incoming_rdma_write_en;
+		qp->incoming_atomic_en = params->incoming_atomic_en;
+	}
+
+	/* Update QP structure with the updated values */
+	if (GET_FIELD(params->modify_flags, QED_ROCE_MODIFY_QP_VALID_ROCE_MODE))
+		qp->roce_mode = params->roce_mode;
+	if (GET_FIELD(params->modify_flags, QED_ROCE_MODIFY_QP_VALID_PKEY))
+		qp->pkey = params->pkey;
+	if (GET_FIELD(params->modify_flags,
+		      QED_ROCE_MODIFY_QP_VALID_E2E_FLOW_CONTROL_EN))
+		qp->e2e_flow_control_en = params->e2e_flow_control_en;
+	if (GET_FIELD(params->modify_flags, QED_ROCE_MODIFY_QP_VALID_DEST_QP))
+		qp->dest_qp = params->dest_qp;
+	if (GET_FIELD(params->modify_flags,
+		      QED_ROCE_MODIFY_QP_VALID_ADDRESS_VECTOR)) {
+		/* Indicates that the following parameters have changed:
+		 * Traffic class, flow label, hop limit, source GID,
+		 * destination GID, loopback indicator
+		 */
+		qp->traffic_class_tos = params->traffic_class_tos;
+		qp->flow_label = params->flow_label;
+		qp->hop_limit_ttl = params->hop_limit_ttl;
+
+		qp->sgid = params->sgid;
+		qp->dgid = params->dgid;
+		qp->udp_src_port = 0;
+		qp->vlan_id = params->vlan_id;
+		qp->mtu = params->mtu;
+		qp->lb_indication = params->lb_indication;
+		memcpy((u8 *)&qp->remote_mac_addr[0],
+		       (u8 *)&params->remote_mac_addr[0], ETH_ALEN);
+		if (params->use_local_mac) {
+			memcpy((u8 *)&qp->local_mac_addr[0],
+			       (u8 *)&params->local_mac_addr[0], ETH_ALEN);
+		} else {
+			memcpy((u8 *)&qp->local_mac_addr[0],
+			       (u8 *)&p_hwfn->hw_info.hw_mac_addr, ETH_ALEN);
+		}
+	}
+	if (GET_FIELD(params->modify_flags, QED_ROCE_MODIFY_QP_VALID_RQ_PSN))
+		qp->rq_psn = params->rq_psn;
+	if (GET_FIELD(params->modify_flags, QED_ROCE_MODIFY_QP_VALID_SQ_PSN))
+		qp->sq_psn = params->sq_psn;
+	if (GET_FIELD(params->modify_flags,
+		      QED_RDMA_MODIFY_QP_VALID_MAX_RD_ATOMIC_REQ))
+		qp->max_rd_atomic_req = params->max_rd_atomic_req;
+	if (GET_FIELD(params->modify_flags,
+		      QED_RDMA_MODIFY_QP_VALID_MAX_RD_ATOMIC_RESP))
+		qp->max_rd_atomic_resp = params->max_rd_atomic_resp;
+	if (GET_FIELD(params->modify_flags,
+		      QED_ROCE_MODIFY_QP_VALID_ACK_TIMEOUT))
+		qp->ack_timeout = params->ack_timeout;
+	if (GET_FIELD(params->modify_flags, QED_ROCE_MODIFY_QP_VALID_RETRY_CNT))
+		qp->retry_cnt = params->retry_cnt;
+	if (GET_FIELD(params->modify_flags,
+		      QED_ROCE_MODIFY_QP_VALID_RNR_RETRY_CNT))
+		qp->rnr_retry_cnt = params->rnr_retry_cnt;
+	if (GET_FIELD(params->modify_flags,
+		      QED_ROCE_MODIFY_QP_VALID_MIN_RNR_NAK_TIMER))
+		qp->min_rnr_nak_timer = params->min_rnr_nak_timer;
+
+	qp->sqd_async = params->sqd_async;
+
+	prev_state = qp->cur_state;
+	if (GET_FIELD(params->modify_flags,
+		      QED_RDMA_MODIFY_QP_VALID_NEW_STATE)) {
+		qp->cur_state = params->new_state;
+		DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "qp->cur_state=%d\n",
+			   qp->cur_state);
+	}
+
+	rc = qed_roce_modify_qp(p_hwfn, qp, prev_state, params);
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Modify QP, rc = %d\n", rc);
+	return rc;
+}
+
+int qed_rdma_register_tid(void *rdma_cxt,
+			  struct qed_rdma_register_tid_in_params *params)
+{
+	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
+	struct rdma_register_tid_ramrod_data *p_ramrod;
+	struct qed_sp_init_data init_data;
+	struct qed_spq_entry *p_ent;
+	enum rdma_tid_type tid_type;
+	u8 fw_return_code;
+	int rc;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "itid = %08x\n", params->itid);
+
+	/* Get SPQ entry */
+	memset(&init_data, 0, sizeof(init_data));
+	init_data.opaque_fid = p_hwfn->hw_info.opaque_fid;
+	init_data.comp_mode = QED_SPQ_MODE_EBLOCK;
+
+	rc = qed_sp_init_request(p_hwfn, &p_ent, RDMA_RAMROD_REGISTER_MR,
+				 p_hwfn->p_rdma_info->proto, &init_data);
+	if (rc) {
+		DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "rc = %d\n", rc);
+		return rc;
+	}
+
+	if (p_hwfn->p_rdma_info->last_tid < params->itid)
+		p_hwfn->p_rdma_info->last_tid = params->itid;
+
+	p_ramrod = &p_ent->ramrod.rdma_register_tid;
+
+	p_ramrod->flags = 0;
+	SET_FIELD(p_ramrod->flags,
+		  RDMA_REGISTER_TID_RAMROD_DATA_TWO_LEVEL_PBL,
+		  params->pbl_two_level);
+
+	SET_FIELD(p_ramrod->flags,
+		  RDMA_REGISTER_TID_RAMROD_DATA_ZERO_BASED, params->zbva);
+
+	SET_FIELD(p_ramrod->flags,
+		  RDMA_REGISTER_TID_RAMROD_DATA_PHY_MR, params->phy_mr);
+
+	/* Don't initialize D/C field, as it may override other bits. */
+	if (!(params->tid_type == QED_RDMA_TID_FMR) && !(params->dma_mr))
+		SET_FIELD(p_ramrod->flags,
+			  RDMA_REGISTER_TID_RAMROD_DATA_PAGE_SIZE_LOG,
+			  params->page_size_log - 12);
+
+	SET_FIELD(p_ramrod->flags,
+		  RDMA_REGISTER_TID_RAMROD_DATA_MAX_ID,
+		  p_hwfn->p_rdma_info->last_tid);
+
+	SET_FIELD(p_ramrod->flags,
+		  RDMA_REGISTER_TID_RAMROD_DATA_REMOTE_READ,
+		  params->remote_read);
+
+	SET_FIELD(p_ramrod->flags,
+		  RDMA_REGISTER_TID_RAMROD_DATA_REMOTE_WRITE,
+		  params->remote_write);
+
+	SET_FIELD(p_ramrod->flags,
+		  RDMA_REGISTER_TID_RAMROD_DATA_REMOTE_ATOMIC,
+		  params->remote_atomic);
+
+	SET_FIELD(p_ramrod->flags,
+		  RDMA_REGISTER_TID_RAMROD_DATA_LOCAL_WRITE,
+		  params->local_write);
+
+	SET_FIELD(p_ramrod->flags,
+		  RDMA_REGISTER_TID_RAMROD_DATA_LOCAL_READ, params->local_read);
+
+	SET_FIELD(p_ramrod->flags,
+		  RDMA_REGISTER_TID_RAMROD_DATA_ENABLE_MW_BIND,
+		  params->mw_bind);
+
+	SET_FIELD(p_ramrod->flags1,
+		  RDMA_REGISTER_TID_RAMROD_DATA_PBL_PAGE_SIZE_LOG,
+		  params->pbl_page_size_log - 12);
+
+	SET_FIELD(p_ramrod->flags2,
+		  RDMA_REGISTER_TID_RAMROD_DATA_DMA_MR, params->dma_mr);
+
+	switch (params->tid_type) {
+	case QED_RDMA_TID_REGISTERED_MR:
+		tid_type = RDMA_TID_REGISTERED_MR;
+		break;
+	case QED_RDMA_TID_FMR:
+		tid_type = RDMA_TID_FMR;
+		break;
+	case QED_RDMA_TID_MW_TYPE1:
+		tid_type = RDMA_TID_MW_TYPE1;
+		break;
+	case QED_RDMA_TID_MW_TYPE2A:
+		tid_type = RDMA_TID_MW_TYPE2A;
+		break;
+	default:
+		rc = -EINVAL;
+		DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "rc = %d\n", rc);
+		return rc;
+	}
+	SET_FIELD(p_ramrod->flags1,
+		  RDMA_REGISTER_TID_RAMROD_DATA_TID_TYPE, tid_type);
+
+	p_ramrod->itid = cpu_to_le32(params->itid);
+	p_ramrod->key = params->key;
+	p_ramrod->pd = cpu_to_le16(params->pd);
+	p_ramrod->length_hi = (u8)(params->length >> 32);
+	p_ramrod->length_lo = DMA_LO_LE(params->length);
+	if (params->zbva) {
+		/* Lower 32 bits of the registered MR address.
+		 * In case of zero based MR, will hold FBO
+		 */
+		p_ramrod->va.hi = 0;
+		p_ramrod->va.lo = cpu_to_le32(params->fbo);
+	} else {
+		DMA_REGPAIR_LE(p_ramrod->va, params->vaddr);
+	}
+	DMA_REGPAIR_LE(p_ramrod->pbl_base, params->pbl_ptr);
+
+	/* DIF */
+	if (params->dif_enabled) {
+		SET_FIELD(p_ramrod->flags2,
+			  RDMA_REGISTER_TID_RAMROD_DATA_DIF_ON_HOST_FLG, 1);
+		DMA_REGPAIR_LE(p_ramrod->dif_error_addr,
+			       params->dif_error_addr);
+		DMA_REGPAIR_LE(p_ramrod->dif_runt_addr, params->dif_runt_addr);
+	}
+
+	rc = qed_spq_post(p_hwfn, p_ent, &fw_return_code);
+
+	if (fw_return_code != RDMA_RETURN_OK) {
+		DP_NOTICE(p_hwfn, "fw_return_code = %d\n", fw_return_code);
+		return -EINVAL;
+	}
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Register TID, rc = %d\n", rc);
+	return rc;
+}
+
+int qed_rdma_deregister_tid(void *rdma_cxt, u32 itid)
+{
+	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
+	struct rdma_deregister_tid_ramrod_data *p_ramrod;
+	struct qed_sp_init_data init_data;
+	struct qed_spq_entry *p_ent;
+	struct qed_ptt *p_ptt;
+	u8 fw_return_code;
+	int rc;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "itid = %08x\n", itid);
+
+	/* Get SPQ entry */
+	memset(&init_data, 0, sizeof(init_data));
+	init_data.opaque_fid = p_hwfn->hw_info.opaque_fid;
+	init_data.comp_mode = QED_SPQ_MODE_EBLOCK;
+
+	rc = qed_sp_init_request(p_hwfn, &p_ent, RDMA_RAMROD_DEREGISTER_MR,
+				 p_hwfn->p_rdma_info->proto, &init_data);
+	if (rc) {
+		DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "rc = %d\n", rc);
+		return rc;
+	}
+
+	p_ramrod = &p_ent->ramrod.rdma_deregister_tid;
+	p_ramrod->itid = cpu_to_le32(itid);
+
+	rc = qed_spq_post(p_hwfn, p_ent, &fw_return_code);
+	if (rc) {
+		DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "rc = %d\n", rc);
+		return rc;
+	}
+
+	if (fw_return_code == RDMA_RETURN_DEREGISTER_MR_BAD_STATE_ERR) {
+		DP_NOTICE(p_hwfn, "fw_return_code = %d\n", fw_return_code);
+		return -EINVAL;
+	} else if (fw_return_code == RDMA_RETURN_NIG_DRAIN_REQ) {
+		/* Bit indicating that the TID is in use and a nig drain is
+		 * required before sending the ramrod again
+		 */
+		p_ptt = qed_ptt_acquire(p_hwfn);
+		if (!p_ptt) {
+			rc = -EBUSY;
+			DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
+				   "Failed to acquire PTT\n");
+			return rc;
+		}
+
+		rc = qed_mcp_drain(p_hwfn, p_ptt);
+		if (rc) {
+			qed_ptt_release(p_hwfn, p_ptt);
+			DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
+				   "Drain failed\n");
+			return rc;
+		}
+
+		qed_ptt_release(p_hwfn, p_ptt);
+
+		/* Resend the ramrod */
+		rc = qed_sp_init_request(p_hwfn, &p_ent,
+					 RDMA_RAMROD_DEREGISTER_MR,
+					 p_hwfn->p_rdma_info->proto,
+					 &init_data);
+		if (rc) {
+			DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
+				   "Failed to init sp-element\n");
+			return rc;
+		}
+
+		rc = qed_spq_post(p_hwfn, p_ent, &fw_return_code);
+		if (rc) {
+			DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
+				   "Ramrod failed\n");
+			return rc;
+		}
+
+		if (fw_return_code != RDMA_RETURN_OK) {
+			DP_NOTICE(p_hwfn, "fw_return_code = %d\n",
+				  fw_return_code);
+			return rc;
+		}
+	}
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "De-registered TID, rc = %d\n", rc);
+	return rc;
+}
+
+static void *qed_rdma_get_rdma_ctx(struct qed_dev *cdev)
+{
+	return QED_LEADING_HWFN(cdev);
+}
+
+static void qed_rdma_dpm_conf(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
+{
+	u32 val;
+
+	val = (p_hwfn->dcbx_no_edpm || p_hwfn->db_bar_no_edpm) ? 0 : 1;
+
+	qed_wr(p_hwfn, p_ptt, DORQ_REG_PF_DPM_ENABLE, val);
+	DP_VERBOSE(p_hwfn, (QED_MSG_DCB | QED_MSG_RDMA),
+		   "Changing DPM_EN state to %d (DCBX=%d, DB_BAR=%d)\n",
+		   val, p_hwfn->dcbx_no_edpm, p_hwfn->db_bar_no_edpm);
+}
+
+void qed_rdma_dpm_bar(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
+{
+	p_hwfn->db_bar_no_edpm = true;
+
+	qed_rdma_dpm_conf(p_hwfn, p_ptt);
+}
+
+int qed_rdma_start(void *rdma_cxt, struct qed_rdma_start_in_params *params)
+{
+	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
+	struct qed_ptt *p_ptt;
+	int rc = -EBUSY;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
+		   "desired_cnq = %08x\n", params->desired_cnq);
+
+	p_ptt = qed_ptt_acquire(p_hwfn);
+	if (!p_ptt)
+		goto err;
+
+	rc = qed_rdma_alloc(p_hwfn, p_ptt, params);
+	if (rc)
+		goto err1;
+
+	rc = qed_rdma_setup(p_hwfn, p_ptt, params);
+	if (rc)
+		goto err2;
+
+	qed_ptt_release(p_hwfn, p_ptt);
+
+	return rc;
+
+err2:
+	qed_rdma_free(p_hwfn);
+err1:
+	qed_ptt_release(p_hwfn, p_ptt);
+err:
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "RDMA start - error, rc = %d\n", rc);
+	return rc;
+}
+
+static int qed_rdma_init(struct qed_dev *cdev,
+			 struct qed_rdma_start_in_params *params)
+{
+	return qed_rdma_start(QED_LEADING_HWFN(cdev), params);
+}
+
+void qed_rdma_remove_user(void *rdma_cxt, u16 dpi)
+{
+	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "dpi = %08x\n", dpi);
+
+	spin_lock_bh(&p_hwfn->p_rdma_info->lock);
+	qed_bmap_release_id(p_hwfn, &p_hwfn->p_rdma_info->dpi_map, dpi);
+	spin_unlock_bh(&p_hwfn->p_rdma_info->lock);
+}
+
+void qed_ll2b_complete_tx_gsi_packet(struct qed_hwfn *p_hwfn,
+				     u8 connection_handle,
+				     void *cookie,
+				     dma_addr_t first_frag_addr,
+				     bool b_last_fragment, bool b_last_packet)
+{
+	struct qed_roce_ll2_packet *packet = cookie;
+	struct qed_roce_ll2_info *roce_ll2 = p_hwfn->ll2;
+
+	roce_ll2->cbs.tx_cb(roce_ll2->cb_cookie, packet);
+}
+
+void qed_ll2b_release_tx_gsi_packet(struct qed_hwfn *p_hwfn,
+				    u8 connection_handle,
+				    void *cookie,
+				    dma_addr_t first_frag_addr,
+				    bool b_last_fragment, bool b_last_packet)
+{
+	qed_ll2b_complete_tx_gsi_packet(p_hwfn, connection_handle,
+					cookie, first_frag_addr,
+					b_last_fragment, b_last_packet);
+}
+
+void qed_ll2b_complete_rx_gsi_packet(struct qed_hwfn *p_hwfn,
+				     u8 connection_handle,
+				     void *cookie,
+				     dma_addr_t rx_buf_addr,
+				     u16 data_length,
+				     u8 data_length_error,
+				     u16 parse_flags,
+				     u16 vlan,
+				     u32 src_mac_addr_hi,
+				     u16 src_mac_addr_lo, bool b_last_packet)
+{
+	struct qed_roce_ll2_info *roce_ll2 = p_hwfn->ll2;
+	struct qed_roce_ll2_rx_params params;
+	struct qed_dev *cdev = p_hwfn->cdev;
+	struct qed_roce_ll2_packet pkt;
+
+	DP_VERBOSE(cdev,
+		   QED_MSG_LL2,
+		   "roce ll2 rx complete: bus_addr=%p, len=%d, data_len_err=%d\n",
+		   (void *)(uintptr_t)rx_buf_addr,
+		   data_length, data_length_error);
+
+	memset(&pkt, 0, sizeof(pkt));
+	pkt.n_seg = 1;
+	pkt.payload[0].baddr = rx_buf_addr;
+	pkt.payload[0].len = data_length;
+
+	memset(&params, 0, sizeof(params));
+	params.vlan_id = vlan;
+	*((u32 *)&params.smac[0]) = ntohl(src_mac_addr_hi);
+	*((u16 *)&params.smac[4]) = ntohs(src_mac_addr_lo);
+
+	if (data_length_error) {
+		DP_ERR(cdev,
+		       "roce ll2 rx complete: data length error %d, length=%d\n",
+		       data_length_error, data_length);
+		params.rc = -EINVAL;
+	}
+
+	roce_ll2->cbs.rx_cb(roce_ll2->cb_cookie, &pkt, &params);
+}
+
+static int qed_roce_ll2_set_mac_filter(struct qed_dev *cdev,
+				       u8 *old_mac_address,
+				       u8 *new_mac_address)
+{
+	struct qed_hwfn *hwfn = QED_LEADING_HWFN(cdev);
+	struct qed_ptt *p_ptt;
+	int rc = 0;
+
+	if (!hwfn->ll2 || hwfn->ll2->handle == QED_LL2_UNUSED_HANDLE) {
+		DP_ERR(cdev,
+		       "qed roce mac filter failed - roce_info/ll2 NULL\n");
+		return -EINVAL;
+	}
+
+	p_ptt = qed_ptt_acquire(QED_LEADING_HWFN(cdev));
+	if (!p_ptt) {
+		DP_ERR(cdev,
+		       "qed roce ll2 mac filter set: failed to acquire PTT\n");
+		return -EINVAL;
+	}
+
+	mutex_lock(&hwfn->ll2->lock);
+	if (old_mac_address)
+		qed_llh_remove_mac_filter(QED_LEADING_HWFN(cdev), p_ptt,
+					  old_mac_address);
+	if (new_mac_address)
+		rc = qed_llh_add_mac_filter(QED_LEADING_HWFN(cdev), p_ptt,
+					    new_mac_address);
+	mutex_unlock(&hwfn->ll2->lock);
+
+	qed_ptt_release(QED_LEADING_HWFN(cdev), p_ptt);
+
+	if (rc)
+		DP_ERR(cdev,
+		       "qed roce ll2 mac filter set: failed to add mac filter\n");
+
+	return rc;
+}
+
+static int qed_roce_ll2_start(struct qed_dev *cdev,
+			      struct qed_roce_ll2_params *params)
+{
+	struct qed_hwfn *hwfn = QED_LEADING_HWFN(cdev);
+	struct qed_roce_ll2_info *roce_ll2;
+	struct qed_ll2_info ll2_params;
+	int rc;
+
+	if (!params) {
+		DP_ERR(cdev, "qed roce ll2 start: failed due to NULL params\n");
+		return -EINVAL;
+	}
+	if (!params->cbs.tx_cb || !params->cbs.rx_cb) {
+		DP_ERR(cdev,
+		       "qed roce ll2 start: failed due to NULL tx/rx. tx_cb=%p, rx_cb=%p\n",
+		       params->cbs.tx_cb, params->cbs.rx_cb);
+		return -EINVAL;
+	}
+	if (!is_valid_ether_addr(params->mac_address)) {
+		DP_ERR(cdev,
+		       "qed roce ll2 start: failed due to invalid Ethernet address %pM\n",
+		       params->mac_address);
+		return -EINVAL;
+	}
+
+	/* Initialize */
+	roce_ll2 = kzalloc(sizeof(*roce_ll2), GFP_ATOMIC);
+	if (!roce_ll2) {
+		DP_ERR(cdev, "qed roce ll2 start: failed memory allocation\n");
+		return -ENOMEM;
+	}
+	memset(roce_ll2, 0, sizeof(*roce_ll2));
+	roce_ll2->handle = QED_LL2_UNUSED_HANDLE;
+	roce_ll2->cbs = params->cbs;
+	roce_ll2->cb_cookie = params->cb_cookie;
+	mutex_init(&roce_ll2->lock);
+
+	memset(&ll2_params, 0, sizeof(ll2_params));
+	ll2_params.conn_type = QED_LL2_TYPE_ROCE;
+	ll2_params.mtu = params->mtu;
+	ll2_params.rx_drop_ttl0_flg = true;
+	ll2_params.rx_vlan_removal_en = false;
+	ll2_params.tx_dest = CORE_TX_DEST_NW;
+	ll2_params.ai_err_packet_too_big = LL2_DROP_PACKET;
+	ll2_params.ai_err_no_buf = LL2_DROP_PACKET;
+	ll2_params.gsi_enable = true;
+
+	rc = qed_ll2_acquire_connection(QED_LEADING_HWFN(cdev), &ll2_params,
+					params->max_rx_buffers,
+					params->max_tx_buffers,
+					&roce_ll2->handle);
+	if (rc) {
+		DP_ERR(cdev,
+		       "qed roce ll2 start: failed to acquire LL2 connection (rc=%d)\n",
+		       rc);
+		goto err;
+	}
+
+	rc = qed_ll2_establish_connection(QED_LEADING_HWFN(cdev),
+					  roce_ll2->handle);
+	if (rc) {
+		DP_ERR(cdev,
+		       "qed roce ll2 start: failed to establish LL2 connection (rc=%d)\n",
+		       rc);
+		goto err1;
+	}
+
+	hwfn->ll2 = roce_ll2;
+
+	rc = qed_roce_ll2_set_mac_filter(cdev, NULL, params->mac_address);
+	if (rc) {
+		hwfn->ll2 = NULL;
+		goto err2;
+	}
+	ether_addr_copy(roce_ll2->mac_address, params->mac_address);
+
+	return 0;
+
+err2:
+	qed_ll2_terminate_connection(QED_LEADING_HWFN(cdev), roce_ll2->handle);
+err1:
+	qed_ll2_release_connection(QED_LEADING_HWFN(cdev), roce_ll2->handle);
+err:
+	kfree(roce_ll2);
+	return rc;
+}
+
+static int qed_roce_ll2_stop(struct qed_dev *cdev)
+{
+	struct qed_hwfn *hwfn = QED_LEADING_HWFN(cdev);
+	struct qed_roce_ll2_info *roce_ll2 = hwfn->ll2;
+	int rc;
+
+	if (!cdev) {
+		DP_ERR(cdev, "qed roce ll2 stop: invalid cdev\n");
+		return -EINVAL;
+	}
+
+	if (roce_ll2->handle == QED_LL2_UNUSED_HANDLE) {
+		DP_ERR(cdev, "qed roce ll2 stop: cannot stop an unused LL2\n");
+		return -EINVAL;
+	}
+
+	/* remove LL2 MAC address filter */
+	rc = qed_roce_ll2_set_mac_filter(cdev, roce_ll2->mac_address, NULL);
+	eth_zero_addr(roce_ll2->mac_address);
+
+	rc = qed_ll2_terminate_connection(QED_LEADING_HWFN(cdev),
+					  roce_ll2->handle);
+	if (rc)
+		DP_ERR(cdev,
+		       "qed roce ll2 stop: failed to terminate LL2 connection (rc=%d)\n",
+		       rc);
+
+	qed_ll2_release_connection(QED_LEADING_HWFN(cdev), roce_ll2->handle);
+
+	roce_ll2->handle = QED_LL2_UNUSED_HANDLE;
+
+	kfree(roce_ll2);
+
+	return rc;
+}
+
+static int qed_roce_ll2_tx(struct qed_dev *cdev,
+			   struct qed_roce_ll2_packet *pkt,
+			   struct qed_roce_ll2_tx_params *params)
+{
+	struct qed_hwfn *hwfn = QED_LEADING_HWFN(cdev);
+	struct qed_roce_ll2_info *roce_ll2 = hwfn->ll2;
+	enum qed_ll2_roce_flavor_type qed_roce_flavor;
+	u8 flags = 0;
+	int rc;
+	int i;
+
+	if (!cdev || !pkt || !params) {
+		DP_ERR(cdev,
+		       "roce ll2 tx: failed tx because one of the following is NULL - drv=%p, pkt=%p, params=%p\n",
+		       cdev, pkt, params);
+		return -EINVAL;
+	}
+
+	qed_roce_flavor = (pkt->roce_mode == ROCE_V1) ? QED_LL2_ROCE
+						      : QED_LL2_RROCE;
+
+	if (pkt->roce_mode == ROCE_V2_IPV4)
+		flags |= BIT(CORE_TX_BD_FLAGS_IP_CSUM_SHIFT);
+
+	/* Tx header */
+	rc = qed_ll2_prepare_tx_packet(QED_LEADING_HWFN(cdev), roce_ll2->handle,
+				       1 + pkt->n_seg, 0, flags, 0,
+				       qed_roce_flavor, pkt->header.baddr,
+				       pkt->header.len, pkt, 1);
+	if (rc) {
+		DP_ERR(cdev, "roce ll2 tx: header failed (rc=%d)\n", rc);
+		return QED_ROCE_TX_HEAD_FAILURE;
+	}
+
+	/* Tx payload */
+	for (i = 0; i < pkt->n_seg; i++) {
+		rc = qed_ll2_set_fragment_of_tx_packet(QED_LEADING_HWFN(cdev),
+						       roce_ll2->handle,
+						       pkt->payload[i].baddr,
+						       pkt->payload[i].len);
+		if (rc) {
+			/* If failed not much to do here, partial packet has
+			 * been posted * we can't free memory, will need to wait
+			 * for completion
+			 */
+			DP_ERR(cdev,
+			       "roce ll2 tx: payload failed (rc=%d)\n", rc);
+			return QED_ROCE_TX_FRAG_FAILURE;
+		}
+	}
+
+	return 0;
+}
+
+static int qed_roce_ll2_post_rx_buffer(struct qed_dev *cdev,
+				       struct qed_roce_ll2_buffer *buf,
+				       u64 cookie, u8 notify_fw)
+{
+	return qed_ll2_post_rx_buffer(QED_LEADING_HWFN(cdev),
+				      QED_LEADING_HWFN(cdev)->ll2->handle,
+				      buf->baddr, buf->len,
+				      (void *)(uintptr_t)cookie, notify_fw);
+}
+
+static int qed_roce_ll2_stats(struct qed_dev *cdev, struct qed_ll2_stats *stats)
+{
+	struct qed_hwfn *hwfn = QED_LEADING_HWFN(cdev);
+	struct qed_roce_ll2_info *roce_ll2 = hwfn->ll2;
+
+	return qed_ll2_get_stats(QED_LEADING_HWFN(cdev),
+				 roce_ll2->handle, stats);
+}
+
+static const struct qed_rdma_ops qed_rdma_ops_pass = {
+	.common = &qed_common_ops_pass,
+	.fill_dev_info = &qed_fill_rdma_dev_info,
+	.rdma_get_rdma_ctx = &qed_rdma_get_rdma_ctx,
+	.rdma_init = &qed_rdma_init,
+	.rdma_add_user = &qed_rdma_add_user,
+	.rdma_remove_user = &qed_rdma_remove_user,
+	.rdma_stop = &qed_rdma_stop,
+	.rdma_query_port = &qed_rdma_query_port,
+	.rdma_query_device = &qed_rdma_query_device,
+	.rdma_get_start_sb = &qed_rdma_get_sb_start,
+	.rdma_get_rdma_int = &qed_rdma_get_int,
+	.rdma_set_rdma_int = &qed_rdma_set_int,
+	.rdma_get_min_cnq_msix = &qed_rdma_get_min_cnq_msix,
+	.rdma_cnq_prod_update = &qed_rdma_cnq_prod_update,
+	.rdma_alloc_pd = &qed_rdma_alloc_pd,
+	.rdma_dealloc_pd = &qed_rdma_free_pd,
+	.rdma_create_cq = &qed_rdma_create_cq,
+	.rdma_destroy_cq = &qed_rdma_destroy_cq,
+	.rdma_create_qp = &qed_rdma_create_qp,
+	.rdma_modify_qp = &qed_rdma_modify_qp,
+	.rdma_query_qp = &qed_rdma_query_qp,
+	.rdma_destroy_qp = &qed_rdma_destroy_qp,
+	.rdma_alloc_tid = &qed_rdma_alloc_tid,
+	.rdma_free_tid = &qed_rdma_free_tid,
+	.rdma_register_tid = &qed_rdma_register_tid,
+	.rdma_deregister_tid = &qed_rdma_deregister_tid,
+	.roce_ll2_start = &qed_roce_ll2_start,
+	.roce_ll2_stop = &qed_roce_ll2_stop,
+	.roce_ll2_tx = &qed_roce_ll2_tx,
+	.roce_ll2_post_rx_buffer = &qed_roce_ll2_post_rx_buffer,
+	.roce_ll2_set_mac_filter = &qed_roce_ll2_set_mac_filter,
+	.roce_ll2_stats = &qed_roce_ll2_stats,
+};
+
+const struct qed_rdma_ops *qed_get_rdma_ops(void)
+{
+	return &qed_rdma_ops_pass;
+}
+EXPORT_SYMBOL(qed_get_rdma_ops);
diff --git a/drivers/net/ethernet/qlogic/qed/qed_roce.h b/drivers/net/ethernet/qlogic/qed/qed_roce.h
new file mode 100644
index 000000000000..2f091e8a0f40
--- /dev/null
+++ b/drivers/net/ethernet/qlogic/qed/qed_roce.h
@@ -0,0 +1,216 @@
+/* QLogic qed NIC Driver
+ * Copyright (c) 2015-2016  QLogic Corporation
+ *
+ * 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 _QED_ROCE_H
+#define _QED_ROCE_H
+#include <linux/types.h>
+#include <linux/bitops.h>
+#include <linux/kernel.h>
+#include <linux/list.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/qed/qed_if.h>
+#include <linux/qed/qed_roce_if.h>
+#include "qed.h"
+#include "qed_dev_api.h"
+#include "qed_hsi.h"
+#include "qed_ll2.h"
+
+#define QED_RDMA_MAX_FMR                    (RDMA_MAX_TIDS)
+#define QED_RDMA_MAX_P_KEY                  (1)
+#define QED_RDMA_MAX_WQE                    (0x7FFF)
+#define QED_RDMA_MAX_SRQ_WQE_ELEM           (0x7FFF)
+#define QED_RDMA_PAGE_SIZE_CAPS             (0xFFFFF000)
+#define QED_RDMA_ACK_DELAY                  (15)
+#define QED_RDMA_MAX_MR_SIZE                (0x10000000000ULL)
+#define QED_RDMA_MAX_CQS                    (RDMA_MAX_CQS)
+#define QED_RDMA_MAX_MRS                    (RDMA_MAX_TIDS)
+/* Add 1 for header element */
+#define QED_RDMA_MAX_SRQ_ELEM_PER_WQE	    (RDMA_MAX_SGE_PER_RQ_WQE + 1)
+#define QED_RDMA_MAX_SGE_PER_SRQ_WQE        (RDMA_MAX_SGE_PER_RQ_WQE)
+#define QED_RDMA_SRQ_WQE_ELEM_SIZE          (16)
+#define QED_RDMA_MAX_SRQS                   (32 * 1024)
+
+#define QED_RDMA_MAX_CQE_32_BIT             (0x7FFFFFFF - 1)
+#define QED_RDMA_MAX_CQE_16_BIT             (0x7FFF - 1)
+
+enum qed_rdma_toggle_bit {
+	QED_RDMA_TOGGLE_BIT_CLEAR = 0,
+	QED_RDMA_TOGGLE_BIT_SET = 1
+};
+
+struct qed_bmap {
+	unsigned long *bitmap;
+	u32 max_count;
+};
+
+struct qed_rdma_info {
+	/* spin lock to protect bitmaps */
+	spinlock_t lock;
+
+	struct qed_bmap cq_map;
+	struct qed_bmap pd_map;
+	struct qed_bmap tid_map;
+	struct qed_bmap qp_map;
+	struct qed_bmap srq_map;
+	struct qed_bmap cid_map;
+	struct qed_bmap dpi_map;
+	struct qed_bmap toggle_bits;
+	struct qed_rdma_events events;
+	struct qed_rdma_device *dev;
+	struct qed_rdma_port *port;
+	u32 last_tid;
+	u8 num_cnqs;
+	u32 num_qps;
+	u32 num_mrs;
+	u16 queue_zone_base;
+	enum protocol_type proto;
+};
+
+struct qed_rdma_resize_cq_in_params {
+	u16 icid;
+	u32 cq_size;
+	bool pbl_two_level;
+	u64 pbl_ptr;
+	u16 pbl_num_pages;
+	u8 pbl_page_size_log;
+};
+
+struct qed_rdma_resize_cq_out_params {
+	u32 prod;
+	u32 cons;
+};
+
+struct qed_rdma_resize_cnq_in_params {
+	u32 cnq_id;
+	u32 pbl_page_size_log;
+	u64 pbl_ptr;
+};
+
+struct qed_rdma_qp {
+	struct regpair qp_handle;
+	struct regpair qp_handle_async;
+	u32 qpid;
+	u16 icid;
+	enum qed_roce_qp_state cur_state;
+	bool use_srq;
+	bool signal_all;
+	bool fmr_and_reserved_lkey;
+
+	bool incoming_rdma_read_en;
+	bool incoming_rdma_write_en;
+	bool incoming_atomic_en;
+	bool e2e_flow_control_en;
+
+	u16 pd;
+	u16 pkey;
+	u32 dest_qp;
+	u16 mtu;
+	u16 srq_id;
+	u8 traffic_class_tos;
+	u8 hop_limit_ttl;
+	u16 dpi;
+	u32 flow_label;
+	bool lb_indication;
+	u16 vlan_id;
+	u32 ack_timeout;
+	u8 retry_cnt;
+	u8 rnr_retry_cnt;
+	u8 min_rnr_nak_timer;
+	bool sqd_async;
+	union qed_gid sgid;
+	union qed_gid dgid;
+	enum roce_mode roce_mode;
+	u16 udp_src_port;
+	u8 stats_queue;
+
+	/* requeseter */
+	u8 max_rd_atomic_req;
+	u32 sq_psn;
+	u16 sq_cq_id;
+	u16 sq_num_pages;
+	dma_addr_t sq_pbl_ptr;
+	void *orq;
+	dma_addr_t orq_phys_addr;
+	u8 orq_num_pages;
+	bool req_offloaded;
+
+	/* responder */
+	u8 max_rd_atomic_resp;
+	u32 rq_psn;
+	u16 rq_cq_id;
+	u16 rq_num_pages;
+	dma_addr_t rq_pbl_ptr;
+	void *irq;
+	dma_addr_t irq_phys_addr;
+	u8 irq_num_pages;
+	bool resp_offloaded;
+
+	u8 remote_mac_addr[6];
+	u8 local_mac_addr[6];
+
+	void *shared_queue;
+	dma_addr_t shared_queue_phys_addr;
+};
+
+int
+qed_rdma_add_user(void *rdma_cxt,
+		  struct qed_rdma_add_user_out_params *out_params);
+int qed_rdma_alloc_pd(void *rdma_cxt, u16 *pd);
+int qed_rdma_alloc_tid(void *rdma_cxt, u32 *tid);
+int qed_rdma_deregister_tid(void *rdma_cxt, u32 tid);
+void qed_rdma_free_tid(void *rdma_cxt, u32 tid);
+struct qed_rdma_device *qed_rdma_query_device(void *rdma_cxt);
+struct qed_rdma_port *qed_rdma_query_port(void *rdma_cxt);
+int
+qed_rdma_register_tid(void *rdma_cxt,
+		      struct qed_rdma_register_tid_in_params *params);
+void qed_rdma_remove_user(void *rdma_cxt, u16 dpi);
+int qed_rdma_start(void *p_hwfn, struct qed_rdma_start_in_params *params);
+int qed_rdma_stop(void *rdma_cxt);
+u32 qed_rdma_get_sb_id(void *p_hwfn, u32 rel_sb_id);
+u32 qed_rdma_query_cau_timer_res(void *p_hwfn);
+void qed_rdma_cnq_prod_update(void *rdma_cxt, u8 cnq_index, u16 prod);
+void qed_rdma_resc_free(struct qed_hwfn *p_hwfn);
+void qed_async_roce_event(struct qed_hwfn *p_hwfn,
+			  struct event_ring_entry *p_eqe);
+int qed_rdma_destroy_qp(void *rdma_cxt, struct qed_rdma_qp *qp);
+int qed_rdma_modify_qp(void *rdma_cxt, struct qed_rdma_qp *qp,
+		       struct qed_rdma_modify_qp_in_params *params);
+int qed_rdma_query_qp(void *rdma_cxt, struct qed_rdma_qp *qp,
+		      struct qed_rdma_query_qp_out_params *out_params);
+
+#if IS_ENABLED(CONFIG_INFINIBAND_QEDR)
+void qed_rdma_dpm_bar(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt);
+#else
+void qed_rdma_dpm_bar(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt) {}
+#endif
+#endif
diff --git a/drivers/net/ethernet/qlogic/qed/qed_selftest.c b/drivers/net/ethernet/qlogic/qed/qed_selftest.c
index a342bfe4280d..9b7678f26909 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_selftest.c
+++ b/drivers/net/ethernet/qlogic/qed/qed_selftest.c
@@ -2,6 +2,7 @@
 #include "qed_dev_api.h"
 #include "qed_mcp.h"
 #include "qed_sp.h"
+#include "qed_selftest.h"
 
 int qed_selftest_memory(struct qed_dev *cdev)
 {
diff --git a/drivers/net/ethernet/qlogic/qed/qed_sp.h b/drivers/net/ethernet/qlogic/qed/qed_sp.h
index a548504c3420..652c90819758 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_sp.h
+++ b/drivers/net/ethernet/qlogic/qed/qed_sp.h
@@ -61,6 +61,10 @@ union ramrod_data {
 	struct vport_start_ramrod_data vport_start;
 	struct vport_stop_ramrod_data vport_stop;
 	struct vport_update_ramrod_data vport_update;
+	struct core_rx_start_ramrod_data core_rx_queue_start;
+	struct core_rx_stop_ramrod_data core_rx_queue_stop;
+	struct core_tx_start_ramrod_data core_tx_queue_start;
+	struct core_tx_stop_ramrod_data core_tx_queue_stop;
 	struct vport_filter_update_ramrod_data vport_filter_update;
 
 	struct rdma_init_func_ramrod_data rdma_init_func;
@@ -81,6 +85,7 @@ union ramrod_data {
 	struct rdma_srq_create_ramrod_data rdma_create_srq;
 	struct rdma_srq_destroy_ramrod_data rdma_destroy_srq;
 	struct rdma_srq_modify_ramrod_data rdma_modify_srq;
+	struct roce_init_func_ramrod_data roce_init_func;
 
 	struct iscsi_slow_path_hdr iscsi_empty;
 	struct iscsi_init_ramrod_params iscsi_init;
diff --git a/drivers/net/ethernet/qlogic/qed/qed_sp_commands.c b/drivers/net/ethernet/qlogic/qed/qed_sp_commands.c
index a52f3fc051f5..2888eb0628f8 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_sp_commands.c
+++ b/drivers/net/ethernet/qlogic/qed/qed_sp_commands.c
@@ -25,9 +25,7 @@
 
 int qed_sp_init_request(struct qed_hwfn *p_hwfn,
 			struct qed_spq_entry **pp_ent,
-			u8 cmd,
-			u8 protocol,
-			struct qed_sp_init_data *p_data)
+			u8 cmd, u8 protocol, struct qed_sp_init_data *p_data)
 {
 	u32 opaque_cid = p_data->opaque_fid << 16 | p_data->cid;
 	struct qed_spq_entry *p_ent = NULL;
@@ -38,7 +36,7 @@ int qed_sp_init_request(struct qed_hwfn *p_hwfn,
 
 	rc = qed_spq_get_entry(p_hwfn, pp_ent);
 
-	if (rc != 0)
+	if (rc)
 		return rc;
 
 	p_ent = *pp_ent;
@@ -321,8 +319,7 @@ int qed_sp_pf_start(struct qed_hwfn *p_hwfn,
 
 	rc = qed_sp_init_request(p_hwfn, &p_ent,
 				 COMMON_RAMROD_PF_START,
-				 PROTOCOLID_COMMON,
-				 &init_data);
+				 PROTOCOLID_COMMON, &init_data);
 	if (rc)
 		return rc;
 
@@ -356,8 +353,7 @@ int qed_sp_pf_start(struct qed_hwfn *p_hwfn,
 	DMA_REGPAIR_LE(p_ramrod->consolid_q_pbl_addr,
 		       p_hwfn->p_consq->chain.pbl.p_phys_table);
 
-	qed_tunn_set_pf_start_params(p_hwfn, p_tunn,
-				     &p_ramrod->tunnel_config);
+	qed_tunn_set_pf_start_params(p_hwfn, p_tunn, &p_ramrod->tunnel_config);
 
 	if (IS_MF_SI(p_hwfn))
 		p_ramrod->allow_npar_tx_switching = allow_npar_tx_switch;
@@ -389,8 +385,7 @@ int qed_sp_pf_start(struct qed_hwfn *p_hwfn,
 
 	DP_VERBOSE(p_hwfn, QED_MSG_SPQ,
 		   "Setting event_ring_sb [id %04x index %02x], outer_tag [%d]\n",
-		   sb, sb_index,
-		   p_ramrod->outer_tag);
+		   sb, sb_index, p_ramrod->outer_tag);
 
 	rc = qed_spq_post(p_hwfn, p_ent, NULL);
 
diff --git a/drivers/net/ethernet/qlogic/qed/qed_spq.c b/drivers/net/ethernet/qlogic/qed/qed_spq.c
index d73456eab1d7..caff41544898 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_spq.c
+++ b/drivers/net/ethernet/qlogic/qed/qed_spq.c
@@ -28,6 +28,9 @@
 #include "qed_reg_addr.h"
 #include "qed_sp.h"
 #include "qed_sriov.h"
+#if IS_ENABLED(CONFIG_INFINIBAND_QEDR)
+#include "qed_roce.h"
+#endif
 
 /***************************************************************************
 * Structures & Definitions
@@ -41,8 +44,7 @@
 ***************************************************************************/
 static void qed_spq_blocking_cb(struct qed_hwfn *p_hwfn,
 				void *cookie,
-				union event_ring_data *data,
-				u8 fw_return_code)
+				union event_ring_data *data, u8 fw_return_code)
 {
 	struct qed_spq_comp_done *comp_done;
 
@@ -109,9 +111,8 @@ static int qed_spq_block(struct qed_hwfn *p_hwfn,
 /***************************************************************************
 * SPQ entries inner API
 ***************************************************************************/
-static int
-qed_spq_fill_entry(struct qed_hwfn *p_hwfn,
-		   struct qed_spq_entry *p_ent)
+static int qed_spq_fill_entry(struct qed_hwfn *p_hwfn,
+			      struct qed_spq_entry *p_ent)
 {
 	p_ent->flags = 0;
 
@@ -189,8 +190,7 @@ static void qed_spq_hw_initialize(struct qed_hwfn *p_hwfn,
 }
 
 static int qed_spq_hw_post(struct qed_hwfn *p_hwfn,
-			   struct qed_spq *p_spq,
-			   struct qed_spq_entry *p_ent)
+			   struct qed_spq *p_spq, struct qed_spq_entry *p_ent)
 {
 	struct qed_chain *p_chain = &p_hwfn->p_spq->chain;
 	u16 echo = qed_chain_get_prod_idx(p_chain);
@@ -240,6 +240,11 @@ qed_async_event_completion(struct qed_hwfn *p_hwfn,
 			   struct event_ring_entry *p_eqe)
 {
 	switch (p_eqe->protocol_id) {
+#if IS_ENABLED(CONFIG_INFINIBAND_QEDR)
+	case PROTOCOLID_ROCE:
+		qed_async_roce_event(p_hwfn, p_eqe);
+		return 0;
+#endif
 	case PROTOCOLID_COMMON:
 		return qed_sriov_eqe_event(p_hwfn,
 					   p_eqe->opcode,
@@ -255,8 +260,7 @@ qed_async_event_completion(struct qed_hwfn *p_hwfn,
 /***************************************************************************
 * EQ API
 ***************************************************************************/
-void qed_eq_prod_update(struct qed_hwfn *p_hwfn,
-			u16 prod)
+void qed_eq_prod_update(struct qed_hwfn *p_hwfn, u16 prod)
 {
 	u32 addr = GTT_BAR0_MAP_REG_USDM_RAM +
 		   USTORM_EQE_CONS_OFFSET(p_hwfn->rel_pf_id);
@@ -267,9 +271,7 @@ void qed_eq_prod_update(struct qed_hwfn *p_hwfn,
 	mmiowb();
 }
 
-int qed_eq_completion(struct qed_hwfn *p_hwfn,
-		      void *cookie)
-
+int qed_eq_completion(struct qed_hwfn *p_hwfn, void *cookie)
 {
 	struct qed_eq *p_eq = cookie;
 	struct qed_chain *p_chain = &p_eq->chain;
@@ -323,17 +325,14 @@ int qed_eq_completion(struct qed_hwfn *p_hwfn,
 	return rc;
 }
 
-struct qed_eq *qed_eq_alloc(struct qed_hwfn *p_hwfn,
-			    u16 num_elem)
+struct qed_eq *qed_eq_alloc(struct qed_hwfn *p_hwfn, u16 num_elem)
 {
 	struct qed_eq *p_eq;
 
 	/* Allocate EQ struct */
 	p_eq = kzalloc(sizeof(*p_eq), GFP_KERNEL);
-	if (!p_eq) {
-		DP_NOTICE(p_hwfn, "Failed to allocate `struct qed_eq'\n");
+	if (!p_eq)
 		return NULL;
-	}
 
 	/* Allocate and initialize EQ chain*/
 	if (qed_chain_alloc(p_hwfn->cdev,
@@ -342,17 +341,12 @@ struct qed_eq *qed_eq_alloc(struct qed_hwfn *p_hwfn,
 			    QED_CHAIN_CNT_TYPE_U16,
 			    num_elem,
 			    sizeof(union event_ring_element),
-			    &p_eq->chain)) {
-		DP_NOTICE(p_hwfn, "Failed to allocate eq chain\n");
+			    &p_eq->chain))
 		goto eq_allocate_fail;
-	}
 
 	/* register EQ completion on the SP SB */
-	qed_int_register_cb(p_hwfn,
-			    qed_eq_completion,
-			    p_eq,
-			    &p_eq->eq_sb_index,
-			    &p_eq->p_fw_cons);
+	qed_int_register_cb(p_hwfn, qed_eq_completion,
+			    p_eq, &p_eq->eq_sb_index, &p_eq->p_fw_cons);
 
 	return p_eq;
 
@@ -361,14 +355,12 @@ eq_allocate_fail:
 	return NULL;
 }
 
-void qed_eq_setup(struct qed_hwfn *p_hwfn,
-		  struct qed_eq *p_eq)
+void qed_eq_setup(struct qed_hwfn *p_hwfn, struct qed_eq *p_eq)
 {
 	qed_chain_reset(&p_eq->chain);
 }
 
-void qed_eq_free(struct qed_hwfn *p_hwfn,
-		 struct qed_eq *p_eq)
+void qed_eq_free(struct qed_hwfn *p_hwfn, struct qed_eq *p_eq)
 {
 	if (!p_eq)
 		return;
@@ -379,10 +371,9 @@ void qed_eq_free(struct qed_hwfn *p_hwfn,
 /***************************************************************************
 * CQE API - manipulate EQ functionality
 ***************************************************************************/
-static int qed_cqe_completion(
-	struct qed_hwfn *p_hwfn,
-	struct eth_slow_path_rx_cqe *cqe,
-	enum protocol_type protocol)
+static int qed_cqe_completion(struct qed_hwfn *p_hwfn,
+			      struct eth_slow_path_rx_cqe *cqe,
+			      enum protocol_type protocol)
 {
 	if (IS_VF(p_hwfn->cdev))
 		return 0;
@@ -463,12 +454,9 @@ int qed_spq_alloc(struct qed_hwfn *p_hwfn)
 	u32 capacity;
 
 	/* SPQ struct */
-	p_spq =
-		kzalloc(sizeof(struct qed_spq), GFP_KERNEL);
-	if (!p_spq) {
-		DP_NOTICE(p_hwfn, "Failed to allocate `struct qed_spq'\n");
+	p_spq = kzalloc(sizeof(struct qed_spq), GFP_KERNEL);
+	if (!p_spq)
 		return -ENOMEM;
-	}
 
 	/* SPQ ring  */
 	if (qed_chain_alloc(p_hwfn->cdev,
@@ -477,18 +465,14 @@ int qed_spq_alloc(struct qed_hwfn *p_hwfn)
 			    QED_CHAIN_CNT_TYPE_U16,
 			    0,   /* N/A when the mode is SINGLE */
 			    sizeof(struct slow_path_element),
-			    &p_spq->chain)) {
-		DP_NOTICE(p_hwfn, "Failed to allocate spq chain\n");
+			    &p_spq->chain))
 		goto spq_allocate_fail;
-	}
 
 	/* allocate and fill the SPQ elements (incl. ramrod data list) */
 	capacity = qed_chain_get_capacity(&p_spq->chain);
 	p_virt = dma_alloc_coherent(&p_hwfn->cdev->pdev->dev,
-				    capacity *
-				    sizeof(struct qed_spq_entry),
+				    capacity * sizeof(struct qed_spq_entry),
 				    &p_phys, GFP_KERNEL);
-
 	if (!p_virt)
 		goto spq_allocate_fail;
 
@@ -525,9 +509,7 @@ void qed_spq_free(struct qed_hwfn *p_hwfn)
 	kfree(p_spq);
 }
 
-int
-qed_spq_get_entry(struct qed_hwfn *p_hwfn,
-		  struct qed_spq_entry **pp_ent)
+int qed_spq_get_entry(struct qed_hwfn *p_hwfn, struct qed_spq_entry **pp_ent)
 {
 	struct qed_spq *p_spq = p_hwfn->p_spq;
 	struct qed_spq_entry *p_ent = NULL;
@@ -538,14 +520,15 @@ qed_spq_get_entry(struct qed_hwfn *p_hwfn,
 	if (list_empty(&p_spq->free_pool)) {
 		p_ent = kzalloc(sizeof(*p_ent), GFP_ATOMIC);
 		if (!p_ent) {
+			DP_NOTICE(p_hwfn,
+				  "Failed to allocate an SPQ entry for a pending ramrod\n");
 			rc = -ENOMEM;
 			goto out_unlock;
 		}
 		p_ent->queue = &p_spq->unlimited_pending;
 	} else {
 		p_ent = list_first_entry(&p_spq->free_pool,
-					 struct qed_spq_entry,
-					 list);
+					 struct qed_spq_entry, list);
 		list_del(&p_ent->list);
 		p_ent->queue = &p_spq->pending;
 	}
@@ -564,8 +547,7 @@ static void __qed_spq_return_entry(struct qed_hwfn *p_hwfn,
 	list_add_tail(&p_ent->list, &p_hwfn->p_spq->free_pool);
 }
 
-void qed_spq_return_entry(struct qed_hwfn *p_hwfn,
-			  struct qed_spq_entry *p_ent)
+void qed_spq_return_entry(struct qed_hwfn *p_hwfn, struct qed_spq_entry *p_ent)
 {
 	spin_lock_bh(&p_hwfn->p_spq->lock);
 	__qed_spq_return_entry(p_hwfn, p_ent);
@@ -586,10 +568,9 @@ void qed_spq_return_entry(struct qed_hwfn *p_hwfn,
  *
  * @return int
  */
-static int
-qed_spq_add_entry(struct qed_hwfn *p_hwfn,
-		  struct qed_spq_entry *p_ent,
-		  enum spq_priority priority)
+static int qed_spq_add_entry(struct qed_hwfn *p_hwfn,
+			     struct qed_spq_entry *p_ent,
+			     enum spq_priority priority)
 {
 	struct qed_spq *p_spq = p_hwfn->p_spq;
 
@@ -604,8 +585,7 @@ qed_spq_add_entry(struct qed_hwfn *p_hwfn,
 			struct qed_spq_entry *p_en2;
 
 			p_en2 = list_first_entry(&p_spq->free_pool,
-						 struct qed_spq_entry,
-						 list);
+						 struct qed_spq_entry, list);
 			list_del(&p_en2->list);
 
 			/* Copy the ring element physical pointer to the new
@@ -655,8 +635,7 @@ u32 qed_spq_get_cid(struct qed_hwfn *p_hwfn)
 * Posting new Ramrods
 ***************************************************************************/
 static int qed_spq_post_list(struct qed_hwfn *p_hwfn,
-			     struct list_head *head,
-			     u32 keep_reserve)
+			     struct list_head *head, u32 keep_reserve)
 {
 	struct qed_spq *p_spq = p_hwfn->p_spq;
 	int rc;
@@ -690,8 +669,7 @@ static int qed_spq_pend_post(struct qed_hwfn *p_hwfn)
 			break;
 
 		p_ent = list_first_entry(&p_spq->unlimited_pending,
-					 struct qed_spq_entry,
-					 list);
+					 struct qed_spq_entry, list);
 		if (!p_ent)
 			return -EINVAL;
 
@@ -705,8 +683,7 @@ static int qed_spq_pend_post(struct qed_hwfn *p_hwfn)
 }
 
 int qed_spq_post(struct qed_hwfn *p_hwfn,
-		 struct qed_spq_entry *p_ent,
-		 u8 *fw_return_code)
+		 struct qed_spq_entry *p_ent, u8 *fw_return_code)
 {
 	int rc = 0;
 	struct qed_spq *p_spq = p_hwfn ? p_hwfn->p_spq : NULL;
@@ -803,8 +780,7 @@ int qed_spq_completion(struct qed_hwfn *p_hwfn,
 		return -EINVAL;
 
 	spin_lock_bh(&p_spq->lock);
-	list_for_each_entry_safe(p_ent, tmp, &p_spq->completion_pending,
-				 list) {
+	list_for_each_entry_safe(p_ent, tmp, &p_spq->completion_pending, list) {
 		if (p_ent->elem.hdr.echo == echo) {
 			u16 pos = le16_to_cpu(echo) % SPQ_RING_SIZE;
 
@@ -846,15 +822,22 @@ int qed_spq_completion(struct qed_hwfn *p_hwfn,
 
 	if (!found) {
 		DP_NOTICE(p_hwfn,
-			  "Failed to find an entry this EQE completes\n");
+			  "Failed to find an entry this EQE [echo %04x] completes\n",
+			  le16_to_cpu(echo));
 		return -EEXIST;
 	}
 
-	DP_VERBOSE(p_hwfn, QED_MSG_SPQ, "Complete: func %p cookie %p)\n",
+	DP_VERBOSE(p_hwfn, QED_MSG_SPQ,
+		   "Complete EQE [echo %04x]: func %p cookie %p)\n",
+		   le16_to_cpu(echo),
 		   p_ent->comp_cb.function, p_ent->comp_cb.cookie);
 	if (found->comp_cb.function)
 		found->comp_cb.function(p_hwfn, found->comp_cb.cookie, p_data,
 					fw_return_code);
+	else
+		DP_VERBOSE(p_hwfn,
+			   QED_MSG_SPQ,
+			   "Got a completion without a callback function\n");
 
 	if ((found->comp_mode != QED_SPQ_MODE_EBLOCK) ||
 	    (found->queue == &p_spq->unlimited_pending))
@@ -878,10 +861,8 @@ struct qed_consq *qed_consq_alloc(struct qed_hwfn *p_hwfn)
 
 	/* Allocate ConsQ struct */
 	p_consq = kzalloc(sizeof(*p_consq), GFP_KERNEL);
-	if (!p_consq) {
-		DP_NOTICE(p_hwfn, "Failed to allocate `struct qed_consq'\n");
+	if (!p_consq)
 		return NULL;
-	}
 
 	/* Allocate and initialize EQ chain*/
 	if (qed_chain_alloc(p_hwfn->cdev,
@@ -889,10 +870,8 @@ struct qed_consq *qed_consq_alloc(struct qed_hwfn *p_hwfn)
 			    QED_CHAIN_MODE_PBL,
 			    QED_CHAIN_CNT_TYPE_U16,
 			    QED_CHAIN_PAGE_SIZE / 0x80,
-			    0x80, &p_consq->chain)) {
-		DP_NOTICE(p_hwfn, "Failed to allocate consq chain");
+			    0x80, &p_consq->chain))
 		goto consq_allocate_fail;
-	}
 
 	return p_consq;
 
@@ -901,14 +880,12 @@ consq_allocate_fail:
 	return NULL;
 }
 
-void qed_consq_setup(struct qed_hwfn *p_hwfn,
-		     struct qed_consq *p_consq)
+void qed_consq_setup(struct qed_hwfn *p_hwfn, struct qed_consq *p_consq)
 {
 	qed_chain_reset(&p_consq->chain);
 }
 
-void qed_consq_free(struct qed_hwfn *p_hwfn,
-		    struct qed_consq *p_consq)
+void qed_consq_free(struct qed_hwfn *p_hwfn, struct qed_consq *p_consq)
 {
 	if (!p_consq)
 		return;
diff --git a/drivers/net/ethernet/qlogic/qed/qed_sriov.c b/drivers/net/ethernet/qlogic/qed/qed_sriov.c
index 15399da268d9..d2d6621fe0e5 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_sriov.c
+++ b/drivers/net/ethernet/qlogic/qed/qed_sriov.c
@@ -60,7 +60,8 @@ static int qed_sp_vf_start(struct qed_hwfn *p_hwfn, struct qed_vf_info *p_vf)
 	}
 
 	fp_minor = p_vf->acquire.vfdev_info.eth_fp_hsi_minor;
-	if (fp_minor > ETH_HSI_VER_MINOR) {
+	if (fp_minor > ETH_HSI_VER_MINOR &&
+	    fp_minor != ETH_HSI_VER_NO_PKT_LEN_TUNN) {
 		DP_VERBOSE(p_hwfn,
 			   QED_MSG_IOV,
 			   "VF [%d] - Requested fp hsi %02x.%02x which is slightly newer than PF's %02x.%02x; Configuring PFs version\n",
@@ -107,8 +108,8 @@ static int qed_sp_vf_stop(struct qed_hwfn *p_hwfn,
 	return qed_spq_post(p_hwfn, p_ent, NULL);
 }
 
-bool qed_iov_is_valid_vfid(struct qed_hwfn *p_hwfn,
-			   int rel_vf_id, bool b_enabled_only)
+static bool qed_iov_is_valid_vfid(struct qed_hwfn *p_hwfn,
+				  int rel_vf_id, bool b_enabled_only)
 {
 	if (!p_hwfn->pf_iov_info) {
 		DP_NOTICE(p_hwfn->cdev, "No iov info\n");
@@ -185,8 +186,8 @@ static bool qed_iov_validate_sb(struct qed_hwfn *p_hwfn,
 	return false;
 }
 
-int qed_iov_post_vf_bulletin(struct qed_hwfn *p_hwfn,
-			     int vfid, struct qed_ptt *p_ptt)
+static int qed_iov_post_vf_bulletin(struct qed_hwfn *p_hwfn,
+				    int vfid, struct qed_ptt *p_ptt)
 {
 	struct qed_bulletin_content *p_bulletin;
 	int crc_size = sizeof(p_bulletin->crc);
@@ -454,10 +455,8 @@ int qed_iov_alloc(struct qed_hwfn *p_hwfn)
 	}
 
 	p_sriov = kzalloc(sizeof(*p_sriov), GFP_KERNEL);
-	if (!p_sriov) {
-		DP_NOTICE(p_hwfn, "Failed to allocate `struct qed_sriov'\n");
+	if (!p_sriov)
 		return -ENOMEM;
-	}
 
 	p_hwfn->pf_iov_info = p_sriov;
 
@@ -506,10 +505,9 @@ int qed_iov_hw_info(struct qed_hwfn *p_hwfn)
 
 	/* Allocate a new struct for IOV information */
 	cdev->p_iov_info = kzalloc(sizeof(*cdev->p_iov_info), GFP_KERNEL);
-	if (!cdev->p_iov_info) {
-		DP_NOTICE(p_hwfn, "Can't support IOV due to lack of memory\n");
+	if (!cdev->p_iov_info)
 		return -ENOMEM;
-	}
+
 	cdev->p_iov_info->pos = pos;
 
 	rc = qed_iov_pci_cfg_info(cdev);
@@ -575,7 +573,7 @@ static void qed_iov_set_vf_to_disable(struct qed_dev *cdev,
 	}
 }
 
-void qed_iov_set_vfs_to_disable(struct qed_dev *cdev, u8 to_disable)
+static void qed_iov_set_vfs_to_disable(struct qed_dev *cdev, u8 to_disable)
 {
 	u16 i;
 
@@ -699,7 +697,7 @@ static void qed_iov_config_perm_table(struct qed_hwfn *p_hwfn,
 				&qzone_id);
 
 		reg_addr = PSWHST_REG_ZONE_PERMISSION_TABLE + qzone_id * 4;
-		val = enable ? (vf->abs_vf_id | (1 << 8)) : 0;
+		val = enable ? (vf->abs_vf_id | BIT(8)) : 0;
 		qed_wr(p_hwfn, p_ptt, reg_addr, val);
 	}
 }
@@ -1090,13 +1088,13 @@ static u16 qed_iov_prep_vp_update_resp_tlvs(struct qed_hwfn *p_hwfn,
 
 	/* Prepare response for all extended tlvs if they are found by PF */
 	for (i = 0; i < QED_IOV_VP_UPDATE_MAX; i++) {
-		if (!(tlvs_mask & (1 << i)))
+		if (!(tlvs_mask & BIT(i)))
 			continue;
 
 		resp = qed_add_tlv(p_hwfn, &p_mbx->offset,
 				   qed_iov_vport_to_tlv(p_hwfn, i), size);
 
-		if (tlvs_accepted & (1 << i))
+		if (tlvs_accepted & BIT(i))
 			resp->hdr.status = status;
 		else
 			resp->hdr.status = PFVF_STATUS_NOT_SUPPORTED;
@@ -1132,9 +1130,10 @@ static void qed_iov_prepare_resp(struct qed_hwfn *p_hwfn,
 	qed_iov_send_response(p_hwfn, p_ptt, vf_info, length, status);
 }
 
-struct qed_public_vf_info *qed_iov_get_public_vf_info(struct qed_hwfn *p_hwfn,
-						      u16 relative_vf_id,
-						      bool b_enabled_only)
+static struct
+qed_public_vf_info *qed_iov_get_public_vf_info(struct qed_hwfn *p_hwfn,
+					       u16 relative_vf_id,
+					       bool b_enabled_only)
 {
 	struct qed_vf_info *vf = NULL;
 
@@ -1145,7 +1144,7 @@ struct qed_public_vf_info *qed_iov_get_public_vf_info(struct qed_hwfn *p_hwfn,
 	return &vf->p_vf_info;
 }
 
-void qed_iov_clean_vf(struct qed_hwfn *p_hwfn, u8 vfid)
+static void qed_iov_clean_vf(struct qed_hwfn *p_hwfn, u8 vfid)
 {
 	struct qed_public_vf_info *vf_info;
 
@@ -1241,6 +1240,16 @@ static u8 qed_iov_vf_mbx_acquire_resc(struct qed_hwfn *p_hwfn,
 			   p_req->num_vlan_filters,
 			   p_resp->num_vlan_filters,
 			   p_req->num_mc_filters, p_resp->num_mc_filters);
+
+		/* Some legacy OSes are incapable of correctly handling this
+		 * failure.
+		 */
+		if ((p_vf->acquire.vfdev_info.eth_fp_hsi_minor ==
+		     ETH_HSI_VER_NO_PKT_LEN_TUNN) &&
+		    (p_vf->acquire.vfdev_info.os_type ==
+		     VFPF_ACQUIRE_OS_WINDOWS))
+			return PFVF_STATUS_SUCCESS;
+
 		return PFVF_STATUS_NO_RESOURCE;
 	}
 
@@ -1280,22 +1289,42 @@ static void qed_iov_vf_mbx_acquire(struct qed_hwfn *p_hwfn,
 
 	memset(resp, 0, sizeof(*resp));
 
+	/* Write the PF version so that VF would know which version
+	 * is supported - might be later overriden. This guarantees that
+	 * VF could recognize legacy PF based on lack of versions in reply.
+	 */
+	pfdev_info->major_fp_hsi = ETH_HSI_VER_MAJOR;
+	pfdev_info->minor_fp_hsi = ETH_HSI_VER_MINOR;
+
+	if (vf->state != VF_FREE && vf->state != VF_STOPPED) {
+		DP_VERBOSE(p_hwfn,
+			   QED_MSG_IOV,
+			   "VF[%d] sent ACQUIRE but is already in state %d - fail request\n",
+			   vf->abs_vf_id, vf->state);
+		goto out;
+	}
+
 	/* Validate FW compatibility */
 	if (req->vfdev_info.eth_fp_hsi_major != ETH_HSI_VER_MAJOR) {
-		DP_INFO(p_hwfn,
-			"VF[%d] needs fastpath HSI %02x.%02x, which is incompatible with loaded FW's faspath HSI %02x.%02x\n",
-			vf->abs_vf_id,
-			req->vfdev_info.eth_fp_hsi_major,
-			req->vfdev_info.eth_fp_hsi_minor,
-			ETH_HSI_VER_MAJOR, ETH_HSI_VER_MINOR);
-
-		/* Write the PF version so that VF would know which version
-		 * is supported.
-		 */
-		pfdev_info->major_fp_hsi = ETH_HSI_VER_MAJOR;
-		pfdev_info->minor_fp_hsi = ETH_HSI_VER_MINOR;
+		if (req->vfdev_info.capabilities &
+		    VFPF_ACQUIRE_CAP_PRE_FP_HSI) {
+			struct vf_pf_vfdev_info *p_vfdev = &req->vfdev_info;
 
-		goto out;
+			DP_VERBOSE(p_hwfn, QED_MSG_IOV,
+				   "VF[%d] is pre-fastpath HSI\n",
+				   vf->abs_vf_id);
+			p_vfdev->eth_fp_hsi_major = ETH_HSI_VER_MAJOR;
+			p_vfdev->eth_fp_hsi_minor = ETH_HSI_VER_NO_PKT_LEN_TUNN;
+		} else {
+			DP_INFO(p_hwfn,
+				"VF[%d] needs fastpath HSI %02x.%02x, which is incompatible with loaded FW's faspath HSI %02x.%02x\n",
+				vf->abs_vf_id,
+				req->vfdev_info.eth_fp_hsi_major,
+				req->vfdev_info.eth_fp_hsi_minor,
+				ETH_HSI_VER_MAJOR, ETH_HSI_VER_MINOR);
+
+			goto out;
+		}
 	}
 
 	/* On 100g PFs, prevent old VFs from loading */
@@ -1334,8 +1363,11 @@ static void qed_iov_vf_mbx_acquire(struct qed_hwfn *p_hwfn,
 	pfdev_info->fw_minor = FW_MINOR_VERSION;
 	pfdev_info->fw_rev = FW_REVISION_VERSION;
 	pfdev_info->fw_eng = FW_ENGINEERING_VERSION;
-	pfdev_info->minor_fp_hsi = min_t(u8,
-					 ETH_HSI_VER_MINOR,
+
+	/* Incorrect when legacy, but doesn't matter as legacy isn't reading
+	 * this field.
+	 */
+	pfdev_info->minor_fp_hsi = min_t(u8, ETH_HSI_VER_MINOR,
 					 req->vfdev_info.eth_fp_hsi_minor);
 	pfdev_info->os_type = VFPF_ACQUIRE_OS_LINUX;
 	qed_mcp_get_mfw_ver(p_hwfn, p_ptt, &pfdev_info->mfw_ver, NULL);
@@ -1438,14 +1470,11 @@ static int qed_iov_reconfigure_unicast_vlan(struct qed_hwfn *p_hwfn,
 
 		filter.type = QED_FILTER_VLAN;
 		filter.vlan = p_vf->shadow_config.vlans[i].vid;
-		DP_VERBOSE(p_hwfn,
-			   QED_MSG_IOV,
+		DP_VERBOSE(p_hwfn, QED_MSG_IOV,
 			   "Reconfiguring VLAN [0x%04x] for VF [%04x]\n",
 			   filter.vlan, p_vf->relative_vf_id);
-		rc = qed_sp_eth_filter_ucast(p_hwfn,
-					     p_vf->opaque_fid,
-					     &filter,
-					     QED_SPQ_MODE_CB, NULL);
+		rc = qed_sp_eth_filter_ucast(p_hwfn, p_vf->opaque_fid,
+					     &filter, QED_SPQ_MODE_CB, NULL);
 		if (rc) {
 			DP_NOTICE(p_hwfn,
 				  "Failed to configure VLAN [%04x] to VF [%04x]\n",
@@ -1463,7 +1492,7 @@ qed_iov_reconfigure_unicast_shadow(struct qed_hwfn *p_hwfn,
 {
 	int rc = 0;
 
-	if ((events & (1 << VLAN_ADDR_FORCED)) &&
+	if ((events & BIT(VLAN_ADDR_FORCED)) &&
 	    !(p_vf->configured_features & (1 << VLAN_ADDR_FORCED)))
 		rc = qed_iov_reconfigure_unicast_vlan(p_hwfn, p_vf);
 
@@ -1479,7 +1508,7 @@ static int qed_iov_configure_vport_forced(struct qed_hwfn *p_hwfn,
 	if (!p_vf->vport_instance)
 		return -EINVAL;
 
-	if (events & (1 << MAC_ADDR_FORCED)) {
+	if (events & BIT(MAC_ADDR_FORCED)) {
 		/* Since there's no way [currently] of removing the MAC,
 		 * we can always assume this means we need to force it.
 		 */
@@ -1502,7 +1531,7 @@ static int qed_iov_configure_vport_forced(struct qed_hwfn *p_hwfn,
 		p_vf->configured_features |= 1 << MAC_ADDR_FORCED;
 	}
 
-	if (events & (1 << VLAN_ADDR_FORCED)) {
+	if (events & BIT(VLAN_ADDR_FORCED)) {
 		struct qed_sp_vport_update_params vport_update;
 		u8 removal;
 		int i;
@@ -1572,7 +1601,7 @@ static int qed_iov_configure_vport_forced(struct qed_hwfn *p_hwfn,
 		if (filter.vlan)
 			p_vf->configured_features |= 1 << VLAN_ADDR_FORCED;
 		else
-			p_vf->configured_features &= ~(1 << VLAN_ADDR_FORCED);
+			p_vf->configured_features &= ~BIT(VLAN_ADDR_FORCED);
 	}
 
 	/* If forced features are terminated, we need to configure the shadow
@@ -1619,8 +1648,7 @@ static void qed_iov_vf_mbx_start_vport(struct qed_hwfn *p_hwfn,
 
 		qed_int_cau_conf_sb(p_hwfn, p_ptt,
 				    start->sb_addr[sb_id],
-				    vf->igu_sbs[sb_id],
-				    vf->abs_vf_id, 1);
+				    vf->igu_sbs[sb_id], vf->abs_vf_id, 1);
 	}
 	qed_iov_enable_vf_traffic(p_hwfn, p_ptt, vf);
 
@@ -1632,7 +1660,7 @@ static void qed_iov_vf_mbx_start_vport(struct qed_hwfn *p_hwfn,
 	 * vfs that would still be fine, since they passed '0' as padding].
 	 */
 	p_bitmap = &vf_info->bulletin.p_virt->valid_bitmap;
-	if (!(*p_bitmap & (1 << VFPF_BULLETIN_UNTAGGED_DEFAULT_FORCED))) {
+	if (!(*p_bitmap & BIT(VFPF_BULLETIN_UNTAGGED_DEFAULT_FORCED))) {
 		u8 vf_req = start->only_untagged;
 
 		vf_info->bulletin.p_virt->default_only_untagged = vf_req;
@@ -1650,9 +1678,10 @@ static void qed_iov_vf_mbx_start_vport(struct qed_hwfn *p_hwfn,
 	params.vport_id = vf->vport_id;
 	params.max_buffers_per_cqe = start->max_buffers_per_cqe;
 	params.mtu = vf->mtu;
+	params.check_mac = true;
 
 	rc = qed_sp_eth_vport_start(p_hwfn, &params);
-	if (rc != 0) {
+	if (rc) {
 		DP_ERR(p_hwfn,
 		       "qed_iov_vf_mbx_start_vport returned error %d\n", rc);
 		status = PFVF_STATUS_FAILURE;
@@ -1679,7 +1708,7 @@ static void qed_iov_vf_mbx_stop_vport(struct qed_hwfn *p_hwfn,
 	vf->spoof_chk = false;
 
 	rc = qed_sp_vport_stop(p_hwfn, vf->opaque_fid, vf->vport_id);
-	if (rc != 0) {
+	if (rc) {
 		DP_ERR(p_hwfn, "qed_iov_vf_mbx_stop_vport returned error %d\n",
 		       rc);
 		status = PFVF_STATUS_FAILURE;
@@ -1695,21 +1724,32 @@ static void qed_iov_vf_mbx_stop_vport(struct qed_hwfn *p_hwfn,
 
 static void qed_iov_vf_mbx_start_rxq_resp(struct qed_hwfn *p_hwfn,
 					  struct qed_ptt *p_ptt,
-					  struct qed_vf_info *vf, u8 status)
+					  struct qed_vf_info *vf,
+					  u8 status, bool b_legacy)
 {
 	struct qed_iov_vf_mbx *mbx = &vf->vf_mbx;
 	struct pfvf_start_queue_resp_tlv *p_tlv;
 	struct vfpf_start_rxq_tlv *req;
+	u16 length;
 
 	mbx->offset = (u8 *)mbx->reply_virt;
 
+	/* Taking a bigger struct instead of adding a TLV to list was a
+	 * mistake, but one which we're now stuck with, as some older
+	 * clients assume the size of the previous response.
+	 */
+	if (!b_legacy)
+		length = sizeof(*p_tlv);
+	else
+		length = sizeof(struct pfvf_def_resp_tlv);
+
 	p_tlv = qed_add_tlv(p_hwfn, &mbx->offset, CHANNEL_TLV_START_RXQ,
-			    sizeof(*p_tlv));
+			    length);
 	qed_add_tlv(p_hwfn, &mbx->offset, CHANNEL_TLV_LIST_END,
 		    sizeof(struct channel_list_end_tlv));
 
 	/* Update the TLV with the response */
-	if (status == PFVF_STATUS_SUCCESS) {
+	if ((status == PFVF_STATUS_SUCCESS) && !b_legacy) {
 		req = &mbx->req_virt->start_rxq;
 		p_tlv->offset = PXP_VF_BAR0_START_MSDM_ZONE_B +
 				offsetof(struct mstorm_vf_zone,
@@ -1717,7 +1757,7 @@ static void qed_iov_vf_mbx_start_rxq_resp(struct qed_hwfn *p_hwfn,
 				sizeof(struct eth_rx_prod_data) * req->rx_qid;
 	}
 
-	qed_iov_send_response(p_hwfn, p_ptt, vf, sizeof(*p_tlv), status);
+	qed_iov_send_response(p_hwfn, p_ptt, vf, length, status);
 }
 
 static void qed_iov_vf_mbx_start_rxq(struct qed_hwfn *p_hwfn,
@@ -1728,6 +1768,7 @@ static void qed_iov_vf_mbx_start_rxq(struct qed_hwfn *p_hwfn,
 	struct qed_iov_vf_mbx *mbx = &vf->vf_mbx;
 	u8 status = PFVF_STATUS_NO_RESOURCE;
 	struct vfpf_start_rxq_tlv *req;
+	bool b_legacy_vf = false;
 	int rc;
 
 	memset(&params, 0, sizeof(params));
@@ -1743,13 +1784,27 @@ static void qed_iov_vf_mbx_start_rxq(struct qed_hwfn *p_hwfn,
 	params.sb = req->hw_sb;
 	params.sb_idx = req->sb_index;
 
+	/* Legacy VFs have their Producers in a different location, which they
+	 * calculate on their own and clean the producer prior to this.
+	 */
+	if (vf->acquire.vfdev_info.eth_fp_hsi_minor ==
+	    ETH_HSI_VER_NO_PKT_LEN_TUNN) {
+		b_legacy_vf = true;
+	} else {
+		REG_WR(p_hwfn,
+		       GTT_BAR0_MAP_REG_MSDM_RAM +
+		       MSTORM_ETH_VF_PRODS_OFFSET(vf->abs_vf_id, req->rx_qid),
+		       0);
+	}
+
 	rc = qed_sp_eth_rxq_start_ramrod(p_hwfn, vf->opaque_fid,
 					 vf->vf_queues[req->rx_qid].fw_cid,
 					 &params,
 					 vf->abs_vf_id + 0x10,
 					 req->bd_max_bytes,
 					 req->rxq_addr,
-					 req->cqe_pbl_addr, req->cqe_pbl_size);
+					 req->cqe_pbl_addr, req->cqe_pbl_size,
+					 b_legacy_vf);
 
 	if (rc) {
 		status = PFVF_STATUS_FAILURE;
@@ -1760,7 +1815,7 @@ static void qed_iov_vf_mbx_start_rxq(struct qed_hwfn *p_hwfn,
 	}
 
 out:
-	qed_iov_vf_mbx_start_rxq_resp(p_hwfn, p_ptt, vf, status);
+	qed_iov_vf_mbx_start_rxq_resp(p_hwfn, p_ptt, vf, status, b_legacy_vf);
 }
 
 static void qed_iov_vf_mbx_start_txq_resp(struct qed_hwfn *p_hwfn,
@@ -1769,23 +1824,38 @@ static void qed_iov_vf_mbx_start_txq_resp(struct qed_hwfn *p_hwfn,
 {
 	struct qed_iov_vf_mbx *mbx = &p_vf->vf_mbx;
 	struct pfvf_start_queue_resp_tlv *p_tlv;
+	bool b_legacy = false;
+	u16 length;
 
 	mbx->offset = (u8 *)mbx->reply_virt;
 
+	/* Taking a bigger struct instead of adding a TLV to list was a
+	 * mistake, but one which we're now stuck with, as some older
+	 * clients assume the size of the previous response.
+	 */
+	if (p_vf->acquire.vfdev_info.eth_fp_hsi_minor ==
+	    ETH_HSI_VER_NO_PKT_LEN_TUNN)
+		b_legacy = true;
+
+	if (!b_legacy)
+		length = sizeof(*p_tlv);
+	else
+		length = sizeof(struct pfvf_def_resp_tlv);
+
 	p_tlv = qed_add_tlv(p_hwfn, &mbx->offset, CHANNEL_TLV_START_TXQ,
-			    sizeof(*p_tlv));
+			    length);
 	qed_add_tlv(p_hwfn, &mbx->offset, CHANNEL_TLV_LIST_END,
 		    sizeof(struct channel_list_end_tlv));
 
 	/* Update the TLV with the response */
-	if (status == PFVF_STATUS_SUCCESS) {
+	if ((status == PFVF_STATUS_SUCCESS) && !b_legacy) {
 		u16 qid = mbx->req_virt->start_txq.tx_qid;
 
-		p_tlv->offset = qed_db_addr(p_vf->vf_queues[qid].fw_cid,
-					    DQ_DEMS_LEGACY);
+		p_tlv->offset = qed_db_addr_vf(p_vf->vf_queues[qid].fw_cid,
+					       DQ_DEMS_LEGACY);
 	}
 
-	qed_iov_send_response(p_hwfn, p_ptt, p_vf, sizeof(*p_tlv), status);
+	qed_iov_send_response(p_hwfn, p_ptt, p_vf, length, status);
 }
 
 static void qed_iov_vf_mbx_start_txq(struct qed_hwfn *p_hwfn,
@@ -2045,7 +2115,7 @@ qed_iov_vp_update_vlan_param(struct qed_hwfn *p_hwfn,
 	p_vf->shadow_config.inner_vlan_removal = p_vlan_tlv->remove_vlan;
 
 	/* Ignore the VF request if we're forcing a vlan */
-	if (!(p_vf->configured_features & (1 << VLAN_ADDR_FORCED))) {
+	if (!(p_vf->configured_features & BIT(VLAN_ADDR_FORCED))) {
 		p_data->update_inner_vlan_removal_flg = 1;
 		p_data->inner_vlan_removal_flg = p_vlan_tlv->remove_vlan;
 	}
@@ -2340,7 +2410,7 @@ static int qed_iov_vf_update_vlan_shadow(struct qed_hwfn *p_hwfn,
 	/* In forced mode, we're willing to remove entries - but we don't add
 	 * new ones.
 	 */
-	if (p_vf->bulletin.p_virt->valid_bitmap & (1 << VLAN_ADDR_FORCED))
+	if (p_vf->bulletin.p_virt->valid_bitmap & BIT(VLAN_ADDR_FORCED))
 		return 0;
 
 	if (p_params->opcode == QED_FILTER_ADD ||
@@ -2374,7 +2444,7 @@ static int qed_iov_vf_update_mac_shadow(struct qed_hwfn *p_hwfn,
 	int i;
 
 	/* If we're in forced-mode, we don't allow any change */
-	if (p_vf->bulletin.p_virt->valid_bitmap & (1 << MAC_ADDR_FORCED))
+	if (p_vf->bulletin.p_virt->valid_bitmap & BIT(MAC_ADDR_FORCED))
 		return 0;
 
 	/* First remove entries and then add new ones */
@@ -2441,8 +2511,8 @@ qed_iov_vf_update_unicast_shadow(struct qed_hwfn *p_hwfn,
 	return rc;
 }
 
-int qed_iov_chk_ucast(struct qed_hwfn *hwfn,
-		      int vfid, struct qed_filter_ucast *params)
+static int qed_iov_chk_ucast(struct qed_hwfn *hwfn,
+			     int vfid, struct qed_filter_ucast *params)
 {
 	struct qed_public_vf_info *vf;
 
@@ -2509,7 +2579,7 @@ static void qed_iov_vf_mbx_ucast_filter(struct qed_hwfn *p_hwfn,
 	}
 
 	/* Determine if the unicast filtering is acceptible by PF */
-	if ((p_bulletin->valid_bitmap & (1 << VLAN_ADDR_FORCED)) &&
+	if ((p_bulletin->valid_bitmap & BIT(VLAN_ADDR_FORCED)) &&
 	    (params.type == QED_FILTER_VLAN ||
 	     params.type == QED_FILTER_MAC_VLAN)) {
 		/* Once VLAN is forced or PVID is set, do not allow
@@ -2521,7 +2591,7 @@ static void qed_iov_vf_mbx_ucast_filter(struct qed_hwfn *p_hwfn,
 		goto out;
 	}
 
-	if ((p_bulletin->valid_bitmap & (1 << MAC_ADDR_FORCED)) &&
+	if ((p_bulletin->valid_bitmap & BIT(MAC_ADDR_FORCED)) &&
 	    (params.type == QED_FILTER_MAC ||
 	     params.type == QED_FILTER_MAC_VLAN)) {
 		if (!ether_addr_equal(p_bulletin->mac, params.mac) ||
@@ -2749,7 +2819,7 @@ cleanup:
 		/* Mark VF for ack and clean pending state */
 		if (p_vf->state == VF_RESET)
 			p_vf->state = VF_STOPPED;
-		ack_vfs[vfid / 32] |= (1 << (vfid % 32));
+		ack_vfs[vfid / 32] |= BIT((vfid % 32));
 		p_hwfn->pf_iov_info->pending_flr[rel_vf_id / 64] &=
 		    ~(1ULL << (rel_vf_id % 64));
 		p_hwfn->pf_iov_info->pending_events[rel_vf_id / 64] &=
@@ -2759,7 +2829,8 @@ cleanup:
 	return rc;
 }
 
-int qed_iov_vf_flr_cleanup(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
+static int
+qed_iov_vf_flr_cleanup(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
 {
 	u32 ack_vfs[VF_MAX_STATIC / 32];
 	int rc = 0;
@@ -2805,7 +2876,7 @@ int qed_iov_mark_vf_flr(struct qed_hwfn *p_hwfn, u32 *p_disabled_vfs)
 			continue;
 
 		vfid = p_vf->abs_vf_id;
-		if ((1 << (vfid % 32)) & p_disabled_vfs[vfid / 32]) {
+		if (BIT((vfid % 32)) & p_disabled_vfs[vfid / 32]) {
 			u64 *p_flr = p_hwfn->pf_iov_info->pending_flr;
 			u16 rel_vf_id = p_vf->relative_vf_id;
 
@@ -2946,7 +3017,7 @@ static void qed_iov_process_mbx_req(struct qed_hwfn *p_hwfn,
 	}
 }
 
-void qed_iov_pf_add_pending_events(struct qed_hwfn *p_hwfn, u8 vfid)
+static void qed_iov_pf_add_pending_events(struct qed_hwfn *p_hwfn, u8 vfid)
 {
 	u64 add_bit = 1ULL << (vfid % 64);
 
@@ -3064,14 +3135,13 @@ static void qed_iov_bulletin_set_forced_mac(struct qed_hwfn *p_hwfn,
 
 	vf_info->bulletin.p_virt->valid_bitmap |= feature;
 	/* Forced MAC will disable MAC_ADDR */
-	vf_info->bulletin.p_virt->valid_bitmap &=
-				~(1 << VFPF_BULLETIN_MAC_ADDR);
+	vf_info->bulletin.p_virt->valid_bitmap &= ~BIT(VFPF_BULLETIN_MAC_ADDR);
 
 	qed_iov_configure_vport_forced(p_hwfn, vf_info, feature);
 }
 
-void qed_iov_bulletin_set_forced_vlan(struct qed_hwfn *p_hwfn,
-				      u16 pvid, int vfid)
+static void qed_iov_bulletin_set_forced_vlan(struct qed_hwfn *p_hwfn,
+					     u16 pvid, int vfid)
 {
 	struct qed_vf_info *vf_info;
 	u64 feature;
@@ -3104,7 +3174,7 @@ static bool qed_iov_vf_has_vport_instance(struct qed_hwfn *p_hwfn, int vfid)
 	return !!p_vf_info->vport_instance;
 }
 
-bool qed_iov_is_vf_stopped(struct qed_hwfn *p_hwfn, int vfid)
+static bool qed_iov_is_vf_stopped(struct qed_hwfn *p_hwfn, int vfid)
 {
 	struct qed_vf_info *p_vf_info;
 
@@ -3126,7 +3196,7 @@ static bool qed_iov_spoofchk_get(struct qed_hwfn *p_hwfn, int vfid)
 	return vf_info->spoof_chk;
 }
 
-int qed_iov_spoofchk_set(struct qed_hwfn *p_hwfn, int vfid, bool val)
+static int qed_iov_spoofchk_set(struct qed_hwfn *p_hwfn, int vfid, bool val)
 {
 	struct qed_vf_info *vf;
 	int rc = -EINVAL;
@@ -3163,13 +3233,14 @@ static u8 *qed_iov_bulletin_get_forced_mac(struct qed_hwfn *p_hwfn,
 	if (!p_vf || !p_vf->bulletin.p_virt)
 		return NULL;
 
-	if (!(p_vf->bulletin.p_virt->valid_bitmap & (1 << MAC_ADDR_FORCED)))
+	if (!(p_vf->bulletin.p_virt->valid_bitmap & BIT(MAC_ADDR_FORCED)))
 		return NULL;
 
 	return p_vf->bulletin.p_virt->mac;
 }
 
-u16 qed_iov_bulletin_get_forced_vlan(struct qed_hwfn *p_hwfn, u16 rel_vf_id)
+static u16
+qed_iov_bulletin_get_forced_vlan(struct qed_hwfn *p_hwfn, u16 rel_vf_id)
 {
 	struct qed_vf_info *p_vf;
 
@@ -3177,7 +3248,7 @@ u16 qed_iov_bulletin_get_forced_vlan(struct qed_hwfn *p_hwfn, u16 rel_vf_id)
 	if (!p_vf || !p_vf->bulletin.p_virt)
 		return 0;
 
-	if (!(p_vf->bulletin.p_virt->valid_bitmap & (1 << VLAN_ADDR_FORCED)))
+	if (!(p_vf->bulletin.p_virt->valid_bitmap & BIT(VLAN_ADDR_FORCED)))
 		return 0;
 
 	return p_vf->bulletin.p_virt->pvid;
@@ -3201,7 +3272,8 @@ static int qed_iov_configure_tx_rate(struct qed_hwfn *p_hwfn,
 	return qed_init_vport_rl(p_hwfn, p_ptt, abs_vp_id, (u32)val);
 }
 
-int qed_iov_configure_min_tx_rate(struct qed_dev *cdev, int vfid, u32 rate)
+static int
+qed_iov_configure_min_tx_rate(struct qed_dev *cdev, int vfid, u32 rate)
 {
 	struct qed_vf_info *vf;
 	u8 vport_id;
@@ -3760,7 +3832,8 @@ static void qed_handle_bulletin_post(struct qed_hwfn *hwfn)
 	qed_ptt_release(hwfn, ptt);
 }
 
-void qed_iov_pf_task(struct work_struct *work)
+static void qed_iov_pf_task(struct work_struct *work)
+
 {
 	struct qed_hwfn *hwfn = container_of(work, struct qed_hwfn,
 					     iov_task.work);
diff --git a/drivers/net/ethernet/qlogic/qed/qed_vf.c b/drivers/net/ethernet/qlogic/qed/qed_vf.c
index 9b780b31b15c..abf5bf11f865 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_vf.c
+++ b/drivers/net/ethernet/qlogic/qed/qed_vf.c
@@ -46,6 +46,17 @@ static void *qed_vf_pf_prep(struct qed_hwfn *p_hwfn, u16 type, u16 length)
 	return p_tlv;
 }
 
+static void qed_vf_pf_req_end(struct qed_hwfn *p_hwfn, int req_status)
+{
+	union pfvf_tlvs *resp = p_hwfn->vf_iov_info->pf2vf_reply;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_IOV,
+		   "VF request status = 0x%x, PF reply status = 0x%x\n",
+		   req_status, resp->default_resp.hdr.status);
+
+	mutex_unlock(&(p_hwfn->vf_iov_info->mutex));
+}
+
 static int qed_send_msg2pf(struct qed_hwfn *p_hwfn, u8 *done, u32 resp_size)
 {
 	union vfpf_tlvs *p_req = p_hwfn->vf_iov_info->vf2pf_request;
@@ -103,16 +114,12 @@ static int qed_send_msg2pf(struct qed_hwfn *p_hwfn, u8 *done, u32 resp_size)
 			   "VF <-- PF Timeout [Type %d]\n",
 			   p_req->first_tlv.tl.type);
 		rc = -EBUSY;
-		goto exit;
 	} else {
 		DP_VERBOSE(p_hwfn, QED_MSG_IOV,
 			   "PF response: %d [Type %d]\n",
 			   *done, p_req->first_tlv.tl.type);
 	}
 
-exit:
-	mutex_unlock(&(p_hwfn->vf_iov_info->mutex));
-
 	return rc;
 }
 
@@ -191,6 +198,9 @@ static int qed_vf_pf_acquire(struct qed_hwfn *p_hwfn)
 		DP_VERBOSE(p_hwfn,
 			   QED_MSG_IOV, "attempting to acquire resources\n");
 
+		/* Clear response buffer, as this might be a re-send */
+		memset(p_iov->pf2vf_reply, 0, sizeof(union pfvf_tlvs));
+
 		/* send acquire request */
 		rc = qed_send_msg2pf(p_hwfn, &resp->hdr.status, sizeof(*resp));
 		if (rc)
@@ -205,9 +215,12 @@ static int qed_vf_pf_acquire(struct qed_hwfn *p_hwfn)
 			/* PF agrees to allocate our resources */
 			if (!(resp->pfdev_info.capabilities &
 			      PFVF_ACQUIRE_CAP_POST_FW_OVERRIDE)) {
-				DP_INFO(p_hwfn,
-					"PF is using old incompatible driver; Either downgrade driver or request provider to update hypervisor version\n");
-				return -EINVAL;
+				/* It's possible legacy PF mistakenly accepted;
+				 * but we don't care - simply mark it as
+				 * legacy and continue.
+				 */
+				req->vfdev_info.capabilities |=
+				    VFPF_ACQUIRE_CAP_PRE_FP_HSI;
 			}
 			DP_VERBOSE(p_hwfn, QED_MSG_IOV, "resources acquired\n");
 			resources_acquired = true;
@@ -215,27 +228,55 @@ static int qed_vf_pf_acquire(struct qed_hwfn *p_hwfn)
 			   attempts < VF_ACQUIRE_THRESH) {
 			qed_vf_pf_acquire_reduce_resc(p_hwfn, p_resc,
 						      &resp->resc);
+		} else if (resp->hdr.status == PFVF_STATUS_NOT_SUPPORTED) {
+			if (pfdev_info->major_fp_hsi &&
+			    (pfdev_info->major_fp_hsi != ETH_HSI_VER_MAJOR)) {
+				DP_NOTICE(p_hwfn,
+					  "PF uses an incompatible fastpath HSI %02x.%02x [VF requires %02x.%02x]. Please change to a VF driver using %02x.xx.\n",
+					  pfdev_info->major_fp_hsi,
+					  pfdev_info->minor_fp_hsi,
+					  ETH_HSI_VER_MAJOR,
+					  ETH_HSI_VER_MINOR,
+					  pfdev_info->major_fp_hsi);
+				rc = -EINVAL;
+				goto exit;
+			}
 
-			/* Clear response buffer */
-			memset(p_iov->pf2vf_reply, 0, sizeof(union pfvf_tlvs));
-		} else if ((resp->hdr.status == PFVF_STATUS_NOT_SUPPORTED) &&
-			   pfdev_info->major_fp_hsi &&
-			   (pfdev_info->major_fp_hsi != ETH_HSI_VER_MAJOR)) {
-			DP_NOTICE(p_hwfn,
-				  "PF uses an incompatible fastpath HSI %02x.%02x [VF requires %02x.%02x]. Please change to a VF driver using %02x.xx.\n",
-				  pfdev_info->major_fp_hsi,
-				  pfdev_info->minor_fp_hsi,
-				  ETH_HSI_VER_MAJOR,
-				  ETH_HSI_VER_MINOR, pfdev_info->major_fp_hsi);
-			return -EINVAL;
+			if (!pfdev_info->major_fp_hsi) {
+				if (req->vfdev_info.capabilities &
+				    VFPF_ACQUIRE_CAP_PRE_FP_HSI) {
+					DP_NOTICE(p_hwfn,
+						  "PF uses very old drivers. Please change to a VF driver using no later than 8.8.x.x.\n");
+					rc = -EINVAL;
+					goto exit;
+				} else {
+					DP_INFO(p_hwfn,
+						"PF is old - try re-acquire to see if it supports FW-version override\n");
+					req->vfdev_info.capabilities |=
+					    VFPF_ACQUIRE_CAP_PRE_FP_HSI;
+					continue;
+				}
+			}
+
+			/* If PF/VF are using same Major, PF must have had
+			 * it's reasons. Simply fail.
+			 */
+			DP_NOTICE(p_hwfn, "PF rejected acquisition by VF\n");
+			rc = -EINVAL;
+			goto exit;
 		} else {
 			DP_ERR(p_hwfn,
 			       "PF returned error %d to VF acquisition request\n",
 			       resp->hdr.status);
-			return -EAGAIN;
+			rc = -EAGAIN;
+			goto exit;
 		}
 	}
 
+	/* Mark the PF as legacy, if needed */
+	if (req->vfdev_info.capabilities & VFPF_ACQUIRE_CAP_PRE_FP_HSI)
+		p_iov->b_pre_fp_hsi = true;
+
 	/* Update bulletin board size with response from PF */
 	p_iov->bulletin.size = resp->bulletin_size;
 
@@ -253,14 +294,18 @@ static int qed_vf_pf_acquire(struct qed_hwfn *p_hwfn)
 		}
 	}
 
-	if (ETH_HSI_VER_MINOR &&
+	if (!p_iov->b_pre_fp_hsi &&
+	    ETH_HSI_VER_MINOR &&
 	    (resp->pfdev_info.minor_fp_hsi < ETH_HSI_VER_MINOR)) {
 		DP_INFO(p_hwfn,
 			"PF is using older fastpath HSI; %02x.%02x is configured\n",
 			ETH_HSI_VER_MAJOR, resp->pfdev_info.minor_fp_hsi);
 	}
 
-	return 0;
+exit:
+	qed_vf_pf_req_end(p_hwfn, rc);
+
+	return rc;
 }
 
 int qed_vf_hw_prepare(struct qed_hwfn *p_hwfn)
@@ -286,31 +331,23 @@ int qed_vf_hw_prepare(struct qed_hwfn *p_hwfn)
 
 	/* Allocate vf sriov info */
 	p_iov = kzalloc(sizeof(*p_iov), GFP_KERNEL);
-	if (!p_iov) {
-		DP_NOTICE(p_hwfn, "Failed to allocate `struct qed_sriov'\n");
+	if (!p_iov)
 		return -ENOMEM;
-	}
 
 	/* Allocate vf2pf msg */
 	p_iov->vf2pf_request = dma_alloc_coherent(&p_hwfn->cdev->pdev->dev,
 						  sizeof(union vfpf_tlvs),
 						  &p_iov->vf2pf_request_phys,
 						  GFP_KERNEL);
-	if (!p_iov->vf2pf_request) {
-		DP_NOTICE(p_hwfn,
-			  "Failed to allocate `vf2pf_request' DMA memory\n");
+	if (!p_iov->vf2pf_request)
 		goto free_p_iov;
-	}
 
 	p_iov->pf2vf_reply = dma_alloc_coherent(&p_hwfn->cdev->pdev->dev,
 						sizeof(union pfvf_tlvs),
 						&p_iov->pf2vf_reply_phys,
 						GFP_KERNEL);
-	if (!p_iov->pf2vf_reply) {
-		DP_NOTICE(p_hwfn,
-			  "Failed to allocate `pf2vf_reply' DMA memory\n");
+	if (!p_iov->pf2vf_reply)
 		goto free_vf2pf_request;
-	}
 
 	DP_VERBOSE(p_hwfn,
 		   QED_MSG_IOV,
@@ -347,6 +384,9 @@ free_p_iov:
 
 	return -ENOMEM;
 }
+#define TSTORM_QZONE_START   PXP_VF_BAR0_START_SDM_ZONE_A
+#define MSTORM_QZONE_START(dev)   (TSTORM_QZONE_START +	\
+				   (TSTORM_QZONE_SIZE * NUM_OF_L2_QUEUES(dev)))
 
 int qed_vf_pf_rxq_start(struct qed_hwfn *p_hwfn,
 			u8 rx_qid,
@@ -374,6 +414,21 @@ int qed_vf_pf_rxq_start(struct qed_hwfn *p_hwfn,
 	req->bd_max_bytes = bd_max_bytes;
 	req->stat_id = -1;
 
+	/* If PF is legacy, we'll need to calculate producers ourselves
+	 * as well as clean them.
+	 */
+	if (pp_prod && p_iov->b_pre_fp_hsi) {
+		u8 hw_qid = p_iov->acquire_resp.resc.hw_qid[rx_qid];
+		u32 init_prod_val = 0;
+
+		*pp_prod = (u8 __iomem *)p_hwfn->regview +
+					 MSTORM_QZONE_START(p_hwfn->cdev) +
+					 hw_qid * MSTORM_QZONE_SIZE;
+
+		/* Init the rcq, rx bd and rx sge (if valid) producers to 0 */
+		__internal_ram_wr(p_hwfn, *pp_prod, sizeof(u32),
+				  (u32 *)(&init_prod_val));
+	}
 	/* add list termination tlv */
 	qed_add_tlv(p_hwfn, &p_iov->offset,
 		    CHANNEL_TLV_LIST_END, sizeof(struct channel_list_end_tlv));
@@ -381,13 +436,15 @@ int qed_vf_pf_rxq_start(struct qed_hwfn *p_hwfn,
 	resp = &p_iov->pf2vf_reply->queue_start;
 	rc = qed_send_msg2pf(p_hwfn, &resp->hdr.status, sizeof(*resp));
 	if (rc)
-		return rc;
+		goto exit;
 
-	if (resp->hdr.status != PFVF_STATUS_SUCCESS)
-		return -EINVAL;
+	if (resp->hdr.status != PFVF_STATUS_SUCCESS) {
+		rc = -EINVAL;
+		goto exit;
+	}
 
 	/* Learn the address of the producer from the response */
-	if (pp_prod) {
+	if (pp_prod && !p_iov->b_pre_fp_hsi) {
 		u32 init_prod_val = 0;
 
 		*pp_prod = (u8 __iomem *)p_hwfn->regview + resp->offset;
@@ -399,6 +456,8 @@ int qed_vf_pf_rxq_start(struct qed_hwfn *p_hwfn,
 		__internal_ram_wr(p_hwfn, *pp_prod, sizeof(u32),
 				  (u32 *)&init_prod_val);
 	}
+exit:
+	qed_vf_pf_req_end(p_hwfn, rc);
 
 	return rc;
 }
@@ -424,10 +483,15 @@ int qed_vf_pf_rxq_stop(struct qed_hwfn *p_hwfn, u16 rx_qid, bool cqe_completion)
 	resp = &p_iov->pf2vf_reply->default_resp;
 	rc = qed_send_msg2pf(p_hwfn, &resp->hdr.status, sizeof(*resp));
 	if (rc)
-		return rc;
+		goto exit;
+
+	if (resp->hdr.status != PFVF_STATUS_SUCCESS) {
+		rc = -EINVAL;
+		goto exit;
+	}
 
-	if (resp->hdr.status != PFVF_STATUS_SUCCESS)
-		return -EINVAL;
+exit:
+	qed_vf_pf_req_end(p_hwfn, rc);
 
 	return rc;
 }
@@ -470,13 +534,27 @@ int qed_vf_pf_txq_start(struct qed_hwfn *p_hwfn,
 	}
 
 	if (pp_doorbell) {
-		*pp_doorbell = (u8 __iomem *)p_hwfn->doorbells + resp->offset;
+		/* Modern PFs provide the actual offsets, while legacy
+		 * provided only the queue id.
+		 */
+		if (!p_iov->b_pre_fp_hsi) {
+			*pp_doorbell = (u8 __iomem *)p_hwfn->doorbells +
+						     resp->offset;
+		} else {
+			u8 cid = p_iov->acquire_resp.resc.cid[tx_queue_id];
+			u32 db_addr;
+
+			db_addr = qed_db_addr_vf(cid, DQ_DEMS_LEGACY);
+			*pp_doorbell = (u8 __iomem *)p_hwfn->doorbells +
+						     db_addr;
+		}
 
 		DP_VERBOSE(p_hwfn, QED_MSG_IOV,
 			   "Txq[0x%02x]: doorbell at %p [offset 0x%08x]\n",
 			   tx_queue_id, *pp_doorbell, resp->offset);
 	}
 exit:
+	qed_vf_pf_req_end(p_hwfn, rc);
 
 	return rc;
 }
@@ -501,10 +579,15 @@ int qed_vf_pf_txq_stop(struct qed_hwfn *p_hwfn, u16 tx_qid)
 	resp = &p_iov->pf2vf_reply->default_resp;
 	rc = qed_send_msg2pf(p_hwfn, &resp->hdr.status, sizeof(*resp));
 	if (rc)
-		return rc;
+		goto exit;
 
-	if (resp->hdr.status != PFVF_STATUS_SUCCESS)
-		return -EINVAL;
+	if (resp->hdr.status != PFVF_STATUS_SUCCESS) {
+		rc = -EINVAL;
+		goto exit;
+	}
+
+exit:
+	qed_vf_pf_req_end(p_hwfn, rc);
 
 	return rc;
 }
@@ -543,10 +626,15 @@ int qed_vf_pf_vport_start(struct qed_hwfn *p_hwfn,
 	resp = &p_iov->pf2vf_reply->default_resp;
 	rc = qed_send_msg2pf(p_hwfn, &resp->hdr.status, sizeof(*resp));
 	if (rc)
-		return rc;
+		goto exit;
 
-	if (resp->hdr.status != PFVF_STATUS_SUCCESS)
-		return -EINVAL;
+	if (resp->hdr.status != PFVF_STATUS_SUCCESS) {
+		rc = -EINVAL;
+		goto exit;
+	}
+
+exit:
+	qed_vf_pf_req_end(p_hwfn, rc);
 
 	return rc;
 }
@@ -567,10 +655,15 @@ int qed_vf_pf_vport_stop(struct qed_hwfn *p_hwfn)
 
 	rc = qed_send_msg2pf(p_hwfn, &resp->hdr.status, sizeof(*resp));
 	if (rc)
-		return rc;
+		goto exit;
+
+	if (resp->hdr.status != PFVF_STATUS_SUCCESS) {
+		rc = -EINVAL;
+		goto exit;
+	}
 
-	if (resp->hdr.status != PFVF_STATUS_SUCCESS)
-		return -EINVAL;
+exit:
+	qed_vf_pf_req_end(p_hwfn, rc);
 
 	return rc;
 }
@@ -770,13 +863,18 @@ int qed_vf_pf_vport_update(struct qed_hwfn *p_hwfn,
 
 	rc = qed_send_msg2pf(p_hwfn, &resp->hdr.status, resp_size);
 	if (rc)
-		return rc;
+		goto exit;
 
-	if (resp->hdr.status != PFVF_STATUS_SUCCESS)
-		return -EINVAL;
+	if (resp->hdr.status != PFVF_STATUS_SUCCESS) {
+		rc = -EINVAL;
+		goto exit;
+	}
 
 	qed_vf_handle_vp_update_tlvs_resp(p_hwfn, p_params);
 
+exit:
+	qed_vf_pf_req_end(p_hwfn, rc);
+
 	return rc;
 }
 
@@ -797,14 +895,19 @@ int qed_vf_pf_reset(struct qed_hwfn *p_hwfn)
 	resp = &p_iov->pf2vf_reply->default_resp;
 	rc = qed_send_msg2pf(p_hwfn, &resp->hdr.status, sizeof(*resp));
 	if (rc)
-		return rc;
+		goto exit;
 
-	if (resp->hdr.status != PFVF_STATUS_SUCCESS)
-		return -EAGAIN;
+	if (resp->hdr.status != PFVF_STATUS_SUCCESS) {
+		rc = -EAGAIN;
+		goto exit;
+	}
 
 	p_hwfn->b_int_enabled = 0;
 
-	return 0;
+exit:
+	qed_vf_pf_req_end(p_hwfn, rc);
+
+	return rc;
 }
 
 int qed_vf_pf_release(struct qed_hwfn *p_hwfn)
@@ -828,6 +931,8 @@ int qed_vf_pf_release(struct qed_hwfn *p_hwfn)
 	if (!rc && resp->hdr.status != PFVF_STATUS_SUCCESS)
 		rc = -EAGAIN;
 
+	qed_vf_pf_req_end(p_hwfn, rc);
+
 	p_hwfn->b_int_enabled = 0;
 
 	if (p_iov->vf2pf_request)
@@ -896,12 +1001,17 @@ int qed_vf_pf_filter_ucast(struct qed_hwfn *p_hwfn,
 	resp = &p_iov->pf2vf_reply->default_resp;
 	rc = qed_send_msg2pf(p_hwfn, &resp->hdr.status, sizeof(*resp));
 	if (rc)
-		return rc;
+		goto exit;
 
-	if (resp->hdr.status != PFVF_STATUS_SUCCESS)
-		return -EAGAIN;
+	if (resp->hdr.status != PFVF_STATUS_SUCCESS) {
+		rc = -EAGAIN;
+		goto exit;
+	}
 
-	return 0;
+exit:
+	qed_vf_pf_req_end(p_hwfn, rc);
+
+	return rc;
 }
 
 int qed_vf_pf_int_cleanup(struct qed_hwfn *p_hwfn)
@@ -920,12 +1030,17 @@ int qed_vf_pf_int_cleanup(struct qed_hwfn *p_hwfn)
 
 	rc = qed_send_msg2pf(p_hwfn, &resp->hdr.status, sizeof(*resp));
 	if (rc)
-		return rc;
+		goto exit;
 
-	if (resp->hdr.status != PFVF_STATUS_SUCCESS)
-		return -EINVAL;
+	if (resp->hdr.status != PFVF_STATUS_SUCCESS) {
+		rc = -EINVAL;
+		goto exit;
+	}
 
-	return 0;
+exit:
+	qed_vf_pf_req_end(p_hwfn, rc);
+
+	return rc;
 }
 
 u16 qed_vf_get_igu_sb_id(struct qed_hwfn *p_hwfn, u16 sb_id)
@@ -1071,8 +1186,8 @@ bool qed_vf_check_mac(struct qed_hwfn *p_hwfn, u8 *mac)
 	return false;
 }
 
-bool qed_vf_bulletin_get_forced_mac(struct qed_hwfn *hwfn,
-				    u8 *dst_mac, u8 *p_is_forced)
+static bool qed_vf_bulletin_get_forced_mac(struct qed_hwfn *hwfn,
+					   u8 *dst_mac, u8 *p_is_forced)
 {
 	struct qed_bulletin_content *bulletin;
 
diff --git a/drivers/net/ethernet/qlogic/qed/qed_vf.h b/drivers/net/ethernet/qlogic/qed/qed_vf.h
index b23ce58e932f..35db7a28aa13 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_vf.h
+++ b/drivers/net/ethernet/qlogic/qed/qed_vf.h
@@ -86,7 +86,7 @@ struct vfpf_acquire_tlv {
 	struct vfpf_first_tlv first_tlv;
 
 	struct vf_pf_vfdev_info {
-#define VFPF_ACQUIRE_CAP_OBSOLETE	(1 << 0)
+#define VFPF_ACQUIRE_CAP_PRE_FP_HSI     (1 << 0) /* VF pre-FP hsi version */
 #define VFPF_ACQUIRE_CAP_100G		(1 << 1) /* VF can support 100g */
 		u64 capabilities;
 		u8 fw_major;
@@ -551,6 +551,11 @@ struct qed_vf_iov {
 
 	/* we set aside a copy of the acquire response */
 	struct pfvf_acquire_resp_tlv acquire_resp;
+
+	/* In case PF originates prior to the fp-hsi version comparison,
+	 * this has to be propagated as it affects the fastpath.
+	 */
+	bool b_pre_fp_hsi;
 };
 
 #ifdef CONFIG_QED_SRIOV
diff --git a/drivers/net/ethernet/qlogic/qede/Makefile b/drivers/net/ethernet/qlogic/qede/Makefile
index 74a49850d74d..28dc58919c85 100644
--- a/drivers/net/ethernet/qlogic/qede/Makefile
+++ b/drivers/net/ethernet/qlogic/qede/Makefile
@@ -2,3 +2,4 @@ obj-$(CONFIG_QEDE) := qede.o
 
 qede-y := qede_main.o qede_ethtool.o
 qede-$(CONFIG_DCB) += qede_dcbnl.o
+qede-$(CONFIG_INFINIBAND_QEDR) += qede_roce.o
diff --git a/drivers/net/ethernet/qlogic/qede/qede.h b/drivers/net/ethernet/qlogic/qede/qede.h
index 02b06d4e40ae..28c0e9f42c9e 100644
--- a/drivers/net/ethernet/qlogic/qede/qede.h
+++ b/drivers/net/ethernet/qlogic/qede/qede.h
@@ -25,7 +25,7 @@
 
 #define QEDE_MAJOR_VERSION		8
 #define QEDE_MINOR_VERSION		10
-#define QEDE_REVISION_VERSION		1
+#define QEDE_REVISION_VERSION		9
 #define QEDE_ENGINEERING_VERSION	20
 #define DRV_MODULE_VERSION __stringify(QEDE_MAJOR_VERSION) "."	\
 		__stringify(QEDE_MINOR_VERSION) "."		\
@@ -36,6 +36,8 @@
 
 struct qede_stats {
 	u64 no_buff_discards;
+	u64 packet_too_big_discard;
+	u64 ttl0_discard;
 	u64 rx_ucast_bytes;
 	u64 rx_mcast_bytes;
 	u64 rx_bcast_bytes;
@@ -104,6 +106,13 @@ struct qede_vlan {
 	bool configured;
 };
 
+struct qede_rdma_dev {
+	struct qedr_dev *qedr_dev;
+	struct list_head entry;
+	struct list_head roce_event_list;
+	struct workqueue_struct *roce_wq;
+};
+
 struct qede_dev {
 	struct qed_dev			*cdev;
 	struct net_device		*ndev;
@@ -124,16 +133,22 @@ struct qede_dev {
 				 (edev)->dev_info.num_tc)
 
 	struct qede_fastpath		*fp_array;
-	u16				req_rss;
-	u16				num_rss;
+	u8				req_num_tx;
+	u8				fp_num_tx;
+	u8				req_num_rx;
+	u8				fp_num_rx;
+	u16				req_queues;
+	u16				num_queues;
 	u8				num_tc;
-#define QEDE_RSS_CNT(edev)		((edev)->num_rss)
-#define QEDE_TSS_CNT(edev)		((edev)->num_rss *	\
-					 (edev)->num_tc)
-#define QEDE_TSS_IDX(edev, txqidx)	((txqidx) % (edev)->num_rss)
-#define QEDE_TC_IDX(edev, txqidx)	((txqidx) / (edev)->num_rss)
+#define QEDE_QUEUE_CNT(edev)	((edev)->num_queues)
+#define QEDE_RSS_COUNT(edev)	((edev)->num_queues - (edev)->fp_num_tx)
+#define QEDE_TSS_COUNT(edev)	(((edev)->num_queues - (edev)->fp_num_rx) * \
+				 (edev)->num_tc)
+#define QEDE_TX_IDX(edev, txqidx)	((edev)->fp_num_rx + (txqidx) % \
+					 QEDE_TSS_COUNT(edev))
+#define QEDE_TC_IDX(edev, txqidx)	((txqidx) / QEDE_TSS_COUNT(edev))
 #define QEDE_TX_QUEUE(edev, txqidx)	\
-	(&(edev)->fp_array[QEDE_TSS_IDX((edev), (txqidx))].txqs[QEDE_TC_IDX( \
+	(&(edev)->fp_array[QEDE_TX_IDX((edev), (txqidx))].txqs[QEDE_TC_IDX(\
 							(edev), (txqidx))])
 
 	struct qed_int_info		int_info;
@@ -177,6 +192,8 @@ struct qede_dev {
 	unsigned long			sp_flags;
 	u16				vxlan_dst_port;
 	u16				geneve_dst_port;
+
+	struct qede_rdma_dev		rdma_info;
 };
 
 enum QEDE_STATE {
@@ -235,6 +252,7 @@ struct qede_rx_queue {
 	u16			num_rx_buffers;
 	u16			rxq_id;
 
+	u64			rcv_pkts;
 	u64			rx_hw_errors;
 	u64			rx_alloc_errors;
 	u64			rx_ip_frags;
@@ -263,6 +281,10 @@ struct qede_tx_queue {
 	union db_prod		tx_db;
 
 	u16			num_tx_buffers;
+	u64			xmit_pkts;
+	u64			stopped_cnt;
+
+	bool			is_legacy;
 };
 
 #define BD_UNMAP_ADDR(bd)		HILO_U64(le32_to_cpu((bd)->addr.hi), \
@@ -277,7 +299,11 @@ struct qede_tx_queue {
 
 struct qede_fastpath {
 	struct qede_dev	*edev;
-	u8			rss_id;
+#define QEDE_FASTPATH_TX	BIT(0)
+#define QEDE_FASTPATH_RX	BIT(1)
+#define QEDE_FASTPATH_COMBINED	(QEDE_FASTPATH_TX | QEDE_FASTPATH_RX)
+	u8			type;
+	u8			id;
 	struct napi_struct	napi;
 	struct qed_sb_info	*sb_info;
 	struct qede_rx_queue	*rxq;
@@ -337,6 +363,6 @@ void qede_recycle_rx_bd_ring(struct qede_rx_queue *rxq, struct qede_dev *edev,
 
 #define QEDE_MIN_PKT_LEN	64
 #define QEDE_RX_HDR_SIZE	256
-#define	for_each_rss(i) for (i = 0; i < edev->num_rss; i++)
+#define	for_each_queue(i) for (i = 0; i < edev->num_queues; i++)
 
 #endif /* _QEDE_H_ */
diff --git a/drivers/net/ethernet/qlogic/qede/qede_ethtool.c b/drivers/net/ethernet/qlogic/qede/qede_ethtool.c
index f8492cac9290..25a9b293ee8f 100644
--- a/drivers/net/ethernet/qlogic/qede/qede_ethtool.c
+++ b/drivers/net/ethernet/qlogic/qede/qede_ethtool.c
@@ -35,6 +35,7 @@ static const struct {
 	u64 offset;
 	char string[ETH_GSTRING_LEN];
 } qede_rqstats_arr[] = {
+	QEDE_RQSTAT(rcv_pkts),
 	QEDE_RQSTAT(rx_hw_errors),
 	QEDE_RQSTAT(rx_alloc_errors),
 	QEDE_RQSTAT(rx_ip_frags),
@@ -44,6 +45,24 @@ static const struct {
 #define QEDE_RQSTATS_DATA(dev, sindex, rqindex) \
 	(*((u64 *)(((char *)(dev->fp_array[(rqindex)].rxq)) +\
 		    qede_rqstats_arr[(sindex)].offset)))
+#define QEDE_TQSTAT_OFFSET(stat_name) \
+	(offsetof(struct qede_tx_queue, stat_name))
+#define QEDE_TQSTAT_STRING(stat_name) (#stat_name)
+#define QEDE_TQSTAT(stat_name) \
+	{QEDE_TQSTAT_OFFSET(stat_name), QEDE_TQSTAT_STRING(stat_name)}
+#define QEDE_NUM_TQSTATS ARRAY_SIZE(qede_tqstats_arr)
+static const struct {
+	u64 offset;
+	char string[ETH_GSTRING_LEN];
+} qede_tqstats_arr[] = {
+	QEDE_TQSTAT(xmit_pkts),
+	QEDE_TQSTAT(stopped_cnt),
+};
+
+#define QEDE_TQSTATS_DATA(dev, sindex, tssid, tcid) \
+	(*((u64 *)(((void *)(&dev->fp_array[tssid].txqs[tcid])) +\
+		   qede_tqstats_arr[(sindex)].offset)))
+
 static const struct {
 	u64 offset;
 	char string[ETH_GSTRING_LEN];
@@ -107,6 +126,8 @@ static const struct {
 	QEDE_PF_STAT(mftag_filter_discards),
 	QEDE_PF_STAT(mac_filter_discards),
 	QEDE_STAT(tx_err_drop_pkts),
+	QEDE_STAT(ttl0_discard),
+	QEDE_STAT(packet_too_big_discard),
 
 	QEDE_STAT(coalesced_pkts),
 	QEDE_STAT(coalesced_events),
@@ -151,17 +172,29 @@ static void qede_get_strings_stats(struct qede_dev *edev, u8 *buf)
 {
 	int i, j, k;
 
+	for (i = 0, k = 0; i < QEDE_QUEUE_CNT(edev); i++) {
+		int tc;
+
+		for (j = 0; j < QEDE_NUM_RQSTATS; j++)
+			sprintf(buf + (k + j) * ETH_GSTRING_LEN,
+				"%d:   %s", i, qede_rqstats_arr[j].string);
+		k += QEDE_NUM_RQSTATS;
+		for (tc = 0; tc < edev->num_tc; tc++) {
+			for (j = 0; j < QEDE_NUM_TQSTATS; j++)
+				sprintf(buf + (k + j) * ETH_GSTRING_LEN,
+					"%d.%d: %s", i, tc,
+					qede_tqstats_arr[j].string);
+			k += QEDE_NUM_TQSTATS;
+		}
+	}
+
 	for (i = 0, j = 0; i < QEDE_NUM_STATS; i++) {
 		if (IS_VF(edev) && qede_stats_arr[i].pf_only)
 			continue;
-		strcpy(buf + j * ETH_GSTRING_LEN,
+		strcpy(buf + (k + j) * ETH_GSTRING_LEN,
 		       qede_stats_arr[i].string);
 		j++;
 	}
-
-	for (k = 0; k < QEDE_NUM_RQSTATS; k++, j++)
-		strcpy(buf + j * ETH_GSTRING_LEN,
-		       qede_rqstats_arr[k].string);
 }
 
 static void qede_get_strings(struct net_device *dev, u32 stringset, u8 *buf)
@@ -197,19 +230,30 @@ static void qede_get_ethtool_stats(struct net_device *dev,
 
 	mutex_lock(&edev->qede_lock);
 
+	for (qid = 0; qid < QEDE_QUEUE_CNT(edev); qid++) {
+		int tc;
+
+		if (edev->fp_array[qid].type & QEDE_FASTPATH_RX) {
+			for (sidx = 0; sidx < QEDE_NUM_RQSTATS; sidx++)
+				buf[cnt++] = QEDE_RQSTATS_DATA(edev, sidx, qid);
+		}
+
+		if (edev->fp_array[qid].type & QEDE_FASTPATH_TX) {
+			for (tc = 0; tc < edev->num_tc; tc++) {
+				for (sidx = 0; sidx < QEDE_NUM_TQSTATS; sidx++)
+					buf[cnt++] = QEDE_TQSTATS_DATA(edev,
+								       sidx,
+								       qid, tc);
+			}
+		}
+	}
+
 	for (sidx = 0; sidx < QEDE_NUM_STATS; sidx++) {
 		if (IS_VF(edev) && qede_stats_arr[sidx].pf_only)
 			continue;
 		buf[cnt++] = QEDE_STATS_DATA(edev, sidx);
 	}
 
-	for (sidx = 0; sidx < QEDE_NUM_RQSTATS; sidx++) {
-		buf[cnt] = 0;
-		for (qid = 0; qid < edev->num_rss; qid++)
-			buf[cnt] += QEDE_RQSTATS_DATA(edev, sidx, qid);
-		cnt++;
-	}
-
 	mutex_unlock(&edev->qede_lock);
 }
 
@@ -227,7 +271,8 @@ static int qede_get_sset_count(struct net_device *dev, int stringset)
 				if (qede_stats_arr[i].pf_only)
 					num_stats--;
 		}
-		return num_stats + QEDE_NUM_RQSTATS;
+		return num_stats + QEDE_RSS_COUNT(edev) * QEDE_NUM_RQSTATS +
+		       QEDE_TSS_COUNT(edev) * QEDE_NUM_TQSTATS * edev->num_tc;
 	case ETH_SS_PRIV_FLAGS:
 		return QEDE_PRI_FLAG_LEN;
 	case ETH_SS_TEST:
@@ -249,78 +294,150 @@ static u32 qede_get_priv_flags(struct net_device *dev)
 	return (!!(edev->dev_info.common.num_hwfns > 1)) << QEDE_PRI_FLAG_CMT;
 }
 
-static int qede_get_settings(struct net_device *dev, struct ethtool_cmd *cmd)
+struct qede_link_mode_mapping {
+	u32 qed_link_mode;
+	u32 ethtool_link_mode;
+};
+
+static const struct qede_link_mode_mapping qed_lm_map[] = {
+	{QED_LM_FIBRE_BIT, ETHTOOL_LINK_MODE_FIBRE_BIT},
+	{QED_LM_Autoneg_BIT, ETHTOOL_LINK_MODE_Autoneg_BIT},
+	{QED_LM_Asym_Pause_BIT, ETHTOOL_LINK_MODE_Asym_Pause_BIT},
+	{QED_LM_Pause_BIT, ETHTOOL_LINK_MODE_Pause_BIT},
+	{QED_LM_1000baseT_Half_BIT, ETHTOOL_LINK_MODE_1000baseT_Half_BIT},
+	{QED_LM_1000baseT_Full_BIT, ETHTOOL_LINK_MODE_1000baseT_Full_BIT},
+	{QED_LM_10000baseKR_Full_BIT, ETHTOOL_LINK_MODE_10000baseKR_Full_BIT},
+	{QED_LM_25000baseKR_Full_BIT, ETHTOOL_LINK_MODE_25000baseKR_Full_BIT},
+	{QED_LM_40000baseLR4_Full_BIT, ETHTOOL_LINK_MODE_40000baseLR4_Full_BIT},
+	{QED_LM_50000baseKR2_Full_BIT, ETHTOOL_LINK_MODE_50000baseKR2_Full_BIT},
+	{QED_LM_100000baseKR4_Full_BIT,
+	 ETHTOOL_LINK_MODE_100000baseKR4_Full_BIT},
+};
+
+#define QEDE_DRV_TO_ETHTOOL_CAPS(caps, lk_ksettings, name)	\
+{								\
+	int i;							\
+								\
+	for (i = 0; i < QED_LM_COUNT; i++) {			\
+		if ((caps) & (qed_lm_map[i].qed_link_mode))	\
+			__set_bit(qed_lm_map[i].ethtool_link_mode,\
+				  lk_ksettings->link_modes.name); \
+	}							\
+}
+
+#define QEDE_ETHTOOL_TO_DRV_CAPS(caps, lk_ksettings, name)	\
+{								\
+	int i;							\
+								\
+	for (i = 0; i < QED_LM_COUNT; i++) {			\
+		if (test_bit(qed_lm_map[i].ethtool_link_mode,	\
+			     lk_ksettings->link_modes.name))	\
+			caps |= qed_lm_map[i].qed_link_mode;	\
+	}							\
+}
+
+static int qede_get_link_ksettings(struct net_device *dev,
+				   struct ethtool_link_ksettings *cmd)
 {
+	struct ethtool_link_settings *base = &cmd->base;
 	struct qede_dev *edev = netdev_priv(dev);
 	struct qed_link_output current_link;
 
 	memset(&current_link, 0, sizeof(current_link));
 	edev->ops->common->get_link(edev->cdev, &current_link);
 
-	cmd->supported = current_link.supported_caps;
-	cmd->advertising = current_link.advertised_caps;
+	ethtool_link_ksettings_zero_link_mode(cmd, supported);
+	QEDE_DRV_TO_ETHTOOL_CAPS(current_link.supported_caps, cmd, supported)
+
+	ethtool_link_ksettings_zero_link_mode(cmd, advertising);
+	QEDE_DRV_TO_ETHTOOL_CAPS(current_link.advertised_caps, cmd, advertising)
+
+	ethtool_link_ksettings_zero_link_mode(cmd, lp_advertising);
+	QEDE_DRV_TO_ETHTOOL_CAPS(current_link.lp_caps, cmd, lp_advertising)
+
 	if ((edev->state == QEDE_STATE_OPEN) && (current_link.link_up)) {
-		ethtool_cmd_speed_set(cmd, current_link.speed);
-		cmd->duplex = current_link.duplex;
+		base->speed = current_link.speed;
+		base->duplex = current_link.duplex;
 	} else {
-		cmd->duplex = DUPLEX_UNKNOWN;
-		ethtool_cmd_speed_set(cmd, SPEED_UNKNOWN);
+		base->speed = SPEED_UNKNOWN;
+		base->duplex = DUPLEX_UNKNOWN;
 	}
-	cmd->port = current_link.port;
-	cmd->autoneg = (current_link.autoneg) ? AUTONEG_ENABLE :
-						AUTONEG_DISABLE;
-	cmd->lp_advertising = current_link.lp_caps;
+	base->port = current_link.port;
+	base->autoneg = (current_link.autoneg) ? AUTONEG_ENABLE :
+			AUTONEG_DISABLE;
 
 	return 0;
 }
 
-static int qede_set_settings(struct net_device *dev, struct ethtool_cmd *cmd)
+static int qede_set_link_ksettings(struct net_device *dev,
+				   const struct ethtool_link_ksettings *cmd)
 {
+	const struct ethtool_link_settings *base = &cmd->base;
 	struct qede_dev *edev = netdev_priv(dev);
 	struct qed_link_output current_link;
 	struct qed_link_params params;
-	u32 speed;
 
 	if (!edev->ops || !edev->ops->common->can_link_change(edev->cdev)) {
-		DP_INFO(edev,
-			"Link settings are not allowed to be changed\n");
+		DP_INFO(edev, "Link settings are not allowed to be changed\n");
 		return -EOPNOTSUPP;
 	}
-
 	memset(&current_link, 0, sizeof(current_link));
 	memset(&params, 0, sizeof(params));
 	edev->ops->common->get_link(edev->cdev, &current_link);
 
-	speed = ethtool_cmd_speed(cmd);
 	params.override_flags |= QED_LINK_OVERRIDE_SPEED_ADV_SPEEDS;
 	params.override_flags |= QED_LINK_OVERRIDE_SPEED_AUTONEG;
-	if (cmd->autoneg == AUTONEG_ENABLE) {
+	if (base->autoneg == AUTONEG_ENABLE) {
 		params.autoneg = true;
 		params.forced_speed = 0;
-		params.adv_speeds = cmd->advertising;
-	} else { /* forced speed */
+		QEDE_ETHTOOL_TO_DRV_CAPS(params.adv_speeds, cmd, advertising)
+	} else {		/* forced speed */
 		params.override_flags |= QED_LINK_OVERRIDE_SPEED_FORCED_SPEED;
 		params.autoneg = false;
-		params.forced_speed = speed;
-		switch (speed) {
+		params.forced_speed = base->speed;
+		switch (base->speed) {
 		case SPEED_10000:
 			if (!(current_link.supported_caps &
-			    SUPPORTED_10000baseKR_Full)) {
+			      QED_LM_10000baseKR_Full_BIT)) {
 				DP_INFO(edev, "10G speed not supported\n");
 				return -EINVAL;
 			}
-			params.adv_speeds = SUPPORTED_10000baseKR_Full;
+			params.adv_speeds = QED_LM_10000baseKR_Full_BIT;
+			break;
+		case SPEED_25000:
+			if (!(current_link.supported_caps &
+			      QED_LM_25000baseKR_Full_BIT)) {
+				DP_INFO(edev, "25G speed not supported\n");
+				return -EINVAL;
+			}
+			params.adv_speeds = QED_LM_25000baseKR_Full_BIT;
 			break;
 		case SPEED_40000:
 			if (!(current_link.supported_caps &
-			    SUPPORTED_40000baseLR4_Full)) {
+			      QED_LM_40000baseLR4_Full_BIT)) {
 				DP_INFO(edev, "40G speed not supported\n");
 				return -EINVAL;
 			}
-			params.adv_speeds = SUPPORTED_40000baseLR4_Full;
+			params.adv_speeds = QED_LM_40000baseLR4_Full_BIT;
+			break;
+		case SPEED_50000:
+			if (!(current_link.supported_caps &
+			      QED_LM_50000baseKR2_Full_BIT)) {
+				DP_INFO(edev, "50G speed not supported\n");
+				return -EINVAL;
+			}
+			params.adv_speeds = QED_LM_50000baseKR2_Full_BIT;
+			break;
+		case SPEED_100000:
+			if (!(current_link.supported_caps &
+			      QED_LM_100000baseKR4_Full_BIT)) {
+				DP_INFO(edev, "100G speed not supported\n");
+				return -EINVAL;
+			}
+			params.adv_speeds = QED_LM_100000baseKR4_Full_BIT;
 			break;
 		default:
-			DP_INFO(edev, "Unsupported speed %u\n", speed);
+			DP_INFO(edev, "Unsupported speed %u\n", base->speed);
 			return -EINVAL;
 		}
 	}
@@ -368,8 +485,7 @@ static u32 qede_get_msglevel(struct net_device *ndev)
 {
 	struct qede_dev *edev = netdev_priv(ndev);
 
-	return ((u32)edev->dp_level << QED_LOG_LEVEL_SHIFT) |
-	       edev->dp_module;
+	return ((u32)edev->dp_level << QED_LOG_LEVEL_SHIFT) | edev->dp_module;
 }
 
 static void qede_set_msglevel(struct net_device *ndev, u32 level)
@@ -393,8 +509,7 @@ static int qede_nway_reset(struct net_device *dev)
 	struct qed_link_params link_params;
 
 	if (!edev->ops || !edev->ops->common->can_link_change(edev->cdev)) {
-		DP_INFO(edev,
-			"Link settings are not allowed to be changed\n");
+		DP_INFO(edev, "Link settings are not allowed to be changed\n");
 		return -EOPNOTSUPP;
 	}
 
@@ -467,7 +582,7 @@ static int qede_set_coalesce(struct net_device *dev,
 
 	rxc = (u16)coal->rx_coalesce_usecs;
 	txc = (u16)coal->tx_coalesce_usecs;
-	for_each_rss(i) {
+	for_each_queue(i) {
 		sb_id = edev->fp_array[i].sb_info->igu_sb_id;
 		rc = edev->ops->common->set_coalesce(edev->cdev, rxc, txc,
 						     (u8)i, sb_id);
@@ -563,7 +678,7 @@ static int qede_set_pauseparam(struct net_device *dev,
 	memset(&params, 0, sizeof(params));
 	params.override_flags |= QED_LINK_OVERRIDE_PAUSE_CONFIG;
 	if (epause->autoneg) {
-		if (!(current_link.supported_caps & SUPPORTED_Autoneg)) {
+		if (!(current_link.supported_caps & QED_LM_Autoneg_BIT)) {
 			DP_INFO(edev, "autoneg not supported\n");
 			return -EINVAL;
 		}
@@ -580,6 +695,28 @@ static int qede_set_pauseparam(struct net_device *dev,
 	return 0;
 }
 
+static void qede_get_regs(struct net_device *ndev,
+			  struct ethtool_regs *regs, void *buffer)
+{
+	struct qede_dev *edev = netdev_priv(ndev);
+
+	regs->version = 0;
+	memset(buffer, 0, regs->len);
+
+	if (edev->ops && edev->ops->common)
+		edev->ops->common->dbg_all_data(edev->cdev, buffer);
+}
+
+static int qede_get_regs_len(struct net_device *ndev)
+{
+	struct qede_dev *edev = netdev_priv(ndev);
+
+	if (edev->ops && edev->ops->common)
+		return edev->ops->common->dbg_all_data_size(edev->cdev);
+	else
+		return -EINVAL;
+}
+
 static void qede_update_mtu(struct qede_dev *edev, union qede_reload_args *args)
 {
 	edev->ndev->mtu = args->mtu;
@@ -619,45 +756,70 @@ static void qede_get_channels(struct net_device *dev,
 	struct qede_dev *edev = netdev_priv(dev);
 
 	channels->max_combined = QEDE_MAX_RSS_CNT(edev);
-	channels->combined_count = QEDE_RSS_CNT(edev);
+	channels->combined_count = QEDE_QUEUE_CNT(edev) - edev->fp_num_tx -
+					edev->fp_num_rx;
+	channels->tx_count = edev->fp_num_tx;
+	channels->rx_count = edev->fp_num_rx;
 }
 
 static int qede_set_channels(struct net_device *dev,
 			     struct ethtool_channels *channels)
 {
 	struct qede_dev *edev = netdev_priv(dev);
+	u32 count;
 
 	DP_VERBOSE(edev, (NETIF_MSG_IFUP | NETIF_MSG_IFDOWN),
 		   "set-channels command parameters: rx = %d, tx = %d, other = %d, combined = %d\n",
 		   channels->rx_count, channels->tx_count,
 		   channels->other_count, channels->combined_count);
 
-	/* We don't support separate rx / tx, nor `other' channels. */
-	if (channels->rx_count || channels->tx_count ||
-	    channels->other_count || (channels->combined_count == 0) ||
-	    (channels->combined_count > QEDE_MAX_RSS_CNT(edev))) {
+	count = channels->rx_count + channels->tx_count +
+			channels->combined_count;
+
+	/* We don't support `other' channels */
+	if (channels->other_count) {
 		DP_VERBOSE(edev, (NETIF_MSG_IFUP | NETIF_MSG_IFDOWN),
 			   "command parameters not supported\n");
 		return -EINVAL;
 	}
 
+	if (!(channels->combined_count || (channels->rx_count &&
+					   channels->tx_count))) {
+		DP_VERBOSE(edev, (NETIF_MSG_IFUP | NETIF_MSG_IFDOWN),
+			   "need to request at least one transmit and one receive channel\n");
+		return -EINVAL;
+	}
+
+	if (count > QEDE_MAX_RSS_CNT(edev)) {
+		DP_VERBOSE(edev, (NETIF_MSG_IFUP | NETIF_MSG_IFDOWN),
+			   "requested channels = %d max supported channels = %d\n",
+			   count, QEDE_MAX_RSS_CNT(edev));
+		return -EINVAL;
+	}
+
 	/* Check if there was a change in the active parameters */
-	if (channels->combined_count == QEDE_RSS_CNT(edev)) {
+	if ((count == QEDE_QUEUE_CNT(edev)) &&
+	    (channels->tx_count == edev->fp_num_tx) &&
+	    (channels->rx_count == edev->fp_num_rx)) {
 		DP_VERBOSE(edev, (NETIF_MSG_IFUP | NETIF_MSG_IFDOWN),
 			   "No change in active parameters\n");
 		return 0;
 	}
 
 	/* We need the number of queues to be divisible between the hwfns */
-	if (channels->combined_count % edev->dev_info.common.num_hwfns) {
+	if ((count % edev->dev_info.common.num_hwfns) ||
+	    (channels->tx_count % edev->dev_info.common.num_hwfns) ||
+	    (channels->rx_count % edev->dev_info.common.num_hwfns)) {
 		DP_VERBOSE(edev, (NETIF_MSG_IFUP | NETIF_MSG_IFDOWN),
-			   "Number of channels must be divisable by %04x\n",
+			   "Number of channels must be divisible by %04x\n",
 			   edev->dev_info.common.num_hwfns);
 		return -EINVAL;
 	}
 
 	/* Set number of queues and reload if necessary */
-	edev->req_rss = channels->combined_count;
+	edev->req_queues = count;
+	edev->req_num_tx = channels->tx_count;
+	edev->req_num_rx = channels->rx_count;
 	if (netif_running(dev))
 		qede_reload(edev, NULL, NULL);
 
@@ -727,7 +889,7 @@ static int qede_get_rxnfc(struct net_device *dev, struct ethtool_rxnfc *info,
 
 	switch (info->cmd) {
 	case ETHTOOL_GRXRINGS:
-		info->data = edev->num_rss;
+		info->data = QEDE_RSS_COUNT(edev);
 		return 0;
 	case ETHTOOL_GRXFH:
 		return qede_get_rss_flags(edev, info);
@@ -930,7 +1092,7 @@ static void qede_netif_start(struct qede_dev *edev)
 	if (!netif_running(edev->ndev))
 		return;
 
-	for_each_rss(i) {
+	for_each_queue(i) {
 		/* Update and reenable interrupts */
 		qed_sb_ack(edev->fp_array[i].sb_info, IGU_INT_ENABLE, 1);
 		napi_enable(&edev->fp_array[i].napi);
@@ -942,7 +1104,7 @@ static void qede_netif_stop(struct qede_dev *edev)
 {
 	int i;
 
-	for_each_rss(i) {
+	for_each_queue(i) {
 		napi_disable(&edev->fp_array[i].napi);
 		/* Disable interrupts */
 		qed_sb_ack(edev->fp_array[i].sb_info, IGU_INT_DISABLE, 0);
@@ -952,11 +1114,23 @@ static void qede_netif_stop(struct qede_dev *edev)
 static int qede_selftest_transmit_traffic(struct qede_dev *edev,
 					  struct sk_buff *skb)
 {
-	struct qede_tx_queue *txq = &edev->fp_array[0].txqs[0];
+	struct qede_tx_queue *txq = NULL;
 	struct eth_tx_1st_bd *first_bd;
 	dma_addr_t mapping;
 	int i, idx, val;
 
+	for_each_queue(i) {
+		if (edev->fp_array[i].type & QEDE_FASTPATH_TX) {
+			txq = edev->fp_array[i].txqs;
+			break;
+		}
+	}
+
+	if (!txq) {
+		DP_NOTICE(edev, "Tx path is not available\n");
+		return -1;
+	}
+
 	/* Fill the entry in the SW ring and the BDs in the FW ring */
 	idx = txq->sw_tx_prod & NUM_TX_BDS_MAX;
 	txq->sw_tx_ring[idx].skb = skb;
@@ -1020,14 +1194,26 @@ static int qede_selftest_transmit_traffic(struct qede_dev *edev,
 
 static int qede_selftest_receive_traffic(struct qede_dev *edev)
 {
-	struct qede_rx_queue *rxq = edev->fp_array[0].rxq;
 	u16 hw_comp_cons, sw_comp_cons, sw_rx_index, len;
 	struct eth_fast_path_rx_reg_cqe *fp_cqe;
+	struct qede_rx_queue *rxq = NULL;
 	struct sw_rx_data *sw_rx_data;
 	union eth_rx_cqe *cqe;
 	u8 *data_ptr;
 	int i;
 
+	for_each_queue(i) {
+		if (edev->fp_array[i].type & QEDE_FASTPATH_RX) {
+			rxq = edev->fp_array[i].rxq;
+			break;
+		}
+	}
+
+	if (!rxq) {
+		DP_NOTICE(edev, "Rx path is not available\n");
+		return -1;
+	}
+
 	/* The packet is expected to receive on rx-queue 0 even though RSS is
 	 * enabled. This is because the queue 0 is configured as the default
 	 * queue and that the loopback traffic is not IP.
@@ -1228,9 +1414,11 @@ static int qede_get_tunable(struct net_device *dev,
 }
 
 static const struct ethtool_ops qede_ethtool_ops = {
-	.get_settings = qede_get_settings,
-	.set_settings = qede_set_settings,
+	.get_link_ksettings = qede_get_link_ksettings,
+	.set_link_ksettings = qede_set_link_ksettings,
 	.get_drvinfo = qede_get_drvinfo,
+	.get_regs_len = qede_get_regs_len,
+	.get_regs = qede_get_regs,
 	.get_msglevel = qede_get_msglevel,
 	.set_msglevel = qede_set_msglevel,
 	.nway_reset = qede_nway_reset,
@@ -1260,7 +1448,7 @@ static const struct ethtool_ops qede_ethtool_ops = {
 };
 
 static const struct ethtool_ops qede_vf_ethtool_ops = {
-	.get_settings = qede_get_settings,
+	.get_link_ksettings = qede_get_link_ksettings,
 	.get_drvinfo = qede_get_drvinfo,
 	.get_msglevel = qede_get_msglevel,
 	.set_msglevel = qede_set_msglevel,
diff --git a/drivers/net/ethernet/qlogic/qede/qede_main.c b/drivers/net/ethernet/qlogic/qede/qede_main.c
index 9544e4c41359..343038ca047d 100644
--- a/drivers/net/ethernet/qlogic/qede/qede_main.c
+++ b/drivers/net/ethernet/qlogic/qede/qede_main.c
@@ -36,7 +36,7 @@
 #include <linux/random.h>
 #include <net/ip6_checksum.h>
 #include <linux/bitops.h>
-
+#include <linux/qed/qede_roce.h>
 #include "qede.h"
 
 static char version[] =
@@ -100,7 +100,8 @@ static int qede_alloc_rx_buffer(struct qede_dev *edev,
 static void qede_link_update(void *dev, struct qed_link_output *link);
 
 #ifdef CONFIG_QED_SRIOV
-static int qede_set_vf_vlan(struct net_device *ndev, int vf, u16 vlan, u8 qos)
+static int qede_set_vf_vlan(struct net_device *ndev, int vf, u16 vlan, u8 qos,
+			    __be16 vlan_proto)
 {
 	struct qede_dev *edev = netdev_priv(ndev);
 
@@ -109,6 +110,9 @@ static int qede_set_vf_vlan(struct net_device *ndev, int vf, u16 vlan, u8 qos)
 		return -EINVAL;
 	}
 
+	if (vlan_proto != htons(ETH_P_8021Q))
+		return -EPROTONOSUPPORT;
+
 	DP_VERBOSE(edev, QED_MSG_IOV, "Setting Vlan 0x%04x to VF [%d]\n",
 		   vlan, vf);
 
@@ -189,8 +193,7 @@ static int qede_netdev_event(struct notifier_block *this, unsigned long event,
 	struct ethtool_drvinfo drvinfo;
 	struct qede_dev *edev;
 
-	/* Currently only support name change */
-	if (event != NETDEV_CHANGENAME)
+	if (event != NETDEV_CHANGENAME && event != NETDEV_CHANGEADDR)
 		goto done;
 
 	/* Check whether this is a qede device */
@@ -203,11 +206,18 @@ static int qede_netdev_event(struct notifier_block *this, unsigned long event,
 		goto done;
 	edev = netdev_priv(ndev);
 
-	/* Notify qed of the name change */
-	if (!edev->ops || !edev->ops->common)
-		goto done;
-	edev->ops->common->set_id(edev->cdev, edev->ndev->name,
-				  "qede");
+	switch (event) {
+	case NETDEV_CHANGENAME:
+		/* Notify qed of the name change */
+		if (!edev->ops || !edev->ops->common)
+			goto done;
+		edev->ops->common->set_id(edev->cdev, edev->ndev->name, "qede");
+		break;
+	case NETDEV_CHANGEADDR:
+		edev = netdev_priv(ndev);
+		qede_roce_event_changeaddr(edev);
+		break;
+	}
 
 done:
 	return NOTIFY_DONE;
@@ -222,7 +232,7 @@ int __init qede_init(void)
 {
 	int ret;
 
-	pr_notice("qede_init: %s\n", version);
+	pr_info("qede_init: %s\n", version);
 
 	qed_ops = qed_get_eth_ops();
 	if (!qed_ops) {
@@ -253,7 +263,8 @@ int __init qede_init(void)
 
 static void __exit qede_cleanup(void)
 {
-	pr_notice("qede_cleanup called\n");
+	if (debug & QED_LOG_INFO_MASK)
+		pr_info("qede_cleanup called\n");
 
 	unregister_netdevice_notifier(&qede_netdev_notifier);
 	pci_unregister_driver(&qede_pci_driver);
@@ -270,8 +281,7 @@ module_exit(qede_cleanup);
 
 /* Unmap the data and free skb */
 static int qede_free_tx_pkt(struct qede_dev *edev,
-			    struct qede_tx_queue *txq,
-			    int *len)
+			    struct qede_tx_queue *txq, int *len)
 {
 	u16 idx = txq->sw_tx_cons & NUM_TX_BDS_MAX;
 	struct sk_buff *skb = txq->sw_tx_ring[idx].skb;
@@ -329,8 +339,7 @@ static int qede_free_tx_pkt(struct qede_dev *edev,
 static void qede_free_failed_tx_pkt(struct qede_dev *edev,
 				    struct qede_tx_queue *txq,
 				    struct eth_tx_1st_bd *first_bd,
-				    int nbd,
-				    bool data_split)
+				    int nbd, bool data_split)
 {
 	u16 idx = txq->sw_tx_prod & NUM_TX_BDS_MAX;
 	struct sk_buff *skb = txq->sw_tx_ring[idx].skb;
@@ -339,8 +348,7 @@ static void qede_free_failed_tx_pkt(struct qede_dev *edev,
 
 	/* Return prod to its position before this skb was handled */
 	qed_chain_set_prod(&txq->tx_pbl,
-			   le16_to_cpu(txq->tx_db.data.bd_prod),
-			   first_bd);
+			   le16_to_cpu(txq->tx_db.data.bd_prod), first_bd);
 
 	first_bd = (struct eth_tx_1st_bd *)qed_chain_produce(&txq->tx_pbl);
 
@@ -366,8 +374,7 @@ static void qede_free_failed_tx_pkt(struct qede_dev *edev,
 
 	/* Return again prod to its position before this skb was handled */
 	qed_chain_set_prod(&txq->tx_pbl,
-			   le16_to_cpu(txq->tx_db.data.bd_prod),
-			   first_bd);
+			   le16_to_cpu(txq->tx_db.data.bd_prod), first_bd);
 
 	/* Free skb */
 	dev_kfree_skb_any(skb);
@@ -376,8 +383,7 @@ static void qede_free_failed_tx_pkt(struct qede_dev *edev,
 }
 
 static u32 qede_xmit_type(struct qede_dev *edev,
-			  struct sk_buff *skb,
-			  int *ipv6_ext)
+			  struct sk_buff *skb, int *ipv6_ext)
 {
 	u32 rc = XMIT_L4_CSUM;
 	__be16 l3_proto;
@@ -434,15 +440,13 @@ static void qede_set_params_for_ipv6_ext(struct sk_buff *skb,
 }
 
 static int map_frag_to_bd(struct qede_dev *edev,
-			  skb_frag_t *frag,
-			  struct eth_tx_bd *bd)
+			  skb_frag_t *frag, struct eth_tx_bd *bd)
 {
 	dma_addr_t mapping;
 
 	/* Map skb non-linear frag data for DMA */
 	mapping = skb_frag_dma_map(&edev->pdev->dev, frag, 0,
-				   skb_frag_size(frag),
-				   DMA_TO_DEVICE);
+				   skb_frag_size(frag), DMA_TO_DEVICE);
 	if (unlikely(dma_mapping_error(&edev->pdev->dev, mapping))) {
 		DP_NOTICE(edev, "Unable to map frag - dropping packet\n");
 		return -ENOMEM;
@@ -504,9 +508,8 @@ static inline void qede_update_tx_producer(struct qede_tx_queue *txq)
 }
 
 /* Main transmit function */
-static
-netdev_tx_t qede_start_xmit(struct sk_buff *skb,
-			    struct net_device *ndev)
+static netdev_tx_t qede_start_xmit(struct sk_buff *skb,
+				   struct net_device *ndev)
 {
 	struct qede_dev *edev = netdev_priv(ndev);
 	struct netdev_queue *netdev_txq;
@@ -526,12 +529,11 @@ netdev_tx_t qede_start_xmit(struct sk_buff *skb,
 
 	/* Get tx-queue context and netdev index */
 	txq_index = skb_get_queue_mapping(skb);
-	WARN_ON(txq_index >= QEDE_TSS_CNT(edev));
+	WARN_ON(txq_index >= QEDE_TSS_COUNT(edev));
 	txq = QEDE_TX_QUEUE(edev, txq_index);
 	netdev_txq = netdev_get_tx_queue(ndev, txq_index);
 
-	WARN_ON(qed_chain_get_elem_left(&txq->tx_pbl) <
-			       (MAX_SKB_FRAGS + 1));
+	WARN_ON(qed_chain_get_elem_left(&txq->tx_pbl) < (MAX_SKB_FRAGS + 1));
 
 	xmit_type = qede_xmit_type(edev, skb, &ipv6_ext);
 
@@ -606,6 +608,14 @@ netdev_tx_t qede_start_xmit(struct sk_buff *skb,
 			    1 << ETH_TX_DATA_1ST_BD_TUNN_FLAG_SHIFT;
 		}
 
+		/* Legacy FW had flipped behavior in regard to this bit -
+		 * I.e., needed to set to prevent FW from touching encapsulated
+		 * packets when it didn't need to.
+		 */
+		if (unlikely(txq->is_legacy))
+			first_bd->data.bitfields ^=
+			    1 << ETH_TX_DATA_1ST_BD_TUNN_FLAG_SHIFT;
+
 		/* If the packet is IPv6 with extension header, indicate that
 		 * to FW and pass few params, since the device cracker doesn't
 		 * support parsing IPv6 with extension header/s.
@@ -731,6 +741,7 @@ netdev_tx_t qede_start_xmit(struct sk_buff *skb,
 			qede_update_tx_producer(txq);
 
 		netif_tx_stop_queue(netdev_txq);
+		txq->stopped_cnt++;
 		DP_VERBOSE(edev, NETIF_MSG_TX_QUEUED,
 			   "Stop queue was called\n");
 		/* paired memory barrier is in qede_tx_int(), we have to keep
@@ -764,8 +775,7 @@ int qede_txq_has_work(struct qede_tx_queue *txq)
 	return hw_bd_cons != qed_chain_get_cons_idx(&txq->tx_pbl);
 }
 
-static int qede_tx_int(struct qede_dev *edev,
-		       struct qede_tx_queue *txq)
+static int qede_tx_int(struct qede_dev *edev, struct qede_tx_queue *txq)
 {
 	struct netdev_queue *netdev_txq;
 	u16 hw_bd_cons;
@@ -791,6 +801,7 @@ static int qede_tx_int(struct qede_dev *edev,
 		bytes_compl += len;
 		pkts_compl++;
 		txq->sw_tx_cons++;
+		txq->xmit_pkts++;
 	}
 
 	netdev_tx_completed_queue(netdev_txq, pkts_compl, bytes_compl);
@@ -963,8 +974,7 @@ static inline void qede_update_rx_prod(struct qede_dev *edev,
 
 static u32 qede_get_rxhash(struct qede_dev *edev,
 			   u8 bitfields,
-			   __le32 rss_hash,
-			   enum pkt_hash_types *rxhash_type)
+			   __le32 rss_hash, enum pkt_hash_types *rxhash_type)
 {
 	enum rss_hash_type htype;
 
@@ -993,12 +1003,10 @@ static void qede_set_skb_csum(struct sk_buff *skb, u8 csum_flag)
 
 static inline void qede_skb_receive(struct qede_dev *edev,
 				    struct qede_fastpath *fp,
-				    struct sk_buff *skb,
-				    u16 vlan_tag)
+				    struct sk_buff *skb, u16 vlan_tag)
 {
 	if (vlan_tag)
-		__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q),
-				       vlan_tag);
+		__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), vlan_tag);
 
 	napi_gro_receive(&fp->napi, skb);
 }
@@ -1021,8 +1029,7 @@ static void qede_set_gro_params(struct qede_dev *edev,
 
 static int qede_fill_frag_skb(struct qede_dev *edev,
 			      struct qede_rx_queue *rxq,
-			      u8 tpa_agg_index,
-			      u16 len_on_bd)
+			      u8 tpa_agg_index, u16 len_on_bd)
 {
 	struct sw_rx_data *current_bd = &rxq->sw_rx_ring[rxq->sw_rx_cons &
 							 NUM_RX_BDS_MAX];
@@ -1209,7 +1216,7 @@ static void qede_gro_receive(struct qede_dev *edev,
 #endif
 
 send_skb:
-	skb_record_rx_queue(skb, fp->rss_id);
+	skb_record_rx_queue(skb, fp->rxq->rxq_id);
 	qede_skb_receive(edev, fp, skb, vlan_tag);
 }
 
@@ -1413,7 +1420,7 @@ static int qede_rx_int(struct qede_fastpath *fp, int budget)
 
 		if (unlikely(cqe_type == ETH_RX_CQE_TYPE_SLOW_PATH)) {
 			edev->ops->eth_cqe_completion(
-					edev->cdev, fp->rss_id,
+					edev->cdev, fp->id,
 					(struct eth_slow_path_rx_cqe *)cqe);
 			goto next_cqe;
 		}
@@ -1470,7 +1477,7 @@ alloc_skb:
 		skb = netdev_alloc_skb(edev->ndev, QEDE_RX_HDR_SIZE);
 		if (unlikely(!skb)) {
 			DP_NOTICE(edev,
-				  "Build_skb failed, dropping incoming packet\n");
+				  "skb allocation failed, dropping incoming packet\n");
 			qede_recycle_rx_bd_ring(rxq, edev, fp_cqe->bd_num);
 			rxq->rx_alloc_errors++;
 			goto next_cqe;
@@ -1578,14 +1585,13 @@ alloc_skb:
 		skb->protocol = eth_type_trans(skb, edev->ndev);
 
 		rx_hash = qede_get_rxhash(edev, fp_cqe->bitfields,
-					  fp_cqe->rss_hash,
-					  &rxhash_type);
+					  fp_cqe->rss_hash, &rxhash_type);
 
 		skb_set_hash(skb, rx_hash, rxhash_type);
 
 		qede_set_skb_csum(skb, csum_flag);
 
-		skb_record_rx_queue(skb, fp->rss_id);
+		skb_record_rx_queue(skb, fp->rxq->rxq_id);
 
 		qede_skb_receive(edev, fp, skb, le16_to_cpu(fp_cqe->vlan_tag));
 next_rx_only:
@@ -1604,6 +1610,8 @@ next_cqe: /* don't consume bd rx buffer */
 	/* Update producers */
 	qede_update_rx_prod(edev, rxq);
 
+	rxq->rcv_pkts += rx_pkt;
+
 	return rx_pkt;
 }
 
@@ -1616,10 +1624,12 @@ static int qede_poll(struct napi_struct *napi, int budget)
 	u8 tc;
 
 	for (tc = 0; tc < edev->num_tc; tc++)
-		if (qede_txq_has_work(&fp->txqs[tc]))
+		if (likely(fp->type & QEDE_FASTPATH_TX) &&
+		    qede_txq_has_work(&fp->txqs[tc]))
 			qede_tx_int(edev, &fp->txqs[tc]);
 
-	rx_work_done = qede_has_rx_work(fp->rxq) ?
+	rx_work_done = (likely(fp->type & QEDE_FASTPATH_RX) &&
+			qede_has_rx_work(fp->rxq)) ?
 			qede_rx_int(fp, budget) : 0;
 	if (rx_work_done < budget) {
 		qed_sb_update_sb_idx(fp->sb_info);
@@ -1639,8 +1649,10 @@ static int qede_poll(struct napi_struct *napi, int budget)
 		rmb();
 
 		/* Fall out from the NAPI loop if needed */
-		if (!(qede_has_rx_work(fp->rxq) ||
-		      qede_has_tx_work(fp))) {
+		if (!((likely(fp->type & QEDE_FASTPATH_RX) &&
+		       qede_has_rx_work(fp->rxq)) ||
+		      (likely(fp->type & QEDE_FASTPATH_TX) &&
+		       qede_has_tx_work(fp)))) {
 			napi_complete(napi);
 
 			/* Update and reenable interrupts */
@@ -1711,6 +1723,8 @@ void qede_fill_by_demand_stats(struct qede_dev *edev)
 
 	edev->ops->get_vport_stats(edev->cdev, &stats);
 	edev->stats.no_buff_discards = stats.no_buff_discards;
+	edev->stats.packet_too_big_discard = stats.packet_too_big_discard;
+	edev->stats.ttl0_discard = stats.ttl0_discard;
 	edev->stats.rx_ucast_bytes = stats.rx_ucast_bytes;
 	edev->stats.rx_mcast_bytes = stats.rx_mcast_bytes;
 	edev->stats.rx_bcast_bytes = stats.rx_bcast_bytes;
@@ -1790,9 +1804,9 @@ void qede_fill_by_demand_stats(struct qede_dev *edev)
 	edev->stats.tx_mac_ctrl_frames = stats.tx_mac_ctrl_frames;
 }
 
-static struct rtnl_link_stats64 *qede_get_stats64(
-			    struct net_device *dev,
-			    struct rtnl_link_stats64 *stats)
+static
+struct rtnl_link_stats64 *qede_get_stats64(struct net_device *dev,
+					   struct rtnl_link_stats64 *stats)
 {
 	struct qede_dev *edev = netdev_priv(dev);
 
@@ -2106,14 +2120,13 @@ static void qede_vlan_mark_nonconfigured(struct qede_dev *edev)
 		}
 
 		DP_VERBOSE(edev, NETIF_MSG_IFDOWN,
-			   "marked vlan %d as non-configured\n",
-			   vlan->vid);
+			   "marked vlan %d as non-configured\n", vlan->vid);
 	}
 
 	edev->accept_any_vlan = false;
 }
 
-int qede_set_features(struct net_device *dev, netdev_features_t features)
+static int qede_set_features(struct net_device *dev, netdev_features_t features)
 {
 	struct qede_dev *edev = netdev_priv(dev);
 	netdev_features_t changes = features ^ dev->features;
@@ -2149,7 +2162,7 @@ static void qede_udp_tunnel_add(struct net_device *dev,
 
 		edev->vxlan_dst_port = t_port;
 
-		DP_VERBOSE(edev, QED_MSG_DEBUG, "Added vxlan port=%d",
+		DP_VERBOSE(edev, QED_MSG_DEBUG, "Added vxlan port=%d\n",
 			   t_port);
 
 		set_bit(QEDE_SP_VXLAN_PORT_CONFIG, &edev->sp_flags);
@@ -2160,7 +2173,7 @@ static void qede_udp_tunnel_add(struct net_device *dev,
 
 		edev->geneve_dst_port = t_port;
 
-		DP_VERBOSE(edev, QED_MSG_DEBUG, "Added geneve port=%d",
+		DP_VERBOSE(edev, QED_MSG_DEBUG, "Added geneve port=%d\n",
 			   t_port);
 		set_bit(QEDE_SP_GENEVE_PORT_CONFIG, &edev->sp_flags);
 		break;
@@ -2184,7 +2197,7 @@ static void qede_udp_tunnel_del(struct net_device *dev,
 
 		edev->vxlan_dst_port = 0;
 
-		DP_VERBOSE(edev, QED_MSG_DEBUG, "Deleted vxlan port=%d",
+		DP_VERBOSE(edev, QED_MSG_DEBUG, "Deleted vxlan port=%d\n",
 			   t_port);
 
 		set_bit(QEDE_SP_VXLAN_PORT_CONFIG, &edev->sp_flags);
@@ -2195,7 +2208,7 @@ static void qede_udp_tunnel_del(struct net_device *dev,
 
 		edev->geneve_dst_port = 0;
 
-		DP_VERBOSE(edev, QED_MSG_DEBUG, "Deleted geneve port=%d",
+		DP_VERBOSE(edev, QED_MSG_DEBUG, "Deleted geneve port=%d\n",
 			   t_port);
 		set_bit(QEDE_SP_GENEVE_PORT_CONFIG, &edev->sp_flags);
 		break;
@@ -2240,15 +2253,13 @@ static const struct net_device_ops qede_netdev_ops = {
 static struct qede_dev *qede_alloc_etherdev(struct qed_dev *cdev,
 					    struct pci_dev *pdev,
 					    struct qed_dev_eth_info *info,
-					    u32 dp_module,
-					    u8 dp_level)
+					    u32 dp_module, u8 dp_level)
 {
 	struct net_device *ndev;
 	struct qede_dev *edev;
 
 	ndev = alloc_etherdev_mqs(sizeof(*edev),
-				  info->num_queues,
-				  info->num_queues);
+				  info->num_queues, info->num_queues);
 	if (!ndev) {
 		pr_err("etherdev allocation failed\n");
 		return NULL;
@@ -2264,6 +2275,9 @@ static struct qede_dev *qede_alloc_etherdev(struct qed_dev *cdev,
 	edev->q_num_rx_buffers = NUM_RX_BDS_DEF;
 	edev->q_num_tx_buffers = NUM_TX_BDS_DEF;
 
+	DP_INFO(edev, "Allocated netdev with %d tx queues and %d rx queues\n",
+		info->num_queues, info->num_queues);
+
 	SET_NETDEV_DEV(ndev, &pdev->dev);
 
 	memset(&edev->stats, 0, sizeof(edev->stats));
@@ -2352,7 +2366,7 @@ static void qede_free_fp_array(struct qede_dev *edev)
 		struct qede_fastpath *fp;
 		int i;
 
-		for_each_rss(i) {
+		for_each_queue(i) {
 			fp = &edev->fp_array[i];
 
 			kfree(fp->sb_info);
@@ -2361,22 +2375,33 @@ static void qede_free_fp_array(struct qede_dev *edev)
 		}
 		kfree(edev->fp_array);
 	}
-	edev->num_rss = 0;
+
+	edev->num_queues = 0;
+	edev->fp_num_tx = 0;
+	edev->fp_num_rx = 0;
 }
 
 static int qede_alloc_fp_array(struct qede_dev *edev)
 {
+	u8 fp_combined, fp_rx = edev->fp_num_rx;
 	struct qede_fastpath *fp;
 	int i;
 
-	edev->fp_array = kcalloc(QEDE_RSS_CNT(edev),
+	edev->fp_array = kcalloc(QEDE_QUEUE_CNT(edev),
 				 sizeof(*edev->fp_array), GFP_KERNEL);
 	if (!edev->fp_array) {
 		DP_NOTICE(edev, "fp array allocation failed\n");
 		goto err;
 	}
 
-	for_each_rss(i) {
+	fp_combined = QEDE_QUEUE_CNT(edev) - fp_rx - edev->fp_num_tx;
+
+	/* Allocate the FP elements for Rx queues followed by combined and then
+	 * the Tx. This ordering should be maintained so that the respective
+	 * queues (Rx or Tx) will be together in the fastpath array and the
+	 * associated ids will be sequential.
+	 */
+	for_each_queue(i) {
 		fp = &edev->fp_array[i];
 
 		fp->sb_info = kcalloc(1, sizeof(*fp->sb_info), GFP_KERNEL);
@@ -2385,16 +2410,33 @@ static int qede_alloc_fp_array(struct qede_dev *edev)
 			goto err;
 		}
 
-		fp->rxq = kcalloc(1, sizeof(*fp->rxq), GFP_KERNEL);
-		if (!fp->rxq) {
-			DP_NOTICE(edev, "RXQ struct allocation failed\n");
-			goto err;
+		if (fp_rx) {
+			fp->type = QEDE_FASTPATH_RX;
+			fp_rx--;
+		} else if (fp_combined) {
+			fp->type = QEDE_FASTPATH_COMBINED;
+			fp_combined--;
+		} else {
+			fp->type = QEDE_FASTPATH_TX;
 		}
 
-		fp->txqs = kcalloc(edev->num_tc, sizeof(*fp->txqs), GFP_KERNEL);
-		if (!fp->txqs) {
-			DP_NOTICE(edev, "TXQ array allocation failed\n");
-			goto err;
+		if (fp->type & QEDE_FASTPATH_TX) {
+			fp->txqs = kcalloc(edev->num_tc, sizeof(*fp->txqs),
+					   GFP_KERNEL);
+			if (!fp->txqs) {
+				DP_NOTICE(edev,
+					  "TXQ array allocation failed\n");
+				goto err;
+			}
+		}
+
+		if (fp->type & QEDE_FASTPATH_RX) {
+			fp->rxq = kcalloc(1, sizeof(*fp->rxq), GFP_KERNEL);
+			if (!fp->rxq) {
+				DP_NOTICE(edev,
+					  "RXQ struct allocation failed\n");
+				goto err;
+			}
 		}
 	}
 
@@ -2456,7 +2498,7 @@ static int __qede_probe(struct pci_dev *pdev, u32 dp_module, u8 dp_level,
 			bool is_vf, enum qede_probe_mode mode)
 {
 	struct qed_probe_params probe_params;
-	struct qed_slowpath_params params;
+	struct qed_slowpath_params sp_params;
 	struct qed_dev_eth_info dev_info;
 	struct qede_dev *edev;
 	struct qed_dev *cdev;
@@ -2479,14 +2521,14 @@ static int __qede_probe(struct pci_dev *pdev, u32 dp_module, u8 dp_level,
 	qede_update_pf_params(cdev);
 
 	/* Start the Slowpath-process */
-	memset(&params, 0, sizeof(struct qed_slowpath_params));
-	params.int_mode = QED_INT_MODE_MSIX;
-	params.drv_major = QEDE_MAJOR_VERSION;
-	params.drv_minor = QEDE_MINOR_VERSION;
-	params.drv_rev = QEDE_REVISION_VERSION;
-	params.drv_eng = QEDE_ENGINEERING_VERSION;
-	strlcpy(params.name, "qede LAN", QED_DRV_VER_STR_SIZE);
-	rc = qed_ops->common->slowpath_start(cdev, &params);
+	memset(&sp_params, 0, sizeof(sp_params));
+	sp_params.int_mode = QED_INT_MODE_MSIX;
+	sp_params.drv_major = QEDE_MAJOR_VERSION;
+	sp_params.drv_minor = QEDE_MINOR_VERSION;
+	sp_params.drv_rev = QEDE_REVISION_VERSION;
+	sp_params.drv_eng = QEDE_ENGINEERING_VERSION;
+	strlcpy(sp_params.name, "qede LAN", QED_DRV_VER_STR_SIZE);
+	rc = qed_ops->common->slowpath_start(cdev, &sp_params);
 	if (rc) {
 		pr_notice("Cannot start slowpath\n");
 		goto err1;
@@ -2509,10 +2551,14 @@ static int __qede_probe(struct pci_dev *pdev, u32 dp_module, u8 dp_level,
 
 	qede_init_ndev(edev);
 
+	rc = qede_roce_dev_add(edev);
+	if (rc)
+		goto err3;
+
 	rc = register_netdev(edev->ndev);
 	if (rc) {
 		DP_NOTICE(edev, "Cannot register net-device\n");
-		goto err3;
+		goto err4;
 	}
 
 	edev->ops->common->set_id(cdev, edev->ndev->name, DRV_MODULE_VERSION);
@@ -2532,6 +2578,8 @@ static int __qede_probe(struct pci_dev *pdev, u32 dp_module, u8 dp_level,
 
 	return 0;
 
+err4:
+	qede_roce_dev_remove(edev);
 err3:
 	free_netdev(edev->ndev);
 err2:
@@ -2578,8 +2626,11 @@ static void __qede_remove(struct pci_dev *pdev, enum qede_remove_mode mode)
 	DP_INFO(edev, "Starting qede_remove\n");
 
 	cancel_delayed_work_sync(&edev->sp_task);
+
 	unregister_netdev(ndev);
 
+	qede_roce_dev_remove(edev);
+
 	edev->ops->common->set_power_state(cdev, PCI_D0);
 
 	pci_set_drvdata(pdev, NULL);
@@ -2590,7 +2641,7 @@ static void __qede_remove(struct pci_dev *pdev, enum qede_remove_mode mode)
 	qed_ops->common->slowpath_stop(cdev);
 	qed_ops->common->remove(cdev);
 
-	pr_notice("Ending successfully qede_remove\n");
+	dev_info(&pdev->dev, "Ending qede_remove successfully\n");
 }
 
 static void qede_remove(struct pci_dev *pdev)
@@ -2609,8 +2660,8 @@ static int qede_set_num_queues(struct qede_dev *edev)
 	u16 rss_num;
 
 	/* Setup queues according to possible resources*/
-	if (edev->req_rss)
-		rss_num = edev->req_rss;
+	if (edev->req_queues)
+		rss_num = edev->req_queues;
 	else
 		rss_num = netif_get_num_default_rss_queues() *
 			  edev->dev_info.common.num_hwfns;
@@ -2620,11 +2671,15 @@ static int qede_set_num_queues(struct qede_dev *edev)
 	rc = edev->ops->common->set_fp_int(edev->cdev, rss_num);
 	if (rc > 0) {
 		/* Managed to request interrupts for our queues */
-		edev->num_rss = rc;
+		edev->num_queues = rc;
 		DP_INFO(edev, "Managed %d [of %d] RSS queues\n",
-			QEDE_RSS_CNT(edev), rss_num);
+			QEDE_QUEUE_CNT(edev), rss_num);
 		rc = 0;
 	}
+
+	edev->fp_num_tx = edev->req_num_tx;
+	edev->fp_num_rx = edev->req_num_rx;
+
 	return rc;
 }
 
@@ -2638,16 +2693,14 @@ static void qede_free_mem_sb(struct qede_dev *edev,
 
 /* This function allocates fast-path status block memory */
 static int qede_alloc_mem_sb(struct qede_dev *edev,
-			     struct qed_sb_info *sb_info,
-			     u16 sb_id)
+			     struct qed_sb_info *sb_info, u16 sb_id)
 {
 	struct status_block *sb_virt;
 	dma_addr_t sb_phys;
 	int rc;
 
 	sb_virt = dma_alloc_coherent(&edev->pdev->dev,
-				     sizeof(*sb_virt),
-				     &sb_phys, GFP_KERNEL);
+				     sizeof(*sb_virt), &sb_phys, GFP_KERNEL);
 	if (!sb_virt) {
 		DP_ERR(edev, "Status block allocation failed\n");
 		return -ENOMEM;
@@ -2679,16 +2732,15 @@ static void qede_free_rx_buffers(struct qede_dev *edev,
 		data = rx_buf->data;
 
 		dma_unmap_page(&edev->pdev->dev,
-			       rx_buf->mapping,
-			       PAGE_SIZE, DMA_FROM_DEVICE);
+			       rx_buf->mapping, PAGE_SIZE, DMA_FROM_DEVICE);
 
 		rx_buf->data = NULL;
 		__free_page(data);
 	}
 }
 
-static void qede_free_sge_mem(struct qede_dev *edev,
-			      struct qede_rx_queue *rxq) {
+static void qede_free_sge_mem(struct qede_dev *edev, struct qede_rx_queue *rxq)
+{
 	int i;
 
 	if (edev->gro_disable)
@@ -2707,8 +2759,7 @@ static void qede_free_sge_mem(struct qede_dev *edev,
 	}
 }
 
-static void qede_free_mem_rxq(struct qede_dev *edev,
-			      struct qede_rx_queue *rxq)
+static void qede_free_mem_rxq(struct qede_dev *edev, struct qede_rx_queue *rxq)
 {
 	qede_free_sge_mem(edev, rxq);
 
@@ -2730,9 +2781,6 @@ static int qede_alloc_rx_buffer(struct qede_dev *edev,
 	struct eth_rx_bd *rx_bd;
 	dma_addr_t mapping;
 	struct page *data;
-	u16 rx_buf_size;
-
-	rx_buf_size = rxq->rx_buf_size;
 
 	data = alloc_pages(GFP_ATOMIC, 0);
 	if (unlikely(!data)) {
@@ -2767,8 +2815,7 @@ static int qede_alloc_rx_buffer(struct qede_dev *edev,
 	return 0;
 }
 
-static int qede_alloc_sge_mem(struct qede_dev *edev,
-			      struct qede_rx_queue *rxq)
+static int qede_alloc_sge_mem(struct qede_dev *edev, struct qede_rx_queue *rxq)
 {
 	dma_addr_t mapping;
 	int i;
@@ -2815,15 +2862,14 @@ err:
 }
 
 /* This function allocates all memory needed per Rx queue */
-static int qede_alloc_mem_rxq(struct qede_dev *edev,
-			      struct qede_rx_queue *rxq)
+static int qede_alloc_mem_rxq(struct qede_dev *edev, struct qede_rx_queue *rxq)
 {
 	int i, rc, size;
 
 	rxq->num_rx_buffers = edev->q_num_rx_buffers;
 
-	rxq->rx_buf_size = NET_IP_ALIGN + ETH_OVERHEAD +
-			   edev->ndev->mtu;
+	rxq->rx_buf_size = NET_IP_ALIGN + ETH_OVERHEAD + edev->ndev->mtu;
+
 	if (rxq->rx_buf_size > PAGE_SIZE)
 		rxq->rx_buf_size = PAGE_SIZE;
 
@@ -2877,8 +2923,7 @@ err:
 	return rc;
 }
 
-static void qede_free_mem_txq(struct qede_dev *edev,
-			      struct qede_tx_queue *txq)
+static void qede_free_mem_txq(struct qede_dev *edev, struct qede_tx_queue *txq)
 {
 	/* Free the parallel SW ring */
 	kfree(txq->sw_tx_ring);
@@ -2888,8 +2933,7 @@ static void qede_free_mem_txq(struct qede_dev *edev,
 }
 
 /* This function allocates all memory needed per Tx queue */
-static int qede_alloc_mem_txq(struct qede_dev *edev,
-			      struct qede_tx_queue *txq)
+static int qede_alloc_mem_txq(struct qede_dev *edev, struct qede_tx_queue *txq)
 {
 	int size, rc;
 	union eth_tx_bd_types *p_virt;
@@ -2921,41 +2965,45 @@ err:
 }
 
 /* This function frees all memory of a single fp */
-static void qede_free_mem_fp(struct qede_dev *edev,
-			     struct qede_fastpath *fp)
+static void qede_free_mem_fp(struct qede_dev *edev, struct qede_fastpath *fp)
 {
 	int tc;
 
 	qede_free_mem_sb(edev, fp->sb_info);
 
-	qede_free_mem_rxq(edev, fp->rxq);
+	if (fp->type & QEDE_FASTPATH_RX)
+		qede_free_mem_rxq(edev, fp->rxq);
 
-	for (tc = 0; tc < edev->num_tc; tc++)
-		qede_free_mem_txq(edev, &fp->txqs[tc]);
+	if (fp->type & QEDE_FASTPATH_TX)
+		for (tc = 0; tc < edev->num_tc; tc++)
+			qede_free_mem_txq(edev, &fp->txqs[tc]);
 }
 
 /* This function allocates all memory needed for a single fp (i.e. an entity
- * which contains status block, one rx queue and multiple per-TC tx queues.
+ * which contains status block, one rx queue and/or multiple per-TC tx queues.
  */
-static int qede_alloc_mem_fp(struct qede_dev *edev,
-			     struct qede_fastpath *fp)
+static int qede_alloc_mem_fp(struct qede_dev *edev, struct qede_fastpath *fp)
 {
 	int rc, tc;
 
-	rc = qede_alloc_mem_sb(edev, fp->sb_info, fp->rss_id);
-	if (rc)
-		goto err;
-
-	rc = qede_alloc_mem_rxq(edev, fp->rxq);
+	rc = qede_alloc_mem_sb(edev, fp->sb_info, fp->id);
 	if (rc)
 		goto err;
 
-	for (tc = 0; tc < edev->num_tc; tc++) {
-		rc = qede_alloc_mem_txq(edev, &fp->txqs[tc]);
+	if (fp->type & QEDE_FASTPATH_RX) {
+		rc = qede_alloc_mem_rxq(edev, fp->rxq);
 		if (rc)
 			goto err;
 	}
 
+	if (fp->type & QEDE_FASTPATH_TX) {
+		for (tc = 0; tc < edev->num_tc; tc++) {
+			rc = qede_alloc_mem_txq(edev, &fp->txqs[tc]);
+			if (rc)
+				goto err;
+		}
+	}
+
 	return 0;
 err:
 	return rc;
@@ -2965,7 +3013,7 @@ static void qede_free_mem_load(struct qede_dev *edev)
 {
 	int i;
 
-	for_each_rss(i) {
+	for_each_queue(i) {
 		struct qede_fastpath *fp = &edev->fp_array[i];
 
 		qede_free_mem_fp(edev, fp);
@@ -2975,16 +3023,16 @@ static void qede_free_mem_load(struct qede_dev *edev)
 /* This function allocates all qede memory at NIC load. */
 static int qede_alloc_mem_load(struct qede_dev *edev)
 {
-	int rc = 0, rss_id;
+	int rc = 0, queue_id;
 
-	for (rss_id = 0; rss_id < QEDE_RSS_CNT(edev); rss_id++) {
-		struct qede_fastpath *fp = &edev->fp_array[rss_id];
+	for (queue_id = 0; queue_id < QEDE_QUEUE_CNT(edev); queue_id++) {
+		struct qede_fastpath *fp = &edev->fp_array[queue_id];
 
 		rc = qede_alloc_mem_fp(edev, fp);
 		if (rc) {
 			DP_ERR(edev,
 			       "Failed to allocate memory for fastpath - rss id = %d\n",
-			       rss_id);
+			       queue_id);
 			qede_free_mem_load(edev);
 			return rc;
 		}
@@ -2996,30 +3044,38 @@ static int qede_alloc_mem_load(struct qede_dev *edev)
 /* This function inits fp content and resets the SB, RXQ and TXQ structures */
 static void qede_init_fp(struct qede_dev *edev)
 {
-	int rss_id, txq_index, tc;
+	int queue_id, rxq_index = 0, txq_index = 0, tc;
 	struct qede_fastpath *fp;
 
-	for_each_rss(rss_id) {
-		fp = &edev->fp_array[rss_id];
+	for_each_queue(queue_id) {
+		fp = &edev->fp_array[queue_id];
 
 		fp->edev = edev;
-		fp->rss_id = rss_id;
+		fp->id = queue_id;
 
 		memset((void *)&fp->napi, 0, sizeof(fp->napi));
 
 		memset((void *)fp->sb_info, 0, sizeof(*fp->sb_info));
 
-		memset((void *)fp->rxq, 0, sizeof(*fp->rxq));
-		fp->rxq->rxq_id = rss_id;
+		if (fp->type & QEDE_FASTPATH_RX) {
+			memset((void *)fp->rxq, 0, sizeof(*fp->rxq));
+			fp->rxq->rxq_id = rxq_index++;
+		}
 
-		memset((void *)fp->txqs, 0, (edev->num_tc * sizeof(*fp->txqs)));
-		for (tc = 0; tc < edev->num_tc; tc++) {
-			txq_index = tc * QEDE_RSS_CNT(edev) + rss_id;
-			fp->txqs[tc].index = txq_index;
+		if (fp->type & QEDE_FASTPATH_TX) {
+			memset((void *)fp->txqs, 0,
+			       (edev->num_tc * sizeof(*fp->txqs)));
+			for (tc = 0; tc < edev->num_tc; tc++) {
+				fp->txqs[tc].index = txq_index +
+				    tc * QEDE_TSS_COUNT(edev);
+				if (edev->dev_info.is_legacy)
+					fp->txqs[tc].is_legacy = true;
+			}
+			txq_index++;
 		}
 
 		snprintf(fp->name, sizeof(fp->name), "%s-fp-%d",
-			 edev->ndev->name, rss_id);
+			 edev->ndev->name, queue_id);
 	}
 
 	edev->gro_disable = !(edev->ndev->features & NETIF_F_GRO);
@@ -3029,12 +3085,13 @@ static int qede_set_real_num_queues(struct qede_dev *edev)
 {
 	int rc = 0;
 
-	rc = netif_set_real_num_tx_queues(edev->ndev, QEDE_TSS_CNT(edev));
+	rc = netif_set_real_num_tx_queues(edev->ndev, QEDE_TSS_COUNT(edev));
 	if (rc) {
 		DP_NOTICE(edev, "Failed to set real number of Tx queues\n");
 		return rc;
 	}
-	rc = netif_set_real_num_rx_queues(edev->ndev, QEDE_RSS_CNT(edev));
+
+	rc = netif_set_real_num_rx_queues(edev->ndev, QEDE_RSS_COUNT(edev));
 	if (rc) {
 		DP_NOTICE(edev, "Failed to set real number of Rx queues\n");
 		return rc;
@@ -3047,7 +3104,7 @@ static void qede_napi_disable_remove(struct qede_dev *edev)
 {
 	int i;
 
-	for_each_rss(i) {
+	for_each_queue(i) {
 		napi_disable(&edev->fp_array[i].napi);
 
 		netif_napi_del(&edev->fp_array[i].napi);
@@ -3059,7 +3116,7 @@ static void qede_napi_add_enable(struct qede_dev *edev)
 	int i;
 
 	/* Add NAPI objects */
-	for_each_rss(i) {
+	for_each_queue(i) {
 		netif_napi_add(edev->ndev, &edev->fp_array[i].napi,
 			       qede_poll, NAPI_POLL_WEIGHT);
 		napi_enable(&edev->fp_array[i].napi);
@@ -3088,14 +3145,14 @@ static int qede_req_msix_irqs(struct qede_dev *edev)
 	int i, rc;
 
 	/* Sanitize number of interrupts == number of prepared RSS queues */
-	if (QEDE_RSS_CNT(edev) > edev->int_info.msix_cnt) {
+	if (QEDE_QUEUE_CNT(edev) > edev->int_info.msix_cnt) {
 		DP_ERR(edev,
 		       "Interrupt mismatch: %d RSS queues > %d MSI-x vectors\n",
-		       QEDE_RSS_CNT(edev), edev->int_info.msix_cnt);
+		       QEDE_QUEUE_CNT(edev), edev->int_info.msix_cnt);
 		return -EINVAL;
 	}
 
-	for (i = 0; i < QEDE_RSS_CNT(edev); i++) {
+	for (i = 0; i < QEDE_QUEUE_CNT(edev); i++) {
 		rc = request_irq(edev->int_info.msix[i].vector,
 				 qede_msix_fp_int, 0, edev->fp_array[i].name,
 				 &edev->fp_array[i]);
@@ -3140,18 +3197,17 @@ static int qede_setup_irqs(struct qede_dev *edev)
 
 		/* qed should learn receive the RSS ids and callbacks */
 		ops = edev->ops->common;
-		for (i = 0; i < QEDE_RSS_CNT(edev); i++)
+		for (i = 0; i < QEDE_QUEUE_CNT(edev); i++)
 			ops->simd_handler_config(edev->cdev,
 						 &edev->fp_array[i], i,
 						 qede_simd_fp_handler);
-		edev->int_info.used_cnt = QEDE_RSS_CNT(edev);
+		edev->int_info.used_cnt = QEDE_QUEUE_CNT(edev);
 	}
 	return 0;
 }
 
 static int qede_drain_txq(struct qede_dev *edev,
-			  struct qede_tx_queue *txq,
-			  bool allow_drain)
+			  struct qede_tx_queue *txq, bool allow_drain)
 {
 	int rc, cnt = 1000;
 
@@ -3203,45 +3259,53 @@ static int qede_stop_queues(struct qede_dev *edev)
 	}
 
 	/* Flush Tx queues. If needed, request drain from MCP */
-	for_each_rss(i) {
+	for_each_queue(i) {
 		struct qede_fastpath *fp = &edev->fp_array[i];
 
-		for (tc = 0; tc < edev->num_tc; tc++) {
-			struct qede_tx_queue *txq = &fp->txqs[tc];
+		if (fp->type & QEDE_FASTPATH_TX) {
+			for (tc = 0; tc < edev->num_tc; tc++) {
+				struct qede_tx_queue *txq = &fp->txqs[tc];
 
-			rc = qede_drain_txq(edev, txq, true);
-			if (rc)
-				return rc;
+				rc = qede_drain_txq(edev, txq, true);
+				if (rc)
+					return rc;
+			}
 		}
 	}
 
-	/* Stop all Queues in reverse order*/
-	for (i = QEDE_RSS_CNT(edev) - 1; i >= 0; i--) {
+	/* Stop all Queues in reverse order */
+	for (i = QEDE_QUEUE_CNT(edev) - 1; i >= 0; i--) {
 		struct qed_stop_rxq_params rx_params;
 
-		/* Stop the Tx Queue(s)*/
-		for (tc = 0; tc < edev->num_tc; tc++) {
-			struct qed_stop_txq_params tx_params;
-
-			tx_params.rss_id = i;
-			tx_params.tx_queue_id = tc * QEDE_RSS_CNT(edev) + i;
-			rc = edev->ops->q_tx_stop(cdev, &tx_params);
-			if (rc) {
-				DP_ERR(edev, "Failed to stop TXQ #%d\n",
-				       tx_params.tx_queue_id);
-				return rc;
+		/* Stop the Tx Queue(s) */
+		if (edev->fp_array[i].type & QEDE_FASTPATH_TX) {
+			for (tc = 0; tc < edev->num_tc; tc++) {
+				struct qed_stop_txq_params tx_params;
+				u8 val;
+
+				tx_params.rss_id = i;
+				val = edev->fp_array[i].txqs[tc].index;
+				tx_params.tx_queue_id = val;
+				rc = edev->ops->q_tx_stop(cdev, &tx_params);
+				if (rc) {
+					DP_ERR(edev, "Failed to stop TXQ #%d\n",
+					       tx_params.tx_queue_id);
+					return rc;
+				}
 			}
 		}
 
-		/* Stop the Rx Queue*/
-		memset(&rx_params, 0, sizeof(rx_params));
-		rx_params.rss_id = i;
-		rx_params.rx_queue_id = i;
+		/* Stop the Rx Queue */
+		if (edev->fp_array[i].type & QEDE_FASTPATH_RX) {
+			memset(&rx_params, 0, sizeof(rx_params));
+			rx_params.rss_id = i;
+			rx_params.rx_queue_id = edev->fp_array[i].rxq->rxq_id;
 
-		rc = edev->ops->q_rx_stop(cdev, &rx_params);
-		if (rc) {
-			DP_ERR(edev, "Failed to stop RXQ #%d\n", i);
-			return rc;
+			rc = edev->ops->q_rx_stop(cdev, &rx_params);
+			if (rc) {
+				DP_ERR(edev, "Failed to stop RXQ #%d\n", i);
+				return rc;
+			}
 		}
 	}
 
@@ -3264,7 +3328,7 @@ static int qede_start_queues(struct qede_dev *edev, bool clear_stats)
 	struct qed_start_vport_params start = {0};
 	bool reset_rss_indir = false;
 
-	if (!edev->num_rss) {
+	if (!edev->num_queues) {
 		DP_ERR(edev,
 		       "Cannot update V-VPORT as active as there are no Rx queues\n");
 		return -EINVAL;
@@ -3288,50 +3352,66 @@ static int qede_start_queues(struct qede_dev *edev, bool clear_stats)
 		   "Start vport ramrod passed, vport_id = %d, MTU = %d, vlan_removal_en = %d\n",
 		   start.vport_id, edev->ndev->mtu + 0xe, vlan_removal_en);
 
-	for_each_rss(i) {
+	for_each_queue(i) {
 		struct qede_fastpath *fp = &edev->fp_array[i];
-		dma_addr_t phys_table = fp->rxq->rx_comp_ring.pbl.p_phys_table;
-
-		memset(&q_params, 0, sizeof(q_params));
-		q_params.rss_id = i;
-		q_params.queue_id = i;
-		q_params.vport_id = 0;
-		q_params.sb = fp->sb_info->igu_sb_id;
-		q_params.sb_idx = RX_PI;
-
-		rc = edev->ops->q_rx_start(cdev, &q_params,
-					   fp->rxq->rx_buf_size,
-					   fp->rxq->rx_bd_ring.p_phys_addr,
-					   phys_table,
-					   fp->rxq->rx_comp_ring.page_cnt,
-					   &fp->rxq->hw_rxq_prod_addr);
-		if (rc) {
-			DP_ERR(edev, "Start RXQ #%d failed %d\n", i, rc);
-			return rc;
-		}
+		dma_addr_t p_phys_table;
+		u32 page_cnt;
 
-		fp->rxq->hw_cons_ptr = &fp->sb_info->sb_virt->pi_array[RX_PI];
+		if (fp->type & QEDE_FASTPATH_RX) {
+			struct qede_rx_queue *rxq = fp->rxq;
+			__le16 *val;
 
-		qede_update_rx_prod(edev, fp->rxq);
+			memset(&q_params, 0, sizeof(q_params));
+			q_params.rss_id = i;
+			q_params.queue_id = rxq->rxq_id;
+			q_params.vport_id = 0;
+			q_params.sb = fp->sb_info->igu_sb_id;
+			q_params.sb_idx = RX_PI;
+
+			p_phys_table =
+			    qed_chain_get_pbl_phys(&rxq->rx_comp_ring);
+			page_cnt = qed_chain_get_page_cnt(&rxq->rx_comp_ring);
+
+			rc = edev->ops->q_rx_start(cdev, &q_params,
+						   rxq->rx_buf_size,
+						   rxq->rx_bd_ring.p_phys_addr,
+						   p_phys_table,
+						   page_cnt,
+						   &rxq->hw_rxq_prod_addr);
+			if (rc) {
+				DP_ERR(edev, "Start RXQ #%d failed %d\n", i,
+				       rc);
+				return rc;
+			}
+
+			val = &fp->sb_info->sb_virt->pi_array[RX_PI];
+			rxq->hw_cons_ptr = val;
+
+			qede_update_rx_prod(edev, rxq);
+		}
+
+		if (!(fp->type & QEDE_FASTPATH_TX))
+			continue;
 
 		for (tc = 0; tc < edev->num_tc; tc++) {
 			struct qede_tx_queue *txq = &fp->txqs[tc];
-			int txq_index = tc * QEDE_RSS_CNT(edev) + i;
+
+			p_phys_table = qed_chain_get_pbl_phys(&txq->tx_pbl);
+			page_cnt = qed_chain_get_page_cnt(&txq->tx_pbl);
 
 			memset(&q_params, 0, sizeof(q_params));
 			q_params.rss_id = i;
-			q_params.queue_id = txq_index;
+			q_params.queue_id = txq->index;
 			q_params.vport_id = 0;
 			q_params.sb = fp->sb_info->igu_sb_id;
 			q_params.sb_idx = TX_PI(tc);
 
 			rc = edev->ops->q_tx_start(cdev, &q_params,
-						   txq->tx_pbl.pbl.p_phys_table,
-						   txq->tx_pbl.page_cnt,
+						   p_phys_table, page_cnt,
 						   &txq->doorbell_addr);
 			if (rc) {
 				DP_ERR(edev, "Start TXQ #%d failed %d\n",
-				       txq_index, rc);
+				       txq->index, rc);
 				return rc;
 			}
 
@@ -3362,13 +3442,13 @@ static int qede_start_queues(struct qede_dev *edev, bool clear_stats)
 	}
 
 	/* Fill struct with RSS params */
-	if (QEDE_RSS_CNT(edev) > 1) {
+	if (QEDE_RSS_COUNT(edev) > 1) {
 		vport_update_params.update_rss_flg = 1;
 
 		/* Need to validate current RSS config uses valid entries */
 		for (i = 0; i < QED_RSS_IND_TABLE_SIZE; i++) {
 			if (edev->rss_params.rss_ind_table[i] >=
-			    edev->num_rss) {
+			    QEDE_RSS_COUNT(edev)) {
 				reset_rss_indir = true;
 				break;
 			}
@@ -3381,7 +3461,7 @@ static int qede_start_queues(struct qede_dev *edev, bool clear_stats)
 			for (i = 0; i < QED_RSS_IND_TABLE_SIZE; i++) {
 				u16 indir_val;
 
-				val = QEDE_RSS_CNT(edev);
+				val = QEDE_RSS_COUNT(edev);
 				indir_val = ethtool_rxfh_indir_default(i, val);
 				edev->rss_params.rss_ind_table[i] = indir_val;
 			}
@@ -3447,6 +3527,7 @@ static void qede_unload(struct qede_dev *edev, enum qede_unload_mode mode)
 
 	DP_INFO(edev, "Starting qede unload\n");
 
+	qede_roce_dev_event_close(edev);
 	mutex_lock(&edev->qede_lock);
 	edev->state = QEDE_STATE_CLOSED;
 
@@ -3510,7 +3591,7 @@ static int qede_load(struct qede_dev *edev, enum qede_load_mode mode)
 	if (rc)
 		goto err1;
 	DP_INFO(edev, "Allocated %d RSS queues on %d TC/s\n",
-		QEDE_RSS_CNT(edev), edev->num_tc);
+		QEDE_QUEUE_CNT(edev), edev->num_tc);
 
 	rc = qede_set_real_num_queues(edev);
 	if (rc)
@@ -3547,6 +3628,7 @@ static int qede_load(struct qede_dev *edev, enum qede_load_mode mode)
 	/* Query whether link is already-up */
 	memset(&link_output, 0, sizeof(link_output));
 	edev->ops->common->get_link(edev->cdev, &link_output);
+	qede_roce_dev_event_open(edev);
 	qede_link_update(edev, &link_output);
 
 	DP_INFO(edev, "Ending successfully qede load\n");
@@ -3563,7 +3645,9 @@ err2:
 err1:
 	edev->ops->common->set_fp_int(edev->cdev, 0);
 	qede_free_fp_array(edev);
-	edev->num_rss = 0;
+	edev->num_queues = 0;
+	edev->fp_num_tx = 0;
+	edev->fp_num_rx = 0;
 err0:
 	return rc;
 }
diff --git a/drivers/net/ethernet/qlogic/qede/qede_roce.c b/drivers/net/ethernet/qlogic/qede/qede_roce.c
new file mode 100644
index 000000000000..9867f960b063
--- /dev/null
+++ b/drivers/net/ethernet/qlogic/qede/qede_roce.c
@@ -0,0 +1,314 @@
+/* QLogic qedr NIC Driver
+ * Copyright (c) 2015-2016  QLogic Corporation
+ *
+ * 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/pci.h>
+#include <linux/netdevice.h>
+#include <linux/list.h>
+#include <linux/mutex.h>
+#include <linux/qed/qede_roce.h>
+#include "qede.h"
+
+static struct qedr_driver *qedr_drv;
+static LIST_HEAD(qedr_dev_list);
+static DEFINE_MUTEX(qedr_dev_list_lock);
+
+bool qede_roce_supported(struct qede_dev *dev)
+{
+	return dev->dev_info.common.rdma_supported;
+}
+
+static void _qede_roce_dev_add(struct qede_dev *edev)
+{
+	if (!qedr_drv)
+		return;
+
+	edev->rdma_info.qedr_dev = qedr_drv->add(edev->cdev, edev->pdev,
+						 edev->ndev);
+}
+
+static int qede_roce_create_wq(struct qede_dev *edev)
+{
+	INIT_LIST_HEAD(&edev->rdma_info.roce_event_list);
+	edev->rdma_info.roce_wq = create_singlethread_workqueue("roce_wq");
+	if (!edev->rdma_info.roce_wq) {
+		DP_NOTICE(edev, "qedr: Could not create workqueue\n");
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+
+static void qede_roce_cleanup_event(struct qede_dev *edev)
+{
+	struct list_head *head = &edev->rdma_info.roce_event_list;
+	struct qede_roce_event_work *event_node;
+
+	flush_workqueue(edev->rdma_info.roce_wq);
+	while (!list_empty(head)) {
+		event_node = list_entry(head->next, struct qede_roce_event_work,
+					list);
+		cancel_work_sync(&event_node->work);
+		list_del(&event_node->list);
+		kfree(event_node);
+	}
+}
+
+static void qede_roce_destroy_wq(struct qede_dev *edev)
+{
+	qede_roce_cleanup_event(edev);
+	destroy_workqueue(edev->rdma_info.roce_wq);
+}
+
+int qede_roce_dev_add(struct qede_dev *edev)
+{
+	int rc = 0;
+
+	if (qede_roce_supported(edev)) {
+		rc = qede_roce_create_wq(edev);
+		if (rc)
+			return rc;
+
+		INIT_LIST_HEAD(&edev->rdma_info.entry);
+		mutex_lock(&qedr_dev_list_lock);
+		list_add_tail(&edev->rdma_info.entry, &qedr_dev_list);
+		_qede_roce_dev_add(edev);
+		mutex_unlock(&qedr_dev_list_lock);
+	}
+
+	return rc;
+}
+
+static void _qede_roce_dev_remove(struct qede_dev *edev)
+{
+	if (qedr_drv && qedr_drv->remove && edev->rdma_info.qedr_dev)
+		qedr_drv->remove(edev->rdma_info.qedr_dev);
+	edev->rdma_info.qedr_dev = NULL;
+}
+
+void qede_roce_dev_remove(struct qede_dev *edev)
+{
+	if (!qede_roce_supported(edev))
+		return;
+
+	qede_roce_destroy_wq(edev);
+	mutex_lock(&qedr_dev_list_lock);
+	_qede_roce_dev_remove(edev);
+	list_del(&edev->rdma_info.entry);
+	mutex_unlock(&qedr_dev_list_lock);
+}
+
+static void _qede_roce_dev_open(struct qede_dev *edev)
+{
+	if (qedr_drv && edev->rdma_info.qedr_dev && qedr_drv->notify)
+		qedr_drv->notify(edev->rdma_info.qedr_dev, QEDE_UP);
+}
+
+static void qede_roce_dev_open(struct qede_dev *edev)
+{
+	if (!qede_roce_supported(edev))
+		return;
+
+	mutex_lock(&qedr_dev_list_lock);
+	_qede_roce_dev_open(edev);
+	mutex_unlock(&qedr_dev_list_lock);
+}
+
+static void _qede_roce_dev_close(struct qede_dev *edev)
+{
+	if (qedr_drv && edev->rdma_info.qedr_dev && qedr_drv->notify)
+		qedr_drv->notify(edev->rdma_info.qedr_dev, QEDE_DOWN);
+}
+
+static void qede_roce_dev_close(struct qede_dev *edev)
+{
+	if (!qede_roce_supported(edev))
+		return;
+
+	mutex_lock(&qedr_dev_list_lock);
+	_qede_roce_dev_close(edev);
+	mutex_unlock(&qedr_dev_list_lock);
+}
+
+static void qede_roce_dev_shutdown(struct qede_dev *edev)
+{
+	if (!qede_roce_supported(edev))
+		return;
+
+	mutex_lock(&qedr_dev_list_lock);
+	if (qedr_drv && edev->rdma_info.qedr_dev && qedr_drv->notify)
+		qedr_drv->notify(edev->rdma_info.qedr_dev, QEDE_CLOSE);
+	mutex_unlock(&qedr_dev_list_lock);
+}
+
+int qede_roce_register_driver(struct qedr_driver *drv)
+{
+	struct qede_dev *edev;
+	u8 qedr_counter = 0;
+
+	mutex_lock(&qedr_dev_list_lock);
+	if (qedr_drv) {
+		mutex_unlock(&qedr_dev_list_lock);
+		return -EINVAL;
+	}
+	qedr_drv = drv;
+
+	list_for_each_entry(edev, &qedr_dev_list, rdma_info.entry) {
+		struct net_device *ndev;
+
+		qedr_counter++;
+		_qede_roce_dev_add(edev);
+		ndev = edev->ndev;
+		if (netif_running(ndev) && netif_oper_up(ndev))
+			_qede_roce_dev_open(edev);
+	}
+	mutex_unlock(&qedr_dev_list_lock);
+
+	DP_INFO(edev, "qedr: discovered and registered %d RoCE funcs\n",
+		qedr_counter);
+
+	return 0;
+}
+EXPORT_SYMBOL(qede_roce_register_driver);
+
+void qede_roce_unregister_driver(struct qedr_driver *drv)
+{
+	struct qede_dev *edev;
+
+	mutex_lock(&qedr_dev_list_lock);
+	list_for_each_entry(edev, &qedr_dev_list, rdma_info.entry) {
+		if (edev->rdma_info.qedr_dev)
+			_qede_roce_dev_remove(edev);
+	}
+	qedr_drv = NULL;
+	mutex_unlock(&qedr_dev_list_lock);
+}
+EXPORT_SYMBOL(qede_roce_unregister_driver);
+
+static void qede_roce_changeaddr(struct qede_dev *edev)
+{
+	if (!qede_roce_supported(edev))
+		return;
+
+	if (qedr_drv && edev->rdma_info.qedr_dev && qedr_drv->notify)
+		qedr_drv->notify(edev->rdma_info.qedr_dev, QEDE_CHANGE_ADDR);
+}
+
+struct qede_roce_event_work *qede_roce_get_free_event_node(struct qede_dev
+							   *edev)
+{
+	struct qede_roce_event_work *event_node = NULL;
+	struct list_head *list_node = NULL;
+	bool found = false;
+
+	list_for_each(list_node, &edev->rdma_info.roce_event_list) {
+		event_node = list_entry(list_node, struct qede_roce_event_work,
+					list);
+		if (!work_pending(&event_node->work)) {
+			found = true;
+			break;
+		}
+	}
+
+	if (!found) {
+		event_node = kzalloc(sizeof(*event_node), GFP_KERNEL);
+		if (!event_node) {
+			DP_NOTICE(edev,
+				  "qedr: Could not allocate memory for roce work\n");
+			return NULL;
+		}
+		list_add_tail(&event_node->list,
+			      &edev->rdma_info.roce_event_list);
+	}
+
+	return event_node;
+}
+
+static void qede_roce_handle_event(struct work_struct *work)
+{
+	struct qede_roce_event_work *event_node;
+	enum qede_roce_event event;
+	struct qede_dev *edev;
+
+	event_node = container_of(work, struct qede_roce_event_work, work);
+	event = event_node->event;
+	edev = event_node->ptr;
+
+	switch (event) {
+	case QEDE_UP:
+		qede_roce_dev_open(edev);
+		break;
+	case QEDE_DOWN:
+		qede_roce_dev_close(edev);
+		break;
+	case QEDE_CLOSE:
+		qede_roce_dev_shutdown(edev);
+		break;
+	case QEDE_CHANGE_ADDR:
+		qede_roce_changeaddr(edev);
+		break;
+	default:
+		DP_NOTICE(edev, "Invalid roce event %d", event);
+	}
+}
+
+static void qede_roce_add_event(struct qede_dev *edev,
+				enum qede_roce_event event)
+{
+	struct qede_roce_event_work *event_node;
+
+	if (!edev->rdma_info.qedr_dev)
+		return;
+
+	event_node = qede_roce_get_free_event_node(edev);
+	if (!event_node)
+		return;
+
+	event_node->event = event;
+	event_node->ptr = edev;
+
+	INIT_WORK(&event_node->work, qede_roce_handle_event);
+	queue_work(edev->rdma_info.roce_wq, &event_node->work);
+}
+
+void qede_roce_dev_event_open(struct qede_dev *edev)
+{
+	qede_roce_add_event(edev, QEDE_UP);
+}
+
+void qede_roce_dev_event_close(struct qede_dev *edev)
+{
+	qede_roce_add_event(edev, QEDE_DOWN);
+}
+
+void qede_roce_event_changeaddr(struct qede_dev *edev)
+{
+	qede_roce_add_event(edev, QEDE_CHANGE_ADDR);
+}
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_main.c b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_main.c
index 3ebef27e0964..3ae3968b0edf 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_main.c
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_main.c
@@ -432,18 +432,19 @@ static int qlcnic_fdb_add(struct ndmsg *ndm, struct nlattr *tb[],
 
 static int qlcnic_fdb_dump(struct sk_buff *skb, struct netlink_callback *ncb,
 			struct net_device *netdev,
-			struct net_device *filter_dev, int idx)
+			struct net_device *filter_dev, int *idx)
 {
 	struct qlcnic_adapter *adapter = netdev_priv(netdev);
+	int err = 0;
 
 	if (!adapter->fdb_mac_learn)
 		return ndo_dflt_fdb_dump(skb, ncb, netdev, filter_dev, idx);
 
 	if ((adapter->flags & QLCNIC_ESWITCH_ENABLED) ||
 	    qlcnic_sriov_check(adapter))
-		idx = ndo_dflt_fdb_dump(skb, ncb, netdev, filter_dev, idx);
+		err = ndo_dflt_fdb_dump(skb, ncb, netdev, filter_dev, idx);
 
-	return idx;
+	return err;
 }
 
 static void qlcnic_82xx_cancel_idc_work(struct qlcnic_adapter *adapter)
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_sriov.h b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_sriov.h
index 24061b9b92e8..5f327659efa7 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_sriov.h
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_sriov.h
@@ -238,7 +238,7 @@ int qlcnic_sriov_set_vf_mac(struct net_device *, int, u8 *);
 int qlcnic_sriov_set_vf_tx_rate(struct net_device *, int, int, int);
 int qlcnic_sriov_get_vf_config(struct net_device *, int ,
 			       struct ifla_vf_info *);
-int qlcnic_sriov_set_vf_vlan(struct net_device *, int, u16, u8);
+int qlcnic_sriov_set_vf_vlan(struct net_device *, int, u16, u8, __be16);
 int qlcnic_sriov_set_vf_spoofchk(struct net_device *, int, bool);
 #else
 static inline void qlcnic_sriov_pf_disable(struct qlcnic_adapter *adapter) {}
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_sriov_pf.c b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_sriov_pf.c
index afd687e5e779..50eaafa3eaba 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_sriov_pf.c
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_sriov_pf.c
@@ -1915,7 +1915,7 @@ int qlcnic_sriov_set_vf_tx_rate(struct net_device *netdev, int vf,
 }
 
 int qlcnic_sriov_set_vf_vlan(struct net_device *netdev, int vf,
-			     u16 vlan, u8 qos)
+			     u16 vlan, u8 qos, __be16 vlan_proto)
 {
 	struct qlcnic_adapter *adapter = netdev_priv(netdev);
 	struct qlcnic_sriov *sriov = adapter->ahw->sriov;
@@ -1928,6 +1928,9 @@ int qlcnic_sriov_set_vf_vlan(struct net_device *netdev, int vf,
 	if (vf >= sriov->num_vfs || qos > 7)
 		return -EINVAL;
 
+	if (vlan_proto != htons(ETH_P_8021Q))
+		return -EPROTONOSUPPORT;
+
 	if (vlan > MAX_VLAN_ID) {
 		netdev_err(netdev,
 			   "Invalid VLAN ID, allowed range is [0 - %d]\n",
diff --git a/drivers/net/ethernet/qualcomm/Kconfig b/drivers/net/ethernet/qualcomm/Kconfig
index a76e380cf89a..d7720bf92d49 100644
--- a/drivers/net/ethernet/qualcomm/Kconfig
+++ b/drivers/net/ethernet/qualcomm/Kconfig
@@ -24,4 +24,17 @@ config QCA7000
 	  To compile this driver as a module, choose M here. The module
 	  will be called qcaspi.
 
+config QCOM_EMAC
+	tristate "Qualcomm Technologies, Inc. EMAC Gigabit Ethernet support"
+	depends on HAS_DMA && HAS_IOMEM
+	select CRC32
+	select PHYLIB
+	---help---
+	  This driver supports the Qualcomm Technologies, Inc. Gigabit
+	  Ethernet Media Access Controller (EMAC). The controller
+	  supports IEEE 802.3-2002, half-duplex mode at 10/100 Mb/s,
+	  full-duplex mode at 10/100/1000Mb/s, Wake On LAN (WOL) for
+	  low power, Receive-Side Scaling (RSS), and IEEE 1588-2008
+	  Precision Clock Synchronization Protocol.
+
 endif # NET_VENDOR_QUALCOMM
diff --git a/drivers/net/ethernet/qualcomm/Makefile b/drivers/net/ethernet/qualcomm/Makefile
index 9da2d75db700..aacb0a585c68 100644
--- a/drivers/net/ethernet/qualcomm/Makefile
+++ b/drivers/net/ethernet/qualcomm/Makefile
@@ -4,3 +4,5 @@
 
 obj-$(CONFIG_QCA7000) += qcaspi.o
 qcaspi-objs := qca_spi.o qca_framing.o qca_7k.o qca_debug.o
+
+obj-y += emac/
diff --git a/drivers/net/ethernet/qualcomm/emac/Makefile b/drivers/net/ethernet/qualcomm/emac/Makefile
new file mode 100644
index 000000000000..01ee144c6386
--- /dev/null
+++ b/drivers/net/ethernet/qualcomm/emac/Makefile
@@ -0,0 +1,7 @@
+#
+# Makefile for the Qualcomm Technologies, Inc. EMAC Gigabit Ethernet driver
+#
+
+obj-$(CONFIG_QCOM_EMAC) += qcom-emac.o
+
+qcom-emac-objs := emac.o emac-mac.o emac-phy.o emac-sgmii.o
diff --git a/drivers/net/ethernet/qualcomm/emac/emac-mac.c b/drivers/net/ethernet/qualcomm/emac/emac-mac.c
new file mode 100644
index 000000000000..e97968ed4b8f
--- /dev/null
+++ b/drivers/net/ethernet/qualcomm/emac/emac-mac.c
@@ -0,0 +1,1528 @@
+/* Copyright (c) 2013-2016, 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.
+ */
+
+/* Qualcomm Technologies, Inc. EMAC Ethernet Controller MAC layer support
+ */
+
+#include <linux/tcp.h>
+#include <linux/ip.h>
+#include <linux/ipv6.h>
+#include <linux/crc32.h>
+#include <linux/if_vlan.h>
+#include <linux/jiffies.h>
+#include <linux/phy.h>
+#include <linux/of.h>
+#include <net/ip6_checksum.h>
+#include "emac.h"
+#include "emac-sgmii.h"
+
+/* EMAC base register offsets */
+#define EMAC_MAC_CTRL			0x001480
+#define EMAC_WOL_CTRL0			0x0014a0
+#define EMAC_RSS_KEY0			0x0014b0
+#define EMAC_H1TPD_BASE_ADDR_LO		0x0014e0
+#define EMAC_H2TPD_BASE_ADDR_LO		0x0014e4
+#define EMAC_H3TPD_BASE_ADDR_LO		0x0014e8
+#define EMAC_INTER_SRAM_PART9		0x001534
+#define EMAC_DESC_CTRL_0		0x001540
+#define EMAC_DESC_CTRL_1		0x001544
+#define EMAC_DESC_CTRL_2		0x001550
+#define EMAC_DESC_CTRL_10		0x001554
+#define EMAC_DESC_CTRL_12		0x001558
+#define EMAC_DESC_CTRL_13		0x00155c
+#define EMAC_DESC_CTRL_3		0x001560
+#define EMAC_DESC_CTRL_4		0x001564
+#define EMAC_DESC_CTRL_5		0x001568
+#define EMAC_DESC_CTRL_14		0x00156c
+#define EMAC_DESC_CTRL_15		0x001570
+#define EMAC_DESC_CTRL_16		0x001574
+#define EMAC_DESC_CTRL_6		0x001578
+#define EMAC_DESC_CTRL_8		0x001580
+#define EMAC_DESC_CTRL_9		0x001584
+#define EMAC_DESC_CTRL_11		0x001588
+#define EMAC_TXQ_CTRL_0			0x001590
+#define EMAC_TXQ_CTRL_1			0x001594
+#define EMAC_TXQ_CTRL_2			0x001598
+#define EMAC_RXQ_CTRL_0			0x0015a0
+#define EMAC_RXQ_CTRL_1			0x0015a4
+#define EMAC_RXQ_CTRL_2			0x0015a8
+#define EMAC_RXQ_CTRL_3			0x0015ac
+#define EMAC_BASE_CPU_NUMBER		0x0015b8
+#define EMAC_DMA_CTRL			0x0015c0
+#define EMAC_MAILBOX_0			0x0015e0
+#define EMAC_MAILBOX_5			0x0015e4
+#define EMAC_MAILBOX_6			0x0015e8
+#define EMAC_MAILBOX_13			0x0015ec
+#define EMAC_MAILBOX_2			0x0015f4
+#define EMAC_MAILBOX_3			0x0015f8
+#define EMAC_MAILBOX_11			0x00160c
+#define EMAC_AXI_MAST_CTRL		0x001610
+#define EMAC_MAILBOX_12			0x001614
+#define EMAC_MAILBOX_9			0x001618
+#define EMAC_MAILBOX_10			0x00161c
+#define EMAC_ATHR_HEADER_CTRL		0x001620
+#define EMAC_CLK_GATE_CTRL		0x001814
+#define EMAC_MISC_CTRL			0x001990
+#define EMAC_MAILBOX_7			0x0019e0
+#define EMAC_MAILBOX_8			0x0019e4
+#define EMAC_MAILBOX_15			0x001bd4
+#define EMAC_MAILBOX_16			0x001bd8
+
+/* EMAC_MAC_CTRL */
+#define SINGLE_PAUSE_MODE       	0x10000000
+#define DEBUG_MODE                      0x08000000
+#define BROAD_EN                        0x04000000
+#define MULTI_ALL                       0x02000000
+#define RX_CHKSUM_EN                    0x01000000
+#define HUGE                            0x00800000
+#define SPEED(x)			(((x) & 0x3) << 20)
+#define SPEED_MASK			SPEED(0x3)
+#define SIMR                            0x00080000
+#define TPAUSE                          0x00010000
+#define PROM_MODE                       0x00008000
+#define VLAN_STRIP                      0x00004000
+#define PRLEN_BMSK                      0x00003c00
+#define PRLEN_SHFT                      10
+#define HUGEN                           0x00000200
+#define FLCHK                           0x00000100
+#define PCRCE                           0x00000080
+#define CRCE                            0x00000040
+#define FULLD                           0x00000020
+#define MAC_LP_EN                       0x00000010
+#define RXFC                            0x00000008
+#define TXFC                            0x00000004
+#define RXEN                            0x00000002
+#define TXEN                            0x00000001
+
+
+/* EMAC_WOL_CTRL0 */
+#define LK_CHG_PME			0x20
+#define LK_CHG_EN			0x10
+#define MG_FRAME_PME			0x8
+#define MG_FRAME_EN			0x4
+#define WK_FRAME_EN			0x1
+
+/* EMAC_DESC_CTRL_3 */
+#define RFD_RING_SIZE_BMSK                                       0xfff
+
+/* EMAC_DESC_CTRL_4 */
+#define RX_BUFFER_SIZE_BMSK                                     0xffff
+
+/* EMAC_DESC_CTRL_6 */
+#define RRD_RING_SIZE_BMSK                                       0xfff
+
+/* EMAC_DESC_CTRL_9 */
+#define TPD_RING_SIZE_BMSK                                      0xffff
+
+/* EMAC_TXQ_CTRL_0 */
+#define NUM_TXF_BURST_PREF_BMSK                             0xffff0000
+#define NUM_TXF_BURST_PREF_SHFT                                     16
+#define LS_8023_SP                                                0x80
+#define TXQ_MODE                                                  0x40
+#define TXQ_EN                                                    0x20
+#define IP_OP_SP                                                  0x10
+#define NUM_TPD_BURST_PREF_BMSK                                    0xf
+#define NUM_TPD_BURST_PREF_SHFT                                      0
+
+/* EMAC_TXQ_CTRL_1 */
+#define JUMBO_TASK_OFFLOAD_THRESHOLD_BMSK                        0x7ff
+
+/* EMAC_TXQ_CTRL_2 */
+#define TXF_HWM_BMSK                                         0xfff0000
+#define TXF_LWM_BMSK                                             0xfff
+
+/* EMAC_RXQ_CTRL_0 */
+#define RXQ_EN                                                 BIT(31)
+#define CUT_THRU_EN                                            BIT(30)
+#define RSS_HASH_EN                                            BIT(29)
+#define NUM_RFD_BURST_PREF_BMSK                              0x3f00000
+#define NUM_RFD_BURST_PREF_SHFT                                     20
+#define IDT_TABLE_SIZE_BMSK                                    0x1ff00
+#define IDT_TABLE_SIZE_SHFT                                          8
+#define SP_IPV6                                                   0x80
+
+/* EMAC_RXQ_CTRL_1 */
+#define JUMBO_1KAH_BMSK                                         0xf000
+#define JUMBO_1KAH_SHFT                                             12
+#define RFD_PREF_LOW_TH                                           0x10
+#define RFD_PREF_LOW_THRESHOLD_BMSK                              0xfc0
+#define RFD_PREF_LOW_THRESHOLD_SHFT                                  6
+#define RFD_PREF_UP_TH                                            0x10
+#define RFD_PREF_UP_THRESHOLD_BMSK                                0x3f
+#define RFD_PREF_UP_THRESHOLD_SHFT                                   0
+
+/* EMAC_RXQ_CTRL_2 */
+#define RXF_DOF_THRESFHOLD                                       0x1a0
+#define RXF_DOF_THRESHOLD_BMSK                               0xfff0000
+#define RXF_DOF_THRESHOLD_SHFT                                      16
+#define RXF_UOF_THRESFHOLD                                        0xbe
+#define RXF_UOF_THRESHOLD_BMSK                                   0xfff
+#define RXF_UOF_THRESHOLD_SHFT                                       0
+
+/* EMAC_RXQ_CTRL_3 */
+#define RXD_TIMER_BMSK                                      0xffff0000
+#define RXD_THRESHOLD_BMSK                                       0xfff
+#define RXD_THRESHOLD_SHFT                                           0
+
+/* EMAC_DMA_CTRL */
+#define DMAW_DLY_CNT_BMSK                                      0xf0000
+#define DMAW_DLY_CNT_SHFT                                           16
+#define DMAR_DLY_CNT_BMSK                                       0xf800
+#define DMAR_DLY_CNT_SHFT                                           11
+#define DMAR_REQ_PRI                                             0x400
+#define REGWRBLEN_BMSK                                           0x380
+#define REGWRBLEN_SHFT                                               7
+#define REGRDBLEN_BMSK                                            0x70
+#define REGRDBLEN_SHFT                                               4
+#define OUT_ORDER_MODE                                             0x4
+#define ENH_ORDER_MODE                                             0x2
+#define IN_ORDER_MODE                                              0x1
+
+/* EMAC_MAILBOX_13 */
+#define RFD3_PROC_IDX_BMSK                                   0xfff0000
+#define RFD3_PROC_IDX_SHFT                                          16
+#define RFD3_PROD_IDX_BMSK                                       0xfff
+#define RFD3_PROD_IDX_SHFT                                           0
+
+/* EMAC_MAILBOX_2 */
+#define NTPD_CONS_IDX_BMSK                                  0xffff0000
+#define NTPD_CONS_IDX_SHFT                                          16
+
+/* EMAC_MAILBOX_3 */
+#define RFD0_CONS_IDX_BMSK                                       0xfff
+#define RFD0_CONS_IDX_SHFT                                           0
+
+/* EMAC_MAILBOX_11 */
+#define H3TPD_PROD_IDX_BMSK                                 0xffff0000
+#define H3TPD_PROD_IDX_SHFT                                         16
+
+/* EMAC_AXI_MAST_CTRL */
+#define DATA_BYTE_SWAP                                             0x8
+#define MAX_BOUND                                                  0x2
+#define MAX_BTYPE                                                  0x1
+
+/* EMAC_MAILBOX_12 */
+#define H3TPD_CONS_IDX_BMSK                                 0xffff0000
+#define H3TPD_CONS_IDX_SHFT                                         16
+
+/* EMAC_MAILBOX_9 */
+#define H2TPD_PROD_IDX_BMSK                                     0xffff
+#define H2TPD_PROD_IDX_SHFT                                          0
+
+/* EMAC_MAILBOX_10 */
+#define H1TPD_CONS_IDX_BMSK                                 0xffff0000
+#define H1TPD_CONS_IDX_SHFT                                         16
+#define H2TPD_CONS_IDX_BMSK                                     0xffff
+#define H2TPD_CONS_IDX_SHFT                                          0
+
+/* EMAC_ATHR_HEADER_CTRL */
+#define HEADER_CNT_EN                                              0x2
+#define HEADER_ENABLE                                              0x1
+
+/* EMAC_MAILBOX_0 */
+#define RFD0_PROC_IDX_BMSK                                   0xfff0000
+#define RFD0_PROC_IDX_SHFT                                          16
+#define RFD0_PROD_IDX_BMSK                                       0xfff
+#define RFD0_PROD_IDX_SHFT                                           0
+
+/* EMAC_MAILBOX_5 */
+#define RFD1_PROC_IDX_BMSK                                   0xfff0000
+#define RFD1_PROC_IDX_SHFT                                          16
+#define RFD1_PROD_IDX_BMSK                                       0xfff
+#define RFD1_PROD_IDX_SHFT                                           0
+
+/* EMAC_MISC_CTRL */
+#define RX_UNCPL_INT_EN                                            0x1
+
+/* EMAC_MAILBOX_7 */
+#define RFD2_CONS_IDX_BMSK                                   0xfff0000
+#define RFD2_CONS_IDX_SHFT                                          16
+#define RFD1_CONS_IDX_BMSK                                       0xfff
+#define RFD1_CONS_IDX_SHFT                                           0
+
+/* EMAC_MAILBOX_8 */
+#define RFD3_CONS_IDX_BMSK                                       0xfff
+#define RFD3_CONS_IDX_SHFT                                           0
+
+/* EMAC_MAILBOX_15 */
+#define NTPD_PROD_IDX_BMSK                                      0xffff
+#define NTPD_PROD_IDX_SHFT                                           0
+
+/* EMAC_MAILBOX_16 */
+#define H1TPD_PROD_IDX_BMSK                                     0xffff
+#define H1TPD_PROD_IDX_SHFT                                          0
+
+#define RXQ0_RSS_HSTYP_IPV6_TCP_EN                                0x20
+#define RXQ0_RSS_HSTYP_IPV6_EN                                    0x10
+#define RXQ0_RSS_HSTYP_IPV4_TCP_EN                                 0x8
+#define RXQ0_RSS_HSTYP_IPV4_EN                                     0x4
+
+/* EMAC_EMAC_WRAPPER_TX_TS_INX */
+#define EMAC_WRAPPER_TX_TS_EMPTY                               BIT(31)
+#define EMAC_WRAPPER_TX_TS_INX_BMSK                             0xffff
+
+struct emac_skb_cb {
+	u32           tpd_idx;
+	unsigned long jiffies;
+};
+
+#define EMAC_SKB_CB(skb)	((struct emac_skb_cb *)(skb)->cb)
+#define EMAC_RSS_IDT_SIZE	256
+#define JUMBO_1KAH		0x4
+#define RXD_TH			0x100
+#define EMAC_TPD_LAST_FRAGMENT	0x80000000
+#define EMAC_TPD_TSTAMP_SAVE	0x80000000
+
+/* EMAC Errors in emac_rrd.word[3] */
+#define EMAC_RRD_L4F		BIT(14)
+#define EMAC_RRD_IPF		BIT(15)
+#define EMAC_RRD_CRC		BIT(21)
+#define EMAC_RRD_FAE		BIT(22)
+#define EMAC_RRD_TRN		BIT(23)
+#define EMAC_RRD_RNT		BIT(24)
+#define EMAC_RRD_INC		BIT(25)
+#define EMAC_RRD_FOV		BIT(29)
+#define EMAC_RRD_LEN		BIT(30)
+
+/* Error bits that will result in a received frame being discarded */
+#define EMAC_RRD_ERROR (EMAC_RRD_IPF | EMAC_RRD_CRC | EMAC_RRD_FAE | \
+			EMAC_RRD_TRN | EMAC_RRD_RNT | EMAC_RRD_INC | \
+			EMAC_RRD_FOV | EMAC_RRD_LEN)
+#define EMAC_RRD_STATS_DW_IDX 3
+
+#define EMAC_RRD(RXQ, SIZE, IDX)	((RXQ)->rrd.v_addr + (SIZE * (IDX)))
+#define EMAC_RFD(RXQ, SIZE, IDX)	((RXQ)->rfd.v_addr + (SIZE * (IDX)))
+#define EMAC_TPD(TXQ, SIZE, IDX)	((TXQ)->tpd.v_addr + (SIZE * (IDX)))
+
+#define GET_RFD_BUFFER(RXQ, IDX)	(&((RXQ)->rfd.rfbuff[(IDX)]))
+#define GET_TPD_BUFFER(RTQ, IDX)	(&((RTQ)->tpd.tpbuff[(IDX)]))
+
+#define EMAC_TX_POLL_HWTXTSTAMP_THRESHOLD	8
+
+#define ISR_RX_PKT      (\
+	RX_PKT_INT0     |\
+	RX_PKT_INT1     |\
+	RX_PKT_INT2     |\
+	RX_PKT_INT3)
+
+#define EMAC_MAC_IRQ_RES                                    	"core0"
+
+void emac_mac_multicast_addr_set(struct emac_adapter *adpt, u8 *addr)
+{
+	u32 crc32, bit, reg, mta;
+
+	/* Calculate the CRC of the MAC address */
+	crc32 = ether_crc(ETH_ALEN, addr);
+
+	/* The HASH Table is an array of 2 32-bit registers. It is
+	 * treated like an array of 64 bits (BitArray[hash_value]).
+	 * Use the upper 6 bits of the above CRC as the hash value.
+	 */
+	reg = (crc32 >> 31) & 0x1;
+	bit = (crc32 >> 26) & 0x1F;
+
+	mta = readl(adpt->base + EMAC_HASH_TAB_REG0 + (reg << 2));
+	mta |= BIT(bit);
+	writel(mta, adpt->base + EMAC_HASH_TAB_REG0 + (reg << 2));
+}
+
+void emac_mac_multicast_addr_clear(struct emac_adapter *adpt)
+{
+	writel(0, adpt->base + EMAC_HASH_TAB_REG0);
+	writel(0, adpt->base + EMAC_HASH_TAB_REG1);
+}
+
+/* definitions for RSS */
+#define EMAC_RSS_KEY(_i, _type) \
+		(EMAC_RSS_KEY0 + ((_i) * sizeof(_type)))
+#define EMAC_RSS_TBL(_i, _type) \
+		(EMAC_IDT_TABLE0 + ((_i) * sizeof(_type)))
+
+/* Config MAC modes */
+void emac_mac_mode_config(struct emac_adapter *adpt)
+{
+	struct net_device *netdev = adpt->netdev;
+	u32 mac;
+
+	mac = readl(adpt->base + EMAC_MAC_CTRL);
+	mac &= ~(VLAN_STRIP | PROM_MODE | MULTI_ALL | MAC_LP_EN);
+
+	if (netdev->features & NETIF_F_HW_VLAN_CTAG_RX)
+		mac |= VLAN_STRIP;
+
+	if (netdev->flags & IFF_PROMISC)
+		mac |= PROM_MODE;
+
+	if (netdev->flags & IFF_ALLMULTI)
+		mac |= MULTI_ALL;
+
+	writel(mac, adpt->base + EMAC_MAC_CTRL);
+}
+
+/* Config descriptor rings */
+static void emac_mac_dma_rings_config(struct emac_adapter *adpt)
+{
+	static const unsigned short tpd_q_offset[] = {
+		EMAC_DESC_CTRL_8,        EMAC_H1TPD_BASE_ADDR_LO,
+		EMAC_H2TPD_BASE_ADDR_LO, EMAC_H3TPD_BASE_ADDR_LO};
+	static const unsigned short rfd_q_offset[] = {
+		EMAC_DESC_CTRL_2,        EMAC_DESC_CTRL_10,
+		EMAC_DESC_CTRL_12,       EMAC_DESC_CTRL_13};
+	static const unsigned short rrd_q_offset[] = {
+		EMAC_DESC_CTRL_5,        EMAC_DESC_CTRL_14,
+		EMAC_DESC_CTRL_15,       EMAC_DESC_CTRL_16};
+
+	/* TPD (Transmit Packet Descriptor) */
+	writel(upper_32_bits(adpt->tx_q.tpd.dma_addr),
+	       adpt->base + EMAC_DESC_CTRL_1);
+
+	writel(lower_32_bits(adpt->tx_q.tpd.dma_addr),
+	       adpt->base + tpd_q_offset[0]);
+
+	writel(adpt->tx_q.tpd.count & TPD_RING_SIZE_BMSK,
+	       adpt->base + EMAC_DESC_CTRL_9);
+
+	/* RFD (Receive Free Descriptor) & RRD (Receive Return Descriptor) */
+	writel(upper_32_bits(adpt->rx_q.rfd.dma_addr),
+	       adpt->base + EMAC_DESC_CTRL_0);
+
+	writel(lower_32_bits(adpt->rx_q.rfd.dma_addr),
+	       adpt->base + rfd_q_offset[0]);
+	writel(lower_32_bits(adpt->rx_q.rrd.dma_addr),
+	       adpt->base + rrd_q_offset[0]);
+
+	writel(adpt->rx_q.rfd.count & RFD_RING_SIZE_BMSK,
+	       adpt->base + EMAC_DESC_CTRL_3);
+	writel(adpt->rx_q.rrd.count & RRD_RING_SIZE_BMSK,
+	       adpt->base + EMAC_DESC_CTRL_6);
+
+	writel(adpt->rxbuf_size & RX_BUFFER_SIZE_BMSK,
+	       adpt->base + EMAC_DESC_CTRL_4);
+
+	writel(0, adpt->base + EMAC_DESC_CTRL_11);
+
+	/* Load all of the base addresses above and ensure that triggering HW to
+	 * read ring pointers is flushed
+	 */
+	writel(1, adpt->base + EMAC_INTER_SRAM_PART9);
+}
+
+/* Config transmit parameters */
+static void emac_mac_tx_config(struct emac_adapter *adpt)
+{
+	u32 val;
+
+	writel((EMAC_MAX_TX_OFFLOAD_THRESH >> 3) &
+	       JUMBO_TASK_OFFLOAD_THRESHOLD_BMSK, adpt->base + EMAC_TXQ_CTRL_1);
+
+	val = (adpt->tpd_burst << NUM_TPD_BURST_PREF_SHFT) &
+	       NUM_TPD_BURST_PREF_BMSK;
+
+	val |= TXQ_MODE | LS_8023_SP;
+	val |= (0x0100 << NUM_TXF_BURST_PREF_SHFT) &
+		NUM_TXF_BURST_PREF_BMSK;
+
+	writel(val, adpt->base + EMAC_TXQ_CTRL_0);
+	emac_reg_update32(adpt->base + EMAC_TXQ_CTRL_2,
+			  (TXF_HWM_BMSK | TXF_LWM_BMSK), 0);
+}
+
+/* Config receive parameters */
+static void emac_mac_rx_config(struct emac_adapter *adpt)
+{
+	u32 val;
+
+	val = (adpt->rfd_burst << NUM_RFD_BURST_PREF_SHFT) &
+	       NUM_RFD_BURST_PREF_BMSK;
+	val |= (SP_IPV6 | CUT_THRU_EN);
+
+	writel(val, adpt->base + EMAC_RXQ_CTRL_0);
+
+	val = readl(adpt->base + EMAC_RXQ_CTRL_1);
+	val &= ~(JUMBO_1KAH_BMSK | RFD_PREF_LOW_THRESHOLD_BMSK |
+		 RFD_PREF_UP_THRESHOLD_BMSK);
+	val |= (JUMBO_1KAH << JUMBO_1KAH_SHFT) |
+		(RFD_PREF_LOW_TH << RFD_PREF_LOW_THRESHOLD_SHFT) |
+		(RFD_PREF_UP_TH  << RFD_PREF_UP_THRESHOLD_SHFT);
+	writel(val, adpt->base + EMAC_RXQ_CTRL_1);
+
+	val = readl(adpt->base + EMAC_RXQ_CTRL_2);
+	val &= ~(RXF_DOF_THRESHOLD_BMSK | RXF_UOF_THRESHOLD_BMSK);
+	val |= (RXF_DOF_THRESFHOLD  << RXF_DOF_THRESHOLD_SHFT) |
+		(RXF_UOF_THRESFHOLD << RXF_UOF_THRESHOLD_SHFT);
+	writel(val, adpt->base + EMAC_RXQ_CTRL_2);
+
+	val = readl(adpt->base + EMAC_RXQ_CTRL_3);
+	val &= ~(RXD_TIMER_BMSK | RXD_THRESHOLD_BMSK);
+	val |= RXD_TH << RXD_THRESHOLD_SHFT;
+	writel(val, adpt->base + EMAC_RXQ_CTRL_3);
+}
+
+/* Config dma */
+static void emac_mac_dma_config(struct emac_adapter *adpt)
+{
+	u32 dma_ctrl = DMAR_REQ_PRI;
+
+	switch (adpt->dma_order) {
+	case emac_dma_ord_in:
+		dma_ctrl |= IN_ORDER_MODE;
+		break;
+	case emac_dma_ord_enh:
+		dma_ctrl |= ENH_ORDER_MODE;
+		break;
+	case emac_dma_ord_out:
+		dma_ctrl |= OUT_ORDER_MODE;
+		break;
+	default:
+		break;
+	}
+
+	dma_ctrl |= (((u32)adpt->dmar_block) << REGRDBLEN_SHFT) &
+						REGRDBLEN_BMSK;
+	dma_ctrl |= (((u32)adpt->dmaw_block) << REGWRBLEN_SHFT) &
+						REGWRBLEN_BMSK;
+	dma_ctrl |= (((u32)adpt->dmar_dly_cnt) << DMAR_DLY_CNT_SHFT) &
+						DMAR_DLY_CNT_BMSK;
+	dma_ctrl |= (((u32)adpt->dmaw_dly_cnt) << DMAW_DLY_CNT_SHFT) &
+						DMAW_DLY_CNT_BMSK;
+
+	/* config DMA and ensure that configuration is flushed to HW */
+	writel(dma_ctrl, adpt->base + EMAC_DMA_CTRL);
+}
+
+/* set MAC address */
+static void emac_set_mac_address(struct emac_adapter *adpt, u8 *addr)
+{
+	u32 sta;
+
+	/* for example: 00-A0-C6-11-22-33
+	 * 0<-->C6112233, 1<-->00A0.
+	 */
+
+	/* low 32bit word */
+	sta = (((u32)addr[2]) << 24) | (((u32)addr[3]) << 16) |
+	      (((u32)addr[4]) << 8)  | (((u32)addr[5]));
+	writel(sta, adpt->base + EMAC_MAC_STA_ADDR0);
+
+	/* hight 32bit word */
+	sta = (((u32)addr[0]) << 8) | (u32)addr[1];
+	writel(sta, adpt->base + EMAC_MAC_STA_ADDR1);
+}
+
+static void emac_mac_config(struct emac_adapter *adpt)
+{
+	struct net_device *netdev = adpt->netdev;
+	unsigned int max_frame;
+	u32 val;
+
+	emac_set_mac_address(adpt, netdev->dev_addr);
+
+	max_frame = netdev->mtu + ETH_HLEN + ETH_FCS_LEN + VLAN_HLEN;
+	adpt->rxbuf_size = netdev->mtu > EMAC_DEF_RX_BUF_SIZE ?
+		ALIGN(max_frame, 8) : EMAC_DEF_RX_BUF_SIZE;
+
+	emac_mac_dma_rings_config(adpt);
+
+	writel(netdev->mtu + ETH_HLEN + VLAN_HLEN + ETH_FCS_LEN,
+	       adpt->base + EMAC_MAX_FRAM_LEN_CTRL);
+
+	emac_mac_tx_config(adpt);
+	emac_mac_rx_config(adpt);
+	emac_mac_dma_config(adpt);
+
+	val = readl(adpt->base + EMAC_AXI_MAST_CTRL);
+	val &= ~(DATA_BYTE_SWAP | MAX_BOUND);
+	val |= MAX_BTYPE;
+	writel(val, adpt->base + EMAC_AXI_MAST_CTRL);
+	writel(0, adpt->base + EMAC_CLK_GATE_CTRL);
+	writel(RX_UNCPL_INT_EN, adpt->base + EMAC_MISC_CTRL);
+}
+
+void emac_mac_reset(struct emac_adapter *adpt)
+{
+	emac_mac_stop(adpt);
+
+	emac_reg_update32(adpt->base + EMAC_DMA_MAS_CTRL, 0, SOFT_RST);
+	usleep_range(100, 150); /* reset may take up to 100usec */
+
+	/* interrupt clear-on-read */
+	emac_reg_update32(adpt->base + EMAC_DMA_MAS_CTRL, 0, INT_RD_CLR_EN);
+}
+
+void emac_mac_start(struct emac_adapter *adpt)
+{
+	struct phy_device *phydev = adpt->phydev;
+	u32 mac, csr1;
+
+	/* enable tx queue */
+	emac_reg_update32(adpt->base + EMAC_TXQ_CTRL_0, 0, TXQ_EN);
+
+	/* enable rx queue */
+	emac_reg_update32(adpt->base + EMAC_RXQ_CTRL_0, 0, RXQ_EN);
+
+	/* enable mac control */
+	mac = readl(adpt->base + EMAC_MAC_CTRL);
+	csr1 = readl(adpt->csr + EMAC_EMAC_WRAPPER_CSR1);
+
+	mac |= TXEN | RXEN;     /* enable RX/TX */
+
+	/* We don't have ethtool support yet, so force flow-control mode
+	 * to 'full' always.
+	 */
+	mac |= TXFC | RXFC;
+
+	/* setup link speed */
+	mac &= ~SPEED_MASK;
+	if (phydev->speed == SPEED_1000) {
+		mac |= SPEED(2);
+		csr1 |= FREQ_MODE;
+	} else {
+		mac |= SPEED(1);
+		csr1 &= ~FREQ_MODE;
+	}
+
+	if (phydev->duplex == DUPLEX_FULL)
+		mac |= FULLD;
+	else
+		mac &= ~FULLD;
+
+	/* other parameters */
+	mac |= (CRCE | PCRCE);
+	mac |= ((adpt->preamble << PRLEN_SHFT) & PRLEN_BMSK);
+	mac |= BROAD_EN;
+	mac |= FLCHK;
+	mac &= ~RX_CHKSUM_EN;
+	mac &= ~(HUGEN | VLAN_STRIP | TPAUSE | SIMR | HUGE | MULTI_ALL |
+		 DEBUG_MODE | SINGLE_PAUSE_MODE);
+
+	writel_relaxed(csr1, adpt->csr + EMAC_EMAC_WRAPPER_CSR1);
+
+	writel_relaxed(mac, adpt->base + EMAC_MAC_CTRL);
+
+	/* enable interrupt read clear, low power sleep mode and
+	 * the irq moderators
+	 */
+
+	writel_relaxed(adpt->irq_mod, adpt->base + EMAC_IRQ_MOD_TIM_INIT);
+	writel_relaxed(INT_RD_CLR_EN | LPW_MODE | IRQ_MODERATOR_EN |
+			IRQ_MODERATOR2_EN, adpt->base + EMAC_DMA_MAS_CTRL);
+
+	emac_mac_mode_config(adpt);
+
+	emac_reg_update32(adpt->base + EMAC_ATHR_HEADER_CTRL,
+			  (HEADER_ENABLE | HEADER_CNT_EN), 0);
+
+	emac_reg_update32(adpt->csr + EMAC_EMAC_WRAPPER_CSR2, 0, WOL_EN);
+}
+
+void emac_mac_stop(struct emac_adapter *adpt)
+{
+	emac_reg_update32(adpt->base + EMAC_RXQ_CTRL_0, RXQ_EN, 0);
+	emac_reg_update32(adpt->base + EMAC_TXQ_CTRL_0, TXQ_EN, 0);
+	emac_reg_update32(adpt->base + EMAC_MAC_CTRL, TXEN | RXEN, 0);
+	usleep_range(1000, 1050); /* stopping mac may take upto 1msec */
+}
+
+/* Free all descriptors of given transmit queue */
+static void emac_tx_q_descs_free(struct emac_adapter *adpt)
+{
+	struct emac_tx_queue *tx_q = &adpt->tx_q;
+	unsigned int i;
+	size_t size;
+
+	/* ring already cleared, nothing to do */
+	if (!tx_q->tpd.tpbuff)
+		return;
+
+	for (i = 0; i < tx_q->tpd.count; i++) {
+		struct emac_buffer *tpbuf = GET_TPD_BUFFER(tx_q, i);
+
+		if (tpbuf->dma_addr) {
+			dma_unmap_single(adpt->netdev->dev.parent,
+					 tpbuf->dma_addr, tpbuf->length,
+					 DMA_TO_DEVICE);
+			tpbuf->dma_addr = 0;
+		}
+		if (tpbuf->skb) {
+			dev_kfree_skb_any(tpbuf->skb);
+			tpbuf->skb = NULL;
+		}
+	}
+
+	size = sizeof(struct emac_buffer) * tx_q->tpd.count;
+	memset(tx_q->tpd.tpbuff, 0, size);
+
+	/* clear the descriptor ring */
+	memset(tx_q->tpd.v_addr, 0, tx_q->tpd.size);
+
+	tx_q->tpd.consume_idx = 0;
+	tx_q->tpd.produce_idx = 0;
+}
+
+/* Free all descriptors of given receive queue */
+static void emac_rx_q_free_descs(struct emac_adapter *adpt)
+{
+	struct device *dev = adpt->netdev->dev.parent;
+	struct emac_rx_queue *rx_q = &adpt->rx_q;
+	unsigned int i;
+	size_t size;
+
+	/* ring already cleared, nothing to do */
+	if (!rx_q->rfd.rfbuff)
+		return;
+
+	for (i = 0; i < rx_q->rfd.count; i++) {
+		struct emac_buffer *rfbuf = GET_RFD_BUFFER(rx_q, i);
+
+		if (rfbuf->dma_addr) {
+			dma_unmap_single(dev, rfbuf->dma_addr, rfbuf->length,
+					 DMA_FROM_DEVICE);
+			rfbuf->dma_addr = 0;
+		}
+		if (rfbuf->skb) {
+			dev_kfree_skb(rfbuf->skb);
+			rfbuf->skb = NULL;
+		}
+	}
+
+	size =  sizeof(struct emac_buffer) * rx_q->rfd.count;
+	memset(rx_q->rfd.rfbuff, 0, size);
+
+	/* clear the descriptor rings */
+	memset(rx_q->rrd.v_addr, 0, rx_q->rrd.size);
+	rx_q->rrd.produce_idx = 0;
+	rx_q->rrd.consume_idx = 0;
+
+	memset(rx_q->rfd.v_addr, 0, rx_q->rfd.size);
+	rx_q->rfd.produce_idx = 0;
+	rx_q->rfd.consume_idx = 0;
+}
+
+/* Free all buffers associated with given transmit queue */
+static void emac_tx_q_bufs_free(struct emac_adapter *adpt)
+{
+	struct emac_tx_queue *tx_q = &adpt->tx_q;
+
+	emac_tx_q_descs_free(adpt);
+
+	kfree(tx_q->tpd.tpbuff);
+	tx_q->tpd.tpbuff = NULL;
+	tx_q->tpd.v_addr = NULL;
+	tx_q->tpd.dma_addr = 0;
+	tx_q->tpd.size = 0;
+}
+
+/* Allocate TX descriptor ring for the given transmit queue */
+static int emac_tx_q_desc_alloc(struct emac_adapter *adpt,
+				struct emac_tx_queue *tx_q)
+{
+	struct emac_ring_header *ring_header = &adpt->ring_header;
+	size_t size;
+
+	size = sizeof(struct emac_buffer) * tx_q->tpd.count;
+	tx_q->tpd.tpbuff = kzalloc(size, GFP_KERNEL);
+	if (!tx_q->tpd.tpbuff)
+		return -ENOMEM;
+
+	tx_q->tpd.size = tx_q->tpd.count * (adpt->tpd_size * 4);
+	tx_q->tpd.dma_addr = ring_header->dma_addr + ring_header->used;
+	tx_q->tpd.v_addr = ring_header->v_addr + ring_header->used;
+	ring_header->used += ALIGN(tx_q->tpd.size, 8);
+	tx_q->tpd.produce_idx = 0;
+	tx_q->tpd.consume_idx = 0;
+
+	return 0;
+}
+
+/* Free all buffers associated with given transmit queue */
+static void emac_rx_q_bufs_free(struct emac_adapter *adpt)
+{
+	struct emac_rx_queue *rx_q = &adpt->rx_q;
+
+	emac_rx_q_free_descs(adpt);
+
+	kfree(rx_q->rfd.rfbuff);
+	rx_q->rfd.rfbuff   = NULL;
+
+	rx_q->rfd.v_addr   = NULL;
+	rx_q->rfd.dma_addr = 0;
+	rx_q->rfd.size     = 0;
+
+	rx_q->rrd.v_addr   = NULL;
+	rx_q->rrd.dma_addr = 0;
+	rx_q->rrd.size     = 0;
+}
+
+/* Allocate RX descriptor rings for the given receive queue */
+static int emac_rx_descs_alloc(struct emac_adapter *adpt)
+{
+	struct emac_ring_header *ring_header = &adpt->ring_header;
+	struct emac_rx_queue *rx_q = &adpt->rx_q;
+	size_t size;
+
+	size = sizeof(struct emac_buffer) * rx_q->rfd.count;
+	rx_q->rfd.rfbuff = kzalloc(size, GFP_KERNEL);
+	if (!rx_q->rfd.rfbuff)
+		return -ENOMEM;
+
+	rx_q->rrd.size = rx_q->rrd.count * (adpt->rrd_size * 4);
+	rx_q->rfd.size = rx_q->rfd.count * (adpt->rfd_size * 4);
+
+	rx_q->rrd.dma_addr = ring_header->dma_addr + ring_header->used;
+	rx_q->rrd.v_addr   = ring_header->v_addr + ring_header->used;
+	ring_header->used += ALIGN(rx_q->rrd.size, 8);
+
+	rx_q->rfd.dma_addr = ring_header->dma_addr + ring_header->used;
+	rx_q->rfd.v_addr   = ring_header->v_addr + ring_header->used;
+	ring_header->used += ALIGN(rx_q->rfd.size, 8);
+
+	rx_q->rrd.produce_idx = 0;
+	rx_q->rrd.consume_idx = 0;
+
+	rx_q->rfd.produce_idx = 0;
+	rx_q->rfd.consume_idx = 0;
+
+	return 0;
+}
+
+/* Allocate all TX and RX descriptor rings */
+int emac_mac_rx_tx_rings_alloc_all(struct emac_adapter *adpt)
+{
+	struct emac_ring_header *ring_header = &adpt->ring_header;
+	struct device *dev = adpt->netdev->dev.parent;
+	unsigned int num_tx_descs = adpt->tx_desc_cnt;
+	unsigned int num_rx_descs = adpt->rx_desc_cnt;
+	int ret;
+
+	adpt->tx_q.tpd.count = adpt->tx_desc_cnt;
+
+	adpt->rx_q.rrd.count = adpt->rx_desc_cnt;
+	adpt->rx_q.rfd.count = adpt->rx_desc_cnt;
+
+	/* Ring DMA buffer. Each ring may need up to 8 bytes for alignment,
+	 * hence the additional padding bytes are allocated.
+	 */
+	ring_header->size = num_tx_descs * (adpt->tpd_size * 4) +
+			    num_rx_descs * (adpt->rfd_size * 4) +
+			    num_rx_descs * (adpt->rrd_size * 4) +
+			    8 + 2 * 8; /* 8 byte per one Tx and two Rx rings */
+
+	ring_header->used = 0;
+	ring_header->v_addr = dma_zalloc_coherent(dev, ring_header->size,
+						 &ring_header->dma_addr,
+						 GFP_KERNEL);
+	if (!ring_header->v_addr)
+		return -ENOMEM;
+
+	ring_header->used = ALIGN(ring_header->dma_addr, 8) -
+							ring_header->dma_addr;
+
+	ret = emac_tx_q_desc_alloc(adpt, &adpt->tx_q);
+	if (ret) {
+		netdev_err(adpt->netdev, "error: Tx Queue alloc failed\n");
+		goto err_alloc_tx;
+	}
+
+	ret = emac_rx_descs_alloc(adpt);
+	if (ret) {
+		netdev_err(adpt->netdev, "error: Rx Queue alloc failed\n");
+		goto err_alloc_rx;
+	}
+
+	return 0;
+
+err_alloc_rx:
+	emac_tx_q_bufs_free(adpt);
+err_alloc_tx:
+	dma_free_coherent(dev, ring_header->size,
+			  ring_header->v_addr, ring_header->dma_addr);
+
+	ring_header->v_addr   = NULL;
+	ring_header->dma_addr = 0;
+	ring_header->size     = 0;
+	ring_header->used     = 0;
+
+	return ret;
+}
+
+/* Free all TX and RX descriptor rings */
+void emac_mac_rx_tx_rings_free_all(struct emac_adapter *adpt)
+{
+	struct emac_ring_header *ring_header = &adpt->ring_header;
+	struct device *dev = adpt->netdev->dev.parent;
+
+	emac_tx_q_bufs_free(adpt);
+	emac_rx_q_bufs_free(adpt);
+
+	dma_free_coherent(dev, ring_header->size,
+			  ring_header->v_addr, ring_header->dma_addr);
+
+	ring_header->v_addr   = NULL;
+	ring_header->dma_addr = 0;
+	ring_header->size     = 0;
+	ring_header->used     = 0;
+}
+
+/* Initialize descriptor rings */
+static void emac_mac_rx_tx_ring_reset_all(struct emac_adapter *adpt)
+{
+	unsigned int i;
+
+	adpt->tx_q.tpd.produce_idx = 0;
+	adpt->tx_q.tpd.consume_idx = 0;
+	for (i = 0; i < adpt->tx_q.tpd.count; i++)
+		adpt->tx_q.tpd.tpbuff[i].dma_addr = 0;
+
+	adpt->rx_q.rrd.produce_idx = 0;
+	adpt->rx_q.rrd.consume_idx = 0;
+	adpt->rx_q.rfd.produce_idx = 0;
+	adpt->rx_q.rfd.consume_idx = 0;
+	for (i = 0; i < adpt->rx_q.rfd.count; i++)
+		adpt->rx_q.rfd.rfbuff[i].dma_addr = 0;
+}
+
+/* Produce new receive free descriptor */
+static void emac_mac_rx_rfd_create(struct emac_adapter *adpt,
+				   struct emac_rx_queue *rx_q,
+				   dma_addr_t addr)
+{
+	u32 *hw_rfd = EMAC_RFD(rx_q, adpt->rfd_size, rx_q->rfd.produce_idx);
+
+	*(hw_rfd++) = lower_32_bits(addr);
+	*hw_rfd = upper_32_bits(addr);
+
+	if (++rx_q->rfd.produce_idx == rx_q->rfd.count)
+		rx_q->rfd.produce_idx = 0;
+}
+
+/* Fill up receive queue's RFD with preallocated receive buffers */
+static void emac_mac_rx_descs_refill(struct emac_adapter *adpt,
+				    struct emac_rx_queue *rx_q)
+{
+	struct emac_buffer *curr_rxbuf;
+	struct emac_buffer *next_rxbuf;
+	unsigned int count = 0;
+	u32 next_produce_idx;
+
+	next_produce_idx = rx_q->rfd.produce_idx + 1;
+	if (next_produce_idx == rx_q->rfd.count)
+		next_produce_idx = 0;
+
+	curr_rxbuf = GET_RFD_BUFFER(rx_q, rx_q->rfd.produce_idx);
+	next_rxbuf = GET_RFD_BUFFER(rx_q, next_produce_idx);
+
+	/* this always has a blank rx_buffer*/
+	while (!next_rxbuf->dma_addr) {
+		struct sk_buff *skb;
+		int ret;
+
+		skb = netdev_alloc_skb_ip_align(adpt->netdev, adpt->rxbuf_size);
+		if (!skb)
+			break;
+
+		curr_rxbuf->dma_addr =
+			dma_map_single(adpt->netdev->dev.parent, skb->data,
+				       curr_rxbuf->length, DMA_FROM_DEVICE);
+		ret = dma_mapping_error(adpt->netdev->dev.parent,
+					curr_rxbuf->dma_addr);
+		if (ret) {
+			dev_kfree_skb(skb);
+			break;
+		}
+		curr_rxbuf->skb = skb;
+		curr_rxbuf->length = adpt->rxbuf_size;
+
+		emac_mac_rx_rfd_create(adpt, rx_q, curr_rxbuf->dma_addr);
+		next_produce_idx = rx_q->rfd.produce_idx + 1;
+		if (next_produce_idx == rx_q->rfd.count)
+			next_produce_idx = 0;
+
+		curr_rxbuf = GET_RFD_BUFFER(rx_q, rx_q->rfd.produce_idx);
+		next_rxbuf = GET_RFD_BUFFER(rx_q, next_produce_idx);
+		count++;
+	}
+
+	if (count) {
+		u32 prod_idx = (rx_q->rfd.produce_idx << rx_q->produce_shift) &
+				rx_q->produce_mask;
+		emac_reg_update32(adpt->base + rx_q->produce_reg,
+				  rx_q->produce_mask, prod_idx);
+	}
+}
+
+static void emac_adjust_link(struct net_device *netdev)
+{
+	struct emac_adapter *adpt = netdev_priv(netdev);
+	struct phy_device *phydev = netdev->phydev;
+
+	if (phydev->link)
+		emac_mac_start(adpt);
+	else
+		emac_mac_stop(adpt);
+
+	phy_print_status(phydev);
+}
+
+/* Bringup the interface/HW */
+int emac_mac_up(struct emac_adapter *adpt)
+{
+	struct net_device *netdev = adpt->netdev;
+	struct emac_irq	*irq = &adpt->irq;
+	int ret;
+
+	emac_mac_rx_tx_ring_reset_all(adpt);
+	emac_mac_config(adpt);
+
+	ret = request_irq(irq->irq, emac_isr, 0, EMAC_MAC_IRQ_RES, irq);
+	if (ret) {
+		netdev_err(adpt->netdev, "could not request %s irq\n",
+			   EMAC_MAC_IRQ_RES);
+		return ret;
+	}
+
+	emac_mac_rx_descs_refill(adpt, &adpt->rx_q);
+
+	ret = phy_connect_direct(netdev, adpt->phydev, emac_adjust_link,
+				 PHY_INTERFACE_MODE_SGMII);
+	if (ret) {
+		netdev_err(adpt->netdev, "could not connect phy\n");
+		free_irq(irq->irq, irq);
+		return ret;
+	}
+
+	/* enable mac irq */
+	writel((u32)~DIS_INT, adpt->base + EMAC_INT_STATUS);
+	writel(adpt->irq.mask, adpt->base + EMAC_INT_MASK);
+
+	adpt->phydev->irq = PHY_IGNORE_INTERRUPT;
+	phy_start(adpt->phydev);
+
+	napi_enable(&adpt->rx_q.napi);
+	netif_start_queue(netdev);
+
+	return 0;
+}
+
+/* Bring down the interface/HW */
+void emac_mac_down(struct emac_adapter *adpt)
+{
+	struct net_device *netdev = adpt->netdev;
+
+	netif_stop_queue(netdev);
+	napi_disable(&adpt->rx_q.napi);
+
+	phy_stop(adpt->phydev);
+	phy_disconnect(adpt->phydev);
+
+	/* disable mac irq */
+	writel(DIS_INT, adpt->base + EMAC_INT_STATUS);
+	writel(0, adpt->base + EMAC_INT_MASK);
+	synchronize_irq(adpt->irq.irq);
+	free_irq(adpt->irq.irq, &adpt->irq);
+
+	emac_mac_reset(adpt);
+
+	emac_tx_q_descs_free(adpt);
+	netdev_reset_queue(adpt->netdev);
+	emac_rx_q_free_descs(adpt);
+}
+
+/* Consume next received packet descriptor */
+static bool emac_rx_process_rrd(struct emac_adapter *adpt,
+				struct emac_rx_queue *rx_q,
+				struct emac_rrd *rrd)
+{
+	u32 *hw_rrd = EMAC_RRD(rx_q, adpt->rrd_size, rx_q->rrd.consume_idx);
+
+	rrd->word[3] = *(hw_rrd + 3);
+
+	if (!RRD_UPDT(rrd))
+		return false;
+
+	rrd->word[4] = 0;
+	rrd->word[5] = 0;
+
+	rrd->word[0] = *(hw_rrd++);
+	rrd->word[1] = *(hw_rrd++);
+	rrd->word[2] = *(hw_rrd++);
+
+	if (unlikely(RRD_NOR(rrd) != 1)) {
+		netdev_err(adpt->netdev,
+			   "error: multi-RFD not support yet! nor:%lu\n",
+			   RRD_NOR(rrd));
+	}
+
+	/* mark rrd as processed */
+	RRD_UPDT_SET(rrd, 0);
+	*hw_rrd = rrd->word[3];
+
+	if (++rx_q->rrd.consume_idx == rx_q->rrd.count)
+		rx_q->rrd.consume_idx = 0;
+
+	return true;
+}
+
+/* Produce new transmit descriptor */
+static void emac_tx_tpd_create(struct emac_adapter *adpt,
+			       struct emac_tx_queue *tx_q, struct emac_tpd *tpd)
+{
+	u32 *hw_tpd;
+
+	tx_q->tpd.last_produce_idx = tx_q->tpd.produce_idx;
+	hw_tpd = EMAC_TPD(tx_q, adpt->tpd_size, tx_q->tpd.produce_idx);
+
+	if (++tx_q->tpd.produce_idx == tx_q->tpd.count)
+		tx_q->tpd.produce_idx = 0;
+
+	*(hw_tpd++) = tpd->word[0];
+	*(hw_tpd++) = tpd->word[1];
+	*(hw_tpd++) = tpd->word[2];
+	*hw_tpd = tpd->word[3];
+}
+
+/* Mark the last transmit descriptor as such (for the transmit packet) */
+static void emac_tx_tpd_mark_last(struct emac_adapter *adpt,
+				  struct emac_tx_queue *tx_q)
+{
+	u32 *hw_tpd =
+		EMAC_TPD(tx_q, adpt->tpd_size, tx_q->tpd.last_produce_idx);
+	u32 tmp_tpd;
+
+	tmp_tpd = *(hw_tpd + 1);
+	tmp_tpd |= EMAC_TPD_LAST_FRAGMENT;
+	*(hw_tpd + 1) = tmp_tpd;
+}
+
+static void emac_rx_rfd_clean(struct emac_rx_queue *rx_q, struct emac_rrd *rrd)
+{
+	struct emac_buffer *rfbuf = rx_q->rfd.rfbuff;
+	u32 consume_idx = RRD_SI(rrd);
+	unsigned int i;
+
+	for (i = 0; i < RRD_NOR(rrd); i++) {
+		rfbuf[consume_idx].skb = NULL;
+		if (++consume_idx == rx_q->rfd.count)
+			consume_idx = 0;
+	}
+
+	rx_q->rfd.consume_idx = consume_idx;
+	rx_q->rfd.process_idx = consume_idx;
+}
+
+/* Push the received skb to upper layers */
+static void emac_receive_skb(struct emac_rx_queue *rx_q,
+			     struct sk_buff *skb,
+			     u16 vlan_tag, bool vlan_flag)
+{
+	if (vlan_flag) {
+		u16 vlan;
+
+		EMAC_TAG_TO_VLAN(vlan_tag, vlan);
+		__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), vlan);
+	}
+
+	napi_gro_receive(&rx_q->napi, skb);
+}
+
+/* Process receive event */
+void emac_mac_rx_process(struct emac_adapter *adpt, struct emac_rx_queue *rx_q,
+			 int *num_pkts, int max_pkts)
+{
+	u32 proc_idx, hw_consume_idx, num_consume_pkts;
+	struct net_device *netdev  = adpt->netdev;
+	struct emac_buffer *rfbuf;
+	unsigned int count = 0;
+	struct emac_rrd rrd;
+	struct sk_buff *skb;
+	u32 reg;
+
+	reg = readl_relaxed(adpt->base + rx_q->consume_reg);
+
+	hw_consume_idx = (reg & rx_q->consume_mask) >> rx_q->consume_shift;
+	num_consume_pkts = (hw_consume_idx >= rx_q->rrd.consume_idx) ?
+		(hw_consume_idx -  rx_q->rrd.consume_idx) :
+		(hw_consume_idx + rx_q->rrd.count - rx_q->rrd.consume_idx);
+
+	do {
+		if (!num_consume_pkts)
+			break;
+
+		if (!emac_rx_process_rrd(adpt, rx_q, &rrd))
+			break;
+
+		if (likely(RRD_NOR(&rrd) == 1)) {
+			/* good receive */
+			rfbuf = GET_RFD_BUFFER(rx_q, RRD_SI(&rrd));
+			dma_unmap_single(adpt->netdev->dev.parent,
+					 rfbuf->dma_addr, rfbuf->length,
+					 DMA_FROM_DEVICE);
+			rfbuf->dma_addr = 0;
+			skb = rfbuf->skb;
+		} else {
+			netdev_err(adpt->netdev,
+				   "error: multi-RFD not support yet!\n");
+			break;
+		}
+		emac_rx_rfd_clean(rx_q, &rrd);
+		num_consume_pkts--;
+		count++;
+
+		/* Due to a HW issue in L4 check sum detection (UDP/TCP frags
+		 * with DF set are marked as error), drop packets based on the
+		 * error mask rather than the summary bit (ignoring L4F errors)
+		 */
+		if (rrd.word[EMAC_RRD_STATS_DW_IDX] & EMAC_RRD_ERROR) {
+			netif_dbg(adpt, rx_status, adpt->netdev,
+				  "Drop error packet[RRD: 0x%x:0x%x:0x%x:0x%x]\n",
+				  rrd.word[0], rrd.word[1],
+				  rrd.word[2], rrd.word[3]);
+
+			dev_kfree_skb(skb);
+			continue;
+		}
+
+		skb_put(skb, RRD_PKT_SIZE(&rrd) - ETH_FCS_LEN);
+		skb->dev = netdev;
+		skb->protocol = eth_type_trans(skb, skb->dev);
+		if (netdev->features & NETIF_F_RXCSUM)
+			skb->ip_summed = RRD_L4F(&rrd) ?
+					  CHECKSUM_NONE : CHECKSUM_UNNECESSARY;
+		else
+			skb_checksum_none_assert(skb);
+
+		emac_receive_skb(rx_q, skb, (u16)RRD_CVALN_TAG(&rrd),
+				 (bool)RRD_CVTAG(&rrd));
+
+		netdev->last_rx = jiffies;
+		(*num_pkts)++;
+	} while (*num_pkts < max_pkts);
+
+	if (count) {
+		proc_idx = (rx_q->rfd.process_idx << rx_q->process_shft) &
+				rx_q->process_mask;
+		emac_reg_update32(adpt->base + rx_q->process_reg,
+				  rx_q->process_mask, proc_idx);
+		emac_mac_rx_descs_refill(adpt, rx_q);
+	}
+}
+
+/* get the number of free transmit descriptors */
+static unsigned int emac_tpd_num_free_descs(struct emac_tx_queue *tx_q)
+{
+	u32 produce_idx = tx_q->tpd.produce_idx;
+	u32 consume_idx = tx_q->tpd.consume_idx;
+
+	return (consume_idx > produce_idx) ?
+		(consume_idx - produce_idx - 1) :
+		(tx_q->tpd.count + consume_idx - produce_idx - 1);
+}
+
+/* Process transmit event */
+void emac_mac_tx_process(struct emac_adapter *adpt, struct emac_tx_queue *tx_q)
+{
+	u32 reg = readl_relaxed(adpt->base + tx_q->consume_reg);
+	u32 hw_consume_idx, pkts_compl = 0, bytes_compl = 0;
+	struct emac_buffer *tpbuf;
+
+	hw_consume_idx = (reg & tx_q->consume_mask) >> tx_q->consume_shift;
+
+	while (tx_q->tpd.consume_idx != hw_consume_idx) {
+		tpbuf = GET_TPD_BUFFER(tx_q, tx_q->tpd.consume_idx);
+		if (tpbuf->dma_addr) {
+			dma_unmap_single(adpt->netdev->dev.parent,
+					 tpbuf->dma_addr, tpbuf->length,
+					 DMA_TO_DEVICE);
+			tpbuf->dma_addr = 0;
+		}
+
+		if (tpbuf->skb) {
+			pkts_compl++;
+			bytes_compl += tpbuf->skb->len;
+			dev_kfree_skb_irq(tpbuf->skb);
+			tpbuf->skb = NULL;
+		}
+
+		if (++tx_q->tpd.consume_idx == tx_q->tpd.count)
+			tx_q->tpd.consume_idx = 0;
+	}
+
+	netdev_completed_queue(adpt->netdev, pkts_compl, bytes_compl);
+
+	if (netif_queue_stopped(adpt->netdev))
+		if (emac_tpd_num_free_descs(tx_q) > (MAX_SKB_FRAGS + 1))
+			netif_wake_queue(adpt->netdev);
+}
+
+/* Initialize all queue data structures */
+void emac_mac_rx_tx_ring_init_all(struct platform_device *pdev,
+				  struct emac_adapter *adpt)
+{
+	adpt->rx_q.netdev = adpt->netdev;
+
+	adpt->rx_q.produce_reg  = EMAC_MAILBOX_0;
+	adpt->rx_q.produce_mask = RFD0_PROD_IDX_BMSK;
+	adpt->rx_q.produce_shift = RFD0_PROD_IDX_SHFT;
+
+	adpt->rx_q.process_reg  = EMAC_MAILBOX_0;
+	adpt->rx_q.process_mask = RFD0_PROC_IDX_BMSK;
+	adpt->rx_q.process_shft = RFD0_PROC_IDX_SHFT;
+
+	adpt->rx_q.consume_reg  = EMAC_MAILBOX_3;
+	adpt->rx_q.consume_mask = RFD0_CONS_IDX_BMSK;
+	adpt->rx_q.consume_shift = RFD0_CONS_IDX_SHFT;
+
+	adpt->rx_q.irq          = &adpt->irq;
+	adpt->rx_q.intr         = adpt->irq.mask & ISR_RX_PKT;
+
+	adpt->tx_q.produce_reg  = EMAC_MAILBOX_15;
+	adpt->tx_q.produce_mask = NTPD_PROD_IDX_BMSK;
+	adpt->tx_q.produce_shift = NTPD_PROD_IDX_SHFT;
+
+	adpt->tx_q.consume_reg  = EMAC_MAILBOX_2;
+	adpt->tx_q.consume_mask = NTPD_CONS_IDX_BMSK;
+	adpt->tx_q.consume_shift = NTPD_CONS_IDX_SHFT;
+}
+
+/* Fill up transmit descriptors with TSO and Checksum offload information */
+static int emac_tso_csum(struct emac_adapter *adpt,
+			 struct emac_tx_queue *tx_q,
+			 struct sk_buff *skb,
+			 struct emac_tpd *tpd)
+{
+	unsigned int hdr_len;
+	int ret;
+
+	if (skb_is_gso(skb)) {
+		if (skb_header_cloned(skb)) {
+			ret = pskb_expand_head(skb, 0, 0, GFP_ATOMIC);
+			if (unlikely(ret))
+				return ret;
+		}
+
+		if (skb->protocol == htons(ETH_P_IP)) {
+			u32 pkt_len = ((unsigned char *)ip_hdr(skb) - skb->data)
+				       + ntohs(ip_hdr(skb)->tot_len);
+			if (skb->len > pkt_len)
+				pskb_trim(skb, pkt_len);
+		}
+
+		hdr_len = skb_transport_offset(skb) + tcp_hdrlen(skb);
+		if (unlikely(skb->len == hdr_len)) {
+			/* we only need to do csum */
+			netif_warn(adpt, tx_err, adpt->netdev,
+				   "tso not needed for packet with 0 data\n");
+			goto do_csum;
+		}
+
+		if (skb_shinfo(skb)->gso_type & SKB_GSO_TCPV4) {
+			ip_hdr(skb)->check = 0;
+			tcp_hdr(skb)->check =
+				~csum_tcpudp_magic(ip_hdr(skb)->saddr,
+						   ip_hdr(skb)->daddr,
+						   0, IPPROTO_TCP, 0);
+			TPD_IPV4_SET(tpd, 1);
+		}
+
+		if (skb_shinfo(skb)->gso_type & SKB_GSO_TCPV6) {
+			/* ipv6 tso need an extra tpd */
+			struct emac_tpd extra_tpd;
+
+			memset(tpd, 0, sizeof(*tpd));
+			memset(&extra_tpd, 0, sizeof(extra_tpd));
+
+			ipv6_hdr(skb)->payload_len = 0;
+			tcp_hdr(skb)->check =
+				~csum_ipv6_magic(&ipv6_hdr(skb)->saddr,
+						 &ipv6_hdr(skb)->daddr,
+						 0, IPPROTO_TCP, 0);
+			TPD_PKT_LEN_SET(&extra_tpd, skb->len);
+			TPD_LSO_SET(&extra_tpd, 1);
+			TPD_LSOV_SET(&extra_tpd, 1);
+			emac_tx_tpd_create(adpt, tx_q, &extra_tpd);
+			TPD_LSOV_SET(tpd, 1);
+		}
+
+		TPD_LSO_SET(tpd, 1);
+		TPD_TCPHDR_OFFSET_SET(tpd, skb_transport_offset(skb));
+		TPD_MSS_SET(tpd, skb_shinfo(skb)->gso_size);
+		return 0;
+	}
+
+do_csum:
+	if (likely(skb->ip_summed == CHECKSUM_PARTIAL)) {
+		unsigned int css, cso;
+
+		cso = skb_transport_offset(skb);
+		if (unlikely(cso & 0x1)) {
+			netdev_err(adpt->netdev,
+				   "error: payload offset should be even\n");
+			return -EINVAL;
+		}
+		css = cso + skb->csum_offset;
+
+		TPD_PAYLOAD_OFFSET_SET(tpd, cso >> 1);
+		TPD_CXSUM_OFFSET_SET(tpd, css >> 1);
+		TPD_CSX_SET(tpd, 1);
+	}
+
+	return 0;
+}
+
+/* Fill up transmit descriptors */
+static void emac_tx_fill_tpd(struct emac_adapter *adpt,
+			     struct emac_tx_queue *tx_q, struct sk_buff *skb,
+			     struct emac_tpd *tpd)
+{
+	unsigned int nr_frags = skb_shinfo(skb)->nr_frags;
+	unsigned int first = tx_q->tpd.produce_idx;
+	unsigned int len = skb_headlen(skb);
+	struct emac_buffer *tpbuf = NULL;
+	unsigned int mapped_len = 0;
+	unsigned int i;
+	int count = 0;
+	int ret;
+
+	/* if Large Segment Offload is (in TCP Segmentation Offload struct) */
+	if (TPD_LSO(tpd)) {
+		mapped_len = skb_transport_offset(skb) + tcp_hdrlen(skb);
+
+		tpbuf = GET_TPD_BUFFER(tx_q, tx_q->tpd.produce_idx);
+		tpbuf->length = mapped_len;
+		tpbuf->dma_addr = dma_map_single(adpt->netdev->dev.parent,
+						 skb->data, tpbuf->length,
+						 DMA_TO_DEVICE);
+		ret = dma_mapping_error(adpt->netdev->dev.parent,
+					tpbuf->dma_addr);
+		if (ret)
+			goto error;
+
+		TPD_BUFFER_ADDR_L_SET(tpd, lower_32_bits(tpbuf->dma_addr));
+		TPD_BUFFER_ADDR_H_SET(tpd, upper_32_bits(tpbuf->dma_addr));
+		TPD_BUF_LEN_SET(tpd, tpbuf->length);
+		emac_tx_tpd_create(adpt, tx_q, tpd);
+		count++;
+	}
+
+	if (mapped_len < len) {
+		tpbuf = GET_TPD_BUFFER(tx_q, tx_q->tpd.produce_idx);
+		tpbuf->length = len - mapped_len;
+		tpbuf->dma_addr = dma_map_single(adpt->netdev->dev.parent,
+						 skb->data + mapped_len,
+						 tpbuf->length, DMA_TO_DEVICE);
+		ret = dma_mapping_error(adpt->netdev->dev.parent,
+					tpbuf->dma_addr);
+		if (ret)
+			goto error;
+
+		TPD_BUFFER_ADDR_L_SET(tpd, lower_32_bits(tpbuf->dma_addr));
+		TPD_BUFFER_ADDR_H_SET(tpd, upper_32_bits(tpbuf->dma_addr));
+		TPD_BUF_LEN_SET(tpd, tpbuf->length);
+		emac_tx_tpd_create(adpt, tx_q, tpd);
+		count++;
+	}
+
+	for (i = 0; i < nr_frags; i++) {
+		struct skb_frag_struct *frag;
+
+		frag = &skb_shinfo(skb)->frags[i];
+
+		tpbuf = GET_TPD_BUFFER(tx_q, tx_q->tpd.produce_idx);
+		tpbuf->length = frag->size;
+		tpbuf->dma_addr = dma_map_page(adpt->netdev->dev.parent,
+					       frag->page.p, frag->page_offset,
+					       tpbuf->length, DMA_TO_DEVICE);
+		ret = dma_mapping_error(adpt->netdev->dev.parent,
+					tpbuf->dma_addr);
+		if (ret)
+			goto error;
+
+		TPD_BUFFER_ADDR_L_SET(tpd, lower_32_bits(tpbuf->dma_addr));
+		TPD_BUFFER_ADDR_H_SET(tpd, upper_32_bits(tpbuf->dma_addr));
+		TPD_BUF_LEN_SET(tpd, tpbuf->length);
+		emac_tx_tpd_create(adpt, tx_q, tpd);
+		count++;
+	}
+
+	/* The last tpd */
+	wmb();
+	emac_tx_tpd_mark_last(adpt, tx_q);
+
+	/* The last buffer info contain the skb address,
+	 * so it will be freed after unmap
+	 */
+	tpbuf->skb = skb;
+
+	return;
+
+error:
+	/* One of the memory mappings failed, so undo everything */
+	tx_q->tpd.produce_idx = first;
+
+	while (count--) {
+		tpbuf = GET_TPD_BUFFER(tx_q, first);
+		dma_unmap_page(adpt->netdev->dev.parent, tpbuf->dma_addr,
+			       tpbuf->length, DMA_TO_DEVICE);
+		tpbuf->dma_addr = 0;
+		tpbuf->length = 0;
+
+		if (++first == tx_q->tpd.count)
+			first = 0;
+	}
+
+	dev_kfree_skb(skb);
+}
+
+/* Transmit the packet using specified transmit queue */
+int emac_mac_tx_buf_send(struct emac_adapter *adpt, struct emac_tx_queue *tx_q,
+			 struct sk_buff *skb)
+{
+	struct emac_tpd tpd;
+	u32 prod_idx;
+
+	memset(&tpd, 0, sizeof(tpd));
+
+	if (emac_tso_csum(adpt, tx_q, skb, &tpd) != 0) {
+		dev_kfree_skb_any(skb);
+		return NETDEV_TX_OK;
+	}
+
+	if (skb_vlan_tag_present(skb)) {
+		u16 tag;
+
+		EMAC_VLAN_TO_TAG(skb_vlan_tag_get(skb), tag);
+		TPD_CVLAN_TAG_SET(&tpd, tag);
+		TPD_INSTC_SET(&tpd, 1);
+	}
+
+	if (skb_network_offset(skb) != ETH_HLEN)
+		TPD_TYP_SET(&tpd, 1);
+
+	emac_tx_fill_tpd(adpt, tx_q, skb, &tpd);
+
+	netdev_sent_queue(adpt->netdev, skb->len);
+
+	/* Make sure the are enough free descriptors to hold one
+	 * maximum-sized SKB.  We need one desc for each fragment,
+	 * one for the checksum (emac_tso_csum), one for TSO, and
+	 * and one for the SKB header.
+	 */
+	if (emac_tpd_num_free_descs(tx_q) < (MAX_SKB_FRAGS + 3))
+		netif_stop_queue(adpt->netdev);
+
+	/* update produce idx */
+	prod_idx = (tx_q->tpd.produce_idx << tx_q->produce_shift) &
+		    tx_q->produce_mask;
+	emac_reg_update32(adpt->base + tx_q->produce_reg,
+			  tx_q->produce_mask, prod_idx);
+
+	return NETDEV_TX_OK;
+}
diff --git a/drivers/net/ethernet/qualcomm/emac/emac-mac.h b/drivers/net/ethernet/qualcomm/emac/emac-mac.h
new file mode 100644
index 000000000000..f3aa24dc4a29
--- /dev/null
+++ b/drivers/net/ethernet/qualcomm/emac/emac-mac.h
@@ -0,0 +1,248 @@
+/* Copyright (c) 2013-2016, 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.
+ */
+
+/* EMAC DMA HW engine uses three rings:
+ * Tx:
+ *   TPD: Transmit Packet Descriptor ring.
+ * Rx:
+ *   RFD: Receive Free Descriptor ring.
+ *     Ring of descriptors with empty buffers to be filled by Rx HW.
+ *   RRD: Receive Return Descriptor ring.
+ *     Ring of descriptors with buffers filled with received data.
+ */
+
+#ifndef _EMAC_HW_H_
+#define _EMAC_HW_H_
+
+/* EMAC_CSR register offsets */
+#define EMAC_EMAC_WRAPPER_CSR1                                0x000000
+#define EMAC_EMAC_WRAPPER_CSR2                                0x000004
+#define EMAC_EMAC_WRAPPER_TX_TS_LO                            0x000104
+#define EMAC_EMAC_WRAPPER_TX_TS_HI                            0x000108
+#define EMAC_EMAC_WRAPPER_TX_TS_INX                           0x00010c
+
+/* DMA Order Settings */
+enum emac_dma_order {
+	emac_dma_ord_in = 1,
+	emac_dma_ord_enh = 2,
+	emac_dma_ord_out = 4
+};
+
+enum emac_dma_req_block {
+	emac_dma_req_128 = 0,
+	emac_dma_req_256 = 1,
+	emac_dma_req_512 = 2,
+	emac_dma_req_1024 = 3,
+	emac_dma_req_2048 = 4,
+	emac_dma_req_4096 = 5
+};
+
+/* Returns the value of bits idx...idx+n_bits */
+#define BITS_GET(val, lo, hi) ((le32_to_cpu(val) & GENMASK((hi), (lo))) >> lo)
+#define BITS_SET(val, lo, hi, new_val) \
+	val = cpu_to_le32((le32_to_cpu(val) & (~GENMASK((hi), (lo)))) |	\
+		(((new_val) << (lo)) & GENMASK((hi), (lo))))
+
+/* RRD (Receive Return Descriptor) */
+struct emac_rrd {
+	u32	word[6];
+
+/* number of RFD */
+#define RRD_NOR(rrd)			BITS_GET((rrd)->word[0], 16, 19)
+/* start consumer index of rfd-ring */
+#define RRD_SI(rrd)			BITS_GET((rrd)->word[0], 20, 31)
+/* vlan-tag (CVID, CFI and PRI) */
+#define RRD_CVALN_TAG(rrd)		BITS_GET((rrd)->word[2], 0, 15)
+/* length of the packet */
+#define RRD_PKT_SIZE(rrd)		BITS_GET((rrd)->word[3], 0, 13)
+/* L4(TCP/UDP) checksum failed */
+#define RRD_L4F(rrd)			BITS_GET((rrd)->word[3], 14, 14)
+/* vlan tagged */
+#define RRD_CVTAG(rrd)			BITS_GET((rrd)->word[3], 16, 16)
+/* When set, indicates that the descriptor is updated by the IP core.
+ * When cleared, indicates that the descriptor is invalid.
+ */
+#define RRD_UPDT(rrd)			BITS_GET((rrd)->word[3], 31, 31)
+#define RRD_UPDT_SET(rrd, val)		BITS_SET((rrd)->word[3], 31, 31, val)
+/* timestamp low */
+#define RRD_TS_LOW(rrd)			BITS_GET((rrd)->word[4], 0, 29)
+/* timestamp high */
+#define RRD_TS_HI(rrd)			le32_to_cpu((rrd)->word[5])
+};
+
+/* TPD (Transmit Packet Descriptor) */
+struct emac_tpd {
+	u32				word[4];
+
+/* Number of bytes of the transmit packet. (include 4-byte CRC) */
+#define TPD_BUF_LEN_SET(tpd, val)	BITS_SET((tpd)->word[0], 0, 15, val)
+/* Custom Checksum Offload: When set, ask IP core to offload custom checksum */
+#define TPD_CSX_SET(tpd, val)		BITS_SET((tpd)->word[1], 8, 8, val)
+/* TCP Large Send Offload: When set, ask IP core to do offload TCP Large Send */
+#define TPD_LSO(tpd)			BITS_GET((tpd)->word[1], 12, 12)
+#define TPD_LSO_SET(tpd, val)		BITS_SET((tpd)->word[1], 12, 12, val)
+/*  Large Send Offload Version: When set, indicates this is an LSOv2
+ * (for both IPv4 and IPv6). When cleared, indicates this is an LSOv1
+ * (only for IPv4).
+ */
+#define TPD_LSOV_SET(tpd, val)		BITS_SET((tpd)->word[1], 13, 13, val)
+/* IPv4 packet: When set, indicates this is an  IPv4 packet, this bit is only
+ * for LSOV2 format.
+ */
+#define TPD_IPV4_SET(tpd, val)		BITS_SET((tpd)->word[1], 16, 16, val)
+/* 0: Ethernet   frame (DA+SA+TYPE+DATA+CRC)
+ * 1: IEEE 802.3 frame (DA+SA+LEN+DSAP+SSAP+CTL+ORG+TYPE+DATA+CRC)
+ */
+#define TPD_TYP_SET(tpd, val)		BITS_SET((tpd)->word[1], 17, 17, val)
+/* Low-32bit Buffer Address */
+#define TPD_BUFFER_ADDR_L_SET(tpd, val)	((tpd)->word[2] = cpu_to_le32(val))
+/* CVLAN Tag to be inserted if INS_VLAN_TAG is set, CVLAN TPID based on global
+ * register configuration.
+ */
+#define TPD_CVLAN_TAG_SET(tpd, val)	BITS_SET((tpd)->word[3], 0, 15, val)
+/*  Insert CVlan Tag: When set, ask MAC to insert CVLAN TAG to outgoing packet
+ */
+#define TPD_INSTC_SET(tpd, val)		BITS_SET((tpd)->word[3], 17, 17, val)
+/* High-14bit Buffer Address, So, the 64b-bit address is
+ * {DESC_CTRL_11_TX_DATA_HIADDR[17:0],(register) BUFFER_ADDR_H, BUFFER_ADDR_L}
+ */
+#define TPD_BUFFER_ADDR_H_SET(tpd, val)	BITS_SET((tpd)->word[3], 18, 30, val)
+/* Format D. Word offset from the 1st byte of this packet to start to calculate
+ * the custom checksum.
+ */
+#define TPD_PAYLOAD_OFFSET_SET(tpd, val) BITS_SET((tpd)->word[1], 0, 7, val)
+/*  Format D. Word offset from the 1st byte of this packet to fill the custom
+ * checksum to
+ */
+#define TPD_CXSUM_OFFSET_SET(tpd, val)	BITS_SET((tpd)->word[1], 18, 25, val)
+
+/* Format C. TCP Header offset from the 1st byte of this packet. (byte unit) */
+#define TPD_TCPHDR_OFFSET_SET(tpd, val)	BITS_SET((tpd)->word[1], 0, 7, val)
+/* Format C. MSS (Maximum Segment Size) got from the protocol layer. (byte unit)
+ */
+#define TPD_MSS_SET(tpd, val)		BITS_SET((tpd)->word[1], 18, 30, val)
+/* packet length in ext tpd */
+#define TPD_PKT_LEN_SET(tpd, val)	((tpd)->word[2] = cpu_to_le32(val))
+};
+
+/* emac_ring_header represents a single, contiguous block of DMA space
+ * mapped for the three descriptor rings (tpd, rfd, rrd)
+ */
+struct emac_ring_header {
+	void			*v_addr;	/* virtual address */
+	dma_addr_t		dma_addr;	/* dma address */
+	size_t			size;		/* length in bytes */
+	size_t			used;
+};
+
+/* emac_buffer is wrapper around a pointer to a socket buffer
+ * so a DMA handle can be stored along with the skb
+ */
+struct emac_buffer {
+	struct sk_buff		*skb;		/* socket buffer */
+	u16			length;		/* rx buffer length */
+	dma_addr_t		dma_addr;	/* dma address */
+};
+
+/* receive free descriptor (rfd) ring */
+struct emac_rfd_ring {
+	struct emac_buffer	*rfbuff;
+	u32			*v_addr;	/* virtual address */
+	dma_addr_t		dma_addr;	/* dma address */
+	size_t			size;		/* length in bytes */
+	unsigned int		count;		/* number of desc in the ring */
+	unsigned int		produce_idx;
+	unsigned int		process_idx;
+	unsigned int		consume_idx;	/* unused */
+};
+
+/* Receive Return Desciptor (RRD) ring */
+struct emac_rrd_ring {
+	u32			*v_addr;	/* virtual address */
+	dma_addr_t		dma_addr;	/* physical address */
+	size_t			size;		/* length in bytes */
+	unsigned int		count;		/* number of desc in the ring */
+	unsigned int		produce_idx;	/* unused */
+	unsigned int		consume_idx;
+};
+
+/* Rx queue */
+struct emac_rx_queue {
+	struct net_device	*netdev;	/* netdev ring belongs to */
+	struct emac_rrd_ring	rrd;
+	struct emac_rfd_ring	rfd;
+	struct napi_struct	napi;
+	struct emac_irq		*irq;
+
+	u32			intr;
+	u32			produce_mask;
+	u32			process_mask;
+	u32			consume_mask;
+
+	u16			produce_reg;
+	u16			process_reg;
+	u16			consume_reg;
+
+	u8			produce_shift;
+	u8			process_shft;
+	u8			consume_shift;
+};
+
+/* Transimit Packet Descriptor (tpd) ring */
+struct emac_tpd_ring {
+	struct emac_buffer	*tpbuff;
+	u32			*v_addr;	/* virtual address */
+	dma_addr_t		dma_addr;	/* dma address */
+
+	size_t			size;		/* length in bytes */
+	unsigned int		count;		/* number of desc in the ring */
+	unsigned int		produce_idx;
+	unsigned int		consume_idx;
+	unsigned int		last_produce_idx;
+};
+
+/* Tx queue */
+struct emac_tx_queue {
+	struct emac_tpd_ring	tpd;
+
+	u32			produce_mask;
+	u32			consume_mask;
+
+	u16			max_packets;	/* max packets per interrupt */
+	u16			produce_reg;
+	u16			consume_reg;
+
+	u8			produce_shift;
+	u8			consume_shift;
+};
+
+struct emac_adapter;
+
+int  emac_mac_up(struct emac_adapter *adpt);
+void emac_mac_down(struct emac_adapter *adpt);
+void emac_mac_reset(struct emac_adapter *adpt);
+void emac_mac_start(struct emac_adapter *adpt);
+void emac_mac_stop(struct emac_adapter *adpt);
+void emac_mac_mode_config(struct emac_adapter *adpt);
+void emac_mac_rx_process(struct emac_adapter *adpt, struct emac_rx_queue *rx_q,
+			 int *num_pkts, int max_pkts);
+int emac_mac_tx_buf_send(struct emac_adapter *adpt, struct emac_tx_queue *tx_q,
+			 struct sk_buff *skb);
+void emac_mac_tx_process(struct emac_adapter *adpt, struct emac_tx_queue *tx_q);
+void emac_mac_rx_tx_ring_init_all(struct platform_device *pdev,
+				  struct emac_adapter *adpt);
+int  emac_mac_rx_tx_rings_alloc_all(struct emac_adapter *adpt);
+void emac_mac_rx_tx_rings_free_all(struct emac_adapter *adpt);
+void emac_mac_multicast_addr_clear(struct emac_adapter *adpt);
+void emac_mac_multicast_addr_set(struct emac_adapter *adpt, u8 *addr);
+
+#endif /*_EMAC_HW_H_*/
diff --git a/drivers/net/ethernet/qualcomm/emac/emac-phy.c b/drivers/net/ethernet/qualcomm/emac/emac-phy.c
new file mode 100644
index 000000000000..da4e90db4d98
--- /dev/null
+++ b/drivers/net/ethernet/qualcomm/emac/emac-phy.c
@@ -0,0 +1,227 @@
+/* Copyright (c) 2013-2016, 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.
+ */
+
+/* Qualcomm Technologies, Inc. EMAC PHY Controller driver.
+ */
+
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_net.h>
+#include <linux/of_mdio.h>
+#include <linux/phy.h>
+#include <linux/iopoll.h>
+#include <linux/acpi.h>
+#include "emac.h"
+#include "emac-mac.h"
+#include "emac-phy.h"
+#include "emac-sgmii.h"
+
+/* EMAC base register offsets */
+#define EMAC_MDIO_CTRL                                        0x001414
+#define EMAC_PHY_STS                                          0x001418
+#define EMAC_MDIO_EX_CTRL                                     0x001440
+
+/* EMAC_MDIO_CTRL */
+#define MDIO_MODE                                              BIT(30)
+#define MDIO_PR                                                BIT(29)
+#define MDIO_AP_EN                                             BIT(28)
+#define MDIO_BUSY                                              BIT(27)
+#define MDIO_CLK_SEL_BMSK                                    0x7000000
+#define MDIO_CLK_SEL_SHFT                                           24
+#define MDIO_START                                             BIT(23)
+#define SUP_PREAMBLE                                           BIT(22)
+#define MDIO_RD_NWR                                            BIT(21)
+#define MDIO_REG_ADDR_BMSK                                    0x1f0000
+#define MDIO_REG_ADDR_SHFT                                          16
+#define MDIO_DATA_BMSK                                          0xffff
+#define MDIO_DATA_SHFT                                               0
+
+/* EMAC_PHY_STS */
+#define PHY_ADDR_BMSK                                         0x1f0000
+#define PHY_ADDR_SHFT                                               16
+
+#define MDIO_CLK_25_4                                                0
+#define MDIO_CLK_25_28                                               7
+
+#define MDIO_WAIT_TIMES                                           1000
+
+#define EMAC_LINK_SPEED_DEFAULT (\
+		EMAC_LINK_SPEED_10_HALF  |\
+		EMAC_LINK_SPEED_10_FULL  |\
+		EMAC_LINK_SPEED_100_HALF |\
+		EMAC_LINK_SPEED_100_FULL |\
+		EMAC_LINK_SPEED_1GB_FULL)
+
+/**
+ * emac_phy_mdio_autopoll_disable() - disable mdio autopoll
+ * @adpt: the emac adapter
+ *
+ * The autopoll feature takes over the MDIO bus.  In order for
+ * the PHY driver to be able to talk to the PHY over the MDIO
+ * bus, we need to temporarily disable the autopoll feature.
+ */
+static int emac_phy_mdio_autopoll_disable(struct emac_adapter *adpt)
+{
+	u32 val;
+
+	/* disable autopoll */
+	emac_reg_update32(adpt->base + EMAC_MDIO_CTRL, MDIO_AP_EN, 0);
+
+	/* wait for any mdio polling to complete */
+	if (!readl_poll_timeout(adpt->base + EMAC_MDIO_CTRL, val,
+				!(val & MDIO_BUSY), 100, MDIO_WAIT_TIMES * 100))
+		return 0;
+
+	/* failed to disable; ensure it is enabled before returning */
+	emac_reg_update32(adpt->base + EMAC_MDIO_CTRL, 0, MDIO_AP_EN);
+
+	return -EBUSY;
+}
+
+/**
+ * emac_phy_mdio_autopoll_disable() - disable mdio autopoll
+ * @adpt: the emac adapter
+ *
+ * The EMAC has the ability to poll the external PHY on the MDIO
+ * bus for link state changes.  This eliminates the need for the
+ * driver to poll the phy.  If if the link state does change,
+ * the EMAC issues an interrupt on behalf of the PHY.
+ */
+static void emac_phy_mdio_autopoll_enable(struct emac_adapter *adpt)
+{
+	emac_reg_update32(adpt->base + EMAC_MDIO_CTRL, 0, MDIO_AP_EN);
+}
+
+static int emac_mdio_read(struct mii_bus *bus, int addr, int regnum)
+{
+	struct emac_adapter *adpt = bus->priv;
+	u32 reg;
+	int ret;
+
+	ret = emac_phy_mdio_autopoll_disable(adpt);
+	if (ret)
+		return ret;
+
+	emac_reg_update32(adpt->base + EMAC_PHY_STS, PHY_ADDR_BMSK,
+			  (addr << PHY_ADDR_SHFT));
+
+	reg = SUP_PREAMBLE |
+	      ((MDIO_CLK_25_4 << MDIO_CLK_SEL_SHFT) & MDIO_CLK_SEL_BMSK) |
+	      ((regnum << MDIO_REG_ADDR_SHFT) & MDIO_REG_ADDR_BMSK) |
+	      MDIO_START | MDIO_RD_NWR;
+
+	writel(reg, adpt->base + EMAC_MDIO_CTRL);
+
+	if (readl_poll_timeout(adpt->base + EMAC_MDIO_CTRL, reg,
+			       !(reg & (MDIO_START | MDIO_BUSY)),
+			       100, MDIO_WAIT_TIMES * 100))
+		ret = -EIO;
+	else
+		ret = (reg >> MDIO_DATA_SHFT) & MDIO_DATA_BMSK;
+
+	emac_phy_mdio_autopoll_enable(adpt);
+
+	return ret;
+}
+
+static int emac_mdio_write(struct mii_bus *bus, int addr, int regnum, u16 val)
+{
+	struct emac_adapter *adpt = bus->priv;
+	u32 reg;
+	int ret;
+
+	ret = emac_phy_mdio_autopoll_disable(adpt);
+	if (ret)
+		return ret;
+
+	emac_reg_update32(adpt->base + EMAC_PHY_STS, PHY_ADDR_BMSK,
+			  (addr << PHY_ADDR_SHFT));
+
+	reg = SUP_PREAMBLE |
+		((MDIO_CLK_25_4 << MDIO_CLK_SEL_SHFT) & MDIO_CLK_SEL_BMSK) |
+		((regnum << MDIO_REG_ADDR_SHFT) & MDIO_REG_ADDR_BMSK) |
+		((val << MDIO_DATA_SHFT) & MDIO_DATA_BMSK) |
+		MDIO_START;
+
+	writel(reg, adpt->base + EMAC_MDIO_CTRL);
+
+	if (readl_poll_timeout(adpt->base + EMAC_MDIO_CTRL, reg,
+			       !(reg & (MDIO_START | MDIO_BUSY)), 100,
+			       MDIO_WAIT_TIMES * 100))
+		ret = -EIO;
+
+	emac_phy_mdio_autopoll_enable(adpt);
+
+	return ret;
+}
+
+/* Configure the MDIO bus and connect the external PHY */
+int emac_phy_config(struct platform_device *pdev, struct emac_adapter *adpt)
+{
+	struct device_node *np = pdev->dev.of_node;
+	struct mii_bus *mii_bus;
+	int ret;
+
+	/* Create the mii_bus object for talking to the MDIO bus */
+	adpt->mii_bus = mii_bus = devm_mdiobus_alloc(&pdev->dev);
+	if (!mii_bus)
+		return -ENOMEM;
+
+	mii_bus->name = "emac-mdio";
+	snprintf(mii_bus->id, MII_BUS_ID_SIZE, "%s", pdev->name);
+	mii_bus->read = emac_mdio_read;
+	mii_bus->write = emac_mdio_write;
+	mii_bus->parent = &pdev->dev;
+	mii_bus->priv = adpt;
+
+	if (has_acpi_companion(&pdev->dev)) {
+		u32 phy_addr;
+
+		ret = mdiobus_register(mii_bus);
+		if (ret) {
+			dev_err(&pdev->dev, "could not register mdio bus\n");
+			return ret;
+		}
+		ret = device_property_read_u32(&pdev->dev, "phy-channel",
+					       &phy_addr);
+		if (ret)
+			/* If we can't read a valid phy address, then assume
+			 * that there is only one phy on this mdio bus.
+			 */
+			adpt->phydev = phy_find_first(mii_bus);
+		else
+			adpt->phydev = mdiobus_get_phy(mii_bus, phy_addr);
+
+	} else {
+		struct device_node *phy_np;
+
+		ret = of_mdiobus_register(mii_bus, np);
+		if (ret) {
+			dev_err(&pdev->dev, "could not register mdio bus\n");
+			return ret;
+		}
+
+		phy_np = of_parse_phandle(np, "phy-handle", 0);
+		adpt->phydev = of_phy_find_device(phy_np);
+	}
+
+	if (!adpt->phydev) {
+		dev_err(&pdev->dev, "could not find external phy\n");
+		mdiobus_unregister(mii_bus);
+		return -ENODEV;
+	}
+
+	if (adpt->phydev->drv)
+		phy_attached_print(adpt->phydev, NULL);
+
+	return 0;
+}
diff --git a/drivers/net/ethernet/qualcomm/emac/emac-phy.h b/drivers/net/ethernet/qualcomm/emac/emac-phy.h
new file mode 100644
index 000000000000..49f3701a6dd7
--- /dev/null
+++ b/drivers/net/ethernet/qualcomm/emac/emac-phy.h
@@ -0,0 +1,33 @@
+/* Copyright (c) 2015-2016, 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 _EMAC_PHY_H_
+#define _EMAC_PHY_H_
+
+typedef int (*emac_sgmii_initialize)(struct emac_adapter *adpt);
+
+/** emac_phy - internal emac phy
+ * @base base address
+ * @digital per-lane digital block
+ * @initialize initialization function
+ */
+struct emac_phy {
+	void __iomem		*base;
+	void __iomem		*digital;
+	emac_sgmii_initialize	initialize;
+};
+
+struct emac_adapter;
+
+int emac_phy_config(struct platform_device *pdev, struct emac_adapter *adpt);
+
+#endif /* _EMAC_PHY_H_ */
diff --git a/drivers/net/ethernet/qualcomm/emac/emac-sgmii.c b/drivers/net/ethernet/qualcomm/emac/emac-sgmii.c
new file mode 100644
index 000000000000..75c1b530e39e
--- /dev/null
+++ b/drivers/net/ethernet/qualcomm/emac/emac-sgmii.c
@@ -0,0 +1,784 @@
+/* Copyright (c) 2015-2016, 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.
+ */
+
+/* Qualcomm Technologies, Inc. EMAC SGMII Controller driver.
+ */
+
+#include <linux/iopoll.h>
+#include <linux/acpi.h>
+#include <linux/of_device.h>
+#include "emac.h"
+#include "emac-mac.h"
+#include "emac-sgmii.h"
+
+/* EMAC_QSERDES register offsets */
+#define EMAC_QSERDES_COM_SYS_CLK_CTRL		0x000000
+#define EMAC_QSERDES_COM_PLL_CNTRL		0x000014
+#define EMAC_QSERDES_COM_PLL_IP_SETI		0x000018
+#define EMAC_QSERDES_COM_PLL_CP_SETI		0x000024
+#define EMAC_QSERDES_COM_PLL_IP_SETP		0x000028
+#define EMAC_QSERDES_COM_PLL_CP_SETP		0x00002c
+#define EMAC_QSERDES_COM_SYSCLK_EN_SEL		0x000038
+#define EMAC_QSERDES_COM_RESETSM_CNTRL		0x000040
+#define EMAC_QSERDES_COM_PLLLOCK_CMP1		0x000044
+#define EMAC_QSERDES_COM_PLLLOCK_CMP2		0x000048
+#define EMAC_QSERDES_COM_PLLLOCK_CMP3		0x00004c
+#define EMAC_QSERDES_COM_PLLLOCK_CMP_EN		0x000050
+#define EMAC_QSERDES_COM_DEC_START1		0x000064
+#define EMAC_QSERDES_COM_DIV_FRAC_START1	0x000098
+#define EMAC_QSERDES_COM_DIV_FRAC_START2	0x00009c
+#define EMAC_QSERDES_COM_DIV_FRAC_START3	0x0000a0
+#define EMAC_QSERDES_COM_DEC_START2		0x0000a4
+#define EMAC_QSERDES_COM_PLL_CRCTRL		0x0000ac
+#define EMAC_QSERDES_COM_RESET_SM		0x0000bc
+#define EMAC_QSERDES_TX_BIST_MODE_LANENO	0x000100
+#define EMAC_QSERDES_TX_TX_EMP_POST1_LVL	0x000108
+#define EMAC_QSERDES_TX_TX_DRV_LVL		0x00010c
+#define EMAC_QSERDES_TX_LANE_MODE		0x000150
+#define EMAC_QSERDES_TX_TRAN_DRVR_EMP_EN	0x000170
+#define EMAC_QSERDES_RX_CDR_CONTROL		0x000200
+#define EMAC_QSERDES_RX_CDR_CONTROL2		0x000210
+#define EMAC_QSERDES_RX_RX_EQ_GAIN12		0x000230
+
+/* EMAC_SGMII register offsets */
+#define EMAC_SGMII_PHY_SERDES_START		0x000000
+#define EMAC_SGMII_PHY_CMN_PWR_CTRL		0x000004
+#define EMAC_SGMII_PHY_RX_PWR_CTRL		0x000008
+#define EMAC_SGMII_PHY_TX_PWR_CTRL		0x00000C
+#define EMAC_SGMII_PHY_LANE_CTRL1		0x000018
+#define EMAC_SGMII_PHY_AUTONEG_CFG2		0x000048
+#define EMAC_SGMII_PHY_CDR_CTRL0		0x000058
+#define EMAC_SGMII_PHY_SPEED_CFG1		0x000074
+#define EMAC_SGMII_PHY_POW_DWN_CTRL0		0x000080
+#define EMAC_SGMII_PHY_RESET_CTRL		0x0000a8
+#define EMAC_SGMII_PHY_IRQ_CMD			0x0000ac
+#define EMAC_SGMII_PHY_INTERRUPT_CLEAR		0x0000b0
+#define EMAC_SGMII_PHY_INTERRUPT_MASK		0x0000b4
+#define EMAC_SGMII_PHY_INTERRUPT_STATUS		0x0000b8
+#define EMAC_SGMII_PHY_RX_CHK_STATUS		0x0000d4
+#define EMAC_SGMII_PHY_AUTONEG0_STATUS		0x0000e0
+#define EMAC_SGMII_PHY_AUTONEG1_STATUS		0x0000e4
+
+/* EMAC_QSERDES_COM_PLL_IP_SETI */
+#define PLL_IPSETI(x)				((x) & 0x3f)
+
+/* EMAC_QSERDES_COM_PLL_CP_SETI */
+#define PLL_CPSETI(x)				((x) & 0xff)
+
+/* EMAC_QSERDES_COM_PLL_IP_SETP */
+#define PLL_IPSETP(x)				((x) & 0x3f)
+
+/* EMAC_QSERDES_COM_PLL_CP_SETP */
+#define PLL_CPSETP(x)				((x) & 0x1f)
+
+/* EMAC_QSERDES_COM_PLL_CRCTRL */
+#define PLL_RCTRL(x)				(((x) & 0xf) << 4)
+#define PLL_CCTRL(x)				((x) & 0xf)
+
+/* SGMII v2 PHY registers per lane */
+#define EMAC_SGMII_PHY_LN_OFFSET		0x0400
+
+/* SGMII v2 digital lane registers */
+#define EMAC_SGMII_LN_DRVR_CTRL0		0x00C
+#define EMAC_SGMII_LN_DRVR_TAP_EN		0x018
+#define EMAC_SGMII_LN_TX_MARGINING		0x01C
+#define EMAC_SGMII_LN_TX_PRE			0x020
+#define EMAC_SGMII_LN_TX_POST			0x024
+#define EMAC_SGMII_LN_TX_BAND_MODE		0x060
+#define EMAC_SGMII_LN_LANE_MODE			0x064
+#define EMAC_SGMII_LN_PARALLEL_RATE		0x078
+#define EMAC_SGMII_LN_CML_CTRL_MODE0		0x0B8
+#define EMAC_SGMII_LN_MIXER_CTRL_MODE0		0x0D0
+#define EMAC_SGMII_LN_VGA_INITVAL		0x134
+#define EMAC_SGMII_LN_UCDR_FO_GAIN_MODE0	0x17C
+#define EMAC_SGMII_LN_UCDR_SO_GAIN_MODE0	0x188
+#define EMAC_SGMII_LN_UCDR_SO_CONFIG		0x194
+#define EMAC_SGMII_LN_RX_BAND			0x19C
+#define EMAC_SGMII_LN_RX_RCVR_PATH1_MODE0	0x1B8
+#define EMAC_SGMII_LN_RSM_CONFIG		0x1F0
+#define EMAC_SGMII_LN_SIGDET_ENABLES		0x224
+#define EMAC_SGMII_LN_SIGDET_CNTRL		0x228
+#define EMAC_SGMII_LN_SIGDET_DEGLITCH_CNTRL	0x22C
+#define EMAC_SGMII_LN_RX_EN_SIGNAL		0x2A0
+#define EMAC_SGMII_LN_RX_MISC_CNTRL0		0x2AC
+#define EMAC_SGMII_LN_DRVR_LOGIC_CLKDIV		0x2BC
+
+/* SGMII v2 digital lane register values */
+#define UCDR_STEP_BY_TWO_MODE0			BIT(7)
+#define UCDR_xO_GAIN_MODE(x)			((x) & 0x7f)
+#define UCDR_ENABLE				BIT(6)
+#define UCDR_SO_SATURATION(x)			((x) & 0x3f)
+#define SIGDET_LP_BYP_PS4			BIT(7)
+#define SIGDET_EN_PS0_TO_PS2			BIT(6)
+#define EN_ACCOUPLEVCM_SW_MUX			BIT(5)
+#define EN_ACCOUPLEVCM_SW			BIT(4)
+#define RX_SYNC_EN				BIT(3)
+#define RXTERM_HIGHZ_PS5			BIT(2)
+#define SIGDET_EN_PS3				BIT(1)
+#define EN_ACCOUPLE_VCM_PS3			BIT(0)
+#define UFS_MODE				BIT(5)
+#define TXVAL_VALID_INIT			BIT(4)
+#define TXVAL_VALID_MUX				BIT(3)
+#define TXVAL_VALID				BIT(2)
+#define USB3P1_MODE				BIT(1)
+#define KR_PCIGEN3_MODE				BIT(0)
+#define PRE_EN					BIT(3)
+#define POST_EN					BIT(2)
+#define MAIN_EN_MUX				BIT(1)
+#define MAIN_EN					BIT(0)
+#define TX_MARGINING_MUX			BIT(6)
+#define TX_MARGINING(x)				((x) & 0x3f)
+#define TX_PRE_MUX				BIT(6)
+#define TX_PRE(x)				((x) & 0x3f)
+#define TX_POST_MUX				BIT(6)
+#define TX_POST(x)				((x) & 0x3f)
+#define CML_GEAR_MODE(x)			(((x) & 7) << 3)
+#define CML2CMOS_IBOOST_MODE(x)			((x) & 7)
+#define MIXER_LOADB_MODE(x)			(((x) & 0xf) << 2)
+#define MIXER_DATARATE_MODE(x)			((x) & 3)
+#define VGA_THRESH_DFE(x)			((x) & 0x3f)
+#define SIGDET_LP_BYP_PS0_TO_PS2		BIT(5)
+#define SIGDET_LP_BYP_MUX			BIT(4)
+#define SIGDET_LP_BYP				BIT(3)
+#define SIGDET_EN_MUX				BIT(2)
+#define SIGDET_EN				BIT(1)
+#define SIGDET_FLT_BYP				BIT(0)
+#define SIGDET_LVL(x)				(((x) & 0xf) << 4)
+#define SIGDET_BW_CTRL(x)			((x) & 0xf)
+#define SIGDET_DEGLITCH_CTRL(x)			(((x) & 0xf) << 1)
+#define SIGDET_DEGLITCH_BYP			BIT(0)
+#define INVERT_PCS_RX_CLK			BIT(7)
+#define PWM_EN					BIT(6)
+#define RXBIAS_SEL(x)				(((x) & 0x3) << 4)
+#define EBDAC_SIGN				BIT(3)
+#define EDAC_SIGN				BIT(2)
+#define EN_AUXTAP1SIGN_INVERT			BIT(1)
+#define EN_DAC_CHOPPING				BIT(0)
+#define DRVR_LOGIC_CLK_EN			BIT(4)
+#define DRVR_LOGIC_CLK_DIV(x)			((x) & 0xf)
+#define PARALLEL_RATE_MODE2(x)			(((x) & 0x3) << 4)
+#define PARALLEL_RATE_MODE1(x)			(((x) & 0x3) << 2)
+#define PARALLEL_RATE_MODE0(x)			((x) & 0x3)
+#define BAND_MODE2(x)				(((x) & 0x3) << 4)
+#define BAND_MODE1(x)				(((x) & 0x3) << 2)
+#define BAND_MODE0(x)				((x) & 0x3)
+#define LANE_SYNC_MODE				BIT(5)
+#define LANE_MODE(x)				((x) & 0x1f)
+#define CDR_PD_SEL_MODE0(x)			(((x) & 0x3) << 5)
+#define EN_DLL_MODE0				BIT(4)
+#define EN_IQ_DCC_MODE0				BIT(3)
+#define EN_IQCAL_MODE0				BIT(2)
+#define EN_QPATH_MODE0				BIT(1)
+#define EN_EPATH_MODE0				BIT(0)
+#define FORCE_TSYNC_ACK				BIT(7)
+#define FORCE_CMN_ACK				BIT(6)
+#define FORCE_CMN_READY				BIT(5)
+#define EN_RCLK_DEGLITCH			BIT(4)
+#define BYPASS_RSM_CDR_RESET			BIT(3)
+#define BYPASS_RSM_TSYNC			BIT(2)
+#define BYPASS_RSM_SAMP_CAL			BIT(1)
+#define BYPASS_RSM_DLL_CAL			BIT(0)
+
+/* EMAC_QSERDES_COM_SYS_CLK_CTRL */
+#define SYSCLK_CM				BIT(4)
+#define SYSCLK_AC_COUPLE			BIT(3)
+
+/* EMAC_QSERDES_COM_PLL_CNTRL */
+#define OCP_EN					BIT(5)
+#define PLL_DIV_FFEN				BIT(2)
+#define PLL_DIV_ORD				BIT(1)
+
+/* EMAC_QSERDES_COM_SYSCLK_EN_SEL */
+#define SYSCLK_SEL_CMOS				BIT(3)
+
+/* EMAC_QSERDES_COM_RESETSM_CNTRL */
+#define FRQ_TUNE_MODE				BIT(4)
+
+/* EMAC_QSERDES_COM_PLLLOCK_CMP_EN */
+#define PLLLOCK_CMP_EN				BIT(0)
+
+/* EMAC_QSERDES_COM_DEC_START1 */
+#define DEC_START1_MUX				BIT(7)
+#define DEC_START1(x)				((x) & 0x7f)
+
+/* EMAC_QSERDES_COM_DIV_FRAC_START1 * EMAC_QSERDES_COM_DIV_FRAC_START2 */
+#define DIV_FRAC_START_MUX			BIT(7)
+#define DIV_FRAC_START(x)			((x) & 0x7f)
+
+/* EMAC_QSERDES_COM_DIV_FRAC_START3 */
+#define DIV_FRAC_START3_MUX			BIT(4)
+#define DIV_FRAC_START3(x)			((x) & 0xf)
+
+/* EMAC_QSERDES_COM_DEC_START2 */
+#define DEC_START2_MUX				BIT(1)
+#define DEC_START2				BIT(0)
+
+/* EMAC_QSERDES_COM_RESET_SM */
+#define READY					BIT(5)
+
+/* EMAC_QSERDES_TX_TX_EMP_POST1_LVL */
+#define TX_EMP_POST1_LVL_MUX			BIT(5)
+#define TX_EMP_POST1_LVL(x)			((x) & 0x1f)
+#define TX_EMP_POST1_LVL_BMSK			0x1f
+#define TX_EMP_POST1_LVL_SHFT			0
+
+/* EMAC_QSERDES_TX_TX_DRV_LVL */
+#define TX_DRV_LVL_MUX				BIT(4)
+#define TX_DRV_LVL(x)				((x) & 0xf)
+
+/* EMAC_QSERDES_TX_TRAN_DRVR_EMP_EN */
+#define EMP_EN_MUX				BIT(1)
+#define EMP_EN					BIT(0)
+
+/* EMAC_QSERDES_RX_CDR_CONTROL & EMAC_QSERDES_RX_CDR_CONTROL2 */
+#define HBW_PD_EN				BIT(7)
+#define SECONDORDERENABLE			BIT(6)
+#define FIRSTORDER_THRESH(x)			(((x) & 0x7) << 3)
+#define SECONDORDERGAIN(x)			((x) & 0x7)
+
+/* EMAC_QSERDES_RX_RX_EQ_GAIN12 */
+#define RX_EQ_GAIN2(x)				(((x) & 0xf) << 4)
+#define RX_EQ_GAIN1(x)				((x) & 0xf)
+
+/* EMAC_SGMII_PHY_SERDES_START */
+#define SERDES_START				BIT(0)
+
+/* EMAC_SGMII_PHY_CMN_PWR_CTRL */
+#define BIAS_EN					BIT(6)
+#define PLL_EN					BIT(5)
+#define SYSCLK_EN				BIT(4)
+#define CLKBUF_L_EN				BIT(3)
+#define PLL_TXCLK_EN				BIT(1)
+#define PLL_RXCLK_EN				BIT(0)
+
+/* EMAC_SGMII_PHY_RX_PWR_CTRL */
+#define L0_RX_SIGDET_EN				BIT(7)
+#define L0_RX_TERM_MODE(x)			(((x) & 3) << 4)
+#define L0_RX_I_EN				BIT(1)
+
+/* EMAC_SGMII_PHY_TX_PWR_CTRL */
+#define L0_TX_EN				BIT(5)
+#define L0_CLKBUF_EN				BIT(4)
+#define L0_TRAN_BIAS_EN				BIT(1)
+
+/* EMAC_SGMII_PHY_LANE_CTRL1 */
+#define L0_RX_EQUALIZE_ENABLE			BIT(6)
+#define L0_RESET_TSYNC_EN			BIT(4)
+#define L0_DRV_LVL(x)				((x) & 0xf)
+
+/* EMAC_SGMII_PHY_AUTONEG_CFG2 */
+#define FORCE_AN_TX_CFG				BIT(5)
+#define FORCE_AN_RX_CFG				BIT(4)
+#define AN_ENABLE				BIT(0)
+
+/* EMAC_SGMII_PHY_SPEED_CFG1 */
+#define DUPLEX_MODE				BIT(4)
+#define SPDMODE_1000				BIT(1)
+#define SPDMODE_100				BIT(0)
+#define SPDMODE_10				0
+#define SPDMODE_BMSK				3
+#define SPDMODE_SHFT				0
+
+/* EMAC_SGMII_PHY_POW_DWN_CTRL0 */
+#define PWRDN_B					BIT(0)
+#define CDR_MAX_CNT(x)				((x) & 0xff)
+
+/* EMAC_QSERDES_TX_BIST_MODE_LANENO */
+#define BIST_LANE_NUMBER(x)			(((x) & 3) << 5)
+#define BISTMODE(x)				((x) & 0x1f)
+
+/* EMAC_QSERDES_COM_PLLLOCK_CMPx */
+#define PLLLOCK_CMP(x)				((x) & 0xff)
+
+/* EMAC_SGMII_PHY_RESET_CTRL */
+#define PHY_SW_RESET				BIT(0)
+
+/* EMAC_SGMII_PHY_IRQ_CMD */
+#define IRQ_GLOBAL_CLEAR			BIT(0)
+
+/* EMAC_SGMII_PHY_INTERRUPT_MASK */
+#define DECODE_CODE_ERR				BIT(7)
+#define DECODE_DISP_ERR				BIT(6)
+#define PLL_UNLOCK				BIT(5)
+#define AN_ILLEGAL_TERM				BIT(4)
+#define SYNC_FAIL				BIT(3)
+#define AN_START				BIT(2)
+#define AN_END					BIT(1)
+#define AN_REQUEST				BIT(0)
+
+#define SGMII_PHY_IRQ_CLR_WAIT_TIME		10
+
+#define SGMII_PHY_INTERRUPT_ERR (\
+	DECODE_CODE_ERR         |\
+	DECODE_DISP_ERR)
+
+#define SGMII_ISR_AN_MASK       (\
+	AN_REQUEST              |\
+	AN_START                |\
+	AN_END                  |\
+	AN_ILLEGAL_TERM         |\
+	PLL_UNLOCK              |\
+	SYNC_FAIL)
+
+#define SGMII_ISR_MASK          (\
+	SGMII_PHY_INTERRUPT_ERR |\
+	SGMII_ISR_AN_MASK)
+
+/* SGMII TX_CONFIG */
+#define TXCFG_LINK				0x8000
+#define TXCFG_MODE_BMSK				0x1c00
+#define TXCFG_1000_FULL				0x1800
+#define TXCFG_100_FULL				0x1400
+#define TXCFG_100_HALF				0x0400
+#define TXCFG_10_FULL				0x1000
+#define TXCFG_10_HALF				0x0000
+
+#define SERDES_START_WAIT_TIMES			100
+
+struct emac_reg_write {
+	unsigned int offset;
+	u32 val;
+};
+
+static void emac_reg_write_all(void __iomem *base,
+			       const struct emac_reg_write *itr, size_t size)
+{
+	size_t i;
+
+	for (i = 0; i < size; ++itr, ++i)
+		writel(itr->val, base + itr->offset);
+}
+
+static const struct emac_reg_write physical_coding_sublayer_programming_v1[] = {
+	{EMAC_SGMII_PHY_CDR_CTRL0, CDR_MAX_CNT(15)},
+	{EMAC_SGMII_PHY_POW_DWN_CTRL0, PWRDN_B},
+	{EMAC_SGMII_PHY_CMN_PWR_CTRL,
+		BIAS_EN | SYSCLK_EN | CLKBUF_L_EN | PLL_TXCLK_EN | PLL_RXCLK_EN},
+	{EMAC_SGMII_PHY_TX_PWR_CTRL, L0_TX_EN | L0_CLKBUF_EN | L0_TRAN_BIAS_EN},
+	{EMAC_SGMII_PHY_RX_PWR_CTRL,
+		L0_RX_SIGDET_EN | L0_RX_TERM_MODE(1) | L0_RX_I_EN},
+	{EMAC_SGMII_PHY_CMN_PWR_CTRL,
+		BIAS_EN | PLL_EN | SYSCLK_EN | CLKBUF_L_EN | PLL_TXCLK_EN |
+		PLL_RXCLK_EN},
+	{EMAC_SGMII_PHY_LANE_CTRL1,
+		L0_RX_EQUALIZE_ENABLE | L0_RESET_TSYNC_EN | L0_DRV_LVL(15)},
+};
+
+static const struct emac_reg_write sysclk_refclk_setting[] = {
+	{EMAC_QSERDES_COM_SYSCLK_EN_SEL, SYSCLK_SEL_CMOS},
+	{EMAC_QSERDES_COM_SYS_CLK_CTRL,	SYSCLK_CM | SYSCLK_AC_COUPLE},
+};
+
+static const struct emac_reg_write pll_setting[] = {
+	{EMAC_QSERDES_COM_PLL_IP_SETI, PLL_IPSETI(1)},
+	{EMAC_QSERDES_COM_PLL_CP_SETI, PLL_CPSETI(59)},
+	{EMAC_QSERDES_COM_PLL_IP_SETP, PLL_IPSETP(10)},
+	{EMAC_QSERDES_COM_PLL_CP_SETP, PLL_CPSETP(9)},
+	{EMAC_QSERDES_COM_PLL_CRCTRL, PLL_RCTRL(15) | PLL_CCTRL(11)},
+	{EMAC_QSERDES_COM_PLL_CNTRL, OCP_EN | PLL_DIV_FFEN | PLL_DIV_ORD},
+	{EMAC_QSERDES_COM_DEC_START1, DEC_START1_MUX | DEC_START1(2)},
+	{EMAC_QSERDES_COM_DEC_START2, DEC_START2_MUX | DEC_START2},
+	{EMAC_QSERDES_COM_DIV_FRAC_START1,
+		DIV_FRAC_START_MUX | DIV_FRAC_START(85)},
+	{EMAC_QSERDES_COM_DIV_FRAC_START2,
+		DIV_FRAC_START_MUX | DIV_FRAC_START(42)},
+	{EMAC_QSERDES_COM_DIV_FRAC_START3,
+		DIV_FRAC_START3_MUX | DIV_FRAC_START3(3)},
+	{EMAC_QSERDES_COM_PLLLOCK_CMP1, PLLLOCK_CMP(43)},
+	{EMAC_QSERDES_COM_PLLLOCK_CMP2, PLLLOCK_CMP(104)},
+	{EMAC_QSERDES_COM_PLLLOCK_CMP3, PLLLOCK_CMP(0)},
+	{EMAC_QSERDES_COM_PLLLOCK_CMP_EN, PLLLOCK_CMP_EN},
+	{EMAC_QSERDES_COM_RESETSM_CNTRL, FRQ_TUNE_MODE},
+};
+
+static const struct emac_reg_write cdr_setting[] = {
+	{EMAC_QSERDES_RX_CDR_CONTROL,
+		SECONDORDERENABLE | FIRSTORDER_THRESH(3) | SECONDORDERGAIN(2)},
+	{EMAC_QSERDES_RX_CDR_CONTROL2,
+		SECONDORDERENABLE | FIRSTORDER_THRESH(3) | SECONDORDERGAIN(4)},
+};
+
+static const struct emac_reg_write tx_rx_setting[] = {
+	{EMAC_QSERDES_TX_BIST_MODE_LANENO, 0},
+	{EMAC_QSERDES_TX_TX_DRV_LVL, TX_DRV_LVL_MUX | TX_DRV_LVL(15)},
+	{EMAC_QSERDES_TX_TRAN_DRVR_EMP_EN, EMP_EN_MUX | EMP_EN},
+	{EMAC_QSERDES_TX_TX_EMP_POST1_LVL,
+		TX_EMP_POST1_LVL_MUX | TX_EMP_POST1_LVL(1)},
+	{EMAC_QSERDES_RX_RX_EQ_GAIN12, RX_EQ_GAIN2(15) | RX_EQ_GAIN1(15)},
+	{EMAC_QSERDES_TX_LANE_MODE, LANE_MODE(8)},
+};
+
+static const struct emac_reg_write sgmii_v2_laned[] = {
+	/* CDR Settings */
+	{EMAC_SGMII_LN_UCDR_FO_GAIN_MODE0,
+		UCDR_STEP_BY_TWO_MODE0 | UCDR_xO_GAIN_MODE(10)},
+	{EMAC_SGMII_LN_UCDR_SO_GAIN_MODE0, UCDR_xO_GAIN_MODE(6)},
+	{EMAC_SGMII_LN_UCDR_SO_CONFIG, UCDR_ENABLE | UCDR_SO_SATURATION(12)},
+
+	/* TX/RX Settings */
+	{EMAC_SGMII_LN_RX_EN_SIGNAL, SIGDET_LP_BYP_PS4 | SIGDET_EN_PS0_TO_PS2},
+
+	{EMAC_SGMII_LN_DRVR_CTRL0, TXVAL_VALID_INIT | KR_PCIGEN3_MODE},
+	{EMAC_SGMII_LN_DRVR_TAP_EN, MAIN_EN},
+	{EMAC_SGMII_LN_TX_MARGINING, TX_MARGINING_MUX | TX_MARGINING(25)},
+	{EMAC_SGMII_LN_TX_PRE, TX_PRE_MUX},
+	{EMAC_SGMII_LN_TX_POST, TX_POST_MUX},
+
+	{EMAC_SGMII_LN_CML_CTRL_MODE0,
+		CML_GEAR_MODE(1) | CML2CMOS_IBOOST_MODE(1)},
+	{EMAC_SGMII_LN_MIXER_CTRL_MODE0,
+		MIXER_LOADB_MODE(12) | MIXER_DATARATE_MODE(1)},
+	{EMAC_SGMII_LN_VGA_INITVAL, VGA_THRESH_DFE(31)},
+	{EMAC_SGMII_LN_SIGDET_ENABLES,
+		SIGDET_LP_BYP_PS0_TO_PS2 | SIGDET_FLT_BYP},
+	{EMAC_SGMII_LN_SIGDET_CNTRL, SIGDET_LVL(8)},
+
+	{EMAC_SGMII_LN_SIGDET_DEGLITCH_CNTRL, SIGDET_DEGLITCH_CTRL(4)},
+	{EMAC_SGMII_LN_RX_MISC_CNTRL0, 0},
+	{EMAC_SGMII_LN_DRVR_LOGIC_CLKDIV,
+		DRVR_LOGIC_CLK_EN | DRVR_LOGIC_CLK_DIV(4)},
+
+	{EMAC_SGMII_LN_PARALLEL_RATE, PARALLEL_RATE_MODE0(1)},
+	{EMAC_SGMII_LN_TX_BAND_MODE, BAND_MODE0(2)},
+	{EMAC_SGMII_LN_RX_BAND, BAND_MODE0(3)},
+	{EMAC_SGMII_LN_LANE_MODE, LANE_MODE(26)},
+	{EMAC_SGMII_LN_RX_RCVR_PATH1_MODE0, CDR_PD_SEL_MODE0(3)},
+	{EMAC_SGMII_LN_RSM_CONFIG, BYPASS_RSM_SAMP_CAL | BYPASS_RSM_DLL_CAL},
+};
+
+static const struct emac_reg_write physical_coding_sublayer_programming_v2[] = {
+	{EMAC_SGMII_PHY_POW_DWN_CTRL0, PWRDN_B},
+	{EMAC_SGMII_PHY_CDR_CTRL0, CDR_MAX_CNT(15)},
+	{EMAC_SGMII_PHY_TX_PWR_CTRL, 0},
+	{EMAC_SGMII_PHY_LANE_CTRL1, L0_RX_EQUALIZE_ENABLE},
+};
+
+static int emac_sgmii_link_init(struct emac_adapter *adpt)
+{
+	struct phy_device *phydev = adpt->phydev;
+	struct emac_phy *phy = &adpt->phy;
+	u32 val;
+
+	val = readl(phy->base + EMAC_SGMII_PHY_AUTONEG_CFG2);
+
+	if (phydev->autoneg == AUTONEG_ENABLE) {
+		val &= ~(FORCE_AN_RX_CFG | FORCE_AN_TX_CFG);
+		val |= AN_ENABLE;
+		writel(val, phy->base + EMAC_SGMII_PHY_AUTONEG_CFG2);
+	} else {
+		u32 speed_cfg;
+
+		switch (phydev->speed) {
+		case SPEED_10:
+			speed_cfg = SPDMODE_10;
+			break;
+		case SPEED_100:
+			speed_cfg = SPDMODE_100;
+			break;
+		case SPEED_1000:
+			speed_cfg = SPDMODE_1000;
+			break;
+		default:
+			return -EINVAL;
+		}
+
+		if (phydev->duplex == DUPLEX_FULL)
+			speed_cfg |= DUPLEX_MODE;
+
+		val &= ~AN_ENABLE;
+		writel(speed_cfg, phy->base + EMAC_SGMII_PHY_SPEED_CFG1);
+		writel(val, phy->base + EMAC_SGMII_PHY_AUTONEG_CFG2);
+	}
+
+	return 0;
+}
+
+static int emac_sgmii_irq_clear(struct emac_adapter *adpt, u32 irq_bits)
+{
+	struct emac_phy *phy = &adpt->phy;
+	u32 status;
+
+	writel_relaxed(irq_bits, phy->base + EMAC_SGMII_PHY_INTERRUPT_CLEAR);
+	writel_relaxed(IRQ_GLOBAL_CLEAR, phy->base + EMAC_SGMII_PHY_IRQ_CMD);
+	/* Ensure interrupt clear command is written to HW */
+	wmb();
+
+	/* After set the IRQ_GLOBAL_CLEAR bit, the status clearing must
+	 * be confirmed before clearing the bits in other registers.
+	 * It takes a few cycles for hw to clear the interrupt status.
+	 */
+	if (readl_poll_timeout_atomic(phy->base +
+				      EMAC_SGMII_PHY_INTERRUPT_STATUS,
+				      status, !(status & irq_bits), 1,
+				      SGMII_PHY_IRQ_CLR_WAIT_TIME)) {
+		netdev_err(adpt->netdev,
+			   "error: failed clear SGMII irq: status:0x%x bits:0x%x\n",
+			   status, irq_bits);
+		return -EIO;
+	}
+
+	/* Finalize clearing procedure */
+	writel_relaxed(0, phy->base + EMAC_SGMII_PHY_IRQ_CMD);
+	writel_relaxed(0, phy->base + EMAC_SGMII_PHY_INTERRUPT_CLEAR);
+
+	/* Ensure that clearing procedure finalization is written to HW */
+	wmb();
+
+	return 0;
+}
+
+int emac_sgmii_init_v1(struct emac_adapter *adpt)
+{
+	struct emac_phy *phy = &adpt->phy;
+	unsigned int i;
+	int ret;
+
+	ret = emac_sgmii_link_init(adpt);
+	if (ret)
+		return ret;
+
+	emac_reg_write_all(phy->base, physical_coding_sublayer_programming_v1,
+			   ARRAY_SIZE(physical_coding_sublayer_programming_v1));
+	emac_reg_write_all(phy->base, sysclk_refclk_setting,
+			   ARRAY_SIZE(sysclk_refclk_setting));
+	emac_reg_write_all(phy->base, pll_setting, ARRAY_SIZE(pll_setting));
+	emac_reg_write_all(phy->base, cdr_setting, ARRAY_SIZE(cdr_setting));
+	emac_reg_write_all(phy->base, tx_rx_setting,
+			   ARRAY_SIZE(tx_rx_setting));
+
+	/* Power up the Ser/Des engine */
+	writel(SERDES_START, phy->base + EMAC_SGMII_PHY_SERDES_START);
+
+	for (i = 0; i < SERDES_START_WAIT_TIMES; i++) {
+		if (readl(phy->base + EMAC_QSERDES_COM_RESET_SM) & READY)
+			break;
+		usleep_range(100, 200);
+	}
+
+	if (i == SERDES_START_WAIT_TIMES) {
+		netdev_err(adpt->netdev, "error: ser/des failed to start\n");
+		return -EIO;
+	}
+	/* Mask out all the SGMII Interrupt */
+	writel(0, phy->base + EMAC_SGMII_PHY_INTERRUPT_MASK);
+
+	emac_sgmii_irq_clear(adpt, SGMII_PHY_INTERRUPT_ERR);
+
+	return 0;
+}
+
+int emac_sgmii_init_v2(struct emac_adapter *adpt)
+{
+	struct emac_phy *phy = &adpt->phy;
+	void __iomem *phy_regs = phy->base;
+	void __iomem *laned = phy->digital;
+	unsigned int i;
+	u32 lnstatus;
+	int ret;
+
+	ret = emac_sgmii_link_init(adpt);
+	if (ret)
+		return ret;
+
+	/* PCS lane-x init */
+	emac_reg_write_all(phy->base, physical_coding_sublayer_programming_v2,
+			   ARRAY_SIZE(physical_coding_sublayer_programming_v2));
+
+	/* SGMII lane-x init */
+	emac_reg_write_all(phy->digital,
+			   sgmii_v2_laned, ARRAY_SIZE(sgmii_v2_laned));
+
+	/* Power up PCS and start reset lane state machine */
+
+	writel(0, phy_regs + EMAC_SGMII_PHY_RESET_CTRL);
+	writel(1, laned + SGMII_LN_RSM_START);
+
+	/* Wait for c_ready assertion */
+	for (i = 0; i < SERDES_START_WAIT_TIMES; i++) {
+		lnstatus = readl(phy_regs + SGMII_PHY_LN_LANE_STATUS);
+		if (lnstatus & BIT(1))
+			break;
+		usleep_range(100, 200);
+	}
+
+	if (i == SERDES_START_WAIT_TIMES) {
+		netdev_err(adpt->netdev, "SGMII failed to start\n");
+		return -EIO;
+	}
+
+	/* Disable digital and SERDES loopback */
+	writel(0, phy_regs + SGMII_PHY_LN_BIST_GEN0);
+	writel(0, phy_regs + SGMII_PHY_LN_BIST_GEN2);
+	writel(0, phy_regs + SGMII_PHY_LN_CDR_CTRL1);
+
+	/* Mask out all the SGMII Interrupt */
+	writel(0, phy_regs + EMAC_SGMII_PHY_INTERRUPT_MASK);
+
+	emac_sgmii_irq_clear(adpt, SGMII_PHY_INTERRUPT_ERR);
+
+	return 0;
+}
+
+static void emac_sgmii_reset_prepare(struct emac_adapter *adpt)
+{
+	struct emac_phy *phy = &adpt->phy;
+	u32 val;
+
+	/* Reset PHY */
+	val = readl(phy->base + EMAC_EMAC_WRAPPER_CSR2);
+	writel(((val & ~PHY_RESET) | PHY_RESET), phy->base +
+	       EMAC_EMAC_WRAPPER_CSR2);
+	/* Ensure phy-reset command is written to HW before the release cmd */
+	msleep(50);
+	val = readl(phy->base + EMAC_EMAC_WRAPPER_CSR2);
+	writel((val & ~PHY_RESET), phy->base + EMAC_EMAC_WRAPPER_CSR2);
+	/* Ensure phy-reset release command is written to HW before initializing
+	 * SGMII
+	 */
+	msleep(50);
+}
+
+void emac_sgmii_reset(struct emac_adapter *adpt)
+{
+	int ret;
+
+	clk_set_rate(adpt->clk[EMAC_CLK_HIGH_SPEED], 19200000);
+	emac_sgmii_reset_prepare(adpt);
+
+	ret = adpt->phy.initialize(adpt);
+	if (ret)
+		netdev_err(adpt->netdev,
+			   "could not reinitialize internal PHY (error=%i)\n",
+			   ret);
+
+	clk_set_rate(adpt->clk[EMAC_CLK_HIGH_SPEED], 125000000);
+}
+
+static int emac_sgmii_acpi_match(struct device *dev, void *data)
+{
+	static const struct acpi_device_id match_table[] = {
+		{
+			.id = "QCOM8071",
+			.driver_data = (kernel_ulong_t)emac_sgmii_init_v2,
+		},
+		{}
+	};
+	const struct acpi_device_id *id = acpi_match_device(match_table, dev);
+	emac_sgmii_initialize *initialize = data;
+
+	if (id)
+		*initialize = (emac_sgmii_initialize)id->driver_data;
+
+	return !!id;
+}
+
+static const struct of_device_id emac_sgmii_dt_match[] = {
+	{
+		.compatible = "qcom,fsm9900-emac-sgmii",
+		.data = emac_sgmii_init_v1,
+	},
+	{
+		.compatible = "qcom,qdf2432-emac-sgmii",
+		.data = emac_sgmii_init_v2,
+	},
+	{}
+};
+
+int emac_sgmii_config(struct platform_device *pdev, struct emac_adapter *adpt)
+{
+	struct platform_device *sgmii_pdev = NULL;
+	struct emac_phy *phy = &adpt->phy;
+	struct resource *res;
+	int ret;
+
+	if (has_acpi_companion(&pdev->dev)) {
+		struct device *dev;
+
+		dev = device_find_child(&pdev->dev, &phy->initialize,
+					emac_sgmii_acpi_match);
+
+		if (!dev) {
+			dev_err(&pdev->dev, "cannot find internal phy node\n");
+			return -ENODEV;
+		}
+
+		sgmii_pdev = to_platform_device(dev);
+	} else {
+		const struct of_device_id *match;
+		struct device_node *np;
+
+		np = of_parse_phandle(pdev->dev.of_node, "internal-phy", 0);
+		if (!np) {
+			dev_err(&pdev->dev, "missing internal-phy property\n");
+			return -ENODEV;
+		}
+
+		sgmii_pdev = of_find_device_by_node(np);
+		if (!sgmii_pdev) {
+			dev_err(&pdev->dev, "invalid internal-phy property\n");
+			return -ENODEV;
+		}
+
+		match = of_match_device(emac_sgmii_dt_match, &sgmii_pdev->dev);
+		if (!match) {
+			dev_err(&pdev->dev, "unrecognized internal phy node\n");
+			ret = -ENODEV;
+			goto error_put_device;
+		}
+
+		phy->initialize = (emac_sgmii_initialize)match->data;
+	}
+
+	/* Base address is the first address */
+	res = platform_get_resource(sgmii_pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		ret = -EINVAL;
+		goto error_put_device;
+	}
+
+	phy->base = ioremap(res->start, resource_size(res));
+	if (!phy->base) {
+		ret = -ENOMEM;
+		goto error_put_device;
+	}
+
+	/* v2 SGMII has a per-lane digital digital, so parse it if it exists */
+	res = platform_get_resource(sgmii_pdev, IORESOURCE_MEM, 1);
+	if (res) {
+		phy->digital = ioremap(res->start, resource_size(res));
+		if (!phy->digital) {
+			ret = -ENOMEM;
+			goto error_unmap_base;
+		}
+	}
+
+	ret = phy->initialize(adpt);
+	if (ret)
+		goto error;
+
+	/* We've remapped the addresses, so we don't need the device any
+	 * more.  of_find_device_by_node() says we should release it.
+	 */
+	put_device(&sgmii_pdev->dev);
+
+	return 0;
+
+error:
+	if (phy->digital)
+		iounmap(phy->digital);
+error_unmap_base:
+	iounmap(phy->base);
+error_put_device:
+	put_device(&sgmii_pdev->dev);
+
+	return ret;
+}
diff --git a/drivers/net/ethernet/qualcomm/emac/emac-sgmii.h b/drivers/net/ethernet/qualcomm/emac/emac-sgmii.h
new file mode 100644
index 000000000000..ce79212ff403
--- /dev/null
+++ b/drivers/net/ethernet/qualcomm/emac/emac-sgmii.h
@@ -0,0 +1,24 @@
+/* Copyright (c) 2015-2016, 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 _EMAC_SGMII_H_
+#define _EMAC_SGMII_H_
+
+struct emac_adapter;
+struct platform_device;
+
+int emac_sgmii_init_v1(struct emac_adapter *adpt);
+int emac_sgmii_init_v2(struct emac_adapter *adpt);
+int emac_sgmii_config(struct platform_device *pdev, struct emac_adapter *adpt);
+void emac_sgmii_reset(struct emac_adapter *adpt);
+
+#endif
diff --git a/drivers/net/ethernet/qualcomm/emac/emac.c b/drivers/net/ethernet/qualcomm/emac/emac.c
new file mode 100644
index 000000000000..9bf3b2b82e95
--- /dev/null
+++ b/drivers/net/ethernet/qualcomm/emac/emac.c
@@ -0,0 +1,755 @@
+/* Copyright (c) 2013-2016, 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.
+ */
+
+/* Qualcomm Technologies, Inc. EMAC Gigabit Ethernet Driver */
+
+#include <linux/if_ether.h>
+#include <linux/if_vlan.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_net.h>
+#include <linux/of_device.h>
+#include <linux/phy.h>
+#include <linux/platform_device.h>
+#include <linux/acpi.h>
+#include "emac.h"
+#include "emac-mac.h"
+#include "emac-phy.h"
+#include "emac-sgmii.h"
+
+#define EMAC_MSG_DEFAULT (NETIF_MSG_DRV | NETIF_MSG_PROBE | NETIF_MSG_LINK |  \
+		NETIF_MSG_TIMER | NETIF_MSG_IFDOWN | NETIF_MSG_IFUP)
+
+#define EMAC_RRD_SIZE					     4
+/* The RRD size if timestamping is enabled: */
+#define EMAC_TS_RRD_SIZE				     6
+#define EMAC_TPD_SIZE					     4
+#define EMAC_RFD_SIZE					     2
+
+#define REG_MAC_RX_STATUS_BIN		 EMAC_RXMAC_STATC_REG0
+#define REG_MAC_RX_STATUS_END		EMAC_RXMAC_STATC_REG22
+#define REG_MAC_TX_STATUS_BIN		 EMAC_TXMAC_STATC_REG0
+#define REG_MAC_TX_STATUS_END		EMAC_TXMAC_STATC_REG24
+
+#define RXQ0_NUM_RFD_PREF_DEF				     8
+#define TXQ0_NUM_TPD_PREF_DEF				     5
+
+#define EMAC_PREAMBLE_DEF				     7
+
+#define DMAR_DLY_CNT_DEF				    15
+#define DMAW_DLY_CNT_DEF				     4
+
+#define IMR_NORMAL_MASK         (\
+		ISR_ERROR       |\
+		ISR_GPHY_LINK   |\
+		ISR_TX_PKT      |\
+		GPHY_WAKEUP_INT)
+
+#define IMR_EXTENDED_MASK       (\
+		SW_MAN_INT      |\
+		ISR_OVER        |\
+		ISR_ERROR       |\
+		ISR_GPHY_LINK   |\
+		ISR_TX_PKT      |\
+		GPHY_WAKEUP_INT)
+
+#define ISR_TX_PKT      (\
+	TX_PKT_INT      |\
+	TX_PKT_INT1     |\
+	TX_PKT_INT2     |\
+	TX_PKT_INT3)
+
+#define ISR_GPHY_LINK        (\
+	GPHY_LINK_UP_INT     |\
+	GPHY_LINK_DOWN_INT)
+
+#define ISR_OVER        (\
+	RFD0_UR_INT     |\
+	RFD1_UR_INT     |\
+	RFD2_UR_INT     |\
+	RFD3_UR_INT     |\
+	RFD4_UR_INT     |\
+	RXF_OF_INT      |\
+	TXF_UR_INT)
+
+#define ISR_ERROR       (\
+	DMAR_TO_INT     |\
+	DMAW_TO_INT     |\
+	TXQ_TO_INT)
+
+/* in sync with enum emac_clk_id */
+static const char * const emac_clk_name[] = {
+	"axi_clk", "cfg_ahb_clk", "high_speed_clk", "mdio_clk", "tx_clk",
+	"rx_clk", "sys_clk"
+};
+
+void emac_reg_update32(void __iomem *addr, u32 mask, u32 val)
+{
+	u32 data = readl(addr);
+
+	writel(((data & ~mask) | val), addr);
+}
+
+/* reinitialize */
+int emac_reinit_locked(struct emac_adapter *adpt)
+{
+	int ret;
+
+	mutex_lock(&adpt->reset_lock);
+
+	emac_mac_down(adpt);
+	emac_sgmii_reset(adpt);
+	ret = emac_mac_up(adpt);
+
+	mutex_unlock(&adpt->reset_lock);
+
+	return ret;
+}
+
+/* NAPI */
+static int emac_napi_rtx(struct napi_struct *napi, int budget)
+{
+	struct emac_rx_queue *rx_q =
+		container_of(napi, struct emac_rx_queue, napi);
+	struct emac_adapter *adpt = netdev_priv(rx_q->netdev);
+	struct emac_irq *irq = rx_q->irq;
+	int work_done = 0;
+
+	emac_mac_rx_process(adpt, rx_q, &work_done, budget);
+
+	if (work_done < budget) {
+		napi_complete(napi);
+
+		irq->mask |= rx_q->intr;
+		writel(irq->mask, adpt->base + EMAC_INT_MASK);
+	}
+
+	return work_done;
+}
+
+/* Transmit the packet */
+static int emac_start_xmit(struct sk_buff *skb, struct net_device *netdev)
+{
+	struct emac_adapter *adpt = netdev_priv(netdev);
+
+	return emac_mac_tx_buf_send(adpt, &adpt->tx_q, skb);
+}
+
+irqreturn_t emac_isr(int _irq, void *data)
+{
+	struct emac_irq *irq = data;
+	struct emac_adapter *adpt =
+		container_of(irq, struct emac_adapter, irq);
+	struct emac_rx_queue *rx_q = &adpt->rx_q;
+	u32 isr, status;
+
+	/* disable the interrupt */
+	writel(0, adpt->base + EMAC_INT_MASK);
+
+	isr = readl_relaxed(adpt->base + EMAC_INT_STATUS);
+
+	status = isr & irq->mask;
+	if (status == 0)
+		goto exit;
+
+	if (status & ISR_ERROR) {
+		netif_warn(adpt,  intr, adpt->netdev,
+			   "warning: error irq status 0x%lx\n",
+			   status & ISR_ERROR);
+		/* reset MAC */
+		schedule_work(&adpt->work_thread);
+	}
+
+	/* Schedule the napi for receive queue with interrupt
+	 * status bit set
+	 */
+	if (status & rx_q->intr) {
+		if (napi_schedule_prep(&rx_q->napi)) {
+			irq->mask &= ~rx_q->intr;
+			__napi_schedule(&rx_q->napi);
+		}
+	}
+
+	if (status & TX_PKT_INT)
+		emac_mac_tx_process(adpt, &adpt->tx_q);
+
+	if (status & ISR_OVER)
+		net_warn_ratelimited("warning: TX/RX overflow\n");
+
+	/* link event */
+	if (status & ISR_GPHY_LINK)
+		phy_mac_interrupt(adpt->phydev, !!(status & GPHY_LINK_UP_INT));
+
+exit:
+	/* enable the interrupt */
+	writel(irq->mask, adpt->base + EMAC_INT_MASK);
+
+	return IRQ_HANDLED;
+}
+
+/* Configure VLAN tag strip/insert feature */
+static int emac_set_features(struct net_device *netdev,
+			     netdev_features_t features)
+{
+	netdev_features_t changed = features ^ netdev->features;
+	struct emac_adapter *adpt = netdev_priv(netdev);
+
+	/* We only need to reprogram the hardware if the VLAN tag features
+	 * have changed, and if it's already running.
+	 */
+	if (!(changed & (NETIF_F_HW_VLAN_CTAG_TX | NETIF_F_HW_VLAN_CTAG_RX)))
+		return 0;
+
+	if (!netif_running(netdev))
+		return 0;
+
+	/* emac_mac_mode_config() uses netdev->features to configure the EMAC,
+	 * so make sure it's set first.
+	 */
+	netdev->features = features;
+
+	return emac_reinit_locked(adpt);
+}
+
+/* Configure Multicast and Promiscuous modes */
+static void emac_rx_mode_set(struct net_device *netdev)
+{
+	struct emac_adapter *adpt = netdev_priv(netdev);
+	struct netdev_hw_addr *ha;
+
+	emac_mac_mode_config(adpt);
+
+	/* update multicast address filtering */
+	emac_mac_multicast_addr_clear(adpt);
+	netdev_for_each_mc_addr(ha, netdev)
+		emac_mac_multicast_addr_set(adpt, ha->addr);
+}
+
+/* Change the Maximum Transfer Unit (MTU) */
+static int emac_change_mtu(struct net_device *netdev, int new_mtu)
+{
+	unsigned int max_frame = new_mtu + ETH_HLEN + ETH_FCS_LEN + VLAN_HLEN;
+	struct emac_adapter *adpt = netdev_priv(netdev);
+
+	if ((max_frame < EMAC_MIN_ETH_FRAME_SIZE) ||
+	    (max_frame > EMAC_MAX_ETH_FRAME_SIZE)) {
+		netdev_err(adpt->netdev, "error: invalid MTU setting\n");
+		return -EINVAL;
+	}
+
+	netif_info(adpt, hw, adpt->netdev,
+		   "changing MTU from %d to %d\n", netdev->mtu,
+		   new_mtu);
+	netdev->mtu = new_mtu;
+
+	if (netif_running(netdev))
+		return emac_reinit_locked(adpt);
+
+	return 0;
+}
+
+/* Called when the network interface is made active */
+static int emac_open(struct net_device *netdev)
+{
+	struct emac_adapter *adpt = netdev_priv(netdev);
+	int ret;
+
+	/* allocate rx/tx dma buffer & descriptors */
+	ret = emac_mac_rx_tx_rings_alloc_all(adpt);
+	if (ret) {
+		netdev_err(adpt->netdev, "error allocating rx/tx rings\n");
+		return ret;
+	}
+
+	ret = emac_mac_up(adpt);
+	if (ret) {
+		emac_mac_rx_tx_rings_free_all(adpt);
+		return ret;
+	}
+
+	emac_mac_start(adpt);
+
+	return 0;
+}
+
+/* Called when the network interface is disabled */
+static int emac_close(struct net_device *netdev)
+{
+	struct emac_adapter *adpt = netdev_priv(netdev);
+
+	mutex_lock(&adpt->reset_lock);
+
+	emac_mac_down(adpt);
+	emac_mac_rx_tx_rings_free_all(adpt);
+
+	mutex_unlock(&adpt->reset_lock);
+
+	return 0;
+}
+
+/* Respond to a TX hang */
+static void emac_tx_timeout(struct net_device *netdev)
+{
+	struct emac_adapter *adpt = netdev_priv(netdev);
+
+	schedule_work(&adpt->work_thread);
+}
+
+/* IOCTL support for the interface */
+static int emac_ioctl(struct net_device *netdev, struct ifreq *ifr, int cmd)
+{
+	if (!netif_running(netdev))
+		return -EINVAL;
+
+	if (!netdev->phydev)
+		return -ENODEV;
+
+	return phy_mii_ioctl(netdev->phydev, ifr, cmd);
+}
+
+/* Provide network statistics info for the interface */
+static struct rtnl_link_stats64 *emac_get_stats64(struct net_device *netdev,
+						  struct rtnl_link_stats64 *net_stats)
+{
+	struct emac_adapter *adpt = netdev_priv(netdev);
+	unsigned int addr = REG_MAC_RX_STATUS_BIN;
+	struct emac_stats *stats = &adpt->stats;
+	u64 *stats_itr = &adpt->stats.rx_ok;
+	u32 val;
+
+	spin_lock(&stats->lock);
+
+	while (addr <= REG_MAC_RX_STATUS_END) {
+		val = readl_relaxed(adpt->base + addr);
+		*stats_itr += val;
+		stats_itr++;
+		addr += sizeof(u32);
+	}
+
+	/* additional rx status */
+	val = readl_relaxed(adpt->base + EMAC_RXMAC_STATC_REG23);
+	adpt->stats.rx_crc_align += val;
+	val = readl_relaxed(adpt->base + EMAC_RXMAC_STATC_REG24);
+	adpt->stats.rx_jabbers += val;
+
+	/* update tx status */
+	addr = REG_MAC_TX_STATUS_BIN;
+	stats_itr = &adpt->stats.tx_ok;
+
+	while (addr <= REG_MAC_TX_STATUS_END) {
+		val = readl_relaxed(adpt->base + addr);
+		*stats_itr += val;
+		++stats_itr;
+		addr += sizeof(u32);
+	}
+
+	/* additional tx status */
+	val = readl_relaxed(adpt->base + EMAC_TXMAC_STATC_REG25);
+	adpt->stats.tx_col += val;
+
+	/* return parsed statistics */
+	net_stats->rx_packets = stats->rx_ok;
+	net_stats->tx_packets = stats->tx_ok;
+	net_stats->rx_bytes = stats->rx_byte_cnt;
+	net_stats->tx_bytes = stats->tx_byte_cnt;
+	net_stats->multicast = stats->rx_mcast;
+	net_stats->collisions = stats->tx_1_col + stats->tx_2_col * 2 +
+				stats->tx_late_col + stats->tx_abort_col;
+
+	net_stats->rx_errors = stats->rx_frag + stats->rx_fcs_err +
+			       stats->rx_len_err + stats->rx_sz_ov +
+			       stats->rx_align_err;
+	net_stats->rx_fifo_errors = stats->rx_rxf_ov;
+	net_stats->rx_length_errors = stats->rx_len_err;
+	net_stats->rx_crc_errors = stats->rx_fcs_err;
+	net_stats->rx_frame_errors = stats->rx_align_err;
+	net_stats->rx_over_errors = stats->rx_rxf_ov;
+	net_stats->rx_missed_errors = stats->rx_rxf_ov;
+
+	net_stats->tx_errors = stats->tx_late_col + stats->tx_abort_col +
+			       stats->tx_underrun + stats->tx_trunc;
+	net_stats->tx_fifo_errors = stats->tx_underrun;
+	net_stats->tx_aborted_errors = stats->tx_abort_col;
+	net_stats->tx_window_errors = stats->tx_late_col;
+
+	spin_unlock(&stats->lock);
+
+	return net_stats;
+}
+
+static const struct net_device_ops emac_netdev_ops = {
+	.ndo_open		= emac_open,
+	.ndo_stop		= emac_close,
+	.ndo_validate_addr	= eth_validate_addr,
+	.ndo_start_xmit		= emac_start_xmit,
+	.ndo_set_mac_address	= eth_mac_addr,
+	.ndo_change_mtu		= emac_change_mtu,
+	.ndo_do_ioctl		= emac_ioctl,
+	.ndo_tx_timeout		= emac_tx_timeout,
+	.ndo_get_stats64	= emac_get_stats64,
+	.ndo_set_features       = emac_set_features,
+	.ndo_set_rx_mode        = emac_rx_mode_set,
+};
+
+/* Watchdog task routine, called to reinitialize the EMAC */
+static void emac_work_thread(struct work_struct *work)
+{
+	struct emac_adapter *adpt =
+		container_of(work, struct emac_adapter, work_thread);
+
+	emac_reinit_locked(adpt);
+}
+
+/* Initialize various data structures  */
+static void emac_init_adapter(struct emac_adapter *adpt)
+{
+	u32 reg;
+
+	/* descriptors */
+	adpt->tx_desc_cnt = EMAC_DEF_TX_DESCS;
+	adpt->rx_desc_cnt = EMAC_DEF_RX_DESCS;
+
+	/* dma */
+	adpt->dma_order = emac_dma_ord_out;
+	adpt->dmar_block = emac_dma_req_4096;
+	adpt->dmaw_block = emac_dma_req_128;
+	adpt->dmar_dly_cnt = DMAR_DLY_CNT_DEF;
+	adpt->dmaw_dly_cnt = DMAW_DLY_CNT_DEF;
+	adpt->tpd_burst = TXQ0_NUM_TPD_PREF_DEF;
+	adpt->rfd_burst = RXQ0_NUM_RFD_PREF_DEF;
+
+	/* irq moderator */
+	reg = ((EMAC_DEF_RX_IRQ_MOD >> 1) << IRQ_MODERATOR2_INIT_SHFT) |
+	      ((EMAC_DEF_TX_IRQ_MOD >> 1) << IRQ_MODERATOR_INIT_SHFT);
+	adpt->irq_mod = reg;
+
+	/* others */
+	adpt->preamble = EMAC_PREAMBLE_DEF;
+}
+
+/* Get the clock */
+static int emac_clks_get(struct platform_device *pdev,
+			 struct emac_adapter *adpt)
+{
+	unsigned int i;
+
+	for (i = 0; i < EMAC_CLK_CNT; i++) {
+		struct clk *clk = devm_clk_get(&pdev->dev, emac_clk_name[i]);
+
+		if (IS_ERR(clk)) {
+			dev_err(&pdev->dev,
+				"could not claim clock %s (error=%li)\n",
+				emac_clk_name[i], PTR_ERR(clk));
+
+			return PTR_ERR(clk);
+		}
+
+		adpt->clk[i] = clk;
+	}
+
+	return 0;
+}
+
+/* Initialize clocks */
+static int emac_clks_phase1_init(struct platform_device *pdev,
+				 struct emac_adapter *adpt)
+{
+	int ret;
+
+	ret = emac_clks_get(pdev, adpt);
+	if (ret)
+		return ret;
+
+	ret = clk_prepare_enable(adpt->clk[EMAC_CLK_AXI]);
+	if (ret)
+		return ret;
+
+	ret = clk_prepare_enable(adpt->clk[EMAC_CLK_CFG_AHB]);
+	if (ret)
+		return ret;
+
+	ret = clk_set_rate(adpt->clk[EMAC_CLK_HIGH_SPEED], 19200000);
+	if (ret)
+		return ret;
+
+	return clk_prepare_enable(adpt->clk[EMAC_CLK_HIGH_SPEED]);
+}
+
+/* Enable clocks; needs emac_clks_phase1_init to be called before */
+static int emac_clks_phase2_init(struct platform_device *pdev,
+				 struct emac_adapter *adpt)
+{
+	int ret;
+
+	ret = clk_set_rate(adpt->clk[EMAC_CLK_TX], 125000000);
+	if (ret)
+		return ret;
+
+	ret = clk_prepare_enable(adpt->clk[EMAC_CLK_TX]);
+	if (ret)
+		return ret;
+
+	ret = clk_set_rate(adpt->clk[EMAC_CLK_HIGH_SPEED], 125000000);
+	if (ret)
+		return ret;
+
+	ret = clk_set_rate(adpt->clk[EMAC_CLK_MDIO], 25000000);
+	if (ret)
+		return ret;
+
+	ret = clk_prepare_enable(adpt->clk[EMAC_CLK_MDIO]);
+	if (ret)
+		return ret;
+
+	ret = clk_prepare_enable(adpt->clk[EMAC_CLK_RX]);
+	if (ret)
+		return ret;
+
+	return clk_prepare_enable(adpt->clk[EMAC_CLK_SYS]);
+}
+
+static void emac_clks_teardown(struct emac_adapter *adpt)
+{
+
+	unsigned int i;
+
+	for (i = 0; i < EMAC_CLK_CNT; i++)
+		clk_disable_unprepare(adpt->clk[i]);
+}
+
+/* Get the resources */
+static int emac_probe_resources(struct platform_device *pdev,
+				struct emac_adapter *adpt)
+{
+	struct net_device *netdev = adpt->netdev;
+	struct resource *res;
+	char maddr[ETH_ALEN];
+	int ret = 0;
+
+	/* get mac address */
+	if (device_get_mac_address(&pdev->dev, maddr, ETH_ALEN))
+		ether_addr_copy(netdev->dev_addr, maddr);
+	else
+		eth_hw_addr_random(netdev);
+
+	/* Core 0 interrupt */
+	ret = platform_get_irq(pdev, 0);
+	if (ret < 0) {
+		dev_err(&pdev->dev,
+			"error: missing core0 irq resource (error=%i)\n", ret);
+		return ret;
+	}
+	adpt->irq.irq = ret;
+
+	/* base register address */
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	adpt->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(adpt->base))
+		return PTR_ERR(adpt->base);
+
+	/* CSR register address */
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	adpt->csr = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(adpt->csr))
+		return PTR_ERR(adpt->csr);
+
+	netdev->base_addr = (unsigned long)adpt->base;
+
+	return 0;
+}
+
+static const struct of_device_id emac_dt_match[] = {
+	{
+		.compatible = "qcom,fsm9900-emac",
+	},
+	{}
+};
+
+#if IS_ENABLED(CONFIG_ACPI)
+static const struct acpi_device_id emac_acpi_match[] = {
+	{
+		.id = "QCOM8070",
+	},
+	{}
+};
+MODULE_DEVICE_TABLE(acpi, emac_acpi_match);
+#endif
+
+static int emac_probe(struct platform_device *pdev)
+{
+	struct net_device *netdev;
+	struct emac_adapter *adpt;
+	struct emac_phy *phy;
+	u16 devid, revid;
+	u32 reg;
+	int ret;
+
+	/* The EMAC itself is capable of 64-bit DMA, so try that first. */
+	ret = dma_set_mask_and_coherent(&pdev->dev, DMA_BIT_MASK(64));
+	if (ret) {
+		/* Some platforms may restrict the EMAC's address bus to less
+		 * then the size of DDR. In this case, we need to try a
+		 * smaller mask.  We could try every possible smaller mask,
+		 * but that's overkill.  Instead, just fall to 32-bit, which
+		 * should always work.
+		 */
+		ret = dma_set_mask_and_coherent(&pdev->dev, DMA_BIT_MASK(32));
+		if (ret) {
+			dev_err(&pdev->dev, "could not set DMA mask\n");
+			return ret;
+		}
+	}
+
+	netdev = alloc_etherdev(sizeof(struct emac_adapter));
+	if (!netdev)
+		return -ENOMEM;
+
+	dev_set_drvdata(&pdev->dev, netdev);
+	SET_NETDEV_DEV(netdev, &pdev->dev);
+
+	adpt = netdev_priv(netdev);
+	adpt->netdev = netdev;
+	adpt->msg_enable = EMAC_MSG_DEFAULT;
+
+	phy = &adpt->phy;
+
+	mutex_init(&adpt->reset_lock);
+	spin_lock_init(&adpt->stats.lock);
+
+	adpt->irq.mask = RX_PKT_INT0 | IMR_NORMAL_MASK;
+
+	ret = emac_probe_resources(pdev, adpt);
+	if (ret)
+		goto err_undo_netdev;
+
+	/* initialize clocks */
+	ret = emac_clks_phase1_init(pdev, adpt);
+	if (ret) {
+		dev_err(&pdev->dev, "could not initialize clocks\n");
+		goto err_undo_netdev;
+	}
+
+	netdev->watchdog_timeo = EMAC_WATCHDOG_TIME;
+	netdev->irq = adpt->irq.irq;
+
+	adpt->rrd_size = EMAC_RRD_SIZE;
+	adpt->tpd_size = EMAC_TPD_SIZE;
+	adpt->rfd_size = EMAC_RFD_SIZE;
+
+	netdev->netdev_ops = &emac_netdev_ops;
+
+	emac_init_adapter(adpt);
+
+	/* init external phy */
+	ret = emac_phy_config(pdev, adpt);
+	if (ret)
+		goto err_undo_clocks;
+
+	/* init internal sgmii phy */
+	ret = emac_sgmii_config(pdev, adpt);
+	if (ret)
+		goto err_undo_mdiobus;
+
+	/* enable clocks */
+	ret = emac_clks_phase2_init(pdev, adpt);
+	if (ret) {
+		dev_err(&pdev->dev, "could not initialize clocks\n");
+		goto err_undo_mdiobus;
+	}
+
+	emac_mac_reset(adpt);
+
+	/* set hw features */
+	netdev->features = NETIF_F_SG | NETIF_F_HW_CSUM | NETIF_F_RXCSUM |
+			NETIF_F_TSO | NETIF_F_TSO6 | NETIF_F_HW_VLAN_CTAG_RX |
+			NETIF_F_HW_VLAN_CTAG_TX;
+	netdev->hw_features = netdev->features;
+
+	netdev->vlan_features |= NETIF_F_SG | NETIF_F_HW_CSUM |
+				 NETIF_F_TSO | NETIF_F_TSO6;
+
+	INIT_WORK(&adpt->work_thread, emac_work_thread);
+
+	/* Initialize queues */
+	emac_mac_rx_tx_ring_init_all(pdev, adpt);
+
+	netif_napi_add(netdev, &adpt->rx_q.napi, emac_napi_rtx,
+		       NAPI_POLL_WEIGHT);
+
+	ret = register_netdev(netdev);
+	if (ret) {
+		dev_err(&pdev->dev, "could not register net device\n");
+		goto err_undo_napi;
+	}
+
+	reg =  readl_relaxed(adpt->base + EMAC_DMA_MAS_CTRL);
+	devid = (reg & DEV_ID_NUM_BMSK)  >> DEV_ID_NUM_SHFT;
+	revid = (reg & DEV_REV_NUM_BMSK) >> DEV_REV_NUM_SHFT;
+	reg = readl_relaxed(adpt->base + EMAC_CORE_HW_VERSION);
+
+	netif_info(adpt, probe, netdev,
+		   "hardware id %d.%d, hardware version %d.%d.%d\n",
+		   devid, revid,
+		   (reg & MAJOR_BMSK) >> MAJOR_SHFT,
+		   (reg & MINOR_BMSK) >> MINOR_SHFT,
+		   (reg & STEP_BMSK)  >> STEP_SHFT);
+
+	return 0;
+
+err_undo_napi:
+	netif_napi_del(&adpt->rx_q.napi);
+err_undo_mdiobus:
+	mdiobus_unregister(adpt->mii_bus);
+err_undo_clocks:
+	emac_clks_teardown(adpt);
+err_undo_netdev:
+	free_netdev(netdev);
+
+	return ret;
+}
+
+static int emac_remove(struct platform_device *pdev)
+{
+	struct net_device *netdev = dev_get_drvdata(&pdev->dev);
+	struct emac_adapter *adpt = netdev_priv(netdev);
+
+	unregister_netdev(netdev);
+	netif_napi_del(&adpt->rx_q.napi);
+
+	emac_clks_teardown(adpt);
+
+	mdiobus_unregister(adpt->mii_bus);
+	free_netdev(netdev);
+
+	if (adpt->phy.digital)
+		iounmap(adpt->phy.digital);
+	iounmap(adpt->phy.base);
+
+	return 0;
+}
+
+static struct platform_driver emac_platform_driver = {
+	.probe	= emac_probe,
+	.remove	= emac_remove,
+	.driver = {
+		.name		= "qcom-emac",
+		.of_match_table = emac_dt_match,
+		.acpi_match_table = ACPI_PTR(emac_acpi_match),
+	},
+};
+
+module_platform_driver(emac_platform_driver);
+
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:qcom-emac");
diff --git a/drivers/net/ethernet/qualcomm/emac/emac.h b/drivers/net/ethernet/qualcomm/emac/emac.h
new file mode 100644
index 000000000000..0c76e6cb8c9e
--- /dev/null
+++ b/drivers/net/ethernet/qualcomm/emac/emac.h
@@ -0,0 +1,335 @@
+/* Copyright (c) 2013-2016, 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 _EMAC_H_
+#define _EMAC_H_
+
+#include <linux/irqreturn.h>
+#include <linux/netdevice.h>
+#include <linux/clk.h>
+#include <linux/platform_device.h>
+#include "emac-mac.h"
+#include "emac-phy.h"
+
+/* EMAC base register offsets */
+#define EMAC_DMA_MAS_CTRL                                     0x001400
+#define EMAC_IRQ_MOD_TIM_INIT                                 0x001408
+#define EMAC_BLK_IDLE_STS                                     0x00140c
+#define EMAC_PHY_LINK_DELAY                                   0x00141c
+#define EMAC_SYS_ALIV_CTRL                                    0x001434
+#define EMAC_MAC_IPGIFG_CTRL                                  0x001484
+#define EMAC_MAC_STA_ADDR0                                    0x001488
+#define EMAC_MAC_STA_ADDR1                                    0x00148c
+#define EMAC_HASH_TAB_REG0                                    0x001490
+#define EMAC_HASH_TAB_REG1                                    0x001494
+#define EMAC_MAC_HALF_DPLX_CTRL                               0x001498
+#define EMAC_MAX_FRAM_LEN_CTRL                                0x00149c
+#define EMAC_INT_STATUS                                       0x001600
+#define EMAC_INT_MASK                                         0x001604
+#define EMAC_RXMAC_STATC_REG0                                 0x001700
+#define EMAC_RXMAC_STATC_REG22                                0x001758
+#define EMAC_TXMAC_STATC_REG0                                 0x001760
+#define EMAC_TXMAC_STATC_REG24                                0x0017c0
+#define EMAC_CORE_HW_VERSION                                  0x001974
+#define EMAC_IDT_TABLE0                                       0x001b00
+#define EMAC_RXMAC_STATC_REG23                                0x001bc8
+#define EMAC_RXMAC_STATC_REG24                                0x001bcc
+#define EMAC_TXMAC_STATC_REG25                                0x001bd0
+#define EMAC_INT1_MASK                                        0x001bf0
+#define EMAC_INT1_STATUS                                      0x001bf4
+#define EMAC_INT2_MASK                                        0x001bf8
+#define EMAC_INT2_STATUS                                      0x001bfc
+#define EMAC_INT3_MASK                                        0x001c00
+#define EMAC_INT3_STATUS                                      0x001c04
+
+/* EMAC_DMA_MAS_CTRL */
+#define DEV_ID_NUM_BMSK                                     0x7f000000
+#define DEV_ID_NUM_SHFT                                             24
+#define DEV_REV_NUM_BMSK                                      0xff0000
+#define DEV_REV_NUM_SHFT                                            16
+#define INT_RD_CLR_EN                                           0x4000
+#define IRQ_MODERATOR2_EN                                        0x800
+#define IRQ_MODERATOR_EN                                         0x400
+#define LPW_CLK_SEL                                               0x80
+#define LPW_STATE                                                 0x20
+#define LPW_MODE                                                  0x10
+#define SOFT_RST                                                   0x1
+
+/* EMAC_IRQ_MOD_TIM_INIT */
+#define IRQ_MODERATOR2_INIT_BMSK                            0xffff0000
+#define IRQ_MODERATOR2_INIT_SHFT                                    16
+#define IRQ_MODERATOR_INIT_BMSK                                 0xffff
+#define IRQ_MODERATOR_INIT_SHFT                                      0
+
+/* EMAC_INT_STATUS */
+#define DIS_INT                                                BIT(31)
+#define PTP_INT                                                BIT(30)
+#define RFD4_UR_INT                                            BIT(29)
+#define TX_PKT_INT3                                            BIT(26)
+#define TX_PKT_INT2                                            BIT(25)
+#define TX_PKT_INT1                                            BIT(24)
+#define RX_PKT_INT3                                            BIT(19)
+#define RX_PKT_INT2                                            BIT(18)
+#define RX_PKT_INT1                                            BIT(17)
+#define RX_PKT_INT0                                            BIT(16)
+#define TX_PKT_INT                                             BIT(15)
+#define TXQ_TO_INT                                             BIT(14)
+#define GPHY_WAKEUP_INT                                        BIT(13)
+#define GPHY_LINK_DOWN_INT                                     BIT(12)
+#define GPHY_LINK_UP_INT                                       BIT(11)
+#define DMAW_TO_INT                                            BIT(10)
+#define DMAR_TO_INT                                             BIT(9)
+#define TXF_UR_INT                                              BIT(8)
+#define RFD3_UR_INT                                             BIT(7)
+#define RFD2_UR_INT                                             BIT(6)
+#define RFD1_UR_INT                                             BIT(5)
+#define RFD0_UR_INT                                             BIT(4)
+#define RXF_OF_INT                                              BIT(3)
+#define SW_MAN_INT                                              BIT(2)
+
+/* EMAC_MAILBOX_6 */
+#define RFD2_PROC_IDX_BMSK                                   0xfff0000
+#define RFD2_PROC_IDX_SHFT                                          16
+#define RFD2_PROD_IDX_BMSK                                       0xfff
+#define RFD2_PROD_IDX_SHFT                                           0
+
+/* EMAC_CORE_HW_VERSION */
+#define MAJOR_BMSK                                          0xf0000000
+#define MAJOR_SHFT                                                  28
+#define MINOR_BMSK                                           0xfff0000
+#define MINOR_SHFT                                                  16
+#define STEP_BMSK                                               0xffff
+#define STEP_SHFT                                                    0
+
+/* EMAC_EMAC_WRAPPER_CSR1 */
+#define TX_INDX_FIFO_SYNC_RST                                  BIT(23)
+#define TX_TS_FIFO_SYNC_RST                                    BIT(22)
+#define RX_TS_FIFO2_SYNC_RST                                   BIT(21)
+#define RX_TS_FIFO1_SYNC_RST                                   BIT(20)
+#define TX_TS_ENABLE                                           BIT(16)
+#define DIS_1588_CLKS                                          BIT(11)
+#define FREQ_MODE                                               BIT(9)
+#define ENABLE_RRD_TIMESTAMP                                    BIT(3)
+
+/* EMAC_EMAC_WRAPPER_CSR2 */
+#define HDRIVE_BMSK                                             0x3000
+#define HDRIVE_SHFT                                                 12
+#define SLB_EN                                                  BIT(9)
+#define PLB_EN                                                  BIT(8)
+#define WOL_EN                                                  BIT(3)
+#define PHY_RESET                                               BIT(0)
+
+#define EMAC_DEV_ID                                             0x0040
+
+/* SGMII v2 per lane registers */
+#define SGMII_LN_RSM_START             0x029C
+
+/* SGMII v2 PHY common registers */
+#define SGMII_PHY_CMN_CTRL            0x0408
+#define SGMII_PHY_CMN_RESET_CTRL      0x0410
+
+/* SGMII v2 PHY registers per lane */
+#define SGMII_PHY_LN_OFFSET          0x0400
+#define SGMII_PHY_LN_LANE_STATUS     0x00DC
+#define SGMII_PHY_LN_BIST_GEN0       0x008C
+#define SGMII_PHY_LN_BIST_GEN1       0x0090
+#define SGMII_PHY_LN_BIST_GEN2       0x0094
+#define SGMII_PHY_LN_BIST_GEN3       0x0098
+#define SGMII_PHY_LN_CDR_CTRL1       0x005C
+
+enum emac_clk_id {
+	EMAC_CLK_AXI,
+	EMAC_CLK_CFG_AHB,
+	EMAC_CLK_HIGH_SPEED,
+	EMAC_CLK_MDIO,
+	EMAC_CLK_TX,
+	EMAC_CLK_RX,
+	EMAC_CLK_SYS,
+	EMAC_CLK_CNT
+};
+
+#define EMAC_LINK_SPEED_UNKNOWN                                    0x0
+#define EMAC_LINK_SPEED_10_HALF                                 BIT(0)
+#define EMAC_LINK_SPEED_10_FULL                                 BIT(1)
+#define EMAC_LINK_SPEED_100_HALF                                BIT(2)
+#define EMAC_LINK_SPEED_100_FULL                                BIT(3)
+#define EMAC_LINK_SPEED_1GB_FULL                                BIT(5)
+
+#define EMAC_MAX_SETUP_LNK_CYCLE                                   100
+
+/* Wake On Lan */
+#define EMAC_WOL_PHY                     0x00000001 /* PHY Status Change */
+#define EMAC_WOL_MAGIC                   0x00000002 /* Magic Packet */
+
+struct emac_stats {
+	/* rx */
+	u64 rx_ok;              /* good packets */
+	u64 rx_bcast;           /* good broadcast packets */
+	u64 rx_mcast;           /* good multicast packets */
+	u64 rx_pause;           /* pause packet */
+	u64 rx_ctrl;            /* control packets other than pause frame. */
+	u64 rx_fcs_err;         /* packets with bad FCS. */
+	u64 rx_len_err;         /* packets with length mismatch */
+	u64 rx_byte_cnt;        /* good bytes count (without FCS) */
+	u64 rx_runt;            /* runt packets */
+	u64 rx_frag;            /* fragment count */
+	u64 rx_sz_64;	        /* packets that are 64 bytes */
+	u64 rx_sz_65_127;       /* packets that are 65-127 bytes */
+	u64 rx_sz_128_255;      /* packets that are 128-255 bytes */
+	u64 rx_sz_256_511;      /* packets that are 256-511 bytes */
+	u64 rx_sz_512_1023;     /* packets that are 512-1023 bytes */
+	u64 rx_sz_1024_1518;    /* packets that are 1024-1518 bytes */
+	u64 rx_sz_1519_max;     /* packets that are 1519-MTU bytes*/
+	u64 rx_sz_ov;           /* packets that are >MTU bytes (truncated) */
+	u64 rx_rxf_ov;          /* packets dropped due to RX FIFO overflow */
+	u64 rx_align_err;       /* alignment errors */
+	u64 rx_bcast_byte_cnt;  /* broadcast packets byte count (without FCS) */
+	u64 rx_mcast_byte_cnt;  /* multicast packets byte count (without FCS) */
+	u64 rx_err_addr;        /* packets dropped due to address filtering */
+	u64 rx_crc_align;       /* CRC align errors */
+	u64 rx_jabbers;         /* jabbers */
+
+	/* tx */
+	u64 tx_ok;              /* good packets */
+	u64 tx_bcast;           /* good broadcast packets */
+	u64 tx_mcast;           /* good multicast packets */
+	u64 tx_pause;           /* pause packets */
+	u64 tx_exc_defer;       /* packets with excessive deferral */
+	u64 tx_ctrl;            /* control packets other than pause frame */
+	u64 tx_defer;           /* packets that are deferred. */
+	u64 tx_byte_cnt;        /* good bytes count (without FCS) */
+	u64 tx_sz_64;           /* packets that are 64 bytes */
+	u64 tx_sz_65_127;       /* packets that are 65-127 bytes */
+	u64 tx_sz_128_255;      /* packets that are 128-255 bytes */
+	u64 tx_sz_256_511;      /* packets that are 256-511 bytes */
+	u64 tx_sz_512_1023;     /* packets that are 512-1023 bytes */
+	u64 tx_sz_1024_1518;    /* packets that are 1024-1518 bytes */
+	u64 tx_sz_1519_max;     /* packets that are 1519-MTU bytes */
+	u64 tx_1_col;           /* packets single prior collision */
+	u64 tx_2_col;           /* packets with multiple prior collisions */
+	u64 tx_late_col;        /* packets with late collisions */
+	u64 tx_abort_col;       /* packets aborted due to excess collisions */
+	u64 tx_underrun;        /* packets aborted due to FIFO underrun */
+	u64 tx_rd_eop;          /* count of reads beyond EOP */
+	u64 tx_len_err;         /* packets with length mismatch */
+	u64 tx_trunc;           /* packets truncated due to size >MTU */
+	u64 tx_bcast_byte;      /* broadcast packets byte count (without FCS) */
+	u64 tx_mcast_byte;      /* multicast packets byte count (without FCS) */
+	u64 tx_col;             /* collisions */
+
+	spinlock_t lock;	/* prevent multiple simultaneous readers */
+};
+
+/* RSS hstype Definitions */
+#define EMAC_RSS_HSTYP_IPV4_EN				    0x00000001
+#define EMAC_RSS_HSTYP_TCP4_EN				    0x00000002
+#define EMAC_RSS_HSTYP_IPV6_EN				    0x00000004
+#define EMAC_RSS_HSTYP_TCP6_EN				    0x00000008
+#define EMAC_RSS_HSTYP_ALL_EN (\
+		EMAC_RSS_HSTYP_IPV4_EN   |\
+		EMAC_RSS_HSTYP_TCP4_EN   |\
+		EMAC_RSS_HSTYP_IPV6_EN   |\
+		EMAC_RSS_HSTYP_TCP6_EN)
+
+#define EMAC_VLAN_TO_TAG(_vlan, _tag) \
+		(_tag =  ((((_vlan) >> 8) & 0xFF) | (((_vlan) & 0xFF) << 8)))
+
+#define EMAC_TAG_TO_VLAN(_tag, _vlan) \
+		(_vlan = ((((_tag) >> 8) & 0xFF) | (((_tag) & 0xFF) << 8)))
+
+#define EMAC_DEF_RX_BUF_SIZE					  1536
+#define EMAC_MAX_JUMBO_PKT_SIZE				    (9 * 1024)
+#define EMAC_MAX_TX_OFFLOAD_THRESH			    (9 * 1024)
+
+#define EMAC_MAX_ETH_FRAME_SIZE		       EMAC_MAX_JUMBO_PKT_SIZE
+#define EMAC_MIN_ETH_FRAME_SIZE					    68
+
+#define EMAC_DEF_TX_QUEUES					     1
+#define EMAC_DEF_RX_QUEUES					     1
+
+#define EMAC_MIN_TX_DESCS					   128
+#define EMAC_MIN_RX_DESCS					   128
+
+#define EMAC_MAX_TX_DESCS					 16383
+#define EMAC_MAX_RX_DESCS					  2047
+
+#define EMAC_DEF_TX_DESCS					   512
+#define EMAC_DEF_RX_DESCS					   256
+
+#define EMAC_DEF_RX_IRQ_MOD					   250
+#define EMAC_DEF_TX_IRQ_MOD					   250
+
+#define EMAC_WATCHDOG_TIME				      (5 * HZ)
+
+/* by default check link every 4 seconds */
+#define EMAC_TRY_LINK_TIMEOUT				      (4 * HZ)
+
+/* emac_irq per-device (per-adapter) irq properties.
+ * @irq:	irq number.
+ * @mask	mask to use over status register.
+ */
+struct emac_irq {
+	unsigned int	irq;
+	u32		mask;
+};
+
+/* The device's main data structure */
+struct emac_adapter {
+	struct net_device		*netdev;
+	struct mii_bus			*mii_bus;
+	struct phy_device		*phydev;
+
+	void __iomem			*base;
+	void __iomem			*csr;
+
+	struct emac_phy			phy;
+	struct emac_stats		stats;
+
+	struct emac_irq			irq;
+	struct clk			*clk[EMAC_CLK_CNT];
+
+	/* All Descriptor memory */
+	struct emac_ring_header		ring_header;
+	struct emac_tx_queue		tx_q;
+	struct emac_rx_queue		rx_q;
+	unsigned int			tx_desc_cnt;
+	unsigned int			rx_desc_cnt;
+	unsigned int			rrd_size; /* in quad words */
+	unsigned int			rfd_size; /* in quad words */
+	unsigned int			tpd_size; /* in quad words */
+
+	unsigned int			rxbuf_size;
+
+	/* Ring parameter */
+	u8				tpd_burst;
+	u8				rfd_burst;
+	unsigned int			dmaw_dly_cnt;
+	unsigned int			dmar_dly_cnt;
+	enum emac_dma_req_block		dmar_block;
+	enum emac_dma_req_block		dmaw_block;
+	enum emac_dma_order		dma_order;
+
+	u32				irq_mod;
+	u32				preamble;
+
+	struct work_struct		work_thread;
+
+	u16				msg_enable;
+
+	struct mutex			reset_lock;
+};
+
+int emac_reinit_locked(struct emac_adapter *adpt);
+void emac_reg_update32(void __iomem *addr, u32 mask, u32 val);
+irqreturn_t emac_isr(int irq, void *data);
+
+#endif /* _EMAC_H_ */
diff --git a/drivers/net/ethernet/rdc/r6040.c b/drivers/net/ethernet/rdc/r6040.c
index cb29ee24cf1b..5ef5d728c250 100644
--- a/drivers/net/ethernet/rdc/r6040.c
+++ b/drivers/net/ethernet/rdc/r6040.c
@@ -1062,14 +1062,12 @@ static int r6040_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	/* this should always be supported */
 	err = pci_set_dma_mask(pdev, DMA_BIT_MASK(32));
 	if (err) {
-		dev_err(&pdev->dev, "32-bit PCI DMA addresses"
-				"not supported by the card\n");
+		dev_err(&pdev->dev, "32-bit PCI DMA addresses not supported by the card\n");
 		goto err_out_disable_dev;
 	}
 	err = pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(32));
 	if (err) {
-		dev_err(&pdev->dev, "32-bit PCI DMA addresses"
-				"not supported by the card\n");
+		dev_err(&pdev->dev, "32-bit PCI DMA addresses not supported by the card\n");
 		goto err_out_disable_dev;
 	}
 
diff --git a/drivers/net/ethernet/renesas/Kconfig b/drivers/net/ethernet/renesas/Kconfig
index 4f132cf177cd..85ec447c2d18 100644
--- a/drivers/net/ethernet/renesas/Kconfig
+++ b/drivers/net/ethernet/renesas/Kconfig
@@ -27,7 +27,7 @@ config SH_ETH
 	  Renesas SuperH Ethernet device driver.
 	  This driver supporting CPUs are:
 		- SH7619, SH7710, SH7712, SH7724, SH7734, SH7763, SH7757,
-		  R8A7740, R8A777x and R8A779x.
+		  R8A7740, R8A774x, R8A777x and R8A779x.
 
 config RAVB
 	tristate "Renesas Ethernet AVB support"
diff --git a/drivers/net/ethernet/renesas/ravb.h b/drivers/net/ethernet/renesas/ravb.h
index 4e5d5e953e15..f1109661a533 100644
--- a/drivers/net/ethernet/renesas/ravb.h
+++ b/drivers/net/ethernet/renesas/ravb.h
@@ -1011,7 +1011,6 @@ struct ravb_private {
 	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;
diff --git a/drivers/net/ethernet/renesas/ravb_main.c b/drivers/net/ethernet/renesas/ravb_main.c
index 1e1cc0fad17f..630536bc72f9 100644
--- a/drivers/net/ethernet/renesas/ravb_main.c
+++ b/drivers/net/ethernet/renesas/ravb_main.c
@@ -942,7 +942,7 @@ out:
 static void ravb_adjust_link(struct net_device *ndev)
 {
 	struct ravb_private *priv = netdev_priv(ndev);
-	struct phy_device *phydev = priv->phydev;
+	struct phy_device *phydev = ndev->phydev;
 	bool new_state = false;
 
 	if (phydev->link) {
@@ -1032,48 +1032,47 @@ static int ravb_phy_init(struct net_device *ndev)
 
 	phy_attached_info(phydev);
 
-	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);
+	phy_start(ndev->phydev);
 
 	return 0;
 }
 
-static int ravb_get_settings(struct net_device *ndev, struct ethtool_cmd *ecmd)
+static int ravb_get_link_ksettings(struct net_device *ndev,
+				   struct ethtool_link_ksettings *cmd)
 {
 	struct ravb_private *priv = netdev_priv(ndev);
 	int error = -ENODEV;
 	unsigned long flags;
 
-	if (priv->phydev) {
+	if (ndev->phydev) {
 		spin_lock_irqsave(&priv->lock, flags);
-		error = phy_ethtool_gset(priv->phydev, ecmd);
+		error = phy_ethtool_ksettings_get(ndev->phydev, cmd);
 		spin_unlock_irqrestore(&priv->lock, flags);
 	}
 
 	return error;
 }
 
-static int ravb_set_settings(struct net_device *ndev, struct ethtool_cmd *ecmd)
+static int ravb_set_link_ksettings(struct net_device *ndev,
+				   const struct ethtool_link_ksettings *cmd)
 {
 	struct ravb_private *priv = netdev_priv(ndev);
 	unsigned long flags;
 	int error;
 
-	if (!priv->phydev)
+	if (!ndev->phydev)
 		return -ENODEV;
 
 	spin_lock_irqsave(&priv->lock, flags);
@@ -1081,11 +1080,11 @@ static int ravb_set_settings(struct net_device *ndev, struct ethtool_cmd *ecmd)
 	/* Disable TX and RX */
 	ravb_rcv_snd_disable(ndev);
 
-	error = phy_ethtool_sset(priv->phydev, ecmd);
+	error = phy_ethtool_ksettings_set(ndev->phydev, cmd);
 	if (error)
 		goto error_exit;
 
-	if (ecmd->duplex == DUPLEX_FULL)
+	if (cmd->base.duplex == DUPLEX_FULL)
 		priv->duplex = 1;
 	else
 		priv->duplex = 0;
@@ -1110,9 +1109,9 @@ static int ravb_nway_reset(struct net_device *ndev)
 	int error = -ENODEV;
 	unsigned long flags;
 
-	if (priv->phydev) {
+	if (ndev->phydev) {
 		spin_lock_irqsave(&priv->lock, flags);
-		error = phy_start_aneg(priv->phydev);
+		error = phy_start_aneg(ndev->phydev);
 		spin_unlock_irqrestore(&priv->lock, flags);
 	}
 
@@ -1309,8 +1308,6 @@ static int ravb_get_ts_info(struct net_device *ndev,
 }
 
 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,
@@ -1321,6 +1318,8 @@ static const struct ethtool_ops ravb_ethtool_ops = {
 	.get_ringparam		= ravb_get_ringparam,
 	.set_ringparam		= ravb_set_ringparam,
 	.get_ts_info		= ravb_get_ts_info,
+	.get_link_ksettings	= ravb_get_link_ksettings,
+	.set_link_ksettings	= ravb_set_link_ksettings,
 };
 
 static inline int ravb_hook_irq(unsigned int irq, irq_handler_t handler,
@@ -1661,10 +1660,9 @@ static int ravb_close(struct net_device *ndev)
 	}
 
 	/* PHY disconnect */
-	if (priv->phydev) {
-		phy_stop(priv->phydev);
-		phy_disconnect(priv->phydev);
-		priv->phydev = NULL;
+	if (ndev->phydev) {
+		phy_stop(ndev->phydev);
+		phy_disconnect(ndev->phydev);
 	}
 
 	if (priv->chip_id != RCAR_GEN2) {
@@ -1753,8 +1751,7 @@ static int ravb_hwtstamp_set(struct net_device *ndev, struct ifreq *req)
 /* 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;
+	struct phy_device *phydev = ndev->phydev;
 
 	if (!netif_running(ndev))
 		return -EINVAL;
@@ -1876,6 +1873,20 @@ static int ravb_set_gti(struct net_device *ndev)
 	return 0;
 }
 
+static void ravb_set_config_mode(struct net_device *ndev)
+{
+	struct ravb_private *priv = netdev_priv(ndev);
+
+	if (priv->chip_id == RCAR_GEN2) {
+		ravb_modify(ndev, CCC, CCC_OPC, CCC_OPC_CONFIG);
+		/* Set CSEL value */
+		ravb_modify(ndev, CCC, CCC_CSEL, CCC_CSEL_HPB);
+	} else {
+		ravb_modify(ndev, CCC, CCC_OPC, CCC_OPC_CONFIG |
+			    CCC_GAC | CCC_CSEL_HPB);
+	}
+}
+
 static int ravb_probe(struct platform_device *pdev)
 {
 	struct device_node *np = pdev->dev.of_node;
@@ -1978,14 +1989,7 @@ static int ravb_probe(struct platform_device *pdev)
 	ndev->ethtool_ops = &ravb_ethtool_ops;
 
 	/* Set AVB config mode */
-	if (chip_id == RCAR_GEN2) {
-		ravb_modify(ndev, CCC, CCC_OPC, CCC_OPC_CONFIG);
-		/* Set CSEL value */
-		ravb_modify(ndev, CCC, CCC_CSEL, CCC_CSEL_HPB);
-	} else {
-		ravb_modify(ndev, CCC, CCC_OPC, CCC_OPC_CONFIG |
-			    CCC_GAC | CCC_CSEL_HPB);
-	}
+	ravb_set_config_mode(ndev);
 
 	/* Set GTI value */
 	error = ravb_set_gti(ndev);
@@ -2096,8 +2100,55 @@ static int ravb_remove(struct platform_device *pdev)
 	return 0;
 }
 
-#ifdef CONFIG_PM
-static int ravb_runtime_nop(struct device *dev)
+static int __maybe_unused ravb_suspend(struct device *dev)
+{
+	struct net_device *ndev = dev_get_drvdata(dev);
+	int ret = 0;
+
+	if (netif_running(ndev)) {
+		netif_device_detach(ndev);
+		ret = ravb_close(ndev);
+	}
+
+	return ret;
+}
+
+static int __maybe_unused ravb_resume(struct device *dev)
+{
+	struct net_device *ndev = dev_get_drvdata(dev);
+	struct ravb_private *priv = netdev_priv(ndev);
+	int ret = 0;
+
+	/* All register have been reset to default values.
+	 * Restore all registers which where setup at probe time and
+	 * reopen device if it was running before system suspended.
+	 */
+
+	/* Set AVB config mode */
+	ravb_set_config_mode(ndev);
+
+	/* Set GTI value */
+	ret = ravb_set_gti(ndev);
+	if (ret)
+		return ret;
+
+	/* Request GTI loading */
+	ravb_modify(ndev, GCCR, GCCR_LTI, GCCR_LTI);
+
+	/* Restore descriptor base address table */
+	ravb_write(ndev, priv->desc_bat_dma, DBAT);
+
+	if (netif_running(ndev)) {
+		ret = ravb_open(ndev);
+		if (ret < 0)
+			return ret;
+		netif_device_attach(ndev);
+	}
+
+	return ret;
+}
+
+static int __maybe_unused ravb_runtime_nop(struct device *dev)
 {
 	/* Runtime PM callback shared between ->runtime_suspend()
 	 * and ->runtime_resume(). Simply returns success.
@@ -2110,20 +2161,16 @@ static int ravb_runtime_nop(struct device *dev)
 }
 
 static const struct dev_pm_ops ravb_dev_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(ravb_suspend, ravb_resume)
 	SET_RUNTIME_PM_OPS(ravb_runtime_nop, ravb_runtime_nop, NULL)
 };
 
-#define RAVB_PM_OPS (&ravb_dev_pm_ops)
-#else
-#define RAVB_PM_OPS NULL
-#endif
-
 static struct platform_driver ravb_driver = {
 	.probe		= ravb_probe,
 	.remove		= ravb_remove,
 	.driver = {
 		.name	= "ravb",
-		.pm	= RAVB_PM_OPS,
+		.pm	= &ravb_dev_pm_ops,
 		.of_match_table = ravb_match_table,
 	},
 };
diff --git a/drivers/net/ethernet/renesas/sh_eth.c b/drivers/net/ethernet/renesas/sh_eth.c
index 054e795df90f..05b0dc55de77 100644
--- a/drivers/net/ethernet/renesas/sh_eth.c
+++ b/drivers/net/ethernet/renesas/sh_eth.c
@@ -1728,7 +1728,7 @@ out:
 static void sh_eth_adjust_link(struct net_device *ndev)
 {
 	struct sh_eth_private *mdp = netdev_priv(ndev);
-	struct phy_device *phydev = mdp->phydev;
+	struct phy_device *phydev = ndev->phydev;
 	int new_state = 0;
 
 	if (phydev->link) {
@@ -1805,51 +1805,48 @@ static int sh_eth_phy_init(struct net_device *ndev)
 
 	phy_attached_info(phydev);
 
-	mdp->phydev = phydev;
-
 	return 0;
 }
 
 /* PHY control start function */
 static int sh_eth_phy_start(struct net_device *ndev)
 {
-	struct sh_eth_private *mdp = netdev_priv(ndev);
 	int ret;
 
 	ret = sh_eth_phy_init(ndev);
 	if (ret)
 		return ret;
 
-	phy_start(mdp->phydev);
+	phy_start(ndev->phydev);
 
 	return 0;
 }
 
-static int sh_eth_get_settings(struct net_device *ndev,
-			       struct ethtool_cmd *ecmd)
+static int sh_eth_get_link_ksettings(struct net_device *ndev,
+				     struct ethtool_link_ksettings *cmd)
 {
 	struct sh_eth_private *mdp = netdev_priv(ndev);
 	unsigned long flags;
 	int ret;
 
-	if (!mdp->phydev)
+	if (!ndev->phydev)
 		return -ENODEV;
 
 	spin_lock_irqsave(&mdp->lock, flags);
-	ret = phy_ethtool_gset(mdp->phydev, ecmd);
+	ret = phy_ethtool_ksettings_get(ndev->phydev, cmd);
 	spin_unlock_irqrestore(&mdp->lock, flags);
 
 	return ret;
 }
 
-static int sh_eth_set_settings(struct net_device *ndev,
-			       struct ethtool_cmd *ecmd)
+static int sh_eth_set_link_ksettings(struct net_device *ndev,
+				     const struct ethtool_link_ksettings *cmd)
 {
 	struct sh_eth_private *mdp = netdev_priv(ndev);
 	unsigned long flags;
 	int ret;
 
-	if (!mdp->phydev)
+	if (!ndev->phydev)
 		return -ENODEV;
 
 	spin_lock_irqsave(&mdp->lock, flags);
@@ -1857,11 +1854,11 @@ static int sh_eth_set_settings(struct net_device *ndev,
 	/* disable tx and rx */
 	sh_eth_rcv_snd_disable(ndev);
 
-	ret = phy_ethtool_sset(mdp->phydev, ecmd);
+	ret = phy_ethtool_ksettings_set(ndev->phydev, cmd);
 	if (ret)
 		goto error_exit;
 
-	if (ecmd->duplex == DUPLEX_FULL)
+	if (cmd->base.duplex == DUPLEX_FULL)
 		mdp->duplex = 1;
 	else
 		mdp->duplex = 0;
@@ -2072,11 +2069,11 @@ static int sh_eth_nway_reset(struct net_device *ndev)
 	unsigned long flags;
 	int ret;
 
-	if (!mdp->phydev)
+	if (!ndev->phydev)
 		return -ENODEV;
 
 	spin_lock_irqsave(&mdp->lock, flags);
-	ret = phy_start_aneg(mdp->phydev);
+	ret = phy_start_aneg(ndev->phydev);
 	spin_unlock_irqrestore(&mdp->lock, flags);
 
 	return ret;
@@ -2203,8 +2200,6 @@ static int sh_eth_set_ringparam(struct net_device *ndev,
 }
 
 static const struct ethtool_ops sh_eth_ethtool_ops = {
-	.get_settings	= sh_eth_get_settings,
-	.set_settings	= sh_eth_set_settings,
 	.get_regs_len	= sh_eth_get_regs_len,
 	.get_regs	= sh_eth_get_regs,
 	.nway_reset	= sh_eth_nway_reset,
@@ -2216,6 +2211,8 @@ static const struct ethtool_ops sh_eth_ethtool_ops = {
 	.get_sset_count     = sh_eth_get_sset_count,
 	.get_ringparam	= sh_eth_get_ringparam,
 	.set_ringparam	= sh_eth_set_ringparam,
+	.get_link_ksettings = sh_eth_get_link_ksettings,
+	.set_link_ksettings = sh_eth_set_link_ksettings,
 };
 
 /* network device open function */
@@ -2413,10 +2410,9 @@ static int sh_eth_close(struct net_device *ndev)
 	sh_eth_dev_exit(ndev);
 
 	/* PHY Disconnect */
-	if (mdp->phydev) {
-		phy_stop(mdp->phydev);
-		phy_disconnect(mdp->phydev);
-		mdp->phydev = NULL;
+	if (ndev->phydev) {
+		phy_stop(ndev->phydev);
+		phy_disconnect(ndev->phydev);
 	}
 
 	free_irq(ndev->irq, ndev);
@@ -2434,8 +2430,7 @@ static int sh_eth_close(struct net_device *ndev)
 /* ioctl to device function */
 static int sh_eth_do_ioctl(struct net_device *ndev, struct ifreq *rq, int cmd)
 {
-	struct sh_eth_private *mdp = netdev_priv(ndev);
-	struct phy_device *phydev = mdp->phydev;
+	struct phy_device *phydev = ndev->phydev;
 
 	if (!netif_running(ndev))
 		return -EINVAL;
@@ -2964,6 +2959,8 @@ static struct sh_eth_plat_data *sh_eth_parse_dt(struct device *dev)
 
 static const struct of_device_id sh_eth_match_table[] = {
 	{ .compatible = "renesas,gether-r8a7740", .data = &r8a7740_data },
+	{ .compatible = "renesas,ether-r8a7743", .data = &r8a779x_data },
+	{ .compatible = "renesas,ether-r8a7745", .data = &r8a779x_data },
 	{ .compatible = "renesas,ether-r8a7778", .data = &r8a777x_data },
 	{ .compatible = "renesas,ether-r8a7779", .data = &r8a777x_data },
 	{ .compatible = "renesas,ether-r8a7790", .data = &r8a779x_data },
diff --git a/drivers/net/ethernet/renesas/sh_eth.h b/drivers/net/ethernet/renesas/sh_eth.h
index c62380e34a1d..d050f37f3e0f 100644
--- a/drivers/net/ethernet/renesas/sh_eth.h
+++ b/drivers/net/ethernet/renesas/sh_eth.h
@@ -518,7 +518,6 @@ struct sh_eth_private {
 	/* MII transceiver section. */
 	u32 phy_id;			/* PHY ID */
 	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;
diff --git a/drivers/net/ethernet/rocker/rocker.h b/drivers/net/ethernet/rocker/rocker.h
index 1ab995f7146b..2eb9b49569d5 100644
--- a/drivers/net/ethernet/rocker/rocker.h
+++ b/drivers/net/ethernet/rocker/rocker.h
@@ -15,6 +15,7 @@
 #include <linux/kernel.h>
 #include <linux/types.h>
 #include <linux/netdevice.h>
+#include <linux/notifier.h>
 #include <net/neighbour.h>
 #include <net/switchdev.h>
 
@@ -52,6 +53,9 @@ struct rocker_port {
 	struct rocker_dma_ring_info rx_ring;
 };
 
+struct rocker_port *rocker_port_dev_lower_find(struct net_device *dev,
+					       struct rocker *rocker);
+
 struct rocker_world_ops;
 
 struct rocker {
@@ -66,6 +70,7 @@ struct rocker {
 	spinlock_t cmd_ring_lock;		/* for cmd ring accesses */
 	struct rocker_dma_ring_info cmd_ring;
 	struct rocker_dma_ring_info event_ring;
+	struct notifier_block fib_nb;
 	struct rocker_world_ops *wops;
 	void *wpriv;
 };
@@ -117,11 +122,6 @@ struct rocker_world_ops {
 	int (*port_obj_vlan_dump)(const struct rocker_port *rocker_port,
 				  struct switchdev_obj_port_vlan *vlan,
 				  switchdev_obj_dump_cb_t *cb);
-	int (*port_obj_fib4_add)(struct rocker_port *rocker_port,
-				 const struct switchdev_obj_ipv4_fib *fib4,
-				 struct switchdev_trans *trans);
-	int (*port_obj_fib4_del)(struct rocker_port *rocker_port,
-				 const struct switchdev_obj_ipv4_fib *fib4);
 	int (*port_obj_fdb_add)(struct rocker_port *rocker_port,
 				const struct switchdev_obj_port_fdb *fdb,
 				struct switchdev_trans *trans);
@@ -141,6 +141,11 @@ struct rocker_world_ops {
 	int (*port_ev_mac_vlan_seen)(struct rocker_port *rocker_port,
 				     const unsigned char *addr,
 				     __be16 vlan_id);
+	int (*fib4_add)(struct rocker *rocker,
+			const struct fib_entry_notifier_info *fen_info);
+	int (*fib4_del)(struct rocker *rocker,
+			const struct fib_entry_notifier_info *fen_info);
+	void (*fib4_abort)(struct rocker *rocker);
 };
 
 extern struct rocker_world_ops rocker_ofdpa_ops;
diff --git a/drivers/net/ethernet/rocker/rocker_main.c b/drivers/net/ethernet/rocker/rocker_main.c
index f0b09b05ed3f..5424fb341613 100644
--- a/drivers/net/ethernet/rocker/rocker_main.c
+++ b/drivers/net/ethernet/rocker/rocker_main.c
@@ -1625,29 +1625,6 @@ rocker_world_port_obj_vlan_dump(const struct rocker_port *rocker_port,
 }
 
 static int
-rocker_world_port_obj_fib4_add(struct rocker_port *rocker_port,
-			       const struct switchdev_obj_ipv4_fib *fib4,
-			       struct switchdev_trans *trans)
-{
-	struct rocker_world_ops *wops = rocker_port->rocker->wops;
-
-	if (!wops->port_obj_fib4_add)
-		return -EOPNOTSUPP;
-	return wops->port_obj_fib4_add(rocker_port, fib4, trans);
-}
-
-static int
-rocker_world_port_obj_fib4_del(struct rocker_port *rocker_port,
-			       const struct switchdev_obj_ipv4_fib *fib4)
-{
-	struct rocker_world_ops *wops = rocker_port->rocker->wops;
-
-	if (!wops->port_obj_fib4_del)
-		return -EOPNOTSUPP;
-	return wops->port_obj_fib4_del(rocker_port, fib4);
-}
-
-static int
 rocker_world_port_obj_fdb_add(struct rocker_port *rocker_port,
 			      const struct switchdev_obj_port_fdb *fdb,
 			      struct switchdev_trans *trans)
@@ -1733,6 +1710,34 @@ static int rocker_world_port_ev_mac_vlan_seen(struct rocker_port *rocker_port,
 	return wops->port_ev_mac_vlan_seen(rocker_port, addr, vlan_id);
 }
 
+static int rocker_world_fib4_add(struct rocker *rocker,
+				 const struct fib_entry_notifier_info *fen_info)
+{
+	struct rocker_world_ops *wops = rocker->wops;
+
+	if (!wops->fib4_add)
+		return 0;
+	return wops->fib4_add(rocker, fen_info);
+}
+
+static int rocker_world_fib4_del(struct rocker *rocker,
+				 const struct fib_entry_notifier_info *fen_info)
+{
+	struct rocker_world_ops *wops = rocker->wops;
+
+	if (!wops->fib4_del)
+		return 0;
+	return wops->fib4_del(rocker, fen_info);
+}
+
+static void rocker_world_fib4_abort(struct rocker *rocker)
+{
+	struct rocker_world_ops *wops = rocker->wops;
+
+	if (wops->fib4_abort)
+		wops->fib4_abort(rocker);
+}
+
 /*****************
  * Net device ops
  *****************/
@@ -2096,11 +2101,6 @@ static int rocker_port_obj_add(struct net_device *dev,
 						     SWITCHDEV_OBJ_PORT_VLAN(obj),
 						     trans);
 		break;
-	case SWITCHDEV_OBJ_ID_IPV4_FIB:
-		err = rocker_world_port_obj_fib4_add(rocker_port,
-						     SWITCHDEV_OBJ_IPV4_FIB(obj),
-						     trans);
-		break;
 	case SWITCHDEV_OBJ_ID_PORT_FDB:
 		err = rocker_world_port_obj_fdb_add(rocker_port,
 						    SWITCHDEV_OBJ_PORT_FDB(obj),
@@ -2125,10 +2125,6 @@ static int rocker_port_obj_del(struct net_device *dev,
 		err = rocker_world_port_obj_vlan_del(rocker_port,
 						     SWITCHDEV_OBJ_PORT_VLAN(obj));
 		break;
-	case SWITCHDEV_OBJ_ID_IPV4_FIB:
-		err = rocker_world_port_obj_fib4_del(rocker_port,
-						     SWITCHDEV_OBJ_IPV4_FIB(obj));
-		break;
 	case SWITCHDEV_OBJ_ID_PORT_FDB:
 		err = rocker_world_port_obj_fdb_del(rocker_port,
 						    SWITCHDEV_OBJ_PORT_FDB(obj));
@@ -2175,6 +2171,31 @@ static const struct switchdev_ops rocker_port_switchdev_ops = {
 	.switchdev_port_obj_dump	= rocker_port_obj_dump,
 };
 
+static int rocker_router_fib_event(struct notifier_block *nb,
+				   unsigned long event, void *ptr)
+{
+	struct rocker *rocker = container_of(nb, struct rocker, fib_nb);
+	struct fib_entry_notifier_info *fen_info = ptr;
+	int err;
+
+	switch (event) {
+	case FIB_EVENT_ENTRY_ADD:
+		err = rocker_world_fib4_add(rocker, fen_info);
+		if (err)
+			rocker_world_fib4_abort(rocker);
+		else
+		break;
+	case FIB_EVENT_ENTRY_DEL:
+		rocker_world_fib4_del(rocker, fen_info);
+		break;
+	case FIB_EVENT_RULE_ADD: /* fall through */
+	case FIB_EVENT_RULE_DEL:
+		rocker_world_fib4_abort(rocker);
+		break;
+	}
+	return NOTIFY_DONE;
+}
+
 /********************
  * ethtool interface
  ********************/
@@ -2412,7 +2433,7 @@ static int rocker_port_rx_proc(const struct rocker *rocker,
 	skb->protocol = eth_type_trans(skb, rocker_port->dev);
 
 	if (rx_flags & ROCKER_RX_FLAGS_FWD_OFFLOAD)
-		skb->offload_fwd_mark = rocker_port->dev->offload_fwd_mark;
+		skb->offload_fwd_mark = 1;
 
 	rocker_port->dev->stats.rx_packets++;
 	rocker_port->dev->stats.rx_bytes += skb->len;
@@ -2740,6 +2761,9 @@ static int rocker_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 		goto err_probe_ports;
 	}
 
+	rocker->fib_nb.notifier_call = rocker_router_fib_event;
+	register_fib_notifier(&rocker->fib_nb);
+
 	dev_info(&pdev->dev, "Rocker switch with id %*phN\n",
 		 (int)sizeof(rocker->hw.id), &rocker->hw.id);
 
@@ -2771,6 +2795,7 @@ static void rocker_remove(struct pci_dev *pdev)
 {
 	struct rocker *rocker = pci_get_drvdata(pdev);
 
+	unregister_fib_notifier(&rocker->fib_nb);
 	rocker_write32(rocker, CONTROL, ROCKER_CONTROL_RESET);
 	rocker_remove_ports(rocker);
 	free_irq(rocker_msix_vector(rocker, ROCKER_MSIX_VEC_EVENT), rocker);
@@ -2799,6 +2824,37 @@ static bool rocker_port_dev_check(const struct net_device *dev)
 	return dev->netdev_ops == &rocker_port_netdev_ops;
 }
 
+static bool rocker_port_dev_check_under(const struct net_device *dev,
+					struct rocker *rocker)
+{
+	struct rocker_port *rocker_port;
+
+	if (!rocker_port_dev_check(dev))
+		return false;
+
+	rocker_port = netdev_priv(dev);
+	if (rocker_port->rocker != rocker)
+		return false;
+
+	return true;
+}
+
+struct rocker_port *rocker_port_dev_lower_find(struct net_device *dev,
+					       struct rocker *rocker)
+{
+	struct net_device *lower_dev;
+	struct list_head *iter;
+
+	if (rocker_port_dev_check_under(dev, rocker))
+		return netdev_priv(dev);
+
+	netdev_for_each_all_lower_dev(dev, lower_dev, iter) {
+		if (rocker_port_dev_check_under(lower_dev, rocker))
+			return netdev_priv(lower_dev);
+	}
+	return NULL;
+}
+
 static int rocker_netdevice_event(struct notifier_block *unused,
 				  unsigned long event, void *ptr)
 {
diff --git a/drivers/net/ethernet/rocker/rocker_ofdpa.c b/drivers/net/ethernet/rocker/rocker_ofdpa.c
index 1ca796316173..431a60804272 100644
--- a/drivers/net/ethernet/rocker/rocker_ofdpa.c
+++ b/drivers/net/ethernet/rocker/rocker_ofdpa.c
@@ -99,6 +99,7 @@ struct ofdpa_flow_tbl_entry {
 	struct ofdpa_flow_tbl_key key;
 	size_t key_len;
 	u32 key_crc32; /* key */
+	struct fib_info *fi;
 };
 
 struct ofdpa_group_tbl_entry {
@@ -189,6 +190,7 @@ struct ofdpa {
 	spinlock_t neigh_tbl_lock;		/* for neigh tbl accesses */
 	u32 neigh_tbl_next_index;
 	unsigned long ageing_time;
+	bool fib_aborted;
 };
 
 struct ofdpa_port {
@@ -1043,7 +1045,8 @@ static int ofdpa_flow_tbl_ucast4_routing(struct ofdpa_port *ofdpa_port,
 					 __be16 eth_type, __be32 dst,
 					 __be32 dst_mask, u32 priority,
 					 enum rocker_of_dpa_table_id goto_tbl,
-					 u32 group_id, int flags)
+					 u32 group_id, struct fib_info *fi,
+					 int flags)
 {
 	struct ofdpa_flow_tbl_entry *entry;
 
@@ -1060,6 +1063,7 @@ static int ofdpa_flow_tbl_ucast4_routing(struct ofdpa_port *ofdpa_port,
 	entry->key.ucast_routing.group_id = group_id;
 	entry->key_len = offsetof(struct ofdpa_flow_tbl_key,
 				  ucast_routing.group_id);
+	entry->fi = fi;
 
 	return ofdpa_flow_tbl_do(ofdpa_port, trans, flags, entry);
 }
@@ -1425,7 +1429,7 @@ static int ofdpa_port_ipv4_neigh(struct ofdpa_port *ofdpa_port,
 						    eth_type, ip_addr,
 						    inet_make_mask(32),
 						    priority, goto_tbl,
-						    group_id, flags);
+						    group_id, NULL, flags);
 
 		if (err)
 			netdev_err(ofdpa_port->dev, "Error (%d) /32 unicast route %pI4 group 0x%08x\n",
@@ -2390,7 +2394,7 @@ found:
 
 static int ofdpa_port_fib_ipv4(struct ofdpa_port *ofdpa_port,
 			       struct switchdev_trans *trans, __be32 dst,
-			       int dst_len, const struct fib_info *fi,
+			       int dst_len, struct fib_info *fi,
 			       u32 tb_id, int flags)
 {
 	const struct fib_nh *nh;
@@ -2426,7 +2430,7 @@ static int ofdpa_port_fib_ipv4(struct ofdpa_port *ofdpa_port,
 
 	err = ofdpa_flow_tbl_ucast4_routing(ofdpa_port, trans, eth_type, dst,
 					    dst_mask, priority, goto_tbl,
-					    group_id, flags);
+					    group_id, fi, flags);
 	if (err)
 		netdev_err(ofdpa_port->dev, "Error (%d) IPv4 route %pI4\n",
 			   err, &dst);
@@ -2558,7 +2562,6 @@ static int ofdpa_port_init(struct rocker_port *rocker_port)
 	struct ofdpa_port *ofdpa_port = rocker_port->wpriv;
 	int err;
 
-	switchdev_port_fwd_mark_set(ofdpa_port->dev, NULL, false);
 	rocker_port_set_learning(rocker_port,
 				 !!(ofdpa_port->brport_flags & BR_LEARNING));
 
@@ -2719,28 +2722,6 @@ static int ofdpa_port_obj_vlan_dump(const struct rocker_port *rocker_port,
 	return err;
 }
 
-static int ofdpa_port_obj_fib4_add(struct rocker_port *rocker_port,
-				   const struct switchdev_obj_ipv4_fib *fib4,
-				   struct switchdev_trans *trans)
-{
-	struct ofdpa_port *ofdpa_port = rocker_port->wpriv;
-
-	return ofdpa_port_fib_ipv4(ofdpa_port, trans,
-				   htonl(fib4->dst), fib4->dst_len,
-				   fib4->fi, fib4->tb_id, 0);
-}
-
-static int ofdpa_port_obj_fib4_del(struct rocker_port *rocker_port,
-				   const struct switchdev_obj_ipv4_fib *fib4)
-{
-	struct ofdpa_port *ofdpa_port = rocker_port->wpriv;
-
-	return ofdpa_port_fib_ipv4(ofdpa_port, NULL,
-				   htonl(fib4->dst), fib4->dst_len,
-				   fib4->fi, fib4->tb_id,
-				   OFDPA_OP_FLAG_REMOVE);
-}
-
 static int ofdpa_port_obj_fdb_add(struct rocker_port *rocker_port,
 				  const struct switchdev_obj_port_fdb *fdb,
 				  struct switchdev_trans *trans)
@@ -2817,7 +2798,6 @@ static int ofdpa_port_bridge_join(struct ofdpa_port *ofdpa_port,
 		ofdpa_port_internal_vlan_id_get(ofdpa_port, bridge->ifindex);
 
 	ofdpa_port->bridge_dev = bridge;
-	switchdev_port_fwd_mark_set(ofdpa_port->dev, bridge, true);
 
 	return ofdpa_port_vlan_add(ofdpa_port, NULL, OFDPA_UNTAGGED_VID, 0);
 }
@@ -2836,8 +2816,6 @@ static int ofdpa_port_bridge_leave(struct ofdpa_port *ofdpa_port)
 		ofdpa_port_internal_vlan_id_get(ofdpa_port,
 						ofdpa_port->dev->ifindex);
 
-	switchdev_port_fwd_mark_set(ofdpa_port->dev, ofdpa_port->bridge_dev,
-				    false);
 	ofdpa_port->bridge_dev = NULL;
 
 	err = ofdpa_port_vlan_add(ofdpa_port, NULL, OFDPA_UNTAGGED_VID, 0);
@@ -2926,6 +2904,82 @@ static int ofdpa_port_ev_mac_vlan_seen(struct rocker_port *rocker_port,
 	return ofdpa_port_fdb(ofdpa_port, NULL, addr, vlan_id, flags);
 }
 
+static struct ofdpa_port *ofdpa_port_dev_lower_find(struct net_device *dev,
+						    struct rocker *rocker)
+{
+	struct rocker_port *rocker_port;
+
+	rocker_port = rocker_port_dev_lower_find(dev, rocker);
+	return rocker_port ? rocker_port->wpriv : NULL;
+}
+
+static int ofdpa_fib4_add(struct rocker *rocker,
+			  const struct fib_entry_notifier_info *fen_info)
+{
+	struct ofdpa *ofdpa = rocker->wpriv;
+	struct ofdpa_port *ofdpa_port;
+	int err;
+
+	if (ofdpa->fib_aborted)
+		return 0;
+	ofdpa_port = ofdpa_port_dev_lower_find(fen_info->fi->fib_dev, rocker);
+	if (!ofdpa_port)
+		return 0;
+	err = ofdpa_port_fib_ipv4(ofdpa_port, NULL, htonl(fen_info->dst),
+				  fen_info->dst_len, fen_info->fi,
+				  fen_info->tb_id, 0);
+	if (err)
+		return err;
+	fib_info_offload_inc(fen_info->fi);
+	return 0;
+}
+
+static int ofdpa_fib4_del(struct rocker *rocker,
+			  const struct fib_entry_notifier_info *fen_info)
+{
+	struct ofdpa *ofdpa = rocker->wpriv;
+	struct ofdpa_port *ofdpa_port;
+
+	if (ofdpa->fib_aborted)
+		return 0;
+	ofdpa_port = ofdpa_port_dev_lower_find(fen_info->fi->fib_dev, rocker);
+	if (!ofdpa_port)
+		return 0;
+	fib_info_offload_dec(fen_info->fi);
+	return ofdpa_port_fib_ipv4(ofdpa_port, NULL, htonl(fen_info->dst),
+				   fen_info->dst_len, fen_info->fi,
+				   fen_info->tb_id, OFDPA_OP_FLAG_REMOVE);
+}
+
+static void ofdpa_fib4_abort(struct rocker *rocker)
+{
+	struct ofdpa *ofdpa = rocker->wpriv;
+	struct ofdpa_port *ofdpa_port;
+	struct ofdpa_flow_tbl_entry *flow_entry;
+	struct hlist_node *tmp;
+	unsigned long flags;
+	int bkt;
+
+	if (ofdpa->fib_aborted)
+		return;
+
+	spin_lock_irqsave(&ofdpa->flow_tbl_lock, flags);
+	hash_for_each_safe(ofdpa->flow_tbl, bkt, tmp, flow_entry, entry) {
+		if (flow_entry->key.tbl_id !=
+		    ROCKER_OF_DPA_TABLE_ID_UNICAST_ROUTING)
+			continue;
+		ofdpa_port = ofdpa_port_dev_lower_find(flow_entry->fi->fib_dev,
+						       rocker);
+		if (!ofdpa_port)
+			continue;
+		fib_info_offload_dec(flow_entry->fi);
+		ofdpa_flow_tbl_del(ofdpa_port, NULL, OFDPA_OP_FLAG_REMOVE,
+				   flow_entry);
+	}
+	spin_unlock_irqrestore(&ofdpa->flow_tbl_lock, flags);
+	ofdpa->fib_aborted = true;
+}
+
 struct rocker_world_ops rocker_ofdpa_ops = {
 	.kind = "ofdpa",
 	.priv_size = sizeof(struct ofdpa),
@@ -2945,8 +2999,6 @@ struct rocker_world_ops rocker_ofdpa_ops = {
 	.port_obj_vlan_add = ofdpa_port_obj_vlan_add,
 	.port_obj_vlan_del = ofdpa_port_obj_vlan_del,
 	.port_obj_vlan_dump = ofdpa_port_obj_vlan_dump,
-	.port_obj_fib4_add = ofdpa_port_obj_fib4_add,
-	.port_obj_fib4_del = ofdpa_port_obj_fib4_del,
 	.port_obj_fdb_add = ofdpa_port_obj_fdb_add,
 	.port_obj_fdb_del = ofdpa_port_obj_fdb_del,
 	.port_obj_fdb_dump = ofdpa_port_obj_fdb_dump,
@@ -2955,4 +3007,7 @@ struct rocker_world_ops rocker_ofdpa_ops = {
 	.port_neigh_update = ofdpa_port_neigh_update,
 	.port_neigh_destroy = ofdpa_port_neigh_destroy,
 	.port_ev_mac_vlan_seen = ofdpa_port_ev_mac_vlan_seen,
+	.fib4_add = ofdpa_fib4_add,
+	.fib4_del = ofdpa_fib4_del,
+	.fib4_abort = ofdpa_fib4_abort,
 };
diff --git a/drivers/net/ethernet/sfc/ef10.c b/drivers/net/ethernet/sfc/ef10.c
index e00a669e9e09..00279da6a1e8 100644
--- a/drivers/net/ethernet/sfc/ef10.c
+++ b/drivers/net/ethernet/sfc/ef10.c
@@ -177,7 +177,7 @@ static int efx_ef10_get_vf_index(struct efx_nic *efx)
 
 static int efx_ef10_init_datapath_caps(struct efx_nic *efx)
 {
-	MCDI_DECLARE_BUF(outbuf, MC_CMD_GET_CAPABILITIES_OUT_LEN);
+	MCDI_DECLARE_BUF(outbuf, MC_CMD_GET_CAPABILITIES_V2_OUT_LEN);
 	struct efx_ef10_nic_data *nic_data = efx->nic_data;
 	size_t outlen;
 	int rc;
@@ -188,7 +188,7 @@ static int efx_ef10_init_datapath_caps(struct efx_nic *efx)
 			  outbuf, sizeof(outbuf), &outlen);
 	if (rc)
 		return rc;
-	if (outlen < sizeof(outbuf)) {
+	if (outlen < MC_CMD_GET_CAPABILITIES_OUT_LEN) {
 		netif_err(efx, drv, efx->net_dev,
 			  "unable to read datapath firmware capabilities\n");
 		return -EIO;
@@ -197,6 +197,12 @@ static int efx_ef10_init_datapath_caps(struct efx_nic *efx)
 	nic_data->datapath_caps =
 		MCDI_DWORD(outbuf, GET_CAPABILITIES_OUT_FLAGS1);
 
+	if (outlen >= MC_CMD_GET_CAPABILITIES_V2_OUT_LEN)
+		nic_data->datapath_caps2 = MCDI_DWORD(outbuf,
+				GET_CAPABILITIES_V2_OUT_FLAGS2);
+	else
+		nic_data->datapath_caps2 = 0;
+
 	/* record the DPCPU firmware IDs to determine VEB vswitching support.
 	 */
 	nic_data->rx_dpcpu_fw_id =
@@ -227,6 +233,116 @@ static int efx_ef10_get_sysclk_freq(struct efx_nic *efx)
 	return rc > 0 ? rc : -ERANGE;
 }
 
+static int efx_ef10_get_timer_workarounds(struct efx_nic *efx)
+{
+	struct efx_ef10_nic_data *nic_data = efx->nic_data;
+	unsigned int implemented;
+	unsigned int enabled;
+	int rc;
+
+	nic_data->workaround_35388 = false;
+	nic_data->workaround_61265 = false;
+
+	rc = efx_mcdi_get_workarounds(efx, &implemented, &enabled);
+
+	if (rc == -ENOSYS) {
+		/* Firmware without GET_WORKAROUNDS - not a problem. */
+		rc = 0;
+	} else if (rc == 0) {
+		/* Bug61265 workaround is always enabled if implemented. */
+		if (enabled & MC_CMD_GET_WORKAROUNDS_OUT_BUG61265)
+			nic_data->workaround_61265 = true;
+
+		if (enabled & MC_CMD_GET_WORKAROUNDS_OUT_BUG35388) {
+			nic_data->workaround_35388 = true;
+		} else if (implemented & MC_CMD_GET_WORKAROUNDS_OUT_BUG35388) {
+			/* Workaround is implemented but not enabled.
+			 * Try to enable it.
+			 */
+			rc = efx_mcdi_set_workaround(efx,
+						     MC_CMD_WORKAROUND_BUG35388,
+						     true, NULL);
+			if (rc == 0)
+				nic_data->workaround_35388 = true;
+			/* If we failed to set the workaround just carry on. */
+			rc = 0;
+		}
+	}
+
+	netif_dbg(efx, probe, efx->net_dev,
+		  "workaround for bug 35388 is %sabled\n",
+		  nic_data->workaround_35388 ? "en" : "dis");
+	netif_dbg(efx, probe, efx->net_dev,
+		  "workaround for bug 61265 is %sabled\n",
+		  nic_data->workaround_61265 ? "en" : "dis");
+
+	return rc;
+}
+
+static void efx_ef10_process_timer_config(struct efx_nic *efx,
+					  const efx_dword_t *data)
+{
+	unsigned int max_count;
+
+	if (EFX_EF10_WORKAROUND_61265(efx)) {
+		efx->timer_quantum_ns = MCDI_DWORD(data,
+			GET_EVQ_TMR_PROPERTIES_OUT_MCDI_TMR_STEP_NS);
+		efx->timer_max_ns = MCDI_DWORD(data,
+			GET_EVQ_TMR_PROPERTIES_OUT_MCDI_TMR_MAX_NS);
+	} else if (EFX_EF10_WORKAROUND_35388(efx)) {
+		efx->timer_quantum_ns = MCDI_DWORD(data,
+			GET_EVQ_TMR_PROPERTIES_OUT_BUG35388_TMR_NS_PER_COUNT);
+		max_count = MCDI_DWORD(data,
+			GET_EVQ_TMR_PROPERTIES_OUT_BUG35388_TMR_MAX_COUNT);
+		efx->timer_max_ns = max_count * efx->timer_quantum_ns;
+	} else {
+		efx->timer_quantum_ns = MCDI_DWORD(data,
+			GET_EVQ_TMR_PROPERTIES_OUT_TMR_REG_NS_PER_COUNT);
+		max_count = MCDI_DWORD(data,
+			GET_EVQ_TMR_PROPERTIES_OUT_TMR_REG_MAX_COUNT);
+		efx->timer_max_ns = max_count * efx->timer_quantum_ns;
+	}
+
+	netif_dbg(efx, probe, efx->net_dev,
+		  "got timer properties from MC: quantum %u ns; max %u ns\n",
+		  efx->timer_quantum_ns, efx->timer_max_ns);
+}
+
+static int efx_ef10_get_timer_config(struct efx_nic *efx)
+{
+	MCDI_DECLARE_BUF(outbuf, MC_CMD_GET_EVQ_TMR_PROPERTIES_OUT_LEN);
+	int rc;
+
+	rc = efx_ef10_get_timer_workarounds(efx);
+	if (rc)
+		return rc;
+
+	rc = efx_mcdi_rpc_quiet(efx, MC_CMD_GET_EVQ_TMR_PROPERTIES, NULL, 0,
+				outbuf, sizeof(outbuf), NULL);
+
+	if (rc == 0) {
+		efx_ef10_process_timer_config(efx, outbuf);
+	} else if (rc == -ENOSYS || rc == -EPERM) {
+		/* Not available - fall back to Huntington defaults. */
+		unsigned int quantum;
+
+		rc = efx_ef10_get_sysclk_freq(efx);
+		if (rc < 0)
+			return rc;
+
+		quantum = 1536000 / rc; /* 1536 cycles */
+		efx->timer_quantum_ns = quantum;
+		efx->timer_max_ns = efx->type->timer_period_max * quantum;
+		rc = 0;
+	} else {
+		efx_mcdi_display_error(efx, MC_CMD_GET_EVQ_TMR_PROPERTIES,
+				       MC_CMD_GET_EVQ_TMR_PROPERTIES_OUT_LEN,
+				       NULL, 0, rc);
+	}
+
+	return rc;
+}
+
 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);
@@ -527,32 +643,9 @@ static int efx_ef10_probe(struct efx_nic *efx)
 	if (rc)
 		goto fail5;
 
-	rc = efx_ef10_get_sysclk_freq(efx);
+	rc = efx_ef10_get_timer_config(efx);
 	if (rc < 0)
 		goto fail5;
-	efx->timer_quantum_ns = 1536000 / rc; /* 1536 cycles */
-
-	/* 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, NULL);
-	if (rc == 0) {
-		nic_data->workaround_35388 = true;
-	} 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 && rc != -EPERM)
@@ -1440,9 +1533,10 @@ static const struct efx_hw_stat_desc efx_ef10_stat_desc[EF10_STAT_COUNT] = {
 			       (1ULL << GENERIC_STAT_rx_nodesc_trunc) |	\
 			       (1ULL << GENERIC_STAT_rx_noskb_drops))
 
-/* These statistics are only provided by the 10G MAC.  For a 10G/40G
- * switchable port we do not expose these because they might not
- * include all the packets they should.
+/* On 7000 series NICs, these statistics are only provided by the 10G MAC.
+ * For a 10G/40G switchable port we do not expose these because they might
+ * not include all the packets they should.
+ * On 8000 series NICs these statistics are always provided.
  */
 #define HUNT_10G_ONLY_STAT_MASK ((1ULL << EF10_STAT_port_tx_control) |	\
 				 (1ULL << EF10_STAT_port_tx_lt64) |	\
@@ -1488,10 +1582,15 @@ static u64 efx_ef10_raw_stat_mask(struct efx_nic *efx)
 	      1 << MC_CMD_DRV_ATTACH_EXT_OUT_FLAG_LINKCTRL))
 		return 0;
 
-	if (port_caps & (1 << MC_CMD_PHY_CAP_40000FDX_LBN))
+	if (port_caps & (1 << MC_CMD_PHY_CAP_40000FDX_LBN)) {
 		raw_mask |= HUNT_40G_EXTRA_STAT_MASK;
-	else
+		/* 8000 series have everything even at 40G */
+		if (nic_data->datapath_caps2 &
+		    (1 << MC_CMD_GET_CAPABILITIES_V2_OUT_MAC_STATS_40G_TX_SIZE_BINS_LBN))
+			raw_mask |= HUNT_10G_ONLY_STAT_MASK;
+	} else {
 		raw_mask |= HUNT_10G_ONLY_STAT_MASK;
+	}
 
 	if (nic_data->datapath_caps &
 	    (1 << MC_CMD_GET_CAPABILITIES_OUT_PM_AND_RXDP_COUNTERS_LBN))
@@ -1617,7 +1716,6 @@ static int efx_ef10_try_update_nic_stats_pf(struct efx_nic *efx)
 	efx_ef10_get_stat_mask(efx, mask);
 
 	dma_stats = efx->stats_buffer.addr;
-	nic_data = efx->nic_data;
 
 	generation_end = dma_stats[MC_CMD_MAC_GENERATION_END];
 	if (generation_end == EFX_MC_STATS_GENERATION_INVALID)
@@ -1744,27 +1842,43 @@ static size_t efx_ef10_update_stats_vf(struct efx_nic *efx, u64 *full_stats,
 static void efx_ef10_push_irq_moderation(struct efx_channel *channel)
 {
 	struct efx_nic *efx = channel->efx;
-	unsigned int mode, value;
+	unsigned int mode, usecs;
 	efx_dword_t timer_cmd;
 
-	if (channel->irq_moderation) {
+	if (channel->irq_moderation_us) {
 		mode = 3;
-		value = channel->irq_moderation - 1;
+		usecs = channel->irq_moderation_us;
 	} else {
 		mode = 0;
-		value = 0;
+		usecs = 0;
 	}
 
-	if (EFX_EF10_WORKAROUND_35388(efx)) {
+	if (EFX_EF10_WORKAROUND_61265(efx)) {
+		MCDI_DECLARE_BUF(inbuf, MC_CMD_SET_EVQ_TMR_IN_LEN);
+		unsigned int ns = usecs * 1000;
+
+		MCDI_SET_DWORD(inbuf, SET_EVQ_TMR_IN_INSTANCE,
+			       channel->channel);
+		MCDI_SET_DWORD(inbuf, SET_EVQ_TMR_IN_TMR_LOAD_REQ_NS, ns);
+		MCDI_SET_DWORD(inbuf, SET_EVQ_TMR_IN_TMR_RELOAD_REQ_NS, ns);
+		MCDI_SET_DWORD(inbuf, SET_EVQ_TMR_IN_TMR_MODE, mode);
+
+		efx_mcdi_rpc_async(efx, MC_CMD_SET_EVQ_TMR,
+				   inbuf, sizeof(inbuf), 0, NULL, 0);
+	} else if (EFX_EF10_WORKAROUND_35388(efx)) {
+		unsigned int ticks = efx_usecs_to_ticks(efx, usecs);
+
 		EFX_POPULATE_DWORD_3(timer_cmd, ERF_DD_EVQ_IND_TIMER_FLAGS,
 				     EFE_DD_EVQ_IND_TIMER_FLAGS,
 				     ERF_DD_EVQ_IND_TIMER_MODE, mode,
-				     ERF_DD_EVQ_IND_TIMER_VAL, value);
+				     ERF_DD_EVQ_IND_TIMER_VAL, ticks);
 		efx_writed_page(efx, &timer_cmd, ER_DD_EVQ_INDIRECT,
 				channel->channel);
 	} else {
+		unsigned int ticks = efx_usecs_to_ticks(efx, usecs);
+
 		EFX_POPULATE_DWORD_2(timer_cmd, ERF_DZ_TC_TIMER_MODE, mode,
-				     ERF_DZ_TC_TIMER_VAL, value);
+				     ERF_DZ_TC_TIMER_VAL, ticks);
 		efx_writed_page(efx, &timer_cmd, ER_DZ_EVQ_TMR,
 				channel->channel);
 	}
@@ -1935,14 +2049,18 @@ static irqreturn_t efx_ef10_legacy_interrupt(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
-static void efx_ef10_irq_test_generate(struct efx_nic *efx)
+static int efx_ef10_irq_test_generate(struct efx_nic *efx)
 {
 	MCDI_DECLARE_BUF(inbuf, MC_CMD_TRIGGER_INTERRUPT_IN_LEN);
 
+	if (efx_mcdi_set_workaround(efx, MC_CMD_WORKAROUND_BUG41750, true,
+				    NULL) == 0)
+		return -ENOTSUPP;
+
 	BUILD_BUG_ON(MC_CMD_TRIGGER_INTERRUPT_OUT_LEN != 0);
 
 	MCDI_SET_DWORD(inbuf, TRIGGER_INTERRUPT_IN_INTR_LEVEL, efx->irq_level);
-	(void) efx_mcdi_rpc(efx, MC_CMD_TRIGGER_INTERRUPT,
+	return efx_mcdi_rpc(efx, MC_CMD_TRIGGER_INTERRUPT,
 			    inbuf, sizeof(inbuf), NULL, 0, NULL);
 }
 
@@ -2536,13 +2654,12 @@ fail:
 static int efx_ef10_ev_init(struct efx_channel *channel)
 {
 	MCDI_DECLARE_BUF(inbuf,
-			 MC_CMD_INIT_EVQ_IN_LEN(EFX_MAX_EVQ_SIZE * 8 /
-						EFX_BUF_SIZE));
-	MCDI_DECLARE_BUF(outbuf, MC_CMD_INIT_EVQ_OUT_LEN);
+			 MC_CMD_INIT_EVQ_V2_IN_LEN(EFX_MAX_EVQ_SIZE * 8 /
+						   EFX_BUF_SIZE));
+	MCDI_DECLARE_BUF(outbuf, MC_CMD_INIT_EVQ_V2_OUT_LEN);
 	size_t entries = channel->eventq.buf.len / EFX_BUF_SIZE;
 	struct efx_nic *efx = channel->efx;
 	struct efx_ef10_nic_data *nic_data;
-	bool supports_rx_merge;
 	size_t inlen, outlen;
 	unsigned int enabled, implemented;
 	dma_addr_t dma_addr;
@@ -2550,9 +2667,6 @@ static int efx_ef10_ev_init(struct efx_channel *channel)
 	int i;
 
 	nic_data = efx->nic_data;
-	supports_rx_merge =
-		!!(nic_data->datapath_caps &
-		   1 << MC_CMD_GET_CAPABILITIES_OUT_RX_BATCHING_LBN);
 
 	/* Fill event queue with all ones (i.e. empty events) */
 	memset(channel->eventq.buf.addr, 0xff, channel->eventq.buf.len);
@@ -2561,11 +2675,6 @@ static int efx_ef10_ev_init(struct efx_channel *channel)
 	MCDI_SET_DWORD(inbuf, INIT_EVQ_IN_INSTANCE, channel->channel);
 	/* INIT_EVQ expects index in vector table, not absolute */
 	MCDI_SET_DWORD(inbuf, INIT_EVQ_IN_IRQ_NUM, channel->channel);
-	MCDI_POPULATE_DWORD_4(inbuf, INIT_EVQ_IN_FLAGS,
-			      INIT_EVQ_IN_FLAG_INTERRUPTING, 1,
-			      INIT_EVQ_IN_FLAG_RX_MERGE, 1,
-			      INIT_EVQ_IN_FLAG_TX_MERGE, 1,
-			      INIT_EVQ_IN_FLAG_CUT_THRU, !supports_rx_merge);
 	MCDI_SET_DWORD(inbuf, INIT_EVQ_IN_TMR_MODE,
 		       MC_CMD_INIT_EVQ_IN_TMR_MODE_DIS);
 	MCDI_SET_DWORD(inbuf, INIT_EVQ_IN_TMR_LOAD, 0);
@@ -2574,6 +2683,27 @@ static int efx_ef10_ev_init(struct efx_channel *channel)
 		       MC_CMD_INIT_EVQ_IN_COUNT_MODE_DIS);
 	MCDI_SET_DWORD(inbuf, INIT_EVQ_IN_COUNT_THRSHLD, 0);
 
+	if (nic_data->datapath_caps2 &
+	    1 << MC_CMD_GET_CAPABILITIES_V2_OUT_INIT_EVQ_V2_LBN) {
+		/* Use the new generic approach to specifying event queue
+		 * configuration, requesting lower latency or higher throughput.
+		 * The options that actually get used appear in the output.
+		 */
+		MCDI_POPULATE_DWORD_2(inbuf, INIT_EVQ_V2_IN_FLAGS,
+				      INIT_EVQ_V2_IN_FLAG_INTERRUPTING, 1,
+				      INIT_EVQ_V2_IN_FLAG_TYPE,
+				      MC_CMD_INIT_EVQ_V2_IN_FLAG_TYPE_AUTO);
+	} else {
+		bool cut_thru = !(nic_data->datapath_caps &
+			1 << MC_CMD_GET_CAPABILITIES_OUT_RX_BATCHING_LBN);
+
+		MCDI_POPULATE_DWORD_4(inbuf, INIT_EVQ_IN_FLAGS,
+				      INIT_EVQ_IN_FLAG_INTERRUPTING, 1,
+				      INIT_EVQ_IN_FLAG_RX_MERGE, 1,
+				      INIT_EVQ_IN_FLAG_TX_MERGE, 1,
+				      INIT_EVQ_IN_FLAG_CUT_THRU, cut_thru);
+	}
+
 	dma_addr = channel->eventq.buf.dma_addr;
 	for (i = 0; i < entries; ++i) {
 		MCDI_SET_ARRAY_QWORD(inbuf, INIT_EVQ_IN_DMA_ADDR, i, dma_addr);
@@ -2584,6 +2714,13 @@ static int efx_ef10_ev_init(struct efx_channel *channel)
 
 	rc = efx_mcdi_rpc(efx, MC_CMD_INIT_EVQ, inbuf, inlen,
 			  outbuf, sizeof(outbuf), &outlen);
+
+	if (outlen >= MC_CMD_INIT_EVQ_V2_OUT_LEN)
+		netif_dbg(efx, drv, efx->net_dev,
+			  "Channel %d using event queue flags %08x\n",
+			  channel->channel,
+			  MCDI_DWORD(outbuf, INIT_EVQ_V2_OUT_FLAGS));
+
 	/* IRQ return is ignored */
 	if (channel->channel || rc)
 		return rc;
@@ -2591,8 +2728,8 @@ static int efx_ef10_ev_init(struct efx_channel *channel)
 	/* Successfully created event queue on channel 0 */
 	rc = efx_mcdi_get_workarounds(efx, &implemented, &enabled);
 	if (rc == -ENOSYS) {
-		/* GET_WORKAROUNDS was implemented before the bug26807
-		 * workaround, thus the latter must be unavailable in this fw
+		/* GET_WORKAROUNDS was implemented before this workaround,
+		 * thus it must be unavailable in this firmware.
 		 */
 		nic_data->workaround_26807 = false;
 		rc = 0;
diff --git a/drivers/net/ethernet/sfc/efx.c b/drivers/net/ethernet/sfc/efx.c
index 14b821b1c880..3cf3557106c2 100644
--- a/drivers/net/ethernet/sfc/efx.c
+++ b/drivers/net/ethernet/sfc/efx.c
@@ -281,6 +281,27 @@ static int efx_process_channel(struct efx_channel *channel, int budget)
  * NAPI guarantees serialisation of polls of the same device, which
  * provides the guarantee required by efx_process_channel().
  */
+static void efx_update_irq_mod(struct efx_nic *efx, struct efx_channel *channel)
+{
+	int step = efx->irq_mod_step_us;
+
+	if (channel->irq_mod_score < irq_adapt_low_thresh) {
+		if (channel->irq_moderation_us > step) {
+			channel->irq_moderation_us -= step;
+			efx->type->push_irq_moderation(channel);
+		}
+	} else if (channel->irq_mod_score > irq_adapt_high_thresh) {
+		if (channel->irq_moderation_us <
+		    efx->irq_rx_moderation_us) {
+			channel->irq_moderation_us += step;
+			efx->type->push_irq_moderation(channel);
+		}
+	}
+
+	channel->irq_count = 0;
+	channel->irq_mod_score = 0;
+}
+
 static int efx_poll(struct napi_struct *napi, int budget)
 {
 	struct efx_channel *channel =
@@ -301,22 +322,7 @@ static int efx_poll(struct napi_struct *napi, int budget)
 		if (efx_channel_has_rx_queue(channel) &&
 		    efx->irq_rx_adaptive &&
 		    unlikely(++channel->irq_count == 1000)) {
-			if (unlikely(channel->irq_mod_score <
-				     irq_adapt_low_thresh)) {
-				if (channel->irq_moderation > 1) {
-					channel->irq_moderation -= 1;
-					efx->type->push_irq_moderation(channel);
-				}
-			} else if (unlikely(channel->irq_mod_score >
-					    irq_adapt_high_thresh)) {
-				if (channel->irq_moderation <
-				    efx->irq_rx_moderation) {
-					channel->irq_moderation += 1;
-					efx->type->push_irq_moderation(channel);
-				}
-			}
-			channel->irq_count = 0;
-			channel->irq_mod_score = 0;
+			efx_update_irq_mod(efx, channel);
 		}
 
 		efx_filter_rfs_expire(channel);
@@ -1703,6 +1709,7 @@ static int efx_probe_nic(struct efx_nic *efx)
 	netif_set_real_num_rx_queues(efx->net_dev, efx->n_rx_channels);
 
 	/* Initialise the interrupt moderation settings */
+	efx->irq_mod_step_us = DIV_ROUND_UP(efx->timer_quantum_ns, 1000);
 	efx_init_irq_moderation(efx, tx_irq_mod_usec, rx_irq_mod_usec, true,
 				true);
 
@@ -1949,14 +1956,21 @@ static void efx_remove_all(struct efx_nic *efx)
  * Interrupt moderation
  *
  **************************************************************************/
-
-static unsigned int irq_mod_ticks(unsigned int usecs, unsigned int quantum_ns)
+unsigned int efx_usecs_to_ticks(struct efx_nic *efx, unsigned int usecs)
 {
 	if (usecs == 0)
 		return 0;
-	if (usecs * 1000 < quantum_ns)
+	if (usecs * 1000 < efx->timer_quantum_ns)
 		return 1; /* never round down to 0 */
-	return usecs * 1000 / quantum_ns;
+	return usecs * 1000 / efx->timer_quantum_ns;
+}
+
+unsigned int efx_ticks_to_usecs(struct efx_nic *efx, unsigned int ticks)
+{
+	/* We must round up when converting ticks to microseconds
+	 * because we round down when converting the other way.
+	 */
+	return DIV_ROUND_UP(ticks * efx->timer_quantum_ns, 1000);
 }
 
 /* Set interrupt moderation parameters */
@@ -1965,21 +1979,16 @@ int efx_init_irq_moderation(struct efx_nic *efx, unsigned int tx_usecs,
 			    bool rx_may_override_tx)
 {
 	struct efx_channel *channel;
-	unsigned int irq_mod_max = DIV_ROUND_UP(efx->type->timer_period_max *
-						efx->timer_quantum_ns,
-						1000);
-	unsigned int tx_ticks;
-	unsigned int rx_ticks;
+	unsigned int timer_max_us;
 
 	EFX_ASSERT_RESET_SERIALISED(efx);
 
-	if (tx_usecs > irq_mod_max || rx_usecs > irq_mod_max)
-		return -EINVAL;
+	timer_max_us = efx->timer_max_ns / 1000;
 
-	tx_ticks = irq_mod_ticks(tx_usecs, efx->timer_quantum_ns);
-	rx_ticks = irq_mod_ticks(rx_usecs, efx->timer_quantum_ns);
+	if (tx_usecs > timer_max_us || rx_usecs > timer_max_us)
+		return -EINVAL;
 
-	if (tx_ticks != rx_ticks && efx->tx_channel_offset == 0 &&
+	if (tx_usecs != rx_usecs && efx->tx_channel_offset == 0 &&
 	    !rx_may_override_tx) {
 		netif_err(efx, drv, efx->net_dev, "Channels are shared. "
 			  "RX and TX IRQ moderation must be equal\n");
@@ -1987,12 +1996,12 @@ int efx_init_irq_moderation(struct efx_nic *efx, unsigned int tx_usecs,
 	}
 
 	efx->irq_rx_adaptive = rx_adaptive;
-	efx->irq_rx_moderation = rx_ticks;
+	efx->irq_rx_moderation_us = rx_usecs;
 	efx_for_each_channel(channel, efx) {
 		if (efx_channel_has_rx_queue(channel))
-			channel->irq_moderation = rx_ticks;
+			channel->irq_moderation_us = rx_usecs;
 		else if (efx_channel_has_tx_queues(channel))
-			channel->irq_moderation = tx_ticks;
+			channel->irq_moderation_us = tx_usecs;
 	}
 
 	return 0;
@@ -2001,26 +2010,21 @@ int efx_init_irq_moderation(struct efx_nic *efx, unsigned int tx_usecs,
 void efx_get_irq_moderation(struct efx_nic *efx, unsigned int *tx_usecs,
 			    unsigned int *rx_usecs, bool *rx_adaptive)
 {
-	/* We must round up when converting ticks to microseconds
-	 * because we round down when converting the other way.
-	 */
-
 	*rx_adaptive = efx->irq_rx_adaptive;
-	*rx_usecs = DIV_ROUND_UP(efx->irq_rx_moderation *
-				 efx->timer_quantum_ns,
-				 1000);
+	*rx_usecs = efx->irq_rx_moderation_us;
 
 	/* If channels are shared between RX and TX, so is IRQ
 	 * moderation.  Otherwise, IRQ moderation is the same for all
 	 * TX channels and is not adaptive.
 	 */
-	if (efx->tx_channel_offset == 0)
+	if (efx->tx_channel_offset == 0) {
 		*tx_usecs = *rx_usecs;
-	else
-		*tx_usecs = DIV_ROUND_UP(
-			efx->channel[efx->tx_channel_offset]->irq_moderation *
-			efx->timer_quantum_ns,
-			1000);
+	} else {
+		struct efx_channel *tx_channel;
+
+		tx_channel = efx->channel[efx->tx_channel_offset];
+		*tx_usecs = tx_channel->irq_moderation_us;
+	}
 }
 
 /**************************************************************************
@@ -2259,8 +2263,18 @@ static int efx_change_mtu(struct net_device *net_dev, int new_mtu)
 	rc = efx_check_disabled(efx);
 	if (rc)
 		return rc;
-	if (new_mtu > EFX_MAX_MTU)
+	if (new_mtu > EFX_MAX_MTU) {
+		netif_err(efx, drv, efx->net_dev,
+			  "Requested MTU of %d too big (max: %d)\n",
+			  new_mtu, EFX_MAX_MTU);
 		return -EINVAL;
+	}
+	if (new_mtu < EFX_MIN_MTU) {
+		netif_err(efx, drv, efx->net_dev,
+			  "Requested MTU of %d too small (min: %d)\n",
+			  new_mtu, EFX_MIN_MTU);
+		return -EINVAL;
+	}
 
 	netif_dbg(efx, drv, efx->net_dev, "changing MTU to %d\n", new_mtu);
 
diff --git a/drivers/net/ethernet/sfc/efx.h b/drivers/net/ethernet/sfc/efx.h
index c3ae739e9c7a..342ae16e1f2d 100644
--- a/drivers/net/ethernet/sfc/efx.h
+++ b/drivers/net/ethernet/sfc/efx.h
@@ -204,6 +204,8 @@ int efx_try_recovery(struct efx_nic *efx);
 
 /* Global */
 void efx_schedule_reset(struct efx_nic *efx, enum reset_type type);
+unsigned int efx_usecs_to_ticks(struct efx_nic *efx, unsigned int usecs);
+unsigned int efx_ticks_to_usecs(struct efx_nic *efx, unsigned int ticks);
 int efx_init_irq_moderation(struct efx_nic *efx, unsigned int tx_usecs,
 			    unsigned int rx_usecs, bool rx_adaptive,
 			    bool rx_may_override_tx);
diff --git a/drivers/net/ethernet/sfc/falcon.c b/drivers/net/ethernet/sfc/falcon.c
index d790cb8d9db3..1a7092602aec 100644
--- a/drivers/net/ethernet/sfc/falcon.c
+++ b/drivers/net/ethernet/sfc/falcon.c
@@ -378,12 +378,15 @@ static void falcon_push_irq_moderation(struct efx_channel *channel)
 	struct efx_nic *efx = channel->efx;
 
 	/* Set timer register */
-	if (channel->irq_moderation) {
+	if (channel->irq_moderation_us) {
+		unsigned int ticks;
+
+		ticks = efx_usecs_to_ticks(efx, channel->irq_moderation_us);
 		EFX_POPULATE_DWORD_2(timer_cmd,
 				     FRF_AB_TC_TIMER_MODE,
 				     FFE_BB_TIMER_MODE_INT_HLDOFF,
 				     FRF_AB_TC_TIMER_VAL,
-				     channel->irq_moderation - 1);
+				     ticks - 1);
 	} else {
 		EFX_POPULATE_DWORD_2(timer_cmd,
 				     FRF_AB_TC_TIMER_MODE,
@@ -2373,6 +2376,8 @@ static int falcon_probe_nic(struct efx_nic *efx)
 			     EFX_MAX_CHANNELS);
 	efx->max_tx_channels = efx->max_channels;
 	efx->timer_quantum_ns = 4968; /* 621 cycles */
+	efx->timer_max_ns = efx->type->timer_period_max *
+			    efx->timer_quantum_ns;
 
 	/* Initialise I2C adapter */
 	board = falcon_board(efx);
diff --git a/drivers/net/ethernet/sfc/falcon_boards.c b/drivers/net/ethernet/sfc/falcon_boards.c
index 1736f4b806af..f6883b2b5da3 100644
--- a/drivers/net/ethernet/sfc/falcon_boards.c
+++ b/drivers/net/ethernet/sfc/falcon_boards.c
@@ -64,7 +64,7 @@
 #define LM87_ALARM_TEMP_INT		0x10
 #define LM87_ALARM_TEMP_EXT1		0x20
 
-#if defined(CONFIG_SENSORS_LM87) || defined(CONFIG_SENSORS_LM87_MODULE)
+#if IS_ENABLED(CONFIG_SENSORS_LM87)
 
 static int efx_poke_lm87(struct i2c_client *client, const u8 *reg_values)
 {
@@ -455,7 +455,7 @@ static int sfe4001_init(struct efx_nic *efx)
 	struct falcon_board *board = falcon_board(efx);
 	int rc;
 
-#if defined(CONFIG_SENSORS_LM90) || defined(CONFIG_SENSORS_LM90_MODULE)
+#if IS_ENABLED(CONFIG_SENSORS_LM90)
 	board->hwmon_client =
 		i2c_new_device(&board->i2c_adap, &sfe4001_hwmon_info);
 #else
diff --git a/drivers/net/ethernet/sfc/farch.c b/drivers/net/ethernet/sfc/farch.c
index 4c83739d158f..4762ec444cb8 100644
--- a/drivers/net/ethernet/sfc/farch.c
+++ b/drivers/net/ethernet/sfc/farch.c
@@ -1477,9 +1477,10 @@ void efx_farch_irq_disable_master(struct efx_nic *efx)
  * Interrupt must already have been enabled, otherwise nasty things
  * may happen.
  */
-void efx_farch_irq_test_generate(struct efx_nic *efx)
+int efx_farch_irq_test_generate(struct efx_nic *efx)
 {
 	efx_farch_interrupts(efx, true, true);
+	return 0;
 }
 
 /* Process a fatal interrupt
diff --git a/drivers/net/ethernet/sfc/mcdi.c b/drivers/net/ethernet/sfc/mcdi.c
index d28e7dd8fa3c..241520943ada 100644
--- a/drivers/net/ethernet/sfc/mcdi.c
+++ b/drivers/net/ethernet/sfc/mcdi.c
@@ -548,7 +548,10 @@ static bool efx_mcdi_complete_async(struct efx_mcdi_iface *mcdi, bool timeout)
 		efx_mcdi_display_error(efx, async->cmd, async->inlen, errbuf,
 				       err_len, rc);
 	}
-	async->complete(efx, async->cookie, rc, outbuf, data_len);
+
+	if (async->complete)
+		async->complete(efx, async->cookie, rc, outbuf,
+				min(async->outlen, data_len));
 	kfree(async);
 
 	efx_mcdi_release(mcdi);
@@ -1153,7 +1156,8 @@ void efx_mcdi_flush_async(struct efx_nic *efx)
 	 * acquired locks in the wrong order.
 	 */
 	list_for_each_entry_safe(async, next, &mcdi->async_list, list) {
-		async->complete(efx, async->cookie, -ENETDOWN, NULL, 0);
+		if (async->complete)
+			async->complete(efx, async->cookie, -ENETDOWN, NULL, 0);
 		list_del(&async->list);
 		kfree(async);
 	}
diff --git a/drivers/net/ethernet/sfc/mcdi_pcol.h b/drivers/net/ethernet/sfc/mcdi_pcol.h
index c9a5b003caaf..ccceafc15896 100644
--- a/drivers/net/ethernet/sfc/mcdi_pcol.h
+++ b/drivers/net/ethernet/sfc/mcdi_pcol.h
@@ -2645,16 +2645,20 @@
 #define          MC_CMD_POLL_BIST_MEM_BUS_MC 0x0
 /* enum: CSR IREG bus. */
 #define          MC_CMD_POLL_BIST_MEM_BUS_CSR 0x1
-/* enum: RX DPCPU bus. */
+/* enum: RX0 DPCPU bus. */
 #define          MC_CMD_POLL_BIST_MEM_BUS_DPCPU_RX 0x2
 /* enum: TX0 DPCPU bus. */
 #define          MC_CMD_POLL_BIST_MEM_BUS_DPCPU_TX0 0x3
 /* enum: TX1 DPCPU bus. */
 #define          MC_CMD_POLL_BIST_MEM_BUS_DPCPU_TX1 0x4
-/* enum: RX DICPU bus. */
+/* enum: RX0 DICPU bus. */
 #define          MC_CMD_POLL_BIST_MEM_BUS_DICPU_RX 0x5
 /* enum: TX DICPU bus. */
 #define          MC_CMD_POLL_BIST_MEM_BUS_DICPU_TX 0x6
+/* enum: RX1 DPCPU bus. */
+#define          MC_CMD_POLL_BIST_MEM_BUS_DPCPU_RX1 0x7
+/* enum: RX1 DICPU bus. */
+#define          MC_CMD_POLL_BIST_MEM_BUS_DICPU_RX1 0x8
 /* Pattern written to RAM / register */
 #define       MC_CMD_POLL_BIST_OUT_MEM_EXPECT_OFST 16
 /* Actual value read from RAM / register */
@@ -3612,6 +3616,8 @@
 #define        MC_CMD_NVRAM_INFO_OUT_PROTECTED_WIDTH 1
 #define        MC_CMD_NVRAM_INFO_OUT_TLV_LBN 1
 #define        MC_CMD_NVRAM_INFO_OUT_TLV_WIDTH 1
+#define        MC_CMD_NVRAM_INFO_OUT_CMAC_LBN 6
+#define        MC_CMD_NVRAM_INFO_OUT_CMAC_WIDTH 1
 #define        MC_CMD_NVRAM_INFO_OUT_A_B_LBN 7
 #define        MC_CMD_NVRAM_INFO_OUT_A_B_WIDTH 1
 #define       MC_CMD_NVRAM_INFO_OUT_PHYSDEV_OFST 16
@@ -4389,6 +4395,8 @@
  * the command will fail with MC_CMD_ERR_FILTERS_PRESENT.
  */
 #define          MC_CMD_WORKAROUND_BUG26807 0x6
+/* enum: Bug 61265 work around (broken EVQ TMR writes). */
+#define          MC_CMD_WORKAROUND_BUG61265 0x7
 /* 0 = disable the workaround indicated by TYPE; any non-zero value = enable
  * the workaround
  */
@@ -4413,7 +4421,6 @@
  * (GET_PHY_CFG_OUT_MEDIA_TYPE); the valid 'page number' input values, and the
  * output data, are interpreted on a per-type basis. For SFP+: PAGE=0 or 1
  * returns a 128-byte block read from module I2C address 0xA0 offset 0 or 0x80.
- * Anything else: currently undefined. Locks required: None. Return code: 0.
  */
 #define MC_CMD_GET_PHY_MEDIA_INFO 0x4b
 
@@ -5479,6 +5486,8 @@
 #define        LICENSED_V3_FEATURES_TX_SNIFF_WIDTH 1
 #define        LICENSED_V3_FEATURES_PROXY_FILTER_OPS_LBN 8
 #define        LICENSED_V3_FEATURES_PROXY_FILTER_OPS_WIDTH 1
+#define        LICENSED_V3_FEATURES_EVENT_CUT_THROUGH_LBN 9
+#define        LICENSED_V3_FEATURES_EVENT_CUT_THROUGH_WIDTH 1
 #define       LICENSED_V3_FEATURES_MASK_LBN 0
 #define       LICENSED_V3_FEATURES_MASK_WIDTH 64
 
@@ -5634,6 +5643,109 @@
 /* Only valid if INTRFLAG was true */
 #define       MC_CMD_INIT_EVQ_OUT_IRQ_OFST 0
 
+/* MC_CMD_INIT_EVQ_V2_IN msgrequest */
+#define    MC_CMD_INIT_EVQ_V2_IN_LENMIN 44
+#define    MC_CMD_INIT_EVQ_V2_IN_LENMAX 548
+#define    MC_CMD_INIT_EVQ_V2_IN_LEN(num) (36+8*(num))
+/* Size, in entries */
+#define       MC_CMD_INIT_EVQ_V2_IN_SIZE_OFST 0
+/* Desired instance. Must be set to a specific instance, which is a function
+ * local queue index.
+ */
+#define       MC_CMD_INIT_EVQ_V2_IN_INSTANCE_OFST 4
+/* The initial timer value. The load value is ignored if the timer mode is DIS.
+ */
+#define       MC_CMD_INIT_EVQ_V2_IN_TMR_LOAD_OFST 8
+/* The reload value is ignored in one-shot modes */
+#define       MC_CMD_INIT_EVQ_V2_IN_TMR_RELOAD_OFST 12
+/* tbd */
+#define       MC_CMD_INIT_EVQ_V2_IN_FLAGS_OFST 16
+#define        MC_CMD_INIT_EVQ_V2_IN_FLAG_INTERRUPTING_LBN 0
+#define        MC_CMD_INIT_EVQ_V2_IN_FLAG_INTERRUPTING_WIDTH 1
+#define        MC_CMD_INIT_EVQ_V2_IN_FLAG_RPTR_DOS_LBN 1
+#define        MC_CMD_INIT_EVQ_V2_IN_FLAG_RPTR_DOS_WIDTH 1
+#define        MC_CMD_INIT_EVQ_V2_IN_FLAG_INT_ARMD_LBN 2
+#define        MC_CMD_INIT_EVQ_V2_IN_FLAG_INT_ARMD_WIDTH 1
+#define        MC_CMD_INIT_EVQ_V2_IN_FLAG_CUT_THRU_LBN 3
+#define        MC_CMD_INIT_EVQ_V2_IN_FLAG_CUT_THRU_WIDTH 1
+#define        MC_CMD_INIT_EVQ_V2_IN_FLAG_RX_MERGE_LBN 4
+#define        MC_CMD_INIT_EVQ_V2_IN_FLAG_RX_MERGE_WIDTH 1
+#define        MC_CMD_INIT_EVQ_V2_IN_FLAG_TX_MERGE_LBN 5
+#define        MC_CMD_INIT_EVQ_V2_IN_FLAG_TX_MERGE_WIDTH 1
+#define        MC_CMD_INIT_EVQ_V2_IN_FLAG_USE_TIMER_LBN 6
+#define        MC_CMD_INIT_EVQ_V2_IN_FLAG_USE_TIMER_WIDTH 1
+#define        MC_CMD_INIT_EVQ_V2_IN_FLAG_TYPE_LBN 7
+#define        MC_CMD_INIT_EVQ_V2_IN_FLAG_TYPE_WIDTH 4
+/* enum: All initialisation flags specified by host. */
+#define          MC_CMD_INIT_EVQ_V2_IN_FLAG_TYPE_MANUAL 0x0
+/* enum: MEDFORD only. Certain initialisation flags specified by host may be
+ * over-ridden by firmware based on licenses and firmware variant in order to
+ * provide the lowest latency achievable. See
+ * MC_CMD_INIT_EVQ_V2/MC_CMD_INIT_EVQ_V2_OUT/FLAGS for list of affected flags.
+ */
+#define          MC_CMD_INIT_EVQ_V2_IN_FLAG_TYPE_LOW_LATENCY 0x1
+/* enum: MEDFORD only. Certain initialisation flags specified by host may be
+ * over-ridden by firmware based on licenses and firmware variant in order to
+ * provide the best throughput achievable. See
+ * MC_CMD_INIT_EVQ_V2/MC_CMD_INIT_EVQ_V2_OUT/FLAGS for list of affected flags.
+ */
+#define          MC_CMD_INIT_EVQ_V2_IN_FLAG_TYPE_THROUGHPUT 0x2
+/* enum: MEDFORD only. Certain initialisation flags may be over-ridden by
+ * firmware based on licenses and firmware variant. See
+ * MC_CMD_INIT_EVQ_V2/MC_CMD_INIT_EVQ_V2_OUT/FLAGS for list of affected flags.
+ */
+#define          MC_CMD_INIT_EVQ_V2_IN_FLAG_TYPE_AUTO 0x3
+#define       MC_CMD_INIT_EVQ_V2_IN_TMR_MODE_OFST 20
+/* enum: Disabled */
+#define          MC_CMD_INIT_EVQ_V2_IN_TMR_MODE_DIS 0x0
+/* enum: Immediate */
+#define          MC_CMD_INIT_EVQ_V2_IN_TMR_IMMED_START 0x1
+/* enum: Triggered */
+#define          MC_CMD_INIT_EVQ_V2_IN_TMR_TRIG_START 0x2
+/* enum: Hold-off */
+#define          MC_CMD_INIT_EVQ_V2_IN_TMR_INT_HLDOFF 0x3
+/* Target EVQ for wakeups if in wakeup mode. */
+#define       MC_CMD_INIT_EVQ_V2_IN_TARGET_EVQ_OFST 24
+/* Target interrupt if in interrupting mode (note union with target EVQ). Use
+ * MC_CMD_RESOURCE_INSTANCE_ANY unless a specific one required for test
+ * purposes.
+ */
+#define       MC_CMD_INIT_EVQ_V2_IN_IRQ_NUM_OFST 24
+/* Event Counter Mode. */
+#define       MC_CMD_INIT_EVQ_V2_IN_COUNT_MODE_OFST 28
+/* enum: Disabled */
+#define          MC_CMD_INIT_EVQ_V2_IN_COUNT_MODE_DIS 0x0
+/* enum: Disabled */
+#define          MC_CMD_INIT_EVQ_V2_IN_COUNT_MODE_RX 0x1
+/* enum: Disabled */
+#define          MC_CMD_INIT_EVQ_V2_IN_COUNT_MODE_TX 0x2
+/* enum: Disabled */
+#define          MC_CMD_INIT_EVQ_V2_IN_COUNT_MODE_RXTX 0x3
+/* Event queue packet count threshold. */
+#define       MC_CMD_INIT_EVQ_V2_IN_COUNT_THRSHLD_OFST 32
+/* 64-bit address of 4k of 4k-aligned host memory buffer */
+#define       MC_CMD_INIT_EVQ_V2_IN_DMA_ADDR_OFST 36
+#define       MC_CMD_INIT_EVQ_V2_IN_DMA_ADDR_LEN 8
+#define       MC_CMD_INIT_EVQ_V2_IN_DMA_ADDR_LO_OFST 36
+#define       MC_CMD_INIT_EVQ_V2_IN_DMA_ADDR_HI_OFST 40
+#define       MC_CMD_INIT_EVQ_V2_IN_DMA_ADDR_MINNUM 1
+#define       MC_CMD_INIT_EVQ_V2_IN_DMA_ADDR_MAXNUM 64
+
+/* MC_CMD_INIT_EVQ_V2_OUT msgresponse */
+#define    MC_CMD_INIT_EVQ_V2_OUT_LEN 8
+/* Only valid if INTRFLAG was true */
+#define       MC_CMD_INIT_EVQ_V2_OUT_IRQ_OFST 0
+/* Actual configuration applied on the card */
+#define       MC_CMD_INIT_EVQ_V2_OUT_FLAGS_OFST 4
+#define        MC_CMD_INIT_EVQ_V2_OUT_FLAG_CUT_THRU_LBN 0
+#define        MC_CMD_INIT_EVQ_V2_OUT_FLAG_CUT_THRU_WIDTH 1
+#define        MC_CMD_INIT_EVQ_V2_OUT_FLAG_RX_MERGE_LBN 1
+#define        MC_CMD_INIT_EVQ_V2_OUT_FLAG_RX_MERGE_WIDTH 1
+#define        MC_CMD_INIT_EVQ_V2_OUT_FLAG_TX_MERGE_LBN 2
+#define        MC_CMD_INIT_EVQ_V2_OUT_FLAG_TX_MERGE_WIDTH 1
+#define        MC_CMD_INIT_EVQ_V2_OUT_FLAG_RXQ_FORCE_EV_MERGING_LBN 3
+#define        MC_CMD_INIT_EVQ_V2_OUT_FLAG_RXQ_FORCE_EV_MERGING_WIDTH 1
+
 /* QUEUE_CRC_MODE structuredef */
 #define    QUEUE_CRC_MODE_LEN 1
 #define       QUEUE_CRC_MODE_MODE_LBN 0
@@ -5697,8 +5809,8 @@
 #define        MC_CMD_INIT_RXQ_IN_FLAG_PREFIX_WIDTH 1
 #define        MC_CMD_INIT_RXQ_IN_FLAG_DISABLE_SCATTER_LBN 9
 #define        MC_CMD_INIT_RXQ_IN_FLAG_DISABLE_SCATTER_WIDTH 1
-#define        MC_CMD_INIT_RXQ_IN_FLAG_FORCE_EV_MERGING_LBN 10
-#define        MC_CMD_INIT_RXQ_IN_FLAG_FORCE_EV_MERGING_WIDTH 1
+#define        MC_CMD_INIT_RXQ_IN_UNUSED_LBN 10
+#define        MC_CMD_INIT_RXQ_IN_UNUSED_WIDTH 1
 /* Owner ID to use if in buffer mode (zero if physical) */
 #define       MC_CMD_INIT_RXQ_IN_OWNER_ID_OFST 20
 /* The port ID associated with the v-adaptor which should contain this DMAQ. */
@@ -7854,6 +7966,20 @@
 #define        MC_CMD_GET_CAPABILITIES_V2_OUT_EVENT_CUT_THROUGH_WIDTH 1
 #define        MC_CMD_GET_CAPABILITIES_V2_OUT_RX_CUT_THROUGH_LBN 4
 #define        MC_CMD_GET_CAPABILITIES_V2_OUT_RX_CUT_THROUGH_WIDTH 1
+#define        MC_CMD_GET_CAPABILITIES_V2_OUT_TX_VFIFO_ULL_MODE_LBN 5
+#define        MC_CMD_GET_CAPABILITIES_V2_OUT_TX_VFIFO_ULL_MODE_WIDTH 1
+#define        MC_CMD_GET_CAPABILITIES_V2_OUT_MAC_STATS_40G_TX_SIZE_BINS_LBN 6
+#define        MC_CMD_GET_CAPABILITIES_V2_OUT_MAC_STATS_40G_TX_SIZE_BINS_WIDTH 1
+#define        MC_CMD_GET_CAPABILITIES_V2_OUT_INIT_EVQ_V2_LBN 7
+#define        MC_CMD_GET_CAPABILITIES_V2_OUT_INIT_EVQ_V2_WIDTH 1
+#define        MC_CMD_GET_CAPABILITIES_V2_OUT_TX_MAC_TIMESTAMPING_LBN 8
+#define        MC_CMD_GET_CAPABILITIES_V2_OUT_TX_MAC_TIMESTAMPING_WIDTH 1
+#define        MC_CMD_GET_CAPABILITIES_V2_OUT_TX_TIMESTAMP_LBN 9
+#define        MC_CMD_GET_CAPABILITIES_V2_OUT_TX_TIMESTAMP_WIDTH 1
+#define        MC_CMD_GET_CAPABILITIES_V2_OUT_RX_SNIFF_LBN 10
+#define        MC_CMD_GET_CAPABILITIES_V2_OUT_RX_SNIFF_WIDTH 1
+#define        MC_CMD_GET_CAPABILITIES_V2_OUT_TX_SNIFF_LBN 11
+#define        MC_CMD_GET_CAPABILITIES_V2_OUT_TX_SNIFF_WIDTH 1
 /* Number of FATSOv2 contexts per datapath supported by this NIC. Not present
  * on older firmware (check the length).
  */
@@ -7910,6 +8036,288 @@
 #define       MC_CMD_GET_CAPABILITIES_V2_OUT_SIZE_PIO_BUFF_OFST 70
 #define       MC_CMD_GET_CAPABILITIES_V2_OUT_SIZE_PIO_BUFF_LEN 2
 
+/* MC_CMD_GET_CAPABILITIES_V3_OUT msgresponse */
+#define    MC_CMD_GET_CAPABILITIES_V3_OUT_LEN 73
+/* First word of flags. */
+#define       MC_CMD_GET_CAPABILITIES_V3_OUT_FLAGS1_OFST 0
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_VPORT_RECONFIGURE_LBN 3
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_VPORT_RECONFIGURE_WIDTH 1
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_TX_STRIPING_LBN 4
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_TX_STRIPING_WIDTH 1
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_VADAPTOR_QUERY_LBN 5
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_VADAPTOR_QUERY_WIDTH 1
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_EVB_PORT_VLAN_RESTRICT_LBN 6
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_EVB_PORT_VLAN_RESTRICT_WIDTH 1
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_DRV_ATTACH_PREBOOT_LBN 7
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_DRV_ATTACH_PREBOOT_WIDTH 1
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_RX_FORCE_EVENT_MERGING_LBN 8
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_RX_FORCE_EVENT_MERGING_WIDTH 1
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_SET_MAC_ENHANCED_LBN 9
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_SET_MAC_ENHANCED_WIDTH 1
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_UNKNOWN_UCAST_DST_FILTER_ALWAYS_MULTI_RECIPIENT_LBN 10
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_UNKNOWN_UCAST_DST_FILTER_ALWAYS_MULTI_RECIPIENT_WIDTH 1
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_VADAPTOR_PERMIT_SET_MAC_WHEN_FILTERS_INSTALLED_LBN 11
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_VADAPTOR_PERMIT_SET_MAC_WHEN_FILTERS_INSTALLED_WIDTH 1
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_TX_MAC_SECURITY_FILTERING_LBN 12
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_TX_MAC_SECURITY_FILTERING_WIDTH 1
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_ADDITIONAL_RSS_MODES_LBN 13
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_ADDITIONAL_RSS_MODES_WIDTH 1
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_QBB_LBN 14
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_QBB_WIDTH 1
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_RX_PACKED_STREAM_VAR_BUFFERS_LBN 15
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_RX_PACKED_STREAM_VAR_BUFFERS_WIDTH 1
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_RX_RSS_LIMITED_LBN 16
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_RX_RSS_LIMITED_WIDTH 1
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_RX_PACKED_STREAM_LBN 17
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_RX_PACKED_STREAM_WIDTH 1
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_RX_INCLUDE_FCS_LBN 18
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_RX_INCLUDE_FCS_WIDTH 1
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_TX_VLAN_INSERTION_LBN 19
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_TX_VLAN_INSERTION_WIDTH 1
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_RX_VLAN_STRIPPING_LBN 20
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_RX_VLAN_STRIPPING_WIDTH 1
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_TX_TSO_LBN 21
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_TX_TSO_WIDTH 1
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_RX_PREFIX_LEN_0_LBN 22
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_RX_PREFIX_LEN_0_WIDTH 1
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_RX_PREFIX_LEN_14_LBN 23
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_RX_PREFIX_LEN_14_WIDTH 1
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_RX_TIMESTAMP_LBN 24
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_RX_TIMESTAMP_WIDTH 1
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_RX_BATCHING_LBN 25
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_RX_BATCHING_WIDTH 1
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_MCAST_FILTER_CHAINING_LBN 26
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_MCAST_FILTER_CHAINING_WIDTH 1
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_PM_AND_RXDP_COUNTERS_LBN 27
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_PM_AND_RXDP_COUNTERS_WIDTH 1
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_RX_DISABLE_SCATTER_LBN 28
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_RX_DISABLE_SCATTER_WIDTH 1
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_TX_MCAST_UDP_LOOPBACK_LBN 29
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_TX_MCAST_UDP_LOOPBACK_WIDTH 1
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_EVB_LBN 30
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_EVB_WIDTH 1
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_VXLAN_NVGRE_LBN 31
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_VXLAN_NVGRE_WIDTH 1
+/* RxDPCPU firmware id. */
+#define       MC_CMD_GET_CAPABILITIES_V3_OUT_RX_DPCPU_FW_ID_OFST 4
+#define       MC_CMD_GET_CAPABILITIES_V3_OUT_RX_DPCPU_FW_ID_LEN 2
+/* enum: Standard RXDP firmware */
+#define          MC_CMD_GET_CAPABILITIES_V3_OUT_RXDP  0x0
+/* enum: Low latency RXDP firmware */
+#define          MC_CMD_GET_CAPABILITIES_V3_OUT_RXDP_LOW_LATENCY  0x1
+/* enum: Packed stream RXDP firmware */
+#define          MC_CMD_GET_CAPABILITIES_V3_OUT_RXDP_PACKED_STREAM  0x2
+/* enum: BIST RXDP firmware */
+#define          MC_CMD_GET_CAPABILITIES_V3_OUT_RXDP_BIST  0x10a
+/* enum: RXDP Test firmware image 1 */
+#define          MC_CMD_GET_CAPABILITIES_V3_OUT_RXDP_TEST_FW_TO_MC_CUT_THROUGH  0x101
+/* enum: RXDP Test firmware image 2 */
+#define          MC_CMD_GET_CAPABILITIES_V3_OUT_RXDP_TEST_FW_TO_MC_STORE_FORWARD  0x102
+/* enum: RXDP Test firmware image 3 */
+#define          MC_CMD_GET_CAPABILITIES_V3_OUT_RXDP_TEST_FW_TO_MC_STORE_FORWARD_FIRST  0x103
+/* enum: RXDP Test firmware image 4 */
+#define          MC_CMD_GET_CAPABILITIES_V3_OUT_RXDP_TEST_EVERY_EVENT_BATCHABLE  0x104
+/* enum: RXDP Test firmware image 5 */
+#define          MC_CMD_GET_CAPABILITIES_V3_OUT_RXDP_TEST_BACKPRESSURE  0x105
+/* enum: RXDP Test firmware image 6 */
+#define          MC_CMD_GET_CAPABILITIES_V3_OUT_RXDP_TEST_FW_PACKET_EDITS  0x106
+/* enum: RXDP Test firmware image 7 */
+#define          MC_CMD_GET_CAPABILITIES_V3_OUT_RXDP_TEST_FW_RX_HDR_SPLIT  0x107
+/* enum: RXDP Test firmware image 8 */
+#define          MC_CMD_GET_CAPABILITIES_V3_OUT_RXDP_TEST_FW_DISABLE_DL  0x108
+/* enum: RXDP Test firmware image 9 */
+#define          MC_CMD_GET_CAPABILITIES_V3_OUT_RXDP_TEST_FW_DOORBELL_DELAY  0x10b
+/* TxDPCPU firmware id. */
+#define       MC_CMD_GET_CAPABILITIES_V3_OUT_TX_DPCPU_FW_ID_OFST 6
+#define       MC_CMD_GET_CAPABILITIES_V3_OUT_TX_DPCPU_FW_ID_LEN 2
+/* enum: Standard TXDP firmware */
+#define          MC_CMD_GET_CAPABILITIES_V3_OUT_TXDP  0x0
+/* enum: Low latency TXDP firmware */
+#define          MC_CMD_GET_CAPABILITIES_V3_OUT_TXDP_LOW_LATENCY  0x1
+/* enum: High packet rate TXDP firmware */
+#define          MC_CMD_GET_CAPABILITIES_V3_OUT_TXDP_HIGH_PACKET_RATE  0x3
+/* enum: BIST TXDP firmware */
+#define          MC_CMD_GET_CAPABILITIES_V3_OUT_TXDP_BIST  0x12d
+/* enum: TXDP Test firmware image 1 */
+#define          MC_CMD_GET_CAPABILITIES_V3_OUT_TXDP_TEST_FW_TSO_EDIT  0x101
+/* enum: TXDP Test firmware image 2 */
+#define          MC_CMD_GET_CAPABILITIES_V3_OUT_TXDP_TEST_FW_PACKET_EDITS  0x102
+/* enum: TXDP CSR bus test firmware */
+#define          MC_CMD_GET_CAPABILITIES_V3_OUT_TXDP_TEST_FW_CSR  0x103
+#define       MC_CMD_GET_CAPABILITIES_V3_OUT_RXPD_FW_VERSION_OFST 8
+#define       MC_CMD_GET_CAPABILITIES_V3_OUT_RXPD_FW_VERSION_LEN 2
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_RXPD_FW_VERSION_REV_LBN 0
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_RXPD_FW_VERSION_REV_WIDTH 12
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_RXPD_FW_VERSION_TYPE_LBN 12
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_RXPD_FW_VERSION_TYPE_WIDTH 4
+/* enum: reserved value - do not use (may indicate alternative interpretation
+ * of REV field in future)
+ */
+#define          MC_CMD_GET_CAPABILITIES_V3_OUT_RXPD_FW_TYPE_RESERVED  0x0
+/* enum: Trivial RX PD firmware for early Huntington development (Huntington
+ * development only)
+ */
+#define          MC_CMD_GET_CAPABILITIES_V3_OUT_RXPD_FW_TYPE_FIRST_PKT  0x1
+/* enum: RX PD firmware with approximately Siena-compatible behaviour
+ * (Huntington development only)
+ */
+#define          MC_CMD_GET_CAPABILITIES_V3_OUT_RXPD_FW_TYPE_SIENA_COMPAT  0x2
+/* enum: Virtual switching (full feature) RX PD production firmware */
+#define          MC_CMD_GET_CAPABILITIES_V3_OUT_RXPD_FW_TYPE_VSWITCH  0x3
+/* enum: siena_compat variant RX PD firmware using PM rather than MAC
+ * (Huntington development only)
+ */
+#define          MC_CMD_GET_CAPABILITIES_V3_OUT_RXPD_FW_TYPE_SIENA_COMPAT_PM  0x4
+/* enum: Low latency RX PD production firmware */
+#define          MC_CMD_GET_CAPABILITIES_V3_OUT_RXPD_FW_TYPE_LOW_LATENCY  0x5
+/* enum: Packed stream RX PD production firmware */
+#define          MC_CMD_GET_CAPABILITIES_V3_OUT_RXPD_FW_TYPE_PACKED_STREAM  0x6
+/* enum: RX PD firmware handling layer 2 only for high packet rate performance
+ * tests (Medford development only)
+ */
+#define          MC_CMD_GET_CAPABILITIES_V3_OUT_RXPD_FW_TYPE_LAYER2_PERF  0x7
+/* enum: Rules engine RX PD production firmware */
+#define          MC_CMD_GET_CAPABILITIES_V3_OUT_RXPD_FW_TYPE_RULES_ENGINE  0x8
+/* enum: RX PD firmware for GUE parsing prototype (Medford development only) */
+#define          MC_CMD_GET_CAPABILITIES_V3_OUT_RXPD_FW_TYPE_TESTFW_GUE_PROTOTYPE  0xe
+/* enum: RX PD firmware parsing but not filtering network overlay tunnel
+ * encapsulations (Medford development only)
+ */
+#define          MC_CMD_GET_CAPABILITIES_V3_OUT_RXPD_FW_TYPE_TESTFW_ENCAP_PARSING_ONLY  0xf
+#define       MC_CMD_GET_CAPABILITIES_V3_OUT_TXPD_FW_VERSION_OFST 10
+#define       MC_CMD_GET_CAPABILITIES_V3_OUT_TXPD_FW_VERSION_LEN 2
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_TXPD_FW_VERSION_REV_LBN 0
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_TXPD_FW_VERSION_REV_WIDTH 12
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_TXPD_FW_VERSION_TYPE_LBN 12
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_TXPD_FW_VERSION_TYPE_WIDTH 4
+/* enum: reserved value - do not use (may indicate alternative interpretation
+ * of REV field in future)
+ */
+#define          MC_CMD_GET_CAPABILITIES_V3_OUT_TXPD_FW_TYPE_RESERVED  0x0
+/* enum: Trivial TX PD firmware for early Huntington development (Huntington
+ * development only)
+ */
+#define          MC_CMD_GET_CAPABILITIES_V3_OUT_TXPD_FW_TYPE_FIRST_PKT  0x1
+/* enum: TX PD firmware with approximately Siena-compatible behaviour
+ * (Huntington development only)
+ */
+#define          MC_CMD_GET_CAPABILITIES_V3_OUT_TXPD_FW_TYPE_SIENA_COMPAT  0x2
+/* enum: Virtual switching (full feature) TX PD production firmware */
+#define          MC_CMD_GET_CAPABILITIES_V3_OUT_TXPD_FW_TYPE_VSWITCH  0x3
+/* enum: siena_compat variant TX PD firmware using PM rather than MAC
+ * (Huntington development only)
+ */
+#define          MC_CMD_GET_CAPABILITIES_V3_OUT_TXPD_FW_TYPE_SIENA_COMPAT_PM  0x4
+#define          MC_CMD_GET_CAPABILITIES_V3_OUT_TXPD_FW_TYPE_LOW_LATENCY  0x5 /* enum */
+/* enum: TX PD firmware handling layer 2 only for high packet rate performance
+ * tests (Medford development only)
+ */
+#define          MC_CMD_GET_CAPABILITIES_V3_OUT_TXPD_FW_TYPE_LAYER2_PERF  0x7
+/* enum: Rules engine TX PD production firmware */
+#define          MC_CMD_GET_CAPABILITIES_V3_OUT_TXPD_FW_TYPE_RULES_ENGINE  0x8
+/* enum: RX PD firmware for GUE parsing prototype (Medford development only) */
+#define          MC_CMD_GET_CAPABILITIES_V3_OUT_TXPD_FW_TYPE_TESTFW_GUE_PROTOTYPE  0xe
+/* Hardware capabilities of NIC */
+#define       MC_CMD_GET_CAPABILITIES_V3_OUT_HW_CAPABILITIES_OFST 12
+/* Licensed capabilities */
+#define       MC_CMD_GET_CAPABILITIES_V3_OUT_LICENSE_CAPABILITIES_OFST 16
+/* Second word of flags. Not present on older firmware (check the length). */
+#define       MC_CMD_GET_CAPABILITIES_V3_OUT_FLAGS2_OFST 20
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_TX_TSO_V2_LBN 0
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_TX_TSO_V2_WIDTH 1
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_TX_TSO_V2_ENCAP_LBN 1
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_TX_TSO_V2_ENCAP_WIDTH 1
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_EVQ_TIMER_CTRL_LBN 2
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_EVQ_TIMER_CTRL_WIDTH 1
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_EVENT_CUT_THROUGH_LBN 3
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_EVENT_CUT_THROUGH_WIDTH 1
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_RX_CUT_THROUGH_LBN 4
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_RX_CUT_THROUGH_WIDTH 1
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_TX_VFIFO_ULL_MODE_LBN 5
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_TX_VFIFO_ULL_MODE_WIDTH 1
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_MAC_STATS_40G_TX_SIZE_BINS_LBN 6
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_MAC_STATS_40G_TX_SIZE_BINS_WIDTH 1
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_INIT_EVQ_V2_LBN 7
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_INIT_EVQ_V2_WIDTH 1
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_TX_MAC_TIMESTAMPING_LBN 8
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_TX_MAC_TIMESTAMPING_WIDTH 1
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_TX_TIMESTAMP_LBN 9
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_TX_TIMESTAMP_WIDTH 1
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_RX_SNIFF_LBN 10
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_RX_SNIFF_WIDTH 1
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_TX_SNIFF_LBN 11
+#define        MC_CMD_GET_CAPABILITIES_V3_OUT_TX_SNIFF_WIDTH 1
+/* Number of FATSOv2 contexts per datapath supported by this NIC. Not present
+ * on older firmware (check the length).
+ */
+#define       MC_CMD_GET_CAPABILITIES_V3_OUT_TX_TSO_V2_N_CONTEXTS_OFST 24
+#define       MC_CMD_GET_CAPABILITIES_V3_OUT_TX_TSO_V2_N_CONTEXTS_LEN 2
+/* One byte per PF containing the number of the external port assigned to this
+ * PF, indexed by PF number. Special values indicate that a PF is either not
+ * present or not assigned.
+ */
+#define       MC_CMD_GET_CAPABILITIES_V3_OUT_PFS_TO_PORTS_ASSIGNMENT_OFST 26
+#define       MC_CMD_GET_CAPABILITIES_V3_OUT_PFS_TO_PORTS_ASSIGNMENT_LEN 1
+#define       MC_CMD_GET_CAPABILITIES_V3_OUT_PFS_TO_PORTS_ASSIGNMENT_NUM 16
+/* enum: The caller is not permitted to access information on this PF. */
+#define          MC_CMD_GET_CAPABILITIES_V3_OUT_ACCESS_NOT_PERMITTED  0xff
+/* enum: PF does not exist. */
+#define          MC_CMD_GET_CAPABILITIES_V3_OUT_PF_NOT_PRESENT  0xfe
+/* enum: PF does exist but is not assigned to any external port. */
+#define          MC_CMD_GET_CAPABILITIES_V3_OUT_PF_NOT_ASSIGNED  0xfd
+/* enum: This value indicates that PF is assigned, but it cannot be expressed
+ * in this field. It is intended for a possible future situation where a more
+ * complex scheme of PFs to ports mapping is being used. The future driver
+ * should look for a new field supporting the new scheme. The current/old
+ * driver should treat this value as PF_NOT_ASSIGNED.
+ */
+#define          MC_CMD_GET_CAPABILITIES_V3_OUT_INCOMPATIBLE_ASSIGNMENT  0xfc
+/* One byte per PF containing the number of its VFs, indexed by PF number. A
+ * special value indicates that a PF is not present.
+ */
+#define       MC_CMD_GET_CAPABILITIES_V3_OUT_NUM_VFS_PER_PF_OFST 42
+#define       MC_CMD_GET_CAPABILITIES_V3_OUT_NUM_VFS_PER_PF_LEN 1
+#define       MC_CMD_GET_CAPABILITIES_V3_OUT_NUM_VFS_PER_PF_NUM 16
+/* enum: The caller is not permitted to access information on this PF. */
+/*               MC_CMD_GET_CAPABILITIES_V3_OUT_ACCESS_NOT_PERMITTED  0xff */
+/* enum: PF does not exist. */
+/*               MC_CMD_GET_CAPABILITIES_V3_OUT_PF_NOT_PRESENT  0xfe */
+/* Number of VIs available for each external port */
+#define       MC_CMD_GET_CAPABILITIES_V3_OUT_NUM_VIS_PER_PORT_OFST 58
+#define       MC_CMD_GET_CAPABILITIES_V3_OUT_NUM_VIS_PER_PORT_LEN 2
+#define       MC_CMD_GET_CAPABILITIES_V3_OUT_NUM_VIS_PER_PORT_NUM 4
+/* Size of RX descriptor cache expressed as binary logarithm The actual size
+ * equals (2 ^ RX_DESC_CACHE_SIZE)
+ */
+#define       MC_CMD_GET_CAPABILITIES_V3_OUT_RX_DESC_CACHE_SIZE_OFST 66
+#define       MC_CMD_GET_CAPABILITIES_V3_OUT_RX_DESC_CACHE_SIZE_LEN 1
+/* Size of TX descriptor cache expressed as binary logarithm The actual size
+ * equals (2 ^ TX_DESC_CACHE_SIZE)
+ */
+#define       MC_CMD_GET_CAPABILITIES_V3_OUT_TX_DESC_CACHE_SIZE_OFST 67
+#define       MC_CMD_GET_CAPABILITIES_V3_OUT_TX_DESC_CACHE_SIZE_LEN 1
+/* Total number of available PIO buffers */
+#define       MC_CMD_GET_CAPABILITIES_V3_OUT_NUM_PIO_BUFFS_OFST 68
+#define       MC_CMD_GET_CAPABILITIES_V3_OUT_NUM_PIO_BUFFS_LEN 2
+/* Size of a single PIO buffer */
+#define       MC_CMD_GET_CAPABILITIES_V3_OUT_SIZE_PIO_BUFF_OFST 70
+#define       MC_CMD_GET_CAPABILITIES_V3_OUT_SIZE_PIO_BUFF_LEN 2
+/* On chips later than Medford the amount of address space assigned to each VI
+ * is configurable. This is a global setting that the driver must query to
+ * discover the VI to address mapping. Cut-through PIO (CTPIO) in not available
+ * with 8k VI windows.
+ */
+#define       MC_CMD_GET_CAPABILITIES_V3_OUT_VI_WINDOW_MODE_OFST 72
+#define       MC_CMD_GET_CAPABILITIES_V3_OUT_VI_WINDOW_MODE_LEN 1
+/* enum: Each VI occupies 8k as on Huntington and Medford. PIO is at offset 4k.
+ * CTPIO is not mapped.
+ */
+#define          MC_CMD_GET_CAPABILITIES_V3_OUT_VI_WINDOW_MODE_8K   0x0
+/* enum: Each VI occupies 16k. PIO is at offset 4k. CTPIO is at offset 12k. */
+#define          MC_CMD_GET_CAPABILITIES_V3_OUT_VI_WINDOW_MODE_16K  0x1
+/* enum: Each VI occupies 64k. PIO is at offset 4k. CTPIO is at offset 12k. */
+#define          MC_CMD_GET_CAPABILITIES_V3_OUT_VI_WINDOW_MODE_64K  0x2
+
 
 /***********************************/
 /* MC_CMD_V2_EXTN
@@ -9026,7 +9434,7 @@
  */
 #define MC_CMD_GET_RXDP_CONFIG 0xc2
 
-#define MC_CMD_0xc2_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+#define MC_CMD_0xc2_PRIVILEGE_CTG SRIOV_CTG_GENERAL
 
 /* MC_CMD_GET_RXDP_CONFIG_IN msgrequest */
 #define    MC_CMD_GET_RXDP_CONFIG_IN_LEN 0
@@ -10125,7 +10533,9 @@
  * that this operation returns a zero-length response
  */
 #define          MC_CMD_LICENSING_V3_IN_OP_UPDATE_LICENSE  0x0
-/* enum: report counts of installed licenses */
+/* enum: report counts of installed licenses Returns EAGAIN if license
+ * processing (updating) has been started but not yet completed.
+ */
 #define          MC_CMD_LICENSING_V3_IN_OP_REPORT_LICENSE  0x1
 
 /* MC_CMD_LICENSING_V3_OUT msgresponse */
@@ -10763,6 +11173,8 @@
 #define          MC_CMD_GET_WORKAROUNDS_OUT_BUG42008 0x20
 /* enum: Bug 26807 features present in firmware (multicast filter chaining) */
 #define          MC_CMD_GET_WORKAROUNDS_OUT_BUG26807 0x40
+/* enum: Bug 61265 work around (broken EVQ TMR writes). */
+#define          MC_CMD_GET_WORKAROUNDS_OUT_BUG61265 0x80
 
 
 /***********************************/
@@ -11280,22 +11692,110 @@
 #define MC_CMD_0x118_PRIVILEGE_CTG SRIOV_CTG_ADMIN
 
 /* MC_CMD_RX_BALANCING_IN msgrequest */
-#define    MC_CMD_RX_BALANCING_IN_LEN 4
+#define    MC_CMD_RX_BALANCING_IN_LEN 16
 /* The RX port whose upconverter table will be modified */
 #define       MC_CMD_RX_BALANCING_IN_PORT_OFST 0
-#define       MC_CMD_RX_BALANCING_IN_PORT_LEN 1
 /* The VLAN priority associated to the table index and vFIFO */
-#define       MC_CMD_RX_BALANCING_IN_PRIORITY_OFST 1
-#define       MC_CMD_RX_BALANCING_IN_PRIORITY_LEN 1
+#define       MC_CMD_RX_BALANCING_IN_PRIORITY_OFST 4
 /* The resulting bit of SRC^DST for indexing the table */
-#define       MC_CMD_RX_BALANCING_IN_SRC_DST_OFST 2
-#define       MC_CMD_RX_BALANCING_IN_SRC_DST_LEN 1
+#define       MC_CMD_RX_BALANCING_IN_SRC_DST_OFST 8
 /* The RX engine to which the vFIFO in the table entry will point to */
-#define       MC_CMD_RX_BALANCING_IN_ENG_OFST 3
-#define       MC_CMD_RX_BALANCING_IN_ENG_LEN 1
+#define       MC_CMD_RX_BALANCING_IN_ENG_OFST 12
 
 /* MC_CMD_RX_BALANCING_OUT msgresponse */
 #define    MC_CMD_RX_BALANCING_OUT_LEN 0
 
+/***********************************/
+/* MC_CMD_SET_EVQ_TMR
+ * Update the timer load, timer reload and timer mode values for a given EVQ.
+ * The requested timer values (in TMR_LOAD_REQ_NS and TMR_RELOAD_REQ_NS) will
+ * be rounded up to the granularity supported by the hardware, then truncated
+ * to the range supported by the hardware. The resulting value after the
+ * rounding and truncation will be returned to the caller (in TMR_LOAD_ACT_NS
+ * and TMR_RELOAD_ACT_NS).
+ */
+#define MC_CMD_SET_EVQ_TMR 0x120
+
+#define MC_CMD_0x120_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
+/* MC_CMD_SET_EVQ_TMR_IN msgrequest */
+#define    MC_CMD_SET_EVQ_TMR_IN_LEN 16
+/* Function-relative queue instance */
+#define       MC_CMD_SET_EVQ_TMR_IN_INSTANCE_OFST 0
+/* Requested value for timer load (in nanoseconds) */
+#define       MC_CMD_SET_EVQ_TMR_IN_TMR_LOAD_REQ_NS_OFST 4
+/* Requested value for timer reload (in nanoseconds) */
+#define       MC_CMD_SET_EVQ_TMR_IN_TMR_RELOAD_REQ_NS_OFST 8
+/* Timer mode. Meanings as per EVQ_TMR_REG.TC_TIMER_VAL */
+#define       MC_CMD_SET_EVQ_TMR_IN_TMR_MODE_OFST 12
+#define          MC_CMD_SET_EVQ_TMR_IN_TIMER_MODE_DIS  0x0 /* enum */
+#define          MC_CMD_SET_EVQ_TMR_IN_TIMER_MODE_IMMED_START  0x1 /* enum */
+#define          MC_CMD_SET_EVQ_TMR_IN_TIMER_MODE_TRIG_START  0x2 /* enum */
+#define          MC_CMD_SET_EVQ_TMR_IN_TIMER_MODE_INT_HLDOFF  0x3 /* enum */
+
+/* MC_CMD_SET_EVQ_TMR_OUT msgresponse */
+#define    MC_CMD_SET_EVQ_TMR_OUT_LEN 8
+/* Actual value for timer load (in nanoseconds) */
+#define       MC_CMD_SET_EVQ_TMR_OUT_TMR_LOAD_ACT_NS_OFST 0
+/* Actual value for timer reload (in nanoseconds) */
+#define       MC_CMD_SET_EVQ_TMR_OUT_TMR_RELOAD_ACT_NS_OFST 4
+
+
+/***********************************/
+/* MC_CMD_GET_EVQ_TMR_PROPERTIES
+ * Query properties about the event queue timers.
+ */
+#define MC_CMD_GET_EVQ_TMR_PROPERTIES 0x122
+
+#define MC_CMD_0x122_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
+/* MC_CMD_GET_EVQ_TMR_PROPERTIES_IN msgrequest */
+#define    MC_CMD_GET_EVQ_TMR_PROPERTIES_IN_LEN 0
+
+/* MC_CMD_GET_EVQ_TMR_PROPERTIES_OUT msgresponse */
+#define    MC_CMD_GET_EVQ_TMR_PROPERTIES_OUT_LEN 36
+/* Reserved for future use. */
+#define       MC_CMD_GET_EVQ_TMR_PROPERTIES_OUT_FLAGS_OFST 0
+/* For timers updated via writes to EVQ_TMR_REG, this is the time interval (in
+ * nanoseconds) for each increment of the timer load/reload count. The
+ * requested duration of a timer is this value multiplied by the timer
+ * load/reload count.
+ */
+#define       MC_CMD_GET_EVQ_TMR_PROPERTIES_OUT_TMR_REG_NS_PER_COUNT_OFST 4
+/* For timers updated via writes to EVQ_TMR_REG, this is the maximum value
+ * allowed for timer load/reload counts.
+ */
+#define       MC_CMD_GET_EVQ_TMR_PROPERTIES_OUT_TMR_REG_MAX_COUNT_OFST 8
+/* For timers updated via writes to EVQ_TMR_REG, timer load/reload counts not a
+ * multiple of this step size will be rounded in an implementation defined
+ * manner.
+ */
+#define       MC_CMD_GET_EVQ_TMR_PROPERTIES_OUT_TMR_REG_STEP_OFST 12
+/* Maximum timer duration (in nanoseconds) for timers updated via MCDI. Only
+ * meaningful if MC_CMD_SET_EVQ_TMR is implemented.
+ */
+#define       MC_CMD_GET_EVQ_TMR_PROPERTIES_OUT_MCDI_TMR_MAX_NS_OFST 16
+/* Timer durations requested via MCDI that are not a multiple of this step size
+ * will be rounded up. Only meaningful if MC_CMD_SET_EVQ_TMR is implemented.
+ */
+#define       MC_CMD_GET_EVQ_TMR_PROPERTIES_OUT_MCDI_TMR_STEP_NS_OFST 20
+/* For timers updated using the bug35388 workaround, this is the time interval
+ * (in nanoseconds) for each increment of the timer load/reload count. The
+ * requested duration of a timer is this value multiplied by the timer
+ * load/reload count. This field is only meaningful if the bug35388 workaround
+ * is enabled.
+ */
+#define       MC_CMD_GET_EVQ_TMR_PROPERTIES_OUT_BUG35388_TMR_NS_PER_COUNT_OFST 24
+/* For timers updated using the bug35388 workaround, this is the maximum value
+ * allowed for timer load/reload counts. This field is only meaningful if the
+ * bug35388 workaround is enabled.
+ */
+#define       MC_CMD_GET_EVQ_TMR_PROPERTIES_OUT_BUG35388_TMR_MAX_COUNT_OFST 28
+/* For timers updated using the bug35388 workaround, timer load/reload counts
+ * not a multiple of this step size will be rounded in an implementation
+ * defined manner. This field is only meaningful if the bug35388 workaround is
+ * enabled.
+ */
+#define       MC_CMD_GET_EVQ_TMR_PROPERTIES_OUT_BUG35388_TMR_STEP_OFST 32
 
 #endif /* MCDI_PCOL_H */
diff --git a/drivers/net/ethernet/sfc/net_driver.h b/drivers/net/ethernet/sfc/net_driver.h
index 9ff062a36ea8..99d8c82124bb 100644
--- a/drivers/net/ethernet/sfc/net_driver.h
+++ b/drivers/net/ethernet/sfc/net_driver.h
@@ -76,6 +76,9 @@
 /* Maximum possible MTU the driver supports */
 #define EFX_MAX_MTU (9 * 1024)
 
+/* Minimum MTU, from RFC791 (IP) */
+#define EFX_MIN_MTU 68
+
 /* Size of an RX scatter buffer.  Small enough to pack 2 into a 4K page,
  * and should be a multiple of the cache line size.
  */
@@ -392,7 +395,7 @@ enum efx_sync_events_state {
  * @eventq_init: Event queue initialised flag
  * @enabled: Channel enabled indicator
  * @irq: IRQ number (MSI and MSI-X only)
- * @irq_moderation: IRQ moderation value (in hardware ticks)
+ * @irq_moderation_us: IRQ moderation value (in microseconds)
  * @napi_dev: Net device used with NAPI
  * @napi_str: NAPI control structure
  * @state: state for NAPI vs busy polling
@@ -433,7 +436,7 @@ struct efx_channel {
 	bool eventq_init;
 	bool enabled;
 	int irq;
-	unsigned int irq_moderation;
+	unsigned int irq_moderation_us;
 	struct net_device *napi_dev;
 	struct napi_struct napi_str;
 #ifdef CONFIG_NET_RX_BUSY_POLL
@@ -810,8 +813,10 @@ struct vfdi_status;
  * @membase: Memory BAR value
  * @interrupt_mode: Interrupt mode
  * @timer_quantum_ns: Interrupt timer quantum, in nanoseconds
+ * @timer_max_ns: Interrupt timer maximum value, in nanoseconds
  * @irq_rx_adaptive: Adaptive IRQ moderation enabled for RX event queues
- * @irq_rx_moderation: IRQ moderation time for RX event queues
+ * @irq_rx_mod_step_us: Step size for IRQ moderation for RX event queues
+ * @irq_rx_moderation_us: IRQ moderation time for RX event queues
  * @msg_enable: Log message enable flags
  * @state: Device state number (%STATE_*). Serialised by the rtnl_lock.
  * @reset_pending: Bitmask for pending resets
@@ -940,8 +945,10 @@ struct efx_nic {
 
 	enum efx_int_mode interrupt_mode;
 	unsigned int timer_quantum_ns;
+	unsigned int timer_max_ns;
 	bool irq_rx_adaptive;
-	unsigned int irq_rx_moderation;
+	unsigned int irq_mod_step_us;
+	unsigned int irq_rx_moderation_us;
 	u32 msg_enable;
 
 	enum nic_state state;
@@ -1271,7 +1278,7 @@ struct efx_nic_type {
 	int (*mcdi_poll_reboot)(struct efx_nic *efx);
 	void (*mcdi_reboot_detected)(struct efx_nic *efx);
 	void (*irq_enable_master)(struct efx_nic *efx);
-	void (*irq_test_generate)(struct efx_nic *efx);
+	int (*irq_test_generate)(struct efx_nic *efx);
 	void (*irq_disable_non_ev)(struct efx_nic *efx);
 	irqreturn_t (*irq_handle_msi)(int irq, void *dev_id);
 	irqreturn_t (*irq_handle_legacy)(int irq, void *dev_id);
diff --git a/drivers/net/ethernet/sfc/nic.c b/drivers/net/ethernet/sfc/nic.c
index 89b83e59e1dc..aa1945a858d5 100644
--- a/drivers/net/ethernet/sfc/nic.c
+++ b/drivers/net/ethernet/sfc/nic.c
@@ -66,11 +66,11 @@ void efx_nic_event_test_start(struct efx_channel *channel)
 	channel->efx->type->ev_test_generate(channel);
 }
 
-void efx_nic_irq_test_start(struct efx_nic *efx)
+int efx_nic_irq_test_start(struct efx_nic *efx)
 {
 	efx->last_irq_cpu = -1;
 	smp_wmb();
-	efx->type->irq_test_generate(efx);
+	return efx->type->irq_test_generate(efx);
 }
 
 /* Hook interrupt handler(s)
diff --git a/drivers/net/ethernet/sfc/nic.h b/drivers/net/ethernet/sfc/nic.h
index 96944c3c9d14..73bee7ea332a 100644
--- a/drivers/net/ethernet/sfc/nic.h
+++ b/drivers/net/ethernet/sfc/nic.h
@@ -507,10 +507,13 @@ enum {
  * @stats: Hardware statistics
  * @workaround_35388: Flag: firmware supports workaround for bug 35388
  * @workaround_26807: Flag: firmware supports workaround for bug 26807
+ * @workaround_61265: Flag: firmware supports workaround for bug 61265
  * @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)
+ * @datapath_caps2: Further Capabilities of datapath firmware (FLAGS2 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
@@ -540,8 +543,10 @@ struct efx_ef10_nic_data {
 	u64 stats[EF10_STAT_COUNT];
 	bool workaround_35388;
 	bool workaround_26807;
+	bool workaround_61265;
 	bool must_check_datapath_caps;
 	u32 datapath_caps;
+	u32 datapath_caps2;
 	unsigned int rx_dpcpu_fw_id;
 	unsigned int tx_dpcpu_fw_id;
 	unsigned int vport_id;
@@ -741,12 +746,12 @@ static inline void efx_update_diff_stat(u64 *stat, u64 diff)
 
 /* Interrupts */
 int efx_nic_init_interrupt(struct efx_nic *efx);
-void efx_nic_irq_test_start(struct efx_nic *efx);
+int efx_nic_irq_test_start(struct efx_nic *efx);
 void efx_nic_fini_interrupt(struct efx_nic *efx);
 
 /* Falcon/Siena interrupts */
 void efx_farch_irq_enable_master(struct efx_nic *efx);
-void efx_farch_irq_test_generate(struct efx_nic *efx);
+int efx_farch_irq_test_generate(struct efx_nic *efx);
 void efx_farch_irq_disable_master(struct efx_nic *efx);
 irqreturn_t efx_farch_msi_interrupt(int irq, void *dev_id);
 irqreturn_t efx_farch_legacy_interrupt(int irq, void *dev_id);
diff --git a/drivers/net/ethernet/sfc/ptp.c b/drivers/net/ethernet/sfc/ptp.c
index c771e0af4e06..77a5364f7a10 100644
--- a/drivers/net/ethernet/sfc/ptp.c
+++ b/drivers/net/ethernet/sfc/ptp.c
@@ -1269,13 +1269,13 @@ int efx_ptp_probe(struct efx_nic *efx, struct efx_channel *channel)
 		if (IS_ERR(ptp->phc_clock)) {
 			rc = PTR_ERR(ptp->phc_clock);
 			goto fail3;
-		}
-
-		INIT_WORK(&ptp->pps_work, efx_ptp_pps_worker);
-		ptp->pps_workwq = create_singlethread_workqueue("sfc_pps");
-		if (!ptp->pps_workwq) {
-			rc = -ENOMEM;
-			goto fail4;
+		} else if (ptp->phc_clock) {
+			INIT_WORK(&ptp->pps_work, efx_ptp_pps_worker);
+			ptp->pps_workwq = create_singlethread_workqueue("sfc_pps");
+			if (!ptp->pps_workwq) {
+				rc = -ENOMEM;
+				goto fail4;
+			}
 		}
 	}
 	ptp->nic_ts_enabled = false;
@@ -1306,7 +1306,7 @@ static int efx_ptp_probe_channel(struct efx_channel *channel)
 {
 	struct efx_nic *efx = channel->efx;
 
-	channel->irq_moderation = 0;
+	channel->irq_moderation_us = 0;
 	channel->rx_queue.core_index = 0;
 
 	return efx_ptp_probe(efx, channel);
diff --git a/drivers/net/ethernet/sfc/selftest.c b/drivers/net/ethernet/sfc/selftest.c
index 9d78830da609..cd38b44ae23a 100644
--- a/drivers/net/ethernet/sfc/selftest.c
+++ b/drivers/net/ethernet/sfc/selftest.c
@@ -135,11 +135,19 @@ static int efx_test_interrupts(struct efx_nic *efx,
 {
 	unsigned long timeout, wait;
 	int cpu;
+	int rc;
 
 	netif_dbg(efx, drv, efx->net_dev, "testing interrupts\n");
 	tests->interrupt = -1;
 
-	efx_nic_irq_test_start(efx);
+	rc = efx_nic_irq_test_start(efx);
+	if (rc == -ENOTSUPP) {
+		netif_dbg(efx, drv, efx->net_dev,
+			  "direct interrupt testing not supported\n");
+		tests->interrupt = 0;
+		return 0;
+	}
+
 	timeout = jiffies + IRQ_TIMEOUT;
 	wait = 1;
 
diff --git a/drivers/net/ethernet/sfc/selftest.h b/drivers/net/ethernet/sfc/selftest.h
index 009dbe88f3be..32a427253a03 100644
--- a/drivers/net/ethernet/sfc/selftest.h
+++ b/drivers/net/ethernet/sfc/selftest.h
@@ -28,7 +28,7 @@ struct efx_loopback_self_tests {
 
 /* Efx self test results
  * For fields which are not counters, 1 indicates success and -1
- * indicates failure.
+ * indicates failure; 0 indicates test could not be run.
  */
 struct efx_self_tests {
 	/* online tests */
diff --git a/drivers/net/ethernet/sfc/siena.c b/drivers/net/ethernet/sfc/siena.c
index 2219b5424d2b..04ed1b4c7cd9 100644
--- a/drivers/net/ethernet/sfc/siena.c
+++ b/drivers/net/ethernet/sfc/siena.c
@@ -34,19 +34,24 @@ static void siena_init_wol(struct efx_nic *efx);
 
 static void siena_push_irq_moderation(struct efx_channel *channel)
 {
+	struct efx_nic *efx = channel->efx;
 	efx_dword_t timer_cmd;
 
-	if (channel->irq_moderation)
+	if (channel->irq_moderation_us) {
+		unsigned int ticks;
+
+		ticks = efx_usecs_to_ticks(efx, channel->irq_moderation_us);
 		EFX_POPULATE_DWORD_2(timer_cmd,
 				     FRF_CZ_TC_TIMER_MODE,
 				     FFE_CZ_TIMER_MODE_INT_HLDOFF,
 				     FRF_CZ_TC_TIMER_VAL,
-				     channel->irq_moderation - 1);
-	else
+				     ticks - 1);
+	} else {
 		EFX_POPULATE_DWORD_2(timer_cmd,
 				     FRF_CZ_TC_TIMER_MODE,
 				     FFE_CZ_TIMER_MODE_DIS,
 				     FRF_CZ_TC_TIMER_VAL, 0);
+	}
 	efx_writed_page_locked(channel->efx, &timer_cmd, FR_BZ_TIMER_COMMAND_P0,
 			       channel->channel);
 }
@@ -222,6 +227,9 @@ static int siena_probe_nvconfig(struct efx_nic *efx)
 	efx->timer_quantum_ns =
 		(caps & (1 << MC_CMD_CAPABILITIES_TURBO_ACTIVE_LBN)) ?
 		3072 : 6144; /* 768 cycles */
+	efx->timer_max_ns = efx->type->timer_period_max *
+			    efx->timer_quantum_ns;
+
 	return rc;
 }
 
diff --git a/drivers/net/ethernet/sfc/sriov.c b/drivers/net/ethernet/sfc/sriov.c
index 816c44689e67..9abcf4aded30 100644
--- a/drivers/net/ethernet/sfc/sriov.c
+++ b/drivers/net/ethernet/sfc/sriov.c
@@ -22,7 +22,7 @@ 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)
+			  u8 qos, __be16 vlan_proto)
 {
 	struct efx_nic *efx = netdev_priv(net_dev);
 
@@ -31,6 +31,9 @@ int efx_sriov_set_vf_vlan(struct net_device *net_dev, int vf_i, u16 vlan,
 		    (qos & ~(VLAN_PRIO_MASK >> VLAN_PRIO_SHIFT)))
 			return -EINVAL;
 
+		if (vlan_proto != htons(ETH_P_8021Q))
+			return -EPROTONOSUPPORT;
+
 		return efx->type->sriov_set_vf_vlan(efx, vf_i, vlan, qos);
 	} else {
 		return -EOPNOTSUPP;
diff --git a/drivers/net/ethernet/sfc/sriov.h b/drivers/net/ethernet/sfc/sriov.h
index 400df526586d..ba1762e7f216 100644
--- a/drivers/net/ethernet/sfc/sriov.h
+++ b/drivers/net/ethernet/sfc/sriov.h
@@ -16,7 +16,7 @@
 
 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);
+			  u8 qos, __be16 vlan_proto);
 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,
diff --git a/drivers/net/ethernet/sfc/workarounds.h b/drivers/net/ethernet/sfc/workarounds.h
index 2310b75d4ec2..351cd14cb9f9 100644
--- a/drivers/net/ethernet/sfc/workarounds.h
+++ b/drivers/net/ethernet/sfc/workarounds.h
@@ -50,4 +50,8 @@
 #define EFX_WORKAROUND_35388(efx)					\
 	(efx_nic_rev(efx) == EFX_REV_HUNT_A0 && EFX_EF10_WORKAROUND_35388(efx))
 
+/* Moderation timer access must go through MCDI */
+#define EFX_EF10_WORKAROUND_61265(efx)					\
+	(((struct efx_ef10_nic_data *)efx->nic_data)->workaround_61265)
+
 #endif /* EFX_WORKAROUNDS_H */
diff --git a/drivers/net/ethernet/sis/sis900.c b/drivers/net/ethernet/sis/sis900.c
index 95001ee408ab..6f85276376e8 100644
--- a/drivers/net/ethernet/sis/sis900.c
+++ b/drivers/net/ethernet/sis/sis900.c
@@ -1426,7 +1426,7 @@ static void sis900_set_mode(struct sis900_private *sp, int speed, int duplex)
 		rx_flags |= RxATX;
 	}
 
-#if defined(CONFIG_VLAN_8021Q) || defined(CONFIG_VLAN_8021Q_MODULE)
+#if IS_ENABLED(CONFIG_VLAN_8021Q)
 	/* Can accept Jumbo packet */
 	rx_flags |= RxAJAB;
 #endif
@@ -1750,7 +1750,7 @@ static int sis900_rx(struct net_device *net_dev)
 		data_size = rx_status & DSIZE;
 		rx_size = data_size - CRC_SIZE;
 
-#if defined(CONFIG_VLAN_8021Q) || defined(CONFIG_VLAN_8021Q_MODULE)
+#if IS_ENABLED(CONFIG_VLAN_8021Q)
 		/* ``TOOLONG'' flag means jumbo packet received. */
 		if ((rx_status & TOOLONG) && data_size <= MAX_FRAME_SIZE)
 			rx_status &= (~ ((unsigned int)TOOLONG));
diff --git a/drivers/net/ethernet/sis/sis900.h b/drivers/net/ethernet/sis/sis900.h
index 7d430d322931..f0da3dc52c01 100644
--- a/drivers/net/ethernet/sis/sis900.h
+++ b/drivers/net/ethernet/sis/sis900.h
@@ -310,7 +310,7 @@ enum sis630_revision_id {
 #define CRC_SIZE                4
 #define MAC_HEADER_SIZE         14
 
-#if defined(CONFIG_VLAN_8021Q) || defined(CONFIG_VLAN_8021Q_MODULE)
+#if IS_ENABLED(CONFIG_VLAN_8021Q)
 #define MAX_FRAME_SIZE  (1518 + 4)
 #else
 #define MAX_FRAME_SIZE  1518
diff --git a/drivers/net/ethernet/smsc/smc91x.c b/drivers/net/ethernet/smsc/smc91x.c
index 503a3b6dce91..73212590d04a 100644
--- a/drivers/net/ethernet/smsc/smc91x.c
+++ b/drivers/net/ethernet/smsc/smc91x.c
@@ -2323,6 +2323,9 @@ static int smc_drv_probe(struct platform_device *pdev)
 		} else {
 			lp->cfg.flags |= SMC91X_USE_16BIT;
 		}
+		if (!device_property_read_u32(&pdev->dev, "reg-shift",
+					      &val))
+			lp->io_shift = val;
 	}
 #endif
 
diff --git a/drivers/net/ethernet/smsc/smsc911x.c b/drivers/net/ethernet/smsc/smsc911x.c
index 4f8910b7db2e..e9b8579e6241 100644
--- a/drivers/net/ethernet/smsc/smsc911x.c
+++ b/drivers/net/ethernet/smsc/smsc911x.c
@@ -62,6 +62,7 @@
 #include <linux/acpi.h>
 #include <linux/pm_runtime.h>
 #include <linux/property.h>
+#include <linux/gpio/consumer.h>
 
 #include "smsc911x.h"
 
@@ -147,6 +148,9 @@ struct smsc911x_data {
 	/* regulators */
 	struct regulator_bulk_data supplies[SMSC911X_NUM_SUPPLIES];
 
+	/* Reset GPIO */
+	struct gpio_desc *reset_gpiod;
+
 	/* clock */
 	struct clk *clk;
 };
@@ -438,6 +442,11 @@ static int smsc911x_request_resources(struct platform_device *pdev)
 		netdev_err(ndev, "couldn't get regulators %d\n",
 				ret);
 
+	/* Request optional RESET GPIO */
+	pdata->reset_gpiod = devm_gpiod_get_optional(&pdev->dev,
+						     "reset",
+						     GPIOD_OUT_LOW);
+
 	/* Request clock */
 	pdata->clk = clk_get(&pdev->dev, NULL);
 	if (IS_ERR(pdata->clk))
diff --git a/drivers/net/ethernet/stmicro/stmmac/Kconfig b/drivers/net/ethernet/stmicro/stmmac/Kconfig
index 8f06a6621ab1..3818c5e06eba 100644
--- a/drivers/net/ethernet/stmicro/stmmac/Kconfig
+++ b/drivers/net/ethernet/stmicro/stmmac/Kconfig
@@ -61,13 +61,13 @@ config DWMAC_LPC18XX
 config DWMAC_MESON
 	tristate "Amlogic Meson dwmac support"
 	default ARCH_MESON
-	depends on OF && (ARCH_MESON || COMPILE_TEST)
+	depends on OF && COMMON_CLK && (ARCH_MESON || COMPILE_TEST)
 	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.
+	  the stmmac device driver. This driver is used for Meson6,
+	  Meson8, Meson8b and GXBB SoCs.
 
 config DWMAC_ROCKCHIP
 	tristate "Rockchip dwmac support"
@@ -104,6 +104,18 @@ config DWMAC_STI
 	  device driver. This driver is used on for the STi series
 	  SOCs GMAC ethernet controller.
 
+config DWMAC_STM32
+	tristate "STM32 DWMAC support"
+	default ARCH_STM32
+	depends on OF && HAS_IOMEM
+	select MFD_SYSCON
+	---help---
+	  Support for ethernet controller on STM32 SOCs.
+
+	  This selects STM32 SoC glue layer support for the stmmac
+	  device driver. This driver is used on for the STM32 series
+	  SOCs GMAC ethernet controller.
+
 config DWMAC_SUNXI
 	tristate "Allwinner GMAC support"
 	default ARCH_SUNXI
diff --git a/drivers/net/ethernet/stmicro/stmmac/Makefile b/drivers/net/ethernet/stmicro/stmmac/Makefile
index 44b630cd1755..5d6ece5919b3 100644
--- a/drivers/net/ethernet/stmicro/stmmac/Makefile
+++ b/drivers/net/ethernet/stmicro/stmmac/Makefile
@@ -9,10 +9,11 @@ stmmac-objs:= stmmac_main.o stmmac_ethtool.o stmmac_mdio.o ring_mode.o	\
 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_MESON)	+= dwmac-meson.o dwmac-meson8b.o
 obj-$(CONFIG_DWMAC_ROCKCHIP)	+= dwmac-rk.o
 obj-$(CONFIG_DWMAC_SOCFPGA)	+= dwmac-altr-socfpga.o
 obj-$(CONFIG_DWMAC_STI)		+= dwmac-sti.o
+obj-$(CONFIG_DWMAC_STM32)	+= dwmac-stm32.o
 obj-$(CONFIG_DWMAC_SUNXI)	+= dwmac-sunxi.o
 obj-$(CONFIG_DWMAC_GENERIC)	+= dwmac-generic.o
 stmmac-platform-objs:= stmmac_platform.o
diff --git a/drivers/net/ethernet/stmicro/stmmac/common.h b/drivers/net/ethernet/stmicro/stmmac/common.h
index 2533b91f1421..d3292c4a6eda 100644
--- a/drivers/net/ethernet/stmicro/stmmac/common.h
+++ b/drivers/net/ethernet/stmicro/stmmac/common.h
@@ -30,7 +30,7 @@
 #include <linux/stmmac.h>
 #include <linux/phy.h>
 #include <linux/module.h>
-#if defined(CONFIG_VLAN_8021Q) || defined(CONFIG_VLAN_8021Q_MODULE)
+#if IS_ENABLED(CONFIG_VLAN_8021Q)
 #define STMMAC_VLAN_TAG_USED
 #include <linux/if_vlan.h>
 #endif
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac-meson.c b/drivers/net/ethernet/stmicro/stmmac/dwmac-meson.c
index c1bac1912b37..309d99536a2c 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac-meson.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-meson.c
@@ -1,5 +1,5 @@
 /*
- * Amlogic Meson DWMAC glue layer
+ * Amlogic Meson6 and Meson8 DWMAC glue layer
  *
  * Copyright (C) 2014 Beniamino Galvani <b.galvani@gmail.com>
  *
@@ -96,5 +96,5 @@ static struct platform_driver meson6_dwmac_driver = {
 module_platform_driver(meson6_dwmac_driver);
 
 MODULE_AUTHOR("Beniamino Galvani <b.galvani@gmail.com>");
-MODULE_DESCRIPTION("Amlogic Meson DWMAC glue layer");
+MODULE_DESCRIPTION("Amlogic Meson6 and Meson8 DWMAC glue layer");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac-meson8b.c b/drivers/net/ethernet/stmicro/stmmac/dwmac-meson8b.c
new file mode 100644
index 000000000000..250e4ceafc8d
--- /dev/null
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-meson8b.c
@@ -0,0 +1,324 @@
+/*
+ * Amlogic Meson8b and GXBB DWMAC glue layer
+ *
+ * Copyright (C) 2016 Martin Blumenstingl <martin.blumenstingl@googlemail.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.
+ *
+ * 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/device.h>
+#include <linux/ethtool.h>
+#include <linux/io.h>
+#include <linux/ioport.h>
+#include <linux/module.h>
+#include <linux/of_net.h>
+#include <linux/mfd/syscon.h>
+#include <linux/platform_device.h>
+#include <linux/stmmac.h>
+
+#include "stmmac_platform.h"
+
+#define PRG_ETH0			0x0
+
+#define PRG_ETH0_RGMII_MODE		BIT(0)
+
+/* mux to choose between fclk_div2 (bit unset) and mpll2 (bit set) */
+#define PRG_ETH0_CLK_M250_SEL_SHIFT	4
+#define PRG_ETH0_CLK_M250_SEL_MASK	GENMASK(4, 4)
+
+#define PRG_ETH0_TXDLY_SHIFT		5
+#define PRG_ETH0_TXDLY_MASK		GENMASK(6, 5)
+#define PRG_ETH0_TXDLY_OFF		(0x0 << PRG_ETH0_TXDLY_SHIFT)
+#define PRG_ETH0_TXDLY_QUARTER		(0x1 << PRG_ETH0_TXDLY_SHIFT)
+#define PRG_ETH0_TXDLY_HALF		(0x2 << PRG_ETH0_TXDLY_SHIFT)
+#define PRG_ETH0_TXDLY_THREE_QUARTERS	(0x3 << PRG_ETH0_TXDLY_SHIFT)
+
+/* divider for the result of m250_sel */
+#define PRG_ETH0_CLK_M250_DIV_SHIFT	7
+#define PRG_ETH0_CLK_M250_DIV_WIDTH	3
+
+/* divides the result of m25_sel by either 5 (bit unset) or 10 (bit set) */
+#define PRG_ETH0_CLK_M25_DIV_SHIFT	10
+#define PRG_ETH0_CLK_M25_DIV_WIDTH	1
+
+#define PRG_ETH0_INVERTED_RMII_CLK	BIT(11)
+#define PRG_ETH0_TX_AND_PHY_REF_CLK	BIT(12)
+
+#define MUX_CLK_NUM_PARENTS		2
+
+struct meson8b_dwmac {
+	struct platform_device	*pdev;
+
+	void __iomem		*regs;
+
+	phy_interface_t		phy_mode;
+
+	struct clk_mux		m250_mux;
+	struct clk		*m250_mux_clk;
+	struct clk		*m250_mux_parent[MUX_CLK_NUM_PARENTS];
+
+	struct clk_divider	m250_div;
+	struct clk		*m250_div_clk;
+
+	struct clk_divider	m25_div;
+	struct clk		*m25_div_clk;
+};
+
+static void meson8b_dwmac_mask_bits(struct meson8b_dwmac *dwmac, u32 reg,
+				    u32 mask, u32 value)
+{
+	u32 data;
+
+	data = readl(dwmac->regs + reg);
+	data &= ~mask;
+	data |= (value & mask);
+
+	writel(data, dwmac->regs + reg);
+}
+
+static int meson8b_init_clk(struct meson8b_dwmac *dwmac)
+{
+	struct clk_init_data init;
+	int i, ret;
+	struct device *dev = &dwmac->pdev->dev;
+	char clk_name[32];
+	const char *clk_div_parents[1];
+	const char *mux_parent_names[MUX_CLK_NUM_PARENTS];
+	static struct clk_div_table clk_25m_div_table[] = {
+		{ .val = 0, .div = 5 },
+		{ .val = 1, .div = 10 },
+		{ /* sentinel */ },
+	};
+
+	/* get the mux parents from DT */
+	for (i = 0; i < MUX_CLK_NUM_PARENTS; i++) {
+		char name[16];
+
+		snprintf(name, sizeof(name), "clkin%d", i);
+		dwmac->m250_mux_parent[i] = devm_clk_get(dev, name);
+		if (IS_ERR(dwmac->m250_mux_parent[i])) {
+			ret = PTR_ERR(dwmac->m250_mux_parent[i]);
+			if (ret != -EPROBE_DEFER)
+				dev_err(dev, "Missing clock %s\n", name);
+			return ret;
+		}
+
+		mux_parent_names[i] =
+			__clk_get_name(dwmac->m250_mux_parent[i]);
+	}
+
+	/* create the m250_mux */
+	snprintf(clk_name, sizeof(clk_name), "%s#m250_sel", dev_name(dev));
+	init.name = clk_name;
+	init.ops = &clk_mux_ops;
+	init.flags = 0;
+	init.parent_names = mux_parent_names;
+	init.num_parents = MUX_CLK_NUM_PARENTS;
+
+	dwmac->m250_mux.reg = dwmac->regs + PRG_ETH0;
+	dwmac->m250_mux.shift = PRG_ETH0_CLK_M250_SEL_SHIFT;
+	dwmac->m250_mux.mask = PRG_ETH0_CLK_M250_SEL_MASK;
+	dwmac->m250_mux.flags = 0;
+	dwmac->m250_mux.table = NULL;
+	dwmac->m250_mux.hw.init = &init;
+
+	dwmac->m250_mux_clk = devm_clk_register(dev, &dwmac->m250_mux.hw);
+	if (WARN_ON(IS_ERR(dwmac->m250_mux_clk)))
+		return PTR_ERR(dwmac->m250_mux_clk);
+
+	/* create the m250_div */
+	snprintf(clk_name, sizeof(clk_name), "%s#m250_div", dev_name(dev));
+	init.name = devm_kstrdup(dev, clk_name, GFP_KERNEL);
+	init.ops = &clk_divider_ops;
+	init.flags = CLK_SET_RATE_PARENT;
+	clk_div_parents[0] = __clk_get_name(dwmac->m250_mux_clk);
+	init.parent_names = clk_div_parents;
+	init.num_parents = ARRAY_SIZE(clk_div_parents);
+
+	dwmac->m250_div.reg = dwmac->regs + PRG_ETH0;
+	dwmac->m250_div.shift = PRG_ETH0_CLK_M250_DIV_SHIFT;
+	dwmac->m250_div.width = PRG_ETH0_CLK_M250_DIV_WIDTH;
+	dwmac->m250_div.hw.init = &init;
+	dwmac->m250_div.flags = CLK_DIVIDER_ONE_BASED | CLK_DIVIDER_ALLOW_ZERO;
+
+	dwmac->m250_div_clk = devm_clk_register(dev, &dwmac->m250_div.hw);
+	if (WARN_ON(IS_ERR(dwmac->m250_div_clk)))
+		return PTR_ERR(dwmac->m250_div_clk);
+
+	/* create the m25_div */
+	snprintf(clk_name, sizeof(clk_name), "%s#m25_div", dev_name(dev));
+	init.name = devm_kstrdup(dev, clk_name, GFP_KERNEL);
+	init.ops = &clk_divider_ops;
+	init.flags = CLK_IS_BASIC | CLK_SET_RATE_PARENT;
+	clk_div_parents[0] = __clk_get_name(dwmac->m250_div_clk);
+	init.parent_names = clk_div_parents;
+	init.num_parents = ARRAY_SIZE(clk_div_parents);
+
+	dwmac->m25_div.reg = dwmac->regs + PRG_ETH0;
+	dwmac->m25_div.shift = PRG_ETH0_CLK_M25_DIV_SHIFT;
+	dwmac->m25_div.width = PRG_ETH0_CLK_M25_DIV_WIDTH;
+	dwmac->m25_div.table = clk_25m_div_table;
+	dwmac->m25_div.hw.init = &init;
+	dwmac->m25_div.flags = CLK_DIVIDER_ALLOW_ZERO;
+
+	dwmac->m25_div_clk = devm_clk_register(dev, &dwmac->m25_div.hw);
+	if (WARN_ON(IS_ERR(dwmac->m25_div_clk)))
+		return PTR_ERR(dwmac->m25_div_clk);
+
+	return 0;
+}
+
+static int meson8b_init_prg_eth(struct meson8b_dwmac *dwmac)
+{
+	int ret;
+	unsigned long clk_rate;
+
+	switch (dwmac->phy_mode) {
+	case PHY_INTERFACE_MODE_RGMII:
+	case PHY_INTERFACE_MODE_RGMII_ID:
+	case PHY_INTERFACE_MODE_RGMII_RXID:
+	case PHY_INTERFACE_MODE_RGMII_TXID:
+		/* Generate a 25MHz clock for the PHY */
+		clk_rate = 25 * 1000 * 1000;
+
+		/* enable RGMII mode */
+		meson8b_dwmac_mask_bits(dwmac, PRG_ETH0, PRG_ETH0_RGMII_MODE,
+					PRG_ETH0_RGMII_MODE);
+
+		/* only relevant for RMII mode -> disable in RGMII mode */
+		meson8b_dwmac_mask_bits(dwmac, PRG_ETH0,
+					PRG_ETH0_INVERTED_RMII_CLK, 0);
+
+		/* TX clock delay - all known boards use a 1/4 cycle delay */
+		meson8b_dwmac_mask_bits(dwmac, PRG_ETH0, PRG_ETH0_TXDLY_MASK,
+					PRG_ETH0_TXDLY_QUARTER);
+		break;
+
+	case PHY_INTERFACE_MODE_RMII:
+		/* Use the rate of the mux clock for the internal RMII PHY */
+		clk_rate = clk_get_rate(dwmac->m250_mux_clk);
+
+		/* disable RGMII mode -> enables RMII mode */
+		meson8b_dwmac_mask_bits(dwmac, PRG_ETH0, PRG_ETH0_RGMII_MODE,
+					0);
+
+		/* invert internal clk_rmii_i to generate 25/2.5 tx_rx_clk */
+		meson8b_dwmac_mask_bits(dwmac, PRG_ETH0,
+					PRG_ETH0_INVERTED_RMII_CLK,
+					PRG_ETH0_INVERTED_RMII_CLK);
+
+		/* TX clock delay cannot be configured in RMII mode */
+		meson8b_dwmac_mask_bits(dwmac, PRG_ETH0, PRG_ETH0_TXDLY_MASK,
+					0);
+
+		break;
+
+	default:
+		dev_err(&dwmac->pdev->dev, "unsupported phy-mode %s\n",
+			phy_modes(dwmac->phy_mode));
+		return -EINVAL;
+	}
+
+	ret = clk_prepare_enable(dwmac->m25_div_clk);
+	if (ret) {
+		dev_err(&dwmac->pdev->dev, "failed to enable the PHY clock\n");
+		return ret;
+	}
+
+	ret = clk_set_rate(dwmac->m25_div_clk, clk_rate);
+	if (ret) {
+		clk_disable_unprepare(dwmac->m25_div_clk);
+
+		dev_err(&dwmac->pdev->dev, "failed to set PHY clock\n");
+		return ret;
+	}
+
+	/* enable TX_CLK and PHY_REF_CLK generator */
+	meson8b_dwmac_mask_bits(dwmac, PRG_ETH0, PRG_ETH0_TX_AND_PHY_REF_CLK,
+				PRG_ETH0_TX_AND_PHY_REF_CLK);
+
+	return 0;
+}
+
+static int meson8b_dwmac_probe(struct platform_device *pdev)
+{
+	struct plat_stmmacenet_data *plat_dat;
+	struct stmmac_resources stmmac_res;
+	struct resource *res;
+	struct meson8b_dwmac *dwmac;
+	int ret;
+
+	ret = stmmac_get_platform_resources(pdev, &stmmac_res);
+	if (ret)
+		return ret;
+
+	plat_dat = stmmac_probe_config_dt(pdev, &stmmac_res.mac);
+	if (IS_ERR(plat_dat))
+		return PTR_ERR(plat_dat);
+
+	dwmac = devm_kzalloc(&pdev->dev, sizeof(*dwmac), GFP_KERNEL);
+	if (!dwmac)
+		return -ENOMEM;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	dwmac->regs = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(dwmac->regs))
+		return PTR_ERR(dwmac->regs);
+
+	dwmac->pdev = pdev;
+	dwmac->phy_mode = of_get_phy_mode(pdev->dev.of_node);
+	if (dwmac->phy_mode < 0) {
+		dev_err(&pdev->dev, "missing phy-mode property\n");
+		return -EINVAL;
+	}
+
+	ret = meson8b_init_clk(dwmac);
+	if (ret)
+		return ret;
+
+	ret = meson8b_init_prg_eth(dwmac);
+	if (ret)
+		return ret;
+
+	plat_dat->bsp_priv = dwmac;
+
+	return stmmac_dvr_probe(&pdev->dev, plat_dat, &stmmac_res);
+}
+
+static int meson8b_dwmac_remove(struct platform_device *pdev)
+{
+	struct meson8b_dwmac *dwmac = get_stmmac_bsp_priv(&pdev->dev);
+
+	clk_disable_unprepare(dwmac->m25_div_clk);
+
+	return stmmac_pltfr_remove(pdev);
+}
+
+static const struct of_device_id meson8b_dwmac_match[] = {
+	{ .compatible = "amlogic,meson8b-dwmac" },
+	{ .compatible = "amlogic,meson-gxbb-dwmac" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, meson8b_dwmac_match);
+
+static struct platform_driver meson8b_dwmac_driver = {
+	.probe  = meson8b_dwmac_probe,
+	.remove = meson8b_dwmac_remove,
+	.driver = {
+		.name           = "meson8b-dwmac",
+		.pm		= &stmmac_pltfr_pm_ops,
+		.of_match_table = meson8b_dwmac_match,
+	},
+};
+module_platform_driver(meson8b_dwmac_driver);
+
+MODULE_AUTHOR("Martin Blumenstingl <martin.blumenstingl@googlemail.com>");
+MODULE_DESCRIPTION("Amlogic Meson8b and GXBB 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 92105916ef40..3740a4417fa0 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac-rk.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-rk.c
@@ -30,6 +30,7 @@
 #include <linux/delay.h>
 #include <linux/mfd/syscon.h>
 #include <linux/regmap.h>
+#include <linux/pm_runtime.h>
 
 #include "stmmac_platform.h"
 
@@ -301,6 +302,118 @@ static const struct rk_gmac_ops rk3288_ops = {
 	.set_rmii_speed = rk3288_set_rmii_speed,
 };
 
+#define RK3366_GRF_SOC_CON6	0x0418
+#define RK3366_GRF_SOC_CON7	0x041c
+
+/* RK3366_GRF_SOC_CON6 */
+#define RK3366_GMAC_PHY_INTF_SEL_RGMII	(GRF_BIT(9) | GRF_CLR_BIT(10) | \
+					 GRF_CLR_BIT(11))
+#define RK3366_GMAC_PHY_INTF_SEL_RMII	(GRF_CLR_BIT(9) | GRF_CLR_BIT(10) | \
+					 GRF_BIT(11))
+#define RK3366_GMAC_FLOW_CTRL		GRF_BIT(8)
+#define RK3366_GMAC_FLOW_CTRL_CLR	GRF_CLR_BIT(8)
+#define RK3366_GMAC_SPEED_10M		GRF_CLR_BIT(7)
+#define RK3366_GMAC_SPEED_100M		GRF_BIT(7)
+#define RK3366_GMAC_RMII_CLK_25M	GRF_BIT(3)
+#define RK3366_GMAC_RMII_CLK_2_5M	GRF_CLR_BIT(3)
+#define RK3366_GMAC_CLK_125M		(GRF_CLR_BIT(4) | GRF_CLR_BIT(5))
+#define RK3366_GMAC_CLK_25M		(GRF_BIT(4) | GRF_BIT(5))
+#define RK3366_GMAC_CLK_2_5M		(GRF_CLR_BIT(4) | GRF_BIT(5))
+#define RK3366_GMAC_RMII_MODE		GRF_BIT(6)
+#define RK3366_GMAC_RMII_MODE_CLR	GRF_CLR_BIT(6)
+
+/* RK3366_GRF_SOC_CON7 */
+#define RK3366_GMAC_TXCLK_DLY_ENABLE	GRF_BIT(7)
+#define RK3366_GMAC_TXCLK_DLY_DISABLE	GRF_CLR_BIT(7)
+#define RK3366_GMAC_RXCLK_DLY_ENABLE	GRF_BIT(15)
+#define RK3366_GMAC_RXCLK_DLY_DISABLE	GRF_CLR_BIT(15)
+#define RK3366_GMAC_CLK_RX_DL_CFG(val)	HIWORD_UPDATE(val, 0x7F, 8)
+#define RK3366_GMAC_CLK_TX_DL_CFG(val)	HIWORD_UPDATE(val, 0x7F, 0)
+
+static void rk3366_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, RK3366_GRF_SOC_CON6,
+		     RK3366_GMAC_PHY_INTF_SEL_RGMII |
+		     RK3366_GMAC_RMII_MODE_CLR);
+	regmap_write(bsp_priv->grf, RK3366_GRF_SOC_CON7,
+		     RK3366_GMAC_RXCLK_DLY_ENABLE |
+		     RK3366_GMAC_TXCLK_DLY_ENABLE |
+		     RK3366_GMAC_CLK_RX_DL_CFG(rx_delay) |
+		     RK3366_GMAC_CLK_TX_DL_CFG(tx_delay));
+}
+
+static void rk3366_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, RK3366_GRF_SOC_CON6,
+		     RK3366_GMAC_PHY_INTF_SEL_RMII | RK3366_GMAC_RMII_MODE);
+}
+
+static void rk3366_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, RK3366_GRF_SOC_CON6,
+			     RK3366_GMAC_CLK_2_5M);
+	else if (speed == 100)
+		regmap_write(bsp_priv->grf, RK3366_GRF_SOC_CON6,
+			     RK3366_GMAC_CLK_25M);
+	else if (speed == 1000)
+		regmap_write(bsp_priv->grf, RK3366_GRF_SOC_CON6,
+			     RK3366_GMAC_CLK_125M);
+	else
+		dev_err(dev, "unknown speed value for RGMII! speed=%d", speed);
+}
+
+static void rk3366_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, RK3366_GRF_SOC_CON6,
+			     RK3366_GMAC_RMII_CLK_2_5M |
+			     RK3366_GMAC_SPEED_10M);
+	} else if (speed == 100) {
+		regmap_write(bsp_priv->grf, RK3366_GRF_SOC_CON6,
+			     RK3366_GMAC_RMII_CLK_25M |
+			     RK3366_GMAC_SPEED_100M);
+	} else {
+		dev_err(dev, "unknown speed value for RMII! speed=%d", speed);
+	}
+}
+
+static const struct rk_gmac_ops rk3366_ops = {
+	.set_to_rgmii = rk3366_set_to_rgmii,
+	.set_to_rmii = rk3366_set_to_rmii,
+	.set_rgmii_speed = rk3366_set_rgmii_speed,
+	.set_rmii_speed = rk3366_set_rmii_speed,
+};
+
 #define RK3368_GRF_SOC_CON15	0x043c
 #define RK3368_GRF_SOC_CON16	0x0440
 
@@ -413,6 +526,118 @@ static const struct rk_gmac_ops rk3368_ops = {
 	.set_rmii_speed = rk3368_set_rmii_speed,
 };
 
+#define RK3399_GRF_SOC_CON5	0xc214
+#define RK3399_GRF_SOC_CON6	0xc218
+
+/* RK3399_GRF_SOC_CON5 */
+#define RK3399_GMAC_PHY_INTF_SEL_RGMII	(GRF_BIT(9) | GRF_CLR_BIT(10) | \
+					 GRF_CLR_BIT(11))
+#define RK3399_GMAC_PHY_INTF_SEL_RMII	(GRF_CLR_BIT(9) | GRF_CLR_BIT(10) | \
+					 GRF_BIT(11))
+#define RK3399_GMAC_FLOW_CTRL		GRF_BIT(8)
+#define RK3399_GMAC_FLOW_CTRL_CLR	GRF_CLR_BIT(8)
+#define RK3399_GMAC_SPEED_10M		GRF_CLR_BIT(7)
+#define RK3399_GMAC_SPEED_100M		GRF_BIT(7)
+#define RK3399_GMAC_RMII_CLK_25M	GRF_BIT(3)
+#define RK3399_GMAC_RMII_CLK_2_5M	GRF_CLR_BIT(3)
+#define RK3399_GMAC_CLK_125M		(GRF_CLR_BIT(4) | GRF_CLR_BIT(5))
+#define RK3399_GMAC_CLK_25M		(GRF_BIT(4) | GRF_BIT(5))
+#define RK3399_GMAC_CLK_2_5M		(GRF_CLR_BIT(4) | GRF_BIT(5))
+#define RK3399_GMAC_RMII_MODE		GRF_BIT(6)
+#define RK3399_GMAC_RMII_MODE_CLR	GRF_CLR_BIT(6)
+
+/* RK3399_GRF_SOC_CON6 */
+#define RK3399_GMAC_TXCLK_DLY_ENABLE	GRF_BIT(7)
+#define RK3399_GMAC_TXCLK_DLY_DISABLE	GRF_CLR_BIT(7)
+#define RK3399_GMAC_RXCLK_DLY_ENABLE	GRF_BIT(15)
+#define RK3399_GMAC_RXCLK_DLY_DISABLE	GRF_CLR_BIT(15)
+#define RK3399_GMAC_CLK_RX_DL_CFG(val)	HIWORD_UPDATE(val, 0x7F, 8)
+#define RK3399_GMAC_CLK_TX_DL_CFG(val)	HIWORD_UPDATE(val, 0x7F, 0)
+
+static void rk3399_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, RK3399_GRF_SOC_CON5,
+		     RK3399_GMAC_PHY_INTF_SEL_RGMII |
+		     RK3399_GMAC_RMII_MODE_CLR);
+	regmap_write(bsp_priv->grf, RK3399_GRF_SOC_CON6,
+		     RK3399_GMAC_RXCLK_DLY_ENABLE |
+		     RK3399_GMAC_TXCLK_DLY_ENABLE |
+		     RK3399_GMAC_CLK_RX_DL_CFG(rx_delay) |
+		     RK3399_GMAC_CLK_TX_DL_CFG(tx_delay));
+}
+
+static void rk3399_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, RK3399_GRF_SOC_CON5,
+		     RK3399_GMAC_PHY_INTF_SEL_RMII | RK3399_GMAC_RMII_MODE);
+}
+
+static void rk3399_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, RK3399_GRF_SOC_CON5,
+			     RK3399_GMAC_CLK_2_5M);
+	else if (speed == 100)
+		regmap_write(bsp_priv->grf, RK3399_GRF_SOC_CON5,
+			     RK3399_GMAC_CLK_25M);
+	else if (speed == 1000)
+		regmap_write(bsp_priv->grf, RK3399_GRF_SOC_CON5,
+			     RK3399_GMAC_CLK_125M);
+	else
+		dev_err(dev, "unknown speed value for RGMII! speed=%d", speed);
+}
+
+static void rk3399_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, RK3399_GRF_SOC_CON5,
+			     RK3399_GMAC_RMII_CLK_2_5M |
+			     RK3399_GMAC_SPEED_10M);
+	} else if (speed == 100) {
+		regmap_write(bsp_priv->grf, RK3399_GRF_SOC_CON5,
+			     RK3399_GMAC_RMII_CLK_25M |
+			     RK3399_GMAC_SPEED_100M);
+	} else {
+		dev_err(dev, "unknown speed value for RMII! speed=%d", speed);
+	}
+}
+
+static const struct rk_gmac_ops rk3399_ops = {
+	.set_to_rgmii = rk3399_set_to_rgmii,
+	.set_to_rmii = rk3399_set_to_rmii,
+	.set_rgmii_speed = rk3399_set_rgmii_speed,
+	.set_rmii_speed = rk3399_set_rmii_speed,
+};
+
 static int gmac_clk_init(struct rk_priv_data *bsp_priv)
 {
 	struct device *dev = &bsp_priv->pdev->dev;
@@ -629,6 +854,16 @@ static struct rk_priv_data *rk_gmac_setup(struct platform_device *pdev,
 							"rockchip,grf");
 	bsp_priv->pdev = pdev;
 
+	gmac_clk_init(bsp_priv);
+
+	return bsp_priv;
+}
+
+static int rk_gmac_powerup(struct rk_priv_data *bsp_priv)
+{
+	int ret;
+	struct device *dev = &bsp_priv->pdev->dev;
+
 	/*rmii or rgmii*/
 	if (bsp_priv->phy_iface == PHY_INTERFACE_MODE_RGMII) {
 		dev_info(dev, "init for RGMII\n");
@@ -641,15 +876,6 @@ static struct rk_priv_data *rk_gmac_setup(struct platform_device *pdev,
 		dev_err(dev, "NO interface defined!\n");
 	}
 
-	gmac_clk_init(bsp_priv);
-
-	return bsp_priv;
-}
-
-static int rk_gmac_powerup(struct rk_priv_data *bsp_priv)
-{
-	int ret;
-
 	ret = phy_power_on(bsp_priv, true);
 	if (ret)
 		return ret;
@@ -658,11 +884,19 @@ static int rk_gmac_powerup(struct rk_priv_data *bsp_priv)
 	if (ret)
 		return ret;
 
+	pm_runtime_enable(dev);
+	pm_runtime_get_sync(dev);
+
 	return 0;
 }
 
 static void rk_gmac_powerdown(struct rk_priv_data *gmac)
 {
+	struct device *dev = &gmac->pdev->dev;
+
+	pm_runtime_put_sync(dev);
+	pm_runtime_disable(dev);
+
 	phy_power_on(gmac, false);
 	gmac_clk_enable(gmac, false);
 }
@@ -760,7 +994,9 @@ static int rk_gmac_probe(struct platform_device *pdev)
 static const struct of_device_id rk_gmac_dwmac_match[] = {
 	{ .compatible = "rockchip,rk3228-gmac", .data = &rk3228_ops },
 	{ .compatible = "rockchip,rk3288-gmac", .data = &rk3288_ops },
+	{ .compatible = "rockchip,rk3366-gmac", .data = &rk3366_ops },
 	{ .compatible = "rockchip,rk3368-gmac", .data = &rk3368_ops },
+	{ .compatible = "rockchip,rk3399-gmac", .data = &rk3399_ops },
 	{ }
 };
 MODULE_DEVICE_TABLE(of, rk_gmac_dwmac_match);
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac-stm32.c b/drivers/net/ethernet/stmicro/stmmac/dwmac-stm32.c
new file mode 100644
index 000000000000..e5a926b8bee7
--- /dev/null
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-stm32.c
@@ -0,0 +1,194 @@
+/*
+ * dwmac-stm32.c - DWMAC Specific Glue layer for STM32 MCU
+ *
+ * Copyright (C) Alexandre Torgue 2015
+ * Author:  Alexandre Torgue <alexandre.torgue@gmail.com>
+ * License terms:  GNU General Public License (GPL), version 2
+ *
+ */
+
+#include <linux/clk.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/of_net.h>
+#include <linux/phy.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+#include <linux/slab.h>
+#include <linux/stmmac.h>
+
+#include "stmmac_platform.h"
+
+#define MII_PHY_SEL_MASK	BIT(23)
+
+struct stm32_dwmac {
+	struct clk *clk_tx;
+	struct clk *clk_rx;
+	u32 mode_reg;		/* MAC glue-logic mode register */
+	struct regmap *regmap;
+	u32 speed;
+};
+
+static int stm32_dwmac_init(struct plat_stmmacenet_data *plat_dat)
+{
+	struct stm32_dwmac *dwmac = plat_dat->bsp_priv;
+	u32 reg = dwmac->mode_reg;
+	u32 val;
+	int ret;
+
+	val = (plat_dat->interface == PHY_INTERFACE_MODE_MII) ? 0 : 1;
+	ret = regmap_update_bits(dwmac->regmap, reg, MII_PHY_SEL_MASK, val);
+	if (ret)
+		return ret;
+
+	ret = clk_prepare_enable(dwmac->clk_tx);
+	if (ret)
+		return ret;
+
+	ret = clk_prepare_enable(dwmac->clk_rx);
+	if (ret)
+		clk_disable_unprepare(dwmac->clk_tx);
+
+	return ret;
+}
+
+static void stm32_dwmac_clk_disable(struct stm32_dwmac *dwmac)
+{
+	clk_disable_unprepare(dwmac->clk_tx);
+	clk_disable_unprepare(dwmac->clk_rx);
+}
+
+static int stm32_dwmac_parse_data(struct stm32_dwmac *dwmac,
+				  struct device *dev)
+{
+	struct device_node *np = dev->of_node;
+	int err;
+
+	/*  Get TX/RX clocks */
+	dwmac->clk_tx = devm_clk_get(dev, "mac-clk-tx");
+	if (IS_ERR(dwmac->clk_tx)) {
+		dev_err(dev, "No tx clock provided...\n");
+		return PTR_ERR(dwmac->clk_tx);
+	}
+	dwmac->clk_rx = devm_clk_get(dev, "mac-clk-rx");
+	if (IS_ERR(dwmac->clk_rx)) {
+		dev_err(dev, "No rx clock provided...\n");
+		return PTR_ERR(dwmac->clk_rx);
+	}
+
+	/* Get mode register */
+	dwmac->regmap = syscon_regmap_lookup_by_phandle(np, "st,syscon");
+	if (IS_ERR(dwmac->regmap))
+		return PTR_ERR(dwmac->regmap);
+
+	err = of_property_read_u32_index(np, "st,syscon", 1, &dwmac->mode_reg);
+	if (err)
+		dev_err(dev, "Can't get sysconfig mode offset (%d)\n", err);
+
+	return err;
+}
+
+static int stm32_dwmac_probe(struct platform_device *pdev)
+{
+	struct plat_stmmacenet_data *plat_dat;
+	struct stmmac_resources stmmac_res;
+	struct stm32_dwmac *dwmac;
+	int ret;
+
+	ret = stmmac_get_platform_resources(pdev, &stmmac_res);
+	if (ret)
+		return ret;
+
+	plat_dat = stmmac_probe_config_dt(pdev, &stmmac_res.mac);
+	if (IS_ERR(plat_dat))
+		return PTR_ERR(plat_dat);
+
+	dwmac = devm_kzalloc(&pdev->dev, sizeof(*dwmac), GFP_KERNEL);
+	if (!dwmac)
+		return -ENOMEM;
+
+	ret = stm32_dwmac_parse_data(dwmac, &pdev->dev);
+	if (ret) {
+		dev_err(&pdev->dev, "Unable to parse OF data\n");
+		return ret;
+	}
+
+	plat_dat->bsp_priv = dwmac;
+
+	ret = stm32_dwmac_init(plat_dat);
+	if (ret)
+		return ret;
+
+	ret = stmmac_dvr_probe(&pdev->dev, plat_dat, &stmmac_res);
+	if (ret)
+		stm32_dwmac_clk_disable(dwmac);
+
+	return ret;
+}
+
+static int stm32_dwmac_remove(struct platform_device *pdev)
+{
+	struct net_device *ndev = platform_get_drvdata(pdev);
+	struct stmmac_priv *priv = netdev_priv(ndev);
+	int ret = stmmac_dvr_remove(&pdev->dev);
+
+	stm32_dwmac_clk_disable(priv->plat->bsp_priv);
+
+	return ret;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int stm32_dwmac_suspend(struct device *dev)
+{
+	struct net_device *ndev = dev_get_drvdata(dev);
+	struct stmmac_priv *priv = netdev_priv(ndev);
+	int ret;
+
+	ret = stmmac_suspend(dev);
+	stm32_dwmac_clk_disable(priv->plat->bsp_priv);
+
+	return ret;
+}
+
+static int stm32_dwmac_resume(struct device *dev)
+{
+	struct net_device *ndev = dev_get_drvdata(dev);
+	struct stmmac_priv *priv = netdev_priv(ndev);
+	int ret;
+
+	ret = stm32_dwmac_init(priv->plat);
+	if (ret)
+		return ret;
+
+	ret = stmmac_resume(dev);
+
+	return ret;
+}
+#endif /* CONFIG_PM_SLEEP */
+
+static SIMPLE_DEV_PM_OPS(stm32_dwmac_pm_ops,
+	stm32_dwmac_suspend, stm32_dwmac_resume);
+
+static const struct of_device_id stm32_dwmac_match[] = {
+	{ .compatible = "st,stm32-dwmac"},
+	{ }
+};
+MODULE_DEVICE_TABLE(of, stm32_dwmac_match);
+
+static struct platform_driver stm32_dwmac_driver = {
+	.probe  = stm32_dwmac_probe,
+	.remove = stm32_dwmac_remove,
+	.driver = {
+		.name           = "stm32-dwmac",
+		.pm		= &stm32_dwmac_pm_ops,
+		.of_match_table = stm32_dwmac_match,
+	},
+};
+module_platform_driver(stm32_dwmac_driver);
+
+MODULE_AUTHOR("Alexandre Torgue <alexandre.torgue@gmail.com>");
+MODULE_DESCRIPTION("STMicroelectronics MCU DWMAC Specific Glue layer");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac1000_core.c b/drivers/net/ethernet/stmicro/stmmac/dwmac1000_core.c
index 885a5e64519d..7df4ff158f3d 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac1000_core.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac1000_core.c
@@ -145,7 +145,7 @@ static void dwmac1000_set_mchash(void __iomem *ioaddr, u32 *mcfilterbits,
 		numhashregs = 8;
 		break;
 	default:
-		pr_debug("STMMAC: err in setting mulitcast filter\n");
+		pr_debug("STMMAC: err in setting multicast filter\n");
 		return;
 		break;
 	}
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
index 4c8c60af7985..6c85b61aaa0b 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
@@ -650,20 +650,27 @@ static int stmmac_init_ptp(struct stmmac_priv *priv)
 	if (IS_ERR(priv->clk_ptp_ref)) {
 		priv->clk_ptp_rate = clk_get_rate(priv->stmmac_clk);
 		priv->clk_ptp_ref = NULL;
+		netdev_dbg(priv->dev, "PTP uses main clock\n");
 	} else {
 		clk_prepare_enable(priv->clk_ptp_ref);
 		priv->clk_ptp_rate = clk_get_rate(priv->clk_ptp_ref);
+		netdev_dbg(priv->dev, "PTP rate %d\n", priv->clk_ptp_rate);
 	}
 
 	priv->adv_ts = 0;
-	if (priv->dma_cap.atime_stamp && priv->extend_desc)
+	/* Check if adv_ts can be enabled for dwmac 4.x core */
+	if (priv->plat->has_gmac4 && priv->dma_cap.atime_stamp)
+		priv->adv_ts = 1;
+	/* Dwmac 3.x core with extend_desc can support adv_ts */
+	else if (priv->extend_desc && priv->dma_cap.atime_stamp)
 		priv->adv_ts = 1;
 
-	if (netif_msg_hw(priv) && priv->dma_cap.time_stamp)
-		pr_debug("IEEE 1588-2002 Time Stamp supported\n");
+	if (priv->dma_cap.time_stamp)
+		netdev_info(priv->dev, "IEEE 1588-2002 Timestamp supported\n");
 
-	if (netif_msg_hw(priv) && priv->adv_ts)
-		pr_debug("IEEE 1588-2008 Advanced Time Stamp supported\n");
+	if (priv->adv_ts)
+		netdev_info(priv->dev,
+			    "IEEE 1588-2008 Advanced Timestamp supported\n");
 
 	priv->hw->ptp = &stmmac_ptp;
 	priv->hwts_tx_en = 0;
@@ -1702,8 +1709,8 @@ static int stmmac_hw_setup(struct net_device *dev, bool init_ptp)
 
 	if (init_ptp) {
 		ret = stmmac_init_ptp(priv);
-		if (ret && ret != -EOPNOTSUPP)
-			pr_warn("%s: failed PTP initialisation\n", __func__);
+		if (ret)
+			netdev_warn(priv->dev, "PTP support cannot init.\n");
 	}
 
 #ifdef CONFIG_DEBUG_FS
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c
index 756bb548e81a..0a0d6a86f397 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c
@@ -265,6 +265,7 @@ stmmac_probe_config_dt(struct platform_device *pdev, const char **mac)
 	 * once needed on other platforms.
 	 */
 	if (of_device_is_compatible(np, "st,spear600-gmac") ||
+		of_device_is_compatible(np, "snps,dwmac-3.50a") ||
 		of_device_is_compatible(np, "snps,dwmac-3.70a") ||
 		of_device_is_compatible(np, "snps,dwmac")) {
 		/* Note that the max-frame-size parameter as defined in the
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.h b/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.h
index ffeb8d9e2b2e..64e147f53a9c 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.h
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.h
@@ -30,4 +30,12 @@ int stmmac_get_platform_resources(struct platform_device *pdev,
 int stmmac_pltfr_remove(struct platform_device *pdev);
 extern const struct dev_pm_ops stmmac_pltfr_pm_ops;
 
+static inline void *get_stmmac_bsp_priv(struct device *dev)
+{
+	struct net_device *ndev = dev_get_drvdata(dev);
+	struct stmmac_priv *priv = netdev_priv(ndev);
+
+	return priv->plat->bsp_priv;